--- /srv/reproducible-results/rbuild-debian/r-b-build.xhHtIGby/b1/gtsam_4.2.0+dfsg-1_amd64.changes +++ /srv/reproducible-results/rbuild-debian/r-b-build.xhHtIGby/b2/gtsam_4.2.0+dfsg-1_amd64.changes ├── Files │ @@ -1,7 +1,7 @@ │ │ b1be64aac3407832fb89c2c611449c0b 362396 libdevel optional libgtsam-dev_4.2.0+dfsg-1_amd64.deb │ - 2281aeeaae73f7cb59bc462b4baf01ce 14619516 doc optional libgtsam-doc_4.2.0+dfsg-1_all.deb │ + 3cb7a8a2c6f1b426294c249337ab7264 14619424 doc optional libgtsam-doc_4.2.0+dfsg-1_all.deb │ 24135b49ed57e3cf58b1827d4095d916 159328876 debug optional libgtsam4-dbgsym_4.2.0+dfsg-1_amd64.deb │ a86765db210e443c7724fec8948bee43 1627232 libs optional libgtsam4_4.2.0+dfsg-1_amd64.deb │ 35581130400639c57d7a0fe112cca983 6324920 debug optional python3-gtsam-dbgsym_4.2.0+dfsg-1_amd64.deb │ 36de366ed03fb91f895a48de1760ad1f 5920812 python optional python3-gtsam_4.2.0+dfsg-1_amd64.deb ├── libgtsam-doc_4.2.0+dfsg-1_all.deb │ ├── file list │ │ @@ -1,3 +1,3 @@ │ │ -rw-r--r-- 0 0 0 4 2024-01-09 05:51:35.000000 debian-binary │ │ --rw-r--r-- 0 0 0 76080 2024-01-09 05:51:35.000000 control.tar.xz │ │ --rw-r--r-- 0 0 0 14543244 2024-01-09 05:51:35.000000 data.tar.xz │ │ +-rw-r--r-- 0 0 0 75752 2024-01-09 05:51:35.000000 control.tar.xz │ │ +-rw-r--r-- 0 0 0 14543480 2024-01-09 05:51:35.000000 data.tar.xz │ ├── control.tar.xz │ │ ├── control.tar │ │ │ ├── ./control │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ Package: libgtsam-doc │ │ │ │ Source: gtsam │ │ │ │ Version: 4.2.0+dfsg-1 │ │ │ │ Architecture: all │ │ │ │ Maintainer: Debian Science Maintainers │ │ │ │ -Installed-Size: 63803 │ │ │ │ +Installed-Size: 63804 │ │ │ │ Depends: libjs-mathjax │ │ │ │ Section: doc │ │ │ │ Priority: optional │ │ │ │ Homepage: http://www.gtsam.org │ │ │ │ Description: Factor graphs for sensor fusion in robotics │ │ │ │ GTSAM is a C++ library that implements sensor fusion for robotics and computer │ │ │ │ vision applications, including SLAM (Simultaneous Localization and Mapping), VO │ │ │ ├── ./md5sums │ │ │ │ ├── ./md5sums │ │ │ │ │┄ Files differ │ │ │ │ ├── line order │ │ │ │ │ @@ -125,957 +125,957 @@ │ │ │ │ │ usr/share/doc/libgtsam-dev/examples/UGM_chain.cpp │ │ │ │ │ usr/share/doc/libgtsam-dev/examples/UGM_small.cpp │ │ │ │ │ usr/share/doc/libgtsam-dev/examples/VisualISAM2Example.cpp.gz │ │ │ │ │ usr/share/doc/libgtsam-dev/examples/VisualISAMExample.cpp.gz │ │ │ │ │ usr/share/doc/libgtsam-dev/examples/easyPoint2KalmanFilter.cpp.gz │ │ │ │ │ usr/share/doc/libgtsam-dev/examples/elaboratePoint2KalmanFilter.cpp.gz │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00002.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00002_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00005.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00002.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00005_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00008.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00008.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00008_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00011.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00011.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00011_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00014.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00014.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00014_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00017.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00017.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00017_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00020.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00020.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00020_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00023.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00023.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00023_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00026.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00026.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00026_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00029.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00029.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00029_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00032.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00032.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00032_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00035.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00035.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00035_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00038.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00038_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00041.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00041.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00041_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00044.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00044.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00044_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00047.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00047.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00047_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00050.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00050_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00053.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00053.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00056.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00056.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00056_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00059.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00065.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00065.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00062.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00062.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00062_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00065_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00068.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00068.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00068_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00071.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00071.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00071_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00074.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00074.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00074_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00077_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00077.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00080.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00080.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00080_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00083.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00083.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00083_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00086.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00086_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00089.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00089.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00089_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00092.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00092.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00092_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00095.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00095.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00095_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00098.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00098.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00098_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00101.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00101.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00101_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00104.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00104.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00104_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00107.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00107.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00107_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00110.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00110.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00110_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00113.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00113_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00116.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00116.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00116_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00119.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00119.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00119_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00122.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00122.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00125.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00125.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00125_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00128.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00128.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00128_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00131.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00131_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00134.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00134.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00134_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00137.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00137_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00140.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00140.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00140_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00143.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00143_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00146.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00146.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00146_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00149.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00149.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00149_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00152.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00152.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00152_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00155.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00155.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00155_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00158.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00158.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00158_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00161.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00161.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00161_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00164.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00164.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00164_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00167.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00167.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00167_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00170.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00170.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00170_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00173.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00173.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00173_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00176.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00176.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00176_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00179.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00182.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00182.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00182_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00185.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00185.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00185_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00188.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00188.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00191.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00194.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00194.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00194_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00197.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00197_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00200.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00200_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00203.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00203.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00203_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00206.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00206.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00206_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00209.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00209.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00209_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00212.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00212.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00212_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00215.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00215.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00215_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00218.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00221.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00221.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00221_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00224.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00224_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00227.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00227.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00227_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00230.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00230.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00230_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00233.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00233.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00233_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00236.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00236.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00236_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00239.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00239.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00239_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00242.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00242.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00242_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00245.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00245.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00245_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00248.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00248.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00248_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00251.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00251.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00251_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00254.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00254.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00257.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00257.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00257_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00260.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00260.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00260_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00263.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00263.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00263_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00266.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00269.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00269_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00272.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00272.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00272_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00275_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00275.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00275.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00278.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00278.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00278_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00281.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00281.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00281_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00284.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00284.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00284_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00287.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00287.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00287_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00290.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00290.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00290_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00293.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00293.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00293_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00296.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00296.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00296_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00299.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00299.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00299_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00302.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00302.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00302_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00305.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00305.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00305_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00308.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00311.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00311.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00311_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00314.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00314.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00314_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00317.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00317.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00317_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00320.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00323.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00326.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00326.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00326_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00329.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00329.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00329_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00332.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00332.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00332_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00335.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00335.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00335_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00338.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00338.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00338_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00341.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00341.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00341_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00344.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00344.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00344_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00347.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00347.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00353.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00353.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00353_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00356.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00359.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00359.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00359_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00362.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00365.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00365.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00365_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00368.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00368.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00368_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00371.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00371.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00371_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00374.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00374.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00374_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00377.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00377.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00377_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00380.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00380.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00380_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00383.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00383.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00383_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00386.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00386.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00389_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00392.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00395.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00395.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00386_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00389.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00389.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00392_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00395_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00398.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00398.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00398_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00401.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00401.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00401_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00404.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00404.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00404_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00407.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00407.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00407_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00410.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00410.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00410_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00413.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00416.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00419.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00422.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00422.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00422_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00425.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00425.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00425_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00428.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00431.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00431.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00431_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00434.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00434.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00434_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00437.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00437.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00437_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00440_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00443.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00443.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00443_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00446.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00446.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00446_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00449.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00449.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00449_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00455.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00458.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00458.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00458_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00452.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00455_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00461.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00461.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00461_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00464.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00464.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00464_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00467.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00467.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00467_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00470.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00470_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00473.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00476.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00476.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00479.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00482.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00482.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00482_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00485.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00485.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00488.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00488.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00488_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00491.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00491.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00494.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00494.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00494_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00497.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00497.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00497_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00500.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00500.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00500_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00503.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00503.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00503_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00506.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00509.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00512.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00512.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00512_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00515.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00515.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00518.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00518.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00518_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00521.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00524.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00524.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00524_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00527.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00527.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00527_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00530.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00533.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00533.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00533_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00536.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00536.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00536_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00539.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00539.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00539_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00542.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00542_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00545.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00545_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00548.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00548.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00548_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00551.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00551.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00551_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00554.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00554.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00554_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00557.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00557.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00557_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00560.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00560.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00560_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00563.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00563.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00563_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00566.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00566.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00566_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00569.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00572.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00572.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00569.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00569_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00572_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00575.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00575.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00575_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00578.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00578.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00578_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00581.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00581.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00581_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00584.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00584.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00584_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00587.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00587.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00587_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00590.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00590.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00590_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00593.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00593_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00596.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00596.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00596_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00599.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00599.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00599_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00602.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00602.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00602_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00605.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00605.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00605_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00608_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00611.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00611.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00611_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00614.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00614_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00617.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00617_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00620.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00620.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00620_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00623.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00623.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00623_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00626.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00626.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00626_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00629.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00632.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00632_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00635.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00635.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00635_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00638.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00641.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00641.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00641_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00644.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00644_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00647.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00647_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00650.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00650.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00650_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00653.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00653.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00653_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00656.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00656.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00656_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00659.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00659_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00662.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00662.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00662_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00665.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00665.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00665_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00668.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00668.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00668_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00671.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00671.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00671_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00674.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00674.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00674_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00677.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00677.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00677_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00680.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00680.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00680_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00683.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00683.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00683_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00686.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00686_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00689.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00689_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00692.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00695.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00695.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00692_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00695_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00698.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00701.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00701.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00701_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00704.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00704_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00707.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00707.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00707_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00710.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00713.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00713.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00713_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00716.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00716.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00716_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00719.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00719.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00719_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00722.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00722.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00722_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00725.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00725.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00725_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00728.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00728.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00728_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00731.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00731.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00731_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00734.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00737.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00737_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00740.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00743.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00746.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00749.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00749.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00749_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00746.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00746_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00752.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00752.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00752_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00755.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00755.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00755_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00758.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00758.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00758_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00761.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00761.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00761_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00764.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00764.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00764_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00767.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00767.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00767_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00770.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00770.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00770_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00773.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00773.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00773_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00779.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00776.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00776.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00776_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00782.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00782.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00782_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00785.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00785.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00785_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00788.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00788.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00788_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00791.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00791.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00794.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00794.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00794_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00797.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00797.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00797_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00800.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00803.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00803.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00803_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00806.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00806.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00806_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00809.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00809.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00812.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00812.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00812_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00815.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00815.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00815_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00818.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00821.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00821.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00821_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00824.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00824.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00824_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00827.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00830.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00830.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00830_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00833.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00833.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00833_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00836.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00836.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00836_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00839.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00839.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00839_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00842.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00842.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00842_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00845.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00845.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00845_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00848.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00848.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00848_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00851.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00851.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00854.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00854_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00857.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00857.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00857_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00860.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00860.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00860_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00863.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00863.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00863_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00866.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00866.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00866_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00869.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00869_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00872.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00872.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00872_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00875.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00875.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00878.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00878.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00878_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00881.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00881.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00881_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00884.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00884.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00884_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00887.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00890_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00887.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00887_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00890.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00890.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00893.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00893.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00893_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00896.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00896.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00896_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00899.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00899.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00899_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00902.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00905.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00905.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00905_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00908.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00908.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00908_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00911.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00911_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00914.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00917.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00920.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00920.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00920_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00914.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00914_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00923.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00923.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00923_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00926.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00926.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00926_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00929.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00929.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00929_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00932.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00932.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00932_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00935.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00935.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00935_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00938.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00938.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00938_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00941.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00941.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00941_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00944.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00944.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00944_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00947.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00947.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00947_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00956.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00956.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00950.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00953.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00959.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00959.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00959_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00965.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00962.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00968.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00971.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00971.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00971_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00974.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00974.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00974_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00977.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00977.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00977_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00980.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00980.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00980_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00983.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00983.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00983_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00986.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a00986.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00986_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00989.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00992.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00992.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00992_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00995.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00995.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00995_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00998.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a00998.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a00998_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01001.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01004.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01004.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01004_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01010.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01010.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01010_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01013.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01013.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01013_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01016.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01016.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01016_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01019.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01019.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01019_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01022.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01022.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01022_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01025.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01025.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01025_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01028.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01028.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01028_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01031.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01031.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01031_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01034.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01034.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01034_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01037.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01037.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01037_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01040.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01040.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01040_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01043.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01043.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01043_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01046.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01046.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01046_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01049.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01049.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01052.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01049_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01055.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01055.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01055_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01058.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01058_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01061.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01061.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01061_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01064.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01064.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01064_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01067.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01067.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01067_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01070.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01070.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01070_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01073.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01073.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01073_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01076.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01076.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01076_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01079.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01082.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01082.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01082_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01085.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01085.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01085_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01088.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01088.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01088_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01091.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01091.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01091_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01094.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01094.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01094_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01097_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01097.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01100.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01100.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01100_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01103.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01103.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01103_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01106.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01106.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01106_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01109_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01112_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01115.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01115.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01109.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01112.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01115_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01118.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01118.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01118_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01121.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01121.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01121_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01124.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01124.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01124_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01127.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01127.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01127_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01130.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01130.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01130_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01133.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01136.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01133.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01133_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01136_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01139.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01139.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01139_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01142.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01142.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01142_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01145.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01145.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01145_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01148.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01148.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01148_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01151.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01151.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01151_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01154.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01154.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01154_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01157.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01157.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01157_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01160.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01160.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01160_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01163.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01163.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01163_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01166.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01166.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01166_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01169.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01169.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01169_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01172.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01172.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01172_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01184.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01184.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01184_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01187.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01187.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01187_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01190.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01190.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01190_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01193.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01193.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01193_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01196.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01196.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01196_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01199.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01199_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01202.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01202.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01202_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01205.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01205_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01208.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01208.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01208_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01211.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01211.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01211_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01214.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01214.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01214_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01217.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01217.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01217_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01220.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01220.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01220_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01223.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01223.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01223_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01226.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01226.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01226_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01229.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01229.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01229_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01232.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01235.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01235.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01238_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01235_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01238.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01241.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01241.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01241_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01244_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01247.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01247.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01250.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01250.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01247_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01250_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01253.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01253.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01253_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01256.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01256_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01259.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01259.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01262.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01262.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01262_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01265.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01265.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01265_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01268.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01268.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01268_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01271.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01271.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01271_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01274.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01274.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01274_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01277_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01277.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01277.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01280.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01280.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01280_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01283.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01283.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01283_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01286_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01289.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01289_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01292.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01292.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01292_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01295.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01295.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01295_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01298_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01301.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01301.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01301_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01304.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01304.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01304_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01307_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01310.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01310.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01310_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01313.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01313.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01313_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01316_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01316.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01319.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01319.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01319_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01322.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01322.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01322_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01325.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01325.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01325_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01328.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01328.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01328_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01331.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01334.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01334.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01331_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01334_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01337.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01337_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01340_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01343.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01343.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01343_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01346.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01346.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01346_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01349.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01349.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01349_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01352.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01352.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01352_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01355.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01355.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01355_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01358.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01358.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01361.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01361.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01361_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01364.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01364.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01364_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01367.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01367.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01367_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01370.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01370.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01370_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01373.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01379.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01379.js │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01379_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01382.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01382.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01385.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01385.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01385_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01388.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01388.js │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01388_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01391.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01391.js │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01391_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01397.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01397_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01400.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01400_source.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01403.html │ │ │ │ │ -usr/share/doc/libgtsam-dev/html/a01403_source.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01406.html │ │ │ │ │ +usr/share/doc/libgtsam-dev/html/a01406_source.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01409.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01410.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01411.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01412.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01413.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01414.html │ │ │ │ │ usr/share/doc/libgtsam-dev/html/a01415.html │ ├── data.tar.xz │ │ ├── data.tar │ │ │ ├── file list │ │ │ │ @@ -1,14 +1,14 @@ │ │ │ │ drwxr-xr-x 0 root (0) root (0) 0 2024-01-09 05:51:35.000000 ./ │ │ │ │ drwxr-xr-x 0 root (0) root (0) 0 2024-01-09 05:51:35.000000 ./usr/ │ │ │ │ drwxr-xr-x 0 root (0) root (0) 0 2024-01-09 05:51:35.000000 ./usr/share/ │ │ │ │ drwxr-xr-x 0 root (0) root (0) 0 2024-01-09 05:51:35.000000 ./usr/share/doc/ │ │ │ │ drwxr-xr-x 0 root (0) root (0) 0 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/ │ │ │ │ -rw-r--r-- 0 root (0) root (0) 79705 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/Hybrid.pdf │ │ │ │ --rw-r--r-- 0 root (0) root (0) 154592 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/ImuFactor.pdf │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 155126 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/ImuFactor.pdf │ │ │ │ -rw-r--r-- 0 root (0) root (0) 42477 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/cholesky.pdf │ │ │ │ drwxr-xr-x 0 root (0) root (0) 0 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/examples/ │ │ │ │ -rw-r--r-- 0 root (0) root (0) 156 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/examples/CMakeLists.txt │ │ │ │ -rw-r--r-- 0 root (0) root (0) 3425 2023-09-04 02:21:18.000000 ./usr/share/doc/libgtsam-dev/examples/CameraResectioning.cpp │ │ │ │ -rw-r--r-- 0 root (0) root (0) 3838 2023-09-04 02:21:18.000000 ./usr/share/doc/libgtsam-dev/examples/CombinedImuFactorsExample.cpp.gz │ │ │ │ -rw-r--r-- 0 root (0) root (0) 3896 2023-09-04 02:21:18.000000 ./usr/share/doc/libgtsam-dev/examples/CreateSFMExampleData.cpp │ │ │ │ drwxr-xr-x 0 root (0) root (0) 0 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/examples/Data/ │ │ │ │ @@ -133,958 +133,958 @@ │ │ │ │ -rw-r--r-- 0 root (0) root (0) 2889 2023-09-04 02:21:18.000000 ./usr/share/doc/libgtsam-dev/examples/UGM_chain.cpp │ │ │ │ -rw-r--r-- 0 root (0) root (0) 2641 2023-09-04 02:21:18.000000 ./usr/share/doc/libgtsam-dev/examples/UGM_small.cpp │ │ │ │ -rw-r--r-- 0 root (0) root (0) 2368 2023-09-04 02:21:18.000000 ./usr/share/doc/libgtsam-dev/examples/VisualISAM2Example.cpp.gz │ │ │ │ -rw-r--r-- 0 root (0) root (0) 2104 2023-09-04 02:21:18.000000 ./usr/share/doc/libgtsam-dev/examples/VisualISAMExample.cpp.gz │ │ │ │ -rw-r--r-- 0 root (0) root (0) 1713 2023-09-04 02:21:18.000000 ./usr/share/doc/libgtsam-dev/examples/easyPoint2KalmanFilter.cpp.gz │ │ │ │ -rw-r--r-- 0 root (0) root (0) 3926 2023-09-04 02:21:18.000000 ./usr/share/doc/libgtsam-dev/examples/elaboratePoint2KalmanFilter.cpp.gz │ │ │ │ drwxr-xr-x 0 root (0) root (0) 0 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/ │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7913 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00002.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 14683 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00002_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7978 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00005.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7389 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00008.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 180 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00008.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 9727 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00008_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 17818 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00011.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 1016 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00011.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 65609 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00011_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 17409 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00014.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 1045 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00014.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 66871 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00014_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8401 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00017.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7983 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00020.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 312 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00020.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 48110 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00020_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7344 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00023.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 180 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00023.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 10629 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00026.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 87 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00026.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 15877 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00026_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 10386 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00029.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 125 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00029.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 27505 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00029_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 66646 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00032.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 2508 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00032.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 192234 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00032_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8418 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00035.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 343 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00035.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 27879 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00035_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 19377 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00038_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6548 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00041.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 65 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00041.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 20753 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00041_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 11015 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00044.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 429 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00044.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 37799 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00044_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 9190 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00047.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 250 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00047.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 12942 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00047_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 17131 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00050.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 35570 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00050_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7057 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00056.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 172 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00056.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 18897 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00056_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5941 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00059.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 27261 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00065.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 1636 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00065.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 37709 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00065_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6595 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00068.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 91 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00068.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6662 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00071.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 18268 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00071_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 9399 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00074.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 237 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00074.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 12509 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00074_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 16112 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00077_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 28203 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00080.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 2121 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00080.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 15649 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00083.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 556 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00083.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 29171 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00083_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5816 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00086.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7079 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00089.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 16163 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00089_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 21582 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00092.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 143 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00092.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 56603 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00092_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6757 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00095.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 156 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00095.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 35281 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00095_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 13624 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00098.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 675 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00098.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 39982 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00098_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6884 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00101.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 94 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00101.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 9315 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00101_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 12084 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00104.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 696 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00104.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 31578 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00104_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 77207 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00107_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6665 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00110.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 18668 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00110_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6657 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00113.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 27843 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00113_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6741 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00116.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 52828 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00116_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 24215 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00119.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 1480 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00119.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 51312 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00119_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5679 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00122.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 18926 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00125.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 1345 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00125.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 26139 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00128_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8433 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00131.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 14770 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00131_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 25653 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00134.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 2086 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00134.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 57397 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00134_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6608 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00137.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 82782 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00137_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6209 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00140.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 85 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00140.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 25066 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00140_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 52200 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00143_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 46759 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00146.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 3446 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00146.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 77745 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00146_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7226 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00152.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 126 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00152.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 13159 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00152_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7825 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00155.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 150 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00155.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 40486 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00155_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6449 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00158.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 68 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00158.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 18960 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00158_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7285 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00161.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 66 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00161.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 22085 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00161_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5706 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00164.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 10375 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00167.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 324 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00167.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 54641 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00167_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5639 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00170.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 13635 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00173.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 803 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00173.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 85202 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00173_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8041 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00176.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 225 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00176.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 24383 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00176_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7344 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00002.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 180 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00002.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 16112 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00005_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7057 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00008.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 172 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00008.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18897 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00008_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18926 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00011.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 1345 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00011.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 27261 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00014.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 1636 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00014.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 37709 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00014_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 66646 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00017.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 2508 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00017.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 192234 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00017_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7913 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00020.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 14683 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00020_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 25653 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00023.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 2086 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00023.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 57397 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00023_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8433 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00026.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 14770 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00026_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 17131 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00029.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 35570 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00029_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6662 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00038.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18268 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00038_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 77207 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00041_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6657 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00044.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 27843 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00044_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6665 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00047.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18668 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00047_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7978 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00050.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6595 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00053.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 91 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00053.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9190 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00056.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 250 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00056.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 12942 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00056_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5679 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00059.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10386 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00062.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 125 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00062.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 27505 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00062_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 26139 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00065_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6548 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00068.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 65 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00068.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 20753 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00068_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 15649 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00071.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 556 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00071.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 29171 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00071_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6884 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00074.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 94 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00074.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9315 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00074_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5816 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00077.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13624 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00080.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 675 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00080.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 39982 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00080_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7983 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00083.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 312 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00083.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 48110 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00083_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 52200 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00086_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8418 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00089.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 343 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00089.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 27879 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00089_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 11015 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00092.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 429 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00092.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 37799 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00092_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 46759 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00095.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 3446 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00095.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 77745 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00095_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 19377 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00098_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5941 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00101.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7389 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00104.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 180 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00104.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9727 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00104_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 12084 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00107.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 696 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00107.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 31578 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00107_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6757 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00110.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 156 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00110.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 35281 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00110_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6608 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00113.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 82782 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00113_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10629 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00116.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 87 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00116.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 15877 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00116_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 21582 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00119.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 143 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00119.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 56603 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00119_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 28203 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00122.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 2121 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00122.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 24215 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00125.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 1480 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00125.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 51312 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00125_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 17818 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00128.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 1016 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00128.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 65609 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00128_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6209 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00134.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 85 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00134.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 25066 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00134_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7079 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00137.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 16163 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00137_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8401 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00140.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6741 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00143.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 52828 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00143_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 17409 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00146.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 1045 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00146.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 66871 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00146_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9399 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00149.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 237 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00149.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 12509 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00149_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5706 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00152.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7226 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00155.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 126 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00155.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13159 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00155_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7825 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00158.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 150 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00158.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 40486 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00158_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8041 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00161.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 225 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00161.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 24383 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00161_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13635 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00164.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 803 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00164.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 85202 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00164_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6449 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00167.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 68 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00167.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18960 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00167_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10375 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00170.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 324 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00170.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 54641 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00170_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7285 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00173.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 66 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00173.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 22085 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00173_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5639 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00176.html │ │ │ │ -rw-r--r-- 0 root (0) root (0) 5540 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00179.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7720 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00182.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 184 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00182.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6594 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00185.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 86 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00185.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 20439 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00185_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5555 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00188.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5552 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00191.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 9286 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00194.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 309 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00194.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 27194 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00194_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5639 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00197.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 206584 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00200_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6998 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00182.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 83 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00182.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 24591 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00182_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5592 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00185.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6491 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00188.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 95 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00188.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5575 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00191.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6434 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00194.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 19109 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00194_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 206584 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00197_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6320 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00200.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 15576 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00200_source.html │ │ │ │ -rw-r--r-- 0 root (0) root (0) 7427 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00203.html │ │ │ │ -rw-r--r-- 0 root (0) root (0) 98 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00203.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 13987 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00203_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7532 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00206.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 162 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00206.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6491 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00209.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 95 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00209.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6998 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00212.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 83 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00212.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 24591 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00212_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6738 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00215.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 85 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00215.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 44827 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00215_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5575 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00218.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 12074 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00221.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 444 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00221.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 69958 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00221_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5566 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00224.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7329 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00227.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 164 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00227.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 40301 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00227_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 9288 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00230.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 337 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00230.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 50616 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00230_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6604 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00233.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 82 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00233.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 32573 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00233_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 10021 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00236.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 244 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00236.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 21706 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00236_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8783 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00239.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 289 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00239.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 24144 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00239_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6278 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00242.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5707 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00245.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7124 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00248.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 144 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00248.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 25959 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00248_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6589 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00251.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 84 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00251.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 47723 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00251_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 13686 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00254.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 244 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00254.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5592 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00257.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6320 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00260.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 15576 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00260_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7580 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00263.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 159 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00263.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 29980 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00263_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6642 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00269.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 18784 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00269_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6434 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00272.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 19109 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00272_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 51201 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00275_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6379 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00278.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5688 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00281.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 31329 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00281_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6883 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00284.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 136 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00284.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 29154 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00284_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 9756 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00287.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 352 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00287.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 26504 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00287_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7569 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00293.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 81 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00293.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 32907 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00293_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7039 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00296.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 223 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00296.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 94047 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00296_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8992 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00299.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 103 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00299.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 24792 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00299_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6968 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00302.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 148 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00302.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 32402 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00302_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7047 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00305.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 144 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00305.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 25061 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00305_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 4906 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00308.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 10697 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00311.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 403 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00311.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 79141 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00311_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6953 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00314.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 144 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00314.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 30607 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00314_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6858 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00317.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 132 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00317.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 19855 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00317_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6268 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00320.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6259 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00323.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6171 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00326.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7527 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00329.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 152 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00329.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 49706 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00329_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7439 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00335.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 258 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00335.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 80368 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00335_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5656 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00338.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 10872 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00341.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 457 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00341.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 61746 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00341_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 15430 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00344.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 661 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00344.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 23382 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00344_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7283 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00347.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 85 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00347.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7636 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00353.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 101 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00353.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 33928 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00353_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6957 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00359.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 144 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00359.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 34458 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00359_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10021 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00206.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 244 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00206.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 21706 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00206_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6594 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00209.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 86 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00209.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 20439 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00209_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6589 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00212.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 84 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00212.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 47723 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00212_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5555 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00215.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5566 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00218.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7580 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00221.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 159 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00221.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 29980 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00221_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6642 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00224.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18784 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00224_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5552 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00227.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7720 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00230.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 184 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00230.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9286 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00233.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 309 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00233.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 27194 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00233_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9288 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00236.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 337 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00236.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 50616 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00236_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13686 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00239.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 244 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00239.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7329 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00242.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 164 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00242.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 40301 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00242_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7124 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00245.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 144 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00245.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 25959 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00245_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6604 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00248.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 82 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00248.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 32573 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00248_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7532 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00251.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 162 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00251.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 12074 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00257.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 444 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00257.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 69958 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00257_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8783 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00260.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 289 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00260.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 24144 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00260_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5639 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00263.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5707 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00266.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6278 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00269.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6738 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00272.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 85 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00272.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 44827 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00272_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10872 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00275.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 327 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00275.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10562 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00278.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 392 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00278.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 84719 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00278_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8154 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00281.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 210 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00281.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 25126 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00281_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 14835 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00284.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 539 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00284.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 22880 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00284_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7138 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00290.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 226 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00290.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 21252 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00290_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7321 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00293.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 136 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00293.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 21198 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00293_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10872 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00296.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 457 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00296.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 61746 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00296_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7527 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00302.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 152 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00302.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 49706 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00302_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7319 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00305.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 164 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00305.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5665 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00308.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8178 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00311.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 265 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00311.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 35173 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00311_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7636 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00314.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 101 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00314.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 33928 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00314_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9756 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00317.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 352 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00317.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 26504 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00317_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6171 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00320.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6322 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00323.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7695 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00326.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 146 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00326.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 34553 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00326_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10132 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00329.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 389 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00329.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 71861 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00329_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 15430 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00332.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 661 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00332.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 23382 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00332_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6302 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00335.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10697 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00338.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 403 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00338.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 79141 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00338_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 4906 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00341.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6350 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00344.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6284 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00347.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7283 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00353.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 85 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00353.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6288 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00356.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7269 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00359.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 211 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00359.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 39700 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00359_source.html │ │ │ │ -rw-r--r-- 0 root (0) root (0) 6253 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00362.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8675 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00365.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 165 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00365.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7581 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00368.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 286 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00368.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 41607 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00368_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6302 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00371.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6284 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00374.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7269 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00377.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 211 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00377.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 39700 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00377_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 10562 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00380.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 392 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00380.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 84719 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00380_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7138 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00383.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 226 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00383.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 21252 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00383_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7319 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00386.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 164 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00386.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 42494 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00389_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6286 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00392.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 11287 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00395.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 454 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00395.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 10132 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00398.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 389 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00398.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 71861 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00398_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7695 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00401.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 146 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00401.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 34553 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00401_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7321 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00404.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 136 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00404.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 21198 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00404_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7206 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00407.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 189 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00407.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 42743 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00407_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6322 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00410.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6288 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00413.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5676 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00416.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8154 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00422.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 210 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00422.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 25126 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00422_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6350 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00425.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7047 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00365.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 144 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00365.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 25061 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00365_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5538 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00368.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7206 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00371.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 189 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00371.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 42743 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00371_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6968 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00374.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 148 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00374.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 32402 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00374_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6343 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00377.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6953 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00380.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 144 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00380.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 30607 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00380_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 11287 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00383.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 454 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00383.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5688 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00386.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 31329 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00386_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8675 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00389.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 165 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00389.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 51201 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00392_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 31104 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00395_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6883 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00398.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 136 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00398.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 29154 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00398_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6957 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00401.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 144 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00401.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 34458 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00401_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7741 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00404.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 174 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00404.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 103910 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00404_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 39426 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00407.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 2373 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00407.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 127405 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00407_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6858 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00410.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 132 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00410.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 19855 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00410_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6268 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00413.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6171 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00419.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7439 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00422.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 258 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00422.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 80368 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00422_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7581 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00425.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 286 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00425.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 41607 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00425_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5565 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00428.html │ │ │ │ -rw-r--r-- 0 root (0) root (0) 7340 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00431.html │ │ │ │ -rw-r--r-- 0 root (0) root (0) 265 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00431.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 32342 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00431_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5565 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00434.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 11986 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00437.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 475 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00437.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 39165 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00437_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 29089 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00440_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5538 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00443.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8178 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00446.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 281 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00446.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 68835 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00446_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 14835 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00449.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 539 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00449.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 22880 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00449_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6171 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00455.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8178 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00458.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 265 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00458.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 35173 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00458_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 39426 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00461.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 2373 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00461.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 127405 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00461_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 31104 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00464_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7741 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00467.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 174 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00467.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 103910 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00467_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5655 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00470.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5665 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00473.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 10872 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00476.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 327 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00476.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6343 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00479.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7797 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00482.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 165 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00482.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 36848 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00482_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 9159 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00485.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 180 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00485.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6506 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00488.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 13719 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00488_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6903 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00491.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 93 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00491.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8025 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00494.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 231 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00494.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 31331 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00494_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6908 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00497.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 134 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00497.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 58194 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00497_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5524 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00500.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6403 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00503.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 79 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00503.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 29636 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00503_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5531 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00506.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5521 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00509.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5713 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00512.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5533 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00515.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7268 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00518.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 77 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00518.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 18804 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00518_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5697 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00521.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7018 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00524.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 80 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00524.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 31373 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00524_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6690 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00527.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 73 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00527.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 57767 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00527_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5665 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00530.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5755 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00533.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6769 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00536.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 84 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00536.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 16945 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00536_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6226 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00539.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 74 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00539.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 15209 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00539_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6322 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00542.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 15766 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00542_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8586 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00548.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 270 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00548.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 41431 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00548_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6737 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00551.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 100 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00551.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6587 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00554.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 162 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00554.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 18018 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00554_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 9935 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00557.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 168 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00557.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 33611 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00557_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6582 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00560.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 83 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00560.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 42027 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00560_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6816 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00563.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 87 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00563.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 29314 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00563_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7749 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00566.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5709 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00569.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 13267 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00572.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 681 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00572.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 22145 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00572_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7505 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00575.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 221 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00575.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 65993 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00575_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 28303 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00578_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 15849 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00581.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 641 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00581.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 30059 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00581_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5529 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00584.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6167 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00587.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 101 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00587.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 42759 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00587_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7023 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00590.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 136 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00590.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 23512 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00590_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5734 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00593.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 46729 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00593_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6459 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00596.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 22692 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00596_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6465 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00599.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 70 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00599.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 18421 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00599_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7205 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00602.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 166 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00602.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 56256 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00602_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 90312 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00605_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 54067 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00608_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7717 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00611.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 262 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00611.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 88802 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00611_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5593 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00614.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 17977 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00614_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6648 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00620.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 229 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00620.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 34362 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00620_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5705 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00623.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 51666 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00623_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8617 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00626.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 96 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00626.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 19774 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00626_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5520 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00629.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6700 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00632.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 15755 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00632_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7807 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00635.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 240 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00635.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 56707 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00635_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5591 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00638.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7687 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00641.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 146 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00641.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 38682 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00641_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6436 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00644.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 22543 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00644_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5671 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00647.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 18304 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00647_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5523 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00650.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6665 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00653.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 178 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00653.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 36162 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00653_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5606 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00656.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 22139 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00656_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5639 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00659.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 66585 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00659_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6363 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00662.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 77 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00662.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 15140 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00662_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 10019 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00665.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 449 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00665.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 31790 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00665_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6687 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00668.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 126 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00668.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 49427 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00668_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 28749 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00671_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6476 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00674.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 88 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00674.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 11023 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00674_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6367 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00677.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6454 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00680.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 103 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00680.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 46554 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00680_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6879 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00683.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 136 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00683.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 38328 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00683_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5637 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00692.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6695 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00695.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 189 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00695.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 62349 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00695_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5666 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00698.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6945 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00701.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 113625 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00701_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7362 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00704.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 13056 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00704_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8332 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00707.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 192 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00707.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7759 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00710.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6141 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00713.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 83 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00713.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 15664 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00713_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5601 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00716.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8297 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00719.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 94 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00719.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7443 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00722.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 163 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00722.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 33932 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00722_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6842 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00725.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 86 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00725.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 40634 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00725_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 37272 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00728_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 69549 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00731_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6475 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00734.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5668 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00737.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 12032 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00737_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8124 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00740.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5557 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00743.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5643 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00746.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7999 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00749.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 276 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00749.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 21749 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00749_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8992 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00434.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 103 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00434.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 24792 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00434_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5656 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00437.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8178 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00443.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 281 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00443.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 68835 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00443_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 11986 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00446.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 475 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00446.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 39165 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00446_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7569 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00449.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 81 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00449.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 32907 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00449_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5676 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00452.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 29089 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00455_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6259 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00461.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7039 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00464.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 223 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00464.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 94047 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00464_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6379 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00467.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 42494 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00470_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6286 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00473.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5655 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00479.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9935 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00482.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 168 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00482.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 33611 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00482_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5713 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00485.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6582 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00488.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 83 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00488.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 42027 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00488_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5521 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00491.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6322 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00494.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 15766 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00494_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8025 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00497.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 231 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00497.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 31331 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00497_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6769 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00500.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 84 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00500.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 16945 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00500_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7749 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00503.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5709 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00506.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6226 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00512.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 74 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00512.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 15209 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00512_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6737 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00515.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 100 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00515.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6903 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00518.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 93 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00518.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5524 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00521.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5755 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00524.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9159 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00527.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 180 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00527.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5697 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00530.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6816 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00533.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 87 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00533.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 29314 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00533_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7797 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00536.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 165 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00536.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 36848 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00536_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6403 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00539.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 79 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00539.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 29636 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00539_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5665 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00542.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6506 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00545.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13719 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00545_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5533 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00548.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6690 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00551.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 73 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00551.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 57767 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00551_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8586 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00554.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 270 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00554.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 41431 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00554_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6587 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00557.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 162 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00557.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18018 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00557_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5531 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00560.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7268 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00563.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 77 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00563.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18804 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00563_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6908 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00566.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 134 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00566.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 58194 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00566_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7018 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00569.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 80 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00569.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 31373 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00569_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 28749 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00572_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7023 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00575.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 136 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00575.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 23512 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00575_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10019 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00578.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 449 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00578.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 31790 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00578_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5705 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00581.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 51666 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00581_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6465 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00584.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 70 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00584.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18421 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00584_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7687 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00587.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 146 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00587.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 38682 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00587_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5639 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00590.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 66585 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00590_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5520 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00593.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6687 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00596.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 126 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00596.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 49427 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00596_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5734 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00599.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 46729 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00599_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6454 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00602.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 103 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00602.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 46554 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00602_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6167 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00605.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 101 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00605.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 42759 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00605_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 28303 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00608_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 15849 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00611.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 641 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00611.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 30059 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00611_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 54067 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00614_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6459 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00617.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 22692 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00617_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5529 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00620.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6879 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00623.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 136 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00623.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 38328 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00623_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5671 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00626.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18304 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00626_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5523 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00629.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5637 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00632.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13267 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00635.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 681 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00635.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 22145 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00635_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5666 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00638.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6945 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00644.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 113625 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00644_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6648 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00650.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 229 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00650.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 34362 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00650_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6363 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00653.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 77 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00653.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 15140 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00653_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7717 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00656.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 262 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00656.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 88802 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00656_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6367 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00662.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6476 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00665.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 88 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00665.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 11023 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00665_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8617 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00668.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 96 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00668.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 19774 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00668_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6665 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00671.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 178 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00671.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 36162 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00671_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7807 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00674.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 240 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00674.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 56707 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00674_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7205 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00677.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 166 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00677.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 56256 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00677_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7505 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00680.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 221 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00680.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 65993 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00680_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5606 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00683.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 22139 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00683_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6436 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00686.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 22543 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00686_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5593 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00689.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 17977 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00689_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6700 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00692.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 15755 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00692_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 90312 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00695_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5591 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00698.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6695 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00701.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 189 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00701.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 62349 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00701_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5691 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00704.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 37272 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00707_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5725 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00710.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6419 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00713.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6428 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00716.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 83 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00716.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 25237 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00716_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7661 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00719.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 188 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00719.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 38060 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00719_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6717 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00722.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 138 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00722.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 22245 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00722_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 27298 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00725_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7445 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00728.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 87 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00728.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 35988 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00728_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7995 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00731.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 216 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00731.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 73746 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00731_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 69549 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00737_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5578 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00740.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5609 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00743.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6532 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00746.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 134 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00746.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 12972 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00746_source.html │ │ │ │ -rw-r--r-- 0 root (0) root (0) 8957 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00752.html │ │ │ │ -rw-r--r-- 0 root (0) root (0) 312 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00752.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 60246 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00752_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 15960 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00755.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 891 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00755.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 125878 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00755_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6428 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00758.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 83 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00758.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 25237 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00758_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7995 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00761.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 216 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00761.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 73746 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00761_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7323 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00764.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 116 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00764.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 16423 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00764_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8668 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00767.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 180 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00767.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 15601 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00767_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7433 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00773.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 207 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00773.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 22427 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00773_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6419 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00779.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6581 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00785.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 17930 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00785_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7058 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00788.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 138 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00788.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 39533 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00788_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 12146 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00791.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 536 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00791.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6717 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00794.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 138 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00794.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 22245 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00794_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7940 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00797.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 245 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00797.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 32692 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00797_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5578 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00800.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7210 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00803.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 176 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00803.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 28405 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00803_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5609 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00806.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5725 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00809.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7195 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00812.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 94 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00812.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 29635 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00812_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7445 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00815.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 87 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00815.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 35988 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00815_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5691 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00818.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7661 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00821.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 188 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00821.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 38060 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00821_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 27298 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00824_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5562 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00827.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7225 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00830.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 78 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00830.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 69400 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00830_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6901 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00833.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 85 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00833.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 46918 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00833_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7316 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00836.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 97 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00836.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5627 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00839.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8871 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00842.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 415 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00842.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 38721 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00842_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6978 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00848.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 82 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00848.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 35496 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00848_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 10979 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00851.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 453 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00851.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5520 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00854.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 13381 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00857.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 665 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00857.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 19609 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00857_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6366 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00860.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 67 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00860.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 13881 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00860_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7128 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00863.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 72 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00863.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 26183 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00863_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6532 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00866.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 134 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00866.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 12972 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00866_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5648 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00869.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5770 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00872.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 13639 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00872_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 9091 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00875.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 185 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00875.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6387 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00878.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 13406 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00878_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 13120 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00881.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 670 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00881.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 28259 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00881_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5571 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00887.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 28869 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00890_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6562 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00893.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 160 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00893.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 56968 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00893_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6663 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00896.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 154 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00896.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 29640 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00896_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8188 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00899.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 313 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00899.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 73410 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00899_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6448 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00905.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 80 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00905.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 23692 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00905_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7763 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00908.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 223 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00908.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 38413 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00908_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6864 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00911.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5660 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00914.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5559 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00917.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6439 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00920.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 140 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00920.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 33448 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00920_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 11831 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00923.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6611 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00926.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 82 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00926.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 18128 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00926_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7321 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00932.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 197 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00932.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 33342 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00932_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5612 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00935.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 9760 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00938.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 316 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00938.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 59601 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00938_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6683 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00941.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 143 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00941.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 41393 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00941_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6921 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00944.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 152 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00944.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 37965 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00944_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 32900 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00947_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6963 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00956.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 93 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00956.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7639 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00959.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 202 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00959.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 39261 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00959_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 9996 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00965.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5609 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00968.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5635 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00971.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8571 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00974.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 153 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00974.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 49274 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00974_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6414 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00977.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 81 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00977.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 25637 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00977_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8465 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00980.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 231 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00980.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 47156 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00980_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7069 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00983.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 78 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00983.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 34621 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00983_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8826 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00986.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 213 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00986.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 55825 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00986_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5669 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00989.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6359 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00992.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 82 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00992.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 24129 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00992_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7029 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00995.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 35940 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00995_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6390 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00998.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 24874 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00998_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5757 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01001.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 10303 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01004.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 693 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01004.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 99817 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01004_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 10777 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01010.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 201 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01010.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 26364 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01010_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6497 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01013.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 89 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01013.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 21256 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01013_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 74370 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01016_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8403 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01019.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 161 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01019.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 23145 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01019_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5672 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01022.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8813 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01025.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 156 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01025.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 60117 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01025_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7509 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01031.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 72 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01031.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 23599 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01031_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6201 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01034.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8182 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01040.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 154 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01040.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 32933 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01040_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6674 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01043.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5791 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01046.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7515 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01049.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 99 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01049.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5575 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01052.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 15640 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01055.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 919 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01055.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 61984 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01055_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5544 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01058.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6662 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01061.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 16604 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01061_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6511 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01064.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 78 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01064.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 17610 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01064_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6802 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01067.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 137 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01067.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 17991 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01067_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7312 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01070.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 126 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01070.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 37468 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01070_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5676 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01073.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6735 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01076.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 131 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01076.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 24143 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01076_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5765 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01079.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5679 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01082.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6590 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01085.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 87 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01085.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 24596 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01085_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7258 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01088.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 153 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01088.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 32517 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01088_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5816 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01091.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6681 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01094.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 150 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01094.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 49490 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01094_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 83353 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01097_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6646 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01100.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 83 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01100.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 13167 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01100_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7262 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01103.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 99 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01103.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8787 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01106.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 314 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01106.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 68014 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01106_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 75095 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01109_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 43820 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01112_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8050 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01115.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 176 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01115.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 37487 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01115_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6403 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01118.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 73 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01118.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 25127 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01118_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6521 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01121.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 84 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01121.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 37759 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01121_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6766 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01124.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 155 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01124.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 35912 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01124_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7249 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01127.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 84 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01127.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 36222 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01127_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5509 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01130.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5542 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01133.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5598 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01136.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7950 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01139.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 364 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01139.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 103846 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01139_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7005 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01142.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 71 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01142.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 54857 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01142_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5537 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01145.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6521 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01148.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 81 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01148.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 13022 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01148_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5641 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01151.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 10047 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01154.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 118 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01154.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 13067 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01154_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 10339 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01157.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 493 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01157.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 50078 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01157_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8791 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01160.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 265 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01160.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 44674 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01160_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5778 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01163.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 30718 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01163_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6719 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01166.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 89 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01166.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 94502 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01166_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 11271 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01169.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 275 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01169.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 88722 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01169_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6866 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01172.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 152 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01172.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 28417 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01172_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7767 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01184.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6298 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01187.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 79 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01187.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 27993 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01187_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6465 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01190.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 18491 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01190_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 12714 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01193.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 674 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01193.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7996 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01196.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 77 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01196.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 18886 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01196_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7490 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01199.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 14520 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01199_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 9089 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01202.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 337 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01202.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 37621 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01202_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5666 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01205.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 9294 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01208.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 108 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01208.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6896 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01211.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 82 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01211.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 22430 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01211_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5657 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01214.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 14404 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01217.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 807 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01217.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 36213 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01217_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 10834 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01220.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 168 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01220.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 15064 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01220_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8734 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01223.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 266 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01223.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 75097 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01223_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7661 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01226.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6626 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01229.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 77 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01229.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 21272 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01229_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7064 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01235.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 100 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01235.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 18044 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01238_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6829 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01241.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 182 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01241.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 123331 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01241_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 30917 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01247.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 877 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01247.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8061 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01250.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 1057 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01250.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 31679 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01250_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7254 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01253.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 100 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01253.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 80135 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01253_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6186 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01256.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 27374 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01256_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5575 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01259.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7033 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01262.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 172 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01262.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 118926 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01262_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 19703 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01265_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6450 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01268.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 85 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01268.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 99463 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01268_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 77347 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01271_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8386 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01274.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 234 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01274.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 42102 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01274_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 34893 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01277_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7589 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01280.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 260 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01280.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 70927 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01280_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6954 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01283.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 120 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01283.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 56560 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01283_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 32324 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01286_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 17845 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01289.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 13824 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01289_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7578 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01292.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 71 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01292.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 18772 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01292_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 9523 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01295.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 304 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01295.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 35653 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01295_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 22375 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01298_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 27308 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01301.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 2405 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01301.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 44752 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01301_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 19122 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01307_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6847 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01310.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 95 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01310.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 47234 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01310_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6936 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01313.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 106 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01313.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 36516 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01313_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 19138 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01316_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7173 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01319.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 97 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01319.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 37474 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01319_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 53045 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01322_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6262 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01325.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 85 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01325.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 28637 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01325_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5751 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01328.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 20551 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01328_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5540 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01331.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6459 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01334.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 88 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01334.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 28186 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01334_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6595 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01337.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 24742 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01337_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 22253 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01340_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8301 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01343.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 100 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01343.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 18569 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01343_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6102 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01346.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 72 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01346.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 11368 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01346_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5566 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01349.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7104 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01352.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 148 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01352.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 37514 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01352_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7426 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01355.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 98 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01355.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 13828 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01355_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5554 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01358.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6590 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01361.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 156 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01361.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 17141 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01361_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6870 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01364.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 82 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01364.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 28867 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01364_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5575 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01367.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5536 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01370.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5537 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01373.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7075 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01379.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 232 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01379.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 45377 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01379_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 6648 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01382.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 100 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01382.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 8056 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01385.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 222 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01385.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 42918 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01385_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 5543 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01388.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7720 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01391.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 280 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01391.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 19653 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01391_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 4958 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01397.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 4933 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01400.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 7579 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01400_source.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 4950 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01403.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 18761 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01403_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5648 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00755.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5562 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00758.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8332 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00761.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 192 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00761.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6366 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00764.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 67 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00764.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13881 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00764_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5601 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00767.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8188 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00770.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 313 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00770.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 73410 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00770_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6901 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00773.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 85 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00773.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 46918 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00773_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6978 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00776.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 82 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00776.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 35496 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00776_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7940 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00782.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 245 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00782.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 32692 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00782_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7433 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00785.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 207 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00785.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 22427 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00785_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6387 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00788.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13406 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00788_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7759 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00791.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6663 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00794.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 154 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00794.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 29640 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00794_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13120 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00797.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 670 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00797.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 28259 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00797_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6475 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00800.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8668 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00803.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 180 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00803.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 15601 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00803_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13381 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00806.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 665 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00806.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 19609 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00806_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7316 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00809.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 97 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00809.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6581 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00812.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 17930 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00812_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7058 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00815.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 138 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00815.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 39533 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00815_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5520 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00818.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5627 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00821.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 15960 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00824.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 891 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00824.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 125878 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00824_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5643 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00827.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 28869 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00833_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6141 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00836.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 83 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00836.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 15664 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00836_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6842 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00839.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 86 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00839.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 40634 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00839_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5571 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00842.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7210 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00845.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 176 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00845.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 28405 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00845_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5770 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00848.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13639 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00848_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8297 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00851.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 94 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00851.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7362 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00854.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13056 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00854_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8871 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00857.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 415 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00857.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 38721 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00857_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7323 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00860.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 116 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00860.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 16423 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00860_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5557 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00866.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5668 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00869.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 12032 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00869_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6562 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00872.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 160 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00872.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 56968 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00872_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8124 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00875.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7443 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00878.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 163 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00878.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 33932 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00878_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7128 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00881.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 72 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00881.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 26183 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00881_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7225 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00884.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 78 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00884.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 69400 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00884_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7999 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00887.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 276 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00887.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 21749 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00887_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 12146 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00890.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 536 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00890.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9091 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00893.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 185 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00893.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10979 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00896.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 453 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00896.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7195 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00899.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 94 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00899.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 29635 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00899_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5635 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00902.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7763 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00905.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 223 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00905.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 38413 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00905_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7639 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00908.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 202 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00908.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 39261 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00908_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6390 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00911.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 24874 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00911_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6683 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00914.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 143 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00914.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 41393 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00914_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8571 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00923.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 153 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00923.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 49274 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00923_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6963 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00926.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 93 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00926.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6448 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00929.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 80 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00929.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 23692 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00929_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6414 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00932.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 81 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00932.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 25637 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00932_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6439 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00935.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 140 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00935.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 33448 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00935_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8826 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00938.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 213 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00938.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 55825 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00938_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 11831 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00941.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5609 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00944.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7321 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00947.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 197 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00947.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 33342 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00947_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5660 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00950.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5559 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00953.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6359 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00959.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 82 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00959.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 24129 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00959_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9996 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00962.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6864 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00968.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6611 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00971.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 82 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00971.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18128 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00971_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5669 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00974.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 32900 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00977_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9760 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00980.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 316 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00980.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 59601 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00980_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7029 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00986.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 35940 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00986_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5612 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00989.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7069 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00992.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 78 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00992.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 34621 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00992_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8465 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00995.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 231 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00995.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 47156 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00995_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6921 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00998.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 152 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00998.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 37965 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a00998_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5544 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01001.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6646 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01004.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 83 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01004.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13167 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01004_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7509 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01010.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 72 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01010.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 23599 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01010_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5537 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01013.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10777 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01016.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 201 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01016.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 26364 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01016_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5509 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01019.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6866 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01022.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 152 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01022.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 28417 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01022_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6735 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01025.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 131 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01025.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 24143 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01025_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10047 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01028.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 118 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01028.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13067 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01028_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5676 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01031.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 15640 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01034.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 919 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01034.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 61984 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01034_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6521 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01037.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 84 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01037.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 37759 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01037_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5598 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01040.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8403 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01043.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 161 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01043.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 23145 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01043_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6766 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01046.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 155 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01046.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 35912 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01046_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8050 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01049.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 176 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01049.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 37487 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01049_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7249 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01055.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 84 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01055.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 36222 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01055_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 43820 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01058_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8813 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01061.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 156 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01061.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 60117 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01061_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6802 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01064.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 137 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01064.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 17991 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01064_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6662 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01067.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 16604 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01067_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5765 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01070.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10303 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01073.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 693 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01073.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 99817 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01073_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5679 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01076.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5641 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01079.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10339 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01082.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 493 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01082.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 50078 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01082_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7312 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01085.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 126 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01085.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 37468 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01085_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5757 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01088.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8791 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01091.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 265 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01091.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 44674 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01091_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5672 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01094.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6674 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01097.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6403 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01100.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 73 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01100.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 25127 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01100_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7950 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01103.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 364 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01103.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 103846 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01103_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6521 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01106.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 81 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01106.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13022 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01106_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5575 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01109.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5816 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01112.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 75095 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01115_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6201 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01118.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8182 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01121.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 154 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01121.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 32933 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01121_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7515 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01124.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 99 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01124.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6719 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01130.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 89 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01130.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 94502 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01130_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 11271 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01133.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 275 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01133.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 88722 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01133_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 83353 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01136_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8787 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01139.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 314 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01139.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 68014 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01139_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5542 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01142.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6681 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01145.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 150 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01145.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 49490 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01145_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7005 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01148.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 71 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01148.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 54857 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01148_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6590 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01151.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 87 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01151.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 24596 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01151_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6511 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01154.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 78 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01154.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 17610 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01154_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7262 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01157.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 99 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01157.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5791 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01160.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6497 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01163.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 89 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01163.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 21256 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01163_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7258 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01166.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 153 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01166.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 32517 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01166_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5778 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01169.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 30718 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01169_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 74370 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01172_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6626 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01184.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 77 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01184.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 21272 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01184_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7767 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01187.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 10834 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01190.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 168 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01190.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 15064 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01190_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6298 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01193.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 79 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01193.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 27993 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01193_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7490 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01196.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 14520 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01196_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7661 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01199.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6465 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01205.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18491 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01205_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6896 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01208.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 82 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01208.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 22430 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01208_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9294 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01211.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 108 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01211.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9089 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01214.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 337 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01214.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 37621 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01214_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5657 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01217.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7996 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01220.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 77 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01220.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18886 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01220_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 12714 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01223.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 674 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01223.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 14404 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01226.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 807 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01226.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 36213 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01226_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8734 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01229.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 266 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01229.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 75097 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01229_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5666 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01232.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6459 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01235.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 88 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01235.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 28186 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01235_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5540 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01238.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7033 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01241.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 172 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01241.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 118926 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01241_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 77347 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01244_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6262 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01247.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 85 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01247.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 28637 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01247_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 22375 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01250_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6186 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01253.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 27374 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01253_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7064 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01259.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 100 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01259.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7589 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01262.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 260 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01262.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 70927 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01262_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6450 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01265.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 85 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01265.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 99463 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01265_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5751 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01268.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 20551 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01268_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7578 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01271.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 71 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01271.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18772 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01271_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6595 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01274.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 24742 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01274_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 30917 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01277.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 877 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01277.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7173 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01280.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 97 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01280.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 37474 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01280_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8386 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01283.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 234 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01283.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 42102 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01283_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 53045 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01286_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 34893 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01289_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 17845 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01292.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13824 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01292_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8061 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01295.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 1057 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01295.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 31679 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01295_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 22253 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01298_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9523 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01301.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 304 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01301.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 35653 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01301_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6954 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01304.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 120 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01304.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 56560 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01304_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 19703 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01307_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6829 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01310.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 182 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01310.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 123331 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01310_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6847 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01313.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 95 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01313.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 47234 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01313_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5575 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01316.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 27308 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01319.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 2405 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01319.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 44752 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01319_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6936 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01322.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 106 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01322.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 36516 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01322_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 19122 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01325_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7254 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01328.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 100 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01328.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 80135 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01328_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18044 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01331_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 32324 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01334_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 19138 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01337_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5536 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01343.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7104 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01346.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 148 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01346.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 37514 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01346_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6590 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01349.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 156 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01349.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 17141 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01349_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5575 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01352.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7720 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01355.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 280 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01355.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 19653 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01355_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6648 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01358.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 100 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01358.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5537 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01361.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8301 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01364.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 100 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01364.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18569 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01364_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7075 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01367.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 232 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01367.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 45377 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01367_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6870 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01370.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 82 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01370.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 28867 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01370_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5566 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01373.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5543 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01379.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 5554 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01382.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 6102 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01385.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 72 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01385.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 11368 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01385_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 8056 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01388.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 222 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01388.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 42918 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01388_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7426 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01391.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 98 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01391.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13828 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01391_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 4933 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01397.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 7579 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01397_source.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 4958 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01400.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 4950 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01406.html │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 18761 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01406_source.html │ │ │ │ -rw-r--r-- 0 root (0) root (0) 9763 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01409.html │ │ │ │ -rw-r--r-- 0 root (0) root (0) 15228 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01410.html │ │ │ │ -rw-r--r-- 0 root (0) root (0) 8481 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01411.html │ │ │ │ -rw-r--r-- 0 root (0) root (0) 4316 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01412.html │ │ │ │ -rw-r--r-- 0 root (0) root (0) 14442 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01413.html │ │ │ │ -rw-r--r-- 0 root (0) root (0) 9641 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01414.html │ │ │ │ -rw-r--r-- 0 root (0) root (0) 20286 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/a01415.html │ │ │ │ @@ -3427,15 +3427,15 @@ │ │ │ │ -rw-r--r-- 0 root (0) root (0) 10749 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/functions_w.html │ │ │ │ -rw-r--r-- 0 root (0) root (0) 5058 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/functions_x.html │ │ │ │ -rw-r--r-- 0 root (0) root (0) 4940 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/functions_y.html │ │ │ │ -rw-r--r-- 0 root (0) root (0) 5144 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/functions_z.html │ │ │ │ -rw-r--r-- 0 root (0) root (0) 12860 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/functions_~.html │ │ │ │ -rw-r--r-- 0 root (0) root (0) 5530 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/globals.html │ │ │ │ -rw-r--r-- 0 root (0) root (0) 5529 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/globals_defs.html │ │ │ │ --rw-r--r-- 0 root (0) root (0) 204495 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/gtsam.tag.gz │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 204462 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/gtsam.tag.gz │ │ │ │ -rw-r--r-- 0 root (0) root (0) 428173 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/hierarchy.html │ │ │ │ -rw-r--r-- 0 root (0) root (0) 80762 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/hierarchy.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 4336 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/index.html │ │ │ │ -rw-r--r-- 0 root (0) root (0) 176630 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/jquery.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 5925 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/menu.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 9869 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/menudata.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 582 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/minus.svg │ │ │ │ @@ -3497,39 +3497,39 @@ │ │ │ │ -rw-r--r-- 0 root (0) root (0) 153 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/nav_f.png │ │ │ │ -rw-r--r-- 0 root (0) root (0) 169 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/nav_fd.png │ │ │ │ -rw-r--r-- 0 root (0) root (0) 95 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/nav_g.png │ │ │ │ -rw-r--r-- 0 root (0) root (0) 98 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/nav_h.png │ │ │ │ -rw-r--r-- 0 root (0) root (0) 114 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/nav_hd.png │ │ │ │ -rw-r--r-- 0 root (0) root (0) 2167 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtree.css │ │ │ │ -rw-r--r-- 0 root (0) root (0) 15935 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtree.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 4369 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreedata.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 13675 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex0.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 10951 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex1.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 4301 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreedata.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13926 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex0.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 11005 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex1.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 15173 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex10.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 14907 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex11.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 14415 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex12.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 14357 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex13.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 15133 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex14.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 15217 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex15.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 14811 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex16.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 13820 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex17.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 14082 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex18.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 14793 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex19.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 9522 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex2.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9327 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex2.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 14797 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex20.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 15745 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex21.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 14997 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex22.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 14791 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex23.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 14412 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex24.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 14245 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex25.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 14589 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex26.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 11449 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex27.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 2879 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex28.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 9864 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex3.js │ │ │ │ --rw-r--r-- 0 root (0) root (0) 13580 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex4.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 9725 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex3.js │ │ │ │ +-rw-r--r-- 0 root (0) root (0) 13609 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex4.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 15499 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex5.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 11980 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex6.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 12384 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex7.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 11740 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex8.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 13164 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/navtreeindex9.js │ │ │ │ -rw-r--r-- 0 root (0) root (0) 123 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/open.png │ │ │ │ -rw-r--r-- 0 root (0) root (0) 4591 2024-01-09 05:51:35.000000 ./usr/share/doc/libgtsam-dev/html/pages.html │ │ │ ├── ./usr/share/doc/libgtsam-dev/ImuFactor.pdf │ │ │ │ ├── pdftotext {} - │ │ │ │ │ @@ -1,10 +1,10 @@ │ │ │ │ │ The New IMU Factor │ │ │ │ │ Frank Dellaert & Varun Agrawal │ │ │ │ │ -December 9, 2025 │ │ │ │ │ +November 7, 2024 │ │ │ │ │ │ │ │ │ │ 1 │ │ │ │ │ │ │ │ │ │ IMU Factor │ │ │ │ │ The IMU factor has 2 variants: │ │ │ │ │ 1. ImuFactor is a 5-way factor between the previous pose and velocity, the current pose │ │ │ │ │ and velocity, and the current IMU bias. │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00002.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/VectorSerialization.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/cholesky.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,51 +94,48 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Namespaces | │ │ │ │ Functions
│ │ │ │ -
VectorSerialization.h File Reference
│ │ │ │ +
cholesky.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

serialization for Vectors │ │ │ │ +

Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained Cholesky. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

│ │ │ │ -template<class Archive >
void boost::serialization::save (Archive &ar, const gtsam::Vector &v, unsigned int)
 
│ │ │ │ -template<class Archive >
void boost::serialization::load (Archive &ar, gtsam::Vector &v, unsigned int)
 
│ │ │ │ -template<class Archive , int D>
void boost::serialization::save (Archive &ar, const Eigen::Matrix< double, D, 1 > &v, unsigned int)
 
│ │ │ │ -template<class Archive , int D>
void boost::serialization::load (Archive &ar, Eigen::Matrix< double, D, 1 > &v, unsigned int)
 
pair< size_t, bool > gtsam::choleskyCareful (Matrix &ATA, int order=-1)
 "Careful" Cholesky computes the positive square-root of a positive symmetric semi-definite matrix (i.e.
 
bool gtsam::choleskyPartial (Matrix &ABC, size_t nFrontal, size_t topleft=0)
 Partial Cholesky computes a factor [R S such that [R' 0 [R S = [A B 0 L] S' I] 0 L] B' C].
 
│ │ │ │

Detailed Description

│ │ │ │ -

serialization for Vectors

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
Date
February 2022
│ │ │ │ +

Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained Cholesky.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +
Date
Nov 5, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,35 +1,35 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_F_u_n_c_t_i_o_n_s │ │ │ │ │ -VectorSerialization.h File Reference │ │ │ │ │ -serialization for Vectors _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ -void  bboooosstt::::sseerriiaalliizzaattiioonn::::ssaavvee (Archive &ar, const gtsam::Vector &v, unsigned │ │ │ │ │ - int) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -void  bboooosstt::::sseerriiaalliizzaattiioonn::::llooaadd (Archive &ar, gtsam::Vector &v, unsigned int) │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +cholesky.cpp File Reference │ │ │ │ │ +Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained │ │ │ │ │ +Cholesky. _M_o_r_e_._._. │ │ │ │ │ +NNaammeessppaacceess │ │ │ │ │ +namespace   _g_t_s_a_m │ │ │ │ │ +  Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -template │ │ │ │ │ -void  bboooosstt::::sseerriiaalliizzaattiioonn::::ssaavvee (Archive &ar, const Eigen::Matrix< double, D, │ │ │ │ │ - 1 > &v, unsigned int) │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +pair< size_t, bool >  _g_t_s_a_m_:_:_c_h_o_l_e_s_k_y_C_a_r_e_f_u_l (Matrix &ATA, int order=-1) │ │ │ │ │ +  "Careful" Cholesky computes the positive square-root of a │ │ │ │ │ + positive symmetric semi-definite matrix (i.e. │ │ │ │ │   │ │ │ │ │ -template │ │ │ │ │ -void  bboooosstt::::sseerriiaalliizzaattiioonn::::llooaadd (Archive &ar, Eigen::Matrix< double, D, 1 > │ │ │ │ │ - &v, unsigned int) │ │ │ │ │ + bool  _g_t_s_a_m_:_:_c_h_o_l_e_s_k_y_P_a_r_t_i_a_l (Matrix &ABC, size_t nFrontal, │ │ │ │ │ + size_t topleft=0) │ │ │ │ │ +  Partial Cholesky computes a factor [R S such that [R' 0 │ │ │ │ │ + [R S = [A B 0 L] S' I] 0 L] B' C]. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -serialization for Vectors │ │ │ │ │ +Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained │ │ │ │ │ +Cholesky. │ │ │ │ │ Author │ │ │ │ │ + Richard Roberts │ │ │ │ │ Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - February 2022 │ │ │ │ │ + Nov 5, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _V_e_c_t_o_r_S_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ + * _c_h_o_l_e_s_k_y_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00008.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/cholesky.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/FastDefaultAllocator.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,48 +94,49 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces | │ │ │ │ -Functions
│ │ │ │ -
cholesky.h File Reference
│ │ │ │ +Macros
│ │ │ │ +
FastDefaultAllocator.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained Cholesky. │ │ │ │ +

An easy way to control which allocator is used for Fast* collections. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

struct  gtsam::internal::FastDefaultAllocator< T >
 Default allocator for list, map, and set types. More...
 
struct  gtsam::internal::FastDefaultVectorAllocator< T >
 Default allocator for vector types (we never use boost pool for vectors) More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

pair< size_t, bool > gtsam::choleskyCareful (Matrix &ATA, int order=-1)
 "Careful" Cholesky computes the positive square-root of a positive symmetric semi-definite matrix (i.e.
 
bool gtsam::choleskyPartial (Matrix &ABC, size_t nFrontal, size_t topleft=0)
 Partial Cholesky computes a factor [R S such that [R' 0 [R S = [A B 0 L] S' I] 0 L] B' C].
 
│ │ │ │

Detailed Description

│ │ │ │ -

Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained Cholesky.

│ │ │ │ +

An easy way to control which allocator is used for Fast* collections.

│ │ │ │
Author
Richard Roberts
│ │ │ │ -
Date
Nov 5, 2010
│ │ │ │ +
Date
Aug 15, 2013
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,35 +1,31 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -cholesky.h File Reference │ │ │ │ │ -Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained │ │ │ │ │ -Cholesky. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s │ │ │ │ │ +FastDefaultAllocator.h File Reference │ │ │ │ │ +An easy way to control which allocator is used for Fast* collections. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +struct   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_<_ _T_ _> │ │ │ │ │ +  Default allocator for list, map, and set types. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_F_a_s_t_D_e_f_a_u_l_t_V_e_c_t_o_r_A_l_l_o_c_a_t_o_r_<_ _T_ _> │ │ │ │ │ +  Default allocator for vector types (we never use boost pool for │ │ │ │ │ + vectors) _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -pair< size_t, bool >  _g_t_s_a_m_:_:_c_h_o_l_e_s_k_y_C_a_r_e_f_u_l (Matrix &ATA, int order=-1) │ │ │ │ │ -  "Careful" Cholesky computes the positive square-root of a │ │ │ │ │ - positive symmetric semi-definite matrix (i.e. │ │ │ │ │ -  │ │ │ │ │ - bool  _g_t_s_a_m_:_:_c_h_o_l_e_s_k_y_P_a_r_t_i_a_l (Matrix &ABC, size_t nFrontal, │ │ │ │ │ - size_t topleft=0) │ │ │ │ │ -  Partial Cholesky computes a factor [R S such that [R' 0 │ │ │ │ │ - [R S = [A B 0 L] S' I] 0 L] B' C]. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained │ │ │ │ │ -Cholesky. │ │ │ │ │ +An easy way to control which allocator is used for Fast* collections. │ │ │ │ │ Author │ │ │ │ │ Richard Roberts │ │ │ │ │ Date │ │ │ │ │ - Nov 5, 2010 │ │ │ │ │ + Aug 15, 2013 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _c_h_o_l_e_s_k_y_._h │ │ │ │ │ + * _F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00008.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a00008 = [ │ │ │ │ │ - ["choleskyCareful", "a00008.html#a747683f736c50bca16b3aab0e95b1b76", null], │ │ │ │ │ - ["choleskyPartial", "a00008.html#abe82fa6aceccfa2360cf314ab44f39ad", null] │ │ │ │ │ + ["gtsam::internal::FastDefaultAllocator< T >", "a02304.html", null], │ │ │ │ │ + ["gtsam::internal::FastDefaultVectorAllocator< T >", "a02308.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00008_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/cholesky.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/FastDefaultAllocator.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,48 +98,106 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
cholesky.h
│ │ │ │ +
FastDefaultAllocator.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/base/Matrix.h>
│ │ │ │ +
19#pragma once
│ │ │ │ +
20#include <gtsam/config.h> // Configuration from CMake
│ │ │ │
21
│ │ │ │ -
22namespace gtsam {
│ │ │ │ -
23
│ │ │ │ -
47GTSAM_EXPORT std::pair<size_t,bool> choleskyCareful(Matrix& ATA, int order = -1);
│ │ │ │ -
48
│ │ │ │ -
62GTSAM_EXPORT bool choleskyPartial(Matrix& ABC, size_t nFrontal, size_t topleft=0);
│ │ │ │ -
63
│ │ │ │ -
64}
│ │ │ │ -
65
│ │ │ │ -
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
22#if !defined GTSAM_ALLOCATOR_BOOSTPOOL && !defined GTSAM_ALLOCATOR_TBB && !defined GTSAM_ALLOCATOR_STL
│ │ │ │ +
23# ifdef GTSAM_USE_TBB
│ │ │ │ +
24// Use TBB allocator by default if we have TBB, otherwise boost pool
│ │ │ │ +
25# define GTSAM_ALLOCATOR_TBB
│ │ │ │ +
26# else
│ │ │ │ +
27# define GTSAM_ALLOCATOR_BOOSTPOOL
│ │ │ │ +
28# endif
│ │ │ │ +
29#endif
│ │ │ │ +
30
│ │ │ │ +
31#if defined GTSAM_ALLOCATOR_BOOSTPOOL
│ │ │ │ +
32# include <boost/pool/pool_alloc.hpp>
│ │ │ │ +
33#elif defined GTSAM_ALLOCATOR_TBB
│ │ │ │ +
34# include <tbb/tbb_allocator.h>
│ │ │ │ +
35# undef min // TBB seems to include Windows.h which defines these macros that cause problems
│ │ │ │ +
36# undef max
│ │ │ │ +
37# undef ERROR
│ │ │ │ +
38#elif defined GTSAM_ALLOCATOR_STL
│ │ │ │ +
39# include <memory>
│ │ │ │ +
40#endif
│ │ │ │ +
41
│ │ │ │ +
42namespace gtsam
│ │ │ │ +
43{
│ │ │ │ +
44
│ │ │ │ +
45 namespace internal
│ │ │ │ +
46 {
│ │ │ │ +
48 template<typename T>
│ │ │ │ +
│ │ │ │ +
49 struct FastDefaultAllocator
│ │ │ │ +
50 {
│ │ │ │ +
51#if defined GTSAM_ALLOCATOR_BOOSTPOOL
│ │ │ │ +
52 typedef boost::fast_pool_allocator<T> type;
│ │ │ │ +
53 static const bool isBoost = true;
│ │ │ │ +
54 static const bool isTBB = false;
│ │ │ │ +
55 static const bool isSTL = false;
│ │ │ │ +
56#elif defined GTSAM_ALLOCATOR_TBB
│ │ │ │ +
57 typedef tbb::tbb_allocator<T> type;
│ │ │ │ +
58 static const bool isBoost = false;
│ │ │ │ +
59 static const bool isTBB = true;
│ │ │ │ +
60 static const bool isSTL = false;
│ │ │ │ +
61#elif defined GTSAM_ALLOCATOR_STL
│ │ │ │ +
62 typedef std::allocator<T> type;
│ │ │ │ +
63 static const bool isBoost = false;
│ │ │ │ +
64 static const bool isTBB = false;
│ │ │ │ +
65 static const bool isSTL = true;
│ │ │ │ +
66#endif
│ │ │ │ +
67 };
│ │ │ │ +
│ │ │ │ +
68
│ │ │ │ +
70 template<typename T>
│ │ │ │ +
│ │ │ │ +
71 struct FastDefaultVectorAllocator
│ │ │ │ +
72 {
│ │ │ │ +
73#if defined GTSAM_ALLOCATOR_TBB
│ │ │ │ +
74 typedef tbb::tbb_allocator<T> type;
│ │ │ │ +
75 static const bool isBoost = false;
│ │ │ │ +
76 static const bool isTBB = true;
│ │ │ │ +
77 static const bool isSTL = false;
│ │ │ │ +
78#else
│ │ │ │ +
79 typedef std::allocator<T> type;
│ │ │ │ +
80 static const bool isBoost = false;
│ │ │ │ +
81 static const bool isTBB = false;
│ │ │ │ +
82 static const bool isSTL = true;
│ │ │ │ +
83#endif
│ │ │ │ +
84 };
│ │ │ │ +
│ │ │ │ +
85 }
│ │ │ │ +
86
│ │ │ │ +
87}
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::choleskyCareful
pair< size_t, bool > choleskyCareful(Matrix &ATA, int order)
"Careful" Cholesky computes the positive square-root of a positive symmetric semi-definite matrix (i....
Definition cholesky.cpp:76
│ │ │ │ -
gtsam::choleskyPartial
bool choleskyPartial(Matrix &ABC, size_t nFrontal, size_t topleft)
Partial Cholesky computes a factor [R S such that [R' 0 [R S = [A B 0 L] S' I] 0 L] B' C].
Definition cholesky.cpp:108
│ │ │ │ +
gtsam::internal::FastDefaultAllocator
Default allocator for list, map, and set types.
Definition FastDefaultAllocator.h:50
│ │ │ │ +
gtsam::internal::FastDefaultVectorAllocator
Default allocator for vector types (we never use boost pool for vectors)
Definition FastDefaultAllocator.h:72
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,53 +1,102 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -cholesky.h │ │ │ │ │ +FastDefaultAllocator.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ +19#pragma once │ │ │ │ │ +20#include // Configuration from CMake │ │ │ │ │ 21 │ │ │ │ │ -22namespace _g_t_s_a_m { │ │ │ │ │ -23 │ │ │ │ │ -47GTSAM_EXPORT std::pair _c_h_o_l_e_s_k_y_C_a_r_e_f_u_l(Matrix& ATA, int order = │ │ │ │ │ --1); │ │ │ │ │ -48 │ │ │ │ │ -62GTSAM_EXPORT bool _c_h_o_l_e_s_k_y_P_a_r_t_i_a_l(Matrix& ABC, size_t nFrontal, size_t │ │ │ │ │ -topleft=0); │ │ │ │ │ -63 │ │ │ │ │ -64} │ │ │ │ │ -65 │ │ │ │ │ -_M_a_t_r_i_x_._h │ │ │ │ │ -typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +22#if !defined GTSAM_ALLOCATOR_BOOSTPOOL && !defined GTSAM_ALLOCATOR_TBB && │ │ │ │ │ +!defined GTSAM_ALLOCATOR_STL │ │ │ │ │ +23# ifdef GTSAM_USE_TBB │ │ │ │ │ +24// Use TBB allocator by default if we have TBB, otherwise boost pool │ │ │ │ │ +25# define GTSAM_ALLOCATOR_TBB │ │ │ │ │ +26# else │ │ │ │ │ +27# define GTSAM_ALLOCATOR_BOOSTPOOL │ │ │ │ │ +28# endif │ │ │ │ │ +29#endif │ │ │ │ │ +30 │ │ │ │ │ +31#if defined GTSAM_ALLOCATOR_BOOSTPOOL │ │ │ │ │ +32# include │ │ │ │ │ +33#elif defined GTSAM_ALLOCATOR_TBB │ │ │ │ │ +34# include │ │ │ │ │ +35# undef min // TBB seems to include Windows.h which defines these macros that │ │ │ │ │ +cause problems │ │ │ │ │ +36# undef max │ │ │ │ │ +37# undef ERROR │ │ │ │ │ +38#elif defined GTSAM_ALLOCATOR_STL │ │ │ │ │ +39# include │ │ │ │ │ +40#endif │ │ │ │ │ +41 │ │ │ │ │ +42namespace _g_t_s_a_m │ │ │ │ │ +43{ │ │ │ │ │ +44 │ │ │ │ │ +45 namespace internal │ │ │ │ │ +46 { │ │ │ │ │ +48 template │ │ │ │ │ +_4_9 struct _F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r │ │ │ │ │ +50 { │ │ │ │ │ +51#if defined GTSAM_ALLOCATOR_BOOSTPOOL │ │ │ │ │ +52 typedef boost::fast_pool_allocator type; │ │ │ │ │ +53 static const bool isBoost = true; │ │ │ │ │ +54 static const bool isTBB = false; │ │ │ │ │ +55 static const bool isSTL = false; │ │ │ │ │ +56#elif defined GTSAM_ALLOCATOR_TBB │ │ │ │ │ +57 typedef tbb::tbb_allocator type; │ │ │ │ │ +58 static const bool isBoost = false; │ │ │ │ │ +59 static const bool isTBB = true; │ │ │ │ │ +60 static const bool isSTL = false; │ │ │ │ │ +61#elif defined GTSAM_ALLOCATOR_STL │ │ │ │ │ +62 typedef std::allocator type; │ │ │ │ │ +63 static const bool isBoost = false; │ │ │ │ │ +64 static const bool isTBB = false; │ │ │ │ │ +65 static const bool isSTL = true; │ │ │ │ │ +66#endif │ │ │ │ │ +67 }; │ │ │ │ │ +68 │ │ │ │ │ +70 template │ │ │ │ │ +_7_1 struct _F_a_s_t_D_e_f_a_u_l_t_V_e_c_t_o_r_A_l_l_o_c_a_t_o_r │ │ │ │ │ +72 { │ │ │ │ │ +73#if defined GTSAM_ALLOCATOR_TBB │ │ │ │ │ +74 typedef tbb::tbb_allocator type; │ │ │ │ │ +75 static const bool isBoost = false; │ │ │ │ │ +76 static const bool isTBB = true; │ │ │ │ │ +77 static const bool isSTL = false; │ │ │ │ │ +78#else │ │ │ │ │ +79 typedef std::allocator type; │ │ │ │ │ +80 static const bool isBoost = false; │ │ │ │ │ +81 static const bool isTBB = false; │ │ │ │ │ +82 static const bool isSTL = true; │ │ │ │ │ +83#endif │ │ │ │ │ +84 }; │ │ │ │ │ +85 } │ │ │ │ │ +86 │ │ │ │ │ +87} │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_c_h_o_l_e_s_k_y_C_a_r_e_f_u_l │ │ │ │ │ -pair< size_t, bool > choleskyCareful(Matrix &ATA, int order) │ │ │ │ │ -"Careful" Cholesky computes the positive square-root of a positive symmetric │ │ │ │ │ -semi-definite matrix (i.... │ │ │ │ │ -DDeeffiinniittiioonn cholesky.cpp:76 │ │ │ │ │ -_g_t_s_a_m_:_:_c_h_o_l_e_s_k_y_P_a_r_t_i_a_l │ │ │ │ │ -bool choleskyPartial(Matrix &ABC, size_t nFrontal, size_t topleft) │ │ │ │ │ -Partial Cholesky computes a factor [R S such that [R' 0 [R S = [A B 0 L] S' I] │ │ │ │ │ -0 L] B' C]. │ │ │ │ │ -DDeeffiinniittiioonn cholesky.cpp:108 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r │ │ │ │ │ +Default allocator for list, map, and set types. │ │ │ │ │ +DDeeffiinniittiioonn FastDefaultAllocator.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_F_a_s_t_D_e_f_a_u_l_t_V_e_c_t_o_r_A_l_l_o_c_a_t_o_r │ │ │ │ │ +Default allocator for vector types (we never use boost pool for vectors) │ │ │ │ │ +DDeeffiinniittiioonn FastDefaultAllocator.h:72 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _c_h_o_l_e_s_k_y_._h │ │ │ │ │ + * _F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00011.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Lie.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Vector.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,138 +94,112 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ -Macros | │ │ │ │ Functions
│ │ │ │ -
Lie.h File Reference
│ │ │ │ +
Vector.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Base class and basic functions for Lie types. │ │ │ │ +

typedef and functions to augment Eigen's Vectors │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

struct  gtsam::LieGroup< Class, N >
 A CRTP helper class that implements Lie group methods Prerequisites: methods operator*, inverse, and AdjointMap, as well as a ChartAtOrigin struct that will be used to define the manifold Chart To use, simply derive, but also say "using LieGroup<Class,N>::inverse" For derivative math, see doc/math.pdf. More...
 
struct  gtsam::lie_group_tag
 tag to assert a type is a Lie group More...
 
struct  gtsam::internal::LieGroupTraits< Class >
 A helper class that implements the traits interface for GTSAM lie groups. More...
 
struct  gtsam::internal::LieGroup< Class >
 Both LieGroupTraits and Testable. More...
 
class  gtsam::IsLieGroup< T >
 Lie Group Concept. More...
 
class  gtsam::TransformCovariance< T >
 Functor for transforming covariance of T. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Macros

#define GTSAM_CONCEPT_LIE_INST(T)   template class gtsam::IsLieGroup<T>;
 Macros for using the LieConcept.
 
│ │ │ │ -#define GTSAM_CONCEPT_LIE_TYPE(T)   using _gtsam_IsLieGroup_##T = gtsam::IsLieGroup<T>;
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

template<class Class >
Class gtsam::between_default (const Class &l1, const Class &l2)
 These core global functions can be specialized by new Lie types for better performance.
 
template<class Class >
Vector gtsam::logmap_default (const Class &l0, const Class &lp)
 Log map centered at l0, s.t.
 
template<class Class >
Class gtsam::expmap_default (const Class &t, const Vector &d)
 Exponential map centered at l0, s.t.
 
template<class T >
T gtsam::BCH (const T &X, const T &Y)
 Three term approximation of the Baker-Campbell-Hausdorff formula In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y) it is not true that Z = X+Y.
 
│ │ │ │ -template<class T >
Matrix gtsam::wedge (const Vector &x)
 Declaration of wedge (see Murray94book) used to convert from n exponential coordinates to n*n element of the Lie algebra.
 
template<class T >
T gtsam::expm (const Vector &x, int K=7)
 Exponential map given exponential coordinates class T needs a wedge<> function and a constructor from Matrix.
 
template<typename T >
T gtsam::interpolate (const T &X, const T &Y, double t, typename MakeOptionalJacobian< T, T >::type Hx=boost::none, typename MakeOptionalJacobian< T, T >::type Hy=boost::none)
 Linear interpolation between X and Y by coefficient t.
 
bool gtsam::fpEqual (double a, double b, double tol, bool check_relative_also=true)
 Ensure we are not including a different version of Eigen in user code than while compiling gtsam, since it can lead to hard-to-understand runtime crashes.
 
│ │ │ │ +void gtsam::print (const Vector &v, const std::string &s, std::ostream &stream)
 print without optional string, must specify cout yourself
 
│ │ │ │ +void gtsam::print (const Vector &v, const std::string &s="")
 print with optional string to cout
 
│ │ │ │ +void gtsam::save (const Vector &A, const std::string &s, const std::string &filename)
 save a vector to file, which can be loaded by matlab
 
│ │ │ │ +bool gtsam::operator== (const Vector &vec1, const Vector &vec2)
 operator==()
 
│ │ │ │ +bool gtsam::greaterThanOrEqual (const Vector &v1, const Vector &v2)
 Greater than or equal to operation returns true if all elements in v1 are greater than corresponding elements in v2.
 
│ │ │ │ +bool gtsam::equal_with_abs_tol (const Vector &vec1, const Vector &vec2, double tol=1e-9)
 VecA == VecB up to tolerance.
 
│ │ │ │ +bool gtsam::equal_with_abs_tol (const SubVector &vec1, const SubVector &vec2, double tol)
 
bool gtsam::assert_equal (const Vector &vec1, const Vector &vec2, double tol=1e-9)
 Same, prints if error.
 
bool gtsam::assert_inequal (const Vector &vec1, const Vector &vec2, double tol=1e-9)
 Not the same, prints if error.
 
bool gtsam::assert_equal (const SubVector &vec1, const SubVector &vec2, double tol=1e-9)
 Same, prints if error.
 
│ │ │ │ +bool gtsam::assert_equal (const ConstSubVector &expected, const ConstSubVector &actual, double tol)
 
bool gtsam::linear_dependent (const Vector &vec1, const Vector &vec2, double tol=1e-9)
 check whether two vectors are linearly dependent
 
Vector gtsam::ediv_ (const Vector &a, const Vector &b)
 elementwise division, but 0/0 = 0, not inf
 
│ │ │ │ +double gtsam::houseInPlace (Vector &x)
 beta = house(x) computes the HouseHolder vector in place
 
pair< double, Vector > gtsam::house (const Vector &x)
 house(x,j) computes HouseHolder vector v and scaling factor beta from x, such that the corresponding Householder reflection zeroes out all but x.
 
│ │ │ │ +double gtsam::weightedPseudoinverse (const Vector &a, const Vector &weights, Vector &pseudo)
 
pair< Vector, double > gtsam::weightedPseudoinverse (const Vector &v, const Vector &weights)
 Weighted Householder solution vector, a.k.a., the pseudoinverse of the column NOTE: if any sigmas are zero (indicating a constraint) the pseudoinverse will be a selection vector, and the variance will be zero.
 
│ │ │ │ +Vector gtsam::concatVectors (const std::list< Vector > &vs)
 concatenate Vectors
 
│ │ │ │ +Vector gtsam::concatVectors (size_t nrVectors,...)
 concatenate Vectors
 
│ │ │ │

Detailed Description

│ │ │ │ -

Base class and basic functions for Lie types.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
│ │ │ │ -Alex Cunningham
│ │ │ │ +

typedef and functions to augment Eigen's Vectors

│ │ │ │ +
Author
Kai Ni
│ │ │ │
│ │ │ │ Frank Dellaert
│ │ │ │
│ │ │ │ -Mike Bosse
│ │ │ │ -
│ │ │ │ -Duy Nguyen Ta
│ │ │ │ -
│ │ │ │ -Yotam Stern
│ │ │ │ -

Macro Definition Documentation

│ │ │ │ - │ │ │ │ -

◆ GTSAM_CONCEPT_LIE_INST

│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
#define GTSAM_CONCEPT_LIE_INST( T)   template class gtsam::IsLieGroup<T>;
│ │ │ │ -
│ │ │ │ - │ │ │ │ -

Macros for using the LieConcept.

│ │ │ │ -
    │ │ │ │ -
  • An instantiation for use inside unit tests
    • │ │ │ │ -
  • A typedef for use inside generic algorithms
    • │ │ │ │ -
│ │ │ │ -

NOTE: intentionally not in the gtsam namespace to allow for classes not in the gtsam namespace to be more easily enforced as testable

│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ +Varun Agrawal │ │ │ │ +
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,101 +1,105 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Lie.h File Reference │ │ │ │ │ -Base class and basic functions for Lie types. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_<_ _C_l_a_s_s_,_ _N_ _> │ │ │ │ │ - A CRTP helper class that implements Lie group methods Prerequisites: │ │ │ │ │ - methods operator*, inverse, and AdjointMap, as well as a ChartAtOrigin │ │ │ │ │ -  struct that will be used to define the manifold Chart To use, simply │ │ │ │ │ - derive, but also say "using LieGroup::inverse" For derivative │ │ │ │ │ - math, see doc/math.pdf. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_l_i_e___g_r_o_u_p___t_a_g │ │ │ │ │ -  tag to assert a type is a Lie group _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p_T_r_a_i_t_s_<_ _C_l_a_s_s_ _> │ │ │ │ │ -  A helper class that implements the traits interface for GTSAM lie │ │ │ │ │ - groups. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p_<_ _C_l_a_s_s_ _> │ │ │ │ │ -  Both _L_i_e_G_r_o_u_p_T_r_a_i_t_s and _T_e_s_t_a_b_l_e. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_I_s_L_i_e_G_r_o_u_p_<_ _T_ _> │ │ │ │ │ -  Lie Group Concept. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_T_r_a_n_s_f_o_r_m_C_o_v_a_r_i_a_n_c_e_<_ _T_ _> │ │ │ │ │ -  Functor for transforming covariance of T. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Vector.cpp File Reference │ │ │ │ │ +typedef and functions to augment Eigen's Vectors _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -MMaaccrrooss │ │ │ │ │ -#define  _G_T_S_A_M___C_O_N_C_E_P_T___L_I_E___I_N_S_T(T)   template class _g_t_s_a_m_:_:_I_s_L_i_e_G_r_o_u_p; │ │ │ │ │ -  Macros for using the LieConcept. │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ + bool  _g_t_s_a_m_:_:_f_p_E_q_u_a_l (double a, double b, double tol, bool │ │ │ │ │ + check_relative_also=true) │ │ │ │ │ + Ensure we are not including a different version of │ │ │ │ │ +  Eigen in user code than while compiling gtsam, since it │ │ │ │ │ + can lead to hard-to-understand runtime crashes. │ │ │ │ │ +  │ │ │ │ │ + void  ggttssaamm::::pprriinntt (const Vector &v, const std::string &s, │ │ │ │ │ + std::ostream &stream) │ │ │ │ │ +  print without optional string, must specify cout │ │ │ │ │ + yourself │ │ │ │ │ +  │ │ │ │ │ + void  ggttssaamm::::pprriinntt (const Vector &v, const std::string &s="") │ │ │ │ │ +  print with optional string to cout │ │ │ │ │ +  │ │ │ │ │ + void  ggttssaamm::::ssaavvee (const Vector &A, const std::string &s, │ │ │ │ │ + const std::string &filename) │ │ │ │ │ +  save a vector to file, which can be loaded by matlab │ │ │ │ │ +  │ │ │ │ │ + bool  ggttssaamm::::ooppeerraattoorr==== (const Vector &vec1, const Vector │ │ │ │ │ + &vec2) │ │ │ │ │ +  _o_p_e_r_a_t_o_r_=_=_(_) │ │ │ │ │ +  │ │ │ │ │ + bool  ggttssaamm::::ggrreeaatteerrTThhaannOOrrEEqquuaall (const Vector &v1, const │ │ │ │ │ + Vector &v2) │ │ │ │ │ + Greater than or equal to operation returns true if all │ │ │ │ │ +  elements in v1 are greater than corresponding elements │ │ │ │ │ + in v2. │ │ │ │ │ +  │ │ │ │ │ + bool  ggttssaamm::::eeqquuaall__wwiitthh__aabbss__ttooll (const Vector &vec1, const │ │ │ │ │ + Vector &vec2, double tol=1e-9) │ │ │ │ │ +  VecA == VecB up to tolerance. │ │ │ │ │ +  │ │ │ │ │ + bool  ggttssaamm::::eeqquuaall__wwiitthh__aabbss__ttooll (const SubVector &vec1, const │ │ │ │ │ + SubVector &vec2, double tol) │ │ │ │ │ +  │ │ │ │ │ + bool  _g_t_s_a_m_:_:_a_s_s_e_r_t___e_q_u_a_l (const Vector &vec1, const Vector │ │ │ │ │ + &vec2, double tol=1e-9) │ │ │ │ │ +  Same, prints if error. │ │ │ │ │ +  │ │ │ │ │ + bool  _g_t_s_a_m_:_:_a_s_s_e_r_t___i_n_e_q_u_a_l (const Vector &vec1, const Vector │ │ │ │ │ + &vec2, double tol=1e-9) │ │ │ │ │ +  Not the same, prints if error. │ │ │ │ │ +  │ │ │ │ │ + bool  _g_t_s_a_m_:_:_a_s_s_e_r_t___e_q_u_a_l (const SubVector &vec1, const │ │ │ │ │ + SubVector &vec2, double tol=1e-9) │ │ │ │ │ +  Same, prints if error. │ │ │ │ │ +  │ │ │ │ │ + bool  ggttssaamm::::aasssseerrtt__eeqquuaall (const ConstSubVector &expected, │ │ │ │ │ + const ConstSubVector &actual, double tol) │ │ │ │ │ +  │ │ │ │ │ + bool  _g_t_s_a_m_:_:_l_i_n_e_a_r___d_e_p_e_n_d_e_n_t (const Vector &vec1, const │ │ │ │ │ + Vector &vec2, double tol=1e-9) │ │ │ │ │ +  check whether two vectors are linearly dependent │ │ │ │ │ +  │ │ │ │ │ + Vector  _g_t_s_a_m_:_:_e_d_i_v__ (const Vector &a, const Vector &b) │ │ │ │ │ +  elementwise division, but 0/0 = 0, not inf │ │ │ │ │ +  │ │ │ │ │ + double  ggttssaamm::::hhoouusseeIInnPPllaaccee (Vector &x) │ │ │ │ │ +  beta = house(x) computes the HouseHolder vector in │ │ │ │ │ + place │ │ │ │ │ +  │ │ │ │ │ +pair< double, Vector >  _g_t_s_a_m_:_:_h_o_u_s_e (const Vector &x) │ │ │ │ │ + house(x,j) computes HouseHolder vector v and scaling │ │ │ │ │ +  factor beta from x, such that the corresponding │ │ │ │ │ + Householder reflection zeroes out all but x. │ │ │ │ │ +  │ │ │ │ │ + double  ggttssaamm::::wweeiigghhtteeddPPsseeuuddooiinnvveerrssee (const Vector &a, const │ │ │ │ │ + Vector &weights, Vector &pseudo) │ │ │ │ │ +  │ │ │ │ │ +pair< Vector, double >  _g_t_s_a_m_:_:_w_e_i_g_h_t_e_d_P_s_e_u_d_o_i_n_v_e_r_s_e (const Vector &v, const │ │ │ │ │ + Vector &weights) │ │ │ │ │ + Weighted Householder solution vector, a.k.a., the │ │ │ │ │ +  pseudoinverse of the column NOTE: if any sigmas are │ │ │ │ │ + zero (indicating a constraint) the pseudoinverse will │ │ │ │ │ + be a selection vector, and the variance will be zero. │ │ │ │ │   │ │ │ │ │ -#define  GGTTSSAAMM__CCOONNCCEEPPTT__LLIIEE__TTYYPPEE(T)   using _gtsam_IsLieGroup_##T = _g_t_s_a_m_:_: │ │ │ │ │ - _I_s_L_i_e_G_r_o_u_p; │ │ │ │ │ + Vector  ggttssaamm::::ccoonnccaattVVeeccttoorrss (const std::list< Vector > &vs) │ │ │ │ │ +  concatenate Vectors │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ - Class  _g_t_s_a_m_:_:_b_e_t_w_e_e_n___d_e_f_a_u_l_t (const Class &l1, const Class &l2) │ │ │ │ │ -  These core global functions can be specialized by new Lie types for │ │ │ │ │ - better performance. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -Vector  _g_t_s_a_m_:_:_l_o_g_m_a_p___d_e_f_a_u_l_t (const Class &l0, const Class &lp) │ │ │ │ │ -  Log map centered at l0, s.t. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - Class  _g_t_s_a_m_:_:_e_x_p_m_a_p___d_e_f_a_u_l_t (const Class &t, const Vector &d) │ │ │ │ │ -  Exponential map centered at l0, s.t. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - T  _g_t_s_a_m_:_:_B_C_H (const T &X, const T &Y) │ │ │ │ │ - Three term approximation of the Baker-Campbell-Hausdorff formula In │ │ │ │ │ -  non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y) it is │ │ │ │ │ - not true that Z = X+Y. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -Matrix  ggttssaamm::::wweeddggee (const Vector &x) │ │ │ │ │ -  Declaration of wedge (see Murray94book) used to convert from n │ │ │ │ │ - exponential coordinates to n*n element of the Lie algebra. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - T  _g_t_s_a_m_:_:_e_x_p_m (const Vector &x, int K=7) │ │ │ │ │ -  Exponential map given exponential coordinates class T needs a wedge<> │ │ │ │ │ - function and a constructor from Matrix. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - T  _g_t_s_a_m_:_:_i_n_t_e_r_p_o_l_a_t_e (const T &X, const T &Y, double t, typename │ │ │ │ │ - _M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< T, T >::type Hx=boost::none, typename │ │ │ │ │ - _M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< T, T >::type Hy=boost::none) │ │ │ │ │ -  Linear interpolation between X and Y by coefficient t. │ │ │ │ │ + Vector  ggttssaamm::::ccoonnccaattVVeeccttoorrss (size_t nrVectors,...) │ │ │ │ │ +  concatenate Vectors │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Base class and basic functions for Lie types. │ │ │ │ │ +typedef and functions to augment Eigen's Vectors │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Alex Cunningham │ │ │ │ │ + Kai Ni │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Mike Bosse │ │ │ │ │ - Duy Nguyen Ta │ │ │ │ │ - Yotam Stern │ │ │ │ │ -********** MMaaccrroo DDeeffiinniittiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ -********** _?◆_? GGTTSSAAMM__CCOONNCCEEPPTT__LLIIEE__IINNSSTT ********** │ │ │ │ │ -#define GTSAM_CONCEPT_LIE_INST (   T )    template class _g_t_s_a_m_:_:_I_s_L_i_e_G_r_o_u_p; │ │ │ │ │ -Macros for using the LieConcept. │ │ │ │ │ - * An instantiation for use inside unit tests │ │ │ │ │ - * A typedef for use inside generic algorithms │ │ │ │ │ -NOTE: intentionally not in the gtsam namespace to allow for classes not in the │ │ │ │ │ -gtsam namespace to be more easily enforced as testable │ │ │ │ │ + Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _L_i_e_._h │ │ │ │ │ + * _V_e_c_t_o_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00011.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,16 +1,19 @@ │ │ │ │ │ var a00011 = [ │ │ │ │ │ - ["gtsam::LieGroup< Class, N >", "a02364.html", "a02364"], │ │ │ │ │ - ["gtsam::lie_group_tag", "a02368.html", null], │ │ │ │ │ - ["gtsam::internal::LieGroupTraits< Class >", "a02372.html", null], │ │ │ │ │ - ["gtsam::internal::LieGroup< Class >", "a02376.html", null], │ │ │ │ │ - ["gtsam::IsLieGroup< T >", "a02380.html", null], │ │ │ │ │ - ["gtsam::TransformCovariance< T >", "a02384.html", null], │ │ │ │ │ - ["GTSAM_CONCEPT_LIE_INST", "a00011.html#aeef5af7d583d275fbc0e6e5af8bf2afc", null], │ │ │ │ │ - ["BCH", "a00011.html#adbd55c711e6ceee791b595558eb3ec8a", null], │ │ │ │ │ - ["between_default", "a00011.html#abd92914d0822d42584d46f31fb500048", null], │ │ │ │ │ - ["expm", "a00011.html#a3041816208c79ef76d3ef2e0991d90b2", null], │ │ │ │ │ - ["expmap_default", "a00011.html#af5a7c0a7422c38a0baa131c073e233ac", null], │ │ │ │ │ - ["interpolate", "a00011.html#adaf06b04fa93b050e99bc3d571d85d53", null], │ │ │ │ │ - ["logmap_default", "a00011.html#a5c6ccec4a3ea452361c0956766bccef5", null], │ │ │ │ │ - ["wedge", "a00011.html#afe28b7f8e3592fb9f5cf9ebae09497ba", null] │ │ │ │ │ + ["assert_equal", "a00011.html#ac3cf8e8e70cd696d9c789c31bc1e89a7", null], │ │ │ │ │ + ["assert_equal", "a00011.html#a9f3ee662d25ffb8c04b4e35c4b02e90b", null], │ │ │ │ │ + ["assert_inequal", "a00011.html#a2ca3db47b15350977c1f03c5560ab332", null], │ │ │ │ │ + ["concatVectors", "a00011.html#ae1c88acfa9575bcea3f2e738cb637a85", null], │ │ │ │ │ + ["concatVectors", "a00011.html#a09610ec69a8b3f3a00399bb378eda9cf", null], │ │ │ │ │ + ["ediv_", "a00011.html#a14844c96706dc362917e0d5e76d65fbe", null], │ │ │ │ │ + ["equal_with_abs_tol", "a00011.html#a8bdced1844ffd6a53af0396d82eaa6da", null], │ │ │ │ │ + ["fpEqual", "a00011.html#ae19c359190a379cba44bf818a2293f7c", null], │ │ │ │ │ + ["greaterThanOrEqual", "a00011.html#a368ee40bd9c0124d572d2e49bcb077bf", null], │ │ │ │ │ + ["house", "a00011.html#afb77eefc25dea0fb3e0fcc8f246b617a", null], │ │ │ │ │ + ["houseInPlace", "a00011.html#ac9c0c05b0198ec0fb6e4115edb755910", null], │ │ │ │ │ + ["linear_dependent", "a00011.html#a2a0cfd7908b06491df49b6a9c9186775", null], │ │ │ │ │ + ["operator==", "a00011.html#a64988014ab746343803620dc42513646", null], │ │ │ │ │ + ["print", "a00011.html#a760cc36e9009b23cf564c436294b3504", null], │ │ │ │ │ + ["print", "a00011.html#a5e5d8018a0f44a6a299406aeda27d44b", null], │ │ │ │ │ + ["save", "a00011.html#a09f2bbdb9f9d633542362dbe8d79f9ab", null], │ │ │ │ │ + ["weightedPseudoinverse", "a00011.html#aca1c56d11a05464a7a5458dc32ccc777", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00014.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/TestableAssertions.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Vector.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,104 +95,185 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces | │ │ │ │ +Macros | │ │ │ │ +Typedefs | │ │ │ │ Functions
│ │ │ │ -
TestableAssertions.h File Reference
│ │ │ │ +
Vector.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Provides additional testing facilities for common data structures. │ │ │ │ +

typedef and functions to augment Eigen's VectorXd │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Macros

│ │ │ │ +#define MKL_BLAS   MKL_DOMAIN_BLAS
 
#define GTSAM_MAKE_VECTOR_DEFS(N)
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Typedefs

│ │ │ │ +typedef Eigen::VectorXd gtsam::Vector
 
│ │ │ │ +typedef Eigen::Matrix< double, 1, 1 > gtsam::Vector1
 
│ │ │ │ +typedef Eigen::Vector2d gtsam::Vector2
 
│ │ │ │ +typedef Eigen::Vector3d gtsam::Vector3
 
│ │ │ │ +typedef Eigen::VectorBlock< Vector > gtsam::SubVector
 
│ │ │ │ +typedef Eigen::VectorBlock< const Vector > gtsam::ConstSubVector
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

│ │ │ │ -bool gtsam::assert_equal (const Key &expected, const Key &actual, double tol=0.0)
 Equals testing for basic types.
 
template<class V >
bool gtsam::assert_equal (const boost::optional< V > &expected, const boost::optional< V > &actual, double tol=1e-9)
 Comparisons for boost.optional objects that checks whether objects exist before comparing their values.
 
│ │ │ │ -template<class V >
bool gtsam::assert_equal (const V &expected, const boost::optional< V > &actual, double tol=1e-9)
 
│ │ │ │ -template<class V >
bool gtsam::assert_equal (const V &expected, const boost::optional< const V & > &actual, double tol=1e-9)
 
│ │ │ │ +
bool gtsam::fpEqual (double a, double b, double tol, bool check_relative_also=true)
 Ensure we are not including a different version of Eigen in user code than while compiling gtsam, since it can lead to hard-to-understand runtime crashes.
 
│ │ │ │ +void gtsam::print (const Vector &v, const std::string &s, std::ostream &stream)
 print without optional string, must specify cout yourself
 
│ │ │ │ +void gtsam::print (const Vector &v, const std::string &s="")
 print with optional string to cout
 
│ │ │ │ +void gtsam::save (const Vector &A, const std::string &s, const std::string &filename)
 save a vector to file, which can be loaded by matlab
 
│ │ │ │ +bool gtsam::operator== (const Vector &vec1, const Vector &vec2)
 operator==()
 
│ │ │ │ +bool gtsam::greaterThanOrEqual (const Vector &v1, const Vector &v2)
 Greater than or equal to operation returns true if all elements in v1 are greater than corresponding elements in v2.
 
│ │ │ │ +bool gtsam::equal_with_abs_tol (const Vector &vec1, const Vector &vec2, double tol=1e-9)
 VecA == VecB up to tolerance.
 
│ │ │ │ +bool gtsam::equal_with_abs_tol (const SubVector &vec1, const SubVector &vec2, double tol)
 
│ │ │ │ +bool gtsam::equal (const Vector &vec1, const Vector &vec2, double tol)
 Override of equal in Lie.h.
 
│ │ │ │ +bool gtsam::equal (const Vector &vec1, const Vector &vec2)
 Override of equal in Lie.h.
 
bool gtsam::assert_equal (const Vector &vec1, const Vector &vec2, double tol=1e-9)
 Same, prints if error.
 
bool gtsam::assert_inequal (const Vector &vec1, const Vector &vec2, double tol=1e-9)
 Not the same, prints if error.
 
bool gtsam::assert_equal (const SubVector &vec1, const SubVector &vec2, double tol=1e-9)
 Same, prints if error.
 
│ │ │ │ +bool gtsam::assert_equal (const ConstSubVector &expected, const ConstSubVector &actual, double tol)
 
bool gtsam::linear_dependent (const Vector &vec1, const Vector &vec2, double tol=1e-9)
 check whether two vectors are linearly dependent
 
Vector gtsam::ediv_ (const Vector &a, const Vector &b)
 elementwise division, but 0/0 = 0, not inf
 
│ │ │ │ template<class V1 , class V2 >
bool gtsam::assert_container_equal (const std::map< V1, V2 > &expected, const std::map< V1, V2 > &actual, double tol=1e-9)
 Function for comparing maps of testable->testable TODO: replace with more generalized version.
 
│ │ │ │ -template<class V2 >
bool gtsam::assert_container_equal (const std::map< size_t, V2 > &expected, const std::map< size_t, V2 > &actual, double tol=1e-9)
 Function for comparing maps of size_t->testable.
 
│ │ │ │ +
double gtsam::dot (const V1 &a, const V2 &b)
 Dot product.
 
│ │ │ │ template<class V1 , class V2 >
bool gtsam::assert_container_equal (const std::vector< std::pair< V1, V2 > > &expected, const std::vector< std::pair< V1, V2 > > &actual, double tol=1e-9)
 Function for comparing vector of pairs (testable, testable)
 
│ │ │ │ -template<class V >
bool gtsam::assert_container_equal (const V &expected, const V &actual, double tol=1e-9)
 General function for comparing containers of testable objects.
 
│ │ │ │ -template<class V2 >
bool gtsam::assert_container_equality (const std::map< size_t, V2 > &expected, const std::map< size_t, V2 > &actual)
 Function for comparing maps of size_t->testable Types are assumed to have operator ==.
 
│ │ │ │ -template<class V >
bool gtsam::assert_container_equality (const V &expected, const V &actual)
 General function for comparing containers of objects with operator==.
 
│ │ │ │ -bool gtsam::assert_equal (const std::string &expected, const std::string &actual)
 Compare strings for unit tests.
 
│ │ │ │ -template<class V >
bool gtsam::assert_inequal (const V &expected, const V &actual, double tol=1e-9)
 Allow for testing inequality.
 
│ │ │ │ -template<class V >
bool gtsam::assert_stdout_equal (const std::string &expected, const V &actual)
 Capture std out via cout stream and compare against string.
 
template<class V >
bool gtsam::assert_print_equal (const std::string &expected, const V &actual, const std::string &s="")
 Capture print function output and compare against string.
 
double gtsam::inner_prod (const V1 &a, const V2 &b)
 compatibility version for ublas' inner_prod()
 
pair< double, Vector > gtsam::house (const Vector &x)
 house(x,j) computes HouseHolder vector v and scaling factor beta from x, such that the corresponding Householder reflection zeroes out all but x.
 
│ │ │ │ +double gtsam::houseInPlace (Vector &x)
 beta = house(x) computes the HouseHolder vector in place
 
pair< Vector, double > gtsam::weightedPseudoinverse (const Vector &v, const Vector &weights)
 Weighted Householder solution vector, a.k.a., the pseudoinverse of the column NOTE: if any sigmas are zero (indicating a constraint) the pseudoinverse will be a selection vector, and the variance will be zero.
 
│ │ │ │ +double gtsam::weightedPseudoinverse (const Vector &a, const Vector &weights, Vector &pseudo)
 
│ │ │ │ +Vector gtsam::concatVectors (const std::list< Vector > &vs)
 concatenate Vectors
 
│ │ │ │ +Vector gtsam::concatVectors (size_t nrVectors,...)
 concatenate Vectors
 
│ │ │ │

Detailed Description

│ │ │ │ -

Provides additional testing facilities for common data structures.

│ │ │ │ -
Author
Alex Cunningham
│ │ │ │ -
│ │ │ │ +

typedef and functions to augment Eigen's VectorXd

│ │ │ │ +
Author
Kai Ni
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +
│ │ │ │ +Alex Hagiopol
│ │ │ │ +
│ │ │ │ +Varun Agrawal
│ │ │ │ +

Macro Definition Documentation

│ │ │ │ + │ │ │ │ +

◆ GTSAM_MAKE_VECTOR_DEFS

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
#define GTSAM_MAKE_VECTOR_DEFS( N)
│ │ │ │ +
│ │ │ │ +Value:
using Vector##N = Eigen::Matrix<double, N, 1>; \
│ │ │ │ +
static const Eigen::MatrixBase<Vector##N>::ConstantReturnType Z_##N##x1 = Vector##N::Zero();
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,90 +1,147 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -TestableAssertions.h File Reference │ │ │ │ │ -Provides additional testing facilities for common data structures. _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Vector.h File Reference │ │ │ │ │ +typedef and functions to augment Eigen's VectorXd _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +MMaaccrrooss │ │ │ │ │ +#define  MMKKLL__BBLLAASS   MKL_DOMAIN_BLAS │ │ │ │ │ +  │ │ │ │ │ +#define  _G_T_S_A_M___M_A_K_E___V_E_C_T_O_R___D_E_F_S(N) │ │ │ │ │ +  │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ + typedef Eigen::VectorXd  ggttssaamm::::VVeeccttoorr │ │ │ │ │ +  │ │ │ │ │ + typedef Eigen::Matrix< double, 1, 1 >  ggttssaamm::::VVeeccttoorr11 │ │ │ │ │ +  │ │ │ │ │ + typedef Eigen::Vector2d  ggttssaamm::::VVeeccttoorr22 │ │ │ │ │ +  │ │ │ │ │ + typedef Eigen::Vector3d  ggttssaamm::::VVeeccttoorr33 │ │ │ │ │ +  │ │ │ │ │ + typedef Eigen::VectorBlock< Vector >  ggttssaamm::::SSuubbVVeeccttoorr │ │ │ │ │ +  │ │ │ │ │ +typedef Eigen::VectorBlock< const Vector >  ggttssaamm::::CCoonnssttSSuubbVVeeccttoorr │ │ │ │ │ +  │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -bool  ggttssaamm::::aasssseerrtt__eeqquuaall (const _K_e_y &expected, const _K_e_y &actual, double │ │ │ │ │ - tol=0.0) │ │ │ │ │ -  Equals testing for basic types. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -bool  _g_t_s_a_m_:_:_a_s_s_e_r_t___e_q_u_a_l (const boost::optional< V > &expected, const boost:: │ │ │ │ │ - optional< V > &actual, double tol=1e-9) │ │ │ │ │ -  Comparisons for boost.optional objects that checks whether objects exist │ │ │ │ │ - before comparing their values. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -bool  ggttssaamm::::aasssseerrtt__eeqquuaall (const V &expected, const boost::optional< V > │ │ │ │ │ - &actual, double tol=1e-9) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -bool  ggttssaamm::::aasssseerrtt__eeqquuaall (const V &expected, const boost::optional< const V & │ │ │ │ │ - > &actual, double tol=1e-9) │ │ │ │ │ + bool  _g_t_s_a_m_:_:_f_p_E_q_u_a_l (double a, double b, double tol, bool │ │ │ │ │ + check_relative_also=true) │ │ │ │ │ + Ensure we are not including a different version of │ │ │ │ │ +  Eigen in user code than while compiling gtsam, since it │ │ │ │ │ + can lead to hard-to-understand runtime crashes. │ │ │ │ │ +  │ │ │ │ │ + void  ggttssaamm::::pprriinntt (const Vector &v, const std::string &s, │ │ │ │ │ + std::ostream &stream) │ │ │ │ │ +  print without optional string, must specify cout │ │ │ │ │ + yourself │ │ │ │ │ +  │ │ │ │ │ + void  ggttssaamm::::pprriinntt (const Vector &v, const std::string &s="") │ │ │ │ │ +  print with optional string to cout │ │ │ │ │ +  │ │ │ │ │ + void  ggttssaamm::::ssaavvee (const Vector &A, const std::string &s, │ │ │ │ │ + const std::string &filename) │ │ │ │ │ +  save a vector to file, which can be loaded by matlab │ │ │ │ │ +  │ │ │ │ │ + bool  ggttssaamm::::ooppeerraattoorr==== (const Vector &vec1, const Vector │ │ │ │ │ + &vec2) │ │ │ │ │ +  _o_p_e_r_a_t_o_r_=_=_(_) │ │ │ │ │ +  │ │ │ │ │ + bool  ggttssaamm::::ggrreeaatteerrTThhaannOOrrEEqquuaall (const Vector &v1, const │ │ │ │ │ + Vector &v2) │ │ │ │ │ + Greater than or equal to operation returns true if all │ │ │ │ │ +  elements in v1 are greater than corresponding elements │ │ │ │ │ + in v2. │ │ │ │ │ +  │ │ │ │ │ + bool  ggttssaamm::::eeqquuaall__wwiitthh__aabbss__ttooll (const Vector &vec1, const │ │ │ │ │ + Vector &vec2, double tol=1e-9) │ │ │ │ │ +  VecA == VecB up to tolerance. │ │ │ │ │ +  │ │ │ │ │ + bool  ggttssaamm::::eeqquuaall__wwiitthh__aabbss__ttooll (const SubVector &vec1, const │ │ │ │ │ + SubVector &vec2, double tol) │ │ │ │ │ +  │ │ │ │ │ + bool  ggttssaamm::::eeqquuaall (const Vector &vec1, const Vector &vec2, │ │ │ │ │ + double tol) │ │ │ │ │ +  Override of equal in _L_i_e_._h. │ │ │ │ │ +  │ │ │ │ │ + bool  ggttssaamm::::eeqquuaall (const Vector &vec1, const Vector &vec2) │ │ │ │ │ +  Override of equal in _L_i_e_._h. │ │ │ │ │ +  │ │ │ │ │ + bool  _g_t_s_a_m_:_:_a_s_s_e_r_t___e_q_u_a_l (const Vector &vec1, const Vector │ │ │ │ │ + &vec2, double tol=1e-9) │ │ │ │ │ +  Same, prints if error. │ │ │ │ │ +  │ │ │ │ │ + bool  _g_t_s_a_m_:_:_a_s_s_e_r_t___i_n_e_q_u_a_l (const Vector &vec1, const Vector │ │ │ │ │ + &vec2, double tol=1e-9) │ │ │ │ │ +  Not the same, prints if error. │ │ │ │ │ +  │ │ │ │ │ + bool  _g_t_s_a_m_:_:_a_s_s_e_r_t___e_q_u_a_l (const SubVector &vec1, const │ │ │ │ │ + SubVector &vec2, double tol=1e-9) │ │ │ │ │ +  Same, prints if error. │ │ │ │ │ +  │ │ │ │ │ + bool  ggttssaamm::::aasssseerrtt__eeqquuaall (const ConstSubVector &expected, │ │ │ │ │ + const ConstSubVector &actual, double tol) │ │ │ │ │ +  │ │ │ │ │ + bool  _g_t_s_a_m_:_:_l_i_n_e_a_r___d_e_p_e_n_d_e_n_t (const Vector &vec1, const │ │ │ │ │ + Vector &vec2, double tol=1e-9) │ │ │ │ │ +  check whether two vectors are linearly dependent │ │ │ │ │ +  │ │ │ │ │ + Vector  _g_t_s_a_m_:_:_e_d_i_v__ (const Vector &a, const Vector &b) │ │ │ │ │ +  elementwise division, but 0/0 = 0, not inf │ │ │ │ │   │ │ │ │ │ template │ │ │ │ │ -bool  ggttssaamm::::aasssseerrtt__ccoonnttaaiinneerr__eeqquuaall (const std::map< V1, V2 > &expected, const │ │ │ │ │ - std::map< V1, V2 > &actual, double tol=1e-9) │ │ │ │ │ -  Function for comparing maps of testable->testable TODO: replace with more │ │ │ │ │ - generalized version. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -bool  ggttssaamm::::aasssseerrtt__ccoonnttaaiinneerr__eeqquuaall (const std::map< size_t, V2 > &expected, │ │ │ │ │ - const std::map< size_t, V2 > &actual, double tol=1e-9) │ │ │ │ │ -  Function for comparing maps of size_t->testable. │ │ │ │ │ + double  ggttssaamm::::ddoott (const V1 &a, const V2 &b) │ │ │ │ │ +  Dot product. │ │ │ │ │   │ │ │ │ │ template │ │ │ │ │ -bool  ggttssaamm::::aasssseerrtt__ccoonnttaaiinneerr__eeqquuaall (const std::vector< std::pair< V1, V2 > > │ │ │ │ │ - &expected, const std::vector< std::pair< V1, V2 > > &actual, double │ │ │ │ │ - tol=1e-9) │ │ │ │ │ -  Function for comparing vector of pairs (testable, testable) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -bool  ggttssaamm::::aasssseerrtt__ccoonnttaaiinneerr__eeqquuaall (const V &expected, const V &actual, double │ │ │ │ │ - tol=1e-9) │ │ │ │ │ -  General function for comparing containers of testable objects. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -bool  ggttssaamm::::aasssseerrtt__ccoonnttaaiinneerr__eeqquuaalliittyy (const std::map< size_t, V2 > &expected, │ │ │ │ │ - const std::map< size_t, V2 > &actual) │ │ │ │ │ -  Function for comparing maps of size_t->testable Types are assumed to have │ │ │ │ │ - operator ==. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -bool  ggttssaamm::::aasssseerrtt__ccoonnttaaiinneerr__eeqquuaalliittyy (const V &expected, const V &actual) │ │ │ │ │ -  General function for comparing containers of objects with operator==. │ │ │ │ │ -  │ │ │ │ │ -bool  ggttssaamm::::aasssseerrtt__eeqquuaall (const std::string &expected, const std::string │ │ │ │ │ - &actual) │ │ │ │ │ -  Compare strings for unit tests. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -bool  ggttssaamm::::aasssseerrtt__iinneeqquuaall (const V &expected, const V &actual, double tol=1e- │ │ │ │ │ - 9) │ │ │ │ │ -  Allow for testing inequality. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -bool  ggttssaamm::::aasssseerrtt__ssttddoouutt__eeqquuaall (const std::string &expected, const V &actual) │ │ │ │ │ -  Capture std out via cout stream and compare against string. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -bool  _g_t_s_a_m_:_:_a_s_s_e_r_t___p_r_i_n_t___e_q_u_a_l (const std::string &expected, const V &actual, │ │ │ │ │ - const std::string &s="") │ │ │ │ │ -  Capture print function output and compare against string. │ │ │ │ │ + double  ggttssaamm::::iinnnneerr__pprroodd (const V1 &a, const V2 &b) │ │ │ │ │ +  compatibility version for ublas' _i_n_n_e_r___p_r_o_d_(_) │ │ │ │ │ +  │ │ │ │ │ +pair< double, Vector >  _g_t_s_a_m_:_:_h_o_u_s_e (const Vector &x) │ │ │ │ │ + house(x,j) computes HouseHolder vector v and scaling │ │ │ │ │ +  factor beta from x, such that the corresponding │ │ │ │ │ + Householder reflection zeroes out all but x. │ │ │ │ │ +  │ │ │ │ │ + double  ggttssaamm::::hhoouusseeIInnPPllaaccee (Vector &x) │ │ │ │ │ +  beta = house(x) computes the HouseHolder vector in │ │ │ │ │ + place │ │ │ │ │ +  │ │ │ │ │ +pair< Vector, double >  _g_t_s_a_m_:_:_w_e_i_g_h_t_e_d_P_s_e_u_d_o_i_n_v_e_r_s_e (const Vector &v, const │ │ │ │ │ + Vector &weights) │ │ │ │ │ + Weighted Householder solution vector, a.k.a., the │ │ │ │ │ +  pseudoinverse of the column NOTE: if any sigmas are │ │ │ │ │ + zero (indicating a constraint) the pseudoinverse will │ │ │ │ │ + be a selection vector, and the variance will be zero. │ │ │ │ │ +  │ │ │ │ │ + double  ggttssaamm::::wweeiigghhtteeddPPsseeuuddooiinnvveerrssee (const Vector &a, const │ │ │ │ │ + Vector &weights, Vector &pseudo) │ │ │ │ │ +  │ │ │ │ │ + Vector  ggttssaamm::::ccoonnccaattVVeeccttoorrss (const std::list< Vector > &vs) │ │ │ │ │ +  concatenate Vectors │ │ │ │ │ +  │ │ │ │ │ + Vector  ggttssaamm::::ccoonnccaattVVeeccttoorrss (size_t nrVectors,...) │ │ │ │ │ +  concatenate Vectors │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Provides additional testing facilities for common data structures. │ │ │ │ │ +typedef and functions to augment Eigen's VectorXd │ │ │ │ │ Author │ │ │ │ │ - Alex Cunningham │ │ │ │ │ + Kai Ni │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Alex Hagiopol │ │ │ │ │ + Varun Agrawal │ │ │ │ │ +********** MMaaccrroo DDeeffiinniittiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ +********** _?◆_? GGTTSSAAMM__MMAAKKEE__VVEECCTTOORR__DDEEFFSS ********** │ │ │ │ │ +#define GTSAM_MAKE_VECTOR_DEFS (   N ) │ │ │ │ │ +VVaalluuee:: │ │ │ │ │ +using Vector##N = Eigen::Matrix; \ │ │ │ │ │ +static const Eigen::MatrixBase::ConstantReturnType Z_##N##x1 = │ │ │ │ │ +Vector##N::Zero(); │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _T_e_s_t_a_b_l_e_A_s_s_e_r_t_i_o_n_s_._h │ │ │ │ │ + * _V_e_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00014.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,14 +1,23 @@ │ │ │ │ │ var a00014 = [ │ │ │ │ │ - ["assert_container_equal", "a00014.html#a2f509195ea2180d0f7dbd3a99a088ff4", null], │ │ │ │ │ - ["assert_container_equal", "a00014.html#a254be27d6d4b416fa2b546c77ae783fc", null], │ │ │ │ │ - ["assert_container_equal", "a00014.html#a0eec17f894b358dd9f30d7af28082ba5", null], │ │ │ │ │ - ["assert_container_equal", "a00014.html#a6d6b2964354593f69848f6bf5d3e0ca1", null], │ │ │ │ │ - ["assert_container_equality", "a00014.html#a59ba6a7db27344c5cb021f1421905020", null], │ │ │ │ │ - ["assert_container_equality", "a00014.html#aa0926a5d779171bd8e1d30fb5982b5c0", null], │ │ │ │ │ - ["assert_equal", "a00014.html#add6d6575582314cc9b5dbdbe2a86374d", null], │ │ │ │ │ - ["assert_equal", "a00014.html#a982034802415eb6e9bd02355257ed96a", null], │ │ │ │ │ - ["assert_equal", "a00014.html#a21e760bc75888053afd86a27d56b6148", null], │ │ │ │ │ - ["assert_inequal", "a00014.html#a2f19ba6625a264457805513fefcb5c32", null], │ │ │ │ │ - ["assert_print_equal", "a00014.html#a830c8c65902d7d0e763562e6c9357346", null], │ │ │ │ │ - ["assert_stdout_equal", "a00014.html#a08641f0f7145716bba9159dd95099a44", null] │ │ │ │ │ + ["assert_equal", "a00014.html#ac3cf8e8e70cd696d9c789c31bc1e89a7", null], │ │ │ │ │ + ["assert_equal", "a00014.html#a9f3ee662d25ffb8c04b4e35c4b02e90b", null], │ │ │ │ │ + ["assert_inequal", "a00014.html#a2ca3db47b15350977c1f03c5560ab332", null], │ │ │ │ │ + ["concatVectors", "a00014.html#ae1c88acfa9575bcea3f2e738cb637a85", null], │ │ │ │ │ + ["concatVectors", "a00014.html#a09610ec69a8b3f3a00399bb378eda9cf", null], │ │ │ │ │ + ["dot", "a00014.html#ad80249acf12bbea741e755cd8fc73042", null], │ │ │ │ │ + ["ediv_", "a00014.html#a14844c96706dc362917e0d5e76d65fbe", null], │ │ │ │ │ + ["equal", "a00014.html#a3d1cad2313f69f9fa5008fdc348d1526", null], │ │ │ │ │ + ["equal", "a00014.html#aa7ed1d4fe2dd6828cfeb2d8f488b818d", null], │ │ │ │ │ + ["equal_with_abs_tol", "a00014.html#a8bdced1844ffd6a53af0396d82eaa6da", null], │ │ │ │ │ + ["fpEqual", "a00014.html#ae19c359190a379cba44bf818a2293f7c", null], │ │ │ │ │ + ["greaterThanOrEqual", "a00014.html#a368ee40bd9c0124d572d2e49bcb077bf", null], │ │ │ │ │ + ["house", "a00014.html#afb77eefc25dea0fb3e0fcc8f246b617a", null], │ │ │ │ │ + ["houseInPlace", "a00014.html#ac9c0c05b0198ec0fb6e4115edb755910", null], │ │ │ │ │ + ["inner_prod", "a00014.html#a4ca41ba9ec9d6d21a2b9f5e00f9f25ed", null], │ │ │ │ │ + ["linear_dependent", "a00014.html#a2a0cfd7908b06491df49b6a9c9186775", null], │ │ │ │ │ + ["operator==", "a00014.html#a64988014ab746343803620dc42513646", null], │ │ │ │ │ + ["print", "a00014.html#a760cc36e9009b23cf564c436294b3504", null], │ │ │ │ │ + ["print", "a00014.html#a5e5d8018a0f44a6a299406aeda27d44b", null], │ │ │ │ │ + ["save", "a00014.html#a09f2bbdb9f9d633542362dbe8d79f9ab", null], │ │ │ │ │ + ["weightedPseudoinverse", "a00014.html#aca1c56d11a05464a7a5458dc32ccc777", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00014_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/TestableAssertions.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Vector.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,399 +98,192 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
TestableAssertions.h
│ │ │ │ +
Vector.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/base/Testable.h>
│ │ │ │ -
21#include <gtsam/global_includes.h>
│ │ │ │ +
21// \callgraph
│ │ │ │
22
│ │ │ │ -
23#include <boost/optional.hpp>
│ │ │ │ -
24#include <map>
│ │ │ │ -
25#include <iostream>
│ │ │ │ -
26#include <sstream>
│ │ │ │ -
27#include <vector>
│ │ │ │ -
28
│ │ │ │ -
29namespace gtsam {
│ │ │ │ -
30
│ │ │ │ -
│ │ │ │ -
34inline bool assert_equal(const Key& expected, const Key& actual, double tol = 0.0) {
│ │ │ │ -
35 if(expected != actual) {
│ │ │ │ -
36 std::cout << "Not equal:\nexpected: " << expected << "\nactual: " << actual << std::endl;
│ │ │ │ -
37 return false;
│ │ │ │ -
38 }
│ │ │ │ -
39 return true;
│ │ │ │ -
40}
│ │ │ │ -
│ │ │ │ -
41
│ │ │ │ -
49template<class V>
│ │ │ │ -
│ │ │ │ -
50bool assert_equal(const boost::optional<V>& expected,
│ │ │ │ -
51 const boost::optional<V>& actual, double tol = 1e-9) {
│ │ │ │ -
52 if (!expected && actual) {
│ │ │ │ -
53 std::cout << "expected is boost::none, while actual is not" << std::endl;
│ │ │ │ -
54 return false;
│ │ │ │ -
55 }
│ │ │ │ -
56 if (expected && !actual) {
│ │ │ │ -
57 std::cout << "actual is boost::none, while expected is not" << std::endl;
│ │ │ │ -
58 return false;
│ │ │ │ -
59 }
│ │ │ │ -
60 if (!expected && !actual)
│ │ │ │ -
61 return true;
│ │ │ │ -
62 return assert_equal(*expected, *actual, tol);
│ │ │ │ -
63}
│ │ │ │ -
│ │ │ │ +
23#pragma once
│ │ │ │ +
24#ifndef MKL_BLAS
│ │ │ │ +
25#define MKL_BLAS MKL_DOMAIN_BLAS
│ │ │ │ +
26#endif
│ │ │ │ +
27
│ │ │ │ +
28#include <gtsam/global_includes.h>
│ │ │ │ +
29#include <Eigen/Core>
│ │ │ │ +
30#include <iosfwd>
│ │ │ │ +
31#include <list>
│ │ │ │ +
32
│ │ │ │ +
33namespace gtsam {
│ │ │ │ +
34
│ │ │ │ +
35// Vector is just a typedef of the Eigen dynamic vector type
│ │ │ │ +
36
│ │ │ │ +
37// Typedef arbitary length vector
│ │ │ │ +
38typedef Eigen::VectorXd Vector;
│ │ │ │ +
39
│ │ │ │ +
40// Commonly used fixed size vectors
│ │ │ │ +
41typedef Eigen::Matrix<double, 1, 1> Vector1;
│ │ │ │ +
42typedef Eigen::Vector2d Vector2;
│ │ │ │ +
43typedef Eigen::Vector3d Vector3;
│ │ │ │ +
44
│ │ │ │ +
45static const Eigen::MatrixBase<Vector2>::ConstantReturnType Z_2x1 = Vector2::Zero();
│ │ │ │ +
46static const Eigen::MatrixBase<Vector3>::ConstantReturnType Z_3x1 = Vector3::Zero();
│ │ │ │ +
47
│ │ │ │ +
48// Create handy typedefs and constants for vectors with N>3
│ │ │ │ +
49// VectorN and Z_Nx1, for N=1..9
│ │ │ │ +
50#define GTSAM_MAKE_VECTOR_DEFS(N) \
│ │ │ │ +
51 using Vector##N = Eigen::Matrix<double, N, 1>; \
│ │ │ │ +
52 static const Eigen::MatrixBase<Vector##N>::ConstantReturnType Z_##N##x1 = Vector##N::Zero();
│ │ │ │ +
53
│ │ │ │ +
54GTSAM_MAKE_VECTOR_DEFS(4)
│ │ │ │ +
55GTSAM_MAKE_VECTOR_DEFS(5)
│ │ │ │ +
56GTSAM_MAKE_VECTOR_DEFS(6)
│ │ │ │ +
57GTSAM_MAKE_VECTOR_DEFS(7)
│ │ │ │ +
58GTSAM_MAKE_VECTOR_DEFS(8)
│ │ │ │ +
59GTSAM_MAKE_VECTOR_DEFS(9)
│ │ │ │ +
60GTSAM_MAKE_VECTOR_DEFS(10)
│ │ │ │ +
61GTSAM_MAKE_VECTOR_DEFS(11)
│ │ │ │ +
62GTSAM_MAKE_VECTOR_DEFS(12)
│ │ │ │ +
63GTSAM_MAKE_VECTOR_DEFS(15)
│ │ │ │
64
│ │ │ │ -
65template<class V>
│ │ │ │ -
66bool assert_equal(const V& expected, const boost::optional<V>& actual, double tol = 1e-9) {
│ │ │ │ -
67 if (!actual) {
│ │ │ │ -
68 std::cout << "actual is boost::none" << std::endl;
│ │ │ │ -
69 return false;
│ │ │ │ -
70 }
│ │ │ │ -
71 return assert_equal(expected, *actual, tol);
│ │ │ │ -
72}
│ │ │ │ -
73
│ │ │ │ -
74template<class V>
│ │ │ │ -
75bool assert_equal(const V& expected, const boost::optional<const V&>& actual, double tol = 1e-9) {
│ │ │ │ -
76 if (!actual) {
│ │ │ │ -
77 std::cout << "actual is boost::none" << std::endl;
│ │ │ │ -
78 return false;
│ │ │ │ -
79 }
│ │ │ │ -
80 return assert_equal(expected, *actual, tol);
│ │ │ │ -
81}
│ │ │ │ -
82
│ │ │ │ -
83#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ -
88template<class V>
│ │ │ │ -
89bool GTSAM_DEPRECATED assert_equal(const std::vector<V>& expected, const std::vector<V>& actual, double tol = 1e-9) {
│ │ │ │ -
90 bool match = true;
│ │ │ │ -
91 if (expected.size() != actual.size())
│ │ │ │ -
92 match = false;
│ │ │ │ -
93 if(match) {
│ │ │ │ -
94 size_t i = 0;
│ │ │ │ -
95 for(const V& a: expected) {
│ │ │ │ -
96 if (!assert_equal(a, actual[i++], tol)) {
│ │ │ │ -
97 match = false;
│ │ │ │ -
98 break;
│ │ │ │ -
99 }
│ │ │ │ -
100 }
│ │ │ │ -
101 }
│ │ │ │ -
102 if(!match) {
│ │ │ │ -
103 std::cout << "expected: " << std::endl;
│ │ │ │ -
104 for(const V& a: expected) { std::cout << a << " "; }
│ │ │ │ -
105 std::cout << "\nactual: " << std::endl;
│ │ │ │ -
106 for(const V& a: actual) { std::cout << a << " "; }
│ │ │ │ -
107 std::cout << std::endl;
│ │ │ │ -
108 return false;
│ │ │ │ -
109 }
│ │ │ │ -
110 return true;
│ │ │ │ -
111}
│ │ │ │ -
112#endif
│ │ │ │ +
65typedef Eigen::VectorBlock<Vector> SubVector;
│ │ │ │ +
66typedef Eigen::VectorBlock<const Vector> ConstSubVector;
│ │ │ │ +
67
│ │ │ │ +
73#if defined(GTSAM_EIGEN_VERSION_WORLD)
│ │ │ │ +
74static_assert(
│ │ │ │ +
75 GTSAM_EIGEN_VERSION_WORLD==EIGEN_WORLD_VERSION &&
│ │ │ │ +
76 GTSAM_EIGEN_VERSION_MAJOR==EIGEN_MAJOR_VERSION,
│ │ │ │ +
77 "Error: GTSAM was built against a different version of Eigen");
│ │ │ │ +
78#endif
│ │ │ │ +
79
│ │ │ │ +
96GTSAM_EXPORT bool fpEqual(double a, double b, double tol,
│ │ │ │ +
97 bool check_relative_also = true);
│ │ │ │ +
98
│ │ │ │ +
102GTSAM_EXPORT void print(const Vector& v, const std::string& s, std::ostream& stream);
│ │ │ │ +
103
│ │ │ │ +
107GTSAM_EXPORT void print(const Vector& v, const std::string& s = "");
│ │ │ │ +
108
│ │ │ │ +
112GTSAM_EXPORT void save(const Vector& A, const std::string &s, const std::string& filename);
│ │ │ │
113
│ │ │ │ -
118template<class V1, class V2>
│ │ │ │ -
│ │ │ │ -
119bool assert_container_equal(const std::map<V1,V2>& expected, const std::map<V1,V2>& actual, double tol = 1e-9) {
│ │ │ │ -
120 typedef typename std::map<V1,V2> Map;
│ │ │ │ -
121 bool match = true;
│ │ │ │ -
122 if (expected.size() != actual.size())
│ │ │ │ -
123 match = false;
│ │ │ │ -
124 typename Map::const_iterator
│ │ │ │ -
125 itExp = expected.begin(),
│ │ │ │ -
126 itAct = actual.begin();
│ │ │ │ -
127 if(match) {
│ │ │ │ -
128 for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) {
│ │ │ │ -
129 if (!assert_equal(itExp->first, itAct->first, tol) ||
│ │ │ │ -
130 !assert_equal(itExp->second, itAct->second, tol)) {
│ │ │ │ -
131 match = false;
│ │ │ │ -
132 break;
│ │ │ │ -
133 }
│ │ │ │ -
134 }
│ │ │ │ -
135 }
│ │ │ │ -
136 if(!match) {
│ │ │ │ -
137 std::cout << "expected: " << std::endl;
│ │ │ │ -
138 for(const typename Map::value_type& a: expected) {
│ │ │ │ -
139 a.first.print("key");
│ │ │ │ -
140 a.second.print(" value");
│ │ │ │ -
141 }
│ │ │ │ -
142 std::cout << "\nactual: " << std::endl;
│ │ │ │ -
143 for(const typename Map::value_type& a: actual) {
│ │ │ │ -
144 a.first.print("key");
│ │ │ │ -
145 a.second.print(" value");
│ │ │ │ -
146 }
│ │ │ │ -
147 std::cout << std::endl;
│ │ │ │ -
148 return false;
│ │ │ │ -
149 }
│ │ │ │ -
150 return true;
│ │ │ │ -
151}
│ │ │ │ -
│ │ │ │ -
152
│ │ │ │ -
156template<class V2>
│ │ │ │ -
│ │ │ │ -
157bool assert_container_equal(const std::map<size_t,V2>& expected, const std::map<size_t,V2>& actual, double tol = 1e-9) {
│ │ │ │ -
158 typedef typename std::map<size_t,V2> Map;
│ │ │ │ -
159 bool match = true;
│ │ │ │ -
160 if (expected.size() != actual.size())
│ │ │ │ -
161 match = false;
│ │ │ │ -
162 typename Map::const_iterator
│ │ │ │ -
163 itExp = expected.begin(),
│ │ │ │ -
164 itAct = actual.begin();
│ │ │ │ -
165 if(match) {
│ │ │ │ -
166 for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) {
│ │ │ │ -
167 if (itExp->first != itAct->first ||
│ │ │ │ -
168 !assert_equal(itExp->second, itAct->second, tol)) {
│ │ │ │ -
169 match = false;
│ │ │ │ -
170 break;
│ │ │ │ -
171 }
│ │ │ │ -
172 }
│ │ │ │ -
173 }
│ │ │ │ -
174 if(!match) {
│ │ │ │ -
175 std::cout << "expected: " << std::endl;
│ │ │ │ -
176 for(const typename Map::value_type& a: expected) {
│ │ │ │ -
177 std::cout << "Key: " << a.first << std::endl;
│ │ │ │ -
178 a.second.print(" value");
│ │ │ │ -
179 }
│ │ │ │ -
180 std::cout << "\nactual: " << std::endl;
│ │ │ │ -
181 for(const typename Map::value_type& a: actual) {
│ │ │ │ -
182 std::cout << "Key: " << a.first << std::endl;
│ │ │ │ -
183 a.second.print(" value");
│ │ │ │ -
184 }
│ │ │ │ -
185 std::cout << std::endl;
│ │ │ │ -
186 return false;
│ │ │ │ -
187 }
│ │ │ │ -
188 return true;
│ │ │ │ -
189}
│ │ │ │ -
│ │ │ │ +
117GTSAM_EXPORT bool operator==(const Vector& vec1,const Vector& vec2);
│ │ │ │ +
118
│ │ │ │ +
124GTSAM_EXPORT bool greaterThanOrEqual(const Vector& v1, const Vector& v2);
│ │ │ │ +
125
│ │ │ │ +
129GTSAM_EXPORT bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol=1e-9);
│ │ │ │ +
130GTSAM_EXPORT bool equal_with_abs_tol(const SubVector& vec1, const SubVector& vec2, double tol=1e-9);
│ │ │ │ +
131
│ │ │ │ +
│ │ │ │ +
135inline bool equal(const Vector& vec1, const Vector& vec2, double tol) {
│ │ │ │ +
136 return equal_with_abs_tol(vec1, vec2, tol);
│ │ │ │ +
137}
│ │ │ │ +
│ │ │ │ +
138
│ │ │ │ +
│ │ │ │ +
142inline bool equal(const Vector& vec1, const Vector& vec2) {
│ │ │ │ +
143 return equal_with_abs_tol(vec1, vec2);
│ │ │ │ +
144}
│ │ │ │ +
│ │ │ │ +
145
│ │ │ │ +
153GTSAM_EXPORT bool assert_equal(const Vector& vec1, const Vector& vec2, double tol=1e-9);
│ │ │ │ +
154
│ │ │ │ +
162GTSAM_EXPORT bool assert_inequal(const Vector& vec1, const Vector& vec2, double tol=1e-9);
│ │ │ │ +
163
│ │ │ │ +
171GTSAM_EXPORT bool assert_equal(const SubVector& vec1, const SubVector& vec2, double tol=1e-9);
│ │ │ │ +
172GTSAM_EXPORT bool assert_equal(const ConstSubVector& vec1, const ConstSubVector& vec2, double tol=1e-9);
│ │ │ │ +
173
│ │ │ │ +
181GTSAM_EXPORT bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol=1e-9);
│ │ │ │ +
182
│ │ │ │ +
189GTSAM_EXPORT Vector ediv_(const Vector &a, const Vector &b);
│ │ │ │
190
│ │ │ │
194template<class V1, class V2>
│ │ │ │
│ │ │ │ -
195bool assert_container_equal(const std::vector<std::pair<V1,V2> >& expected,
│ │ │ │ -
196 const std::vector<std::pair<V1,V2> >& actual, double tol = 1e-9) {
│ │ │ │ -
197 typedef typename std::vector<std::pair<V1,V2> > VectorPair;
│ │ │ │ -
198 bool match = true;
│ │ │ │ -
199 if (expected.size() != actual.size())
│ │ │ │ -
200 match = false;
│ │ │ │ -
201 typename VectorPair::const_iterator
│ │ │ │ -
202 itExp = expected.begin(),
│ │ │ │ -
203 itAct = actual.begin();
│ │ │ │ -
204 if(match) {
│ │ │ │ -
205 for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) {
│ │ │ │ -
206 if (!assert_equal(itExp->first, itAct->first, tol) ||
│ │ │ │ -
207 !assert_equal(itExp->second, itAct->second, tol)) {
│ │ │ │ -
208 match = false;
│ │ │ │ -
209 break;
│ │ │ │ -
210 }
│ │ │ │ -
211 }
│ │ │ │ -
212 }
│ │ │ │ -
213 if(!match) {
│ │ │ │ -
214 std::cout << "expected: " << std::endl;
│ │ │ │ -
215 for(const typename VectorPair::value_type& a: expected) {
│ │ │ │ -
216 a.first.print( " first ");
│ │ │ │ -
217 a.second.print(" second");
│ │ │ │ -
218 }
│ │ │ │ -
219 std::cout << "\nactual: " << std::endl;
│ │ │ │ -
220 for(const typename VectorPair::value_type& a: actual) {
│ │ │ │ -
221 a.first.print( " first ");
│ │ │ │ -
222 a.second.print(" second");
│ │ │ │ -
223 }
│ │ │ │ -
224 std::cout << std::endl;
│ │ │ │ -
225 return false;
│ │ │ │ -
226 }
│ │ │ │ -
227 return true;
│ │ │ │ -
228}
│ │ │ │ -
│ │ │ │ -
229
│ │ │ │ -
230
│ │ │ │ -
234template<class V>
│ │ │ │ -
│ │ │ │ -
235bool assert_container_equal(const V& expected, const V& actual, double tol = 1e-9) {
│ │ │ │ -
236 bool match = true;
│ │ │ │ -
237 typename V::const_iterator
│ │ │ │ -
238 itExp = expected.begin(),
│ │ │ │ -
239 itAct = actual.begin();
│ │ │ │ -
240 if(match) {
│ │ │ │ -
241 for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) {
│ │ │ │ -
242 if (!assert_equal(*itExp, *itAct, tol)) {
│ │ │ │ -
243 match = false;
│ │ │ │ -
244 break;
│ │ │ │ -
245 }
│ │ │ │ -
246 }
│ │ │ │ -
247 if(itExp != expected.end() || itAct != actual.end())
│ │ │ │ -
248 match = false;
│ │ │ │ -
249 }
│ │ │ │ -
250 if(!match) {
│ │ │ │ -
251 std::cout << "expected: " << std::endl;
│ │ │ │ -
252 for(const typename V::value_type& a: expected) { a.print(" "); }
│ │ │ │ -
253 std::cout << "\nactual: " << std::endl;
│ │ │ │ -
254 for(const typename V::value_type& a: actual) { a.print(" "); }
│ │ │ │ -
255 std::cout << std::endl;
│ │ │ │ -
256 return false;
│ │ │ │ -
257 }
│ │ │ │ -
258 return true;
│ │ │ │ -
259}
│ │ │ │ -
│ │ │ │ -
260
│ │ │ │ -
265template<class V2>
│ │ │ │ -
│ │ │ │ -
266bool assert_container_equality(const std::map<size_t,V2>& expected, const std::map<size_t,V2>& actual) {
│ │ │ │ -
267 typedef typename std::map<size_t,V2> Map;
│ │ │ │ -
268 bool match = true;
│ │ │ │ -
269 if (expected.size() != actual.size())
│ │ │ │ -
270 match = false;
│ │ │ │ -
271 typename Map::const_iterator
│ │ │ │ -
272 itExp = expected.begin(),
│ │ │ │ -
273 itAct = actual.begin();
│ │ │ │ -
274 if(match) {
│ │ │ │ -
275 for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) {
│ │ │ │ -
276 if (itExp->first != itAct->first || itExp->second != itAct->second) {
│ │ │ │ -
277 match = false;
│ │ │ │ -
278 break;
│ │ │ │ -
279 }
│ │ │ │ -
280 }
│ │ │ │ -
281 }
│ │ │ │ -
282 if(!match) {
│ │ │ │ -
283 std::cout << "expected: " << std::endl;
│ │ │ │ -
284 for(const typename Map::value_type& a: expected) {
│ │ │ │ -
285 std::cout << "Key: " << a.first << std::endl;
│ │ │ │ -
286 std::cout << "Value: " << a.second << std::endl;
│ │ │ │ -
287 }
│ │ │ │ -
288 std::cout << "\nactual: " << std::endl;
│ │ │ │ -
289 for(const typename Map::value_type& a: actual) {
│ │ │ │ -
290 std::cout << "Key: " << a.first << std::endl;
│ │ │ │ -
291 std::cout << "Value: " << a.second << std::endl;
│ │ │ │ -
292 }
│ │ │ │ -
293 std::cout << std::endl;
│ │ │ │ -
294 return false;
│ │ │ │ -
295 }
│ │ │ │ -
296 return true;
│ │ │ │ -
297}
│ │ │ │ -
│ │ │ │ -
298
│ │ │ │ -
299
│ │ │ │ -
303template<class V>
│ │ │ │ -
│ │ │ │ -
304bool assert_container_equality(const V& expected, const V& actual) {
│ │ │ │ -
305 bool match = true;
│ │ │ │ -
306 if (expected.size() != actual.size())
│ │ │ │ -
307 match = false;
│ │ │ │ -
308 typename V::const_iterator
│ │ │ │ -
309 itExp = expected.begin(),
│ │ │ │ -
310 itAct = actual.begin();
│ │ │ │ -
311 if(match) {
│ │ │ │ -
312 for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) {
│ │ │ │ -
313 if (*itExp != *itAct) {
│ │ │ │ -
314 match = false;
│ │ │ │ -
315 break;
│ │ │ │ -
316 }
│ │ │ │ -
317 }
│ │ │ │ -
318 }
│ │ │ │ -
319 if(!match) {
│ │ │ │ -
320 std::cout << "expected: " << std::endl;
│ │ │ │ -
321 for(const typename V::value_type& a: expected) { std::cout << a << " "; }
│ │ │ │ -
322 std::cout << "\nactual: " << std::endl;
│ │ │ │ -
323 for(const typename V::value_type& a: actual) { std::cout << a << " "; }
│ │ │ │ -
324 std::cout << std::endl;
│ │ │ │ -
325 return false;
│ │ │ │ -
326 }
│ │ │ │ -
327 return true;
│ │ │ │ -
328}
│ │ │ │ -
│ │ │ │ -
329
│ │ │ │ -
│ │ │ │ -
333inline bool assert_equal(const std::string& expected, const std::string& actual) {
│ │ │ │ -
334 if (expected == actual)
│ │ │ │ -
335 return true;
│ │ │ │ -
336 printf("Not equal:\n");
│ │ │ │ -
337 std::cout << "expected: [" << expected << "]\n";
│ │ │ │ -
338 std::cout << "actual: [" << actual << "]" << std::endl;
│ │ │ │ -
339 return false;
│ │ │ │ -
340}
│ │ │ │ -
│ │ │ │ -
341
│ │ │ │ -
345template<class V>
│ │ │ │ -
│ │ │ │ -
346bool assert_inequal(const V& expected, const V& actual, double tol = 1e-9) {
│ │ │ │ -
347 if (!actual.equals(expected, tol))
│ │ │ │ -
348 return true;
│ │ │ │ -
349 printf("Erroneously equal:\n");
│ │ │ │ -
350 expected.print("expected");
│ │ │ │ -
351 actual.print("actual");
│ │ │ │ -
352 return false;
│ │ │ │ -
353}
│ │ │ │ -
│ │ │ │ -
354
│ │ │ │ -
358template<class V>
│ │ │ │ -
│ │ │ │ -
359bool assert_stdout_equal(const std::string& expected, const V& actual) {
│ │ │ │ -
360 // Redirect output to buffer so we can compare
│ │ │ │ -
361 std::stringstream buffer;
│ │ │ │ -
362 // Save the original output stream so we can reset later
│ │ │ │ -
363 std::streambuf* old = std::cout.rdbuf(buffer.rdbuf());
│ │ │ │ -
364
│ │ │ │ -
365 // We test against actual std::cout for faithful reproduction
│ │ │ │ -
366 std::cout << actual;
│ │ │ │ -
367
│ │ │ │ -
368 // Get output string and reset stdout
│ │ │ │ -
369 std::string actual_ = buffer.str();
│ │ │ │ -
370 std::cout.rdbuf(old);
│ │ │ │ -
371
│ │ │ │ -
372 return assert_equal(expected, actual_);
│ │ │ │ -
373}
│ │ │ │ -
│ │ │ │ -
374
│ │ │ │ -
380template <class V>
│ │ │ │ -
│ │ │ │ -
381bool assert_print_equal(const std::string& expected, const V& actual,
│ │ │ │ -
382 const std::string& s = "") {
│ │ │ │ -
383 // Redirect output to buffer so we can compare
│ │ │ │ -
384 std::stringstream buffer;
│ │ │ │ -
385 // Save the original output stream so we can reset later
│ │ │ │ -
386 std::streambuf* old = std::cout.rdbuf(buffer.rdbuf());
│ │ │ │ -
387
│ │ │ │ -
388 // We test against actual std::cout for faithful reproduction
│ │ │ │ -
389 actual.print(s);
│ │ │ │ -
390
│ │ │ │ -
391 // Get output string and reset stdout
│ │ │ │ -
392 std::string actual_ = buffer.str();
│ │ │ │ -
393 std::cout.rdbuf(old);
│ │ │ │ -
394
│ │ │ │ -
395 return assert_equal(expected, actual_);
│ │ │ │ -
396}
│ │ │ │ -
│ │ │ │ -
397
│ │ │ │ -
398} // \namespace gtsam
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ -
global_includes.h
Included from all GTSAM files.
│ │ │ │ +
195inline double dot(const V1 &a, const V2& b) {
│ │ │ │ +
196 assert (b.size()==a.size());
│ │ │ │ +
197 return a.dot(b);
│ │ │ │ +
198}
│ │ │ │ +
│ │ │ │ +
199
│ │ │ │ +
201template<class V1, class V2>
│ │ │ │ +
│ │ │ │ +
202inline double inner_prod(const V1 &a, const V2& b) {
│ │ │ │ +
203 assert (b.size()==a.size());
│ │ │ │ +
204 return a.dot(b);
│ │ │ │ +
205}
│ │ │ │ +
│ │ │ │ +
206
│ │ │ │ +
207#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ +
212inline void GTSAM_DEPRECATED scal(double alpha, Vector& x) { x *= alpha; }
│ │ │ │ +
213
│ │ │ │ +
218template<class V1, class V2>
│ │ │ │ +
219inline void GTSAM_DEPRECATED axpy(double alpha, const V1& x, V2& y) {
│ │ │ │ +
220 assert (y.size()==x.size());
│ │ │ │ +
221 y += alpha * x;
│ │ │ │ +
222}
│ │ │ │ +
223inline void axpy(double alpha, const Vector& x, SubVector y) {
│ │ │ │ +
224 assert (y.size()==x.size());
│ │ │ │ +
225 y += alpha * x;
│ │ │ │ +
226}
│ │ │ │ +
227#endif
│ │ │ │ +
228
│ │ │ │ +
234GTSAM_EXPORT std::pair<double,Vector> house(const Vector &x);
│ │ │ │ +
235
│ │ │ │ +
237GTSAM_EXPORT double houseInPlace(Vector &x);
│ │ │ │ +
238
│ │ │ │ +
249GTSAM_EXPORT std::pair<Vector, double>
│ │ │ │ +
250weightedPseudoinverse(const Vector& v, const Vector& weights);
│ │ │ │ +
251
│ │ │ │ +
252/*
│ │ │ │ +
253 * Fast version *no error checking* !
│ │ │ │ +
254 * Pass in initialized vector pseudo of size(weights) or will crash !
│ │ │ │ +
255 * @return the precision, pseudoinverse in third argument
│ │ │ │ +
256 */
│ │ │ │ +
257GTSAM_EXPORT double weightedPseudoinverse(const Vector& a, const Vector& weights, Vector& pseudo);
│ │ │ │ +
258
│ │ │ │ +
262GTSAM_EXPORT Vector concatVectors(const std::list<Vector>& vs);
│ │ │ │ +
263
│ │ │ │ +
267GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...);
│ │ │ │ +
268} // namespace gtsam
│ │ │ │ +
global_includes.h
Included from all GTSAM files.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::assert_stdout_equal
bool assert_stdout_equal(const std::string &expected, const V &actual)
Capture std out via cout stream and compare against string.
Definition TestableAssertions.h:359
│ │ │ │ -
gtsam::assert_container_equal
bool assert_container_equal(const std::map< V1, V2 > &expected, const std::map< V1, V2 > &actual, double tol=1e-9)
Function for comparing maps of testable->testable TODO: replace with more generalized version.
Definition TestableAssertions.h:119
│ │ │ │ +
gtsam::ediv_
Vector ediv_(const Vector &a, const Vector &b)
elementwise division, but 0/0 = 0, not inf
Definition Vector.cpp:199
│ │ │ │ +
gtsam::greaterThanOrEqual
bool greaterThanOrEqual(const Vector &vec1, const Vector &vec2)
Greater than or equal to operation returns true if all elements in v1 are greater than corresponding ...
Definition Vector.cpp:114
│ │ │ │ +
gtsam::save
void save(const Matrix &A, const string &s, const string &filename)
save a matrix to file, which can be loaded by matlab
Definition Matrix.cpp:167
│ │ │ │
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
equals with an tolerance, prints out message if unequal
Definition Matrix.cpp:43
│ │ │ │ -
gtsam::assert_container_equality
bool assert_container_equality(const std::map< size_t, V2 > &expected, const std::map< size_t, V2 > &actual)
Function for comparing maps of size_t->testable Types are assumed to have operator ==.
Definition TestableAssertions.h:266
│ │ │ │ +
gtsam::linear_dependent
bool linear_dependent(const Matrix &A, const Matrix &B, double tol)
check whether the rows of two matrices are linear dependent
Definition Matrix.cpp:117
│ │ │ │ +
gtsam::inner_prod
double inner_prod(const V1 &a, const V2 &b)
compatibility version for ublas' inner_prod()
Definition Vector.h:202
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::axpy
void axpy(double alpha, const Errors &x, Errors &y)
BLAS level 2 style AXPY, y := alpha*x + y
Definition Errors.cpp:111
│ │ │ │
gtsam::assert_inequal
bool assert_inequal(const Matrix &A, const Matrix &B, double tol)
inequals with an tolerance, prints out message if within tolerance
Definition Matrix.cpp:63
│ │ │ │ -
gtsam::assert_print_equal
bool assert_print_equal(const std::string &expected, const V &actual, const std::string &s="")
Capture print function output and compare against string.
Definition TestableAssertions.h:381
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::houseInPlace
double houseInPlace(Vector &v)
beta = house(x) computes the HouseHolder vector in place
Definition Vector.cpp:212
│ │ │ │ +
gtsam::equal
bool equal(const T &obj1, const T &obj2, double tol)
Call equal on the object.
Definition Testable.h:84
│ │ │ │ +
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
│ │ │ │ +
gtsam::fpEqual
bool fpEqual(double a, double b, double tol, bool check_relative_also)
Ensure we are not including a different version of Eigen in user code than while compiling gtsam,...
Definition Vector.cpp:42
│ │ │ │ +
gtsam::concatVectors
Vector concatVectors(const std::list< Vector > &vs)
concatenate Vectors
Definition Vector.cpp:302
│ │ │ │ +
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition Matrix.h:81
│ │ │ │ +
gtsam::house
pair< double, Vector > house(const Vector &x)
house(x,j) computes HouseHolder vector v and scaling factor beta from x, such that the corresponding ...
Definition Vector.cpp:237
│ │ │ │ +
gtsam::operator==
bool operator==(const Matrix &A, const Matrix &B)
equality is just equal_with_abs_tol 1e-9
Definition Matrix.h:100
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,407 +1,247 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -TestableAssertions.h │ │ │ │ │ +Vector.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ +21// \callgraph │ │ │ │ │ 22 │ │ │ │ │ -23#include │ │ │ │ │ -24#include │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ -28 │ │ │ │ │ -29namespace _g_t_s_a_m { │ │ │ │ │ -30 │ │ │ │ │ -_3_4inline bool _a_s_s_e_r_t___e_q_u_a_l(const _K_e_y& expected, const _K_e_y& actual, double tol = │ │ │ │ │ -0.0) { │ │ │ │ │ -35 if(expected != actual) { │ │ │ │ │ -36 std::cout << "Not equal:\nexpected: " << expected << "\nactual: " << actual │ │ │ │ │ -<< std::endl; │ │ │ │ │ -37 return false; │ │ │ │ │ -38 } │ │ │ │ │ -39 return true; │ │ │ │ │ -40} │ │ │ │ │ -41 │ │ │ │ │ -49template │ │ │ │ │ -_5_0bool _a_s_s_e_r_t___e_q_u_a_l(const boost::optional& expected, │ │ │ │ │ -51 const boost::optional& actual, double tol = 1e-9) { │ │ │ │ │ -52 if (!expected && actual) { │ │ │ │ │ -53 std::cout << "expected is boost::none, while actual is not" << std::endl; │ │ │ │ │ -54 return false; │ │ │ │ │ -55 } │ │ │ │ │ -56 if (expected && !actual) { │ │ │ │ │ -57 std::cout << "actual is boost::none, while expected is not" << std::endl; │ │ │ │ │ -58 return false; │ │ │ │ │ -59 } │ │ │ │ │ -60 if (!expected && !actual) │ │ │ │ │ -61 return true; │ │ │ │ │ -62 return _a_s_s_e_r_t___e_q_u_a_l(*expected, *actual, tol); │ │ │ │ │ -63} │ │ │ │ │ +23#pragma once │ │ │ │ │ +24#ifndef MKL_BLAS │ │ │ │ │ +25#define MKL_BLAS MKL_DOMAIN_BLAS │ │ │ │ │ +26#endif │ │ │ │ │ +27 │ │ │ │ │ +28#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ +29#include │ │ │ │ │ +30#include │ │ │ │ │ +31#include │ │ │ │ │ +32 │ │ │ │ │ +33namespace _g_t_s_a_m { │ │ │ │ │ +34 │ │ │ │ │ +35// Vector is just a typedef of the Eigen dynamic vector type │ │ │ │ │ +36 │ │ │ │ │ +37// Typedef arbitary length vector │ │ │ │ │ +38typedef Eigen::VectorXd Vector; │ │ │ │ │ +39 │ │ │ │ │ +40// Commonly used fixed size vectors │ │ │ │ │ +41typedef Eigen::Matrix Vector1; │ │ │ │ │ +42typedef Eigen::Vector2d Vector2; │ │ │ │ │ +43typedef Eigen::Vector3d Vector3; │ │ │ │ │ +44 │ │ │ │ │ +45static const Eigen::MatrixBase::ConstantReturnType Z_2x1 = Vector2:: │ │ │ │ │ +Zero(); │ │ │ │ │ +46static const Eigen::MatrixBase::ConstantReturnType Z_3x1 = Vector3:: │ │ │ │ │ +Zero(); │ │ │ │ │ +47 │ │ │ │ │ +48// Create handy typedefs and constants for vectors with N>3 │ │ │ │ │ +49// VectorN and Z_Nx1, for N=1..9 │ │ │ │ │ +50#define GTSAM_MAKE_VECTOR_DEFS(N) \ │ │ │ │ │ +51 using Vector##N = Eigen::Matrix; \ │ │ │ │ │ +52 static const Eigen::MatrixBase::ConstantReturnType Z_##N##x1 = │ │ │ │ │ +Vector##N::Zero(); │ │ │ │ │ +53 │ │ │ │ │ +54GTSAM_MAKE_VECTOR_DEFS(4) │ │ │ │ │ +55GTSAM_MAKE_VECTOR_DEFS(5) │ │ │ │ │ +56GTSAM_MAKE_VECTOR_DEFS(6) │ │ │ │ │ +57GTSAM_MAKE_VECTOR_DEFS(7) │ │ │ │ │ +58GTSAM_MAKE_VECTOR_DEFS(8) │ │ │ │ │ +59GTSAM_MAKE_VECTOR_DEFS(9) │ │ │ │ │ +60GTSAM_MAKE_VECTOR_DEFS(10) │ │ │ │ │ +61GTSAM_MAKE_VECTOR_DEFS(11) │ │ │ │ │ +62GTSAM_MAKE_VECTOR_DEFS(12) │ │ │ │ │ +63GTSAM_MAKE_VECTOR_DEFS(15) │ │ │ │ │ 64 │ │ │ │ │ -65template │ │ │ │ │ -66bool _a_s_s_e_r_t___e_q_u_a_l(const V& expected, const boost::optional& actual, double │ │ │ │ │ -tol = 1e-9) { │ │ │ │ │ -67 if (!actual) { │ │ │ │ │ -68 std::cout << "actual is boost::none" << std::endl; │ │ │ │ │ -69 return false; │ │ │ │ │ -70 } │ │ │ │ │ -71 return _a_s_s_e_r_t___e_q_u_a_l(expected, *actual, tol); │ │ │ │ │ -72} │ │ │ │ │ -73 │ │ │ │ │ -74template │ │ │ │ │ -75bool _a_s_s_e_r_t___e_q_u_a_l(const V& expected, const boost::optional& actual, │ │ │ │ │ -double tol = 1e-9) { │ │ │ │ │ -76 if (!actual) { │ │ │ │ │ -77 std::cout << "actual is boost::none" << std::endl; │ │ │ │ │ -78 return false; │ │ │ │ │ -79 } │ │ │ │ │ -80 return _a_s_s_e_r_t___e_q_u_a_l(expected, *actual, tol); │ │ │ │ │ -81} │ │ │ │ │ -82 │ │ │ │ │ -83#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ -88template │ │ │ │ │ -89bool GTSAM_DEPRECATED _a_s_s_e_r_t___e_q_u_a_l(const std::vector& expected, const │ │ │ │ │ -std::vector& actual, double tol = 1e-9) { │ │ │ │ │ -90 bool match = true; │ │ │ │ │ -91 if (expected.size() != actual.size()) │ │ │ │ │ -92 match = false; │ │ │ │ │ -93 if(match) { │ │ │ │ │ -94 size_t i = 0; │ │ │ │ │ -95 for(const V& a: expected) { │ │ │ │ │ -96 if (!_a_s_s_e_r_t___e_q_u_a_l(a, actual[i++], tol)) { │ │ │ │ │ -97 match = false; │ │ │ │ │ -98 break; │ │ │ │ │ -99 } │ │ │ │ │ -100 } │ │ │ │ │ -101 } │ │ │ │ │ -102 if(!match) { │ │ │ │ │ -103 std::cout << "expected: " << std::endl; │ │ │ │ │ -104 for(const V& a: expected) { std::cout << a << " "; } │ │ │ │ │ -105 std::cout << "\nactual: " << std::endl; │ │ │ │ │ -106 for(const V& a: actual) { std::cout << a << " "; } │ │ │ │ │ -107 std::cout << std::endl; │ │ │ │ │ -108 return false; │ │ │ │ │ -109 } │ │ │ │ │ -110 return true; │ │ │ │ │ -111} │ │ │ │ │ -112#endif │ │ │ │ │ +65typedef Eigen::VectorBlock SubVector; │ │ │ │ │ +66typedef Eigen::VectorBlock ConstSubVector; │ │ │ │ │ +67 │ │ │ │ │ +73#if defined(GTSAM_EIGEN_VERSION_WORLD) │ │ │ │ │ +74static_assert( │ │ │ │ │ +75 GTSAM_EIGEN_VERSION_WORLD==EIGEN_WORLD_VERSION && │ │ │ │ │ +76 GTSAM_EIGEN_VERSION_MAJOR==EIGEN_MAJOR_VERSION, │ │ │ │ │ +77 "Error: GTSAM was built against a different version of Eigen"); │ │ │ │ │ +78#endif │ │ │ │ │ +79 │ │ │ │ │ +96GTSAM_EXPORT bool _f_p_E_q_u_a_l(double a, double b, double tol, │ │ │ │ │ +97 bool check_relative_also = true); │ │ │ │ │ +98 │ │ │ │ │ +102GTSAM_EXPORT void _p_r_i_n_t(const Vector& v, const std::string& s, std::ostream& │ │ │ │ │ +stream); │ │ │ │ │ +103 │ │ │ │ │ +107GTSAM_EXPORT void _p_r_i_n_t(const Vector& v, const std::string& s = ""); │ │ │ │ │ +108 │ │ │ │ │ +112GTSAM_EXPORT void _s_a_v_e(const Vector& A, const std::string &s, const std:: │ │ │ │ │ +string& filename); │ │ │ │ │ 113 │ │ │ │ │ -118template │ │ │ │ │ -_1_1_9bool _a_s_s_e_r_t___c_o_n_t_a_i_n_e_r___e_q_u_a_l(const std::map& expected, const std:: │ │ │ │ │ -map& actual, double tol = 1e-9) { │ │ │ │ │ -120 typedef typename std::map Map; │ │ │ │ │ -121 bool match = true; │ │ │ │ │ -122 if (expected.size() != actual.size()) │ │ │ │ │ -123 match = false; │ │ │ │ │ -124 typename Map::const_iterator │ │ │ │ │ -125 itExp = expected.begin(), │ │ │ │ │ -126 itAct = actual.begin(); │ │ │ │ │ -127 if(match) { │ │ │ │ │ -128 for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { │ │ │ │ │ -129 if (!_a_s_s_e_r_t___e_q_u_a_l(itExp->first, itAct->first, tol) || │ │ │ │ │ -130 !_a_s_s_e_r_t___e_q_u_a_l(itExp->second, itAct->second, tol)) { │ │ │ │ │ -131 match = false; │ │ │ │ │ -132 break; │ │ │ │ │ -133 } │ │ │ │ │ -134 } │ │ │ │ │ -135 } │ │ │ │ │ -136 if(!match) { │ │ │ │ │ -137 std::cout << "expected: " << std::endl; │ │ │ │ │ -138 for(const typename Map::value_type& a: expected) { │ │ │ │ │ -139 a.first.print("key"); │ │ │ │ │ -140 a.second.print(" value"); │ │ │ │ │ -141 } │ │ │ │ │ -142 std::cout << "\nactual: " << std::endl; │ │ │ │ │ -143 for(const typename Map::value_type& a: actual) { │ │ │ │ │ -144 a.first.print("key"); │ │ │ │ │ -145 a.second.print(" value"); │ │ │ │ │ -146 } │ │ │ │ │ -147 std::cout << std::endl; │ │ │ │ │ -148 return false; │ │ │ │ │ -149 } │ │ │ │ │ -150 return true; │ │ │ │ │ -151} │ │ │ │ │ -152 │ │ │ │ │ -156template │ │ │ │ │ -_1_5_7bool _a_s_s_e_r_t___c_o_n_t_a_i_n_e_r___e_q_u_a_l(const std::map& expected, const std:: │ │ │ │ │ -map& actual, double tol = 1e-9) { │ │ │ │ │ -158 typedef typename std::map Map; │ │ │ │ │ -159 bool match = true; │ │ │ │ │ -160 if (expected.size() != actual.size()) │ │ │ │ │ -161 match = false; │ │ │ │ │ -162 typename Map::const_iterator │ │ │ │ │ -163 itExp = expected.begin(), │ │ │ │ │ -164 itAct = actual.begin(); │ │ │ │ │ -165 if(match) { │ │ │ │ │ -166 for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { │ │ │ │ │ -167 if (itExp->first != itAct->first || │ │ │ │ │ -168 !_a_s_s_e_r_t___e_q_u_a_l(itExp->second, itAct->second, tol)) { │ │ │ │ │ -169 match = false; │ │ │ │ │ -170 break; │ │ │ │ │ -171 } │ │ │ │ │ -172 } │ │ │ │ │ -173 } │ │ │ │ │ -174 if(!match) { │ │ │ │ │ -175 std::cout << "expected: " << std::endl; │ │ │ │ │ -176 for(const typename Map::value_type& a: expected) { │ │ │ │ │ -177 std::cout << "Key: " << a.first << std::endl; │ │ │ │ │ -178 a.second.print(" value"); │ │ │ │ │ -179 } │ │ │ │ │ -180 std::cout << "\nactual: " << std::endl; │ │ │ │ │ -181 for(const typename Map::value_type& a: actual) { │ │ │ │ │ -182 std::cout << "Key: " << a.first << std::endl; │ │ │ │ │ -183 a.second.print(" value"); │ │ │ │ │ -184 } │ │ │ │ │ -185 std::cout << std::endl; │ │ │ │ │ -186 return false; │ │ │ │ │ -187 } │ │ │ │ │ -188 return true; │ │ │ │ │ -189} │ │ │ │ │ +117GTSAM_EXPORT bool _o_p_e_r_a_t_o_r_=_=(const Vector& vec1,const Vector& vec2); │ │ │ │ │ +118 │ │ │ │ │ +124GTSAM_EXPORT bool _g_r_e_a_t_e_r_T_h_a_n_O_r_E_q_u_a_l(const Vector& v1, const Vector& v2); │ │ │ │ │ +125 │ │ │ │ │ +129GTSAM_EXPORT bool _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(const Vector& vec1, const Vector& vec2, │ │ │ │ │ +double tol=1e-9); │ │ │ │ │ +130GTSAM_EXPORT bool _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(const SubVector& vec1, const SubVector& │ │ │ │ │ +vec2, double tol=1e-9); │ │ │ │ │ +131 │ │ │ │ │ +_1_3_5inline bool _e_q_u_a_l(const Vector& vec1, const Vector& vec2, double tol) { │ │ │ │ │ +136 return _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(vec1, vec2, tol); │ │ │ │ │ +137} │ │ │ │ │ +138 │ │ │ │ │ +_1_4_2inline bool _e_q_u_a_l(const Vector& vec1, const Vector& vec2) { │ │ │ │ │ +143 return _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(vec1, vec2); │ │ │ │ │ +144} │ │ │ │ │ +145 │ │ │ │ │ +153GTSAM_EXPORT bool _a_s_s_e_r_t___e_q_u_a_l(const Vector& vec1, const Vector& vec2, │ │ │ │ │ +double tol=1e-9); │ │ │ │ │ +154 │ │ │ │ │ +162GTSAM_EXPORT bool _a_s_s_e_r_t___i_n_e_q_u_a_l(const Vector& vec1, const Vector& vec2, │ │ │ │ │ +double tol=1e-9); │ │ │ │ │ +163 │ │ │ │ │ +171GTSAM_EXPORT bool _a_s_s_e_r_t___e_q_u_a_l(const SubVector& vec1, const SubVector& vec2, │ │ │ │ │ +double tol=1e-9); │ │ │ │ │ +172GTSAM_EXPORT bool _a_s_s_e_r_t___e_q_u_a_l(const ConstSubVector& vec1, const │ │ │ │ │ +ConstSubVector& vec2, double tol=1e-9); │ │ │ │ │ +173 │ │ │ │ │ +181GTSAM_EXPORT bool _l_i_n_e_a_r___d_e_p_e_n_d_e_n_t(const Vector& vec1, const Vector& vec2, │ │ │ │ │ +double tol=1e-9); │ │ │ │ │ +182 │ │ │ │ │ +189GTSAM_EXPORT Vector _e_d_i_v__(const Vector &a, const Vector &b); │ │ │ │ │ 190 │ │ │ │ │ 194template │ │ │ │ │ -_1_9_5bool _a_s_s_e_r_t___c_o_n_t_a_i_n_e_r___e_q_u_a_l(const std::vector >& expected, │ │ │ │ │ -196 const std::vector >& actual, double tol = 1e-9) { │ │ │ │ │ -197 typedef typename std::vector > VectorPair; │ │ │ │ │ -198 bool match = true; │ │ │ │ │ -199 if (expected.size() != actual.size()) │ │ │ │ │ -200 match = false; │ │ │ │ │ -201 typename VectorPair::const_iterator │ │ │ │ │ -202 itExp = expected.begin(), │ │ │ │ │ -203 itAct = actual.begin(); │ │ │ │ │ -204 if(match) { │ │ │ │ │ -205 for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { │ │ │ │ │ -206 if (!_a_s_s_e_r_t___e_q_u_a_l(itExp->first, itAct->first, tol) || │ │ │ │ │ -207 !_a_s_s_e_r_t___e_q_u_a_l(itExp->second, itAct->second, tol)) { │ │ │ │ │ -208 match = false; │ │ │ │ │ -209 break; │ │ │ │ │ -210 } │ │ │ │ │ -211 } │ │ │ │ │ -212 } │ │ │ │ │ -213 if(!match) { │ │ │ │ │ -214 std::cout << "expected: " << std::endl; │ │ │ │ │ -215 for(const typename VectorPair::value_type& a: expected) { │ │ │ │ │ -216 a.first.print( " first "); │ │ │ │ │ -217 a.second.print(" second"); │ │ │ │ │ -218 } │ │ │ │ │ -219 std::cout << "\nactual: " << std::endl; │ │ │ │ │ -220 for(const typename VectorPair::value_type& a: actual) { │ │ │ │ │ -221 a.first.print( " first "); │ │ │ │ │ -222 a.second.print(" second"); │ │ │ │ │ -223 } │ │ │ │ │ -224 std::cout << std::endl; │ │ │ │ │ -225 return false; │ │ │ │ │ -226 } │ │ │ │ │ -227 return true; │ │ │ │ │ -228} │ │ │ │ │ -229 │ │ │ │ │ -230 │ │ │ │ │ -234template │ │ │ │ │ -_2_3_5bool _a_s_s_e_r_t___c_o_n_t_a_i_n_e_r___e_q_u_a_l(const V& expected, const V& actual, double tol = │ │ │ │ │ -1e-9) { │ │ │ │ │ -236 bool match = true; │ │ │ │ │ -237 typename V::const_iterator │ │ │ │ │ -238 itExp = expected.begin(), │ │ │ │ │ -239 itAct = actual.begin(); │ │ │ │ │ -240 if(match) { │ │ │ │ │ -241 for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { │ │ │ │ │ -242 if (!_a_s_s_e_r_t___e_q_u_a_l(*itExp, *itAct, tol)) { │ │ │ │ │ -243 match = false; │ │ │ │ │ -244 break; │ │ │ │ │ -245 } │ │ │ │ │ -246 } │ │ │ │ │ -247 if(itExp != expected.end() || itAct != actual.end()) │ │ │ │ │ -248 match = false; │ │ │ │ │ -249 } │ │ │ │ │ -250 if(!match) { │ │ │ │ │ -251 std::cout << "expected: " << std::endl; │ │ │ │ │ -252 for(const typename V::value_type& a: expected) { a.print(" "); } │ │ │ │ │ -253 std::cout << "\nactual: " << std::endl; │ │ │ │ │ -254 for(const typename V::value_type& a: actual) { a.print(" "); } │ │ │ │ │ -255 std::cout << std::endl; │ │ │ │ │ -256 return false; │ │ │ │ │ -257 } │ │ │ │ │ -258 return true; │ │ │ │ │ -259} │ │ │ │ │ -260 │ │ │ │ │ -265template │ │ │ │ │ -_2_6_6bool _a_s_s_e_r_t___c_o_n_t_a_i_n_e_r___e_q_u_a_l_i_t_y(const std::map& expected, const │ │ │ │ │ -std::map& actual) { │ │ │ │ │ -267 typedef typename std::map Map; │ │ │ │ │ -268 bool match = true; │ │ │ │ │ -269 if (expected.size() != actual.size()) │ │ │ │ │ -270 match = false; │ │ │ │ │ -271 typename Map::const_iterator │ │ │ │ │ -272 itExp = expected.begin(), │ │ │ │ │ -273 itAct = actual.begin(); │ │ │ │ │ -274 if(match) { │ │ │ │ │ -275 for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { │ │ │ │ │ -276 if (itExp->first != itAct->first || itExp->second != itAct->second) { │ │ │ │ │ -277 match = false; │ │ │ │ │ -278 break; │ │ │ │ │ -279 } │ │ │ │ │ -280 } │ │ │ │ │ -281 } │ │ │ │ │ -282 if(!match) { │ │ │ │ │ -283 std::cout << "expected: " << std::endl; │ │ │ │ │ -284 for(const typename Map::value_type& a: expected) { │ │ │ │ │ -285 std::cout << "Key: " << a.first << std::endl; │ │ │ │ │ -286 std::cout << "Value: " << a.second << std::endl; │ │ │ │ │ -287 } │ │ │ │ │ -288 std::cout << "\nactual: " << std::endl; │ │ │ │ │ -289 for(const typename Map::value_type& a: actual) { │ │ │ │ │ -290 std::cout << "Key: " << a.first << std::endl; │ │ │ │ │ -291 std::cout << "Value: " << a.second << std::endl; │ │ │ │ │ -292 } │ │ │ │ │ -293 std::cout << std::endl; │ │ │ │ │ -294 return false; │ │ │ │ │ -295 } │ │ │ │ │ -296 return true; │ │ │ │ │ -297} │ │ │ │ │ -298 │ │ │ │ │ -299 │ │ │ │ │ -303template │ │ │ │ │ -_3_0_4bool _a_s_s_e_r_t___c_o_n_t_a_i_n_e_r___e_q_u_a_l_i_t_y(const V& expected, const V& actual) { │ │ │ │ │ -305 bool match = true; │ │ │ │ │ -306 if (expected.size() != actual.size()) │ │ │ │ │ -307 match = false; │ │ │ │ │ -308 typename V::const_iterator │ │ │ │ │ -309 itExp = expected.begin(), │ │ │ │ │ -310 itAct = actual.begin(); │ │ │ │ │ -311 if(match) { │ │ │ │ │ -312 for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { │ │ │ │ │ -313 if (*itExp != *itAct) { │ │ │ │ │ -314 match = false; │ │ │ │ │ -315 break; │ │ │ │ │ -316 } │ │ │ │ │ -317 } │ │ │ │ │ -318 } │ │ │ │ │ -319 if(!match) { │ │ │ │ │ -320 std::cout << "expected: " << std::endl; │ │ │ │ │ -321 for(const typename V::value_type& a: expected) { std::cout << a << " "; } │ │ │ │ │ -322 std::cout << "\nactual: " << std::endl; │ │ │ │ │ -323 for(const typename V::value_type& a: actual) { std::cout << a << " "; } │ │ │ │ │ -324 std::cout << std::endl; │ │ │ │ │ -325 return false; │ │ │ │ │ -326 } │ │ │ │ │ -327 return true; │ │ │ │ │ -328} │ │ │ │ │ -329 │ │ │ │ │ -_3_3_3inline bool _a_s_s_e_r_t___e_q_u_a_l(const std::string& expected, const std::string& │ │ │ │ │ -actual) { │ │ │ │ │ -334 if (expected == actual) │ │ │ │ │ -335 return true; │ │ │ │ │ -336 printf("Not equal:\n"); │ │ │ │ │ -337 std::cout << "expected: [" << expected << "]\n"; │ │ │ │ │ -338 std::cout << "actual: [" << actual << "]" << std::endl; │ │ │ │ │ -339 return false; │ │ │ │ │ -340} │ │ │ │ │ -341 │ │ │ │ │ -345template │ │ │ │ │ -_3_4_6bool _a_s_s_e_r_t___i_n_e_q_u_a_l(const V& expected, const V& actual, double tol = 1e-9) { │ │ │ │ │ -347 if (!actual.equals(expected, tol)) │ │ │ │ │ -348 return true; │ │ │ │ │ -349 printf("Erroneously equal:\n"); │ │ │ │ │ -350 expected.print("expected"); │ │ │ │ │ -351 actual.print("actual"); │ │ │ │ │ -352 return false; │ │ │ │ │ -353} │ │ │ │ │ -354 │ │ │ │ │ -358template │ │ │ │ │ -_3_5_9bool _a_s_s_e_r_t___s_t_d_o_u_t___e_q_u_a_l(const std::string& expected, const V& actual) { │ │ │ │ │ -360 // Redirect output to buffer so we can compare │ │ │ │ │ -361 std::stringstream buffer; │ │ │ │ │ -362 // Save the original output stream so we can reset later │ │ │ │ │ -363 std::streambuf* old = std::cout.rdbuf(buffer.rdbuf()); │ │ │ │ │ -364 │ │ │ │ │ -365 // We test against actual std::cout for faithful reproduction │ │ │ │ │ -366 std::cout << actual; │ │ │ │ │ -367 │ │ │ │ │ -368 // Get output string and reset stdout │ │ │ │ │ -369 std::string actual_ = buffer.str(); │ │ │ │ │ -370 std::cout.rdbuf(old); │ │ │ │ │ -371 │ │ │ │ │ -372 return _a_s_s_e_r_t___e_q_u_a_l(expected, actual_); │ │ │ │ │ -373} │ │ │ │ │ -374 │ │ │ │ │ -380template │ │ │ │ │ -_3_8_1bool _a_s_s_e_r_t___p_r_i_n_t___e_q_u_a_l(const std::string& expected, const V& actual, │ │ │ │ │ -382 const std::string& s = "") { │ │ │ │ │ -383 // Redirect output to buffer so we can compare │ │ │ │ │ -384 std::stringstream buffer; │ │ │ │ │ -385 // Save the original output stream so we can reset later │ │ │ │ │ -386 std::streambuf* old = std::cout.rdbuf(buffer.rdbuf()); │ │ │ │ │ -387 │ │ │ │ │ -388 // We test against actual std::cout for faithful reproduction │ │ │ │ │ -389 actual.print(s); │ │ │ │ │ -390 │ │ │ │ │ -391 // Get output string and reset stdout │ │ │ │ │ -392 std::string actual_ = buffer.str(); │ │ │ │ │ -393 std::cout.rdbuf(old); │ │ │ │ │ -394 │ │ │ │ │ -395 return _a_s_s_e_r_t___e_q_u_a_l(expected, actual_); │ │ │ │ │ -396} │ │ │ │ │ -397 │ │ │ │ │ -398} // \namespace gtsam │ │ │ │ │ -_T_e_s_t_a_b_l_e_._h │ │ │ │ │ -Concept check for values that can be used in unit tests. │ │ │ │ │ +_1_9_5inline double _d_o_t(const V1 &a, const V2& b) { │ │ │ │ │ +196 assert (b.size()==a.size()); │ │ │ │ │ +197 return a.dot(b); │ │ │ │ │ +198} │ │ │ │ │ +199 │ │ │ │ │ +201template │ │ │ │ │ +_2_0_2inline double _i_n_n_e_r___p_r_o_d(const V1 &a, const V2& b) { │ │ │ │ │ +203 assert (b.size()==a.size()); │ │ │ │ │ +204 return a.dot(b); │ │ │ │ │ +205} │ │ │ │ │ +206 │ │ │ │ │ +207#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ +212inline void GTSAM_DEPRECATED scal(double alpha, Vector& x) { x *= alpha; } │ │ │ │ │ +213 │ │ │ │ │ +218template │ │ │ │ │ +219inline void GTSAM_DEPRECATED _a_x_p_y(double alpha, const V1& x, V2& y) { │ │ │ │ │ +220 assert (y.size()==x.size()); │ │ │ │ │ +221 y += alpha * x; │ │ │ │ │ +222} │ │ │ │ │ +223inline void _a_x_p_y(double alpha, const Vector& x, SubVector y) { │ │ │ │ │ +224 assert (y.size()==x.size()); │ │ │ │ │ +225 y += alpha * x; │ │ │ │ │ +226} │ │ │ │ │ +227#endif │ │ │ │ │ +228 │ │ │ │ │ +234GTSAM_EXPORT std::pair _h_o_u_s_e(const Vector &x); │ │ │ │ │ +235 │ │ │ │ │ +237GTSAM_EXPORT double _h_o_u_s_e_I_n_P_l_a_c_e(Vector &x); │ │ │ │ │ +238 │ │ │ │ │ +249GTSAM_EXPORT std::pair │ │ │ │ │ +250weightedPseudoinverse(const Vector& v, const Vector& weights); │ │ │ │ │ +251 │ │ │ │ │ +252/* │ │ │ │ │ +253 * Fast version *no error checking* ! │ │ │ │ │ +254 * Pass in initialized vector pseudo of size(weights) or will crash ! │ │ │ │ │ +255 * @return the precision, pseudoinverse in third argument │ │ │ │ │ +256 */ │ │ │ │ │ +257GTSAM_EXPORT double weightedPseudoinverse(const Vector& a, const Vector& │ │ │ │ │ +weights, Vector& pseudo); │ │ │ │ │ +258 │ │ │ │ │ +262GTSAM_EXPORT Vector _c_o_n_c_a_t_V_e_c_t_o_r_s(const std::list& vs); │ │ │ │ │ +263 │ │ │ │ │ +267GTSAM_EXPORT Vector _c_o_n_c_a_t_V_e_c_t_o_r_s(size_t nrVectors, ...); │ │ │ │ │ +268} // namespace gtsam │ │ │ │ │ _g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ Included from all GTSAM files. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_a_s_s_e_r_t___s_t_d_o_u_t___e_q_u_a_l │ │ │ │ │ -bool assert_stdout_equal(const std::string &expected, const V &actual) │ │ │ │ │ -Capture std out via cout stream and compare against string. │ │ │ │ │ -DDeeffiinniittiioonn TestableAssertions.h:359 │ │ │ │ │ -_g_t_s_a_m_:_:_a_s_s_e_r_t___c_o_n_t_a_i_n_e_r___e_q_u_a_l │ │ │ │ │ -bool assert_container_equal(const std::map< V1, V2 > &expected, const std::map< │ │ │ │ │ -V1, V2 > &actual, double tol=1e-9) │ │ │ │ │ -Function for comparing maps of testable->testable TODO: replace with more │ │ │ │ │ -generalized version. │ │ │ │ │ -DDeeffiinniittiioonn TestableAssertions.h:119 │ │ │ │ │ +_g_t_s_a_m_:_:_e_d_i_v__ │ │ │ │ │ +Vector ediv_(const Vector &a, const Vector &b) │ │ │ │ │ +elementwise division, but 0/0 = 0, not inf │ │ │ │ │ +DDeeffiinniittiioonn Vector.cpp:199 │ │ │ │ │ +_g_t_s_a_m_:_:_g_r_e_a_t_e_r_T_h_a_n_O_r_E_q_u_a_l │ │ │ │ │ +bool greaterThanOrEqual(const Vector &vec1, const Vector &vec2) │ │ │ │ │ +Greater than or equal to operation returns true if all elements in v1 are │ │ │ │ │ +greater than corresponding ... │ │ │ │ │ +DDeeffiinniittiioonn Vector.cpp:114 │ │ │ │ │ +_g_t_s_a_m_:_:_s_a_v_e │ │ │ │ │ +void save(const Matrix &A, const string &s, const string &filename) │ │ │ │ │ +save a matrix to file, which can be loaded by matlab │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:167 │ │ │ │ │ _g_t_s_a_m_:_:_a_s_s_e_r_t___e_q_u_a_l │ │ │ │ │ bool assert_equal(const Matrix &expected, const Matrix &actual, double tol) │ │ │ │ │ equals with an tolerance, prints out message if unequal │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:43 │ │ │ │ │ -_g_t_s_a_m_:_:_a_s_s_e_r_t___c_o_n_t_a_i_n_e_r___e_q_u_a_l_i_t_y │ │ │ │ │ -bool assert_container_equality(const std::map< size_t, V2 > &expected, const │ │ │ │ │ -std::map< size_t, V2 > &actual) │ │ │ │ │ -Function for comparing maps of size_t->testable Types are assumed to have │ │ │ │ │ -operator ==. │ │ │ │ │ -DDeeffiinniittiioonn TestableAssertions.h:266 │ │ │ │ │ +_g_t_s_a_m_:_:_l_i_n_e_a_r___d_e_p_e_n_d_e_n_t │ │ │ │ │ +bool linear_dependent(const Matrix &A, const Matrix &B, double tol) │ │ │ │ │ +check whether the rows of two matrices are linear dependent │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:117 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_n_e_r___p_r_o_d │ │ │ │ │ +double inner_prod(const V1 &a, const V2 &b) │ │ │ │ │ +compatibility version for ublas' inner_prod() │ │ │ │ │ +DDeeffiinniittiioonn Vector.h:202 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_a_x_p_y │ │ │ │ │ +void axpy(double alpha, const Errors &x, Errors &y) │ │ │ │ │ +BLAS level 2 style AXPY, y := alpha*x + y │ │ │ │ │ +DDeeffiinniittiioonn Errors.cpp:111 │ │ │ │ │ _g_t_s_a_m_:_:_a_s_s_e_r_t___i_n_e_q_u_a_l │ │ │ │ │ bool assert_inequal(const Matrix &A, const Matrix &B, double tol) │ │ │ │ │ inequals with an tolerance, prints out message if within tolerance │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:63 │ │ │ │ │ -_g_t_s_a_m_:_:_a_s_s_e_r_t___p_r_i_n_t___e_q_u_a_l │ │ │ │ │ -bool assert_print_equal(const std::string &expected, const V &actual, const │ │ │ │ │ -std::string &s="") │ │ │ │ │ -Capture print function output and compare against string. │ │ │ │ │ -DDeeffiinniittiioonn TestableAssertions.h:381 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_h_o_u_s_e_I_n_P_l_a_c_e │ │ │ │ │ +double houseInPlace(Vector &v) │ │ │ │ │ +beta = house(x) computes the HouseHolder vector in place │ │ │ │ │ +DDeeffiinniittiioonn Vector.cpp:212 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l │ │ │ │ │ +bool equal(const T &obj1, const T &obj2, double tol) │ │ │ │ │ +Call equal on the object. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:84 │ │ │ │ │ +_g_t_s_a_m_:_:_d_o_t │ │ │ │ │ +double dot(const V1 &a, const V2 &b) │ │ │ │ │ +Dot product. │ │ │ │ │ +DDeeffiinniittiioonn Vector.h:195 │ │ │ │ │ +_g_t_s_a_m_:_:_f_p_E_q_u_a_l │ │ │ │ │ +bool fpEqual(double a, double b, double tol, bool check_relative_also) │ │ │ │ │ +Ensure we are not including a different version of Eigen in user code than │ │ │ │ │ +while compiling gtsam,... │ │ │ │ │ +DDeeffiinniittiioonn Vector.cpp:42 │ │ │ │ │ +_g_t_s_a_m_:_:_c_o_n_c_a_t_V_e_c_t_o_r_s │ │ │ │ │ +Vector concatVectors(const std::list< Vector > &vs) │ │ │ │ │ +concatenate Vectors │ │ │ │ │ +DDeeffiinniittiioonn Vector.cpp:302 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l___w_i_t_h___a_b_s___t_o_l │ │ │ │ │ +bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen:: │ │ │ │ │ +DenseBase< MATRIX > &B, double tol=1e-9) │ │ │ │ │ +equals with a tolerance │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:81 │ │ │ │ │ +_g_t_s_a_m_:_:_h_o_u_s_e │ │ │ │ │ +pair< double, Vector > house(const Vector &x) │ │ │ │ │ +house(x,j) computes HouseHolder vector v and scaling factor beta from x, such │ │ │ │ │ +that the corresponding ... │ │ │ │ │ +DDeeffiinniittiioonn Vector.cpp:237 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_=_= │ │ │ │ │ +bool operator==(const Matrix &A, const Matrix &B) │ │ │ │ │ +equality is just equal_with_abs_tol 1e-9 │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:100 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _T_e_s_t_a_b_l_e_A_s_s_e_r_t_i_o_n_s_._h │ │ │ │ │ + * _V_e_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00017.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/timing.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/numericalDerivative.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,55 +94,306 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces | │ │ │ │ Functions
│ │ │ │ -
timing.cpp File Reference
│ │ │ │ +
numericalDerivative.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Timing utilities. │ │ │ │ +

Some functions to compute numerical derivatives. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

struct  gtsam::internal::FixedSizeMatrix< Y, X >
 
class  gtsam::G_x1< X1, X2 >
 Helper class that computes the derivative of f w.r.t. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

│ │ │ │ -GTSAM_EXPORT boost::shared_ptr< TimingOutline > gtsam::internal::gTimingRoot (new TimingOutline("Total", getTicTocID("Total")))
 
│ │ │ │ -GTSAM_EXPORT boost::weak_ptr< TimingOutline > gtsam::internal::gCurrentTimer (gTimingRoot)
 
│ │ │ │ -size_t gtsam::internal::getTicTocID (const char *descriptionC)
 
│ │ │ │ -void gtsam::internal::tic (size_t id, const char *labelC)
 
│ │ │ │ -void gtsam::internal::toc (size_t id, const char *label)
 
template<class X , int N = traits<X>::dimension>
Eigen::Matrix< double, N, 1 > gtsam::numericalGradient (std::function< double(const X &)> h, const X &x, double delta=1e-5)
 Numerically compute gradient of scalar function.
 
template<class Y , class X , int N = traits<X>::dimension>
internal::FixedSizeMatrix< Y, X >::type gtsam::numericalDerivative11 (std::function< Y(const X &)> h, const X &x, double delta=1e-5)
 New-style numerical derivatives using manifold_traits.
 
│ │ │ │ +template<class Y , class X >
internal::FixedSizeMatrix< Y, X >::type gtsam::numericalDerivative11 (Y(*h)(const X &), const X &x, double delta=1e-5)
 use a raw C++ function pointer
 
template<class Y , class X1 , class X2 , int N = traits<X1>::dimension>
internal::FixedSizeMatrix< Y, X1 >::type gtsam::numericalDerivative21 (const std::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5)
 Compute numerical derivative in argument 1 of binary function.
 
│ │ │ │ +template<class Y , class X1 , class X2 >
internal::FixedSizeMatrix< Y, X1 >::type gtsam::numericalDerivative21 (Y(*h)(const X1 &, const X2 &), const X1 &x1, const X2 &x2, double delta=1e-5)
 use a raw C++ function pointer
 
template<class Y , class X1 , class X2 , int N = traits<X2>::dimension>
internal::FixedSizeMatrix< Y, X2 >::type gtsam::numericalDerivative22 (std::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
 Compute numerical derivative in argument 2 of binary function.
 
│ │ │ │ +template<class Y , class X1 , class X2 >
internal::FixedSizeMatrix< Y, X2 >::type gtsam::numericalDerivative22 (Y(*h)(const X1 &, const X2 &), const X1 &x1, const X2 &x2, double delta=1e-5)
 use a raw C++ function pointer
 
template<class Y , class X1 , class X2 , class X3 , int N = traits<X1>::dimension>
internal::FixedSizeMatrix< Y, X1 >::type gtsam::numericalDerivative31 (std::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
 Compute numerical derivative in argument 1 of ternary function.
 
│ │ │ │ +template<class Y , class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< Y, X1 >::type gtsam::numericalDerivative31 (Y(*h)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
 
template<class Y , class X1 , class X2 , class X3 , int N = traits<X2>::dimension>
internal::FixedSizeMatrix< Y, X2 >::type gtsam::numericalDerivative32 (std::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
 Compute numerical derivative in argument 2 of ternary function.
 
│ │ │ │ +template<class Y , class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< Y, X2 >::type gtsam::numericalDerivative32 (Y(*h)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
 
template<class Y , class X1 , class X2 , class X3 , int N = traits<X3>::dimension>
internal::FixedSizeMatrix< Y, X3 >::type gtsam::numericalDerivative33 (std::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
 Compute numerical derivative in argument 3 of ternary function.
 
│ │ │ │ +template<class Y , class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< Y, X3 >::type gtsam::numericalDerivative33 (Y(*h)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
 
template<class Y , class X1 , class X2 , class X3 , class X4 , int N = traits<X1>::dimension>
internal::FixedSizeMatrix< Y, X1 >::type gtsam::numericalDerivative41 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
 Compute numerical derivative in argument 1 of 4-argument function.
 
│ │ │ │ +template<class Y , class X1 , class X2 , class X3 , class X4 >
internal::FixedSizeMatrix< Y, X1 >::type gtsam::numericalDerivative41 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
 
template<class Y , class X1 , class X2 , class X3 , class X4 , int N = traits<X2>::dimension>
internal::FixedSizeMatrix< Y, X2 >::type gtsam::numericalDerivative42 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
 Compute numerical derivative in argument 2 of 4-argument function.
 
│ │ │ │ +template<class Y , class X1 , class X2 , class X3 , class X4 >
internal::FixedSizeMatrix< Y, X2 >::type gtsam::numericalDerivative42 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
 
template<class Y , class X1 , class X2 , class X3 , class X4 , int N = traits<X3>::dimension>
internal::FixedSizeMatrix< Y, X3 >::type gtsam::numericalDerivative43 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
 Compute numerical derivative in argument 3 of 4-argument function.
 
│ │ │ │ +template<class Y , class X1 , class X2 , class X3 , class X4 >
internal::FixedSizeMatrix< Y, X3 >::type gtsam::numericalDerivative43 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
 
template<class Y , class X1 , class X2 , class X3 , class X4 , int N = traits<X4>::dimension>
internal::FixedSizeMatrix< Y, X4 >::type gtsam::numericalDerivative44 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
 Compute numerical derivative in argument 4 of 4-argument function.
 
│ │ │ │ +template<class Y , class X1 , class X2 , class X3 , class X4 >
internal::FixedSizeMatrix< Y, X4 >::type gtsam::numericalDerivative44 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
 
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , int N = traits<X1>::dimension>
internal::FixedSizeMatrix< Y, X1 >::type gtsam::numericalDerivative51 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
 Compute numerical derivative in argument 1 of 5-argument function.
 
│ │ │ │ +template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 >
internal::FixedSizeMatrix< Y, X1 >::type gtsam::numericalDerivative51 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
 
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , int N = traits<X2>::dimension>
internal::FixedSizeMatrix< Y, X2 >::type gtsam::numericalDerivative52 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
 Compute numerical derivative in argument 2 of 5-argument function.
 
│ │ │ │ +template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 >
internal::FixedSizeMatrix< Y, X2 >::type gtsam::numericalDerivative52 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
 
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , int N = traits<X3>::dimension>
internal::FixedSizeMatrix< Y, X3 >::type gtsam::numericalDerivative53 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
 Compute numerical derivative in argument 3 of 5-argument function.
 
│ │ │ │ +template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 >
internal::FixedSizeMatrix< Y, X3 >::type gtsam::numericalDerivative53 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
 
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , int N = traits<X4>::dimension>
internal::FixedSizeMatrix< Y, X4 >::type gtsam::numericalDerivative54 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
 Compute numerical derivative in argument 4 of 5-argument function.
 
│ │ │ │ +template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 >
internal::FixedSizeMatrix< Y, X4 >::type gtsam::numericalDerivative54 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
 
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , int N = traits<X5>::dimension>
internal::FixedSizeMatrix< Y, X5 >::type gtsam::numericalDerivative55 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
 Compute numerical derivative in argument 5 of 5-argument function.
 
│ │ │ │ +template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 >
internal::FixedSizeMatrix< Y, X5 >::type gtsam::numericalDerivative55 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
 
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 , int N = traits<X1>::dimension>
internal::FixedSizeMatrix< Y, X1 >::type gtsam::numericalDerivative61 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
 Compute numerical derivative in argument 1 of 6-argument function.
 
│ │ │ │ +template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 >
internal::FixedSizeMatrix< Y, X1 >::type gtsam::numericalDerivative61 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
 
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 , int N = traits<X2>::dimension>
internal::FixedSizeMatrix< Y, X2 >::type gtsam::numericalDerivative62 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
 Compute numerical derivative in argument 2 of 6-argument function.
 
│ │ │ │ +template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 >
internal::FixedSizeMatrix< Y, X2 >::type gtsam::numericalDerivative62 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
 
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 , int N = traits<X3>::dimension>
internal::FixedSizeMatrix< Y, X3 >::type gtsam::numericalDerivative63 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
 Compute numerical derivative in argument 3 of 6-argument function.
 
│ │ │ │ +template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 >
internal::FixedSizeMatrix< Y, X3 >::type gtsam::numericalDerivative63 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
 
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 , int N = traits<X4>::dimension>
internal::FixedSizeMatrix< Y, X4 >::type gtsam::numericalDerivative64 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
 Compute numerical derivative in argument 4 of 6-argument function.
 
│ │ │ │ +template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 >
internal::FixedSizeMatrix< Y, X4 >::type gtsam::numericalDerivative64 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
 
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 , int N = traits<X5>::dimension>
internal::FixedSizeMatrix< Y, X5 >::type gtsam::numericalDerivative65 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
 Compute numerical derivative in argument 5 of 6-argument function.
 
│ │ │ │ +template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 >
internal::FixedSizeMatrix< Y, X5 >::type gtsam::numericalDerivative65 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
 
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 , int N = traits<X6>::dimension>
internal::FixedSizeMatrix< Y, X6 >::type gtsam::numericalDerivative66 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
 Compute numerical derivative in argument 6 of 6-argument function.
 
│ │ │ │ +template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 >
internal::FixedSizeMatrix< Y, X6 >::type gtsam::numericalDerivative66 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
 
template<class X >
internal::FixedSizeMatrix< X, X >::type gtsam::numericalHessian (std::function< double(const X &)> f, const X &x, double delta=1e-5)
 Compute numerical Hessian matrix.
 
│ │ │ │ +template<class X >
internal::FixedSizeMatrix< X, X >::type gtsam::numericalHessian (double(*f)(const X &), const X &x, double delta=1e-5)
 
│ │ │ │ +template<class X1 , class X2 >
internal::FixedSizeMatrix< X1, X2 >::type gtsam::numericalHessian212 (std::function< double(const X1 &, const X2 &)> f, const X1 &x1, const X2 &x2, double delta=1e-5)
 
│ │ │ │ +template<class X1 , class X2 >
internal::FixedSizeMatrix< X1, X2 >::type gtsam::numericalHessian212 (double(*f)(const X1 &, const X2 &), const X1 &x1, const X2 &x2, double delta=1e-5)
 
│ │ │ │ +template<class X1 , class X2 >
internal::FixedSizeMatrix< X1, X1 >::type gtsam::numericalHessian211 (std::function< double(const X1 &, const X2 &)> f, const X1 &x1, const X2 &x2, double delta=1e-5)
 
│ │ │ │ +template<class X1 , class X2 >
internal::FixedSizeMatrix< X1, X1 >::type gtsam::numericalHessian211 (double(*f)(const X1 &, const X2 &), const X1 &x1, const X2 &x2, double delta=1e-5)
 
│ │ │ │ +template<class X1 , class X2 >
internal::FixedSizeMatrix< X2, X2 >::type gtsam::numericalHessian222 (std::function< double(const X1 &, const X2 &)> f, const X1 &x1, const X2 &x2, double delta=1e-5)
 
│ │ │ │ +template<class X1 , class X2 >
internal::FixedSizeMatrix< X2, X2 >::type gtsam::numericalHessian222 (double(*f)(const X1 &, const X2 &), const X1 &x1, const X2 &x2, double delta=1e-5)
 
│ │ │ │ +template<class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< X1, X1 >::type gtsam::numericalHessian311 (std::function< double(const X1 &, const X2 &, const X3 &)> f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
 Numerical Hessian for tenary functions.
 
│ │ │ │ +template<class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< X1, X1 >::type gtsam::numericalHessian311 (double(*f)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
 
│ │ │ │ +template<class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< X2, X2 >::type gtsam::numericalHessian322 (std::function< double(const X1 &, const X2 &, const X3 &)> f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
 
│ │ │ │ +template<class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< X2, X2 >::type gtsam::numericalHessian322 (double(*f)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
 
│ │ │ │ +template<class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< X3, X3 >::type gtsam::numericalHessian333 (std::function< double(const X1 &, const X2 &, const X3 &)> f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
 
│ │ │ │ +template<class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< X3, X3 >::type gtsam::numericalHessian333 (double(*f)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
 
│ │ │ │ +template<class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< X1, X2 >::type gtsam::numericalHessian312 (std::function< double(const X1 &, const X2 &, const X3 &)> f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
 
│ │ │ │ +template<class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< X1, X3 >::type gtsam::numericalHessian313 (std::function< double(const X1 &, const X2 &, const X3 &)> f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
 
│ │ │ │ +template<class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< X2, X3 >::type gtsam::numericalHessian323 (std::function< double(const X1 &, const X2 &, const X3 &)> f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
 
│ │ │ │ +template<class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< X1, X2 >::type gtsam::numericalHessian312 (double(*f)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
 
│ │ │ │ +template<class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< X1, X3 >::type gtsam::numericalHessian313 (double(*f)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
 
│ │ │ │ +template<class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< X2, X3 >::type gtsam::numericalHessian323 (double(*f)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Timing utilities.

│ │ │ │ -
Author
Richard Roberts, Michael Kaess
│ │ │ │ -
Date
Oct 5, 2010
│ │ │ │ +

Some functions to compute numerical derivatives.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,39 +1,516 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -timing.cpp File Reference │ │ │ │ │ -Timing utilities. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +numericalDerivative.h File Reference │ │ │ │ │ +Some functions to compute numerical derivatives. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +struct   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x_<_ _Y_,_ _X_ _> │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_G___x_1_<_ _X_1_,_ _X_2_ _> │ │ │ │ │ +  Helper class that computes the derivative of f w.r.t. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -GTSAM_EXPORT boost::shared_ptr< ggttssaamm::::iinntteerrnnaall::::ggTTiimmiinnggRRoooott (new │ │ │ │ │ - _T_i_m_i_n_g_O_u_t_l_i_n_e >  _T_i_m_i_n_g_O_u_t_l_i_n_e("Total", getTicTocID │ │ │ │ │ - ("Total"))) │ │ │ │ │ +template::dimension> │ │ │ │ │ + Eigen::Matrix< double, N, 1 >  _g_t_s_a_m_:_:_n_u_m_e_r_i_c_a_l_G_r_a_d_i_e_n_t (std::function< │ │ │ │ │ + double(const X &)> h, const X &x, double │ │ │ │ │ + delta=1e-5) │ │ │ │ │ +  Numerically compute gradient of scalar │ │ │ │ │ + function. │ │ │ │ │ +  │ │ │ │ │ +template::dimension> │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X >:: _g_t_s_a_m_:_:_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_1_1 (std:: │ │ │ │ │ + type  function< Y(const X &)> h, const X &x, │ │ │ │ │ + double delta=1e-5) │ │ │ │ │ +  New-style numerical derivatives using │ │ │ │ │ + manifold_traits. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X >:: ggttssaamm::::nnuummeerriiccaallDDeerriivvaattiivvee1111 (Y(*h) │ │ │ │ │ + type  (const X &), const X &x, double │ │ │ │ │ + delta=1e-5) │ │ │ │ │ +  use a raw C++ function pointer │ │ │ │ │ +  │ │ │ │ │ +template::dimension> │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X1 >:: _g_t_s_a_m_:_:_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_2_1 (const │ │ │ │ │ + type  std::function< Y(const X1 &, const X2 │ │ │ │ │ + &)> &h, const X1 &x1, const X2 &x2, │ │ │ │ │ + double delta=1e-5) │ │ │ │ │ +  Compute numerical derivative in argument │ │ │ │ │ + 1 of binary function. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X1 >:: ggttssaamm::::nnuummeerriiccaallDDeerriivvaattiivvee2211 (Y(*h) │ │ │ │ │ + type  (const X1 &, const X2 &), const X1 &x1, │ │ │ │ │ + const X2 &x2, double delta=1e-5) │ │ │ │ │ +  use a raw C++ function pointer │ │ │ │ │ +  │ │ │ │ │ +template::dimension> │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X2 >:: _g_t_s_a_m_:_:_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_2_2 (std:: │ │ │ │ │ + type  function< Y(const X1 &, const X2 &)> h, │ │ │ │ │ + const X1 &x1, const X2 &x2, double │ │ │ │ │ + delta=1e-5) │ │ │ │ │ +  Compute numerical derivative in argument │ │ │ │ │ + 2 of binary function. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X2 >:: ggttssaamm::::nnuummeerriiccaallDDeerriivvaattiivvee2222 (Y(*h) │ │ │ │ │ + type  (const X1 &, const X2 &), const X1 &x1, │ │ │ │ │ + const X2 &x2, double delta=1e-5) │ │ │ │ │ +  use a raw C++ function pointer │ │ │ │ │ +  │ │ │ │ │ +template:: │ │ │ │ │ +dimension> │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X1 >:: _g_t_s_a_m_:_:_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_3_1 (std:: │ │ │ │ │ + type  function< Y(const X1 &, const X2 &, │ │ │ │ │ + const X3 &)> h, const X1 &x1, const X2 │ │ │ │ │ + &x2, const X3 &x3, double delta=1e-5) │ │ │ │ │ +  Compute numerical derivative in argument │ │ │ │ │ + 1 of ternary function. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X1 >:: ggttssaamm::::nnuummeerriiccaallDDeerriivvaattiivvee3311 (Y(*h) │ │ │ │ │ + type  (const X1 &, const X2 &, const X3 &), │ │ │ │ │ + const X1 &x1, const X2 &x2, const X3 │ │ │ │ │ + &x3, double delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template:: │ │ │ │ │ +dimension> │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X2 >:: _g_t_s_a_m_:_:_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_3_2 (std:: │ │ │ │ │ + type  function< Y(const X1 &, const X2 &, │ │ │ │ │ + const X3 &)> h, const X1 &x1, const X2 │ │ │ │ │ + &x2, const X3 &x3, double delta=1e-5) │ │ │ │ │ +  Compute numerical derivative in argument │ │ │ │ │ + 2 of ternary function. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X2 >:: ggttssaamm::::nnuummeerriiccaallDDeerriivvaattiivvee3322 (Y(*h) │ │ │ │ │ + type  (const X1 &, const X2 &, const X3 &), │ │ │ │ │ + const X1 &x1, const X2 &x2, const X3 │ │ │ │ │ + &x3, double delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template:: │ │ │ │ │ +dimension> │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X3 >:: _g_t_s_a_m_:_:_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_3_3 (std:: │ │ │ │ │ + type  function< Y(const X1 &, const X2 &, │ │ │ │ │ + const X3 &)> h, const X1 &x1, const X2 │ │ │ │ │ + &x2, const X3 &x3, double delta=1e-5) │ │ │ │ │ +  Compute numerical derivative in argument │ │ │ │ │ + 3 of ternary function. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X3 >:: ggttssaamm::::nnuummeerriiccaallDDeerriivvaattiivvee3333 (Y(*h) │ │ │ │ │ + type  (const X1 &, const X2 &, const X3 &), │ │ │ │ │ + const X1 &x1, const X2 &x2, const X3 │ │ │ │ │ + &x3, double delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template::dimension> │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X1 >:: _g_t_s_a_m_:_:_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_4_1 (std:: │ │ │ │ │ + type  function< Y(const X1 &, const X2 &, │ │ │ │ │ + const X3 &, const X4 &)> h, const X1 │ │ │ │ │ + &x1, const X2 &x2, const X3 &x3, const │ │ │ │ │ + X4 &x4, double delta=1e-5) │ │ │ │ │ +  Compute numerical derivative in argument │ │ │ │ │ + 1 of 4-argument function. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X1 >:: ggttssaamm::::nnuummeerriiccaallDDeerriivvaattiivvee4411 (Y(*h) │ │ │ │ │ + type  (const X1 &, const X2 &, const X3 &, │ │ │ │ │ + const X4 &), const X1 &x1, const X2 &x2, │ │ │ │ │ + const X3 &x3, const X4 &x4, double │ │ │ │ │ + delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template::dimension> │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X2 >:: _g_t_s_a_m_:_:_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_4_2 (std:: │ │ │ │ │ + type  function< Y(const X1 &, const X2 &, │ │ │ │ │ + const X3 &, const X4 &)> h, const X1 │ │ │ │ │ + &x1, const X2 &x2, const X3 &x3, const │ │ │ │ │ + X4 &x4, double delta=1e-5) │ │ │ │ │ +  Compute numerical derivative in argument │ │ │ │ │ + 2 of 4-argument function. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X2 >:: ggttssaamm::::nnuummeerriiccaallDDeerriivvaattiivvee4422 (Y(*h) │ │ │ │ │ + type  (const X1 &, const X2 &, const X3 &, │ │ │ │ │ + const X4 &), const X1 &x1, const X2 &x2, │ │ │ │ │ + const X3 &x3, const X4 &x4, double │ │ │ │ │ + delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template::dimension> │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X3 >:: _g_t_s_a_m_:_:_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_4_3 (std:: │ │ │ │ │ + type  function< Y(const X1 &, const X2 &, │ │ │ │ │ + const X3 &, const X4 &)> h, const X1 │ │ │ │ │ + &x1, const X2 &x2, const X3 &x3, const │ │ │ │ │ + X4 &x4, double delta=1e-5) │ │ │ │ │ +  Compute numerical derivative in argument │ │ │ │ │ + 3 of 4-argument function. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X3 >:: ggttssaamm::::nnuummeerriiccaallDDeerriivvaattiivvee4433 (Y(*h) │ │ │ │ │ + type  (const X1 &, const X2 &, const X3 &, │ │ │ │ │ + const X4 &), const X1 &x1, const X2 &x2, │ │ │ │ │ + const X3 &x3, const X4 &x4, double │ │ │ │ │ + delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template::dimension> │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X4 >:: _g_t_s_a_m_:_:_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_4_4 (std:: │ │ │ │ │ + type  function< Y(const X1 &, const X2 &, │ │ │ │ │ + const X3 &, const X4 &)> h, const X1 │ │ │ │ │ + &x1, const X2 &x2, const X3 &x3, const │ │ │ │ │ + X4 &x4, double delta=1e-5) │ │ │ │ │ +  Compute numerical derivative in argument │ │ │ │ │ + 4 of 4-argument function. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X4 >:: ggttssaamm::::nnuummeerriiccaallDDeerriivvaattiivvee4444 (Y(*h) │ │ │ │ │ + type  (const X1 &, const X2 &, const X3 &, │ │ │ │ │ + const X4 &), const X1 &x1, const X2 &x2, │ │ │ │ │ + const X3 &x3, const X4 &x4, double │ │ │ │ │ + delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template::dimension> │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X1 >:: _g_t_s_a_m_:_:_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_5_1 (std:: │ │ │ │ │ + type  function< Y(const X1 &, const X2 &, │ │ │ │ │ + const X3 &, const X4 &, const X5 &)> h, │ │ │ │ │ + const X1 &x1, const X2 &x2, const X3 │ │ │ │ │ + &x3, const X4 &x4, const X5 &x5, double │ │ │ │ │ + delta=1e-5) │ │ │ │ │ +  Compute numerical derivative in argument │ │ │ │ │ + 1 of 5-argument function. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X1 >:: ggttssaamm::::nnuummeerriiccaallDDeerriivvaattiivvee5511 (Y(*h) │ │ │ │ │ + type  (const X1 &, const X2 &, const X3 &, │ │ │ │ │ + const X4 &, const X5 &), const X1 &x1, │ │ │ │ │ + const X2 &x2, const X3 &x3, const X4 │ │ │ │ │ + &x4, const X5 &x5, double delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template::dimension> │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X2 >:: _g_t_s_a_m_:_:_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_5_2 (std:: │ │ │ │ │ + type  function< Y(const X1 &, const X2 &, │ │ │ │ │ + const X3 &, const X4 &, const X5 &)> h, │ │ │ │ │ + const X1 &x1, const X2 &x2, const X3 │ │ │ │ │ + &x3, const X4 &x4, const X5 &x5, double │ │ │ │ │ + delta=1e-5) │ │ │ │ │ +  Compute numerical derivative in argument │ │ │ │ │ + 2 of 5-argument function. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X2 >:: ggttssaamm::::nnuummeerriiccaallDDeerriivvaattiivvee5522 (Y(*h) │ │ │ │ │ + type  (const X1 &, const X2 &, const X3 &, │ │ │ │ │ + const X4 &, const X5 &), const X1 &x1, │ │ │ │ │ + const X2 &x2, const X3 &x3, const X4 │ │ │ │ │ + &x4, const X5 &x5, double delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template::dimension> │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X3 >:: _g_t_s_a_m_:_:_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_5_3 (std:: │ │ │ │ │ + type  function< Y(const X1 &, const X2 &, │ │ │ │ │ + const X3 &, const X4 &, const X5 &)> h, │ │ │ │ │ + const X1 &x1, const X2 &x2, const X3 │ │ │ │ │ + &x3, const X4 &x4, const X5 &x5, double │ │ │ │ │ + delta=1e-5) │ │ │ │ │ +  Compute numerical derivative in argument │ │ │ │ │ + 3 of 5-argument function. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X3 >:: ggttssaamm::::nnuummeerriiccaallDDeerriivvaattiivvee5533 (Y(*h) │ │ │ │ │ + type  (const X1 &, const X2 &, const X3 &, │ │ │ │ │ + const X4 &, const X5 &), const X1 &x1, │ │ │ │ │ + const X2 &x2, const X3 &x3, const X4 │ │ │ │ │ + &x4, const X5 &x5, double delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template::dimension> │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X4 >:: _g_t_s_a_m_:_:_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_5_4 (std:: │ │ │ │ │ + type  function< Y(const X1 &, const X2 &, │ │ │ │ │ + const X3 &, const X4 &, const X5 &)> h, │ │ │ │ │ + const X1 &x1, const X2 &x2, const X3 │ │ │ │ │ + &x3, const X4 &x4, const X5 &x5, double │ │ │ │ │ + delta=1e-5) │ │ │ │ │ +  Compute numerical derivative in argument │ │ │ │ │ + 4 of 5-argument function. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X4 >:: ggttssaamm::::nnuummeerriiccaallDDeerriivvaattiivvee5544 (Y(*h) │ │ │ │ │ + type  (const X1 &, const X2 &, const X3 &, │ │ │ │ │ + const X4 &, const X5 &), const X1 &x1, │ │ │ │ │ + const X2 &x2, const X3 &x3, const X4 │ │ │ │ │ + &x4, const X5 &x5, double delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template::dimension> │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X5 >:: _g_t_s_a_m_:_:_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_5_5 (std:: │ │ │ │ │ + type  function< Y(const X1 &, const X2 &, │ │ │ │ │ + const X3 &, const X4 &, const X5 &)> h, │ │ │ │ │ + const X1 &x1, const X2 &x2, const X3 │ │ │ │ │ + &x3, const X4 &x4, const X5 &x5, double │ │ │ │ │ + delta=1e-5) │ │ │ │ │ +  Compute numerical derivative in argument │ │ │ │ │ + 5 of 5-argument function. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X5 >:: ggttssaamm::::nnuummeerriiccaallDDeerriivvaattiivvee5555 (Y(*h) │ │ │ │ │ + type  (const X1 &, const X2 &, const X3 &, │ │ │ │ │ + const X4 &, const X5 &), const X1 &x1, │ │ │ │ │ + const X2 &x2, const X3 &x3, const X4 │ │ │ │ │ + &x4, const X5 &x5, double delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template::dimension> │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X1 >:: _g_t_s_a_m_:_:_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_6_1 (std:: │ │ │ │ │ + type  function< Y(const X1 &, const X2 &, │ │ │ │ │ + const X3 &, const X4 &, const X5 &, │ │ │ │ │ + const X6 &)> h, const X1 &x1, const X2 │ │ │ │ │ + &x2, const X3 &x3, const X4 &x4, const │ │ │ │ │ + X5 &x5, const X6 &x6, double delta=1e-5) │ │ │ │ │ +  Compute numerical derivative in argument │ │ │ │ │ + 1 of 6-argument function. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X1 >:: ggttssaamm::::nnuummeerriiccaallDDeerriivvaattiivvee6611 (Y(*h) │ │ │ │ │ + type  (const X1 &, const X2 &, const X3 &, │ │ │ │ │ + const X4 &, const X5 &, const X6 &), │ │ │ │ │ + const X1 &x1, const X2 &x2, const X3 │ │ │ │ │ + &x3, const X4 &x4, const X5 &x5, const │ │ │ │ │ + X6 &x6, double delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template::dimension> │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X2 >:: _g_t_s_a_m_:_:_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_6_2 (std:: │ │ │ │ │ + type  function< Y(const X1 &, const X2 &, │ │ │ │ │ + const X3 &, const X4 &, const X5 &, │ │ │ │ │ + const X6 &)> h, const X1 &x1, const X2 │ │ │ │ │ + &x2, const X3 &x3, const X4 &x4, const │ │ │ │ │ + X5 &x5, const X6 &x6, double delta=1e-5) │ │ │ │ │ +  Compute numerical derivative in argument │ │ │ │ │ + 2 of 6-argument function. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X2 >:: ggttssaamm::::nnuummeerriiccaallDDeerriivvaattiivvee6622 (Y(*h) │ │ │ │ │ + type  (const X1 &, const X2 &, const X3 &, │ │ │ │ │ + const X4 &, const X5 &, const X6 &), │ │ │ │ │ + const X1 &x1, const X2 &x2, const X3 │ │ │ │ │ + &x3, const X4 &x4, const X5 &x5, const │ │ │ │ │ + X6 &x6, double delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template::dimension> │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X3 >:: _g_t_s_a_m_:_:_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_6_3 (std:: │ │ │ │ │ + type  function< Y(const X1 &, const X2 &, │ │ │ │ │ + const X3 &, const X4 &, const X5 &, │ │ │ │ │ + const X6 &)> h, const X1 &x1, const X2 │ │ │ │ │ + &x2, const X3 &x3, const X4 &x4, const │ │ │ │ │ + X5 &x5, const X6 &x6, double delta=1e-5) │ │ │ │ │ +  Compute numerical derivative in argument │ │ │ │ │ + 3 of 6-argument function. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X3 >:: ggttssaamm::::nnuummeerriiccaallDDeerriivvaattiivvee6633 (Y(*h) │ │ │ │ │ + type  (const X1 &, const X2 &, const X3 &, │ │ │ │ │ + const X4 &, const X5 &, const X6 &), │ │ │ │ │ + const X1 &x1, const X2 &x2, const X3 │ │ │ │ │ + &x3, const X4 &x4, const X5 &x5, const │ │ │ │ │ + X6 &x6, double delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template::dimension> │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X4 >:: _g_t_s_a_m_:_:_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_6_4 (std:: │ │ │ │ │ + type  function< Y(const X1 &, const X2 &, │ │ │ │ │ + const X3 &, const X4 &, const X5 &, │ │ │ │ │ + const X6 &)> h, const X1 &x1, const X2 │ │ │ │ │ + &x2, const X3 &x3, const X4 &x4, const │ │ │ │ │ + X5 &x5, const X6 &x6, double delta=1e-5) │ │ │ │ │ +  Compute numerical derivative in argument │ │ │ │ │ + 4 of 6-argument function. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X4 >:: ggttssaamm::::nnuummeerriiccaallDDeerriivvaattiivvee6644 (Y(*h) │ │ │ │ │ + type  (const X1 &, const X2 &, const X3 &, │ │ │ │ │ + const X4 &, const X5 &, const X6 &), │ │ │ │ │ + const X1 &x1, const X2 &x2, const X3 │ │ │ │ │ + &x3, const X4 &x4, const X5 &x5, const │ │ │ │ │ + X6 &x6, double delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template::dimension> │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X5 >:: _g_t_s_a_m_:_:_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_6_5 (std:: │ │ │ │ │ + type  function< Y(const X1 &, const X2 &, │ │ │ │ │ + const X3 &, const X4 &, const X5 &, │ │ │ │ │ + const X6 &)> h, const X1 &x1, const X2 │ │ │ │ │ + &x2, const X3 &x3, const X4 &x4, const │ │ │ │ │ + X5 &x5, const X6 &x6, double delta=1e-5) │ │ │ │ │ +  Compute numerical derivative in argument │ │ │ │ │ + 5 of 6-argument function. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X5 >:: ggttssaamm::::nnuummeerriiccaallDDeerriivvaattiivvee6655 (Y(*h) │ │ │ │ │ + type  (const X1 &, const X2 &, const X3 &, │ │ │ │ │ + const X4 &, const X5 &, const X6 &), │ │ │ │ │ + const X1 &x1, const X2 &x2, const X3 │ │ │ │ │ + &x3, const X4 &x4, const X5 &x5, const │ │ │ │ │ + X6 &x6, double delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template::dimension> │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X6 >:: _g_t_s_a_m_:_:_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_6_6 (std:: │ │ │ │ │ + type  function< Y(const X1 &, const X2 &, │ │ │ │ │ + const X3 &, const X4 &, const X5 &, │ │ │ │ │ + const X6 &)> h, const X1 &x1, const X2 │ │ │ │ │ + &x2, const X3 &x3, const X4 &x4, const │ │ │ │ │ + X5 &x5, const X6 &x6, double delta=1e-5) │ │ │ │ │ +  Compute numerical derivative in argument │ │ │ │ │ + 6 of 6-argument function. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< Y, X6 >:: ggttssaamm::::nnuummeerriiccaallDDeerriivvaattiivvee6666 (Y(*h) │ │ │ │ │ + type  (const X1 &, const X2 &, const X3 &, │ │ │ │ │ + const X4 &, const X5 &, const X6 &), │ │ │ │ │ + const X1 &x1, const X2 &x2, const X3 │ │ │ │ │ + &x3, const X4 &x4, const X5 &x5, const │ │ │ │ │ + X6 &x6, double delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< X, X >:: _g_t_s_a_m_:_:_n_u_m_e_r_i_c_a_l_H_e_s_s_i_a_n (std::function< │ │ │ │ │ + type  double(const X &)> f, const X &x, double │ │ │ │ │ + delta=1e-5) │ │ │ │ │ +  Compute numerical Hessian matrix. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< X, X >:: ggttssaamm::::nnuummeerriiccaallHHeessssiiaann (double(*f) │ │ │ │ │ + type  (const X &), const X &x, double │ │ │ │ │ + delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +_i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< X1, X2 >:: ggttssaamm::::nnuummeerriiccaallHHeessssiiaann221122 (std:: │ │ │ │ │ + type  function< double(const X1 &, const X2 │ │ │ │ │ + &)> f, const X1 &x1, const X2 &x2, │ │ │ │ │ + double delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +_i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< X1, X2 >:: ggttssaamm::::nnuummeerriiccaallHHeessssiiaann221122 (double(*f) │ │ │ │ │ + type  (const X1 &, const X2 &), const X1 &x1, │ │ │ │ │ + const X2 &x2, double delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +_i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< X1, X1 >:: ggttssaamm::::nnuummeerriiccaallHHeessssiiaann221111 (std:: │ │ │ │ │ + type  function< double(const X1 &, const X2 │ │ │ │ │ + &)> f, const X1 &x1, const X2 &x2, │ │ │ │ │ + double delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +_i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< X1, X1 >:: ggttssaamm::::nnuummeerriiccaallHHeessssiiaann221111 (double(*f) │ │ │ │ │ + type  (const X1 &, const X2 &), const X1 &x1, │ │ │ │ │ + const X2 &x2, double delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +_i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< X2, X2 >:: ggttssaamm::::nnuummeerriiccaallHHeessssiiaann222222 (std:: │ │ │ │ │ + type  function< double(const X1 &, const X2 │ │ │ │ │ + &)> f, const X1 &x1, const X2 &x2, │ │ │ │ │ + double delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +_i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< X2, X2 >:: ggttssaamm::::nnuummeerriiccaallHHeessssiiaann222222 (double(*f) │ │ │ │ │ + type  (const X1 &, const X2 &), const X1 &x1, │ │ │ │ │ + const X2 &x2, double delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +_i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< X1, X1 >:: ggttssaamm::::nnuummeerriiccaallHHeessssiiaann331111 (std:: │ │ │ │ │ + type  function< double(const X1 &, const X2 &, │ │ │ │ │ + const X3 &)> f, const X1 &x1, const X2 │ │ │ │ │ + &x2, const X3 &x3, double delta=1e-5) │ │ │ │ │ +  Numerical Hessian for tenary functions. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +_i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< X1, X1 >:: ggttssaamm::::nnuummeerriiccaallHHeessssiiaann331111 (double(*f) │ │ │ │ │ + type  (const X1 &, const X2 &, const X3 &), │ │ │ │ │ + const X1 &x1, const X2 &x2, const X3 │ │ │ │ │ + &x3, double delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +_i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< X2, X2 >:: ggttssaamm::::nnuummeerriiccaallHHeessssiiaann332222 (std:: │ │ │ │ │ + type  function< double(const X1 &, const X2 &, │ │ │ │ │ + const X3 &)> f, const X1 &x1, const X2 │ │ │ │ │ + &x2, const X3 &x3, double delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +_i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< X2, X2 >:: ggttssaamm::::nnuummeerriiccaallHHeessssiiaann332222 (double(*f) │ │ │ │ │ + type  (const X1 &, const X2 &, const X3 &), │ │ │ │ │ + const X1 &x1, const X2 &x2, const X3 │ │ │ │ │ + &x3, double delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +_i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< X3, X3 >:: ggttssaamm::::nnuummeerriiccaallHHeessssiiaann333333 (std:: │ │ │ │ │ + type  function< double(const X1 &, const X2 &, │ │ │ │ │ + const X3 &)> f, const X1 &x1, const X2 │ │ │ │ │ + &x2, const X3 &x3, double delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +_i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< X3, X3 >:: ggttssaamm::::nnuummeerriiccaallHHeessssiiaann333333 (double(*f) │ │ │ │ │ + type  (const X1 &, const X2 &, const X3 &), │ │ │ │ │ + const X1 &x1, const X2 &x2, const X3 │ │ │ │ │ + &x3, double delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +_i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< X1, X2 >:: ggttssaamm::::nnuummeerriiccaallHHeessssiiaann331122 (std:: │ │ │ │ │ + type  function< double(const X1 &, const X2 &, │ │ │ │ │ + const X3 &)> f, const X1 &x1, const X2 │ │ │ │ │ + &x2, const X3 &x3, double delta=1e-5) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +_i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< X1, X3 >:: ggttssaamm::::nnuummeerriiccaallHHeessssiiaann331133 (std:: │ │ │ │ │ + type  function< double(const X1 &, const X2 &, │ │ │ │ │ + const X3 &)> f, const X1 &x1, const X2 │ │ │ │ │ + &x2, const X3 &x3, double delta=1e-5) │ │ │ │ │   │ │ │ │ │ - GTSAM_EXPORT boost::weak_ptr< ggttssaamm::::iinntteerrnnaall::::ggCCuurrrreennttTTiimmeerr │ │ │ │ │ - _T_i_m_i_n_g_O_u_t_l_i_n_e >  (gTimingRoot) │ │ │ │ │ +template │ │ │ │ │ +_i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< X2, X3 >:: ggttssaamm::::nnuummeerriiccaallHHeessssiiaann332233 (std:: │ │ │ │ │ + type  function< double(const X1 &, const X2 &, │ │ │ │ │ + const X3 &)> f, const X1 &x1, const X2 │ │ │ │ │ + &x2, const X3 &x3, double delta=1e-5) │ │ │ │ │   │ │ │ │ │ - size_t  ggttssaamm::::iinntteerrnnaall::::ggeettTTiiccTTooccIIDD (const char │ │ │ │ │ - *descriptionC) │ │ │ │ │ +template │ │ │ │ │ +_i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< X1, X2 >:: ggttssaamm::::nnuummeerriiccaallHHeessssiiaann331122 (double(*f) │ │ │ │ │ + type  (const X1 &, const X2 &, const X3 &), │ │ │ │ │ + const X1 &x1, const X2 &x2, const X3 │ │ │ │ │ + &x3, double delta=1e-5) │ │ │ │ │   │ │ │ │ │ - void  ggttssaamm::::iinntteerrnnaall::::ttiicc (size_t id, const char │ │ │ │ │ - *labelC) │ │ │ │ │ +template │ │ │ │ │ +_i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< X1, X3 >:: ggttssaamm::::nnuummeerriiccaallHHeessssiiaann331133 (double(*f) │ │ │ │ │ + type  (const X1 &, const X2 &, const X3 &), │ │ │ │ │ + const X1 &x1, const X2 &x2, const X3 │ │ │ │ │ + &x3, double delta=1e-5) │ │ │ │ │   │ │ │ │ │ - void  ggttssaamm::::iinntteerrnnaall::::ttoocc (size_t id, const char │ │ │ │ │ - *label) │ │ │ │ │ +template │ │ │ │ │ +_i_n_t_e_r_n_a_l_:_:_F_i_x_e_d_S_i_z_e_M_a_t_r_i_x< X2, X3 >:: ggttssaamm::::nnuummeerriiccaallHHeessssiiaann332233 (double(*f) │ │ │ │ │ + type  (const X1 &, const X2 &, const X3 &), │ │ │ │ │ + const X1 &x1, const X2 &x2, const X3 │ │ │ │ │ + &x3, double delta=1e-5) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Timing utilities. │ │ │ │ │ +Some functions to compute numerical derivatives. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts, Michael Kaess │ │ │ │ │ - Date │ │ │ │ │ - Oct 5, 2010 │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _t_i_m_i_n_g_._c_p_p │ │ │ │ │ + * _n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00020.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/OptionalJacobian.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/VectorSerialization.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,55 +94,51 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
OptionalJacobian.h File Reference
│ │ │ │ +Functions
│ │ │ │ +
VectorSerialization.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Special class for optional Jacobian arguments. │ │ │ │ +

serialization for Vectors │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::OptionalJacobian< Rows, Cols >
 OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size or dynamic Eigen matrix. More...
 
class  gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >
 
struct  gtsam::MakeJacobian< T, A >
 : meta-function to generate Jacobian More...
 
struct  gtsam::MakeOptionalJacobian< T, A >
 : meta-function to generate JacobianTA optional reference Used mainly by Expressions More...
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ -Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 

│ │ │ │ +Functions

│ │ │ │ +template<class Archive >
void boost::serialization::save (Archive &ar, const gtsam::Vector &v, unsigned int)
 
│ │ │ │ +template<class Archive >
void boost::serialization::load (Archive &ar, gtsam::Vector &v, unsigned int)
 
│ │ │ │ +template<class Archive , int D>
void boost::serialization::save (Archive &ar, const Eigen::Matrix< double, D, 1 > &v, unsigned int)
 
│ │ │ │ +template<class Archive , int D>
void boost::serialization::load (Archive &ar, Eigen::Matrix< double, D, 1 > &v, unsigned int)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Special class for optional Jacobian arguments.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ -Natesh Srinivasan
│ │ │ │ -
Date
Nov 28, 2014
│ │ │ │ +

serialization for Vectors

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
Date
February 2022
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,38 +1,35 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -OptionalJacobian.h File Reference │ │ │ │ │ -Special class for optional Jacobian arguments. _M_o_r_e_._._. │ │ │ │ │ +_F_u_n_c_t_i_o_n_s │ │ │ │ │ +VectorSerialization.h File Reference │ │ │ │ │ +serialization for Vectors _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_ _R_o_w_s_,_ _C_o_l_s_ _> │ │ │ │ │ -  _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n is an Eigen::Ref like class that can take be │ │ │ │ │ - constructed using either a fixed size or dynamic Eigen matrix. _M_o_r_e_._._. │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +template │ │ │ │ │ +void  bboooosstt::::sseerriiaalliizzaattiioonn::::ssaavvee (Archive &ar, const gtsam::Vector &v, unsigned │ │ │ │ │ + int) │ │ │ │ │   │ │ │ │ │ - class   _g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_,_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_ _> │ │ │ │ │ +template │ │ │ │ │ +void  bboooosstt::::sseerriiaalliizzaattiioonn::::llooaadd (Archive &ar, gtsam::Vector &v, unsigned int) │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_M_a_k_e_J_a_c_o_b_i_a_n_<_ _T_,_ _A_ _> │ │ │ │ │ -  : meta-function to generate Jacobian _M_o_r_e_._._. │ │ │ │ │ +template │ │ │ │ │ +void  bboooosstt::::sseerriiaalliizzaattiioonn::::ssaavvee (Archive &ar, const Eigen::Matrix< double, D, │ │ │ │ │ + 1 > &v, unsigned int) │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_ _T_,_ _A_ _> │ │ │ │ │ -  : meta-function to generate JacobianTA optional reference Used mainly │ │ │ │ │ - by Expressions _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -NNaammeessppaacceess │ │ │ │ │ -namespace   _g_t_s_a_m │ │ │ │ │ -  Global functions in a separate testing namespace. │ │ │ │ │ +template │ │ │ │ │ +void  bboooosstt::::sseerriiaalliizzaattiioonn::::llooaadd (Archive &ar, Eigen::Matrix< double, D, 1 > │ │ │ │ │ + &v, unsigned int) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Special class for optional Jacobian arguments. │ │ │ │ │ +serialization for Vectors │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Natesh Srinivasan │ │ │ │ │ Date │ │ │ │ │ - Nov 28, 2014 │ │ │ │ │ + February 2022 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h │ │ │ │ │ + * _V_e_c_t_o_r_S_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00020_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/OptionalJacobian.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/VectorSerialization.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,292 +98,81 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
OptionalJacobian.h
│ │ │ │ +
VectorSerialization.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21#include <gtsam/config.h> // Configuration from CMake
│ │ │ │ -
22#include <Eigen/Dense>
│ │ │ │ -
23#include <stdexcept>
│ │ │ │ -
24#include <string>
│ │ │ │ -
25
│ │ │ │ -
26#ifndef OPTIONALJACOBIAN_NOBOOST
│ │ │ │ -
27#include <boost/optional.hpp>
│ │ │ │ -
28#endif
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/base/Vector.h>
│ │ │ │ +
22
│ │ │ │ +
23#include <boost/serialization/array.hpp>
│ │ │ │ +
24#include <boost/serialization/nvp.hpp>
│ │ │ │ +
25#include <boost/serialization/split_free.hpp>
│ │ │ │ +
26
│ │ │ │ +
27namespace boost {
│ │ │ │ +
28namespace serialization {
│ │ │ │
29
│ │ │ │ -
30namespace gtsam {
│ │ │ │ -
31
│ │ │ │ -
40template<int Rows, int Cols>
│ │ │ │ -
│ │ │ │ -
41class OptionalJacobian {
│ │ │ │ -
42
│ │ │ │ -
43public:
│ │ │ │ -
44
│ │ │ │ -
47 typedef Eigen::Matrix<double, Rows, Cols> Jacobian;
│ │ │ │ -
48
│ │ │ │ -
49private:
│ │ │ │ -
50
│ │ │ │ -
51 Eigen::Map<Jacobian> map_;
│ │ │ │ +
30// split version - copies into an STL vector for serialization
│ │ │ │ +
31template <class Archive>
│ │ │ │ +
32void save(Archive& ar, const gtsam::Vector& v, unsigned int /*version*/) {
│ │ │ │ +
33 const size_t size = v.size();
│ │ │ │ +
34 ar << BOOST_SERIALIZATION_NVP(size);
│ │ │ │ +
35 ar << make_nvp("data", make_array(v.data(), v.size()));
│ │ │ │ +
36}
│ │ │ │ +
37
│ │ │ │ +
38template <class Archive>
│ │ │ │ +
39void load(Archive& ar, gtsam::Vector& v, unsigned int /*version*/) {
│ │ │ │ +
40 size_t size;
│ │ │ │ +
41 ar >> BOOST_SERIALIZATION_NVP(size);
│ │ │ │ +
42 v.resize(size);
│ │ │ │ +
43 ar >> make_nvp("data", make_array(v.data(), v.size()));
│ │ │ │ +
44}
│ │ │ │ +
45
│ │ │ │ +
46// split version - copies into an STL vector for serialization
│ │ │ │ +
47template <class Archive, int D>
│ │ │ │ +
48void save(Archive& ar, const Eigen::Matrix<double, D, 1>& v,
│ │ │ │ +
49 unsigned int /*version*/) {
│ │ │ │ +
50 ar << make_nvp("data", make_array(v.data(), v.RowsAtCompileTime));
│ │ │ │ +
51}
│ │ │ │
52
│ │ │ │ -
53 // Trick from http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html
│ │ │ │ -
54 // uses "placement new" to make map_ usurp the memory of the fixed size matrix
│ │ │ │ -
55 void usurp(double* data) {
│ │ │ │ -
56 new (&map_) Eigen::Map<Jacobian>(data);
│ │ │ │ -
57 }
│ │ │ │ +
53template <class Archive, int D>
│ │ │ │ +
54void load(Archive& ar, Eigen::Matrix<double, D, 1>& v,
│ │ │ │ +
55 unsigned int /*version*/) {
│ │ │ │ +
56 ar >> make_nvp("data", make_array(v.data(), v.RowsAtCompileTime));
│ │ │ │ +
57}
│ │ │ │
58
│ │ │ │ -
59 // Private and very dangerous constructor straight from memory
│ │ │ │ -
60 OptionalJacobian(double* data) : map_(nullptr) {
│ │ │ │ -
61 if (data) usurp(data);
│ │ │ │ -
62 }
│ │ │ │ -
63
│ │ │ │ -
64 template<int M, int N>
│ │ │ │ -
65 friend class OptionalJacobian;
│ │ │ │ -
66
│ │ │ │ -
67public:
│ │ │ │ -
68
│ │ │ │ -
│ │ │ │ -
70 OptionalJacobian() :
│ │ │ │ -
71 map_(nullptr) {
│ │ │ │ -
72 }
│ │ │ │ -
│ │ │ │ -
73
│ │ │ │ -
│ │ │ │ -
75 OptionalJacobian(Jacobian& fixed) :
│ │ │ │ -
76 map_(nullptr) {
│ │ │ │ -
77 usurp(fixed.data());
│ │ │ │ -
78 }
│ │ │ │ -
│ │ │ │ -
79
│ │ │ │ -
│ │ │ │ -
81 OptionalJacobian(Jacobian* fixedPtr) :
│ │ │ │ -
82 map_(nullptr) {
│ │ │ │ -
83 if (fixedPtr)
│ │ │ │ -
84 usurp(fixedPtr->data());
│ │ │ │ -
85 }
│ │ │ │ -
│ │ │ │ -
86
│ │ │ │ -
│ │ │ │ -
88 OptionalJacobian(Eigen::MatrixXd& dynamic) :
│ │ │ │ -
89 map_(nullptr) {
│ │ │ │ -
90 dynamic.resize(Rows, Cols); // no malloc if correct size
│ │ │ │ -
91 usurp(dynamic.data());
│ │ │ │ -
92 }
│ │ │ │ -
│ │ │ │ -
93
│ │ │ │ -
│ │ │ │ -
95 OptionalJacobian(Eigen::MatrixXd* dynamic) :
│ │ │ │ -
96 map_(nullptr) {
│ │ │ │ -
97 dynamic->resize(Rows, Cols); // no malloc if correct size
│ │ │ │ -
98 usurp(dynamic->data());
│ │ │ │ -
99 }
│ │ │ │ -
│ │ │ │ -
100
│ │ │ │ -
105 template<class MATRIX>
│ │ │ │ -
│ │ │ │ -
106 OptionalJacobian(Eigen::Ref<MATRIX> dynamic_ref) :
│ │ │ │ -
107 map_(nullptr) {
│ │ │ │ -
108 if (dynamic_ref.rows() == Rows && dynamic_ref.cols() == Cols && !dynamic_ref.IsRowMajor) {
│ │ │ │ -
109 usurp(dynamic_ref.data());
│ │ │ │ -
110 } else {
│ │ │ │ -
111 throw std::invalid_argument(
│ │ │ │ -
112 std::string("OptionalJacobian called with wrong dimensions or "
│ │ │ │ -
113 "storage order.\n"
│ │ │ │ -
114 "Expected: ") +
│ │ │ │ -
115 "(" + std::to_string(Rows) + ", " + std::to_string(Cols) + ")");
│ │ │ │ -
116 }
│ │ │ │ -
117 }
│ │ │ │ -
│ │ │ │ -
118
│ │ │ │ -
119#ifndef OPTIONALJACOBIAN_NOBOOST
│ │ │ │ -
120
│ │ │ │ -
│ │ │ │ -
122 OptionalJacobian(boost::none_t /*none*/) :
│ │ │ │ -
123 map_(nullptr) {
│ │ │ │ -
124 }
│ │ │ │ -
│ │ │ │ -
125
│ │ │ │ -
│ │ │ │ -
127 OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
│ │ │ │ -
128 map_(nullptr) {
│ │ │ │ -
129 if (optional) {
│ │ │ │ -
130 optional->resize(Rows, Cols);
│ │ │ │ -
131 usurp(optional->data());
│ │ │ │ -
132 }
│ │ │ │ -
133 }
│ │ │ │ -
│ │ │ │ -
134
│ │ │ │ -
135#endif
│ │ │ │ -
136
│ │ │ │ -
139 // template <typename Derived, bool InnerPanel>
│ │ │ │ -
140 // OptionalJacobian(Eigen::Block<Derived,Rows,Cols,InnerPanel> block) : map_(nullptr) { ?? }
│ │ │ │ -
141
│ │ │ │ -
│ │ │ │ -
143 operator bool() const {
│ │ │ │ -
144 return map_.data() != nullptr;
│ │ │ │ -
145 }
│ │ │ │ -
│ │ │ │ -
146
│ │ │ │ -
│ │ │ │ -
148 Eigen::Map<Jacobian>& operator*() {
│ │ │ │ -
149 return map_;
│ │ │ │ -
150 }
│ │ │ │ -
│ │ │ │ -
151
│ │ │ │ -
│ │ │ │ -
153 Eigen::Map<Jacobian>* operator->() {
│ │ │ │ -
154 return &map_;
│ │ │ │ -
155 }
│ │ │ │ -
│ │ │ │ -
156
│ │ │ │ -
159 // template <int M, int N>
│ │ │ │ -
160 // OptionalJacobian<M, N> block(int startRow, int startCol) {
│ │ │ │ -
161 // if (*this)
│ │ │ │ -
162 // OptionalJacobian<M, N>(map_.block<M, N>(startRow, startCol));
│ │ │ │ -
163 // else
│ │ │ │ -
164 // return OptionalJacobian<M, N>();
│ │ │ │ -
165 // }
│ │ │ │ -
166
│ │ │ │ -
170 template <int N>
│ │ │ │ -
│ │ │ │ -
171 OptionalJacobian<Rows, N> cols(int startCol) {
│ │ │ │ -
172 if (*this)
│ │ │ │ -
173 return OptionalJacobian<Rows, N>(&map_(0,startCol));
│ │ │ │ -
174 else
│ │ │ │ -
175 return OptionalJacobian<Rows, N>();
│ │ │ │ -
176 }
│ │ │ │ -
│ │ │ │ -
177
│ │ │ │ -
182};
│ │ │ │ -
│ │ │ │ -
183
│ │ │ │ -
184// The pure dynamic specialization of this is needed to support
│ │ │ │ -
185// variable-sized types. Note that this is designed to work like the
│ │ │ │ -
186// boost optional scheme from GTSAM 3.
│ │ │ │ -
187template<>
│ │ │ │ -
│ │ │ │ -
188class OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic> {
│ │ │ │ -
189
│ │ │ │ -
190public:
│ │ │ │ -
191
│ │ │ │ -
193 typedef Eigen::MatrixXd Jacobian;
│ │ │ │ -
194
│ │ │ │ -
195private:
│ │ │ │ -
196
│ │ │ │ -
197 Jacobian* pointer_;
│ │ │ │ -
198
│ │ │ │ -
199public:
│ │ │ │ -
200
│ │ │ │ -
│ │ │ │ -
202 OptionalJacobian() :
│ │ │ │ -
203 pointer_(nullptr) {
│ │ │ │ -
204 }
│ │ │ │ -
│ │ │ │ -
205
│ │ │ │ -
207 OptionalJacobian(Jacobian* pointer) : pointer_(pointer) {}
│ │ │ │ -
208
│ │ │ │ -
210 OptionalJacobian(Jacobian& dynamic) : pointer_(&dynamic) {}
│ │ │ │ -
211
│ │ │ │ -
212#ifndef OPTIONALJACOBIAN_NOBOOST
│ │ │ │ -
213
│ │ │ │ -
│ │ │ │ -
215 OptionalJacobian(boost::none_t /*none*/) :
│ │ │ │ -
216 pointer_(nullptr) {
│ │ │ │ -
217 }
│ │ │ │ -
│ │ │ │ -
218
│ │ │ │ -
│ │ │ │ -
220 OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
│ │ │ │ -
221 pointer_(nullptr) {
│ │ │ │ -
222 if (optional) pointer_ = &(*optional);
│ │ │ │ -
223 }
│ │ │ │ -
│ │ │ │ -
224
│ │ │ │ -
225#endif
│ │ │ │ -
226
│ │ │ │ -
│ │ │ │ -
228 operator bool() const {
│ │ │ │ -
229 return pointer_!=nullptr;
│ │ │ │ -
230 }
│ │ │ │ -
│ │ │ │ -
231
│ │ │ │ -
│ │ │ │ -
233 Jacobian& operator*() {
│ │ │ │ -
234 return *pointer_;
│ │ │ │ -
235 }
│ │ │ │ -
│ │ │ │ -
236
│ │ │ │ -
238 Jacobian* operator->(){ return pointer_; }
│ │ │ │ -
239};
│ │ │ │ -
│ │ │ │ -
240
│ │ │ │ -
241// forward declare
│ │ │ │ -
242template <typename T> struct traits;
│ │ │ │ -
243
│ │ │ │ -
249template <class T, class A>
│ │ │ │ -
│ │ │ │ -
250struct MakeJacobian {
│ │ │ │ -
251 typedef Eigen::Matrix<double, traits<T>::dimension, traits<A>::dimension> type;
│ │ │ │ -
252};
│ │ │ │ -
│ │ │ │ -
253
│ │ │ │ -
260template<class T, class A>
│ │ │ │ -
│ │ │ │ -
261struct MakeOptionalJacobian {
│ │ │ │ -
262 typedef OptionalJacobian<traits<T>::dimension,
│ │ │ │ -
263 traits<A>::dimension> type;
│ │ │ │ -
264};
│ │ │ │ -
│ │ │ │ -
265
│ │ │ │ -
266} // namespace gtsam
│ │ │ │ -
267
│ │ │ │ -
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian(const boost::optional< Eigen::MatrixXd & > optional)
Constructor compatible with old-style derivatives.
Definition OptionalJacobian.h:127
│ │ │ │ -
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian(boost::none_t)
Constructor with boost::none just makes empty.
Definition OptionalJacobian.h:122
│ │ │ │ -
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian(Eigen::MatrixXd &dynamic)
Constructor that will resize a dynamic matrix (unless already correct)
Definition OptionalJacobian.h:88
│ │ │ │ -
gtsam::OptionalJacobian::operator*
Eigen::Map< Jacobian > & operator*()
De-reference, like boost optional.
Definition OptionalJacobian.h:148
│ │ │ │ -
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian()
Default constructor acts like boost::none.
Definition OptionalJacobian.h:70
│ │ │ │ -
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian(Jacobian &fixed)
Constructor that will usurp data of a fixed-size matrix.
Definition OptionalJacobian.h:75
│ │ │ │ -
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian(Eigen::Ref< MATRIX > dynamic_ref)
Constructor from an Eigen::Ref value.
Definition OptionalJacobian.h:106
│ │ │ │ -
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian(Eigen::MatrixXd *dynamic)
Constructor that will resize a dynamic matrix (unless already correct)
Definition OptionalJacobian.h:95
│ │ │ │ -
gtsam::OptionalJacobian::operator->
Eigen::Map< Jacobian > * operator->()
operator->()
Definition OptionalJacobian.h:153
│ │ │ │ -
gtsam::OptionalJacobian::Jacobian
Eigen::Matrix< double, Rows, Cols > Jacobian
Jacobian size type TODO(frank): how to enforce RowMajor? Or better, make it work with any storage ord...
Definition OptionalJacobian.h:47
│ │ │ │ -
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian(Jacobian *fixedPtr)
Constructor that will usurp data of a fixed-size matrix, pointer version.
Definition OptionalJacobian.h:81
│ │ │ │ -
gtsam::OptionalJacobian::cols
OptionalJacobian< Rows, N > cols(int startCol)
Access M*N sub-block if we are allocated, otherwise none TODO(frank): this could work as is below if ...
Definition OptionalJacobian.h:171
│ │ │ │ -
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::OptionalJacobian
OptionalJacobian()
View on constructor argument, if given.
Definition OptionalJacobian.h:202
│ │ │ │ -
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::OptionalJacobian
OptionalJacobian(const boost::optional< Eigen::MatrixXd & > optional)
Constructor compatible with old-style derivatives.
Definition OptionalJacobian.h:220
│ │ │ │ -
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::Jacobian
Eigen::MatrixXd Jacobian
Jacobian size type.
Definition OptionalJacobian.h:193
│ │ │ │ -
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::operator*
Jacobian & operator*()
De-reference, like boost optional.
Definition OptionalJacobian.h:233
│ │ │ │ -
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::OptionalJacobian
OptionalJacobian(boost::none_t)
Constructor with boost::none just makes empty.
Definition OptionalJacobian.h:215
│ │ │ │ -
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::OptionalJacobian
OptionalJacobian(Jacobian *pointer)
Construct from pointer to dynamic matrix.
Definition OptionalJacobian.h:207
│ │ │ │ -
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::OptionalJacobian
OptionalJacobian(Jacobian &dynamic)
Construct from refrence to dynamic matrix.
Definition OptionalJacobian.h:210
│ │ │ │ -
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::operator->
Jacobian * operator->()
TODO: operator->()
Definition OptionalJacobian.h:238
│ │ │ │ -
gtsam::MakeJacobian
: meta-function to generate Jacobian
Definition OptionalJacobian.h:250
│ │ │ │ -
gtsam::MakeOptionalJacobian
: meta-function to generate JacobianTA optional reference Used mainly by Expressions
Definition OptionalJacobian.h:261
│ │ │ │ +
59} // namespace serialization
│ │ │ │ +
60} // namespace boost
│ │ │ │ +
61
│ │ │ │ +
62BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector)
│ │ │ │ +
63BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector2)
│ │ │ │ +
64BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector3)
│ │ │ │ +
65BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector6)
│ │ │ │ +
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ +
gtsam::save
void save(const Matrix &A, const string &s, const string &filename)
save a matrix to file, which can be loaded by matlab
Definition Matrix.cpp:167
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,321 +1,77 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -OptionalJacobian.h │ │ │ │ │ +VectorSerialization.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21#include // Configuration from CMake │ │ │ │ │ -22#include │ │ │ │ │ -23#include │ │ │ │ │ -24#include │ │ │ │ │ -25 │ │ │ │ │ -26#ifndef OPTIONALJACOBIAN_NOBOOST │ │ │ │ │ -27#include │ │ │ │ │ -28#endif │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_c_t_o_r_._h> │ │ │ │ │ +22 │ │ │ │ │ +23#include │ │ │ │ │ +24#include │ │ │ │ │ +25#include │ │ │ │ │ +26 │ │ │ │ │ +27namespace boost { │ │ │ │ │ +28namespace serialization { │ │ │ │ │ 29 │ │ │ │ │ -30namespace _g_t_s_a_m { │ │ │ │ │ -31 │ │ │ │ │ -40template │ │ │ │ │ -_4_1class _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n { │ │ │ │ │ -42 │ │ │ │ │ -43public: │ │ │ │ │ -44 │ │ │ │ │ -_4_7 typedef Eigen::Matrix _J_a_c_o_b_i_a_n; │ │ │ │ │ -48 │ │ │ │ │ -49private: │ │ │ │ │ -50 │ │ │ │ │ -51 Eigen::Map map_; │ │ │ │ │ +30// split version - copies into an STL vector for serialization │ │ │ │ │ +31template │ │ │ │ │ +32void _s_a_v_e(Archive& ar, const gtsam::Vector& v, unsigned int /*version*/) { │ │ │ │ │ +33 const size_t size = v.size(); │ │ │ │ │ +34 ar << BOOST_SERIALIZATION_NVP(size); │ │ │ │ │ +35 ar << make_nvp("data", make_array(v.data(), v.size())); │ │ │ │ │ +36} │ │ │ │ │ +37 │ │ │ │ │ +38template │ │ │ │ │ +39void load(Archive& ar, gtsam::Vector& v, unsigned int /*version*/) { │ │ │ │ │ +40 size_t size; │ │ │ │ │ +41 ar >> BOOST_SERIALIZATION_NVP(size); │ │ │ │ │ +42 v.resize(size); │ │ │ │ │ +43 ar >> make_nvp("data", make_array(v.data(), v.size())); │ │ │ │ │ +44} │ │ │ │ │ +45 │ │ │ │ │ +46// split version - copies into an STL vector for serialization │ │ │ │ │ +47template │ │ │ │ │ +48void _s_a_v_e(Archive& ar, const Eigen::Matrix& v, │ │ │ │ │ +49 unsigned int /*version*/) { │ │ │ │ │ +50 ar << make_nvp("data", make_array(v.data(), v.RowsAtCompileTime)); │ │ │ │ │ +51} │ │ │ │ │ 52 │ │ │ │ │ -53 // Trick from http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html │ │ │ │ │ -54 // uses "placement new" to make map_ usurp the memory of the fixed size │ │ │ │ │ -matrix │ │ │ │ │ -55 void usurp(double* data) { │ │ │ │ │ -56 new (&map_) Eigen::Map(data); │ │ │ │ │ -57 } │ │ │ │ │ +53template │ │ │ │ │ +54void load(Archive& ar, Eigen::Matrix& v, │ │ │ │ │ +55 unsigned int /*version*/) { │ │ │ │ │ +56 ar >> make_nvp("data", make_array(v.data(), v.RowsAtCompileTime)); │ │ │ │ │ +57} │ │ │ │ │ 58 │ │ │ │ │ -59 // Private and very dangerous constructor straight from memory │ │ │ │ │ -60 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n(double* data) : map_(nullptr) { │ │ │ │ │ -61 if (data) usurp(data); │ │ │ │ │ -62 } │ │ │ │ │ -63 │ │ │ │ │ -64 template │ │ │ │ │ -65 friend class OptionalJacobian; │ │ │ │ │ -66 │ │ │ │ │ -67public: │ │ │ │ │ -68 │ │ │ │ │ -_7_0 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n() : │ │ │ │ │ -71 map_(nullptr) { │ │ │ │ │ -72 } │ │ │ │ │ -73 │ │ │ │ │ -_7_5 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n(_J_a_c_o_b_i_a_n& fixed) : │ │ │ │ │ -76 map_(nullptr) { │ │ │ │ │ -77 usurp(fixed.data()); │ │ │ │ │ -78 } │ │ │ │ │ -79 │ │ │ │ │ -_8_1 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n(_J_a_c_o_b_i_a_n* fixedPtr) : │ │ │ │ │ -82 map_(nullptr) { │ │ │ │ │ -83 if (fixedPtr) │ │ │ │ │ -84 usurp(fixedPtr->data()); │ │ │ │ │ -85 } │ │ │ │ │ -86 │ │ │ │ │ -_8_8 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n(Eigen::MatrixXd& dynamic) : │ │ │ │ │ -89 map_(nullptr) { │ │ │ │ │ -90 dynamic.resize(Rows, Cols); // no malloc if correct size │ │ │ │ │ -91 usurp(dynamic.data()); │ │ │ │ │ -92 } │ │ │ │ │ -93 │ │ │ │ │ -_9_5 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n(Eigen::MatrixXd* dynamic) : │ │ │ │ │ -96 map_(nullptr) { │ │ │ │ │ -97 dynamic->resize(Rows, Cols); // no malloc if correct size │ │ │ │ │ -98 usurp(dynamic->data()); │ │ │ │ │ -99 } │ │ │ │ │ -100 │ │ │ │ │ -105 template │ │ │ │ │ -_1_0_6 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n(Eigen::Ref dynamic_ref) : │ │ │ │ │ -107 map_(nullptr) { │ │ │ │ │ -108 if (dynamic_ref.rows() == Rows && dynamic_ref.cols() == Cols && │ │ │ │ │ -!dynamic_ref.IsRowMajor) { │ │ │ │ │ -109 usurp(dynamic_ref.data()); │ │ │ │ │ -110 } else { │ │ │ │ │ -111 throw std::invalid_argument( │ │ │ │ │ -112 std::string("OptionalJacobian called with wrong dimensions or " │ │ │ │ │ -113 "storage order.\n" │ │ │ │ │ -114 "Expected: ") + │ │ │ │ │ -115 "(" + std::to_string(Rows) + ", " + std::to_string(Cols) + ")"); │ │ │ │ │ -116 } │ │ │ │ │ -117 } │ │ │ │ │ -118 │ │ │ │ │ -119#ifndef OPTIONALJACOBIAN_NOBOOST │ │ │ │ │ -120 │ │ │ │ │ -_1_2_2 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n(boost::none_t /*none*/) : │ │ │ │ │ -123 map_(nullptr) { │ │ │ │ │ -124 } │ │ │ │ │ -125 │ │ │ │ │ -_1_2_7 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n(const boost::optional optional) : │ │ │ │ │ -128 map_(nullptr) { │ │ │ │ │ -129 if (optional) { │ │ │ │ │ -130 optional->resize(Rows, Cols); │ │ │ │ │ -131 usurp(optional->data()); │ │ │ │ │ -132 } │ │ │ │ │ -133 } │ │ │ │ │ -134 │ │ │ │ │ -135#endif │ │ │ │ │ -136 │ │ │ │ │ -139 // template │ │ │ │ │ -140 // OptionalJacobian(Eigen::Block block) : │ │ │ │ │ -map_(nullptr) { ?? } │ │ │ │ │ -141 │ │ │ │ │ -_1_4_3 operator bool() const { │ │ │ │ │ -144 return map_.data() != nullptr; │ │ │ │ │ -145 } │ │ │ │ │ -146 │ │ │ │ │ -_1_4_8 Eigen::Map& _o_p_e_r_a_t_o_r_*() { │ │ │ │ │ -149 return map_; │ │ │ │ │ -150 } │ │ │ │ │ -151 │ │ │ │ │ -_1_5_3 Eigen::Map* _o_p_e_r_a_t_o_r_-_>() { │ │ │ │ │ -154 return &map_; │ │ │ │ │ -155 } │ │ │ │ │ -156 │ │ │ │ │ -159 // template │ │ │ │ │ -160 // OptionalJacobian block(int startRow, int startCol) { │ │ │ │ │ -161 // if (*this) │ │ │ │ │ -162 // OptionalJacobian(map_.block(startRow, startCol)); │ │ │ │ │ -163 // else │ │ │ │ │ -164 // return OptionalJacobian(); │ │ │ │ │ -165 // } │ │ │ │ │ -166 │ │ │ │ │ -170 template │ │ │ │ │ -_1_7_1 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_R_o_w_s_,_ _N_> _c_o_l_s(int startCol) { │ │ │ │ │ -172 if (*this) │ │ │ │ │ -173 return _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_R_o_w_s_,_ _N_>(&map_(0,startCol)); │ │ │ │ │ -174 else │ │ │ │ │ -175 return _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_R_o_w_s_,_ _N_>(); │ │ │ │ │ -176 } │ │ │ │ │ -177 │ │ │ │ │ -182}; │ │ │ │ │ -183 │ │ │ │ │ -184// The pure dynamic specialization of this is needed to support │ │ │ │ │ -185// variable-sized types. Note that this is designed to work like the │ │ │ │ │ -186// boost optional scheme from GTSAM 3. │ │ │ │ │ -187template<> │ │ │ │ │ -_1_8_8class _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n { │ │ │ │ │ -189 │ │ │ │ │ -190public: │ │ │ │ │ -191 │ │ │ │ │ -_1_9_3 typedef Eigen::MatrixXd _J_a_c_o_b_i_a_n; │ │ │ │ │ -194 │ │ │ │ │ -195private: │ │ │ │ │ -196 │ │ │ │ │ -197 _J_a_c_o_b_i_a_n* pointer_; │ │ │ │ │ -198 │ │ │ │ │ -199public: │ │ │ │ │ -200 │ │ │ │ │ -_2_0_2 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n() : │ │ │ │ │ -203 pointer_(nullptr) { │ │ │ │ │ -204 } │ │ │ │ │ -205 │ │ │ │ │ -_2_0_7 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n(_J_a_c_o_b_i_a_n* pointer) : pointer_(pointer) {} │ │ │ │ │ -208 │ │ │ │ │ -_2_1_0 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n(_J_a_c_o_b_i_a_n& dynamic) : pointer_(&dynamic) {} │ │ │ │ │ -211 │ │ │ │ │ -212#ifndef OPTIONALJACOBIAN_NOBOOST │ │ │ │ │ -213 │ │ │ │ │ -_2_1_5 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n(boost::none_t /*none*/) : │ │ │ │ │ -216 pointer_(nullptr) { │ │ │ │ │ -217 } │ │ │ │ │ -218 │ │ │ │ │ -_2_2_0 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n(const boost::optional optional) : │ │ │ │ │ -221 pointer_(nullptr) { │ │ │ │ │ -222 if (optional) pointer_ = &(*optional); │ │ │ │ │ -223 } │ │ │ │ │ -224 │ │ │ │ │ -225#endif │ │ │ │ │ -226 │ │ │ │ │ -_2_2_8 operator bool() const { │ │ │ │ │ -229 return pointer_!=nullptr; │ │ │ │ │ -230 } │ │ │ │ │ -231 │ │ │ │ │ -_2_3_3 _J_a_c_o_b_i_a_n& _o_p_e_r_a_t_o_r_*() { │ │ │ │ │ -234 return *pointer_; │ │ │ │ │ -235 } │ │ │ │ │ -236 │ │ │ │ │ -_2_3_8 _J_a_c_o_b_i_a_n* _o_p_e_r_a_t_o_r_-_>(){ return pointer_; } │ │ │ │ │ -239}; │ │ │ │ │ -240 │ │ │ │ │ -241// forward declare │ │ │ │ │ -242template struct traits; │ │ │ │ │ -243 │ │ │ │ │ -249template │ │ │ │ │ -_2_5_0struct _M_a_k_e_J_a_c_o_b_i_a_n { │ │ │ │ │ -251 typedef Eigen::Matrix::dimension, _t_r_a_i_t_s_<_A_>_:_:_d_i_m_e_n_s_i_o_n> │ │ │ │ │ -type; │ │ │ │ │ -252}; │ │ │ │ │ -253 │ │ │ │ │ -260template │ │ │ │ │ -_2_6_1struct _M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n { │ │ │ │ │ -262 typedef _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_t_r_a_i_t_s_<_T_>_:_:_d_i_m_e_n_s_i_o_n, │ │ │ │ │ -263 _t_r_a_i_t_s_<_A_>_:_:_d_i_m_e_n_s_i_o_n> type; │ │ │ │ │ -264}; │ │ │ │ │ -265 │ │ │ │ │ -266} // namespace gtsam │ │ │ │ │ -267 │ │ │ │ │ -_g_t_s_a_m │ │ │ │ │ -Global functions in a separate testing namespace. │ │ │ │ │ -DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ -either a fixed size o... │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian(const boost::optional< Eigen::MatrixXd & > optional) │ │ │ │ │ -Constructor compatible with old-style derivatives. │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:127 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian(boost::none_t) │ │ │ │ │ -Constructor with boost::none just makes empty. │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:122 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian(Eigen::MatrixXd &dynamic) │ │ │ │ │ -Constructor that will resize a dynamic matrix (unless already correct) │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:88 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ -Eigen::Map< Jacobian > & operator*() │ │ │ │ │ -De-reference, like boost optional. │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:148 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian() │ │ │ │ │ -Default constructor acts like boost::none. │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:70 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian(Jacobian &fixed) │ │ │ │ │ -Constructor that will usurp data of a fixed-size matrix. │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian(Eigen::Ref< MATRIX > dynamic_ref) │ │ │ │ │ -Constructor from an Eigen::Ref value. │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:106 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian(Eigen::MatrixXd *dynamic) │ │ │ │ │ -Constructor that will resize a dynamic matrix (unless already correct) │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:95 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_:_:_o_p_e_r_a_t_o_r_-_> │ │ │ │ │ -Eigen::Map< Jacobian > * operator->() │ │ │ │ │ -operator->() │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:153 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_:_:_J_a_c_o_b_i_a_n │ │ │ │ │ -Eigen::Matrix< double, Rows, Cols > Jacobian │ │ │ │ │ -Jacobian size type TODO(frank): how to enforce RowMajor? Or better, make it │ │ │ │ │ -work with any storage ord... │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian(Jacobian *fixedPtr) │ │ │ │ │ -Constructor that will usurp data of a fixed-size matrix, pointer version. │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:81 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_:_:_c_o_l_s │ │ │ │ │ -OptionalJacobian< Rows, N > cols(int startCol) │ │ │ │ │ -Access M*N sub-block if we are allocated, otherwise none TODO(frank): this │ │ │ │ │ -could work as is below if ... │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:171 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_,_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_ _>_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian() │ │ │ │ │ -View on constructor argument, if given. │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:202 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_,_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_ _>_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian(const boost::optional< Eigen::MatrixXd & > optional) │ │ │ │ │ -Constructor compatible with old-style derivatives. │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:220 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_,_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_ _>_:_:_J_a_c_o_b_i_a_n │ │ │ │ │ -Eigen::MatrixXd Jacobian │ │ │ │ │ -Jacobian size type. │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:193 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_,_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_ _>_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ -Jacobian & operator*() │ │ │ │ │ -De-reference, like boost optional. │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:233 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_,_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_ _>_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian(boost::none_t) │ │ │ │ │ -Constructor with boost::none just makes empty. │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:215 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_,_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_ _>_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian(Jacobian *pointer) │ │ │ │ │ -Construct from pointer to dynamic matrix. │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:207 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_,_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_ _>_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian(Jacobian &dynamic) │ │ │ │ │ -Construct from refrence to dynamic matrix. │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:210 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_,_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_ _>_:_:_o_p_e_r_a_t_o_r_-_> │ │ │ │ │ -Jacobian * operator->() │ │ │ │ │ -TODO: operator->() │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:238 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_k_e_J_a_c_o_b_i_a_n │ │ │ │ │ -: meta-function to generate Jacobian │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:250 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -: meta-function to generate JacobianTA optional reference Used mainly by │ │ │ │ │ -Expressions │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:261 │ │ │ │ │ +59} // namespace serialization │ │ │ │ │ +60} // namespace boost │ │ │ │ │ +61 │ │ │ │ │ +62BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector) │ │ │ │ │ +63BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector2) │ │ │ │ │ +64BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector3) │ │ │ │ │ +65BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector6) │ │ │ │ │ +_V_e_c_t_o_r_._h │ │ │ │ │ +typedef and functions to augment Eigen's VectorXd │ │ │ │ │ +_g_t_s_a_m_:_:_s_a_v_e │ │ │ │ │ +void save(const Matrix &A, const string &s, const string &filename) │ │ │ │ │ +save a matrix to file, which can be loaded by matlab │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:167 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h │ │ │ │ │ + * _V_e_c_t_o_r_S_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00023.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/cholesky.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/serialization.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,48 +94,166 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
cholesky.cpp File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
serialization.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained Cholesky. │ │ │ │ +

Convenience functions for serializing data structures via boost.serialization. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

pair< size_t, bool > gtsam::choleskyCareful (Matrix &ATA, int order=-1)
 "Careful" Cholesky computes the positive square-root of a positive symmetric semi-definite matrix (i.e.
 
bool gtsam::choleskyPartial (Matrix &ABC, size_t nFrontal, size_t topleft=0)
 Partial Cholesky computes a factor [R S such that [R' 0 [R S = [A B 0 L] S' I] 0 L] B' C].
 
Standard serialization

Serialization in default compressed format

│ │ │ │ +
template<class T >
void gtsam::serializeToStream (const T &input, std::ostream &out_archive_stream)
 
│ │ │ │ +template<class T >
void gtsam::deserializeFromStream (std::istream &in_archive_stream, T &output)
 deserializes from a stream
 
│ │ │ │ +template<class T >
std::string gtsam::serializeToString (const T &input)
 serializes to a string
 
│ │ │ │ +template<class T >
void gtsam::deserializeFromString (const std::string &serialized, T &output)
 deserializes from a string
 
│ │ │ │ +template<class T >
bool gtsam::serializeToFile (const T &input, const std::string &filename)
 serializes to a file
 
│ │ │ │ +template<class T >
bool gtsam::deserializeFromFile (const std::string &filename, T &output)
 deserializes from a file
 
│ │ │ │ +template<class T >
std::string gtsam::serialize (const T &input)
 serializes to a string
 
│ │ │ │ +template<class T >
void gtsam::deserialize (const std::string &serialized, T &output)
 deserializes from a string
 
XML Serialization

Serialization to XML format with named structures

│ │ │ │ +
template<class T >
void gtsam::serializeToXMLStream (const T &input, std::ostream &out_archive_stream, const std::string &name="data")
 
│ │ │ │ +template<class T >
void gtsam::deserializeFromXMLStream (std::istream &in_archive_stream, T &output, const std::string &name="data")
 deserializes from a stream in XML
 
│ │ │ │ +template<class T >
std::string gtsam::serializeToXMLString (const T &input, const std::string &name="data")
 serializes to a string in XML
 
│ │ │ │ +template<class T >
void gtsam::deserializeFromXMLString (const std::string &serialized, T &output, const std::string &name="data")
 deserializes from a string in XML
 
│ │ │ │ +template<class T >
bool gtsam::serializeToXMLFile (const T &input, const std::string &filename, const std::string &name="data")
 serializes to an XML file
 
│ │ │ │ +template<class T >
bool gtsam::deserializeFromXMLFile (const std::string &filename, T &output, const std::string &name="data")
 deserializes from an XML file
 
│ │ │ │ +template<class T >
std::string gtsam::serializeXML (const T &input, const std::string &name="data")
 serializes to a string in XML
 
│ │ │ │ +template<class T >
void gtsam::deserializeXML (const std::string &serialized, T &output, const std::string &name="data")
 deserializes from a string in XML
 
Binary Serialization

Serialization to binary format with named structures

│ │ │ │ +
template<class T >
void gtsam::serializeToBinaryStream (const T &input, std::ostream &out_archive_stream, const std::string &name="data")
 
│ │ │ │ +template<class T >
void gtsam::deserializeFromBinaryStream (std::istream &in_archive_stream, T &output, const std::string &name="data")
 deserializes from a stream in binary
 
│ │ │ │ +template<class T >
std::string gtsam::serializeToBinaryString (const T &input, const std::string &name="data")
 serializes to a string in binary
 
│ │ │ │ +template<class T >
void gtsam::deserializeFromBinaryString (const std::string &serialized, T &output, const std::string &name="data")
 deserializes from a string in binary
 
│ │ │ │ +template<class T >
bool gtsam::serializeToBinaryFile (const T &input, const std::string &filename, const std::string &name="data")
 serializes to a binary file
 
│ │ │ │ +template<class T >
bool gtsam::deserializeFromBinaryFile (const std::string &filename, T &output, const std::string &name="data")
 deserializes from a binary file
 
│ │ │ │ +template<class T >
std::string gtsam::serializeBinary (const T &input, const std::string &name="data")
 serializes to a string in binary
 
│ │ │ │ +template<class T >
void gtsam::deserializeBinary (const std::string &serialized, T &output, const std::string &name="data")
 deserializes from a string in binary
 
│ │ │ │

Detailed Description

│ │ │ │ -

Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained Cholesky.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ +

Convenience functions for serializing data structures via boost.serialization.

│ │ │ │ +
Author
Alex Cunningham
│ │ │ │
│ │ │ │ -Frank Dellaert
│ │ │ │ -
Date
Nov 5, 2010
│ │ │ │ +Richard Roberts
│ │ │ │ +
Date
Feb 7, 2012
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,35 +1,146 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -cholesky.cpp File Reference │ │ │ │ │ -Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained │ │ │ │ │ -Cholesky. _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +serialization.h File Reference │ │ │ │ │ +Convenience functions for serializing data structures via boost.serialization. │ │ │ │ │ +_M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -pair< size_t, bool >  _g_t_s_a_m_:_:_c_h_o_l_e_s_k_y_C_a_r_e_f_u_l (Matrix &ATA, int order=-1) │ │ │ │ │ -  "Careful" Cholesky computes the positive square-root of a │ │ │ │ │ - positive symmetric semi-definite matrix (i.e. │ │ │ │ │ -  │ │ │ │ │ - bool  _g_t_s_a_m_:_:_c_h_o_l_e_s_k_y_P_a_r_t_i_a_l (Matrix &ABC, size_t nFrontal, │ │ │ │ │ - size_t topleft=0) │ │ │ │ │ -  Partial Cholesky computes a factor [R S such that [R' 0 │ │ │ │ │ - [R S = [A B 0 L] S' I] 0 L] B' C]. │ │ │ │ │ +Standard serialization │ │ │ │ │ +Serialization in default compressed format │ │ │ │ │ +template │ │ │ │ │ + void  _g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_S_t_r_e_a_m (const T &input, std::ostream │ │ │ │ │ + &out_archive_stream) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + void  ggttssaamm::::ddeesseerriiaalliizzeeFFrroommSSttrreeaamm (std::istream &in_archive_stream, T │ │ │ │ │ + &output) │ │ │ │ │ +  deserializes from a stream │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +std::string  ggttssaamm::::sseerriiaalliizzeeTTooSSttrriinngg (const T &input) │ │ │ │ │ +  serializes to a string │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + void  ggttssaamm::::ddeesseerriiaalliizzeeFFrroommSSttrriinngg (const std::string &serialized, T │ │ │ │ │ + &output) │ │ │ │ │ +  deserializes from a string │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + bool  ggttssaamm::::sseerriiaalliizzeeTTooFFiillee (const T &input, const std::string │ │ │ │ │ + &filename) │ │ │ │ │ +  serializes to a file │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + bool  ggttssaamm::::ddeesseerriiaalliizzeeFFrroommFFiillee (const std::string &filename, T │ │ │ │ │ + &output) │ │ │ │ │ +  deserializes from a file │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +std::string  ggttssaamm::::sseerriiaalliizzee (const T &input) │ │ │ │ │ +  serializes to a string │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + void  ggttssaamm::::ddeesseerriiaalliizzee (const std::string &serialized, T &output) │ │ │ │ │ +  deserializes from a string │ │ │ │ │ +  │ │ │ │ │ +XML Serialization │ │ │ │ │ +Serialization to XML format with named structures │ │ │ │ │ +template │ │ │ │ │ + void  _g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_X_M_L_S_t_r_e_a_m (const T &input, std::ostream │ │ │ │ │ + &out_archive_stream, const std::string &name="data") │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + void  ggttssaamm::::ddeesseerriiaalliizzeeFFrroommXXMMLLSSttrreeaamm (std::istream &in_archive_stream, │ │ │ │ │ + T &output, const std::string &name="data") │ │ │ │ │ +  deserializes from a stream in XML │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +std::string  ggttssaamm::::sseerriiaalliizzeeTTooXXMMLLSSttrriinngg (const T &input, const std::string │ │ │ │ │ + &name="data") │ │ │ │ │ +  serializes to a string in XML │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + void  ggttssaamm::::ddeesseerriiaalliizzeeFFrroommXXMMLLSSttrriinngg (const std::string &serialized, T │ │ │ │ │ + &output, const std::string &name="data") │ │ │ │ │ +  deserializes from a string in XML │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + bool  ggttssaamm::::sseerriiaalliizzeeTTooXXMMLLFFiillee (const T &input, const std::string │ │ │ │ │ + &filename, const std::string &name="data") │ │ │ │ │ +  serializes to an XML file │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + bool  ggttssaamm::::ddeesseerriiaalliizzeeFFrroommXXMMLLFFiillee (const std::string &filename, T │ │ │ │ │ + &output, const std::string &name="data") │ │ │ │ │ +  deserializes from an XML file │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +std::string  ggttssaamm::::sseerriiaalliizzeeXXMMLL (const T &input, const std::string │ │ │ │ │ + &name="data") │ │ │ │ │ +  serializes to a string in XML │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + void  ggttssaamm::::ddeesseerriiaalliizzeeXXMMLL (const std::string &serialized, T &output, │ │ │ │ │ + const std::string &name="data") │ │ │ │ │ +  deserializes from a string in XML │ │ │ │ │ +  │ │ │ │ │ +Binary Serialization │ │ │ │ │ +Serialization to binary format with named structures │ │ │ │ │ +template │ │ │ │ │ + void  _g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_B_i_n_a_r_y_S_t_r_e_a_m (const T &input, std::ostream │ │ │ │ │ + &out_archive_stream, const std::string &name="data") │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + void  ggttssaamm::::ddeesseerriiaalliizzeeFFrroommBBiinnaarryySSttrreeaamm (std::istream │ │ │ │ │ + &in_archive_stream, T &output, const std::string &name="data") │ │ │ │ │ +  deserializes from a stream in binary │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +std::string  ggttssaamm::::sseerriiaalliizzeeTTooBBiinnaarryySSttrriinngg (const T &input, const std::string │ │ │ │ │ + &name="data") │ │ │ │ │ +  serializes to a string in binary │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + void  ggttssaamm::::ddeesseerriiaalliizzeeFFrroommBBiinnaarryySSttrriinngg (const std::string &serialized, │ │ │ │ │ + T &output, const std::string &name="data") │ │ │ │ │ +  deserializes from a string in binary │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + bool  ggttssaamm::::sseerriiaalliizzeeTTooBBiinnaarryyFFiillee (const T &input, const std::string │ │ │ │ │ + &filename, const std::string &name="data") │ │ │ │ │ +  serializes to a binary file │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + bool  ggttssaamm::::ddeesseerriiaalliizzeeFFrroommBBiinnaarryyFFiillee (const std::string &filename, T │ │ │ │ │ + &output, const std::string &name="data") │ │ │ │ │ +  deserializes from a binary file │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +std::string  ggttssaamm::::sseerriiaalliizzeeBBiinnaarryy (const T &input, const std::string │ │ │ │ │ + &name="data") │ │ │ │ │ +  serializes to a string in binary │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + void  ggttssaamm::::ddeesseerriiaalliizzeeBBiinnaarryy (const std::string &serialized, T │ │ │ │ │ + &output, const std::string &name="data") │ │ │ │ │ +  deserializes from a string in binary │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained │ │ │ │ │ -Cholesky. │ │ │ │ │ +Convenience functions for serializing data structures via boost.serialization. │ │ │ │ │ Author │ │ │ │ │ + Alex Cunningham │ │ │ │ │ Richard Roberts │ │ │ │ │ - Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - Nov 5, 2010 │ │ │ │ │ + Feb 7, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _c_h_o_l_e_s_k_y_._c_p_p │ │ │ │ │ + * _s_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00023.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,26 @@ │ │ │ │ │ var a00023 = [ │ │ │ │ │ - ["choleskyCareful", "a00023.html#a747683f736c50bca16b3aab0e95b1b76", null], │ │ │ │ │ - ["choleskyPartial", "a00023.html#abe82fa6aceccfa2360cf314ab44f39ad", null] │ │ │ │ │ + ["deserialize", "a00023.html#aa4d516d0d98946341ad2766521011d1b", null], │ │ │ │ │ + ["deserializeBinary", "a00023.html#a56c41958e57a4bc7361ab593011462fd", null], │ │ │ │ │ + ["deserializeFromBinaryFile", "a00023.html#aefeb77f2324e4bbfca2c8152c49a4565", null], │ │ │ │ │ + ["deserializeFromBinaryStream", "a00023.html#a8eb16192d8ebe2532d8a69c9f2bf15b2", null], │ │ │ │ │ + ["deserializeFromBinaryString", "a00023.html#a349b7ed4a67a8c3d80737e5383507bc4", null], │ │ │ │ │ + ["deserializeFromFile", "a00023.html#adbb4a71187706918faca3a11519b62f6", null], │ │ │ │ │ + ["deserializeFromStream", "a00023.html#ab181d2a3de466fc0c87bd29501bbc7ab", null], │ │ │ │ │ + ["deserializeFromString", "a00023.html#a262487af458d1f48dd7d3bc549caae00", null], │ │ │ │ │ + ["deserializeFromXMLFile", "a00023.html#a0d8faf2bb0a908ff484677bb2fcf707c", null], │ │ │ │ │ + ["deserializeFromXMLStream", "a00023.html#a8d7ea177ef3e93af15a4a77c2bd7fa8b", null], │ │ │ │ │ + ["deserializeFromXMLString", "a00023.html#a5f2fe2bfc17737ff9ae1fb8b210604f0", null], │ │ │ │ │ + ["deserializeXML", "a00023.html#a85ab51b2e8f4cea69f72e06a5c136e85", null], │ │ │ │ │ + ["serialize", "a00023.html#a332bad7d1e70d3c04ceca35adce7a134", null], │ │ │ │ │ + ["serializeBinary", "a00023.html#abb476c3e77878f7e0f70dd51871f48e7", null], │ │ │ │ │ + ["serializeToBinaryFile", "a00023.html#abdff0a4b4fdf0e609aa62fb8d8b1dbba", null], │ │ │ │ │ + ["serializeToBinaryStream", "a00023.html#aa5d177a3bf16f9f5acf45ee62c816479", null], │ │ │ │ │ + ["serializeToBinaryString", "a00023.html#a7fad959f63f2eac1478c5ede7b9ae4db", null], │ │ │ │ │ + ["serializeToFile", "a00023.html#aa3261c2d455f892dae1921f8c3a39267", null], │ │ │ │ │ + ["serializeToStream", "a00023.html#a007080ca4741cc3d3d6a209b0ae3fe61", null], │ │ │ │ │ + ["serializeToString", "a00023.html#aa12c78301a9d2968afe5a1a14baa7e90", null], │ │ │ │ │ + ["serializeToXMLFile", "a00023.html#a89449123a746480a1d668cecb5576708", null], │ │ │ │ │ + ["serializeToXMLStream", "a00023.html#ae2e939177cf58b42d7ed4c9856d536c5", null], │ │ │ │ │ + ["serializeToXMLString", "a00023.html#a16c154fe5e35242e26af8286e77eea11", null], │ │ │ │ │ + ["serializeXML", "a00023.html#a976ff7999fcc13ef602b6ece1b078bca", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00026.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/MatrixSerialization.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/debug.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,94 +94,61 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Namespaces | │ │ │ │ +Macros | │ │ │ │ Functions
│ │ │ │ -
MatrixSerialization.h File Reference
│ │ │ │ +
debug.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Serialization for matrices. │ │ │ │ +

Global debugging flags. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Macros

│ │ │ │ +#define ISDEBUG(S)   (gtsam::guardedIsDebug(S))
 
│ │ │ │ +#define SETDEBUG(S, V)   ((void)(gtsam::guardedSetDebug(S,V)))
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

template<class Archive , typename Scalar_ , int Rows_, int Cols_, int Ops_, int MaxRows_, int MaxCols_>
void boost::serialization::save (Archive &ar, const Eigen::Matrix< Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_ > &m, const unsigned int)
 Ref.
 
│ │ │ │ -template<class Archive , typename Scalar_ , int Rows_, int Cols_, int Ops_, int MaxRows_, int MaxCols_>
void boost::serialization::load (Archive &ar, Eigen::Matrix< Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_ > &m, const unsigned int)
 
│ │ │ │ -template<class Archive , typename Scalar_ , int Rows_, int Cols_, int Ops_, int MaxRows_, int MaxCols_>
void boost::serialization::serialize (Archive &ar, Eigen::Matrix< Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_ > &m, const unsigned int version)
 
│ │ │ │ -template<class Archive >
void boost::serialization::serialize (Archive &ar, gtsam::Matrix &m, const unsigned int version)
 
│ │ │ │ +bool gtsam::guardedIsDebug (const std::string &s)
 
│ │ │ │ +void gtsam::guardedSetDebug (const std::string &s, const bool v)
 
│ │ │ │ +bool gtsam::isDebugVersion ()
 
│ │ │ │

Detailed Description

│ │ │ │ -

Serialization for matrices.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
Date
February 2022
│ │ │ │ -

Function Documentation

│ │ │ │ - │ │ │ │ -

◆ save()

│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -template<class Archive , typename Scalar_ , int Rows_, int Cols_, int Ops_, int MaxRows_, int MaxCols_>
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
void boost::serialization::save (Archive & ar,
const Eigen::Matrix< Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_ > & m,
const unsigned int  
)
│ │ │ │ -
│ │ │ │ - │ │ │ │ -

Ref.

│ │ │ │ -

https://stackoverflow.com/questions/18382457/eigen-and-boostserialize/22903063#22903063

│ │ │ │ -

Eigen supports calling resize() on both static and dynamic matrices. This allows for a uniform API, with resize having no effect if the static matrix is already the correct size. https://eigen.tuxfamily.org/dox/group__TutorialMatrixClass.html#TutorialMatrixSizesResizing

│ │ │ │ -

We use all the Matrix template parameters to ensure wide compatibility.

│ │ │ │ -

eigen_typekit in ROS uses the same code http://docs.ros.org/lunar/api/eigen_typekit/html/eigen__mqueue_8cpp_source.html

│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ +

Global debugging flags.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Feb 1, 2011
│ │ │ │ +
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,59 +1,35 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_F_u_n_c_t_i_o_n_s │ │ │ │ │ -MatrixSerialization.h File Reference │ │ │ │ │ -Serialization for matrices. _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +debug.h File Reference │ │ │ │ │ +Global debugging flags. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +NNaammeessppaacceess │ │ │ │ │ +namespace   _g_t_s_a_m │ │ │ │ │ +  Global functions in a separate testing namespace. │ │ │ │ │ +  │ │ │ │ │ +MMaaccrrooss │ │ │ │ │ +#define  IISSDDEEBBUUGG(S)   (gtsam::guardedIsDebug(S)) │ │ │ │ │ +  │ │ │ │ │ +#define  SSEETTDDEEBBUUGG(S, V)   ((void)(gtsam::guardedSetDebug(S,V))) │ │ │ │ │ +  │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ -void  _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_s_a_v_e (Archive &ar, const Eigen::Matrix< Scalar_, │ │ │ │ │ - Rows_, Cols_, Ops_, MaxRows_, MaxCols_ > &m, const unsigned int) │ │ │ │ │ -  Ref. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -void  bboooosstt::::sseerriiaalliizzaattiioonn::::llooaadd (Archive &ar, Eigen::Matrix< Scalar_, Rows_, │ │ │ │ │ - Cols_, Ops_, MaxRows_, MaxCols_ > &m, const unsigned int) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -void  bboooosstt::::sseerriiaalliizzaattiioonn::::sseerriiaalliizzee (Archive &ar, Eigen::Matrix< Scalar_, │ │ │ │ │ - Rows_, Cols_, Ops_, MaxRows_, MaxCols_ > &m, const unsigned int version) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -void  bboooosstt::::sseerriiaalliizzaattiioonn::::sseerriiaalliizzee (Archive &ar, gtsam::Matrix &m, const │ │ │ │ │ - unsigned int version) │ │ │ │ │ +bool  ggttssaamm::::gguuaarrddeeddIIssDDeebbuugg (const std::string &s) │ │ │ │ │ +  │ │ │ │ │ +void  ggttssaamm::::gguuaarrddeeddSSeettDDeebbuugg (const std::string &s, const bool v) │ │ │ │ │ +  │ │ │ │ │ +bool  ggttssaamm::::iissDDeebbuuggVVeerrssiioonn () │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Serialization for matrices. │ │ │ │ │ +Global debugging flags. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ Date │ │ │ │ │ - February 2022 │ │ │ │ │ -********** FFuunnccttiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ -********** _?◆_? ssaavvee(()) ********** │ │ │ │ │ -template │ │ │ │ │ -void boost::serialization::save ( Archive &  aarr, │ │ │ │ │ - const Eigen::Matrix< Scalar_, Rows_, mm, │ │ │ │ │ - Cols_, Ops_, MaxRows_, MaxCols_ > &  │ │ │ │ │ - const unsigned int    │ │ │ │ │ - ) │ │ │ │ │ -Ref. │ │ │ │ │ -_h_t_t_p_s_:_/_/_s_t_a_c_k_o_v_e_r_f_l_o_w_._c_o_m_/_q_u_e_s_t_i_o_n_s_/_1_8_3_8_2_4_5_7_/_e_i_g_e_n_-_a_n_d_-_b_o_o_s_t_s_e_r_i_a_l_i_z_e_/ │ │ │ │ │ -_2_2_9_0_3_0_6_3_#_2_2_9_0_3_0_6_3 │ │ │ │ │ -Eigen supports calling resize() on both static and dynamic matrices. This │ │ │ │ │ -allows for a uniform API, with resize having no effect if the static matrix is │ │ │ │ │ -already the correct size. _h_t_t_p_s_:_/_/_e_i_g_e_n_._t_u_x_f_a_m_i_l_y_._o_r_g_/_d_o_x_/ │ │ │ │ │ -_g_r_o_u_p_____T_u_t_o_r_i_a_l_M_a_t_r_i_x_C_l_a_s_s_._h_t_m_l_#_T_u_t_o_r_i_a_l_M_a_t_r_i_x_S_i_z_e_s_R_e_s_i_z_i_n_g │ │ │ │ │ -We use all the Matrix template parameters to ensure wide compatibility. │ │ │ │ │ -eigen_typekit in ROS uses the same code _h_t_t_p_:_/_/_d_o_c_s_._r_o_s_._o_r_g_/_l_u_n_a_r_/_a_p_i_/ │ │ │ │ │ -_e_i_g_e_n___t_y_p_e_k_i_t_/_h_t_m_l_/_e_i_g_e_n_____m_q_u_e_u_e___8_c_p_p___s_o_u_r_c_e_._h_t_m_l │ │ │ │ │ + Feb 1, 2011 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _M_a_t_r_i_x_S_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ + * _d_e_b_u_g_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00026_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/MatrixSerialization.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/debug.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,91 +98,86 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
MatrixSerialization.h
│ │ │ │ +
debug.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19// \callgraph
│ │ │ │ -
20
│ │ │ │ -
21#pragma once
│ │ │ │ +
19#include <gtsam/base/FastMap.h>
│ │ │ │ +
20#include <gtsam/global_includes.h>
│ │ │ │ +
21#include <string>
│ │ │ │
22
│ │ │ │ -
23#include <gtsam/base/Matrix.h>
│ │ │ │ -
24
│ │ │ │ -
25#include <boost/serialization/array.hpp>
│ │ │ │ -
26#include <boost/serialization/nvp.hpp>
│ │ │ │ -
27#include <boost/serialization/split_free.hpp>
│ │ │ │ -
28
│ │ │ │ -
29namespace boost {
│ │ │ │ -
30namespace serialization {
│ │ │ │ -
31
│ │ │ │ -
47// split version - sends sizes ahead
│ │ │ │ -
48template <class Archive, typename Scalar_, int Rows_, int Cols_, int Ops_,
│ │ │ │ -
49 int MaxRows_, int MaxCols_>
│ │ │ │ -
│ │ │ │ -
50void save(
│ │ │ │ -
51 Archive& ar,
│ │ │ │ -
52 const Eigen::Matrix<Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_>& m,
│ │ │ │ -
53 const unsigned int /*version*/) {
│ │ │ │ -
54 const size_t rows = m.rows(), cols = m.cols();
│ │ │ │ -
55 ar << BOOST_SERIALIZATION_NVP(rows);
│ │ │ │ -
56 ar << BOOST_SERIALIZATION_NVP(cols);
│ │ │ │ -
57 ar << make_nvp("data", make_array(m.data(), m.size()));
│ │ │ │ -
58}
│ │ │ │ -
│ │ │ │ +
23
│ │ │ │ +
24// This file defines granular debugging flags that may be switched on and off
│ │ │ │ +
25// at run time. Typical usage is 'if(ISDEBUG("myFunction"))' to check if the
│ │ │ │ +
26// 'myFunction' flag is enabled, and SETDEBUG("myFunction", true) to enable
│ │ │ │ +
27// this flag, or SETDEBUG("myFunction", false) to disable it.
│ │ │ │ +
28//
│ │ │ │ +
29// Debug flags are created automatically as they are accessed, so they can be
│ │ │ │ +
30// used immediately without explicitly creating them. Each flag defaults to
│ │ │ │ +
31// 'false', i.e. disabled.
│ │ │ │ +
32//
│ │ │ │ +
33// For these macro to have any effect, granular debugging must be enabled by
│ │ │ │ +
34// defining GTSAM_ENABLE_DEBUG. If NDEBUG is not defined, then
│ │ │ │ +
35// GTSAM_ENABLE_DEBUG will be automatically defined and thus granular
│ │ │ │ +
36// debugging enabled.
│ │ │ │ +
37
│ │ │ │ +
38#ifndef NDEBUG
│ │ │ │ +
39#ifndef GTSAM_ENABLE_DEBUG
│ │ │ │ +
40#define GTSAM_ENABLE_DEBUG
│ │ │ │ +
41#endif
│ │ │ │ +
42#endif
│ │ │ │ +
43
│ │ │ │ +
44namespace gtsam {
│ │ │ │ +
45 GTSAM_EXTERN_EXPORT FastMap<std::string, ValueWithDefault<bool,false> > debugFlags;
│ │ │ │ +
46
│ │ │ │ +
47 // Non-guarded use led to crashes, and solved in commit cd35db2
│ │ │ │ +
48 bool GTSAM_EXPORT guardedIsDebug(const std::string& s);
│ │ │ │ +
49 void GTSAM_EXPORT guardedSetDebug(const std::string& s, const bool v);
│ │ │ │ +
50
│ │ │ │ +
51 // function to check if compiled version has debug information
│ │ │ │ +
52 bool GTSAM_EXPORT isDebugVersion();
│ │ │ │ +
53}
│ │ │ │ +
54
│ │ │ │ +
55#undef ISDEBUG
│ │ │ │ +
56#undef SETDEBUG
│ │ │ │ +
57
│ │ │ │ +
58#ifdef GTSAM_ENABLE_DEBUG
│ │ │ │
59
│ │ │ │ -
60template <class Archive, typename Scalar_, int Rows_, int Cols_, int Ops_,
│ │ │ │ -
61 int MaxRows_, int MaxCols_>
│ │ │ │ -
62void load(Archive& ar,
│ │ │ │ -
63 Eigen::Matrix<Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_>& m,
│ │ │ │ -
64 const unsigned int /*version*/) {
│ │ │ │ -
65 size_t rows, cols;
│ │ │ │ -
66 ar >> BOOST_SERIALIZATION_NVP(rows);
│ │ │ │ -
67 ar >> BOOST_SERIALIZATION_NVP(cols);
│ │ │ │ -
68 m.resize(rows, cols);
│ │ │ │ -
69 ar >> make_nvp("data", make_array(m.data(), m.size()));
│ │ │ │ -
70}
│ │ │ │ -
71
│ │ │ │ -
72// templated version of BOOST_SERIALIZATION_SPLIT_FREE(Eigen::Matrix);
│ │ │ │ -
73template <class Archive, typename Scalar_, int Rows_, int Cols_, int Ops_,
│ │ │ │ -
74 int MaxRows_, int MaxCols_>
│ │ │ │ -
75void serialize(
│ │ │ │ -
76 Archive& ar,
│ │ │ │ -
77 Eigen::Matrix<Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_>& m,
│ │ │ │ -
78 const unsigned int version) {
│ │ │ │ -
79 split_free(ar, m, version);
│ │ │ │ -
80}
│ │ │ │ -
81
│ │ │ │ -
82// specialized to Matrix for MATLAB wrapper
│ │ │ │ -
83template <class Archive>
│ │ │ │ -
84void serialize(Archive& ar, gtsam::Matrix& m, const unsigned int version) {
│ │ │ │ -
85 split_free(ar, m, version);
│ │ │ │ -
86}
│ │ │ │ -
87
│ │ │ │ -
88} // namespace serialization
│ │ │ │ -
89} // namespace boost
│ │ │ │ -
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
60#define ISDEBUG(S) (gtsam::guardedIsDebug(S))
│ │ │ │ +
61#define SETDEBUG(S,V) ((void)(gtsam::guardedSetDebug(S,V)))
│ │ │ │ +
62
│ │ │ │ +
63#else
│ │ │ │ +
64
│ │ │ │ +
65#define ISDEBUG(S) (false)
│ │ │ │ +
66#define SETDEBUG(S,V) ((void)false)
│ │ │ │ +
67
│ │ │ │ +
68#endif
│ │ │ │ +
69
│ │ │ │ +
FastMap.h
A thin wrapper around std::map that uses boost's fast_pool_allocator.
│ │ │ │ +
global_includes.h
Included from all GTSAM files.
│ │ │ │ +
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,82 +1,83 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -MatrixSerialization.h │ │ │ │ │ +debug.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19// \callgraph │ │ │ │ │ -20 │ │ │ │ │ -21#pragma once │ │ │ │ │ +19#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_M_a_p_._h> │ │ │ │ │ +20#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ +21#include │ │ │ │ │ 22 │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ -24 │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ -28 │ │ │ │ │ -29namespace boost { │ │ │ │ │ -30namespace serialization { │ │ │ │ │ -31 │ │ │ │ │ -47// split version - sends sizes ahead │ │ │ │ │ -48template │ │ │ │ │ -_5_0void save( │ │ │ │ │ -51 Archive& ar, │ │ │ │ │ -52 const Eigen::Matrix& m, │ │ │ │ │ -53 const unsigned int /*version*/) { │ │ │ │ │ -54 const size_t rows = m.rows(), cols = m.cols(); │ │ │ │ │ -55 ar << BOOST_SERIALIZATION_NVP(rows); │ │ │ │ │ -56 ar << BOOST_SERIALIZATION_NVP(cols); │ │ │ │ │ -57 ar << make_nvp("data", make_array(m.data(), m.size())); │ │ │ │ │ -58} │ │ │ │ │ +23 │ │ │ │ │ +24// This file defines granular debugging flags that may be switched on and off │ │ │ │ │ +25// at run time. Typical usage is 'if(ISDEBUG("myFunction"))' to check if the │ │ │ │ │ +26// 'myFunction' flag is enabled, and SETDEBUG("myFunction", true) to enable │ │ │ │ │ +27// this flag, or SETDEBUG("myFunction", false) to disable it. │ │ │ │ │ +28// │ │ │ │ │ +29// Debug flags are created automatically as they are accessed, so they can be │ │ │ │ │ +30// used immediately without explicitly creating them. Each flag defaults to │ │ │ │ │ +31// 'false', i.e. disabled. │ │ │ │ │ +32// │ │ │ │ │ +33// For these macro to have any effect, granular debugging must be enabled by │ │ │ │ │ +34// defining GTSAM_ENABLE_DEBUG. If NDEBUG is not defined, then │ │ │ │ │ +35// GTSAM_ENABLE_DEBUG will be automatically defined and thus granular │ │ │ │ │ +36// debugging enabled. │ │ │ │ │ +37 │ │ │ │ │ +38#ifndef NDEBUG │ │ │ │ │ +39#ifndef GTSAM_ENABLE_DEBUG │ │ │ │ │ +40#define GTSAM_ENABLE_DEBUG │ │ │ │ │ +41#endif │ │ │ │ │ +42#endif │ │ │ │ │ +43 │ │ │ │ │ +44namespace _g_t_s_a_m { │ │ │ │ │ +45 GTSAM_EXTERN_EXPORT FastMap > │ │ │ │ │ +debugFlags; │ │ │ │ │ +46 │ │ │ │ │ +47 // Non-guarded use led to crashes, and solved in commit cd35db2 │ │ │ │ │ +48 bool GTSAM_EXPORT guardedIsDebug(const std::string& s); │ │ │ │ │ +49 void GTSAM_EXPORT guardedSetDebug(const std::string& s, const bool v); │ │ │ │ │ +50 │ │ │ │ │ +51 // function to check if compiled version has debug information │ │ │ │ │ +52 bool GTSAM_EXPORT isDebugVersion(); │ │ │ │ │ +53} │ │ │ │ │ +54 │ │ │ │ │ +55#undef ISDEBUG │ │ │ │ │ +56#undef SETDEBUG │ │ │ │ │ +57 │ │ │ │ │ +58#ifdef GTSAM_ENABLE_DEBUG │ │ │ │ │ 59 │ │ │ │ │ -60template │ │ │ │ │ -62void load(Archive& ar, │ │ │ │ │ -63 Eigen::Matrix& m, │ │ │ │ │ -64 const unsigned int /*version*/) { │ │ │ │ │ -65 size_t rows, cols; │ │ │ │ │ -66 ar >> BOOST_SERIALIZATION_NVP(rows); │ │ │ │ │ -67 ar >> BOOST_SERIALIZATION_NVP(cols); │ │ │ │ │ -68 m.resize(rows, cols); │ │ │ │ │ -69 ar >> make_nvp("data", make_array(m.data(), m.size())); │ │ │ │ │ -70} │ │ │ │ │ -71 │ │ │ │ │ -72// templated version of BOOST_SERIALIZATION_SPLIT_FREE(Eigen::Matrix); │ │ │ │ │ -73template │ │ │ │ │ -75void serialize( │ │ │ │ │ -76 Archive& ar, │ │ │ │ │ -77 Eigen::Matrix& m, │ │ │ │ │ -78 const unsigned int version) { │ │ │ │ │ -79 split_free(ar, m, version); │ │ │ │ │ -80} │ │ │ │ │ -81 │ │ │ │ │ -82// specialized to Matrix for MATLAB wrapper │ │ │ │ │ -83template │ │ │ │ │ -84void serialize(Archive& ar, gtsam::Matrix& m, const unsigned int version) { │ │ │ │ │ -85 split_free(ar, m, version); │ │ │ │ │ -86} │ │ │ │ │ -87 │ │ │ │ │ -88} // namespace serialization │ │ │ │ │ -89} // namespace boost │ │ │ │ │ -_M_a_t_r_i_x_._h │ │ │ │ │ -typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +60#define ISDEBUG(S) (gtsam::guardedIsDebug(S)) │ │ │ │ │ +61#define SETDEBUG(S,V) ((void)(gtsam::guardedSetDebug(S,V))) │ │ │ │ │ +62 │ │ │ │ │ +63#else │ │ │ │ │ +64 │ │ │ │ │ +65#define ISDEBUG(S) (false) │ │ │ │ │ +66#define SETDEBUG(S,V) ((void)false) │ │ │ │ │ +67 │ │ │ │ │ +68#endif │ │ │ │ │ +69 │ │ │ │ │ +_F_a_s_t_M_a_p_._h │ │ │ │ │ +A thin wrapper around std::map that uses boost's fast_pool_allocator. │ │ │ │ │ +_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ +Included from all GTSAM files. │ │ │ │ │ +_g_t_s_a_m │ │ │ │ │ +Global functions in a separate testing namespace. │ │ │ │ │ +DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _M_a_t_r_i_x_S_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ + * _d_e_b_u_g_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00029.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/DSFMap.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/serializationTestHelpers.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,74 +94,114 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
│ │ │ │ -
DSFMap.h File Reference
│ │ │ │ +Functions | │ │ │ │ +Variables
│ │ │ │ +
serializationTestHelpers.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Allow for arbitrary type in DSF. │ │ │ │ -More...

│ │ │ │ - │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::DSFMap< KEY >
 Disjoint set forest using an STL map data structure underneath Uses rank compression and union by rank, iterator version. More...
 
struct  gtsam::DSFMap< KEY >::Entry
 We store the forest in an STL map, but parents are done with pointers. More...
 
class  gtsam::IndexPair
 Small utility class for representing a wrappable pairs of ints. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Typedefs

│ │ │ │ -typedef std::vector< IndexPair > gtsam::IndexPairVector
 
│ │ │ │ -typedef std::set< IndexPair > gtsam::IndexPairSet
 
│ │ │ │ -typedef std::map< IndexPair, IndexPairSet > gtsam::IndexPairSetMap
 
│ │ │ │ -typedef DSFMap< IndexPair > gtsam::DSFMapIndexPair
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ Functions

│ │ │ │ -IndexPairVector gtsam::IndexPairSetAsArray (IndexPairSet &set)
 
│ │ │ │ +template<class T >
T gtsam::serializationTestHelpers::create ()
 
│ │ │ │ +boost::filesystem::path gtsam::serializationTestHelpers::resetFilesystem (boost::filesystem::path folder="actual")
 
│ │ │ │ +template<class T >
void gtsam::serializationTestHelpers::roundtrip (const T &input, T &output)
 
│ │ │ │ +template<class T >
void gtsam::serializationTestHelpers::roundtripFile (const T &input, T &output)
 
│ │ │ │ +template<class T >
bool gtsam::serializationTestHelpers::equality (const T &input=T())
 
│ │ │ │ +template<class T >
bool gtsam::serializationTestHelpers::equalsObj (const T &input=T())
 
│ │ │ │ +template<class T >
bool gtsam::serializationTestHelpers::equalsDereferenced (const T &input)
 
│ │ │ │ +template<class T >
void gtsam::serializationTestHelpers::roundtripXML (const T &input, T &output)
 
│ │ │ │ +template<class T >
void gtsam::serializationTestHelpers::roundtripXMLFile (const T &input, T &output)
 
│ │ │ │ +template<class T >
bool gtsam::serializationTestHelpers::equalityXML (const T &input=T())
 
│ │ │ │ +template<class T >
bool gtsam::serializationTestHelpers::equalsXML (const T &input=T())
 
│ │ │ │ +template<class T >
bool gtsam::serializationTestHelpers::equalsDereferencedXML (const T &input=T())
 
│ │ │ │ +template<class T >
void gtsam::serializationTestHelpers::roundtripBinary (const T &input, T &output)
 
│ │ │ │ +template<class T >
void gtsam::serializationTestHelpers::roundtripBinaryFile (const T &input, T &output)
 
│ │ │ │ +template<class T >
bool gtsam::serializationTestHelpers::equalityBinary (const T &input=T())
 
│ │ │ │ +template<class T >
bool gtsam::serializationTestHelpers::equalsBinary (const T &input=T())
 
│ │ │ │ +template<class T >
bool gtsam::serializationTestHelpers::equalsDereferencedBinary (const T &input=T())
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Variables

│ │ │ │ +const bool verbose = false
 
│ │ │ │

Detailed Description

│ │ │ │ -

Allow for arbitrary type in DSF.

│ │ │ │ -
Date
Oct 26, 2013
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +
Author
Alex Cunningham
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │ +
Date
Feb 7, 2012
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,48 +1,92 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -DSFMap.h File Reference │ │ │ │ │ -Allow for arbitrary type in DSF. _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s | _V_a_r_i_a_b_l_e_s │ │ │ │ │ +serializationTestHelpers.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_D_S_F_M_a_p_<_ _K_E_Y_ _> │ │ │ │ │ -  Disjoint set forest using an STL map data structure underneath Uses │ │ │ │ │ - rank compression and union by rank, iterator version. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_D_S_F_M_a_p_<_ _K_E_Y_ _>_:_:_E_n_t_r_y │ │ │ │ │ -  We store the forest in an STL map, but parents are done with pointers. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_I_n_d_e_x_P_a_i_r │ │ │ │ │ -  Small utility class for representing a wrappable pairs of ints. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ - typedef std::vector< _I_n_d_e_x_P_a_i_r >  ggttssaamm::::IInnddeexxPPaaiirrVVeeccttoorr │ │ │ │ │ -  │ │ │ │ │ - typedef std::set< _I_n_d_e_x_P_a_i_r >  ggttssaamm::::IInnddeexxPPaaiirrSSeett │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +template │ │ │ │ │ + T  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::ccrreeaattee () │ │ │ │ │   │ │ │ │ │ -typedef std::map< _I_n_d_e_x_P_a_i_r, IndexPairSet >  ggttssaamm::::IInnddeexxPPaaiirrSSeettMMaapp │ │ │ │ │ +boost::filesystem::path  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::rreesseettFFiilleessyysstteemm │ │ │ │ │ + (boost::filesystem::path folder="actual") │ │ │ │ │   │ │ │ │ │ - typedef _D_S_F_M_a_p< _I_n_d_e_x_P_a_i_r >  ggttssaamm::::DDSSFFMMaappIInnddeexxPPaaiirr │ │ │ │ │ +template │ │ │ │ │ + void  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::rroouunnddttrriipp (const T │ │ │ │ │ + &input, T &output) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + void  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::rroouunnddttrriippFFiillee (const │ │ │ │ │ + T &input, T &output) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + bool  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::eeqquuaalliittyy (const T │ │ │ │ │ + &input=T()) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + bool  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::eeqquuaallssOObbjj (const T │ │ │ │ │ + &input=T()) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + bool  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::eeqquuaallssDDeerreeffeerreenncceedd │ │ │ │ │ + (const T &input) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + void  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::rroouunnddttrriippXXMMLL (const T │ │ │ │ │ + &input, T &output) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + void  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::rroouunnddttrriippXXMMLLFFiillee │ │ │ │ │ + (const T &input, T &output) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + bool  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::eeqquuaalliittyyXXMMLL (const T │ │ │ │ │ + &input=T()) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + bool  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::eeqquuaallssXXMMLL (const T │ │ │ │ │ + &input=T()) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + bool  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::eeqquuaallssDDeerreeffeerreenncceeddXXMMLL │ │ │ │ │ + (const T &input=T()) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + void  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::rroouunnddttrriippBBiinnaarryy │ │ │ │ │ + (const T &input, T &output) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + void  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::rroouunnddttrriippBBiinnaarryyFFiillee │ │ │ │ │ + (const T &input, T &output) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + bool  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::eeqquuaalliittyyBBiinnaarryy (const │ │ │ │ │ + T &input=T()) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + bool  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::eeqquuaallssBBiinnaarryy (const T │ │ │ │ │ + &input=T()) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + bool  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss:::: │ │ │ │ │ + eeqquuaallssDDeerreeffeerreenncceeddBBiinnaarryy (const T &input=T()) │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -IndexPairVector  ggttssaamm::::IInnddeexxPPaaiirrSSeettAAssAArrrraayy (IndexPairSet &set) │ │ │ │ │ +VVaarriiaabblleess │ │ │ │ │ +const bool  vveerrbboossee = false │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Allow for arbitrary type in DSF. │ │ │ │ │ - Date │ │ │ │ │ - Oct 26, 2013 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Alex Cunningham │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Feb 7, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _D_S_F_M_a_p_._h │ │ │ │ │ + * _s_e_r_i_a_l_i_z_a_t_i_o_n_T_e_s_t_H_e_l_p_e_r_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00029_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/DSFMap.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/serializationTestHelpers.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,161 +98,202 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
DSFMap.h
│ │ │ │ +
serializationTestHelpers.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <cstdlib> // Provides size_t
│ │ │ │ -
22#include <map>
│ │ │ │ -
23#include <set>
│ │ │ │ -
24#include <vector>
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <iostream>
│ │ │ │ +
23#include <sstream>
│ │ │ │ +
24#include <string>
│ │ │ │
25
│ │ │ │ -
26namespace gtsam {
│ │ │ │ +
26#include <gtsam/base/serialization.h>
│ │ │ │
27
│ │ │ │ -
33template <class KEY>
│ │ │ │ -
│ │ │ │ -
34class DSFMap {
│ │ │ │ -
35 protected:
│ │ │ │ -
│ │ │ │ -
37 struct Entry {
│ │ │ │ -
38 typename std::map<KEY, Entry>::iterator parent_;
│ │ │ │ -
39 size_t rank_;
│ │ │ │ -
40 Entry() {}
│ │ │ │ -
41 };
│ │ │ │ -
│ │ │ │ -
42
│ │ │ │ -
43 typedef typename std::map<KEY, Entry> Map;
│ │ │ │ -
44 typedef typename Map::iterator iterator;
│ │ │ │ -
45 mutable Map entries_;
│ │ │ │ -
46
│ │ │ │ -
│ │ │ │ -
48 iterator find__(const KEY& key) const {
│ │ │ │ -
49 static const Entry empty;
│ │ │ │ -
50 iterator it = entries_.find(key);
│ │ │ │ -
51 // if key does not exist, create and return itself
│ │ │ │ -
52 if (it == entries_.end()) {
│ │ │ │ -
53 it = entries_.insert(std::make_pair(key, empty)).first;
│ │ │ │ -
54 it->second.parent_ = it;
│ │ │ │ -
55 it->second.rank_ = 0;
│ │ │ │ -
56 }
│ │ │ │ -
57 return it;
│ │ │ │ -
58 }
│ │ │ │ -
│ │ │ │ +
28#include <boost/serialization/serialization.hpp>
│ │ │ │ +
29#include <boost/filesystem.hpp>
│ │ │ │ +
30
│ │ │ │ +
31
│ │ │ │ +
32// whether to print the serialized text to stdout
│ │ │ │ +
33const bool verbose = false;
│ │ │ │ +
34
│ │ │ │ +
35namespace gtsam {
│ │ │ │ +
36namespace serializationTestHelpers {
│ │ │ │ +
37
│ │ │ │ +
38// templated default object creation so we only need to declare one friend (if applicable)
│ │ │ │ +
39template<class T>
│ │ │ │ +
40T create() {
│ │ │ │ +
41 return T();
│ │ │ │ +
42}
│ │ │ │ +
43
│ │ │ │ +
44// Creates or empties a folder in the build folder and returns the relative path
│ │ │ │ +
45inline boost::filesystem::path resetFilesystem(
│ │ │ │ +
46 boost::filesystem::path folder = "actual") {
│ │ │ │ +
47 boost::filesystem::remove_all(folder);
│ │ │ │ +
48 boost::filesystem::create_directory(folder);
│ │ │ │ +
49 return folder;
│ │ │ │ +
50}
│ │ │ │ +
51
│ │ │ │ +
52// Templated round-trip serialization
│ │ │ │ +
53template<class T>
│ │ │ │ +
54void roundtrip(const T& input, T& output) {
│ │ │ │ +
55 std::string serialized = serialize(input);
│ │ │ │ +
56 if (verbose) std::cout << serialized << std::endl << std::endl;
│ │ │ │ +
57 deserialize(serialized, output);
│ │ │ │ +
58}
│ │ │ │
59
│ │ │ │ -
│ │ │ │ -
61 iterator find_(const iterator& it) const {
│ │ │ │ -
62 // follow parent pointers until we reach set representative
│ │ │ │ -
63 iterator& parent = it->second.parent_;
│ │ │ │ -
64 if (parent != it) parent = find_(parent); // not yet, recurse!
│ │ │ │ -
65 return parent;
│ │ │ │ -
66 }
│ │ │ │ -
│ │ │ │ +
60// Templated round-trip serialization using a file
│ │ │ │ +
61template<class T>
│ │ │ │ +
62void roundtripFile(const T& input, T& output) {
│ │ │ │ +
63 boost::filesystem::path path = resetFilesystem()/"graph.dat";
│ │ │ │ +
64 serializeToFile(input, path.string());
│ │ │ │ +
65 deserializeFromFile(path.string(), output);
│ │ │ │ +
66}
│ │ │ │
67
│ │ │ │ -
│ │ │ │ -
69 inline iterator find_(const KEY& key) const {
│ │ │ │ -
70 iterator initial = find__(key);
│ │ │ │ -
71 return find_(initial);
│ │ │ │ -
72 }
│ │ │ │ -
│ │ │ │ -
73
│ │ │ │ -
74 public:
│ │ │ │ -
75 typedef std::set<KEY> Set;
│ │ │ │ +
68// This version requires equality operator and uses string and file round-trips
│ │ │ │ +
69template<class T>
│ │ │ │ +
70bool equality(const T& input = T()) {
│ │ │ │ +
71 T output = create<T>(), outputf = create<T>();
│ │ │ │ +
72 roundtrip<T>(input,output);
│ │ │ │ +
73 roundtripFile<T>(input,outputf);
│ │ │ │ +
74 return (input==output) && (input==outputf);
│ │ │ │ +
75}
│ │ │ │
76
│ │ │ │ -
78 DSFMap() {}
│ │ │ │ -
79
│ │ │ │ -
│ │ │ │ -
81 inline KEY find(const KEY& key) const {
│ │ │ │ -
82 iterator root = find_(key);
│ │ │ │ -
83 return root->first;
│ │ │ │ -
84 }
│ │ │ │ -
│ │ │ │ -
85
│ │ │ │ -
│ │ │ │ -
87 void merge(const KEY& x, const KEY& y) {
│ │ │ │ -
88 // straight from http://en.wikipedia.org/wiki/Disjoint-set_data_structure
│ │ │ │ -
89 iterator xRoot = find_(x);
│ │ │ │ -
90 iterator yRoot = find_(y);
│ │ │ │ -
91 if (xRoot == yRoot) return;
│ │ │ │ +
77// This version requires Testable
│ │ │ │ +
78template<class T>
│ │ │ │ +
79bool equalsObj(const T& input = T()) {
│ │ │ │ +
80 T output = create<T>();
│ │ │ │ +
81 roundtrip<T>(input,output);
│ │ │ │ +
82 return assert_equal(input, output);
│ │ │ │ +
83}
│ │ │ │ +
84
│ │ │ │ +
85// De-referenced version for pointers, requires equals method
│ │ │ │ +
86template<class T>
│ │ │ │ +
87bool equalsDereferenced(const T& input) {
│ │ │ │ +
88 T output = create<T>();
│ │ │ │ +
89 roundtrip<T>(input,output);
│ │ │ │ +
90 return input->equals(*output);
│ │ │ │ +
91}
│ │ │ │
92
│ │ │ │ -
93 // Merge sets
│ │ │ │ -
94 if (xRoot->second.rank_ < yRoot->second.rank_)
│ │ │ │ -
95 xRoot->second.parent_ = yRoot;
│ │ │ │ -
96 else if (xRoot->second.rank_ > yRoot->second.rank_)
│ │ │ │ -
97 yRoot->second.parent_ = xRoot;
│ │ │ │ -
98 else {
│ │ │ │ -
99 yRoot->second.parent_ = xRoot;
│ │ │ │ -
100 xRoot->second.rank_ = xRoot->second.rank_ + 1;
│ │ │ │ -
101 }
│ │ │ │ -
102 }
│ │ │ │ -
│ │ │ │ -
103
│ │ │ │ -
│ │ │ │ -
105 std::map<KEY, Set> sets() const {
│ │ │ │ -
106 std::map<KEY, Set> sets;
│ │ │ │ -
107 iterator it = entries_.begin();
│ │ │ │ -
108 for (; it != entries_.end(); it++) {
│ │ │ │ -
109 iterator root = find_(it);
│ │ │ │ -
110 sets[root->first].insert(it->first);
│ │ │ │ -
111 }
│ │ │ │ -
112 return sets;
│ │ │ │ -
113 }
│ │ │ │ -
│ │ │ │ -
114};
│ │ │ │ -
│ │ │ │ -
115
│ │ │ │ -
│ │ │ │ -
117class IndexPair : public std::pair<size_t,size_t> {
│ │ │ │ -
118 public:
│ │ │ │ -
119 inline IndexPair(): std::pair<size_t,size_t>(0,0) {}
│ │ │ │ -
120 inline IndexPair(size_t i, size_t j) : std::pair<size_t,size_t>(i,j) {}
│ │ │ │ -
121 inline size_t i() const { return first; };
│ │ │ │ -
122 inline size_t j() const { return second; };
│ │ │ │ -
123};
│ │ │ │ -
│ │ │ │ -
124
│ │ │ │ -
125typedef std::vector<IndexPair> IndexPairVector;
│ │ │ │ -
126typedef std::set<IndexPair> IndexPairSet;
│ │ │ │ -
127
│ │ │ │ -
128inline IndexPairVector IndexPairSetAsArray(IndexPairSet& set) { return IndexPairVector(set.begin(), set.end()); }
│ │ │ │ -
129
│ │ │ │ -
130typedef std::map<IndexPair, IndexPairSet> IndexPairSetMap;
│ │ │ │ -
131typedef DSFMap<IndexPair> DSFMapIndexPair;
│ │ │ │ -
132} // namespace gtsam
│ │ │ │ +
93// Templated round-trip serialization using XML
│ │ │ │ +
94template<class T>
│ │ │ │ +
95void roundtripXML(const T& input, T& output) {
│ │ │ │ +
96 std::string serialized = serializeXML<T>(input);
│ │ │ │ +
97 if (verbose) std::cout << serialized << std::endl << std::endl;
│ │ │ │ +
98 deserializeXML(serialized, output);
│ │ │ │ +
99}
│ │ │ │ +
100
│ │ │ │ +
101// Templated round-trip serialization using XML File
│ │ │ │ +
102template<class T>
│ │ │ │ +
103void roundtripXMLFile(const T& input, T& output) {
│ │ │ │ +
104 boost::filesystem::path path = resetFilesystem()/"graph.xml";
│ │ │ │ +
105 serializeToXMLFile(input, path.string());
│ │ │ │ +
106 deserializeFromXMLFile(path.string(), output);
│ │ │ │ +
107}
│ │ │ │ +
108
│ │ │ │ +
109// This version requires equality operator
│ │ │ │ +
110template<class T>
│ │ │ │ +
111bool equalityXML(const T& input = T()) {
│ │ │ │ +
112 T output = create<T>(), outputf = create<T>();
│ │ │ │ +
113 roundtripXML<T>(input,output);
│ │ │ │ +
114 roundtripXMLFile<T>(input,outputf);
│ │ │ │ +
115 return (input==output) && (input==outputf);
│ │ │ │ +
116}
│ │ │ │ +
117
│ │ │ │ +
118// This version requires Testable
│ │ │ │ +
119template<class T>
│ │ │ │ +
120bool equalsXML(const T& input = T()) {
│ │ │ │ +
121 T output = create<T>();
│ │ │ │ +
122 roundtripXML<T>(input,output);
│ │ │ │ +
123 return assert_equal(input, output);
│ │ │ │ +
124}
│ │ │ │ +
125
│ │ │ │ +
126// This version is for pointers, requires equals method
│ │ │ │ +
127template<class T>
│ │ │ │ +
128bool equalsDereferencedXML(const T& input = T()) {
│ │ │ │ +
129 T output = create<T>();
│ │ │ │ +
130 roundtripXML<T>(input,output);
│ │ │ │ +
131 return input->equals(*output);
│ │ │ │ +
132}
│ │ │ │ +
133
│ │ │ │ +
134// Templated round-trip serialization using XML
│ │ │ │ +
135template<class T>
│ │ │ │ +
136void roundtripBinary(const T& input, T& output) {
│ │ │ │ +
137 std::string serialized = serializeBinary<T>(input);
│ │ │ │ +
138 if (verbose) std::cout << serialized << std::endl << std::endl;
│ │ │ │ +
139 deserializeBinary(serialized, output);
│ │ │ │ +
140}
│ │ │ │ +
141
│ │ │ │ +
142// Templated round-trip serialization using Binary file
│ │ │ │ +
143template<class T>
│ │ │ │ +
144void roundtripBinaryFile(const T& input, T& output) {
│ │ │ │ +
145 boost::filesystem::path path = resetFilesystem()/"graph.bin";
│ │ │ │ +
146 serializeToBinaryFile(input, path.string());
│ │ │ │ +
147 deserializeFromBinaryFile(path.string(), output);
│ │ │ │ +
148}
│ │ │ │ +
149
│ │ │ │ +
150// This version requires equality operator
│ │ │ │ +
151template<class T>
│ │ │ │ +
152bool equalityBinary(const T& input = T()) {
│ │ │ │ +
153 T output = create<T>(), outputf = create<T>();
│ │ │ │ +
154 roundtripBinary<T>(input,output);
│ │ │ │ +
155 roundtripBinaryFile<T>(input,outputf);
│ │ │ │ +
156 return (input==output) && (input==outputf);
│ │ │ │ +
157}
│ │ │ │ +
158
│ │ │ │ +
159// This version requires Testable
│ │ │ │ +
160template<class T>
│ │ │ │ +
161bool equalsBinary(const T& input = T()) {
│ │ │ │ +
162 T output = create<T>();
│ │ │ │ +
163 roundtripBinary<T>(input,output);
│ │ │ │ +
164 return assert_equal(input, output);
│ │ │ │ +
165}
│ │ │ │ +
166
│ │ │ │ +
167// This version is for pointers, requires equals method
│ │ │ │ +
168template<class T>
│ │ │ │ +
169bool equalsDereferencedBinary(const T& input = T()) {
│ │ │ │ +
170 T output = create<T>();
│ │ │ │ +
171 roundtripBinary<T>(input,output);
│ │ │ │ +
172 return input->equals(*output);
│ │ │ │ +
173}
│ │ │ │ +
174
│ │ │ │ +
175} // \namespace serializationTestHelpers
│ │ │ │ +
176} // \namespace gtsam
│ │ │ │ +
serialization.h
Convenience functions for serializing data structures via boost.serialization.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::DSFMap
Disjoint set forest using an STL map data structure underneath Uses rank compression and union by ran...
Definition DSFMap.h:34
│ │ │ │ -
gtsam::DSFMap::find__
iterator find__(const KEY &key) const
Given key, find iterator to initial entry.
Definition DSFMap.h:48
│ │ │ │ -
gtsam::DSFMap::find_
iterator find_(const iterator &it) const
Given iterator to initial entry, find the root Entry.
Definition DSFMap.h:61
│ │ │ │ -
gtsam::DSFMap::find_
iterator find_(const KEY &key) const
Given key, find the root Entry.
Definition DSFMap.h:69
│ │ │ │ -
gtsam::DSFMap::sets
std::map< KEY, Set > sets() const
return all sets, i.e. a partition of all elements
Definition DSFMap.h:105
│ │ │ │ -
gtsam::DSFMap::DSFMap
DSFMap()
constructor
Definition DSFMap.h:78
│ │ │ │ -
gtsam::DSFMap::find
KEY find(const KEY &key) const
Given key, find the representative key for the set in which it lives.
Definition DSFMap.h:81
│ │ │ │ -
gtsam::DSFMap::merge
void merge(const KEY &x, const KEY &y)
Merge two sets.
Definition DSFMap.h:87
│ │ │ │ -
gtsam::DSFMap::Entry
We store the forest in an STL map, but parents are done with pointers.
Definition DSFMap.h:37
│ │ │ │ -
gtsam::IndexPair
Small utility class for representing a wrappable pairs of ints.
Definition DSFMap.h:117
│ │ │ │ +
gtsam::deserializeFromXMLFile
bool deserializeFromXMLFile(const std::string &filename, T &output, const std::string &name="data")
deserializes from an XML file
Definition serialization.h:174
│ │ │ │ +
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition serialization.h:113
│ │ │ │ +
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
equals with an tolerance, prints out message if unequal
Definition Matrix.cpp:43
│ │ │ │ +
gtsam::deserializeBinary
void deserializeBinary(const std::string &serialized, T &output, const std::string &name="data")
deserializes from a string in binary
Definition serialization.h:266
│ │ │ │ +
gtsam::deserializeXML
void deserializeXML(const std::string &serialized, T &output, const std::string &name="data")
deserializes from a string in XML
Definition serialization.h:192
│ │ │ │ +
gtsam::serializeToXMLFile
bool serializeToXMLFile(const T &input, const std::string &filename, const std::string &name="data")
serializes to an XML file
Definition serialization.h:163
│ │ │ │ +
gtsam::serializeToFile
bool serializeToFile(const T &input, const std::string &filename)
serializes to a file
Definition serialization.h:93
│ │ │ │ +
gtsam::deserialize
void deserialize(const std::string &serialized, T &output)
deserializes from a string
Definition serialization.h:119
│ │ │ │ +
gtsam::serializeToBinaryFile
bool serializeToBinaryFile(const T &input, const std::string &filename, const std::string &name="data")
serializes to a binary file
Definition serialization.h:237
│ │ │ │ +
gtsam::deserializeFromFile
bool deserializeFromFile(const std::string &filename, T &output)
deserializes from a file
Definition serialization.h:103
│ │ │ │ +
gtsam::deserializeFromBinaryFile
bool deserializeFromBinaryFile(const std::string &filename, T &output, const std::string &name="data")
deserializes from a binary file
Definition serialization.h:248
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,166 +1,239 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DSFMap.h │ │ │ │ │ +serializationTestHelpers.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include // Provides size_t │ │ │ │ │ -22#include │ │ │ │ │ -23#include │ │ │ │ │ -24#include │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include │ │ │ │ │ +23#include │ │ │ │ │ +24#include │ │ │ │ │ 25 │ │ │ │ │ -26namespace _g_t_s_a_m { │ │ │ │ │ +26#include <_g_t_s_a_m_/_b_a_s_e_/_s_e_r_i_a_l_i_z_a_t_i_o_n_._h> │ │ │ │ │ 27 │ │ │ │ │ -33template │ │ │ │ │ -_3_4class _D_S_F_M_a_p { │ │ │ │ │ -35 protected: │ │ │ │ │ -_3_7 struct _E_n_t_r_y { │ │ │ │ │ -38 typename std::map::iterator parent_; │ │ │ │ │ -39 size_t rank_; │ │ │ │ │ -40 _E_n_t_r_y() {} │ │ │ │ │ -41 }; │ │ │ │ │ -42 │ │ │ │ │ -43 typedef typename std::map Map; │ │ │ │ │ -44 typedef typename Map::iterator iterator; │ │ │ │ │ -45 mutable Map entries_; │ │ │ │ │ -46 │ │ │ │ │ -_4_8 iterator _f_i_n_d____(const KEY& key) const { │ │ │ │ │ -49 static const _E_n_t_r_y empty; │ │ │ │ │ -50 iterator it = entries_.find(key); │ │ │ │ │ -51 // if key does not exist, create and return itself │ │ │ │ │ -52 if (it == entries_.end()) { │ │ │ │ │ -53 it = entries_.insert(std::make_pair(key, empty)).first; │ │ │ │ │ -54 it->second.parent_ = it; │ │ │ │ │ -55 it->second.rank_ = 0; │ │ │ │ │ -56 } │ │ │ │ │ -57 return it; │ │ │ │ │ -58 } │ │ │ │ │ +28#include │ │ │ │ │ +29#include │ │ │ │ │ +30 │ │ │ │ │ +31 │ │ │ │ │ +32// whether to print the serialized text to stdout │ │ │ │ │ +33const bool verbose = false; │ │ │ │ │ +34 │ │ │ │ │ +35namespace _g_t_s_a_m { │ │ │ │ │ +36namespace serializationTestHelpers { │ │ │ │ │ +37 │ │ │ │ │ +38// templated default object creation so we only need to declare one friend │ │ │ │ │ +(if applicable) │ │ │ │ │ +39template │ │ │ │ │ +40T create() { │ │ │ │ │ +41 return T(); │ │ │ │ │ +42} │ │ │ │ │ +43 │ │ │ │ │ +44// Creates or empties a folder in the build folder and returns the relative │ │ │ │ │ +path │ │ │ │ │ +45inline boost::filesystem::path resetFilesystem( │ │ │ │ │ +46 boost::filesystem::path folder = "actual") { │ │ │ │ │ +47 boost::filesystem::remove_all(folder); │ │ │ │ │ +48 boost::filesystem::create_directory(folder); │ │ │ │ │ +49 return folder; │ │ │ │ │ +50} │ │ │ │ │ +51 │ │ │ │ │ +52// Templated round-trip serialization │ │ │ │ │ +53template │ │ │ │ │ +54void roundtrip(const T& input, T& output) { │ │ │ │ │ +55 std::string serialized = _s_e_r_i_a_l_i_z_e(input); │ │ │ │ │ +56 if (verbose) std::cout << serialized << std::endl << std::endl; │ │ │ │ │ +57 _d_e_s_e_r_i_a_l_i_z_e(serialized, output); │ │ │ │ │ +58} │ │ │ │ │ 59 │ │ │ │ │ -_6_1 iterator _f_i_n_d__(const iterator& it) const { │ │ │ │ │ -62 // follow parent pointers until we reach set representative │ │ │ │ │ -63 iterator& parent = it->second.parent_; │ │ │ │ │ -64 if (parent != it) parent = _f_i_n_d__(parent); // not yet, recurse! │ │ │ │ │ -65 return parent; │ │ │ │ │ -66 } │ │ │ │ │ +60// Templated round-trip serialization using a file │ │ │ │ │ +61template │ │ │ │ │ +62void roundtripFile(const T& input, T& output) { │ │ │ │ │ +63 boost::filesystem::path path = resetFilesystem()/"graph.dat"; │ │ │ │ │ +64 _s_e_r_i_a_l_i_z_e_T_o_F_i_l_e(input, path.string()); │ │ │ │ │ +65 _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_F_i_l_e(path.string(), output); │ │ │ │ │ +66} │ │ │ │ │ 67 │ │ │ │ │ -_6_9 inline iterator _f_i_n_d__(const KEY& key) const { │ │ │ │ │ -70 iterator initial = _f_i_n_d____(key); │ │ │ │ │ -71 return _f_i_n_d__(initial); │ │ │ │ │ -72 } │ │ │ │ │ -73 │ │ │ │ │ -74 public: │ │ │ │ │ -75 typedef std::set Set; │ │ │ │ │ +68// This version requires equality operator and uses string and file round- │ │ │ │ │ +trips │ │ │ │ │ +69template │ │ │ │ │ +70bool equality(const T& input = T()) { │ │ │ │ │ +71 T output = create(), outputf = create(); │ │ │ │ │ +72 roundtrip(input,output); │ │ │ │ │ +73 roundtripFile(input,outputf); │ │ │ │ │ +74 return (input==output) && (input==outputf); │ │ │ │ │ +75} │ │ │ │ │ 76 │ │ │ │ │ -_7_8 _D_S_F_M_a_p() {} │ │ │ │ │ -79 │ │ │ │ │ -_8_1 inline KEY _f_i_n_d(const KEY& key) const { │ │ │ │ │ -82 iterator root = _f_i_n_d__(key); │ │ │ │ │ -83 return root->first; │ │ │ │ │ -84 } │ │ │ │ │ -85 │ │ │ │ │ -_8_7 void _m_e_r_g_e(const KEY& x, const KEY& y) { │ │ │ │ │ -88 // straight from http://en.wikipedia.org/wiki/Disjoint-set_data_structure │ │ │ │ │ -89 iterator xRoot = _f_i_n_d__(x); │ │ │ │ │ -90 iterator yRoot = _f_i_n_d__(y); │ │ │ │ │ -91 if (xRoot == yRoot) return; │ │ │ │ │ +77// This version requires Testable │ │ │ │ │ +78template │ │ │ │ │ +79bool equalsObj(const T& input = T()) { │ │ │ │ │ +80 T output = create(); │ │ │ │ │ +81 roundtrip(input,output); │ │ │ │ │ +82 return _a_s_s_e_r_t___e_q_u_a_l(input, output); │ │ │ │ │ +83} │ │ │ │ │ +84 │ │ │ │ │ +85// De-referenced version for pointers, requires equals method │ │ │ │ │ +86template │ │ │ │ │ +87bool equalsDereferenced(const T& input) { │ │ │ │ │ +88 T output = create(); │ │ │ │ │ +89 roundtrip(input,output); │ │ │ │ │ +90 return input->equals(*output); │ │ │ │ │ +91} │ │ │ │ │ 92 │ │ │ │ │ -93 // Merge sets │ │ │ │ │ -94 if (xRoot->second.rank_ < yRoot->second.rank_) │ │ │ │ │ -95 xRoot->second.parent_ = yRoot; │ │ │ │ │ -96 else if (xRoot->second.rank_ > yRoot->second.rank_) │ │ │ │ │ -97 yRoot->second.parent_ = xRoot; │ │ │ │ │ -98 else { │ │ │ │ │ -99 yRoot->second.parent_ = xRoot; │ │ │ │ │ -100 xRoot->second.rank_ = xRoot->second.rank_ + 1; │ │ │ │ │ -101 } │ │ │ │ │ -102 } │ │ │ │ │ -103 │ │ │ │ │ -_1_0_5 std::map _s_e_t_s() const { │ │ │ │ │ -106 std::map _s_e_t_s; │ │ │ │ │ -107 iterator it = entries_.begin(); │ │ │ │ │ -108 for (; it != entries_.end(); it++) { │ │ │ │ │ -109 iterator root = _f_i_n_d__(it); │ │ │ │ │ -110 _s_e_t_s[root->first].insert(it->first); │ │ │ │ │ -111 } │ │ │ │ │ -112 return _s_e_t_s; │ │ │ │ │ -113 } │ │ │ │ │ -114}; │ │ │ │ │ -115 │ │ │ │ │ -_1_1_7class _I_n_d_e_x_P_a_i_r : public std::pair { │ │ │ │ │ -118 public: │ │ │ │ │ -119 inline _I_n_d_e_x_P_a_i_r(): std::pair(0,0) {} │ │ │ │ │ -120 inline _I_n_d_e_x_P_a_i_r(size_t i, size_t j) : std::pair(i,j) {} │ │ │ │ │ -121 inline size_t i() const { return first; }; │ │ │ │ │ -122 inline size_t j() const { return second; }; │ │ │ │ │ -123}; │ │ │ │ │ -124 │ │ │ │ │ -125typedef std::vector IndexPairVector; │ │ │ │ │ -126typedef std::set IndexPairSet; │ │ │ │ │ -127 │ │ │ │ │ -128inline IndexPairVector IndexPairSetAsArray(IndexPairSet& set) { return │ │ │ │ │ -IndexPairVector(set.begin(), set.end()); } │ │ │ │ │ -129 │ │ │ │ │ -130typedef std::map IndexPairSetMap; │ │ │ │ │ -131typedef DSFMap DSFMapIndexPair; │ │ │ │ │ -132} // namespace gtsam │ │ │ │ │ +93// Templated round-trip serialization using XML │ │ │ │ │ +94template │ │ │ │ │ +95void roundtripXML(const T& input, T& output) { │ │ │ │ │ +96 std::string serialized = serializeXML(input); │ │ │ │ │ +97 if (verbose) std::cout << serialized << std::endl << std::endl; │ │ │ │ │ +98 _d_e_s_e_r_i_a_l_i_z_e_X_M_L(serialized, output); │ │ │ │ │ +99} │ │ │ │ │ +100 │ │ │ │ │ +101// Templated round-trip serialization using XML File │ │ │ │ │ +102template │ │ │ │ │ +103void roundtripXMLFile(const T& input, T& output) { │ │ │ │ │ +104 boost::filesystem::path path = resetFilesystem()/"graph.xml"; │ │ │ │ │ +105 _s_e_r_i_a_l_i_z_e_T_o_X_M_L_F_i_l_e(input, path.string()); │ │ │ │ │ +106 _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_X_M_L_F_i_l_e(path.string(), output); │ │ │ │ │ +107} │ │ │ │ │ +108 │ │ │ │ │ +109// This version requires equality operator │ │ │ │ │ +110template │ │ │ │ │ +111bool equalityXML(const T& input = T()) { │ │ │ │ │ +112 T output = create(), outputf = create(); │ │ │ │ │ +113 roundtripXML(input,output); │ │ │ │ │ +114 roundtripXMLFile(input,outputf); │ │ │ │ │ +115 return (input==output) && (input==outputf); │ │ │ │ │ +116} │ │ │ │ │ +117 │ │ │ │ │ +118// This version requires Testable │ │ │ │ │ +119template │ │ │ │ │ +120bool equalsXML(const T& input = T()) { │ │ │ │ │ +121 T output = create(); │ │ │ │ │ +122 roundtripXML(input,output); │ │ │ │ │ +123 return _a_s_s_e_r_t___e_q_u_a_l(input, output); │ │ │ │ │ +124} │ │ │ │ │ +125 │ │ │ │ │ +126// This version is for pointers, requires equals method │ │ │ │ │ +127template │ │ │ │ │ +128bool equalsDereferencedXML(const T& input = T()) { │ │ │ │ │ +129 T output = create(); │ │ │ │ │ +130 roundtripXML(input,output); │ │ │ │ │ +131 return input->equals(*output); │ │ │ │ │ +132} │ │ │ │ │ +133 │ │ │ │ │ +134// Templated round-trip serialization using XML │ │ │ │ │ +135template │ │ │ │ │ +136void roundtripBinary(const T& input, T& output) { │ │ │ │ │ +137 std::string serialized = serializeBinary(input); │ │ │ │ │ +138 if (verbose) std::cout << serialized << std::endl << std::endl; │ │ │ │ │ +139 _d_e_s_e_r_i_a_l_i_z_e_B_i_n_a_r_y(serialized, output); │ │ │ │ │ +140} │ │ │ │ │ +141 │ │ │ │ │ +142// Templated round-trip serialization using Binary file │ │ │ │ │ +143template │ │ │ │ │ +144void roundtripBinaryFile(const T& input, T& output) { │ │ │ │ │ +145 boost::filesystem::path path = resetFilesystem()/"graph.bin"; │ │ │ │ │ +146 _s_e_r_i_a_l_i_z_e_T_o_B_i_n_a_r_y_F_i_l_e(input, path.string()); │ │ │ │ │ +147 _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_B_i_n_a_r_y_F_i_l_e(path.string(), output); │ │ │ │ │ +148} │ │ │ │ │ +149 │ │ │ │ │ +150// This version requires equality operator │ │ │ │ │ +151template │ │ │ │ │ +152bool equalityBinary(const T& input = T()) { │ │ │ │ │ +153 T output = create(), outputf = create(); │ │ │ │ │ +154 roundtripBinary(input,output); │ │ │ │ │ +155 roundtripBinaryFile(input,outputf); │ │ │ │ │ +156 return (input==output) && (input==outputf); │ │ │ │ │ +157} │ │ │ │ │ +158 │ │ │ │ │ +159// This version requires Testable │ │ │ │ │ +160template │ │ │ │ │ +161bool equalsBinary(const T& input = T()) { │ │ │ │ │ +162 T output = create(); │ │ │ │ │ +163 roundtripBinary(input,output); │ │ │ │ │ +164 return _a_s_s_e_r_t___e_q_u_a_l(input, output); │ │ │ │ │ +165} │ │ │ │ │ +166 │ │ │ │ │ +167// This version is for pointers, requires equals method │ │ │ │ │ +168template │ │ │ │ │ +169bool equalsDereferencedBinary(const T& input = T()) { │ │ │ │ │ +170 T output = create(); │ │ │ │ │ +171 roundtripBinary(input,output); │ │ │ │ │ +172 return input->equals(*output); │ │ │ │ │ +173} │ │ │ │ │ +174 │ │ │ │ │ +175} // \namespace serializationTestHelpers │ │ │ │ │ +176} // \namespace gtsam │ │ │ │ │ +_s_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ +Convenience functions for serializing data structures via boost.serialization. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_D_S_F_M_a_p │ │ │ │ │ -Disjoint set forest using an STL map data structure underneath Uses rank │ │ │ │ │ -compression and union by ran... │ │ │ │ │ -DDeeffiinniittiioonn DSFMap.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_D_S_F_M_a_p_:_:_f_i_n_d____ │ │ │ │ │ -iterator find__(const KEY &key) const │ │ │ │ │ -Given key, find iterator to initial entry. │ │ │ │ │ -DDeeffiinniittiioonn DSFMap.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_D_S_F_M_a_p_:_:_f_i_n_d__ │ │ │ │ │ -iterator find_(const iterator &it) const │ │ │ │ │ -Given iterator to initial entry, find the root Entry. │ │ │ │ │ -DDeeffiinniittiioonn DSFMap.h:61 │ │ │ │ │ -_g_t_s_a_m_:_:_D_S_F_M_a_p_:_:_f_i_n_d__ │ │ │ │ │ -iterator find_(const KEY &key) const │ │ │ │ │ -Given key, find the root Entry. │ │ │ │ │ -DDeeffiinniittiioonn DSFMap.h:69 │ │ │ │ │ -_g_t_s_a_m_:_:_D_S_F_M_a_p_:_:_s_e_t_s │ │ │ │ │ -std::map< KEY, Set > sets() const │ │ │ │ │ -return all sets, i.e. a partition of all elements │ │ │ │ │ -DDeeffiinniittiioonn DSFMap.h:105 │ │ │ │ │ -_g_t_s_a_m_:_:_D_S_F_M_a_p_:_:_D_S_F_M_a_p │ │ │ │ │ -DSFMap() │ │ │ │ │ -constructor │ │ │ │ │ -DDeeffiinniittiioonn DSFMap.h:78 │ │ │ │ │ -_g_t_s_a_m_:_:_D_S_F_M_a_p_:_:_f_i_n_d │ │ │ │ │ -KEY find(const KEY &key) const │ │ │ │ │ -Given key, find the representative key for the set in which it lives. │ │ │ │ │ -DDeeffiinniittiioonn DSFMap.h:81 │ │ │ │ │ -_g_t_s_a_m_:_:_D_S_F_M_a_p_:_:_m_e_r_g_e │ │ │ │ │ -void merge(const KEY &x, const KEY &y) │ │ │ │ │ -Merge two sets. │ │ │ │ │ -DDeeffiinniittiioonn DSFMap.h:87 │ │ │ │ │ -_g_t_s_a_m_:_:_D_S_F_M_a_p_:_:_E_n_t_r_y │ │ │ │ │ -We store the forest in an STL map, but parents are done with pointers. │ │ │ │ │ -DDeeffiinniittiioonn DSFMap.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_I_n_d_e_x_P_a_i_r │ │ │ │ │ -Small utility class for representing a wrappable pairs of ints. │ │ │ │ │ -DDeeffiinniittiioonn DSFMap.h:117 │ │ │ │ │ +_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_X_M_L_F_i_l_e │ │ │ │ │ +bool deserializeFromXMLFile(const std::string &filename, T &output, const std:: │ │ │ │ │ +string &name="data") │ │ │ │ │ +deserializes from an XML file │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:174 │ │ │ │ │ +_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e │ │ │ │ │ +std::string serialize(const T &input) │ │ │ │ │ +serializes to a string │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:113 │ │ │ │ │ +_g_t_s_a_m_:_:_a_s_s_e_r_t___e_q_u_a_l │ │ │ │ │ +bool assert_equal(const Matrix &expected, const Matrix &actual, double tol) │ │ │ │ │ +equals with an tolerance, prints out message if unequal │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:43 │ │ │ │ │ +_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_B_i_n_a_r_y │ │ │ │ │ +void deserializeBinary(const std::string &serialized, T &output, const std:: │ │ │ │ │ +string &name="data") │ │ │ │ │ +deserializes from a string in binary │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:266 │ │ │ │ │ +_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_X_M_L │ │ │ │ │ +void deserializeXML(const std::string &serialized, T &output, const std::string │ │ │ │ │ +&name="data") │ │ │ │ │ +deserializes from a string in XML │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:192 │ │ │ │ │ +_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_X_M_L_F_i_l_e │ │ │ │ │ +bool serializeToXMLFile(const T &input, const std::string &filename, const │ │ │ │ │ +std::string &name="data") │ │ │ │ │ +serializes to an XML file │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:163 │ │ │ │ │ +_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_F_i_l_e │ │ │ │ │ +bool serializeToFile(const T &input, const std::string &filename) │ │ │ │ │ +serializes to a file │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:93 │ │ │ │ │ +_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e │ │ │ │ │ +void deserialize(const std::string &serialized, T &output) │ │ │ │ │ +deserializes from a string │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:119 │ │ │ │ │ +_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_B_i_n_a_r_y_F_i_l_e │ │ │ │ │ +bool serializeToBinaryFile(const T &input, const std::string &filename, const │ │ │ │ │ +std::string &name="data") │ │ │ │ │ +serializes to a binary file │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:237 │ │ │ │ │ +_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_F_i_l_e │ │ │ │ │ +bool deserializeFromFile(const std::string &filename, T &output) │ │ │ │ │ +deserializes from a file │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:103 │ │ │ │ │ +_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_B_i_n_a_r_y_F_i_l_e │ │ │ │ │ +bool deserializeFromBinaryFile(const std::string &filename, T &output, const │ │ │ │ │ +std::string &name="data") │ │ │ │ │ +deserializes from a binary file │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:248 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _D_S_F_M_a_p_._h │ │ │ │ │ + * _s_e_r_i_a_l_i_z_a_t_i_o_n_T_e_s_t_H_e_l_p_e_r_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00038_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/testLie.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/FastMap.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,111 +98,91 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
testLie.h
│ │ │ │ +
FastMap.h
│ │ │ │
│ │ │ │
│ │ │ │ -
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
12/*
│ │ │ │ -
13 * @file testLie.h
│ │ │ │ -
14 * @brief Test utilities for Lie groups
│ │ │ │ -
15 * @date November, 2014
│ │ │ │ -
16 * @author Paul Furgale
│ │ │ │ -
17 */
│ │ │ │ -
18
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/base/Lie.h>
│ │ │ │ -
22#include <gtsam/base/numericalDerivative.h>
│ │ │ │ -
23
│ │ │ │ -
24#include <CppUnitLite/TestResult.h>
│ │ │ │ -
25#include <CppUnitLite/Test.h>
│ │ │ │ -
26#include <CppUnitLite/Failure.h>
│ │ │ │ +
21#include <gtsam/base/FastDefaultAllocator.h>
│ │ │ │ +
22#include <boost/serialization/nvp.hpp>
│ │ │ │ +
23#include <boost/serialization/map.hpp>
│ │ │ │ +
24#include <map>
│ │ │ │ +
25
│ │ │ │ +
26namespace gtsam {
│ │ │ │
27
│ │ │ │ -
28namespace gtsam {
│ │ │ │ -
29
│ │ │ │ -
30// Do a comprehensive test of Lie Group derivatives
│ │ │ │ -
31template<typename G>
│ │ │ │ -
32void testLieGroupDerivatives(TestResult& result_, const std::string& name_,
│ │ │ │ -
33 const G& t1, const G& t2) {
│ │ │ │ -
34
│ │ │ │ -
35 Matrix H1, H2;
│ │ │ │ -
36 typedef traits<G> T;
│ │ │ │ -
37 typedef OptionalJacobian<T::dimension,T::dimension> OJ;
│ │ │ │ -
38
│ │ │ │ -
39 // Inverse
│ │ │ │ -
40 OJ none;
│ │ │ │ -
41 EXPECT(assert_equal<G>(t1.inverse(),T::Inverse(t1, H1)));
│ │ │ │ -
42 EXPECT(assert_equal(numericalDerivative21<G,G,OJ>(T::Inverse, t1, none),H1));
│ │ │ │ -
43
│ │ │ │ -
44 EXPECT(assert_equal<G>(t2.inverse(),T::Inverse(t2, H1)));
│ │ │ │ -
45 EXPECT(assert_equal(numericalDerivative21<G,G,OJ>(T::Inverse, t2, none),H1));
│ │ │ │ -
46
│ │ │ │ -
47 // Compose
│ │ │ │ -
48 EXPECT(assert_equal<G>(t1 * t2,T::Compose(t1, t2, H1, H2)));
│ │ │ │ -
49 EXPECT(assert_equal(numericalDerivative41<G,G,G,OJ,OJ>(T::Compose, t1, t2, none, none), H1));
│ │ │ │ -
50 EXPECT(assert_equal(numericalDerivative42<G,G,G,OJ,OJ>(T::Compose, t1, t2, none, none), H2));
│ │ │ │ +
36template<typename KEY, typename VALUE>
│ │ │ │ +
│ │ │ │ +
37class FastMap : public std::map<KEY, VALUE, std::less<KEY>,
│ │ │ │ +
38 typename internal::FastDefaultAllocator<std::pair<const KEY, VALUE> >::type> {
│ │ │ │ +
39
│ │ │ │ +
40public:
│ │ │ │ +
41
│ │ │ │ +
42 typedef std::map<KEY, VALUE, std::less<KEY>,
│ │ │ │ +
43 typename internal::FastDefaultAllocator<std::pair<const KEY, VALUE> >::type > Base;
│ │ │ │ +
44
│ │ │ │ +
46 FastMap() {}
│ │ │ │ +
47
│ │ │ │ +
49 template<typename INPUTITERATOR>
│ │ │ │ +
50 explicit FastMap(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {}
│ │ │ │
51
│ │ │ │ -
52 // Between
│ │ │ │ -
53 EXPECT(assert_equal<G>(t1.inverse() * t2,T::Between(t1, t2, H1, H2)));
│ │ │ │ -
54 EXPECT(assert_equal(numericalDerivative41<G,G,G,OJ,OJ>(T::Between, t1, t2, none, none), H1));
│ │ │ │ -
55 EXPECT(assert_equal(numericalDerivative42<G,G,G,OJ,OJ>(T::Between, t1, t2, none, none), H2));
│ │ │ │ -
56}
│ │ │ │ +
53 FastMap(const FastMap<KEY,VALUE>& x) : Base(x) {}
│ │ │ │ +
54
│ │ │ │ +
56 FastMap(const Base& x) : Base(x) {}
│ │ │ │
57
│ │ │ │ -
58// Do a comprehensive test of Lie Group Chart derivatives
│ │ │ │ -
59template<typename G>
│ │ │ │ -
60void testChartDerivatives(TestResult& result_, const std::string& name_,
│ │ │ │ -
61 const G& t1, const G& t2) {
│ │ │ │ +
│ │ │ │ +
59 operator std::map<KEY,VALUE>() const {
│ │ │ │ +
60 return std::map<KEY,VALUE>(this->begin(), this->end());
│ │ │ │ +
61 }
│ │ │ │ +
│ │ │ │
62
│ │ │ │ -
63 Matrix H1, H2;
│ │ │ │ -
64 typedef traits<G> T;
│ │ │ │ -
65 typedef typename T::TangentVector V;
│ │ │ │ -
66 typedef OptionalJacobian<T::dimension,T::dimension> OJ;
│ │ │ │ -
67
│ │ │ │ -
68 // Retract
│ │ │ │ -
69 OJ none;
│ │ │ │ -
70 V w12 = T::Local(t1, t2);
│ │ │ │ -
71 EXPECT(assert_equal<G>(t2, T::Retract(t1,w12, H1, H2)));
│ │ │ │ -
72 EXPECT(assert_equal(numericalDerivative41<G,G,V,OJ,OJ>(T::Retract, t1, w12, none, none), H1));
│ │ │ │ -
73 EXPECT(assert_equal(numericalDerivative42<G,G,V,OJ,OJ>(T::Retract, t1, w12, none, none), H2));
│ │ │ │ -
74
│ │ │ │ -
75 // Local
│ │ │ │ -
76 EXPECT(assert_equal(w12, T::Local(t1, t2, H1, H2)));
│ │ │ │ -
77 EXPECT(assert_equal(numericalDerivative41<V,G,G,OJ,OJ>(T::Local, t1, t2, none, none), H1));
│ │ │ │ -
78 EXPECT(assert_equal(numericalDerivative42<V,G,G,OJ,OJ>(T::Local, t1, t2, none, none), H2));
│ │ │ │ -
79}
│ │ │ │ -
80} // namespace gtsam
│ │ │ │ -
81
│ │ │ │ -
82#define CHECK_LIE_GROUP_DERIVATIVES(t1,t2) \
│ │ │ │ -
83 { gtsam::testLieGroupDerivatives(result_, name_, t1, t2); }
│ │ │ │ -
84
│ │ │ │ -
85#define CHECK_CHART_DERIVATIVES(t1,t2) \
│ │ │ │ -
86 { gtsam::testChartDerivatives(result_, name_, t1, t2); }
│ │ │ │ -
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ -
numericalDerivative.h
Some functions to compute numerical derivatives.
│ │ │ │ +
64 bool insert2(const KEY& key, const VALUE& val) { return Base::insert(std::make_pair(key, val)).second; }
│ │ │ │ +
65
│ │ │ │ +
67 bool exists(const KEY& e) const { return this->find(e) != this->end(); }
│ │ │ │ +
68
│ │ │ │ +
69private:
│ │ │ │ +
71 friend class boost::serialization::access;
│ │ │ │ +
72 template<class ARCHIVE>
│ │ │ │ +
73 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
74 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
75 }
│ │ │ │ +
76};
│ │ │ │ +
│ │ │ │ +
77
│ │ │ │ +
78}
│ │ │ │ +
FastDefaultAllocator.h
An easy way to control which allocator is used for Fast* collections.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
equals with an tolerance, prints out message if unequal
Definition Matrix.cpp:43
│ │ │ │ +
gtsam::internal::FastDefaultAllocator
Default allocator for list, map, and set types.
Definition FastDefaultAllocator.h:50
│ │ │ │ +
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ +
gtsam::FastMap::FastMap
FastMap(const Base &x)
Copy constructor from the base map class.
Definition FastMap.h:56
│ │ │ │ +
gtsam::FastMap::FastMap
FastMap(INPUTITERATOR first, INPUTITERATOR last)
Constructor from a range, passes through to base class.
Definition FastMap.h:50
│ │ │ │ +
gtsam::FastMap::insert2
bool insert2(const KEY &key, const VALUE &val)
Handy 'insert' function for Matlab wrapper.
Definition FastMap.h:64
│ │ │ │ +
gtsam::FastMap::FastMap
FastMap(const FastMap< KEY, VALUE > &x)
Copy constructor from another FastMap.
Definition FastMap.h:53
│ │ │ │ +
gtsam::FastMap::FastMap
FastMap()
Default constructor.
Definition FastMap.h:46
│ │ │ │ +
gtsam::FastMap::exists
bool exists(const KEY &e) const
Handy 'exists' function.
Definition FastMap.h:67
│ │ │ │ +
gtsam::FastMap::access
friend class boost::serialization::access
Serialization function.
Definition FastMap.h:71
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,119 +1,112 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -testLie.h │ │ │ │ │ +FastMap.h │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -12/* │ │ │ │ │ -13 * @file testLie.h │ │ │ │ │ -14 * @brief Test utilities for Lie groups │ │ │ │ │ -15 * @date November, 2014 │ │ │ │ │ -16 * @author Paul Furgale │ │ │ │ │ -17 */ │ │ │ │ │ -18 │ │ │ │ │ 19#pragma once │ │ │ │ │ 20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_b_a_s_e_/_L_i_e_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_._h> │ │ │ │ │ -23 │ │ │ │ │ -24#include │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ +21#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_._h> │ │ │ │ │ +22#include │ │ │ │ │ +23#include │ │ │ │ │ +24#include │ │ │ │ │ +25 │ │ │ │ │ +26namespace _g_t_s_a_m { │ │ │ │ │ 27 │ │ │ │ │ -28namespace _g_t_s_a_m { │ │ │ │ │ -29 │ │ │ │ │ -30// Do a comprehensive test of Lie Group derivatives │ │ │ │ │ -31template │ │ │ │ │ -32void testLieGroupDerivatives(TestResult& result_, const std::string& name_, │ │ │ │ │ -33 const G& t1, const G& t2) { │ │ │ │ │ -34 │ │ │ │ │ -35 Matrix H1, H2; │ │ │ │ │ -36 typedef traits T; │ │ │ │ │ -37 typedef OptionalJacobian OJ; │ │ │ │ │ -38 │ │ │ │ │ -39 // Inverse │ │ │ │ │ -40 OJ none; │ │ │ │ │ -41 EXPECT(assert_equal(t1.inverse(),T::Inverse(t1, H1))); │ │ │ │ │ -42 EXPECT(_a_s_s_e_r_t___e_q_u_a_l(numericalDerivative21(T::Inverse, t1, │ │ │ │ │ -none),H1)); │ │ │ │ │ -43 │ │ │ │ │ -44 EXPECT(assert_equal(t2.inverse(),T::Inverse(t2, H1))); │ │ │ │ │ -45 EXPECT(_a_s_s_e_r_t___e_q_u_a_l(numericalDerivative21(T::Inverse, t2, │ │ │ │ │ -none),H1)); │ │ │ │ │ -46 │ │ │ │ │ -47 // Compose │ │ │ │ │ -48 EXPECT(assert_equal(t1 * t2,T::Compose(t1, t2, H1, H2))); │ │ │ │ │ -49 EXPECT(_a_s_s_e_r_t___e_q_u_a_l(numericalDerivative41(T::Compose, t1, t2, │ │ │ │ │ -none, none), H1)); │ │ │ │ │ -50 EXPECT(_a_s_s_e_r_t___e_q_u_a_l(numericalDerivative42(T::Compose, t1, t2, │ │ │ │ │ -none, none), H2)); │ │ │ │ │ +36template │ │ │ │ │ +_3_7class _F_a_s_t_M_a_p : public std::map, │ │ │ │ │ +38 typename internal::FastDefaultAllocator >::type> │ │ │ │ │ +{ │ │ │ │ │ +39 │ │ │ │ │ +40public: │ │ │ │ │ +41 │ │ │ │ │ +42 typedef std::map, │ │ │ │ │ +43 typename _i_n_t_e_r_n_a_l_:_:_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_<_s_t_d_:_:_p_a_i_r_<_c_o_n_s_t_ _K_E_Y_,_ _V_A_L_U_E_> >::type │ │ │ │ │ +> Base; │ │ │ │ │ +44 │ │ │ │ │ +_4_6 _F_a_s_t_M_a_p() {} │ │ │ │ │ +47 │ │ │ │ │ +49 template │ │ │ │ │ +_5_0 explicit _F_a_s_t_M_a_p(INPUTITERATOR first, INPUTITERATOR last) : Base(first, │ │ │ │ │ +last) {} │ │ │ │ │ 51 │ │ │ │ │ -52 // Between │ │ │ │ │ -53 EXPECT(assert_equal(t1.inverse() * t2,T::Between(t1, t2, H1, H2))); │ │ │ │ │ -54 EXPECT(_a_s_s_e_r_t___e_q_u_a_l(numericalDerivative41(T::Between, t1, t2, │ │ │ │ │ -none, none), H1)); │ │ │ │ │ -55 EXPECT(_a_s_s_e_r_t___e_q_u_a_l(numericalDerivative42(T::Between, t1, t2, │ │ │ │ │ -none, none), H2)); │ │ │ │ │ -56} │ │ │ │ │ +_5_3 _F_a_s_t_M_a_p(const _F_a_s_t_M_a_p_<_K_E_Y_,_V_A_L_U_E_>& x) : Base(x) {} │ │ │ │ │ +54 │ │ │ │ │ +_5_6 _F_a_s_t_M_a_p(const Base& x) : Base(x) {} │ │ │ │ │ 57 │ │ │ │ │ -58// Do a comprehensive test of Lie Group Chart derivatives │ │ │ │ │ -59template │ │ │ │ │ -60void testChartDerivatives(TestResult& result_, const std::string& name_, │ │ │ │ │ -61 const G& t1, const G& t2) { │ │ │ │ │ +_5_9 operator std::map() const { │ │ │ │ │ +60 return std::map(this->begin(), this->end()); │ │ │ │ │ +61 } │ │ │ │ │ 62 │ │ │ │ │ -63 Matrix H1, H2; │ │ │ │ │ -64 typedef traits T; │ │ │ │ │ -65 typedef typename T::TangentVector V; │ │ │ │ │ -66 typedef OptionalJacobian OJ; │ │ │ │ │ -67 │ │ │ │ │ -68 // Retract │ │ │ │ │ -69 OJ none; │ │ │ │ │ -70 V w12 = T::Local(t1, t2); │ │ │ │ │ -71 EXPECT(assert_equal(t2, T::Retract(t1,w12, H1, H2))); │ │ │ │ │ -72 EXPECT(_a_s_s_e_r_t___e_q_u_a_l(numericalDerivative41(T::Retract, t1, w12, │ │ │ │ │ -none, none), H1)); │ │ │ │ │ -73 EXPECT(_a_s_s_e_r_t___e_q_u_a_l(numericalDerivative42(T::Retract, t1, w12, │ │ │ │ │ -none, none), H2)); │ │ │ │ │ -74 │ │ │ │ │ -75 // Local │ │ │ │ │ -76 EXPECT(_a_s_s_e_r_t___e_q_u_a_l(w12, T::Local(t1, t2, H1, H2))); │ │ │ │ │ -77 EXPECT(_a_s_s_e_r_t___e_q_u_a_l(numericalDerivative41(T::Local, t1, t2, │ │ │ │ │ -none, none), H1)); │ │ │ │ │ -78 EXPECT(_a_s_s_e_r_t___e_q_u_a_l(numericalDerivative42(T::Local, t1, t2, │ │ │ │ │ -none, none), H2)); │ │ │ │ │ -79} │ │ │ │ │ -80} // namespace gtsam │ │ │ │ │ -81 │ │ │ │ │ -82#define CHECK_LIE_GROUP_DERIVATIVES(t1,t2) \ │ │ │ │ │ -83 { gtsam::testLieGroupDerivatives(result_, name_, t1, t2); } │ │ │ │ │ -84 │ │ │ │ │ -85#define CHECK_CHART_DERIVATIVES(t1,t2) \ │ │ │ │ │ -86 { gtsam::testChartDerivatives(result_, name_, t1, t2); } │ │ │ │ │ -_L_i_e_._h │ │ │ │ │ -Base class and basic functions for Lie types. │ │ │ │ │ -_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_._h │ │ │ │ │ -Some functions to compute numerical derivatives. │ │ │ │ │ +_6_4 bool _i_n_s_e_r_t_2(const KEY& key, const VALUE& val) { return Base::insert(std:: │ │ │ │ │ +make_pair(key, val)).second; } │ │ │ │ │ +65 │ │ │ │ │ +_6_7 bool _e_x_i_s_t_s(const KEY& e) const { return this->find(e) != this->end(); } │ │ │ │ │ +68 │ │ │ │ │ +69private: │ │ │ │ │ +_7_1 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +72 template │ │ │ │ │ +73 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +74 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); │ │ │ │ │ +75 } │ │ │ │ │ +76}; │ │ │ │ │ +77 │ │ │ │ │ +78} │ │ │ │ │ +_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_._h │ │ │ │ │ +An easy way to control which allocator is used for Fast* collections. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_a_s_s_e_r_t___e_q_u_a_l │ │ │ │ │ -bool assert_equal(const Matrix &expected, const Matrix &actual, double tol) │ │ │ │ │ -equals with an tolerance, prints out message if unequal │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:43 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r │ │ │ │ │ +Default allocator for list, map, and set types. │ │ │ │ │ +DDeeffiinniittiioonn FastDefaultAllocator.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ +FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ +fast_pool_allocator instead of the defa... │ │ │ │ │ +DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_M_a_p_:_:_F_a_s_t_M_a_p │ │ │ │ │ +FastMap(const Base &x) │ │ │ │ │ +Copy constructor from the base map class. │ │ │ │ │ +DDeeffiinniittiioonn FastMap.h:56 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_M_a_p_:_:_F_a_s_t_M_a_p │ │ │ │ │ +FastMap(INPUTITERATOR first, INPUTITERATOR last) │ │ │ │ │ +Constructor from a range, passes through to base class. │ │ │ │ │ +DDeeffiinniittiioonn FastMap.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_M_a_p_:_:_i_n_s_e_r_t_2 │ │ │ │ │ +bool insert2(const KEY &key, const VALUE &val) │ │ │ │ │ +Handy 'insert' function for Matlab wrapper. │ │ │ │ │ +DDeeffiinniittiioonn FastMap.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_M_a_p_:_:_F_a_s_t_M_a_p │ │ │ │ │ +FastMap(const FastMap< KEY, VALUE > &x) │ │ │ │ │ +Copy constructor from another FastMap. │ │ │ │ │ +DDeeffiinniittiioonn FastMap.h:53 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_M_a_p_:_:_F_a_s_t_M_a_p │ │ │ │ │ +FastMap() │ │ │ │ │ +Default constructor. │ │ │ │ │ +DDeeffiinniittiioonn FastMap.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_M_a_p_:_:_e_x_i_s_t_s │ │ │ │ │ +bool exists(const KEY &e) const │ │ │ │ │ +Handy 'exists' function. │ │ │ │ │ +DDeeffiinniittiioonn FastMap.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_M_a_p_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn FastMap.h:71 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * tteessttLLiiee..hh │ │ │ │ │ + * _F_a_s_t_M_a_p_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00041_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Value.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/VectorSpace.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,100 +98,487 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Value.h
│ │ │ │ +
VectorSpace.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ -
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ -
4 * Atlanta, Georgia 30332-0415
│ │ │ │ -
5 * All Rights Reserved
│ │ │ │ -
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ -
7
│ │ │ │ -
8 * See LICENSE for the license information
│ │ │ │ -
9
│ │ │ │ -
10 * -------------------------------------------------------------------------- */
│ │ │ │ -
11
│ │ │ │ -
19#pragma once
│ │ │ │ +
1/*
│ │ │ │ +
2 * VectorSpace.h
│ │ │ │ +
3 *
│ │ │ │ +
4 * @date December 21, 2014
│ │ │ │ +
5 * @author Mike Bosse
│ │ │ │ +
6 * @author Frank Dellaert
│ │ │ │ +
7 */
│ │ │ │ +
8
│ │ │ │ +
9#pragma once
│ │ │ │ +
10
│ │ │ │ +
11#include <gtsam/base/Lie.h>
│ │ │ │ +
12
│ │ │ │ +
13namespace gtsam {
│ │ │ │ +
14
│ │ │ │ +
│ │ │ │ +
16struct vector_space_tag: public lie_group_tag {
│ │ │ │ +
17};
│ │ │ │ +
│ │ │ │ +
18
│ │ │ │ +
19template<typename T> struct traits;
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/config.h> // Configuration from CMake
│ │ │ │ +
21namespace internal {
│ │ │ │
22
│ │ │ │ -
23#include <gtsam/base/Vector.h>
│ │ │ │ -
24#include <boost/serialization/nvp.hpp>
│ │ │ │ -
25#include <boost/serialization/assume_abstract.hpp>
│ │ │ │ -
26#include <memory>
│ │ │ │ -
27
│ │ │ │ -
28namespace gtsam {
│ │ │ │ -
29
│ │ │ │ -
│ │ │ │ -
37 class GTSAM_EXPORT Value {
│ │ │ │ -
38 public:
│ │ │ │ -
39
│ │ │ │ -
41 virtual Value* clone_() const = 0;
│ │ │ │ -
42
│ │ │ │ -
44 virtual void deallocate_() const = 0;
│ │ │ │ -
45
│ │ │ │ -
47 virtual boost::shared_ptr<Value> clone() const = 0;
│ │ │ │ +
24template<class Class, int N>
│ │ │ │ +
│ │ │ │ +
25struct VectorSpaceImpl {
│ │ │ │ +
26
│ │ │ │ +
29 typedef Eigen::Matrix<double, N, 1> TangentVector;
│ │ │ │ +
30 typedef OptionalJacobian<N, N> ChartJacobian;
│ │ │ │ +
31 typedef Eigen::Matrix<double, N, N> Jacobian;
│ │ │ │ +
32 static int GetDimension(const Class&) { return N;}
│ │ │ │ +
33
│ │ │ │ +
34 static TangentVector Local(const Class& origin, const Class& other,
│ │ │ │ +
35 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ +
36 if (H1) *H1 = - Jacobian::Identity();
│ │ │ │ +
37 if (H2) *H2 = Jacobian::Identity();
│ │ │ │ +
38 Class v = other-origin;
│ │ │ │ +
39 return v.vector();
│ │ │ │ +
40 }
│ │ │ │ +
41
│ │ │ │ +
42 static Class Retract(const Class& origin, const TangentVector& v,
│ │ │ │ +
43 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ +
44 if (H1) *H1 = Jacobian::Identity();
│ │ │ │ +
45 if (H2) *H2 = Jacobian::Identity();
│ │ │ │ +
46 return origin + v;
│ │ │ │ +
47 }
│ │ │ │
48
│ │ │ │ -
50 virtual bool equals_(const Value& other, double tol = 1e-9) const = 0;
│ │ │ │ -
51
│ │ │ │ -
53 virtual void print(const std::string& str = "") const = 0;
│ │ │ │ -
54
│ │ │ │ -
60 virtual size_t dim() const = 0;
│ │ │ │ -
61
│ │ │ │ -
68 virtual Value* retract_(const Vector& delta) const = 0;
│ │ │ │ -
69
│ │ │ │ -
76 virtual Vector localCoordinates_(const Value& value) const = 0;
│ │ │ │ +
50
│ │ │ │ +
53
│ │ │ │ +
54 static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) {
│ │ │ │ +
55 if (Hm) *Hm = Jacobian::Identity();
│ │ │ │ +
56 return m.vector();
│ │ │ │ +
57 }
│ │ │ │ +
58
│ │ │ │ +
59 static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
│ │ │ │ +
60 if (Hv) *Hv = Jacobian::Identity();
│ │ │ │ +
61 return Class(v);
│ │ │ │ +
62 }
│ │ │ │ +
63
│ │ │ │ +
64 static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1 = boost::none,
│ │ │ │ +
65 ChartJacobian H2 = boost::none) {
│ │ │ │ +
66 if (H1) *H1 = Jacobian::Identity();
│ │ │ │ +
67 if (H2) *H2 = Jacobian::Identity();
│ │ │ │ +
68 return v1 + v2;
│ │ │ │ +
69 }
│ │ │ │ +
70
│ │ │ │ +
71 static Class Between(const Class& v1, const Class& v2, ChartJacobian H1 = boost::none,
│ │ │ │ +
72 ChartJacobian H2 = boost::none) {
│ │ │ │ +
73 if (H1) *H1 = - Jacobian::Identity();
│ │ │ │ +
74 if (H2) *H2 = Jacobian::Identity();
│ │ │ │ +
75 return v2 - v1;
│ │ │ │ +
76 }
│ │ │ │
77
│ │ │ │ -
│ │ │ │ -
79 virtual Value& operator=(const Value& /*rhs*/) {
│ │ │ │ -
80 //needs a empty definition so recursion in implicit derived assignment operators work
│ │ │ │ -
81 return *this;
│ │ │ │ -
82 }
│ │ │ │ -
│ │ │ │ -
83
│ │ │ │ -
85 template<typename ValueType>
│ │ │ │ -
86 const ValueType& cast() const;
│ │ │ │ -
87
│ │ │ │ -
89 virtual ~Value() {}
│ │ │ │ -
90
│ │ │ │ -
91 private:
│ │ │ │ -
122 friend class boost::serialization::access;
│ │ │ │ -
123 template<class ARCHIVE>
│ │ │ │ -
124 void serialize(ARCHIVE & /*ar*/, const unsigned int /*version*/) {
│ │ │ │ -
125 }
│ │ │ │ -
126
│ │ │ │ -
127 };
│ │ │ │ -
│ │ │ │ -
128
│ │ │ │ -
129} /* namespace gtsam */
│ │ │ │ -
130
│ │ │ │ -
131BOOST_SERIALIZATION_ASSUME_ABSTRACT(gtsam::Value)
│ │ │ │ -
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ +
78 static Class Inverse(const Class& v, ChartJacobian H = boost::none) {
│ │ │ │ +
79 if (H) *H = - Jacobian::Identity();
│ │ │ │ +
80 return -v;
│ │ │ │ +
81 }
│ │ │ │ +
82
│ │ │ │ +
84};
│ │ │ │ +
│ │ │ │ +
85
│ │ │ │ +
87template<class Class>
│ │ │ │ +
│ │ │ │ +
88struct VectorSpaceImpl<Class,Eigen::Dynamic> {
│ │ │ │ +
89
│ │ │ │ +
92 static Class Compose(const Class& v1, const Class& v2) { return v1+v2;}
│ │ │ │ +
93 static Class Between(const Class& v1, const Class& v2) { return v2-v1;}
│ │ │ │ +
94 static Class Inverse(const Class& m) { return -m;}
│ │ │ │ +
96
│ │ │ │ +
99 typedef Eigen::VectorXd TangentVector;
│ │ │ │ +
100 typedef OptionalJacobian<Eigen::Dynamic,Eigen::Dynamic> ChartJacobian;
│ │ │ │ +
101 static int GetDimension(const Class& m) { return m.dim();}
│ │ │ │ +
102
│ │ │ │ +
103 static Eigen::MatrixXd Eye(const Class& m) {
│ │ │ │ +
104 int dim = GetDimension(m);
│ │ │ │ +
105 return Eigen::MatrixXd::Identity(dim, dim);
│ │ │ │ +
106 }
│ │ │ │ +
107
│ │ │ │ +
108 static TangentVector Local(const Class& origin, const Class& other,
│ │ │ │ +
109 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ +
110 if (H1) *H1 = - Eye(origin);
│ │ │ │ +
111 if (H2) *H2 = Eye(other);
│ │ │ │ +
112 Class v = other-origin;
│ │ │ │ +
113 return v.vector();
│ │ │ │ +
114 }
│ │ │ │ +
115
│ │ │ │ +
116 static Class Retract(const Class& origin, const TangentVector& v,
│ │ │ │ +
117 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ +
118 if (H1) *H1 = Eye(origin);
│ │ │ │ +
119 if (H2) *H2 = Eye(origin);
│ │ │ │ +
120 return origin + v;
│ │ │ │ +
121 }
│ │ │ │ +
122
│ │ │ │ +
124
│ │ │ │ +
127
│ │ │ │ +
128 static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) {
│ │ │ │ +
129 if (Hm) *Hm = Eye(m);
│ │ │ │ +
130 return m.vector();
│ │ │ │ +
131 }
│ │ │ │ +
132
│ │ │ │ +
133 static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
│ │ │ │ +
134 Class result(v);
│ │ │ │ +
135 if (Hv)
│ │ │ │ +
136 *Hv = Eye(v);
│ │ │ │ +
137 return result;
│ │ │ │ +
138 }
│ │ │ │ +
139
│ │ │ │ +
140 static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1,
│ │ │ │ +
141 ChartJacobian H2 = boost::none) {
│ │ │ │ +
142 if (H1) *H1 = Eye(v1);
│ │ │ │ +
143 if (H2) *H2 = Eye(v2);
│ │ │ │ +
144 return v1 + v2;
│ │ │ │ +
145 }
│ │ │ │ +
146
│ │ │ │ +
147 static Class Between(const Class& v1, const Class& v2, ChartJacobian H1,
│ │ │ │ +
148 ChartJacobian H2 = boost::none) {
│ │ │ │ +
149 if (H1) *H1 = - Eye(v1);
│ │ │ │ +
150 if (H2) *H2 = Eye(v2);
│ │ │ │ +
151 return v2 - v1;
│ │ │ │ +
152 }
│ │ │ │ +
153
│ │ │ │ +
154 static Class Inverse(const Class& v, ChartJacobian H) {
│ │ │ │ +
155 if (H) *H = -Eye(v);
│ │ │ │ +
156 return -v;
│ │ │ │ +
157 }
│ │ │ │ +
158
│ │ │ │ +
160};
│ │ │ │ +
│ │ │ │ +
161
│ │ │ │ +
163template<class Class>
│ │ │ │ +
│ │ │ │ +
164struct HasVectorSpacePrereqs {
│ │ │ │ +
165
│ │ │ │ +
166 enum { dim = Class::dimension };
│ │ │ │ +
167
│ │ │ │ +
168 Class p, q;
│ │ │ │ +
169 Vector v;
│ │ │ │ +
170
│ │ │ │ +
171 BOOST_CONCEPT_USAGE(HasVectorSpacePrereqs) {
│ │ │ │ +
172 p = Class::Identity(); // identity
│ │ │ │ +
173 q = p + p; // addition
│ │ │ │ +
174 q = p - p; // subtraction
│ │ │ │ +
175 v = p.vector(); // conversion to vector
│ │ │ │ +
176 q = p + v; // addition of a vector on the right
│ │ │ │ +
177 }
│ │ │ │ +
178};
│ │ │ │ +
│ │ │ │ +
179
│ │ │ │ +
184template<class Class>
│ │ │ │ +
│ │ │ │ +
185struct VectorSpaceTraits: VectorSpaceImpl<Class, Class::dimension> {
│ │ │ │ +
186
│ │ │ │ +
187 // Check that Class has the necessary machinery
│ │ │ │ +
188 BOOST_CONCEPT_ASSERT((HasVectorSpacePrereqs<Class>));
│ │ │ │ +
189
│ │ │ │ +
190 typedef vector_space_tag structure_category;
│ │ │ │ +
191
│ │ │ │ +
194 typedef additive_group_tag group_flavor;
│ │ │ │ +
195 static Class Identity() { return Class::Identity();}
│ │ │ │ +
197
│ │ │ │ +
200 enum { dimension = Class::dimension};
│ │ │ │ +
201 typedef Class ManifoldType;
│ │ │ │ +
203};
│ │ │ │ +
│ │ │ │ +
204
│ │ │ │ +
206template<class Class>
│ │ │ │ +
207struct VectorSpace: Testable<Class>, VectorSpaceTraits<Class> {};
│ │ │ │ +
208
│ │ │ │ +
211template<typename Scalar>
│ │ │ │ +
│ │ │ │ +
212struct ScalarTraits : VectorSpaceImpl<Scalar, 1> {
│ │ │ │ +
213
│ │ │ │ +
214 typedef vector_space_tag structure_category;
│ │ │ │ +
215
│ │ │ │ +
218 static void Print(Scalar m, const std::string& str = "") {
│ │ │ │ +
219 gtsam::print(m, str);
│ │ │ │ +
220 }
│ │ │ │ +
221 static bool Equals(Scalar v1, Scalar v2, double tol = 1e-8) {
│ │ │ │ +
222 return std::abs(v1 - v2) < tol;
│ │ │ │ +
223 }
│ │ │ │ +
225
│ │ │ │ +
228 typedef additive_group_tag group_flavor;
│ │ │ │ +
229 static Scalar Identity() { return 0;}
│ │ │ │ +
231
│ │ │ │ +
234 typedef Scalar ManifoldType;
│ │ │ │ +
235 enum { dimension = 1 };
│ │ │ │ +
236 typedef Eigen::Matrix<double, 1, 1> TangentVector;
│ │ │ │ +
237 typedef OptionalJacobian<1, 1> ChartJacobian;
│ │ │ │ +
238
│ │ │ │ +
239 static TangentVector Local(Scalar origin, Scalar other,
│ │ │ │ +
240 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ +
241 if (H1) (*H1)[0] = -1.0;
│ │ │ │ +
242 if (H2) (*H2)[0] = 1.0;
│ │ │ │ +
243 TangentVector result;
│ │ │ │ +
244 result(0) = other - origin;
│ │ │ │ +
245 return result;
│ │ │ │ +
246 }
│ │ │ │ +
247
│ │ │ │ +
248 static Scalar Retract(Scalar origin, const TangentVector& v,
│ │ │ │ +
249 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ +
250 if (H1) (*H1)[0] = 1.0;
│ │ │ │ +
251 if (H2) (*H2)[0] = 1.0;
│ │ │ │ +
252 return origin + v[0];
│ │ │ │ +
253 }
│ │ │ │ +
255
│ │ │ │ +
258 static TangentVector Logmap(Scalar m, ChartJacobian H = boost::none) {
│ │ │ │ +
259 if (H) (*H)[0] = 1.0;
│ │ │ │ +
260 return Local(0, m);
│ │ │ │ +
261 }
│ │ │ │ +
262
│ │ │ │ +
263 static Scalar Expmap(const TangentVector& v, ChartJacobian H = boost::none) {
│ │ │ │ +
264 if (H) (*H)[0] = 1.0;
│ │ │ │ +
265 return v[0];
│ │ │ │ +
266 }
│ │ │ │ +
268
│ │ │ │ +
269};
│ │ │ │ +
│ │ │ │ +
270
│ │ │ │ +
271} // namespace internal
│ │ │ │ +
272
│ │ │ │ +
│ │ │ │ +
274template<> struct traits<double> : public internal::ScalarTraits<double> {
│ │ │ │ +
275};
│ │ │ │ +
│ │ │ │ +
276
│ │ │ │ +
│ │ │ │ +
278template<> struct traits<float> : public internal::ScalarTraits<float> {
│ │ │ │ +
279};
│ │ │ │ +
│ │ │ │ +
280
│ │ │ │ +
281// traits for any fixed double Eigen matrix
│ │ │ │ +
282template<int M, int N, int Options, int MaxRows, int MaxCols>
│ │ │ │ +
│ │ │ │ +
283struct traits<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > :
│ │ │ │ +
284 internal::VectorSpaceImpl<
│ │ │ │ +
285 Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols>, M * N> {
│ │ │ │ +
286
│ │ │ │ +
287 typedef vector_space_tag structure_category;
│ │ │ │ +
288 typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> Fixed;
│ │ │ │ +
289
│ │ │ │ +
292 static void Print(const Fixed& m, const std::string& str = "") {
│ │ │ │ +
293 gtsam::print(Eigen::MatrixXd(m), str);
│ │ │ │ +
294 }
│ │ │ │ +
295 static bool Equals(const Fixed& v1, const Fixed& v2, double tol = 1e-8) {
│ │ │ │ +
296 return equal_with_abs_tol(v1, v2, tol);
│ │ │ │ +
297 }
│ │ │ │ +
299
│ │ │ │ +
302 typedef additive_group_tag group_flavor;
│ │ │ │ +
303 static Fixed Identity() { return Fixed::Zero();}
│ │ │ │ +
305
│ │ │ │ +
308 enum { dimension = M*N};
│ │ │ │ +
309 typedef Fixed ManifoldType;
│ │ │ │ +
310 typedef Eigen::Matrix<double, dimension, 1> TangentVector;
│ │ │ │ +
311 typedef Eigen::Matrix<double, dimension, dimension> Jacobian;
│ │ │ │ +
312 typedef OptionalJacobian<dimension, dimension> ChartJacobian;
│ │ │ │ +
313
│ │ │ │ +
314 static TangentVector Local(const Fixed& origin, const Fixed& other,
│ │ │ │ +
315 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ +
316 if (H1) (*H1) = -Jacobian::Identity();
│ │ │ │ +
317 if (H2) (*H2) = Jacobian::Identity();
│ │ │ │ +
318 TangentVector result;
│ │ │ │ +
319 Eigen::Map<Fixed>(result.data()) = other - origin;
│ │ │ │ +
320 return result;
│ │ │ │ +
321 }
│ │ │ │ +
322
│ │ │ │ +
323 static Fixed Retract(const Fixed& origin, const TangentVector& v,
│ │ │ │ +
324 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ +
325 if (H1) (*H1) = Jacobian::Identity();
│ │ │ │ +
326 if (H2) (*H2) = Jacobian::Identity();
│ │ │ │ +
327 return origin + Eigen::Map<const Fixed>(v.data());
│ │ │ │ +
328 }
│ │ │ │ +
330
│ │ │ │ +
333 static TangentVector Logmap(const Fixed& m, ChartJacobian H = boost::none) {
│ │ │ │ +
334 if (H) *H = Jacobian::Identity();
│ │ │ │ +
335 TangentVector result;
│ │ │ │ +
336 Eigen::Map<Fixed>(result.data()) = m;
│ │ │ │ +
337 return result;
│ │ │ │ +
338 }
│ │ │ │ +
339
│ │ │ │ +
340 static Fixed Expmap(const TangentVector& v, ChartJacobian H = boost::none) {
│ │ │ │ +
341 Fixed m;
│ │ │ │ +
342 m.setZero();
│ │ │ │ +
343 if (H) *H = Jacobian::Identity();
│ │ │ │ +
344 return m + Eigen::Map<const Fixed>(v.data());
│ │ │ │ +
345 }
│ │ │ │ +
347};
│ │ │ │ +
│ │ │ │ +
348
│ │ │ │ +
349
│ │ │ │ +
350namespace internal {
│ │ │ │ +
351
│ │ │ │ +
352// traits for dynamic Eigen matrices
│ │ │ │ +
353template<int M, int N, int Options, int MaxRows, int MaxCols>
│ │ │ │ +
│ │ │ │ +
354struct DynamicTraits {
│ │ │ │ +
355
│ │ │ │ +
356 typedef vector_space_tag structure_category;
│ │ │ │ +
357 typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> Dynamic;
│ │ │ │ +
358
│ │ │ │ +
361 static void Print(const Dynamic& m, const std::string& str = "") {
│ │ │ │ +
362 gtsam::print(Eigen::MatrixXd(m), str);
│ │ │ │ +
363 }
│ │ │ │ +
364 static bool Equals(const Dynamic& v1, const Dynamic& v2,
│ │ │ │ +
365 double tol = 1e-8) {
│ │ │ │ +
366 return equal_with_abs_tol(v1, v2, tol);
│ │ │ │ +
367 }
│ │ │ │ +
369
│ │ │ │ +
372 typedef additive_group_tag group_flavor;
│ │ │ │ +
373 static Dynamic Identity() {
│ │ │ │ +
374 throw std::runtime_error("Identity not defined for dynamic types");
│ │ │ │ +
375 }
│ │ │ │ +
377
│ │ │ │ +
380 enum { dimension = Eigen::Dynamic };
│ │ │ │ +
381 typedef Eigen::VectorXd TangentVector;
│ │ │ │ +
382 typedef Eigen::MatrixXd Jacobian;
│ │ │ │ +
383 typedef OptionalJacobian<dimension, dimension> ChartJacobian;
│ │ │ │ +
384 typedef Dynamic ManifoldType;
│ │ │ │ +
385
│ │ │ │ +
386 static int GetDimension(const Dynamic& m) {
│ │ │ │ +
387 return m.rows() * m.cols();
│ │ │ │ +
388 }
│ │ │ │ +
389
│ │ │ │ +
390 static Jacobian Eye(const Dynamic& m) {
│ │ │ │ +
391 int dim = GetDimension(m);
│ │ │ │ +
392 return Eigen::Matrix<double, dimension, dimension>::Identity(dim, dim);
│ │ │ │ +
393 }
│ │ │ │ +
394
│ │ │ │ +
395 static TangentVector Local(const Dynamic& m, const Dynamic& other, //
│ │ │ │ +
396 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ +
397 if (H1) *H1 = -Eye(m);
│ │ │ │ +
398 if (H2) *H2 = Eye(m);
│ │ │ │ +
399 TangentVector v(GetDimension(m));
│ │ │ │ +
400 Eigen::Map<Dynamic>(v.data(), m.rows(), m.cols()) = other - m;
│ │ │ │ +
401 return v;
│ │ │ │ +
402 }
│ │ │ │ +
403
│ │ │ │ +
404 static Dynamic Retract(const Dynamic& m, const TangentVector& v, //
│ │ │ │ +
405 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ +
406 if (H1) *H1 = Eye(m);
│ │ │ │ +
407 if (H2) *H2 = Eye(m);
│ │ │ │ +
408 return m + Eigen::Map<const Dynamic>(v.data(), m.rows(), m.cols());
│ │ │ │ +
409 }
│ │ │ │ +
411
│ │ │ │ +
414 static TangentVector Logmap(const Dynamic& m, ChartJacobian H = boost::none) {
│ │ │ │ +
415 if (H) *H = Eye(m);
│ │ │ │ +
416 TangentVector result(GetDimension(m));
│ │ │ │ +
417 Eigen::Map<Dynamic>(result.data(), m.cols(), m.rows()) = m;
│ │ │ │ +
418 return result;
│ │ │ │ +
419 }
│ │ │ │ +
420
│ │ │ │ +
421 static Dynamic Expmap(const TangentVector& /*v*/, ChartJacobian H = boost::none) {
│ │ │ │ +
422 static_cast<void>(H);
│ │ │ │ +
423 throw std::runtime_error("Expmap not defined for dynamic types");
│ │ │ │ +
424 }
│ │ │ │ +
425
│ │ │ │ +
426 static Dynamic Inverse(const Dynamic& m, ChartJacobian H = boost::none) {
│ │ │ │ +
427 if (H) *H = -Eye(m);
│ │ │ │ +
428 return -m;
│ │ │ │ +
429 }
│ │ │ │ +
430
│ │ │ │ +
431 static Dynamic Compose(const Dynamic& v1, const Dynamic& v2,
│ │ │ │ +
432 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ +
433 if (H1) *H1 = Eye(v1);
│ │ │ │ +
434 if (H2) *H2 = Eye(v1);
│ │ │ │ +
435 return v1 + v2;
│ │ │ │ +
436 }
│ │ │ │ +
437
│ │ │ │ +
438 static Dynamic Between(const Dynamic& v1, const Dynamic& v2,
│ │ │ │ +
439 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ +
440 if (H1) *H1 = -Eye(v1);
│ │ │ │ +
441 if (H2) *H2 = Eye(v1);
│ │ │ │ +
442 return v2 - v1;
│ │ │ │ +
443 }
│ │ │ │ +
445
│ │ │ │ +
446};
│ │ │ │ +
│ │ │ │ +
447
│ │ │ │ +
448} // \ internal
│ │ │ │ +
449
│ │ │ │ +
450// traits for fully dynamic matrix
│ │ │ │ +
451template<int Options, int MaxRows, int MaxCols>
│ │ │ │ +
│ │ │ │ +
452struct traits<Eigen::Matrix<double, -1, -1, Options, MaxRows, MaxCols> > :
│ │ │ │ +
453 public internal::DynamicTraits<-1, -1, Options, MaxRows, MaxCols> {
│ │ │ │ +
454};
│ │ │ │ +
│ │ │ │ +
455
│ │ │ │ +
456// traits for dynamic column vector
│ │ │ │ +
457template<int Options, int MaxRows, int MaxCols>
│ │ │ │ +
│ │ │ │ +
458struct traits<Eigen::Matrix<double, -1, 1, Options, MaxRows, MaxCols> > :
│ │ │ │ +
459 public internal::DynamicTraits<-1, 1, Options, MaxRows, MaxCols> {
│ │ │ │ +
460};
│ │ │ │ +
│ │ │ │ +
461
│ │ │ │ +
462// traits for dynamic row vector
│ │ │ │ +
463template<int Options, int MaxRows, int MaxCols>
│ │ │ │ +
│ │ │ │ +
464struct traits<Eigen::Matrix<double, 1, -1, Options, MaxRows, MaxCols> > :
│ │ │ │ +
465 public internal::DynamicTraits<1, -1, Options, MaxRows, MaxCols> {
│ │ │ │ +
466};
│ │ │ │ +
│ │ │ │ +
467
│ │ │ │ +
469template<typename T>
│ │ │ │ +
│ │ │ │ +
470class IsVectorSpace: public IsLieGroup<T> {
│ │ │ │ +
471public:
│ │ │ │ +
472
│ │ │ │ +
473 typedef typename traits<T>::structure_category structure_category_tag;
│ │ │ │ +
474
│ │ │ │ +
475 BOOST_CONCEPT_USAGE(IsVectorSpace) {
│ │ │ │ +
476 BOOST_STATIC_ASSERT_MSG(
│ │ │ │ +
477 (boost::is_base_of<vector_space_tag, structure_category_tag>::value),
│ │ │ │ +
478 "This type's trait does not assert it as a vector space (or derived)");
│ │ │ │ +
479 r = p + q;
│ │ │ │ +
480 r = -p;
│ │ │ │ +
481 r = p - q;
│ │ │ │ +
482 }
│ │ │ │ +
483
│ │ │ │ +
484private:
│ │ │ │ +
485 T p, q, r;
│ │ │ │ +
486};
│ │ │ │ +
│ │ │ │ +
487
│ │ │ │ +
488} // namespace gtsam
│ │ │ │ +
489
│ │ │ │ +
Lie.h
Base class and basic functions for Lie types.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::Value
This is the base class for any type to be stored in Values.
Definition Value.h:37
│ │ │ │ -
gtsam::Value::dim
virtual size_t dim() const =0
Return the dimensionality of the tangent space of this value.
│ │ │ │ -
gtsam::Value::retract_
virtual Value * retract_(const Vector &delta) const =0
Increment the value, by mapping from the vector delta in the tangent space of the current value back ...
│ │ │ │ -
gtsam::Value::deallocate_
virtual void deallocate_() const =0
Deallocate a raw pointer of this value.
│ │ │ │ -
gtsam::Value::~Value
virtual ~Value()
Virutal destructor.
Definition Value.h:89
│ │ │ │ -
gtsam::Value::equals_
virtual bool equals_(const Value &other, double tol=1e-9) const =0
Compare this Value with another for equality.
│ │ │ │ -
gtsam::Value::localCoordinates_
virtual Vector localCoordinates_(const Value &value) const =0
Compute the coordinates in the tangent space of this value that retract() would map to value.
│ │ │ │ -
gtsam::Value::operator=
virtual Value & operator=(const Value &)
Assignment operator.
Definition Value.h:79
│ │ │ │ -
gtsam::Value::print
virtual void print(const std::string &str="") const =0
Print this value, for debugging and unit tests.
│ │ │ │ -
gtsam::Value::clone_
virtual Value * clone_() const =0
Clone this value in a special memory pool, must be deleted with Value::deallocate_,...
│ │ │ │ -
gtsam::Value::clone
virtual boost::shared_ptr< Value > clone() const =0
Clone this value (normal clone on the heap, delete with 'delete' operator)
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition Matrix.h:81
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::additive_group_tag
Definition Group.h:38
│ │ │ │ +
gtsam::lie_group_tag
tag to assert a type is a Lie group
Definition Lie.h:164
│ │ │ │ +
gtsam::IsLieGroup
Lie Group Concept.
Definition Lie.h:260
│ │ │ │ +
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ +
gtsam::OptionalJacobian::cols
OptionalJacobian< Rows, N > cols(int startCol)
Access M*N sub-block if we are allocated, otherwise none TODO(frank): this could work as is below if ...
Definition OptionalJacobian.h:171
│ │ │ │ + │ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::vector_space_tag
tag to assert a type is a vector space
Definition VectorSpace.h:16
│ │ │ │ +
gtsam::internal::VectorSpaceImpl
VectorSpaceTraits Implementation for Fixed sizes.
Definition VectorSpace.h:25
│ │ │ │ +
gtsam::internal::HasVectorSpacePrereqs
Requirements on type to pass it to Manifold template below.
Definition VectorSpace.h:164
│ │ │ │ +
gtsam::internal::VectorSpaceTraits
A helper that implements the traits interface for classes that define vector spaces To use this for y...
Definition VectorSpace.h:185
│ │ │ │ +
gtsam::internal::VectorSpace
VectorSpace provides both Testable and VectorSpaceTraits.
Definition VectorSpace.h:207
│ │ │ │ +
gtsam::internal::ScalarTraits
A helper that implements the traits interface for scalar vector spaces.
Definition VectorSpace.h:212
│ │ │ │ +
gtsam::internal::DynamicTraits
Definition VectorSpace.h:354
│ │ │ │ +
gtsam::IsVectorSpace
Vector Space concept.
Definition VectorSpace.h:470
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,119 +1,503 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Value.h │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -1/* --------------------------------------------------------------------------- │ │ │ │ │ -- │ │ │ │ │ -2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ -4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -5 * All Rights Reserved │ │ │ │ │ -6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ -7 │ │ │ │ │ -8 * See LICENSE for the license information │ │ │ │ │ -9 │ │ │ │ │ -10 * ------------------------------------------------------------------------- │ │ │ │ │ -- */ │ │ │ │ │ -11 │ │ │ │ │ -19#pragma once │ │ │ │ │ +VectorSpace.h │ │ │ │ │ +1/* │ │ │ │ │ +2 * VectorSpace.h │ │ │ │ │ +3 * │ │ │ │ │ +4 * @date December 21, 2014 │ │ │ │ │ +5 * @author Mike Bosse │ │ │ │ │ +6 * @author Frank Dellaert │ │ │ │ │ +7 */ │ │ │ │ │ +8 │ │ │ │ │ +9#pragma once │ │ │ │ │ +10 │ │ │ │ │ +11#include <_g_t_s_a_m_/_b_a_s_e_/_L_i_e_._h> │ │ │ │ │ +12 │ │ │ │ │ +13namespace _g_t_s_a_m { │ │ │ │ │ +14 │ │ │ │ │ +_1_6struct _v_e_c_t_o_r___s_p_a_c_e___t_a_g: public _l_i_e___g_r_o_u_p___t_a_g { │ │ │ │ │ +17}; │ │ │ │ │ +18 │ │ │ │ │ +19template struct _t_r_a_i_t_s; │ │ │ │ │ 20 │ │ │ │ │ -21#include // Configuration from CMake │ │ │ │ │ +21namespace internal { │ │ │ │ │ 22 │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_c_t_o_r_._h> │ │ │ │ │ -24#include │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ -27 │ │ │ │ │ -28namespace _g_t_s_a_m { │ │ │ │ │ -29 │ │ │ │ │ -_3_7 class GTSAM_EXPORT _V_a_l_u_e { │ │ │ │ │ -38 public: │ │ │ │ │ -39 │ │ │ │ │ -_4_1 virtual _V_a_l_u_e* _c_l_o_n_e__() const = 0; │ │ │ │ │ -42 │ │ │ │ │ -_4_4 virtual void _d_e_a_l_l_o_c_a_t_e__() const = 0; │ │ │ │ │ -45 │ │ │ │ │ -_4_7 virtual boost::shared_ptr _c_l_o_n_e() const = 0; │ │ │ │ │ +24template │ │ │ │ │ +_2_5struct _V_e_c_t_o_r_S_p_a_c_e_I_m_p_l { │ │ │ │ │ +26 │ │ │ │ │ +29 typedef Eigen::Matrix TangentVector; │ │ │ │ │ +30 typedef _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_N_,_ _N_> ChartJacobian; │ │ │ │ │ +31 typedef Eigen::Matrix Jacobian; │ │ │ │ │ +32 static int GetDimension(const Class&) { return N;} │ │ │ │ │ +33 │ │ │ │ │ +34 static TangentVector Local(const Class& origin, const Class& other, │ │ │ │ │ +35 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { │ │ │ │ │ +36 if (H1) *H1 = - Jacobian::Identity(); │ │ │ │ │ +37 if (H2) *H2 = Jacobian::Identity(); │ │ │ │ │ +38 Class v = other-origin; │ │ │ │ │ +39 return v.vector(); │ │ │ │ │ +40 } │ │ │ │ │ +41 │ │ │ │ │ +42 static Class Retract(const Class& origin, const TangentVector& v, │ │ │ │ │ +43 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { │ │ │ │ │ +44 if (H1) *H1 = Jacobian::Identity(); │ │ │ │ │ +45 if (H2) *H2 = Jacobian::Identity(); │ │ │ │ │ +46 return origin + v; │ │ │ │ │ +47 } │ │ │ │ │ 48 │ │ │ │ │ -_5_0 virtual bool _e_q_u_a_l_s__(const _V_a_l_u_e& other, double tol = 1e-9) const = 0; │ │ │ │ │ -51 │ │ │ │ │ -_5_3 virtual void _p_r_i_n_t(const std::string& str = "") const = 0; │ │ │ │ │ -54 │ │ │ │ │ -_6_0 virtual size_t _d_i_m() const = 0; │ │ │ │ │ -61 │ │ │ │ │ -_6_8 virtual _V_a_l_u_e* _r_e_t_r_a_c_t__(const Vector& delta) const = 0; │ │ │ │ │ -69 │ │ │ │ │ -_7_6 virtual Vector _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s__(const _V_a_l_u_e& value) const = 0; │ │ │ │ │ +50 │ │ │ │ │ +53 │ │ │ │ │ +54 static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) │ │ │ │ │ +{ │ │ │ │ │ +55 if (Hm) *Hm = Jacobian::Identity(); │ │ │ │ │ +56 return m.vector(); │ │ │ │ │ +57 } │ │ │ │ │ +58 │ │ │ │ │ +59 static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) │ │ │ │ │ +{ │ │ │ │ │ +60 if (Hv) *Hv = Jacobian::Identity(); │ │ │ │ │ +61 return Class(v); │ │ │ │ │ +62 } │ │ │ │ │ +63 │ │ │ │ │ +64 static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1 = │ │ │ │ │ +boost::none, │ │ │ │ │ +65 ChartJacobian H2 = boost::none) { │ │ │ │ │ +66 if (H1) *H1 = Jacobian::Identity(); │ │ │ │ │ +67 if (H2) *H2 = Jacobian::Identity(); │ │ │ │ │ +68 return v1 + v2; │ │ │ │ │ +69 } │ │ │ │ │ +70 │ │ │ │ │ +71 static Class Between(const Class& v1, const Class& v2, ChartJacobian H1 = │ │ │ │ │ +boost::none, │ │ │ │ │ +72 ChartJacobian H2 = boost::none) { │ │ │ │ │ +73 if (H1) *H1 = - Jacobian::Identity(); │ │ │ │ │ +74 if (H2) *H2 = Jacobian::Identity(); │ │ │ │ │ +75 return v2 - v1; │ │ │ │ │ +76 } │ │ │ │ │ 77 │ │ │ │ │ -_7_9 virtual _V_a_l_u_e& _o_p_e_r_a_t_o_r_=(const _V_a_l_u_e& /*rhs*/) { │ │ │ │ │ -80 //needs a empty definition so recursion in implicit derived assignment │ │ │ │ │ -operators work │ │ │ │ │ -81 return *this; │ │ │ │ │ -82 } │ │ │ │ │ -83 │ │ │ │ │ -85 template │ │ │ │ │ -86 const ValueType& cast() const; │ │ │ │ │ -87 │ │ │ │ │ -_8_9 virtual _~_V_a_l_u_e() {} │ │ │ │ │ -90 │ │ │ │ │ -91 private: │ │ │ │ │ -_1_2_2 friend class boost::serialization::access; │ │ │ │ │ -123 template │ │ │ │ │ -124 void serialize(ARCHIVE & /*ar*/, const unsigned int /*version*/) { │ │ │ │ │ -125 } │ │ │ │ │ -126 │ │ │ │ │ -127 }; │ │ │ │ │ -128 │ │ │ │ │ -129} /* namespace gtsam */ │ │ │ │ │ -130 │ │ │ │ │ -131BOOST_SERIALIZATION_ASSUME_ABSTRACT(_g_t_s_a_m_:_:_V_a_l_u_e) │ │ │ │ │ -_V_e_c_t_o_r_._h │ │ │ │ │ -typedef and functions to augment Eigen's VectorXd │ │ │ │ │ +78 static Class Inverse(const Class& v, ChartJacobian H = boost::none) { │ │ │ │ │ +79 if (H) *H = - Jacobian::Identity(); │ │ │ │ │ +80 return -v; │ │ │ │ │ +81 } │ │ │ │ │ +82 │ │ │ │ │ +84}; │ │ │ │ │ +85 │ │ │ │ │ +87template │ │ │ │ │ +_8_8struct _V_e_c_t_o_r_S_p_a_c_e_I_m_p_l { │ │ │ │ │ +89 │ │ │ │ │ +92 static Class Compose(const Class& v1, const Class& v2) { return v1+v2;} │ │ │ │ │ +93 static Class Between(const Class& v1, const Class& v2) { return v2-v1;} │ │ │ │ │ +94 static Class Inverse(const Class& m) { return -m;} │ │ │ │ │ +96 │ │ │ │ │ +99 typedef Eigen::VectorXd TangentVector; │ │ │ │ │ +100 typedef _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_E_i_g_e_n_:_:_D_y_n_a_m_i_c_,_E_i_g_e_n_:_:_D_y_n_a_m_i_c_> _C_h_a_r_t_J_a_c_o_b_i_a_n; │ │ │ │ │ +101 static int GetDimension(const Class& m) { return m.dim();} │ │ │ │ │ +102 │ │ │ │ │ +103 static Eigen::MatrixXd Eye(const Class& m) { │ │ │ │ │ +104 int dim = GetDimension(m); │ │ │ │ │ +105 return Eigen::MatrixXd::Identity(dim, dim); │ │ │ │ │ +106 } │ │ │ │ │ +107 │ │ │ │ │ +108 static TangentVector Local(const Class& origin, const Class& other, │ │ │ │ │ +109 _C_h_a_r_t_J_a_c_o_b_i_a_n H1 = boost::none, _C_h_a_r_t_J_a_c_o_b_i_a_n H2 = boost::none) { │ │ │ │ │ +110 if (H1) *H1 = - Eye(origin); │ │ │ │ │ +111 if (H2) *H2 = Eye(other); │ │ │ │ │ +112 Class v = other-origin; │ │ │ │ │ +113 return v.vector(); │ │ │ │ │ +114 } │ │ │ │ │ +115 │ │ │ │ │ +116 static Class Retract(const Class& origin, const TangentVector& v, │ │ │ │ │ +117 _C_h_a_r_t_J_a_c_o_b_i_a_n H1 = boost::none, _C_h_a_r_t_J_a_c_o_b_i_a_n H2 = boost::none) { │ │ │ │ │ +118 if (H1) *H1 = Eye(origin); │ │ │ │ │ +119 if (H2) *H2 = Eye(origin); │ │ │ │ │ +120 return origin + v; │ │ │ │ │ +121 } │ │ │ │ │ +122 │ │ │ │ │ +124 │ │ │ │ │ +127 │ │ │ │ │ +128 static TangentVector Logmap(const Class& m, _C_h_a_r_t_J_a_c_o_b_i_a_n Hm = boost::none) │ │ │ │ │ +{ │ │ │ │ │ +129 if (Hm) *Hm = Eye(m); │ │ │ │ │ +130 return m.vector(); │ │ │ │ │ +131 } │ │ │ │ │ +132 │ │ │ │ │ +133 static Class Expmap(const TangentVector& v, _C_h_a_r_t_J_a_c_o_b_i_a_n Hv = boost::none) │ │ │ │ │ +{ │ │ │ │ │ +134 Class result(v); │ │ │ │ │ +135 if (Hv) │ │ │ │ │ +136 *Hv = Eye(v); │ │ │ │ │ +137 return result; │ │ │ │ │ +138 } │ │ │ │ │ +139 │ │ │ │ │ +140 static Class Compose(const Class& v1, const Class& v2, _C_h_a_r_t_J_a_c_o_b_i_a_n H1, │ │ │ │ │ +141 _C_h_a_r_t_J_a_c_o_b_i_a_n H2 = boost::none) { │ │ │ │ │ +142 if (H1) *H1 = Eye(v1); │ │ │ │ │ +143 if (H2) *H2 = Eye(v2); │ │ │ │ │ +144 return v1 + v2; │ │ │ │ │ +145 } │ │ │ │ │ +146 │ │ │ │ │ +147 static Class Between(const Class& v1, const Class& v2, _C_h_a_r_t_J_a_c_o_b_i_a_n H1, │ │ │ │ │ +148 _C_h_a_r_t_J_a_c_o_b_i_a_n H2 = boost::none) { │ │ │ │ │ +149 if (H1) *H1 = - Eye(v1); │ │ │ │ │ +150 if (H2) *H2 = Eye(v2); │ │ │ │ │ +151 return v2 - v1; │ │ │ │ │ +152 } │ │ │ │ │ +153 │ │ │ │ │ +154 static Class Inverse(const Class& v, _C_h_a_r_t_J_a_c_o_b_i_a_n H) { │ │ │ │ │ +155 if (H) *H = -Eye(v); │ │ │ │ │ +156 return -v; │ │ │ │ │ +157 } │ │ │ │ │ +158 │ │ │ │ │ +160}; │ │ │ │ │ +161 │ │ │ │ │ +163template │ │ │ │ │ +_1_6_4struct _H_a_s_V_e_c_t_o_r_S_p_a_c_e_P_r_e_r_e_q_s { │ │ │ │ │ +165 │ │ │ │ │ +166 enum { dim = Class::dimension }; │ │ │ │ │ +167 │ │ │ │ │ +168 Class p, q; │ │ │ │ │ +169 Vector v; │ │ │ │ │ +170 │ │ │ │ │ +171 BOOST_CONCEPT_USAGE(_H_a_s_V_e_c_t_o_r_S_p_a_c_e_P_r_e_r_e_q_s) { │ │ │ │ │ +172 p = Class::Identity(); // identity │ │ │ │ │ +173 q = p + p; // addition │ │ │ │ │ +174 q = p - p; // subtraction │ │ │ │ │ +175 v = p.vector(); // conversion to vector │ │ │ │ │ +176 q = p + v; // addition of a vector on the right │ │ │ │ │ +177 } │ │ │ │ │ +178}; │ │ │ │ │ +179 │ │ │ │ │ +184template │ │ │ │ │ +_1_8_5struct _V_e_c_t_o_r_S_p_a_c_e_T_r_a_i_t_s: _V_e_c_t_o_r_S_p_a_c_e_I_m_p_l { │ │ │ │ │ +186 │ │ │ │ │ +187 // Check that Class has the necessary machinery │ │ │ │ │ +188 BOOST_CONCEPT_ASSERT((_H_a_s_V_e_c_t_o_r_S_p_a_c_e_P_r_e_r_e_q_s_<_C_l_a_s_s_>)); │ │ │ │ │ +189 │ │ │ │ │ +190 typedef _v_e_c_t_o_r___s_p_a_c_e___t_a_g _s_t_r_u_c_t_u_r_e___c_a_t_e_g_o_r_y; │ │ │ │ │ +191 │ │ │ │ │ +194 typedef _a_d_d_i_t_i_v_e___g_r_o_u_p___t_a_g _g_r_o_u_p___f_l_a_v_o_r; │ │ │ │ │ +195 static Class Identity() { return Class::Identity();} │ │ │ │ │ +197 │ │ │ │ │ +200 enum { dimension = Class::dimension}; │ │ │ │ │ +201 typedef Class ManifoldType; │ │ │ │ │ +203}; │ │ │ │ │ +204 │ │ │ │ │ +206template │ │ │ │ │ +_2_0_7struct _V_e_c_t_o_r_S_p_a_c_e: _T_e_s_t_a_b_l_e, _V_e_c_t_o_r_S_p_a_c_e_T_r_a_i_t_s {}; │ │ │ │ │ +208 │ │ │ │ │ +211template │ │ │ │ │ +_2_1_2struct _S_c_a_l_a_r_T_r_a_i_t_s : _V_e_c_t_o_r_S_p_a_c_e_I_m_p_l { │ │ │ │ │ +213 │ │ │ │ │ +214 typedef _v_e_c_t_o_r___s_p_a_c_e___t_a_g _s_t_r_u_c_t_u_r_e___c_a_t_e_g_o_r_y; │ │ │ │ │ +215 │ │ │ │ │ +218 static void Print(Scalar m, const std::string& str = "") { │ │ │ │ │ +219 _g_t_s_a_m_:_:_p_r_i_n_t(m, str); │ │ │ │ │ +220 } │ │ │ │ │ +221 static bool Equals(Scalar v1, Scalar v2, double tol = 1e-8) { │ │ │ │ │ +222 return std::abs(v1 - v2) < tol; │ │ │ │ │ +223 } │ │ │ │ │ +225 │ │ │ │ │ +228 typedef _a_d_d_i_t_i_v_e___g_r_o_u_p___t_a_g _g_r_o_u_p___f_l_a_v_o_r; │ │ │ │ │ +229 static Scalar Identity() { return 0;} │ │ │ │ │ +231 │ │ │ │ │ +234 typedef Scalar ManifoldType; │ │ │ │ │ +235 enum { dimension = 1 }; │ │ │ │ │ +236 typedef Eigen::Matrix TangentVector; │ │ │ │ │ +237 typedef _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _1_> ChartJacobian; │ │ │ │ │ +238 │ │ │ │ │ +239 static TangentVector Local(Scalar origin, Scalar other, │ │ │ │ │ +240 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { │ │ │ │ │ +241 if (H1) (*H1)[0] = -1.0; │ │ │ │ │ +242 if (H2) (*H2)[0] = 1.0; │ │ │ │ │ +243 TangentVector result; │ │ │ │ │ +244 result(0) = other - origin; │ │ │ │ │ +245 return result; │ │ │ │ │ +246 } │ │ │ │ │ +247 │ │ │ │ │ +248 static Scalar Retract(Scalar origin, const TangentVector& v, │ │ │ │ │ +249 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { │ │ │ │ │ +250 if (H1) (*H1)[0] = 1.0; │ │ │ │ │ +251 if (H2) (*H2)[0] = 1.0; │ │ │ │ │ +252 return origin + v[0]; │ │ │ │ │ +253 } │ │ │ │ │ +255 │ │ │ │ │ +258 static TangentVector Logmap(Scalar m, ChartJacobian H = boost::none) { │ │ │ │ │ +259 if (H) (*H)[0] = 1.0; │ │ │ │ │ +260 return Local(0, m); │ │ │ │ │ +261 } │ │ │ │ │ +262 │ │ │ │ │ +263 static Scalar Expmap(const TangentVector& v, ChartJacobian H = boost::none) │ │ │ │ │ +{ │ │ │ │ │ +264 if (H) (*H)[0] = 1.0; │ │ │ │ │ +265 return v[0]; │ │ │ │ │ +266 } │ │ │ │ │ +268 │ │ │ │ │ +269}; │ │ │ │ │ +270 │ │ │ │ │ +271} // namespace internal │ │ │ │ │ +272 │ │ │ │ │ +_2_7_4template<> struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_S_c_a_l_a_r_T_r_a_i_t_s { │ │ │ │ │ +275}; │ │ │ │ │ +276 │ │ │ │ │ +_2_7_8template<> struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_S_c_a_l_a_r_T_r_a_i_t_s { │ │ │ │ │ +279}; │ │ │ │ │ +280 │ │ │ │ │ +281// traits for any fixed double Eigen matrix │ │ │ │ │ +282template │ │ │ │ │ +_2_8_3struct _t_r_a_i_t_s > : │ │ │ │ │ +284 _i_n_t_e_r_n_a_l_:_:_V_e_c_t_o_r_S_p_a_c_e_I_m_p_l< │ │ │ │ │ +285 Eigen::Matrix, M * N> { │ │ │ │ │ +286 │ │ │ │ │ +287 typedef _v_e_c_t_o_r___s_p_a_c_e___t_a_g _s_t_r_u_c_t_u_r_e___c_a_t_e_g_o_r_y; │ │ │ │ │ +288 typedef Eigen::Matrix Fixed; │ │ │ │ │ +289 │ │ │ │ │ +292 static void Print(const Fixed& m, const std::string& str = "") { │ │ │ │ │ +293 _g_t_s_a_m_:_:_p_r_i_n_t(Eigen::MatrixXd(m), str); │ │ │ │ │ +294 } │ │ │ │ │ +295 static bool Equals(const Fixed& v1, const Fixed& v2, double tol = 1e-8) { │ │ │ │ │ +296 return _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(v1, v2, tol); │ │ │ │ │ +297 } │ │ │ │ │ +299 │ │ │ │ │ +302 typedef _a_d_d_i_t_i_v_e___g_r_o_u_p___t_a_g _g_r_o_u_p___f_l_a_v_o_r; │ │ │ │ │ +303 static Fixed Identity() { return Fixed::Zero();} │ │ │ │ │ +305 │ │ │ │ │ +308 enum { dimension = M*N}; │ │ │ │ │ +309 typedef Fixed ManifoldType; │ │ │ │ │ +310 typedef Eigen::Matrix TangentVector; │ │ │ │ │ +311 typedef Eigen::Matrix Jacobian; │ │ │ │ │ +312 typedef _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_d_i_m_e_n_s_i_o_n_,_ _d_i_m_e_n_s_i_o_n_> _C_h_a_r_t_J_a_c_o_b_i_a_n; │ │ │ │ │ +313 │ │ │ │ │ +314 static TangentVector Local(const Fixed& origin, const Fixed& other, │ │ │ │ │ +315 _C_h_a_r_t_J_a_c_o_b_i_a_n H1 = boost::none, _C_h_a_r_t_J_a_c_o_b_i_a_n H2 = boost::none) { │ │ │ │ │ +316 if (H1) (*H1) = -Jacobian::Identity(); │ │ │ │ │ +317 if (H2) (*H2) = Jacobian::Identity(); │ │ │ │ │ +318 TangentVector result; │ │ │ │ │ +319 Eigen::Map(result.data()) = other - origin; │ │ │ │ │ +320 return result; │ │ │ │ │ +321 } │ │ │ │ │ +322 │ │ │ │ │ +323 static Fixed Retract(const Fixed& origin, const TangentVector& v, │ │ │ │ │ +324 _C_h_a_r_t_J_a_c_o_b_i_a_n H1 = boost::none, _C_h_a_r_t_J_a_c_o_b_i_a_n H2 = boost::none) { │ │ │ │ │ +325 if (H1) (*H1) = Jacobian::Identity(); │ │ │ │ │ +326 if (H2) (*H2) = Jacobian::Identity(); │ │ │ │ │ +327 return origin + Eigen::Map(v.data()); │ │ │ │ │ +328 } │ │ │ │ │ +330 │ │ │ │ │ +333 static TangentVector Logmap(const Fixed& m, _C_h_a_r_t_J_a_c_o_b_i_a_n H = boost::none) │ │ │ │ │ +{ │ │ │ │ │ +334 if (H) *H = Jacobian::Identity(); │ │ │ │ │ +335 TangentVector result; │ │ │ │ │ +336 Eigen::Map(result.data()) = m; │ │ │ │ │ +337 return result; │ │ │ │ │ +338 } │ │ │ │ │ +339 │ │ │ │ │ +340 static Fixed Expmap(const TangentVector& v, _C_h_a_r_t_J_a_c_o_b_i_a_n H = boost::none) │ │ │ │ │ +{ │ │ │ │ │ +341 Fixed m; │ │ │ │ │ +342 m.setZero(); │ │ │ │ │ +343 if (H) *H = Jacobian::Identity(); │ │ │ │ │ +344 return m + Eigen::Map(v.data()); │ │ │ │ │ +345 } │ │ │ │ │ +347}; │ │ │ │ │ +348 │ │ │ │ │ +349 │ │ │ │ │ +350namespace internal { │ │ │ │ │ +351 │ │ │ │ │ +352// traits for dynamic Eigen matrices │ │ │ │ │ +353template │ │ │ │ │ +_3_5_4struct _D_y_n_a_m_i_c_T_r_a_i_t_s { │ │ │ │ │ +355 │ │ │ │ │ +356 typedef _v_e_c_t_o_r___s_p_a_c_e___t_a_g _s_t_r_u_c_t_u_r_e___c_a_t_e_g_o_r_y; │ │ │ │ │ +357 typedef Eigen::Matrix Dynamic; │ │ │ │ │ +358 │ │ │ │ │ +361 static void Print(const Dynamic& m, const std::string& str = "") { │ │ │ │ │ +362 _g_t_s_a_m_:_:_p_r_i_n_t(Eigen::MatrixXd(m), str); │ │ │ │ │ +363 } │ │ │ │ │ +364 static bool Equals(const Dynamic& v1, const Dynamic& v2, │ │ │ │ │ +365 double tol = 1e-8) { │ │ │ │ │ +366 return _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(v1, v2, tol); │ │ │ │ │ +367 } │ │ │ │ │ +369 │ │ │ │ │ +372 typedef _a_d_d_i_t_i_v_e___g_r_o_u_p___t_a_g _g_r_o_u_p___f_l_a_v_o_r; │ │ │ │ │ +373 static Dynamic Identity() { │ │ │ │ │ +374 throw std::runtime_error("Identity not defined for dynamic types"); │ │ │ │ │ +375 } │ │ │ │ │ +377 │ │ │ │ │ +380 enum { dimension = Eigen::Dynamic }; │ │ │ │ │ +381 typedef Eigen::VectorXd TangentVector; │ │ │ │ │ +382 typedef Eigen::MatrixXd Jacobian; │ │ │ │ │ +383 typedef _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_d_i_m_e_n_s_i_o_n_,_ _d_i_m_e_n_s_i_o_n_> ChartJacobian; │ │ │ │ │ +384 typedef Dynamic ManifoldType; │ │ │ │ │ +385 │ │ │ │ │ +386 static int GetDimension(const Dynamic& m) { │ │ │ │ │ +387 return m.rows() * m._c_o_l_s(); │ │ │ │ │ +388 } │ │ │ │ │ +389 │ │ │ │ │ +390 static Jacobian Eye(const Dynamic& m) { │ │ │ │ │ +391 int dim = GetDimension(m); │ │ │ │ │ +392 return Eigen::Matrix::Identity(dim, dim); │ │ │ │ │ +393 } │ │ │ │ │ +394 │ │ │ │ │ +395 static TangentVector Local(const Dynamic& m, const Dynamic& other, // │ │ │ │ │ +396 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { │ │ │ │ │ +397 if (H1) *H1 = -Eye(m); │ │ │ │ │ +398 if (H2) *H2 = Eye(m); │ │ │ │ │ +399 TangentVector v(GetDimension(m)); │ │ │ │ │ +400 Eigen::Map(v.data(), m.rows(), m.cols()) = other - m; │ │ │ │ │ +401 return v; │ │ │ │ │ +402 } │ │ │ │ │ +403 │ │ │ │ │ +404 static Dynamic Retract(const Dynamic& m, const TangentVector& v, // │ │ │ │ │ +405 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { │ │ │ │ │ +406 if (H1) *H1 = Eye(m); │ │ │ │ │ +407 if (H2) *H2 = Eye(m); │ │ │ │ │ +408 return m + Eigen::Map(v.data(), m.rows(), m.cols()); │ │ │ │ │ +409 } │ │ │ │ │ +411 │ │ │ │ │ +414 static TangentVector Logmap(const Dynamic& m, ChartJacobian H = boost:: │ │ │ │ │ +none) { │ │ │ │ │ +415 if (H) *H = Eye(m); │ │ │ │ │ +416 TangentVector result(GetDimension(m)); │ │ │ │ │ +417 Eigen::Map(result.data(), m.cols(), m.rows()) = m; │ │ │ │ │ +418 return result; │ │ │ │ │ +419 } │ │ │ │ │ +420 │ │ │ │ │ +421 static Dynamic Expmap(const TangentVector& /*v*/, ChartJacobian H = boost:: │ │ │ │ │ +none) { │ │ │ │ │ +422 static_cast(H); │ │ │ │ │ +423 throw std::runtime_error("Expmap not defined for dynamic types"); │ │ │ │ │ +424 } │ │ │ │ │ +425 │ │ │ │ │ +426 static Dynamic Inverse(const Dynamic& m, ChartJacobian H = boost::none) { │ │ │ │ │ +427 if (H) *H = -Eye(m); │ │ │ │ │ +428 return -m; │ │ │ │ │ +429 } │ │ │ │ │ +430 │ │ │ │ │ +431 static Dynamic Compose(const Dynamic& v1, const Dynamic& v2, │ │ │ │ │ +432 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { │ │ │ │ │ +433 if (H1) *H1 = Eye(v1); │ │ │ │ │ +434 if (H2) *H2 = Eye(v1); │ │ │ │ │ +435 return v1 + v2; │ │ │ │ │ +436 } │ │ │ │ │ +437 │ │ │ │ │ +438 static Dynamic Between(const Dynamic& v1, const Dynamic& v2, │ │ │ │ │ +439 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { │ │ │ │ │ +440 if (H1) *H1 = -Eye(v1); │ │ │ │ │ +441 if (H2) *H2 = Eye(v1); │ │ │ │ │ +442 return v2 - v1; │ │ │ │ │ +443 } │ │ │ │ │ +445 │ │ │ │ │ +446}; │ │ │ │ │ +447 │ │ │ │ │ +448} // \ internal │ │ │ │ │ +449 │ │ │ │ │ +450// traits for fully dynamic matrix │ │ │ │ │ +451template │ │ │ │ │ +_4_5_2struct _t_r_a_i_t_s > : │ │ │ │ │ +453 public _i_n_t_e_r_n_a_l_:_:_D_y_n_a_m_i_c_T_r_a_i_t_s<-1, -1, Options, MaxRows, MaxCols> { │ │ │ │ │ +454}; │ │ │ │ │ +455 │ │ │ │ │ +456// traits for dynamic column vector │ │ │ │ │ +457template │ │ │ │ │ +_4_5_8struct _t_r_a_i_t_s > : │ │ │ │ │ +459 public _i_n_t_e_r_n_a_l_:_:_D_y_n_a_m_i_c_T_r_a_i_t_s<-1, 1, Options, MaxRows, MaxCols> { │ │ │ │ │ +460}; │ │ │ │ │ +461 │ │ │ │ │ +462// traits for dynamic row vector │ │ │ │ │ +463template │ │ │ │ │ +_4_6_4struct _t_r_a_i_t_s > : │ │ │ │ │ +465 public _i_n_t_e_r_n_a_l_:_:_D_y_n_a_m_i_c_T_r_a_i_t_s<1, -1, Options, MaxRows, MaxCols> { │ │ │ │ │ +466}; │ │ │ │ │ +467 │ │ │ │ │ +469template │ │ │ │ │ +_4_7_0class _I_s_V_e_c_t_o_r_S_p_a_c_e: public _I_s_L_i_e_G_r_o_u_p { │ │ │ │ │ +471public: │ │ │ │ │ +472 │ │ │ │ │ +473 typedef typename _t_r_a_i_t_s_<_T_>_:_:_s_t_r_u_c_t_u_r_e___c_a_t_e_g_o_r_y structure_category_tag; │ │ │ │ │ +474 │ │ │ │ │ +475 BOOST_CONCEPT_USAGE(_I_s_V_e_c_t_o_r_S_p_a_c_e) { │ │ │ │ │ +476 BOOST_STATIC_ASSERT_MSG( │ │ │ │ │ +477 (boost::is_base_of::value), │ │ │ │ │ +478 "This type's trait does not assert it as a vector space (or derived)"); │ │ │ │ │ +479 r = p + q; │ │ │ │ │ +480 r = -p; │ │ │ │ │ +481 r = p - q; │ │ │ │ │ +482 } │ │ │ │ │ +483 │ │ │ │ │ +484private: │ │ │ │ │ +485 T p, q, r; │ │ │ │ │ +486}; │ │ │ │ │ +487 │ │ │ │ │ +488} // namespace gtsam │ │ │ │ │ +489 │ │ │ │ │ +_L_i_e_._h │ │ │ │ │ +Base class and basic functions for Lie types. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e │ │ │ │ │ -This is the base class for any type to be stored in Values. │ │ │ │ │ -DDeeffiinniittiioonn Value.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_:_:_d_i_m │ │ │ │ │ -virtual size_t dim() const =0 │ │ │ │ │ -Return the dimensionality of the tangent space of this value. │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_:_:_r_e_t_r_a_c_t__ │ │ │ │ │ -virtual Value * retract_(const Vector &delta) const =0 │ │ │ │ │ -Increment the value, by mapping from the vector delta in the tangent space of │ │ │ │ │ -the current value back ... │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_:_:_d_e_a_l_l_o_c_a_t_e__ │ │ │ │ │ -virtual void deallocate_() const =0 │ │ │ │ │ -Deallocate a raw pointer of this value. │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_:_:_~_V_a_l_u_e │ │ │ │ │ -virtual ~Value() │ │ │ │ │ -Virutal destructor. │ │ │ │ │ -DDeeffiinniittiioonn Value.h:89 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_:_:_e_q_u_a_l_s__ │ │ │ │ │ -virtual bool equals_(const Value &other, double tol=1e-9) const =0 │ │ │ │ │ -Compare this Value with another for equality. │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s__ │ │ │ │ │ -virtual Vector localCoordinates_(const Value &value) const =0 │ │ │ │ │ -Compute the coordinates in the tangent space of this value that retract() would │ │ │ │ │ -map to value. │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ -virtual Value & operator=(const Value &) │ │ │ │ │ -Assignment operator. │ │ │ │ │ -DDeeffiinniittiioonn Value.h:79 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_:_:_p_r_i_n_t │ │ │ │ │ -virtual void print(const std::string &str="") const =0 │ │ │ │ │ -Print this value, for debugging and unit tests. │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_:_:_c_l_o_n_e__ │ │ │ │ │ -virtual Value * clone_() const =0 │ │ │ │ │ -Clone this value in a special memory pool, must be deleted with Value:: │ │ │ │ │ -deallocate_,... │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_:_:_c_l_o_n_e │ │ │ │ │ -virtual boost::shared_ptr< Value > clone() const =0 │ │ │ │ │ -Clone this value (normal clone on the heap, delete with 'delete' operator) │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l___w_i_t_h___a_b_s___t_o_l │ │ │ │ │ +bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen:: │ │ │ │ │ +DenseBase< MATRIX > &B, double tol=1e-9) │ │ │ │ │ +equals with a tolerance │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:81 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_a_d_d_i_t_i_v_e___g_r_o_u_p___t_a_g │ │ │ │ │ +DDeeffiinniittiioonn Group.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_l_i_e___g_r_o_u_p___t_a_g │ │ │ │ │ +tag to assert a type is a Lie group │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:164 │ │ │ │ │ +_g_t_s_a_m_:_:_I_s_L_i_e_G_r_o_u_p │ │ │ │ │ +Lie Group Concept. │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:260 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ +either a fixed size o... │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_:_:_c_o_l_s │ │ │ │ │ +OptionalJacobian< Rows, N > cols(int startCol) │ │ │ │ │ +Access M*N sub-block if we are allocated, otherwise none TODO(frank): this │ │ │ │ │ +could work as is below if ... │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:171 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_,_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_ _> │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:188 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_v_e_c_t_o_r___s_p_a_c_e___t_a_g │ │ │ │ │ +tag to assert a type is a vector space │ │ │ │ │ +DDeeffiinniittiioonn VectorSpace.h:16 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_V_e_c_t_o_r_S_p_a_c_e_I_m_p_l │ │ │ │ │ +VectorSpaceTraits Implementation for Fixed sizes. │ │ │ │ │ +DDeeffiinniittiioonn VectorSpace.h:25 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_H_a_s_V_e_c_t_o_r_S_p_a_c_e_P_r_e_r_e_q_s │ │ │ │ │ +Requirements on type to pass it to Manifold template below. │ │ │ │ │ +DDeeffiinniittiioonn VectorSpace.h:164 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_V_e_c_t_o_r_S_p_a_c_e_T_r_a_i_t_s │ │ │ │ │ +A helper that implements the traits interface for classes that define vector │ │ │ │ │ +spaces To use this for y... │ │ │ │ │ +DDeeffiinniittiioonn VectorSpace.h:185 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_V_e_c_t_o_r_S_p_a_c_e │ │ │ │ │ +VectorSpace provides both Testable and VectorSpaceTraits. │ │ │ │ │ +DDeeffiinniittiioonn VectorSpace.h:207 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_S_c_a_l_a_r_T_r_a_i_t_s │ │ │ │ │ +A helper that implements the traits interface for scalar vector spaces. │ │ │ │ │ +DDeeffiinniittiioonn VectorSpace.h:212 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_D_y_n_a_m_i_c_T_r_a_i_t_s │ │ │ │ │ +DDeeffiinniittiioonn VectorSpace.h:354 │ │ │ │ │ +_g_t_s_a_m_:_:_I_s_V_e_c_t_o_r_S_p_a_c_e │ │ │ │ │ +Vector Space concept. │ │ │ │ │ +DDeeffiinniittiioonn VectorSpace.h:470 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _V_a_l_u_e_._h │ │ │ │ │ + * VVeeccttoorrSSppaaccee..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00044.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/treeTraversal-inst.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/FastSet.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,61 +94,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
treeTraversal-inst.h File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
FastSet.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ +

A thin wrapper around std::set that uses boost's fast_pool_allocator. │ │ │ │ +More...

│ │ │ │ + │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::FastSet< VALUE >
 FastSet is a thin wrapper around std::set that uses the boost fast_pool_allocator instead of the default STL allocator. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
namespace  gtsam::treeTraversal
 Internal functions used for traversing trees.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

template<class FOREST , typename DATA , typename VISITOR_PRE , typename VISITOR_POST >
void gtsam::treeTraversal::DepthFirstForest (FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, VISITOR_POST &visitorPost)
 Traverse a forest depth-first with pre-order and post-order visits.
 
template<class FOREST , typename DATA , typename VISITOR_PRE >
void gtsam::treeTraversal::DepthFirstForest (FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre)
 Traverse a forest depth-first, with a pre-order visit but no post-order visit.
 
template<class FOREST , typename DATA , typename VISITOR_PRE , typename VISITOR_POST >
void gtsam::treeTraversal::DepthFirstForestParallel (FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, VISITOR_POST &visitorPost, int problemSizeThreshold=10)
 Traverse a forest depth-first with pre-order and post-order visits.
 
template<class FOREST >
FastVector< boost::shared_ptr< typename FOREST::Node > > gtsam::treeTraversal::CloneForest (const FOREST &forest)
 Clone a tree, copy-constructing new nodes (calling boost::make_shared) and setting up child pointers for a clone of the original tree.
 
template<class FOREST >
void gtsam::treeTraversal::PrintForest (const FOREST &forest, std::string str, const KeyFormatter &keyFormatter)
 Print a tree, prefixing each line with str, and formatting keys using keyFormatter.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
April 9, 2013
│ │ │ │ +

A thin wrapper around std::set that uses boost's fast_pool_allocator.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Oct 17, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,68 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -treeTraversal-inst.h File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +FastSet.h File Reference │ │ │ │ │ +A thin wrapper around std::set that uses boost's fast_pool_allocator. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _V_A_L_U_E_ _> │ │ │ │ │ +  _F_a_s_t_S_e_t is a thin wrapper around std::set that uses the boost │ │ │ │ │ + fast_pool_allocator instead of the default STL allocator. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -namespace   _g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l │ │ │ │ │ -  Internal functions used for traversing trees. │ │ │ │ │ -  │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ - void  _g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t │ │ │ │ │ - (FOREST &forest, DATA &rootData, │ │ │ │ │ - VISITOR_PRE &visitorPre, VISITOR_POST │ │ │ │ │ - &visitorPost) │ │ │ │ │ -  Traverse a forest depth-first with pre- │ │ │ │ │ - order and post-order visits. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - void  _g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t │ │ │ │ │ - (FOREST &forest, DATA &rootData, │ │ │ │ │ - VISITOR_PRE &visitorPre) │ │ │ │ │ - Traverse a forest depth-first, with a │ │ │ │ │ -  pre-order visit but no post-order │ │ │ │ │ - visit. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - void  _g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_: │ │ │ │ │ - _D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t_P_a_r_a_l_l_e_l (FOREST │ │ │ │ │ - &forest, DATA &rootData, VISITOR_PRE │ │ │ │ │ - &visitorPre, VISITOR_POST &visitorPost, │ │ │ │ │ - int problemSizeThreshold=10) │ │ │ │ │ -  Traverse a forest depth-first with pre- │ │ │ │ │ - order and post-order visits. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -_F_a_s_t_V_e_c_t_o_r< boost::shared_ptr< typename _g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_C_l_o_n_e_F_o_r_e_s_t │ │ │ │ │ - FOREST::Node > >  (const FOREST &forest) │ │ │ │ │ - Clone a tree, copy-constructing new │ │ │ │ │ -  nodes (calling boost::make_shared) and │ │ │ │ │ - setting up child pointers for a clone │ │ │ │ │ - of the original tree. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - void  _g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_P_r_i_n_t_F_o_r_e_s_t │ │ │ │ │ - (const FOREST &forest, std::string str, │ │ │ │ │ - const _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter) │ │ │ │ │ - Print a tree, prefixing each line with │ │ │ │ │ -  str, and formatting keys using │ │ │ │ │ - keyFormatter. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +A thin wrapper around std::set that uses boost's fast_pool_allocator. │ │ │ │ │ Author │ │ │ │ │ Richard Roberts │ │ │ │ │ Date │ │ │ │ │ - April 9, 2013 │ │ │ │ │ + Oct 17, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _t_r_e_e_T_r_a_v_e_r_s_a_l_-_i_n_s_t_._h │ │ │ │ │ + * _F_a_s_t_S_e_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00044_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/treeTraversal-inst.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/FastSet.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,217 +98,162 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
treeTraversal-inst.h
│ │ │ │ +
FastSet.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
17#pragma once
│ │ │ │ -
18
│ │ │ │ -
19#include <gtsam/base/treeTraversal/parallelTraversalTasks.h>
│ │ │ │ -
20#include <gtsam/base/treeTraversal/statistics.h>
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/base/FastList.h>
│ │ │ │ -
23#include <gtsam/base/FastVector.h>
│ │ │ │ -
24#include <gtsam/inference/Key.h>
│ │ │ │ -
25#include <gtsam/config.h> // for GTSAM_USE_TBB
│ │ │ │ -
26
│ │ │ │ -
27#include <stack>
│ │ │ │ -
28#include <vector>
│ │ │ │ -
29#include <string>
│ │ │ │ -
30#include <boost/shared_ptr.hpp>
│ │ │ │ -
31#include <boost/make_shared.hpp>
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <boost/version.hpp>
│ │ │ │ +
22#if BOOST_VERSION >= 107400
│ │ │ │ +
23#include <boost/serialization/library_version_type.hpp>
│ │ │ │ +
24#endif
│ │ │ │ +
25#include <boost/serialization/nvp.hpp>
│ │ │ │ +
26#include <boost/serialization/set.hpp>
│ │ │ │ +
27#include <gtsam/base/FastDefaultAllocator.h>
│ │ │ │ +
28#include <gtsam/base/Testable.h>
│ │ │ │ +
29
│ │ │ │ +
30#include <functional>
│ │ │ │ +
31#include <set>
│ │ │ │
32
│ │ │ │ -
33namespace gtsam {
│ │ │ │ -
34
│ │ │ │ -
│ │ │ │ -
36namespace treeTraversal {
│ │ │ │ -
37
│ │ │ │ -
38/* ************************************************************************* */
│ │ │ │ -
39namespace {
│ │ │ │ -
40// Internal node used in DFS preorder stack
│ │ │ │ -
41template<typename NODE, typename DATA>
│ │ │ │ -
42struct TraversalNode {
│ │ │ │ -
43 bool expanded;
│ │ │ │ -
44 const boost::shared_ptr<NODE>& treeNode;
│ │ │ │ -
45 DATA& parentData;
│ │ │ │ -
46 typename FastList<DATA>::iterator dataPointer;
│ │ │ │ -
47 TraversalNode(const boost::shared_ptr<NODE>& _treeNode, DATA& _parentData) :
│ │ │ │ -
48 expanded(false), treeNode(_treeNode), parentData(_parentData) {
│ │ │ │ -
49 }
│ │ │ │ -
50};
│ │ │ │ +
33namespace boost {
│ │ │ │ +
34namespace serialization {
│ │ │ │ +
35class access;
│ │ │ │ +
36} /* namespace serialization */
│ │ │ │ +
37} /* namespace boost */
│ │ │ │ +
38
│ │ │ │ +
39namespace gtsam {
│ │ │ │ +
40
│ │ │ │ +
48template<typename VALUE>
│ │ │ │ +
│ │ │ │ +
49class FastSet: public std::set<VALUE, std::less<VALUE>,
│ │ │ │ +
50 typename internal::FastDefaultAllocator<VALUE>::type> {
│ │ │ │
51
│ │ │ │ -
52// Do nothing - default argument for post-visitor for tree traversal
│ │ │ │ -
53struct no_op {
│ │ │ │ -
54 template<typename NODE, typename DATA>
│ │ │ │ -
55 void operator()(const boost::shared_ptr<NODE>& node, const DATA& data) {
│ │ │ │ -
56 }
│ │ │ │ -
57};
│ │ │ │ +
52 BOOST_CONCEPT_ASSERT ((IsTestable<VALUE> ));
│ │ │ │ +
53
│ │ │ │ +
54public:
│ │ │ │ +
55
│ │ │ │ +
56 typedef std::set<VALUE, std::less<VALUE>,
│ │ │ │ +
57 typename internal::FastDefaultAllocator<VALUE>::type> Base;
│ │ │ │
58
│ │ │ │ -
59}
│ │ │ │ +
59 using Base::Base; // Inherit the set constructors
│ │ │ │
60
│ │ │ │ -
75template<class FOREST, typename DATA, typename VISITOR_PRE,
│ │ │ │ -
76 typename VISITOR_POST>
│ │ │ │ -
│ │ │ │ -
77void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre,
│ │ │ │ -
78 VISITOR_POST& visitorPost) {
│ │ │ │ -
79 // Typedefs
│ │ │ │ -
80 typedef typename FOREST::Node Node;
│ │ │ │ -
81 typedef boost::shared_ptr<Node> sharedNode;
│ │ │ │ -
82
│ │ │ │ -
83 // Depth first traversal stack
│ │ │ │ -
84 typedef TraversalNode<typename FOREST::Node, DATA> TraversalNode;
│ │ │ │ -
85 typedef FastList<TraversalNode> Stack;
│ │ │ │ -
86 Stack stack;
│ │ │ │ -
87 FastList<DATA> dataList; // List to store node data as it is returned from the pre-order visitor
│ │ │ │ -
88
│ │ │ │ -
89 // Add roots to stack (insert such that they are visited and processed in order
│ │ │ │ -
90 {
│ │ │ │ -
91 typename Stack::iterator insertLocation = stack.begin();
│ │ │ │ -
92 for(const sharedNode& root: forest.roots())
│ │ │ │ -
93 stack.insert(insertLocation, TraversalNode(root, rootData));
│ │ │ │ -
94 }
│ │ │ │ -
95
│ │ │ │ -
96 // Traverse
│ │ │ │ -
97 while (!stack.empty()) {
│ │ │ │ -
98 // Get next node
│ │ │ │ -
99 TraversalNode& node = stack.front();
│ │ │ │ -
100
│ │ │ │ -
101 if (node.expanded) {
│ │ │ │ -
102 // If already expanded, then the data stored in the node is no longer needed, so visit
│ │ │ │ -
103 // then delete it.
│ │ │ │ -
104 (void) visitorPost(node.treeNode, *node.dataPointer);
│ │ │ │ -
105 dataList.erase(node.dataPointer);
│ │ │ │ -
106 stack.pop_front();
│ │ │ │ -
107 } else {
│ │ │ │ -
108 // If not already visited, visit the node and add its children (use reverse iterators so
│ │ │ │ -
109 // children are processed in the order they appear)
│ │ │ │ -
110 node.dataPointer = dataList.insert(dataList.end(),
│ │ │ │ -
111 visitorPre(node.treeNode, node.parentData));
│ │ │ │ -
112 typename Stack::iterator insertLocation = stack.begin();
│ │ │ │ -
113 for(const sharedNode& child: node.treeNode->children)
│ │ │ │ -
114 stack.insert(insertLocation, TraversalNode(child, *node.dataPointer));
│ │ │ │ -
115 node.expanded = true;
│ │ │ │ -
116 }
│ │ │ │ -
117 }
│ │ │ │ -
118 assert(dataList.empty());
│ │ │ │ -
119}
│ │ │ │ -
│ │ │ │ -
120
│ │ │ │ -
132template<class FOREST, typename DATA, typename VISITOR_PRE>
│ │ │ │ -
│ │ │ │ -
133void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre) {
│ │ │ │ -
134 no_op visitorPost;
│ │ │ │ -
135 DepthFirstForest(forest, rootData, visitorPre, visitorPost);
│ │ │ │ -
136}
│ │ │ │ -
│ │ │ │ -
137
│ │ │ │ -
152template<class FOREST, typename DATA, typename VISITOR_PRE,
│ │ │ │ -
153 typename VISITOR_POST>
│ │ │ │ -
│ │ │ │ -
154void DepthFirstForestParallel(FOREST& forest, DATA& rootData,
│ │ │ │ -
155 VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost,
│ │ │ │ -
156 int problemSizeThreshold = 10) {
│ │ │ │ -
157#ifdef GTSAM_USE_TBB
│ │ │ │ -
158 // Typedefs
│ │ │ │ -
159 typedef typename FOREST::Node Node;
│ │ │ │ -
160
│ │ │ │ -
161 internal::CreateRootTask<Node>(forest.roots(), rootData, visitorPre,
│ │ │ │ -
162 visitorPost, problemSizeThreshold);
│ │ │ │ -
163#else
│ │ │ │ -
164 DepthFirstForest(forest, rootData, visitorPre, visitorPost);
│ │ │ │ -
165#endif
│ │ │ │ -
166}
│ │ │ │ -
│ │ │ │ -
167
│ │ │ │ -
168/* ************************************************************************* */
│ │ │ │ -
170namespace {
│ │ │ │ -
171template<typename NODE>
│ │ │ │ -
172boost::shared_ptr<NODE> CloneForestVisitorPre(
│ │ │ │ -
173 const boost::shared_ptr<NODE>& node,
│ │ │ │ -
174 const boost::shared_ptr<NODE>& parentPointer) {
│ │ │ │ -
175 // Clone the current node and add it to its cloned parent
│ │ │ │ -
176 boost::shared_ptr<NODE> clone = boost::make_shared<NODE>(*node);
│ │ │ │ -
177 clone->children.clear();
│ │ │ │ -
178 parentPointer->children.push_back(clone);
│ │ │ │ -
179 return clone;
│ │ │ │ -
180}
│ │ │ │ -
181}
│ │ │ │ -
182
│ │ │ │ -
188template<class FOREST>
│ │ │ │ -
│ │ │ │ -
189FastVector<boost::shared_ptr<typename FOREST::Node> > CloneForest(
│ │ │ │ -
190 const FOREST& forest) {
│ │ │ │ -
191 typedef typename FOREST::Node Node;
│ │ │ │ -
192 boost::shared_ptr<Node> rootContainer = boost::make_shared<Node>();
│ │ │ │ -
193 DepthFirstForest(forest, rootContainer, CloneForestVisitorPre<Node>);
│ │ │ │ -
194 return FastVector<boost::shared_ptr<Node> >(rootContainer->children.begin(),
│ │ │ │ -
195 rootContainer->children.end());
│ │ │ │ -
196}
│ │ │ │ -
│ │ │ │ -
197
│ │ │ │ -
198/* ************************************************************************* */
│ │ │ │ -
200namespace {
│ │ │ │ -
201struct PrintForestVisitorPre {
│ │ │ │ -
202 const KeyFormatter& formatter;
│ │ │ │ -
203 PrintForestVisitorPre(const KeyFormatter& formatter) :
│ │ │ │ -
204 formatter(formatter) {
│ │ │ │ -
205 }
│ │ │ │ -
206 template<typename NODE> std::string operator()(
│ │ │ │ -
207 const boost::shared_ptr<NODE>& node, const std::string& parentString) {
│ │ │ │ -
208 // Print the current node
│ │ │ │ -
209 node->print(parentString + "-", formatter);
│ │ │ │ -
210 // Increment the indentation
│ │ │ │ -
211 return parentString + "| ";
│ │ │ │ -
212 }
│ │ │ │ -
213};
│ │ │ │ -
214}
│ │ │ │ -
215
│ │ │ │ -
218template<class FOREST>
│ │ │ │ -
│ │ │ │ -
219void PrintForest(const FOREST& forest, std::string str,
│ │ │ │ -
220 const KeyFormatter& keyFormatter) {
│ │ │ │ -
221 PrintForestVisitorPre visitor(keyFormatter);
│ │ │ │ -
222 DepthFirstForest(forest, str, visitor);
│ │ │ │ -
223}
│ │ │ │ -
│ │ │ │ -
224} // namespace treeTraversal
│ │ │ │ -
│ │ │ │ -
225
│ │ │ │ -
226} // namespace gtsam
│ │ │ │ -
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ -
FastList.h
A thin wrapper around std::list that uses boost's fast_pool_allocator.
│ │ │ │ - │ │ │ │ -
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │ +
61 FastSet() = default;
│ │ │ │ +
62
│ │ │ │ +
64 template<typename INPUTCONTAINER>
│ │ │ │ +
│ │ │ │ +
65 explicit FastSet(const INPUTCONTAINER& container) :
│ │ │ │ +
66 Base(container.begin(), container.end()) {
│ │ │ │ +
67 }
│ │ │ │ +
│ │ │ │ +
68
│ │ │ │ +
│ │ │ │ +
70 FastSet(const FastSet<VALUE>& x) :
│ │ │ │ +
71 Base(x) {
│ │ │ │ +
72 }
│ │ │ │ +
│ │ │ │ +
73
│ │ │ │ +
│ │ │ │ +
75 FastSet(const Base& x) :
│ │ │ │ +
76 Base(x) {
│ │ │ │ +
77 }
│ │ │ │ +
│ │ │ │ +
78
│ │ │ │ +
79#ifdef GTSAM_ALLOCATOR_BOOSTPOOL
│ │ │ │ +
81 FastSet(const std::set<VALUE>& x) {
│ │ │ │ +
82 // This if statement works around a bug in boost pool allocator and/or
│ │ │ │ +
83 // STL vector where if the size is zero, the pool allocator will allocate
│ │ │ │ +
84 // huge amounts of memory.
│ │ │ │ +
85 if(x.size() > 0)
│ │ │ │ +
86 Base::insert(x.begin(), x.end());
│ │ │ │ +
87 }
│ │ │ │ +
88#endif
│ │ │ │ +
89
│ │ │ │ +
│ │ │ │ +
91 operator std::set<VALUE>() const {
│ │ │ │ +
92 return std::set<VALUE>(this->begin(), this->end());
│ │ │ │ +
93 }
│ │ │ │ +
│ │ │ │ +
94
│ │ │ │ +
│ │ │ │ +
96 bool exists(const VALUE& e) const {
│ │ │ │ +
97 return this->find(e) != this->end();
│ │ │ │ +
98 }
│ │ │ │ +
│ │ │ │ +
99
│ │ │ │ +
│ │ │ │ +
101 void print(const std::string& str = "") const {
│ │ │ │ +
102 for (typename Base::const_iterator it = this->begin(); it != this->end(); ++it)
│ │ │ │ +
103 traits<VALUE>::Print(*it, str);
│ │ │ │ +
104 }
│ │ │ │ +
│ │ │ │ +
105
│ │ │ │ +
│ │ │ │ +
107 bool equals(const FastSet<VALUE>& other, double tol = 1e-9) const {
│ │ │ │ +
108 typename Base::const_iterator it1 = this->begin(), it2 = other.begin();
│ │ │ │ +
109 while (it1 != this->end()) {
│ │ │ │ +
110 if (it2 == other.end() || !traits<VALUE>::Equals(*it2, *it2, tol))
│ │ │ │ +
111 return false;
│ │ │ │ +
112 ++it1;
│ │ │ │ +
113 ++it2;
│ │ │ │ +
114 }
│ │ │ │ +
115 return true;
│ │ │ │ +
116 }
│ │ │ │ +
│ │ │ │ +
117
│ │ │ │ +
│ │ │ │ +
119 void merge(const FastSet& other) {
│ │ │ │ +
120 Base::insert(other.begin(), other.end());
│ │ │ │ +
121 }
│ │ │ │ +
│ │ │ │ +
122
│ │ │ │ +
123private:
│ │ │ │ +
125 friend class boost::serialization::access;
│ │ │ │ +
126 template<class ARCHIVE>
│ │ │ │ +
127 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
128 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
129 }
│ │ │ │ +
130};
│ │ │ │ +
│ │ │ │ +
131
│ │ │ │ +
132}
│ │ │ │ +
FastDefaultAllocator.h
An easy way to control which allocator is used for Fast* collections.
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::stack
Matrix stack(size_t nrMatrices,...)
create a matrix by stacking other matrices Given a set of matrices: A1, A2, A3...
Definition Matrix.cpp:397
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::treeTraversal::DepthFirstForest
void DepthFirstForest(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, VISITOR_POST &visitorPost)
Traverse a forest depth-first with pre-order and post-order visits.
Definition treeTraversal-inst.h:77
│ │ │ │ -
gtsam::treeTraversal::CloneForest
FastVector< boost::shared_ptr< typename FOREST::Node > > CloneForest(const FOREST &forest)
Clone a tree, copy-constructing new nodes (calling boost::make_shared) and setting up child pointers ...
Definition treeTraversal-inst.h:189
│ │ │ │ -
gtsam::treeTraversal::PrintForest
void PrintForest(const FOREST &forest, std::string str, const KeyFormatter &keyFormatter)
Print a tree, prefixing each line with str, and formatting keys using keyFormatter.
Definition treeTraversal-inst.h:219
│ │ │ │ -
gtsam::treeTraversal::DepthFirstForestParallel
void DepthFirstForestParallel(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, VISITOR_POST &visitorPost, int problemSizeThreshold=10)
Traverse a forest depth-first with pre-order and post-order visits.
Definition treeTraversal-inst.h:154
│ │ │ │ -
gtsam::FastList
FastList is a thin wrapper around std::list that uses the boost fast_pool_allocator instead of the de...
Definition FastList.h:40
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::FastSet
FastSet is a thin wrapper around std::set that uses the boost fast_pool_allocator instead of the defa...
Definition FastSet.h:50
│ │ │ │ +
gtsam::FastSet::merge
void merge(const FastSet &other)
insert another set: handy for MATLAB access
Definition FastSet.h:119
│ │ │ │ +
gtsam::FastSet::print
void print(const std::string &str="") const
Print to implement Testable: pretty basic.
Definition FastSet.h:101
│ │ │ │ +
gtsam::FastSet::FastSet
FastSet(const INPUTCONTAINER &container)
Constructor from a iterable container, passes through to base class.
Definition FastSet.h:65
│ │ │ │ +
gtsam::FastSet::FastSet
FastSet(const FastSet< VALUE > &x)
Copy constructor from another FastSet.
Definition FastSet.h:70
│ │ │ │ +
gtsam::FastSet::exists
bool exists(const VALUE &e) const
Handy 'exists' function.
Definition FastSet.h:96
│ │ │ │ +
gtsam::FastSet::equals
bool equals(const FastSet< VALUE > &other, double tol=1e-9) const
Check for equality within tolerance to implement Testable.
Definition FastSet.h:107
│ │ │ │ +
gtsam::FastSet::access
friend class boost::serialization::access
Serialization function.
Definition FastSet.h:125
│ │ │ │ +
gtsam::FastSet::FastSet
FastSet(const Base &x)
Copy constructor from the base set class.
Definition FastSet.h:75
│ │ │ │ +
gtsam::FastSet::FastSet
FastSet()=default
Default constructor.
│ │ │ │ +
gtsam::IsTestable
A testable concept check that should be placed in applicable unit tests and in generic algorithms.
Definition Testable.h:58
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,243 +1,176 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -treeTraversal-inst.h │ │ │ │ │ +FastSet.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -17#pragma once │ │ │ │ │ -18 │ │ │ │ │ -19#include │ │ │ │ │ -20#include │ │ │ │ │ -21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_L_i_s_t_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ -25#include // for GTSAM_USE_TBB │ │ │ │ │ -26 │ │ │ │ │ -27#include │ │ │ │ │ -28#include │ │ │ │ │ -29#include │ │ │ │ │ -30#include │ │ │ │ │ -31#include │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include │ │ │ │ │ +22#if BOOST_VERSION >= 107400 │ │ │ │ │ +23#include │ │ │ │ │ +24#endif │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ +27#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_._h> │ │ │ │ │ +28#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +29 │ │ │ │ │ +30#include │ │ │ │ │ +31#include │ │ │ │ │ 32 │ │ │ │ │ -33namespace _g_t_s_a_m { │ │ │ │ │ -34 │ │ │ │ │ -_3_6namespace treeTraversal { │ │ │ │ │ -37 │ │ │ │ │ -38/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -39namespace { │ │ │ │ │ -40// Internal node used in DFS preorder stack │ │ │ │ │ -41template │ │ │ │ │ -42struct TraversalNode { │ │ │ │ │ -43 bool expanded; │ │ │ │ │ -44 const boost::shared_ptr& treeNode; │ │ │ │ │ -45 DATA& parentData; │ │ │ │ │ -46 typename _F_a_s_t_L_i_s_t_<_D_A_T_A_>_:_:_i_t_e_r_a_t_o_r dataPointer; │ │ │ │ │ -47 TraversalNode(const boost::shared_ptr& _treeNode, DATA& _parentData) : │ │ │ │ │ -48 expanded(false), treeNode(_treeNode), parentData(_parentData) { │ │ │ │ │ -49 } │ │ │ │ │ -50}; │ │ │ │ │ +33namespace boost { │ │ │ │ │ +34namespace serialization { │ │ │ │ │ +35class access; │ │ │ │ │ +36} /* namespace serialization */ │ │ │ │ │ +37} /* namespace boost */ │ │ │ │ │ +38 │ │ │ │ │ +39namespace _g_t_s_a_m { │ │ │ │ │ +40 │ │ │ │ │ +48template │ │ │ │ │ +_4_9class _F_a_s_t_S_e_t: public std::set, │ │ │ │ │ +50 typename internal::FastDefaultAllocator::type> { │ │ │ │ │ 51 │ │ │ │ │ -52// Do nothing - default argument for post-visitor for tree traversal │ │ │ │ │ -53struct no_op { │ │ │ │ │ -54 template │ │ │ │ │ -55 void operator()(const boost::shared_ptr& node, const DATA& data) { │ │ │ │ │ -56 } │ │ │ │ │ -57}; │ │ │ │ │ +52 BOOST_CONCEPT_ASSERT ((_I_s_T_e_s_t_a_b_l_e_<_V_A_L_U_E_> )); │ │ │ │ │ +53 │ │ │ │ │ +54public: │ │ │ │ │ +55 │ │ │ │ │ +56 typedef std::set, │ │ │ │ │ +57 typename internal::FastDefaultAllocator::type> Base; │ │ │ │ │ 58 │ │ │ │ │ -59} │ │ │ │ │ +59 using Base::Base; // Inherit the set constructors │ │ │ │ │ 60 │ │ │ │ │ -75template │ │ │ │ │ -_7_7void _D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t(FOREST& forest, DATA& rootData, VISITOR_PRE& │ │ │ │ │ -visitorPre, │ │ │ │ │ -78 VISITOR_POST& visitorPost) { │ │ │ │ │ -79 // Typedefs │ │ │ │ │ -80 typedef typename FOREST::Node Node; │ │ │ │ │ -81 typedef boost::shared_ptr sharedNode; │ │ │ │ │ -82 │ │ │ │ │ -83 // Depth first traversal stack │ │ │ │ │ -84 typedef TraversalNode TraversalNode; │ │ │ │ │ -85 typedef _F_a_s_t_L_i_s_t_<_T_r_a_v_e_r_s_a_l_N_o_d_e_> Stack; │ │ │ │ │ -86 Stack _s_t_a_c_k; │ │ │ │ │ -87 _F_a_s_t_L_i_s_t_<_D_A_T_A_> dataList; // List to store node data as it is returned from │ │ │ │ │ -the pre-order visitor │ │ │ │ │ -88 │ │ │ │ │ -89 // Add roots to stack (insert such that they are visited and processed in │ │ │ │ │ -order │ │ │ │ │ -90 { │ │ │ │ │ -91 typename Stack::iterator insertLocation = _s_t_a_c_k.begin(); │ │ │ │ │ -92 for(const sharedNode& root: forest.roots()) │ │ │ │ │ -93 _s_t_a_c_k.insert(insertLocation, TraversalNode(root, rootData)); │ │ │ │ │ -94 } │ │ │ │ │ -95 │ │ │ │ │ -96 // Traverse │ │ │ │ │ -97 while (!_s_t_a_c_k.empty()) { │ │ │ │ │ -98 // Get next node │ │ │ │ │ -99 TraversalNode& node = _s_t_a_c_k.front(); │ │ │ │ │ -100 │ │ │ │ │ -101 if (node.expanded) { │ │ │ │ │ -102 // If already expanded, then the data stored in the node is no longer │ │ │ │ │ -needed, so visit │ │ │ │ │ -103 // then delete it. │ │ │ │ │ -104 (void) visitorPost(node.treeNode, *node.dataPointer); │ │ │ │ │ -105 dataList.erase(node.dataPointer); │ │ │ │ │ -106 _s_t_a_c_k.pop_front(); │ │ │ │ │ -107 } else { │ │ │ │ │ -108 // If not already visited, visit the node and add its children (use reverse │ │ │ │ │ -iterators so │ │ │ │ │ -109 // children are processed in the order they appear) │ │ │ │ │ -110 node.dataPointer = dataList.insert(dataList.end(), │ │ │ │ │ -111 visitorPre(node.treeNode, node.parentData)); │ │ │ │ │ -112 typename Stack::iterator insertLocation = _s_t_a_c_k.begin(); │ │ │ │ │ -113 for(const sharedNode& child: node.treeNode->children) │ │ │ │ │ -114 _s_t_a_c_k.insert(insertLocation, TraversalNode(child, *node.dataPointer)); │ │ │ │ │ -115 node.expanded = true; │ │ │ │ │ +_6_1 _F_a_s_t_S_e_t() = default; │ │ │ │ │ +62 │ │ │ │ │ +64 template │ │ │ │ │ +_6_5 explicit _F_a_s_t_S_e_t(const INPUTCONTAINER& container) : │ │ │ │ │ +66 Base(container.begin(), container.end()) { │ │ │ │ │ +67 } │ │ │ │ │ +68 │ │ │ │ │ +_7_0 _F_a_s_t_S_e_t(const _F_a_s_t_S_e_t_<_V_A_L_U_E_>& x) : │ │ │ │ │ +71 Base(x) { │ │ │ │ │ +72 } │ │ │ │ │ +73 │ │ │ │ │ +_7_5 _F_a_s_t_S_e_t(const Base& x) : │ │ │ │ │ +76 Base(x) { │ │ │ │ │ +77 } │ │ │ │ │ +78 │ │ │ │ │ +79#ifdef GTSAM_ALLOCATOR_BOOSTPOOL │ │ │ │ │ +81 _F_a_s_t_S_e_t(const std::set& x) { │ │ │ │ │ +82 // This if statement works around a bug in boost pool allocator and/or │ │ │ │ │ +83 // STL vector where if the size is zero, the pool allocator will allocate │ │ │ │ │ +84 // huge amounts of memory. │ │ │ │ │ +85 if(x.size() > 0) │ │ │ │ │ +86 Base::insert(x.begin(), x.end()); │ │ │ │ │ +87 } │ │ │ │ │ +88#endif │ │ │ │ │ +89 │ │ │ │ │ +_9_1 operator std::set() const { │ │ │ │ │ +92 return std::set(this->begin(), this->end()); │ │ │ │ │ +93 } │ │ │ │ │ +94 │ │ │ │ │ +_9_6 bool _e_x_i_s_t_s(const VALUE& e) const { │ │ │ │ │ +97 return this->find(e) != this->end(); │ │ │ │ │ +98 } │ │ │ │ │ +99 │ │ │ │ │ +_1_0_1 void _p_r_i_n_t(const std::string& str = "") const { │ │ │ │ │ +102 for (typename Base::const_iterator it = this->begin(); it != this->end(); │ │ │ │ │ +++it) │ │ │ │ │ +103 _t_r_a_i_t_s_<_V_A_L_U_E_>_:_:_P_r_i_n_t(*it, str); │ │ │ │ │ +104 } │ │ │ │ │ +105 │ │ │ │ │ +_1_0_7 bool _e_q_u_a_l_s(const _F_a_s_t_S_e_t_<_V_A_L_U_E_>& other, double tol = 1e-9) const { │ │ │ │ │ +108 typename Base::const_iterator it1 = this->begin(), it2 = other.begin(); │ │ │ │ │ +109 while (it1 != this->end()) { │ │ │ │ │ +110 if (it2 == other.end() || !_t_r_a_i_t_s_<_V_A_L_U_E_>_:_:_E_q_u_a_l_s(*it2, *it2, tol)) │ │ │ │ │ +111 return false; │ │ │ │ │ +112 ++it1; │ │ │ │ │ +113 ++it2; │ │ │ │ │ +114 } │ │ │ │ │ +115 return true; │ │ │ │ │ 116 } │ │ │ │ │ -117 } │ │ │ │ │ -118 assert(dataList.empty()); │ │ │ │ │ -119} │ │ │ │ │ -120 │ │ │ │ │ -132template │ │ │ │ │ -_1_3_3void _D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t(FOREST& forest, DATA& rootData, VISITOR_PRE& │ │ │ │ │ -visitorPre) { │ │ │ │ │ -134 no_op visitorPost; │ │ │ │ │ -135 _D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t(forest, rootData, visitorPre, visitorPost); │ │ │ │ │ -136} │ │ │ │ │ -137 │ │ │ │ │ -152template │ │ │ │ │ -_1_5_4void _D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t_P_a_r_a_l_l_e_l(FOREST& forest, DATA& rootData, │ │ │ │ │ -155 VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, │ │ │ │ │ -156 int problemSizeThreshold = 10) { │ │ │ │ │ -157#ifdef GTSAM_USE_TBB │ │ │ │ │ -158 // Typedefs │ │ │ │ │ -159 typedef typename FOREST::Node Node; │ │ │ │ │ -160 │ │ │ │ │ -161 internal::CreateRootTask(forest.roots(), rootData, visitorPre, │ │ │ │ │ -162 visitorPost, problemSizeThreshold); │ │ │ │ │ -163#else │ │ │ │ │ -164 _D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t(forest, rootData, visitorPre, visitorPost); │ │ │ │ │ -165#endif │ │ │ │ │ -166} │ │ │ │ │ -167 │ │ │ │ │ -168/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -170namespace { │ │ │ │ │ -171template │ │ │ │ │ -172boost::shared_ptr CloneForestVisitorPre( │ │ │ │ │ -173 const boost::shared_ptr& node, │ │ │ │ │ -174 const boost::shared_ptr& parentPointer) { │ │ │ │ │ -175 // Clone the current node and add it to its cloned parent │ │ │ │ │ -176 boost::shared_ptr clone = boost::make_shared(*node); │ │ │ │ │ -177 clone->children.clear(); │ │ │ │ │ -178 parentPointer->children.push_back(clone); │ │ │ │ │ -179 return clone; │ │ │ │ │ -180} │ │ │ │ │ -181} │ │ │ │ │ -182 │ │ │ │ │ -188template │ │ │ │ │ -_1_8_9_F_a_s_t_V_e_c_t_o_r_<_b_o_o_s_t_:_:_s_h_a_r_e_d___p_t_r_<_t_y_p_e_n_a_m_e_ _F_O_R_E_S_T_:_:_N_o_d_e_> > _C_l_o_n_e_F_o_r_e_s_t( │ │ │ │ │ -190 const FOREST& forest) { │ │ │ │ │ -191 typedef typename FOREST::Node Node; │ │ │ │ │ -192 boost::shared_ptr rootContainer = boost::make_shared(); │ │ │ │ │ -193 _D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t(forest, rootContainer, CloneForestVisitorPre); │ │ │ │ │ -194 return _F_a_s_t_V_e_c_t_o_r_<_b_o_o_s_t_:_:_s_h_a_r_e_d___p_t_r_<_N_o_d_e_> >(rootContainer->children.begin │ │ │ │ │ -(), │ │ │ │ │ -195 rootContainer->children.end()); │ │ │ │ │ -196} │ │ │ │ │ -197 │ │ │ │ │ -198/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -200namespace { │ │ │ │ │ -201struct PrintForestVisitorPre { │ │ │ │ │ -202 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter; │ │ │ │ │ -203 PrintForestVisitorPre(const _K_e_y_F_o_r_m_a_t_t_e_r& formatter) : │ │ │ │ │ -204 formatter(formatter) { │ │ │ │ │ -205 } │ │ │ │ │ -206 template std::string operator()( │ │ │ │ │ -207 const boost::shared_ptr& node, const std::string& parentString) { │ │ │ │ │ -208 // Print the current node │ │ │ │ │ -209 node->print(parentString + "-", formatter); │ │ │ │ │ -210 // Increment the indentation │ │ │ │ │ -211 return parentString + "| "; │ │ │ │ │ -212 } │ │ │ │ │ -213}; │ │ │ │ │ -214} │ │ │ │ │ -215 │ │ │ │ │ -218template │ │ │ │ │ -_2_1_9void _P_r_i_n_t_F_o_r_e_s_t(const FOREST& forest, std::string str, │ │ │ │ │ -220 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter) { │ │ │ │ │ -221 PrintForestVisitorPre visitor(keyFormatter); │ │ │ │ │ -222 _D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t(forest, str, visitor); │ │ │ │ │ -223} │ │ │ │ │ -224} // namespace treeTraversal │ │ │ │ │ -225 │ │ │ │ │ -226} // namespace gtsam │ │ │ │ │ -_F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ -A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ -_F_a_s_t_L_i_s_t_._h │ │ │ │ │ -A thin wrapper around std::list that uses boost's fast_pool_allocator. │ │ │ │ │ -_K_e_y_._h │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ -std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ -FastVector │ │ │ │ │ -FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ -DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ +117 │ │ │ │ │ +_1_1_9 void _m_e_r_g_e(const _F_a_s_t_S_e_t& other) { │ │ │ │ │ +120 Base::insert(other.begin(), other.end()); │ │ │ │ │ +121 } │ │ │ │ │ +122 │ │ │ │ │ +123private: │ │ │ │ │ +_1_2_5 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +126 template │ │ │ │ │ +127 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +128 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); │ │ │ │ │ +129 } │ │ │ │ │ +130}; │ │ │ │ │ +131 │ │ │ │ │ +132} │ │ │ │ │ +_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_._h │ │ │ │ │ +An easy way to control which allocator is used for Fast* collections. │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_s_t_a_c_k │ │ │ │ │ -Matrix stack(size_t nrMatrices,...) │ │ │ │ │ -create a matrix by stacking other matrices Given a set of matrices: A1, A2, │ │ │ │ │ -A3... │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:397 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t │ │ │ │ │ -void DepthFirstForest(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, │ │ │ │ │ -VISITOR_POST &visitorPost) │ │ │ │ │ -Traverse a forest depth-first with pre-order and post-order visits. │ │ │ │ │ -DDeeffiinniittiioonn treeTraversal-inst.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_C_l_o_n_e_F_o_r_e_s_t │ │ │ │ │ -FastVector< boost::shared_ptr< typename FOREST::Node > > CloneForest(const │ │ │ │ │ -FOREST &forest) │ │ │ │ │ -Clone a tree, copy-constructing new nodes (calling boost::make_shared) and │ │ │ │ │ -setting up child pointers ... │ │ │ │ │ -DDeeffiinniittiioonn treeTraversal-inst.h:189 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_P_r_i_n_t_F_o_r_e_s_t │ │ │ │ │ -void PrintForest(const FOREST &forest, std::string str, const KeyFormatter │ │ │ │ │ -&keyFormatter) │ │ │ │ │ -Print a tree, prefixing each line with str, and formatting keys using │ │ │ │ │ -keyFormatter. │ │ │ │ │ -DDeeffiinniittiioonn treeTraversal-inst.h:219 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t_P_a_r_a_l_l_e_l │ │ │ │ │ -void DepthFirstForestParallel(FOREST &forest, DATA &rootData, VISITOR_PRE │ │ │ │ │ -&visitorPre, VISITOR_POST &visitorPost, int problemSizeThreshold=10) │ │ │ │ │ -Traverse a forest depth-first with pre-order and post-order visits. │ │ │ │ │ -DDeeffiinniittiioonn treeTraversal-inst.h:154 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t │ │ │ │ │ -FastList is a thin wrapper around std::list that uses the boost │ │ │ │ │ -fast_pool_allocator instead of the de... │ │ │ │ │ -DDeeffiinniittiioonn FastList.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_S_e_t │ │ │ │ │ +FastSet is a thin wrapper around std::set that uses the boost │ │ │ │ │ +fast_pool_allocator instead of the defa... │ │ │ │ │ +DDeeffiinniittiioonn FastSet.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_S_e_t_:_:_m_e_r_g_e │ │ │ │ │ +void merge(const FastSet &other) │ │ │ │ │ +insert another set: handy for MATLAB access │ │ │ │ │ +DDeeffiinniittiioonn FastSet.h:119 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_S_e_t_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &str="") const │ │ │ │ │ +Print to implement Testable: pretty basic. │ │ │ │ │ +DDeeffiinniittiioonn FastSet.h:101 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_S_e_t_:_:_F_a_s_t_S_e_t │ │ │ │ │ +FastSet(const INPUTCONTAINER &container) │ │ │ │ │ +Constructor from a iterable container, passes through to base class. │ │ │ │ │ +DDeeffiinniittiioonn FastSet.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_S_e_t_:_:_F_a_s_t_S_e_t │ │ │ │ │ +FastSet(const FastSet< VALUE > &x) │ │ │ │ │ +Copy constructor from another FastSet. │ │ │ │ │ +DDeeffiinniittiioonn FastSet.h:70 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_S_e_t_:_:_e_x_i_s_t_s │ │ │ │ │ +bool exists(const VALUE &e) const │ │ │ │ │ +Handy 'exists' function. │ │ │ │ │ +DDeeffiinniittiioonn FastSet.h:96 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_S_e_t_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const FastSet< VALUE > &other, double tol=1e-9) const │ │ │ │ │ +Check for equality within tolerance to implement Testable. │ │ │ │ │ +DDeeffiinniittiioonn FastSet.h:107 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_S_e_t_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn FastSet.h:125 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_S_e_t_:_:_F_a_s_t_S_e_t │ │ │ │ │ +FastSet(const Base &x) │ │ │ │ │ +Copy constructor from the base set class. │ │ │ │ │ +DDeeffiinniittiioonn FastSet.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_S_e_t_:_:_F_a_s_t_S_e_t │ │ │ │ │ +FastSet()=default │ │ │ │ │ +Default constructor. │ │ │ │ │ +_g_t_s_a_m_:_:_I_s_T_e_s_t_a_b_l_e │ │ │ │ │ +A testable concept check that should be placed in applicable unit tests and in │ │ │ │ │ +generic algorithms. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:58 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _t_r_e_e_T_r_a_v_e_r_s_a_l_-_i_n_s_t_._h │ │ │ │ │ + * _F_a_s_t_S_e_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00047.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/make_shared.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/FastList.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,59 +94,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
│ │ │ │ -
make_shared.h File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
FastList.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

make_shared trampoline function to ensure proper alignment │ │ │ │ +

A thin wrapper around std::list that uses boost's fast_pool_allocator. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::FastList< VALUE >
 FastList is a thin wrapper around std::list that uses the boost fast_pool_allocator instead of the default STL allocator. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Typedefs

│ │ │ │ -template<bool B, class T = void>
using gtsam::enable_if_t = typename std::enable_if< B, T >::type
 An shorthand alias for accessing the ::type inside std::enable_if that can be used in a template directly.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

template<typename T , typename ... Args>
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost::shared_ptr< T > > gtsam::make_shared (Args &&... args)
 Add our own make_shared as a layer of wrapping on boost::make_shared This solves the problem with the stock make_shared that custom alignment is not respected, causing SEGFAULTs at runtime, which is notoriously hard to debug.
 
│ │ │ │ -template<typename T , typename ... Args>
gtsam::enable_if_t<!needs_eigen_aligned_allocator< T >::value, boost::shared_ptr< T > > gtsam::make_shared (Args &&... args)
 Fall back to the boost version if no need for alignment.
 
│ │ │ │

Detailed Description

│ │ │ │ -

make_shared trampoline function to ensure proper alignment

│ │ │ │ -
Author
Fan Jiang
│ │ │ │ +

A thin wrapper around std::list that uses boost's fast_pool_allocator.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Oct 22, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,47 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -make_shared.h File Reference │ │ │ │ │ -make_shared trampoline function to ensure proper alignment _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +FastList.h File Reference │ │ │ │ │ +A thin wrapper around std::list that uses boost's fast_pool_allocator. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_F_a_s_t_L_i_s_t_<_ _V_A_L_U_E_ _> │ │ │ │ │ +  _F_a_s_t_L_i_s_t is a thin wrapper around std::list that uses the boost │ │ │ │ │ + fast_pool_allocator instead of the default STL allocator. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -template │ │ │ │ │ -using  ggttssaamm::::eennaabbllee__iiff__tt = typename std::enable_if< B, T >::type │ │ │ │ │ -  An shorthand alias for accessing the ::type inside std::enable_if that │ │ │ │ │ - can be used in a template directly. │ │ │ │ │ -  │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ - _g_t_s_a_m_:_:_e_n_a_b_l_e___i_f___t< │ │ │ │ │ - _n_e_e_d_s___e_i_g_e_n___a_l_i_g_n_e_d___a_l_l_o_c_a_t_o_r< T >::value, _g_t_s_a_m_:_:_m_a_k_e___s_h_a_r_e_d (Args &&... │ │ │ │ │ - boost::shared_ptr< T > >  args) │ │ │ │ │ - Add our own make_shared as a layer │ │ │ │ │ - of wrapping on boost::make_shared │ │ │ │ │ - This solves the problem with the │ │ │ │ │ -  stock make_shared that custom │ │ │ │ │ - alignment is not respected, causing │ │ │ │ │ - SEGFAULTs at runtime, which is │ │ │ │ │ - notoriously hard to debug. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - _g_t_s_a_m_:_: │ │ │ │ │ -_e_n_a_b_l_e___i_f___t::value, boost::shared_ptr< T > >  args) │ │ │ │ │ -  Fall back to the boost version if │ │ │ │ │ - no need for alignment. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -make_shared trampoline function to ensure proper alignment │ │ │ │ │ +A thin wrapper around std::list that uses boost's fast_pool_allocator. │ │ │ │ │ Author │ │ │ │ │ - Fan Jiang │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Oct 22, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _m_a_k_e___s_h_a_r_e_d_._h │ │ │ │ │ + * _F_a_s_t_L_i_s_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00047_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/make_shared.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/FastList.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,68 +98,99 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
make_shared.h
│ │ │ │ +
FastList.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2020, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/base/types.h>
│ │ │ │ -
21
│ │ │ │ -
22#include <Eigen/Core>
│ │ │ │ -
23
│ │ │ │ -
24#include <boost/make_shared.hpp>
│ │ │ │ -
25
│ │ │ │ -
26#include <type_traits>
│ │ │ │ -
27
│ │ │ │ -
28namespace gtsam {
│ │ │ │ -
30 template<bool B, class T = void>
│ │ │ │ -
31 using enable_if_t = typename std::enable_if<B, T>::type;
│ │ │ │ -
32}
│ │ │ │ -
33
│ │ │ │ -
34namespace gtsam {
│ │ │ │ -
35
│ │ │ │ -
56 template<typename T, typename ... Args>
│ │ │ │ -
│ │ │ │ -
57 gtsam::enable_if_t<needs_eigen_aligned_allocator<T>::value, boost::shared_ptr<T>> make_shared(Args &&... args) {
│ │ │ │ -
58 return boost::allocate_shared<T>(Eigen::aligned_allocator<T>(), std::forward<Args>(args)...);
│ │ │ │ -
59 }
│ │ │ │ -
│ │ │ │ -
60
│ │ │ │ -
62 template<typename T, typename ... Args>
│ │ │ │ -
│ │ │ │ -
63 gtsam::enable_if_t<!needs_eigen_aligned_allocator<T>::value, boost::shared_ptr<T>> make_shared(Args &&... args) {
│ │ │ │ -
64 return boost::make_shared<T>(std::forward<Args>(args)...);
│ │ │ │ -
65 }
│ │ │ │ -
│ │ │ │ -
66
│ │ │ │ -
67}
│ │ │ │ -
types.h
Typedefs for easier changing of types.
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/base/FastDefaultAllocator.h>
│ │ │ │ +
22#include <list>
│ │ │ │ +
23#include <boost/utility/enable_if.hpp>
│ │ │ │ +
24#include <boost/serialization/nvp.hpp>
│ │ │ │ +
25#include <boost/serialization/version.hpp>
│ │ │ │ +
26#include <boost/serialization/optional.hpp>
│ │ │ │ +
27#include <boost/serialization/list.hpp>
│ │ │ │ +
28
│ │ │ │ +
29namespace gtsam {
│ │ │ │ +
30
│ │ │ │ +
39template<typename VALUE>
│ │ │ │ +
│ │ │ │ +
40class FastList: public std::list<VALUE, typename internal::FastDefaultAllocator<VALUE>::type> {
│ │ │ │ +
41
│ │ │ │ +
42public:
│ │ │ │ +
43
│ │ │ │ +
44 typedef std::list<VALUE, typename internal::FastDefaultAllocator<VALUE>::type> Base;
│ │ │ │ +
45
│ │ │ │ +
47 FastList() {}
│ │ │ │ +
48
│ │ │ │ +
50 template<typename INPUTITERATOR>
│ │ │ │ +
51 explicit FastList(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {}
│ │ │ │ +
52
│ │ │ │ +
54 FastList(const FastList<VALUE>& x) : Base(x) {}
│ │ │ │ +
55
│ │ │ │ +
57 FastList(const Base& x) : Base(x) {}
│ │ │ │ +
58
│ │ │ │ +
60 FastList(std::initializer_list<VALUE> l) : Base(l) {}
│ │ │ │ +
61
│ │ │ │ +
62#ifdef GTSAM_ALLOCATOR_BOOSTPOOL
│ │ │ │ +
64 FastList(const std::list<VALUE>& x) {
│ │ │ │ +
65 // This if statement works around a bug in boost pool allocator and/or
│ │ │ │ +
66 // STL vector where if the size is zero, the pool allocator will allocate
│ │ │ │ +
67 // huge amounts of memory.
│ │ │ │ +
68 if(x.size() > 0)
│ │ │ │ +
69 Base::assign(x.begin(), x.end());
│ │ │ │ +
70 }
│ │ │ │ +
71#endif
│ │ │ │ +
72
│ │ │ │ +
│ │ │ │ +
74 operator std::list<VALUE>() const {
│ │ │ │ +
75 return std::list<VALUE>(this->begin(), this->end());
│ │ │ │ +
76 }
│ │ │ │ +
│ │ │ │ +
77
│ │ │ │ +
78private:
│ │ │ │ +
80 friend class boost::serialization::access;
│ │ │ │ +
81 template<class ARCHIVE>
│ │ │ │ +
82 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
83 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
84 }
│ │ │ │ +
85
│ │ │ │ +
86};
│ │ │ │ +
│ │ │ │ +
87
│ │ │ │ +
88}
│ │ │ │ +
FastDefaultAllocator.h
An easy way to control which allocator is used for Fast* collections.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::enable_if_t
typename std::enable_if< B, T >::type enable_if_t
An shorthand alias for accessing the ::type inside std::enable_if that can be used in a template dire...
Definition make_shared.h:31
│ │ │ │ -
gtsam::make_shared
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost::shared_ptr< T > > make_shared(Args &&... args)
Add our own make_shared as a layer of wrapping on boost::make_shared This solves the problem with the...
Definition make_shared.h:57
│ │ │ │ +
gtsam::FastList
FastList is a thin wrapper around std::list that uses the boost fast_pool_allocator instead of the de...
Definition FastList.h:40
│ │ │ │ +
gtsam::FastList::FastList
FastList(INPUTITERATOR first, INPUTITERATOR last)
Constructor from a range, passes through to base class.
Definition FastList.h:51
│ │ │ │ +
gtsam::FastList::FastList
FastList()
Default constructor.
Definition FastList.h:47
│ │ │ │ +
gtsam::FastList::FastList
FastList(std::initializer_list< VALUE > l)
Construct from c++11 initializer list:
Definition FastList.h:60
│ │ │ │ +
gtsam::FastList::FastList
FastList(const FastList< VALUE > &x)
Copy constructor from another FastList.
Definition FastList.h:54
│ │ │ │ +
gtsam::FastList::access
friend class boost::serialization::access
Serialization function.
Definition FastList.h:80
│ │ │ │ +
gtsam::FastList::FastList
FastList(const Base &x)
Copy constructor from the base list class.
Definition FastList.h:57
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,71 +1,114 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -make_shared.h │ │ │ │ │ +FastList.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2020, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ -21 │ │ │ │ │ -22#include │ │ │ │ │ -23 │ │ │ │ │ -24#include │ │ │ │ │ -25 │ │ │ │ │ -26#include │ │ │ │ │ -27 │ │ │ │ │ -28namespace _g_t_s_a_m { │ │ │ │ │ -30 template │ │ │ │ │ -_3_1 using _e_n_a_b_l_e___i_f___t = typename std::enable_if::type; │ │ │ │ │ -32} │ │ │ │ │ -33 │ │ │ │ │ -34namespace _g_t_s_a_m { │ │ │ │ │ -35 │ │ │ │ │ -56 template │ │ │ │ │ -_5_7 _g_t_s_a_m_:_:_e_n_a_b_l_e___i_f___t_<_n_e_e_d_s___e_i_g_e_n___a_l_i_g_n_e_d___a_l_l_o_c_a_t_o_r_<_T_>_:_:_v_a_l_u_e, boost:: │ │ │ │ │ -shared_ptr> _m_a_k_e___s_h_a_r_e_d(Args &&... args) { │ │ │ │ │ -58 return boost::allocate_shared(Eigen::aligned_allocator(), std:: │ │ │ │ │ -forward(args)...); │ │ │ │ │ -59 } │ │ │ │ │ -60 │ │ │ │ │ -62 template │ │ │ │ │ -_6_3 _g_t_s_a_m_:_:_e_n_a_b_l_e___i_f___t_<_!_n_e_e_d_s___e_i_g_e_n___a_l_i_g_n_e_d___a_l_l_o_c_a_t_o_r_<_T_>_:_:_v_a_l_u_e, boost:: │ │ │ │ │ -shared_ptr> _m_a_k_e___s_h_a_r_e_d(Args &&... args) { │ │ │ │ │ -64 return boost::make_shared(std::forward(args)...); │ │ │ │ │ -65 } │ │ │ │ │ -66 │ │ │ │ │ -67} │ │ │ │ │ -_t_y_p_e_s_._h │ │ │ │ │ -Typedefs for easier changing of types. │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_._h> │ │ │ │ │ +22#include │ │ │ │ │ +23#include │ │ │ │ │ +24#include │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28 │ │ │ │ │ +29namespace _g_t_s_a_m { │ │ │ │ │ +30 │ │ │ │ │ +39template │ │ │ │ │ +_4_0class _F_a_s_t_L_i_s_t: public std::list::type> { │ │ │ │ │ +41 │ │ │ │ │ +42public: │ │ │ │ │ +43 │ │ │ │ │ +44 typedef std::list:: │ │ │ │ │ +type> Base; │ │ │ │ │ +45 │ │ │ │ │ +_4_7 _F_a_s_t_L_i_s_t() {} │ │ │ │ │ +48 │ │ │ │ │ +50 template │ │ │ │ │ +_5_1 explicit _F_a_s_t_L_i_s_t(INPUTITERATOR first, INPUTITERATOR last) : Base(first, │ │ │ │ │ +last) {} │ │ │ │ │ +52 │ │ │ │ │ +_5_4 _F_a_s_t_L_i_s_t(const _F_a_s_t_L_i_s_t_<_V_A_L_U_E_>& x) : Base(x) {} │ │ │ │ │ +55 │ │ │ │ │ +_5_7 _F_a_s_t_L_i_s_t(const Base& x) : Base(x) {} │ │ │ │ │ +58 │ │ │ │ │ +_6_0 _F_a_s_t_L_i_s_t(std::initializer_list l) : Base(l) {} │ │ │ │ │ +61 │ │ │ │ │ +62#ifdef GTSAM_ALLOCATOR_BOOSTPOOL │ │ │ │ │ +64 _F_a_s_t_L_i_s_t(const std::list& x) { │ │ │ │ │ +65 // This if statement works around a bug in boost pool allocator and/or │ │ │ │ │ +66 // STL vector where if the size is zero, the pool allocator will allocate │ │ │ │ │ +67 // huge amounts of memory. │ │ │ │ │ +68 if(x.size() > 0) │ │ │ │ │ +69 Base::assign(x.begin(), x.end()); │ │ │ │ │ +70 } │ │ │ │ │ +71#endif │ │ │ │ │ +72 │ │ │ │ │ +_7_4 operator std::list() const { │ │ │ │ │ +75 return std::list(this->begin(), this->end()); │ │ │ │ │ +76 } │ │ │ │ │ +77 │ │ │ │ │ +78private: │ │ │ │ │ +_8_0 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +81 template │ │ │ │ │ +82 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +83 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); │ │ │ │ │ +84 } │ │ │ │ │ +85 │ │ │ │ │ +86}; │ │ │ │ │ +87 │ │ │ │ │ +88} │ │ │ │ │ +_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_._h │ │ │ │ │ +An easy way to control which allocator is used for Fast* collections. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_e_n_a_b_l_e___i_f___t │ │ │ │ │ -typename std::enable_if< B, T >::type enable_if_t │ │ │ │ │ -An shorthand alias for accessing the ::type inside std::enable_if that can be │ │ │ │ │ -used in a template dire... │ │ │ │ │ -DDeeffiinniittiioonn make_shared.h:31 │ │ │ │ │ -_g_t_s_a_m_:_:_m_a_k_e___s_h_a_r_e_d │ │ │ │ │ -gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost:: │ │ │ │ │ -shared_ptr< T > > make_shared(Args &&... args) │ │ │ │ │ -Add our own make_shared as a layer of wrapping on boost::make_shared This │ │ │ │ │ -solves the problem with the... │ │ │ │ │ -DDeeffiinniittiioonn make_shared.h:57 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t │ │ │ │ │ +FastList is a thin wrapper around std::list that uses the boost │ │ │ │ │ +fast_pool_allocator instead of the de... │ │ │ │ │ +DDeeffiinniittiioonn FastList.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t_:_:_F_a_s_t_L_i_s_t │ │ │ │ │ +FastList(INPUTITERATOR first, INPUTITERATOR last) │ │ │ │ │ +Constructor from a range, passes through to base class. │ │ │ │ │ +DDeeffiinniittiioonn FastList.h:51 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t_:_:_F_a_s_t_L_i_s_t │ │ │ │ │ +FastList() │ │ │ │ │ +Default constructor. │ │ │ │ │ +DDeeffiinniittiioonn FastList.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t_:_:_F_a_s_t_L_i_s_t │ │ │ │ │ +FastList(std::initializer_list< VALUE > l) │ │ │ │ │ +Construct from c++11 initializer list: │ │ │ │ │ +DDeeffiinniittiioonn FastList.h:60 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t_:_:_F_a_s_t_L_i_s_t │ │ │ │ │ +FastList(const FastList< VALUE > &x) │ │ │ │ │ +Copy constructor from another FastList. │ │ │ │ │ +DDeeffiinniittiioonn FastList.h:54 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn FastList.h:80 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t_:_:_F_a_s_t_L_i_s_t │ │ │ │ │ +FastList(const Base &x) │ │ │ │ │ +Copy constructor from the base list class. │ │ │ │ │ +DDeeffiinniittiioonn FastList.h:57 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _m_a_k_e___s_h_a_r_e_d_._h │ │ │ │ │ + * _F_a_s_t_L_i_s_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00050.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/serializationTestHelpers.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/debug.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -97,111 +97,53 @@ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces | │ │ │ │ Functions | │ │ │ │ Variables
│ │ │ │ -
serializationTestHelpers.h File Reference
│ │ │ │ +
debug.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Global debugging flags. │ │ │ │ +More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

│ │ │ │ -template<class T >
T gtsam::serializationTestHelpers::create ()
 
│ │ │ │ -boost::filesystem::path gtsam::serializationTestHelpers::resetFilesystem (boost::filesystem::path folder="actual")
 
│ │ │ │ -template<class T >
void gtsam::serializationTestHelpers::roundtrip (const T &input, T &output)
 
│ │ │ │ -template<class T >
void gtsam::serializationTestHelpers::roundtripFile (const T &input, T &output)
 
│ │ │ │ -template<class T >
bool gtsam::serializationTestHelpers::equality (const T &input=T())
 
│ │ │ │ -template<class T >
bool gtsam::serializationTestHelpers::equalsObj (const T &input=T())
 
│ │ │ │ -template<class T >
bool gtsam::serializationTestHelpers::equalsDereferenced (const T &input)
 
│ │ │ │ -template<class T >
void gtsam::serializationTestHelpers::roundtripXML (const T &input, T &output)
 
│ │ │ │ -template<class T >
void gtsam::serializationTestHelpers::roundtripXMLFile (const T &input, T &output)
 
│ │ │ │ -template<class T >
bool gtsam::serializationTestHelpers::equalityXML (const T &input=T())
 
│ │ │ │ -template<class T >
bool gtsam::serializationTestHelpers::equalsXML (const T &input=T())
 
│ │ │ │ -template<class T >
bool gtsam::serializationTestHelpers::equalsDereferencedXML (const T &input=T())
 
│ │ │ │ -template<class T >
void gtsam::serializationTestHelpers::roundtripBinary (const T &input, T &output)
 
│ │ │ │ -template<class T >
void gtsam::serializationTestHelpers::roundtripBinaryFile (const T &input, T &output)
 
│ │ │ │ -template<class T >
bool gtsam::serializationTestHelpers::equalityBinary (const T &input=T())
 
│ │ │ │ -template<class T >
bool gtsam::serializationTestHelpers::equalsBinary (const T &input=T())
 
│ │ │ │ -template<class T >
bool gtsam::serializationTestHelpers::equalsDereferencedBinary (const T &input=T())
 
│ │ │ │ +bool gtsam::guardedIsDebug (const std::string &s)
 
│ │ │ │ +void gtsam::guardedSetDebug (const std::string &s, const bool v)
 
│ │ │ │ +bool gtsam::isDebugVersion ()
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Variables

│ │ │ │ -const bool verbose = false
 
│ │ │ │ +GTSAM_EXPORT FastMap< std::string, ValueWithDefault< bool, false > > gtsam::debugFlags
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Alex Cunningham
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ -
Date
Feb 7, 2012
│ │ │ │ +

Global debugging flags.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Feb 1, 2011
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,92 +1,33 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s | _V_a_r_i_a_b_l_e_s │ │ │ │ │ -serializationTestHelpers.h File Reference │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +debug.cpp File Reference │ │ │ │ │ +Global debugging flags. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ - T  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::ccrreeaattee () │ │ │ │ │ +bool  ggttssaamm::::gguuaarrddeeddIIssDDeebbuugg (const std::string &s) │ │ │ │ │   │ │ │ │ │ -boost::filesystem::path  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::rreesseettFFiilleessyysstteemm │ │ │ │ │ - (boost::filesystem::path folder="actual") │ │ │ │ │ +void  ggttssaamm::::gguuaarrddeeddSSeettDDeebbuugg (const std::string &s, const bool v) │ │ │ │ │   │ │ │ │ │ -template │ │ │ │ │ - void  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::rroouunnddttrriipp (const T │ │ │ │ │ - &input, T &output) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - void  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::rroouunnddttrriippFFiillee (const │ │ │ │ │ - T &input, T &output) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - bool  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::eeqquuaalliittyy (const T │ │ │ │ │ - &input=T()) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - bool  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::eeqquuaallssOObbjj (const T │ │ │ │ │ - &input=T()) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - bool  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::eeqquuaallssDDeerreeffeerreenncceedd │ │ │ │ │ - (const T &input) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - void  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::rroouunnddttrriippXXMMLL (const T │ │ │ │ │ - &input, T &output) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - void  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::rroouunnddttrriippXXMMLLFFiillee │ │ │ │ │ - (const T &input, T &output) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - bool  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::eeqquuaalliittyyXXMMLL (const T │ │ │ │ │ - &input=T()) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - bool  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::eeqquuaallssXXMMLL (const T │ │ │ │ │ - &input=T()) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - bool  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::eeqquuaallssDDeerreeffeerreenncceeddXXMMLL │ │ │ │ │ - (const T &input=T()) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - void  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::rroouunnddttrriippBBiinnaarryy │ │ │ │ │ - (const T &input, T &output) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - void  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::rroouunnddttrriippBBiinnaarryyFFiillee │ │ │ │ │ - (const T &input, T &output) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - bool  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::eeqquuaalliittyyBBiinnaarryy (const │ │ │ │ │ - T &input=T()) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - bool  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss::::eeqquuaallssBBiinnaarryy (const T │ │ │ │ │ - &input=T()) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - bool  ggttssaamm::::sseerriiaalliizzaattiioonnTTeessttHHeellppeerrss:::: │ │ │ │ │ - eeqquuaallssDDeerreeffeerreenncceeddBBiinnaarryy (const T &input=T()) │ │ │ │ │ +bool  ggttssaamm::::iissDDeebbuuggVVeerrssiioonn () │ │ │ │ │   │ │ │ │ │ VVaarriiaabblleess │ │ │ │ │ -const bool  vveerrbboossee = false │ │ │ │ │ +GTSAM_EXPORT _F_a_s_t_M_a_p< std::string, _V_a_l_u_e_W_i_t_h_D_e_f_a_u_l_t< bool, │ │ │ │ │ + false > >  ggttssaamm::::ddeebbuuggFFllaaggss │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Global debugging flags. │ │ │ │ │ Author │ │ │ │ │ - Alex Cunningham │ │ │ │ │ Richard Roberts │ │ │ │ │ Date │ │ │ │ │ - Feb 7, 2012 │ │ │ │ │ + Feb 1, 2011 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _s_e_r_i_a_l_i_z_a_t_i_o_n_T_e_s_t_H_e_l_p_e_r_s_._h │ │ │ │ │ + * _d_e_b_u_g_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00056.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/FastDefaultAllocator.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/make_shared.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,49 +94,59 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ -Macros
│ │ │ │ -
FastDefaultAllocator.h File Reference
│ │ │ │ +Typedefs | │ │ │ │ +Functions
│ │ │ │ +
make_shared.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

An easy way to control which allocator is used for Fast* collections. │ │ │ │ +

make_shared trampoline function to ensure proper alignment │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

struct  gtsam::internal::FastDefaultAllocator< T >
 Default allocator for list, map, and set types. More...
 
struct  gtsam::internal::FastDefaultVectorAllocator< T >
 Default allocator for vector types (we never use boost pool for vectors) More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Typedefs

│ │ │ │ +template<bool B, class T = void>
using gtsam::enable_if_t = typename std::enable_if< B, T >::type
 An shorthand alias for accessing the ::type inside std::enable_if that can be used in a template directly.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

template<typename T , typename ... Args>
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost::shared_ptr< T > > gtsam::make_shared (Args &&... args)
 Add our own make_shared as a layer of wrapping on boost::make_shared This solves the problem with the stock make_shared that custom alignment is not respected, causing SEGFAULTs at runtime, which is notoriously hard to debug.
 
│ │ │ │ +template<typename T , typename ... Args>
gtsam::enable_if_t<!needs_eigen_aligned_allocator< T >::value, boost::shared_ptr< T > > gtsam::make_shared (Args &&... args)
 Fall back to the boost version if no need for alignment.
 
│ │ │ │

Detailed Description

│ │ │ │ -

An easy way to control which allocator is used for Fast* collections.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Aug 15, 2013
│ │ │ │ +

make_shared trampoline function to ensure proper alignment

│ │ │ │ +
Author
Fan Jiang
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,31 +1,47 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s │ │ │ │ │ -FastDefaultAllocator.h File Reference │ │ │ │ │ -An easy way to control which allocator is used for Fast* collections. _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +make_shared.h File Reference │ │ │ │ │ +make_shared trampoline function to ensure proper alignment _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_<_ _T_ _> │ │ │ │ │ -  Default allocator for list, map, and set types. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_F_a_s_t_D_e_f_a_u_l_t_V_e_c_t_o_r_A_l_l_o_c_a_t_o_r_<_ _T_ _> │ │ │ │ │ -  Default allocator for vector types (we never use boost pool for │ │ │ │ │ - vectors) _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +template │ │ │ │ │ +using  ggttssaamm::::eennaabbllee__iiff__tt = typename std::enable_if< B, T >::type │ │ │ │ │ +  An shorthand alias for accessing the ::type inside std::enable_if that │ │ │ │ │ + can be used in a template directly. │ │ │ │ │ +  │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +template │ │ │ │ │ + _g_t_s_a_m_:_:_e_n_a_b_l_e___i_f___t< │ │ │ │ │ + _n_e_e_d_s___e_i_g_e_n___a_l_i_g_n_e_d___a_l_l_o_c_a_t_o_r< T >::value, _g_t_s_a_m_:_:_m_a_k_e___s_h_a_r_e_d (Args &&... │ │ │ │ │ + boost::shared_ptr< T > >  args) │ │ │ │ │ + Add our own make_shared as a layer │ │ │ │ │ + of wrapping on boost::make_shared │ │ │ │ │ + This solves the problem with the │ │ │ │ │ +  stock make_shared that custom │ │ │ │ │ + alignment is not respected, causing │ │ │ │ │ + SEGFAULTs at runtime, which is │ │ │ │ │ + notoriously hard to debug. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _g_t_s_a_m_:_: │ │ │ │ │ +_e_n_a_b_l_e___i_f___t::value, boost::shared_ptr< T > >  args) │ │ │ │ │ +  Fall back to the boost version if │ │ │ │ │ + no need for alignment. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -An easy way to control which allocator is used for Fast* collections. │ │ │ │ │ +make_shared trampoline function to ensure proper alignment │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Date │ │ │ │ │ - Aug 15, 2013 │ │ │ │ │ + Fan Jiang │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_._h │ │ │ │ │ + * _m_a_k_e___s_h_a_r_e_d_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00056.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,5 @@ │ │ │ │ │ var a00056 = [ │ │ │ │ │ - ["gtsam::internal::FastDefaultAllocator< T >", "a02304.html", null], │ │ │ │ │ - ["gtsam::internal::FastDefaultVectorAllocator< T >", "a02308.html", null] │ │ │ │ │ + ["enable_if_t", "a00056.html#a47e06cd29f4e8a20a8a7842e4045845f", null], │ │ │ │ │ + ["make_shared", "a00056.html#abcb9ece2bd206193ac40f118f1815c0e", null], │ │ │ │ │ + ["make_shared", "a00056.html#a6f805b32c5544e5552d702d5e2b4e801", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00056_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/FastDefaultAllocator.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/make_shared.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,106 +98,68 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
FastDefaultAllocator.h
│ │ │ │ +
make_shared.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2020, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20#include <gtsam/config.h> // Configuration from CMake
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/base/types.h>
│ │ │ │
21
│ │ │ │ -
22#if !defined GTSAM_ALLOCATOR_BOOSTPOOL && !defined GTSAM_ALLOCATOR_TBB && !defined GTSAM_ALLOCATOR_STL
│ │ │ │ -
23# ifdef GTSAM_USE_TBB
│ │ │ │ -
24// Use TBB allocator by default if we have TBB, otherwise boost pool
│ │ │ │ -
25# define GTSAM_ALLOCATOR_TBB
│ │ │ │ -
26# else
│ │ │ │ -
27# define GTSAM_ALLOCATOR_BOOSTPOOL
│ │ │ │ -
28# endif
│ │ │ │ -
29#endif
│ │ │ │ -
30
│ │ │ │ -
31#if defined GTSAM_ALLOCATOR_BOOSTPOOL
│ │ │ │ -
32# include <boost/pool/pool_alloc.hpp>
│ │ │ │ -
33#elif defined GTSAM_ALLOCATOR_TBB
│ │ │ │ -
34# include <tbb/tbb_allocator.h>
│ │ │ │ -
35# undef min // TBB seems to include Windows.h which defines these macros that cause problems
│ │ │ │ -
36# undef max
│ │ │ │ -
37# undef ERROR
│ │ │ │ -
38#elif defined GTSAM_ALLOCATOR_STL
│ │ │ │ -
39# include <memory>
│ │ │ │ -
40#endif
│ │ │ │ -
41
│ │ │ │ -
42namespace gtsam
│ │ │ │ -
43{
│ │ │ │ -
44
│ │ │ │ -
45 namespace internal
│ │ │ │ -
46 {
│ │ │ │ -
48 template<typename T>
│ │ │ │ -
│ │ │ │ -
49 struct FastDefaultAllocator
│ │ │ │ -
50 {
│ │ │ │ -
51#if defined GTSAM_ALLOCATOR_BOOSTPOOL
│ │ │ │ -
52 typedef boost::fast_pool_allocator<T> type;
│ │ │ │ -
53 static const bool isBoost = true;
│ │ │ │ -
54 static const bool isTBB = false;
│ │ │ │ -
55 static const bool isSTL = false;
│ │ │ │ -
56#elif defined GTSAM_ALLOCATOR_TBB
│ │ │ │ -
57 typedef tbb::tbb_allocator<T> type;
│ │ │ │ -
58 static const bool isBoost = false;
│ │ │ │ -
59 static const bool isTBB = true;
│ │ │ │ -
60 static const bool isSTL = false;
│ │ │ │ -
61#elif defined GTSAM_ALLOCATOR_STL
│ │ │ │ -
62 typedef std::allocator<T> type;
│ │ │ │ -
63 static const bool isBoost = false;
│ │ │ │ -
64 static const bool isTBB = false;
│ │ │ │ -
65 static const bool isSTL = true;
│ │ │ │ -
66#endif
│ │ │ │ -
67 };
│ │ │ │ -
│ │ │ │ -
68
│ │ │ │ -
70 template<typename T>
│ │ │ │ -
│ │ │ │ -
71 struct FastDefaultVectorAllocator
│ │ │ │ -
72 {
│ │ │ │ -
73#if defined GTSAM_ALLOCATOR_TBB
│ │ │ │ -
74 typedef tbb::tbb_allocator<T> type;
│ │ │ │ -
75 static const bool isBoost = false;
│ │ │ │ -
76 static const bool isTBB = true;
│ │ │ │ -
77 static const bool isSTL = false;
│ │ │ │ -
78#else
│ │ │ │ -
79 typedef std::allocator<T> type;
│ │ │ │ -
80 static const bool isBoost = false;
│ │ │ │ -
81 static const bool isTBB = false;
│ │ │ │ -
82 static const bool isSTL = true;
│ │ │ │ -
83#endif
│ │ │ │ -
84 };
│ │ │ │ -
│ │ │ │ -
85 }
│ │ │ │ -
86
│ │ │ │ -
87}
│ │ │ │ +
22#include <Eigen/Core>
│ │ │ │ +
23
│ │ │ │ +
24#include <boost/make_shared.hpp>
│ │ │ │ +
25
│ │ │ │ +
26#include <type_traits>
│ │ │ │ +
27
│ │ │ │ +
28namespace gtsam {
│ │ │ │ +
30 template<bool B, class T = void>
│ │ │ │ +
31 using enable_if_t = typename std::enable_if<B, T>::type;
│ │ │ │ +
32}
│ │ │ │ +
33
│ │ │ │ +
34namespace gtsam {
│ │ │ │ +
35
│ │ │ │ +
56 template<typename T, typename ... Args>
│ │ │ │ +
│ │ │ │ +
57 gtsam::enable_if_t<needs_eigen_aligned_allocator<T>::value, boost::shared_ptr<T>> make_shared(Args &&... args) {
│ │ │ │ +
58 return boost::allocate_shared<T>(Eigen::aligned_allocator<T>(), std::forward<Args>(args)...);
│ │ │ │ +
59 }
│ │ │ │ +
│ │ │ │ +
60
│ │ │ │ +
62 template<typename T, typename ... Args>
│ │ │ │ +
│ │ │ │ +
63 gtsam::enable_if_t<!needs_eigen_aligned_allocator<T>::value, boost::shared_ptr<T>> make_shared(Args &&... args) {
│ │ │ │ +
64 return boost::make_shared<T>(std::forward<Args>(args)...);
│ │ │ │ +
65 }
│ │ │ │ +
│ │ │ │ +
66
│ │ │ │ +
67}
│ │ │ │ +
types.h
Typedefs for easier changing of types.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::internal::FastDefaultAllocator
Default allocator for list, map, and set types.
Definition FastDefaultAllocator.h:50
│ │ │ │ -
gtsam::internal::FastDefaultVectorAllocator
Default allocator for vector types (we never use boost pool for vectors)
Definition FastDefaultAllocator.h:72
│ │ │ │ +
gtsam::enable_if_t
typename std::enable_if< B, T >::type enable_if_t
An shorthand alias for accessing the ::type inside std::enable_if that can be used in a template dire...
Definition make_shared.h:31
│ │ │ │ +
gtsam::make_shared
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost::shared_ptr< T > > make_shared(Args &&... args)
Add our own make_shared as a layer of wrapping on boost::make_shared This solves the problem with the...
Definition make_shared.h:57
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,102 +1,71 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -FastDefaultAllocator.h │ │ │ │ │ +make_shared.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2020, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20#include // Configuration from CMake │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ 21 │ │ │ │ │ -22#if !defined GTSAM_ALLOCATOR_BOOSTPOOL && !defined GTSAM_ALLOCATOR_TBB && │ │ │ │ │ -!defined GTSAM_ALLOCATOR_STL │ │ │ │ │ -23# ifdef GTSAM_USE_TBB │ │ │ │ │ -24// Use TBB allocator by default if we have TBB, otherwise boost pool │ │ │ │ │ -25# define GTSAM_ALLOCATOR_TBB │ │ │ │ │ -26# else │ │ │ │ │ -27# define GTSAM_ALLOCATOR_BOOSTPOOL │ │ │ │ │ -28# endif │ │ │ │ │ -29#endif │ │ │ │ │ -30 │ │ │ │ │ -31#if defined GTSAM_ALLOCATOR_BOOSTPOOL │ │ │ │ │ -32# include │ │ │ │ │ -33#elif defined GTSAM_ALLOCATOR_TBB │ │ │ │ │ -34# include │ │ │ │ │ -35# undef min // TBB seems to include Windows.h which defines these macros that │ │ │ │ │ -cause problems │ │ │ │ │ -36# undef max │ │ │ │ │ -37# undef ERROR │ │ │ │ │ -38#elif defined GTSAM_ALLOCATOR_STL │ │ │ │ │ -39# include │ │ │ │ │ -40#endif │ │ │ │ │ -41 │ │ │ │ │ -42namespace _g_t_s_a_m │ │ │ │ │ -43{ │ │ │ │ │ -44 │ │ │ │ │ -45 namespace internal │ │ │ │ │ -46 { │ │ │ │ │ -48 template │ │ │ │ │ -_4_9 struct _F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r │ │ │ │ │ -50 { │ │ │ │ │ -51#if defined GTSAM_ALLOCATOR_BOOSTPOOL │ │ │ │ │ -52 typedef boost::fast_pool_allocator type; │ │ │ │ │ -53 static const bool isBoost = true; │ │ │ │ │ -54 static const bool isTBB = false; │ │ │ │ │ -55 static const bool isSTL = false; │ │ │ │ │ -56#elif defined GTSAM_ALLOCATOR_TBB │ │ │ │ │ -57 typedef tbb::tbb_allocator type; │ │ │ │ │ -58 static const bool isBoost = false; │ │ │ │ │ -59 static const bool isTBB = true; │ │ │ │ │ -60 static const bool isSTL = false; │ │ │ │ │ -61#elif defined GTSAM_ALLOCATOR_STL │ │ │ │ │ -62 typedef std::allocator type; │ │ │ │ │ -63 static const bool isBoost = false; │ │ │ │ │ -64 static const bool isTBB = false; │ │ │ │ │ -65 static const bool isSTL = true; │ │ │ │ │ -66#endif │ │ │ │ │ -67 }; │ │ │ │ │ -68 │ │ │ │ │ -70 template │ │ │ │ │ -_7_1 struct _F_a_s_t_D_e_f_a_u_l_t_V_e_c_t_o_r_A_l_l_o_c_a_t_o_r │ │ │ │ │ -72 { │ │ │ │ │ -73#if defined GTSAM_ALLOCATOR_TBB │ │ │ │ │ -74 typedef tbb::tbb_allocator type; │ │ │ │ │ -75 static const bool isBoost = false; │ │ │ │ │ -76 static const bool isTBB = true; │ │ │ │ │ -77 static const bool isSTL = false; │ │ │ │ │ -78#else │ │ │ │ │ -79 typedef std::allocator type; │ │ │ │ │ -80 static const bool isBoost = false; │ │ │ │ │ -81 static const bool isTBB = false; │ │ │ │ │ -82 static const bool isSTL = true; │ │ │ │ │ -83#endif │ │ │ │ │ -84 }; │ │ │ │ │ -85 } │ │ │ │ │ -86 │ │ │ │ │ -87} │ │ │ │ │ +22#include │ │ │ │ │ +23 │ │ │ │ │ +24#include │ │ │ │ │ +25 │ │ │ │ │ +26#include │ │ │ │ │ +27 │ │ │ │ │ +28namespace _g_t_s_a_m { │ │ │ │ │ +30 template │ │ │ │ │ +_3_1 using _e_n_a_b_l_e___i_f___t = typename std::enable_if::type; │ │ │ │ │ +32} │ │ │ │ │ +33 │ │ │ │ │ +34namespace _g_t_s_a_m { │ │ │ │ │ +35 │ │ │ │ │ +56 template │ │ │ │ │ +_5_7 _g_t_s_a_m_:_:_e_n_a_b_l_e___i_f___t_<_n_e_e_d_s___e_i_g_e_n___a_l_i_g_n_e_d___a_l_l_o_c_a_t_o_r_<_T_>_:_:_v_a_l_u_e, boost:: │ │ │ │ │ +shared_ptr> _m_a_k_e___s_h_a_r_e_d(Args &&... args) { │ │ │ │ │ +58 return boost::allocate_shared(Eigen::aligned_allocator(), std:: │ │ │ │ │ +forward(args)...); │ │ │ │ │ +59 } │ │ │ │ │ +60 │ │ │ │ │ +62 template │ │ │ │ │ +_6_3 _g_t_s_a_m_:_:_e_n_a_b_l_e___i_f___t_<_!_n_e_e_d_s___e_i_g_e_n___a_l_i_g_n_e_d___a_l_l_o_c_a_t_o_r_<_T_>_:_:_v_a_l_u_e, boost:: │ │ │ │ │ +shared_ptr> _m_a_k_e___s_h_a_r_e_d(Args &&... args) { │ │ │ │ │ +64 return boost::make_shared(std::forward(args)...); │ │ │ │ │ +65 } │ │ │ │ │ +66 │ │ │ │ │ +67} │ │ │ │ │ +_t_y_p_e_s_._h │ │ │ │ │ +Typedefs for easier changing of types. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r │ │ │ │ │ -Default allocator for list, map, and set types. │ │ │ │ │ -DDeeffiinniittiioonn FastDefaultAllocator.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_F_a_s_t_D_e_f_a_u_l_t_V_e_c_t_o_r_A_l_l_o_c_a_t_o_r │ │ │ │ │ -Default allocator for vector types (we never use boost pool for vectors) │ │ │ │ │ -DDeeffiinniittiioonn FastDefaultAllocator.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_e_n_a_b_l_e___i_f___t │ │ │ │ │ +typename std::enable_if< B, T >::type enable_if_t │ │ │ │ │ +An shorthand alias for accessing the ::type inside std::enable_if that can be │ │ │ │ │ +used in a template dire... │ │ │ │ │ +DDeeffiinniittiioonn make_shared.h:31 │ │ │ │ │ +_g_t_s_a_m_:_:_m_a_k_e___s_h_a_r_e_d │ │ │ │ │ +gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost:: │ │ │ │ │ +shared_ptr< T > > make_shared(Args &&... args) │ │ │ │ │ +Add our own make_shared as a layer of wrapping on boost::make_shared This │ │ │ │ │ +solves the problem with the... │ │ │ │ │ +DDeeffiinniittiioonn make_shared.h:57 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_._h │ │ │ │ │ + * _m_a_k_e___s_h_a_r_e_d_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00059.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/VerticalBlockMatrix.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/DSFVector.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,35 +95,35 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces
│ │ │ │ -
VerticalBlockMatrix.cpp File Reference
│ │ │ │ +
DSFVector.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

A matrix with column blocks of pre-defined sizes. │ │ │ │ +

a faster implementation for DSF, which uses vector rather than btree. │ │ │ │ More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

A matrix with column blocks of pre-defined sizes.

│ │ │ │ -

Used in JacobianFactor and GaussianConditional.

Author
Richard Roberts
│ │ │ │ -
Date
Sep 18, 2010
│ │ │ │ +

a faster implementation for DSF, which uses vector rather than btree.

│ │ │ │ +
Date
Jun 25, 2010
│ │ │ │ +
Author
Kai Ni
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,22 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -VerticalBlockMatrix.cpp File Reference │ │ │ │ │ -A matrix with column blocks of pre-defined sizes. _M_o_r_e_._._. │ │ │ │ │ +DSFVector.cpp File Reference │ │ │ │ │ +a faster implementation for DSF, which uses vector rather than btree. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A matrix with column blocks of pre-defined sizes. │ │ │ │ │ -Used in _J_a_c_o_b_i_a_n_F_a_c_t_o_r and _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l. │ │ │ │ │ - Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ +a faster implementation for DSF, which uses vector rather than btree. │ │ │ │ │ Date │ │ │ │ │ - Sep 18, 2010 │ │ │ │ │ + Jun 25, 2010 │ │ │ │ │ + Author │ │ │ │ │ + Kai Ni │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_._c_p_p │ │ │ │ │ + * _D_S_F_V_e_c_t_o_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00065_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Vector.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/ConcurrentMap.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,192 +98,140 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Vector.h
│ │ │ │ +
ConcurrentMap.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
21// \callgraph
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/global_includes.h>
│ │ │ │
22
│ │ │ │ -
23#pragma once
│ │ │ │ -
24#ifndef MKL_BLAS
│ │ │ │ -
25#define MKL_BLAS MKL_DOMAIN_BLAS
│ │ │ │ -
26#endif
│ │ │ │ -
27
│ │ │ │ -
28#include <gtsam/global_includes.h>
│ │ │ │ -
29#include <Eigen/Core>
│ │ │ │ -
30#include <iosfwd>
│ │ │ │ -
31#include <list>
│ │ │ │ -
32
│ │ │ │ -
33namespace gtsam {
│ │ │ │ -
34
│ │ │ │ -
35// Vector is just a typedef of the Eigen dynamic vector type
│ │ │ │ -
36
│ │ │ │ -
37// Typedef arbitary length vector
│ │ │ │ -
38typedef Eigen::VectorXd Vector;
│ │ │ │ -
39
│ │ │ │ -
40// Commonly used fixed size vectors
│ │ │ │ -
41typedef Eigen::Matrix<double, 1, 1> Vector1;
│ │ │ │ -
42typedef Eigen::Vector2d Vector2;
│ │ │ │ -
43typedef Eigen::Vector3d Vector3;
│ │ │ │ -
44
│ │ │ │ -
45static const Eigen::MatrixBase<Vector2>::ConstantReturnType Z_2x1 = Vector2::Zero();
│ │ │ │ -
46static const Eigen::MatrixBase<Vector3>::ConstantReturnType Z_3x1 = Vector3::Zero();
│ │ │ │ -
47
│ │ │ │ -
48// Create handy typedefs and constants for vectors with N>3
│ │ │ │ -
49// VectorN and Z_Nx1, for N=1..9
│ │ │ │ -
50#define GTSAM_MAKE_VECTOR_DEFS(N) \
│ │ │ │ -
51 using Vector##N = Eigen::Matrix<double, N, 1>; \
│ │ │ │ -
52 static const Eigen::MatrixBase<Vector##N>::ConstantReturnType Z_##N##x1 = Vector##N::Zero();
│ │ │ │ -
53
│ │ │ │ -
54GTSAM_MAKE_VECTOR_DEFS(4)
│ │ │ │ -
55GTSAM_MAKE_VECTOR_DEFS(5)
│ │ │ │ -
56GTSAM_MAKE_VECTOR_DEFS(6)
│ │ │ │ -
57GTSAM_MAKE_VECTOR_DEFS(7)
│ │ │ │ -
58GTSAM_MAKE_VECTOR_DEFS(8)
│ │ │ │ -
59GTSAM_MAKE_VECTOR_DEFS(9)
│ │ │ │ -
60GTSAM_MAKE_VECTOR_DEFS(10)
│ │ │ │ -
61GTSAM_MAKE_VECTOR_DEFS(11)
│ │ │ │ -
62GTSAM_MAKE_VECTOR_DEFS(12)
│ │ │ │ -
63GTSAM_MAKE_VECTOR_DEFS(15)
│ │ │ │ -
64
│ │ │ │ -
65typedef Eigen::VectorBlock<Vector> SubVector;
│ │ │ │ -
66typedef Eigen::VectorBlock<const Vector> ConstSubVector;
│ │ │ │ -
67
│ │ │ │ -
73#if defined(GTSAM_EIGEN_VERSION_WORLD)
│ │ │ │ -
74static_assert(
│ │ │ │ -
75 GTSAM_EIGEN_VERSION_WORLD==EIGEN_WORLD_VERSION &&
│ │ │ │ -
76 GTSAM_EIGEN_VERSION_MAJOR==EIGEN_MAJOR_VERSION,
│ │ │ │ -
77 "Error: GTSAM was built against a different version of Eigen");
│ │ │ │ -
78#endif
│ │ │ │ -
79
│ │ │ │ -
96GTSAM_EXPORT bool fpEqual(double a, double b, double tol,
│ │ │ │ -
97 bool check_relative_also = true);
│ │ │ │ -
98
│ │ │ │ -
102GTSAM_EXPORT void print(const Vector& v, const std::string& s, std::ostream& stream);
│ │ │ │ -
103
│ │ │ │ -
107GTSAM_EXPORT void print(const Vector& v, const std::string& s = "");
│ │ │ │ -
108
│ │ │ │ -
112GTSAM_EXPORT void save(const Vector& A, const std::string &s, const std::string& filename);
│ │ │ │ -
113
│ │ │ │ -
117GTSAM_EXPORT bool operator==(const Vector& vec1,const Vector& vec2);
│ │ │ │ -
118
│ │ │ │ -
124GTSAM_EXPORT bool greaterThanOrEqual(const Vector& v1, const Vector& v2);
│ │ │ │ -
125
│ │ │ │ -
129GTSAM_EXPORT bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol=1e-9);
│ │ │ │ -
130GTSAM_EXPORT bool equal_with_abs_tol(const SubVector& vec1, const SubVector& vec2, double tol=1e-9);
│ │ │ │ -
131
│ │ │ │ -
│ │ │ │ -
135inline bool equal(const Vector& vec1, const Vector& vec2, double tol) {
│ │ │ │ -
136 return equal_with_abs_tol(vec1, vec2, tol);
│ │ │ │ -
137}
│ │ │ │ -
│ │ │ │ -
138
│ │ │ │ -
│ │ │ │ -
142inline bool equal(const Vector& vec1, const Vector& vec2) {
│ │ │ │ -
143 return equal_with_abs_tol(vec1, vec2);
│ │ │ │ -
144}
│ │ │ │ -
│ │ │ │ -
145
│ │ │ │ -
153GTSAM_EXPORT bool assert_equal(const Vector& vec1, const Vector& vec2, double tol=1e-9);
│ │ │ │ -
154
│ │ │ │ -
162GTSAM_EXPORT bool assert_inequal(const Vector& vec1, const Vector& vec2, double tol=1e-9);
│ │ │ │ -
163
│ │ │ │ -
171GTSAM_EXPORT bool assert_equal(const SubVector& vec1, const SubVector& vec2, double tol=1e-9);
│ │ │ │ -
172GTSAM_EXPORT bool assert_equal(const ConstSubVector& vec1, const ConstSubVector& vec2, double tol=1e-9);
│ │ │ │ -
173
│ │ │ │ -
181GTSAM_EXPORT bool linear_dependent(const Vector& vec1, const Vector& vec2, double tol=1e-9);
│ │ │ │ -
182
│ │ │ │ -
189GTSAM_EXPORT Vector ediv_(const Vector &a, const Vector &b);
│ │ │ │ -
190
│ │ │ │ -
194template<class V1, class V2>
│ │ │ │ -
│ │ │ │ -
195inline double dot(const V1 &a, const V2& b) {
│ │ │ │ -
196 assert (b.size()==a.size());
│ │ │ │ -
197 return a.dot(b);
│ │ │ │ -
198}
│ │ │ │ -
│ │ │ │ -
199
│ │ │ │ -
201template<class V1, class V2>
│ │ │ │ -
│ │ │ │ -
202inline double inner_prod(const V1 &a, const V2& b) {
│ │ │ │ -
203 assert (b.size()==a.size());
│ │ │ │ -
204 return a.dot(b);
│ │ │ │ -
205}
│ │ │ │ -
│ │ │ │ -
206
│ │ │ │ -
207#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ -
212inline void GTSAM_DEPRECATED scal(double alpha, Vector& x) { x *= alpha; }
│ │ │ │ -
213
│ │ │ │ -
218template<class V1, class V2>
│ │ │ │ -
219inline void GTSAM_DEPRECATED axpy(double alpha, const V1& x, V2& y) {
│ │ │ │ -
220 assert (y.size()==x.size());
│ │ │ │ -
221 y += alpha * x;
│ │ │ │ -
222}
│ │ │ │ -
223inline void axpy(double alpha, const Vector& x, SubVector y) {
│ │ │ │ -
224 assert (y.size()==x.size());
│ │ │ │ -
225 y += alpha * x;
│ │ │ │ -
226}
│ │ │ │ -
227#endif
│ │ │ │ -
228
│ │ │ │ -
234GTSAM_EXPORT std::pair<double,Vector> house(const Vector &x);
│ │ │ │ -
235
│ │ │ │ -
237GTSAM_EXPORT double houseInPlace(Vector &x);
│ │ │ │ -
238
│ │ │ │ -
249GTSAM_EXPORT std::pair<Vector, double>
│ │ │ │ -
250weightedPseudoinverse(const Vector& v, const Vector& weights);
│ │ │ │ -
251
│ │ │ │ -
252/*
│ │ │ │ -
253 * Fast version *no error checking* !
│ │ │ │ -
254 * Pass in initialized vector pseudo of size(weights) or will crash !
│ │ │ │ -
255 * @return the precision, pseudoinverse in third argument
│ │ │ │ -
256 */
│ │ │ │ -
257GTSAM_EXPORT double weightedPseudoinverse(const Vector& a, const Vector& weights, Vector& pseudo);
│ │ │ │ -
258
│ │ │ │ -
262GTSAM_EXPORT Vector concatVectors(const std::list<Vector>& vs);
│ │ │ │ -
263
│ │ │ │ -
267GTSAM_EXPORT Vector concatVectors(size_t nrVectors, ...);
│ │ │ │ -
268} // namespace gtsam
│ │ │ │ -
global_includes.h
Included from all GTSAM files.
│ │ │ │ +
23// Change class depending on whether we are using TBB
│ │ │ │ +
24#ifdef GTSAM_USE_TBB
│ │ │ │ +
25
│ │ │ │ +
26// Include TBB header
│ │ │ │ +
27# include <tbb/concurrent_unordered_map.h>
│ │ │ │ +
28# undef min // TBB seems to include Windows.h which defines these macros that cause problems
│ │ │ │ +
29# undef max
│ │ │ │ +
30# undef ERROR
│ │ │ │ +
31
│ │ │ │ +
32#include <functional> // std::hash()
│ │ │ │ +
33
│ │ │ │ +
34// Use TBB concurrent_unordered_map for ConcurrentMap
│ │ │ │ +
35template <typename KEY, typename VALUE>
│ │ │ │ +
36using ConcurrentMapBase = tbb::concurrent_unordered_map<
│ │ │ │ +
37 KEY,
│ │ │ │ +
38 VALUE,
│ │ │ │ +
39 std::hash<KEY>
│ │ │ │ +
40 >;
│ │ │ │ +
41
│ │ │ │ +
42#else
│ │ │ │ +
43
│ │ │ │ +
44// If we're not using TBB, use a FastMap for ConcurrentMap
│ │ │ │ +
45#include <gtsam/base/FastMap.h>
│ │ │ │ +
46template <typename KEY, typename VALUE>
│ │ │ │ +
47using ConcurrentMapBase = gtsam::FastMap<KEY, VALUE>;
│ │ │ │ +
48
│ │ │ │ +
49#endif
│ │ │ │ +
50
│ │ │ │ +
51#include <boost/serialization/nvp.hpp>
│ │ │ │ +
52#include <boost/serialization/split_member.hpp>
│ │ │ │ +
53#include <boost/static_assert.hpp>
│ │ │ │ +
54
│ │ │ │ +
55#include <gtsam/base/FastVector.h>
│ │ │ │ +
56
│ │ │ │ +
57namespace gtsam {
│ │ │ │ +
58
│ │ │ │ +
67template<typename KEY, typename VALUE>
│ │ │ │ +
│ │ │ │ +
68class ConcurrentMap : public ConcurrentMapBase<KEY,VALUE> {
│ │ │ │ +
69
│ │ │ │ +
70public:
│ │ │ │ +
71
│ │ │ │ +
72 typedef ConcurrentMapBase<KEY,VALUE> Base;
│ │ │ │ +
73
│ │ │ │ +
75 ConcurrentMap() {}
│ │ │ │ +
76
│ │ │ │ +
78 template<typename INPUTITERATOR>
│ │ │ │ +
79 ConcurrentMap(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {}
│ │ │ │ +
80
│ │ │ │ +
82 ConcurrentMap(const ConcurrentMap<KEY,VALUE>& x) : Base(x) {}
│ │ │ │ +
83
│ │ │ │ +
85 ConcurrentMap(const Base& x) : Base(x) {}
│ │ │ │ +
86
│ │ │ │ +
88 bool exists(const KEY& e) const { return this->count(e); }
│ │ │ │ +
89
│ │ │ │ +
90#ifndef GTSAM_USE_TBB
│ │ │ │ +
91 // If we're not using TBB and this is actually a FastMap, we need to add these functions and hide
│ │ │ │ +
92 // the original erase functions.
│ │ │ │ +
93 void unsafe_erase(typename Base::iterator position) { ((Base*)this)->erase(position); }
│ │ │ │ +
94 typename Base::size_type unsafe_erase(const KEY& k) { return ((Base*)this)->erase(k); }
│ │ │ │ +
95 void unsafe_erase(typename Base::iterator first, typename Base::iterator last) {
│ │ │ │ +
96 return ((Base*)this)->erase(first, last); }
│ │ │ │ +
97private:
│ │ │ │ +
98 void erase() {}
│ │ │ │ +
99public:
│ │ │ │ +
100#endif
│ │ │ │ +
101
│ │ │ │ +
102private:
│ │ │ │ +
104 friend class boost::serialization::access;
│ │ │ │ +
105 template<class Archive>
│ │ │ │ +
106 void save(Archive& ar, const unsigned int /*version*/) const
│ │ │ │ +
107 {
│ │ │ │ +
108 // Copy to an STL container and serialize that
│ │ │ │ +
109 FastVector<std::pair<KEY, VALUE> > map(this->size());
│ │ │ │ +
110 std::copy(this->begin(), this->end(), map.begin());
│ │ │ │ +
111 ar & BOOST_SERIALIZATION_NVP(map);
│ │ │ │ +
112 }
│ │ │ │ +
113 template<class Archive>
│ │ │ │ +
114 void load(Archive& ar, const unsigned int /*version*/)
│ │ │ │ +
115 {
│ │ │ │ +
116 this->clear();
│ │ │ │ +
117 // Load into STL container and then fill our map
│ │ │ │ +
118 FastVector<std::pair<KEY, VALUE> > map;
│ │ │ │ +
119 ar & BOOST_SERIALIZATION_NVP(map);
│ │ │ │ +
120 this->insert(map.begin(), map.end());
│ │ │ │ +
121 }
│ │ │ │ +
122 BOOST_SERIALIZATION_SPLIT_MEMBER()
│ │ │ │ +
123};
│ │ │ │ +
│ │ │ │ +
124
│ │ │ │ +
125}
│ │ │ │ +
FastMap.h
A thin wrapper around std::map that uses boost's fast_pool_allocator.
│ │ │ │ +
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ +
global_includes.h
Included from all GTSAM files.
│ │ │ │ +
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::ediv_
Vector ediv_(const Vector &a, const Vector &b)
elementwise division, but 0/0 = 0, not inf
Definition Vector.cpp:199
│ │ │ │ -
gtsam::greaterThanOrEqual
bool greaterThanOrEqual(const Vector &vec1, const Vector &vec2)
Greater than or equal to operation returns true if all elements in v1 are greater than corresponding ...
Definition Vector.cpp:114
│ │ │ │ -
gtsam::save
void save(const Matrix &A, const string &s, const string &filename)
save a matrix to file, which can be loaded by matlab
Definition Matrix.cpp:167
│ │ │ │ -
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
equals with an tolerance, prints out message if unequal
Definition Matrix.cpp:43
│ │ │ │ -
gtsam::linear_dependent
bool linear_dependent(const Matrix &A, const Matrix &B, double tol)
check whether the rows of two matrices are linear dependent
Definition Matrix.cpp:117
│ │ │ │ -
gtsam::inner_prod
double inner_prod(const V1 &a, const V2 &b)
compatibility version for ublas' inner_prod()
Definition Vector.h:202
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::axpy
void axpy(double alpha, const Errors &x, Errors &y)
BLAS level 2 style AXPY, y := alpha*x + y
Definition Errors.cpp:111
│ │ │ │ -
gtsam::assert_inequal
bool assert_inequal(const Matrix &A, const Matrix &B, double tol)
inequals with an tolerance, prints out message if within tolerance
Definition Matrix.cpp:63
│ │ │ │ -
gtsam::houseInPlace
double houseInPlace(Vector &v)
beta = house(x) computes the HouseHolder vector in place
Definition Vector.cpp:212
│ │ │ │ -
gtsam::equal
bool equal(const T &obj1, const T &obj2, double tol)
Call equal on the object.
Definition Testable.h:84
│ │ │ │ -
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
│ │ │ │ -
gtsam::fpEqual
bool fpEqual(double a, double b, double tol, bool check_relative_also)
Ensure we are not including a different version of Eigen in user code than while compiling gtsam,...
Definition Vector.cpp:42
│ │ │ │ -
gtsam::concatVectors
Vector concatVectors(const std::list< Vector > &vs)
concatenate Vectors
Definition Vector.cpp:302
│ │ │ │ -
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition Matrix.h:81
│ │ │ │ -
gtsam::house
pair< double, Vector > house(const Vector &x)
house(x,j) computes HouseHolder vector v and scaling factor beta from x, such that the corresponding ...
Definition Vector.cpp:237
│ │ │ │ -
gtsam::operator==
bool operator==(const Matrix &A, const Matrix &B)
equality is just equal_with_abs_tol 1e-9
Definition Matrix.h:100
│ │ │ │ +
gtsam::ConcurrentMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition ConcurrentMap.h:68
│ │ │ │ +
gtsam::ConcurrentMap::ConcurrentMap
ConcurrentMap(INPUTITERATOR first, INPUTITERATOR last)
Constructor from a range, passes through to base class.
Definition ConcurrentMap.h:79
│ │ │ │ +
gtsam::ConcurrentMap::ConcurrentMap
ConcurrentMap(const ConcurrentMap< KEY, VALUE > &x)
Copy constructor from another ConcurrentMap.
Definition ConcurrentMap.h:82
│ │ │ │ +
gtsam::ConcurrentMap::exists
bool exists(const KEY &e) const
Handy 'exists' function.
Definition ConcurrentMap.h:88
│ │ │ │ +
gtsam::ConcurrentMap::ConcurrentMap
ConcurrentMap(const Base &x)
Copy constructor from the base map class.
Definition ConcurrentMap.h:85
│ │ │ │ +
gtsam::ConcurrentMap::ConcurrentMap
ConcurrentMap()
Default constructor.
Definition ConcurrentMap.h:75
│ │ │ │ +
gtsam::ConcurrentMap::access
friend class boost::serialization::access
Serialization function.
Definition ConcurrentMap.h:104
│ │ │ │ +
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,247 +1,168 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Vector.h │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +ConcurrentMap.h │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -21// \callgraph │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ 22 │ │ │ │ │ -23#pragma once │ │ │ │ │ -24#ifndef MKL_BLAS │ │ │ │ │ -25#define MKL_BLAS MKL_DOMAIN_BLAS │ │ │ │ │ -26#endif │ │ │ │ │ -27 │ │ │ │ │ -28#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ -29#include │ │ │ │ │ -30#include │ │ │ │ │ -31#include │ │ │ │ │ -32 │ │ │ │ │ -33namespace _g_t_s_a_m { │ │ │ │ │ -34 │ │ │ │ │ -35// Vector is just a typedef of the Eigen dynamic vector type │ │ │ │ │ -36 │ │ │ │ │ -37// Typedef arbitary length vector │ │ │ │ │ -38typedef Eigen::VectorXd Vector; │ │ │ │ │ -39 │ │ │ │ │ -40// Commonly used fixed size vectors │ │ │ │ │ -41typedef Eigen::Matrix Vector1; │ │ │ │ │ -42typedef Eigen::Vector2d Vector2; │ │ │ │ │ -43typedef Eigen::Vector3d Vector3; │ │ │ │ │ -44 │ │ │ │ │ -45static const Eigen::MatrixBase::ConstantReturnType Z_2x1 = Vector2:: │ │ │ │ │ -Zero(); │ │ │ │ │ -46static const Eigen::MatrixBase::ConstantReturnType Z_3x1 = Vector3:: │ │ │ │ │ -Zero(); │ │ │ │ │ -47 │ │ │ │ │ -48// Create handy typedefs and constants for vectors with N>3 │ │ │ │ │ -49// VectorN and Z_Nx1, for N=1..9 │ │ │ │ │ -50#define GTSAM_MAKE_VECTOR_DEFS(N) \ │ │ │ │ │ -51 using Vector##N = Eigen::Matrix; \ │ │ │ │ │ -52 static const Eigen::MatrixBase::ConstantReturnType Z_##N##x1 = │ │ │ │ │ -Vector##N::Zero(); │ │ │ │ │ -53 │ │ │ │ │ -54GTSAM_MAKE_VECTOR_DEFS(4) │ │ │ │ │ -55GTSAM_MAKE_VECTOR_DEFS(5) │ │ │ │ │ -56GTSAM_MAKE_VECTOR_DEFS(6) │ │ │ │ │ -57GTSAM_MAKE_VECTOR_DEFS(7) │ │ │ │ │ -58GTSAM_MAKE_VECTOR_DEFS(8) │ │ │ │ │ -59GTSAM_MAKE_VECTOR_DEFS(9) │ │ │ │ │ -60GTSAM_MAKE_VECTOR_DEFS(10) │ │ │ │ │ -61GTSAM_MAKE_VECTOR_DEFS(11) │ │ │ │ │ -62GTSAM_MAKE_VECTOR_DEFS(12) │ │ │ │ │ -63GTSAM_MAKE_VECTOR_DEFS(15) │ │ │ │ │ -64 │ │ │ │ │ -65typedef Eigen::VectorBlock SubVector; │ │ │ │ │ -66typedef Eigen::VectorBlock ConstSubVector; │ │ │ │ │ -67 │ │ │ │ │ -73#if defined(GTSAM_EIGEN_VERSION_WORLD) │ │ │ │ │ -74static_assert( │ │ │ │ │ -75 GTSAM_EIGEN_VERSION_WORLD==EIGEN_WORLD_VERSION && │ │ │ │ │ -76 GTSAM_EIGEN_VERSION_MAJOR==EIGEN_MAJOR_VERSION, │ │ │ │ │ -77 "Error: GTSAM was built against a different version of Eigen"); │ │ │ │ │ -78#endif │ │ │ │ │ -79 │ │ │ │ │ -96GTSAM_EXPORT bool _f_p_E_q_u_a_l(double a, double b, double tol, │ │ │ │ │ -97 bool check_relative_also = true); │ │ │ │ │ -98 │ │ │ │ │ -102GTSAM_EXPORT void _p_r_i_n_t(const Vector& v, const std::string& s, std::ostream& │ │ │ │ │ -stream); │ │ │ │ │ -103 │ │ │ │ │ -107GTSAM_EXPORT void _p_r_i_n_t(const Vector& v, const std::string& s = ""); │ │ │ │ │ -108 │ │ │ │ │ -112GTSAM_EXPORT void _s_a_v_e(const Vector& A, const std::string &s, const std:: │ │ │ │ │ -string& filename); │ │ │ │ │ -113 │ │ │ │ │ -117GTSAM_EXPORT bool _o_p_e_r_a_t_o_r_=_=(const Vector& vec1,const Vector& vec2); │ │ │ │ │ -118 │ │ │ │ │ -124GTSAM_EXPORT bool _g_r_e_a_t_e_r_T_h_a_n_O_r_E_q_u_a_l(const Vector& v1, const Vector& v2); │ │ │ │ │ -125 │ │ │ │ │ -129GTSAM_EXPORT bool _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(const Vector& vec1, const Vector& vec2, │ │ │ │ │ -double tol=1e-9); │ │ │ │ │ -130GTSAM_EXPORT bool _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(const SubVector& vec1, const SubVector& │ │ │ │ │ -vec2, double tol=1e-9); │ │ │ │ │ -131 │ │ │ │ │ -_1_3_5inline bool _e_q_u_a_l(const Vector& vec1, const Vector& vec2, double tol) { │ │ │ │ │ -136 return _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(vec1, vec2, tol); │ │ │ │ │ -137} │ │ │ │ │ -138 │ │ │ │ │ -_1_4_2inline bool _e_q_u_a_l(const Vector& vec1, const Vector& vec2) { │ │ │ │ │ -143 return _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(vec1, vec2); │ │ │ │ │ -144} │ │ │ │ │ -145 │ │ │ │ │ -153GTSAM_EXPORT bool _a_s_s_e_r_t___e_q_u_a_l(const Vector& vec1, const Vector& vec2, │ │ │ │ │ -double tol=1e-9); │ │ │ │ │ -154 │ │ │ │ │ -162GTSAM_EXPORT bool _a_s_s_e_r_t___i_n_e_q_u_a_l(const Vector& vec1, const Vector& vec2, │ │ │ │ │ -double tol=1e-9); │ │ │ │ │ -163 │ │ │ │ │ -171GTSAM_EXPORT bool _a_s_s_e_r_t___e_q_u_a_l(const SubVector& vec1, const SubVector& vec2, │ │ │ │ │ -double tol=1e-9); │ │ │ │ │ -172GTSAM_EXPORT bool _a_s_s_e_r_t___e_q_u_a_l(const ConstSubVector& vec1, const │ │ │ │ │ -ConstSubVector& vec2, double tol=1e-9); │ │ │ │ │ -173 │ │ │ │ │ -181GTSAM_EXPORT bool _l_i_n_e_a_r___d_e_p_e_n_d_e_n_t(const Vector& vec1, const Vector& vec2, │ │ │ │ │ -double tol=1e-9); │ │ │ │ │ -182 │ │ │ │ │ -189GTSAM_EXPORT Vector _e_d_i_v__(const Vector &a, const Vector &b); │ │ │ │ │ -190 │ │ │ │ │ -194template │ │ │ │ │ -_1_9_5inline double _d_o_t(const V1 &a, const V2& b) { │ │ │ │ │ -196 assert (b.size()==a.size()); │ │ │ │ │ -197 return a.dot(b); │ │ │ │ │ -198} │ │ │ │ │ -199 │ │ │ │ │ -201template │ │ │ │ │ -_2_0_2inline double _i_n_n_e_r___p_r_o_d(const V1 &a, const V2& b) { │ │ │ │ │ -203 assert (b.size()==a.size()); │ │ │ │ │ -204 return a.dot(b); │ │ │ │ │ -205} │ │ │ │ │ -206 │ │ │ │ │ -207#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ -212inline void GTSAM_DEPRECATED scal(double alpha, Vector& x) { x *= alpha; } │ │ │ │ │ -213 │ │ │ │ │ -218template │ │ │ │ │ -219inline void GTSAM_DEPRECATED _a_x_p_y(double alpha, const V1& x, V2& y) { │ │ │ │ │ -220 assert (y.size()==x.size()); │ │ │ │ │ -221 y += alpha * x; │ │ │ │ │ -222} │ │ │ │ │ -223inline void _a_x_p_y(double alpha, const Vector& x, SubVector y) { │ │ │ │ │ -224 assert (y.size()==x.size()); │ │ │ │ │ -225 y += alpha * x; │ │ │ │ │ -226} │ │ │ │ │ -227#endif │ │ │ │ │ -228 │ │ │ │ │ -234GTSAM_EXPORT std::pair _h_o_u_s_e(const Vector &x); │ │ │ │ │ -235 │ │ │ │ │ -237GTSAM_EXPORT double _h_o_u_s_e_I_n_P_l_a_c_e(Vector &x); │ │ │ │ │ -238 │ │ │ │ │ -249GTSAM_EXPORT std::pair │ │ │ │ │ -250weightedPseudoinverse(const Vector& v, const Vector& weights); │ │ │ │ │ -251 │ │ │ │ │ -252/* │ │ │ │ │ -253 * Fast version *no error checking* ! │ │ │ │ │ -254 * Pass in initialized vector pseudo of size(weights) or will crash ! │ │ │ │ │ -255 * @return the precision, pseudoinverse in third argument │ │ │ │ │ -256 */ │ │ │ │ │ -257GTSAM_EXPORT double weightedPseudoinverse(const Vector& a, const Vector& │ │ │ │ │ -weights, Vector& pseudo); │ │ │ │ │ -258 │ │ │ │ │ -262GTSAM_EXPORT Vector _c_o_n_c_a_t_V_e_c_t_o_r_s(const std::list& vs); │ │ │ │ │ -263 │ │ │ │ │ -267GTSAM_EXPORT Vector _c_o_n_c_a_t_V_e_c_t_o_r_s(size_t nrVectors, ...); │ │ │ │ │ -268} // namespace gtsam │ │ │ │ │ +23// Change class depending on whether we are using TBB │ │ │ │ │ +24#ifdef GTSAM_USE_TBB │ │ │ │ │ +25 │ │ │ │ │ +26// Include TBB header │ │ │ │ │ +27# include │ │ │ │ │ +28# undef min // TBB seems to include Windows.h which defines these macros that │ │ │ │ │ +cause problems │ │ │ │ │ +29# undef max │ │ │ │ │ +30# undef ERROR │ │ │ │ │ +31 │ │ │ │ │ +32#include // std::hash() │ │ │ │ │ +33 │ │ │ │ │ +34// Use TBB concurrent_unordered_map for ConcurrentMap │ │ │ │ │ +35template │ │ │ │ │ +36using _C_o_n_c_u_r_r_e_n_t_M_a_p_B_a_s_e = tbb::concurrent_unordered_map< │ │ │ │ │ +37 KEY, │ │ │ │ │ +38 VALUE, │ │ │ │ │ +39 std::hash │ │ │ │ │ +40 >; │ │ │ │ │ +41 │ │ │ │ │ +42#else │ │ │ │ │ +43 │ │ │ │ │ +44// If we're not using TBB, use a FastMap for ConcurrentMap │ │ │ │ │ +45#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_M_a_p_._h> │ │ │ │ │ +46template │ │ │ │ │ +47using _C_o_n_c_u_r_r_e_n_t_M_a_p_B_a_s_e = _g_t_s_a_m_:_:_F_a_s_t_M_a_p_<_K_E_Y_,_ _V_A_L_U_E_>; │ │ │ │ │ +48 │ │ │ │ │ +49#endif │ │ │ │ │ +50 │ │ │ │ │ +51#include │ │ │ │ │ +52#include │ │ │ │ │ +53#include │ │ │ │ │ +54 │ │ │ │ │ +55#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ +56 │ │ │ │ │ +57namespace _g_t_s_a_m { │ │ │ │ │ +58 │ │ │ │ │ +67template │ │ │ │ │ +_6_8class _C_o_n_c_u_r_r_e_n_t_M_a_p : public _C_o_n_c_u_r_r_e_n_t_M_a_p_B_a_s_e { │ │ │ │ │ +69 │ │ │ │ │ +70public: │ │ │ │ │ +71 │ │ │ │ │ +72 typedef _C_o_n_c_u_r_r_e_n_t_M_a_p_B_a_s_e_<_K_E_Y_,_V_A_L_U_E_> Base; │ │ │ │ │ +73 │ │ │ │ │ +_7_5 _C_o_n_c_u_r_r_e_n_t_M_a_p() {} │ │ │ │ │ +76 │ │ │ │ │ +78 template │ │ │ │ │ +_7_9 _C_o_n_c_u_r_r_e_n_t_M_a_p(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) │ │ │ │ │ +{} │ │ │ │ │ +80 │ │ │ │ │ +_8_2 _C_o_n_c_u_r_r_e_n_t_M_a_p(const _C_o_n_c_u_r_r_e_n_t_M_a_p_<_K_E_Y_,_V_A_L_U_E_>& x) : Base(x) {} │ │ │ │ │ +83 │ │ │ │ │ +_8_5 _C_o_n_c_u_r_r_e_n_t_M_a_p(const Base& x) : Base(x) {} │ │ │ │ │ +86 │ │ │ │ │ +_8_8 bool _e_x_i_s_t_s(const KEY& e) const { return this->count(e); } │ │ │ │ │ +89 │ │ │ │ │ +90#ifndef GTSAM_USE_TBB │ │ │ │ │ +91 // If we're not using TBB and this is actually a FastMap, we need to add │ │ │ │ │ +these functions and hide │ │ │ │ │ +92 // the original erase functions. │ │ │ │ │ +93 void unsafe_erase(typename Base::iterator position) { ((Base*)this)->erase │ │ │ │ │ +(position); } │ │ │ │ │ +94 typename Base::size_type unsafe_erase(const KEY& k) { return ((Base*)this)- │ │ │ │ │ +>erase(k); } │ │ │ │ │ +95 void unsafe_erase(typename Base::iterator first, typename Base::iterator │ │ │ │ │ +last) { │ │ │ │ │ +96 return ((Base*)this)->erase(first, last); } │ │ │ │ │ +97private: │ │ │ │ │ +98 void erase() {} │ │ │ │ │ +99public: │ │ │ │ │ +100#endif │ │ │ │ │ +101 │ │ │ │ │ +102private: │ │ │ │ │ +_1_0_4 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +105 template │ │ │ │ │ +106 void save(Archive& ar, const unsigned int /*version*/) const │ │ │ │ │ +107 { │ │ │ │ │ +108 // Copy to an STL container and serialize that │ │ │ │ │ +109 _F_a_s_t_V_e_c_t_o_r_<_s_t_d_:_:_p_a_i_r_<_K_E_Y_,_ _V_A_L_U_E_> > map(this->size()); │ │ │ │ │ +110 std::copy(this->begin(), this->end(), map.begin()); │ │ │ │ │ +111 ar & BOOST_SERIALIZATION_NVP(map); │ │ │ │ │ +112 } │ │ │ │ │ +113 template │ │ │ │ │ +114 void load(Archive& ar, const unsigned int /*version*/) │ │ │ │ │ +115 { │ │ │ │ │ +116 this->clear(); │ │ │ │ │ +117 // Load into STL container and then fill our map │ │ │ │ │ +118 _F_a_s_t_V_e_c_t_o_r_<_s_t_d_:_:_p_a_i_r_<_K_E_Y_,_ _V_A_L_U_E_> > map; │ │ │ │ │ +119 ar & BOOST_SERIALIZATION_NVP(map); │ │ │ │ │ +120 this->insert(map.begin(), map.end()); │ │ │ │ │ +121 } │ │ │ │ │ +122 BOOST_SERIALIZATION_SPLIT_MEMBER() │ │ │ │ │ +123}; │ │ │ │ │ +124 │ │ │ │ │ +125} │ │ │ │ │ +_F_a_s_t_M_a_p_._h │ │ │ │ │ +A thin wrapper around std::map that uses boost's fast_pool_allocator. │ │ │ │ │ +_F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ +A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ _g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ Included from all GTSAM files. │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ +std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ +FastVector │ │ │ │ │ +FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ +DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_e_d_i_v__ │ │ │ │ │ -Vector ediv_(const Vector &a, const Vector &b) │ │ │ │ │ -elementwise division, but 0/0 = 0, not inf │ │ │ │ │ -DDeeffiinniittiioonn Vector.cpp:199 │ │ │ │ │ -_g_t_s_a_m_:_:_g_r_e_a_t_e_r_T_h_a_n_O_r_E_q_u_a_l │ │ │ │ │ -bool greaterThanOrEqual(const Vector &vec1, const Vector &vec2) │ │ │ │ │ -Greater than or equal to operation returns true if all elements in v1 are │ │ │ │ │ -greater than corresponding ... │ │ │ │ │ -DDeeffiinniittiioonn Vector.cpp:114 │ │ │ │ │ -_g_t_s_a_m_:_:_s_a_v_e │ │ │ │ │ -void save(const Matrix &A, const string &s, const string &filename) │ │ │ │ │ -save a matrix to file, which can be loaded by matlab │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:167 │ │ │ │ │ -_g_t_s_a_m_:_:_a_s_s_e_r_t___e_q_u_a_l │ │ │ │ │ -bool assert_equal(const Matrix &expected, const Matrix &actual, double tol) │ │ │ │ │ -equals with an tolerance, prints out message if unequal │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:43 │ │ │ │ │ -_g_t_s_a_m_:_:_l_i_n_e_a_r___d_e_p_e_n_d_e_n_t │ │ │ │ │ -bool linear_dependent(const Matrix &A, const Matrix &B, double tol) │ │ │ │ │ -check whether the rows of two matrices are linear dependent │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:117 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_n_e_r___p_r_o_d │ │ │ │ │ -double inner_prod(const V1 &a, const V2 &b) │ │ │ │ │ -compatibility version for ublas' inner_prod() │ │ │ │ │ -DDeeffiinniittiioonn Vector.h:202 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_a_x_p_y │ │ │ │ │ -void axpy(double alpha, const Errors &x, Errors &y) │ │ │ │ │ -BLAS level 2 style AXPY, y := alpha*x + y │ │ │ │ │ -DDeeffiinniittiioonn Errors.cpp:111 │ │ │ │ │ -_g_t_s_a_m_:_:_a_s_s_e_r_t___i_n_e_q_u_a_l │ │ │ │ │ -bool assert_inequal(const Matrix &A, const Matrix &B, double tol) │ │ │ │ │ -inequals with an tolerance, prints out message if within tolerance │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:63 │ │ │ │ │ -_g_t_s_a_m_:_:_h_o_u_s_e_I_n_P_l_a_c_e │ │ │ │ │ -double houseInPlace(Vector &v) │ │ │ │ │ -beta = house(x) computes the HouseHolder vector in place │ │ │ │ │ -DDeeffiinniittiioonn Vector.cpp:212 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l │ │ │ │ │ -bool equal(const T &obj1, const T &obj2, double tol) │ │ │ │ │ -Call equal on the object. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:84 │ │ │ │ │ -_g_t_s_a_m_:_:_d_o_t │ │ │ │ │ -double dot(const V1 &a, const V2 &b) │ │ │ │ │ -Dot product. │ │ │ │ │ -DDeeffiinniittiioonn Vector.h:195 │ │ │ │ │ -_g_t_s_a_m_:_:_f_p_E_q_u_a_l │ │ │ │ │ -bool fpEqual(double a, double b, double tol, bool check_relative_also) │ │ │ │ │ -Ensure we are not including a different version of Eigen in user code than │ │ │ │ │ -while compiling gtsam,... │ │ │ │ │ -DDeeffiinniittiioonn Vector.cpp:42 │ │ │ │ │ -_g_t_s_a_m_:_:_c_o_n_c_a_t_V_e_c_t_o_r_s │ │ │ │ │ -Vector concatVectors(const std::list< Vector > &vs) │ │ │ │ │ -concatenate Vectors │ │ │ │ │ -DDeeffiinniittiioonn Vector.cpp:302 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l___w_i_t_h___a_b_s___t_o_l │ │ │ │ │ -bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen:: │ │ │ │ │ -DenseBase< MATRIX > &B, double tol=1e-9) │ │ │ │ │ -equals with a tolerance │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:81 │ │ │ │ │ -_g_t_s_a_m_:_:_h_o_u_s_e │ │ │ │ │ -pair< double, Vector > house(const Vector &x) │ │ │ │ │ -house(x,j) computes HouseHolder vector v and scaling factor beta from x, such │ │ │ │ │ -that the corresponding ... │ │ │ │ │ -DDeeffiinniittiioonn Vector.cpp:237 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_=_= │ │ │ │ │ -bool operator==(const Matrix &A, const Matrix &B) │ │ │ │ │ -equality is just equal_with_abs_tol 1e-9 │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_c_u_r_r_e_n_t_M_a_p │ │ │ │ │ +FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ +fast_pool_allocator instead of the defa... │ │ │ │ │ +DDeeffiinniittiioonn ConcurrentMap.h:68 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_c_u_r_r_e_n_t_M_a_p_:_:_C_o_n_c_u_r_r_e_n_t_M_a_p │ │ │ │ │ +ConcurrentMap(INPUTITERATOR first, INPUTITERATOR last) │ │ │ │ │ +Constructor from a range, passes through to base class. │ │ │ │ │ +DDeeffiinniittiioonn ConcurrentMap.h:79 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_c_u_r_r_e_n_t_M_a_p_:_:_C_o_n_c_u_r_r_e_n_t_M_a_p │ │ │ │ │ +ConcurrentMap(const ConcurrentMap< KEY, VALUE > &x) │ │ │ │ │ +Copy constructor from another ConcurrentMap. │ │ │ │ │ +DDeeffiinniittiioonn ConcurrentMap.h:82 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_c_u_r_r_e_n_t_M_a_p_:_:_e_x_i_s_t_s │ │ │ │ │ +bool exists(const KEY &e) const │ │ │ │ │ +Handy 'exists' function. │ │ │ │ │ +DDeeffiinniittiioonn ConcurrentMap.h:88 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_c_u_r_r_e_n_t_M_a_p_:_:_C_o_n_c_u_r_r_e_n_t_M_a_p │ │ │ │ │ +ConcurrentMap(const Base &x) │ │ │ │ │ +Copy constructor from the base map class. │ │ │ │ │ +DDeeffiinniittiioonn ConcurrentMap.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_c_u_r_r_e_n_t_M_a_p_:_:_C_o_n_c_u_r_r_e_n_t_M_a_p │ │ │ │ │ +ConcurrentMap() │ │ │ │ │ +Default constructor. │ │ │ │ │ +DDeeffiinniittiioonn ConcurrentMap.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_c_u_r_r_e_n_t_M_a_p_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn ConcurrentMap.h:104 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ +FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ +fast_pool_allocator instead of the defa... │ │ │ │ │ +DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _V_e_c_t_o_r_._h │ │ │ │ │ + * CCoonnccuurrrreennttMMaapp..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00068.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/types.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Value.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,43 +94,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
types.cpp File Reference
Base
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
Value.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Functions for handling type information. │ │ │ │ +

The base class for any variable that can be optimized or used in a factor. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::Value
 This is the base class for any type to be stored in Values. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

std::string gtsam::demangle (const char *name)
 Pretty print Value type name.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Functions for handling type information.

│ │ │ │ -
Author
Varun Agrawal
│ │ │ │ -
Date
May 18, 2020
│ │ │ │ +

The base class for any variable that can be optimized or used in a factor.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Jan 14, 2012
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,27 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -types.cpp File Reference │ │ │ │ │ -_B_a_s_e │ │ │ │ │ -Functions for handling type information. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +Value.h File Reference │ │ │ │ │ +The base class for any variable that can be optimized or used in a factor. │ │ │ │ │ +_M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_V_a_l_u_e │ │ │ │ │ +  This is the base class for any type to be stored in _V_a_l_u_e_s. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -std::string  _g_t_s_a_m_:_:_d_e_m_a_n_g_l_e (const char *name) │ │ │ │ │ -  Pretty print _V_a_l_u_e type name. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Functions for handling type information. │ │ │ │ │ +The base class for any variable that can be optimized or used in a factor. │ │ │ │ │ Author │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + Richard Roberts │ │ │ │ │ Date │ │ │ │ │ - May 18, 2020 │ │ │ │ │ + Jan 14, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _t_y_p_e_s_._c_p_p │ │ │ │ │ + * _V_a_l_u_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00068.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a00068 = [ │ │ │ │ │ - ["demangle", "a00068.html#a0b97fc96e5f69236e81489c66d5b92ba", null] │ │ │ │ │ + ["gtsam::Value", "a02596.html", "a02596"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00071.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/FastMap.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Testable.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,44 +95,122 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
FastMap.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Macros | │ │ │ │ +Functions
│ │ │ │ +
Testable.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

A thin wrapper around std::map that uses boost's fast_pool_allocator. │ │ │ │ +

Concept check for values that can be used in unit tests. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::FastMap< KEY, VALUE >
 FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the default STL allocator. More...
class  gtsam::IsTestable< T >
 A testable concept check that should be placed in applicable unit tests and in generic algorithms. More...
 
struct  gtsam::equals< V >
 Template to create a binary predicate. More...
 
struct  gtsam::equals_star< V >
 Binary predicate on shared pointers. More...
 
struct  gtsam::HasTestablePrereqs< T >
 Requirements on type to pass it to Testable template below. More...
 
struct  gtsam::Testable< T >
 A helper that implements the traits interface for GTSAM types. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Macros

│ │ │ │ +#define GTSAM_PRINT(x)   ((x).print(#x))
 
#define GTSAM_CONCEPT_TESTABLE_INST(T)   template class gtsam::IsTestable<T>;
 Macros for using the TestableConcept.
 
│ │ │ │ +#define GTSAM_CONCEPT_TESTABLE_TYPE(T)   using _gtsam_Testable_##T = gtsam::IsTestable<T>;
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +void gtsam::print (float v, const std::string &s="")
 
│ │ │ │ +void gtsam::print (double v, const std::string &s="")
 
│ │ │ │ +template<class T >
bool gtsam::equal (const T &obj1, const T &obj2, double tol)
 Call equal on the object.
 
│ │ │ │ +template<class T >
bool gtsam::equal (const T &obj1, const T &obj2)
 Call equal without tolerance (use default tolerance)
 
│ │ │ │ +template<class V >
bool gtsam::assert_equal (const V &expected, const V &actual, double tol=1e-9)
 This template works for any type with equals.
 
│ │ │ │

Detailed Description

│ │ │ │ -

A thin wrapper around std::map that uses boost's fast_pool_allocator.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Oct 17, 2010
│ │ │ │ -
│ │ │ │ +

Concept check for values that can be used in unit tests.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +

The necessary functions to implement for Testable are defined below with additional details as to the interface. The concept checking function will check whether or not the function exists in derived class and throw compile-time errors.

│ │ │ │ +

print with optional string naming the object void print(const std::string& name) const = 0;

│ │ │ │ +

equality up to tolerance tricky to implement, see PriorFactor for an example equals is not supposed to print out anything, just return true|false bool equals(const Derived& expected, double tol) const = 0;

│ │ │ │ +

Macro Definition Documentation

│ │ │ │ + │ │ │ │ +

◆ GTSAM_CONCEPT_TESTABLE_INST

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
#define GTSAM_CONCEPT_TESTABLE_INST( T)   template class gtsam::IsTestable<T>;
│ │ │ │ +
│ │ │ │ + │ │ │ │ +

Macros for using the TestableConcept.

│ │ │ │ +
    │ │ │ │ +
  • An instantiation for use inside unit tests
    • │ │ │ │ +
  • A typedef for use inside generic algorithms
    • │ │ │ │ +
│ │ │ │ +

NOTE: intentionally not in the gtsam namespace to allow for classes not in the gtsam namespace to be more easily enforced as testable

Deprecated:
please use BOOST_CONCEPT_ASSERT and
│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,84 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -FastMap.h File Reference │ │ │ │ │ -A thin wrapper around std::map that uses boost's fast_pool_allocator. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Testable.h File Reference │ │ │ │ │ +Concept check for values that can be used in unit tests. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_F_a_s_t_M_a_p_<_ _K_E_Y_,_ _V_A_L_U_E_ _> │ │ │ │ │ -  _F_a_s_t_M_a_p is a thin wrapper around std::map that uses the boost │ │ │ │ │ - fast_pool_allocator instead of the default STL allocator. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_I_s_T_e_s_t_a_b_l_e_<_ _T_ _> │ │ │ │ │ +  A testable concept check that should be placed in applicable unit │ │ │ │ │ + tests and in generic algorithms. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_e_q_u_a_l_s_<_ _V_ _> │ │ │ │ │ +  Template to create a binary predicate. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_e_q_u_a_l_s___s_t_a_r_<_ _V_ _> │ │ │ │ │ +  Binary predicate on shared pointers. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_H_a_s_T_e_s_t_a_b_l_e_P_r_e_r_e_q_s_<_ _T_ _> │ │ │ │ │ +  Requirements on type to pass it to _T_e_s_t_a_b_l_e template below. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_T_e_s_t_a_b_l_e_<_ _T_ _> │ │ │ │ │ +  A helper that implements the traits interface for GTSAM types. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +MMaaccrrooss │ │ │ │ │ +#define  GGTTSSAAMM__PPRRIINNTT(x)   ((x).print(#x)) │ │ │ │ │ +  │ │ │ │ │ +#define  _G_T_S_A_M___C_O_N_C_E_P_T___T_E_S_T_A_B_L_E___I_N_S_T(T)   template class _g_t_s_a_m_:_:_I_s_T_e_s_t_a_b_l_e; │ │ │ │ │ +  Macros for using the TestableConcept. │ │ │ │ │ +  │ │ │ │ │ +#define  GGTTSSAAMM__CCOONNCCEEPPTT__TTEESSTTAABBLLEE__TTYYPPEE(T)   using _gtsam_Testable_##T = _g_t_s_a_m_:_: │ │ │ │ │ + _I_s_T_e_s_t_a_b_l_e; │ │ │ │ │ +  │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +void  ggttssaamm::::pprriinntt (float v, const std::string &s="") │ │ │ │ │ +  │ │ │ │ │ +void  ggttssaamm::::pprriinntt (double v, const std::string &s="") │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +bool  ggttssaamm::::eeqquuaall (const T &obj1, const T &obj2, double tol) │ │ │ │ │ +  Call equal on the object. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +bool  ggttssaamm::::eeqquuaall (const T &obj1, const T &obj2) │ │ │ │ │ +  Call equal without tolerance (use default tolerance) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +bool  ggttssaamm::::aasssseerrtt__eeqquuaall (const V &expected, const V &actual, double tol=1e-9) │ │ │ │ │ +  This template works for any type with equals. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A thin wrapper around std::map that uses boost's fast_pool_allocator. │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Date │ │ │ │ │ - Oct 17, 2010 │ │ │ │ │ + Frank Dellaert │ │ │ │ │ +The necessary functions to implement for Testable are defined below with │ │ │ │ │ +additional details as to the interface. The concept checking function will │ │ │ │ │ +check whether or not the function exists in derived class and throw compile- │ │ │ │ │ +time errors. │ │ │ │ │ +print with optional string naming the object void print(const std::string& │ │ │ │ │ +name) const = 0; │ │ │ │ │ +equality up to tolerance tricky to implement, see PriorFactor for an example │ │ │ │ │ +equals is not supposed to print out aannyytthhiinngg, just return true|false bool │ │ │ │ │ +equals(const Derived& expected, double tol) const = 0; │ │ │ │ │ +********** MMaaccrroo DDeeffiinniittiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ +********** _?◆_? GGTTSSAAMM__CCOONNCCEEPPTT__TTEESSTTAABBLLEE__IINNSSTT ********** │ │ │ │ │ +#define GTSAM_CONCEPT_TESTABLE_INST (   T )    template class _g_t_s_a_m_:_: │ │ │ │ │ + _I_s_T_e_s_t_a_b_l_e; │ │ │ │ │ +Macros for using the TestableConcept. │ │ │ │ │ + * An instantiation for use inside unit tests │ │ │ │ │ + * A typedef for use inside generic algorithms │ │ │ │ │ +NOTE: intentionally not in the gtsam namespace to allow for classes not in the │ │ │ │ │ +gtsam namespace to be more easily enforced as testable │ │ │ │ │ + _DD_ee_pp_rr_ee_cc_aa_tt_ee_dd_:: │ │ │ │ │ + please use BOOST_CONCEPT_ASSERT and │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _F_a_s_t_M_a_p_._h │ │ │ │ │ + * _T_e_s_t_a_b_l_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00071_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/FastMap.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Testable.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,91 +98,171 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
FastMap.h
│ │ │ │ +
Testable.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/base/FastDefaultAllocator.h>
│ │ │ │ -
22#include <boost/serialization/nvp.hpp>
│ │ │ │ -
23#include <boost/serialization/map.hpp>
│ │ │ │ -
24#include <map>
│ │ │ │ -
25
│ │ │ │ -
26namespace gtsam {
│ │ │ │ -
27
│ │ │ │ -
36template<typename KEY, typename VALUE>
│ │ │ │ -
│ │ │ │ -
37class FastMap : public std::map<KEY, VALUE, std::less<KEY>,
│ │ │ │ -
38 typename internal::FastDefaultAllocator<std::pair<const KEY, VALUE> >::type> {
│ │ │ │ -
39
│ │ │ │ -
40public:
│ │ │ │ +
32// \callgraph
│ │ │ │ +
33
│ │ │ │ +
34#pragma once
│ │ │ │ +
35
│ │ │ │ +
36#include <boost/concept_check.hpp>
│ │ │ │ +
37#include <functional>
│ │ │ │ +
38#include <iostream>
│ │ │ │ +
39#include <memory>
│ │ │ │ +
40#include <string>
│ │ │ │
41
│ │ │ │ -
42 typedef std::map<KEY, VALUE, std::less<KEY>,
│ │ │ │ -
43 typename internal::FastDefaultAllocator<std::pair<const KEY, VALUE> >::type > Base;
│ │ │ │ -
44
│ │ │ │ -
46 FastMap() {}
│ │ │ │ -
47
│ │ │ │ -
49 template<typename INPUTITERATOR>
│ │ │ │ -
50 explicit FastMap(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {}
│ │ │ │ -
51
│ │ │ │ -
53 FastMap(const FastMap<KEY,VALUE>& x) : Base(x) {}
│ │ │ │ -
54
│ │ │ │ -
56 FastMap(const Base& x) : Base(x) {}
│ │ │ │ -
57
│ │ │ │ -
│ │ │ │ -
59 operator std::map<KEY,VALUE>() const {
│ │ │ │ -
60 return std::map<KEY,VALUE>(this->begin(), this->end());
│ │ │ │ -
61 }
│ │ │ │ -
│ │ │ │ +
42#define GTSAM_PRINT(x)((x).print(#x))
│ │ │ │ +
43
│ │ │ │ +
44namespace gtsam {
│ │ │ │ +
45
│ │ │ │ +
46 // Forward declaration
│ │ │ │ +
47 template <typename T> struct traits;
│ │ │ │ +
48
│ │ │ │ +
57 template <class T>
│ │ │ │ +
│ │ │ │ +
58 class IsTestable {
│ │ │ │ +
59 T t;
│ │ │ │ +
60 bool r1,r2;
│ │ │ │ +
61 public:
│ │ │ │
62
│ │ │ │ -
64 bool insert2(const KEY& key, const VALUE& val) { return Base::insert(std::make_pair(key, val)).second; }
│ │ │ │ -
65
│ │ │ │ -
67 bool exists(const KEY& e) const { return this->find(e) != this->end(); }
│ │ │ │ -
68
│ │ │ │ -
69private:
│ │ │ │ -
71 friend class boost::serialization::access;
│ │ │ │ -
72 template<class ARCHIVE>
│ │ │ │ -
73 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
74 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
75 }
│ │ │ │ -
76};
│ │ │ │ -
│ │ │ │ -
77
│ │ │ │ -
78}
│ │ │ │ -
FastDefaultAllocator.h
An easy way to control which allocator is used for Fast* collections.
│ │ │ │ +
63 BOOST_CONCEPT_USAGE(IsTestable) {
│ │ │ │ +
64 // check print function, with optional string
│ │ │ │ +
65 traits<T>::Print(t, std::string());
│ │ │ │ +
66 traits<T>::Print(t);
│ │ │ │ +
67
│ │ │ │ +
68 // check print, with optional threshold
│ │ │ │ +
69 double tol = 1.0;
│ │ │ │ +
70 r1 = traits<T>::Equals(t,t,tol);
│ │ │ │ +
71 r2 = traits<T>::Equals(t,t);
│ │ │ │ +
72 }
│ │ │ │ +
73 }; // \ Testable
│ │ │ │ +
│ │ │ │ +
74
│ │ │ │ +
75 inline void print(float v, const std::string& s = "") {
│ │ │ │ +
76 std::cout << (s.empty() ? s : s + " ") << v << std::endl;
│ │ │ │ +
77 }
│ │ │ │ +
78 inline void print(double v, const std::string& s = "") {
│ │ │ │ +
79 std::cout << (s.empty() ? s : s + " ") << v << std::endl;
│ │ │ │ +
80 }
│ │ │ │ +
81
│ │ │ │ +
83 template<class T>
│ │ │ │ +
│ │ │ │ +
84 inline bool equal(const T& obj1, const T& obj2, double tol) {
│ │ │ │ +
85 return traits<T>::Equals(obj1,obj2, tol);
│ │ │ │ +
86 }
│ │ │ │ +
│ │ │ │ +
87
│ │ │ │ +
89 template<class T>
│ │ │ │ +
│ │ │ │ +
90 inline bool equal(const T& obj1, const T& obj2) {
│ │ │ │ +
91 return traits<T>::Equals(obj1,obj2);
│ │ │ │ +
92 }
│ │ │ │ +
│ │ │ │ +
93
│ │ │ │ +
97 template<class V>
│ │ │ │ +
│ │ │ │ +
98 bool assert_equal(const V& expected, const V& actual, double tol = 1e-9) {
│ │ │ │ +
99 if (traits<V>::Equals(actual,expected, tol))
│ │ │ │ +
100 return true;
│ │ │ │ +
101 printf("Not equal:\n");
│ │ │ │ +
102 traits<V>::Print(expected,"expected:\n");
│ │ │ │ +
103 traits<V>::Print(actual,"actual:\n");
│ │ │ │ +
104 return false;
│ │ │ │ +
105 }
│ │ │ │ +
│ │ │ │ +
106
│ │ │ │ +
110 template<class V>
│ │ │ │ +
│ │ │ │ +
111 struct equals : public std::function<bool(const V&, const V&)> {
│ │ │ │ +
112 double tol_;
│ │ │ │ +
113 equals(double tol = 1e-9) : tol_(tol) {}
│ │ │ │ +
114 bool operator()(const V& expected, const V& actual) {
│ │ │ │ +
115 return (traits<V>::Equals(actual, expected, tol_));
│ │ │ │ +
116 }
│ │ │ │ +
117 };
│ │ │ │ +
│ │ │ │ +
118
│ │ │ │ +
122 template<class V>
│ │ │ │ +
│ │ │ │ +
123 struct equals_star : public std::function<bool(const std::shared_ptr<V>&, const std::shared_ptr<V>&)> {
│ │ │ │ +
124 double tol_;
│ │ │ │ +
125 equals_star(double tol = 1e-9) : tol_(tol) {}
│ │ │ │ +
126 bool operator()(const std::shared_ptr<V>& expected, const std::shared_ptr<V>& actual) {
│ │ │ │ +
127 if (!actual && !expected) return true;
│ │ │ │ +
128 return actual && expected && traits<V>::Equals(*actual,*expected, tol_);
│ │ │ │ +
129 }
│ │ │ │ +
130 };
│ │ │ │ +
│ │ │ │ +
131
│ │ │ │ +
133 template<typename T>
│ │ │ │ +
│ │ │ │ +
134 struct HasTestablePrereqs {
│ │ │ │ +
135
│ │ │ │ +
136 BOOST_CONCEPT_USAGE(HasTestablePrereqs) {
│ │ │ │ +
137 t->print(str);
│ │ │ │ +
138 b = t->equals(*s,tol);
│ │ │ │ +
139 }
│ │ │ │ +
140
│ │ │ │ +
141 T *t, *s; // Pointer is to allow abstract classes
│ │ │ │ +
142 bool b;
│ │ │ │ +
143 double tol;
│ │ │ │ +
144 std::string str;
│ │ │ │ +
145 };
│ │ │ │ +
│ │ │ │ +
146
│ │ │ │ +
150 template<typename T>
│ │ │ │ +
│ │ │ │ +
151 struct Testable {
│ │ │ │ +
152
│ │ │ │ +
153 // Check that T has the necessary methods
│ │ │ │ +
154 BOOST_CONCEPT_ASSERT((HasTestablePrereqs<T>));
│ │ │ │ +
155
│ │ │ │ +
156 static void Print(const T& m, const std::string& str = "") {
│ │ │ │ +
157 m.print(str);
│ │ │ │ +
158 }
│ │ │ │ +
159 static bool Equals(const T& m1, const T& m2, double tol = 1e-8) {
│ │ │ │ +
160 return m1.equals(m2, tol);
│ │ │ │ +
161 }
│ │ │ │ +
162 };
│ │ │ │ +
│ │ │ │ +
163
│ │ │ │ +
164} // \namespace gtsam
│ │ │ │ +
165
│ │ │ │ +
175#define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::IsTestable<T>;
│ │ │ │ +
176#define GTSAM_CONCEPT_TESTABLE_TYPE(T) using _gtsam_Testable_##T = gtsam::IsTestable<T>;
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::internal::FastDefaultAllocator
Default allocator for list, map, and set types.
Definition FastDefaultAllocator.h:50
│ │ │ │ -
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ -
gtsam::FastMap::FastMap
FastMap(const Base &x)
Copy constructor from the base map class.
Definition FastMap.h:56
│ │ │ │ -
gtsam::FastMap::FastMap
FastMap(INPUTITERATOR first, INPUTITERATOR last)
Constructor from a range, passes through to base class.
Definition FastMap.h:50
│ │ │ │ -
gtsam::FastMap::insert2
bool insert2(const KEY &key, const VALUE &val)
Handy 'insert' function for Matlab wrapper.
Definition FastMap.h:64
│ │ │ │ -
gtsam::FastMap::FastMap
FastMap(const FastMap< KEY, VALUE > &x)
Copy constructor from another FastMap.
Definition FastMap.h:53
│ │ │ │ -
gtsam::FastMap::FastMap
FastMap()
Default constructor.
Definition FastMap.h:46
│ │ │ │ -
gtsam::FastMap::exists
bool exists(const KEY &e) const
Handy 'exists' function.
Definition FastMap.h:67
│ │ │ │ -
gtsam::FastMap::access
friend class boost::serialization::access
Serialization function.
Definition FastMap.h:71
│ │ │ │ +
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
equals with an tolerance, prints out message if unequal
Definition Matrix.cpp:43
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::equal
bool equal(const T &obj1, const T &obj2, double tol)
Call equal on the object.
Definition Testable.h:84
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::IsTestable
A testable concept check that should be placed in applicable unit tests and in generic algorithms.
Definition Testable.h:58
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::equals_star
Binary predicate on shared pointers.
Definition Testable.h:123
│ │ │ │ +
gtsam::HasTestablePrereqs
Requirements on type to pass it to Testable template below.
Definition Testable.h:134
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,112 +1,175 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -FastMap.h │ │ │ │ │ +Testable.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_._h> │ │ │ │ │ -22#include │ │ │ │ │ -23#include │ │ │ │ │ -24#include │ │ │ │ │ -25 │ │ │ │ │ -26namespace _g_t_s_a_m { │ │ │ │ │ -27 │ │ │ │ │ -36template │ │ │ │ │ -_3_7class _F_a_s_t_M_a_p : public std::map, │ │ │ │ │ -38 typename internal::FastDefaultAllocator >::type> │ │ │ │ │ -{ │ │ │ │ │ -39 │ │ │ │ │ -40public: │ │ │ │ │ +32// \callgraph │ │ │ │ │ +33 │ │ │ │ │ +34#pragma once │ │ │ │ │ +35 │ │ │ │ │ +36#include │ │ │ │ │ +37#include │ │ │ │ │ +38#include │ │ │ │ │ +39#include │ │ │ │ │ +40#include │ │ │ │ │ 41 │ │ │ │ │ -42 typedef std::map, │ │ │ │ │ -43 typename _i_n_t_e_r_n_a_l_:_:_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_<_s_t_d_:_:_p_a_i_r_<_c_o_n_s_t_ _K_E_Y_,_ _V_A_L_U_E_> >::type │ │ │ │ │ -> Base; │ │ │ │ │ -44 │ │ │ │ │ -_4_6 _F_a_s_t_M_a_p() {} │ │ │ │ │ -47 │ │ │ │ │ -49 template │ │ │ │ │ -_5_0 explicit _F_a_s_t_M_a_p(INPUTITERATOR first, INPUTITERATOR last) : Base(first, │ │ │ │ │ -last) {} │ │ │ │ │ -51 │ │ │ │ │ -_5_3 _F_a_s_t_M_a_p(const _F_a_s_t_M_a_p_<_K_E_Y_,_V_A_L_U_E_>& x) : Base(x) {} │ │ │ │ │ -54 │ │ │ │ │ -_5_6 _F_a_s_t_M_a_p(const Base& x) : Base(x) {} │ │ │ │ │ -57 │ │ │ │ │ -_5_9 operator std::map() const { │ │ │ │ │ -60 return std::map(this->begin(), this->end()); │ │ │ │ │ -61 } │ │ │ │ │ +42#define GTSAM_PRINT(x)((x).print(#x)) │ │ │ │ │ +43 │ │ │ │ │ +44namespace _g_t_s_a_m { │ │ │ │ │ +45 │ │ │ │ │ +46 // Forward declaration │ │ │ │ │ +47 template struct traits; │ │ │ │ │ +48 │ │ │ │ │ +57 template │ │ │ │ │ +_5_8 class _I_s_T_e_s_t_a_b_l_e { │ │ │ │ │ +59 T t; │ │ │ │ │ +60 bool r1,r2; │ │ │ │ │ +61 public: │ │ │ │ │ 62 │ │ │ │ │ -_6_4 bool _i_n_s_e_r_t_2(const KEY& key, const VALUE& val) { return Base::insert(std:: │ │ │ │ │ -make_pair(key, val)).second; } │ │ │ │ │ -65 │ │ │ │ │ -_6_7 bool _e_x_i_s_t_s(const KEY& e) const { return this->find(e) != this->end(); } │ │ │ │ │ -68 │ │ │ │ │ -69private: │ │ │ │ │ -_7_1 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -72 template │ │ │ │ │ -73 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -74 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); │ │ │ │ │ -75 } │ │ │ │ │ -76}; │ │ │ │ │ -77 │ │ │ │ │ -78} │ │ │ │ │ -_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_._h │ │ │ │ │ -An easy way to control which allocator is used for Fast* collections. │ │ │ │ │ +63 BOOST_CONCEPT_USAGE(_I_s_T_e_s_t_a_b_l_e) { │ │ │ │ │ +64 // check print function, with optional string │ │ │ │ │ +65 _t_r_a_i_t_s_<_T_>_:_:_P_r_i_n_t(t, std::string()); │ │ │ │ │ +66 _t_r_a_i_t_s_<_T_>_:_:_P_r_i_n_t(t); │ │ │ │ │ +67 │ │ │ │ │ +68 // check print, with optional threshold │ │ │ │ │ +69 double tol = 1.0; │ │ │ │ │ +70 r1 = _t_r_a_i_t_s_<_T_>_:_:_E_q_u_a_l_s(t,t,tol); │ │ │ │ │ +71 r2 = _t_r_a_i_t_s_<_T_>_:_:_E_q_u_a_l_s(t,t); │ │ │ │ │ +72 } │ │ │ │ │ +73 }; // \ Testable │ │ │ │ │ +74 │ │ │ │ │ +75 inline void _p_r_i_n_t(float v, const std::string& s = "") { │ │ │ │ │ +76 std::cout << (s.empty() ? s : s + " ") << v << std::endl; │ │ │ │ │ +77 } │ │ │ │ │ +78 inline void _p_r_i_n_t(double v, const std::string& s = "") { │ │ │ │ │ +79 std::cout << (s.empty() ? s : s + " ") << v << std::endl; │ │ │ │ │ +80 } │ │ │ │ │ +81 │ │ │ │ │ +83 template │ │ │ │ │ +_8_4 inline bool _e_q_u_a_l(const T& obj1, const T& obj2, double tol) { │ │ │ │ │ +85 return _t_r_a_i_t_s_<_T_>_:_:_E_q_u_a_l_s(obj1,obj2, tol); │ │ │ │ │ +86 } │ │ │ │ │ +87 │ │ │ │ │ +89 template │ │ │ │ │ +_9_0 inline bool _e_q_u_a_l(const T& obj1, const T& obj2) { │ │ │ │ │ +91 return _t_r_a_i_t_s_<_T_>_:_:_E_q_u_a_l_s(obj1,obj2); │ │ │ │ │ +92 } │ │ │ │ │ +93 │ │ │ │ │ +97 template │ │ │ │ │ +_9_8 bool _a_s_s_e_r_t___e_q_u_a_l(const V& expected, const V& actual, double tol = 1e-9) { │ │ │ │ │ +99 if (_t_r_a_i_t_s_<_V_>_:_:_E_q_u_a_l_s(actual,expected, tol)) │ │ │ │ │ +100 return true; │ │ │ │ │ +101 printf("Not equal:\n"); │ │ │ │ │ +102 _t_r_a_i_t_s_<_V_>_:_:_P_r_i_n_t(expected,"expected:\n"); │ │ │ │ │ +103 _t_r_a_i_t_s_<_V_>_:_:_P_r_i_n_t(actual,"actual:\n"); │ │ │ │ │ +104 return false; │ │ │ │ │ +105 } │ │ │ │ │ +106 │ │ │ │ │ +110 template │ │ │ │ │ +_1_1_1 struct _e_q_u_a_l_s : public std::function { │ │ │ │ │ +112 double tol_; │ │ │ │ │ +113 _e_q_u_a_l_s(double tol = 1e-9) : tol_(tol) {} │ │ │ │ │ +114 bool operator()(const V& expected, const V& actual) { │ │ │ │ │ +115 return (_t_r_a_i_t_s_<_V_>_:_:_E_q_u_a_l_s(actual, expected, tol_)); │ │ │ │ │ +116 } │ │ │ │ │ +117 }; │ │ │ │ │ +118 │ │ │ │ │ +122 template │ │ │ │ │ +_1_2_3 struct _e_q_u_a_l_s___s_t_a_r : public std::function&, │ │ │ │ │ +const std::shared_ptr&)> { │ │ │ │ │ +124 double tol_; │ │ │ │ │ +125 _e_q_u_a_l_s___s_t_a_r(double tol = 1e-9) : tol_(tol) {} │ │ │ │ │ +126 bool operator()(const std::shared_ptr& expected, const std:: │ │ │ │ │ +shared_ptr& actual) { │ │ │ │ │ +127 if (!actual && !expected) return true; │ │ │ │ │ +128 return actual && expected && _t_r_a_i_t_s_<_V_>_:_:_E_q_u_a_l_s(*actual,*expected, tol_); │ │ │ │ │ +129 } │ │ │ │ │ +130 }; │ │ │ │ │ +131 │ │ │ │ │ +133 template │ │ │ │ │ +_1_3_4 struct _H_a_s_T_e_s_t_a_b_l_e_P_r_e_r_e_q_s { │ │ │ │ │ +135 │ │ │ │ │ +136 BOOST_CONCEPT_USAGE(_H_a_s_T_e_s_t_a_b_l_e_P_r_e_r_e_q_s) { │ │ │ │ │ +137 t->print(str); │ │ │ │ │ +138 b = t->equals(*s,tol); │ │ │ │ │ +139 } │ │ │ │ │ +140 │ │ │ │ │ +141 T *t, *s; // Pointer is to allow abstract classes │ │ │ │ │ +142 bool b; │ │ │ │ │ +143 double tol; │ │ │ │ │ +144 std::string str; │ │ │ │ │ +145 }; │ │ │ │ │ +146 │ │ │ │ │ +150 template │ │ │ │ │ +_1_5_1 struct _T_e_s_t_a_b_l_e { │ │ │ │ │ +152 │ │ │ │ │ +153 // Check that T has the necessary methods │ │ │ │ │ +154 BOOST_CONCEPT_ASSERT((_H_a_s_T_e_s_t_a_b_l_e_P_r_e_r_e_q_s_<_T_>)); │ │ │ │ │ +155 │ │ │ │ │ +156 static void Print(const T& m, const std::string& str = "") { │ │ │ │ │ +157 m.print(str); │ │ │ │ │ +158 } │ │ │ │ │ +159 static bool Equals(const T& m1, const T& m2, double tol = 1e-8) { │ │ │ │ │ +160 return m1.equals(m2, tol); │ │ │ │ │ +161 } │ │ │ │ │ +162 }; │ │ │ │ │ +163 │ │ │ │ │ +164} // \namespace gtsam │ │ │ │ │ +165 │ │ │ │ │ +_1_7_5#define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::IsTestable; │ │ │ │ │ +176#define GTSAM_CONCEPT_TESTABLE_TYPE(T) using _gtsam_Testable_##T = gtsam:: │ │ │ │ │ +IsTestable; │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r │ │ │ │ │ -Default allocator for list, map, and set types. │ │ │ │ │ -DDeeffiinniittiioonn FastDefaultAllocator.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ -FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ -fast_pool_allocator instead of the defa... │ │ │ │ │ -DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_M_a_p_:_:_F_a_s_t_M_a_p │ │ │ │ │ -FastMap(const Base &x) │ │ │ │ │ -Copy constructor from the base map class. │ │ │ │ │ -DDeeffiinniittiioonn FastMap.h:56 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_M_a_p_:_:_F_a_s_t_M_a_p │ │ │ │ │ -FastMap(INPUTITERATOR first, INPUTITERATOR last) │ │ │ │ │ -Constructor from a range, passes through to base class. │ │ │ │ │ -DDeeffiinniittiioonn FastMap.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_M_a_p_:_:_i_n_s_e_r_t_2 │ │ │ │ │ -bool insert2(const KEY &key, const VALUE &val) │ │ │ │ │ -Handy 'insert' function for Matlab wrapper. │ │ │ │ │ -DDeeffiinniittiioonn FastMap.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_M_a_p_:_:_F_a_s_t_M_a_p │ │ │ │ │ -FastMap(const FastMap< KEY, VALUE > &x) │ │ │ │ │ -Copy constructor from another FastMap. │ │ │ │ │ -DDeeffiinniittiioonn FastMap.h:53 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_M_a_p_:_:_F_a_s_t_M_a_p │ │ │ │ │ -FastMap() │ │ │ │ │ -Default constructor. │ │ │ │ │ -DDeeffiinniittiioonn FastMap.h:46 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_M_a_p_:_:_e_x_i_s_t_s │ │ │ │ │ -bool exists(const KEY &e) const │ │ │ │ │ -Handy 'exists' function. │ │ │ │ │ -DDeeffiinniittiioonn FastMap.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_M_a_p_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn FastMap.h:71 │ │ │ │ │ +_g_t_s_a_m_:_:_a_s_s_e_r_t___e_q_u_a_l │ │ │ │ │ +bool assert_equal(const Matrix &expected, const Matrix &actual, double tol) │ │ │ │ │ +equals with an tolerance, prints out message if unequal │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:43 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l │ │ │ │ │ +bool equal(const T &obj1, const T &obj2, double tol) │ │ │ │ │ +Call equal on the object. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:84 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_I_s_T_e_s_t_a_b_l_e │ │ │ │ │ +A testable concept check that should be placed in applicable unit tests and in │ │ │ │ │ +generic algorithms. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s___s_t_a_r │ │ │ │ │ +Binary predicate on shared pointers. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:123 │ │ │ │ │ +_g_t_s_a_m_:_:_H_a_s_T_e_s_t_a_b_l_e_P_r_e_r_e_q_s │ │ │ │ │ +Requirements on type to pass it to Testable template below. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:134 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _F_a_s_t_M_a_p_._h │ │ │ │ │ + * _T_e_s_t_a_b_l_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00074.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/lieProxies.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/FastVector.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,63 +95,47 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces | │ │ │ │ -Functions
│ │ │ │ -
lieProxies.h File Reference
│ │ │ │ +Typedefs
│ │ │ │ +
FastVector.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Provides convenient mappings of common member functions for testing. │ │ │ │ +

A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -template<class T >
T gtsam::testing::between (const T &t1, const T &t2)
 binary functions
 
│ │ │ │ -template<class T >
T gtsam::testing::compose (const T &t1, const T &t2)
 
│ │ │ │ -template<class T >
T gtsam::testing::inverse (const T &t)
 unary functions
 
│ │ │ │ -template<class T , class P >
P gtsam::testing::rotate (const T &r, const P &pt)
 rotation functions
 
│ │ │ │ -template<class T , class P >
P gtsam::testing::unrotate (const T &r, const P &pt)
 

│ │ │ │ +Typedefs

template<typename T >
using gtsam::FastVector = std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type >
 FastVector is a type alias to a std::vector with a custom memory allocator.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Provides convenient mappings of common member functions for testing.

│ │ │ │ -
Author
Alex Cunningham
│ │ │ │ +

A thin wrapper around std::vector that uses a custom allocator.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +
Date
Feb 9, 2011
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,40 +1,31 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -lieProxies.h File Reference │ │ │ │ │ -Provides convenient mappings of common member functions for testing. _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ +FastVector.h File Reference │ │ │ │ │ +A thin wrapper around std::vector that uses a custom allocator. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ -T  ggttssaamm::::tteessttiinngg::::bbeettwweeeenn (const T &t1, const T &t2) │ │ │ │ │ -  binary functions │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -T  ggttssaamm::::tteessttiinngg::::ccoommppoossee (const T &t1, const T &t2) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -T  ggttssaamm::::tteessttiinngg::::iinnvveerrssee (const T &t) │ │ │ │ │ -  unary functions │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -P  ggttssaamm::::tteessttiinngg::::rroottaattee (const T &r, const P &pt) │ │ │ │ │ -  rotation functions │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -P  ggttssaamm::::tteessttiinngg::::uunnrroottaattee (const T &r, const P &pt) │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +template │ │ │ │ │ +using  _g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r = std::vector< T, typename _i_n_t_e_r_n_a_l_:_: │ │ │ │ │ + _F_a_s_t_D_e_f_a_u_l_t_V_e_c_t_o_r_A_l_l_o_c_a_t_o_r< T >::type > │ │ │ │ │ +  FastVector is a type alias to a std::vector with a custom memory │ │ │ │ │ + allocator. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Provides convenient mappings of common member functions for testing. │ │ │ │ │ +A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ Author │ │ │ │ │ - Alex Cunningham │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + Feb 9, 2011 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _l_i_e_P_r_o_x_i_e_s_._h │ │ │ │ │ + * _F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00074.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,5 +1,3 @@ │ │ │ │ │ var a00074 = [ │ │ │ │ │ - ["between", "a00074.html#a72a7c561cc91f437a22023297fb7f658", null], │ │ │ │ │ - ["inverse", "a00074.html#aaa623dd059a67523b029af6ec20a38da", null], │ │ │ │ │ - ["rotate", "a00074.html#a644bbe6719d8dd756e11e8b3d2fd229a", null] │ │ │ │ │ + ["FastVector", "a00074.html#gafeaa7d82369f3ff952157586240273c8", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00074_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/lieProxies.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/FastVector.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,63 +98,47 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
lieProxies.h
│ │ │ │ +
FastVector.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/global_includes.h>
│ │ │ │ +
20#pragma once
│ │ │ │
21
│ │ │ │ -
31namespace gtsam {
│ │ │ │ -
32namespace testing {
│ │ │ │ -
33
│ │ │ │ -
35 template<class T>
│ │ │ │ -
36 T between(const T& t1, const T& t2) { return t1.between(t2); }
│ │ │ │ -
37
│ │ │ │ -
38 template<class T>
│ │ │ │ -
39 T compose(const T& t1, const T& t2) { return t1.compose(t2); }
│ │ │ │ -
40
│ │ │ │ -
42 template<class T>
│ │ │ │ -
43 T inverse(const T& t) { return t.inverse(); }
│ │ │ │ -
44
│ │ │ │ -
46 template<class T, class P>
│ │ │ │ -
47 P rotate(const T& r, const P& pt) { return r.rotate(pt); }
│ │ │ │ -
48
│ │ │ │ -
49 template<class T, class P>
│ │ │ │ -
50 P unrotate(const T& r, const P& pt) { return r.unrotate(pt); }
│ │ │ │ -
51
│ │ │ │ -
52} // \namespace testing
│ │ │ │ -
53} // \namespace gtsam
│ │ │ │ -
54
│ │ │ │ -
55
│ │ │ │ -
gtsam::testing::rotate
P rotate(const T &r, const P &pt)
rotation functions
Definition lieProxies.h:47
│ │ │ │ -
gtsam::testing::between
T between(const T &t1, const T &t2)
binary functions
Definition lieProxies.h:36
│ │ │ │ -
gtsam::testing::inverse
T inverse(const T &t)
unary functions
Definition lieProxies.h:43
│ │ │ │ -
global_includes.h
Included from all GTSAM files.
│ │ │ │ +
22#include <gtsam/base/FastDefaultAllocator.h>
│ │ │ │ +
23#include <vector>
│ │ │ │ +
24
│ │ │ │ +
25namespace gtsam {
│ │ │ │ +
26
│ │ │ │ +
32template <typename T>
│ │ │ │ +
33using FastVector =
│ │ │ │ +
34 std::vector<T, typename internal::FastDefaultVectorAllocator<T>::type>;
│ │ │ │ +
35
│ │ │ │ +
36} // namespace gtsam
│ │ │ │ +
FastDefaultAllocator.h
An easy way to control which allocator is used for Fast* collections.
│ │ │ │ +
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,67 +1,46 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -lieProxies.h │ │ │ │ │ +FastVector.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ +20#pragma once │ │ │ │ │ 21 │ │ │ │ │ -31namespace _g_t_s_a_m { │ │ │ │ │ -32namespace testing { │ │ │ │ │ -33 │ │ │ │ │ -35 template │ │ │ │ │ -_3_6 T _b_e_t_w_e_e_n(const T& t1, const T& t2) { return t1.between(t2); } │ │ │ │ │ -37 │ │ │ │ │ -38 template │ │ │ │ │ -39 T compose(const T& t1, const T& t2) { return t1.compose(t2); } │ │ │ │ │ -40 │ │ │ │ │ -42 template │ │ │ │ │ -_4_3 T _i_n_v_e_r_s_e(const T& t) { return t.inverse(); } │ │ │ │ │ -44 │ │ │ │ │ -46 template │ │ │ │ │ -_4_7 P _r_o_t_a_t_e(const T& r, const P& pt) { return r.rotate(pt); } │ │ │ │ │ -48 │ │ │ │ │ -49 template │ │ │ │ │ -50 P unrotate(const T& r, const P& pt) { return r.unrotate(pt); } │ │ │ │ │ -51 │ │ │ │ │ -52} // \namespace testing │ │ │ │ │ -53} // \namespace gtsam │ │ │ │ │ -54 │ │ │ │ │ -55 │ │ │ │ │ -_g_t_s_a_m_:_:_t_e_s_t_i_n_g_:_:_r_o_t_a_t_e │ │ │ │ │ -P rotate(const T &r, const P &pt) │ │ │ │ │ -rotation functions │ │ │ │ │ -DDeeffiinniittiioonn lieProxies.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_t_e_s_t_i_n_g_:_:_b_e_t_w_e_e_n │ │ │ │ │ -T between(const T &t1, const T &t2) │ │ │ │ │ -binary functions │ │ │ │ │ -DDeeffiinniittiioonn lieProxies.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_t_e_s_t_i_n_g_:_:_i_n_v_e_r_s_e │ │ │ │ │ -T inverse(const T &t) │ │ │ │ │ -unary functions │ │ │ │ │ -DDeeffiinniittiioonn lieProxies.h:43 │ │ │ │ │ -_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ -Included from all GTSAM files. │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_._h> │ │ │ │ │ +23#include │ │ │ │ │ +24 │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ +26 │ │ │ │ │ +32template │ │ │ │ │ +_3_3using _F_a_s_t_V_e_c_t_o_r = │ │ │ │ │ +34 std::vector::type>; │ │ │ │ │ +35 │ │ │ │ │ +36} // namespace gtsam │ │ │ │ │ +_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_._h │ │ │ │ │ +An easy way to control which allocator is used for Fast* collections. │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ +std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ +FastVector │ │ │ │ │ +FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ +DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _l_i_e_P_r_o_x_i_e_s_._h │ │ │ │ │ + * _F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00080.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Matrix.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Group.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,159 +94,119 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces | │ │ │ │ -Functions
│ │ │ │ -
Matrix.cpp File Reference
│ │ │ │ +Macros | │ │ │ │ +Functions | │ │ │ │ +Variables
│ │ │ │ +
Group.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

matrix class │ │ │ │ +

Concept check class for variable types with Group properties. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

struct  gtsam::group_tag
 tag to assert a type is a group More...
 
struct  gtsam::multiplicative_group_tag
 Group operator syntax flavors. More...
 
struct  gtsam::additive_group_tag
 
class  gtsam::IsGroup< G >
 Group Concept. More...
 
class  gtsam::DirectProduct
 
struct  gtsam::traits< DirectProduct< G, H > >
 
class  gtsam::DirectSum< G, H >
 Template to construct the direct sum of two additive groups Assumes existence of three additive operators for both groups. More...
 
struct  gtsam::traits< DirectSum< G, H > >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Macros

#define GTSAM_CONCEPT_GROUP_INST(T)   template class gtsam::IsGroup<T>;
 Macros for using the IsGroup.
 
│ │ │ │ +#define GTSAM_CONCEPT_GROUP_TYPE(T)   typedef gtsam::IsGroup<T> _gtsam_IsGroup_##T;
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ Functions

│ │ │ │ -bool gtsam::assert_equal (const Matrix &A, const Matrix &B, double tol=1e-9)
 equals with an tolerance, prints out message if unequal
 
│ │ │ │ -bool gtsam::assert_inequal (const Matrix &A, const Matrix &B, double tol=1e-9)
 inequals with an tolerance, prints out message if within tolerance
 
│ │ │ │ -bool gtsam::assert_equal (const std::list< Matrix > &As, const std::list< Matrix > &Bs, double tol=1e-9)
 equals with an tolerance, prints out message if unequal
 
│ │ │ │ -bool gtsam::linear_independent (const Matrix &A, const Matrix &B, double tol=1e-9)
 check whether the rows of two matrices are linear independent
 
│ │ │ │ -bool gtsam::linear_dependent (const Matrix &A, const Matrix &B, double tol=1e-9)
 check whether the rows of two matrices are linear dependent
 
│ │ │ │ -Vector gtsam::operator^ (const Matrix &A, const Vector &v)
 overload ^ for trans(A)*v We transpose the vectors for speed.
 
│ │ │ │ -const Eigen::IOFormat & gtsam::matlabFormat ()
 
│ │ │ │ -void gtsam::print (const Matrix &A, const std::string &s, std::ostream &stream)
 print without optional string, must specify cout yourself
 
│ │ │ │ -void gtsam::print (const Matrix &A, const std::string &s="")
 print with optional string to cout
 
│ │ │ │ -void gtsam::save (const Matrix &A, const std::string &s, const std::string &filename)
 save a matrix to file, which can be loaded by matlab
 
istream & gtsam::operator>> (std::istream &inputStream, Matrix &destinationMatrix)
 Read a matrix from an input stream, such as a file.
 
│ │ │ │ -Matrix gtsam::diag (const std::vector< Matrix > &Hs)
 Create a matrix with submatrices along its diagonal.
 
│ │ │ │ -Vector gtsam::columnNormSquare (const Matrix &A)
 
pair< Matrix, Matrix > gtsam::qr (const Matrix &A)
 Householder QR factorization, Golub & Van Loan p 224, explicit version
│ │ │ │ -
 
list< boost::tuple< Vector, double, double > > gtsam::weighted_eliminate (Matrix &A, Vector &b, const Vector &sigmas)
 Imperative algorithm for in-place full elimination with weights and constraint handling.
 
void gtsam::householder_ (Matrix &A, size_t k, bool copy_vectors)
 Imperative version of Householder QR factorization, Golub & Van Loan p 224 version with Householder vectors below diagonal, as in GVL.
 
void gtsam::householder (Matrix &A, size_t k)
 Householder tranformation, zeros below diagonal.
 
Vector gtsam::backSubstituteLower (const Matrix &L, const Vector &b, bool unit=false)
 backSubstitute L*x=b
 
Vector gtsam::backSubstituteUpper (const Matrix &U, const Vector &b, bool unit=false)
 backSubstitute U*x=b
 
Vector gtsam::backSubstituteUpper (const Vector &b, const Matrix &U, bool unit=false)
 backSubstitute x'*U=b'
 
Matrix gtsam::stack (size_t nrMatrices,...)
 create a matrix by stacking other matrices Given a set of matrices: A1, A2, A3...
 
│ │ │ │ -Matrix gtsam::stack (const std::vector< Matrix > &blocks)
 
Matrix gtsam::collect (const std::vector< const Matrix * > &matrices, size_t m=0, size_t n=0)
 create a matrix by concatenating Given a set of matrices: A1, A2, A3... If all matrices have the same size, specifying single matrix dimensions will avoid the lookup of dimensions
 
│ │ │ │ -Matrix gtsam::collect (size_t nrMatrices,...)
 
void gtsam::vector_scale_inplace (const Vector &v, Matrix &A, bool inf_mask=false)
 scales a matrix row or column by the values in a vector Arguments (Matrix, Vector) scales the columns, (Vector, Matrix) scales the rows
 
│ │ │ │ -Matrix gtsam::vector_scale (const Vector &v, const Matrix &A, bool inf_mask)
 
│ │ │ │ -Matrix gtsam::vector_scale (const Matrix &A, const Vector &v, bool inf_mask)
 
│ │ │ │ -Matrix gtsam::LLt (const Matrix &A)
 
│ │ │ │ -Matrix gtsam::RtR (const Matrix &A)
 
Matrix gtsam::cholesky_inverse (const Matrix &A)
 Return the inverse of a S.P.D.
 
│ │ │ │ -Matrix gtsam::inverse_square_root (const Matrix &A)
 Use Cholesky to calculate inverse square root of a matrix.
 
void gtsam::svd (const Matrix &A, Matrix &U, Vector &S, Matrix &V)
 SVD computes economy SVD A=U*S*V'.
 
boost::tuple< int, double, Vector > gtsam::DLT (const Matrix &A, double rank_tol=1e-9)
 Direct linear transform algorithm that calls svd to find a vector v that minimizes the algebraic error A*v.
 
Matrix gtsam::expm (const Matrix &A, size_t K=7)
 Numerical exponential map, naive approach, not industrial strength !!!
 
│ │ │ │ -std::string gtsam::formatMatrixIndented (const std::string &label, const Matrix &matrix, bool makeVectorHorizontal)
 
void gtsam::inplace_QR (Matrix &A)
 QR factorization using Eigen's internal block QR algorithm.
 
│ │ │ │ +template<typename G >
 gtsam::BOOST_CONCEPT_REQUIRES (((IsGroup< G >)),(bool)) check_group_invariants(const G &a
 Check invariants.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Variables

│ │ │ │ +const G & gtsam::b
 
│ │ │ │ +const G double gtsam::tol
 
│ │ │ │

Detailed Description

│ │ │ │ -

matrix class

│ │ │ │ -
Author
Christian Potthast
│ │ │ │ -
│ │ │ │ +

Concept check class for variable types with Group properties.

│ │ │ │ +
Date
November, 2011
│ │ │ │ +
Author
Alex Cunningham
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +

Macro Definition Documentation

│ │ │ │ + │ │ │ │ +

◆ GTSAM_CONCEPT_GROUP_INST

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
#define GTSAM_CONCEPT_GROUP_INST( T)   template class gtsam::IsGroup<T>;
│ │ │ │ +
│ │ │ │ + │ │ │ │ +

Macros for using the IsGroup.

│ │ │ │ +
    │ │ │ │ +
  • An instantiation for use inside unit tests
    • │ │ │ │ +
  • A typedef for use inside generic algorithms
    • │ │ │ │ +
│ │ │ │ +

NOTE: intentionally not in the gtsam namespace to allow for classes not in the gtsam namespace to be more easily enforced as testable

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,179 +1,72 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Matrix.cpp File Reference │ │ │ │ │ -matrix class _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _F_u_n_c_t_i_o_n_s | _V_a_r_i_a_b_l_e_s │ │ │ │ │ +Group.h File Reference │ │ │ │ │ +Concept check class for variable types with Group properties. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +struct   _g_t_s_a_m_:_:_g_r_o_u_p___t_a_g │ │ │ │ │ +  tag to assert a type is a group _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_m_u_l_t_i_p_l_i_c_a_t_i_v_e___g_r_o_u_p___t_a_g │ │ │ │ │ +  Group operator syntax flavors. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_a_d_d_i_t_i_v_e___g_r_o_u_p___t_a_g │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_I_s_G_r_o_u_p_<_ _G_ _> │ │ │ │ │ +  Group Concept. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_D_i_r_e_c_t_P_r_o_d_u_c_t │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _D_i_r_e_c_t_P_r_o_d_u_c_t_<_ _G_,_ _H_ _>_ _> │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_D_i_r_e_c_t_S_u_m_<_ _G_,_ _H_ _> │ │ │ │ │ +  Template to construct the direct sum of two additive groups Assumes │ │ │ │ │ + existence of three additive operators for both groups. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _D_i_r_e_c_t_S_u_m_<_ _G_,_ _H_ _>_ _> │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +MMaaccrrooss │ │ │ │ │ +#define  _G_T_S_A_M___C_O_N_C_E_P_T___G_R_O_U_P___I_N_S_T(T)   template class _g_t_s_a_m_:_:_I_s_G_r_o_u_p; │ │ │ │ │ +  Macros for using the IsGroup. │ │ │ │ │ +  │ │ │ │ │ +#define  GGTTSSAAMM__CCOONNCCEEPPTT__GGRROOUUPP__TTYYPPEE(T)   typedef _g_t_s_a_m_:_:_I_s_G_r_o_u_p │ │ │ │ │ + _gtsam_IsGroup_##T; │ │ │ │ │ +  │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ - bool  ggttssaamm::::aasssseerrtt__eeqquuaall (const Matrix &A, │ │ │ │ │ - const Matrix &B, double tol=1e-9) │ │ │ │ │ -  equals with an tolerance, prints out │ │ │ │ │ - message if unequal │ │ │ │ │ -  │ │ │ │ │ - bool  ggttssaamm::::aasssseerrtt__iinneeqquuaall (const Matrix &A, │ │ │ │ │ - const Matrix &B, double tol=1e-9) │ │ │ │ │ -  inequals with an tolerance, prints out │ │ │ │ │ - message if within tolerance │ │ │ │ │ -  │ │ │ │ │ - bool  ggttssaamm::::aasssseerrtt__eeqquuaall (const std::list< │ │ │ │ │ - Matrix > &As, const std::list< Matrix > │ │ │ │ │ - &Bs, double tol=1e-9) │ │ │ │ │ -  equals with an tolerance, prints out │ │ │ │ │ - message if unequal │ │ │ │ │ -  │ │ │ │ │ - bool  ggttssaamm::::lliinneeaarr__iinnddeeppeennddeenntt (const Matrix │ │ │ │ │ - &A, const Matrix &B, double tol=1e-9) │ │ │ │ │ -  check whether the rows of two matrices are │ │ │ │ │ - linear independent │ │ │ │ │ -  │ │ │ │ │ - bool  ggttssaamm::::lliinneeaarr__ddeeppeennddeenntt (const Matrix &A, │ │ │ │ │ - const Matrix &B, double tol=1e-9) │ │ │ │ │ -  check whether the rows of two matrices are │ │ │ │ │ - linear dependent │ │ │ │ │ -  │ │ │ │ │ - Vector  ggttssaamm::::ooppeerraattoorr^^ (const Matrix &A, const │ │ │ │ │ - Vector &v) │ │ │ │ │ -  overload ^ for trans(A)*v We transpose the │ │ │ │ │ - vectors for speed. │ │ │ │ │ -  │ │ │ │ │ - const Eigen::IOFormat &  ggttssaamm::::mmaattllaabbFFoorrmmaatt () │ │ │ │ │ -  │ │ │ │ │ - void  ggttssaamm::::pprriinntt (const Matrix &A, const std:: │ │ │ │ │ - string &s, std::ostream &stream) │ │ │ │ │ -  print without optional string, must │ │ │ │ │ - specify cout yourself │ │ │ │ │ -  │ │ │ │ │ - void  ggttssaamm::::pprriinntt (const Matrix &A, const std:: │ │ │ │ │ - string &s="") │ │ │ │ │ -  print with optional string to cout │ │ │ │ │ -  │ │ │ │ │ - void  ggttssaamm::::ssaavvee (const Matrix &A, const std:: │ │ │ │ │ - string &s, const std::string &filename) │ │ │ │ │ -  save a matrix to file, which can be loaded │ │ │ │ │ - by matlab │ │ │ │ │ -  │ │ │ │ │ - istream &  _g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_>_> (std::istream │ │ │ │ │ - &inputStream, Matrix &destinationMatrix) │ │ │ │ │ -  Read a matrix from an input stream, such │ │ │ │ │ - as a file. │ │ │ │ │ -  │ │ │ │ │ - Matrix  ggttssaamm::::ddiiaagg (const std::vector< Matrix > │ │ │ │ │ - &Hs) │ │ │ │ │ -  Create a matrix with submatrices along its │ │ │ │ │ - diagonal. │ │ │ │ │ -  │ │ │ │ │ - Vector  ggttssaamm::::ccoolluummnnNNoorrmmSSqquuaarree (const Matrix &A) │ │ │ │ │ -  │ │ │ │ │ - pair< Matrix, Matrix >  _g_t_s_a_m_:_:_q_r (const Matrix &A) │ │ │ │ │ - Householder QR factorization, Golub & Van │ │ │ │ │ -  Loan p 224, explicit version │ │ │ │ │ - │ │ │ │ │ -  │ │ │ │ │ - list< boost::tuple< Vector, double, _g_t_s_a_m_:_:_w_e_i_g_h_t_e_d___e_l_i_m_i_n_a_t_e (Matrix &A, │ │ │ │ │ - double > >  Vector &b, const Vector &sigmas) │ │ │ │ │ - Imperative algorithm for in-place full │ │ │ │ │ -  elimination with weights and constraint │ │ │ │ │ - handling. │ │ │ │ │ -  │ │ │ │ │ - void  _g_t_s_a_m_:_:_h_o_u_s_e_h_o_l_d_e_r__ (Matrix &A, size_t k, │ │ │ │ │ - bool copy_vectors) │ │ │ │ │ - Imperative version of Householder QR │ │ │ │ │ -  factorization, Golub & Van Loan p 224 │ │ │ │ │ - version with Householder vectors below │ │ │ │ │ - diagonal, as in GVL. │ │ │ │ │ -  │ │ │ │ │ - void  _g_t_s_a_m_:_:_h_o_u_s_e_h_o_l_d_e_r (Matrix &A, size_t k) │ │ │ │ │ -  Householder tranformation, zeros below │ │ │ │ │ - diagonal. │ │ │ │ │ -  │ │ │ │ │ - Vector  _g_t_s_a_m_:_:_b_a_c_k_S_u_b_s_t_i_t_u_t_e_L_o_w_e_r (const Matrix │ │ │ │ │ - &L, const Vector &b, bool unit=false) │ │ │ │ │ -  backSubstitute L*x=b │ │ │ │ │ -  │ │ │ │ │ - Vector  _g_t_s_a_m_:_:_b_a_c_k_S_u_b_s_t_i_t_u_t_e_U_p_p_e_r (const Matrix │ │ │ │ │ - &U, const Vector &b, bool unit=false) │ │ │ │ │ -  backSubstitute U*x=b │ │ │ │ │ -  │ │ │ │ │ - Vector  _g_t_s_a_m_:_:_b_a_c_k_S_u_b_s_t_i_t_u_t_e_U_p_p_e_r (const Vector │ │ │ │ │ - &b, const Matrix &U, bool unit=false) │ │ │ │ │ -  backSubstitute x'*U=b' │ │ │ │ │ -  │ │ │ │ │ - Matrix  _g_t_s_a_m_:_:_s_t_a_c_k (size_t nrMatrices,...) │ │ │ │ │ -  create a matrix by stacking other matrices │ │ │ │ │ - Given a set of matrices: A1, A2, A3... │ │ │ │ │ -  │ │ │ │ │ - Matrix  ggttssaamm::::ssttaacckk (const std::vector< Matrix > │ │ │ │ │ - &blocks) │ │ │ │ │ -  │ │ │ │ │ - Matrix  _g_t_s_a_m_:_:_c_o_l_l_e_c_t (const std::vector< const │ │ │ │ │ - Matrix * > &matrices, size_t m=0, size_t │ │ │ │ │ - n=0) │ │ │ │ │ - create a matrix by concatenating Given a │ │ │ │ │ - set of matrices: A1, A2, A3... If all │ │ │ │ │ -  matrices have the same size, specifying │ │ │ │ │ - single matrix dimensions will avoid the │ │ │ │ │ - lookup of dimensions │ │ │ │ │ -  │ │ │ │ │ - Matrix  ggttssaamm::::ccoolllleecctt (size_t nrMatrices,...) │ │ │ │ │ -  │ │ │ │ │ - void  _g_t_s_a_m_:_:_v_e_c_t_o_r___s_c_a_l_e___i_n_p_l_a_c_e (const Vector │ │ │ │ │ - &v, Matrix &A, bool inf_mask=false) │ │ │ │ │ - scales a matrix row or column by the │ │ │ │ │ -  values in a vector Arguments (Matrix, │ │ │ │ │ - Vector) scales the columns, (Vector, │ │ │ │ │ - Matrix) scales the rows │ │ │ │ │ -  │ │ │ │ │ - Matrix  ggttssaamm::::vveeccttoorr__ssccaallee (const Vector &v, │ │ │ │ │ - const Matrix &A, bool inf_mask) │ │ │ │ │ -  │ │ │ │ │ - Matrix  ggttssaamm::::vveeccttoorr__ssccaallee (const Matrix &A, │ │ │ │ │ - const Vector &v, bool inf_mask) │ │ │ │ │ -  │ │ │ │ │ - Matrix  ggttssaamm::::LLLLtt (const Matrix &A) │ │ │ │ │ -  │ │ │ │ │ - Matrix  ggttssaamm::::RRttRR (const Matrix &A) │ │ │ │ │ -  │ │ │ │ │ - Matrix  _g_t_s_a_m_:_:_c_h_o_l_e_s_k_y___i_n_v_e_r_s_e (const Matrix &A) │ │ │ │ │ -  Return the inverse of a S.P.D. │ │ │ │ │ -  │ │ │ │ │ - Matrix  ggttssaamm::::iinnvveerrssee__ssqquuaarree__rroooott (const Matrix │ │ │ │ │ - &A) │ │ │ │ │ -  Use Cholesky to calculate inverse square │ │ │ │ │ - root of a matrix. │ │ │ │ │ -  │ │ │ │ │ - void  _g_t_s_a_m_:_:_s_v_d (const Matrix &A, Matrix &U, │ │ │ │ │ - Vector &S, Matrix &V) │ │ │ │ │ -  SVD computes economy SVD A=U*S*V'. │ │ │ │ │ -  │ │ │ │ │ -boost::tuple< int, double, Vector >  _g_t_s_a_m_:_:_D_L_T (const Matrix &A, double │ │ │ │ │ - rank_tol=1e-9) │ │ │ │ │ - Direct linear transform algorithm that │ │ │ │ │ -  calls svd to find a vector v that │ │ │ │ │ - minimizes the algebraic error A*v. │ │ │ │ │ -  │ │ │ │ │ - Matrix  _g_t_s_a_m_:_:_e_x_p_m (const Matrix &A, size_t K=7) │ │ │ │ │ -  Numerical exponential map, naive approach, │ │ │ │ │ - not industrial strength !!! │ │ │ │ │ -  │ │ │ │ │ - std::string  ggttssaamm::::ffoorrmmaattMMaattrriixxIInnddeenntteedd (const std:: │ │ │ │ │ - string &label, const Matrix &matrix, bool │ │ │ │ │ - makeVectorHorizontal) │ │ │ │ │ -  │ │ │ │ │ - void  _g_t_s_a_m_:_:_i_n_p_l_a_c_e___Q_R (Matrix &A) │ │ │ │ │ -  QR factorization using Eigen's internal │ │ │ │ │ - block QR algorithm. │ │ │ │ │ +template │ │ │ │ │ +  ggttssaamm::::BBOOOOSSTT__CCOONNCCEEPPTT__RREEQQUUIIRREESS (((_I_s_G_r_o_u_p< G >)),(bool)) │ │ │ │ │ + check_group_invariants(const G &a │ │ │ │ │ +  Check invariants. │ │ │ │ │ +  │ │ │ │ │ +VVaarriiaabblleess │ │ │ │ │ + const G &  ggttssaamm::::bb │ │ │ │ │ +  │ │ │ │ │ +const G double  ggttssaamm::::ttooll │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -matrix class │ │ │ │ │ +Concept check class for variable types with Group properties. │ │ │ │ │ + Date │ │ │ │ │ + November, 2011 │ │ │ │ │ Author │ │ │ │ │ - Christian Potthast │ │ │ │ │ + Alex Cunningham │ │ │ │ │ + Frank Dellaert │ │ │ │ │ +********** MMaaccrroo DDeeffiinniittiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ +********** _?◆_? GGTTSSAAMM__CCOONNCCEEPPTT__GGRROOUUPP__IINNSSTT ********** │ │ │ │ │ +#define GTSAM_CONCEPT_GROUP_INST (   T )    template class _g_t_s_a_m_:_:_I_s_G_r_o_u_p; │ │ │ │ │ +Macros for using the IsGroup. │ │ │ │ │ + * An instantiation for use inside unit tests │ │ │ │ │ + * A typedef for use inside generic algorithms │ │ │ │ │ +NOTE: intentionally not in the gtsam namespace to allow for classes not in the │ │ │ │ │ +gtsam namespace to be more easily enforced as testable │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _M_a_t_r_i_x_._c_p_p │ │ │ │ │ + * _G_r_o_u_p_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00080.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,29 +1,12 @@ │ │ │ │ │ var a00080 = [ │ │ │ │ │ - ["assert_equal", "a00080.html#a3f9622226dfe06908f11b42bf0bdd22d", null], │ │ │ │ │ - ["assert_equal", "a00080.html#abb0e19bbbeaca95843e8161b89a12fda", null], │ │ │ │ │ - ["assert_inequal", "a00080.html#a7a116d0643f123ef3b15d91056506492", null], │ │ │ │ │ - ["backSubstituteLower", "a00080.html#a0d998e1b770c9864946ddb031b1c4522", null], │ │ │ │ │ - ["backSubstituteUpper", "a00080.html#a6c4876cbe85d5651a52eda0e97c60f2f", null], │ │ │ │ │ - ["backSubstituteUpper", "a00080.html#a8b0bf332d52b333dab2b20d763c8925b", null], │ │ │ │ │ - ["cholesky_inverse", "a00080.html#aba542e2cd85f08b76f80a0871a4ea713", null], │ │ │ │ │ - ["collect", "a00080.html#ad8b524ba6c9aed0d21a020999c9b5d88", null], │ │ │ │ │ - ["diag", "a00080.html#af1c32907adce74cf9edd6ee5bba5a085", null], │ │ │ │ │ - ["DLT", "a00080.html#ab2c65f1a69009a306b6a5f9ef31dcee2", null], │ │ │ │ │ - ["expm", "a00080.html#ab15462d8c16813d0a7a5b1f76a2f64b7", null], │ │ │ │ │ - ["householder", "a00080.html#a7baa2f3184a444adce108633c0265e0c", null], │ │ │ │ │ - ["householder_", "a00080.html#abdd1ab05e8ac90b340fbd8f3b322dc6d", null], │ │ │ │ │ - ["inplace_QR", "a00080.html#a7eec9339ab5a008a28ddfaa6b2c94611", null], │ │ │ │ │ - ["inverse_square_root", "a00080.html#aff78dee59ac0250432081f39deb5f6d1", null], │ │ │ │ │ - ["linear_dependent", "a00080.html#a4b81794af72954abafbb726fc712f5db", null], │ │ │ │ │ - ["linear_independent", "a00080.html#ad8f0349471464c1fb515819d9503849a", null], │ │ │ │ │ - ["operator>>", "a00080.html#a7d85957bab2d18cf56ab9aaf95a106de", null], │ │ │ │ │ - ["operator^", "a00080.html#a2218e53a2b99c449e70aa5b7805895fc", null], │ │ │ │ │ - ["print", "a00080.html#a54fa43c89c5334314c8c75939dd5c2d7", null], │ │ │ │ │ - ["print", "a00080.html#a44ead03912d5dcf094d8421e1702ee71", null], │ │ │ │ │ - ["qr", "a00080.html#ae32c295e2c40c1e85f146a8a6266eaa8", null], │ │ │ │ │ - ["save", "a00080.html#a3eb7eed9019d3fda8fcf74fbf85b85c9", null], │ │ │ │ │ - ["stack", "a00080.html#a5c82884a356ddd09229a5283aed04df9", null], │ │ │ │ │ - ["svd", "a00080.html#a8d7e46204d953f64a39445599dbd7eee", null], │ │ │ │ │ - ["vector_scale_inplace", "a00080.html#a24d3ad0252f91f0ec301593c45cf5af7", null], │ │ │ │ │ - ["weighted_eliminate", "a00080.html#a60019a03f26b92c6b2a08e43d153d4c0", null] │ │ │ │ │ + ["gtsam::group_tag", "a02332.html", null], │ │ │ │ │ + ["gtsam::multiplicative_group_tag", "a02336.html", null], │ │ │ │ │ + ["gtsam::additive_group_tag", "a02340.html", null], │ │ │ │ │ + ["gtsam::IsGroup< G >", "a02344.html", null], │ │ │ │ │ + ["gtsam::DirectProduct", "a02348.html", "a02348"], │ │ │ │ │ + ["gtsam::traits< DirectProduct< G, H > >", "a02352.html", null], │ │ │ │ │ + ["gtsam::DirectSum< G, H >", "a02356.html", "a02356"], │ │ │ │ │ + ["gtsam::traits< DirectSum< G, H > >", "a02360.html", null], │ │ │ │ │ + ["GTSAM_CONCEPT_GROUP_INST", "a00080.html#aac1ea136475ea605715c809f4b3f0156", null], │ │ │ │ │ + ["BOOST_CONCEPT_REQUIRES", "a00080.html#a2e95b5b5508cf3284107e979401bd1a4", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00083.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Testable.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/OptionalJacobian.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,122 +95,54 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Macros | │ │ │ │ -Functions
│ │ │ │ -
Testable.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
OptionalJacobian.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Concept check for values that can be used in unit tests. │ │ │ │ +

Special class for optional Jacobian arguments. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::IsTestable< T >
 A testable concept check that should be placed in applicable unit tests and in generic algorithms. More...
class  gtsam::OptionalJacobian< Rows, Cols >
 OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size or dynamic Eigen matrix. More...
 
struct  gtsam::equals< V >
 Template to create a binary predicate. More...
class  gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >
 
struct  gtsam::equals_star< V >
 Binary predicate on shared pointers. More...
struct  gtsam::MakeJacobian< T, A >
 : meta-function to generate Jacobian More...
 
struct  gtsam::HasTestablePrereqs< T >
 Requirements on type to pass it to Testable template below. More...
 
struct  gtsam::Testable< T >
 A helper that implements the traits interface for GTSAM types. More...
struct  gtsam::MakeOptionalJacobian< T, A >
 : meta-function to generate JacobianTA optional reference Used mainly by Expressions More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Macros

│ │ │ │ -#define GTSAM_PRINT(x)   ((x).print(#x))
 
#define GTSAM_CONCEPT_TESTABLE_INST(T)   template class gtsam::IsTestable<T>;
 Macros for using the TestableConcept.
 
│ │ │ │ -#define GTSAM_CONCEPT_TESTABLE_TYPE(T)   using _gtsam_Testable_##T = gtsam::IsTestable<T>;
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -void gtsam::print (float v, const std::string &s="")
 
│ │ │ │ -void gtsam::print (double v, const std::string &s="")
 
│ │ │ │ -template<class T >
bool gtsam::equal (const T &obj1, const T &obj2, double tol)
 Call equal on the object.
 
│ │ │ │ -template<class T >
bool gtsam::equal (const T &obj1, const T &obj2)
 Call equal without tolerance (use default tolerance)
 
│ │ │ │ -template<class V >
bool gtsam::assert_equal (const V &expected, const V &actual, double tol=1e-9)
 This template works for any type with equals.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Concept check for values that can be used in unit tests.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -

The necessary functions to implement for Testable are defined below with additional details as to the interface. The concept checking function will check whether or not the function exists in derived class and throw compile-time errors.

│ │ │ │ -

print with optional string naming the object void print(const std::string& name) const = 0;

│ │ │ │ -

equality up to tolerance tricky to implement, see PriorFactor for an example equals is not supposed to print out anything, just return true|false bool equals(const Derived& expected, double tol) const = 0;

│ │ │ │ -

Macro Definition Documentation

│ │ │ │ - │ │ │ │ -

◆ GTSAM_CONCEPT_TESTABLE_INST

│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
#define GTSAM_CONCEPT_TESTABLE_INST( T)   template class gtsam::IsTestable<T>;
│ │ │ │ -
│ │ │ │ - │ │ │ │ -

Macros for using the TestableConcept.

│ │ │ │ -
    │ │ │ │ -
  • An instantiation for use inside unit tests
    • │ │ │ │ -
  • A typedef for use inside generic algorithms
    • │ │ │ │ -
│ │ │ │ -

NOTE: intentionally not in the gtsam namespace to allow for classes not in the gtsam namespace to be more easily enforced as testable

Deprecated:
please use BOOST_CONCEPT_ASSERT and
│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ +

Special class for optional Jacobian arguments.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ +Natesh Srinivasan
│ │ │ │ +
Date
Nov 28, 2014
│ │ │ │ +
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,84 +1,38 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Testable.h File Reference │ │ │ │ │ -Concept check for values that can be used in unit tests. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +OptionalJacobian.h File Reference │ │ │ │ │ +Special class for optional Jacobian arguments. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_I_s_T_e_s_t_a_b_l_e_<_ _T_ _> │ │ │ │ │ -  A testable concept check that should be placed in applicable unit │ │ │ │ │ - tests and in generic algorithms. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_e_q_u_a_l_s_<_ _V_ _> │ │ │ │ │ -  Template to create a binary predicate. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_e_q_u_a_l_s___s_t_a_r_<_ _V_ _> │ │ │ │ │ -  Binary predicate on shared pointers. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_H_a_s_T_e_s_t_a_b_l_e_P_r_e_r_e_q_s_<_ _T_ _> │ │ │ │ │ -  Requirements on type to pass it to _T_e_s_t_a_b_l_e template below. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_T_e_s_t_a_b_l_e_<_ _T_ _> │ │ │ │ │ -  A helper that implements the traits interface for GTSAM types. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_ _R_o_w_s_,_ _C_o_l_s_ _> │ │ │ │ │ +  _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n is an Eigen::Ref like class that can take be │ │ │ │ │ + constructed using either a fixed size or dynamic Eigen matrix. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_,_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_M_a_k_e_J_a_c_o_b_i_a_n_<_ _T_,_ _A_ _> │ │ │ │ │ +  : meta-function to generate Jacobian _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_ _T_,_ _A_ _> │ │ │ │ │ +  : meta-function to generate JacobianTA optional reference Used mainly │ │ │ │ │ + by Expressions _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -MMaaccrrooss │ │ │ │ │ -#define  GGTTSSAAMM__PPRRIINNTT(x)   ((x).print(#x)) │ │ │ │ │ -  │ │ │ │ │ -#define  _G_T_S_A_M___C_O_N_C_E_P_T___T_E_S_T_A_B_L_E___I_N_S_T(T)   template class _g_t_s_a_m_:_:_I_s_T_e_s_t_a_b_l_e; │ │ │ │ │ -  Macros for using the TestableConcept. │ │ │ │ │ -  │ │ │ │ │ -#define  GGTTSSAAMM__CCOONNCCEEPPTT__TTEESSTTAABBLLEE__TTYYPPEE(T)   using _gtsam_Testable_##T = _g_t_s_a_m_:_: │ │ │ │ │ - _I_s_T_e_s_t_a_b_l_e; │ │ │ │ │ -  │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -void  ggttssaamm::::pprriinntt (float v, const std::string &s="") │ │ │ │ │ -  │ │ │ │ │ -void  ggttssaamm::::pprriinntt (double v, const std::string &s="") │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -bool  ggttssaamm::::eeqquuaall (const T &obj1, const T &obj2, double tol) │ │ │ │ │ -  Call equal on the object. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -bool  ggttssaamm::::eeqquuaall (const T &obj1, const T &obj2) │ │ │ │ │ -  Call equal without tolerance (use default tolerance) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -bool  ggttssaamm::::aasssseerrtt__eeqquuaall (const V &expected, const V &actual, double tol=1e-9) │ │ │ │ │ -  This template works for any type with equals. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Concept check for values that can be used in unit tests. │ │ │ │ │ +Special class for optional Jacobian arguments. │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ -The necessary functions to implement for Testable are defined below with │ │ │ │ │ -additional details as to the interface. The concept checking function will │ │ │ │ │ -check whether or not the function exists in derived class and throw compile- │ │ │ │ │ -time errors. │ │ │ │ │ -print with optional string naming the object void print(const std::string& │ │ │ │ │ -name) const = 0; │ │ │ │ │ -equality up to tolerance tricky to implement, see PriorFactor for an example │ │ │ │ │ -equals is not supposed to print out aannyytthhiinngg, just return true|false bool │ │ │ │ │ -equals(const Derived& expected, double tol) const = 0; │ │ │ │ │ -********** MMaaccrroo DDeeffiinniittiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ -********** _?◆_? GGTTSSAAMM__CCOONNCCEEPPTT__TTEESSTTAABBLLEE__IINNSSTT ********** │ │ │ │ │ -#define GTSAM_CONCEPT_TESTABLE_INST (   T )    template class _g_t_s_a_m_:_: │ │ │ │ │ - _I_s_T_e_s_t_a_b_l_e; │ │ │ │ │ -Macros for using the TestableConcept. │ │ │ │ │ - * An instantiation for use inside unit tests │ │ │ │ │ - * A typedef for use inside generic algorithms │ │ │ │ │ -NOTE: intentionally not in the gtsam namespace to allow for classes not in the │ │ │ │ │ -gtsam namespace to be more easily enforced as testable │ │ │ │ │ - _DD_ee_pp_rr_ee_cc_aa_tt_ee_dd_:: │ │ │ │ │ - please use BOOST_CONCEPT_ASSERT and │ │ │ │ │ + Natesh Srinivasan │ │ │ │ │ + Date │ │ │ │ │ + Nov 28, 2014 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _T_e_s_t_a_b_l_e_._h │ │ │ │ │ + * _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00083.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,10 +1,6 @@ │ │ │ │ │ var a00083 = [ │ │ │ │ │ - ["gtsam::equals< V >", "a02480.html", null], │ │ │ │ │ - ["gtsam::equals_star< V >", "a02484.html", null], │ │ │ │ │ - ["gtsam::HasTestablePrereqs< T >", "a02488.html", null], │ │ │ │ │ - ["gtsam::Testable< T >", "a02492.html", null], │ │ │ │ │ - ["GTSAM_CONCEPT_TESTABLE_INST", "a00083.html#abe0a238cdd07d484ff0be56ae945b182", null], │ │ │ │ │ - ["assert_equal", "a00083.html#a659619cca082d1c10b07c033d48c54da", null], │ │ │ │ │ - ["equal", "a00083.html#a1f3dc67ad88b799c469088e428c583b1", null], │ │ │ │ │ - ["equal", "a00083.html#ad2dbee4a72127938c79162cc8b6d5152", null] │ │ │ │ │ + ["gtsam::OptionalJacobian< Rows, Cols >", "a02448.html", "a02448"], │ │ │ │ │ + ["gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >", "a02452.html", "a02452"], │ │ │ │ │ + ["gtsam::MakeJacobian< T, A >", "a02456.html", null], │ │ │ │ │ + ["gtsam::MakeOptionalJacobian< T, A >", "a02460.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00083_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Testable.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/OptionalJacobian.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,171 +98,292 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Testable.h
│ │ │ │ +
OptionalJacobian.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
32// \callgraph
│ │ │ │ -
33
│ │ │ │ -
34#pragma once
│ │ │ │ -
35
│ │ │ │ -
36#include <boost/concept_check.hpp>
│ │ │ │ -
37#include <functional>
│ │ │ │ -
38#include <iostream>
│ │ │ │ -
39#include <memory>
│ │ │ │ -
40#include <string>
│ │ │ │ -
41
│ │ │ │ -
42#define GTSAM_PRINT(x)((x).print(#x))
│ │ │ │ -
43
│ │ │ │ -
44namespace gtsam {
│ │ │ │ -
45
│ │ │ │ -
46 // Forward declaration
│ │ │ │ -
47 template <typename T> struct traits;
│ │ │ │ +
20#pragma once
│ │ │ │ +
21#include <gtsam/config.h> // Configuration from CMake
│ │ │ │ +
22#include <Eigen/Dense>
│ │ │ │ +
23#include <stdexcept>
│ │ │ │ +
24#include <string>
│ │ │ │ +
25
│ │ │ │ +
26#ifndef OPTIONALJACOBIAN_NOBOOST
│ │ │ │ +
27#include <boost/optional.hpp>
│ │ │ │ +
28#endif
│ │ │ │ +
29
│ │ │ │ +
30namespace gtsam {
│ │ │ │ +
31
│ │ │ │ +
40template<int Rows, int Cols>
│ │ │ │ +
│ │ │ │ +
41class OptionalJacobian {
│ │ │ │ +
42
│ │ │ │ +
43public:
│ │ │ │ +
44
│ │ │ │ +
47 typedef Eigen::Matrix<double, Rows, Cols> Jacobian;
│ │ │ │
48
│ │ │ │ -
57 template <class T>
│ │ │ │ -
│ │ │ │ -
58 class IsTestable {
│ │ │ │ -
59 T t;
│ │ │ │ -
60 bool r1,r2;
│ │ │ │ -
61 public:
│ │ │ │ -
62
│ │ │ │ -
63 BOOST_CONCEPT_USAGE(IsTestable) {
│ │ │ │ -
64 // check print function, with optional string
│ │ │ │ -
65 traits<T>::Print(t, std::string());
│ │ │ │ -
66 traits<T>::Print(t);
│ │ │ │ -
67
│ │ │ │ -
68 // check print, with optional threshold
│ │ │ │ -
69 double tol = 1.0;
│ │ │ │ -
70 r1 = traits<T>::Equals(t,t,tol);
│ │ │ │ -
71 r2 = traits<T>::Equals(t,t);
│ │ │ │ -
72 }
│ │ │ │ -
73 }; // \ Testable
│ │ │ │ -
│ │ │ │ -
74
│ │ │ │ -
75 inline void print(float v, const std::string& s = "") {
│ │ │ │ -
76 std::cout << (s.empty() ? s : s + " ") << v << std::endl;
│ │ │ │ -
77 }
│ │ │ │ -
78 inline void print(double v, const std::string& s = "") {
│ │ │ │ -
79 std::cout << (s.empty() ? s : s + " ") << v << std::endl;
│ │ │ │ -
80 }
│ │ │ │ -
81
│ │ │ │ -
83 template<class T>
│ │ │ │ -
│ │ │ │ -
84 inline bool equal(const T& obj1, const T& obj2, double tol) {
│ │ │ │ -
85 return traits<T>::Equals(obj1,obj2, tol);
│ │ │ │ -
86 }
│ │ │ │ -
│ │ │ │ -
87
│ │ │ │ -
89 template<class T>
│ │ │ │ -
│ │ │ │ -
90 inline bool equal(const T& obj1, const T& obj2) {
│ │ │ │ -
91 return traits<T>::Equals(obj1,obj2);
│ │ │ │ +
49private:
│ │ │ │ +
50
│ │ │ │ +
51 Eigen::Map<Jacobian> map_;
│ │ │ │ +
52
│ │ │ │ +
53 // Trick from http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html
│ │ │ │ +
54 // uses "placement new" to make map_ usurp the memory of the fixed size matrix
│ │ │ │ +
55 void usurp(double* data) {
│ │ │ │ +
56 new (&map_) Eigen::Map<Jacobian>(data);
│ │ │ │ +
57 }
│ │ │ │ +
58
│ │ │ │ +
59 // Private and very dangerous constructor straight from memory
│ │ │ │ +
60 OptionalJacobian(double* data) : map_(nullptr) {
│ │ │ │ +
61 if (data) usurp(data);
│ │ │ │ +
62 }
│ │ │ │ +
63
│ │ │ │ +
64 template<int M, int N>
│ │ │ │ +
65 friend class OptionalJacobian;
│ │ │ │ +
66
│ │ │ │ +
67public:
│ │ │ │ +
68
│ │ │ │ +
│ │ │ │ +
70 OptionalJacobian() :
│ │ │ │ +
71 map_(nullptr) {
│ │ │ │ +
72 }
│ │ │ │ +
│ │ │ │ +
73
│ │ │ │ +
│ │ │ │ +
75 OptionalJacobian(Jacobian& fixed) :
│ │ │ │ +
76 map_(nullptr) {
│ │ │ │ +
77 usurp(fixed.data());
│ │ │ │ +
78 }
│ │ │ │ +
│ │ │ │ +
79
│ │ │ │ +
│ │ │ │ +
81 OptionalJacobian(Jacobian* fixedPtr) :
│ │ │ │ +
82 map_(nullptr) {
│ │ │ │ +
83 if (fixedPtr)
│ │ │ │ +
84 usurp(fixedPtr->data());
│ │ │ │ +
85 }
│ │ │ │ +
│ │ │ │ +
86
│ │ │ │ +
│ │ │ │ +
88 OptionalJacobian(Eigen::MatrixXd& dynamic) :
│ │ │ │ +
89 map_(nullptr) {
│ │ │ │ +
90 dynamic.resize(Rows, Cols); // no malloc if correct size
│ │ │ │ +
91 usurp(dynamic.data());
│ │ │ │
92 }
│ │ │ │
│ │ │ │
93
│ │ │ │ -
97 template<class V>
│ │ │ │ -
│ │ │ │ -
98 bool assert_equal(const V& expected, const V& actual, double tol = 1e-9) {
│ │ │ │ -
99 if (traits<V>::Equals(actual,expected, tol))
│ │ │ │ -
100 return true;
│ │ │ │ -
101 printf("Not equal:\n");
│ │ │ │ -
102 traits<V>::Print(expected,"expected:\n");
│ │ │ │ -
103 traits<V>::Print(actual,"actual:\n");
│ │ │ │ -
104 return false;
│ │ │ │ -
105 }
│ │ │ │ -
│ │ │ │ -
106
│ │ │ │ -
110 template<class V>
│ │ │ │ -
│ │ │ │ -
111 struct equals : public std::function<bool(const V&, const V&)> {
│ │ │ │ -
112 double tol_;
│ │ │ │ -
113 equals(double tol = 1e-9) : tol_(tol) {}
│ │ │ │ -
114 bool operator()(const V& expected, const V& actual) {
│ │ │ │ -
115 return (traits<V>::Equals(actual, expected, tol_));
│ │ │ │ +
│ │ │ │ +
95 OptionalJacobian(Eigen::MatrixXd* dynamic) :
│ │ │ │ +
96 map_(nullptr) {
│ │ │ │ +
97 dynamic->resize(Rows, Cols); // no malloc if correct size
│ │ │ │ +
98 usurp(dynamic->data());
│ │ │ │ +
99 }
│ │ │ │ +
│ │ │ │ +
100
│ │ │ │ +
105 template<class MATRIX>
│ │ │ │ +
│ │ │ │ +
106 OptionalJacobian(Eigen::Ref<MATRIX> dynamic_ref) :
│ │ │ │ +
107 map_(nullptr) {
│ │ │ │ +
108 if (dynamic_ref.rows() == Rows && dynamic_ref.cols() == Cols && !dynamic_ref.IsRowMajor) {
│ │ │ │ +
109 usurp(dynamic_ref.data());
│ │ │ │ +
110 } else {
│ │ │ │ +
111 throw std::invalid_argument(
│ │ │ │ +
112 std::string("OptionalJacobian called with wrong dimensions or "
│ │ │ │ +
113 "storage order.\n"
│ │ │ │ +
114 "Expected: ") +
│ │ │ │ +
115 "(" + std::to_string(Rows) + ", " + std::to_string(Cols) + ")");
│ │ │ │
116 }
│ │ │ │ -
117 };
│ │ │ │ +
117 }
│ │ │ │
│ │ │ │
118
│ │ │ │ -
122 template<class V>
│ │ │ │ -
│ │ │ │ -
123 struct equals_star : public std::function<bool(const std::shared_ptr<V>&, const std::shared_ptr<V>&)> {
│ │ │ │ -
124 double tol_;
│ │ │ │ -
125 equals_star(double tol = 1e-9) : tol_(tol) {}
│ │ │ │ -
126 bool operator()(const std::shared_ptr<V>& expected, const std::shared_ptr<V>& actual) {
│ │ │ │ -
127 if (!actual && !expected) return true;
│ │ │ │ -
128 return actual && expected && traits<V>::Equals(*actual,*expected, tol_);
│ │ │ │ -
129 }
│ │ │ │ -
130 };
│ │ │ │ -
│ │ │ │ -
131
│ │ │ │ -
133 template<typename T>
│ │ │ │ -
│ │ │ │ -
134 struct HasTestablePrereqs {
│ │ │ │ -
135
│ │ │ │ -
136 BOOST_CONCEPT_USAGE(HasTestablePrereqs) {
│ │ │ │ -
137 t->print(str);
│ │ │ │ -
138 b = t->equals(*s,tol);
│ │ │ │ -
139 }
│ │ │ │ -
140
│ │ │ │ -
141 T *t, *s; // Pointer is to allow abstract classes
│ │ │ │ -
142 bool b;
│ │ │ │ -
143 double tol;
│ │ │ │ -
144 std::string str;
│ │ │ │ -
145 };
│ │ │ │ +
119#ifndef OPTIONALJACOBIAN_NOBOOST
│ │ │ │ +
120
│ │ │ │ +
│ │ │ │ +
122 OptionalJacobian(boost::none_t /*none*/) :
│ │ │ │ +
123 map_(nullptr) {
│ │ │ │ +
124 }
│ │ │ │ +
│ │ │ │ +
125
│ │ │ │ +
│ │ │ │ +
127 OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
│ │ │ │ +
128 map_(nullptr) {
│ │ │ │ +
129 if (optional) {
│ │ │ │ +
130 optional->resize(Rows, Cols);
│ │ │ │ +
131 usurp(optional->data());
│ │ │ │ +
132 }
│ │ │ │ +
133 }
│ │ │ │ +
│ │ │ │ +
134
│ │ │ │ +
135#endif
│ │ │ │ +
136
│ │ │ │ +
139 // template <typename Derived, bool InnerPanel>
│ │ │ │ +
140 // OptionalJacobian(Eigen::Block<Derived,Rows,Cols,InnerPanel> block) : map_(nullptr) { ?? }
│ │ │ │ +
141
│ │ │ │ +
│ │ │ │ +
143 operator bool() const {
│ │ │ │ +
144 return map_.data() != nullptr;
│ │ │ │ +
145 }
│ │ │ │
│ │ │ │
146
│ │ │ │ -
150 template<typename T>
│ │ │ │ -
│ │ │ │ -
151 struct Testable {
│ │ │ │ -
152
│ │ │ │ -
153 // Check that T has the necessary methods
│ │ │ │ -
154 BOOST_CONCEPT_ASSERT((HasTestablePrereqs<T>));
│ │ │ │ -
155
│ │ │ │ -
156 static void Print(const T& m, const std::string& str = "") {
│ │ │ │ -
157 m.print(str);
│ │ │ │ -
158 }
│ │ │ │ -
159 static bool Equals(const T& m1, const T& m2, double tol = 1e-8) {
│ │ │ │ -
160 return m1.equals(m2, tol);
│ │ │ │ -
161 }
│ │ │ │ -
162 };
│ │ │ │ -
│ │ │ │ -
163
│ │ │ │ -
164} // \namespace gtsam
│ │ │ │ -
165
│ │ │ │ -
175#define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::IsTestable<T>;
│ │ │ │ -
176#define GTSAM_CONCEPT_TESTABLE_TYPE(T) using _gtsam_Testable_##T = gtsam::IsTestable<T>;
│ │ │ │ +
│ │ │ │ +
148 Eigen::Map<Jacobian>& operator*() {
│ │ │ │ +
149 return map_;
│ │ │ │ +
150 }
│ │ │ │ +
│ │ │ │ +
151
│ │ │ │ +
│ │ │ │ +
153 Eigen::Map<Jacobian>* operator->() {
│ │ │ │ +
154 return &map_;
│ │ │ │ +
155 }
│ │ │ │ +
│ │ │ │ +
156
│ │ │ │ +
159 // template <int M, int N>
│ │ │ │ +
160 // OptionalJacobian<M, N> block(int startRow, int startCol) {
│ │ │ │ +
161 // if (*this)
│ │ │ │ +
162 // OptionalJacobian<M, N>(map_.block<M, N>(startRow, startCol));
│ │ │ │ +
163 // else
│ │ │ │ +
164 // return OptionalJacobian<M, N>();
│ │ │ │ +
165 // }
│ │ │ │ +
166
│ │ │ │ +
170 template <int N>
│ │ │ │ +
│ │ │ │ +
171 OptionalJacobian<Rows, N> cols(int startCol) {
│ │ │ │ +
172 if (*this)
│ │ │ │ +
173 return OptionalJacobian<Rows, N>(&map_(0,startCol));
│ │ │ │ +
174 else
│ │ │ │ +
175 return OptionalJacobian<Rows, N>();
│ │ │ │ +
176 }
│ │ │ │ +
│ │ │ │ +
177
│ │ │ │ +
182};
│ │ │ │ +
│ │ │ │ +
183
│ │ │ │ +
184// The pure dynamic specialization of this is needed to support
│ │ │ │ +
185// variable-sized types. Note that this is designed to work like the
│ │ │ │ +
186// boost optional scheme from GTSAM 3.
│ │ │ │ +
187template<>
│ │ │ │ +
│ │ │ │ +
188class OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic> {
│ │ │ │ +
189
│ │ │ │ +
190public:
│ │ │ │ +
191
│ │ │ │ +
193 typedef Eigen::MatrixXd Jacobian;
│ │ │ │ +
194
│ │ │ │ +
195private:
│ │ │ │ +
196
│ │ │ │ +
197 Jacobian* pointer_;
│ │ │ │ +
198
│ │ │ │ +
199public:
│ │ │ │ +
200
│ │ │ │ +
│ │ │ │ +
202 OptionalJacobian() :
│ │ │ │ +
203 pointer_(nullptr) {
│ │ │ │ +
204 }
│ │ │ │ +
│ │ │ │ +
205
│ │ │ │ +
207 OptionalJacobian(Jacobian* pointer) : pointer_(pointer) {}
│ │ │ │ +
208
│ │ │ │ +
210 OptionalJacobian(Jacobian& dynamic) : pointer_(&dynamic) {}
│ │ │ │ +
211
│ │ │ │ +
212#ifndef OPTIONALJACOBIAN_NOBOOST
│ │ │ │ +
213
│ │ │ │ +
│ │ │ │ +
215 OptionalJacobian(boost::none_t /*none*/) :
│ │ │ │ +
216 pointer_(nullptr) {
│ │ │ │ +
217 }
│ │ │ │ +
│ │ │ │ +
218
│ │ │ │ +
│ │ │ │ +
220 OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
│ │ │ │ +
221 pointer_(nullptr) {
│ │ │ │ +
222 if (optional) pointer_ = &(*optional);
│ │ │ │ +
223 }
│ │ │ │ +
│ │ │ │ +
224
│ │ │ │ +
225#endif
│ │ │ │ +
226
│ │ │ │ +
│ │ │ │ +
228 operator bool() const {
│ │ │ │ +
229 return pointer_!=nullptr;
│ │ │ │ +
230 }
│ │ │ │ +
│ │ │ │ +
231
│ │ │ │ +
│ │ │ │ +
233 Jacobian& operator*() {
│ │ │ │ +
234 return *pointer_;
│ │ │ │ +
235 }
│ │ │ │ +
│ │ │ │ +
236
│ │ │ │ +
238 Jacobian* operator->(){ return pointer_; }
│ │ │ │ +
239};
│ │ │ │ +
│ │ │ │ +
240
│ │ │ │ +
241// forward declare
│ │ │ │ +
242template <typename T> struct traits;
│ │ │ │ +
243
│ │ │ │ +
249template <class T, class A>
│ │ │ │ +
│ │ │ │ +
250struct MakeJacobian {
│ │ │ │ +
251 typedef Eigen::Matrix<double, traits<T>::dimension, traits<A>::dimension> type;
│ │ │ │ +
252};
│ │ │ │ +
│ │ │ │ +
253
│ │ │ │ +
260template<class T, class A>
│ │ │ │ +
│ │ │ │ +
261struct MakeOptionalJacobian {
│ │ │ │ +
262 typedef OptionalJacobian<traits<T>::dimension,
│ │ │ │ +
263 traits<A>::dimension> type;
│ │ │ │ +
264};
│ │ │ │ +
│ │ │ │ +
265
│ │ │ │ +
266} // namespace gtsam
│ │ │ │ +
267
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
equals with an tolerance, prints out message if unequal
Definition Matrix.cpp:43
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::equal
bool equal(const T &obj1, const T &obj2, double tol)
Call equal on the object.
Definition Testable.h:84
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::IsTestable
A testable concept check that should be placed in applicable unit tests and in generic algorithms.
Definition Testable.h:58
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::equals_star
Binary predicate on shared pointers.
Definition Testable.h:123
│ │ │ │ -
gtsam::HasTestablePrereqs
Requirements on type to pass it to Testable template below.
Definition Testable.h:134
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ +
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian(const boost::optional< Eigen::MatrixXd & > optional)
Constructor compatible with old-style derivatives.
Definition OptionalJacobian.h:127
│ │ │ │ +
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian(boost::none_t)
Constructor with boost::none just makes empty.
Definition OptionalJacobian.h:122
│ │ │ │ +
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian(Eigen::MatrixXd &dynamic)
Constructor that will resize a dynamic matrix (unless already correct)
Definition OptionalJacobian.h:88
│ │ │ │ +
gtsam::OptionalJacobian::operator*
Eigen::Map< Jacobian > & operator*()
De-reference, like boost optional.
Definition OptionalJacobian.h:148
│ │ │ │ +
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian()
Default constructor acts like boost::none.
Definition OptionalJacobian.h:70
│ │ │ │ +
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian(Jacobian &fixed)
Constructor that will usurp data of a fixed-size matrix.
Definition OptionalJacobian.h:75
│ │ │ │ +
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian(Eigen::Ref< MATRIX > dynamic_ref)
Constructor from an Eigen::Ref value.
Definition OptionalJacobian.h:106
│ │ │ │ +
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian(Eigen::MatrixXd *dynamic)
Constructor that will resize a dynamic matrix (unless already correct)
Definition OptionalJacobian.h:95
│ │ │ │ +
gtsam::OptionalJacobian::operator->
Eigen::Map< Jacobian > * operator->()
operator->()
Definition OptionalJacobian.h:153
│ │ │ │ +
gtsam::OptionalJacobian::Jacobian
Eigen::Matrix< double, Rows, Cols > Jacobian
Jacobian size type TODO(frank): how to enforce RowMajor? Or better, make it work with any storage ord...
Definition OptionalJacobian.h:47
│ │ │ │ +
gtsam::OptionalJacobian::OptionalJacobian
OptionalJacobian(Jacobian *fixedPtr)
Constructor that will usurp data of a fixed-size matrix, pointer version.
Definition OptionalJacobian.h:81
│ │ │ │ +
gtsam::OptionalJacobian::cols
OptionalJacobian< Rows, N > cols(int startCol)
Access M*N sub-block if we are allocated, otherwise none TODO(frank): this could work as is below if ...
Definition OptionalJacobian.h:171
│ │ │ │ +
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::OptionalJacobian
OptionalJacobian()
View on constructor argument, if given.
Definition OptionalJacobian.h:202
│ │ │ │ +
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::OptionalJacobian
OptionalJacobian(const boost::optional< Eigen::MatrixXd & > optional)
Constructor compatible with old-style derivatives.
Definition OptionalJacobian.h:220
│ │ │ │ +
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::Jacobian
Eigen::MatrixXd Jacobian
Jacobian size type.
Definition OptionalJacobian.h:193
│ │ │ │ +
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::operator*
Jacobian & operator*()
De-reference, like boost optional.
Definition OptionalJacobian.h:233
│ │ │ │ +
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::OptionalJacobian
OptionalJacobian(boost::none_t)
Constructor with boost::none just makes empty.
Definition OptionalJacobian.h:215
│ │ │ │ +
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::OptionalJacobian
OptionalJacobian(Jacobian *pointer)
Construct from pointer to dynamic matrix.
Definition OptionalJacobian.h:207
│ │ │ │ +
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::OptionalJacobian
OptionalJacobian(Jacobian &dynamic)
Construct from refrence to dynamic matrix.
Definition OptionalJacobian.h:210
│ │ │ │ +
gtsam::OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >::operator->
Jacobian * operator->()
TODO: operator->()
Definition OptionalJacobian.h:238
│ │ │ │ +
gtsam::MakeJacobian
: meta-function to generate Jacobian
Definition OptionalJacobian.h:250
│ │ │ │ +
gtsam::MakeOptionalJacobian
: meta-function to generate JacobianTA optional reference Used mainly by Expressions
Definition OptionalJacobian.h:261
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,175 +1,321 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Testable.h │ │ │ │ │ +OptionalJacobian.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -32// \callgraph │ │ │ │ │ -33 │ │ │ │ │ -34#pragma once │ │ │ │ │ -35 │ │ │ │ │ -36#include │ │ │ │ │ -37#include │ │ │ │ │ -38#include │ │ │ │ │ -39#include │ │ │ │ │ -40#include │ │ │ │ │ -41 │ │ │ │ │ -42#define GTSAM_PRINT(x)((x).print(#x)) │ │ │ │ │ -43 │ │ │ │ │ -44namespace _g_t_s_a_m { │ │ │ │ │ -45 │ │ │ │ │ -46 // Forward declaration │ │ │ │ │ -47 template struct traits; │ │ │ │ │ +20#pragma once │ │ │ │ │ +21#include // Configuration from CMake │ │ │ │ │ +22#include │ │ │ │ │ +23#include │ │ │ │ │ +24#include │ │ │ │ │ +25 │ │ │ │ │ +26#ifndef OPTIONALJACOBIAN_NOBOOST │ │ │ │ │ +27#include │ │ │ │ │ +28#endif │ │ │ │ │ +29 │ │ │ │ │ +30namespace _g_t_s_a_m { │ │ │ │ │ +31 │ │ │ │ │ +40template │ │ │ │ │ +_4_1class _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n { │ │ │ │ │ +42 │ │ │ │ │ +43public: │ │ │ │ │ +44 │ │ │ │ │ +_4_7 typedef Eigen::Matrix _J_a_c_o_b_i_a_n; │ │ │ │ │ 48 │ │ │ │ │ -57 template │ │ │ │ │ -_5_8 class _I_s_T_e_s_t_a_b_l_e { │ │ │ │ │ -59 T t; │ │ │ │ │ -60 bool r1,r2; │ │ │ │ │ -61 public: │ │ │ │ │ -62 │ │ │ │ │ -63 BOOST_CONCEPT_USAGE(_I_s_T_e_s_t_a_b_l_e) { │ │ │ │ │ -64 // check print function, with optional string │ │ │ │ │ -65 _t_r_a_i_t_s_<_T_>_:_:_P_r_i_n_t(t, std::string()); │ │ │ │ │ -66 _t_r_a_i_t_s_<_T_>_:_:_P_r_i_n_t(t); │ │ │ │ │ -67 │ │ │ │ │ -68 // check print, with optional threshold │ │ │ │ │ -69 double tol = 1.0; │ │ │ │ │ -70 r1 = _t_r_a_i_t_s_<_T_>_:_:_E_q_u_a_l_s(t,t,tol); │ │ │ │ │ -71 r2 = _t_r_a_i_t_s_<_T_>_:_:_E_q_u_a_l_s(t,t); │ │ │ │ │ +49private: │ │ │ │ │ +50 │ │ │ │ │ +51 Eigen::Map map_; │ │ │ │ │ +52 │ │ │ │ │ +53 // Trick from http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html │ │ │ │ │ +54 // uses "placement new" to make map_ usurp the memory of the fixed size │ │ │ │ │ +matrix │ │ │ │ │ +55 void usurp(double* data) { │ │ │ │ │ +56 new (&map_) Eigen::Map(data); │ │ │ │ │ +57 } │ │ │ │ │ +58 │ │ │ │ │ +59 // Private and very dangerous constructor straight from memory │ │ │ │ │ +60 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n(double* data) : map_(nullptr) { │ │ │ │ │ +61 if (data) usurp(data); │ │ │ │ │ +62 } │ │ │ │ │ +63 │ │ │ │ │ +64 template │ │ │ │ │ +65 friend class OptionalJacobian; │ │ │ │ │ +66 │ │ │ │ │ +67public: │ │ │ │ │ +68 │ │ │ │ │ +_7_0 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n() : │ │ │ │ │ +71 map_(nullptr) { │ │ │ │ │ 72 } │ │ │ │ │ -73 }; // \ Testable │ │ │ │ │ -74 │ │ │ │ │ -75 inline void _p_r_i_n_t(float v, const std::string& s = "") { │ │ │ │ │ -76 std::cout << (s.empty() ? s : s + " ") << v << std::endl; │ │ │ │ │ -77 } │ │ │ │ │ -78 inline void _p_r_i_n_t(double v, const std::string& s = "") { │ │ │ │ │ -79 std::cout << (s.empty() ? s : s + " ") << v << std::endl; │ │ │ │ │ -80 } │ │ │ │ │ -81 │ │ │ │ │ -83 template │ │ │ │ │ -_8_4 inline bool _e_q_u_a_l(const T& obj1, const T& obj2, double tol) { │ │ │ │ │ -85 return _t_r_a_i_t_s_<_T_>_:_:_E_q_u_a_l_s(obj1,obj2, tol); │ │ │ │ │ -86 } │ │ │ │ │ -87 │ │ │ │ │ -89 template │ │ │ │ │ -_9_0 inline bool _e_q_u_a_l(const T& obj1, const T& obj2) { │ │ │ │ │ -91 return _t_r_a_i_t_s_<_T_>_:_:_E_q_u_a_l_s(obj1,obj2); │ │ │ │ │ +73 │ │ │ │ │ +_7_5 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n(_J_a_c_o_b_i_a_n& fixed) : │ │ │ │ │ +76 map_(nullptr) { │ │ │ │ │ +77 usurp(fixed.data()); │ │ │ │ │ +78 } │ │ │ │ │ +79 │ │ │ │ │ +_8_1 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n(_J_a_c_o_b_i_a_n* fixedPtr) : │ │ │ │ │ +82 map_(nullptr) { │ │ │ │ │ +83 if (fixedPtr) │ │ │ │ │ +84 usurp(fixedPtr->data()); │ │ │ │ │ +85 } │ │ │ │ │ +86 │ │ │ │ │ +_8_8 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n(Eigen::MatrixXd& dynamic) : │ │ │ │ │ +89 map_(nullptr) { │ │ │ │ │ +90 dynamic.resize(Rows, Cols); // no malloc if correct size │ │ │ │ │ +91 usurp(dynamic.data()); │ │ │ │ │ 92 } │ │ │ │ │ 93 │ │ │ │ │ -97 template │ │ │ │ │ -_9_8 bool _a_s_s_e_r_t___e_q_u_a_l(const V& expected, const V& actual, double tol = 1e-9) { │ │ │ │ │ -99 if (_t_r_a_i_t_s_<_V_>_:_:_E_q_u_a_l_s(actual,expected, tol)) │ │ │ │ │ -100 return true; │ │ │ │ │ -101 printf("Not equal:\n"); │ │ │ │ │ -102 _t_r_a_i_t_s_<_V_>_:_:_P_r_i_n_t(expected,"expected:\n"); │ │ │ │ │ -103 _t_r_a_i_t_s_<_V_>_:_:_P_r_i_n_t(actual,"actual:\n"); │ │ │ │ │ -104 return false; │ │ │ │ │ -105 } │ │ │ │ │ -106 │ │ │ │ │ -110 template │ │ │ │ │ -_1_1_1 struct _e_q_u_a_l_s : public std::function { │ │ │ │ │ -112 double tol_; │ │ │ │ │ -113 _e_q_u_a_l_s(double tol = 1e-9) : tol_(tol) {} │ │ │ │ │ -114 bool operator()(const V& expected, const V& actual) { │ │ │ │ │ -115 return (_t_r_a_i_t_s_<_V_>_:_:_E_q_u_a_l_s(actual, expected, tol_)); │ │ │ │ │ +_9_5 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n(Eigen::MatrixXd* dynamic) : │ │ │ │ │ +96 map_(nullptr) { │ │ │ │ │ +97 dynamic->resize(Rows, Cols); // no malloc if correct size │ │ │ │ │ +98 usurp(dynamic->data()); │ │ │ │ │ +99 } │ │ │ │ │ +100 │ │ │ │ │ +105 template │ │ │ │ │ +_1_0_6 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n(Eigen::Ref dynamic_ref) : │ │ │ │ │ +107 map_(nullptr) { │ │ │ │ │ +108 if (dynamic_ref.rows() == Rows && dynamic_ref.cols() == Cols && │ │ │ │ │ +!dynamic_ref.IsRowMajor) { │ │ │ │ │ +109 usurp(dynamic_ref.data()); │ │ │ │ │ +110 } else { │ │ │ │ │ +111 throw std::invalid_argument( │ │ │ │ │ +112 std::string("OptionalJacobian called with wrong dimensions or " │ │ │ │ │ +113 "storage order.\n" │ │ │ │ │ +114 "Expected: ") + │ │ │ │ │ +115 "(" + std::to_string(Rows) + ", " + std::to_string(Cols) + ")"); │ │ │ │ │ 116 } │ │ │ │ │ -117 }; │ │ │ │ │ +117 } │ │ │ │ │ 118 │ │ │ │ │ -122 template │ │ │ │ │ -_1_2_3 struct _e_q_u_a_l_s___s_t_a_r : public std::function&, │ │ │ │ │ -const std::shared_ptr&)> { │ │ │ │ │ -124 double tol_; │ │ │ │ │ -125 _e_q_u_a_l_s___s_t_a_r(double tol = 1e-9) : tol_(tol) {} │ │ │ │ │ -126 bool operator()(const std::shared_ptr& expected, const std:: │ │ │ │ │ -shared_ptr& actual) { │ │ │ │ │ -127 if (!actual && !expected) return true; │ │ │ │ │ -128 return actual && expected && _t_r_a_i_t_s_<_V_>_:_:_E_q_u_a_l_s(*actual,*expected, tol_); │ │ │ │ │ -129 } │ │ │ │ │ -130 }; │ │ │ │ │ -131 │ │ │ │ │ -133 template │ │ │ │ │ -_1_3_4 struct _H_a_s_T_e_s_t_a_b_l_e_P_r_e_r_e_q_s { │ │ │ │ │ -135 │ │ │ │ │ -136 BOOST_CONCEPT_USAGE(_H_a_s_T_e_s_t_a_b_l_e_P_r_e_r_e_q_s) { │ │ │ │ │ -137 t->print(str); │ │ │ │ │ -138 b = t->equals(*s,tol); │ │ │ │ │ -139 } │ │ │ │ │ -140 │ │ │ │ │ -141 T *t, *s; // Pointer is to allow abstract classes │ │ │ │ │ -142 bool b; │ │ │ │ │ -143 double tol; │ │ │ │ │ -144 std::string str; │ │ │ │ │ -145 }; │ │ │ │ │ +119#ifndef OPTIONALJACOBIAN_NOBOOST │ │ │ │ │ +120 │ │ │ │ │ +_1_2_2 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n(boost::none_t /*none*/) : │ │ │ │ │ +123 map_(nullptr) { │ │ │ │ │ +124 } │ │ │ │ │ +125 │ │ │ │ │ +_1_2_7 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n(const boost::optional optional) : │ │ │ │ │ +128 map_(nullptr) { │ │ │ │ │ +129 if (optional) { │ │ │ │ │ +130 optional->resize(Rows, Cols); │ │ │ │ │ +131 usurp(optional->data()); │ │ │ │ │ +132 } │ │ │ │ │ +133 } │ │ │ │ │ +134 │ │ │ │ │ +135#endif │ │ │ │ │ +136 │ │ │ │ │ +139 // template │ │ │ │ │ +140 // OptionalJacobian(Eigen::Block block) : │ │ │ │ │ +map_(nullptr) { ?? } │ │ │ │ │ +141 │ │ │ │ │ +_1_4_3 operator bool() const { │ │ │ │ │ +144 return map_.data() != nullptr; │ │ │ │ │ +145 } │ │ │ │ │ 146 │ │ │ │ │ -150 template │ │ │ │ │ -_1_5_1 struct _T_e_s_t_a_b_l_e { │ │ │ │ │ -152 │ │ │ │ │ -153 // Check that T has the necessary methods │ │ │ │ │ -154 BOOST_CONCEPT_ASSERT((_H_a_s_T_e_s_t_a_b_l_e_P_r_e_r_e_q_s_<_T_>)); │ │ │ │ │ -155 │ │ │ │ │ -156 static void Print(const T& m, const std::string& str = "") { │ │ │ │ │ -157 m.print(str); │ │ │ │ │ -158 } │ │ │ │ │ -159 static bool Equals(const T& m1, const T& m2, double tol = 1e-8) { │ │ │ │ │ -160 return m1.equals(m2, tol); │ │ │ │ │ -161 } │ │ │ │ │ -162 }; │ │ │ │ │ -163 │ │ │ │ │ -164} // \namespace gtsam │ │ │ │ │ -165 │ │ │ │ │ -_1_7_5#define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::IsTestable; │ │ │ │ │ -176#define GTSAM_CONCEPT_TESTABLE_TYPE(T) using _gtsam_Testable_##T = gtsam:: │ │ │ │ │ -IsTestable; │ │ │ │ │ +_1_4_8 Eigen::Map& _o_p_e_r_a_t_o_r_*() { │ │ │ │ │ +149 return map_; │ │ │ │ │ +150 } │ │ │ │ │ +151 │ │ │ │ │ +_1_5_3 Eigen::Map* _o_p_e_r_a_t_o_r_-_>() { │ │ │ │ │ +154 return &map_; │ │ │ │ │ +155 } │ │ │ │ │ +156 │ │ │ │ │ +159 // template │ │ │ │ │ +160 // OptionalJacobian block(int startRow, int startCol) { │ │ │ │ │ +161 // if (*this) │ │ │ │ │ +162 // OptionalJacobian(map_.block(startRow, startCol)); │ │ │ │ │ +163 // else │ │ │ │ │ +164 // return OptionalJacobian(); │ │ │ │ │ +165 // } │ │ │ │ │ +166 │ │ │ │ │ +170 template │ │ │ │ │ +_1_7_1 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_R_o_w_s_,_ _N_> _c_o_l_s(int startCol) { │ │ │ │ │ +172 if (*this) │ │ │ │ │ +173 return _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_R_o_w_s_,_ _N_>(&map_(0,startCol)); │ │ │ │ │ +174 else │ │ │ │ │ +175 return _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_R_o_w_s_,_ _N_>(); │ │ │ │ │ +176 } │ │ │ │ │ +177 │ │ │ │ │ +182}; │ │ │ │ │ +183 │ │ │ │ │ +184// The pure dynamic specialization of this is needed to support │ │ │ │ │ +185// variable-sized types. Note that this is designed to work like the │ │ │ │ │ +186// boost optional scheme from GTSAM 3. │ │ │ │ │ +187template<> │ │ │ │ │ +_1_8_8class _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n { │ │ │ │ │ +189 │ │ │ │ │ +190public: │ │ │ │ │ +191 │ │ │ │ │ +_1_9_3 typedef Eigen::MatrixXd _J_a_c_o_b_i_a_n; │ │ │ │ │ +194 │ │ │ │ │ +195private: │ │ │ │ │ +196 │ │ │ │ │ +197 _J_a_c_o_b_i_a_n* pointer_; │ │ │ │ │ +198 │ │ │ │ │ +199public: │ │ │ │ │ +200 │ │ │ │ │ +_2_0_2 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n() : │ │ │ │ │ +203 pointer_(nullptr) { │ │ │ │ │ +204 } │ │ │ │ │ +205 │ │ │ │ │ +_2_0_7 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n(_J_a_c_o_b_i_a_n* pointer) : pointer_(pointer) {} │ │ │ │ │ +208 │ │ │ │ │ +_2_1_0 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n(_J_a_c_o_b_i_a_n& dynamic) : pointer_(&dynamic) {} │ │ │ │ │ +211 │ │ │ │ │ +212#ifndef OPTIONALJACOBIAN_NOBOOST │ │ │ │ │ +213 │ │ │ │ │ +_2_1_5 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n(boost::none_t /*none*/) : │ │ │ │ │ +216 pointer_(nullptr) { │ │ │ │ │ +217 } │ │ │ │ │ +218 │ │ │ │ │ +_2_2_0 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n(const boost::optional optional) : │ │ │ │ │ +221 pointer_(nullptr) { │ │ │ │ │ +222 if (optional) pointer_ = &(*optional); │ │ │ │ │ +223 } │ │ │ │ │ +224 │ │ │ │ │ +225#endif │ │ │ │ │ +226 │ │ │ │ │ +_2_2_8 operator bool() const { │ │ │ │ │ +229 return pointer_!=nullptr; │ │ │ │ │ +230 } │ │ │ │ │ +231 │ │ │ │ │ +_2_3_3 _J_a_c_o_b_i_a_n& _o_p_e_r_a_t_o_r_*() { │ │ │ │ │ +234 return *pointer_; │ │ │ │ │ +235 } │ │ │ │ │ +236 │ │ │ │ │ +_2_3_8 _J_a_c_o_b_i_a_n* _o_p_e_r_a_t_o_r_-_>(){ return pointer_; } │ │ │ │ │ +239}; │ │ │ │ │ +240 │ │ │ │ │ +241// forward declare │ │ │ │ │ +242template struct traits; │ │ │ │ │ +243 │ │ │ │ │ +249template │ │ │ │ │ +_2_5_0struct _M_a_k_e_J_a_c_o_b_i_a_n { │ │ │ │ │ +251 typedef Eigen::Matrix::dimension, _t_r_a_i_t_s_<_A_>_:_:_d_i_m_e_n_s_i_o_n> │ │ │ │ │ +type; │ │ │ │ │ +252}; │ │ │ │ │ +253 │ │ │ │ │ +260template │ │ │ │ │ +_2_6_1struct _M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n { │ │ │ │ │ +262 typedef _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_t_r_a_i_t_s_<_T_>_:_:_d_i_m_e_n_s_i_o_n, │ │ │ │ │ +263 _t_r_a_i_t_s_<_A_>_:_:_d_i_m_e_n_s_i_o_n> type; │ │ │ │ │ +264}; │ │ │ │ │ +265 │ │ │ │ │ +266} // namespace gtsam │ │ │ │ │ +267 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_a_s_s_e_r_t___e_q_u_a_l │ │ │ │ │ -bool assert_equal(const Matrix &expected, const Matrix &actual, double tol) │ │ │ │ │ -equals with an tolerance, prints out message if unequal │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:43 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l │ │ │ │ │ -bool equal(const T &obj1, const T &obj2, double tol) │ │ │ │ │ -Call equal on the object. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:84 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_I_s_T_e_s_t_a_b_l_e │ │ │ │ │ -A testable concept check that should be placed in applicable unit tests and in │ │ │ │ │ -generic algorithms. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s___s_t_a_r │ │ │ │ │ -Binary predicate on shared pointers. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:123 │ │ │ │ │ -_g_t_s_a_m_:_:_H_a_s_T_e_s_t_a_b_l_e_P_r_e_r_e_q_s │ │ │ │ │ -Requirements on type to pass it to Testable template below. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:134 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ +either a fixed size o... │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian(const boost::optional< Eigen::MatrixXd & > optional) │ │ │ │ │ +Constructor compatible with old-style derivatives. │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:127 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian(boost::none_t) │ │ │ │ │ +Constructor with boost::none just makes empty. │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:122 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian(Eigen::MatrixXd &dynamic) │ │ │ │ │ +Constructor that will resize a dynamic matrix (unless already correct) │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:88 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ +Eigen::Map< Jacobian > & operator*() │ │ │ │ │ +De-reference, like boost optional. │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:148 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian() │ │ │ │ │ +Default constructor acts like boost::none. │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:70 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian(Jacobian &fixed) │ │ │ │ │ +Constructor that will usurp data of a fixed-size matrix. │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian(Eigen::Ref< MATRIX > dynamic_ref) │ │ │ │ │ +Constructor from an Eigen::Ref value. │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:106 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian(Eigen::MatrixXd *dynamic) │ │ │ │ │ +Constructor that will resize a dynamic matrix (unless already correct) │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:95 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_:_:_o_p_e_r_a_t_o_r_-_> │ │ │ │ │ +Eigen::Map< Jacobian > * operator->() │ │ │ │ │ +operator->() │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:153 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_:_:_J_a_c_o_b_i_a_n │ │ │ │ │ +Eigen::Matrix< double, Rows, Cols > Jacobian │ │ │ │ │ +Jacobian size type TODO(frank): how to enforce RowMajor? Or better, make it │ │ │ │ │ +work with any storage ord... │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian(Jacobian *fixedPtr) │ │ │ │ │ +Constructor that will usurp data of a fixed-size matrix, pointer version. │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:81 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_:_:_c_o_l_s │ │ │ │ │ +OptionalJacobian< Rows, N > cols(int startCol) │ │ │ │ │ +Access M*N sub-block if we are allocated, otherwise none TODO(frank): this │ │ │ │ │ +could work as is below if ... │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:171 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_,_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_ _>_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian() │ │ │ │ │ +View on constructor argument, if given. │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:202 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_,_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_ _>_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian(const boost::optional< Eigen::MatrixXd & > optional) │ │ │ │ │ +Constructor compatible with old-style derivatives. │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:220 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_,_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_ _>_:_:_J_a_c_o_b_i_a_n │ │ │ │ │ +Eigen::MatrixXd Jacobian │ │ │ │ │ +Jacobian size type. │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:193 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_,_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_ _>_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ +Jacobian & operator*() │ │ │ │ │ +De-reference, like boost optional. │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:233 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_,_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_ _>_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian(boost::none_t) │ │ │ │ │ +Constructor with boost::none just makes empty. │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:215 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_,_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_ _>_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian(Jacobian *pointer) │ │ │ │ │ +Construct from pointer to dynamic matrix. │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:207 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_,_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_ _>_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian(Jacobian &dynamic) │ │ │ │ │ +Construct from refrence to dynamic matrix. │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:210 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_,_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_ _>_:_:_o_p_e_r_a_t_o_r_-_> │ │ │ │ │ +Jacobian * operator->() │ │ │ │ │ +TODO: operator->() │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:238 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_k_e_J_a_c_o_b_i_a_n │ │ │ │ │ +: meta-function to generate Jacobian │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:250 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +: meta-function to generate JacobianTA optional reference Used mainly by │ │ │ │ │ +Expressions │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:261 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _T_e_s_t_a_b_l_e_._h │ │ │ │ │ + * _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00089.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/DSFVector.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/ThreadsafeException.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,46 +96,55 @@ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
DSFVector.h File Reference
│ │ │ │ +
ThreadsafeException.h File Reference
Base
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

A faster implementation for DSF, which uses vector rather than btree. │ │ │ │ +

Base exception type that uses tbb_allocator if GTSAM is compiled with TBB. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::DSFBase
 A fast implementation of disjoint set forests that uses vector as underly data structure. More...
class  gtsam::ThreadsafeException< DERIVED >
 Base exception type that uses tbb_allocator if GTSAM is compiled with TBB. More...
 
class  gtsam::DSFVector
 DSFVector additionally keeps a vector of keys to support more expensive operations. More...
class  gtsam::RuntimeErrorThreadsafe
 Thread-safe runtime error exception. More...
 
class  gtsam::OutOfRangeThreadsafe
 Thread-safe out of range exception. More...
 
class  gtsam::InvalidArgumentThreadsafe
 Thread-safe invalid argument exception. More...
 
class  gtsam::CholeskyFailed
 Indicate Cholesky factorization failure. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

A faster implementation for DSF, which uses vector rather than btree.

│ │ │ │ -
Date
Jun 25, 2010
│ │ │ │ -
Author
Kai Ni
│ │ │ │ +

Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Aug 21, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,32 +1,42 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -DSFVector.h File Reference │ │ │ │ │ -A faster implementation for DSF, which uses vector rather than btree. _M_o_r_e_._._. │ │ │ │ │ +ThreadsafeException.h File Reference │ │ │ │ │ +_B_a_s_e │ │ │ │ │ +Base exception type that uses tbb_allocator if GTSAM is compiled with TBB. │ │ │ │ │ +_M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_D_S_F_B_a_s_e │ │ │ │ │ -  A fast implementation of disjoint set forests that uses vector as │ │ │ │ │ - underly data structure. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_D_S_F_V_e_c_t_o_r │ │ │ │ │ -  _D_S_F_V_e_c_t_o_r additionally keeps a vector of keys to support more expensive │ │ │ │ │ - operations. _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_<_ _D_E_R_I_V_E_D_ _> │ │ │ │ │ +  Base exception type that uses tbb_allocator if GTSAM is compiled with │ │ │ │ │ + TBB. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_R_u_n_t_i_m_e_E_r_r_o_r_T_h_r_e_a_d_s_a_f_e │ │ │ │ │ +  Thread-safe runtime error exception. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_O_u_t_O_f_R_a_n_g_e_T_h_r_e_a_d_s_a_f_e │ │ │ │ │ +  Thread-safe out of range exception. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_I_n_v_a_l_i_d_A_r_g_u_m_e_n_t_T_h_r_e_a_d_s_a_f_e │ │ │ │ │ +  Thread-safe invalid argument exception. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_C_h_o_l_e_s_k_y_F_a_i_l_e_d │ │ │ │ │ +  Indicate Cholesky factorization failure. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A faster implementation for DSF, which uses vector rather than btree. │ │ │ │ │ - Date │ │ │ │ │ - Jun 25, 2010 │ │ │ │ │ +Base exception type that uses tbb_allocator if GTSAM is compiled with TBB. │ │ │ │ │ Author │ │ │ │ │ - Kai Ni │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Aug 21, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _D_S_F_V_e_c_t_o_r_._h │ │ │ │ │ + * _T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00089_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/DSFVector.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/ThreadsafeException.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,96 +98,160 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
DSFVector.h
│ │ │ │ +
ThreadsafeException.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/dllexport.h>
│ │ │ │ -
22#include <gtsam/global_includes.h>
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/config.h> // for GTSAM_USE_TBB
│ │ │ │
23
│ │ │ │ -
24#include <boost/shared_ptr.hpp>
│ │ │ │ -
25
│ │ │ │ -
26#include <vector>
│ │ │ │ -
27#include <set>
│ │ │ │ -
28#include <map>
│ │ │ │ +
24#include <boost/optional/optional.hpp>
│ │ │ │ +
25#include <gtsam/dllexport.h>
│ │ │ │ +
26#include <string>
│ │ │ │ +
27#include <typeinfo>
│ │ │ │ +
28#include <exception>
│ │ │ │
29
│ │ │ │ -
30namespace gtsam {
│ │ │ │ -
31
│ │ │ │ -
│ │ │ │ -
38class GTSAM_EXPORT DSFBase {
│ │ │ │ -
39
│ │ │ │ -
40public:
│ │ │ │ -
41 typedef std::vector<size_t> V;
│ │ │ │ -
42
│ │ │ │ +
30#ifdef GTSAM_USE_TBB
│ │ │ │ +
31#include <tbb/tbb_allocator.h>
│ │ │ │ +
32#include <tbb/scalable_allocator.h>
│ │ │ │ +
33#include <iostream>
│ │ │ │ +
34#endif
│ │ │ │ +
35
│ │ │ │ +
36namespace gtsam {
│ │ │ │ +
37
│ │ │ │ +
39template<class DERIVED>
│ │ │ │ +
│ │ │ │ +
40class ThreadsafeException:
│ │ │ │ +
41public std::exception
│ │ │ │ +
42{
│ │ │ │
43private:
│ │ │ │ -
44 boost::shared_ptr<V> v_;
│ │ │ │ -
45
│ │ │ │ -
46public:
│ │ │ │ -
48 DSFBase(const size_t numNodes);
│ │ │ │ -
49
│ │ │ │ -
51 DSFBase(const boost::shared_ptr<V>& v_in);
│ │ │ │ -
52
│ │ │ │ -
54 size_t find(size_t key) const;
│ │ │ │ -
55
│ │ │ │ -
57 void merge(const size_t& i1, const size_t& i2);
│ │ │ │ -
58};
│ │ │ │ -
│ │ │ │ -
59
│ │ │ │ -
│ │ │ │ -
64class GTSAM_EXPORT DSFVector: public DSFBase {
│ │ │ │ -
65
│ │ │ │ -
66private:
│ │ │ │ -
67 std::vector<size_t> keys_;
│ │ │ │ -
68
│ │ │ │ -
69public:
│ │ │ │ -
71 DSFVector(const size_t numNodes);
│ │ │ │ -
72
│ │ │ │ -
74 DSFVector(const std::vector<size_t>& keys);
│ │ │ │ -
75
│ │ │ │ -
77 DSFVector(const boost::shared_ptr<V>& v_in, const std::vector<size_t>& keys);
│ │ │ │ -
78
│ │ │ │ -
79 // All operations below loop over all keys and hence are *at least* O(n)
│ │ │ │ -
80
│ │ │ │ -
82 bool isSingleton(const size_t& label) const;
│ │ │ │ +
44 typedef std::exception Base;
│ │ │ │ +
45#ifdef GTSAM_USE_TBB
│ │ │ │ +
46protected:
│ │ │ │ +
47 typedef std::basic_string<char, std::char_traits<char>,
│ │ │ │ +
48 tbb::tbb_allocator<char> > String;
│ │ │ │ +
49#else
│ │ │ │ +
50protected:
│ │ │ │ +
51 typedef std::string String;
│ │ │ │ +
52#endif
│ │ │ │ +
53
│ │ │ │ +
54protected:
│ │ │ │ +
55 bool dynamic_;
│ │ │ │ +
56 mutable boost::optional<String> description_;
│ │ │ │ +
57
│ │ │ │ +
│ │ │ │ +
59 ThreadsafeException() :
│ │ │ │ +
60 dynamic_(false) {
│ │ │ │ +
61 }
│ │ │ │ +
│ │ │ │ +
62
│ │ │ │ +
│ │ │ │ +
64 ThreadsafeException(const ThreadsafeException& other) :
│ │ │ │ +
65 Base(other), dynamic_(false) {
│ │ │ │ +
66 }
│ │ │ │ +
│ │ │ │ +
67
│ │ │ │ +
│ │ │ │ +
69 ThreadsafeException(const std::string& description) :
│ │ │ │ +
70 dynamic_(false), description_(
│ │ │ │ +
71 String(description.begin(), description.end())) {
│ │ │ │ +
72 }
│ │ │ │ +
│ │ │ │ +
73
│ │ │ │ +
│ │ │ │ +
75 ~ThreadsafeException() noexcept override {
│ │ │ │ +
76 }
│ │ │ │ +
│ │ │ │ +
77
│ │ │ │ +
78public:
│ │ │ │ +
79 const char* what() const noexcept override {
│ │ │ │ +
80 return description_ ? description_->c_str() : "";
│ │ │ │ +
81 }
│ │ │ │ +
82};
│ │ │ │ +
│ │ │ │
83
│ │ │ │ -
85 std::set<size_t> set(const size_t& label) const;
│ │ │ │ -
86
│ │ │ │ -
88 std::map<size_t, std::set<size_t> > sets() const;
│ │ │ │ -
89
│ │ │ │ -
91 std::map<size_t, std::vector<size_t> > arrays() const;
│ │ │ │ -
92};
│ │ │ │ -
│ │ │ │ -
93
│ │ │ │ -
94}
│ │ │ │ -
global_includes.h
Included from all GTSAM files.
│ │ │ │ +
│ │ │ │ +
85class GTSAM_EXPORT RuntimeErrorThreadsafe: public ThreadsafeException<RuntimeErrorThreadsafe> {
│ │ │ │ +
86public:
│ │ │ │ +
│ │ │ │ +
88 RuntimeErrorThreadsafe(const std::string& description) :
│ │ │ │ +
89 ThreadsafeException<RuntimeErrorThreadsafe>(description) {
│ │ │ │ +
90 }
│ │ │ │ +
│ │ │ │ +
91};
│ │ │ │ +
│ │ │ │ +
92
│ │ │ │ +
│ │ │ │ +
94class OutOfRangeThreadsafe: public ThreadsafeException<OutOfRangeThreadsafe> {
│ │ │ │ +
95public:
│ │ │ │ +
│ │ │ │ +
97 OutOfRangeThreadsafe(const std::string& description) :
│ │ │ │ +
98 ThreadsafeException<OutOfRangeThreadsafe>(description) {
│ │ │ │ +
99 }
│ │ │ │ +
│ │ │ │ +
100};
│ │ │ │ +
│ │ │ │ +
101
│ │ │ │ +
│ │ │ │ +
103class InvalidArgumentThreadsafe: public ThreadsafeException<
│ │ │ │ +
104 InvalidArgumentThreadsafe> {
│ │ │ │ +
105public:
│ │ │ │ +
│ │ │ │ +
107 InvalidArgumentThreadsafe(const std::string& description) :
│ │ │ │ +
108 ThreadsafeException<InvalidArgumentThreadsafe>(description) {
│ │ │ │ +
109 }
│ │ │ │ +
│ │ │ │ +
110};
│ │ │ │ +
│ │ │ │ +
111
│ │ │ │ +
│ │ │ │ +
113class CholeskyFailed : public gtsam::ThreadsafeException<CholeskyFailed>
│ │ │ │ +
114{
│ │ │ │ +
115public:
│ │ │ │ +
116 CholeskyFailed() noexcept {}
│ │ │ │ +
117 ~CholeskyFailed() noexcept override {}
│ │ │ │ +
118};
│ │ │ │ +
│ │ │ │ +
119
│ │ │ │ +
120} // namespace gtsam
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::DSFBase
A fast implementation of disjoint set forests that uses vector as underly data structure.
Definition DSFVector.h:38
│ │ │ │ -
gtsam::DSFBase::V
std::vector< size_t > V
Vector of ints.
Definition DSFVector.h:41
│ │ │ │ -
gtsam::DSFVector
DSFVector additionally keeps a vector of keys to support more expensive operations.
Definition DSFVector.h:64
│ │ │ │ +
gtsam::ThreadsafeException
Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
Definition ThreadsafeException.h:42
│ │ │ │ +
gtsam::ThreadsafeException::~ThreadsafeException
~ThreadsafeException() noexcept override
Default destructor doesn't have the noexcept.
Definition ThreadsafeException.h:75
│ │ │ │ +
gtsam::ThreadsafeException::dynamic_
bool dynamic_
Whether this object was moved.
Definition ThreadsafeException.h:55
│ │ │ │ +
gtsam::ThreadsafeException::ThreadsafeException
ThreadsafeException()
Default constructor is protected - may only be created from derived classes.
Definition ThreadsafeException.h:59
│ │ │ │ +
gtsam::ThreadsafeException::ThreadsafeException
ThreadsafeException(const ThreadsafeException &other)
Copy constructor is protected - may only be created from derived classes.
Definition ThreadsafeException.h:64
│ │ │ │ +
gtsam::ThreadsafeException::ThreadsafeException
ThreadsafeException(const std::string &description)
Construct with description string.
Definition ThreadsafeException.h:69
│ │ │ │ +
gtsam::ThreadsafeException::description_
boost::optional< String > description_
Optional description.
Definition ThreadsafeException.h:56
│ │ │ │ +
gtsam::RuntimeErrorThreadsafe
Thread-safe runtime error exception.
Definition ThreadsafeException.h:85
│ │ │ │ +
gtsam::RuntimeErrorThreadsafe::RuntimeErrorThreadsafe
RuntimeErrorThreadsafe(const std::string &description)
Construct with a string describing the exception.
Definition ThreadsafeException.h:88
│ │ │ │ +
gtsam::OutOfRangeThreadsafe
Thread-safe out of range exception.
Definition ThreadsafeException.h:94
│ │ │ │ +
gtsam::OutOfRangeThreadsafe::OutOfRangeThreadsafe
OutOfRangeThreadsafe(const std::string &description)
Construct with a string describing the exception.
Definition ThreadsafeException.h:97
│ │ │ │ +
gtsam::InvalidArgumentThreadsafe
Thread-safe invalid argument exception.
Definition ThreadsafeException.h:104
│ │ │ │ +
gtsam::InvalidArgumentThreadsafe::InvalidArgumentThreadsafe
InvalidArgumentThreadsafe(const std::string &description)
Construct with a string describing the exception.
Definition ThreadsafeException.h:107
│ │ │ │ +
gtsam::CholeskyFailed
Indicate Cholesky factorization failure.
Definition ThreadsafeException.h:114
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,97 +1,169 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DSFVector.h │ │ │ │ │ +ThreadsafeException.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include │ │ │ │ │ -22#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include // for GTSAM_USE_TBB │ │ │ │ │ 23 │ │ │ │ │ -24#include │ │ │ │ │ -25 │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ -28#include │ │ │ │ │ +24#include │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28#include │ │ │ │ │ 29 │ │ │ │ │ -30namespace _g_t_s_a_m { │ │ │ │ │ -31 │ │ │ │ │ -_3_8class GTSAM_EXPORT _D_S_F_B_a_s_e { │ │ │ │ │ -39 │ │ │ │ │ -40public: │ │ │ │ │ -_4_1 typedef std::vector _V; │ │ │ │ │ -42 │ │ │ │ │ +30#ifdef GTSAM_USE_TBB │ │ │ │ │ +31#include │ │ │ │ │ +32#include │ │ │ │ │ +33#include │ │ │ │ │ +34#endif │ │ │ │ │ +35 │ │ │ │ │ +36namespace _g_t_s_a_m { │ │ │ │ │ +37 │ │ │ │ │ +39template │ │ │ │ │ +_4_0class _T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n: │ │ │ │ │ +41public std::exception │ │ │ │ │ +42{ │ │ │ │ │ 43private: │ │ │ │ │ -44 boost::shared_ptr v_; │ │ │ │ │ -45 │ │ │ │ │ -46public: │ │ │ │ │ -48 _D_S_F_B_a_s_e(const size_t numNodes); │ │ │ │ │ -49 │ │ │ │ │ -51 _D_S_F_B_a_s_e(const boost::shared_ptr& v_in); │ │ │ │ │ -52 │ │ │ │ │ -54 size_t find(size_t key) const; │ │ │ │ │ -55 │ │ │ │ │ -57 void merge(const size_t& i1, const size_t& i2); │ │ │ │ │ -58}; │ │ │ │ │ -59 │ │ │ │ │ -_6_4class GTSAM_EXPORT _D_S_F_V_e_c_t_o_r: public _D_S_F_B_a_s_e { │ │ │ │ │ -65 │ │ │ │ │ -66private: │ │ │ │ │ -67 std::vector keys_; │ │ │ │ │ -68 │ │ │ │ │ -69public: │ │ │ │ │ -71 _D_S_F_V_e_c_t_o_r(const size_t numNodes); │ │ │ │ │ -72 │ │ │ │ │ -74 _D_S_F_V_e_c_t_o_r(const std::vector& keys); │ │ │ │ │ -75 │ │ │ │ │ -77 _D_S_F_V_e_c_t_o_r(const boost::shared_ptr& v_in, const std::vector& │ │ │ │ │ -keys); │ │ │ │ │ -78 │ │ │ │ │ -79 // All operations below loop over all keys and hence are *at least* O(n) │ │ │ │ │ -80 │ │ │ │ │ -82 bool isSingleton(const size_t& label) const; │ │ │ │ │ +44 typedef std::exception Base; │ │ │ │ │ +45#ifdef GTSAM_USE_TBB │ │ │ │ │ +46protected: │ │ │ │ │ +47 typedef std::basic_string, │ │ │ │ │ +48 tbb::tbb_allocator > String; │ │ │ │ │ +49#else │ │ │ │ │ +50protected: │ │ │ │ │ +51 typedef std::string String; │ │ │ │ │ +52#endif │ │ │ │ │ +53 │ │ │ │ │ +54protected: │ │ │ │ │ +_5_5 bool _d_y_n_a_m_i_c__; │ │ │ │ │ +_5_6 mutable boost::optional _d_e_s_c_r_i_p_t_i_o_n__; │ │ │ │ │ +57 │ │ │ │ │ +_5_9 _T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n() : │ │ │ │ │ +60 _d_y_n_a_m_i_c__(false) { │ │ │ │ │ +61 } │ │ │ │ │ +62 │ │ │ │ │ +_6_4 _T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n(const _T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n& other) : │ │ │ │ │ +65 Base(other), _d_y_n_a_m_i_c__(false) { │ │ │ │ │ +66 } │ │ │ │ │ +67 │ │ │ │ │ +_6_9 _T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n(const std::string& description) : │ │ │ │ │ +70 _d_y_n_a_m_i_c__(false), _d_e_s_c_r_i_p_t_i_o_n__( │ │ │ │ │ +71 String(description.begin(), description.end())) { │ │ │ │ │ +72 } │ │ │ │ │ +73 │ │ │ │ │ +_7_5 _~_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n() noexcept override { │ │ │ │ │ +76 } │ │ │ │ │ +77 │ │ │ │ │ +78public: │ │ │ │ │ +79 const char* what() const noexcept override { │ │ │ │ │ +80 return _d_e_s_c_r_i_p_t_i_o_n__ ? _d_e_s_c_r_i_p_t_i_o_n__->c_str() : ""; │ │ │ │ │ +81 } │ │ │ │ │ +82}; │ │ │ │ │ 83 │ │ │ │ │ -85 std::set set(const size_t& label) const; │ │ │ │ │ -86 │ │ │ │ │ -88 std::map > sets() const; │ │ │ │ │ -89 │ │ │ │ │ -91 std::map > arrays() const; │ │ │ │ │ -92}; │ │ │ │ │ -93 │ │ │ │ │ -94} │ │ │ │ │ -_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ -Included from all GTSAM files. │ │ │ │ │ +_8_5class GTSAM_EXPORT _R_u_n_t_i_m_e_E_r_r_o_r_T_h_r_e_a_d_s_a_f_e: public │ │ │ │ │ +_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n { │ │ │ │ │ +86public: │ │ │ │ │ +_8_8 _R_u_n_t_i_m_e_E_r_r_o_r_T_h_r_e_a_d_s_a_f_e(const std::string& description) : │ │ │ │ │ +89 _T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n<_R_u_n_t_i_m_e_E_r_r_o_r_T_h_r_e_a_d_s_a_f_e>(description) { │ │ │ │ │ +90 } │ │ │ │ │ +91}; │ │ │ │ │ +92 │ │ │ │ │ +_9_4class _O_u_t_O_f_R_a_n_g_e_T_h_r_e_a_d_s_a_f_e: public _T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n │ │ │ │ │ +{ │ │ │ │ │ +95public: │ │ │ │ │ +_9_7 _O_u_t_O_f_R_a_n_g_e_T_h_r_e_a_d_s_a_f_e(const std::string& description) : │ │ │ │ │ +98 _T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n<_O_u_t_O_f_R_a_n_g_e_T_h_r_e_a_d_s_a_f_e>(description) { │ │ │ │ │ +99 } │ │ │ │ │ +100}; │ │ │ │ │ +101 │ │ │ │ │ +_1_0_3class _I_n_v_a_l_i_d_A_r_g_u_m_e_n_t_T_h_r_e_a_d_s_a_f_e: public _T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n< │ │ │ │ │ +104 InvalidArgumentThreadsafe> { │ │ │ │ │ +105public: │ │ │ │ │ +_1_0_7 _I_n_v_a_l_i_d_A_r_g_u_m_e_n_t_T_h_r_e_a_d_s_a_f_e(const std::string& description) : │ │ │ │ │ +108 _T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n<_I_n_v_a_l_i_d_A_r_g_u_m_e_n_t_T_h_r_e_a_d_s_a_f_e>(description) { │ │ │ │ │ +109 } │ │ │ │ │ +110}; │ │ │ │ │ +111 │ │ │ │ │ +_1_1_3class _C_h_o_l_e_s_k_y_F_a_i_l_e_d : public _g_t_s_a_m_:_:_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n │ │ │ │ │ +114{ │ │ │ │ │ +115public: │ │ │ │ │ +116 _C_h_o_l_e_s_k_y_F_a_i_l_e_d() noexcept {} │ │ │ │ │ +117 _~_C_h_o_l_e_s_k_y_F_a_i_l_e_d() noexcept override {} │ │ │ │ │ +118}; │ │ │ │ │ +119 │ │ │ │ │ +120} // namespace gtsam │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_D_S_F_B_a_s_e │ │ │ │ │ -A fast implementation of disjoint set forests that uses vector as underly data │ │ │ │ │ -structure. │ │ │ │ │ -DDeeffiinniittiioonn DSFVector.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_D_S_F_B_a_s_e_:_:_V │ │ │ │ │ -std::vector< size_t > V │ │ │ │ │ -Vector of ints. │ │ │ │ │ -DDeeffiinniittiioonn DSFVector.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_D_S_F_V_e_c_t_o_r │ │ │ │ │ -DSFVector additionally keeps a vector of keys to support more expensive │ │ │ │ │ -operations. │ │ │ │ │ -DDeeffiinniittiioonn DSFVector.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n │ │ │ │ │ +Base exception type that uses tbb_allocator if GTSAM is compiled with TBB. │ │ │ │ │ +DDeeffiinniittiioonn ThreadsafeException.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_:_:_~_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n │ │ │ │ │ +~ThreadsafeException() noexcept override │ │ │ │ │ +Default destructor doesn't have the noexcept. │ │ │ │ │ +DDeeffiinniittiioonn ThreadsafeException.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_:_:_d_y_n_a_m_i_c__ │ │ │ │ │ +bool dynamic_ │ │ │ │ │ +Whether this object was moved. │ │ │ │ │ +DDeeffiinniittiioonn ThreadsafeException.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_:_:_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n │ │ │ │ │ +ThreadsafeException() │ │ │ │ │ +Default constructor is protected - may only be created from derived classes. │ │ │ │ │ +DDeeffiinniittiioonn ThreadsafeException.h:59 │ │ │ │ │ +_g_t_s_a_m_:_:_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_:_:_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n │ │ │ │ │ +ThreadsafeException(const ThreadsafeException &other) │ │ │ │ │ +Copy constructor is protected - may only be created from derived classes. │ │ │ │ │ +DDeeffiinniittiioonn ThreadsafeException.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_:_:_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n │ │ │ │ │ +ThreadsafeException(const std::string &description) │ │ │ │ │ +Construct with description string. │ │ │ │ │ +DDeeffiinniittiioonn ThreadsafeException.h:69 │ │ │ │ │ +_g_t_s_a_m_:_:_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_:_:_d_e_s_c_r_i_p_t_i_o_n__ │ │ │ │ │ +boost::optional< String > description_ │ │ │ │ │ +Optional description. │ │ │ │ │ +DDeeffiinniittiioonn ThreadsafeException.h:56 │ │ │ │ │ +_g_t_s_a_m_:_:_R_u_n_t_i_m_e_E_r_r_o_r_T_h_r_e_a_d_s_a_f_e │ │ │ │ │ +Thread-safe runtime error exception. │ │ │ │ │ +DDeeffiinniittiioonn ThreadsafeException.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_R_u_n_t_i_m_e_E_r_r_o_r_T_h_r_e_a_d_s_a_f_e_:_:_R_u_n_t_i_m_e_E_r_r_o_r_T_h_r_e_a_d_s_a_f_e │ │ │ │ │ +RuntimeErrorThreadsafe(const std::string &description) │ │ │ │ │ +Construct with a string describing the exception. │ │ │ │ │ +DDeeffiinniittiioonn ThreadsafeException.h:88 │ │ │ │ │ +_g_t_s_a_m_:_:_O_u_t_O_f_R_a_n_g_e_T_h_r_e_a_d_s_a_f_e │ │ │ │ │ +Thread-safe out of range exception. │ │ │ │ │ +DDeeffiinniittiioonn ThreadsafeException.h:94 │ │ │ │ │ +_g_t_s_a_m_:_:_O_u_t_O_f_R_a_n_g_e_T_h_r_e_a_d_s_a_f_e_:_:_O_u_t_O_f_R_a_n_g_e_T_h_r_e_a_d_s_a_f_e │ │ │ │ │ +OutOfRangeThreadsafe(const std::string &description) │ │ │ │ │ +Construct with a string describing the exception. │ │ │ │ │ +DDeeffiinniittiioonn ThreadsafeException.h:97 │ │ │ │ │ +_g_t_s_a_m_:_:_I_n_v_a_l_i_d_A_r_g_u_m_e_n_t_T_h_r_e_a_d_s_a_f_e │ │ │ │ │ +Thread-safe invalid argument exception. │ │ │ │ │ +DDeeffiinniittiioonn ThreadsafeException.h:104 │ │ │ │ │ +_g_t_s_a_m_:_:_I_n_v_a_l_i_d_A_r_g_u_m_e_n_t_T_h_r_e_a_d_s_a_f_e_:_:_I_n_v_a_l_i_d_A_r_g_u_m_e_n_t_T_h_r_e_a_d_s_a_f_e │ │ │ │ │ +InvalidArgumentThreadsafe(const std::string &description) │ │ │ │ │ +Construct with a string describing the exception. │ │ │ │ │ +DDeeffiinniittiioonn ThreadsafeException.h:107 │ │ │ │ │ +_g_t_s_a_m_:_:_C_h_o_l_e_s_k_y_F_a_i_l_e_d │ │ │ │ │ +Indicate Cholesky factorization failure. │ │ │ │ │ +DDeeffiinniittiioonn ThreadsafeException.h:114 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _D_S_F_V_e_c_t_o_r_._h │ │ │ │ │ + * _T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00092.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/timing.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/treeTraversal-inst.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,212 +94,61 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ -Macros | │ │ │ │ -Functions | │ │ │ │ -Variables
│ │ │ │ -
timing.h File Reference
│ │ │ │ +Functions
│ │ │ │ +
treeTraversal-inst.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Timing utilities. │ │ │ │ -More...

│ │ │ │ - │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::internal::TimingOutline
 Timing Entry, arranged in a tree. More...
 
class  gtsam::internal::AutoTicToc
 Small class that calls internal::tic at construction, and internol::toc when destroyed. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ -Macros

#define gttic_(label)
 
│ │ │ │ -#define gttoc_(label)    label##_obj.stop()
 
#define longtic_(label)
 
#define longtoc_(label)
 
#define tictoc_getNode(variable, label)
 
│ │ │ │ -#define gttic(label)   ((void)0)
 
│ │ │ │ -#define gttoc(label)   ((void)0)
 
│ │ │ │ -#define longtic(label)   ((void)0)
 
│ │ │ │ -#define longtoc(label)   ((void)0)
 
│ │ │ │ -#define tictoc_finishedIteration()   ((void)0)
 
│ │ │ │ -#define tictoc_print()   ((void)0)
 
│ │ │ │ -#define tictoc_reset()   ((void)0)
 
namespace  gtsam::treeTraversal
 Internal functions used for traversing trees.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ Functions

│ │ │ │ -size_t gtsam::internal::getTicTocID (const char *descriptionC)
 
│ │ │ │ -void gtsam::internal::tic (size_t id, const char *labelC)
 
│ │ │ │ -void gtsam::internal::toc (size_t id, const char *label)
 
│ │ │ │ -void gtsam::tictoc_finishedIteration_ ()
 
│ │ │ │ -void gtsam::tictoc_print_ ()
 
│ │ │ │ -void gtsam::tictoc_print2_ ()
 
│ │ │ │ -void gtsam::tictoc_reset_ ()
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ -Variables

│ │ │ │ -GTSAM_EXTERN_EXPORT boost::shared_ptr< TimingOutline > gtsam::internal::gTimingRoot
 
│ │ │ │ -GTSAM_EXTERN_EXPORT boost::weak_ptr< TimingOutline > gtsam::internal::gCurrentTimer
 
template<class FOREST , typename DATA , typename VISITOR_PRE , typename VISITOR_POST >
void gtsam::treeTraversal::DepthFirstForest (FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, VISITOR_POST &visitorPost)
 Traverse a forest depth-first with pre-order and post-order visits.
 
template<class FOREST , typename DATA , typename VISITOR_PRE >
void gtsam::treeTraversal::DepthFirstForest (FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre)
 Traverse a forest depth-first, with a pre-order visit but no post-order visit.
 
template<class FOREST , typename DATA , typename VISITOR_PRE , typename VISITOR_POST >
void gtsam::treeTraversal::DepthFirstForestParallel (FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, VISITOR_POST &visitorPost, int problemSizeThreshold=10)
 Traverse a forest depth-first with pre-order and post-order visits.
 
template<class FOREST >
FastVector< boost::shared_ptr< typename FOREST::Node > > gtsam::treeTraversal::CloneForest (const FOREST &forest)
 Clone a tree, copy-constructing new nodes (calling boost::make_shared) and setting up child pointers for a clone of the original tree.
 
template<class FOREST >
void gtsam::treeTraversal::PrintForest (const FOREST &forest, std::string str, const KeyFormatter &keyFormatter)
 Print a tree, prefixing each line with str, and formatting keys using keyFormatter.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Timing utilities.

│ │ │ │ -
Author
Richard Roberts, Michael Kaess
│ │ │ │ -
Date
Oct 5, 2010
│ │ │ │ -

Macro Definition Documentation

│ │ │ │ - │ │ │ │ -

◆ gttic_

│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
#define gttic_( label)
│ │ │ │ -
│ │ │ │ -Value:
static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \
│ │ │ │ -
::gtsam::internal::AutoTicToc label##_obj(label##_id_tic, #label)
│ │ │ │ -
gtsam::internal::AutoTicToc
Small class that calls internal::tic at construction, and internol::toc when destroyed.
Definition timing.h:199
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ - │ │ │ │ -

◆ longtic_

│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
#define longtic_( label)
│ │ │ │ -
│ │ │ │ -Value:
static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \
│ │ │ │ -
::gtsam::internal::ticInternal(label##_id_tic, #label)
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ - │ │ │ │ -

◆ longtoc_

│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
#define longtoc_( label)
│ │ │ │ -
│ │ │ │ -Value:
static const size_t label##_id_toc = ::gtsam::internal::getTicTocID(#label); \
│ │ │ │ -
::gtsam::internal::tocInternal(label##_id_toc, #label)
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ - │ │ │ │ -

◆ tictoc_getNode

│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
#define tictoc_getNode( variable,
 label 
)
│ │ │ │ -
│ │ │ │ -Value:
static const size_t label##_id_getnode = ::gtsam::internal::getTicTocID(#label); \
│ │ │ │ -
const boost::shared_ptr<const ::gtsam::internal::TimingOutline> variable = \
│ │ │ │ -
::gtsam::internal::gCurrentTimer.lock()->child(label##_id_getnode, #label, ::gtsam::internal::gCurrentTimer);
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
April 9, 2013
│ │ │ │ +
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,108 +1,68 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _F_u_n_c_t_i_o_n_s | _V_a_r_i_a_b_l_e_s │ │ │ │ │ -timing.h File Reference │ │ │ │ │ -Timing utilities. _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +treeTraversal-inst.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_T_i_m_i_n_g_O_u_t_l_i_n_e │ │ │ │ │ -  Timing Entry, arranged in a tree. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_A_u_t_o_T_i_c_T_o_c │ │ │ │ │ -  Small class that calls internal::tic at construction, and internol::toc │ │ │ │ │ - when destroyed. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -MMaaccrrooss │ │ │ │ │ -#define  _g_t_t_i_c__(label) │ │ │ │ │ -  │ │ │ │ │ -#define  ggttttoocc__(label)    label##_obj.stop() │ │ │ │ │ -  │ │ │ │ │ -#define  _l_o_n_g_t_i_c__(label) │ │ │ │ │ -  │ │ │ │ │ -#define  _l_o_n_g_t_o_c__(label) │ │ │ │ │ -  │ │ │ │ │ -#define  _t_i_c_t_o_c___g_e_t_N_o_d_e(variable, label) │ │ │ │ │ -  │ │ │ │ │ -#define  ggttttiicc(label)   ((void)0) │ │ │ │ │ -  │ │ │ │ │ -#define  ggttttoocc(label)   ((void)0) │ │ │ │ │ -  │ │ │ │ │ -#define  lloonnggttiicc(label)   ((void)0) │ │ │ │ │ -  │ │ │ │ │ -#define  lloonnggttoocc(label)   ((void)0) │ │ │ │ │ -  │ │ │ │ │ -#define  ttiiccttoocc__ffiinniisshheeddIItteerraattiioonn()   ((void)0) │ │ │ │ │ -  │ │ │ │ │ -#define  ttiiccttoocc__pprriinntt()   ((void)0) │ │ │ │ │ -  │ │ │ │ │ -#define  ttiiccttoocc__rreesseett()   ((void)0) │ │ │ │ │ +namespace   _g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l │ │ │ │ │ +  Internal functions used for traversing trees. │ │ │ │ │   │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -size_t  ggttssaamm::::iinntteerrnnaall::::ggeettTTiiccTTooccIIDD (const char *descriptionC) │ │ │ │ │ -  │ │ │ │ │ - void  ggttssaamm::::iinntteerrnnaall::::ttiicc (size_t id, const char *labelC) │ │ │ │ │ -  │ │ │ │ │ - void  ggttssaamm::::iinntteerrnnaall::::ttoocc (size_t id, const char *label) │ │ │ │ │ -  │ │ │ │ │ - void  ggttssaamm::::ttiiccttoocc__ffiinniisshheeddIItteerraattiioonn__ () │ │ │ │ │ -  │ │ │ │ │ - void  ggttssaamm::::ttiiccttoocc__pprriinntt__ () │ │ │ │ │ -  │ │ │ │ │ - void  ggttssaamm::::ttiiccttoocc__pprriinntt22__ () │ │ │ │ │ -  │ │ │ │ │ - void  ggttssaamm::::ttiiccttoocc__rreesseett__ () │ │ │ │ │ -  │ │ │ │ │ -VVaarriiaabblleess │ │ │ │ │ -GTSAM_EXTERN_EXPORT boost::shared_ptr< │ │ │ │ │ - _T_i_m_i_n_g_O_u_t_l_i_n_e >  ggttssaamm::::iinntteerrnnaall::::ggTTiimmiinnggRRoooott │ │ │ │ │ -  │ │ │ │ │ - GTSAM_EXTERN_EXPORT boost::weak_ptr< │ │ │ │ │ - _T_i_m_i_n_g_O_u_t_l_i_n_e >  ggttssaamm::::iinntteerrnnaall::::ggCCuurrrreennttTTiimmeerr │ │ │ │ │ +template │ │ │ │ │ + void  _g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t │ │ │ │ │ + (FOREST &forest, DATA &rootData, │ │ │ │ │ + VISITOR_PRE &visitorPre, VISITOR_POST │ │ │ │ │ + &visitorPost) │ │ │ │ │ +  Traverse a forest depth-first with pre- │ │ │ │ │ + order and post-order visits. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + void  _g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t │ │ │ │ │ + (FOREST &forest, DATA &rootData, │ │ │ │ │ + VISITOR_PRE &visitorPre) │ │ │ │ │ + Traverse a forest depth-first, with a │ │ │ │ │ +  pre-order visit but no post-order │ │ │ │ │ + visit. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + void  _g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_: │ │ │ │ │ + _D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t_P_a_r_a_l_l_e_l (FOREST │ │ │ │ │ + &forest, DATA &rootData, VISITOR_PRE │ │ │ │ │ + &visitorPre, VISITOR_POST &visitorPost, │ │ │ │ │ + int problemSizeThreshold=10) │ │ │ │ │ +  Traverse a forest depth-first with pre- │ │ │ │ │ + order and post-order visits. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +_F_a_s_t_V_e_c_t_o_r< boost::shared_ptr< typename _g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_C_l_o_n_e_F_o_r_e_s_t │ │ │ │ │ + FOREST::Node > >  (const FOREST &forest) │ │ │ │ │ + Clone a tree, copy-constructing new │ │ │ │ │ +  nodes (calling boost::make_shared) and │ │ │ │ │ + setting up child pointers for a clone │ │ │ │ │ + of the original tree. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + void  _g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_P_r_i_n_t_F_o_r_e_s_t │ │ │ │ │ + (const FOREST &forest, std::string str, │ │ │ │ │ + const _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter) │ │ │ │ │ + Print a tree, prefixing each line with │ │ │ │ │ +  str, and formatting keys using │ │ │ │ │ + keyFormatter. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Timing utilities. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts, Michael Kaess │ │ │ │ │ + Richard Roberts │ │ │ │ │ Date │ │ │ │ │ - Oct 5, 2010 │ │ │ │ │ -********** MMaaccrroo DDeeffiinniittiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ -********** _?◆_? ggttttiicc__ ********** │ │ │ │ │ -#define gttic_ (   label ) │ │ │ │ │ -VVaalluuee:: │ │ │ │ │ -static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \ │ │ │ │ │ -_:_:_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_A_u_t_o_T_i_c_T_o_c label##_obj(label##_id_tic, #label) │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_A_u_t_o_T_i_c_T_o_c │ │ │ │ │ -Small class that calls internal::tic at construction, and internol::toc when │ │ │ │ │ -destroyed. │ │ │ │ │ -DDeeffiinniittiioonn timing.h:199 │ │ │ │ │ -********** _?◆_? lloonnggttiicc__ ********** │ │ │ │ │ -#define longtic_ (   label ) │ │ │ │ │ -VVaalluuee:: │ │ │ │ │ -static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \ │ │ │ │ │ -::gtsam::internal::ticInternal(label##_id_tic, #label) │ │ │ │ │ -********** _?◆_? lloonnggttoocc__ ********** │ │ │ │ │ -#define longtoc_ (   label ) │ │ │ │ │ -VVaalluuee:: │ │ │ │ │ -static const size_t label##_id_toc = ::gtsam::internal::getTicTocID(#label); \ │ │ │ │ │ -::gtsam::internal::tocInternal(label##_id_toc, #label) │ │ │ │ │ -********** _?◆_? ttiiccttoocc__ggeettNNooddee ********** │ │ │ │ │ -#define tictoc_getNode (   variable, │ │ │ │ │ -   label  │ │ │ │ │ - ) │ │ │ │ │ -VVaalluuee:: │ │ │ │ │ -static const size_t label##_id_getnode = ::gtsam::internal::getTicTocID │ │ │ │ │ -(#label); \ │ │ │ │ │ -const boost::shared_ptr variable = \ │ │ │ │ │ -::gtsam::internal::gCurrentTimer.lock()->child(label##_id_getnode, #label, :: │ │ │ │ │ -gtsam::internal::gCurrentTimer); │ │ │ │ │ + April 9, 2013 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _t_i_m_i_n_g_._h │ │ │ │ │ + * _t_r_e_e_T_r_a_v_e_r_s_a_l_-_i_n_s_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00092.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,7 @@ │ │ │ │ │ var a00092 = [ │ │ │ │ │ - ["gtsam::internal::TimingOutline", "a02516.html", "a02516"], │ │ │ │ │ - ["gtsam::internal::AutoTicToc", "a02520.html", null] │ │ │ │ │ + ["CloneForest", "a00092.html#a67e7c6f5c4916c4a58a783e6b566d55a", null], │ │ │ │ │ + ["DepthFirstForest", "a00092.html#a3735d0cee6f4d530e75479f7b1ebaf07", null], │ │ │ │ │ + ["DepthFirstForest", "a00092.html#a50e24f0a9085721425089cdce18f2475", null], │ │ │ │ │ + ["DepthFirstForestParallel", "a00092.html#afd7410807789da9ad91d6630063be36f", null], │ │ │ │ │ + ["PrintForest", "a00092.html#a7228bbaf6ad62b65be7a2084c01aae5c", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00092_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/timing.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/treeTraversal-inst.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,328 +98,217 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
timing.h
│ │ │ │ +
treeTraversal-inst.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/base/FastMap.h>
│ │ │ │ -
21#include <gtsam/dllexport.h>
│ │ │ │ -
22#include <gtsam/config.h> // for GTSAM_USE_TBB
│ │ │ │ -
23
│ │ │ │ -
24#include <boost/smart_ptr/shared_ptr.hpp>
│ │ │ │ -
25#include <boost/smart_ptr/weak_ptr.hpp>
│ │ │ │ -
26#include <boost/version.hpp>
│ │ │ │ -
27
│ │ │ │ -
28#include <cstddef>
│ │ │ │ +
17#pragma once
│ │ │ │ +
18
│ │ │ │ +
19#include <gtsam/base/treeTraversal/parallelTraversalTasks.h>
│ │ │ │ +
20#include <gtsam/base/treeTraversal/statistics.h>
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/base/FastList.h>
│ │ │ │ +
23#include <gtsam/base/FastVector.h>
│ │ │ │ +
24#include <gtsam/inference/Key.h>
│ │ │ │ +
25#include <gtsam/config.h> // for GTSAM_USE_TBB
│ │ │ │ +
26
│ │ │ │ +
27#include <stack>
│ │ │ │ +
28#include <vector>
│ │ │ │
29#include <string>
│ │ │ │ -
30
│ │ │ │ -
31// This file contains the GTSAM timing instrumentation library, a low-overhead method for
│ │ │ │ -
32// learning at a medium-fine level how much time various components of an algorithm take
│ │ │ │ -
33// in CPU and wall time.
│ │ │ │ -
34//
│ │ │ │ -
35// The output of this instrumentation is a call-tree-like printout containing statistics
│ │ │ │ -
36// about each instrumented code block. To print this output at any time, call
│ │ │ │ -
37// tictoc_print() or tictoc_print_().
│ │ │ │ -
38//
│ │ │ │ -
39// An overall point to be aware of is that there are two versions of each function - one
│ │ │ │ -
40// ending in an underscore '_' and one without the trailing underscore. The underscore
│ │ │ │ -
41// versions always are active, but the versions without an underscore are active only when
│ │ │ │ -
42// GTSAM_ENABLE_TIMING is defined (automatically defined in our CMake Timing build type).
│ │ │ │ -
43// GTSAM algorithms are all instrumented with the non-underscore versions, so generally
│ │ │ │ -
44// you should use the underscore versions in your own code to leave out the GTSAM detail.
│ │ │ │ -
45//
│ │ │ │ -
46// gttic and gttoc start and stop a timed section, respectively. gttic creates a *scoped*
│ │ │ │ -
47// object - when it goes out of scope gttoc is called automatically. Thus, you do not
│ │ │ │ -
48// need to call gttoc if you are timing an entire function (see basic use examples below).
│ │ │ │ -
49// However, you must be *aware* of this scoped nature - putting gttic inside of an if(...)
│ │ │ │ -
50// block, for example, will only time code until the closing brace '}'. See advanced
│ │ │ │ -
51// usage below if you need to avoid this.
│ │ │ │ -
52//
│ │ │ │ -
53// Multiple calls nest automatically - each gttic nests under the previous gttic called
│ │ │ │ -
54// for which gttoc has not been called (or the previous gttic did not go out of scope).
│ │ │ │ -
55//
│ │ │ │ -
56// Basic usage examples are as follows:
│ │ │ │ -
57//
│ │ │ │ -
58// - Timing an entire function:
│ │ │ │ -
59// void myFunction() {
│ │ │ │ -
60// gttic_(myFunction);
│ │ │ │ -
61// ........
│ │ │ │ -
62// }
│ │ │ │ -
63//
│ │ │ │ -
64// - Timing an entire function as well as its component parts:
│ │ │ │ -
65// void myLongFunction() {
│ │ │ │ -
66// gttic_(myLongFunction);
│ │ │ │ -
67// gttic_(step1); // Will nest under the 'myLongFunction' label
│ │ │ │ -
68// ........
│ │ │ │ -
69// gttoc_(step1);
│ │ │ │ -
70// gttic_(step2); // Will nest under the 'myLongFunction' label
│ │ │ │ -
71// ........
│ │ │ │ -
72// gttoc_(step2);
│ │ │ │ -
73// ........
│ │ │ │ -
74// }
│ │ │ │ -
75//
│ │ │ │ -
76// - Timing functions calling/called by other functions:
│ │ │ │ -
77// void oneStep() {
│ │ │ │ -
78// gttic_(oneStep); // Will automatically nest under the gttic label of the calling function
│ │ │ │ -
79// .......
│ │ │ │ -
80// }
│ │ │ │ -
81// void algorithm() {
│ │ │ │ -
82// gttic_(algorithm);
│ │ │ │ -
83// oneStep(); // gttic's inside this function will automatically nest inside our 'algorithm' label
│ │ │ │ -
84// twoStep(); // gttic's inside this function will automatically nest inside our 'algorithm' label
│ │ │ │ -
85// }
│ │ │ │ -
86//
│ │ │ │ -
87//
│ │ │ │ -
88// Advanced usage:
│ │ │ │ -
89//
│ │ │ │ -
90// - "Finishing iterations" - to get correct min/max times for each call, you must define
│ │ │ │ -
91// in your code what constitutes an iteration. A single sum for the min/max times is
│ │ │ │ -
92// accumulated within each iteration. If you don't care about min/max times, you don't
│ │ │ │ -
93// need to worry about this. For example:
│ │ │ │ -
94// void myOuterLoop() {
│ │ │ │ -
95// while(true) {
│ │ │ │ -
96// iterateMyAlgorithm();
│ │ │ │ -
97// tictoc_finishedIteration_();
│ │ │ │ -
98// tictoc_print_(); // Optional
│ │ │ │ -
99// }
│ │ │ │ -
100// }
│ │ │ │ -
101//
│ │ │ │ -
102// - Stopping timing a section in a different scope than it is started. Normally, a gttoc
│ │ │ │ -
103// statement goes out of scope at end of C++ scope. However, you can use longtic and
│ │ │ │ -
104// longtoc to start and stop timing with the specified label at any point, without regard
│ │ │ │ -
105// too scope. Note that if you use these, it may become difficult to ensure that you
│ │ │ │ -
106// have matching gttic/gttoc statments. You may want to consider reorganizing your timing
│ │ │ │ -
107// outline to match the scope of your code.
│ │ │ │ -
108
│ │ │ │ -
109// Automatically use the new Boost timers if version is recent enough.
│ │ │ │ -
110#if BOOST_VERSION >= 104800
│ │ │ │ -
111# ifndef GTSAM_DISABLE_NEW_TIMERS
│ │ │ │ -
112# define GTSAM_USING_NEW_BOOST_TIMERS
│ │ │ │ -
113# endif
│ │ │ │ -
114#endif
│ │ │ │ -
115
│ │ │ │ -
116#ifdef GTSAM_USING_NEW_BOOST_TIMERS
│ │ │ │ -
117# include <boost/timer/timer.hpp>
│ │ │ │ -
118#else
│ │ │ │ -
119# include <boost/timer.hpp>
│ │ │ │ -
120# include <gtsam/base/types.h>
│ │ │ │ -
121#endif
│ │ │ │ -
122
│ │ │ │ -
123#ifdef GTSAM_USE_TBB
│ │ │ │ -
124# include <tbb/tick_count.h>
│ │ │ │ -
125# undef min
│ │ │ │ -
126# undef max
│ │ │ │ -
127# undef ERROR
│ │ │ │ -
128#endif
│ │ │ │ -
129
│ │ │ │ -
130namespace gtsam {
│ │ │ │ -
131
│ │ │ │ -
132 namespace internal {
│ │ │ │ -
133 // Generate/retrieve a unique global ID number that will be used to look up tic/toc statements
│ │ │ │ -
134 GTSAM_EXPORT size_t getTicTocID(const char *description);
│ │ │ │ -
135
│ │ │ │ -
136 // Create new TimingOutline child for gCurrentTimer, make it gCurrentTimer, and call tic method
│ │ │ │ -
137 GTSAM_EXPORT void tic(size_t id, const char *label);
│ │ │ │ -
138
│ │ │ │ -
139 // Call toc on gCurrentTimer and then set gCurrentTimer to the parent of gCurrentTimer
│ │ │ │ -
140 GTSAM_EXPORT void toc(size_t id, const char *label);
│ │ │ │ -
141
│ │ │ │ -
│ │ │ │ -
145 class TimingOutline {
│ │ │ │ -
146 protected:
│ │ │ │ -
147 size_t id_;
│ │ │ │ -
148 size_t t_;
│ │ │ │ -
149 size_t tWall_;
│ │ │ │ -
150 double t2_ ;
│ │ │ │ -
151 size_t tIt_;
│ │ │ │ -
152 size_t tMax_;
│ │ │ │ -
153 size_t tMin_;
│ │ │ │ -
154 size_t n_;
│ │ │ │ -
155 size_t myOrder_;
│ │ │ │ -
156 size_t lastChildOrder_;
│ │ │ │ -
157 std::string label_;
│ │ │ │ -
158
│ │ │ │ -
159 // Tree structure
│ │ │ │ -
160 boost::weak_ptr<TimingOutline> parent_;
│ │ │ │ -
161 typedef FastMap<size_t, boost::shared_ptr<TimingOutline> > ChildMap;
│ │ │ │ -
162 ChildMap children_;
│ │ │ │ -
163
│ │ │ │ -
164#ifdef GTSAM_USING_NEW_BOOST_TIMERS
│ │ │ │ -
165 boost::timer::cpu_timer timer_;
│ │ │ │ -
166#else
│ │ │ │ -
167 boost::timer timer_;
│ │ │ │ -
168 gtsam::ValueWithDefault<bool,false> timerActive_;
│ │ │ │ -
169#endif
│ │ │ │ -
170#ifdef GTSAM_USE_TBB
│ │ │ │ -
171 tbb::tick_count tbbTimer_;
│ │ │ │ -
172#endif
│ │ │ │ -
173 void add(size_t usecs, size_t usecsWall);
│ │ │ │ -
174
│ │ │ │ -
175 public:
│ │ │ │ -
177 GTSAM_EXPORT TimingOutline(const std::string& label, size_t myId);
│ │ │ │ -
178 GTSAM_EXPORT size_t time() const;
│ │ │ │ -
179 double secs() const { return double(time()) / 1000000.0;}
│ │ │ │ -
│ │ │ │ -
180 double self() const { return double(t_) / 1000000.0;}
│ │ │ │ -
│ │ │ │ -
181 double wall() const { return double(tWall_) / 1000000.0;}
│ │ │ │ -
│ │ │ │ -
182 double min() const { return double(tMin_) / 1000000.0;}
│ │ │ │ -
│ │ │ │ -
183 double max() const { return double(tMax_) / 1000000.0;}
│ │ │ │ -
│ │ │ │ -
184 double mean() const { return self() / double(n_); }
│ │ │ │ -
185 GTSAM_EXPORT void print(const std::string& outline = "") const;
│ │ │ │ -
186 GTSAM_EXPORT void print2(const std::string& outline = "", const double parentTotal = -1.0) const;
│ │ │ │ -
187 GTSAM_EXPORT const boost::shared_ptr<TimingOutline>&
│ │ │ │ -
188 child(size_t child, const std::string& label, const boost::weak_ptr<TimingOutline>& thisPtr);
│ │ │ │ -
189 GTSAM_EXPORT void tic();
│ │ │ │ -
190 GTSAM_EXPORT void toc();
│ │ │ │ -
191 GTSAM_EXPORT void finishedIteration();
│ │ │ │ -
192
│ │ │ │ -
193 GTSAM_EXPORT friend void toc(size_t id, const char *label);
│ │ │ │ -
194 }; // \TimingOutline
│ │ │ │ -
195
│ │ │ │ -
│ │ │ │ -
199 class GTSAM_EXPORT AutoTicToc {
│ │ │ │ -
200 private:
│ │ │ │ -
201 size_t id_;
│ │ │ │ -
202 const char* label_;
│ │ │ │ -
203 bool isSet_;
│ │ │ │ -
204
│ │ │ │ -
205 public:
│ │ │ │ -
206 AutoTicToc(size_t id, const char* label)
│ │ │ │ -
207 : id_(id), label_(label), isSet_(true) {
│ │ │ │ -
208 tic(id_, label_);
│ │ │ │ -
209 }
│ │ │ │ -
210 void stop() {
│ │ │ │ -
211 toc(id_, label_);
│ │ │ │ -
212 isSet_ = false;
│ │ │ │ -
213 }
│ │ │ │ -
214 ~AutoTicToc() {
│ │ │ │ -
215 if (isSet_) stop();
│ │ │ │ -
216 }
│ │ │ │ -
217 };
│ │ │ │ -
│ │ │ │ -
218
│ │ │ │ -
219 GTSAM_EXTERN_EXPORT boost::shared_ptr<TimingOutline> gTimingRoot;
│ │ │ │ -
220 GTSAM_EXTERN_EXPORT boost::weak_ptr<TimingOutline> gCurrentTimer;
│ │ │ │ -
221 }
│ │ │ │ -
222
│ │ │ │ -
223// Tic and toc functions that are always active (whether or not ENABLE_TIMING is defined)
│ │ │ │ -
224// There is a trick being used here to achieve near-zero runtime overhead, in that a
│ │ │ │ -
225// static variable is created for each tic/toc statement storing an integer ID, but the
│ │ │ │ -
226// integer ID is only looked up by string once when the static variable is initialized
│ │ │ │ -
227// as the program starts.
│ │ │ │ -
228
│ │ │ │ -
229// tic
│ │ │ │ -
230#define gttic_(label) \
│ │ │ │ -
231 static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \
│ │ │ │ -
232 ::gtsam::internal::AutoTicToc label##_obj(label##_id_tic, #label)
│ │ │ │ -
233
│ │ │ │ -
234// toc
│ │ │ │ -
235#define gttoc_(label) \
│ │ │ │ -
236 label##_obj.stop()
│ │ │ │ -
237
│ │ │ │ -
238// tic
│ │ │ │ -
239#define longtic_(label) \
│ │ │ │ -
240 static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \
│ │ │ │ -
241 ::gtsam::internal::ticInternal(label##_id_tic, #label)
│ │ │ │ -
242
│ │ │ │ -
243// toc
│ │ │ │ -
244#define longtoc_(label) \
│ │ │ │ -
245 static const size_t label##_id_toc = ::gtsam::internal::getTicTocID(#label); \
│ │ │ │ -
246 ::gtsam::internal::tocInternal(label##_id_toc, #label)
│ │ │ │ -
247
│ │ │ │ -
248// indicate iteration is finished
│ │ │ │ -
249inline void tictoc_finishedIteration_() {
│ │ │ │ -
250 ::gtsam::internal::gTimingRoot->finishedIteration(); }
│ │ │ │ -
251
│ │ │ │ -
252// print
│ │ │ │ -
253inline void tictoc_print_() {
│ │ │ │ -
254 ::gtsam::internal::gTimingRoot->print(); }
│ │ │ │ -
255
│ │ │ │ -
256// print mean and standard deviation
│ │ │ │ -
257inline void tictoc_print2_() {
│ │ │ │ -
258 ::gtsam::internal::gTimingRoot->print2(); }
│ │ │ │ -
259
│ │ │ │ -
260// get a node by label and assign it to variable
│ │ │ │ -
261#define tictoc_getNode(variable, label) \
│ │ │ │ -
262 static const size_t label##_id_getnode = ::gtsam::internal::getTicTocID(#label); \
│ │ │ │ -
263 const boost::shared_ptr<const ::gtsam::internal::TimingOutline> variable = \
│ │ │ │ -
264 ::gtsam::internal::gCurrentTimer.lock()->child(label##_id_getnode, #label, ::gtsam::internal::gCurrentTimer);
│ │ │ │ -
265
│ │ │ │ -
266// reset
│ │ │ │ -
267inline void tictoc_reset_() {
│ │ │ │ -
268 ::gtsam::internal::gTimingRoot.reset(new ::gtsam::internal::TimingOutline("Total", ::gtsam::internal::getTicTocID("Total")));
│ │ │ │ -
269 ::gtsam::internal::gCurrentTimer = ::gtsam::internal::gTimingRoot; }
│ │ │ │ -
270
│ │ │ │ -
271#ifdef ENABLE_TIMING
│ │ │ │ -
272#define gttic(label) gttic_(label)
│ │ │ │ -
273#define gttoc(label) gttoc_(label)
│ │ │ │ -
274#define longtic(label) longtic_(label)
│ │ │ │ -
275#define longtoc(label) longtoc_(label)
│ │ │ │ -
276#define tictoc_finishedIteration tictoc_finishedIteration_
│ │ │ │ -
277#define tictoc_print tictoc_print_
│ │ │ │ -
278#define tictoc_reset tictoc_reset_
│ │ │ │ -
279#else
│ │ │ │ -
280#define gttic(label) ((void)0)
│ │ │ │ -
281#define gttoc(label) ((void)0)
│ │ │ │ -
282#define longtic(label) ((void)0)
│ │ │ │ -
283#define longtoc(label) ((void)0)
│ │ │ │ -
284#define tictoc_finishedIteration() ((void)0)
│ │ │ │ -
285#define tictoc_print() ((void)0)
│ │ │ │ -
286#define tictoc_reset() ((void)0)
│ │ │ │ -
287#endif
│ │ │ │ -
288
│ │ │ │ -
289}
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
FastMap.h
A thin wrapper around std::map that uses boost's fast_pool_allocator.
│ │ │ │ -
types.h
Typedefs for easier changing of types.
│ │ │ │ +
30#include <boost/shared_ptr.hpp>
│ │ │ │ +
31#include <boost/make_shared.hpp>
│ │ │ │ +
32
│ │ │ │ +
33namespace gtsam {
│ │ │ │ +
34
│ │ │ │ +
│ │ │ │ +
36namespace treeTraversal {
│ │ │ │ +
37
│ │ │ │ +
38/* ************************************************************************* */
│ │ │ │ +
39namespace {
│ │ │ │ +
40// Internal node used in DFS preorder stack
│ │ │ │ +
41template<typename NODE, typename DATA>
│ │ │ │ +
42struct TraversalNode {
│ │ │ │ +
43 bool expanded;
│ │ │ │ +
44 const boost::shared_ptr<NODE>& treeNode;
│ │ │ │ +
45 DATA& parentData;
│ │ │ │ +
46 typename FastList<DATA>::iterator dataPointer;
│ │ │ │ +
47 TraversalNode(const boost::shared_ptr<NODE>& _treeNode, DATA& _parentData) :
│ │ │ │ +
48 expanded(false), treeNode(_treeNode), parentData(_parentData) {
│ │ │ │ +
49 }
│ │ │ │ +
50};
│ │ │ │ +
51
│ │ │ │ +
52// Do nothing - default argument for post-visitor for tree traversal
│ │ │ │ +
53struct no_op {
│ │ │ │ +
54 template<typename NODE, typename DATA>
│ │ │ │ +
55 void operator()(const boost::shared_ptr<NODE>& node, const DATA& data) {
│ │ │ │ +
56 }
│ │ │ │ +
57};
│ │ │ │ +
58
│ │ │ │ +
59}
│ │ │ │ +
60
│ │ │ │ +
75template<class FOREST, typename DATA, typename VISITOR_PRE,
│ │ │ │ +
76 typename VISITOR_POST>
│ │ │ │ +
│ │ │ │ +
77void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre,
│ │ │ │ +
78 VISITOR_POST& visitorPost) {
│ │ │ │ +
79 // Typedefs
│ │ │ │ +
80 typedef typename FOREST::Node Node;
│ │ │ │ +
81 typedef boost::shared_ptr<Node> sharedNode;
│ │ │ │ +
82
│ │ │ │ +
83 // Depth first traversal stack
│ │ │ │ +
84 typedef TraversalNode<typename FOREST::Node, DATA> TraversalNode;
│ │ │ │ +
85 typedef FastList<TraversalNode> Stack;
│ │ │ │ +
86 Stack stack;
│ │ │ │ +
87 FastList<DATA> dataList; // List to store node data as it is returned from the pre-order visitor
│ │ │ │ +
88
│ │ │ │ +
89 // Add roots to stack (insert such that they are visited and processed in order
│ │ │ │ +
90 {
│ │ │ │ +
91 typename Stack::iterator insertLocation = stack.begin();
│ │ │ │ +
92 for(const sharedNode& root: forest.roots())
│ │ │ │ +
93 stack.insert(insertLocation, TraversalNode(root, rootData));
│ │ │ │ +
94 }
│ │ │ │ +
95
│ │ │ │ +
96 // Traverse
│ │ │ │ +
97 while (!stack.empty()) {
│ │ │ │ +
98 // Get next node
│ │ │ │ +
99 TraversalNode& node = stack.front();
│ │ │ │ +
100
│ │ │ │ +
101 if (node.expanded) {
│ │ │ │ +
102 // If already expanded, then the data stored in the node is no longer needed, so visit
│ │ │ │ +
103 // then delete it.
│ │ │ │ +
104 (void) visitorPost(node.treeNode, *node.dataPointer);
│ │ │ │ +
105 dataList.erase(node.dataPointer);
│ │ │ │ +
106 stack.pop_front();
│ │ │ │ +
107 } else {
│ │ │ │ +
108 // If not already visited, visit the node and add its children (use reverse iterators so
│ │ │ │ +
109 // children are processed in the order they appear)
│ │ │ │ +
110 node.dataPointer = dataList.insert(dataList.end(),
│ │ │ │ +
111 visitorPre(node.treeNode, node.parentData));
│ │ │ │ +
112 typename Stack::iterator insertLocation = stack.begin();
│ │ │ │ +
113 for(const sharedNode& child: node.treeNode->children)
│ │ │ │ +
114 stack.insert(insertLocation, TraversalNode(child, *node.dataPointer));
│ │ │ │ +
115 node.expanded = true;
│ │ │ │ +
116 }
│ │ │ │ +
117 }
│ │ │ │ +
118 assert(dataList.empty());
│ │ │ │ +
119}
│ │ │ │ +
│ │ │ │ +
120
│ │ │ │ +
132template<class FOREST, typename DATA, typename VISITOR_PRE>
│ │ │ │ +
│ │ │ │ +
133void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre) {
│ │ │ │ +
134 no_op visitorPost;
│ │ │ │ +
135 DepthFirstForest(forest, rootData, visitorPre, visitorPost);
│ │ │ │ +
136}
│ │ │ │ +
│ │ │ │ +
137
│ │ │ │ +
152template<class FOREST, typename DATA, typename VISITOR_PRE,
│ │ │ │ +
153 typename VISITOR_POST>
│ │ │ │ +
│ │ │ │ +
154void DepthFirstForestParallel(FOREST& forest, DATA& rootData,
│ │ │ │ +
155 VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost,
│ │ │ │ +
156 int problemSizeThreshold = 10) {
│ │ │ │ +
157#ifdef GTSAM_USE_TBB
│ │ │ │ +
158 // Typedefs
│ │ │ │ +
159 typedef typename FOREST::Node Node;
│ │ │ │ +
160
│ │ │ │ +
161 internal::CreateRootTask<Node>(forest.roots(), rootData, visitorPre,
│ │ │ │ +
162 visitorPost, problemSizeThreshold);
│ │ │ │ +
163#else
│ │ │ │ +
164 DepthFirstForest(forest, rootData, visitorPre, visitorPost);
│ │ │ │ +
165#endif
│ │ │ │ +
166}
│ │ │ │ +
│ │ │ │ +
167
│ │ │ │ +
168/* ************************************************************************* */
│ │ │ │ +
170namespace {
│ │ │ │ +
171template<typename NODE>
│ │ │ │ +
172boost::shared_ptr<NODE> CloneForestVisitorPre(
│ │ │ │ +
173 const boost::shared_ptr<NODE>& node,
│ │ │ │ +
174 const boost::shared_ptr<NODE>& parentPointer) {
│ │ │ │ +
175 // Clone the current node and add it to its cloned parent
│ │ │ │ +
176 boost::shared_ptr<NODE> clone = boost::make_shared<NODE>(*node);
│ │ │ │ +
177 clone->children.clear();
│ │ │ │ +
178 parentPointer->children.push_back(clone);
│ │ │ │ +
179 return clone;
│ │ │ │ +
180}
│ │ │ │ +
181}
│ │ │ │ +
182
│ │ │ │ +
188template<class FOREST>
│ │ │ │ +
│ │ │ │ +
189FastVector<boost::shared_ptr<typename FOREST::Node> > CloneForest(
│ │ │ │ +
190 const FOREST& forest) {
│ │ │ │ +
191 typedef typename FOREST::Node Node;
│ │ │ │ +
192 boost::shared_ptr<Node> rootContainer = boost::make_shared<Node>();
│ │ │ │ +
193 DepthFirstForest(forest, rootContainer, CloneForestVisitorPre<Node>);
│ │ │ │ +
194 return FastVector<boost::shared_ptr<Node> >(rootContainer->children.begin(),
│ │ │ │ +
195 rootContainer->children.end());
│ │ │ │ +
196}
│ │ │ │ +
│ │ │ │ +
197
│ │ │ │ +
198/* ************************************************************************* */
│ │ │ │ +
200namespace {
│ │ │ │ +
201struct PrintForestVisitorPre {
│ │ │ │ +
202 const KeyFormatter& formatter;
│ │ │ │ +
203 PrintForestVisitorPre(const KeyFormatter& formatter) :
│ │ │ │ +
204 formatter(formatter) {
│ │ │ │ +
205 }
│ │ │ │ +
206 template<typename NODE> std::string operator()(
│ │ │ │ +
207 const boost::shared_ptr<NODE>& node, const std::string& parentString) {
│ │ │ │ +
208 // Print the current node
│ │ │ │ +
209 node->print(parentString + "-", formatter);
│ │ │ │ +
210 // Increment the indentation
│ │ │ │ +
211 return parentString + "| ";
│ │ │ │ +
212 }
│ │ │ │ +
213};
│ │ │ │ +
214}
│ │ │ │ +
215
│ │ │ │ +
218template<class FOREST>
│ │ │ │ +
│ │ │ │ +
219void PrintForest(const FOREST& forest, std::string str,
│ │ │ │ +
220 const KeyFormatter& keyFormatter) {
│ │ │ │ +
221 PrintForestVisitorPre visitor(keyFormatter);
│ │ │ │ +
222 DepthFirstForest(forest, str, visitor);
│ │ │ │ +
223}
│ │ │ │ +
│ │ │ │ +
224} // namespace treeTraversal
│ │ │ │ +
│ │ │ │ +
225
│ │ │ │ +
226} // namespace gtsam
│ │ │ │ +
FastList.h
A thin wrapper around std::list that uses boost's fast_pool_allocator.
│ │ │ │ +
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ + │ │ │ │ +
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ -
gtsam::internal::TimingOutline
Timing Entry, arranged in a tree.
Definition timing.h:145
│ │ │ │ -
gtsam::internal::TimingOutline::t2_
double t2_
cache the
Definition timing.h:150
│ │ │ │ -
gtsam::internal::TimingOutline::children_
ChildMap children_
subtrees
Definition timing.h:162
│ │ │ │ -
gtsam::internal::TimingOutline::max
double max() const
max time, in seconds
Definition timing.h:183
│ │ │ │ -
gtsam::internal::TimingOutline::wall
double wall() const
wall time, in seconds
Definition timing.h:181
│ │ │ │ -
gtsam::internal::TimingOutline::secs
double secs() const
time taken, in seconds, including children
Definition timing.h:179
│ │ │ │ -
gtsam::internal::TimingOutline::self
double self() const
self time only, in seconds
Definition timing.h:180
│ │ │ │ -
gtsam::internal::TimingOutline::parent_
boost::weak_ptr< TimingOutline > parent_
parent pointer
Definition timing.h:160
│ │ │ │ -
gtsam::internal::TimingOutline::min
double min() const
min time, in seconds
Definition timing.h:182
│ │ │ │ -
gtsam::internal::TimingOutline::mean
double mean() const
mean self time, in seconds
Definition timing.h:184
│ │ │ │ -
gtsam::internal::TimingOutline::time
GTSAM_EXPORT size_t time() const
time taken, including children
Definition timing.cpp:65
│ │ │ │ -
gtsam::internal::AutoTicToc
Small class that calls internal::tic at construction, and internol::toc when destroyed.
Definition timing.h:199
│ │ │ │ -
gtsam::ValueWithDefault
Helper struct that encapsulates a value with a default, this is just used as a member object so you d...
Definition types.h:137
│ │ │ │ +
gtsam::stack
Matrix stack(size_t nrMatrices,...)
create a matrix by stacking other matrices Given a set of matrices: A1, A2, A3...
Definition Matrix.cpp:397
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::treeTraversal::DepthFirstForest
void DepthFirstForest(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, VISITOR_POST &visitorPost)
Traverse a forest depth-first with pre-order and post-order visits.
Definition treeTraversal-inst.h:77
│ │ │ │ +
gtsam::treeTraversal::CloneForest
FastVector< boost::shared_ptr< typename FOREST::Node > > CloneForest(const FOREST &forest)
Clone a tree, copy-constructing new nodes (calling boost::make_shared) and setting up child pointers ...
Definition treeTraversal-inst.h:189
│ │ │ │ +
gtsam::treeTraversal::PrintForest
void PrintForest(const FOREST &forest, std::string str, const KeyFormatter &keyFormatter)
Print a tree, prefixing each line with str, and formatting keys using keyFormatter.
Definition treeTraversal-inst.h:219
│ │ │ │ +
gtsam::treeTraversal::DepthFirstForestParallel
void DepthFirstForestParallel(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, VISITOR_POST &visitorPost, int problemSizeThreshold=10)
Traverse a forest depth-first with pre-order and post-order visits.
Definition treeTraversal-inst.h:154
│ │ │ │ +
gtsam::FastList
FastList is a thin wrapper around std::list that uses the boost fast_pool_allocator instead of the de...
Definition FastList.h:40
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,394 +1,243 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -timing.h │ │ │ │ │ +treeTraversal-inst.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_M_a_p_._h> │ │ │ │ │ -21#include │ │ │ │ │ -22#include // for GTSAM_USE_TBB │ │ │ │ │ -23 │ │ │ │ │ -24#include │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ -27 │ │ │ │ │ -28#include │ │ │ │ │ +17#pragma once │ │ │ │ │ +18 │ │ │ │ │ +19#include │ │ │ │ │ +20#include │ │ │ │ │ +21 │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_L_i_s_t_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ +25#include // for GTSAM_USE_TBB │ │ │ │ │ +26 │ │ │ │ │ +27#include │ │ │ │ │ +28#include │ │ │ │ │ 29#include │ │ │ │ │ -30 │ │ │ │ │ -31// This file contains the GTSAM timing instrumentation library, a low- │ │ │ │ │ -overhead method for │ │ │ │ │ -32// learning at a medium-fine level how much time various components of an │ │ │ │ │ -algorithm take │ │ │ │ │ -33// in CPU and wall time. │ │ │ │ │ -34// │ │ │ │ │ -35// The output of this instrumentation is a call-tree-like printout containing │ │ │ │ │ -statistics │ │ │ │ │ -36// about each instrumented code block. To print this output at any time, call │ │ │ │ │ -37// tictoc_print() or tictoc_print_(). │ │ │ │ │ -38// │ │ │ │ │ -39// An overall point to be aware of is that there are two versions of each │ │ │ │ │ -function - one │ │ │ │ │ -40// ending in an underscore '_' and one without the trailing underscore. The │ │ │ │ │ -underscore │ │ │ │ │ -41// versions always are active, but the versions without an underscore are │ │ │ │ │ -active only when │ │ │ │ │ -42// GTSAM_ENABLE_TIMING is defined (automatically defined in our CMake Timing │ │ │ │ │ -build type). │ │ │ │ │ -43// GTSAM algorithms are all instrumented with the non-underscore versions, so │ │ │ │ │ -generally │ │ │ │ │ -44// you should use the underscore versions in your own code to leave out the │ │ │ │ │ -GTSAM detail. │ │ │ │ │ -45// │ │ │ │ │ -46// gttic and gttoc start and stop a timed section, respectively. gttic │ │ │ │ │ -creates a *scoped* │ │ │ │ │ -47// object - when it goes out of scope gttoc is called automatically. Thus, │ │ │ │ │ -you do not │ │ │ │ │ -48// need to call gttoc if you are timing an entire function (see basic use │ │ │ │ │ -examples below). │ │ │ │ │ -49// However, you must be *aware* of this scoped nature - putting gttic inside │ │ │ │ │ -of an if(...) │ │ │ │ │ -50// block, for example, will only time code until the closing brace '}'. See │ │ │ │ │ -advanced │ │ │ │ │ -51// usage below if you need to avoid this. │ │ │ │ │ -52// │ │ │ │ │ -53// Multiple calls nest automatically - each gttic nests under the previous │ │ │ │ │ -gttic called │ │ │ │ │ -54// for which gttoc has not been called (or the previous gttic did not go out │ │ │ │ │ -of scope). │ │ │ │ │ -55// │ │ │ │ │ -56// Basic usage examples are as follows: │ │ │ │ │ -57// │ │ │ │ │ -58// - Timing an entire function: │ │ │ │ │ -59// void myFunction() { │ │ │ │ │ -60// gttic_(myFunction); │ │ │ │ │ -61// ........ │ │ │ │ │ -62// } │ │ │ │ │ -63// │ │ │ │ │ -64// - Timing an entire function as well as its component parts: │ │ │ │ │ -65// void myLongFunction() { │ │ │ │ │ -66// gttic_(myLongFunction); │ │ │ │ │ -67// gttic_(step1); // Will nest under the 'myLongFunction' label │ │ │ │ │ -68// ........ │ │ │ │ │ -69// gttoc_(step1); │ │ │ │ │ -70// gttic_(step2); // Will nest under the 'myLongFunction' label │ │ │ │ │ -71// ........ │ │ │ │ │ -72// gttoc_(step2); │ │ │ │ │ -73// ........ │ │ │ │ │ -74// } │ │ │ │ │ -75// │ │ │ │ │ -76// - Timing functions calling/called by other functions: │ │ │ │ │ -77// void oneStep() { │ │ │ │ │ -78// gttic_(oneStep); // Will automatically nest under the gttic label of the │ │ │ │ │ -calling function │ │ │ │ │ -79// ....... │ │ │ │ │ -80// } │ │ │ │ │ -81// void algorithm() { │ │ │ │ │ -82// gttic_(algorithm); │ │ │ │ │ -83// oneStep(); // gttic's inside this function will automatically nest inside │ │ │ │ │ -our 'algorithm' label │ │ │ │ │ -84// twoStep(); // gttic's inside this function will automatically nest inside │ │ │ │ │ -our 'algorithm' label │ │ │ │ │ -85// } │ │ │ │ │ -86// │ │ │ │ │ -87// │ │ │ │ │ -88// Advanced usage: │ │ │ │ │ -89// │ │ │ │ │ -90// - "Finishing iterations" - to get correct min/max times for each call, you │ │ │ │ │ -must define │ │ │ │ │ -91// in your code what constitutes an iteration. A single sum for the min/max │ │ │ │ │ -times is │ │ │ │ │ -92// accumulated within each iteration. If you don't care about min/max times, │ │ │ │ │ -you don't │ │ │ │ │ -93// need to worry about this. For example: │ │ │ │ │ -94// void myOuterLoop() { │ │ │ │ │ -95// while(true) { │ │ │ │ │ -96// iterateMyAlgorithm(); │ │ │ │ │ -97// tictoc_finishedIteration_(); │ │ │ │ │ -98// tictoc_print_(); // Optional │ │ │ │ │ -99// } │ │ │ │ │ -100// } │ │ │ │ │ -101// │ │ │ │ │ -102// - Stopping timing a section in a different scope than it is started. │ │ │ │ │ -Normally, a gttoc │ │ │ │ │ -103// statement goes out of scope at end of C++ scope. However, you can use │ │ │ │ │ -longtic and │ │ │ │ │ -104// longtoc to start and stop timing with the specified label at any point, │ │ │ │ │ -without regard │ │ │ │ │ -105// too scope. Note that if you use these, it may become difficult to ensure │ │ │ │ │ -that you │ │ │ │ │ -106// have matching gttic/gttoc statments. You may want to consider │ │ │ │ │ -reorganizing your timing │ │ │ │ │ -107// outline to match the scope of your code. │ │ │ │ │ -108 │ │ │ │ │ -109// Automatically use the new Boost timers if version is recent enough. │ │ │ │ │ -110#if BOOST_VERSION >= 104800 │ │ │ │ │ -111# ifndef GTSAM_DISABLE_NEW_TIMERS │ │ │ │ │ -112# define GTSAM_USING_NEW_BOOST_TIMERS │ │ │ │ │ -113# endif │ │ │ │ │ -114#endif │ │ │ │ │ -115 │ │ │ │ │ -116#ifdef GTSAM_USING_NEW_BOOST_TIMERS │ │ │ │ │ -117# include │ │ │ │ │ -118#else │ │ │ │ │ -119# include │ │ │ │ │ -120# include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ -121#endif │ │ │ │ │ -122 │ │ │ │ │ -123#ifdef GTSAM_USE_TBB │ │ │ │ │ -124# include │ │ │ │ │ -125# undef min │ │ │ │ │ -126# undef max │ │ │ │ │ -127# undef ERROR │ │ │ │ │ -128#endif │ │ │ │ │ -129 │ │ │ │ │ -130namespace _g_t_s_a_m { │ │ │ │ │ -131 │ │ │ │ │ -132 namespace internal { │ │ │ │ │ -133 // Generate/retrieve a unique global ID number that will be used to look up │ │ │ │ │ -tic/toc statements │ │ │ │ │ -134 GTSAM_EXPORT size_t getTicTocID(const char *description); │ │ │ │ │ -135 │ │ │ │ │ -136 // Create new TimingOutline child for gCurrentTimer, make it gCurrentTimer, │ │ │ │ │ -and call tic method │ │ │ │ │ -137 GTSAM_EXPORT void tic(size_t id, const char *label); │ │ │ │ │ -138 │ │ │ │ │ -139 // Call toc on gCurrentTimer and then set gCurrentTimer to the parent of │ │ │ │ │ -gCurrentTimer │ │ │ │ │ -140 GTSAM_EXPORT void toc(size_t id, const char *label); │ │ │ │ │ -141 │ │ │ │ │ -_1_4_5 class _T_i_m_i_n_g_O_u_t_l_i_n_e { │ │ │ │ │ -146 protected: │ │ │ │ │ -147 size_t id_; │ │ │ │ │ -148 size_t t_; │ │ │ │ │ -149 size_t tWall_; │ │ │ │ │ -_1_5_0 double _t_2__ ; │ │ │ │ │ -151 size_t tIt_; │ │ │ │ │ -152 size_t tMax_; │ │ │ │ │ -153 size_t tMin_; │ │ │ │ │ -154 size_t n_; │ │ │ │ │ -155 size_t myOrder_; │ │ │ │ │ -156 size_t lastChildOrder_; │ │ │ │ │ -157 std::string label_; │ │ │ │ │ -158 │ │ │ │ │ -159 // Tree structure │ │ │ │ │ -_1_6_0 boost::weak_ptr _p_a_r_e_n_t__; │ │ │ │ │ -161 typedef _F_a_s_t_M_a_p_<_s_i_z_e___t_,_ _b_o_o_s_t_:_:_s_h_a_r_e_d___p_t_r_<_T_i_m_i_n_g_O_u_t_l_i_n_e_> > _C_h_i_l_d_M_a_p; │ │ │ │ │ -_1_6_2 _C_h_i_l_d_M_a_p _c_h_i_l_d_r_e_n__; │ │ │ │ │ -163 │ │ │ │ │ -164#ifdef GTSAM_USING_NEW_BOOST_TIMERS │ │ │ │ │ -165 boost::timer::cpu_timer timer_; │ │ │ │ │ -166#else │ │ │ │ │ -167 boost::timer timer_; │ │ │ │ │ -168 _g_t_s_a_m_:_:_V_a_l_u_e_W_i_t_h_D_e_f_a_u_l_t_<_b_o_o_l_,_f_a_l_s_e_> timerActive_; │ │ │ │ │ -169#endif │ │ │ │ │ -170#ifdef GTSAM_USE_TBB │ │ │ │ │ -171 tbb::tick_count tbbTimer_; │ │ │ │ │ -172#endif │ │ │ │ │ -173 void add(size_t usecs, size_t usecsWall); │ │ │ │ │ -174 │ │ │ │ │ -175 public: │ │ │ │ │ -177 GTSAM_EXPORT _T_i_m_i_n_g_O_u_t_l_i_n_e(const std::string& label, size_t myId); │ │ │ │ │ -178 GTSAM_EXPORT size_t _t_i_m_e() const; │ │ │ │ │ -_1_7_9 double _s_e_c_s() const { return double(_t_i_m_e()) / 1000000.0;} │ │ │ │ │ -_1_8_0 double _s_e_l_f() const { return double(t_) / 1000000.0;} │ │ │ │ │ -_1_8_1 double _w_a_l_l() const { return double(tWall_) / 1000000.0;} │ │ │ │ │ -_1_8_2 double _m_i_n() const { return double(tMin_) / 1000000.0;} │ │ │ │ │ -_1_8_3 double _m_a_x() const { return double(tMax_) / 1000000.0;} │ │ │ │ │ -_1_8_4 double _m_e_a_n() const { return _s_e_l_f() / double(n_); } │ │ │ │ │ -185 GTSAM_EXPORT void print(const std::string& outline = "") const; │ │ │ │ │ -186 GTSAM_EXPORT void print2(const std::string& outline = "", const double │ │ │ │ │ -parentTotal = -1.0) const; │ │ │ │ │ -187 GTSAM_EXPORT const boost::shared_ptr& │ │ │ │ │ -188 child(size_t child, const std::string& label, const boost:: │ │ │ │ │ -weak_ptr& thisPtr); │ │ │ │ │ -189 GTSAM_EXPORT void tic(); │ │ │ │ │ -190 GTSAM_EXPORT void toc(); │ │ │ │ │ -191 GTSAM_EXPORT void finishedIteration(); │ │ │ │ │ -192 │ │ │ │ │ -193 GTSAM_EXPORT friend void toc(size_t id, const char *label); │ │ │ │ │ -194 }; // \TimingOutline │ │ │ │ │ -195 │ │ │ │ │ -_1_9_9 class GTSAM_EXPORT _A_u_t_o_T_i_c_T_o_c { │ │ │ │ │ -200 private: │ │ │ │ │ -201 size_t id_; │ │ │ │ │ -202 const char* label_; │ │ │ │ │ -203 bool isSet_; │ │ │ │ │ -204 │ │ │ │ │ -205 public: │ │ │ │ │ -206 _A_u_t_o_T_i_c_T_o_c(size_t id, const char* label) │ │ │ │ │ -207 : id_(id), label_(label), isSet_(true) { │ │ │ │ │ -208 tic(id_, label_); │ │ │ │ │ -209 } │ │ │ │ │ -210 void stop() { │ │ │ │ │ -211 toc(id_, label_); │ │ │ │ │ -212 isSet_ = false; │ │ │ │ │ -213 } │ │ │ │ │ -214 _~_A_u_t_o_T_i_c_T_o_c() { │ │ │ │ │ -215 if (isSet_) stop(); │ │ │ │ │ -216 } │ │ │ │ │ -217 }; │ │ │ │ │ -218 │ │ │ │ │ -219 GTSAM_EXTERN_EXPORT boost::shared_ptr gTimingRoot; │ │ │ │ │ -220 GTSAM_EXTERN_EXPORT boost::weak_ptr gCurrentTimer; │ │ │ │ │ -221 } │ │ │ │ │ -222 │ │ │ │ │ -223// Tic and toc functions that are always active (whether or not │ │ │ │ │ -ENABLE_TIMING is defined) │ │ │ │ │ -224// There is a trick being used here to achieve near-zero runtime overhead, │ │ │ │ │ -in that a │ │ │ │ │ -225// static variable is created for each tic/toc statement storing an integer │ │ │ │ │ -ID, but the │ │ │ │ │ -226// integer ID is only looked up by string once when the static variable is │ │ │ │ │ -initialized │ │ │ │ │ -227// as the program starts. │ │ │ │ │ -228 │ │ │ │ │ -229// tic │ │ │ │ │ -230#define gttic_(label) \ │ │ │ │ │ -231 static const size_t label##_id_tic = ::gtsam::internal::getTicTocID │ │ │ │ │ -(#label); \ │ │ │ │ │ -232 ::gtsam::internal::AutoTicToc label##_obj(label##_id_tic, #label) │ │ │ │ │ -233 │ │ │ │ │ -234// toc │ │ │ │ │ -235#define gttoc_(label) \ │ │ │ │ │ -236 label##_obj.stop() │ │ │ │ │ -237 │ │ │ │ │ -238// tic │ │ │ │ │ -239#define longtic_(label) \ │ │ │ │ │ -240 static const size_t label##_id_tic = ::gtsam::internal::getTicTocID │ │ │ │ │ -(#label); \ │ │ │ │ │ -241 ::gtsam::internal::ticInternal(label##_id_tic, #label) │ │ │ │ │ -242 │ │ │ │ │ -243// toc │ │ │ │ │ -244#define longtoc_(label) \ │ │ │ │ │ -245 static const size_t label##_id_toc = ::gtsam::internal::getTicTocID │ │ │ │ │ -(#label); \ │ │ │ │ │ -246 ::gtsam::internal::tocInternal(label##_id_toc, #label) │ │ │ │ │ -247 │ │ │ │ │ -248// indicate iteration is finished │ │ │ │ │ -249inline void tictoc_finishedIteration_() { │ │ │ │ │ -250 ::gtsam::internal::gTimingRoot->finishedIteration(); } │ │ │ │ │ -251 │ │ │ │ │ -252// print │ │ │ │ │ -253inline void tictoc_print_() { │ │ │ │ │ -254 ::gtsam::internal::gTimingRoot->print(); } │ │ │ │ │ -255 │ │ │ │ │ -256// print mean and standard deviation │ │ │ │ │ -257inline void tictoc_print2_() { │ │ │ │ │ -258 ::gtsam::internal::gTimingRoot->print2(); } │ │ │ │ │ -259 │ │ │ │ │ -260// get a node by label and assign it to variable │ │ │ │ │ -261#define tictoc_getNode(variable, label) \ │ │ │ │ │ -262 static const size_t label##_id_getnode = ::gtsam::internal::getTicTocID │ │ │ │ │ -(#label); \ │ │ │ │ │ -263 const boost::shared_ptr variable = │ │ │ │ │ -\ │ │ │ │ │ -264 ::gtsam::internal::gCurrentTimer.lock()->child(label##_id_getnode, #label, │ │ │ │ │ -::gtsam::internal::gCurrentTimer); │ │ │ │ │ -265 │ │ │ │ │ -266// reset │ │ │ │ │ -267inline void tictoc_reset_() { │ │ │ │ │ -268 ::gtsam::internal::gTimingRoot.reset(new ::gtsam::internal::TimingOutline │ │ │ │ │ -("Total", ::gtsam::internal::getTicTocID("Total"))); │ │ │ │ │ -269 ::gtsam::internal::gCurrentTimer = ::gtsam::internal::gTimingRoot; } │ │ │ │ │ -270 │ │ │ │ │ -271#ifdef ENABLE_TIMING │ │ │ │ │ -272#define gttic(label) gttic_(label) │ │ │ │ │ -273#define gttoc(label) gttoc_(label) │ │ │ │ │ -274#define longtic(label) longtic_(label) │ │ │ │ │ -275#define longtoc(label) longtoc_(label) │ │ │ │ │ -276#define tictoc_finishedIteration tictoc_finishedIteration_ │ │ │ │ │ -277#define tictoc_print tictoc_print_ │ │ │ │ │ -278#define tictoc_reset tictoc_reset_ │ │ │ │ │ -279#else │ │ │ │ │ -280#define gttic(label) ((void)0) │ │ │ │ │ -281#define gttoc(label) ((void)0) │ │ │ │ │ -282#define longtic(label) ((void)0) │ │ │ │ │ -283#define longtoc(label) ((void)0) │ │ │ │ │ -284#define tictoc_finishedIteration() ((void)0) │ │ │ │ │ -285#define tictoc_print() ((void)0) │ │ │ │ │ -286#define tictoc_reset() ((void)0) │ │ │ │ │ -287#endif │ │ │ │ │ -288 │ │ │ │ │ -289} │ │ │ │ │ -_F_a_s_t_M_a_p_._h │ │ │ │ │ -A thin wrapper around std::map that uses boost's fast_pool_allocator. │ │ │ │ │ -_t_y_p_e_s_._h │ │ │ │ │ -Typedefs for easier changing of types. │ │ │ │ │ +30#include │ │ │ │ │ +31#include │ │ │ │ │ +32 │ │ │ │ │ +33namespace _g_t_s_a_m { │ │ │ │ │ +34 │ │ │ │ │ +_3_6namespace treeTraversal { │ │ │ │ │ +37 │ │ │ │ │ +38/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +39namespace { │ │ │ │ │ +40// Internal node used in DFS preorder stack │ │ │ │ │ +41template │ │ │ │ │ +42struct TraversalNode { │ │ │ │ │ +43 bool expanded; │ │ │ │ │ +44 const boost::shared_ptr& treeNode; │ │ │ │ │ +45 DATA& parentData; │ │ │ │ │ +46 typename _F_a_s_t_L_i_s_t_<_D_A_T_A_>_:_:_i_t_e_r_a_t_o_r dataPointer; │ │ │ │ │ +47 TraversalNode(const boost::shared_ptr& _treeNode, DATA& _parentData) : │ │ │ │ │ +48 expanded(false), treeNode(_treeNode), parentData(_parentData) { │ │ │ │ │ +49 } │ │ │ │ │ +50}; │ │ │ │ │ +51 │ │ │ │ │ +52// Do nothing - default argument for post-visitor for tree traversal │ │ │ │ │ +53struct no_op { │ │ │ │ │ +54 template │ │ │ │ │ +55 void operator()(const boost::shared_ptr& node, const DATA& data) { │ │ │ │ │ +56 } │ │ │ │ │ +57}; │ │ │ │ │ +58 │ │ │ │ │ +59} │ │ │ │ │ +60 │ │ │ │ │ +75template │ │ │ │ │ +_7_7void _D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t(FOREST& forest, DATA& rootData, VISITOR_PRE& │ │ │ │ │ +visitorPre, │ │ │ │ │ +78 VISITOR_POST& visitorPost) { │ │ │ │ │ +79 // Typedefs │ │ │ │ │ +80 typedef typename FOREST::Node Node; │ │ │ │ │ +81 typedef boost::shared_ptr sharedNode; │ │ │ │ │ +82 │ │ │ │ │ +83 // Depth first traversal stack │ │ │ │ │ +84 typedef TraversalNode TraversalNode; │ │ │ │ │ +85 typedef _F_a_s_t_L_i_s_t_<_T_r_a_v_e_r_s_a_l_N_o_d_e_> Stack; │ │ │ │ │ +86 Stack _s_t_a_c_k; │ │ │ │ │ +87 _F_a_s_t_L_i_s_t_<_D_A_T_A_> dataList; // List to store node data as it is returned from │ │ │ │ │ +the pre-order visitor │ │ │ │ │ +88 │ │ │ │ │ +89 // Add roots to stack (insert such that they are visited and processed in │ │ │ │ │ +order │ │ │ │ │ +90 { │ │ │ │ │ +91 typename Stack::iterator insertLocation = _s_t_a_c_k.begin(); │ │ │ │ │ +92 for(const sharedNode& root: forest.roots()) │ │ │ │ │ +93 _s_t_a_c_k.insert(insertLocation, TraversalNode(root, rootData)); │ │ │ │ │ +94 } │ │ │ │ │ +95 │ │ │ │ │ +96 // Traverse │ │ │ │ │ +97 while (!_s_t_a_c_k.empty()) { │ │ │ │ │ +98 // Get next node │ │ │ │ │ +99 TraversalNode& node = _s_t_a_c_k.front(); │ │ │ │ │ +100 │ │ │ │ │ +101 if (node.expanded) { │ │ │ │ │ +102 // If already expanded, then the data stored in the node is no longer │ │ │ │ │ +needed, so visit │ │ │ │ │ +103 // then delete it. │ │ │ │ │ +104 (void) visitorPost(node.treeNode, *node.dataPointer); │ │ │ │ │ +105 dataList.erase(node.dataPointer); │ │ │ │ │ +106 _s_t_a_c_k.pop_front(); │ │ │ │ │ +107 } else { │ │ │ │ │ +108 // If not already visited, visit the node and add its children (use reverse │ │ │ │ │ +iterators so │ │ │ │ │ +109 // children are processed in the order they appear) │ │ │ │ │ +110 node.dataPointer = dataList.insert(dataList.end(), │ │ │ │ │ +111 visitorPre(node.treeNode, node.parentData)); │ │ │ │ │ +112 typename Stack::iterator insertLocation = _s_t_a_c_k.begin(); │ │ │ │ │ +113 for(const sharedNode& child: node.treeNode->children) │ │ │ │ │ +114 _s_t_a_c_k.insert(insertLocation, TraversalNode(child, *node.dataPointer)); │ │ │ │ │ +115 node.expanded = true; │ │ │ │ │ +116 } │ │ │ │ │ +117 } │ │ │ │ │ +118 assert(dataList.empty()); │ │ │ │ │ +119} │ │ │ │ │ +120 │ │ │ │ │ +132template │ │ │ │ │ +_1_3_3void _D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t(FOREST& forest, DATA& rootData, VISITOR_PRE& │ │ │ │ │ +visitorPre) { │ │ │ │ │ +134 no_op visitorPost; │ │ │ │ │ +135 _D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t(forest, rootData, visitorPre, visitorPost); │ │ │ │ │ +136} │ │ │ │ │ +137 │ │ │ │ │ +152template │ │ │ │ │ +_1_5_4void _D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t_P_a_r_a_l_l_e_l(FOREST& forest, DATA& rootData, │ │ │ │ │ +155 VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, │ │ │ │ │ +156 int problemSizeThreshold = 10) { │ │ │ │ │ +157#ifdef GTSAM_USE_TBB │ │ │ │ │ +158 // Typedefs │ │ │ │ │ +159 typedef typename FOREST::Node Node; │ │ │ │ │ +160 │ │ │ │ │ +161 internal::CreateRootTask(forest.roots(), rootData, visitorPre, │ │ │ │ │ +162 visitorPost, problemSizeThreshold); │ │ │ │ │ +163#else │ │ │ │ │ +164 _D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t(forest, rootData, visitorPre, visitorPost); │ │ │ │ │ +165#endif │ │ │ │ │ +166} │ │ │ │ │ +167 │ │ │ │ │ +168/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +170namespace { │ │ │ │ │ +171template │ │ │ │ │ +172boost::shared_ptr CloneForestVisitorPre( │ │ │ │ │ +173 const boost::shared_ptr& node, │ │ │ │ │ +174 const boost::shared_ptr& parentPointer) { │ │ │ │ │ +175 // Clone the current node and add it to its cloned parent │ │ │ │ │ +176 boost::shared_ptr clone = boost::make_shared(*node); │ │ │ │ │ +177 clone->children.clear(); │ │ │ │ │ +178 parentPointer->children.push_back(clone); │ │ │ │ │ +179 return clone; │ │ │ │ │ +180} │ │ │ │ │ +181} │ │ │ │ │ +182 │ │ │ │ │ +188template │ │ │ │ │ +_1_8_9_F_a_s_t_V_e_c_t_o_r_<_b_o_o_s_t_:_:_s_h_a_r_e_d___p_t_r_<_t_y_p_e_n_a_m_e_ _F_O_R_E_S_T_:_:_N_o_d_e_> > _C_l_o_n_e_F_o_r_e_s_t( │ │ │ │ │ +190 const FOREST& forest) { │ │ │ │ │ +191 typedef typename FOREST::Node Node; │ │ │ │ │ +192 boost::shared_ptr rootContainer = boost::make_shared(); │ │ │ │ │ +193 _D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t(forest, rootContainer, CloneForestVisitorPre); │ │ │ │ │ +194 return _F_a_s_t_V_e_c_t_o_r_<_b_o_o_s_t_:_:_s_h_a_r_e_d___p_t_r_<_N_o_d_e_> >(rootContainer->children.begin │ │ │ │ │ +(), │ │ │ │ │ +195 rootContainer->children.end()); │ │ │ │ │ +196} │ │ │ │ │ +197 │ │ │ │ │ +198/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +200namespace { │ │ │ │ │ +201struct PrintForestVisitorPre { │ │ │ │ │ +202 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter; │ │ │ │ │ +203 PrintForestVisitorPre(const _K_e_y_F_o_r_m_a_t_t_e_r& formatter) : │ │ │ │ │ +204 formatter(formatter) { │ │ │ │ │ +205 } │ │ │ │ │ +206 template std::string operator()( │ │ │ │ │ +207 const boost::shared_ptr& node, const std::string& parentString) { │ │ │ │ │ +208 // Print the current node │ │ │ │ │ +209 node->print(parentString + "-", formatter); │ │ │ │ │ +210 // Increment the indentation │ │ │ │ │ +211 return parentString + "| "; │ │ │ │ │ +212 } │ │ │ │ │ +213}; │ │ │ │ │ +214} │ │ │ │ │ +215 │ │ │ │ │ +218template │ │ │ │ │ +_2_1_9void _P_r_i_n_t_F_o_r_e_s_t(const FOREST& forest, std::string str, │ │ │ │ │ +220 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter) { │ │ │ │ │ +221 PrintForestVisitorPre visitor(keyFormatter); │ │ │ │ │ +222 _D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t(forest, str, visitor); │ │ │ │ │ +223} │ │ │ │ │ +224} // namespace treeTraversal │ │ │ │ │ +225 │ │ │ │ │ +226} // namespace gtsam │ │ │ │ │ +_F_a_s_t_L_i_s_t_._h │ │ │ │ │ +A thin wrapper around std::list that uses boost's fast_pool_allocator. │ │ │ │ │ +_F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ +A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ +_K_e_y_._h │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ +std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ +FastVector │ │ │ │ │ +FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ +DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ -FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ -fast_pool_allocator instead of the defa... │ │ │ │ │ -DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_T_i_m_i_n_g_O_u_t_l_i_n_e │ │ │ │ │ -Timing Entry, arranged in a tree. │ │ │ │ │ -DDeeffiinniittiioonn timing.h:145 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_T_i_m_i_n_g_O_u_t_l_i_n_e_:_:_t_2__ │ │ │ │ │ -double t2_ │ │ │ │ │ -cache the │ │ │ │ │ -DDeeffiinniittiioonn timing.h:150 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_T_i_m_i_n_g_O_u_t_l_i_n_e_:_:_c_h_i_l_d_r_e_n__ │ │ │ │ │ -ChildMap children_ │ │ │ │ │ -subtrees │ │ │ │ │ -DDeeffiinniittiioonn timing.h:162 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_T_i_m_i_n_g_O_u_t_l_i_n_e_:_:_m_a_x │ │ │ │ │ -double max() const │ │ │ │ │ -max time, in seconds │ │ │ │ │ -DDeeffiinniittiioonn timing.h:183 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_T_i_m_i_n_g_O_u_t_l_i_n_e_:_:_w_a_l_l │ │ │ │ │ -double wall() const │ │ │ │ │ -wall time, in seconds │ │ │ │ │ -DDeeffiinniittiioonn timing.h:181 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_T_i_m_i_n_g_O_u_t_l_i_n_e_:_:_s_e_c_s │ │ │ │ │ -double secs() const │ │ │ │ │ -time taken, in seconds, including children │ │ │ │ │ -DDeeffiinniittiioonn timing.h:179 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_T_i_m_i_n_g_O_u_t_l_i_n_e_:_:_s_e_l_f │ │ │ │ │ -double self() const │ │ │ │ │ -self time only, in seconds │ │ │ │ │ -DDeeffiinniittiioonn timing.h:180 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_T_i_m_i_n_g_O_u_t_l_i_n_e_:_:_p_a_r_e_n_t__ │ │ │ │ │ -boost::weak_ptr< TimingOutline > parent_ │ │ │ │ │ -parent pointer │ │ │ │ │ -DDeeffiinniittiioonn timing.h:160 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_T_i_m_i_n_g_O_u_t_l_i_n_e_:_:_m_i_n │ │ │ │ │ -double min() const │ │ │ │ │ -min time, in seconds │ │ │ │ │ -DDeeffiinniittiioonn timing.h:182 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_T_i_m_i_n_g_O_u_t_l_i_n_e_:_:_m_e_a_n │ │ │ │ │ -double mean() const │ │ │ │ │ -mean self time, in seconds │ │ │ │ │ -DDeeffiinniittiioonn timing.h:184 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_T_i_m_i_n_g_O_u_t_l_i_n_e_:_:_t_i_m_e │ │ │ │ │ -GTSAM_EXPORT size_t time() const │ │ │ │ │ -time taken, including children │ │ │ │ │ -DDeeffiinniittiioonn timing.cpp:65 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_A_u_t_o_T_i_c_T_o_c │ │ │ │ │ -Small class that calls internal::tic at construction, and internol::toc when │ │ │ │ │ -destroyed. │ │ │ │ │ -DDeeffiinniittiioonn timing.h:199 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_W_i_t_h_D_e_f_a_u_l_t │ │ │ │ │ -Helper struct that encapsulates a value with a default, this is just used as a │ │ │ │ │ -member object so you d... │ │ │ │ │ -DDeeffiinniittiioonn types.h:137 │ │ │ │ │ +_g_t_s_a_m_:_:_s_t_a_c_k │ │ │ │ │ +Matrix stack(size_t nrMatrices,...) │ │ │ │ │ +create a matrix by stacking other matrices Given a set of matrices: A1, A2, │ │ │ │ │ +A3... │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:397 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t │ │ │ │ │ +void DepthFirstForest(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, │ │ │ │ │ +VISITOR_POST &visitorPost) │ │ │ │ │ +Traverse a forest depth-first with pre-order and post-order visits. │ │ │ │ │ +DDeeffiinniittiioonn treeTraversal-inst.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_C_l_o_n_e_F_o_r_e_s_t │ │ │ │ │ +FastVector< boost::shared_ptr< typename FOREST::Node > > CloneForest(const │ │ │ │ │ +FOREST &forest) │ │ │ │ │ +Clone a tree, copy-constructing new nodes (calling boost::make_shared) and │ │ │ │ │ +setting up child pointers ... │ │ │ │ │ +DDeeffiinniittiioonn treeTraversal-inst.h:189 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_P_r_i_n_t_F_o_r_e_s_t │ │ │ │ │ +void PrintForest(const FOREST &forest, std::string str, const KeyFormatter │ │ │ │ │ +&keyFormatter) │ │ │ │ │ +Print a tree, prefixing each line with str, and formatting keys using │ │ │ │ │ +keyFormatter. │ │ │ │ │ +DDeeffiinniittiioonn treeTraversal-inst.h:219 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t_P_a_r_a_l_l_e_l │ │ │ │ │ +void DepthFirstForestParallel(FOREST &forest, DATA &rootData, VISITOR_PRE │ │ │ │ │ +&visitorPre, VISITOR_POST &visitorPost, int problemSizeThreshold=10) │ │ │ │ │ +Traverse a forest depth-first with pre-order and post-order visits. │ │ │ │ │ +DDeeffiinniittiioonn treeTraversal-inst.h:154 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t │ │ │ │ │ +FastList is a thin wrapper around std::list that uses the boost │ │ │ │ │ +fast_pool_allocator instead of the de... │ │ │ │ │ +DDeeffiinniittiioonn FastList.h:40 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _t_i_m_i_n_g_._h │ │ │ │ │ + * _t_r_e_e_T_r_a_v_e_r_s_a_l_-_i_n_s_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00095.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/ProductLieGroup.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Matrix.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,46 +95,298 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
ProductLieGroup.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Macros | │ │ │ │ +Typedefs | │ │ │ │ +Functions
│ │ │ │ +
Matrix.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Group product of two Lie Groups. │ │ │ │ +

typedef and functions to augment Eigen's MatrixXd │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::ProductLieGroup< G, H >
 Template to construct the product Lie group of two other Lie groups Assumes Lie group structure for G and H. More...
struct  gtsam::Reshape< OutM, OutN, OutOptions, InM, InN, InOptions >
 Reshape functor. More...
 
struct  gtsam::traits< ProductLieGroup< G, H > >
struct  gtsam::Reshape< M, M, InOptions, M, M, InOptions >
 Reshape specialization that does nothing as shape stays the same (needed to not be ambiguous for square input equals square output) More...
 
struct  gtsam::Reshape< M, N, InOptions, M, N, InOptions >
 Reshape specialization that does nothing as shape stays the same. More...
 
struct  gtsam::Reshape< N, M, InOptions, M, N, InOptions >
 Reshape specialization that does transpose. More...
 
struct  gtsam::MultiplyWithInverse< N >
 Functor that implements multiplication of a vector b with the inverse of a matrix A. More...
 
struct  gtsam::MultiplyWithInverseFunction< T, N >
 Functor that implements multiplication with the inverse of a matrix, itself the result of a function f. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Macros

#define GTSAM_MAKE_MATRIX_DEFS(N)
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Typedefs

│ │ │ │ +typedef Eigen::MatrixXd gtsam::Matrix
 
│ │ │ │ +typedef Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > gtsam::MatrixRowMajor
 
│ │ │ │ +typedef Eigen::Block< Matrix > gtsam::SubMatrix
 
│ │ │ │ +typedef Eigen::Block< const Matrix > gtsam::ConstSubMatrix
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +const Eigen::IOFormat & gtsam::matlabFormat ()
 
│ │ │ │ +template<class MATRIX >
bool gtsam::equal_with_abs_tol (const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
 equals with a tolerance
 
│ │ │ │ +bool gtsam::operator== (const Matrix &A, const Matrix &B)
 equality is just equal_with_abs_tol 1e-9
 
│ │ │ │ +bool gtsam::operator!= (const Matrix &A, const Matrix &B)
 inequality
 
│ │ │ │ +bool gtsam::assert_equal (const Matrix &A, const Matrix &B, double tol=1e-9)
 equals with an tolerance, prints out message if unequal
 
│ │ │ │ +bool gtsam::assert_inequal (const Matrix &A, const Matrix &B, double tol=1e-9)
 inequals with an tolerance, prints out message if within tolerance
 
│ │ │ │ +bool gtsam::assert_equal (const std::list< Matrix > &As, const std::list< Matrix > &Bs, double tol=1e-9)
 equals with an tolerance, prints out message if unequal
 
│ │ │ │ +bool gtsam::linear_independent (const Matrix &A, const Matrix &B, double tol=1e-9)
 check whether the rows of two matrices are linear independent
 
│ │ │ │ +bool gtsam::linear_dependent (const Matrix &A, const Matrix &B, double tol=1e-9)
 check whether the rows of two matrices are linear dependent
 
│ │ │ │ +Vector gtsam::operator^ (const Matrix &A, const Vector &v)
 overload ^ for trans(A)*v We transpose the vectors for speed.
 
│ │ │ │ +template<class MATRIX >
MATRIX gtsam::prod (const MATRIX &A, const MATRIX &B)
 products using old-style format to improve compatibility
 
│ │ │ │ +void gtsam::print (const Matrix &A, const std::string &s, std::ostream &stream)
 print without optional string, must specify cout yourself
 
│ │ │ │ +void gtsam::print (const Matrix &A, const std::string &s="")
 print with optional string to cout
 
│ │ │ │ +void gtsam::save (const Matrix &A, const std::string &s, const std::string &filename)
 save a matrix to file, which can be loaded by matlab
 
istream & gtsam::operator>> (std::istream &inputStream, Matrix &destinationMatrix)
 Read a matrix from an input stream, such as a file.
 
template<class MATRIX >
Eigen::Block< const MATRIX > gtsam::sub (const MATRIX &A, size_t i1, size_t i2, size_t j1, size_t j2)
 extract submatrix, slice semantics, i.e.
 
template<typename Derived1 , typename Derived2 >
void gtsam::insertSub (Eigen::MatrixBase< Derived1 > &fullMatrix, const Eigen::MatrixBase< Derived2 > &subMatrix, size_t i, size_t j)
 insert a submatrix IN PLACE at a specified location in a larger matrix NOTE: there is no size checking
 
│ │ │ │ +Matrix gtsam::diag (const std::vector< Matrix > &Hs)
 Create a matrix with submatrices along its diagonal.
 
template<class MATRIX >
const MATRIX::ConstColXpr gtsam::column (const MATRIX &A, size_t j)
 Extracts a column view from a matrix that avoids a copy.
 
template<class MATRIX >
const MATRIX::ConstRowXpr gtsam::row (const MATRIX &A, size_t j)
 Extracts a row view from a matrix that avoids a copy.
 
template<class MATRIX >
void gtsam::zeroBelowDiagonal (MATRIX &A, size_t cols=0)
 Zeros all of the elements below the diagonal of a matrix, in place.
 
│ │ │ │ +Matrix gtsam::trans (const Matrix &A)
 static transpose function, just calls Eigen transpose member function
 
│ │ │ │ +template<int OutM, int OutN, int OutOptions, int InM, int InN, int InOptions>
Reshape< OutM, OutN, OutOptions, InM, InN, InOptions >::ReshapedType gtsam::reshape (const Eigen::Matrix< double, InM, InN, InOptions > &m)
 
pair< Matrix, Matrix > gtsam::qr (const Matrix &A)
 Householder QR factorization, Golub & Van Loan p 224, explicit version
│ │ │ │ +
 
void gtsam::inplace_QR (Matrix &A)
 QR factorization using Eigen's internal block QR algorithm.
 
list< boost::tuple< Vector, double, double > > gtsam::weighted_eliminate (Matrix &A, Vector &b, const Vector &sigmas)
 Imperative algorithm for in-place full elimination with weights and constraint handling.
 
void gtsam::householder_ (Matrix &A, size_t k, bool copy_vectors)
 Imperative version of Householder QR factorization, Golub & Van Loan p 224 version with Householder vectors below diagonal, as in GVL.
 
void gtsam::householder (Matrix &A, size_t k)
 Householder tranformation, zeros below diagonal.
 
Vector gtsam::backSubstituteUpper (const Matrix &U, const Vector &b, bool unit=false)
 backSubstitute U*x=b
 
Vector gtsam::backSubstituteUpper (const Vector &b, const Matrix &U, bool unit=false)
 backSubstitute x'*U=b'
 
Vector gtsam::backSubstituteLower (const Matrix &L, const Vector &b, bool unit=false)
 backSubstitute L*x=b
 
Matrix gtsam::stack (size_t nrMatrices,...)
 create a matrix by stacking other matrices Given a set of matrices: A1, A2, A3...
 
│ │ │ │ +Matrix gtsam::stack (const std::vector< Matrix > &blocks)
 
Matrix gtsam::collect (const std::vector< const Matrix * > &matrices, size_t m=0, size_t n=0)
 create a matrix by concatenating Given a set of matrices: A1, A2, A3... If all matrices have the same size, specifying single matrix dimensions will avoid the lookup of dimensions
 
│ │ │ │ +Matrix gtsam::collect (size_t nrMatrices,...)
 
void gtsam::vector_scale_inplace (const Vector &v, Matrix &A, bool inf_mask=false)
 scales a matrix row or column by the values in a vector Arguments (Matrix, Vector) scales the columns, (Vector, Matrix) scales the rows
 
│ │ │ │ +Matrix gtsam::vector_scale (const Vector &v, const Matrix &A, bool inf_mask)
 
│ │ │ │ +Matrix gtsam::vector_scale (const Matrix &A, const Vector &v, bool inf_mask)
 
Matrix3 gtsam::skewSymmetric (double wx, double wy, double wz)
 skew symmetric matrix returns this: 0 -wz wy wz 0 -wx -wy wx 0
 
│ │ │ │ +template<class Derived >
Matrix3 gtsam::skewSymmetric (const Eigen::MatrixBase< Derived > &w)
 
│ │ │ │ +Matrix gtsam::inverse_square_root (const Matrix &A)
 Use Cholesky to calculate inverse square root of a matrix.
 
Matrix gtsam::cholesky_inverse (const Matrix &A)
 Return the inverse of a S.P.D.
 
void gtsam::svd (const Matrix &A, Matrix &U, Vector &S, Matrix &V)
 SVD computes economy SVD A=U*S*V'.
 
boost::tuple< int, double, Vector > gtsam::DLT (const Matrix &A, double rank_tol=1e-9)
 Direct linear transform algorithm that calls svd to find a vector v that minimizes the algebraic error A*v.
 
Matrix gtsam::expm (const Matrix &A, size_t K=7)
 Numerical exponential map, naive approach, not industrial strength !!!
 
│ │ │ │ +std::string gtsam::formatMatrixIndented (const std::string &label, const Matrix &matrix, bool makeVectorHorizontal)
 
│ │ │ │ +Matrix gtsam::LLt (const Matrix &A)
 
│ │ │ │ +Matrix gtsam::RtR (const Matrix &A)
 
│ │ │ │ +Vector gtsam::columnNormSquare (const Matrix &A)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Group product of two Lie Groups.

│ │ │ │ -
Date
May, 2015
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ +

typedef and functions to augment Eigen's MatrixXd

│ │ │ │ +
Author
Christian Potthast
│ │ │ │ +
│ │ │ │ +Kai Ni
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +
│ │ │ │ +Alex Cunningham
│ │ │ │ +
│ │ │ │ +Alex Hagiopol
│ │ │ │ +
│ │ │ │ +Varun Agrawal
│ │ │ │ +

Macro Definition Documentation

│ │ │ │ + │ │ │ │ +

◆ GTSAM_MAKE_MATRIX_DEFS

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
#define GTSAM_MAKE_MATRIX_DEFS( N)
│ │ │ │ +
│ │ │ │ +Value:
using Matrix##N = Eigen::Matrix<double, N, N>; \
│ │ │ │ +
using Matrix1##N = Eigen::Matrix<double, 1, N>; \
│ │ │ │ +
using Matrix2##N = Eigen::Matrix<double, 2, N>; \
│ │ │ │ +
using Matrix3##N = Eigen::Matrix<double, 3, N>; \
│ │ │ │ +
using Matrix4##N = Eigen::Matrix<double, 4, N>; \
│ │ │ │ +
using Matrix5##N = Eigen::Matrix<double, 5, N>; \
│ │ │ │ +
using Matrix6##N = Eigen::Matrix<double, 6, N>; \
│ │ │ │ +
using Matrix7##N = Eigen::Matrix<double, 7, N>; \
│ │ │ │ +
using Matrix8##N = Eigen::Matrix<double, 8, N>; \
│ │ │ │ +
using Matrix9##N = Eigen::Matrix<double, 9, N>; \
│ │ │ │ +
static const Eigen::MatrixBase<Matrix##N>::IdentityReturnType I_##N##x##N = Matrix##N::Identity(); \
│ │ │ │ +
static const Eigen::MatrixBase<Matrix##N>::ConstantReturnType Z_##N##x##N = Matrix##N::Zero();
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,30 +1,308 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -ProductLieGroup.h File Reference │ │ │ │ │ -Group product of two Lie Groups. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Matrix.h File Reference │ │ │ │ │ +typedef and functions to augment Eigen's MatrixXd _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_P_r_o_d_u_c_t_L_i_e_G_r_o_u_p_<_ _G_,_ _H_ _> │ │ │ │ │ -  Template to construct the product Lie group of two other Lie groups │ │ │ │ │ - Assumes Lie group structure for G and H. _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_R_e_s_h_a_p_e_<_ _O_u_t_M_,_ _O_u_t_N_,_ _O_u_t_O_p_t_i_o_n_s_,_ _I_n_M_,_ _I_n_N_,_ _I_n_O_p_t_i_o_n_s_ _> │ │ │ │ │ +  _R_e_s_h_a_p_e functor. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p_<_ _G_,_ _H_ _>_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_R_e_s_h_a_p_e_<_ _M_,_ _M_,_ _I_n_O_p_t_i_o_n_s_,_ _M_,_ _M_,_ _I_n_O_p_t_i_o_n_s_ _> │ │ │ │ │ + _R_e_s_h_a_p_e specialization that does nothing as shape stays the same │ │ │ │ │ +  (needed to not be ambiguous for square input equals square output) │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_R_e_s_h_a_p_e_<_ _M_,_ _N_,_ _I_n_O_p_t_i_o_n_s_,_ _M_,_ _N_,_ _I_n_O_p_t_i_o_n_s_ _> │ │ │ │ │ +  _R_e_s_h_a_p_e specialization that does nothing as shape stays the same. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_R_e_s_h_a_p_e_<_ _N_,_ _M_,_ _I_n_O_p_t_i_o_n_s_,_ _M_,_ _N_,_ _I_n_O_p_t_i_o_n_s_ _> │ │ │ │ │ +  _R_e_s_h_a_p_e specialization that does transpose. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_M_u_l_t_i_p_l_y_W_i_t_h_I_n_v_e_r_s_e_<_ _N_ _> │ │ │ │ │ +  Functor that implements multiplication of a vector b with the inverse │ │ │ │ │ + of a matrix A. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_M_u_l_t_i_p_l_y_W_i_t_h_I_n_v_e_r_s_e_F_u_n_c_t_i_o_n_<_ _T_,_ _N_ _> │ │ │ │ │ +  Functor that implements multiplication with the inverse of a matrix, │ │ │ │ │ + itself the result of a function f. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +MMaaccrrooss │ │ │ │ │ +#define  _G_T_S_A_M___M_A_K_E___M_A_T_R_I_X___D_E_F_S(N) │ │ │ │ │ +  │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ + typedef Eigen::MatrixXd  ggttssaamm::::MMaattrriixx │ │ │ │ │ +  │ │ │ │ │ +typedef Eigen::Matrix< double, Eigen::Dynamic, Eigen:: │ │ │ │ │ + Dynamic, Eigen::RowMajor >  ggttssaamm::::MMaattrriixxRRoowwMMaajjoorr │ │ │ │ │ +  │ │ │ │ │ + typedef Eigen::Block< Matrix >  ggttssaamm::::SSuubbMMaattrriixx │ │ │ │ │ +  │ │ │ │ │ + typedef Eigen::Block< const Matrix >  ggttssaamm::::CCoonnssttSSuubbMMaattrriixx │ │ │ │ │ +  │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ + const Eigen::IOFormat &  ggttssaamm::::mmaattllaabbFFoorrmmaatt () │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + bool  ggttssaamm::::eeqquuaall__wwiitthh__aabbss__ttooll (const Eigen:: │ │ │ │ │ + DenseBase< MATRIX > &A, const Eigen:: │ │ │ │ │ + DenseBase< MATRIX > &B, double tol=1e-9) │ │ │ │ │ +  equals with a tolerance │ │ │ │ │ +  │ │ │ │ │ + bool  ggttssaamm::::ooppeerraattoorr==== (const Matrix &A, const │ │ │ │ │ + Matrix &B) │ │ │ │ │ +  equality is just equal_with_abs_tol 1e-9 │ │ │ │ │ +  │ │ │ │ │ + bool  ggttssaamm::::ooppeerraattoorr!!== (const Matrix &A, const │ │ │ │ │ + Matrix &B) │ │ │ │ │ +  inequality │ │ │ │ │ +  │ │ │ │ │ + bool  ggttssaamm::::aasssseerrtt__eeqquuaall (const Matrix &A, │ │ │ │ │ + const Matrix &B, double tol=1e-9) │ │ │ │ │ +  equals with an tolerance, prints out │ │ │ │ │ + message if unequal │ │ │ │ │ +  │ │ │ │ │ + bool  ggttssaamm::::aasssseerrtt__iinneeqquuaall (const Matrix &A, │ │ │ │ │ + const Matrix &B, double tol=1e-9) │ │ │ │ │ +  inequals with an tolerance, prints out │ │ │ │ │ + message if within tolerance │ │ │ │ │ +  │ │ │ │ │ + bool  ggttssaamm::::aasssseerrtt__eeqquuaall (const std::list< │ │ │ │ │ + Matrix > &As, const std::list< Matrix > │ │ │ │ │ + &Bs, double tol=1e-9) │ │ │ │ │ +  equals with an tolerance, prints out │ │ │ │ │ + message if unequal │ │ │ │ │ +  │ │ │ │ │ + bool  ggttssaamm::::lliinneeaarr__iinnddeeppeennddeenntt (const Matrix │ │ │ │ │ + &A, const Matrix &B, double tol=1e-9) │ │ │ │ │ +  check whether the rows of two matrices │ │ │ │ │ + are linear independent │ │ │ │ │ +  │ │ │ │ │ + bool  ggttssaamm::::lliinneeaarr__ddeeppeennddeenntt (const Matrix &A, │ │ │ │ │ + const Matrix &B, double tol=1e-9) │ │ │ │ │ +  check whether the rows of two matrices │ │ │ │ │ + are linear dependent │ │ │ │ │ +  │ │ │ │ │ + Vector  ggttssaamm::::ooppeerraattoorr^^ (const Matrix &A, const │ │ │ │ │ + Vector &v) │ │ │ │ │ +  overload ^ for trans(A)*v We transpose │ │ │ │ │ + the vectors for speed. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + MATRIX  ggttssaamm::::pprroodd (const MATRIX &A, const │ │ │ │ │ + MATRIX &B) │ │ │ │ │ +  products using old-style format to │ │ │ │ │ + improve compatibility │ │ │ │ │ +  │ │ │ │ │ + void  ggttssaamm::::pprriinntt (const Matrix &A, const │ │ │ │ │ + std::string &s, std::ostream &stream) │ │ │ │ │ +  print without optional string, must │ │ │ │ │ + specify cout yourself │ │ │ │ │ +  │ │ │ │ │ + void  ggttssaamm::::pprriinntt (const Matrix &A, const │ │ │ │ │ + std::string &s="") │ │ │ │ │ +  print with optional string to cout │ │ │ │ │ +  │ │ │ │ │ + void  ggttssaamm::::ssaavvee (const Matrix &A, const std:: │ │ │ │ │ + string &s, const std::string &filename) │ │ │ │ │ +  save a matrix to file, which can be │ │ │ │ │ + loaded by matlab │ │ │ │ │ +  │ │ │ │ │ + istream &  _g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_>_> (std::istream │ │ │ │ │ + &inputStream, Matrix &destinationMatrix) │ │ │ │ │ +  Read a matrix from an input stream, such │ │ │ │ │ + as a file. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + Eigen::Block< const MATRIX >  _g_t_s_a_m_:_:_s_u_b (const MATRIX &A, size_t i1, │ │ │ │ │ + size_t i2, size_t j1, size_t j2) │ │ │ │ │ +  extract submatrix, slice semantics, i.e. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + void  _g_t_s_a_m_:_:_i_n_s_e_r_t_S_u_b (Eigen::MatrixBase< │ │ │ │ │ + Derived1 > &fullMatrix, const Eigen:: │ │ │ │ │ + MatrixBase< Derived2 > &subMatrix, size_t │ │ │ │ │ + i, size_t j) │ │ │ │ │ + insert a submatrix IN PLACE at a │ │ │ │ │ +  specified location in a larger matrix │ │ │ │ │ + NOTE: there is no size checking │ │ │ │ │ +  │ │ │ │ │ + Matrix  ggttssaamm::::ddiiaagg (const std::vector< Matrix > │ │ │ │ │ + &Hs) │ │ │ │ │ +  Create a matrix with submatrices along │ │ │ │ │ + its diagonal. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + const MATRIX::ConstColXpr  _g_t_s_a_m_:_:_c_o_l_u_m_n (const MATRIX &A, size_t j) │ │ │ │ │ +  Extracts a column view from a matrix that │ │ │ │ │ + avoids a copy. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + const MATRIX::ConstRowXpr  _g_t_s_a_m_:_:_r_o_w (const MATRIX &A, size_t j) │ │ │ │ │ +  Extracts a row view from a matrix that │ │ │ │ │ + avoids a copy. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + void  _g_t_s_a_m_:_:_z_e_r_o_B_e_l_o_w_D_i_a_g_o_n_a_l (MATRIX &A, │ │ │ │ │ + size_t cols=0) │ │ │ │ │ +  Zeros all of the elements below the │ │ │ │ │ + diagonal of a matrix, in place. │ │ │ │ │ +  │ │ │ │ │ + Matrix  ggttssaamm::::ttrraannss (const Matrix &A) │ │ │ │ │ +  static transpose function, just calls │ │ │ │ │ + Eigen transpose member function │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +_R_e_s_h_a_p_e< OutM, OutN, OutOptions, InM, ggttssaamm::::rreesshhaappee (const Eigen::Matrix< │ │ │ │ │ + InN, InOptions >::ReshapedType  double, InM, InN, InOptions > &m) │ │ │ │ │ +  │ │ │ │ │ + pair< Matrix, Matrix >  _g_t_s_a_m_:_:_q_r (const Matrix &A) │ │ │ │ │ + Householder QR factorization, Golub & Van │ │ │ │ │ +  Loan p 224, explicit version │ │ │ │ │ + │ │ │ │ │ +  │ │ │ │ │ + void  _g_t_s_a_m_:_:_i_n_p_l_a_c_e___Q_R (Matrix &A) │ │ │ │ │ +  QR factorization using Eigen's internal │ │ │ │ │ + block QR algorithm. │ │ │ │ │ +  │ │ │ │ │ + list< boost::tuple< Vector, double, _g_t_s_a_m_:_:_w_e_i_g_h_t_e_d___e_l_i_m_i_n_a_t_e (Matrix &A, │ │ │ │ │ + double > >  Vector &b, const Vector &sigmas) │ │ │ │ │ + Imperative algorithm for in-place full │ │ │ │ │ +  elimination with weights and constraint │ │ │ │ │ + handling. │ │ │ │ │ +  │ │ │ │ │ + void  _g_t_s_a_m_:_:_h_o_u_s_e_h_o_l_d_e_r__ (Matrix &A, size_t k, │ │ │ │ │ + bool copy_vectors) │ │ │ │ │ + Imperative version of Householder QR │ │ │ │ │ +  factorization, Golub & Van Loan p 224 │ │ │ │ │ + version with Householder vectors below │ │ │ │ │ + diagonal, as in GVL. │ │ │ │ │ +  │ │ │ │ │ + void  _g_t_s_a_m_:_:_h_o_u_s_e_h_o_l_d_e_r (Matrix &A, size_t k) │ │ │ │ │ +  Householder tranformation, zeros below │ │ │ │ │ + diagonal. │ │ │ │ │ +  │ │ │ │ │ + Vector  _g_t_s_a_m_:_:_b_a_c_k_S_u_b_s_t_i_t_u_t_e_U_p_p_e_r (const Matrix │ │ │ │ │ + &U, const Vector &b, bool unit=false) │ │ │ │ │ +  backSubstitute U*x=b │ │ │ │ │ +  │ │ │ │ │ + Vector  _g_t_s_a_m_:_:_b_a_c_k_S_u_b_s_t_i_t_u_t_e_U_p_p_e_r (const Vector │ │ │ │ │ + &b, const Matrix &U, bool unit=false) │ │ │ │ │ +  backSubstitute x'*U=b' │ │ │ │ │ +  │ │ │ │ │ + Vector  _g_t_s_a_m_:_:_b_a_c_k_S_u_b_s_t_i_t_u_t_e_L_o_w_e_r (const Matrix │ │ │ │ │ + &L, const Vector &b, bool unit=false) │ │ │ │ │ +  backSubstitute L*x=b │ │ │ │ │ +  │ │ │ │ │ + Matrix  _g_t_s_a_m_:_:_s_t_a_c_k (size_t nrMatrices,...) │ │ │ │ │ + create a matrix by stacking other │ │ │ │ │ +  matrices Given a set of matrices: A1, A2, │ │ │ │ │ + A3... │ │ │ │ │ +  │ │ │ │ │ + Matrix  ggttssaamm::::ssttaacckk (const std::vector< Matrix > │ │ │ │ │ + &blocks) │ │ │ │ │ +  │ │ │ │ │ + Matrix  _g_t_s_a_m_:_:_c_o_l_l_e_c_t (const std::vector< const │ │ │ │ │ + Matrix * > &matrices, size_t m=0, size_t │ │ │ │ │ + n=0) │ │ │ │ │ + create a matrix by concatenating Given a │ │ │ │ │ + set of matrices: A1, A2, A3... If all │ │ │ │ │ +  matrices have the same size, specifying │ │ │ │ │ + single matrix dimensions will avoid the │ │ │ │ │ + lookup of dimensions │ │ │ │ │ +  │ │ │ │ │ + Matrix  ggttssaamm::::ccoolllleecctt (size_t nrMatrices,...) │ │ │ │ │ +  │ │ │ │ │ + void  _g_t_s_a_m_:_:_v_e_c_t_o_r___s_c_a_l_e___i_n_p_l_a_c_e (const Vector │ │ │ │ │ + &v, Matrix &A, bool inf_mask=false) │ │ │ │ │ + scales a matrix row or column by the │ │ │ │ │ +  values in a vector Arguments (Matrix, │ │ │ │ │ + Vector) scales the columns, (Vector, │ │ │ │ │ + Matrix) scales the rows │ │ │ │ │ +  │ │ │ │ │ + Matrix  ggttssaamm::::vveeccttoorr__ssccaallee (const Vector &v, │ │ │ │ │ + const Matrix &A, bool inf_mask) │ │ │ │ │ +  │ │ │ │ │ + Matrix  ggttssaamm::::vveeccttoorr__ssccaallee (const Matrix &A, │ │ │ │ │ + const Vector &v, bool inf_mask) │ │ │ │ │ +  │ │ │ │ │ + Matrix3  _g_t_s_a_m_:_:_s_k_e_w_S_y_m_m_e_t_r_i_c (double wx, double │ │ │ │ │ + wy, double wz) │ │ │ │ │ +  skew symmetric matrix returns this: 0 -wz │ │ │ │ │ + wy wz 0 -wx -wy wx 0 │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + Matrix3  ggttssaamm::::sskkeewwSSyymmmmeettrriicc (const Eigen:: │ │ │ │ │ + MatrixBase< Derived > &w) │ │ │ │ │ +  │ │ │ │ │ + Matrix  ggttssaamm::::iinnvveerrssee__ssqquuaarree__rroooott (const Matrix │ │ │ │ │ + &A) │ │ │ │ │ +  Use Cholesky to calculate inverse square │ │ │ │ │ + root of a matrix. │ │ │ │ │ +  │ │ │ │ │ + Matrix  _g_t_s_a_m_:_:_c_h_o_l_e_s_k_y___i_n_v_e_r_s_e (const Matrix &A) │ │ │ │ │ +  Return the inverse of a S.P.D. │ │ │ │ │ +  │ │ │ │ │ + void  _g_t_s_a_m_:_:_s_v_d (const Matrix &A, Matrix &U, │ │ │ │ │ + Vector &S, Matrix &V) │ │ │ │ │ +  SVD computes economy SVD A=U*S*V'. │ │ │ │ │ +  │ │ │ │ │ + boost::tuple< int, double, Vector >  _g_t_s_a_m_:_:_D_L_T (const Matrix &A, double │ │ │ │ │ + rank_tol=1e-9) │ │ │ │ │ + Direct linear transform algorithm that │ │ │ │ │ +  calls svd to find a vector v that │ │ │ │ │ + minimizes the algebraic error A*v. │ │ │ │ │ +  │ │ │ │ │ + Matrix  _g_t_s_a_m_:_:_e_x_p_m (const Matrix &A, size_t K=7) │ │ │ │ │ +  Numerical exponential map, naive │ │ │ │ │ + approach, not industrial strength !!! │ │ │ │ │ +  │ │ │ │ │ + std::string  ggttssaamm::::ffoorrmmaattMMaattrriixxIInnddeenntteedd (const std:: │ │ │ │ │ + string &label, const Matrix &matrix, bool │ │ │ │ │ + makeVectorHorizontal) │ │ │ │ │ +  │ │ │ │ │ + Matrix  ggttssaamm::::LLLLtt (const Matrix &A) │ │ │ │ │ +  │ │ │ │ │ + Matrix  ggttssaamm::::RRttRR (const Matrix &A) │ │ │ │ │ +  │ │ │ │ │ + Vector  ggttssaamm::::ccoolluummnnNNoorrmmSSqquuaarree (const Matrix &A) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Group product of two Lie Groups. │ │ │ │ │ - Date │ │ │ │ │ - May, 2015 │ │ │ │ │ +typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ Author │ │ │ │ │ + Christian Potthast │ │ │ │ │ + Kai Ni │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Alex Cunningham │ │ │ │ │ + Alex Hagiopol │ │ │ │ │ + Varun Agrawal │ │ │ │ │ +********** MMaaccrroo DDeeffiinniittiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ +********** _?◆_? GGTTSSAAMM__MMAAKKEE__MMAATTRRIIXX__DDEEFFSS ********** │ │ │ │ │ +#define GTSAM_MAKE_MATRIX_DEFS (   N ) │ │ │ │ │ +VVaalluuee:: │ │ │ │ │ +using Matrix##N = Eigen::Matrix; \ │ │ │ │ │ +using Matrix1##N = Eigen::Matrix; \ │ │ │ │ │ +using Matrix2##N = Eigen::Matrix; \ │ │ │ │ │ +using Matrix3##N = Eigen::Matrix; \ │ │ │ │ │ +using Matrix4##N = Eigen::Matrix; \ │ │ │ │ │ +using Matrix5##N = Eigen::Matrix; \ │ │ │ │ │ +using Matrix6##N = Eigen::Matrix; \ │ │ │ │ │ +using Matrix7##N = Eigen::Matrix; \ │ │ │ │ │ +using Matrix8##N = Eigen::Matrix; \ │ │ │ │ │ +using Matrix9##N = Eigen::Matrix; \ │ │ │ │ │ +static const Eigen::MatrixBase::IdentityReturnType I_##N##x##N = │ │ │ │ │ +Matrix##N::Identity(); \ │ │ │ │ │ +static const Eigen::MatrixBase::ConstantReturnType Z_##N##x##N = │ │ │ │ │ +Matrix##N::Zero(); │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p_._h │ │ │ │ │ + * _M_a_t_r_i_x_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00095.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,46 @@ │ │ │ │ │ var a00095 = [ │ │ │ │ │ - ["gtsam::ProductLieGroup< G, H >", "a02464.html", "a02464"], │ │ │ │ │ - ["gtsam::traits< ProductLieGroup< G, H > >", "a02468.html", null] │ │ │ │ │ + ["gtsam::Reshape< OutM, OutN, OutOptions, InM, InN, InOptions >", "a02416.html", null], │ │ │ │ │ + ["gtsam::Reshape< M, M, InOptions, M, M, InOptions >", "a02420.html", null], │ │ │ │ │ + ["gtsam::Reshape< M, N, InOptions, M, N, InOptions >", "a02424.html", null], │ │ │ │ │ + ["gtsam::Reshape< N, M, InOptions, M, N, InOptions >", "a02428.html", null], │ │ │ │ │ + ["gtsam::MultiplyWithInverse< N >", "a02432.html", "a02432"], │ │ │ │ │ + ["gtsam::MultiplyWithInverseFunction< T, N >", "a02436.html", "a02436"], │ │ │ │ │ + ["assert_equal", "a00095.html#a3f9622226dfe06908f11b42bf0bdd22d", null], │ │ │ │ │ + ["assert_equal", "a00095.html#abb0e19bbbeaca95843e8161b89a12fda", null], │ │ │ │ │ + ["assert_inequal", "a00095.html#a7a116d0643f123ef3b15d91056506492", null], │ │ │ │ │ + ["backSubstituteLower", "a00095.html#a0d998e1b770c9864946ddb031b1c4522", null], │ │ │ │ │ + ["backSubstituteUpper", "a00095.html#a6c4876cbe85d5651a52eda0e97c60f2f", null], │ │ │ │ │ + ["backSubstituteUpper", "a00095.html#a8b0bf332d52b333dab2b20d763c8925b", null], │ │ │ │ │ + ["cholesky_inverse", "a00095.html#aba542e2cd85f08b76f80a0871a4ea713", null], │ │ │ │ │ + ["collect", "a00095.html#ad8b524ba6c9aed0d21a020999c9b5d88", null], │ │ │ │ │ + ["column", "a00095.html#a559dba69e2854eb66e34222f60f55722", null], │ │ │ │ │ + ["diag", "a00095.html#af1c32907adce74cf9edd6ee5bba5a085", null], │ │ │ │ │ + ["DLT", "a00095.html#ab2c65f1a69009a306b6a5f9ef31dcee2", null], │ │ │ │ │ + ["equal_with_abs_tol", "a00095.html#af31e657258505b2e5148846ebbaa3195", null], │ │ │ │ │ + ["expm", "a00095.html#ab15462d8c16813d0a7a5b1f76a2f64b7", null], │ │ │ │ │ + ["householder", "a00095.html#a7baa2f3184a444adce108633c0265e0c", null], │ │ │ │ │ + ["householder_", "a00095.html#abdd1ab05e8ac90b340fbd8f3b322dc6d", null], │ │ │ │ │ + ["inplace_QR", "a00095.html#a7eec9339ab5a008a28ddfaa6b2c94611", null], │ │ │ │ │ + ["insertSub", "a00095.html#ad1088fa2d1494e6a4123a6bc3f5c2d7e", null], │ │ │ │ │ + ["inverse_square_root", "a00095.html#aff78dee59ac0250432081f39deb5f6d1", null], │ │ │ │ │ + ["linear_dependent", "a00095.html#a4b81794af72954abafbb726fc712f5db", null], │ │ │ │ │ + ["linear_independent", "a00095.html#ad8f0349471464c1fb515819d9503849a", null], │ │ │ │ │ + ["operator!=", "a00095.html#ab235abf7505b634be2165e0db58239dd", null], │ │ │ │ │ + ["operator==", "a00095.html#afd5b3cf3f54adcbdd6d9e7403f1a792f", null], │ │ │ │ │ + ["operator>>", "a00095.html#a7d85957bab2d18cf56ab9aaf95a106de", null], │ │ │ │ │ + ["operator^", "a00095.html#a2218e53a2b99c449e70aa5b7805895fc", null], │ │ │ │ │ + ["print", "a00095.html#a54fa43c89c5334314c8c75939dd5c2d7", null], │ │ │ │ │ + ["print", "a00095.html#a44ead03912d5dcf094d8421e1702ee71", null], │ │ │ │ │ + ["prod", "a00095.html#abc29e3164ed30e785a3c48dfd1aa6ca5", null], │ │ │ │ │ + ["qr", "a00095.html#ae32c295e2c40c1e85f146a8a6266eaa8", null], │ │ │ │ │ + ["row", "a00095.html#a2754f325c8600303d627d9e8cf1f9949", null], │ │ │ │ │ + ["save", "a00095.html#a3eb7eed9019d3fda8fcf74fbf85b85c9", null], │ │ │ │ │ + ["skewSymmetric", "a00095.html#a97f108d19e52c83c331c55d35b23796e", null], │ │ │ │ │ + ["stack", "a00095.html#a5c82884a356ddd09229a5283aed04df9", null], │ │ │ │ │ + ["sub", "a00095.html#a9ae1d9fe2ccad044fbb88b5c1d9e035a", null], │ │ │ │ │ + ["svd", "a00095.html#a8d7e46204d953f64a39445599dbd7eee", null], │ │ │ │ │ + ["trans", "a00095.html#aa2e36d7ab63000feddaeb61bbfcf2db1", null], │ │ │ │ │ + ["vector_scale_inplace", "a00095.html#a24d3ad0252f91f0ec301593c45cf5af7", null], │ │ │ │ │ + ["weighted_eliminate", "a00095.html#a60019a03f26b92c6b2a08e43d153d4c0", null], │ │ │ │ │ + ["zeroBelowDiagonal", "a00095.html#a57edf6ed7312f63d35f73233665c334d", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00095_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/ProductLieGroup.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Matrix.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,193 +98,397 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ProductLieGroup.h
│ │ │ │ +
Matrix.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │ -
10 * -------------------------------1------------------------------------------- */
│ │ │ │ +
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/base/Lie.h>
│ │ │ │ -
22#include <utility> // pair
│ │ │ │ -
23
│ │ │ │ -
24namespace gtsam {
│ │ │ │ -
25
│ │ │ │ -
28template<typename G, typename H>
│ │ │ │ -
│ │ │ │ -
29class ProductLieGroup: public std::pair<G, H> {
│ │ │ │ -
30 BOOST_CONCEPT_ASSERT((IsLieGroup<G>));
│ │ │ │ -
31 BOOST_CONCEPT_ASSERT((IsLieGroup<H>));
│ │ │ │ -
32 typedef std::pair<G, H> Base;
│ │ │ │ -
33
│ │ │ │ -
34protected:
│ │ │ │ -
35 enum {dimension1 = traits<G>::dimension};
│ │ │ │ -
36 enum {dimension2 = traits<H>::dimension};
│ │ │ │ -
37
│ │ │ │ -
38public:
│ │ │ │ -
40 ProductLieGroup():Base(traits<G>::Identity(),traits<H>::Identity()) {}
│ │ │ │ -
41
│ │ │ │ -
42 // Construct from two subgroup elements
│ │ │ │ -
43 ProductLieGroup(const G& g, const H& h):Base(g,h) {}
│ │ │ │ -
44
│ │ │ │ -
45 // Construct from base
│ │ │ │ -
46 ProductLieGroup(const Base& base):Base(base) {}
│ │ │ │ -
47
│ │ │ │ -
50 typedef multiplicative_group_tag group_flavor;
│ │ │ │ -
51 static ProductLieGroup Identity() {return ProductLieGroup();}
│ │ │ │ -
52
│ │ │ │ -
53 ProductLieGroup operator*(const ProductLieGroup& other) const {
│ │ │ │ -
54 return ProductLieGroup(traits<G>::Compose(this->first,other.first),
│ │ │ │ -
55 traits<H>::Compose(this->second,other.second));
│ │ │ │ -
56 }
│ │ │ │ -
57 ProductLieGroup inverse() const {
│ │ │ │ -
58 return ProductLieGroup(traits<G>::Inverse(this->first), traits<H>::Inverse(this->second));
│ │ │ │ -
59 }
│ │ │ │ -
60 ProductLieGroup compose(const ProductLieGroup& g) const {
│ │ │ │ -
61 return (*this) * g;
│ │ │ │ -
62 }
│ │ │ │ -
63 ProductLieGroup between(const ProductLieGroup& g) const {
│ │ │ │ -
64 return this->inverse() * g;
│ │ │ │ -
65 }
│ │ │ │ -
67
│ │ │ │ -
70 enum {dimension = dimension1 + dimension2};
│ │ │ │ -
71 inline static size_t Dim() {return dimension;}
│ │ │ │ -
72 inline size_t dim() const {return dimension;}
│ │ │ │ -
73
│ │ │ │ -
74 typedef Eigen::Matrix<double, dimension, 1> TangentVector;
│ │ │ │ -
75 typedef OptionalJacobian<dimension, dimension> ChartJacobian;
│ │ │ │ +
23// \callgraph
│ │ │ │ +
24
│ │ │ │ +
25#pragma once
│ │ │ │ +
26
│ │ │ │ +
27#include <gtsam/base/OptionalJacobian.h>
│ │ │ │ +
28#include <gtsam/base/Vector.h>
│ │ │ │ +
29#include <boost/tuple/tuple.hpp>
│ │ │ │ +
30
│ │ │ │ +
31#include <vector>
│ │ │ │ +
32
│ │ │ │ +
38namespace gtsam {
│ │ │ │ +
39
│ │ │ │ +
40typedef Eigen::MatrixXd Matrix;
│ │ │ │ +
41typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRowMajor;
│ │ │ │ +
42
│ │ │ │ +
43// Create handy typedefs and constants for square-size matrices
│ │ │ │ +
44// MatrixMN, MatrixN = MatrixNN, I_NxN, and Z_NxN, for M,N=1..9
│ │ │ │ +
45#define GTSAM_MAKE_MATRIX_DEFS(N) \
│ │ │ │ +
46using Matrix##N = Eigen::Matrix<double, N, N>; \
│ │ │ │ +
47using Matrix1##N = Eigen::Matrix<double, 1, N>; \
│ │ │ │ +
48using Matrix2##N = Eigen::Matrix<double, 2, N>; \
│ │ │ │ +
49using Matrix3##N = Eigen::Matrix<double, 3, N>; \
│ │ │ │ +
50using Matrix4##N = Eigen::Matrix<double, 4, N>; \
│ │ │ │ +
51using Matrix5##N = Eigen::Matrix<double, 5, N>; \
│ │ │ │ +
52using Matrix6##N = Eigen::Matrix<double, 6, N>; \
│ │ │ │ +
53using Matrix7##N = Eigen::Matrix<double, 7, N>; \
│ │ │ │ +
54using Matrix8##N = Eigen::Matrix<double, 8, N>; \
│ │ │ │ +
55using Matrix9##N = Eigen::Matrix<double, 9, N>; \
│ │ │ │ +
56static const Eigen::MatrixBase<Matrix##N>::IdentityReturnType I_##N##x##N = Matrix##N::Identity(); \
│ │ │ │ +
57static const Eigen::MatrixBase<Matrix##N>::ConstantReturnType Z_##N##x##N = Matrix##N::Zero();
│ │ │ │ +
58
│ │ │ │ +
59GTSAM_MAKE_MATRIX_DEFS(1)
│ │ │ │ +
60GTSAM_MAKE_MATRIX_DEFS(2)
│ │ │ │ +
61GTSAM_MAKE_MATRIX_DEFS(3)
│ │ │ │ +
62GTSAM_MAKE_MATRIX_DEFS(4)
│ │ │ │ +
63GTSAM_MAKE_MATRIX_DEFS(5)
│ │ │ │ +
64GTSAM_MAKE_MATRIX_DEFS(6)
│ │ │ │ +
65GTSAM_MAKE_MATRIX_DEFS(7)
│ │ │ │ +
66GTSAM_MAKE_MATRIX_DEFS(8)
│ │ │ │ +
67GTSAM_MAKE_MATRIX_DEFS(9)
│ │ │ │ +
68
│ │ │ │ +
69// Matrix expressions for accessing parts of matrices
│ │ │ │ +
70typedef Eigen::Block<Matrix> SubMatrix;
│ │ │ │ +
71typedef Eigen::Block<const Matrix> ConstSubMatrix;
│ │ │ │ +
72
│ │ │ │ +
73// Matrix formatting arguments when printing.
│ │ │ │ +
74// Akin to Matlab style.
│ │ │ │ +
75const Eigen::IOFormat& matlabFormat();
│ │ │ │
76
│ │ │ │ -
77 ProductLieGroup retract(const TangentVector& v, //
│ │ │ │ -
78 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const {
│ │ │ │ -
79 if (H1||H2) throw std::runtime_error("ProductLieGroup::retract derivatives not implemented yet");
│ │ │ │ -
80 G g = traits<G>::Retract(this->first, v.template head<dimension1>());
│ │ │ │ -
81 H h = traits<H>::Retract(this->second, v.template tail<dimension2>());
│ │ │ │ -
82 return ProductLieGroup(g,h);
│ │ │ │ -
83 }
│ │ │ │ -
84 TangentVector localCoordinates(const ProductLieGroup& g, //
│ │ │ │ -
85 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const {
│ │ │ │ -
86 if (H1||H2) throw std::runtime_error("ProductLieGroup::localCoordinates derivatives not implemented yet");
│ │ │ │ -
87 typename traits<G>::TangentVector v1 = traits<G>::Local(this->first, g.first);
│ │ │ │ -
88 typename traits<H>::TangentVector v2 = traits<H>::Local(this->second, g.second);
│ │ │ │ -
89 TangentVector v;
│ │ │ │ -
90 v << v1, v2;
│ │ │ │ -
91 return v;
│ │ │ │ -
92 }
│ │ │ │ -
94
│ │ │ │ -
97protected:
│ │ │ │ -
98 typedef Eigen::Matrix<double, dimension, dimension> Jacobian;
│ │ │ │ -
99 typedef Eigen::Matrix<double, dimension1, dimension1> Jacobian1;
│ │ │ │ -
100 typedef Eigen::Matrix<double, dimension2, dimension2> Jacobian2;
│ │ │ │ -
101
│ │ │ │ -
102public:
│ │ │ │ -
103 ProductLieGroup compose(const ProductLieGroup& other, ChartJacobian H1,
│ │ │ │ -
104 ChartJacobian H2 = boost::none) const {
│ │ │ │ -
105 Jacobian1 D_g_first; Jacobian2 D_h_second;
│ │ │ │ -
106 G g = traits<G>::Compose(this->first,other.first, H1 ? &D_g_first : 0);
│ │ │ │ -
107 H h = traits<H>::Compose(this->second,other.second, H1 ? &D_h_second : 0);
│ │ │ │ -
108 if (H1) {
│ │ │ │ -
109 H1->setZero();
│ │ │ │ -
110 H1->template topLeftCorner<dimension1,dimension1>() = D_g_first;
│ │ │ │ -
111 H1->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
│ │ │ │ -
112 }
│ │ │ │ -
113 if (H2) *H2 = Jacobian::Identity();
│ │ │ │ -
114 return ProductLieGroup(g,h);
│ │ │ │ -
115 }
│ │ │ │ -
116 ProductLieGroup between(const ProductLieGroup& other, ChartJacobian H1,
│ │ │ │ -
117 ChartJacobian H2 = boost::none) const {
│ │ │ │ -
118 Jacobian1 D_g_first; Jacobian2 D_h_second;
│ │ │ │ -
119 G g = traits<G>::Between(this->first,other.first, H1 ? &D_g_first : 0);
│ │ │ │ -
120 H h = traits<H>::Between(this->second,other.second, H1 ? &D_h_second : 0);
│ │ │ │ -
121 if (H1) {
│ │ │ │ -
122 H1->setZero();
│ │ │ │ -
123 H1->template topLeftCorner<dimension1,dimension1>() = D_g_first;
│ │ │ │ -
124 H1->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
│ │ │ │ -
125 }
│ │ │ │ -
126 if (H2) *H2 = Jacobian::Identity();
│ │ │ │ -
127 return ProductLieGroup(g,h);
│ │ │ │ -
128 }
│ │ │ │ -
129 ProductLieGroup inverse(ChartJacobian D) const {
│ │ │ │ -
130 Jacobian1 D_g_first; Jacobian2 D_h_second;
│ │ │ │ -
131 G g = traits<G>::Inverse(this->first, D ? &D_g_first : 0);
│ │ │ │ -
132 H h = traits<H>::Inverse(this->second, D ? &D_h_second : 0);
│ │ │ │ -
133 if (D) {
│ │ │ │ -
134 D->setZero();
│ │ │ │ -
135 D->template topLeftCorner<dimension1,dimension1>() = D_g_first;
│ │ │ │ -
136 D->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
│ │ │ │ -
137 }
│ │ │ │ -
138 return ProductLieGroup(g,h);
│ │ │ │ -
139 }
│ │ │ │ -
140 static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
│ │ │ │ -
141 Jacobian1 D_g_first; Jacobian2 D_h_second;
│ │ │ │ -
142 G g = traits<G>::Expmap(v.template head<dimension1>(), Hv ? &D_g_first : 0);
│ │ │ │ -
143 H h = traits<H>::Expmap(v.template tail<dimension2>(), Hv ? &D_h_second : 0);
│ │ │ │ -
144 if (Hv) {
│ │ │ │ -
145 Hv->setZero();
│ │ │ │ -
146 Hv->template topLeftCorner<dimension1,dimension1>() = D_g_first;
│ │ │ │ -
147 Hv->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
│ │ │ │ -
148 }
│ │ │ │ -
149 return ProductLieGroup(g,h);
│ │ │ │ -
150 }
│ │ │ │ -
151 static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = boost::none) {
│ │ │ │ -
152 Jacobian1 D_g_first; Jacobian2 D_h_second;
│ │ │ │ -
153 typename traits<G>::TangentVector v1 = traits<G>::Logmap(p.first, Hp ? &D_g_first : 0);
│ │ │ │ -
154 typename traits<H>::TangentVector v2 = traits<H>::Logmap(p.second, Hp ? &D_h_second : 0);
│ │ │ │ -
155 TangentVector v;
│ │ │ │ -
156 v << v1, v2;
│ │ │ │ -
157 if (Hp) {
│ │ │ │ -
158 Hp->setZero();
│ │ │ │ -
159 Hp->template topLeftCorner<dimension1,dimension1>() = D_g_first;
│ │ │ │ -
160 Hp->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
│ │ │ │ -
161 }
│ │ │ │ -
162 return v;
│ │ │ │ -
163 }
│ │ │ │ -
164 static TangentVector LocalCoordinates(const ProductLieGroup& p, ChartJacobian Hp = boost::none) {
│ │ │ │ -
165 return Logmap(p, Hp);
│ │ │ │ -
166 }
│ │ │ │ -
167 ProductLieGroup expmap(const TangentVector& v) const {
│ │ │ │ -
168 return compose(ProductLieGroup::Expmap(v));
│ │ │ │ -
169 }
│ │ │ │ -
170 TangentVector logmap(const ProductLieGroup& g) const {
│ │ │ │ -
171 return ProductLieGroup::Logmap(between(g));
│ │ │ │ -
172 }
│ │ │ │ -
174
│ │ │ │ -
175};
│ │ │ │ -
│ │ │ │ -
176
│ │ │ │ -
177// Define any direct product group to be a model of the multiplicative Group concept
│ │ │ │ -
178template<typename G, typename H>
│ │ │ │ -
179struct traits<ProductLieGroup<G, H> > : internal::LieGroupTraits<ProductLieGroup<G, H> > {};
│ │ │ │ -
180
│ │ │ │ -
181} // namespace gtsam
│ │ │ │ -
182
│ │ │ │ -
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ +
80template <class MATRIX>
│ │ │ │ +
│ │ │ │ +
81bool equal_with_abs_tol(const Eigen::DenseBase<MATRIX>& A, const Eigen::DenseBase<MATRIX>& B, double tol = 1e-9) {
│ │ │ │ +
82
│ │ │ │ +
83 const size_t n1 = A.cols(), m1 = A.rows();
│ │ │ │ +
84 const size_t n2 = B.cols(), m2 = B.rows();
│ │ │ │ +
85
│ │ │ │ +
86 if(m1!=m2 || n1!=n2) return false;
│ │ │ │ +
87
│ │ │ │ +
88 for(size_t i=0; i<m1; i++)
│ │ │ │ +
89 for(size_t j=0; j<n1; j++) {
│ │ │ │ +
90 if(!fpEqual(A(i,j), B(i,j), tol, false)) {
│ │ │ │ +
91 return false;
│ │ │ │ +
92 }
│ │ │ │ +
93 }
│ │ │ │ +
94 return true;
│ │ │ │ +
95}
│ │ │ │ +
│ │ │ │ +
96
│ │ │ │ +
│ │ │ │ +
100inline bool operator==(const Matrix& A, const Matrix& B) {
│ │ │ │ +
101 return equal_with_abs_tol(A,B,1e-9);
│ │ │ │ +
102}
│ │ │ │ +
│ │ │ │ +
103
│ │ │ │ +
│ │ │ │ +
107inline bool operator!=(const Matrix& A, const Matrix& B) {
│ │ │ │ +
108 return !(A==B);
│ │ │ │ +
109 }
│ │ │ │ +
│ │ │ │ +
110
│ │ │ │ +
114GTSAM_EXPORT bool assert_equal(const Matrix& A, const Matrix& B, double tol = 1e-9);
│ │ │ │ +
115
│ │ │ │ +
119GTSAM_EXPORT bool assert_inequal(const Matrix& A, const Matrix& B, double tol = 1e-9);
│ │ │ │ +
120
│ │ │ │ +
124GTSAM_EXPORT bool assert_equal(const std::list<Matrix>& As, const std::list<Matrix>& Bs, double tol = 1e-9);
│ │ │ │ +
125
│ │ │ │ +
129GTSAM_EXPORT bool linear_independent(const Matrix& A, const Matrix& B, double tol = 1e-9);
│ │ │ │ +
130
│ │ │ │ +
134GTSAM_EXPORT bool linear_dependent(const Matrix& A, const Matrix& B, double tol = 1e-9);
│ │ │ │ +
135
│ │ │ │ +
140GTSAM_EXPORT Vector operator^(const Matrix& A, const Vector & v);
│ │ │ │ +
141
│ │ │ │ +
143template<class MATRIX>
│ │ │ │ +
│ │ │ │ +
144inline MATRIX prod(const MATRIX& A, const MATRIX&B) {
│ │ │ │ +
145 MATRIX result = A * B;
│ │ │ │ +
146 return result;
│ │ │ │ +
147}
│ │ │ │ +
│ │ │ │ +
148
│ │ │ │ +
152GTSAM_EXPORT void print(const Matrix& A, const std::string& s, std::ostream& stream);
│ │ │ │ +
153
│ │ │ │ +
157GTSAM_EXPORT void print(const Matrix& A, const std::string& s = "");
│ │ │ │ +
158
│ │ │ │ +
162GTSAM_EXPORT void save(const Matrix& A, const std::string &s, const std::string& filename);
│ │ │ │ +
163
│ │ │ │ +
169GTSAM_EXPORT std::istream& operator>>(std::istream& inputStream, Matrix& destinationMatrix);
│ │ │ │ +
170
│ │ │ │ +
180template<class MATRIX>
│ │ │ │ +
│ │ │ │ +
181Eigen::Block<const MATRIX> sub(const MATRIX& A, size_t i1, size_t i2, size_t j1, size_t j2) {
│ │ │ │ +
182 size_t m=i2-i1, n=j2-j1;
│ │ │ │ +
183 return A.block(i1,j1,m,n);
│ │ │ │ +
184}
│ │ │ │ +
│ │ │ │ +
185
│ │ │ │ +
194template <typename Derived1, typename Derived2>
│ │ │ │ +
│ │ │ │ +
195void insertSub(Eigen::MatrixBase<Derived1>& fullMatrix, const Eigen::MatrixBase<Derived2>& subMatrix, size_t i, size_t j) {
│ │ │ │ +
196 fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix;
│ │ │ │ +
197}
│ │ │ │ +
│ │ │ │ +
198
│ │ │ │ +
202GTSAM_EXPORT Matrix diag(const std::vector<Matrix>& Hs);
│ │ │ │ +
203
│ │ │ │ +
210template<class MATRIX>
│ │ │ │ +
│ │ │ │ +
211const typename MATRIX::ConstColXpr column(const MATRIX& A, size_t j) {
│ │ │ │ +
212 return A.col(j);
│ │ │ │ +
213}
│ │ │ │ +
│ │ │ │ +
214
│ │ │ │ +
221template<class MATRIX>
│ │ │ │ +
│ │ │ │ +
222const typename MATRIX::ConstRowXpr row(const MATRIX& A, size_t j) {
│ │ │ │ +
223 return A.row(j);
│ │ │ │ +
224}
│ │ │ │ +
│ │ │ │ +
225
│ │ │ │ +
231template<class MATRIX>
│ │ │ │ +
│ │ │ │ +
232void zeroBelowDiagonal(MATRIX& A, size_t cols=0) {
│ │ │ │ +
233 const size_t m = A.rows(), n = A.cols();
│ │ │ │ +
234 const size_t k = (cols) ? std::min(cols, std::min(m,n)) : std::min(m,n);
│ │ │ │ +
235 for (size_t j=0; j<k; ++j)
│ │ │ │ +
236 A.col(j).segment(j+1, m-(j+1)).setZero();
│ │ │ │ +
237}
│ │ │ │ +
│ │ │ │ +
238
│ │ │ │ +
242inline Matrix trans(const Matrix& A) { return A.transpose(); }
│ │ │ │ +
243
│ │ │ │ +
245template <int OutM, int OutN, int OutOptions, int InM, int InN, int InOptions>
│ │ │ │ +
│ │ │ │ +
246struct Reshape {
│ │ │ │ +
247 //TODO replace this with Eigen's reshape function as soon as available. (There is a PR already pending : https://bitbucket.org/eigen/eigen/pull-request/41/reshape/diff)
│ │ │ │ +
248 typedef Eigen::Map<const Eigen::Matrix<double, OutM, OutN, OutOptions> > ReshapedType;
│ │ │ │ +
249 static inline ReshapedType reshape(const Eigen::Matrix<double, InM, InN, InOptions> & in) {
│ │ │ │ +
250 return in.data();
│ │ │ │ +
251 }
│ │ │ │ +
252};
│ │ │ │ +
│ │ │ │ +
253
│ │ │ │ +
255template <int M, int InOptions>
│ │ │ │ +
│ │ │ │ +
256struct Reshape<M, M, InOptions, M, M, InOptions> {
│ │ │ │ +
257 typedef const Eigen::Matrix<double, M, M, InOptions> & ReshapedType;
│ │ │ │ +
258 static inline ReshapedType reshape(const Eigen::Matrix<double, M, M, InOptions> & in) {
│ │ │ │ +
259 return in;
│ │ │ │ +
260 }
│ │ │ │ +
261};
│ │ │ │ +
│ │ │ │ +
262
│ │ │ │ +
264template <int M, int N, int InOptions>
│ │ │ │ +
│ │ │ │ +
265struct Reshape<M, N, InOptions, M, N, InOptions> {
│ │ │ │ +
266 typedef const Eigen::Matrix<double, M, N, InOptions> & ReshapedType;
│ │ │ │ +
267 static inline ReshapedType reshape(const Eigen::Matrix<double, M, N, InOptions> & in) {
│ │ │ │ +
268 return in;
│ │ │ │ +
269 }
│ │ │ │ +
270};
│ │ │ │ +
│ │ │ │ +
271
│ │ │ │ +
273template <int M, int N, int InOptions>
│ │ │ │ +
│ │ │ │ +
274struct Reshape<N, M, InOptions, M, N, InOptions> {
│ │ │ │ +
275 typedef typename Eigen::Matrix<double, M, N, InOptions>::ConstTransposeReturnType ReshapedType;
│ │ │ │ +
276 static inline ReshapedType reshape(const Eigen::Matrix<double, M, N, InOptions> & in) {
│ │ │ │ +
277 return in.transpose();
│ │ │ │ +
278 }
│ │ │ │ +
279};
│ │ │ │ +
│ │ │ │ +
280
│ │ │ │ +
281template <int OutM, int OutN, int OutOptions, int InM, int InN, int InOptions>
│ │ │ │ +
282inline typename Reshape<OutM, OutN, OutOptions, InM, InN, InOptions>::ReshapedType reshape(const Eigen::Matrix<double, InM, InN, InOptions> & m){
│ │ │ │ +
283 BOOST_STATIC_ASSERT(InM * InN == OutM * OutN);
│ │ │ │ +
284 return Reshape<OutM, OutN, OutOptions, InM, InN, InOptions>::reshape(m);
│ │ │ │ +
285}
│ │ │ │ +
286
│ │ │ │ +
293GTSAM_EXPORT std::pair<Matrix,Matrix> qr(const Matrix& A);
│ │ │ │ +
294
│ │ │ │ +
300GTSAM_EXPORT void inplace_QR(Matrix& A);
│ │ │ │ +
301
│ │ │ │ +
310GTSAM_EXPORT std::list<boost::tuple<Vector, double, double> >
│ │ │ │ +
311weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas);
│ │ │ │ +
312
│ │ │ │ +
320GTSAM_EXPORT void householder_(Matrix& A, size_t k, bool copy_vectors=true);
│ │ │ │ +
321
│ │ │ │ +
328GTSAM_EXPORT void householder(Matrix& A, size_t k);
│ │ │ │ +
329
│ │ │ │ +
337GTSAM_EXPORT Vector backSubstituteUpper(const Matrix& U, const Vector& b, bool unit=false);
│ │ │ │ +
338
│ │ │ │ +
346//TODO: is this function necessary? it isn't used
│ │ │ │ +
347GTSAM_EXPORT Vector backSubstituteUpper(const Vector& b, const Matrix& U, bool unit=false);
│ │ │ │ +
348
│ │ │ │ +
356GTSAM_EXPORT Vector backSubstituteLower(const Matrix& L, const Vector& b, bool unit=false);
│ │ │ │ +
357
│ │ │ │ +
364GTSAM_EXPORT Matrix stack(size_t nrMatrices, ...);
│ │ │ │ +
365GTSAM_EXPORT Matrix stack(const std::vector<Matrix>& blocks);
│ │ │ │ +
366
│ │ │ │ +
377GTSAM_EXPORT Matrix collect(const std::vector<const Matrix *>& matrices, size_t m = 0, size_t n = 0);
│ │ │ │ +
378GTSAM_EXPORT Matrix collect(size_t nrMatrices, ...);
│ │ │ │ +
379
│ │ │ │ +
386GTSAM_EXPORT void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask = false); // row
│ │ │ │ +
387GTSAM_EXPORT Matrix vector_scale(const Vector& v, const Matrix& A, bool inf_mask = false); // row
│ │ │ │ +
388GTSAM_EXPORT Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask = false); // column
│ │ │ │ +
389
│ │ │ │ +
│ │ │ │ +
401inline Matrix3 skewSymmetric(double wx, double wy, double wz) {
│ │ │ │ +
402 return (Matrix3() << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0).finished();
│ │ │ │ +
403}
│ │ │ │ +
│ │ │ │ +
404
│ │ │ │ +
405template <class Derived>
│ │ │ │ +
406inline Matrix3 skewSymmetric(const Eigen::MatrixBase<Derived>& w) {
│ │ │ │ +
407 return skewSymmetric(w(0), w(1), w(2));
│ │ │ │ +
408}
│ │ │ │ +
409
│ │ │ │ +
411GTSAM_EXPORT Matrix inverse_square_root(const Matrix& A);
│ │ │ │ +
412
│ │ │ │ +
414GTSAM_EXPORT Matrix cholesky_inverse(const Matrix &A);
│ │ │ │ +
415
│ │ │ │ +
428GTSAM_EXPORT void svd(const Matrix& A, Matrix& U, Vector& S, Matrix& V);
│ │ │ │ +
429
│ │ │ │ +
437GTSAM_EXPORT boost::tuple<int, double, Vector>
│ │ │ │ +
438DLT(const Matrix& A, double rank_tol = 1e-9);
│ │ │ │ +
439
│ │ │ │ +
445GTSAM_EXPORT Matrix expm(const Matrix& A, size_t K=7);
│ │ │ │ +
446
│ │ │ │ +
447std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal = false);
│ │ │ │ +
448
│ │ │ │ +
455template <int N>
│ │ │ │ +
│ │ │ │ +
456struct MultiplyWithInverse {
│ │ │ │ +
457 typedef Eigen::Matrix<double, N, 1> VectorN;
│ │ │ │ +
458 typedef Eigen::Matrix<double, N, N> MatrixN;
│ │ │ │ +
459
│ │ │ │ +
│ │ │ │ +
461 VectorN operator()(const MatrixN& A, const VectorN& b,
│ │ │ │ +
462 OptionalJacobian<N, N* N> H1 = boost::none,
│ │ │ │ +
463 OptionalJacobian<N, N> H2 = boost::none) const {
│ │ │ │ +
464 const MatrixN invA = A.inverse();
│ │ │ │ +
465 const VectorN c = invA * b;
│ │ │ │ +
466 // The derivative in A is just -[c[0]*invA c[1]*invA ... c[N-1]*invA]
│ │ │ │ +
467 if (H1)
│ │ │ │ +
468 for (size_t j = 0; j < N; j++)
│ │ │ │ +
469 H1->template middleCols<N>(N * j) = -c[j] * invA;
│ │ │ │ +
470 // The derivative in b is easy, as invA*b is just a linear map:
│ │ │ │ +
471 if (H2) *H2 = invA;
│ │ │ │ +
472 return c;
│ │ │ │ +
473 }
│ │ │ │ +
│ │ │ │ +
474};
│ │ │ │ +
│ │ │ │ +
475
│ │ │ │ +
481template <typename T, int N>
│ │ │ │ +
│ │ │ │ +
482struct MultiplyWithInverseFunction {
│ │ │ │ +
483 enum { M = traits<T>::dimension };
│ │ │ │ +
484 typedef Eigen::Matrix<double, N, 1> VectorN;
│ │ │ │ +
485 typedef Eigen::Matrix<double, N, N> MatrixN;
│ │ │ │ +
486
│ │ │ │ +
487 // The function phi should calculate f(a)*b, with derivatives in a and b.
│ │ │ │ +
488 // Naturally, the derivative in b is f(a).
│ │ │ │ +
489 typedef std::function<VectorN(
│ │ │ │ +
490 const T&, const VectorN&, OptionalJacobian<N, M>, OptionalJacobian<N, N>)>
│ │ │ │ +
491 Operator;
│ │ │ │ +
492
│ │ │ │ +
494 MultiplyWithInverseFunction(const Operator& phi) : phi_(phi) {}
│ │ │ │ +
495
│ │ │ │ +
│ │ │ │ +
497 VectorN operator()(const T& a, const VectorN& b,
│ │ │ │ +
498 OptionalJacobian<N, M> H1 = boost::none,
│ │ │ │ +
499 OptionalJacobian<N, N> H2 = boost::none) const {
│ │ │ │ +
500 MatrixN A;
│ │ │ │ +
501 phi_(a, b, boost::none, A); // get A = f(a) by calling f once
│ │ │ │ +
502 const MatrixN invA = A.inverse();
│ │ │ │ +
503 const VectorN c = invA * b;
│ │ │ │ +
504
│ │ │ │ +
505 if (H1) {
│ │ │ │ +
506 Eigen::Matrix<double, N, M> H;
│ │ │ │ +
507 phi_(a, c, H, boost::none); // get derivative H of forward mapping
│ │ │ │ +
508 *H1 = -invA* H;
│ │ │ │ +
509 }
│ │ │ │ +
510 if (H2) *H2 = invA;
│ │ │ │ +
511 return c;
│ │ │ │ +
512 }
│ │ │ │ +
│ │ │ │ +
513
│ │ │ │ +
514 private:
│ │ │ │ +
515 const Operator phi_;
│ │ │ │ +
516};
│ │ │ │ +
│ │ │ │ +
517
│ │ │ │ +
518GTSAM_EXPORT Matrix LLt(const Matrix& A);
│ │ │ │ +
519
│ │ │ │ +
520GTSAM_EXPORT Matrix RtR(const Matrix& A);
│ │ │ │ +
521
│ │ │ │ +
522GTSAM_EXPORT Vector columnNormSquare(const Matrix &A);
│ │ │ │ +
523} // namespace gtsam
│ │ │ │ +
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ +
OptionalJacobian.h
Special class for optional Jacobian arguments.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::backSubstituteLower
Vector backSubstituteLower(const Matrix &L, const Vector &b, bool unit)
backSubstitute L*x=b
Definition Matrix.cpp:367
│ │ │ │ +
gtsam::operator^
Vector operator^(const Matrix &A, const Vector &v)
overload ^ for trans(A)*v We transpose the vectors for speed.
Definition Matrix.cpp:131
│ │ │ │ +
gtsam::vector_scale_inplace
void vector_scale_inplace(const Vector &v, Matrix &A, bool inf_mask)
scales a matrix row or column by the values in a vector Arguments (Matrix, Vector) scales the columns...
Definition Matrix.cpp:482
│ │ │ │ +
gtsam::row
const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j)
Extracts a row view from a matrix that avoids a copy.
Definition Matrix.h:222
│ │ │ │ +
gtsam::expm
T expm(const Vector &x, int K=7)
Exponential map given exponential coordinates class T needs a wedge<> function and a constructor from...
Definition Lie.h:317
│ │ │ │ +
gtsam::save
void save(const Matrix &A, const string &s, const string &filename)
save a matrix to file, which can be loaded by matlab
Definition Matrix.cpp:167
│ │ │ │ +
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
equals with an tolerance, prints out message if unequal
Definition Matrix.cpp:43
│ │ │ │ +
gtsam::linear_dependent
bool linear_dependent(const Matrix &A, const Matrix &B, double tol)
check whether the rows of two matrices are linear dependent
Definition Matrix.cpp:117
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::column
const MATRIX::ConstColXpr column(const MATRIX &A, size_t j)
Extracts a column view from a matrix that avoids a copy.
Definition Matrix.h:211
│ │ │ │ +
gtsam::zeroBelowDiagonal
void zeroBelowDiagonal(MATRIX &A, size_t cols=0)
Zeros all of the elements below the diagonal of a matrix, in place.
Definition Matrix.h:232
│ │ │ │ +
gtsam::weighted_eliminate
list< boost::tuple< Vector, double, double > > weighted_eliminate(Matrix &A, Vector &b, const Vector &sigmas)
Imperative algorithm for in-place full elimination with weights and constraint handling.
Definition Matrix.cpp:273
│ │ │ │ +
gtsam::backSubstituteUpper
Vector backSubstituteUpper(const Matrix &U, const Vector &b, bool unit)
backSubstitute U*x=b
Definition Matrix.cpp:377
│ │ │ │ +
gtsam::assert_inequal
bool assert_inequal(const Matrix &A, const Matrix &B, double tol)
inequals with an tolerance, prints out message if within tolerance
Definition Matrix.cpp:63
│ │ │ │ +
gtsam::householder
void householder(Matrix &A, size_t k)
Householder tranformation, zeros below diagonal.
Definition Matrix.cpp:354
│ │ │ │ +
gtsam::operator>>
istream & operator>>(istream &inputStream, Matrix &destinationMatrix)
Read a matrix from an input stream, such as a file.
Definition Matrix.cpp:174
│ │ │ │ +
gtsam::inplace_QR
void inplace_QR(Matrix &A)
QR factorization using Eigen's internal block QR algorithm.
Definition Matrix.cpp:636
│ │ │ │ +
gtsam::svd
void svd(const Matrix &A, Matrix &U, Vector &S, Matrix &V)
SVD computes economy SVD A=U*S*V'.
Definition Matrix.cpp:560
│ │ │ │ +
gtsam::skewSymmetric
Matrix3 skewSymmetric(double wx, double wy, double wz)
skew symmetric matrix returns this: 0 -wz wy wz 0 -wx -wy wx 0
Definition Matrix.h:401
│ │ │ │ +
gtsam::sub
Eigen::Block< const MATRIX > sub(const MATRIX &A, size_t i1, size_t i2, size_t j1, size_t j2)
extract submatrix, slice semantics, i.e.
Definition Matrix.h:181
│ │ │ │ +
gtsam::trans
Matrix trans(const Matrix &A)
static transpose function, just calls Eigen transpose member function
Definition Matrix.h:242
│ │ │ │ +
gtsam::operator!=
bool operator!=(const Matrix &A, const Matrix &B)
inequality
Definition Matrix.h:107
│ │ │ │ +
gtsam::DLT
boost::tuple< int, double, Vector > DLT(const Matrix &A, double rank_tol)
Direct linear transform algorithm that calls svd to find a vector v that minimizes the algebraic erro...
Definition Matrix.cpp:568
│ │ │ │ +
gtsam::cholesky_inverse
Matrix cholesky_inverse(const Matrix &A)
Return the inverse of a S.P.D.
Definition Matrix.cpp:539
│ │ │ │ +
gtsam::prod
MATRIX prod(const MATRIX &A, const MATRIX &B)
products using old-style format to improve compatibility
Definition Matrix.h:144
│ │ │ │ +
gtsam::householder_
void householder_(Matrix &A, size_t k, bool copy_vectors)
Imperative version of Householder QR factorization, Golub & Van Loan p 224 version with Householder v...
Definition Matrix.cpp:327
│ │ │ │ +
gtsam::insertSub
void insertSub(Eigen::MatrixBase< Derived1 > &fullMatrix, const Eigen::MatrixBase< Derived2 > &subMatrix, size_t i, size_t j)
insert a submatrix IN PLACE at a specified location in a larger matrix NOTE: there is no size checkin...
Definition Matrix.h:195
│ │ │ │ +
gtsam::collect
Matrix collect(const std::vector< const Matrix * > &matrices, size_t m, size_t n)
create a matrix by concatenating Given a set of matrices: A1, A2, A3... If all matrices have the same...
Definition Matrix.cpp:443
│ │ │ │ +
gtsam::linear_independent
bool linear_independent(const Matrix &A, const Matrix &B, double tol)
check whether the rows of two matrices are linear independent
Definition Matrix.cpp:103
│ │ │ │ +
gtsam::fpEqual
bool fpEqual(double a, double b, double tol, bool check_relative_also)
Ensure we are not including a different version of Eigen in user code than while compiling gtsam,...
Definition Vector.cpp:42
│ │ │ │ +
gtsam::qr
pair< Matrix, Matrix > qr(const Matrix &A)
Householder QR factorization, Golub & Van Loan p 224, explicit version
Definition Matrix.cpp:235
│ │ │ │ +
gtsam::diag
Matrix diag(const std::vector< Matrix > &Hs)
Create a matrix with submatrices along its diagonal.
Definition Matrix.cpp:207
│ │ │ │ +
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition Matrix.h:81
│ │ │ │ +
gtsam::operator==
bool operator==(const Matrix &A, const Matrix &B)
equality is just equal_with_abs_tol 1e-9
Definition Matrix.h:100
│ │ │ │ +
gtsam::inverse_square_root
Matrix inverse_square_root(const Matrix &A)
Use Cholesky to calculate inverse square root of a matrix.
Definition Matrix.cpp:552
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::internal::LieGroupTraits
A helper class that implements the traits interface for GTSAM lie groups.
Definition Lie.h:174
│ │ │ │ -
gtsam::IsLieGroup
Lie Group Concept.
Definition Lie.h:260
│ │ │ │ -
gtsam::ProductLieGroup
Template to construct the product Lie group of two other Lie groups Assumes Lie group structure for G...
Definition ProductLieGroup.h:29
│ │ │ │ -
gtsam::ProductLieGroup::ProductLieGroup
ProductLieGroup()
Default constructor yields identity.
Definition ProductLieGroup.h:40
│ │ │ │ +
gtsam::Reshape
Reshape functor.
Definition Matrix.h:246
│ │ │ │ +
gtsam::MultiplyWithInverse
Functor that implements multiplication of a vector b with the inverse of a matrix A.
Definition Matrix.h:456
│ │ │ │ +
gtsam::MultiplyWithInverse::operator()
VectorN operator()(const MatrixN &A, const VectorN &b, OptionalJacobian< N, N *N > H1=boost::none, OptionalJacobian< N, N > H2=boost::none) const
A.inverse() * b, with optional derivatives.
Definition Matrix.h:461
│ │ │ │ +
gtsam::MultiplyWithInverseFunction
Functor that implements multiplication with the inverse of a matrix, itself the result of a function ...
Definition Matrix.h:482
│ │ │ │ +
gtsam::MultiplyWithInverseFunction::operator()
VectorN operator()(const T &a, const VectorN &b, OptionalJacobian< N, M > H1=boost::none, OptionalJacobian< N, N > H2=boost::none) const
f(a).inverse() * b, with optional derivatives
Definition Matrix.h:497
│ │ │ │ +
gtsam::MultiplyWithInverseFunction::MultiplyWithInverseFunction
MultiplyWithInverseFunction(const Operator &phi)
Construct with function as explained above.
Definition Matrix.h:494
│ │ │ │ +
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,213 +1,534 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ProductLieGroup.h │ │ │ │ │ +Matrix.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ -10 * -------------------------------1------------------------------------------ │ │ │ │ │ +10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_b_a_s_e_/_L_i_e_._h> │ │ │ │ │ -22#include // pair │ │ │ │ │ -23 │ │ │ │ │ -24namespace _g_t_s_a_m { │ │ │ │ │ -25 │ │ │ │ │ -28template │ │ │ │ │ -_2_9class _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p: public std::pair { │ │ │ │ │ -30 BOOST_CONCEPT_ASSERT((_I_s_L_i_e_G_r_o_u_p_<_G_>)); │ │ │ │ │ -31 BOOST_CONCEPT_ASSERT((_I_s_L_i_e_G_r_o_u_p_<_H_>)); │ │ │ │ │ -32 typedef std::pair Base; │ │ │ │ │ -33 │ │ │ │ │ -34protected: │ │ │ │ │ -35 enum {dimension1 = _t_r_a_i_t_s_<_G_>_:_:_d_i_m_e_n_s_i_o_n}; │ │ │ │ │ -36 enum {dimension2 = _t_r_a_i_t_s_<_H_>_:_:_d_i_m_e_n_s_i_o_n}; │ │ │ │ │ -37 │ │ │ │ │ -38public: │ │ │ │ │ -_4_0 _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p():Base(_t_r_a_i_t_s::Identity(),_t_r_a_i_t_s::Identity()) {} │ │ │ │ │ -41 │ │ │ │ │ -42 // Construct from two subgroup elements │ │ │ │ │ -43 _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p(const G& g, const H& h):Base(g,h) {} │ │ │ │ │ -44 │ │ │ │ │ -45 // Construct from base │ │ │ │ │ -46 _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p(const Base& base):Base(base) {} │ │ │ │ │ -47 │ │ │ │ │ -50 typedef multiplicative_group_tag group_flavor; │ │ │ │ │ -51 static _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p Identity() {return _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p();} │ │ │ │ │ -52 │ │ │ │ │ -53 _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p operator*(const _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p& other) const { │ │ │ │ │ -54 return _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p(traits::Compose(this->first,other.first), │ │ │ │ │ -55 traits::Compose(this->second,other.second)); │ │ │ │ │ -56 } │ │ │ │ │ -57 _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p inverse() const { │ │ │ │ │ -58 return _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p(traits::Inverse(this->first), traits::Inverse │ │ │ │ │ -(this->second)); │ │ │ │ │ -59 } │ │ │ │ │ -60 _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p compose(const _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p& g) const { │ │ │ │ │ -61 return (*this) * g; │ │ │ │ │ -62 } │ │ │ │ │ -63 _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p between(const _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p& g) const { │ │ │ │ │ -64 return this->inverse() * g; │ │ │ │ │ -65 } │ │ │ │ │ -67 │ │ │ │ │ -70 enum {dimension = dimension1 + dimension2}; │ │ │ │ │ -71 inline static size_t Dim() {return dimension;} │ │ │ │ │ -72 inline size_t dim() const {return dimension;} │ │ │ │ │ -73 │ │ │ │ │ -74 typedef Eigen::Matrix TangentVector; │ │ │ │ │ -75 typedef OptionalJacobian ChartJacobian; │ │ │ │ │ +23// \callgraph │ │ │ │ │ +24 │ │ │ │ │ +25#pragma once │ │ │ │ │ +26 │ │ │ │ │ +27#include <_g_t_s_a_m_/_b_a_s_e_/_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h> │ │ │ │ │ +28#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_c_t_o_r_._h> │ │ │ │ │ +29#include │ │ │ │ │ +30 │ │ │ │ │ +31#include │ │ │ │ │ +32 │ │ │ │ │ +38namespace _g_t_s_a_m { │ │ │ │ │ +39 │ │ │ │ │ +40typedef Eigen::MatrixXd Matrix; │ │ │ │ │ +41typedef Eigen::Matrix MatrixRowMajor; │ │ │ │ │ +42 │ │ │ │ │ +43// Create handy typedefs and constants for square-size matrices │ │ │ │ │ +44// MatrixMN, MatrixN = MatrixNN, I_NxN, and Z_NxN, for M,N=1..9 │ │ │ │ │ +45#define GTSAM_MAKE_MATRIX_DEFS(N) \ │ │ │ │ │ +46using Matrix##N = Eigen::Matrix; \ │ │ │ │ │ +47using Matrix1##N = Eigen::Matrix; \ │ │ │ │ │ +48using Matrix2##N = Eigen::Matrix; \ │ │ │ │ │ +49using Matrix3##N = Eigen::Matrix; \ │ │ │ │ │ +50using Matrix4##N = Eigen::Matrix; \ │ │ │ │ │ +51using Matrix5##N = Eigen::Matrix; \ │ │ │ │ │ +52using Matrix6##N = Eigen::Matrix; \ │ │ │ │ │ +53using Matrix7##N = Eigen::Matrix; \ │ │ │ │ │ +54using Matrix8##N = Eigen::Matrix; \ │ │ │ │ │ +55using Matrix9##N = Eigen::Matrix; \ │ │ │ │ │ +56static const Eigen::MatrixBase::IdentityReturnType I_##N##x##N = │ │ │ │ │ +Matrix##N::Identity(); \ │ │ │ │ │ +57static const Eigen::MatrixBase::ConstantReturnType Z_##N##x##N = │ │ │ │ │ +Matrix##N::Zero(); │ │ │ │ │ +58 │ │ │ │ │ +59GTSAM_MAKE_MATRIX_DEFS(1) │ │ │ │ │ +60GTSAM_MAKE_MATRIX_DEFS(2) │ │ │ │ │ +61GTSAM_MAKE_MATRIX_DEFS(3) │ │ │ │ │ +62GTSAM_MAKE_MATRIX_DEFS(4) │ │ │ │ │ +63GTSAM_MAKE_MATRIX_DEFS(5) │ │ │ │ │ +64GTSAM_MAKE_MATRIX_DEFS(6) │ │ │ │ │ +65GTSAM_MAKE_MATRIX_DEFS(7) │ │ │ │ │ +66GTSAM_MAKE_MATRIX_DEFS(8) │ │ │ │ │ +67GTSAM_MAKE_MATRIX_DEFS(9) │ │ │ │ │ +68 │ │ │ │ │ +69// Matrix expressions for accessing parts of matrices │ │ │ │ │ +70typedef Eigen::Block SubMatrix; │ │ │ │ │ +71typedef Eigen::Block ConstSubMatrix; │ │ │ │ │ +72 │ │ │ │ │ +73// Matrix formatting arguments when printing. │ │ │ │ │ +74// Akin to Matlab style. │ │ │ │ │ +75const Eigen::IOFormat& matlabFormat(); │ │ │ │ │ 76 │ │ │ │ │ -77 _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p retract(const TangentVector& v, // │ │ │ │ │ -78 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const { │ │ │ │ │ -79 if (H1||H2) throw std::runtime_error("ProductLieGroup::retract derivatives │ │ │ │ │ -not implemented yet"); │ │ │ │ │ -80 G g = traits::Retract(this->first, v.template head()); │ │ │ │ │ -81 H h = traits::Retract(this->second, v.template tail()); │ │ │ │ │ -82 return _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p(g,h); │ │ │ │ │ -83 } │ │ │ │ │ -84 TangentVector localCoordinates(const _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p& g, // │ │ │ │ │ -85 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const { │ │ │ │ │ -86 if (H1||H2) throw std::runtime_error("ProductLieGroup::localCoordinates │ │ │ │ │ -derivatives not implemented yet"); │ │ │ │ │ -87 typename traits::TangentVector v1 = traits::Local(this->first, │ │ │ │ │ -g.first); │ │ │ │ │ -88 typename traits::TangentVector v2 = traits::Local(this->second, │ │ │ │ │ -g.second); │ │ │ │ │ -89 TangentVector v; │ │ │ │ │ -90 v << v1, v2; │ │ │ │ │ -91 return v; │ │ │ │ │ +80template │ │ │ │ │ +_8_1bool _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(const Eigen::DenseBase& A, const Eigen:: │ │ │ │ │ +DenseBase& B, double tol = 1e-9) { │ │ │ │ │ +82 │ │ │ │ │ +83 const size_t n1 = A.cols(), m1 = A.rows(); │ │ │ │ │ +84 const size_t n2 = B.cols(), m2 = B.rows(); │ │ │ │ │ +85 │ │ │ │ │ +86 if(m1!=m2 || n1!=n2) return false; │ │ │ │ │ +87 │ │ │ │ │ +88 for(size_t i=0; i Jacobian; │ │ │ │ │ -99 typedef Eigen::Matrix Jacobian1; │ │ │ │ │ -100 typedef Eigen::Matrix Jacobian2; │ │ │ │ │ -101 │ │ │ │ │ -102public: │ │ │ │ │ -103 _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p compose(const _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p& other, ChartJacobian H1, │ │ │ │ │ -104 ChartJacobian H2 = boost::none) const { │ │ │ │ │ -105 Jacobian1 D_g_first; Jacobian2 D_h_second; │ │ │ │ │ -106 G g = traits::Compose(this->first,other.first, H1 ? &D_g_first : 0); │ │ │ │ │ -107 H h = traits::Compose(this->second,other.second, H1 ? &D_h_second : 0); │ │ │ │ │ -108 if (H1) { │ │ │ │ │ -109 H1->setZero(); │ │ │ │ │ -110 H1->template topLeftCorner() = D_g_first; │ │ │ │ │ -111 H1->template bottomRightCorner() = D_h_second; │ │ │ │ │ -112 } │ │ │ │ │ -113 if (H2) *H2 = Jacobian::Identity(); │ │ │ │ │ -114 return _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p(g,h); │ │ │ │ │ -115 } │ │ │ │ │ -116 _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p between(const _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p& other, ChartJacobian H1, │ │ │ │ │ -117 ChartJacobian H2 = boost::none) const { │ │ │ │ │ -118 Jacobian1 D_g_first; Jacobian2 D_h_second; │ │ │ │ │ -119 G g = traits::Between(this->first,other.first, H1 ? &D_g_first : 0); │ │ │ │ │ -120 H h = traits::Between(this->second,other.second, H1 ? &D_h_second : 0); │ │ │ │ │ -121 if (H1) { │ │ │ │ │ -122 H1->setZero(); │ │ │ │ │ -123 H1->template topLeftCorner() = D_g_first; │ │ │ │ │ -124 H1->template bottomRightCorner() = D_h_second; │ │ │ │ │ -125 } │ │ │ │ │ -126 if (H2) *H2 = Jacobian::Identity(); │ │ │ │ │ -127 return _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p(g,h); │ │ │ │ │ -128 } │ │ │ │ │ -129 _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p inverse(ChartJacobian D) const { │ │ │ │ │ -130 Jacobian1 D_g_first; Jacobian2 D_h_second; │ │ │ │ │ -131 G g = traits::Inverse(this->first, D ? &D_g_first : 0); │ │ │ │ │ -132 H h = traits::Inverse(this->second, D ? &D_h_second : 0); │ │ │ │ │ -133 if (D) { │ │ │ │ │ -134 D->setZero(); │ │ │ │ │ -135 D->template topLeftCorner() = D_g_first; │ │ │ │ │ -136 D->template bottomRightCorner() = D_h_second; │ │ │ │ │ -137 } │ │ │ │ │ -138 return _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p(g,h); │ │ │ │ │ -139 } │ │ │ │ │ -140 static _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p Expmap(const TangentVector& v, ChartJacobian Hv = │ │ │ │ │ -boost::none) { │ │ │ │ │ -141 Jacobian1 D_g_first; Jacobian2 D_h_second; │ │ │ │ │ -142 G g = traits::Expmap(v.template head(), Hv ? &D_g_first : │ │ │ │ │ -0); │ │ │ │ │ -143 H h = traits::Expmap(v.template tail(), Hv ? &D_h_second : │ │ │ │ │ -0); │ │ │ │ │ -144 if (Hv) { │ │ │ │ │ -145 Hv->setZero(); │ │ │ │ │ -146 Hv->template topLeftCorner() = D_g_first; │ │ │ │ │ -147 Hv->template bottomRightCorner() = D_h_second; │ │ │ │ │ -148 } │ │ │ │ │ -149 return _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p(g,h); │ │ │ │ │ -150 } │ │ │ │ │ -151 static TangentVector Logmap(const _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p& p, ChartJacobian Hp = │ │ │ │ │ -boost::none) { │ │ │ │ │ -152 Jacobian1 D_g_first; Jacobian2 D_h_second; │ │ │ │ │ -153 typename traits::TangentVector v1 = traits::Logmap(p.first, Hp ? │ │ │ │ │ -&D_g_first : 0); │ │ │ │ │ -154 typename traits::TangentVector v2 = traits::Logmap(p.second, Hp ? │ │ │ │ │ -&D_h_second : 0); │ │ │ │ │ -155 TangentVector v; │ │ │ │ │ -156 v << v1, v2; │ │ │ │ │ -157 if (Hp) { │ │ │ │ │ -158 Hp->setZero(); │ │ │ │ │ -159 Hp->template topLeftCorner() = D_g_first; │ │ │ │ │ -160 Hp->template bottomRightCorner() = D_h_second; │ │ │ │ │ -161 } │ │ │ │ │ -162 return v; │ │ │ │ │ -163 } │ │ │ │ │ -164 static TangentVector LocalCoordinates(const _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p& p, │ │ │ │ │ -ChartJacobian Hp = boost::none) { │ │ │ │ │ -165 return Logmap(p, Hp); │ │ │ │ │ -166 } │ │ │ │ │ -167 _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p expmap(const TangentVector& v) const { │ │ │ │ │ -168 return compose(ProductLieGroup::Expmap(v)); │ │ │ │ │ -169 } │ │ │ │ │ -170 TangentVector logmap(const _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p& g) const { │ │ │ │ │ -171 return ProductLieGroup::Logmap(between(g)); │ │ │ │ │ -172 } │ │ │ │ │ -174 │ │ │ │ │ -175}; │ │ │ │ │ -176 │ │ │ │ │ -177// Define any direct product group to be a model of the multiplicative Group │ │ │ │ │ -concept │ │ │ │ │ -178template │ │ │ │ │ -_1_7_9struct _t_r_a_i_t_s<_P_r_o_d_u_c_t_L_i_e_G_r_o_u_p > : _i_n_t_e_r_n_a_l_:_: │ │ │ │ │ -_L_i_e_G_r_o_u_p_T_r_a_i_t_s > {}; │ │ │ │ │ -180 │ │ │ │ │ -181} // namespace gtsam │ │ │ │ │ -182 │ │ │ │ │ -_L_i_e_._h │ │ │ │ │ -Base class and basic functions for Lie types. │ │ │ │ │ +93 } │ │ │ │ │ +94 return true; │ │ │ │ │ +95} │ │ │ │ │ +96 │ │ │ │ │ +_1_0_0inline bool _o_p_e_r_a_t_o_r_=_=(const Matrix& A, const Matrix& B) { │ │ │ │ │ +101 return _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(A,B,1e-9); │ │ │ │ │ +102} │ │ │ │ │ +103 │ │ │ │ │ +_1_0_7inline bool _o_p_e_r_a_t_o_r_!_=(const Matrix& A, const Matrix& B) { │ │ │ │ │ +108 return !(A==B); │ │ │ │ │ +109 } │ │ │ │ │ +110 │ │ │ │ │ +114GTSAM_EXPORT bool _a_s_s_e_r_t___e_q_u_a_l(const Matrix& A, const Matrix& B, double tol │ │ │ │ │ += 1e-9); │ │ │ │ │ +115 │ │ │ │ │ +119GTSAM_EXPORT bool _a_s_s_e_r_t___i_n_e_q_u_a_l(const Matrix& A, const Matrix& B, double │ │ │ │ │ +tol = 1e-9); │ │ │ │ │ +120 │ │ │ │ │ +124GTSAM_EXPORT bool _a_s_s_e_r_t___e_q_u_a_l(const std::list& As, const std:: │ │ │ │ │ +list& Bs, double tol = 1e-9); │ │ │ │ │ +125 │ │ │ │ │ +129GTSAM_EXPORT bool _l_i_n_e_a_r___i_n_d_e_p_e_n_d_e_n_t(const Matrix& A, const Matrix& B, │ │ │ │ │ +double tol = 1e-9); │ │ │ │ │ +130 │ │ │ │ │ +134GTSAM_EXPORT bool _l_i_n_e_a_r___d_e_p_e_n_d_e_n_t(const Matrix& A, const Matrix& B, double │ │ │ │ │ +tol = 1e-9); │ │ │ │ │ +135 │ │ │ │ │ +140GTSAM_EXPORT Vector _o_p_e_r_a_t_o_r_^(const Matrix& A, const Vector & v); │ │ │ │ │ +141 │ │ │ │ │ +143template │ │ │ │ │ +_1_4_4inline MATRIX _p_r_o_d(const MATRIX& A, const MATRIX&B) { │ │ │ │ │ +145 MATRIX result = A * B; │ │ │ │ │ +146 return result; │ │ │ │ │ +147} │ │ │ │ │ +148 │ │ │ │ │ +152GTSAM_EXPORT void _p_r_i_n_t(const Matrix& A, const std::string& s, std::ostream& │ │ │ │ │ +stream); │ │ │ │ │ +153 │ │ │ │ │ +157GTSAM_EXPORT void _p_r_i_n_t(const Matrix& A, const std::string& s = ""); │ │ │ │ │ +158 │ │ │ │ │ +162GTSAM_EXPORT void _s_a_v_e(const Matrix& A, const std::string &s, const std:: │ │ │ │ │ +string& filename); │ │ │ │ │ +163 │ │ │ │ │ +169GTSAM_EXPORT std::istream& _o_p_e_r_a_t_o_r_>_>(std::istream& inputStream, Matrix& │ │ │ │ │ +destinationMatrix); │ │ │ │ │ +170 │ │ │ │ │ +180template │ │ │ │ │ +_1_8_1Eigen::Block _s_u_b(const MATRIX& A, size_t i1, size_t i2, size_t │ │ │ │ │ +j1, size_t j2) { │ │ │ │ │ +182 size_t m=i2-i1, n=j2-j1; │ │ │ │ │ +183 return A.block(i1,j1,m,n); │ │ │ │ │ +184} │ │ │ │ │ +185 │ │ │ │ │ +194template │ │ │ │ │ +_1_9_5void _i_n_s_e_r_t_S_u_b(Eigen::MatrixBase& fullMatrix, const Eigen:: │ │ │ │ │ +MatrixBase& subMatrix, size_t i, size_t j) { │ │ │ │ │ +196 fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix; │ │ │ │ │ +197} │ │ │ │ │ +198 │ │ │ │ │ +202GTSAM_EXPORT Matrix _d_i_a_g(const std::vector& Hs); │ │ │ │ │ +203 │ │ │ │ │ +210template │ │ │ │ │ +_2_1_1const typename MATRIX::ConstColXpr _c_o_l_u_m_n(const MATRIX& A, size_t j) { │ │ │ │ │ +212 return A.col(j); │ │ │ │ │ +213} │ │ │ │ │ +214 │ │ │ │ │ +221template │ │ │ │ │ +_2_2_2const typename MATRIX::ConstRowXpr _r_o_w(const MATRIX& A, size_t j) { │ │ │ │ │ +223 return A.row(j); │ │ │ │ │ +224} │ │ │ │ │ +225 │ │ │ │ │ +231template │ │ │ │ │ +_2_3_2void _z_e_r_o_B_e_l_o_w_D_i_a_g_o_n_a_l(MATRIX& A, size_t cols=0) { │ │ │ │ │ +233 const size_t m = A.rows(), n = A.cols(); │ │ │ │ │ +234 const size_t k = (cols) ? std::min(cols, std::min(m,n)) : std::min(m,n); │ │ │ │ │ +235 for (size_t j=0; j │ │ │ │ │ +_2_4_6struct _R_e_s_h_a_p_e { │ │ │ │ │ +247 //TODO replace this with Eigen's reshape function as soon as available. │ │ │ │ │ +(There is a PR already pending : https://bitbucket.org/eigen/eigen/pull- │ │ │ │ │ +request/41/reshape/diff) │ │ │ │ │ +248 typedef Eigen::Map > │ │ │ │ │ +ReshapedType; │ │ │ │ │ +249 static inline ReshapedType reshape(const Eigen::Matrix & in) { │ │ │ │ │ +250 return in.data(); │ │ │ │ │ +251 } │ │ │ │ │ +252}; │ │ │ │ │ +253 │ │ │ │ │ +255template │ │ │ │ │ +_2_5_6struct _R_e_s_h_a_p_e { │ │ │ │ │ +257 typedef const Eigen::Matrix & ReshapedType; │ │ │ │ │ +258 static inline ReshapedType reshape(const Eigen::Matrix & in) { │ │ │ │ │ +259 return in; │ │ │ │ │ +260 } │ │ │ │ │ +261}; │ │ │ │ │ +262 │ │ │ │ │ +264template │ │ │ │ │ +_2_6_5struct _R_e_s_h_a_p_e { │ │ │ │ │ +266 typedef const Eigen::Matrix & ReshapedType; │ │ │ │ │ +267 static inline ReshapedType reshape(const Eigen::Matrix & in) { │ │ │ │ │ +268 return in; │ │ │ │ │ +269 } │ │ │ │ │ +270}; │ │ │ │ │ +271 │ │ │ │ │ +273template │ │ │ │ │ +_2_7_4struct _R_e_s_h_a_p_e { │ │ │ │ │ +275 typedef typename Eigen::Matrix:: │ │ │ │ │ +ConstTransposeReturnType ReshapedType; │ │ │ │ │ +276 static inline ReshapedType reshape(const Eigen::Matrix & in) { │ │ │ │ │ +277 return in.transpose(); │ │ │ │ │ +278 } │ │ │ │ │ +279}; │ │ │ │ │ +280 │ │ │ │ │ +281template │ │ │ │ │ +282inline typename Reshape:: │ │ │ │ │ +ReshapedType reshape(const Eigen::Matrix & m){ │ │ │ │ │ +283 BOOST_STATIC_ASSERT(InM * InN == OutM * OutN); │ │ │ │ │ +284 return _R_e_s_h_a_p_e_<_O_u_t_M_,_ _O_u_t_N_,_ _O_u_t_O_p_t_i_o_n_s_,_ _I_n_M_,_ _I_n_N_,_ _I_n_O_p_t_i_o_n_s_>_:_:_r_e_s_h_a_p_e(m); │ │ │ │ │ +285} │ │ │ │ │ +286 │ │ │ │ │ +293GTSAM_EXPORT std::pair _q_r(const Matrix& A); │ │ │ │ │ +294 │ │ │ │ │ +300GTSAM_EXPORT void _i_n_p_l_a_c_e___Q_R(Matrix& A); │ │ │ │ │ +301 │ │ │ │ │ +310GTSAM_EXPORT std::list > │ │ │ │ │ +311_w_e_i_g_h_t_e_d___e_l_i_m_i_n_a_t_e(Matrix& A, Vector& b, const Vector& sigmas); │ │ │ │ │ +312 │ │ │ │ │ +320GTSAM_EXPORT void _h_o_u_s_e_h_o_l_d_e_r__(Matrix& A, size_t k, bool copy_vectors=true); │ │ │ │ │ +321 │ │ │ │ │ +328GTSAM_EXPORT void _h_o_u_s_e_h_o_l_d_e_r(Matrix& A, size_t k); │ │ │ │ │ +329 │ │ │ │ │ +337GTSAM_EXPORT Vector _b_a_c_k_S_u_b_s_t_i_t_u_t_e_U_p_p_e_r(const Matrix& U, const Vector& b, │ │ │ │ │ +bool unit=false); │ │ │ │ │ +338 │ │ │ │ │ +346//TODO: is this function necessary? it isn't used │ │ │ │ │ +347GTSAM_EXPORT Vector _b_a_c_k_S_u_b_s_t_i_t_u_t_e_U_p_p_e_r(const Vector& b, const Matrix& U, │ │ │ │ │ +bool unit=false); │ │ │ │ │ +348 │ │ │ │ │ +356GTSAM_EXPORT Vector _b_a_c_k_S_u_b_s_t_i_t_u_t_e_L_o_w_e_r(const Matrix& L, const Vector& b, │ │ │ │ │ +bool unit=false); │ │ │ │ │ +357 │ │ │ │ │ +364GTSAM_EXPORT Matrix stack(size_t nrMatrices, ...); │ │ │ │ │ +365GTSAM_EXPORT Matrix stack(const std::vector& blocks); │ │ │ │ │ +366 │ │ │ │ │ +377GTSAM_EXPORT Matrix _c_o_l_l_e_c_t(const std::vector& matrices, │ │ │ │ │ +size_t m = 0, size_t n = 0); │ │ │ │ │ +378GTSAM_EXPORT Matrix _c_o_l_l_e_c_t(size_t nrMatrices, ...); │ │ │ │ │ +379 │ │ │ │ │ +386GTSAM_EXPORT void _v_e_c_t_o_r___s_c_a_l_e___i_n_p_l_a_c_e(const Vector& v, Matrix& A, bool │ │ │ │ │ +inf_mask = false); // row │ │ │ │ │ +387GTSAM_EXPORT Matrix vector_scale(const Vector& v, const Matrix& A, bool │ │ │ │ │ +inf_mask = false); // row │ │ │ │ │ +388GTSAM_EXPORT Matrix vector_scale(const Matrix& A, const Vector& v, bool │ │ │ │ │ +inf_mask = false); // column │ │ │ │ │ +389 │ │ │ │ │ +_4_0_1inline Matrix3 _s_k_e_w_S_y_m_m_e_t_r_i_c(double wx, double wy, double wz) { │ │ │ │ │ +402 return (Matrix3() << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0).finished │ │ │ │ │ +(); │ │ │ │ │ +403} │ │ │ │ │ +404 │ │ │ │ │ +405template │ │ │ │ │ +406inline Matrix3 _s_k_e_w_S_y_m_m_e_t_r_i_c(const Eigen::MatrixBase& w) { │ │ │ │ │ +407 return _s_k_e_w_S_y_m_m_e_t_r_i_c(w(0), w(1), w(2)); │ │ │ │ │ +408} │ │ │ │ │ +409 │ │ │ │ │ +411GTSAM_EXPORT Matrix _i_n_v_e_r_s_e___s_q_u_a_r_e___r_o_o_t(const Matrix& A); │ │ │ │ │ +412 │ │ │ │ │ +414GTSAM_EXPORT Matrix _c_h_o_l_e_s_k_y___i_n_v_e_r_s_e(const Matrix &A); │ │ │ │ │ +415 │ │ │ │ │ +428GTSAM_EXPORT void _s_v_d(const Matrix& A, Matrix& U, Vector& S, Matrix& V); │ │ │ │ │ +429 │ │ │ │ │ +437GTSAM_EXPORT boost::tuple │ │ │ │ │ +438_D_L_T(const Matrix& A, double rank_tol = 1e-9); │ │ │ │ │ +439 │ │ │ │ │ +445GTSAM_EXPORT Matrix _e_x_p_m(const Matrix& A, size_t K=7); │ │ │ │ │ +446 │ │ │ │ │ +447std::string formatMatrixIndented(const std::string& label, const Matrix& │ │ │ │ │ +matrix, bool makeVectorHorizontal = false); │ │ │ │ │ +448 │ │ │ │ │ +455template │ │ │ │ │ +_4_5_6struct _M_u_l_t_i_p_l_y_W_i_t_h_I_n_v_e_r_s_e { │ │ │ │ │ +457 typedef Eigen::Matrix VectorN; │ │ │ │ │ +458 typedef Eigen::Matrix MatrixN; │ │ │ │ │ +459 │ │ │ │ │ +_4_6_1 VectorN _o_p_e_r_a_t_o_r_(_)(const MatrixN& A, const VectorN& b, │ │ │ │ │ +462 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_N_,_ _N_*_ _N_> H1 = boost::none, │ │ │ │ │ +463 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_N_,_ _N_> H2 = boost::none) const { │ │ │ │ │ +464 const MatrixN invA = A.inverse(); │ │ │ │ │ +465 const VectorN c = invA * b; │ │ │ │ │ +466 // The derivative in A is just -[c[0]*invA c[1]*invA ... c[N-1]*invA] │ │ │ │ │ +467 if (H1) │ │ │ │ │ +468 for (size_t j = 0; j < N; j++) │ │ │ │ │ +469 H1->template middleCols(N * j) = -c[j] * invA; │ │ │ │ │ +470 // The derivative in b is easy, as invA*b is just a linear map: │ │ │ │ │ +471 if (H2) *H2 = invA; │ │ │ │ │ +472 return c; │ │ │ │ │ +473 } │ │ │ │ │ +474}; │ │ │ │ │ +475 │ │ │ │ │ +481template │ │ │ │ │ +_4_8_2struct _M_u_l_t_i_p_l_y_W_i_t_h_I_n_v_e_r_s_e_F_u_n_c_t_i_o_n { │ │ │ │ │ +483 enum { M = _t_r_a_i_t_s_<_T_>_:_:_d_i_m_e_n_s_i_o_n }; │ │ │ │ │ +484 typedef Eigen::Matrix VectorN; │ │ │ │ │ +485 typedef Eigen::Matrix MatrixN; │ │ │ │ │ +486 │ │ │ │ │ +487 // The function phi should calculate f(a)*b, with derivatives in a and b. │ │ │ │ │ +488 // Naturally, the derivative in b is f(a). │ │ │ │ │ +489 typedef std::function, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_N_,_ _N_>)> │ │ │ │ │ +491 Operator; │ │ │ │ │ +492 │ │ │ │ │ +_4_9_4 _M_u_l_t_i_p_l_y_W_i_t_h_I_n_v_e_r_s_e_F_u_n_c_t_i_o_n(const Operator& phi) : phi_(phi) {} │ │ │ │ │ +495 │ │ │ │ │ +_4_9_7 VectorN _o_p_e_r_a_t_o_r_(_)(const T& a, const VectorN& b, │ │ │ │ │ +498 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_N_,_ _M_> H1 = boost::none, │ │ │ │ │ +499 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_N_,_ _N_> H2 = boost::none) const { │ │ │ │ │ +500 MatrixN A; │ │ │ │ │ +501 phi_(a, b, boost::none, A); // get A = f(a) by calling f once │ │ │ │ │ +502 const MatrixN invA = A.inverse(); │ │ │ │ │ +503 const VectorN c = invA * b; │ │ │ │ │ +504 │ │ │ │ │ +505 if (H1) { │ │ │ │ │ +506 Eigen::Matrix H; │ │ │ │ │ +507 phi_(a, c, H, boost::none); // get derivative H of forward mapping │ │ │ │ │ +508 *H1 = -invA* H; │ │ │ │ │ +509 } │ │ │ │ │ +510 if (H2) *H2 = invA; │ │ │ │ │ +511 return c; │ │ │ │ │ +512 } │ │ │ │ │ +513 │ │ │ │ │ +514 private: │ │ │ │ │ +515 const Operator phi_; │ │ │ │ │ +516}; │ │ │ │ │ +517 │ │ │ │ │ +518GTSAM_EXPORT Matrix LLt(const Matrix& A); │ │ │ │ │ +519 │ │ │ │ │ +520GTSAM_EXPORT Matrix RtR(const Matrix& A); │ │ │ │ │ +521 │ │ │ │ │ +522GTSAM_EXPORT Vector columnNormSquare(const Matrix &A); │ │ │ │ │ +523} // namespace gtsam │ │ │ │ │ +_V_e_c_t_o_r_._h │ │ │ │ │ +typedef and functions to augment Eigen's VectorXd │ │ │ │ │ +_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h │ │ │ │ │ +Special class for optional Jacobian arguments. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_b_a_c_k_S_u_b_s_t_i_t_u_t_e_L_o_w_e_r │ │ │ │ │ +Vector backSubstituteLower(const Matrix &L, const Vector &b, bool unit) │ │ │ │ │ +backSubstitute L*x=b │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:367 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_^ │ │ │ │ │ +Vector operator^(const Matrix &A, const Vector &v) │ │ │ │ │ +overload ^ for trans(A)*v We transpose the vectors for speed. │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:131 │ │ │ │ │ +_g_t_s_a_m_:_:_v_e_c_t_o_r___s_c_a_l_e___i_n_p_l_a_c_e │ │ │ │ │ +void vector_scale_inplace(const Vector &v, Matrix &A, bool inf_mask) │ │ │ │ │ +scales a matrix row or column by the values in a vector Arguments (Matrix, │ │ │ │ │ +Vector) scales the columns... │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:482 │ │ │ │ │ +_g_t_s_a_m_:_:_r_o_w │ │ │ │ │ +const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j) │ │ │ │ │ +Extracts a row view from a matrix that avoids a copy. │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:222 │ │ │ │ │ +_g_t_s_a_m_:_:_e_x_p_m │ │ │ │ │ +T expm(const Vector &x, int K=7) │ │ │ │ │ +Exponential map given exponential coordinates class T needs a wedge<> function │ │ │ │ │ +and a constructor from... │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:317 │ │ │ │ │ +_g_t_s_a_m_:_:_s_a_v_e │ │ │ │ │ +void save(const Matrix &A, const string &s, const string &filename) │ │ │ │ │ +save a matrix to file, which can be loaded by matlab │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:167 │ │ │ │ │ +_g_t_s_a_m_:_:_a_s_s_e_r_t___e_q_u_a_l │ │ │ │ │ +bool assert_equal(const Matrix &expected, const Matrix &actual, double tol) │ │ │ │ │ +equals with an tolerance, prints out message if unequal │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:43 │ │ │ │ │ +_g_t_s_a_m_:_:_l_i_n_e_a_r___d_e_p_e_n_d_e_n_t │ │ │ │ │ +bool linear_dependent(const Matrix &A, const Matrix &B, double tol) │ │ │ │ │ +check whether the rows of two matrices are linear dependent │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:117 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_c_o_l_u_m_n │ │ │ │ │ +const MATRIX::ConstColXpr column(const MATRIX &A, size_t j) │ │ │ │ │ +Extracts a column view from a matrix that avoids a copy. │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:211 │ │ │ │ │ +_g_t_s_a_m_:_:_z_e_r_o_B_e_l_o_w_D_i_a_g_o_n_a_l │ │ │ │ │ +void zeroBelowDiagonal(MATRIX &A, size_t cols=0) │ │ │ │ │ +Zeros all of the elements below the diagonal of a matrix, in place. │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:232 │ │ │ │ │ +_g_t_s_a_m_:_:_w_e_i_g_h_t_e_d___e_l_i_m_i_n_a_t_e │ │ │ │ │ +list< boost::tuple< Vector, double, double > > weighted_eliminate(Matrix &A, │ │ │ │ │ +Vector &b, const Vector &sigmas) │ │ │ │ │ +Imperative algorithm for in-place full elimination with weights and constraint │ │ │ │ │ +handling. │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:273 │ │ │ │ │ +_g_t_s_a_m_:_:_b_a_c_k_S_u_b_s_t_i_t_u_t_e_U_p_p_e_r │ │ │ │ │ +Vector backSubstituteUpper(const Matrix &U, const Vector &b, bool unit) │ │ │ │ │ +backSubstitute U*x=b │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:377 │ │ │ │ │ +_g_t_s_a_m_:_:_a_s_s_e_r_t___i_n_e_q_u_a_l │ │ │ │ │ +bool assert_inequal(const Matrix &A, const Matrix &B, double tol) │ │ │ │ │ +inequals with an tolerance, prints out message if within tolerance │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:63 │ │ │ │ │ +_g_t_s_a_m_:_:_h_o_u_s_e_h_o_l_d_e_r │ │ │ │ │ +void householder(Matrix &A, size_t k) │ │ │ │ │ +Householder tranformation, zeros below diagonal. │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:354 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_>_> │ │ │ │ │ +istream & operator>>(istream &inputStream, Matrix &destinationMatrix) │ │ │ │ │ +Read a matrix from an input stream, such as a file. │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:174 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_p_l_a_c_e___Q_R │ │ │ │ │ +void inplace_QR(Matrix &A) │ │ │ │ │ +QR factorization using Eigen's internal block QR algorithm. │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:636 │ │ │ │ │ +_g_t_s_a_m_:_:_s_v_d │ │ │ │ │ +void svd(const Matrix &A, Matrix &U, Vector &S, Matrix &V) │ │ │ │ │ +SVD computes economy SVD A=U*S*V'. │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:560 │ │ │ │ │ +_g_t_s_a_m_:_:_s_k_e_w_S_y_m_m_e_t_r_i_c │ │ │ │ │ +Matrix3 skewSymmetric(double wx, double wy, double wz) │ │ │ │ │ +skew symmetric matrix returns this: 0 -wz wy wz 0 -wx -wy wx 0 │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:401 │ │ │ │ │ +_g_t_s_a_m_:_:_s_u_b │ │ │ │ │ +Eigen::Block< const MATRIX > sub(const MATRIX &A, size_t i1, size_t i2, size_t │ │ │ │ │ +j1, size_t j2) │ │ │ │ │ +extract submatrix, slice semantics, i.e. │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:181 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_n_s │ │ │ │ │ +Matrix trans(const Matrix &A) │ │ │ │ │ +static transpose function, just calls Eigen transpose member function │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:242 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_!_= │ │ │ │ │ +bool operator!=(const Matrix &A, const Matrix &B) │ │ │ │ │ +inequality │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:107 │ │ │ │ │ +_g_t_s_a_m_:_:_D_L_T │ │ │ │ │ +boost::tuple< int, double, Vector > DLT(const Matrix &A, double rank_tol) │ │ │ │ │ +Direct linear transform algorithm that calls svd to find a vector v that │ │ │ │ │ +minimizes the algebraic erro... │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:568 │ │ │ │ │ +_g_t_s_a_m_:_:_c_h_o_l_e_s_k_y___i_n_v_e_r_s_e │ │ │ │ │ +Matrix cholesky_inverse(const Matrix &A) │ │ │ │ │ +Return the inverse of a S.P.D. │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:539 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_o_d │ │ │ │ │ +MATRIX prod(const MATRIX &A, const MATRIX &B) │ │ │ │ │ +products using old-style format to improve compatibility │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:144 │ │ │ │ │ +_g_t_s_a_m_:_:_h_o_u_s_e_h_o_l_d_e_r__ │ │ │ │ │ +void householder_(Matrix &A, size_t k, bool copy_vectors) │ │ │ │ │ +Imperative version of Householder QR factorization, Golub & Van Loan p 224 │ │ │ │ │ +version with Householder v... │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:327 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_s_e_r_t_S_u_b │ │ │ │ │ +void insertSub(Eigen::MatrixBase< Derived1 > &fullMatrix, const Eigen:: │ │ │ │ │ +MatrixBase< Derived2 > &subMatrix, size_t i, size_t j) │ │ │ │ │ +insert a submatrix IN PLACE at a specified location in a larger matrix NOTE: │ │ │ │ │ +there is no size checkin... │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:195 │ │ │ │ │ +_g_t_s_a_m_:_:_c_o_l_l_e_c_t │ │ │ │ │ +Matrix collect(const std::vector< const Matrix * > &matrices, size_t m, size_t │ │ │ │ │ +n) │ │ │ │ │ +create a matrix by concatenating Given a set of matrices: A1, A2, A3... If all │ │ │ │ │ +matrices have the same... │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:443 │ │ │ │ │ +_g_t_s_a_m_:_:_l_i_n_e_a_r___i_n_d_e_p_e_n_d_e_n_t │ │ │ │ │ +bool linear_independent(const Matrix &A, const Matrix &B, double tol) │ │ │ │ │ +check whether the rows of two matrices are linear independent │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:103 │ │ │ │ │ +_g_t_s_a_m_:_:_f_p_E_q_u_a_l │ │ │ │ │ +bool fpEqual(double a, double b, double tol, bool check_relative_also) │ │ │ │ │ +Ensure we are not including a different version of Eigen in user code than │ │ │ │ │ +while compiling gtsam,... │ │ │ │ │ +DDeeffiinniittiioonn Vector.cpp:42 │ │ │ │ │ +_g_t_s_a_m_:_:_q_r │ │ │ │ │ +pair< Matrix, Matrix > qr(const Matrix &A) │ │ │ │ │ +Householder QR factorization, Golub & Van Loan p 224, explicit version │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:235 │ │ │ │ │ +_g_t_s_a_m_:_:_d_i_a_g │ │ │ │ │ +Matrix diag(const std::vector< Matrix > &Hs) │ │ │ │ │ +Create a matrix with submatrices along its diagonal. │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:207 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l___w_i_t_h___a_b_s___t_o_l │ │ │ │ │ +bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen:: │ │ │ │ │ +DenseBase< MATRIX > &B, double tol=1e-9) │ │ │ │ │ +equals with a tolerance │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:81 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_=_= │ │ │ │ │ +bool operator==(const Matrix &A, const Matrix &B) │ │ │ │ │ +equality is just equal_with_abs_tol 1e-9 │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_v_e_r_s_e___s_q_u_a_r_e___r_o_o_t │ │ │ │ │ +Matrix inverse_square_root(const Matrix &A) │ │ │ │ │ +Use Cholesky to calculate inverse square root of a matrix. │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:552 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p_T_r_a_i_t_s │ │ │ │ │ -A helper class that implements the traits interface for GTSAM lie groups. │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:174 │ │ │ │ │ -_g_t_s_a_m_:_:_I_s_L_i_e_G_r_o_u_p │ │ │ │ │ -Lie Group Concept. │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:260 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_o_d_u_c_t_L_i_e_G_r_o_u_p │ │ │ │ │ -Template to construct the product Lie group of two other Lie groups Assumes Lie │ │ │ │ │ -group structure for G... │ │ │ │ │ -DDeeffiinniittiioonn ProductLieGroup.h:29 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_o_d_u_c_t_L_i_e_G_r_o_u_p_:_:_P_r_o_d_u_c_t_L_i_e_G_r_o_u_p │ │ │ │ │ -ProductLieGroup() │ │ │ │ │ -Default constructor yields identity. │ │ │ │ │ -DDeeffiinniittiioonn ProductLieGroup.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_s_h_a_p_e │ │ │ │ │ +Reshape functor. │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:246 │ │ │ │ │ +_g_t_s_a_m_:_:_M_u_l_t_i_p_l_y_W_i_t_h_I_n_v_e_r_s_e │ │ │ │ │ +Functor that implements multiplication of a vector b with the inverse of a │ │ │ │ │ +matrix A. │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:456 │ │ │ │ │ +_g_t_s_a_m_:_:_M_u_l_t_i_p_l_y_W_i_t_h_I_n_v_e_r_s_e_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ +VectorN operator()(const MatrixN &A, const VectorN &b, OptionalJacobian< N, N │ │ │ │ │ +*N > H1=boost::none, OptionalJacobian< N, N > H2=boost::none) const │ │ │ │ │ +A.inverse() * b, with optional derivatives. │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:461 │ │ │ │ │ +_g_t_s_a_m_:_:_M_u_l_t_i_p_l_y_W_i_t_h_I_n_v_e_r_s_e_F_u_n_c_t_i_o_n │ │ │ │ │ +Functor that implements multiplication with the inverse of a matrix, itself the │ │ │ │ │ +result of a function ... │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:482 │ │ │ │ │ +_g_t_s_a_m_:_:_M_u_l_t_i_p_l_y_W_i_t_h_I_n_v_e_r_s_e_F_u_n_c_t_i_o_n_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ +VectorN operator()(const T &a, const VectorN &b, OptionalJacobian< N, M > │ │ │ │ │ +H1=boost::none, OptionalJacobian< N, N > H2=boost::none) const │ │ │ │ │ +f(a).inverse() * b, with optional derivatives │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:497 │ │ │ │ │ +_g_t_s_a_m_:_:_M_u_l_t_i_p_l_y_W_i_t_h_I_n_v_e_r_s_e_F_u_n_c_t_i_o_n_:_:_M_u_l_t_i_p_l_y_W_i_t_h_I_n_v_e_r_s_e_F_u_n_c_t_i_o_n │ │ │ │ │ +MultiplyWithInverseFunction(const Operator &phi) │ │ │ │ │ +Construct with function as explained above. │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:494 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ +either a fixed size o... │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p_._h │ │ │ │ │ + * _M_a_t_r_i_x_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00098_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Group.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/testLie.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,220 +98,111 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Group.h
│ │ │ │ +
testLie.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
21#pragma once
│ │ │ │ -
22
│ │ │ │ -
23#include <gtsam/base/Testable.h>
│ │ │ │ -
24
│ │ │ │ -
25#include <boost/concept_check.hpp>
│ │ │ │ -
26#include <boost/concept/requires.hpp>
│ │ │ │ -
27#include <boost/type_traits/is_base_of.hpp>
│ │ │ │ -
28#include <boost/static_assert.hpp>
│ │ │ │ -
29#include <utility>
│ │ │ │ -
30
│ │ │ │ -
31namespace gtsam {
│ │ │ │ -
32
│ │ │ │ -
34struct group_tag {};
│ │ │ │ -
35
│ │ │ │ -
37struct multiplicative_group_tag {};
│ │ │ │ -
38struct additive_group_tag {};
│ │ │ │ -
39
│ │ │ │ -
40template <typename T> struct traits;
│ │ │ │ -
41
│ │ │ │ -
45template<typename G>
│ │ │ │ -
│ │ │ │ -
46class IsGroup {
│ │ │ │ -
47public:
│ │ │ │ -
48 typedef typename traits<G>::structure_category structure_category_tag;
│ │ │ │ -
49 typedef typename traits<G>::group_flavor flavor_tag;
│ │ │ │ -
50 //typedef typename traits<G>::identity::value_type identity_value_type;
│ │ │ │ +
12/*
│ │ │ │ +
13 * @file testLie.h
│ │ │ │ +
14 * @brief Test utilities for Lie groups
│ │ │ │ +
15 * @date November, 2014
│ │ │ │ +
16 * @author Paul Furgale
│ │ │ │ +
17 */
│ │ │ │ +
18
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/base/Lie.h>
│ │ │ │ +
22#include <gtsam/base/numericalDerivative.h>
│ │ │ │ +
23
│ │ │ │ +
24#include <CppUnitLite/TestResult.h>
│ │ │ │ +
25#include <CppUnitLite/Test.h>
│ │ │ │ +
26#include <CppUnitLite/Failure.h>
│ │ │ │ +
27
│ │ │ │ +
28namespace gtsam {
│ │ │ │ +
29
│ │ │ │ +
30// Do a comprehensive test of Lie Group derivatives
│ │ │ │ +
31template<typename G>
│ │ │ │ +
32void testLieGroupDerivatives(TestResult& result_, const std::string& name_,
│ │ │ │ +
33 const G& t1, const G& t2) {
│ │ │ │ +
34
│ │ │ │ +
35 Matrix H1, H2;
│ │ │ │ +
36 typedef traits<G> T;
│ │ │ │ +
37 typedef OptionalJacobian<T::dimension,T::dimension> OJ;
│ │ │ │ +
38
│ │ │ │ +
39 // Inverse
│ │ │ │ +
40 OJ none;
│ │ │ │ +
41 EXPECT(assert_equal<G>(t1.inverse(),T::Inverse(t1, H1)));
│ │ │ │ +
42 EXPECT(assert_equal(numericalDerivative21<G,G,OJ>(T::Inverse, t1, none),H1));
│ │ │ │ +
43
│ │ │ │ +
44 EXPECT(assert_equal<G>(t2.inverse(),T::Inverse(t2, H1)));
│ │ │ │ +
45 EXPECT(assert_equal(numericalDerivative21<G,G,OJ>(T::Inverse, t2, none),H1));
│ │ │ │ +
46
│ │ │ │ +
47 // Compose
│ │ │ │ +
48 EXPECT(assert_equal<G>(t1 * t2,T::Compose(t1, t2, H1, H2)));
│ │ │ │ +
49 EXPECT(assert_equal(numericalDerivative41<G,G,G,OJ,OJ>(T::Compose, t1, t2, none, none), H1));
│ │ │ │ +
50 EXPECT(assert_equal(numericalDerivative42<G,G,G,OJ,OJ>(T::Compose, t1, t2, none, none), H2));
│ │ │ │
51
│ │ │ │ -
52 BOOST_CONCEPT_USAGE(IsGroup) {
│ │ │ │ -
53 BOOST_STATIC_ASSERT_MSG(
│ │ │ │ -
54 (boost::is_base_of<group_tag, structure_category_tag>::value),
│ │ │ │ -
55 "This type's structure_category trait does not assert it as a group (or derived)");
│ │ │ │ -
56 e = traits<G>::Identity();
│ │ │ │ -
57 e = traits<G>::Compose(g, h);
│ │ │ │ -
58 e = traits<G>::Between(g, h);
│ │ │ │ -
59 e = traits<G>::Inverse(g);
│ │ │ │ -
60 operator_usage(flavor);
│ │ │ │ -
61 // todo: how do we test the act concept? or do we even need to?
│ │ │ │ -
62 }
│ │ │ │ -
63
│ │ │ │ -
64private:
│ │ │ │ -
65 void operator_usage(multiplicative_group_tag) {
│ │ │ │ -
66 e = g * h;
│ │ │ │ -
67 //e = -g; // todo this should work, but it is failing for Quaternions
│ │ │ │ -
68 }
│ │ │ │ -
69 void operator_usage(additive_group_tag) {
│ │ │ │ -
70 e = g + h;
│ │ │ │ -
71 e = h - g;
│ │ │ │ -
72 e = -g;
│ │ │ │ -
73 }
│ │ │ │ +
52 // Between
│ │ │ │ +
53 EXPECT(assert_equal<G>(t1.inverse() * t2,T::Between(t1, t2, H1, H2)));
│ │ │ │ +
54 EXPECT(assert_equal(numericalDerivative41<G,G,G,OJ,OJ>(T::Between, t1, t2, none, none), H1));
│ │ │ │ +
55 EXPECT(assert_equal(numericalDerivative42<G,G,G,OJ,OJ>(T::Between, t1, t2, none, none), H2));
│ │ │ │ +
56}
│ │ │ │ +
57
│ │ │ │ +
58// Do a comprehensive test of Lie Group Chart derivatives
│ │ │ │ +
59template<typename G>
│ │ │ │ +
60void testChartDerivatives(TestResult& result_, const std::string& name_,
│ │ │ │ +
61 const G& t1, const G& t2) {
│ │ │ │ +
62
│ │ │ │ +
63 Matrix H1, H2;
│ │ │ │ +
64 typedef traits<G> T;
│ │ │ │ +
65 typedef typename T::TangentVector V;
│ │ │ │ +
66 typedef OptionalJacobian<T::dimension,T::dimension> OJ;
│ │ │ │ +
67
│ │ │ │ +
68 // Retract
│ │ │ │ +
69 OJ none;
│ │ │ │ +
70 V w12 = T::Local(t1, t2);
│ │ │ │ +
71 EXPECT(assert_equal<G>(t2, T::Retract(t1,w12, H1, H2)));
│ │ │ │ +
72 EXPECT(assert_equal(numericalDerivative41<G,G,V,OJ,OJ>(T::Retract, t1, w12, none, none), H1));
│ │ │ │ +
73 EXPECT(assert_equal(numericalDerivative42<G,G,V,OJ,OJ>(T::Retract, t1, w12, none, none), H2));
│ │ │ │
74
│ │ │ │ -
75 flavor_tag flavor;
│ │ │ │ -
76 G e, g, h;
│ │ │ │ -
77 bool b;
│ │ │ │ -
78};
│ │ │ │ -
│ │ │ │ -
79
│ │ │ │ -
81template<typename G>
│ │ │ │ -
82BOOST_CONCEPT_REQUIRES(((IsGroup<G>)),(bool)) //
│ │ │ │ -
83check_group_invariants(const G& a, const G& b, double tol = 1e-9) {
│ │ │ │ -
84 G e = traits<G>::Identity();
│ │ │ │ -
85 return traits<G>::Equals(traits<G>::Compose(a, traits<G>::Inverse(a)), e, tol)
│ │ │ │ -
86 && traits<G>::Equals(traits<G>::Between(a, b), traits<G>::Compose(traits<G>::Inverse(a), b), tol)
│ │ │ │ -
87 && traits<G>::Equals(traits<G>::Compose(a, traits<G>::Between(a, b)), b, tol);
│ │ │ │ -
88}
│ │ │ │ -
89
│ │ │ │ -
90namespace internal {
│ │ │ │ -
91
│ │ │ │ -
94template<class Class>
│ │ │ │ -
95struct MultiplicativeGroupTraits {
│ │ │ │ -
96 typedef group_tag structure_category;
│ │ │ │ -
97 typedef multiplicative_group_tag group_flavor;
│ │ │ │ -
98 static Class Identity() { return Class::Identity(); }
│ │ │ │ -
99 static Class Compose(const Class &g, const Class & h) { return g * h;}
│ │ │ │ -
100 static Class Between(const Class &g, const Class & h) { return g.inverse() * h;}
│ │ │ │ -
101 static Class Inverse(const Class &g) { return g.inverse();}
│ │ │ │ -
102};
│ │ │ │ -
103
│ │ │ │ -
105template<class Class>
│ │ │ │ -
106struct MultiplicativeGroup : MultiplicativeGroupTraits<Class>, Testable<Class> {};
│ │ │ │ -
107
│ │ │ │ -
110template<class Class>
│ │ │ │ -
111struct AdditiveGroupTraits {
│ │ │ │ -
112 typedef group_tag structure_category;
│ │ │ │ -
113 typedef additive_group_tag group_flavor;
│ │ │ │ -
114 static Class Identity() { return Class::Identity(); }
│ │ │ │ -
115 static Class Compose(const Class &g, const Class & h) { return g + h;}
│ │ │ │ -
116 static Class Between(const Class &g, const Class & h) { return h - g;}
│ │ │ │ -
117 static Class Inverse(const Class &g) { return -g;}
│ │ │ │ -
118};
│ │ │ │ -
119
│ │ │ │ -
121template<class Class>
│ │ │ │ -
122struct AdditiveGroup : AdditiveGroupTraits<Class>, Testable<Class> {};
│ │ │ │ -
123
│ │ │ │ -
124} // namespace internal
│ │ │ │ -
125
│ │ │ │ -
127template<typename G>
│ │ │ │ -
128BOOST_CONCEPT_REQUIRES(((IsGroup<G>)),(G)) //
│ │ │ │ -
129compose_pow(const G& g, size_t n) {
│ │ │ │ -
130 if (n == 0) return traits<G>::Identity();
│ │ │ │ -
131 else if (n == 1) return g;
│ │ │ │ -
132 else return traits<G>::Compose(compose_pow(g, n - 1), g);
│ │ │ │ -
133}
│ │ │ │ -
134
│ │ │ │ -
137template<typename G, typename H>
│ │ │ │ -
│ │ │ │ -
138class DirectProduct: public std::pair<G, H> {
│ │ │ │ -
139 BOOST_CONCEPT_ASSERT((IsGroup<G>));
│ │ │ │ -
140 BOOST_CONCEPT_ASSERT((IsGroup<H>));
│ │ │ │ -
141
│ │ │ │ -
142public:
│ │ │ │ -
144 DirectProduct():std::pair<G,H>(traits<G>::Identity(),traits<H>::Identity()) {}
│ │ │ │ -
145
│ │ │ │ -
146 // Construct from two subgroup elements
│ │ │ │ -
147 DirectProduct(const G& g, const H& h):std::pair<G,H>(g,h) {}
│ │ │ │ -
148
│ │ │ │ -
149 // identity
│ │ │ │ -
150 static DirectProduct Identity() { return DirectProduct(); }
│ │ │ │ -
151
│ │ │ │ -
152 DirectProduct operator*(const DirectProduct& other) const {
│ │ │ │ -
153 return DirectProduct(traits<G>::Compose(this->first, other.first),
│ │ │ │ -
154 traits<H>::Compose(this->second, other.second));
│ │ │ │ -
155 }
│ │ │ │ -
156 DirectProduct inverse() const {
│ │ │ │ -
157 return DirectProduct(this->first.inverse(), this->second.inverse());
│ │ │ │ -
158 }
│ │ │ │ -
159};
│ │ │ │ -
│ │ │ │ -
160
│ │ │ │ -
161// Define any direct product group to be a model of the multiplicative Group concept
│ │ │ │ -
162template<typename G, typename H>
│ │ │ │ -
│ │ │ │ -
163struct traits<DirectProduct<G, H> > :
│ │ │ │ -
164 internal::MultiplicativeGroupTraits<DirectProduct<G, H> > {};
│ │ │ │ -
│ │ │ │ -
165
│ │ │ │ -
168template<typename G, typename H>
│ │ │ │ -
│ │ │ │ -
169class DirectSum: public std::pair<G, H> {
│ │ │ │ -
170 BOOST_CONCEPT_ASSERT((IsGroup<G>)); // TODO(frank): check additive
│ │ │ │ -
171 BOOST_CONCEPT_ASSERT((IsGroup<H>)); // TODO(frank): check additive
│ │ │ │ -
172
│ │ │ │ -
173 const G& g() const { return this->first; }
│ │ │ │ -
174 const H& h() const { return this->second;}
│ │ │ │ -
175
│ │ │ │ -
176public:
│ │ │ │ -
178 DirectSum():std::pair<G,H>(traits<G>::Identity(),traits<H>::Identity()) {}
│ │ │ │ -
179
│ │ │ │ -
180 // Construct from two subgroup elements
│ │ │ │ -
181 DirectSum(const G& g, const H& h):std::pair<G,H>(g,h) {}
│ │ │ │ -
182
│ │ │ │ -
183 // identity
│ │ │ │ -
184 static DirectSum Identity() { return DirectSum(); }
│ │ │ │ -
185
│ │ │ │ -
186 DirectSum operator+(const DirectSum& other) const {
│ │ │ │ -
187 return DirectSum(g()+other.g(), h()+other.h());
│ │ │ │ -
188 }
│ │ │ │ -
189 DirectSum operator-(const DirectSum& other) const {
│ │ │ │ -
190 return DirectSum(g()-other.g(), h()-other.h());
│ │ │ │ -
191 }
│ │ │ │ -
192 DirectSum operator-() const {
│ │ │ │ -
193 return DirectSum(- g(), - h());
│ │ │ │ -
194 }
│ │ │ │ -
195};
│ │ │ │ -
│ │ │ │ -
196
│ │ │ │ -
197// Define direct sums to be a model of the Additive Group concept
│ │ │ │ -
198template<typename G, typename H>
│ │ │ │ -
│ │ │ │ -
199struct traits<DirectSum<G, H> > :
│ │ │ │ -
200 internal::AdditiveGroupTraits<DirectSum<G, H> > {};
│ │ │ │ -
│ │ │ │ -
201
│ │ │ │ -
202} // namespace gtsam
│ │ │ │ -
203
│ │ │ │ -
212#define GTSAM_CONCEPT_GROUP_INST(T) template class gtsam::IsGroup<T>;
│ │ │ │ -
213#define GTSAM_CONCEPT_GROUP_TYPE(T) typedef gtsam::IsGroup<T> _gtsam_IsGroup_##T;
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
75 // Local
│ │ │ │ +
76 EXPECT(assert_equal(w12, T::Local(t1, t2, H1, H2)));
│ │ │ │ +
77 EXPECT(assert_equal(numericalDerivative41<V,G,G,OJ,OJ>(T::Local, t1, t2, none, none), H1));
│ │ │ │ +
78 EXPECT(assert_equal(numericalDerivative42<V,G,G,OJ,OJ>(T::Local, t1, t2, none, none), H2));
│ │ │ │ +
79}
│ │ │ │ +
80} // namespace gtsam
│ │ │ │ +
81
│ │ │ │ +
82#define CHECK_LIE_GROUP_DERIVATIVES(t1,t2) \
│ │ │ │ +
83 { gtsam::testLieGroupDerivatives(result_, name_, t1, t2); }
│ │ │ │ +
84
│ │ │ │ +
85#define CHECK_CHART_DERIVATIVES(t1,t2) \
│ │ │ │ +
86 { gtsam::testChartDerivatives(result_, name_, t1, t2); }
│ │ │ │ +
numericalDerivative.h
Some functions to compute numerical derivatives.
│ │ │ │ +
Lie.h
Base class and basic functions for Lie types.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::BOOST_CONCEPT_REQUIRES
BOOST_CONCEPT_REQUIRES(((IsGroup< G >)),(bool)) check_group_invariants(const G &a
Check invariants.
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::group_tag
tag to assert a type is a group
Definition Group.h:34
│ │ │ │ -
gtsam::multiplicative_group_tag
Group operator syntax flavors.
Definition Group.h:37
│ │ │ │ -
gtsam::additive_group_tag
Definition Group.h:38
│ │ │ │ -
gtsam::IsGroup
Group Concept.
Definition Group.h:46
│ │ │ │ -
gtsam::DirectProduct
Definition Group.h:138
│ │ │ │ -
gtsam::DirectProduct::DirectProduct
DirectProduct()
Default constructor yields identity.
Definition Group.h:144
│ │ │ │ -
gtsam::DirectSum
Template to construct the direct sum of two additive groups Assumes existence of three additive opera...
Definition Group.h:169
│ │ │ │ -
gtsam::DirectSum::DirectSum
DirectSum()
Default constructor yields identity.
Definition Group.h:178
│ │ │ │ +
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
equals with an tolerance, prints out message if unequal
Definition Matrix.cpp:43
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,237 +1,119 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Group.h │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +testLie.h │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -21#pragma once │ │ │ │ │ -22 │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ -24 │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ -28#include │ │ │ │ │ -29#include │ │ │ │ │ -30 │ │ │ │ │ -31namespace _g_t_s_a_m { │ │ │ │ │ -32 │ │ │ │ │ -_3_4struct _g_r_o_u_p___t_a_g {}; │ │ │ │ │ -35 │ │ │ │ │ -_3_7struct _m_u_l_t_i_p_l_i_c_a_t_i_v_e___g_r_o_u_p___t_a_g {}; │ │ │ │ │ -_3_8struct _a_d_d_i_t_i_v_e___g_r_o_u_p___t_a_g {}; │ │ │ │ │ -39 │ │ │ │ │ -40template struct _t_r_a_i_t_s; │ │ │ │ │ -41 │ │ │ │ │ -45template │ │ │ │ │ -_4_6class _I_s_G_r_o_u_p { │ │ │ │ │ -47public: │ │ │ │ │ -48 typedef typename _t_r_a_i_t_s_<_G_>_:_:_s_t_r_u_c_t_u_r_e___c_a_t_e_g_o_r_y structure_category_tag; │ │ │ │ │ -49 typedef typename _t_r_a_i_t_s_<_G_>_:_:_g_r_o_u_p___f_l_a_v_o_r flavor_tag; │ │ │ │ │ -50 //typedef typename traits::identity::value_type identity_value_type; │ │ │ │ │ +12/* │ │ │ │ │ +13 * @file testLie.h │ │ │ │ │ +14 * @brief Test utilities for Lie groups │ │ │ │ │ +15 * @date November, 2014 │ │ │ │ │ +16 * @author Paul Furgale │ │ │ │ │ +17 */ │ │ │ │ │ +18 │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_b_a_s_e_/_L_i_e_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_._h> │ │ │ │ │ +23 │ │ │ │ │ +24#include │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ +27 │ │ │ │ │ +28namespace _g_t_s_a_m { │ │ │ │ │ +29 │ │ │ │ │ +30// Do a comprehensive test of Lie Group derivatives │ │ │ │ │ +31template │ │ │ │ │ +32void testLieGroupDerivatives(TestResult& result_, const std::string& name_, │ │ │ │ │ +33 const G& t1, const G& t2) { │ │ │ │ │ +34 │ │ │ │ │ +35 Matrix H1, H2; │ │ │ │ │ +36 typedef traits T; │ │ │ │ │ +37 typedef OptionalJacobian OJ; │ │ │ │ │ +38 │ │ │ │ │ +39 // Inverse │ │ │ │ │ +40 OJ none; │ │ │ │ │ +41 EXPECT(assert_equal(t1.inverse(),T::Inverse(t1, H1))); │ │ │ │ │ +42 EXPECT(_a_s_s_e_r_t___e_q_u_a_l(numericalDerivative21(T::Inverse, t1, │ │ │ │ │ +none),H1)); │ │ │ │ │ +43 │ │ │ │ │ +44 EXPECT(assert_equal(t2.inverse(),T::Inverse(t2, H1))); │ │ │ │ │ +45 EXPECT(_a_s_s_e_r_t___e_q_u_a_l(numericalDerivative21(T::Inverse, t2, │ │ │ │ │ +none),H1)); │ │ │ │ │ +46 │ │ │ │ │ +47 // Compose │ │ │ │ │ +48 EXPECT(assert_equal(t1 * t2,T::Compose(t1, t2, H1, H2))); │ │ │ │ │ +49 EXPECT(_a_s_s_e_r_t___e_q_u_a_l(numericalDerivative41(T::Compose, t1, t2, │ │ │ │ │ +none, none), H1)); │ │ │ │ │ +50 EXPECT(_a_s_s_e_r_t___e_q_u_a_l(numericalDerivative42(T::Compose, t1, t2, │ │ │ │ │ +none, none), H2)); │ │ │ │ │ 51 │ │ │ │ │ -52 BOOST_CONCEPT_USAGE(_I_s_G_r_o_u_p) { │ │ │ │ │ -53 BOOST_STATIC_ASSERT_MSG( │ │ │ │ │ -54 (boost::is_base_of::value), │ │ │ │ │ -55 "This type's structure_category trait does not assert it as a group (or │ │ │ │ │ -derived)"); │ │ │ │ │ -56 e = _t_r_a_i_t_s_<_G_>_:_:_I_d_e_n_t_i_t_y(); │ │ │ │ │ -57 e = _t_r_a_i_t_s_<_G_>_:_:_C_o_m_p_o_s_e(g, h); │ │ │ │ │ -58 e = _t_r_a_i_t_s_<_G_>_:_:_B_e_t_w_e_e_n(g, h); │ │ │ │ │ -59 e = _t_r_a_i_t_s_<_G_>_:_:_I_n_v_e_r_s_e(g); │ │ │ │ │ -60 operator_usage(flavor); │ │ │ │ │ -61 // todo: how do we test the act concept? or do we even need to? │ │ │ │ │ -62 } │ │ │ │ │ -63 │ │ │ │ │ -64private: │ │ │ │ │ -65 void operator_usage(_m_u_l_t_i_p_l_i_c_a_t_i_v_e___g_r_o_u_p___t_a_g) { │ │ │ │ │ -66 e = g * h; │ │ │ │ │ -67 //e = -g; // todo this should work, but it is failing for Quaternions │ │ │ │ │ -68 } │ │ │ │ │ -69 void operator_usage(_a_d_d_i_t_i_v_e___g_r_o_u_p___t_a_g) { │ │ │ │ │ -70 e = g + h; │ │ │ │ │ -71 e = h - g; │ │ │ │ │ -72 e = -g; │ │ │ │ │ -73 } │ │ │ │ │ +52 // Between │ │ │ │ │ +53 EXPECT(assert_equal(t1.inverse() * t2,T::Between(t1, t2, H1, H2))); │ │ │ │ │ +54 EXPECT(_a_s_s_e_r_t___e_q_u_a_l(numericalDerivative41(T::Between, t1, t2, │ │ │ │ │ +none, none), H1)); │ │ │ │ │ +55 EXPECT(_a_s_s_e_r_t___e_q_u_a_l(numericalDerivative42(T::Between, t1, t2, │ │ │ │ │ +none, none), H2)); │ │ │ │ │ +56} │ │ │ │ │ +57 │ │ │ │ │ +58// Do a comprehensive test of Lie Group Chart derivatives │ │ │ │ │ +59template │ │ │ │ │ +60void testChartDerivatives(TestResult& result_, const std::string& name_, │ │ │ │ │ +61 const G& t1, const G& t2) { │ │ │ │ │ +62 │ │ │ │ │ +63 Matrix H1, H2; │ │ │ │ │ +64 typedef traits T; │ │ │ │ │ +65 typedef typename T::TangentVector V; │ │ │ │ │ +66 typedef OptionalJacobian OJ; │ │ │ │ │ +67 │ │ │ │ │ +68 // Retract │ │ │ │ │ +69 OJ none; │ │ │ │ │ +70 V w12 = T::Local(t1, t2); │ │ │ │ │ +71 EXPECT(assert_equal(t2, T::Retract(t1,w12, H1, H2))); │ │ │ │ │ +72 EXPECT(_a_s_s_e_r_t___e_q_u_a_l(numericalDerivative41(T::Retract, t1, w12, │ │ │ │ │ +none, none), H1)); │ │ │ │ │ +73 EXPECT(_a_s_s_e_r_t___e_q_u_a_l(numericalDerivative42(T::Retract, t1, w12, │ │ │ │ │ +none, none), H2)); │ │ │ │ │ 74 │ │ │ │ │ -75 flavor_tag flavor; │ │ │ │ │ -76 G e, g, h; │ │ │ │ │ -77 bool b; │ │ │ │ │ -78}; │ │ │ │ │ -79 │ │ │ │ │ -81template │ │ │ │ │ -_8_2_B_O_O_S_T___C_O_N_C_E_P_T___R_E_Q_U_I_R_E_S(((_I_s_G_r_o_u_p_<_G_>)),(bool)) // │ │ │ │ │ -83check_group_invariants(const G& a, const G& b, double tol = 1e-9) { │ │ │ │ │ -84 G e = _t_r_a_i_t_s_<_G_>_:_:_I_d_e_n_t_i_t_y(); │ │ │ │ │ -85 return _t_r_a_i_t_s_<_G_>_:_:_E_q_u_a_l_s(_t_r_a_i_t_s_<_G_>_:_:_C_o_m_p_o_s_e(a, _t_r_a_i_t_s_<_G_>_:_:_I_n_v_e_r_s_e(a)), e, │ │ │ │ │ -tol) │ │ │ │ │ -86 && _t_r_a_i_t_s_<_G_>_:_:_E_q_u_a_l_s(_t_r_a_i_t_s_<_G_>_:_:_B_e_t_w_e_e_n(a, b), _t_r_a_i_t_s_<_G_>_:_:_C_o_m_p_o_s_e │ │ │ │ │ -(_t_r_a_i_t_s_<_G_>_:_:_I_n_v_e_r_s_e(a), b), tol) │ │ │ │ │ -87 && _t_r_a_i_t_s_<_G_>_:_:_E_q_u_a_l_s(_t_r_a_i_t_s_<_G_>_:_:_C_o_m_p_o_s_e(a, _t_r_a_i_t_s_<_G_>_:_:_B_e_t_w_e_e_n(a, b)), b, │ │ │ │ │ -tol); │ │ │ │ │ -88} │ │ │ │ │ -89 │ │ │ │ │ -90namespace internal { │ │ │ │ │ -91 │ │ │ │ │ -94template │ │ │ │ │ -95struct MultiplicativeGroupTraits { │ │ │ │ │ -96 typedef group_tag structure_category; │ │ │ │ │ -97 typedef multiplicative_group_tag group_flavor; │ │ │ │ │ -98 static Class Identity() { return Class::Identity(); } │ │ │ │ │ -99 static Class Compose(const Class &g, const Class & h) { return g * h;} │ │ │ │ │ -100 static Class Between(const Class &g, const Class & h) { return g.inverse() │ │ │ │ │ -* h;} │ │ │ │ │ -101 static Class Inverse(const Class &g) { return g.inverse();} │ │ │ │ │ -102}; │ │ │ │ │ -103 │ │ │ │ │ -105template │ │ │ │ │ -106struct MultiplicativeGroup : MultiplicativeGroupTraits, │ │ │ │ │ -Testable {}; │ │ │ │ │ -107 │ │ │ │ │ -110template │ │ │ │ │ -111struct AdditiveGroupTraits { │ │ │ │ │ -112 typedef group_tag structure_category; │ │ │ │ │ -113 typedef additive_group_tag group_flavor; │ │ │ │ │ -114 static Class Identity() { return Class::Identity(); } │ │ │ │ │ -115 static Class Compose(const Class &g, const Class & h) { return g + h;} │ │ │ │ │ -116 static Class Between(const Class &g, const Class & h) { return h - g;} │ │ │ │ │ -117 static Class Inverse(const Class &g) { return -g;} │ │ │ │ │ -118}; │ │ │ │ │ -119 │ │ │ │ │ -121template │ │ │ │ │ -122struct AdditiveGroup : AdditiveGroupTraits, Testable {}; │ │ │ │ │ -123 │ │ │ │ │ -124} // namespace internal │ │ │ │ │ -125 │ │ │ │ │ -127template │ │ │ │ │ -128_B_O_O_S_T___C_O_N_C_E_P_T___R_E_Q_U_I_R_E_S(((IsGroup)),(G)) // │ │ │ │ │ -129compose_pow(const G& g, size_t n) { │ │ │ │ │ -130 if (n == 0) return traits::Identity(); │ │ │ │ │ -131 else if (n == 1) return g; │ │ │ │ │ -132 else return traits::Compose(compose_pow(g, n - 1), g); │ │ │ │ │ -133} │ │ │ │ │ -134 │ │ │ │ │ -137template │ │ │ │ │ -_1_3_8class _D_i_r_e_c_t_P_r_o_d_u_c_t: public std::pair { │ │ │ │ │ -139 BOOST_CONCEPT_ASSERT((_I_s_G_r_o_u_p_<_G_>)); │ │ │ │ │ -140 BOOST_CONCEPT_ASSERT((_I_s_G_r_o_u_p_<_H_>)); │ │ │ │ │ -141 │ │ │ │ │ -142public: │ │ │ │ │ -_1_4_4 _D_i_r_e_c_t_P_r_o_d_u_c_t():std::pair(_t_r_a_i_t_s::Identity(),_t_r_a_i_t_s::Identity()) │ │ │ │ │ -{} │ │ │ │ │ -145 │ │ │ │ │ -146 // Construct from two subgroup elements │ │ │ │ │ -147 _D_i_r_e_c_t_P_r_o_d_u_c_t(const G& g, const H& h):std::pair(g,h) {} │ │ │ │ │ -148 │ │ │ │ │ -149 // identity │ │ │ │ │ -150 static _D_i_r_e_c_t_P_r_o_d_u_c_t Identity() { return _D_i_r_e_c_t_P_r_o_d_u_c_t(); } │ │ │ │ │ -151 │ │ │ │ │ -152 _D_i_r_e_c_t_P_r_o_d_u_c_t operator*(const _D_i_r_e_c_t_P_r_o_d_u_c_t& other) const { │ │ │ │ │ -153 return _D_i_r_e_c_t_P_r_o_d_u_c_t(traits::Compose(this->first, other.first), │ │ │ │ │ -154 traits::Compose(this->second, other.second)); │ │ │ │ │ -155 } │ │ │ │ │ -156 _D_i_r_e_c_t_P_r_o_d_u_c_t inverse() const { │ │ │ │ │ -157 return _D_i_r_e_c_t_P_r_o_d_u_c_t(this->first.inverse(), this->second.inverse()); │ │ │ │ │ -158 } │ │ │ │ │ -159}; │ │ │ │ │ -160 │ │ │ │ │ -161// Define any direct product group to be a model of the multiplicative Group │ │ │ │ │ -concept │ │ │ │ │ -162template │ │ │ │ │ -_1_6_3struct _t_r_a_i_t_s<_D_i_r_e_c_t_P_r_o_d_u_c_t > : │ │ │ │ │ -164 internal::MultiplicativeGroupTraits > {}; │ │ │ │ │ -165 │ │ │ │ │ -168template │ │ │ │ │ -_1_6_9class _D_i_r_e_c_t_S_u_m: public std::pair { │ │ │ │ │ -170 BOOST_CONCEPT_ASSERT((_I_s_G_r_o_u_p_<_G_>)); // TODO(frank): check additive │ │ │ │ │ -171 BOOST_CONCEPT_ASSERT((_I_s_G_r_o_u_p_<_H_>)); // TODO(frank): check additive │ │ │ │ │ -172 │ │ │ │ │ -173 const G& g() const { return this->first; } │ │ │ │ │ -174 const H& h() const { return this->second;} │ │ │ │ │ -175 │ │ │ │ │ -176public: │ │ │ │ │ -_1_7_8 _D_i_r_e_c_t_S_u_m():std::pair(_t_r_a_i_t_s::Identity(),_t_r_a_i_t_s::Identity()) {} │ │ │ │ │ -179 │ │ │ │ │ -180 // Construct from two subgroup elements │ │ │ │ │ -181 _D_i_r_e_c_t_S_u_m(const G& g, const H& h):std::pair(g,h) {} │ │ │ │ │ -182 │ │ │ │ │ -183 // identity │ │ │ │ │ -184 static _D_i_r_e_c_t_S_u_m Identity() { return _D_i_r_e_c_t_S_u_m(); } │ │ │ │ │ -185 │ │ │ │ │ -186 _D_i_r_e_c_t_S_u_m operator+(const _D_i_r_e_c_t_S_u_m& other) const { │ │ │ │ │ -187 return _D_i_r_e_c_t_S_u_m(g()+other.g(), h()+other.h()); │ │ │ │ │ -188 } │ │ │ │ │ -189 _D_i_r_e_c_t_S_u_m operator-(const _D_i_r_e_c_t_S_u_m& other) const { │ │ │ │ │ -190 return _D_i_r_e_c_t_S_u_m(g()-other.g(), h()-other.h()); │ │ │ │ │ -191 } │ │ │ │ │ -192 _D_i_r_e_c_t_S_u_m operator-() const { │ │ │ │ │ -193 return _D_i_r_e_c_t_S_u_m(- g(), - h()); │ │ │ │ │ -194 } │ │ │ │ │ -195}; │ │ │ │ │ -196 │ │ │ │ │ -197// Define direct sums to be a model of the Additive Group concept │ │ │ │ │ -198template │ │ │ │ │ -_1_9_9struct _t_r_a_i_t_s<_D_i_r_e_c_t_S_u_m > : │ │ │ │ │ -200 internal::AdditiveGroupTraits > {}; │ │ │ │ │ -201 │ │ │ │ │ -202} // namespace gtsam │ │ │ │ │ -203 │ │ │ │ │ -_2_1_2#define GTSAM_CONCEPT_GROUP_INST(T) template class gtsam::IsGroup; │ │ │ │ │ -213#define GTSAM_CONCEPT_GROUP_TYPE(T) typedef gtsam::IsGroup │ │ │ │ │ -_gtsam_IsGroup_##T; │ │ │ │ │ -_T_e_s_t_a_b_l_e_._h │ │ │ │ │ -Concept check for values that can be used in unit tests. │ │ │ │ │ +75 // Local │ │ │ │ │ +76 EXPECT(_a_s_s_e_r_t___e_q_u_a_l(w12, T::Local(t1, t2, H1, H2))); │ │ │ │ │ +77 EXPECT(_a_s_s_e_r_t___e_q_u_a_l(numericalDerivative41(T::Local, t1, t2, │ │ │ │ │ +none, none), H1)); │ │ │ │ │ +78 EXPECT(_a_s_s_e_r_t___e_q_u_a_l(numericalDerivative42(T::Local, t1, t2, │ │ │ │ │ +none, none), H2)); │ │ │ │ │ +79} │ │ │ │ │ +80} // namespace gtsam │ │ │ │ │ +81 │ │ │ │ │ +82#define CHECK_LIE_GROUP_DERIVATIVES(t1,t2) \ │ │ │ │ │ +83 { gtsam::testLieGroupDerivatives(result_, name_, t1, t2); } │ │ │ │ │ +84 │ │ │ │ │ +85#define CHECK_CHART_DERIVATIVES(t1,t2) \ │ │ │ │ │ +86 { gtsam::testChartDerivatives(result_, name_, t1, t2); } │ │ │ │ │ +_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_._h │ │ │ │ │ +Some functions to compute numerical derivatives. │ │ │ │ │ +_L_i_e_._h │ │ │ │ │ +Base class and basic functions for Lie types. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_B_O_O_S_T___C_O_N_C_E_P_T___R_E_Q_U_I_R_E_S │ │ │ │ │ -BOOST_CONCEPT_REQUIRES(((IsGroup< G >)),(bool)) check_group_invariants(const G │ │ │ │ │ -&a │ │ │ │ │ -Check invariants. │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_g_r_o_u_p___t_a_g │ │ │ │ │ -tag to assert a type is a group │ │ │ │ │ -DDeeffiinniittiioonn Group.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_m_u_l_t_i_p_l_i_c_a_t_i_v_e___g_r_o_u_p___t_a_g │ │ │ │ │ -Group operator syntax flavors. │ │ │ │ │ -DDeeffiinniittiioonn Group.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_a_d_d_i_t_i_v_e___g_r_o_u_p___t_a_g │ │ │ │ │ -DDeeffiinniittiioonn Group.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_I_s_G_r_o_u_p │ │ │ │ │ -Group Concept. │ │ │ │ │ -DDeeffiinniittiioonn Group.h:46 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_r_e_c_t_P_r_o_d_u_c_t │ │ │ │ │ -DDeeffiinniittiioonn Group.h:138 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_r_e_c_t_P_r_o_d_u_c_t_:_:_D_i_r_e_c_t_P_r_o_d_u_c_t │ │ │ │ │ -DirectProduct() │ │ │ │ │ -Default constructor yields identity. │ │ │ │ │ -DDeeffiinniittiioonn Group.h:144 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_r_e_c_t_S_u_m │ │ │ │ │ -Template to construct the direct sum of two additive groups Assumes existence │ │ │ │ │ -of three additive opera... │ │ │ │ │ -DDeeffiinniittiioonn Group.h:169 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_r_e_c_t_S_u_m_:_:_D_i_r_e_c_t_S_u_m │ │ │ │ │ -DirectSum() │ │ │ │ │ -Default constructor yields identity. │ │ │ │ │ -DDeeffiinniittiioonn Group.h:178 │ │ │ │ │ +_g_t_s_a_m_:_:_a_s_s_e_r_t___e_q_u_a_l │ │ │ │ │ +bool assert_equal(const Matrix &expected, const Matrix &actual, double tol) │ │ │ │ │ +equals with an tolerance, prints out message if unequal │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:43 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _G_r_o_u_p_._h │ │ │ │ │ + * tteessttLLiiee..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00101.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/FastVector.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/VerticalBlockMatrix.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,48 +94,36 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Typedefs
│ │ │ │ -
FastVector.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
VerticalBlockMatrix.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ +

A matrix with column blocks of pre-defined sizes. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Typedefs

template<typename T >
using gtsam::FastVector = std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type >
 FastVector is a type alias to a std::vector with a custom memory allocator.
 
│ │ │ │

Detailed Description

│ │ │ │ -

A thin wrapper around std::vector that uses a custom allocator.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ -
Date
Feb 9, 2011
│ │ │ │ +

A matrix with column blocks of pre-defined sizes.

│ │ │ │ +

Used in JacobianFactor and GaussianConditional.

Author
Richard Roberts
│ │ │ │ +
Date
Sep 18, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,31 +1,23 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ -FastVector.h File Reference │ │ │ │ │ -A thin wrapper around std::vector that uses a custom allocator. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +VerticalBlockMatrix.cpp File Reference │ │ │ │ │ +A matrix with column blocks of pre-defined sizes. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -template │ │ │ │ │ -using  _g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r = std::vector< T, typename _i_n_t_e_r_n_a_l_:_: │ │ │ │ │ - _F_a_s_t_D_e_f_a_u_l_t_V_e_c_t_o_r_A_l_l_o_c_a_t_o_r< T >::type > │ │ │ │ │ -  FastVector is a type alias to a std::vector with a custom memory │ │ │ │ │ - allocator. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ +A matrix with column blocks of pre-defined sizes. │ │ │ │ │ +Used in _J_a_c_o_b_i_a_n_F_a_c_t_o_r and _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l. │ │ │ │ │ Author │ │ │ │ │ Richard Roberts │ │ │ │ │ - Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - Feb 9, 2011 │ │ │ │ │ + Sep 18, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ + * _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00104.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Manifold.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/cholesky.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,86 +94,48 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ -Macros | │ │ │ │ Functions
│ │ │ │ -
Manifold.h File Reference
│ │ │ │ +
cholesky.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Base class and basic functions for Manifold types. │ │ │ │ +

Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained Cholesky. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

struct  gtsam::manifold_tag
 tag to assert a type is a manifold More...
 
struct  gtsam::internal::HasManifoldPrereqs< Class >
 Requirements on type to pass it to Manifold template below. More...
 
struct  gtsam::internal::GetDimensionImpl< Class, N >
 Extra manifold traits for fixed-dimension types. More...
 
struct  gtsam::internal::GetDimensionImpl< Class, Eigen::Dynamic >
 Extra manifold traits for variable-dimension types. More...
 
struct  gtsam::internal::ManifoldTraits< Class >
 A helper that implements the traits interface for GTSAM manifolds. More...
 
struct  gtsam::internal::Manifold< Class >
 Both ManifoldTraits and Testable. More...
 
struct  gtsam::FixedDimension< T >
 Give fixed size dimension of a type, fails at compile time if dynamic. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Macros

│ │ │ │ -#define GTSAM_CONCEPT_MANIFOLD_INST(T)   template class gtsam::IsManifold<T>;
 ‍**
 
│ │ │ │ -#define GTSAM_CONCEPT_MANIFOLD_TYPE(T)   using _gtsam_IsManifold_##T = gtsam::IsManifold<T>;
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

│ │ │ │ -template<typename T >
 gtsam::BOOST_CONCEPT_REQUIRES (((IsTestable< T >)),(bool)) check_manifold_invariants(const T &a
 Check invariants for Manifold type.
 
pair< size_t, bool > gtsam::choleskyCareful (Matrix &ATA, int order=-1)
 "Careful" Cholesky computes the positive square-root of a positive symmetric semi-definite matrix (i.e.
 
bool gtsam::choleskyPartial (Matrix &ABC, size_t nFrontal, size_t topleft=0)
 Partial Cholesky computes a factor [R S such that [R' 0 [R S = [A B 0 L] S' I] 0 L] B' C].
 
│ │ │ │

Detailed Description

│ │ │ │ -

Base class and basic functions for Manifold types.

│ │ │ │ -
Author
Alex Cunningham
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ -
│ │ │ │ -Mike Bosse
│ │ │ │ +

Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained Cholesky.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Nov 5, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,60 +1,35 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Manifold.h File Reference │ │ │ │ │ -Base class and basic functions for Manifold types. _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +cholesky.h File Reference │ │ │ │ │ +Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained │ │ │ │ │ +Cholesky. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_m_a_n_i_f_o_l_d___t_a_g │ │ │ │ │ -  tag to assert a type is a manifold _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_H_a_s_M_a_n_i_f_o_l_d_P_r_e_r_e_q_s_<_ _C_l_a_s_s_ _> │ │ │ │ │ -  Requirements on type to pass it to _M_a_n_i_f_o_l_d template below. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_G_e_t_D_i_m_e_n_s_i_o_n_I_m_p_l_<_ _C_l_a_s_s_,_ _N_ _> │ │ │ │ │ -  Extra manifold traits for fixed-dimension types. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_G_e_t_D_i_m_e_n_s_i_o_n_I_m_p_l_<_ _C_l_a_s_s_,_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_ _> │ │ │ │ │ -  Extra manifold traits for variable-dimension types. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d_T_r_a_i_t_s_<_ _C_l_a_s_s_ _> │ │ │ │ │ -  A helper that implements the traits interface for GTSAM manifolds. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d_<_ _C_l_a_s_s_ _> │ │ │ │ │ -  Both _M_a_n_i_f_o_l_d_T_r_a_i_t_s and _T_e_s_t_a_b_l_e. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_F_i_x_e_d_D_i_m_e_n_s_i_o_n_<_ _T_ _> │ │ │ │ │ -  Give fixed size dimension of a type, fails at compile time if dynamic. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -MMaaccrrooss │ │ │ │ │ -#define  GGTTSSAAMM__CCOONNCCEEPPTT__MMAANNIIFFOOLLDD__IINNSSTT(T)   template class gtsam::IsManifold; │ │ │ │ │ -  ‍** │ │ │ │ │ -  │ │ │ │ │ -#define  GGTTSSAAMM__CCOONNCCEEPPTT__MMAANNIIFFOOLLDD__TTYYPPEE(T)   using _gtsam_IsManifold_##T = gtsam:: │ │ │ │ │ - IsManifold; │ │ │ │ │ -  │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ -  ggttssaamm::::BBOOOOSSTT__CCOONNCCEEPPTT__RREEQQUUIIRREESS (((_I_s_T_e_s_t_a_b_l_e< T >)),(bool)) │ │ │ │ │ - check_manifold_invariants(const T &a │ │ │ │ │ -  Check invariants for Manifold type. │ │ │ │ │ +pair< size_t, bool >  _g_t_s_a_m_:_:_c_h_o_l_e_s_k_y_C_a_r_e_f_u_l (Matrix &ATA, int order=-1) │ │ │ │ │ +  "Careful" Cholesky computes the positive square-root of a │ │ │ │ │ + positive symmetric semi-definite matrix (i.e. │ │ │ │ │ +  │ │ │ │ │ + bool  _g_t_s_a_m_:_:_c_h_o_l_e_s_k_y_P_a_r_t_i_a_l (Matrix &ABC, size_t nFrontal, │ │ │ │ │ + size_t topleft=0) │ │ │ │ │ +  Partial Cholesky computes a factor [R S such that [R' 0 │ │ │ │ │ + [R S = [A B 0 L] S' I] 0 L] B' C]. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Base class and basic functions for Manifold types. │ │ │ │ │ +Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained │ │ │ │ │ +Cholesky. │ │ │ │ │ Author │ │ │ │ │ - Alex Cunningham │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Mike Bosse │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Nov 5, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _M_a_n_i_f_o_l_d_._h │ │ │ │ │ + * _c_h_o_l_e_s_k_y_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00104.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,11 +1,4 @@ │ │ │ │ │ var a00104 = [ │ │ │ │ │ - ["gtsam::manifold_tag", "a02388.html", null], │ │ │ │ │ - ["gtsam::internal::HasManifoldPrereqs< Class >", "a02392.html", null], │ │ │ │ │ - ["gtsam::internal::GetDimensionImpl< Class, N >", "a02396.html", null], │ │ │ │ │ - ["gtsam::internal::GetDimensionImpl< Class, Eigen::Dynamic >", "a02400.html", null], │ │ │ │ │ - ["gtsam::internal::ManifoldTraits< Class >", "a02404.html", null], │ │ │ │ │ - ["gtsam::internal::Manifold< Class >", "a02408.html", null], │ │ │ │ │ - ["gtsam::FixedDimension< T >", "a02412.html", null], │ │ │ │ │ - ["GTSAM_CONCEPT_MANIFOLD_INST", "a00104.html#a64b5e2e3c812d2cdea5ee294100f825f", null], │ │ │ │ │ - ["BOOST_CONCEPT_REQUIRES", "a00104.html#ae03a6aedf0a82f67c2f3e987fb3aacba", null] │ │ │ │ │ + ["choleskyCareful", "a00104.html#a747683f736c50bca16b3aab0e95b1b76", null], │ │ │ │ │ + ["choleskyPartial", "a00104.html#abe82fa6aceccfa2360cf314ab44f39ad", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00104_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Manifold.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/cholesky.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,189 +98,48 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Manifold.h
│ │ │ │ +
cholesky.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/base/Matrix.h>
│ │ │ │
21
│ │ │ │ -
22#include <gtsam/base/Matrix.h>
│ │ │ │ -
23#include <gtsam/base/Testable.h>
│ │ │ │ -
24#include <gtsam/base/OptionalJacobian.h>
│ │ │ │ -
25
│ │ │ │ -
26#include <boost/concept_check.hpp>
│ │ │ │ -
27#include <boost/concept/requires.hpp>
│ │ │ │ -
28#include <boost/type_traits/is_base_of.hpp>
│ │ │ │ -
29
│ │ │ │ -
30namespace gtsam {
│ │ │ │ -
31
│ │ │ │ -
33struct manifold_tag {};
│ │ │ │ -
34
│ │ │ │ -
53template <typename T> struct traits;
│ │ │ │ -
54
│ │ │ │ -
55namespace internal {
│ │ │ │ -
56
│ │ │ │ -
58template<class Class>
│ │ │ │ -
│ │ │ │ -
59struct HasManifoldPrereqs {
│ │ │ │ -
60
│ │ │ │ -
61 enum { dim = Class::dimension };
│ │ │ │ -
62
│ │ │ │ -
63 Class p, q;
│ │ │ │ -
64 Eigen::Matrix<double, dim, 1> v;
│ │ │ │ -
65 OptionalJacobian<dim, dim> Hp, Hq, Hv;
│ │ │ │ -
66
│ │ │ │ -
67 BOOST_CONCEPT_USAGE(HasManifoldPrereqs) {
│ │ │ │ -
68 v = p.localCoordinates(q);
│ │ │ │ -
69 q = p.retract(v);
│ │ │ │ -
70 }
│ │ │ │ -
71};
│ │ │ │ -
│ │ │ │ -
72
│ │ │ │ -
74template<class Class, int N>
│ │ │ │ -
│ │ │ │ -
75struct GetDimensionImpl {
│ │ │ │ -
76 // Compile-time dimensionality
│ │ │ │ -
77 static int GetDimension(const Class&) {
│ │ │ │ -
78 return N;
│ │ │ │ -
79 }
│ │ │ │ -
80};
│ │ │ │ -
│ │ │ │ -
81
│ │ │ │ -
83template<class Class>
│ │ │ │ -
│ │ │ │ -
84struct GetDimensionImpl<Class, Eigen::Dynamic> {
│ │ │ │ -
85 // Run-time dimensionality
│ │ │ │ -
86 static int GetDimension(const Class& m) {
│ │ │ │ -
87 return m.dim();
│ │ │ │ -
88 }
│ │ │ │ -
89};
│ │ │ │ -
│ │ │ │ -
90
│ │ │ │ -
94template<class Class>
│ │ │ │ -
│ │ │ │ -
95struct ManifoldTraits: GetDimensionImpl<Class, Class::dimension> {
│ │ │ │ -
96
│ │ │ │ -
97 // Check that Class has the necessary machinery
│ │ │ │ -
98 BOOST_CONCEPT_ASSERT((HasManifoldPrereqs<Class>));
│ │ │ │ -
99
│ │ │ │ -
100 // Dimension of the manifold
│ │ │ │ -
101 enum { dimension = Class::dimension };
│ │ │ │ -
102
│ │ │ │ -
103 // Typedefs required by all manifold types.
│ │ │ │ -
104 typedef Class ManifoldType;
│ │ │ │ -
105 typedef manifold_tag structure_category;
│ │ │ │ -
106 typedef Eigen::Matrix<double, dimension, 1> TangentVector;
│ │ │ │ -
107
│ │ │ │ -
108 // Local coordinates
│ │ │ │ -
109 static TangentVector Local(const Class& origin, const Class& other) {
│ │ │ │ -
110 return origin.localCoordinates(other);
│ │ │ │ -
111 }
│ │ │ │ -
112
│ │ │ │ -
113 // Retraction back to manifold
│ │ │ │ -
114 static Class Retract(const Class& origin, const TangentVector& v) {
│ │ │ │ -
115 return origin.retract(v);
│ │ │ │ -
116 }
│ │ │ │ -
117};
│ │ │ │ -
│ │ │ │ -
118
│ │ │ │ -
120template<class Class> struct Manifold: ManifoldTraits<Class>, Testable<Class> {};
│ │ │ │ -
121
│ │ │ │ -
122} // \ namespace internal
│ │ │ │ -
123
│ │ │ │ -
125template<typename T>
│ │ │ │ -
126BOOST_CONCEPT_REQUIRES(((IsTestable<T>)),(bool)) //
│ │ │ │ -
127check_manifold_invariants(const T& a, const T& b, double tol=1e-9) {
│ │ │ │ -
128 typename traits<T>::TangentVector v0 = traits<T>::Local(a,a);
│ │ │ │ -
129 typename traits<T>::TangentVector v = traits<T>::Local(a,b);
│ │ │ │ -
130 T c = traits<T>::Retract(a,v);
│ │ │ │ -
131 return v0.norm() < tol && traits<T>::Equals(b,c,tol);
│ │ │ │ -
132}
│ │ │ │ -
133
│ │ │ │ -
135template<typename T>
│ │ │ │ -
136class IsManifold {
│ │ │ │ -
137
│ │ │ │ -
138public:
│ │ │ │ -
139
│ │ │ │ -
140 typedef typename traits<T>::structure_category structure_category_tag;
│ │ │ │ -
141 static const int dim = traits<T>::dimension;
│ │ │ │ -
142 typedef typename traits<T>::ManifoldType ManifoldType;
│ │ │ │ -
143 typedef typename traits<T>::TangentVector TangentVector;
│ │ │ │ -
144
│ │ │ │ -
145 BOOST_CONCEPT_USAGE(IsManifold) {
│ │ │ │ -
146 BOOST_STATIC_ASSERT_MSG(
│ │ │ │ -
147 (boost::is_base_of<manifold_tag, structure_category_tag>::value),
│ │ │ │ -
148 "This type's structure_category trait does not assert it as a manifold (or derived)");
│ │ │ │ -
149 BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim);
│ │ │ │ -
150
│ │ │ │ -
151 // make sure Chart methods are defined
│ │ │ │ -
152 v = traits<T>::Local(p, q);
│ │ │ │ -
153 q = traits<T>::Retract(p, v);
│ │ │ │ -
154 }
│ │ │ │ -
155
│ │ │ │ -
156private:
│ │ │ │ -
157
│ │ │ │ -
158 TangentVector v;
│ │ │ │ -
159 ManifoldType p, q;
│ │ │ │ -
160};
│ │ │ │ -
161
│ │ │ │ -
163template<typename T>
│ │ │ │ -
│ │ │ │ -
164struct FixedDimension {
│ │ │ │ -
165 typedef const int value_type;
│ │ │ │ -
166 static const int value = traits<T>::dimension;
│ │ │ │ -
167 BOOST_STATIC_ASSERT_MSG(value != Eigen::Dynamic,
│ │ │ │ -
168 "FixedDimension instantiated for dymanically-sized type.");
│ │ │ │ -
169};
│ │ │ │ -
│ │ │ │ -
170} // \ namespace gtsam
│ │ │ │ -
171
│ │ │ │ -
173// * Macros for using the ManifoldConcept
│ │ │ │ -
174// * - An instantiation for use inside unit tests
│ │ │ │ -
175// * - A typedef for use inside generic algorithms
│ │ │ │ -
176// *
│ │ │ │ -
177// * NOTE: intentionally not in the gtsam namespace to allow for classes not in
│ │ │ │ -
178// * the gtsam namespace to be more easily enforced as testable
│ │ │ │ -
179// */
│ │ │ │ -
180#define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::IsManifold<T>;
│ │ │ │ -
181#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) using _gtsam_IsManifold_##T = gtsam::IsManifold<T>;
│ │ │ │ -
OptionalJacobian.h
Special class for optional Jacobian arguments.
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ -
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
22namespace gtsam {
│ │ │ │ +
23
│ │ │ │ +
47GTSAM_EXPORT std::pair<size_t,bool> choleskyCareful(Matrix& ATA, int order = -1);
│ │ │ │ +
48
│ │ │ │ +
62GTSAM_EXPORT bool choleskyPartial(Matrix& ABC, size_t nFrontal, size_t topleft=0);
│ │ │ │ +
63
│ │ │ │ +
64}
│ │ │ │ +
65
│ │ │ │ +
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::BOOST_CONCEPT_REQUIRES
BOOST_CONCEPT_REQUIRES(((IsGroup< G >)),(bool)) check_group_invariants(const G &a
Check invariants.
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::manifold_tag
tag to assert a type is a manifold
Definition Manifold.h:33
│ │ │ │ -
gtsam::internal::HasManifoldPrereqs
Requirements on type to pass it to Manifold template below.
Definition Manifold.h:59
│ │ │ │ -
gtsam::internal::GetDimensionImpl
Extra manifold traits for fixed-dimension types.
Definition Manifold.h:75
│ │ │ │ -
gtsam::internal::ManifoldTraits
A helper that implements the traits interface for GTSAM manifolds.
Definition Manifold.h:95
│ │ │ │ -
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
│ │ │ │ -
gtsam::FixedDimension
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition Manifold.h:164
│ │ │ │ -
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
gtsam::IsTestable
A testable concept check that should be placed in applicable unit tests and in generic algorithms.
Definition Testable.h:58
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::choleskyCareful
pair< size_t, bool > choleskyCareful(Matrix &ATA, int order)
"Careful" Cholesky computes the positive square-root of a positive symmetric semi-definite matrix (i....
Definition cholesky.cpp:76
│ │ │ │ +
gtsam::choleskyPartial
bool choleskyPartial(Matrix &ABC, size_t nFrontal, size_t topleft)
Partial Cholesky computes a factor [R S such that [R' 0 [R S = [A B 0 L] S' I] 0 L] B' C].
Definition cholesky.cpp:108
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,206 +1,53 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Manifold.h │ │ │ │ │ +cholesky.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ 21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_b_a_s_e_/_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h> │ │ │ │ │ -25 │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ -28#include │ │ │ │ │ -29 │ │ │ │ │ -30namespace _g_t_s_a_m { │ │ │ │ │ -31 │ │ │ │ │ -_3_3struct _m_a_n_i_f_o_l_d___t_a_g {}; │ │ │ │ │ -34 │ │ │ │ │ -53template struct _t_r_a_i_t_s; │ │ │ │ │ -54 │ │ │ │ │ -55namespace internal { │ │ │ │ │ -56 │ │ │ │ │ -58template │ │ │ │ │ -_5_9struct _H_a_s_M_a_n_i_f_o_l_d_P_r_e_r_e_q_s { │ │ │ │ │ -60 │ │ │ │ │ -61 enum { dim = Class::dimension }; │ │ │ │ │ -62 │ │ │ │ │ -63 Class p, q; │ │ │ │ │ -64 Eigen::Matrix v; │ │ │ │ │ -65 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_d_i_m_,_ _d_i_m_> Hp, Hq, Hv; │ │ │ │ │ -66 │ │ │ │ │ -67 BOOST_CONCEPT_USAGE(_H_a_s_M_a_n_i_f_o_l_d_P_r_e_r_e_q_s) { │ │ │ │ │ -68 v = p.localCoordinates(q); │ │ │ │ │ -69 q = p.retract(v); │ │ │ │ │ -70 } │ │ │ │ │ -71}; │ │ │ │ │ -72 │ │ │ │ │ -74template │ │ │ │ │ -_7_5struct _G_e_t_D_i_m_e_n_s_i_o_n_I_m_p_l { │ │ │ │ │ -76 // Compile-time dimensionality │ │ │ │ │ -77 static int GetDimension(const Class&) { │ │ │ │ │ -78 return N; │ │ │ │ │ -79 } │ │ │ │ │ -80}; │ │ │ │ │ -81 │ │ │ │ │ -83template │ │ │ │ │ -_8_4struct _G_e_t_D_i_m_e_n_s_i_o_n_I_m_p_l { │ │ │ │ │ -85 // Run-time dimensionality │ │ │ │ │ -86 static int GetDimension(const Class& m) { │ │ │ │ │ -87 return m.dim(); │ │ │ │ │ -88 } │ │ │ │ │ -89}; │ │ │ │ │ -90 │ │ │ │ │ -94template │ │ │ │ │ -_9_5struct _M_a_n_i_f_o_l_d_T_r_a_i_t_s: _G_e_t_D_i_m_e_n_s_i_o_n_I_m_p_l { │ │ │ │ │ -96 │ │ │ │ │ -97 // Check that Class has the necessary machinery │ │ │ │ │ -98 BOOST_CONCEPT_ASSERT((_H_a_s_M_a_n_i_f_o_l_d_P_r_e_r_e_q_s_<_C_l_a_s_s_>)); │ │ │ │ │ -99 │ │ │ │ │ -100 // Dimension of the manifold │ │ │ │ │ -101 enum { dimension = Class::dimension }; │ │ │ │ │ -102 │ │ │ │ │ -103 // Typedefs required by all manifold types. │ │ │ │ │ -104 typedef Class ManifoldType; │ │ │ │ │ -105 typedef _m_a_n_i_f_o_l_d___t_a_g _s_t_r_u_c_t_u_r_e___c_a_t_e_g_o_r_y; │ │ │ │ │ -106 typedef Eigen::Matrix TangentVector; │ │ │ │ │ -107 │ │ │ │ │ -108 // Local coordinates │ │ │ │ │ -109 static TangentVector Local(const Class& origin, const Class& other) { │ │ │ │ │ -110 return origin.localCoordinates(other); │ │ │ │ │ -111 } │ │ │ │ │ -112 │ │ │ │ │ -113 // Retraction back to manifold │ │ │ │ │ -114 static Class Retract(const Class& origin, const TangentVector& v) { │ │ │ │ │ -115 return origin.retract(v); │ │ │ │ │ -116 } │ │ │ │ │ -117}; │ │ │ │ │ -118 │ │ │ │ │ -_1_2_0template struct _M_a_n_i_f_o_l_d: _M_a_n_i_f_o_l_d_T_r_a_i_t_s, │ │ │ │ │ -_T_e_s_t_a_b_l_e {}; │ │ │ │ │ -121 │ │ │ │ │ -122} // \ namespace internal │ │ │ │ │ -123 │ │ │ │ │ -125template │ │ │ │ │ -_1_2_6_B_O_O_S_T___C_O_N_C_E_P_T___R_E_Q_U_I_R_E_S(((_I_s_T_e_s_t_a_b_l_e_<_T_>)),(bool)) // │ │ │ │ │ -127check_manifold_invariants(const T& a, const T& b, double tol=1e-9) { │ │ │ │ │ -128 typename _t_r_a_i_t_s_<_T_>_:_:_T_a_n_g_e_n_t_V_e_c_t_o_r v0 = _t_r_a_i_t_s_<_T_>_:_:_L_o_c_a_l(a,a); │ │ │ │ │ -129 typename _t_r_a_i_t_s_<_T_>_:_:_T_a_n_g_e_n_t_V_e_c_t_o_r v = _t_r_a_i_t_s_<_T_>_:_:_L_o_c_a_l(a,b); │ │ │ │ │ -130 T c = _t_r_a_i_t_s_<_T_>_:_:_R_e_t_r_a_c_t(a,v); │ │ │ │ │ -131 return v0.norm() < tol && _t_r_a_i_t_s_<_T_>_:_:_E_q_u_a_l_s(b,c,tol); │ │ │ │ │ -132} │ │ │ │ │ -133 │ │ │ │ │ -135template │ │ │ │ │ -136class IsManifold { │ │ │ │ │ -137 │ │ │ │ │ -138public: │ │ │ │ │ -139 │ │ │ │ │ -140 typedef typename traits::structure_category structure_category_tag; │ │ │ │ │ -141 static const int dim = traits::dimension; │ │ │ │ │ -142 typedef typename traits::ManifoldType ManifoldType; │ │ │ │ │ -143 typedef typename traits::TangentVector TangentVector; │ │ │ │ │ -144 │ │ │ │ │ -145 BOOST_CONCEPT_USAGE(IsManifold) { │ │ │ │ │ -146 BOOST_STATIC_ASSERT_MSG( │ │ │ │ │ -147 (boost::is_base_of::value), │ │ │ │ │ -148 "This type's structure_category trait does not assert it as a manifold (or │ │ │ │ │ -derived)"); │ │ │ │ │ -149 BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); │ │ │ │ │ -150 │ │ │ │ │ -151 // make sure Chart methods are defined │ │ │ │ │ -152 v = traits::Local(p, q); │ │ │ │ │ -153 q = traits::Retract(p, v); │ │ │ │ │ -154 } │ │ │ │ │ -155 │ │ │ │ │ -156private: │ │ │ │ │ -157 │ │ │ │ │ -158 TangentVector v; │ │ │ │ │ -159 ManifoldType p, q; │ │ │ │ │ -160}; │ │ │ │ │ -161 │ │ │ │ │ -163template │ │ │ │ │ -_1_6_4struct _F_i_x_e_d_D_i_m_e_n_s_i_o_n { │ │ │ │ │ -165 typedef const int value_type; │ │ │ │ │ -166 static const int value = _t_r_a_i_t_s_<_T_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ -167 BOOST_STATIC_ASSERT_MSG(value != Eigen::Dynamic, │ │ │ │ │ -168 "FixedDimension instantiated for dymanically-sized type."); │ │ │ │ │ -169}; │ │ │ │ │ -170} // \ namespace gtsam │ │ │ │ │ -171 │ │ │ │ │ -173// * Macros for using the ManifoldConcept │ │ │ │ │ -174// * - An instantiation for use inside unit tests │ │ │ │ │ -175// * - A typedef for use inside generic algorithms │ │ │ │ │ -176// * │ │ │ │ │ -177// * NOTE: intentionally not in the gtsam namespace to allow for classes not │ │ │ │ │ -in │ │ │ │ │ -178// * the gtsam namespace to be more easily enforced as testable │ │ │ │ │ -179// */ │ │ │ │ │ -_1_8_0#define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::IsManifold; │ │ │ │ │ -181#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) using _gtsam_IsManifold_##T = gtsam:: │ │ │ │ │ -IsManifold; │ │ │ │ │ -_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h │ │ │ │ │ -Special class for optional Jacobian arguments. │ │ │ │ │ -_T_e_s_t_a_b_l_e_._h │ │ │ │ │ -Concept check for values that can be used in unit tests. │ │ │ │ │ +22namespace _g_t_s_a_m { │ │ │ │ │ +23 │ │ │ │ │ +47GTSAM_EXPORT std::pair _c_h_o_l_e_s_k_y_C_a_r_e_f_u_l(Matrix& ATA, int order = │ │ │ │ │ +-1); │ │ │ │ │ +48 │ │ │ │ │ +62GTSAM_EXPORT bool _c_h_o_l_e_s_k_y_P_a_r_t_i_a_l(Matrix& ABC, size_t nFrontal, size_t │ │ │ │ │ +topleft=0); │ │ │ │ │ +63 │ │ │ │ │ +64} │ │ │ │ │ +65 │ │ │ │ │ _M_a_t_r_i_x_._h │ │ │ │ │ typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_B_O_O_S_T___C_O_N_C_E_P_T___R_E_Q_U_I_R_E_S │ │ │ │ │ -BOOST_CONCEPT_REQUIRES(((IsGroup< G >)),(bool)) check_group_invariants(const G │ │ │ │ │ -&a │ │ │ │ │ -Check invariants. │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_m_a_n_i_f_o_l_d___t_a_g │ │ │ │ │ -tag to assert a type is a manifold │ │ │ │ │ -DDeeffiinniittiioonn Manifold.h:33 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_H_a_s_M_a_n_i_f_o_l_d_P_r_e_r_e_q_s │ │ │ │ │ -Requirements on type to pass it to Manifold template below. │ │ │ │ │ -DDeeffiinniittiioonn Manifold.h:59 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_G_e_t_D_i_m_e_n_s_i_o_n_I_m_p_l │ │ │ │ │ -Extra manifold traits for fixed-dimension types. │ │ │ │ │ -DDeeffiinniittiioonn Manifold.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d_T_r_a_i_t_s │ │ │ │ │ -A helper that implements the traits interface for GTSAM manifolds. │ │ │ │ │ -DDeeffiinniittiioonn Manifold.h:95 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ -Both ManifoldTraits and Testable. │ │ │ │ │ -DDeeffiinniittiioonn Manifold.h:120 │ │ │ │ │ -_g_t_s_a_m_:_:_F_i_x_e_d_D_i_m_e_n_s_i_o_n │ │ │ │ │ -Give fixed size dimension of a type, fails at compile time if dynamic. │ │ │ │ │ -DDeeffiinniittiioonn Manifold.h:164 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ -either a fixed size o... │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_I_s_T_e_s_t_a_b_l_e │ │ │ │ │ -A testable concept check that should be placed in applicable unit tests and in │ │ │ │ │ -generic algorithms. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_c_h_o_l_e_s_k_y_C_a_r_e_f_u_l │ │ │ │ │ +pair< size_t, bool > choleskyCareful(Matrix &ATA, int order) │ │ │ │ │ +"Careful" Cholesky computes the positive square-root of a positive symmetric │ │ │ │ │ +semi-definite matrix (i.... │ │ │ │ │ +DDeeffiinniittiioonn cholesky.cpp:76 │ │ │ │ │ +_g_t_s_a_m_:_:_c_h_o_l_e_s_k_y_P_a_r_t_i_a_l │ │ │ │ │ +bool choleskyPartial(Matrix &ABC, size_t nFrontal, size_t topleft) │ │ │ │ │ +Partial Cholesky computes a factor [R S such that [R' 0 [R S = [A B 0 L] S' I] │ │ │ │ │ +0 L] B' C]. │ │ │ │ │ +DDeeffiinniittiioonn cholesky.cpp:108 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _M_a_n_i_f_o_l_d_._h │ │ │ │ │ + * _c_h_o_l_e_s_k_y_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00107_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/VectorSpace.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Manifold.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,487 +98,189 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
VectorSpace.h
│ │ │ │ +
Manifold.h
│ │ │ │
│ │ │ │
│ │ │ │ -
1/*
│ │ │ │ -
2 * VectorSpace.h
│ │ │ │ -
3 *
│ │ │ │ -
4 * @date December 21, 2014
│ │ │ │ -
5 * @author Mike Bosse
│ │ │ │ -
6 * @author Frank Dellaert
│ │ │ │ -
7 */
│ │ │ │ -
8
│ │ │ │ -
9#pragma once
│ │ │ │ -
10
│ │ │ │ -
11#include <gtsam/base/Lie.h>
│ │ │ │ -
12
│ │ │ │ -
13namespace gtsam {
│ │ │ │ -
14
│ │ │ │ -
│ │ │ │ -
16struct vector_space_tag: public lie_group_tag {
│ │ │ │ -
17};
│ │ │ │ -
│ │ │ │ -
18
│ │ │ │ -
19template<typename T> struct traits;
│ │ │ │ -
20
│ │ │ │ -
21namespace internal {
│ │ │ │ -
22
│ │ │ │ -
24template<class Class, int N>
│ │ │ │ -
│ │ │ │ -
25struct VectorSpaceImpl {
│ │ │ │ -
26
│ │ │ │ -
29 typedef Eigen::Matrix<double, N, 1> TangentVector;
│ │ │ │ -
30 typedef OptionalJacobian<N, N> ChartJacobian;
│ │ │ │ -
31 typedef Eigen::Matrix<double, N, N> Jacobian;
│ │ │ │ -
32 static int GetDimension(const Class&) { return N;}
│ │ │ │ -
33
│ │ │ │ -
34 static TangentVector Local(const Class& origin, const Class& other,
│ │ │ │ -
35 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ -
36 if (H1) *H1 = - Jacobian::Identity();
│ │ │ │ -
37 if (H2) *H2 = Jacobian::Identity();
│ │ │ │ -
38 Class v = other-origin;
│ │ │ │ -
39 return v.vector();
│ │ │ │ -
40 }
│ │ │ │ -
41
│ │ │ │ -
42 static Class Retract(const Class& origin, const TangentVector& v,
│ │ │ │ -
43 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ -
44 if (H1) *H1 = Jacobian::Identity();
│ │ │ │ -
45 if (H2) *H2 = Jacobian::Identity();
│ │ │ │ -
46 return origin + v;
│ │ │ │ -
47 }
│ │ │ │ -
48
│ │ │ │ -
50
│ │ │ │ -
53
│ │ │ │ -
54 static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) {
│ │ │ │ -
55 if (Hm) *Hm = Jacobian::Identity();
│ │ │ │ -
56 return m.vector();
│ │ │ │ -
57 }
│ │ │ │ -
58
│ │ │ │ -
59 static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
│ │ │ │ -
60 if (Hv) *Hv = Jacobian::Identity();
│ │ │ │ -
61 return Class(v);
│ │ │ │ -
62 }
│ │ │ │ -
63
│ │ │ │ -
64 static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1 = boost::none,
│ │ │ │ -
65 ChartJacobian H2 = boost::none) {
│ │ │ │ -
66 if (H1) *H1 = Jacobian::Identity();
│ │ │ │ -
67 if (H2) *H2 = Jacobian::Identity();
│ │ │ │ -
68 return v1 + v2;
│ │ │ │ -
69 }
│ │ │ │ -
70
│ │ │ │ -
71 static Class Between(const Class& v1, const Class& v2, ChartJacobian H1 = boost::none,
│ │ │ │ -
72 ChartJacobian H2 = boost::none) {
│ │ │ │ -
73 if (H1) *H1 = - Jacobian::Identity();
│ │ │ │ -
74 if (H2) *H2 = Jacobian::Identity();
│ │ │ │ -
75 return v2 - v1;
│ │ │ │ -
76 }
│ │ │ │ -
77
│ │ │ │ -
78 static Class Inverse(const Class& v, ChartJacobian H = boost::none) {
│ │ │ │ -
79 if (H) *H = - Jacobian::Identity();
│ │ │ │ -
80 return -v;
│ │ │ │ -
81 }
│ │ │ │ -
82
│ │ │ │ -
84};
│ │ │ │ -
│ │ │ │ -
85
│ │ │ │ -
87template<class Class>
│ │ │ │ -
│ │ │ │ -
88struct VectorSpaceImpl<Class,Eigen::Dynamic> {
│ │ │ │ -
89
│ │ │ │ -
92 static Class Compose(const Class& v1, const Class& v2) { return v1+v2;}
│ │ │ │ -
93 static Class Between(const Class& v1, const Class& v2) { return v2-v1;}
│ │ │ │ -
94 static Class Inverse(const Class& m) { return -m;}
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +
2
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
4 * Atlanta, Georgia 30332-0415
│ │ │ │ +
5 * All Rights Reserved
│ │ │ │ +
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
7
│ │ │ │ +
8 * See LICENSE for the license information
│ │ │ │ +
9
│ │ │ │ +
10 * -------------------------------------------------------------------------- */
│ │ │ │ +
11
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/base/Matrix.h>
│ │ │ │ +
23#include <gtsam/base/Testable.h>
│ │ │ │ +
24#include <gtsam/base/OptionalJacobian.h>
│ │ │ │ +
25
│ │ │ │ +
26#include <boost/concept_check.hpp>
│ │ │ │ +
27#include <boost/concept/requires.hpp>
│ │ │ │ +
28#include <boost/type_traits/is_base_of.hpp>
│ │ │ │ +
29
│ │ │ │ +
30namespace gtsam {
│ │ │ │ +
31
│ │ │ │ +
33struct manifold_tag {};
│ │ │ │ +
34
│ │ │ │ +
53template <typename T> struct traits;
│ │ │ │ +
54
│ │ │ │ +
55namespace internal {
│ │ │ │ +
56
│ │ │ │ +
58template<class Class>
│ │ │ │ +
│ │ │ │ +
59struct HasManifoldPrereqs {
│ │ │ │ +
60
│ │ │ │ +
61 enum { dim = Class::dimension };
│ │ │ │ +
62
│ │ │ │ +
63 Class p, q;
│ │ │ │ +
64 Eigen::Matrix<double, dim, 1> v;
│ │ │ │ +
65 OptionalJacobian<dim, dim> Hp, Hq, Hv;
│ │ │ │ +
66
│ │ │ │ +
67 BOOST_CONCEPT_USAGE(HasManifoldPrereqs) {
│ │ │ │ +
68 v = p.localCoordinates(q);
│ │ │ │ +
69 q = p.retract(v);
│ │ │ │ +
70 }
│ │ │ │ +
71};
│ │ │ │ +
│ │ │ │ +
72
│ │ │ │ +
74template<class Class, int N>
│ │ │ │ +
│ │ │ │ +
75struct GetDimensionImpl {
│ │ │ │ +
76 // Compile-time dimensionality
│ │ │ │ +
77 static int GetDimension(const Class&) {
│ │ │ │ +
78 return N;
│ │ │ │ +
79 }
│ │ │ │ +
80};
│ │ │ │ +
│ │ │ │ +
81
│ │ │ │ +
83template<class Class>
│ │ │ │ +
│ │ │ │ +
84struct GetDimensionImpl<Class, Eigen::Dynamic> {
│ │ │ │ +
85 // Run-time dimensionality
│ │ │ │ +
86 static int GetDimension(const Class& m) {
│ │ │ │ +
87 return m.dim();
│ │ │ │ +
88 }
│ │ │ │ +
89};
│ │ │ │ +
│ │ │ │ +
90
│ │ │ │ +
94template<class Class>
│ │ │ │ +
│ │ │ │ +
95struct ManifoldTraits: GetDimensionImpl<Class, Class::dimension> {
│ │ │ │
96
│ │ │ │ -
99 typedef Eigen::VectorXd TangentVector;
│ │ │ │ -
100 typedef OptionalJacobian<Eigen::Dynamic,Eigen::Dynamic> ChartJacobian;
│ │ │ │ -
101 static int GetDimension(const Class& m) { return m.dim();}
│ │ │ │ +
97 // Check that Class has the necessary machinery
│ │ │ │ +
98 BOOST_CONCEPT_ASSERT((HasManifoldPrereqs<Class>));
│ │ │ │ +
99
│ │ │ │ +
100 // Dimension of the manifold
│ │ │ │ +
101 enum { dimension = Class::dimension };
│ │ │ │
102
│ │ │ │ -
103 static Eigen::MatrixXd Eye(const Class& m) {
│ │ │ │ -
104 int dim = GetDimension(m);
│ │ │ │ -
105 return Eigen::MatrixXd::Identity(dim, dim);
│ │ │ │ -
106 }
│ │ │ │ +
103 // Typedefs required by all manifold types.
│ │ │ │ +
104 typedef Class ManifoldType;
│ │ │ │ +
105 typedef manifold_tag structure_category;
│ │ │ │ +
106 typedef Eigen::Matrix<double, dimension, 1> TangentVector;
│ │ │ │
107
│ │ │ │ -
108 static TangentVector Local(const Class& origin, const Class& other,
│ │ │ │ -
109 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ -
110 if (H1) *H1 = - Eye(origin);
│ │ │ │ -
111 if (H2) *H2 = Eye(other);
│ │ │ │ -
112 Class v = other-origin;
│ │ │ │ -
113 return v.vector();
│ │ │ │ -
114 }
│ │ │ │ -
115
│ │ │ │ -
116 static Class Retract(const Class& origin, const TangentVector& v,
│ │ │ │ -
117 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ -
118 if (H1) *H1 = Eye(origin);
│ │ │ │ -
119 if (H2) *H2 = Eye(origin);
│ │ │ │ -
120 return origin + v;
│ │ │ │ -
121 }
│ │ │ │ -
122
│ │ │ │ -
124
│ │ │ │ -
127
│ │ │ │ -
128 static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) {
│ │ │ │ -
129 if (Hm) *Hm = Eye(m);
│ │ │ │ -
130 return m.vector();
│ │ │ │ -
131 }
│ │ │ │ -
132
│ │ │ │ -
133 static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
│ │ │ │ -
134 Class result(v);
│ │ │ │ -
135 if (Hv)
│ │ │ │ -
136 *Hv = Eye(v);
│ │ │ │ -
137 return result;
│ │ │ │ -
138 }
│ │ │ │ +
108 // Local coordinates
│ │ │ │ +
109 static TangentVector Local(const Class& origin, const Class& other) {
│ │ │ │ +
110 return origin.localCoordinates(other);
│ │ │ │ +
111 }
│ │ │ │ +
112
│ │ │ │ +
113 // Retraction back to manifold
│ │ │ │ +
114 static Class Retract(const Class& origin, const TangentVector& v) {
│ │ │ │ +
115 return origin.retract(v);
│ │ │ │ +
116 }
│ │ │ │ +
117};
│ │ │ │ +
│ │ │ │ +
118
│ │ │ │ +
120template<class Class> struct Manifold: ManifoldTraits<Class>, Testable<Class> {};
│ │ │ │ +
121
│ │ │ │ +
122} // \ namespace internal
│ │ │ │ +
123
│ │ │ │ +
125template<typename T>
│ │ │ │ +
126BOOST_CONCEPT_REQUIRES(((IsTestable<T>)),(bool)) //
│ │ │ │ +
127check_manifold_invariants(const T& a, const T& b, double tol=1e-9) {
│ │ │ │ +
128 typename traits<T>::TangentVector v0 = traits<T>::Local(a,a);
│ │ │ │ +
129 typename traits<T>::TangentVector v = traits<T>::Local(a,b);
│ │ │ │ +
130 T c = traits<T>::Retract(a,v);
│ │ │ │ +
131 return v0.norm() < tol && traits<T>::Equals(b,c,tol);
│ │ │ │ +
132}
│ │ │ │ +
133
│ │ │ │ +
135template<typename T>
│ │ │ │ +
136class IsManifold {
│ │ │ │ +
137
│ │ │ │ +
138public:
│ │ │ │
139
│ │ │ │ -
140 static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1,
│ │ │ │ -
141 ChartJacobian H2 = boost::none) {
│ │ │ │ -
142 if (H1) *H1 = Eye(v1);
│ │ │ │ -
143 if (H2) *H2 = Eye(v2);
│ │ │ │ -
144 return v1 + v2;
│ │ │ │ -
145 }
│ │ │ │ -
146
│ │ │ │ -
147 static Class Between(const Class& v1, const Class& v2, ChartJacobian H1,
│ │ │ │ -
148 ChartJacobian H2 = boost::none) {
│ │ │ │ -
149 if (H1) *H1 = - Eye(v1);
│ │ │ │ -
150 if (H2) *H2 = Eye(v2);
│ │ │ │ -
151 return v2 - v1;
│ │ │ │ -
152 }
│ │ │ │ -
153
│ │ │ │ -
154 static Class Inverse(const Class& v, ChartJacobian H) {
│ │ │ │ -
155 if (H) *H = -Eye(v);
│ │ │ │ -
156 return -v;
│ │ │ │ -
157 }
│ │ │ │ -
158
│ │ │ │ +
140 typedef typename traits<T>::structure_category structure_category_tag;
│ │ │ │ +
141 static const int dim = traits<T>::dimension;
│ │ │ │ +
142 typedef typename traits<T>::ManifoldType ManifoldType;
│ │ │ │ +
143 typedef typename traits<T>::TangentVector TangentVector;
│ │ │ │ +
144
│ │ │ │ +
145 BOOST_CONCEPT_USAGE(IsManifold) {
│ │ │ │ +
146 BOOST_STATIC_ASSERT_MSG(
│ │ │ │ +
147 (boost::is_base_of<manifold_tag, structure_category_tag>::value),
│ │ │ │ +
148 "This type's structure_category trait does not assert it as a manifold (or derived)");
│ │ │ │ +
149 BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim);
│ │ │ │ +
150
│ │ │ │ +
151 // make sure Chart methods are defined
│ │ │ │ +
152 v = traits<T>::Local(p, q);
│ │ │ │ +
153 q = traits<T>::Retract(p, v);
│ │ │ │ +
154 }
│ │ │ │ +
155
│ │ │ │ +
156private:
│ │ │ │ +
157
│ │ │ │ +
158 TangentVector v;
│ │ │ │ +
159 ManifoldType p, q;
│ │ │ │
160};
│ │ │ │ -
│ │ │ │
161
│ │ │ │ -
163template<class Class>
│ │ │ │ +
163template<typename T>
│ │ │ │
│ │ │ │ -
164struct HasVectorSpacePrereqs {
│ │ │ │ -
165
│ │ │ │ -
166 enum { dim = Class::dimension };
│ │ │ │ -
167
│ │ │ │ -
168 Class p, q;
│ │ │ │ -
169 Vector v;
│ │ │ │ -
170
│ │ │ │ -
171 BOOST_CONCEPT_USAGE(HasVectorSpacePrereqs) {
│ │ │ │ -
172 p = Class::Identity(); // identity
│ │ │ │ -
173 q = p + p; // addition
│ │ │ │ -
174 q = p - p; // subtraction
│ │ │ │ -
175 v = p.vector(); // conversion to vector
│ │ │ │ -
176 q = p + v; // addition of a vector on the right
│ │ │ │ -
177 }
│ │ │ │ -
178};
│ │ │ │ -
│ │ │ │ -
179
│ │ │ │ -
184template<class Class>
│ │ │ │ -
│ │ │ │ -
185struct VectorSpaceTraits: VectorSpaceImpl<Class, Class::dimension> {
│ │ │ │ -
186
│ │ │ │ -
187 // Check that Class has the necessary machinery
│ │ │ │ -
188 BOOST_CONCEPT_ASSERT((HasVectorSpacePrereqs<Class>));
│ │ │ │ -
189
│ │ │ │ -
190 typedef vector_space_tag structure_category;
│ │ │ │ -
191
│ │ │ │ -
194 typedef additive_group_tag group_flavor;
│ │ │ │ -
195 static Class Identity() { return Class::Identity();}
│ │ │ │ -
197
│ │ │ │ -
200 enum { dimension = Class::dimension};
│ │ │ │ -
201 typedef Class ManifoldType;
│ │ │ │ -
203};
│ │ │ │ -
│ │ │ │ -
204
│ │ │ │ -
206template<class Class>
│ │ │ │ -
207struct VectorSpace: Testable<Class>, VectorSpaceTraits<Class> {};
│ │ │ │ -
208
│ │ │ │ -
211template<typename Scalar>
│ │ │ │ -
│ │ │ │ -
212struct ScalarTraits : VectorSpaceImpl<Scalar, 1> {
│ │ │ │ -
213
│ │ │ │ -
214 typedef vector_space_tag structure_category;
│ │ │ │ -
215
│ │ │ │ -
218 static void Print(Scalar m, const std::string& str = "") {
│ │ │ │ -
219 gtsam::print(m, str);
│ │ │ │ -
220 }
│ │ │ │ -
221 static bool Equals(Scalar v1, Scalar v2, double tol = 1e-8) {
│ │ │ │ -
222 return std::abs(v1 - v2) < tol;
│ │ │ │ -
223 }
│ │ │ │ -
225
│ │ │ │ -
228 typedef additive_group_tag group_flavor;
│ │ │ │ -
229 static Scalar Identity() { return 0;}
│ │ │ │ -
231
│ │ │ │ -
234 typedef Scalar ManifoldType;
│ │ │ │ -
235 enum { dimension = 1 };
│ │ │ │ -
236 typedef Eigen::Matrix<double, 1, 1> TangentVector;
│ │ │ │ -
237 typedef OptionalJacobian<1, 1> ChartJacobian;
│ │ │ │ -
238
│ │ │ │ -
239 static TangentVector Local(Scalar origin, Scalar other,
│ │ │ │ -
240 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ -
241 if (H1) (*H1)[0] = -1.0;
│ │ │ │ -
242 if (H2) (*H2)[0] = 1.0;
│ │ │ │ -
243 TangentVector result;
│ │ │ │ -
244 result(0) = other - origin;
│ │ │ │ -
245 return result;
│ │ │ │ -
246 }
│ │ │ │ -
247
│ │ │ │ -
248 static Scalar Retract(Scalar origin, const TangentVector& v,
│ │ │ │ -
249 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ -
250 if (H1) (*H1)[0] = 1.0;
│ │ │ │ -
251 if (H2) (*H2)[0] = 1.0;
│ │ │ │ -
252 return origin + v[0];
│ │ │ │ -
253 }
│ │ │ │ -
255
│ │ │ │ -
258 static TangentVector Logmap(Scalar m, ChartJacobian H = boost::none) {
│ │ │ │ -
259 if (H) (*H)[0] = 1.0;
│ │ │ │ -
260 return Local(0, m);
│ │ │ │ -
261 }
│ │ │ │ -
262
│ │ │ │ -
263 static Scalar Expmap(const TangentVector& v, ChartJacobian H = boost::none) {
│ │ │ │ -
264 if (H) (*H)[0] = 1.0;
│ │ │ │ -
265 return v[0];
│ │ │ │ -
266 }
│ │ │ │ -
268
│ │ │ │ -
269};
│ │ │ │ -
│ │ │ │ -
270
│ │ │ │ -
271} // namespace internal
│ │ │ │ -
272
│ │ │ │ -
│ │ │ │ -
274template<> struct traits<double> : public internal::ScalarTraits<double> {
│ │ │ │ -
275};
│ │ │ │ -
│ │ │ │ -
276
│ │ │ │ -
│ │ │ │ -
278template<> struct traits<float> : public internal::ScalarTraits<float> {
│ │ │ │ -
279};
│ │ │ │ -
│ │ │ │ -
280
│ │ │ │ -
281// traits for any fixed double Eigen matrix
│ │ │ │ -
282template<int M, int N, int Options, int MaxRows, int MaxCols>
│ │ │ │ -
│ │ │ │ -
283struct traits<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > :
│ │ │ │ -
284 internal::VectorSpaceImpl<
│ │ │ │ -
285 Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols>, M * N> {
│ │ │ │ -
286
│ │ │ │ -
287 typedef vector_space_tag structure_category;
│ │ │ │ -
288 typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> Fixed;
│ │ │ │ -
289
│ │ │ │ -
292 static void Print(const Fixed& m, const std::string& str = "") {
│ │ │ │ -
293 gtsam::print(Eigen::MatrixXd(m), str);
│ │ │ │ -
294 }
│ │ │ │ -
295 static bool Equals(const Fixed& v1, const Fixed& v2, double tol = 1e-8) {
│ │ │ │ -
296 return equal_with_abs_tol(v1, v2, tol);
│ │ │ │ -
297 }
│ │ │ │ -
299
│ │ │ │ -
302 typedef additive_group_tag group_flavor;
│ │ │ │ -
303 static Fixed Identity() { return Fixed::Zero();}
│ │ │ │ -
305
│ │ │ │ -
308 enum { dimension = M*N};
│ │ │ │ -
309 typedef Fixed ManifoldType;
│ │ │ │ -
310 typedef Eigen::Matrix<double, dimension, 1> TangentVector;
│ │ │ │ -
311 typedef Eigen::Matrix<double, dimension, dimension> Jacobian;
│ │ │ │ -
312 typedef OptionalJacobian<dimension, dimension> ChartJacobian;
│ │ │ │ -
313
│ │ │ │ -
314 static TangentVector Local(const Fixed& origin, const Fixed& other,
│ │ │ │ -
315 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ -
316 if (H1) (*H1) = -Jacobian::Identity();
│ │ │ │ -
317 if (H2) (*H2) = Jacobian::Identity();
│ │ │ │ -
318 TangentVector result;
│ │ │ │ -
319 Eigen::Map<Fixed>(result.data()) = other - origin;
│ │ │ │ -
320 return result;
│ │ │ │ -
321 }
│ │ │ │ -
322
│ │ │ │ -
323 static Fixed Retract(const Fixed& origin, const TangentVector& v,
│ │ │ │ -
324 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ -
325 if (H1) (*H1) = Jacobian::Identity();
│ │ │ │ -
326 if (H2) (*H2) = Jacobian::Identity();
│ │ │ │ -
327 return origin + Eigen::Map<const Fixed>(v.data());
│ │ │ │ -
328 }
│ │ │ │ -
330
│ │ │ │ -
333 static TangentVector Logmap(const Fixed& m, ChartJacobian H = boost::none) {
│ │ │ │ -
334 if (H) *H = Jacobian::Identity();
│ │ │ │ -
335 TangentVector result;
│ │ │ │ -
336 Eigen::Map<Fixed>(result.data()) = m;
│ │ │ │ -
337 return result;
│ │ │ │ -
338 }
│ │ │ │ -
339
│ │ │ │ -
340 static Fixed Expmap(const TangentVector& v, ChartJacobian H = boost::none) {
│ │ │ │ -
341 Fixed m;
│ │ │ │ -
342 m.setZero();
│ │ │ │ -
343 if (H) *H = Jacobian::Identity();
│ │ │ │ -
344 return m + Eigen::Map<const Fixed>(v.data());
│ │ │ │ -
345 }
│ │ │ │ -
347};
│ │ │ │ -
│ │ │ │ -
348
│ │ │ │ -
349
│ │ │ │ -
350namespace internal {
│ │ │ │ -
351
│ │ │ │ -
352// traits for dynamic Eigen matrices
│ │ │ │ -
353template<int M, int N, int Options, int MaxRows, int MaxCols>
│ │ │ │ -
│ │ │ │ -
354struct DynamicTraits {
│ │ │ │ -
355
│ │ │ │ -
356 typedef vector_space_tag structure_category;
│ │ │ │ -
357 typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> Dynamic;
│ │ │ │ -
358
│ │ │ │ -
361 static void Print(const Dynamic& m, const std::string& str = "") {
│ │ │ │ -
362 gtsam::print(Eigen::MatrixXd(m), str);
│ │ │ │ -
363 }
│ │ │ │ -
364 static bool Equals(const Dynamic& v1, const Dynamic& v2,
│ │ │ │ -
365 double tol = 1e-8) {
│ │ │ │ -
366 return equal_with_abs_tol(v1, v2, tol);
│ │ │ │ -
367 }
│ │ │ │ -
369
│ │ │ │ -
372 typedef additive_group_tag group_flavor;
│ │ │ │ -
373 static Dynamic Identity() {
│ │ │ │ -
374 throw std::runtime_error("Identity not defined for dynamic types");
│ │ │ │ -
375 }
│ │ │ │ -
377
│ │ │ │ -
380 enum { dimension = Eigen::Dynamic };
│ │ │ │ -
381 typedef Eigen::VectorXd TangentVector;
│ │ │ │ -
382 typedef Eigen::MatrixXd Jacobian;
│ │ │ │ -
383 typedef OptionalJacobian<dimension, dimension> ChartJacobian;
│ │ │ │ -
384 typedef Dynamic ManifoldType;
│ │ │ │ -
385
│ │ │ │ -
386 static int GetDimension(const Dynamic& m) {
│ │ │ │ -
387 return m.rows() * m.cols();
│ │ │ │ -
388 }
│ │ │ │ -
389
│ │ │ │ -
390 static Jacobian Eye(const Dynamic& m) {
│ │ │ │ -
391 int dim = GetDimension(m);
│ │ │ │ -
392 return Eigen::Matrix<double, dimension, dimension>::Identity(dim, dim);
│ │ │ │ -
393 }
│ │ │ │ -
394
│ │ │ │ -
395 static TangentVector Local(const Dynamic& m, const Dynamic& other, //
│ │ │ │ -
396 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ -
397 if (H1) *H1 = -Eye(m);
│ │ │ │ -
398 if (H2) *H2 = Eye(m);
│ │ │ │ -
399 TangentVector v(GetDimension(m));
│ │ │ │ -
400 Eigen::Map<Dynamic>(v.data(), m.rows(), m.cols()) = other - m;
│ │ │ │ -
401 return v;
│ │ │ │ -
402 }
│ │ │ │ -
403
│ │ │ │ -
404 static Dynamic Retract(const Dynamic& m, const TangentVector& v, //
│ │ │ │ -
405 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ -
406 if (H1) *H1 = Eye(m);
│ │ │ │ -
407 if (H2) *H2 = Eye(m);
│ │ │ │ -
408 return m + Eigen::Map<const Dynamic>(v.data(), m.rows(), m.cols());
│ │ │ │ -
409 }
│ │ │ │ -
411
│ │ │ │ -
414 static TangentVector Logmap(const Dynamic& m, ChartJacobian H = boost::none) {
│ │ │ │ -
415 if (H) *H = Eye(m);
│ │ │ │ -
416 TangentVector result(GetDimension(m));
│ │ │ │ -
417 Eigen::Map<Dynamic>(result.data(), m.cols(), m.rows()) = m;
│ │ │ │ -
418 return result;
│ │ │ │ -
419 }
│ │ │ │ -
420
│ │ │ │ -
421 static Dynamic Expmap(const TangentVector& /*v*/, ChartJacobian H = boost::none) {
│ │ │ │ -
422 static_cast<void>(H);
│ │ │ │ -
423 throw std::runtime_error("Expmap not defined for dynamic types");
│ │ │ │ -
424 }
│ │ │ │ -
425
│ │ │ │ -
426 static Dynamic Inverse(const Dynamic& m, ChartJacobian H = boost::none) {
│ │ │ │ -
427 if (H) *H = -Eye(m);
│ │ │ │ -
428 return -m;
│ │ │ │ -
429 }
│ │ │ │ -
430
│ │ │ │ -
431 static Dynamic Compose(const Dynamic& v1, const Dynamic& v2,
│ │ │ │ -
432 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ -
433 if (H1) *H1 = Eye(v1);
│ │ │ │ -
434 if (H2) *H2 = Eye(v1);
│ │ │ │ -
435 return v1 + v2;
│ │ │ │ -
436 }
│ │ │ │ -
437
│ │ │ │ -
438 static Dynamic Between(const Dynamic& v1, const Dynamic& v2,
│ │ │ │ -
439 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ -
440 if (H1) *H1 = -Eye(v1);
│ │ │ │ -
441 if (H2) *H2 = Eye(v1);
│ │ │ │ -
442 return v2 - v1;
│ │ │ │ -
443 }
│ │ │ │ -
445
│ │ │ │ -
446};
│ │ │ │ -
│ │ │ │ -
447
│ │ │ │ -
448} // \ internal
│ │ │ │ -
449
│ │ │ │ -
450// traits for fully dynamic matrix
│ │ │ │ -
451template<int Options, int MaxRows, int MaxCols>
│ │ │ │ -
│ │ │ │ -
452struct traits<Eigen::Matrix<double, -1, -1, Options, MaxRows, MaxCols> > :
│ │ │ │ -
453 public internal::DynamicTraits<-1, -1, Options, MaxRows, MaxCols> {
│ │ │ │ -
454};
│ │ │ │ -
│ │ │ │ -
455
│ │ │ │ -
456// traits for dynamic column vector
│ │ │ │ -
457template<int Options, int MaxRows, int MaxCols>
│ │ │ │ -
│ │ │ │ -
458struct traits<Eigen::Matrix<double, -1, 1, Options, MaxRows, MaxCols> > :
│ │ │ │ -
459 public internal::DynamicTraits<-1, 1, Options, MaxRows, MaxCols> {
│ │ │ │ -
460};
│ │ │ │ -
│ │ │ │ -
461
│ │ │ │ -
462// traits for dynamic row vector
│ │ │ │ -
463template<int Options, int MaxRows, int MaxCols>
│ │ │ │ -
│ │ │ │ -
464struct traits<Eigen::Matrix<double, 1, -1, Options, MaxRows, MaxCols> > :
│ │ │ │ -
465 public internal::DynamicTraits<1, -1, Options, MaxRows, MaxCols> {
│ │ │ │ -
466};
│ │ │ │ -
│ │ │ │ -
467
│ │ │ │ -
469template<typename T>
│ │ │ │ -
│ │ │ │ -
470class IsVectorSpace: public IsLieGroup<T> {
│ │ │ │ -
471public:
│ │ │ │ -
472
│ │ │ │ -
473 typedef typename traits<T>::structure_category structure_category_tag;
│ │ │ │ -
474
│ │ │ │ -
475 BOOST_CONCEPT_USAGE(IsVectorSpace) {
│ │ │ │ -
476 BOOST_STATIC_ASSERT_MSG(
│ │ │ │ -
477 (boost::is_base_of<vector_space_tag, structure_category_tag>::value),
│ │ │ │ -
478 "This type's trait does not assert it as a vector space (or derived)");
│ │ │ │ -
479 r = p + q;
│ │ │ │ -
480 r = -p;
│ │ │ │ -
481 r = p - q;
│ │ │ │ -
482 }
│ │ │ │ -
483
│ │ │ │ -
484private:
│ │ │ │ -
485 T p, q, r;
│ │ │ │ -
486};
│ │ │ │ -
│ │ │ │ -
487
│ │ │ │ -
488} // namespace gtsam
│ │ │ │ -
489
│ │ │ │ -
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ +
164struct FixedDimension {
│ │ │ │ +
165 typedef const int value_type;
│ │ │ │ +
166 static const int value = traits<T>::dimension;
│ │ │ │ +
167 BOOST_STATIC_ASSERT_MSG(value != Eigen::Dynamic,
│ │ │ │ +
168 "FixedDimension instantiated for dymanically-sized type.");
│ │ │ │ +
169};
│ │ │ │ +
│ │ │ │ +
170} // \ namespace gtsam
│ │ │ │ +
171
│ │ │ │ +
173// * Macros for using the ManifoldConcept
│ │ │ │ +
174// * - An instantiation for use inside unit tests
│ │ │ │ +
175// * - A typedef for use inside generic algorithms
│ │ │ │ +
176// *
│ │ │ │ +
177// * NOTE: intentionally not in the gtsam namespace to allow for classes not in
│ │ │ │ +
178// * the gtsam namespace to be more easily enforced as testable
│ │ │ │ +
179// */
│ │ │ │ +
180#define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::IsManifold<T>;
│ │ │ │ +
181#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) using _gtsam_IsManifold_##T = gtsam::IsManifold<T>;
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
OptionalJacobian.h
Special class for optional Jacobian arguments.
│ │ │ │ +
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition Matrix.h:81
│ │ │ │ +
gtsam::BOOST_CONCEPT_REQUIRES
BOOST_CONCEPT_REQUIRES(((IsGroup< G >)),(bool)) check_group_invariants(const G &a
Check invariants.
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::additive_group_tag
Definition Group.h:38
│ │ │ │ -
gtsam::lie_group_tag
tag to assert a type is a Lie group
Definition Lie.h:164
│ │ │ │ -
gtsam::IsLieGroup
Lie Group Concept.
Definition Lie.h:260
│ │ │ │ +
gtsam::manifold_tag
tag to assert a type is a manifold
Definition Manifold.h:33
│ │ │ │ +
gtsam::internal::HasManifoldPrereqs
Requirements on type to pass it to Manifold template below.
Definition Manifold.h:59
│ │ │ │ +
gtsam::internal::GetDimensionImpl
Extra manifold traits for fixed-dimension types.
Definition Manifold.h:75
│ │ │ │ +
gtsam::internal::ManifoldTraits
A helper that implements the traits interface for GTSAM manifolds.
Definition Manifold.h:95
│ │ │ │ +
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
│ │ │ │ +
gtsam::FixedDimension
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition Manifold.h:164
│ │ │ │
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
gtsam::OptionalJacobian::cols
OptionalJacobian< Rows, N > cols(int startCol)
Access M*N sub-block if we are allocated, otherwise none TODO(frank): this could work as is below if ...
Definition OptionalJacobian.h:171
│ │ │ │ - │ │ │ │ +
gtsam::IsTestable
A testable concept check that should be placed in applicable unit tests and in generic algorithms.
Definition Testable.h:58
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::vector_space_tag
tag to assert a type is a vector space
Definition VectorSpace.h:16
│ │ │ │ -
gtsam::internal::VectorSpaceImpl
VectorSpaceTraits Implementation for Fixed sizes.
Definition VectorSpace.h:25
│ │ │ │ -
gtsam::internal::HasVectorSpacePrereqs
Requirements on type to pass it to Manifold template below.
Definition VectorSpace.h:164
│ │ │ │ -
gtsam::internal::VectorSpaceTraits
A helper that implements the traits interface for classes that define vector spaces To use this for y...
Definition VectorSpace.h:185
│ │ │ │ -
gtsam::internal::VectorSpace
VectorSpace provides both Testable and VectorSpaceTraits.
Definition VectorSpace.h:207
│ │ │ │ -
gtsam::internal::ScalarTraits
A helper that implements the traits interface for scalar vector spaces.
Definition VectorSpace.h:212
│ │ │ │ -
gtsam::internal::DynamicTraits
Definition VectorSpace.h:354
│ │ │ │ -
gtsam::IsVectorSpace
Vector Space concept.
Definition VectorSpace.h:470
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,503 +1,206 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -VectorSpace.h │ │ │ │ │ -1/* │ │ │ │ │ -2 * VectorSpace.h │ │ │ │ │ -3 * │ │ │ │ │ -4 * @date December 21, 2014 │ │ │ │ │ -5 * @author Mike Bosse │ │ │ │ │ -6 * @author Frank Dellaert │ │ │ │ │ -7 */ │ │ │ │ │ -8 │ │ │ │ │ -9#pragma once │ │ │ │ │ -10 │ │ │ │ │ -11#include <_g_t_s_a_m_/_b_a_s_e_/_L_i_e_._h> │ │ │ │ │ -12 │ │ │ │ │ -13namespace _g_t_s_a_m { │ │ │ │ │ -14 │ │ │ │ │ -_1_6struct _v_e_c_t_o_r___s_p_a_c_e___t_a_g: public _l_i_e___g_r_o_u_p___t_a_g { │ │ │ │ │ -17}; │ │ │ │ │ -18 │ │ │ │ │ -19template struct _t_r_a_i_t_s; │ │ │ │ │ -20 │ │ │ │ │ -21namespace internal { │ │ │ │ │ -22 │ │ │ │ │ -24template │ │ │ │ │ -_2_5struct _V_e_c_t_o_r_S_p_a_c_e_I_m_p_l { │ │ │ │ │ -26 │ │ │ │ │ -29 typedef Eigen::Matrix TangentVector; │ │ │ │ │ -30 typedef _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_N_,_ _N_> ChartJacobian; │ │ │ │ │ -31 typedef Eigen::Matrix Jacobian; │ │ │ │ │ -32 static int GetDimension(const Class&) { return N;} │ │ │ │ │ -33 │ │ │ │ │ -34 static TangentVector Local(const Class& origin, const Class& other, │ │ │ │ │ -35 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { │ │ │ │ │ -36 if (H1) *H1 = - Jacobian::Identity(); │ │ │ │ │ -37 if (H2) *H2 = Jacobian::Identity(); │ │ │ │ │ -38 Class v = other-origin; │ │ │ │ │ -39 return v.vector(); │ │ │ │ │ -40 } │ │ │ │ │ -41 │ │ │ │ │ -42 static Class Retract(const Class& origin, const TangentVector& v, │ │ │ │ │ -43 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { │ │ │ │ │ -44 if (H1) *H1 = Jacobian::Identity(); │ │ │ │ │ -45 if (H2) *H2 = Jacobian::Identity(); │ │ │ │ │ -46 return origin + v; │ │ │ │ │ -47 } │ │ │ │ │ -48 │ │ │ │ │ -50 │ │ │ │ │ -53 │ │ │ │ │ -54 static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) │ │ │ │ │ -{ │ │ │ │ │ -55 if (Hm) *Hm = Jacobian::Identity(); │ │ │ │ │ -56 return m.vector(); │ │ │ │ │ -57 } │ │ │ │ │ -58 │ │ │ │ │ -59 static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) │ │ │ │ │ -{ │ │ │ │ │ -60 if (Hv) *Hv = Jacobian::Identity(); │ │ │ │ │ -61 return Class(v); │ │ │ │ │ -62 } │ │ │ │ │ -63 │ │ │ │ │ -64 static Class Compose(const Class& v1, const Class& v2, ChartJacobian H1 = │ │ │ │ │ -boost::none, │ │ │ │ │ -65 ChartJacobian H2 = boost::none) { │ │ │ │ │ -66 if (H1) *H1 = Jacobian::Identity(); │ │ │ │ │ -67 if (H2) *H2 = Jacobian::Identity(); │ │ │ │ │ -68 return v1 + v2; │ │ │ │ │ -69 } │ │ │ │ │ -70 │ │ │ │ │ -71 static Class Between(const Class& v1, const Class& v2, ChartJacobian H1 = │ │ │ │ │ -boost::none, │ │ │ │ │ -72 ChartJacobian H2 = boost::none) { │ │ │ │ │ -73 if (H1) *H1 = - Jacobian::Identity(); │ │ │ │ │ -74 if (H2) *H2 = Jacobian::Identity(); │ │ │ │ │ -75 return v2 - v1; │ │ │ │ │ -76 } │ │ │ │ │ -77 │ │ │ │ │ -78 static Class Inverse(const Class& v, ChartJacobian H = boost::none) { │ │ │ │ │ -79 if (H) *H = - Jacobian::Identity(); │ │ │ │ │ -80 return -v; │ │ │ │ │ -81 } │ │ │ │ │ -82 │ │ │ │ │ -84}; │ │ │ │ │ -85 │ │ │ │ │ -87template │ │ │ │ │ -_8_8struct _V_e_c_t_o_r_S_p_a_c_e_I_m_p_l { │ │ │ │ │ -89 │ │ │ │ │ -92 static Class Compose(const Class& v1, const Class& v2) { return v1+v2;} │ │ │ │ │ -93 static Class Between(const Class& v1, const Class& v2) { return v2-v1;} │ │ │ │ │ -94 static Class Inverse(const Class& m) { return -m;} │ │ │ │ │ +Manifold.h │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +1/* --------------------------------------------------------------------------- │ │ │ │ │ +- │ │ │ │ │ +2 │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +5 * All Rights Reserved │ │ │ │ │ +6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +7 │ │ │ │ │ +8 * See LICENSE for the license information │ │ │ │ │ +9 │ │ │ │ │ +10 * ------------------------------------------------------------------------- │ │ │ │ │ +- */ │ │ │ │ │ +11 │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_b_a_s_e_/_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h> │ │ │ │ │ +25 │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28#include │ │ │ │ │ +29 │ │ │ │ │ +30namespace _g_t_s_a_m { │ │ │ │ │ +31 │ │ │ │ │ +_3_3struct _m_a_n_i_f_o_l_d___t_a_g {}; │ │ │ │ │ +34 │ │ │ │ │ +53template struct _t_r_a_i_t_s; │ │ │ │ │ +54 │ │ │ │ │ +55namespace internal { │ │ │ │ │ +56 │ │ │ │ │ +58template │ │ │ │ │ +_5_9struct _H_a_s_M_a_n_i_f_o_l_d_P_r_e_r_e_q_s { │ │ │ │ │ +60 │ │ │ │ │ +61 enum { dim = Class::dimension }; │ │ │ │ │ +62 │ │ │ │ │ +63 Class p, q; │ │ │ │ │ +64 Eigen::Matrix v; │ │ │ │ │ +65 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_d_i_m_,_ _d_i_m_> Hp, Hq, Hv; │ │ │ │ │ +66 │ │ │ │ │ +67 BOOST_CONCEPT_USAGE(_H_a_s_M_a_n_i_f_o_l_d_P_r_e_r_e_q_s) { │ │ │ │ │ +68 v = p.localCoordinates(q); │ │ │ │ │ +69 q = p.retract(v); │ │ │ │ │ +70 } │ │ │ │ │ +71}; │ │ │ │ │ +72 │ │ │ │ │ +74template │ │ │ │ │ +_7_5struct _G_e_t_D_i_m_e_n_s_i_o_n_I_m_p_l { │ │ │ │ │ +76 // Compile-time dimensionality │ │ │ │ │ +77 static int GetDimension(const Class&) { │ │ │ │ │ +78 return N; │ │ │ │ │ +79 } │ │ │ │ │ +80}; │ │ │ │ │ +81 │ │ │ │ │ +83template │ │ │ │ │ +_8_4struct _G_e_t_D_i_m_e_n_s_i_o_n_I_m_p_l { │ │ │ │ │ +85 // Run-time dimensionality │ │ │ │ │ +86 static int GetDimension(const Class& m) { │ │ │ │ │ +87 return m.dim(); │ │ │ │ │ +88 } │ │ │ │ │ +89}; │ │ │ │ │ +90 │ │ │ │ │ +94template │ │ │ │ │ +_9_5struct _M_a_n_i_f_o_l_d_T_r_a_i_t_s: _G_e_t_D_i_m_e_n_s_i_o_n_I_m_p_l { │ │ │ │ │ 96 │ │ │ │ │ -99 typedef Eigen::VectorXd TangentVector; │ │ │ │ │ -100 typedef _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_E_i_g_e_n_:_:_D_y_n_a_m_i_c_,_E_i_g_e_n_:_:_D_y_n_a_m_i_c_> _C_h_a_r_t_J_a_c_o_b_i_a_n; │ │ │ │ │ -101 static int GetDimension(const Class& m) { return m.dim();} │ │ │ │ │ +97 // Check that Class has the necessary machinery │ │ │ │ │ +98 BOOST_CONCEPT_ASSERT((_H_a_s_M_a_n_i_f_o_l_d_P_r_e_r_e_q_s_<_C_l_a_s_s_>)); │ │ │ │ │ +99 │ │ │ │ │ +100 // Dimension of the manifold │ │ │ │ │ +101 enum { dimension = Class::dimension }; │ │ │ │ │ 102 │ │ │ │ │ -103 static Eigen::MatrixXd Eye(const Class& m) { │ │ │ │ │ -104 int dim = GetDimension(m); │ │ │ │ │ -105 return Eigen::MatrixXd::Identity(dim, dim); │ │ │ │ │ -106 } │ │ │ │ │ +103 // Typedefs required by all manifold types. │ │ │ │ │ +104 typedef Class ManifoldType; │ │ │ │ │ +105 typedef _m_a_n_i_f_o_l_d___t_a_g _s_t_r_u_c_t_u_r_e___c_a_t_e_g_o_r_y; │ │ │ │ │ +106 typedef Eigen::Matrix TangentVector; │ │ │ │ │ 107 │ │ │ │ │ -108 static TangentVector Local(const Class& origin, const Class& other, │ │ │ │ │ -109 _C_h_a_r_t_J_a_c_o_b_i_a_n H1 = boost::none, _C_h_a_r_t_J_a_c_o_b_i_a_n H2 = boost::none) { │ │ │ │ │ -110 if (H1) *H1 = - Eye(origin); │ │ │ │ │ -111 if (H2) *H2 = Eye(other); │ │ │ │ │ -112 Class v = other-origin; │ │ │ │ │ -113 return v.vector(); │ │ │ │ │ -114 } │ │ │ │ │ -115 │ │ │ │ │ -116 static Class Retract(const Class& origin, const TangentVector& v, │ │ │ │ │ -117 _C_h_a_r_t_J_a_c_o_b_i_a_n H1 = boost::none, _C_h_a_r_t_J_a_c_o_b_i_a_n H2 = boost::none) { │ │ │ │ │ -118 if (H1) *H1 = Eye(origin); │ │ │ │ │ -119 if (H2) *H2 = Eye(origin); │ │ │ │ │ -120 return origin + v; │ │ │ │ │ -121 } │ │ │ │ │ -122 │ │ │ │ │ -124 │ │ │ │ │ -127 │ │ │ │ │ -128 static TangentVector Logmap(const Class& m, _C_h_a_r_t_J_a_c_o_b_i_a_n Hm = boost::none) │ │ │ │ │ -{ │ │ │ │ │ -129 if (Hm) *Hm = Eye(m); │ │ │ │ │ -130 return m.vector(); │ │ │ │ │ -131 } │ │ │ │ │ -132 │ │ │ │ │ -133 static Class Expmap(const TangentVector& v, _C_h_a_r_t_J_a_c_o_b_i_a_n Hv = boost::none) │ │ │ │ │ -{ │ │ │ │ │ -134 Class result(v); │ │ │ │ │ -135 if (Hv) │ │ │ │ │ -136 *Hv = Eye(v); │ │ │ │ │ -137 return result; │ │ │ │ │ -138 } │ │ │ │ │ +108 // Local coordinates │ │ │ │ │ +109 static TangentVector Local(const Class& origin, const Class& other) { │ │ │ │ │ +110 return origin.localCoordinates(other); │ │ │ │ │ +111 } │ │ │ │ │ +112 │ │ │ │ │ +113 // Retraction back to manifold │ │ │ │ │ +114 static Class Retract(const Class& origin, const TangentVector& v) { │ │ │ │ │ +115 return origin.retract(v); │ │ │ │ │ +116 } │ │ │ │ │ +117}; │ │ │ │ │ +118 │ │ │ │ │ +_1_2_0template struct _M_a_n_i_f_o_l_d: _M_a_n_i_f_o_l_d_T_r_a_i_t_s, │ │ │ │ │ +_T_e_s_t_a_b_l_e {}; │ │ │ │ │ +121 │ │ │ │ │ +122} // \ namespace internal │ │ │ │ │ +123 │ │ │ │ │ +125template │ │ │ │ │ +_1_2_6_B_O_O_S_T___C_O_N_C_E_P_T___R_E_Q_U_I_R_E_S(((_I_s_T_e_s_t_a_b_l_e_<_T_>)),(bool)) // │ │ │ │ │ +127check_manifold_invariants(const T& a, const T& b, double tol=1e-9) { │ │ │ │ │ +128 typename _t_r_a_i_t_s_<_T_>_:_:_T_a_n_g_e_n_t_V_e_c_t_o_r v0 = _t_r_a_i_t_s_<_T_>_:_:_L_o_c_a_l(a,a); │ │ │ │ │ +129 typename _t_r_a_i_t_s_<_T_>_:_:_T_a_n_g_e_n_t_V_e_c_t_o_r v = _t_r_a_i_t_s_<_T_>_:_:_L_o_c_a_l(a,b); │ │ │ │ │ +130 T c = _t_r_a_i_t_s_<_T_>_:_:_R_e_t_r_a_c_t(a,v); │ │ │ │ │ +131 return v0.norm() < tol && _t_r_a_i_t_s_<_T_>_:_:_E_q_u_a_l_s(b,c,tol); │ │ │ │ │ +132} │ │ │ │ │ +133 │ │ │ │ │ +135template │ │ │ │ │ +136class IsManifold { │ │ │ │ │ +137 │ │ │ │ │ +138public: │ │ │ │ │ 139 │ │ │ │ │ -140 static Class Compose(const Class& v1, const Class& v2, _C_h_a_r_t_J_a_c_o_b_i_a_n H1, │ │ │ │ │ -141 _C_h_a_r_t_J_a_c_o_b_i_a_n H2 = boost::none) { │ │ │ │ │ -142 if (H1) *H1 = Eye(v1); │ │ │ │ │ -143 if (H2) *H2 = Eye(v2); │ │ │ │ │ -144 return v1 + v2; │ │ │ │ │ -145 } │ │ │ │ │ -146 │ │ │ │ │ -147 static Class Between(const Class& v1, const Class& v2, _C_h_a_r_t_J_a_c_o_b_i_a_n H1, │ │ │ │ │ -148 _C_h_a_r_t_J_a_c_o_b_i_a_n H2 = boost::none) { │ │ │ │ │ -149 if (H1) *H1 = - Eye(v1); │ │ │ │ │ -150 if (H2) *H2 = Eye(v2); │ │ │ │ │ -151 return v2 - v1; │ │ │ │ │ -152 } │ │ │ │ │ -153 │ │ │ │ │ -154 static Class Inverse(const Class& v, _C_h_a_r_t_J_a_c_o_b_i_a_n H) { │ │ │ │ │ -155 if (H) *H = -Eye(v); │ │ │ │ │ -156 return -v; │ │ │ │ │ -157 } │ │ │ │ │ -158 │ │ │ │ │ +140 typedef typename traits::structure_category structure_category_tag; │ │ │ │ │ +141 static const int dim = traits::dimension; │ │ │ │ │ +142 typedef typename traits::ManifoldType ManifoldType; │ │ │ │ │ +143 typedef typename traits::TangentVector TangentVector; │ │ │ │ │ +144 │ │ │ │ │ +145 BOOST_CONCEPT_USAGE(IsManifold) { │ │ │ │ │ +146 BOOST_STATIC_ASSERT_MSG( │ │ │ │ │ +147 (boost::is_base_of::value), │ │ │ │ │ +148 "This type's structure_category trait does not assert it as a manifold (or │ │ │ │ │ +derived)"); │ │ │ │ │ +149 BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); │ │ │ │ │ +150 │ │ │ │ │ +151 // make sure Chart methods are defined │ │ │ │ │ +152 v = traits::Local(p, q); │ │ │ │ │ +153 q = traits::Retract(p, v); │ │ │ │ │ +154 } │ │ │ │ │ +155 │ │ │ │ │ +156private: │ │ │ │ │ +157 │ │ │ │ │ +158 TangentVector v; │ │ │ │ │ +159 ManifoldType p, q; │ │ │ │ │ 160}; │ │ │ │ │ 161 │ │ │ │ │ -163template │ │ │ │ │ -_1_6_4struct _H_a_s_V_e_c_t_o_r_S_p_a_c_e_P_r_e_r_e_q_s { │ │ │ │ │ -165 │ │ │ │ │ -166 enum { dim = Class::dimension }; │ │ │ │ │ -167 │ │ │ │ │ -168 Class p, q; │ │ │ │ │ -169 Vector v; │ │ │ │ │ -170 │ │ │ │ │ -171 BOOST_CONCEPT_USAGE(_H_a_s_V_e_c_t_o_r_S_p_a_c_e_P_r_e_r_e_q_s) { │ │ │ │ │ -172 p = Class::Identity(); // identity │ │ │ │ │ -173 q = p + p; // addition │ │ │ │ │ -174 q = p - p; // subtraction │ │ │ │ │ -175 v = p.vector(); // conversion to vector │ │ │ │ │ -176 q = p + v; // addition of a vector on the right │ │ │ │ │ -177 } │ │ │ │ │ -178}; │ │ │ │ │ -179 │ │ │ │ │ -184template │ │ │ │ │ -_1_8_5struct _V_e_c_t_o_r_S_p_a_c_e_T_r_a_i_t_s: _V_e_c_t_o_r_S_p_a_c_e_I_m_p_l { │ │ │ │ │ -186 │ │ │ │ │ -187 // Check that Class has the necessary machinery │ │ │ │ │ -188 BOOST_CONCEPT_ASSERT((_H_a_s_V_e_c_t_o_r_S_p_a_c_e_P_r_e_r_e_q_s_<_C_l_a_s_s_>)); │ │ │ │ │ -189 │ │ │ │ │ -190 typedef _v_e_c_t_o_r___s_p_a_c_e___t_a_g _s_t_r_u_c_t_u_r_e___c_a_t_e_g_o_r_y; │ │ │ │ │ -191 │ │ │ │ │ -194 typedef _a_d_d_i_t_i_v_e___g_r_o_u_p___t_a_g _g_r_o_u_p___f_l_a_v_o_r; │ │ │ │ │ -195 static Class Identity() { return Class::Identity();} │ │ │ │ │ -197 │ │ │ │ │ -200 enum { dimension = Class::dimension}; │ │ │ │ │ -201 typedef Class ManifoldType; │ │ │ │ │ -203}; │ │ │ │ │ -204 │ │ │ │ │ -206template │ │ │ │ │ -_2_0_7struct _V_e_c_t_o_r_S_p_a_c_e: _T_e_s_t_a_b_l_e, _V_e_c_t_o_r_S_p_a_c_e_T_r_a_i_t_s {}; │ │ │ │ │ -208 │ │ │ │ │ -211template │ │ │ │ │ -_2_1_2struct _S_c_a_l_a_r_T_r_a_i_t_s : _V_e_c_t_o_r_S_p_a_c_e_I_m_p_l { │ │ │ │ │ -213 │ │ │ │ │ -214 typedef _v_e_c_t_o_r___s_p_a_c_e___t_a_g _s_t_r_u_c_t_u_r_e___c_a_t_e_g_o_r_y; │ │ │ │ │ -215 │ │ │ │ │ -218 static void Print(Scalar m, const std::string& str = "") { │ │ │ │ │ -219 _g_t_s_a_m_:_:_p_r_i_n_t(m, str); │ │ │ │ │ -220 } │ │ │ │ │ -221 static bool Equals(Scalar v1, Scalar v2, double tol = 1e-8) { │ │ │ │ │ -222 return std::abs(v1 - v2) < tol; │ │ │ │ │ -223 } │ │ │ │ │ -225 │ │ │ │ │ -228 typedef _a_d_d_i_t_i_v_e___g_r_o_u_p___t_a_g _g_r_o_u_p___f_l_a_v_o_r; │ │ │ │ │ -229 static Scalar Identity() { return 0;} │ │ │ │ │ -231 │ │ │ │ │ -234 typedef Scalar ManifoldType; │ │ │ │ │ -235 enum { dimension = 1 }; │ │ │ │ │ -236 typedef Eigen::Matrix TangentVector; │ │ │ │ │ -237 typedef _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _1_> ChartJacobian; │ │ │ │ │ -238 │ │ │ │ │ -239 static TangentVector Local(Scalar origin, Scalar other, │ │ │ │ │ -240 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { │ │ │ │ │ -241 if (H1) (*H1)[0] = -1.0; │ │ │ │ │ -242 if (H2) (*H2)[0] = 1.0; │ │ │ │ │ -243 TangentVector result; │ │ │ │ │ -244 result(0) = other - origin; │ │ │ │ │ -245 return result; │ │ │ │ │ -246 } │ │ │ │ │ -247 │ │ │ │ │ -248 static Scalar Retract(Scalar origin, const TangentVector& v, │ │ │ │ │ -249 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { │ │ │ │ │ -250 if (H1) (*H1)[0] = 1.0; │ │ │ │ │ -251 if (H2) (*H2)[0] = 1.0; │ │ │ │ │ -252 return origin + v[0]; │ │ │ │ │ -253 } │ │ │ │ │ -255 │ │ │ │ │ -258 static TangentVector Logmap(Scalar m, ChartJacobian H = boost::none) { │ │ │ │ │ -259 if (H) (*H)[0] = 1.0; │ │ │ │ │ -260 return Local(0, m); │ │ │ │ │ -261 } │ │ │ │ │ -262 │ │ │ │ │ -263 static Scalar Expmap(const TangentVector& v, ChartJacobian H = boost::none) │ │ │ │ │ -{ │ │ │ │ │ -264 if (H) (*H)[0] = 1.0; │ │ │ │ │ -265 return v[0]; │ │ │ │ │ -266 } │ │ │ │ │ -268 │ │ │ │ │ -269}; │ │ │ │ │ -270 │ │ │ │ │ -271} // namespace internal │ │ │ │ │ -272 │ │ │ │ │ -_2_7_4template<> struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_S_c_a_l_a_r_T_r_a_i_t_s { │ │ │ │ │ -275}; │ │ │ │ │ -276 │ │ │ │ │ -_2_7_8template<> struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_S_c_a_l_a_r_T_r_a_i_t_s { │ │ │ │ │ -279}; │ │ │ │ │ -280 │ │ │ │ │ -281// traits for any fixed double Eigen matrix │ │ │ │ │ -282template │ │ │ │ │ -_2_8_3struct _t_r_a_i_t_s > : │ │ │ │ │ -284 _i_n_t_e_r_n_a_l_:_:_V_e_c_t_o_r_S_p_a_c_e_I_m_p_l< │ │ │ │ │ -285 Eigen::Matrix, M * N> { │ │ │ │ │ -286 │ │ │ │ │ -287 typedef _v_e_c_t_o_r___s_p_a_c_e___t_a_g _s_t_r_u_c_t_u_r_e___c_a_t_e_g_o_r_y; │ │ │ │ │ -288 typedef Eigen::Matrix Fixed; │ │ │ │ │ -289 │ │ │ │ │ -292 static void Print(const Fixed& m, const std::string& str = "") { │ │ │ │ │ -293 _g_t_s_a_m_:_:_p_r_i_n_t(Eigen::MatrixXd(m), str); │ │ │ │ │ -294 } │ │ │ │ │ -295 static bool Equals(const Fixed& v1, const Fixed& v2, double tol = 1e-8) { │ │ │ │ │ -296 return _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(v1, v2, tol); │ │ │ │ │ -297 } │ │ │ │ │ -299 │ │ │ │ │ -302 typedef _a_d_d_i_t_i_v_e___g_r_o_u_p___t_a_g _g_r_o_u_p___f_l_a_v_o_r; │ │ │ │ │ -303 static Fixed Identity() { return Fixed::Zero();} │ │ │ │ │ -305 │ │ │ │ │ -308 enum { dimension = M*N}; │ │ │ │ │ -309 typedef Fixed ManifoldType; │ │ │ │ │ -310 typedef Eigen::Matrix TangentVector; │ │ │ │ │ -311 typedef Eigen::Matrix Jacobian; │ │ │ │ │ -312 typedef _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_d_i_m_e_n_s_i_o_n_,_ _d_i_m_e_n_s_i_o_n_> _C_h_a_r_t_J_a_c_o_b_i_a_n; │ │ │ │ │ -313 │ │ │ │ │ -314 static TangentVector Local(const Fixed& origin, const Fixed& other, │ │ │ │ │ -315 _C_h_a_r_t_J_a_c_o_b_i_a_n H1 = boost::none, _C_h_a_r_t_J_a_c_o_b_i_a_n H2 = boost::none) { │ │ │ │ │ -316 if (H1) (*H1) = -Jacobian::Identity(); │ │ │ │ │ -317 if (H2) (*H2) = Jacobian::Identity(); │ │ │ │ │ -318 TangentVector result; │ │ │ │ │ -319 Eigen::Map(result.data()) = other - origin; │ │ │ │ │ -320 return result; │ │ │ │ │ -321 } │ │ │ │ │ -322 │ │ │ │ │ -323 static Fixed Retract(const Fixed& origin, const TangentVector& v, │ │ │ │ │ -324 _C_h_a_r_t_J_a_c_o_b_i_a_n H1 = boost::none, _C_h_a_r_t_J_a_c_o_b_i_a_n H2 = boost::none) { │ │ │ │ │ -325 if (H1) (*H1) = Jacobian::Identity(); │ │ │ │ │ -326 if (H2) (*H2) = Jacobian::Identity(); │ │ │ │ │ -327 return origin + Eigen::Map(v.data()); │ │ │ │ │ -328 } │ │ │ │ │ -330 │ │ │ │ │ -333 static TangentVector Logmap(const Fixed& m, _C_h_a_r_t_J_a_c_o_b_i_a_n H = boost::none) │ │ │ │ │ -{ │ │ │ │ │ -334 if (H) *H = Jacobian::Identity(); │ │ │ │ │ -335 TangentVector result; │ │ │ │ │ -336 Eigen::Map(result.data()) = m; │ │ │ │ │ -337 return result; │ │ │ │ │ -338 } │ │ │ │ │ -339 │ │ │ │ │ -340 static Fixed Expmap(const TangentVector& v, _C_h_a_r_t_J_a_c_o_b_i_a_n H = boost::none) │ │ │ │ │ -{ │ │ │ │ │ -341 Fixed m; │ │ │ │ │ -342 m.setZero(); │ │ │ │ │ -343 if (H) *H = Jacobian::Identity(); │ │ │ │ │ -344 return m + Eigen::Map(v.data()); │ │ │ │ │ -345 } │ │ │ │ │ -347}; │ │ │ │ │ -348 │ │ │ │ │ -349 │ │ │ │ │ -350namespace internal { │ │ │ │ │ -351 │ │ │ │ │ -352// traits for dynamic Eigen matrices │ │ │ │ │ -353template │ │ │ │ │ -_3_5_4struct _D_y_n_a_m_i_c_T_r_a_i_t_s { │ │ │ │ │ -355 │ │ │ │ │ -356 typedef _v_e_c_t_o_r___s_p_a_c_e___t_a_g _s_t_r_u_c_t_u_r_e___c_a_t_e_g_o_r_y; │ │ │ │ │ -357 typedef Eigen::Matrix Dynamic; │ │ │ │ │ -358 │ │ │ │ │ -361 static void Print(const Dynamic& m, const std::string& str = "") { │ │ │ │ │ -362 _g_t_s_a_m_:_:_p_r_i_n_t(Eigen::MatrixXd(m), str); │ │ │ │ │ -363 } │ │ │ │ │ -364 static bool Equals(const Dynamic& v1, const Dynamic& v2, │ │ │ │ │ -365 double tol = 1e-8) { │ │ │ │ │ -366 return _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(v1, v2, tol); │ │ │ │ │ -367 } │ │ │ │ │ -369 │ │ │ │ │ -372 typedef _a_d_d_i_t_i_v_e___g_r_o_u_p___t_a_g _g_r_o_u_p___f_l_a_v_o_r; │ │ │ │ │ -373 static Dynamic Identity() { │ │ │ │ │ -374 throw std::runtime_error("Identity not defined for dynamic types"); │ │ │ │ │ -375 } │ │ │ │ │ -377 │ │ │ │ │ -380 enum { dimension = Eigen::Dynamic }; │ │ │ │ │ -381 typedef Eigen::VectorXd TangentVector; │ │ │ │ │ -382 typedef Eigen::MatrixXd Jacobian; │ │ │ │ │ -383 typedef _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_d_i_m_e_n_s_i_o_n_,_ _d_i_m_e_n_s_i_o_n_> ChartJacobian; │ │ │ │ │ -384 typedef Dynamic ManifoldType; │ │ │ │ │ -385 │ │ │ │ │ -386 static int GetDimension(const Dynamic& m) { │ │ │ │ │ -387 return m.rows() * m._c_o_l_s(); │ │ │ │ │ -388 } │ │ │ │ │ -389 │ │ │ │ │ -390 static Jacobian Eye(const Dynamic& m) { │ │ │ │ │ -391 int dim = GetDimension(m); │ │ │ │ │ -392 return Eigen::Matrix::Identity(dim, dim); │ │ │ │ │ -393 } │ │ │ │ │ -394 │ │ │ │ │ -395 static TangentVector Local(const Dynamic& m, const Dynamic& other, // │ │ │ │ │ -396 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { │ │ │ │ │ -397 if (H1) *H1 = -Eye(m); │ │ │ │ │ -398 if (H2) *H2 = Eye(m); │ │ │ │ │ -399 TangentVector v(GetDimension(m)); │ │ │ │ │ -400 Eigen::Map(v.data(), m.rows(), m.cols()) = other - m; │ │ │ │ │ -401 return v; │ │ │ │ │ -402 } │ │ │ │ │ -403 │ │ │ │ │ -404 static Dynamic Retract(const Dynamic& m, const TangentVector& v, // │ │ │ │ │ -405 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { │ │ │ │ │ -406 if (H1) *H1 = Eye(m); │ │ │ │ │ -407 if (H2) *H2 = Eye(m); │ │ │ │ │ -408 return m + Eigen::Map(v.data(), m.rows(), m.cols()); │ │ │ │ │ -409 } │ │ │ │ │ -411 │ │ │ │ │ -414 static TangentVector Logmap(const Dynamic& m, ChartJacobian H = boost:: │ │ │ │ │ -none) { │ │ │ │ │ -415 if (H) *H = Eye(m); │ │ │ │ │ -416 TangentVector result(GetDimension(m)); │ │ │ │ │ -417 Eigen::Map(result.data(), m.cols(), m.rows()) = m; │ │ │ │ │ -418 return result; │ │ │ │ │ -419 } │ │ │ │ │ -420 │ │ │ │ │ -421 static Dynamic Expmap(const TangentVector& /*v*/, ChartJacobian H = boost:: │ │ │ │ │ -none) { │ │ │ │ │ -422 static_cast(H); │ │ │ │ │ -423 throw std::runtime_error("Expmap not defined for dynamic types"); │ │ │ │ │ -424 } │ │ │ │ │ -425 │ │ │ │ │ -426 static Dynamic Inverse(const Dynamic& m, ChartJacobian H = boost::none) { │ │ │ │ │ -427 if (H) *H = -Eye(m); │ │ │ │ │ -428 return -m; │ │ │ │ │ -429 } │ │ │ │ │ -430 │ │ │ │ │ -431 static Dynamic Compose(const Dynamic& v1, const Dynamic& v2, │ │ │ │ │ -432 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { │ │ │ │ │ -433 if (H1) *H1 = Eye(v1); │ │ │ │ │ -434 if (H2) *H2 = Eye(v1); │ │ │ │ │ -435 return v1 + v2; │ │ │ │ │ -436 } │ │ │ │ │ -437 │ │ │ │ │ -438 static Dynamic Between(const Dynamic& v1, const Dynamic& v2, │ │ │ │ │ -439 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { │ │ │ │ │ -440 if (H1) *H1 = -Eye(v1); │ │ │ │ │ -441 if (H2) *H2 = Eye(v1); │ │ │ │ │ -442 return v2 - v1; │ │ │ │ │ -443 } │ │ │ │ │ -445 │ │ │ │ │ -446}; │ │ │ │ │ -447 │ │ │ │ │ -448} // \ internal │ │ │ │ │ -449 │ │ │ │ │ -450// traits for fully dynamic matrix │ │ │ │ │ -451template │ │ │ │ │ -_4_5_2struct _t_r_a_i_t_s > : │ │ │ │ │ -453 public _i_n_t_e_r_n_a_l_:_:_D_y_n_a_m_i_c_T_r_a_i_t_s<-1, -1, Options, MaxRows, MaxCols> { │ │ │ │ │ -454}; │ │ │ │ │ -455 │ │ │ │ │ -456// traits for dynamic column vector │ │ │ │ │ -457template │ │ │ │ │ -_4_5_8struct _t_r_a_i_t_s > : │ │ │ │ │ -459 public _i_n_t_e_r_n_a_l_:_:_D_y_n_a_m_i_c_T_r_a_i_t_s<-1, 1, Options, MaxRows, MaxCols> { │ │ │ │ │ -460}; │ │ │ │ │ -461 │ │ │ │ │ -462// traits for dynamic row vector │ │ │ │ │ -463template │ │ │ │ │ -_4_6_4struct _t_r_a_i_t_s > : │ │ │ │ │ -465 public _i_n_t_e_r_n_a_l_:_:_D_y_n_a_m_i_c_T_r_a_i_t_s<1, -1, Options, MaxRows, MaxCols> { │ │ │ │ │ -466}; │ │ │ │ │ -467 │ │ │ │ │ -469template │ │ │ │ │ -_4_7_0class _I_s_V_e_c_t_o_r_S_p_a_c_e: public _I_s_L_i_e_G_r_o_u_p { │ │ │ │ │ -471public: │ │ │ │ │ -472 │ │ │ │ │ -473 typedef typename _t_r_a_i_t_s_<_T_>_:_:_s_t_r_u_c_t_u_r_e___c_a_t_e_g_o_r_y structure_category_tag; │ │ │ │ │ -474 │ │ │ │ │ -475 BOOST_CONCEPT_USAGE(_I_s_V_e_c_t_o_r_S_p_a_c_e) { │ │ │ │ │ -476 BOOST_STATIC_ASSERT_MSG( │ │ │ │ │ -477 (boost::is_base_of::value), │ │ │ │ │ -478 "This type's trait does not assert it as a vector space (or derived)"); │ │ │ │ │ -479 r = p + q; │ │ │ │ │ -480 r = -p; │ │ │ │ │ -481 r = p - q; │ │ │ │ │ -482 } │ │ │ │ │ -483 │ │ │ │ │ -484private: │ │ │ │ │ -485 T p, q, r; │ │ │ │ │ -486}; │ │ │ │ │ -487 │ │ │ │ │ -488} // namespace gtsam │ │ │ │ │ -489 │ │ │ │ │ -_L_i_e_._h │ │ │ │ │ -Base class and basic functions for Lie types. │ │ │ │ │ +163template │ │ │ │ │ +_1_6_4struct _F_i_x_e_d_D_i_m_e_n_s_i_o_n { │ │ │ │ │ +165 typedef const int value_type; │ │ │ │ │ +166 static const int value = _t_r_a_i_t_s_<_T_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ +167 BOOST_STATIC_ASSERT_MSG(value != Eigen::Dynamic, │ │ │ │ │ +168 "FixedDimension instantiated for dymanically-sized type."); │ │ │ │ │ +169}; │ │ │ │ │ +170} // \ namespace gtsam │ │ │ │ │ +171 │ │ │ │ │ +173// * Macros for using the ManifoldConcept │ │ │ │ │ +174// * - An instantiation for use inside unit tests │ │ │ │ │ +175// * - A typedef for use inside generic algorithms │ │ │ │ │ +176// * │ │ │ │ │ +177// * NOTE: intentionally not in the gtsam namespace to allow for classes not │ │ │ │ │ +in │ │ │ │ │ +178// * the gtsam namespace to be more easily enforced as testable │ │ │ │ │ +179// */ │ │ │ │ │ +_1_8_0#define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::IsManifold; │ │ │ │ │ +181#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) using _gtsam_IsManifold_##T = gtsam:: │ │ │ │ │ +IsManifold; │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ +_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h │ │ │ │ │ +Special class for optional Jacobian arguments. │ │ │ │ │ +_M_a_t_r_i_x_._h │ │ │ │ │ +typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l___w_i_t_h___a_b_s___t_o_l │ │ │ │ │ -bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen:: │ │ │ │ │ -DenseBase< MATRIX > &B, double tol=1e-9) │ │ │ │ │ -equals with a tolerance │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:81 │ │ │ │ │ +_g_t_s_a_m_:_:_B_O_O_S_T___C_O_N_C_E_P_T___R_E_Q_U_I_R_E_S │ │ │ │ │ +BOOST_CONCEPT_REQUIRES(((IsGroup< G >)),(bool)) check_group_invariants(const G │ │ │ │ │ +&a │ │ │ │ │ +Check invariants. │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_a_d_d_i_t_i_v_e___g_r_o_u_p___t_a_g │ │ │ │ │ -DDeeffiinniittiioonn Group.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_l_i_e___g_r_o_u_p___t_a_g │ │ │ │ │ -tag to assert a type is a Lie group │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:164 │ │ │ │ │ -_g_t_s_a_m_:_:_I_s_L_i_e_G_r_o_u_p │ │ │ │ │ -Lie Group Concept. │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:260 │ │ │ │ │ +_g_t_s_a_m_:_:_m_a_n_i_f_o_l_d___t_a_g │ │ │ │ │ +tag to assert a type is a manifold │ │ │ │ │ +DDeeffiinniittiioonn Manifold.h:33 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_H_a_s_M_a_n_i_f_o_l_d_P_r_e_r_e_q_s │ │ │ │ │ +Requirements on type to pass it to Manifold template below. │ │ │ │ │ +DDeeffiinniittiioonn Manifold.h:59 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_G_e_t_D_i_m_e_n_s_i_o_n_I_m_p_l │ │ │ │ │ +Extra manifold traits for fixed-dimension types. │ │ │ │ │ +DDeeffiinniittiioonn Manifold.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d_T_r_a_i_t_s │ │ │ │ │ +A helper that implements the traits interface for GTSAM manifolds. │ │ │ │ │ +DDeeffiinniittiioonn Manifold.h:95 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ +Both ManifoldTraits and Testable. │ │ │ │ │ +DDeeffiinniittiioonn Manifold.h:120 │ │ │ │ │ +_g_t_s_a_m_:_:_F_i_x_e_d_D_i_m_e_n_s_i_o_n │ │ │ │ │ +Give fixed size dimension of a type, fails at compile time if dynamic. │ │ │ │ │ +DDeeffiinniittiioonn Manifold.h:164 │ │ │ │ │ _g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ either a fixed size o... │ │ │ │ │ DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_:_:_c_o_l_s │ │ │ │ │ -OptionalJacobian< Rows, N > cols(int startCol) │ │ │ │ │ -Access M*N sub-block if we are allocated, otherwise none TODO(frank): this │ │ │ │ │ -could work as is below if ... │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:171 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_,_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_ _> │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:188 │ │ │ │ │ +_g_t_s_a_m_:_:_I_s_T_e_s_t_a_b_l_e │ │ │ │ │ +A testable concept check that should be placed in applicable unit tests and in │ │ │ │ │ +generic algorithms. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:58 │ │ │ │ │ _g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ A helper that implements the traits interface for GTSAM types. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_v_e_c_t_o_r___s_p_a_c_e___t_a_g │ │ │ │ │ -tag to assert a type is a vector space │ │ │ │ │ -DDeeffiinniittiioonn VectorSpace.h:16 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_V_e_c_t_o_r_S_p_a_c_e_I_m_p_l │ │ │ │ │ -VectorSpaceTraits Implementation for Fixed sizes. │ │ │ │ │ -DDeeffiinniittiioonn VectorSpace.h:25 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_H_a_s_V_e_c_t_o_r_S_p_a_c_e_P_r_e_r_e_q_s │ │ │ │ │ -Requirements on type to pass it to Manifold template below. │ │ │ │ │ -DDeeffiinniittiioonn VectorSpace.h:164 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_V_e_c_t_o_r_S_p_a_c_e_T_r_a_i_t_s │ │ │ │ │ -A helper that implements the traits interface for classes that define vector │ │ │ │ │ -spaces To use this for y... │ │ │ │ │ -DDeeffiinniittiioonn VectorSpace.h:185 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_V_e_c_t_o_r_S_p_a_c_e │ │ │ │ │ -VectorSpace provides both Testable and VectorSpaceTraits. │ │ │ │ │ -DDeeffiinniittiioonn VectorSpace.h:207 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_S_c_a_l_a_r_T_r_a_i_t_s │ │ │ │ │ -A helper that implements the traits interface for scalar vector spaces. │ │ │ │ │ -DDeeffiinniittiioonn VectorSpace.h:212 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_D_y_n_a_m_i_c_T_r_a_i_t_s │ │ │ │ │ -DDeeffiinniittiioonn VectorSpace.h:354 │ │ │ │ │ -_g_t_s_a_m_:_:_I_s_V_e_c_t_o_r_S_p_a_c_e │ │ │ │ │ -Vector Space concept. │ │ │ │ │ -DDeeffiinniittiioonn VectorSpace.h:470 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * VVeeccttoorrSSppaaccee..hh │ │ │ │ │ + * _M_a_n_i_f_o_l_d_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00110.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/FastList.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/ProductLieGroup.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,43 +96,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
FastList.h File Reference
│ │ │ │ +
ProductLieGroup.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

A thin wrapper around std::list that uses boost's fast_pool_allocator. │ │ │ │ +

Group product of two Lie Groups. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::FastList< VALUE >
 FastList is a thin wrapper around std::list that uses the boost fast_pool_allocator instead of the default STL allocator. More...
class  gtsam::ProductLieGroup< G, H >
 Template to construct the product Lie group of two other Lie groups Assumes Lie group structure for G and H. More...
 
struct  gtsam::traits< ProductLieGroup< G, H > >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

A thin wrapper around std::list that uses boost's fast_pool_allocator.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Oct 22, 2010
│ │ │ │ +

Group product of two Lie Groups.

│ │ │ │ +
Date
May, 2015
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,30 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -FastList.h File Reference │ │ │ │ │ -A thin wrapper around std::list that uses boost's fast_pool_allocator. _M_o_r_e_._._. │ │ │ │ │ +ProductLieGroup.h File Reference │ │ │ │ │ +Group product of two Lie Groups. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_F_a_s_t_L_i_s_t_<_ _V_A_L_U_E_ _> │ │ │ │ │ -  _F_a_s_t_L_i_s_t is a thin wrapper around std::list that uses the boost │ │ │ │ │ - fast_pool_allocator instead of the default STL allocator. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_P_r_o_d_u_c_t_L_i_e_G_r_o_u_p_<_ _G_,_ _H_ _> │ │ │ │ │ +  Template to construct the product Lie group of two other Lie groups │ │ │ │ │ + Assumes Lie group structure for G and H. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p_<_ _G_,_ _H_ _>_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A thin wrapper around std::list that uses boost's fast_pool_allocator. │ │ │ │ │ - Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ +Group product of two Lie Groups. │ │ │ │ │ Date │ │ │ │ │ - Oct 22, 2010 │ │ │ │ │ + May, 2015 │ │ │ │ │ + Author │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _F_a_s_t_L_i_s_t_._h │ │ │ │ │ + * _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00110_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/FastList.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/ProductLieGroup.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,99 +98,193 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
FastList.h
│ │ │ │ +
ProductLieGroup.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │ -
10 * -------------------------------------------------------------------------- */
│ │ │ │ +
10 * -------------------------------1------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/base/FastDefaultAllocator.h>
│ │ │ │ -
22#include <list>
│ │ │ │ -
23#include <boost/utility/enable_if.hpp>
│ │ │ │ -
24#include <boost/serialization/nvp.hpp>
│ │ │ │ -
25#include <boost/serialization/version.hpp>
│ │ │ │ -
26#include <boost/serialization/optional.hpp>
│ │ │ │ -
27#include <boost/serialization/list.hpp>
│ │ │ │ -
28
│ │ │ │ -
29namespace gtsam {
│ │ │ │ -
30
│ │ │ │ -
39template<typename VALUE>
│ │ │ │ -
│ │ │ │ -
40class FastList: public std::list<VALUE, typename internal::FastDefaultAllocator<VALUE>::type> {
│ │ │ │ +
21#include <gtsam/base/Lie.h>
│ │ │ │ +
22#include <utility> // pair
│ │ │ │ +
23
│ │ │ │ +
24namespace gtsam {
│ │ │ │ +
25
│ │ │ │ +
28template<typename G, typename H>
│ │ │ │ +
│ │ │ │ +
29class ProductLieGroup: public std::pair<G, H> {
│ │ │ │ +
30 BOOST_CONCEPT_ASSERT((IsLieGroup<G>));
│ │ │ │ +
31 BOOST_CONCEPT_ASSERT((IsLieGroup<H>));
│ │ │ │ +
32 typedef std::pair<G, H> Base;
│ │ │ │ +
33
│ │ │ │ +
34protected:
│ │ │ │ +
35 enum {dimension1 = traits<G>::dimension};
│ │ │ │ +
36 enum {dimension2 = traits<H>::dimension};
│ │ │ │ +
37
│ │ │ │ +
38public:
│ │ │ │ +
40 ProductLieGroup():Base(traits<G>::Identity(),traits<H>::Identity()) {}
│ │ │ │
41
│ │ │ │ -
42public:
│ │ │ │ -
43
│ │ │ │ -
44 typedef std::list<VALUE, typename internal::FastDefaultAllocator<VALUE>::type> Base;
│ │ │ │ -
45
│ │ │ │ -
47 FastList() {}
│ │ │ │ -
48
│ │ │ │ -
50 template<typename INPUTITERATOR>
│ │ │ │ -
51 explicit FastList(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {}
│ │ │ │ +
42 // Construct from two subgroup elements
│ │ │ │ +
43 ProductLieGroup(const G& g, const H& h):Base(g,h) {}
│ │ │ │ +
44
│ │ │ │ +
45 // Construct from base
│ │ │ │ +
46 ProductLieGroup(const Base& base):Base(base) {}
│ │ │ │ +
47
│ │ │ │ +
50 typedef multiplicative_group_tag group_flavor;
│ │ │ │ +
51 static ProductLieGroup Identity() {return ProductLieGroup();}
│ │ │ │
52
│ │ │ │ -
54 FastList(const FastList<VALUE>& x) : Base(x) {}
│ │ │ │ -
55
│ │ │ │ -
57 FastList(const Base& x) : Base(x) {}
│ │ │ │ -
58
│ │ │ │ -
60 FastList(std::initializer_list<VALUE> l) : Base(l) {}
│ │ │ │ -
61
│ │ │ │ -
62#ifdef GTSAM_ALLOCATOR_BOOSTPOOL
│ │ │ │ -
64 FastList(const std::list<VALUE>& x) {
│ │ │ │ -
65 // This if statement works around a bug in boost pool allocator and/or
│ │ │ │ -
66 // STL vector where if the size is zero, the pool allocator will allocate
│ │ │ │ -
67 // huge amounts of memory.
│ │ │ │ -
68 if(x.size() > 0)
│ │ │ │ -
69 Base::assign(x.begin(), x.end());
│ │ │ │ -
70 }
│ │ │ │ -
71#endif
│ │ │ │ -
72
│ │ │ │ -
│ │ │ │ -
74 operator std::list<VALUE>() const {
│ │ │ │ -
75 return std::list<VALUE>(this->begin(), this->end());
│ │ │ │ -
76 }
│ │ │ │ -
│ │ │ │ -
77
│ │ │ │ -
78private:
│ │ │ │ -
80 friend class boost::serialization::access;
│ │ │ │ -
81 template<class ARCHIVE>
│ │ │ │ -
82 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
83 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
84 }
│ │ │ │ -
85
│ │ │ │ -
86};
│ │ │ │ -
│ │ │ │ -
87
│ │ │ │ -
88}
│ │ │ │ -
FastDefaultAllocator.h
An easy way to control which allocator is used for Fast* collections.
│ │ │ │ +
53 ProductLieGroup operator*(const ProductLieGroup& other) const {
│ │ │ │ +
54 return ProductLieGroup(traits<G>::Compose(this->first,other.first),
│ │ │ │ +
55 traits<H>::Compose(this->second,other.second));
│ │ │ │ +
56 }
│ │ │ │ +
57 ProductLieGroup inverse() const {
│ │ │ │ +
58 return ProductLieGroup(traits<G>::Inverse(this->first), traits<H>::Inverse(this->second));
│ │ │ │ +
59 }
│ │ │ │ +
60 ProductLieGroup compose(const ProductLieGroup& g) const {
│ │ │ │ +
61 return (*this) * g;
│ │ │ │ +
62 }
│ │ │ │ +
63 ProductLieGroup between(const ProductLieGroup& g) const {
│ │ │ │ +
64 return this->inverse() * g;
│ │ │ │ +
65 }
│ │ │ │ +
67
│ │ │ │ +
70 enum {dimension = dimension1 + dimension2};
│ │ │ │ +
71 inline static size_t Dim() {return dimension;}
│ │ │ │ +
72 inline size_t dim() const {return dimension;}
│ │ │ │ +
73
│ │ │ │ +
74 typedef Eigen::Matrix<double, dimension, 1> TangentVector;
│ │ │ │ +
75 typedef OptionalJacobian<dimension, dimension> ChartJacobian;
│ │ │ │ +
76
│ │ │ │ +
77 ProductLieGroup retract(const TangentVector& v, //
│ │ │ │ +
78 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const {
│ │ │ │ +
79 if (H1||H2) throw std::runtime_error("ProductLieGroup::retract derivatives not implemented yet");
│ │ │ │ +
80 G g = traits<G>::Retract(this->first, v.template head<dimension1>());
│ │ │ │ +
81 H h = traits<H>::Retract(this->second, v.template tail<dimension2>());
│ │ │ │ +
82 return ProductLieGroup(g,h);
│ │ │ │ +
83 }
│ │ │ │ +
84 TangentVector localCoordinates(const ProductLieGroup& g, //
│ │ │ │ +
85 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const {
│ │ │ │ +
86 if (H1||H2) throw std::runtime_error("ProductLieGroup::localCoordinates derivatives not implemented yet");
│ │ │ │ +
87 typename traits<G>::TangentVector v1 = traits<G>::Local(this->first, g.first);
│ │ │ │ +
88 typename traits<H>::TangentVector v2 = traits<H>::Local(this->second, g.second);
│ │ │ │ +
89 TangentVector v;
│ │ │ │ +
90 v << v1, v2;
│ │ │ │ +
91 return v;
│ │ │ │ +
92 }
│ │ │ │ +
94
│ │ │ │ +
97protected:
│ │ │ │ +
98 typedef Eigen::Matrix<double, dimension, dimension> Jacobian;
│ │ │ │ +
99 typedef Eigen::Matrix<double, dimension1, dimension1> Jacobian1;
│ │ │ │ +
100 typedef Eigen::Matrix<double, dimension2, dimension2> Jacobian2;
│ │ │ │ +
101
│ │ │ │ +
102public:
│ │ │ │ +
103 ProductLieGroup compose(const ProductLieGroup& other, ChartJacobian H1,
│ │ │ │ +
104 ChartJacobian H2 = boost::none) const {
│ │ │ │ +
105 Jacobian1 D_g_first; Jacobian2 D_h_second;
│ │ │ │ +
106 G g = traits<G>::Compose(this->first,other.first, H1 ? &D_g_first : 0);
│ │ │ │ +
107 H h = traits<H>::Compose(this->second,other.second, H1 ? &D_h_second : 0);
│ │ │ │ +
108 if (H1) {
│ │ │ │ +
109 H1->setZero();
│ │ │ │ +
110 H1->template topLeftCorner<dimension1,dimension1>() = D_g_first;
│ │ │ │ +
111 H1->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
│ │ │ │ +
112 }
│ │ │ │ +
113 if (H2) *H2 = Jacobian::Identity();
│ │ │ │ +
114 return ProductLieGroup(g,h);
│ │ │ │ +
115 }
│ │ │ │ +
116 ProductLieGroup between(const ProductLieGroup& other, ChartJacobian H1,
│ │ │ │ +
117 ChartJacobian H2 = boost::none) const {
│ │ │ │ +
118 Jacobian1 D_g_first; Jacobian2 D_h_second;
│ │ │ │ +
119 G g = traits<G>::Between(this->first,other.first, H1 ? &D_g_first : 0);
│ │ │ │ +
120 H h = traits<H>::Between(this->second,other.second, H1 ? &D_h_second : 0);
│ │ │ │ +
121 if (H1) {
│ │ │ │ +
122 H1->setZero();
│ │ │ │ +
123 H1->template topLeftCorner<dimension1,dimension1>() = D_g_first;
│ │ │ │ +
124 H1->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
│ │ │ │ +
125 }
│ │ │ │ +
126 if (H2) *H2 = Jacobian::Identity();
│ │ │ │ +
127 return ProductLieGroup(g,h);
│ │ │ │ +
128 }
│ │ │ │ +
129 ProductLieGroup inverse(ChartJacobian D) const {
│ │ │ │ +
130 Jacobian1 D_g_first; Jacobian2 D_h_second;
│ │ │ │ +
131 G g = traits<G>::Inverse(this->first, D ? &D_g_first : 0);
│ │ │ │ +
132 H h = traits<H>::Inverse(this->second, D ? &D_h_second : 0);
│ │ │ │ +
133 if (D) {
│ │ │ │ +
134 D->setZero();
│ │ │ │ +
135 D->template topLeftCorner<dimension1,dimension1>() = D_g_first;
│ │ │ │ +
136 D->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
│ │ │ │ +
137 }
│ │ │ │ +
138 return ProductLieGroup(g,h);
│ │ │ │ +
139 }
│ │ │ │ +
140 static ProductLieGroup Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
│ │ │ │ +
141 Jacobian1 D_g_first; Jacobian2 D_h_second;
│ │ │ │ +
142 G g = traits<G>::Expmap(v.template head<dimension1>(), Hv ? &D_g_first : 0);
│ │ │ │ +
143 H h = traits<H>::Expmap(v.template tail<dimension2>(), Hv ? &D_h_second : 0);
│ │ │ │ +
144 if (Hv) {
│ │ │ │ +
145 Hv->setZero();
│ │ │ │ +
146 Hv->template topLeftCorner<dimension1,dimension1>() = D_g_first;
│ │ │ │ +
147 Hv->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
│ │ │ │ +
148 }
│ │ │ │ +
149 return ProductLieGroup(g,h);
│ │ │ │ +
150 }
│ │ │ │ +
151 static TangentVector Logmap(const ProductLieGroup& p, ChartJacobian Hp = boost::none) {
│ │ │ │ +
152 Jacobian1 D_g_first; Jacobian2 D_h_second;
│ │ │ │ +
153 typename traits<G>::TangentVector v1 = traits<G>::Logmap(p.first, Hp ? &D_g_first : 0);
│ │ │ │ +
154 typename traits<H>::TangentVector v2 = traits<H>::Logmap(p.second, Hp ? &D_h_second : 0);
│ │ │ │ +
155 TangentVector v;
│ │ │ │ +
156 v << v1, v2;
│ │ │ │ +
157 if (Hp) {
│ │ │ │ +
158 Hp->setZero();
│ │ │ │ +
159 Hp->template topLeftCorner<dimension1,dimension1>() = D_g_first;
│ │ │ │ +
160 Hp->template bottomRightCorner<dimension2,dimension2>() = D_h_second;
│ │ │ │ +
161 }
│ │ │ │ +
162 return v;
│ │ │ │ +
163 }
│ │ │ │ +
164 static TangentVector LocalCoordinates(const ProductLieGroup& p, ChartJacobian Hp = boost::none) {
│ │ │ │ +
165 return Logmap(p, Hp);
│ │ │ │ +
166 }
│ │ │ │ +
167 ProductLieGroup expmap(const TangentVector& v) const {
│ │ │ │ +
168 return compose(ProductLieGroup::Expmap(v));
│ │ │ │ +
169 }
│ │ │ │ +
170 TangentVector logmap(const ProductLieGroup& g) const {
│ │ │ │ +
171 return ProductLieGroup::Logmap(between(g));
│ │ │ │ +
172 }
│ │ │ │ +
174
│ │ │ │ +
175};
│ │ │ │ +
│ │ │ │ +
176
│ │ │ │ +
177// Define any direct product group to be a model of the multiplicative Group concept
│ │ │ │ +
178template<typename G, typename H>
│ │ │ │ +
179struct traits<ProductLieGroup<G, H> > : internal::LieGroupTraits<ProductLieGroup<G, H> > {};
│ │ │ │ +
180
│ │ │ │ +
181} // namespace gtsam
│ │ │ │ +
182
│ │ │ │ +
Lie.h
Base class and basic functions for Lie types.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::FastList
FastList is a thin wrapper around std::list that uses the boost fast_pool_allocator instead of the de...
Definition FastList.h:40
│ │ │ │ -
gtsam::FastList::FastList
FastList(INPUTITERATOR first, INPUTITERATOR last)
Constructor from a range, passes through to base class.
Definition FastList.h:51
│ │ │ │ -
gtsam::FastList::FastList
FastList()
Default constructor.
Definition FastList.h:47
│ │ │ │ -
gtsam::FastList::FastList
FastList(std::initializer_list< VALUE > l)
Construct from c++11 initializer list:
Definition FastList.h:60
│ │ │ │ -
gtsam::FastList::FastList
FastList(const FastList< VALUE > &x)
Copy constructor from another FastList.
Definition FastList.h:54
│ │ │ │ -
gtsam::FastList::access
friend class boost::serialization::access
Serialization function.
Definition FastList.h:80
│ │ │ │ -
gtsam::FastList::FastList
FastList(const Base &x)
Copy constructor from the base list class.
Definition FastList.h:57
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::internal::LieGroupTraits
A helper class that implements the traits interface for GTSAM lie groups.
Definition Lie.h:174
│ │ │ │ +
gtsam::IsLieGroup
Lie Group Concept.
Definition Lie.h:260
│ │ │ │ +
gtsam::ProductLieGroup
Template to construct the product Lie group of two other Lie groups Assumes Lie group structure for G...
Definition ProductLieGroup.h:29
│ │ │ │ +
gtsam::ProductLieGroup::ProductLieGroup
ProductLieGroup()
Default constructor yields identity.
Definition ProductLieGroup.h:40
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,114 +1,213 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -FastList.h │ │ │ │ │ +ProductLieGroup.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ -10 * ------------------------------------------------------------------------- │ │ │ │ │ +10 * -------------------------------1------------------------------------------ │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 19#pragma once │ │ │ │ │ 20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_._h> │ │ │ │ │ -22#include │ │ │ │ │ -23#include │ │ │ │ │ -24#include │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ -28 │ │ │ │ │ -29namespace _g_t_s_a_m { │ │ │ │ │ -30 │ │ │ │ │ -39template │ │ │ │ │ -_4_0class _F_a_s_t_L_i_s_t: public std::list::type> { │ │ │ │ │ +21#include <_g_t_s_a_m_/_b_a_s_e_/_L_i_e_._h> │ │ │ │ │ +22#include // pair │ │ │ │ │ +23 │ │ │ │ │ +24namespace _g_t_s_a_m { │ │ │ │ │ +25 │ │ │ │ │ +28template │ │ │ │ │ +_2_9class _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p: public std::pair { │ │ │ │ │ +30 BOOST_CONCEPT_ASSERT((_I_s_L_i_e_G_r_o_u_p_<_G_>)); │ │ │ │ │ +31 BOOST_CONCEPT_ASSERT((_I_s_L_i_e_G_r_o_u_p_<_H_>)); │ │ │ │ │ +32 typedef std::pair Base; │ │ │ │ │ +33 │ │ │ │ │ +34protected: │ │ │ │ │ +35 enum {dimension1 = _t_r_a_i_t_s_<_G_>_:_:_d_i_m_e_n_s_i_o_n}; │ │ │ │ │ +36 enum {dimension2 = _t_r_a_i_t_s_<_H_>_:_:_d_i_m_e_n_s_i_o_n}; │ │ │ │ │ +37 │ │ │ │ │ +38public: │ │ │ │ │ +_4_0 _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p():Base(_t_r_a_i_t_s::Identity(),_t_r_a_i_t_s::Identity()) {} │ │ │ │ │ 41 │ │ │ │ │ -42public: │ │ │ │ │ -43 │ │ │ │ │ -44 typedef std::list:: │ │ │ │ │ -type> Base; │ │ │ │ │ -45 │ │ │ │ │ -_4_7 _F_a_s_t_L_i_s_t() {} │ │ │ │ │ -48 │ │ │ │ │ -50 template │ │ │ │ │ -_5_1 explicit _F_a_s_t_L_i_s_t(INPUTITERATOR first, INPUTITERATOR last) : Base(first, │ │ │ │ │ -last) {} │ │ │ │ │ +42 // Construct from two subgroup elements │ │ │ │ │ +43 _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p(const G& g, const H& h):Base(g,h) {} │ │ │ │ │ +44 │ │ │ │ │ +45 // Construct from base │ │ │ │ │ +46 _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p(const Base& base):Base(base) {} │ │ │ │ │ +47 │ │ │ │ │ +50 typedef multiplicative_group_tag group_flavor; │ │ │ │ │ +51 static _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p Identity() {return _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p();} │ │ │ │ │ 52 │ │ │ │ │ -_5_4 _F_a_s_t_L_i_s_t(const _F_a_s_t_L_i_s_t_<_V_A_L_U_E_>& x) : Base(x) {} │ │ │ │ │ -55 │ │ │ │ │ -_5_7 _F_a_s_t_L_i_s_t(const Base& x) : Base(x) {} │ │ │ │ │ -58 │ │ │ │ │ -_6_0 _F_a_s_t_L_i_s_t(std::initializer_list l) : Base(l) {} │ │ │ │ │ -61 │ │ │ │ │ -62#ifdef GTSAM_ALLOCATOR_BOOSTPOOL │ │ │ │ │ -64 _F_a_s_t_L_i_s_t(const std::list& x) { │ │ │ │ │ -65 // This if statement works around a bug in boost pool allocator and/or │ │ │ │ │ -66 // STL vector where if the size is zero, the pool allocator will allocate │ │ │ │ │ -67 // huge amounts of memory. │ │ │ │ │ -68 if(x.size() > 0) │ │ │ │ │ -69 Base::assign(x.begin(), x.end()); │ │ │ │ │ -70 } │ │ │ │ │ -71#endif │ │ │ │ │ -72 │ │ │ │ │ -_7_4 operator std::list() const { │ │ │ │ │ -75 return std::list(this->begin(), this->end()); │ │ │ │ │ -76 } │ │ │ │ │ -77 │ │ │ │ │ -78private: │ │ │ │ │ -_8_0 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -81 template │ │ │ │ │ -82 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -83 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); │ │ │ │ │ -84 } │ │ │ │ │ -85 │ │ │ │ │ -86}; │ │ │ │ │ -87 │ │ │ │ │ -88} │ │ │ │ │ -_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_._h │ │ │ │ │ -An easy way to control which allocator is used for Fast* collections. │ │ │ │ │ +53 _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p operator*(const _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p& other) const { │ │ │ │ │ +54 return _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p(traits::Compose(this->first,other.first), │ │ │ │ │ +55 traits::Compose(this->second,other.second)); │ │ │ │ │ +56 } │ │ │ │ │ +57 _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p inverse() const { │ │ │ │ │ +58 return _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p(traits::Inverse(this->first), traits::Inverse │ │ │ │ │ +(this->second)); │ │ │ │ │ +59 } │ │ │ │ │ +60 _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p compose(const _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p& g) const { │ │ │ │ │ +61 return (*this) * g; │ │ │ │ │ +62 } │ │ │ │ │ +63 _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p between(const _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p& g) const { │ │ │ │ │ +64 return this->inverse() * g; │ │ │ │ │ +65 } │ │ │ │ │ +67 │ │ │ │ │ +70 enum {dimension = dimension1 + dimension2}; │ │ │ │ │ +71 inline static size_t Dim() {return dimension;} │ │ │ │ │ +72 inline size_t dim() const {return dimension;} │ │ │ │ │ +73 │ │ │ │ │ +74 typedef Eigen::Matrix TangentVector; │ │ │ │ │ +75 typedef OptionalJacobian ChartJacobian; │ │ │ │ │ +76 │ │ │ │ │ +77 _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p retract(const TangentVector& v, // │ │ │ │ │ +78 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const { │ │ │ │ │ +79 if (H1||H2) throw std::runtime_error("ProductLieGroup::retract derivatives │ │ │ │ │ +not implemented yet"); │ │ │ │ │ +80 G g = traits::Retract(this->first, v.template head()); │ │ │ │ │ +81 H h = traits::Retract(this->second, v.template tail()); │ │ │ │ │ +82 return _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p(g,h); │ │ │ │ │ +83 } │ │ │ │ │ +84 TangentVector localCoordinates(const _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p& g, // │ │ │ │ │ +85 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const { │ │ │ │ │ +86 if (H1||H2) throw std::runtime_error("ProductLieGroup::localCoordinates │ │ │ │ │ +derivatives not implemented yet"); │ │ │ │ │ +87 typename traits::TangentVector v1 = traits::Local(this->first, │ │ │ │ │ +g.first); │ │ │ │ │ +88 typename traits::TangentVector v2 = traits::Local(this->second, │ │ │ │ │ +g.second); │ │ │ │ │ +89 TangentVector v; │ │ │ │ │ +90 v << v1, v2; │ │ │ │ │ +91 return v; │ │ │ │ │ +92 } │ │ │ │ │ +94 │ │ │ │ │ +97protected: │ │ │ │ │ +98 typedef Eigen::Matrix Jacobian; │ │ │ │ │ +99 typedef Eigen::Matrix Jacobian1; │ │ │ │ │ +100 typedef Eigen::Matrix Jacobian2; │ │ │ │ │ +101 │ │ │ │ │ +102public: │ │ │ │ │ +103 _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p compose(const _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p& other, ChartJacobian H1, │ │ │ │ │ +104 ChartJacobian H2 = boost::none) const { │ │ │ │ │ +105 Jacobian1 D_g_first; Jacobian2 D_h_second; │ │ │ │ │ +106 G g = traits::Compose(this->first,other.first, H1 ? &D_g_first : 0); │ │ │ │ │ +107 H h = traits::Compose(this->second,other.second, H1 ? &D_h_second : 0); │ │ │ │ │ +108 if (H1) { │ │ │ │ │ +109 H1->setZero(); │ │ │ │ │ +110 H1->template topLeftCorner() = D_g_first; │ │ │ │ │ +111 H1->template bottomRightCorner() = D_h_second; │ │ │ │ │ +112 } │ │ │ │ │ +113 if (H2) *H2 = Jacobian::Identity(); │ │ │ │ │ +114 return _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p(g,h); │ │ │ │ │ +115 } │ │ │ │ │ +116 _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p between(const _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p& other, ChartJacobian H1, │ │ │ │ │ +117 ChartJacobian H2 = boost::none) const { │ │ │ │ │ +118 Jacobian1 D_g_first; Jacobian2 D_h_second; │ │ │ │ │ +119 G g = traits::Between(this->first,other.first, H1 ? &D_g_first : 0); │ │ │ │ │ +120 H h = traits::Between(this->second,other.second, H1 ? &D_h_second : 0); │ │ │ │ │ +121 if (H1) { │ │ │ │ │ +122 H1->setZero(); │ │ │ │ │ +123 H1->template topLeftCorner() = D_g_first; │ │ │ │ │ +124 H1->template bottomRightCorner() = D_h_second; │ │ │ │ │ +125 } │ │ │ │ │ +126 if (H2) *H2 = Jacobian::Identity(); │ │ │ │ │ +127 return _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p(g,h); │ │ │ │ │ +128 } │ │ │ │ │ +129 _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p inverse(ChartJacobian D) const { │ │ │ │ │ +130 Jacobian1 D_g_first; Jacobian2 D_h_second; │ │ │ │ │ +131 G g = traits::Inverse(this->first, D ? &D_g_first : 0); │ │ │ │ │ +132 H h = traits::Inverse(this->second, D ? &D_h_second : 0); │ │ │ │ │ +133 if (D) { │ │ │ │ │ +134 D->setZero(); │ │ │ │ │ +135 D->template topLeftCorner() = D_g_first; │ │ │ │ │ +136 D->template bottomRightCorner() = D_h_second; │ │ │ │ │ +137 } │ │ │ │ │ +138 return _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p(g,h); │ │ │ │ │ +139 } │ │ │ │ │ +140 static _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p Expmap(const TangentVector& v, ChartJacobian Hv = │ │ │ │ │ +boost::none) { │ │ │ │ │ +141 Jacobian1 D_g_first; Jacobian2 D_h_second; │ │ │ │ │ +142 G g = traits::Expmap(v.template head(), Hv ? &D_g_first : │ │ │ │ │ +0); │ │ │ │ │ +143 H h = traits::Expmap(v.template tail(), Hv ? &D_h_second : │ │ │ │ │ +0); │ │ │ │ │ +144 if (Hv) { │ │ │ │ │ +145 Hv->setZero(); │ │ │ │ │ +146 Hv->template topLeftCorner() = D_g_first; │ │ │ │ │ +147 Hv->template bottomRightCorner() = D_h_second; │ │ │ │ │ +148 } │ │ │ │ │ +149 return _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p(g,h); │ │ │ │ │ +150 } │ │ │ │ │ +151 static TangentVector Logmap(const _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p& p, ChartJacobian Hp = │ │ │ │ │ +boost::none) { │ │ │ │ │ +152 Jacobian1 D_g_first; Jacobian2 D_h_second; │ │ │ │ │ +153 typename traits::TangentVector v1 = traits::Logmap(p.first, Hp ? │ │ │ │ │ +&D_g_first : 0); │ │ │ │ │ +154 typename traits::TangentVector v2 = traits::Logmap(p.second, Hp ? │ │ │ │ │ +&D_h_second : 0); │ │ │ │ │ +155 TangentVector v; │ │ │ │ │ +156 v << v1, v2; │ │ │ │ │ +157 if (Hp) { │ │ │ │ │ +158 Hp->setZero(); │ │ │ │ │ +159 Hp->template topLeftCorner() = D_g_first; │ │ │ │ │ +160 Hp->template bottomRightCorner() = D_h_second; │ │ │ │ │ +161 } │ │ │ │ │ +162 return v; │ │ │ │ │ +163 } │ │ │ │ │ +164 static TangentVector LocalCoordinates(const _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p& p, │ │ │ │ │ +ChartJacobian Hp = boost::none) { │ │ │ │ │ +165 return Logmap(p, Hp); │ │ │ │ │ +166 } │ │ │ │ │ +167 _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p expmap(const TangentVector& v) const { │ │ │ │ │ +168 return compose(ProductLieGroup::Expmap(v)); │ │ │ │ │ +169 } │ │ │ │ │ +170 TangentVector logmap(const _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p& g) const { │ │ │ │ │ +171 return ProductLieGroup::Logmap(between(g)); │ │ │ │ │ +172 } │ │ │ │ │ +174 │ │ │ │ │ +175}; │ │ │ │ │ +176 │ │ │ │ │ +177// Define any direct product group to be a model of the multiplicative Group │ │ │ │ │ +concept │ │ │ │ │ +178template │ │ │ │ │ +_1_7_9struct _t_r_a_i_t_s<_P_r_o_d_u_c_t_L_i_e_G_r_o_u_p > : _i_n_t_e_r_n_a_l_:_: │ │ │ │ │ +_L_i_e_G_r_o_u_p_T_r_a_i_t_s > {}; │ │ │ │ │ +180 │ │ │ │ │ +181} // namespace gtsam │ │ │ │ │ +182 │ │ │ │ │ +_L_i_e_._h │ │ │ │ │ +Base class and basic functions for Lie types. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t │ │ │ │ │ -FastList is a thin wrapper around std::list that uses the boost │ │ │ │ │ -fast_pool_allocator instead of the de... │ │ │ │ │ -DDeeffiinniittiioonn FastList.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t_:_:_F_a_s_t_L_i_s_t │ │ │ │ │ -FastList(INPUTITERATOR first, INPUTITERATOR last) │ │ │ │ │ -Constructor from a range, passes through to base class. │ │ │ │ │ -DDeeffiinniittiioonn FastList.h:51 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t_:_:_F_a_s_t_L_i_s_t │ │ │ │ │ -FastList() │ │ │ │ │ -Default constructor. │ │ │ │ │ -DDeeffiinniittiioonn FastList.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t_:_:_F_a_s_t_L_i_s_t │ │ │ │ │ -FastList(std::initializer_list< VALUE > l) │ │ │ │ │ -Construct from c++11 initializer list: │ │ │ │ │ -DDeeffiinniittiioonn FastList.h:60 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t_:_:_F_a_s_t_L_i_s_t │ │ │ │ │ -FastList(const FastList< VALUE > &x) │ │ │ │ │ -Copy constructor from another FastList. │ │ │ │ │ -DDeeffiinniittiioonn FastList.h:54 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn FastList.h:80 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t_:_:_F_a_s_t_L_i_s_t │ │ │ │ │ -FastList(const Base &x) │ │ │ │ │ -Copy constructor from the base list class. │ │ │ │ │ -DDeeffiinniittiioonn FastList.h:57 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p_T_r_a_i_t_s │ │ │ │ │ +A helper class that implements the traits interface for GTSAM lie groups. │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:174 │ │ │ │ │ +_g_t_s_a_m_:_:_I_s_L_i_e_G_r_o_u_p │ │ │ │ │ +Lie Group Concept. │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:260 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_o_d_u_c_t_L_i_e_G_r_o_u_p │ │ │ │ │ +Template to construct the product Lie group of two other Lie groups Assumes Lie │ │ │ │ │ +group structure for G... │ │ │ │ │ +DDeeffiinniittiioonn ProductLieGroup.h:29 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_o_d_u_c_t_L_i_e_G_r_o_u_p_:_:_P_r_o_d_u_c_t_L_i_e_G_r_o_u_p │ │ │ │ │ +ProductLieGroup() │ │ │ │ │ +Default constructor yields identity. │ │ │ │ │ +DDeeffiinniittiioonn ProductLieGroup.h:40 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _F_a_s_t_L_i_s_t_._h │ │ │ │ │ + * _P_r_o_d_u_c_t_L_i_e_G_r_o_u_p_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00113.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/FastSet.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/SymmetricBlockMatrix.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,43 +96,43 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
FastSet.h File Reference
│ │ │ │ +
SymmetricBlockMatrix.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

A thin wrapper around std::set that uses boost's fast_pool_allocator. │ │ │ │ +

Access to matrices via blocks of pre-defined sizes. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::FastSet< VALUE >
 FastSet is a thin wrapper around std::set that uses the boost fast_pool_allocator instead of the default STL allocator. More...
class  gtsam::SymmetricBlockMatrix
 This class stores a dense matrix and allows it to be accessed as a collection of blocks. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

A thin wrapper around std::set that uses boost's fast_pool_allocator.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Oct 17, 2010
│ │ │ │ +

Access to matrices via blocks of pre-defined sizes.

│ │ │ │ +

Used in GaussianFactor and GaussianConditional.

Author
Richard Roberts
│ │ │ │ +
Date
Sep 18, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,29 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -FastSet.h File Reference │ │ │ │ │ -A thin wrapper around std::set that uses boost's fast_pool_allocator. _M_o_r_e_._._. │ │ │ │ │ +SymmetricBlockMatrix.h File Reference │ │ │ │ │ +Access to matrices via blocks of pre-defined sizes. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _V_A_L_U_E_ _> │ │ │ │ │ -  _F_a_s_t_S_e_t is a thin wrapper around std::set that uses the boost │ │ │ │ │ - fast_pool_allocator instead of the default STL allocator. _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +  This class stores a dense matrix and allows it to be accessed as a │ │ │ │ │ + collection of blocks. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A thin wrapper around std::set that uses boost's fast_pool_allocator. │ │ │ │ │ +Access to matrices via blocks of pre-defined sizes. │ │ │ │ │ +Used in GaussianFactor and _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l. │ │ │ │ │ Author │ │ │ │ │ Richard Roberts │ │ │ │ │ Date │ │ │ │ │ - Oct 17, 2010 │ │ │ │ │ + Sep 18, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _F_a_s_t_S_e_t_._h │ │ │ │ │ + * _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00113_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/FastSet.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/SymmetricBlockMatrix.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,162 +98,439 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
FastSet.h
│ │ │ │ +
SymmetricBlockMatrix.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ -
4 * Atlanta, Georgia 30332-0415
│ │ │ │ -
5 * All Rights Reserved
│ │ │ │ -
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
3* GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
4* Atlanta, Georgia 30332-0415
│ │ │ │ +
5* All Rights Reserved
│ │ │ │ +
6* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │ -
8 * See LICENSE for the license information
│ │ │ │ +
8* See LICENSE for the license information
│ │ │ │
9
│ │ │ │ -
10 * -------------------------------------------------------------------------- */
│ │ │ │ +
10* -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <boost/version.hpp>
│ │ │ │ -
22#if BOOST_VERSION >= 107400
│ │ │ │ -
23#include <boost/serialization/library_version_type.hpp>
│ │ │ │ -
24#endif
│ │ │ │ -
25#include <boost/serialization/nvp.hpp>
│ │ │ │ -
26#include <boost/serialization/set.hpp>
│ │ │ │ -
27#include <gtsam/base/FastDefaultAllocator.h>
│ │ │ │ -
28#include <gtsam/base/Testable.h>
│ │ │ │ -
29
│ │ │ │ -
30#include <functional>
│ │ │ │ -
31#include <set>
│ │ │ │ -
32
│ │ │ │ -
33namespace boost {
│ │ │ │ -
34namespace serialization {
│ │ │ │ -
35class access;
│ │ │ │ -
36} /* namespace serialization */
│ │ │ │ -
37} /* namespace boost */
│ │ │ │ -
38
│ │ │ │ -
39namespace gtsam {
│ │ │ │ -
40
│ │ │ │ -
48template<typename VALUE>
│ │ │ │ -
│ │ │ │ -
49class FastSet: public std::set<VALUE, std::less<VALUE>,
│ │ │ │ -
50 typename internal::FastDefaultAllocator<VALUE>::type> {
│ │ │ │ -
51
│ │ │ │ -
52 BOOST_CONCEPT_ASSERT ((IsTestable<VALUE> ));
│ │ │ │ -
53
│ │ │ │ -
54public:
│ │ │ │ -
55
│ │ │ │ -
56 typedef std::set<VALUE, std::less<VALUE>,
│ │ │ │ -
57 typename internal::FastDefaultAllocator<VALUE>::type> Base;
│ │ │ │ -
58
│ │ │ │ -
59 using Base::Base; // Inherit the set constructors
│ │ │ │ -
60
│ │ │ │ -
61 FastSet() = default;
│ │ │ │ -
62
│ │ │ │ -
64 template<typename INPUTCONTAINER>
│ │ │ │ -
│ │ │ │ -
65 explicit FastSet(const INPUTCONTAINER& container) :
│ │ │ │ -
66 Base(container.begin(), container.end()) {
│ │ │ │ -
67 }
│ │ │ │ -
│ │ │ │ -
68
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/base/FastVector.h>
│ │ │ │ +
21#include <gtsam/base/Matrix.h>
│ │ │ │ +
22#include <gtsam/base/types.h>
│ │ │ │ +
23#include <gtsam/dllexport.h>
│ │ │ │ +
24#include <boost/serialization/nvp.hpp>
│ │ │ │ +
25#include <cassert>
│ │ │ │ +
26#include <stdexcept>
│ │ │ │ +
27#include <array>
│ │ │ │ +
28
│ │ │ │ +
29namespace boost {
│ │ │ │ +
30namespace serialization {
│ │ │ │ +
31class access;
│ │ │ │ +
32} /* namespace serialization */
│ │ │ │ +
33} /* namespace boost */
│ │ │ │ +
34
│ │ │ │ +
35namespace gtsam {
│ │ │ │ +
36
│ │ │ │ +
37 // Forward declarations
│ │ │ │ +
38 class VerticalBlockMatrix;
│ │ │ │ +
39
│ │ │ │ +
│ │ │ │ +
51 class GTSAM_EXPORT SymmetricBlockMatrix
│ │ │ │ +
52 {
│ │ │ │ +
53 public:
│ │ │ │ +
54 typedef SymmetricBlockMatrix This;
│ │ │ │ +
55 typedef Eigen::Block<Matrix> Block;
│ │ │ │ +
56 typedef Eigen::Block<const Matrix> constBlock;
│ │ │ │ +
57
│ │ │ │ +
58 protected:
│ │ │ │ +
59 Matrix matrix_;
│ │ │ │ +
60 FastVector<DenseIndex> variableColOffsets_;
│ │ │ │ +
61
│ │ │ │ +
62 DenseIndex blockStart_;
│ │ │ │ +
63
│ │ │ │ +
64 public:
│ │ │ │ +
66 SymmetricBlockMatrix();
│ │ │ │ +
67
│ │ │ │ +
69 template<typename CONTAINER>
│ │ │ │
│ │ │ │ -
70 FastSet(const FastSet<VALUE>& x) :
│ │ │ │ -
71 Base(x) {
│ │ │ │ -
72 }
│ │ │ │ -
│ │ │ │ -
73
│ │ │ │ -
│ │ │ │ -
75 FastSet(const Base& x) :
│ │ │ │ -
76 Base(x) {
│ │ │ │ -
77 }
│ │ │ │ -
│ │ │ │ -
78
│ │ │ │ -
79#ifdef GTSAM_ALLOCATOR_BOOSTPOOL
│ │ │ │ -
81 FastSet(const std::set<VALUE>& x) {
│ │ │ │ -
82 // This if statement works around a bug in boost pool allocator and/or
│ │ │ │ -
83 // STL vector where if the size is zero, the pool allocator will allocate
│ │ │ │ -
84 // huge amounts of memory.
│ │ │ │ -
85 if(x.size() > 0)
│ │ │ │ -
86 Base::insert(x.begin(), x.end());
│ │ │ │ -
87 }
│ │ │ │ -
88#endif
│ │ │ │ -
89
│ │ │ │ -
│ │ │ │ -
91 operator std::set<VALUE>() const {
│ │ │ │ -
92 return std::set<VALUE>(this->begin(), this->end());
│ │ │ │ -
93 }
│ │ │ │ -
│ │ │ │ -
94
│ │ │ │ -
│ │ │ │ -
96 bool exists(const VALUE& e) const {
│ │ │ │ -
97 return this->find(e) != this->end();
│ │ │ │ -
98 }
│ │ │ │ -
│ │ │ │ -
99
│ │ │ │ -
│ │ │ │ -
101 void print(const std::string& str = "") const {
│ │ │ │ -
102 for (typename Base::const_iterator it = this->begin(); it != this->end(); ++it)
│ │ │ │ -
103 traits<VALUE>::Print(*it, str);
│ │ │ │ -
104 }
│ │ │ │ -
│ │ │ │ -
105
│ │ │ │ -
│ │ │ │ -
107 bool equals(const FastSet<VALUE>& other, double tol = 1e-9) const {
│ │ │ │ -
108 typename Base::const_iterator it1 = this->begin(), it2 = other.begin();
│ │ │ │ -
109 while (it1 != this->end()) {
│ │ │ │ -
110 if (it2 == other.end() || !traits<VALUE>::Equals(*it2, *it2, tol))
│ │ │ │ -
111 return false;
│ │ │ │ -
112 ++it1;
│ │ │ │ -
113 ++it2;
│ │ │ │ -
114 }
│ │ │ │ -
115 return true;
│ │ │ │ -
116 }
│ │ │ │ -
│ │ │ │ -
117
│ │ │ │ -
│ │ │ │ -
119 void merge(const FastSet& other) {
│ │ │ │ -
120 Base::insert(other.begin(), other.end());
│ │ │ │ -
121 }
│ │ │ │ -
│ │ │ │ -
122
│ │ │ │ -
123private:
│ │ │ │ -
125 friend class boost::serialization::access;
│ │ │ │ -
126 template<class ARCHIVE>
│ │ │ │ -
127 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
128 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
129 }
│ │ │ │ -
130};
│ │ │ │ -
│ │ │ │ -
131
│ │ │ │ -
132}
│ │ │ │ -
FastDefaultAllocator.h
An easy way to control which allocator is used for Fast* collections.
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
70 SymmetricBlockMatrix(const CONTAINER& dimensions, bool appendOneDimension = false) :
│ │ │ │ +
71 blockStart_(0)
│ │ │ │ +
72 {
│ │ │ │ +
73 fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
│ │ │ │ +
74 matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
│ │ │ │ +
75 assertInvariants();
│ │ │ │ +
76 }
│ │ │ │ +
│ │ │ │ +
77
│ │ │ │ +
79 template<typename ITERATOR>
│ │ │ │ +
│ │ │ │ +
80 SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension = false) :
│ │ │ │ +
81 blockStart_(0)
│ │ │ │ +
82 {
│ │ │ │ +
83 fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension);
│ │ │ │ +
84 matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
│ │ │ │ +
85 assertInvariants();
│ │ │ │ +
86 }
│ │ │ │ +
│ │ │ │ +
87
│ │ │ │ +
89 template<typename CONTAINER>
│ │ │ │ +
│ │ │ │ +
90 SymmetricBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix, bool appendOneDimension = false) :
│ │ │ │ +
91 blockStart_(0)
│ │ │ │ +
92 {
│ │ │ │ +
93 matrix_.resize(matrix.rows(), matrix.cols());
│ │ │ │ +
94 matrix_.triangularView<Eigen::Upper>() = matrix.triangularView<Eigen::Upper>();
│ │ │ │ +
95 fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
│ │ │ │ +
96 if(matrix_.rows() != matrix_.cols())
│ │ │ │ +
97 throw std::invalid_argument("Requested to create a SymmetricBlockMatrix from a non-square matrix.");
│ │ │ │ +
98 if(variableColOffsets_.back() != matrix_.cols())
│ │ │ │ +
99 throw std::invalid_argument("Requested to create a SymmetricBlockMatrix with dimensions that do not sum to the total size of the provided matrix.");
│ │ │ │ +
100 assertInvariants();
│ │ │ │ +
101 }
│ │ │ │ +
│ │ │ │ +
102
│ │ │ │ +
106 static SymmetricBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix& other);
│ │ │ │ +
107
│ │ │ │ +
111 static SymmetricBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& other);
│ │ │ │ +
112
│ │ │ │ +
114 DenseIndex rows() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; }
│ │ │ │ +
115
│ │ │ │ +
117 DenseIndex cols() const { return rows(); }
│ │ │ │ +
118
│ │ │ │ +
120 DenseIndex nBlocks() const { return nActualBlocks() - blockStart_; }
│ │ │ │ +
121
│ │ │ │ +
│ │ │ │ +
123 DenseIndex getDim(DenseIndex block) const {
│ │ │ │ +
124 return calcIndices(block, block, 1, 1)[2];
│ │ │ │ +
125 }
│ │ │ │ +
│ │ │ │ +
126
│ │ │ │ +
129
│ │ │ │ +
132 Matrix block(DenseIndex I, DenseIndex J) const;
│ │ │ │ +
133
│ │ │ │ +
│ │ │ │ +
135 Eigen::SelfAdjointView<Block, Eigen::Upper> diagonalBlock(DenseIndex J) {
│ │ │ │ +
136 return block_(J, J).selfadjointView<Eigen::Upper>();
│ │ │ │ +
137 }
│ │ │ │ +
│ │ │ │ +
138
│ │ │ │ +
│ │ │ │ +
140 Eigen::SelfAdjointView<constBlock, Eigen::Upper> diagonalBlock(DenseIndex J) const {
│ │ │ │ +
141 return block_(J, J).selfadjointView<Eigen::Upper>();
│ │ │ │ +
142 }
│ │ │ │ +
│ │ │ │ +
143
│ │ │ │ +
│ │ │ │ +
145 Vector diagonal(DenseIndex J) const {
│ │ │ │ +
146 return block_(J, J).diagonal();
│ │ │ │ +
147 }
│ │ │ │ +
│ │ │ │ +
148
│ │ │ │ +
│ │ │ │ +
150 constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const {
│ │ │ │ +
151 assert(I < J);
│ │ │ │ +
152 return block_(I, J);
│ │ │ │ +
153 }
│ │ │ │ +
│ │ │ │ +
154
│ │ │ │ +
│ │ │ │ +
156 Eigen::SelfAdjointView<constBlock, Eigen::Upper> selfadjointView(
│ │ │ │ +
157 DenseIndex I, DenseIndex J) const {
│ │ │ │ +
158 assert(J > I);
│ │ │ │ +
159 return block_(I, I, J - I, J - I).selfadjointView<Eigen::Upper>();
│ │ │ │ +
160 }
│ │ │ │ +
│ │ │ │ +
161
│ │ │ │ +
│ │ │ │ +
163 Eigen::TriangularView<constBlock, Eigen::Upper> triangularView(DenseIndex I,
│ │ │ │ +
164 DenseIndex J) const {
│ │ │ │ +
165 assert(J > I);
│ │ │ │ +
166 return block_(I, I, J - I, J - I).triangularView<Eigen::Upper>();
│ │ │ │ +
167 }
│ │ │ │ +
│ │ │ │ +
168
│ │ │ │ +
│ │ │ │ +
170 constBlock aboveDiagonalRange(DenseIndex i_startBlock,
│ │ │ │ +
171 DenseIndex i_endBlock,
│ │ │ │ +
172 DenseIndex j_startBlock,
│ │ │ │ +
173 DenseIndex j_endBlock) const {
│ │ │ │ +
174 assert(i_startBlock < j_startBlock);
│ │ │ │ +
175 assert(i_endBlock <= j_startBlock);
│ │ │ │ +
176 return block_(i_startBlock, j_startBlock, i_endBlock - i_startBlock,
│ │ │ │ +
177 j_endBlock - j_startBlock);
│ │ │ │ +
178 }
│ │ │ │ +
│ │ │ │ +
179
│ │ │ │ +
│ │ │ │ +
181 Block aboveDiagonalRange(DenseIndex i_startBlock, DenseIndex i_endBlock,
│ │ │ │ +
182 DenseIndex j_startBlock, DenseIndex j_endBlock) {
│ │ │ │ +
183 assert(i_startBlock < j_startBlock);
│ │ │ │ +
184 assert(i_endBlock <= j_startBlock);
│ │ │ │ +
185 return block_(i_startBlock, j_startBlock, i_endBlock - i_startBlock,
│ │ │ │ +
186 j_endBlock - j_startBlock);
│ │ │ │ +
187 }
│ │ │ │ +
│ │ │ │ +
188
│ │ │ │ +
192
│ │ │ │ +
194 template <typename XprType>
│ │ │ │ +
│ │ │ │ +
195 void setDiagonalBlock(DenseIndex I, const XprType& xpr) {
│ │ │ │ +
196 block_(I, I).triangularView<Eigen::Upper>() = xpr.template triangularView<Eigen::Upper>();
│ │ │ │ +
197 }
│ │ │ │ +
│ │ │ │ +
198
│ │ │ │ +
200 template <typename XprType>
│ │ │ │ +
│ │ │ │ +
201 void setOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType& xpr) {
│ │ │ │ +
202 assert(I != J);
│ │ │ │ +
203 if (I < J) {
│ │ │ │ +
204 block_(I, J) = xpr;
│ │ │ │ +
205 } else {
│ │ │ │ +
206 block_(J, I) = xpr.transpose();
│ │ │ │ +
207 }
│ │ │ │ +
208 }
│ │ │ │ +
│ │ │ │ +
209
│ │ │ │ +
211 template <typename XprType>
│ │ │ │ +
│ │ │ │ +
212 void updateDiagonalBlock(DenseIndex I, const XprType& xpr) {
│ │ │ │ +
213 // TODO(gareth): Eigen won't let us add triangular or self-adjoint views
│ │ │ │ +
214 // here, so we do it manually.
│ │ │ │ +
215 auto dest = block_(I, I);
│ │ │ │ +
216 assert(dest.rows() == xpr.rows());
│ │ │ │ +
217 assert(dest.cols() == xpr.cols());
│ │ │ │ +
218 for (DenseIndex col = 0; col < dest.cols(); ++col) {
│ │ │ │ +
219 for (DenseIndex row = 0; row <= col; ++row) {
│ │ │ │ +
220 dest(row, col) += xpr(row, col);
│ │ │ │ +
221 }
│ │ │ │ +
222 }
│ │ │ │ +
223 }
│ │ │ │ +
│ │ │ │ +
224
│ │ │ │ +
227 template <typename XprType>
│ │ │ │ +
│ │ │ │ +
228 void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType& xpr) {
│ │ │ │ +
229 assert(I != J);
│ │ │ │ +
230 if (I < J) {
│ │ │ │ +
231 block_(I, J).noalias() += xpr;
│ │ │ │ +
232 } else {
│ │ │ │ +
233 block_(J, I).noalias() += xpr.transpose();
│ │ │ │ +
234 }
│ │ │ │ +
235 }
│ │ │ │ +
│ │ │ │ +
236
│ │ │ │ +
240
│ │ │ │ +
│ │ │ │ +
242 Eigen::SelfAdjointView<Block, Eigen::Upper> selfadjointView() {
│ │ │ │ +
243 return full().selfadjointView<Eigen::Upper>();
│ │ │ │ +
244 }
│ │ │ │ +
│ │ │ │ +
245
│ │ │ │ +
│ │ │ │ +
247 Eigen::SelfAdjointView<constBlock, Eigen::Upper> selfadjointView() const {
│ │ │ │ +
248 return full().selfadjointView<Eigen::Upper>();
│ │ │ │ +
249 }
│ │ │ │ +
│ │ │ │ +
250
│ │ │ │ +
252 template <typename XprType>
│ │ │ │ +
│ │ │ │ +
253 void setFullMatrix(const XprType& xpr) {
│ │ │ │ +
254 full().triangularView<Eigen::Upper>() = xpr.template triangularView<Eigen::Upper>();
│ │ │ │ +
255 }
│ │ │ │ +
│ │ │ │ +
256
│ │ │ │ +
│ │ │ │ +
258 void setZero() {
│ │ │ │ +
259 full().triangularView<Eigen::Upper>().setZero();
│ │ │ │ +
260 }
│ │ │ │ +
│ │ │ │ +
261
│ │ │ │ +
263 void negate();
│ │ │ │ +
264
│ │ │ │ +
266 void invertInPlace();
│ │ │ │ +
267
│ │ │ │ +
269
│ │ │ │ +
273 DenseIndex& blockStart() { return blockStart_; }
│ │ │ │ +
274
│ │ │ │ +
277 DenseIndex blockStart() const { return blockStart_; }
│ │ │ │ +
278
│ │ │ │ +
289 void choleskyPartial(DenseIndex nFrontals);
│ │ │ │ +
290
│ │ │ │ +
296 VerticalBlockMatrix split(DenseIndex nFrontals);
│ │ │ │ +
297
│ │ │ │ +
298 protected:
│ │ │ │ +
299
│ │ │ │ +
│ │ │ │ +
301 DenseIndex nOffsets() const {
│ │ │ │ +
302 return variableColOffsets_.size();
│ │ │ │ +
303 }
│ │ │ │ +
│ │ │ │ +
304
│ │ │ │ +
│ │ │ │ +
306 DenseIndex nActualBlocks() const {
│ │ │ │ +
307 return nOffsets() - 1;
│ │ │ │ +
308 }
│ │ │ │ +
│ │ │ │ +
309
│ │ │ │ +
│ │ │ │ +
311 DenseIndex offset(DenseIndex block) const {
│ │ │ │ +
312 assert(block >= 0);
│ │ │ │ +
313 const DenseIndex actual_index = block + blockStart();
│ │ │ │ +
314 assert(actual_index < nOffsets());
│ │ │ │ +
315 return variableColOffsets_[actual_index];
│ │ │ │ +
316 }
│ │ │ │ +
│ │ │ │ +
317
│ │ │ │ +
│ │ │ │ +
319 constBlock block_(DenseIndex iBlock, DenseIndex jBlock,
│ │ │ │ +
320 DenseIndex blockRows = 1, DenseIndex blockCols = 1) const {
│ │ │ │ +
321 const std::array<DenseIndex, 4> indices =
│ │ │ │ +
322 calcIndices(iBlock, jBlock, blockRows, blockCols);
│ │ │ │ +
323 return matrix_.block(indices[0], indices[1], indices[2], indices[3]);
│ │ │ │ +
324 }
│ │ │ │ +
│ │ │ │ +
325
│ │ │ │ +
│ │ │ │ +
327 Block block_(DenseIndex iBlock, DenseIndex jBlock, DenseIndex blockRows = 1,
│ │ │ │ +
328 DenseIndex blockCols = 1) {
│ │ │ │ +
329 const std::array<DenseIndex, 4> indices =
│ │ │ │ +
330 calcIndices(iBlock, jBlock, blockRows, blockCols);
│ │ │ │ +
331 return matrix_.block(indices[0], indices[1], indices[2], indices[3]);
│ │ │ │ +
332 }
│ │ │ │ +
│ │ │ │ +
333
│ │ │ │ +
│ │ │ │ +
335 constBlock full() const {
│ │ │ │ +
336 return block_(0, 0, nBlocks(), nBlocks());
│ │ │ │ +
337 }
│ │ │ │ +
│ │ │ │ +
338
│ │ │ │ +
│ │ │ │ +
340 Block full() {
│ │ │ │ +
341 return block_(0, 0, nBlocks(), nBlocks());
│ │ │ │ +
342 }
│ │ │ │ +
│ │ │ │ +
343
│ │ │ │ +
│ │ │ │ +
345 std::array<DenseIndex, 4> calcIndices(DenseIndex iBlock, DenseIndex jBlock,
│ │ │ │ +
346 DenseIndex blockRows,
│ │ │ │ +
347 DenseIndex blockCols) const {
│ │ │ │ +
348 assert(blockRows >= 0);
│ │ │ │ +
349 assert(blockCols >= 0);
│ │ │ │ +
350
│ │ │ │ +
351 // adjust indices to account for start and size of blocks
│ │ │ │ +
352 const DenseIndex denseI = offset(iBlock);
│ │ │ │ +
353 const DenseIndex denseJ = offset(jBlock);
│ │ │ │ +
354 const DenseIndex denseRows = offset(iBlock + blockRows) - denseI;
│ │ │ │ +
355 const DenseIndex denseCols = offset(jBlock + blockCols) - denseJ;
│ │ │ │ +
356 return {{denseI, denseJ, denseRows, denseCols}};
│ │ │ │ +
357 }
│ │ │ │ +
│ │ │ │ +
358
│ │ │ │ +
359 void assertInvariants() const
│ │ │ │ +
360 {
│ │ │ │ +
361 assert(matrix_.rows() == matrix_.cols());
│ │ │ │ +
362 assert(matrix_.cols() == variableColOffsets_.back());
│ │ │ │ +
363 assert(blockStart_ < (DenseIndex)variableColOffsets_.size());
│ │ │ │ +
364 }
│ │ │ │ +
365
│ │ │ │ +
366 template<typename ITERATOR>
│ │ │ │ +
367 void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension)
│ │ │ │ +
368 {
│ │ │ │ +
369 variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + (appendOneDimension ? 1 : 0));
│ │ │ │ +
370 variableColOffsets_[0] = 0;
│ │ │ │ +
371 DenseIndex j=0;
│ │ │ │ +
372 for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) {
│ │ │ │ +
373 variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
│ │ │ │ +
374 ++ j;
│ │ │ │ +
375 }
│ │ │ │ +
376 if(appendOneDimension)
│ │ │ │ +
377 {
│ │ │ │ +
378 variableColOffsets_[j+1] = variableColOffsets_[j] + 1;
│ │ │ │ +
379 ++ j;
│ │ │ │ +
380 }
│ │ │ │ +
381 }
│ │ │ │ +
382
│ │ │ │ +
383 friend class VerticalBlockMatrix;
│ │ │ │ +
384 template<typename SymmetricBlockMatrixType> friend class SymmetricBlockMatrixBlockExpr;
│ │ │ │ +
385
│ │ │ │ +
386 private:
│ │ │ │ +
388 friend class boost::serialization::access;
│ │ │ │ +
389 template<class ARCHIVE>
│ │ │ │ +
390 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
391 // Fill in the lower triangle part of the matrix, so boost::serialization won't
│ │ │ │ +
392 // complain about uninitialized data with an input_stream_error exception
│ │ │ │ +
393 // http://www.boost.org/doc/libs/1_37_0/libs/serialization/doc/exceptions.html#stream_error
│ │ │ │ +
394 matrix_.triangularView<Eigen::Lower>() = matrix_.triangularView<Eigen::Upper>().transpose();
│ │ │ │ +
395 ar & BOOST_SERIALIZATION_NVP(matrix_);
│ │ │ │ +
396 ar & BOOST_SERIALIZATION_NVP(variableColOffsets_);
│ │ │ │ +
397 ar & BOOST_SERIALIZATION_NVP(blockStart_);
│ │ │ │ +
398 }
│ │ │ │ +
399 };
│ │ │ │ +
│ │ │ │ +
400
│ │ │ │ +
402 class CholeskyFailed;
│ │ │ │ +
403
│ │ │ │ +
404}
│ │ │ │ +
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ +
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
types.h
Typedefs for easier changing of types.
│ │ │ │ +
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::FastSet
FastSet is a thin wrapper around std::set that uses the boost fast_pool_allocator instead of the defa...
Definition FastSet.h:50
│ │ │ │ -
gtsam::FastSet::merge
void merge(const FastSet &other)
insert another set: handy for MATLAB access
Definition FastSet.h:119
│ │ │ │ -
gtsam::FastSet::print
void print(const std::string &str="") const
Print to implement Testable: pretty basic.
Definition FastSet.h:101
│ │ │ │ -
gtsam::FastSet::FastSet
FastSet(const INPUTCONTAINER &container)
Constructor from a iterable container, passes through to base class.
Definition FastSet.h:65
│ │ │ │ -
gtsam::FastSet::FastSet
FastSet(const FastSet< VALUE > &x)
Copy constructor from another FastSet.
Definition FastSet.h:70
│ │ │ │ -
gtsam::FastSet::exists
bool exists(const VALUE &e) const
Handy 'exists' function.
Definition FastSet.h:96
│ │ │ │ -
gtsam::FastSet::equals
bool equals(const FastSet< VALUE > &other, double tol=1e-9) const
Check for equality within tolerance to implement Testable.
Definition FastSet.h:107
│ │ │ │ -
gtsam::FastSet::access
friend class boost::serialization::access
Serialization function.
Definition FastSet.h:125
│ │ │ │ -
gtsam::FastSet::FastSet
FastSet(const Base &x)
Copy constructor from the base set class.
Definition FastSet.h:75
│ │ │ │ -
gtsam::FastSet::FastSet
FastSet()=default
Default constructor.
│ │ │ │ -
gtsam::IsTestable
A testable concept check that should be placed in applicable unit tests and in generic algorithms.
Definition Testable.h:58
│ │ │ │ +
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │ +
gtsam::row
const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j)
Extracts a row view from a matrix that avoids a copy.
Definition Matrix.h:222
│ │ │ │ +
gtsam::split
void split(const G &g, const PredecessorMap< KEY > &tree, G &Ab1, G &Ab2)
Split the graph into two parts: one corresponds to the given spanning tree, and the other corresponds...
Definition graph-inl.h:255
│ │ │ │ +
gtsam::choleskyPartial
bool choleskyPartial(Matrix &ABC, size_t nFrontal, size_t topleft)
Partial Cholesky computes a factor [R S such that [R' 0 [R S = [A B 0 L] S' I] 0 L] B' C].
Definition cholesky.cpp:108
│ │ │ │ +
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
│ │ │ │ +
gtsam::SymmetricBlockMatrix::full
Block full()
Get the full matrix as a block.
Definition SymmetricBlockMatrix.h:340
│ │ │ │ +
gtsam::SymmetricBlockMatrix::blockStart_
DenseIndex blockStart_
Changes apparent matrix view, see main class comment.
Definition SymmetricBlockMatrix.h:62
│ │ │ │ +
gtsam::SymmetricBlockMatrix::setDiagonalBlock
void setDiagonalBlock(DenseIndex I, const XprType &xpr)
Set a diagonal block. Only the upper triangular portion of xpr is evaluated.
Definition SymmetricBlockMatrix.h:195
│ │ │ │ +
gtsam::SymmetricBlockMatrix::nActualBlocks
DenseIndex nActualBlocks() const
Number of actual blocks in the full matrix.
Definition SymmetricBlockMatrix.h:306
│ │ │ │ +
gtsam::SymmetricBlockMatrix::diagonal
Vector diagonal(DenseIndex J) const
Get the diagonal of the J'th diagonal block.
Definition SymmetricBlockMatrix.h:145
│ │ │ │ +
gtsam::SymmetricBlockMatrix::matrix_
Matrix matrix_
The full matrix.
Definition SymmetricBlockMatrix.h:59
│ │ │ │ +
gtsam::SymmetricBlockMatrix::cols
DenseIndex cols() const
Column size.
Definition SymmetricBlockMatrix.h:117
│ │ │ │ +
gtsam::SymmetricBlockMatrix::getDim
DenseIndex getDim(DenseIndex block) const
Number of dimensions for variable on this diagonal block.
Definition SymmetricBlockMatrix.h:123
│ │ │ │ +
gtsam::SymmetricBlockMatrix::setZero
void setZero()
Set the entire active matrix zero.
Definition SymmetricBlockMatrix.h:258
│ │ │ │ +
gtsam::SymmetricBlockMatrix::setOffDiagonalBlock
void setOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
Set an off-diagonal block. Only the upper triangular portion of xpr is evaluated.
Definition SymmetricBlockMatrix.h:201
│ │ │ │ +
gtsam::SymmetricBlockMatrix::setFullMatrix
void setFullMatrix(const XprType &xpr)
Set the entire active matrix. Only reads the upper triangular part of xpr.
Definition SymmetricBlockMatrix.h:253
│ │ │ │ +
gtsam::SymmetricBlockMatrix::selfadjointView
Eigen::SelfAdjointView< Block, Eigen::Upper > selfadjointView()
Get self adjoint view.
Definition SymmetricBlockMatrix.h:242
│ │ │ │ +
gtsam::SymmetricBlockMatrix::aboveDiagonalBlock
constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
Get block above the diagonal (I, J).
Definition SymmetricBlockMatrix.h:150
│ │ │ │ +
gtsam::SymmetricBlockMatrix::block_
constBlock block_(DenseIndex iBlock, DenseIndex jBlock, DenseIndex blockRows=1, DenseIndex blockCols=1) const
Get an arbitrary block from the matrix. Indices are in block units.
Definition SymmetricBlockMatrix.h:319
│ │ │ │ +
gtsam::SymmetricBlockMatrix::full
constBlock full() const
Get the full matrix as a block.
Definition SymmetricBlockMatrix.h:335
│ │ │ │ +
gtsam::SymmetricBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition SymmetricBlockMatrix.h:114
│ │ │ │ +
gtsam::SymmetricBlockMatrix::aboveDiagonalRange
Block aboveDiagonalRange(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock)
Get a range [i,j) from the matrix. Indices are in block units.
Definition SymmetricBlockMatrix.h:181
│ │ │ │ +
gtsam::SymmetricBlockMatrix::triangularView
Eigen::TriangularView< constBlock, Eigen::Upper > triangularView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j) as a triangular view.
Definition SymmetricBlockMatrix.h:163
│ │ │ │ +
gtsam::SymmetricBlockMatrix::diagonalBlock
Eigen::SelfAdjointView< constBlock, Eigen::Upper > diagonalBlock(DenseIndex J) const
Return the J'th diagonal block as a self adjoint view.
Definition SymmetricBlockMatrix.h:140
│ │ │ │ +
gtsam::SymmetricBlockMatrix::SymmetricBlockMatrix
SymmetricBlockMatrix(const CONTAINER &dimensions, const Matrix &matrix, bool appendOneDimension=false)
Construct from a container of the sizes of each vertical block and a pre-prepared matrix.
Definition SymmetricBlockMatrix.h:90
│ │ │ │ +
gtsam::SymmetricBlockMatrix::blockStart
DenseIndex blockStart() const
Retrieve the first logical block, i.e.
Definition SymmetricBlockMatrix.h:277
│ │ │ │ +
gtsam::SymmetricBlockMatrix::selfadjointView
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView() const
Get self adjoint view.
Definition SymmetricBlockMatrix.h:247
│ │ │ │ +
gtsam::SymmetricBlockMatrix::updateOffDiagonalBlock
void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
Update an off diagonal block.
Definition SymmetricBlockMatrix.h:228
│ │ │ │ +
gtsam::SymmetricBlockMatrix::SymmetricBlockMatrix
SymmetricBlockMatrix(const CONTAINER &dimensions, bool appendOneDimension=false)
Construct from a container of the sizes of each block.
Definition SymmetricBlockMatrix.h:70
│ │ │ │ +
gtsam::SymmetricBlockMatrix::SymmetricBlockMatrix
SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension=false)
Construct from iterator over the sizes of each vertical block.
Definition SymmetricBlockMatrix.h:80
│ │ │ │ +
gtsam::SymmetricBlockMatrix::blockStart
DenseIndex & blockStart()
Retrieve or modify the first logical block, i.e.
Definition SymmetricBlockMatrix.h:273
│ │ │ │ +
gtsam::SymmetricBlockMatrix::aboveDiagonalRange
constBlock aboveDiagonalRange(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const
Get a range [i,j) from the matrix. Indices are in block units.
Definition SymmetricBlockMatrix.h:170
│ │ │ │ +
gtsam::SymmetricBlockMatrix::diagonalBlock
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J'th diagonal block as a self adjoint view.
Definition SymmetricBlockMatrix.h:135
│ │ │ │ +
gtsam::SymmetricBlockMatrix::block_
Block block_(DenseIndex iBlock, DenseIndex jBlock, DenseIndex blockRows=1, DenseIndex blockCols=1)
Get an arbitrary block from the matrix. Indices are in block units.
Definition SymmetricBlockMatrix.h:327
│ │ │ │ +
gtsam::SymmetricBlockMatrix::updateDiagonalBlock
void updateDiagonalBlock(DenseIndex I, const XprType &xpr)
Increment the diagonal block by the values in xpr. Only reads the upper triangular part of xpr.
Definition SymmetricBlockMatrix.h:212
│ │ │ │ +
gtsam::SymmetricBlockMatrix::nOffsets
DenseIndex nOffsets() const
Number of offsets in the full matrix.
Definition SymmetricBlockMatrix.h:301
│ │ │ │ +
gtsam::SymmetricBlockMatrix::calcIndices
std::array< DenseIndex, 4 > calcIndices(DenseIndex iBlock, DenseIndex jBlock, DenseIndex blockRows, DenseIndex blockCols) const
Compute the indices into the underlying matrix for a given block.
Definition SymmetricBlockMatrix.h:345
│ │ │ │ +
gtsam::SymmetricBlockMatrix::nBlocks
DenseIndex nBlocks() const
Block count.
Definition SymmetricBlockMatrix.h:120
│ │ │ │ +
gtsam::SymmetricBlockMatrix::variableColOffsets_
FastVector< DenseIndex > variableColOffsets_
the starting columns of each block (0-based)
Definition SymmetricBlockMatrix.h:60
│ │ │ │ +
gtsam::SymmetricBlockMatrix::selfadjointView
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j).
Definition SymmetricBlockMatrix.h:156
│ │ │ │ +
gtsam::SymmetricBlockMatrix::offset
DenseIndex offset(DenseIndex block) const
Get an offset for a block index (in the active view).
Definition SymmetricBlockMatrix.h:311
│ │ │ │ +
gtsam::VerticalBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,176 +1,546 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -FastSet.h │ │ │ │ │ +SymmetricBlockMatrix.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ -4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -5 * All Rights Reserved │ │ │ │ │ -6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +3* GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4* Atlanta, Georgia 30332-0415 │ │ │ │ │ +5* All Rights Reserved │ │ │ │ │ +6* Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ -8 * See LICENSE for the license information │ │ │ │ │ +8* See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ -10 * ------------------------------------------------------------------------- │ │ │ │ │ +10* ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include │ │ │ │ │ -22#if BOOST_VERSION >= 107400 │ │ │ │ │ -23#include │ │ │ │ │ -24#endif │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ -27#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_._h> │ │ │ │ │ -28#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ -29 │ │ │ │ │ -30#include │ │ │ │ │ -31#include │ │ │ │ │ -32 │ │ │ │ │ -33namespace boost { │ │ │ │ │ -34namespace serialization { │ │ │ │ │ -35class access; │ │ │ │ │ -36} /* namespace serialization */ │ │ │ │ │ -37} /* namespace boost */ │ │ │ │ │ -38 │ │ │ │ │ -39namespace _g_t_s_a_m { │ │ │ │ │ -40 │ │ │ │ │ -48template │ │ │ │ │ -_4_9class _F_a_s_t_S_e_t: public std::set, │ │ │ │ │ -50 typename internal::FastDefaultAllocator::type> { │ │ │ │ │ -51 │ │ │ │ │ -52 BOOST_CONCEPT_ASSERT ((_I_s_T_e_s_t_a_b_l_e_<_V_A_L_U_E_> )); │ │ │ │ │ -53 │ │ │ │ │ -54public: │ │ │ │ │ -55 │ │ │ │ │ -56 typedef std::set, │ │ │ │ │ -57 typename internal::FastDefaultAllocator::type> Base; │ │ │ │ │ -58 │ │ │ │ │ -59 using Base::Base; // Inherit the set constructors │ │ │ │ │ -60 │ │ │ │ │ -_6_1 _F_a_s_t_S_e_t() = default; │ │ │ │ │ -62 │ │ │ │ │ -64 template │ │ │ │ │ -_6_5 explicit _F_a_s_t_S_e_t(const INPUTCONTAINER& container) : │ │ │ │ │ -66 Base(container.begin(), container.end()) { │ │ │ │ │ -67 } │ │ │ │ │ -68 │ │ │ │ │ -_7_0 _F_a_s_t_S_e_t(const _F_a_s_t_S_e_t_<_V_A_L_U_E_>& x) : │ │ │ │ │ -71 Base(x) { │ │ │ │ │ -72 } │ │ │ │ │ -73 │ │ │ │ │ -_7_5 _F_a_s_t_S_e_t(const Base& x) : │ │ │ │ │ -76 Base(x) { │ │ │ │ │ -77 } │ │ │ │ │ -78 │ │ │ │ │ -79#ifdef GTSAM_ALLOCATOR_BOOSTPOOL │ │ │ │ │ -81 _F_a_s_t_S_e_t(const std::set& x) { │ │ │ │ │ -82 // This if statement works around a bug in boost pool allocator and/or │ │ │ │ │ -83 // STL vector where if the size is zero, the pool allocator will allocate │ │ │ │ │ -84 // huge amounts of memory. │ │ │ │ │ -85 if(x.size() > 0) │ │ │ │ │ -86 Base::insert(x.begin(), x.end()); │ │ │ │ │ -87 } │ │ │ │ │ -88#endif │ │ │ │ │ -89 │ │ │ │ │ -_9_1 operator std::set() const { │ │ │ │ │ -92 return std::set(this->begin(), this->end()); │ │ │ │ │ -93 } │ │ │ │ │ -94 │ │ │ │ │ -_9_6 bool _e_x_i_s_t_s(const VALUE& e) const { │ │ │ │ │ -97 return this->find(e) != this->end(); │ │ │ │ │ -98 } │ │ │ │ │ -99 │ │ │ │ │ -_1_0_1 void _p_r_i_n_t(const std::string& str = "") const { │ │ │ │ │ -102 for (typename Base::const_iterator it = this->begin(); it != this->end(); │ │ │ │ │ -++it) │ │ │ │ │ -103 _t_r_a_i_t_s_<_V_A_L_U_E_>_:_:_P_r_i_n_t(*it, str); │ │ │ │ │ -104 } │ │ │ │ │ -105 │ │ │ │ │ -_1_0_7 bool _e_q_u_a_l_s(const _F_a_s_t_S_e_t_<_V_A_L_U_E_>& other, double tol = 1e-9) const { │ │ │ │ │ -108 typename Base::const_iterator it1 = this->begin(), it2 = other.begin(); │ │ │ │ │ -109 while (it1 != this->end()) { │ │ │ │ │ -110 if (it2 == other.end() || !_t_r_a_i_t_s_<_V_A_L_U_E_>_:_:_E_q_u_a_l_s(*it2, *it2, tol)) │ │ │ │ │ -111 return false; │ │ │ │ │ -112 ++it1; │ │ │ │ │ -113 ++it2; │ │ │ │ │ -114 } │ │ │ │ │ -115 return true; │ │ │ │ │ -116 } │ │ │ │ │ -117 │ │ │ │ │ -_1_1_9 void _m_e_r_g_e(const _F_a_s_t_S_e_t& other) { │ │ │ │ │ -120 Base::insert(other.begin(), other.end()); │ │ │ │ │ -121 } │ │ │ │ │ -122 │ │ │ │ │ -123private: │ │ │ │ │ -_1_2_5 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -126 template │ │ │ │ │ -127 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -128 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); │ │ │ │ │ -129 } │ │ │ │ │ -130}; │ │ │ │ │ -131 │ │ │ │ │ -132} │ │ │ │ │ -_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_._h │ │ │ │ │ -An easy way to control which allocator is used for Fast* collections. │ │ │ │ │ -_T_e_s_t_a_b_l_e_._h │ │ │ │ │ -Concept check for values that can be used in unit tests. │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ +23#include │ │ │ │ │ +24#include │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28 │ │ │ │ │ +29namespace boost { │ │ │ │ │ +30namespace serialization { │ │ │ │ │ +31class access; │ │ │ │ │ +32} /* namespace serialization */ │ │ │ │ │ +33} /* namespace boost */ │ │ │ │ │ +34 │ │ │ │ │ +35namespace _g_t_s_a_m { │ │ │ │ │ +36 │ │ │ │ │ +37 // Forward declarations │ │ │ │ │ +38 class VerticalBlockMatrix; │ │ │ │ │ +39 │ │ │ │ │ +_5_1 class GTSAM_EXPORT _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +52 { │ │ │ │ │ +53 public: │ │ │ │ │ +54 typedef _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x _T_h_i_s; │ │ │ │ │ +55 typedef Eigen::Block Block; │ │ │ │ │ +56 typedef Eigen::Block constBlock; │ │ │ │ │ +57 │ │ │ │ │ +58 protected: │ │ │ │ │ +_5_9 Matrix _m_a_t_r_i_x__; │ │ │ │ │ +_6_0 _F_a_s_t_V_e_c_t_o_r_<_D_e_n_s_e_I_n_d_e_x_> _v_a_r_i_a_b_l_e_C_o_l_O_f_f_s_e_t_s__; │ │ │ │ │ +61 │ │ │ │ │ +_6_2 _D_e_n_s_e_I_n_d_e_x _b_l_o_c_k_S_t_a_r_t__; │ │ │ │ │ +63 │ │ │ │ │ +64 public: │ │ │ │ │ +66 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x(); │ │ │ │ │ +67 │ │ │ │ │ +69 template │ │ │ │ │ +_7_0 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x(const CONTAINER& dimensions, bool appendOneDimension = │ │ │ │ │ +false) : │ │ │ │ │ +71 blockStart_(0) │ │ │ │ │ +72 { │ │ │ │ │ +73 fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); │ │ │ │ │ +74 matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back()); │ │ │ │ │ +75 assertInvariants(); │ │ │ │ │ +76 } │ │ │ │ │ +77 │ │ │ │ │ +79 template │ │ │ │ │ +_8_0 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool │ │ │ │ │ +appendOneDimension = false) : │ │ │ │ │ +81 blockStart_(0) │ │ │ │ │ +82 { │ │ │ │ │ +83 fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension); │ │ │ │ │ +84 matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back()); │ │ │ │ │ +85 assertInvariants(); │ │ │ │ │ +86 } │ │ │ │ │ +87 │ │ │ │ │ +89 template │ │ │ │ │ +_9_0 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x(const CONTAINER& dimensions, const Matrix& matrix, bool │ │ │ │ │ +appendOneDimension = false) : │ │ │ │ │ +91 blockStart_(0) │ │ │ │ │ +92 { │ │ │ │ │ +93 matrix_.resize(matrix.rows(), matrix.cols()); │ │ │ │ │ +94 matrix_.triangularView() = matrix.triangularView │ │ │ │ │ +(); │ │ │ │ │ +95 fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); │ │ │ │ │ +96 if(matrix_.rows() != matrix_.cols()) │ │ │ │ │ +97 throw std::invalid_argument("Requested to create a SymmetricBlockMatrix from │ │ │ │ │ +a non-square matrix."); │ │ │ │ │ +98 if(variableColOffsets_.back() != matrix_.cols()) │ │ │ │ │ +99 throw std::invalid_argument("Requested to create a SymmetricBlockMatrix with │ │ │ │ │ +dimensions that do not sum to the total size of the provided matrix."); │ │ │ │ │ +100 assertInvariants(); │ │ │ │ │ +101 } │ │ │ │ │ +102 │ │ │ │ │ +106 static _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x LikeActiveViewOf(const _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x& │ │ │ │ │ +other); │ │ │ │ │ +107 │ │ │ │ │ +111 static _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x LikeActiveViewOf(const _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x& │ │ │ │ │ +other); │ │ │ │ │ +112 │ │ │ │ │ +_1_1_4 _D_e_n_s_e_I_n_d_e_x _r_o_w_s() const { assertInvariants(); return │ │ │ │ │ +variableColOffsets_.back() - variableColOffsets_[blockStart_]; } │ │ │ │ │ +115 │ │ │ │ │ +_1_1_7 _D_e_n_s_e_I_n_d_e_x _c_o_l_s() const { return rows(); } │ │ │ │ │ +118 │ │ │ │ │ +_1_2_0 _D_e_n_s_e_I_n_d_e_x _n_B_l_o_c_k_s() const { return nActualBlocks() - blockStart_; } │ │ │ │ │ +121 │ │ │ │ │ +_1_2_3 _D_e_n_s_e_I_n_d_e_x _g_e_t_D_i_m(_D_e_n_s_e_I_n_d_e_x block) const { │ │ │ │ │ +124 return calcIndices(block, block, 1, 1)[2]; │ │ │ │ │ +125 } │ │ │ │ │ +126 │ │ │ │ │ +129 │ │ │ │ │ +132 Matrix block(_D_e_n_s_e_I_n_d_e_x I, _D_e_n_s_e_I_n_d_e_x J) const; │ │ │ │ │ +133 │ │ │ │ │ +_1_3_5 Eigen::SelfAdjointView _d_i_a_g_o_n_a_l_B_l_o_c_k(_D_e_n_s_e_I_n_d_e_x J) { │ │ │ │ │ +136 return block_(J, J).selfadjointView(); │ │ │ │ │ +137 } │ │ │ │ │ +138 │ │ │ │ │ +_1_4_0 Eigen::SelfAdjointView _d_i_a_g_o_n_a_l_B_l_o_c_k(_D_e_n_s_e_I_n_d_e_x │ │ │ │ │ +J) const { │ │ │ │ │ +141 return block_(J, J).selfadjointView(); │ │ │ │ │ +142 } │ │ │ │ │ +143 │ │ │ │ │ +_1_4_5 Vector _d_i_a_g_o_n_a_l(_D_e_n_s_e_I_n_d_e_x J) const { │ │ │ │ │ +146 return block_(J, J).diagonal(); │ │ │ │ │ +147 } │ │ │ │ │ +148 │ │ │ │ │ +_1_5_0 constBlock _a_b_o_v_e_D_i_a_g_o_n_a_l_B_l_o_c_k(_D_e_n_s_e_I_n_d_e_x I, _D_e_n_s_e_I_n_d_e_x J) const { │ │ │ │ │ +151 assert(I < J); │ │ │ │ │ +152 return block_(I, J); │ │ │ │ │ +153 } │ │ │ │ │ +154 │ │ │ │ │ +_1_5_6 Eigen::SelfAdjointView _s_e_l_f_a_d_j_o_i_n_t_V_i_e_w( │ │ │ │ │ +157 _D_e_n_s_e_I_n_d_e_x I, _D_e_n_s_e_I_n_d_e_x J) const { │ │ │ │ │ +158 assert(J > I); │ │ │ │ │ +159 return block_(I, I, J - I, J - I).selfadjointView(); │ │ │ │ │ +160 } │ │ │ │ │ +161 │ │ │ │ │ +_1_6_3 Eigen::TriangularView _t_r_i_a_n_g_u_l_a_r_V_i_e_w(_D_e_n_s_e_I_n_d_e_x │ │ │ │ │ +I, │ │ │ │ │ +164 _D_e_n_s_e_I_n_d_e_x J) const { │ │ │ │ │ +165 assert(J > I); │ │ │ │ │ +166 return block_(I, I, J - I, J - I).triangularView(); │ │ │ │ │ +167 } │ │ │ │ │ +168 │ │ │ │ │ +_1_7_0 constBlock _a_b_o_v_e_D_i_a_g_o_n_a_l_R_a_n_g_e(_D_e_n_s_e_I_n_d_e_x i_startBlock, │ │ │ │ │ +171 _D_e_n_s_e_I_n_d_e_x i_endBlock, │ │ │ │ │ +172 _D_e_n_s_e_I_n_d_e_x j_startBlock, │ │ │ │ │ +173 _D_e_n_s_e_I_n_d_e_x j_endBlock) const { │ │ │ │ │ +174 assert(i_startBlock < j_startBlock); │ │ │ │ │ +175 assert(i_endBlock <= j_startBlock); │ │ │ │ │ +176 return block_(i_startBlock, j_startBlock, i_endBlock - i_startBlock, │ │ │ │ │ +177 j_endBlock - j_startBlock); │ │ │ │ │ +178 } │ │ │ │ │ +179 │ │ │ │ │ +_1_8_1 Block _a_b_o_v_e_D_i_a_g_o_n_a_l_R_a_n_g_e(_D_e_n_s_e_I_n_d_e_x i_startBlock, _D_e_n_s_e_I_n_d_e_x i_endBlock, │ │ │ │ │ +182 _D_e_n_s_e_I_n_d_e_x j_startBlock, _D_e_n_s_e_I_n_d_e_x j_endBlock) { │ │ │ │ │ +183 assert(i_startBlock < j_startBlock); │ │ │ │ │ +184 assert(i_endBlock <= j_startBlock); │ │ │ │ │ +185 return block_(i_startBlock, j_startBlock, i_endBlock - i_startBlock, │ │ │ │ │ +186 j_endBlock - j_startBlock); │ │ │ │ │ +187 } │ │ │ │ │ +188 │ │ │ │ │ +192 │ │ │ │ │ +194 template │ │ │ │ │ +_1_9_5 void _s_e_t_D_i_a_g_o_n_a_l_B_l_o_c_k(_D_e_n_s_e_I_n_d_e_x I, const XprType& xpr) { │ │ │ │ │ +196 block_(I, I).triangularView() = xpr.template │ │ │ │ │ +triangularView(); │ │ │ │ │ +197 } │ │ │ │ │ +198 │ │ │ │ │ +200 template │ │ │ │ │ +_2_0_1 void _s_e_t_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k(_D_e_n_s_e_I_n_d_e_x I, _D_e_n_s_e_I_n_d_e_x J, const XprType& xpr) { │ │ │ │ │ +202 assert(I != J); │ │ │ │ │ +203 if (I < J) { │ │ │ │ │ +204 block_(I, J) = xpr; │ │ │ │ │ +205 } else { │ │ │ │ │ +206 block_(J, I) = xpr.transpose(); │ │ │ │ │ +207 } │ │ │ │ │ +208 } │ │ │ │ │ +209 │ │ │ │ │ +211 template │ │ │ │ │ +_2_1_2 void _u_p_d_a_t_e_D_i_a_g_o_n_a_l_B_l_o_c_k(_D_e_n_s_e_I_n_d_e_x I, const XprType& xpr) { │ │ │ │ │ +213 // TODO(gareth): Eigen won't let us add triangular or self-adjoint views │ │ │ │ │ +214 // here, so we do it manually. │ │ │ │ │ +215 auto dest = block_(I, I); │ │ │ │ │ +216 assert(dest.rows() == xpr.rows()); │ │ │ │ │ +217 assert(dest.cols() == xpr.cols()); │ │ │ │ │ +218 for (_D_e_n_s_e_I_n_d_e_x col = 0; col < dest.cols(); ++col) { │ │ │ │ │ +219 for (_D_e_n_s_e_I_n_d_e_x _r_o_w = 0; _r_o_w <= col; ++_r_o_w) { │ │ │ │ │ +220 dest(_r_o_w, col) += xpr(_r_o_w, col); │ │ │ │ │ +221 } │ │ │ │ │ +222 } │ │ │ │ │ +223 } │ │ │ │ │ +224 │ │ │ │ │ +227 template │ │ │ │ │ +_2_2_8 void _u_p_d_a_t_e_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k(_D_e_n_s_e_I_n_d_e_x I, _D_e_n_s_e_I_n_d_e_x J, const XprType& xpr) │ │ │ │ │ +{ │ │ │ │ │ +229 assert(I != J); │ │ │ │ │ +230 if (I < J) { │ │ │ │ │ +231 block_(I, J).noalias() += xpr; │ │ │ │ │ +232 } else { │ │ │ │ │ +233 block_(J, I).noalias() += xpr.transpose(); │ │ │ │ │ +234 } │ │ │ │ │ +235 } │ │ │ │ │ +236 │ │ │ │ │ +240 │ │ │ │ │ +_2_4_2 Eigen::SelfAdjointView _s_e_l_f_a_d_j_o_i_n_t_V_i_e_w() { │ │ │ │ │ +243 return full().selfadjointView(); │ │ │ │ │ +244 } │ │ │ │ │ +245 │ │ │ │ │ +_2_4_7 Eigen::SelfAdjointView _s_e_l_f_a_d_j_o_i_n_t_V_i_e_w() const { │ │ │ │ │ +248 return full().selfadjointView(); │ │ │ │ │ +249 } │ │ │ │ │ +250 │ │ │ │ │ +252 template │ │ │ │ │ +_2_5_3 void _s_e_t_F_u_l_l_M_a_t_r_i_x(const XprType& xpr) { │ │ │ │ │ +254 full().triangularView() = xpr.template triangularView(); │ │ │ │ │ +255 } │ │ │ │ │ +256 │ │ │ │ │ +_2_5_8 void _s_e_t_Z_e_r_o() { │ │ │ │ │ +259 full().triangularView().setZero(); │ │ │ │ │ +260 } │ │ │ │ │ +261 │ │ │ │ │ +263 void negate(); │ │ │ │ │ +264 │ │ │ │ │ +266 void invertInPlace(); │ │ │ │ │ +267 │ │ │ │ │ +269 │ │ │ │ │ +_2_7_3 _D_e_n_s_e_I_n_d_e_x& _b_l_o_c_k_S_t_a_r_t() { return blockStart_; } │ │ │ │ │ +274 │ │ │ │ │ +_2_7_7 _D_e_n_s_e_I_n_d_e_x _b_l_o_c_k_S_t_a_r_t() const { return blockStart_; } │ │ │ │ │ +278 │ │ │ │ │ +289 void _c_h_o_l_e_s_k_y_P_a_r_t_i_a_l(_D_e_n_s_e_I_n_d_e_x nFrontals); │ │ │ │ │ +290 │ │ │ │ │ +296 _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x _s_p_l_i_t(_D_e_n_s_e_I_n_d_e_x nFrontals); │ │ │ │ │ +297 │ │ │ │ │ +298 protected: │ │ │ │ │ +299 │ │ │ │ │ +_3_0_1 _D_e_n_s_e_I_n_d_e_x _n_O_f_f_s_e_t_s() const { │ │ │ │ │ +302 return variableColOffsets_.size(); │ │ │ │ │ +303 } │ │ │ │ │ +304 │ │ │ │ │ +_3_0_6 _D_e_n_s_e_I_n_d_e_x _n_A_c_t_u_a_l_B_l_o_c_k_s() const { │ │ │ │ │ +307 return nOffsets() - 1; │ │ │ │ │ +308 } │ │ │ │ │ +309 │ │ │ │ │ +_3_1_1 _D_e_n_s_e_I_n_d_e_x _o_f_f_s_e_t(_D_e_n_s_e_I_n_d_e_x block) const { │ │ │ │ │ +312 assert(block >= 0); │ │ │ │ │ +313 const _D_e_n_s_e_I_n_d_e_x actual_index = block + blockStart(); │ │ │ │ │ +314 assert(actual_index < nOffsets()); │ │ │ │ │ +315 return variableColOffsets_[actual_index]; │ │ │ │ │ +316 } │ │ │ │ │ +317 │ │ │ │ │ +_3_1_9 constBlock _b_l_o_c_k__(_D_e_n_s_e_I_n_d_e_x iBlock, _D_e_n_s_e_I_n_d_e_x jBlock, │ │ │ │ │ +320 _D_e_n_s_e_I_n_d_e_x blockRows = 1, _D_e_n_s_e_I_n_d_e_x blockCols = 1) const { │ │ │ │ │ +321 const std::array indices = │ │ │ │ │ +322 calcIndices(iBlock, jBlock, blockRows, blockCols); │ │ │ │ │ +323 return matrix_.block(indices[0], indices[1], indices[2], indices[3]); │ │ │ │ │ +324 } │ │ │ │ │ +325 │ │ │ │ │ +_3_2_7 Block _b_l_o_c_k__(_D_e_n_s_e_I_n_d_e_x iBlock, _D_e_n_s_e_I_n_d_e_x jBlock, _D_e_n_s_e_I_n_d_e_x blockRows = │ │ │ │ │ +1, │ │ │ │ │ +328 _D_e_n_s_e_I_n_d_e_x blockCols = 1) { │ │ │ │ │ +329 const std::array indices = │ │ │ │ │ +330 calcIndices(iBlock, jBlock, blockRows, blockCols); │ │ │ │ │ +331 return matrix_.block(indices[0], indices[1], indices[2], indices[3]); │ │ │ │ │ +332 } │ │ │ │ │ +333 │ │ │ │ │ +_3_3_5 constBlock _f_u_l_l() const { │ │ │ │ │ +336 return block_(0, 0, nBlocks(), nBlocks()); │ │ │ │ │ +337 } │ │ │ │ │ +338 │ │ │ │ │ +_3_4_0 Block _f_u_l_l() { │ │ │ │ │ +341 return block_(0, 0, nBlocks(), nBlocks()); │ │ │ │ │ +342 } │ │ │ │ │ +343 │ │ │ │ │ +_3_4_5 std::array _c_a_l_c_I_n_d_i_c_e_s(_D_e_n_s_e_I_n_d_e_x iBlock, _D_e_n_s_e_I_n_d_e_x jBlock, │ │ │ │ │ +346 _D_e_n_s_e_I_n_d_e_x blockRows, │ │ │ │ │ +347 _D_e_n_s_e_I_n_d_e_x blockCols) const { │ │ │ │ │ +348 assert(blockRows >= 0); │ │ │ │ │ +349 assert(blockCols >= 0); │ │ │ │ │ +350 │ │ │ │ │ +351 // adjust indices to account for start and size of blocks │ │ │ │ │ +352 const _D_e_n_s_e_I_n_d_e_x denseI = offset(iBlock); │ │ │ │ │ +353 const _D_e_n_s_e_I_n_d_e_x denseJ = offset(jBlock); │ │ │ │ │ +354 const _D_e_n_s_e_I_n_d_e_x denseRows = offset(iBlock + blockRows) - denseI; │ │ │ │ │ +355 const _D_e_n_s_e_I_n_d_e_x denseCols = offset(jBlock + blockCols) - denseJ; │ │ │ │ │ +356 return {{denseI, denseJ, denseRows, denseCols}}; │ │ │ │ │ +357 } │ │ │ │ │ +358 │ │ │ │ │ +359 void assertInvariants() const │ │ │ │ │ +360 { │ │ │ │ │ +361 assert(matrix_.rows() == matrix_.cols()); │ │ │ │ │ +362 assert(matrix_.cols() == variableColOffsets_.back()); │ │ │ │ │ +363 assert(blockStart_ < (_D_e_n_s_e_I_n_d_e_x)variableColOffsets_.size()); │ │ │ │ │ +364 } │ │ │ │ │ +365 │ │ │ │ │ +366 template │ │ │ │ │ +367 void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool │ │ │ │ │ +appendOneDimension) │ │ │ │ │ +368 { │ │ │ │ │ +369 variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + │ │ │ │ │ +(appendOneDimension ? 1 : 0)); │ │ │ │ │ +370 variableColOffsets_[0] = 0; │ │ │ │ │ +371 DenseIndex j=0; │ │ │ │ │ +372 for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) { │ │ │ │ │ +373 variableColOffsets_[j+1] = variableColOffsets_[j] + *dim; │ │ │ │ │ +374 ++ j; │ │ │ │ │ +375 } │ │ │ │ │ +376 if(appendOneDimension) │ │ │ │ │ +377 { │ │ │ │ │ +378 variableColOffsets_[j+1] = variableColOffsets_[j] + 1; │ │ │ │ │ +379 ++ j; │ │ │ │ │ +380 } │ │ │ │ │ +381 } │ │ │ │ │ +382 │ │ │ │ │ +383 friend class VerticalBlockMatrix; │ │ │ │ │ +384 template friend class │ │ │ │ │ +SymmetricBlockMatrixBlockExpr; │ │ │ │ │ +385 │ │ │ │ │ +386 private: │ │ │ │ │ +_3_8_8 friend class boost::serialization::access; │ │ │ │ │ +389 template │ │ │ │ │ +390 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +391 // Fill in the lower triangle part of the matrix, so boost::serialization │ │ │ │ │ +won't │ │ │ │ │ +392 // complain about uninitialized data with an input_stream_error exception │ │ │ │ │ +393 // http://www.boost.org/doc/libs/1_37_0/libs/serialization/doc/ │ │ │ │ │ +exceptions.html#stream_error │ │ │ │ │ +394 matrix_.triangularView() = matrix_.triangularView().transpose(); │ │ │ │ │ +395 ar & BOOST_SERIALIZATION_NVP(matrix_); │ │ │ │ │ +396 ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); │ │ │ │ │ +397 ar & BOOST_SERIALIZATION_NVP(blockStart_); │ │ │ │ │ +398 } │ │ │ │ │ +399 }; │ │ │ │ │ +400 │ │ │ │ │ +402 class CholeskyFailed; │ │ │ │ │ +403 │ │ │ │ │ +404} │ │ │ │ │ +_F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ +A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ +_M_a_t_r_i_x_._h │ │ │ │ │ +typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +_t_y_p_e_s_._h │ │ │ │ │ +Typedefs for easier changing of types. │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ +std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ +FastVector │ │ │ │ │ +FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ +DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_S_e_t │ │ │ │ │ -FastSet is a thin wrapper around std::set that uses the boost │ │ │ │ │ -fast_pool_allocator instead of the defa... │ │ │ │ │ -DDeeffiinniittiioonn FastSet.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_S_e_t_:_:_m_e_r_g_e │ │ │ │ │ -void merge(const FastSet &other) │ │ │ │ │ -insert another set: handy for MATLAB access │ │ │ │ │ -DDeeffiinniittiioonn FastSet.h:119 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_S_e_t_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &str="") const │ │ │ │ │ -Print to implement Testable: pretty basic. │ │ │ │ │ -DDeeffiinniittiioonn FastSet.h:101 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_S_e_t_:_:_F_a_s_t_S_e_t │ │ │ │ │ -FastSet(const INPUTCONTAINER &container) │ │ │ │ │ -Constructor from a iterable container, passes through to base class. │ │ │ │ │ -DDeeffiinniittiioonn FastSet.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_S_e_t_:_:_F_a_s_t_S_e_t │ │ │ │ │ -FastSet(const FastSet< VALUE > &x) │ │ │ │ │ -Copy constructor from another FastSet. │ │ │ │ │ -DDeeffiinniittiioonn FastSet.h:70 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_S_e_t_:_:_e_x_i_s_t_s │ │ │ │ │ -bool exists(const VALUE &e) const │ │ │ │ │ -Handy 'exists' function. │ │ │ │ │ -DDeeffiinniittiioonn FastSet.h:96 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_S_e_t_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const FastSet< VALUE > &other, double tol=1e-9) const │ │ │ │ │ -Check for equality within tolerance to implement Testable. │ │ │ │ │ -DDeeffiinniittiioonn FastSet.h:107 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_S_e_t_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn FastSet.h:125 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_S_e_t_:_:_F_a_s_t_S_e_t │ │ │ │ │ -FastSet(const Base &x) │ │ │ │ │ -Copy constructor from the base set class. │ │ │ │ │ -DDeeffiinniittiioonn FastSet.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_S_e_t_:_:_F_a_s_t_S_e_t │ │ │ │ │ -FastSet()=default │ │ │ │ │ -Default constructor. │ │ │ │ │ -_g_t_s_a_m_:_:_I_s_T_e_s_t_a_b_l_e │ │ │ │ │ -A testable concept check that should be placed in applicable unit tests and in │ │ │ │ │ -generic algorithms. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_n_s_e_I_n_d_e_x │ │ │ │ │ +ptrdiff_t DenseIndex │ │ │ │ │ +The index type for Eigen objects. │ │ │ │ │ +DDeeffiinniittiioonn types.h:106 │ │ │ │ │ +_g_t_s_a_m_:_:_r_o_w │ │ │ │ │ +const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j) │ │ │ │ │ +Extracts a row view from a matrix that avoids a copy. │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:222 │ │ │ │ │ +_g_t_s_a_m_:_:_s_p_l_i_t │ │ │ │ │ +void split(const G &g, const PredecessorMap< KEY > &tree, G &Ab1, G &Ab2) │ │ │ │ │ +Split the graph into two parts: one corresponds to the given spanning tree, and │ │ │ │ │ +the other corresponds... │ │ │ │ │ +DDeeffiinniittiioonn graph-inl.h:255 │ │ │ │ │ +_g_t_s_a_m_:_:_c_h_o_l_e_s_k_y_P_a_r_t_i_a_l │ │ │ │ │ +bool choleskyPartial(Matrix &ABC, size_t nFrontal, size_t topleft) │ │ │ │ │ +Partial Cholesky computes a factor [R S such that [R' 0 [R S = [A B 0 L] S' I] │ │ │ │ │ +0 L] B' C]. │ │ │ │ │ +DDeeffiinniittiioonn cholesky.cpp:108 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ +of blocks. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_f_u_l_l │ │ │ │ │ +Block full() │ │ │ │ │ +Get the full matrix as a block. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:340 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_b_l_o_c_k_S_t_a_r_t__ │ │ │ │ │ +DenseIndex blockStart_ │ │ │ │ │ +Changes apparent matrix view, see main class comment. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:62 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_s_e_t_D_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ +void setDiagonalBlock(DenseIndex I, const XprType &xpr) │ │ │ │ │ +Set a diagonal block. Only the upper triangular portion of xpr is evaluated. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:195 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_n_A_c_t_u_a_l_B_l_o_c_k_s │ │ │ │ │ +DenseIndex nActualBlocks() const │ │ │ │ │ +Number of actual blocks in the full matrix. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:306 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_d_i_a_g_o_n_a_l │ │ │ │ │ +Vector diagonal(DenseIndex J) const │ │ │ │ │ +Get the diagonal of the J'th diagonal block. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:145 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_m_a_t_r_i_x__ │ │ │ │ │ +Matrix matrix_ │ │ │ │ │ +The full matrix. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:59 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_c_o_l_s │ │ │ │ │ +DenseIndex cols() const │ │ │ │ │ +Column size. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:117 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_g_e_t_D_i_m │ │ │ │ │ +DenseIndex getDim(DenseIndex block) const │ │ │ │ │ +Number of dimensions for variable on this diagonal block. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:123 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_s_e_t_Z_e_r_o │ │ │ │ │ +void setZero() │ │ │ │ │ +Set the entire active matrix zero. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:258 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_s_e_t_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ +void setOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr) │ │ │ │ │ +Set an off-diagonal block. Only the upper triangular portion of xpr is │ │ │ │ │ +evaluated. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:201 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_s_e_t_F_u_l_l_M_a_t_r_i_x │ │ │ │ │ +void setFullMatrix(const XprType &xpr) │ │ │ │ │ +Set the entire active matrix. Only reads the upper triangular part of xpr. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:253 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_s_e_l_f_a_d_j_o_i_n_t_V_i_e_w │ │ │ │ │ +Eigen::SelfAdjointView< Block, Eigen::Upper > selfadjointView() │ │ │ │ │ +Get self adjoint view. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:242 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_a_b_o_v_e_D_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ +constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const │ │ │ │ │ +Get block above the diagonal (I, J). │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:150 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_b_l_o_c_k__ │ │ │ │ │ +constBlock block_(DenseIndex iBlock, DenseIndex jBlock, DenseIndex blockRows=1, │ │ │ │ │ +DenseIndex blockCols=1) const │ │ │ │ │ +Get an arbitrary block from the matrix. Indices are in block units. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:319 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_f_u_l_l │ │ │ │ │ +constBlock full() const │ │ │ │ │ +Get the full matrix as a block. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:335 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_r_o_w_s │ │ │ │ │ +DenseIndex rows() const │ │ │ │ │ +Row size. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:114 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_a_b_o_v_e_D_i_a_g_o_n_a_l_R_a_n_g_e │ │ │ │ │ +Block aboveDiagonalRange(DenseIndex i_startBlock, DenseIndex i_endBlock, │ │ │ │ │ +DenseIndex j_startBlock, DenseIndex j_endBlock) │ │ │ │ │ +Get a range [i,j) from the matrix. Indices are in block units. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:181 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_t_r_i_a_n_g_u_l_a_r_V_i_e_w │ │ │ │ │ +Eigen::TriangularView< constBlock, Eigen::Upper > triangularView(DenseIndex I, │ │ │ │ │ +DenseIndex J) const │ │ │ │ │ +Return the square sub-matrix that contains blocks(i:j, i:j) as a triangular │ │ │ │ │ +view. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:163 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_d_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ +Eigen::SelfAdjointView< constBlock, Eigen::Upper > diagonalBlock(DenseIndex J) │ │ │ │ │ +const │ │ │ │ │ +Return the J'th diagonal block as a self adjoint view. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:140 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +SymmetricBlockMatrix(const CONTAINER &dimensions, const Matrix &matrix, bool │ │ │ │ │ +appendOneDimension=false) │ │ │ │ │ +Construct from a container of the sizes of each vertical block and a pre- │ │ │ │ │ +prepared matrix. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:90 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_b_l_o_c_k_S_t_a_r_t │ │ │ │ │ +DenseIndex blockStart() const │ │ │ │ │ +Retrieve the first logical block, i.e. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:277 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_s_e_l_f_a_d_j_o_i_n_t_V_i_e_w │ │ │ │ │ +Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView() const │ │ │ │ │ +Get self adjoint view. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:247 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_u_p_d_a_t_e_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ +void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr) │ │ │ │ │ +Update an off diagonal block. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:228 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +SymmetricBlockMatrix(const CONTAINER &dimensions, bool │ │ │ │ │ +appendOneDimension=false) │ │ │ │ │ +Construct from a container of the sizes of each block. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:70 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool │ │ │ │ │ +appendOneDimension=false) │ │ │ │ │ +Construct from iterator over the sizes of each vertical block. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:80 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_b_l_o_c_k_S_t_a_r_t │ │ │ │ │ +DenseIndex & blockStart() │ │ │ │ │ +Retrieve or modify the first logical block, i.e. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:273 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_a_b_o_v_e_D_i_a_g_o_n_a_l_R_a_n_g_e │ │ │ │ │ +constBlock aboveDiagonalRange(DenseIndex i_startBlock, DenseIndex i_endBlock, │ │ │ │ │ +DenseIndex j_startBlock, DenseIndex j_endBlock) const │ │ │ │ │ +Get a range [i,j) from the matrix. Indices are in block units. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:170 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_d_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ +Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J) │ │ │ │ │ +Return the J'th diagonal block as a self adjoint view. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:135 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_b_l_o_c_k__ │ │ │ │ │ +Block block_(DenseIndex iBlock, DenseIndex jBlock, DenseIndex blockRows=1, │ │ │ │ │ +DenseIndex blockCols=1) │ │ │ │ │ +Get an arbitrary block from the matrix. Indices are in block units. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:327 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_u_p_d_a_t_e_D_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ +void updateDiagonalBlock(DenseIndex I, const XprType &xpr) │ │ │ │ │ +Increment the diagonal block by the values in xpr. Only reads the upper │ │ │ │ │ +triangular part of xpr. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:212 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_n_O_f_f_s_e_t_s │ │ │ │ │ +DenseIndex nOffsets() const │ │ │ │ │ +Number of offsets in the full matrix. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:301 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_c_a_l_c_I_n_d_i_c_e_s │ │ │ │ │ +std::array< DenseIndex, 4 > calcIndices(DenseIndex iBlock, DenseIndex jBlock, │ │ │ │ │ +DenseIndex blockRows, DenseIndex blockCols) const │ │ │ │ │ +Compute the indices into the underlying matrix for a given block. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:345 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_n_B_l_o_c_k_s │ │ │ │ │ +DenseIndex nBlocks() const │ │ │ │ │ +Block count. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:120 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_v_a_r_i_a_b_l_e_C_o_l_O_f_f_s_e_t_s__ │ │ │ │ │ +FastVector< DenseIndex > variableColOffsets_ │ │ │ │ │ +the starting columns of each block (0-based) │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:60 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_s_e_l_f_a_d_j_o_i_n_t_V_i_e_w │ │ │ │ │ +Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex │ │ │ │ │ +I, DenseIndex J) const │ │ │ │ │ +Return the square sub-matrix that contains blocks(i:j, i:j). │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:156 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_o_f_f_s_e_t │ │ │ │ │ +DenseIndex offset(DenseIndex block) const │ │ │ │ │ +Get an offset for a block index (in the active view). │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:311 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ +of vertical blocks. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:43 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _F_a_s_t_S_e_t_._h │ │ │ │ │ + * _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00116.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/VerticalBlockMatrix.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/MatrixSerialization.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,45 +94,94 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
VerticalBlockMatrix.h File Reference
│ │ │ │ +Functions
│ │ │ │ +
MatrixSerialization.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

A matrix with column blocks of pre-defined sizes. │ │ │ │ +

Serialization for matrices. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::VerticalBlockMatrix
 This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks. More...
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ -Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 

│ │ │ │ +Functions

template<class Archive , typename Scalar_ , int Rows_, int Cols_, int Ops_, int MaxRows_, int MaxCols_>
void boost::serialization::save (Archive &ar, const Eigen::Matrix< Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_ > &m, const unsigned int)
 Ref.
 
│ │ │ │ +template<class Archive , typename Scalar_ , int Rows_, int Cols_, int Ops_, int MaxRows_, int MaxCols_>
void boost::serialization::load (Archive &ar, Eigen::Matrix< Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_ > &m, const unsigned int)
 
│ │ │ │ +template<class Archive , typename Scalar_ , int Rows_, int Cols_, int Ops_, int MaxRows_, int MaxCols_>
void boost::serialization::serialize (Archive &ar, Eigen::Matrix< Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_ > &m, const unsigned int version)
 
│ │ │ │ +template<class Archive >
void boost::serialization::serialize (Archive &ar, gtsam::Matrix &m, const unsigned int version)
 
│ │ │ │

Detailed Description

│ │ │ │ -

A matrix with column blocks of pre-defined sizes.

│ │ │ │ -

Used in JacobianFactor and GaussianConditional.

Author
Richard Roberts
│ │ │ │ -
Date
Sep 18, 2010
│ │ │ │ -
│ │ │ │ +

Serialization for matrices.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
Date
February 2022
│ │ │ │ +

Function Documentation

│ │ │ │ + │ │ │ │ +

◆ save()

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +template<class Archive , typename Scalar_ , int Rows_, int Cols_, int Ops_, int MaxRows_, int MaxCols_>
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
void boost::serialization::save (Archive & ar,
const Eigen::Matrix< Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_ > & m,
const unsigned int  
)
│ │ │ │ +
│ │ │ │ + │ │ │ │ +

Ref.

│ │ │ │ +

https://stackoverflow.com/questions/18382457/eigen-and-boostserialize/22903063#22903063

│ │ │ │ +

Eigen supports calling resize() on both static and dynamic matrices. This allows for a uniform API, with resize having no effect if the static matrix is already the correct size. https://eigen.tuxfamily.org/dox/group__TutorialMatrixClass.html#TutorialMatrixSizesResizing

│ │ │ │ +

We use all the Matrix template parameters to ensure wide compatibility.

│ │ │ │ +

eigen_typekit in ROS uses the same code http://docs.ros.org/lunar/api/eigen_typekit/html/eigen__mqueue_8cpp_source.html

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,29 +1,59 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -VerticalBlockMatrix.h File Reference │ │ │ │ │ -A matrix with column blocks of pre-defined sizes. _M_o_r_e_._._. │ │ │ │ │ +_F_u_n_c_t_i_o_n_s │ │ │ │ │ +MatrixSerialization.h File Reference │ │ │ │ │ +Serialization for matrices. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -  This class stores a dense matrix and allows it to be accessed as a │ │ │ │ │ - collection of vertical blocks. _M_o_r_e_._._. │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +template │ │ │ │ │ +void  _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_s_a_v_e (Archive &ar, const Eigen::Matrix< Scalar_, │ │ │ │ │ + Rows_, Cols_, Ops_, MaxRows_, MaxCols_ > &m, const unsigned int) │ │ │ │ │ +  Ref. │ │ │ │ │   │ │ │ │ │ -NNaammeessppaacceess │ │ │ │ │ -namespace   _g_t_s_a_m │ │ │ │ │ -  Global functions in a separate testing namespace. │ │ │ │ │ +template │ │ │ │ │ +void  bboooosstt::::sseerriiaalliizzaattiioonn::::llooaadd (Archive &ar, Eigen::Matrix< Scalar_, Rows_, │ │ │ │ │ + Cols_, Ops_, MaxRows_, MaxCols_ > &m, const unsigned int) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +void  bboooosstt::::sseerriiaalliizzaattiioonn::::sseerriiaalliizzee (Archive &ar, Eigen::Matrix< Scalar_, │ │ │ │ │ + Rows_, Cols_, Ops_, MaxRows_, MaxCols_ > &m, const unsigned int version) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +void  bboooosstt::::sseerriiaalliizzaattiioonn::::sseerriiaalliizzee (Archive &ar, gtsam::Matrix &m, const │ │ │ │ │ + unsigned int version) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A matrix with column blocks of pre-defined sizes. │ │ │ │ │ -Used in _J_a_c_o_b_i_a_n_F_a_c_t_o_r and _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l. │ │ │ │ │ +Serialization for matrices. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - Sep 18, 2010 │ │ │ │ │ + February 2022 │ │ │ │ │ +********** FFuunnccttiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ +********** _?◆_? ssaavvee(()) ********** │ │ │ │ │ +template │ │ │ │ │ +void boost::serialization::save ( Archive &  aarr, │ │ │ │ │ + const Eigen::Matrix< Scalar_, Rows_, mm, │ │ │ │ │ + Cols_, Ops_, MaxRows_, MaxCols_ > &  │ │ │ │ │ + const unsigned int    │ │ │ │ │ + ) │ │ │ │ │ +Ref. │ │ │ │ │ +_h_t_t_p_s_:_/_/_s_t_a_c_k_o_v_e_r_f_l_o_w_._c_o_m_/_q_u_e_s_t_i_o_n_s_/_1_8_3_8_2_4_5_7_/_e_i_g_e_n_-_a_n_d_-_b_o_o_s_t_s_e_r_i_a_l_i_z_e_/ │ │ │ │ │ +_2_2_9_0_3_0_6_3_#_2_2_9_0_3_0_6_3 │ │ │ │ │ +Eigen supports calling resize() on both static and dynamic matrices. This │ │ │ │ │ +allows for a uniform API, with resize having no effect if the static matrix is │ │ │ │ │ +already the correct size. _h_t_t_p_s_:_/_/_e_i_g_e_n_._t_u_x_f_a_m_i_l_y_._o_r_g_/_d_o_x_/ │ │ │ │ │ +_g_r_o_u_p_____T_u_t_o_r_i_a_l_M_a_t_r_i_x_C_l_a_s_s_._h_t_m_l_#_T_u_t_o_r_i_a_l_M_a_t_r_i_x_S_i_z_e_s_R_e_s_i_z_i_n_g │ │ │ │ │ +We use all the Matrix template parameters to ensure wide compatibility. │ │ │ │ │ +eigen_typekit in ROS uses the same code _h_t_t_p_:_/_/_d_o_c_s_._r_o_s_._o_r_g_/_l_u_n_a_r_/_a_p_i_/ │ │ │ │ │ +_e_i_g_e_n___t_y_p_e_k_i_t_/_h_t_m_l_/_e_i_g_e_n_____m_q_u_e_u_e___8_c_p_p___s_o_u_r_c_e_._h_t_m_l │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_._h │ │ │ │ │ + * _M_a_t_r_i_x_S_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00116_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/VerticalBlockMatrix.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/MatrixSerialization.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,252 +98,91 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
VerticalBlockMatrix.h
│ │ │ │ +
MatrixSerialization.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/base/Matrix.h>
│ │ │ │ -
21#include <gtsam/base/MatrixSerialization.h>
│ │ │ │ -
22#include <gtsam/base/FastVector.h>
│ │ │ │ -
23
│ │ │ │ -
24namespace gtsam {
│ │ │ │ -
25
│ │ │ │ -
26 // Forward declarations
│ │ │ │ -
27 class SymmetricBlockMatrix;
│ │ │ │ +
19// \callgraph
│ │ │ │ +
20
│ │ │ │ +
21#pragma once
│ │ │ │ +
22
│ │ │ │ +
23#include <gtsam/base/Matrix.h>
│ │ │ │ +
24
│ │ │ │ +
25#include <boost/serialization/array.hpp>
│ │ │ │ +
26#include <boost/serialization/nvp.hpp>
│ │ │ │ +
27#include <boost/serialization/split_free.hpp>
│ │ │ │
28
│ │ │ │ -
│ │ │ │ -
42 class GTSAM_EXPORT VerticalBlockMatrix
│ │ │ │ -
43 {
│ │ │ │ -
44 public:
│ │ │ │ -
45 typedef VerticalBlockMatrix This;
│ │ │ │ -
46 typedef Eigen::Block<Matrix> Block;
│ │ │ │ -
47 typedef Eigen::Block<const Matrix> constBlock;
│ │ │ │ -
48
│ │ │ │ -
49 protected:
│ │ │ │ -
50 Matrix matrix_;
│ │ │ │ -
51 FastVector<DenseIndex> variableColOffsets_;
│ │ │ │ -
52
│ │ │ │ -
53 DenseIndex rowStart_;
│ │ │ │ -
54 DenseIndex rowEnd_;
│ │ │ │ -
55 DenseIndex blockStart_;
│ │ │ │ -
56
│ │ │ │ -
57 public:
│ │ │ │ -
58
│ │ │ │ -
│ │ │ │ -
60 VerticalBlockMatrix() :
│ │ │ │ -
61 rowStart_(0), rowEnd_(0), blockStart_(0)
│ │ │ │ -
62 {
│ │ │ │ -
63 variableColOffsets_.push_back(0);
│ │ │ │ -
64 assertInvariants();
│ │ │ │ -
65 }
│ │ │ │ -
│ │ │ │ -
66
│ │ │ │ -
68 template<typename CONTAINER>
│ │ │ │ -
│ │ │ │ -
69 VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height,
│ │ │ │ -
70 bool appendOneDimension = false) :
│ │ │ │ -
71 variableColOffsets_(dimensions.size() + (appendOneDimension ? 2 : 1)),
│ │ │ │ -
72 rowStart_(0), rowEnd_(height), blockStart_(0) {
│ │ │ │ -
73 fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
│ │ │ │ -
74 matrix_.resize(height, variableColOffsets_.back());
│ │ │ │ -
75 assertInvariants();
│ │ │ │ -
76 }
│ │ │ │ -
│ │ │ │ -
77
│ │ │ │ -
79 template<typename CONTAINER, typename DERIVED>
│ │ │ │ -
│ │ │ │ -
80 VerticalBlockMatrix(const CONTAINER& dimensions,
│ │ │ │ -
81 const Eigen::MatrixBase<DERIVED>& matrix, bool appendOneDimension = false) :
│ │ │ │ -
82 matrix_(matrix), variableColOffsets_(dimensions.size() + (appendOneDimension ? 2 : 1)),
│ │ │ │ -
83 rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) {
│ │ │ │ -
84 fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
│ │ │ │ -
85 if (variableColOffsets_.back() != matrix_.cols())
│ │ │ │ -
86 throw std::invalid_argument(
│ │ │ │ -
87 "Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix.");
│ │ │ │ -
88 assertInvariants();
│ │ │ │ -
89 }
│ │ │ │ -
│ │ │ │ -
90
│ │ │ │ -
92 template<typename ITERATOR>
│ │ │ │ -
│ │ │ │ -
93 VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim,
│ │ │ │ -
94 DenseIndex height, bool appendOneDimension = false) :
│ │ │ │ -
95 variableColOffsets_((lastBlockDim-firstBlockDim) + (appendOneDimension ? 2 : 1)),
│ │ │ │ -
96 rowStart_(0), rowEnd_(height), blockStart_(0) {
│ │ │ │ -
97 fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension);
│ │ │ │ -
98 matrix_.resize(height, variableColOffsets_.back());
│ │ │ │ -
99 assertInvariants();
│ │ │ │ -
100 }
│ │ │ │ -
│ │ │ │ -
101
│ │ │ │ -
107 static VerticalBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& rhs);
│ │ │ │ -
108
│ │ │ │ -
112 static VerticalBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix& rhs, DenseIndex height);
│ │ │ │ -
113
│ │ │ │ -
115 DenseIndex rows() const { assertInvariants(); return rowEnd_ - rowStart_; }
│ │ │ │ -
116
│ │ │ │ -
118 DenseIndex cols() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; }
│ │ │ │ -
119
│ │ │ │ -
121 DenseIndex nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; }
│ │ │ │ -
122
│ │ │ │ -
124 Block operator()(DenseIndex block) { return range(block, block+1); }
│ │ │ │ -
125
│ │ │ │ -
127 const constBlock operator()(DenseIndex block) const { return range(block, block+1); }
│ │ │ │ -
128
│ │ │ │ -
│ │ │ │ -
130 Block range(DenseIndex startBlock, DenseIndex endBlock) {
│ │ │ │ -
131 assertInvariants();
│ │ │ │ -
132 DenseIndex actualStartBlock = startBlock + blockStart_;
│ │ │ │ -
133 DenseIndex actualEndBlock = endBlock + blockStart_;
│ │ │ │ -
134 if(startBlock != 0 || endBlock != 0) {
│ │ │ │ -
135 checkBlock(actualStartBlock);
│ │ │ │ -
136 assert(actualEndBlock < (DenseIndex)variableColOffsets_.size());
│ │ │ │ -
137 }
│ │ │ │ -
138 const DenseIndex startCol = variableColOffsets_[actualStartBlock];
│ │ │ │ -
139 const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - startCol;
│ │ │ │ -
140 return matrix_.block(rowStart_, startCol, this->rows(), rangeCols);
│ │ │ │ -
141 }
│ │ │ │ -
│ │ │ │ -
142
│ │ │ │ -
143 const constBlock range(DenseIndex startBlock, DenseIndex endBlock) const {
│ │ │ │ -
144 assertInvariants();
│ │ │ │ -
145 DenseIndex actualStartBlock = startBlock + blockStart_;
│ │ │ │ -
146 DenseIndex actualEndBlock = endBlock + blockStart_;
│ │ │ │ -
147 if(startBlock != 0 || endBlock != 0) {
│ │ │ │ -
148 checkBlock(actualStartBlock);
│ │ │ │ -
149 assert(actualEndBlock < (DenseIndex)variableColOffsets_.size());
│ │ │ │ -
150 }
│ │ │ │ -
151 const DenseIndex startCol = variableColOffsets_[actualStartBlock];
│ │ │ │ -
152 const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - startCol;
│ │ │ │ -
153 return ((const Matrix&)matrix_).block(rowStart_, startCol, this->rows(), rangeCols);
│ │ │ │ -
154 }
│ │ │ │ -
155
│ │ │ │ -
157 Block full() { return range(0, nBlocks()); }
│ │ │ │ -
158
│ │ │ │ -
160 const constBlock full() const { return range(0, nBlocks()); }
│ │ │ │ -
161
│ │ │ │ -
162 DenseIndex offset(DenseIndex block) const {
│ │ │ │ -
163 assertInvariants();
│ │ │ │ -
164 DenseIndex actualBlock = block + blockStart_;
│ │ │ │ -
165 checkBlock(actualBlock);
│ │ │ │ -
166 return variableColOffsets_[actualBlock];
│ │ │ │ -
167 }
│ │ │ │ -
168
│ │ │ │ -
170 const DenseIndex& rowStart() const { return rowStart_; }
│ │ │ │ -
171
│ │ │ │ -
173 DenseIndex& rowStart() { return rowStart_; }
│ │ │ │ -
174
│ │ │ │ -
176 const DenseIndex& rowEnd() const { return rowEnd_; }
│ │ │ │ -
177
│ │ │ │ -
179 DenseIndex& rowEnd() { return rowEnd_; }
│ │ │ │ -
180
│ │ │ │ -
182 const DenseIndex& firstBlock() const { return blockStart_; }
│ │ │ │ -
183
│ │ │ │ -
185 DenseIndex& firstBlock() { return blockStart_; }
│ │ │ │ -
186
│ │ │ │ -
188 const Matrix& matrix() const { return matrix_; }
│ │ │ │ -
189
│ │ │ │ -
191 Matrix& matrix() { return matrix_; }
│ │ │ │ -
192
│ │ │ │ -
193 protected:
│ │ │ │ -
194 void assertInvariants() const {
│ │ │ │ -
195 assert(matrix_.cols() == variableColOffsets_.back());
│ │ │ │ -
196 assert(blockStart_ < (DenseIndex)variableColOffsets_.size());
│ │ │ │ -
197 assert(rowStart_ <= matrix_.rows());
│ │ │ │ -
198 assert(rowEnd_ <= matrix_.rows());
│ │ │ │ -
199 assert(rowStart_ <= rowEnd_);
│ │ │ │ -
200 }
│ │ │ │ -
201
│ │ │ │ -
202 void checkBlock(DenseIndex block) const {
│ │ │ │ -
203 static_cast<void>(block); //Disable unused varibale warnings.
│ │ │ │ -
204 assert(matrix_.cols() == variableColOffsets_.back());
│ │ │ │ -
205 assert(block < (DenseIndex)variableColOffsets_.size() - 1);
│ │ │ │ -
206 assert(variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols());
│ │ │ │ -
207 }
│ │ │ │ -
208
│ │ │ │ -
209 template<typename ITERATOR>
│ │ │ │ -
210 void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension) {
│ │ │ │ -
211 variableColOffsets_[0] = 0;
│ │ │ │ -
212 DenseIndex j=0;
│ │ │ │ -
213 for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim, ++j)
│ │ │ │ -
214 variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
│ │ │ │ -
215 if(appendOneDimension)
│ │ │ │ -
216 variableColOffsets_[j+1] = variableColOffsets_[j] + 1;
│ │ │ │ -
217 }
│ │ │ │ -
218
│ │ │ │ -
219 friend class SymmetricBlockMatrix;
│ │ │ │ -
220
│ │ │ │ -
221 private:
│ │ │ │ -
223 friend class boost::serialization::access;
│ │ │ │ -
224 template<class ARCHIVE>
│ │ │ │ -
225 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
226 ar & BOOST_SERIALIZATION_NVP(matrix_);
│ │ │ │ -
227 ar & BOOST_SERIALIZATION_NVP(variableColOffsets_);
│ │ │ │ -
228 ar & BOOST_SERIALIZATION_NVP(rowStart_);
│ │ │ │ -
229 ar & BOOST_SERIALIZATION_NVP(rowEnd_);
│ │ │ │ -
230 ar & BOOST_SERIALIZATION_NVP(blockStart_);
│ │ │ │ -
231 }
│ │ │ │ -
232 };
│ │ │ │ -
│ │ │ │ -
233
│ │ │ │ -
234}
│ │ │ │ -
MatrixSerialization.h
Serialization for matrices.
│ │ │ │ -
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ -
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ -
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │ -
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │ -
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
│ │ │ │ -
gtsam::VerticalBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43
│ │ │ │ -
gtsam::VerticalBlockMatrix::VerticalBlockMatrix
VerticalBlockMatrix()
Construct an empty VerticalBlockMatrix.
Definition VerticalBlockMatrix.h:60
│ │ │ │ -
gtsam::VerticalBlockMatrix::full
Block full()
Return the full matrix, not including any portions excluded by rowStart(), rowEnd(),...
Definition VerticalBlockMatrix.h:157
│ │ │ │ -
gtsam::VerticalBlockMatrix::range
Block range(DenseIndex startBlock, DenseIndex endBlock)
access ranges of blocks at a time
Definition VerticalBlockMatrix.h:130
│ │ │ │ -
gtsam::VerticalBlockMatrix::firstBlock
const DenseIndex & firstBlock() const
Get the apparent first block for all operations.
Definition VerticalBlockMatrix.h:182
│ │ │ │ -
gtsam::VerticalBlockMatrix::matrix
const Matrix & matrix() const
Access to full matrix (including any portions excluded by rowStart(), rowEnd(), and firstBlock())
Definition VerticalBlockMatrix.h:188
│ │ │ │ -
gtsam::VerticalBlockMatrix::VerticalBlockMatrix
VerticalBlockMatrix(const CONTAINER &dimensions, DenseIndex height, bool appendOneDimension=false)
Construct from a container of the sizes of each vertical block.
Definition VerticalBlockMatrix.h:69
│ │ │ │ -
gtsam::VerticalBlockMatrix::rowEnd_
DenseIndex rowEnd_
Changes apparent matrix view, see main class comment.
Definition VerticalBlockMatrix.h:54
│ │ │ │ -
gtsam::VerticalBlockMatrix::rowEnd
const DenseIndex & rowEnd() const
Get the apparent last row (exclusive, i.e.
Definition VerticalBlockMatrix.h:176
│ │ │ │ -
gtsam::VerticalBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition VerticalBlockMatrix.h:115
│ │ │ │ -
gtsam::VerticalBlockMatrix::operator()
Block operator()(DenseIndex block)
Access a single block in the underlying matrix with read/write access.
Definition VerticalBlockMatrix.h:124
│ │ │ │ -
gtsam::VerticalBlockMatrix::firstBlock
DenseIndex & firstBlock()
Get or set the apparent first block for all operations.
Definition VerticalBlockMatrix.h:185
│ │ │ │ -
gtsam::VerticalBlockMatrix::cols
DenseIndex cols() const
Column size.
Definition VerticalBlockMatrix.h:118
│ │ │ │ -
gtsam::VerticalBlockMatrix::VerticalBlockMatrix
VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height, bool appendOneDimension=false)
Construct from iterator over the sizes of each vertical block.
Definition VerticalBlockMatrix.h:93
│ │ │ │ -
gtsam::VerticalBlockMatrix::operator()
const constBlock operator()(DenseIndex block) const
Access a const block view.
Definition VerticalBlockMatrix.h:127
│ │ │ │ -
gtsam::VerticalBlockMatrix::rowStart
const DenseIndex & rowStart() const
Get the apparent first row of the underlying matrix for all operations.
Definition VerticalBlockMatrix.h:170
│ │ │ │ -
gtsam::VerticalBlockMatrix::matrix_
Matrix matrix_
The full matrix.
Definition VerticalBlockMatrix.h:50
│ │ │ │ -
gtsam::VerticalBlockMatrix::nBlocks
DenseIndex nBlocks() const
Block count.
Definition VerticalBlockMatrix.h:121
│ │ │ │ -
gtsam::VerticalBlockMatrix::full
const constBlock full() const
Return the full matrix, not including any portions excluded by rowStart(), rowEnd(),...
Definition VerticalBlockMatrix.h:160
│ │ │ │ -
gtsam::VerticalBlockMatrix::rowStart
DenseIndex & rowStart()
Get or set the apparent first row of the underlying matrix for all operations.
Definition VerticalBlockMatrix.h:173
│ │ │ │ -
gtsam::VerticalBlockMatrix::matrix
Matrix & matrix()
Non-const access to full matrix (including any portions excluded by rowStart(), rowEnd(),...
Definition VerticalBlockMatrix.h:191
│ │ │ │ -
gtsam::VerticalBlockMatrix::rowEnd
DenseIndex & rowEnd()
Get or set the apparent last row (exclusive, i.e.
Definition VerticalBlockMatrix.h:179
│ │ │ │ -
gtsam::VerticalBlockMatrix::rowStart_
DenseIndex rowStart_
Changes apparent matrix view, see main class comment.
Definition VerticalBlockMatrix.h:53
│ │ │ │ -
gtsam::VerticalBlockMatrix::VerticalBlockMatrix
VerticalBlockMatrix(const CONTAINER &dimensions, const Eigen::MatrixBase< DERIVED > &matrix, bool appendOneDimension=false)
Construct from a container of the sizes of each vertical block and a pre-prepared matrix.
Definition VerticalBlockMatrix.h:80
│ │ │ │ -
gtsam::VerticalBlockMatrix::blockStart_
DenseIndex blockStart_
Changes apparent matrix view, see main class comment.
Definition VerticalBlockMatrix.h:55
│ │ │ │ -
gtsam::VerticalBlockMatrix::variableColOffsets_
FastVector< DenseIndex > variableColOffsets_
the starting columns of each block (0-based)
Definition VerticalBlockMatrix.h:51
│ │ │ │ +
29namespace boost {
│ │ │ │ +
30namespace serialization {
│ │ │ │ +
31
│ │ │ │ +
47// split version - sends sizes ahead
│ │ │ │ +
48template <class Archive, typename Scalar_, int Rows_, int Cols_, int Ops_,
│ │ │ │ +
49 int MaxRows_, int MaxCols_>
│ │ │ │ +
│ │ │ │ +
50void save(
│ │ │ │ +
51 Archive& ar,
│ │ │ │ +
52 const Eigen::Matrix<Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_>& m,
│ │ │ │ +
53 const unsigned int /*version*/) {
│ │ │ │ +
54 const size_t rows = m.rows(), cols = m.cols();
│ │ │ │ +
55 ar << BOOST_SERIALIZATION_NVP(rows);
│ │ │ │ +
56 ar << BOOST_SERIALIZATION_NVP(cols);
│ │ │ │ +
57 ar << make_nvp("data", make_array(m.data(), m.size()));
│ │ │ │ +
58}
│ │ │ │ +
│ │ │ │ +
59
│ │ │ │ +
60template <class Archive, typename Scalar_, int Rows_, int Cols_, int Ops_,
│ │ │ │ +
61 int MaxRows_, int MaxCols_>
│ │ │ │ +
62void load(Archive& ar,
│ │ │ │ +
63 Eigen::Matrix<Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_>& m,
│ │ │ │ +
64 const unsigned int /*version*/) {
│ │ │ │ +
65 size_t rows, cols;
│ │ │ │ +
66 ar >> BOOST_SERIALIZATION_NVP(rows);
│ │ │ │ +
67 ar >> BOOST_SERIALIZATION_NVP(cols);
│ │ │ │ +
68 m.resize(rows, cols);
│ │ │ │ +
69 ar >> make_nvp("data", make_array(m.data(), m.size()));
│ │ │ │ +
70}
│ │ │ │ +
71
│ │ │ │ +
72// templated version of BOOST_SERIALIZATION_SPLIT_FREE(Eigen::Matrix);
│ │ │ │ +
73template <class Archive, typename Scalar_, int Rows_, int Cols_, int Ops_,
│ │ │ │ +
74 int MaxRows_, int MaxCols_>
│ │ │ │ +
75void serialize(
│ │ │ │ +
76 Archive& ar,
│ │ │ │ +
77 Eigen::Matrix<Scalar_, Rows_, Cols_, Ops_, MaxRows_, MaxCols_>& m,
│ │ │ │ +
78 const unsigned int version) {
│ │ │ │ +
79 split_free(ar, m, version);
│ │ │ │ +
80}
│ │ │ │ +
81
│ │ │ │ +
82// specialized to Matrix for MATLAB wrapper
│ │ │ │ +
83template <class Archive>
│ │ │ │ +
84void serialize(Archive& ar, gtsam::Matrix& m, const unsigned int version) {
│ │ │ │ +
85 split_free(ar, m, version);
│ │ │ │ +
86}
│ │ │ │ +
87
│ │ │ │ +
88} // namespace serialization
│ │ │ │ +
89} // namespace boost
│ │ │ │ +
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,346 +1,82 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -VerticalBlockMatrix.h │ │ │ │ │ +MatrixSerialization.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_S_e_r_i_a_l_i_z_a_t_i_o_n_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ -23 │ │ │ │ │ -24namespace _g_t_s_a_m { │ │ │ │ │ -25 │ │ │ │ │ -26 // Forward declarations │ │ │ │ │ -27 class SymmetricBlockMatrix; │ │ │ │ │ +19// \callgraph │ │ │ │ │ +20 │ │ │ │ │ +21#pragma once │ │ │ │ │ +22 │ │ │ │ │ +23#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ +24 │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ 28 │ │ │ │ │ -_4_2 class GTSAM_EXPORT _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -43 { │ │ │ │ │ -44 public: │ │ │ │ │ -45 typedef _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x _T_h_i_s; │ │ │ │ │ -46 typedef Eigen::Block Block; │ │ │ │ │ -47 typedef Eigen::Block constBlock; │ │ │ │ │ -48 │ │ │ │ │ -49 protected: │ │ │ │ │ -_5_0 Matrix _m_a_t_r_i_x__; │ │ │ │ │ -_5_1 _F_a_s_t_V_e_c_t_o_r_<_D_e_n_s_e_I_n_d_e_x_> _v_a_r_i_a_b_l_e_C_o_l_O_f_f_s_e_t_s__; │ │ │ │ │ -52 │ │ │ │ │ -_5_3 _D_e_n_s_e_I_n_d_e_x _r_o_w_S_t_a_r_t__; │ │ │ │ │ -_5_4 _D_e_n_s_e_I_n_d_e_x _r_o_w_E_n_d__; │ │ │ │ │ -_5_5 _D_e_n_s_e_I_n_d_e_x _b_l_o_c_k_S_t_a_r_t__; │ │ │ │ │ -56 │ │ │ │ │ -57 public: │ │ │ │ │ -58 │ │ │ │ │ -_6_0 _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x() : │ │ │ │ │ -61 rowStart_(0), rowEnd_(0), blockStart_(0) │ │ │ │ │ -62 { │ │ │ │ │ -63 variableColOffsets_.push_back(0); │ │ │ │ │ -64 assertInvariants(); │ │ │ │ │ -65 } │ │ │ │ │ -66 │ │ │ │ │ -68 template │ │ │ │ │ -_6_9 _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x(const CONTAINER& dimensions, _D_e_n_s_e_I_n_d_e_x height, │ │ │ │ │ -70 bool appendOneDimension = false) : │ │ │ │ │ -71 variableColOffsets_(dimensions.size() + (appendOneDimension ? 2 : 1)), │ │ │ │ │ -72 rowStart_(0), rowEnd_(height), blockStart_(0) { │ │ │ │ │ -73 fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); │ │ │ │ │ -74 matrix_.resize(height, variableColOffsets_.back()); │ │ │ │ │ -75 assertInvariants(); │ │ │ │ │ -76 } │ │ │ │ │ -77 │ │ │ │ │ -79 template │ │ │ │ │ -_8_0 _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x(const CONTAINER& dimensions, │ │ │ │ │ -81 const Eigen::MatrixBase& matrix, bool appendOneDimension = false) : │ │ │ │ │ -82 matrix_(matrix), variableColOffsets_(dimensions.size() + (appendOneDimension │ │ │ │ │ -? 2 : 1)), │ │ │ │ │ -83 rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) { │ │ │ │ │ -84 fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); │ │ │ │ │ -85 if (variableColOffsets_.back() != matrix_.cols()) │ │ │ │ │ -86 throw std::invalid_argument( │ │ │ │ │ -87 "Requested to create a VerticalBlockMatrix with dimensions that do not sum │ │ │ │ │ -to the total columns of the provided matrix."); │ │ │ │ │ -88 assertInvariants(); │ │ │ │ │ -89 } │ │ │ │ │ -90 │ │ │ │ │ -92 template │ │ │ │ │ -_9_3 _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x(ITERATOR firstBlockDim, ITERATOR lastBlockDim, │ │ │ │ │ -94 _D_e_n_s_e_I_n_d_e_x height, bool appendOneDimension = false) : │ │ │ │ │ -95 variableColOffsets_((lastBlockDim-firstBlockDim) + (appendOneDimension ? 2 : │ │ │ │ │ -1)), │ │ │ │ │ -96 rowStart_(0), rowEnd_(height), blockStart_(0) { │ │ │ │ │ -97 fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension); │ │ │ │ │ -98 matrix_.resize(height, variableColOffsets_.back()); │ │ │ │ │ -99 assertInvariants(); │ │ │ │ │ -100 } │ │ │ │ │ -101 │ │ │ │ │ -107 static _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x LikeActiveViewOf(const _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x& │ │ │ │ │ -rhs); │ │ │ │ │ -108 │ │ │ │ │ -112 static _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x LikeActiveViewOf(const _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x& │ │ │ │ │ -rhs, _D_e_n_s_e_I_n_d_e_x height); │ │ │ │ │ -113 │ │ │ │ │ -_1_1_5 _D_e_n_s_e_I_n_d_e_x _r_o_w_s() const { assertInvariants(); return rowEnd_ - rowStart_; } │ │ │ │ │ -116 │ │ │ │ │ -_1_1_8 _D_e_n_s_e_I_n_d_e_x _c_o_l_s() const { assertInvariants(); return │ │ │ │ │ -variableColOffsets_.back() - variableColOffsets_[blockStart_]; } │ │ │ │ │ -119 │ │ │ │ │ -_1_2_1 _D_e_n_s_e_I_n_d_e_x _n_B_l_o_c_k_s() const { assertInvariants(); return │ │ │ │ │ -variableColOffsets_.size() - 1 - blockStart_; } │ │ │ │ │ -122 │ │ │ │ │ -_1_2_4 Block _o_p_e_r_a_t_o_r_(_)(_D_e_n_s_e_I_n_d_e_x block) { return range(block, block+1); } │ │ │ │ │ -125 │ │ │ │ │ -_1_2_7 const constBlock _o_p_e_r_a_t_o_r_(_)(_D_e_n_s_e_I_n_d_e_x block) const { return range(block, │ │ │ │ │ -block+1); } │ │ │ │ │ -128 │ │ │ │ │ -_1_3_0 Block _r_a_n_g_e(_D_e_n_s_e_I_n_d_e_x startBlock, _D_e_n_s_e_I_n_d_e_x endBlock) { │ │ │ │ │ -131 assertInvariants(); │ │ │ │ │ -132 _D_e_n_s_e_I_n_d_e_x actualStartBlock = startBlock + blockStart_; │ │ │ │ │ -133 _D_e_n_s_e_I_n_d_e_x actualEndBlock = endBlock + blockStart_; │ │ │ │ │ -134 if(startBlock != 0 || endBlock != 0) { │ │ │ │ │ -135 checkBlock(actualStartBlock); │ │ │ │ │ -136 assert(actualEndBlock < (_D_e_n_s_e_I_n_d_e_x)variableColOffsets_.size()); │ │ │ │ │ -137 } │ │ │ │ │ -138 const _D_e_n_s_e_I_n_d_e_x startCol = variableColOffsets_[actualStartBlock]; │ │ │ │ │ -139 const _D_e_n_s_e_I_n_d_e_x rangeCols = variableColOffsets_[actualEndBlock] - │ │ │ │ │ -startCol; │ │ │ │ │ -140 return matrix_.block(rowStart_, startCol, this->rows(), rangeCols); │ │ │ │ │ -141 } │ │ │ │ │ -142 │ │ │ │ │ -143 const constBlock range(_D_e_n_s_e_I_n_d_e_x startBlock, _D_e_n_s_e_I_n_d_e_x endBlock) const { │ │ │ │ │ -144 assertInvariants(); │ │ │ │ │ -145 _D_e_n_s_e_I_n_d_e_x actualStartBlock = startBlock + blockStart_; │ │ │ │ │ -146 _D_e_n_s_e_I_n_d_e_x actualEndBlock = endBlock + blockStart_; │ │ │ │ │ -147 if(startBlock != 0 || endBlock != 0) { │ │ │ │ │ -148 checkBlock(actualStartBlock); │ │ │ │ │ -149 assert(actualEndBlock < (_D_e_n_s_e_I_n_d_e_x)variableColOffsets_.size()); │ │ │ │ │ -150 } │ │ │ │ │ -151 const DenseIndex startCol = variableColOffsets_[actualStartBlock]; │ │ │ │ │ -152 const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - │ │ │ │ │ -startCol; │ │ │ │ │ -153 return ((const Matrix&)matrix_).block(rowStart_, startCol, this->rows(), │ │ │ │ │ -rangeCols); │ │ │ │ │ -154 } │ │ │ │ │ -155 │ │ │ │ │ -_1_5_7 Block _f_u_l_l() { return range(0, nBlocks()); } │ │ │ │ │ -158 │ │ │ │ │ -_1_6_0 const constBlock _f_u_l_l() const { return range(0, nBlocks()); } │ │ │ │ │ -161 │ │ │ │ │ -162 _D_e_n_s_e_I_n_d_e_x offset(_D_e_n_s_e_I_n_d_e_x block) const { │ │ │ │ │ -163 assertInvariants(); │ │ │ │ │ -164 _D_e_n_s_e_I_n_d_e_x actualBlock = block + blockStart_; │ │ │ │ │ -165 checkBlock(actualBlock); │ │ │ │ │ -166 return variableColOffsets_[actualBlock]; │ │ │ │ │ -167 } │ │ │ │ │ -168 │ │ │ │ │ -_1_7_0 const _D_e_n_s_e_I_n_d_e_x& _r_o_w_S_t_a_r_t() const { return rowStart_; } │ │ │ │ │ -171 │ │ │ │ │ -_1_7_3 _D_e_n_s_e_I_n_d_e_x& _r_o_w_S_t_a_r_t() { return rowStart_; } │ │ │ │ │ -174 │ │ │ │ │ -_1_7_6 const _D_e_n_s_e_I_n_d_e_x& _r_o_w_E_n_d() const { return rowEnd_; } │ │ │ │ │ -177 │ │ │ │ │ -_1_7_9 _D_e_n_s_e_I_n_d_e_x& _r_o_w_E_n_d() { return rowEnd_; } │ │ │ │ │ -180 │ │ │ │ │ -_1_8_2 const _D_e_n_s_e_I_n_d_e_x& _f_i_r_s_t_B_l_o_c_k() const { return blockStart_; } │ │ │ │ │ -183 │ │ │ │ │ -_1_8_5 _D_e_n_s_e_I_n_d_e_x& _f_i_r_s_t_B_l_o_c_k() { return blockStart_; } │ │ │ │ │ -186 │ │ │ │ │ -_1_8_8 const Matrix& _m_a_t_r_i_x() const { return matrix_; } │ │ │ │ │ -189 │ │ │ │ │ -_1_9_1 Matrix& _m_a_t_r_i_x() { return matrix_; } │ │ │ │ │ -192 │ │ │ │ │ -193 protected: │ │ │ │ │ -194 void assertInvariants() const { │ │ │ │ │ -195 assert(matrix_.cols() == variableColOffsets_.back()); │ │ │ │ │ -196 assert(blockStart_ < (_D_e_n_s_e_I_n_d_e_x)variableColOffsets_.size()); │ │ │ │ │ -197 assert(rowStart_ <= matrix_.rows()); │ │ │ │ │ -198 assert(rowEnd_ <= matrix_.rows()); │ │ │ │ │ -199 assert(rowStart_ <= rowEnd_); │ │ │ │ │ -200 } │ │ │ │ │ -201 │ │ │ │ │ -202 void checkBlock(DenseIndex block) const { │ │ │ │ │ -203 static_cast(block); //Disable unused varibale warnings. │ │ │ │ │ -204 assert(matrix_.cols() == variableColOffsets_.back()); │ │ │ │ │ -205 assert(block < (DenseIndex)variableColOffsets_.size() - 1); │ │ │ │ │ -206 assert(variableColOffsets_[block] < matrix_.cols() && variableColOffsets_ │ │ │ │ │ -[block+1] <= matrix_.cols()); │ │ │ │ │ -207 } │ │ │ │ │ -208 │ │ │ │ │ -209 template │ │ │ │ │ -210 void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool │ │ │ │ │ -appendOneDimension) { │ │ │ │ │ -211 variableColOffsets_[0] = 0; │ │ │ │ │ -212 _D_e_n_s_e_I_n_d_e_x j=0; │ │ │ │ │ -213 for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim, ++j) │ │ │ │ │ -214 variableColOffsets_[j+1] = variableColOffsets_[j] + *dim; │ │ │ │ │ -215 if(appendOneDimension) │ │ │ │ │ -216 variableColOffsets_[j+1] = variableColOffsets_[j] + 1; │ │ │ │ │ -217 } │ │ │ │ │ -218 │ │ │ │ │ -219 friend class SymmetricBlockMatrix; │ │ │ │ │ -220 │ │ │ │ │ -221 private: │ │ │ │ │ -_2_2_3 friend class boost::serialization::access; │ │ │ │ │ -224 template │ │ │ │ │ -225 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -226 ar & BOOST_SERIALIZATION_NVP(matrix_); │ │ │ │ │ -227 ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); │ │ │ │ │ -228 ar & BOOST_SERIALIZATION_NVP(rowStart_); │ │ │ │ │ -229 ar & BOOST_SERIALIZATION_NVP(rowEnd_); │ │ │ │ │ -230 ar & BOOST_SERIALIZATION_NVP(blockStart_); │ │ │ │ │ -231 } │ │ │ │ │ -232 }; │ │ │ │ │ -233 │ │ │ │ │ -234} │ │ │ │ │ -_M_a_t_r_i_x_S_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ -Serialization for matrices. │ │ │ │ │ -_F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ -A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ +29namespace boost { │ │ │ │ │ +30namespace serialization { │ │ │ │ │ +31 │ │ │ │ │ +47// split version - sends sizes ahead │ │ │ │ │ +48template │ │ │ │ │ +_5_0void save( │ │ │ │ │ +51 Archive& ar, │ │ │ │ │ +52 const Eigen::Matrix& m, │ │ │ │ │ +53 const unsigned int /*version*/) { │ │ │ │ │ +54 const size_t rows = m.rows(), cols = m.cols(); │ │ │ │ │ +55 ar << BOOST_SERIALIZATION_NVP(rows); │ │ │ │ │ +56 ar << BOOST_SERIALIZATION_NVP(cols); │ │ │ │ │ +57 ar << make_nvp("data", make_array(m.data(), m.size())); │ │ │ │ │ +58} │ │ │ │ │ +59 │ │ │ │ │ +60template │ │ │ │ │ +62void load(Archive& ar, │ │ │ │ │ +63 Eigen::Matrix& m, │ │ │ │ │ +64 const unsigned int /*version*/) { │ │ │ │ │ +65 size_t rows, cols; │ │ │ │ │ +66 ar >> BOOST_SERIALIZATION_NVP(rows); │ │ │ │ │ +67 ar >> BOOST_SERIALIZATION_NVP(cols); │ │ │ │ │ +68 m.resize(rows, cols); │ │ │ │ │ +69 ar >> make_nvp("data", make_array(m.data(), m.size())); │ │ │ │ │ +70} │ │ │ │ │ +71 │ │ │ │ │ +72// templated version of BOOST_SERIALIZATION_SPLIT_FREE(Eigen::Matrix); │ │ │ │ │ +73template │ │ │ │ │ +75void serialize( │ │ │ │ │ +76 Archive& ar, │ │ │ │ │ +77 Eigen::Matrix& m, │ │ │ │ │ +78 const unsigned int version) { │ │ │ │ │ +79 split_free(ar, m, version); │ │ │ │ │ +80} │ │ │ │ │ +81 │ │ │ │ │ +82// specialized to Matrix for MATLAB wrapper │ │ │ │ │ +83template │ │ │ │ │ +84void serialize(Archive& ar, gtsam::Matrix& m, const unsigned int version) { │ │ │ │ │ +85 split_free(ar, m, version); │ │ │ │ │ +86} │ │ │ │ │ +87 │ │ │ │ │ +88} // namespace serialization │ │ │ │ │ +89} // namespace boost │ │ │ │ │ _M_a_t_r_i_x_._h │ │ │ │ │ typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ -std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ -FastVector │ │ │ │ │ -FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ -DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ -_g_t_s_a_m │ │ │ │ │ -Global functions in a separate testing namespace. │ │ │ │ │ -DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_n_s_e_I_n_d_e_x │ │ │ │ │ -ptrdiff_t DenseIndex │ │ │ │ │ -The index type for Eigen objects. │ │ │ │ │ -DDeeffiinniittiioonn types.h:106 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ -of blocks. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ -of vertical blocks. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -VerticalBlockMatrix() │ │ │ │ │ -Construct an empty VerticalBlockMatrix. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:60 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_f_u_l_l │ │ │ │ │ -Block full() │ │ │ │ │ -Return the full matrix, not including any portions excluded by rowStart(), │ │ │ │ │ -rowEnd(),... │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:157 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_r_a_n_g_e │ │ │ │ │ -Block range(DenseIndex startBlock, DenseIndex endBlock) │ │ │ │ │ -access ranges of blocks at a time │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:130 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_f_i_r_s_t_B_l_o_c_k │ │ │ │ │ -const DenseIndex & firstBlock() const │ │ │ │ │ -Get the apparent first block for all operations. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:182 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_m_a_t_r_i_x │ │ │ │ │ -const Matrix & matrix() const │ │ │ │ │ -Access to full matrix (including any portions excluded by rowStart(), rowEnd(), │ │ │ │ │ -and firstBlock()) │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:188 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -VerticalBlockMatrix(const CONTAINER &dimensions, DenseIndex height, bool │ │ │ │ │ -appendOneDimension=false) │ │ │ │ │ -Construct from a container of the sizes of each vertical block. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:69 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_r_o_w_E_n_d__ │ │ │ │ │ -DenseIndex rowEnd_ │ │ │ │ │ -Changes apparent matrix view, see main class comment. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:54 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_r_o_w_E_n_d │ │ │ │ │ -const DenseIndex & rowEnd() const │ │ │ │ │ -Get the apparent last row (exclusive, i.e. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:176 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_r_o_w_s │ │ │ │ │ -DenseIndex rows() const │ │ │ │ │ -Row size. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:115 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ -Block operator()(DenseIndex block) │ │ │ │ │ -Access a single block in the underlying matrix with read/write access. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:124 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_f_i_r_s_t_B_l_o_c_k │ │ │ │ │ -DenseIndex & firstBlock() │ │ │ │ │ -Get or set the apparent first block for all operations. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:185 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_c_o_l_s │ │ │ │ │ -DenseIndex cols() const │ │ │ │ │ -Column size. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:118 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex │ │ │ │ │ -height, bool appendOneDimension=false) │ │ │ │ │ -Construct from iterator over the sizes of each vertical block. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:93 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ -const constBlock operator()(DenseIndex block) const │ │ │ │ │ -Access a const block view. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:127 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_r_o_w_S_t_a_r_t │ │ │ │ │ -const DenseIndex & rowStart() const │ │ │ │ │ -Get the apparent first row of the underlying matrix for all operations. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:170 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_m_a_t_r_i_x__ │ │ │ │ │ -Matrix matrix_ │ │ │ │ │ -The full matrix. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_n_B_l_o_c_k_s │ │ │ │ │ -DenseIndex nBlocks() const │ │ │ │ │ -Block count. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:121 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_f_u_l_l │ │ │ │ │ -const constBlock full() const │ │ │ │ │ -Return the full matrix, not including any portions excluded by rowStart(), │ │ │ │ │ -rowEnd(),... │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:160 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_r_o_w_S_t_a_r_t │ │ │ │ │ -DenseIndex & rowStart() │ │ │ │ │ -Get or set the apparent first row of the underlying matrix for all operations. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:173 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_m_a_t_r_i_x │ │ │ │ │ -Matrix & matrix() │ │ │ │ │ -Non-const access to full matrix (including any portions excluded by rowStart(), │ │ │ │ │ -rowEnd(),... │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:191 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_r_o_w_E_n_d │ │ │ │ │ -DenseIndex & rowEnd() │ │ │ │ │ -Get or set the apparent last row (exclusive, i.e. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:179 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_r_o_w_S_t_a_r_t__ │ │ │ │ │ -DenseIndex rowStart_ │ │ │ │ │ -Changes apparent matrix view, see main class comment. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:53 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -VerticalBlockMatrix(const CONTAINER &dimensions, const Eigen::MatrixBase< │ │ │ │ │ -DERIVED > &matrix, bool appendOneDimension=false) │ │ │ │ │ -Construct from a container of the sizes of each vertical block and a pre- │ │ │ │ │ -prepared matrix. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:80 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_b_l_o_c_k_S_t_a_r_t__ │ │ │ │ │ -DenseIndex blockStart_ │ │ │ │ │ -Changes apparent matrix view, see main class comment. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_v_a_r_i_a_b_l_e_C_o_l_O_f_f_s_e_t_s__ │ │ │ │ │ -FastVector< DenseIndex > variableColOffsets_ │ │ │ │ │ -the starting columns of each block (0-based) │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:51 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_._h │ │ │ │ │ + * _M_a_t_r_i_x_S_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00119.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/types.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/timing.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -97,205 +97,209 @@ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ Macros | │ │ │ │ -Typedefs | │ │ │ │ -Functions
│ │ │ │ -
types.h File Reference
Base
│ │ │ │ +Functions | │ │ │ │ +Variables
│ │ │ │ +
timing.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Typedefs for easier changing of types. │ │ │ │ +

Timing utilities. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

struct  gtsam::const_selector< TEST_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST >
 Helper class that uses templates to select between two types based on whether TEST_TYPE is const or not. More...
class  gtsam::internal::TimingOutline
 Timing Entry, arranged in a tree. More...
 
struct  gtsam::const_selector< BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST >
 Specialization for the non-const version. More...
 
struct  gtsam::const_selector< const BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST >
 Specialization for the const version. More...
 
struct  gtsam::ValueWithDefault< T, defaultValue >
 Helper struct that encapsulates a value with a default, this is just used as a member object so you don't have to specify defaults in the class constructor. More...
 
class  gtsam::ListOfOneContainer< T >
 A helper class that behaves as a container with one element, and works with boost::range. More...
 
class  gtsam::TbbOpenMPMixedScope
 An object whose scope defines a block where TBB and OpenMP parallelism are mixed. More...
 
struct  gtsam::needs_eigen_aligned_allocator< typename, typename >
 A SFINAE trait to mark classes that need special alignment. More...
 
struct  gtsam::needs_eigen_aligned_allocator< T, void_t< typename T::_eigen_aligned_allocator_trait > >
class  gtsam::internal::AutoTicToc
 Small class that calls internal::tic at construction, and internol::toc when destroyed. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ Macros

│ │ │ │ -#define GTSAM_DEPRECATED
 
│ │ │ │ -#define DO_PRAGMA(x)   _Pragma (#x)
 
│ │ │ │ -#define CLANG_DIAGNOSTIC_PUSH_IGNORE(diag)
 
│ │ │ │ -#define GCC_DIAGNOSTIC_PUSH_IGNORE(diag)
 
│ │ │ │ -#define MSVC_DIAGNOSTIC_PUSH_IGNORE(code)
 
│ │ │ │ -#define DIAGNOSTIC_POP()
 
#define assert_throw(CONDITION, EXCEPTION)
 An assertion that throws an exception if NDEBUG is not defined and evaluates to an empty statement otherwise.
 
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 This marks a GTSAM object to require alignment.
 
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 This marks a GTSAM object to require alignment.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ -Typedefs

│ │ │ │ -typedef std::uint64_t gtsam::Key
 Integer nonlinear key type.
 
│ │ │ │ -typedef std::uint64_t gtsam::FactorIndex
 Integer nonlinear factor index type.
 
│ │ │ │ -typedef ptrdiff_t gtsam::DenseIndex
 The index type for Eigen objects.
 
│ │ │ │ -template<typename ... >
using gtsam::void_t = void
 Convenience void_t as we assume C++11, it will not conflict the std one in C++17 as this is in gtsam::
 
#define gttic_(label)
 
│ │ │ │ +#define gttoc_(label)    label##_obj.stop()
 
#define longtic_(label)
 
#define longtoc_(label)
 
#define tictoc_getNode(variable, label)
 
│ │ │ │ +#define gttic(label)   ((void)0)
 
│ │ │ │ +#define gttoc(label)   ((void)0)
 
│ │ │ │ +#define longtic(label)   ((void)0)
 
│ │ │ │ +#define longtoc(label)   ((void)0)
 
│ │ │ │ +#define tictoc_finishedIteration()   ((void)0)
 
│ │ │ │ +#define tictoc_print()   ((void)0)
 
│ │ │ │ +#define tictoc_reset()   ((void)0)
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ Functions

std::string gtsam::demangle (const char *name)
 Pretty print Value type name.
 
│ │ │ │ - gtsam::BOOST_CONCEPT_ASSERT ((boost::RandomAccessRangeConcept< ListOfOneContainer< int > >))
 
│ │ │ │ -template<typename T >
ListOfOneContainer< T > gtsam::ListOfOne (const T &element)
 Factory function for ListOfOneContainer to enable ListOfOne(e) syntax.
 
│ │ │ │ +size_t gtsam::internal::getTicTocID (const char *descriptionC)
 
│ │ │ │ +void gtsam::internal::tic (size_t id, const char *labelC)
 
│ │ │ │ +void gtsam::internal::toc (size_t id, const char *label)
 
│ │ │ │ +void gtsam::tictoc_finishedIteration_ ()
 
│ │ │ │ +void gtsam::tictoc_print_ ()
 
│ │ │ │ +void gtsam::tictoc_print2_ ()
 
│ │ │ │ +void gtsam::tictoc_reset_ ()
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Variables

│ │ │ │ +GTSAM_EXTERN_EXPORT boost::shared_ptr< TimingOutline > gtsam::internal::gTimingRoot
 
│ │ │ │ +GTSAM_EXTERN_EXPORT boost::weak_ptr< TimingOutline > gtsam::internal::gCurrentTimer
 
│ │ │ │

Detailed Description

│ │ │ │ -

Typedefs for easier changing of types.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Aug 21, 2010
│ │ │ │ +

Timing utilities.

│ │ │ │ +
Author
Richard Roberts, Michael Kaess
│ │ │ │ +
Date
Oct 5, 2010
│ │ │ │

Macro Definition Documentation

│ │ │ │ - │ │ │ │ -

◆ assert_throw

│ │ │ │ + │ │ │ │ +

◆ gttic_

│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ │ │ │ │ +
#define assert_throw#define gttic_( CONDITION,
label) EXCEPTION 
│ │ │ │ +
│ │ │ │ +Value:
static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \
│ │ │ │ +
::gtsam::internal::AutoTicToc label##_obj(label##_id_tic, #label)
│ │ │ │ +
gtsam::internal::AutoTicToc
Small class that calls internal::tic at construction, and internol::toc when destroyed.
Definition timing.h:199
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ +

◆ longtic_

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ │ │ │ │
#define longtic_( label))
│ │ │ │
│ │ │ │ -Value:
if (!(CONDITION)) { \
│ │ │ │ -
throw (EXCEPTION); \
│ │ │ │ -
}
│ │ │ │ +Value:
static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \
│ │ │ │ +
::gtsam::internal::ticInternal(label##_id_tic, #label)
│ │ │ │
│ │ │ │ -

An assertion that throws an exception if NDEBUG is not defined and evaluates to an empty statement otherwise.

│ │ │ │ - │ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -

◆ GTSAM_MAKE_ALIGNED_OPERATOR_NEW

│ │ │ │ + │ │ │ │ +

◆ longtoc_

│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW#define longtoc_( label)
│ │ │ │
│ │ │ │ -Value:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW \
│ │ │ │ -
using _eigen_aligned_allocator_trait = void;
│ │ │ │ +Value:
static const size_t label##_id_toc = ::gtsam::internal::getTicTocID(#label); \
│ │ │ │ +
::gtsam::internal::tocInternal(label##_id_toc, #label)
│ │ │ │
│ │ │ │ -

This marks a GTSAM object to require alignment.

│ │ │ │ -

With this macro an object will automatically be allocated in aligned memory when one uses gtsam::make_shared. It reduces future misalignment problems that is hard to debug. See https://eigen.tuxfamily.org/dox/group__DenseMatrixManipulation__Alignement.html for detailed explanation.

│ │ │ │ - │ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -

◆ GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF

│ │ │ │ + │ │ │ │ +

◆ tictoc_getNode

│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF#define tictoc_getNode( NeedsToAlign)variable,
 label 
)
│ │ │ │
│ │ │ │ -Value:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \
│ │ │ │ -
using _eigen_aligned_allocator_trait = void;
│ │ │ │ +Value:
static const size_t label##_id_getnode = ::gtsam::internal::getTicTocID(#label); \
│ │ │ │ +
const boost::shared_ptr<const ::gtsam::internal::TimingOutline> variable = \
│ │ │ │ +
::gtsam::internal::gCurrentTimer.lock()->child(label##_id_getnode, #label, ::gtsam::internal::gCurrentTimer);
│ │ │ │
│ │ │ │ -

This marks a GTSAM object to require alignment.

│ │ │ │ -

With this macro an object will automatically be allocated in aligned memory when one uses gtsam::make_shared. It reduces future misalignment problems that is hard to debug. See https://eigen.tuxfamily.org/dox/group__DenseMatrixManipulation__Alignement.html for detailed explanation.

│ │ │ │ - │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,138 +1,108 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -types.h File Reference │ │ │ │ │ -_B_a_s_e │ │ │ │ │ -Typedefs for easier changing of types. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _F_u_n_c_t_i_o_n_s | _V_a_r_i_a_b_l_e_s │ │ │ │ │ +timing.h File Reference │ │ │ │ │ +Timing utilities. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_c_o_n_s_t___s_e_l_e_c_t_o_r_<_ _T_E_S_T___T_Y_P_E_,_ _B_A_S_I_C___T_Y_P_E_,_ _A_S___N_O_N___C_O_N_S_T_,_ _A_S___C_O_N_S_T_ _> │ │ │ │ │ -  Helper class that uses templates to select between two types based on │ │ │ │ │ - whether TEST_TYPE is const or not. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_c_o_n_s_t___s_e_l_e_c_t_o_r_<_ _B_A_S_I_C___T_Y_P_E_,_ _B_A_S_I_C___T_Y_P_E_,_ _A_S___N_O_N___C_O_N_S_T_,_ _A_S___C_O_N_S_T │ │ │ │ │ - _> │ │ │ │ │ -  Specialization for the non-const version. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_c_o_n_s_t___s_e_l_e_c_t_o_r_<_ _c_o_n_s_t_ _B_A_S_I_C___T_Y_P_E_,_ _B_A_S_I_C___T_Y_P_E_,_ _A_S___N_O_N___C_O_N_S_T_, │ │ │ │ │ - _A_S___C_O_N_S_T_ _> │ │ │ │ │ -  Specialization for the const version. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_V_a_l_u_e_W_i_t_h_D_e_f_a_u_l_t_<_ _T_,_ _d_e_f_a_u_l_t_V_a_l_u_e_ _> │ │ │ │ │ - Helper struct that encapsulates a value with a default, this is just │ │ │ │ │ -  used as a member object so you don't have to specify defaults in the │ │ │ │ │ - class constructor. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_L_i_s_t_O_f_O_n_e_C_o_n_t_a_i_n_e_r_<_ _T_ _> │ │ │ │ │ -  A helper class that behaves as a container with one element, and works │ │ │ │ │ - with boost::range. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_T_b_b_O_p_e_n_M_P_M_i_x_e_d_S_c_o_p_e │ │ │ │ │ -  An object whose scope defines a block where TBB and OpenMP parallelism │ │ │ │ │ - are mixed. _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_T_i_m_i_n_g_O_u_t_l_i_n_e │ │ │ │ │ +  Timing Entry, arranged in a tree. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_n_e_e_d_s___e_i_g_e_n___a_l_i_g_n_e_d___a_l_l_o_c_a_t_o_r_<_ _t_y_p_e_n_a_m_e_,_ _t_y_p_e_n_a_m_e_ _> │ │ │ │ │ -  A SFINAE trait to mark classes that need special alignment. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_n_e_e_d_s___e_i_g_e_n___a_l_i_g_n_e_d___a_l_l_o_c_a_t_o_r_<_ _T_,_ _v_o_i_d___t_<_ _t_y_p_e_n_a_m_e_ _T_:_: │ │ │ │ │ - ___e_i_g_e_n___a_l_i_g_n_e_d___a_l_l_o_c_a_t_o_r___t_r_a_i_t_ _>_ _> │ │ │ │ │ +class   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_A_u_t_o_T_i_c_T_o_c │ │ │ │ │ +  Small class that calls internal::tic at construction, and internol::toc │ │ │ │ │ + when destroyed. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ MMaaccrrooss │ │ │ │ │ -#define  GGTTSSAAMM__DDEEPPRREECCAATTEEDD │ │ │ │ │ -  │ │ │ │ │ -#define  DDOO__PPRRAAGGMMAA(x)   _Pragma (#x) │ │ │ │ │ +#define  _g_t_t_i_c__(label) │ │ │ │ │   │ │ │ │ │ -#define  CCLLAANNGG__DDIIAAGGNNOOSSTTIICC__PPUUSSHH__IIGGNNOORREE(diag) │ │ │ │ │ +#define  ggttttoocc__(label)    label##_obj.stop() │ │ │ │ │   │ │ │ │ │ -#define  GGCCCC__DDIIAAGGNNOOSSTTIICC__PPUUSSHH__IIGGNNOORREE(diag) │ │ │ │ │ +#define  _l_o_n_g_t_i_c__(label) │ │ │ │ │   │ │ │ │ │ -#define  MMSSVVCC__DDIIAAGGNNOOSSTTIICC__PPUUSSHH__IIGGNNOORREE(code) │ │ │ │ │ +#define  _l_o_n_g_t_o_c__(label) │ │ │ │ │   │ │ │ │ │ -#define  DDIIAAGGNNOOSSTTIICC__PPOOPP() │ │ │ │ │ +#define  _t_i_c_t_o_c___g_e_t_N_o_d_e(variable, label) │ │ │ │ │   │ │ │ │ │ -#define  _a_s_s_e_r_t___t_h_r_o_w(CONDITION, EXCEPTION) │ │ │ │ │ -  An assertion that throws an exception if NDEBUG is not defined and │ │ │ │ │ - evaluates to an empty statement otherwise. │ │ │ │ │ +#define  ggttttiicc(label)   ((void)0) │ │ │ │ │   │ │ │ │ │ -#define  _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -  This marks a GTSAM object to require alignment. │ │ │ │ │ +#define  ggttttoocc(label)   ((void)0) │ │ │ │ │   │ │ │ │ │ -#define  _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W___I_F(NeedsToAlign) │ │ │ │ │ -  This marks a GTSAM object to require alignment. │ │ │ │ │ +#define  lloonnggttiicc(label)   ((void)0) │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -typedef std::uint64_t  ggttssaamm::::KKeeyy │ │ │ │ │ -  Integer nonlinear key type. │ │ │ │ │ +#define  lloonnggttoocc(label)   ((void)0) │ │ │ │ │   │ │ │ │ │ -typedef std::uint64_t  ggttssaamm::::FFaaccttoorrIInnddeexx │ │ │ │ │ -  Integer nonlinear factor index type. │ │ │ │ │ +#define  ttiiccttoocc__ffiinniisshheeddIItteerraattiioonn()   ((void)0) │ │ │ │ │   │ │ │ │ │ - typedef ptrdiff_t  ggttssaamm::::DDeennsseeIInnddeexx │ │ │ │ │ -  The index type for Eigen objects. │ │ │ │ │ +#define  ttiiccttoocc__pprriinntt()   ((void)0) │ │ │ │ │   │ │ │ │ │ -template │ │ │ │ │ - using  ggttssaamm::::vvooiidd__tt = void │ │ │ │ │ -  Convenience void_t as we assume C++11, it will not │ │ │ │ │ - conflict the std one in C++17 as this is in gtsam:: │ │ │ │ │ +#define  ttiiccttoocc__rreesseett()   ((void)0) │ │ │ │ │   │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ - std::string  _g_t_s_a_m_:_:_d_e_m_a_n_g_l_e (const char *name) │ │ │ │ │ -  Pretty print _V_a_l_u_e type name. │ │ │ │ │ +size_t  ggttssaamm::::iinntteerrnnaall::::ggeettTTiiccTTooccIIDD (const char *descriptionC) │ │ │ │ │ +  │ │ │ │ │ + void  ggttssaamm::::iinntteerrnnaall::::ttiicc (size_t id, const char *labelC) │ │ │ │ │ +  │ │ │ │ │ + void  ggttssaamm::::iinntteerrnnaall::::ttoocc (size_t id, const char *label) │ │ │ │ │ +  │ │ │ │ │ + void  ggttssaamm::::ttiiccttoocc__ffiinniisshheeddIItteerraattiioonn__ () │ │ │ │ │   │ │ │ │ │ -   ggttssaamm::::BBOOOOSSTT__CCOONNCCEEPPTT__AASSSSEERRTT ((boost:: │ │ │ │ │ - RandomAccessRangeConcept< _L_i_s_t_O_f_O_n_e_C_o_n_t_a_i_n_e_r< int > │ │ │ │ │ - >)) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -_L_i_s_t_O_f_O_n_e_C_o_n_t_a_i_n_e_r< T >  ggttssaamm::::LLiissttOOffOOnnee (const T &element) │ │ │ │ │ -  Factory function for _L_i_s_t_O_f_O_n_e_C_o_n_t_a_i_n_e_r to enable │ │ │ │ │ - ListOfOne(e) syntax. │ │ │ │ │ + void  ggttssaamm::::ttiiccttoocc__pprriinntt__ () │ │ │ │ │ +  │ │ │ │ │ + void  ggttssaamm::::ttiiccttoocc__pprriinntt22__ () │ │ │ │ │ +  │ │ │ │ │ + void  ggttssaamm::::ttiiccttoocc__rreesseett__ () │ │ │ │ │ +  │ │ │ │ │ +VVaarriiaabblleess │ │ │ │ │ +GTSAM_EXTERN_EXPORT boost::shared_ptr< │ │ │ │ │ + _T_i_m_i_n_g_O_u_t_l_i_n_e >  ggttssaamm::::iinntteerrnnaall::::ggTTiimmiinnggRRoooott │ │ │ │ │ +  │ │ │ │ │ + GTSAM_EXTERN_EXPORT boost::weak_ptr< │ │ │ │ │ + _T_i_m_i_n_g_O_u_t_l_i_n_e >  ggttssaamm::::iinntteerrnnaall::::ggCCuurrrreennttTTiimmeerr │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Typedefs for easier changing of types. │ │ │ │ │ +Timing utilities. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Richard Roberts, Michael Kaess │ │ │ │ │ Date │ │ │ │ │ - Aug 21, 2010 │ │ │ │ │ + Oct 5, 2010 │ │ │ │ │ ********** MMaaccrroo DDeeffiinniittiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ -********** _?◆_? aasssseerrtt__tthhrrooww ********** │ │ │ │ │ -#define assert_throw (   CONDITION, │ │ │ │ │ -   EXCEPTION  │ │ │ │ │ - ) │ │ │ │ │ +********** _?◆_? ggttttiicc__ ********** │ │ │ │ │ +#define gttic_ (   label ) │ │ │ │ │ +VVaalluuee:: │ │ │ │ │ +static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \ │ │ │ │ │ +_:_:_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_A_u_t_o_T_i_c_T_o_c label##_obj(label##_id_tic, #label) │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_A_u_t_o_T_i_c_T_o_c │ │ │ │ │ +Small class that calls internal::tic at construction, and internol::toc when │ │ │ │ │ +destroyed. │ │ │ │ │ +DDeeffiinniittiioonn timing.h:199 │ │ │ │ │ +********** _?◆_? lloonnggttiicc__ ********** │ │ │ │ │ +#define longtic_ (   label ) │ │ │ │ │ VVaalluuee:: │ │ │ │ │ -if (!(CONDITION)) { \ │ │ │ │ │ -throw (EXCEPTION); \ │ │ │ │ │ -} │ │ │ │ │ -An assertion that throws an exception if NDEBUG is not defined and evaluates to │ │ │ │ │ -an empty statement otherwise. │ │ │ │ │ -********** _?◆_? GGTTSSAAMM__MMAAKKEE__AALLIIGGNNEEDD__OOPPEERRAATTOORR__NNEEWW ********** │ │ │ │ │ -#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ +static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \ │ │ │ │ │ +::gtsam::internal::ticInternal(label##_id_tic, #label) │ │ │ │ │ +********** _?◆_? lloonnggttoocc__ ********** │ │ │ │ │ +#define longtoc_ (   label ) │ │ │ │ │ VVaalluuee:: │ │ │ │ │ -EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ │ │ │ │ │ -using _eigen_aligned_allocator_trait = void; │ │ │ │ │ -This marks a GTSAM object to require alignment. │ │ │ │ │ -With this macro an object will automatically be allocated in aligned memory │ │ │ │ │ -when one uses _g_t_s_a_m_:_:_m_a_k_e___s_h_a_r_e_d. It reduces future misalignment problems that │ │ │ │ │ -is hard to debug. See _h_t_t_p_s_:_/_/_e_i_g_e_n_._t_u_x_f_a_m_i_l_y_._o_r_g_/_d_o_x_/ │ │ │ │ │ -_g_r_o_u_p_____D_e_n_s_e_M_a_t_r_i_x_M_a_n_i_p_u_l_a_t_i_o_n_____A_l_i_g_n_e_m_e_n_t_._h_t_m_l for detailed explanation. │ │ │ │ │ -********** _?◆_? GGTTSSAAMM__MMAAKKEE__AALLIIGGNNEEDD__OOPPEERRAATTOORR__NNEEWW__IIFF ********** │ │ │ │ │ -#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF (   NeedsToAlign ) │ │ │ │ │ +static const size_t label##_id_toc = ::gtsam::internal::getTicTocID(#label); \ │ │ │ │ │ +::gtsam::internal::tocInternal(label##_id_toc, #label) │ │ │ │ │ +********** _?◆_? ttiiccttoocc__ggeettNNooddee ********** │ │ │ │ │ +#define tictoc_getNode (   variable, │ │ │ │ │ +   label  │ │ │ │ │ + ) │ │ │ │ │ VVaalluuee:: │ │ │ │ │ -EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \ │ │ │ │ │ -using _eigen_aligned_allocator_trait = void; │ │ │ │ │ -This marks a GTSAM object to require alignment. │ │ │ │ │ -With this macro an object will automatically be allocated in aligned memory │ │ │ │ │ -when one uses _g_t_s_a_m_:_:_m_a_k_e___s_h_a_r_e_d. It reduces future misalignment problems that │ │ │ │ │ -is hard to debug. See _h_t_t_p_s_:_/_/_e_i_g_e_n_._t_u_x_f_a_m_i_l_y_._o_r_g_/_d_o_x_/ │ │ │ │ │ -_g_r_o_u_p_____D_e_n_s_e_M_a_t_r_i_x_M_a_n_i_p_u_l_a_t_i_o_n_____A_l_i_g_n_e_m_e_n_t_._h_t_m_l for detailed explanation. │ │ │ │ │ +static const size_t label##_id_getnode = ::gtsam::internal::getTicTocID │ │ │ │ │ +(#label); \ │ │ │ │ │ +const boost::shared_ptr variable = \ │ │ │ │ │ +::gtsam::internal::gCurrentTimer.lock()->child(label##_id_getnode, #label, :: │ │ │ │ │ +gtsam::internal::gCurrentTimer); │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _t_y_p_e_s_._h │ │ │ │ │ + * _t_i_m_i_n_g_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00119.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,19 +1,4 @@ │ │ │ │ │ var a00119 = [ │ │ │ │ │ - ["gtsam::const_selector< TEST_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST >", "a02536.html", null], │ │ │ │ │ - ["gtsam::const_selector< BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST >", "a02540.html", null], │ │ │ │ │ - ["gtsam::const_selector< const BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST >", "a02544.html", null], │ │ │ │ │ - ["gtsam::ValueWithDefault< T, defaultValue >", "a02548.html", "a02548"], │ │ │ │ │ - ["gtsam::ListOfOneContainer< T >", "a02552.html", null], │ │ │ │ │ - ["gtsam::TbbOpenMPMixedScope", "a02556.html", null], │ │ │ │ │ - ["gtsam::needs_eigen_aligned_allocator< typename, typename >", "a02560.html", null], │ │ │ │ │ - ["gtsam::needs_eigen_aligned_allocator< T, void_t< typename T::_eigen_aligned_allocator_trait > >", "a02564.html", null], │ │ │ │ │ - ["assert_throw", "a00119.html#a30d3af2da1ea940197addce4f3c223c8", null], │ │ │ │ │ - ["GTSAM_MAKE_ALIGNED_OPERATOR_NEW", "a00119.html#aa123dd61687e08ae328154a1890b7b86", null], │ │ │ │ │ - ["GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF", "a00119.html#a352ab46cee3a766e30b049690802fb0a", null], │ │ │ │ │ - ["DenseIndex", "a00119.html#a21006f326f57cdf7d3f35fec40f43e40", null], │ │ │ │ │ - ["FactorIndex", "a00119.html#a19c53f2faabb0f4b4b78ce2f7168cc14", null], │ │ │ │ │ - ["Key", "a00119.html#adad029f5f6ffce610428b5fe768b0df2", null], │ │ │ │ │ - ["void_t", "a00119.html#a6525d741529c7a2cd91fa913431f5fb7", null], │ │ │ │ │ - ["demangle", "a00119.html#a0b97fc96e5f69236e81489c66d5b92ba", null], │ │ │ │ │ - ["ListOfOne", "a00119.html#a045b43784acac77531813c4ab6c4d281", null] │ │ │ │ │ + ["gtsam::internal::TimingOutline", "a02516.html", "a02516"], │ │ │ │ │ + ["gtsam::internal::AutoTicToc", "a02520.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00119_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/types.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/timing.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,318 +98,328 @@ │ │ │ │
No Matches
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
types.h
│ │ │ │ +
timing.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/dllexport.h>
│ │ │ │ -
23#include <boost/concept/assert.hpp>
│ │ │ │ -
24#include <boost/range/concepts.hpp>
│ │ │ │ -
25#include <gtsam/config.h> // for GTSAM_USE_TBB
│ │ │ │ -
26
│ │ │ │ -
27#include <cstddef>
│ │ │ │ -
28#include <cstdint>
│ │ │ │ -
29
│ │ │ │ -
30#include <exception>
│ │ │ │ -
31#include <string>
│ │ │ │ -
32
│ │ │ │ -
33#ifdef GTSAM_USE_TBB
│ │ │ │ -
34#include <tbb/scalable_allocator.h>
│ │ │ │ -
35#endif
│ │ │ │ -
36
│ │ │ │ -
37#if defined(__GNUC__) || defined(__clang__)
│ │ │ │ -
38#define GTSAM_DEPRECATED __attribute__((deprecated))
│ │ │ │ -
39#elif defined(_MSC_VER)
│ │ │ │ -
40#define GTSAM_DEPRECATED __declspec(deprecated)
│ │ │ │ -
41#else
│ │ │ │ -
42#define GTSAM_DEPRECATED
│ │ │ │ -
43#endif
│ │ │ │ -
44
│ │ │ │ -
45#ifdef GTSAM_USE_EIGEN_MKL_OPENMP
│ │ │ │ -
46#include <omp.h>
│ │ │ │ -
47#endif
│ │ │ │ -
48
│ │ │ │ -
49/* Define macros for ignoring compiler warnings.
│ │ │ │ -
50 * Usage Example:
│ │ │ │ -
51 * ```
│ │ │ │ -
52 * CLANG_DIAGNOSTIC_PUSH_IGNORE("-Wdeprecated-declarations")
│ │ │ │ -
53 * GCC_DIAGNOSTIC_PUSH_IGNORE("-Wdeprecated-declarations")
│ │ │ │ -
54 * MSVC_DIAGNOSTIC_PUSH_IGNORE(4996)
│ │ │ │ -
55 * // ... code you want to suppress deprecation warnings for ...
│ │ │ │ -
56 * DIAGNOSTIC_POP()
│ │ │ │ -
57 * ```
│ │ │ │ -
58 */
│ │ │ │ -
59#define DO_PRAGMA(x) _Pragma (#x)
│ │ │ │ -
60#ifdef __clang__
│ │ │ │ -
61# define CLANG_DIAGNOSTIC_PUSH_IGNORE(diag) \
│ │ │ │ -
62 _Pragma("clang diagnostic push") \
│ │ │ │ -
63 DO_PRAGMA(clang diagnostic ignored diag)
│ │ │ │ -
64#else
│ │ │ │ -
65# define CLANG_DIAGNOSTIC_PUSH_IGNORE(diag)
│ │ │ │ -
66#endif
│ │ │ │ -
67
│ │ │ │ -
68#ifdef __GNUC__
│ │ │ │ -
69# define GCC_DIAGNOSTIC_PUSH_IGNORE(diag) \
│ │ │ │ -
70 _Pragma("GCC diagnostic push") \
│ │ │ │ -
71 DO_PRAGMA(GCC diagnostic ignored diag)
│ │ │ │ -
72#else
│ │ │ │ -
73# define GCC_DIAGNOSTIC_PUSH_IGNORE(diag)
│ │ │ │ -
74#endif
│ │ │ │ -
75
│ │ │ │ -
76#ifdef _MSC_VER
│ │ │ │ -
77# define MSVC_DIAGNOSTIC_PUSH_IGNORE(code) \
│ │ │ │ -
78 _Pragma("warning ( push )") \
│ │ │ │ -
79 DO_PRAGMA(warning ( disable : code ))
│ │ │ │ -
80#else
│ │ │ │ -
81# define MSVC_DIAGNOSTIC_PUSH_IGNORE(code)
│ │ │ │ -
82#endif
│ │ │ │ -
83
│ │ │ │ -
84#if defined(__clang__)
│ │ │ │ -
85# define DIAGNOSTIC_POP() _Pragma("clang diagnostic pop")
│ │ │ │ -
86#elif defined(__GNUC__)
│ │ │ │ -
87# define DIAGNOSTIC_POP() _Pragma("GCC diagnostic pop")
│ │ │ │ -
88#elif defined(_MSC_VER)
│ │ │ │ -
89# define DIAGNOSTIC_POP() _Pragma("warning ( pop )")
│ │ │ │ -
90#else
│ │ │ │ -
91# define DIAGNOSTIC_POP()
│ │ │ │ -
92#endif
│ │ │ │ -
93
│ │ │ │ -
94namespace gtsam {
│ │ │ │ -
95
│ │ │ │ -
97 std::string GTSAM_EXPORT demangle(const char* name);
│ │ │ │ -
98
│ │ │ │ -
100 typedef std::uint64_t Key;
│ │ │ │ -
101
│ │ │ │ -
103 typedef std::uint64_t FactorIndex;
│ │ │ │ -
104
│ │ │ │ -
106 typedef ptrdiff_t DenseIndex;
│ │ │ │ -
107
│ │ │ │ -
108 /* ************************************************************************* */
│ │ │ │ -
113 template<typename TEST_TYPE, typename BASIC_TYPE, typename AS_NON_CONST,
│ │ │ │ -
114 typename AS_CONST>
│ │ │ │ -
│ │ │ │ -
115 struct const_selector {
│ │ │ │ -
116 };
│ │ │ │ -
│ │ │ │ -
117
│ │ │ │ -
119 template<typename BASIC_TYPE, typename AS_NON_CONST, typename AS_CONST>
│ │ │ │ -
│ │ │ │ -
120 struct const_selector<BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST> {
│ │ │ │ -
121 typedef AS_NON_CONST type;
│ │ │ │ -
122 };
│ │ │ │ -
│ │ │ │ -
123
│ │ │ │ -
125 template<typename BASIC_TYPE, typename AS_NON_CONST, typename AS_CONST>
│ │ │ │ -
│ │ │ │ -
126 struct const_selector<const BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST> {
│ │ │ │ -
127 typedef AS_CONST type;
│ │ │ │ -
128 };
│ │ │ │ -
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/base/FastMap.h>
│ │ │ │ +
21#include <gtsam/dllexport.h>
│ │ │ │ +
22#include <gtsam/config.h> // for GTSAM_USE_TBB
│ │ │ │ +
23
│ │ │ │ +
24#include <boost/smart_ptr/shared_ptr.hpp>
│ │ │ │ +
25#include <boost/smart_ptr/weak_ptr.hpp>
│ │ │ │ +
26#include <boost/version.hpp>
│ │ │ │ +
27
│ │ │ │ +
28#include <cstddef>
│ │ │ │ +
29#include <string>
│ │ │ │ +
30
│ │ │ │ +
31// This file contains the GTSAM timing instrumentation library, a low-overhead method for
│ │ │ │ +
32// learning at a medium-fine level how much time various components of an algorithm take
│ │ │ │ +
33// in CPU and wall time.
│ │ │ │ +
34//
│ │ │ │ +
35// The output of this instrumentation is a call-tree-like printout containing statistics
│ │ │ │ +
36// about each instrumented code block. To print this output at any time, call
│ │ │ │ +
37// tictoc_print() or tictoc_print_().
│ │ │ │ +
38//
│ │ │ │ +
39// An overall point to be aware of is that there are two versions of each function - one
│ │ │ │ +
40// ending in an underscore '_' and one without the trailing underscore. The underscore
│ │ │ │ +
41// versions always are active, but the versions without an underscore are active only when
│ │ │ │ +
42// GTSAM_ENABLE_TIMING is defined (automatically defined in our CMake Timing build type).
│ │ │ │ +
43// GTSAM algorithms are all instrumented with the non-underscore versions, so generally
│ │ │ │ +
44// you should use the underscore versions in your own code to leave out the GTSAM detail.
│ │ │ │ +
45//
│ │ │ │ +
46// gttic and gttoc start and stop a timed section, respectively. gttic creates a *scoped*
│ │ │ │ +
47// object - when it goes out of scope gttoc is called automatically. Thus, you do not
│ │ │ │ +
48// need to call gttoc if you are timing an entire function (see basic use examples below).
│ │ │ │ +
49// However, you must be *aware* of this scoped nature - putting gttic inside of an if(...)
│ │ │ │ +
50// block, for example, will only time code until the closing brace '}'. See advanced
│ │ │ │ +
51// usage below if you need to avoid this.
│ │ │ │ +
52//
│ │ │ │ +
53// Multiple calls nest automatically - each gttic nests under the previous gttic called
│ │ │ │ +
54// for which gttoc has not been called (or the previous gttic did not go out of scope).
│ │ │ │ +
55//
│ │ │ │ +
56// Basic usage examples are as follows:
│ │ │ │ +
57//
│ │ │ │ +
58// - Timing an entire function:
│ │ │ │ +
59// void myFunction() {
│ │ │ │ +
60// gttic_(myFunction);
│ │ │ │ +
61// ........
│ │ │ │ +
62// }
│ │ │ │ +
63//
│ │ │ │ +
64// - Timing an entire function as well as its component parts:
│ │ │ │ +
65// void myLongFunction() {
│ │ │ │ +
66// gttic_(myLongFunction);
│ │ │ │ +
67// gttic_(step1); // Will nest under the 'myLongFunction' label
│ │ │ │ +
68// ........
│ │ │ │ +
69// gttoc_(step1);
│ │ │ │ +
70// gttic_(step2); // Will nest under the 'myLongFunction' label
│ │ │ │ +
71// ........
│ │ │ │ +
72// gttoc_(step2);
│ │ │ │ +
73// ........
│ │ │ │ +
74// }
│ │ │ │ +
75//
│ │ │ │ +
76// - Timing functions calling/called by other functions:
│ │ │ │ +
77// void oneStep() {
│ │ │ │ +
78// gttic_(oneStep); // Will automatically nest under the gttic label of the calling function
│ │ │ │ +
79// .......
│ │ │ │ +
80// }
│ │ │ │ +
81// void algorithm() {
│ │ │ │ +
82// gttic_(algorithm);
│ │ │ │ +
83// oneStep(); // gttic's inside this function will automatically nest inside our 'algorithm' label
│ │ │ │ +
84// twoStep(); // gttic's inside this function will automatically nest inside our 'algorithm' label
│ │ │ │ +
85// }
│ │ │ │ +
86//
│ │ │ │ +
87//
│ │ │ │ +
88// Advanced usage:
│ │ │ │ +
89//
│ │ │ │ +
90// - "Finishing iterations" - to get correct min/max times for each call, you must define
│ │ │ │ +
91// in your code what constitutes an iteration. A single sum for the min/max times is
│ │ │ │ +
92// accumulated within each iteration. If you don't care about min/max times, you don't
│ │ │ │ +
93// need to worry about this. For example:
│ │ │ │ +
94// void myOuterLoop() {
│ │ │ │ +
95// while(true) {
│ │ │ │ +
96// iterateMyAlgorithm();
│ │ │ │ +
97// tictoc_finishedIteration_();
│ │ │ │ +
98// tictoc_print_(); // Optional
│ │ │ │ +
99// }
│ │ │ │ +
100// }
│ │ │ │ +
101//
│ │ │ │ +
102// - Stopping timing a section in a different scope than it is started. Normally, a gttoc
│ │ │ │ +
103// statement goes out of scope at end of C++ scope. However, you can use longtic and
│ │ │ │ +
104// longtoc to start and stop timing with the specified label at any point, without regard
│ │ │ │ +
105// too scope. Note that if you use these, it may become difficult to ensure that you
│ │ │ │ +
106// have matching gttic/gttoc statments. You may want to consider reorganizing your timing
│ │ │ │ +
107// outline to match the scope of your code.
│ │ │ │ +
108
│ │ │ │ +
109// Automatically use the new Boost timers if version is recent enough.
│ │ │ │ +
110#if BOOST_VERSION >= 104800
│ │ │ │ +
111# ifndef GTSAM_DISABLE_NEW_TIMERS
│ │ │ │ +
112# define GTSAM_USING_NEW_BOOST_TIMERS
│ │ │ │ +
113# endif
│ │ │ │ +
114#endif
│ │ │ │ +
115
│ │ │ │ +
116#ifdef GTSAM_USING_NEW_BOOST_TIMERS
│ │ │ │ +
117# include <boost/timer/timer.hpp>
│ │ │ │ +
118#else
│ │ │ │ +
119# include <boost/timer.hpp>
│ │ │ │ +
120# include <gtsam/base/types.h>
│ │ │ │ +
121#endif
│ │ │ │ +
122
│ │ │ │ +
123#ifdef GTSAM_USE_TBB
│ │ │ │ +
124# include <tbb/tick_count.h>
│ │ │ │ +
125# undef min
│ │ │ │ +
126# undef max
│ │ │ │ +
127# undef ERROR
│ │ │ │ +
128#endif
│ │ │ │
129
│ │ │ │ -
130 /* ************************************************************************* */
│ │ │ │ -
136 template<typename T, T defaultValue>
│ │ │ │ -
│ │ │ │ -
137 struct ValueWithDefault {
│ │ │ │ -
138 T value;
│ │ │ │ -
139
│ │ │ │ -
141 ValueWithDefault() : value(defaultValue) {}
│ │ │ │ -
142
│ │ │ │ -
144 ValueWithDefault(const T& _value) : value(_value) {}
│ │ │ │ -
145
│ │ │ │ -
147 T& operator*() { return value; }
│ │ │ │ -
148
│ │ │ │ -
150 const T& operator*() const { return value; }
│ │ │ │ -
151
│ │ │ │ -
153 operator T() const { return value; }
│ │ │ │ -
154 };
│ │ │ │ -
│ │ │ │ -
155
│ │ │ │ -
156 /* ************************************************************************* */
│ │ │ │ -
159 template<typename T>
│ │ │ │ -
│ │ │ │ -
160 class ListOfOneContainer {
│ │ │ │ -
161 T element_;
│ │ │ │ -
162 public:
│ │ │ │ -
163 typedef T value_type;
│ │ │ │ -
164 typedef const T* const_iterator;
│ │ │ │ -
165 typedef T* iterator;
│ │ │ │ -
166 ListOfOneContainer(const T& element) : element_(element) {}
│ │ │ │ -
167 const T* begin() const { return &element_; }
│ │ │ │ -
168 const T* end() const { return &element_ + 1; }
│ │ │ │ -
169 T* begin() { return &element_; }
│ │ │ │ -
170 T* end() { return &element_ + 1; }
│ │ │ │ -
171 size_t size() const { return 1; }
│ │ │ │ -
172 };
│ │ │ │ -
│ │ │ │ -
173
│ │ │ │ -
174 BOOST_CONCEPT_ASSERT((boost::RandomAccessRangeConcept<ListOfOneContainer<int> >));
│ │ │ │ -
175
│ │ │ │ -
177 template<typename T>
│ │ │ │ -
│ │ │ │ -
178 ListOfOneContainer<T> ListOfOne(const T& element) {
│ │ │ │ -
179 return ListOfOneContainer<T>(element);
│ │ │ │ -
180 }
│ │ │ │ -
│ │ │ │ -
181
│ │ │ │ -
182 /* ************************************************************************* */
│ │ │ │ -
183#ifdef __clang__
│ │ │ │ -
184# pragma clang diagnostic push
│ │ │ │ -
185# pragma clang diagnostic ignored "-Wunused-private-field" // Clang complains that previousOpenMPThreads is unused in the #else case below
│ │ │ │ -
186#endif
│ │ │ │ -
187
│ │ │ │ -
│ │ │ │ -
191 class TbbOpenMPMixedScope
│ │ │ │ -
192 {
│ │ │ │ -
193 int previousOpenMPThreads;
│ │ │ │ -
194
│ │ │ │ -
195 public:
│ │ │ │ -
196#if defined GTSAM_USE_TBB && defined GTSAM_USE_EIGEN_MKL_OPENMP
│ │ │ │ -
197 TbbOpenMPMixedScope() :
│ │ │ │ -
198 previousOpenMPThreads(omp_get_num_threads())
│ │ │ │ -
199 {
│ │ │ │ -
200 omp_set_num_threads(omp_get_num_procs() / 4);
│ │ │ │ -
201 }
│ │ │ │ -
202
│ │ │ │ -
203 ~TbbOpenMPMixedScope()
│ │ │ │ -
204 {
│ │ │ │ -
205 omp_set_num_threads(previousOpenMPThreads);
│ │ │ │ -
206 }
│ │ │ │ -
207#else
│ │ │ │ -
208 TbbOpenMPMixedScope() : previousOpenMPThreads(-1) {}
│ │ │ │ -
209 ~TbbOpenMPMixedScope() {}
│ │ │ │ -
210#endif
│ │ │ │ -
211 };
│ │ │ │ -
│ │ │ │ -
212
│ │ │ │ -
213#ifdef __clang__
│ │ │ │ -
214# pragma clang diagnostic pop
│ │ │ │ -
215#endif
│ │ │ │ -
216
│ │ │ │ -
217}
│ │ │ │ +
130namespace gtsam {
│ │ │ │ +
131
│ │ │ │ +
132 namespace internal {
│ │ │ │ +
133 // Generate/retrieve a unique global ID number that will be used to look up tic/toc statements
│ │ │ │ +
134 GTSAM_EXPORT size_t getTicTocID(const char *description);
│ │ │ │ +
135
│ │ │ │ +
136 // Create new TimingOutline child for gCurrentTimer, make it gCurrentTimer, and call tic method
│ │ │ │ +
137 GTSAM_EXPORT void tic(size_t id, const char *label);
│ │ │ │ +
138
│ │ │ │ +
139 // Call toc on gCurrentTimer and then set gCurrentTimer to the parent of gCurrentTimer
│ │ │ │ +
140 GTSAM_EXPORT void toc(size_t id, const char *label);
│ │ │ │ +
141
│ │ │ │ +
│ │ │ │ +
145 class TimingOutline {
│ │ │ │ +
146 protected:
│ │ │ │ +
147 size_t id_;
│ │ │ │ +
148 size_t t_;
│ │ │ │ +
149 size_t tWall_;
│ │ │ │ +
150 double t2_ ;
│ │ │ │ +
151 size_t tIt_;
│ │ │ │ +
152 size_t tMax_;
│ │ │ │ +
153 size_t tMin_;
│ │ │ │ +
154 size_t n_;
│ │ │ │ +
155 size_t myOrder_;
│ │ │ │ +
156 size_t lastChildOrder_;
│ │ │ │ +
157 std::string label_;
│ │ │ │ +
158
│ │ │ │ +
159 // Tree structure
│ │ │ │ +
160 boost::weak_ptr<TimingOutline> parent_;
│ │ │ │ +
161 typedef FastMap<size_t, boost::shared_ptr<TimingOutline> > ChildMap;
│ │ │ │ +
162 ChildMap children_;
│ │ │ │ +
163
│ │ │ │ +
164#ifdef GTSAM_USING_NEW_BOOST_TIMERS
│ │ │ │ +
165 boost::timer::cpu_timer timer_;
│ │ │ │ +
166#else
│ │ │ │ +
167 boost::timer timer_;
│ │ │ │ +
168 gtsam::ValueWithDefault<bool,false> timerActive_;
│ │ │ │ +
169#endif
│ │ │ │ +
170#ifdef GTSAM_USE_TBB
│ │ │ │ +
171 tbb::tick_count tbbTimer_;
│ │ │ │ +
172#endif
│ │ │ │ +
173 void add(size_t usecs, size_t usecsWall);
│ │ │ │ +
174
│ │ │ │ +
175 public:
│ │ │ │ +
177 GTSAM_EXPORT TimingOutline(const std::string& label, size_t myId);
│ │ │ │ +
178 GTSAM_EXPORT size_t time() const;
│ │ │ │ +
179 double secs() const { return double(time()) / 1000000.0;}
│ │ │ │ +
│ │ │ │ +
180 double self() const { return double(t_) / 1000000.0;}
│ │ │ │ +
│ │ │ │ +
181 double wall() const { return double(tWall_) / 1000000.0;}
│ │ │ │ +
│ │ │ │ +
182 double min() const { return double(tMin_) / 1000000.0;}
│ │ │ │ +
│ │ │ │ +
183 double max() const { return double(tMax_) / 1000000.0;}
│ │ │ │ +
│ │ │ │ +
184 double mean() const { return self() / double(n_); }
│ │ │ │ +
185 GTSAM_EXPORT void print(const std::string& outline = "") const;
│ │ │ │ +
186 GTSAM_EXPORT void print2(const std::string& outline = "", const double parentTotal = -1.0) const;
│ │ │ │ +
187 GTSAM_EXPORT const boost::shared_ptr<TimingOutline>&
│ │ │ │ +
188 child(size_t child, const std::string& label, const boost::weak_ptr<TimingOutline>& thisPtr);
│ │ │ │ +
189 GTSAM_EXPORT void tic();
│ │ │ │ +
190 GTSAM_EXPORT void toc();
│ │ │ │ +
191 GTSAM_EXPORT void finishedIteration();
│ │ │ │ +
192
│ │ │ │ +
193 GTSAM_EXPORT friend void toc(size_t id, const char *label);
│ │ │ │ +
194 }; // \TimingOutline
│ │ │ │ +
195
│ │ │ │ +
│ │ │ │ +
199 class GTSAM_EXPORT AutoTicToc {
│ │ │ │ +
200 private:
│ │ │ │ +
201 size_t id_;
│ │ │ │ +
202 const char* label_;
│ │ │ │ +
203 bool isSet_;
│ │ │ │ +
204
│ │ │ │ +
205 public:
│ │ │ │ +
206 AutoTicToc(size_t id, const char* label)
│ │ │ │ +
207 : id_(id), label_(label), isSet_(true) {
│ │ │ │ +
208 tic(id_, label_);
│ │ │ │ +
209 }
│ │ │ │ +
210 void stop() {
│ │ │ │ +
211 toc(id_, label_);
│ │ │ │ +
212 isSet_ = false;
│ │ │ │ +
213 }
│ │ │ │ +
214 ~AutoTicToc() {
│ │ │ │ +
215 if (isSet_) stop();
│ │ │ │ +
216 }
│ │ │ │ +
217 };
│ │ │ │ +
│ │ │ │
218
│ │ │ │ -
219/* ************************************************************************* */
│ │ │ │ -
222#ifdef NDEBUG
│ │ │ │ -
223#define assert_throw(CONDITION, EXCEPTION) ((void)0)
│ │ │ │ -
224#else
│ │ │ │ -
│ │ │ │ -
225#define assert_throw(CONDITION, EXCEPTION) \
│ │ │ │ -
226 if (!(CONDITION)) { \
│ │ │ │ -
227 throw (EXCEPTION); \
│ │ │ │ -
228 }
│ │ │ │ -
│ │ │ │ -
229#endif
│ │ │ │ -
230
│ │ │ │ -
231#ifdef _MSC_VER
│ │ │ │ -
232
│ │ │ │ -
233// Define some common g++ functions and macros we use that MSVC does not have
│ │ │ │ -
234
│ │ │ │ -
235#if (_MSC_VER < 1800)
│ │ │ │ -
236
│ │ │ │ -
237#include <boost/math/special_functions/fpclassify.hpp>
│ │ │ │ -
238namespace std {
│ │ │ │ -
239 template<typename T> inline int isfinite(T a) {
│ │ │ │ -
240 return (int)boost::math::isfinite(a); }
│ │ │ │ -
241 template<typename T> inline int isnan(T a) {
│ │ │ │ -
242 return (int)boost::math::isnan(a); }
│ │ │ │ -
243 template<typename T> inline int isinf(T a) {
│ │ │ │ -
244 return (int)boost::math::isinf(a); }
│ │ │ │ -
245}
│ │ │ │ -
246
│ │ │ │ -
247#endif
│ │ │ │ -
248
│ │ │ │ -
249#include <boost/math/constants/constants.hpp>
│ │ │ │ -
250#ifndef M_PI
│ │ │ │ -
251#define M_PI (boost::math::constants::pi<double>())
│ │ │ │ -
252#endif
│ │ │ │ -
253#ifndef M_PI_2
│ │ │ │ -
254#define M_PI_2 (boost::math::constants::pi<double>() / 2.0)
│ │ │ │ -
255#endif
│ │ │ │ -
256#ifndef M_PI_4
│ │ │ │ -
257#define M_PI_4 (boost::math::constants::pi<double>() / 4.0)
│ │ │ │ -
258#endif
│ │ │ │ +
219 GTSAM_EXTERN_EXPORT boost::shared_ptr<TimingOutline> gTimingRoot;
│ │ │ │ +
220 GTSAM_EXTERN_EXPORT boost::weak_ptr<TimingOutline> gCurrentTimer;
│ │ │ │ +
221 }
│ │ │ │ +
222
│ │ │ │ +
223// Tic and toc functions that are always active (whether or not ENABLE_TIMING is defined)
│ │ │ │ +
224// There is a trick being used here to achieve near-zero runtime overhead, in that a
│ │ │ │ +
225// static variable is created for each tic/toc statement storing an integer ID, but the
│ │ │ │ +
226// integer ID is only looked up by string once when the static variable is initialized
│ │ │ │ +
227// as the program starts.
│ │ │ │ +
228
│ │ │ │ +
229// tic
│ │ │ │ +
230#define gttic_(label) \
│ │ │ │ +
231 static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \
│ │ │ │ +
232 ::gtsam::internal::AutoTicToc label##_obj(label##_id_tic, #label)
│ │ │ │ +
233
│ │ │ │ +
234// toc
│ │ │ │ +
235#define gttoc_(label) \
│ │ │ │ +
236 label##_obj.stop()
│ │ │ │ +
237
│ │ │ │ +
238// tic
│ │ │ │ +
239#define longtic_(label) \
│ │ │ │ +
240 static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \
│ │ │ │ +
241 ::gtsam::internal::ticInternal(label##_id_tic, #label)
│ │ │ │ +
242
│ │ │ │ +
243// toc
│ │ │ │ +
244#define longtoc_(label) \
│ │ │ │ +
245 static const size_t label##_id_toc = ::gtsam::internal::getTicTocID(#label); \
│ │ │ │ +
246 ::gtsam::internal::tocInternal(label##_id_toc, #label)
│ │ │ │ +
247
│ │ │ │ +
248// indicate iteration is finished
│ │ │ │ +
249inline void tictoc_finishedIteration_() {
│ │ │ │ +
250 ::gtsam::internal::gTimingRoot->finishedIteration(); }
│ │ │ │ +
251
│ │ │ │ +
252// print
│ │ │ │ +
253inline void tictoc_print_() {
│ │ │ │ +
254 ::gtsam::internal::gTimingRoot->print(); }
│ │ │ │ +
255
│ │ │ │ +
256// print mean and standard deviation
│ │ │ │ +
257inline void tictoc_print2_() {
│ │ │ │ +
258 ::gtsam::internal::gTimingRoot->print2(); }
│ │ │ │
259
│ │ │ │ -
260#endif
│ │ │ │ -
261
│ │ │ │ -
262#ifdef min
│ │ │ │ -
263#undef min
│ │ │ │ -
264#endif
│ │ │ │ +
260// get a node by label and assign it to variable
│ │ │ │ +
261#define tictoc_getNode(variable, label) \
│ │ │ │ +
262 static const size_t label##_id_getnode = ::gtsam::internal::getTicTocID(#label); \
│ │ │ │ +
263 const boost::shared_ptr<const ::gtsam::internal::TimingOutline> variable = \
│ │ │ │ +
264 ::gtsam::internal::gCurrentTimer.lock()->child(label##_id_getnode, #label, ::gtsam::internal::gCurrentTimer);
│ │ │ │
265
│ │ │ │ -
266#ifdef max
│ │ │ │ -
267#undef max
│ │ │ │ -
268#endif
│ │ │ │ -
269
│ │ │ │ -
270#ifdef ERROR
│ │ │ │ -
271#undef ERROR
│ │ │ │ -
272#endif
│ │ │ │ -
273
│ │ │ │ -
274namespace gtsam {
│ │ │ │ -
275
│ │ │ │ -
277 template<typename ...> using void_t = void;
│ │ │ │ -
278
│ │ │ │ -
294 template<typename, typename = void_t<>>
│ │ │ │ -
│ │ │ │ -
295 struct needs_eigen_aligned_allocator : std::false_type {
│ │ │ │ -
296 };
│ │ │ │ -
│ │ │ │ -
297 template<typename T>
│ │ │ │ -
│ │ │ │ -
298 struct needs_eigen_aligned_allocator<T, void_t<typename T::_eigen_aligned_allocator_trait>> : std::true_type {
│ │ │ │ -
299 };
│ │ │ │ -
│ │ │ │ -
300
│ │ │ │ -
301}
│ │ │ │ -
302
│ │ │ │ -
│ │ │ │ -
308#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW \
│ │ │ │ -
309 EIGEN_MAKE_ALIGNED_OPERATOR_NEW \
│ │ │ │ -
310 using _eigen_aligned_allocator_trait = void;
│ │ │ │ -
│ │ │ │ -
311
│ │ │ │ -
│ │ │ │ -
317#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \
│ │ │ │ -
318 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \
│ │ │ │ -
319 using _eigen_aligned_allocator_trait = void;
│ │ │ │ +
266// reset
│ │ │ │ +
267inline void tictoc_reset_() {
│ │ │ │ +
268 ::gtsam::internal::gTimingRoot.reset(new ::gtsam::internal::TimingOutline("Total", ::gtsam::internal::getTicTocID("Total")));
│ │ │ │ +
269 ::gtsam::internal::gCurrentTimer = ::gtsam::internal::gTimingRoot; }
│ │ │ │ +
270
│ │ │ │ +
271#ifdef ENABLE_TIMING
│ │ │ │ +
272#define gttic(label) gttic_(label)
│ │ │ │ +
273#define gttoc(label) gttoc_(label)
│ │ │ │ +
274#define longtic(label) longtic_(label)
│ │ │ │ +
275#define longtoc(label) longtoc_(label)
│ │ │ │ +
276#define tictoc_finishedIteration tictoc_finishedIteration_
│ │ │ │ +
277#define tictoc_print tictoc_print_
│ │ │ │ +
278#define tictoc_reset tictoc_reset_
│ │ │ │ +
279#else
│ │ │ │ +
280#define gttic(label) ((void)0)
│ │ │ │ +
281#define gttoc(label) ((void)0)
│ │ │ │ +
282#define longtic(label) ((void)0)
│ │ │ │ +
283#define longtoc(label) ((void)0)
│ │ │ │ +
284#define tictoc_finishedIteration() ((void)0)
│ │ │ │ +
285#define tictoc_print() ((void)0)
│ │ │ │ +
286#define tictoc_reset() ((void)0)
│ │ │ │ +
287#endif
│ │ │ │ +
288
│ │ │ │ +
289}
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │
│ │ │ │ +
FastMap.h
A thin wrapper around std::map that uses boost's fast_pool_allocator.
│ │ │ │ +
types.h
Typedefs for easier changing of types.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::ListOfOne
ListOfOneContainer< T > ListOfOne(const T &element)
Factory function for ListOfOneContainer to enable ListOfOne(e) syntax.
Definition types.h:178
│ │ │ │ -
gtsam::demangle
std::string demangle(const char *name)
Pretty print Value type name.
Definition types.cpp:37
│ │ │ │ -
gtsam::FactorIndex
std::uint64_t FactorIndex
Integer nonlinear factor index type.
Definition types.h:103
│ │ │ │ -
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │ -
gtsam::void_t
void void_t
Convenience void_t as we assume C++11, it will not conflict the std one in C++17 as this is in gtsam:...
Definition types.h:277
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::const_selector
Helper class that uses templates to select between two types based on whether TEST_TYPE is const or n...
Definition types.h:115
│ │ │ │ +
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ +
gtsam::internal::TimingOutline
Timing Entry, arranged in a tree.
Definition timing.h:145
│ │ │ │ +
gtsam::internal::TimingOutline::t2_
double t2_
cache the
Definition timing.h:150
│ │ │ │ +
gtsam::internal::TimingOutline::children_
ChildMap children_
subtrees
Definition timing.h:162
│ │ │ │ +
gtsam::internal::TimingOutline::max
double max() const
max time, in seconds
Definition timing.h:183
│ │ │ │ +
gtsam::internal::TimingOutline::wall
double wall() const
wall time, in seconds
Definition timing.h:181
│ │ │ │ +
gtsam::internal::TimingOutline::secs
double secs() const
time taken, in seconds, including children
Definition timing.h:179
│ │ │ │ +
gtsam::internal::TimingOutline::self
double self() const
self time only, in seconds
Definition timing.h:180
│ │ │ │ +
gtsam::internal::TimingOutline::parent_
boost::weak_ptr< TimingOutline > parent_
parent pointer
Definition timing.h:160
│ │ │ │ +
gtsam::internal::TimingOutline::min
double min() const
min time, in seconds
Definition timing.h:182
│ │ │ │ +
gtsam::internal::TimingOutline::mean
double mean() const
mean self time, in seconds
Definition timing.h:184
│ │ │ │ +
gtsam::internal::TimingOutline::time
GTSAM_EXPORT size_t time() const
time taken, including children
Definition timing.cpp:65
│ │ │ │ +
gtsam::internal::AutoTicToc
Small class that calls internal::tic at construction, and internol::toc when destroyed.
Definition timing.h:199
│ │ │ │
gtsam::ValueWithDefault
Helper struct that encapsulates a value with a default, this is just used as a member object so you d...
Definition types.h:137
│ │ │ │ -
gtsam::ValueWithDefault::operator*
T & operator*()
Operator to access the value.
Definition types.h:147
│ │ │ │ -
gtsam::ValueWithDefault::operator*
const T & operator*() const
Operator to access the value.
Definition types.h:150
│ │ │ │ -
gtsam::ValueWithDefault::ValueWithDefault
ValueWithDefault()
Default constructor, initialize to default value supplied in template argument.
Definition types.h:141
│ │ │ │ -
gtsam::ValueWithDefault::ValueWithDefault
ValueWithDefault(const T &_value)
Initialize to the given value.
Definition types.h:144
│ │ │ │ -
gtsam::ListOfOneContainer
A helper class that behaves as a container with one element, and works with boost::range.
Definition types.h:160
│ │ │ │ -
gtsam::TbbOpenMPMixedScope
An object whose scope defines a block where TBB and OpenMP parallelism are mixed.
Definition types.h:192
│ │ │ │ -
gtsam::needs_eigen_aligned_allocator
A SFINAE trait to mark classes that need special alignment.
Definition types.h:295
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,343 +1,394 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -types.h │ │ │ │ │ +timing.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include │ │ │ │ │ -23#include │ │ │ │ │ -24#include │ │ │ │ │ -25#include // for GTSAM_USE_TBB │ │ │ │ │ -26 │ │ │ │ │ -27#include │ │ │ │ │ -28#include │ │ │ │ │ -29 │ │ │ │ │ -30#include │ │ │ │ │ -31#include │ │ │ │ │ -32 │ │ │ │ │ -33#ifdef GTSAM_USE_TBB │ │ │ │ │ -34#include │ │ │ │ │ -35#endif │ │ │ │ │ -36 │ │ │ │ │ -37#if defined(__GNUC__) || defined(__clang__) │ │ │ │ │ -38#define GTSAM_DEPRECATED __attribute__((deprecated)) │ │ │ │ │ -39#elif defined(_MSC_VER) │ │ │ │ │ -40#define GTSAM_DEPRECATED __declspec(deprecated) │ │ │ │ │ -41#else │ │ │ │ │ -42#define GTSAM_DEPRECATED │ │ │ │ │ -43#endif │ │ │ │ │ -44 │ │ │ │ │ -45#ifdef GTSAM_USE_EIGEN_MKL_OPENMP │ │ │ │ │ -46#include │ │ │ │ │ -47#endif │ │ │ │ │ -48 │ │ │ │ │ -49/* Define macros for ignoring compiler warnings. │ │ │ │ │ -50 * Usage Example: │ │ │ │ │ -51 * ``` │ │ │ │ │ -52 * CLANG_DIAGNOSTIC_PUSH_IGNORE("-Wdeprecated-declarations") │ │ │ │ │ -53 * GCC_DIAGNOSTIC_PUSH_IGNORE("-Wdeprecated-declarations") │ │ │ │ │ -54 * MSVC_DIAGNOSTIC_PUSH_IGNORE(4996) │ │ │ │ │ -55 * // ... code you want to suppress deprecation warnings for ... │ │ │ │ │ -56 * DIAGNOSTIC_POP() │ │ │ │ │ -57 * ``` │ │ │ │ │ -58 */ │ │ │ │ │ -59#define DO_PRAGMA(x) _Pragma (#x) │ │ │ │ │ -60#ifdef __clang__ │ │ │ │ │ -61# define CLANG_DIAGNOSTIC_PUSH_IGNORE(diag) \ │ │ │ │ │ -62 _Pragma("clang diagnostic push") \ │ │ │ │ │ -63 DO_PRAGMA(clang diagnostic ignored diag) │ │ │ │ │ -64#else │ │ │ │ │ -65# define CLANG_DIAGNOSTIC_PUSH_IGNORE(diag) │ │ │ │ │ -66#endif │ │ │ │ │ -67 │ │ │ │ │ -68#ifdef __GNUC__ │ │ │ │ │ -69# define GCC_DIAGNOSTIC_PUSH_IGNORE(diag) \ │ │ │ │ │ -70 _Pragma("GCC diagnostic push") \ │ │ │ │ │ -71 DO_PRAGMA(GCC diagnostic ignored diag) │ │ │ │ │ -72#else │ │ │ │ │ -73# define GCC_DIAGNOSTIC_PUSH_IGNORE(diag) │ │ │ │ │ -74#endif │ │ │ │ │ -75 │ │ │ │ │ -76#ifdef _MSC_VER │ │ │ │ │ -77# define MSVC_DIAGNOSTIC_PUSH_IGNORE(code) \ │ │ │ │ │ -78 _Pragma("warning ( push )") \ │ │ │ │ │ -79 DO_PRAGMA(warning ( disable : code )) │ │ │ │ │ -80#else │ │ │ │ │ -81# define MSVC_DIAGNOSTIC_PUSH_IGNORE(code) │ │ │ │ │ -82#endif │ │ │ │ │ -83 │ │ │ │ │ -84#if defined(__clang__) │ │ │ │ │ -85# define DIAGNOSTIC_POP() _Pragma("clang diagnostic pop") │ │ │ │ │ -86#elif defined(__GNUC__) │ │ │ │ │ -87# define DIAGNOSTIC_POP() _Pragma("GCC diagnostic pop") │ │ │ │ │ -88#elif defined(_MSC_VER) │ │ │ │ │ -89# define DIAGNOSTIC_POP() _Pragma("warning ( pop )") │ │ │ │ │ -90#else │ │ │ │ │ -91# define DIAGNOSTIC_POP() │ │ │ │ │ -92#endif │ │ │ │ │ -93 │ │ │ │ │ -94namespace _g_t_s_a_m { │ │ │ │ │ -95 │ │ │ │ │ -97 std::string GTSAM_EXPORT _d_e_m_a_n_g_l_e(const char* name); │ │ │ │ │ -98 │ │ │ │ │ -_1_0_0 typedef std::uint64_t _K_e_y; │ │ │ │ │ -101 │ │ │ │ │ -_1_0_3 typedef std::uint64_t _F_a_c_t_o_r_I_n_d_e_x; │ │ │ │ │ -104 │ │ │ │ │ -_1_0_6 typedef ptrdiff_t _D_e_n_s_e_I_n_d_e_x; │ │ │ │ │ -107 │ │ │ │ │ -108 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -113 template │ │ │ │ │ -_1_1_5 struct _c_o_n_s_t___s_e_l_e_c_t_o_r { │ │ │ │ │ -116 }; │ │ │ │ │ -117 │ │ │ │ │ -119 template │ │ │ │ │ -_1_2_0 struct _c_o_n_s_t___s_e_l_e_c_t_o_r { │ │ │ │ │ -121 typedef AS_NON_CONST type; │ │ │ │ │ -122 }; │ │ │ │ │ -123 │ │ │ │ │ -125 template │ │ │ │ │ -_1_2_6 struct _c_o_n_s_t___s_e_l_e_c_t_o_r │ │ │ │ │ -{ │ │ │ │ │ -127 typedef AS_CONST type; │ │ │ │ │ -128 }; │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_M_a_p_._h> │ │ │ │ │ +21#include │ │ │ │ │ +22#include // for GTSAM_USE_TBB │ │ │ │ │ +23 │ │ │ │ │ +24#include │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ +27 │ │ │ │ │ +28#include │ │ │ │ │ +29#include │ │ │ │ │ +30 │ │ │ │ │ +31// This file contains the GTSAM timing instrumentation library, a low- │ │ │ │ │ +overhead method for │ │ │ │ │ +32// learning at a medium-fine level how much time various components of an │ │ │ │ │ +algorithm take │ │ │ │ │ +33// in CPU and wall time. │ │ │ │ │ +34// │ │ │ │ │ +35// The output of this instrumentation is a call-tree-like printout containing │ │ │ │ │ +statistics │ │ │ │ │ +36// about each instrumented code block. To print this output at any time, call │ │ │ │ │ +37// tictoc_print() or tictoc_print_(). │ │ │ │ │ +38// │ │ │ │ │ +39// An overall point to be aware of is that there are two versions of each │ │ │ │ │ +function - one │ │ │ │ │ +40// ending in an underscore '_' and one without the trailing underscore. The │ │ │ │ │ +underscore │ │ │ │ │ +41// versions always are active, but the versions without an underscore are │ │ │ │ │ +active only when │ │ │ │ │ +42// GTSAM_ENABLE_TIMING is defined (automatically defined in our CMake Timing │ │ │ │ │ +build type). │ │ │ │ │ +43// GTSAM algorithms are all instrumented with the non-underscore versions, so │ │ │ │ │ +generally │ │ │ │ │ +44// you should use the underscore versions in your own code to leave out the │ │ │ │ │ +GTSAM detail. │ │ │ │ │ +45// │ │ │ │ │ +46// gttic and gttoc start and stop a timed section, respectively. gttic │ │ │ │ │ +creates a *scoped* │ │ │ │ │ +47// object - when it goes out of scope gttoc is called automatically. Thus, │ │ │ │ │ +you do not │ │ │ │ │ +48// need to call gttoc if you are timing an entire function (see basic use │ │ │ │ │ +examples below). │ │ │ │ │ +49// However, you must be *aware* of this scoped nature - putting gttic inside │ │ │ │ │ +of an if(...) │ │ │ │ │ +50// block, for example, will only time code until the closing brace '}'. See │ │ │ │ │ +advanced │ │ │ │ │ +51// usage below if you need to avoid this. │ │ │ │ │ +52// │ │ │ │ │ +53// Multiple calls nest automatically - each gttic nests under the previous │ │ │ │ │ +gttic called │ │ │ │ │ +54// for which gttoc has not been called (or the previous gttic did not go out │ │ │ │ │ +of scope). │ │ │ │ │ +55// │ │ │ │ │ +56// Basic usage examples are as follows: │ │ │ │ │ +57// │ │ │ │ │ +58// - Timing an entire function: │ │ │ │ │ +59// void myFunction() { │ │ │ │ │ +60// gttic_(myFunction); │ │ │ │ │ +61// ........ │ │ │ │ │ +62// } │ │ │ │ │ +63// │ │ │ │ │ +64// - Timing an entire function as well as its component parts: │ │ │ │ │ +65// void myLongFunction() { │ │ │ │ │ +66// gttic_(myLongFunction); │ │ │ │ │ +67// gttic_(step1); // Will nest under the 'myLongFunction' label │ │ │ │ │ +68// ........ │ │ │ │ │ +69// gttoc_(step1); │ │ │ │ │ +70// gttic_(step2); // Will nest under the 'myLongFunction' label │ │ │ │ │ +71// ........ │ │ │ │ │ +72// gttoc_(step2); │ │ │ │ │ +73// ........ │ │ │ │ │ +74// } │ │ │ │ │ +75// │ │ │ │ │ +76// - Timing functions calling/called by other functions: │ │ │ │ │ +77// void oneStep() { │ │ │ │ │ +78// gttic_(oneStep); // Will automatically nest under the gttic label of the │ │ │ │ │ +calling function │ │ │ │ │ +79// ....... │ │ │ │ │ +80// } │ │ │ │ │ +81// void algorithm() { │ │ │ │ │ +82// gttic_(algorithm); │ │ │ │ │ +83// oneStep(); // gttic's inside this function will automatically nest inside │ │ │ │ │ +our 'algorithm' label │ │ │ │ │ +84// twoStep(); // gttic's inside this function will automatically nest inside │ │ │ │ │ +our 'algorithm' label │ │ │ │ │ +85// } │ │ │ │ │ +86// │ │ │ │ │ +87// │ │ │ │ │ +88// Advanced usage: │ │ │ │ │ +89// │ │ │ │ │ +90// - "Finishing iterations" - to get correct min/max times for each call, you │ │ │ │ │ +must define │ │ │ │ │ +91// in your code what constitutes an iteration. A single sum for the min/max │ │ │ │ │ +times is │ │ │ │ │ +92// accumulated within each iteration. If you don't care about min/max times, │ │ │ │ │ +you don't │ │ │ │ │ +93// need to worry about this. For example: │ │ │ │ │ +94// void myOuterLoop() { │ │ │ │ │ +95// while(true) { │ │ │ │ │ +96// iterateMyAlgorithm(); │ │ │ │ │ +97// tictoc_finishedIteration_(); │ │ │ │ │ +98// tictoc_print_(); // Optional │ │ │ │ │ +99// } │ │ │ │ │ +100// } │ │ │ │ │ +101// │ │ │ │ │ +102// - Stopping timing a section in a different scope than it is started. │ │ │ │ │ +Normally, a gttoc │ │ │ │ │ +103// statement goes out of scope at end of C++ scope. However, you can use │ │ │ │ │ +longtic and │ │ │ │ │ +104// longtoc to start and stop timing with the specified label at any point, │ │ │ │ │ +without regard │ │ │ │ │ +105// too scope. Note that if you use these, it may become difficult to ensure │ │ │ │ │ +that you │ │ │ │ │ +106// have matching gttic/gttoc statments. You may want to consider │ │ │ │ │ +reorganizing your timing │ │ │ │ │ +107// outline to match the scope of your code. │ │ │ │ │ +108 │ │ │ │ │ +109// Automatically use the new Boost timers if version is recent enough. │ │ │ │ │ +110#if BOOST_VERSION >= 104800 │ │ │ │ │ +111# ifndef GTSAM_DISABLE_NEW_TIMERS │ │ │ │ │ +112# define GTSAM_USING_NEW_BOOST_TIMERS │ │ │ │ │ +113# endif │ │ │ │ │ +114#endif │ │ │ │ │ +115 │ │ │ │ │ +116#ifdef GTSAM_USING_NEW_BOOST_TIMERS │ │ │ │ │ +117# include │ │ │ │ │ +118#else │ │ │ │ │ +119# include │ │ │ │ │ +120# include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ +121#endif │ │ │ │ │ +122 │ │ │ │ │ +123#ifdef GTSAM_USE_TBB │ │ │ │ │ +124# include │ │ │ │ │ +125# undef min │ │ │ │ │ +126# undef max │ │ │ │ │ +127# undef ERROR │ │ │ │ │ +128#endif │ │ │ │ │ 129 │ │ │ │ │ -130 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -136 template │ │ │ │ │ -_1_3_7 struct _V_a_l_u_e_W_i_t_h_D_e_f_a_u_l_t { │ │ │ │ │ -138 T value; │ │ │ │ │ -139 │ │ │ │ │ -_1_4_1 _V_a_l_u_e_W_i_t_h_D_e_f_a_u_l_t() : value(defaultValue) {} │ │ │ │ │ -142 │ │ │ │ │ -_1_4_4 _V_a_l_u_e_W_i_t_h_D_e_f_a_u_l_t(const T& _value) : value(_value) {} │ │ │ │ │ -145 │ │ │ │ │ -_1_4_7 T& _o_p_e_r_a_t_o_r_*() { return value; } │ │ │ │ │ -148 │ │ │ │ │ -_1_5_0 const T& _o_p_e_r_a_t_o_r_*() const { return value; } │ │ │ │ │ -151 │ │ │ │ │ -_1_5_3 operator T() const { return value; } │ │ │ │ │ -154 }; │ │ │ │ │ -155 │ │ │ │ │ -156 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -159 template │ │ │ │ │ -_1_6_0 class _L_i_s_t_O_f_O_n_e_C_o_n_t_a_i_n_e_r { │ │ │ │ │ -161 T element_; │ │ │ │ │ -162 public: │ │ │ │ │ -163 typedef T value_type; │ │ │ │ │ -164 typedef const T* const_iterator; │ │ │ │ │ -165 typedef T* iterator; │ │ │ │ │ -166 _L_i_s_t_O_f_O_n_e_C_o_n_t_a_i_n_e_r(const T& element) : element_(element) {} │ │ │ │ │ -167 const T* begin() const { return &element_; } │ │ │ │ │ -168 const T* end() const { return &element_ + 1; } │ │ │ │ │ -169 T* begin() { return &element_; } │ │ │ │ │ -170 T* end() { return &element_ + 1; } │ │ │ │ │ -171 size_t size() const { return 1; } │ │ │ │ │ -172 }; │ │ │ │ │ -173 │ │ │ │ │ -174 BOOST_CONCEPT_ASSERT((boost:: │ │ │ │ │ -RandomAccessRangeConcept<_L_i_s_t_O_f_O_n_e_C_o_n_t_a_i_n_e_r_<_i_n_t_> >)); │ │ │ │ │ -175 │ │ │ │ │ -177 template │ │ │ │ │ -_1_7_8 _L_i_s_t_O_f_O_n_e_C_o_n_t_a_i_n_e_r_<_T_> _L_i_s_t_O_f_O_n_e(const T& element) { │ │ │ │ │ -179 return _L_i_s_t_O_f_O_n_e_C_o_n_t_a_i_n_e_r_<_T_>(element); │ │ │ │ │ -180 } │ │ │ │ │ -181 │ │ │ │ │ -182 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -183#ifdef __clang__ │ │ │ │ │ -184# pragma clang diagnostic push │ │ │ │ │ -185# pragma clang diagnostic ignored "-Wunused-private-field" // Clang │ │ │ │ │ -complains that previousOpenMPThreads is unused in the #else case below │ │ │ │ │ -186#endif │ │ │ │ │ -187 │ │ │ │ │ -_1_9_1 class _T_b_b_O_p_e_n_M_P_M_i_x_e_d_S_c_o_p_e │ │ │ │ │ -192 { │ │ │ │ │ -193 int previousOpenMPThreads; │ │ │ │ │ -194 │ │ │ │ │ -195 public: │ │ │ │ │ -196#if defined GTSAM_USE_TBB && defined GTSAM_USE_EIGEN_MKL_OPENMP │ │ │ │ │ -197 _T_b_b_O_p_e_n_M_P_M_i_x_e_d_S_c_o_p_e() : │ │ │ │ │ -198 previousOpenMPThreads(omp_get_num_threads()) │ │ │ │ │ -199 { │ │ │ │ │ -200 omp_set_num_threads(omp_get_num_procs() / 4); │ │ │ │ │ -201 } │ │ │ │ │ -202 │ │ │ │ │ -203 _~_T_b_b_O_p_e_n_M_P_M_i_x_e_d_S_c_o_p_e() │ │ │ │ │ -204 { │ │ │ │ │ -205 omp_set_num_threads(previousOpenMPThreads); │ │ │ │ │ -206 } │ │ │ │ │ -207#else │ │ │ │ │ -208 _T_b_b_O_p_e_n_M_P_M_i_x_e_d_S_c_o_p_e() : previousOpenMPThreads(-1) {} │ │ │ │ │ -209 _~_T_b_b_O_p_e_n_M_P_M_i_x_e_d_S_c_o_p_e() {} │ │ │ │ │ -210#endif │ │ │ │ │ -211 }; │ │ │ │ │ -212 │ │ │ │ │ -213#ifdef __clang__ │ │ │ │ │ -214# pragma clang diagnostic pop │ │ │ │ │ -215#endif │ │ │ │ │ -216 │ │ │ │ │ -217} │ │ │ │ │ +130namespace _g_t_s_a_m { │ │ │ │ │ +131 │ │ │ │ │ +132 namespace internal { │ │ │ │ │ +133 // Generate/retrieve a unique global ID number that will be used to look up │ │ │ │ │ +tic/toc statements │ │ │ │ │ +134 GTSAM_EXPORT size_t getTicTocID(const char *description); │ │ │ │ │ +135 │ │ │ │ │ +136 // Create new TimingOutline child for gCurrentTimer, make it gCurrentTimer, │ │ │ │ │ +and call tic method │ │ │ │ │ +137 GTSAM_EXPORT void tic(size_t id, const char *label); │ │ │ │ │ +138 │ │ │ │ │ +139 // Call toc on gCurrentTimer and then set gCurrentTimer to the parent of │ │ │ │ │ +gCurrentTimer │ │ │ │ │ +140 GTSAM_EXPORT void toc(size_t id, const char *label); │ │ │ │ │ +141 │ │ │ │ │ +_1_4_5 class _T_i_m_i_n_g_O_u_t_l_i_n_e { │ │ │ │ │ +146 protected: │ │ │ │ │ +147 size_t id_; │ │ │ │ │ +148 size_t t_; │ │ │ │ │ +149 size_t tWall_; │ │ │ │ │ +_1_5_0 double _t_2__ ; │ │ │ │ │ +151 size_t tIt_; │ │ │ │ │ +152 size_t tMax_; │ │ │ │ │ +153 size_t tMin_; │ │ │ │ │ +154 size_t n_; │ │ │ │ │ +155 size_t myOrder_; │ │ │ │ │ +156 size_t lastChildOrder_; │ │ │ │ │ +157 std::string label_; │ │ │ │ │ +158 │ │ │ │ │ +159 // Tree structure │ │ │ │ │ +_1_6_0 boost::weak_ptr _p_a_r_e_n_t__; │ │ │ │ │ +161 typedef _F_a_s_t_M_a_p_<_s_i_z_e___t_,_ _b_o_o_s_t_:_:_s_h_a_r_e_d___p_t_r_<_T_i_m_i_n_g_O_u_t_l_i_n_e_> > _C_h_i_l_d_M_a_p; │ │ │ │ │ +_1_6_2 _C_h_i_l_d_M_a_p _c_h_i_l_d_r_e_n__; │ │ │ │ │ +163 │ │ │ │ │ +164#ifdef GTSAM_USING_NEW_BOOST_TIMERS │ │ │ │ │ +165 boost::timer::cpu_timer timer_; │ │ │ │ │ +166#else │ │ │ │ │ +167 boost::timer timer_; │ │ │ │ │ +168 _g_t_s_a_m_:_:_V_a_l_u_e_W_i_t_h_D_e_f_a_u_l_t_<_b_o_o_l_,_f_a_l_s_e_> timerActive_; │ │ │ │ │ +169#endif │ │ │ │ │ +170#ifdef GTSAM_USE_TBB │ │ │ │ │ +171 tbb::tick_count tbbTimer_; │ │ │ │ │ +172#endif │ │ │ │ │ +173 void add(size_t usecs, size_t usecsWall); │ │ │ │ │ +174 │ │ │ │ │ +175 public: │ │ │ │ │ +177 GTSAM_EXPORT _T_i_m_i_n_g_O_u_t_l_i_n_e(const std::string& label, size_t myId); │ │ │ │ │ +178 GTSAM_EXPORT size_t _t_i_m_e() const; │ │ │ │ │ +_1_7_9 double _s_e_c_s() const { return double(_t_i_m_e()) / 1000000.0;} │ │ │ │ │ +_1_8_0 double _s_e_l_f() const { return double(t_) / 1000000.0;} │ │ │ │ │ +_1_8_1 double _w_a_l_l() const { return double(tWall_) / 1000000.0;} │ │ │ │ │ +_1_8_2 double _m_i_n() const { return double(tMin_) / 1000000.0;} │ │ │ │ │ +_1_8_3 double _m_a_x() const { return double(tMax_) / 1000000.0;} │ │ │ │ │ +_1_8_4 double _m_e_a_n() const { return _s_e_l_f() / double(n_); } │ │ │ │ │ +185 GTSAM_EXPORT void print(const std::string& outline = "") const; │ │ │ │ │ +186 GTSAM_EXPORT void print2(const std::string& outline = "", const double │ │ │ │ │ +parentTotal = -1.0) const; │ │ │ │ │ +187 GTSAM_EXPORT const boost::shared_ptr& │ │ │ │ │ +188 child(size_t child, const std::string& label, const boost:: │ │ │ │ │ +weak_ptr& thisPtr); │ │ │ │ │ +189 GTSAM_EXPORT void tic(); │ │ │ │ │ +190 GTSAM_EXPORT void toc(); │ │ │ │ │ +191 GTSAM_EXPORT void finishedIteration(); │ │ │ │ │ +192 │ │ │ │ │ +193 GTSAM_EXPORT friend void toc(size_t id, const char *label); │ │ │ │ │ +194 }; // \TimingOutline │ │ │ │ │ +195 │ │ │ │ │ +_1_9_9 class GTSAM_EXPORT _A_u_t_o_T_i_c_T_o_c { │ │ │ │ │ +200 private: │ │ │ │ │ +201 size_t id_; │ │ │ │ │ +202 const char* label_; │ │ │ │ │ +203 bool isSet_; │ │ │ │ │ +204 │ │ │ │ │ +205 public: │ │ │ │ │ +206 _A_u_t_o_T_i_c_T_o_c(size_t id, const char* label) │ │ │ │ │ +207 : id_(id), label_(label), isSet_(true) { │ │ │ │ │ +208 tic(id_, label_); │ │ │ │ │ +209 } │ │ │ │ │ +210 void stop() { │ │ │ │ │ +211 toc(id_, label_); │ │ │ │ │ +212 isSet_ = false; │ │ │ │ │ +213 } │ │ │ │ │ +214 _~_A_u_t_o_T_i_c_T_o_c() { │ │ │ │ │ +215 if (isSet_) stop(); │ │ │ │ │ +216 } │ │ │ │ │ +217 }; │ │ │ │ │ 218 │ │ │ │ │ -219/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -222#ifdef NDEBUG │ │ │ │ │ -223#define assert_throw(CONDITION, EXCEPTION) ((void)0) │ │ │ │ │ -224#else │ │ │ │ │ -_2_2_5#define assert_throw(CONDITION, EXCEPTION) \ │ │ │ │ │ -226 if (!(CONDITION)) { \ │ │ │ │ │ -227 throw (EXCEPTION); \ │ │ │ │ │ -228 } │ │ │ │ │ -229#endif │ │ │ │ │ -230 │ │ │ │ │ -231#ifdef _MSC_VER │ │ │ │ │ -232 │ │ │ │ │ -233// Define some common g++ functions and macros we use that MSVC does not │ │ │ │ │ -have │ │ │ │ │ -234 │ │ │ │ │ -235#if (_MSC_VER < 1800) │ │ │ │ │ -236 │ │ │ │ │ -237#include │ │ │ │ │ -238namespace std { │ │ │ │ │ -239 template inline int isfinite(T a) { │ │ │ │ │ -240 return (int)boost::math::isfinite(a); } │ │ │ │ │ -241 template inline int isnan(T a) { │ │ │ │ │ -242 return (int)boost::math::isnan(a); } │ │ │ │ │ -243 template inline int isinf(T a) { │ │ │ │ │ -244 return (int)boost::math::isinf(a); } │ │ │ │ │ -245} │ │ │ │ │ -246 │ │ │ │ │ -247#endif │ │ │ │ │ -248 │ │ │ │ │ -249#include │ │ │ │ │ -250#ifndef M_PI │ │ │ │ │ -251#define M_PI (boost::math::constants::pi()) │ │ │ │ │ -252#endif │ │ │ │ │ -253#ifndef M_PI_2 │ │ │ │ │ -254#define M_PI_2 (boost::math::constants::pi() / 2.0) │ │ │ │ │ -255#endif │ │ │ │ │ -256#ifndef M_PI_4 │ │ │ │ │ -257#define M_PI_4 (boost::math::constants::pi() / 4.0) │ │ │ │ │ -258#endif │ │ │ │ │ +219 GTSAM_EXTERN_EXPORT boost::shared_ptr gTimingRoot; │ │ │ │ │ +220 GTSAM_EXTERN_EXPORT boost::weak_ptr gCurrentTimer; │ │ │ │ │ +221 } │ │ │ │ │ +222 │ │ │ │ │ +223// Tic and toc functions that are always active (whether or not │ │ │ │ │ +ENABLE_TIMING is defined) │ │ │ │ │ +224// There is a trick being used here to achieve near-zero runtime overhead, │ │ │ │ │ +in that a │ │ │ │ │ +225// static variable is created for each tic/toc statement storing an integer │ │ │ │ │ +ID, but the │ │ │ │ │ +226// integer ID is only looked up by string once when the static variable is │ │ │ │ │ +initialized │ │ │ │ │ +227// as the program starts. │ │ │ │ │ +228 │ │ │ │ │ +229// tic │ │ │ │ │ +230#define gttic_(label) \ │ │ │ │ │ +231 static const size_t label##_id_tic = ::gtsam::internal::getTicTocID │ │ │ │ │ +(#label); \ │ │ │ │ │ +232 ::gtsam::internal::AutoTicToc label##_obj(label##_id_tic, #label) │ │ │ │ │ +233 │ │ │ │ │ +234// toc │ │ │ │ │ +235#define gttoc_(label) \ │ │ │ │ │ +236 label##_obj.stop() │ │ │ │ │ +237 │ │ │ │ │ +238// tic │ │ │ │ │ +239#define longtic_(label) \ │ │ │ │ │ +240 static const size_t label##_id_tic = ::gtsam::internal::getTicTocID │ │ │ │ │ +(#label); \ │ │ │ │ │ +241 ::gtsam::internal::ticInternal(label##_id_tic, #label) │ │ │ │ │ +242 │ │ │ │ │ +243// toc │ │ │ │ │ +244#define longtoc_(label) \ │ │ │ │ │ +245 static const size_t label##_id_toc = ::gtsam::internal::getTicTocID │ │ │ │ │ +(#label); \ │ │ │ │ │ +246 ::gtsam::internal::tocInternal(label##_id_toc, #label) │ │ │ │ │ +247 │ │ │ │ │ +248// indicate iteration is finished │ │ │ │ │ +249inline void tictoc_finishedIteration_() { │ │ │ │ │ +250 ::gtsam::internal::gTimingRoot->finishedIteration(); } │ │ │ │ │ +251 │ │ │ │ │ +252// print │ │ │ │ │ +253inline void tictoc_print_() { │ │ │ │ │ +254 ::gtsam::internal::gTimingRoot->print(); } │ │ │ │ │ +255 │ │ │ │ │ +256// print mean and standard deviation │ │ │ │ │ +257inline void tictoc_print2_() { │ │ │ │ │ +258 ::gtsam::internal::gTimingRoot->print2(); } │ │ │ │ │ 259 │ │ │ │ │ -260#endif │ │ │ │ │ -261 │ │ │ │ │ -262#ifdef min │ │ │ │ │ -263#undef min │ │ │ │ │ -264#endif │ │ │ │ │ +260// get a node by label and assign it to variable │ │ │ │ │ +261#define tictoc_getNode(variable, label) \ │ │ │ │ │ +262 static const size_t label##_id_getnode = ::gtsam::internal::getTicTocID │ │ │ │ │ +(#label); \ │ │ │ │ │ +263 const boost::shared_ptr variable = │ │ │ │ │ +\ │ │ │ │ │ +264 ::gtsam::internal::gCurrentTimer.lock()->child(label##_id_getnode, #label, │ │ │ │ │ +::gtsam::internal::gCurrentTimer); │ │ │ │ │ 265 │ │ │ │ │ -266#ifdef max │ │ │ │ │ -267#undef max │ │ │ │ │ -268#endif │ │ │ │ │ -269 │ │ │ │ │ -270#ifdef ERROR │ │ │ │ │ -271#undef ERROR │ │ │ │ │ -272#endif │ │ │ │ │ -273 │ │ │ │ │ -274namespace _g_t_s_a_m { │ │ │ │ │ -275 │ │ │ │ │ -_2_7_7 template using _v_o_i_d___t = void; │ │ │ │ │ -278 │ │ │ │ │ -294 template> │ │ │ │ │ -_2_9_5 struct _n_e_e_d_s___e_i_g_e_n___a_l_i_g_n_e_d___a_l_l_o_c_a_t_o_r : std::false_type { │ │ │ │ │ -296 }; │ │ │ │ │ -297 template │ │ │ │ │ -_2_9_8 struct _n_e_e_d_s___e_i_g_e_n___a_l_i_g_n_e_d___a_l_l_o_c_a_t_o_r_<_T_,_ _v_o_i_d___t_<_t_y_p_e_n_a_m_e_ _T_:_: │ │ │ │ │ -___e_i_g_e_n___a_l_i_g_n_e_d___a_l_l_o_c_a_t_o_r___t_r_a_i_t_>> : std::true_type { │ │ │ │ │ -299 }; │ │ │ │ │ -300 │ │ │ │ │ -301} │ │ │ │ │ -302 │ │ │ │ │ -_3_0_8#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW \ │ │ │ │ │ -309 EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ │ │ │ │ │ -310 using _eigen_aligned_allocator_trait = void; │ │ │ │ │ -311 │ │ │ │ │ -_3_1_7#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \ │ │ │ │ │ -318 EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \ │ │ │ │ │ -319 using _eigen_aligned_allocator_trait = void; │ │ │ │ │ +266// reset │ │ │ │ │ +267inline void tictoc_reset_() { │ │ │ │ │ +268 ::gtsam::internal::gTimingRoot.reset(new ::gtsam::internal::TimingOutline │ │ │ │ │ +("Total", ::gtsam::internal::getTicTocID("Total"))); │ │ │ │ │ +269 ::gtsam::internal::gCurrentTimer = ::gtsam::internal::gTimingRoot; } │ │ │ │ │ +270 │ │ │ │ │ +271#ifdef ENABLE_TIMING │ │ │ │ │ +272#define gttic(label) gttic_(label) │ │ │ │ │ +273#define gttoc(label) gttoc_(label) │ │ │ │ │ +274#define longtic(label) longtic_(label) │ │ │ │ │ +275#define longtoc(label) longtoc_(label) │ │ │ │ │ +276#define tictoc_finishedIteration tictoc_finishedIteration_ │ │ │ │ │ +277#define tictoc_print tictoc_print_ │ │ │ │ │ +278#define tictoc_reset tictoc_reset_ │ │ │ │ │ +279#else │ │ │ │ │ +280#define gttic(label) ((void)0) │ │ │ │ │ +281#define gttoc(label) ((void)0) │ │ │ │ │ +282#define longtic(label) ((void)0) │ │ │ │ │ +283#define longtoc(label) ((void)0) │ │ │ │ │ +284#define tictoc_finishedIteration() ((void)0) │ │ │ │ │ +285#define tictoc_print() ((void)0) │ │ │ │ │ +286#define tictoc_reset() ((void)0) │ │ │ │ │ +287#endif │ │ │ │ │ +288 │ │ │ │ │ +289} │ │ │ │ │ +_F_a_s_t_M_a_p_._h │ │ │ │ │ +A thin wrapper around std::map that uses boost's fast_pool_allocator. │ │ │ │ │ +_t_y_p_e_s_._h │ │ │ │ │ +Typedefs for easier changing of types. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_s_t_O_f_O_n_e │ │ │ │ │ -ListOfOneContainer< T > ListOfOne(const T &element) │ │ │ │ │ -Factory function for ListOfOneContainer to enable ListOfOne(e) syntax. │ │ │ │ │ -DDeeffiinniittiioonn types.h:178 │ │ │ │ │ -_g_t_s_a_m_:_:_d_e_m_a_n_g_l_e │ │ │ │ │ -std::string demangle(const char *name) │ │ │ │ │ -Pretty print Value type name. │ │ │ │ │ -DDeeffiinniittiioonn types.cpp:37 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_I_n_d_e_x │ │ │ │ │ -std::uint64_t FactorIndex │ │ │ │ │ -Integer nonlinear factor index type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:103 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_n_s_e_I_n_d_e_x │ │ │ │ │ -ptrdiff_t DenseIndex │ │ │ │ │ -The index type for Eigen objects. │ │ │ │ │ -DDeeffiinniittiioonn types.h:106 │ │ │ │ │ -_g_t_s_a_m_:_:_v_o_i_d___t │ │ │ │ │ -void void_t │ │ │ │ │ -Convenience void_t as we assume C++11, it will not conflict the std one in │ │ │ │ │ -C++17 as this is in gtsam:... │ │ │ │ │ -DDeeffiinniittiioonn types.h:277 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_c_o_n_s_t___s_e_l_e_c_t_o_r │ │ │ │ │ -Helper class that uses templates to select between two types based on whether │ │ │ │ │ -TEST_TYPE is const or n... │ │ │ │ │ -DDeeffiinniittiioonn types.h:115 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ +FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ +fast_pool_allocator instead of the defa... │ │ │ │ │ +DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_T_i_m_i_n_g_O_u_t_l_i_n_e │ │ │ │ │ +Timing Entry, arranged in a tree. │ │ │ │ │ +DDeeffiinniittiioonn timing.h:145 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_T_i_m_i_n_g_O_u_t_l_i_n_e_:_:_t_2__ │ │ │ │ │ +double t2_ │ │ │ │ │ +cache the │ │ │ │ │ +DDeeffiinniittiioonn timing.h:150 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_T_i_m_i_n_g_O_u_t_l_i_n_e_:_:_c_h_i_l_d_r_e_n__ │ │ │ │ │ +ChildMap children_ │ │ │ │ │ +subtrees │ │ │ │ │ +DDeeffiinniittiioonn timing.h:162 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_T_i_m_i_n_g_O_u_t_l_i_n_e_:_:_m_a_x │ │ │ │ │ +double max() const │ │ │ │ │ +max time, in seconds │ │ │ │ │ +DDeeffiinniittiioonn timing.h:183 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_T_i_m_i_n_g_O_u_t_l_i_n_e_:_:_w_a_l_l │ │ │ │ │ +double wall() const │ │ │ │ │ +wall time, in seconds │ │ │ │ │ +DDeeffiinniittiioonn timing.h:181 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_T_i_m_i_n_g_O_u_t_l_i_n_e_:_:_s_e_c_s │ │ │ │ │ +double secs() const │ │ │ │ │ +time taken, in seconds, including children │ │ │ │ │ +DDeeffiinniittiioonn timing.h:179 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_T_i_m_i_n_g_O_u_t_l_i_n_e_:_:_s_e_l_f │ │ │ │ │ +double self() const │ │ │ │ │ +self time only, in seconds │ │ │ │ │ +DDeeffiinniittiioonn timing.h:180 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_T_i_m_i_n_g_O_u_t_l_i_n_e_:_:_p_a_r_e_n_t__ │ │ │ │ │ +boost::weak_ptr< TimingOutline > parent_ │ │ │ │ │ +parent pointer │ │ │ │ │ +DDeeffiinniittiioonn timing.h:160 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_T_i_m_i_n_g_O_u_t_l_i_n_e_:_:_m_i_n │ │ │ │ │ +double min() const │ │ │ │ │ +min time, in seconds │ │ │ │ │ +DDeeffiinniittiioonn timing.h:182 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_T_i_m_i_n_g_O_u_t_l_i_n_e_:_:_m_e_a_n │ │ │ │ │ +double mean() const │ │ │ │ │ +mean self time, in seconds │ │ │ │ │ +DDeeffiinniittiioonn timing.h:184 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_T_i_m_i_n_g_O_u_t_l_i_n_e_:_:_t_i_m_e │ │ │ │ │ +GTSAM_EXPORT size_t time() const │ │ │ │ │ +time taken, including children │ │ │ │ │ +DDeeffiinniittiioonn timing.cpp:65 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_A_u_t_o_T_i_c_T_o_c │ │ │ │ │ +Small class that calls internal::tic at construction, and internol::toc when │ │ │ │ │ +destroyed. │ │ │ │ │ +DDeeffiinniittiioonn timing.h:199 │ │ │ │ │ _g_t_s_a_m_:_:_V_a_l_u_e_W_i_t_h_D_e_f_a_u_l_t │ │ │ │ │ Helper struct that encapsulates a value with a default, this is just used as a │ │ │ │ │ member object so you d... │ │ │ │ │ DDeeffiinniittiioonn types.h:137 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_W_i_t_h_D_e_f_a_u_l_t_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ -T & operator*() │ │ │ │ │ -Operator to access the value. │ │ │ │ │ -DDeeffiinniittiioonn types.h:147 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_W_i_t_h_D_e_f_a_u_l_t_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ -const T & operator*() const │ │ │ │ │ -Operator to access the value. │ │ │ │ │ -DDeeffiinniittiioonn types.h:150 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_W_i_t_h_D_e_f_a_u_l_t_:_:_V_a_l_u_e_W_i_t_h_D_e_f_a_u_l_t │ │ │ │ │ -ValueWithDefault() │ │ │ │ │ -Default constructor, initialize to default value supplied in template argument. │ │ │ │ │ -DDeeffiinniittiioonn types.h:141 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_W_i_t_h_D_e_f_a_u_l_t_:_:_V_a_l_u_e_W_i_t_h_D_e_f_a_u_l_t │ │ │ │ │ -ValueWithDefault(const T &_value) │ │ │ │ │ -Initialize to the given value. │ │ │ │ │ -DDeeffiinniittiioonn types.h:144 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_s_t_O_f_O_n_e_C_o_n_t_a_i_n_e_r │ │ │ │ │ -A helper class that behaves as a container with one element, and works with │ │ │ │ │ -boost::range. │ │ │ │ │ -DDeeffiinniittiioonn types.h:160 │ │ │ │ │ -_g_t_s_a_m_:_:_T_b_b_O_p_e_n_M_P_M_i_x_e_d_S_c_o_p_e │ │ │ │ │ -An object whose scope defines a block where TBB and OpenMP parallelism are │ │ │ │ │ -mixed. │ │ │ │ │ -DDeeffiinniittiioonn types.h:192 │ │ │ │ │ -_g_t_s_a_m_:_:_n_e_e_d_s___e_i_g_e_n___a_l_i_g_n_e_d___a_l_l_o_c_a_t_o_r │ │ │ │ │ -A SFINAE trait to mark classes that need special alignment. │ │ │ │ │ -DDeeffiinniittiioonn types.h:295 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _t_y_p_e_s_._h │ │ │ │ │ + * _t_i_m_i_n_g_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00122.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/DSFVector.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Matrix.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,36 +94,159 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces
│ │ │ │ -
DSFVector.cpp File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
Matrix.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

a faster implementation for DSF, which uses vector rather than btree. │ │ │ │ +

matrix class │ │ │ │ More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +bool gtsam::assert_equal (const Matrix &A, const Matrix &B, double tol=1e-9)
 equals with an tolerance, prints out message if unequal
 
│ │ │ │ +bool gtsam::assert_inequal (const Matrix &A, const Matrix &B, double tol=1e-9)
 inequals with an tolerance, prints out message if within tolerance
 
│ │ │ │ +bool gtsam::assert_equal (const std::list< Matrix > &As, const std::list< Matrix > &Bs, double tol=1e-9)
 equals with an tolerance, prints out message if unequal
 
│ │ │ │ +bool gtsam::linear_independent (const Matrix &A, const Matrix &B, double tol=1e-9)
 check whether the rows of two matrices are linear independent
 
│ │ │ │ +bool gtsam::linear_dependent (const Matrix &A, const Matrix &B, double tol=1e-9)
 check whether the rows of two matrices are linear dependent
 
│ │ │ │ +Vector gtsam::operator^ (const Matrix &A, const Vector &v)
 overload ^ for trans(A)*v We transpose the vectors for speed.
 
│ │ │ │ +const Eigen::IOFormat & gtsam::matlabFormat ()
 
│ │ │ │ +void gtsam::print (const Matrix &A, const std::string &s, std::ostream &stream)
 print without optional string, must specify cout yourself
 
│ │ │ │ +void gtsam::print (const Matrix &A, const std::string &s="")
 print with optional string to cout
 
│ │ │ │ +void gtsam::save (const Matrix &A, const std::string &s, const std::string &filename)
 save a matrix to file, which can be loaded by matlab
 
istream & gtsam::operator>> (std::istream &inputStream, Matrix &destinationMatrix)
 Read a matrix from an input stream, such as a file.
 
│ │ │ │ +Matrix gtsam::diag (const std::vector< Matrix > &Hs)
 Create a matrix with submatrices along its diagonal.
 
│ │ │ │ +Vector gtsam::columnNormSquare (const Matrix &A)
 
pair< Matrix, Matrix > gtsam::qr (const Matrix &A)
 Householder QR factorization, Golub & Van Loan p 224, explicit version
│ │ │ │ +
 
list< boost::tuple< Vector, double, double > > gtsam::weighted_eliminate (Matrix &A, Vector &b, const Vector &sigmas)
 Imperative algorithm for in-place full elimination with weights and constraint handling.
 
void gtsam::householder_ (Matrix &A, size_t k, bool copy_vectors)
 Imperative version of Householder QR factorization, Golub & Van Loan p 224 version with Householder vectors below diagonal, as in GVL.
 
void gtsam::householder (Matrix &A, size_t k)
 Householder tranformation, zeros below diagonal.
 
Vector gtsam::backSubstituteLower (const Matrix &L, const Vector &b, bool unit=false)
 backSubstitute L*x=b
 
Vector gtsam::backSubstituteUpper (const Matrix &U, const Vector &b, bool unit=false)
 backSubstitute U*x=b
 
Vector gtsam::backSubstituteUpper (const Vector &b, const Matrix &U, bool unit=false)
 backSubstitute x'*U=b'
 
Matrix gtsam::stack (size_t nrMatrices,...)
 create a matrix by stacking other matrices Given a set of matrices: A1, A2, A3...
 
│ │ │ │ +Matrix gtsam::stack (const std::vector< Matrix > &blocks)
 
Matrix gtsam::collect (const std::vector< const Matrix * > &matrices, size_t m=0, size_t n=0)
 create a matrix by concatenating Given a set of matrices: A1, A2, A3... If all matrices have the same size, specifying single matrix dimensions will avoid the lookup of dimensions
 
│ │ │ │ +Matrix gtsam::collect (size_t nrMatrices,...)
 
void gtsam::vector_scale_inplace (const Vector &v, Matrix &A, bool inf_mask=false)
 scales a matrix row or column by the values in a vector Arguments (Matrix, Vector) scales the columns, (Vector, Matrix) scales the rows
 
│ │ │ │ +Matrix gtsam::vector_scale (const Vector &v, const Matrix &A, bool inf_mask)
 
│ │ │ │ +Matrix gtsam::vector_scale (const Matrix &A, const Vector &v, bool inf_mask)
 
│ │ │ │ +Matrix gtsam::LLt (const Matrix &A)
 
│ │ │ │ +Matrix gtsam::RtR (const Matrix &A)
 
Matrix gtsam::cholesky_inverse (const Matrix &A)
 Return the inverse of a S.P.D.
 
│ │ │ │ +Matrix gtsam::inverse_square_root (const Matrix &A)
 Use Cholesky to calculate inverse square root of a matrix.
 
void gtsam::svd (const Matrix &A, Matrix &U, Vector &S, Matrix &V)
 SVD computes economy SVD A=U*S*V'.
 
boost::tuple< int, double, Vector > gtsam::DLT (const Matrix &A, double rank_tol=1e-9)
 Direct linear transform algorithm that calls svd to find a vector v that minimizes the algebraic error A*v.
 
Matrix gtsam::expm (const Matrix &A, size_t K=7)
 Numerical exponential map, naive approach, not industrial strength !!!
 
│ │ │ │ +std::string gtsam::formatMatrixIndented (const std::string &label, const Matrix &matrix, bool makeVectorHorizontal)
 
void gtsam::inplace_QR (Matrix &A)
 QR factorization using Eigen's internal block QR algorithm.
 
│ │ │ │

Detailed Description

│ │ │ │ -

a faster implementation for DSF, which uses vector rather than btree.

│ │ │ │ -
Date
Jun 25, 2010
│ │ │ │ -
Author
Kai Ni
│ │ │ │ +

matrix class

│ │ │ │ +
Author
Christian Potthast
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,179 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -DSFVector.cpp File Reference │ │ │ │ │ -a faster implementation for DSF, which uses vector rather than btree. _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Matrix.cpp File Reference │ │ │ │ │ +matrix class _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ + bool  ggttssaamm::::aasssseerrtt__eeqquuaall (const Matrix &A, │ │ │ │ │ + const Matrix &B, double tol=1e-9) │ │ │ │ │ +  equals with an tolerance, prints out │ │ │ │ │ + message if unequal │ │ │ │ │ +  │ │ │ │ │ + bool  ggttssaamm::::aasssseerrtt__iinneeqquuaall (const Matrix &A, │ │ │ │ │ + const Matrix &B, double tol=1e-9) │ │ │ │ │ +  inequals with an tolerance, prints out │ │ │ │ │ + message if within tolerance │ │ │ │ │ +  │ │ │ │ │ + bool  ggttssaamm::::aasssseerrtt__eeqquuaall (const std::list< │ │ │ │ │ + Matrix > &As, const std::list< Matrix > │ │ │ │ │ + &Bs, double tol=1e-9) │ │ │ │ │ +  equals with an tolerance, prints out │ │ │ │ │ + message if unequal │ │ │ │ │ +  │ │ │ │ │ + bool  ggttssaamm::::lliinneeaarr__iinnddeeppeennddeenntt (const Matrix │ │ │ │ │ + &A, const Matrix &B, double tol=1e-9) │ │ │ │ │ +  check whether the rows of two matrices are │ │ │ │ │ + linear independent │ │ │ │ │ +  │ │ │ │ │ + bool  ggttssaamm::::lliinneeaarr__ddeeppeennddeenntt (const Matrix &A, │ │ │ │ │ + const Matrix &B, double tol=1e-9) │ │ │ │ │ +  check whether the rows of two matrices are │ │ │ │ │ + linear dependent │ │ │ │ │ +  │ │ │ │ │ + Vector  ggttssaamm::::ooppeerraattoorr^^ (const Matrix &A, const │ │ │ │ │ + Vector &v) │ │ │ │ │ +  overload ^ for trans(A)*v We transpose the │ │ │ │ │ + vectors for speed. │ │ │ │ │ +  │ │ │ │ │ + const Eigen::IOFormat &  ggttssaamm::::mmaattllaabbFFoorrmmaatt () │ │ │ │ │ +  │ │ │ │ │ + void  ggttssaamm::::pprriinntt (const Matrix &A, const std:: │ │ │ │ │ + string &s, std::ostream &stream) │ │ │ │ │ +  print without optional string, must │ │ │ │ │ + specify cout yourself │ │ │ │ │ +  │ │ │ │ │ + void  ggttssaamm::::pprriinntt (const Matrix &A, const std:: │ │ │ │ │ + string &s="") │ │ │ │ │ +  print with optional string to cout │ │ │ │ │ +  │ │ │ │ │ + void  ggttssaamm::::ssaavvee (const Matrix &A, const std:: │ │ │ │ │ + string &s, const std::string &filename) │ │ │ │ │ +  save a matrix to file, which can be loaded │ │ │ │ │ + by matlab │ │ │ │ │ +  │ │ │ │ │ + istream &  _g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_>_> (std::istream │ │ │ │ │ + &inputStream, Matrix &destinationMatrix) │ │ │ │ │ +  Read a matrix from an input stream, such │ │ │ │ │ + as a file. │ │ │ │ │ +  │ │ │ │ │ + Matrix  ggttssaamm::::ddiiaagg (const std::vector< Matrix > │ │ │ │ │ + &Hs) │ │ │ │ │ +  Create a matrix with submatrices along its │ │ │ │ │ + diagonal. │ │ │ │ │ +  │ │ │ │ │ + Vector  ggttssaamm::::ccoolluummnnNNoorrmmSSqquuaarree (const Matrix &A) │ │ │ │ │ +  │ │ │ │ │ + pair< Matrix, Matrix >  _g_t_s_a_m_:_:_q_r (const Matrix &A) │ │ │ │ │ + Householder QR factorization, Golub & Van │ │ │ │ │ +  Loan p 224, explicit version │ │ │ │ │ + │ │ │ │ │ +  │ │ │ │ │ + list< boost::tuple< Vector, double, _g_t_s_a_m_:_:_w_e_i_g_h_t_e_d___e_l_i_m_i_n_a_t_e (Matrix &A, │ │ │ │ │ + double > >  Vector &b, const Vector &sigmas) │ │ │ │ │ + Imperative algorithm for in-place full │ │ │ │ │ +  elimination with weights and constraint │ │ │ │ │ + handling. │ │ │ │ │ +  │ │ │ │ │ + void  _g_t_s_a_m_:_:_h_o_u_s_e_h_o_l_d_e_r__ (Matrix &A, size_t k, │ │ │ │ │ + bool copy_vectors) │ │ │ │ │ + Imperative version of Householder QR │ │ │ │ │ +  factorization, Golub & Van Loan p 224 │ │ │ │ │ + version with Householder vectors below │ │ │ │ │ + diagonal, as in GVL. │ │ │ │ │ +  │ │ │ │ │ + void  _g_t_s_a_m_:_:_h_o_u_s_e_h_o_l_d_e_r (Matrix &A, size_t k) │ │ │ │ │ +  Householder tranformation, zeros below │ │ │ │ │ + diagonal. │ │ │ │ │ +  │ │ │ │ │ + Vector  _g_t_s_a_m_:_:_b_a_c_k_S_u_b_s_t_i_t_u_t_e_L_o_w_e_r (const Matrix │ │ │ │ │ + &L, const Vector &b, bool unit=false) │ │ │ │ │ +  backSubstitute L*x=b │ │ │ │ │ +  │ │ │ │ │ + Vector  _g_t_s_a_m_:_:_b_a_c_k_S_u_b_s_t_i_t_u_t_e_U_p_p_e_r (const Matrix │ │ │ │ │ + &U, const Vector &b, bool unit=false) │ │ │ │ │ +  backSubstitute U*x=b │ │ │ │ │ +  │ │ │ │ │ + Vector  _g_t_s_a_m_:_:_b_a_c_k_S_u_b_s_t_i_t_u_t_e_U_p_p_e_r (const Vector │ │ │ │ │ + &b, const Matrix &U, bool unit=false) │ │ │ │ │ +  backSubstitute x'*U=b' │ │ │ │ │ +  │ │ │ │ │ + Matrix  _g_t_s_a_m_:_:_s_t_a_c_k (size_t nrMatrices,...) │ │ │ │ │ +  create a matrix by stacking other matrices │ │ │ │ │ + Given a set of matrices: A1, A2, A3... │ │ │ │ │ +  │ │ │ │ │ + Matrix  ggttssaamm::::ssttaacckk (const std::vector< Matrix > │ │ │ │ │ + &blocks) │ │ │ │ │ +  │ │ │ │ │ + Matrix  _g_t_s_a_m_:_:_c_o_l_l_e_c_t (const std::vector< const │ │ │ │ │ + Matrix * > &matrices, size_t m=0, size_t │ │ │ │ │ + n=0) │ │ │ │ │ + create a matrix by concatenating Given a │ │ │ │ │ + set of matrices: A1, A2, A3... If all │ │ │ │ │ +  matrices have the same size, specifying │ │ │ │ │ + single matrix dimensions will avoid the │ │ │ │ │ + lookup of dimensions │ │ │ │ │ +  │ │ │ │ │ + Matrix  ggttssaamm::::ccoolllleecctt (size_t nrMatrices,...) │ │ │ │ │ +  │ │ │ │ │ + void  _g_t_s_a_m_:_:_v_e_c_t_o_r___s_c_a_l_e___i_n_p_l_a_c_e (const Vector │ │ │ │ │ + &v, Matrix &A, bool inf_mask=false) │ │ │ │ │ + scales a matrix row or column by the │ │ │ │ │ +  values in a vector Arguments (Matrix, │ │ │ │ │ + Vector) scales the columns, (Vector, │ │ │ │ │ + Matrix) scales the rows │ │ │ │ │ +  │ │ │ │ │ + Matrix  ggttssaamm::::vveeccttoorr__ssccaallee (const Vector &v, │ │ │ │ │ + const Matrix &A, bool inf_mask) │ │ │ │ │ +  │ │ │ │ │ + Matrix  ggttssaamm::::vveeccttoorr__ssccaallee (const Matrix &A, │ │ │ │ │ + const Vector &v, bool inf_mask) │ │ │ │ │ +  │ │ │ │ │ + Matrix  ggttssaamm::::LLLLtt (const Matrix &A) │ │ │ │ │ +  │ │ │ │ │ + Matrix  ggttssaamm::::RRttRR (const Matrix &A) │ │ │ │ │ +  │ │ │ │ │ + Matrix  _g_t_s_a_m_:_:_c_h_o_l_e_s_k_y___i_n_v_e_r_s_e (const Matrix &A) │ │ │ │ │ +  Return the inverse of a S.P.D. │ │ │ │ │ +  │ │ │ │ │ + Matrix  ggttssaamm::::iinnvveerrssee__ssqquuaarree__rroooott (const Matrix │ │ │ │ │ + &A) │ │ │ │ │ +  Use Cholesky to calculate inverse square │ │ │ │ │ + root of a matrix. │ │ │ │ │ +  │ │ │ │ │ + void  _g_t_s_a_m_:_:_s_v_d (const Matrix &A, Matrix &U, │ │ │ │ │ + Vector &S, Matrix &V) │ │ │ │ │ +  SVD computes economy SVD A=U*S*V'. │ │ │ │ │ +  │ │ │ │ │ +boost::tuple< int, double, Vector >  _g_t_s_a_m_:_:_D_L_T (const Matrix &A, double │ │ │ │ │ + rank_tol=1e-9) │ │ │ │ │ + Direct linear transform algorithm that │ │ │ │ │ +  calls svd to find a vector v that │ │ │ │ │ + minimizes the algebraic error A*v. │ │ │ │ │ +  │ │ │ │ │ + Matrix  _g_t_s_a_m_:_:_e_x_p_m (const Matrix &A, size_t K=7) │ │ │ │ │ +  Numerical exponential map, naive approach, │ │ │ │ │ + not industrial strength !!! │ │ │ │ │ +  │ │ │ │ │ + std::string  ggttssaamm::::ffoorrmmaattMMaattrriixxIInnddeenntteedd (const std:: │ │ │ │ │ + string &label, const Matrix &matrix, bool │ │ │ │ │ + makeVectorHorizontal) │ │ │ │ │ +  │ │ │ │ │ + void  _g_t_s_a_m_:_:_i_n_p_l_a_c_e___Q_R (Matrix &A) │ │ │ │ │ +  QR factorization using Eigen's internal │ │ │ │ │ + block QR algorithm. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -a faster implementation for DSF, which uses vector rather than btree. │ │ │ │ │ - Date │ │ │ │ │ - Jun 25, 2010 │ │ │ │ │ +matrix class │ │ │ │ │ Author │ │ │ │ │ - Kai Ni │ │ │ │ │ + Christian Potthast │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _D_S_F_V_e_c_t_o_r_._c_p_p │ │ │ │ │ + * _M_a_t_r_i_x_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00125.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Vector.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/types.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,112 +94,208 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces | │ │ │ │ +Macros | │ │ │ │ +Typedefs | │ │ │ │ Functions
│ │ │ │ -
Vector.cpp File Reference
│ │ │ │ +
types.h File Reference
Base
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

typedef and functions to augment Eigen's Vectors │ │ │ │ +

Typedefs for easier changing of types. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

struct  gtsam::const_selector< TEST_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST >
 Helper class that uses templates to select between two types based on whether TEST_TYPE is const or not. More...
 
struct  gtsam::const_selector< BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST >
 Specialization for the non-const version. More...
 
struct  gtsam::const_selector< const BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST >
 Specialization for the const version. More...
 
struct  gtsam::ValueWithDefault< T, defaultValue >
 Helper struct that encapsulates a value with a default, this is just used as a member object so you don't have to specify defaults in the class constructor. More...
 
class  gtsam::ListOfOneContainer< T >
 A helper class that behaves as a container with one element, and works with boost::range. More...
 
class  gtsam::TbbOpenMPMixedScope
 An object whose scope defines a block where TBB and OpenMP parallelism are mixed. More...
 
struct  gtsam::needs_eigen_aligned_allocator< typename, typename >
 A SFINAE trait to mark classes that need special alignment. More...
 
struct  gtsam::needs_eigen_aligned_allocator< T, void_t< typename T::_eigen_aligned_allocator_trait > >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Macros

│ │ │ │ +#define GTSAM_DEPRECATED
 
│ │ │ │ +#define DO_PRAGMA(x)   _Pragma (#x)
 
│ │ │ │ +#define CLANG_DIAGNOSTIC_PUSH_IGNORE(diag)
 
│ │ │ │ +#define GCC_DIAGNOSTIC_PUSH_IGNORE(diag)
 
│ │ │ │ +#define MSVC_DIAGNOSTIC_PUSH_IGNORE(code)
 
│ │ │ │ +#define DIAGNOSTIC_POP()
 
#define assert_throw(CONDITION, EXCEPTION)
 An assertion that throws an exception if NDEBUG is not defined and evaluates to an empty statement otherwise.
 
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
 This marks a GTSAM object to require alignment.
 
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
 This marks a GTSAM object to require alignment.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Typedefs

│ │ │ │ +typedef std::uint64_t gtsam::Key
 Integer nonlinear key type.
 
│ │ │ │ +typedef std::uint64_t gtsam::FactorIndex
 Integer nonlinear factor index type.
 
│ │ │ │ +typedef ptrdiff_t gtsam::DenseIndex
 The index type for Eigen objects.
 
│ │ │ │ +template<typename ... >
using gtsam::void_t = void
 Convenience void_t as we assume C++11, it will not conflict the std one in C++17 as this is in gtsam::
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

bool gtsam::fpEqual (double a, double b, double tol, bool check_relative_also=true)
 Ensure we are not including a different version of Eigen in user code than while compiling gtsam, since it can lead to hard-to-understand runtime crashes.
 
│ │ │ │ -void gtsam::print (const Vector &v, const std::string &s, std::ostream &stream)
 print without optional string, must specify cout yourself
 
│ │ │ │ -void gtsam::print (const Vector &v, const std::string &s="")
 print with optional string to cout
 
│ │ │ │ -void gtsam::save (const Vector &A, const std::string &s, const std::string &filename)
 save a vector to file, which can be loaded by matlab
 
│ │ │ │ -bool gtsam::operator== (const Vector &vec1, const Vector &vec2)
 operator==()
 
│ │ │ │ -bool gtsam::greaterThanOrEqual (const Vector &v1, const Vector &v2)
 Greater than or equal to operation returns true if all elements in v1 are greater than corresponding elements in v2.
 
│ │ │ │ -bool gtsam::equal_with_abs_tol (const Vector &vec1, const Vector &vec2, double tol=1e-9)
 VecA == VecB up to tolerance.
 
│ │ │ │ -bool gtsam::equal_with_abs_tol (const SubVector &vec1, const SubVector &vec2, double tol)
 
bool gtsam::assert_equal (const Vector &vec1, const Vector &vec2, double tol=1e-9)
 Same, prints if error.
 
bool gtsam::assert_inequal (const Vector &vec1, const Vector &vec2, double tol=1e-9)
 Not the same, prints if error.
 
bool gtsam::assert_equal (const SubVector &vec1, const SubVector &vec2, double tol=1e-9)
 Same, prints if error.
 
│ │ │ │ -bool gtsam::assert_equal (const ConstSubVector &expected, const ConstSubVector &actual, double tol)
 
bool gtsam::linear_dependent (const Vector &vec1, const Vector &vec2, double tol=1e-9)
 check whether two vectors are linearly dependent
 
Vector gtsam::ediv_ (const Vector &a, const Vector &b)
 elementwise division, but 0/0 = 0, not inf
 
│ │ │ │ -double gtsam::houseInPlace (Vector &x)
 beta = house(x) computes the HouseHolder vector in place
 
pair< double, Vector > gtsam::house (const Vector &x)
 house(x,j) computes HouseHolder vector v and scaling factor beta from x, such that the corresponding Householder reflection zeroes out all but x.
 
│ │ │ │ -double gtsam::weightedPseudoinverse (const Vector &a, const Vector &weights, Vector &pseudo)
 
pair< Vector, double > gtsam::weightedPseudoinverse (const Vector &v, const Vector &weights)
 Weighted Householder solution vector, a.k.a., the pseudoinverse of the column NOTE: if any sigmas are zero (indicating a constraint) the pseudoinverse will be a selection vector, and the variance will be zero.
 
│ │ │ │ -Vector gtsam::concatVectors (const std::list< Vector > &vs)
 concatenate Vectors
 
│ │ │ │ -Vector gtsam::concatVectors (size_t nrVectors,...)
 concatenate Vectors
 
std::string gtsam::demangle (const char *name)
 Pretty print Value type name.
 
│ │ │ │ + gtsam::BOOST_CONCEPT_ASSERT ((boost::RandomAccessRangeConcept< ListOfOneContainer< int > >))
 
│ │ │ │ +template<typename T >
ListOfOneContainer< T > gtsam::ListOfOne (const T &element)
 Factory function for ListOfOneContainer to enable ListOfOne(e) syntax.
 
│ │ │ │

Detailed Description

│ │ │ │ -

typedef and functions to augment Eigen's Vectors

│ │ │ │ -
Author
Kai Ni
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ -
│ │ │ │ -Varun Agrawal
│ │ │ │ -
│ │ │ │ +

Typedefs for easier changing of types.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Aug 21, 2010
│ │ │ │ +

Macro Definition Documentation

│ │ │ │ + │ │ │ │ +

◆ assert_throw

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
#define assert_throw( CONDITION,
 EXCEPTION 
)
│ │ │ │ +
│ │ │ │ +Value:
if (!(CONDITION)) { \
│ │ │ │ +
throw (EXCEPTION); \
│ │ │ │ +
}
│ │ │ │ +
│ │ │ │ +

An assertion that throws an exception if NDEBUG is not defined and evaluates to an empty statement otherwise.

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ +

◆ GTSAM_MAKE_ALIGNED_OPERATOR_NEW

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
│ │ │ │ +Value:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW \
│ │ │ │ +
using _eigen_aligned_allocator_trait = void;
│ │ │ │ +
│ │ │ │ +

This marks a GTSAM object to require alignment.

│ │ │ │ +

With this macro an object will automatically be allocated in aligned memory when one uses gtsam::make_shared. It reduces future misalignment problems that is hard to debug. See https://eigen.tuxfamily.org/dox/group__DenseMatrixManipulation__Alignement.html for detailed explanation.

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ +

◆ GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF( NeedsToAlign)
│ │ │ │ +
│ │ │ │ +Value:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \
│ │ │ │ +
using _eigen_aligned_allocator_trait = void;
│ │ │ │ +
│ │ │ │ +

This marks a GTSAM object to require alignment.

│ │ │ │ +

With this macro an object will automatically be allocated in aligned memory when one uses gtsam::make_shared. It reduces future misalignment problems that is hard to debug. See https://eigen.tuxfamily.org/dox/group__DenseMatrixManipulation__Alignement.html for detailed explanation.

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,105 +1,138 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Vector.cpp File Reference │ │ │ │ │ -typedef and functions to augment Eigen's Vectors _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +types.h File Reference │ │ │ │ │ +_B_a_s_e │ │ │ │ │ +Typedefs for easier changing of types. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +struct   _g_t_s_a_m_:_:_c_o_n_s_t___s_e_l_e_c_t_o_r_<_ _T_E_S_T___T_Y_P_E_,_ _B_A_S_I_C___T_Y_P_E_,_ _A_S___N_O_N___C_O_N_S_T_,_ _A_S___C_O_N_S_T_ _> │ │ │ │ │ +  Helper class that uses templates to select between two types based on │ │ │ │ │ + whether TEST_TYPE is const or not. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_c_o_n_s_t___s_e_l_e_c_t_o_r_<_ _B_A_S_I_C___T_Y_P_E_,_ _B_A_S_I_C___T_Y_P_E_,_ _A_S___N_O_N___C_O_N_S_T_,_ _A_S___C_O_N_S_T │ │ │ │ │ + _> │ │ │ │ │ +  Specialization for the non-const version. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_c_o_n_s_t___s_e_l_e_c_t_o_r_<_ _c_o_n_s_t_ _B_A_S_I_C___T_Y_P_E_,_ _B_A_S_I_C___T_Y_P_E_,_ _A_S___N_O_N___C_O_N_S_T_, │ │ │ │ │ + _A_S___C_O_N_S_T_ _> │ │ │ │ │ +  Specialization for the const version. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_V_a_l_u_e_W_i_t_h_D_e_f_a_u_l_t_<_ _T_,_ _d_e_f_a_u_l_t_V_a_l_u_e_ _> │ │ │ │ │ + Helper struct that encapsulates a value with a default, this is just │ │ │ │ │ +  used as a member object so you don't have to specify defaults in the │ │ │ │ │ + class constructor. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_L_i_s_t_O_f_O_n_e_C_o_n_t_a_i_n_e_r_<_ _T_ _> │ │ │ │ │ +  A helper class that behaves as a container with one element, and works │ │ │ │ │ + with boost::range. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_T_b_b_O_p_e_n_M_P_M_i_x_e_d_S_c_o_p_e │ │ │ │ │ +  An object whose scope defines a block where TBB and OpenMP parallelism │ │ │ │ │ + are mixed. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_n_e_e_d_s___e_i_g_e_n___a_l_i_g_n_e_d___a_l_l_o_c_a_t_o_r_<_ _t_y_p_e_n_a_m_e_,_ _t_y_p_e_n_a_m_e_ _> │ │ │ │ │ +  A SFINAE trait to mark classes that need special alignment. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_n_e_e_d_s___e_i_g_e_n___a_l_i_g_n_e_d___a_l_l_o_c_a_t_o_r_<_ _T_,_ _v_o_i_d___t_<_ _t_y_p_e_n_a_m_e_ _T_:_: │ │ │ │ │ + ___e_i_g_e_n___a_l_i_g_n_e_d___a_l_l_o_c_a_t_o_r___t_r_a_i_t_ _>_ _> │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ - bool  _g_t_s_a_m_:_:_f_p_E_q_u_a_l (double a, double b, double tol, bool │ │ │ │ │ - check_relative_also=true) │ │ │ │ │ - Ensure we are not including a different version of │ │ │ │ │ -  Eigen in user code than while compiling gtsam, since it │ │ │ │ │ - can lead to hard-to-understand runtime crashes. │ │ │ │ │ -  │ │ │ │ │ - void  ggttssaamm::::pprriinntt (const Vector &v, const std::string &s, │ │ │ │ │ - std::ostream &stream) │ │ │ │ │ -  print without optional string, must specify cout │ │ │ │ │ - yourself │ │ │ │ │ -  │ │ │ │ │ - void  ggttssaamm::::pprriinntt (const Vector &v, const std::string &s="") │ │ │ │ │ -  print with optional string to cout │ │ │ │ │ -  │ │ │ │ │ - void  ggttssaamm::::ssaavvee (const Vector &A, const std::string &s, │ │ │ │ │ - const std::string &filename) │ │ │ │ │ -  save a vector to file, which can be loaded by matlab │ │ │ │ │ -  │ │ │ │ │ - bool  ggttssaamm::::ooppeerraattoorr==== (const Vector &vec1, const Vector │ │ │ │ │ - &vec2) │ │ │ │ │ -  _o_p_e_r_a_t_o_r_=_=_(_) │ │ │ │ │ -  │ │ │ │ │ - bool  ggttssaamm::::ggrreeaatteerrTThhaannOOrrEEqquuaall (const Vector &v1, const │ │ │ │ │ - Vector &v2) │ │ │ │ │ - Greater than or equal to operation returns true if all │ │ │ │ │ -  elements in v1 are greater than corresponding elements │ │ │ │ │ - in v2. │ │ │ │ │ -  │ │ │ │ │ - bool  ggttssaamm::::eeqquuaall__wwiitthh__aabbss__ttooll (const Vector &vec1, const │ │ │ │ │ - Vector &vec2, double tol=1e-9) │ │ │ │ │ -  VecA == VecB up to tolerance. │ │ │ │ │ -  │ │ │ │ │ - bool  ggttssaamm::::eeqquuaall__wwiitthh__aabbss__ttooll (const SubVector &vec1, const │ │ │ │ │ - SubVector &vec2, double tol) │ │ │ │ │ -  │ │ │ │ │ - bool  _g_t_s_a_m_:_:_a_s_s_e_r_t___e_q_u_a_l (const Vector &vec1, const Vector │ │ │ │ │ - &vec2, double tol=1e-9) │ │ │ │ │ -  Same, prints if error. │ │ │ │ │ -  │ │ │ │ │ - bool  _g_t_s_a_m_:_:_a_s_s_e_r_t___i_n_e_q_u_a_l (const Vector &vec1, const Vector │ │ │ │ │ - &vec2, double tol=1e-9) │ │ │ │ │ -  Not the same, prints if error. │ │ │ │ │ -  │ │ │ │ │ - bool  _g_t_s_a_m_:_:_a_s_s_e_r_t___e_q_u_a_l (const SubVector &vec1, const │ │ │ │ │ - SubVector &vec2, double tol=1e-9) │ │ │ │ │ -  Same, prints if error. │ │ │ │ │ -  │ │ │ │ │ - bool  ggttssaamm::::aasssseerrtt__eeqquuaall (const ConstSubVector &expected, │ │ │ │ │ - const ConstSubVector &actual, double tol) │ │ │ │ │ -  │ │ │ │ │ - bool  _g_t_s_a_m_:_:_l_i_n_e_a_r___d_e_p_e_n_d_e_n_t (const Vector &vec1, const │ │ │ │ │ - Vector &vec2, double tol=1e-9) │ │ │ │ │ -  check whether two vectors are linearly dependent │ │ │ │ │ -  │ │ │ │ │ - Vector  _g_t_s_a_m_:_:_e_d_i_v__ (const Vector &a, const Vector &b) │ │ │ │ │ -  elementwise division, but 0/0 = 0, not inf │ │ │ │ │ -  │ │ │ │ │ - double  ggttssaamm::::hhoouusseeIInnPPllaaccee (Vector &x) │ │ │ │ │ -  beta = house(x) computes the HouseHolder vector in │ │ │ │ │ - place │ │ │ │ │ -  │ │ │ │ │ -pair< double, Vector >  _g_t_s_a_m_:_:_h_o_u_s_e (const Vector &x) │ │ │ │ │ - house(x,j) computes HouseHolder vector v and scaling │ │ │ │ │ -  factor beta from x, such that the corresponding │ │ │ │ │ - Householder reflection zeroes out all but x. │ │ │ │ │ -  │ │ │ │ │ - double  ggttssaamm::::wweeiigghhtteeddPPsseeuuddooiinnvveerrssee (const Vector &a, const │ │ │ │ │ - Vector &weights, Vector &pseudo) │ │ │ │ │ -  │ │ │ │ │ -pair< Vector, double >  _g_t_s_a_m_:_:_w_e_i_g_h_t_e_d_P_s_e_u_d_o_i_n_v_e_r_s_e (const Vector &v, const │ │ │ │ │ - Vector &weights) │ │ │ │ │ - Weighted Householder solution vector, a.k.a., the │ │ │ │ │ -  pseudoinverse of the column NOTE: if any sigmas are │ │ │ │ │ - zero (indicating a constraint) the pseudoinverse will │ │ │ │ │ - be a selection vector, and the variance will be zero. │ │ │ │ │ +MMaaccrrooss │ │ │ │ │ +#define  GGTTSSAAMM__DDEEPPRREECCAATTEEDD │ │ │ │ │ +  │ │ │ │ │ +#define  DDOO__PPRRAAGGMMAA(x)   _Pragma (#x) │ │ │ │ │ +  │ │ │ │ │ +#define  CCLLAANNGG__DDIIAAGGNNOOSSTTIICC__PPUUSSHH__IIGGNNOORREE(diag) │ │ │ │ │ +  │ │ │ │ │ +#define  GGCCCC__DDIIAAGGNNOOSSTTIICC__PPUUSSHH__IIGGNNOORREE(diag) │ │ │ │ │ +  │ │ │ │ │ +#define  MMSSVVCC__DDIIAAGGNNOOSSTTIICC__PPUUSSHH__IIGGNNOORREE(code) │ │ │ │ │ +  │ │ │ │ │ +#define  DDIIAAGGNNOOSSTTIICC__PPOOPP() │ │ │ │ │   │ │ │ │ │ - Vector  ggttssaamm::::ccoonnccaattVVeeccttoorrss (const std::list< Vector > &vs) │ │ │ │ │ -  concatenate Vectors │ │ │ │ │ +#define  _a_s_s_e_r_t___t_h_r_o_w(CONDITION, EXCEPTION) │ │ │ │ │ +  An assertion that throws an exception if NDEBUG is not defined and │ │ │ │ │ + evaluates to an empty statement otherwise. │ │ │ │ │ +  │ │ │ │ │ +#define  _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +  This marks a GTSAM object to require alignment. │ │ │ │ │ +  │ │ │ │ │ +#define  _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W___I_F(NeedsToAlign) │ │ │ │ │ +  This marks a GTSAM object to require alignment. │ │ │ │ │ +  │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +typedef std::uint64_t  ggttssaamm::::KKeeyy │ │ │ │ │ +  Integer nonlinear key type. │ │ │ │ │ +  │ │ │ │ │ +typedef std::uint64_t  ggttssaamm::::FFaaccttoorrIInnddeexx │ │ │ │ │ +  Integer nonlinear factor index type. │ │ │ │ │ +  │ │ │ │ │ + typedef ptrdiff_t  ggttssaamm::::DDeennsseeIInnddeexx │ │ │ │ │ +  The index type for Eigen objects. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + using  ggttssaamm::::vvooiidd__tt = void │ │ │ │ │ +  Convenience void_t as we assume C++11, it will not │ │ │ │ │ + conflict the std one in C++17 as this is in gtsam:: │ │ │ │ │ +  │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ + std::string  _g_t_s_a_m_:_:_d_e_m_a_n_g_l_e (const char *name) │ │ │ │ │ +  Pretty print _V_a_l_u_e type name. │ │ │ │ │   │ │ │ │ │ - Vector  ggttssaamm::::ccoonnccaattVVeeccttoorrss (size_t nrVectors,...) │ │ │ │ │ -  concatenate Vectors │ │ │ │ │ +   ggttssaamm::::BBOOOOSSTT__CCOONNCCEEPPTT__AASSSSEERRTT ((boost:: │ │ │ │ │ + RandomAccessRangeConcept< _L_i_s_t_O_f_O_n_e_C_o_n_t_a_i_n_e_r< int > │ │ │ │ │ + >)) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +_L_i_s_t_O_f_O_n_e_C_o_n_t_a_i_n_e_r< T >  ggttssaamm::::LLiissttOOffOOnnee (const T &element) │ │ │ │ │ +  Factory function for _L_i_s_t_O_f_O_n_e_C_o_n_t_a_i_n_e_r to enable │ │ │ │ │ + ListOfOne(e) syntax. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -typedef and functions to augment Eigen's Vectors │ │ │ │ │ +Typedefs for easier changing of types. │ │ │ │ │ Author │ │ │ │ │ - Kai Ni │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Aug 21, 2010 │ │ │ │ │ +********** MMaaccrroo DDeeffiinniittiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ +********** _?◆_? aasssseerrtt__tthhrrooww ********** │ │ │ │ │ +#define assert_throw (   CONDITION, │ │ │ │ │ +   EXCEPTION  │ │ │ │ │ + ) │ │ │ │ │ +VVaalluuee:: │ │ │ │ │ +if (!(CONDITION)) { \ │ │ │ │ │ +throw (EXCEPTION); \ │ │ │ │ │ +} │ │ │ │ │ +An assertion that throws an exception if NDEBUG is not defined and evaluates to │ │ │ │ │ +an empty statement otherwise. │ │ │ │ │ +********** _?◆_? GGTTSSAAMM__MMAAKKEE__AALLIIGGNNEEDD__OOPPEERRAATTOORR__NNEEWW ********** │ │ │ │ │ +#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ +VVaalluuee:: │ │ │ │ │ +EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ │ │ │ │ │ +using _eigen_aligned_allocator_trait = void; │ │ │ │ │ +This marks a GTSAM object to require alignment. │ │ │ │ │ +With this macro an object will automatically be allocated in aligned memory │ │ │ │ │ +when one uses _g_t_s_a_m_:_:_m_a_k_e___s_h_a_r_e_d. It reduces future misalignment problems that │ │ │ │ │ +is hard to debug. See _h_t_t_p_s_:_/_/_e_i_g_e_n_._t_u_x_f_a_m_i_l_y_._o_r_g_/_d_o_x_/ │ │ │ │ │ +_g_r_o_u_p_____D_e_n_s_e_M_a_t_r_i_x_M_a_n_i_p_u_l_a_t_i_o_n_____A_l_i_g_n_e_m_e_n_t_._h_t_m_l for detailed explanation. │ │ │ │ │ +********** _?◆_? GGTTSSAAMM__MMAAKKEE__AALLIIGGNNEEDD__OOPPEERRAATTOORR__NNEEWW__IIFF ********** │ │ │ │ │ +#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF (   NeedsToAlign ) │ │ │ │ │ +VVaalluuee:: │ │ │ │ │ +EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \ │ │ │ │ │ +using _eigen_aligned_allocator_trait = void; │ │ │ │ │ +This marks a GTSAM object to require alignment. │ │ │ │ │ +With this macro an object will automatically be allocated in aligned memory │ │ │ │ │ +when one uses _g_t_s_a_m_:_:_m_a_k_e___s_h_a_r_e_d. It reduces future misalignment problems that │ │ │ │ │ +is hard to debug. See _h_t_t_p_s_:_/_/_e_i_g_e_n_._t_u_x_f_a_m_i_l_y_._o_r_g_/_d_o_x_/ │ │ │ │ │ +_g_r_o_u_p_____D_e_n_s_e_M_a_t_r_i_x_M_a_n_i_p_u_l_a_t_i_o_n_____A_l_i_g_n_e_m_e_n_t_._h_t_m_l for detailed explanation. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _V_e_c_t_o_r_._c_p_p │ │ │ │ │ + * _t_y_p_e_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00125.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,19 +1,19 @@ │ │ │ │ │ var a00125 = [ │ │ │ │ │ - ["assert_equal", "a00125.html#ac3cf8e8e70cd696d9c789c31bc1e89a7", null], │ │ │ │ │ - ["assert_equal", "a00125.html#a9f3ee662d25ffb8c04b4e35c4b02e90b", null], │ │ │ │ │ - ["assert_inequal", "a00125.html#a2ca3db47b15350977c1f03c5560ab332", null], │ │ │ │ │ - ["concatVectors", "a00125.html#ae1c88acfa9575bcea3f2e738cb637a85", null], │ │ │ │ │ - ["concatVectors", "a00125.html#a09610ec69a8b3f3a00399bb378eda9cf", null], │ │ │ │ │ - ["ediv_", "a00125.html#a14844c96706dc362917e0d5e76d65fbe", null], │ │ │ │ │ - ["equal_with_abs_tol", "a00125.html#a8bdced1844ffd6a53af0396d82eaa6da", null], │ │ │ │ │ - ["fpEqual", "a00125.html#ae19c359190a379cba44bf818a2293f7c", null], │ │ │ │ │ - ["greaterThanOrEqual", "a00125.html#a368ee40bd9c0124d572d2e49bcb077bf", null], │ │ │ │ │ - ["house", "a00125.html#afb77eefc25dea0fb3e0fcc8f246b617a", null], │ │ │ │ │ - ["houseInPlace", "a00125.html#ac9c0c05b0198ec0fb6e4115edb755910", null], │ │ │ │ │ - ["linear_dependent", "a00125.html#a2a0cfd7908b06491df49b6a9c9186775", null], │ │ │ │ │ - ["operator==", "a00125.html#a64988014ab746343803620dc42513646", null], │ │ │ │ │ - ["print", "a00125.html#a760cc36e9009b23cf564c436294b3504", null], │ │ │ │ │ - ["print", "a00125.html#a5e5d8018a0f44a6a299406aeda27d44b", null], │ │ │ │ │ - ["save", "a00125.html#a09f2bbdb9f9d633542362dbe8d79f9ab", null], │ │ │ │ │ - ["weightedPseudoinverse", "a00125.html#aca1c56d11a05464a7a5458dc32ccc777", null] │ │ │ │ │ + ["gtsam::const_selector< TEST_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST >", "a02536.html", null], │ │ │ │ │ + ["gtsam::const_selector< BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST >", "a02540.html", null], │ │ │ │ │ + ["gtsam::const_selector< const BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST >", "a02544.html", null], │ │ │ │ │ + ["gtsam::ValueWithDefault< T, defaultValue >", "a02548.html", "a02548"], │ │ │ │ │ + ["gtsam::ListOfOneContainer< T >", "a02552.html", null], │ │ │ │ │ + ["gtsam::TbbOpenMPMixedScope", "a02556.html", null], │ │ │ │ │ + ["gtsam::needs_eigen_aligned_allocator< typename, typename >", "a02560.html", null], │ │ │ │ │ + ["gtsam::needs_eigen_aligned_allocator< T, void_t< typename T::_eigen_aligned_allocator_trait > >", "a02564.html", null], │ │ │ │ │ + ["assert_throw", "a00125.html#a30d3af2da1ea940197addce4f3c223c8", null], │ │ │ │ │ + ["GTSAM_MAKE_ALIGNED_OPERATOR_NEW", "a00125.html#aa123dd61687e08ae328154a1890b7b86", null], │ │ │ │ │ + ["GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF", "a00125.html#a352ab46cee3a766e30b049690802fb0a", null], │ │ │ │ │ + ["DenseIndex", "a00125.html#a21006f326f57cdf7d3f35fec40f43e40", null], │ │ │ │ │ + ["FactorIndex", "a00125.html#a19c53f2faabb0f4b4b78ce2f7168cc14", null], │ │ │ │ │ + ["Key", "a00125.html#adad029f5f6ffce610428b5fe768b0df2", null], │ │ │ │ │ + ["void_t", "a00125.html#a6525d741529c7a2cd91fa913431f5fb7", null], │ │ │ │ │ + ["demangle", "a00125.html#a0b97fc96e5f69236e81489c66d5b92ba", null], │ │ │ │ │ + ["ListOfOne", "a00125.html#a045b43784acac77531813c4ab6c4d281", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00128_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/ConcurrentMap.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Lie.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,140 +98,382 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ConcurrentMap.h
│ │ │ │ +
Lie.h
│ │ │ │
│ │ │ │
│ │ │ │ -
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/global_includes.h>
│ │ │ │ -
22
│ │ │ │ -
23// Change class depending on whether we are using TBB
│ │ │ │ -
24#ifdef GTSAM_USE_TBB
│ │ │ │ +
24#pragma once
│ │ │ │
25
│ │ │ │ -
26// Include TBB header
│ │ │ │ -
27# include <tbb/concurrent_unordered_map.h>
│ │ │ │ -
28# undef min // TBB seems to include Windows.h which defines these macros that cause problems
│ │ │ │ -
29# undef max
│ │ │ │ -
30# undef ERROR
│ │ │ │ -
31
│ │ │ │ -
32#include <functional> // std::hash()
│ │ │ │ -
33
│ │ │ │ -
34// Use TBB concurrent_unordered_map for ConcurrentMap
│ │ │ │ -
35template <typename KEY, typename VALUE>
│ │ │ │ -
36using ConcurrentMapBase = tbb::concurrent_unordered_map<
│ │ │ │ -
37 KEY,
│ │ │ │ -
38 VALUE,
│ │ │ │ -
39 std::hash<KEY>
│ │ │ │ -
40 >;
│ │ │ │ -
41
│ │ │ │ -
42#else
│ │ │ │ +
26#include <gtsam/base/Manifold.h>
│ │ │ │ +
27#include <gtsam/base/Group.h>
│ │ │ │ +
28
│ │ │ │ +
29namespace gtsam {
│ │ │ │ +
30
│ │ │ │ +
36template <class Class, int N>
│ │ │ │ +
│ │ │ │ +
37struct LieGroup {
│ │ │ │ +
38
│ │ │ │ +
39 enum { dimension = N };
│ │ │ │ +
40 typedef OptionalJacobian<N, N> ChartJacobian;
│ │ │ │ +
41 typedef Eigen::Matrix<double, N, N> Jacobian;
│ │ │ │ +
42 typedef Eigen::Matrix<double, N, 1> TangentVector;
│ │ │ │
43
│ │ │ │ -
44// If we're not using TBB, use a FastMap for ConcurrentMap
│ │ │ │ -
45#include <gtsam/base/FastMap.h>
│ │ │ │ -
46template <typename KEY, typename VALUE>
│ │ │ │ -
47using ConcurrentMapBase = gtsam::FastMap<KEY, VALUE>;
│ │ │ │ -
48
│ │ │ │ -
49#endif
│ │ │ │ -
50
│ │ │ │ -
51#include <boost/serialization/nvp.hpp>
│ │ │ │ -
52#include <boost/serialization/split_member.hpp>
│ │ │ │ -
53#include <boost/static_assert.hpp>
│ │ │ │ -
54
│ │ │ │ -
55#include <gtsam/base/FastVector.h>
│ │ │ │ -
56
│ │ │ │ -
57namespace gtsam {
│ │ │ │ -
58
│ │ │ │ -
67template<typename KEY, typename VALUE>
│ │ │ │ -
│ │ │ │ -
68class ConcurrentMap : public ConcurrentMapBase<KEY,VALUE> {
│ │ │ │ -
69
│ │ │ │ -
70public:
│ │ │ │ -
71
│ │ │ │ -
72 typedef ConcurrentMapBase<KEY,VALUE> Base;
│ │ │ │ -
73
│ │ │ │ -
75 ConcurrentMap() {}
│ │ │ │ -
76
│ │ │ │ -
78 template<typename INPUTITERATOR>
│ │ │ │ -
79 ConcurrentMap(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {}
│ │ │ │ -
80
│ │ │ │ -
82 ConcurrentMap(const ConcurrentMap<KEY,VALUE>& x) : Base(x) {}
│ │ │ │ -
83
│ │ │ │ -
85 ConcurrentMap(const Base& x) : Base(x) {}
│ │ │ │ -
86
│ │ │ │ -
88 bool exists(const KEY& e) const { return this->count(e); }
│ │ │ │ -
89
│ │ │ │ -
90#ifndef GTSAM_USE_TBB
│ │ │ │ -
91 // If we're not using TBB and this is actually a FastMap, we need to add these functions and hide
│ │ │ │ -
92 // the original erase functions.
│ │ │ │ -
93 void unsafe_erase(typename Base::iterator position) { ((Base*)this)->erase(position); }
│ │ │ │ -
94 typename Base::size_type unsafe_erase(const KEY& k) { return ((Base*)this)->erase(k); }
│ │ │ │ -
95 void unsafe_erase(typename Base::iterator first, typename Base::iterator last) {
│ │ │ │ -
96 return ((Base*)this)->erase(first, last); }
│ │ │ │ -
97private:
│ │ │ │ -
98 void erase() {}
│ │ │ │ -
99public:
│ │ │ │ -
100#endif
│ │ │ │ -
101
│ │ │ │ -
102private:
│ │ │ │ -
104 friend class boost::serialization::access;
│ │ │ │ -
105 template<class Archive>
│ │ │ │ -
106 void save(Archive& ar, const unsigned int /*version*/) const
│ │ │ │ -
107 {
│ │ │ │ -
108 // Copy to an STL container and serialize that
│ │ │ │ -
109 FastVector<std::pair<KEY, VALUE> > map(this->size());
│ │ │ │ -
110 std::copy(this->begin(), this->end(), map.begin());
│ │ │ │ -
111 ar & BOOST_SERIALIZATION_NVP(map);
│ │ │ │ -
112 }
│ │ │ │ -
113 template<class Archive>
│ │ │ │ -
114 void load(Archive& ar, const unsigned int /*version*/)
│ │ │ │ -
115 {
│ │ │ │ -
116 this->clear();
│ │ │ │ -
117 // Load into STL container and then fill our map
│ │ │ │ -
118 FastVector<std::pair<KEY, VALUE> > map;
│ │ │ │ -
119 ar & BOOST_SERIALIZATION_NVP(map);
│ │ │ │ -
120 this->insert(map.begin(), map.end());
│ │ │ │ -
121 }
│ │ │ │ -
122 BOOST_SERIALIZATION_SPLIT_MEMBER()
│ │ │ │ -
123};
│ │ │ │ +
44 const Class & derived() const {
│ │ │ │ +
45 return static_cast<const Class&>(*this);
│ │ │ │ +
46 }
│ │ │ │ +
47
│ │ │ │ +
48 Class compose(const Class& g) const {
│ │ │ │ +
49 return derived() * g;
│ │ │ │ +
50 }
│ │ │ │ +
51
│ │ │ │ +
52 Class between(const Class& g) const {
│ │ │ │ +
53 return derived().inverse() * g;
│ │ │ │ +
54 }
│ │ │ │ +
55
│ │ │ │ +
56 Class compose(const Class& g, ChartJacobian H1,
│ │ │ │ +
57 ChartJacobian H2 = boost::none) const {
│ │ │ │ +
58 if (H1) *H1 = g.inverse().AdjointMap();
│ │ │ │ +
59 if (H2) *H2 = Eigen::Matrix<double, N, N>::Identity();
│ │ │ │ +
60 return derived() * g;
│ │ │ │ +
61 }
│ │ │ │ +
62
│ │ │ │ +
63 Class between(const Class& g, ChartJacobian H1,
│ │ │ │ +
64 ChartJacobian H2 = boost::none) const {
│ │ │ │ +
65 Class result = derived().inverse() * g;
│ │ │ │ +
66 if (H1) *H1 = - result.inverse().AdjointMap();
│ │ │ │ +
67 if (H2) *H2 = Eigen::Matrix<double, N, N>::Identity();
│ │ │ │ +
68 return result;
│ │ │ │ +
69 }
│ │ │ │ +
70
│ │ │ │ +
71 Class inverse(ChartJacobian H) const {
│ │ │ │ +
72 if (H) *H = - derived().AdjointMap();
│ │ │ │ +
73 return derived().inverse();
│ │ │ │ +
74 }
│ │ │ │ +
75
│ │ │ │ +
│ │ │ │ +
78 Class expmap(const TangentVector& v) const {
│ │ │ │ +
79 return compose(Class::Expmap(v));
│ │ │ │ +
80 }
│ │ │ │ +
│ │ │ │ +
81
│ │ │ │ +
│ │ │ │ +
84 TangentVector logmap(const Class& g) const {
│ │ │ │ +
85 return Class::Logmap(between(g));
│ │ │ │ +
86 }
│ │ │ │ +
│ │ │ │ +
87
│ │ │ │ +
│ │ │ │ +
89 Class expmap(const TangentVector& v, //
│ │ │ │ +
90 ChartJacobian H1, ChartJacobian H2 = boost::none) const {
│ │ │ │ +
91 Jacobian D_g_v;
│ │ │ │ +
92 Class g = Class::Expmap(v,H2 ? &D_g_v : 0);
│ │ │ │ +
93 Class h = compose(g); // derivatives inlined below
│ │ │ │ +
94 if (H1) *H1 = g.inverse().AdjointMap();
│ │ │ │ +
95 if (H2) *H2 = D_g_v;
│ │ │ │ +
96 return h;
│ │ │ │ +
97 }
│ │ │ │ +
│ │ │ │ +
98
│ │ │ │ +
│ │ │ │ +
100 TangentVector logmap(const Class& g, //
│ │ │ │ +
101 ChartJacobian H1, ChartJacobian H2 = boost::none) const {
│ │ │ │ +
102 Class h = between(g); // derivatives inlined below
│ │ │ │ +
103 Jacobian D_v_h;
│ │ │ │ +
104 TangentVector v = Class::Logmap(h, (H1 || H2) ? &D_v_h : 0);
│ │ │ │ +
105 if (H1) *H1 = - D_v_h * h.inverse().AdjointMap();
│ │ │ │ +
106 if (H2) *H2 = D_v_h;
│ │ │ │ +
107 return v;
│ │ │ │ +
108 }
│ │ │ │ +
│ │ │ │ +
109
│ │ │ │ +
│ │ │ │ +
111 static Class Retract(const TangentVector& v) {
│ │ │ │ +
112 return Class::ChartAtOrigin::Retract(v);
│ │ │ │ +
113 }
│ │ │ │ +
│ │ │ │ +
114
│ │ │ │ +
│ │ │ │ +
116 static TangentVector LocalCoordinates(const Class& g) {
│ │ │ │ +
117 return Class::ChartAtOrigin::Local(g);
│ │ │ │ +
118 }
│ │ │ │ +
│ │ │ │ +
119
│ │ │ │ +
│ │ │ │ +
121 static Class Retract(const TangentVector& v, ChartJacobian H) {
│ │ │ │ +
122 return Class::ChartAtOrigin::Retract(v,H);
│ │ │ │ +
123 }
│ │ │ │
│ │ │ │
124
│ │ │ │ -
125}
│ │ │ │ -
FastMap.h
A thin wrapper around std::map that uses boost's fast_pool_allocator.
│ │ │ │ -
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ -
global_includes.h
Included from all GTSAM files.
│ │ │ │ -
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │ +
│ │ │ │ +
126 static TangentVector LocalCoordinates(const Class& g, ChartJacobian H) {
│ │ │ │ +
127 return Class::ChartAtOrigin::Local(g,H);
│ │ │ │ +
128 }
│ │ │ │ +
│ │ │ │ +
129
│ │ │ │ +
│ │ │ │ +
131 Class retract(const TangentVector& v) const {
│ │ │ │ +
132 return compose(Class::ChartAtOrigin::Retract(v));
│ │ │ │ +
133 }
│ │ │ │ +
│ │ │ │ +
134
│ │ │ │ +
│ │ │ │ +
136 TangentVector localCoordinates(const Class& g) const {
│ │ │ │ +
137 return Class::ChartAtOrigin::Local(between(g));
│ │ │ │ +
138 }
│ │ │ │ +
│ │ │ │ +
139
│ │ │ │ +
│ │ │ │ +
141 Class retract(const TangentVector& v, //
│ │ │ │ +
142 ChartJacobian H1, ChartJacobian H2 = boost::none) const {
│ │ │ │ +
143 Jacobian D_g_v;
│ │ │ │ +
144 Class g = Class::ChartAtOrigin::Retract(v, H2 ? &D_g_v : 0);
│ │ │ │ +
145 Class h = compose(g); // derivatives inlined below
│ │ │ │ +
146 if (H1) *H1 = g.inverse().AdjointMap();
│ │ │ │ +
147 if (H2) *H2 = D_g_v;
│ │ │ │ +
148 return h;
│ │ │ │ +
149 }
│ │ │ │ +
│ │ │ │ +
150
│ │ │ │ +
│ │ │ │ +
152 TangentVector localCoordinates(const Class& g, //
│ │ │ │ +
153 ChartJacobian H1, ChartJacobian H2 = boost::none) const {
│ │ │ │ +
154 Class h = between(g); // derivatives inlined below
│ │ │ │ +
155 Jacobian D_v_h;
│ │ │ │ +
156 TangentVector v = Class::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0);
│ │ │ │ +
157 if (H1) *H1 = - D_v_h * h.inverse().AdjointMap();
│ │ │ │ +
158 if (H2) *H2 = D_v_h;
│ │ │ │ +
159 return v;
│ │ │ │ +
160 }
│ │ │ │ +
│ │ │ │ +
161};
│ │ │ │ +
│ │ │ │ +
162
│ │ │ │ +
164struct lie_group_tag: public manifold_tag, public group_tag {};
│ │ │ │ +
165
│ │ │ │ +
166namespace internal {
│ │ │ │ +
167
│ │ │ │ +
173template<class Class>
│ │ │ │ +
│ │ │ │ +
174struct LieGroupTraits: GetDimensionImpl<Class, Class::dimension> {
│ │ │ │ +
175 typedef lie_group_tag structure_category;
│ │ │ │ +
176
│ │ │ │ +
179 typedef multiplicative_group_tag group_flavor;
│ │ │ │ +
180 static Class Identity() { return Class::Identity();}
│ │ │ │ +
182
│ │ │ │ +
185 typedef Class ManifoldType;
│ │ │ │ +
186 enum { dimension = Class::dimension };
│ │ │ │ +
187 typedef Eigen::Matrix<double, dimension, 1> TangentVector;
│ │ │ │ +
188 typedef OptionalJacobian<dimension, dimension> ChartJacobian;
│ │ │ │ +
189
│ │ │ │ +
190 static TangentVector Local(const Class& origin, const Class& other,
│ │ │ │ +
191 ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
│ │ │ │ +
192 return origin.localCoordinates(other, Horigin, Hother);
│ │ │ │ +
193 }
│ │ │ │ +
194
│ │ │ │ +
195 static Class Retract(const Class& origin, const TangentVector& v,
│ │ │ │ +
196 ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) {
│ │ │ │ +
197 return origin.retract(v, Horigin, Hv);
│ │ │ │ +
198 }
│ │ │ │ +
200
│ │ │ │ +
203 static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) {
│ │ │ │ +
204 return Class::Logmap(m, Hm);
│ │ │ │ +
205 }
│ │ │ │ +
206
│ │ │ │ +
207 static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
│ │ │ │ +
208 return Class::Expmap(v, Hv);
│ │ │ │ +
209 }
│ │ │ │ +
210
│ │ │ │ +
211 static Class Compose(const Class& m1, const Class& m2, //
│ │ │ │ +
212 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ +
213 return m1.compose(m2, H1, H2);
│ │ │ │ +
214 }
│ │ │ │ +
215
│ │ │ │ +
216 static Class Between(const Class& m1, const Class& m2, //
│ │ │ │ +
217 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ +
218 return m1.between(m2, H1, H2);
│ │ │ │ +
219 }
│ │ │ │ +
220
│ │ │ │ +
221 static Class Inverse(const Class& m, //
│ │ │ │ +
222 ChartJacobian H = boost::none) {
│ │ │ │ +
223 return m.inverse(H);
│ │ │ │ +
224 }
│ │ │ │ +
226};
│ │ │ │ +
│ │ │ │ +
227
│ │ │ │ +
229template<class Class> struct LieGroup: LieGroupTraits<Class>, Testable<Class> {};
│ │ │ │ +
230
│ │ │ │ +
231} // \ namepsace internal
│ │ │ │ +
232
│ │ │ │ +
239template<class Class>
│ │ │ │ +
│ │ │ │ +
240inline Class between_default(const Class& l1, const Class& l2) {
│ │ │ │ +
241 return l1.inverse().compose(l2);
│ │ │ │ +
242}
│ │ │ │ +
│ │ │ │ +
243
│ │ │ │ +
245template<class Class>
│ │ │ │ +
│ │ │ │ +
246inline Vector logmap_default(const Class& l0, const Class& lp) {
│ │ │ │ +
247 return Class::Logmap(l0.between(lp));
│ │ │ │ +
248}
│ │ │ │ +
│ │ │ │ +
249
│ │ │ │ +
251template<class Class>
│ │ │ │ +
│ │ │ │ +
252inline Class expmap_default(const Class& t, const Vector& d) {
│ │ │ │ +
253 return t.compose(Class::Expmap(d));
│ │ │ │ +
254}
│ │ │ │ +
│ │ │ │ +
255
│ │ │ │ +
259template<typename T>
│ │ │ │ +
│ │ │ │ +
260class IsLieGroup: public IsGroup<T>, public IsManifold<T> {
│ │ │ │ +
261public:
│ │ │ │ +
262 typedef typename traits<T>::structure_category structure_category_tag;
│ │ │ │ +
263 typedef typename traits<T>::ManifoldType ManifoldType;
│ │ │ │ +
264 typedef typename traits<T>::TangentVector TangentVector;
│ │ │ │ +
265 typedef typename traits<T>::ChartJacobian ChartJacobian;
│ │ │ │ +
266
│ │ │ │ +
267 BOOST_CONCEPT_USAGE(IsLieGroup) {
│ │ │ │ +
268 BOOST_STATIC_ASSERT_MSG(
│ │ │ │ +
269 (boost::is_base_of<lie_group_tag, structure_category_tag>::value),
│ │ │ │ +
270 "This type's trait does not assert it is a Lie group (or derived)");
│ │ │ │ +
271
│ │ │ │ +
272 // group opertations with Jacobians
│ │ │ │ +
273 g = traits<T>::Compose(g, h, Hg, Hh);
│ │ │ │ +
274 g = traits<T>::Between(g, h, Hg, Hh);
│ │ │ │ +
275 g = traits<T>::Inverse(g, Hg);
│ │ │ │ +
276 // log and exp map without Jacobians
│ │ │ │ +
277 g = traits<T>::Expmap(v);
│ │ │ │ +
278 v = traits<T>::Logmap(g);
│ │ │ │ +
279 // log and exponential map with Jacobians
│ │ │ │ +
280 g = traits<T>::Expmap(v, Hg);
│ │ │ │ +
281 v = traits<T>::Logmap(g, Hg);
│ │ │ │ +
282 }
│ │ │ │ +
283private:
│ │ │ │ +
284 T g, h;
│ │ │ │ +
285 TangentVector v;
│ │ │ │ +
286 ChartJacobian Hg, Hh;
│ │ │ │ +
287};
│ │ │ │ +
│ │ │ │ +
288
│ │ │ │ +
297template<class T>
│ │ │ │ +
│ │ │ │ +
298T BCH(const T& X, const T& Y) {
│ │ │ │ +
299 static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.;
│ │ │ │ +
300 T X_Y = bracket(X, Y);
│ │ │ │ +
301 return T(X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, bracket(X, X_Y)));
│ │ │ │ +
302}
│ │ │ │ +
│ │ │ │ +
303
│ │ │ │ +
308template <class T> Matrix wedge(const Vector& x);
│ │ │ │ +
309
│ │ │ │ +
316template <class T>
│ │ │ │ +
│ │ │ │ +
317T expm(const Vector& x, int K=7) {
│ │ │ │ +
318 Matrix xhat = wedge<T>(x);
│ │ │ │ +
319 return T(expm(xhat,K));
│ │ │ │ +
320}
│ │ │ │ +
│ │ │ │ +
321
│ │ │ │ +
326template <typename T>
│ │ │ │ +
│ │ │ │ +
327T interpolate(const T& X, const T& Y, double t,
│ │ │ │ +
328 typename MakeOptionalJacobian<T, T>::type Hx = boost::none,
│ │ │ │ +
329 typename MakeOptionalJacobian<T, T>::type Hy = boost::none) {
│ │ │ │ +
330 if (Hx || Hy) {
│ │ │ │ +
331 typename MakeJacobian<T, T>::type between_H_x, log_H, exp_H, compose_H_x;
│ │ │ │ +
332 const T between =
│ │ │ │ +
333 traits<T>::Between(X, Y, between_H_x); // between_H_y = identity
│ │ │ │ +
334 typename traits<T>::TangentVector delta = traits<T>::Logmap(between, log_H);
│ │ │ │ +
335 const T Delta = traits<T>::Expmap(t * delta, exp_H);
│ │ │ │ +
336 const T result = traits<T>::Compose(
│ │ │ │ +
337 X, Delta, compose_H_x); // compose_H_xinv_y = identity
│ │ │ │ +
338
│ │ │ │ +
339 if (Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x;
│ │ │ │ +
340 if (Hy) *Hy = t * exp_H * log_H;
│ │ │ │ +
341 return result;
│ │ │ │ +
342 }
│ │ │ │ +
343 return traits<T>::Compose(
│ │ │ │ +
344 X, traits<T>::Expmap(t * traits<T>::Logmap(traits<T>::Between(X, Y))));
│ │ │ │ +
345}
│ │ │ │ +
│ │ │ │ +
346
│ │ │ │ +
351template<class T>
│ │ │ │ +
│ │ │ │ +
352class TransformCovariance
│ │ │ │ +
353{
│ │ │ │ +
354private:
│ │ │ │ +
355 typename T::Jacobian adjointMap_;
│ │ │ │ +
356public:
│ │ │ │ +
357 explicit TransformCovariance(const T &X) : adjointMap_{X.AdjointMap()} {}
│ │ │ │ +
358 typename T::Jacobian operator()(const typename T::Jacobian &covariance)
│ │ │ │ +
359 { return adjointMap_ * covariance * adjointMap_.transpose(); }
│ │ │ │ +
360};
│ │ │ │ +
│ │ │ │ +
361
│ │ │ │ +
362} // namespace gtsam
│ │ │ │ +
363
│ │ │ │ +
372#define GTSAM_CONCEPT_LIE_INST(T) template class gtsam::IsLieGroup<T>;
│ │ │ │ +
373#define GTSAM_CONCEPT_LIE_TYPE(T) using _gtsam_IsLieGroup_##T = gtsam::IsLieGroup<T>;
│ │ │ │ +
Group.h
Concept check class for variable types with Group properties.
│ │ │ │ +
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::ConcurrentMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition ConcurrentMap.h:68
│ │ │ │ -
gtsam::ConcurrentMap::ConcurrentMap
ConcurrentMap(INPUTITERATOR first, INPUTITERATOR last)
Constructor from a range, passes through to base class.
Definition ConcurrentMap.h:79
│ │ │ │ -
gtsam::ConcurrentMap::ConcurrentMap
ConcurrentMap(const ConcurrentMap< KEY, VALUE > &x)
Copy constructor from another ConcurrentMap.
Definition ConcurrentMap.h:82
│ │ │ │ -
gtsam::ConcurrentMap::exists
bool exists(const KEY &e) const
Handy 'exists' function.
Definition ConcurrentMap.h:88
│ │ │ │ -
gtsam::ConcurrentMap::ConcurrentMap
ConcurrentMap(const Base &x)
Copy constructor from the base map class.
Definition ConcurrentMap.h:85
│ │ │ │ -
gtsam::ConcurrentMap::ConcurrentMap
ConcurrentMap()
Default constructor.
Definition ConcurrentMap.h:75
│ │ │ │ -
gtsam::ConcurrentMap::access
friend class boost::serialization::access
Serialization function.
Definition ConcurrentMap.h:104
│ │ │ │ -
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ +
gtsam::expm
T expm(const Vector &x, int K=7)
Exponential map given exponential coordinates class T needs a wedge<> function and a constructor from...
Definition Lie.h:317
│ │ │ │ +
gtsam::logmap_default
Vector logmap_default(const Class &l0, const Class &lp)
Log map centered at l0, s.t.
Definition Lie.h:246
│ │ │ │ +
gtsam::between_default
Class between_default(const Class &l1, const Class &l2)
These core global functions can be specialized by new Lie types for better performance.
Definition Lie.h:240
│ │ │ │ +
gtsam::interpolate
T interpolate(const T &X, const T &Y, double t, typename MakeOptionalJacobian< T, T >::type Hx=boost::none, typename MakeOptionalJacobian< T, T >::type Hy=boost::none)
Linear interpolation between X and Y by coefficient t.
Definition Lie.h:327
│ │ │ │ +
gtsam::BCH
T BCH(const T &X, const T &Y)
Three term approximation of the Baker-Campbell-Hausdorff formula In non-commutative Lie groups,...
Definition Lie.h:298
│ │ │ │ +
gtsam::expmap_default
Class expmap_default(const Class &t, const Vector &d)
Exponential map centered at l0, s.t.
Definition Lie.h:252
│ │ │ │ +
gtsam::wedge
Matrix wedge(const Vector &x)
Declaration of wedge (see Murray94book) used to convert from n exponential coordinates to n*n element...
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::group_tag
tag to assert a type is a group
Definition Group.h:34
│ │ │ │ +
gtsam::multiplicative_group_tag
Group operator syntax flavors.
Definition Group.h:37
│ │ │ │ +
gtsam::IsGroup
Group Concept.
Definition Group.h:46
│ │ │ │ +
gtsam::LieGroup
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition Lie.h:37
│ │ │ │ +
gtsam::LieGroup::Retract
static Class Retract(const TangentVector &v, ChartJacobian H)
Retract at origin with optional derivative.
Definition Lie.h:121
│ │ │ │ +
gtsam::LieGroup::localCoordinates
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g
Definition Lie.h:136
│ │ │ │ +
gtsam::LieGroup::expmap
Class expmap(const TangentVector &v, ChartJacobian H1, ChartJacobian H2=boost::none) const
expmap with optional derivatives
Definition Lie.h:89
│ │ │ │ +
gtsam::LieGroup::localCoordinates
TangentVector localCoordinates(const Class &g, ChartJacobian H1, ChartJacobian H2=boost::none) const
localCoordinates with optional derivatives
Definition Lie.h:152
│ │ │ │ +
gtsam::LieGroup::logmap
TangentVector logmap(const Class &g) const
logmap as required by manifold concept Applies logarithmic map to group element that takes *this to g
Definition Lie.h:84
│ │ │ │ +
gtsam::LieGroup::logmap
TangentVector logmap(const Class &g, ChartJacobian H1, ChartJacobian H2=boost::none) const
logmap with optional derivatives
Definition Lie.h:100
│ │ │ │ +
gtsam::LieGroup::LocalCoordinates
static TangentVector LocalCoordinates(const Class &g)
LocalCoordinates at origin: possible in Lie group because it has an identity.
Definition Lie.h:116
│ │ │ │ +
gtsam::LieGroup::retract
Class retract(const TangentVector &v, ChartJacobian H1, ChartJacobian H2=boost::none) const
retract with optional derivatives
Definition Lie.h:141
│ │ │ │ +
gtsam::LieGroup::LocalCoordinates
static TangentVector LocalCoordinates(const Class &g, ChartJacobian H)
LocalCoordinates at origin with optional derivative.
Definition Lie.h:126
│ │ │ │ +
gtsam::LieGroup::retract
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition Lie.h:131
│ │ │ │ +
gtsam::LieGroup::expmap
Class expmap(const TangentVector &v) const
expmap as required by manifold concept Applies exponential map to v and composes with *this
Definition Lie.h:78
│ │ │ │ +
gtsam::LieGroup::Retract
static Class Retract(const TangentVector &v)
Retract at origin: possible in Lie group because it has an identity.
Definition Lie.h:111
│ │ │ │ +
gtsam::lie_group_tag
tag to assert a type is a Lie group
Definition Lie.h:164
│ │ │ │ +
gtsam::internal::LieGroupTraits
A helper class that implements the traits interface for GTSAM lie groups.
Definition Lie.h:174
│ │ │ │ +
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition Lie.h:229
│ │ │ │ +
gtsam::IsLieGroup
Lie Group Concept.
Definition Lie.h:260
│ │ │ │ +
gtsam::TransformCovariance
Functor for transforming covariance of T.
Definition Lie.h:353
│ │ │ │ +
gtsam::manifold_tag
tag to assert a type is a manifold
Definition Manifold.h:33
│ │ │ │ +
gtsam::internal::GetDimensionImpl
Extra manifold traits for fixed-dimension types.
Definition Manifold.h:75
│ │ │ │ +
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,168 +1,440 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ConcurrentMap.h │ │ │ │ │ +Lie.h │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ -22 │ │ │ │ │ -23// Change class depending on whether we are using TBB │ │ │ │ │ -24#ifdef GTSAM_USE_TBB │ │ │ │ │ +24#pragma once │ │ │ │ │ 25 │ │ │ │ │ -26// Include TBB header │ │ │ │ │ -27# include │ │ │ │ │ -28# undef min // TBB seems to include Windows.h which defines these macros that │ │ │ │ │ -cause problems │ │ │ │ │ -29# undef max │ │ │ │ │ -30# undef ERROR │ │ │ │ │ -31 │ │ │ │ │ -32#include // std::hash() │ │ │ │ │ -33 │ │ │ │ │ -34// Use TBB concurrent_unordered_map for ConcurrentMap │ │ │ │ │ -35template │ │ │ │ │ -36using _C_o_n_c_u_r_r_e_n_t_M_a_p_B_a_s_e = tbb::concurrent_unordered_map< │ │ │ │ │ -37 KEY, │ │ │ │ │ -38 VALUE, │ │ │ │ │ -39 std::hash │ │ │ │ │ -40 >; │ │ │ │ │ -41 │ │ │ │ │ -42#else │ │ │ │ │ +26#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ +27#include <_g_t_s_a_m_/_b_a_s_e_/_G_r_o_u_p_._h> │ │ │ │ │ +28 │ │ │ │ │ +29namespace _g_t_s_a_m { │ │ │ │ │ +30 │ │ │ │ │ +36template │ │ │ │ │ +_3_7struct _L_i_e_G_r_o_u_p { │ │ │ │ │ +38 │ │ │ │ │ +39 enum { dimension = N }; │ │ │ │ │ +40 typedef _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_N_,_ _N_> ChartJacobian; │ │ │ │ │ +41 typedef Eigen::Matrix Jacobian; │ │ │ │ │ +42 typedef Eigen::Matrix TangentVector; │ │ │ │ │ 43 │ │ │ │ │ -44// If we're not using TBB, use a FastMap for ConcurrentMap │ │ │ │ │ -45#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_M_a_p_._h> │ │ │ │ │ -46template │ │ │ │ │ -47using _C_o_n_c_u_r_r_e_n_t_M_a_p_B_a_s_e = _g_t_s_a_m_:_:_F_a_s_t_M_a_p_<_K_E_Y_,_ _V_A_L_U_E_>; │ │ │ │ │ -48 │ │ │ │ │ -49#endif │ │ │ │ │ -50 │ │ │ │ │ -51#include │ │ │ │ │ -52#include │ │ │ │ │ -53#include │ │ │ │ │ -54 │ │ │ │ │ -55#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ -56 │ │ │ │ │ -57namespace _g_t_s_a_m { │ │ │ │ │ -58 │ │ │ │ │ -67template │ │ │ │ │ -_6_8class _C_o_n_c_u_r_r_e_n_t_M_a_p : public _C_o_n_c_u_r_r_e_n_t_M_a_p_B_a_s_e { │ │ │ │ │ -69 │ │ │ │ │ -70public: │ │ │ │ │ -71 │ │ │ │ │ -72 typedef _C_o_n_c_u_r_r_e_n_t_M_a_p_B_a_s_e_<_K_E_Y_,_V_A_L_U_E_> Base; │ │ │ │ │ -73 │ │ │ │ │ -_7_5 _C_o_n_c_u_r_r_e_n_t_M_a_p() {} │ │ │ │ │ -76 │ │ │ │ │ -78 template │ │ │ │ │ -_7_9 _C_o_n_c_u_r_r_e_n_t_M_a_p(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) │ │ │ │ │ -{} │ │ │ │ │ -80 │ │ │ │ │ -_8_2 _C_o_n_c_u_r_r_e_n_t_M_a_p(const _C_o_n_c_u_r_r_e_n_t_M_a_p_<_K_E_Y_,_V_A_L_U_E_>& x) : Base(x) {} │ │ │ │ │ -83 │ │ │ │ │ -_8_5 _C_o_n_c_u_r_r_e_n_t_M_a_p(const Base& x) : Base(x) {} │ │ │ │ │ -86 │ │ │ │ │ -_8_8 bool _e_x_i_s_t_s(const KEY& e) const { return this->count(e); } │ │ │ │ │ -89 │ │ │ │ │ -90#ifndef GTSAM_USE_TBB │ │ │ │ │ -91 // If we're not using TBB and this is actually a FastMap, we need to add │ │ │ │ │ -these functions and hide │ │ │ │ │ -92 // the original erase functions. │ │ │ │ │ -93 void unsafe_erase(typename Base::iterator position) { ((Base*)this)->erase │ │ │ │ │ -(position); } │ │ │ │ │ -94 typename Base::size_type unsafe_erase(const KEY& k) { return ((Base*)this)- │ │ │ │ │ ->erase(k); } │ │ │ │ │ -95 void unsafe_erase(typename Base::iterator first, typename Base::iterator │ │ │ │ │ -last) { │ │ │ │ │ -96 return ((Base*)this)->erase(first, last); } │ │ │ │ │ -97private: │ │ │ │ │ -98 void erase() {} │ │ │ │ │ -99public: │ │ │ │ │ -100#endif │ │ │ │ │ -101 │ │ │ │ │ -102private: │ │ │ │ │ -_1_0_4 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -105 template │ │ │ │ │ -106 void save(Archive& ar, const unsigned int /*version*/) const │ │ │ │ │ -107 { │ │ │ │ │ -108 // Copy to an STL container and serialize that │ │ │ │ │ -109 _F_a_s_t_V_e_c_t_o_r_<_s_t_d_:_:_p_a_i_r_<_K_E_Y_,_ _V_A_L_U_E_> > map(this->size()); │ │ │ │ │ -110 std::copy(this->begin(), this->end(), map.begin()); │ │ │ │ │ -111 ar & BOOST_SERIALIZATION_NVP(map); │ │ │ │ │ -112 } │ │ │ │ │ -113 template │ │ │ │ │ -114 void load(Archive& ar, const unsigned int /*version*/) │ │ │ │ │ -115 { │ │ │ │ │ -116 this->clear(); │ │ │ │ │ -117 // Load into STL container and then fill our map │ │ │ │ │ -118 _F_a_s_t_V_e_c_t_o_r_<_s_t_d_:_:_p_a_i_r_<_K_E_Y_,_ _V_A_L_U_E_> > map; │ │ │ │ │ -119 ar & BOOST_SERIALIZATION_NVP(map); │ │ │ │ │ -120 this->insert(map.begin(), map.end()); │ │ │ │ │ -121 } │ │ │ │ │ -122 BOOST_SERIALIZATION_SPLIT_MEMBER() │ │ │ │ │ -123}; │ │ │ │ │ +44 const Class & derived() const { │ │ │ │ │ +45 return static_cast(*this); │ │ │ │ │ +46 } │ │ │ │ │ +47 │ │ │ │ │ +48 Class compose(const Class& g) const { │ │ │ │ │ +49 return derived() * g; │ │ │ │ │ +50 } │ │ │ │ │ +51 │ │ │ │ │ +52 Class between(const Class& g) const { │ │ │ │ │ +53 return derived().inverse() * g; │ │ │ │ │ +54 } │ │ │ │ │ +55 │ │ │ │ │ +56 Class compose(const Class& g, ChartJacobian H1, │ │ │ │ │ +57 ChartJacobian H2 = boost::none) const { │ │ │ │ │ +58 if (H1) *H1 = g.inverse().AdjointMap(); │ │ │ │ │ +59 if (H2) *H2 = Eigen::Matrix::Identity(); │ │ │ │ │ +60 return derived() * g; │ │ │ │ │ +61 } │ │ │ │ │ +62 │ │ │ │ │ +63 Class between(const Class& g, ChartJacobian H1, │ │ │ │ │ +64 ChartJacobian H2 = boost::none) const { │ │ │ │ │ +65 Class result = derived().inverse() * g; │ │ │ │ │ +66 if (H1) *H1 = - result.inverse().AdjointMap(); │ │ │ │ │ +67 if (H2) *H2 = Eigen::Matrix::Identity(); │ │ │ │ │ +68 return result; │ │ │ │ │ +69 } │ │ │ │ │ +70 │ │ │ │ │ +71 Class inverse(ChartJacobian H) const { │ │ │ │ │ +72 if (H) *H = - derived().AdjointMap(); │ │ │ │ │ +73 return derived().inverse(); │ │ │ │ │ +74 } │ │ │ │ │ +75 │ │ │ │ │ +_7_8 Class _e_x_p_m_a_p(const TangentVector& v) const { │ │ │ │ │ +79 return compose(Class::Expmap(v)); │ │ │ │ │ +80 } │ │ │ │ │ +81 │ │ │ │ │ +_8_4 TangentVector _l_o_g_m_a_p(const Class& g) const { │ │ │ │ │ +85 return Class::Logmap(between(g)); │ │ │ │ │ +86 } │ │ │ │ │ +87 │ │ │ │ │ +_8_9 Class _e_x_p_m_a_p(const TangentVector& v, // │ │ │ │ │ +90 ChartJacobian H1, ChartJacobian H2 = boost::none) const { │ │ │ │ │ +91 Jacobian D_g_v; │ │ │ │ │ +92 Class g = Class::Expmap(v,H2 ? &D_g_v : 0); │ │ │ │ │ +93 Class h = compose(g); // derivatives inlined below │ │ │ │ │ +94 if (H1) *H1 = g.inverse().AdjointMap(); │ │ │ │ │ +95 if (H2) *H2 = D_g_v; │ │ │ │ │ +96 return h; │ │ │ │ │ +97 } │ │ │ │ │ +98 │ │ │ │ │ +_1_0_0 TangentVector _l_o_g_m_a_p(const Class& g, // │ │ │ │ │ +101 ChartJacobian H1, ChartJacobian H2 = boost::none) const { │ │ │ │ │ +102 Class h = between(g); // derivatives inlined below │ │ │ │ │ +103 Jacobian D_v_h; │ │ │ │ │ +104 TangentVector v = Class::Logmap(h, (H1 || H2) ? &D_v_h : 0); │ │ │ │ │ +105 if (H1) *H1 = - D_v_h * h.inverse().AdjointMap(); │ │ │ │ │ +106 if (H2) *H2 = D_v_h; │ │ │ │ │ +107 return v; │ │ │ │ │ +108 } │ │ │ │ │ +109 │ │ │ │ │ +_1_1_1 static Class _R_e_t_r_a_c_t(const TangentVector& v) { │ │ │ │ │ +112 return Class::ChartAtOrigin::Retract(v); │ │ │ │ │ +113 } │ │ │ │ │ +114 │ │ │ │ │ +_1_1_6 static TangentVector _L_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(const Class& g) { │ │ │ │ │ +117 return Class::ChartAtOrigin::Local(g); │ │ │ │ │ +118 } │ │ │ │ │ +119 │ │ │ │ │ +_1_2_1 static Class _R_e_t_r_a_c_t(const TangentVector& v, ChartJacobian H) { │ │ │ │ │ +122 return Class::ChartAtOrigin::Retract(v,H); │ │ │ │ │ +123 } │ │ │ │ │ 124 │ │ │ │ │ -125} │ │ │ │ │ -_F_a_s_t_M_a_p_._h │ │ │ │ │ -A thin wrapper around std::map that uses boost's fast_pool_allocator. │ │ │ │ │ -_F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ -A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ -_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ -Included from all GTSAM files. │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ -std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ -FastVector │ │ │ │ │ -FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ -DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ +_1_2_6 static TangentVector _L_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(const Class& g, ChartJacobian H) { │ │ │ │ │ +127 return Class::ChartAtOrigin::Local(g,H); │ │ │ │ │ +128 } │ │ │ │ │ +129 │ │ │ │ │ +_1_3_1 Class _r_e_t_r_a_c_t(const TangentVector& v) const { │ │ │ │ │ +132 return compose(Class::ChartAtOrigin::Retract(v)); │ │ │ │ │ +133 } │ │ │ │ │ +134 │ │ │ │ │ +_1_3_6 TangentVector _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(const Class& g) const { │ │ │ │ │ +137 return Class::ChartAtOrigin::Local(between(g)); │ │ │ │ │ +138 } │ │ │ │ │ +139 │ │ │ │ │ +_1_4_1 Class _r_e_t_r_a_c_t(const TangentVector& v, // │ │ │ │ │ +142 ChartJacobian H1, ChartJacobian H2 = boost::none) const { │ │ │ │ │ +143 Jacobian D_g_v; │ │ │ │ │ +144 Class g = Class::ChartAtOrigin::Retract(v, H2 ? &D_g_v : 0); │ │ │ │ │ +145 Class h = compose(g); // derivatives inlined below │ │ │ │ │ +146 if (H1) *H1 = g.inverse().AdjointMap(); │ │ │ │ │ +147 if (H2) *H2 = D_g_v; │ │ │ │ │ +148 return h; │ │ │ │ │ +149 } │ │ │ │ │ +150 │ │ │ │ │ +_1_5_2 TangentVector _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(const Class& g, // │ │ │ │ │ +153 ChartJacobian H1, ChartJacobian H2 = boost::none) const { │ │ │ │ │ +154 Class h = between(g); // derivatives inlined below │ │ │ │ │ +155 Jacobian D_v_h; │ │ │ │ │ +156 TangentVector v = Class::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0); │ │ │ │ │ +157 if (H1) *H1 = - D_v_h * h.inverse().AdjointMap(); │ │ │ │ │ +158 if (H2) *H2 = D_v_h; │ │ │ │ │ +159 return v; │ │ │ │ │ +160 } │ │ │ │ │ +161}; │ │ │ │ │ +162 │ │ │ │ │ +_1_6_4struct _l_i_e___g_r_o_u_p___t_a_g: public _m_a_n_i_f_o_l_d___t_a_g, public _g_r_o_u_p___t_a_g {}; │ │ │ │ │ +165 │ │ │ │ │ +166namespace internal { │ │ │ │ │ +167 │ │ │ │ │ +173template │ │ │ │ │ +_1_7_4struct _L_i_e_G_r_o_u_p_T_r_a_i_t_s: _G_e_t_D_i_m_e_n_s_i_o_n_I_m_p_l { │ │ │ │ │ +175 typedef _l_i_e___g_r_o_u_p___t_a_g _s_t_r_u_c_t_u_r_e___c_a_t_e_g_o_r_y; │ │ │ │ │ +176 │ │ │ │ │ +179 typedef _m_u_l_t_i_p_l_i_c_a_t_i_v_e___g_r_o_u_p___t_a_g _g_r_o_u_p___f_l_a_v_o_r; │ │ │ │ │ +180 static Class Identity() { return Class::Identity();} │ │ │ │ │ +182 │ │ │ │ │ +185 typedef Class ManifoldType; │ │ │ │ │ +186 enum { dimension = Class::dimension }; │ │ │ │ │ +187 typedef Eigen::Matrix TangentVector; │ │ │ │ │ +188 typedef _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_d_i_m_e_n_s_i_o_n_,_ _d_i_m_e_n_s_i_o_n_> ChartJacobian; │ │ │ │ │ +189 │ │ │ │ │ +190 static TangentVector Local(const Class& origin, const Class& other, │ │ │ │ │ +191 ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { │ │ │ │ │ +192 return origin.localCoordinates(other, Horigin, Hother); │ │ │ │ │ +193 } │ │ │ │ │ +194 │ │ │ │ │ +195 static Class Retract(const Class& origin, const TangentVector& v, │ │ │ │ │ +196 ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) { │ │ │ │ │ +197 return origin.retract(v, Horigin, Hv); │ │ │ │ │ +198 } │ │ │ │ │ +200 │ │ │ │ │ +203 static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) │ │ │ │ │ +{ │ │ │ │ │ +204 return Class::Logmap(m, Hm); │ │ │ │ │ +205 } │ │ │ │ │ +206 │ │ │ │ │ +207 static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) │ │ │ │ │ +{ │ │ │ │ │ +208 return Class::Expmap(v, Hv); │ │ │ │ │ +209 } │ │ │ │ │ +210 │ │ │ │ │ +211 static Class Compose(const Class& m1, const Class& m2, // │ │ │ │ │ +212 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { │ │ │ │ │ +213 return m1.compose(m2, H1, H2); │ │ │ │ │ +214 } │ │ │ │ │ +215 │ │ │ │ │ +216 static Class Between(const Class& m1, const Class& m2, // │ │ │ │ │ +217 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { │ │ │ │ │ +218 return m1.between(m2, H1, H2); │ │ │ │ │ +219 } │ │ │ │ │ +220 │ │ │ │ │ +221 static Class Inverse(const Class& m, // │ │ │ │ │ +222 ChartJacobian H = boost::none) { │ │ │ │ │ +223 return m.inverse(H); │ │ │ │ │ +224 } │ │ │ │ │ +226}; │ │ │ │ │ +227 │ │ │ │ │ +_2_2_9template struct _L_i_e_G_r_o_u_p: _L_i_e_G_r_o_u_p_T_r_a_i_t_s, │ │ │ │ │ +_T_e_s_t_a_b_l_e {}; │ │ │ │ │ +230 │ │ │ │ │ +231} // \ namepsace internal │ │ │ │ │ +232 │ │ │ │ │ +239template │ │ │ │ │ +_2_4_0inline Class _b_e_t_w_e_e_n___d_e_f_a_u_l_t(const Class& l1, const Class& l2) { │ │ │ │ │ +241 return l1.inverse().compose(l2); │ │ │ │ │ +242} │ │ │ │ │ +243 │ │ │ │ │ +245template │ │ │ │ │ +_2_4_6inline Vector _l_o_g_m_a_p___d_e_f_a_u_l_t(const Class& l0, const Class& lp) { │ │ │ │ │ +247 return Class::Logmap(l0.between(lp)); │ │ │ │ │ +248} │ │ │ │ │ +249 │ │ │ │ │ +251template │ │ │ │ │ +_2_5_2inline Class _e_x_p_m_a_p___d_e_f_a_u_l_t(const Class& t, const Vector& d) { │ │ │ │ │ +253 return t.compose(Class::Expmap(d)); │ │ │ │ │ +254} │ │ │ │ │ +255 │ │ │ │ │ +259template │ │ │ │ │ +_2_6_0class _I_s_L_i_e_G_r_o_u_p: public _I_s_G_r_o_u_p, public IsManifold { │ │ │ │ │ +261public: │ │ │ │ │ +262 typedef typename _t_r_a_i_t_s_<_T_>_:_:_s_t_r_u_c_t_u_r_e___c_a_t_e_g_o_r_y structure_category_tag; │ │ │ │ │ +263 typedef typename _t_r_a_i_t_s_<_T_>_:_:_M_a_n_i_f_o_l_d_T_y_p_e ManifoldType; │ │ │ │ │ +264 typedef typename _t_r_a_i_t_s_<_T_>_:_:_T_a_n_g_e_n_t_V_e_c_t_o_r TangentVector; │ │ │ │ │ +265 typedef typename _t_r_a_i_t_s_<_T_>_:_:_C_h_a_r_t_J_a_c_o_b_i_a_n ChartJacobian; │ │ │ │ │ +266 │ │ │ │ │ +267 BOOST_CONCEPT_USAGE(_I_s_L_i_e_G_r_o_u_p) { │ │ │ │ │ +268 BOOST_STATIC_ASSERT_MSG( │ │ │ │ │ +269 (boost::is_base_of::value), │ │ │ │ │ +270 "This type's trait does not assert it is a Lie group (or derived)"); │ │ │ │ │ +271 │ │ │ │ │ +272 // group opertations with Jacobians │ │ │ │ │ +273 g = _t_r_a_i_t_s_<_T_>_:_:_C_o_m_p_o_s_e(g, h, Hg, Hh); │ │ │ │ │ +274 g = _t_r_a_i_t_s_<_T_>_:_:_B_e_t_w_e_e_n(g, h, Hg, Hh); │ │ │ │ │ +275 g = _t_r_a_i_t_s_<_T_>_:_:_I_n_v_e_r_s_e(g, Hg); │ │ │ │ │ +276 // log and exp map without Jacobians │ │ │ │ │ +277 g = _t_r_a_i_t_s_<_T_>_:_:_E_x_p_m_a_p(v); │ │ │ │ │ +278 v = _t_r_a_i_t_s_<_T_>_:_:_L_o_g_m_a_p(g); │ │ │ │ │ +279 // log and exponential map with Jacobians │ │ │ │ │ +280 g = _t_r_a_i_t_s_<_T_>_:_:_E_x_p_m_a_p(v, Hg); │ │ │ │ │ +281 v = _t_r_a_i_t_s_<_T_>_:_:_L_o_g_m_a_p(g, Hg); │ │ │ │ │ +282 } │ │ │ │ │ +283private: │ │ │ │ │ +284 T g, h; │ │ │ │ │ +285 TangentVector v; │ │ │ │ │ +286 ChartJacobian Hg, Hh; │ │ │ │ │ +287}; │ │ │ │ │ +288 │ │ │ │ │ +297template │ │ │ │ │ +_2_9_8T _B_C_H(const T& X, const T& Y) { │ │ │ │ │ +299 static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.; │ │ │ │ │ +300 T X_Y = bracket(X, Y); │ │ │ │ │ +301 return T(X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, │ │ │ │ │ +bracket(X, X_Y))); │ │ │ │ │ +302} │ │ │ │ │ +303 │ │ │ │ │ +_3_0_8template Matrix _w_e_d_g_e(const Vector& x); │ │ │ │ │ +309 │ │ │ │ │ +316template │ │ │ │ │ +_3_1_7T _e_x_p_m(const Vector& x, int K=7) { │ │ │ │ │ +318 Matrix xhat = wedge(x); │ │ │ │ │ +319 return T(_e_x_p_m(xhat,K)); │ │ │ │ │ +320} │ │ │ │ │ +321 │ │ │ │ │ +326template │ │ │ │ │ +_3_2_7T _i_n_t_e_r_p_o_l_a_t_e(const T& X, const T& Y, double t, │ │ │ │ │ +328 typename MakeOptionalJacobian::type Hx = boost::none, │ │ │ │ │ +329 typename MakeOptionalJacobian::type Hy = boost::none) { │ │ │ │ │ +330 if (Hx || Hy) { │ │ │ │ │ +331 typename MakeJacobian::type between_H_x, log_H, exp_H, compose_H_x; │ │ │ │ │ +332 const T between = │ │ │ │ │ +333 _t_r_a_i_t_s_<_T_>_:_:_B_e_t_w_e_e_n(X, Y, between_H_x); // between_H_y = identity │ │ │ │ │ +334 typename _t_r_a_i_t_s_<_T_>_:_:_T_a_n_g_e_n_t_V_e_c_t_o_r delta = _t_r_a_i_t_s_<_T_>_:_:_L_o_g_m_a_p(between, │ │ │ │ │ +log_H); │ │ │ │ │ +335 const T Delta = _t_r_a_i_t_s_<_T_>_:_:_E_x_p_m_a_p(t * delta, exp_H); │ │ │ │ │ +336 const T result = _t_r_a_i_t_s_<_T_>_:_:_C_o_m_p_o_s_e( │ │ │ │ │ +337 X, Delta, compose_H_x); // compose_H_xinv_y = identity │ │ │ │ │ +338 │ │ │ │ │ +339 if (Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x; │ │ │ │ │ +340 if (Hy) *Hy = t * exp_H * log_H; │ │ │ │ │ +341 return result; │ │ │ │ │ +342 } │ │ │ │ │ +343 return _t_r_a_i_t_s_<_T_>_:_:_C_o_m_p_o_s_e( │ │ │ │ │ +344 X, _t_r_a_i_t_s_<_T_>_:_:_E_x_p_m_a_p(t * _t_r_a_i_t_s_<_T_>_:_:_L_o_g_m_a_p(_t_r_a_i_t_s_<_T_>_:_:_B_e_t_w_e_e_n(X, Y)))); │ │ │ │ │ +345} │ │ │ │ │ +346 │ │ │ │ │ +351template │ │ │ │ │ +_3_5_2class _T_r_a_n_s_f_o_r_m_C_o_v_a_r_i_a_n_c_e │ │ │ │ │ +353{ │ │ │ │ │ +354private: │ │ │ │ │ +355 typename T::Jacobian adjointMap_; │ │ │ │ │ +356public: │ │ │ │ │ +357 explicit _T_r_a_n_s_f_o_r_m_C_o_v_a_r_i_a_n_c_e(const T &X) : adjointMap_{X.AdjointMap()} {} │ │ │ │ │ +358 typename T::Jacobian operator()(const typename T::Jacobian &covariance) │ │ │ │ │ +359 { return adjointMap_ * covariance * adjointMap_.transpose(); } │ │ │ │ │ +360}; │ │ │ │ │ +361 │ │ │ │ │ +362} // namespace gtsam │ │ │ │ │ +363 │ │ │ │ │ +_3_7_2#define GTSAM_CONCEPT_LIE_INST(T) template class gtsam::IsLieGroup; │ │ │ │ │ +373#define GTSAM_CONCEPT_LIE_TYPE(T) using _gtsam_IsLieGroup_##T = gtsam:: │ │ │ │ │ +IsLieGroup; │ │ │ │ │ +_G_r_o_u_p_._h │ │ │ │ │ +Concept check class for variable types with Group properties. │ │ │ │ │ +_M_a_n_i_f_o_l_d_._h │ │ │ │ │ +Base class and basic functions for Manifold types. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_c_u_r_r_e_n_t_M_a_p │ │ │ │ │ -FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ -fast_pool_allocator instead of the defa... │ │ │ │ │ -DDeeffiinniittiioonn ConcurrentMap.h:68 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_c_u_r_r_e_n_t_M_a_p_:_:_C_o_n_c_u_r_r_e_n_t_M_a_p │ │ │ │ │ -ConcurrentMap(INPUTITERATOR first, INPUTITERATOR last) │ │ │ │ │ -Constructor from a range, passes through to base class. │ │ │ │ │ -DDeeffiinniittiioonn ConcurrentMap.h:79 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_c_u_r_r_e_n_t_M_a_p_:_:_C_o_n_c_u_r_r_e_n_t_M_a_p │ │ │ │ │ -ConcurrentMap(const ConcurrentMap< KEY, VALUE > &x) │ │ │ │ │ -Copy constructor from another ConcurrentMap. │ │ │ │ │ -DDeeffiinniittiioonn ConcurrentMap.h:82 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_c_u_r_r_e_n_t_M_a_p_:_:_e_x_i_s_t_s │ │ │ │ │ -bool exists(const KEY &e) const │ │ │ │ │ -Handy 'exists' function. │ │ │ │ │ -DDeeffiinniittiioonn ConcurrentMap.h:88 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_c_u_r_r_e_n_t_M_a_p_:_:_C_o_n_c_u_r_r_e_n_t_M_a_p │ │ │ │ │ -ConcurrentMap(const Base &x) │ │ │ │ │ -Copy constructor from the base map class. │ │ │ │ │ -DDeeffiinniittiioonn ConcurrentMap.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_c_u_r_r_e_n_t_M_a_p_:_:_C_o_n_c_u_r_r_e_n_t_M_a_p │ │ │ │ │ -ConcurrentMap() │ │ │ │ │ -Default constructor. │ │ │ │ │ -DDeeffiinniittiioonn ConcurrentMap.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_c_u_r_r_e_n_t_M_a_p_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn ConcurrentMap.h:104 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ -FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ -fast_pool_allocator instead of the defa... │ │ │ │ │ -DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_e_x_p_m │ │ │ │ │ +T expm(const Vector &x, int K=7) │ │ │ │ │ +Exponential map given exponential coordinates class T needs a wedge<> function │ │ │ │ │ +and a constructor from... │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:317 │ │ │ │ │ +_g_t_s_a_m_:_:_l_o_g_m_a_p___d_e_f_a_u_l_t │ │ │ │ │ +Vector logmap_default(const Class &l0, const Class &lp) │ │ │ │ │ +Log map centered at l0, s.t. │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:246 │ │ │ │ │ +_g_t_s_a_m_:_:_b_e_t_w_e_e_n___d_e_f_a_u_l_t │ │ │ │ │ +Class between_default(const Class &l1, const Class &l2) │ │ │ │ │ +These core global functions can be specialized by new Lie types for better │ │ │ │ │ +performance. │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:240 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_p_o_l_a_t_e │ │ │ │ │ +T interpolate(const T &X, const T &Y, double t, typename MakeOptionalJacobian< │ │ │ │ │ +T, T >::type Hx=boost::none, typename MakeOptionalJacobian< T, T >::type │ │ │ │ │ +Hy=boost::none) │ │ │ │ │ +Linear interpolation between X and Y by coefficient t. │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:327 │ │ │ │ │ +_g_t_s_a_m_:_:_B_C_H │ │ │ │ │ +T BCH(const T &X, const T &Y) │ │ │ │ │ +Three term approximation of the Baker-Campbell-Hausdorff formula In non- │ │ │ │ │ +commutative Lie groups,... │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:298 │ │ │ │ │ +_g_t_s_a_m_:_:_e_x_p_m_a_p___d_e_f_a_u_l_t │ │ │ │ │ +Class expmap_default(const Class &t, const Vector &d) │ │ │ │ │ +Exponential map centered at l0, s.t. │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:252 │ │ │ │ │ +_g_t_s_a_m_:_:_w_e_d_g_e │ │ │ │ │ +Matrix wedge(const Vector &x) │ │ │ │ │ +Declaration of wedge (see Murray94book) used to convert from n exponential │ │ │ │ │ +coordinates to n*n element... │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_g_r_o_u_p___t_a_g │ │ │ │ │ +tag to assert a type is a group │ │ │ │ │ +DDeeffiinniittiioonn Group.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_m_u_l_t_i_p_l_i_c_a_t_i_v_e___g_r_o_u_p___t_a_g │ │ │ │ │ +Group operator syntax flavors. │ │ │ │ │ +DDeeffiinniittiioonn Group.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_I_s_G_r_o_u_p │ │ │ │ │ +Group Concept. │ │ │ │ │ +DDeeffiinniittiioonn Group.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ +A CRTP helper class that implements Lie group methods Prerequisites: methods │ │ │ │ │ +operator*,... │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_R_e_t_r_a_c_t │ │ │ │ │ +static Class Retract(const TangentVector &v, ChartJacobian H) │ │ │ │ │ +Retract at origin with optional derivative. │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:121 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ +TangentVector localCoordinates(const Class &g) const │ │ │ │ │ +localCoordinates as required by manifold concept: finds tangent vector between │ │ │ │ │ +*this and g │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:136 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_e_x_p_m_a_p │ │ │ │ │ +Class expmap(const TangentVector &v, ChartJacobian H1, ChartJacobian H2=boost:: │ │ │ │ │ +none) const │ │ │ │ │ +expmap with optional derivatives │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:89 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ +TangentVector localCoordinates(const Class &g, ChartJacobian H1, ChartJacobian │ │ │ │ │ +H2=boost::none) const │ │ │ │ │ +localCoordinates with optional derivatives │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:152 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_l_o_g_m_a_p │ │ │ │ │ +TangentVector logmap(const Class &g) const │ │ │ │ │ +logmap as required by manifold concept Applies logarithmic map to group element │ │ │ │ │ +that takes *this to g │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:84 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_l_o_g_m_a_p │ │ │ │ │ +TangentVector logmap(const Class &g, ChartJacobian H1, ChartJacobian H2=boost:: │ │ │ │ │ +none) const │ │ │ │ │ +logmap with optional derivatives │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_L_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ +static TangentVector LocalCoordinates(const Class &g) │ │ │ │ │ +LocalCoordinates at origin: possible in Lie group because it has an identity. │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:116 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_r_e_t_r_a_c_t │ │ │ │ │ +Class retract(const TangentVector &v, ChartJacobian H1, ChartJacobian │ │ │ │ │ +H2=boost::none) const │ │ │ │ │ +retract with optional derivatives │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:141 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_L_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ +static TangentVector LocalCoordinates(const Class &g, ChartJacobian H) │ │ │ │ │ +LocalCoordinates at origin with optional derivative. │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:126 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_r_e_t_r_a_c_t │ │ │ │ │ +Class retract(const TangentVector &v) const │ │ │ │ │ +retract as required by manifold concept: applies v at *this │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:131 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_e_x_p_m_a_p │ │ │ │ │ +Class expmap(const TangentVector &v) const │ │ │ │ │ +expmap as required by manifold concept Applies exponential map to v and │ │ │ │ │ +composes with *this │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:78 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_R_e_t_r_a_c_t │ │ │ │ │ +static Class Retract(const TangentVector &v) │ │ │ │ │ +Retract at origin: possible in Lie group because it has an identity. │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_l_i_e___g_r_o_u_p___t_a_g │ │ │ │ │ +tag to assert a type is a Lie group │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:164 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p_T_r_a_i_t_s │ │ │ │ │ +A helper class that implements the traits interface for GTSAM lie groups. │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:174 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ +Both LieGroupTraits and Testable. │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:229 │ │ │ │ │ +_g_t_s_a_m_:_:_I_s_L_i_e_G_r_o_u_p │ │ │ │ │ +Lie Group Concept. │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:260 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_a_n_s_f_o_r_m_C_o_v_a_r_i_a_n_c_e │ │ │ │ │ +Functor for transforming covariance of T. │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:353 │ │ │ │ │ +_g_t_s_a_m_:_:_m_a_n_i_f_o_l_d___t_a_g │ │ │ │ │ +tag to assert a type is a manifold │ │ │ │ │ +DDeeffiinniittiioonn Manifold.h:33 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_G_e_t_D_i_m_e_n_s_i_o_n_I_m_p_l │ │ │ │ │ +Extra manifold traits for fixed-dimension types. │ │ │ │ │ +DDeeffiinniittiioonn Manifold.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ +either a fixed size o... │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * CCoonnccuurrrreennttMMaapp..hh │ │ │ │ │ + * _L_i_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00134.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/serialization.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/WeightedSampler.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,166 +94,44 @@ │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
serialization.h File Reference
│ │ │ │ +
WeightedSampler.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Convenience functions for serializing data structures via boost.serialization. │ │ │ │ +

Fast sampling without replacement. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::WeightedSampler< Engine >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

Standard serialization

Serialization in default compressed format

│ │ │ │ -
template<class T >
void gtsam::serializeToStream (const T &input, std::ostream &out_archive_stream)
 
│ │ │ │ -template<class T >
void gtsam::deserializeFromStream (std::istream &in_archive_stream, T &output)
 deserializes from a stream
 
│ │ │ │ -template<class T >
std::string gtsam::serializeToString (const T &input)
 serializes to a string
 
│ │ │ │ -template<class T >
void gtsam::deserializeFromString (const std::string &serialized, T &output)
 deserializes from a string
 
│ │ │ │ -template<class T >
bool gtsam::serializeToFile (const T &input, const std::string &filename)
 serializes to a file
 
│ │ │ │ -template<class T >
bool gtsam::deserializeFromFile (const std::string &filename, T &output)
 deserializes from a file
 
│ │ │ │ -template<class T >
std::string gtsam::serialize (const T &input)
 serializes to a string
 
│ │ │ │ -template<class T >
void gtsam::deserialize (const std::string &serialized, T &output)
 deserializes from a string
 
XML Serialization

Serialization to XML format with named structures

│ │ │ │ -
template<class T >
void gtsam::serializeToXMLStream (const T &input, std::ostream &out_archive_stream, const std::string &name="data")
 
│ │ │ │ -template<class T >
void gtsam::deserializeFromXMLStream (std::istream &in_archive_stream, T &output, const std::string &name="data")
 deserializes from a stream in XML
 
│ │ │ │ -template<class T >
std::string gtsam::serializeToXMLString (const T &input, const std::string &name="data")
 serializes to a string in XML
 
│ │ │ │ -template<class T >
void gtsam::deserializeFromXMLString (const std::string &serialized, T &output, const std::string &name="data")
 deserializes from a string in XML
 
│ │ │ │ -template<class T >
bool gtsam::serializeToXMLFile (const T &input, const std::string &filename, const std::string &name="data")
 serializes to an XML file
 
│ │ │ │ -template<class T >
bool gtsam::deserializeFromXMLFile (const std::string &filename, T &output, const std::string &name="data")
 deserializes from an XML file
 
│ │ │ │ -template<class T >
std::string gtsam::serializeXML (const T &input, const std::string &name="data")
 serializes to a string in XML
 
│ │ │ │ -template<class T >
void gtsam::deserializeXML (const std::string &serialized, T &output, const std::string &name="data")
 deserializes from a string in XML
 
Binary Serialization

Serialization to binary format with named structures

│ │ │ │ -
template<class T >
void gtsam::serializeToBinaryStream (const T &input, std::ostream &out_archive_stream, const std::string &name="data")
 
│ │ │ │ -template<class T >
void gtsam::deserializeFromBinaryStream (std::istream &in_archive_stream, T &output, const std::string &name="data")
 deserializes from a stream in binary
 
│ │ │ │ -template<class T >
std::string gtsam::serializeToBinaryString (const T &input, const std::string &name="data")
 serializes to a string in binary
 
│ │ │ │ -template<class T >
void gtsam::deserializeFromBinaryString (const std::string &serialized, T &output, const std::string &name="data")
 deserializes from a string in binary
 
│ │ │ │ -template<class T >
bool gtsam::serializeToBinaryFile (const T &input, const std::string &filename, const std::string &name="data")
 serializes to a binary file
 
│ │ │ │ -template<class T >
bool gtsam::deserializeFromBinaryFile (const std::string &filename, T &output, const std::string &name="data")
 deserializes from a binary file
 
│ │ │ │ -template<class T >
std::string gtsam::serializeBinary (const T &input, const std::string &name="data")
 serializes to a string in binary
 
│ │ │ │ -template<class T >
void gtsam::deserializeBinary (const std::string &serialized, T &output, const std::string &name="data")
 deserializes from a string in binary
 
│ │ │ │

Detailed Description

│ │ │ │ -

Convenience functions for serializing data structures via boost.serialization.

│ │ │ │ -
Author
Alex Cunningham
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ -
Date
Feb 7, 2012
│ │ │ │ +

Fast sampling without replacement.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
Date
May 2019
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,146 +1,26 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -serialization.h File Reference │ │ │ │ │ -Convenience functions for serializing data structures via boost.serialization. │ │ │ │ │ -_M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +WeightedSampler.h File Reference │ │ │ │ │ +Fast sampling without replacement. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_W_e_i_g_h_t_e_d_S_a_m_p_l_e_r_<_ _E_n_g_i_n_e_ _> │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -Standard serialization │ │ │ │ │ -Serialization in default compressed format │ │ │ │ │ -template │ │ │ │ │ - void  _g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_S_t_r_e_a_m (const T &input, std::ostream │ │ │ │ │ - &out_archive_stream) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - void  ggttssaamm::::ddeesseerriiaalliizzeeFFrroommSSttrreeaamm (std::istream &in_archive_stream, T │ │ │ │ │ - &output) │ │ │ │ │ -  deserializes from a stream │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -std::string  ggttssaamm::::sseerriiaalliizzeeTTooSSttrriinngg (const T &input) │ │ │ │ │ -  serializes to a string │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - void  ggttssaamm::::ddeesseerriiaalliizzeeFFrroommSSttrriinngg (const std::string &serialized, T │ │ │ │ │ - &output) │ │ │ │ │ -  deserializes from a string │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - bool  ggttssaamm::::sseerriiaalliizzeeTTooFFiillee (const T &input, const std::string │ │ │ │ │ - &filename) │ │ │ │ │ -  serializes to a file │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - bool  ggttssaamm::::ddeesseerriiaalliizzeeFFrroommFFiillee (const std::string &filename, T │ │ │ │ │ - &output) │ │ │ │ │ -  deserializes from a file │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -std::string  ggttssaamm::::sseerriiaalliizzee (const T &input) │ │ │ │ │ -  serializes to a string │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - void  ggttssaamm::::ddeesseerriiaalliizzee (const std::string &serialized, T &output) │ │ │ │ │ -  deserializes from a string │ │ │ │ │ -  │ │ │ │ │ -XML Serialization │ │ │ │ │ -Serialization to XML format with named structures │ │ │ │ │ -template │ │ │ │ │ - void  _g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_X_M_L_S_t_r_e_a_m (const T &input, std::ostream │ │ │ │ │ - &out_archive_stream, const std::string &name="data") │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - void  ggttssaamm::::ddeesseerriiaalliizzeeFFrroommXXMMLLSSttrreeaamm (std::istream &in_archive_stream, │ │ │ │ │ - T &output, const std::string &name="data") │ │ │ │ │ -  deserializes from a stream in XML │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -std::string  ggttssaamm::::sseerriiaalliizzeeTTooXXMMLLSSttrriinngg (const T &input, const std::string │ │ │ │ │ - &name="data") │ │ │ │ │ -  serializes to a string in XML │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - void  ggttssaamm::::ddeesseerriiaalliizzeeFFrroommXXMMLLSSttrriinngg (const std::string &serialized, T │ │ │ │ │ - &output, const std::string &name="data") │ │ │ │ │ -  deserializes from a string in XML │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - bool  ggttssaamm::::sseerriiaalliizzeeTTooXXMMLLFFiillee (const T &input, const std::string │ │ │ │ │ - &filename, const std::string &name="data") │ │ │ │ │ -  serializes to an XML file │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - bool  ggttssaamm::::ddeesseerriiaalliizzeeFFrroommXXMMLLFFiillee (const std::string &filename, T │ │ │ │ │ - &output, const std::string &name="data") │ │ │ │ │ -  deserializes from an XML file │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -std::string  ggttssaamm::::sseerriiaalliizzeeXXMMLL (const T &input, const std::string │ │ │ │ │ - &name="data") │ │ │ │ │ -  serializes to a string in XML │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - void  ggttssaamm::::ddeesseerriiaalliizzeeXXMMLL (const std::string &serialized, T &output, │ │ │ │ │ - const std::string &name="data") │ │ │ │ │ -  deserializes from a string in XML │ │ │ │ │ -  │ │ │ │ │ -Binary Serialization │ │ │ │ │ -Serialization to binary format with named structures │ │ │ │ │ -template │ │ │ │ │ - void  _g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_B_i_n_a_r_y_S_t_r_e_a_m (const T &input, std::ostream │ │ │ │ │ - &out_archive_stream, const std::string &name="data") │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - void  ggttssaamm::::ddeesseerriiaalliizzeeFFrroommBBiinnaarryySSttrreeaamm (std::istream │ │ │ │ │ - &in_archive_stream, T &output, const std::string &name="data") │ │ │ │ │ -  deserializes from a stream in binary │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -std::string  ggttssaamm::::sseerriiaalliizzeeTTooBBiinnaarryySSttrriinngg (const T &input, const std::string │ │ │ │ │ - &name="data") │ │ │ │ │ -  serializes to a string in binary │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - void  ggttssaamm::::ddeesseerriiaalliizzeeFFrroommBBiinnaarryySSttrriinngg (const std::string &serialized, │ │ │ │ │ - T &output, const std::string &name="data") │ │ │ │ │ -  deserializes from a string in binary │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - bool  ggttssaamm::::sseerriiaalliizzeeTTooBBiinnaarryyFFiillee (const T &input, const std::string │ │ │ │ │ - &filename, const std::string &name="data") │ │ │ │ │ -  serializes to a binary file │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - bool  ggttssaamm::::ddeesseerriiaalliizzeeFFrroommBBiinnaarryyFFiillee (const std::string &filename, T │ │ │ │ │ - &output, const std::string &name="data") │ │ │ │ │ -  deserializes from a binary file │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -std::string  ggttssaamm::::sseerriiaalliizzeeBBiinnaarryy (const T &input, const std::string │ │ │ │ │ - &name="data") │ │ │ │ │ -  serializes to a string in binary │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - void  ggttssaamm::::ddeesseerriiaalliizzeeBBiinnaarryy (const std::string &serialized, T │ │ │ │ │ - &output, const std::string &name="data") │ │ │ │ │ -  deserializes from a string in binary │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Convenience functions for serializing data structures via boost.serialization. │ │ │ │ │ +Fast sampling without replacement. │ │ │ │ │ Author │ │ │ │ │ - Alex Cunningham │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - Feb 7, 2012 │ │ │ │ │ + May 2019 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _s_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ + * _W_e_i_g_h_t_e_d_S_a_m_p_l_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00134.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,26 +1,3 @@ │ │ │ │ │ var a00134 = [ │ │ │ │ │ - ["deserialize", "a00134.html#aa4d516d0d98946341ad2766521011d1b", null], │ │ │ │ │ - ["deserializeBinary", "a00134.html#a56c41958e57a4bc7361ab593011462fd", null], │ │ │ │ │ - ["deserializeFromBinaryFile", "a00134.html#aefeb77f2324e4bbfca2c8152c49a4565", null], │ │ │ │ │ - ["deserializeFromBinaryStream", "a00134.html#a8eb16192d8ebe2532d8a69c9f2bf15b2", null], │ │ │ │ │ - ["deserializeFromBinaryString", "a00134.html#a349b7ed4a67a8c3d80737e5383507bc4", null], │ │ │ │ │ - ["deserializeFromFile", "a00134.html#adbb4a71187706918faca3a11519b62f6", null], │ │ │ │ │ - ["deserializeFromStream", "a00134.html#ab181d2a3de466fc0c87bd29501bbc7ab", null], │ │ │ │ │ - ["deserializeFromString", "a00134.html#a262487af458d1f48dd7d3bc549caae00", null], │ │ │ │ │ - ["deserializeFromXMLFile", "a00134.html#a0d8faf2bb0a908ff484677bb2fcf707c", null], │ │ │ │ │ - ["deserializeFromXMLStream", "a00134.html#a8d7ea177ef3e93af15a4a77c2bd7fa8b", null], │ │ │ │ │ - ["deserializeFromXMLString", "a00134.html#a5f2fe2bfc17737ff9ae1fb8b210604f0", null], │ │ │ │ │ - ["deserializeXML", "a00134.html#a85ab51b2e8f4cea69f72e06a5c136e85", null], │ │ │ │ │ - ["serialize", "a00134.html#a332bad7d1e70d3c04ceca35adce7a134", null], │ │ │ │ │ - ["serializeBinary", "a00134.html#abb476c3e77878f7e0f70dd51871f48e7", null], │ │ │ │ │ - ["serializeToBinaryFile", "a00134.html#abdff0a4b4fdf0e609aa62fb8d8b1dbba", null], │ │ │ │ │ - ["serializeToBinaryStream", "a00134.html#aa5d177a3bf16f9f5acf45ee62c816479", null], │ │ │ │ │ - ["serializeToBinaryString", "a00134.html#a7fad959f63f2eac1478c5ede7b9ae4db", null], │ │ │ │ │ - ["serializeToFile", "a00134.html#aa3261c2d455f892dae1921f8c3a39267", null], │ │ │ │ │ - ["serializeToStream", "a00134.html#a007080ca4741cc3d3d6a209b0ae3fe61", null], │ │ │ │ │ - ["serializeToString", "a00134.html#aa12c78301a9d2968afe5a1a14baa7e90", null], │ │ │ │ │ - ["serializeToXMLFile", "a00134.html#a89449123a746480a1d668cecb5576708", null], │ │ │ │ │ - ["serializeToXMLStream", "a00134.html#ae2e939177cf58b42d7ed4c9856d536c5", null], │ │ │ │ │ - ["serializeToXMLString", "a00134.html#a16c154fe5e35242e26af8286e77eea11", null], │ │ │ │ │ - ["serializeXML", "a00134.html#a976ff7999fcc13ef602b6ece1b078bca", null] │ │ │ │ │ + ["gtsam::WeightedSampler< Engine >", "a02664.html", "a02664"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00134_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/serialization.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/WeightedSampler.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,319 +98,152 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
serialization.h
│ │ │ │ +
WeightedSampler.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <Eigen/Core>
│ │ │ │ -
23#include <fstream>
│ │ │ │ -
24#include <sstream>
│ │ │ │ -
25#include <string>
│ │ │ │ -
26
│ │ │ │ -
27// includes for standard serialization types
│ │ │ │ -
28#include <boost/serialization/version.hpp>
│ │ │ │ -
29#include <boost/serialization/optional.hpp>
│ │ │ │ -
30#include <boost/serialization/shared_ptr.hpp>
│ │ │ │ -
31#include <boost/serialization/vector.hpp>
│ │ │ │ -
32#include <boost/serialization/map.hpp>
│ │ │ │ -
33#include <boost/serialization/list.hpp>
│ │ │ │ -
34#include <boost/serialization/deque.hpp>
│ │ │ │ -
35#include <boost/serialization/weak_ptr.hpp>
│ │ │ │ -
36
│ │ │ │ -
37#include <boost/archive/text_oarchive.hpp>
│ │ │ │ -
38#include <boost/archive/text_iarchive.hpp>
│ │ │ │ -
39#include <boost/archive/xml_iarchive.hpp>
│ │ │ │ -
40#include <boost/archive/xml_oarchive.hpp>
│ │ │ │ -
41#include <boost/archive/binary_iarchive.hpp>
│ │ │ │ -
42#include <boost/archive/binary_oarchive.hpp>
│ │ │ │ -
43#include <boost/serialization/export.hpp>
│ │ │ │ -
44
│ │ │ │ -
45// Workaround a bug in GCC >= 7 and C++17
│ │ │ │ -
46// ref. https://gitlab.com/libeigen/eigen/-/issues/1676
│ │ │ │ -
47#ifdef __GNUC__
│ │ │ │ -
48#if __GNUC__ >= 7 && __cplusplus >= 201703L
│ │ │ │ -
49namespace boost { namespace serialization { struct U; } }
│ │ │ │ -
50namespace Eigen { namespace internal {
│ │ │ │ -
51template<> struct traits<boost::serialization::U> {enum {Flags=0};};
│ │ │ │ -
52} }
│ │ │ │ -
53#endif
│ │ │ │ -
54#endif
│ │ │ │ -
55
│ │ │ │ -
56namespace gtsam {
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <cmath>
│ │ │ │ +
22#include <queue>
│ │ │ │ +
23#include <random>
│ │ │ │ +
24#include <stdexcept>
│ │ │ │ +
25#include <utility>
│ │ │ │ +
26#include <vector>
│ │ │ │ +
27
│ │ │ │ +
28namespace gtsam {
│ │ │ │ +
29/*
│ │ │ │ +
30 * Fast sampling without replacement.
│ │ │ │ +
31 * Example usage:
│ │ │ │ +
32 * std::mt19937 rng(42);
│ │ │ │ +
33 * WeightedSampler<std::mt19937> sampler(&rng);
│ │ │ │ +
34 * auto samples = sampler.sampleWithoutReplacement(5, weights);
│ │ │ │ +
35 */
│ │ │ │ +
36template <class Engine = std::mt19937>
│ │ │ │ +
│ │ │ │ +
37class WeightedSampler {
│ │ │ │ +
38 private:
│ │ │ │ +
39 Engine* engine_; // random number generation engine
│ │ │ │ +
40
│ │ │ │ +
41 public:
│ │ │ │ +
46 explicit WeightedSampler(Engine* engine) : engine_(engine) {}
│ │ │ │ +
47
│ │ │ │ +
48 std::vector<size_t> sampleWithoutReplacement(
│ │ │ │ +
49 size_t numSamples, const std::vector<double>& weights) {
│ │ │ │ +
50 // Implementation adapted from code accompanying paper at
│ │ │ │ +
51 // https://www.ethz.ch/content/dam/ethz/special-interest/baug/ivt/ivt-dam/vpl/reports/1101-1200/ab1141.pdf
│ │ │ │ +
52 const size_t n = weights.size();
│ │ │ │ +
53 if (n < numSamples) {
│ │ │ │ +
54 throw std::runtime_error(
│ │ │ │ +
55 "numSamples must be smaller than weights.size()");
│ │ │ │ +
56 }
│ │ │ │
57
│ │ │ │ -
63template <class T>
│ │ │ │ -
│ │ │ │ -
64void serializeToStream(const T& input, std::ostream& out_archive_stream) {
│ │ │ │ -
65 boost::archive::text_oarchive out_archive(out_archive_stream);
│ │ │ │ -
66 out_archive << input;
│ │ │ │ -
67}
│ │ │ │ -
│ │ │ │ +
58 // Return empty array if numSamples==0
│ │ │ │ +
59 std::vector<size_t> result(numSamples);
│ │ │ │ +
60 if (numSamples == 0) return result;
│ │ │ │ +
61
│ │ │ │ +
62 // Step 1: The first m items of V are inserted into reservoir
│ │ │ │ +
63 // Step 2: For each item v_i ∈ reservoir: Calculate a key k_i = u_i^(1/w),
│ │ │ │ +
64 // where u_i = random(0, 1)
│ │ │ │ +
65 // (Modification: Calculate and store -log k_i = e_i / w where e_i = exp(1),
│ │ │ │ +
66 // reservoir is a priority queue that pops the *maximum* elements)
│ │ │ │ +
67 std::priority_queue<std::pair<double, size_t> > reservoir;
│ │ │ │
68
│ │ │ │ -
70template <class T>
│ │ │ │ -
│ │ │ │ -
71void deserializeFromStream(std::istream& in_archive_stream, T& output) {
│ │ │ │ -
72 boost::archive::text_iarchive in_archive(in_archive_stream);
│ │ │ │ -
73 in_archive >> output;
│ │ │ │ -
74}
│ │ │ │ -
│ │ │ │ -
75
│ │ │ │ -
77template <class T>
│ │ │ │ -
│ │ │ │ -
78std::string serializeToString(const T& input) {
│ │ │ │ -
79 std::ostringstream out_archive_stream;
│ │ │ │ -
80 serializeToStream(input, out_archive_stream);
│ │ │ │ -
81 return out_archive_stream.str();
│ │ │ │ -
82}
│ │ │ │ -
│ │ │ │ -
83
│ │ │ │ -
85template <class T>
│ │ │ │ -
│ │ │ │ -
86void deserializeFromString(const std::string& serialized, T& output) {
│ │ │ │ -
87 std::istringstream in_archive_stream(serialized);
│ │ │ │ -
88 deserializeFromStream(in_archive_stream, output);
│ │ │ │ -
89}
│ │ │ │ -
│ │ │ │ -
90
│ │ │ │ -
92template <class T>
│ │ │ │ -
│ │ │ │ -
93bool serializeToFile(const T& input, const std::string& filename) {
│ │ │ │ -
94 std::ofstream out_archive_stream(filename.c_str());
│ │ │ │ -
95 if (!out_archive_stream.is_open()) return false;
│ │ │ │ -
96 serializeToStream(input, out_archive_stream);
│ │ │ │ -
97 out_archive_stream.close();
│ │ │ │ -
98 return true;
│ │ │ │ -
99}
│ │ │ │ -
│ │ │ │ -
100
│ │ │ │ -
102template <class T>
│ │ │ │ -
│ │ │ │ -
103bool deserializeFromFile(const std::string& filename, T& output) {
│ │ │ │ -
104 std::ifstream in_archive_stream(filename.c_str());
│ │ │ │ -
105 if (!in_archive_stream.is_open()) return false;
│ │ │ │ -
106 deserializeFromStream(in_archive_stream, output);
│ │ │ │ -
107 in_archive_stream.close();
│ │ │ │ -
108 return true;
│ │ │ │ -
109}
│ │ │ │ -
│ │ │ │ -
110
│ │ │ │ -
112template <class T>
│ │ │ │ -
│ │ │ │ -
113std::string serialize(const T& input) {
│ │ │ │ -
114 return serializeToString(input);
│ │ │ │ -
115}
│ │ │ │ -
│ │ │ │ +
69 static const double kexp1 = std::exp(1.0);
│ │ │ │ +
70 for (auto it = weights.begin(); it != weights.begin() + numSamples; ++it) {
│ │ │ │ +
71 const double k_i = kexp1 / *it;
│ │ │ │ +
72 reservoir.push(std::make_pair(k_i, it - weights.begin() + 1));
│ │ │ │ +
73 }
│ │ │ │ +
74
│ │ │ │ +
75 // Step 4: Repeat Steps 5–10 until the population is exhausted
│ │ │ │ +
76 {
│ │ │ │ +
77 // Step 3: The threshold T_w is the minimum key of reservoir
│ │ │ │ +
78 // (Modification: This is now the logarithm)
│ │ │ │ +
79 // Step 10: The new threshold T w is the new minimum key of reservoir
│ │ │ │ +
80 const std::pair<double, size_t>& T_w = reservoir.top();
│ │ │ │ +
81
│ │ │ │ +
82 // Incrementing it is part of Step 7
│ │ │ │ +
83 for (auto it = weights.begin() + numSamples; it != weights.end(); ++it) {
│ │ │ │ +
84 // Step 5: Let r = random(0, 1) and X_w = log(r) / log(T_w)
│ │ │ │ +
85 // (Modification: Use e = -exp(1) instead of log(r))
│ │ │ │ +
86 const double X_w = kexp1 / T_w.first;
│ │ │ │ +
87
│ │ │ │ +
88 // Step 6: From the current item v_c skip items until item v_i, such
│ │ │ │ +
89 // that:
│ │ │ │ +
90 double w = 0.0;
│ │ │ │ +
91
│ │ │ │ +
92 // Step 7: w_c + w_{c+1} + ··· + w_{i−1} < X_w <= w_c + w_{c+1} + ··· +
│ │ │ │ +
93 // w_{i−1} + w_i
│ │ │ │ +
94 for (; it != weights.end(); ++it) {
│ │ │ │ +
95 w += *it;
│ │ │ │ +
96 if (X_w <= w) break;
│ │ │ │ +
97 }
│ │ │ │ +
98
│ │ │ │ +
99 // Step 7: No such item, terminate
│ │ │ │ +
100 if (it == weights.end()) break;
│ │ │ │ +
101
│ │ │ │ +
102 // Step 9: Let t_w = T_w^{w_i}, r_2 = random(t_w, 1) and v_i’s key: k_i
│ │ │ │ +
103 // = (r_2)^{1/w_i} (Mod: Let t_w = log(T_w) * {w_i}, e_2 =
│ │ │ │ +
104 // log(random(e^{t_w}, 1)) and v_i’s key: k_i = -e_2 / w_i)
│ │ │ │ +
105 const double t_w = -T_w.first * *it;
│ │ │ │ +
106 std::uniform_real_distribution<double> randomAngle(std::exp(t_w), 1.0);
│ │ │ │ +
107 const double e_2 = std::log(randomAngle(*engine_));
│ │ │ │ +
108 const double k_i = -e_2 / *it;
│ │ │ │ +
109
│ │ │ │ +
110 // Step 8: The item in reservoir with the minimum key is replaced by
│ │ │ │ +
111 // item v_i
│ │ │ │ +
112 reservoir.pop();
│ │ │ │ +
113 reservoir.push(std::make_pair(k_i, it - weights.begin() + 1));
│ │ │ │ +
114 }
│ │ │ │ +
115 }
│ │ │ │
116
│ │ │ │ -
118template <class T>
│ │ │ │ -
│ │ │ │ -
119void deserialize(const std::string& serialized, T& output) {
│ │ │ │ -
120 deserializeFromString(serialized, output);
│ │ │ │ -
121}
│ │ │ │ -
│ │ │ │ -
123
│ │ │ │ -
129template <class T>
│ │ │ │ -
│ │ │ │ -
130void serializeToXMLStream(const T& input, std::ostream& out_archive_stream,
│ │ │ │ -
131 const std::string& name = "data") {
│ │ │ │ -
132 boost::archive::xml_oarchive out_archive(out_archive_stream);
│ │ │ │ -
133 out_archive << boost::serialization::make_nvp(name.c_str(), input);
│ │ │ │ -
134}
│ │ │ │ -
│ │ │ │ -
135
│ │ │ │ -
137template <class T>
│ │ │ │ -
│ │ │ │ -
138void deserializeFromXMLStream(std::istream& in_archive_stream, T& output,
│ │ │ │ -
139 const std::string& name = "data") {
│ │ │ │ -
140 boost::archive::xml_iarchive in_archive(in_archive_stream);
│ │ │ │ -
141 in_archive >> boost::serialization::make_nvp(name.c_str(), output);
│ │ │ │ -
142}
│ │ │ │ -
│ │ │ │ -
143
│ │ │ │ -
145template <class T>
│ │ │ │ -
│ │ │ │ -
146std::string serializeToXMLString(const T& input,
│ │ │ │ -
147 const std::string& name = "data") {
│ │ │ │ -
148 std::ostringstream out_archive_stream;
│ │ │ │ -
149 serializeToXMLStream(input, out_archive_stream, name);
│ │ │ │ -
150 return out_archive_stream.str();
│ │ │ │ -
151}
│ │ │ │ -
│ │ │ │ -
152
│ │ │ │ -
154template <class T>
│ │ │ │ -
│ │ │ │ -
155void deserializeFromXMLString(const std::string& serialized, T& output,
│ │ │ │ -
156 const std::string& name = "data") {
│ │ │ │ -
157 std::istringstream in_archive_stream(serialized);
│ │ │ │ -
158 deserializeFromXMLStream(in_archive_stream, output, name);
│ │ │ │ -
159}
│ │ │ │ -
│ │ │ │ -
160
│ │ │ │ -
162template <class T>
│ │ │ │ -
│ │ │ │ -
163bool serializeToXMLFile(const T& input, const std::string& filename,
│ │ │ │ -
164 const std::string& name = "data") {
│ │ │ │ -
165 std::ofstream out_archive_stream(filename.c_str());
│ │ │ │ -
166 if (!out_archive_stream.is_open()) return false;
│ │ │ │ -
167 serializeToXMLStream(input, out_archive_stream, name);
│ │ │ │ -
168 out_archive_stream.close();
│ │ │ │ -
169 return true;
│ │ │ │ -
170}
│ │ │ │ -
│ │ │ │ -
171
│ │ │ │ -
173template <class T>
│ │ │ │ -
│ │ │ │ -
174bool deserializeFromXMLFile(const std::string& filename, T& output,
│ │ │ │ -
175 const std::string& name = "data") {
│ │ │ │ -
176 std::ifstream in_archive_stream(filename.c_str());
│ │ │ │ -
177 if (!in_archive_stream.is_open()) return false;
│ │ │ │ -
178 deserializeFromXMLStream(in_archive_stream, output, name);
│ │ │ │ -
179 in_archive_stream.close();
│ │ │ │ -
180 return true;
│ │ │ │ -
181}
│ │ │ │ -
│ │ │ │ -
182
│ │ │ │ -
184template <class T>
│ │ │ │ -
│ │ │ │ -
185std::string serializeXML(const T& input,
│ │ │ │ -
186 const std::string& name = "data") {
│ │ │ │ -
187 return serializeToXMLString(input, name);
│ │ │ │ -
188}
│ │ │ │ -
│ │ │ │ -
189
│ │ │ │ -
191template <class T>
│ │ │ │ -
│ │ │ │ -
192void deserializeXML(const std::string& serialized, T& output,
│ │ │ │ -
193 const std::string& name = "data") {
│ │ │ │ -
194 deserializeFromXMLString(serialized, output, name);
│ │ │ │ -
195}
│ │ │ │ -
│ │ │ │ -
197
│ │ │ │ -
203template <class T>
│ │ │ │ -
│ │ │ │ -
204void serializeToBinaryStream(const T& input, std::ostream& out_archive_stream,
│ │ │ │ -
205 const std::string& name = "data") {
│ │ │ │ -
206 boost::archive::binary_oarchive out_archive(out_archive_stream);
│ │ │ │ -
207 out_archive << boost::serialization::make_nvp(name.c_str(), input);
│ │ │ │ -
208}
│ │ │ │ -
│ │ │ │ -
209
│ │ │ │ -
211template <class T>
│ │ │ │ -
│ │ │ │ -
212void deserializeFromBinaryStream(std::istream& in_archive_stream, T& output,
│ │ │ │ -
213 const std::string& name = "data") {
│ │ │ │ -
214 boost::archive::binary_iarchive in_archive(in_archive_stream);
│ │ │ │ -
215 in_archive >> boost::serialization::make_nvp(name.c_str(), output);
│ │ │ │ -
216}
│ │ │ │ -
│ │ │ │ -
217
│ │ │ │ -
219template <class T>
│ │ │ │ -
│ │ │ │ -
220std::string serializeToBinaryString(const T& input,
│ │ │ │ -
221 const std::string& name = "data") {
│ │ │ │ -
222 std::ostringstream out_archive_stream;
│ │ │ │ -
223 serializeToBinaryStream(input, out_archive_stream, name);
│ │ │ │ -
224 return out_archive_stream.str();
│ │ │ │ -
225}
│ │ │ │ -
│ │ │ │ -
226
│ │ │ │ -
228template <class T>
│ │ │ │ -
│ │ │ │ -
229void deserializeFromBinaryString(const std::string& serialized, T& output,
│ │ │ │ -
230 const std::string& name = "data") {
│ │ │ │ -
231 std::istringstream in_archive_stream(serialized);
│ │ │ │ -
232 deserializeFromBinaryStream(in_archive_stream, output, name);
│ │ │ │ -
233}
│ │ │ │ -
│ │ │ │ -
234
│ │ │ │ -
236template <class T>
│ │ │ │ -
│ │ │ │ -
237bool serializeToBinaryFile(const T& input, const std::string& filename,
│ │ │ │ -
238 const std::string& name = "data") {
│ │ │ │ -
239 std::ofstream out_archive_stream(filename.c_str());
│ │ │ │ -
240 if (!out_archive_stream.is_open()) return false;
│ │ │ │ -
241 serializeToBinaryStream(input, out_archive_stream, name);
│ │ │ │ -
242 out_archive_stream.close();
│ │ │ │ -
243 return true;
│ │ │ │ -
244}
│ │ │ │ -
│ │ │ │ -
245
│ │ │ │ -
247template <class T>
│ │ │ │ -
│ │ │ │ -
248bool deserializeFromBinaryFile(const std::string& filename, T& output,
│ │ │ │ -
249 const std::string& name = "data") {
│ │ │ │ -
250 std::ifstream in_archive_stream(filename.c_str());
│ │ │ │ -
251 if (!in_archive_stream.is_open()) return false;
│ │ │ │ -
252 deserializeFromBinaryStream(in_archive_stream, output, name);
│ │ │ │ -
253 in_archive_stream.close();
│ │ │ │ -
254 return true;
│ │ │ │ -
255}
│ │ │ │ -
│ │ │ │ -
256
│ │ │ │ -
258template <class T>
│ │ │ │ -
│ │ │ │ -
259std::string serializeBinary(const T& input,
│ │ │ │ -
260 const std::string& name = "data") {
│ │ │ │ -
261 return serializeToBinaryString(input, name);
│ │ │ │ -
262}
│ │ │ │ -
│ │ │ │ -
263
│ │ │ │ -
265template <class T>
│ │ │ │ -
│ │ │ │ -
266void deserializeBinary(const std::string& serialized, T& output,
│ │ │ │ -
267 const std::string& name = "data") {
│ │ │ │ -
268 deserializeFromBinaryString(serialized, output, name);
│ │ │ │ -
269}
│ │ │ │ +
117 for (auto iret = result.end(); iret != result.begin();) {
│ │ │ │ +
118 --iret;
│ │ │ │ +
119
│ │ │ │ +
120 if (reservoir.empty()) {
│ │ │ │ +
121 throw std::runtime_error(
│ │ │ │ +
122 "Reservoir empty before all elements have been filled");
│ │ │ │ +
123 }
│ │ │ │ +
124
│ │ │ │ +
125 *iret = reservoir.top().second - 1;
│ │ │ │ +
126 reservoir.pop();
│ │ │ │ +
127 }
│ │ │ │ +
128
│ │ │ │ +
129 if (!reservoir.empty()) {
│ │ │ │ +
130 throw std::runtime_error(
│ │ │ │ +
131 "Reservoir not empty after all elements have been filled");
│ │ │ │ +
132 }
│ │ │ │ +
133
│ │ │ │ +
134 return result;
│ │ │ │ +
135 }
│ │ │ │ +
136}; // namespace gtsam
│ │ │ │
│ │ │ │ -
271
│ │ │ │ -
272} // namespace gtsam
│ │ │ │ +
137} // namespace gtsam
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::serializeToStream
void serializeToStream(const T &input, std::ostream &out_archive_stream)
Definition serialization.h:64
│ │ │ │ -
gtsam::deserializeFromXMLFile
bool deserializeFromXMLFile(const std::string &filename, T &output, const std::string &name="data")
deserializes from an XML file
Definition serialization.h:174
│ │ │ │ -
gtsam::serializeToXMLString
std::string serializeToXMLString(const T &input, const std::string &name="data")
serializes to a string in XML
Definition serialization.h:146
│ │ │ │ -
gtsam::deserializeFromString
void deserializeFromString(const std::string &serialized, T &output)
deserializes from a string
Definition serialization.h:86
│ │ │ │ -
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition serialization.h:113
│ │ │ │ -
gtsam::deserializeFromBinaryString
void deserializeFromBinaryString(const std::string &serialized, T &output, const std::string &name="data")
deserializes from a string in binary
Definition serialization.h:229
│ │ │ │ -
gtsam::deserializeBinary
void deserializeBinary(const std::string &serialized, T &output, const std::string &name="data")
deserializes from a string in binary
Definition serialization.h:266
│ │ │ │ -
gtsam::deserializeFromXMLString
void deserializeFromXMLString(const std::string &serialized, T &output, const std::string &name="data")
deserializes from a string in XML
Definition serialization.h:155
│ │ │ │ -
gtsam::serializeToBinaryString
std::string serializeToBinaryString(const T &input, const std::string &name="data")
serializes to a string in binary
Definition serialization.h:220
│ │ │ │ -
gtsam::deserializeXML
void deserializeXML(const std::string &serialized, T &output, const std::string &name="data")
deserializes from a string in XML
Definition serialization.h:192
│ │ │ │ -
gtsam::serializeToXMLFile
bool serializeToXMLFile(const T &input, const std::string &filename, const std::string &name="data")
serializes to an XML file
Definition serialization.h:163
│ │ │ │ -
gtsam::deserializeFromXMLStream
void deserializeFromXMLStream(std::istream &in_archive_stream, T &output, const std::string &name="data")
deserializes from a stream in XML
Definition serialization.h:138
│ │ │ │ -
gtsam::deserializeFromBinaryStream
void deserializeFromBinaryStream(std::istream &in_archive_stream, T &output, const std::string &name="data")
deserializes from a stream in binary
Definition serialization.h:212
│ │ │ │ -
gtsam::serializeXML
std::string serializeXML(const T &input, const std::string &name="data")
serializes to a string in XML
Definition serialization.h:185
│ │ │ │ -
gtsam::serializeToString
std::string serializeToString(const T &input)
serializes to a string
Definition serialization.h:78
│ │ │ │ -
gtsam::serializeToFile
bool serializeToFile(const T &input, const std::string &filename)
serializes to a file
Definition serialization.h:93
│ │ │ │ -
gtsam::deserialize
void deserialize(const std::string &serialized, T &output)
deserializes from a string
Definition serialization.h:119
│ │ │ │ -
gtsam::serializeToBinaryStream
void serializeToBinaryStream(const T &input, std::ostream &out_archive_stream, const std::string &name="data")
Definition serialization.h:204
│ │ │ │ -
gtsam::deserializeFromStream
void deserializeFromStream(std::istream &in_archive_stream, T &output)
deserializes from a stream
Definition serialization.h:71
│ │ │ │ -
gtsam::serializeBinary
std::string serializeBinary(const T &input, const std::string &name="data")
serializes to a string in binary
Definition serialization.h:259
│ │ │ │ -
gtsam::serializeToBinaryFile
bool serializeToBinaryFile(const T &input, const std::string &filename, const std::string &name="data")
serializes to a binary file
Definition serialization.h:237
│ │ │ │ -
gtsam::deserializeFromFile
bool deserializeFromFile(const std::string &filename, T &output)
deserializes from a file
Definition serialization.h:103
│ │ │ │ -
gtsam::serializeToXMLStream
void serializeToXMLStream(const T &input, std::ostream &out_archive_stream, const std::string &name="data")
Definition serialization.h:130
│ │ │ │ -
gtsam::deserializeFromBinaryFile
bool deserializeFromBinaryFile(const std::string &filename, T &output, const std::string &name="data")
deserializes from a binary file
Definition serialization.h:248
│ │ │ │ +
gtsam::WeightedSampler
Definition WeightedSampler.h:37
│ │ │ │ +
gtsam::WeightedSampler::WeightedSampler
WeightedSampler(Engine *engine)
Construct from random number generation engine We only store a pointer to it.
Definition WeightedSampler.h:46
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── encoding │ │ │ │ │ @@ -1 +1 @@ │ │ │ │ │ -us-ascii │ │ │ │ │ +utf-8 │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,349 +1,150 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -serialization.h │ │ │ │ │ +WeightedSampler.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include │ │ │ │ │ -23#include │ │ │ │ │ -24#include │ │ │ │ │ -25#include │ │ │ │ │ -26 │ │ │ │ │ -27// includes for standard serialization types │ │ │ │ │ -28#include │ │ │ │ │ -29#include │ │ │ │ │ -30#include │ │ │ │ │ -31#include │ │ │ │ │ -32#include │ │ │ │ │ -33#include │ │ │ │ │ -34#include │ │ │ │ │ -35#include │ │ │ │ │ -36 │ │ │ │ │ -37#include │ │ │ │ │ -38#include │ │ │ │ │ -39#include │ │ │ │ │ -40#include │ │ │ │ │ -41#include │ │ │ │ │ -42#include │ │ │ │ │ -43#include │ │ │ │ │ -44 │ │ │ │ │ -45// Workaround a bug in GCC >= 7 and C++17 │ │ │ │ │ -46// ref. https://gitlab.com/libeigen/eigen/-/issues/1676 │ │ │ │ │ -47#ifdef __GNUC__ │ │ │ │ │ -48#if __GNUC__ >= 7 && __cplusplus >= 201703L │ │ │ │ │ -49namespace boost { namespace serialization { struct U; } } │ │ │ │ │ -50namespace Eigen { namespace internal { │ │ │ │ │ -51template<> struct traits {enum {Flags=0};}; │ │ │ │ │ -52} } │ │ │ │ │ -53#endif │ │ │ │ │ -54#endif │ │ │ │ │ -55 │ │ │ │ │ -56namespace _g_t_s_a_m { │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include │ │ │ │ │ +22#include │ │ │ │ │ +23#include │ │ │ │ │ +24#include │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ +27 │ │ │ │ │ +28namespace _g_t_s_a_m { │ │ │ │ │ +29/* │ │ │ │ │ +30 * Fast sampling without replacement. │ │ │ │ │ +31 * Example usage: │ │ │ │ │ +32 * std::mt19937 rng(42); │ │ │ │ │ +33 * WeightedSampler sampler(&rng); │ │ │ │ │ +34 * auto samples = sampler.sampleWithoutReplacement(5, weights); │ │ │ │ │ +35 */ │ │ │ │ │ +36template │ │ │ │ │ +_3_7class _W_e_i_g_h_t_e_d_S_a_m_p_l_e_r { │ │ │ │ │ +38 private: │ │ │ │ │ +39 Engine* engine_; // random number generation engine │ │ │ │ │ +40 │ │ │ │ │ +41 public: │ │ │ │ │ +_4_6 explicit _W_e_i_g_h_t_e_d_S_a_m_p_l_e_r(Engine* engine) : engine_(engine) {} │ │ │ │ │ +47 │ │ │ │ │ +48 std::vector sampleWithoutReplacement( │ │ │ │ │ +49 size_t numSamples, const std::vector& weights) { │ │ │ │ │ +50 // Implementation adapted from code accompanying paper at │ │ │ │ │ +51 // https://www.ethz.ch/content/dam/ethz/special-interest/baug/ivt/ivt-dam/ │ │ │ │ │ +vpl/reports/1101-1200/ab1141.pdf │ │ │ │ │ +52 const size_t n = weights.size(); │ │ │ │ │ +53 if (n < numSamples) { │ │ │ │ │ +54 throw std::runtime_error( │ │ │ │ │ +55 "numSamples must be smaller than weights.size()"); │ │ │ │ │ +56 } │ │ │ │ │ 57 │ │ │ │ │ -63template │ │ │ │ │ -_6_4void _s_e_r_i_a_l_i_z_e_T_o_S_t_r_e_a_m(const T& input, std::ostream& out_archive_stream) { │ │ │ │ │ -65 boost::archive::text_oarchive out_archive(out_archive_stream); │ │ │ │ │ -66 out_archive << input; │ │ │ │ │ -67} │ │ │ │ │ +58 // Return empty array if numSamples==0 │ │ │ │ │ +59 std::vector result(numSamples); │ │ │ │ │ +60 if (numSamples == 0) return result; │ │ │ │ │ +61 │ │ │ │ │ +62 // Step 1: The first m items of V are inserted into reservoir │ │ │ │ │ +63 // Step 2: For each item v_i ∈ reservoir: Calculate a key k_i = u_i^(1/w), │ │ │ │ │ +64 // where u_i = random(0, 1) │ │ │ │ │ +65 // (Modification: Calculate and store -log k_i = e_i / w where e_i = exp(1), │ │ │ │ │ +66 // reservoir is a priority queue that pops the *maximum* elements) │ │ │ │ │ +67 std::priority_queue > reservoir; │ │ │ │ │ 68 │ │ │ │ │ -70template │ │ │ │ │ -_7_1void _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_S_t_r_e_a_m(std::istream& in_archive_stream, T& output) { │ │ │ │ │ -72 boost::archive::text_iarchive in_archive(in_archive_stream); │ │ │ │ │ -73 in_archive >> output; │ │ │ │ │ -74} │ │ │ │ │ -75 │ │ │ │ │ -77template │ │ │ │ │ -_7_8std::string _s_e_r_i_a_l_i_z_e_T_o_S_t_r_i_n_g(const T& input) { │ │ │ │ │ -79 std::ostringstream out_archive_stream; │ │ │ │ │ -80 _s_e_r_i_a_l_i_z_e_T_o_S_t_r_e_a_m(input, out_archive_stream); │ │ │ │ │ -81 return out_archive_stream.str(); │ │ │ │ │ -82} │ │ │ │ │ -83 │ │ │ │ │ -85template │ │ │ │ │ -_8_6void _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_S_t_r_i_n_g(const std::string& serialized, T& output) { │ │ │ │ │ -87 std::istringstream in_archive_stream(serialized); │ │ │ │ │ -88 _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_S_t_r_e_a_m(in_archive_stream, output); │ │ │ │ │ -89} │ │ │ │ │ -90 │ │ │ │ │ -92template │ │ │ │ │ -_9_3bool _s_e_r_i_a_l_i_z_e_T_o_F_i_l_e(const T& input, const std::string& filename) { │ │ │ │ │ -94 std::ofstream out_archive_stream(filename.c_str()); │ │ │ │ │ -95 if (!out_archive_stream.is_open()) return false; │ │ │ │ │ -96 _s_e_r_i_a_l_i_z_e_T_o_S_t_r_e_a_m(input, out_archive_stream); │ │ │ │ │ -97 out_archive_stream.close(); │ │ │ │ │ -98 return true; │ │ │ │ │ -99} │ │ │ │ │ -100 │ │ │ │ │ -102template │ │ │ │ │ -_1_0_3bool _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_F_i_l_e(const std::string& filename, T& output) { │ │ │ │ │ -104 std::ifstream in_archive_stream(filename.c_str()); │ │ │ │ │ -105 if (!in_archive_stream.is_open()) return false; │ │ │ │ │ -106 _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_S_t_r_e_a_m(in_archive_stream, output); │ │ │ │ │ -107 in_archive_stream.close(); │ │ │ │ │ -108 return true; │ │ │ │ │ -109} │ │ │ │ │ -110 │ │ │ │ │ -112template │ │ │ │ │ -_1_1_3std::string _s_e_r_i_a_l_i_z_e(const T& input) { │ │ │ │ │ -114 return _s_e_r_i_a_l_i_z_e_T_o_S_t_r_i_n_g(input); │ │ │ │ │ -115} │ │ │ │ │ +69 static const double kexp1 = std::exp(1.0); │ │ │ │ │ +70 for (auto it = weights.begin(); it != weights.begin() + numSamples; ++it) { │ │ │ │ │ +71 const double k_i = kexp1 / *it; │ │ │ │ │ +72 reservoir.push(std::make_pair(k_i, it - weights.begin() + 1)); │ │ │ │ │ +73 } │ │ │ │ │ +74 │ │ │ │ │ +75 // Step 4: Repeat Steps 5–10 until the population is exhausted │ │ │ │ │ +76 { │ │ │ │ │ +77 // Step 3: The threshold T_w is the minimum key of reservoir │ │ │ │ │ +78 // (Modification: This is now the logarithm) │ │ │ │ │ +79 // Step 10: The new threshold T w is the new minimum key of reservoir │ │ │ │ │ +80 const std::pair& T_w = reservoir.top(); │ │ │ │ │ +81 │ │ │ │ │ +82 // Incrementing it is part of Step 7 │ │ │ │ │ +83 for (auto it = weights.begin() + numSamples; it != weights.end(); ++it) { │ │ │ │ │ +84 // Step 5: Let r = random(0, 1) and X_w = log(r) / log(T_w) │ │ │ │ │ +85 // (Modification: Use e = -exp(1) instead of log(r)) │ │ │ │ │ +86 const double X_w = kexp1 / T_w.first; │ │ │ │ │ +87 │ │ │ │ │ +88 // Step 6: From the current item v_c skip items until item v_i, such │ │ │ │ │ +89 // that: │ │ │ │ │ +90 double w = 0.0; │ │ │ │ │ +91 │ │ │ │ │ +92 // Step 7: w_c + w_{c+1} + ··· + w_{i−1} < X_w <= w_c + w_{c+1} + │ │ │ │ │ +··· + │ │ │ │ │ +93 // w_{i−1} + w_i │ │ │ │ │ +94 for (; it != weights.end(); ++it) { │ │ │ │ │ +95 w += *it; │ │ │ │ │ +96 if (X_w <= w) break; │ │ │ │ │ +97 } │ │ │ │ │ +98 │ │ │ │ │ +99 // Step 7: No such item, terminate │ │ │ │ │ +100 if (it == weights.end()) break; │ │ │ │ │ +101 │ │ │ │ │ +102 // Step 9: Let t_w = T_w^{w_i}, r_2 = random(t_w, 1) and v_i’s key: k_i │ │ │ │ │ +103 // = (r_2)^{1/w_i} (Mod: Let t_w = log(T_w) * {w_i}, e_2 = │ │ │ │ │ +104 // log(random(e^{t_w}, 1)) and v_i’s key: k_i = -e_2 / w_i) │ │ │ │ │ +105 const double t_w = -T_w.first * *it; │ │ │ │ │ +106 std::uniform_real_distribution randomAngle(std::exp(t_w), 1.0); │ │ │ │ │ +107 const double e_2 = std::log(randomAngle(*engine_)); │ │ │ │ │ +108 const double k_i = -e_2 / *it; │ │ │ │ │ +109 │ │ │ │ │ +110 // Step 8: The item in reservoir with the minimum key is replaced by │ │ │ │ │ +111 // item v_i │ │ │ │ │ +112 reservoir.pop(); │ │ │ │ │ +113 reservoir.push(std::make_pair(k_i, it - weights.begin() + 1)); │ │ │ │ │ +114 } │ │ │ │ │ +115 } │ │ │ │ │ 116 │ │ │ │ │ -118template │ │ │ │ │ -_1_1_9void _d_e_s_e_r_i_a_l_i_z_e(const std::string& serialized, T& output) { │ │ │ │ │ -120 _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_S_t_r_i_n_g(serialized, output); │ │ │ │ │ -121} │ │ │ │ │ -123 │ │ │ │ │ -129template │ │ │ │ │ -_1_3_0void _s_e_r_i_a_l_i_z_e_T_o_X_M_L_S_t_r_e_a_m(const T& input, std::ostream& out_archive_stream, │ │ │ │ │ -131 const std::string& name = "data") { │ │ │ │ │ -132 boost::archive::xml_oarchive out_archive(out_archive_stream); │ │ │ │ │ -133 out_archive << boost::serialization::make_nvp(name.c_str(), input); │ │ │ │ │ -134} │ │ │ │ │ -135 │ │ │ │ │ -137template │ │ │ │ │ -_1_3_8void _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_X_M_L_S_t_r_e_a_m(std::istream& in_archive_stream, T& output, │ │ │ │ │ -139 const std::string& name = "data") { │ │ │ │ │ -140 boost::archive::xml_iarchive in_archive(in_archive_stream); │ │ │ │ │ -141 in_archive >> boost::serialization::make_nvp(name.c_str(), output); │ │ │ │ │ -142} │ │ │ │ │ -143 │ │ │ │ │ -145template │ │ │ │ │ -_1_4_6std::string _s_e_r_i_a_l_i_z_e_T_o_X_M_L_S_t_r_i_n_g(const T& input, │ │ │ │ │ -147 const std::string& name = "data") { │ │ │ │ │ -148 std::ostringstream out_archive_stream; │ │ │ │ │ -149 _s_e_r_i_a_l_i_z_e_T_o_X_M_L_S_t_r_e_a_m(input, out_archive_stream, name); │ │ │ │ │ -150 return out_archive_stream.str(); │ │ │ │ │ -151} │ │ │ │ │ -152 │ │ │ │ │ -154template │ │ │ │ │ -_1_5_5void _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_X_M_L_S_t_r_i_n_g(const std::string& serialized, T& output, │ │ │ │ │ -156 const std::string& name = "data") { │ │ │ │ │ -157 std::istringstream in_archive_stream(serialized); │ │ │ │ │ -158 _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_X_M_L_S_t_r_e_a_m(in_archive_stream, output, name); │ │ │ │ │ -159} │ │ │ │ │ -160 │ │ │ │ │ -162template │ │ │ │ │ -_1_6_3bool _s_e_r_i_a_l_i_z_e_T_o_X_M_L_F_i_l_e(const T& input, const std::string& filename, │ │ │ │ │ -164 const std::string& name = "data") { │ │ │ │ │ -165 std::ofstream out_archive_stream(filename.c_str()); │ │ │ │ │ -166 if (!out_archive_stream.is_open()) return false; │ │ │ │ │ -167 _s_e_r_i_a_l_i_z_e_T_o_X_M_L_S_t_r_e_a_m(input, out_archive_stream, name); │ │ │ │ │ -168 out_archive_stream.close(); │ │ │ │ │ -169 return true; │ │ │ │ │ -170} │ │ │ │ │ -171 │ │ │ │ │ -173template │ │ │ │ │ -_1_7_4bool _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_X_M_L_F_i_l_e(const std::string& filename, T& output, │ │ │ │ │ -175 const std::string& name = "data") { │ │ │ │ │ -176 std::ifstream in_archive_stream(filename.c_str()); │ │ │ │ │ -177 if (!in_archive_stream.is_open()) return false; │ │ │ │ │ -178 _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_X_M_L_S_t_r_e_a_m(in_archive_stream, output, name); │ │ │ │ │ -179 in_archive_stream.close(); │ │ │ │ │ -180 return true; │ │ │ │ │ -181} │ │ │ │ │ -182 │ │ │ │ │ -184template │ │ │ │ │ -_1_8_5std::string _s_e_r_i_a_l_i_z_e_X_M_L(const T& input, │ │ │ │ │ -186 const std::string& name = "data") { │ │ │ │ │ -187 return _s_e_r_i_a_l_i_z_e_T_o_X_M_L_S_t_r_i_n_g(input, name); │ │ │ │ │ -188} │ │ │ │ │ -189 │ │ │ │ │ -191template │ │ │ │ │ -_1_9_2void _d_e_s_e_r_i_a_l_i_z_e_X_M_L(const std::string& serialized, T& output, │ │ │ │ │ -193 const std::string& name = "data") { │ │ │ │ │ -194 _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_X_M_L_S_t_r_i_n_g(serialized, output, name); │ │ │ │ │ -195} │ │ │ │ │ -197 │ │ │ │ │ -203template │ │ │ │ │ -_2_0_4void _s_e_r_i_a_l_i_z_e_T_o_B_i_n_a_r_y_S_t_r_e_a_m(const T& input, std::ostream& │ │ │ │ │ -out_archive_stream, │ │ │ │ │ -205 const std::string& name = "data") { │ │ │ │ │ -206 boost::archive::binary_oarchive out_archive(out_archive_stream); │ │ │ │ │ -207 out_archive << boost::serialization::make_nvp(name.c_str(), input); │ │ │ │ │ -208} │ │ │ │ │ -209 │ │ │ │ │ -211template │ │ │ │ │ -_2_1_2void _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_B_i_n_a_r_y_S_t_r_e_a_m(std::istream& in_archive_stream, T& output, │ │ │ │ │ -213 const std::string& name = "data") { │ │ │ │ │ -214 boost::archive::binary_iarchive in_archive(in_archive_stream); │ │ │ │ │ -215 in_archive >> boost::serialization::make_nvp(name.c_str(), output); │ │ │ │ │ -216} │ │ │ │ │ -217 │ │ │ │ │ -219template │ │ │ │ │ -_2_2_0std::string _s_e_r_i_a_l_i_z_e_T_o_B_i_n_a_r_y_S_t_r_i_n_g(const T& input, │ │ │ │ │ -221 const std::string& name = "data") { │ │ │ │ │ -222 std::ostringstream out_archive_stream; │ │ │ │ │ -223 _s_e_r_i_a_l_i_z_e_T_o_B_i_n_a_r_y_S_t_r_e_a_m(input, out_archive_stream, name); │ │ │ │ │ -224 return out_archive_stream.str(); │ │ │ │ │ -225} │ │ │ │ │ -226 │ │ │ │ │ -228template │ │ │ │ │ -_2_2_9void _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_B_i_n_a_r_y_S_t_r_i_n_g(const std::string& serialized, T& output, │ │ │ │ │ -230 const std::string& name = "data") { │ │ │ │ │ -231 std::istringstream in_archive_stream(serialized); │ │ │ │ │ -232 _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_B_i_n_a_r_y_S_t_r_e_a_m(in_archive_stream, output, name); │ │ │ │ │ -233} │ │ │ │ │ -234 │ │ │ │ │ -236template │ │ │ │ │ -_2_3_7bool _s_e_r_i_a_l_i_z_e_T_o_B_i_n_a_r_y_F_i_l_e(const T& input, const std::string& filename, │ │ │ │ │ -238 const std::string& name = "data") { │ │ │ │ │ -239 std::ofstream out_archive_stream(filename.c_str()); │ │ │ │ │ -240 if (!out_archive_stream.is_open()) return false; │ │ │ │ │ -241 _s_e_r_i_a_l_i_z_e_T_o_B_i_n_a_r_y_S_t_r_e_a_m(input, out_archive_stream, name); │ │ │ │ │ -242 out_archive_stream.close(); │ │ │ │ │ -243 return true; │ │ │ │ │ -244} │ │ │ │ │ -245 │ │ │ │ │ -247template │ │ │ │ │ -_2_4_8bool _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_B_i_n_a_r_y_F_i_l_e(const std::string& filename, T& output, │ │ │ │ │ -249 const std::string& name = "data") { │ │ │ │ │ -250 std::ifstream in_archive_stream(filename.c_str()); │ │ │ │ │ -251 if (!in_archive_stream.is_open()) return false; │ │ │ │ │ -252 _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_B_i_n_a_r_y_S_t_r_e_a_m(in_archive_stream, output, name); │ │ │ │ │ -253 in_archive_stream.close(); │ │ │ │ │ -254 return true; │ │ │ │ │ -255} │ │ │ │ │ -256 │ │ │ │ │ -258template │ │ │ │ │ -_2_5_9std::string _s_e_r_i_a_l_i_z_e_B_i_n_a_r_y(const T& input, │ │ │ │ │ -260 const std::string& name = "data") { │ │ │ │ │ -261 return _s_e_r_i_a_l_i_z_e_T_o_B_i_n_a_r_y_S_t_r_i_n_g(input, name); │ │ │ │ │ -262} │ │ │ │ │ -263 │ │ │ │ │ -265template │ │ │ │ │ -_2_6_6void _d_e_s_e_r_i_a_l_i_z_e_B_i_n_a_r_y(const std::string& serialized, T& output, │ │ │ │ │ -267 const std::string& name = "data") { │ │ │ │ │ -268 _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_B_i_n_a_r_y_S_t_r_i_n_g(serialized, output, name); │ │ │ │ │ -269} │ │ │ │ │ -271 │ │ │ │ │ -272} // namespace gtsam │ │ │ │ │ +117 for (auto iret = result.end(); iret != result.begin();) { │ │ │ │ │ +118 --iret; │ │ │ │ │ +119 │ │ │ │ │ +120 if (reservoir.empty()) { │ │ │ │ │ +121 throw std::runtime_error( │ │ │ │ │ +122 "Reservoir empty before all elements have been filled"); │ │ │ │ │ +123 } │ │ │ │ │ +124 │ │ │ │ │ +125 *iret = reservoir.top().second - 1; │ │ │ │ │ +126 reservoir.pop(); │ │ │ │ │ +127 } │ │ │ │ │ +128 │ │ │ │ │ +129 if (!reservoir.empty()) { │ │ │ │ │ +130 throw std::runtime_error( │ │ │ │ │ +131 "Reservoir not empty after all elements have been filled"); │ │ │ │ │ +132 } │ │ │ │ │ +133 │ │ │ │ │ +134 return result; │ │ │ │ │ +135 } │ │ │ │ │ +136}; // namespace gtsam │ │ │ │ │ +137} // namespace gtsam │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_S_t_r_e_a_m │ │ │ │ │ -void serializeToStream(const T &input, std::ostream &out_archive_stream) │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_X_M_L_F_i_l_e │ │ │ │ │ -bool deserializeFromXMLFile(const std::string &filename, T &output, const std:: │ │ │ │ │ -string &name="data") │ │ │ │ │ -deserializes from an XML file │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:174 │ │ │ │ │ -_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_X_M_L_S_t_r_i_n_g │ │ │ │ │ -std::string serializeToXMLString(const T &input, const std::string │ │ │ │ │ -&name="data") │ │ │ │ │ -serializes to a string in XML │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:146 │ │ │ │ │ -_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_S_t_r_i_n_g │ │ │ │ │ -void deserializeFromString(const std::string &serialized, T &output) │ │ │ │ │ -deserializes from a string │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e │ │ │ │ │ -std::string serialize(const T &input) │ │ │ │ │ -serializes to a string │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:113 │ │ │ │ │ -_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_B_i_n_a_r_y_S_t_r_i_n_g │ │ │ │ │ -void deserializeFromBinaryString(const std::string &serialized, T &output, │ │ │ │ │ -const std::string &name="data") │ │ │ │ │ -deserializes from a string in binary │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:229 │ │ │ │ │ -_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_B_i_n_a_r_y │ │ │ │ │ -void deserializeBinary(const std::string &serialized, T &output, const std:: │ │ │ │ │ -string &name="data") │ │ │ │ │ -deserializes from a string in binary │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:266 │ │ │ │ │ -_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_X_M_L_S_t_r_i_n_g │ │ │ │ │ -void deserializeFromXMLString(const std::string &serialized, T &output, const │ │ │ │ │ -std::string &name="data") │ │ │ │ │ -deserializes from a string in XML │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:155 │ │ │ │ │ -_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_B_i_n_a_r_y_S_t_r_i_n_g │ │ │ │ │ -std::string serializeToBinaryString(const T &input, const std::string │ │ │ │ │ -&name="data") │ │ │ │ │ -serializes to a string in binary │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:220 │ │ │ │ │ -_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_X_M_L │ │ │ │ │ -void deserializeXML(const std::string &serialized, T &output, const std::string │ │ │ │ │ -&name="data") │ │ │ │ │ -deserializes from a string in XML │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:192 │ │ │ │ │ -_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_X_M_L_F_i_l_e │ │ │ │ │ -bool serializeToXMLFile(const T &input, const std::string &filename, const │ │ │ │ │ -std::string &name="data") │ │ │ │ │ -serializes to an XML file │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:163 │ │ │ │ │ -_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_X_M_L_S_t_r_e_a_m │ │ │ │ │ -void deserializeFromXMLStream(std::istream &in_archive_stream, T &output, const │ │ │ │ │ -std::string &name="data") │ │ │ │ │ -deserializes from a stream in XML │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:138 │ │ │ │ │ -_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_B_i_n_a_r_y_S_t_r_e_a_m │ │ │ │ │ -void deserializeFromBinaryStream(std::istream &in_archive_stream, T &output, │ │ │ │ │ -const std::string &name="data") │ │ │ │ │ -deserializes from a stream in binary │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:212 │ │ │ │ │ -_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_X_M_L │ │ │ │ │ -std::string serializeXML(const T &input, const std::string &name="data") │ │ │ │ │ -serializes to a string in XML │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:185 │ │ │ │ │ -_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_S_t_r_i_n_g │ │ │ │ │ -std::string serializeToString(const T &input) │ │ │ │ │ -serializes to a string │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:78 │ │ │ │ │ -_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_F_i_l_e │ │ │ │ │ -bool serializeToFile(const T &input, const std::string &filename) │ │ │ │ │ -serializes to a file │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:93 │ │ │ │ │ -_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e │ │ │ │ │ -void deserialize(const std::string &serialized, T &output) │ │ │ │ │ -deserializes from a string │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:119 │ │ │ │ │ -_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_B_i_n_a_r_y_S_t_r_e_a_m │ │ │ │ │ -void serializeToBinaryStream(const T &input, std::ostream &out_archive_stream, │ │ │ │ │ -const std::string &name="data") │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:204 │ │ │ │ │ -_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_S_t_r_e_a_m │ │ │ │ │ -void deserializeFromStream(std::istream &in_archive_stream, T &output) │ │ │ │ │ -deserializes from a stream │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:71 │ │ │ │ │ -_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_B_i_n_a_r_y │ │ │ │ │ -std::string serializeBinary(const T &input, const std::string &name="data") │ │ │ │ │ -serializes to a string in binary │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:259 │ │ │ │ │ -_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_B_i_n_a_r_y_F_i_l_e │ │ │ │ │ -bool serializeToBinaryFile(const T &input, const std::string &filename, const │ │ │ │ │ -std::string &name="data") │ │ │ │ │ -serializes to a binary file │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:237 │ │ │ │ │ -_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_F_i_l_e │ │ │ │ │ -bool deserializeFromFile(const std::string &filename, T &output) │ │ │ │ │ -deserializes from a file │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:103 │ │ │ │ │ -_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_X_M_L_S_t_r_e_a_m │ │ │ │ │ -void serializeToXMLStream(const T &input, std::ostream &out_archive_stream, │ │ │ │ │ -const std::string &name="data") │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:130 │ │ │ │ │ -_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_B_i_n_a_r_y_F_i_l_e │ │ │ │ │ -bool deserializeFromBinaryFile(const std::string &filename, T &output, const │ │ │ │ │ -std::string &name="data") │ │ │ │ │ -deserializes from a binary file │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:248 │ │ │ │ │ +_g_t_s_a_m_:_:_W_e_i_g_h_t_e_d_S_a_m_p_l_e_r │ │ │ │ │ +DDeeffiinniittiioonn WeightedSampler.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_W_e_i_g_h_t_e_d_S_a_m_p_l_e_r_:_:_W_e_i_g_h_t_e_d_S_a_m_p_l_e_r │ │ │ │ │ +WeightedSampler(Engine *engine) │ │ │ │ │ +Construct from random number generation engine We only store a pointer to it. │ │ │ │ │ +DDeeffiinniittiioonn WeightedSampler.h:46 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _s_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ + * _W_e_i_g_h_t_e_d_S_a_m_p_l_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00137.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/SymmetricBlockMatrix.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/DSFVector.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,43 +96,46 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
SymmetricBlockMatrix.h File Reference
│ │ │ │ +
DSFVector.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Access to matrices via blocks of pre-defined sizes. │ │ │ │ +

A faster implementation for DSF, which uses vector rather than btree. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::SymmetricBlockMatrix
 This class stores a dense matrix and allows it to be accessed as a collection of blocks. More...
class  gtsam::DSFBase
 A fast implementation of disjoint set forests that uses vector as underly data structure. More...
 
class  gtsam::DSFVector
 DSFVector additionally keeps a vector of keys to support more expensive operations. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Access to matrices via blocks of pre-defined sizes.

│ │ │ │ -

Used in GaussianFactor and GaussianConditional.

Author
Richard Roberts
│ │ │ │ -
Date
Sep 18, 2010
│ │ │ │ +

A faster implementation for DSF, which uses vector rather than btree.

│ │ │ │ +
Date
Jun 25, 2010
│ │ │ │ +
Author
Kai Ni
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,29 +1,32 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SymmetricBlockMatrix.h File Reference │ │ │ │ │ -Access to matrices via blocks of pre-defined sizes. _M_o_r_e_._._. │ │ │ │ │ +DSFVector.h File Reference │ │ │ │ │ +A faster implementation for DSF, which uses vector rather than btree. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -  This class stores a dense matrix and allows it to be accessed as a │ │ │ │ │ - collection of blocks. _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_D_S_F_B_a_s_e │ │ │ │ │ +  A fast implementation of disjoint set forests that uses vector as │ │ │ │ │ + underly data structure. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_D_S_F_V_e_c_t_o_r │ │ │ │ │ +  _D_S_F_V_e_c_t_o_r additionally keeps a vector of keys to support more expensive │ │ │ │ │ + operations. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Access to matrices via blocks of pre-defined sizes. │ │ │ │ │ -Used in GaussianFactor and _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l. │ │ │ │ │ - Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ +A faster implementation for DSF, which uses vector rather than btree. │ │ │ │ │ Date │ │ │ │ │ - Sep 18, 2010 │ │ │ │ │ + Jun 25, 2010 │ │ │ │ │ + Author │ │ │ │ │ + Kai Ni │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_._h │ │ │ │ │ + * _D_S_F_V_e_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00137_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/SymmetricBlockMatrix.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/DSFVector.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,439 +98,96 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
SymmetricBlockMatrix.h
│ │ │ │ +
DSFVector.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3* GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ -
4* Atlanta, Georgia 30332-0415
│ │ │ │ -
5* All Rights Reserved
│ │ │ │ -
6* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
4 * Atlanta, Georgia 30332-0415
│ │ │ │ +
5 * All Rights Reserved
│ │ │ │ +
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │ -
8* See LICENSE for the license information
│ │ │ │ +
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │ -
10* -------------------------------------------------------------------------- */
│ │ │ │ +
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/base/FastVector.h>
│ │ │ │ -
21#include <gtsam/base/Matrix.h>
│ │ │ │ -
22#include <gtsam/base/types.h>
│ │ │ │ -
23#include <gtsam/dllexport.h>
│ │ │ │ -
24#include <boost/serialization/nvp.hpp>
│ │ │ │ -
25#include <cassert>
│ │ │ │ -
26#include <stdexcept>
│ │ │ │ -
27#include <array>
│ │ │ │ -
28
│ │ │ │ -
29namespace boost {
│ │ │ │ -
30namespace serialization {
│ │ │ │ -
31class access;
│ │ │ │ -
32} /* namespace serialization */
│ │ │ │ -
33} /* namespace boost */
│ │ │ │ -
34
│ │ │ │ -
35namespace gtsam {
│ │ │ │ -
36
│ │ │ │ -
37 // Forward declarations
│ │ │ │ -
38 class VerticalBlockMatrix;
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/dllexport.h>
│ │ │ │ +
22#include <gtsam/global_includes.h>
│ │ │ │ +
23
│ │ │ │ +
24#include <boost/shared_ptr.hpp>
│ │ │ │ +
25
│ │ │ │ +
26#include <vector>
│ │ │ │ +
27#include <set>
│ │ │ │ +
28#include <map>
│ │ │ │ +
29
│ │ │ │ +
30namespace gtsam {
│ │ │ │ +
31
│ │ │ │ +
│ │ │ │ +
38class GTSAM_EXPORT DSFBase {
│ │ │ │
39
│ │ │ │ -
│ │ │ │ -
51 class GTSAM_EXPORT SymmetricBlockMatrix
│ │ │ │ -
52 {
│ │ │ │ -
53 public:
│ │ │ │ -
54 typedef SymmetricBlockMatrix This;
│ │ │ │ -
55 typedef Eigen::Block<Matrix> Block;
│ │ │ │ -
56 typedef Eigen::Block<const Matrix> constBlock;
│ │ │ │ -
57
│ │ │ │ -
58 protected:
│ │ │ │ -
59 Matrix matrix_;
│ │ │ │ -
60 FastVector<DenseIndex> variableColOffsets_;
│ │ │ │ -
61
│ │ │ │ -
62 DenseIndex blockStart_;
│ │ │ │ -
63
│ │ │ │ -
64 public:
│ │ │ │ -
66 SymmetricBlockMatrix();
│ │ │ │ -
67
│ │ │ │ -
69 template<typename CONTAINER>
│ │ │ │ -
│ │ │ │ -
70 SymmetricBlockMatrix(const CONTAINER& dimensions, bool appendOneDimension = false) :
│ │ │ │ -
71 blockStart_(0)
│ │ │ │ -
72 {
│ │ │ │ -
73 fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
│ │ │ │ -
74 matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
│ │ │ │ -
75 assertInvariants();
│ │ │ │ -
76 }
│ │ │ │ -
│ │ │ │ -
77
│ │ │ │ -
79 template<typename ITERATOR>
│ │ │ │ -
│ │ │ │ -
80 SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension = false) :
│ │ │ │ -
81 blockStart_(0)
│ │ │ │ -
82 {
│ │ │ │ -
83 fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension);
│ │ │ │ -
84 matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back());
│ │ │ │ -
85 assertInvariants();
│ │ │ │ -
86 }
│ │ │ │ -
│ │ │ │ -
87
│ │ │ │ -
89 template<typename CONTAINER>
│ │ │ │ -
│ │ │ │ -
90 SymmetricBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix, bool appendOneDimension = false) :
│ │ │ │ -
91 blockStart_(0)
│ │ │ │ -
92 {
│ │ │ │ -
93 matrix_.resize(matrix.rows(), matrix.cols());
│ │ │ │ -
94 matrix_.triangularView<Eigen::Upper>() = matrix.triangularView<Eigen::Upper>();
│ │ │ │ -
95 fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
│ │ │ │ -
96 if(matrix_.rows() != matrix_.cols())
│ │ │ │ -
97 throw std::invalid_argument("Requested to create a SymmetricBlockMatrix from a non-square matrix.");
│ │ │ │ -
98 if(variableColOffsets_.back() != matrix_.cols())
│ │ │ │ -
99 throw std::invalid_argument("Requested to create a SymmetricBlockMatrix with dimensions that do not sum to the total size of the provided matrix.");
│ │ │ │ -
100 assertInvariants();
│ │ │ │ -
101 }
│ │ │ │ -
│ │ │ │ -
102
│ │ │ │ -
106 static SymmetricBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix& other);
│ │ │ │ -
107
│ │ │ │ -
111 static SymmetricBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& other);
│ │ │ │ -
112
│ │ │ │ -
114 DenseIndex rows() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; }
│ │ │ │ -
115
│ │ │ │ -
117 DenseIndex cols() const { return rows(); }
│ │ │ │ -
118
│ │ │ │ -
120 DenseIndex nBlocks() const { return nActualBlocks() - blockStart_; }
│ │ │ │ -
121
│ │ │ │ -
│ │ │ │ -
123 DenseIndex getDim(DenseIndex block) const {
│ │ │ │ -
124 return calcIndices(block, block, 1, 1)[2];
│ │ │ │ -
125 }
│ │ │ │ -
│ │ │ │ -
126
│ │ │ │ -
129
│ │ │ │ -
132 Matrix block(DenseIndex I, DenseIndex J) const;
│ │ │ │ -
133
│ │ │ │ -
│ │ │ │ -
135 Eigen::SelfAdjointView<Block, Eigen::Upper> diagonalBlock(DenseIndex J) {
│ │ │ │ -
136 return block_(J, J).selfadjointView<Eigen::Upper>();
│ │ │ │ -
137 }
│ │ │ │ -
│ │ │ │ -
138
│ │ │ │ -
│ │ │ │ -
140 Eigen::SelfAdjointView<constBlock, Eigen::Upper> diagonalBlock(DenseIndex J) const {
│ │ │ │ -
141 return block_(J, J).selfadjointView<Eigen::Upper>();
│ │ │ │ -
142 }
│ │ │ │ -
│ │ │ │ -
143
│ │ │ │ -
│ │ │ │ -
145 Vector diagonal(DenseIndex J) const {
│ │ │ │ -
146 return block_(J, J).diagonal();
│ │ │ │ -
147 }
│ │ │ │ -
│ │ │ │ -
148
│ │ │ │ -
│ │ │ │ -
150 constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const {
│ │ │ │ -
151 assert(I < J);
│ │ │ │ -
152 return block_(I, J);
│ │ │ │ -
153 }
│ │ │ │ -
│ │ │ │ -
154
│ │ │ │ -
│ │ │ │ -
156 Eigen::SelfAdjointView<constBlock, Eigen::Upper> selfadjointView(
│ │ │ │ -
157 DenseIndex I, DenseIndex J) const {
│ │ │ │ -
158 assert(J > I);
│ │ │ │ -
159 return block_(I, I, J - I, J - I).selfadjointView<Eigen::Upper>();
│ │ │ │ -
160 }
│ │ │ │ -
│ │ │ │ -
161
│ │ │ │ -
│ │ │ │ -
163 Eigen::TriangularView<constBlock, Eigen::Upper> triangularView(DenseIndex I,
│ │ │ │ -
164 DenseIndex J) const {
│ │ │ │ -
165 assert(J > I);
│ │ │ │ -
166 return block_(I, I, J - I, J - I).triangularView<Eigen::Upper>();
│ │ │ │ -
167 }
│ │ │ │ -
│ │ │ │ -
168
│ │ │ │ -
│ │ │ │ -
170 constBlock aboveDiagonalRange(DenseIndex i_startBlock,
│ │ │ │ -
171 DenseIndex i_endBlock,
│ │ │ │ -
172 DenseIndex j_startBlock,
│ │ │ │ -
173 DenseIndex j_endBlock) const {
│ │ │ │ -
174 assert(i_startBlock < j_startBlock);
│ │ │ │ -
175 assert(i_endBlock <= j_startBlock);
│ │ │ │ -
176 return block_(i_startBlock, j_startBlock, i_endBlock - i_startBlock,
│ │ │ │ -
177 j_endBlock - j_startBlock);
│ │ │ │ -
178 }
│ │ │ │ -
│ │ │ │ -
179
│ │ │ │ -
│ │ │ │ -
181 Block aboveDiagonalRange(DenseIndex i_startBlock, DenseIndex i_endBlock,
│ │ │ │ -
182 DenseIndex j_startBlock, DenseIndex j_endBlock) {
│ │ │ │ -
183 assert(i_startBlock < j_startBlock);
│ │ │ │ -
184 assert(i_endBlock <= j_startBlock);
│ │ │ │ -
185 return block_(i_startBlock, j_startBlock, i_endBlock - i_startBlock,
│ │ │ │ -
186 j_endBlock - j_startBlock);
│ │ │ │ -
187 }
│ │ │ │ -
│ │ │ │ -
188
│ │ │ │ -
192
│ │ │ │ -
194 template <typename XprType>
│ │ │ │ -
│ │ │ │ -
195 void setDiagonalBlock(DenseIndex I, const XprType& xpr) {
│ │ │ │ -
196 block_(I, I).triangularView<Eigen::Upper>() = xpr.template triangularView<Eigen::Upper>();
│ │ │ │ -
197 }
│ │ │ │ -
│ │ │ │ -
198
│ │ │ │ -
200 template <typename XprType>
│ │ │ │ -
│ │ │ │ -
201 void setOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType& xpr) {
│ │ │ │ -
202 assert(I != J);
│ │ │ │ -
203 if (I < J) {
│ │ │ │ -
204 block_(I, J) = xpr;
│ │ │ │ -
205 } else {
│ │ │ │ -
206 block_(J, I) = xpr.transpose();
│ │ │ │ -
207 }
│ │ │ │ -
208 }
│ │ │ │ -
│ │ │ │ -
209
│ │ │ │ -
211 template <typename XprType>
│ │ │ │ -
│ │ │ │ -
212 void updateDiagonalBlock(DenseIndex I, const XprType& xpr) {
│ │ │ │ -
213 // TODO(gareth): Eigen won't let us add triangular or self-adjoint views
│ │ │ │ -
214 // here, so we do it manually.
│ │ │ │ -
215 auto dest = block_(I, I);
│ │ │ │ -
216 assert(dest.rows() == xpr.rows());
│ │ │ │ -
217 assert(dest.cols() == xpr.cols());
│ │ │ │ -
218 for (DenseIndex col = 0; col < dest.cols(); ++col) {
│ │ │ │ -
219 for (DenseIndex row = 0; row <= col; ++row) {
│ │ │ │ -
220 dest(row, col) += xpr(row, col);
│ │ │ │ -
221 }
│ │ │ │ -
222 }
│ │ │ │ -
223 }
│ │ │ │ -
│ │ │ │ -
224
│ │ │ │ -
227 template <typename XprType>
│ │ │ │ -
│ │ │ │ -
228 void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType& xpr) {
│ │ │ │ -
229 assert(I != J);
│ │ │ │ -
230 if (I < J) {
│ │ │ │ -
231 block_(I, J).noalias() += xpr;
│ │ │ │ -
232 } else {
│ │ │ │ -
233 block_(J, I).noalias() += xpr.transpose();
│ │ │ │ -
234 }
│ │ │ │ -
235 }
│ │ │ │ -
│ │ │ │ -
236
│ │ │ │ -
240
│ │ │ │ -
│ │ │ │ -
242 Eigen::SelfAdjointView<Block, Eigen::Upper> selfadjointView() {
│ │ │ │ -
243 return full().selfadjointView<Eigen::Upper>();
│ │ │ │ -
244 }
│ │ │ │ -
│ │ │ │ -
245
│ │ │ │ -
│ │ │ │ -
247 Eigen::SelfAdjointView<constBlock, Eigen::Upper> selfadjointView() const {
│ │ │ │ -
248 return full().selfadjointView<Eigen::Upper>();
│ │ │ │ -
249 }
│ │ │ │ -
│ │ │ │ -
250
│ │ │ │ -
252 template <typename XprType>
│ │ │ │ -
│ │ │ │ -
253 void setFullMatrix(const XprType& xpr) {
│ │ │ │ -
254 full().triangularView<Eigen::Upper>() = xpr.template triangularView<Eigen::Upper>();
│ │ │ │ -
255 }
│ │ │ │ -
│ │ │ │ -
256
│ │ │ │ -
│ │ │ │ -
258 void setZero() {
│ │ │ │ -
259 full().triangularView<Eigen::Upper>().setZero();
│ │ │ │ -
260 }
│ │ │ │ -
│ │ │ │ -
261
│ │ │ │ -
263 void negate();
│ │ │ │ -
264
│ │ │ │ -
266 void invertInPlace();
│ │ │ │ -
267
│ │ │ │ -
269
│ │ │ │ -
273 DenseIndex& blockStart() { return blockStart_; }
│ │ │ │ -
274
│ │ │ │ -
277 DenseIndex blockStart() const { return blockStart_; }
│ │ │ │ -
278
│ │ │ │ -
289 void choleskyPartial(DenseIndex nFrontals);
│ │ │ │ -
290
│ │ │ │ -
296 VerticalBlockMatrix split(DenseIndex nFrontals);
│ │ │ │ -
297
│ │ │ │ -
298 protected:
│ │ │ │ -
299
│ │ │ │ -
│ │ │ │ -
301 DenseIndex nOffsets() const {
│ │ │ │ -
302 return variableColOffsets_.size();
│ │ │ │ -
303 }
│ │ │ │ -
│ │ │ │ -
304
│ │ │ │ -
│ │ │ │ -
306 DenseIndex nActualBlocks() const {
│ │ │ │ -
307 return nOffsets() - 1;
│ │ │ │ -
308 }
│ │ │ │ -
│ │ │ │ -
309
│ │ │ │ -
│ │ │ │ -
311 DenseIndex offset(DenseIndex block) const {
│ │ │ │ -
312 assert(block >= 0);
│ │ │ │ -
313 const DenseIndex actual_index = block + blockStart();
│ │ │ │ -
314 assert(actual_index < nOffsets());
│ │ │ │ -
315 return variableColOffsets_[actual_index];
│ │ │ │ -
316 }
│ │ │ │ -
│ │ │ │ -
317
│ │ │ │ -
│ │ │ │ -
319 constBlock block_(DenseIndex iBlock, DenseIndex jBlock,
│ │ │ │ -
320 DenseIndex blockRows = 1, DenseIndex blockCols = 1) const {
│ │ │ │ -
321 const std::array<DenseIndex, 4> indices =
│ │ │ │ -
322 calcIndices(iBlock, jBlock, blockRows, blockCols);
│ │ │ │ -
323 return matrix_.block(indices[0], indices[1], indices[2], indices[3]);
│ │ │ │ -
324 }
│ │ │ │ -
│ │ │ │ -
325
│ │ │ │ -
│ │ │ │ -
327 Block block_(DenseIndex iBlock, DenseIndex jBlock, DenseIndex blockRows = 1,
│ │ │ │ -
328 DenseIndex blockCols = 1) {
│ │ │ │ -
329 const std::array<DenseIndex, 4> indices =
│ │ │ │ -
330 calcIndices(iBlock, jBlock, blockRows, blockCols);
│ │ │ │ -
331 return matrix_.block(indices[0], indices[1], indices[2], indices[3]);
│ │ │ │ -
332 }
│ │ │ │ -
│ │ │ │ -
333
│ │ │ │ -
│ │ │ │ -
335 constBlock full() const {
│ │ │ │ -
336 return block_(0, 0, nBlocks(), nBlocks());
│ │ │ │ -
337 }
│ │ │ │ -
│ │ │ │ -
338
│ │ │ │ -
│ │ │ │ -
340 Block full() {
│ │ │ │ -
341 return block_(0, 0, nBlocks(), nBlocks());
│ │ │ │ -
342 }
│ │ │ │ -
│ │ │ │ -
343
│ │ │ │ -
│ │ │ │ -
345 std::array<DenseIndex, 4> calcIndices(DenseIndex iBlock, DenseIndex jBlock,
│ │ │ │ -
346 DenseIndex blockRows,
│ │ │ │ -
347 DenseIndex blockCols) const {
│ │ │ │ -
348 assert(blockRows >= 0);
│ │ │ │ -
349 assert(blockCols >= 0);
│ │ │ │ -
350
│ │ │ │ -
351 // adjust indices to account for start and size of blocks
│ │ │ │ -
352 const DenseIndex denseI = offset(iBlock);
│ │ │ │ -
353 const DenseIndex denseJ = offset(jBlock);
│ │ │ │ -
354 const DenseIndex denseRows = offset(iBlock + blockRows) - denseI;
│ │ │ │ -
355 const DenseIndex denseCols = offset(jBlock + blockCols) - denseJ;
│ │ │ │ -
356 return {{denseI, denseJ, denseRows, denseCols}};
│ │ │ │ -
357 }
│ │ │ │ -
│ │ │ │ -
358
│ │ │ │ -
359 void assertInvariants() const
│ │ │ │ -
360 {
│ │ │ │ -
361 assert(matrix_.rows() == matrix_.cols());
│ │ │ │ -
362 assert(matrix_.cols() == variableColOffsets_.back());
│ │ │ │ -
363 assert(blockStart_ < (DenseIndex)variableColOffsets_.size());
│ │ │ │ -
364 }
│ │ │ │ -
365
│ │ │ │ -
366 template<typename ITERATOR>
│ │ │ │ -
367 void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension)
│ │ │ │ -
368 {
│ │ │ │ -
369 variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + (appendOneDimension ? 1 : 0));
│ │ │ │ -
370 variableColOffsets_[0] = 0;
│ │ │ │ -
371 DenseIndex j=0;
│ │ │ │ -
372 for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) {
│ │ │ │ -
373 variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
│ │ │ │ -
374 ++ j;
│ │ │ │ -
375 }
│ │ │ │ -
376 if(appendOneDimension)
│ │ │ │ -
377 {
│ │ │ │ -
378 variableColOffsets_[j+1] = variableColOffsets_[j] + 1;
│ │ │ │ -
379 ++ j;
│ │ │ │ -
380 }
│ │ │ │ -
381 }
│ │ │ │ -
382
│ │ │ │ -
383 friend class VerticalBlockMatrix;
│ │ │ │ -
384 template<typename SymmetricBlockMatrixType> friend class SymmetricBlockMatrixBlockExpr;
│ │ │ │ -
385
│ │ │ │ -
386 private:
│ │ │ │ -
388 friend class boost::serialization::access;
│ │ │ │ -
389 template<class ARCHIVE>
│ │ │ │ -
390 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
391 // Fill in the lower triangle part of the matrix, so boost::serialization won't
│ │ │ │ -
392 // complain about uninitialized data with an input_stream_error exception
│ │ │ │ -
393 // http://www.boost.org/doc/libs/1_37_0/libs/serialization/doc/exceptions.html#stream_error
│ │ │ │ -
394 matrix_.triangularView<Eigen::Lower>() = matrix_.triangularView<Eigen::Upper>().transpose();
│ │ │ │ -
395 ar & BOOST_SERIALIZATION_NVP(matrix_);
│ │ │ │ -
396 ar & BOOST_SERIALIZATION_NVP(variableColOffsets_);
│ │ │ │ -
397 ar & BOOST_SERIALIZATION_NVP(blockStart_);
│ │ │ │ -
398 }
│ │ │ │ -
399 };
│ │ │ │ -
│ │ │ │ -
400
│ │ │ │ -
402 class CholeskyFailed;
│ │ │ │ -
403
│ │ │ │ -
404}
│ │ │ │ -
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ -
types.h
Typedefs for easier changing of types.
│ │ │ │ -
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ -
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │ +
40public:
│ │ │ │ +
41 typedef std::vector<size_t> V;
│ │ │ │ +
42
│ │ │ │ +
43private:
│ │ │ │ +
44 boost::shared_ptr<V> v_;
│ │ │ │ +
45
│ │ │ │ +
46public:
│ │ │ │ +
48 DSFBase(const size_t numNodes);
│ │ │ │ +
49
│ │ │ │ +
51 DSFBase(const boost::shared_ptr<V>& v_in);
│ │ │ │ +
52
│ │ │ │ +
54 size_t find(size_t key) const;
│ │ │ │ +
55
│ │ │ │ +
57 void merge(const size_t& i1, const size_t& i2);
│ │ │ │ +
58};
│ │ │ │ +
│ │ │ │ +
59
│ │ │ │ +
│ │ │ │ +
64class GTSAM_EXPORT DSFVector: public DSFBase {
│ │ │ │ +
65
│ │ │ │ +
66private:
│ │ │ │ +
67 std::vector<size_t> keys_;
│ │ │ │ +
68
│ │ │ │ +
69public:
│ │ │ │ +
71 DSFVector(const size_t numNodes);
│ │ │ │ +
72
│ │ │ │ +
74 DSFVector(const std::vector<size_t>& keys);
│ │ │ │ +
75
│ │ │ │ +
77 DSFVector(const boost::shared_ptr<V>& v_in, const std::vector<size_t>& keys);
│ │ │ │ +
78
│ │ │ │ +
79 // All operations below loop over all keys and hence are *at least* O(n)
│ │ │ │ +
80
│ │ │ │ +
82 bool isSingleton(const size_t& label) const;
│ │ │ │ +
83
│ │ │ │ +
85 std::set<size_t> set(const size_t& label) const;
│ │ │ │ +
86
│ │ │ │ +
88 std::map<size_t, std::set<size_t> > sets() const;
│ │ │ │ +
89
│ │ │ │ +
91 std::map<size_t, std::vector<size_t> > arrays() const;
│ │ │ │ +
92};
│ │ │ │ +
│ │ │ │ +
93
│ │ │ │ +
94}
│ │ │ │ +
global_includes.h
Included from all GTSAM files.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │ -
gtsam::row
const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j)
Extracts a row view from a matrix that avoids a copy.
Definition Matrix.h:222
│ │ │ │ -
gtsam::split
void split(const G &g, const PredecessorMap< KEY > &tree, G &Ab1, G &Ab2)
Split the graph into two parts: one corresponds to the given spanning tree, and the other corresponds...
Definition graph-inl.h:255
│ │ │ │ -
gtsam::choleskyPartial
bool choleskyPartial(Matrix &ABC, size_t nFrontal, size_t topleft)
Partial Cholesky computes a factor [R S such that [R' 0 [R S = [A B 0 L] S' I] 0 L] B' C].
Definition cholesky.cpp:108
│ │ │ │ -
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
│ │ │ │ -
gtsam::SymmetricBlockMatrix::full
Block full()
Get the full matrix as a block.
Definition SymmetricBlockMatrix.h:340
│ │ │ │ -
gtsam::SymmetricBlockMatrix::blockStart_
DenseIndex blockStart_
Changes apparent matrix view, see main class comment.
Definition SymmetricBlockMatrix.h:62
│ │ │ │ -
gtsam::SymmetricBlockMatrix::setDiagonalBlock
void setDiagonalBlock(DenseIndex I, const XprType &xpr)
Set a diagonal block. Only the upper triangular portion of xpr is evaluated.
Definition SymmetricBlockMatrix.h:195
│ │ │ │ -
gtsam::SymmetricBlockMatrix::nActualBlocks
DenseIndex nActualBlocks() const
Number of actual blocks in the full matrix.
Definition SymmetricBlockMatrix.h:306
│ │ │ │ -
gtsam::SymmetricBlockMatrix::diagonal
Vector diagonal(DenseIndex J) const
Get the diagonal of the J'th diagonal block.
Definition SymmetricBlockMatrix.h:145
│ │ │ │ -
gtsam::SymmetricBlockMatrix::matrix_
Matrix matrix_
The full matrix.
Definition SymmetricBlockMatrix.h:59
│ │ │ │ -
gtsam::SymmetricBlockMatrix::cols
DenseIndex cols() const
Column size.
Definition SymmetricBlockMatrix.h:117
│ │ │ │ -
gtsam::SymmetricBlockMatrix::getDim
DenseIndex getDim(DenseIndex block) const
Number of dimensions for variable on this diagonal block.
Definition SymmetricBlockMatrix.h:123
│ │ │ │ -
gtsam::SymmetricBlockMatrix::setZero
void setZero()
Set the entire active matrix zero.
Definition SymmetricBlockMatrix.h:258
│ │ │ │ -
gtsam::SymmetricBlockMatrix::setOffDiagonalBlock
void setOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
Set an off-diagonal block. Only the upper triangular portion of xpr is evaluated.
Definition SymmetricBlockMatrix.h:201
│ │ │ │ -
gtsam::SymmetricBlockMatrix::setFullMatrix
void setFullMatrix(const XprType &xpr)
Set the entire active matrix. Only reads the upper triangular part of xpr.
Definition SymmetricBlockMatrix.h:253
│ │ │ │ -
gtsam::SymmetricBlockMatrix::selfadjointView
Eigen::SelfAdjointView< Block, Eigen::Upper > selfadjointView()
Get self adjoint view.
Definition SymmetricBlockMatrix.h:242
│ │ │ │ -
gtsam::SymmetricBlockMatrix::aboveDiagonalBlock
constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
Get block above the diagonal (I, J).
Definition SymmetricBlockMatrix.h:150
│ │ │ │ -
gtsam::SymmetricBlockMatrix::block_
constBlock block_(DenseIndex iBlock, DenseIndex jBlock, DenseIndex blockRows=1, DenseIndex blockCols=1) const
Get an arbitrary block from the matrix. Indices are in block units.
Definition SymmetricBlockMatrix.h:319
│ │ │ │ -
gtsam::SymmetricBlockMatrix::full
constBlock full() const
Get the full matrix as a block.
Definition SymmetricBlockMatrix.h:335
│ │ │ │ -
gtsam::SymmetricBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition SymmetricBlockMatrix.h:114
│ │ │ │ -
gtsam::SymmetricBlockMatrix::aboveDiagonalRange
Block aboveDiagonalRange(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock)
Get a range [i,j) from the matrix. Indices are in block units.
Definition SymmetricBlockMatrix.h:181
│ │ │ │ -
gtsam::SymmetricBlockMatrix::triangularView
Eigen::TriangularView< constBlock, Eigen::Upper > triangularView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j) as a triangular view.
Definition SymmetricBlockMatrix.h:163
│ │ │ │ -
gtsam::SymmetricBlockMatrix::diagonalBlock
Eigen::SelfAdjointView< constBlock, Eigen::Upper > diagonalBlock(DenseIndex J) const
Return the J'th diagonal block as a self adjoint view.
Definition SymmetricBlockMatrix.h:140
│ │ │ │ -
gtsam::SymmetricBlockMatrix::SymmetricBlockMatrix
SymmetricBlockMatrix(const CONTAINER &dimensions, const Matrix &matrix, bool appendOneDimension=false)
Construct from a container of the sizes of each vertical block and a pre-prepared matrix.
Definition SymmetricBlockMatrix.h:90
│ │ │ │ -
gtsam::SymmetricBlockMatrix::blockStart
DenseIndex blockStart() const
Retrieve the first logical block, i.e.
Definition SymmetricBlockMatrix.h:277
│ │ │ │ -
gtsam::SymmetricBlockMatrix::selfadjointView
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView() const
Get self adjoint view.
Definition SymmetricBlockMatrix.h:247
│ │ │ │ -
gtsam::SymmetricBlockMatrix::updateOffDiagonalBlock
void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
Update an off diagonal block.
Definition SymmetricBlockMatrix.h:228
│ │ │ │ -
gtsam::SymmetricBlockMatrix::SymmetricBlockMatrix
SymmetricBlockMatrix(const CONTAINER &dimensions, bool appendOneDimension=false)
Construct from a container of the sizes of each block.
Definition SymmetricBlockMatrix.h:70
│ │ │ │ -
gtsam::SymmetricBlockMatrix::SymmetricBlockMatrix
SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension=false)
Construct from iterator over the sizes of each vertical block.
Definition SymmetricBlockMatrix.h:80
│ │ │ │ -
gtsam::SymmetricBlockMatrix::blockStart
DenseIndex & blockStart()
Retrieve or modify the first logical block, i.e.
Definition SymmetricBlockMatrix.h:273
│ │ │ │ -
gtsam::SymmetricBlockMatrix::aboveDiagonalRange
constBlock aboveDiagonalRange(DenseIndex i_startBlock, DenseIndex i_endBlock, DenseIndex j_startBlock, DenseIndex j_endBlock) const
Get a range [i,j) from the matrix. Indices are in block units.
Definition SymmetricBlockMatrix.h:170
│ │ │ │ -
gtsam::SymmetricBlockMatrix::diagonalBlock
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J'th diagonal block as a self adjoint view.
Definition SymmetricBlockMatrix.h:135
│ │ │ │ -
gtsam::SymmetricBlockMatrix::block_
Block block_(DenseIndex iBlock, DenseIndex jBlock, DenseIndex blockRows=1, DenseIndex blockCols=1)
Get an arbitrary block from the matrix. Indices are in block units.
Definition SymmetricBlockMatrix.h:327
│ │ │ │ -
gtsam::SymmetricBlockMatrix::updateDiagonalBlock
void updateDiagonalBlock(DenseIndex I, const XprType &xpr)
Increment the diagonal block by the values in xpr. Only reads the upper triangular part of xpr.
Definition SymmetricBlockMatrix.h:212
│ │ │ │ -
gtsam::SymmetricBlockMatrix::nOffsets
DenseIndex nOffsets() const
Number of offsets in the full matrix.
Definition SymmetricBlockMatrix.h:301
│ │ │ │ -
gtsam::SymmetricBlockMatrix::calcIndices
std::array< DenseIndex, 4 > calcIndices(DenseIndex iBlock, DenseIndex jBlock, DenseIndex blockRows, DenseIndex blockCols) const
Compute the indices into the underlying matrix for a given block.
Definition SymmetricBlockMatrix.h:345
│ │ │ │ -
gtsam::SymmetricBlockMatrix::nBlocks
DenseIndex nBlocks() const
Block count.
Definition SymmetricBlockMatrix.h:120
│ │ │ │ -
gtsam::SymmetricBlockMatrix::variableColOffsets_
FastVector< DenseIndex > variableColOffsets_
the starting columns of each block (0-based)
Definition SymmetricBlockMatrix.h:60
│ │ │ │ -
gtsam::SymmetricBlockMatrix::selfadjointView
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j).
Definition SymmetricBlockMatrix.h:156
│ │ │ │ -
gtsam::SymmetricBlockMatrix::offset
DenseIndex offset(DenseIndex block) const
Get an offset for a block index (in the active view).
Definition SymmetricBlockMatrix.h:311
│ │ │ │ -
gtsam::VerticalBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43
│ │ │ │ +
gtsam::DSFBase
A fast implementation of disjoint set forests that uses vector as underly data structure.
Definition DSFVector.h:38
│ │ │ │ +
gtsam::DSFBase::V
std::vector< size_t > V
Vector of ints.
Definition DSFVector.h:41
│ │ │ │ +
gtsam::DSFVector
DSFVector additionally keeps a vector of keys to support more expensive operations.
Definition DSFVector.h:64
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,546 +1,97 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SymmetricBlockMatrix.h │ │ │ │ │ +DSFVector.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3* GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ -4* Atlanta, Georgia 30332-0415 │ │ │ │ │ -5* All Rights Reserved │ │ │ │ │ -6* Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +5 * All Rights Reserved │ │ │ │ │ +6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ -8* See LICENSE for the license information │ │ │ │ │ +8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ -10* ------------------------------------------------------------------------- │ │ │ │ │ +10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ -23#include │ │ │ │ │ -24#include │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ -28 │ │ │ │ │ -29namespace boost { │ │ │ │ │ -30namespace serialization { │ │ │ │ │ -31class access; │ │ │ │ │ -32} /* namespace serialization */ │ │ │ │ │ -33} /* namespace boost */ │ │ │ │ │ -34 │ │ │ │ │ -35namespace _g_t_s_a_m { │ │ │ │ │ -36 │ │ │ │ │ -37 // Forward declarations │ │ │ │ │ -38 class VerticalBlockMatrix; │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include │ │ │ │ │ +22#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ +23 │ │ │ │ │ +24#include │ │ │ │ │ +25 │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28#include │ │ │ │ │ +29 │ │ │ │ │ +30namespace _g_t_s_a_m { │ │ │ │ │ +31 │ │ │ │ │ +_3_8class GTSAM_EXPORT _D_S_F_B_a_s_e { │ │ │ │ │ 39 │ │ │ │ │ -_5_1 class GTSAM_EXPORT _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -52 { │ │ │ │ │ -53 public: │ │ │ │ │ -54 typedef _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x _T_h_i_s; │ │ │ │ │ -55 typedef Eigen::Block Block; │ │ │ │ │ -56 typedef Eigen::Block constBlock; │ │ │ │ │ -57 │ │ │ │ │ -58 protected: │ │ │ │ │ -_5_9 Matrix _m_a_t_r_i_x__; │ │ │ │ │ -_6_0 _F_a_s_t_V_e_c_t_o_r_<_D_e_n_s_e_I_n_d_e_x_> _v_a_r_i_a_b_l_e_C_o_l_O_f_f_s_e_t_s__; │ │ │ │ │ -61 │ │ │ │ │ -_6_2 _D_e_n_s_e_I_n_d_e_x _b_l_o_c_k_S_t_a_r_t__; │ │ │ │ │ -63 │ │ │ │ │ -64 public: │ │ │ │ │ -66 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x(); │ │ │ │ │ -67 │ │ │ │ │ -69 template │ │ │ │ │ -_7_0 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x(const CONTAINER& dimensions, bool appendOneDimension = │ │ │ │ │ -false) : │ │ │ │ │ -71 blockStart_(0) │ │ │ │ │ -72 { │ │ │ │ │ -73 fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); │ │ │ │ │ -74 matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back()); │ │ │ │ │ -75 assertInvariants(); │ │ │ │ │ -76 } │ │ │ │ │ -77 │ │ │ │ │ -79 template │ │ │ │ │ -_8_0 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool │ │ │ │ │ -appendOneDimension = false) : │ │ │ │ │ -81 blockStart_(0) │ │ │ │ │ -82 { │ │ │ │ │ -83 fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension); │ │ │ │ │ -84 matrix_.resize(variableColOffsets_.back(), variableColOffsets_.back()); │ │ │ │ │ -85 assertInvariants(); │ │ │ │ │ -86 } │ │ │ │ │ -87 │ │ │ │ │ -89 template │ │ │ │ │ -_9_0 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x(const CONTAINER& dimensions, const Matrix& matrix, bool │ │ │ │ │ -appendOneDimension = false) : │ │ │ │ │ -91 blockStart_(0) │ │ │ │ │ -92 { │ │ │ │ │ -93 matrix_.resize(matrix.rows(), matrix.cols()); │ │ │ │ │ -94 matrix_.triangularView() = matrix.triangularView │ │ │ │ │ -(); │ │ │ │ │ -95 fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); │ │ │ │ │ -96 if(matrix_.rows() != matrix_.cols()) │ │ │ │ │ -97 throw std::invalid_argument("Requested to create a SymmetricBlockMatrix from │ │ │ │ │ -a non-square matrix."); │ │ │ │ │ -98 if(variableColOffsets_.back() != matrix_.cols()) │ │ │ │ │ -99 throw std::invalid_argument("Requested to create a SymmetricBlockMatrix with │ │ │ │ │ -dimensions that do not sum to the total size of the provided matrix."); │ │ │ │ │ -100 assertInvariants(); │ │ │ │ │ -101 } │ │ │ │ │ -102 │ │ │ │ │ -106 static _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x LikeActiveViewOf(const _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x& │ │ │ │ │ -other); │ │ │ │ │ -107 │ │ │ │ │ -111 static _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x LikeActiveViewOf(const _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x& │ │ │ │ │ -other); │ │ │ │ │ -112 │ │ │ │ │ -_1_1_4 _D_e_n_s_e_I_n_d_e_x _r_o_w_s() const { assertInvariants(); return │ │ │ │ │ -variableColOffsets_.back() - variableColOffsets_[blockStart_]; } │ │ │ │ │ -115 │ │ │ │ │ -_1_1_7 _D_e_n_s_e_I_n_d_e_x _c_o_l_s() const { return rows(); } │ │ │ │ │ -118 │ │ │ │ │ -_1_2_0 _D_e_n_s_e_I_n_d_e_x _n_B_l_o_c_k_s() const { return nActualBlocks() - blockStart_; } │ │ │ │ │ -121 │ │ │ │ │ -_1_2_3 _D_e_n_s_e_I_n_d_e_x _g_e_t_D_i_m(_D_e_n_s_e_I_n_d_e_x block) const { │ │ │ │ │ -124 return calcIndices(block, block, 1, 1)[2]; │ │ │ │ │ -125 } │ │ │ │ │ -126 │ │ │ │ │ -129 │ │ │ │ │ -132 Matrix block(_D_e_n_s_e_I_n_d_e_x I, _D_e_n_s_e_I_n_d_e_x J) const; │ │ │ │ │ -133 │ │ │ │ │ -_1_3_5 Eigen::SelfAdjointView _d_i_a_g_o_n_a_l_B_l_o_c_k(_D_e_n_s_e_I_n_d_e_x J) { │ │ │ │ │ -136 return block_(J, J).selfadjointView(); │ │ │ │ │ -137 } │ │ │ │ │ -138 │ │ │ │ │ -_1_4_0 Eigen::SelfAdjointView _d_i_a_g_o_n_a_l_B_l_o_c_k(_D_e_n_s_e_I_n_d_e_x │ │ │ │ │ -J) const { │ │ │ │ │ -141 return block_(J, J).selfadjointView(); │ │ │ │ │ -142 } │ │ │ │ │ -143 │ │ │ │ │ -_1_4_5 Vector _d_i_a_g_o_n_a_l(_D_e_n_s_e_I_n_d_e_x J) const { │ │ │ │ │ -146 return block_(J, J).diagonal(); │ │ │ │ │ -147 } │ │ │ │ │ -148 │ │ │ │ │ -_1_5_0 constBlock _a_b_o_v_e_D_i_a_g_o_n_a_l_B_l_o_c_k(_D_e_n_s_e_I_n_d_e_x I, _D_e_n_s_e_I_n_d_e_x J) const { │ │ │ │ │ -151 assert(I < J); │ │ │ │ │ -152 return block_(I, J); │ │ │ │ │ -153 } │ │ │ │ │ -154 │ │ │ │ │ -_1_5_6 Eigen::SelfAdjointView _s_e_l_f_a_d_j_o_i_n_t_V_i_e_w( │ │ │ │ │ -157 _D_e_n_s_e_I_n_d_e_x I, _D_e_n_s_e_I_n_d_e_x J) const { │ │ │ │ │ -158 assert(J > I); │ │ │ │ │ -159 return block_(I, I, J - I, J - I).selfadjointView(); │ │ │ │ │ -160 } │ │ │ │ │ -161 │ │ │ │ │ -_1_6_3 Eigen::TriangularView _t_r_i_a_n_g_u_l_a_r_V_i_e_w(_D_e_n_s_e_I_n_d_e_x │ │ │ │ │ -I, │ │ │ │ │ -164 _D_e_n_s_e_I_n_d_e_x J) const { │ │ │ │ │ -165 assert(J > I); │ │ │ │ │ -166 return block_(I, I, J - I, J - I).triangularView(); │ │ │ │ │ -167 } │ │ │ │ │ -168 │ │ │ │ │ -_1_7_0 constBlock _a_b_o_v_e_D_i_a_g_o_n_a_l_R_a_n_g_e(_D_e_n_s_e_I_n_d_e_x i_startBlock, │ │ │ │ │ -171 _D_e_n_s_e_I_n_d_e_x i_endBlock, │ │ │ │ │ -172 _D_e_n_s_e_I_n_d_e_x j_startBlock, │ │ │ │ │ -173 _D_e_n_s_e_I_n_d_e_x j_endBlock) const { │ │ │ │ │ -174 assert(i_startBlock < j_startBlock); │ │ │ │ │ -175 assert(i_endBlock <= j_startBlock); │ │ │ │ │ -176 return block_(i_startBlock, j_startBlock, i_endBlock - i_startBlock, │ │ │ │ │ -177 j_endBlock - j_startBlock); │ │ │ │ │ -178 } │ │ │ │ │ -179 │ │ │ │ │ -_1_8_1 Block _a_b_o_v_e_D_i_a_g_o_n_a_l_R_a_n_g_e(_D_e_n_s_e_I_n_d_e_x i_startBlock, _D_e_n_s_e_I_n_d_e_x i_endBlock, │ │ │ │ │ -182 _D_e_n_s_e_I_n_d_e_x j_startBlock, _D_e_n_s_e_I_n_d_e_x j_endBlock) { │ │ │ │ │ -183 assert(i_startBlock < j_startBlock); │ │ │ │ │ -184 assert(i_endBlock <= j_startBlock); │ │ │ │ │ -185 return block_(i_startBlock, j_startBlock, i_endBlock - i_startBlock, │ │ │ │ │ -186 j_endBlock - j_startBlock); │ │ │ │ │ -187 } │ │ │ │ │ -188 │ │ │ │ │ -192 │ │ │ │ │ -194 template │ │ │ │ │ -_1_9_5 void _s_e_t_D_i_a_g_o_n_a_l_B_l_o_c_k(_D_e_n_s_e_I_n_d_e_x I, const XprType& xpr) { │ │ │ │ │ -196 block_(I, I).triangularView() = xpr.template │ │ │ │ │ -triangularView(); │ │ │ │ │ -197 } │ │ │ │ │ -198 │ │ │ │ │ -200 template │ │ │ │ │ -_2_0_1 void _s_e_t_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k(_D_e_n_s_e_I_n_d_e_x I, _D_e_n_s_e_I_n_d_e_x J, const XprType& xpr) { │ │ │ │ │ -202 assert(I != J); │ │ │ │ │ -203 if (I < J) { │ │ │ │ │ -204 block_(I, J) = xpr; │ │ │ │ │ -205 } else { │ │ │ │ │ -206 block_(J, I) = xpr.transpose(); │ │ │ │ │ -207 } │ │ │ │ │ -208 } │ │ │ │ │ -209 │ │ │ │ │ -211 template │ │ │ │ │ -_2_1_2 void _u_p_d_a_t_e_D_i_a_g_o_n_a_l_B_l_o_c_k(_D_e_n_s_e_I_n_d_e_x I, const XprType& xpr) { │ │ │ │ │ -213 // TODO(gareth): Eigen won't let us add triangular or self-adjoint views │ │ │ │ │ -214 // here, so we do it manually. │ │ │ │ │ -215 auto dest = block_(I, I); │ │ │ │ │ -216 assert(dest.rows() == xpr.rows()); │ │ │ │ │ -217 assert(dest.cols() == xpr.cols()); │ │ │ │ │ -218 for (_D_e_n_s_e_I_n_d_e_x col = 0; col < dest.cols(); ++col) { │ │ │ │ │ -219 for (_D_e_n_s_e_I_n_d_e_x _r_o_w = 0; _r_o_w <= col; ++_r_o_w) { │ │ │ │ │ -220 dest(_r_o_w, col) += xpr(_r_o_w, col); │ │ │ │ │ -221 } │ │ │ │ │ -222 } │ │ │ │ │ -223 } │ │ │ │ │ -224 │ │ │ │ │ -227 template │ │ │ │ │ -_2_2_8 void _u_p_d_a_t_e_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k(_D_e_n_s_e_I_n_d_e_x I, _D_e_n_s_e_I_n_d_e_x J, const XprType& xpr) │ │ │ │ │ -{ │ │ │ │ │ -229 assert(I != J); │ │ │ │ │ -230 if (I < J) { │ │ │ │ │ -231 block_(I, J).noalias() += xpr; │ │ │ │ │ -232 } else { │ │ │ │ │ -233 block_(J, I).noalias() += xpr.transpose(); │ │ │ │ │ -234 } │ │ │ │ │ -235 } │ │ │ │ │ -236 │ │ │ │ │ -240 │ │ │ │ │ -_2_4_2 Eigen::SelfAdjointView _s_e_l_f_a_d_j_o_i_n_t_V_i_e_w() { │ │ │ │ │ -243 return full().selfadjointView(); │ │ │ │ │ -244 } │ │ │ │ │ -245 │ │ │ │ │ -_2_4_7 Eigen::SelfAdjointView _s_e_l_f_a_d_j_o_i_n_t_V_i_e_w() const { │ │ │ │ │ -248 return full().selfadjointView(); │ │ │ │ │ -249 } │ │ │ │ │ -250 │ │ │ │ │ -252 template │ │ │ │ │ -_2_5_3 void _s_e_t_F_u_l_l_M_a_t_r_i_x(const XprType& xpr) { │ │ │ │ │ -254 full().triangularView() = xpr.template triangularView(); │ │ │ │ │ -255 } │ │ │ │ │ -256 │ │ │ │ │ -_2_5_8 void _s_e_t_Z_e_r_o() { │ │ │ │ │ -259 full().triangularView().setZero(); │ │ │ │ │ -260 } │ │ │ │ │ -261 │ │ │ │ │ -263 void negate(); │ │ │ │ │ -264 │ │ │ │ │ -266 void invertInPlace(); │ │ │ │ │ -267 │ │ │ │ │ -269 │ │ │ │ │ -_2_7_3 _D_e_n_s_e_I_n_d_e_x& _b_l_o_c_k_S_t_a_r_t() { return blockStart_; } │ │ │ │ │ -274 │ │ │ │ │ -_2_7_7 _D_e_n_s_e_I_n_d_e_x _b_l_o_c_k_S_t_a_r_t() const { return blockStart_; } │ │ │ │ │ -278 │ │ │ │ │ -289 void _c_h_o_l_e_s_k_y_P_a_r_t_i_a_l(_D_e_n_s_e_I_n_d_e_x nFrontals); │ │ │ │ │ -290 │ │ │ │ │ -296 _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x _s_p_l_i_t(_D_e_n_s_e_I_n_d_e_x nFrontals); │ │ │ │ │ -297 │ │ │ │ │ -298 protected: │ │ │ │ │ -299 │ │ │ │ │ -_3_0_1 _D_e_n_s_e_I_n_d_e_x _n_O_f_f_s_e_t_s() const { │ │ │ │ │ -302 return variableColOffsets_.size(); │ │ │ │ │ -303 } │ │ │ │ │ -304 │ │ │ │ │ -_3_0_6 _D_e_n_s_e_I_n_d_e_x _n_A_c_t_u_a_l_B_l_o_c_k_s() const { │ │ │ │ │ -307 return nOffsets() - 1; │ │ │ │ │ -308 } │ │ │ │ │ -309 │ │ │ │ │ -_3_1_1 _D_e_n_s_e_I_n_d_e_x _o_f_f_s_e_t(_D_e_n_s_e_I_n_d_e_x block) const { │ │ │ │ │ -312 assert(block >= 0); │ │ │ │ │ -313 const _D_e_n_s_e_I_n_d_e_x actual_index = block + blockStart(); │ │ │ │ │ -314 assert(actual_index < nOffsets()); │ │ │ │ │ -315 return variableColOffsets_[actual_index]; │ │ │ │ │ -316 } │ │ │ │ │ -317 │ │ │ │ │ -_3_1_9 constBlock _b_l_o_c_k__(_D_e_n_s_e_I_n_d_e_x iBlock, _D_e_n_s_e_I_n_d_e_x jBlock, │ │ │ │ │ -320 _D_e_n_s_e_I_n_d_e_x blockRows = 1, _D_e_n_s_e_I_n_d_e_x blockCols = 1) const { │ │ │ │ │ -321 const std::array indices = │ │ │ │ │ -322 calcIndices(iBlock, jBlock, blockRows, blockCols); │ │ │ │ │ -323 return matrix_.block(indices[0], indices[1], indices[2], indices[3]); │ │ │ │ │ -324 } │ │ │ │ │ -325 │ │ │ │ │ -_3_2_7 Block _b_l_o_c_k__(_D_e_n_s_e_I_n_d_e_x iBlock, _D_e_n_s_e_I_n_d_e_x jBlock, _D_e_n_s_e_I_n_d_e_x blockRows = │ │ │ │ │ -1, │ │ │ │ │ -328 _D_e_n_s_e_I_n_d_e_x blockCols = 1) { │ │ │ │ │ -329 const std::array indices = │ │ │ │ │ -330 calcIndices(iBlock, jBlock, blockRows, blockCols); │ │ │ │ │ -331 return matrix_.block(indices[0], indices[1], indices[2], indices[3]); │ │ │ │ │ -332 } │ │ │ │ │ -333 │ │ │ │ │ -_3_3_5 constBlock _f_u_l_l() const { │ │ │ │ │ -336 return block_(0, 0, nBlocks(), nBlocks()); │ │ │ │ │ -337 } │ │ │ │ │ -338 │ │ │ │ │ -_3_4_0 Block _f_u_l_l() { │ │ │ │ │ -341 return block_(0, 0, nBlocks(), nBlocks()); │ │ │ │ │ -342 } │ │ │ │ │ -343 │ │ │ │ │ -_3_4_5 std::array _c_a_l_c_I_n_d_i_c_e_s(_D_e_n_s_e_I_n_d_e_x iBlock, _D_e_n_s_e_I_n_d_e_x jBlock, │ │ │ │ │ -346 _D_e_n_s_e_I_n_d_e_x blockRows, │ │ │ │ │ -347 _D_e_n_s_e_I_n_d_e_x blockCols) const { │ │ │ │ │ -348 assert(blockRows >= 0); │ │ │ │ │ -349 assert(blockCols >= 0); │ │ │ │ │ -350 │ │ │ │ │ -351 // adjust indices to account for start and size of blocks │ │ │ │ │ -352 const _D_e_n_s_e_I_n_d_e_x denseI = offset(iBlock); │ │ │ │ │ -353 const _D_e_n_s_e_I_n_d_e_x denseJ = offset(jBlock); │ │ │ │ │ -354 const _D_e_n_s_e_I_n_d_e_x denseRows = offset(iBlock + blockRows) - denseI; │ │ │ │ │ -355 const _D_e_n_s_e_I_n_d_e_x denseCols = offset(jBlock + blockCols) - denseJ; │ │ │ │ │ -356 return {{denseI, denseJ, denseRows, denseCols}}; │ │ │ │ │ -357 } │ │ │ │ │ -358 │ │ │ │ │ -359 void assertInvariants() const │ │ │ │ │ -360 { │ │ │ │ │ -361 assert(matrix_.rows() == matrix_.cols()); │ │ │ │ │ -362 assert(matrix_.cols() == variableColOffsets_.back()); │ │ │ │ │ -363 assert(blockStart_ < (_D_e_n_s_e_I_n_d_e_x)variableColOffsets_.size()); │ │ │ │ │ -364 } │ │ │ │ │ -365 │ │ │ │ │ -366 template │ │ │ │ │ -367 void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool │ │ │ │ │ -appendOneDimension) │ │ │ │ │ -368 { │ │ │ │ │ -369 variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + │ │ │ │ │ -(appendOneDimension ? 1 : 0)); │ │ │ │ │ -370 variableColOffsets_[0] = 0; │ │ │ │ │ -371 DenseIndex j=0; │ │ │ │ │ -372 for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) { │ │ │ │ │ -373 variableColOffsets_[j+1] = variableColOffsets_[j] + *dim; │ │ │ │ │ -374 ++ j; │ │ │ │ │ -375 } │ │ │ │ │ -376 if(appendOneDimension) │ │ │ │ │ -377 { │ │ │ │ │ -378 variableColOffsets_[j+1] = variableColOffsets_[j] + 1; │ │ │ │ │ -379 ++ j; │ │ │ │ │ -380 } │ │ │ │ │ -381 } │ │ │ │ │ -382 │ │ │ │ │ -383 friend class VerticalBlockMatrix; │ │ │ │ │ -384 template friend class │ │ │ │ │ -SymmetricBlockMatrixBlockExpr; │ │ │ │ │ -385 │ │ │ │ │ -386 private: │ │ │ │ │ -_3_8_8 friend class boost::serialization::access; │ │ │ │ │ -389 template │ │ │ │ │ -390 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -391 // Fill in the lower triangle part of the matrix, so boost::serialization │ │ │ │ │ -won't │ │ │ │ │ -392 // complain about uninitialized data with an input_stream_error exception │ │ │ │ │ -393 // http://www.boost.org/doc/libs/1_37_0/libs/serialization/doc/ │ │ │ │ │ -exceptions.html#stream_error │ │ │ │ │ -394 matrix_.triangularView() = matrix_.triangularView().transpose(); │ │ │ │ │ -395 ar & BOOST_SERIALIZATION_NVP(matrix_); │ │ │ │ │ -396 ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); │ │ │ │ │ -397 ar & BOOST_SERIALIZATION_NVP(blockStart_); │ │ │ │ │ -398 } │ │ │ │ │ -399 }; │ │ │ │ │ -400 │ │ │ │ │ -402 class CholeskyFailed; │ │ │ │ │ -403 │ │ │ │ │ -404} │ │ │ │ │ -_F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ -A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ -_t_y_p_e_s_._h │ │ │ │ │ -Typedefs for easier changing of types. │ │ │ │ │ -_M_a_t_r_i_x_._h │ │ │ │ │ -typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ -std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ -FastVector │ │ │ │ │ -FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ -DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ +40public: │ │ │ │ │ +_4_1 typedef std::vector _V; │ │ │ │ │ +42 │ │ │ │ │ +43private: │ │ │ │ │ +44 boost::shared_ptr v_; │ │ │ │ │ +45 │ │ │ │ │ +46public: │ │ │ │ │ +48 _D_S_F_B_a_s_e(const size_t numNodes); │ │ │ │ │ +49 │ │ │ │ │ +51 _D_S_F_B_a_s_e(const boost::shared_ptr& v_in); │ │ │ │ │ +52 │ │ │ │ │ +54 size_t find(size_t key) const; │ │ │ │ │ +55 │ │ │ │ │ +57 void merge(const size_t& i1, const size_t& i2); │ │ │ │ │ +58}; │ │ │ │ │ +59 │ │ │ │ │ +_6_4class GTSAM_EXPORT _D_S_F_V_e_c_t_o_r: public _D_S_F_B_a_s_e { │ │ │ │ │ +65 │ │ │ │ │ +66private: │ │ │ │ │ +67 std::vector keys_; │ │ │ │ │ +68 │ │ │ │ │ +69public: │ │ │ │ │ +71 _D_S_F_V_e_c_t_o_r(const size_t numNodes); │ │ │ │ │ +72 │ │ │ │ │ +74 _D_S_F_V_e_c_t_o_r(const std::vector& keys); │ │ │ │ │ +75 │ │ │ │ │ +77 _D_S_F_V_e_c_t_o_r(const boost::shared_ptr& v_in, const std::vector& │ │ │ │ │ +keys); │ │ │ │ │ +78 │ │ │ │ │ +79 // All operations below loop over all keys and hence are *at least* O(n) │ │ │ │ │ +80 │ │ │ │ │ +82 bool isSingleton(const size_t& label) const; │ │ │ │ │ +83 │ │ │ │ │ +85 std::set set(const size_t& label) const; │ │ │ │ │ +86 │ │ │ │ │ +88 std::map > sets() const; │ │ │ │ │ +89 │ │ │ │ │ +91 std::map > arrays() const; │ │ │ │ │ +92}; │ │ │ │ │ +93 │ │ │ │ │ +94} │ │ │ │ │ +_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ +Included from all GTSAM files. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_n_s_e_I_n_d_e_x │ │ │ │ │ -ptrdiff_t DenseIndex │ │ │ │ │ -The index type for Eigen objects. │ │ │ │ │ -DDeeffiinniittiioonn types.h:106 │ │ │ │ │ -_g_t_s_a_m_:_:_r_o_w │ │ │ │ │ -const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j) │ │ │ │ │ -Extracts a row view from a matrix that avoids a copy. │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:222 │ │ │ │ │ -_g_t_s_a_m_:_:_s_p_l_i_t │ │ │ │ │ -void split(const G &g, const PredecessorMap< KEY > &tree, G &Ab1, G &Ab2) │ │ │ │ │ -Split the graph into two parts: one corresponds to the given spanning tree, and │ │ │ │ │ -the other corresponds... │ │ │ │ │ -DDeeffiinniittiioonn graph-inl.h:255 │ │ │ │ │ -_g_t_s_a_m_:_:_c_h_o_l_e_s_k_y_P_a_r_t_i_a_l │ │ │ │ │ -bool choleskyPartial(Matrix &ABC, size_t nFrontal, size_t topleft) │ │ │ │ │ -Partial Cholesky computes a factor [R S such that [R' 0 [R S = [A B 0 L] S' I] │ │ │ │ │ -0 L] B' C]. │ │ │ │ │ -DDeeffiinniittiioonn cholesky.cpp:108 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ -of blocks. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_f_u_l_l │ │ │ │ │ -Block full() │ │ │ │ │ -Get the full matrix as a block. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:340 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_b_l_o_c_k_S_t_a_r_t__ │ │ │ │ │ -DenseIndex blockStart_ │ │ │ │ │ -Changes apparent matrix view, see main class comment. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:62 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_s_e_t_D_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ -void setDiagonalBlock(DenseIndex I, const XprType &xpr) │ │ │ │ │ -Set a diagonal block. Only the upper triangular portion of xpr is evaluated. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:195 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_n_A_c_t_u_a_l_B_l_o_c_k_s │ │ │ │ │ -DenseIndex nActualBlocks() const │ │ │ │ │ -Number of actual blocks in the full matrix. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:306 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_d_i_a_g_o_n_a_l │ │ │ │ │ -Vector diagonal(DenseIndex J) const │ │ │ │ │ -Get the diagonal of the J'th diagonal block. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:145 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_m_a_t_r_i_x__ │ │ │ │ │ -Matrix matrix_ │ │ │ │ │ -The full matrix. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:59 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_c_o_l_s │ │ │ │ │ -DenseIndex cols() const │ │ │ │ │ -Column size. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:117 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_g_e_t_D_i_m │ │ │ │ │ -DenseIndex getDim(DenseIndex block) const │ │ │ │ │ -Number of dimensions for variable on this diagonal block. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:123 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_s_e_t_Z_e_r_o │ │ │ │ │ -void setZero() │ │ │ │ │ -Set the entire active matrix zero. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:258 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_s_e_t_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ -void setOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr) │ │ │ │ │ -Set an off-diagonal block. Only the upper triangular portion of xpr is │ │ │ │ │ -evaluated. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:201 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_s_e_t_F_u_l_l_M_a_t_r_i_x │ │ │ │ │ -void setFullMatrix(const XprType &xpr) │ │ │ │ │ -Set the entire active matrix. Only reads the upper triangular part of xpr. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:253 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_s_e_l_f_a_d_j_o_i_n_t_V_i_e_w │ │ │ │ │ -Eigen::SelfAdjointView< Block, Eigen::Upper > selfadjointView() │ │ │ │ │ -Get self adjoint view. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:242 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_a_b_o_v_e_D_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ -constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const │ │ │ │ │ -Get block above the diagonal (I, J). │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:150 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_b_l_o_c_k__ │ │ │ │ │ -constBlock block_(DenseIndex iBlock, DenseIndex jBlock, DenseIndex blockRows=1, │ │ │ │ │ -DenseIndex blockCols=1) const │ │ │ │ │ -Get an arbitrary block from the matrix. Indices are in block units. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:319 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_f_u_l_l │ │ │ │ │ -constBlock full() const │ │ │ │ │ -Get the full matrix as a block. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:335 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_r_o_w_s │ │ │ │ │ -DenseIndex rows() const │ │ │ │ │ -Row size. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:114 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_a_b_o_v_e_D_i_a_g_o_n_a_l_R_a_n_g_e │ │ │ │ │ -Block aboveDiagonalRange(DenseIndex i_startBlock, DenseIndex i_endBlock, │ │ │ │ │ -DenseIndex j_startBlock, DenseIndex j_endBlock) │ │ │ │ │ -Get a range [i,j) from the matrix. Indices are in block units. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:181 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_t_r_i_a_n_g_u_l_a_r_V_i_e_w │ │ │ │ │ -Eigen::TriangularView< constBlock, Eigen::Upper > triangularView(DenseIndex I, │ │ │ │ │ -DenseIndex J) const │ │ │ │ │ -Return the square sub-matrix that contains blocks(i:j, i:j) as a triangular │ │ │ │ │ -view. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:163 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_d_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ -Eigen::SelfAdjointView< constBlock, Eigen::Upper > diagonalBlock(DenseIndex J) │ │ │ │ │ -const │ │ │ │ │ -Return the J'th diagonal block as a self adjoint view. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:140 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -SymmetricBlockMatrix(const CONTAINER &dimensions, const Matrix &matrix, bool │ │ │ │ │ -appendOneDimension=false) │ │ │ │ │ -Construct from a container of the sizes of each vertical block and a pre- │ │ │ │ │ -prepared matrix. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:90 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_b_l_o_c_k_S_t_a_r_t │ │ │ │ │ -DenseIndex blockStart() const │ │ │ │ │ -Retrieve the first logical block, i.e. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:277 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_s_e_l_f_a_d_j_o_i_n_t_V_i_e_w │ │ │ │ │ -Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView() const │ │ │ │ │ -Get self adjoint view. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:247 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_u_p_d_a_t_e_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ -void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr) │ │ │ │ │ -Update an off diagonal block. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:228 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -SymmetricBlockMatrix(const CONTAINER &dimensions, bool │ │ │ │ │ -appendOneDimension=false) │ │ │ │ │ -Construct from a container of the sizes of each block. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:70 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -SymmetricBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool │ │ │ │ │ -appendOneDimension=false) │ │ │ │ │ -Construct from iterator over the sizes of each vertical block. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:80 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_b_l_o_c_k_S_t_a_r_t │ │ │ │ │ -DenseIndex & blockStart() │ │ │ │ │ -Retrieve or modify the first logical block, i.e. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:273 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_a_b_o_v_e_D_i_a_g_o_n_a_l_R_a_n_g_e │ │ │ │ │ -constBlock aboveDiagonalRange(DenseIndex i_startBlock, DenseIndex i_endBlock, │ │ │ │ │ -DenseIndex j_startBlock, DenseIndex j_endBlock) const │ │ │ │ │ -Get a range [i,j) from the matrix. Indices are in block units. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:170 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_d_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ -Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J) │ │ │ │ │ -Return the J'th diagonal block as a self adjoint view. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:135 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_b_l_o_c_k__ │ │ │ │ │ -Block block_(DenseIndex iBlock, DenseIndex jBlock, DenseIndex blockRows=1, │ │ │ │ │ -DenseIndex blockCols=1) │ │ │ │ │ -Get an arbitrary block from the matrix. Indices are in block units. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:327 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_u_p_d_a_t_e_D_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ -void updateDiagonalBlock(DenseIndex I, const XprType &xpr) │ │ │ │ │ -Increment the diagonal block by the values in xpr. Only reads the upper │ │ │ │ │ -triangular part of xpr. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:212 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_n_O_f_f_s_e_t_s │ │ │ │ │ -DenseIndex nOffsets() const │ │ │ │ │ -Number of offsets in the full matrix. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:301 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_c_a_l_c_I_n_d_i_c_e_s │ │ │ │ │ -std::array< DenseIndex, 4 > calcIndices(DenseIndex iBlock, DenseIndex jBlock, │ │ │ │ │ -DenseIndex blockRows, DenseIndex blockCols) const │ │ │ │ │ -Compute the indices into the underlying matrix for a given block. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:345 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_n_B_l_o_c_k_s │ │ │ │ │ -DenseIndex nBlocks() const │ │ │ │ │ -Block count. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:120 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_v_a_r_i_a_b_l_e_C_o_l_O_f_f_s_e_t_s__ │ │ │ │ │ -FastVector< DenseIndex > variableColOffsets_ │ │ │ │ │ -the starting columns of each block (0-based) │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:60 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_s_e_l_f_a_d_j_o_i_n_t_V_i_e_w │ │ │ │ │ -Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex │ │ │ │ │ -I, DenseIndex J) const │ │ │ │ │ -Return the square sub-matrix that contains blocks(i:j, i:j). │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:156 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_o_f_f_s_e_t │ │ │ │ │ -DenseIndex offset(DenseIndex block) const │ │ │ │ │ -Get an offset for a block index (in the active view). │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:311 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ -of vertical blocks. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_D_S_F_B_a_s_e │ │ │ │ │ +A fast implementation of disjoint set forests that uses vector as underly data │ │ │ │ │ +structure. │ │ │ │ │ +DDeeffiinniittiioonn DSFVector.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_D_S_F_B_a_s_e_:_:_V │ │ │ │ │ +std::vector< size_t > V │ │ │ │ │ +Vector of ints. │ │ │ │ │ +DDeeffiinniittiioonn DSFVector.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_D_S_F_V_e_c_t_o_r │ │ │ │ │ +DSFVector additionally keeps a vector of keys to support more expensive │ │ │ │ │ +operations. │ │ │ │ │ +DDeeffiinniittiioonn DSFVector.h:64 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_._h │ │ │ │ │ + * _D_S_F_V_e_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00140.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/WeightedSampler.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/timing.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,44 +94,55 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
WeightedSampler.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
timing.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Fast sampling without replacement. │ │ │ │ +

Timing utilities. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::WeightedSampler< Engine >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +GTSAM_EXPORT boost::shared_ptr< TimingOutline > gtsam::internal::gTimingRoot (new TimingOutline("Total", getTicTocID("Total")))
 
│ │ │ │ +GTSAM_EXPORT boost::weak_ptr< TimingOutline > gtsam::internal::gCurrentTimer (gTimingRoot)
 
│ │ │ │ +size_t gtsam::internal::getTicTocID (const char *descriptionC)
 
│ │ │ │ +void gtsam::internal::tic (size_t id, const char *labelC)
 
│ │ │ │ +void gtsam::internal::toc (size_t id, const char *label)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Fast sampling without replacement.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
Date
May 2019
│ │ │ │ +

Timing utilities.

│ │ │ │ +
Author
Richard Roberts, Michael Kaess
│ │ │ │ +
Date
Oct 5, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,26 +1,39 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -WeightedSampler.h File Reference │ │ │ │ │ -Fast sampling without replacement. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_W_e_i_g_h_t_e_d_S_a_m_p_l_e_r_<_ _E_n_g_i_n_e_ _> │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +timing.cpp File Reference │ │ │ │ │ +Timing utilities. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +GTSAM_EXPORT boost::shared_ptr< ggttssaamm::::iinntteerrnnaall::::ggTTiimmiinnggRRoooott (new │ │ │ │ │ + _T_i_m_i_n_g_O_u_t_l_i_n_e >  _T_i_m_i_n_g_O_u_t_l_i_n_e("Total", getTicTocID │ │ │ │ │ + ("Total"))) │ │ │ │ │ +  │ │ │ │ │ + GTSAM_EXPORT boost::weak_ptr< ggttssaamm::::iinntteerrnnaall::::ggCCuurrrreennttTTiimmeerr │ │ │ │ │ + _T_i_m_i_n_g_O_u_t_l_i_n_e >  (gTimingRoot) │ │ │ │ │ +  │ │ │ │ │ + size_t  ggttssaamm::::iinntteerrnnaall::::ggeettTTiiccTTooccIIDD (const char │ │ │ │ │ + *descriptionC) │ │ │ │ │ +  │ │ │ │ │ + void  ggttssaamm::::iinntteerrnnaall::::ttiicc (size_t id, const char │ │ │ │ │ + *labelC) │ │ │ │ │ +  │ │ │ │ │ + void  ggttssaamm::::iinntteerrnnaall::::ttoocc (size_t id, const char │ │ │ │ │ + *label) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Fast sampling without replacement. │ │ │ │ │ +Timing utilities. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Richard Roberts, Michael Kaess │ │ │ │ │ Date │ │ │ │ │ - May 2019 │ │ │ │ │ + Oct 5, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _W_e_i_g_h_t_e_d_S_a_m_p_l_e_r_._h │ │ │ │ │ + * _t_i_m_i_n_g_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00143_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/GenericValue.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/VerticalBlockMatrix.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,281 +98,252 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
GenericValue.h
│ │ │ │ +
VerticalBlockMatrix.h
│ │ │ │
│ │ │ │
│ │ │ │ -
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
12/*
│ │ │ │ -
13 * @file GenericValue.h
│ │ │ │ -
14 * @brief Wraps any type T so it can play as a Value
│ │ │ │ -
15 * @date October, 2014
│ │ │ │ -
16 * @author Michael Bosse, Abel Gawel, Renaud Dube
│ │ │ │ -
17 * based on DerivedValue.h by Duy Nguyen Ta
│ │ │ │ -
18 */
│ │ │ │ +
18#pragma once
│ │ │ │
19
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/base/Manifold.h>
│ │ │ │ -
23#include <gtsam/base/types.h>
│ │ │ │ -
24#include <gtsam/base/Value.h>
│ │ │ │ +
20#include <gtsam/base/Matrix.h>
│ │ │ │ +
21#include <gtsam/base/MatrixSerialization.h>
│ │ │ │ +
22#include <gtsam/base/FastVector.h>
│ │ │ │ +
23
│ │ │ │ +
24namespace gtsam {
│ │ │ │
25
│ │ │ │ -
26#include <boost/make_shared.hpp>
│ │ │ │ -
27#include <boost/pool/pool_alloc.hpp>
│ │ │ │ +
26 // Forward declarations
│ │ │ │ +
27 class SymmetricBlockMatrix;
│ │ │ │
28
│ │ │ │ -
29#include <cmath>
│ │ │ │ -
30#include <iostream>
│ │ │ │ -
31#include <typeinfo> // operator typeid
│ │ │ │ -
32
│ │ │ │ -
33#ifdef _WIN32
│ │ │ │ -
34#define GENERICVALUE_VISIBILITY
│ │ │ │ -
35#else
│ │ │ │ -
36// This will trigger a LNKxxxx on MSVC, so disable for MSVC build
│ │ │ │ -
37// Please refer to https://github.com/borglab/gtsam/blob/develop/Using-GTSAM-EXPORT.md
│ │ │ │ -
38#define GENERICVALUE_VISIBILITY GTSAM_EXPORT
│ │ │ │ -
39#endif
│ │ │ │ -
40
│ │ │ │ -
41namespace gtsam {
│ │ │ │ -
42
│ │ │ │ -
46template<class T>
│ │ │ │ -
│ │ │ │ -
47class GenericValue: public Value {
│ │ │ │ +
│ │ │ │ +
42 class GTSAM_EXPORT VerticalBlockMatrix
│ │ │ │ +
43 {
│ │ │ │ +
44 public:
│ │ │ │ +
45 typedef VerticalBlockMatrix This;
│ │ │ │ +
46 typedef Eigen::Block<Matrix> Block;
│ │ │ │ +
47 typedef Eigen::Block<const Matrix> constBlock;
│ │ │ │
48
│ │ │ │ -
49public:
│ │ │ │ -
50
│ │ │ │ -
51 typedef T type;
│ │ │ │ +
49 protected:
│ │ │ │ +
50 Matrix matrix_;
│ │ │ │ +
51 FastVector<DenseIndex> variableColOffsets_;
│ │ │ │
52
│ │ │ │ -
53protected:
│ │ │ │ -
54
│ │ │ │ -
55 T value_;
│ │ │ │ +
53 DenseIndex rowStart_;
│ │ │ │ +
54 DenseIndex rowEnd_;
│ │ │ │ +
55 DenseIndex blockStart_;
│ │ │ │
56
│ │ │ │ -
57public:
│ │ │ │ -
58 // Only needed for serialization.
│ │ │ │ -
59 GenericValue(){}
│ │ │ │ -
60
│ │ │ │ -
│ │ │ │ -
62 GenericValue(const T& value) :
│ │ │ │ -
63 value_(value) {
│ │ │ │ -
64 }
│ │ │ │ -
│ │ │ │ -
65
│ │ │ │ -
│ │ │ │ -
67 const T& value() const {
│ │ │ │ -
68 return value_;
│ │ │ │ -
69 }
│ │ │ │ -
│ │ │ │ -
70
│ │ │ │ -
│ │ │ │ -
72 T& value() {
│ │ │ │ -
73 return value_;
│ │ │ │ -
74 }
│ │ │ │ -
│ │ │ │ -
75
│ │ │ │ -
│ │ │ │ -
77 ~GenericValue() override {
│ │ │ │ -
78 }
│ │ │ │ -
│ │ │ │ -
79
│ │ │ │ -
│ │ │ │ -
81 bool equals_(const Value& p, double tol = 1e-9) const override {
│ │ │ │ -
82 // Cast the base class Value pointer to a templated generic class pointer
│ │ │ │ -
83 const GenericValue& genericValue2 = static_cast<const GenericValue&>(p);
│ │ │ │ -
84 // Return the result of using the equals traits for the derived class
│ │ │ │ -
85 return traits<T>::Equals(this->value_, genericValue2.value_, tol);
│ │ │ │ -
86 }
│ │ │ │ -
│ │ │ │ -
87
│ │ │ │ -
│ │ │ │ -
89 bool equals(const GenericValue &other, double tol = 1e-9) const {
│ │ │ │ -
90 return traits<T>::Equals(this->value(), other.value(), tol);
│ │ │ │ -
91 }
│ │ │ │ -
│ │ │ │ -
92
│ │ │ │ -
│ │ │ │ -
94 void print(const std::string& str) const override {
│ │ │ │ -
95 std::cout << "(" << demangle(typeid(T).name()) << ")\n";
│ │ │ │ -
96 traits<T>::Print(value_, str);
│ │ │ │ -
97 }
│ │ │ │ -
│ │ │ │ -
98
│ │ │ │ -
│ │ │ │ -
102 Value* clone_() const override {
│ │ │ │ -
103 GenericValue* ptr = new GenericValue(*this); // calls copy constructor to fill in
│ │ │ │ -
104 return ptr;
│ │ │ │ -
105 }
│ │ │ │ -
│ │ │ │ -
106
│ │ │ │ -
│ │ │ │ -
110 void deallocate_() const override {
│ │ │ │ -
111 delete this;
│ │ │ │ -
112 }
│ │ │ │ -
│ │ │ │ +
57 public:
│ │ │ │ +
58
│ │ │ │ +
│ │ │ │ +
60 VerticalBlockMatrix() :
│ │ │ │ +
61 rowStart_(0), rowEnd_(0), blockStart_(0)
│ │ │ │ +
62 {
│ │ │ │ +
63 variableColOffsets_.push_back(0);
│ │ │ │ +
64 assertInvariants();
│ │ │ │ +
65 }
│ │ │ │ +
│ │ │ │ +
66
│ │ │ │ +
68 template<typename CONTAINER>
│ │ │ │ +
│ │ │ │ +
69 VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height,
│ │ │ │ +
70 bool appendOneDimension = false) :
│ │ │ │ +
71 variableColOffsets_(dimensions.size() + (appendOneDimension ? 2 : 1)),
│ │ │ │ +
72 rowStart_(0), rowEnd_(height), blockStart_(0) {
│ │ │ │ +
73 fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
│ │ │ │ +
74 matrix_.resize(height, variableColOffsets_.back());
│ │ │ │ +
75 assertInvariants();
│ │ │ │ +
76 }
│ │ │ │ +
│ │ │ │ +
77
│ │ │ │ +
79 template<typename CONTAINER, typename DERIVED>
│ │ │ │ +
│ │ │ │ +
80 VerticalBlockMatrix(const CONTAINER& dimensions,
│ │ │ │ +
81 const Eigen::MatrixBase<DERIVED>& matrix, bool appendOneDimension = false) :
│ │ │ │ +
82 matrix_(matrix), variableColOffsets_(dimensions.size() + (appendOneDimension ? 2 : 1)),
│ │ │ │ +
83 rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) {
│ │ │ │ +
84 fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
│ │ │ │ +
85 if (variableColOffsets_.back() != matrix_.cols())
│ │ │ │ +
86 throw std::invalid_argument(
│ │ │ │ +
87 "Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix.");
│ │ │ │ +
88 assertInvariants();
│ │ │ │ +
89 }
│ │ │ │ +
│ │ │ │ +
90
│ │ │ │ +
92 template<typename ITERATOR>
│ │ │ │ +
│ │ │ │ +
93 VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim,
│ │ │ │ +
94 DenseIndex height, bool appendOneDimension = false) :
│ │ │ │ +
95 variableColOffsets_((lastBlockDim-firstBlockDim) + (appendOneDimension ? 2 : 1)),
│ │ │ │ +
96 rowStart_(0), rowEnd_(height), blockStart_(0) {
│ │ │ │ +
97 fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension);
│ │ │ │ +
98 matrix_.resize(height, variableColOffsets_.back());
│ │ │ │ +
99 assertInvariants();
│ │ │ │ +
100 }
│ │ │ │ +
│ │ │ │ +
101
│ │ │ │ +
107 static VerticalBlockMatrix LikeActiveViewOf(const VerticalBlockMatrix& rhs);
│ │ │ │ +
108
│ │ │ │ +
112 static VerticalBlockMatrix LikeActiveViewOf(const SymmetricBlockMatrix& rhs, DenseIndex height);
│ │ │ │
113
│ │ │ │ -
│ │ │ │ -
117 boost::shared_ptr<Value> clone() const override {
│ │ │ │ -
118 return boost::allocate_shared<GenericValue>(Eigen::aligned_allocator<GenericValue>(), *this);
│ │ │ │ -
119 }
│ │ │ │ -
│ │ │ │ -
120
│ │ │ │ -
│ │ │ │ -
122 Value* retract_(const Vector& delta) const override {
│ │ │ │ -
123 // Call retract on the derived class using the retract trait function
│ │ │ │ -
124 const T retractResult = traits<T>::Retract(GenericValue<T>::value(), delta);
│ │ │ │ +
115 DenseIndex rows() const { assertInvariants(); return rowEnd_ - rowStart_; }
│ │ │ │ +
116
│ │ │ │ +
118 DenseIndex cols() const { assertInvariants(); return variableColOffsets_.back() - variableColOffsets_[blockStart_]; }
│ │ │ │ +
119
│ │ │ │ +
121 DenseIndex nBlocks() const { assertInvariants(); return variableColOffsets_.size() - 1 - blockStart_; }
│ │ │ │ +
122
│ │ │ │ +
124 Block operator()(DenseIndex block) { return range(block, block+1); }
│ │ │ │
125
│ │ │ │ -
126 Value* resultAsValue = new GenericValue(retractResult);
│ │ │ │ -
127
│ │ │ │ -
128 // Return the pointer to the Value base class
│ │ │ │ -
129 return resultAsValue;
│ │ │ │ -
130 }
│ │ │ │ -
│ │ │ │ -
131
│ │ │ │ -
│ │ │ │ -
133 Vector localCoordinates_(const Value& value2) const override {
│ │ │ │ -
134 // Cast the base class Value pointer to a templated generic class pointer
│ │ │ │ -
135 const GenericValue<T>& genericValue2 =
│ │ │ │ -
136 static_cast<const GenericValue<T>&>(value2);
│ │ │ │ -
137
│ │ │ │ -
138 // Return the result of calling localCoordinates trait on the derived class
│ │ │ │ -
139 return traits<T>::Local(GenericValue<T>::value(), genericValue2.value());
│ │ │ │ -
140 }
│ │ │ │ -
│ │ │ │ -
141
│ │ │ │ -
│ │ │ │ -
143 GenericValue retract(const Vector& delta) const {
│ │ │ │ -
144 return GenericValue(traits<T>::Retract(GenericValue<T>::value(), delta));
│ │ │ │ -
145 }
│ │ │ │ -
│ │ │ │ -
146
│ │ │ │ -
│ │ │ │ -
148 Vector localCoordinates(const GenericValue& value2) const {
│ │ │ │ -
149 return localCoordinates_(value2);
│ │ │ │ -
150 }
│ │ │ │ -
│ │ │ │ -
151
│ │ │ │ -
│ │ │ │ -
153 size_t dim() const override {
│ │ │ │ -
154 return traits<T>::GetDimension(value_);
│ │ │ │ -
155 }
│ │ │ │ -
│ │ │ │ -
156
│ │ │ │ -
│ │ │ │ -
158 Value& operator=(const Value& rhs) override {
│ │ │ │ -
159 // Cast the base class Value pointer to a derived class pointer
│ │ │ │ -
160 const GenericValue& derivedRhs = static_cast<const GenericValue&>(rhs);
│ │ │ │ +
127 const constBlock operator()(DenseIndex block) const { return range(block, block+1); }
│ │ │ │ +
128
│ │ │ │ +
│ │ │ │ +
130 Block range(DenseIndex startBlock, DenseIndex endBlock) {
│ │ │ │ +
131 assertInvariants();
│ │ │ │ +
132 DenseIndex actualStartBlock = startBlock + blockStart_;
│ │ │ │ +
133 DenseIndex actualEndBlock = endBlock + blockStart_;
│ │ │ │ +
134 if(startBlock != 0 || endBlock != 0) {
│ │ │ │ +
135 checkBlock(actualStartBlock);
│ │ │ │ +
136 assert(actualEndBlock < (DenseIndex)variableColOffsets_.size());
│ │ │ │ +
137 }
│ │ │ │ +
138 const DenseIndex startCol = variableColOffsets_[actualStartBlock];
│ │ │ │ +
139 const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - startCol;
│ │ │ │ +
140 return matrix_.block(rowStart_, startCol, this->rows(), rangeCols);
│ │ │ │ +
141 }
│ │ │ │ +
│ │ │ │ +
142
│ │ │ │ +
143 const constBlock range(DenseIndex startBlock, DenseIndex endBlock) const {
│ │ │ │ +
144 assertInvariants();
│ │ │ │ +
145 DenseIndex actualStartBlock = startBlock + blockStart_;
│ │ │ │ +
146 DenseIndex actualEndBlock = endBlock + blockStart_;
│ │ │ │ +
147 if(startBlock != 0 || endBlock != 0) {
│ │ │ │ +
148 checkBlock(actualStartBlock);
│ │ │ │ +
149 assert(actualEndBlock < (DenseIndex)variableColOffsets_.size());
│ │ │ │ +
150 }
│ │ │ │ +
151 const DenseIndex startCol = variableColOffsets_[actualStartBlock];
│ │ │ │ +
152 const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - startCol;
│ │ │ │ +
153 return ((const Matrix&)matrix_).block(rowStart_, startCol, this->rows(), rangeCols);
│ │ │ │ +
154 }
│ │ │ │ +
155
│ │ │ │ +
157 Block full() { return range(0, nBlocks()); }
│ │ │ │ +
158
│ │ │ │ +
160 const constBlock full() const { return range(0, nBlocks()); }
│ │ │ │
161
│ │ │ │ -
162 // Do the assignment and return the result
│ │ │ │ -
163 *this = GenericValue(derivedRhs); // calls copy constructor
│ │ │ │ -
164 return *this;
│ │ │ │ -
165 }
│ │ │ │ -
│ │ │ │ -
166
│ │ │ │ -
167 protected:
│ │ │ │ +
162 DenseIndex offset(DenseIndex block) const {
│ │ │ │ +
163 assertInvariants();
│ │ │ │ +
164 DenseIndex actualBlock = block + blockStart_;
│ │ │ │ +
165 checkBlock(actualBlock);
│ │ │ │ +
166 return variableColOffsets_[actualBlock];
│ │ │ │ +
167 }
│ │ │ │
168
│ │ │ │ -
│ │ │ │ -
171 GenericValue<T>& operator=(const GenericValue<T>& rhs) {
│ │ │ │ -
172 Value::operator=(static_cast<Value const&>(rhs));
│ │ │ │ -
173 value_ = rhs.value_;
│ │ │ │ -
174 return *this;
│ │ │ │ -
175 }
│ │ │ │ -
│ │ │ │ -
176
│ │ │ │ -
177 private:
│ │ │ │ -
178
│ │ │ │ -
180 friend class boost::serialization::access;
│ │ │ │ -
181 template<class ARCHIVE>
│ │ │ │ -
182 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
183 ar & boost::serialization::make_nvp("GenericValue",
│ │ │ │ -
184 boost::serialization::base_object<Value>(*this));
│ │ │ │ -
185 ar & boost::serialization::make_nvp("value", value_);
│ │ │ │ -
186 }
│ │ │ │ -
187
│ │ │ │ -
188
│ │ │ │ -
189 // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
│ │ │ │ -
190 enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
│ │ │ │ -
191public:
│ │ │ │ -
192 GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
│ │ │ │ -
193};
│ │ │ │ -
│ │ │ │ -
194
│ │ │ │ -
196#define GTSAM_VALUE_EXPORT(Type) BOOST_CLASS_EXPORT(gtsam::GenericValue<Type>)
│ │ │ │ -
197
│ │ │ │ -
198// traits
│ │ │ │ -
199template <typename ValueType>
│ │ │ │ -
│ │ │ │ -
200struct traits<GenericValue<ValueType> >
│ │ │ │ -
201 : public Testable<GenericValue<ValueType> > {};
│ │ │ │ -
│ │ │ │ -
202
│ │ │ │ -
203// define Value::cast here since now GenericValue has been declared
│ │ │ │ -
204template<typename ValueType>
│ │ │ │ -
│ │ │ │ -
205const ValueType& Value::cast() const {
│ │ │ │ -
206 return dynamic_cast<const GenericValue<ValueType>&>(*this).value();
│ │ │ │ -
207}
│ │ │ │ -
│ │ │ │ +
170 const DenseIndex& rowStart() const { return rowStart_; }
│ │ │ │ +
171
│ │ │ │ +
173 DenseIndex& rowStart() { return rowStart_; }
│ │ │ │ +
174
│ │ │ │ +
176 const DenseIndex& rowEnd() const { return rowEnd_; }
│ │ │ │ +
177
│ │ │ │ +
179 DenseIndex& rowEnd() { return rowEnd_; }
│ │ │ │ +
180
│ │ │ │ +
182 const DenseIndex& firstBlock() const { return blockStart_; }
│ │ │ │ +
183
│ │ │ │ +
185 DenseIndex& firstBlock() { return blockStart_; }
│ │ │ │ +
186
│ │ │ │ +
188 const Matrix& matrix() const { return matrix_; }
│ │ │ │ +
189
│ │ │ │ +
191 Matrix& matrix() { return matrix_; }
│ │ │ │ +
192
│ │ │ │ +
193 protected:
│ │ │ │ +
194 void assertInvariants() const {
│ │ │ │ +
195 assert(matrix_.cols() == variableColOffsets_.back());
│ │ │ │ +
196 assert(blockStart_ < (DenseIndex)variableColOffsets_.size());
│ │ │ │ +
197 assert(rowStart_ <= matrix_.rows());
│ │ │ │ +
198 assert(rowEnd_ <= matrix_.rows());
│ │ │ │ +
199 assert(rowStart_ <= rowEnd_);
│ │ │ │ +
200 }
│ │ │ │ +
201
│ │ │ │ +
202 void checkBlock(DenseIndex block) const {
│ │ │ │ +
203 static_cast<void>(block); //Disable unused varibale warnings.
│ │ │ │ +
204 assert(matrix_.cols() == variableColOffsets_.back());
│ │ │ │ +
205 assert(block < (DenseIndex)variableColOffsets_.size() - 1);
│ │ │ │ +
206 assert(variableColOffsets_[block] < matrix_.cols() && variableColOffsets_[block+1] <= matrix_.cols());
│ │ │ │ +
207 }
│ │ │ │
208
│ │ │ │ -
211template<class T>
│ │ │ │ -
│ │ │ │ -
212GenericValue<T> genericValue(const T& v) {
│ │ │ │ -
213 return GenericValue<T>(v);
│ │ │ │ -
214}
│ │ │ │ -
│ │ │ │ -
215
│ │ │ │ -
216
│ │ │ │ -
217} /* namespace gtsam */
│ │ │ │ -
Value.h
The base class for any variable that can be optimized or used in a factor.
│ │ │ │ -
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ -
types.h
Typedefs for easier changing of types.
│ │ │ │ -
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
This marks a GTSAM object to require alignment.
Definition types.h:317
│ │ │ │ +
209 template<typename ITERATOR>
│ │ │ │ +
210 void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension) {
│ │ │ │ +
211 variableColOffsets_[0] = 0;
│ │ │ │ +
212 DenseIndex j=0;
│ │ │ │ +
213 for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim, ++j)
│ │ │ │ +
214 variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
│ │ │ │ +
215 if(appendOneDimension)
│ │ │ │ +
216 variableColOffsets_[j+1] = variableColOffsets_[j] + 1;
│ │ │ │ +
217 }
│ │ │ │ +
218
│ │ │ │ +
219 friend class SymmetricBlockMatrix;
│ │ │ │ +
220
│ │ │ │ +
221 private:
│ │ │ │ +
223 friend class boost::serialization::access;
│ │ │ │ +
224 template<class ARCHIVE>
│ │ │ │ +
225 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
226 ar & BOOST_SERIALIZATION_NVP(matrix_);
│ │ │ │ +
227 ar & BOOST_SERIALIZATION_NVP(variableColOffsets_);
│ │ │ │ +
228 ar & BOOST_SERIALIZATION_NVP(rowStart_);
│ │ │ │ +
229 ar & BOOST_SERIALIZATION_NVP(rowEnd_);
│ │ │ │ +
230 ar & BOOST_SERIALIZATION_NVP(blockStart_);
│ │ │ │ +
231 }
│ │ │ │ +
232 };
│ │ │ │ +
│ │ │ │ +
233
│ │ │ │ +
234}
│ │ │ │ +
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ +
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
MatrixSerialization.h
Serialization for matrices.
│ │ │ │ +
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::demangle
std::string demangle(const char *name)
Pretty print Value type name.
Definition types.cpp:37
│ │ │ │ -
gtsam::genericValue
GenericValue< T > genericValue(const T &v)
Functional constructor of GenericValue<T> so T can be automatically deduced.
Definition GenericValue.h:212
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::GenericValue
Wraps any type T so it can play as a Value.
Definition GenericValue.h:47
│ │ │ │ -
gtsam::GenericValue::clone_
Value * clone_() const override
Create a duplicate object returned as a pointer to the generic Value interface.
Definition GenericValue.h:102
│ │ │ │ -
gtsam::GenericValue::value
T & value()
Return the value.
Definition GenericValue.h:72
│ │ │ │ -
gtsam::GenericValue::retract_
Value * retract_(const Vector &delta) const override
Generic Value interface version of retract.
Definition GenericValue.h:122
│ │ │ │ -
gtsam::GenericValue::operator=
Value & operator=(const Value &rhs) override
Assignment operator.
Definition GenericValue.h:158
│ │ │ │ -
gtsam::GenericValue::print
void print(const std::string &str) const override
Virtual print function, uses traits.
Definition GenericValue.h:94
│ │ │ │ -
gtsam::GenericValue::equals
bool equals(const GenericValue &other, double tol=1e-9) const
non virtual equals function, uses traits
Definition GenericValue.h:89
│ │ │ │ -
gtsam::GenericValue::dim
size_t dim() const override
Return run-time dimensionality.
Definition GenericValue.h:153
│ │ │ │ -
gtsam::GenericValue::GenericValue
GenericValue(const T &value)
Construct from value.
Definition GenericValue.h:62
│ │ │ │ -
gtsam::GenericValue::deallocate_
void deallocate_() const override
Destroy and deallocate this object, only if it was originally allocated using clone_().
Definition GenericValue.h:110
│ │ │ │ -
gtsam::GenericValue::value_
T value_
The wrapped value.
Definition GenericValue.h:55
│ │ │ │ -
gtsam::GenericValue::operator=
GenericValue< T > & operator=(const GenericValue< T > &rhs)
Assignment operator, protected because only the Value or DERIVED assignment operators should be used.
Definition GenericValue.h:171
│ │ │ │ -
gtsam::GenericValue::localCoordinates
Vector localCoordinates(const GenericValue &value2) const
Non-virtual version of localCoordinates.
Definition GenericValue.h:148
│ │ │ │ -
gtsam::GenericValue::access
friend class boost::serialization::access
Serialization function.
Definition GenericValue.h:180
│ │ │ │ -
gtsam::GenericValue::equals_
bool equals_(const Value &p, double tol=1e-9) const override
equals implementing generic Value interface
Definition GenericValue.h:81
│ │ │ │ -
gtsam::GenericValue::value
const T & value() const
Return a constant value.
Definition GenericValue.h:67
│ │ │ │ -
gtsam::GenericValue::~GenericValue
~GenericValue() override
Destructor.
Definition GenericValue.h:77
│ │ │ │ -
gtsam::GenericValue::retract
GenericValue retract(const Vector &delta) const
Non-virtual version of retract.
Definition GenericValue.h:143
│ │ │ │ -
gtsam::GenericValue::localCoordinates_
Vector localCoordinates_(const Value &value2) const override
Generic Value interface version of localCoordinates.
Definition GenericValue.h:133
│ │ │ │ -
gtsam::GenericValue::clone
boost::shared_ptr< Value > clone() const override
Clone this value (normal clone on the heap, delete with 'delete' operator)
Definition GenericValue.h:117
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::Value
This is the base class for any type to be stored in Values.
Definition Value.h:37
│ │ │ │ -
gtsam::Value::cast
const ValueType & cast() const
Cast to known ValueType.
Definition GenericValue.h:205
│ │ │ │ -
gtsam::Value::operator=
virtual Value & operator=(const Value &)
Assignment operator.
Definition Value.h:79
│ │ │ │ +
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │ +
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
│ │ │ │ +
gtsam::VerticalBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43
│ │ │ │ +
gtsam::VerticalBlockMatrix::VerticalBlockMatrix
VerticalBlockMatrix()
Construct an empty VerticalBlockMatrix.
Definition VerticalBlockMatrix.h:60
│ │ │ │ +
gtsam::VerticalBlockMatrix::full
Block full()
Return the full matrix, not including any portions excluded by rowStart(), rowEnd(),...
Definition VerticalBlockMatrix.h:157
│ │ │ │ +
gtsam::VerticalBlockMatrix::range
Block range(DenseIndex startBlock, DenseIndex endBlock)
access ranges of blocks at a time
Definition VerticalBlockMatrix.h:130
│ │ │ │ +
gtsam::VerticalBlockMatrix::firstBlock
const DenseIndex & firstBlock() const
Get the apparent first block for all operations.
Definition VerticalBlockMatrix.h:182
│ │ │ │ +
gtsam::VerticalBlockMatrix::matrix
const Matrix & matrix() const
Access to full matrix (including any portions excluded by rowStart(), rowEnd(), and firstBlock())
Definition VerticalBlockMatrix.h:188
│ │ │ │ +
gtsam::VerticalBlockMatrix::VerticalBlockMatrix
VerticalBlockMatrix(const CONTAINER &dimensions, DenseIndex height, bool appendOneDimension=false)
Construct from a container of the sizes of each vertical block.
Definition VerticalBlockMatrix.h:69
│ │ │ │ +
gtsam::VerticalBlockMatrix::rowEnd_
DenseIndex rowEnd_
Changes apparent matrix view, see main class comment.
Definition VerticalBlockMatrix.h:54
│ │ │ │ +
gtsam::VerticalBlockMatrix::rowEnd
const DenseIndex & rowEnd() const
Get the apparent last row (exclusive, i.e.
Definition VerticalBlockMatrix.h:176
│ │ │ │ +
gtsam::VerticalBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition VerticalBlockMatrix.h:115
│ │ │ │ +
gtsam::VerticalBlockMatrix::operator()
Block operator()(DenseIndex block)
Access a single block in the underlying matrix with read/write access.
Definition VerticalBlockMatrix.h:124
│ │ │ │ +
gtsam::VerticalBlockMatrix::firstBlock
DenseIndex & firstBlock()
Get or set the apparent first block for all operations.
Definition VerticalBlockMatrix.h:185
│ │ │ │ +
gtsam::VerticalBlockMatrix::cols
DenseIndex cols() const
Column size.
Definition VerticalBlockMatrix.h:118
│ │ │ │ +
gtsam::VerticalBlockMatrix::VerticalBlockMatrix
VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height, bool appendOneDimension=false)
Construct from iterator over the sizes of each vertical block.
Definition VerticalBlockMatrix.h:93
│ │ │ │ +
gtsam::VerticalBlockMatrix::operator()
const constBlock operator()(DenseIndex block) const
Access a const block view.
Definition VerticalBlockMatrix.h:127
│ │ │ │ +
gtsam::VerticalBlockMatrix::rowStart
const DenseIndex & rowStart() const
Get the apparent first row of the underlying matrix for all operations.
Definition VerticalBlockMatrix.h:170
│ │ │ │ +
gtsam::VerticalBlockMatrix::matrix_
Matrix matrix_
The full matrix.
Definition VerticalBlockMatrix.h:50
│ │ │ │ +
gtsam::VerticalBlockMatrix::nBlocks
DenseIndex nBlocks() const
Block count.
Definition VerticalBlockMatrix.h:121
│ │ │ │ +
gtsam::VerticalBlockMatrix::full
const constBlock full() const
Return the full matrix, not including any portions excluded by rowStart(), rowEnd(),...
Definition VerticalBlockMatrix.h:160
│ │ │ │ +
gtsam::VerticalBlockMatrix::rowStart
DenseIndex & rowStart()
Get or set the apparent first row of the underlying matrix for all operations.
Definition VerticalBlockMatrix.h:173
│ │ │ │ +
gtsam::VerticalBlockMatrix::matrix
Matrix & matrix()
Non-const access to full matrix (including any portions excluded by rowStart(), rowEnd(),...
Definition VerticalBlockMatrix.h:191
│ │ │ │ +
gtsam::VerticalBlockMatrix::rowEnd
DenseIndex & rowEnd()
Get or set the apparent last row (exclusive, i.e.
Definition VerticalBlockMatrix.h:179
│ │ │ │ +
gtsam::VerticalBlockMatrix::rowStart_
DenseIndex rowStart_
Changes apparent matrix view, see main class comment.
Definition VerticalBlockMatrix.h:53
│ │ │ │ +
gtsam::VerticalBlockMatrix::VerticalBlockMatrix
VerticalBlockMatrix(const CONTAINER &dimensions, const Eigen::MatrixBase< DERIVED > &matrix, bool appendOneDimension=false)
Construct from a container of the sizes of each vertical block and a pre-prepared matrix.
Definition VerticalBlockMatrix.h:80
│ │ │ │ +
gtsam::VerticalBlockMatrix::blockStart_
DenseIndex blockStart_
Changes apparent matrix view, see main class comment.
Definition VerticalBlockMatrix.h:55
│ │ │ │ +
gtsam::VerticalBlockMatrix::variableColOffsets_
FastVector< DenseIndex > variableColOffsets_
the starting columns of each block (0-based)
Definition VerticalBlockMatrix.h:51
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,324 +1,346 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GenericValue.h │ │ │ │ │ +VerticalBlockMatrix.h │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -12/* │ │ │ │ │ -13 * @file GenericValue.h │ │ │ │ │ -14 * @brief Wraps any type T so it can play as a Value │ │ │ │ │ -15 * @date October, 2014 │ │ │ │ │ -16 * @author Michael Bosse, Abel Gawel, Renaud Dube │ │ │ │ │ -17 * based on DerivedValue.h by Duy Nguyen Ta │ │ │ │ │ -18 */ │ │ │ │ │ +18#pragma once │ │ │ │ │ 19 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_b_a_s_e_/_V_a_l_u_e_._h> │ │ │ │ │ +20#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_S_e_r_i_a_l_i_z_a_t_i_o_n_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ +23 │ │ │ │ │ +24namespace _g_t_s_a_m { │ │ │ │ │ 25 │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ +26 // Forward declarations │ │ │ │ │ +27 class SymmetricBlockMatrix; │ │ │ │ │ 28 │ │ │ │ │ -29#include │ │ │ │ │ -30#include │ │ │ │ │ -31#include // operator typeid │ │ │ │ │ -32 │ │ │ │ │ -33#ifdef _WIN32 │ │ │ │ │ -34#define GENERICVALUE_VISIBILITY │ │ │ │ │ -35#else │ │ │ │ │ -36// This will trigger a LNKxxxx on MSVC, so disable for MSVC build │ │ │ │ │ -37// Please refer to https://github.com/borglab/gtsam/blob/develop/Using-GTSAM- │ │ │ │ │ -EXPORT.md │ │ │ │ │ -38#define GENERICVALUE_VISIBILITY GTSAM_EXPORT │ │ │ │ │ -39#endif │ │ │ │ │ -40 │ │ │ │ │ -41namespace _g_t_s_a_m { │ │ │ │ │ -42 │ │ │ │ │ -46template │ │ │ │ │ -_4_7class _G_e_n_e_r_i_c_V_a_l_u_e: public _V_a_l_u_e { │ │ │ │ │ +_4_2 class GTSAM_EXPORT _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +43 { │ │ │ │ │ +44 public: │ │ │ │ │ +45 typedef _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x _T_h_i_s; │ │ │ │ │ +46 typedef Eigen::Block Block; │ │ │ │ │ +47 typedef Eigen::Block constBlock; │ │ │ │ │ 48 │ │ │ │ │ -49public: │ │ │ │ │ -50 │ │ │ │ │ -51 typedef T type; │ │ │ │ │ +49 protected: │ │ │ │ │ +_5_0 Matrix _m_a_t_r_i_x__; │ │ │ │ │ +_5_1 _F_a_s_t_V_e_c_t_o_r_<_D_e_n_s_e_I_n_d_e_x_> _v_a_r_i_a_b_l_e_C_o_l_O_f_f_s_e_t_s__; │ │ │ │ │ 52 │ │ │ │ │ -53protected: │ │ │ │ │ -54 │ │ │ │ │ -_5_5 T _v_a_l_u_e__; │ │ │ │ │ +_5_3 _D_e_n_s_e_I_n_d_e_x _r_o_w_S_t_a_r_t__; │ │ │ │ │ +_5_4 _D_e_n_s_e_I_n_d_e_x _r_o_w_E_n_d__; │ │ │ │ │ +_5_5 _D_e_n_s_e_I_n_d_e_x _b_l_o_c_k_S_t_a_r_t__; │ │ │ │ │ 56 │ │ │ │ │ -57public: │ │ │ │ │ -58 // Only needed for serialization. │ │ │ │ │ -59 _G_e_n_e_r_i_c_V_a_l_u_e(){} │ │ │ │ │ -60 │ │ │ │ │ -_6_2 _G_e_n_e_r_i_c_V_a_l_u_e(const T& _v_a_l_u_e) : │ │ │ │ │ -63 _v_a_l_u_e__(_v_a_l_u_e) { │ │ │ │ │ -64 } │ │ │ │ │ -65 │ │ │ │ │ -_6_7 const T& _v_a_l_u_e() const { │ │ │ │ │ -68 return _v_a_l_u_e__; │ │ │ │ │ -69 } │ │ │ │ │ -70 │ │ │ │ │ -_7_2 T& _v_a_l_u_e() { │ │ │ │ │ -73 return _v_a_l_u_e__; │ │ │ │ │ -74 } │ │ │ │ │ -75 │ │ │ │ │ -_7_7 _~_G_e_n_e_r_i_c_V_a_l_u_e() override { │ │ │ │ │ -78 } │ │ │ │ │ -79 │ │ │ │ │ -_8_1 bool _e_q_u_a_l_s__(const _V_a_l_u_e& p, double tol = 1e-9) const override { │ │ │ │ │ -82 // Cast the base class Value pointer to a templated generic class pointer │ │ │ │ │ -83 const _G_e_n_e_r_i_c_V_a_l_u_e& genericValue2 = static_cast(p); │ │ │ │ │ -84 // Return the result of using the equals traits for the derived class │ │ │ │ │ -85 return _t_r_a_i_t_s_<_T_>_:_:_E_q_u_a_l_s(this->value_, genericValue2._v_a_l_u_e__, tol); │ │ │ │ │ -86 } │ │ │ │ │ -87 │ │ │ │ │ -_8_9 bool _e_q_u_a_l_s(const _G_e_n_e_r_i_c_V_a_l_u_e &other, double tol = 1e-9) const { │ │ │ │ │ -90 return _t_r_a_i_t_s_<_T_>_:_:_E_q_u_a_l_s(this->_v_a_l_u_e(), other._v_a_l_u_e(), tol); │ │ │ │ │ -91 } │ │ │ │ │ -92 │ │ │ │ │ -_9_4 void _p_r_i_n_t(const std::string& str) const override { │ │ │ │ │ -95 std::cout << "(" << _d_e_m_a_n_g_l_e(typeid(T).name()) << ")\n"; │ │ │ │ │ -96 _t_r_a_i_t_s_<_T_>_:_:_P_r_i_n_t(_v_a_l_u_e__, str); │ │ │ │ │ -97 } │ │ │ │ │ -98 │ │ │ │ │ -_1_0_2 _V_a_l_u_e* _c_l_o_n_e__() const override { │ │ │ │ │ -103 _G_e_n_e_r_i_c_V_a_l_u_e* ptr = new _G_e_n_e_r_i_c_V_a_l_u_e(*this); // calls copy constructor to │ │ │ │ │ -fill in │ │ │ │ │ -104 return ptr; │ │ │ │ │ -105 } │ │ │ │ │ -106 │ │ │ │ │ -_1_1_0 void _d_e_a_l_l_o_c_a_t_e__() const override { │ │ │ │ │ -111 delete this; │ │ │ │ │ -112 } │ │ │ │ │ +57 public: │ │ │ │ │ +58 │ │ │ │ │ +_6_0 _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x() : │ │ │ │ │ +61 rowStart_(0), rowEnd_(0), blockStart_(0) │ │ │ │ │ +62 { │ │ │ │ │ +63 variableColOffsets_.push_back(0); │ │ │ │ │ +64 assertInvariants(); │ │ │ │ │ +65 } │ │ │ │ │ +66 │ │ │ │ │ +68 template │ │ │ │ │ +_6_9 _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x(const CONTAINER& dimensions, _D_e_n_s_e_I_n_d_e_x height, │ │ │ │ │ +70 bool appendOneDimension = false) : │ │ │ │ │ +71 variableColOffsets_(dimensions.size() + (appendOneDimension ? 2 : 1)), │ │ │ │ │ +72 rowStart_(0), rowEnd_(height), blockStart_(0) { │ │ │ │ │ +73 fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); │ │ │ │ │ +74 matrix_.resize(height, variableColOffsets_.back()); │ │ │ │ │ +75 assertInvariants(); │ │ │ │ │ +76 } │ │ │ │ │ +77 │ │ │ │ │ +79 template │ │ │ │ │ +_8_0 _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x(const CONTAINER& dimensions, │ │ │ │ │ +81 const Eigen::MatrixBase& matrix, bool appendOneDimension = false) : │ │ │ │ │ +82 matrix_(matrix), variableColOffsets_(dimensions.size() + (appendOneDimension │ │ │ │ │ +? 2 : 1)), │ │ │ │ │ +83 rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) { │ │ │ │ │ +84 fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); │ │ │ │ │ +85 if (variableColOffsets_.back() != matrix_.cols()) │ │ │ │ │ +86 throw std::invalid_argument( │ │ │ │ │ +87 "Requested to create a VerticalBlockMatrix with dimensions that do not sum │ │ │ │ │ +to the total columns of the provided matrix."); │ │ │ │ │ +88 assertInvariants(); │ │ │ │ │ +89 } │ │ │ │ │ +90 │ │ │ │ │ +92 template │ │ │ │ │ +_9_3 _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x(ITERATOR firstBlockDim, ITERATOR lastBlockDim, │ │ │ │ │ +94 _D_e_n_s_e_I_n_d_e_x height, bool appendOneDimension = false) : │ │ │ │ │ +95 variableColOffsets_((lastBlockDim-firstBlockDim) + (appendOneDimension ? 2 : │ │ │ │ │ +1)), │ │ │ │ │ +96 rowStart_(0), rowEnd_(height), blockStart_(0) { │ │ │ │ │ +97 fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension); │ │ │ │ │ +98 matrix_.resize(height, variableColOffsets_.back()); │ │ │ │ │ +99 assertInvariants(); │ │ │ │ │ +100 } │ │ │ │ │ +101 │ │ │ │ │ +107 static _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x LikeActiveViewOf(const _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x& │ │ │ │ │ +rhs); │ │ │ │ │ +108 │ │ │ │ │ +112 static _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x LikeActiveViewOf(const _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x& │ │ │ │ │ +rhs, _D_e_n_s_e_I_n_d_e_x height); │ │ │ │ │ 113 │ │ │ │ │ -_1_1_7 boost::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -118 return boost::allocate_shared(Eigen:: │ │ │ │ │ -aligned_allocator(), *this); │ │ │ │ │ -119 } │ │ │ │ │ -120 │ │ │ │ │ -_1_2_2 _V_a_l_u_e* _r_e_t_r_a_c_t__(const Vector& delta) const override { │ │ │ │ │ -123 // Call retract on the derived class using the retract trait function │ │ │ │ │ -124 const T retractResult = _t_r_a_i_t_s_<_T_>_:_:_R_e_t_r_a_c_t(_G_e_n_e_r_i_c_V_a_l_u_e_<_T_>_:_:_v_a_l_u_e(), │ │ │ │ │ -delta); │ │ │ │ │ +_1_1_5 _D_e_n_s_e_I_n_d_e_x _r_o_w_s() const { assertInvariants(); return rowEnd_ - rowStart_; } │ │ │ │ │ +116 │ │ │ │ │ +_1_1_8 _D_e_n_s_e_I_n_d_e_x _c_o_l_s() const { assertInvariants(); return │ │ │ │ │ +variableColOffsets_.back() - variableColOffsets_[blockStart_]; } │ │ │ │ │ +119 │ │ │ │ │ +_1_2_1 _D_e_n_s_e_I_n_d_e_x _n_B_l_o_c_k_s() const { assertInvariants(); return │ │ │ │ │ +variableColOffsets_.size() - 1 - blockStart_; } │ │ │ │ │ +122 │ │ │ │ │ +_1_2_4 Block _o_p_e_r_a_t_o_r_(_)(_D_e_n_s_e_I_n_d_e_x block) { return range(block, block+1); } │ │ │ │ │ 125 │ │ │ │ │ -126 _V_a_l_u_e* resultAsValue = new _G_e_n_e_r_i_c_V_a_l_u_e(retractResult); │ │ │ │ │ -127 │ │ │ │ │ -128 // Return the pointer to the Value base class │ │ │ │ │ -129 return resultAsValue; │ │ │ │ │ -130 } │ │ │ │ │ -131 │ │ │ │ │ -_1_3_3 Vector _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s__(const _V_a_l_u_e& value2) const override { │ │ │ │ │ -134 // Cast the base class Value pointer to a templated generic class pointer │ │ │ │ │ -135 const _G_e_n_e_r_i_c_V_a_l_u_e_<_T_>& genericValue2 = │ │ │ │ │ -136 static_cast&>(value2); │ │ │ │ │ -137 │ │ │ │ │ -138 // Return the result of calling localCoordinates trait on the derived class │ │ │ │ │ -139 return _t_r_a_i_t_s_<_T_>_:_:_L_o_c_a_l(_G_e_n_e_r_i_c_V_a_l_u_e_<_T_>_:_:_v_a_l_u_e(), genericValue2._v_a_l_u_e()); │ │ │ │ │ -140 } │ │ │ │ │ -141 │ │ │ │ │ -_1_4_3 _G_e_n_e_r_i_c_V_a_l_u_e _r_e_t_r_a_c_t(const Vector& delta) const { │ │ │ │ │ -144 return _G_e_n_e_r_i_c_V_a_l_u_e(_t_r_a_i_t_s_<_T_>_:_:_R_e_t_r_a_c_t(_G_e_n_e_r_i_c_V_a_l_u_e_<_T_>_:_:_v_a_l_u_e(), delta)); │ │ │ │ │ -145 } │ │ │ │ │ -146 │ │ │ │ │ -_1_4_8 Vector _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(const _G_e_n_e_r_i_c_V_a_l_u_e& value2) const { │ │ │ │ │ -149 return _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s__(value2); │ │ │ │ │ +_1_2_7 const constBlock _o_p_e_r_a_t_o_r_(_)(_D_e_n_s_e_I_n_d_e_x block) const { return range(block, │ │ │ │ │ +block+1); } │ │ │ │ │ +128 │ │ │ │ │ +_1_3_0 Block _r_a_n_g_e(_D_e_n_s_e_I_n_d_e_x startBlock, _D_e_n_s_e_I_n_d_e_x endBlock) { │ │ │ │ │ +131 assertInvariants(); │ │ │ │ │ +132 _D_e_n_s_e_I_n_d_e_x actualStartBlock = startBlock + blockStart_; │ │ │ │ │ +133 _D_e_n_s_e_I_n_d_e_x actualEndBlock = endBlock + blockStart_; │ │ │ │ │ +134 if(startBlock != 0 || endBlock != 0) { │ │ │ │ │ +135 checkBlock(actualStartBlock); │ │ │ │ │ +136 assert(actualEndBlock < (_D_e_n_s_e_I_n_d_e_x)variableColOffsets_.size()); │ │ │ │ │ +137 } │ │ │ │ │ +138 const _D_e_n_s_e_I_n_d_e_x startCol = variableColOffsets_[actualStartBlock]; │ │ │ │ │ +139 const _D_e_n_s_e_I_n_d_e_x rangeCols = variableColOffsets_[actualEndBlock] - │ │ │ │ │ +startCol; │ │ │ │ │ +140 return matrix_.block(rowStart_, startCol, this->rows(), rangeCols); │ │ │ │ │ +141 } │ │ │ │ │ +142 │ │ │ │ │ +143 const constBlock range(_D_e_n_s_e_I_n_d_e_x startBlock, _D_e_n_s_e_I_n_d_e_x endBlock) const { │ │ │ │ │ +144 assertInvariants(); │ │ │ │ │ +145 _D_e_n_s_e_I_n_d_e_x actualStartBlock = startBlock + blockStart_; │ │ │ │ │ +146 _D_e_n_s_e_I_n_d_e_x actualEndBlock = endBlock + blockStart_; │ │ │ │ │ +147 if(startBlock != 0 || endBlock != 0) { │ │ │ │ │ +148 checkBlock(actualStartBlock); │ │ │ │ │ +149 assert(actualEndBlock < (_D_e_n_s_e_I_n_d_e_x)variableColOffsets_.size()); │ │ │ │ │ 150 } │ │ │ │ │ -151 │ │ │ │ │ -_1_5_3 size_t _d_i_m() const override { │ │ │ │ │ -154 return _t_r_a_i_t_s_<_T_>_:_:_G_e_t_D_i_m_e_n_s_i_o_n(_v_a_l_u_e__); │ │ │ │ │ -155 } │ │ │ │ │ -156 │ │ │ │ │ -_1_5_8 _V_a_l_u_e& _o_p_e_r_a_t_o_r_=(const _V_a_l_u_e& rhs) override { │ │ │ │ │ -159 // Cast the base class Value pointer to a derived class pointer │ │ │ │ │ -160 const _G_e_n_e_r_i_c_V_a_l_u_e& derivedRhs = static_cast(rhs); │ │ │ │ │ +151 const DenseIndex startCol = variableColOffsets_[actualStartBlock]; │ │ │ │ │ +152 const DenseIndex rangeCols = variableColOffsets_[actualEndBlock] - │ │ │ │ │ +startCol; │ │ │ │ │ +153 return ((const Matrix&)matrix_).block(rowStart_, startCol, this->rows(), │ │ │ │ │ +rangeCols); │ │ │ │ │ +154 } │ │ │ │ │ +155 │ │ │ │ │ +_1_5_7 Block _f_u_l_l() { return range(0, nBlocks()); } │ │ │ │ │ +158 │ │ │ │ │ +_1_6_0 const constBlock _f_u_l_l() const { return range(0, nBlocks()); } │ │ │ │ │ 161 │ │ │ │ │ -162 // Do the assignment and return the result │ │ │ │ │ -163 *this = _G_e_n_e_r_i_c_V_a_l_u_e(derivedRhs); // calls copy constructor │ │ │ │ │ -164 return *this; │ │ │ │ │ -165 } │ │ │ │ │ -166 │ │ │ │ │ -167 protected: │ │ │ │ │ +162 _D_e_n_s_e_I_n_d_e_x offset(_D_e_n_s_e_I_n_d_e_x block) const { │ │ │ │ │ +163 assertInvariants(); │ │ │ │ │ +164 _D_e_n_s_e_I_n_d_e_x actualBlock = block + blockStart_; │ │ │ │ │ +165 checkBlock(actualBlock); │ │ │ │ │ +166 return variableColOffsets_[actualBlock]; │ │ │ │ │ +167 } │ │ │ │ │ 168 │ │ │ │ │ -_1_7_1 _G_e_n_e_r_i_c_V_a_l_u_e_<_T_>& _o_p_e_r_a_t_o_r_=(const _G_e_n_e_r_i_c_V_a_l_u_e_<_T_>& rhs) { │ │ │ │ │ -172 _V_a_l_u_e_:_:_o_p_e_r_a_t_o_r_=(static_cast<_V_a_l_u_e const&>(rhs)); │ │ │ │ │ -173 _v_a_l_u_e__ = rhs._v_a_l_u_e__; │ │ │ │ │ -174 return *this; │ │ │ │ │ -175 } │ │ │ │ │ -176 │ │ │ │ │ -177 private: │ │ │ │ │ -178 │ │ │ │ │ -_1_8_0 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -181 template │ │ │ │ │ -182 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -183 ar & boost::serialization::make_nvp("GenericValue", │ │ │ │ │ -184 boost::serialization::base_object(*this)); │ │ │ │ │ -185 ar & boost::serialization::make_nvp("value", _v_a_l_u_e__); │ │ │ │ │ -186 } │ │ │ │ │ -187 │ │ │ │ │ -188 │ │ │ │ │ -189 // Alignment, see https://eigen.tuxfamily.org/dox/ │ │ │ │ │ -group__TopicStructHavingEigenMembers.html │ │ │ │ │ -190 enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; │ │ │ │ │ -191public: │ │ │ │ │ -192 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W___I_F(NeedsToAlign) │ │ │ │ │ -193}; │ │ │ │ │ -194 │ │ │ │ │ -196#define GTSAM_VALUE_EXPORT(Type) BOOST_CLASS_EXPORT(gtsam:: │ │ │ │ │ -GenericValue) │ │ │ │ │ -197 │ │ │ │ │ -198// traits │ │ │ │ │ -199template │ │ │ │ │ -_2_0_0struct _t_r_a_i_t_s<_G_e_n_e_r_i_c_V_a_l_u_e > │ │ │ │ │ -201 : public _T_e_s_t_a_b_l_e > {}; │ │ │ │ │ -202 │ │ │ │ │ -203// define Value::cast here since now GenericValue has been declared │ │ │ │ │ -204template │ │ │ │ │ -_2_0_5const ValueType& _V_a_l_u_e_:_:_c_a_s_t() const { │ │ │ │ │ -206 return dynamic_cast&>(*this)._v_a_l_u_e(); │ │ │ │ │ -207} │ │ │ │ │ +_1_7_0 const _D_e_n_s_e_I_n_d_e_x& _r_o_w_S_t_a_r_t() const { return rowStart_; } │ │ │ │ │ +171 │ │ │ │ │ +_1_7_3 _D_e_n_s_e_I_n_d_e_x& _r_o_w_S_t_a_r_t() { return rowStart_; } │ │ │ │ │ +174 │ │ │ │ │ +_1_7_6 const _D_e_n_s_e_I_n_d_e_x& _r_o_w_E_n_d() const { return rowEnd_; } │ │ │ │ │ +177 │ │ │ │ │ +_1_7_9 _D_e_n_s_e_I_n_d_e_x& _r_o_w_E_n_d() { return rowEnd_; } │ │ │ │ │ +180 │ │ │ │ │ +_1_8_2 const _D_e_n_s_e_I_n_d_e_x& _f_i_r_s_t_B_l_o_c_k() const { return blockStart_; } │ │ │ │ │ +183 │ │ │ │ │ +_1_8_5 _D_e_n_s_e_I_n_d_e_x& _f_i_r_s_t_B_l_o_c_k() { return blockStart_; } │ │ │ │ │ +186 │ │ │ │ │ +_1_8_8 const Matrix& _m_a_t_r_i_x() const { return matrix_; } │ │ │ │ │ +189 │ │ │ │ │ +_1_9_1 Matrix& _m_a_t_r_i_x() { return matrix_; } │ │ │ │ │ +192 │ │ │ │ │ +193 protected: │ │ │ │ │ +194 void assertInvariants() const { │ │ │ │ │ +195 assert(matrix_.cols() == variableColOffsets_.back()); │ │ │ │ │ +196 assert(blockStart_ < (_D_e_n_s_e_I_n_d_e_x)variableColOffsets_.size()); │ │ │ │ │ +197 assert(rowStart_ <= matrix_.rows()); │ │ │ │ │ +198 assert(rowEnd_ <= matrix_.rows()); │ │ │ │ │ +199 assert(rowStart_ <= rowEnd_); │ │ │ │ │ +200 } │ │ │ │ │ +201 │ │ │ │ │ +202 void checkBlock(DenseIndex block) const { │ │ │ │ │ +203 static_cast(block); //Disable unused varibale warnings. │ │ │ │ │ +204 assert(matrix_.cols() == variableColOffsets_.back()); │ │ │ │ │ +205 assert(block < (DenseIndex)variableColOffsets_.size() - 1); │ │ │ │ │ +206 assert(variableColOffsets_[block] < matrix_.cols() && variableColOffsets_ │ │ │ │ │ +[block+1] <= matrix_.cols()); │ │ │ │ │ +207 } │ │ │ │ │ 208 │ │ │ │ │ -211template │ │ │ │ │ -_2_1_2_G_e_n_e_r_i_c_V_a_l_u_e_<_T_> _g_e_n_e_r_i_c_V_a_l_u_e(const T& v) { │ │ │ │ │ -213 return _G_e_n_e_r_i_c_V_a_l_u_e_<_T_>(v); │ │ │ │ │ -214} │ │ │ │ │ -215 │ │ │ │ │ -216 │ │ │ │ │ -217} /* namespace gtsam */ │ │ │ │ │ -_V_a_l_u_e_._h │ │ │ │ │ -The base class for any variable that can be optimized or used in a factor. │ │ │ │ │ -_M_a_n_i_f_o_l_d_._h │ │ │ │ │ -Base class and basic functions for Manifold types. │ │ │ │ │ -_t_y_p_e_s_._h │ │ │ │ │ -Typedefs for easier changing of types. │ │ │ │ │ -_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W___I_F │ │ │ │ │ -#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) │ │ │ │ │ -This marks a GTSAM object to require alignment. │ │ │ │ │ -DDeeffiinniittiioonn types.h:317 │ │ │ │ │ +209 template │ │ │ │ │ +210 void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool │ │ │ │ │ +appendOneDimension) { │ │ │ │ │ +211 variableColOffsets_[0] = 0; │ │ │ │ │ +212 _D_e_n_s_e_I_n_d_e_x j=0; │ │ │ │ │ +213 for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim, ++j) │ │ │ │ │ +214 variableColOffsets_[j+1] = variableColOffsets_[j] + *dim; │ │ │ │ │ +215 if(appendOneDimension) │ │ │ │ │ +216 variableColOffsets_[j+1] = variableColOffsets_[j] + 1; │ │ │ │ │ +217 } │ │ │ │ │ +218 │ │ │ │ │ +219 friend class SymmetricBlockMatrix; │ │ │ │ │ +220 │ │ │ │ │ +221 private: │ │ │ │ │ +_2_2_3 friend class boost::serialization::access; │ │ │ │ │ +224 template │ │ │ │ │ +225 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +226 ar & BOOST_SERIALIZATION_NVP(matrix_); │ │ │ │ │ +227 ar & BOOST_SERIALIZATION_NVP(variableColOffsets_); │ │ │ │ │ +228 ar & BOOST_SERIALIZATION_NVP(rowStart_); │ │ │ │ │ +229 ar & BOOST_SERIALIZATION_NVP(rowEnd_); │ │ │ │ │ +230 ar & BOOST_SERIALIZATION_NVP(blockStart_); │ │ │ │ │ +231 } │ │ │ │ │ +232 }; │ │ │ │ │ +233 │ │ │ │ │ +234} │ │ │ │ │ +_F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ +A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ +_M_a_t_r_i_x_._h │ │ │ │ │ +typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +_M_a_t_r_i_x_S_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ +Serialization for matrices. │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ +std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ +FastVector │ │ │ │ │ +FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ +DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_d_e_m_a_n_g_l_e │ │ │ │ │ -std::string demangle(const char *name) │ │ │ │ │ -Pretty print Value type name. │ │ │ │ │ -DDeeffiinniittiioonn types.cpp:37 │ │ │ │ │ -_g_t_s_a_m_:_:_g_e_n_e_r_i_c_V_a_l_u_e │ │ │ │ │ -GenericValue< T > genericValue(const T &v) │ │ │ │ │ -Functional constructor of GenericValue so T can be automatically deduced. │ │ │ │ │ -DDeeffiinniittiioonn GenericValue.h:212 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e │ │ │ │ │ -Wraps any type T so it can play as a Value. │ │ │ │ │ -DDeeffiinniittiioonn GenericValue.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_c_l_o_n_e__ │ │ │ │ │ -Value * clone_() const override │ │ │ │ │ -Create a duplicate object returned as a pointer to the generic Value interface. │ │ │ │ │ -DDeeffiinniittiioonn GenericValue.h:102 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_v_a_l_u_e │ │ │ │ │ -T & value() │ │ │ │ │ -Return the value. │ │ │ │ │ -DDeeffiinniittiioonn GenericValue.h:72 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_r_e_t_r_a_c_t__ │ │ │ │ │ -Value * retract_(const Vector &delta) const override │ │ │ │ │ -Generic Value interface version of retract. │ │ │ │ │ -DDeeffiinniittiioonn GenericValue.h:122 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ -Value & operator=(const Value &rhs) override │ │ │ │ │ -Assignment operator. │ │ │ │ │ -DDeeffiinniittiioonn GenericValue.h:158 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &str) const override │ │ │ │ │ -Virtual print function, uses traits. │ │ │ │ │ -DDeeffiinniittiioonn GenericValue.h:94 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const GenericValue &other, double tol=1e-9) const │ │ │ │ │ -non virtual equals function, uses traits │ │ │ │ │ -DDeeffiinniittiioonn GenericValue.h:89 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_d_i_m │ │ │ │ │ -size_t dim() const override │ │ │ │ │ -Return run-time dimensionality. │ │ │ │ │ -DDeeffiinniittiioonn GenericValue.h:153 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_G_e_n_e_r_i_c_V_a_l_u_e │ │ │ │ │ -GenericValue(const T &value) │ │ │ │ │ -Construct from value. │ │ │ │ │ -DDeeffiinniittiioonn GenericValue.h:62 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_d_e_a_l_l_o_c_a_t_e__ │ │ │ │ │ -void deallocate_() const override │ │ │ │ │ -Destroy and deallocate this object, only if it was originally allocated using │ │ │ │ │ -clone_(). │ │ │ │ │ -DDeeffiinniittiioonn GenericValue.h:110 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_v_a_l_u_e__ │ │ │ │ │ -T value_ │ │ │ │ │ -The wrapped value. │ │ │ │ │ -DDeeffiinniittiioonn GenericValue.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ -GenericValue< T > & operator=(const GenericValue< T > &rhs) │ │ │ │ │ -Assignment operator, protected because only the Value or DERIVED assignment │ │ │ │ │ -operators should be used. │ │ │ │ │ -DDeeffiinniittiioonn GenericValue.h:171 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ -Vector localCoordinates(const GenericValue &value2) const │ │ │ │ │ -Non-virtual version of localCoordinates. │ │ │ │ │ -DDeeffiinniittiioonn GenericValue.h:148 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn GenericValue.h:180 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_e_q_u_a_l_s__ │ │ │ │ │ -bool equals_(const Value &p, double tol=1e-9) const override │ │ │ │ │ -equals implementing generic Value interface │ │ │ │ │ -DDeeffiinniittiioonn GenericValue.h:81 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_v_a_l_u_e │ │ │ │ │ -const T & value() const │ │ │ │ │ -Return a constant value. │ │ │ │ │ -DDeeffiinniittiioonn GenericValue.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_~_G_e_n_e_r_i_c_V_a_l_u_e │ │ │ │ │ -~GenericValue() override │ │ │ │ │ -Destructor. │ │ │ │ │ -DDeeffiinniittiioonn GenericValue.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_r_e_t_r_a_c_t │ │ │ │ │ -GenericValue retract(const Vector &delta) const │ │ │ │ │ -Non-virtual version of retract. │ │ │ │ │ -DDeeffiinniittiioonn GenericValue.h:143 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s__ │ │ │ │ │ -Vector localCoordinates_(const Value &value2) const override │ │ │ │ │ -Generic Value interface version of localCoordinates. │ │ │ │ │ -DDeeffiinniittiioonn GenericValue.h:133 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_c_l_o_n_e │ │ │ │ │ -boost::shared_ptr< Value > clone() const override │ │ │ │ │ -Clone this value (normal clone on the heap, delete with 'delete' operator) │ │ │ │ │ -DDeeffiinniittiioonn GenericValue.h:117 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e │ │ │ │ │ -This is the base class for any type to be stored in Values. │ │ │ │ │ -DDeeffiinniittiioonn Value.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_:_:_c_a_s_t │ │ │ │ │ -const ValueType & cast() const │ │ │ │ │ -Cast to known ValueType. │ │ │ │ │ -DDeeffiinniittiioonn GenericValue.h:205 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ -virtual Value & operator=(const Value &) │ │ │ │ │ -Assignment operator. │ │ │ │ │ -DDeeffiinniittiioonn Value.h:79 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_n_s_e_I_n_d_e_x │ │ │ │ │ +ptrdiff_t DenseIndex │ │ │ │ │ +The index type for Eigen objects. │ │ │ │ │ +DDeeffiinniittiioonn types.h:106 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ +of blocks. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ +of vertical blocks. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +VerticalBlockMatrix() │ │ │ │ │ +Construct an empty VerticalBlockMatrix. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:60 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_f_u_l_l │ │ │ │ │ +Block full() │ │ │ │ │ +Return the full matrix, not including any portions excluded by rowStart(), │ │ │ │ │ +rowEnd(),... │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:157 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_r_a_n_g_e │ │ │ │ │ +Block range(DenseIndex startBlock, DenseIndex endBlock) │ │ │ │ │ +access ranges of blocks at a time │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:130 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_f_i_r_s_t_B_l_o_c_k │ │ │ │ │ +const DenseIndex & firstBlock() const │ │ │ │ │ +Get the apparent first block for all operations. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:182 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_m_a_t_r_i_x │ │ │ │ │ +const Matrix & matrix() const │ │ │ │ │ +Access to full matrix (including any portions excluded by rowStart(), rowEnd(), │ │ │ │ │ +and firstBlock()) │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:188 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +VerticalBlockMatrix(const CONTAINER &dimensions, DenseIndex height, bool │ │ │ │ │ +appendOneDimension=false) │ │ │ │ │ +Construct from a container of the sizes of each vertical block. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:69 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_r_o_w_E_n_d__ │ │ │ │ │ +DenseIndex rowEnd_ │ │ │ │ │ +Changes apparent matrix view, see main class comment. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:54 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_r_o_w_E_n_d │ │ │ │ │ +const DenseIndex & rowEnd() const │ │ │ │ │ +Get the apparent last row (exclusive, i.e. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:176 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_r_o_w_s │ │ │ │ │ +DenseIndex rows() const │ │ │ │ │ +Row size. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:115 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ +Block operator()(DenseIndex block) │ │ │ │ │ +Access a single block in the underlying matrix with read/write access. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:124 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_f_i_r_s_t_B_l_o_c_k │ │ │ │ │ +DenseIndex & firstBlock() │ │ │ │ │ +Get or set the apparent first block for all operations. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:185 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_c_o_l_s │ │ │ │ │ +DenseIndex cols() const │ │ │ │ │ +Column size. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:118 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex │ │ │ │ │ +height, bool appendOneDimension=false) │ │ │ │ │ +Construct from iterator over the sizes of each vertical block. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:93 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ +const constBlock operator()(DenseIndex block) const │ │ │ │ │ +Access a const block view. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:127 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_r_o_w_S_t_a_r_t │ │ │ │ │ +const DenseIndex & rowStart() const │ │ │ │ │ +Get the apparent first row of the underlying matrix for all operations. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:170 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_m_a_t_r_i_x__ │ │ │ │ │ +Matrix matrix_ │ │ │ │ │ +The full matrix. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_n_B_l_o_c_k_s │ │ │ │ │ +DenseIndex nBlocks() const │ │ │ │ │ +Block count. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:121 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_f_u_l_l │ │ │ │ │ +const constBlock full() const │ │ │ │ │ +Return the full matrix, not including any portions excluded by rowStart(), │ │ │ │ │ +rowEnd(),... │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:160 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_r_o_w_S_t_a_r_t │ │ │ │ │ +DenseIndex & rowStart() │ │ │ │ │ +Get or set the apparent first row of the underlying matrix for all operations. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:173 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_m_a_t_r_i_x │ │ │ │ │ +Matrix & matrix() │ │ │ │ │ +Non-const access to full matrix (including any portions excluded by rowStart(), │ │ │ │ │ +rowEnd(),... │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:191 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_r_o_w_E_n_d │ │ │ │ │ +DenseIndex & rowEnd() │ │ │ │ │ +Get or set the apparent last row (exclusive, i.e. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:179 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_r_o_w_S_t_a_r_t__ │ │ │ │ │ +DenseIndex rowStart_ │ │ │ │ │ +Changes apparent matrix view, see main class comment. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:53 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +VerticalBlockMatrix(const CONTAINER &dimensions, const Eigen::MatrixBase< │ │ │ │ │ +DERIVED > &matrix, bool appendOneDimension=false) │ │ │ │ │ +Construct from a container of the sizes of each vertical block and a pre- │ │ │ │ │ +prepared matrix. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:80 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_b_l_o_c_k_S_t_a_r_t__ │ │ │ │ │ +DenseIndex blockStart_ │ │ │ │ │ +Changes apparent matrix view, see main class comment. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_v_a_r_i_a_b_l_e_C_o_l_O_f_f_s_e_t_s__ │ │ │ │ │ +FastVector< DenseIndex > variableColOffsets_ │ │ │ │ │ +the starting columns of each block (0-based) │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:51 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * GGeenneerriiccVVaalluuee..hh │ │ │ │ │ + * _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00146.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Matrix.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/TestableAssertions.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,299 +94,105 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ -Macros | │ │ │ │ -Typedefs | │ │ │ │ Functions
│ │ │ │ -
Matrix.h File Reference
│ │ │ │ +
TestableAssertions.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

typedef and functions to augment Eigen's MatrixXd │ │ │ │ +

Provides additional testing facilities for common data structures. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

struct  gtsam::Reshape< OutM, OutN, OutOptions, InM, InN, InOptions >
 Reshape functor. More...
 
struct  gtsam::Reshape< M, M, InOptions, M, M, InOptions >
 Reshape specialization that does nothing as shape stays the same (needed to not be ambiguous for square input equals square output) More...
 
struct  gtsam::Reshape< M, N, InOptions, M, N, InOptions >
 Reshape specialization that does nothing as shape stays the same. More...
 
struct  gtsam::Reshape< N, M, InOptions, M, N, InOptions >
 Reshape specialization that does transpose. More...
 
struct  gtsam::MultiplyWithInverse< N >
 Functor that implements multiplication of a vector b with the inverse of a matrix A. More...
 
struct  gtsam::MultiplyWithInverseFunction< T, N >
 Functor that implements multiplication with the inverse of a matrix, itself the result of a function f. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Macros

#define GTSAM_MAKE_MATRIX_DEFS(N)
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Typedefs

│ │ │ │ -typedef Eigen::MatrixXd gtsam::Matrix
 
│ │ │ │ -typedef Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > gtsam::MatrixRowMajor
 
│ │ │ │ -typedef Eigen::Block< Matrix > gtsam::SubMatrix
 
│ │ │ │ -typedef Eigen::Block< const Matrix > gtsam::ConstSubMatrix
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

│ │ │ │ -const Eigen::IOFormat & gtsam::matlabFormat ()
 
│ │ │ │ -template<class MATRIX >
bool gtsam::equal_with_abs_tol (const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
 equals with a tolerance
 
│ │ │ │ -bool gtsam::operator== (const Matrix &A, const Matrix &B)
 equality is just equal_with_abs_tol 1e-9
 
│ │ │ │ -bool gtsam::operator!= (const Matrix &A, const Matrix &B)
 inequality
 
│ │ │ │ -bool gtsam::assert_equal (const Matrix &A, const Matrix &B, double tol=1e-9)
 equals with an tolerance, prints out message if unequal
 
│ │ │ │ -bool gtsam::assert_inequal (const Matrix &A, const Matrix &B, double tol=1e-9)
 inequals with an tolerance, prints out message if within tolerance
 
│ │ │ │ -bool gtsam::assert_equal (const std::list< Matrix > &As, const std::list< Matrix > &Bs, double tol=1e-9)
 equals with an tolerance, prints out message if unequal
 
│ │ │ │ -bool gtsam::linear_independent (const Matrix &A, const Matrix &B, double tol=1e-9)
 check whether the rows of two matrices are linear independent
 
│ │ │ │ -bool gtsam::linear_dependent (const Matrix &A, const Matrix &B, double tol=1e-9)
 check whether the rows of two matrices are linear dependent
 
│ │ │ │ -Vector gtsam::operator^ (const Matrix &A, const Vector &v)
 overload ^ for trans(A)*v We transpose the vectors for speed.
 
│ │ │ │ -template<class MATRIX >
MATRIX gtsam::prod (const MATRIX &A, const MATRIX &B)
 products using old-style format to improve compatibility
 
│ │ │ │ -void gtsam::print (const Matrix &A, const std::string &s, std::ostream &stream)
 print without optional string, must specify cout yourself
 
│ │ │ │ -void gtsam::print (const Matrix &A, const std::string &s="")
 print with optional string to cout
 
│ │ │ │ -void gtsam::save (const Matrix &A, const std::string &s, const std::string &filename)
 save a matrix to file, which can be loaded by matlab
 
istream & gtsam::operator>> (std::istream &inputStream, Matrix &destinationMatrix)
 Read a matrix from an input stream, such as a file.
 
template<class MATRIX >
Eigen::Block< const MATRIX > gtsam::sub (const MATRIX &A, size_t i1, size_t i2, size_t j1, size_t j2)
 extract submatrix, slice semantics, i.e.
 
template<typename Derived1 , typename Derived2 >
void gtsam::insertSub (Eigen::MatrixBase< Derived1 > &fullMatrix, const Eigen::MatrixBase< Derived2 > &subMatrix, size_t i, size_t j)
 insert a submatrix IN PLACE at a specified location in a larger matrix NOTE: there is no size checking
 
│ │ │ │ -Matrix gtsam::diag (const std::vector< Matrix > &Hs)
 Create a matrix with submatrices along its diagonal.
 
template<class MATRIX >
const MATRIX::ConstColXpr gtsam::column (const MATRIX &A, size_t j)
 Extracts a column view from a matrix that avoids a copy.
 
template<class MATRIX >
const MATRIX::ConstRowXpr gtsam::row (const MATRIX &A, size_t j)
 Extracts a row view from a matrix that avoids a copy.
 
template<class MATRIX >
void gtsam::zeroBelowDiagonal (MATRIX &A, size_t cols=0)
 Zeros all of the elements below the diagonal of a matrix, in place.
 
│ │ │ │ -Matrix gtsam::trans (const Matrix &A)
 static transpose function, just calls Eigen transpose member function
 
│ │ │ │ -template<int OutM, int OutN, int OutOptions, int InM, int InN, int InOptions>
Reshape< OutM, OutN, OutOptions, InM, InN, InOptions >::ReshapedType gtsam::reshape (const Eigen::Matrix< double, InM, InN, InOptions > &m)
 
pair< Matrix, Matrix > gtsam::qr (const Matrix &A)
 Householder QR factorization, Golub & Van Loan p 224, explicit version
│ │ │ │ -
 
void gtsam::inplace_QR (Matrix &A)
 QR factorization using Eigen's internal block QR algorithm.
 
list< boost::tuple< Vector, double, double > > gtsam::weighted_eliminate (Matrix &A, Vector &b, const Vector &sigmas)
 Imperative algorithm for in-place full elimination with weights and constraint handling.
 
void gtsam::householder_ (Matrix &A, size_t k, bool copy_vectors)
 Imperative version of Householder QR factorization, Golub & Van Loan p 224 version with Householder vectors below diagonal, as in GVL.
 
void gtsam::householder (Matrix &A, size_t k)
 Householder tranformation, zeros below diagonal.
 
Vector gtsam::backSubstituteUpper (const Matrix &U, const Vector &b, bool unit=false)
 backSubstitute U*x=b
 
Vector gtsam::backSubstituteUpper (const Vector &b, const Matrix &U, bool unit=false)
 backSubstitute x'*U=b'
 
Vector gtsam::backSubstituteLower (const Matrix &L, const Vector &b, bool unit=false)
 backSubstitute L*x=b
 
Matrix gtsam::stack (size_t nrMatrices,...)
 create a matrix by stacking other matrices Given a set of matrices: A1, A2, A3...
 
│ │ │ │ -Matrix gtsam::stack (const std::vector< Matrix > &blocks)
 
Matrix gtsam::collect (const std::vector< const Matrix * > &matrices, size_t m=0, size_t n=0)
 create a matrix by concatenating Given a set of matrices: A1, A2, A3... If all matrices have the same size, specifying single matrix dimensions will avoid the lookup of dimensions
 
│ │ │ │ -Matrix gtsam::collect (size_t nrMatrices,...)
 
void gtsam::vector_scale_inplace (const Vector &v, Matrix &A, bool inf_mask=false)
 scales a matrix row or column by the values in a vector Arguments (Matrix, Vector) scales the columns, (Vector, Matrix) scales the rows
 
│ │ │ │ -Matrix gtsam::vector_scale (const Vector &v, const Matrix &A, bool inf_mask)
 
│ │ │ │ -Matrix gtsam::vector_scale (const Matrix &A, const Vector &v, bool inf_mask)
 
Matrix3 gtsam::skewSymmetric (double wx, double wy, double wz)
 skew symmetric matrix returns this: 0 -wz wy wz 0 -wx -wy wx 0
 
│ │ │ │ -template<class Derived >
Matrix3 gtsam::skewSymmetric (const Eigen::MatrixBase< Derived > &w)
 
│ │ │ │ -Matrix gtsam::inverse_square_root (const Matrix &A)
 Use Cholesky to calculate inverse square root of a matrix.
 
Matrix gtsam::cholesky_inverse (const Matrix &A)
 Return the inverse of a S.P.D.
 
void gtsam::svd (const Matrix &A, Matrix &U, Vector &S, Matrix &V)
 SVD computes economy SVD A=U*S*V'.
 
boost::tuple< int, double, Vector > gtsam::DLT (const Matrix &A, double rank_tol=1e-9)
 Direct linear transform algorithm that calls svd to find a vector v that minimizes the algebraic error A*v.
 
Matrix gtsam::expm (const Matrix &A, size_t K=7)
 Numerical exponential map, naive approach, not industrial strength !!!
 
│ │ │ │ -std::string gtsam::formatMatrixIndented (const std::string &label, const Matrix &matrix, bool makeVectorHorizontal)
 
│ │ │ │ -Matrix gtsam::LLt (const Matrix &A)
 
│ │ │ │ -Matrix gtsam::RtR (const Matrix &A)
 
│ │ │ │ -Vector gtsam::columnNormSquare (const Matrix &A)
 
│ │ │ │ +bool gtsam::assert_equal (const Key &expected, const Key &actual, double tol=0.0)
 Equals testing for basic types.
 
template<class V >
bool gtsam::assert_equal (const boost::optional< V > &expected, const boost::optional< V > &actual, double tol=1e-9)
 Comparisons for boost.optional objects that checks whether objects exist before comparing their values.
 
│ │ │ │ +template<class V >
bool gtsam::assert_equal (const V &expected, const boost::optional< V > &actual, double tol=1e-9)
 
│ │ │ │ +template<class V >
bool gtsam::assert_equal (const V &expected, const boost::optional< const V & > &actual, double tol=1e-9)
 
│ │ │ │ +template<class V1 , class V2 >
bool gtsam::assert_container_equal (const std::map< V1, V2 > &expected, const std::map< V1, V2 > &actual, double tol=1e-9)
 Function for comparing maps of testable->testable TODO: replace with more generalized version.
 
│ │ │ │ +template<class V2 >
bool gtsam::assert_container_equal (const std::map< size_t, V2 > &expected, const std::map< size_t, V2 > &actual, double tol=1e-9)
 Function for comparing maps of size_t->testable.
 
│ │ │ │ +template<class V1 , class V2 >
bool gtsam::assert_container_equal (const std::vector< std::pair< V1, V2 > > &expected, const std::vector< std::pair< V1, V2 > > &actual, double tol=1e-9)
 Function for comparing vector of pairs (testable, testable)
 
│ │ │ │ +template<class V >
bool gtsam::assert_container_equal (const V &expected, const V &actual, double tol=1e-9)
 General function for comparing containers of testable objects.
 
│ │ │ │ +template<class V2 >
bool gtsam::assert_container_equality (const std::map< size_t, V2 > &expected, const std::map< size_t, V2 > &actual)
 Function for comparing maps of size_t->testable Types are assumed to have operator ==.
 
│ │ │ │ +template<class V >
bool gtsam::assert_container_equality (const V &expected, const V &actual)
 General function for comparing containers of objects with operator==.
 
│ │ │ │ +bool gtsam::assert_equal (const std::string &expected, const std::string &actual)
 Compare strings for unit tests.
 
│ │ │ │ +template<class V >
bool gtsam::assert_inequal (const V &expected, const V &actual, double tol=1e-9)
 Allow for testing inequality.
 
│ │ │ │ +template<class V >
bool gtsam::assert_stdout_equal (const std::string &expected, const V &actual)
 Capture std out via cout stream and compare against string.
 
template<class V >
bool gtsam::assert_print_equal (const std::string &expected, const V &actual, const std::string &s="")
 Capture print function output and compare against string.
 
│ │ │ │

Detailed Description

│ │ │ │ -

typedef and functions to augment Eigen's MatrixXd

│ │ │ │ -
Author
Christian Potthast
│ │ │ │ -
│ │ │ │ -Kai Ni
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ -
│ │ │ │ -Alex Cunningham
│ │ │ │ -
│ │ │ │ -Alex Hagiopol
│ │ │ │ -
│ │ │ │ -Varun Agrawal
│ │ │ │ -

Macro Definition Documentation

│ │ │ │ - │ │ │ │ -

◆ GTSAM_MAKE_MATRIX_DEFS

│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
#define GTSAM_MAKE_MATRIX_DEFS( N)
│ │ │ │ -
│ │ │ │ -Value:
using Matrix##N = Eigen::Matrix<double, N, N>; \
│ │ │ │ -
using Matrix1##N = Eigen::Matrix<double, 1, N>; \
│ │ │ │ -
using Matrix2##N = Eigen::Matrix<double, 2, N>; \
│ │ │ │ -
using Matrix3##N = Eigen::Matrix<double, 3, N>; \
│ │ │ │ -
using Matrix4##N = Eigen::Matrix<double, 4, N>; \
│ │ │ │ -
using Matrix5##N = Eigen::Matrix<double, 5, N>; \
│ │ │ │ -
using Matrix6##N = Eigen::Matrix<double, 6, N>; \
│ │ │ │ -
using Matrix7##N = Eigen::Matrix<double, 7, N>; \
│ │ │ │ -
using Matrix8##N = Eigen::Matrix<double, 8, N>; \
│ │ │ │ -
using Matrix9##N = Eigen::Matrix<double, 9, N>; \
│ │ │ │ -
static const Eigen::MatrixBase<Matrix##N>::IdentityReturnType I_##N##x##N = Matrix##N::Identity(); \
│ │ │ │ -
static const Eigen::MatrixBase<Matrix##N>::ConstantReturnType Z_##N##x##N = Matrix##N::Zero();
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ +

Provides additional testing facilities for common data structures.

│ │ │ │ +
Author
Alex Cunningham
│ │ │ │ +
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,308 +1,90 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Matrix.h File Reference │ │ │ │ │ -typedef and functions to augment Eigen's MatrixXd _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +TestableAssertions.h File Reference │ │ │ │ │ +Provides additional testing facilities for common data structures. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_R_e_s_h_a_p_e_<_ _O_u_t_M_,_ _O_u_t_N_,_ _O_u_t_O_p_t_i_o_n_s_,_ _I_n_M_,_ _I_n_N_,_ _I_n_O_p_t_i_o_n_s_ _> │ │ │ │ │ -  _R_e_s_h_a_p_e functor. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_R_e_s_h_a_p_e_<_ _M_,_ _M_,_ _I_n_O_p_t_i_o_n_s_,_ _M_,_ _M_,_ _I_n_O_p_t_i_o_n_s_ _> │ │ │ │ │ - _R_e_s_h_a_p_e specialization that does nothing as shape stays the same │ │ │ │ │ -  (needed to not be ambiguous for square input equals square output) │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_R_e_s_h_a_p_e_<_ _M_,_ _N_,_ _I_n_O_p_t_i_o_n_s_,_ _M_,_ _N_,_ _I_n_O_p_t_i_o_n_s_ _> │ │ │ │ │ -  _R_e_s_h_a_p_e specialization that does nothing as shape stays the same. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_R_e_s_h_a_p_e_<_ _N_,_ _M_,_ _I_n_O_p_t_i_o_n_s_,_ _M_,_ _N_,_ _I_n_O_p_t_i_o_n_s_ _> │ │ │ │ │ -  _R_e_s_h_a_p_e specialization that does transpose. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_M_u_l_t_i_p_l_y_W_i_t_h_I_n_v_e_r_s_e_<_ _N_ _> │ │ │ │ │ -  Functor that implements multiplication of a vector b with the inverse │ │ │ │ │ - of a matrix A. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_M_u_l_t_i_p_l_y_W_i_t_h_I_n_v_e_r_s_e_F_u_n_c_t_i_o_n_<_ _T_,_ _N_ _> │ │ │ │ │ -  Functor that implements multiplication with the inverse of a matrix, │ │ │ │ │ - itself the result of a function f. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -MMaaccrrooss │ │ │ │ │ -#define  _G_T_S_A_M___M_A_K_E___M_A_T_R_I_X___D_E_F_S(N) │ │ │ │ │ -  │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ - typedef Eigen::MatrixXd  ggttssaamm::::MMaattrriixx │ │ │ │ │ -  │ │ │ │ │ -typedef Eigen::Matrix< double, Eigen::Dynamic, Eigen:: │ │ │ │ │ - Dynamic, Eigen::RowMajor >  ggttssaamm::::MMaattrriixxRRoowwMMaajjoorr │ │ │ │ │ -  │ │ │ │ │ - typedef Eigen::Block< Matrix >  ggttssaamm::::SSuubbMMaattrriixx │ │ │ │ │ -  │ │ │ │ │ - typedef Eigen::Block< const Matrix >  ggttssaamm::::CCoonnssttSSuubbMMaattrriixx │ │ │ │ │ -  │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ - const Eigen::IOFormat &  ggttssaamm::::mmaattllaabbFFoorrmmaatt () │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - bool  ggttssaamm::::eeqquuaall__wwiitthh__aabbss__ttooll (const Eigen:: │ │ │ │ │ - DenseBase< MATRIX > &A, const Eigen:: │ │ │ │ │ - DenseBase< MATRIX > &B, double tol=1e-9) │ │ │ │ │ -  equals with a tolerance │ │ │ │ │ -  │ │ │ │ │ - bool  ggttssaamm::::ooppeerraattoorr==== (const Matrix &A, const │ │ │ │ │ - Matrix &B) │ │ │ │ │ -  equality is just equal_with_abs_tol 1e-9 │ │ │ │ │ -  │ │ │ │ │ - bool  ggttssaamm::::ooppeerraattoorr!!== (const Matrix &A, const │ │ │ │ │ - Matrix &B) │ │ │ │ │ -  inequality │ │ │ │ │ -  │ │ │ │ │ - bool  ggttssaamm::::aasssseerrtt__eeqquuaall (const Matrix &A, │ │ │ │ │ - const Matrix &B, double tol=1e-9) │ │ │ │ │ -  equals with an tolerance, prints out │ │ │ │ │ - message if unequal │ │ │ │ │ -  │ │ │ │ │ - bool  ggttssaamm::::aasssseerrtt__iinneeqquuaall (const Matrix &A, │ │ │ │ │ - const Matrix &B, double tol=1e-9) │ │ │ │ │ -  inequals with an tolerance, prints out │ │ │ │ │ - message if within tolerance │ │ │ │ │ -  │ │ │ │ │ - bool  ggttssaamm::::aasssseerrtt__eeqquuaall (const std::list< │ │ │ │ │ - Matrix > &As, const std::list< Matrix > │ │ │ │ │ - &Bs, double tol=1e-9) │ │ │ │ │ -  equals with an tolerance, prints out │ │ │ │ │ - message if unequal │ │ │ │ │ -  │ │ │ │ │ - bool  ggttssaamm::::lliinneeaarr__iinnddeeppeennddeenntt (const Matrix │ │ │ │ │ - &A, const Matrix &B, double tol=1e-9) │ │ │ │ │ -  check whether the rows of two matrices │ │ │ │ │ - are linear independent │ │ │ │ │ -  │ │ │ │ │ - bool  ggttssaamm::::lliinneeaarr__ddeeppeennddeenntt (const Matrix &A, │ │ │ │ │ - const Matrix &B, double tol=1e-9) │ │ │ │ │ -  check whether the rows of two matrices │ │ │ │ │ - are linear dependent │ │ │ │ │ -  │ │ │ │ │ - Vector  ggttssaamm::::ooppeerraattoorr^^ (const Matrix &A, const │ │ │ │ │ - Vector &v) │ │ │ │ │ -  overload ^ for trans(A)*v We transpose │ │ │ │ │ - the vectors for speed. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - MATRIX  ggttssaamm::::pprroodd (const MATRIX &A, const │ │ │ │ │ - MATRIX &B) │ │ │ │ │ -  products using old-style format to │ │ │ │ │ - improve compatibility │ │ │ │ │ -  │ │ │ │ │ - void  ggttssaamm::::pprriinntt (const Matrix &A, const │ │ │ │ │ - std::string &s, std::ostream &stream) │ │ │ │ │ -  print without optional string, must │ │ │ │ │ - specify cout yourself │ │ │ │ │ -  │ │ │ │ │ - void  ggttssaamm::::pprriinntt (const Matrix &A, const │ │ │ │ │ - std::string &s="") │ │ │ │ │ -  print with optional string to cout │ │ │ │ │ -  │ │ │ │ │ - void  ggttssaamm::::ssaavvee (const Matrix &A, const std:: │ │ │ │ │ - string &s, const std::string &filename) │ │ │ │ │ -  save a matrix to file, which can be │ │ │ │ │ - loaded by matlab │ │ │ │ │ -  │ │ │ │ │ - istream &  _g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_>_> (std::istream │ │ │ │ │ - &inputStream, Matrix &destinationMatrix) │ │ │ │ │ -  Read a matrix from an input stream, such │ │ │ │ │ - as a file. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - Eigen::Block< const MATRIX >  _g_t_s_a_m_:_:_s_u_b (const MATRIX &A, size_t i1, │ │ │ │ │ - size_t i2, size_t j1, size_t j2) │ │ │ │ │ -  extract submatrix, slice semantics, i.e. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - void  _g_t_s_a_m_:_:_i_n_s_e_r_t_S_u_b (Eigen::MatrixBase< │ │ │ │ │ - Derived1 > &fullMatrix, const Eigen:: │ │ │ │ │ - MatrixBase< Derived2 > &subMatrix, size_t │ │ │ │ │ - i, size_t j) │ │ │ │ │ - insert a submatrix IN PLACE at a │ │ │ │ │ -  specified location in a larger matrix │ │ │ │ │ - NOTE: there is no size checking │ │ │ │ │ -  │ │ │ │ │ - Matrix  ggttssaamm::::ddiiaagg (const std::vector< Matrix > │ │ │ │ │ - &Hs) │ │ │ │ │ -  Create a matrix with submatrices along │ │ │ │ │ - its diagonal. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - const MATRIX::ConstColXpr  _g_t_s_a_m_:_:_c_o_l_u_m_n (const MATRIX &A, size_t j) │ │ │ │ │ -  Extracts a column view from a matrix that │ │ │ │ │ - avoids a copy. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - const MATRIX::ConstRowXpr  _g_t_s_a_m_:_:_r_o_w (const MATRIX &A, size_t j) │ │ │ │ │ -  Extracts a row view from a matrix that │ │ │ │ │ - avoids a copy. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - void  _g_t_s_a_m_:_:_z_e_r_o_B_e_l_o_w_D_i_a_g_o_n_a_l (MATRIX &A, │ │ │ │ │ - size_t cols=0) │ │ │ │ │ -  Zeros all of the elements below the │ │ │ │ │ - diagonal of a matrix, in place. │ │ │ │ │ -  │ │ │ │ │ - Matrix  ggttssaamm::::ttrraannss (const Matrix &A) │ │ │ │ │ -  static transpose function, just calls │ │ │ │ │ - Eigen transpose member function │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -_R_e_s_h_a_p_e< OutM, OutN, OutOptions, InM, ggttssaamm::::rreesshhaappee (const Eigen::Matrix< │ │ │ │ │ - InN, InOptions >::ReshapedType  double, InM, InN, InOptions > &m) │ │ │ │ │ -  │ │ │ │ │ - pair< Matrix, Matrix >  _g_t_s_a_m_:_:_q_r (const Matrix &A) │ │ │ │ │ - Householder QR factorization, Golub & Van │ │ │ │ │ -  Loan p 224, explicit version │ │ │ │ │ - │ │ │ │ │ -  │ │ │ │ │ - void  _g_t_s_a_m_:_:_i_n_p_l_a_c_e___Q_R (Matrix &A) │ │ │ │ │ -  QR factorization using Eigen's internal │ │ │ │ │ - block QR algorithm. │ │ │ │ │ -  │ │ │ │ │ - list< boost::tuple< Vector, double, _g_t_s_a_m_:_:_w_e_i_g_h_t_e_d___e_l_i_m_i_n_a_t_e (Matrix &A, │ │ │ │ │ - double > >  Vector &b, const Vector &sigmas) │ │ │ │ │ - Imperative algorithm for in-place full │ │ │ │ │ -  elimination with weights and constraint │ │ │ │ │ - handling. │ │ │ │ │ -  │ │ │ │ │ - void  _g_t_s_a_m_:_:_h_o_u_s_e_h_o_l_d_e_r__ (Matrix &A, size_t k, │ │ │ │ │ - bool copy_vectors) │ │ │ │ │ - Imperative version of Householder QR │ │ │ │ │ -  factorization, Golub & Van Loan p 224 │ │ │ │ │ - version with Householder vectors below │ │ │ │ │ - diagonal, as in GVL. │ │ │ │ │ -  │ │ │ │ │ - void  _g_t_s_a_m_:_:_h_o_u_s_e_h_o_l_d_e_r (Matrix &A, size_t k) │ │ │ │ │ -  Householder tranformation, zeros below │ │ │ │ │ - diagonal. │ │ │ │ │ -  │ │ │ │ │ - Vector  _g_t_s_a_m_:_:_b_a_c_k_S_u_b_s_t_i_t_u_t_e_U_p_p_e_r (const Matrix │ │ │ │ │ - &U, const Vector &b, bool unit=false) │ │ │ │ │ -  backSubstitute U*x=b │ │ │ │ │ -  │ │ │ │ │ - Vector  _g_t_s_a_m_:_:_b_a_c_k_S_u_b_s_t_i_t_u_t_e_U_p_p_e_r (const Vector │ │ │ │ │ - &b, const Matrix &U, bool unit=false) │ │ │ │ │ -  backSubstitute x'*U=b' │ │ │ │ │ -  │ │ │ │ │ - Vector  _g_t_s_a_m_:_:_b_a_c_k_S_u_b_s_t_i_t_u_t_e_L_o_w_e_r (const Matrix │ │ │ │ │ - &L, const Vector &b, bool unit=false) │ │ │ │ │ -  backSubstitute L*x=b │ │ │ │ │ -  │ │ │ │ │ - Matrix  _g_t_s_a_m_:_:_s_t_a_c_k (size_t nrMatrices,...) │ │ │ │ │ - create a matrix by stacking other │ │ │ │ │ -  matrices Given a set of matrices: A1, A2, │ │ │ │ │ - A3... │ │ │ │ │ -  │ │ │ │ │ - Matrix  ggttssaamm::::ssttaacckk (const std::vector< Matrix > │ │ │ │ │ - &blocks) │ │ │ │ │ -  │ │ │ │ │ - Matrix  _g_t_s_a_m_:_:_c_o_l_l_e_c_t (const std::vector< const │ │ │ │ │ - Matrix * > &matrices, size_t m=0, size_t │ │ │ │ │ - n=0) │ │ │ │ │ - create a matrix by concatenating Given a │ │ │ │ │ - set of matrices: A1, A2, A3... If all │ │ │ │ │ -  matrices have the same size, specifying │ │ │ │ │ - single matrix dimensions will avoid the │ │ │ │ │ - lookup of dimensions │ │ │ │ │ -  │ │ │ │ │ - Matrix  ggttssaamm::::ccoolllleecctt (size_t nrMatrices,...) │ │ │ │ │ -  │ │ │ │ │ - void  _g_t_s_a_m_:_:_v_e_c_t_o_r___s_c_a_l_e___i_n_p_l_a_c_e (const Vector │ │ │ │ │ - &v, Matrix &A, bool inf_mask=false) │ │ │ │ │ - scales a matrix row or column by the │ │ │ │ │ -  values in a vector Arguments (Matrix, │ │ │ │ │ - Vector) scales the columns, (Vector, │ │ │ │ │ - Matrix) scales the rows │ │ │ │ │ -  │ │ │ │ │ - Matrix  ggttssaamm::::vveeccttoorr__ssccaallee (const Vector &v, │ │ │ │ │ - const Matrix &A, bool inf_mask) │ │ │ │ │ -  │ │ │ │ │ - Matrix  ggttssaamm::::vveeccttoorr__ssccaallee (const Matrix &A, │ │ │ │ │ - const Vector &v, bool inf_mask) │ │ │ │ │ -  │ │ │ │ │ - Matrix3  _g_t_s_a_m_:_:_s_k_e_w_S_y_m_m_e_t_r_i_c (double wx, double │ │ │ │ │ - wy, double wz) │ │ │ │ │ -  skew symmetric matrix returns this: 0 -wz │ │ │ │ │ - wy wz 0 -wx -wy wx 0 │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - Matrix3  ggttssaamm::::sskkeewwSSyymmmmeettrriicc (const Eigen:: │ │ │ │ │ - MatrixBase< Derived > &w) │ │ │ │ │ -  │ │ │ │ │ - Matrix  ggttssaamm::::iinnvveerrssee__ssqquuaarree__rroooott (const Matrix │ │ │ │ │ - &A) │ │ │ │ │ -  Use Cholesky to calculate inverse square │ │ │ │ │ - root of a matrix. │ │ │ │ │ -  │ │ │ │ │ - Matrix  _g_t_s_a_m_:_:_c_h_o_l_e_s_k_y___i_n_v_e_r_s_e (const Matrix &A) │ │ │ │ │ -  Return the inverse of a S.P.D. │ │ │ │ │ -  │ │ │ │ │ - void  _g_t_s_a_m_:_:_s_v_d (const Matrix &A, Matrix &U, │ │ │ │ │ - Vector &S, Matrix &V) │ │ │ │ │ -  SVD computes economy SVD A=U*S*V'. │ │ │ │ │ -  │ │ │ │ │ - boost::tuple< int, double, Vector >  _g_t_s_a_m_:_:_D_L_T (const Matrix &A, double │ │ │ │ │ - rank_tol=1e-9) │ │ │ │ │ - Direct linear transform algorithm that │ │ │ │ │ -  calls svd to find a vector v that │ │ │ │ │ - minimizes the algebraic error A*v. │ │ │ │ │ -  │ │ │ │ │ - Matrix  _g_t_s_a_m_:_:_e_x_p_m (const Matrix &A, size_t K=7) │ │ │ │ │ -  Numerical exponential map, naive │ │ │ │ │ - approach, not industrial strength !!! │ │ │ │ │ -  │ │ │ │ │ - std::string  ggttssaamm::::ffoorrmmaattMMaattrriixxIInnddeenntteedd (const std:: │ │ │ │ │ - string &label, const Matrix &matrix, bool │ │ │ │ │ - makeVectorHorizontal) │ │ │ │ │ -  │ │ │ │ │ - Matrix  ggttssaamm::::LLLLtt (const Matrix &A) │ │ │ │ │ -  │ │ │ │ │ - Matrix  ggttssaamm::::RRttRR (const Matrix &A) │ │ │ │ │ -  │ │ │ │ │ - Vector  ggttssaamm::::ccoolluummnnNNoorrmmSSqquuaarree (const Matrix &A) │ │ │ │ │ +bool  ggttssaamm::::aasssseerrtt__eeqquuaall (const _K_e_y &expected, const _K_e_y &actual, double │ │ │ │ │ + tol=0.0) │ │ │ │ │ +  Equals testing for basic types. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +bool  _g_t_s_a_m_:_:_a_s_s_e_r_t___e_q_u_a_l (const boost::optional< V > &expected, const boost:: │ │ │ │ │ + optional< V > &actual, double tol=1e-9) │ │ │ │ │ +  Comparisons for boost.optional objects that checks whether objects exist │ │ │ │ │ + before comparing their values. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +bool  ggttssaamm::::aasssseerrtt__eeqquuaall (const V &expected, const boost::optional< V > │ │ │ │ │ + &actual, double tol=1e-9) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +bool  ggttssaamm::::aasssseerrtt__eeqquuaall (const V &expected, const boost::optional< const V & │ │ │ │ │ + > &actual, double tol=1e-9) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +bool  ggttssaamm::::aasssseerrtt__ccoonnttaaiinneerr__eeqquuaall (const std::map< V1, V2 > &expected, const │ │ │ │ │ + std::map< V1, V2 > &actual, double tol=1e-9) │ │ │ │ │ +  Function for comparing maps of testable->testable TODO: replace with more │ │ │ │ │ + generalized version. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +bool  ggttssaamm::::aasssseerrtt__ccoonnttaaiinneerr__eeqquuaall (const std::map< size_t, V2 > &expected, │ │ │ │ │ + const std::map< size_t, V2 > &actual, double tol=1e-9) │ │ │ │ │ +  Function for comparing maps of size_t->testable. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +bool  ggttssaamm::::aasssseerrtt__ccoonnttaaiinneerr__eeqquuaall (const std::vector< std::pair< V1, V2 > > │ │ │ │ │ + &expected, const std::vector< std::pair< V1, V2 > > &actual, double │ │ │ │ │ + tol=1e-9) │ │ │ │ │ +  Function for comparing vector of pairs (testable, testable) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +bool  ggttssaamm::::aasssseerrtt__ccoonnttaaiinneerr__eeqquuaall (const V &expected, const V &actual, double │ │ │ │ │ + tol=1e-9) │ │ │ │ │ +  General function for comparing containers of testable objects. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +bool  ggttssaamm::::aasssseerrtt__ccoonnttaaiinneerr__eeqquuaalliittyy (const std::map< size_t, V2 > &expected, │ │ │ │ │ + const std::map< size_t, V2 > &actual) │ │ │ │ │ +  Function for comparing maps of size_t->testable Types are assumed to have │ │ │ │ │ + operator ==. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +bool  ggttssaamm::::aasssseerrtt__ccoonnttaaiinneerr__eeqquuaalliittyy (const V &expected, const V &actual) │ │ │ │ │ +  General function for comparing containers of objects with operator==. │ │ │ │ │ +  │ │ │ │ │ +bool  ggttssaamm::::aasssseerrtt__eeqquuaall (const std::string &expected, const std::string │ │ │ │ │ + &actual) │ │ │ │ │ +  Compare strings for unit tests. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +bool  ggttssaamm::::aasssseerrtt__iinneeqquuaall (const V &expected, const V &actual, double tol=1e- │ │ │ │ │ + 9) │ │ │ │ │ +  Allow for testing inequality. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +bool  ggttssaamm::::aasssseerrtt__ssttddoouutt__eeqquuaall (const std::string &expected, const V &actual) │ │ │ │ │ +  Capture std out via cout stream and compare against string. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +bool  _g_t_s_a_m_:_:_a_s_s_e_r_t___p_r_i_n_t___e_q_u_a_l (const std::string &expected, const V &actual, │ │ │ │ │ + const std::string &s="") │ │ │ │ │ +  Capture print function output and compare against string. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +Provides additional testing facilities for common data structures. │ │ │ │ │ Author │ │ │ │ │ - Christian Potthast │ │ │ │ │ - Kai Ni │ │ │ │ │ - Frank Dellaert │ │ │ │ │ Alex Cunningham │ │ │ │ │ - Alex Hagiopol │ │ │ │ │ - Varun Agrawal │ │ │ │ │ -********** MMaaccrroo DDeeffiinniittiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ -********** _?◆_? GGTTSSAAMM__MMAAKKEE__MMAATTRRIIXX__DDEEFFSS ********** │ │ │ │ │ -#define GTSAM_MAKE_MATRIX_DEFS (   N ) │ │ │ │ │ -VVaalluuee:: │ │ │ │ │ -using Matrix##N = Eigen::Matrix; \ │ │ │ │ │ -using Matrix1##N = Eigen::Matrix; \ │ │ │ │ │ -using Matrix2##N = Eigen::Matrix; \ │ │ │ │ │ -using Matrix3##N = Eigen::Matrix; \ │ │ │ │ │ -using Matrix4##N = Eigen::Matrix; \ │ │ │ │ │ -using Matrix5##N = Eigen::Matrix; \ │ │ │ │ │ -using Matrix6##N = Eigen::Matrix; \ │ │ │ │ │ -using Matrix7##N = Eigen::Matrix; \ │ │ │ │ │ -using Matrix8##N = Eigen::Matrix; \ │ │ │ │ │ -using Matrix9##N = Eigen::Matrix; \ │ │ │ │ │ -static const Eigen::MatrixBase::IdentityReturnType I_##N##x##N = │ │ │ │ │ -Matrix##N::Identity(); \ │ │ │ │ │ -static const Eigen::MatrixBase::ConstantReturnType Z_##N##x##N = │ │ │ │ │ -Matrix##N::Zero(); │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _M_a_t_r_i_x_._h │ │ │ │ │ + * _T_e_s_t_a_b_l_e_A_s_s_e_r_t_i_o_n_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00146.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,46 +1,14 @@ │ │ │ │ │ var a00146 = [ │ │ │ │ │ - ["gtsam::Reshape< OutM, OutN, OutOptions, InM, InN, InOptions >", "a02416.html", null], │ │ │ │ │ - ["gtsam::Reshape< M, M, InOptions, M, M, InOptions >", "a02420.html", null], │ │ │ │ │ - ["gtsam::Reshape< M, N, InOptions, M, N, InOptions >", "a02424.html", null], │ │ │ │ │ - ["gtsam::Reshape< N, M, InOptions, M, N, InOptions >", "a02428.html", null], │ │ │ │ │ - ["gtsam::MultiplyWithInverse< N >", "a02432.html", "a02432"], │ │ │ │ │ - ["gtsam::MultiplyWithInverseFunction< T, N >", "a02436.html", "a02436"], │ │ │ │ │ - ["assert_equal", "a00146.html#a3f9622226dfe06908f11b42bf0bdd22d", null], │ │ │ │ │ - ["assert_equal", "a00146.html#abb0e19bbbeaca95843e8161b89a12fda", null], │ │ │ │ │ - ["assert_inequal", "a00146.html#a7a116d0643f123ef3b15d91056506492", null], │ │ │ │ │ - ["backSubstituteLower", "a00146.html#a0d998e1b770c9864946ddb031b1c4522", null], │ │ │ │ │ - ["backSubstituteUpper", "a00146.html#a6c4876cbe85d5651a52eda0e97c60f2f", null], │ │ │ │ │ - ["backSubstituteUpper", "a00146.html#a8b0bf332d52b333dab2b20d763c8925b", null], │ │ │ │ │ - ["cholesky_inverse", "a00146.html#aba542e2cd85f08b76f80a0871a4ea713", null], │ │ │ │ │ - ["collect", "a00146.html#ad8b524ba6c9aed0d21a020999c9b5d88", null], │ │ │ │ │ - ["column", "a00146.html#a559dba69e2854eb66e34222f60f55722", null], │ │ │ │ │ - ["diag", "a00146.html#af1c32907adce74cf9edd6ee5bba5a085", null], │ │ │ │ │ - ["DLT", "a00146.html#ab2c65f1a69009a306b6a5f9ef31dcee2", null], │ │ │ │ │ - ["equal_with_abs_tol", "a00146.html#af31e657258505b2e5148846ebbaa3195", null], │ │ │ │ │ - ["expm", "a00146.html#ab15462d8c16813d0a7a5b1f76a2f64b7", null], │ │ │ │ │ - ["householder", "a00146.html#a7baa2f3184a444adce108633c0265e0c", null], │ │ │ │ │ - ["householder_", "a00146.html#abdd1ab05e8ac90b340fbd8f3b322dc6d", null], │ │ │ │ │ - ["inplace_QR", "a00146.html#a7eec9339ab5a008a28ddfaa6b2c94611", null], │ │ │ │ │ - ["insertSub", "a00146.html#ad1088fa2d1494e6a4123a6bc3f5c2d7e", null], │ │ │ │ │ - ["inverse_square_root", "a00146.html#aff78dee59ac0250432081f39deb5f6d1", null], │ │ │ │ │ - ["linear_dependent", "a00146.html#a4b81794af72954abafbb726fc712f5db", null], │ │ │ │ │ - ["linear_independent", "a00146.html#ad8f0349471464c1fb515819d9503849a", null], │ │ │ │ │ - ["operator!=", "a00146.html#ab235abf7505b634be2165e0db58239dd", null], │ │ │ │ │ - ["operator==", "a00146.html#afd5b3cf3f54adcbdd6d9e7403f1a792f", null], │ │ │ │ │ - ["operator>>", "a00146.html#a7d85957bab2d18cf56ab9aaf95a106de", null], │ │ │ │ │ - ["operator^", "a00146.html#a2218e53a2b99c449e70aa5b7805895fc", null], │ │ │ │ │ - ["print", "a00146.html#a54fa43c89c5334314c8c75939dd5c2d7", null], │ │ │ │ │ - ["print", "a00146.html#a44ead03912d5dcf094d8421e1702ee71", null], │ │ │ │ │ - ["prod", "a00146.html#abc29e3164ed30e785a3c48dfd1aa6ca5", null], │ │ │ │ │ - ["qr", "a00146.html#ae32c295e2c40c1e85f146a8a6266eaa8", null], │ │ │ │ │ - ["row", "a00146.html#a2754f325c8600303d627d9e8cf1f9949", null], │ │ │ │ │ - ["save", "a00146.html#a3eb7eed9019d3fda8fcf74fbf85b85c9", null], │ │ │ │ │ - ["skewSymmetric", "a00146.html#a97f108d19e52c83c331c55d35b23796e", null], │ │ │ │ │ - ["stack", "a00146.html#a5c82884a356ddd09229a5283aed04df9", null], │ │ │ │ │ - ["sub", "a00146.html#a9ae1d9fe2ccad044fbb88b5c1d9e035a", null], │ │ │ │ │ - ["svd", "a00146.html#a8d7e46204d953f64a39445599dbd7eee", null], │ │ │ │ │ - ["trans", "a00146.html#aa2e36d7ab63000feddaeb61bbfcf2db1", null], │ │ │ │ │ - ["vector_scale_inplace", "a00146.html#a24d3ad0252f91f0ec301593c45cf5af7", null], │ │ │ │ │ - ["weighted_eliminate", "a00146.html#a60019a03f26b92c6b2a08e43d153d4c0", null], │ │ │ │ │ - ["zeroBelowDiagonal", "a00146.html#a57edf6ed7312f63d35f73233665c334d", null] │ │ │ │ │ + ["assert_container_equal", "a00146.html#a2f509195ea2180d0f7dbd3a99a088ff4", null], │ │ │ │ │ + ["assert_container_equal", "a00146.html#a254be27d6d4b416fa2b546c77ae783fc", null], │ │ │ │ │ + ["assert_container_equal", "a00146.html#a0eec17f894b358dd9f30d7af28082ba5", null], │ │ │ │ │ + ["assert_container_equal", "a00146.html#a6d6b2964354593f69848f6bf5d3e0ca1", null], │ │ │ │ │ + ["assert_container_equality", "a00146.html#a59ba6a7db27344c5cb021f1421905020", null], │ │ │ │ │ + ["assert_container_equality", "a00146.html#aa0926a5d779171bd8e1d30fb5982b5c0", null], │ │ │ │ │ + ["assert_equal", "a00146.html#add6d6575582314cc9b5dbdbe2a86374d", null], │ │ │ │ │ + ["assert_equal", "a00146.html#a982034802415eb6e9bd02355257ed96a", null], │ │ │ │ │ + ["assert_equal", "a00146.html#a21e760bc75888053afd86a27d56b6148", null], │ │ │ │ │ + ["assert_inequal", "a00146.html#a2f19ba6625a264457805513fefcb5c32", null], │ │ │ │ │ + ["assert_print_equal", "a00146.html#a830c8c65902d7d0e763562e6c9357346", null], │ │ │ │ │ + ["assert_stdout_equal", "a00146.html#a08641f0f7145716bba9159dd95099a44", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00146_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Matrix.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/TestableAssertions.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,397 +98,399 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Matrix.h
│ │ │ │ +
TestableAssertions.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
23// \callgraph
│ │ │ │ -
24
│ │ │ │ -
25#pragma once
│ │ │ │ -
26
│ │ │ │ -
27#include <gtsam/base/OptionalJacobian.h>
│ │ │ │ -
28#include <gtsam/base/Vector.h>
│ │ │ │ -
29#include <boost/tuple/tuple.hpp>
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/base/Testable.h>
│ │ │ │ +
21#include <gtsam/global_includes.h>
│ │ │ │ +
22
│ │ │ │ +
23#include <boost/optional.hpp>
│ │ │ │ +
24#include <map>
│ │ │ │ +
25#include <iostream>
│ │ │ │ +
26#include <sstream>
│ │ │ │ +
27#include <vector>
│ │ │ │ +
28
│ │ │ │ +
29namespace gtsam {
│ │ │ │
30
│ │ │ │ -
31#include <vector>
│ │ │ │ -
32
│ │ │ │ -
38namespace gtsam {
│ │ │ │ -
39
│ │ │ │ -
40typedef Eigen::MatrixXd Matrix;
│ │ │ │ -
41typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRowMajor;
│ │ │ │ -
42
│ │ │ │ -
43// Create handy typedefs and constants for square-size matrices
│ │ │ │ -
44// MatrixMN, MatrixN = MatrixNN, I_NxN, and Z_NxN, for M,N=1..9
│ │ │ │ -
45#define GTSAM_MAKE_MATRIX_DEFS(N) \
│ │ │ │ -
46using Matrix##N = Eigen::Matrix<double, N, N>; \
│ │ │ │ -
47using Matrix1##N = Eigen::Matrix<double, 1, N>; \
│ │ │ │ -
48using Matrix2##N = Eigen::Matrix<double, 2, N>; \
│ │ │ │ -
49using Matrix3##N = Eigen::Matrix<double, 3, N>; \
│ │ │ │ -
50using Matrix4##N = Eigen::Matrix<double, 4, N>; \
│ │ │ │ -
51using Matrix5##N = Eigen::Matrix<double, 5, N>; \
│ │ │ │ -
52using Matrix6##N = Eigen::Matrix<double, 6, N>; \
│ │ │ │ -
53using Matrix7##N = Eigen::Matrix<double, 7, N>; \
│ │ │ │ -
54using Matrix8##N = Eigen::Matrix<double, 8, N>; \
│ │ │ │ -
55using Matrix9##N = Eigen::Matrix<double, 9, N>; \
│ │ │ │ -
56static const Eigen::MatrixBase<Matrix##N>::IdentityReturnType I_##N##x##N = Matrix##N::Identity(); \
│ │ │ │ -
57static const Eigen::MatrixBase<Matrix##N>::ConstantReturnType Z_##N##x##N = Matrix##N::Zero();
│ │ │ │ -
58
│ │ │ │ -
59GTSAM_MAKE_MATRIX_DEFS(1)
│ │ │ │ -
60GTSAM_MAKE_MATRIX_DEFS(2)
│ │ │ │ -
61GTSAM_MAKE_MATRIX_DEFS(3)
│ │ │ │ -
62GTSAM_MAKE_MATRIX_DEFS(4)
│ │ │ │ -
63GTSAM_MAKE_MATRIX_DEFS(5)
│ │ │ │ -
64GTSAM_MAKE_MATRIX_DEFS(6)
│ │ │ │ -
65GTSAM_MAKE_MATRIX_DEFS(7)
│ │ │ │ -
66GTSAM_MAKE_MATRIX_DEFS(8)
│ │ │ │ -
67GTSAM_MAKE_MATRIX_DEFS(9)
│ │ │ │ -
68
│ │ │ │ -
69// Matrix expressions for accessing parts of matrices
│ │ │ │ -
70typedef Eigen::Block<Matrix> SubMatrix;
│ │ │ │ -
71typedef Eigen::Block<const Matrix> ConstSubMatrix;
│ │ │ │ -
72
│ │ │ │ -
73// Matrix formatting arguments when printing.
│ │ │ │ -
74// Akin to Matlab style.
│ │ │ │ -
75const Eigen::IOFormat& matlabFormat();
│ │ │ │ -
76
│ │ │ │ -
80template <class MATRIX>
│ │ │ │ -
│ │ │ │ -
81bool equal_with_abs_tol(const Eigen::DenseBase<MATRIX>& A, const Eigen::DenseBase<MATRIX>& B, double tol = 1e-9) {
│ │ │ │ +
│ │ │ │ +
34inline bool assert_equal(const Key& expected, const Key& actual, double tol = 0.0) {
│ │ │ │ +
35 if(expected != actual) {
│ │ │ │ +
36 std::cout << "Not equal:\nexpected: " << expected << "\nactual: " << actual << std::endl;
│ │ │ │ +
37 return false;
│ │ │ │ +
38 }
│ │ │ │ +
39 return true;
│ │ │ │ +
40}
│ │ │ │ +
│ │ │ │ +
41
│ │ │ │ +
49template<class V>
│ │ │ │ +
│ │ │ │ +
50bool assert_equal(const boost::optional<V>& expected,
│ │ │ │ +
51 const boost::optional<V>& actual, double tol = 1e-9) {
│ │ │ │ +
52 if (!expected && actual) {
│ │ │ │ +
53 std::cout << "expected is boost::none, while actual is not" << std::endl;
│ │ │ │ +
54 return false;
│ │ │ │ +
55 }
│ │ │ │ +
56 if (expected && !actual) {
│ │ │ │ +
57 std::cout << "actual is boost::none, while expected is not" << std::endl;
│ │ │ │ +
58 return false;
│ │ │ │ +
59 }
│ │ │ │ +
60 if (!expected && !actual)
│ │ │ │ +
61 return true;
│ │ │ │ +
62 return assert_equal(*expected, *actual, tol);
│ │ │ │ +
63}
│ │ │ │ +
│ │ │ │ +
64
│ │ │ │ +
65template<class V>
│ │ │ │ +
66bool assert_equal(const V& expected, const boost::optional<V>& actual, double tol = 1e-9) {
│ │ │ │ +
67 if (!actual) {
│ │ │ │ +
68 std::cout << "actual is boost::none" << std::endl;
│ │ │ │ +
69 return false;
│ │ │ │ +
70 }
│ │ │ │ +
71 return assert_equal(expected, *actual, tol);
│ │ │ │ +
72}
│ │ │ │ +
73
│ │ │ │ +
74template<class V>
│ │ │ │ +
75bool assert_equal(const V& expected, const boost::optional<const V&>& actual, double tol = 1e-9) {
│ │ │ │ +
76 if (!actual) {
│ │ │ │ +
77 std::cout << "actual is boost::none" << std::endl;
│ │ │ │ +
78 return false;
│ │ │ │ +
79 }
│ │ │ │ +
80 return assert_equal(expected, *actual, tol);
│ │ │ │ +
81}
│ │ │ │
82
│ │ │ │ -
83 const size_t n1 = A.cols(), m1 = A.rows();
│ │ │ │ -
84 const size_t n2 = B.cols(), m2 = B.rows();
│ │ │ │ -
85
│ │ │ │ -
86 if(m1!=m2 || n1!=n2) return false;
│ │ │ │ -
87
│ │ │ │ -
88 for(size_t i=0; i<m1; i++)
│ │ │ │ -
89 for(size_t j=0; j<n1; j++) {
│ │ │ │ -
90 if(!fpEqual(A(i,j), B(i,j), tol, false)) {
│ │ │ │ -
91 return false;
│ │ │ │ -
92 }
│ │ │ │ -
93 }
│ │ │ │ -
94 return true;
│ │ │ │ -
95}
│ │ │ │ -
│ │ │ │ -
96
│ │ │ │ -
│ │ │ │ -
100inline bool operator==(const Matrix& A, const Matrix& B) {
│ │ │ │ -
101 return equal_with_abs_tol(A,B,1e-9);
│ │ │ │ -
102}
│ │ │ │ -
│ │ │ │ -
103
│ │ │ │ -
│ │ │ │ -
107inline bool operator!=(const Matrix& A, const Matrix& B) {
│ │ │ │ -
108 return !(A==B);
│ │ │ │ -
109 }
│ │ │ │ -
│ │ │ │ -
110
│ │ │ │ -
114GTSAM_EXPORT bool assert_equal(const Matrix& A, const Matrix& B, double tol = 1e-9);
│ │ │ │ -
115
│ │ │ │ -
119GTSAM_EXPORT bool assert_inequal(const Matrix& A, const Matrix& B, double tol = 1e-9);
│ │ │ │ -
120
│ │ │ │ -
124GTSAM_EXPORT bool assert_equal(const std::list<Matrix>& As, const std::list<Matrix>& Bs, double tol = 1e-9);
│ │ │ │ -
125
│ │ │ │ -
129GTSAM_EXPORT bool linear_independent(const Matrix& A, const Matrix& B, double tol = 1e-9);
│ │ │ │ -
130
│ │ │ │ -
134GTSAM_EXPORT bool linear_dependent(const Matrix& A, const Matrix& B, double tol = 1e-9);
│ │ │ │ -
135
│ │ │ │ -
140GTSAM_EXPORT Vector operator^(const Matrix& A, const Vector & v);
│ │ │ │ -
141
│ │ │ │ -
143template<class MATRIX>
│ │ │ │ -
│ │ │ │ -
144inline MATRIX prod(const MATRIX& A, const MATRIX&B) {
│ │ │ │ -
145 MATRIX result = A * B;
│ │ │ │ -
146 return result;
│ │ │ │ -
147}
│ │ │ │ -
│ │ │ │ -
148
│ │ │ │ -
152GTSAM_EXPORT void print(const Matrix& A, const std::string& s, std::ostream& stream);
│ │ │ │ -
153
│ │ │ │ -
157GTSAM_EXPORT void print(const Matrix& A, const std::string& s = "");
│ │ │ │ -
158
│ │ │ │ -
162GTSAM_EXPORT void save(const Matrix& A, const std::string &s, const std::string& filename);
│ │ │ │ -
163
│ │ │ │ -
169GTSAM_EXPORT std::istream& operator>>(std::istream& inputStream, Matrix& destinationMatrix);
│ │ │ │ -
170
│ │ │ │ -
180template<class MATRIX>
│ │ │ │ -
│ │ │ │ -
181Eigen::Block<const MATRIX> sub(const MATRIX& A, size_t i1, size_t i2, size_t j1, size_t j2) {
│ │ │ │ -
182 size_t m=i2-i1, n=j2-j1;
│ │ │ │ -
183 return A.block(i1,j1,m,n);
│ │ │ │ -
184}
│ │ │ │ +
83#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ +
88template<class V>
│ │ │ │ +
89bool GTSAM_DEPRECATED assert_equal(const std::vector<V>& expected, const std::vector<V>& actual, double tol = 1e-9) {
│ │ │ │ +
90 bool match = true;
│ │ │ │ +
91 if (expected.size() != actual.size())
│ │ │ │ +
92 match = false;
│ │ │ │ +
93 if(match) {
│ │ │ │ +
94 size_t i = 0;
│ │ │ │ +
95 for(const V& a: expected) {
│ │ │ │ +
96 if (!assert_equal(a, actual[i++], tol)) {
│ │ │ │ +
97 match = false;
│ │ │ │ +
98 break;
│ │ │ │ +
99 }
│ │ │ │ +
100 }
│ │ │ │ +
101 }
│ │ │ │ +
102 if(!match) {
│ │ │ │ +
103 std::cout << "expected: " << std::endl;
│ │ │ │ +
104 for(const V& a: expected) { std::cout << a << " "; }
│ │ │ │ +
105 std::cout << "\nactual: " << std::endl;
│ │ │ │ +
106 for(const V& a: actual) { std::cout << a << " "; }
│ │ │ │ +
107 std::cout << std::endl;
│ │ │ │ +
108 return false;
│ │ │ │ +
109 }
│ │ │ │ +
110 return true;
│ │ │ │ +
111}
│ │ │ │ +
112#endif
│ │ │ │ +
113
│ │ │ │ +
118template<class V1, class V2>
│ │ │ │ +
│ │ │ │ +
119bool assert_container_equal(const std::map<V1,V2>& expected, const std::map<V1,V2>& actual, double tol = 1e-9) {
│ │ │ │ +
120 typedef typename std::map<V1,V2> Map;
│ │ │ │ +
121 bool match = true;
│ │ │ │ +
122 if (expected.size() != actual.size())
│ │ │ │ +
123 match = false;
│ │ │ │ +
124 typename Map::const_iterator
│ │ │ │ +
125 itExp = expected.begin(),
│ │ │ │ +
126 itAct = actual.begin();
│ │ │ │ +
127 if(match) {
│ │ │ │ +
128 for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) {
│ │ │ │ +
129 if (!assert_equal(itExp->first, itAct->first, tol) ||
│ │ │ │ +
130 !assert_equal(itExp->second, itAct->second, tol)) {
│ │ │ │ +
131 match = false;
│ │ │ │ +
132 break;
│ │ │ │ +
133 }
│ │ │ │ +
134 }
│ │ │ │ +
135 }
│ │ │ │ +
136 if(!match) {
│ │ │ │ +
137 std::cout << "expected: " << std::endl;
│ │ │ │ +
138 for(const typename Map::value_type& a: expected) {
│ │ │ │ +
139 a.first.print("key");
│ │ │ │ +
140 a.second.print(" value");
│ │ │ │ +
141 }
│ │ │ │ +
142 std::cout << "\nactual: " << std::endl;
│ │ │ │ +
143 for(const typename Map::value_type& a: actual) {
│ │ │ │ +
144 a.first.print("key");
│ │ │ │ +
145 a.second.print(" value");
│ │ │ │ +
146 }
│ │ │ │ +
147 std::cout << std::endl;
│ │ │ │ +
148 return false;
│ │ │ │ +
149 }
│ │ │ │ +
150 return true;
│ │ │ │ +
151}
│ │ │ │ +
│ │ │ │ +
152
│ │ │ │ +
156template<class V2>
│ │ │ │ +
│ │ │ │ +
157bool assert_container_equal(const std::map<size_t,V2>& expected, const std::map<size_t,V2>& actual, double tol = 1e-9) {
│ │ │ │ +
158 typedef typename std::map<size_t,V2> Map;
│ │ │ │ +
159 bool match = true;
│ │ │ │ +
160 if (expected.size() != actual.size())
│ │ │ │ +
161 match = false;
│ │ │ │ +
162 typename Map::const_iterator
│ │ │ │ +
163 itExp = expected.begin(),
│ │ │ │ +
164 itAct = actual.begin();
│ │ │ │ +
165 if(match) {
│ │ │ │ +
166 for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) {
│ │ │ │ +
167 if (itExp->first != itAct->first ||
│ │ │ │ +
168 !assert_equal(itExp->second, itAct->second, tol)) {
│ │ │ │ +
169 match = false;
│ │ │ │ +
170 break;
│ │ │ │ +
171 }
│ │ │ │ +
172 }
│ │ │ │ +
173 }
│ │ │ │ +
174 if(!match) {
│ │ │ │ +
175 std::cout << "expected: " << std::endl;
│ │ │ │ +
176 for(const typename Map::value_type& a: expected) {
│ │ │ │ +
177 std::cout << "Key: " << a.first << std::endl;
│ │ │ │ +
178 a.second.print(" value");
│ │ │ │ +
179 }
│ │ │ │ +
180 std::cout << "\nactual: " << std::endl;
│ │ │ │ +
181 for(const typename Map::value_type& a: actual) {
│ │ │ │ +
182 std::cout << "Key: " << a.first << std::endl;
│ │ │ │ +
183 a.second.print(" value");
│ │ │ │ +
184 }
│ │ │ │ +
185 std::cout << std::endl;
│ │ │ │ +
186 return false;
│ │ │ │ +
187 }
│ │ │ │ +
188 return true;
│ │ │ │ +
189}
│ │ │ │
│ │ │ │ -
185
│ │ │ │ -
194template <typename Derived1, typename Derived2>
│ │ │ │ +
190
│ │ │ │ +
194template<class V1, class V2>
│ │ │ │
│ │ │ │ -
195void insertSub(Eigen::MatrixBase<Derived1>& fullMatrix, const Eigen::MatrixBase<Derived2>& subMatrix, size_t i, size_t j) {
│ │ │ │ -
196 fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix;
│ │ │ │ -
197}
│ │ │ │ -
│ │ │ │ -
198
│ │ │ │ -
202GTSAM_EXPORT Matrix diag(const std::vector<Matrix>& Hs);
│ │ │ │ -
203
│ │ │ │ -
210template<class MATRIX>
│ │ │ │ -
│ │ │ │ -
211const typename MATRIX::ConstColXpr column(const MATRIX& A, size_t j) {
│ │ │ │ -
212 return A.col(j);
│ │ │ │ -
213}
│ │ │ │ -
│ │ │ │ -
214
│ │ │ │ -
221template<class MATRIX>
│ │ │ │ -
│ │ │ │ -
222const typename MATRIX::ConstRowXpr row(const MATRIX& A, size_t j) {
│ │ │ │ -
223 return A.row(j);
│ │ │ │ -
224}
│ │ │ │ -
│ │ │ │ -
225
│ │ │ │ -
231template<class MATRIX>
│ │ │ │ -
│ │ │ │ -
232void zeroBelowDiagonal(MATRIX& A, size_t cols=0) {
│ │ │ │ -
233 const size_t m = A.rows(), n = A.cols();
│ │ │ │ -
234 const size_t k = (cols) ? std::min(cols, std::min(m,n)) : std::min(m,n);
│ │ │ │ -
235 for (size_t j=0; j<k; ++j)
│ │ │ │ -
236 A.col(j).segment(j+1, m-(j+1)).setZero();
│ │ │ │ -
237}
│ │ │ │ -
│ │ │ │ -
238
│ │ │ │ -
242inline Matrix trans(const Matrix& A) { return A.transpose(); }
│ │ │ │ -
243
│ │ │ │ -
245template <int OutM, int OutN, int OutOptions, int InM, int InN, int InOptions>
│ │ │ │ -
│ │ │ │ -
246struct Reshape {
│ │ │ │ -
247 //TODO replace this with Eigen's reshape function as soon as available. (There is a PR already pending : https://bitbucket.org/eigen/eigen/pull-request/41/reshape/diff)
│ │ │ │ -
248 typedef Eigen::Map<const Eigen::Matrix<double, OutM, OutN, OutOptions> > ReshapedType;
│ │ │ │ -
249 static inline ReshapedType reshape(const Eigen::Matrix<double, InM, InN, InOptions> & in) {
│ │ │ │ -
250 return in.data();
│ │ │ │ -
251 }
│ │ │ │ -
252};
│ │ │ │ -
│ │ │ │ -
253
│ │ │ │ -
255template <int M, int InOptions>
│ │ │ │ -
│ │ │ │ -
256struct Reshape<M, M, InOptions, M, M, InOptions> {
│ │ │ │ -
257 typedef const Eigen::Matrix<double, M, M, InOptions> & ReshapedType;
│ │ │ │ -
258 static inline ReshapedType reshape(const Eigen::Matrix<double, M, M, InOptions> & in) {
│ │ │ │ -
259 return in;
│ │ │ │ -
260 }
│ │ │ │ -
261};
│ │ │ │ -
│ │ │ │ -
262
│ │ │ │ -
264template <int M, int N, int InOptions>
│ │ │ │ -
│ │ │ │ -
265struct Reshape<M, N, InOptions, M, N, InOptions> {
│ │ │ │ -
266 typedef const Eigen::Matrix<double, M, N, InOptions> & ReshapedType;
│ │ │ │ -
267 static inline ReshapedType reshape(const Eigen::Matrix<double, M, N, InOptions> & in) {
│ │ │ │ -
268 return in;
│ │ │ │ -
269 }
│ │ │ │ -
270};
│ │ │ │ -
│ │ │ │ -
271
│ │ │ │ -
273template <int M, int N, int InOptions>
│ │ │ │ -
│ │ │ │ -
274struct Reshape<N, M, InOptions, M, N, InOptions> {
│ │ │ │ -
275 typedef typename Eigen::Matrix<double, M, N, InOptions>::ConstTransposeReturnType ReshapedType;
│ │ │ │ -
276 static inline ReshapedType reshape(const Eigen::Matrix<double, M, N, InOptions> & in) {
│ │ │ │ -
277 return in.transpose();
│ │ │ │ -
278 }
│ │ │ │ -
279};
│ │ │ │ -
│ │ │ │ -
280
│ │ │ │ -
281template <int OutM, int OutN, int OutOptions, int InM, int InN, int InOptions>
│ │ │ │ -
282inline typename Reshape<OutM, OutN, OutOptions, InM, InN, InOptions>::ReshapedType reshape(const Eigen::Matrix<double, InM, InN, InOptions> & m){
│ │ │ │ -
283 BOOST_STATIC_ASSERT(InM * InN == OutM * OutN);
│ │ │ │ -
284 return Reshape<OutM, OutN, OutOptions, InM, InN, InOptions>::reshape(m);
│ │ │ │ -
285}
│ │ │ │ -
286
│ │ │ │ -
293GTSAM_EXPORT std::pair<Matrix,Matrix> qr(const Matrix& A);
│ │ │ │ -
294
│ │ │ │ -
300GTSAM_EXPORT void inplace_QR(Matrix& A);
│ │ │ │ -
301
│ │ │ │ -
310GTSAM_EXPORT std::list<boost::tuple<Vector, double, double> >
│ │ │ │ -
311weighted_eliminate(Matrix& A, Vector& b, const Vector& sigmas);
│ │ │ │ -
312
│ │ │ │ -
320GTSAM_EXPORT void householder_(Matrix& A, size_t k, bool copy_vectors=true);
│ │ │ │ -
321
│ │ │ │ -
328GTSAM_EXPORT void householder(Matrix& A, size_t k);
│ │ │ │ +
195bool assert_container_equal(const std::vector<std::pair<V1,V2> >& expected,
│ │ │ │ +
196 const std::vector<std::pair<V1,V2> >& actual, double tol = 1e-9) {
│ │ │ │ +
197 typedef typename std::vector<std::pair<V1,V2> > VectorPair;
│ │ │ │ +
198 bool match = true;
│ │ │ │ +
199 if (expected.size() != actual.size())
│ │ │ │ +
200 match = false;
│ │ │ │ +
201 typename VectorPair::const_iterator
│ │ │ │ +
202 itExp = expected.begin(),
│ │ │ │ +
203 itAct = actual.begin();
│ │ │ │ +
204 if(match) {
│ │ │ │ +
205 for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) {
│ │ │ │ +
206 if (!assert_equal(itExp->first, itAct->first, tol) ||
│ │ │ │ +
207 !assert_equal(itExp->second, itAct->second, tol)) {
│ │ │ │ +
208 match = false;
│ │ │ │ +
209 break;
│ │ │ │ +
210 }
│ │ │ │ +
211 }
│ │ │ │ +
212 }
│ │ │ │ +
213 if(!match) {
│ │ │ │ +
214 std::cout << "expected: " << std::endl;
│ │ │ │ +
215 for(const typename VectorPair::value_type& a: expected) {
│ │ │ │ +
216 a.first.print( " first ");
│ │ │ │ +
217 a.second.print(" second");
│ │ │ │ +
218 }
│ │ │ │ +
219 std::cout << "\nactual: " << std::endl;
│ │ │ │ +
220 for(const typename VectorPair::value_type& a: actual) {
│ │ │ │ +
221 a.first.print( " first ");
│ │ │ │ +
222 a.second.print(" second");
│ │ │ │ +
223 }
│ │ │ │ +
224 std::cout << std::endl;
│ │ │ │ +
225 return false;
│ │ │ │ +
226 }
│ │ │ │ +
227 return true;
│ │ │ │ +
228}
│ │ │ │ +
│ │ │ │ +
229
│ │ │ │ +
230
│ │ │ │ +
234template<class V>
│ │ │ │ +
│ │ │ │ +
235bool assert_container_equal(const V& expected, const V& actual, double tol = 1e-9) {
│ │ │ │ +
236 bool match = true;
│ │ │ │ +
237 typename V::const_iterator
│ │ │ │ +
238 itExp = expected.begin(),
│ │ │ │ +
239 itAct = actual.begin();
│ │ │ │ +
240 if(match) {
│ │ │ │ +
241 for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) {
│ │ │ │ +
242 if (!assert_equal(*itExp, *itAct, tol)) {
│ │ │ │ +
243 match = false;
│ │ │ │ +
244 break;
│ │ │ │ +
245 }
│ │ │ │ +
246 }
│ │ │ │ +
247 if(itExp != expected.end() || itAct != actual.end())
│ │ │ │ +
248 match = false;
│ │ │ │ +
249 }
│ │ │ │ +
250 if(!match) {
│ │ │ │ +
251 std::cout << "expected: " << std::endl;
│ │ │ │ +
252 for(const typename V::value_type& a: expected) { a.print(" "); }
│ │ │ │ +
253 std::cout << "\nactual: " << std::endl;
│ │ │ │ +
254 for(const typename V::value_type& a: actual) { a.print(" "); }
│ │ │ │ +
255 std::cout << std::endl;
│ │ │ │ +
256 return false;
│ │ │ │ +
257 }
│ │ │ │ +
258 return true;
│ │ │ │ +
259}
│ │ │ │ +
│ │ │ │ +
260
│ │ │ │ +
265template<class V2>
│ │ │ │ +
│ │ │ │ +
266bool assert_container_equality(const std::map<size_t,V2>& expected, const std::map<size_t,V2>& actual) {
│ │ │ │ +
267 typedef typename std::map<size_t,V2> Map;
│ │ │ │ +
268 bool match = true;
│ │ │ │ +
269 if (expected.size() != actual.size())
│ │ │ │ +
270 match = false;
│ │ │ │ +
271 typename Map::const_iterator
│ │ │ │ +
272 itExp = expected.begin(),
│ │ │ │ +
273 itAct = actual.begin();
│ │ │ │ +
274 if(match) {
│ │ │ │ +
275 for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) {
│ │ │ │ +
276 if (itExp->first != itAct->first || itExp->second != itAct->second) {
│ │ │ │ +
277 match = false;
│ │ │ │ +
278 break;
│ │ │ │ +
279 }
│ │ │ │ +
280 }
│ │ │ │ +
281 }
│ │ │ │ +
282 if(!match) {
│ │ │ │ +
283 std::cout << "expected: " << std::endl;
│ │ │ │ +
284 for(const typename Map::value_type& a: expected) {
│ │ │ │ +
285 std::cout << "Key: " << a.first << std::endl;
│ │ │ │ +
286 std::cout << "Value: " << a.second << std::endl;
│ │ │ │ +
287 }
│ │ │ │ +
288 std::cout << "\nactual: " << std::endl;
│ │ │ │ +
289 for(const typename Map::value_type& a: actual) {
│ │ │ │ +
290 std::cout << "Key: " << a.first << std::endl;
│ │ │ │ +
291 std::cout << "Value: " << a.second << std::endl;
│ │ │ │ +
292 }
│ │ │ │ +
293 std::cout << std::endl;
│ │ │ │ +
294 return false;
│ │ │ │ +
295 }
│ │ │ │ +
296 return true;
│ │ │ │ +
297}
│ │ │ │ +
│ │ │ │ +
298
│ │ │ │ +
299
│ │ │ │ +
303template<class V>
│ │ │ │ +
│ │ │ │ +
304bool assert_container_equality(const V& expected, const V& actual) {
│ │ │ │ +
305 bool match = true;
│ │ │ │ +
306 if (expected.size() != actual.size())
│ │ │ │ +
307 match = false;
│ │ │ │ +
308 typename V::const_iterator
│ │ │ │ +
309 itExp = expected.begin(),
│ │ │ │ +
310 itAct = actual.begin();
│ │ │ │ +
311 if(match) {
│ │ │ │ +
312 for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) {
│ │ │ │ +
313 if (*itExp != *itAct) {
│ │ │ │ +
314 match = false;
│ │ │ │ +
315 break;
│ │ │ │ +
316 }
│ │ │ │ +
317 }
│ │ │ │ +
318 }
│ │ │ │ +
319 if(!match) {
│ │ │ │ +
320 std::cout << "expected: " << std::endl;
│ │ │ │ +
321 for(const typename V::value_type& a: expected) { std::cout << a << " "; }
│ │ │ │ +
322 std::cout << "\nactual: " << std::endl;
│ │ │ │ +
323 for(const typename V::value_type& a: actual) { std::cout << a << " "; }
│ │ │ │ +
324 std::cout << std::endl;
│ │ │ │ +
325 return false;
│ │ │ │ +
326 }
│ │ │ │ +
327 return true;
│ │ │ │ +
328}
│ │ │ │ +
│ │ │ │
329
│ │ │ │ -
337GTSAM_EXPORT Vector backSubstituteUpper(const Matrix& U, const Vector& b, bool unit=false);
│ │ │ │ -
338
│ │ │ │ -
346//TODO: is this function necessary? it isn't used
│ │ │ │ -
347GTSAM_EXPORT Vector backSubstituteUpper(const Vector& b, const Matrix& U, bool unit=false);
│ │ │ │ -
348
│ │ │ │ -
356GTSAM_EXPORT Vector backSubstituteLower(const Matrix& L, const Vector& b, bool unit=false);
│ │ │ │ -
357
│ │ │ │ -
364GTSAM_EXPORT Matrix stack(size_t nrMatrices, ...);
│ │ │ │ -
365GTSAM_EXPORT Matrix stack(const std::vector<Matrix>& blocks);
│ │ │ │ -
366
│ │ │ │ -
377GTSAM_EXPORT Matrix collect(const std::vector<const Matrix *>& matrices, size_t m = 0, size_t n = 0);
│ │ │ │ -
378GTSAM_EXPORT Matrix collect(size_t nrMatrices, ...);
│ │ │ │ -
379
│ │ │ │ -
386GTSAM_EXPORT void vector_scale_inplace(const Vector& v, Matrix& A, bool inf_mask = false); // row
│ │ │ │ -
387GTSAM_EXPORT Matrix vector_scale(const Vector& v, const Matrix& A, bool inf_mask = false); // row
│ │ │ │ -
388GTSAM_EXPORT Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask = false); // column
│ │ │ │ -
389
│ │ │ │ -
│ │ │ │ -
401inline Matrix3 skewSymmetric(double wx, double wy, double wz) {
│ │ │ │ -
402 return (Matrix3() << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0).finished();
│ │ │ │ -
403}
│ │ │ │ -
│ │ │ │ -
404
│ │ │ │ -
405template <class Derived>
│ │ │ │ -
406inline Matrix3 skewSymmetric(const Eigen::MatrixBase<Derived>& w) {
│ │ │ │ -
407 return skewSymmetric(w(0), w(1), w(2));
│ │ │ │ -
408}
│ │ │ │ -
409
│ │ │ │ -
411GTSAM_EXPORT Matrix inverse_square_root(const Matrix& A);
│ │ │ │ -
412
│ │ │ │ -
414GTSAM_EXPORT Matrix cholesky_inverse(const Matrix &A);
│ │ │ │ -
415
│ │ │ │ -
428GTSAM_EXPORT void svd(const Matrix& A, Matrix& U, Vector& S, Matrix& V);
│ │ │ │ -
429
│ │ │ │ -
437GTSAM_EXPORT boost::tuple<int, double, Vector>
│ │ │ │ -
438DLT(const Matrix& A, double rank_tol = 1e-9);
│ │ │ │ -
439
│ │ │ │ -
445GTSAM_EXPORT Matrix expm(const Matrix& A, size_t K=7);
│ │ │ │ -
446
│ │ │ │ -
447std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal = false);
│ │ │ │ -
448
│ │ │ │ -
455template <int N>
│ │ │ │ -
│ │ │ │ -
456struct MultiplyWithInverse {
│ │ │ │ -
457 typedef Eigen::Matrix<double, N, 1> VectorN;
│ │ │ │ -
458 typedef Eigen::Matrix<double, N, N> MatrixN;
│ │ │ │ -
459
│ │ │ │ -
│ │ │ │ -
461 VectorN operator()(const MatrixN& A, const VectorN& b,
│ │ │ │ -
462 OptionalJacobian<N, N* N> H1 = boost::none,
│ │ │ │ -
463 OptionalJacobian<N, N> H2 = boost::none) const {
│ │ │ │ -
464 const MatrixN invA = A.inverse();
│ │ │ │ -
465 const VectorN c = invA * b;
│ │ │ │ -
466 // The derivative in A is just -[c[0]*invA c[1]*invA ... c[N-1]*invA]
│ │ │ │ -
467 if (H1)
│ │ │ │ -
468 for (size_t j = 0; j < N; j++)
│ │ │ │ -
469 H1->template middleCols<N>(N * j) = -c[j] * invA;
│ │ │ │ -
470 // The derivative in b is easy, as invA*b is just a linear map:
│ │ │ │ -
471 if (H2) *H2 = invA;
│ │ │ │ -
472 return c;
│ │ │ │ -
473 }
│ │ │ │ -
│ │ │ │ -
474};
│ │ │ │ -
│ │ │ │ -
475
│ │ │ │ -
481template <typename T, int N>
│ │ │ │ -
│ │ │ │ -
482struct MultiplyWithInverseFunction {
│ │ │ │ -
483 enum { M = traits<T>::dimension };
│ │ │ │ -
484 typedef Eigen::Matrix<double, N, 1> VectorN;
│ │ │ │ -
485 typedef Eigen::Matrix<double, N, N> MatrixN;
│ │ │ │ -
486
│ │ │ │ -
487 // The function phi should calculate f(a)*b, with derivatives in a and b.
│ │ │ │ -
488 // Naturally, the derivative in b is f(a).
│ │ │ │ -
489 typedef std::function<VectorN(
│ │ │ │ -
490 const T&, const VectorN&, OptionalJacobian<N, M>, OptionalJacobian<N, N>)>
│ │ │ │ -
491 Operator;
│ │ │ │ -
492
│ │ │ │ -
494 MultiplyWithInverseFunction(const Operator& phi) : phi_(phi) {}
│ │ │ │ -
495
│ │ │ │ -
│ │ │ │ -
497 VectorN operator()(const T& a, const VectorN& b,
│ │ │ │ -
498 OptionalJacobian<N, M> H1 = boost::none,
│ │ │ │ -
499 OptionalJacobian<N, N> H2 = boost::none) const {
│ │ │ │ -
500 MatrixN A;
│ │ │ │ -
501 phi_(a, b, boost::none, A); // get A = f(a) by calling f once
│ │ │ │ -
502 const MatrixN invA = A.inverse();
│ │ │ │ -
503 const VectorN c = invA * b;
│ │ │ │ -
504
│ │ │ │ -
505 if (H1) {
│ │ │ │ -
506 Eigen::Matrix<double, N, M> H;
│ │ │ │ -
507 phi_(a, c, H, boost::none); // get derivative H of forward mapping
│ │ │ │ -
508 *H1 = -invA* H;
│ │ │ │ -
509 }
│ │ │ │ -
510 if (H2) *H2 = invA;
│ │ │ │ -
511 return c;
│ │ │ │ -
512 }
│ │ │ │ -
│ │ │ │ -
513
│ │ │ │ -
514 private:
│ │ │ │ -
515 const Operator phi_;
│ │ │ │ -
516};
│ │ │ │ -
│ │ │ │ -
517
│ │ │ │ -
518GTSAM_EXPORT Matrix LLt(const Matrix& A);
│ │ │ │ -
519
│ │ │ │ -
520GTSAM_EXPORT Matrix RtR(const Matrix& A);
│ │ │ │ -
521
│ │ │ │ -
522GTSAM_EXPORT Vector columnNormSquare(const Matrix &A);
│ │ │ │ -
523} // namespace gtsam
│ │ │ │ -
OptionalJacobian.h
Special class for optional Jacobian arguments.
│ │ │ │ -
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ +
│ │ │ │ +
333inline bool assert_equal(const std::string& expected, const std::string& actual) {
│ │ │ │ +
334 if (expected == actual)
│ │ │ │ +
335 return true;
│ │ │ │ +
336 printf("Not equal:\n");
│ │ │ │ +
337 std::cout << "expected: [" << expected << "]\n";
│ │ │ │ +
338 std::cout << "actual: [" << actual << "]" << std::endl;
│ │ │ │ +
339 return false;
│ │ │ │ +
340}
│ │ │ │ +
│ │ │ │ +
341
│ │ │ │ +
345template<class V>
│ │ │ │ +
│ │ │ │ +
346bool assert_inequal(const V& expected, const V& actual, double tol = 1e-9) {
│ │ │ │ +
347 if (!actual.equals(expected, tol))
│ │ │ │ +
348 return true;
│ │ │ │ +
349 printf("Erroneously equal:\n");
│ │ │ │ +
350 expected.print("expected");
│ │ │ │ +
351 actual.print("actual");
│ │ │ │ +
352 return false;
│ │ │ │ +
353}
│ │ │ │ +
│ │ │ │ +
354
│ │ │ │ +
358template<class V>
│ │ │ │ +
│ │ │ │ +
359bool assert_stdout_equal(const std::string& expected, const V& actual) {
│ │ │ │ +
360 // Redirect output to buffer so we can compare
│ │ │ │ +
361 std::stringstream buffer;
│ │ │ │ +
362 // Save the original output stream so we can reset later
│ │ │ │ +
363 std::streambuf* old = std::cout.rdbuf(buffer.rdbuf());
│ │ │ │ +
364
│ │ │ │ +
365 // We test against actual std::cout for faithful reproduction
│ │ │ │ +
366 std::cout << actual;
│ │ │ │ +
367
│ │ │ │ +
368 // Get output string and reset stdout
│ │ │ │ +
369 std::string actual_ = buffer.str();
│ │ │ │ +
370 std::cout.rdbuf(old);
│ │ │ │ +
371
│ │ │ │ +
372 return assert_equal(expected, actual_);
│ │ │ │ +
373}
│ │ │ │ +
│ │ │ │ +
374
│ │ │ │ +
380template <class V>
│ │ │ │ +
│ │ │ │ +
381bool assert_print_equal(const std::string& expected, const V& actual,
│ │ │ │ +
382 const std::string& s = "") {
│ │ │ │ +
383 // Redirect output to buffer so we can compare
│ │ │ │ +
384 std::stringstream buffer;
│ │ │ │ +
385 // Save the original output stream so we can reset later
│ │ │ │ +
386 std::streambuf* old = std::cout.rdbuf(buffer.rdbuf());
│ │ │ │ +
387
│ │ │ │ +
388 // We test against actual std::cout for faithful reproduction
│ │ │ │ +
389 actual.print(s);
│ │ │ │ +
390
│ │ │ │ +
391 // Get output string and reset stdout
│ │ │ │ +
392 std::string actual_ = buffer.str();
│ │ │ │ +
393 std::cout.rdbuf(old);
│ │ │ │ +
394
│ │ │ │ +
395 return assert_equal(expected, actual_);
│ │ │ │ +
396}
│ │ │ │ +
│ │ │ │ +
397
│ │ │ │ +
398} // \namespace gtsam
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
global_includes.h
Included from all GTSAM files.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::backSubstituteLower
Vector backSubstituteLower(const Matrix &L, const Vector &b, bool unit)
backSubstitute L*x=b
Definition Matrix.cpp:367
│ │ │ │ -
gtsam::operator^
Vector operator^(const Matrix &A, const Vector &v)
overload ^ for trans(A)*v We transpose the vectors for speed.
Definition Matrix.cpp:131
│ │ │ │ -
gtsam::vector_scale_inplace
void vector_scale_inplace(const Vector &v, Matrix &A, bool inf_mask)
scales a matrix row or column by the values in a vector Arguments (Matrix, Vector) scales the columns...
Definition Matrix.cpp:482
│ │ │ │ -
gtsam::row
const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j)
Extracts a row view from a matrix that avoids a copy.
Definition Matrix.h:222
│ │ │ │ -
gtsam::expm
T expm(const Vector &x, int K=7)
Exponential map given exponential coordinates class T needs a wedge<> function and a constructor from...
Definition Lie.h:317
│ │ │ │ -
gtsam::save
void save(const Matrix &A, const string &s, const string &filename)
save a matrix to file, which can be loaded by matlab
Definition Matrix.cpp:167
│ │ │ │ +
gtsam::assert_stdout_equal
bool assert_stdout_equal(const std::string &expected, const V &actual)
Capture std out via cout stream and compare against string.
Definition TestableAssertions.h:359
│ │ │ │ +
gtsam::assert_container_equal
bool assert_container_equal(const std::map< V1, V2 > &expected, const std::map< V1, V2 > &actual, double tol=1e-9)
Function for comparing maps of testable->testable TODO: replace with more generalized version.
Definition TestableAssertions.h:119
│ │ │ │
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
equals with an tolerance, prints out message if unequal
Definition Matrix.cpp:43
│ │ │ │ -
gtsam::linear_dependent
bool linear_dependent(const Matrix &A, const Matrix &B, double tol)
check whether the rows of two matrices are linear dependent
Definition Matrix.cpp:117
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::column
const MATRIX::ConstColXpr column(const MATRIX &A, size_t j)
Extracts a column view from a matrix that avoids a copy.
Definition Matrix.h:211
│ │ │ │ -
gtsam::zeroBelowDiagonal
void zeroBelowDiagonal(MATRIX &A, size_t cols=0)
Zeros all of the elements below the diagonal of a matrix, in place.
Definition Matrix.h:232
│ │ │ │ -
gtsam::weighted_eliminate
list< boost::tuple< Vector, double, double > > weighted_eliminate(Matrix &A, Vector &b, const Vector &sigmas)
Imperative algorithm for in-place full elimination with weights and constraint handling.
Definition Matrix.cpp:273
│ │ │ │ -
gtsam::backSubstituteUpper
Vector backSubstituteUpper(const Matrix &U, const Vector &b, bool unit)
backSubstitute U*x=b
Definition Matrix.cpp:377
│ │ │ │ +
gtsam::assert_container_equality
bool assert_container_equality(const std::map< size_t, V2 > &expected, const std::map< size_t, V2 > &actual)
Function for comparing maps of size_t->testable Types are assumed to have operator ==.
Definition TestableAssertions.h:266
│ │ │ │
gtsam::assert_inequal
bool assert_inequal(const Matrix &A, const Matrix &B, double tol)
inequals with an tolerance, prints out message if within tolerance
Definition Matrix.cpp:63
│ │ │ │ -
gtsam::householder
void householder(Matrix &A, size_t k)
Householder tranformation, zeros below diagonal.
Definition Matrix.cpp:354
│ │ │ │ -
gtsam::operator>>
istream & operator>>(istream &inputStream, Matrix &destinationMatrix)
Read a matrix from an input stream, such as a file.
Definition Matrix.cpp:174
│ │ │ │ -
gtsam::inplace_QR
void inplace_QR(Matrix &A)
QR factorization using Eigen's internal block QR algorithm.
Definition Matrix.cpp:636
│ │ │ │ -
gtsam::svd
void svd(const Matrix &A, Matrix &U, Vector &S, Matrix &V)
SVD computes economy SVD A=U*S*V'.
Definition Matrix.cpp:560
│ │ │ │ -
gtsam::skewSymmetric
Matrix3 skewSymmetric(double wx, double wy, double wz)
skew symmetric matrix returns this: 0 -wz wy wz 0 -wx -wy wx 0
Definition Matrix.h:401
│ │ │ │ -
gtsam::sub
Eigen::Block< const MATRIX > sub(const MATRIX &A, size_t i1, size_t i2, size_t j1, size_t j2)
extract submatrix, slice semantics, i.e.
Definition Matrix.h:181
│ │ │ │ -
gtsam::trans
Matrix trans(const Matrix &A)
static transpose function, just calls Eigen transpose member function
Definition Matrix.h:242
│ │ │ │ -
gtsam::operator!=
bool operator!=(const Matrix &A, const Matrix &B)
inequality
Definition Matrix.h:107
│ │ │ │ -
gtsam::DLT
boost::tuple< int, double, Vector > DLT(const Matrix &A, double rank_tol)
Direct linear transform algorithm that calls svd to find a vector v that minimizes the algebraic erro...
Definition Matrix.cpp:568
│ │ │ │ -
gtsam::cholesky_inverse
Matrix cholesky_inverse(const Matrix &A)
Return the inverse of a S.P.D.
Definition Matrix.cpp:539
│ │ │ │ -
gtsam::prod
MATRIX prod(const MATRIX &A, const MATRIX &B)
products using old-style format to improve compatibility
Definition Matrix.h:144
│ │ │ │ -
gtsam::householder_
void householder_(Matrix &A, size_t k, bool copy_vectors)
Imperative version of Householder QR factorization, Golub & Van Loan p 224 version with Householder v...
Definition Matrix.cpp:327
│ │ │ │ -
gtsam::insertSub
void insertSub(Eigen::MatrixBase< Derived1 > &fullMatrix, const Eigen::MatrixBase< Derived2 > &subMatrix, size_t i, size_t j)
insert a submatrix IN PLACE at a specified location in a larger matrix NOTE: there is no size checkin...
Definition Matrix.h:195
│ │ │ │ -
gtsam::collect
Matrix collect(const std::vector< const Matrix * > &matrices, size_t m, size_t n)
create a matrix by concatenating Given a set of matrices: A1, A2, A3... If all matrices have the same...
Definition Matrix.cpp:443
│ │ │ │ -
gtsam::linear_independent
bool linear_independent(const Matrix &A, const Matrix &B, double tol)
check whether the rows of two matrices are linear independent
Definition Matrix.cpp:103
│ │ │ │ -
gtsam::fpEqual
bool fpEqual(double a, double b, double tol, bool check_relative_also)
Ensure we are not including a different version of Eigen in user code than while compiling gtsam,...
Definition Vector.cpp:42
│ │ │ │ -
gtsam::qr
pair< Matrix, Matrix > qr(const Matrix &A)
Householder QR factorization, Golub & Van Loan p 224, explicit version
Definition Matrix.cpp:235
│ │ │ │ -
gtsam::diag
Matrix diag(const std::vector< Matrix > &Hs)
Create a matrix with submatrices along its diagonal.
Definition Matrix.cpp:207
│ │ │ │ -
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition Matrix.h:81
│ │ │ │ -
gtsam::operator==
bool operator==(const Matrix &A, const Matrix &B)
equality is just equal_with_abs_tol 1e-9
Definition Matrix.h:100
│ │ │ │ -
gtsam::inverse_square_root
Matrix inverse_square_root(const Matrix &A)
Use Cholesky to calculate inverse square root of a matrix.
Definition Matrix.cpp:552
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::Reshape
Reshape functor.
Definition Matrix.h:246
│ │ │ │ -
gtsam::MultiplyWithInverse
Functor that implements multiplication of a vector b with the inverse of a matrix A.
Definition Matrix.h:456
│ │ │ │ -
gtsam::MultiplyWithInverse::operator()
VectorN operator()(const MatrixN &A, const VectorN &b, OptionalJacobian< N, N *N > H1=boost::none, OptionalJacobian< N, N > H2=boost::none) const
A.inverse() * b, with optional derivatives.
Definition Matrix.h:461
│ │ │ │ -
gtsam::MultiplyWithInverseFunction
Functor that implements multiplication with the inverse of a matrix, itself the result of a function ...
Definition Matrix.h:482
│ │ │ │ -
gtsam::MultiplyWithInverseFunction::operator()
VectorN operator()(const T &a, const VectorN &b, OptionalJacobian< N, M > H1=boost::none, OptionalJacobian< N, N > H2=boost::none) const
f(a).inverse() * b, with optional derivatives
Definition Matrix.h:497
│ │ │ │ -
gtsam::MultiplyWithInverseFunction::MultiplyWithInverseFunction
MultiplyWithInverseFunction(const Operator &phi)
Construct with function as explained above.
Definition Matrix.h:494
│ │ │ │ -
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ +
gtsam::assert_print_equal
bool assert_print_equal(const std::string &expected, const V &actual, const std::string &s="")
Capture print function output and compare against string.
Definition TestableAssertions.h:381
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,534 +1,407 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Matrix.h │ │ │ │ │ +TestableAssertions.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -23// \callgraph │ │ │ │ │ -24 │ │ │ │ │ -25#pragma once │ │ │ │ │ -26 │ │ │ │ │ -27#include <_g_t_s_a_m_/_b_a_s_e_/_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h> │ │ │ │ │ -28#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_c_t_o_r_._h> │ │ │ │ │ -29#include │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ +22 │ │ │ │ │ +23#include │ │ │ │ │ +24#include │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28 │ │ │ │ │ +29namespace _g_t_s_a_m { │ │ │ │ │ 30 │ │ │ │ │ -31#include │ │ │ │ │ -32 │ │ │ │ │ -38namespace _g_t_s_a_m { │ │ │ │ │ -39 │ │ │ │ │ -40typedef Eigen::MatrixXd Matrix; │ │ │ │ │ -41typedef Eigen::Matrix MatrixRowMajor; │ │ │ │ │ -42 │ │ │ │ │ -43// Create handy typedefs and constants for square-size matrices │ │ │ │ │ -44// MatrixMN, MatrixN = MatrixNN, I_NxN, and Z_NxN, for M,N=1..9 │ │ │ │ │ -45#define GTSAM_MAKE_MATRIX_DEFS(N) \ │ │ │ │ │ -46using Matrix##N = Eigen::Matrix; \ │ │ │ │ │ -47using Matrix1##N = Eigen::Matrix; \ │ │ │ │ │ -48using Matrix2##N = Eigen::Matrix; \ │ │ │ │ │ -49using Matrix3##N = Eigen::Matrix; \ │ │ │ │ │ -50using Matrix4##N = Eigen::Matrix; \ │ │ │ │ │ -51using Matrix5##N = Eigen::Matrix; \ │ │ │ │ │ -52using Matrix6##N = Eigen::Matrix; \ │ │ │ │ │ -53using Matrix7##N = Eigen::Matrix; \ │ │ │ │ │ -54using Matrix8##N = Eigen::Matrix; \ │ │ │ │ │ -55using Matrix9##N = Eigen::Matrix; \ │ │ │ │ │ -56static const Eigen::MatrixBase::IdentityReturnType I_##N##x##N = │ │ │ │ │ -Matrix##N::Identity(); \ │ │ │ │ │ -57static const Eigen::MatrixBase::ConstantReturnType Z_##N##x##N = │ │ │ │ │ -Matrix##N::Zero(); │ │ │ │ │ -58 │ │ │ │ │ -59GTSAM_MAKE_MATRIX_DEFS(1) │ │ │ │ │ -60GTSAM_MAKE_MATRIX_DEFS(2) │ │ │ │ │ -61GTSAM_MAKE_MATRIX_DEFS(3) │ │ │ │ │ -62GTSAM_MAKE_MATRIX_DEFS(4) │ │ │ │ │ -63GTSAM_MAKE_MATRIX_DEFS(5) │ │ │ │ │ -64GTSAM_MAKE_MATRIX_DEFS(6) │ │ │ │ │ -65GTSAM_MAKE_MATRIX_DEFS(7) │ │ │ │ │ -66GTSAM_MAKE_MATRIX_DEFS(8) │ │ │ │ │ -67GTSAM_MAKE_MATRIX_DEFS(9) │ │ │ │ │ -68 │ │ │ │ │ -69// Matrix expressions for accessing parts of matrices │ │ │ │ │ -70typedef Eigen::Block SubMatrix; │ │ │ │ │ -71typedef Eigen::Block ConstSubMatrix; │ │ │ │ │ -72 │ │ │ │ │ -73// Matrix formatting arguments when printing. │ │ │ │ │ -74// Akin to Matlab style. │ │ │ │ │ -75const Eigen::IOFormat& matlabFormat(); │ │ │ │ │ -76 │ │ │ │ │ -80template │ │ │ │ │ -_8_1bool _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(const Eigen::DenseBase& A, const Eigen:: │ │ │ │ │ -DenseBase& B, double tol = 1e-9) { │ │ │ │ │ +_3_4inline bool _a_s_s_e_r_t___e_q_u_a_l(const _K_e_y& expected, const _K_e_y& actual, double tol = │ │ │ │ │ +0.0) { │ │ │ │ │ +35 if(expected != actual) { │ │ │ │ │ +36 std::cout << "Not equal:\nexpected: " << expected << "\nactual: " << actual │ │ │ │ │ +<< std::endl; │ │ │ │ │ +37 return false; │ │ │ │ │ +38 } │ │ │ │ │ +39 return true; │ │ │ │ │ +40} │ │ │ │ │ +41 │ │ │ │ │ +49template │ │ │ │ │ +_5_0bool _a_s_s_e_r_t___e_q_u_a_l(const boost::optional& expected, │ │ │ │ │ +51 const boost::optional& actual, double tol = 1e-9) { │ │ │ │ │ +52 if (!expected && actual) { │ │ │ │ │ +53 std::cout << "expected is boost::none, while actual is not" << std::endl; │ │ │ │ │ +54 return false; │ │ │ │ │ +55 } │ │ │ │ │ +56 if (expected && !actual) { │ │ │ │ │ +57 std::cout << "actual is boost::none, while expected is not" << std::endl; │ │ │ │ │ +58 return false; │ │ │ │ │ +59 } │ │ │ │ │ +60 if (!expected && !actual) │ │ │ │ │ +61 return true; │ │ │ │ │ +62 return _a_s_s_e_r_t___e_q_u_a_l(*expected, *actual, tol); │ │ │ │ │ +63} │ │ │ │ │ +64 │ │ │ │ │ +65template │ │ │ │ │ +66bool _a_s_s_e_r_t___e_q_u_a_l(const V& expected, const boost::optional& actual, double │ │ │ │ │ +tol = 1e-9) { │ │ │ │ │ +67 if (!actual) { │ │ │ │ │ +68 std::cout << "actual is boost::none" << std::endl; │ │ │ │ │ +69 return false; │ │ │ │ │ +70 } │ │ │ │ │ +71 return _a_s_s_e_r_t___e_q_u_a_l(expected, *actual, tol); │ │ │ │ │ +72} │ │ │ │ │ +73 │ │ │ │ │ +74template │ │ │ │ │ +75bool _a_s_s_e_r_t___e_q_u_a_l(const V& expected, const boost::optional& actual, │ │ │ │ │ +double tol = 1e-9) { │ │ │ │ │ +76 if (!actual) { │ │ │ │ │ +77 std::cout << "actual is boost::none" << std::endl; │ │ │ │ │ +78 return false; │ │ │ │ │ +79 } │ │ │ │ │ +80 return _a_s_s_e_r_t___e_q_u_a_l(expected, *actual, tol); │ │ │ │ │ +81} │ │ │ │ │ 82 │ │ │ │ │ -83 const size_t n1 = A.cols(), m1 = A.rows(); │ │ │ │ │ -84 const size_t n2 = B.cols(), m2 = B.rows(); │ │ │ │ │ -85 │ │ │ │ │ -86 if(m1!=m2 || n1!=n2) return false; │ │ │ │ │ -87 │ │ │ │ │ -88 for(size_t i=0; i │ │ │ │ │ +89bool GTSAM_DEPRECATED _a_s_s_e_r_t___e_q_u_a_l(const std::vector& expected, const │ │ │ │ │ +std::vector& actual, double tol = 1e-9) { │ │ │ │ │ +90 bool match = true; │ │ │ │ │ +91 if (expected.size() != actual.size()) │ │ │ │ │ +92 match = false; │ │ │ │ │ +93 if(match) { │ │ │ │ │ +94 size_t i = 0; │ │ │ │ │ +95 for(const V& a: expected) { │ │ │ │ │ +96 if (!_a_s_s_e_r_t___e_q_u_a_l(a, actual[i++], tol)) { │ │ │ │ │ +97 match = false; │ │ │ │ │ +98 break; │ │ │ │ │ +99 } │ │ │ │ │ +100 } │ │ │ │ │ +101 } │ │ │ │ │ +102 if(!match) { │ │ │ │ │ +103 std::cout << "expected: " << std::endl; │ │ │ │ │ +104 for(const V& a: expected) { std::cout << a << " "; } │ │ │ │ │ +105 std::cout << "\nactual: " << std::endl; │ │ │ │ │ +106 for(const V& a: actual) { std::cout << a << " "; } │ │ │ │ │ +107 std::cout << std::endl; │ │ │ │ │ +108 return false; │ │ │ │ │ 109 } │ │ │ │ │ -110 │ │ │ │ │ -114GTSAM_EXPORT bool _a_s_s_e_r_t___e_q_u_a_l(const Matrix& A, const Matrix& B, double tol │ │ │ │ │ -= 1e-9); │ │ │ │ │ -115 │ │ │ │ │ -119GTSAM_EXPORT bool _a_s_s_e_r_t___i_n_e_q_u_a_l(const Matrix& A, const Matrix& B, double │ │ │ │ │ -tol = 1e-9); │ │ │ │ │ -120 │ │ │ │ │ -124GTSAM_EXPORT bool _a_s_s_e_r_t___e_q_u_a_l(const std::list& As, const std:: │ │ │ │ │ -list& Bs, double tol = 1e-9); │ │ │ │ │ -125 │ │ │ │ │ -129GTSAM_EXPORT bool _l_i_n_e_a_r___i_n_d_e_p_e_n_d_e_n_t(const Matrix& A, const Matrix& B, │ │ │ │ │ -double tol = 1e-9); │ │ │ │ │ -130 │ │ │ │ │ -134GTSAM_EXPORT bool _l_i_n_e_a_r___d_e_p_e_n_d_e_n_t(const Matrix& A, const Matrix& B, double │ │ │ │ │ -tol = 1e-9); │ │ │ │ │ -135 │ │ │ │ │ -140GTSAM_EXPORT Vector _o_p_e_r_a_t_o_r_^(const Matrix& A, const Vector & v); │ │ │ │ │ -141 │ │ │ │ │ -143template │ │ │ │ │ -_1_4_4inline MATRIX _p_r_o_d(const MATRIX& A, const MATRIX&B) { │ │ │ │ │ -145 MATRIX result = A * B; │ │ │ │ │ -146 return result; │ │ │ │ │ -147} │ │ │ │ │ -148 │ │ │ │ │ -152GTSAM_EXPORT void _p_r_i_n_t(const Matrix& A, const std::string& s, std::ostream& │ │ │ │ │ -stream); │ │ │ │ │ -153 │ │ │ │ │ -157GTSAM_EXPORT void _p_r_i_n_t(const Matrix& A, const std::string& s = ""); │ │ │ │ │ -158 │ │ │ │ │ -162GTSAM_EXPORT void _s_a_v_e(const Matrix& A, const std::string &s, const std:: │ │ │ │ │ -string& filename); │ │ │ │ │ -163 │ │ │ │ │ -169GTSAM_EXPORT std::istream& _o_p_e_r_a_t_o_r_>_>(std::istream& inputStream, Matrix& │ │ │ │ │ -destinationMatrix); │ │ │ │ │ -170 │ │ │ │ │ -180template │ │ │ │ │ -_1_8_1Eigen::Block _s_u_b(const MATRIX& A, size_t i1, size_t i2, size_t │ │ │ │ │ -j1, size_t j2) { │ │ │ │ │ -182 size_t m=i2-i1, n=j2-j1; │ │ │ │ │ -183 return A.block(i1,j1,m,n); │ │ │ │ │ -184} │ │ │ │ │ -185 │ │ │ │ │ -194template │ │ │ │ │ -_1_9_5void _i_n_s_e_r_t_S_u_b(Eigen::MatrixBase& fullMatrix, const Eigen:: │ │ │ │ │ -MatrixBase& subMatrix, size_t i, size_t j) { │ │ │ │ │ -196 fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix; │ │ │ │ │ -197} │ │ │ │ │ -198 │ │ │ │ │ -202GTSAM_EXPORT Matrix _d_i_a_g(const std::vector& Hs); │ │ │ │ │ -203 │ │ │ │ │ -210template │ │ │ │ │ -_2_1_1const typename MATRIX::ConstColXpr _c_o_l_u_m_n(const MATRIX& A, size_t j) { │ │ │ │ │ -212 return A.col(j); │ │ │ │ │ -213} │ │ │ │ │ -214 │ │ │ │ │ -221template │ │ │ │ │ -_2_2_2const typename MATRIX::ConstRowXpr _r_o_w(const MATRIX& A, size_t j) { │ │ │ │ │ -223 return A.row(j); │ │ │ │ │ -224} │ │ │ │ │ -225 │ │ │ │ │ -231template │ │ │ │ │ -_2_3_2void _z_e_r_o_B_e_l_o_w_D_i_a_g_o_n_a_l(MATRIX& A, size_t cols=0) { │ │ │ │ │ -233 const size_t m = A.rows(), n = A.cols(); │ │ │ │ │ -234 const size_t k = (cols) ? std::min(cols, std::min(m,n)) : std::min(m,n); │ │ │ │ │ -235 for (size_t j=0; j │ │ │ │ │ -_2_4_6struct _R_e_s_h_a_p_e { │ │ │ │ │ -247 //TODO replace this with Eigen's reshape function as soon as available. │ │ │ │ │ -(There is a PR already pending : https://bitbucket.org/eigen/eigen/pull- │ │ │ │ │ -request/41/reshape/diff) │ │ │ │ │ -248 typedef Eigen::Map > │ │ │ │ │ -ReshapedType; │ │ │ │ │ -249 static inline ReshapedType reshape(const Eigen::Matrix & in) { │ │ │ │ │ -250 return in.data(); │ │ │ │ │ -251 } │ │ │ │ │ -252}; │ │ │ │ │ -253 │ │ │ │ │ -255template │ │ │ │ │ -_2_5_6struct _R_e_s_h_a_p_e { │ │ │ │ │ -257 typedef const Eigen::Matrix & ReshapedType; │ │ │ │ │ -258 static inline ReshapedType reshape(const Eigen::Matrix & in) { │ │ │ │ │ -259 return in; │ │ │ │ │ -260 } │ │ │ │ │ -261}; │ │ │ │ │ -262 │ │ │ │ │ -264template │ │ │ │ │ -_2_6_5struct _R_e_s_h_a_p_e { │ │ │ │ │ -266 typedef const Eigen::Matrix & ReshapedType; │ │ │ │ │ -267 static inline ReshapedType reshape(const Eigen::Matrix & in) { │ │ │ │ │ -268 return in; │ │ │ │ │ -269 } │ │ │ │ │ -270}; │ │ │ │ │ -271 │ │ │ │ │ -273template │ │ │ │ │ -_2_7_4struct _R_e_s_h_a_p_e { │ │ │ │ │ -275 typedef typename Eigen::Matrix:: │ │ │ │ │ -ConstTransposeReturnType ReshapedType; │ │ │ │ │ -276 static inline ReshapedType reshape(const Eigen::Matrix & in) { │ │ │ │ │ -277 return in.transpose(); │ │ │ │ │ -278 } │ │ │ │ │ -279}; │ │ │ │ │ -280 │ │ │ │ │ -281template │ │ │ │ │ -282inline typename Reshape:: │ │ │ │ │ -ReshapedType reshape(const Eigen::Matrix & m){ │ │ │ │ │ -283 BOOST_STATIC_ASSERT(InM * InN == OutM * OutN); │ │ │ │ │ -284 return _R_e_s_h_a_p_e_<_O_u_t_M_,_ _O_u_t_N_,_ _O_u_t_O_p_t_i_o_n_s_,_ _I_n_M_,_ _I_n_N_,_ _I_n_O_p_t_i_o_n_s_>_:_:_r_e_s_h_a_p_e(m); │ │ │ │ │ -285} │ │ │ │ │ -286 │ │ │ │ │ -293GTSAM_EXPORT std::pair _q_r(const Matrix& A); │ │ │ │ │ -294 │ │ │ │ │ -300GTSAM_EXPORT void _i_n_p_l_a_c_e___Q_R(Matrix& A); │ │ │ │ │ -301 │ │ │ │ │ -310GTSAM_EXPORT std::list > │ │ │ │ │ -311_w_e_i_g_h_t_e_d___e_l_i_m_i_n_a_t_e(Matrix& A, Vector& b, const Vector& sigmas); │ │ │ │ │ -312 │ │ │ │ │ -320GTSAM_EXPORT void _h_o_u_s_e_h_o_l_d_e_r__(Matrix& A, size_t k, bool copy_vectors=true); │ │ │ │ │ -321 │ │ │ │ │ -328GTSAM_EXPORT void _h_o_u_s_e_h_o_l_d_e_r(Matrix& A, size_t k); │ │ │ │ │ +110 return true; │ │ │ │ │ +111} │ │ │ │ │ +112#endif │ │ │ │ │ +113 │ │ │ │ │ +118template │ │ │ │ │ +_1_1_9bool _a_s_s_e_r_t___c_o_n_t_a_i_n_e_r___e_q_u_a_l(const std::map& expected, const std:: │ │ │ │ │ +map& actual, double tol = 1e-9) { │ │ │ │ │ +120 typedef typename std::map Map; │ │ │ │ │ +121 bool match = true; │ │ │ │ │ +122 if (expected.size() != actual.size()) │ │ │ │ │ +123 match = false; │ │ │ │ │ +124 typename Map::const_iterator │ │ │ │ │ +125 itExp = expected.begin(), │ │ │ │ │ +126 itAct = actual.begin(); │ │ │ │ │ +127 if(match) { │ │ │ │ │ +128 for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { │ │ │ │ │ +129 if (!_a_s_s_e_r_t___e_q_u_a_l(itExp->first, itAct->first, tol) || │ │ │ │ │ +130 !_a_s_s_e_r_t___e_q_u_a_l(itExp->second, itAct->second, tol)) { │ │ │ │ │ +131 match = false; │ │ │ │ │ +132 break; │ │ │ │ │ +133 } │ │ │ │ │ +134 } │ │ │ │ │ +135 } │ │ │ │ │ +136 if(!match) { │ │ │ │ │ +137 std::cout << "expected: " << std::endl; │ │ │ │ │ +138 for(const typename Map::value_type& a: expected) { │ │ │ │ │ +139 a.first.print("key"); │ │ │ │ │ +140 a.second.print(" value"); │ │ │ │ │ +141 } │ │ │ │ │ +142 std::cout << "\nactual: " << std::endl; │ │ │ │ │ +143 for(const typename Map::value_type& a: actual) { │ │ │ │ │ +144 a.first.print("key"); │ │ │ │ │ +145 a.second.print(" value"); │ │ │ │ │ +146 } │ │ │ │ │ +147 std::cout << std::endl; │ │ │ │ │ +148 return false; │ │ │ │ │ +149 } │ │ │ │ │ +150 return true; │ │ │ │ │ +151} │ │ │ │ │ +152 │ │ │ │ │ +156template │ │ │ │ │ +_1_5_7bool _a_s_s_e_r_t___c_o_n_t_a_i_n_e_r___e_q_u_a_l(const std::map& expected, const std:: │ │ │ │ │ +map& actual, double tol = 1e-9) { │ │ │ │ │ +158 typedef typename std::map Map; │ │ │ │ │ +159 bool match = true; │ │ │ │ │ +160 if (expected.size() != actual.size()) │ │ │ │ │ +161 match = false; │ │ │ │ │ +162 typename Map::const_iterator │ │ │ │ │ +163 itExp = expected.begin(), │ │ │ │ │ +164 itAct = actual.begin(); │ │ │ │ │ +165 if(match) { │ │ │ │ │ +166 for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { │ │ │ │ │ +167 if (itExp->first != itAct->first || │ │ │ │ │ +168 !_a_s_s_e_r_t___e_q_u_a_l(itExp->second, itAct->second, tol)) { │ │ │ │ │ +169 match = false; │ │ │ │ │ +170 break; │ │ │ │ │ +171 } │ │ │ │ │ +172 } │ │ │ │ │ +173 } │ │ │ │ │ +174 if(!match) { │ │ │ │ │ +175 std::cout << "expected: " << std::endl; │ │ │ │ │ +176 for(const typename Map::value_type& a: expected) { │ │ │ │ │ +177 std::cout << "Key: " << a.first << std::endl; │ │ │ │ │ +178 a.second.print(" value"); │ │ │ │ │ +179 } │ │ │ │ │ +180 std::cout << "\nactual: " << std::endl; │ │ │ │ │ +181 for(const typename Map::value_type& a: actual) { │ │ │ │ │ +182 std::cout << "Key: " << a.first << std::endl; │ │ │ │ │ +183 a.second.print(" value"); │ │ │ │ │ +184 } │ │ │ │ │ +185 std::cout << std::endl; │ │ │ │ │ +186 return false; │ │ │ │ │ +187 } │ │ │ │ │ +188 return true; │ │ │ │ │ +189} │ │ │ │ │ +190 │ │ │ │ │ +194template │ │ │ │ │ +_1_9_5bool _a_s_s_e_r_t___c_o_n_t_a_i_n_e_r___e_q_u_a_l(const std::vector >& expected, │ │ │ │ │ +196 const std::vector >& actual, double tol = 1e-9) { │ │ │ │ │ +197 typedef typename std::vector > VectorPair; │ │ │ │ │ +198 bool match = true; │ │ │ │ │ +199 if (expected.size() != actual.size()) │ │ │ │ │ +200 match = false; │ │ │ │ │ +201 typename VectorPair::const_iterator │ │ │ │ │ +202 itExp = expected.begin(), │ │ │ │ │ +203 itAct = actual.begin(); │ │ │ │ │ +204 if(match) { │ │ │ │ │ +205 for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { │ │ │ │ │ +206 if (!_a_s_s_e_r_t___e_q_u_a_l(itExp->first, itAct->first, tol) || │ │ │ │ │ +207 !_a_s_s_e_r_t___e_q_u_a_l(itExp->second, itAct->second, tol)) { │ │ │ │ │ +208 match = false; │ │ │ │ │ +209 break; │ │ │ │ │ +210 } │ │ │ │ │ +211 } │ │ │ │ │ +212 } │ │ │ │ │ +213 if(!match) { │ │ │ │ │ +214 std::cout << "expected: " << std::endl; │ │ │ │ │ +215 for(const typename VectorPair::value_type& a: expected) { │ │ │ │ │ +216 a.first.print( " first "); │ │ │ │ │ +217 a.second.print(" second"); │ │ │ │ │ +218 } │ │ │ │ │ +219 std::cout << "\nactual: " << std::endl; │ │ │ │ │ +220 for(const typename VectorPair::value_type& a: actual) { │ │ │ │ │ +221 a.first.print( " first "); │ │ │ │ │ +222 a.second.print(" second"); │ │ │ │ │ +223 } │ │ │ │ │ +224 std::cout << std::endl; │ │ │ │ │ +225 return false; │ │ │ │ │ +226 } │ │ │ │ │ +227 return true; │ │ │ │ │ +228} │ │ │ │ │ +229 │ │ │ │ │ +230 │ │ │ │ │ +234template │ │ │ │ │ +_2_3_5bool _a_s_s_e_r_t___c_o_n_t_a_i_n_e_r___e_q_u_a_l(const V& expected, const V& actual, double tol = │ │ │ │ │ +1e-9) { │ │ │ │ │ +236 bool match = true; │ │ │ │ │ +237 typename V::const_iterator │ │ │ │ │ +238 itExp = expected.begin(), │ │ │ │ │ +239 itAct = actual.begin(); │ │ │ │ │ +240 if(match) { │ │ │ │ │ +241 for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { │ │ │ │ │ +242 if (!_a_s_s_e_r_t___e_q_u_a_l(*itExp, *itAct, tol)) { │ │ │ │ │ +243 match = false; │ │ │ │ │ +244 break; │ │ │ │ │ +245 } │ │ │ │ │ +246 } │ │ │ │ │ +247 if(itExp != expected.end() || itAct != actual.end()) │ │ │ │ │ +248 match = false; │ │ │ │ │ +249 } │ │ │ │ │ +250 if(!match) { │ │ │ │ │ +251 std::cout << "expected: " << std::endl; │ │ │ │ │ +252 for(const typename V::value_type& a: expected) { a.print(" "); } │ │ │ │ │ +253 std::cout << "\nactual: " << std::endl; │ │ │ │ │ +254 for(const typename V::value_type& a: actual) { a.print(" "); } │ │ │ │ │ +255 std::cout << std::endl; │ │ │ │ │ +256 return false; │ │ │ │ │ +257 } │ │ │ │ │ +258 return true; │ │ │ │ │ +259} │ │ │ │ │ +260 │ │ │ │ │ +265template │ │ │ │ │ +_2_6_6bool _a_s_s_e_r_t___c_o_n_t_a_i_n_e_r___e_q_u_a_l_i_t_y(const std::map& expected, const │ │ │ │ │ +std::map& actual) { │ │ │ │ │ +267 typedef typename std::map Map; │ │ │ │ │ +268 bool match = true; │ │ │ │ │ +269 if (expected.size() != actual.size()) │ │ │ │ │ +270 match = false; │ │ │ │ │ +271 typename Map::const_iterator │ │ │ │ │ +272 itExp = expected.begin(), │ │ │ │ │ +273 itAct = actual.begin(); │ │ │ │ │ +274 if(match) { │ │ │ │ │ +275 for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { │ │ │ │ │ +276 if (itExp->first != itAct->first || itExp->second != itAct->second) { │ │ │ │ │ +277 match = false; │ │ │ │ │ +278 break; │ │ │ │ │ +279 } │ │ │ │ │ +280 } │ │ │ │ │ +281 } │ │ │ │ │ +282 if(!match) { │ │ │ │ │ +283 std::cout << "expected: " << std::endl; │ │ │ │ │ +284 for(const typename Map::value_type& a: expected) { │ │ │ │ │ +285 std::cout << "Key: " << a.first << std::endl; │ │ │ │ │ +286 std::cout << "Value: " << a.second << std::endl; │ │ │ │ │ +287 } │ │ │ │ │ +288 std::cout << "\nactual: " << std::endl; │ │ │ │ │ +289 for(const typename Map::value_type& a: actual) { │ │ │ │ │ +290 std::cout << "Key: " << a.first << std::endl; │ │ │ │ │ +291 std::cout << "Value: " << a.second << std::endl; │ │ │ │ │ +292 } │ │ │ │ │ +293 std::cout << std::endl; │ │ │ │ │ +294 return false; │ │ │ │ │ +295 } │ │ │ │ │ +296 return true; │ │ │ │ │ +297} │ │ │ │ │ +298 │ │ │ │ │ +299 │ │ │ │ │ +303template │ │ │ │ │ +_3_0_4bool _a_s_s_e_r_t___c_o_n_t_a_i_n_e_r___e_q_u_a_l_i_t_y(const V& expected, const V& actual) { │ │ │ │ │ +305 bool match = true; │ │ │ │ │ +306 if (expected.size() != actual.size()) │ │ │ │ │ +307 match = false; │ │ │ │ │ +308 typename V::const_iterator │ │ │ │ │ +309 itExp = expected.begin(), │ │ │ │ │ +310 itAct = actual.begin(); │ │ │ │ │ +311 if(match) { │ │ │ │ │ +312 for (; itExp!=expected.end() && itAct!=actual.end(); ++itExp, ++itAct) { │ │ │ │ │ +313 if (*itExp != *itAct) { │ │ │ │ │ +314 match = false; │ │ │ │ │ +315 break; │ │ │ │ │ +316 } │ │ │ │ │ +317 } │ │ │ │ │ +318 } │ │ │ │ │ +319 if(!match) { │ │ │ │ │ +320 std::cout << "expected: " << std::endl; │ │ │ │ │ +321 for(const typename V::value_type& a: expected) { std::cout << a << " "; } │ │ │ │ │ +322 std::cout << "\nactual: " << std::endl; │ │ │ │ │ +323 for(const typename V::value_type& a: actual) { std::cout << a << " "; } │ │ │ │ │ +324 std::cout << std::endl; │ │ │ │ │ +325 return false; │ │ │ │ │ +326 } │ │ │ │ │ +327 return true; │ │ │ │ │ +328} │ │ │ │ │ 329 │ │ │ │ │ -337GTSAM_EXPORT Vector _b_a_c_k_S_u_b_s_t_i_t_u_t_e_U_p_p_e_r(const Matrix& U, const Vector& b, │ │ │ │ │ -bool unit=false); │ │ │ │ │ -338 │ │ │ │ │ -346//TODO: is this function necessary? it isn't used │ │ │ │ │ -347GTSAM_EXPORT Vector _b_a_c_k_S_u_b_s_t_i_t_u_t_e_U_p_p_e_r(const Vector& b, const Matrix& U, │ │ │ │ │ -bool unit=false); │ │ │ │ │ -348 │ │ │ │ │ -356GTSAM_EXPORT Vector _b_a_c_k_S_u_b_s_t_i_t_u_t_e_L_o_w_e_r(const Matrix& L, const Vector& b, │ │ │ │ │ -bool unit=false); │ │ │ │ │ -357 │ │ │ │ │ -364GTSAM_EXPORT Matrix stack(size_t nrMatrices, ...); │ │ │ │ │ -365GTSAM_EXPORT Matrix stack(const std::vector& blocks); │ │ │ │ │ -366 │ │ │ │ │ -377GTSAM_EXPORT Matrix _c_o_l_l_e_c_t(const std::vector& matrices, │ │ │ │ │ -size_t m = 0, size_t n = 0); │ │ │ │ │ -378GTSAM_EXPORT Matrix _c_o_l_l_e_c_t(size_t nrMatrices, ...); │ │ │ │ │ -379 │ │ │ │ │ -386GTSAM_EXPORT void _v_e_c_t_o_r___s_c_a_l_e___i_n_p_l_a_c_e(const Vector& v, Matrix& A, bool │ │ │ │ │ -inf_mask = false); // row │ │ │ │ │ -387GTSAM_EXPORT Matrix vector_scale(const Vector& v, const Matrix& A, bool │ │ │ │ │ -inf_mask = false); // row │ │ │ │ │ -388GTSAM_EXPORT Matrix vector_scale(const Matrix& A, const Vector& v, bool │ │ │ │ │ -inf_mask = false); // column │ │ │ │ │ -389 │ │ │ │ │ -_4_0_1inline Matrix3 _s_k_e_w_S_y_m_m_e_t_r_i_c(double wx, double wy, double wz) { │ │ │ │ │ -402 return (Matrix3() << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0).finished │ │ │ │ │ -(); │ │ │ │ │ -403} │ │ │ │ │ -404 │ │ │ │ │ -405template │ │ │ │ │ -406inline Matrix3 _s_k_e_w_S_y_m_m_e_t_r_i_c(const Eigen::MatrixBase& w) { │ │ │ │ │ -407 return _s_k_e_w_S_y_m_m_e_t_r_i_c(w(0), w(1), w(2)); │ │ │ │ │ -408} │ │ │ │ │ -409 │ │ │ │ │ -411GTSAM_EXPORT Matrix _i_n_v_e_r_s_e___s_q_u_a_r_e___r_o_o_t(const Matrix& A); │ │ │ │ │ -412 │ │ │ │ │ -414GTSAM_EXPORT Matrix _c_h_o_l_e_s_k_y___i_n_v_e_r_s_e(const Matrix &A); │ │ │ │ │ -415 │ │ │ │ │ -428GTSAM_EXPORT void _s_v_d(const Matrix& A, Matrix& U, Vector& S, Matrix& V); │ │ │ │ │ -429 │ │ │ │ │ -437GTSAM_EXPORT boost::tuple │ │ │ │ │ -438_D_L_T(const Matrix& A, double rank_tol = 1e-9); │ │ │ │ │ -439 │ │ │ │ │ -445GTSAM_EXPORT Matrix _e_x_p_m(const Matrix& A, size_t K=7); │ │ │ │ │ -446 │ │ │ │ │ -447std::string formatMatrixIndented(const std::string& label, const Matrix& │ │ │ │ │ -matrix, bool makeVectorHorizontal = false); │ │ │ │ │ -448 │ │ │ │ │ -455template │ │ │ │ │ -_4_5_6struct _M_u_l_t_i_p_l_y_W_i_t_h_I_n_v_e_r_s_e { │ │ │ │ │ -457 typedef Eigen::Matrix VectorN; │ │ │ │ │ -458 typedef Eigen::Matrix MatrixN; │ │ │ │ │ -459 │ │ │ │ │ -_4_6_1 VectorN _o_p_e_r_a_t_o_r_(_)(const MatrixN& A, const VectorN& b, │ │ │ │ │ -462 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_N_,_ _N_*_ _N_> H1 = boost::none, │ │ │ │ │ -463 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_N_,_ _N_> H2 = boost::none) const { │ │ │ │ │ -464 const MatrixN invA = A.inverse(); │ │ │ │ │ -465 const VectorN c = invA * b; │ │ │ │ │ -466 // The derivative in A is just -[c[0]*invA c[1]*invA ... c[N-1]*invA] │ │ │ │ │ -467 if (H1) │ │ │ │ │ -468 for (size_t j = 0; j < N; j++) │ │ │ │ │ -469 H1->template middleCols(N * j) = -c[j] * invA; │ │ │ │ │ -470 // The derivative in b is easy, as invA*b is just a linear map: │ │ │ │ │ -471 if (H2) *H2 = invA; │ │ │ │ │ -472 return c; │ │ │ │ │ -473 } │ │ │ │ │ -474}; │ │ │ │ │ -475 │ │ │ │ │ -481template │ │ │ │ │ -_4_8_2struct _M_u_l_t_i_p_l_y_W_i_t_h_I_n_v_e_r_s_e_F_u_n_c_t_i_o_n { │ │ │ │ │ -483 enum { M = _t_r_a_i_t_s_<_T_>_:_:_d_i_m_e_n_s_i_o_n }; │ │ │ │ │ -484 typedef Eigen::Matrix VectorN; │ │ │ │ │ -485 typedef Eigen::Matrix MatrixN; │ │ │ │ │ -486 │ │ │ │ │ -487 // The function phi should calculate f(a)*b, with derivatives in a and b. │ │ │ │ │ -488 // Naturally, the derivative in b is f(a). │ │ │ │ │ -489 typedef std::function, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_N_,_ _N_>)> │ │ │ │ │ -491 Operator; │ │ │ │ │ -492 │ │ │ │ │ -_4_9_4 _M_u_l_t_i_p_l_y_W_i_t_h_I_n_v_e_r_s_e_F_u_n_c_t_i_o_n(const Operator& phi) : phi_(phi) {} │ │ │ │ │ -495 │ │ │ │ │ -_4_9_7 VectorN _o_p_e_r_a_t_o_r_(_)(const T& a, const VectorN& b, │ │ │ │ │ -498 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_N_,_ _M_> H1 = boost::none, │ │ │ │ │ -499 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_N_,_ _N_> H2 = boost::none) const { │ │ │ │ │ -500 MatrixN A; │ │ │ │ │ -501 phi_(a, b, boost::none, A); // get A = f(a) by calling f once │ │ │ │ │ -502 const MatrixN invA = A.inverse(); │ │ │ │ │ -503 const VectorN c = invA * b; │ │ │ │ │ -504 │ │ │ │ │ -505 if (H1) { │ │ │ │ │ -506 Eigen::Matrix H; │ │ │ │ │ -507 phi_(a, c, H, boost::none); // get derivative H of forward mapping │ │ │ │ │ -508 *H1 = -invA* H; │ │ │ │ │ -509 } │ │ │ │ │ -510 if (H2) *H2 = invA; │ │ │ │ │ -511 return c; │ │ │ │ │ -512 } │ │ │ │ │ -513 │ │ │ │ │ -514 private: │ │ │ │ │ -515 const Operator phi_; │ │ │ │ │ -516}; │ │ │ │ │ -517 │ │ │ │ │ -518GTSAM_EXPORT Matrix LLt(const Matrix& A); │ │ │ │ │ -519 │ │ │ │ │ -520GTSAM_EXPORT Matrix RtR(const Matrix& A); │ │ │ │ │ -521 │ │ │ │ │ -522GTSAM_EXPORT Vector columnNormSquare(const Matrix &A); │ │ │ │ │ -523} // namespace gtsam │ │ │ │ │ -_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h │ │ │ │ │ -Special class for optional Jacobian arguments. │ │ │ │ │ -_V_e_c_t_o_r_._h │ │ │ │ │ -typedef and functions to augment Eigen's VectorXd │ │ │ │ │ +_3_3_3inline bool _a_s_s_e_r_t___e_q_u_a_l(const std::string& expected, const std::string& │ │ │ │ │ +actual) { │ │ │ │ │ +334 if (expected == actual) │ │ │ │ │ +335 return true; │ │ │ │ │ +336 printf("Not equal:\n"); │ │ │ │ │ +337 std::cout << "expected: [" << expected << "]\n"; │ │ │ │ │ +338 std::cout << "actual: [" << actual << "]" << std::endl; │ │ │ │ │ +339 return false; │ │ │ │ │ +340} │ │ │ │ │ +341 │ │ │ │ │ +345template │ │ │ │ │ +_3_4_6bool _a_s_s_e_r_t___i_n_e_q_u_a_l(const V& expected, const V& actual, double tol = 1e-9) { │ │ │ │ │ +347 if (!actual.equals(expected, tol)) │ │ │ │ │ +348 return true; │ │ │ │ │ +349 printf("Erroneously equal:\n"); │ │ │ │ │ +350 expected.print("expected"); │ │ │ │ │ +351 actual.print("actual"); │ │ │ │ │ +352 return false; │ │ │ │ │ +353} │ │ │ │ │ +354 │ │ │ │ │ +358template │ │ │ │ │ +_3_5_9bool _a_s_s_e_r_t___s_t_d_o_u_t___e_q_u_a_l(const std::string& expected, const V& actual) { │ │ │ │ │ +360 // Redirect output to buffer so we can compare │ │ │ │ │ +361 std::stringstream buffer; │ │ │ │ │ +362 // Save the original output stream so we can reset later │ │ │ │ │ +363 std::streambuf* old = std::cout.rdbuf(buffer.rdbuf()); │ │ │ │ │ +364 │ │ │ │ │ +365 // We test against actual std::cout for faithful reproduction │ │ │ │ │ +366 std::cout << actual; │ │ │ │ │ +367 │ │ │ │ │ +368 // Get output string and reset stdout │ │ │ │ │ +369 std::string actual_ = buffer.str(); │ │ │ │ │ +370 std::cout.rdbuf(old); │ │ │ │ │ +371 │ │ │ │ │ +372 return _a_s_s_e_r_t___e_q_u_a_l(expected, actual_); │ │ │ │ │ +373} │ │ │ │ │ +374 │ │ │ │ │ +380template │ │ │ │ │ +_3_8_1bool _a_s_s_e_r_t___p_r_i_n_t___e_q_u_a_l(const std::string& expected, const V& actual, │ │ │ │ │ +382 const std::string& s = "") { │ │ │ │ │ +383 // Redirect output to buffer so we can compare │ │ │ │ │ +384 std::stringstream buffer; │ │ │ │ │ +385 // Save the original output stream so we can reset later │ │ │ │ │ +386 std::streambuf* old = std::cout.rdbuf(buffer.rdbuf()); │ │ │ │ │ +387 │ │ │ │ │ +388 // We test against actual std::cout for faithful reproduction │ │ │ │ │ +389 actual.print(s); │ │ │ │ │ +390 │ │ │ │ │ +391 // Get output string and reset stdout │ │ │ │ │ +392 std::string actual_ = buffer.str(); │ │ │ │ │ +393 std::cout.rdbuf(old); │ │ │ │ │ +394 │ │ │ │ │ +395 return _a_s_s_e_r_t___e_q_u_a_l(expected, actual_); │ │ │ │ │ +396} │ │ │ │ │ +397 │ │ │ │ │ +398} // \namespace gtsam │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ +_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ +Included from all GTSAM files. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_b_a_c_k_S_u_b_s_t_i_t_u_t_e_L_o_w_e_r │ │ │ │ │ -Vector backSubstituteLower(const Matrix &L, const Vector &b, bool unit) │ │ │ │ │ -backSubstitute L*x=b │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:367 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_^ │ │ │ │ │ -Vector operator^(const Matrix &A, const Vector &v) │ │ │ │ │ -overload ^ for trans(A)*v We transpose the vectors for speed. │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:131 │ │ │ │ │ -_g_t_s_a_m_:_:_v_e_c_t_o_r___s_c_a_l_e___i_n_p_l_a_c_e │ │ │ │ │ -void vector_scale_inplace(const Vector &v, Matrix &A, bool inf_mask) │ │ │ │ │ -scales a matrix row or column by the values in a vector Arguments (Matrix, │ │ │ │ │ -Vector) scales the columns... │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:482 │ │ │ │ │ -_g_t_s_a_m_:_:_r_o_w │ │ │ │ │ -const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j) │ │ │ │ │ -Extracts a row view from a matrix that avoids a copy. │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:222 │ │ │ │ │ -_g_t_s_a_m_:_:_e_x_p_m │ │ │ │ │ -T expm(const Vector &x, int K=7) │ │ │ │ │ -Exponential map given exponential coordinates class T needs a wedge<> function │ │ │ │ │ -and a constructor from... │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:317 │ │ │ │ │ -_g_t_s_a_m_:_:_s_a_v_e │ │ │ │ │ -void save(const Matrix &A, const string &s, const string &filename) │ │ │ │ │ -save a matrix to file, which can be loaded by matlab │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:167 │ │ │ │ │ +_g_t_s_a_m_:_:_a_s_s_e_r_t___s_t_d_o_u_t___e_q_u_a_l │ │ │ │ │ +bool assert_stdout_equal(const std::string &expected, const V &actual) │ │ │ │ │ +Capture std out via cout stream and compare against string. │ │ │ │ │ +DDeeffiinniittiioonn TestableAssertions.h:359 │ │ │ │ │ +_g_t_s_a_m_:_:_a_s_s_e_r_t___c_o_n_t_a_i_n_e_r___e_q_u_a_l │ │ │ │ │ +bool assert_container_equal(const std::map< V1, V2 > &expected, const std::map< │ │ │ │ │ +V1, V2 > &actual, double tol=1e-9) │ │ │ │ │ +Function for comparing maps of testable->testable TODO: replace with more │ │ │ │ │ +generalized version. │ │ │ │ │ +DDeeffiinniittiioonn TestableAssertions.h:119 │ │ │ │ │ _g_t_s_a_m_:_:_a_s_s_e_r_t___e_q_u_a_l │ │ │ │ │ bool assert_equal(const Matrix &expected, const Matrix &actual, double tol) │ │ │ │ │ equals with an tolerance, prints out message if unequal │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:43 │ │ │ │ │ -_g_t_s_a_m_:_:_l_i_n_e_a_r___d_e_p_e_n_d_e_n_t │ │ │ │ │ -bool linear_dependent(const Matrix &A, const Matrix &B, double tol) │ │ │ │ │ -check whether the rows of two matrices are linear dependent │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:117 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_c_o_l_u_m_n │ │ │ │ │ -const MATRIX::ConstColXpr column(const MATRIX &A, size_t j) │ │ │ │ │ -Extracts a column view from a matrix that avoids a copy. │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:211 │ │ │ │ │ -_g_t_s_a_m_:_:_z_e_r_o_B_e_l_o_w_D_i_a_g_o_n_a_l │ │ │ │ │ -void zeroBelowDiagonal(MATRIX &A, size_t cols=0) │ │ │ │ │ -Zeros all of the elements below the diagonal of a matrix, in place. │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:232 │ │ │ │ │ -_g_t_s_a_m_:_:_w_e_i_g_h_t_e_d___e_l_i_m_i_n_a_t_e │ │ │ │ │ -list< boost::tuple< Vector, double, double > > weighted_eliminate(Matrix &A, │ │ │ │ │ -Vector &b, const Vector &sigmas) │ │ │ │ │ -Imperative algorithm for in-place full elimination with weights and constraint │ │ │ │ │ -handling. │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:273 │ │ │ │ │ -_g_t_s_a_m_:_:_b_a_c_k_S_u_b_s_t_i_t_u_t_e_U_p_p_e_r │ │ │ │ │ -Vector backSubstituteUpper(const Matrix &U, const Vector &b, bool unit) │ │ │ │ │ -backSubstitute U*x=b │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:377 │ │ │ │ │ +_g_t_s_a_m_:_:_a_s_s_e_r_t___c_o_n_t_a_i_n_e_r___e_q_u_a_l_i_t_y │ │ │ │ │ +bool assert_container_equality(const std::map< size_t, V2 > &expected, const │ │ │ │ │ +std::map< size_t, V2 > &actual) │ │ │ │ │ +Function for comparing maps of size_t->testable Types are assumed to have │ │ │ │ │ +operator ==. │ │ │ │ │ +DDeeffiinniittiioonn TestableAssertions.h:266 │ │ │ │ │ _g_t_s_a_m_:_:_a_s_s_e_r_t___i_n_e_q_u_a_l │ │ │ │ │ bool assert_inequal(const Matrix &A, const Matrix &B, double tol) │ │ │ │ │ inequals with an tolerance, prints out message if within tolerance │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:63 │ │ │ │ │ -_g_t_s_a_m_:_:_h_o_u_s_e_h_o_l_d_e_r │ │ │ │ │ -void householder(Matrix &A, size_t k) │ │ │ │ │ -Householder tranformation, zeros below diagonal. │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:354 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_>_> │ │ │ │ │ -istream & operator>>(istream &inputStream, Matrix &destinationMatrix) │ │ │ │ │ -Read a matrix from an input stream, such as a file. │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:174 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_p_l_a_c_e___Q_R │ │ │ │ │ -void inplace_QR(Matrix &A) │ │ │ │ │ -QR factorization using Eigen's internal block QR algorithm. │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:636 │ │ │ │ │ -_g_t_s_a_m_:_:_s_v_d │ │ │ │ │ -void svd(const Matrix &A, Matrix &U, Vector &S, Matrix &V) │ │ │ │ │ -SVD computes economy SVD A=U*S*V'. │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:560 │ │ │ │ │ -_g_t_s_a_m_:_:_s_k_e_w_S_y_m_m_e_t_r_i_c │ │ │ │ │ -Matrix3 skewSymmetric(double wx, double wy, double wz) │ │ │ │ │ -skew symmetric matrix returns this: 0 -wz wy wz 0 -wx -wy wx 0 │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:401 │ │ │ │ │ -_g_t_s_a_m_:_:_s_u_b │ │ │ │ │ -Eigen::Block< const MATRIX > sub(const MATRIX &A, size_t i1, size_t i2, size_t │ │ │ │ │ -j1, size_t j2) │ │ │ │ │ -extract submatrix, slice semantics, i.e. │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:181 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_n_s │ │ │ │ │ -Matrix trans(const Matrix &A) │ │ │ │ │ -static transpose function, just calls Eigen transpose member function │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:242 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_!_= │ │ │ │ │ -bool operator!=(const Matrix &A, const Matrix &B) │ │ │ │ │ -inequality │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:107 │ │ │ │ │ -_g_t_s_a_m_:_:_D_L_T │ │ │ │ │ -boost::tuple< int, double, Vector > DLT(const Matrix &A, double rank_tol) │ │ │ │ │ -Direct linear transform algorithm that calls svd to find a vector v that │ │ │ │ │ -minimizes the algebraic erro... │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:568 │ │ │ │ │ -_g_t_s_a_m_:_:_c_h_o_l_e_s_k_y___i_n_v_e_r_s_e │ │ │ │ │ -Matrix cholesky_inverse(const Matrix &A) │ │ │ │ │ -Return the inverse of a S.P.D. │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:539 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_o_d │ │ │ │ │ -MATRIX prod(const MATRIX &A, const MATRIX &B) │ │ │ │ │ -products using old-style format to improve compatibility │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:144 │ │ │ │ │ -_g_t_s_a_m_:_:_h_o_u_s_e_h_o_l_d_e_r__ │ │ │ │ │ -void householder_(Matrix &A, size_t k, bool copy_vectors) │ │ │ │ │ -Imperative version of Householder QR factorization, Golub & Van Loan p 224 │ │ │ │ │ -version with Householder v... │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:327 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_s_e_r_t_S_u_b │ │ │ │ │ -void insertSub(Eigen::MatrixBase< Derived1 > &fullMatrix, const Eigen:: │ │ │ │ │ -MatrixBase< Derived2 > &subMatrix, size_t i, size_t j) │ │ │ │ │ -insert a submatrix IN PLACE at a specified location in a larger matrix NOTE: │ │ │ │ │ -there is no size checkin... │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:195 │ │ │ │ │ -_g_t_s_a_m_:_:_c_o_l_l_e_c_t │ │ │ │ │ -Matrix collect(const std::vector< const Matrix * > &matrices, size_t m, size_t │ │ │ │ │ -n) │ │ │ │ │ -create a matrix by concatenating Given a set of matrices: A1, A2, A3... If all │ │ │ │ │ -matrices have the same... │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:443 │ │ │ │ │ -_g_t_s_a_m_:_:_l_i_n_e_a_r___i_n_d_e_p_e_n_d_e_n_t │ │ │ │ │ -bool linear_independent(const Matrix &A, const Matrix &B, double tol) │ │ │ │ │ -check whether the rows of two matrices are linear independent │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:103 │ │ │ │ │ -_g_t_s_a_m_:_:_f_p_E_q_u_a_l │ │ │ │ │ -bool fpEqual(double a, double b, double tol, bool check_relative_also) │ │ │ │ │ -Ensure we are not including a different version of Eigen in user code than │ │ │ │ │ -while compiling gtsam,... │ │ │ │ │ -DDeeffiinniittiioonn Vector.cpp:42 │ │ │ │ │ -_g_t_s_a_m_:_:_q_r │ │ │ │ │ -pair< Matrix, Matrix > qr(const Matrix &A) │ │ │ │ │ -Householder QR factorization, Golub & Van Loan p 224, explicit version │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:235 │ │ │ │ │ -_g_t_s_a_m_:_:_d_i_a_g │ │ │ │ │ -Matrix diag(const std::vector< Matrix > &Hs) │ │ │ │ │ -Create a matrix with submatrices along its diagonal. │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:207 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l___w_i_t_h___a_b_s___t_o_l │ │ │ │ │ -bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen:: │ │ │ │ │ -DenseBase< MATRIX > &B, double tol=1e-9) │ │ │ │ │ -equals with a tolerance │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:81 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_=_= │ │ │ │ │ -bool operator==(const Matrix &A, const Matrix &B) │ │ │ │ │ -equality is just equal_with_abs_tol 1e-9 │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_v_e_r_s_e___s_q_u_a_r_e___r_o_o_t │ │ │ │ │ -Matrix inverse_square_root(const Matrix &A) │ │ │ │ │ -Use Cholesky to calculate inverse square root of a matrix. │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:552 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_s_h_a_p_e │ │ │ │ │ -Reshape functor. │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:246 │ │ │ │ │ -_g_t_s_a_m_:_:_M_u_l_t_i_p_l_y_W_i_t_h_I_n_v_e_r_s_e │ │ │ │ │ -Functor that implements multiplication of a vector b with the inverse of a │ │ │ │ │ -matrix A. │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:456 │ │ │ │ │ -_g_t_s_a_m_:_:_M_u_l_t_i_p_l_y_W_i_t_h_I_n_v_e_r_s_e_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ -VectorN operator()(const MatrixN &A, const VectorN &b, OptionalJacobian< N, N │ │ │ │ │ -*N > H1=boost::none, OptionalJacobian< N, N > H2=boost::none) const │ │ │ │ │ -A.inverse() * b, with optional derivatives. │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:461 │ │ │ │ │ -_g_t_s_a_m_:_:_M_u_l_t_i_p_l_y_W_i_t_h_I_n_v_e_r_s_e_F_u_n_c_t_i_o_n │ │ │ │ │ -Functor that implements multiplication with the inverse of a matrix, itself the │ │ │ │ │ -result of a function ... │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:482 │ │ │ │ │ -_g_t_s_a_m_:_:_M_u_l_t_i_p_l_y_W_i_t_h_I_n_v_e_r_s_e_F_u_n_c_t_i_o_n_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ -VectorN operator()(const T &a, const VectorN &b, OptionalJacobian< N, M > │ │ │ │ │ -H1=boost::none, OptionalJacobian< N, N > H2=boost::none) const │ │ │ │ │ -f(a).inverse() * b, with optional derivatives │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:497 │ │ │ │ │ -_g_t_s_a_m_:_:_M_u_l_t_i_p_l_y_W_i_t_h_I_n_v_e_r_s_e_F_u_n_c_t_i_o_n_:_:_M_u_l_t_i_p_l_y_W_i_t_h_I_n_v_e_r_s_e_F_u_n_c_t_i_o_n │ │ │ │ │ -MultiplyWithInverseFunction(const Operator &phi) │ │ │ │ │ -Construct with function as explained above. │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:494 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ -either a fixed size o... │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_a_s_s_e_r_t___p_r_i_n_t___e_q_u_a_l │ │ │ │ │ +bool assert_print_equal(const std::string &expected, const V &actual, const │ │ │ │ │ +std::string &s="") │ │ │ │ │ +Capture print function output and compare against string. │ │ │ │ │ +DDeeffiinniittiioonn TestableAssertions.h:381 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _M_a_t_r_i_x_._h │ │ │ │ │ + * _T_e_s_t_a_b_l_e_A_s_s_e_r_t_i_o_n_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00152.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Chebyshev.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Chebyshev2.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,48 +94,36 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
Chebyshev.h File Reference
│ │ │ │ +
Chebyshev2.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Chebyshev basis decompositions. │ │ │ │ +

Chebyshev parameterizations on Chebyshev points of second kind. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

struct  gtsam::Chebyshev1Basis
 Basis of Chebyshev polynomials of the first kind https://en.wikipedia.org/wiki/Chebyshev_polynomials#First_kind These are typically denoted with the symbol T_n, where n is the degree. More...
 
struct  gtsam::Chebyshev2Basis
 Basis of Chebyshev polynomials of the second kind. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Chebyshev basis decompositions.

│ │ │ │ +

Chebyshev parameterizations on Chebyshev points of second kind.

│ │ │ │
Author
Varun Agrawal, Jing Dong, Frank Dellaert
│ │ │ │
Date
July 4, 2020
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,32 +1,22 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Chebyshev.h File Reference │ │ │ │ │ -Chebyshev basis decompositions. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_C_h_e_b_y_s_h_e_v_1_B_a_s_i_s │ │ │ │ │ - _B_a_s_i_s of Chebyshev polynomials of the first kind _h_t_t_p_s_:_/_/ │ │ │ │ │ -  _e_n_._w_i_k_i_p_e_d_i_a_._o_r_g_/_w_i_k_i_/_C_h_e_b_y_s_h_e_v___p_o_l_y_n_o_m_i_a_l_s_#_F_i_r_s_t___k_i_n_d These are │ │ │ │ │ - typically denoted with the symbol T_n, where n is the degree. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_C_h_e_b_y_s_h_e_v_2_B_a_s_i_s │ │ │ │ │ -  _B_a_s_i_s of Chebyshev polynomials of the second kind. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +Chebyshev2.cpp File Reference │ │ │ │ │ +Chebyshev parameterizations on Chebyshev points of second kind. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Chebyshev basis decompositions. │ │ │ │ │ +Chebyshev parameterizations on Chebyshev points of second kind. │ │ │ │ │ Author │ │ │ │ │ Varun Agrawal, Jing Dong, Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ July 4, 2020 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_i_s │ │ │ │ │ - * _C_h_e_b_y_s_h_e_v_._h │ │ │ │ │ + * _C_h_e_b_y_s_h_e_v_2_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00155.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/ParameterMatrix.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Chebyshev.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,54 +95,47 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
ParameterMatrix.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
Chebyshev.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Define ParameterMatrix class which is used to store values at interpolation points. │ │ │ │ +

Chebyshev basis decompositions. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::ParameterMatrix< M >
 A matrix abstraction of MxN values at the Basis points. More...
struct  gtsam::Chebyshev1Basis
 Basis of Chebyshev polynomials of the first kind https://en.wikipedia.org/wiki/Chebyshev_polynomials#First_kind These are typically denoted with the symbol T_n, where n is the degree. More...
 
struct  gtsam::traits< ParameterMatrix< M > >
struct  gtsam::Chebyshev2Basis
 Basis of Chebyshev polynomials of the second kind. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -template<int M>
std::ostream & gtsam::operator<< (std::ostream &os, const ParameterMatrix< M > &parameterMatrix)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Define ParameterMatrix class which is used to store values at interpolation points.

│ │ │ │ -
Author
Varun Agrawal, Frank Dellaert
│ │ │ │ -
Date
September 21, 2020
│ │ │ │ +

Chebyshev basis decompositions.

│ │ │ │ +
Author
Varun Agrawal, Jing Dong, Frank Dellaert
│ │ │ │ +
Date
July 4, 2020
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,36 +1,32 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -ParameterMatrix.h File Reference │ │ │ │ │ -Define ParameterMatrix class which is used to store values at interpolation │ │ │ │ │ -points. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +Chebyshev.h File Reference │ │ │ │ │ +Chebyshev basis decompositions. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_ _M_ _> │ │ │ │ │ -  A matrix abstraction of MxN values at the _B_a_s_i_s points. _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_C_h_e_b_y_s_h_e_v_1_B_a_s_i_s │ │ │ │ │ + _B_a_s_i_s of Chebyshev polynomials of the first kind _h_t_t_p_s_:_/_/ │ │ │ │ │ +  _e_n_._w_i_k_i_p_e_d_i_a_._o_r_g_/_w_i_k_i_/_C_h_e_b_y_s_h_e_v___p_o_l_y_n_o_m_i_a_l_s_#_F_i_r_s_t___k_i_n_d These are │ │ │ │ │ + typically denoted with the symbol T_n, where n is the degree. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_ _M_ _>_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_C_h_e_b_y_s_h_e_v_2_B_a_s_i_s │ │ │ │ │ +  _B_a_s_i_s of Chebyshev polynomials of the second kind. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ -std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x< M > │ │ │ │ │ - ¶meterMatrix) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Define ParameterMatrix class which is used to store values at interpolation │ │ │ │ │ -points. │ │ │ │ │ +Chebyshev basis decompositions. │ │ │ │ │ Author │ │ │ │ │ - Varun Agrawal, Frank Dellaert │ │ │ │ │ + Varun Agrawal, Jing Dong, Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - September 21, 2020 │ │ │ │ │ + July 4, 2020 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_i_s │ │ │ │ │ - * _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_._h │ │ │ │ │ + * _C_h_e_b_y_s_h_e_v_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00155.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a00155 = [ │ │ │ │ │ - ["gtsam::ParameterMatrix< M >", "a02752.html", "a02752"], │ │ │ │ │ - ["gtsam::traits< ParameterMatrix< M > >", "a02756.html", null] │ │ │ │ │ + ["gtsam::Chebyshev1Basis", "a02732.html", null], │ │ │ │ │ + ["gtsam::Chebyshev2Basis", "a02736.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00155_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/ParameterMatrix.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Chebyshev.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,208 +98,72 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ParameterMatrix.h
│ │ │ │ +
Chebyshev.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/base/Matrix.h>
│ │ │ │ -
23#include <gtsam/base/Testable.h>
│ │ │ │ -
24#include <gtsam/base/VectorSpace.h>
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/base/Manifold.h>
│ │ │ │ +
22#include <gtsam/basis/Basis.h>
│ │ │ │ +
23
│ │ │ │ +
24namespace gtsam {
│ │ │ │
25
│ │ │ │ -
26#include <iostream>
│ │ │ │ -
27
│ │ │ │ -
28namespace gtsam {
│ │ │ │ -
29
│ │ │ │ -
37template <int M>
│ │ │ │ -
│ │ │ │ -
38class ParameterMatrix {
│ │ │ │ -
39 using MatrixType = Eigen::Matrix<double, M, -1>;
│ │ │ │ -
40
│ │ │ │ -
41 private:
│ │ │ │ -
42 MatrixType matrix_;
│ │ │ │ -
43
│ │ │ │ -
44 public:
│ │ │ │ -
45 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
46
│ │ │ │ -
47 enum { dimension = Eigen::Dynamic };
│ │ │ │ -
48
│ │ │ │ -
53 ParameterMatrix(const size_t N) : matrix_(M, N) { matrix_.setZero(); }
│ │ │ │ -
54
│ │ │ │ -
59 ParameterMatrix(const MatrixType& matrix) : matrix_(matrix) {}
│ │ │ │ -
60
│ │ │ │ -
62 size_t rows() const { return matrix_.rows(); }
│ │ │ │ -
63
│ │ │ │ -
65 size_t cols() const { return matrix_.cols(); }
│ │ │ │ -
66
│ │ │ │ -
68 MatrixType matrix() const { return matrix_; }
│ │ │ │ -
69
│ │ │ │ -
71 Eigen::Matrix<double, -1, M> transpose() const { return matrix_.transpose(); }
│ │ │ │ -
72
│ │ │ │ -
│ │ │ │ -
77 Eigen::Matrix<double, 1, -1> row(size_t index) const {
│ │ │ │ -
78 return matrix_.row(index);
│ │ │ │ -
79 }
│ │ │ │ -
│ │ │ │ -
80
│ │ │ │ -
│ │ │ │ -
85 auto row(size_t index) -> Eigen::Block<MatrixType, 1, -1, false> {
│ │ │ │ -
86 return matrix_.row(index);
│ │ │ │ -
87 }
│ │ │ │ -
│ │ │ │ -
88
│ │ │ │ -
│ │ │ │ -
93 Eigen::Matrix<double, M, 1> col(size_t index) const {
│ │ │ │ -
94 return matrix_.col(index);
│ │ │ │ -
95 }
│ │ │ │ -
│ │ │ │ -
96
│ │ │ │ -
│ │ │ │ -
101 auto col(size_t index) -> Eigen::Block<MatrixType, M, 1, true> {
│ │ │ │ -
102 return matrix_.col(index);
│ │ │ │ -
103 }
│ │ │ │ -
│ │ │ │ -
104
│ │ │ │ -
108 void setZero() { matrix_.setZero(); }
│ │ │ │ -
109
│ │ │ │ -
│ │ │ │ -
114 ParameterMatrix<M> operator+(const ParameterMatrix<M>& other) const {
│ │ │ │ -
115 return ParameterMatrix<M>(matrix_ + other.matrix());
│ │ │ │ -
116 }
│ │ │ │ -
│ │ │ │ -
117
│ │ │ │ -
│ │ │ │ -
122 ParameterMatrix<M> operator+(
│ │ │ │ -
123 const Eigen::Matrix<double, -1, 1>& other) const {
│ │ │ │ -
124 // This form avoids a deep copy and instead typecasts `other`.
│ │ │ │ -
125 Eigen::Map<const MatrixType> other_(other.data(), M, cols());
│ │ │ │ -
126 return ParameterMatrix<M>(matrix_ + other_);
│ │ │ │ -
127 }
│ │ │ │ -
│ │ │ │ -
128
│ │ │ │ -
│ │ │ │ -
133 ParameterMatrix<M> operator-(const ParameterMatrix<M>& other) const {
│ │ │ │ -
134 return ParameterMatrix<M>(matrix_ - other.matrix());
│ │ │ │ -
135 }
│ │ │ │ -
│ │ │ │ -
136
│ │ │ │ -
│ │ │ │ -
141 ParameterMatrix<M> operator-(
│ │ │ │ -
142 const Eigen::Matrix<double, -1, 1>& other) const {
│ │ │ │ -
143 Eigen::Map<const MatrixType> other_(other.data(), M, cols());
│ │ │ │ -
144 return ParameterMatrix<M>(matrix_ - other_);
│ │ │ │ -
145 }
│ │ │ │ +
│ │ │ │ +
32struct GTSAM_EXPORT Chebyshev1Basis : Basis<Chebyshev1Basis> {
│ │ │ │ +
33 using Parameters = Eigen::Matrix<double, -1, 1 /*Nx1*/>;
│ │ │ │ +
34
│ │ │ │ +
35 Parameters parameters_;
│ │ │ │ +
36
│ │ │ │ +
45 static Weights CalculateWeights(size_t N, double x, double a = -1,
│ │ │ │ +
46 double b = 1);
│ │ │ │ +
47
│ │ │ │ +
66 static Weights DerivativeWeights(size_t N, double x, double a = -1,
│ │ │ │ +
67 double b = 1);
│ │ │ │ +
68}; // Chebyshev1Basis
│ │ │ │
│ │ │ │ -
146
│ │ │ │ -
│ │ │ │ -
152 MatrixType operator*(const Eigen::Matrix<double, -1, -1>& other) const {
│ │ │ │ -
153 return matrix_ * other;
│ │ │ │ -
154 }
│ │ │ │ -
│ │ │ │ -
155
│ │ │ │ -
158
│ │ │ │ -
│ │ │ │ -
163 void print(const std::string& s = "") const {
│ │ │ │ -
164 std::cout << (s == "" ? s : s + " ") << matrix_ << std::endl;
│ │ │ │ -
165 }
│ │ │ │ -
│ │ │ │ -
166
│ │ │ │ -
│ │ │ │ -
172 bool equals(const ParameterMatrix<M>& other, double tol = 1e-8) const {
│ │ │ │ -
173 return gtsam::equal_with_abs_tol(matrix_, other.matrix(), tol);
│ │ │ │ -
174 }
│ │ │ │ -
│ │ │ │ -
175
│ │ │ │ -
177 inline size_t dim() const { return matrix_.size(); }
│ │ │ │ -
178
│ │ │ │ -
│ │ │ │ -
180 inline Vector vector() const {
│ │ │ │ -
181 using RowMajor = Eigen::Matrix<double, -1, -1, Eigen::RowMajor>;
│ │ │ │ -
182 Vector result(matrix_.size());
│ │ │ │ -
183 Eigen::Map<RowMajor>(&result(0), rows(), cols()) = matrix_;
│ │ │ │ -
184 return result;
│ │ │ │ -
185 }
│ │ │ │ -
│ │ │ │ -
186
│ │ │ │ -
│ │ │ │ -
192 inline static ParameterMatrix Identity() {
│ │ │ │ -
193 // throw std::runtime_error(
│ │ │ │ -
194 // "ParameterMatrix::Identity(): Don't use this function");
│ │ │ │ -
195 return ParameterMatrix(0);
│ │ │ │ -
196 }
│ │ │ │ -
│ │ │ │ -
197
│ │ │ │ -
199};
│ │ │ │ -
│ │ │ │ -
200
│ │ │ │ -
201// traits for ParameterMatrix
│ │ │ │ -
202template <int M>
│ │ │ │ -
│ │ │ │ -
203struct traits<ParameterMatrix<M>>
│ │ │ │ -
204 : public internal::VectorSpace<ParameterMatrix<M>> {};
│ │ │ │ -
│ │ │ │ -
205
│ │ │ │ -
206/* ************************************************************************* */
│ │ │ │ -
207// Stream operator that takes a ParameterMatrix. Used for printing.
│ │ │ │ -
208template <int M>
│ │ │ │ -
209inline std::ostream& operator<<(std::ostream& os,
│ │ │ │ -
210 const ParameterMatrix<M>& parameterMatrix) {
│ │ │ │ -
211 os << parameterMatrix.matrix();
│ │ │ │ -
212 return os;
│ │ │ │ -
213}
│ │ │ │ -
214
│ │ │ │ -
215} // namespace gtsam
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ -
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
69
│ │ │ │ +
│ │ │ │ +
80struct GTSAM_EXPORT Chebyshev2Basis : Basis<Chebyshev2Basis> {
│ │ │ │ +
81 using Parameters = Eigen::Matrix<double, -1, 1 /*Nx1*/>;
│ │ │ │ +
82
│ │ │ │ +
91 static Weights CalculateWeights(size_t N, double x, double a = -1,
│ │ │ │ +
92 double b = 1);
│ │ │ │ +
93
│ │ │ │ +
103 static Weights DerivativeWeights(size_t N, double x, double a = -1,
│ │ │ │ +
104 double b = 1);
│ │ │ │ +
105}; // Chebyshev2Basis
│ │ │ │ +
│ │ │ │ +
106
│ │ │ │ +
107} // namespace gtsam
│ │ │ │ +
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ +
Basis.h
Compute an interpolating basis.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition Matrix.h:81
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::internal::VectorSpace
VectorSpace provides both Testable and VectorSpaceTraits.
Definition VectorSpace.h:207
│ │ │ │ -
gtsam::ParameterMatrix
A matrix abstraction of MxN values at the Basis points.
Definition ParameterMatrix.h:38
│ │ │ │ -
gtsam::ParameterMatrix::operator*
MatrixType operator*(const Eigen::Matrix< double, -1, -1 > &other) const
Multiply ParameterMatrix with an Eigen matrix.
Definition ParameterMatrix.h:152
│ │ │ │ -
gtsam::ParameterMatrix::operator-
ParameterMatrix< M > operator-(const Eigen::Matrix< double, -1, 1 > &other) const
Subtract a MxN-sized vector from the ParameterMatrix.
Definition ParameterMatrix.h:141
│ │ │ │ -
gtsam::ParameterMatrix::row
Eigen::Matrix< double, 1, -1 > row(size_t index) const
Get the matrix row specified by index.
Definition ParameterMatrix.h:77
│ │ │ │ -
gtsam::ParameterMatrix::ParameterMatrix
ParameterMatrix(const size_t N)
Create ParameterMatrix using the number of basis points.
Definition ParameterMatrix.h:53
│ │ │ │ -
gtsam::ParameterMatrix::Identity
static ParameterMatrix Identity()
Identity function to satisfy VectorSpace traits.
Definition ParameterMatrix.h:192
│ │ │ │ -
gtsam::ParameterMatrix::setZero
void setZero()
Set all matrix coefficients to zero.
Definition ParameterMatrix.h:108
│ │ │ │ -
gtsam::ParameterMatrix::matrix
MatrixType matrix() const
Get the underlying matrix.
Definition ParameterMatrix.h:68
│ │ │ │ -
gtsam::ParameterMatrix::ParameterMatrix
ParameterMatrix(const MatrixType &matrix)
Create ParameterMatrix from an MxN Eigen Matrix.
Definition ParameterMatrix.h:59
│ │ │ │ -
gtsam::ParameterMatrix::col
Eigen::Matrix< double, M, 1 > col(size_t index) const
Get the matrix column specified by index.
Definition ParameterMatrix.h:93
│ │ │ │ -
gtsam::ParameterMatrix::vector
Vector vector() const
Convert to vector form, is done row-wise.
Definition ParameterMatrix.h:180
│ │ │ │ -
gtsam::ParameterMatrix::equals
bool equals(const ParameterMatrix< M > &other, double tol=1e-8) const
Check for equality up to absolute tolerance.
Definition ParameterMatrix.h:172
│ │ │ │ -
gtsam::ParameterMatrix::operator+
ParameterMatrix< M > operator+(const ParameterMatrix< M > &other) const
Add a ParameterMatrix to another.
Definition ParameterMatrix.h:114
│ │ │ │ -
gtsam::ParameterMatrix::rows
size_t rows() const
Get the number of rows.
Definition ParameterMatrix.h:62
│ │ │ │ -
gtsam::ParameterMatrix::row
auto row(size_t index) -> Eigen::Block< MatrixType, 1, -1, false >
Set the matrix row specified by index.
Definition ParameterMatrix.h:85
│ │ │ │ -
gtsam::ParameterMatrix::print
void print(const std::string &s="") const
Print the ParameterMatrix.
Definition ParameterMatrix.h:163
│ │ │ │ -
gtsam::ParameterMatrix::col
auto col(size_t index) -> Eigen::Block< MatrixType, M, 1, true >
Set the matrix column specified by index.
Definition ParameterMatrix.h:101
│ │ │ │ -
gtsam::ParameterMatrix::operator+
ParameterMatrix< M > operator+(const Eigen::Matrix< double, -1, 1 > &other) const
Add a MxN-sized vector to the ParameterMatrix.
Definition ParameterMatrix.h:122
│ │ │ │ -
gtsam::ParameterMatrix::operator-
ParameterMatrix< M > operator-(const ParameterMatrix< M > &other) const
Subtract a ParameterMatrix from another.
Definition ParameterMatrix.h:133
│ │ │ │ -
gtsam::ParameterMatrix::dim
size_t dim() const
Returns dimensionality of the tangent space.
Definition ParameterMatrix.h:177
│ │ │ │ -
gtsam::ParameterMatrix::cols
size_t cols() const
Get the number of columns.
Definition ParameterMatrix.h:65
│ │ │ │ -
gtsam::ParameterMatrix::transpose
Eigen::Matrix< double, -1, M > transpose() const
Return the tranpose of the underlying matrix.
Definition ParameterMatrix.h:71
│ │ │ │ +
gtsam::Basis
CRTP Base class for function bases.
Definition Basis.h:100
│ │ │ │ +
gtsam::Chebyshev1Basis
Basis of Chebyshev polynomials of the first kind https://en.wikipedia.org/wiki/Chebyshev_polynomials#...
Definition Chebyshev.h:32
│ │ │ │ +
gtsam::Chebyshev2Basis
Basis of Chebyshev polynomials of the second kind.
Definition Chebyshev.h:80
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,252 +1,71 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ParameterMatrix.h │ │ │ │ │ +Chebyshev.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ -24#include │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_i_s_/_B_a_s_i_s_._h> │ │ │ │ │ +23 │ │ │ │ │ +24namespace _g_t_s_a_m { │ │ │ │ │ 25 │ │ │ │ │ -26#include │ │ │ │ │ -27 │ │ │ │ │ -28namespace _g_t_s_a_m { │ │ │ │ │ -29 │ │ │ │ │ -37template │ │ │ │ │ -_3_8class _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x { │ │ │ │ │ -39 using MatrixType = Eigen::Matrix; │ │ │ │ │ -40 │ │ │ │ │ -41 private: │ │ │ │ │ -42 MatrixType matrix_; │ │ │ │ │ -43 │ │ │ │ │ -44 public: │ │ │ │ │ -45 EIGEN_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ -46 │ │ │ │ │ -47 enum { dimension = Eigen::Dynamic }; │ │ │ │ │ -48 │ │ │ │ │ -_5_3 _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x(const size_t N) : matrix_(M, N) { matrix_.setZero(); } │ │ │ │ │ -54 │ │ │ │ │ -_5_9 _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x(const MatrixType& _m_a_t_r_i_x) : matrix_(_m_a_t_r_i_x) {} │ │ │ │ │ -60 │ │ │ │ │ -_6_2 size_t _r_o_w_s() const { return matrix_.rows(); } │ │ │ │ │ -63 │ │ │ │ │ -_6_5 size_t _c_o_l_s() const { return matrix_.cols(); } │ │ │ │ │ -66 │ │ │ │ │ -_6_8 MatrixType _m_a_t_r_i_x() const { return matrix_; } │ │ │ │ │ +_3_2struct GTSAM_EXPORT _C_h_e_b_y_s_h_e_v_1_B_a_s_i_s : _B_a_s_i_s { │ │ │ │ │ +33 using Parameters = Eigen::Matrix; │ │ │ │ │ +34 │ │ │ │ │ +35 Parameters parameters_; │ │ │ │ │ +36 │ │ │ │ │ +45 static Weights CalculateWeights(size_t N, double x, double a = -1, │ │ │ │ │ +46 double b = 1); │ │ │ │ │ +47 │ │ │ │ │ +66 static Weights DerivativeWeights(size_t N, double x, double a = -1, │ │ │ │ │ +67 double b = 1); │ │ │ │ │ +68}; // Chebyshev1Basis │ │ │ │ │ 69 │ │ │ │ │ -_7_1 Eigen::Matrix _t_r_a_n_s_p_o_s_e() const { return matrix_.transpose(); │ │ │ │ │ -} │ │ │ │ │ -72 │ │ │ │ │ -_7_7 Eigen::Matrix _r_o_w(size_t index) const { │ │ │ │ │ -78 return matrix_.row(index); │ │ │ │ │ -79 } │ │ │ │ │ -80 │ │ │ │ │ -_8_5 auto _r_o_w(size_t index) -> Eigen::Block { │ │ │ │ │ -86 return matrix_.row(index); │ │ │ │ │ -87 } │ │ │ │ │ -88 │ │ │ │ │ -_9_3 Eigen::Matrix _c_o_l(size_t index) const { │ │ │ │ │ -94 return matrix_.col(index); │ │ │ │ │ -95 } │ │ │ │ │ -96 │ │ │ │ │ -_1_0_1 auto _c_o_l(size_t index) -> Eigen::Block { │ │ │ │ │ -102 return matrix_.col(index); │ │ │ │ │ -103 } │ │ │ │ │ -104 │ │ │ │ │ -_1_0_8 void _s_e_t_Z_e_r_o() { matrix_.setZero(); } │ │ │ │ │ -109 │ │ │ │ │ -_1_1_4 _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_> _o_p_e_r_a_t_o_r_+(const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>& other) const { │ │ │ │ │ -115 return _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>(matrix_ + other._m_a_t_r_i_x()); │ │ │ │ │ -116 } │ │ │ │ │ -117 │ │ │ │ │ -_1_2_2 _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_> _o_p_e_r_a_t_o_r_+( │ │ │ │ │ -123 const Eigen::Matrix& other) const { │ │ │ │ │ -124 // This form avoids a deep copy and instead typecasts `other`. │ │ │ │ │ -125 Eigen::Map other_(other.data(), M, _c_o_l_s()); │ │ │ │ │ -126 return _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>(matrix_ + other_); │ │ │ │ │ -127 } │ │ │ │ │ -128 │ │ │ │ │ -_1_3_3 _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_> _o_p_e_r_a_t_o_r_-(const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>& other) const { │ │ │ │ │ -134 return _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>(matrix_ - other._m_a_t_r_i_x()); │ │ │ │ │ -135 } │ │ │ │ │ -136 │ │ │ │ │ -_1_4_1 _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_> _o_p_e_r_a_t_o_r_-( │ │ │ │ │ -142 const Eigen::Matrix& other) const { │ │ │ │ │ -143 Eigen::Map other_(other.data(), M, _c_o_l_s()); │ │ │ │ │ -144 return _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>(matrix_ - other_); │ │ │ │ │ -145 } │ │ │ │ │ -146 │ │ │ │ │ -_1_5_2 MatrixType _o_p_e_r_a_t_o_r_*(const Eigen::Matrix& other) const { │ │ │ │ │ -153 return matrix_ * other; │ │ │ │ │ -154 } │ │ │ │ │ -155 │ │ │ │ │ -158 │ │ │ │ │ -_1_6_3 void _p_r_i_n_t(const std::string& s = "") const { │ │ │ │ │ -164 std::cout << (s == "" ? s : s + " ") << matrix_ << std::endl; │ │ │ │ │ -165 } │ │ │ │ │ -166 │ │ │ │ │ -_1_7_2 bool _e_q_u_a_l_s(const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>& other, double tol = 1e-8) const { │ │ │ │ │ -173 return _g_t_s_a_m_:_:_e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(matrix_, other._m_a_t_r_i_x(), tol); │ │ │ │ │ -174 } │ │ │ │ │ -175 │ │ │ │ │ -_1_7_7 inline size_t _d_i_m() const { return matrix_.size(); } │ │ │ │ │ -178 │ │ │ │ │ -_1_8_0 inline Vector _v_e_c_t_o_r() const { │ │ │ │ │ -181 using RowMajor = Eigen::Matrix; │ │ │ │ │ -182 Vector result(matrix_.size()); │ │ │ │ │ -183 Eigen::Map(&result(0), _r_o_w_s(), _c_o_l_s()) = matrix_; │ │ │ │ │ -184 return result; │ │ │ │ │ -185 } │ │ │ │ │ -186 │ │ │ │ │ -_1_9_2 inline static _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x _I_d_e_n_t_i_t_y() { │ │ │ │ │ -193 // throw std::runtime_error( │ │ │ │ │ -194 // "ParameterMatrix::Identity(): Don't use this function"); │ │ │ │ │ -195 return _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x(0); │ │ │ │ │ -196 } │ │ │ │ │ -197 │ │ │ │ │ -199}; │ │ │ │ │ -200 │ │ │ │ │ -201// traits for ParameterMatrix │ │ │ │ │ -202template │ │ │ │ │ -_2_0_3struct _t_r_a_i_t_s<_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x> │ │ │ │ │ -204 : public _i_n_t_e_r_n_a_l_:_:_V_e_c_t_o_r_S_p_a_c_e> {}; │ │ │ │ │ -205 │ │ │ │ │ -206/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -207// Stream operator that takes a ParameterMatrix. Used for printing. │ │ │ │ │ -208template │ │ │ │ │ -209inline std::ostream& operator<<(std::ostream& os, │ │ │ │ │ -210 const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>& parameterMatrix) { │ │ │ │ │ -211 os << parameterMatrix._m_a_t_r_i_x(); │ │ │ │ │ -212 return os; │ │ │ │ │ -213} │ │ │ │ │ -214 │ │ │ │ │ -215} // namespace gtsam │ │ │ │ │ -_T_e_s_t_a_b_l_e_._h │ │ │ │ │ -Concept check for values that can be used in unit tests. │ │ │ │ │ -_M_a_t_r_i_x_._h │ │ │ │ │ -typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +_8_0struct GTSAM_EXPORT _C_h_e_b_y_s_h_e_v_2_B_a_s_i_s : _B_a_s_i_s { │ │ │ │ │ +81 using Parameters = Eigen::Matrix; │ │ │ │ │ +82 │ │ │ │ │ +91 static Weights CalculateWeights(size_t N, double x, double a = -1, │ │ │ │ │ +92 double b = 1); │ │ │ │ │ +93 │ │ │ │ │ +103 static Weights DerivativeWeights(size_t N, double x, double a = -1, │ │ │ │ │ +104 double b = 1); │ │ │ │ │ +105}; // Chebyshev2Basis │ │ │ │ │ +106 │ │ │ │ │ +107} // namespace gtsam │ │ │ │ │ +_M_a_n_i_f_o_l_d_._h │ │ │ │ │ +Base class and basic functions for Manifold types. │ │ │ │ │ +_B_a_s_i_s_._h │ │ │ │ │ +Compute an interpolating basis. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l___w_i_t_h___a_b_s___t_o_l │ │ │ │ │ -bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen:: │ │ │ │ │ -DenseBase< MATRIX > &B, double tol=1e-9) │ │ │ │ │ -equals with a tolerance │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:81 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_V_e_c_t_o_r_S_p_a_c_e │ │ │ │ │ -VectorSpace provides both Testable and VectorSpaceTraits. │ │ │ │ │ -DDeeffiinniittiioonn VectorSpace.h:207 │ │ │ │ │ -_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x │ │ │ │ │ -A matrix abstraction of MxN values at the Basis points. │ │ │ │ │ -DDeeffiinniittiioonn ParameterMatrix.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ -MatrixType operator*(const Eigen::Matrix< double, -1, -1 > &other) const │ │ │ │ │ -Multiply ParameterMatrix with an Eigen matrix. │ │ │ │ │ -DDeeffiinniittiioonn ParameterMatrix.h:152 │ │ │ │ │ -_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_o_p_e_r_a_t_o_r_- │ │ │ │ │ -ParameterMatrix< M > operator-(const Eigen::Matrix< double, -1, 1 > &other) │ │ │ │ │ -const │ │ │ │ │ -Subtract a MxN-sized vector from the ParameterMatrix. │ │ │ │ │ -DDeeffiinniittiioonn ParameterMatrix.h:141 │ │ │ │ │ -_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_r_o_w │ │ │ │ │ -Eigen::Matrix< double, 1, -1 > row(size_t index) const │ │ │ │ │ -Get the matrix row specified by index. │ │ │ │ │ -DDeeffiinniittiioonn ParameterMatrix.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x │ │ │ │ │ -ParameterMatrix(const size_t N) │ │ │ │ │ -Create ParameterMatrix using the number of basis points. │ │ │ │ │ -DDeeffiinniittiioonn ParameterMatrix.h:53 │ │ │ │ │ -_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_I_d_e_n_t_i_t_y │ │ │ │ │ -static ParameterMatrix Identity() │ │ │ │ │ -Identity function to satisfy VectorSpace traits. │ │ │ │ │ -DDeeffiinniittiioonn ParameterMatrix.h:192 │ │ │ │ │ -_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_s_e_t_Z_e_r_o │ │ │ │ │ -void setZero() │ │ │ │ │ -Set all matrix coefficients to zero. │ │ │ │ │ -DDeeffiinniittiioonn ParameterMatrix.h:108 │ │ │ │ │ -_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_m_a_t_r_i_x │ │ │ │ │ -MatrixType matrix() const │ │ │ │ │ -Get the underlying matrix. │ │ │ │ │ -DDeeffiinniittiioonn ParameterMatrix.h:68 │ │ │ │ │ -_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x │ │ │ │ │ -ParameterMatrix(const MatrixType &matrix) │ │ │ │ │ -Create ParameterMatrix from an MxN Eigen Matrix. │ │ │ │ │ -DDeeffiinniittiioonn ParameterMatrix.h:59 │ │ │ │ │ -_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_c_o_l │ │ │ │ │ -Eigen::Matrix< double, M, 1 > col(size_t index) const │ │ │ │ │ -Get the matrix column specified by index. │ │ │ │ │ -DDeeffiinniittiioonn ParameterMatrix.h:93 │ │ │ │ │ -_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_v_e_c_t_o_r │ │ │ │ │ -Vector vector() const │ │ │ │ │ -Convert to vector form, is done row-wise. │ │ │ │ │ -DDeeffiinniittiioonn ParameterMatrix.h:180 │ │ │ │ │ -_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const ParameterMatrix< M > &other, double tol=1e-8) const │ │ │ │ │ -Check for equality up to absolute tolerance. │ │ │ │ │ -DDeeffiinniittiioonn ParameterMatrix.h:172 │ │ │ │ │ -_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_o_p_e_r_a_t_o_r_+ │ │ │ │ │ -ParameterMatrix< M > operator+(const ParameterMatrix< M > &other) const │ │ │ │ │ -Add a ParameterMatrix to another. │ │ │ │ │ -DDeeffiinniittiioonn ParameterMatrix.h:114 │ │ │ │ │ -_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_r_o_w_s │ │ │ │ │ -size_t rows() const │ │ │ │ │ -Get the number of rows. │ │ │ │ │ -DDeeffiinniittiioonn ParameterMatrix.h:62 │ │ │ │ │ -_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_r_o_w │ │ │ │ │ -auto row(size_t index) -> Eigen::Block< MatrixType, 1, -1, false > │ │ │ │ │ -Set the matrix row specified by index. │ │ │ │ │ -DDeeffiinniittiioonn ParameterMatrix.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="") const │ │ │ │ │ -Print the ParameterMatrix. │ │ │ │ │ -DDeeffiinniittiioonn ParameterMatrix.h:163 │ │ │ │ │ -_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_c_o_l │ │ │ │ │ -auto col(size_t index) -> Eigen::Block< MatrixType, M, 1, true > │ │ │ │ │ -Set the matrix column specified by index. │ │ │ │ │ -DDeeffiinniittiioonn ParameterMatrix.h:101 │ │ │ │ │ -_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_o_p_e_r_a_t_o_r_+ │ │ │ │ │ -ParameterMatrix< M > operator+(const Eigen::Matrix< double, -1, 1 > &other) │ │ │ │ │ -const │ │ │ │ │ -Add a MxN-sized vector to the ParameterMatrix. │ │ │ │ │ -DDeeffiinniittiioonn ParameterMatrix.h:122 │ │ │ │ │ -_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_o_p_e_r_a_t_o_r_- │ │ │ │ │ -ParameterMatrix< M > operator-(const ParameterMatrix< M > &other) const │ │ │ │ │ -Subtract a ParameterMatrix from another. │ │ │ │ │ -DDeeffiinniittiioonn ParameterMatrix.h:133 │ │ │ │ │ -_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_d_i_m │ │ │ │ │ -size_t dim() const │ │ │ │ │ -Returns dimensionality of the tangent space. │ │ │ │ │ -DDeeffiinniittiioonn ParameterMatrix.h:177 │ │ │ │ │ -_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_c_o_l_s │ │ │ │ │ -size_t cols() const │ │ │ │ │ -Get the number of columns. │ │ │ │ │ -DDeeffiinniittiioonn ParameterMatrix.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_t_r_a_n_s_p_o_s_e │ │ │ │ │ -Eigen::Matrix< double, -1, M > transpose() const │ │ │ │ │ -Return the tranpose of the underlying matrix. │ │ │ │ │ -DDeeffiinniittiioonn ParameterMatrix.h:71 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s │ │ │ │ │ +CRTP Base class for function bases. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_C_h_e_b_y_s_h_e_v_1_B_a_s_i_s │ │ │ │ │ +Basis of Chebyshev polynomials of the first kind https://en.wikipedia.org/wiki/ │ │ │ │ │ +Chebyshev_polynomials#... │ │ │ │ │ +DDeeffiinniittiioonn Chebyshev.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_C_h_e_b_y_s_h_e_v_2_B_a_s_i_s │ │ │ │ │ +Basis of Chebyshev polynomials of the second kind. │ │ │ │ │ +DDeeffiinniittiioonn Chebyshev.h:80 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_i_s │ │ │ │ │ - * _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_._h │ │ │ │ │ + * _C_h_e_b_y_s_h_e_v_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00158.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Fourier.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/ParameterMatrix.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,44 +95,54 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
Fourier.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
ParameterMatrix.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Fourier decomposition, see e.g. │ │ │ │ +

Define ParameterMatrix class which is used to store values at interpolation points. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::FourierBasis
 Fourier basis. More...
class  gtsam::ParameterMatrix< M >
 A matrix abstraction of MxN values at the Basis points. More...
 
struct  gtsam::traits< ParameterMatrix< M > >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +template<int M>
std::ostream & gtsam::operator<< (std::ostream &os, const ParameterMatrix< M > &parameterMatrix)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Fourier decomposition, see e.g.

│ │ │ │ -

http://mathworld.wolfram.com/FourierSeries.html

Author
Varun Agrawal, Frank Dellaert
│ │ │ │ -
Date
July 4, 2020
│ │ │ │ +

Define ParameterMatrix class which is used to store values at interpolation points.

│ │ │ │ +
Author
Varun Agrawal, Frank Dellaert
│ │ │ │ +
Date
September 21, 2020
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,36 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Fourier.h File Reference │ │ │ │ │ -Fourier decomposition, see e.g. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +ParameterMatrix.h File Reference │ │ │ │ │ +Define ParameterMatrix class which is used to store values at interpolation │ │ │ │ │ +points. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_F_o_u_r_i_e_r_B_a_s_i_s │ │ │ │ │ -  Fourier basis. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_ _M_ _> │ │ │ │ │ +  A matrix abstraction of MxN values at the _B_a_s_i_s points. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_ _M_ _>_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +template │ │ │ │ │ +std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x< M > │ │ │ │ │ + ¶meterMatrix) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Fourier decomposition, see e.g. │ │ │ │ │ -_h_t_t_p_:_/_/_m_a_t_h_w_o_r_l_d_._w_o_l_f_r_a_m_._c_o_m_/_F_o_u_r_i_e_r_S_e_r_i_e_s_._h_t_m_l │ │ │ │ │ +Define ParameterMatrix class which is used to store values at interpolation │ │ │ │ │ +points. │ │ │ │ │ Author │ │ │ │ │ Varun Agrawal, Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - July 4, 2020 │ │ │ │ │ + September 21, 2020 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_i_s │ │ │ │ │ - * _F_o_u_r_i_e_r_._h │ │ │ │ │ + * _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00158.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,4 @@ │ │ │ │ │ var a00158 = [ │ │ │ │ │ - ["gtsam::FourierBasis", "a02748.html", null] │ │ │ │ │ + ["gtsam::ParameterMatrix< M >", "a02752.html", "a02752"], │ │ │ │ │ + ["gtsam::traits< ParameterMatrix< M > >", "a02756.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00158_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Fourier.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/ParameterMatrix.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,15 +98,15 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Fourier.h
│ │ │ │ +
ParameterMatrix.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │ @@ -114,90 +114,192 @@ │ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
20#pragma once
│ │ │ │
21
│ │ │ │ -
22#include <gtsam/basis/Basis.h>
│ │ │ │ -
23
│ │ │ │ -
24namespace gtsam {
│ │ │ │ +
22#include <gtsam/base/Matrix.h>
│ │ │ │ +
23#include <gtsam/base/Testable.h>
│ │ │ │ +
24#include <gtsam/base/VectorSpace.h>
│ │ │ │
25
│ │ │ │ -
│ │ │ │ -
27class FourierBasis : public Basis<FourierBasis> {
│ │ │ │ -
28 public:
│ │ │ │ -
29 using Parameters = Eigen::Matrix<double, /*Nx1*/ -1, 1>;
│ │ │ │ -
30 using DiffMatrix = Eigen::Matrix<double, /*NxN*/ -1, -1>;
│ │ │ │ -
31
│ │ │ │ -
│ │ │ │ -
40 static Weights CalculateWeights(size_t N, double x) {
│ │ │ │ -
41 Weights b(N);
│ │ │ │ -
42 b[0] = 1;
│ │ │ │ -
43 for (size_t i = 1, n = 1; i < N; i++) {
│ │ │ │ -
44 if (i % 2 == 1) {
│ │ │ │ -
45 b[i] = cos(n * x);
│ │ │ │ -
46 } else {
│ │ │ │ -
47 b[i] = sin(n * x);
│ │ │ │ -
48 n++;
│ │ │ │ -
49 }
│ │ │ │ -
50 }
│ │ │ │ -
51 return b;
│ │ │ │ -
52 }
│ │ │ │ -
│ │ │ │ -
53
│ │ │ │ -
│ │ │ │ -
64 static Weights CalculateWeights(size_t N, double x, double a, double b) {
│ │ │ │ -
65 // TODO(Varun) How do we enforce an interval for Fourier series?
│ │ │ │ -
66 return CalculateWeights(N, x);
│ │ │ │ -
67 }
│ │ │ │ -
│ │ │ │ -
68
│ │ │ │ -
│ │ │ │ -
73 static DiffMatrix DifferentiationMatrix(size_t N) {
│ │ │ │ -
74 DiffMatrix D = DiffMatrix::Zero(N, N);
│ │ │ │ -
75 double k = 1;
│ │ │ │ -
76 for (size_t i = 1; i < N; i += 2) {
│ │ │ │ -
77 D(i, i + 1) = k; // sin'(k*x) = k*cos(k*x)
│ │ │ │ -
78 D(i + 1, i) = -k; // cos'(k*x) = -k*sin(k*x)
│ │ │ │ -
79 k += 1;
│ │ │ │ -
80 }
│ │ │ │ -
81
│ │ │ │ -
82 return D;
│ │ │ │ -
83 }
│ │ │ │ -
│ │ │ │ -
84
│ │ │ │ -
│ │ │ │ -
92 static Weights DerivativeWeights(size_t N, double x) {
│ │ │ │ -
93 return CalculateWeights(N, x) * DifferentiationMatrix(N);
│ │ │ │ -
94 }
│ │ │ │ -
│ │ │ │ -
95
│ │ │ │ -
│ │ │ │ -
106 static Weights DerivativeWeights(size_t N, double x, double a, double b) {
│ │ │ │ -
107 return CalculateWeights(N, x, a, b) * DifferentiationMatrix(N);
│ │ │ │ -
108 }
│ │ │ │ +
26#include <iostream>
│ │ │ │ +
27
│ │ │ │ +
28namespace gtsam {
│ │ │ │ +
29
│ │ │ │ +
37template <int M>
│ │ │ │ +
│ │ │ │ +
38class ParameterMatrix {
│ │ │ │ +
39 using MatrixType = Eigen::Matrix<double, M, -1>;
│ │ │ │ +
40
│ │ │ │ +
41 private:
│ │ │ │ +
42 MatrixType matrix_;
│ │ │ │ +
43
│ │ │ │ +
44 public:
│ │ │ │ +
45 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
46
│ │ │ │ +
47 enum { dimension = Eigen::Dynamic };
│ │ │ │ +
48
│ │ │ │ +
53 ParameterMatrix(const size_t N) : matrix_(M, N) { matrix_.setZero(); }
│ │ │ │ +
54
│ │ │ │ +
59 ParameterMatrix(const MatrixType& matrix) : matrix_(matrix) {}
│ │ │ │ +
60
│ │ │ │ +
62 size_t rows() const { return matrix_.rows(); }
│ │ │ │ +
63
│ │ │ │ +
65 size_t cols() const { return matrix_.cols(); }
│ │ │ │ +
66
│ │ │ │ +
68 MatrixType matrix() const { return matrix_; }
│ │ │ │ +
69
│ │ │ │ +
71 Eigen::Matrix<double, -1, M> transpose() const { return matrix_.transpose(); }
│ │ │ │ +
72
│ │ │ │ +
│ │ │ │ +
77 Eigen::Matrix<double, 1, -1> row(size_t index) const {
│ │ │ │ +
78 return matrix_.row(index);
│ │ │ │ +
79 }
│ │ │ │ +
│ │ │ │ +
80
│ │ │ │ +
│ │ │ │ +
85 auto row(size_t index) -> Eigen::Block<MatrixType, 1, -1, false> {
│ │ │ │ +
86 return matrix_.row(index);
│ │ │ │ +
87 }
│ │ │ │ +
│ │ │ │ +
88
│ │ │ │ +
│ │ │ │ +
93 Eigen::Matrix<double, M, 1> col(size_t index) const {
│ │ │ │ +
94 return matrix_.col(index);
│ │ │ │ +
95 }
│ │ │ │ +
│ │ │ │ +
96
│ │ │ │ +
│ │ │ │ +
101 auto col(size_t index) -> Eigen::Block<MatrixType, M, 1, true> {
│ │ │ │ +
102 return matrix_.col(index);
│ │ │ │ +
103 }
│ │ │ │
│ │ │ │ +
104
│ │ │ │ +
108 void setZero() { matrix_.setZero(); }
│ │ │ │
109
│ │ │ │ -
110}; // FourierBasis
│ │ │ │ -
│ │ │ │ -
111
│ │ │ │ -
112} // namespace gtsam
│ │ │ │ -
Basis.h
Compute an interpolating basis.
│ │ │ │ +
│ │ │ │ +
114 ParameterMatrix<M> operator+(const ParameterMatrix<M>& other) const {
│ │ │ │ +
115 return ParameterMatrix<M>(matrix_ + other.matrix());
│ │ │ │ +
116 }
│ │ │ │ +
│ │ │ │ +
117
│ │ │ │ +
│ │ │ │ +
122 ParameterMatrix<M> operator+(
│ │ │ │ +
123 const Eigen::Matrix<double, -1, 1>& other) const {
│ │ │ │ +
124 // This form avoids a deep copy and instead typecasts `other`.
│ │ │ │ +
125 Eigen::Map<const MatrixType> other_(other.data(), M, cols());
│ │ │ │ +
126 return ParameterMatrix<M>(matrix_ + other_);
│ │ │ │ +
127 }
│ │ │ │ +
│ │ │ │ +
128
│ │ │ │ +
│ │ │ │ +
133 ParameterMatrix<M> operator-(const ParameterMatrix<M>& other) const {
│ │ │ │ +
134 return ParameterMatrix<M>(matrix_ - other.matrix());
│ │ │ │ +
135 }
│ │ │ │ +
│ │ │ │ +
136
│ │ │ │ +
│ │ │ │ +
141 ParameterMatrix<M> operator-(
│ │ │ │ +
142 const Eigen::Matrix<double, -1, 1>& other) const {
│ │ │ │ +
143 Eigen::Map<const MatrixType> other_(other.data(), M, cols());
│ │ │ │ +
144 return ParameterMatrix<M>(matrix_ - other_);
│ │ │ │ +
145 }
│ │ │ │ +
│ │ │ │ +
146
│ │ │ │ +
│ │ │ │ +
152 MatrixType operator*(const Eigen::Matrix<double, -1, -1>& other) const {
│ │ │ │ +
153 return matrix_ * other;
│ │ │ │ +
154 }
│ │ │ │ +
│ │ │ │ +
155
│ │ │ │ +
158
│ │ │ │ +
│ │ │ │ +
163 void print(const std::string& s = "") const {
│ │ │ │ +
164 std::cout << (s == "" ? s : s + " ") << matrix_ << std::endl;
│ │ │ │ +
165 }
│ │ │ │ +
│ │ │ │ +
166
│ │ │ │ +
│ │ │ │ +
172 bool equals(const ParameterMatrix<M>& other, double tol = 1e-8) const {
│ │ │ │ +
173 return gtsam::equal_with_abs_tol(matrix_, other.matrix(), tol);
│ │ │ │ +
174 }
│ │ │ │ +
│ │ │ │ +
175
│ │ │ │ +
177 inline size_t dim() const { return matrix_.size(); }
│ │ │ │ +
178
│ │ │ │ +
│ │ │ │ +
180 inline Vector vector() const {
│ │ │ │ +
181 using RowMajor = Eigen::Matrix<double, -1, -1, Eigen::RowMajor>;
│ │ │ │ +
182 Vector result(matrix_.size());
│ │ │ │ +
183 Eigen::Map<RowMajor>(&result(0), rows(), cols()) = matrix_;
│ │ │ │ +
184 return result;
│ │ │ │ +
185 }
│ │ │ │ +
│ │ │ │ +
186
│ │ │ │ +
│ │ │ │ +
192 inline static ParameterMatrix Identity() {
│ │ │ │ +
193 // throw std::runtime_error(
│ │ │ │ +
194 // "ParameterMatrix::Identity(): Don't use this function");
│ │ │ │ +
195 return ParameterMatrix(0);
│ │ │ │ +
196 }
│ │ │ │ +
│ │ │ │ +
197
│ │ │ │ +
199};
│ │ │ │ +
│ │ │ │ +
200
│ │ │ │ +
201// traits for ParameterMatrix
│ │ │ │ +
202template <int M>
│ │ │ │ +
│ │ │ │ +
203struct traits<ParameterMatrix<M>>
│ │ │ │ +
204 : public internal::VectorSpace<ParameterMatrix<M>> {};
│ │ │ │ +
│ │ │ │ +
205
│ │ │ │ +
206/* ************************************************************************* */
│ │ │ │ +
207// Stream operator that takes a ParameterMatrix. Used for printing.
│ │ │ │ +
208template <int M>
│ │ │ │ +
209inline std::ostream& operator<<(std::ostream& os,
│ │ │ │ +
210 const ParameterMatrix<M>& parameterMatrix) {
│ │ │ │ +
211 os << parameterMatrix.matrix();
│ │ │ │ +
212 return os;
│ │ │ │ +
213}
│ │ │ │ +
214
│ │ │ │ +
215} // namespace gtsam
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::Basis
CRTP Base class for function bases.
Definition Basis.h:100
│ │ │ │ -
gtsam::FourierBasis
Fourier basis.
Definition Fourier.h:27
│ │ │ │ -
gtsam::FourierBasis::DifferentiationMatrix
static DiffMatrix DifferentiationMatrix(size_t N)
Compute D = differentiation matrix.
Definition Fourier.h:73
│ │ │ │ -
gtsam::FourierBasis::DerivativeWeights
static Weights DerivativeWeights(size_t N, double x)
Get weights at a given x that calculate the derivative.
Definition Fourier.h:92
│ │ │ │ -
gtsam::FourierBasis::CalculateWeights
static Weights CalculateWeights(size_t N, double x)
Evaluate Real Fourier Weights of size N in interval [a, b], e.g.
Definition Fourier.h:40
│ │ │ │ -
gtsam::FourierBasis::DerivativeWeights
static Weights DerivativeWeights(size_t N, double x, double a, double b)
Get derivative weights at a given x that calculate the derivative, in the interval [a,...
Definition Fourier.h:106
│ │ │ │ -
gtsam::FourierBasis::CalculateWeights
static Weights CalculateWeights(size_t N, double x, double a, double b)
Evaluate Real Fourier Weights of size N in interval [a, b], e.g.
Definition Fourier.h:64
│ │ │ │ +
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition Matrix.h:81
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::internal::VectorSpace
VectorSpace provides both Testable and VectorSpaceTraits.
Definition VectorSpace.h:207
│ │ │ │ +
gtsam::ParameterMatrix
A matrix abstraction of MxN values at the Basis points.
Definition ParameterMatrix.h:38
│ │ │ │ +
gtsam::ParameterMatrix::operator*
MatrixType operator*(const Eigen::Matrix< double, -1, -1 > &other) const
Multiply ParameterMatrix with an Eigen matrix.
Definition ParameterMatrix.h:152
│ │ │ │ +
gtsam::ParameterMatrix::operator-
ParameterMatrix< M > operator-(const Eigen::Matrix< double, -1, 1 > &other) const
Subtract a MxN-sized vector from the ParameterMatrix.
Definition ParameterMatrix.h:141
│ │ │ │ +
gtsam::ParameterMatrix::row
Eigen::Matrix< double, 1, -1 > row(size_t index) const
Get the matrix row specified by index.
Definition ParameterMatrix.h:77
│ │ │ │ +
gtsam::ParameterMatrix::ParameterMatrix
ParameterMatrix(const size_t N)
Create ParameterMatrix using the number of basis points.
Definition ParameterMatrix.h:53
│ │ │ │ +
gtsam::ParameterMatrix::Identity
static ParameterMatrix Identity()
Identity function to satisfy VectorSpace traits.
Definition ParameterMatrix.h:192
│ │ │ │ +
gtsam::ParameterMatrix::setZero
void setZero()
Set all matrix coefficients to zero.
Definition ParameterMatrix.h:108
│ │ │ │ +
gtsam::ParameterMatrix::matrix
MatrixType matrix() const
Get the underlying matrix.
Definition ParameterMatrix.h:68
│ │ │ │ +
gtsam::ParameterMatrix::ParameterMatrix
ParameterMatrix(const MatrixType &matrix)
Create ParameterMatrix from an MxN Eigen Matrix.
Definition ParameterMatrix.h:59
│ │ │ │ +
gtsam::ParameterMatrix::col
Eigen::Matrix< double, M, 1 > col(size_t index) const
Get the matrix column specified by index.
Definition ParameterMatrix.h:93
│ │ │ │ +
gtsam::ParameterMatrix::vector
Vector vector() const
Convert to vector form, is done row-wise.
Definition ParameterMatrix.h:180
│ │ │ │ +
gtsam::ParameterMatrix::equals
bool equals(const ParameterMatrix< M > &other, double tol=1e-8) const
Check for equality up to absolute tolerance.
Definition ParameterMatrix.h:172
│ │ │ │ +
gtsam::ParameterMatrix::operator+
ParameterMatrix< M > operator+(const ParameterMatrix< M > &other) const
Add a ParameterMatrix to another.
Definition ParameterMatrix.h:114
│ │ │ │ +
gtsam::ParameterMatrix::rows
size_t rows() const
Get the number of rows.
Definition ParameterMatrix.h:62
│ │ │ │ +
gtsam::ParameterMatrix::row
auto row(size_t index) -> Eigen::Block< MatrixType, 1, -1, false >
Set the matrix row specified by index.
Definition ParameterMatrix.h:85
│ │ │ │ +
gtsam::ParameterMatrix::print
void print(const std::string &s="") const
Print the ParameterMatrix.
Definition ParameterMatrix.h:163
│ │ │ │ +
gtsam::ParameterMatrix::col
auto col(size_t index) -> Eigen::Block< MatrixType, M, 1, true >
Set the matrix column specified by index.
Definition ParameterMatrix.h:101
│ │ │ │ +
gtsam::ParameterMatrix::operator+
ParameterMatrix< M > operator+(const Eigen::Matrix< double, -1, 1 > &other) const
Add a MxN-sized vector to the ParameterMatrix.
Definition ParameterMatrix.h:122
│ │ │ │ +
gtsam::ParameterMatrix::operator-
ParameterMatrix< M > operator-(const ParameterMatrix< M > &other) const
Subtract a ParameterMatrix from another.
Definition ParameterMatrix.h:133
│ │ │ │ +
gtsam::ParameterMatrix::dim
size_t dim() const
Returns dimensionality of the tangent space.
Definition ParameterMatrix.h:177
│ │ │ │ +
gtsam::ParameterMatrix::cols
size_t cols() const
Get the number of columns.
Definition ParameterMatrix.h:65
│ │ │ │ +
gtsam::ParameterMatrix::transpose
Eigen::Matrix< double, -1, M > transpose() const
Return the tranpose of the underlying matrix.
Definition ParameterMatrix.h:71
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Fourier.h │ │ │ │ │ +ParameterMatrix.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ @@ -16,94 +16,237 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 20#pragma once │ │ │ │ │ 21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_i_s_/_B_a_s_i_s_._h> │ │ │ │ │ -23 │ │ │ │ │ -24namespace _g_t_s_a_m { │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +24#include │ │ │ │ │ 25 │ │ │ │ │ -_2_7class _F_o_u_r_i_e_r_B_a_s_i_s : public _B_a_s_i_s { │ │ │ │ │ -28 public: │ │ │ │ │ -29 using Parameters = Eigen::Matrix; │ │ │ │ │ -30 using DiffMatrix = Eigen::Matrix; │ │ │ │ │ -31 │ │ │ │ │ -_4_0 static Weights _C_a_l_c_u_l_a_t_e_W_e_i_g_h_t_s(size_t N, double x) { │ │ │ │ │ -41 Weights b(N); │ │ │ │ │ -42 b[0] = 1; │ │ │ │ │ -43 for (size_t i = 1, n = 1; i < N; i++) { │ │ │ │ │ -44 if (i % 2 == 1) { │ │ │ │ │ -45 b[i] = cos(n * x); │ │ │ │ │ -46 } else { │ │ │ │ │ -47 b[i] = sin(n * x); │ │ │ │ │ -48 n++; │ │ │ │ │ -49 } │ │ │ │ │ -50 } │ │ │ │ │ -51 return b; │ │ │ │ │ -52 } │ │ │ │ │ -53 │ │ │ │ │ -_6_4 static Weights _C_a_l_c_u_l_a_t_e_W_e_i_g_h_t_s(size_t N, double x, double a, double b) { │ │ │ │ │ -65 // TODO(Varun) How do we enforce an interval for Fourier series? │ │ │ │ │ -66 return _C_a_l_c_u_l_a_t_e_W_e_i_g_h_t_s(N, x); │ │ │ │ │ -67 } │ │ │ │ │ -68 │ │ │ │ │ -_7_3 static DiffMatrix _D_i_f_f_e_r_e_n_t_i_a_t_i_o_n_M_a_t_r_i_x(size_t N) { │ │ │ │ │ -74 DiffMatrix D = DiffMatrix::Zero(N, N); │ │ │ │ │ -75 double k = 1; │ │ │ │ │ -76 for (size_t i = 1; i < N; i += 2) { │ │ │ │ │ -77 D(i, i + 1) = k; // sin'(k*x) = k*cos(k*x) │ │ │ │ │ -78 D(i + 1, i) = -k; // cos'(k*x) = -k*sin(k*x) │ │ │ │ │ -79 k += 1; │ │ │ │ │ -80 } │ │ │ │ │ -81 │ │ │ │ │ -82 return D; │ │ │ │ │ -83 } │ │ │ │ │ -84 │ │ │ │ │ -_9_2 static Weights _D_e_r_i_v_a_t_i_v_e_W_e_i_g_h_t_s(size_t N, double x) { │ │ │ │ │ -93 return _C_a_l_c_u_l_a_t_e_W_e_i_g_h_t_s(N, x) * _D_i_f_f_e_r_e_n_t_i_a_t_i_o_n_M_a_t_r_i_x(N); │ │ │ │ │ -94 } │ │ │ │ │ -95 │ │ │ │ │ -_1_0_6 static Weights _D_e_r_i_v_a_t_i_v_e_W_e_i_g_h_t_s(size_t N, double x, double a, double b) { │ │ │ │ │ -107 return _C_a_l_c_u_l_a_t_e_W_e_i_g_h_t_s(N, x, a, b) * _D_i_f_f_e_r_e_n_t_i_a_t_i_o_n_M_a_t_r_i_x(N); │ │ │ │ │ -108 } │ │ │ │ │ +26#include │ │ │ │ │ +27 │ │ │ │ │ +28namespace _g_t_s_a_m { │ │ │ │ │ +29 │ │ │ │ │ +37template │ │ │ │ │ +_3_8class _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x { │ │ │ │ │ +39 using MatrixType = Eigen::Matrix; │ │ │ │ │ +40 │ │ │ │ │ +41 private: │ │ │ │ │ +42 MatrixType matrix_; │ │ │ │ │ +43 │ │ │ │ │ +44 public: │ │ │ │ │ +45 EIGEN_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ +46 │ │ │ │ │ +47 enum { dimension = Eigen::Dynamic }; │ │ │ │ │ +48 │ │ │ │ │ +_5_3 _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x(const size_t N) : matrix_(M, N) { matrix_.setZero(); } │ │ │ │ │ +54 │ │ │ │ │ +_5_9 _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x(const MatrixType& _m_a_t_r_i_x) : matrix_(_m_a_t_r_i_x) {} │ │ │ │ │ +60 │ │ │ │ │ +_6_2 size_t _r_o_w_s() const { return matrix_.rows(); } │ │ │ │ │ +63 │ │ │ │ │ +_6_5 size_t _c_o_l_s() const { return matrix_.cols(); } │ │ │ │ │ +66 │ │ │ │ │ +_6_8 MatrixType _m_a_t_r_i_x() const { return matrix_; } │ │ │ │ │ +69 │ │ │ │ │ +_7_1 Eigen::Matrix _t_r_a_n_s_p_o_s_e() const { return matrix_.transpose(); │ │ │ │ │ +} │ │ │ │ │ +72 │ │ │ │ │ +_7_7 Eigen::Matrix _r_o_w(size_t index) const { │ │ │ │ │ +78 return matrix_.row(index); │ │ │ │ │ +79 } │ │ │ │ │ +80 │ │ │ │ │ +_8_5 auto _r_o_w(size_t index) -> Eigen::Block { │ │ │ │ │ +86 return matrix_.row(index); │ │ │ │ │ +87 } │ │ │ │ │ +88 │ │ │ │ │ +_9_3 Eigen::Matrix _c_o_l(size_t index) const { │ │ │ │ │ +94 return matrix_.col(index); │ │ │ │ │ +95 } │ │ │ │ │ +96 │ │ │ │ │ +_1_0_1 auto _c_o_l(size_t index) -> Eigen::Block { │ │ │ │ │ +102 return matrix_.col(index); │ │ │ │ │ +103 } │ │ │ │ │ +104 │ │ │ │ │ +_1_0_8 void _s_e_t_Z_e_r_o() { matrix_.setZero(); } │ │ │ │ │ 109 │ │ │ │ │ -110}; // FourierBasis │ │ │ │ │ -111 │ │ │ │ │ -112} // namespace gtsam │ │ │ │ │ -_B_a_s_i_s_._h │ │ │ │ │ -Compute an interpolating basis. │ │ │ │ │ +_1_1_4 _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_> _o_p_e_r_a_t_o_r_+(const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>& other) const { │ │ │ │ │ +115 return _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>(matrix_ + other._m_a_t_r_i_x()); │ │ │ │ │ +116 } │ │ │ │ │ +117 │ │ │ │ │ +_1_2_2 _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_> _o_p_e_r_a_t_o_r_+( │ │ │ │ │ +123 const Eigen::Matrix& other) const { │ │ │ │ │ +124 // This form avoids a deep copy and instead typecasts `other`. │ │ │ │ │ +125 Eigen::Map other_(other.data(), M, _c_o_l_s()); │ │ │ │ │ +126 return _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>(matrix_ + other_); │ │ │ │ │ +127 } │ │ │ │ │ +128 │ │ │ │ │ +_1_3_3 _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_> _o_p_e_r_a_t_o_r_-(const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>& other) const { │ │ │ │ │ +134 return _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>(matrix_ - other._m_a_t_r_i_x()); │ │ │ │ │ +135 } │ │ │ │ │ +136 │ │ │ │ │ +_1_4_1 _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_> _o_p_e_r_a_t_o_r_-( │ │ │ │ │ +142 const Eigen::Matrix& other) const { │ │ │ │ │ +143 Eigen::Map other_(other.data(), M, _c_o_l_s()); │ │ │ │ │ +144 return _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>(matrix_ - other_); │ │ │ │ │ +145 } │ │ │ │ │ +146 │ │ │ │ │ +_1_5_2 MatrixType _o_p_e_r_a_t_o_r_*(const Eigen::Matrix& other) const { │ │ │ │ │ +153 return matrix_ * other; │ │ │ │ │ +154 } │ │ │ │ │ +155 │ │ │ │ │ +158 │ │ │ │ │ +_1_6_3 void _p_r_i_n_t(const std::string& s = "") const { │ │ │ │ │ +164 std::cout << (s == "" ? s : s + " ") << matrix_ << std::endl; │ │ │ │ │ +165 } │ │ │ │ │ +166 │ │ │ │ │ +_1_7_2 bool _e_q_u_a_l_s(const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>& other, double tol = 1e-8) const { │ │ │ │ │ +173 return _g_t_s_a_m_:_:_e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(matrix_, other._m_a_t_r_i_x(), tol); │ │ │ │ │ +174 } │ │ │ │ │ +175 │ │ │ │ │ +_1_7_7 inline size_t _d_i_m() const { return matrix_.size(); } │ │ │ │ │ +178 │ │ │ │ │ +_1_8_0 inline Vector _v_e_c_t_o_r() const { │ │ │ │ │ +181 using RowMajor = Eigen::Matrix; │ │ │ │ │ +182 Vector result(matrix_.size()); │ │ │ │ │ +183 Eigen::Map(&result(0), _r_o_w_s(), _c_o_l_s()) = matrix_; │ │ │ │ │ +184 return result; │ │ │ │ │ +185 } │ │ │ │ │ +186 │ │ │ │ │ +_1_9_2 inline static _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x _I_d_e_n_t_i_t_y() { │ │ │ │ │ +193 // throw std::runtime_error( │ │ │ │ │ +194 // "ParameterMatrix::Identity(): Don't use this function"); │ │ │ │ │ +195 return _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x(0); │ │ │ │ │ +196 } │ │ │ │ │ +197 │ │ │ │ │ +199}; │ │ │ │ │ +200 │ │ │ │ │ +201// traits for ParameterMatrix │ │ │ │ │ +202template │ │ │ │ │ +_2_0_3struct _t_r_a_i_t_s<_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x> │ │ │ │ │ +204 : public _i_n_t_e_r_n_a_l_:_:_V_e_c_t_o_r_S_p_a_c_e> {}; │ │ │ │ │ +205 │ │ │ │ │ +206/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +207// Stream operator that takes a ParameterMatrix. Used for printing. │ │ │ │ │ +208template │ │ │ │ │ +209inline std::ostream& operator<<(std::ostream& os, │ │ │ │ │ +210 const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>& parameterMatrix) { │ │ │ │ │ +211 os << parameterMatrix._m_a_t_r_i_x(); │ │ │ │ │ +212 return os; │ │ │ │ │ +213} │ │ │ │ │ +214 │ │ │ │ │ +215} // namespace gtsam │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ +_M_a_t_r_i_x_._h │ │ │ │ │ +typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s │ │ │ │ │ -CRTP Base class for function bases. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_F_o_u_r_i_e_r_B_a_s_i_s │ │ │ │ │ -Fourier basis. │ │ │ │ │ -DDeeffiinniittiioonn Fourier.h:27 │ │ │ │ │ -_g_t_s_a_m_:_:_F_o_u_r_i_e_r_B_a_s_i_s_:_:_D_i_f_f_e_r_e_n_t_i_a_t_i_o_n_M_a_t_r_i_x │ │ │ │ │ -static DiffMatrix DifferentiationMatrix(size_t N) │ │ │ │ │ -Compute D = differentiation matrix. │ │ │ │ │ -DDeeffiinniittiioonn Fourier.h:73 │ │ │ │ │ -_g_t_s_a_m_:_:_F_o_u_r_i_e_r_B_a_s_i_s_:_:_D_e_r_i_v_a_t_i_v_e_W_e_i_g_h_t_s │ │ │ │ │ -static Weights DerivativeWeights(size_t N, double x) │ │ │ │ │ -Get weights at a given x that calculate the derivative. │ │ │ │ │ -DDeeffiinniittiioonn Fourier.h:92 │ │ │ │ │ -_g_t_s_a_m_:_:_F_o_u_r_i_e_r_B_a_s_i_s_:_:_C_a_l_c_u_l_a_t_e_W_e_i_g_h_t_s │ │ │ │ │ -static Weights CalculateWeights(size_t N, double x) │ │ │ │ │ -Evaluate Real Fourier Weights of size N in interval [a, b], e.g. │ │ │ │ │ -DDeeffiinniittiioonn Fourier.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_F_o_u_r_i_e_r_B_a_s_i_s_:_:_D_e_r_i_v_a_t_i_v_e_W_e_i_g_h_t_s │ │ │ │ │ -static Weights DerivativeWeights(size_t N, double x, double a, double b) │ │ │ │ │ -Get derivative weights at a given x that calculate the derivative, in the │ │ │ │ │ -interval [a,... │ │ │ │ │ -DDeeffiinniittiioonn Fourier.h:106 │ │ │ │ │ -_g_t_s_a_m_:_:_F_o_u_r_i_e_r_B_a_s_i_s_:_:_C_a_l_c_u_l_a_t_e_W_e_i_g_h_t_s │ │ │ │ │ -static Weights CalculateWeights(size_t N, double x, double a, double b) │ │ │ │ │ -Evaluate Real Fourier Weights of size N in interval [a, b], e.g. │ │ │ │ │ -DDeeffiinniittiioonn Fourier.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l___w_i_t_h___a_b_s___t_o_l │ │ │ │ │ +bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen:: │ │ │ │ │ +DenseBase< MATRIX > &B, double tol=1e-9) │ │ │ │ │ +equals with a tolerance │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:81 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_V_e_c_t_o_r_S_p_a_c_e │ │ │ │ │ +VectorSpace provides both Testable and VectorSpaceTraits. │ │ │ │ │ +DDeeffiinniittiioonn VectorSpace.h:207 │ │ │ │ │ +_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x │ │ │ │ │ +A matrix abstraction of MxN values at the Basis points. │ │ │ │ │ +DDeeffiinniittiioonn ParameterMatrix.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ +MatrixType operator*(const Eigen::Matrix< double, -1, -1 > &other) const │ │ │ │ │ +Multiply ParameterMatrix with an Eigen matrix. │ │ │ │ │ +DDeeffiinniittiioonn ParameterMatrix.h:152 │ │ │ │ │ +_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_o_p_e_r_a_t_o_r_- │ │ │ │ │ +ParameterMatrix< M > operator-(const Eigen::Matrix< double, -1, 1 > &other) │ │ │ │ │ +const │ │ │ │ │ +Subtract a MxN-sized vector from the ParameterMatrix. │ │ │ │ │ +DDeeffiinniittiioonn ParameterMatrix.h:141 │ │ │ │ │ +_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_r_o_w │ │ │ │ │ +Eigen::Matrix< double, 1, -1 > row(size_t index) const │ │ │ │ │ +Get the matrix row specified by index. │ │ │ │ │ +DDeeffiinniittiioonn ParameterMatrix.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x │ │ │ │ │ +ParameterMatrix(const size_t N) │ │ │ │ │ +Create ParameterMatrix using the number of basis points. │ │ │ │ │ +DDeeffiinniittiioonn ParameterMatrix.h:53 │ │ │ │ │ +_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_I_d_e_n_t_i_t_y │ │ │ │ │ +static ParameterMatrix Identity() │ │ │ │ │ +Identity function to satisfy VectorSpace traits. │ │ │ │ │ +DDeeffiinniittiioonn ParameterMatrix.h:192 │ │ │ │ │ +_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_s_e_t_Z_e_r_o │ │ │ │ │ +void setZero() │ │ │ │ │ +Set all matrix coefficients to zero. │ │ │ │ │ +DDeeffiinniittiioonn ParameterMatrix.h:108 │ │ │ │ │ +_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_m_a_t_r_i_x │ │ │ │ │ +MatrixType matrix() const │ │ │ │ │ +Get the underlying matrix. │ │ │ │ │ +DDeeffiinniittiioonn ParameterMatrix.h:68 │ │ │ │ │ +_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x │ │ │ │ │ +ParameterMatrix(const MatrixType &matrix) │ │ │ │ │ +Create ParameterMatrix from an MxN Eigen Matrix. │ │ │ │ │ +DDeeffiinniittiioonn ParameterMatrix.h:59 │ │ │ │ │ +_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_c_o_l │ │ │ │ │ +Eigen::Matrix< double, M, 1 > col(size_t index) const │ │ │ │ │ +Get the matrix column specified by index. │ │ │ │ │ +DDeeffiinniittiioonn ParameterMatrix.h:93 │ │ │ │ │ +_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_v_e_c_t_o_r │ │ │ │ │ +Vector vector() const │ │ │ │ │ +Convert to vector form, is done row-wise. │ │ │ │ │ +DDeeffiinniittiioonn ParameterMatrix.h:180 │ │ │ │ │ +_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const ParameterMatrix< M > &other, double tol=1e-8) const │ │ │ │ │ +Check for equality up to absolute tolerance. │ │ │ │ │ +DDeeffiinniittiioonn ParameterMatrix.h:172 │ │ │ │ │ +_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_o_p_e_r_a_t_o_r_+ │ │ │ │ │ +ParameterMatrix< M > operator+(const ParameterMatrix< M > &other) const │ │ │ │ │ +Add a ParameterMatrix to another. │ │ │ │ │ +DDeeffiinniittiioonn ParameterMatrix.h:114 │ │ │ │ │ +_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_r_o_w_s │ │ │ │ │ +size_t rows() const │ │ │ │ │ +Get the number of rows. │ │ │ │ │ +DDeeffiinniittiioonn ParameterMatrix.h:62 │ │ │ │ │ +_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_r_o_w │ │ │ │ │ +auto row(size_t index) -> Eigen::Block< MatrixType, 1, -1, false > │ │ │ │ │ +Set the matrix row specified by index. │ │ │ │ │ +DDeeffiinniittiioonn ParameterMatrix.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="") const │ │ │ │ │ +Print the ParameterMatrix. │ │ │ │ │ +DDeeffiinniittiioonn ParameterMatrix.h:163 │ │ │ │ │ +_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_c_o_l │ │ │ │ │ +auto col(size_t index) -> Eigen::Block< MatrixType, M, 1, true > │ │ │ │ │ +Set the matrix column specified by index. │ │ │ │ │ +DDeeffiinniittiioonn ParameterMatrix.h:101 │ │ │ │ │ +_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_o_p_e_r_a_t_o_r_+ │ │ │ │ │ +ParameterMatrix< M > operator+(const Eigen::Matrix< double, -1, 1 > &other) │ │ │ │ │ +const │ │ │ │ │ +Add a MxN-sized vector to the ParameterMatrix. │ │ │ │ │ +DDeeffiinniittiioonn ParameterMatrix.h:122 │ │ │ │ │ +_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_o_p_e_r_a_t_o_r_- │ │ │ │ │ +ParameterMatrix< M > operator-(const ParameterMatrix< M > &other) const │ │ │ │ │ +Subtract a ParameterMatrix from another. │ │ │ │ │ +DDeeffiinniittiioonn ParameterMatrix.h:133 │ │ │ │ │ +_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_d_i_m │ │ │ │ │ +size_t dim() const │ │ │ │ │ +Returns dimensionality of the tangent space. │ │ │ │ │ +DDeeffiinniittiioonn ParameterMatrix.h:177 │ │ │ │ │ +_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_c_o_l_s │ │ │ │ │ +size_t cols() const │ │ │ │ │ +Get the number of columns. │ │ │ │ │ +DDeeffiinniittiioonn ParameterMatrix.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_:_:_t_r_a_n_s_p_o_s_e │ │ │ │ │ +Eigen::Matrix< double, -1, M > transpose() const │ │ │ │ │ +Return the tranpose of the underlying matrix. │ │ │ │ │ +DDeeffiinniittiioonn ParameterMatrix.h:71 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_i_s │ │ │ │ │ - * _F_o_u_r_i_e_r_._h │ │ │ │ │ + * _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00161.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Chebyshev2.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/FitBasis.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,47 +95,56 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
Chebyshev2.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Typedefs
│ │ │ │ +
FitBasis.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Pseudo-spectral parameterization for Chebyshev polynomials of the second kind. │ │ │ │ +

Fit a Basis using least-squares. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::Chebyshev2
 Chebyshev Interpolation on Chebyshev points of the second kind Note that N here, the number of points, is one less than N from 'Approximation Theory and Approximation Practice by L. More...
class  gtsam::FitBasis< Basis >
 Class that does regression via least squares Example usage: size_t N = 3; auto fit = FitBasis<Chebyshev2>(data_points, noise_model, N); Vector coefficients = fit.parameters();. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Typedefs

│ │ │ │ +using gtsam::Sequence = std::map< double, double >
 Our sequence representation is a map of {x: y} values where y = f(x)
 
│ │ │ │ +using gtsam::Sample = std::pair< double, double >
 A sample is a key-value pair from a sequence.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Pseudo-spectral parameterization for Chebyshev polynomials of the second kind.

│ │ │ │ -

In a pseudo-spectral case, rather than the parameters acting as weights for the bases polynomials (as in Chebyshev2Basis), here the parameters are the values at a specific set of points in the interval, the "Chebyshev points". These values uniquely determine the polynomial that interpolates them at the Chebyshev points.

│ │ │ │ -

This is different from Chebyshev.h since it leverage ideas from pseudo-spectral optimization, i.e. we don't decompose into basis functions, rather estimate function values at the Chebyshev points.

│ │ │ │ -

Please refer to Agrawal21icra for more details.

│ │ │ │ -
Author
Varun Agrawal, Jing Dong, Frank Dellaert
│ │ │ │ +

Fit a Basis using least-squares.

│ │ │ │
Date
July 4, 2020
│ │ │ │ +
Author
Varun Agrawal, Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,39 +1,36 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Chebyshev2.h File Reference │ │ │ │ │ -Pseudo-spectral parameterization for Chebyshev polynomials of the second kind. │ │ │ │ │ -_M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ +FitBasis.h File Reference │ │ │ │ │ +Fit a Basis using least-squares. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_C_h_e_b_y_s_h_e_v_2 │ │ │ │ │ - Chebyshev Interpolation on Chebyshev points of the second kind Note │ │ │ │ │ -  that N here, the number of points, is one less than N from │ │ │ │ │ - 'Approximation Theory and Approximation Practice by L. _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_F_i_t_B_a_s_i_s_<_ _B_a_s_i_s_ _> │ │ │ │ │ + Class that does regression via least squares Example usage: size_t N = │ │ │ │ │ +  3; auto fit = FitBasis(data_points, noise_model, N); Vector │ │ │ │ │ + coefficients = fit.parameters();. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +using  ggttssaamm::::SSeeqquueennccee = std::map< double, double > │ │ │ │ │ +  Our sequence representation is a map of {x: y} values where y = f(x) │ │ │ │ │ +  │ │ │ │ │ +using  ggttssaamm::::SSaammppllee = std::pair< double, double > │ │ │ │ │ +  A sample is a key-value pair from a sequence. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Pseudo-spectral parameterization for Chebyshev polynomials of the second kind. │ │ │ │ │ -In a pseudo-spectral case, rather than the parameters acting as weights for the │ │ │ │ │ -bases polynomials (as in Chebyshev2Basis), here the parameters are the vvaalluueess │ │ │ │ │ -at a specific set of points in the interval, the "Chebyshev points". These │ │ │ │ │ -values uniquely determine the polynomial that interpolates them at the │ │ │ │ │ -Chebyshev points. │ │ │ │ │ -This is different from _C_h_e_b_y_s_h_e_v_._h since it leverage ideas from pseudo-spectral │ │ │ │ │ -optimization, i.e. we don't decompose into basis functions, rather estimate │ │ │ │ │ -function values at the Chebyshev points. │ │ │ │ │ -Please refer to Agrawal21icra for more details. │ │ │ │ │ - Author │ │ │ │ │ - Varun Agrawal, Jing Dong, Frank Dellaert │ │ │ │ │ +Fit a Basis using least-squares. │ │ │ │ │ Date │ │ │ │ │ July 4, 2020 │ │ │ │ │ + Author │ │ │ │ │ + Varun Agrawal, Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_i_s │ │ │ │ │ - * _C_h_e_b_y_s_h_e_v_2_._h │ │ │ │ │ + * _F_i_t_B_a_s_i_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00161.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,5 @@ │ │ │ │ │ var a00161 = [ │ │ │ │ │ - ["gtsam::Chebyshev2", "a02740.html", null] │ │ │ │ │ + ["gtsam::FitBasis< Basis >", "a02744.html", "a02744"], │ │ │ │ │ + ["Sample", "a00161.html#a9920fe883a2f506440aca7c1a2bd1a00", null], │ │ │ │ │ + ["Sequence", "a00161.html#a24ec5d6ec6b211d0cbaf645c11c198de", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00161_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Chebyshev2.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/FitBasis.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,122 +98,119 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Chebyshev2.h
│ │ │ │ +
FitBasis.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
33#pragma once
│ │ │ │ +
19/*
│ │ │ │ +
20 * Concept needed for LS. Parameters = Coefficients | Values
│ │ │ │ +
21 * - Parameters, Jacobian
│ │ │ │ +
22 * - PredictFactor(double x)(Parameters p, OptionalJacobian<1,N> H)
│ │ │ │ +
23 */
│ │ │ │ +
24
│ │ │ │ +
25#pragma once
│ │ │ │ +
26
│ │ │ │ +
27#include <gtsam/basis/Basis.h>
│ │ │ │ +
28#include <gtsam/basis/BasisFactors.h>
│ │ │ │ +
29#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ +
30#include <gtsam/linear/VectorValues.h>
│ │ │ │ +
31#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ +
32
│ │ │ │ +
33namespace gtsam {
│ │ │ │
34
│ │ │ │ -
35#include <gtsam/base/Manifold.h>
│ │ │ │ -
36#include <gtsam/base/OptionalJacobian.h>
│ │ │ │ -
37#include <gtsam/basis/Basis.h>
│ │ │ │ -
38
│ │ │ │ -
39namespace gtsam {
│ │ │ │ -
40
│ │ │ │ -
│ │ │ │ -
46class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
│ │ │ │ -
47 public:
│ │ │ │ -
48 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
49
│ │ │ │ -
50 using Base = Basis<Chebyshev2>;
│ │ │ │ -
51 using Parameters = Eigen::Matrix<double, /*Nx1*/ -1, 1>;
│ │ │ │ -
52 using DiffMatrix = Eigen::Matrix<double, /*NxN*/ -1, -1>;
│ │ │ │ -
53
│ │ │ │ -
│ │ │ │ -
55 static double Point(size_t N, int j) {
│ │ │ │ -
56 assert(j >= 0 && size_t(j) < N);
│ │ │ │ -
57 const double dtheta = M_PI / (N > 1 ? (N - 1) : 1);
│ │ │ │ -
58 // We add -PI so that we get values ordered from -1 to +1
│ │ │ │ -
59 // sin(- M_PI_2 + dtheta*j); also works
│ │ │ │ -
60 return cos(-M_PI + dtheta * j);
│ │ │ │ -
61 }
│ │ │ │ -
│ │ │ │ -
62
│ │ │ │ -
│ │ │ │ -
64 static double Point(size_t N, int j, double a, double b) {
│ │ │ │ -
65 assert(j >= 0 && size_t(j) < N);
│ │ │ │ -
66 const double dtheta = M_PI / (N - 1);
│ │ │ │ -
67 // We add -PI so that we get values ordered from -1 to +1
│ │ │ │ -
68 return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2;
│ │ │ │ -
69 }
│ │ │ │ -
│ │ │ │ -
70
│ │ │ │ -
│ │ │ │ -
72 static Vector Points(size_t N) {
│ │ │ │ -
73 Vector points(N);
│ │ │ │ -
74 for (size_t j = 0; j < N; j++) points(j) = Point(N, j);
│ │ │ │ -
75 return points;
│ │ │ │ -
76 }
│ │ │ │ -
│ │ │ │ -
77
│ │ │ │ -
│ │ │ │ -
79 static Vector Points(size_t N, double a, double b) {
│ │ │ │ -
80 Vector points = Points(N);
│ │ │ │ -
81 const double T1 = (a + b) / 2, T2 = (b - a) / 2;
│ │ │ │ -
82 points = T1 + (T2 * points).array();
│ │ │ │ -
83 return points;
│ │ │ │ -
84 }
│ │ │ │ -
│ │ │ │ -
85
│ │ │ │ -
94 static Weights CalculateWeights(size_t N, double x, double a = -1,
│ │ │ │ -
95 double b = 1);
│ │ │ │ -
96
│ │ │ │ -
101 static Weights DerivativeWeights(size_t N, double x, double a = -1,
│ │ │ │ -
102 double b = 1);
│ │ │ │ -
103
│ │ │ │ -
108 static DiffMatrix DifferentiationMatrix(size_t N, double a = -1,
│ │ │ │ -
109 double b = 1);
│ │ │ │ -
110
│ │ │ │ -
129 static Weights IntegrationWeights(size_t N, double a = -1, double b = 1);
│ │ │ │ -
130
│ │ │ │ -
134 template <size_t M>
│ │ │ │ -
│ │ │ │ -
135 static Matrix matrix(std::function<Eigen::Matrix<double, M, 1>(double)> f,
│ │ │ │ -
136 size_t N, double a = -1, double b = 1) {
│ │ │ │ -
137 Matrix Xmat(M, N);
│ │ │ │ -
138 for (size_t j = 0; j < N; j++) {
│ │ │ │ -
139 Xmat.col(j) = f(Point(N, j, a, b));
│ │ │ │ -
140 }
│ │ │ │ -
141 return Xmat;
│ │ │ │ -
142 }
│ │ │ │ -
│ │ │ │ -
143}; // \ Chebyshev2
│ │ │ │ -
│ │ │ │ -
144
│ │ │ │ -
145} // namespace gtsam
│ │ │ │ -
OptionalJacobian.h
Special class for optional Jacobian arguments.
│ │ │ │ -
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ -
Basis.h
Compute an interpolating basis.
│ │ │ │ +
36using Sequence = std::map<double, double>;
│ │ │ │ +
38using Sample = std::pair<double, double>;
│ │ │ │ +
39
│ │ │ │ +
51template <class Basis>
│ │ │ │ +
│ │ │ │ +
52class FitBasis {
│ │ │ │ +
53 public:
│ │ │ │ +
54 using Parameters = typename Basis::Parameters;
│ │ │ │ +
55
│ │ │ │ +
56 private:
│ │ │ │ +
57 Parameters parameters_;
│ │ │ │ +
58
│ │ │ │ +
59 public:
│ │ │ │ +
│ │ │ │ +
61 static NonlinearFactorGraph NonlinearGraph(const Sequence& sequence,
│ │ │ │ +
62 const SharedNoiseModel& model,
│ │ │ │ +
63 size_t N) {
│ │ │ │ +
64 NonlinearFactorGraph graph;
│ │ │ │ +
65 for (const Sample sample : sequence) {
│ │ │ │ +
66 graph.emplace_shared<EvaluationFactor<Basis>>(0, sample.second, model, N,
│ │ │ │ +
67 sample.first);
│ │ │ │ +
68 }
│ │ │ │ +
69 return graph;
│ │ │ │ +
70 }
│ │ │ │ +
│ │ │ │ +
71
│ │ │ │ +
│ │ │ │ +
73 static GaussianFactorGraph::shared_ptr LinearGraph(
│ │ │ │ +
74 const Sequence& sequence, const SharedNoiseModel& model, size_t N) {
│ │ │ │ +
75 NonlinearFactorGraph graph = NonlinearGraph(sequence, model, N);
│ │ │ │ +
76 Values values;
│ │ │ │ +
77 values.insert<Parameters>(0, Parameters::Zero(N));
│ │ │ │ +
78 GaussianFactorGraph::shared_ptr gfg = graph.linearize(values);
│ │ │ │ +
79 return gfg;
│ │ │ │ +
80 }
│ │ │ │ +
│ │ │ │ +
81
│ │ │ │ +
│ │ │ │ +
89 FitBasis(const Sequence& sequence, const SharedNoiseModel& model, size_t N) {
│ │ │ │ +
90 GaussianFactorGraph::shared_ptr gfg = LinearGraph(sequence, model, N);
│ │ │ │ +
91 VectorValues solution = gfg->optimize();
│ │ │ │ +
92 parameters_ = solution.at(0);
│ │ │ │ +
93 }
│ │ │ │ +
│ │ │ │ +
94
│ │ │ │ +
96 Parameters parameters() const { return parameters_; }
│ │ │ │ +
97};
│ │ │ │ +
│ │ │ │ +
98
│ │ │ │ +
99} // namespace gtsam
│ │ │ │ +
Basis.h
Compute an interpolating basis.
│ │ │ │ +
BasisFactors.h
Factor definitions for various Basis functors.
│ │ │ │ +
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ +
VectorValues.h
Factor Graph Values.
│ │ │ │ +
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::Basis
CRTP Base class for function bases.
Definition Basis.h:100
│ │ │ │ -
gtsam::Chebyshev2
Chebyshev Interpolation on Chebyshev points of the second kind Note that N here, the number of points...
Definition Chebyshev2.h:46
│ │ │ │ -
gtsam::Chebyshev2::Point
static double Point(size_t N, int j)
Specific Chebyshev point.
Definition Chebyshev2.h:55
│ │ │ │ -
gtsam::Chebyshev2::Point
static double Point(size_t N, int j, double a, double b)
Specific Chebyshev point, within [a,b] interval.
Definition Chebyshev2.h:64
│ │ │ │ -
gtsam::Chebyshev2::Points
static Vector Points(size_t N)
All Chebyshev points.
Definition Chebyshev2.h:72
│ │ │ │ -
gtsam::Chebyshev2::Points
static Vector Points(size_t N, double a, double b)
All Chebyshev points, within [a,b] interval.
Definition Chebyshev2.h:79
│ │ │ │ -
gtsam::Chebyshev2::matrix
static Matrix matrix(std::function< Eigen::Matrix< double, M, 1 >(double)> f, size_t N, double a=-1, double b=1)
Create matrix of values at Chebyshev points given vector-valued function.
Definition Chebyshev2.h:135
│ │ │ │ +
gtsam::Sequence
std::map< double, double > Sequence
Our sequence representation is a map of {x: y} values where y = f(x)
Definition FitBasis.h:36
│ │ │ │ +
gtsam::Sample
std::pair< double, double > Sample
A sample is a key-value pair from a sequence.
Definition FitBasis.h:38
│ │ │ │ +
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ +
gtsam::EvaluationFactor
Factor for enforcing the scalar value of the polynomial BASIS representation at x is the same as the ...
Definition BasisFactors.h:39
│ │ │ │ +
gtsam::FitBasis
Class that does regression via least squares Example usage: size_t N = 3; auto fit = FitBasis<Chebysh...
Definition FitBasis.h:52
│ │ │ │ +
gtsam::FitBasis::NonlinearGraph
static NonlinearFactorGraph NonlinearGraph(const Sequence &sequence, const SharedNoiseModel &model, size_t N)
Create nonlinear FG from Sequence.
Definition FitBasis.h:61
│ │ │ │ +
gtsam::FitBasis::parameters
Parameters parameters() const
Return Fourier coefficients.
Definition FitBasis.h:96
│ │ │ │ +
gtsam::FitBasis::FitBasis
FitBasis(const Sequence &sequence, const SharedNoiseModel &model, size_t N)
Construct a new FitBasis object.
Definition FitBasis.h:89
│ │ │ │ +
gtsam::FitBasis::LinearGraph
static GaussianFactorGraph::shared_ptr LinearGraph(const Sequence &sequence, const SharedNoiseModel &model, size_t N)
Create linear FG from Sequence.
Definition FitBasis.h:73
│ │ │ │ +
gtsam::GaussianFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactorGraph.h:81
│ │ │ │ +
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
gtsam::VectorValues::at
Vector & at(Key j)
Read/write access to the vector value with key j, throws std::out_of_range if j does not exist,...
Definition VectorValues.h:139
│ │ │ │ +
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │ +
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
gtsam::Values::insert
void insert(Key j, const Value &val)
Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present.
Definition Values.cpp:157
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,128 +1,157 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Chebyshev2.h │ │ │ │ │ +FitBasis.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -33#pragma once │ │ │ │ │ +19/* │ │ │ │ │ +20 * Concept needed for LS. Parameters = Coefficients | Values │ │ │ │ │ +21 * - Parameters, Jacobian │ │ │ │ │ +22 * - PredictFactor(double x)(Parameters p, OptionalJacobian<1,N> H) │ │ │ │ │ +23 */ │ │ │ │ │ +24 │ │ │ │ │ +25#pragma once │ │ │ │ │ +26 │ │ │ │ │ +27#include <_g_t_s_a_m_/_b_a_s_i_s_/_B_a_s_i_s_._h> │ │ │ │ │ +28#include <_g_t_s_a_m_/_b_a_s_i_s_/_B_a_s_i_s_F_a_c_t_o_r_s_._h> │ │ │ │ │ +29#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +30#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ +31#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +32 │ │ │ │ │ +33namespace _g_t_s_a_m { │ │ │ │ │ 34 │ │ │ │ │ -35#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ -36#include <_g_t_s_a_m_/_b_a_s_e_/_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h> │ │ │ │ │ -37#include <_g_t_s_a_m_/_b_a_s_i_s_/_B_a_s_i_s_._h> │ │ │ │ │ -38 │ │ │ │ │ -39namespace _g_t_s_a_m { │ │ │ │ │ -40 │ │ │ │ │ -_4_6class GTSAM_EXPORT _C_h_e_b_y_s_h_e_v_2 : public _B_a_s_i_s { │ │ │ │ │ -47 public: │ │ │ │ │ -48 EIGEN_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ -49 │ │ │ │ │ -50 using _B_a_s_e = _B_a_s_i_s_<_C_h_e_b_y_s_h_e_v_2_>; │ │ │ │ │ -51 using Parameters = Eigen::Matrix; │ │ │ │ │ -52 using DiffMatrix = Eigen::Matrix; │ │ │ │ │ -53 │ │ │ │ │ -_5_5 static double _P_o_i_n_t(size_t N, int j) { │ │ │ │ │ -56 assert(j >= 0 && size_t(j) < N); │ │ │ │ │ -57 const double dtheta = M_PI / (N > 1 ? (N - 1) : 1); │ │ │ │ │ -58 // We add -PI so that we get values ordered from -1 to +1 │ │ │ │ │ -59 // sin(- M_PI_2 + dtheta*j); also works │ │ │ │ │ -60 return cos(-M_PI + dtheta * j); │ │ │ │ │ -61 } │ │ │ │ │ -62 │ │ │ │ │ -_6_4 static double _P_o_i_n_t(size_t N, int j, double a, double b) { │ │ │ │ │ -65 assert(j >= 0 && size_t(j) < N); │ │ │ │ │ -66 const double dtheta = M_PI / (N - 1); │ │ │ │ │ -67 // We add -PI so that we get values ordered from -1 to +1 │ │ │ │ │ -68 return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2; │ │ │ │ │ -69 } │ │ │ │ │ -70 │ │ │ │ │ -_7_2 static Vector _P_o_i_n_t_s(size_t N) { │ │ │ │ │ -73 Vector points(N); │ │ │ │ │ -74 for (size_t j = 0; j < N; j++) points(j) = Point(N, j); │ │ │ │ │ -75 return points; │ │ │ │ │ -76 } │ │ │ │ │ -77 │ │ │ │ │ -_7_9 static Vector _P_o_i_n_t_s(size_t N, double a, double b) { │ │ │ │ │ -80 Vector points = Points(N); │ │ │ │ │ -81 const double T1 = (a + b) / 2, T2 = (b - a) / 2; │ │ │ │ │ -82 points = T1 + (T2 * points).array(); │ │ │ │ │ -83 return points; │ │ │ │ │ -84 } │ │ │ │ │ -85 │ │ │ │ │ -94 static Weights CalculateWeights(size_t N, double x, double a = -1, │ │ │ │ │ -95 double b = 1); │ │ │ │ │ -96 │ │ │ │ │ -101 static Weights DerivativeWeights(size_t N, double x, double a = -1, │ │ │ │ │ -102 double b = 1); │ │ │ │ │ -103 │ │ │ │ │ -108 static DiffMatrix DifferentiationMatrix(size_t N, double a = -1, │ │ │ │ │ -109 double b = 1); │ │ │ │ │ -110 │ │ │ │ │ -129 static Weights IntegrationWeights(size_t N, double a = -1, double b = 1); │ │ │ │ │ -130 │ │ │ │ │ -134 template │ │ │ │ │ -_1_3_5 static Matrix _m_a_t_r_i_x(std::function(double)> f, │ │ │ │ │ -136 size_t N, double a = -1, double b = 1) { │ │ │ │ │ -137 Matrix Xmat(M, N); │ │ │ │ │ -138 for (size_t j = 0; j < N; j++) { │ │ │ │ │ -139 Xmat.col(j) = f(Point(N, j, a, b)); │ │ │ │ │ -140 } │ │ │ │ │ -141 return Xmat; │ │ │ │ │ -142 } │ │ │ │ │ -143}; // \ Chebyshev2 │ │ │ │ │ -144 │ │ │ │ │ -145} // namespace gtsam │ │ │ │ │ -_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h │ │ │ │ │ -Special class for optional Jacobian arguments. │ │ │ │ │ -_M_a_n_i_f_o_l_d_._h │ │ │ │ │ -Base class and basic functions for Manifold types. │ │ │ │ │ +_3_6using _S_e_q_u_e_n_c_e = std::map; │ │ │ │ │ +_3_8using _S_a_m_p_l_e = std::pair; │ │ │ │ │ +39 │ │ │ │ │ +51template │ │ │ │ │ +_5_2class _F_i_t_B_a_s_i_s { │ │ │ │ │ +53 public: │ │ │ │ │ +54 using Parameters = typename Basis::Parameters; │ │ │ │ │ +55 │ │ │ │ │ +56 private: │ │ │ │ │ +57 Parameters parameters_; │ │ │ │ │ +58 │ │ │ │ │ +59 public: │ │ │ │ │ +_6_1 static _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h _N_o_n_l_i_n_e_a_r_G_r_a_p_h(const _S_e_q_u_e_n_c_e& sequence, │ │ │ │ │ +62 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model, │ │ │ │ │ +63 size_t N) { │ │ │ │ │ +64 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h graph; │ │ │ │ │ +65 for (const _S_a_m_p_l_e sample : sequence) { │ │ │ │ │ +66 graph.emplace_shared<_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r_<_B_a_s_i_s_>>(0, sample.second, model, N, │ │ │ │ │ +67 sample.first); │ │ │ │ │ +68 } │ │ │ │ │ +69 return graph; │ │ │ │ │ +70 } │ │ │ │ │ +71 │ │ │ │ │ +_7_3 static _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r _L_i_n_e_a_r_G_r_a_p_h( │ │ │ │ │ +74 const _S_e_q_u_e_n_c_e& sequence, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model, size_t N) { │ │ │ │ │ +75 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h graph = _N_o_n_l_i_n_e_a_r_G_r_a_p_h(sequence, model, N); │ │ │ │ │ +76 _V_a_l_u_e_s values; │ │ │ │ │ +77 values._i_n_s_e_r_t(0, Parameters::Zero(N)); │ │ │ │ │ +78 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r gfg = graph.linearize(values); │ │ │ │ │ +79 return gfg; │ │ │ │ │ +80 } │ │ │ │ │ +81 │ │ │ │ │ +_8_9 _F_i_t_B_a_s_i_s(const _S_e_q_u_e_n_c_e& sequence, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model, size_t N) │ │ │ │ │ +{ │ │ │ │ │ +90 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r gfg = _L_i_n_e_a_r_G_r_a_p_h(sequence, model, N); │ │ │ │ │ +91 _V_e_c_t_o_r_V_a_l_u_e_s solution = gfg->optimize(); │ │ │ │ │ +92 parameters_ = solution._a_t(0); │ │ │ │ │ +93 } │ │ │ │ │ +94 │ │ │ │ │ +_9_6 Parameters _p_a_r_a_m_e_t_e_r_s() const { return parameters_; } │ │ │ │ │ +97}; │ │ │ │ │ +98 │ │ │ │ │ +99} // namespace gtsam │ │ │ │ │ _B_a_s_i_s_._h │ │ │ │ │ Compute an interpolating basis. │ │ │ │ │ +_B_a_s_i_s_F_a_c_t_o_r_s_._h │ │ │ │ │ +Factor definitions for various Basis functors. │ │ │ │ │ +_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ +Factor Graph Values. │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Factor Graph consisting of non-linear factors. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s │ │ │ │ │ -CRTP Base class for function bases. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_C_h_e_b_y_s_h_e_v_2 │ │ │ │ │ -Chebyshev Interpolation on Chebyshev points of the second kind Note that N │ │ │ │ │ -here, the number of points... │ │ │ │ │ -DDeeffiinniittiioonn Chebyshev2.h:46 │ │ │ │ │ -_g_t_s_a_m_:_:_C_h_e_b_y_s_h_e_v_2_:_:_P_o_i_n_t │ │ │ │ │ -static double Point(size_t N, int j) │ │ │ │ │ -Specific Chebyshev point. │ │ │ │ │ -DDeeffiinniittiioonn Chebyshev2.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_C_h_e_b_y_s_h_e_v_2_:_:_P_o_i_n_t │ │ │ │ │ -static double Point(size_t N, int j, double a, double b) │ │ │ │ │ -Specific Chebyshev point, within [a,b] interval. │ │ │ │ │ -DDeeffiinniittiioonn Chebyshev2.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_C_h_e_b_y_s_h_e_v_2_:_:_P_o_i_n_t_s │ │ │ │ │ -static Vector Points(size_t N) │ │ │ │ │ -All Chebyshev points. │ │ │ │ │ -DDeeffiinniittiioonn Chebyshev2.h:72 │ │ │ │ │ -_g_t_s_a_m_:_:_C_h_e_b_y_s_h_e_v_2_:_:_P_o_i_n_t_s │ │ │ │ │ -static Vector Points(size_t N, double a, double b) │ │ │ │ │ -All Chebyshev points, within [a,b] interval. │ │ │ │ │ -DDeeffiinniittiioonn Chebyshev2.h:79 │ │ │ │ │ -_g_t_s_a_m_:_:_C_h_e_b_y_s_h_e_v_2_:_:_m_a_t_r_i_x │ │ │ │ │ -static Matrix matrix(std::function< Eigen::Matrix< double, M, 1 >(double)> f, │ │ │ │ │ -size_t N, double a=-1, double b=1) │ │ │ │ │ -Create matrix of values at Chebyshev points given vector-valued function. │ │ │ │ │ -DDeeffiinniittiioonn Chebyshev2.h:135 │ │ │ │ │ +_g_t_s_a_m_:_:_S_e_q_u_e_n_c_e │ │ │ │ │ +std::map< double, double > Sequence │ │ │ │ │ +Our sequence representation is a map of {x: y} values where y = f(x) │ │ │ │ │ +DDeeffiinniittiioonn FitBasis.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_S_a_m_p_l_e │ │ │ │ │ +std::pair< double, double > Sample │ │ │ │ │ +A sample is a key-value pair from a sequence. │ │ │ │ │ +DDeeffiinniittiioonn FitBasis.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ +Aliases. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ +_g_t_s_a_m_:_:_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +Factor for enforcing the scalar value of the polynomial BASIS representation at │ │ │ │ │ +x is the same as the ... │ │ │ │ │ +DDeeffiinniittiioonn BasisFactors.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_F_i_t_B_a_s_i_s │ │ │ │ │ +Class that does regression via least squares Example usage: size_t N = 3; auto │ │ │ │ │ +fit = FitBasis shared_ptr │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:81 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ +with a unique integer... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_a_t │ │ │ │ │ +Vector & at(Key j) │ │ │ │ │ +Read/write access to the vector value with key j, throws std::out_of_range if j │ │ │ │ │ +does not exist,... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:139 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_i_n_s_e_r_t │ │ │ │ │ +void insert(Key j, const Value &val) │ │ │ │ │ +Add a variable with the given j, throws KeyAlreadyExists if j is already │ │ │ │ │ +present. │ │ │ │ │ +DDeeffiinniittiioonn Values.cpp:157 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_i_s │ │ │ │ │ - * _C_h_e_b_y_s_h_e_v_2_._h │ │ │ │ │ + * _F_i_t_B_a_s_i_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00164.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Chebyshev2.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Basis.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,36 +94,84 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces
│ │ │ │ -
Chebyshev2.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ +Functions
│ │ │ │ +
Basis.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Chebyshev parameterizations on Chebyshev points of second kind. │ │ │ │ +

Compute an interpolating basis. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::Basis< DERIVED >
 CRTP Base class for function bases. More...
 
class  gtsam::Basis< DERIVED >::EvaluationFunctor
 An instance of an EvaluationFunctor calculates f(x;p) at a given x, applied to Parameters p. More...
 
class  gtsam::Basis< DERIVED >::VectorEvaluationFunctor< M >
 VectorEvaluationFunctor at a given x, applied to ParameterMatrix<M>. More...
 
class  gtsam::Basis< DERIVED >::VectorComponentFunctor< M >
 Given a M*N Matrix of M-vectors at N polynomial points, an instance of VectorComponentFunctor computes the N-vector value for a specific row component of the M-vectors at all the polynomial points. More...
 
class  gtsam::Basis< DERIVED >::ManifoldEvaluationFunctor< T >
 Manifold EvaluationFunctor at a given x, applied to ParameterMatrix<M>. More...
 
class  gtsam::Basis< DERIVED >::DerivativeFunctorBase
 Base class for functors below that calculate derivative weights. More...
 
class  gtsam::Basis< DERIVED >::DerivativeFunctor
 An instance of a DerivativeFunctor calculates f'(x;p) at a given x, applied to Parameters p. More...
 
class  gtsam::Basis< DERIVED >::VectorDerivativeFunctor< M >
 VectorDerivativeFunctor at a given x, applied to ParameterMatrix<M>. More...
 
class  gtsam::Basis< DERIVED >::ComponentDerivativeFunctor< M >
 Given a M*N Matrix of M-vectors at N polynomial points, an instance of ComponentDerivativeFunctor computes the N-vector derivative for a specific row component of the M-vectors at all the polynomial points. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Typedefs

│ │ │ │ +using gtsam::Weights = Eigen::Matrix< double, 1, -1 >
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

template<size_t M>
Matrix gtsam::kroneckerProductIdentity (const Weights &w)
 Function for computing the kronecker product of the 1*N Weight vector w with the MxM identity matrix I efficiently.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Chebyshev parameterizations on Chebyshev points of second kind.

│ │ │ │ +

Compute an interpolating basis.

│ │ │ │
Author
Varun Agrawal, Jing Dong, Frank Dellaert
│ │ │ │
Date
July 4, 2020
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,71 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Chebyshev2.cpp File Reference │ │ │ │ │ -Chebyshev parameterizations on Chebyshev points of second kind. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Basis.h File Reference │ │ │ │ │ +Compute an interpolating basis. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_B_a_s_i_s_<_ _D_E_R_I_V_E_D_ _> │ │ │ │ │ +  CRTP Base class for function bases. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_B_a_s_i_s_<_ _D_E_R_I_V_E_D_ _>_:_:_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r │ │ │ │ │ +  An instance of an _E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r calculates f(x;p) at a given x, │ │ │ │ │ + applied to Parameters p. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_B_a_s_i_s_<_ _D_E_R_I_V_E_D_ _>_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_<_ _M_ _> │ │ │ │ │ +  _V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r at a given x, applied to ParameterMatrix. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_B_a_s_i_s_<_ _D_E_R_I_V_E_D_ _>_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r_<_ _M_ _> │ │ │ │ │ + Given a M*N Matrix of M-vectors at N polynomial points, an instance of │ │ │ │ │ +  _V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r computes the N-vector value for a specific row │ │ │ │ │ + component of the M-vectors at all the polynomial points. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_B_a_s_i_s_<_ _D_E_R_I_V_E_D_ _>_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_<_ _T_ _> │ │ │ │ │ +  Manifold _E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r at a given x, applied to ParameterMatrix. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_B_a_s_i_s_<_ _D_E_R_I_V_E_D_ _>_:_:_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e │ │ │ │ │ +  Base class for functors below that calculate derivative weights. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_B_a_s_i_s_<_ _D_E_R_I_V_E_D_ _>_:_:_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r │ │ │ │ │ +  An instance of a _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r calculates f'(x;p) at a given x, │ │ │ │ │ + applied to Parameters p. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_B_a_s_i_s_<_ _D_E_R_I_V_E_D_ _>_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_<_ _M_ _> │ │ │ │ │ +  _V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r at a given x, applied to ParameterMatrix. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_B_a_s_i_s_<_ _D_E_R_I_V_E_D_ _>_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_<_ _M_ _> │ │ │ │ │ + Given a M*N Matrix of M-vectors at N polynomial points, an instance of │ │ │ │ │ +  _C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r computes the N-vector derivative for a │ │ │ │ │ + specific row component of the M-vectors at all the polynomial points. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +using  ggttssaamm::::WWeeiigghhttss = Eigen::Matrix< double, 1, -1 > │ │ │ │ │ +  │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +template │ │ │ │ │ +Matrix  _g_t_s_a_m_:_:_k_r_o_n_e_c_k_e_r_P_r_o_d_u_c_t_I_d_e_n_t_i_t_y (const Weights &w) │ │ │ │ │ +  Function for computing the kronecker product of the 1*N Weight vector w │ │ │ │ │ + with the MxM identity matrix I efficiently. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Chebyshev parameterizations on Chebyshev points of second kind. │ │ │ │ │ +Compute an interpolating basis. │ │ │ │ │ Author │ │ │ │ │ Varun Agrawal, Jing Dong, Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ July 4, 2020 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_i_s │ │ │ │ │ - * _C_h_e_b_y_s_h_e_v_2_._c_p_p │ │ │ │ │ + * _B_a_s_i_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00167.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/BasisFactors.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Fourier.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,61 +96,43 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
BasisFactors.h File Reference
│ │ │ │ +
Fourier.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Factor definitions for various Basis functors. │ │ │ │ +

Fourier decomposition, see e.g. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::EvaluationFactor< BASIS >
 Factor for enforcing the scalar value of the polynomial BASIS representation at x is the same as the measurement z when using a pseudo-spectral parameterization. More...
 
class  gtsam::VectorEvaluationFactor< BASIS, M >
 Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix of size (M, N) is equal to a vector-valued measurement at the same point, when using a pseudo-spectral parameterization. More...
 
class  gtsam::VectorComponentFactor< BASIS, P >
 Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix of size (P, N) is equal to specified measurement at the same point, when using a pseudo-spectral parameterization. More...
 
class  gtsam::ManifoldEvaluationFactor< BASIS, T >
 For a measurement value of type T i.e. More...
 
class  gtsam::DerivativeFactor< BASIS >
 A unary factor which enforces the evaluation of the derivative of a BASIS polynomial at a specified pointx is equal to the scalar measurement z. More...
 
class  gtsam::VectorDerivativeFactor< BASIS, M >
 A unary factor which enforces the evaluation of the derivative of a BASIS polynomial at a specified point x is equal to the vector value z. More...
 
class  gtsam::ComponentDerivativeFactor< BASIS, P >
 A unary factor which enforces the evaluation of the derivative of a BASIS polynomial is equal to the scalar value at a specific index i of a vector-valued measurement z. More...
class  gtsam::FourierBasis
 Fourier basis. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Factor definitions for various Basis functors.

│ │ │ │ -
Author
Varun Agrawal
│ │ │ │ +

Fourier decomposition, see e.g.

│ │ │ │ +

http://mathworld.wolfram.com/FourierSeries.html

Author
Varun Agrawal, Frank Dellaert
│ │ │ │
Date
July 4, 2020
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,58 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -BasisFactors.h File Reference │ │ │ │ │ -Factor definitions for various Basis functors. _M_o_r_e_._._. │ │ │ │ │ +Fourier.h File Reference │ │ │ │ │ +Fourier decomposition, see e.g. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r_<_ _B_A_S_I_S_ _> │ │ │ │ │ - _F_a_c_t_o_r for enforcing the scalar value of the polynomial BASIS │ │ │ │ │ -  representation at x is the same as the measurement z when using a │ │ │ │ │ - pseudo-spectral parameterization. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r_<_ _B_A_S_I_S_,_ _M_ _> │ │ │ │ │ - Unary factor for enforcing BASIS polynomial evaluation on a │ │ │ │ │ -  _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x of size (M, N) is equal to a vector-valued measurement │ │ │ │ │ - at the same point, when using a pseudo-spectral parameterization. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_a_c_t_o_r_<_ _B_A_S_I_S_,_ _P_ _> │ │ │ │ │ - Unary factor for enforcing BASIS polynomial evaluation on a │ │ │ │ │ -  _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x of size (P, N) is equal to specified measurement at the │ │ │ │ │ - same point, when using a pseudo-spectral parameterization. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r_<_ _B_A_S_I_S_,_ _T_ _> │ │ │ │ │ -  For a measurement value of type T i.e. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r_<_ _B_A_S_I_S_ _> │ │ │ │ │ - A unary factor which enforces the evaluation of the derivative of a │ │ │ │ │ -  BASIS polynomial at a specified pointx is equal to the scalar │ │ │ │ │ - measurement z. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r_<_ _B_A_S_I_S_,_ _M_ _> │ │ │ │ │ - A unary factor which enforces the evaluation of the derivative of a │ │ │ │ │ -  BASIS polynomial at a specified point x is equal to the vector value z. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r_<_ _B_A_S_I_S_,_ _P_ _> │ │ │ │ │ - A unary factor which enforces the evaluation of the derivative of a │ │ │ │ │ -  BASIS polynomial is equal to the scalar value at a specific index i of │ │ │ │ │ - a vector-valued measurement z. _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_F_o_u_r_i_e_r_B_a_s_i_s │ │ │ │ │ +  Fourier basis. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Factor definitions for various Basis functors. │ │ │ │ │ +Fourier decomposition, see e.g. │ │ │ │ │ +_h_t_t_p_:_/_/_m_a_t_h_w_o_r_l_d_._w_o_l_f_r_a_m_._c_o_m_/_F_o_u_r_i_e_r_S_e_r_i_e_s_._h_t_m_l │ │ │ │ │ Author │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + Varun Agrawal, Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ July 4, 2020 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_i_s │ │ │ │ │ - * _B_a_s_i_s_F_a_c_t_o_r_s_._h │ │ │ │ │ + * _F_o_u_r_i_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00167.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,6 +1,3 @@ │ │ │ │ │ var a00167 = [ │ │ │ │ │ - ["gtsam::ManifoldEvaluationFactor< BASIS, T >", "a02716.html", "a02716"], │ │ │ │ │ - ["gtsam::DerivativeFactor< BASIS >", "a02720.html", "a02720"], │ │ │ │ │ - ["gtsam::VectorDerivativeFactor< BASIS, M >", "a02724.html", "a02724"], │ │ │ │ │ - ["gtsam::ComponentDerivativeFactor< BASIS, P >", "a02728.html", "a02728"] │ │ │ │ │ + ["gtsam::FourierBasis", "a02748.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00167_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/BasisFactors.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Fourier.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,267 +98,106 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
BasisFactors.h
│ │ │ │ +
Fourier.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/basis/Basis.h>
│ │ │ │ -
22#include <gtsam/nonlinear/FunctorizedFactor.h>
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/basis/Basis.h>
│ │ │ │
23
│ │ │ │
24namespace gtsam {
│ │ │ │
25
│ │ │ │ -
38template <class BASIS>
│ │ │ │ -
│ │ │ │ -
39class EvaluationFactor : public FunctorizedFactor<double, Vector> {
│ │ │ │ -
40 private:
│ │ │ │ -
41 using Base = FunctorizedFactor<double, Vector>;
│ │ │ │ -
42
│ │ │ │ -
43 public:
│ │ │ │ -
44 EvaluationFactor() {}
│ │ │ │ -
45
│ │ │ │ -
│ │ │ │ -
55 EvaluationFactor(Key key, double z, const SharedNoiseModel &model,
│ │ │ │ -
56 const size_t N, double x)
│ │ │ │ -
57 : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x)) {}
│ │ │ │ -
│ │ │ │ -
58
│ │ │ │ -
│ │ │ │ -
70 EvaluationFactor(Key key, double z, const SharedNoiseModel &model,
│ │ │ │ -
71 const size_t N, double x, double a, double b)
│ │ │ │ -
72 : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x, a, b)) {}
│ │ │ │ -
│ │ │ │ -
73
│ │ │ │ -
74 virtual ~EvaluationFactor() {}
│ │ │ │ -
75};
│ │ │ │ -
│ │ │ │ -
76
│ │ │ │ -
94template <class BASIS, int M>
│ │ │ │ -
│ │ │ │ -
95class VectorEvaluationFactor
│ │ │ │ -
96 : public FunctorizedFactor<Vector, ParameterMatrix<M>> {
│ │ │ │ -
97 private:
│ │ │ │ -
98 using Base = FunctorizedFactor<Vector, ParameterMatrix<M>>;
│ │ │ │ -
99
│ │ │ │ -
100 public:
│ │ │ │ -
101 VectorEvaluationFactor() {}
│ │ │ │ -
102
│ │ │ │ -
│ │ │ │ -
113 VectorEvaluationFactor(Key key, const Vector &z,
│ │ │ │ -
114 const SharedNoiseModel &model, const size_t N,
│ │ │ │ -
115 double x)
│ │ │ │ -
116 : Base(key, z, model,
│ │ │ │ -
117 typename BASIS::template VectorEvaluationFunctor<M>(N, x)) {}
│ │ │ │ -
│ │ │ │ -
118
│ │ │ │ -
│ │ │ │ -
131 VectorEvaluationFactor(Key key, const Vector &z,
│ │ │ │ -
132 const SharedNoiseModel &model, const size_t N,
│ │ │ │ -
133 double x, double a, double b)
│ │ │ │ -
134 : Base(key, z, model,
│ │ │ │ -
135 typename BASIS::template VectorEvaluationFunctor<M>(N, x, a, b)) {}
│ │ │ │ -
│ │ │ │ -
136
│ │ │ │ -
137 virtual ~VectorEvaluationFactor() {}
│ │ │ │ -
138};
│ │ │ │ -
│ │ │ │ -
139
│ │ │ │ -
159template <class BASIS, size_t P>
│ │ │ │ -
│ │ │ │ -
160class VectorComponentFactor
│ │ │ │ -
161 : public FunctorizedFactor<double, ParameterMatrix<P>> {
│ │ │ │ -
162 private:
│ │ │ │ -
163 using Base = FunctorizedFactor<double, ParameterMatrix<P>>;
│ │ │ │ -
164
│ │ │ │ -
165 public:
│ │ │ │ -
166 VectorComponentFactor() {}
│ │ │ │ -
167
│ │ │ │ -
│ │ │ │ -
181 VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model,
│ │ │ │ -
182 const size_t N, size_t i, double x)
│ │ │ │ -
183 : Base(key, z, model,
│ │ │ │ -
184 typename BASIS::template VectorComponentFunctor<P>(N, i, x)) {}
│ │ │ │ -
│ │ │ │ -
185
│ │ │ │ -
│ │ │ │ -
201 VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model,
│ │ │ │ -
202 const size_t N, size_t i, double x, double a, double b)
│ │ │ │ -
203 : Base(
│ │ │ │ -
204 key, z, model,
│ │ │ │ -
205 typename BASIS::template VectorComponentFunctor<P>(N, i, x, a, b)) {
│ │ │ │ -
206 }
│ │ │ │ -
│ │ │ │ -
207
│ │ │ │ -
208 virtual ~VectorComponentFactor() {}
│ │ │ │ -
209};
│ │ │ │ -
│ │ │ │ -
210
│ │ │ │ -
228template <class BASIS, typename T>
│ │ │ │ -
│ │ │ │ -
229class ManifoldEvaluationFactor
│ │ │ │ -
230 : public FunctorizedFactor<T, ParameterMatrix<traits<T>::dimension>> {
│ │ │ │ -
231 private:
│ │ │ │ -
232 using Base = FunctorizedFactor<T, ParameterMatrix<traits<T>::dimension>>;
│ │ │ │ -
233
│ │ │ │ -
234 public:
│ │ │ │ -
235 ManifoldEvaluationFactor() {}
│ │ │ │ -
236
│ │ │ │ -
│ │ │ │ -
247 ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model,
│ │ │ │ -
248 const size_t N, double x)
│ │ │ │ -
249 : Base(key, z, model,
│ │ │ │ -
250 typename BASIS::template ManifoldEvaluationFunctor<T>(N, x)) {}
│ │ │ │ -
│ │ │ │ -
251
│ │ │ │ -
│ │ │ │ -
264 ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model,
│ │ │ │ -
265 const size_t N, double x, double a, double b)
│ │ │ │ -
266 : Base(
│ │ │ │ -
267 key, z, model,
│ │ │ │ -
268 typename BASIS::template ManifoldEvaluationFunctor<T>(N, x, a, b)) {
│ │ │ │ -
269 }
│ │ │ │ -
│ │ │ │ -
270
│ │ │ │ -
271 virtual ~ManifoldEvaluationFactor() {}
│ │ │ │ -
272};
│ │ │ │ -
│ │ │ │ -
273
│ │ │ │ -
280template <class BASIS>
│ │ │ │ -
│ │ │ │ -
281class DerivativeFactor
│ │ │ │ -
282 : public FunctorizedFactor<double, typename BASIS::Parameters> {
│ │ │ │ -
283 private:
│ │ │ │ -
284 using Base = FunctorizedFactor<double, typename BASIS::Parameters>;
│ │ │ │ -
285
│ │ │ │ -
286 public:
│ │ │ │ -
287 DerivativeFactor() {}
│ │ │ │ -
288
│ │ │ │ -
│ │ │ │ -
299 DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model,
│ │ │ │ -
300 const size_t N, double x)
│ │ │ │ -
301 : Base(key, z, model, typename BASIS::DerivativeFunctor(N, x)) {}
│ │ │ │ -
│ │ │ │ -
302
│ │ │ │ -
│ │ │ │ -
315 DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model,
│ │ │ │ -
316 const size_t N, double x, double a, double b)
│ │ │ │ -
317 : Base(key, z, model, typename BASIS::DerivativeFunctor(N, x, a, b)) {}
│ │ │ │ -
│ │ │ │ -
318
│ │ │ │ -
319 virtual ~DerivativeFactor() {}
│ │ │ │ -
320};
│ │ │ │ -
│ │ │ │ -
321
│ │ │ │ -
329template <class BASIS, int M>
│ │ │ │ -
│ │ │ │ -
330class VectorDerivativeFactor
│ │ │ │ -
331 : public FunctorizedFactor<Vector, ParameterMatrix<M>> {
│ │ │ │ -
332 private:
│ │ │ │ -
333 using Base = FunctorizedFactor<Vector, ParameterMatrix<M>>;
│ │ │ │ -
334 using Func = typename BASIS::template VectorDerivativeFunctor<M>;
│ │ │ │ -
335
│ │ │ │ -
336 public:
│ │ │ │ -
337 VectorDerivativeFactor() {}
│ │ │ │ -
338
│ │ │ │ -
│ │ │ │ -
349 VectorDerivativeFactor(Key key, const Vector &z,
│ │ │ │ -
350 const SharedNoiseModel &model, const size_t N,
│ │ │ │ -
351 double x)
│ │ │ │ -
352 : Base(key, z, model, Func(N, x)) {}
│ │ │ │ -
│ │ │ │ -
353
│ │ │ │ -
│ │ │ │ -
366 VectorDerivativeFactor(Key key, const Vector &z,
│ │ │ │ -
367 const SharedNoiseModel &model, const size_t N,
│ │ │ │ -
368 double x, double a, double b)
│ │ │ │ -
369 : Base(key, z, model, Func(N, x, a, b)) {}
│ │ │ │ -
│ │ │ │ -
370
│ │ │ │ -
371 virtual ~VectorDerivativeFactor() {}
│ │ │ │ -
372};
│ │ │ │ -
│ │ │ │ -
373
│ │ │ │ -
382template <class BASIS, int P>
│ │ │ │ -
│ │ │ │ -
383class ComponentDerivativeFactor
│ │ │ │ -
384 : public FunctorizedFactor<double, ParameterMatrix<P>> {
│ │ │ │ -
385 private:
│ │ │ │ -
386 using Base = FunctorizedFactor<double, ParameterMatrix<P>>;
│ │ │ │ -
387 using Func = typename BASIS::template ComponentDerivativeFunctor<P>;
│ │ │ │ -
388
│ │ │ │ -
389 public:
│ │ │ │ -
390 ComponentDerivativeFactor() {}
│ │ │ │ -
391
│ │ │ │ -
│ │ │ │ -
405 ComponentDerivativeFactor(Key key, const double &z,
│ │ │ │ -
406 const SharedNoiseModel &model, const size_t N,
│ │ │ │ -
407 size_t i, double x)
│ │ │ │ -
408 : Base(key, z, model, Func(N, i, x)) {}
│ │ │ │ -
│ │ │ │ -
409
│ │ │ │ -
│ │ │ │ -
425 ComponentDerivativeFactor(Key key, const double &z,
│ │ │ │ -
426 const SharedNoiseModel &model, const size_t N,
│ │ │ │ -
427 size_t i, double x, double a, double b)
│ │ │ │ -
428 : Base(key, z, model, Func(N, i, x, a, b)) {}
│ │ │ │ -
│ │ │ │ -
429
│ │ │ │ -
430 virtual ~ComponentDerivativeFactor() {}
│ │ │ │ -
431};
│ │ │ │ -
│ │ │ │ -
432
│ │ │ │ -
433} // namespace gtsam
│ │ │ │ -
Basis.h
Compute an interpolating basis.
│ │ │ │ - │ │ │ │ +
│ │ │ │ +
27class FourierBasis : public Basis<FourierBasis> {
│ │ │ │ +
28 public:
│ │ │ │ +
29 using Parameters = Eigen::Matrix<double, /*Nx1*/ -1, 1>;
│ │ │ │ +
30 using DiffMatrix = Eigen::Matrix<double, /*NxN*/ -1, -1>;
│ │ │ │ +
31
│ │ │ │ +
│ │ │ │ +
40 static Weights CalculateWeights(size_t N, double x) {
│ │ │ │ +
41 Weights b(N);
│ │ │ │ +
42 b[0] = 1;
│ │ │ │ +
43 for (size_t i = 1, n = 1; i < N; i++) {
│ │ │ │ +
44 if (i % 2 == 1) {
│ │ │ │ +
45 b[i] = cos(n * x);
│ │ │ │ +
46 } else {
│ │ │ │ +
47 b[i] = sin(n * x);
│ │ │ │ +
48 n++;
│ │ │ │ +
49 }
│ │ │ │ +
50 }
│ │ │ │ +
51 return b;
│ │ │ │ +
52 }
│ │ │ │ +
│ │ │ │ +
53
│ │ │ │ +
│ │ │ │ +
64 static Weights CalculateWeights(size_t N, double x, double a, double b) {
│ │ │ │ +
65 // TODO(Varun) How do we enforce an interval for Fourier series?
│ │ │ │ +
66 return CalculateWeights(N, x);
│ │ │ │ +
67 }
│ │ │ │ +
│ │ │ │ +
68
│ │ │ │ +
│ │ │ │ +
73 static DiffMatrix DifferentiationMatrix(size_t N) {
│ │ │ │ +
74 DiffMatrix D = DiffMatrix::Zero(N, N);
│ │ │ │ +
75 double k = 1;
│ │ │ │ +
76 for (size_t i = 1; i < N; i += 2) {
│ │ │ │ +
77 D(i, i + 1) = k; // sin'(k*x) = k*cos(k*x)
│ │ │ │ +
78 D(i + 1, i) = -k; // cos'(k*x) = -k*sin(k*x)
│ │ │ │ +
79 k += 1;
│ │ │ │ +
80 }
│ │ │ │ +
81
│ │ │ │ +
82 return D;
│ │ │ │ +
83 }
│ │ │ │ +
│ │ │ │ +
84
│ │ │ │ +
│ │ │ │ +
92 static Weights DerivativeWeights(size_t N, double x) {
│ │ │ │ +
93 return CalculateWeights(N, x) * DifferentiationMatrix(N);
│ │ │ │ +
94 }
│ │ │ │ +
│ │ │ │ +
95
│ │ │ │ +
│ │ │ │ +
106 static Weights DerivativeWeights(size_t N, double x, double a, double b) {
│ │ │ │ +
107 return CalculateWeights(N, x, a, b) * DifferentiationMatrix(N);
│ │ │ │ +
108 }
│ │ │ │ +
│ │ │ │ +
109
│ │ │ │ +
110}; // FourierBasis
│ │ │ │ +
│ │ │ │ +
111
│ │ │ │ +
112} // namespace gtsam
│ │ │ │ +
Basis.h
Compute an interpolating basis.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::EvaluationFactor
Factor for enforcing the scalar value of the polynomial BASIS representation at x is the same as the ...
Definition BasisFactors.h:39
│ │ │ │ -
gtsam::EvaluationFactor::EvaluationFactor
EvaluationFactor(Key key, double z, const SharedNoiseModel &model, const size_t N, double x)
Construct a new EvaluationFactor object.
Definition BasisFactors.h:55
│ │ │ │ -
gtsam::EvaluationFactor::EvaluationFactor
EvaluationFactor(Key key, double z, const SharedNoiseModel &model, const size_t N, double x, double a, double b)
Construct a new EvaluationFactor object.
Definition BasisFactors.h:70
│ │ │ │ -
gtsam::VectorEvaluationFactor
Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix of size (M,...
Definition BasisFactors.h:96
│ │ │ │ -
gtsam::VectorEvaluationFactor::VectorEvaluationFactor
VectorEvaluationFactor(Key key, const Vector &z, const SharedNoiseModel &model, const size_t N, double x, double a, double b)
Construct a new VectorEvaluationFactor object.
Definition BasisFactors.h:131
│ │ │ │ -
gtsam::VectorEvaluationFactor::VectorEvaluationFactor
VectorEvaluationFactor(Key key, const Vector &z, const SharedNoiseModel &model, const size_t N, double x)
Construct a new VectorEvaluationFactor object.
Definition BasisFactors.h:113
│ │ │ │ -
gtsam::VectorComponentFactor
Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix of size (P,...
Definition BasisFactors.h:161
│ │ │ │ -
gtsam::VectorComponentFactor::VectorComponentFactor
VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model, const size_t N, size_t i, double x, double a, double b)
Construct a new VectorComponentFactor object.
Definition BasisFactors.h:201
│ │ │ │ -
gtsam::VectorComponentFactor::VectorComponentFactor
VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model, const size_t N, size_t i, double x)
Construct a new VectorComponentFactor object.
Definition BasisFactors.h:181
│ │ │ │ -
gtsam::ManifoldEvaluationFactor
For a measurement value of type T i.e.
Definition BasisFactors.h:230
│ │ │ │ -
gtsam::ManifoldEvaluationFactor::ManifoldEvaluationFactor
ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model, const size_t N, double x, double a, double b)
Construct a new ManifoldEvaluationFactor object.
Definition BasisFactors.h:264
│ │ │ │ -
gtsam::ManifoldEvaluationFactor::ManifoldEvaluationFactor
ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model, const size_t N, double x)
Construct a new ManifoldEvaluationFactor object.
Definition BasisFactors.h:247
│ │ │ │ -
gtsam::DerivativeFactor
A unary factor which enforces the evaluation of the derivative of a BASIS polynomial at a specified p...
Definition BasisFactors.h:282
│ │ │ │ -
gtsam::DerivativeFactor::DerivativeFactor
DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model, const size_t N, double x, double a, double b)
Construct a new DerivativeFactor object.
Definition BasisFactors.h:315
│ │ │ │ -
gtsam::DerivativeFactor::DerivativeFactor
DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model, const size_t N, double x)
Construct a new DerivativeFactor object.
Definition BasisFactors.h:299
│ │ │ │ -
gtsam::VectorDerivativeFactor
A unary factor which enforces the evaluation of the derivative of a BASIS polynomial at a specified p...
Definition BasisFactors.h:331
│ │ │ │ -
gtsam::VectorDerivativeFactor::VectorDerivativeFactor
VectorDerivativeFactor(Key key, const Vector &z, const SharedNoiseModel &model, const size_t N, double x)
Construct a new VectorDerivativeFactor object.
Definition BasisFactors.h:349
│ │ │ │ -
gtsam::VectorDerivativeFactor::VectorDerivativeFactor
VectorDerivativeFactor(Key key, const Vector &z, const SharedNoiseModel &model, const size_t N, double x, double a, double b)
Construct a new VectorDerivativeFactor object.
Definition BasisFactors.h:366
│ │ │ │ -
gtsam::ComponentDerivativeFactor
A unary factor which enforces the evaluation of the derivative of a BASIS polynomial is equal to the ...
Definition BasisFactors.h:384
│ │ │ │ -
gtsam::ComponentDerivativeFactor::ComponentDerivativeFactor
ComponentDerivativeFactor(Key key, const double &z, const SharedNoiseModel &model, const size_t N, size_t i, double x, double a, double b)
Construct a new ComponentDerivativeFactor object.
Definition BasisFactors.h:425
│ │ │ │ -
gtsam::ComponentDerivativeFactor::ComponentDerivativeFactor
ComponentDerivativeFactor(Key key, const double &z, const SharedNoiseModel &model, const size_t N, size_t i, double x)
Construct a new ComponentDerivativeFactor object.
Definition BasisFactors.h:405
│ │ │ │ -
gtsam::FunctorizedFactor
Factor which evaluates provided unary functor and uses the result to compute error with respect to th...
Definition FunctorizedFactor.h:59
│ │ │ │ -
gtsam::NoiseModelFactorN::key
Key key() const
Returns a key.
Definition NonlinearFactor.h:518
│ │ │ │ +
gtsam::Basis
CRTP Base class for function bases.
Definition Basis.h:100
│ │ │ │ +
gtsam::FourierBasis
Fourier basis.
Definition Fourier.h:27
│ │ │ │ +
gtsam::FourierBasis::DifferentiationMatrix
static DiffMatrix DifferentiationMatrix(size_t N)
Compute D = differentiation matrix.
Definition Fourier.h:73
│ │ │ │ +
gtsam::FourierBasis::DerivativeWeights
static Weights DerivativeWeights(size_t N, double x)
Get weights at a given x that calculate the derivative.
Definition Fourier.h:92
│ │ │ │ +
gtsam::FourierBasis::CalculateWeights
static Weights CalculateWeights(size_t N, double x)
Evaluate Real Fourier Weights of size N in interval [a, b], e.g.
Definition Fourier.h:40
│ │ │ │ +
gtsam::FourierBasis::DerivativeWeights
static Weights DerivativeWeights(size_t N, double x, double a, double b)
Get derivative weights at a given x that calculate the derivative, in the interval [a,...
Definition Fourier.h:106
│ │ │ │ +
gtsam::FourierBasis::CalculateWeights
static Weights CalculateWeights(size_t N, double x, double a, double b)
Evaluate Real Fourier Weights of size N in interval [a, b], e.g.
Definition Fourier.h:64
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,312 +1,109 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -BasisFactors.h │ │ │ │ │ +Fourier.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_b_a_s_i_s_/_B_a_s_i_s_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_._h> │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_i_s_/_B_a_s_i_s_._h> │ │ │ │ │ 23 │ │ │ │ │ 24namespace _g_t_s_a_m { │ │ │ │ │ 25 │ │ │ │ │ -38template │ │ │ │ │ -_3_9class _E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r : public _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r { │ │ │ │ │ -40 private: │ │ │ │ │ -41 using _B_a_s_e = _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_<_d_o_u_b_l_e_,_ _V_e_c_t_o_r_>; │ │ │ │ │ -42 │ │ │ │ │ -43 public: │ │ │ │ │ -44 _E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r() {} │ │ │ │ │ -45 │ │ │ │ │ -_5_5 _E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r(_K_e_y _k_e_y, double z, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, │ │ │ │ │ -56 const size_t N, double x) │ │ │ │ │ -57 : _B_a_s_e(_k_e_y, z, model, typename BASIS::EvaluationFunctor(N, x)) {} │ │ │ │ │ -58 │ │ │ │ │ -_7_0 _E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r(_K_e_y _k_e_y, double z, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, │ │ │ │ │ -71 const size_t N, double x, double a, double b) │ │ │ │ │ -72 : _B_a_s_e(_k_e_y, z, model, typename BASIS::EvaluationFunctor(N, x, a, b)) {} │ │ │ │ │ -73 │ │ │ │ │ -74 virtual _~_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r() {} │ │ │ │ │ -75}; │ │ │ │ │ -76 │ │ │ │ │ -94template │ │ │ │ │ -_9_5class _V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -96 : public _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r> { │ │ │ │ │ -97 private: │ │ │ │ │ -98 using _B_a_s_e = _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_<_V_e_c_t_o_r_,_ _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>>; │ │ │ │ │ -99 │ │ │ │ │ -100 public: │ │ │ │ │ -101 _V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r() {} │ │ │ │ │ -102 │ │ │ │ │ -_1_1_3 _V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r(_K_e_y _k_e_y, const Vector &z, │ │ │ │ │ -114 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, const size_t N, │ │ │ │ │ -115 double x) │ │ │ │ │ -116 : _B_a_s_e(_k_e_y, z, model, │ │ │ │ │ -117 typename BASIS::template VectorEvaluationFunctor(N, x)) {} │ │ │ │ │ -118 │ │ │ │ │ -_1_3_1 _V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r(_K_e_y _k_e_y, const Vector &z, │ │ │ │ │ -132 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, const size_t N, │ │ │ │ │ -133 double x, double a, double b) │ │ │ │ │ -134 : _B_a_s_e(_k_e_y, z, model, │ │ │ │ │ -135 typename BASIS::template VectorEvaluationFunctor(N, x, a, b)) {} │ │ │ │ │ -136 │ │ │ │ │ -137 virtual _~_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r() {} │ │ │ │ │ -138}; │ │ │ │ │ -139 │ │ │ │ │ -159template │ │ │ │ │ -_1_6_0class _V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_a_c_t_o_r │ │ │ │ │ -161 : public _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r> { │ │ │ │ │ -162 private: │ │ │ │ │ -163 using _B_a_s_e = _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_<_d_o_u_b_l_e_,_ _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_P_>>; │ │ │ │ │ -164 │ │ │ │ │ -165 public: │ │ │ │ │ -166 _V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_a_c_t_o_r() {} │ │ │ │ │ -167 │ │ │ │ │ -_1_8_1 _V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_a_c_t_o_r(_K_e_y _k_e_y, const double &z, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -&model, │ │ │ │ │ -182 const size_t N, size_t i, double x) │ │ │ │ │ -183 : _B_a_s_e(_k_e_y, z, model, │ │ │ │ │ -184 typename BASIS::template VectorComponentFunctor

(N, i, x)) {} │ │ │ │ │ -185 │ │ │ │ │ -_2_0_1 _V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_a_c_t_o_r(_K_e_y _k_e_y, const double &z, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -&model, │ │ │ │ │ -202 const size_t N, size_t i, double x, double a, double b) │ │ │ │ │ -203 : _B_a_s_e( │ │ │ │ │ -204 _k_e_y, z, model, │ │ │ │ │ -205 typename BASIS::template VectorComponentFunctor

(N, i, x, a, b)) { │ │ │ │ │ -206 } │ │ │ │ │ -207 │ │ │ │ │ -208 virtual _~_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_a_c_t_o_r() {} │ │ │ │ │ -209}; │ │ │ │ │ -210 │ │ │ │ │ -228template │ │ │ │ │ -_2_2_9class _M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -230 : public _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r::dimension>> { │ │ │ │ │ -231 private: │ │ │ │ │ -232 using _B_a_s_e = _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_<_T_,_ _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_t_r_a_i_t_s_<_T_>_:_:_d_i_m_e_n_s_i_o_n>>; │ │ │ │ │ -233 │ │ │ │ │ -234 public: │ │ │ │ │ -235 _M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r() {} │ │ │ │ │ -236 │ │ │ │ │ -_2_4_7 _M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r(_K_e_y _k_e_y, const T &z, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -&model, │ │ │ │ │ -248 const size_t N, double x) │ │ │ │ │ -249 : _B_a_s_e(_k_e_y, z, model, │ │ │ │ │ -250 typename BASIS::template ManifoldEvaluationFunctor(N, x)) {} │ │ │ │ │ -251 │ │ │ │ │ -_2_6_4 _M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r(_K_e_y _k_e_y, const T &z, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -&model, │ │ │ │ │ -265 const size_t N, double x, double a, double b) │ │ │ │ │ -266 : _B_a_s_e( │ │ │ │ │ -267 _k_e_y, z, model, │ │ │ │ │ -268 typename BASIS::template ManifoldEvaluationFunctor(N, x, a, b)) { │ │ │ │ │ -269 } │ │ │ │ │ -270 │ │ │ │ │ -271 virtual _~_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r() {} │ │ │ │ │ -272}; │ │ │ │ │ -273 │ │ │ │ │ -280template │ │ │ │ │ -_2_8_1class _D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r │ │ │ │ │ -282 : public _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r { │ │ │ │ │ -283 private: │ │ │ │ │ -284 using Base = _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_<_d_o_u_b_l_e_,_ _t_y_p_e_n_a_m_e_ _B_A_S_I_S_:_:_P_a_r_a_m_e_t_e_r_s_>; │ │ │ │ │ -285 │ │ │ │ │ -286 public: │ │ │ │ │ -287 _D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r() {} │ │ │ │ │ -288 │ │ │ │ │ -_2_9_9 _D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r(_K_e_y _k_e_y, const double &z, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, │ │ │ │ │ -300 const size_t N, double x) │ │ │ │ │ -301 : Base(_k_e_y, z, model, typename BASIS::DerivativeFunctor(N, x)) {} │ │ │ │ │ -302 │ │ │ │ │ -_3_1_5 _D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r(_K_e_y _k_e_y, const double &z, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, │ │ │ │ │ -316 const size_t N, double x, double a, double b) │ │ │ │ │ -317 : Base(_k_e_y, z, model, typename BASIS::DerivativeFunctor(N, x, a, b)) {} │ │ │ │ │ -318 │ │ │ │ │ -319 virtual _~_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r() {} │ │ │ │ │ -320}; │ │ │ │ │ -321 │ │ │ │ │ -329template │ │ │ │ │ -_3_3_0class _V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r │ │ │ │ │ -331 : public _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r> { │ │ │ │ │ -332 private: │ │ │ │ │ -333 using _B_a_s_e = _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_<_V_e_c_t_o_r_,_ _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>>; │ │ │ │ │ -334 using Func = typename BASIS::template VectorDerivativeFunctor; │ │ │ │ │ -335 │ │ │ │ │ -336 public: │ │ │ │ │ -337 _V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r() {} │ │ │ │ │ -338 │ │ │ │ │ -_3_4_9 _V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r(_K_e_y _k_e_y, const Vector &z, │ │ │ │ │ -350 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, const size_t N, │ │ │ │ │ -351 double x) │ │ │ │ │ -352 : _B_a_s_e(_k_e_y, z, model, Func(N, x)) {} │ │ │ │ │ -353 │ │ │ │ │ -_3_6_6 _V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r(_K_e_y _k_e_y, const Vector &z, │ │ │ │ │ -367 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, const size_t N, │ │ │ │ │ -368 double x, double a, double b) │ │ │ │ │ -369 : _B_a_s_e(_k_e_y, z, model, Func(N, x, a, b)) {} │ │ │ │ │ -370 │ │ │ │ │ -371 virtual _~_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r() {} │ │ │ │ │ -372}; │ │ │ │ │ -373 │ │ │ │ │ -382template │ │ │ │ │ -_3_8_3class _C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r │ │ │ │ │ -384 : public _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r> { │ │ │ │ │ -385 private: │ │ │ │ │ -386 using _B_a_s_e = _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_<_d_o_u_b_l_e_,_ _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_P_>>; │ │ │ │ │ -387 using Func = typename BASIS::template ComponentDerivativeFunctor

; │ │ │ │ │ -388 │ │ │ │ │ -389 public: │ │ │ │ │ -390 _C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r() {} │ │ │ │ │ -391 │ │ │ │ │ -_4_0_5 _C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r(_K_e_y _k_e_y, const double &z, │ │ │ │ │ -406 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, const size_t N, │ │ │ │ │ -407 size_t i, double x) │ │ │ │ │ -408 : _B_a_s_e(_k_e_y, z, model, Func(N, i, x)) {} │ │ │ │ │ -409 │ │ │ │ │ -_4_2_5 _C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r(_K_e_y _k_e_y, const double &z, │ │ │ │ │ -426 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, const size_t N, │ │ │ │ │ -427 size_t i, double x, double a, double b) │ │ │ │ │ -428 : _B_a_s_e(_k_e_y, z, model, Func(N, i, x, a, b)) {} │ │ │ │ │ -429 │ │ │ │ │ -430 virtual _~_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r() {} │ │ │ │ │ -431}; │ │ │ │ │ -432 │ │ │ │ │ -433} // namespace gtsam │ │ │ │ │ +_2_7class _F_o_u_r_i_e_r_B_a_s_i_s : public _B_a_s_i_s { │ │ │ │ │ +28 public: │ │ │ │ │ +29 using Parameters = Eigen::Matrix; │ │ │ │ │ +30 using DiffMatrix = Eigen::Matrix; │ │ │ │ │ +31 │ │ │ │ │ +_4_0 static Weights _C_a_l_c_u_l_a_t_e_W_e_i_g_h_t_s(size_t N, double x) { │ │ │ │ │ +41 Weights b(N); │ │ │ │ │ +42 b[0] = 1; │ │ │ │ │ +43 for (size_t i = 1, n = 1; i < N; i++) { │ │ │ │ │ +44 if (i % 2 == 1) { │ │ │ │ │ +45 b[i] = cos(n * x); │ │ │ │ │ +46 } else { │ │ │ │ │ +47 b[i] = sin(n * x); │ │ │ │ │ +48 n++; │ │ │ │ │ +49 } │ │ │ │ │ +50 } │ │ │ │ │ +51 return b; │ │ │ │ │ +52 } │ │ │ │ │ +53 │ │ │ │ │ +_6_4 static Weights _C_a_l_c_u_l_a_t_e_W_e_i_g_h_t_s(size_t N, double x, double a, double b) { │ │ │ │ │ +65 // TODO(Varun) How do we enforce an interval for Fourier series? │ │ │ │ │ +66 return _C_a_l_c_u_l_a_t_e_W_e_i_g_h_t_s(N, x); │ │ │ │ │ +67 } │ │ │ │ │ +68 │ │ │ │ │ +_7_3 static DiffMatrix _D_i_f_f_e_r_e_n_t_i_a_t_i_o_n_M_a_t_r_i_x(size_t N) { │ │ │ │ │ +74 DiffMatrix D = DiffMatrix::Zero(N, N); │ │ │ │ │ +75 double k = 1; │ │ │ │ │ +76 for (size_t i = 1; i < N; i += 2) { │ │ │ │ │ +77 D(i, i + 1) = k; // sin'(k*x) = k*cos(k*x) │ │ │ │ │ +78 D(i + 1, i) = -k; // cos'(k*x) = -k*sin(k*x) │ │ │ │ │ +79 k += 1; │ │ │ │ │ +80 } │ │ │ │ │ +81 │ │ │ │ │ +82 return D; │ │ │ │ │ +83 } │ │ │ │ │ +84 │ │ │ │ │ +_9_2 static Weights _D_e_r_i_v_a_t_i_v_e_W_e_i_g_h_t_s(size_t N, double x) { │ │ │ │ │ +93 return _C_a_l_c_u_l_a_t_e_W_e_i_g_h_t_s(N, x) * _D_i_f_f_e_r_e_n_t_i_a_t_i_o_n_M_a_t_r_i_x(N); │ │ │ │ │ +94 } │ │ │ │ │ +95 │ │ │ │ │ +_1_0_6 static Weights _D_e_r_i_v_a_t_i_v_e_W_e_i_g_h_t_s(size_t N, double x, double a, double b) { │ │ │ │ │ +107 return _C_a_l_c_u_l_a_t_e_W_e_i_g_h_t_s(N, x, a, b) * _D_i_f_f_e_r_e_n_t_i_a_t_i_o_n_M_a_t_r_i_x(N); │ │ │ │ │ +108 } │ │ │ │ │ +109 │ │ │ │ │ +110}; // FourierBasis │ │ │ │ │ +111 │ │ │ │ │ +112} // namespace gtsam │ │ │ │ │ _B_a_s_i_s_._h │ │ │ │ │ Compute an interpolating basis. │ │ │ │ │ -_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ -Aliases. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -Factor for enforcing the scalar value of the polynomial BASIS representation at │ │ │ │ │ -x is the same as the ... │ │ │ │ │ -DDeeffiinniittiioonn BasisFactors.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r_:_:_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -EvaluationFactor(Key key, double z, const SharedNoiseModel &model, const size_t │ │ │ │ │ -N, double x) │ │ │ │ │ -Construct a new EvaluationFactor object. │ │ │ │ │ -DDeeffiinniittiioonn BasisFactors.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r_:_:_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -EvaluationFactor(Key key, double z, const SharedNoiseModel &model, const size_t │ │ │ │ │ -N, double x, double a, double b) │ │ │ │ │ -Construct a new EvaluationFactor object. │ │ │ │ │ -DDeeffiinniittiioonn BasisFactors.h:70 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix of │ │ │ │ │ -size (M,... │ │ │ │ │ -DDeeffiinniittiioonn BasisFactors.h:96 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -VectorEvaluationFactor(Key key, const Vector &z, const SharedNoiseModel &model, │ │ │ │ │ -const size_t N, double x, double a, double b) │ │ │ │ │ -Construct a new VectorEvaluationFactor object. │ │ │ │ │ -DDeeffiinniittiioonn BasisFactors.h:131 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -VectorEvaluationFactor(Key key, const Vector &z, const SharedNoiseModel &model, │ │ │ │ │ -const size_t N, double x) │ │ │ │ │ -Construct a new VectorEvaluationFactor object. │ │ │ │ │ -DDeeffiinniittiioonn BasisFactors.h:113 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_a_c_t_o_r │ │ │ │ │ -Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix of │ │ │ │ │ -size (P,... │ │ │ │ │ -DDeeffiinniittiioonn BasisFactors.h:161 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_a_c_t_o_r_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_a_c_t_o_r │ │ │ │ │ -VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model, │ │ │ │ │ -const size_t N, size_t i, double x, double a, double b) │ │ │ │ │ -Construct a new VectorComponentFactor object. │ │ │ │ │ -DDeeffiinniittiioonn BasisFactors.h:201 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_a_c_t_o_r_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_a_c_t_o_r │ │ │ │ │ -VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model, │ │ │ │ │ -const size_t N, size_t i, double x) │ │ │ │ │ -Construct a new VectorComponentFactor object. │ │ │ │ │ -DDeeffiinniittiioonn BasisFactors.h:181 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -For a measurement value of type T i.e. │ │ │ │ │ -DDeeffiinniittiioonn BasisFactors.h:230 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model, │ │ │ │ │ -const size_t N, double x, double a, double b) │ │ │ │ │ -Construct a new ManifoldEvaluationFactor object. │ │ │ │ │ -DDeeffiinniittiioonn BasisFactors.h:264 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model, │ │ │ │ │ -const size_t N, double x) │ │ │ │ │ -Construct a new ManifoldEvaluationFactor object. │ │ │ │ │ -DDeeffiinniittiioonn BasisFactors.h:247 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r │ │ │ │ │ -A unary factor which enforces the evaluation of the derivative of a BASIS │ │ │ │ │ -polynomial at a specified p... │ │ │ │ │ -DDeeffiinniittiioonn BasisFactors.h:282 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r_:_:_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r │ │ │ │ │ -DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model, const │ │ │ │ │ -size_t N, double x, double a, double b) │ │ │ │ │ -Construct a new DerivativeFactor object. │ │ │ │ │ -DDeeffiinniittiioonn BasisFactors.h:315 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r_:_:_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r │ │ │ │ │ -DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model, const │ │ │ │ │ -size_t N, double x) │ │ │ │ │ -Construct a new DerivativeFactor object. │ │ │ │ │ -DDeeffiinniittiioonn BasisFactors.h:299 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r │ │ │ │ │ -A unary factor which enforces the evaluation of the derivative of a BASIS │ │ │ │ │ -polynomial at a specified p... │ │ │ │ │ -DDeeffiinniittiioonn BasisFactors.h:331 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r │ │ │ │ │ -VectorDerivativeFactor(Key key, const Vector &z, const SharedNoiseModel &model, │ │ │ │ │ -const size_t N, double x) │ │ │ │ │ -Construct a new VectorDerivativeFactor object. │ │ │ │ │ -DDeeffiinniittiioonn BasisFactors.h:349 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r │ │ │ │ │ -VectorDerivativeFactor(Key key, const Vector &z, const SharedNoiseModel &model, │ │ │ │ │ -const size_t N, double x, double a, double b) │ │ │ │ │ -Construct a new VectorDerivativeFactor object. │ │ │ │ │ -DDeeffiinniittiioonn BasisFactors.h:366 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r │ │ │ │ │ -A unary factor which enforces the evaluation of the derivative of a BASIS │ │ │ │ │ -polynomial is equal to the ... │ │ │ │ │ -DDeeffiinniittiioonn BasisFactors.h:384 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r │ │ │ │ │ -ComponentDerivativeFactor(Key key, const double &z, const SharedNoiseModel │ │ │ │ │ -&model, const size_t N, size_t i, double x, double a, double b) │ │ │ │ │ -Construct a new ComponentDerivativeFactor object. │ │ │ │ │ -DDeeffiinniittiioonn BasisFactors.h:425 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r │ │ │ │ │ -ComponentDerivativeFactor(Key key, const double &z, const SharedNoiseModel │ │ │ │ │ -&model, const size_t N, size_t i, double x) │ │ │ │ │ -Construct a new ComponentDerivativeFactor object. │ │ │ │ │ -DDeeffiinniittiioonn BasisFactors.h:405 │ │ │ │ │ -_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r │ │ │ │ │ -Factor which evaluates provided unary functor and uses the result to compute │ │ │ │ │ -error with respect to th... │ │ │ │ │ -DDeeffiinniittiioonn FunctorizedFactor.h:59 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_:_:_k_e_y │ │ │ │ │ -Key key() const │ │ │ │ │ -Returns a key. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:518 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s │ │ │ │ │ +CRTP Base class for function bases. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_F_o_u_r_i_e_r_B_a_s_i_s │ │ │ │ │ +Fourier basis. │ │ │ │ │ +DDeeffiinniittiioonn Fourier.h:27 │ │ │ │ │ +_g_t_s_a_m_:_:_F_o_u_r_i_e_r_B_a_s_i_s_:_:_D_i_f_f_e_r_e_n_t_i_a_t_i_o_n_M_a_t_r_i_x │ │ │ │ │ +static DiffMatrix DifferentiationMatrix(size_t N) │ │ │ │ │ +Compute D = differentiation matrix. │ │ │ │ │ +DDeeffiinniittiioonn Fourier.h:73 │ │ │ │ │ +_g_t_s_a_m_:_:_F_o_u_r_i_e_r_B_a_s_i_s_:_:_D_e_r_i_v_a_t_i_v_e_W_e_i_g_h_t_s │ │ │ │ │ +static Weights DerivativeWeights(size_t N, double x) │ │ │ │ │ +Get weights at a given x that calculate the derivative. │ │ │ │ │ +DDeeffiinniittiioonn Fourier.h:92 │ │ │ │ │ +_g_t_s_a_m_:_:_F_o_u_r_i_e_r_B_a_s_i_s_:_:_C_a_l_c_u_l_a_t_e_W_e_i_g_h_t_s │ │ │ │ │ +static Weights CalculateWeights(size_t N, double x) │ │ │ │ │ +Evaluate Real Fourier Weights of size N in interval [a, b], e.g. │ │ │ │ │ +DDeeffiinniittiioonn Fourier.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_F_o_u_r_i_e_r_B_a_s_i_s_:_:_D_e_r_i_v_a_t_i_v_e_W_e_i_g_h_t_s │ │ │ │ │ +static Weights DerivativeWeights(size_t N, double x, double a, double b) │ │ │ │ │ +Get derivative weights at a given x that calculate the derivative, in the │ │ │ │ │ +interval [a,... │ │ │ │ │ +DDeeffiinniittiioonn Fourier.h:106 │ │ │ │ │ +_g_t_s_a_m_:_:_F_o_u_r_i_e_r_B_a_s_i_s_:_:_C_a_l_c_u_l_a_t_e_W_e_i_g_h_t_s │ │ │ │ │ +static Weights CalculateWeights(size_t N, double x, double a, double b) │ │ │ │ │ +Evaluate Real Fourier Weights of size N in interval [a, b], e.g. │ │ │ │ │ +DDeeffiinniittiioonn Fourier.h:64 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_i_s │ │ │ │ │ - * _B_a_s_i_s_F_a_c_t_o_r_s_._h │ │ │ │ │ + * _F_o_u_r_i_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00170.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Chebyshev.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/BasisFactors.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,36 +94,63 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
Chebyshev.cpp File Reference
│ │ │ │ +
BasisFactors.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Chebyshev basis decompositions. │ │ │ │ +

Factor definitions for various Basis functors. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::EvaluationFactor< BASIS >
 Factor for enforcing the scalar value of the polynomial BASIS representation at x is the same as the measurement z when using a pseudo-spectral parameterization. More...
 
class  gtsam::VectorEvaluationFactor< BASIS, M >
 Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix of size (M, N) is equal to a vector-valued measurement at the same point, when using a pseudo-spectral parameterization. More...
 
class  gtsam::VectorComponentFactor< BASIS, P >
 Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix of size (P, N) is equal to specified measurement at the same point, when using a pseudo-spectral parameterization. More...
 
class  gtsam::ManifoldEvaluationFactor< BASIS, T >
 For a measurement value of type T i.e. More...
 
class  gtsam::DerivativeFactor< BASIS >
 A unary factor which enforces the evaluation of the derivative of a BASIS polynomial at a specified pointx is equal to the scalar measurement z. More...
 
class  gtsam::VectorDerivativeFactor< BASIS, M >
 A unary factor which enforces the evaluation of the derivative of a BASIS polynomial at a specified point x is equal to the vector value z. More...
 
class  gtsam::ComponentDerivativeFactor< BASIS, P >
 A unary factor which enforces the evaluation of the derivative of a BASIS polynomial is equal to the scalar value at a specific index i of a vector-valued measurement z. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Chebyshev basis decompositions.

│ │ │ │ -
Author
Varun Agrawal, Jing Dong, Frank Dellaert
│ │ │ │ +

Factor definitions for various Basis functors.

│ │ │ │ +
Author
Varun Agrawal
│ │ │ │
Date
July 4, 2020
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,58 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Chebyshev.cpp File Reference │ │ │ │ │ -Chebyshev basis decompositions. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +BasisFactors.h File Reference │ │ │ │ │ +Factor definitions for various Basis functors. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r_<_ _B_A_S_I_S_ _> │ │ │ │ │ + _F_a_c_t_o_r for enforcing the scalar value of the polynomial BASIS │ │ │ │ │ +  representation at x is the same as the measurement z when using a │ │ │ │ │ + pseudo-spectral parameterization. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r_<_ _B_A_S_I_S_,_ _M_ _> │ │ │ │ │ + Unary factor for enforcing BASIS polynomial evaluation on a │ │ │ │ │ +  _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x of size (M, N) is equal to a vector-valued measurement │ │ │ │ │ + at the same point, when using a pseudo-spectral parameterization. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_a_c_t_o_r_<_ _B_A_S_I_S_,_ _P_ _> │ │ │ │ │ + Unary factor for enforcing BASIS polynomial evaluation on a │ │ │ │ │ +  _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x of size (P, N) is equal to specified measurement at the │ │ │ │ │ + same point, when using a pseudo-spectral parameterization. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r_<_ _B_A_S_I_S_,_ _T_ _> │ │ │ │ │ +  For a measurement value of type T i.e. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r_<_ _B_A_S_I_S_ _> │ │ │ │ │ + A unary factor which enforces the evaluation of the derivative of a │ │ │ │ │ +  BASIS polynomial at a specified pointx is equal to the scalar │ │ │ │ │ + measurement z. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r_<_ _B_A_S_I_S_,_ _M_ _> │ │ │ │ │ + A unary factor which enforces the evaluation of the derivative of a │ │ │ │ │ +  BASIS polynomial at a specified point x is equal to the vector value z. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r_<_ _B_A_S_I_S_,_ _P_ _> │ │ │ │ │ + A unary factor which enforces the evaluation of the derivative of a │ │ │ │ │ +  BASIS polynomial is equal to the scalar value at a specific index i of │ │ │ │ │ + a vector-valued measurement z. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Chebyshev basis decompositions. │ │ │ │ │ +Factor definitions for various Basis functors. │ │ │ │ │ Author │ │ │ │ │ - Varun Agrawal, Jing Dong, Frank Dellaert │ │ │ │ │ + Varun Agrawal │ │ │ │ │ Date │ │ │ │ │ July 4, 2020 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_i_s │ │ │ │ │ - * _C_h_e_b_y_s_h_e_v_._c_p_p │ │ │ │ │ + * _B_a_s_i_s_F_a_c_t_o_r_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00173.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Basis.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Chebyshev2.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,83 +95,47 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
│ │ │ │ -
Basis.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
Chebyshev2.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Compute an interpolating basis. │ │ │ │ +

Pseudo-spectral parameterization for Chebyshev polynomials of the second kind. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::Basis< DERIVED >
 CRTP Base class for function bases. More...
 
class  gtsam::Basis< DERIVED >::EvaluationFunctor
 An instance of an EvaluationFunctor calculates f(x;p) at a given x, applied to Parameters p. More...
 
class  gtsam::Basis< DERIVED >::VectorEvaluationFunctor< M >
 VectorEvaluationFunctor at a given x, applied to ParameterMatrix<M>. More...
 
class  gtsam::Basis< DERIVED >::VectorComponentFunctor< M >
 Given a M*N Matrix of M-vectors at N polynomial points, an instance of VectorComponentFunctor computes the N-vector value for a specific row component of the M-vectors at all the polynomial points. More...
 
class  gtsam::Basis< DERIVED >::ManifoldEvaluationFunctor< T >
 Manifold EvaluationFunctor at a given x, applied to ParameterMatrix<M>. More...
 
class  gtsam::Basis< DERIVED >::DerivativeFunctorBase
 Base class for functors below that calculate derivative weights. More...
 
class  gtsam::Basis< DERIVED >::DerivativeFunctor
 An instance of a DerivativeFunctor calculates f'(x;p) at a given x, applied to Parameters p. More...
 
class  gtsam::Basis< DERIVED >::VectorDerivativeFunctor< M >
 VectorDerivativeFunctor at a given x, applied to ParameterMatrix<M>. More...
 
class  gtsam::Basis< DERIVED >::ComponentDerivativeFunctor< M >
 Given a M*N Matrix of M-vectors at N polynomial points, an instance of ComponentDerivativeFunctor computes the N-vector derivative for a specific row component of the M-vectors at all the polynomial points. More...
class  gtsam::Chebyshev2
 Chebyshev Interpolation on Chebyshev points of the second kind Note that N here, the number of points, is one less than N from 'Approximation Theory and Approximation Practice by L. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Typedefs

│ │ │ │ -using gtsam::Weights = Eigen::Matrix< double, 1, -1 >
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

template<size_t M>
Matrix gtsam::kroneckerProductIdentity (const Weights &w)
 Function for computing the kronecker product of the 1*N Weight vector w with the MxM identity matrix I efficiently.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Compute an interpolating basis.

│ │ │ │ +

Pseudo-spectral parameterization for Chebyshev polynomials of the second kind.

│ │ │ │ +

In a pseudo-spectral case, rather than the parameters acting as weights for the bases polynomials (as in Chebyshev2Basis), here the parameters are the values at a specific set of points in the interval, the "Chebyshev points". These values uniquely determine the polynomial that interpolates them at the Chebyshev points.

│ │ │ │ +

This is different from Chebyshev.h since it leverage ideas from pseudo-spectral optimization, i.e. we don't decompose into basis functions, rather estimate function values at the Chebyshev points.

│ │ │ │ +

Please refer to Agrawal21icra for more details.

│ │ │ │
Author
Varun Agrawal, Jing Dong, Frank Dellaert
│ │ │ │
Date
July 4, 2020
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,71 +1,39 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Basis.h File Reference │ │ │ │ │ -Compute an interpolating basis. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +Chebyshev2.h File Reference │ │ │ │ │ +Pseudo-spectral parameterization for Chebyshev polynomials of the second kind. │ │ │ │ │ +_M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_B_a_s_i_s_<_ _D_E_R_I_V_E_D_ _> │ │ │ │ │ -  CRTP Base class for function bases. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_B_a_s_i_s_<_ _D_E_R_I_V_E_D_ _>_:_:_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r │ │ │ │ │ -  An instance of an _E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r calculates f(x;p) at a given x, │ │ │ │ │ - applied to Parameters p. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_B_a_s_i_s_<_ _D_E_R_I_V_E_D_ _>_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_<_ _M_ _> │ │ │ │ │ -  _V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r at a given x, applied to ParameterMatrix. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_B_a_s_i_s_<_ _D_E_R_I_V_E_D_ _>_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r_<_ _M_ _> │ │ │ │ │ - Given a M*N Matrix of M-vectors at N polynomial points, an instance of │ │ │ │ │ -  _V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r computes the N-vector value for a specific row │ │ │ │ │ - component of the M-vectors at all the polynomial points. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_B_a_s_i_s_<_ _D_E_R_I_V_E_D_ _>_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_<_ _T_ _> │ │ │ │ │ -  Manifold _E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r at a given x, applied to ParameterMatrix. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_B_a_s_i_s_<_ _D_E_R_I_V_E_D_ _>_:_:_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e │ │ │ │ │ -  Base class for functors below that calculate derivative weights. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_B_a_s_i_s_<_ _D_E_R_I_V_E_D_ _>_:_:_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r │ │ │ │ │ -  An instance of a _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r calculates f'(x;p) at a given x, │ │ │ │ │ - applied to Parameters p. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_B_a_s_i_s_<_ _D_E_R_I_V_E_D_ _>_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_<_ _M_ _> │ │ │ │ │ -  _V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r at a given x, applied to ParameterMatrix. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_B_a_s_i_s_<_ _D_E_R_I_V_E_D_ _>_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_<_ _M_ _> │ │ │ │ │ - Given a M*N Matrix of M-vectors at N polynomial points, an instance of │ │ │ │ │ -  _C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r computes the N-vector derivative for a │ │ │ │ │ - specific row component of the M-vectors at all the polynomial points. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_C_h_e_b_y_s_h_e_v_2 │ │ │ │ │ + Chebyshev Interpolation on Chebyshev points of the second kind Note │ │ │ │ │ +  that N here, the number of points, is one less than N from │ │ │ │ │ + 'Approximation Theory and Approximation Practice by L. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -using  ggttssaamm::::WWeeiigghhttss = Eigen::Matrix< double, 1, -1 > │ │ │ │ │ -  │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ -Matrix  _g_t_s_a_m_:_:_k_r_o_n_e_c_k_e_r_P_r_o_d_u_c_t_I_d_e_n_t_i_t_y (const Weights &w) │ │ │ │ │ -  Function for computing the kronecker product of the 1*N Weight vector w │ │ │ │ │ - with the MxM identity matrix I efficiently. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Compute an interpolating basis. │ │ │ │ │ +Pseudo-spectral parameterization for Chebyshev polynomials of the second kind. │ │ │ │ │ +In a pseudo-spectral case, rather than the parameters acting as weights for the │ │ │ │ │ +bases polynomials (as in Chebyshev2Basis), here the parameters are the vvaalluueess │ │ │ │ │ +at a specific set of points in the interval, the "Chebyshev points". These │ │ │ │ │ +values uniquely determine the polynomial that interpolates them at the │ │ │ │ │ +Chebyshev points. │ │ │ │ │ +This is different from _C_h_e_b_y_s_h_e_v_._h since it leverage ideas from pseudo-spectral │ │ │ │ │ +optimization, i.e. we don't decompose into basis functions, rather estimate │ │ │ │ │ +function values at the Chebyshev points. │ │ │ │ │ +Please refer to Agrawal21icra for more details. │ │ │ │ │ Author │ │ │ │ │ Varun Agrawal, Jing Dong, Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ July 4, 2020 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_i_s │ │ │ │ │ - * _B_a_s_i_s_._h │ │ │ │ │ + * _C_h_e_b_y_s_h_e_v_2_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00173.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,11 +1,3 @@ │ │ │ │ │ var a00173 = [ │ │ │ │ │ - ["gtsam::Basis< DERIVED >::EvaluationFunctor", "a02672.html", "a02672"], │ │ │ │ │ - ["gtsam::Basis< DERIVED >::VectorEvaluationFunctor< M >", "a02676.html", "a02676"], │ │ │ │ │ - ["gtsam::Basis< DERIVED >::VectorComponentFunctor< M >", "a02680.html", "a02680"], │ │ │ │ │ - ["gtsam::Basis< DERIVED >::ManifoldEvaluationFunctor< T >", "a02684.html", "a02684"], │ │ │ │ │ - ["gtsam::Basis< DERIVED >::DerivativeFunctorBase", "a02688.html", "a02688"], │ │ │ │ │ - ["gtsam::Basis< DERIVED >::DerivativeFunctor", "a02692.html", "a02692"], │ │ │ │ │ - ["gtsam::Basis< DERIVED >::VectorDerivativeFunctor< M >", "a02696.html", "a02696"], │ │ │ │ │ - ["gtsam::Basis< DERIVED >::ComponentDerivativeFunctor< M >", "a02700.html", "a02700"], │ │ │ │ │ - ["kroneckerProductIdentity", "a00173.html#ga2ce44d31cae1eda44c2a86d32d92b935", null] │ │ │ │ │ + ["gtsam::Chebyshev2", "a02740.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00173_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Basis.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Chebyshev2.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,473 +98,122 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Basis.h
│ │ │ │ +
Chebyshev2.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/base/Matrix.h>
│ │ │ │ -
22#include <gtsam/base/OptionalJacobian.h>
│ │ │ │ -
23#include <gtsam/basis/ParameterMatrix.h>
│ │ │ │ -
24
│ │ │ │ -
25#include <iostream>
│ │ │ │ -
26
│ │ │ │ -
68namespace gtsam {
│ │ │ │ -
69
│ │ │ │ -
70using Weights = Eigen::Matrix<double, 1, -1>; /* 1xN vector */
│ │ │ │ -
71
│ │ │ │ -
84template <size_t M>
│ │ │ │ -
│ │ │ │ -
85Matrix kroneckerProductIdentity(const Weights& w) {
│ │ │ │ -
86 Matrix result(M, w.cols() * M);
│ │ │ │ -
87 result.setZero();
│ │ │ │ -
88
│ │ │ │ -
89 for (int i = 0; i < w.cols(); i++) {
│ │ │ │ -
90 result.block(0, i * M, M, M).diagonal().array() = w(i);
│ │ │ │ -
91 }
│ │ │ │ -
92 return result;
│ │ │ │ -
93}
│ │ │ │ -
│ │ │ │ -
94
│ │ │ │ -
99template <typename DERIVED>
│ │ │ │ -
│ │ │ │ -
100class Basis {
│ │ │ │ -
101 public:
│ │ │ │ -
│ │ │ │ -
107 static Matrix WeightMatrix(size_t N, const Vector& X) {
│ │ │ │ -
108 Matrix W(X.size(), N);
│ │ │ │ -
109 for (int i = 0; i < X.size(); i++)
│ │ │ │ -
110 W.row(i) = DERIVED::CalculateWeights(N, X(i));
│ │ │ │ -
111 return W;
│ │ │ │ -
112 }
│ │ │ │ -
│ │ │ │ -
113
│ │ │ │ -
│ │ │ │ -
123 static Matrix WeightMatrix(size_t N, const Vector& X, double a, double b) {
│ │ │ │ -
124 Matrix W(X.size(), N);
│ │ │ │ -
125 for (int i = 0; i < X.size(); i++)
│ │ │ │ -
126 W.row(i) = DERIVED::CalculateWeights(N, X(i), a, b);
│ │ │ │ -
127 return W;
│ │ │ │ -
128 }
│ │ │ │ -
│ │ │ │ -
129
│ │ │ │ -
│ │ │ │ -
137 class EvaluationFunctor {
│ │ │ │ -
138 protected:
│ │ │ │ -
139 Weights weights_;
│ │ │ │ -
140
│ │ │ │ -
141 public:
│ │ │ │ -
143 EvaluationFunctor() {}
│ │ │ │ +
33#pragma once
│ │ │ │ +
34
│ │ │ │ +
35#include <gtsam/base/Manifold.h>
│ │ │ │ +
36#include <gtsam/base/OptionalJacobian.h>
│ │ │ │ +
37#include <gtsam/basis/Basis.h>
│ │ │ │ +
38
│ │ │ │ +
39namespace gtsam {
│ │ │ │ +
40
│ │ │ │ +
│ │ │ │ +
46class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
│ │ │ │ +
47 public:
│ │ │ │ +
48 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
49
│ │ │ │ +
50 using Base = Basis<Chebyshev2>;
│ │ │ │ +
51 using Parameters = Eigen::Matrix<double, /*Nx1*/ -1, 1>;
│ │ │ │ +
52 using DiffMatrix = Eigen::Matrix<double, /*NxN*/ -1, -1>;
│ │ │ │ +
53
│ │ │ │ +
│ │ │ │ +
55 static double Point(size_t N, int j) {
│ │ │ │ +
56 assert(j >= 0 && size_t(j) < N);
│ │ │ │ +
57 const double dtheta = M_PI / (N > 1 ? (N - 1) : 1);
│ │ │ │ +
58 // We add -PI so that we get values ordered from -1 to +1
│ │ │ │ +
59 // sin(- M_PI_2 + dtheta*j); also works
│ │ │ │ +
60 return cos(-M_PI + dtheta * j);
│ │ │ │ +
61 }
│ │ │ │ +
│ │ │ │ +
62
│ │ │ │ +
│ │ │ │ +
64 static double Point(size_t N, int j, double a, double b) {
│ │ │ │ +
65 assert(j >= 0 && size_t(j) < N);
│ │ │ │ +
66 const double dtheta = M_PI / (N - 1);
│ │ │ │ +
67 // We add -PI so that we get values ordered from -1 to +1
│ │ │ │ +
68 return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2;
│ │ │ │ +
69 }
│ │ │ │ +
│ │ │ │ +
70
│ │ │ │ +
│ │ │ │ +
72 static Vector Points(size_t N) {
│ │ │ │ +
73 Vector points(N);
│ │ │ │ +
74 for (size_t j = 0; j < N; j++) points(j) = Point(N, j);
│ │ │ │ +
75 return points;
│ │ │ │ +
76 }
│ │ │ │ +
│ │ │ │ +
77
│ │ │ │ +
│ │ │ │ +
79 static Vector Points(size_t N, double a, double b) {
│ │ │ │ +
80 Vector points = Points(N);
│ │ │ │ +
81 const double T1 = (a + b) / 2, T2 = (b - a) / 2;
│ │ │ │ +
82 points = T1 + (T2 * points).array();
│ │ │ │ +
83 return points;
│ │ │ │ +
84 }
│ │ │ │ +
│ │ │ │ +
85
│ │ │ │ +
94 static Weights CalculateWeights(size_t N, double x, double a = -1,
│ │ │ │ +
95 double b = 1);
│ │ │ │ +
96
│ │ │ │ +
101 static Weights DerivativeWeights(size_t N, double x, double a = -1,
│ │ │ │ +
102 double b = 1);
│ │ │ │ +
103
│ │ │ │ +
108 static DiffMatrix DifferentiationMatrix(size_t N, double a = -1,
│ │ │ │ +
109 double b = 1);
│ │ │ │ +
110
│ │ │ │ +
129 static Weights IntegrationWeights(size_t N, double a = -1, double b = 1);
│ │ │ │ +
130
│ │ │ │ +
134 template <size_t M>
│ │ │ │ +
│ │ │ │ +
135 static Matrix matrix(std::function<Eigen::Matrix<double, M, 1>(double)> f,
│ │ │ │ +
136 size_t N, double a = -1, double b = 1) {
│ │ │ │ +
137 Matrix Xmat(M, N);
│ │ │ │ +
138 for (size_t j = 0; j < N; j++) {
│ │ │ │ +
139 Xmat.col(j) = f(Point(N, j, a, b));
│ │ │ │ +
140 }
│ │ │ │ +
141 return Xmat;
│ │ │ │ +
142 }
│ │ │ │ +
│ │ │ │ +
143}; // \ Chebyshev2
│ │ │ │ +
│ │ │ │
144
│ │ │ │ -
│ │ │ │ -
146 EvaluationFunctor(size_t N, double x)
│ │ │ │ -
147 : weights_(DERIVED::CalculateWeights(N, x)) {}
│ │ │ │ -
│ │ │ │ -
148
│ │ │ │ -
│ │ │ │ -
150 EvaluationFunctor(size_t N, double x, double a, double b)
│ │ │ │ -
151 : weights_(DERIVED::CalculateWeights(N, x, a, b)) {}
│ │ │ │ -
│ │ │ │ -
152
│ │ │ │ -
│ │ │ │ -
154 double apply(const typename DERIVED::Parameters& p,
│ │ │ │ -
155 OptionalJacobian<-1, -1> H = boost::none) const {
│ │ │ │ -
156 if (H) *H = weights_;
│ │ │ │ -
157 return (weights_ * p)(0);
│ │ │ │ -
158 }
│ │ │ │ -
│ │ │ │ -
159
│ │ │ │ -
│ │ │ │ -
161 double operator()(const typename DERIVED::Parameters& p,
│ │ │ │ -
162 OptionalJacobian<-1, -1> H = boost::none) const {
│ │ │ │ -
163 return apply(p, H); // might call apply in derived
│ │ │ │ -
164 }
│ │ │ │ -
│ │ │ │ -
165
│ │ │ │ -
166 void print(const std::string& s = "") const {
│ │ │ │ -
167 std::cout << s << (s != "" ? " " : "") << weights_ << std::endl;
│ │ │ │ -
168 }
│ │ │ │ -
169 };
│ │ │ │ -
│ │ │ │ -
170
│ │ │ │ -
177 template <int M>
│ │ │ │ -
│ │ │ │ -
178 class VectorEvaluationFunctor : protected EvaluationFunctor {
│ │ │ │ -
179 protected:
│ │ │ │ -
180 using VectorM = Eigen::Matrix<double, M, 1>;
│ │ │ │ -
181 using Jacobian = Eigen::Matrix<double, /*MxMN*/ M, -1>;
│ │ │ │ -
182 Jacobian H_;
│ │ │ │ -
183
│ │ │ │ -
│ │ │ │ -
192 void calculateJacobian() {
│ │ │ │ -
193 H_ = kroneckerProductIdentity<M>(this->weights_);
│ │ │ │ -
194 }
│ │ │ │ -
│ │ │ │ -
195
│ │ │ │ -
196 public:
│ │ │ │ -
197 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
198
│ │ │ │ -
200 VectorEvaluationFunctor() {}
│ │ │ │ -
201
│ │ │ │ -
│ │ │ │ -
203 VectorEvaluationFunctor(size_t N, double x) : EvaluationFunctor(N, x) {
│ │ │ │ -
204 calculateJacobian();
│ │ │ │ -
205 }
│ │ │ │ -
│ │ │ │ -
206
│ │ │ │ -
│ │ │ │ -
208 VectorEvaluationFunctor(size_t N, double x, double a, double b)
│ │ │ │ -
209 : EvaluationFunctor(N, x, a, b) {
│ │ │ │ -
210 calculateJacobian();
│ │ │ │ -
211 }
│ │ │ │ -
│ │ │ │ -
212
│ │ │ │ -
│ │ │ │ -
214 VectorM apply(const ParameterMatrix<M>& P,
│ │ │ │ -
215 OptionalJacobian</*MxN*/ -1, -1> H = boost::none) const {
│ │ │ │ -
216 if (H) *H = H_;
│ │ │ │ -
217 return P.matrix() * this->weights_.transpose();
│ │ │ │ -
218 }
│ │ │ │ -
│ │ │ │ -
219
│ │ │ │ -
│ │ │ │ -
221 VectorM operator()(const ParameterMatrix<M>& P,
│ │ │ │ -
222 OptionalJacobian</*MxN*/ -1, -1> H = boost::none) const {
│ │ │ │ -
223 return apply(P, H);
│ │ │ │ -
224 }
│ │ │ │ -
│ │ │ │ -
225 };
│ │ │ │ -
│ │ │ │ -
226
│ │ │ │ -
234 template <int M>
│ │ │ │ -
│ │ │ │ -
235 class VectorComponentFunctor : public EvaluationFunctor {
│ │ │ │ -
236 protected:
│ │ │ │ -
237 using Jacobian = Eigen::Matrix<double, /*1xMN*/ 1, -1>;
│ │ │ │ -
238 size_t rowIndex_;
│ │ │ │ -
239 Jacobian H_;
│ │ │ │ -
240
│ │ │ │ -
241 /*
│ │ │ │ -
242 * Calculate the `1*(M*N)` Jacobian of this functor with respect to
│ │ │ │ -
243 * the M*N parameter matrix `P`.
│ │ │ │ -
244 * We flatten assuming column-major order, e.g., if N=3 and M=2, we have
│ │ │ │ -
245 * H=[w(0) 0 w(1) 0 w(2) 0] for rowIndex==0
│ │ │ │ -
246 * H=[0 w(0) 0 w(1) 0 w(2)] for rowIndex==1
│ │ │ │ -
247 * i.e., one row of the Kronecker product of weights_ with the
│ │ │ │ -
248 * MxM identity matrix. See also VectorEvaluationFunctor.
│ │ │ │ -
249 */
│ │ │ │ -
250 void calculateJacobian(size_t N) {
│ │ │ │ -
251 H_.setZero(1, M * N);
│ │ │ │ -
252 for (int j = 0; j < EvaluationFunctor::weights_.size(); j++)
│ │ │ │ -
253 H_(0, rowIndex_ + j * M) = EvaluationFunctor::weights_(j);
│ │ │ │ -
254 }
│ │ │ │ -
255
│ │ │ │ -
256 public:
│ │ │ │ -
258 VectorComponentFunctor() {}
│ │ │ │ -
259
│ │ │ │ -
│ │ │ │ -
261 VectorComponentFunctor(size_t N, size_t i, double x)
│ │ │ │ -
262 : EvaluationFunctor(N, x), rowIndex_(i) {
│ │ │ │ -
263 calculateJacobian(N);
│ │ │ │ -
264 }
│ │ │ │ -
│ │ │ │ -
265
│ │ │ │ -
│ │ │ │ -
267 VectorComponentFunctor(size_t N, size_t i, double x, double a, double b)
│ │ │ │ -
268 : EvaluationFunctor(N, x, a, b), rowIndex_(i) {
│ │ │ │ -
269 calculateJacobian(N);
│ │ │ │ -
270 }
│ │ │ │ -
│ │ │ │ -
271
│ │ │ │ -
│ │ │ │ -
273 double apply(const ParameterMatrix<M>& P,
│ │ │ │ -
274 OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
│ │ │ │ -
275 if (H) *H = H_;
│ │ │ │ -
276 return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose();
│ │ │ │ -
277 }
│ │ │ │ -
│ │ │ │ -
278
│ │ │ │ -
│ │ │ │ -
280 double operator()(const ParameterMatrix<M>& P,
│ │ │ │ -
281 OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
│ │ │ │ -
282 return apply(P, H);
│ │ │ │ -
283 }
│ │ │ │ -
│ │ │ │ -
284 };
│ │ │ │ -
│ │ │ │ -
285
│ │ │ │ -
299 template <class T>
│ │ │ │ -
│ │ │ │ -
300 class ManifoldEvaluationFunctor
│ │ │ │ -
301 : public VectorEvaluationFunctor<traits<T>::dimension> {
│ │ │ │ -
302 enum { M = traits<T>::dimension };
│ │ │ │ -
303 using Base = VectorEvaluationFunctor<M>;
│ │ │ │ -
304
│ │ │ │ -
305 public:
│ │ │ │ -
307 ManifoldEvaluationFunctor() {}
│ │ │ │ -
308
│ │ │ │ -
310 ManifoldEvaluationFunctor(size_t N, double x) : Base(N, x) {}
│ │ │ │ -
311
│ │ │ │ -
│ │ │ │ -
313 ManifoldEvaluationFunctor(size_t N, double x, double a, double b)
│ │ │ │ -
314 : Base(N, x, a, b) {}
│ │ │ │ -
│ │ │ │ -
315
│ │ │ │ -
│ │ │ │ -
317 T apply(const ParameterMatrix<M>& P,
│ │ │ │ -
318 OptionalJacobian</*MxMN*/ -1, -1> H = boost::none) const {
│ │ │ │ -
319 // Interpolate the M-dimensional vector to yield a vector in tangent space
│ │ │ │ -
320 Eigen::Matrix<double, M, 1> xi = Base::operator()(P, H);
│ │ │ │ -
321
│ │ │ │ -
322 // Now call retract with this M-vector, possibly with derivatives
│ │ │ │ -
323 Eigen::Matrix<double, M, M> D_result_xi;
│ │ │ │ -
324 T result = T::ChartAtOrigin::Retract(xi, H ? &D_result_xi : 0);
│ │ │ │ -
325
│ │ │ │ -
326 // Finally, if derivatives are asked, apply chain rule where H is Mx(M*N)
│ │ │ │ -
327 // derivative of interpolation and D_result_xi is MxM derivative of
│ │ │ │ -
328 // retract.
│ │ │ │ -
329 if (H) *H = D_result_xi * (*H);
│ │ │ │ -
330
│ │ │ │ -
331 // and return a T
│ │ │ │ -
332 return result;
│ │ │ │ -
333 }
│ │ │ │ -
│ │ │ │ -
334
│ │ │ │ -
│ │ │ │ -
336 T operator()(const ParameterMatrix<M>& P,
│ │ │ │ -
337 OptionalJacobian</*MxN*/ -1, -1> H = boost::none) const {
│ │ │ │ -
338 return apply(P, H); // might call apply in derived
│ │ │ │ -
339 }
│ │ │ │ -
│ │ │ │ -
340 };
│ │ │ │ -
│ │ │ │ -
341
│ │ │ │ -
│ │ │ │ -
343 class DerivativeFunctorBase {
│ │ │ │ -
344 protected:
│ │ │ │ -
345 Weights weights_;
│ │ │ │ -
346
│ │ │ │ -
347 public:
│ │ │ │ -
349 DerivativeFunctorBase() {}
│ │ │ │ -
350
│ │ │ │ -
351 DerivativeFunctorBase(size_t N, double x)
│ │ │ │ -
352 : weights_(DERIVED::DerivativeWeights(N, x)) {}
│ │ │ │ -
353
│ │ │ │ -
354 DerivativeFunctorBase(size_t N, double x, double a, double b)
│ │ │ │ -
355 : weights_(DERIVED::DerivativeWeights(N, x, a, b)) {}
│ │ │ │ -
356
│ │ │ │ -
357 void print(const std::string& s = "") const {
│ │ │ │ -
358 std::cout << s << (s != "" ? " " : "") << weights_ << std::endl;
│ │ │ │ -
359 }
│ │ │ │ -
360 };
│ │ │ │ -
│ │ │ │ -
361
│ │ │ │ -
│ │ │ │ -
369 class DerivativeFunctor : protected DerivativeFunctorBase {
│ │ │ │ -
370 public:
│ │ │ │ -
372 DerivativeFunctor() {}
│ │ │ │ -
373
│ │ │ │ -
374 DerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) {}
│ │ │ │ -
375
│ │ │ │ -
376 DerivativeFunctor(size_t N, double x, double a, double b)
│ │ │ │ -
377 : DerivativeFunctorBase(N, x, a, b) {}
│ │ │ │ -
378
│ │ │ │ -
379 double apply(const typename DERIVED::Parameters& p,
│ │ │ │ -
380 OptionalJacobian</*1xN*/ -1, -1> H = boost::none) const {
│ │ │ │ -
381 if (H) *H = this->weights_;
│ │ │ │ -
382 return (this->weights_ * p)(0);
│ │ │ │ -
383 }
│ │ │ │ -
│ │ │ │ -
385 double operator()(const typename DERIVED::Parameters& p,
│ │ │ │ -
386 OptionalJacobian</*1xN*/ -1, -1> H = boost::none) const {
│ │ │ │ -
387 return apply(p, H); // might call apply in derived
│ │ │ │ -
388 }
│ │ │ │ -
│ │ │ │ -
389 };
│ │ │ │ -
│ │ │ │ -
390
│ │ │ │ -
399 template <int M>
│ │ │ │ -
│ │ │ │ -
400 class VectorDerivativeFunctor : protected DerivativeFunctorBase {
│ │ │ │ -
401 protected:
│ │ │ │ -
402 using VectorM = Eigen::Matrix<double, M, 1>;
│ │ │ │ -
403 using Jacobian = Eigen::Matrix<double, /*MxMN*/ M, -1>;
│ │ │ │ -
404 Jacobian H_;
│ │ │ │ -
405
│ │ │ │ -
│ │ │ │ -
414 void calculateJacobian() {
│ │ │ │ -
415 H_ = kroneckerProductIdentity<M>(this->weights_);
│ │ │ │ -
416 }
│ │ │ │ -
│ │ │ │ -
417
│ │ │ │ -
418 public:
│ │ │ │ -
419 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
420
│ │ │ │ -
422 VectorDerivativeFunctor() {}
│ │ │ │ -
423
│ │ │ │ -
│ │ │ │ -
425 VectorDerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) {
│ │ │ │ -
426 calculateJacobian();
│ │ │ │ -
427 }
│ │ │ │ -
│ │ │ │ -
428
│ │ │ │ -
│ │ │ │ -
430 VectorDerivativeFunctor(size_t N, double x, double a, double b)
│ │ │ │ -
431 : DerivativeFunctorBase(N, x, a, b) {
│ │ │ │ -
432 calculateJacobian();
│ │ │ │ -
433 }
│ │ │ │ -
│ │ │ │ -
434
│ │ │ │ -
435 VectorM apply(const ParameterMatrix<M>& P,
│ │ │ │ -
436 OptionalJacobian</*MxMN*/ -1, -1> H = boost::none) const {
│ │ │ │ -
437 if (H) *H = H_;
│ │ │ │ -
438 return P.matrix() * this->weights_.transpose();
│ │ │ │ -
439 }
│ │ │ │ -
│ │ │ │ -
441 VectorM operator()(
│ │ │ │ -
442 const ParameterMatrix<M>& P,
│ │ │ │ -
443 OptionalJacobian</*MxMN*/ -1, -1> H = boost::none) const {
│ │ │ │ -
444 return apply(P, H);
│ │ │ │ -
445 }
│ │ │ │ -
│ │ │ │ -
446 };
│ │ │ │ -
│ │ │ │ -
447
│ │ │ │ -
455 template <int M>
│ │ │ │ -
│ │ │ │ -
456 class ComponentDerivativeFunctor : protected DerivativeFunctorBase {
│ │ │ │ -
457 protected:
│ │ │ │ -
458 using Jacobian = Eigen::Matrix<double, /*1xMN*/ 1, -1>;
│ │ │ │ -
459 size_t rowIndex_;
│ │ │ │ -
460 Jacobian H_;
│ │ │ │ -
461
│ │ │ │ -
462 /*
│ │ │ │ -
463 * Calculate the `1*(M*N)` Jacobian of this functor with respect to
│ │ │ │ -
464 * the M*N parameter matrix `P`.
│ │ │ │ -
465 * We flatten assuming column-major order, e.g., if N=3 and M=2, we have
│ │ │ │ -
466 * H=[w(0) 0 w(1) 0 w(2) 0] for rowIndex==0
│ │ │ │ -
467 * H=[0 w(0) 0 w(1) 0 w(2)] for rowIndex==1
│ │ │ │ -
468 * i.e., one row of the Kronecker product of weights_ with the
│ │ │ │ -
469 * MxM identity matrix. See also VectorDerivativeFunctor.
│ │ │ │ -
470 */
│ │ │ │ -
471 void calculateJacobian(size_t N) {
│ │ │ │ -
472 H_.setZero(1, M * N);
│ │ │ │ -
473 for (int j = 0; j < this->weights_.size(); j++)
│ │ │ │ -
474 H_(0, rowIndex_ + j * M) = this->weights_(j);
│ │ │ │ -
475 }
│ │ │ │ -
476
│ │ │ │ -
477 public:
│ │ │ │ -
479 ComponentDerivativeFunctor() {}
│ │ │ │ -
480
│ │ │ │ -
│ │ │ │ -
482 ComponentDerivativeFunctor(size_t N, size_t i, double x)
│ │ │ │ -
483 : DerivativeFunctorBase(N, x), rowIndex_(i) {
│ │ │ │ -
484 calculateJacobian(N);
│ │ │ │ -
485 }
│ │ │ │ -
│ │ │ │ -
486
│ │ │ │ -
│ │ │ │ -
488 ComponentDerivativeFunctor(size_t N, size_t i, double x, double a, double b)
│ │ │ │ -
489 : DerivativeFunctorBase(N, x, a, b), rowIndex_(i) {
│ │ │ │ -
490 calculateJacobian(N);
│ │ │ │ -
491 }
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
493 double apply(const ParameterMatrix<M>& P,
│ │ │ │ -
494 OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
│ │ │ │ -
495 if (H) *H = H_;
│ │ │ │ -
496 return P.row(rowIndex_) * this->weights_.transpose();
│ │ │ │ -
497 }
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
499 double operator()(const ParameterMatrix<M>& P,
│ │ │ │ -
500 OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
│ │ │ │ -
501 return apply(P, H);
│ │ │ │ -
502 }
│ │ │ │ -
│ │ │ │ -
503 };
│ │ │ │ -
│ │ │ │ -
504
│ │ │ │ -
505};
│ │ │ │ -
│ │ │ │ -
506
│ │ │ │ -
507} // namespace gtsam
│ │ │ │ -
OptionalJacobian.h
Special class for optional Jacobian arguments.
│ │ │ │ -
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ -
ParameterMatrix.h
Define ParameterMatrix class which is used to store values at interpolation points.
│ │ │ │ -
gtsam::kroneckerProductIdentity
Matrix kroneckerProductIdentity(const Weights &w)
Function for computing the kronecker product of the 1*N Weight vector w with the MxM identity matrix ...
Definition Basis.h:85
│ │ │ │ +
145} // namespace gtsam
│ │ │ │ +
OptionalJacobian.h
Special class for optional Jacobian arguments.
│ │ │ │ +
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ +
Basis.h
Compute an interpolating basis.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │
gtsam::Basis
CRTP Base class for function bases.
Definition Basis.h:100
│ │ │ │ -
gtsam::Basis::WeightMatrix
static Matrix WeightMatrix(size_t N, const Vector &X, double a, double b)
Calculate weights for all x in vector X, with interval [a,b].
Definition Basis.h:123
│ │ │ │ -
gtsam::Basis::WeightMatrix
static Matrix WeightMatrix(size_t N, const Vector &X)
Calculate weights for all x in vector X.
Definition Basis.h:107
│ │ │ │ -
gtsam::Basis::EvaluationFunctor
An instance of an EvaluationFunctor calculates f(x;p) at a given x, applied to Parameters p.
Definition Basis.h:137
│ │ │ │ -
gtsam::Basis::EvaluationFunctor::EvaluationFunctor
EvaluationFunctor(size_t N, double x)
Constructor with interval [a,b].
Definition Basis.h:146
│ │ │ │ -
gtsam::Basis::EvaluationFunctor::EvaluationFunctor
EvaluationFunctor(size_t N, double x, double a, double b)
Constructor with interval [a,b].
Definition Basis.h:150
│ │ │ │ -
gtsam::Basis::EvaluationFunctor::apply
double apply(const typename DERIVED::Parameters &p, OptionalJacobian<-1, -1 > H=boost::none) const
Regular 1D evaluation.
Definition Basis.h:154
│ │ │ │ -
gtsam::Basis::EvaluationFunctor::EvaluationFunctor
EvaluationFunctor()
For serialization.
Definition Basis.h:143
│ │ │ │ -
gtsam::Basis::EvaluationFunctor::operator()
double operator()(const typename DERIVED::Parameters &p, OptionalJacobian<-1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:161
│ │ │ │ -
gtsam::Basis::VectorEvaluationFunctor
VectorEvaluationFunctor at a given x, applied to ParameterMatrix<M>.
Definition Basis.h:178
│ │ │ │ -
gtsam::Basis::VectorEvaluationFunctor::VectorEvaluationFunctor
VectorEvaluationFunctor(size_t N, double x, double a, double b)
Constructor, with interval [a,b].
Definition Basis.h:208
│ │ │ │ -
gtsam::Basis::VectorEvaluationFunctor::VectorEvaluationFunctor
EIGEN_MAKE_ALIGNED_OPERATOR_NEW VectorEvaluationFunctor()
For serialization.
Definition Basis.h:200
│ │ │ │ -
gtsam::Basis::VectorEvaluationFunctor::VectorEvaluationFunctor
VectorEvaluationFunctor(size_t N, double x)
Default Constructor.
Definition Basis.h:203
│ │ │ │ -
gtsam::Basis::VectorEvaluationFunctor::calculateJacobian
void calculateJacobian()
Calculate the M*(M*N) Jacobian of this functor with respect to the M*N parameter matrix P.
Definition Basis.h:192
│ │ │ │ -
gtsam::Basis::VectorEvaluationFunctor::operator()
VectorM operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:221
│ │ │ │ -
gtsam::Basis::VectorEvaluationFunctor::apply
VectorM apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
M-dimensional evaluation.
Definition Basis.h:214
│ │ │ │ -
gtsam::Basis::VectorComponentFunctor
Given a M*N Matrix of M-vectors at N polynomial points, an instance of VectorComponentFunctor compute...
Definition Basis.h:235
│ │ │ │ -
gtsam::Basis::VectorComponentFunctor::operator()
double operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:280
│ │ │ │ -
gtsam::Basis::VectorComponentFunctor::VectorComponentFunctor
VectorComponentFunctor()
For serialization.
Definition Basis.h:258
│ │ │ │ -
gtsam::Basis::VectorComponentFunctor::VectorComponentFunctor
VectorComponentFunctor(size_t N, size_t i, double x)
Construct with row index.
Definition Basis.h:261
│ │ │ │ -
gtsam::Basis::VectorComponentFunctor::VectorComponentFunctor
VectorComponentFunctor(size_t N, size_t i, double x, double a, double b)
Construct with row index and interval.
Definition Basis.h:267
│ │ │ │ -
gtsam::Basis::VectorComponentFunctor::apply
double apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
Calculate component of component rowIndex_ of P.
Definition Basis.h:273
│ │ │ │ -
gtsam::Basis::ManifoldEvaluationFunctor
Manifold EvaluationFunctor at a given x, applied to ParameterMatrix<M>.
Definition Basis.h:301
│ │ │ │ -
gtsam::Basis::ManifoldEvaluationFunctor::ManifoldEvaluationFunctor
ManifoldEvaluationFunctor(size_t N, double x, double a, double b)
Constructor, with interval [a,b].
Definition Basis.h:313
│ │ │ │ -
gtsam::Basis::ManifoldEvaluationFunctor::apply
T apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
Manifold evaluation.
Definition Basis.h:317
│ │ │ │ -
gtsam::Basis::ManifoldEvaluationFunctor::ManifoldEvaluationFunctor
ManifoldEvaluationFunctor()
For serialization.
Definition Basis.h:307
│ │ │ │ -
gtsam::Basis::ManifoldEvaluationFunctor::ManifoldEvaluationFunctor
ManifoldEvaluationFunctor(size_t N, double x)
Default Constructor.
Definition Basis.h:310
│ │ │ │ -
gtsam::Basis::ManifoldEvaluationFunctor::operator()
T operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:336
│ │ │ │ -
gtsam::Basis::DerivativeFunctorBase
Base class for functors below that calculate derivative weights.
Definition Basis.h:343
│ │ │ │ -
gtsam::Basis::DerivativeFunctorBase::DerivativeFunctorBase
DerivativeFunctorBase()
For serialization.
Definition Basis.h:349
│ │ │ │ -
gtsam::Basis::DerivativeFunctor
An instance of a DerivativeFunctor calculates f'(x;p) at a given x, applied to Parameters p.
Definition Basis.h:369
│ │ │ │ -
gtsam::Basis::DerivativeFunctor::DerivativeFunctor
DerivativeFunctor()
For serialization.
Definition Basis.h:372
│ │ │ │ -
gtsam::Basis::DerivativeFunctor::operator()
double operator()(const typename DERIVED::Parameters &p, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:385
│ │ │ │ -
gtsam::Basis::VectorDerivativeFunctor
VectorDerivativeFunctor at a given x, applied to ParameterMatrix<M>.
Definition Basis.h:400
│ │ │ │ -
gtsam::Basis::VectorDerivativeFunctor::calculateJacobian
void calculateJacobian()
Calculate the M*(M*N) Jacobian of this functor with respect to the M*N parameter matrix P.
Definition Basis.h:414
│ │ │ │ -
gtsam::Basis::VectorDerivativeFunctor::VectorDerivativeFunctor
VectorDerivativeFunctor(size_t N, double x, double a, double b)
Constructor, with optional interval [a,b].
Definition Basis.h:430
│ │ │ │ -
gtsam::Basis::VectorDerivativeFunctor::operator()
VectorM operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:441
│ │ │ │ -
gtsam::Basis::VectorDerivativeFunctor::VectorDerivativeFunctor
EIGEN_MAKE_ALIGNED_OPERATOR_NEW VectorDerivativeFunctor()
For serialization.
Definition Basis.h:422
│ │ │ │ -
gtsam::Basis::VectorDerivativeFunctor::VectorDerivativeFunctor
VectorDerivativeFunctor(size_t N, double x)
Default Constructor.
Definition Basis.h:425
│ │ │ │ -
gtsam::Basis::ComponentDerivativeFunctor
Given a M*N Matrix of M-vectors at N polynomial points, an instance of ComponentDerivativeFunctor com...
Definition Basis.h:456
│ │ │ │ -
gtsam::Basis::ComponentDerivativeFunctor::ComponentDerivativeFunctor
ComponentDerivativeFunctor(size_t N, size_t i, double x)
Construct with row index.
Definition Basis.h:482
│ │ │ │ -
gtsam::Basis::ComponentDerivativeFunctor::operator()
double operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:499
│ │ │ │ -
gtsam::Basis::ComponentDerivativeFunctor::ComponentDerivativeFunctor
ComponentDerivativeFunctor()
For serialization.
Definition Basis.h:479
│ │ │ │ -
gtsam::Basis::ComponentDerivativeFunctor::ComponentDerivativeFunctor
ComponentDerivativeFunctor(size_t N, size_t i, double x, double a, double b)
Construct with row index and interval.
Definition Basis.h:488
│ │ │ │ -
gtsam::Basis::ComponentDerivativeFunctor::apply
double apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
Calculate derivative of component rowIndex_ of F.
Definition Basis.h:493
│ │ │ │ -
gtsam::ParameterMatrix
A matrix abstraction of MxN values at the Basis points.
Definition ParameterMatrix.h:38
│ │ │ │ +
gtsam::Chebyshev2
Chebyshev Interpolation on Chebyshev points of the second kind Note that N here, the number of points...
Definition Chebyshev2.h:46
│ │ │ │ +
gtsam::Chebyshev2::Point
static double Point(size_t N, int j)
Specific Chebyshev point.
Definition Chebyshev2.h:55
│ │ │ │ +
gtsam::Chebyshev2::Point
static double Point(size_t N, int j, double a, double b)
Specific Chebyshev point, within [a,b] interval.
Definition Chebyshev2.h:64
│ │ │ │ +
gtsam::Chebyshev2::Points
static Vector Points(size_t N)
All Chebyshev points.
Definition Chebyshev2.h:72
│ │ │ │ +
gtsam::Chebyshev2::Points
static Vector Points(size_t N, double a, double b)
All Chebyshev points, within [a,b] interval.
Definition Chebyshev2.h:79
│ │ │ │ +
gtsam::Chebyshev2::matrix
static Matrix matrix(std::function< Eigen::Matrix< double, M, 1 >(double)> f, size_t N, double a=-1, double b=1)
Create matrix of values at Chebyshev points given vector-valued function.
Definition Chebyshev2.h:135
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,554 +1,128 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Basis.h │ │ │ │ │ +Chebyshev2.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_i_s_/_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_._h> │ │ │ │ │ -24 │ │ │ │ │ -25#include │ │ │ │ │ -26 │ │ │ │ │ -68namespace _g_t_s_a_m { │ │ │ │ │ -69 │ │ │ │ │ -70using Weights = Eigen::Matrix; /* 1xN vector */ │ │ │ │ │ -71 │ │ │ │ │ -84template │ │ │ │ │ -_8_5Matrix _k_r_o_n_e_c_k_e_r_P_r_o_d_u_c_t_I_d_e_n_t_i_t_y(const Weights& w) { │ │ │ │ │ -86 Matrix result(M, w.cols() * M); │ │ │ │ │ -87 result.setZero(); │ │ │ │ │ -88 │ │ │ │ │ -89 for (int i = 0; i < w.cols(); i++) { │ │ │ │ │ -90 result.block(0, i * M, M, M).diagonal().array() = w(i); │ │ │ │ │ -91 } │ │ │ │ │ -92 return result; │ │ │ │ │ -93} │ │ │ │ │ -94 │ │ │ │ │ -99template │ │ │ │ │ -_1_0_0class _B_a_s_i_s { │ │ │ │ │ -101 public: │ │ │ │ │ -_1_0_7 static Matrix _W_e_i_g_h_t_M_a_t_r_i_x(size_t N, const Vector& X) { │ │ │ │ │ -108 Matrix W(X.size(), N); │ │ │ │ │ -109 for (int i = 0; i < X.size(); i++) │ │ │ │ │ -110 W.row(i) = DERIVED::CalculateWeights(N, X(i)); │ │ │ │ │ -111 return W; │ │ │ │ │ -112 } │ │ │ │ │ -113 │ │ │ │ │ -_1_2_3 static Matrix _W_e_i_g_h_t_M_a_t_r_i_x(size_t N, const Vector& X, double a, double b) { │ │ │ │ │ -124 Matrix W(X.size(), N); │ │ │ │ │ -125 for (int i = 0; i < X.size(); i++) │ │ │ │ │ -126 W.row(i) = DERIVED::CalculateWeights(N, X(i), a, b); │ │ │ │ │ -127 return W; │ │ │ │ │ -128 } │ │ │ │ │ -129 │ │ │ │ │ -_1_3_7 class _E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r { │ │ │ │ │ -138 protected: │ │ │ │ │ -139 Weights weights_; │ │ │ │ │ -140 │ │ │ │ │ -141 public: │ │ │ │ │ -_1_4_3 _E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r() {} │ │ │ │ │ +33#pragma once │ │ │ │ │ +34 │ │ │ │ │ +35#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ +36#include <_g_t_s_a_m_/_b_a_s_e_/_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h> │ │ │ │ │ +37#include <_g_t_s_a_m_/_b_a_s_i_s_/_B_a_s_i_s_._h> │ │ │ │ │ +38 │ │ │ │ │ +39namespace _g_t_s_a_m { │ │ │ │ │ +40 │ │ │ │ │ +_4_6class GTSAM_EXPORT _C_h_e_b_y_s_h_e_v_2 : public _B_a_s_i_s { │ │ │ │ │ +47 public: │ │ │ │ │ +48 EIGEN_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ +49 │ │ │ │ │ +50 using _B_a_s_e = _B_a_s_i_s_<_C_h_e_b_y_s_h_e_v_2_>; │ │ │ │ │ +51 using Parameters = Eigen::Matrix; │ │ │ │ │ +52 using DiffMatrix = Eigen::Matrix; │ │ │ │ │ +53 │ │ │ │ │ +_5_5 static double _P_o_i_n_t(size_t N, int j) { │ │ │ │ │ +56 assert(j >= 0 && size_t(j) < N); │ │ │ │ │ +57 const double dtheta = M_PI / (N > 1 ? (N - 1) : 1); │ │ │ │ │ +58 // We add -PI so that we get values ordered from -1 to +1 │ │ │ │ │ +59 // sin(- M_PI_2 + dtheta*j); also works │ │ │ │ │ +60 return cos(-M_PI + dtheta * j); │ │ │ │ │ +61 } │ │ │ │ │ +62 │ │ │ │ │ +_6_4 static double _P_o_i_n_t(size_t N, int j, double a, double b) { │ │ │ │ │ +65 assert(j >= 0 && size_t(j) < N); │ │ │ │ │ +66 const double dtheta = M_PI / (N - 1); │ │ │ │ │ +67 // We add -PI so that we get values ordered from -1 to +1 │ │ │ │ │ +68 return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2; │ │ │ │ │ +69 } │ │ │ │ │ +70 │ │ │ │ │ +_7_2 static Vector _P_o_i_n_t_s(size_t N) { │ │ │ │ │ +73 Vector points(N); │ │ │ │ │ +74 for (size_t j = 0; j < N; j++) points(j) = Point(N, j); │ │ │ │ │ +75 return points; │ │ │ │ │ +76 } │ │ │ │ │ +77 │ │ │ │ │ +_7_9 static Vector _P_o_i_n_t_s(size_t N, double a, double b) { │ │ │ │ │ +80 Vector points = Points(N); │ │ │ │ │ +81 const double T1 = (a + b) / 2, T2 = (b - a) / 2; │ │ │ │ │ +82 points = T1 + (T2 * points).array(); │ │ │ │ │ +83 return points; │ │ │ │ │ +84 } │ │ │ │ │ +85 │ │ │ │ │ +94 static Weights CalculateWeights(size_t N, double x, double a = -1, │ │ │ │ │ +95 double b = 1); │ │ │ │ │ +96 │ │ │ │ │ +101 static Weights DerivativeWeights(size_t N, double x, double a = -1, │ │ │ │ │ +102 double b = 1); │ │ │ │ │ +103 │ │ │ │ │ +108 static DiffMatrix DifferentiationMatrix(size_t N, double a = -1, │ │ │ │ │ +109 double b = 1); │ │ │ │ │ +110 │ │ │ │ │ +129 static Weights IntegrationWeights(size_t N, double a = -1, double b = 1); │ │ │ │ │ +130 │ │ │ │ │ +134 template │ │ │ │ │ +_1_3_5 static Matrix _m_a_t_r_i_x(std::function(double)> f, │ │ │ │ │ +136 size_t N, double a = -1, double b = 1) { │ │ │ │ │ +137 Matrix Xmat(M, N); │ │ │ │ │ +138 for (size_t j = 0; j < N; j++) { │ │ │ │ │ +139 Xmat.col(j) = f(Point(N, j, a, b)); │ │ │ │ │ +140 } │ │ │ │ │ +141 return Xmat; │ │ │ │ │ +142 } │ │ │ │ │ +143}; // \ Chebyshev2 │ │ │ │ │ 144 │ │ │ │ │ -_1_4_6 _E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r(size_t N, double x) │ │ │ │ │ -147 : weights_(DERIVED::CalculateWeights(N, x)) {} │ │ │ │ │ -148 │ │ │ │ │ -_1_5_0 _E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r(size_t N, double x, double a, double b) │ │ │ │ │ -151 : weights_(DERIVED::CalculateWeights(N, x, a, b)) {} │ │ │ │ │ -152 │ │ │ │ │ -_1_5_4 double _a_p_p_l_y(const typename DERIVED::Parameters& p, │ │ │ │ │ -155 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_-_1_,_ _-_1_> H = boost::none) const { │ │ │ │ │ -156 if (H) *H = weights_; │ │ │ │ │ -157 return (weights_ * p)(0); │ │ │ │ │ -158 } │ │ │ │ │ -159 │ │ │ │ │ -_1_6_1 double _o_p_e_r_a_t_o_r_(_)(const typename DERIVED::Parameters& p, │ │ │ │ │ -162 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_-_1_,_ _-_1_> H = boost::none) const { │ │ │ │ │ -163 return _a_p_p_l_y(p, H); // might call apply in derived │ │ │ │ │ -164 } │ │ │ │ │ -165 │ │ │ │ │ -166 void print(const std::string& s = "") const { │ │ │ │ │ -167 std::cout << s << (s != "" ? " " : "") << weights_ << std::endl; │ │ │ │ │ -168 } │ │ │ │ │ -169 }; │ │ │ │ │ -170 │ │ │ │ │ -177 template │ │ │ │ │ -_1_7_8 class _V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r : protected _E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r { │ │ │ │ │ -179 protected: │ │ │ │ │ -180 using VectorM = Eigen::Matrix; │ │ │ │ │ -181 using Jacobian = Eigen::Matrix; │ │ │ │ │ -182 Jacobian H_; │ │ │ │ │ -183 │ │ │ │ │ -_1_9_2 void _c_a_l_c_u_l_a_t_e_J_a_c_o_b_i_a_n() { │ │ │ │ │ -193 H_ = kroneckerProductIdentity(this->weights_); │ │ │ │ │ -194 } │ │ │ │ │ -195 │ │ │ │ │ -196 public: │ │ │ │ │ -197 EIGEN_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ -198 │ │ │ │ │ -_2_0_0 _V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r() {} │ │ │ │ │ -201 │ │ │ │ │ -_2_0_3 _V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r(size_t N, double x) : _E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r(N, x) { │ │ │ │ │ -204 _c_a_l_c_u_l_a_t_e_J_a_c_o_b_i_a_n(); │ │ │ │ │ -205 } │ │ │ │ │ -206 │ │ │ │ │ -_2_0_8 _V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r(size_t N, double x, double a, double b) │ │ │ │ │ -209 : _E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r(N, x, a, b) { │ │ │ │ │ -210 _c_a_l_c_u_l_a_t_e_J_a_c_o_b_i_a_n(); │ │ │ │ │ -211 } │ │ │ │ │ -212 │ │ │ │ │ -_2_1_4 VectorM _a_p_p_l_y(const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>& P, │ │ │ │ │ -215 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_/_*_M_x_N_*_/_ _-_1_,_ _-_1_> H = boost::none) const { │ │ │ │ │ -216 if (H) *H = H_; │ │ │ │ │ -217 return P.matrix() * this->weights_.transpose(); │ │ │ │ │ -218 } │ │ │ │ │ -219 │ │ │ │ │ -_2_2_1 VectorM _o_p_e_r_a_t_o_r_(_)(const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>& P, │ │ │ │ │ -222 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_/_*_M_x_N_*_/_ _-_1_,_ _-_1_> H = boost::none) const { │ │ │ │ │ -223 return _a_p_p_l_y(P, H); │ │ │ │ │ -224 } │ │ │ │ │ -225 }; │ │ │ │ │ -226 │ │ │ │ │ -234 template │ │ │ │ │ -_2_3_5 class _V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r : public _E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r { │ │ │ │ │ -236 protected: │ │ │ │ │ -237 using Jacobian = Eigen::Matrix; │ │ │ │ │ -238 size_t rowIndex_; │ │ │ │ │ -239 Jacobian H_; │ │ │ │ │ -240 │ │ │ │ │ -241 /* │ │ │ │ │ -242 * Calculate the `1*(M*N)` Jacobian of this functor with respect to │ │ │ │ │ -243 * the M*N parameter matrix `P`. │ │ │ │ │ -244 * We flatten assuming column-major order, e.g., if N=3 and M=2, we have │ │ │ │ │ -245 * H=[w(0) 0 w(1) 0 w(2) 0] for rowIndex==0 │ │ │ │ │ -246 * H=[0 w(0) 0 w(1) 0 w(2)] for rowIndex==1 │ │ │ │ │ -247 * i.e., one row of the Kronecker product of weights_ with the │ │ │ │ │ -248 * MxM identity matrix. See also VectorEvaluationFunctor. │ │ │ │ │ -249 */ │ │ │ │ │ -250 void calculateJacobian(size_t N) { │ │ │ │ │ -251 H_.setZero(1, M * N); │ │ │ │ │ -252 for (int j = 0; j < EvaluationFunctor::weights_.size(); j++) │ │ │ │ │ -253 H_(0, rowIndex_ + j * M) = EvaluationFunctor::weights_(j); │ │ │ │ │ -254 } │ │ │ │ │ -255 │ │ │ │ │ -256 public: │ │ │ │ │ -_2_5_8 _V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r() {} │ │ │ │ │ -259 │ │ │ │ │ -_2_6_1 _V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r(size_t N, size_t i, double x) │ │ │ │ │ -262 : _E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r(N, x), rowIndex_(i) { │ │ │ │ │ -263 calculateJacobian(N); │ │ │ │ │ -264 } │ │ │ │ │ -265 │ │ │ │ │ -_2_6_7 _V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r(size_t N, size_t i, double x, double a, double b) │ │ │ │ │ -268 : _E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r(N, x, a, b), rowIndex_(i) { │ │ │ │ │ -269 calculateJacobian(N); │ │ │ │ │ -270 } │ │ │ │ │ -271 │ │ │ │ │ -_2_7_3 double _a_p_p_l_y(const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>& P, │ │ │ │ │ -274 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_/_*_1_x_M_N_*_/_ _-_1_,_ _-_1_> H = boost::none) const { │ │ │ │ │ -275 if (H) *H = H_; │ │ │ │ │ -276 return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose(); │ │ │ │ │ -277 } │ │ │ │ │ -278 │ │ │ │ │ -_2_8_0 double _o_p_e_r_a_t_o_r_(_)(const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>& P, │ │ │ │ │ -281 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_/_*_1_x_M_N_*_/_ _-_1_,_ _-_1_> H = boost::none) const { │ │ │ │ │ -282 return _a_p_p_l_y(P, H); │ │ │ │ │ -283 } │ │ │ │ │ -284 }; │ │ │ │ │ -285 │ │ │ │ │ -299 template │ │ │ │ │ -_3_0_0 class _M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r │ │ │ │ │ -301 : public _V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r::dimension> { │ │ │ │ │ -302 enum { M = _t_r_a_i_t_s_<_T_>_:_:_d_i_m_e_n_s_i_o_n }; │ │ │ │ │ -303 using Base = _V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_<_M_>; │ │ │ │ │ -304 │ │ │ │ │ -305 public: │ │ │ │ │ -_3_0_7 _M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r() {} │ │ │ │ │ -308 │ │ │ │ │ -_3_1_0 _M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r(size_t N, double x) : Base(N, x) {} │ │ │ │ │ -311 │ │ │ │ │ -_3_1_3 _M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r(size_t N, double x, double a, double b) │ │ │ │ │ -314 : Base(N, x, a, b) {} │ │ │ │ │ -315 │ │ │ │ │ -_3_1_7 T _a_p_p_l_y(const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>& P, │ │ │ │ │ -318 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_/_*_M_x_M_N_*_/_ _-_1_,_ _-_1_> H = boost::none) const { │ │ │ │ │ -319 // Interpolate the M-dimensional vector to yield a vector in tangent space │ │ │ │ │ -320 Eigen::Matrix xi = Base::operator()(P, H); │ │ │ │ │ -321 │ │ │ │ │ -322 // Now call retract with this M-vector, possibly with derivatives │ │ │ │ │ -323 Eigen::Matrix D_result_xi; │ │ │ │ │ -324 T result = T::ChartAtOrigin::Retract(xi, H ? &D_result_xi : 0); │ │ │ │ │ -325 │ │ │ │ │ -326 // Finally, if derivatives are asked, apply chain rule where H is Mx(M*N) │ │ │ │ │ -327 // derivative of interpolation and D_result_xi is MxM derivative of │ │ │ │ │ -328 // retract. │ │ │ │ │ -329 if (H) *H = D_result_xi * (*H); │ │ │ │ │ -330 │ │ │ │ │ -331 // and return a T │ │ │ │ │ -332 return result; │ │ │ │ │ -333 } │ │ │ │ │ -334 │ │ │ │ │ -_3_3_6 T _o_p_e_r_a_t_o_r_(_)(const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>& P, │ │ │ │ │ -337 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_/_*_M_x_N_*_/_ _-_1_,_ _-_1_> H = boost::none) const { │ │ │ │ │ -338 return _a_p_p_l_y(P, H); // might call apply in derived │ │ │ │ │ -339 } │ │ │ │ │ -340 }; │ │ │ │ │ -341 │ │ │ │ │ -_3_4_3 class _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e { │ │ │ │ │ -344 protected: │ │ │ │ │ -345 Weights weights_; │ │ │ │ │ -346 │ │ │ │ │ -347 public: │ │ │ │ │ -_3_4_9 _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e() {} │ │ │ │ │ -350 │ │ │ │ │ -351 _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e(size_t N, double x) │ │ │ │ │ -352 : weights_(DERIVED::DerivativeWeights(N, x)) {} │ │ │ │ │ -353 │ │ │ │ │ -354 _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e(size_t N, double x, double a, double b) │ │ │ │ │ -355 : weights_(DERIVED::DerivativeWeights(N, x, a, b)) {} │ │ │ │ │ -356 │ │ │ │ │ -357 void print(const std::string& s = "") const { │ │ │ │ │ -358 std::cout << s << (s != "" ? " " : "") << weights_ << std::endl; │ │ │ │ │ -359 } │ │ │ │ │ -360 }; │ │ │ │ │ -361 │ │ │ │ │ -_3_6_9 class _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r : protected _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e { │ │ │ │ │ -370 public: │ │ │ │ │ -_3_7_2 _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r() {} │ │ │ │ │ -373 │ │ │ │ │ -374 _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r(size_t N, double x) : _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e(N, x) {} │ │ │ │ │ -375 │ │ │ │ │ -376 _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r(size_t N, double x, double a, double b) │ │ │ │ │ -377 : _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e(N, x, a, b) {} │ │ │ │ │ -378 │ │ │ │ │ -379 double apply(const typename DERIVED::Parameters& p, │ │ │ │ │ -380 OptionalJacobian H = boost::none) const { │ │ │ │ │ -381 if (H) *H = this->weights_; │ │ │ │ │ -382 return (this->weights_ * p)(0); │ │ │ │ │ -383 } │ │ │ │ │ -_3_8_5 double _o_p_e_r_a_t_o_r_(_)(const typename DERIVED::Parameters& p, │ │ │ │ │ -386 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_/_*_1_x_N_*_/_ _-_1_,_ _-_1_> H = boost::none) const { │ │ │ │ │ -387 return apply(p, H); // might call apply in derived │ │ │ │ │ -388 } │ │ │ │ │ -389 }; │ │ │ │ │ -390 │ │ │ │ │ -399 template │ │ │ │ │ -_4_0_0 class _V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r : protected _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e { │ │ │ │ │ -401 protected: │ │ │ │ │ -402 using VectorM = Eigen::Matrix; │ │ │ │ │ -403 using Jacobian = Eigen::Matrix; │ │ │ │ │ -404 Jacobian H_; │ │ │ │ │ -405 │ │ │ │ │ -_4_1_4 void _c_a_l_c_u_l_a_t_e_J_a_c_o_b_i_a_n() { │ │ │ │ │ -415 H_ = kroneckerProductIdentity(this->weights_); │ │ │ │ │ -416 } │ │ │ │ │ -417 │ │ │ │ │ -418 public: │ │ │ │ │ -419 EIGEN_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ -420 │ │ │ │ │ -_4_2_2 _V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r() {} │ │ │ │ │ -423 │ │ │ │ │ -_4_2_5 _V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r(size_t N, double x) : _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e(N, x) { │ │ │ │ │ -426 _c_a_l_c_u_l_a_t_e_J_a_c_o_b_i_a_n(); │ │ │ │ │ -427 } │ │ │ │ │ -428 │ │ │ │ │ -_4_3_0 _V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r(size_t N, double x, double a, double b) │ │ │ │ │ -431 : _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e(N, x, a, b) { │ │ │ │ │ -432 _c_a_l_c_u_l_a_t_e_J_a_c_o_b_i_a_n(); │ │ │ │ │ -433 } │ │ │ │ │ -434 │ │ │ │ │ -435 VectorM apply(const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>& P, │ │ │ │ │ -436 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_/_*_M_x_M_N_*_/_ _-_1_,_ _-_1_> H = boost::none) const { │ │ │ │ │ -437 if (H) *H = H_; │ │ │ │ │ -438 return P.matrix() * this->weights_.transpose(); │ │ │ │ │ -439 } │ │ │ │ │ -_4_4_1 VectorM _o_p_e_r_a_t_o_r_(_)( │ │ │ │ │ -442 const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>& P, │ │ │ │ │ -443 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_/_*_M_x_M_N_*_/_ _-_1_,_ _-_1_> H = boost::none) const { │ │ │ │ │ -444 return apply(P, H); │ │ │ │ │ -445 } │ │ │ │ │ -446 }; │ │ │ │ │ -447 │ │ │ │ │ -455 template │ │ │ │ │ -_4_5_6 class _C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r : protected _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e { │ │ │ │ │ -457 protected: │ │ │ │ │ -458 using Jacobian = Eigen::Matrix; │ │ │ │ │ -459 size_t rowIndex_; │ │ │ │ │ -460 Jacobian H_; │ │ │ │ │ -461 │ │ │ │ │ -462 /* │ │ │ │ │ -463 * Calculate the `1*(M*N)` Jacobian of this functor with respect to │ │ │ │ │ -464 * the M*N parameter matrix `P`. │ │ │ │ │ -465 * We flatten assuming column-major order, e.g., if N=3 and M=2, we have │ │ │ │ │ -466 * H=[w(0) 0 w(1) 0 w(2) 0] for rowIndex==0 │ │ │ │ │ -467 * H=[0 w(0) 0 w(1) 0 w(2)] for rowIndex==1 │ │ │ │ │ -468 * i.e., one row of the Kronecker product of weights_ with the │ │ │ │ │ -469 * MxM identity matrix. See also VectorDerivativeFunctor. │ │ │ │ │ -470 */ │ │ │ │ │ -471 void calculateJacobian(size_t N) { │ │ │ │ │ -472 H_.setZero(1, M * N); │ │ │ │ │ -473 for (int j = 0; j < this->weights_.size(); j++) │ │ │ │ │ -474 H_(0, rowIndex_ + j * M) = this->weights_(j); │ │ │ │ │ -475 } │ │ │ │ │ -476 │ │ │ │ │ -477 public: │ │ │ │ │ -_4_7_9 _C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r() {} │ │ │ │ │ -480 │ │ │ │ │ -_4_8_2 _C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r(size_t N, size_t i, double x) │ │ │ │ │ -483 : _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e(N, x), rowIndex_(i) { │ │ │ │ │ -484 calculateJacobian(N); │ │ │ │ │ -485 } │ │ │ │ │ -486 │ │ │ │ │ -_4_8_8 _C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r(size_t N, size_t i, double x, double a, double │ │ │ │ │ -b) │ │ │ │ │ -489 : _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e(N, x, a, b), rowIndex_(i) { │ │ │ │ │ -490 calculateJacobian(N); │ │ │ │ │ -491 } │ │ │ │ │ -_4_9_3 double _a_p_p_l_y(const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>& P, │ │ │ │ │ -494 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_/_*_1_x_M_N_*_/_ _-_1_,_ _-_1_> H = boost::none) const { │ │ │ │ │ -495 if (H) *H = H_; │ │ │ │ │ -496 return P.row(rowIndex_) * this->weights_.transpose(); │ │ │ │ │ -497 } │ │ │ │ │ -_4_9_9 double _o_p_e_r_a_t_o_r_(_)(const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>& P, │ │ │ │ │ -500 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_/_*_1_x_M_N_*_/_ _-_1_,_ _-_1_> H = boost::none) const { │ │ │ │ │ -501 return _a_p_p_l_y(P, H); │ │ │ │ │ -502 } │ │ │ │ │ -503 }; │ │ │ │ │ -504 │ │ │ │ │ -505}; │ │ │ │ │ -506 │ │ │ │ │ -507} // namespace gtsam │ │ │ │ │ +145} // namespace gtsam │ │ │ │ │ _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h │ │ │ │ │ Special class for optional Jacobian arguments. │ │ │ │ │ -_M_a_t_r_i_x_._h │ │ │ │ │ -typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ -_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_._h │ │ │ │ │ -Define ParameterMatrix class which is used to store values at interpolation │ │ │ │ │ -points. │ │ │ │ │ -_g_t_s_a_m_:_:_k_r_o_n_e_c_k_e_r_P_r_o_d_u_c_t_I_d_e_n_t_i_t_y │ │ │ │ │ -Matrix kroneckerProductIdentity(const Weights &w) │ │ │ │ │ -Function for computing the kronecker product of the 1*N Weight vector w with │ │ │ │ │ -the MxM identity matrix ... │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:85 │ │ │ │ │ +_M_a_n_i_f_o_l_d_._h │ │ │ │ │ +Base class and basic functions for Manifold types. │ │ │ │ │ +_B_a_s_i_s_._h │ │ │ │ │ +Compute an interpolating basis. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ -either a fixed size o... │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ _g_t_s_a_m_:_:_B_a_s_i_s │ │ │ │ │ CRTP Base class for function bases. │ │ │ │ │ DDeeffiinniittiioonn Basis.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_W_e_i_g_h_t_M_a_t_r_i_x │ │ │ │ │ -static Matrix WeightMatrix(size_t N, const Vector &X, double a, double b) │ │ │ │ │ -Calculate weights for all x in vector X, with interval [a,b]. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:123 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_W_e_i_g_h_t_M_a_t_r_i_x │ │ │ │ │ -static Matrix WeightMatrix(size_t N, const Vector &X) │ │ │ │ │ -Calculate weights for all x in vector X. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:107 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r │ │ │ │ │ -An instance of an EvaluationFunctor calculates f(x;p) at a given x, applied to │ │ │ │ │ -Parameters p. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:137 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r │ │ │ │ │ -EvaluationFunctor(size_t N, double x) │ │ │ │ │ -Constructor with interval [a,b]. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:146 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r │ │ │ │ │ -EvaluationFunctor(size_t N, double x, double a, double b) │ │ │ │ │ -Constructor with interval [a,b]. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:150 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_a_p_p_l_y │ │ │ │ │ -double apply(const typename DERIVED::Parameters &p, OptionalJacobian<-1, -1 > │ │ │ │ │ -H=boost::none) const │ │ │ │ │ -Regular 1D evaluation. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:154 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r │ │ │ │ │ -EvaluationFunctor() │ │ │ │ │ -For serialization. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:143 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ -double operator()(const typename DERIVED::Parameters &p, OptionalJacobian<-1, - │ │ │ │ │ -1 > H=boost::none) const │ │ │ │ │ -c++ sugar │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:161 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r │ │ │ │ │ -VectorEvaluationFunctor at a given x, applied to ParameterMatrix. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:178 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r │ │ │ │ │ -VectorEvaluationFunctor(size_t N, double x, double a, double b) │ │ │ │ │ -Constructor, with interval [a,b]. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:208 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r │ │ │ │ │ -EIGEN_MAKE_ALIGNED_OPERATOR_NEW VectorEvaluationFunctor() │ │ │ │ │ -For serialization. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:200 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r │ │ │ │ │ -VectorEvaluationFunctor(size_t N, double x) │ │ │ │ │ -Default Constructor. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:203 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_c_a_l_c_u_l_a_t_e_J_a_c_o_b_i_a_n │ │ │ │ │ -void calculateJacobian() │ │ │ │ │ -Calculate the M*(M*N) Jacobian of this functor with respect to the M*N │ │ │ │ │ -parameter matrix P. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:192 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ -VectorM operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > │ │ │ │ │ -H=boost::none) const │ │ │ │ │ -c++ sugar │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:221 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_a_p_p_l_y │ │ │ │ │ -VectorM apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > │ │ │ │ │ -H=boost::none) const │ │ │ │ │ -M-dimensional evaluation. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:214 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r │ │ │ │ │ -Given a M*N Matrix of M-vectors at N polynomial points, an instance of │ │ │ │ │ -VectorComponentFunctor compute... │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:235 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ -double operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > │ │ │ │ │ -H=boost::none) const │ │ │ │ │ -c++ sugar │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:280 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r │ │ │ │ │ -VectorComponentFunctor() │ │ │ │ │ -For serialization. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:258 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r │ │ │ │ │ -VectorComponentFunctor(size_t N, size_t i, double x) │ │ │ │ │ -Construct with row index. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:261 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r │ │ │ │ │ -VectorComponentFunctor(size_t N, size_t i, double x, double a, double b) │ │ │ │ │ -Construct with row index and interval. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:267 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r_:_:_a_p_p_l_y │ │ │ │ │ -double apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > │ │ │ │ │ -H=boost::none) const │ │ │ │ │ -Calculate component of component rowIndex_ of P. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:273 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r │ │ │ │ │ -Manifold EvaluationFunctor at a given x, applied to ParameterMatrix. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:301 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r │ │ │ │ │ -ManifoldEvaluationFunctor(size_t N, double x, double a, double b) │ │ │ │ │ -Constructor, with interval [a,b]. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:313 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_a_p_p_l_y │ │ │ │ │ -T apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost:: │ │ │ │ │ -none) const │ │ │ │ │ -Manifold evaluation. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:317 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r │ │ │ │ │ -ManifoldEvaluationFunctor() │ │ │ │ │ -For serialization. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:307 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r │ │ │ │ │ -ManifoldEvaluationFunctor(size_t N, double x) │ │ │ │ │ -Default Constructor. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:310 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ -T operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > │ │ │ │ │ -H=boost::none) const │ │ │ │ │ -c++ sugar │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:336 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e │ │ │ │ │ -Base class for functors below that calculate derivative weights. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:343 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e_:_:_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e │ │ │ │ │ -DerivativeFunctorBase() │ │ │ │ │ -For serialization. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:349 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r │ │ │ │ │ -An instance of a DerivativeFunctor calculates f'(x;p) at a given x, applied to │ │ │ │ │ -Parameters p. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:369 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_:_:_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r │ │ │ │ │ -DerivativeFunctor() │ │ │ │ │ -For serialization. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:372 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ -double operator()(const typename DERIVED::Parameters &p, OptionalJacobian< -1, │ │ │ │ │ --1 > H=boost::none) const │ │ │ │ │ -c++ sugar │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:385 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r │ │ │ │ │ -VectorDerivativeFunctor at a given x, applied to ParameterMatrix. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:400 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_:_:_c_a_l_c_u_l_a_t_e_J_a_c_o_b_i_a_n │ │ │ │ │ -void calculateJacobian() │ │ │ │ │ -Calculate the M*(M*N) Jacobian of this functor with respect to the M*N │ │ │ │ │ -parameter matrix P. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:414 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r │ │ │ │ │ -VectorDerivativeFunctor(size_t N, double x, double a, double b) │ │ │ │ │ -Constructor, with optional interval [a,b]. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:430 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ -VectorM operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > │ │ │ │ │ -H=boost::none) const │ │ │ │ │ -c++ sugar │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:441 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r │ │ │ │ │ -EIGEN_MAKE_ALIGNED_OPERATOR_NEW VectorDerivativeFunctor() │ │ │ │ │ -For serialization. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:422 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r │ │ │ │ │ -VectorDerivativeFunctor(size_t N, double x) │ │ │ │ │ -Default Constructor. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:425 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r │ │ │ │ │ -Given a M*N Matrix of M-vectors at N polynomial points, an instance of │ │ │ │ │ -ComponentDerivativeFunctor com... │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:456 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r │ │ │ │ │ -ComponentDerivativeFunctor(size_t N, size_t i, double x) │ │ │ │ │ -Construct with row index. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:482 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ -double operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > │ │ │ │ │ -H=boost::none) const │ │ │ │ │ -c++ sugar │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:499 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r │ │ │ │ │ -ComponentDerivativeFunctor() │ │ │ │ │ -For serialization. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:479 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r │ │ │ │ │ -ComponentDerivativeFunctor(size_t N, size_t i, double x, double a, double b) │ │ │ │ │ -Construct with row index and interval. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:488 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_:_:_a_p_p_l_y │ │ │ │ │ -double apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > │ │ │ │ │ -H=boost::none) const │ │ │ │ │ -Calculate derivative of component rowIndex_ of F. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:493 │ │ │ │ │ -_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x │ │ │ │ │ -A matrix abstraction of MxN values at the Basis points. │ │ │ │ │ -DDeeffiinniittiioonn ParameterMatrix.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_C_h_e_b_y_s_h_e_v_2 │ │ │ │ │ +Chebyshev Interpolation on Chebyshev points of the second kind Note that N │ │ │ │ │ +here, the number of points... │ │ │ │ │ +DDeeffiinniittiioonn Chebyshev2.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_C_h_e_b_y_s_h_e_v_2_:_:_P_o_i_n_t │ │ │ │ │ +static double Point(size_t N, int j) │ │ │ │ │ +Specific Chebyshev point. │ │ │ │ │ +DDeeffiinniittiioonn Chebyshev2.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_C_h_e_b_y_s_h_e_v_2_:_:_P_o_i_n_t │ │ │ │ │ +static double Point(size_t N, int j, double a, double b) │ │ │ │ │ +Specific Chebyshev point, within [a,b] interval. │ │ │ │ │ +DDeeffiinniittiioonn Chebyshev2.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_C_h_e_b_y_s_h_e_v_2_:_:_P_o_i_n_t_s │ │ │ │ │ +static Vector Points(size_t N) │ │ │ │ │ +All Chebyshev points. │ │ │ │ │ +DDeeffiinniittiioonn Chebyshev2.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_C_h_e_b_y_s_h_e_v_2_:_:_P_o_i_n_t_s │ │ │ │ │ +static Vector Points(size_t N, double a, double b) │ │ │ │ │ +All Chebyshev points, within [a,b] interval. │ │ │ │ │ +DDeeffiinniittiioonn Chebyshev2.h:79 │ │ │ │ │ +_g_t_s_a_m_:_:_C_h_e_b_y_s_h_e_v_2_:_:_m_a_t_r_i_x │ │ │ │ │ +static Matrix matrix(std::function< Eigen::Matrix< double, M, 1 >(double)> f, │ │ │ │ │ +size_t N, double a=-1, double b=1) │ │ │ │ │ +Create matrix of values at Chebyshev points given vector-valued function. │ │ │ │ │ +DDeeffiinniittiioonn Chebyshev2.h:135 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_i_s │ │ │ │ │ - * _B_a_s_i_s_._h │ │ │ │ │ + * _C_h_e_b_y_s_h_e_v_2_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00176.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/FitBasis.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Chebyshev.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,57 +94,36 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs
│ │ │ │ -
FitBasis.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
Chebyshev.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Fit a Basis using least-squares. │ │ │ │ +

Chebyshev basis decompositions. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::FitBasis< Basis >
 Class that does regression via least squares Example usage: size_t N = 3; auto fit = FitBasis<Chebyshev2>(data_points, noise_model, N); Vector coefficients = fit.parameters();. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Typedefs

│ │ │ │ -using gtsam::Sequence = std::map< double, double >
 Our sequence representation is a map of {x: y} values where y = f(x)
 
│ │ │ │ -using gtsam::Sample = std::pair< double, double >
 A sample is a key-value pair from a sequence.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Fit a Basis using least-squares.

│ │ │ │ +

Chebyshev basis decompositions.

│ │ │ │ +
Author
Varun Agrawal, Jing Dong, Frank Dellaert
│ │ │ │
Date
July 4, 2020
│ │ │ │ -
Author
Varun Agrawal, Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,36 +1,22 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ -FitBasis.h File Reference │ │ │ │ │ -Fit a Basis using least-squares. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_F_i_t_B_a_s_i_s_<_ _B_a_s_i_s_ _> │ │ │ │ │ - Class that does regression via least squares Example usage: size_t N = │ │ │ │ │ -  3; auto fit = FitBasis(data_points, noise_model, N); Vector │ │ │ │ │ - coefficients = fit.parameters();. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +Chebyshev.cpp File Reference │ │ │ │ │ +Chebyshev basis decompositions. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -using  ggttssaamm::::SSeeqquueennccee = std::map< double, double > │ │ │ │ │ -  Our sequence representation is a map of {x: y} values where y = f(x) │ │ │ │ │ -  │ │ │ │ │ -using  ggttssaamm::::SSaammppllee = std::pair< double, double > │ │ │ │ │ -  A sample is a key-value pair from a sequence. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Fit a Basis using least-squares. │ │ │ │ │ +Chebyshev basis decompositions. │ │ │ │ │ + Author │ │ │ │ │ + Varun Agrawal, Jing Dong, Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ July 4, 2020 │ │ │ │ │ - Author │ │ │ │ │ - Varun Agrawal, Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_i_s │ │ │ │ │ - * _F_i_t_B_a_s_i_s_._h │ │ │ │ │ + * _C_h_e_b_y_s_h_e_v_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00182.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteFactorGraph.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteBayesTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,44 +94,49 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
DiscreteFactorGraph.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
DiscreteBayesTree.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ + │ │ │ │ +

Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree. │ │ │ │ +More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::DiscreteBayesTreeClique
 A clique in a DiscreteBayesTree. More...
 
class  gtsam::DiscreteBayesTree
 A Bayes tree representing a Discrete density. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

std::pair< DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr > gtsam::EliminateForMPE (const DiscreteFactorGraph &factors, const Ordering &frontalKeys)
 Alternate elimination function for that creates non-normalized lookup tables.
 
std::pair< DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr > gtsam::EliminateDiscrete (const DiscreteFactorGraph &factors, const Ordering &frontalKeys)
 Main elimination function for DiscreteFactorGraph.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Feb 14, 2011
│ │ │ │ -
Author
Duy-Nguyen Ta
│ │ │ │ +

Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree.

│ │ │ │ +

DiscreteBayesTree

Author
Frank Dellaert
│ │ │ │
│ │ │ │ -Frank Dellaert
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,34 +1,30 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -DiscreteFactorGraph.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +DiscreteBayesTree.h File Reference │ │ │ │ │ +Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e │ │ │ │ │ +  A clique in a _D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e │ │ │ │ │ +  A Bayes tree representing a Discrete density. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -std::pair< _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_: _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_F_o_r_M_P_E (const │ │ │ │ │ -_s_h_a_r_e_d___p_t_r, DecisionTreeFactor:: _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h &factors, const │ │ │ │ │ - shared_ptr >  _O_r_d_e_r_i_n_g &frontalKeys) │ │ │ │ │ -  Alternate elimination function for that │ │ │ │ │ - creates non-normalized lookup tables. │ │ │ │ │ -  │ │ │ │ │ -std::pair< _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_: _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_D_i_s_c_r_e_t_e (const │ │ │ │ │ -_s_h_a_r_e_d___p_t_r, DecisionTreeFactor:: _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h &factors, const │ │ │ │ │ - shared_ptr >  _O_r_d_e_r_i_n_g &frontalKeys) │ │ │ │ │ -  Main elimination function for │ │ │ │ │ - _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Feb 14, 2011 │ │ │ │ │ +Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree. │ │ │ │ │ +DiscreteBayesTree │ │ │ │ │ Author │ │ │ │ │ - Duy-Nguyen Ta │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_._c_p_p │ │ │ │ │ + * _D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00182.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,3 @@ │ │ │ │ │ var a00182 = [ │ │ │ │ │ - ["EliminateDiscrete", "a00182.html#ga6996e9c84467350b071645649231223a", null], │ │ │ │ │ - ["EliminateForMPE", "a00182.html#ga9bd0b2859a7513198a369f5de3c8cde3", null] │ │ │ │ │ + ["gtsam::DiscreteBayesTreeClique", "a02824.html", "a02824"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00185.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteDistribution.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/AlgebraicDecisionTree.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,43 +94,36 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
DiscreteDistribution.h File Reference
│ │ │ │ +
AlgebraicDecisionTree.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::DiscreteDistribution
 A prior probability on a set of discrete variables. More...
 
struct  gtsam::traits< DiscreteDistribution >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
December 2021
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +
Date
Feb 20, 2022
│ │ │ │ +
Author
Mike Sheffler
│ │ │ │ +
│ │ │ │ +Duy-Nguyen Ta
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,27 +1,22 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -DiscreteDistribution.h File Reference │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n │ │ │ │ │ -  A prior probability on a set of discrete variables. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n_ _> │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +AlgebraicDecisionTree.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Date │ │ │ │ │ - December 2021 │ │ │ │ │ + Feb 20, 2022 │ │ │ │ │ Author │ │ │ │ │ + Mike Sheffler │ │ │ │ │ + Duy-Nguyen Ta │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n_._h │ │ │ │ │ + * _A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00188.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteLookupDAG.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,34 +94,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces
│ │ │ │ -
DiscreteLookupDAG.cpp File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
DiscreteFactor.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ + │ │ │ │ +

discrete factor │ │ │ │ +More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

std::vector< double > gtsam::expNormalize (const std::vector< double > &logProbs)
 Normalize a set of log probabilities.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Feb 14, 2011
│ │ │ │ +

discrete factor

│ │ │ │ +
Date
Feb 14, 2011
│ │ │ │
Author
Duy-Nguyen Ta
│ │ │ │
│ │ │ │ Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -DiscreteLookupDAG.cpp File Reference │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +DiscreteFactor.cpp File Reference │ │ │ │ │ +discrete factor _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +std::vector< double >  _g_t_s_a_m_:_:_e_x_p_N_o_r_m_a_l_i_z_e (const std::vector< double > │ │ │ │ │ + &logProbs) │ │ │ │ │ +  Normalize a set of log probabilities. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +discrete factor │ │ │ │ │ Date │ │ │ │ │ Feb 14, 2011 │ │ │ │ │ Author │ │ │ │ │ Duy-Nguyen Ta │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G_._c_p_p │ │ │ │ │ + * _D_i_s_c_r_e_t_e_F_a_c_t_o_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00191.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteBayesNet.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteEliminationTree.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,33 +95,33 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces
│ │ │ │ -
DiscreteBayesNet.cpp File Reference
│ │ │ │ +
DiscreteEliminationTree.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Feb 15, 2011
│ │ │ │ -
Author
Duy-Nguyen Ta
│ │ │ │ +
Date
Mar 29, 2013
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │ -Frank Dellaert
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,21 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -DiscreteBayesNet.cpp File Reference │ │ │ │ │ +DiscreteEliminationTree.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Date │ │ │ │ │ - Feb 15, 2011 │ │ │ │ │ + Mar 29, 2013 │ │ │ │ │ Author │ │ │ │ │ - Duy-Nguyen Ta │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_._c_p_p │ │ │ │ │ + * _D_i_s_c_r_e_t_e_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00194.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteValues.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/Assignment.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,58 +95,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
DiscreteValues.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
Assignment.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ +

An assignment from labels to a discrete value index (size_t) │ │ │ │ +More...

│ │ │ │ + │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::DiscreteValues
 A map from keys to values. More...
 
struct  gtsam::traits< DiscreteValues >
class  gtsam::Assignment< L >
 An assignment from labels to value index (size_t). More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -std::vector< DiscreteValues > gtsam::cartesianProduct (const DiscreteKeys &keys)
 Free version of CartesianProduct.
 
│ │ │ │ -string gtsam::markdown (const DiscreteValues &values, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DiscreteValues::Names &names={})
 Free version of markdown.
 
│ │ │ │ -string gtsam::html (const DiscreteValues &values, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DiscreteValues::Names &names={})
 Free version of html.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Dec 13, 2021
│ │ │ │ +

An assignment from labels to a discrete value index (size_t)

│ │ │ │
Author
Frank Dellaert
│ │ │ │ +
Date
Feb 5, 2012
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,43 +1,27 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -DiscreteValues.h File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +Assignment.h File Reference │ │ │ │ │ +An assignment from labels to a discrete value index (size_t) _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ -  A map from keys to values. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_V_a_l_u_e_s_ _> │ │ │ │ │ +class   _g_t_s_a_m_:_:_A_s_s_i_g_n_m_e_n_t_<_ _L_ _> │ │ │ │ │ +  An assignment from labels to value index (size_t). _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -std::vector< _D_i_s_c_r_e_t_e_V_a_l_u_e_s >  ggttssaamm::::ccaarrtteessiiaannPPrroodduucctt (const _D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ - &keys) │ │ │ │ │ -  Free version of CartesianProduct. │ │ │ │ │ -  │ │ │ │ │ - string  ggttssaamm::::mmaarrkkddoowwnn (const _D_i_s_c_r_e_t_e_V_a_l_u_e_s &values, │ │ │ │ │ - const _K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ - &keyFormatter=DefaultKeyFormatter, const │ │ │ │ │ - _D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_N_a_m_e_s &names={}) │ │ │ │ │ -  Free version of markdown. │ │ │ │ │ -  │ │ │ │ │ - string  ggttssaamm::::hhttmmll (const _D_i_s_c_r_e_t_e_V_a_l_u_e_s &values, const │ │ │ │ │ - _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter=DefaultKeyFormatter, │ │ │ │ │ - const _D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_N_a_m_e_s &names={}) │ │ │ │ │ -  Free version of html. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Dec 13, 2021 │ │ │ │ │ +An assignment from labels to a discrete value index (size_t) │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + Feb 5, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_V_a_l_u_e_s_._h │ │ │ │ │ + * _A_s_s_i_g_n_m_e_n_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00194_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteValues.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/Assignment.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,136 +98,110 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
DiscreteValues.h
│ │ │ │ +
Assignment.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/discrete/Assignment.h>
│ │ │ │ -
21#include <gtsam/discrete/DiscreteKey.h>
│ │ │ │ -
22#include <gtsam/inference/Key.h>
│ │ │ │ -
23
│ │ │ │ -
24#include <map>
│ │ │ │ -
25#include <string>
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <functional>
│ │ │ │ +
22#include <iostream>
│ │ │ │ +
23#include <map>
│ │ │ │ +
24#include <sstream>
│ │ │ │ +
25#include <utility>
│ │ │ │
26#include <vector>
│ │ │ │
27
│ │ │ │
28namespace gtsam {
│ │ │ │
29
│ │ │ │ -
│ │ │ │ -
34class GTSAM_EXPORT DiscreteValues : public Assignment<Key> {
│ │ │ │ -
35 public:
│ │ │ │ -
36 using Base = Assignment<Key>; // base class
│ │ │ │ -
37
│ │ │ │ -
40 using Assignment::Assignment; // all constructors
│ │ │ │ -
41
│ │ │ │ -
42 // Define the implicit default constructor.
│ │ │ │ -
43 DiscreteValues() = default;
│ │ │ │ -
44
│ │ │ │ -
45 // Construct from assignment.
│ │ │ │ -
46 explicit DiscreteValues(const Base& a) : Base(a) {}
│ │ │ │ -
47
│ │ │ │ -
48 // Construct from initializer list.
│ │ │ │ -
49 DiscreteValues(std::initializer_list<std::pair<const Key, size_t>> init)
│ │ │ │ -
50 : Assignment<Key>{init} {}
│ │ │ │ -
51
│ │ │ │ -
55
│ │ │ │ -
57 void print(const std::string& s = "",
│ │ │ │ -
58 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ -
59
│ │ │ │ -
61 bool equals(const DiscreteValues& x, double tol = 1e-9) const;
│ │ │ │ -
62
│ │ │ │ -
66
│ │ │ │ -
67 // insert in base class;
│ │ │ │ -
68 std::pair<iterator, bool> insert( const value_type& value ){
│ │ │ │ -
69 return Base::insert(value);
│ │ │ │ +
36template <class L>
│ │ │ │ +
│ │ │ │ +
37class Assignment : public std::map<L, size_t> {
│ │ │ │ +
45 static std::string DefaultFormatter(const L& x) {
│ │ │ │ +
46 std::stringstream ss;
│ │ │ │ +
47 ss << x;
│ │ │ │ +
48 return ss.str();
│ │ │ │ +
49 }
│ │ │ │ +
50
│ │ │ │ +
51 public:
│ │ │ │ +
52 using std::map<L, size_t>::operator=;
│ │ │ │ +
53
│ │ │ │ +
54 // Define the implicit default constructor.
│ │ │ │ +
55 Assignment() = default;
│ │ │ │ +
56
│ │ │ │ +
57 // Construct from initializer list.
│ │ │ │ +
58 Assignment(std::initializer_list<std::pair<const L, size_t>> init)
│ │ │ │ +
59 : std::map<L, size_t>{init} {}
│ │ │ │ +
60
│ │ │ │ +
61 void print(const std::string& s = "Assignment: ",
│ │ │ │ +
62 const std::function<std::string(L)>& labelFormatter =
│ │ │ │ +
63 &DefaultFormatter) const {
│ │ │ │ +
64 std::cout << s << ": ";
│ │ │ │ +
65 for (const typename Assignment::value_type& keyValue : *this) {
│ │ │ │ +
66 std::cout << "(" << labelFormatter(keyValue.first) << ", "
│ │ │ │ +
67 << keyValue.second << ")";
│ │ │ │ +
68 }
│ │ │ │ +
69 std::cout << std::endl;
│ │ │ │
70 }
│ │ │ │
71
│ │ │ │ -
74 DiscreteValues& insert(const DiscreteValues& values);
│ │ │ │ +
72 bool equals(const Assignment& other, double tol = 1e-9) const {
│ │ │ │ +
73 return (*this == other);
│ │ │ │ +
74 }
│ │ │ │
75
│ │ │ │ -
79 DiscreteValues& update(const DiscreteValues& values);
│ │ │ │ -
80
│ │ │ │ -
│ │ │ │ -
85 static std::vector<DiscreteValues> CartesianProduct(
│ │ │ │ -
86 const DiscreteKeys& keys) {
│ │ │ │ -
87 return Base::CartesianProduct<DiscreteValues>(keys);
│ │ │ │ -
88 }
│ │ │ │ -
│ │ │ │ -
89
│ │ │ │ -
93
│ │ │ │ -
95 using Names = std::map<Key, std::vector<std::string>>;
│ │ │ │ -
96
│ │ │ │ -
98 static std::string Translate(const Names& names, Key key, size_t index);
│ │ │ │ -
99
│ │ │ │ -
107 std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
108 const Names& names = {}) const;
│ │ │ │ -
109
│ │ │ │ -
117 std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
118 const Names& names = {}) const;
│ │ │ │ -
119
│ │ │ │ -
121};
│ │ │ │ -
│ │ │ │ -
122
│ │ │ │ -
│ │ │ │ -
124inline std::vector<DiscreteValues> cartesianProduct(const DiscreteKeys& keys) {
│ │ │ │ -
125 return DiscreteValues::CartesianProduct(keys);
│ │ │ │ -
126}
│ │ │ │ -
│ │ │ │ -
127
│ │ │ │ -
129std::string markdown(const DiscreteValues& values,
│ │ │ │ -
130 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
131 const DiscreteValues::Names& names = {});
│ │ │ │ -
132
│ │ │ │ -
134std::string html(const DiscreteValues& values,
│ │ │ │ -
135 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
136 const DiscreteValues::Names& names = {});
│ │ │ │ -
137
│ │ │ │ -
138// traits
│ │ │ │ -
139template <>
│ │ │ │ -
140struct traits<DiscreteValues> : public Testable<DiscreteValues> {};
│ │ │ │ -
141
│ │ │ │ -
142} // namespace gtsam
│ │ │ │ -
DiscreteKey.h
specialized key for discrete variables
│ │ │ │ -
Assignment.h
An assignment from labels to a discrete value index (size_t)
│ │ │ │ - │ │ │ │ +
88 template <typename Derived = Assignment<L>>
│ │ │ │ +
│ │ │ │ +
89 static std::vector<Derived> CartesianProduct(
│ │ │ │ +
90 const std::vector<std::pair<L, size_t>>& keys) {
│ │ │ │ +
91 std::vector<Derived> allPossValues;
│ │ │ │ +
92 Derived values;
│ │ │ │ +
93 typedef std::pair<L, size_t> DiscreteKey;
│ │ │ │ +
94 for (const DiscreteKey& key : keys)
│ │ │ │ +
95 values[key.first] = 0; // Initialize from 0
│ │ │ │ +
96 while (1) {
│ │ │ │ +
97 allPossValues.push_back(values);
│ │ │ │ +
98 size_t j = 0;
│ │ │ │ +
99 for (j = 0; j < keys.size(); j++) {
│ │ │ │ +
100 L idx = keys[j].first;
│ │ │ │ +
101 values[idx]++;
│ │ │ │ +
102 if (values[idx] < keys[j].second) break;
│ │ │ │ +
103 // Wrap condition
│ │ │ │ +
104 values[idx] = 0;
│ │ │ │ +
105 }
│ │ │ │ +
106 if (j == keys.size()) break;
│ │ │ │ +
107 }
│ │ │ │ +
108 return allPossValues;
│ │ │ │ +
109 }
│ │ │ │ +
│ │ │ │ +
110}; // Assignment
│ │ │ │ +
│ │ │ │ +
111
│ │ │ │ +
112} // namespace gtsam
│ │ │ │ +
gtsam::DiscreteKey
std::pair< Key, size_t > DiscreteKey
Key type for discrete variables.
Definition DiscreteKey.h:36
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::html
string html(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
Free version of html.
Definition DiscreteValues.cpp:134
│ │ │ │ -
gtsam::markdown
string markdown(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
Free version of markdown.
Definition DiscreteValues.cpp:129
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::cartesianProduct
std::vector< DiscreteValues > cartesianProduct(const DiscreteKeys &keys)
Free version of CartesianProduct.
Definition DiscreteValues.h:124
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │
gtsam::Assignment
An assignment from labels to value index (size_t).
Definition Assignment.h:37
│ │ │ │ -
gtsam::DiscreteKeys
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition DiscreteKey.h:39
│ │ │ │ -
gtsam::DiscreteValues
A map from keys to values.
Definition DiscreteValues.h:34
│ │ │ │ -
gtsam::DiscreteValues::Names
std::map< Key, std::vector< std::string > > Names
Translation table from values to strings.
Definition DiscreteValues.h:95
│ │ │ │ -
gtsam::DiscreteValues::CartesianProduct
static std::vector< DiscreteValues > CartesianProduct(const DiscreteKeys &keys)
Return a vector of DiscreteValues, one for each possible combination of values.
Definition DiscreteValues.h:85
│ │ │ │ -
DiscreteValues
The Factor::error simply extracts the.
│ │ │ │ +
gtsam::Assignment::CartesianProduct
static std::vector< Derived > CartesianProduct(const std::vector< std::pair< L, size_t > > &keys)
Get Cartesian product consisting all possible configurations.
Definition Assignment.h:89
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,168 +1,111 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DiscreteValues.h │ │ │ │ │ +Assignment.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_A_s_s_i_g_n_m_e_n_t_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_K_e_y_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ -23 │ │ │ │ │ -24#include │ │ │ │ │ -25#include │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include │ │ │ │ │ +22#include │ │ │ │ │ +23#include │ │ │ │ │ +24#include │ │ │ │ │ +25#include │ │ │ │ │ 26#include │ │ │ │ │ 27 │ │ │ │ │ 28namespace _g_t_s_a_m { │ │ │ │ │ 29 │ │ │ │ │ -_3_4class GTSAM_EXPORT _D_i_s_c_r_e_t_e_V_a_l_u_e_s : public _A_s_s_i_g_n_m_e_n_t { │ │ │ │ │ -35 public: │ │ │ │ │ -36 using _B_a_s_e = _A_s_s_i_g_n_m_e_n_t_<_K_e_y_>; // base class │ │ │ │ │ -37 │ │ │ │ │ -40 using Assignment::Assignment; // all constructors │ │ │ │ │ -41 │ │ │ │ │ -42 // Define the implicit default constructor. │ │ │ │ │ -43 _D_i_s_c_r_e_t_e_V_a_l_u_e_s() = default; │ │ │ │ │ -44 │ │ │ │ │ -45 // Construct from assignment. │ │ │ │ │ -46 explicit _D_i_s_c_r_e_t_e_V_a_l_u_e_s(const _B_a_s_e& a) : _B_a_s_e(a) {} │ │ │ │ │ -47 │ │ │ │ │ -48 // Construct from initializer list. │ │ │ │ │ -49 _D_i_s_c_r_e_t_e_V_a_l_u_e_s(std::initializer_list> init) │ │ │ │ │ -50 : _A_s_s_i_g_n_m_e_n_t_<_K_e_y_>{init} {} │ │ │ │ │ -51 │ │ │ │ │ -55 │ │ │ │ │ -57 void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ -58 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const; │ │ │ │ │ -59 │ │ │ │ │ -61 bool _e_q_u_a_l_s(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& x, double tol = 1e-9) const; │ │ │ │ │ -62 │ │ │ │ │ -66 │ │ │ │ │ -67 // insert in base class; │ │ │ │ │ -68 std::pair insert( const value_type& value ){ │ │ │ │ │ -69 return Base::insert(value); │ │ │ │ │ +36template │ │ │ │ │ +_3_7class _A_s_s_i_g_n_m_e_n_t : public std::map { │ │ │ │ │ +45 static std::string DefaultFormatter(const L& x) { │ │ │ │ │ +46 std::stringstream ss; │ │ │ │ │ +47 ss << x; │ │ │ │ │ +48 return ss.str(); │ │ │ │ │ +49 } │ │ │ │ │ +50 │ │ │ │ │ +51 public: │ │ │ │ │ +52 using std::map::operator=; │ │ │ │ │ +53 │ │ │ │ │ +54 // Define the implicit default constructor. │ │ │ │ │ +55 _A_s_s_i_g_n_m_e_n_t() = default; │ │ │ │ │ +56 │ │ │ │ │ +57 // Construct from initializer list. │ │ │ │ │ +58 _A_s_s_i_g_n_m_e_n_t(std::initializer_list> init) │ │ │ │ │ +59 : std::map{init} {} │ │ │ │ │ +60 │ │ │ │ │ +61 void print(const std::string& s = "Assignment: ", │ │ │ │ │ +62 const std::function& labelFormatter = │ │ │ │ │ +63 &DefaultFormatter) const { │ │ │ │ │ +64 std::cout << s << ": "; │ │ │ │ │ +65 for (const typename Assignment::value_type& keyValue : *this) { │ │ │ │ │ +66 std::cout << "(" << labelFormatter(keyValue.first) << ", " │ │ │ │ │ +67 << keyValue.second << ")"; │ │ │ │ │ +68 } │ │ │ │ │ +69 std::cout << std::endl; │ │ │ │ │ 70 } │ │ │ │ │ 71 │ │ │ │ │ -74 _D_i_s_c_r_e_t_e_V_a_l_u_e_s& insert(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values); │ │ │ │ │ +72 bool _e_q_u_a_l_s(const _A_s_s_i_g_n_m_e_n_t& other, double tol = 1e-9) const { │ │ │ │ │ +73 return (*this == other); │ │ │ │ │ +74 } │ │ │ │ │ 75 │ │ │ │ │ -79 _D_i_s_c_r_e_t_e_V_a_l_u_e_s& update(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values); │ │ │ │ │ -80 │ │ │ │ │ -_8_5 static std::vector _C_a_r_t_e_s_i_a_n_P_r_o_d_u_c_t( │ │ │ │ │ -86 const _D_i_s_c_r_e_t_e_K_e_y_s& keys) { │ │ │ │ │ -87 return Base::CartesianProduct(keys); │ │ │ │ │ -88 } │ │ │ │ │ -89 │ │ │ │ │ -93 │ │ │ │ │ -_9_5 using _N_a_m_e_s = std::map>; │ │ │ │ │ -96 │ │ │ │ │ -98 static std::string Translate(const _N_a_m_e_s& names, _K_e_y key, size_t index); │ │ │ │ │ -99 │ │ │ │ │ -107 std::string _m_a_r_k_d_o_w_n(const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ -DefaultKeyFormatter, │ │ │ │ │ -108 const _N_a_m_e_s& names = {}) const; │ │ │ │ │ -109 │ │ │ │ │ -117 std::string _h_t_m_l(const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ -118 const Names& names = {}) const; │ │ │ │ │ -119 │ │ │ │ │ -121}; │ │ │ │ │ -122 │ │ │ │ │ -_1_2_4inline std::vector _c_a_r_t_e_s_i_a_n_P_r_o_d_u_c_t(const _D_i_s_c_r_e_t_e_K_e_y_s& │ │ │ │ │ -keys) { │ │ │ │ │ -125 return _D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_C_a_r_t_e_s_i_a_n_P_r_o_d_u_c_t(keys); │ │ │ │ │ -126} │ │ │ │ │ -127 │ │ │ │ │ -129std::string _m_a_r_k_d_o_w_n(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values, │ │ │ │ │ -130 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ -131 const _D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_N_a_m_e_s& names = {}); │ │ │ │ │ -132 │ │ │ │ │ -134std::string _h_t_m_l(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values, │ │ │ │ │ -135 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ -136 const _D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_N_a_m_e_s& names = {}); │ │ │ │ │ -137 │ │ │ │ │ -138// traits │ │ │ │ │ -139template <> │ │ │ │ │ -_1_4_0struct _t_r_a_i_t_s<_D_i_s_c_r_e_t_e_V_a_l_u_e_s> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ -141 │ │ │ │ │ -142} // namespace gtsam │ │ │ │ │ -_D_i_s_c_r_e_t_e_K_e_y_._h │ │ │ │ │ -specialized key for discrete variables │ │ │ │ │ -_A_s_s_i_g_n_m_e_n_t_._h │ │ │ │ │ -An assignment from labels to a discrete value index (size_t) │ │ │ │ │ -_K_e_y_._h │ │ │ │ │ +88 template > │ │ │ │ │ +_8_9 static std::vector _C_a_r_t_e_s_i_a_n_P_r_o_d_u_c_t( │ │ │ │ │ +90 const std::vector>& keys) { │ │ │ │ │ +91 std::vector allPossValues; │ │ │ │ │ +92 Derived values; │ │ │ │ │ +93 typedef std::pair _D_i_s_c_r_e_t_e_K_e_y; │ │ │ │ │ +94 for (const _D_i_s_c_r_e_t_e_K_e_y& key : keys) │ │ │ │ │ +95 values[key.first] = 0; // Initialize from 0 │ │ │ │ │ +96 while (1) { │ │ │ │ │ +97 allPossValues.push_back(values); │ │ │ │ │ +98 size_t j = 0; │ │ │ │ │ +99 for (j = 0; j < keys.size(); j++) { │ │ │ │ │ +100 L idx = keys[j].first; │ │ │ │ │ +101 values[idx]++; │ │ │ │ │ +102 if (values[idx] < keys[j].second) break; │ │ │ │ │ +103 // Wrap condition │ │ │ │ │ +104 values[idx] = 0; │ │ │ │ │ +105 } │ │ │ │ │ +106 if (j == keys.size()) break; │ │ │ │ │ +107 } │ │ │ │ │ +108 return allPossValues; │ │ │ │ │ +109 } │ │ │ │ │ +110}; // Assignment │ │ │ │ │ +111 │ │ │ │ │ +112} // namespace gtsam │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y │ │ │ │ │ +std::pair< Key, size_t > DiscreteKey │ │ │ │ │ +Key type for discrete variables. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteKey.h:36 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_h_t_m_l │ │ │ │ │ -string html(const DiscreteValues &values, const KeyFormatter &keyFormatter, │ │ │ │ │ -const DiscreteValues::Names &names) │ │ │ │ │ -Free version of html. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteValues.cpp:134 │ │ │ │ │ -_g_t_s_a_m_:_:_m_a_r_k_d_o_w_n │ │ │ │ │ -string markdown(const DiscreteValues &values, const KeyFormatter &keyFormatter, │ │ │ │ │ -const DiscreteValues::Names &names) │ │ │ │ │ -Free version of markdown. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteValues.cpp:129 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_c_a_r_t_e_s_i_a_n_P_r_o_d_u_c_t │ │ │ │ │ -std::vector< DiscreteValues > cartesianProduct(const DiscreteKeys &keys) │ │ │ │ │ -Free version of CartesianProduct. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteValues.h:124 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ _g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ Template to create a binary predicate. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ _g_t_s_a_m_:_:_A_s_s_i_g_n_m_e_n_t │ │ │ │ │ An assignment from labels to value index (size_t). │ │ │ │ │ DDeeffiinniittiioonn Assignment.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ -DiscreteKeys is a set of keys that can be assembled using the & operator. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteKey.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ -A map from keys to values. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteValues.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_N_a_m_e_s │ │ │ │ │ -std::map< Key, std::vector< std::string > > Names │ │ │ │ │ -Translation table from values to strings. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteValues.h:95 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_C_a_r_t_e_s_i_a_n_P_r_o_d_u_c_t │ │ │ │ │ -static std::vector< DiscreteValues > CartesianProduct(const DiscreteKeys &keys) │ │ │ │ │ -Return a vector of DiscreteValues, one for each possible combination of values. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteValues.h:85 │ │ │ │ │ -_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ -The Factor::error simply extracts the. │ │ │ │ │ +_g_t_s_a_m_:_:_A_s_s_i_g_n_m_e_n_t_:_:_C_a_r_t_e_s_i_a_n_P_r_o_d_u_c_t │ │ │ │ │ +static std::vector< Derived > CartesianProduct(const std::vector< std::pair< L, │ │ │ │ │ +size_t > > &keys) │ │ │ │ │ +Get Cartesian product consisting all possible configurations. │ │ │ │ │ +DDeeffiinniittiioonn Assignment.h:89 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_V_a_l_u_e_s_._h │ │ │ │ │ + * _A_s_s_i_g_n_m_e_n_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00200_source.html │ │ │ │ @@ -25,12888 +25,950 @@ │ │ │ │ 00000180: 656e 743d 2277 6964 7468 3d64 6576 6963 ent="width=devic │ │ │ │ 00000190: 652d 7769 6474 682c 2069 6e69 7469 616c e-width, initial │ │ │ │ 000001a0: 2d73 6361 6c65 3d31 222f 3e0a 3c74 6974 -scale=1"/>.gtsam: /build │ │ │ │ 000001c0: 2f72 6570 726f 6475 6369 626c 652d 7061 /reproducible-pa │ │ │ │ 000001d0: 7468 2f67 7473 616d 2d34 2e32 2e30 2b64 th/gtsam-4.2.0+d │ │ │ │ 000001e0: 6673 672f 6774 7361 6d2f 6469 7363 7265 fsg/gtsam/discre │ │ │ │ -000001f0: 7465 2f44 6563 6973 696f 6e54 7265 652d te/DecisionTree- │ │ │ │ -00000200: 696e 6c2e 6820 536f 7572 6365 2046 696c inl.h Source Fil │ │ │ │ -00000210: 653c 2f74 6974 6c65 3e0a 3c6c 696e 6b20 e... │ │ │ │ -000002c0: 3c2f 7363 7269 7074 3e0a 3c6c 696e 6b20 ...< │ │ │ │ -00000340: 7363 7269 7074 2074 7970 653d 2274 6578 script type="tex │ │ │ │ -00000350: 742f 6a61 7661 7363 7269 7074 2220 7372 t/javascript" sr │ │ │ │ -00000360: 633d 226e 6176 7472 6565 6461 7461 2e6a c="navtreedata.j │ │ │ │ -00000370: 7322 3e3c 2f73 6372 6970 743e 0a3c 7363 s">......MathJax.Hub.C │ │ │ │ -000004b0: 6f6e 6669 6728 7b0a 2020 6578 7465 6e73 onfig({. extens │ │ │ │ -000004c0: 696f 6e73 3a20 5b22 7465 7832 6a61 782e ions: ["tex2jax. │ │ │ │ -000004d0: 6a73 225d 2c0a 2020 6a61 783a 205b 2269 js"],. jax: ["i │ │ │ │ -000004e0: 6e70 7574 2f54 6558 222c 226f 7574 7075 nput/TeX","outpu │ │ │ │ -000004f0: 742f 4854 4d4c 2d43 5353 225d 2c0a 7d29 t/HTML-CSS"],.}) │ │ │ │ -00000500: 3b0a 3c2f 7363 7269 7074 3e0a 3c73 6372 ;.. │ │ │ │ -00000590: 0a3c 6c69 6e6b 2068 7265 663d 2264 6f78 .....
.. . │ │ │ │ -00000680: 0a20 203c 7464 2069 643d 2270 726f 6a65 . . . ....../* @li │ │ │ │ -000007a0: 6365 6e73 6520 6d61 676e 6574 3a3f 7874 cense magnet:?xt │ │ │ │ -000007b0: 3d75 726e 3a62 7469 683a 6433 6439 6139 =urn:btih:d3d9a9 │ │ │ │ -000007c0: 6136 3539 3535 3231 6639 3636 3661 3565 a6595521f9666a5e │ │ │ │ -000007d0: 3934 6363 3833 3064 6162 3833 6236 3536 94cc830dab83b656 │ │ │ │ -000007e0: 3939 2661 6d70 3b64 6e3d 6578 7061 742e 99&dn=expat. │ │ │ │ -000007f0: 7478 7420 4d49 5420 2a2f 0a76 6172 2073 txt MIT */.var s │ │ │ │ -00000800: 6561 7263 6842 6f78 203d 206e 6577 2053 earchBox = new S │ │ │ │ -00000810: 6561 7263 6842 6f78 2822 7365 6172 6368 earchBox("search │ │ │ │ -00000820: 426f 7822 2c20 2273 6561 7263 682f 222c Box", "search/", │ │ │ │ -00000830: 272e 6874 6d6c 2729 3b0a 2f2a 2040 6c69 '.html');./* @li │ │ │ │ -00000840: 6365 6e73 652d 656e 6420 2a2f 0a3c 2f73 cense-end */....< │ │ │ │ -000009e0: 6469 7620 6964 3d22 6d61 696e 2d6e 6176 div id="main-nav │ │ │ │ -000009f0: 223e 3c2f 6469 763e 0a3c 7363 7269 7074 ">...< │ │ │ │ -00000ae0: 6469 7620 6964 3d22 7369 6465 2d6e 6176 div id="side-nav │ │ │ │ -00000af0: 2220 636c 6173 733d 2275 692d 7265 7369 " class="ui-resi │ │ │ │ -00000b00: 7a61 626c 6520 7369 6465 2d6e 6176 2d72 zable side-nav-r │ │ │ │ -00000b10: 6573 697a 6162 6c65 223e 0a20 203c 6469 esizable">. │ │ │ │ -00000b30: 0a20 2020 203c 6469 7620 6964 3d22 6e61 .
.
. │ │ │ │ -00000b80: 2020 203c 2f64 6976 3e0a 2020 3c2f 6469
. .
. .
../* @ │ │ │ │ -00000c20: 6c69 6365 6e73 6520 6d61 676e 6574 3a3f license magnet:? │ │ │ │ -00000c30: 7874 3d75 726e 3a62 7469 683a 6433 6439 xt=urn:btih:d3d9 │ │ │ │ -00000c40: 6139 6136 3539 3535 3231 6639 3636 3661 a9a6595521f9666a │ │ │ │ -00000c50: 3565 3934 6363 3833 3064 6162 3833 6236 5e94cc830dab83b6 │ │ │ │ -00000c60: 3536 3939 2661 6d70 3b64 6e3d 6578 7061 5699&dn=expa │ │ │ │ -00000c70: 742e 7478 7420 4d49 5420 2a2f 0a24 2864 t.txt MIT */.$(d │ │ │ │ -00000c80: 6f63 756d 656e 7429 2e72 6561 6479 2866 ocument).ready(f │ │ │ │ -00000c90: 756e 6374 696f 6e28 297b 696e 6974 4e61 unction(){initNa │ │ │ │ -00000ca0: 7654 7265 6528 2761 3030 3230 305f 736f vTree('a00200_so │ │ │ │ -00000cb0: 7572 6365 2e68 746d 6c27 2c27 2729 3b20 urce.html',''); │ │ │ │ -00000cc0: 696e 6974 5265 7369 7a61 626c 6528 293b initResizable(); │ │ │ │ -00000cd0: 207d 293b 0a2f 2a20 406c 6963 656e 7365 });./* @license │ │ │ │ -00000ce0: 2d65 6e64 202a 2f0a 3c2f 7363 7269 7074 -end */..
..
.< │ │ │ │ -00000e00: 2f64 6976 3e0a 0a3c 212d 2d20 6966 7261 /div>...
.
. │ │ │ │ -00000e80: 3c64 6976 2063 6c61 7373 3d22 5352 5061
.
.
.
Loading... │ │ │ │ -00000ef0: 3c2f 6469 763e 0a3c 6469 7620 636c 6173
.
Sear │ │ │ │ -00000f20: 6368 696e 672e 2e2e 3c2f 6469 763e 0a3c ching...
.< │ │ │ │ -00000f30: 6469 7620 636c 6173 733d 2253 5253 7461 div class="SRSta │ │ │ │ -00000f40: 7475 7322 2069 643d 224e 6f4d 6174 6368 tus" id="NoMatch │ │ │ │ -00000f50: 6573 223e 4e6f 204d 6174 6368 6573 3c2f es">No Matches.
. │ │ │ │ +00000220: 0a3c 6c69 6e6b 2068 7265 663d 2274 6162 ... │ │ │ │ +000002d0: 0a3c 6c69 6e6b 2068 7265 663d 226e 6176 ..... │ │ │ │ +00000400: 3c73 6372 6970 7420 7479 7065 3d22 7465 .< │ │ │ │ +00000890: 2f73 6372 6970 743e 0a3c 7363 7269 7074 /script>.. │ │ │ │ +000008d0: 3c73 6372 6970 7420 7479 7065 3d22 7465 │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,44 +94,61 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces | │ │ │ │ Functions
│ │ │ │ -
DiscreteValues.cpp File Reference
│ │ │ │ +
Signature.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ + │ │ │ │ +

signatures for conditional densities │ │ │ │ +More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │
. gtsam 4.2.0. . │ │ │ │ -000006f0: 203c 6469 7620 6964 3d22 7072 6f6a 6563
gtsam.
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::Signature
 Signature for a discrete conditional density, used to construct conditionals. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

│ │ │ │ -string gtsam::markdown (const DiscreteValues &values, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DiscreteValues::Names &names={})
 Free version of markdown.
 
│ │ │ │ -string gtsam::html (const DiscreteValues &values, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DiscreteValues::Names &names={})
 Free version of html.
 
│ │ │ │ +Signature gtsam::operator| (const DiscreteKey &key, const DiscreteKey &parent)
 Helper function to create Signature objects example: Signature s = D | E;.
 
│ │ │ │ +Signature gtsam::operator% (const DiscreteKey &key, const std::string &parent)
 Helper function to create Signature objects example: Signature s(D % "99/1");.
 
│ │ │ │ +Signature gtsam::operator% (const DiscreteKey &key, const Signature::Table &parent)
 Helper function to create Signature objects, using table construction directly example: Signature s(D % table);.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
January, 2022
│ │ │ │ +

signatures for conditional densities

│ │ │ │
Author
Frank Dellaert
│ │ │ │ +
Date
Feb 27, 2011
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,31 +1,42 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -DiscreteValues.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Signature.h File Reference │ │ │ │ │ +signatures for conditional densities _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_S_i_g_n_a_t_u_r_e │ │ │ │ │ +  _S_i_g_n_a_t_u_r_e for a discrete conditional density, used to construct │ │ │ │ │ + conditionals. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -string  ggttssaamm::::mmaarrkkddoowwnn (const _D_i_s_c_r_e_t_e_V_a_l_u_e_s &values, const _K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ - &keyFormatter=DefaultKeyFormatter, const _D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_N_a_m_e_s &names= │ │ │ │ │ - {}) │ │ │ │ │ -  Free version of markdown. │ │ │ │ │ +_S_i_g_n_a_t_u_r_e  ggttssaamm::::ooppeerraattoorr|| (const _D_i_s_c_r_e_t_e_K_e_y &key, const _D_i_s_c_r_e_t_e_K_e_y &parent) │ │ │ │ │ +  Helper function to create _S_i_g_n_a_t_u_r_e objects example: _S_i_g_n_a_t_u_r_e s = D │ │ │ │ │ + | E;. │ │ │ │ │ +  │ │ │ │ │ +_S_i_g_n_a_t_u_r_e  ggttssaamm::::ooppeerraattoorr%% (const _D_i_s_c_r_e_t_e_K_e_y &key, const std::string &parent) │ │ │ │ │ +  Helper function to create _S_i_g_n_a_t_u_r_e objects example: _S_i_g_n_a_t_u_r_e s(D % │ │ │ │ │ + "99/1");. │ │ │ │ │   │ │ │ │ │ -string  ggttssaamm::::hhttmmll (const _D_i_s_c_r_e_t_e_V_a_l_u_e_s &values, const _K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ - &keyFormatter=DefaultKeyFormatter, const _D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_N_a_m_e_s &names= │ │ │ │ │ - {}) │ │ │ │ │ -  Free version of html. │ │ │ │ │ +_S_i_g_n_a_t_u_r_e  ggttssaamm::::ooppeerraattoorr%% (const _D_i_s_c_r_e_t_e_K_e_y &key, const Signature::Table │ │ │ │ │ + &parent) │ │ │ │ │ +  Helper function to create _S_i_g_n_a_t_u_r_e objects, using table │ │ │ │ │ + construction directly example: _S_i_g_n_a_t_u_r_e s(D % table);. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - January, 2022 │ │ │ │ │ +signatures for conditional densities │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + Feb 27, 2011 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_V_a_l_u_e_s_._c_p_p │ │ │ │ │ + * _S_i_g_n_a_t_u_r_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00206.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,5 @@ │ │ │ │ │ var a00206 = [ │ │ │ │ │ - ["html", "a00206.html#a147114735037c2260d4810a8820e72f6", null], │ │ │ │ │ - ["markdown", "a00206.html#a2b6707b04064df79d26d1016dfa2a013", null] │ │ │ │ │ + ["operator%", "a00206.html#ae4856250df52ce4b9b4b16c7a833408c", null], │ │ │ │ │ + ["operator%", "a00206.html#ab98cf2a4777006c28d6983db5d8ec574", null], │ │ │ │ │ + ["operator|", "a00206.html#aa6c58dfb2fc9398a026e3268ba2edfad", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00209.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteFactor.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteDistribution.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,45 +94,43 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
DiscreteFactor.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
DiscreteDistribution.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

discrete factor │ │ │ │ -More...

│ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::DiscreteDistribution
 A prior probability on a set of discrete variables. More...
 
struct  gtsam::traits< DiscreteDistribution >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

std::vector< double > gtsam::expNormalize (const std::vector< double > &logProbs)
 Normalize a set of log probabilities.
 
│ │ │ │

Detailed Description

│ │ │ │ -

discrete factor

│ │ │ │ -
Date
Feb 14, 2011
│ │ │ │ -
Author
Duy-Nguyen Ta
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ +
Date
December 2021
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,27 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -DiscreteFactor.cpp File Reference │ │ │ │ │ -discrete factor _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +DiscreteDistribution.h File Reference │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n │ │ │ │ │ +  A prior probability on a set of discrete variables. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n_ _> │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -std::vector< double >  _g_t_s_a_m_:_:_e_x_p_N_o_r_m_a_l_i_z_e (const std::vector< double > │ │ │ │ │ - &logProbs) │ │ │ │ │ -  Normalize a set of log probabilities. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -discrete factor │ │ │ │ │ Date │ │ │ │ │ - Feb 14, 2011 │ │ │ │ │ + December 2021 │ │ │ │ │ Author │ │ │ │ │ - Duy-Nguyen Ta │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_F_a_c_t_o_r_._c_p_p │ │ │ │ │ + * _D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00209.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a00209 = [ │ │ │ │ │ - ["expNormalize", "a00209.html#ae1c1177c8725296b34601409322400ea", null] │ │ │ │ │ + ["gtsam::traits< DiscreteDistribution >", "a02844.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00212.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteBayesTree.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DecisionTreeFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,47 +96,43 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
DiscreteBayesTree.h File Reference
│ │ │ │ +
DecisionTreeFactor.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree. │ │ │ │ -More...

│ │ │ │ - │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::DiscreteBayesTreeClique
 A clique in a DiscreteBayesTree. More...
class  gtsam::DecisionTreeFactor
 A discrete probabilistic factor. More...
 
class  gtsam::DiscreteBayesTree
 A Bayes tree representing a Discrete density. More...
struct  gtsam::traits< DecisionTreeFactor >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree.

│ │ │ │ -

DiscreteBayesTree

Author
Frank Dellaert
│ │ │ │ +
Date
Feb 14, 2011
│ │ │ │ +
Author
Duy-Nguyen Ta
│ │ │ │
│ │ │ │ -Richard Roberts
│ │ │ │ +Frank Dellaert
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,30 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -DiscreteBayesTree.h File Reference │ │ │ │ │ -Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree. _M_o_r_e_._._. │ │ │ │ │ +DecisionTreeFactor.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e │ │ │ │ │ -  A clique in a _D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r │ │ │ │ │ +  A discrete probabilistic factor. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -class   _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e │ │ │ │ │ -  A Bayes tree representing a Discrete density. _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree. │ │ │ │ │ -DiscreteBayesTree │ │ │ │ │ + Date │ │ │ │ │ + Feb 14, 2011 │ │ │ │ │ Author │ │ │ │ │ + Duy-Nguyen Ta │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ + * _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00212.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a00212 = [ │ │ │ │ │ - ["gtsam::DiscreteBayesTreeClique", "a02824.html", "a02824"] │ │ │ │ │ + ["gtsam::traits< DecisionTreeFactor >", "a02812.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00212_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteBayesTree.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DecisionTreeFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,134 +98,241 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
DiscreteBayesTree.h
│ │ │ │ +
DecisionTreeFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
21#pragma once
│ │ │ │ -
22
│ │ │ │ -
23#include <gtsam/discrete/DiscreteBayesNet.h>
│ │ │ │ -
24#include <gtsam/discrete/DiscreteFactorGraph.h>
│ │ │ │ -
25#include <gtsam/inference/BayesTree.h>
│ │ │ │ -
26#include <gtsam/inference/Conditional.h>
│ │ │ │ -
27#include <gtsam/inference/BayesTreeCliqueBase.h>
│ │ │ │ -
28
│ │ │ │ -
29#include <string>
│ │ │ │ -
30
│ │ │ │ -
31namespace gtsam {
│ │ │ │ -
32
│ │ │ │ -
33// Forward declarations
│ │ │ │ -
34class DiscreteConditional;
│ │ │ │ -
35class VectorValues;
│ │ │ │ -
36
│ │ │ │ -
37/* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
39class GTSAM_EXPORT DiscreteBayesTreeClique
│ │ │ │ -
40 : public BayesTreeCliqueBase<DiscreteBayesTreeClique, DiscreteFactorGraph> {
│ │ │ │ -
41 public:
│ │ │ │ -
42 typedef DiscreteBayesTreeClique This;
│ │ │ │ -
43 typedef BayesTreeCliqueBase<DiscreteBayesTreeClique, DiscreteFactorGraph>
│ │ │ │ -
44 Base;
│ │ │ │ -
45 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
46 typedef boost::weak_ptr<This> weak_ptr;
│ │ │ │ -
47 DiscreteBayesTreeClique() {}
│ │ │ │ -
48 virtual ~DiscreteBayesTreeClique() {}
│ │ │ │ -
49 DiscreteBayesTreeClique(
│ │ │ │ -
50 const boost::shared_ptr<DiscreteConditional>& conditional)
│ │ │ │ -
51 : Base(conditional) {}
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/discrete/AlgebraicDecisionTree.h>
│ │ │ │ +
22#include <gtsam/discrete/DiscreteFactor.h>
│ │ │ │ +
23#include <gtsam/discrete/DiscreteKey.h>
│ │ │ │ +
24#include <gtsam/inference/Ordering.h>
│ │ │ │ +
25
│ │ │ │ +
26#include <algorithm>
│ │ │ │ +
27#include <boost/shared_ptr.hpp>
│ │ │ │ +
28#include <map>
│ │ │ │ +
29#include <stdexcept>
│ │ │ │ +
30#include <string>
│ │ │ │ +
31#include <utility>
│ │ │ │ +
32#include <vector>
│ │ │ │ +
33
│ │ │ │ +
34namespace gtsam {
│ │ │ │ +
35
│ │ │ │ +
36 class DiscreteConditional;
│ │ │ │ +
37 class HybridValues;
│ │ │ │ +
38
│ │ │ │ +
│ │ │ │ +
44 class GTSAM_EXPORT DecisionTreeFactor : public DiscreteFactor,
│ │ │ │ +
45 public AlgebraicDecisionTree<Key> {
│ │ │ │ +
46 public:
│ │ │ │ +
47 // typedefs needed to play nice with gtsam
│ │ │ │ +
48 typedef DecisionTreeFactor This;
│ │ │ │ +
49 typedef DiscreteFactor Base;
│ │ │ │ +
50 typedef boost::shared_ptr<DecisionTreeFactor> shared_ptr;
│ │ │ │ +
51 typedef AlgebraicDecisionTree<Key> ADT;
│ │ │ │
52
│ │ │ │ -
│ │ │ │ -
54 void printSignature(
│ │ │ │ -
55 const std::string& s = "Clique: ",
│ │ │ │ -
56 const KeyFormatter& formatter = DefaultKeyFormatter) const {
│ │ │ │ -
57 conditional_->printSignature(s, formatter);
│ │ │ │ -
58 }
│ │ │ │ -
│ │ │ │ +
53 protected:
│ │ │ │ +
54 std::map<Key, size_t> cardinalities_;
│ │ │ │ +
55
│ │ │ │ +
56 public:
│ │ │ │
59
│ │ │ │ -
60 //** evaluate conditional probability of subtree for given DiscreteValues */
│ │ │ │ -
61 double evaluate(const DiscreteValues& values) const;
│ │ │ │ +
61 DecisionTreeFactor();
│ │ │ │
62
│ │ │ │ -
63 //** (Preferred) sugar for the above for given DiscreteValues */
│ │ │ │ -
64 double operator()(const DiscreteValues& values) const {
│ │ │ │ -
65 return evaluate(values);
│ │ │ │ -
66 }
│ │ │ │ -
67};
│ │ │ │ -
│ │ │ │ -
68
│ │ │ │ -
69/* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
74class GTSAM_EXPORT DiscreteBayesTree
│ │ │ │ -
75 : public BayesTree<DiscreteBayesTreeClique> {
│ │ │ │ -
76 private:
│ │ │ │ -
77 typedef BayesTree<DiscreteBayesTreeClique> Base;
│ │ │ │ -
78
│ │ │ │ -
79 public:
│ │ │ │ -
80 typedef DiscreteBayesTree This;
│ │ │ │ -
81 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
82
│ │ │ │ -
85
│ │ │ │ -
86 DiscreteBayesTree() {}
│ │ │ │ +
64 DecisionTreeFactor(const DiscreteKeys& keys, const ADT& potentials);
│ │ │ │ +
65
│ │ │ │ +
85 DecisionTreeFactor(const DiscreteKeys& keys,
│ │ │ │ +
86 const std::vector<double>& table);
│ │ │ │
87
│ │ │ │ -
89 bool equals(const This& other, double tol = 1e-9) const;
│ │ │ │ -
90
│ │ │ │ -
91 //** evaluate probability for given DiscreteValues */
│ │ │ │ -
92 double evaluate(const DiscreteValues& values) const;
│ │ │ │ -
93
│ │ │ │ -
94 //** (Preferred) sugar for the above for given DiscreteValues */
│ │ │ │ -
95 double operator()(const DiscreteValues& values) const {
│ │ │ │ -
96 return evaluate(values);
│ │ │ │ -
97 }
│ │ │ │ -
98
│ │ │ │ -
102
│ │ │ │ -
104 std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
105 const DiscreteFactor::Names& names = {}) const;
│ │ │ │ -
106
│ │ │ │ -
108 std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
109 const DiscreteFactor::Names& names = {}) const;
│ │ │ │ -
110
│ │ │ │ -
112};
│ │ │ │ -
│ │ │ │ -
113
│ │ │ │ -
114} // namespace gtsam
│ │ │ │ - │ │ │ │ - │ │ │ │ -
BayesTree.h
Bayes Tree is a tree of cliques of a Bayes Chain.
│ │ │ │ -
Conditional.h
Base class for conditional densities.
│ │ │ │ -
BayesTreeCliqueBase.h
Base class for cliques of a BayesTree.
│ │ │ │ +
106 DecisionTreeFactor(const DiscreteKeys& keys, const std::string& table);
│ │ │ │ +
107
│ │ │ │ +
109 template <class SOURCE>
│ │ │ │ +
│ │ │ │ +
110 DecisionTreeFactor(const DiscreteKey& key, SOURCE table)
│ │ │ │ +
111 : DecisionTreeFactor(DiscreteKeys{key}, table) {}
│ │ │ │ +
│ │ │ │ +
112
│ │ │ │ +
│ │ │ │ +
114 DecisionTreeFactor(const DiscreteKey& key, const std::vector<double>& row)
│ │ │ │ +
115 : DecisionTreeFactor(DiscreteKeys{key}, row) {}
│ │ │ │ +
│ │ │ │ +
116
│ │ │ │ +
118 explicit DecisionTreeFactor(const DiscreteConditional& c);
│ │ │ │ +
119
│ │ │ │ +
123
│ │ │ │ +
125 bool equals(const DiscreteFactor& other, double tol = 1e-9) const override;
│ │ │ │ +
126
│ │ │ │ +
127 // print
│ │ │ │ +
128 void print(
│ │ │ │ +
129 const std::string& s = "DecisionTreeFactor:\n",
│ │ │ │ +
130 const KeyFormatter& formatter = DefaultKeyFormatter) const override;
│ │ │ │ +
131
│ │ │ │ +
135
│ │ │ │ +
│ │ │ │ +
138 double evaluate(const DiscreteValues& values) const {
│ │ │ │ +
139 return ADT::operator()(values);
│ │ │ │ +
140 }
│ │ │ │ +
│ │ │ │ +
141
│ │ │ │ +
│ │ │ │ +
143 double operator()(const DiscreteValues& values) const override {
│ │ │ │ +
144 return ADT::operator()(values);
│ │ │ │ +
145 }
│ │ │ │ +
│ │ │ │ +
146
│ │ │ │ +
148 double error(const DiscreteValues& values) const;
│ │ │ │ +
149
│ │ │ │ +
│ │ │ │ +
151 DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override {
│ │ │ │ +
152 return apply(f, ADT::Ring::mul);
│ │ │ │ +
153 }
│ │ │ │ +
│ │ │ │ +
154
│ │ │ │ +
155 static double safe_div(const double& a, const double& b);
│ │ │ │ +
156
│ │ │ │ +
157 size_t cardinality(Key j) const { return cardinalities_.at(j); }
│ │ │ │ +
158
│ │ │ │ +
│ │ │ │ +
160 DecisionTreeFactor operator/(const DecisionTreeFactor& f) const {
│ │ │ │ +
161 return apply(f, safe_div);
│ │ │ │ +
162 }
│ │ │ │ +
│ │ │ │ +
163
│ │ │ │ +
165 DecisionTreeFactor toDecisionTreeFactor() const override { return *this; }
│ │ │ │ +
166
│ │ │ │ +
│ │ │ │ +
168 shared_ptr sum(size_t nrFrontals) const {
│ │ │ │ +
169 return combine(nrFrontals, ADT::Ring::add);
│ │ │ │ +
170 }
│ │ │ │ +
│ │ │ │ +
171
│ │ │ │ +
│ │ │ │ +
173 shared_ptr sum(const Ordering& keys) const {
│ │ │ │ +
174 return combine(keys, ADT::Ring::add);
│ │ │ │ +
175 }
│ │ │ │ +
│ │ │ │ +
176
│ │ │ │ +
│ │ │ │ +
178 shared_ptr max(size_t nrFrontals) const {
│ │ │ │ +
179 return combine(nrFrontals, ADT::Ring::max);
│ │ │ │ +
180 }
│ │ │ │ +
│ │ │ │ +
181
│ │ │ │ +
│ │ │ │ +
183 shared_ptr max(const Ordering& keys) const {
│ │ │ │ +
184 return combine(keys, ADT::Ring::max);
│ │ │ │ +
185 }
│ │ │ │ +
│ │ │ │ +
186
│ │ │ │ +
190
│ │ │ │ +
196 DecisionTreeFactor apply(const DecisionTreeFactor& f, ADT::Binary op) const;
│ │ │ │ +
197
│ │ │ │ +
204 shared_ptr combine(size_t nrFrontals, ADT::Binary op) const;
│ │ │ │ +
205
│ │ │ │ +
212 shared_ptr combine(const Ordering& keys, ADT::Binary op) const;
│ │ │ │ +
213
│ │ │ │ +
215 std::vector<std::pair<DiscreteValues, double>> enumerate() const;
│ │ │ │ +
216
│ │ │ │ +
218 DiscreteKeys discreteKeys() const;
│ │ │ │ +
219
│ │ │ │ +
238 DecisionTreeFactor prune(size_t maxNrAssignments) const;
│ │ │ │ +
239
│ │ │ │ +
243
│ │ │ │ +
245 void dot(std::ostream& os,
│ │ │ │ +
246 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
247 bool showZero = true) const;
│ │ │ │ +
248
│ │ │ │ +
250 void dot(const std::string& name,
│ │ │ │ +
251 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
252 bool showZero = true) const;
│ │ │ │ +
253
│ │ │ │ +
255 std::string dot(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
256 bool showZero = true) const;
│ │ │ │ +
257
│ │ │ │ +
265 std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
266 const Names& names = {}) const override;
│ │ │ │ +
267
│ │ │ │ +
275 std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
276 const Names& names = {}) const override;
│ │ │ │ +
277
│ │ │ │ +
281
│ │ │ │ +
286 double error(const HybridValues& values) const override;
│ │ │ │ +
287
│ │ │ │ +
289
│ │ │ │ +
290 private:
│ │ │ │ +
292 friend class boost::serialization::access;
│ │ │ │ +
293 template <class ARCHIVE>
│ │ │ │ +
294 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ +
295 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
296 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(ADT);
│ │ │ │ +
297 ar& BOOST_SERIALIZATION_NVP(cardinalities_);
│ │ │ │ +
298 }
│ │ │ │ +
299 };
│ │ │ │ +
│ │ │ │ +
300
│ │ │ │ +
301// traits
│ │ │ │ +
302template <>
│ │ │ │ +
303struct traits<DecisionTreeFactor> : public Testable<DecisionTreeFactor> {};
│ │ │ │ +
304
│ │ │ │ +
305} // namespace gtsam
│ │ │ │ + │ │ │ │ +
AlgebraicDecisionTree.h
Algebraic Decision Trees.
│ │ │ │ +
DiscreteKey.h
specialized key for discrete variables
│ │ │ │ +
Ordering.h
Variable ordering for the elimination algorithm.
│ │ │ │ +
gtsam::DiscreteKey
std::pair< Key, size_t > DiscreteKey
Key type for discrete variables.
Definition DiscreteKey.h:36
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::html
string html(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
Free version of html.
Definition DiscreteValues.cpp:134
│ │ │ │ +
gtsam::row
const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j)
Extracts a row view from a matrix that avoids a copy.
Definition Matrix.h:222
│ │ │ │ +
gtsam::markdown
string markdown(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
Free version of markdown.
Definition DiscreteValues.cpp:129
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::apply
DecisionTree< L, Y > apply(const DecisionTree< L, Y > &f, const typename DecisionTree< L, Y >::Unary &op)
free versions of apply
Definition DecisionTree.h:413
│ │ │ │ +
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::DiscreteBayesTreeClique
A clique in a DiscreteBayesTree.
Definition DiscreteBayesTree.h:40
│ │ │ │ -
gtsam::DiscreteBayesTreeClique::printSignature
void printSignature(const std::string &s="Clique: ", const KeyFormatter &formatter=DefaultKeyFormatter) const
print index signature only
Definition DiscreteBayesTree.h:54
│ │ │ │ -
gtsam::DiscreteBayesTree
A Bayes tree representing a Discrete density.
Definition DiscreteBayesTree.h:75
│ │ │ │ -
gtsam::DiscreteBayesTree::DiscreteBayesTree
DiscreteBayesTree()
Default constructor, creates an empty Bayes tree.
Definition DiscreteBayesTree.h:86
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::AlgebraicDecisionTree
An algebraic decision tree fixes the range of a DecisionTree to double.
Definition AlgebraicDecisionTree.h:38
│ │ │ │ +
gtsam::DecisionTreeFactor
A discrete probabilistic factor.
Definition DecisionTreeFactor.h:45
│ │ │ │ +
gtsam::DecisionTreeFactor::sum
shared_ptr sum(const Ordering &keys) const
Create new factor by summing all values with the same separator values.
Definition DecisionTreeFactor.h:173
│ │ │ │ +
gtsam::DecisionTreeFactor::operator()
double operator()(const DiscreteValues &values) const override
Evaluate probability density, sugar.
Definition DecisionTreeFactor.h:143
│ │ │ │ +
gtsam::DecisionTreeFactor::operator*
DecisionTreeFactor operator*(const DecisionTreeFactor &f) const override
multiply two factors
Definition DecisionTreeFactor.h:151
│ │ │ │ +
gtsam::DecisionTreeFactor::max
shared_ptr max(size_t nrFrontals) const
Create new factor by maximizing over all values with the same separator.
Definition DecisionTreeFactor.h:178
│ │ │ │ +
gtsam::DecisionTreeFactor::max
shared_ptr max(const Ordering &keys) const
Create new factor by maximizing over all values with the same separator.
Definition DecisionTreeFactor.h:183
│ │ │ │ +
gtsam::DecisionTreeFactor::evaluate
double evaluate(const DiscreteValues &values) const
Calculate probability for given values x, is just look up in AlgebraicDecisionTree.
Definition DecisionTreeFactor.h:138
│ │ │ │ +
gtsam::DecisionTreeFactor::Base
DiscreteFactor Base
Typedef to base class.
Definition DecisionTreeFactor.h:49
│ │ │ │ +
gtsam::DecisionTreeFactor::sum
shared_ptr sum(size_t nrFrontals) const
Create new factor by summing all values with the same separator values.
Definition DecisionTreeFactor.h:168
│ │ │ │ +
gtsam::DecisionTreeFactor::DecisionTreeFactor
DecisionTreeFactor(const DiscreteKey &key, SOURCE table)
Single-key specialization.
Definition DecisionTreeFactor.h:110
│ │ │ │ +
gtsam::DecisionTreeFactor::toDecisionTreeFactor
DecisionTreeFactor toDecisionTreeFactor() const override
Convert into a decisiontree.
Definition DecisionTreeFactor.h:165
│ │ │ │ +
gtsam::DecisionTreeFactor::operator/
DecisionTreeFactor operator/(const DecisionTreeFactor &f) const
divide by factor f (safely)
Definition DecisionTreeFactor.h:160
│ │ │ │ +
gtsam::DecisionTreeFactor::DecisionTreeFactor
DecisionTreeFactor(const DiscreteKey &key, const std::vector< double > &row)
Single-key specialization, with vector of doubles.
Definition DecisionTreeFactor.h:114
│ │ │ │ +
gtsam::DiscreteConditional
Discrete Conditional Density Derives from DecisionTreeFactor.
Definition DiscreteConditional.h:40
│ │ │ │ +
gtsam::DiscreteFactor
Base class for discrete probabilistic factors The most general one is the derived DecisionTreeFactor.
Definition DiscreteFactor.h:38
│ │ │ │ +
gtsam::DiscreteKeys
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition DiscreteKey.h:39
│ │ │ │
gtsam::DiscreteValues
A map from keys to values.
Definition DiscreteValues.h:34
│ │ │ │ -
gtsam::BayesTree
Bayes tree.
Definition BayesTree.h:67
│ │ │ │ -
gtsam::BayesTreeCliqueBase
This is the base class for BayesTree cliques.
Definition BayesTreeCliqueBase.h:50
│ │ │ │ -
VectorValues
The Factor::error simply extracts the.
│ │ │ │ +
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
HybridValues
the error.
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,155 +1,303 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DiscreteBayesTree.h │ │ │ │ │ +DecisionTreeFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -21#pragma once │ │ │ │ │ -22 │ │ │ │ │ -23#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ -27#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._h> │ │ │ │ │ -28 │ │ │ │ │ -29#include │ │ │ │ │ -30 │ │ │ │ │ -31namespace _g_t_s_a_m { │ │ │ │ │ -32 │ │ │ │ │ -33// Forward declarations │ │ │ │ │ -34class DiscreteConditional; │ │ │ │ │ -35class _V_e_c_t_o_r_V_a_l_u_e_s; │ │ │ │ │ -36 │ │ │ │ │ -37/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -_3_9class GTSAM_EXPORT _D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e │ │ │ │ │ -40 : public _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e { │ │ │ │ │ -41 public: │ │ │ │ │ -42 typedef _D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e _T_h_i_s; │ │ │ │ │ -43 typedef _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_,_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_> │ │ │ │ │ -44 _B_a_s_e; │ │ │ │ │ -45 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -46 typedef boost::weak_ptr weak_ptr; │ │ │ │ │ -47 _D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e() {} │ │ │ │ │ -48 virtual _~_D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e() {} │ │ │ │ │ -49 _D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e( │ │ │ │ │ -50 const boost::shared_ptr& conditional) │ │ │ │ │ -51 : _B_a_s_e(conditional) {} │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_F_a_c_t_o_r_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_K_e_y_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_O_r_d_e_r_i_n_g_._h> │ │ │ │ │ +25 │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28#include │ │ │ │ │ +29#include │ │ │ │ │ +30#include │ │ │ │ │ +31#include │ │ │ │ │ +32#include │ │ │ │ │ +33 │ │ │ │ │ +34namespace _g_t_s_a_m { │ │ │ │ │ +35 │ │ │ │ │ +36 class DiscreteConditional; │ │ │ │ │ +37 class _H_y_b_r_i_d_V_a_l_u_e_s; │ │ │ │ │ +38 │ │ │ │ │ +_4_4 class GTSAM_EXPORT _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r : public _D_i_s_c_r_e_t_e_F_a_c_t_o_r, │ │ │ │ │ +45 public _A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e { │ │ │ │ │ +46 public: │ │ │ │ │ +47 // typedefs needed to play nice with gtsam │ │ │ │ │ +48 typedef _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ +_4_9 typedef _D_i_s_c_r_e_t_e_F_a_c_t_o_r _B_a_s_e; │ │ │ │ │ +50 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +51 typedef _A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_<_K_e_y_> _A_D_T; │ │ │ │ │ 52 │ │ │ │ │ -_5_4 void _p_r_i_n_t_S_i_g_n_a_t_u_r_e( │ │ │ │ │ -55 const std::string& s = "Clique: ", │ │ │ │ │ -56 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const { │ │ │ │ │ -57 conditional_->printSignature(s, formatter); │ │ │ │ │ -58 } │ │ │ │ │ +53 protected: │ │ │ │ │ +54 std::map cardinalities_; │ │ │ │ │ +55 │ │ │ │ │ +56 public: │ │ │ │ │ 59 │ │ │ │ │ -60 //** evaluate conditional probability of subtree for given DiscreteValues */ │ │ │ │ │ -61 double evaluate(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values) const; │ │ │ │ │ +61 _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r(); │ │ │ │ │ 62 │ │ │ │ │ -63 //** (Preferred) sugar for the above for given DiscreteValues */ │ │ │ │ │ -64 double operator()(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values) const { │ │ │ │ │ -65 return evaluate(values); │ │ │ │ │ -66 } │ │ │ │ │ -67}; │ │ │ │ │ -68 │ │ │ │ │ -69/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -_7_4class GTSAM_EXPORT _D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e │ │ │ │ │ -75 : public _B_a_y_e_s_T_r_e_e { │ │ │ │ │ -76 private: │ │ │ │ │ -77 typedef _B_a_y_e_s_T_r_e_e_<_D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_> _B_a_s_e; │ │ │ │ │ -78 │ │ │ │ │ -79 public: │ │ │ │ │ -80 typedef _D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e _T_h_i_s; │ │ │ │ │ -81 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -82 │ │ │ │ │ -85 │ │ │ │ │ -_8_6 _D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e() {} │ │ │ │ │ +64 _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r(const _D_i_s_c_r_e_t_e_K_e_y_s& keys, const _A_D_T& potentials); │ │ │ │ │ +65 │ │ │ │ │ +85 _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r(const _D_i_s_c_r_e_t_e_K_e_y_s& keys, │ │ │ │ │ +86 const std::vector& table); │ │ │ │ │ 87 │ │ │ │ │ -89 bool _e_q_u_a_l_s(const This& other, double tol = 1e-9) const; │ │ │ │ │ -90 │ │ │ │ │ -91 //** evaluate probability for given DiscreteValues */ │ │ │ │ │ -92 double evaluate(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values) const; │ │ │ │ │ -93 │ │ │ │ │ -94 //** (Preferred) sugar for the above for given DiscreteValues */ │ │ │ │ │ -95 double operator()(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values) const { │ │ │ │ │ -96 return evaluate(values); │ │ │ │ │ -97 } │ │ │ │ │ -98 │ │ │ │ │ -102 │ │ │ │ │ -104 std::string markdown(const KeyFormatter& keyFormatter = │ │ │ │ │ +106 _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r(const _D_i_s_c_r_e_t_e_K_e_y_s& keys, const std::string& table); │ │ │ │ │ +107 │ │ │ │ │ +109 template │ │ │ │ │ +_1_1_0 _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r(const _D_i_s_c_r_e_t_e_K_e_y& key, SOURCE table) │ │ │ │ │ +111 : _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r(_D_i_s_c_r_e_t_e_K_e_y_s{key}, table) {} │ │ │ │ │ +112 │ │ │ │ │ +_1_1_4 _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r(const _D_i_s_c_r_e_t_e_K_e_y& key, const std::vector& row) │ │ │ │ │ +115 : _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r(_D_i_s_c_r_e_t_e_K_e_y_s{key}, _r_o_w) {} │ │ │ │ │ +116 │ │ │ │ │ +118 explicit _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r(const _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l& c); │ │ │ │ │ +119 │ │ │ │ │ +123 │ │ │ │ │ +125 bool _e_q_u_a_l_s(const _D_i_s_c_r_e_t_e_F_a_c_t_o_r& other, double tol = 1e-9) const override; │ │ │ │ │ +126 │ │ │ │ │ +127 // print │ │ │ │ │ +128 void _p_r_i_n_t( │ │ │ │ │ +129 const std::string& s = "DecisionTreeFactor:\n", │ │ │ │ │ +130 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override; │ │ │ │ │ +131 │ │ │ │ │ +135 │ │ │ │ │ +_1_3_8 double _e_v_a_l_u_a_t_e(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values) const { │ │ │ │ │ +139 return ADT::operator()(values); │ │ │ │ │ +140 } │ │ │ │ │ +141 │ │ │ │ │ +_1_4_3 double _o_p_e_r_a_t_o_r_(_)(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values) const override { │ │ │ │ │ +144 return ADT::operator()(values); │ │ │ │ │ +145 } │ │ │ │ │ +146 │ │ │ │ │ +148 double error(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values) const; │ │ │ │ │ +149 │ │ │ │ │ +_1_5_1 _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r _o_p_e_r_a_t_o_r_*(const _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r& f) const override { │ │ │ │ │ +152 return _a_p_p_l_y(f, ADT::Ring::mul); │ │ │ │ │ +153 } │ │ │ │ │ +154 │ │ │ │ │ +155 static double safe_div(const double& a, const double& b); │ │ │ │ │ +156 │ │ │ │ │ +157 size_t cardinality(_K_e_y j) const { return cardinalities_.at(j); } │ │ │ │ │ +158 │ │ │ │ │ +_1_6_0 _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r _o_p_e_r_a_t_o_r_/(const _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r& f) const { │ │ │ │ │ +161 return _a_p_p_l_y(f, safe_div); │ │ │ │ │ +162 } │ │ │ │ │ +163 │ │ │ │ │ +_1_6_5 _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r _t_o_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r() const override { return *this; } │ │ │ │ │ +166 │ │ │ │ │ +_1_6_8 shared_ptr _s_u_m(size_t nrFrontals) const { │ │ │ │ │ +169 return combine(nrFrontals, ADT::Ring::add); │ │ │ │ │ +170 } │ │ │ │ │ +171 │ │ │ │ │ +_1_7_3 shared_ptr _s_u_m(const _O_r_d_e_r_i_n_g& keys) const { │ │ │ │ │ +174 return combine(keys, ADT::Ring::add); │ │ │ │ │ +175 } │ │ │ │ │ +176 │ │ │ │ │ +_1_7_8 shared_ptr _m_a_x(size_t nrFrontals) const { │ │ │ │ │ +179 return combine(nrFrontals, ADT::Ring::max); │ │ │ │ │ +180 } │ │ │ │ │ +181 │ │ │ │ │ +_1_8_3 shared_ptr _m_a_x(const _O_r_d_e_r_i_n_g& keys) const { │ │ │ │ │ +184 return combine(keys, ADT::Ring::max); │ │ │ │ │ +185 } │ │ │ │ │ +186 │ │ │ │ │ +190 │ │ │ │ │ +196 _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r _a_p_p_l_y(const _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r& f, ADT::Binary op) │ │ │ │ │ +const; │ │ │ │ │ +197 │ │ │ │ │ +204 shared_ptr combine(size_t nrFrontals, ADT::Binary op) const; │ │ │ │ │ +205 │ │ │ │ │ +212 shared_ptr combine(const _O_r_d_e_r_i_n_g& keys, ADT::Binary op) const; │ │ │ │ │ +213 │ │ │ │ │ +215 std::vector> enumerate() const; │ │ │ │ │ +216 │ │ │ │ │ +218 _D_i_s_c_r_e_t_e_K_e_y_s discreteKeys() const; │ │ │ │ │ +219 │ │ │ │ │ +238 _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r prune(size_t maxNrAssignments) const; │ │ │ │ │ +239 │ │ │ │ │ +243 │ │ │ │ │ +245 void _d_o_t(std::ostream& os, │ │ │ │ │ +246 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ +247 bool showZero = true) const; │ │ │ │ │ +248 │ │ │ │ │ +250 void _d_o_t(const std::string& name, │ │ │ │ │ +251 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ +252 bool showZero = true) const; │ │ │ │ │ +253 │ │ │ │ │ +255 std::string _d_o_t(const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ +256 bool showZero = true) const; │ │ │ │ │ +257 │ │ │ │ │ +265 std::string _m_a_r_k_d_o_w_n(const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ DefaultKeyFormatter, │ │ │ │ │ -105 const DiscreteFactor::Names& names = {}) const; │ │ │ │ │ -106 │ │ │ │ │ -108 std::string _h_t_m_l(const KeyFormatter& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ -109 const DiscreteFactor::Names& names = {}) const; │ │ │ │ │ -110 │ │ │ │ │ -112}; │ │ │ │ │ -113 │ │ │ │ │ -114} // namespace gtsam │ │ │ │ │ -_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_._h │ │ │ │ │ -_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ -Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ │ -_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ -Base class for conditional densities. │ │ │ │ │ -_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._h │ │ │ │ │ -Base class for cliques of a BayesTree. │ │ │ │ │ +266 const Names& names = {}) const override; │ │ │ │ │ +267 │ │ │ │ │ +275 std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ +276 const Names& names = {}) const override; │ │ │ │ │ +277 │ │ │ │ │ +281 │ │ │ │ │ +286 double error(const _H_y_b_r_i_d_V_a_l_u_e_s& values) const override; │ │ │ │ │ +287 │ │ │ │ │ +289 │ │ │ │ │ +290 private: │ │ │ │ │ +_2_9_2 friend class boost::serialization::access; │ │ │ │ │ +293 template │ │ │ │ │ +294 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ +295 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +296 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(_A_D_T); │ │ │ │ │ +297 ar& BOOST_SERIALIZATION_NVP(cardinalities_); │ │ │ │ │ +298 } │ │ │ │ │ +299 }; │ │ │ │ │ +300 │ │ │ │ │ +301// traits │ │ │ │ │ +302template <> │ │ │ │ │ +_3_0_3struct _t_r_a_i_t_s<_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ +304 │ │ │ │ │ +305} // namespace gtsam │ │ │ │ │ +_D_i_s_c_r_e_t_e_F_a_c_t_o_r_._h │ │ │ │ │ +_A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_._h │ │ │ │ │ +Algebraic Decision Trees. │ │ │ │ │ +_D_i_s_c_r_e_t_e_K_e_y_._h │ │ │ │ │ +specialized key for discrete variables │ │ │ │ │ +_O_r_d_e_r_i_n_g_._h │ │ │ │ │ +Variable ordering for the elimination algorithm. │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y │ │ │ │ │ +std::pair< Key, size_t > DiscreteKey │ │ │ │ │ +Key type for discrete variables. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteKey.h:36 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_h_t_m_l │ │ │ │ │ -string html(const DiscreteValues &values, const KeyFormatter &keyFormatter, │ │ │ │ │ +_g_t_s_a_m_:_:_r_o_w │ │ │ │ │ +const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j) │ │ │ │ │ +Extracts a row view from a matrix that avoids a copy. │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:222 │ │ │ │ │ +_g_t_s_a_m_:_:_m_a_r_k_d_o_w_n │ │ │ │ │ +string markdown(const DiscreteValues &values, const KeyFormatter &keyFormatter, │ │ │ │ │ const DiscreteValues::Names &names) │ │ │ │ │ -Free version of html. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteValues.cpp:134 │ │ │ │ │ +Free version of markdown. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteValues.cpp:129 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_a_p_p_l_y │ │ │ │ │ +DecisionTree< L, Y > apply(const DecisionTree< L, Y > &f, const typename │ │ │ │ │ +DecisionTree< L, Y >::Unary &op) │ │ │ │ │ +free versions of apply │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree.h:413 │ │ │ │ │ +_g_t_s_a_m_:_:_d_o_t │ │ │ │ │ +double dot(const V1 &a, const V2 &b) │ │ │ │ │ +Dot product. │ │ │ │ │ +DDeeffiinniittiioonn Vector.h:195 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ _g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ Template to create a binary predicate. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e │ │ │ │ │ -A clique in a DiscreteBayesTree. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteBayesTree.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_:_:_p_r_i_n_t_S_i_g_n_a_t_u_r_e │ │ │ │ │ -void printSignature(const std::string &s="Clique: ", const KeyFormatter │ │ │ │ │ -&formatter=DefaultKeyFormatter) const │ │ │ │ │ -print index signature only │ │ │ │ │ -DDeeffiinniittiioonn DiscreteBayesTree.h:54 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e │ │ │ │ │ -A Bayes tree representing a Discrete density. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteBayesTree.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e │ │ │ │ │ -DiscreteBayesTree() │ │ │ │ │ -Default constructor, creates an empty Bayes tree. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteBayesTree.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e │ │ │ │ │ +An algebraic decision tree fixes the range of a DecisionTree to double. │ │ │ │ │ +DDeeffiinniittiioonn AlgebraicDecisionTree.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r │ │ │ │ │ +A discrete probabilistic factor. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTreeFactor.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_:_:_s_u_m │ │ │ │ │ +shared_ptr sum(const Ordering &keys) const │ │ │ │ │ +Create new factor by summing all values with the same separator values. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTreeFactor.h:173 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ +double operator()(const DiscreteValues &values) const override │ │ │ │ │ +Evaluate probability density, sugar. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTreeFactor.h:143 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ +DecisionTreeFactor operator*(const DecisionTreeFactor &f) const override │ │ │ │ │ +multiply two factors │ │ │ │ │ +DDeeffiinniittiioonn DecisionTreeFactor.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_:_:_m_a_x │ │ │ │ │ +shared_ptr max(size_t nrFrontals) const │ │ │ │ │ +Create new factor by maximizing over all values with the same separator. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTreeFactor.h:178 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_:_:_m_a_x │ │ │ │ │ +shared_ptr max(const Ordering &keys) const │ │ │ │ │ +Create new factor by maximizing over all values with the same separator. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTreeFactor.h:183 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e │ │ │ │ │ +double evaluate(const DiscreteValues &values) const │ │ │ │ │ +Calculate probability for given values x, is just look up in │ │ │ │ │ +AlgebraicDecisionTree. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTreeFactor.h:138 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_:_:_B_a_s_e │ │ │ │ │ +DiscreteFactor Base │ │ │ │ │ +Typedef to base class. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTreeFactor.h:49 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_:_:_s_u_m │ │ │ │ │ +shared_ptr sum(size_t nrFrontals) const │ │ │ │ │ +Create new factor by summing all values with the same separator values. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTreeFactor.h:168 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r │ │ │ │ │ +DecisionTreeFactor(const DiscreteKey &key, SOURCE table) │ │ │ │ │ +Single-key specialization. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTreeFactor.h:110 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_:_:_t_o_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r │ │ │ │ │ +DecisionTreeFactor toDecisionTreeFactor() const override │ │ │ │ │ +Convert into a decisiontree. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTreeFactor.h:165 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_:_:_o_p_e_r_a_t_o_r_/ │ │ │ │ │ +DecisionTreeFactor operator/(const DecisionTreeFactor &f) const │ │ │ │ │ +divide by factor f (safely) │ │ │ │ │ +DDeeffiinniittiioonn DecisionTreeFactor.h:160 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r │ │ │ │ │ +DecisionTreeFactor(const DiscreteKey &key, const std::vector< double > &row) │ │ │ │ │ +Single-key specialization, with vector of doubles. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTreeFactor.h:114 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +Discrete Conditional Density Derives from DecisionTreeFactor. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteConditional.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r │ │ │ │ │ +Base class for discrete probabilistic factors The most general one is the │ │ │ │ │ +derived DecisionTreeFactor. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactor.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ +DiscreteKeys is a set of keys that can be assembled using the & operator. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteKey.h:39 │ │ │ │ │ _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ A map from keys to values. │ │ │ │ │ DDeeffiinniittiioonn DiscreteValues.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e │ │ │ │ │ -Bayes tree. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e │ │ │ │ │ -This is the base class for BayesTree cliques. │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase.h:50 │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -The Factor::error simply extracts the. │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +the error. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ + * _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00215.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteConditional.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteLookupDAG.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,30 +94,19 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
DiscreteConditional.h File Reference
│ │ │ │ +
DiscreteLookupDAG.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::DiscreteConditional
 Discrete Conditional Density Derives from DecisionTreeFactor. More...
 
struct  gtsam::traits< DiscreteConditional >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ @@ -126,13 +115,13 @@ │ │ │ │
│ │ │ │ Frank Dellaert
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,21 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -DiscreteConditional.h File Reference │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -  Discrete _C_o_n_d_i_t_i_o_n_a_l Density Derives from _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_ _> │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +DiscreteLookupDAG.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Date │ │ │ │ │ Feb 14, 2011 │ │ │ │ │ Author │ │ │ │ │ Duy-Nguyen Ta │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ + * _D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00218.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteEliminationTree.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteJunctionTree.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,15 +95,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces
│ │ │ │ -
DiscreteEliminationTree.cpp File Reference
│ │ │ │ +
DiscreteJunctionTree.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -115,13 +115,13 @@ │ │ │ │
│ │ │ │ Richard Roberts
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,21 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -DiscreteEliminationTree.cpp File Reference │ │ │ │ │ +DiscreteJunctionTree.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Date │ │ │ │ │ Mar 29, 2013 │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._c_p_p │ │ │ │ │ + * _D_i_s_c_r_e_t_e_J_u_n_c_t_i_o_n_T_r_e_e_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00221.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DecisionTree.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -97,71 +97,49 @@ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ Functions
│ │ │ │ -
DecisionTree.h File Reference
│ │ │ │ +
DiscreteFactor.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Decision Tree for use in DiscreteFactors. │ │ │ │ -More...

│ │ │ │ - │ │ │ │

Go to the source code of this file.

│ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::DecisionTree< L, Y >
 a decision tree is a function from assignments to values. More...
 
struct  gtsam::DecisionTree< L, Y >::Node
 ---------------------— Node base class ------------------------— More...
class  gtsam::DiscreteFactor
 Base class for discrete probabilistic factors The most general one is the derived DecisionTreeFactor. More...
 
struct  gtsam::traits< DecisionTree< L, Y > >
struct  gtsam::traits< DiscreteFactor >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

template<typename L , typename Y >
DecisionTree< L, Y > gtsam::apply (const DecisionTree< L, Y > &f, const typename DecisionTree< L, Y >::Unary &op)
 free versions of apply
 
│ │ │ │ -template<typename L , typename Y >
DecisionTree< L, Y > gtsam::apply (const DecisionTree< L, Y > &f, const typename DecisionTree< L, Y >::UnaryAssignment &op)
 Apply unary operator op with Assignment to DecisionTree f.
 
│ │ │ │ -template<typename L , typename Y >
DecisionTree< L, Y > gtsam::apply (const DecisionTree< L, Y > &f, const DecisionTree< L, Y > &g, const typename DecisionTree< L, Y >::Binary &op)
 Apply binary operator op to DecisionTree f.
 
template<typename L , typename T1 , typename T2 >
std::pair< DecisionTree< L, T1 >, DecisionTree< L, T2 > > gtsam::unzip (const DecisionTree< L, std::pair< T1, T2 > > &input)
 unzip a DecisionTree with std::pair values.
 
std::vector< double > gtsam::expNormalize (const std::vector< double > &logProbs)
 Normalize a set of log probabilities.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Decision Tree for use in DiscreteFactors.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +
Date
Feb 14, 2011
│ │ │ │ +
Author
Duy-Nguyen Ta
│ │ │ │
│ │ │ │ -Can Erdogan
│ │ │ │ -
Date
Jan 30, 2012
│ │ │ │ +Frank Dellaert
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── encoding │ │ │ │ │ @@ -1 +1 @@ │ │ │ │ │ -utf-8 │ │ │ │ │ +us-ascii │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,59 +1,34 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -DecisionTree.h File Reference │ │ │ │ │ -Decision Tree for use in DiscreteFactors. _M_o_r_e_._._. │ │ │ │ │ +DiscreteFactor.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_<_ _L_,_ _Y_ _> │ │ │ │ │ -  a decision tree is a function from assignments to values. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r │ │ │ │ │ +  Base class for discrete probabilistic factors The most general one is │ │ │ │ │ + the derived _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_<_ _L_,_ _Y_ _>_:_:_N_o_d_e │ │ │ │ │ -  ---------------------— _N_o_d_e base class ------------------------ │ │ │ │ │ - — _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _D_e_c_i_s_i_o_n_T_r_e_e_<_ _L_,_ _Y_ _>_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ - _D_e_c_i_s_i_o_n_T_r_e_e< L, Y >  _g_t_s_a_m_:_:_a_p_p_l_y (const _D_e_c_i_s_i_o_n_T_r_e_e< L, Y > &f, │ │ │ │ │ - const typename _D_e_c_i_s_i_o_n_T_r_e_e< L, Y >::Unary │ │ │ │ │ - &op) │ │ │ │ │ -  free versions of apply │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - _D_e_c_i_s_i_o_n_T_r_e_e< L, Y >  ggttssaamm::::aappppllyy (const _D_e_c_i_s_i_o_n_T_r_e_e< L, Y > &f, │ │ │ │ │ - const typename _D_e_c_i_s_i_o_n_T_r_e_e< L, Y >:: │ │ │ │ │ - UnaryAssignment &op) │ │ │ │ │ -  Apply unary operator op with _A_s_s_i_g_n_m_e_n_t to │ │ │ │ │ - _D_e_c_i_s_i_o_n_T_r_e_e f. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - _D_e_c_i_s_i_o_n_T_r_e_e< L, Y >  ggttssaamm::::aappppllyy (const _D_e_c_i_s_i_o_n_T_r_e_e< L, Y > &f, │ │ │ │ │ - const _D_e_c_i_s_i_o_n_T_r_e_e< L, Y > &g, const typename │ │ │ │ │ - _D_e_c_i_s_i_o_n_T_r_e_e< L, Y >::Binary &op) │ │ │ │ │ -  Apply binary operator op to _D_e_c_i_s_i_o_n_T_r_e_e f. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -std::pair< _D_e_c_i_s_i_o_n_T_r_e_e< L, T1 >, _g_t_s_a_m_:_:_u_n_z_i_p (const _D_e_c_i_s_i_o_n_T_r_e_e< L, std:: │ │ │ │ │ - _D_e_c_i_s_i_o_n_T_r_e_e< L, T2 > >  pair< T1, T2 > > &input) │ │ │ │ │ -  unzip a _D_e_c_i_s_i_o_n_T_r_e_e with std::pair values. │ │ │ │ │ +std::vector< double >  _g_t_s_a_m_:_:_e_x_p_N_o_r_m_a_l_i_z_e (const std::vector< double > │ │ │ │ │ + &logProbs) │ │ │ │ │ +  Normalize a set of log probabilities. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Decision Tree for use in DiscreteFactors. │ │ │ │ │ + Date │ │ │ │ │ + Feb 14, 2011 │ │ │ │ │ Author │ │ │ │ │ + Duy-Nguyen Ta │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Can Erdogan │ │ │ │ │ - Date │ │ │ │ │ - Jan 30, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_e_c_i_s_i_o_n_T_r_e_e_._h │ │ │ │ │ + * _D_i_s_c_r_e_t_e_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00221.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,8 +1,4 @@ │ │ │ │ │ var a00221 = [ │ │ │ │ │ - ["gtsam::DecisionTree< L, Y >::Node", "a02800.html", "a02800"], │ │ │ │ │ - ["gtsam::traits< DecisionTree< L, Y > >", "a02804.html", null], │ │ │ │ │ - ["apply", "a00221.html#ae890a8919b441443ffdc0d8ab047838d", null], │ │ │ │ │ - ["apply", "a00221.html#ad19a5c8f7d997b0b16cf646659834b0f", null], │ │ │ │ │ - ["apply", "a00221.html#aaf5f5013828ac5e4750377dcad969276", null], │ │ │ │ │ - ["unzip", "a00221.html#a1744c5e018220146e8df7a68a05d5c43", null] │ │ │ │ │ + ["gtsam::traits< DiscreteFactor >", "a02856.html", null], │ │ │ │ │ + ["expNormalize", "a00221.html#ae1c1177c8725296b34601409322400ea", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00221_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DecisionTree.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,333 +98,143 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
DecisionTree.h
│ │ │ │ +
DiscreteFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/base/Testable.h>
│ │ │ │ -
23#include <gtsam/base/types.h>
│ │ │ │ -
24#include <gtsam/discrete/Assignment.h>
│ │ │ │ -
25
│ │ │ │ -
26#include <boost/serialization/nvp.hpp>
│ │ │ │ -
27#include <boost/shared_ptr.hpp>
│ │ │ │ -
28#include <functional>
│ │ │ │ -
29#include <iostream>
│ │ │ │ -
30#include <map>
│ │ │ │ -
31#include <set>
│ │ │ │ -
32#include <sstream>
│ │ │ │ -
33#include <string>
│ │ │ │ -
34#include <utility>
│ │ │ │ -
35#include <vector>
│ │ │ │ -
36
│ │ │ │ -
37namespace gtsam {
│ │ │ │ -
38
│ │ │ │ -
60 template<typename L, typename Y>
│ │ │ │ -
│ │ │ │ -
61 class DecisionTree {
│ │ │ │ -
62 protected:
│ │ │ │ -
│ │ │ │ -
64 static bool DefaultCompare(const Y& a, const Y& b) {
│ │ │ │ -
65 return a == b;
│ │ │ │ -
66 }
│ │ │ │ -
│ │ │ │ -
67
│ │ │ │ -
68 public:
│ │ │ │ -
69 using LabelFormatter = std::function<std::string(L)>;
│ │ │ │ -
70 using ValueFormatter = std::function<std::string(Y)>;
│ │ │ │ -
71 using CompareFunc = std::function<bool(const Y&, const Y&)>;
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/discrete/DiscreteValues.h>
│ │ │ │ +
22#include <gtsam/inference/Factor.h>
│ │ │ │ +
23#include <gtsam/base/Testable.h>
│ │ │ │ +
24
│ │ │ │ +
25#include <string>
│ │ │ │ +
26namespace gtsam {
│ │ │ │ +
27
│ │ │ │ +
28class DecisionTreeFactor;
│ │ │ │ +
29class DiscreteConditional;
│ │ │ │ +
30class HybridValues;
│ │ │ │ +
31
│ │ │ │ +
│ │ │ │ +
38class GTSAM_EXPORT DiscreteFactor: public Factor {
│ │ │ │ +
39
│ │ │ │ +
40public:
│ │ │ │ +
41
│ │ │ │ +
42 // typedefs needed to play nice with gtsam
│ │ │ │ +
43 typedef DiscreteFactor This;
│ │ │ │ +
44 typedef boost::shared_ptr<DiscreteFactor> shared_ptr;
│ │ │ │ +
45 typedef Factor Base;
│ │ │ │ +
46
│ │ │ │ +
47 using Values = DiscreteValues;
│ │ │ │ +
48
│ │ │ │ +
49public:
│ │ │ │ +
50
│ │ │ │ +
53
│ │ │ │ +
55 DiscreteFactor() {}
│ │ │ │ +
56
│ │ │ │ +
59 template<typename CONTAINER>
│ │ │ │ +
60 DiscreteFactor(const CONTAINER& keys) : Base(keys) {}
│ │ │ │ +
61
│ │ │ │ +
│ │ │ │ +
63 virtual ~DiscreteFactor() {
│ │ │ │ +
64 }
│ │ │ │ +
│ │ │ │ +
65
│ │ │ │ +
69
│ │ │ │ +
71 virtual bool equals(const DiscreteFactor& lf, double tol = 1e-9) const = 0;
│ │ │ │
72
│ │ │ │ -
74 using Unary = std::function<Y(const Y&)>;
│ │ │ │ -
75 using UnaryAssignment = std::function<Y(const Assignment<L>&, const Y&)>;
│ │ │ │ -
76 using Binary = std::function<Y(const Y&, const Y&)>;
│ │ │ │ -
77
│ │ │ │ -
79 using LabelC = std::pair<L, size_t>;
│ │ │ │ -
80
│ │ │ │ -
82 struct Leaf;
│ │ │ │ -
83 struct Choice;
│ │ │ │ -
84
│ │ │ │ -
│ │ │ │ -
86 struct Node {
│ │ │ │ -
87 using Ptr = boost::shared_ptr<const Node>;
│ │ │ │ -
88
│ │ │ │ -
89#ifdef DT_DEBUG_MEMORY
│ │ │ │ -
90 static int nrNodes;
│ │ │ │ -
91#endif
│ │ │ │ -
92
│ │ │ │ -
93 // Constructor
│ │ │ │ -
94 Node() {
│ │ │ │ -
95#ifdef DT_DEBUG_MEMORY
│ │ │ │ -
96 std::cout << ++nrNodes << " constructed " << id() << std::endl;
│ │ │ │ -
97 std::cout.flush();
│ │ │ │ -
98#endif
│ │ │ │ -
99 }
│ │ │ │ +
│ │ │ │ +
74 void print(
│ │ │ │ +
75 const std::string& s = "DiscreteFactor\n",
│ │ │ │ +
76 const KeyFormatter& formatter = DefaultKeyFormatter) const override {
│ │ │ │ +
77 Base::print(s, formatter);
│ │ │ │ +
78 }
│ │ │ │ +
│ │ │ │ +
79
│ │ │ │ +
83
│ │ │ │ +
85 virtual double operator()(const DiscreteValues&) const = 0;
│ │ │ │ +
86
│ │ │ │ +
88 double error(const DiscreteValues& values) const;
│ │ │ │ +
89
│ │ │ │ +
94 double error(const HybridValues& c) const override;
│ │ │ │ +
95
│ │ │ │ +
97 virtual DecisionTreeFactor operator*(const DecisionTreeFactor&) const = 0;
│ │ │ │ +
98
│ │ │ │ +
99 virtual DecisionTreeFactor toDecisionTreeFactor() const = 0;
│ │ │ │
100
│ │ │ │ -
101 // Destructor
│ │ │ │ -
102 virtual ~Node() {
│ │ │ │ -
103#ifdef DT_DEBUG_MEMORY
│ │ │ │ -
104 std::cout << --nrNodes << " destructed " << id() << std::endl;
│ │ │ │ -
105 std::cout.flush();
│ │ │ │ -
106#endif
│ │ │ │ -
107 }
│ │ │ │ -
108
│ │ │ │ -
109 // Unique ID for dot files
│ │ │ │ -
110 const void* id() const { return this; }
│ │ │ │ -
111
│ │ │ │ -
112 // everything else is virtual, no documentation here as internal
│ │ │ │ -
113 virtual void print(const std::string& s,
│ │ │ │ -
114 const LabelFormatter& labelFormatter,
│ │ │ │ -
115 const ValueFormatter& valueFormatter) const = 0;
│ │ │ │ -
116 virtual void dot(std::ostream& os, const LabelFormatter& labelFormatter,
│ │ │ │ -
117 const ValueFormatter& valueFormatter,
│ │ │ │ -
118 bool showZero) const = 0;
│ │ │ │ -
119 virtual bool sameLeaf(const Leaf& q) const = 0;
│ │ │ │ -
120 virtual bool sameLeaf(const Node& q) const = 0;
│ │ │ │ -
121 virtual bool equals(const Node& other, const CompareFunc& compare =
│ │ │ │ -
122 &DefaultCompare) const = 0;
│ │ │ │ -
123 virtual const Y& operator()(const Assignment<L>& x) const = 0;
│ │ │ │ -
124 virtual Ptr apply(const Unary& op) const = 0;
│ │ │ │ -
125 virtual Ptr apply(const UnaryAssignment& op,
│ │ │ │ -
126 const Assignment<L>& assignment) const = 0;
│ │ │ │ -
127 virtual Ptr apply_f_op_g(const Node&, const Binary&) const = 0;
│ │ │ │ -
128 virtual Ptr apply_g_op_fL(const Leaf&, const Binary&) const = 0;
│ │ │ │ -
129 virtual Ptr apply_g_op_fC(const Choice&, const Binary&) const = 0;
│ │ │ │ -
130 virtual Ptr choose(const L& label, size_t index) const = 0;
│ │ │ │ -
131 virtual bool isLeaf() const = 0;
│ │ │ │ -
132
│ │ │ │ -
133 private:
│ │ │ │ -
135 friend class boost::serialization::access;
│ │ │ │ -
136 template <class ARCHIVE>
│ │ │ │ -
137 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {}
│ │ │ │ -
138 };
│ │ │ │ -
│ │ │ │ -
141 public:
│ │ │ │ -
143 using NodePtr = typename Node::Ptr;
│ │ │ │ -
144
│ │ │ │ -
146 NodePtr root_;
│ │ │ │ -
147
│ │ │ │ -
148 protected:
│ │ │ │ -
153 template<typename It, typename ValueIt>
│ │ │ │ -
154 NodePtr create(It begin, It end, ValueIt beginY, ValueIt endY) const;
│ │ │ │ +
104
│ │ │ │ +
106 using Names = DiscreteValues::Names;
│ │ │ │ +
107
│ │ │ │ +
115 virtual std::string markdown(
│ │ │ │ +
116 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
117 const Names& names = {}) const = 0;
│ │ │ │ +
118
│ │ │ │ +
126 virtual std::string html(
│ │ │ │ +
127 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
128 const Names& names = {}) const = 0;
│ │ │ │ +
129
│ │ │ │ +
131};
│ │ │ │ +
│ │ │ │ +
132// DiscreteFactor
│ │ │ │ +
133
│ │ │ │ +
134// traits
│ │ │ │ +
135template<> struct traits<DiscreteFactor> : public Testable<DiscreteFactor> {};
│ │ │ │ +
136
│ │ │ │ +
137
│ │ │ │ +
154std::vector<double> expNormalize(const std::vector<double> &logProbs);
│ │ │ │
155
│ │ │ │ -
166 template <typename M, typename X>
│ │ │ │ -
167 NodePtr convertFrom(const typename DecisionTree<M, X>::NodePtr& f,
│ │ │ │ -
168 std::function<L(const M&)> L_of_M,
│ │ │ │ -
169 std::function<Y(const X&)> Y_of_X) const;
│ │ │ │ -
170
│ │ │ │ -
171 public:
│ │ │ │ -
174
│ │ │ │ -
176 DecisionTree();
│ │ │ │ -
177
│ │ │ │ -
179 explicit DecisionTree(const Y& y);
│ │ │ │ -
180
│ │ │ │ -
188 DecisionTree(const L& label, const Y& y1, const Y& y2);
│ │ │ │ -
189
│ │ │ │ -
191 DecisionTree(const LabelC& label, const Y& y1, const Y& y2);
│ │ │ │ -
192
│ │ │ │ -
194 DecisionTree(const std::vector<LabelC>& labelCs, const std::vector<Y>& ys);
│ │ │ │ -
195
│ │ │ │ -
197 DecisionTree(const std::vector<LabelC>& labelCs, const std::string& table);
│ │ │ │ -
198
│ │ │ │ -
200 template<typename Iterator>
│ │ │ │ -
201 DecisionTree(Iterator begin, Iterator end, const L& label);
│ │ │ │ -
202
│ │ │ │ -
204 DecisionTree(const L& label, const DecisionTree& f0,
│ │ │ │ -
205 const DecisionTree& f1);
│ │ │ │ -
206
│ │ │ │ -
214 template <typename X, typename Func>
│ │ │ │ -
215 DecisionTree(const DecisionTree<L, X>& other, Func Y_of_X);
│ │ │ │ -
216
│ │ │ │ -
227 template <typename M, typename X, typename Func>
│ │ │ │ -
228 DecisionTree(const DecisionTree<M, X>& other, const std::map<M, L>& map,
│ │ │ │ -
229 Func Y_of_X);
│ │ │ │ -
230
│ │ │ │ -
234
│ │ │ │ -
242 void print(const std::string& s, const LabelFormatter& labelFormatter,
│ │ │ │ -
243 const ValueFormatter& valueFormatter) const;
│ │ │ │ -
244
│ │ │ │ -
245 // Testable
│ │ │ │ -
246 bool equals(const DecisionTree& other,
│ │ │ │ -
247 const CompareFunc& compare = &DefaultCompare) const;
│ │ │ │ -
248
│ │ │ │ -
252
│ │ │ │ -
254 virtual ~DecisionTree() = default;
│ │ │ │ -
255
│ │ │ │ -
257 bool empty() const { return !root_; }
│ │ │ │ -
258
│ │ │ │ -
260 bool operator==(const DecisionTree& q) const;
│ │ │ │ -
261
│ │ │ │ -
263 const Y& operator()(const Assignment<L>& x) const;
│ │ │ │ -
264
│ │ │ │ -
279 template <typename Func>
│ │ │ │ -
280 void visit(Func f) const;
│ │ │ │ -
281
│ │ │ │ -
296 template <typename Func>
│ │ │ │ -
297 void visitLeaf(Func f) const;
│ │ │ │ -
298
│ │ │ │ -
313 template <typename Func>
│ │ │ │ -
314 void visitWith(Func f) const;
│ │ │ │ -
315
│ │ │ │ -
317 size_t nrLeaves() const;
│ │ │ │ -
318
│ │ │ │ -
334 template <typename Func, typename X>
│ │ │ │ -
335 X fold(Func f, X x0) const;
│ │ │ │ -
336
│ │ │ │ -
338 std::set<L> labels() const;
│ │ │ │ -
339
│ │ │ │ -
341 DecisionTree apply(const Unary& op) const;
│ │ │ │ -
342
│ │ │ │ -
351 DecisionTree apply(const UnaryAssignment& op) const;
│ │ │ │ -
352
│ │ │ │ -
354 DecisionTree apply(const DecisionTree& g, const Binary& op) const;
│ │ │ │ -
355
│ │ │ │ -
│ │ │ │ -
358 DecisionTree choose(const L& label, size_t index) const {
│ │ │ │ -
359 NodePtr newRoot = root_->choose(label, index);
│ │ │ │ -
360 return DecisionTree(newRoot);
│ │ │ │ -
361 }
│ │ │ │ -
│ │ │ │ -
362
│ │ │ │ -
364 DecisionTree combine(const L& label, size_t cardinality,
│ │ │ │ -
365 const Binary& op) const;
│ │ │ │ -
366
│ │ │ │ -
│ │ │ │ -
368 DecisionTree combine(const LabelC& labelC, const Binary& op) const {
│ │ │ │ -
369 return combine(labelC.first, labelC.second, op);
│ │ │ │ -
370 }
│ │ │ │ -
│ │ │ │ -
371
│ │ │ │ -
373 void dot(std::ostream& os, const LabelFormatter& labelFormatter,
│ │ │ │ -
374 const ValueFormatter& valueFormatter, bool showZero = true) const;
│ │ │ │ -
375
│ │ │ │ -
377 void dot(const std::string& name, const LabelFormatter& labelFormatter,
│ │ │ │ -
378 const ValueFormatter& valueFormatter, bool showZero = true) const;
│ │ │ │ -
379
│ │ │ │ -
381 std::string dot(const LabelFormatter& labelFormatter,
│ │ │ │ -
382 const ValueFormatter& valueFormatter,
│ │ │ │ -
383 bool showZero = true) const;
│ │ │ │ -
384
│ │ │ │ -
387
│ │ │ │ -
388 // internal use only
│ │ │ │ -
389 explicit DecisionTree(const NodePtr& root);
│ │ │ │ -
390
│ │ │ │ -
391 // internal use only
│ │ │ │ -
392 template<typename Iterator> NodePtr
│ │ │ │ -
393 compose(Iterator begin, Iterator end, const L& label) const;
│ │ │ │ -
394
│ │ │ │ -
396
│ │ │ │ -
397 private:
│ │ │ │ -
399 friend class boost::serialization::access;
│ │ │ │ -
400 template <class ARCHIVE>
│ │ │ │ -
401 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ -
402 ar& BOOST_SERIALIZATION_NVP(root_);
│ │ │ │ -
403 }
│ │ │ │ -
404 }; // DecisionTree
│ │ │ │ -
│ │ │ │ -
405
│ │ │ │ -
406 template <class L, class Y>
│ │ │ │ -
407 struct traits<DecisionTree<L, Y>> : public Testable<DecisionTree<L, Y>> {};
│ │ │ │ -
408
│ │ │ │ -
412 template<typename L, typename Y>
│ │ │ │ -
│ │ │ │ -
413 DecisionTree<L, Y> apply(const DecisionTree<L, Y>& f,
│ │ │ │ -
414 const typename DecisionTree<L, Y>::Unary& op) {
│ │ │ │ -
415 return f.apply(op);
│ │ │ │ -
416 }
│ │ │ │ -
│ │ │ │ -
417
│ │ │ │ -
419 template<typename L, typename Y>
│ │ │ │ -
│ │ │ │ -
420 DecisionTree<L, Y> apply(const DecisionTree<L, Y>& f,
│ │ │ │ -
421 const typename DecisionTree<L, Y>::UnaryAssignment& op) {
│ │ │ │ -
422 return f.apply(op);
│ │ │ │ -
423 }
│ │ │ │ -
│ │ │ │ -
424
│ │ │ │ -
426 template<typename L, typename Y>
│ │ │ │ -
│ │ │ │ -
427 DecisionTree<L, Y> apply(const DecisionTree<L, Y>& f,
│ │ │ │ -
428 const DecisionTree<L, Y>& g,
│ │ │ │ -
429 const typename DecisionTree<L, Y>::Binary& op) {
│ │ │ │ -
430 return f.apply(g, op);
│ │ │ │ -
431 }
│ │ │ │ -
│ │ │ │ -
432
│ │ │ │ -
439 template <typename L, typename T1, typename T2>
│ │ │ │ -
│ │ │ │ -
440 std::pair<DecisionTree<L, T1>, DecisionTree<L, T2> > unzip(
│ │ │ │ -
441 const DecisionTree<L, std::pair<T1, T2> >& input) {
│ │ │ │ -
442 return std::make_pair(
│ │ │ │ -
443 DecisionTree<L, T1>(input, [](std::pair<T1, T2> i) { return i.first; }),
│ │ │ │ -
444 DecisionTree<L, T2>(input,
│ │ │ │ -
445 [](std::pair<T1, T2> i) { return i.second; }));
│ │ │ │ -
446 }
│ │ │ │ -
│ │ │ │ -
447
│ │ │ │ -
448} // namespace gtsam
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ -
types.h
Typedefs for easier changing of types.
│ │ │ │ -
Assignment.h
An assignment from labels to a discrete value index (size_t)
│ │ │ │ +
156
│ │ │ │ +
157}// namespace gtsam
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ + │ │ │ │ +
Factor.h
The base class for all factors.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::unzip
std::pair< DecisionTree< L, T1 >, DecisionTree< L, T2 > > unzip(const DecisionTree< L, std::pair< T1, T2 > > &input)
unzip a DecisionTree with std::pair values.
Definition DecisionTree.h:440
│ │ │ │ -
gtsam::apply
DecisionTree< L, Y > apply(const DecisionTree< L, Y > &f, const typename DecisionTree< L, Y >::Unary &op)
free versions of apply
Definition DecisionTree.h:413
│ │ │ │ +
gtsam::expNormalize
std::vector< double > expNormalize(const std::vector< double > &logProbs)
Normalize a set of log probabilities.
Definition DiscreteFactor.cpp:42
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::Assignment
An assignment from labels to value index (size_t).
Definition Assignment.h:37
│ │ │ │ -
gtsam::DecisionTree::Leaf
Definition DecisionTree-inl.h:52
│ │ │ │ -
gtsam::DecisionTree::Choice
Definition DecisionTree-inl.h:172
│ │ │ │ -
gtsam::DecisionTree
a decision tree is a function from assignments to values.
Definition DecisionTree.h:61
│ │ │ │ -
gtsam::DecisionTree::apply
DecisionTree apply(const Unary &op) const
apply Unary operation "op" to f
Definition DecisionTree-inl.h:889
│ │ │ │ -
gtsam::DecisionTree::choose
DecisionTree choose(const L &label, size_t index) const
create a new function where value(label)==index It's like "restrict" in Darwiche09book pg329,...
Definition DecisionTree.h:358
│ │ │ │ -
gtsam::DecisionTree::convertFrom
NodePtr convertFrom(const typename DecisionTree< M, X >::NodePtr &f, std::function< L(const M &)> L_of_M, std::function< Y(const X &)> Y_of_X) const
Convert from a DecisionTree<M, X> to DecisionTree<L, Y>.
Definition DecisionTree-inl.h:671
│ │ │ │ -
gtsam::DecisionTree::combine
DecisionTree combine(const LabelC &labelC, const Binary &op) const
combine with LabelC for convenience
Definition DecisionTree.h:368
│ │ │ │ -
gtsam::DecisionTree::create
NodePtr create(It begin, It end, ValueIt beginY, ValueIt endY) const
Internal recursive function to create from keys, cardinalities, and Y values.
Definition DecisionTree-inl.h:630
│ │ │ │ -
gtsam::DecisionTree::~DecisionTree
virtual ~DecisionTree()=default
Make virtual.
│ │ │ │ -
gtsam::DecisionTree::DefaultCompare
static bool DefaultCompare(const Y &a, const Y &b)
Default method for comparison of two objects of type Y.
Definition DecisionTree.h:64
│ │ │ │ -
gtsam::DecisionTree::NodePtr
typename Node::Ptr NodePtr
---------------------— Node base class ------------------------—
Definition DecisionTree.h:143
│ │ │ │ -
gtsam::DecisionTree::labels
std::set< L > labels() const
Retrieve all unique labels as a set.
Definition DecisionTree-inl.h:853
│ │ │ │ -
gtsam::DecisionTree::empty
bool empty() const
Check if tree is empty.
Definition DecisionTree.h:257
│ │ │ │ -
gtsam::DecisionTree::visit
void visit(Func f) const
Visit all leaves in depth-first fashion.
Definition DecisionTree-inl.h:736
│ │ │ │ -
gtsam::DecisionTree::visitLeaf
void visitLeaf(Func f) const
Visit all leaves in depth-first fashion.
Definition DecisionTree-inl.h:773
│ │ │ │ -
gtsam::DecisionTree::Unary
std::function< Y(const Y &)> Unary
Handy typedefs for unary and binary function types.
Definition DecisionTree.h:74
│ │ │ │ -
gtsam::DecisionTree::fold
X fold(Func f, X x0) const
Fold a binary function over the tree, returning accumulator.
Definition DecisionTree-inl.h:833
│ │ │ │ -
gtsam::DecisionTree::root_
NodePtr root_
A DecisionTree just contains the root. TODO(dellaert): make protected.
Definition DecisionTree.h:146
│ │ │ │ -
gtsam::DecisionTree::print
void print(const std::string &s, const LabelFormatter &labelFormatter, const ValueFormatter &valueFormatter) const
GTSAM-style print.
Definition DecisionTree-inl.h:872
│ │ │ │ -
gtsam::DecisionTree::combine
DecisionTree combine(const L &label, size_t cardinality, const Binary &op) const
combine subtrees on key with binary operation "op"
Definition DecisionTree-inl.h:937
│ │ │ │ -
gtsam::DecisionTree::visitWith
void visitWith(Func f) const
Visit all leaves in depth-first fashion.
Definition DecisionTree-inl.h:816
│ │ │ │ -
gtsam::DecisionTree::operator()
const Y & operator()(const Assignment< L > &x) const
evaluate
Definition DecisionTree-inl.h:884
│ │ │ │ -
gtsam::DecisionTree::dot
void dot(std::ostream &os, const LabelFormatter &labelFormatter, const ValueFormatter &valueFormatter, bool showZero=true) const
output to graphviz format, stream version
Definition DecisionTree-inl.h:949
│ │ │ │ -
gtsam::DecisionTree::access
friend class boost::serialization::access
Serialization function.
Definition DecisionTree.h:399
│ │ │ │ -
gtsam::DecisionTree::operator==
bool operator==(const DecisionTree &q) const
equality
Definition DecisionTree-inl.h:879
│ │ │ │ -
gtsam::DecisionTree::LabelC
std::pair< L, size_t > LabelC
A label annotated with cardinality.
Definition DecisionTree.h:79
│ │ │ │ -
gtsam::DecisionTree::nrLeaves
size_t nrLeaves() const
Return the number of leaves in the tree.
Definition DecisionTree-inl.h:823
│ │ │ │ -
gtsam::DecisionTree::DecisionTree
DecisionTree()
Default constructor (for serialization)
Definition DecisionTree-inl.h:462
│ │ │ │ -
gtsam::DecisionTree::Node
---------------------— Node base class ------------------------—
Definition DecisionTree.h:86
│ │ │ │ -
gtsam::DecisionTree::Node::access
friend class boost::serialization::access
Serialization function.
Definition DecisionTree.h:135
│ │ │ │ +
gtsam::DecisionTreeFactor
A discrete probabilistic factor.
Definition DecisionTreeFactor.h:45
│ │ │ │ +
gtsam::DiscreteFactor
Base class for discrete probabilistic factors The most general one is the derived DecisionTreeFactor.
Definition DiscreteFactor.h:38
│ │ │ │ +
gtsam::DiscreteFactor::html
virtual std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter, const Names &names={}) const =0
Render as html table.
│ │ │ │ +
gtsam::DiscreteFactor::Base
Factor Base
Our base class.
Definition DiscreteFactor.h:45
│ │ │ │ +
gtsam::DiscreteFactor::print
void print(const std::string &s="DiscreteFactor\n", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print
Definition DiscreteFactor.h:74
│ │ │ │ +
gtsam::DiscreteFactor::shared_ptr
boost::shared_ptr< DiscreteFactor > shared_ptr
shared_ptr to this class
Definition DiscreteFactor.h:44
│ │ │ │ +
gtsam::DiscreteFactor::~DiscreteFactor
virtual ~DiscreteFactor()
Virtual destructor.
Definition DiscreteFactor.h:63
│ │ │ │ +
gtsam::DiscreteFactor::Names
DiscreteValues::Names Names
Translation table from values to strings.
Definition DiscreteFactor.h:106
│ │ │ │ +
gtsam::DiscreteFactor::equals
virtual bool equals(const DiscreteFactor &lf, double tol=1e-9) const =0
equals
│ │ │ │ +
gtsam::DiscreteFactor::This
DiscreteFactor This
This class.
Definition DiscreteFactor.h:43
│ │ │ │ +
gtsam::DiscreteFactor::operator*
virtual DecisionTreeFactor operator*(const DecisionTreeFactor &) const =0
Multiply in a DecisionTreeFactor and return the result as DecisionTreeFactor.
│ │ │ │ +
gtsam::DiscreteFactor::operator()
virtual double operator()(const DiscreteValues &) const =0
Find value for given assignment of values to variables.
│ │ │ │ +
gtsam::DiscreteFactor::markdown
virtual std::string markdown(const KeyFormatter &keyFormatter=DefaultKeyFormatter, const Names &names={}) const =0
Render as markdown table.
│ │ │ │ +
gtsam::DiscreteFactor::DiscreteFactor
DiscreteFactor(const CONTAINER &keys)
Construct from container of keys.
Definition DiscreteFactor.h:60
│ │ │ │ +
gtsam::DiscreteFactor::DiscreteFactor
DiscreteFactor()
Default constructor creates empty factor.
Definition DiscreteFactor.h:55
│ │ │ │ +
gtsam::DiscreteValues
A map from keys to values.
Definition DiscreteValues.h:34
│ │ │ │ +
gtsam::DiscreteValues::Names
std::map< Key, std::vector< std::string > > Names
Translation table from values to strings.
Definition DiscreteValues.h:95
│ │ │ │ +
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ +
gtsam::Factor
Definition Factor.h:68
│ │ │ │ +
HybridValues
the error.
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── encoding │ │ │ │ │ @@ -1 +1 @@ │ │ │ │ │ -utf-8 │ │ │ │ │ +us-ascii │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,417 +1,196 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DecisionTree.h │ │ │ │ │ +DiscreteFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_A_s_s_i_g_n_m_e_n_t_._h> │ │ │ │ │ -25 │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ -28#include │ │ │ │ │ -29#include │ │ │ │ │ -30#include │ │ │ │ │ -31#include │ │ │ │ │ -32#include │ │ │ │ │ -33#include │ │ │ │ │ -34#include │ │ │ │ │ -35#include │ │ │ │ │ -36 │ │ │ │ │ -37namespace _g_t_s_a_m { │ │ │ │ │ -38 │ │ │ │ │ -60 template │ │ │ │ │ -_6_1 class _D_e_c_i_s_i_o_n_T_r_e_e { │ │ │ │ │ -62 protected: │ │ │ │ │ -_6_4 static bool _D_e_f_a_u_l_t_C_o_m_p_a_r_e(const Y& a, const Y& b) { │ │ │ │ │ -65 return a == b; │ │ │ │ │ -66 } │ │ │ │ │ -67 │ │ │ │ │ -68 public: │ │ │ │ │ -69 using LabelFormatter = std::function; │ │ │ │ │ -70 using ValueFormatter = std::function; │ │ │ │ │ -71 using CompareFunc = std::function; │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_V_a_l_u_e_s_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +24 │ │ │ │ │ +25#include │ │ │ │ │ +26namespace _g_t_s_a_m { │ │ │ │ │ +27 │ │ │ │ │ +28class DecisionTreeFactor; │ │ │ │ │ +29class DiscreteConditional; │ │ │ │ │ +30class _H_y_b_r_i_d_V_a_l_u_e_s; │ │ │ │ │ +31 │ │ │ │ │ +_3_8class GTSAM_EXPORT _D_i_s_c_r_e_t_e_F_a_c_t_o_r: public _F_a_c_t_o_r { │ │ │ │ │ +39 │ │ │ │ │ +40public: │ │ │ │ │ +41 │ │ │ │ │ +42 // typedefs needed to play nice with gtsam │ │ │ │ │ +_4_3 typedef _D_i_s_c_r_e_t_e_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ +_4_4 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +_4_5 typedef _F_a_c_t_o_r _B_a_s_e; │ │ │ │ │ +46 │ │ │ │ │ +_4_7 using _V_a_l_u_e_s = _D_i_s_c_r_e_t_e_V_a_l_u_e_s; │ │ │ │ │ +48 │ │ │ │ │ +49public: │ │ │ │ │ +50 │ │ │ │ │ +53 │ │ │ │ │ +_5_5 _D_i_s_c_r_e_t_e_F_a_c_t_o_r() {} │ │ │ │ │ +56 │ │ │ │ │ +59 template │ │ │ │ │ +_6_0 _D_i_s_c_r_e_t_e_F_a_c_t_o_r(const CONTAINER& keys) : _B_a_s_e(keys) {} │ │ │ │ │ +61 │ │ │ │ │ +_6_3 virtual _~_D_i_s_c_r_e_t_e_F_a_c_t_o_r() { │ │ │ │ │ +64 } │ │ │ │ │ +65 │ │ │ │ │ +69 │ │ │ │ │ +_7_1 virtual bool _e_q_u_a_l_s(const _D_i_s_c_r_e_t_e_F_a_c_t_o_r& lf, double tol = 1e-9) const = 0; │ │ │ │ │ 72 │ │ │ │ │ -_7_4 using _U_n_a_r_y = std::function; │ │ │ │ │ -75 using UnaryAssignment = std::function&, const Y&)>; │ │ │ │ │ -76 using Binary = std::function; │ │ │ │ │ -77 │ │ │ │ │ -_7_9 using _L_a_b_e_l_C = std::pair; │ │ │ │ │ -80 │ │ │ │ │ -82 struct _L_e_a_f; │ │ │ │ │ -83 struct _C_h_o_i_c_e; │ │ │ │ │ -84 │ │ │ │ │ -_8_6 struct _N_o_d_e { │ │ │ │ │ -87 using Ptr = boost::shared_ptr; │ │ │ │ │ -88 │ │ │ │ │ -89#ifdef DT_DEBUG_MEMORY │ │ │ │ │ -90 static int nrNodes; │ │ │ │ │ -91#endif │ │ │ │ │ -92 │ │ │ │ │ -93 // Constructor │ │ │ │ │ -94 _N_o_d_e() { │ │ │ │ │ -95#ifdef DT_DEBUG_MEMORY │ │ │ │ │ -96 std::cout << ++nrNodes << " constructed " << id() << std::endl; │ │ │ │ │ -97 std::cout.flush(); │ │ │ │ │ -98#endif │ │ │ │ │ -99 } │ │ │ │ │ +_7_4 void _p_r_i_n_t( │ │ │ │ │ +75 const std::string& s = "DiscreteFactor\n", │ │ │ │ │ +76 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override { │ │ │ │ │ +77 Base::print(s, formatter); │ │ │ │ │ +78 } │ │ │ │ │ +79 │ │ │ │ │ +83 │ │ │ │ │ +_8_5 virtual double _o_p_e_r_a_t_o_r_(_)(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s&) const = 0; │ │ │ │ │ +86 │ │ │ │ │ +88 double error(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values) const; │ │ │ │ │ +89 │ │ │ │ │ +94 double error(const _H_y_b_r_i_d_V_a_l_u_e_s& c) const override; │ │ │ │ │ +95 │ │ │ │ │ +_9_7 virtual _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r _o_p_e_r_a_t_o_r_*(const _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r&) const = 0; │ │ │ │ │ +98 │ │ │ │ │ +99 virtual _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r toDecisionTreeFactor() const = 0; │ │ │ │ │ 100 │ │ │ │ │ -101 // Destructor │ │ │ │ │ -102 virtual _~_N_o_d_e() { │ │ │ │ │ -103#ifdef DT_DEBUG_MEMORY │ │ │ │ │ -104 std::cout << --nrNodes << " destructed " << id() << std::endl; │ │ │ │ │ -105 std::cout.flush(); │ │ │ │ │ -106#endif │ │ │ │ │ -107 } │ │ │ │ │ -108 │ │ │ │ │ -109 // Unique ID for dot files │ │ │ │ │ -110 const void* id() const { return this; } │ │ │ │ │ -111 │ │ │ │ │ -112 // everything else is virtual, no documentation here as internal │ │ │ │ │ -113 virtual void print(const std::string& s, │ │ │ │ │ -114 const LabelFormatter& labelFormatter, │ │ │ │ │ -115 const ValueFormatter& valueFormatter) const = 0; │ │ │ │ │ -116 virtual void dot(std::ostream& os, const LabelFormatter& labelFormatter, │ │ │ │ │ -117 const ValueFormatter& valueFormatter, │ │ │ │ │ -118 bool showZero) const = 0; │ │ │ │ │ -119 virtual bool sameLeaf(const _L_e_a_f& q) const = 0; │ │ │ │ │ -120 virtual bool sameLeaf(const _N_o_d_e& q) const = 0; │ │ │ │ │ -121 virtual bool _e_q_u_a_l_s(const _N_o_d_e& other, const CompareFunc& compare = │ │ │ │ │ -122 &_D_e_f_a_u_l_t_C_o_m_p_a_r_e) const = 0; │ │ │ │ │ -123 virtual const Y& operator()(const _A_s_s_i_g_n_m_e_n_t_<_L_>& x) const = 0; │ │ │ │ │ -124 virtual Ptr apply(const _U_n_a_r_y& op) const = 0; │ │ │ │ │ -125 virtual Ptr apply(const UnaryAssignment& op, │ │ │ │ │ -126 const _A_s_s_i_g_n_m_e_n_t_<_L_>& assignment) const = 0; │ │ │ │ │ -127 virtual Ptr apply_f_op_g(const _N_o_d_e&, const Binary&) const = 0; │ │ │ │ │ -128 virtual Ptr apply_g_op_fL(const _L_e_a_f&, const Binary&) const = 0; │ │ │ │ │ -129 virtual Ptr apply_g_op_fC(const _C_h_o_i_c_e&, const Binary&) const = 0; │ │ │ │ │ -130 virtual Ptr choose(const L& label, size_t index) const = 0; │ │ │ │ │ -131 virtual bool isLeaf() const = 0; │ │ │ │ │ -132 │ │ │ │ │ -133 private: │ │ │ │ │ -_1_3_5 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -136 template │ │ │ │ │ -137 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {} │ │ │ │ │ -138 }; │ │ │ │ │ -141 public: │ │ │ │ │ -_1_4_3 using _N_o_d_e_P_t_r = typename Node::Ptr; │ │ │ │ │ -144 │ │ │ │ │ -_1_4_6 _N_o_d_e_P_t_r _r_o_o_t__; │ │ │ │ │ -147 │ │ │ │ │ -148 protected: │ │ │ │ │ -153 template │ │ │ │ │ -154 _N_o_d_e_P_t_r _c_r_e_a_t_e(It begin, It end, ValueIt beginY, ValueIt endY) const; │ │ │ │ │ +104 │ │ │ │ │ +_1_0_6 using _N_a_m_e_s = _D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_N_a_m_e_s; │ │ │ │ │ +107 │ │ │ │ │ +_1_1_5 virtual std::string _m_a_r_k_d_o_w_n( │ │ │ │ │ +116 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ +117 const _N_a_m_e_s& names = {}) const = 0; │ │ │ │ │ +118 │ │ │ │ │ +_1_2_6 virtual std::string _h_t_m_l( │ │ │ │ │ +127 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ +128 const _N_a_m_e_s& names = {}) const = 0; │ │ │ │ │ +129 │ │ │ │ │ +131}; │ │ │ │ │ +132// DiscreteFactor │ │ │ │ │ +133 │ │ │ │ │ +134// traits │ │ │ │ │ +_1_3_5template<> struct _t_r_a_i_t_s<_D_i_s_c_r_e_t_e_F_a_c_t_o_r> : public _T_e_s_t_a_b_l_e │ │ │ │ │ +{}; │ │ │ │ │ +136 │ │ │ │ │ +137 │ │ │ │ │ +154std::vector _e_x_p_N_o_r_m_a_l_i_z_e(const std::vector &logProbs); │ │ │ │ │ 155 │ │ │ │ │ -166 template │ │ │ │ │ -167 _N_o_d_e_P_t_r _c_o_n_v_e_r_t_F_r_o_m(const typename _D_e_c_i_s_i_o_n_T_r_e_e_<_M_,_ _X_>_:_:_N_o_d_e_P_t_r& f, │ │ │ │ │ -168 std::function L_of_M, │ │ │ │ │ -169 std::function Y_of_X) const; │ │ │ │ │ -170 │ │ │ │ │ -171 public: │ │ │ │ │ -174 │ │ │ │ │ -176 _D_e_c_i_s_i_o_n_T_r_e_e(); │ │ │ │ │ -177 │ │ │ │ │ -179 explicit _D_e_c_i_s_i_o_n_T_r_e_e(const Y& y); │ │ │ │ │ -180 │ │ │ │ │ -188 _D_e_c_i_s_i_o_n_T_r_e_e(const L& label, const Y& y1, const Y& y2); │ │ │ │ │ -189 │ │ │ │ │ -191 _D_e_c_i_s_i_o_n_T_r_e_e(const _L_a_b_e_l_C& label, const Y& y1, const Y& y2); │ │ │ │ │ -192 │ │ │ │ │ -194 _D_e_c_i_s_i_o_n_T_r_e_e(const std::vector& labelCs, const std::vector& ys); │ │ │ │ │ -195 │ │ │ │ │ -197 _D_e_c_i_s_i_o_n_T_r_e_e(const std::vector& labelCs, const std::string& table); │ │ │ │ │ -198 │ │ │ │ │ -200 template │ │ │ │ │ -201 _D_e_c_i_s_i_o_n_T_r_e_e(Iterator begin, Iterator end, const L& label); │ │ │ │ │ -202 │ │ │ │ │ -204 _D_e_c_i_s_i_o_n_T_r_e_e(const L& label, const _D_e_c_i_s_i_o_n_T_r_e_e& f0, │ │ │ │ │ -205 const _D_e_c_i_s_i_o_n_T_r_e_e& f1); │ │ │ │ │ -206 │ │ │ │ │ -214 template │ │ │ │ │ -215 _D_e_c_i_s_i_o_n_T_r_e_e(const _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _X_>& other, Func Y_of_X); │ │ │ │ │ -216 │ │ │ │ │ -227 template │ │ │ │ │ -228 _D_e_c_i_s_i_o_n_T_r_e_e(const _D_e_c_i_s_i_o_n_T_r_e_e_<_M_,_ _X_>& other, const std::map& map, │ │ │ │ │ -229 Func Y_of_X); │ │ │ │ │ -230 │ │ │ │ │ -234 │ │ │ │ │ -242 void _p_r_i_n_t(const std::string& s, const LabelFormatter& labelFormatter, │ │ │ │ │ -243 const ValueFormatter& valueFormatter) const; │ │ │ │ │ -244 │ │ │ │ │ -245 // Testable │ │ │ │ │ -246 bool _e_q_u_a_l_s(const _D_e_c_i_s_i_o_n_T_r_e_e& other, │ │ │ │ │ -247 const CompareFunc& compare = &_D_e_f_a_u_l_t_C_o_m_p_a_r_e) const; │ │ │ │ │ -248 │ │ │ │ │ -252 │ │ │ │ │ -_2_5_4 virtual _~_D_e_c_i_s_i_o_n_T_r_e_e() = default; │ │ │ │ │ -255 │ │ │ │ │ -_2_5_7 bool _e_m_p_t_y() const { return !_r_o_o_t__; } │ │ │ │ │ -258 │ │ │ │ │ -260 bool _o_p_e_r_a_t_o_r_=_=(const _D_e_c_i_s_i_o_n_T_r_e_e& q) const; │ │ │ │ │ -261 │ │ │ │ │ -263 const Y& _o_p_e_r_a_t_o_r_(_)(const _A_s_s_i_g_n_m_e_n_t_<_L_>& x) const; │ │ │ │ │ -264 │ │ │ │ │ -279 template │ │ │ │ │ -280 void _v_i_s_i_t(Func f) const; │ │ │ │ │ -281 │ │ │ │ │ -296 template │ │ │ │ │ -297 void _v_i_s_i_t_L_e_a_f(Func f) const; │ │ │ │ │ -298 │ │ │ │ │ -313 template │ │ │ │ │ -314 void _v_i_s_i_t_W_i_t_h(Func f) const; │ │ │ │ │ -315 │ │ │ │ │ -317 size_t _n_r_L_e_a_v_e_s() const; │ │ │ │ │ -318 │ │ │ │ │ -334 template │ │ │ │ │ -335 X _f_o_l_d(Func f, X x0) const; │ │ │ │ │ -336 │ │ │ │ │ -338 std::set _l_a_b_e_l_s() const; │ │ │ │ │ -339 │ │ │ │ │ -341 _D_e_c_i_s_i_o_n_T_r_e_e _a_p_p_l_y(const _U_n_a_r_y& op) const; │ │ │ │ │ -342 │ │ │ │ │ -351 _D_e_c_i_s_i_o_n_T_r_e_e _a_p_p_l_y(const UnaryAssignment& op) const; │ │ │ │ │ -352 │ │ │ │ │ -354 _D_e_c_i_s_i_o_n_T_r_e_e _a_p_p_l_y(const _D_e_c_i_s_i_o_n_T_r_e_e& g, const Binary& op) const; │ │ │ │ │ -355 │ │ │ │ │ -_3_5_8 _D_e_c_i_s_i_o_n_T_r_e_e _c_h_o_o_s_e(const L& label, size_t index) const { │ │ │ │ │ -359 _N_o_d_e_P_t_r newRoot = _r_o_o_t__->_c_h_o_o_s_e(label, index); │ │ │ │ │ -360 return _D_e_c_i_s_i_o_n_T_r_e_e(newRoot); │ │ │ │ │ -361 } │ │ │ │ │ -362 │ │ │ │ │ -364 _D_e_c_i_s_i_o_n_T_r_e_e combine(const L& label, size_t cardinality, │ │ │ │ │ -365 const Binary& op) const; │ │ │ │ │ -366 │ │ │ │ │ -_3_6_8 _D_e_c_i_s_i_o_n_T_r_e_e _c_o_m_b_i_n_e(const _L_a_b_e_l_C& labelC, const Binary& op) const { │ │ │ │ │ -369 return _c_o_m_b_i_n_e(labelC.first, labelC.second, op); │ │ │ │ │ -370 } │ │ │ │ │ -371 │ │ │ │ │ -373 void _d_o_t(std::ostream& os, const LabelFormatter& labelFormatter, │ │ │ │ │ -374 const ValueFormatter& valueFormatter, bool showZero = true) const; │ │ │ │ │ -375 │ │ │ │ │ -377 void _d_o_t(const std::string& name, const LabelFormatter& labelFormatter, │ │ │ │ │ -378 const ValueFormatter& valueFormatter, bool showZero = true) const; │ │ │ │ │ -379 │ │ │ │ │ -381 std::string _d_o_t(const LabelFormatter& labelFormatter, │ │ │ │ │ -382 const ValueFormatter& valueFormatter, │ │ │ │ │ -383 bool showZero = true) const; │ │ │ │ │ -384 │ │ │ │ │ -387 │ │ │ │ │ -388 // internal use only │ │ │ │ │ -389 explicit _D_e_c_i_s_i_o_n_T_r_e_e(const _N_o_d_e_P_t_r& root); │ │ │ │ │ -390 │ │ │ │ │ -391 // internal use only │ │ │ │ │ -392 template _N_o_d_e_P_t_r │ │ │ │ │ -393 compose(Iterator begin, Iterator end, const L& label) const; │ │ │ │ │ -394 │ │ │ │ │ -396 │ │ │ │ │ -397 private: │ │ │ │ │ -_3_9_9 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -400 template │ │ │ │ │ -401 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ -402 ar& BOOST_SERIALIZATION_NVP(_r_o_o_t__); │ │ │ │ │ -403 } │ │ │ │ │ -404 }; // DecisionTree │ │ │ │ │ -405 │ │ │ │ │ -406 template │ │ │ │ │ -_4_0_7 struct _t_r_a_i_t_s<_D_e_c_i_s_i_o_n_T_r_e_e> : public _T_e_s_t_a_b_l_e> {}; │ │ │ │ │ -408 │ │ │ │ │ -412 template │ │ │ │ │ -_4_1_3 _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_> _a_p_p_l_y(const _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>& f, │ │ │ │ │ -414 const typename _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_U_n_a_r_y& op) { │ │ │ │ │ -415 return f._a_p_p_l_y(op); │ │ │ │ │ -416 } │ │ │ │ │ -417 │ │ │ │ │ -419 template │ │ │ │ │ -_4_2_0 _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_> _a_p_p_l_y(const _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>& f, │ │ │ │ │ -421 const typename DecisionTree::UnaryAssignment& op) { │ │ │ │ │ -422 return f._a_p_p_l_y(op); │ │ │ │ │ -423 } │ │ │ │ │ -424 │ │ │ │ │ -426 template │ │ │ │ │ -_4_2_7 _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_> _a_p_p_l_y(const _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>& f, │ │ │ │ │ -428 const _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>& g, │ │ │ │ │ -429 const typename DecisionTree::Binary& op) { │ │ │ │ │ -430 return f._a_p_p_l_y(g, op); │ │ │ │ │ -431 } │ │ │ │ │ -432 │ │ │ │ │ -439 template │ │ │ │ │ -_4_4_0 std::pair, _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _T_2_> > _u_n_z_i_p( │ │ │ │ │ -441 const _D_e_c_i_s_i_o_n_T_r_e_e >& input) { │ │ │ │ │ -442 return std::make_pair( │ │ │ │ │ -443 _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _T_1_>(input, [](std::pair i) { return i.first; }), │ │ │ │ │ -444 _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _T_2_>(input, │ │ │ │ │ -445 [](std::pair i) { return i.second; })); │ │ │ │ │ -446 } │ │ │ │ │ -447 │ │ │ │ │ -448} // namespace gtsam │ │ │ │ │ +156 │ │ │ │ │ +157}// namespace gtsam │ │ │ │ │ _T_e_s_t_a_b_l_e_._h │ │ │ │ │ Concept check for values that can be used in unit tests. │ │ │ │ │ -_t_y_p_e_s_._h │ │ │ │ │ -Typedefs for easier changing of types. │ │ │ │ │ -_A_s_s_i_g_n_m_e_n_t_._h │ │ │ │ │ -An assignment from labels to a discrete value index (size_t) │ │ │ │ │ +_D_i_s_c_r_e_t_e_V_a_l_u_e_s_._h │ │ │ │ │ +_F_a_c_t_o_r_._h │ │ │ │ │ +The base class for all factors. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_u_n_z_i_p │ │ │ │ │ -std::pair< DecisionTree< L, T1 >, DecisionTree< L, T2 > > unzip(const │ │ │ │ │ -DecisionTree< L, std::pair< T1, T2 > > &input) │ │ │ │ │ -unzip a DecisionTree with std::pair values. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree.h:440 │ │ │ │ │ -_g_t_s_a_m_:_:_a_p_p_l_y │ │ │ │ │ -DecisionTree< L, Y > apply(const DecisionTree< L, Y > &f, const typename │ │ │ │ │ -DecisionTree< L, Y >::Unary &op) │ │ │ │ │ -free versions of apply │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree.h:413 │ │ │ │ │ +_g_t_s_a_m_:_:_e_x_p_N_o_r_m_a_l_i_z_e │ │ │ │ │ +std::vector< double > expNormalize(const std::vector< double > &logProbs) │ │ │ │ │ +Normalize a set of log probabilities. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactor.cpp:42 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ _g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ A helper that implements the traits interface for GTSAM types. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_A_s_s_i_g_n_m_e_n_t │ │ │ │ │ -An assignment from labels to value index (size_t). │ │ │ │ │ -DDeeffiinniittiioonn Assignment.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_L_e_a_f │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_C_h_o_i_c_e │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:172 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e │ │ │ │ │ -a decision tree is a function from assignments to values. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree.h:61 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_a_p_p_l_y │ │ │ │ │ -DecisionTree apply(const Unary &op) const │ │ │ │ │ -apply Unary operation "op" to f │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:889 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_c_h_o_o_s_e │ │ │ │ │ -DecisionTree choose(const L &label, size_t index) const │ │ │ │ │ -create a new function where value(label)==index It's like "restrict" in │ │ │ │ │ -Darwiche09book pg329,... │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree.h:358 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_c_o_n_v_e_r_t_F_r_o_m │ │ │ │ │ -NodePtr convertFrom(const typename DecisionTree< M, X >::NodePtr &f, std:: │ │ │ │ │ -function< L(const M &)> L_of_M, std::function< Y(const X &)> Y_of_X) const │ │ │ │ │ -Convert from a DecisionTree to DecisionTree. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:671 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_c_o_m_b_i_n_e │ │ │ │ │ -DecisionTree combine(const LabelC &labelC, const Binary &op) const │ │ │ │ │ -combine with LabelC for convenience │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree.h:368 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_c_r_e_a_t_e │ │ │ │ │ -NodePtr create(It begin, It end, ValueIt beginY, ValueIt endY) const │ │ │ │ │ -Internal recursive function to create from keys, cardinalities, and Y values. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:630 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_~_D_e_c_i_s_i_o_n_T_r_e_e │ │ │ │ │ -virtual ~DecisionTree()=default │ │ │ │ │ -Make virtual. │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_D_e_f_a_u_l_t_C_o_m_p_a_r_e │ │ │ │ │ -static bool DefaultCompare(const Y &a, const Y &b) │ │ │ │ │ -Default method for comparison of two objects of type Y. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_N_o_d_e_P_t_r │ │ │ │ │ -typename Node::Ptr NodePtr │ │ │ │ │ ----------------------— Node base class ------------------------— │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree.h:143 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_l_a_b_e_l_s │ │ │ │ │ -std::set< L > labels() const │ │ │ │ │ -Retrieve all unique labels as a set. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:853 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_e_m_p_t_y │ │ │ │ │ -bool empty() const │ │ │ │ │ -Check if tree is empty. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree.h:257 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_v_i_s_i_t │ │ │ │ │ -void visit(Func f) const │ │ │ │ │ -Visit all leaves in depth-first fashion. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:736 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_v_i_s_i_t_L_e_a_f │ │ │ │ │ -void visitLeaf(Func f) const │ │ │ │ │ -Visit all leaves in depth-first fashion. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:773 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_U_n_a_r_y │ │ │ │ │ -std::function< Y(const Y &)> Unary │ │ │ │ │ -Handy typedefs for unary and binary function types. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_f_o_l_d │ │ │ │ │ -X fold(Func f, X x0) const │ │ │ │ │ -Fold a binary function over the tree, returning accumulator. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:833 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_r_o_o_t__ │ │ │ │ │ -NodePtr root_ │ │ │ │ │ -A DecisionTree just contains the root. TODO(dellaert): make protected. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree.h:146 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s, const LabelFormatter &labelFormatter, const │ │ │ │ │ -ValueFormatter &valueFormatter) const │ │ │ │ │ -GTSAM-style print. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:872 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_c_o_m_b_i_n_e │ │ │ │ │ -DecisionTree combine(const L &label, size_t cardinality, const Binary &op) │ │ │ │ │ -const │ │ │ │ │ -combine subtrees on key with binary operation "op" │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:937 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_v_i_s_i_t_W_i_t_h │ │ │ │ │ -void visitWith(Func f) const │ │ │ │ │ -Visit all leaves in depth-first fashion. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:816 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ -const Y & operator()(const Assignment< L > &x) const │ │ │ │ │ -evaluate │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:884 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_d_o_t │ │ │ │ │ -void dot(std::ostream &os, const LabelFormatter &labelFormatter, const │ │ │ │ │ -ValueFormatter &valueFormatter, bool showZero=true) const │ │ │ │ │ -output to graphviz format, stream version │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:949 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree.h:399 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_o_p_e_r_a_t_o_r_=_= │ │ │ │ │ -bool operator==(const DecisionTree &q) const │ │ │ │ │ -equality │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:879 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_L_a_b_e_l_C │ │ │ │ │ -std::pair< L, size_t > LabelC │ │ │ │ │ -A label annotated with cardinality. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree.h:79 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_n_r_L_e_a_v_e_s │ │ │ │ │ -size_t nrLeaves() const │ │ │ │ │ -Return the number of leaves in the tree. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:823 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_D_e_c_i_s_i_o_n_T_r_e_e │ │ │ │ │ -DecisionTree() │ │ │ │ │ -Default constructor (for serialization) │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree-inl.h:462 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_N_o_d_e │ │ │ │ │ ----------------------— Node base class ------------------------— │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_N_o_d_e_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree.h:135 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r │ │ │ │ │ +A discrete probabilistic factor. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTreeFactor.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r │ │ │ │ │ +Base class for discrete probabilistic factors The most general one is the │ │ │ │ │ +derived DecisionTreeFactor. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactor.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_h_t_m_l │ │ │ │ │ +virtual std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter, │ │ │ │ │ +const Names &names={}) const =0 │ │ │ │ │ +Render as html table. │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_B_a_s_e │ │ │ │ │ +Factor Base │ │ │ │ │ +Our base class. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactor.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="DiscreteFactor\n", const KeyFormatter │ │ │ │ │ +&formatter=DefaultKeyFormatter) const override │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactor.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< DiscreteFactor > shared_ptr │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactor.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_~_D_i_s_c_r_e_t_e_F_a_c_t_o_r │ │ │ │ │ +virtual ~DiscreteFactor() │ │ │ │ │ +Virtual destructor. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactor.h:63 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_N_a_m_e_s │ │ │ │ │ +DiscreteValues::Names Names │ │ │ │ │ +Translation table from values to strings. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactor.h:106 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +virtual bool equals(const DiscreteFactor &lf, double tol=1e-9) const =0 │ │ │ │ │ +equals │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_T_h_i_s │ │ │ │ │ +DiscreteFactor This │ │ │ │ │ +This class. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactor.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ +virtual DecisionTreeFactor operator*(const DecisionTreeFactor &) const =0 │ │ │ │ │ +Multiply in a DecisionTreeFactor and return the result as DecisionTreeFactor. │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ +virtual double operator()(const DiscreteValues &) const =0 │ │ │ │ │ +Find value for given assignment of values to variables. │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_m_a_r_k_d_o_w_n │ │ │ │ │ +virtual std::string markdown(const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter, const Names &names={}) const =0 │ │ │ │ │ +Render as markdown table. │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r │ │ │ │ │ +DiscreteFactor(const CONTAINER &keys) │ │ │ │ │ +Construct from container of keys. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactor.h:60 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r │ │ │ │ │ +DiscreteFactor() │ │ │ │ │ +Default constructor creates empty factor. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactor.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ +A map from keys to values. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteValues.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_N_a_m_e_s │ │ │ │ │ +std::map< Key, std::vector< std::string > > Names │ │ │ │ │ +Translation table from values to strings. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteValues.h:95 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:68 │ │ │ │ │ +_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +the error. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_e_c_i_s_i_o_n_T_r_e_e_._h │ │ │ │ │ + * _D_i_s_c_r_e_t_e_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00224.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteJunctionTree.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteMarginals.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,34 +94,49 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
DiscreteJunctionTree.cpp File Reference
│ │ │ │ +
DiscreteMarginals.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ + │ │ │ │ +

A class for computing marginals in a DiscreteFactorGraph. │ │ │ │ +More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::DiscreteMarginals
 A class for computing marginals of variables in a DiscreteFactorGraph. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Mar 29, 2013
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +

A class for computing marginals in a DiscreteFactorGraph.

│ │ │ │ +
Author
Abhijit Kundu
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │ -Richard Roberts
│ │ │ │ +Frank Dellaert
│ │ │ │ +
Date
June 4, 2012
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,30 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -DiscreteJunctionTree.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +DiscreteMarginals.h File Reference │ │ │ │ │ +A class for computing marginals in a DiscreteFactorGraph. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_M_a_r_g_i_n_a_l_s │ │ │ │ │ +  A class for computing marginals of variables in a _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Mar 29, 2013 │ │ │ │ │ +A class for computing marginals in a DiscreteFactorGraph. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Abhijit Kundu │ │ │ │ │ Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + June 4, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_J_u_n_c_t_i_o_n_T_r_e_e_._c_p_p │ │ │ │ │ + * _D_i_s_c_r_e_t_e_M_a_r_g_i_n_a_l_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00227.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/AlgebraicDecisionTree.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteBayesNet.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,50 +94,34 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
AlgebraicDecisionTree.h File Reference
│ │ │ │ +
DiscreteBayesNet.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -

Algebraic Decision Trees. │ │ │ │ -More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::AlgebraicDecisionTree< L >
 An algebraic decision tree fixes the range of a DecisionTree to double. More...
 
struct  gtsam::AlgebraicDecisionTree< L >::Ring
 The Real ring with addition and multiplication. More...
 
struct  gtsam::traits< AlgebraicDecisionTree< T > >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Algebraic Decision Trees.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
Date
Mar 14, 2011
│ │ │ │ +
Date
Feb 15, 2011
│ │ │ │ +
Author
Duy-Nguyen Ta
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,33 +1,21 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -AlgebraicDecisionTree.h File Reference │ │ │ │ │ -Algebraic Decision Trees. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_<_ _L_ _> │ │ │ │ │ -  An algebraic decision tree fixes the range of a _D_e_c_i_s_i_o_n_T_r_e_e to │ │ │ │ │ - double. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_<_ _L_ _>_:_:_R_i_n_g │ │ │ │ │ -  The Real ring with addition and multiplication. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_<_ _T_ _>_ _> │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +DiscreteBayesNet.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Algebraic Decision Trees. │ │ │ │ │ + Date │ │ │ │ │ + Feb 15, 2011 │ │ │ │ │ Author │ │ │ │ │ + Duy-Nguyen Ta │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - Mar 14, 2011 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_._h │ │ │ │ │ + * _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00230.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteFactorGraph.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteFactorGraph.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,58 +94,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ Functions
│ │ │ │ -
DiscreteFactorGraph.h File Reference
│ │ │ │ +
DiscreteFactorGraph.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

struct  gtsam::EliminationTraits< DiscreteFactorGraph >
 
class  gtsam::DiscreteFactorGraph
 A Discrete Factor Graph is a factor graph where all factors are Discrete, i.e. More...
 
struct  gtsam::traits< DiscreteFactorGraph >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

std::pair< DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr > gtsam::EliminateDiscrete (const DiscreteFactorGraph &factors, const Ordering &frontalKeys)
 Main elimination function for DiscreteFactorGraph.
 
std::pair< DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr > gtsam::EliminateForMPE (const DiscreteFactorGraph &factors, const Ordering &frontalKeys)
 Alternate elimination function for that creates non-normalized lookup tables.
 
std::pair< DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr > gtsam::EliminateDiscrete (const DiscreteFactorGraph &factors, const Ordering &frontalKeys)
 Main elimination function for DiscreteFactorGraph.
 
│ │ │ │

Detailed Description

│ │ │ │
Date
Feb 14, 2011
│ │ │ │
Author
Duy-Nguyen Ta
│ │ │ │
│ │ │ │ Frank Dellaert
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,45 +1,34 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -DiscreteFactorGraph.h File Reference │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_ _> │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -  A Discrete _F_a_c_t_o_r Graph is a factor graph where all factors are │ │ │ │ │ - Discrete, i.e. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +DiscreteFactorGraph.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -std::pair< _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_: _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_D_i_s_c_r_e_t_e (const │ │ │ │ │ -_s_h_a_r_e_d___p_t_r, DecisionTreeFactor:: _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h &factors, const │ │ │ │ │ - shared_ptr >  _O_r_d_e_r_i_n_g &frontalKeys) │ │ │ │ │ -  Main elimination function for │ │ │ │ │ - _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h. │ │ │ │ │ -  │ │ │ │ │ std::pair< _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_: _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_F_o_r_M_P_E (const │ │ │ │ │ _s_h_a_r_e_d___p_t_r, DecisionTreeFactor:: _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h &factors, const │ │ │ │ │ shared_ptr >  _O_r_d_e_r_i_n_g &frontalKeys) │ │ │ │ │   Alternate elimination function for that │ │ │ │ │ creates non-normalized lookup tables. │ │ │ │ │   │ │ │ │ │ +std::pair< _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_: _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_D_i_s_c_r_e_t_e (const │ │ │ │ │ +_s_h_a_r_e_d___p_t_r, DecisionTreeFactor:: _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h &factors, const │ │ │ │ │ + shared_ptr >  _O_r_d_e_r_i_n_g &frontalKeys) │ │ │ │ │ +  Main elimination function for │ │ │ │ │ + _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Date │ │ │ │ │ Feb 14, 2011 │ │ │ │ │ Author │ │ │ │ │ Duy-Nguyen Ta │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ + * _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00230.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,6 +1,4 @@ │ │ │ │ │ var a00230 = [ │ │ │ │ │ - ["gtsam::EliminationTraits< DiscreteFactorGraph >", "a02860.html", "a02860"], │ │ │ │ │ - ["gtsam::traits< DiscreteFactorGraph >", "a02868.html", null], │ │ │ │ │ ["EliminateDiscrete", "a00230.html#ga6996e9c84467350b071645649231223a", null], │ │ │ │ │ ["EliminateForMPE", "a00230.html#ga9bd0b2859a7513198a369f5de3c8cde3", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00233.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteBayesNet.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteValues.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,44 +95,58 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
DiscreteBayesNet.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
DiscreteValues.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::DiscreteBayesNet
 A Bayes net made from discrete conditional distributions. More...
class  gtsam::DiscreteValues
 A map from keys to values. More...
 
struct  gtsam::traits< DiscreteBayesNet >
struct  gtsam::traits< DiscreteValues >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +std::vector< DiscreteValues > gtsam::cartesianProduct (const DiscreteKeys &keys)
 Free version of CartesianProduct.
 
│ │ │ │ +string gtsam::markdown (const DiscreteValues &values, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DiscreteValues::Names &names={})
 Free version of markdown.
 
│ │ │ │ +string gtsam::html (const DiscreteValues &values, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DiscreteValues::Names &names={})
 Free version of html.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Feb 15, 2011
│ │ │ │ -
Author
Duy-Nguyen Ta
│ │ │ │ -
│ │ │ │ -Frank dellaert
│ │ │ │ +
Date
Dec 13, 2021
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,43 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -DiscreteBayesNet.h File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +DiscreteValues.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t │ │ │ │ │ -  A Bayes net made from discrete conditional distributions. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ +  A map from keys to values. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_V_a_l_u_e_s_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +std::vector< _D_i_s_c_r_e_t_e_V_a_l_u_e_s >  ggttssaamm::::ccaarrtteessiiaannPPrroodduucctt (const _D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ + &keys) │ │ │ │ │ +  Free version of CartesianProduct. │ │ │ │ │ +  │ │ │ │ │ + string  ggttssaamm::::mmaarrkkddoowwnn (const _D_i_s_c_r_e_t_e_V_a_l_u_e_s &values, │ │ │ │ │ + const _K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ + &keyFormatter=DefaultKeyFormatter, const │ │ │ │ │ + _D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_N_a_m_e_s &names={}) │ │ │ │ │ +  Free version of markdown. │ │ │ │ │ +  │ │ │ │ │ + string  ggttssaamm::::hhttmmll (const _D_i_s_c_r_e_t_e_V_a_l_u_e_s &values, const │ │ │ │ │ + _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter=DefaultKeyFormatter, │ │ │ │ │ + const _D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_N_a_m_e_s &names={}) │ │ │ │ │ +  Free version of html. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Date │ │ │ │ │ - Feb 15, 2011 │ │ │ │ │ + Dec 13, 2021 │ │ │ │ │ Author │ │ │ │ │ - Duy-Nguyen Ta │ │ │ │ │ - Frank dellaert │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_._h │ │ │ │ │ + * _D_i_s_c_r_e_t_e_V_a_l_u_e_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00233.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,6 @@ │ │ │ │ │ var a00233 = [ │ │ │ │ │ - ["gtsam::traits< DiscreteBayesNet >", "a02820.html", null] │ │ │ │ │ + ["gtsam::traits< DiscreteValues >", "a02904.html", null], │ │ │ │ │ + ["cartesianProduct", "a00233.html#a55a86a4657d82f6ff048a10d058c529f", null], │ │ │ │ │ + ["html", "a00233.html#a147114735037c2260d4810a8820e72f6", null], │ │ │ │ │ + ["markdown", "a00233.html#a2b6707b04064df79d26d1016dfa2a013", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00233_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteBayesNet.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteValues.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,173 +98,136 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
DiscreteBayesNet.h
│ │ │ │ +
DiscreteValues.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/discrete/DiscreteConditional.h>
│ │ │ │ -
22#include <gtsam/discrete/DiscreteDistribution.h>
│ │ │ │ -
23#include <gtsam/inference/BayesNet.h>
│ │ │ │ -
24#include <gtsam/inference/FactorGraph.h>
│ │ │ │ -
25
│ │ │ │ -
26#include <boost/shared_ptr.hpp>
│ │ │ │ -
27#include <map>
│ │ │ │ -
28#include <string>
│ │ │ │ -
29#include <utility>
│ │ │ │ -
30#include <vector>
│ │ │ │ -
31
│ │ │ │ -
32namespace gtsam {
│ │ │ │ -
33
│ │ │ │ -
│ │ │ │ -
38class GTSAM_EXPORT DiscreteBayesNet: public BayesNet<DiscreteConditional> {
│ │ │ │ -
39 public:
│ │ │ │ -
40 typedef BayesNet<DiscreteConditional> Base;
│ │ │ │ -
41 typedef DiscreteBayesNet This;
│ │ │ │ -
42 typedef DiscreteConditional ConditionalType;
│ │ │ │ -
43 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
44 typedef boost::shared_ptr<ConditionalType> sharedConditional;
│ │ │ │ -
45
│ │ │ │ -
48
│ │ │ │ -
50 DiscreteBayesNet() {}
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/discrete/Assignment.h>
│ │ │ │ +
21#include <gtsam/discrete/DiscreteKey.h>
│ │ │ │ +
22#include <gtsam/inference/Key.h>
│ │ │ │ +
23
│ │ │ │ +
24#include <map>
│ │ │ │ +
25#include <string>
│ │ │ │ +
26#include <vector>
│ │ │ │ +
27
│ │ │ │ +
28namespace gtsam {
│ │ │ │ +
29
│ │ │ │ +
│ │ │ │ +
34class GTSAM_EXPORT DiscreteValues : public Assignment<Key> {
│ │ │ │ +
35 public:
│ │ │ │ +
36 using Base = Assignment<Key>; // base class
│ │ │ │ +
37
│ │ │ │ +
40 using Assignment::Assignment; // all constructors
│ │ │ │ +
41
│ │ │ │ +
42 // Define the implicit default constructor.
│ │ │ │ +
43 DiscreteValues() = default;
│ │ │ │ +
44
│ │ │ │ +
45 // Construct from assignment.
│ │ │ │ +
46 explicit DiscreteValues(const Base& a) : Base(a) {}
│ │ │ │ +
47
│ │ │ │ +
48 // Construct from initializer list.
│ │ │ │ +
49 DiscreteValues(std::initializer_list<std::pair<const Key, size_t>> init)
│ │ │ │ +
50 : Assignment<Key>{init} {}
│ │ │ │
51
│ │ │ │ -
53 template <typename ITERATOR>
│ │ │ │ -
│ │ │ │ -
54 DiscreteBayesNet(ITERATOR firstConditional, ITERATOR lastConditional)
│ │ │ │ -
55 : Base(firstConditional, lastConditional) {}
│ │ │ │ -
│ │ │ │ -
56
│ │ │ │ -
58 template <class CONTAINER>
│ │ │ │ -
│ │ │ │ -
59 explicit DiscreteBayesNet(const CONTAINER& conditionals)
│ │ │ │ -
60 : Base(conditionals) {}
│ │ │ │ -
│ │ │ │ -
61
│ │ │ │ -
64 template <class DERIVEDCONDITIONAL>
│ │ │ │ -
│ │ │ │ -
65 DiscreteBayesNet(const FactorGraph<DERIVEDCONDITIONAL>& graph)
│ │ │ │ -
66 : Base(graph) {}
│ │ │ │ -
│ │ │ │ -
67
│ │ │ │ -
69 virtual ~DiscreteBayesNet() {}
│ │ │ │ -
70
│ │ │ │ -
72
│ │ │ │ +
55
│ │ │ │ +
57 void print(const std::string& s = "",
│ │ │ │ +
58 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ +
59
│ │ │ │ +
61 bool equals(const DiscreteValues& x, double tol = 1e-9) const;
│ │ │ │ +
62
│ │ │ │ +
66
│ │ │ │ +
67 // insert in base class;
│ │ │ │ +
68 std::pair<iterator, bool> insert( const value_type& value ){
│ │ │ │ +
69 return Base::insert(value);
│ │ │ │ +
70 }
│ │ │ │ +
71
│ │ │ │ +
74 DiscreteValues& insert(const DiscreteValues& values);
│ │ │ │
75
│ │ │ │ -
77 bool equals(const This& bn, double tol = 1e-9) const;
│ │ │ │ -
78
│ │ │ │ +
79 DiscreteValues& update(const DiscreteValues& values);
│ │ │ │
80
│ │ │ │ -
83
│ │ │ │ -
84 // Add inherited versions of add.
│ │ │ │ -
85 using Base::add;
│ │ │ │ -
86
│ │ │ │ -
│ │ │ │ -
88 void add(const DiscreteKey& key, const std::string& spec) {
│ │ │ │ -
89 emplace_shared<DiscreteDistribution>(key, spec);
│ │ │ │ -
90 }
│ │ │ │ -
│ │ │ │ -
91
│ │ │ │ -
93 template <typename... Args>
│ │ │ │ -
│ │ │ │ -
94 void add(Args&&... args) {
│ │ │ │ -
95 emplace_shared<DiscreteConditional>(std::forward<Args>(args)...);
│ │ │ │ -
96 }
│ │ │ │ -
│ │ │ │ -
97
│ │ │ │ -
98 //** evaluate for given DiscreteValues */
│ │ │ │ -
99 double evaluate(const DiscreteValues & values) const;
│ │ │ │ -
100
│ │ │ │ -
101 //** (Preferred) sugar for the above for given DiscreteValues */
│ │ │ │ -
102 double operator()(const DiscreteValues & values) const {
│ │ │ │ -
103 return evaluate(values);
│ │ │ │ -
104 }
│ │ │ │ -
105
│ │ │ │ -
106 //** log(evaluate(values)) for given DiscreteValues */
│ │ │ │ -
107 double logProbability(const DiscreteValues & values) const;
│ │ │ │ -
108
│ │ │ │ -
118 DiscreteValues sample() const;
│ │ │ │ +
│ │ │ │ +
85 static std::vector<DiscreteValues> CartesianProduct(
│ │ │ │ +
86 const DiscreteKeys& keys) {
│ │ │ │ +
87 return Base::CartesianProduct<DiscreteValues>(keys);
│ │ │ │ +
88 }
│ │ │ │ +
│ │ │ │ +
89
│ │ │ │ +
93
│ │ │ │ +
95 using Names = std::map<Key, std::vector<std::string>>;
│ │ │ │ +
96
│ │ │ │ +
98 static std::string Translate(const Names& names, Key key, size_t index);
│ │ │ │ +
99
│ │ │ │ +
107 std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
108 const Names& names = {}) const;
│ │ │ │ +
109
│ │ │ │ +
117 std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
118 const Names& names = {}) const;
│ │ │ │
119
│ │ │ │ -
128 DiscreteValues sample(DiscreteValues given) const;
│ │ │ │ -
129
│ │ │ │ -
133
│ │ │ │ -
135 std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
136 const DiscreteFactor::Names& names = {}) const;
│ │ │ │ +
121};
│ │ │ │ +
│ │ │ │ +
122
│ │ │ │ +
│ │ │ │ +
124inline std::vector<DiscreteValues> cartesianProduct(const DiscreteKeys& keys) {
│ │ │ │ +
125 return DiscreteValues::CartesianProduct(keys);
│ │ │ │ +
126}
│ │ │ │ +
│ │ │ │ +
127
│ │ │ │ +
129std::string markdown(const DiscreteValues& values,
│ │ │ │ +
130 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
131 const DiscreteValues::Names& names = {});
│ │ │ │ +
132
│ │ │ │ +
134std::string html(const DiscreteValues& values,
│ │ │ │ +
135 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
136 const DiscreteValues::Names& names = {});
│ │ │ │
137
│ │ │ │ -
139 std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
140 const DiscreteFactor::Names& names = {}) const;
│ │ │ │ +
138// traits
│ │ │ │ +
139template <>
│ │ │ │ +
140struct traits<DiscreteValues> : public Testable<DiscreteValues> {};
│ │ │ │
141
│ │ │ │ -
145
│ │ │ │ -
146 using Base::error; // Expose error(const HybridValues&) method..
│ │ │ │ -
147 using Base::evaluate; // Expose evaluate(const HybridValues&) method..
│ │ │ │ -
148 using Base::logProbability; // Expose logProbability(const HybridValues&)
│ │ │ │ -
149
│ │ │ │ -
151
│ │ │ │ -
152#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ -
155
│ │ │ │ -
156 DiscreteValues GTSAM_DEPRECATED optimize() const;
│ │ │ │ -
157 DiscreteValues GTSAM_DEPRECATED optimize(DiscreteValues given) const;
│ │ │ │ -
159#endif
│ │ │ │ -
160
│ │ │ │ -
161 private:
│ │ │ │ -
163 friend class boost::serialization::access;
│ │ │ │ -
164 template<class ARCHIVE>
│ │ │ │ -
165 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
166 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
167 }
│ │ │ │ -
168 };
│ │ │ │ -
│ │ │ │ -
169
│ │ │ │ -
170// traits
│ │ │ │ -
171template<> struct traits<DiscreteBayesNet> : public Testable<DiscreteBayesNet> {};
│ │ │ │ -
172
│ │ │ │ -
173} // \ namespace gtsam
│ │ │ │ -
174
│ │ │ │ - │ │ │ │ - │ │ │ │ -
FactorGraph.h
Factor Graph Base Class.
│ │ │ │ -
BayesNet.h
Bayes network.
│ │ │ │ -
gtsam::DiscreteKey
std::pair< Key, size_t > DiscreteKey
Key type for discrete variables.
Definition DiscreteKey.h:36
│ │ │ │ +
142} // namespace gtsam
│ │ │ │ +
Assignment.h
An assignment from labels to a discrete value index (size_t)
│ │ │ │ +
DiscreteKey.h
specialized key for discrete variables
│ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::html
string html(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
Free version of html.
Definition DiscreteValues.cpp:134
│ │ │ │ -
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
│ │ │ │ +
gtsam::markdown
string markdown(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
Free version of markdown.
Definition DiscreteValues.cpp:129
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::cartesianProduct
std::vector< DiscreteValues > cartesianProduct(const DiscreteKeys &keys)
Free version of CartesianProduct.
Definition DiscreteValues.h:124
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::DiscreteBayesNet
A Bayes net made from discrete conditional distributions.
Definition DiscreteBayesNet.h:38
│ │ │ │ -
gtsam::DiscreteBayesNet::DiscreteBayesNet
DiscreteBayesNet(ITERATOR firstConditional, ITERATOR lastConditional)
Construct from iterator over conditionals.
Definition DiscreteBayesNet.h:54
│ │ │ │ -
gtsam::DiscreteBayesNet::DiscreteBayesNet
DiscreteBayesNet(const FactorGraph< DERIVEDCONDITIONAL > &graph)
Implicit copy/downcast constructor to override explicit template container constructor.
Definition DiscreteBayesNet.h:65
│ │ │ │ -
gtsam::DiscreteBayesNet::DiscreteBayesNet
DiscreteBayesNet(const CONTAINER &conditionals)
Construct from container of factors (shared_ptr or plain objects)
Definition DiscreteBayesNet.h:59
│ │ │ │ -
gtsam::DiscreteBayesNet::DiscreteBayesNet
DiscreteBayesNet()
Construct empty Bayes net.
Definition DiscreteBayesNet.h:50
│ │ │ │ -
gtsam::DiscreteBayesNet::~DiscreteBayesNet
virtual ~DiscreteBayesNet()
Destructor.
Definition DiscreteBayesNet.h:69
│ │ │ │ -
gtsam::DiscreteBayesNet::add
void add(Args &&... args)
Add a DiscreteCondtional.
Definition DiscreteBayesNet.h:94
│ │ │ │ -
gtsam::DiscreteBayesNet::add
void add(const DiscreteKey &key, const std::string &spec)
Add a DiscreteDistribution using a table or a string.
Definition DiscreteBayesNet.h:88
│ │ │ │ -
gtsam::DiscreteConditional
Discrete Conditional Density Derives from DecisionTreeFactor.
Definition DiscreteConditional.h:40
│ │ │ │ +
gtsam::Assignment
An assignment from labels to value index (size_t).
Definition Assignment.h:37
│ │ │ │ +
gtsam::DiscreteKeys
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition DiscreteKey.h:39
│ │ │ │
gtsam::DiscreteValues
A map from keys to values.
Definition DiscreteValues.h:34
│ │ │ │ -
gtsam::BayesNet
A BayesNet is a tree of conditionals, stored in elimination order.
Definition BayesNet.h:35
│ │ │ │ -
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ +
gtsam::DiscreteValues::Names
std::map< Key, std::vector< std::string > > Names
Translation table from values to strings.
Definition DiscreteValues.h:95
│ │ │ │ +
gtsam::DiscreteValues::CartesianProduct
static std::vector< DiscreteValues > CartesianProduct(const DiscreteKeys &keys)
Return a vector of DiscreteValues, one for each possible combination of values.
Definition DiscreteValues.h:85
│ │ │ │
DiscreteValues
The Factor::error simply extracts the.
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,211 +1,168 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DiscreteBayesNet.h │ │ │ │ │ +DiscreteValues.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -25 │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ -28#include │ │ │ │ │ -29#include │ │ │ │ │ -30#include │ │ │ │ │ -31 │ │ │ │ │ -32namespace _g_t_s_a_m { │ │ │ │ │ -33 │ │ │ │ │ -_3_8class GTSAM_EXPORT _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t: public _B_a_y_e_s_N_e_t { │ │ │ │ │ -39 public: │ │ │ │ │ -40 typedef _B_a_y_e_s_N_e_t_<_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_> _B_a_s_e; │ │ │ │ │ -41 typedef _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t _T_h_i_s; │ │ │ │ │ -42 typedef _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l _C_o_n_d_i_t_i_o_n_a_l_T_y_p_e; │ │ │ │ │ -43 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -44 typedef boost::shared_ptr sharedConditional; │ │ │ │ │ -45 │ │ │ │ │ -48 │ │ │ │ │ -_5_0 _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t() {} │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_A_s_s_i_g_n_m_e_n_t_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_K_e_y_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ +23 │ │ │ │ │ +24#include │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ +27 │ │ │ │ │ +28namespace _g_t_s_a_m { │ │ │ │ │ +29 │ │ │ │ │ +_3_4class GTSAM_EXPORT _D_i_s_c_r_e_t_e_V_a_l_u_e_s : public _A_s_s_i_g_n_m_e_n_t { │ │ │ │ │ +35 public: │ │ │ │ │ +36 using _B_a_s_e = _A_s_s_i_g_n_m_e_n_t_<_K_e_y_>; // base class │ │ │ │ │ +37 │ │ │ │ │ +40 using Assignment::Assignment; // all constructors │ │ │ │ │ +41 │ │ │ │ │ +42 // Define the implicit default constructor. │ │ │ │ │ +43 _D_i_s_c_r_e_t_e_V_a_l_u_e_s() = default; │ │ │ │ │ +44 │ │ │ │ │ +45 // Construct from assignment. │ │ │ │ │ +46 explicit _D_i_s_c_r_e_t_e_V_a_l_u_e_s(const _B_a_s_e& a) : _B_a_s_e(a) {} │ │ │ │ │ +47 │ │ │ │ │ +48 // Construct from initializer list. │ │ │ │ │ +49 _D_i_s_c_r_e_t_e_V_a_l_u_e_s(std::initializer_list> init) │ │ │ │ │ +50 : _A_s_s_i_g_n_m_e_n_t_<_K_e_y_>{init} {} │ │ │ │ │ 51 │ │ │ │ │ -53 template │ │ │ │ │ -_5_4 _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t(ITERATOR firstConditional, ITERATOR lastConditional) │ │ │ │ │ -55 : _B_a_s_e(firstConditional, lastConditional) {} │ │ │ │ │ -56 │ │ │ │ │ -58 template │ │ │ │ │ -_5_9 explicit _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t(const CONTAINER& conditionals) │ │ │ │ │ -60 : _B_a_s_e(conditionals) {} │ │ │ │ │ -61 │ │ │ │ │ -64 template │ │ │ │ │ -_6_5 _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t(const _F_a_c_t_o_r_G_r_a_p_h_<_D_E_R_I_V_E_D_C_O_N_D_I_T_I_O_N_A_L_>& graph) │ │ │ │ │ -66 : _B_a_s_e(graph) {} │ │ │ │ │ -67 │ │ │ │ │ -_6_9 virtual _~_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t() {} │ │ │ │ │ -70 │ │ │ │ │ -72 │ │ │ │ │ +55 │ │ │ │ │ +57 void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ +58 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const; │ │ │ │ │ +59 │ │ │ │ │ +61 bool _e_q_u_a_l_s(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& x, double tol = 1e-9) const; │ │ │ │ │ +62 │ │ │ │ │ +66 │ │ │ │ │ +67 // insert in base class; │ │ │ │ │ +68 std::pair insert( const value_type& value ){ │ │ │ │ │ +69 return Base::insert(value); │ │ │ │ │ +70 } │ │ │ │ │ +71 │ │ │ │ │ +74 _D_i_s_c_r_e_t_e_V_a_l_u_e_s& insert(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values); │ │ │ │ │ 75 │ │ │ │ │ -77 bool _e_q_u_a_l_s(const This& bn, double tol = 1e-9) const; │ │ │ │ │ -78 │ │ │ │ │ +79 _D_i_s_c_r_e_t_e_V_a_l_u_e_s& update(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values); │ │ │ │ │ 80 │ │ │ │ │ -83 │ │ │ │ │ -84 // Add inherited versions of add. │ │ │ │ │ -85 using Base::add; │ │ │ │ │ -86 │ │ │ │ │ -_8_8 void _a_d_d(const _D_i_s_c_r_e_t_e_K_e_y& key, const std::string& spec) { │ │ │ │ │ -89 emplace_shared(key, spec); │ │ │ │ │ -90 } │ │ │ │ │ -91 │ │ │ │ │ -93 template │ │ │ │ │ -_9_4 void _a_d_d(Args&&... args) { │ │ │ │ │ -95 emplace_shared(std::forward(args)...); │ │ │ │ │ -96 } │ │ │ │ │ -97 │ │ │ │ │ -98 //** evaluate for given DiscreteValues */ │ │ │ │ │ -99 double evaluate(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s & values) const; │ │ │ │ │ -100 │ │ │ │ │ -101 //** (Preferred) sugar for the above for given DiscreteValues */ │ │ │ │ │ -102 double operator()(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s & values) const { │ │ │ │ │ -103 return evaluate(values); │ │ │ │ │ -104 } │ │ │ │ │ -105 │ │ │ │ │ -106 //** log(evaluate(values)) for given DiscreteValues */ │ │ │ │ │ -107 double logProbability(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s & values) const; │ │ │ │ │ -108 │ │ │ │ │ -118 _D_i_s_c_r_e_t_e_V_a_l_u_e_s sample() const; │ │ │ │ │ -119 │ │ │ │ │ -128 _D_i_s_c_r_e_t_e_V_a_l_u_e_s sample(_D_i_s_c_r_e_t_e_V_a_l_u_e_s given) const; │ │ │ │ │ -129 │ │ │ │ │ -133 │ │ │ │ │ -135 std::string markdown(const KeyFormatter& keyFormatter = │ │ │ │ │ +_8_5 static std::vector _C_a_r_t_e_s_i_a_n_P_r_o_d_u_c_t( │ │ │ │ │ +86 const _D_i_s_c_r_e_t_e_K_e_y_s& keys) { │ │ │ │ │ +87 return Base::CartesianProduct(keys); │ │ │ │ │ +88 } │ │ │ │ │ +89 │ │ │ │ │ +93 │ │ │ │ │ +_9_5 using _N_a_m_e_s = std::map>; │ │ │ │ │ +96 │ │ │ │ │ +98 static std::string Translate(const _N_a_m_e_s& names, _K_e_y key, size_t index); │ │ │ │ │ +99 │ │ │ │ │ +107 std::string _m_a_r_k_d_o_w_n(const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ DefaultKeyFormatter, │ │ │ │ │ -136 const DiscreteFactor::Names& names = {}) const; │ │ │ │ │ +108 const _N_a_m_e_s& names = {}) const; │ │ │ │ │ +109 │ │ │ │ │ +117 std::string _h_t_m_l(const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ +118 const Names& names = {}) const; │ │ │ │ │ +119 │ │ │ │ │ +121}; │ │ │ │ │ +122 │ │ │ │ │ +_1_2_4inline std::vector _c_a_r_t_e_s_i_a_n_P_r_o_d_u_c_t(const _D_i_s_c_r_e_t_e_K_e_y_s& │ │ │ │ │ +keys) { │ │ │ │ │ +125 return _D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_C_a_r_t_e_s_i_a_n_P_r_o_d_u_c_t(keys); │ │ │ │ │ +126} │ │ │ │ │ +127 │ │ │ │ │ +129std::string _m_a_r_k_d_o_w_n(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values, │ │ │ │ │ +130 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ +131 const _D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_N_a_m_e_s& names = {}); │ │ │ │ │ +132 │ │ │ │ │ +134std::string _h_t_m_l(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values, │ │ │ │ │ +135 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ +136 const _D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_N_a_m_e_s& names = {}); │ │ │ │ │ 137 │ │ │ │ │ -139 std::string _h_t_m_l(const KeyFormatter& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ -140 const DiscreteFactor::Names& names = {}) const; │ │ │ │ │ +138// traits │ │ │ │ │ +139template <> │ │ │ │ │ +_1_4_0struct _t_r_a_i_t_s<_D_i_s_c_r_e_t_e_V_a_l_u_e_s> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ 141 │ │ │ │ │ -145 │ │ │ │ │ -146 using Base::error; // Expose error(const HybridValues&) method.. │ │ │ │ │ -147 using Base::evaluate; // Expose evaluate(const HybridValues&) method.. │ │ │ │ │ -148 using Base::logProbability; // Expose logProbability(const HybridValues&) │ │ │ │ │ -149 │ │ │ │ │ -151 │ │ │ │ │ -152#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ -155 │ │ │ │ │ -156 _D_i_s_c_r_e_t_e_V_a_l_u_e_s GTSAM_DEPRECATED _o_p_t_i_m_i_z_e() const; │ │ │ │ │ -157 _D_i_s_c_r_e_t_e_V_a_l_u_e_s GTSAM_DEPRECATED _o_p_t_i_m_i_z_e(_D_i_s_c_r_e_t_e_V_a_l_u_e_s given) const; │ │ │ │ │ -159#endif │ │ │ │ │ -160 │ │ │ │ │ -161 private: │ │ │ │ │ -_1_6_3 friend class boost::serialization::access; │ │ │ │ │ -164 template │ │ │ │ │ -165 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -166 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -167 } │ │ │ │ │ -168 }; │ │ │ │ │ -169 │ │ │ │ │ -170// traits │ │ │ │ │ -_1_7_1template<> struct _t_r_a_i_t_s<_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t> : public │ │ │ │ │ -_T_e_s_t_a_b_l_e {}; │ │ │ │ │ -172 │ │ │ │ │ -173} // \ namespace gtsam │ │ │ │ │ -174 │ │ │ │ │ -_D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n_._h │ │ │ │ │ -_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ -_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Factor Graph Base Class. │ │ │ │ │ -_B_a_y_e_s_N_e_t_._h │ │ │ │ │ -Bayes network. │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y │ │ │ │ │ -std::pair< Key, size_t > DiscreteKey │ │ │ │ │ -Key type for discrete variables. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteKey.h:36 │ │ │ │ │ +142} // namespace gtsam │ │ │ │ │ +_A_s_s_i_g_n_m_e_n_t_._h │ │ │ │ │ +An assignment from labels to a discrete value index (size_t) │ │ │ │ │ +_D_i_s_c_r_e_t_e_K_e_y_._h │ │ │ │ │ +specialized key for discrete variables │ │ │ │ │ +_K_e_y_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_h_t_m_l │ │ │ │ │ string html(const DiscreteValues &values, const KeyFormatter &keyFormatter, │ │ │ │ │ const DiscreteValues::Names &names) │ │ │ │ │ Free version of html. │ │ │ │ │ DDeeffiinniittiioonn DiscreteValues.cpp:134 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ -Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key │ │ │ │ │ -landmarkKey) │ │ │ │ │ -Optimize for triangulation. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.cpp:155 │ │ │ │ │ +_g_t_s_a_m_:_:_m_a_r_k_d_o_w_n │ │ │ │ │ +string markdown(const DiscreteValues &values, const KeyFormatter &keyFormatter, │ │ │ │ │ +const DiscreteValues::Names &names) │ │ │ │ │ +Free version of markdown. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteValues.cpp:129 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_c_a_r_t_e_s_i_a_n_P_r_o_d_u_c_t │ │ │ │ │ +std::vector< DiscreteValues > cartesianProduct(const DiscreteKeys &keys) │ │ │ │ │ +Free version of CartesianProduct. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteValues.h:124 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ _g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ Template to create a binary predicate. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ _g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ A helper that implements the traits interface for GTSAM types. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t │ │ │ │ │ -A Bayes net made from discrete conditional distributions. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteBayesNet.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t │ │ │ │ │ -DiscreteBayesNet(ITERATOR firstConditional, ITERATOR lastConditional) │ │ │ │ │ -Construct from iterator over conditionals. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteBayesNet.h:54 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t │ │ │ │ │ -DiscreteBayesNet(const FactorGraph< DERIVEDCONDITIONAL > &graph) │ │ │ │ │ -Implicit copy/downcast constructor to override explicit template container │ │ │ │ │ -constructor. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteBayesNet.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t │ │ │ │ │ -DiscreteBayesNet(const CONTAINER &conditionals) │ │ │ │ │ -Construct from container of factors (shared_ptr or plain objects) │ │ │ │ │ -DDeeffiinniittiioonn DiscreteBayesNet.h:59 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t │ │ │ │ │ -DiscreteBayesNet() │ │ │ │ │ -Construct empty Bayes net. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteBayesNet.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_:_:_~_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t │ │ │ │ │ -virtual ~DiscreteBayesNet() │ │ │ │ │ -Destructor. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteBayesNet.h:69 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_:_:_a_d_d │ │ │ │ │ -void add(Args &&... args) │ │ │ │ │ -Add a DiscreteCondtional. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteBayesNet.h:94 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_:_:_a_d_d │ │ │ │ │ -void add(const DiscreteKey &key, const std::string &spec) │ │ │ │ │ -Add a DiscreteDistribution using a table or a string. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteBayesNet.h:88 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -Discrete Conditional Density Derives from DecisionTreeFactor. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteConditional.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_A_s_s_i_g_n_m_e_n_t │ │ │ │ │ +An assignment from labels to value index (size_t). │ │ │ │ │ +DDeeffiinniittiioonn Assignment.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ +DiscreteKeys is a set of keys that can be assembled using the & operator. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteKey.h:39 │ │ │ │ │ _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ A map from keys to values. │ │ │ │ │ DDeeffiinniittiioonn DiscreteValues.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t │ │ │ │ │ -A BayesNet is a tree of conditionals, stored in elimination order. │ │ │ │ │ -DDeeffiinniittiioonn BayesNet.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ -nodes. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_N_a_m_e_s │ │ │ │ │ +std::map< Key, std::vector< std::string > > Names │ │ │ │ │ +Translation table from values to strings. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteValues.h:95 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_C_a_r_t_e_s_i_a_n_P_r_o_d_u_c_t │ │ │ │ │ +static std::vector< DiscreteValues > CartesianProduct(const DiscreteKeys &keys) │ │ │ │ │ +Return a vector of DiscreteValues, one for each possible combination of values. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteValues.h:85 │ │ │ │ │ _D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ The Factor::error simply extracts the. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_._h │ │ │ │ │ + * _D_i_s_c_r_e_t_e_V_a_l_u_e_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00236.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/Signature.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteFactorGraph.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -97,58 +97,55 @@ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ Functions
│ │ │ │ -
Signature.h File Reference
│ │ │ │ +
DiscreteFactorGraph.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

signatures for conditional densities │ │ │ │ -More...

│ │ │ │ - │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::Signature
 Signature for a discrete conditional density, used to construct conditionals. More...
struct  gtsam::EliminationTraits< DiscreteFactorGraph >
 
class  gtsam::DiscreteFactorGraph
 A Discrete Factor Graph is a factor graph where all factors are Discrete, i.e. More...
 
struct  gtsam::traits< DiscreteFactorGraph >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

│ │ │ │ -Signature gtsam::operator| (const DiscreteKey &key, const DiscreteKey &parent)
 Helper function to create Signature objects example: Signature s = D | E;.
 
│ │ │ │ -Signature gtsam::operator% (const DiscreteKey &key, const std::string &parent)
 Helper function to create Signature objects example: Signature s(D % "99/1");.
 
│ │ │ │ -Signature gtsam::operator% (const DiscreteKey &key, const Signature::Table &parent)
 Helper function to create Signature objects, using table construction directly example: Signature s(D % table);.
 
std::pair< DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr > gtsam::EliminateDiscrete (const DiscreteFactorGraph &factors, const Ordering &frontalKeys)
 Main elimination function for DiscreteFactorGraph.
 
std::pair< DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr > gtsam::EliminateForMPE (const DiscreteFactorGraph &factors, const Ordering &frontalKeys)
 Alternate elimination function for that creates non-normalized lookup tables.
 
│ │ │ │

Detailed Description

│ │ │ │ -

signatures for conditional densities

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
Date
Feb 27, 2011
│ │ │ │ +
Date
Feb 14, 2011
│ │ │ │ +
Author
Duy-Nguyen Ta
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,42 +1,45 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Signature.h File Reference │ │ │ │ │ -signatures for conditional densities _M_o_r_e_._._. │ │ │ │ │ +DiscreteFactorGraph.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_S_i_g_n_a_t_u_r_e │ │ │ │ │ -  _S_i_g_n_a_t_u_r_e for a discrete conditional density, used to construct │ │ │ │ │ - conditionals. _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_ _> │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +  A Discrete _F_a_c_t_o_r Graph is a factor graph where all factors are │ │ │ │ │ + Discrete, i.e. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -_S_i_g_n_a_t_u_r_e  ggttssaamm::::ooppeerraattoorr|| (const _D_i_s_c_r_e_t_e_K_e_y &key, const _D_i_s_c_r_e_t_e_K_e_y &parent) │ │ │ │ │ -  Helper function to create _S_i_g_n_a_t_u_r_e objects example: _S_i_g_n_a_t_u_r_e s = D │ │ │ │ │ - | E;. │ │ │ │ │ -  │ │ │ │ │ -_S_i_g_n_a_t_u_r_e  ggttssaamm::::ooppeerraattoorr%% (const _D_i_s_c_r_e_t_e_K_e_y &key, const std::string &parent) │ │ │ │ │ -  Helper function to create _S_i_g_n_a_t_u_r_e objects example: _S_i_g_n_a_t_u_r_e s(D % │ │ │ │ │ - "99/1");. │ │ │ │ │ -  │ │ │ │ │ -_S_i_g_n_a_t_u_r_e  ggttssaamm::::ooppeerraattoorr%% (const _D_i_s_c_r_e_t_e_K_e_y &key, const Signature::Table │ │ │ │ │ - &parent) │ │ │ │ │ -  Helper function to create _S_i_g_n_a_t_u_r_e objects, using table │ │ │ │ │ - construction directly example: _S_i_g_n_a_t_u_r_e s(D % table);. │ │ │ │ │ +std::pair< _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_: _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_D_i_s_c_r_e_t_e (const │ │ │ │ │ +_s_h_a_r_e_d___p_t_r, DecisionTreeFactor:: _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h &factors, const │ │ │ │ │ + shared_ptr >  _O_r_d_e_r_i_n_g &frontalKeys) │ │ │ │ │ +  Main elimination function for │ │ │ │ │ + _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h. │ │ │ │ │ +  │ │ │ │ │ +std::pair< _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_: _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_F_o_r_M_P_E (const │ │ │ │ │ +_s_h_a_r_e_d___p_t_r, DecisionTreeFactor:: _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h &factors, const │ │ │ │ │ + shared_ptr >  _O_r_d_e_r_i_n_g &frontalKeys) │ │ │ │ │ +  Alternate elimination function for that │ │ │ │ │ + creates non-normalized lookup tables. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -signatures for conditional densities │ │ │ │ │ + Date │ │ │ │ │ + Feb 14, 2011 │ │ │ │ │ Author │ │ │ │ │ + Duy-Nguyen Ta │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - Feb 27, 2011 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _S_i_g_n_a_t_u_r_e_._h │ │ │ │ │ + * _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00236.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,5 +1,6 @@ │ │ │ │ │ var a00236 = [ │ │ │ │ │ - ["operator%", "a00236.html#ae4856250df52ce4b9b4b16c7a833408c", null], │ │ │ │ │ - ["operator%", "a00236.html#ab98cf2a4777006c28d6983db5d8ec574", null], │ │ │ │ │ - ["operator|", "a00236.html#aa6c58dfb2fc9398a026e3268ba2edfad", null] │ │ │ │ │ + ["gtsam::EliminationTraits< DiscreteFactorGraph >", "a02860.html", "a02860"], │ │ │ │ │ + ["gtsam::traits< DiscreteFactorGraph >", "a02868.html", null], │ │ │ │ │ + ["EliminateDiscrete", "a00236.html#ga6996e9c84467350b071645649231223a", null], │ │ │ │ │ + ["EliminateForMPE", "a00236.html#ga9bd0b2859a7513198a369f5de3c8cde3", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00236_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/Signature.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteFactorGraph.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,112 +98,233 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Signature.h
│ │ │ │ +
DiscreteFactorGraph.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │ -
20#include <string>
│ │ │ │ -
21#include <vector>
│ │ │ │ -
22#include <boost/optional.hpp>
│ │ │ │ -
23#include <gtsam/discrete/DiscreteKey.h>
│ │ │ │ -
24
│ │ │ │ -
25namespace gtsam {
│ │ │ │ -
26
│ │ │ │ -
│ │ │ │ -
54 class GTSAM_EXPORT Signature {
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/discrete/DecisionTreeFactor.h>
│ │ │ │ +
22#include <gtsam/discrete/DiscreteLookupDAG.h>
│ │ │ │ +
23#include <gtsam/inference/EliminateableFactorGraph.h>
│ │ │ │ +
24#include <gtsam/inference/FactorGraph.h>
│ │ │ │ +
25#include <gtsam/inference/Ordering.h>
│ │ │ │ +
26#include <gtsam/base/FastSet.h>
│ │ │ │ +
27
│ │ │ │ +
28#include <boost/make_shared.hpp>
│ │ │ │ +
29#include <string>
│ │ │ │ +
30#include <utility>
│ │ │ │ +
31#include <vector>
│ │ │ │ +
32
│ │ │ │ +
33namespace gtsam {
│ │ │ │ +
34
│ │ │ │ +
35// Forward declarations
│ │ │ │ +
36class DiscreteFactorGraph;
│ │ │ │ +
37class DiscreteConditional;
│ │ │ │ +
38class DiscreteBayesNet;
│ │ │ │ +
39class DiscreteEliminationTree;
│ │ │ │ +
40class DiscreteBayesTree;
│ │ │ │ +
41class DiscreteJunctionTree;
│ │ │ │ +
42
│ │ │ │ +
51GTSAM_EXPORT
│ │ │ │ +
52std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>
│ │ │ │ +
53EliminateDiscrete(const DiscreteFactorGraph& factors,
│ │ │ │ +
54 const Ordering& frontalKeys);
│ │ │ │
55
│ │ │ │ -
56 public:
│ │ │ │ -
57
│ │ │ │ -
59 typedef std::vector<double> Row;
│ │ │ │ -
60 typedef std::vector<Row> Table;
│ │ │ │ -
61
│ │ │ │ -
62 private:
│ │ │ │ -
63
│ │ │ │ -
65 DiscreteKey key_;
│ │ │ │ -
66
│ │ │ │ -
68 DiscreteKeys parents_;
│ │ │ │ -
69
│ │ │ │ -
70 // the given CPT specification string
│ │ │ │ -
71 boost::optional<std::string> spec_;
│ │ │ │ -
72
│ │ │ │ -
73 // the CPT as parsed, if successful
│ │ │ │ -
74 boost::optional<Table> table_;
│ │ │ │ -
75
│ │ │ │ -
76 public:
│ │ │ │ -
88 Signature(const DiscreteKey& key, const DiscreteKeys& parents,
│ │ │ │ -
89 const Table& table);
│ │ │ │ -
90
│ │ │ │ -
102 Signature(const DiscreteKey& key, const DiscreteKeys& parents,
│ │ │ │ -
103 const std::string& spec);
│ │ │ │ -
104
│ │ │ │ -
112 Signature(const DiscreteKey& key);
│ │ │ │ -
113
│ │ │ │ -
115 const DiscreteKey& key() const { return key_; }
│ │ │ │ -
116
│ │ │ │ -
118 const DiscreteKeys& parents() const { return parents_; }
│ │ │ │ -
119
│ │ │ │ -
121 DiscreteKeys discreteKeys() const;
│ │ │ │ -
122
│ │ │ │ -
124 KeyVector indices() const;
│ │ │ │ -
125
│ │ │ │ -
126 // the CPT as parsed, if successful
│ │ │ │ -
127 const boost::optional<Table>& table() const { return table_; }
│ │ │ │ -
128
│ │ │ │ -
129 // the CPT as a vector of doubles, with key's values most rapidly changing
│ │ │ │ -
130 std::vector<double> cpt() const;
│ │ │ │ -
131
│ │ │ │ -
133 Signature& operator,(const DiscreteKey& parent);
│ │ │ │ -
134
│ │ │ │ -
136 Signature& operator=(const std::string& spec);
│ │ │ │ +
64GTSAM_EXPORT
│ │ │ │ +
65std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>
│ │ │ │ +
66EliminateForMPE(const DiscreteFactorGraph& factors,
│ │ │ │ +
67 const Ordering& frontalKeys);
│ │ │ │ +
68
│ │ │ │ +
│ │ │ │ +
69template<> struct EliminationTraits<DiscreteFactorGraph>
│ │ │ │ +
70{
│ │ │ │ +
71 typedef DiscreteFactor FactorType;
│ │ │ │ +
72 typedef DiscreteFactorGraph FactorGraphType;
│ │ │ │ +
73 typedef DiscreteConditional ConditionalType;
│ │ │ │ +
74 typedef DiscreteBayesNet BayesNetType;
│ │ │ │ +
75 typedef DiscreteEliminationTree EliminationTreeType;
│ │ │ │ +
76 typedef DiscreteBayesTree BayesTreeType;
│ │ │ │ +
77 typedef DiscreteJunctionTree JunctionTreeType;
│ │ │ │ +
78
│ │ │ │ +
80 static std::pair<boost::shared_ptr<ConditionalType>,
│ │ │ │ +
81 boost::shared_ptr<FactorType> >
│ │ │ │ +
│ │ │ │ +
82 DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) {
│ │ │ │ +
83 return EliminateDiscrete(factors, keys);
│ │ │ │ +
84 }
│ │ │ │ +
│ │ │ │ +
85
│ │ │ │ +
│ │ │ │ +
87 static Ordering DefaultOrderingFunc(
│ │ │ │ +
88 const FactorGraphType& graph,
│ │ │ │ +
89 boost::optional<const VariableIndex&> variableIndex) {
│ │ │ │ +
90 return Ordering::Colamd(*variableIndex);
│ │ │ │ +
91 }
│ │ │ │ +
│ │ │ │ +
92};
│ │ │ │ +
│ │ │ │ +
93
│ │ │ │ +
│ │ │ │ +
99class GTSAM_EXPORT DiscreteFactorGraph
│ │ │ │ +
100 : public FactorGraph<DiscreteFactor>,
│ │ │ │ +
101 public EliminateableFactorGraph<DiscreteFactorGraph> {
│ │ │ │ +
102 public:
│ │ │ │ +
103 using This = DiscreteFactorGraph;
│ │ │ │ +
104 using Base = FactorGraph<DiscreteFactor>;
│ │ │ │ +
105 using BaseEliminateable =
│ │ │ │ +
106 EliminateableFactorGraph<This>;
│ │ │ │ +
107 using shared_ptr = boost::shared_ptr<This>;
│ │ │ │ +
108
│ │ │ │ +
109 using Values = DiscreteValues;
│ │ │ │ +
110
│ │ │ │ +
111 using Indices = KeyVector;
│ │ │ │ +
112
│ │ │ │ +
114 DiscreteFactorGraph() {}
│ │ │ │ +
115
│ │ │ │ +
117 template <typename ITERATOR>
│ │ │ │ +
│ │ │ │ +
118 DiscreteFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
│ │ │ │ +
119 : Base(firstFactor, lastFactor) {}
│ │ │ │ +
│ │ │ │ +
120
│ │ │ │ +
122 template <class CONTAINER>
│ │ │ │ +
123 explicit DiscreteFactorGraph(const CONTAINER& factors) : Base(factors) {}
│ │ │ │ +
124
│ │ │ │ +
127 template <class DERIVED_FACTOR>
│ │ │ │ +
128 DiscreteFactorGraph(const FactorGraph<DERIVED_FACTOR>& graph) : Base(graph) {}
│ │ │ │ +
129
│ │ │ │ +
131 virtual ~DiscreteFactorGraph() {}
│ │ │ │ +
132
│ │ │ │ +
135
│ │ │ │ +
136 bool equals(const This& fg, double tol = 1e-9) const;
│ │ │ │
137
│ │ │ │ -
139 Signature& operator=(const Table& table);
│ │ │ │ -
140
│ │ │ │ -
142 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
│ │ │ │ -
143 const Signature& s);
│ │ │ │ -
144 };
│ │ │ │ +
139
│ │ │ │ +
141 template <typename... Args>
│ │ │ │ +
│ │ │ │ +
142 void add(Args&&... args) {
│ │ │ │ +
143 emplace_shared<DecisionTreeFactor>(std::forward<Args>(args)...);
│ │ │ │ +
144 }
│ │ │ │
│ │ │ │
145
│ │ │ │ -
150 GTSAM_EXPORT Signature operator|(const DiscreteKey& key, const DiscreteKey& parent);
│ │ │ │ +
147 KeySet keys() const;
│ │ │ │ +
148
│ │ │ │ +
150 DiscreteKeys discreteKeys() const;
│ │ │ │
151
│ │ │ │ -
156 GTSAM_EXPORT Signature operator%(const DiscreteKey& key, const std::string& parent);
│ │ │ │ -
157
│ │ │ │ -
162 GTSAM_EXPORT Signature operator%(const DiscreteKey& key, const Signature::Table& parent);
│ │ │ │ -
163
│ │ │ │ -
164}
│ │ │ │ -
DiscreteKey.h
specialized key for discrete variables
│ │ │ │ -
gtsam::DiscreteKey
std::pair< Key, size_t > DiscreteKey
Key type for discrete variables.
Definition DiscreteKey.h:36
│ │ │ │ +
153 DecisionTreeFactor product() const;
│ │ │ │ +
154
│ │ │ │ +
159 double operator()(const DiscreteValues& values) const;
│ │ │ │ +
160
│ │ │ │ +
162 void print(
│ │ │ │ +
163 const std::string& s = "DiscreteFactorGraph",
│ │ │ │ +
164 const KeyFormatter& formatter = DefaultKeyFormatter) const override;
│ │ │ │ +
165
│ │ │ │ +
172 DiscreteBayesNet sumProduct(
│ │ │ │ +
173 OptionalOrderingType orderingType = boost::none) const;
│ │ │ │ +
174
│ │ │ │ +
181 DiscreteBayesNet sumProduct(const Ordering& ordering) const;
│ │ │ │ +
182
│ │ │ │ +
189 DiscreteLookupDAG maxProduct(
│ │ │ │ +
190 OptionalOrderingType orderingType = boost::none) const;
│ │ │ │ +
191
│ │ │ │ +
198 DiscreteLookupDAG maxProduct(const Ordering& ordering) const;
│ │ │ │ +
199
│ │ │ │ +
206 DiscreteValues optimize(
│ │ │ │ +
207 OptionalOrderingType orderingType = boost::none) const;
│ │ │ │ +
208
│ │ │ │ +
215 DiscreteValues optimize(const Ordering& ordering) const;
│ │ │ │ +
216
│ │ │ │ +
219
│ │ │ │ +
227 std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
228 const DiscreteFactor::Names& names = {}) const;
│ │ │ │ +
229
│ │ │ │ +
237 std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
238 const DiscreteFactor::Names& names = {}) const;
│ │ │ │ +
239
│ │ │ │ +
243
│ │ │ │ +
244 using Base::error; // Expose error(const HybridValues&) method..
│ │ │ │ +
245
│ │ │ │ +
247}; // \ DiscreteFactorGraph
│ │ │ │ +
│ │ │ │ +
248
│ │ │ │ +
250template <>
│ │ │ │ +
251struct traits<DiscreteFactorGraph> : public Testable<DiscreteFactorGraph> {};
│ │ │ │ +
252
│ │ │ │ +
253} // namespace gtsam
│ │ │ │ +
FastSet.h
A thin wrapper around std::set that uses boost's fast_pool_allocator.
│ │ │ │ + │ │ │ │ + │ │ │ │ +
Ordering.h
Variable ordering for the elimination algorithm.
│ │ │ │ +
FactorGraph.h
Factor Graph Base Class.
│ │ │ │ +
EliminateableFactorGraph.h
Variable elimination algorithms for factor graphs.
│ │ │ │ +
gtsam::EliminateDiscrete
std::pair< DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr > EliminateDiscrete(const DiscreteFactorGraph &factors, const Ordering &frontalKeys)
Main elimination function for DiscreteFactorGraph.
Definition DiscreteFactorGraph.cpp:200
│ │ │ │ +
gtsam::EliminateForMPE
std::pair< DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr > EliminateForMPE(const DiscreteFactorGraph &factors, const Ordering &frontalKeys)
Alternate elimination function for that creates non-normalized lookup tables.
Definition DiscreteFactorGraph.cpp:116
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::operator|
Signature operator|(const DiscreteKey &key, const DiscreteKey &parent)
Helper function to create Signature objects example: Signature s = D | E;.
Definition Signature.cpp:178
│ │ │ │ -
gtsam::operator%
Signature operator%(const DiscreteKey &key, const string &parent)
Helper function to create Signature objects example: Signature s(D % "99/1");.
Definition Signature.cpp:183
│ │ │ │ +
gtsam::markdown
string markdown(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
Free version of markdown.
Definition DiscreteValues.cpp:129
│ │ │ │ +
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ + │ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::DecisionTreeFactor
A discrete probabilistic factor.
Definition DecisionTreeFactor.h:45
│ │ │ │ +
gtsam::DiscreteBayesNet
A Bayes net made from discrete conditional distributions.
Definition DiscreteBayesNet.h:38
│ │ │ │ +
gtsam::DiscreteBayesTree
A Bayes tree representing a Discrete density.
Definition DiscreteBayesTree.h:75
│ │ │ │ +
gtsam::DiscreteConditional
Discrete Conditional Density Derives from DecisionTreeFactor.
Definition DiscreteConditional.h:40
│ │ │ │ +
gtsam::DiscreteEliminationTree
Elimination tree for discrete factors.
Definition DiscreteEliminationTree.h:33
│ │ │ │ +
gtsam::DiscreteFactor
Base class for discrete probabilistic factors The most general one is the derived DecisionTreeFactor.
Definition DiscreteFactor.h:38
│ │ │ │ +
gtsam::DiscreteFactor::Names
DiscreteValues::Names Names
Translation table from values to strings.
Definition DiscreteFactor.h:106
│ │ │ │ +
gtsam::EliminationTraits< DiscreteFactorGraph >::FactorGraphType
DiscreteFactorGraph FactorGraphType
Type of the factor graph (e.g. DiscreteFactorGraph)
Definition DiscreteFactorGraph.h:72
│ │ │ │ +
gtsam::EliminationTraits< DiscreteFactorGraph >::JunctionTreeType
DiscreteJunctionTree JunctionTreeType
Type of Junction tree.
Definition DiscreteFactorGraph.h:77
│ │ │ │ +
gtsam::EliminationTraits< DiscreteFactorGraph >::FactorType
DiscreteFactor FactorType
Type of factors in factor graph.
Definition DiscreteFactorGraph.h:71
│ │ │ │ +
gtsam::EliminationTraits< DiscreteFactorGraph >::DefaultEliminate
static std::pair< boost::shared_ptr< ConditionalType >, boost::shared_ptr< FactorType > > DefaultEliminate(const FactorGraphType &factors, const Ordering &keys)
The default dense elimination function.
Definition DiscreteFactorGraph.h:82
│ │ │ │ +
gtsam::EliminationTraits< DiscreteFactorGraph >::BayesTreeType
DiscreteBayesTree BayesTreeType
Type of Bayes tree.
Definition DiscreteFactorGraph.h:76
│ │ │ │ +
gtsam::EliminationTraits< DiscreteFactorGraph >::BayesNetType
DiscreteBayesNet BayesNetType
Type of Bayes net from sequential elimination.
Definition DiscreteFactorGraph.h:74
│ │ │ │ +
gtsam::EliminationTraits< DiscreteFactorGraph >::ConditionalType
DiscreteConditional ConditionalType
Type of conditionals from elimination.
Definition DiscreteFactorGraph.h:73
│ │ │ │ +
gtsam::EliminationTraits< DiscreteFactorGraph >::DefaultOrderingFunc
static Ordering DefaultOrderingFunc(const FactorGraphType &graph, boost::optional< const VariableIndex & > variableIndex)
The default ordering generation function.
Definition DiscreteFactorGraph.h:87
│ │ │ │ +
gtsam::EliminationTraits< DiscreteFactorGraph >::EliminationTreeType
DiscreteEliminationTree EliminationTreeType
Type of elimination tree.
Definition DiscreteFactorGraph.h:75
│ │ │ │ +
gtsam::DiscreteFactorGraph
A Discrete Factor Graph is a factor graph where all factors are Discrete, i.e.
Definition DiscreteFactorGraph.h:101
│ │ │ │ +
gtsam::DiscreteFactorGraph::DiscreteFactorGraph
DiscreteFactorGraph(const CONTAINER &factors)
Construct from container of factors (shared_ptr or plain objects)
Definition DiscreteFactorGraph.h:123
│ │ │ │ +
gtsam::DiscreteFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to This
Definition DiscreteFactorGraph.h:107
│ │ │ │ +
gtsam::DiscreteFactorGraph::DiscreteFactorGraph
DiscreteFactorGraph()
‍map from keys to values
Definition DiscreteFactorGraph.h:114
│ │ │ │ +
gtsam::DiscreteFactorGraph::DiscreteFactorGraph
DiscreteFactorGraph(const FactorGraph< DERIVED_FACTOR > &graph)
Implicit copy/downcast constructor to override explicit template container constructor.
Definition DiscreteFactorGraph.h:128
│ │ │ │ +
gtsam::DiscreteFactorGraph::DiscreteFactorGraph
DiscreteFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
Construct from iterator over factors.
Definition DiscreteFactorGraph.h:118
│ │ │ │ +
gtsam::DiscreteFactorGraph::~DiscreteFactorGraph
virtual ~DiscreteFactorGraph()
Destructor.
Definition DiscreteFactorGraph.h:131
│ │ │ │ +
gtsam::DiscreteFactorGraph::BaseEliminateable
EliminateableFactorGraph< This > BaseEliminateable
for elimination
Definition DiscreteFactorGraph.h:106
│ │ │ │ +
gtsam::DiscreteFactorGraph::add
void add(Args &&... args)
Add a decision-tree factor.
Definition DiscreteFactorGraph.h:142
│ │ │ │ +
gtsam::DiscreteJunctionTree
An EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree,...
Definition DiscreteJunctionTree.h:52
│ │ │ │
gtsam::DiscreteKeys
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition DiscreteKey.h:39
│ │ │ │ -
gtsam::Signature
Signature for a discrete conditional density, used to construct conditionals.
Definition Signature.h:54
│ │ │ │ -
gtsam::Signature::Row
std::vector< double > Row
Data type for the CPT.
Definition Signature.h:59
│ │ │ │ -
gtsam::Signature::key
const DiscreteKey & key() const
the variable key
Definition Signature.h:115
│ │ │ │ -
gtsam::Signature::parents
const DiscreteKeys & parents() const
the parent keys
Definition Signature.h:118
│ │ │ │ +
gtsam::DiscreteLookupDAG
A DAG made from lookup tables, as defined above.
Definition DiscreteLookupDAG.h:77
│ │ │ │ +
gtsam::DiscreteValues
A map from keys to values.
Definition DiscreteValues.h:34
│ │ │ │ +
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ +
gtsam::EliminationTraits
Traits class for eliminateable factor graphs, specifies the types that result from elimination,...
Definition EliminateableFactorGraph.h:36
│ │ │ │ +
gtsam::EliminateableFactorGraph
EliminateableFactorGraph is a base class for factor graphs that contains elimination algorithms.
Definition EliminateableFactorGraph.h:57
│ │ │ │ +
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
gtsam::Ordering::Colamd
static Ordering Colamd(const FACTOR_GRAPH &graph)
Compute a fill-reducing ordering using COLAMD from a factor graph (see details for note on performanc...
Definition Ordering.h:95
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── encoding │ │ │ │ │ @@ -1 +1 @@ │ │ │ │ │ -us-ascii │ │ │ │ │ +utf-8 │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Signature.h │ │ │ │ │ +DiscreteFactorGraph.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ @@ -15,119 +15,338 @@ │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 19#pragma once │ │ │ │ │ -20#include │ │ │ │ │ -21#include │ │ │ │ │ -22#include │ │ │ │ │ -23#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_K_e_y_._h> │ │ │ │ │ -24 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ -26 │ │ │ │ │ -_5_4 class GTSAM_EXPORT _S_i_g_n_a_t_u_r_e { │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_O_r_d_e_r_i_n_g_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_S_e_t_._h> │ │ │ │ │ +27 │ │ │ │ │ +28#include │ │ │ │ │ +29#include │ │ │ │ │ +30#include │ │ │ │ │ +31#include │ │ │ │ │ +32 │ │ │ │ │ +33namespace _g_t_s_a_m { │ │ │ │ │ +34 │ │ │ │ │ +35// Forward declarations │ │ │ │ │ +36class DiscreteFactorGraph; │ │ │ │ │ +37class DiscreteConditional; │ │ │ │ │ +38class DiscreteBayesNet; │ │ │ │ │ +39class DiscreteEliminationTree; │ │ │ │ │ +40class DiscreteBayesTree; │ │ │ │ │ +41class DiscreteJunctionTree; │ │ │ │ │ +42 │ │ │ │ │ +51GTSAM_EXPORT │ │ │ │ │ +52std::pair │ │ │ │ │ +53_E_l_i_m_i_n_a_t_e_D_i_s_c_r_e_t_e(const DiscreteFactorGraph& factors, │ │ │ │ │ +54 const Ordering& frontalKeys); │ │ │ │ │ 55 │ │ │ │ │ -56 public: │ │ │ │ │ -57 │ │ │ │ │ -_5_9 typedef std::vector _R_o_w; │ │ │ │ │ -60 typedef std::vector Table; │ │ │ │ │ -61 │ │ │ │ │ -62 private: │ │ │ │ │ -63 │ │ │ │ │ -65 _D_i_s_c_r_e_t_e_K_e_y key_; │ │ │ │ │ -66 │ │ │ │ │ -68 _D_i_s_c_r_e_t_e_K_e_y_s parents_; │ │ │ │ │ -69 │ │ │ │ │ -70 // the given CPT specification string │ │ │ │ │ -71 boost::optional spec_; │ │ │ │ │ -72 │ │ │ │ │ -73 // the CPT as parsed, if successful │ │ │ │ │ -74 boost::optional table_; │ │ │ │ │ -75 │ │ │ │ │ -76 public: │ │ │ │ │ -88 _S_i_g_n_a_t_u_r_e(const _D_i_s_c_r_e_t_e_K_e_y& key, const _D_i_s_c_r_e_t_e_K_e_y_s& parents, │ │ │ │ │ -89 const Table& table); │ │ │ │ │ -90 │ │ │ │ │ -102 _S_i_g_n_a_t_u_r_e(const _D_i_s_c_r_e_t_e_K_e_y& key, const _D_i_s_c_r_e_t_e_K_e_y_s& parents, │ │ │ │ │ -103 const std::string& spec); │ │ │ │ │ -104 │ │ │ │ │ -112 _S_i_g_n_a_t_u_r_e(const _D_i_s_c_r_e_t_e_K_e_y& key); │ │ │ │ │ -113 │ │ │ │ │ -_1_1_5 const _D_i_s_c_r_e_t_e_K_e_y& _k_e_y() const { return key_; } │ │ │ │ │ -116 │ │ │ │ │ -_1_1_8 const _D_i_s_c_r_e_t_e_K_e_y_s& _p_a_r_e_n_t_s() const { return parents_; } │ │ │ │ │ -119 │ │ │ │ │ -121 _D_i_s_c_r_e_t_e_K_e_y_s discreteKeys() const; │ │ │ │ │ -122 │ │ │ │ │ -124 _K_e_y_V_e_c_t_o_r indices() const; │ │ │ │ │ -125 │ │ │ │ │ -126 // the CPT as parsed, if successful │ │ │ │ │ -127 const boost::optional
& table() const { return table_; } │ │ │ │ │ -128 │ │ │ │ │ -129 // the CPT as a vector of doubles, with key's values most rapidly changing │ │ │ │ │ -130 std::vector cpt() const; │ │ │ │ │ -131 │ │ │ │ │ -133 Signature& operator,(const DiscreteKey& parent); │ │ │ │ │ -134 │ │ │ │ │ -136 Signature& operator=(const std::string& spec); │ │ │ │ │ +64GTSAM_EXPORT │ │ │ │ │ +65std::pair │ │ │ │ │ +66_E_l_i_m_i_n_a_t_e_F_o_r_M_P_E(const DiscreteFactorGraph& factors, │ │ │ │ │ +67 const Ordering& frontalKeys); │ │ │ │ │ +68 │ │ │ │ │ +_6_9template<> struct _E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s<_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h> │ │ │ │ │ +70{ │ │ │ │ │ +_7_1 typedef _D_i_s_c_r_e_t_e_F_a_c_t_o_r _F_a_c_t_o_r_T_y_p_e; │ │ │ │ │ +_7_2 typedef _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e; │ │ │ │ │ +_7_3 typedef _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l _C_o_n_d_i_t_i_o_n_a_l_T_y_p_e; │ │ │ │ │ +_7_4 typedef _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t _B_a_y_e_s_N_e_t_T_y_p_e; │ │ │ │ │ +_7_5 typedef _D_i_s_c_r_e_t_e_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_T_y_p_e; │ │ │ │ │ +_7_6 typedef _D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e _B_a_y_e_s_T_r_e_e_T_y_p_e; │ │ │ │ │ +_7_7 typedef _D_i_s_c_r_e_t_e_J_u_n_c_t_i_o_n_T_r_e_e _J_u_n_c_t_i_o_n_T_r_e_e_T_y_p_e; │ │ │ │ │ +78 │ │ │ │ │ +80 static std::pair, │ │ │ │ │ +81 boost::shared_ptr > │ │ │ │ │ +_8_2 _D_e_f_a_u_l_t_E_l_i_m_i_n_a_t_e(const _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e& factors, const _O_r_d_e_r_i_n_g& keys) { │ │ │ │ │ +83 return _E_l_i_m_i_n_a_t_e_D_i_s_c_r_e_t_e(factors, keys); │ │ │ │ │ +84 } │ │ │ │ │ +85 │ │ │ │ │ +_8_7 static _O_r_d_e_r_i_n_g _D_e_f_a_u_l_t_O_r_d_e_r_i_n_g_F_u_n_c( │ │ │ │ │ +88 const _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e& graph, │ │ │ │ │ +89 boost::optional variableIndex) { │ │ │ │ │ +90 return _O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d(*variableIndex); │ │ │ │ │ +91 } │ │ │ │ │ +92}; │ │ │ │ │ +93 │ │ │ │ │ +_9_9class GTSAM_EXPORT _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +100 : public _F_a_c_t_o_r_G_r_a_p_h, │ │ │ │ │ +101 public _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h { │ │ │ │ │ +102 public: │ │ │ │ │ +_1_0_3 using _T_h_i_s = _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h; │ │ │ │ │ +_1_0_4 using _B_a_s_e = _F_a_c_t_o_r_G_r_a_p_h_<_D_i_s_c_r_e_t_e_F_a_c_t_o_r_>; │ │ │ │ │ +_1_0_5 using _B_a_s_e_E_l_i_m_i_n_a_t_e_a_b_l_e = │ │ │ │ │ +106 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_T_h_i_s_>; │ │ │ │ │ +_1_0_7 using _s_h_a_r_e_d___p_t_r = boost::shared_ptr; │ │ │ │ │ +108 │ │ │ │ │ +_1_0_9 using _V_a_l_u_e_s = _D_i_s_c_r_e_t_e_V_a_l_u_e_s; │ │ │ │ │ +110 │ │ │ │ │ +111 using Indices = _K_e_y_V_e_c_t_o_r; │ │ │ │ │ +112 │ │ │ │ │ +_1_1_4 _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h() {} │ │ │ │ │ +115 │ │ │ │ │ +117 template │ │ │ │ │ +_1_1_8 _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h(ITERATOR firstFactor, ITERATOR lastFactor) │ │ │ │ │ +119 : _B_a_s_e(firstFactor, lastFactor) {} │ │ │ │ │ +120 │ │ │ │ │ +122 template │ │ │ │ │ +_1_2_3 explicit _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h(const CONTAINER& factors) : _B_a_s_e(factors) {} │ │ │ │ │ +124 │ │ │ │ │ +127 template │ │ │ │ │ +_1_2_8 _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h(const _F_a_c_t_o_r_G_r_a_p_h_<_D_E_R_I_V_E_D___F_A_C_T_O_R_>& graph) : _B_a_s_e(graph) │ │ │ │ │ +{} │ │ │ │ │ +129 │ │ │ │ │ +_1_3_1 virtual _~_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h() {} │ │ │ │ │ +132 │ │ │ │ │ +135 │ │ │ │ │ +136 bool _e_q_u_a_l_s(const This& fg, double tol = 1e-9) const; │ │ │ │ │ 137 │ │ │ │ │ -139 Signature& operator=(const Table& table); │ │ │ │ │ -140 │ │ │ │ │ -142 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, │ │ │ │ │ -143 const Signature& s); │ │ │ │ │ -144 }; │ │ │ │ │ +139 │ │ │ │ │ +141 template │ │ │ │ │ +_1_4_2 void _a_d_d(Args&&... args) { │ │ │ │ │ +143 emplace_shared(std::forward(args)...); │ │ │ │ │ +144 } │ │ │ │ │ 145 │ │ │ │ │ -150 GTSAM_EXPORT Signature _o_p_e_r_a_t_o_r_|(const _D_i_s_c_r_e_t_e_K_e_y& key, const _D_i_s_c_r_e_t_e_K_e_y& │ │ │ │ │ -parent); │ │ │ │ │ +147 _K_e_y_S_e_t keys() const; │ │ │ │ │ +148 │ │ │ │ │ +150 _D_i_s_c_r_e_t_e_K_e_y_s discreteKeys() const; │ │ │ │ │ 151 │ │ │ │ │ -156 GTSAM_EXPORT Signature _o_p_e_r_a_t_o_r_%(const _D_i_s_c_r_e_t_e_K_e_y& key, const std::string& │ │ │ │ │ -parent); │ │ │ │ │ -157 │ │ │ │ │ -162 GTSAM_EXPORT Signature _o_p_e_r_a_t_o_r_%(const _D_i_s_c_r_e_t_e_K_e_y& key, const Signature:: │ │ │ │ │ -Table& parent); │ │ │ │ │ -163 │ │ │ │ │ -164} │ │ │ │ │ -_D_i_s_c_r_e_t_e_K_e_y_._h │ │ │ │ │ -specialized key for discrete variables │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y │ │ │ │ │ -std::pair< Key, size_t > DiscreteKey │ │ │ │ │ -Key type for discrete variables. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteKey.h:36 │ │ │ │ │ +153 _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r product() const; │ │ │ │ │ +154 │ │ │ │ │ +159 double operator()(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values) const; │ │ │ │ │ +160 │ │ │ │ │ +162 void _p_r_i_n_t( │ │ │ │ │ +163 const std::string& s = "DiscreteFactorGraph", │ │ │ │ │ +164 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override; │ │ │ │ │ +165 │ │ │ │ │ +172 _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t sumProduct( │ │ │ │ │ +173 OptionalOrderingType orderingType = boost::none) const; │ │ │ │ │ +174 │ │ │ │ │ +181 _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t sumProduct(const _O_r_d_e_r_i_n_g& ordering) const; │ │ │ │ │ +182 │ │ │ │ │ +189 _D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G maxProduct( │ │ │ │ │ +190 OptionalOrderingType orderingType = boost::none) const; │ │ │ │ │ +191 │ │ │ │ │ +198 _D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G maxProduct(const _O_r_d_e_r_i_n_g& ordering) const; │ │ │ │ │ +199 │ │ │ │ │ +206 _D_i_s_c_r_e_t_e_V_a_l_u_e_s _o_p_t_i_m_i_z_e( │ │ │ │ │ +207 OptionalOrderingType orderingType = boost::none) const; │ │ │ │ │ +208 │ │ │ │ │ +215 _D_i_s_c_r_e_t_e_V_a_l_u_e_s _o_p_t_i_m_i_z_e(const _O_r_d_e_r_i_n_g& ordering) const; │ │ │ │ │ +216 │ │ │ │ │ +219 │ │ │ │ │ +227 std::string _m_a_r_k_d_o_w_n(const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ +DefaultKeyFormatter, │ │ │ │ │ +228 const _D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_N_a_m_e_s& names = {}) const; │ │ │ │ │ +229 │ │ │ │ │ +237 std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ +238 const DiscreteFactor::Names& names = {}) const; │ │ │ │ │ +239 │ │ │ │ │ +243 │ │ │ │ │ +244 using Base::error; // Expose error(const HybridValues&) method.. │ │ │ │ │ +245 │ │ │ │ │ +247}; // \ DiscreteFactorGraph │ │ │ │ │ +248 │ │ │ │ │ +250template <> │ │ │ │ │ +_2_5_1struct _t_r_a_i_t_s<_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h> : public _T_e_s_t_a_b_l_e │ │ │ │ │ +{}; │ │ │ │ │ +252 │ │ │ │ │ +253} // namespace gtsam │ │ │ │ │ +_F_a_s_t_S_e_t_._h │ │ │ │ │ +A thin wrapper around std::set that uses boost's fast_pool_allocator. │ │ │ │ │ +_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_._h │ │ │ │ │ +_D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G_._h │ │ │ │ │ +_O_r_d_e_r_i_n_g_._h │ │ │ │ │ +Variable ordering for the elimination algorithm. │ │ │ │ │ +_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Factor Graph Base Class. │ │ │ │ │ +_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Variable elimination algorithms for factor graphs. │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_D_i_s_c_r_e_t_e │ │ │ │ │ +std::pair< DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr > │ │ │ │ │ +EliminateDiscrete(const DiscreteFactorGraph &factors, const Ordering │ │ │ │ │ +&frontalKeys) │ │ │ │ │ +Main elimination function for DiscreteFactorGraph. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactorGraph.cpp:200 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_F_o_r_M_P_E │ │ │ │ │ +std::pair< DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr > │ │ │ │ │ +EliminateForMPE(const DiscreteFactorGraph &factors, const Ordering │ │ │ │ │ +&frontalKeys) │ │ │ │ │ +Alternate elimination function for that creates non-normalized lookup tables. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactorGraph.cpp:116 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ FastVector< Key > KeyVector │ │ │ │ │ Define collection type once and for all - also used in wrappers. │ │ │ │ │ DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_| │ │ │ │ │ -Signature operator|(const DiscreteKey &key, const DiscreteKey &parent) │ │ │ │ │ -Helper function to create Signature objects example: Signature s = D | E;. │ │ │ │ │ -DDeeffiinniittiioonn Signature.cpp:178 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_% │ │ │ │ │ -Signature operator%(const DiscreteKey &key, const string &parent) │ │ │ │ │ -Helper function to create Signature objects example: Signature s(D % "99/1");. │ │ │ │ │ -DDeeffiinniittiioonn Signature.cpp:183 │ │ │ │ │ +_g_t_s_a_m_:_:_m_a_r_k_d_o_w_n │ │ │ │ │ +string markdown(const DiscreteValues &values, const KeyFormatter &keyFormatter, │ │ │ │ │ +const DiscreteValues::Names &names) │ │ │ │ │ +Free version of markdown. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteValues.cpp:129 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ +Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key │ │ │ │ │ +landmarkKey) │ │ │ │ │ +Optimize for triangulation. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.cpp:155 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r │ │ │ │ │ +A discrete probabilistic factor. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTreeFactor.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t │ │ │ │ │ +A Bayes net made from discrete conditional distributions. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteBayesNet.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e │ │ │ │ │ +A Bayes tree representing a Discrete density. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteBayesTree.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +Discrete Conditional Density Derives from DecisionTreeFactor. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteConditional.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ +Elimination tree for discrete factors. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteEliminationTree.h:33 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r │ │ │ │ │ +Base class for discrete probabilistic factors The most general one is the │ │ │ │ │ +derived DecisionTreeFactor. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactor.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_N_a_m_e_s │ │ │ │ │ +DiscreteValues::Names Names │ │ │ │ │ +Translation table from values to strings. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactor.h:106 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_F_a_c_t_o_r_G_r_a_p_h_T_y_p_e │ │ │ │ │ +DiscreteFactorGraph FactorGraphType │ │ │ │ │ +Type of the factor graph (e.g. DiscreteFactorGraph) │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactorGraph.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_J_u_n_c_t_i_o_n_T_r_e_e_T_y_p_e │ │ │ │ │ +DiscreteJunctionTree JunctionTreeType │ │ │ │ │ +Type of Junction tree. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactorGraph.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_F_a_c_t_o_r_T_y_p_e │ │ │ │ │ +DiscreteFactor FactorType │ │ │ │ │ +Type of factors in factor graph. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactorGraph.h:71 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_D_e_f_a_u_l_t_E_l_i_m_i_n_a_t_e │ │ │ │ │ +static std::pair< boost::shared_ptr< ConditionalType >, boost::shared_ptr< │ │ │ │ │ +FactorType > > DefaultEliminate(const FactorGraphType &factors, const Ordering │ │ │ │ │ +&keys) │ │ │ │ │ +The default dense elimination function. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactorGraph.h:82 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_B_a_y_e_s_T_r_e_e_T_y_p_e │ │ │ │ │ +DiscreteBayesTree BayesTreeType │ │ │ │ │ +Type of Bayes tree. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactorGraph.h:76 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_B_a_y_e_s_N_e_t_T_y_p_e │ │ │ │ │ +DiscreteBayesNet BayesNetType │ │ │ │ │ +Type of Bayes net from sequential elimination. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactorGraph.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_C_o_n_d_i_t_i_o_n_a_l_T_y_p_e │ │ │ │ │ +DiscreteConditional ConditionalType │ │ │ │ │ +Type of conditionals from elimination. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactorGraph.h:73 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_D_e_f_a_u_l_t_O_r_d_e_r_i_n_g_F_u_n_c │ │ │ │ │ +static Ordering DefaultOrderingFunc(const FactorGraphType &graph, boost:: │ │ │ │ │ +optional< const VariableIndex & > variableIndex) │ │ │ │ │ +The default ordering generation function. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactorGraph.h:87 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_T_y_p_e │ │ │ │ │ +DiscreteEliminationTree EliminationTreeType │ │ │ │ │ +Type of elimination tree. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactorGraph.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A Discrete Factor Graph is a factor graph where all factors are Discrete, i.e. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactorGraph.h:101 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +DiscreteFactorGraph(const CONTAINER &factors) │ │ │ │ │ +Construct from container of factors (shared_ptr or plain objects) │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactorGraph.h:123 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to This │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactorGraph.h:107 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +DiscreteFactorGraph() │ │ │ │ │ +‍map from keys to values │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactorGraph.h:114 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +DiscreteFactorGraph(const FactorGraph< DERIVED_FACTOR > &graph) │ │ │ │ │ +Implicit copy/downcast constructor to override explicit template container │ │ │ │ │ +constructor. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactorGraph.h:128 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +DiscreteFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) │ │ │ │ │ +Construct from iterator over factors. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactorGraph.h:118 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_:_:_~_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +virtual ~DiscreteFactorGraph() │ │ │ │ │ +Destructor. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactorGraph.h:131 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_:_:_B_a_s_e_E_l_i_m_i_n_a_t_e_a_b_l_e │ │ │ │ │ +EliminateableFactorGraph< This > BaseEliminateable │ │ │ │ │ +for elimination │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactorGraph.h:106 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d │ │ │ │ │ +void add(Args &&... args) │ │ │ │ │ +Add a decision-tree factor. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactorGraph.h:142 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ +An EliminatableClusterTree, i.e., a set of variable clusters with factors, │ │ │ │ │ +arranged in a tree,... │ │ │ │ │ +DDeeffiinniittiioonn DiscreteJunctionTree.h:52 │ │ │ │ │ _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ DiscreteKeys is a set of keys that can be assembled using the & operator. │ │ │ │ │ DDeeffiinniittiioonn DiscreteKey.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_S_i_g_n_a_t_u_r_e │ │ │ │ │ -Signature for a discrete conditional density, used to construct conditionals. │ │ │ │ │ -DDeeffiinniittiioonn Signature.h:54 │ │ │ │ │ -_g_t_s_a_m_:_:_S_i_g_n_a_t_u_r_e_:_:_R_o_w │ │ │ │ │ -std::vector< double > Row │ │ │ │ │ -Data type for the CPT. │ │ │ │ │ -DDeeffiinniittiioonn Signature.h:59 │ │ │ │ │ -_g_t_s_a_m_:_:_S_i_g_n_a_t_u_r_e_:_:_k_e_y │ │ │ │ │ -const DiscreteKey & key() const │ │ │ │ │ -the variable key │ │ │ │ │ -DDeeffiinniittiioonn Signature.h:115 │ │ │ │ │ -_g_t_s_a_m_:_:_S_i_g_n_a_t_u_r_e_:_:_p_a_r_e_n_t_s │ │ │ │ │ -const DiscreteKeys & parents() const │ │ │ │ │ -the parent keys │ │ │ │ │ -DDeeffiinniittiioonn Signature.h:118 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G │ │ │ │ │ +A DAG made from lookup tables, as defined above. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteLookupDAG.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ +A map from keys to values. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteValues.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ +nodes. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s │ │ │ │ │ +Traits class for eliminateable factor graphs, specifies the types that result │ │ │ │ │ +from elimination,... │ │ │ │ │ +DDeeffiinniittiioonn EliminateableFactorGraph.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +EliminateableFactorGraph is a base class for factor graphs that contains │ │ │ │ │ +elimination algorithms. │ │ │ │ │ +DDeeffiinniittiioonn EliminateableFactorGraph.h:57 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d │ │ │ │ │ +static Ordering Colamd(const FACTOR_GRAPH &graph) │ │ │ │ │ +Compute a fill-reducing ordering using COLAMD from a factor graph (see details │ │ │ │ │ +for note on performanc... │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:95 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _S_i_g_n_a_t_u_r_e_._h │ │ │ │ │ + * _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00239.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteKey.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/Signature.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,62 +94,84 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ Typedefs | │ │ │ │ -Functions
│ │ │ │ -
DiscreteKey.h File Reference
│ │ │ │ +Functions | │ │ │ │ +Variables
│ │ │ │ +
Signature.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

specialized key for discrete variables │ │ │ │ +

signatures for conditional densities │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

struct  gtsam::DiscreteKeys
 DiscreteKeys is a set of keys that can be assembled using the & operator. More...
 
struct  gtsam::traits< DiscreteKeys >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Typedefs

using gtsam::DiscreteKey = std::pair< Key, size_t >
 Key type for discrete variables.
 
│ │ │ │ +typedef string::const_iterator gtsam::parser::It
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ Functions

│ │ │ │ -DiscreteKeys gtsam::operator& (const DiscreteKey &key1, const DiscreteKey &key2)
 Create a list from two keys.
 
│ │ │ │ +Signature::Table gtsam::parser::logic (bool ff, bool ft, bool tf, bool tt)
 
│ │ │ │ +ostream & gtsam::operator<< (ostream &os, const Signature::Row &row)
 
│ │ │ │ +ostream & gtsam::operator<< (ostream &os, const Signature::Table &table)
 
│ │ │ │ +ostream & gtsam::operator<< (ostream &os, const Signature &s)
 
│ │ │ │ +Signature gtsam::operator| (const DiscreteKey &key, const DiscreteKey &parent)
 Helper function to create Signature objects example: Signature s = D | E;.
 
│ │ │ │ +Signature gtsam::operator% (const DiscreteKey &key, const std::string &parent)
 Helper function to create Signature objects example: Signature s(D % "99/1");.
 
│ │ │ │ +Signature gtsam::operator% (const DiscreteKey &key, const Signature::Table &parent)
 Helper function to create Signature objects, using table construction directly example: Signature s(D % table);.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Variables

│ │ │ │ +Signature::Row gtsam::parser::F {1, 0}
 
│ │ │ │ +Signature::Row gtsam::parser::T {0, 1}
 
│ │ │ │ +struct gtsam::parser::Grammar gtsam::parser::grammar
 
│ │ │ │

Detailed Description

│ │ │ │ -

specialized key for discrete variables

│ │ │ │ +

signatures for conditional densities

│ │ │ │
Author
Frank Dellaert
│ │ │ │ -
Date
Feb 28, 2011
│ │ │ │ +
Date
Feb 27, 2011
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,39 +1,57 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -DiscreteKey.h File Reference │ │ │ │ │ -specialized key for discrete variables _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ -  _D_i_s_c_r_e_t_e_K_e_y_s is a set of keys that can be assembled using the & │ │ │ │ │ - operator. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_K_e_y_s_ _> │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s | _V_a_r_i_a_b_l_e_s │ │ │ │ │ +Signature.cpp File Reference │ │ │ │ │ +signatures for conditional densities _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ TTyyppeeddeeffss │ │ │ │ │ -using  _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y = std::pair< _K_e_y, size_t > │ │ │ │ │ -  Key type for discrete variables. │ │ │ │ │ +typedef string::const_iterator  ggttssaamm::::ppaarrsseerr::::IItt │ │ │ │ │   │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -_D_i_s_c_r_e_t_e_K_e_y_s  ggttssaamm::::ooppeerraattoorr&& (const _D_i_s_c_r_e_t_e_K_e_y &key1, const _D_i_s_c_r_e_t_e_K_e_y │ │ │ │ │ - &key2) │ │ │ │ │ -  Create a list from two keys. │ │ │ │ │ +Signature::Table  ggttssaamm::::ppaarrsseerr::::llooggiicc (bool ff, bool ft, bool tf, bool tt) │ │ │ │ │ +  │ │ │ │ │ + ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const _S_i_g_n_a_t_u_r_e_:_:_R_o_w &_r_o_w) │ │ │ │ │ +  │ │ │ │ │ + ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const Signature::Table │ │ │ │ │ + &table) │ │ │ │ │ +  │ │ │ │ │ + ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const _S_i_g_n_a_t_u_r_e &s) │ │ │ │ │ +  │ │ │ │ │ + _S_i_g_n_a_t_u_r_e  ggttssaamm::::ooppeerraattoorr|| (const _D_i_s_c_r_e_t_e_K_e_y &key, const _D_i_s_c_r_e_t_e_K_e_y │ │ │ │ │ + &parent) │ │ │ │ │ +  Helper function to create _S_i_g_n_a_t_u_r_e objects example: │ │ │ │ │ + _S_i_g_n_a_t_u_r_e s = D | E;. │ │ │ │ │ +  │ │ │ │ │ + _S_i_g_n_a_t_u_r_e  ggttssaamm::::ooppeerraattoorr%% (const _D_i_s_c_r_e_t_e_K_e_y &key, const std::string │ │ │ │ │ + &parent) │ │ │ │ │ +  Helper function to create _S_i_g_n_a_t_u_r_e objects example: │ │ │ │ │ + _S_i_g_n_a_t_u_r_e s(D % "99/1");. │ │ │ │ │ +  │ │ │ │ │ + _S_i_g_n_a_t_u_r_e  ggttssaamm::::ooppeerraattoorr%% (const _D_i_s_c_r_e_t_e_K_e_y &key, const Signature:: │ │ │ │ │ + Table &parent) │ │ │ │ │ +  Helper function to create _S_i_g_n_a_t_u_r_e objects, using table │ │ │ │ │ + construction directly example: _S_i_g_n_a_t_u_r_e s(D % table);. │ │ │ │ │ +  │ │ │ │ │ +VVaarriiaabblleess │ │ │ │ │ + _S_i_g_n_a_t_u_r_e_:_:_R_o_w  ggttssaamm::::ppaarrsseerr::::FF {1, 0} │ │ │ │ │ +  │ │ │ │ │ + _S_i_g_n_a_t_u_r_e_:_:_R_o_w  ggttssaamm::::ppaarrsseerr::::TT {0, 1} │ │ │ │ │ +  │ │ │ │ │ +struct gtsam::parser::Grammar  ggttssaamm::::ppaarrsseerr::::ggrraammmmaarr │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -specialized key for discrete variables │ │ │ │ │ +signatures for conditional densities │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - Feb 28, 2011 │ │ │ │ │ + Feb 27, 2011 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_K_e_y_._h │ │ │ │ │ + * _S_i_g_n_a_t_u_r_e_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00239.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,6 +1,5 @@ │ │ │ │ │ var a00239 = [ │ │ │ │ │ - ["gtsam::DiscreteKeys", "a02876.html", "a02876"], │ │ │ │ │ - ["gtsam::traits< DiscreteKeys >", "a02880.html", null], │ │ │ │ │ - ["DiscreteKey", "a00239.html#gad529b58f00471d8f8b25a89b48b7afc2", null], │ │ │ │ │ - ["operator&", "a00239.html#a0be4f243161c2fd937df60aee511eb16", null] │ │ │ │ │ + ["operator%", "a00239.html#ae4856250df52ce4b9b4b16c7a833408c", null], │ │ │ │ │ + ["operator%", "a00239.html#ab98cf2a4777006c28d6983db5d8ec574", null], │ │ │ │ │ + ["operator|", "a00239.html#aa6c58dfb2fc9398a026e3268ba2edfad", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00242.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteConditional.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/AlgebraicDecisionTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,41 +94,50 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Variables
│ │ │ │ -
DiscreteConditional.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
AlgebraicDecisionTree.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ + │ │ │ │ +

Algebraic Decision Trees. │ │ │ │ +More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::AlgebraicDecisionTree< L >
 An algebraic decision tree fixes the range of a DecisionTree to double. More...
 
struct  gtsam::AlgebraicDecisionTree< L >::Ring
 The Real ring with addition and multiplication. More...
 
struct  gtsam::traits< AlgebraicDecisionTree< T > >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Variables

│ │ │ │ -template class GTSAM_EXPORT gtsam::Conditional< DecisionTreeFactor, DiscreteConditional >
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Feb 14, 2011
│ │ │ │ -
Author
Duy-Nguyen Ta
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ +

Algebraic Decision Trees.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
Date
Mar 14, 2011
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,25 +1,33 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _V_a_r_i_a_b_l_e_s │ │ │ │ │ -DiscreteConditional.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +AlgebraicDecisionTree.h File Reference │ │ │ │ │ +Algebraic Decision Trees. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_<_ _L_ _> │ │ │ │ │ +  An algebraic decision tree fixes the range of a _D_e_c_i_s_i_o_n_T_r_e_e to │ │ │ │ │ + double. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_<_ _L_ _>_:_:_R_i_n_g │ │ │ │ │ +  The Real ring with addition and multiplication. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_<_ _T_ _>_ _> │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -VVaarriiaabblleess │ │ │ │ │ -template class GTSAM_EXPORT  ggttssaamm::::CCoonnddiittiioonnaall<< DDeecciissiioonnTTrreeeeFFaaccttoorr,, │ │ │ │ │ - DDiissccrreetteeCCoonnddiittiioonnaall >> │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Feb 14, 2011 │ │ │ │ │ +Algebraic Decision Trees. │ │ │ │ │ Author │ │ │ │ │ - Duy-Nguyen Ta │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + Mar 14, 2011 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_._c_p_p │ │ │ │ │ + * _A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00245.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteBayesTree.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteLookupDAG.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,37 +94,46 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
DiscreteBayesTree.cpp File Reference
│ │ │ │ +
DiscreteLookupDAG.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree. │ │ │ │ -More...

│ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::DiscreteLookupTable
 DiscreteLookupTable table for max-product. More...
 
class  gtsam::DiscreteLookupDAG
 A DAG made from lookup tables, as defined above. More...
 
struct  gtsam::traits< DiscreteLookupDAG >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree.

│ │ │ │ -

DiscreteBayesTree

Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ +
Date
January, 2022
│ │ │ │ +
Author
Frank dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,30 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -DiscreteBayesTree.cpp File Reference │ │ │ │ │ -Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +DiscreteLookupDAG.h File Reference │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_L_o_o_k_u_p_T_a_b_l_e │ │ │ │ │ +  _D_i_s_c_r_e_t_e_L_o_o_k_u_p_T_a_b_l_e table for max-product. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G │ │ │ │ │ +  A DAG made from lookup tables, as defined above. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G_ _> │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree. │ │ │ │ │ -DiscreteBayesTree │ │ │ │ │ + Date │ │ │ │ │ + January, 2022 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Frank dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e_._c_p_p │ │ │ │ │ + * _D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00248.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteLookupDAG.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteBayesNet.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,44 +96,43 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
DiscreteLookupDAG.h File Reference
│ │ │ │ +
DiscreteBayesNet.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::DiscreteLookupTable
 DiscreteLookupTable table for max-product. More...
class  gtsam::DiscreteBayesNet
 A Bayes net made from discrete conditional distributions. More...
 
class  gtsam::DiscreteLookupDAG
 A DAG made from lookup tables, as defined above. More...
 
struct  gtsam::traits< DiscreteLookupDAG >
struct  gtsam::traits< DiscreteBayesNet >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
January, 2022
│ │ │ │ -
Author
Frank dellaert
│ │ │ │ +
Date
Feb 15, 2011
│ │ │ │ +
Author
Duy-Nguyen Ta
│ │ │ │ +
│ │ │ │ +Frank dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,30 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -DiscreteLookupDAG.h File Reference │ │ │ │ │ +DiscreteBayesNet.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_L_o_o_k_u_p_T_a_b_l_e │ │ │ │ │ -  _D_i_s_c_r_e_t_e_L_o_o_k_u_p_T_a_b_l_e table for max-product. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t │ │ │ │ │ +  A Bayes net made from discrete conditional distributions. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ - class   _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G │ │ │ │ │ -  A DAG made from lookup tables, as defined above. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Date │ │ │ │ │ - January, 2022 │ │ │ │ │ + Feb 15, 2011 │ │ │ │ │ Author │ │ │ │ │ + Duy-Nguyen Ta │ │ │ │ │ Frank dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G_._h │ │ │ │ │ + * _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00248.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,3 @@ │ │ │ │ │ var a00248 = [ │ │ │ │ │ - ["gtsam::DiscreteLookupDAG", "a02888.html", "a02888"], │ │ │ │ │ - ["gtsam::traits< DiscreteLookupDAG >", "a02892.html", null] │ │ │ │ │ + ["gtsam::traits< DiscreteBayesNet >", "a02820.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00248_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteLookupDAG.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteBayesNet.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,135 +98,173 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
DiscreteLookupDAG.h
│ │ │ │ +
DiscreteBayesNet.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/discrete/DiscreteDistribution.h>
│ │ │ │ -
21#include <gtsam/inference/BayesNet.h>
│ │ │ │ -
22#include <gtsam/inference/FactorGraph.h>
│ │ │ │ -
23
│ │ │ │ -
24#include <boost/shared_ptr.hpp>
│ │ │ │ -
25#include <string>
│ │ │ │ -
26#include <utility>
│ │ │ │ -
27#include <vector>
│ │ │ │ -
28
│ │ │ │ -
29namespace gtsam {
│ │ │ │ -
30
│ │ │ │ -
31class DiscreteBayesNet;
│ │ │ │ -
32
│ │ │ │ -
│ │ │ │ -
40class GTSAM_EXPORT DiscreteLookupTable : public DiscreteConditional {
│ │ │ │ -
41 public:
│ │ │ │ -
42 using This = DiscreteLookupTable;
│ │ │ │ -
43 using shared_ptr = boost::shared_ptr<This>;
│ │ │ │ -
44 using BaseConditional = Conditional<DecisionTreeFactor, This>;
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/discrete/DiscreteConditional.h>
│ │ │ │ +
22#include <gtsam/discrete/DiscreteDistribution.h>
│ │ │ │ +
23#include <gtsam/inference/BayesNet.h>
│ │ │ │ +
24#include <gtsam/inference/FactorGraph.h>
│ │ │ │ +
25
│ │ │ │ +
26#include <boost/shared_ptr.hpp>
│ │ │ │ +
27#include <map>
│ │ │ │ +
28#include <string>
│ │ │ │ +
29#include <utility>
│ │ │ │ +
30#include <vector>
│ │ │ │ +
31
│ │ │ │ +
32namespace gtsam {
│ │ │ │ +
33
│ │ │ │ +
│ │ │ │ +
38class GTSAM_EXPORT DiscreteBayesNet: public BayesNet<DiscreteConditional> {
│ │ │ │ +
39 public:
│ │ │ │ +
40 typedef BayesNet<DiscreteConditional> Base;
│ │ │ │ +
41 typedef DiscreteBayesNet This;
│ │ │ │ +
42 typedef DiscreteConditional ConditionalType;
│ │ │ │ +
43 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
44 typedef boost::shared_ptr<ConditionalType> sharedConditional;
│ │ │ │
45
│ │ │ │ -
│ │ │ │ -
53 DiscreteLookupTable(size_t nFrontals, const DiscreteKeys& keys,
│ │ │ │ -
54 const ADT& potentials)
│ │ │ │ -
55 : DiscreteConditional(nFrontals, keys, potentials) {}
│ │ │ │ +
48
│ │ │ │ +
50 DiscreteBayesNet() {}
│ │ │ │ +
51
│ │ │ │ +
53 template <typename ITERATOR>
│ │ │ │ +
│ │ │ │ +
54 DiscreteBayesNet(ITERATOR firstConditional, ITERATOR lastConditional)
│ │ │ │ +
55 : Base(firstConditional, lastConditional) {}
│ │ │ │
│ │ │ │
56
│ │ │ │ -
58 void print(
│ │ │ │ -
59 const std::string& s = "Discrete Lookup Table: ",
│ │ │ │ -
60 const KeyFormatter& formatter = DefaultKeyFormatter) const override;
│ │ │ │ -
61
│ │ │ │ -
67 size_t argmax(const DiscreteValues& parentsValues) const;
│ │ │ │ -
68
│ │ │ │ -
73 void argmaxInPlace(DiscreteValues* parentsValues) const;
│ │ │ │ -
74};
│ │ │ │ +
58 template <class CONTAINER>
│ │ │ │ +
│ │ │ │ +
59 explicit DiscreteBayesNet(const CONTAINER& conditionals)
│ │ │ │ +
60 : Base(conditionals) {}
│ │ │ │
│ │ │ │ +
61
│ │ │ │ +
64 template <class DERIVEDCONDITIONAL>
│ │ │ │ +
│ │ │ │ +
65 DiscreteBayesNet(const FactorGraph<DERIVEDCONDITIONAL>& graph)
│ │ │ │ +
66 : Base(graph) {}
│ │ │ │ +
│ │ │ │ +
67
│ │ │ │ +
69 virtual ~DiscreteBayesNet() {}
│ │ │ │ +
70
│ │ │ │ +
72
│ │ │ │
75
│ │ │ │ -
│ │ │ │ -
77class GTSAM_EXPORT DiscreteLookupDAG : public BayesNet<DiscreteLookupTable> {
│ │ │ │ -
78 public:
│ │ │ │ -
79 using Base = BayesNet<DiscreteLookupTable>;
│ │ │ │ -
80 using This = DiscreteLookupDAG;
│ │ │ │ -
81 using shared_ptr = boost::shared_ptr<This>;
│ │ │ │ -
82
│ │ │ │ -
85
│ │ │ │ -
87 DiscreteLookupDAG() {}
│ │ │ │ -
88
│ │ │ │ -
90 static DiscreteLookupDAG FromBayesNet(const DiscreteBayesNet& bayesNet);
│ │ │ │ +
77 bool equals(const This& bn, double tol = 1e-9) const;
│ │ │ │ +
78
│ │ │ │ +
80
│ │ │ │ +
83
│ │ │ │ +
84 // Add inherited versions of add.
│ │ │ │ +
85 using Base::add;
│ │ │ │ +
86
│ │ │ │ +
│ │ │ │ +
88 void add(const DiscreteKey& key, const std::string& spec) {
│ │ │ │ +
89 emplace_shared<DiscreteDistribution>(key, spec);
│ │ │ │ +
90 }
│ │ │ │ +
│ │ │ │
91
│ │ │ │ -
93 virtual ~DiscreteLookupDAG() {}
│ │ │ │ -
94
│ │ │ │ -
96
│ │ │ │ -
99
│ │ │ │ -
101 bool equals(const This& bn, double tol = 1e-9) const;
│ │ │ │ -
102
│ │ │ │ -
104
│ │ │ │ -
107
│ │ │ │ -
109 template <typename... Args>
│ │ │ │ -
│ │ │ │ -
110 void add(Args&&... args) {
│ │ │ │ -
111 emplace_shared<DiscreteLookupTable>(std::forward<Args>(args)...);
│ │ │ │ -
112 }
│ │ │ │ -
│ │ │ │ -
113
│ │ │ │ -
125 DiscreteValues argmax(DiscreteValues given = DiscreteValues()) const;
│ │ │ │ -
127
│ │ │ │ -
128 private:
│ │ │ │ -
130 friend class boost::serialization::access;
│ │ │ │ -
131 template <class ARCHIVE>
│ │ │ │ -
132 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ -
133 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
134 }
│ │ │ │ -
135};
│ │ │ │ -
│ │ │ │ -
136
│ │ │ │ -
137// traits
│ │ │ │ -
138template <>
│ │ │ │ -
139struct traits<DiscreteLookupDAG> : public Testable<DiscreteLookupDAG> {};
│ │ │ │ -
140
│ │ │ │ -
141} // namespace gtsam
│ │ │ │ - │ │ │ │ -
FactorGraph.h
Factor Graph Base Class.
│ │ │ │ -
BayesNet.h
Bayes network.
│ │ │ │ +
93 template <typename... Args>
│ │ │ │ +
│ │ │ │ +
94 void add(Args&&... args) {
│ │ │ │ +
95 emplace_shared<DiscreteConditional>(std::forward<Args>(args)...);
│ │ │ │ +
96 }
│ │ │ │ +
│ │ │ │ +
97
│ │ │ │ +
98 //** evaluate for given DiscreteValues */
│ │ │ │ +
99 double evaluate(const DiscreteValues & values) const;
│ │ │ │ +
100
│ │ │ │ +
101 //** (Preferred) sugar for the above for given DiscreteValues */
│ │ │ │ +
102 double operator()(const DiscreteValues & values) const {
│ │ │ │ +
103 return evaluate(values);
│ │ │ │ +
104 }
│ │ │ │ +
105
│ │ │ │ +
106 //** log(evaluate(values)) for given DiscreteValues */
│ │ │ │ +
107 double logProbability(const DiscreteValues & values) const;
│ │ │ │ +
108
│ │ │ │ +
118 DiscreteValues sample() const;
│ │ │ │ +
119
│ │ │ │ +
128 DiscreteValues sample(DiscreteValues given) const;
│ │ │ │ +
129
│ │ │ │ +
133
│ │ │ │ +
135 std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
136 const DiscreteFactor::Names& names = {}) const;
│ │ │ │ +
137
│ │ │ │ +
139 std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
140 const DiscreteFactor::Names& names = {}) const;
│ │ │ │ +
141
│ │ │ │ +
145
│ │ │ │ +
146 using Base::error; // Expose error(const HybridValues&) method..
│ │ │ │ +
147 using Base::evaluate; // Expose evaluate(const HybridValues&) method..
│ │ │ │ +
148 using Base::logProbability; // Expose logProbability(const HybridValues&)
│ │ │ │ +
149
│ │ │ │ +
151
│ │ │ │ +
152#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ +
155
│ │ │ │ +
156 DiscreteValues GTSAM_DEPRECATED optimize() const;
│ │ │ │ +
157 DiscreteValues GTSAM_DEPRECATED optimize(DiscreteValues given) const;
│ │ │ │ +
159#endif
│ │ │ │ +
160
│ │ │ │ +
161 private:
│ │ │ │ +
163 friend class boost::serialization::access;
│ │ │ │ +
164 template<class ARCHIVE>
│ │ │ │ +
165 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
166 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
167 }
│ │ │ │ +
168 };
│ │ │ │ +
│ │ │ │ +
169
│ │ │ │ +
170// traits
│ │ │ │ +
171template<> struct traits<DiscreteBayesNet> : public Testable<DiscreteBayesNet> {};
│ │ │ │ +
172
│ │ │ │ +
173} // \ namespace gtsam
│ │ │ │ +
174
│ │ │ │ + │ │ │ │ + │ │ │ │ +
FactorGraph.h
Factor Graph Base Class.
│ │ │ │ +
BayesNet.h
Bayes network.
│ │ │ │ +
gtsam::DiscreteKey
std::pair< Key, size_t > DiscreteKey
Key type for discrete variables.
Definition DiscreteKey.h:36
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::html
string html(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
Free version of html.
Definition DiscreteValues.cpp:134
│ │ │ │ +
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ - │ │ │ │
gtsam::DiscreteBayesNet
A Bayes net made from discrete conditional distributions.
Definition DiscreteBayesNet.h:38
│ │ │ │ +
gtsam::DiscreteBayesNet::DiscreteBayesNet
DiscreteBayesNet(ITERATOR firstConditional, ITERATOR lastConditional)
Construct from iterator over conditionals.
Definition DiscreteBayesNet.h:54
│ │ │ │ +
gtsam::DiscreteBayesNet::DiscreteBayesNet
DiscreteBayesNet(const FactorGraph< DERIVEDCONDITIONAL > &graph)
Implicit copy/downcast constructor to override explicit template container constructor.
Definition DiscreteBayesNet.h:65
│ │ │ │ +
gtsam::DiscreteBayesNet::DiscreteBayesNet
DiscreteBayesNet(const CONTAINER &conditionals)
Construct from container of factors (shared_ptr or plain objects)
Definition DiscreteBayesNet.h:59
│ │ │ │ +
gtsam::DiscreteBayesNet::DiscreteBayesNet
DiscreteBayesNet()
Construct empty Bayes net.
Definition DiscreteBayesNet.h:50
│ │ │ │ +
gtsam::DiscreteBayesNet::~DiscreteBayesNet
virtual ~DiscreteBayesNet()
Destructor.
Definition DiscreteBayesNet.h:69
│ │ │ │ +
gtsam::DiscreteBayesNet::add
void add(Args &&... args)
Add a DiscreteCondtional.
Definition DiscreteBayesNet.h:94
│ │ │ │ +
gtsam::DiscreteBayesNet::add
void add(const DiscreteKey &key, const std::string &spec)
Add a DiscreteDistribution using a table or a string.
Definition DiscreteBayesNet.h:88
│ │ │ │
gtsam::DiscreteConditional
Discrete Conditional Density Derives from DecisionTreeFactor.
Definition DiscreteConditional.h:40
│ │ │ │ -
gtsam::DiscreteKeys
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition DiscreteKey.h:39
│ │ │ │ -
gtsam::DiscreteLookupTable
DiscreteLookupTable table for max-product.
Definition DiscreteLookupDAG.h:40
│ │ │ │ -
gtsam::DiscreteLookupTable::DiscreteLookupTable
DiscreteLookupTable(size_t nFrontals, const DiscreteKeys &keys, const ADT &potentials)
Construct a new Discrete Lookup Table object.
Definition DiscreteLookupDAG.h:53
│ │ │ │ -
gtsam::DiscreteLookupDAG
A DAG made from lookup tables, as defined above.
Definition DiscreteLookupDAG.h:77
│ │ │ │ -
gtsam::DiscreteLookupDAG::equals
bool equals(const This &bn, double tol=1e-9) const
Check equality.
│ │ │ │ -
gtsam::DiscreteLookupDAG::DiscreteLookupDAG
DiscreteLookupDAG()
Construct empty DAG.
Definition DiscreteLookupDAG.h:87
│ │ │ │ -
gtsam::DiscreteLookupDAG::add
void add(Args &&... args)
Add a DiscreteLookupTable.
Definition DiscreteLookupDAG.h:110
│ │ │ │ -
gtsam::DiscreteLookupDAG::~DiscreteLookupDAG
virtual ~DiscreteLookupDAG()
Destructor.
Definition DiscreteLookupDAG.h:93
│ │ │ │
gtsam::DiscreteValues
A map from keys to values.
Definition DiscreteValues.h:34
│ │ │ │
gtsam::BayesNet
A BayesNet is a tree of conditionals, stored in elimination order.
Definition BayesNet.h:35
│ │ │ │ -
gtsam::Conditional
Definition Conditional.h:64
│ │ │ │ +
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ +
DiscreteValues
The Factor::error simply extracts the.
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,164 +1,211 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DiscreteLookupDAG.h │ │ │ │ │ +DiscreteBayesNet.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -23 │ │ │ │ │ -24#include │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ -28 │ │ │ │ │ -29namespace _g_t_s_a_m { │ │ │ │ │ -30 │ │ │ │ │ -31class DiscreteBayesNet; │ │ │ │ │ -32 │ │ │ │ │ -_4_0class GTSAM_EXPORT _D_i_s_c_r_e_t_e_L_o_o_k_u_p_T_a_b_l_e : public _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l { │ │ │ │ │ -41 public: │ │ │ │ │ -42 using _T_h_i_s = _D_i_s_c_r_e_t_e_L_o_o_k_u_p_T_a_b_l_e; │ │ │ │ │ -43 using shared_ptr = boost::shared_ptr; │ │ │ │ │ -44 using BaseConditional = _C_o_n_d_i_t_i_o_n_a_l_<_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_,_ _T_h_i_s_>; │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +25 │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28#include │ │ │ │ │ +29#include │ │ │ │ │ +30#include │ │ │ │ │ +31 │ │ │ │ │ +32namespace _g_t_s_a_m { │ │ │ │ │ +33 │ │ │ │ │ +_3_8class GTSAM_EXPORT _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t: public _B_a_y_e_s_N_e_t { │ │ │ │ │ +39 public: │ │ │ │ │ +40 typedef _B_a_y_e_s_N_e_t_<_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_> _B_a_s_e; │ │ │ │ │ +41 typedef _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t _T_h_i_s; │ │ │ │ │ +42 typedef _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l _C_o_n_d_i_t_i_o_n_a_l_T_y_p_e; │ │ │ │ │ +43 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +44 typedef boost::shared_ptr sharedConditional; │ │ │ │ │ 45 │ │ │ │ │ -_5_3 _D_i_s_c_r_e_t_e_L_o_o_k_u_p_T_a_b_l_e(size_t nFrontals, const _D_i_s_c_r_e_t_e_K_e_y_s& keys, │ │ │ │ │ -54 const _A_D_T& potentials) │ │ │ │ │ -55 : _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l(nFrontals, keys, potentials) {} │ │ │ │ │ +48 │ │ │ │ │ +_5_0 _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t() {} │ │ │ │ │ +51 │ │ │ │ │ +53 template │ │ │ │ │ +_5_4 _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t(ITERATOR firstConditional, ITERATOR lastConditional) │ │ │ │ │ +55 : _B_a_s_e(firstConditional, lastConditional) {} │ │ │ │ │ 56 │ │ │ │ │ -58 void _p_r_i_n_t( │ │ │ │ │ -59 const std::string& s = "Discrete Lookup Table: ", │ │ │ │ │ -60 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override; │ │ │ │ │ +58 template │ │ │ │ │ +_5_9 explicit _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t(const CONTAINER& conditionals) │ │ │ │ │ +60 : _B_a_s_e(conditionals) {} │ │ │ │ │ 61 │ │ │ │ │ -67 size_t argmax(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& parentsValues) const; │ │ │ │ │ -68 │ │ │ │ │ -73 void argmaxInPlace(_D_i_s_c_r_e_t_e_V_a_l_u_e_s* parentsValues) const; │ │ │ │ │ -74}; │ │ │ │ │ +64 template │ │ │ │ │ +_6_5 _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t(const _F_a_c_t_o_r_G_r_a_p_h_<_D_E_R_I_V_E_D_C_O_N_D_I_T_I_O_N_A_L_>& graph) │ │ │ │ │ +66 : _B_a_s_e(graph) {} │ │ │ │ │ +67 │ │ │ │ │ +_6_9 virtual _~_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t() {} │ │ │ │ │ +70 │ │ │ │ │ +72 │ │ │ │ │ 75 │ │ │ │ │ -_7_7class GTSAM_EXPORT _D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G : public _B_a_y_e_s_N_e_t { │ │ │ │ │ -78 public: │ │ │ │ │ -79 using _B_a_s_e = _B_a_y_e_s_N_e_t_<_D_i_s_c_r_e_t_e_L_o_o_k_u_p_T_a_b_l_e_>; │ │ │ │ │ -80 using _T_h_i_s = _D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G; │ │ │ │ │ -81 using shared_ptr = boost::shared_ptr; │ │ │ │ │ -82 │ │ │ │ │ -85 │ │ │ │ │ -_8_7 _D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G() {} │ │ │ │ │ -88 │ │ │ │ │ -90 static _D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G FromBayesNet(const _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t& bayesNet); │ │ │ │ │ +77 bool _e_q_u_a_l_s(const This& bn, double tol = 1e-9) const; │ │ │ │ │ +78 │ │ │ │ │ +80 │ │ │ │ │ +83 │ │ │ │ │ +84 // Add inherited versions of add. │ │ │ │ │ +85 using Base::add; │ │ │ │ │ +86 │ │ │ │ │ +_8_8 void _a_d_d(const _D_i_s_c_r_e_t_e_K_e_y& key, const std::string& spec) { │ │ │ │ │ +89 emplace_shared(key, spec); │ │ │ │ │ +90 } │ │ │ │ │ 91 │ │ │ │ │ -_9_3 virtual _~_D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G() {} │ │ │ │ │ -94 │ │ │ │ │ -96 │ │ │ │ │ -99 │ │ │ │ │ -_1_0_1 bool _e_q_u_a_l_s(const _T_h_i_s& bn, double tol = 1e-9) const; │ │ │ │ │ -102 │ │ │ │ │ -104 │ │ │ │ │ -107 │ │ │ │ │ -109 template │ │ │ │ │ -_1_1_0 void _a_d_d(Args&&... args) { │ │ │ │ │ -111 emplace_shared(std::forward(args)...); │ │ │ │ │ -112 } │ │ │ │ │ -113 │ │ │ │ │ -125 _D_i_s_c_r_e_t_e_V_a_l_u_e_s argmax(_D_i_s_c_r_e_t_e_V_a_l_u_e_s given = _D_i_s_c_r_e_t_e_V_a_l_u_e_s()) const; │ │ │ │ │ -127 │ │ │ │ │ -128 private: │ │ │ │ │ -_1_3_0 friend class boost::serialization::access; │ │ │ │ │ -131 template │ │ │ │ │ -132 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ -133 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -134 } │ │ │ │ │ -135}; │ │ │ │ │ -136 │ │ │ │ │ -137// traits │ │ │ │ │ -138template <> │ │ │ │ │ -_1_3_9struct _t_r_a_i_t_s<_D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ -140 │ │ │ │ │ -141} // namespace gtsam │ │ │ │ │ +93 template │ │ │ │ │ +_9_4 void _a_d_d(Args&&... args) { │ │ │ │ │ +95 emplace_shared(std::forward(args)...); │ │ │ │ │ +96 } │ │ │ │ │ +97 │ │ │ │ │ +98 //** evaluate for given DiscreteValues */ │ │ │ │ │ +99 double evaluate(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s & values) const; │ │ │ │ │ +100 │ │ │ │ │ +101 //** (Preferred) sugar for the above for given DiscreteValues */ │ │ │ │ │ +102 double operator()(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s & values) const { │ │ │ │ │ +103 return evaluate(values); │ │ │ │ │ +104 } │ │ │ │ │ +105 │ │ │ │ │ +106 //** log(evaluate(values)) for given DiscreteValues */ │ │ │ │ │ +107 double logProbability(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s & values) const; │ │ │ │ │ +108 │ │ │ │ │ +118 _D_i_s_c_r_e_t_e_V_a_l_u_e_s sample() const; │ │ │ │ │ +119 │ │ │ │ │ +128 _D_i_s_c_r_e_t_e_V_a_l_u_e_s sample(_D_i_s_c_r_e_t_e_V_a_l_u_e_s given) const; │ │ │ │ │ +129 │ │ │ │ │ +133 │ │ │ │ │ +135 std::string markdown(const KeyFormatter& keyFormatter = │ │ │ │ │ +DefaultKeyFormatter, │ │ │ │ │ +136 const DiscreteFactor::Names& names = {}) const; │ │ │ │ │ +137 │ │ │ │ │ +139 std::string _h_t_m_l(const KeyFormatter& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ +140 const DiscreteFactor::Names& names = {}) const; │ │ │ │ │ +141 │ │ │ │ │ +145 │ │ │ │ │ +146 using Base::error; // Expose error(const HybridValues&) method.. │ │ │ │ │ +147 using Base::evaluate; // Expose evaluate(const HybridValues&) method.. │ │ │ │ │ +148 using Base::logProbability; // Expose logProbability(const HybridValues&) │ │ │ │ │ +149 │ │ │ │ │ +151 │ │ │ │ │ +152#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ +155 │ │ │ │ │ +156 _D_i_s_c_r_e_t_e_V_a_l_u_e_s GTSAM_DEPRECATED _o_p_t_i_m_i_z_e() const; │ │ │ │ │ +157 _D_i_s_c_r_e_t_e_V_a_l_u_e_s GTSAM_DEPRECATED _o_p_t_i_m_i_z_e(_D_i_s_c_r_e_t_e_V_a_l_u_e_s given) const; │ │ │ │ │ +159#endif │ │ │ │ │ +160 │ │ │ │ │ +161 private: │ │ │ │ │ +_1_6_3 friend class boost::serialization::access; │ │ │ │ │ +164 template │ │ │ │ │ +165 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +166 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +167 } │ │ │ │ │ +168 }; │ │ │ │ │ +169 │ │ │ │ │ +170// traits │ │ │ │ │ +_1_7_1template<> struct _t_r_a_i_t_s<_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t> : public │ │ │ │ │ +_T_e_s_t_a_b_l_e {}; │ │ │ │ │ +172 │ │ │ │ │ +173} // \ namespace gtsam │ │ │ │ │ +174 │ │ │ │ │ _D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n_._h │ │ │ │ │ +_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ _F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ Factor Graph Base Class. │ │ │ │ │ _B_a_y_e_s_N_e_t_._h │ │ │ │ │ Bayes network. │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y │ │ │ │ │ +std::pair< Key, size_t > DiscreteKey │ │ │ │ │ +Key type for discrete variables. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteKey.h:36 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_h_t_m_l │ │ │ │ │ +string html(const DiscreteValues &values, const KeyFormatter &keyFormatter, │ │ │ │ │ +const DiscreteValues::Names &names) │ │ │ │ │ +Free version of html. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteValues.cpp:134 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ +Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key │ │ │ │ │ +landmarkKey) │ │ │ │ │ +Optimize for triangulation. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.cpp:155 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ _g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ A helper that implements the traits interface for GTSAM types. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_<_ _K_e_y_ _> │ │ │ │ │ _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t │ │ │ │ │ A Bayes net made from discrete conditional distributions. │ │ │ │ │ DDeeffiinniittiioonn DiscreteBayesNet.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t │ │ │ │ │ +DiscreteBayesNet(ITERATOR firstConditional, ITERATOR lastConditional) │ │ │ │ │ +Construct from iterator over conditionals. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteBayesNet.h:54 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t │ │ │ │ │ +DiscreteBayesNet(const FactorGraph< DERIVEDCONDITIONAL > &graph) │ │ │ │ │ +Implicit copy/downcast constructor to override explicit template container │ │ │ │ │ +constructor. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteBayesNet.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t │ │ │ │ │ +DiscreteBayesNet(const CONTAINER &conditionals) │ │ │ │ │ +Construct from container of factors (shared_ptr or plain objects) │ │ │ │ │ +DDeeffiinniittiioonn DiscreteBayesNet.h:59 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t │ │ │ │ │ +DiscreteBayesNet() │ │ │ │ │ +Construct empty Bayes net. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteBayesNet.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_:_:_~_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t │ │ │ │ │ +virtual ~DiscreteBayesNet() │ │ │ │ │ +Destructor. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteBayesNet.h:69 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_:_:_a_d_d │ │ │ │ │ +void add(Args &&... args) │ │ │ │ │ +Add a DiscreteCondtional. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteBayesNet.h:94 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_:_:_a_d_d │ │ │ │ │ +void add(const DiscreteKey &key, const std::string &spec) │ │ │ │ │ +Add a DiscreteDistribution using a table or a string. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteBayesNet.h:88 │ │ │ │ │ _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ Discrete Conditional Density Derives from DecisionTreeFactor. │ │ │ │ │ DDeeffiinniittiioonn DiscreteConditional.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ -DiscreteKeys is a set of keys that can be assembled using the & operator. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteKey.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_L_o_o_k_u_p_T_a_b_l_e │ │ │ │ │ -DiscreteLookupTable table for max-product. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteLookupDAG.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_L_o_o_k_u_p_T_a_b_l_e_:_:_D_i_s_c_r_e_t_e_L_o_o_k_u_p_T_a_b_l_e │ │ │ │ │ -DiscreteLookupTable(size_t nFrontals, const DiscreteKeys &keys, const ADT │ │ │ │ │ -&potentials) │ │ │ │ │ -Construct a new Discrete Lookup Table object. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteLookupDAG.h:53 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G │ │ │ │ │ -A DAG made from lookup tables, as defined above. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteLookupDAG.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const This &bn, double tol=1e-9) const │ │ │ │ │ -Check equality. │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G_:_:_D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G │ │ │ │ │ -DiscreteLookupDAG() │ │ │ │ │ -Construct empty DAG. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteLookupDAG.h:87 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G_:_:_a_d_d │ │ │ │ │ -void add(Args &&... args) │ │ │ │ │ -Add a DiscreteLookupTable. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteLookupDAG.h:110 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G_:_:_~_D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G │ │ │ │ │ -virtual ~DiscreteLookupDAG() │ │ │ │ │ -Destructor. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteLookupDAG.h:93 │ │ │ │ │ _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ A map from keys to values. │ │ │ │ │ DDeeffiinniittiioonn DiscreteValues.h:34 │ │ │ │ │ _g_t_s_a_m_:_:_B_a_y_e_s_N_e_t │ │ │ │ │ A BayesNet is a tree of conditionals, stored in elimination order. │ │ │ │ │ DDeeffiinniittiioonn BayesNet.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ +nodes. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ +_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ +The Factor::error simply extracts the. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G_._h │ │ │ │ │ + * _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00251.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DecisionTreeFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteValues.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,45 +94,44 @@ │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
DecisionTreeFactor.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
DiscreteValues.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::DecisionTreeFactor
 A discrete probabilistic factor. More...
 
struct  gtsam::traits< DecisionTreeFactor >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +string gtsam::markdown (const DiscreteValues &values, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DiscreteValues::Names &names={})
 Free version of markdown.
 
│ │ │ │ +string gtsam::html (const DiscreteValues &values, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DiscreteValues::Names &names={})
 Free version of html.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Feb 14, 2011
│ │ │ │ -
Author
Duy-Nguyen Ta
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ +
Date
January, 2022
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,31 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -DecisionTreeFactor.h File Reference │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r │ │ │ │ │ -  A discrete probabilistic factor. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_ _> │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +DiscreteValues.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +string  ggttssaamm::::mmaarrkkddoowwnn (const _D_i_s_c_r_e_t_e_V_a_l_u_e_s &values, const _K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ + &keyFormatter=DefaultKeyFormatter, const _D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_N_a_m_e_s &names= │ │ │ │ │ + {}) │ │ │ │ │ +  Free version of markdown. │ │ │ │ │ +  │ │ │ │ │ +string  ggttssaamm::::hhttmmll (const _D_i_s_c_r_e_t_e_V_a_l_u_e_s &values, const _K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ + &keyFormatter=DefaultKeyFormatter, const _D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_N_a_m_e_s &names= │ │ │ │ │ + {}) │ │ │ │ │ +  Free version of html. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Date │ │ │ │ │ - Feb 14, 2011 │ │ │ │ │ + January, 2022 │ │ │ │ │ Author │ │ │ │ │ - Duy-Nguyen Ta │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_._h │ │ │ │ │ + * _D_i_s_c_r_e_t_e_V_a_l_u_e_s_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00251.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,4 @@ │ │ │ │ │ var a00251 = [ │ │ │ │ │ - ["gtsam::traits< DecisionTreeFactor >", "a02812.html", null] │ │ │ │ │ + ["html", "a00251.html#a147114735037c2260d4810a8820e72f6", null], │ │ │ │ │ + ["markdown", "a00251.html#a2b6707b04064df79d26d1016dfa2a013", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00257.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/AlgebraicDecisionTree.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DecisionTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,36 +94,74 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces
│ │ │ │ -
AlgebraicDecisionTree.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
DecisionTree.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ + │ │ │ │ +

Decision Tree for use in DiscreteFactors. │ │ │ │ +More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::DecisionTree< L, Y >
 a decision tree is a function from assignments to values. More...
 
struct  gtsam::DecisionTree< L, Y >::Node
 ---------------------— Node base class ------------------------— More...
 
struct  gtsam::traits< DecisionTree< L, Y > >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

template<typename L , typename Y >
DecisionTree< L, Y > gtsam::apply (const DecisionTree< L, Y > &f, const typename DecisionTree< L, Y >::Unary &op)
 free versions of apply
 
│ │ │ │ +template<typename L , typename Y >
DecisionTree< L, Y > gtsam::apply (const DecisionTree< L, Y > &f, const typename DecisionTree< L, Y >::UnaryAssignment &op)
 Apply unary operator op with Assignment to DecisionTree f.
 
│ │ │ │ +template<typename L , typename Y >
DecisionTree< L, Y > gtsam::apply (const DecisionTree< L, Y > &f, const DecisionTree< L, Y > &g, const typename DecisionTree< L, Y >::Binary &op)
 Apply binary operator op to DecisionTree f.
 
template<typename L , typename T1 , typename T2 >
std::pair< DecisionTree< L, T1 >, DecisionTree< L, T2 > > gtsam::unzip (const DecisionTree< L, std::pair< T1, T2 > > &input)
 unzip a DecisionTree with std::pair values.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Feb 20, 2022
│ │ │ │ -
Author
Mike Sheffler
│ │ │ │ -
│ │ │ │ -Duy-Nguyen Ta
│ │ │ │ +

Decision Tree for use in DiscreteFactors.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │ -Frank Dellaert
│ │ │ │ +Can Erdogan
│ │ │ │ +
Date
Jan 30, 2012
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── encoding │ │ │ │ │ @@ -1 +1 @@ │ │ │ │ │ -us-ascii │ │ │ │ │ +utf-8 │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,59 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -AlgebraicDecisionTree.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +DecisionTree.h File Reference │ │ │ │ │ +Decision Tree for use in DiscreteFactors. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_<_ _L_,_ _Y_ _> │ │ │ │ │ +  a decision tree is a function from assignments to values. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_<_ _L_,_ _Y_ _>_:_:_N_o_d_e │ │ │ │ │ +  ---------------------— _N_o_d_e base class ------------------------ │ │ │ │ │ + — _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _D_e_c_i_s_i_o_n_T_r_e_e_<_ _L_,_ _Y_ _>_ _> │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +template │ │ │ │ │ + _D_e_c_i_s_i_o_n_T_r_e_e< L, Y >  _g_t_s_a_m_:_:_a_p_p_l_y (const _D_e_c_i_s_i_o_n_T_r_e_e< L, Y > &f, │ │ │ │ │ + const typename _D_e_c_i_s_i_o_n_T_r_e_e< L, Y >::Unary │ │ │ │ │ + &op) │ │ │ │ │ +  free versions of apply │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _D_e_c_i_s_i_o_n_T_r_e_e< L, Y >  ggttssaamm::::aappppllyy (const _D_e_c_i_s_i_o_n_T_r_e_e< L, Y > &f, │ │ │ │ │ + const typename _D_e_c_i_s_i_o_n_T_r_e_e< L, Y >:: │ │ │ │ │ + UnaryAssignment &op) │ │ │ │ │ +  Apply unary operator op with _A_s_s_i_g_n_m_e_n_t to │ │ │ │ │ + _D_e_c_i_s_i_o_n_T_r_e_e f. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _D_e_c_i_s_i_o_n_T_r_e_e< L, Y >  ggttssaamm::::aappppllyy (const _D_e_c_i_s_i_o_n_T_r_e_e< L, Y > &f, │ │ │ │ │ + const _D_e_c_i_s_i_o_n_T_r_e_e< L, Y > &g, const typename │ │ │ │ │ + _D_e_c_i_s_i_o_n_T_r_e_e< L, Y >::Binary &op) │ │ │ │ │ +  Apply binary operator op to _D_e_c_i_s_i_o_n_T_r_e_e f. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +std::pair< _D_e_c_i_s_i_o_n_T_r_e_e< L, T1 >, _g_t_s_a_m_:_:_u_n_z_i_p (const _D_e_c_i_s_i_o_n_T_r_e_e< L, std:: │ │ │ │ │ + _D_e_c_i_s_i_o_n_T_r_e_e< L, T2 > >  pair< T1, T2 > > &input) │ │ │ │ │ +  unzip a _D_e_c_i_s_i_o_n_T_r_e_e with std::pair values. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Feb 20, 2022 │ │ │ │ │ +Decision Tree for use in DiscreteFactors. │ │ │ │ │ Author │ │ │ │ │ - Mike Sheffler │ │ │ │ │ - Duy-Nguyen Ta │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Can Erdogan │ │ │ │ │ + Date │ │ │ │ │ + Jan 30, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_._c_p_p │ │ │ │ │ + * _D_e_c_i_s_i_o_n_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00260.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteEliminationTree.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteKey.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,42 +95,61 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
DiscreteEliminationTree.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ +Functions
│ │ │ │ +
DiscreteKey.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ +

specialized key for discrete variables │ │ │ │ +More...

│ │ │ │ + │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::DiscreteEliminationTree
 Elimination tree for discrete factors. More...
struct  gtsam::DiscreteKeys
 DiscreteKeys is a set of keys that can be assembled using the & operator. More...
 
struct  gtsam::traits< DiscreteKeys >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Typedefs

using gtsam::DiscreteKey = std::pair< Key, size_t >
 Key type for discrete variables.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +DiscreteKeys gtsam::operator& (const DiscreteKey &key1, const DiscreteKey &key2)
 Create a list from two keys.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Mar 29, 2013
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ +

specialized key for discrete variables

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
Date
Feb 28, 2011
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,26 +1,39 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -DiscreteEliminationTree.h File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +DiscreteKey.h File Reference │ │ │ │ │ +specialized key for discrete variables _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ -  Elimination tree for discrete factors. _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ +  _D_i_s_c_r_e_t_e_K_e_y_s is a set of keys that can be assembled using the & │ │ │ │ │ + operator. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_K_e_y_s_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +using  _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y = std::pair< _K_e_y, size_t > │ │ │ │ │ +  Key type for discrete variables. │ │ │ │ │ +  │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +_D_i_s_c_r_e_t_e_K_e_y_s  ggttssaamm::::ooppeerraattoorr&& (const _D_i_s_c_r_e_t_e_K_e_y &key1, const _D_i_s_c_r_e_t_e_K_e_y │ │ │ │ │ + &key2) │ │ │ │ │ +  Create a list from two keys. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Mar 29, 2013 │ │ │ │ │ +specialized key for discrete variables │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Feb 28, 2011 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h │ │ │ │ │ + * _D_i_s_c_r_e_t_e_K_e_y_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00260_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteEliminationTree.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteKey.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,15 +98,15 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
DiscreteEliminationTree.h
│ │ │ │ +
DiscreteKey.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │ @@ -114,62 +114,105 @@ │ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/discrete/DiscreteBayesNet.h>
│ │ │ │ -
22#include <gtsam/discrete/DiscreteFactorGraph.h>
│ │ │ │ -
23#include <gtsam/inference/EliminationTree.h>
│ │ │ │ -
24
│ │ │ │ -
25namespace gtsam {
│ │ │ │ -
26
│ │ │ │ -
│ │ │ │ -
31 class GTSAM_EXPORT DiscreteEliminationTree :
│ │ │ │ -
32 public EliminationTree<DiscreteBayesNet, DiscreteFactorGraph>
│ │ │ │ -
33 {
│ │ │ │ -
34 public:
│ │ │ │ -
35 typedef EliminationTree<DiscreteBayesNet, DiscreteFactorGraph> Base;
│ │ │ │ -
36 typedef DiscreteEliminationTree This;
│ │ │ │ -
37 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
38
│ │ │ │ -
47 DiscreteEliminationTree(const DiscreteFactorGraph& factorGraph,
│ │ │ │ -
48 const VariableIndex& structure, const Ordering& order);
│ │ │ │ +
21#include <gtsam/global_includes.h>
│ │ │ │ +
22#include <gtsam/inference/Key.h>
│ │ │ │ +
23
│ │ │ │ +
24#include <boost/serialization/vector.hpp>
│ │ │ │ +
25#include <map>
│ │ │ │ +
26#include <string>
│ │ │ │ +
27#include <vector>
│ │ │ │ +
28
│ │ │ │ +
29namespace gtsam {
│ │ │ │ +
30
│ │ │ │ +
36 using DiscreteKey = std::pair<Key,size_t>;
│ │ │ │ +
37
│ │ │ │ +
│ │ │ │ +
39 struct GTSAM_EXPORT DiscreteKeys: public std::vector<DiscreteKey> {
│ │ │ │ +
40
│ │ │ │ +
41 // Forward all constructors.
│ │ │ │ +
42 using std::vector<DiscreteKey>::vector;
│ │ │ │ +
43
│ │ │ │ +
45 DiscreteKeys() : std::vector<DiscreteKey>::vector() {}
│ │ │ │ +
46
│ │ │ │ +
48 explicit DiscreteKeys(const DiscreteKey& key) { push_back(key); }
│ │ │ │
49
│ │ │ │ -
55 DiscreteEliminationTree(const DiscreteFactorGraph& factorGraph,
│ │ │ │ -
56 const Ordering& order);
│ │ │ │ -
57
│ │ │ │ -
59 bool equals(const This& other, double tol = 1e-9) const;
│ │ │ │ -
60
│ │ │ │ -
61 private:
│ │ │ │ +
│ │ │ │ +
51 explicit DiscreteKeys(std::map<Key, size_t> cardinalities) {
│ │ │ │ +
52 for (auto&& kv : cardinalities) emplace_back(kv);
│ │ │ │ +
53 }
│ │ │ │ +
│ │ │ │ +
54
│ │ │ │ +
│ │ │ │ +
56 DiscreteKeys(const std::vector<DiscreteKey>& keys) :
│ │ │ │ +
57 std::vector<DiscreteKey>(keys) {
│ │ │ │ +
58 }
│ │ │ │ +
│ │ │ │ +
59
│ │ │ │ +
61 DiscreteKeys(const std::vector<int>& cs);
│ │ │ │
62
│ │ │ │ -
63 friend class ::EliminationTreeTester;
│ │ │ │ -
64
│ │ │ │ -
65 };
│ │ │ │ -
│ │ │ │ -
66
│ │ │ │ -
67}
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ +
64 KeyVector indices() const;
│ │ │ │ +
65
│ │ │ │ +
67 std::map<Key,size_t> cardinalities() const;
│ │ │ │ +
68
│ │ │ │ +
│ │ │ │ +
70 DiscreteKeys& operator&(const DiscreteKey& key) {
│ │ │ │ +
71 push_back(key);
│ │ │ │ +
72 return *this;
│ │ │ │ +
73 }
│ │ │ │ +
│ │ │ │ +
74
│ │ │ │ +
76 void print(const std::string& s = "",
│ │ │ │ +
77 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ +
78
│ │ │ │ +
80 bool equals(const DiscreteKeys& other, double tol = 0) const;
│ │ │ │ +
81
│ │ │ │ +
83 friend class boost::serialization::access;
│ │ │ │ +
84 template <class ARCHIVE>
│ │ │ │ +
85 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ +
86 ar& boost::serialization::make_nvp(
│ │ │ │ +
87 "DiscreteKeys",
│ │ │ │ +
88 boost::serialization::base_object<std::vector<DiscreteKey>>(*this));
│ │ │ │ +
89 }
│ │ │ │ +
90
│ │ │ │ +
91 }; // DiscreteKeys
│ │ │ │ +
│ │ │ │ +
92
│ │ │ │ +
94 GTSAM_EXPORT DiscreteKeys operator&(const DiscreteKey& key1, const DiscreteKey& key2);
│ │ │ │ +
95
│ │ │ │ +
96 // traits
│ │ │ │ +
97 template <>
│ │ │ │ +
98 struct traits<DiscreteKeys> : public Testable<DiscreteKeys> {};
│ │ │ │ +
99
│ │ │ │ +
100 } // namespace gtsam
│ │ │ │ + │ │ │ │ +
global_includes.h
Included from all GTSAM files.
│ │ │ │ +
gtsam::DiscreteKey
std::pair< Key, size_t > DiscreteKey
Key type for discrete variables.
Definition DiscreteKey.h:36
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::operator&
DiscreteKeys operator&(const DiscreteKey &key1, const DiscreteKey &key2)
Create a list from two keys.
Definition DiscreteKey.cpp:46
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::DiscreteEliminationTree
Elimination tree for discrete factors.
Definition DiscreteEliminationTree.h:33
│ │ │ │ -
gtsam::DiscreteEliminationTree::This
DiscreteEliminationTree This
This class.
Definition DiscreteEliminationTree.h:36
│ │ │ │ -
gtsam::DiscreteEliminationTree::shared_ptr
boost::shared_ptr< This > shared_ptr
Shared pointer to this class.
Definition DiscreteEliminationTree.h:37
│ │ │ │ -
gtsam::DiscreteEliminationTree::Base
EliminationTree< DiscreteBayesNet, DiscreteFactorGraph > Base
Base class.
Definition DiscreteEliminationTree.h:35
│ │ │ │ -
gtsam::DiscreteFactorGraph
A Discrete Factor Graph is a factor graph where all factors are Discrete, i.e.
Definition DiscreteFactorGraph.h:101
│ │ │ │ -
gtsam::EliminationTree
An elimination tree is a data structure used intermediately during elimination.
Definition EliminationTree.h:52
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ -
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition VariableIndex.h:43
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::DiscreteKeys
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition DiscreteKey.h:39
│ │ │ │ +
gtsam::DiscreteKeys::operator&
DiscreteKeys & operator&(const DiscreteKey &key)
Add a key (non-const!)
Definition DiscreteKey.h:70
│ │ │ │ +
gtsam::DiscreteKeys::DiscreteKeys
DiscreteKeys()
Constructor for serialization.
Definition DiscreteKey.h:45
│ │ │ │ +
gtsam::DiscreteKeys::DiscreteKeys
DiscreteKeys(const DiscreteKey &key)
Construct from a key.
Definition DiscreteKey.h:48
│ │ │ │ +
gtsam::DiscreteKeys::DiscreteKeys
DiscreteKeys(const std::vector< DiscreteKey > &keys)
Construct from a vector of keys.
Definition DiscreteKey.h:56
│ │ │ │ +
gtsam::DiscreteKeys::DiscreteKeys
DiscreteKeys(std::map< Key, size_t > cardinalities)
Construct from cardinalities.
Definition DiscreteKey.h:51
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DiscreteEliminationTree.h │ │ │ │ │ +DiscreteKey.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ @@ -16,76 +16,133 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 19#pragma once │ │ │ │ │ 20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h> │ │ │ │ │ -24 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ -26 │ │ │ │ │ -_3_1 class GTSAM_EXPORT _D_i_s_c_r_e_t_e_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e : │ │ │ │ │ -32 public _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ -33 { │ │ │ │ │ -34 public: │ │ │ │ │ -_3_5 typedef _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_,_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_> _B_a_s_e; │ │ │ │ │ -_3_6 typedef _D_i_s_c_r_e_t_e_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e _T_h_i_s; │ │ │ │ │ -_3_7 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -38 │ │ │ │ │ -47 _D_i_s_c_r_e_t_e_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e(const _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h& factorGraph, │ │ │ │ │ -48 const _V_a_r_i_a_b_l_e_I_n_d_e_x& structure, const _O_r_d_e_r_i_n_g& order); │ │ │ │ │ +21#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ +23 │ │ │ │ │ +24#include │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28 │ │ │ │ │ +29namespace _g_t_s_a_m { │ │ │ │ │ +30 │ │ │ │ │ +_3_6 using _D_i_s_c_r_e_t_e_K_e_y = std::pair; │ │ │ │ │ +37 │ │ │ │ │ +_3_9 struct GTSAM_EXPORT _D_i_s_c_r_e_t_e_K_e_y_s: public std::vector { │ │ │ │ │ +40 │ │ │ │ │ +41 // Forward all constructors. │ │ │ │ │ +42 using std::vector<_D_i_s_c_r_e_t_e_K_e_y>::vector; │ │ │ │ │ +43 │ │ │ │ │ +_4_5 _D_i_s_c_r_e_t_e_K_e_y_s() : std::vector<_D_i_s_c_r_e_t_e_K_e_y>::vector() {} │ │ │ │ │ +46 │ │ │ │ │ +_4_8 explicit _D_i_s_c_r_e_t_e_K_e_y_s(const _D_i_s_c_r_e_t_e_K_e_y& key) { push_back(key); } │ │ │ │ │ 49 │ │ │ │ │ -55 _D_i_s_c_r_e_t_e_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e(const _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h& factorGraph, │ │ │ │ │ -56 const _O_r_d_e_r_i_n_g& order); │ │ │ │ │ -57 │ │ │ │ │ -59 bool _e_q_u_a_l_s(const _T_h_i_s& other, double tol = 1e-9) const; │ │ │ │ │ -60 │ │ │ │ │ -61 private: │ │ │ │ │ +_5_1 explicit _D_i_s_c_r_e_t_e_K_e_y_s(std::map cardinalities) { │ │ │ │ │ +52 for (auto&& kv : cardinalities) emplace_back(kv); │ │ │ │ │ +53 } │ │ │ │ │ +54 │ │ │ │ │ +_5_6 _D_i_s_c_r_e_t_e_K_e_y_s(const std::vector& keys) : │ │ │ │ │ +57 std::vector<_D_i_s_c_r_e_t_e_K_e_y>(keys) { │ │ │ │ │ +58 } │ │ │ │ │ +59 │ │ │ │ │ +61 _D_i_s_c_r_e_t_e_K_e_y_s(const std::vector& cs); │ │ │ │ │ 62 │ │ │ │ │ -63 friend class ::EliminationTreeTester; │ │ │ │ │ -64 │ │ │ │ │ -65 }; │ │ │ │ │ -66 │ │ │ │ │ -67} │ │ │ │ │ -_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_._h │ │ │ │ │ -_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h │ │ │ │ │ +64 _K_e_y_V_e_c_t_o_r indices() const; │ │ │ │ │ +65 │ │ │ │ │ +67 std::map cardinalities() const; │ │ │ │ │ +68 │ │ │ │ │ +_7_0 _D_i_s_c_r_e_t_e_K_e_y_s& _o_p_e_r_a_t_o_r_&(const _D_i_s_c_r_e_t_e_K_e_y& key) { │ │ │ │ │ +71 push_back(key); │ │ │ │ │ +72 return *this; │ │ │ │ │ +73 } │ │ │ │ │ +74 │ │ │ │ │ +76 void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ +77 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const; │ │ │ │ │ +78 │ │ │ │ │ +80 bool _e_q_u_a_l_s(const _D_i_s_c_r_e_t_e_K_e_y_s& other, double tol = 0) const; │ │ │ │ │ +81 │ │ │ │ │ +_8_3 friend class boost::serialization::access; │ │ │ │ │ +84 template │ │ │ │ │ +85 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ +86 ar& boost::serialization::make_nvp( │ │ │ │ │ +87 "DiscreteKeys", │ │ │ │ │ +88 boost::serialization::base_object>(*this)); │ │ │ │ │ +89 } │ │ │ │ │ +90 │ │ │ │ │ +91 }; // DiscreteKeys │ │ │ │ │ +92 │ │ │ │ │ +94 GTSAM_EXPORT DiscreteKeys _o_p_e_r_a_t_o_r_&(const _D_i_s_c_r_e_t_e_K_e_y& key1, const │ │ │ │ │ +_D_i_s_c_r_e_t_e_K_e_y& key2); │ │ │ │ │ +95 │ │ │ │ │ +96 // traits │ │ │ │ │ +97 template <> │ │ │ │ │ +_9_8 struct _t_r_a_i_t_s<_D_i_s_c_r_e_t_e_K_e_y_s> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ +99 │ │ │ │ │ +100 } // namespace gtsam │ │ │ │ │ +_K_e_y_._h │ │ │ │ │ +_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ +Included from all GTSAM files. │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y │ │ │ │ │ +std::pair< Key, size_t > DiscreteKey │ │ │ │ │ +Key type for discrete variables. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteKey.h:36 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_& │ │ │ │ │ +DiscreteKeys operator&(const DiscreteKey &key1, const DiscreteKey &key2) │ │ │ │ │ +Create a list from two keys. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteKey.cpp:46 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ _g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ Template to create a binary predicate. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ -Elimination tree for discrete factors. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteEliminationTree.h:33 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_T_h_i_s │ │ │ │ │ -DiscreteEliminationTree This │ │ │ │ │ -This class. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteEliminationTree.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -Shared pointer to this class. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteEliminationTree.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_B_a_s_e │ │ │ │ │ -EliminationTree< DiscreteBayesNet, DiscreteFactorGraph > Base │ │ │ │ │ -Base class. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteEliminationTree.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A Discrete Factor Graph is a factor graph where all factors are Discrete, i.e. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactorGraph.h:101 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ -An elimination tree is a data structure used intermediately during elimination. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ -The VariableIndex class computes and stores the block column structure of a │ │ │ │ │ -factor graph. │ │ │ │ │ -DDeeffiinniittiioonn VariableIndex.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ +DiscreteKeys is a set of keys that can be assembled using the & operator. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteKey.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s_:_:_o_p_e_r_a_t_o_r_& │ │ │ │ │ +DiscreteKeys & operator&(const DiscreteKey &key) │ │ │ │ │ +Add a key (non-const!) │ │ │ │ │ +DDeeffiinniittiioonn DiscreteKey.h:70 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ +DiscreteKeys() │ │ │ │ │ +Constructor for serialization. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteKey.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ +DiscreteKeys(const DiscreteKey &key) │ │ │ │ │ +Construct from a key. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteKey.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ +DiscreteKeys(const std::vector< DiscreteKey > &keys) │ │ │ │ │ +Construct from a vector of keys. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteKey.h:56 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ +DiscreteKeys(std::map< Key, size_t > cardinalities) │ │ │ │ │ +Construct from cardinalities. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteKey.h:51 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h │ │ │ │ │ + * _D_i_s_c_r_e_t_e_K_e_y_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00263.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DecisionTreeFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,52 +94,38 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
DiscreteFactor.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
DecisionTreeFactor.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

discrete factor │ │ │ │ +More...

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::DiscreteFactor
 Base class for discrete probabilistic factors The most general one is the derived DecisionTreeFactor. More...
 
struct  gtsam::traits< DiscreteFactor >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

std::vector< double > gtsam::expNormalize (const std::vector< double > &logProbs)
 Normalize a set of log probabilities.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Feb 14, 2011
│ │ │ │ +

discrete factor

│ │ │ │ +
Date
Feb 14, 2011
│ │ │ │
Author
Duy-Nguyen Ta
│ │ │ │
│ │ │ │ Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,34 +1,23 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -DiscreteFactor.h File Reference │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r │ │ │ │ │ -  Base class for discrete probabilistic factors The most general one is │ │ │ │ │ - the derived _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_ _> │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +DecisionTreeFactor.cpp File Reference │ │ │ │ │ +discrete factor _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -std::vector< double >  _g_t_s_a_m_:_:_e_x_p_N_o_r_m_a_l_i_z_e (const std::vector< double > │ │ │ │ │ - &logProbs) │ │ │ │ │ -  Normalize a set of log probabilities. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +discrete factor │ │ │ │ │ Date │ │ │ │ │ Feb 14, 2011 │ │ │ │ │ Author │ │ │ │ │ Duy-Nguyen Ta │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_F_a_c_t_o_r_._h │ │ │ │ │ + * _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00269.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteMarginals.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteConditional.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,49 +94,41 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
DiscreteMarginals.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Variables
│ │ │ │ +
DiscreteConditional.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ - │ │ │ │ -

A class for computing marginals in a DiscreteFactorGraph. │ │ │ │ -More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::DiscreteMarginals
 A class for computing marginals of variables in a DiscreteFactorGraph. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Variables

│ │ │ │ +template class GTSAM_EXPORT gtsam::Conditional< DecisionTreeFactor, DiscreteConditional >
 
│ │ │ │

Detailed Description

│ │ │ │ -

A class for computing marginals in a DiscreteFactorGraph.

│ │ │ │ -
Author
Abhijit Kundu
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ +
Date
Feb 14, 2011
│ │ │ │ +
Author
Duy-Nguyen Ta
│ │ │ │
│ │ │ │ Frank Dellaert
│ │ │ │ -
Date
June 4, 2012
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,30 +1,25 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -DiscreteMarginals.h File Reference │ │ │ │ │ -A class for computing marginals in a DiscreteFactorGraph. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_M_a_r_g_i_n_a_l_s │ │ │ │ │ -  A class for computing marginals of variables in a _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _V_a_r_i_a_b_l_e_s │ │ │ │ │ +DiscreteConditional.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +VVaarriiaabblleess │ │ │ │ │ +template class GTSAM_EXPORT  ggttssaamm::::CCoonnddiittiioonnaall<< DDeecciissiioonnTTrreeeeFFaaccttoorr,, │ │ │ │ │ + DDiissccrreetteeCCoonnddiittiioonnaall >> │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A class for computing marginals in a DiscreteFactorGraph. │ │ │ │ │ + Date │ │ │ │ │ + Feb 14, 2011 │ │ │ │ │ Author │ │ │ │ │ - Abhijit Kundu │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Duy-Nguyen Ta │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - June 4, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_M_a_r_g_i_n_a_l_s_._h │ │ │ │ │ + * _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00272.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/Assignment.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteConditional.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,43 +96,43 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
Assignment.h File Reference
│ │ │ │ +
DiscreteConditional.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

An assignment from labels to a discrete value index (size_t) │ │ │ │ -More...

│ │ │ │ - │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::Assignment< L >
 An assignment from labels to value index (size_t). More...
class  gtsam::DiscreteConditional
 Discrete Conditional Density Derives from DecisionTreeFactor. More...
 
struct  gtsam::traits< DiscreteConditional >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

An assignment from labels to a discrete value index (size_t)

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
Date
Feb 5, 2012
│ │ │ │ +
Date
Feb 14, 2011
│ │ │ │ +
Author
Duy-Nguyen Ta
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,27 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Assignment.h File Reference │ │ │ │ │ -An assignment from labels to a discrete value index (size_t) _M_o_r_e_._._. │ │ │ │ │ +DiscreteConditional.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_A_s_s_i_g_n_m_e_n_t_<_ _L_ _> │ │ │ │ │ -  An assignment from labels to value index (size_t). _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +  Discrete _C_o_n_d_i_t_i_o_n_a_l Density Derives from _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -An assignment from labels to a discrete value index (size_t) │ │ │ │ │ + Date │ │ │ │ │ + Feb 14, 2011 │ │ │ │ │ Author │ │ │ │ │ + Duy-Nguyen Ta │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - Feb 5, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _A_s_s_i_g_n_m_e_n_t_._h │ │ │ │ │ + * _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00272_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/Assignment.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteConditional.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,15 +98,15 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Assignment.h
│ │ │ │ +
DiscreteConditional.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │ @@ -114,94 +114,212 @@ │ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <functional>
│ │ │ │ -
22#include <iostream>
│ │ │ │ -
23#include <map>
│ │ │ │ -
24#include <sstream>
│ │ │ │ -
25#include <utility>
│ │ │ │ -
26#include <vector>
│ │ │ │ -
27
│ │ │ │ -
28namespace gtsam {
│ │ │ │ +
21#include <gtsam/inference/Conditional-inst.h>
│ │ │ │ +
22#include <gtsam/discrete/DecisionTreeFactor.h>
│ │ │ │ +
23#include <gtsam/discrete/Signature.h>
│ │ │ │ +
24
│ │ │ │ +
25#include <boost/make_shared.hpp>
│ │ │ │ +
26#include <boost/shared_ptr.hpp>
│ │ │ │ +
27#include <string>
│ │ │ │ +
28#include <vector>
│ │ │ │
29
│ │ │ │ -
36template <class L>
│ │ │ │ -
│ │ │ │ -
37class Assignment : public std::map<L, size_t> {
│ │ │ │ -
45 static std::string DefaultFormatter(const L& x) {
│ │ │ │ -
46 std::stringstream ss;
│ │ │ │ -
47 ss << x;
│ │ │ │ -
48 return ss.str();
│ │ │ │ -
49 }
│ │ │ │ +
30namespace gtsam {
│ │ │ │ +
31
│ │ │ │ +
│ │ │ │ +
38class GTSAM_EXPORT DiscreteConditional
│ │ │ │ +
39 : public DecisionTreeFactor,
│ │ │ │ +
40 public Conditional<DecisionTreeFactor, DiscreteConditional> {
│ │ │ │ +
41 public:
│ │ │ │ +
42 // typedefs needed to play nice with gtsam
│ │ │ │ +
43 typedef DiscreteConditional This;
│ │ │ │ +
44 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
45 typedef DecisionTreeFactor BaseFactor;
│ │ │ │ +
46 typedef Conditional<BaseFactor, This>
│ │ │ │ +
47 BaseConditional;
│ │ │ │ +
48
│ │ │ │ +
49 using Values = DiscreteValues;
│ │ │ │
50
│ │ │ │ -
51 public:
│ │ │ │ -
52 using std::map<L, size_t>::operator=;
│ │ │ │
53
│ │ │ │ -
54 // Define the implicit default constructor.
│ │ │ │ -
55 Assignment() = default;
│ │ │ │ +
55 DiscreteConditional() {}
│ │ │ │
56
│ │ │ │ -
57 // Construct from initializer list.
│ │ │ │ -
58 Assignment(std::initializer_list<std::pair<const L, size_t>> init)
│ │ │ │ -
59 : std::map<L, size_t>{init} {}
│ │ │ │ -
60
│ │ │ │ -
61 void print(const std::string& s = "Assignment: ",
│ │ │ │ -
62 const std::function<std::string(L)>& labelFormatter =
│ │ │ │ -
63 &DefaultFormatter) const {
│ │ │ │ -
64 std::cout << s << ": ";
│ │ │ │ -
65 for (const typename Assignment::value_type& keyValue : *this) {
│ │ │ │ -
66 std::cout << "(" << labelFormatter(keyValue.first) << ", "
│ │ │ │ -
67 << keyValue.second << ")";
│ │ │ │ -
68 }
│ │ │ │ -
69 std::cout << std::endl;
│ │ │ │ -
70 }
│ │ │ │ -
71
│ │ │ │ -
72 bool equals(const Assignment& other, double tol = 1e-9) const {
│ │ │ │ -
73 return (*this == other);
│ │ │ │ -
74 }
│ │ │ │ -
75
│ │ │ │ -
88 template <typename Derived = Assignment<L>>
│ │ │ │ -
│ │ │ │ -
89 static std::vector<Derived> CartesianProduct(
│ │ │ │ -
90 const std::vector<std::pair<L, size_t>>& keys) {
│ │ │ │ -
91 std::vector<Derived> allPossValues;
│ │ │ │ -
92 Derived values;
│ │ │ │ -
93 typedef std::pair<L, size_t> DiscreteKey;
│ │ │ │ -
94 for (const DiscreteKey& key : keys)
│ │ │ │ -
95 values[key.first] = 0; // Initialize from 0
│ │ │ │ -
96 while (1) {
│ │ │ │ -
97 allPossValues.push_back(values);
│ │ │ │ -
98 size_t j = 0;
│ │ │ │ -
99 for (j = 0; j < keys.size(); j++) {
│ │ │ │ -
100 L idx = keys[j].first;
│ │ │ │ -
101 values[idx]++;
│ │ │ │ -
102 if (values[idx] < keys[j].second) break;
│ │ │ │ -
103 // Wrap condition
│ │ │ │ -
104 values[idx] = 0;
│ │ │ │ -
105 }
│ │ │ │ -
106 if (j == keys.size()) break;
│ │ │ │ -
107 }
│ │ │ │ -
108 return allPossValues;
│ │ │ │ -
109 }
│ │ │ │ +
58 DiscreteConditional(size_t nFrontals, const DecisionTreeFactor& f);
│ │ │ │ +
59
│ │ │ │ +
64 DiscreteConditional(size_t nFrontals, const DiscreteKeys& keys,
│ │ │ │ +
65 const ADT& potentials);
│ │ │ │ +
66
│ │ │ │ +
68 explicit DiscreteConditional(const Signature& signature);
│ │ │ │ +
69
│ │ │ │ +
│ │ │ │ +
77 DiscreteConditional(const DiscreteKey& key, const DiscreteKeys& parents,
│ │ │ │ +
78 const Signature::Table& table)
│ │ │ │ +
79 : DiscreteConditional(Signature(key, parents, table)) {}
│ │ │ │
│ │ │ │ -
110}; // Assignment
│ │ │ │ +
80
│ │ │ │ +
│ │ │ │ +
90 DiscreteConditional(const DiscreteKey& key, const DiscreteKeys& parents,
│ │ │ │ +
91 const std::string& spec)
│ │ │ │ +
92 : DiscreteConditional(Signature(key, parents, spec)) {}
│ │ │ │
│ │ │ │ -
111
│ │ │ │ -
112} // namespace gtsam
│ │ │ │ +
93
│ │ │ │ +
│ │ │ │ +
95 DiscreteConditional(const DiscreteKey& key, const std::string& spec)
│ │ │ │ +
96 : DiscreteConditional(Signature(key, {}, spec)) {}
│ │ │ │ +
│ │ │ │ +
97
│ │ │ │ +
102 DiscreteConditional(const DecisionTreeFactor& joint,
│ │ │ │ +
103 const DecisionTreeFactor& marginal);
│ │ │ │ +
104
│ │ │ │ +
110 DiscreteConditional(const DecisionTreeFactor& joint,
│ │ │ │ +
111 const DecisionTreeFactor& marginal,
│ │ │ │ +
112 const Ordering& orderedKeys);
│ │ │ │ +
113
│ │ │ │ +
129 DiscreteConditional operator*(const DiscreteConditional& other) const;
│ │ │ │ +
130
│ │ │ │ +
132 DiscreteConditional marginal(Key key) const;
│ │ │ │ +
133
│ │ │ │ +
137
│ │ │ │ +
139 void print(
│ │ │ │ +
140 const std::string& s = "Discrete Conditional: ",
│ │ │ │ +
141 const KeyFormatter& formatter = DefaultKeyFormatter) const override;
│ │ │ │ +
142
│ │ │ │ +
144 bool equals(const DiscreteFactor& other, double tol = 1e-9) const override;
│ │ │ │ +
145
│ │ │ │ +
149
│ │ │ │ +
│ │ │ │ +
151 double logProbability(const DiscreteValues& x) const {
│ │ │ │ +
152 return -error(x);
│ │ │ │ +
153 }
│ │ │ │ +
│ │ │ │ +
154
│ │ │ │ +
│ │ │ │ +
156 void printSignature(
│ │ │ │ +
157 const std::string& s = "Discrete Conditional: ",
│ │ │ │ +
158 const KeyFormatter& formatter = DefaultKeyFormatter) const {
│ │ │ │ +
159 static_cast<const BaseConditional*>(this)->print(s, formatter);
│ │ │ │ +
160 }
│ │ │ │ +
│ │ │ │ +
161
│ │ │ │ +
│ │ │ │ +
163 double evaluate(const DiscreteValues& values) const {
│ │ │ │ +
164 return ADT::operator()(values);
│ │ │ │ +
165 }
│ │ │ │ +
│ │ │ │ +
166
│ │ │ │ +
167 using DecisionTreeFactor::error;
│ │ │ │ +
168 using DecisionTreeFactor::operator();
│ │ │ │ +
169
│ │ │ │ +
183 shared_ptr choose(const DiscreteValues& given) const;
│ │ │ │ +
184
│ │ │ │ +
186 DecisionTreeFactor::shared_ptr likelihood(
│ │ │ │ +
187 const DiscreteValues& frontalValues) const;
│ │ │ │ +
188
│ │ │ │ +
190 DecisionTreeFactor::shared_ptr likelihood(size_t frontal) const;
│ │ │ │ +
191
│ │ │ │ +
197 size_t sample(const DiscreteValues& parentsValues) const;
│ │ │ │ +
198
│ │ │ │ +
200 size_t sample(size_t parent_value) const;
│ │ │ │ +
201
│ │ │ │ +
203 size_t sample() const;
│ │ │ │ +
204
│ │ │ │ +
209 size_t argmax() const;
│ │ │ │ +
210
│ │ │ │ +
214
│ │ │ │ +
216 void sampleInPlace(DiscreteValues* parentsValues) const;
│ │ │ │ +
217
│ │ │ │ +
219 std::vector<DiscreteValues> frontalAssignments() const;
│ │ │ │ +
220
│ │ │ │ +
222 std::vector<DiscreteValues> allAssignments() const;
│ │ │ │ +
223
│ │ │ │ +
227
│ │ │ │ +
229 std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
230 const Names& names = {}) const override;
│ │ │ │ +
231
│ │ │ │ +
233 std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
234 const Names& names = {}) const override;
│ │ │ │ +
235
│ │ │ │ +
236
│ │ │ │ +
240
│ │ │ │ +
245 double evaluate(const HybridValues& x) const override;
│ │ │ │ +
246
│ │ │ │ +
247 using BaseConditional::operator();
│ │ │ │ +
248
│ │ │ │ +
│ │ │ │ +
253 double logProbability(const HybridValues& x) const override {
│ │ │ │ +
254 return -error(x);
│ │ │ │ +
255 }
│ │ │ │ +
│ │ │ │ +
256
│ │ │ │ +
262 double logNormalizationConstant() const override { return 0.0; }
│ │ │ │ +
263
│ │ │ │ +
265
│ │ │ │ +
266#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ +
269 size_t GTSAM_DEPRECATED solve(const DiscreteValues& parentsValues) const;
│ │ │ │ +
270 void GTSAM_DEPRECATED solveInPlace(DiscreteValues* parentsValues) const;
│ │ │ │ +
272#endif
│ │ │ │ +
273
│ │ │ │ +
274 protected:
│ │ │ │ +
276 DiscreteConditional::ADT choose(const DiscreteValues& given,
│ │ │ │ +
277 bool forceComplete) const;
│ │ │ │ +
278
│ │ │ │ +
279 private:
│ │ │ │ +
281 friend class boost::serialization::access;
│ │ │ │ +
282 template <class Archive>
│ │ │ │ +
283 void serialize(Archive& ar, const unsigned int /*version*/) {
│ │ │ │ +
284 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseFactor);
│ │ │ │ +
285 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseConditional);
│ │ │ │ +
286 }
│ │ │ │ +
287};
│ │ │ │ +
│ │ │ │ +
288// DiscreteConditional
│ │ │ │ +
289
│ │ │ │ +
290// traits
│ │ │ │ +
291template <>
│ │ │ │ +
292struct traits<DiscreteConditional> : public Testable<DiscreteConditional> {};
│ │ │ │ +
293
│ │ │ │ +
294} // namespace gtsam
│ │ │ │ +
Signature.h
signatures for conditional densities
│ │ │ │ + │ │ │ │
gtsam::DiscreteKey
std::pair< Key, size_t > DiscreteKey
Key type for discrete variables.
Definition DiscreteKey.h:36
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Assignment
An assignment from labels to value index (size_t).
Definition Assignment.h:37
│ │ │ │ -
gtsam::Assignment::CartesianProduct
static std::vector< Derived > CartesianProduct(const std::vector< std::pair< L, size_t > > &keys)
Get Cartesian product consisting all possible configurations.
Definition Assignment.h:89
│ │ │ │ +
gtsam::markdown
string markdown(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
Free version of markdown.
Definition DiscreteValues.cpp:129
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ + │ │ │ │ +
gtsam::DecisionTreeFactor
A discrete probabilistic factor.
Definition DecisionTreeFactor.h:45
│ │ │ │ +
gtsam::DiscreteConditional
Discrete Conditional Density Derives from DecisionTreeFactor.
Definition DiscreteConditional.h:40
│ │ │ │ +
gtsam::DiscreteConditional::DiscreteConditional
DiscreteConditional(const DiscreteKey &key, const std::string &spec)
No-parent specialization; can also use DiscreteDistribution.
Definition DiscreteConditional.h:95
│ │ │ │ +
gtsam::DiscreteConditional::DiscreteConditional
DiscreteConditional(const DiscreteKey &key, const DiscreteKeys &parents, const std::string &spec)
Construct from key, parents, and a string specifying the conditional probability table (CPT) in 00 01...
Definition DiscreteConditional.h:90
│ │ │ │ +
gtsam::DiscreteConditional::evaluate
double evaluate(const DiscreteValues &values) const
Evaluate, just look up in AlgebraicDecisonTree.
Definition DiscreteConditional.h:163
│ │ │ │ +
gtsam::DiscreteConditional::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition DiscreteConditional.h:44
│ │ │ │ +
gtsam::DiscreteConditional::DiscreteConditional
DiscreteConditional(const DiscreteKey &key, const DiscreteKeys &parents, const Signature::Table &table)
Construct from key, parents, and a Signature::Table specifying the conditional probability table (CPT...
Definition DiscreteConditional.h:77
│ │ │ │ +
gtsam::DiscreteConditional::BaseConditional
Conditional< BaseFactor, This > BaseConditional
Typedef to our conditional base class.
Definition DiscreteConditional.h:47
│ │ │ │ +
gtsam::DiscreteConditional::logProbability
double logProbability(const HybridValues &x) const override
< HybridValues version
Definition DiscreteConditional.h:253
│ │ │ │ +
gtsam::DiscreteConditional::This
DiscreteConditional This
Typedef to this class.
Definition DiscreteConditional.h:43
│ │ │ │ +
gtsam::DiscreteConditional::BaseFactor
DecisionTreeFactor BaseFactor
Typedef to our factor base class.
Definition DiscreteConditional.h:45
│ │ │ │ +
gtsam::DiscreteConditional::DiscreteConditional
DiscreteConditional()
Default constructor needed for serialization.
Definition DiscreteConditional.h:55
│ │ │ │ +
gtsam::DiscreteConditional::logProbability
double logProbability(const DiscreteValues &x) const
Log-probability is just -error(x).
Definition DiscreteConditional.h:151
│ │ │ │ +
gtsam::DiscreteConditional::printSignature
void printSignature(const std::string &s="Discrete Conditional: ", const KeyFormatter &formatter=DefaultKeyFormatter) const
print index signature only
Definition DiscreteConditional.h:156
│ │ │ │ +
gtsam::DiscreteConditional::logNormalizationConstant
double logNormalizationConstant() const override
logNormalizationConstant K is just zero, such that logProbability(x) = log(evaluate(x)) = - error(x) ...
Definition DiscreteConditional.h:262
│ │ │ │ +
gtsam::DiscreteKeys
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition DiscreteKey.h:39
│ │ │ │ +
gtsam::DiscreteValues
A map from keys to values.
Definition DiscreteValues.h:34
│ │ │ │ +
gtsam::Signature
Signature for a discrete conditional density, used to construct conditionals.
Definition Signature.h:54
│ │ │ │ +
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ +
gtsam::Conditional
Definition Conditional.h:64
│ │ │ │ +
HybridValues
the error.
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Assignment.h │ │ │ │ │ +DiscreteConditional.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ @@ -16,96 +16,271 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 19#pragma once │ │ │ │ │ 20 │ │ │ │ │ -21#include │ │ │ │ │ -22#include │ │ │ │ │ -23#include │ │ │ │ │ -24#include │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ -27 │ │ │ │ │ -28namespace _g_t_s_a_m { │ │ │ │ │ +21#include │ │ │ │ │ +22#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_S_i_g_n_a_t_u_r_e_._h> │ │ │ │ │ +24 │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28#include │ │ │ │ │ 29 │ │ │ │ │ -36template │ │ │ │ │ -_3_7class _A_s_s_i_g_n_m_e_n_t : public std::map { │ │ │ │ │ -45 static std::string DefaultFormatter(const L& x) { │ │ │ │ │ -46 std::stringstream ss; │ │ │ │ │ -47 ss << x; │ │ │ │ │ -48 return ss.str(); │ │ │ │ │ -49 } │ │ │ │ │ +30namespace _g_t_s_a_m { │ │ │ │ │ +31 │ │ │ │ │ +_3_8class GTSAM_EXPORT _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +39 : public _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r, │ │ │ │ │ +40 public _C_o_n_d_i_t_i_o_n_a_l { │ │ │ │ │ +41 public: │ │ │ │ │ +42 // typedefs needed to play nice with gtsam │ │ │ │ │ +_4_3 typedef _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l _T_h_i_s; │ │ │ │ │ +_4_4 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +_4_5 typedef _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r _B_a_s_e_F_a_c_t_o_r; │ │ │ │ │ +46 typedef _C_o_n_d_i_t_i_o_n_a_l_<_B_a_s_e_F_a_c_t_o_r_,_ _T_h_i_s_> │ │ │ │ │ +_4_7 _B_a_s_e_C_o_n_d_i_t_i_o_n_a_l; │ │ │ │ │ +48 │ │ │ │ │ +_4_9 using _V_a_l_u_e_s = _D_i_s_c_r_e_t_e_V_a_l_u_e_s; │ │ │ │ │ 50 │ │ │ │ │ -51 public: │ │ │ │ │ -52 using std::map::operator=; │ │ │ │ │ 53 │ │ │ │ │ -54 // Define the implicit default constructor. │ │ │ │ │ -55 _A_s_s_i_g_n_m_e_n_t() = default; │ │ │ │ │ +_5_5 _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l() {} │ │ │ │ │ 56 │ │ │ │ │ -57 // Construct from initializer list. │ │ │ │ │ -58 _A_s_s_i_g_n_m_e_n_t(std::initializer_list> init) │ │ │ │ │ -59 : std::map{init} {} │ │ │ │ │ -60 │ │ │ │ │ -61 void print(const std::string& s = "Assignment: ", │ │ │ │ │ -62 const std::function& labelFormatter = │ │ │ │ │ -63 &DefaultFormatter) const { │ │ │ │ │ -64 std::cout << s << ": "; │ │ │ │ │ -65 for (const typename Assignment::value_type& keyValue : *this) { │ │ │ │ │ -66 std::cout << "(" << labelFormatter(keyValue.first) << ", " │ │ │ │ │ -67 << keyValue.second << ")"; │ │ │ │ │ -68 } │ │ │ │ │ -69 std::cout << std::endl; │ │ │ │ │ -70 } │ │ │ │ │ -71 │ │ │ │ │ -72 bool _e_q_u_a_l_s(const _A_s_s_i_g_n_m_e_n_t& other, double tol = 1e-9) const { │ │ │ │ │ -73 return (*this == other); │ │ │ │ │ -74 } │ │ │ │ │ -75 │ │ │ │ │ -88 template > │ │ │ │ │ -_8_9 static std::vector _C_a_r_t_e_s_i_a_n_P_r_o_d_u_c_t( │ │ │ │ │ -90 const std::vector>& keys) { │ │ │ │ │ -91 std::vector allPossValues; │ │ │ │ │ -92 Derived values; │ │ │ │ │ -93 typedef std::pair _D_i_s_c_r_e_t_e_K_e_y; │ │ │ │ │ -94 for (const _D_i_s_c_r_e_t_e_K_e_y& key : keys) │ │ │ │ │ -95 values[key.first] = 0; // Initialize from 0 │ │ │ │ │ -96 while (1) { │ │ │ │ │ -97 allPossValues.push_back(values); │ │ │ │ │ -98 size_t j = 0; │ │ │ │ │ -99 for (j = 0; j < keys.size(); j++) { │ │ │ │ │ -100 L idx = keys[j].first; │ │ │ │ │ -101 values[idx]++; │ │ │ │ │ -102 if (values[idx] < keys[j].second) break; │ │ │ │ │ -103 // Wrap condition │ │ │ │ │ -104 values[idx] = 0; │ │ │ │ │ -105 } │ │ │ │ │ -106 if (j == keys.size()) break; │ │ │ │ │ -107 } │ │ │ │ │ -108 return allPossValues; │ │ │ │ │ -109 } │ │ │ │ │ -110}; // Assignment │ │ │ │ │ -111 │ │ │ │ │ -112} // namespace gtsam │ │ │ │ │ +58 _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l(size_t nFrontals, const _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r& f); │ │ │ │ │ +59 │ │ │ │ │ +64 _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l(size_t nFrontals, const _D_i_s_c_r_e_t_e_K_e_y_s& keys, │ │ │ │ │ +65 const ADT& potentials); │ │ │ │ │ +66 │ │ │ │ │ +68 explicit _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l(const _S_i_g_n_a_t_u_r_e& signature); │ │ │ │ │ +69 │ │ │ │ │ +_7_7 _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l(const _D_i_s_c_r_e_t_e_K_e_y& key, const _D_i_s_c_r_e_t_e_K_e_y_s& parents, │ │ │ │ │ +78 const Signature::Table& table) │ │ │ │ │ +79 : _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l(_S_i_g_n_a_t_u_r_e(key, parents, table)) {} │ │ │ │ │ +80 │ │ │ │ │ +_9_0 _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l(const _D_i_s_c_r_e_t_e_K_e_y& key, const _D_i_s_c_r_e_t_e_K_e_y_s& parents, │ │ │ │ │ +91 const std::string& spec) │ │ │ │ │ +92 : _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l(_S_i_g_n_a_t_u_r_e(key, parents, spec)) {} │ │ │ │ │ +93 │ │ │ │ │ +_9_5 _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l(const _D_i_s_c_r_e_t_e_K_e_y& key, const std::string& spec) │ │ │ │ │ +96 : _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l(_S_i_g_n_a_t_u_r_e(key, {}, spec)) {} │ │ │ │ │ +97 │ │ │ │ │ +102 DiscreteConditional(const DecisionTreeFactor& joint, │ │ │ │ │ +103 const DecisionTreeFactor& marginal); │ │ │ │ │ +104 │ │ │ │ │ +110 DiscreteConditional(const DecisionTreeFactor& joint, │ │ │ │ │ +111 const DecisionTreeFactor& marginal, │ │ │ │ │ +112 const Ordering& orderedKeys); │ │ │ │ │ +113 │ │ │ │ │ +129 DiscreteConditional operator*(const DiscreteConditional& other) const; │ │ │ │ │ +130 │ │ │ │ │ +132 DiscreteConditional marginal(Key key) const; │ │ │ │ │ +133 │ │ │ │ │ +137 │ │ │ │ │ +139 void print( │ │ │ │ │ +140 const std::string& s = "Discrete Conditional: ", │ │ │ │ │ +141 const KeyFormatter& formatter = DefaultKeyFormatter) const override; │ │ │ │ │ +142 │ │ │ │ │ +144 bool equals(const DiscreteFactor& other, double tol = 1e-9) const override; │ │ │ │ │ +145 │ │ │ │ │ +149 │ │ │ │ │ +_1_5_1 double _l_o_g_P_r_o_b_a_b_i_l_i_t_y(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& x) const { │ │ │ │ │ +152 return -error(x); │ │ │ │ │ +153 } │ │ │ │ │ +154 │ │ │ │ │ +_1_5_6 void _p_r_i_n_t_S_i_g_n_a_t_u_r_e( │ │ │ │ │ +157 const std::string& s = "Discrete Conditional: ", │ │ │ │ │ +158 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const { │ │ │ │ │ +159 static_cast(this)->_p_r_i_n_t(s, formatter); │ │ │ │ │ +160 } │ │ │ │ │ +161 │ │ │ │ │ +_1_6_3 double _e_v_a_l_u_a_t_e(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values) const { │ │ │ │ │ +164 return ADT::operator()(values); │ │ │ │ │ +165 } │ │ │ │ │ +166 │ │ │ │ │ +167 using DecisionTreeFactor::error; │ │ │ │ │ +168 using DecisionTreeFactor::operator(); │ │ │ │ │ +169 │ │ │ │ │ +183 shared_ptr choose(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& given) const; │ │ │ │ │ +184 │ │ │ │ │ +186 DecisionTreeFactor::shared_ptr likelihood( │ │ │ │ │ +187 const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& frontalValues) const; │ │ │ │ │ +188 │ │ │ │ │ +190 DecisionTreeFactor::shared_ptr likelihood(size_t frontal) const; │ │ │ │ │ +191 │ │ │ │ │ +197 size_t sample(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& parentsValues) const; │ │ │ │ │ +198 │ │ │ │ │ +200 size_t sample(size_t parent_value) const; │ │ │ │ │ +201 │ │ │ │ │ +203 size_t sample() const; │ │ │ │ │ +204 │ │ │ │ │ +209 size_t argmax() const; │ │ │ │ │ +210 │ │ │ │ │ +214 │ │ │ │ │ +216 void sampleInPlace(_D_i_s_c_r_e_t_e_V_a_l_u_e_s* parentsValues) const; │ │ │ │ │ +217 │ │ │ │ │ +219 std::vector frontalAssignments() const; │ │ │ │ │ +220 │ │ │ │ │ +222 std::vector allAssignments() const; │ │ │ │ │ +223 │ │ │ │ │ +227 │ │ │ │ │ +229 std::string _m_a_r_k_d_o_w_n(const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ +DefaultKeyFormatter, │ │ │ │ │ +230 const Names& names = {}) const override; │ │ │ │ │ +231 │ │ │ │ │ +233 std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ +234 const Names& names = {}) const override; │ │ │ │ │ +235 │ │ │ │ │ +236 │ │ │ │ │ +240 │ │ │ │ │ +245 double evaluate(const _H_y_b_r_i_d_V_a_l_u_e_s& x) const override; │ │ │ │ │ +246 │ │ │ │ │ +247 using BaseConditional::operator(); │ │ │ │ │ +248 │ │ │ │ │ +_2_5_3 double _l_o_g_P_r_o_b_a_b_i_l_i_t_y(const _H_y_b_r_i_d_V_a_l_u_e_s& x) const override { │ │ │ │ │ +254 return -error(x); │ │ │ │ │ +255 } │ │ │ │ │ +256 │ │ │ │ │ +_2_6_2 double _l_o_g_N_o_r_m_a_l_i_z_a_t_i_o_n_C_o_n_s_t_a_n_t() const override { return 0.0; } │ │ │ │ │ +263 │ │ │ │ │ +265 │ │ │ │ │ +266#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ +269 size_t GTSAM_DEPRECATED solve(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& parentsValues) const; │ │ │ │ │ +270 void GTSAM_DEPRECATED solveInPlace(_D_i_s_c_r_e_t_e_V_a_l_u_e_s* parentsValues) const; │ │ │ │ │ +272#endif │ │ │ │ │ +273 │ │ │ │ │ +274 protected: │ │ │ │ │ +276 _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_A_D_T choose(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& given, │ │ │ │ │ +277 bool forceComplete) const; │ │ │ │ │ +278 │ │ │ │ │ +279 private: │ │ │ │ │ +_2_8_1 friend class boost::serialization::access; │ │ │ │ │ +282 template │ │ │ │ │ +283 void serialize(Archive& ar, const unsigned int /*version*/) { │ │ │ │ │ +284 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e_F_a_c_t_o_r); │ │ │ │ │ +285 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e_C_o_n_d_i_t_i_o_n_a_l); │ │ │ │ │ +286 } │ │ │ │ │ +287}; │ │ │ │ │ +288// DiscreteConditional │ │ │ │ │ +289 │ │ │ │ │ +290// traits │ │ │ │ │ +291template <> │ │ │ │ │ +_2_9_2struct _t_r_a_i_t_s<_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l> : public _T_e_s_t_a_b_l_e │ │ │ │ │ +{}; │ │ │ │ │ +293 │ │ │ │ │ +294} // namespace gtsam │ │ │ │ │ +_S_i_g_n_a_t_u_r_e_._h │ │ │ │ │ +signatures for conditional densities │ │ │ │ │ +_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_._h │ │ │ │ │ _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y │ │ │ │ │ std::pair< Key, size_t > DiscreteKey │ │ │ │ │ Key type for discrete variables. │ │ │ │ │ DDeeffiinniittiioonn DiscreteKey.h:36 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_A_s_s_i_g_n_m_e_n_t │ │ │ │ │ -An assignment from labels to value index (size_t). │ │ │ │ │ -DDeeffiinniittiioonn Assignment.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_A_s_s_i_g_n_m_e_n_t_:_:_C_a_r_t_e_s_i_a_n_P_r_o_d_u_c_t │ │ │ │ │ -static std::vector< Derived > CartesianProduct(const std::vector< std::pair< L, │ │ │ │ │ -size_t > > &keys) │ │ │ │ │ -Get Cartesian product consisting all possible configurations. │ │ │ │ │ -DDeeffiinniittiioonn Assignment.h:89 │ │ │ │ │ +_g_t_s_a_m_:_:_m_a_r_k_d_o_w_n │ │ │ │ │ +string markdown(const DiscreteValues &values, const KeyFormatter &keyFormatter, │ │ │ │ │ +const DiscreteValues::Names &names) │ │ │ │ │ +Free version of markdown. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteValues.cpp:129 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_<_ _K_e_y_ _> │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r │ │ │ │ │ +A discrete probabilistic factor. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTreeFactor.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +Discrete Conditional Density Derives from DecisionTreeFactor. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteConditional.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +DiscreteConditional(const DiscreteKey &key, const std::string &spec) │ │ │ │ │ +No-parent specialization; can also use DiscreteDistribution. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteConditional.h:95 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +DiscreteConditional(const DiscreteKey &key, const DiscreteKeys &parents, const │ │ │ │ │ +std::string &spec) │ │ │ │ │ +Construct from key, parents, and a string specifying the conditional │ │ │ │ │ +probability table (CPT) in 00 01... │ │ │ │ │ +DDeeffiinniittiioonn DiscreteConditional.h:90 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_e_v_a_l_u_a_t_e │ │ │ │ │ +double evaluate(const DiscreteValues &values) const │ │ │ │ │ +Evaluate, just look up in AlgebraicDecisonTree. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteConditional.h:163 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn DiscreteConditional.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +DiscreteConditional(const DiscreteKey &key, const DiscreteKeys &parents, const │ │ │ │ │ +Signature::Table &table) │ │ │ │ │ +Construct from key, parents, and a Signature::Table specifying the conditional │ │ │ │ │ +probability table (CPT... │ │ │ │ │ +DDeeffiinniittiioonn DiscreteConditional.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_B_a_s_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +Conditional< BaseFactor, This > BaseConditional │ │ │ │ │ +Typedef to our conditional base class. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteConditional.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_l_o_g_P_r_o_b_a_b_i_l_i_t_y │ │ │ │ │ +double logProbability(const HybridValues &x) const override │ │ │ │ │ +< HybridValues version │ │ │ │ │ +DDeeffiinniittiioonn DiscreteConditional.h:253 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_T_h_i_s │ │ │ │ │ +DiscreteConditional This │ │ │ │ │ +Typedef to this class. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteConditional.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_B_a_s_e_F_a_c_t_o_r │ │ │ │ │ +DecisionTreeFactor BaseFactor │ │ │ │ │ +Typedef to our factor base class. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteConditional.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +DiscreteConditional() │ │ │ │ │ +Default constructor needed for serialization. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteConditional.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_l_o_g_P_r_o_b_a_b_i_l_i_t_y │ │ │ │ │ +double logProbability(const DiscreteValues &x) const │ │ │ │ │ +Log-probability is just -error(x). │ │ │ │ │ +DDeeffiinniittiioonn DiscreteConditional.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_p_r_i_n_t_S_i_g_n_a_t_u_r_e │ │ │ │ │ +void printSignature(const std::string &s="Discrete Conditional: ", const │ │ │ │ │ +KeyFormatter &formatter=DefaultKeyFormatter) const │ │ │ │ │ +print index signature only │ │ │ │ │ +DDeeffiinniittiioonn DiscreteConditional.h:156 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_l_o_g_N_o_r_m_a_l_i_z_a_t_i_o_n_C_o_n_s_t_a_n_t │ │ │ │ │ +double logNormalizationConstant() const override │ │ │ │ │ +logNormalizationConstant K is just zero, such that logProbability(x) = log │ │ │ │ │ +(evaluate(x)) = - error(x) ... │ │ │ │ │ +DDeeffiinniittiioonn DiscreteConditional.h:262 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ +DiscreteKeys is a set of keys that can be assembled using the & operator. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteKey.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ +A map from keys to values. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteValues.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_S_i_g_n_a_t_u_r_e │ │ │ │ │ +Signature for a discrete conditional density, used to construct conditionals. │ │ │ │ │ +DDeeffiinniittiioonn Signature.h:54 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:64 │ │ │ │ │ +_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +the error. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _A_s_s_i_g_n_m_e_n_t_._h │ │ │ │ │ + * _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00278.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3_S2Stereo.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Rot3.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,42 +94,84 @@ │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces | │ │ │ │ +Macros | │ │ │ │ +Typedefs | │ │ │ │ Functions
│ │ │ │ -
Cal3_S2Stereo.cpp File Reference
│ │ │ │ +
Rot3.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

The most common 5DOF 3D->2D calibration + Stereo baseline. │ │ │ │ +

3D rotation represented as a rotation matrix or quaternion │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::Rot3
 Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion if it is defined. More...
 
struct  gtsam::Rot3::CayleyChart
 
struct  gtsam::Rot3::ChartAtOrigin
 
struct  gtsam::traits< Rot3 >
 
struct  gtsam::traits< const Rot3 >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Macros

│ │ │ │ +#define ROT3_DEFAULT_COORDINATES_MODE   Rot3::CAYLEY
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Typedefs

│ │ │ │ +using gtsam::Rot3Vector = std::vector< Rot3, Eigen::aligned_allocator< Rot3 > >
 std::vector of Rot3s, mainly for wrapper
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

│ │ │ │ -std::ostream & gtsam::operator<< (std::ostream &os, const Cal3_S2Stereo &cal)
 
pair< Matrix3, Vector3 > gtsam::RQ (const Matrix3 &A, OptionalJacobian< 3, 9 > H=boost::none)
 [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx' such that A = R*Q = R*Qz'*Qy'*Qx'.
 
│ │ │ │

Detailed Description

│ │ │ │ -

The most common 5DOF 3D->2D calibration + Stereo baseline.

│ │ │ │ -
Author
Chris Beall
│ │ │ │ +

3D rotation represented as a rotation matrix or quaternion

│ │ │ │ +
Author
Alireza Fathi
│ │ │ │ +
│ │ │ │ +Christian Potthast
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │ +
│ │ │ │ +Luca Carlone
│ │ │ │ +
│ │ │ │ +Varun Agrawal
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,56 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Cal3_S2Stereo.cpp File Reference │ │ │ │ │ -The most common 5DOF 3D->2D calibration + Stereo baseline. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Rot3.h File Reference │ │ │ │ │ +3D rotation represented as a rotation matrix or quaternion _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ + _R_o_t_3 is a 3D rotation represented as a rotation matrix if the │ │ │ │ │ +  preprocessor symbol GTSAM_USE_QUATERNIONS is not defined, or as a │ │ │ │ │ + quaternion if it is defined. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_R_o_t_3_:_:_C_a_y_l_e_y_C_h_a_r_t │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_R_o_t_3_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _R_o_t_3_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _R_o_t_3_ _> │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +MMaaccrrooss │ │ │ │ │ +#define  RROOTT33__DDEEFFAAUULLTT__CCOOOORRDDIINNAATTEESS__MMOODDEE   Rot3::CAYLEY │ │ │ │ │ +  │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +using  ggttssaamm::::RRoott33VVeeccttoorr = std::vector< _R_o_t_3, Eigen::aligned_allocator< _R_o_t_3 > │ │ │ │ │ + > │ │ │ │ │ +  std::vector of Rot3s, mainly for wrapper │ │ │ │ │ +  │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _C_a_l_3___S_2_S_t_e_r_e_o &cal) │ │ │ │ │ +pair< Matrix3, Vector3 >  _g_t_s_a_m_:_:_R_Q (const Matrix3 &A, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 3, 9 > │ │ │ │ │ + H=boost::none) │ │ │ │ │ + [RQ] receives a 3 by 3 matrix and returns an upper │ │ │ │ │ +  triangular matrix R and 3 rotation angles │ │ │ │ │ + corresponding to the rotation matrix Q=Qz'*Qy'*Qx' │ │ │ │ │ + such that A = R*Q = R*Qz'*Qy'*Qx'. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -The most common 5DOF 3D->2D calibration + Stereo baseline. │ │ │ │ │ +3D rotation represented as a rotation matrix or quaternion │ │ │ │ │ Author │ │ │ │ │ - Chris Beall │ │ │ │ │ + Alireza Fathi │ │ │ │ │ + Christian Potthast │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3___S_2_S_t_e_r_e_o_._c_p_p │ │ │ │ │ + * _R_o_t_3_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00281.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SOn-inl.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Line3.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,38 +94,57 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces
│ │ │ │ -
SOn-inl.h File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
Line3.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Template implementations for SO(n) │ │ │ │ +

4 dimensional manifold of 3D lines │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::Line3
 A 3D line (R,a,b) : (Rot3,Scalar,Scalar) More...
 
struct  gtsam::traits< Line3 >
 
struct  gtsam::traits< const Line3 >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

Line3 gtsam::transformTo (const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose=boost::none, OptionalJacobian< 4, 4 > Dline=boost::none)
 Transform a line from world to camera frame.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Template implementations for SO(n)

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
Date
March 2019
│ │ │ │ +

4 dimensional manifold of 3D lines

│ │ │ │ +
Author
Akshay Krishnan
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,35 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SOn-inl.h File Reference │ │ │ │ │ -Template implementations for SO(n) _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Line3.h File Reference │ │ │ │ │ +4 dimensional manifold of 3D lines _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_L_i_n_e_3 │ │ │ │ │ +  A 3D line (R,a,b) : (_R_o_t_3,Scalar,Scalar) _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _L_i_n_e_3_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _L_i_n_e_3_ _> │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +_L_i_n_e_3  _g_t_s_a_m_:_:_t_r_a_n_s_f_o_r_m_T_o (const _P_o_s_e_3 &wTc, const _L_i_n_e_3 &wL, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< │ │ │ │ │ + 4, 6 > Dpose=boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 4, 4 > Dline=boost::none) │ │ │ │ │ +  Transform a line from world to camera frame. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Template implementations for SO(n) │ │ │ │ │ +4 dimensional manifold of 3D lines │ │ │ │ │ Author │ │ │ │ │ + Akshay Krishnan │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - March 2019 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_O_n_-_i_n_l_._h │ │ │ │ │ + * _L_i_n_e_3_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00281_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SOn-inl.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Line3.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,162 +98,130 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
SOn-inl.h
│ │ │ │ +
Line3.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ -
2
│ │ │ │ -
3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
│ │ │ │ -
4 * Atlanta, Georgia 30332-0415
│ │ │ │ -
5 * All Rights Reserved
│ │ │ │ -
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ -
7
│ │ │ │ -
8 * See LICENSE for the license information
│ │ │ │ -
9
│ │ │ │ -
10 * -------------------------------------------------------------------------- */
│ │ │ │ -
11
│ │ │ │ -
12#pragma once
│ │ │ │ -
13
│ │ │ │ -
21#include <gtsam/base/Matrix.h>
│ │ │ │ -
22
│ │ │ │ -
23#include <iostream>
│ │ │ │ -
24
│ │ │ │ -
25namespace gtsam {
│ │ │ │ -
26
│ │ │ │ -
27// Implementation for N>=5 just uses dynamic version
│ │ │ │ -
28template <int N>
│ │ │ │ -
│ │ │ │ -
29typename SO<N>::MatrixNN SO<N>::Hat(const TangentVector& xi) {
│ │ │ │ -
30 return SOn::Hat(xi);
│ │ │ │ -
31}
│ │ │ │ -
│ │ │ │ -
32
│ │ │ │ -
33// Implementation for N>=5 just uses dynamic version
│ │ │ │ -
34template <int N>
│ │ │ │ -
│ │ │ │ -
35typename SO<N>::TangentVector SO<N>::Vee(const MatrixNN& X) {
│ │ │ │ -
36 return SOn::Vee(X);
│ │ │ │ -
37}
│ │ │ │ -
│ │ │ │ +
2 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
3 * Atlanta, Georgia 30332-0415
│ │ │ │ +
4 * All Rights Reserved
│ │ │ │ +
5 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
6 * See LICENSE for the license information
│ │ │ │ +
7 * -------------------------------------------------------------------------- */
│ │ │ │ +
8
│ │ │ │ +
15// \callgraph
│ │ │ │ +
16
│ │ │ │ +
17#pragma once
│ │ │ │ +
18
│ │ │ │ +
19#include <gtsam/geometry/Rot3.h>
│ │ │ │ +
20#include <gtsam/geometry/Pose3.h>
│ │ │ │ +
21
│ │ │ │ +
22namespace gtsam {
│ │ │ │ +
23
│ │ │ │ +
24class Line3;
│ │ │ │ +
25
│ │ │ │ +
34GTSAM_EXPORT Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
│ │ │ │ +
35 OptionalJacobian<4, 6> Dpose = boost::none,
│ │ │ │ +
36 OptionalJacobian<4, 4> Dline = boost::none);
│ │ │ │ +
37
│ │ │ │
38
│ │ │ │ -
39template <int N>
│ │ │ │ -
│ │ │ │ -
40SO<N> SO<N>::ChartAtOrigin::Retract(const TangentVector& xi, ChartJacobian H) {
│ │ │ │ -
41 if (H) throw std::runtime_error("SO<N>::Retract jacobian not implemented.");
│ │ │ │ -
42 const Matrix X = Hat(xi / 2.0);
│ │ │ │ -
43 size_t n = AmbientDim(xi.size());
│ │ │ │ -
44 const auto I = Eigen::MatrixXd::Identity(n, n);
│ │ │ │ -
45 // https://pdfs.semanticscholar.org/6165/0347b2ccac34b5f423081d1ce4dbc4d09475.pdf
│ │ │ │ -
46 return SO((I + X) * (I - X).inverse());
│ │ │ │ -
47}
│ │ │ │ -
│ │ │ │ -
48
│ │ │ │ -
49template <int N>
│ │ │ │ -
│ │ │ │ -
50typename SO<N>::TangentVector SO<N>::ChartAtOrigin::Local(const SO& R,
│ │ │ │ -
51 ChartJacobian H) {
│ │ │ │ -
52 if (H) throw std::runtime_error("SO<N>::Local jacobian not implemented.");
│ │ │ │ -
53 const size_t n = R.rows();
│ │ │ │ -
54 const auto I = Eigen::MatrixXd::Identity(n, n);
│ │ │ │ -
55 const Matrix X = (I - R.matrix_) * (I + R.matrix_).inverse();
│ │ │ │ -
56 return -2 * Vee(X);
│ │ │ │ -
57}
│ │ │ │ -
│ │ │ │ -
58
│ │ │ │ -
59template <int N>
│ │ │ │ -
60typename SO<N>::MatrixDD SO<N>::AdjointMap() const {
│ │ │ │ -
61 if (N==2) return I_1x1; // SO(2) case
│ │ │ │ -
62 throw std::runtime_error(
│ │ │ │ -
63 "SO<N>::AdjointMap only implemented for SO2, SO3 and SO4.");
│ │ │ │ -
64}
│ │ │ │ -
65
│ │ │ │ -
66template <int N>
│ │ │ │ -
│ │ │ │ -
67SO<N> SO<N>::Expmap(const TangentVector& omega, ChartJacobian H) {
│ │ │ │ -
68 throw std::runtime_error("SO<N>::Expmap only implemented for SO3 and SO4.");
│ │ │ │ -
69}
│ │ │ │ -
│ │ │ │ -
70
│ │ │ │ -
71template <int N>
│ │ │ │ -
│ │ │ │ -
72typename SO<N>::MatrixDD SO<N>::ExpmapDerivative(const TangentVector& omega) {
│ │ │ │ -
73 throw std::runtime_error("SO<N>::ExpmapDerivative only implemented for SO3.");
│ │ │ │ -
74}
│ │ │ │ -
│ │ │ │ -
75
│ │ │ │ -
76template <int N>
│ │ │ │ -
│ │ │ │ -
77typename SO<N>::TangentVector SO<N>::Logmap(const SO& R, ChartJacobian H) {
│ │ │ │ -
78 throw std::runtime_error("SO<N>::Logmap only implemented for SO3.");
│ │ │ │ -
79}
│ │ │ │ -
│ │ │ │ -
80
│ │ │ │ -
81template <int N>
│ │ │ │ -
│ │ │ │ -
82typename SO<N>::MatrixDD SO<N>::LogmapDerivative(const TangentVector& omega) {
│ │ │ │ -
83 throw std::runtime_error("O<N>::LogmapDerivative only implemented for SO3.");
│ │ │ │ -
84}
│ │ │ │ -
│ │ │ │ -
85
│ │ │ │ -
86// Default fixed size version (but specialized elsewehere for N=2,3,4)
│ │ │ │ -
87template <int N>
│ │ │ │ -
│ │ │ │ -
88typename SO<N>::VectorN2 SO<N>::vec(
│ │ │ │ -
89 OptionalJacobian<internal::NSquaredSO(N), dimension> H) const {
│ │ │ │ -
90 // Vectorize
│ │ │ │ -
91 VectorN2 X = Eigen::Map<const VectorN2>(matrix_.data());
│ │ │ │ -
92
│ │ │ │ -
93 // If requested, calculate H as (I \oplus Q) * P,
│ │ │ │ -
94 // where Q is the N*N rotation matrix, and P is calculated below.
│ │ │ │ -
95 if (H) {
│ │ │ │ -
96 // Calculate P matrix of vectorized generators
│ │ │ │ -
97 // TODO(duy): Should we refactor this as the jacobian of Hat?
│ │ │ │ -
98 Matrix P = SO<N>::VectorizedGenerators();
│ │ │ │ -
99 for (size_t i = 0; i < N; i++) {
│ │ │ │ -
100 H->block(i * N, 0, N, dimension) =
│ │ │ │ -
101 matrix_ * P.block(i * N, 0, N, dimension);
│ │ │ │ -
102 }
│ │ │ │ -
103 }
│ │ │ │ -
104 return X;
│ │ │ │ -
105}
│ │ │ │ -
│ │ │ │ -
106
│ │ │ │ -
107template <int N>
│ │ │ │ -
108void SO<N>::print(const std::string& s) const {
│ │ │ │ -
109 std::cout << s << matrix_ << std::endl;
│ │ │ │ -
110}
│ │ │ │ -
111
│ │ │ │ -
112} // namespace gtsam
│ │ │ │ -
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
│ │ │ │ +
44class GTSAM_EXPORT Line3 {
│ │ │ │ +
45 private:
│ │ │ │ +
46 Rot3 R_; // Rotation of line about x and y in world frame
│ │ │ │ +
47 double a_, b_; // Intersection of line with the world x-y plane rotated by R_
│ │ │ │ +
48 // Also the closest point on line to origin
│ │ │ │ +
49 public:
│ │ │ │ +
50 enum { dimension = 4 };
│ │ │ │ +
51
│ │ │ │ +
│ │ │ │ +
53 Line3() :
│ │ │ │ +
54 a_(0), b_(0) {}
│ │ │ │ +
│ │ │ │ +
55
│ │ │ │ +
│ │ │ │ +
57 Line3(const Rot3 &R, const double a, const double b) :
│ │ │ │ +
58 R_(R), a_(a), b_(b) {}
│ │ │ │ +
│ │ │ │ +
59
│ │ │ │ +
71 Line3 retract(const Vector4 &v,
│ │ │ │ +
72 OptionalJacobian<4, 4> Dp = boost::none,
│ │ │ │ +
73 OptionalJacobian<4, 4> Dv = boost::none) const;
│ │ │ │ +
74
│ │ │ │ +
84 Vector4 localCoordinates(const Line3 &q,
│ │ │ │ +
85 OptionalJacobian<4, 4> Dp = boost::none,
│ │ │ │ +
86 OptionalJacobian<4, 4> Dq = boost::none) const;
│ │ │ │ +
87
│ │ │ │ +
92 void print(const std::string &s = "") const;
│ │ │ │ +
93
│ │ │ │ +
100 bool equals(const Line3 &l2, double tol = 10e-9) const;
│ │ │ │ +
101
│ │ │ │ +
108 Unit3 project(OptionalJacobian<2, 4> Dline = boost::none) const;
│ │ │ │ +
109
│ │ │ │ +
118 Point3 point(double distance = 0) const;
│ │ │ │ +
119
│ │ │ │ +
│ │ │ │ +
123 inline Rot3 R() const {
│ │ │ │ +
124 return R_;
│ │ │ │ +
125 }
│ │ │ │ +
│ │ │ │ +
126
│ │ │ │ +
│ │ │ │ +
130 inline double a() const {
│ │ │ │ +
131 return a_;
│ │ │ │ +
132 }
│ │ │ │ +
│ │ │ │ +
133
│ │ │ │ +
│ │ │ │ +
137 inline double b() const {
│ │ │ │ +
138 return b_;
│ │ │ │ +
139 }
│ │ │ │ +
│ │ │ │ +
140
│ │ │ │ +
149 friend Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
│ │ │ │ +
150 OptionalJacobian<4, 6> Dpose,
│ │ │ │ +
151 OptionalJacobian<4, 4> Dline);
│ │ │ │ +
152};
│ │ │ │ +
│ │ │ │ +
153
│ │ │ │ +
154template<>
│ │ │ │ +
155struct traits<Line3> : public internal::Manifold<Line3> {};
│ │ │ │ +
156
│ │ │ │ +
157template<>
│ │ │ │ +
158struct traits<const Line3> : public internal::Manifold<Line3> {};
│ │ │ │ +
159}
│ │ │ │ +
Rot3.h
3D rotation represented as a rotation matrix or quaternion
│ │ │ │ +
Pose3.h
3D Pose
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ +
gtsam::transformTo
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Transform a line from world to camera frame.
Definition Line3.cpp:94
│ │ │ │ +
gtsam::project
Point2_ project(const Point3_ &p_cam)
Expression version of PinholeBase::Project.
Definition expressions.h:131
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
│ │ │ │
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
gtsam::SO
Manifold of special orthogonal rotation matrices SO<N>.
Definition SOn.h:52
│ │ │ │ -
gtsam::SO::Expmap
static SO Expmap(const TangentVector &omega, ChartJacobian H=boost::none)
Exponential map at identity - create a rotation from canonical coordinates.
Definition SOn-inl.h:67
│ │ │ │ -
gtsam::SO::VectorizedGenerators
static Matrix VectorizedGenerators()
Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N)
Definition SOn.h:300
│ │ │ │ -
gtsam::SO::inverse
SO inverse() const
inverse of a rotation = transpose
Definition SOn.h:193
│ │ │ │ -
gtsam::SO::vec
VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost::none) const
Return vectorized rotation matrix in column order.
Definition SOn-inl.h:88
│ │ │ │ -
gtsam::SO::Vee
static TangentVector Vee(const MatrixNN &X)
Inverse of Hat. See note about xi element order in Hat.
Definition SOn-inl.h:35
│ │ │ │ -
gtsam::SO::matrix_
MatrixNN matrix_
Rotation matrix.
Definition SOn.h:62
│ │ │ │ -
gtsam::SO::Logmap
static TangentVector Logmap(const SO &R, ChartJacobian H=boost::none)
Log map at identity - returns the canonical coordinates of this rotation.
Definition SOn-inl.h:77
│ │ │ │ -
gtsam::SO::AdjointMap
MatrixDD AdjointMap() const
Adjoint map.
Definition SO4.cpp:159
│ │ │ │ -
gtsam::SO::Hat
static MatrixNN Hat(const TangentVector &xi)
Hat operator creates Lie algebra element corresponding to d-vector, where d is the dimensionality of ...
Definition SOn-inl.h:29
│ │ │ │ -
gtsam::SO::ExpmapDerivative
static MatrixDD ExpmapDerivative(const TangentVector &omega)
Derivative of Expmap, currently only defined for SO3.
Definition SOn-inl.h:72
│ │ │ │ -
gtsam::SO::LogmapDerivative
static MatrixDD LogmapDerivative(const TangentVector &omega)
Derivative of Logmap, currently only defined for SO3.
Definition SOn-inl.h:82
│ │ │ │ -
gtsam::SO::SO
SO()
Construct SO<N> identity for N >= 2.
Definition SOn.h:79
│ │ │ │ -
gtsam::SO::ChartAtOrigin::Local
static TangentVector Local(const SO &R, ChartJacobian H=boost::none)
Inverse of Retract.
Definition SOn-inl.h:50
│ │ │ │ -
gtsam::SO::ChartAtOrigin::Retract
static SO Retract(const TangentVector &xi, ChartJacobian H=boost::none)
Retract uses Cayley map.
Definition SOn-inl.h:40
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::Line3
A 3D line (R,a,b) : (Rot3,Scalar,Scalar)
Definition Line3.h:44
│ │ │ │ +
gtsam::Line3::R
Rot3 R() const
Return the rotation of the line.
Definition Line3.h:123
│ │ │ │ +
gtsam::Line3::Line3
Line3()
Default constructor is the Z axis.
Definition Line3.h:53
│ │ │ │ +
gtsam::Line3::Line3
Line3(const Rot3 &R, const double a, const double b)
Constructor for general line from (R, a, b)
Definition Line3.h:57
│ │ │ │ +
gtsam::Line3::b
double b() const
Return the y-coordinate of the intersection of the line with the xy plane.
Definition Line3.h:137
│ │ │ │ +
gtsam::Line3::a
double a() const
Return the x-coordinate of the intersection of the line with the xy plane.
Definition Line3.h:130
│ │ │ │ +
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ +
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
│ │ │ │ +
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,194 +1,163 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SOn-inl.h │ │ │ │ │ +Line3.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ -2 │ │ │ │ │ -3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, │ │ │ │ │ -4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -5 * All Rights Reserved │ │ │ │ │ -6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ -7 │ │ │ │ │ -8 * See LICENSE for the license information │ │ │ │ │ -9 │ │ │ │ │ -10 * ------------------------------------------------------------------------- │ │ │ │ │ +2 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +3 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +4 * All Rights Reserved │ │ │ │ │ +5 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +6 * See LICENSE for the license information │ │ │ │ │ +7 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ -11 │ │ │ │ │ -12#pragma once │ │ │ │ │ -13 │ │ │ │ │ -21#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ -22 │ │ │ │ │ -23#include │ │ │ │ │ -24 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ -26 │ │ │ │ │ -27// Implementation for N>=5 just uses dynamic version │ │ │ │ │ -28template │ │ │ │ │ -_2_9typename SO::MatrixNN _S_O_<_N_>_:_:_H_a_t(const TangentVector& xi) { │ │ │ │ │ -30 return _S_O_n_:_:_H_a_t(xi); │ │ │ │ │ -31} │ │ │ │ │ -32 │ │ │ │ │ -33// Implementation for N>=5 just uses dynamic version │ │ │ │ │ -34template │ │ │ │ │ -_3_5typename SO::TangentVector _S_O_<_N_>_:_:_V_e_e(const MatrixNN& X) { │ │ │ │ │ -36 return _S_O_n_:_:_V_e_e(X); │ │ │ │ │ -37} │ │ │ │ │ +8 │ │ │ │ │ +15// \callgraph │ │ │ │ │ +16 │ │ │ │ │ +17#pragma once │ │ │ │ │ +18 │ │ │ │ │ +19#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_3_._h> │ │ │ │ │ +20#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_3_._h> │ │ │ │ │ +21 │ │ │ │ │ +22namespace _g_t_s_a_m { │ │ │ │ │ +23 │ │ │ │ │ +24class Line3; │ │ │ │ │ +25 │ │ │ │ │ +34GTSAM_EXPORT Line3 _t_r_a_n_s_f_o_r_m_T_o(const Pose3 &wTc, const Line3 &wL, │ │ │ │ │ +35 OptionalJacobian<4, 6> Dpose = boost::none, │ │ │ │ │ +36 OptionalJacobian<4, 4> Dline = boost::none); │ │ │ │ │ +37 │ │ │ │ │ 38 │ │ │ │ │ -39template │ │ │ │ │ -_4_0_S_O_<_N_> _S_O_<_N_>_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n_:_:_R_e_t_r_a_c_t(const TangentVector& xi, _C_h_a_r_t_J_a_c_o_b_i_a_n H) │ │ │ │ │ -{ │ │ │ │ │ -41 if (H) throw std::runtime_error("SO::Retract jacobian not implemented."); │ │ │ │ │ -42 const Matrix X = _H_a_t(xi / 2.0); │ │ │ │ │ -43 size_t n = AmbientDim(xi.size()); │ │ │ │ │ -44 const auto I = Eigen::MatrixXd::Identity(n, n); │ │ │ │ │ -45 // https://pdfs.semanticscholar.org/6165/ │ │ │ │ │ -0347b2ccac34b5f423081d1ce4dbc4d09475.pdf │ │ │ │ │ -46 return _S_O((I + X) * (I - X)._i_n_v_e_r_s_e()); │ │ │ │ │ -47} │ │ │ │ │ -48 │ │ │ │ │ -49template │ │ │ │ │ -_5_0typename SO::TangentVector _S_O_<_N_>_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n_:_:_L_o_c_a_l(const _S_O& R, │ │ │ │ │ -51 _C_h_a_r_t_J_a_c_o_b_i_a_n H) { │ │ │ │ │ -52 if (H) throw std::runtime_error("SO::Local jacobian not implemented."); │ │ │ │ │ -53 const size_t n = R.rows(); │ │ │ │ │ -54 const auto I = Eigen::MatrixXd::Identity(n, n); │ │ │ │ │ -55 const Matrix X = (I - R.matrix_) * (I + R.matrix_).inverse(); │ │ │ │ │ -56 return -2 * _V_e_e(X); │ │ │ │ │ -57} │ │ │ │ │ -58 │ │ │ │ │ -59template │ │ │ │ │ -60typename SO::MatrixDD _S_O_<_N_>_:_:_A_d_j_o_i_n_t_M_a_p() const { │ │ │ │ │ -61 if (N==2) return I_1x1; // SO(2) case │ │ │ │ │ -62 throw std::runtime_error( │ │ │ │ │ -63 "SO::AdjointMap only implemented for SO2, SO3 and SO4."); │ │ │ │ │ -64} │ │ │ │ │ -65 │ │ │ │ │ -66template │ │ │ │ │ -_6_7_S_O_<_N_> _S_O_<_N_>_:_:_E_x_p_m_a_p(const TangentVector& omega, _C_h_a_r_t_J_a_c_o_b_i_a_n H) { │ │ │ │ │ -68 throw std::runtime_error("SO::Expmap only implemented for SO3 and SO4."); │ │ │ │ │ -69} │ │ │ │ │ -70 │ │ │ │ │ -71template │ │ │ │ │ -_7_2typename SO::MatrixDD _S_O_<_N_>_:_:_E_x_p_m_a_p_D_e_r_i_v_a_t_i_v_e(const TangentVector& omega) │ │ │ │ │ -{ │ │ │ │ │ -73 throw std::runtime_error("SO::ExpmapDerivative only implemented for │ │ │ │ │ -SO3."); │ │ │ │ │ -74} │ │ │ │ │ -75 │ │ │ │ │ -76template │ │ │ │ │ -_7_7typename SO::TangentVector _S_O_<_N_>_:_:_L_o_g_m_a_p(const _S_O& R, _C_h_a_r_t_J_a_c_o_b_i_a_n H) { │ │ │ │ │ -78 throw std::runtime_error("SO::Logmap only implemented for SO3."); │ │ │ │ │ -79} │ │ │ │ │ -80 │ │ │ │ │ -81template │ │ │ │ │ -_8_2typename SO::MatrixDD _S_O_<_N_>_:_:_L_o_g_m_a_p_D_e_r_i_v_a_t_i_v_e(const TangentVector& omega) │ │ │ │ │ -{ │ │ │ │ │ -83 throw std::runtime_error("O::LogmapDerivative only implemented for │ │ │ │ │ -SO3."); │ │ │ │ │ -84} │ │ │ │ │ -85 │ │ │ │ │ -86// Default fixed size version (but specialized elsewehere for N=2,3,4) │ │ │ │ │ -87template │ │ │ │ │ -_8_8typename SO::VectorN2 _S_O_<_N_>_:_:_v_e_c( │ │ │ │ │ -89 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n H) const { │ │ │ │ │ -90 // Vectorize │ │ │ │ │ -91 VectorN2 X = Eigen::Map(_m_a_t_r_i_x__.data()); │ │ │ │ │ -92 │ │ │ │ │ -93 // If requested, calculate H as (I \oplus Q) * P, │ │ │ │ │ -94 // where Q is the N*N rotation matrix, and P is calculated below. │ │ │ │ │ -95 if (H) { │ │ │ │ │ -96 // Calculate P matrix of vectorized generators │ │ │ │ │ -97 // TODO(duy): Should we refactor this as the jacobian of Hat? │ │ │ │ │ -98 Matrix P = _S_O_<_N_>_:_:_V_e_c_t_o_r_i_z_e_d_G_e_n_e_r_a_t_o_r_s(); │ │ │ │ │ -99 for (size_t i = 0; i < N; i++) { │ │ │ │ │ -100 H->block(i * N, 0, N, dimension) = │ │ │ │ │ -101 _m_a_t_r_i_x__ * P.block(i * N, 0, N, dimension); │ │ │ │ │ -102 } │ │ │ │ │ -103 } │ │ │ │ │ -104 return X; │ │ │ │ │ -105} │ │ │ │ │ -106 │ │ │ │ │ -107template │ │ │ │ │ -108void _S_O_<_N_>_:_:_p_r_i_n_t(const std::string& s) const { │ │ │ │ │ -109 std::cout << s << _m_a_t_r_i_x__ << std::endl; │ │ │ │ │ -110} │ │ │ │ │ -111 │ │ │ │ │ -112} // namespace gtsam │ │ │ │ │ -_M_a_t_r_i_x_._h │ │ │ │ │ -typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +_4_4class GTSAM_EXPORT _L_i_n_e_3 { │ │ │ │ │ +45 private: │ │ │ │ │ +46 _R_o_t_3 R_; // Rotation of line about x and y in world frame │ │ │ │ │ +47 double a_, b_; // Intersection of line with the world x-y plane rotated by │ │ │ │ │ +R_ │ │ │ │ │ +48 // Also the closest point on line to origin │ │ │ │ │ +49 public: │ │ │ │ │ +50 enum { dimension = 4 }; │ │ │ │ │ +51 │ │ │ │ │ +_5_3 _L_i_n_e_3() : │ │ │ │ │ +54 a_(0), b_(0) {} │ │ │ │ │ +55 │ │ │ │ │ +_5_7 _L_i_n_e_3(const _R_o_t_3 &R, const double a, const double b) : │ │ │ │ │ +58 R_(R), a_(a), b_(b) {} │ │ │ │ │ +59 │ │ │ │ │ +71 _L_i_n_e_3 retract(const Vector4 &v, │ │ │ │ │ +72 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_4_,_ _4_> Dp = boost::none, │ │ │ │ │ +73 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_4_,_ _4_> Dv = boost::none) const; │ │ │ │ │ +74 │ │ │ │ │ +84 Vector4 localCoordinates(const _L_i_n_e_3 &q, │ │ │ │ │ +85 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_4_,_ _4_> Dp = boost::none, │ │ │ │ │ +86 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_4_,_ _4_> Dq = boost::none) const; │ │ │ │ │ +87 │ │ │ │ │ +92 void _p_r_i_n_t(const std::string &s = "") const; │ │ │ │ │ +93 │ │ │ │ │ +100 bool _e_q_u_a_l_s(const _L_i_n_e_3 &l2, double tol = 10e-9) const; │ │ │ │ │ +101 │ │ │ │ │ +108 _U_n_i_t_3 _p_r_o_j_e_c_t(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _4_> Dline = boost::none) const; │ │ │ │ │ +109 │ │ │ │ │ +118 _P_o_i_n_t_3 point(double distance = 0) const; │ │ │ │ │ +119 │ │ │ │ │ +_1_2_3 inline _R_o_t_3 _R() const { │ │ │ │ │ +124 return R_; │ │ │ │ │ +125 } │ │ │ │ │ +126 │ │ │ │ │ +_1_3_0 inline double _a() const { │ │ │ │ │ +131 return a_; │ │ │ │ │ +132 } │ │ │ │ │ +133 │ │ │ │ │ +_1_3_7 inline double _b() const { │ │ │ │ │ +138 return b_; │ │ │ │ │ +139 } │ │ │ │ │ +140 │ │ │ │ │ +149 friend _L_i_n_e_3 _t_r_a_n_s_f_o_r_m_T_o(const _P_o_s_e_3 &wTc, const _L_i_n_e_3 &wL, │ │ │ │ │ +150 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_4_,_ _6_> Dpose, │ │ │ │ │ +151 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_4_,_ _4_> Dline); │ │ │ │ │ +152}; │ │ │ │ │ +153 │ │ │ │ │ +154template<> │ │ │ │ │ +_1_5_5struct _t_r_a_i_t_s<_L_i_n_e_3> : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d {}; │ │ │ │ │ +156 │ │ │ │ │ +157template<> │ │ │ │ │ +_1_5_8struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d {}; │ │ │ │ │ +159} │ │ │ │ │ +_R_o_t_3_._h │ │ │ │ │ +3D rotation represented as a rotation matrix or quaternion │ │ │ │ │ +_P_o_s_e_3_._h │ │ │ │ │ +3D Pose │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ +Vector3 Point3 │ │ │ │ │ +As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ +typedef Point3 to Vector3... │ │ │ │ │ +DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_n_s_f_o_r_m_T_o │ │ │ │ │ +Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > │ │ │ │ │ +Dpose, OptionalJacobian< 4, 4 > Dline) │ │ │ │ │ +Transform a line from world to camera frame. │ │ │ │ │ +DDeeffiinniittiioonn Line3.cpp:94 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_o_j_e_c_t │ │ │ │ │ +Point2_ project(const Point3_ &p_cam) │ │ │ │ │ +Expression version of PinholeBase::Project. │ │ │ │ │ +DDeeffiinniittiioonn expressions.h:131 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ +Both ManifoldTraits and Testable. │ │ │ │ │ +DDeeffiinniittiioonn Manifold.h:120 │ │ │ │ │ _g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ either a fixed size o... │ │ │ │ │ DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O │ │ │ │ │ -Manifold of special orthogonal rotation matrices SO. │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_E_x_p_m_a_p │ │ │ │ │ -static SO Expmap(const TangentVector &omega, ChartJacobian H=boost::none) │ │ │ │ │ -Exponential map at identity - create a rotation from canonical coordinates. │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_V_e_c_t_o_r_i_z_e_d_G_e_n_e_r_a_t_o_r_s │ │ │ │ │ -static Matrix VectorizedGenerators() │ │ │ │ │ -Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N) │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:300 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_i_n_v_e_r_s_e │ │ │ │ │ -SO inverse() const │ │ │ │ │ -inverse of a rotation = transpose │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:193 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_v_e_c │ │ │ │ │ -VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost:: │ │ │ │ │ -none) const │ │ │ │ │ -Return vectorized rotation matrix in column order. │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:88 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_V_e_e │ │ │ │ │ -static TangentVector Vee(const MatrixNN &X) │ │ │ │ │ -Inverse of Hat. See note about xi element order in Hat. │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_m_a_t_r_i_x__ │ │ │ │ │ -MatrixNN matrix_ │ │ │ │ │ -Rotation matrix. │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:62 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_L_o_g_m_a_p │ │ │ │ │ -static TangentVector Logmap(const SO &R, ChartJacobian H=boost::none) │ │ │ │ │ -Log map at identity - returns the canonical coordinates of this rotation. │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_A_d_j_o_i_n_t_M_a_p │ │ │ │ │ -MatrixDD AdjointMap() const │ │ │ │ │ -Adjoint map. │ │ │ │ │ -DDeeffiinniittiioonn SO4.cpp:159 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_H_a_t │ │ │ │ │ -static MatrixNN Hat(const TangentVector &xi) │ │ │ │ │ -Hat operator creates Lie algebra element corresponding to d-vector, where d is │ │ │ │ │ -the dimensionality of ... │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:29 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_E_x_p_m_a_p_D_e_r_i_v_a_t_i_v_e │ │ │ │ │ -static MatrixDD ExpmapDerivative(const TangentVector &omega) │ │ │ │ │ -Derivative of Expmap, currently only defined for SO3. │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:72 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_L_o_g_m_a_p_D_e_r_i_v_a_t_i_v_e │ │ │ │ │ -static MatrixDD LogmapDerivative(const TangentVector &omega) │ │ │ │ │ -Derivative of Logmap, currently only defined for SO3. │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:82 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_S_O │ │ │ │ │ -SO() │ │ │ │ │ -Construct SO identity for N >= 2. │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:79 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n_:_:_L_o_c_a_l │ │ │ │ │ -static TangentVector Local(const SO &R, ChartJacobian H=boost::none) │ │ │ │ │ -Inverse of Retract. │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n_:_:_R_e_t_r_a_c_t │ │ │ │ │ -static SO Retract(const TangentVector &xi, ChartJacobian H=boost::none) │ │ │ │ │ -Retract uses Cayley map. │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_n_e_3 │ │ │ │ │ +A 3D line (R,a,b) : (Rot3,Scalar,Scalar) │ │ │ │ │ +DDeeffiinniittiioonn Line3.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_n_e_3_:_:_R │ │ │ │ │ +Rot3 R() const │ │ │ │ │ +Return the rotation of the line. │ │ │ │ │ +DDeeffiinniittiioonn Line3.h:123 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_n_e_3_:_:_L_i_n_e_3 │ │ │ │ │ +Line3() │ │ │ │ │ +Default constructor is the Z axis. │ │ │ │ │ +DDeeffiinniittiioonn Line3.h:53 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_n_e_3_:_:_L_i_n_e_3 │ │ │ │ │ +Line3(const Rot3 &R, const double a, const double b) │ │ │ │ │ +Constructor for general line from (R, a, b) │ │ │ │ │ +DDeeffiinniittiioonn Line3.h:57 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_n_e_3_:_:_b │ │ │ │ │ +double b() const │ │ │ │ │ +Return the y-coordinate of the intersection of the line with the xy plane. │ │ │ │ │ +DDeeffiinniittiioonn Line3.h:137 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_n_e_3_:_:_a │ │ │ │ │ +double a() const │ │ │ │ │ +Return the x-coordinate of the intersection of the line with the xy plane. │ │ │ │ │ +DDeeffiinniittiioonn Line3.h:130 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ +A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ +Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor │ │ │ │ │ +symbol GTSAM_USE_QUATERNIO... │ │ │ │ │ +DDeeffiinniittiioonn Rot3.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_U_n_i_t_3 │ │ │ │ │ +Represents a 3D point on a unit sphere. │ │ │ │ │ +DDeeffiinniittiioonn Unit3.h:43 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_O_n_-_i_n_l_._h │ │ │ │ │ + * _L_i_n_e_3_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00284.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3_S2.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Point2.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,47 +95,91 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
Cal3_S2.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ +Functions
│ │ │ │ +
Point2.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

The most common 5DOF 3D->2D calibration. │ │ │ │ +

2D Point │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::Cal3_S2
 The most common 5DOF 3D->2D calibration. More...
 
struct  gtsam::traits< Cal3_S2 >
 
struct  gtsam::traits< const Cal3_S2 >
struct  gtsam::Range< Point2, Point2 >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Typedefs

│ │ │ │ +typedef Vector2 gtsam::Point2
 As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2.
 
│ │ │ │ +using gtsam::Point2Pair = std::pair< Point2, Point2 >
 
│ │ │ │ +using gtsam::Point2Pairs = std::vector< Point2Pair >
 
│ │ │ │ +typedef std::vector< Point2, Eigen::aligned_allocator< Point2 > > gtsam::Point2Vector
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +ostream & gtsam::operator<< (ostream &os, const gtsam::Point2Pair &p)
 
│ │ │ │ +double gtsam::norm2 (const Point2 &p, OptionalJacobian< 1, 2 > H=boost::none)
 Distance of the point from the origin, with Jacobian.
 
│ │ │ │ +double gtsam::distance2 (const Point2 &p1, const Point2 &q, OptionalJacobian< 1, 2 > H1=boost::none, OptionalJacobian< 1, 2 > H2=boost::none)
 distance between two points
 
│ │ │ │ +Point2 gtsam::operator* (double s, const Point2 &p)
 multiply with scalar
 
│ │ │ │ +boost::optional< Point2 > gtsam::circleCircleIntersection (double R_d, double r_d, double tol)
 
│ │ │ │ +list< Point2 > gtsam::circleCircleIntersection (Point2 c1, Point2 c2, boost::optional< Point2 > fh)
 
│ │ │ │ +Point2Pair gtsam::means (const std::vector< Point2Pair > &abPointPairs)
 Calculate the two means of a set of Point2 pairs.
 
list< Point2 > gtsam::circleCircleIntersection (Point2 c1, double r1, Point2 c2, double r2, double tol=1e-9)
 Intersect 2 circles.
 
│ │ │ │

Detailed Description

│ │ │ │ -

The most common 5DOF 3D->2D calibration.

│ │ │ │ +

2D Point

│ │ │ │
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,29 +1,70 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Cal3_S2.h File Reference │ │ │ │ │ -The most common 5DOF 3D->2D calibration. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Point2.h File Reference │ │ │ │ │ +2D Point _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_C_a_l_3___S_2 │ │ │ │ │ -  The most common 5DOF 3D->2D calibration. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _C_a_l_3___S_2_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _C_a_l_3___S_2_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_R_a_n_g_e_<_ _P_o_i_n_t_2_,_ _P_o_i_n_t_2_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ + typedef Vector2  ggttssaamm::::PPooiinntt22 │ │ │ │ │ + As of GTSAM 4, in order to make GTSAM more │ │ │ │ │ +  lean, it is now possible to just typedef │ │ │ │ │ + Point2 to Vector2. │ │ │ │ │ +  │ │ │ │ │ + using  ggttssaamm::::PPooiinntt22PPaaiirr = std::pair< _P_o_i_n_t_2, │ │ │ │ │ + _P_o_i_n_t_2 > │ │ │ │ │ +  │ │ │ │ │ + using  ggttssaamm::::PPooiinntt22PPaaiirrss = std::vector< │ │ │ │ │ + Point2Pair > │ │ │ │ │ +  │ │ │ │ │ +typedef std::vector< _P_o_i_n_t_2, Eigen:: │ │ │ │ │ + aligned_allocator< _P_o_i_n_t_2 > >  ggttssaamm::::PPooiinntt22VVeeccttoorr │ │ │ │ │ +  │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ + ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const gtsam:: │ │ │ │ │ + Point2Pair &p) │ │ │ │ │ +  │ │ │ │ │ + double  ggttssaamm::::nnoorrmm22 (const _P_o_i_n_t_2 &p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 1, │ │ │ │ │ + 2 > H=boost::none) │ │ │ │ │ +  Distance of the point from the origin, with │ │ │ │ │ + Jacobian. │ │ │ │ │ +  │ │ │ │ │ + double  ggttssaamm::::ddiissttaannccee22 (const _P_o_i_n_t_2 &p1, const _P_o_i_n_t_2 &q, │ │ │ │ │ + _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 1, 2 > H1=boost::none, │ │ │ │ │ + _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 1, 2 > H2=boost::none) │ │ │ │ │ +  distance between two points │ │ │ │ │ +  │ │ │ │ │ + _P_o_i_n_t_2  ggttssaamm::::ooppeerraattoorr** (double s, const _P_o_i_n_t_2 &p) │ │ │ │ │ +  multiply with scalar │ │ │ │ │ +  │ │ │ │ │ +boost::optional< _P_o_i_n_t_2 >  ggttssaamm::::cciirrcclleeCCiirrcclleeIInntteerrsseeccttiioonn (double R_d, double │ │ │ │ │ + r_d, double tol) │ │ │ │ │ +  │ │ │ │ │ + list< _P_o_i_n_t_2 >  ggttssaamm::::cciirrcclleeCCiirrcclleeIInntteerrsseeccttiioonn (_P_o_i_n_t_2 c1, _P_o_i_n_t_2 │ │ │ │ │ + c2, boost::optional< _P_o_i_n_t_2 > fh) │ │ │ │ │ +  │ │ │ │ │ + Point2Pair  ggttssaamm::::mmeeaannss (const std::vector< Point2Pair > │ │ │ │ │ + &abPointPairs) │ │ │ │ │ +  Calculate the two means of a set of Point2 pairs. │ │ │ │ │ +  │ │ │ │ │ + list< _P_o_i_n_t_2 >  _g_t_s_a_m_:_:_c_i_r_c_l_e_C_i_r_c_l_e_I_n_t_e_r_s_e_c_t_i_o_n (_P_o_i_n_t_2 c1, double │ │ │ │ │ + r1, _P_o_i_n_t_2 c2, double r2, double tol=1e-9) │ │ │ │ │ +  Intersect 2 circles. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -The most common 5DOF 3D->2D calibration. │ │ │ │ │ +2D Point │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3___S_2_._h │ │ │ │ │ + * _P_o_i_n_t_2_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00284.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,9 @@ │ │ │ │ │ var a00284 = [ │ │ │ │ │ - ["gtsam::traits< Cal3_S2 >", "a02948.html", null], │ │ │ │ │ - ["gtsam::traits< const Cal3_S2 >", "a02952.html", null] │ │ │ │ │ + ["gtsam::Range< Point2, Point2 >", "a03148.html", null], │ │ │ │ │ + ["Point2", "a00284.html#a6ede8384dee0353a0ce5fb54ea50c21d", null], │ │ │ │ │ + ["circleCircleIntersection", "a00284.html#a93d4e38a582b6f32fc4f301df10721d5", null], │ │ │ │ │ + ["distance2", "a00284.html#afd1282dfb080cb393ae60188b4582cb2", null], │ │ │ │ │ + ["means", "a00284.html#a9e357cda5287fae1438f86bc4df27a80", null], │ │ │ │ │ + ["norm2", "a00284.html#afee4e6aa4aba9b6a6b421ddd75b52dfc", null], │ │ │ │ │ + ["operator*", "a00284.html#a63d86e99c211c6daeac2b7b4dd9d928e", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00284_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3_S2.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Point2.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,142 +98,116 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Cal3_S2.h
│ │ │ │ +
Point2.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
22#pragma once
│ │ │ │ -
23
│ │ │ │ -
24#include <gtsam/geometry/Cal3.h>
│ │ │ │ -
25#include <gtsam/geometry/Point2.h>
│ │ │ │ -
26
│ │ │ │ -
27namespace gtsam {
│ │ │ │ -
28
│ │ │ │ -
│ │ │ │ -
34class GTSAM_EXPORT Cal3_S2 : public Cal3 {
│ │ │ │ -
35 public:
│ │ │ │ -
36 enum { dimension = 5 };
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/base/VectorSpace.h>
│ │ │ │ +
21#include <boost/serialization/nvp.hpp>
│ │ │ │ +
22
│ │ │ │ +
23namespace gtsam {
│ │ │ │ +
24
│ │ │ │ +
27typedef Vector2 Point2;
│ │ │ │ +
28
│ │ │ │ +
29// Convenience typedef
│ │ │ │ +
30using Point2Pair = std::pair<Point2, Point2>;
│ │ │ │ +
31GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p);
│ │ │ │ +
32
│ │ │ │ +
33using Point2Pairs = std::vector<Point2Pair>;
│ │ │ │ +
34
│ │ │ │ +
36GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none);
│ │ │ │
37
│ │ │ │ -
39 using shared_ptr = boost::shared_ptr<Cal3_S2>;
│ │ │ │ -
40
│ │ │ │ -
43
│ │ │ │ -
45 Cal3_S2() = default;
│ │ │ │ -
46
│ │ │ │ -
│ │ │ │ -
48 Cal3_S2(double fx, double fy, double s, double u0, double v0)
│ │ │ │ -
49 : Cal3(fx, fy, s, u0, v0) {}
│ │ │ │ +
39GTSAM_EXPORT double distance2(const Point2& p1, const Point2& q,
│ │ │ │ +
40 OptionalJacobian<1, 2> H1 = boost::none,
│ │ │ │ +
41 OptionalJacobian<1, 2> H2 = boost::none);
│ │ │ │ +
42
│ │ │ │ +
43// For MATLAB wrapper
│ │ │ │ +
44typedef std::vector<Point2, Eigen::aligned_allocator<Point2> > Point2Vector;
│ │ │ │ +
45
│ │ │ │ +
│ │ │ │ +
47inline Point2 operator*(double s, const Point2& p) {
│ │ │ │ +
48 return Point2(s * p.x(), s * p.y());
│ │ │ │ +
49}
│ │ │ │
│ │ │ │
50
│ │ │ │ -
52 Cal3_S2(const Vector5& d) : Cal3(d) {}
│ │ │ │ -
53
│ │ │ │ -
60 Cal3_S2(double fov, int w, int h) : Cal3(fov, w, h) {}
│ │ │ │ -
61
│ │ │ │ -
69 Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = boost::none,
│ │ │ │ -
70 OptionalJacobian<2, 2> Dp = boost::none) const;
│ │ │ │ -
71
│ │ │ │ -
79 Point2 calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = boost::none,
│ │ │ │ -
80 OptionalJacobian<2, 2> Dp = boost::none) const;
│ │ │ │ -
81
│ │ │ │ -
87 Vector3 calibrate(const Vector3& p) const;
│ │ │ │ -
88
│ │ │ │ +
51/*
│ │ │ │ +
52 * @brief Circle-circle intersection, given normalized radii.
│ │ │ │ +
53 * Calculate f and h, respectively the parallel and perpendicular distance of
│ │ │ │ +
54 * the intersections of two circles along and from the line connecting the centers.
│ │ │ │ +
55 * Both are dimensionless fractions of the distance d between the circle centers.
│ │ │ │ +
56 * If the circles do not intersect or they are identical, returns boost::none.
│ │ │ │ +
57 * If one solution (touching circles, as determined by tol), h will be exactly zero.
│ │ │ │ +
58 * h is a good measure for how accurate the intersection will be, as when circles touch
│ │ │ │ +
59 * or nearly touch, the intersection is ill-defined with noisy radius measurements.
│ │ │ │ +
60 * @param R_d : R/d, ratio of radius of first circle to distance between centers
│ │ │ │ +
61 * @param r_d : r/d, ratio of radius of second circle to distance between centers
│ │ │ │ +
62 * @param tol: absolute tolerance below which we consider touching circles
│ │ │ │ +
63 * @return optional Point2 with f and h, boost::none if no solution.
│ │ │ │ +
64 */
│ │ │ │ +
65GTSAM_EXPORT boost::optional<Point2> circleCircleIntersection(double R_d, double r_d, double tol = 1e-9);
│ │ │ │ +
66
│ │ │ │ +
67/*
│ │ │ │ +
68 * @brief Circle-circle intersection, from the normalized radii solution.
│ │ │ │ +
69 * @param c1 center of first circle
│ │ │ │ +
70 * @param c2 center of second circle
│ │ │ │ +
71 * @return list of solutions (0,1, or 2). Identical circles will return empty list, as well.
│ │ │ │ +
72 */
│ │ │ │ +
73GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh);
│ │ │ │ +
74
│ │ │ │ +
76GTSAM_EXPORT Point2Pair means(const std::vector<Point2Pair> &abPointPairs);
│ │ │ │ +
77
│ │ │ │ +
87GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, double r1,
│ │ │ │ +
88 Point2 c2, double r2, double tol = 1e-9);
│ │ │ │ +
89
│ │ │ │ +
90template <typename A1, typename A2>
│ │ │ │ +
91struct Range;
│ │ │ │
92
│ │ │ │ -
94 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
│ │ │ │ -
95 const Cal3_S2& cal);
│ │ │ │ -
96
│ │ │ │ -
98 void print(const std::string& s = "Cal3_S2") const override;
│ │ │ │ -
99
│ │ │ │ -
101 bool equals(const Cal3_S2& K, double tol = 10e-9) const;
│ │ │ │ +
93template <>
│ │ │ │ +
│ │ │ │ +
94struct Range<Point2, Point2> {
│ │ │ │ +
95 typedef double result_type;
│ │ │ │ +
96 double operator()(const Point2& p, const Point2& q,
│ │ │ │ +
97 OptionalJacobian<1, 2> H1 = boost::none,
│ │ │ │ +
98 OptionalJacobian<1, 2> H2 = boost::none) {
│ │ │ │ +
99 return distance2(p, q, H1, H2);
│ │ │ │ +
100 }
│ │ │ │ +
101};
│ │ │ │ +
│ │ │ │
102
│ │ │ │ -
│ │ │ │ -
104 inline Cal3_S2 between(const Cal3_S2& q,
│ │ │ │ -
105 OptionalJacobian<5, 5> H1 = boost::none,
│ │ │ │ -
106 OptionalJacobian<5, 5> H2 = boost::none) const {
│ │ │ │ -
107 if (H1) *H1 = -I_5x5;
│ │ │ │ -
108 if (H2) *H2 = I_5x5;
│ │ │ │ -
109 return Cal3_S2(q.fx_ - fx_, q.fy_ - fy_, q.s_ - s_, q.u0_ - u0_,
│ │ │ │ -
110 q.v0_ - v0_);
│ │ │ │ -
111 }
│ │ │ │ -
│ │ │ │ -
112
│ │ │ │ -
116
│ │ │ │ -
118 inline static size_t Dim() { return dimension; }
│ │ │ │ -
119
│ │ │ │ -
│ │ │ │ -
121 inline Cal3_S2 retract(const Vector& d) const {
│ │ │ │ -
122 return Cal3_S2(fx_ + d(0), fy_ + d(1), s_ + d(2), u0_ + d(3), v0_ + d(4));
│ │ │ │ -
123 }
│ │ │ │ -
│ │ │ │ -
124
│ │ │ │ -
│ │ │ │ -
126 Vector5 localCoordinates(const Cal3_S2& T2) const {
│ │ │ │ -
127 return T2.vector() - vector();
│ │ │ │ -
128 }
│ │ │ │ -
│ │ │ │ -
129
│ │ │ │ -
133
│ │ │ │ -
134 private:
│ │ │ │ -
136 friend class boost::serialization::access;
│ │ │ │ -
137 template <class Archive>
│ │ │ │ -
138 void serialize(Archive& ar, const unsigned int /*version*/) {
│ │ │ │ -
139 ar& boost::serialization::make_nvp(
│ │ │ │ -
140 "Cal3_S2", boost::serialization::base_object<Cal3>(*this));
│ │ │ │ -
141 }
│ │ │ │ -
142
│ │ │ │ -
144};
│ │ │ │ -
│ │ │ │ -
145
│ │ │ │ -
146template <>
│ │ │ │ -
147struct traits<Cal3_S2> : public internal::Manifold<Cal3_S2> {};
│ │ │ │ -
148
│ │ │ │ -
149template <>
│ │ │ │ -
150struct traits<const Cal3_S2> : public internal::Manifold<Cal3_S2> {};
│ │ │ │ -
151
│ │ │ │ -
152} // \ namespace gtsam
│ │ │ │ -
Cal3.h
Common code for all Calibration models.
│ │ │ │ -
Point2.h
2D Point
│ │ │ │ +
103} // \ namespace gtsam
│ │ │ │ +
104
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
│ │ │ │
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
│ │ │ │ +
gtsam::means
Point2Pair means(const std::vector< Point2Pair > &abPointPairs)
Calculate the two means of a set of Point2 pairs.
Definition Point2.cpp:116
│ │ │ │ +
gtsam::distance2
double distance2(const Point2 &p, const Point2 &q, OptionalJacobian< 1, 2 > H1, OptionalJacobian< 1, 2 > H2)
distance between two points
Definition Point2.cpp:39
│ │ │ │ +
gtsam::norm2
double norm2(const Point2 &p, OptionalJacobian< 1, 2 > H)
Distance of the point from the origin, with Jacobian.
Definition Point2.cpp:27
│ │ │ │
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Cal3
Common base class for all calibration models.
Definition Cal3.h:69
│ │ │ │ -
gtsam::Cal3::vector
Vector5 vector() const
vectorized form (column-wise)
Definition Cal3.h:160
│ │ │ │ -
gtsam::Cal3::fy_
double fy_
focal length
Definition Cal3.h:71
│ │ │ │ -
gtsam::Cal3::s_
double s_
skew
Definition Cal3.h:72
│ │ │ │ -
gtsam::Cal3::v0_
double v0_
principal point
Definition Cal3.h:73
│ │ │ │ -
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition Cal3_S2.h:34
│ │ │ │ -
gtsam::Cal3_S2::Cal3_S2
Cal3_S2()=default
Create a default calibration that leaves coordinates unchanged.
│ │ │ │ -
gtsam::Cal3_S2::Cal3_S2
Cal3_S2(double fx, double fy, double s, double u0, double v0)
constructor from doubles
Definition Cal3_S2.h:48
│ │ │ │ -
gtsam::Cal3_S2::between
Cal3_S2 between(const Cal3_S2 &q, OptionalJacobian< 5, 5 > H1=boost::none, OptionalJacobian< 5, 5 > H2=boost::none) const
"Between", subtracts calibrations. between(p,q) == compose(inverse(p),q)
Definition Cal3_S2.h:104
│ │ │ │ -
gtsam::Cal3_S2::Dim
static size_t Dim()
return DOF, dimensionality of tangent space
Definition Cal3_S2.h:118
│ │ │ │ -
gtsam::Cal3_S2::Cal3_S2
Cal3_S2(const Vector5 &d)
constructor from vector
Definition Cal3_S2.h:52
│ │ │ │ -
gtsam::Cal3_S2::localCoordinates
Vector5 localCoordinates(const Cal3_S2 &T2) const
Unretraction for the calibration.
Definition Cal3_S2.h:126
│ │ │ │ -
gtsam::Cal3_S2::Cal3_S2
Cal3_S2(double fov, int w, int h)
Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect.
Definition Cal3_S2.h:60
│ │ │ │ -
gtsam::Cal3_S2::retract
Cal3_S2 retract(const Vector &d) const
Given 5-dim tangent vector, create new calibration.
Definition Cal3_S2.h:121
│ │ │ │ +
gtsam::Range
Definition BearingRange.h:40
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,186 +1,140 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Cal3_S2.h │ │ │ │ │ +Point2.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -22#pragma once │ │ │ │ │ -23 │ │ │ │ │ -24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ -26 │ │ │ │ │ -27namespace _g_t_s_a_m { │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include │ │ │ │ │ +21#include │ │ │ │ │ +22 │ │ │ │ │ +23namespace _g_t_s_a_m { │ │ │ │ │ +24 │ │ │ │ │ +_2_7typedef Vector2 _P_o_i_n_t_2; │ │ │ │ │ 28 │ │ │ │ │ -_3_4class GTSAM_EXPORT _C_a_l_3___S_2 : public _C_a_l_3 { │ │ │ │ │ -35 public: │ │ │ │ │ -36 enum { dimension = 5 }; │ │ │ │ │ +29// Convenience typedef │ │ │ │ │ +30using Point2Pair = std::pair; │ │ │ │ │ +31GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam:: │ │ │ │ │ +Point2Pair &p); │ │ │ │ │ +32 │ │ │ │ │ +33using Point2Pairs = std::vector; │ │ │ │ │ +34 │ │ │ │ │ +36GTSAM_EXPORT double _n_o_r_m_2(const _P_o_i_n_t_2& p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _2_> H = boost:: │ │ │ │ │ +none); │ │ │ │ │ 37 │ │ │ │ │ -39 using shared_ptr = boost::shared_ptr; │ │ │ │ │ -40 │ │ │ │ │ -43 │ │ │ │ │ -_4_5 _C_a_l_3___S_2() = default; │ │ │ │ │ -46 │ │ │ │ │ -_4_8 _C_a_l_3___S_2(double fx, double fy, double s, double u0, double v0) │ │ │ │ │ -49 : _C_a_l_3(fx, fy, s, u0, v0) {} │ │ │ │ │ +39GTSAM_EXPORT double _d_i_s_t_a_n_c_e_2(const _P_o_i_n_t_2& p1, const _P_o_i_n_t_2& q, │ │ │ │ │ +40 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _2_> H1 = boost::none, │ │ │ │ │ +41 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _2_> H2 = boost::none); │ │ │ │ │ +42 │ │ │ │ │ +43// For MATLAB wrapper │ │ │ │ │ +44typedef std::vector > Point2Vector; │ │ │ │ │ +45 │ │ │ │ │ +_4_7inline _P_o_i_n_t_2 _o_p_e_r_a_t_o_r_*(double s, const _P_o_i_n_t_2& p) { │ │ │ │ │ +48 return _P_o_i_n_t_2(s * p.x(), s * p.y()); │ │ │ │ │ +49} │ │ │ │ │ 50 │ │ │ │ │ -_5_2 _C_a_l_3___S_2(const Vector5& d) : _C_a_l_3(d) {} │ │ │ │ │ -53 │ │ │ │ │ -_6_0 _C_a_l_3___S_2(double fov, int w, int h) : _C_a_l_3(fov, w, h) {} │ │ │ │ │ -61 │ │ │ │ │ -69 _P_o_i_n_t_2 uncalibrate(const _P_o_i_n_t_2& p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _5_> Dcal = boost:: │ │ │ │ │ -none, │ │ │ │ │ -70 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dp = boost::none) const; │ │ │ │ │ -71 │ │ │ │ │ -79 _P_o_i_n_t_2 calibrate(const _P_o_i_n_t_2& p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _5_> Dcal = boost::none, │ │ │ │ │ -80 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dp = boost::none) const; │ │ │ │ │ -81 │ │ │ │ │ -87 Vector3 calibrate(const Vector3& p) const; │ │ │ │ │ -88 │ │ │ │ │ +51/* │ │ │ │ │ +52 * @brief Circle-circle intersection, given normalized radii. │ │ │ │ │ +53 * Calculate f and h, respectively the parallel and perpendicular distance of │ │ │ │ │ +54 * the intersections of two circles along and from the line connecting the │ │ │ │ │ +centers. │ │ │ │ │ +55 * Both are dimensionless fractions of the distance d between the circle │ │ │ │ │ +centers. │ │ │ │ │ +56 * If the circles do not intersect or they are identical, returns boost:: │ │ │ │ │ +none. │ │ │ │ │ +57 * If one solution (touching circles, as determined by tol), h will be │ │ │ │ │ +exactly zero. │ │ │ │ │ +58 * h is a good measure for how accurate the intersection will be, as when │ │ │ │ │ +circles touch │ │ │ │ │ +59 * or nearly touch, the intersection is ill-defined with noisy radius │ │ │ │ │ +measurements. │ │ │ │ │ +60 * @param R_d : R/d, ratio of radius of first circle to distance between │ │ │ │ │ +centers │ │ │ │ │ +61 * @param r_d : r/d, ratio of radius of second circle to distance between │ │ │ │ │ +centers │ │ │ │ │ +62 * @param tol: absolute tolerance below which we consider touching circles │ │ │ │ │ +63 * @return optional Point2 with f and h, boost::none if no solution. │ │ │ │ │ +64 */ │ │ │ │ │ +65GTSAM_EXPORT boost::optional circleCircleIntersection(double R_d, │ │ │ │ │ +double r_d, double tol = 1e-9); │ │ │ │ │ +66 │ │ │ │ │ +67/* │ │ │ │ │ +68 * @brief Circle-circle intersection, from the normalized radii solution. │ │ │ │ │ +69 * @param c1 center of first circle │ │ │ │ │ +70 * @param c2 center of second circle │ │ │ │ │ +71 * @return list of solutions (0,1, or 2). Identical circles will return empty │ │ │ │ │ +list, as well. │ │ │ │ │ +72 */ │ │ │ │ │ +73GTSAM_EXPORT std::list circleCircleIntersection(_P_o_i_n_t_2 c1, _P_o_i_n_t_2 c2, │ │ │ │ │ +boost::optional fh); │ │ │ │ │ +74 │ │ │ │ │ +76GTSAM_EXPORT Point2Pair _m_e_a_n_s(const std::vector &abPointPairs); │ │ │ │ │ +77 │ │ │ │ │ +87GTSAM_EXPORT std::list circleCircleIntersection(_P_o_i_n_t_2 c1, double r1, │ │ │ │ │ +88 _P_o_i_n_t_2 c2, double r2, double tol = 1e-9); │ │ │ │ │ +89 │ │ │ │ │ +90template │ │ │ │ │ +91struct Range; │ │ │ │ │ 92 │ │ │ │ │ -94 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, │ │ │ │ │ -95 const _C_a_l_3___S_2& cal); │ │ │ │ │ -96 │ │ │ │ │ -98 void _p_r_i_n_t(const std::string& s = "Cal3_S2") const override; │ │ │ │ │ -99 │ │ │ │ │ -101 bool _e_q_u_a_l_s(const _C_a_l_3___S_2& K, double tol = 10e-9) const; │ │ │ │ │ +93template <> │ │ │ │ │ +_9_4struct _R_a_n_g_e<_P_o_i_n_t_2, _P_o_i_n_t_2> { │ │ │ │ │ +95 typedef double result_type; │ │ │ │ │ +96 double operator()(const _P_o_i_n_t_2& p, const _P_o_i_n_t_2& q, │ │ │ │ │ +97 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _2_> H1 = boost::none, │ │ │ │ │ +98 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _2_> H2 = boost::none) { │ │ │ │ │ +99 return _d_i_s_t_a_n_c_e_2(p, q, H1, H2); │ │ │ │ │ +100 } │ │ │ │ │ +101}; │ │ │ │ │ 102 │ │ │ │ │ -_1_0_4 inline _C_a_l_3___S_2 _b_e_t_w_e_e_n(const _C_a_l_3___S_2& q, │ │ │ │ │ -105 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_5_,_ _5_> H1 = boost::none, │ │ │ │ │ -106 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_5_,_ _5_> H2 = boost::none) const { │ │ │ │ │ -107 if (H1) *H1 = -I_5x5; │ │ │ │ │ -108 if (H2) *H2 = I_5x5; │ │ │ │ │ -109 return _C_a_l_3___S_2(q.fx_ - fx_, q._f_y__ - fy_, q._s__ - s_, q.u0_ - u0_, │ │ │ │ │ -110 q._v_0__ - v0_); │ │ │ │ │ -111 } │ │ │ │ │ -112 │ │ │ │ │ -116 │ │ │ │ │ -_1_1_8 inline static size_t _D_i_m() { return dimension; } │ │ │ │ │ -119 │ │ │ │ │ -_1_2_1 inline _C_a_l_3___S_2 _r_e_t_r_a_c_t(const Vector& d) const { │ │ │ │ │ -122 return _C_a_l_3___S_2(fx_ + d(0), fy_ + d(1), s_ + d(2), u0_ + d(3), v0_ + d(4)); │ │ │ │ │ -123 } │ │ │ │ │ -124 │ │ │ │ │ -_1_2_6 Vector5 _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(const _C_a_l_3___S_2& T2) const { │ │ │ │ │ -127 return T2._v_e_c_t_o_r() - vector(); │ │ │ │ │ -128 } │ │ │ │ │ -129 │ │ │ │ │ -133 │ │ │ │ │ -134 private: │ │ │ │ │ -_1_3_6 friend class boost::serialization::access; │ │ │ │ │ -137 template │ │ │ │ │ -138 void serialize(Archive& ar, const unsigned int /*version*/) { │ │ │ │ │ -139 ar& boost::serialization::make_nvp( │ │ │ │ │ -140 "Cal3_S2", boost::serialization::base_object(*this)); │ │ │ │ │ -141 } │ │ │ │ │ -142 │ │ │ │ │ -144}; │ │ │ │ │ -145 │ │ │ │ │ -146template <> │ │ │ │ │ -_1_4_7struct _t_r_a_i_t_s<_C_a_l_3___S_2> : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d {}; │ │ │ │ │ -148 │ │ │ │ │ -149template <> │ │ │ │ │ -_1_5_0struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d {}; │ │ │ │ │ -151 │ │ │ │ │ -152} // \ namespace gtsam │ │ │ │ │ -_C_a_l_3_._h │ │ │ │ │ -Common code for all Calibration models. │ │ │ │ │ -_P_o_i_n_t_2_._h │ │ │ │ │ -2D Point │ │ │ │ │ +103} // \ namespace gtsam │ │ │ │ │ +104 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ +Point2 operator*(double s, const Point2 &p) │ │ │ │ │ +multiply with scalar │ │ │ │ │ +DDeeffiinniittiioonn Point2.h:47 │ │ │ │ │ _g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ Vector2 Point2 │ │ │ │ │ As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ typedef Point2 to Vector2... │ │ │ │ │ DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ -Both ManifoldTraits and Testable. │ │ │ │ │ -DDeeffiinniittiioonn Manifold.h:120 │ │ │ │ │ +_g_t_s_a_m_:_:_m_e_a_n_s │ │ │ │ │ +Point2Pair means(const std::vector< Point2Pair > &abPointPairs) │ │ │ │ │ +Calculate the two means of a set of Point2 pairs. │ │ │ │ │ +DDeeffiinniittiioonn Point2.cpp:116 │ │ │ │ │ +_g_t_s_a_m_:_:_d_i_s_t_a_n_c_e_2 │ │ │ │ │ +double distance2(const Point2 &p, const Point2 &q, OptionalJacobian< 1, 2 > H1, │ │ │ │ │ +OptionalJacobian< 1, 2 > H2) │ │ │ │ │ +distance between two points │ │ │ │ │ +DDeeffiinniittiioonn Point2.cpp:39 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_r_m_2 │ │ │ │ │ +double norm2(const Point2 &p, OptionalJacobian< 1, 2 > H) │ │ │ │ │ +Distance of the point from the origin, with Jacobian. │ │ │ │ │ +DDeeffiinniittiioonn Point2.cpp:27 │ │ │ │ │ _g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ either a fixed size o... │ │ │ │ │ DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3 │ │ │ │ │ -Common base class for all calibration models. │ │ │ │ │ -DDeeffiinniittiioonn Cal3.h:69 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_:_:_v_e_c_t_o_r │ │ │ │ │ -Vector5 vector() const │ │ │ │ │ -vectorized form (column-wise) │ │ │ │ │ -DDeeffiinniittiioonn Cal3.h:160 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_:_:_f_y__ │ │ │ │ │ -double fy_ │ │ │ │ │ -focal length │ │ │ │ │ -DDeeffiinniittiioonn Cal3.h:71 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_:_:_s__ │ │ │ │ │ -double s_ │ │ │ │ │ -skew │ │ │ │ │ -DDeeffiinniittiioonn Cal3.h:72 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_:_:_v_0__ │ │ │ │ │ -double v0_ │ │ │ │ │ -principal point │ │ │ │ │ -DDeeffiinniittiioonn Cal3.h:73 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3___S_2 │ │ │ │ │ -The most common 5DOF 3D->2D calibration. │ │ │ │ │ -DDeeffiinniittiioonn Cal3_S2.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3___S_2_:_:_C_a_l_3___S_2 │ │ │ │ │ -Cal3_S2()=default │ │ │ │ │ -Create a default calibration that leaves coordinates unchanged. │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3___S_2_:_:_C_a_l_3___S_2 │ │ │ │ │ -Cal3_S2(double fx, double fy, double s, double u0, double v0) │ │ │ │ │ -constructor from doubles │ │ │ │ │ -DDeeffiinniittiioonn Cal3_S2.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3___S_2_:_:_b_e_t_w_e_e_n │ │ │ │ │ -Cal3_S2 between(const Cal3_S2 &q, OptionalJacobian< 5, 5 > H1=boost::none, │ │ │ │ │ -OptionalJacobian< 5, 5 > H2=boost::none) const │ │ │ │ │ -"Between", subtracts calibrations. between(p,q) == compose(inverse(p),q) │ │ │ │ │ -DDeeffiinniittiioonn Cal3_S2.h:104 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3___S_2_:_:_D_i_m │ │ │ │ │ -static size_t Dim() │ │ │ │ │ -return DOF, dimensionality of tangent space │ │ │ │ │ -DDeeffiinniittiioonn Cal3_S2.h:118 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3___S_2_:_:_C_a_l_3___S_2 │ │ │ │ │ -Cal3_S2(const Vector5 &d) │ │ │ │ │ -constructor from vector │ │ │ │ │ -DDeeffiinniittiioonn Cal3_S2.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3___S_2_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ -Vector5 localCoordinates(const Cal3_S2 &T2) const │ │ │ │ │ -Unretraction for the calibration. │ │ │ │ │ -DDeeffiinniittiioonn Cal3_S2.h:126 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3___S_2_:_:_C_a_l_3___S_2 │ │ │ │ │ -Cal3_S2(double fov, int w, int h) │ │ │ │ │ -Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect. │ │ │ │ │ -DDeeffiinniittiioonn Cal3_S2.h:60 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3___S_2_:_:_r_e_t_r_a_c_t │ │ │ │ │ -Cal3_S2 retract(const Vector &d) const │ │ │ │ │ -Given 5-dim tangent vector, create new calibration. │ │ │ │ │ -DDeeffiinniittiioonn Cal3_S2.h:121 │ │ │ │ │ +_g_t_s_a_m_:_:_R_a_n_g_e │ │ │ │ │ +DDeeffiinniittiioonn BearingRange.h:40 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3___S_2_._h │ │ │ │ │ + * _P_o_i_n_t_2_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00293.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Quaternion.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3DS2.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,56 +95,53 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Macros | │ │ │ │ -Typedefs
│ │ │ │ -
Quaternion.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
Cal3DS2.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Lie Group wrapper for Eigen Quaternions. │ │ │ │ +

Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

struct  gtsam::traits< QUATERNION_TYPE >
class  gtsam::Cal3DS2
 Calibration of a camera with radial distortion that also supports Lie-group behaviors for optimization. More...
 
struct  gtsam::traits< Cal3DS2 >
 
struct  gtsam::traits< const Cal3DS2 >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Macros

│ │ │ │ -#define QUATERNION_TYPE   Eigen::Quaternion<_Scalar,_Options>
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Typedefs

│ │ │ │ -typedef Eigen::Quaternion< double, Eigen::DontAlign > gtsam::Quaternion
 
│ │ │ │

Detailed Description

│ │ │ │ -

Lie Group wrapper for Eigen Quaternions.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +

Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base.

│ │ │ │ +

Calibration of a camera with radial distortion.

│ │ │ │ +
Date
Feb 28, 2010
│ │ │ │ +
Author
ydjian @autho Varun Agrawal
│ │ │ │ +
Date
Feb 28, 2010
│ │ │ │ +
Author
ydjian
│ │ │ │ +
│ │ │ │ +Varun Agrawal
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,30 +1,40 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _T_y_p_e_d_e_f_s │ │ │ │ │ -Quaternion.h File Reference │ │ │ │ │ -Lie Group wrapper for Eigen Quaternions. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +Cal3DS2.h File Reference │ │ │ │ │ +Calibration of a camera with radial distortion, calculations in base class │ │ │ │ │ +Cal3DS2_Base. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _Q_U_A_T_E_R_N_I_O_N___T_Y_P_E_ _> │ │ │ │ │ + class   _g_t_s_a_m_:_:_C_a_l_3_D_S_2 │ │ │ │ │ +  Calibration of a camera with radial distortion that also supports Lie- │ │ │ │ │ + group behaviors for optimization. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _C_a_l_3_D_S_2_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _C_a_l_3_D_S_2_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -MMaaccrrooss │ │ │ │ │ -#define  QQUUAATTEERRNNIIOONN__TTYYPPEE   Eigen::Quaternion<_Scalar,_Options> │ │ │ │ │ -  │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -typedef Eigen::Quaternion< double, Eigen::DontAlign >  ggttssaamm::::QQuuaatteerrnniioonn │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Lie Group wrapper for Eigen Quaternions. │ │ │ │ │ +Calibration of a camera with radial distortion, calculations in base class │ │ │ │ │ +Cal3DS2_Base. │ │ │ │ │ +Calibration of a camera with radial distortion. │ │ │ │ │ + Date │ │ │ │ │ + Feb 28, 2010 │ │ │ │ │ + Author │ │ │ │ │ + ydjian @autho Varun Agrawal │ │ │ │ │ + Date │ │ │ │ │ + Feb 28, 2010 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + ydjian │ │ │ │ │ + Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _Q_u_a_t_e_r_n_i_o_n_._h │ │ │ │ │ + * _C_a_l_3_D_S_2_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00293.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,4 @@ │ │ │ │ │ var a00293 = [ │ │ │ │ │ - ["gtsam::traits< QUATERNION_TYPE >", "a03208.html", null] │ │ │ │ │ + ["gtsam::traits< Cal3DS2 >", "a02984.html", null], │ │ │ │ │ + ["gtsam::traits< const Cal3DS2 >", "a02988.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00293_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Quaternion.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3DS2.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,200 +98,113 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Quaternion.h
│ │ │ │ +
Cal3DS2.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/base/Lie.h>
│ │ │ │ -
21#include <gtsam/base/concepts.h>
│ │ │ │ -
22#include <gtsam/geometry/SO3.h> // Logmap/Expmap derivatives
│ │ │ │ -
23#include <limits>
│ │ │ │ -
24#include <iostream>
│ │ │ │ +
21#pragma once
│ │ │ │ +
22
│ │ │ │ +
23#include <gtsam/geometry/Cal3DS2_Base.h>
│ │ │ │ +
24#include <boost/shared_ptr.hpp>
│ │ │ │
25
│ │ │ │ -
26#define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options>
│ │ │ │ +
26namespace gtsam {
│ │ │ │
27
│ │ │ │ -
28namespace gtsam {
│ │ │ │ -
29
│ │ │ │ -
30// Define traits
│ │ │ │ -
31template<typename _Scalar, int _Options>
│ │ │ │ -
│ │ │ │ -
32struct traits<QUATERNION_TYPE> {
│ │ │ │ -
33 typedef QUATERNION_TYPE ManifoldType;
│ │ │ │ -
34 typedef QUATERNION_TYPE Q;
│ │ │ │ -
35
│ │ │ │ -
36 typedef lie_group_tag structure_category;
│ │ │ │ -
37 typedef multiplicative_group_tag group_flavor;
│ │ │ │ -
38
│ │ │ │ -
41 static Q Identity() {
│ │ │ │ -
42 return Q::Identity();
│ │ │ │ -
43 }
│ │ │ │ -
44
│ │ │ │ -
48 enum {
│ │ │ │ -
49 dimension = 3
│ │ │ │ -
50 };
│ │ │ │ -
51 typedef OptionalJacobian<3, 3> ChartJacobian;
│ │ │ │ -
52 typedef Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> TangentVector;
│ │ │ │ +
│ │ │ │ +
35class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
│ │ │ │ +
36 using Base = Cal3DS2_Base;
│ │ │ │ +
37
│ │ │ │ +
38 public:
│ │ │ │ +
39 enum { dimension = 9 };
│ │ │ │ +
40
│ │ │ │ +
42 using shared_ptr = boost::shared_ptr<Cal3DS2>;
│ │ │ │ +
43
│ │ │ │ +
46
│ │ │ │ +
48 Cal3DS2() = default;
│ │ │ │ +
49
│ │ │ │ +
50 Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1,
│ │ │ │ +
51 double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5)
│ │ │ │ +
52 : Base(fx, fy, s, u0, v0, k1, k2, p1, p2, tol) {}
│ │ │ │
53
│ │ │ │ -
57 static Q Compose(const Q &g, const Q & h,
│ │ │ │ -
58 ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) {
│ │ │ │ -
59 if (Hg) *Hg = h.toRotationMatrix().transpose();
│ │ │ │ -
60 if (Hh) *Hh = I_3x3;
│ │ │ │ -
61 return g * h;
│ │ │ │ -
62 }
│ │ │ │ -
63
│ │ │ │ -
64 static Q Between(const Q &g, const Q & h,
│ │ │ │ -
65 ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) {
│ │ │ │ -
66 Q d = g.inverse() * h;
│ │ │ │ -
67 if (Hg) *Hg = -d.toRotationMatrix().transpose();
│ │ │ │ -
68 if (Hh) *Hh = I_3x3;
│ │ │ │ -
69 return d;
│ │ │ │ -
70 }
│ │ │ │ -
71
│ │ │ │ -
72 static Q Inverse(const Q &g,
│ │ │ │ -
73 ChartJacobian H = boost::none) {
│ │ │ │ -
74 if (H) *H = -g.toRotationMatrix();
│ │ │ │ -
75 return g.inverse();
│ │ │ │ -
76 }
│ │ │ │ -
77
│ │ │ │ -
│ │ │ │ -
79 static Q Expmap(const Eigen::Ref<const TangentVector>& omega,
│ │ │ │ -
80 ChartJacobian H = boost::none) {
│ │ │ │ -
81 using std::cos;
│ │ │ │ -
82 using std::sin;
│ │ │ │ -
83 if (H) *H = SO3::ExpmapDerivative(omega.template cast<double>());
│ │ │ │ -
84 _Scalar theta2 = omega.dot(omega);
│ │ │ │ -
85 if (theta2 > std::numeric_limits<_Scalar>::epsilon()) {
│ │ │ │ -
86 _Scalar theta = std::sqrt(theta2);
│ │ │ │ -
87 _Scalar ha = _Scalar(0.5) * theta;
│ │ │ │ -
88 Vector3 vec = (sin(ha) / theta) * omega;
│ │ │ │ -
89 return Q(cos(ha), vec.x(), vec.y(), vec.z());
│ │ │ │ -
90 } else {
│ │ │ │ -
91 // first order approximation sin(theta/2)/theta = 0.5
│ │ │ │ -
92 Vector3 vec = _Scalar(0.5) * omega;
│ │ │ │ -
93 return Q(1.0, vec.x(), vec.y(), vec.z());
│ │ │ │ -
94 }
│ │ │ │ -
95 }
│ │ │ │ -
│ │ │ │ -
96
│ │ │ │ -
│ │ │ │ -
98 static TangentVector Logmap(const Q& q, ChartJacobian H = boost::none) {
│ │ │ │ -
99 using std::acos;
│ │ │ │ -
100 using std::sqrt;
│ │ │ │ -
101
│ │ │ │ -
102 // define these compile time constants to avoid std::abs:
│ │ │ │ -
103 static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10,
│ │ │ │ -
104 NearlyNegativeOne = -1.0 + 1e-10;
│ │ │ │ -
105
│ │ │ │ -
106 TangentVector omega;
│ │ │ │ -
107
│ │ │ │ -
108 const _Scalar qw = q.w();
│ │ │ │ -
109 // See Quaternion-Logmap.nb in doc for Taylor expansions
│ │ │ │ -
110 if (qw > NearlyOne) {
│ │ │ │ -
111 // Taylor expansion of (angle / s) at 1
│ │ │ │ -
112 // (2 + 2 * (1-qw) / 3) * q.vec();
│ │ │ │ -
113 omega = ( 8. / 3. - 2. / 3. * qw) * q.vec();
│ │ │ │ -
114 } else if (qw < NearlyNegativeOne) {
│ │ │ │ -
115 // Taylor expansion of (angle / s) at -1
│ │ │ │ -
116 // (-2 - 2 * (1 + qw) / 3) * q.vec();
│ │ │ │ -
117 omega = (-8. / 3. - 2. / 3. * qw) * q.vec();
│ │ │ │ -
118 } else {
│ │ │ │ -
119 // Normal, away from zero case
│ │ │ │ -
120 if (qw > 0) {
│ │ │ │ -
121 _Scalar angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
│ │ │ │ -
122 // Important: convert to [-pi,pi] to keep error continuous
│ │ │ │ -
123 if (angle > M_PI)
│ │ │ │ -
124 angle -= twoPi;
│ │ │ │ -
125 else if (angle < -M_PI)
│ │ │ │ -
126 angle += twoPi;
│ │ │ │ -
127 omega = (angle / s) * q.vec();
│ │ │ │ -
128 } else {
│ │ │ │ -
129 // Make sure that we are using a canonical quaternion with w > 0
│ │ │ │ -
130 _Scalar angle = 2 * acos(-qw), s = sqrt(1 - qw * qw);
│ │ │ │ -
131 if (angle > M_PI)
│ │ │ │ -
132 angle -= twoPi;
│ │ │ │ -
133 else if (angle < -M_PI)
│ │ │ │ -
134 angle += twoPi;
│ │ │ │ -
135 omega = (angle / s) * -q.vec();
│ │ │ │ -
136 }
│ │ │ │ -
137 }
│ │ │ │ -
138
│ │ │ │ -
139 if(H) *H = SO3::LogmapDerivative(omega.template cast<double>());
│ │ │ │ -
140 return omega;
│ │ │ │ -
141 }
│ │ │ │ -
│ │ │ │ -
142
│ │ │ │ -
146
│ │ │ │ -
147 static TangentVector Local(const Q& g, const Q& h,
│ │ │ │ -
148 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ -
149 Q b = Between(g, h, H1, H2);
│ │ │ │ -
150 Matrix3 D_v_b;
│ │ │ │ -
151 TangentVector v = Logmap(b, (H1 || H2) ? &D_v_b : 0);
│ │ │ │ -
152 if (H1) *H1 = D_v_b * (*H1);
│ │ │ │ -
153 if (H2) *H2 = D_v_b * (*H2);
│ │ │ │ -
154 return v;
│ │ │ │ -
155 }
│ │ │ │ -
156
│ │ │ │ -
157 static Q Retract(const Q& g, const TangentVector& v,
│ │ │ │ -
158 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ -
159 Matrix3 D_h_v;
│ │ │ │ -
160 Q b = Expmap(v,H2 ? &D_h_v : 0);
│ │ │ │ -
161 Q h = Compose(g, b, H1, H2);
│ │ │ │ -
162 if (H2) *H2 = (*H2) * D_h_v;
│ │ │ │ -
163 return h;
│ │ │ │ -
164 }
│ │ │ │ -
165
│ │ │ │ -
169 static void Print(const Q& q, const std::string& str = "") {
│ │ │ │ -
170 if (str.size() == 0)
│ │ │ │ -
171 std::cout << "Eigen::Quaternion: ";
│ │ │ │ -
172 else
│ │ │ │ -
173 std::cout << str << " ";
│ │ │ │ -
174 std::cout << q.vec().transpose() << std::endl;
│ │ │ │ -
175 }
│ │ │ │ -
176 static bool Equals(const Q& q1, const Q& q2, double tol = 1e-8) {
│ │ │ │ -
177 return Between(q1, q2).vec().array().abs().maxCoeff() < tol;
│ │ │ │ -
178 }
│ │ │ │ -
180};
│ │ │ │ -
│ │ │ │ -
181
│ │ │ │ -
182typedef Eigen::Quaternion<double, Eigen::DontAlign> Quaternion;
│ │ │ │ -
183
│ │ │ │ -
184} // \namespace gtsam
│ │ │ │ -
185
│ │ │ │ -
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ -
SO3.h
3*3 matrix representation of SO(3)
│ │ │ │ +
54 ~Cal3DS2() override {}
│ │ │ │ +
55
│ │ │ │ +
59
│ │ │ │ +
60 Cal3DS2(const Vector9& v) : Base(v) {}
│ │ │ │ +
61
│ │ │ │ +
65
│ │ │ │ +
67 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
│ │ │ │ +
68 const Cal3DS2& cal);
│ │ │ │ +
69
│ │ │ │ +
71 void print(const std::string& s = "") const override;
│ │ │ │ +
72
│ │ │ │ +
74 bool equals(const Cal3DS2& K, double tol = 10e-9) const;
│ │ │ │ +
75
│ │ │ │ +
79
│ │ │ │ +
81 Cal3DS2 retract(const Vector& d) const;
│ │ │ │ +
82
│ │ │ │ +
84 Vector localCoordinates(const Cal3DS2& T2) const;
│ │ │ │ +
85
│ │ │ │ +
87 size_t dim() const override { return Dim(); }
│ │ │ │ +
88
│ │ │ │ +
90 inline static size_t Dim() { return dimension; }
│ │ │ │ +
91
│ │ │ │ +
95
│ │ │ │ +
│ │ │ │ +
97 boost::shared_ptr<Base> clone() const override {
│ │ │ │ +
98 return boost::shared_ptr<Base>(new Cal3DS2(*this));
│ │ │ │ +
99 }
│ │ │ │ +
│ │ │ │ +
100
│ │ │ │ +
102
│ │ │ │ +
103 private:
│ │ │ │ +
106
│ │ │ │ +
108 friend class boost::serialization::access;
│ │ │ │ +
109 template <class Archive>
│ │ │ │ +
110 void serialize(Archive& ar, const unsigned int /*version*/) {
│ │ │ │ +
111 ar& boost::serialization::make_nvp(
│ │ │ │ +
112 "Cal3DS2", boost::serialization::base_object<Cal3DS2_Base>(*this));
│ │ │ │ +
113 }
│ │ │ │ +
114
│ │ │ │ +
116};
│ │ │ │ +
│ │ │ │ +
117
│ │ │ │ +
118template <>
│ │ │ │ +
119struct traits<Cal3DS2> : public internal::Manifold<Cal3DS2> {};
│ │ │ │ +
120
│ │ │ │ +
121template <>
│ │ │ │ +
122struct traits<const Cal3DS2> : public internal::Manifold<Cal3DS2> {};
│ │ │ │ +
123}
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::multiplicative_group_tag
Group operator syntax flavors.
Definition Group.h:37
│ │ │ │ -
gtsam::lie_group_tag
tag to assert a type is a Lie group
Definition Lie.h:164
│ │ │ │ -
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
gtsam::traits< QUATERNION_TYPE >::Expmap
static Q Expmap(const Eigen::Ref< const TangentVector > &omega, ChartJacobian H=boost::none)
Exponential map, using the inlined code from Eigen's conversion from axis/angle.
Definition Quaternion.h:79
│ │ │ │ -
gtsam::traits< QUATERNION_TYPE >::Logmap
static TangentVector Logmap(const Q &q, ChartJacobian H=boost::none)
We use our own Logmap, as there is a slight bug in Eigen.
Definition Quaternion.h:98
│ │ │ │ -
gtsam::SO< 3 >::ExpmapDerivative
static MatrixDD ExpmapDerivative(const TangentVector &omega)
Derivative of Expmap, currently only defined for SO3.
Definition SOn-inl.h:72
│ │ │ │ -
gtsam::SO< 3 >::LogmapDerivative
static MatrixDD LogmapDerivative(const TangentVector &omega)
Derivative of Logmap, currently only defined for SO3.
Definition SOn-inl.h:82
│ │ │ │ +
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
│ │ │ │ +
gtsam::Cal3DS2
Calibration of a camera with radial distortion that also supports Lie-group behaviors for optimizatio...
Definition Cal3DS2.h:35
│ │ │ │ +
gtsam::Cal3DS2::dim
size_t dim() const override
Return dimensions of calibration manifold object.
Definition Cal3DS2.h:87
│ │ │ │ +
gtsam::Cal3DS2::clone
boost::shared_ptr< Base > clone() const override
Definition Cal3DS2.h:97
│ │ │ │ +
gtsam::Cal3DS2::Dim
static size_t Dim()
Return dimensions of calibration manifold object.
Definition Cal3DS2.h:90
│ │ │ │ +
gtsam::Cal3DS2::Cal3DS2
Cal3DS2()=default
Default Constructor with only unit focal length.
│ │ │ │ +
gtsam::Cal3DS2_Base
Calibration of a camera with radial distortion.
Definition Cal3DS2_Base.h:42
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,214 +1,126 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Quaternion.h │ │ │ │ │ +Cal3DS2.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_b_a_s_e_/_L_i_e_._h> │ │ │ │ │ -21#include │ │ │ │ │ -22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_S_O_3_._h> // Logmap/Expmap derivatives │ │ │ │ │ -23#include │ │ │ │ │ -24#include │ │ │ │ │ +21#pragma once │ │ │ │ │ +22 │ │ │ │ │ +23#include │ │ │ │ │ +24#include │ │ │ │ │ 25 │ │ │ │ │ -26#define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options> │ │ │ │ │ +26namespace _g_t_s_a_m { │ │ │ │ │ 27 │ │ │ │ │ -28namespace _g_t_s_a_m { │ │ │ │ │ -29 │ │ │ │ │ -30// Define traits │ │ │ │ │ -31template │ │ │ │ │ -_3_2struct _t_r_a_i_t_s { │ │ │ │ │ -33 typedef QUATERNION_TYPE ManifoldType; │ │ │ │ │ -34 typedef QUATERNION_TYPE Q; │ │ │ │ │ -35 │ │ │ │ │ -36 typedef _l_i_e___g_r_o_u_p___t_a_g _s_t_r_u_c_t_u_r_e___c_a_t_e_g_o_r_y; │ │ │ │ │ -37 typedef _m_u_l_t_i_p_l_i_c_a_t_i_v_e___g_r_o_u_p___t_a_g _g_r_o_u_p___f_l_a_v_o_r; │ │ │ │ │ -38 │ │ │ │ │ -41 static Q Identity() { │ │ │ │ │ -42 return Q::Identity(); │ │ │ │ │ -43 } │ │ │ │ │ -44 │ │ │ │ │ -48 enum { │ │ │ │ │ -49 dimension = 3 │ │ │ │ │ -50 }; │ │ │ │ │ -51 typedef _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> ChartJacobian; │ │ │ │ │ -52 typedef Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> TangentVector; │ │ │ │ │ +_3_5class GTSAM_EXPORT _C_a_l_3_D_S_2 : public _C_a_l_3_D_S_2___B_a_s_e { │ │ │ │ │ +36 using _B_a_s_e = _C_a_l_3_D_S_2___B_a_s_e; │ │ │ │ │ +37 │ │ │ │ │ +38 public: │ │ │ │ │ +39 enum { dimension = 9 }; │ │ │ │ │ +40 │ │ │ │ │ +42 using shared_ptr = boost::shared_ptr; │ │ │ │ │ +43 │ │ │ │ │ +46 │ │ │ │ │ +_4_8 _C_a_l_3_D_S_2() = default; │ │ │ │ │ +49 │ │ │ │ │ +50 _C_a_l_3_D_S_2(double fx, double fy, double s, double u0, double v0, double k1, │ │ │ │ │ +51 double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5) │ │ │ │ │ +52 : _B_a_s_e(fx, fy, s, u0, v0, k1, k2, p1, p2, tol) {} │ │ │ │ │ 53 │ │ │ │ │ -57 static Q Compose(const Q &g, const Q & h, │ │ │ │ │ -58 ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) { │ │ │ │ │ -59 if (Hg) *Hg = h.toRotationMatrix().transpose(); │ │ │ │ │ -60 if (Hh) *Hh = I_3x3; │ │ │ │ │ -61 return g * h; │ │ │ │ │ -62 } │ │ │ │ │ -63 │ │ │ │ │ -64 static Q Between(const Q &g, const Q & h, │ │ │ │ │ -65 ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) { │ │ │ │ │ -66 Q d = g.inverse() * h; │ │ │ │ │ -67 if (Hg) *Hg = -d.toRotationMatrix().transpose(); │ │ │ │ │ -68 if (Hh) *Hh = I_3x3; │ │ │ │ │ -69 return d; │ │ │ │ │ -70 } │ │ │ │ │ -71 │ │ │ │ │ -72 static Q Inverse(const Q &g, │ │ │ │ │ -73 ChartJacobian H = boost::none) { │ │ │ │ │ -74 if (H) *H = -g.toRotationMatrix(); │ │ │ │ │ -75 return g.inverse(); │ │ │ │ │ -76 } │ │ │ │ │ -77 │ │ │ │ │ -_7_9 static Q _E_x_p_m_a_p(const Eigen::Ref& omega, │ │ │ │ │ -80 ChartJacobian H = boost::none) { │ │ │ │ │ -81 using std::cos; │ │ │ │ │ -82 using std::sin; │ │ │ │ │ -83 if (H) *H = _S_O_3_:_:_E_x_p_m_a_p_D_e_r_i_v_a_t_i_v_e(omega.template cast()); │ │ │ │ │ -84 _Scalar theta2 = omega.dot(omega); │ │ │ │ │ -85 if (theta2 > std::numeric_limits<_Scalar>::epsilon()) { │ │ │ │ │ -86 _Scalar theta = std::sqrt(theta2); │ │ │ │ │ -87 _Scalar ha = _Scalar(0.5) * theta; │ │ │ │ │ -88 Vector3 vec = (sin(ha) / theta) * omega; │ │ │ │ │ -89 return Q(cos(ha), vec.x(), vec.y(), vec.z()); │ │ │ │ │ -90 } else { │ │ │ │ │ -91 // first order approximation sin(theta/2)/theta = 0.5 │ │ │ │ │ -92 Vector3 vec = _Scalar(0.5) * omega; │ │ │ │ │ -93 return Q(1.0, vec.x(), vec.y(), vec.z()); │ │ │ │ │ -94 } │ │ │ │ │ -95 } │ │ │ │ │ -96 │ │ │ │ │ -_9_8 static TangentVector _L_o_g_m_a_p(const Q& q, ChartJacobian H = boost::none) { │ │ │ │ │ -99 using std::acos; │ │ │ │ │ -100 using std::sqrt; │ │ │ │ │ -101 │ │ │ │ │ -102 // define these compile time constants to avoid std::abs: │ │ │ │ │ -103 static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10, │ │ │ │ │ -104 NearlyNegativeOne = -1.0 + 1e-10; │ │ │ │ │ -105 │ │ │ │ │ -106 TangentVector omega; │ │ │ │ │ -107 │ │ │ │ │ -108 const _Scalar qw = q.w(); │ │ │ │ │ -109 // See Quaternion-Logmap.nb in doc for Taylor expansions │ │ │ │ │ -110 if (qw > NearlyOne) { │ │ │ │ │ -111 // Taylor expansion of (angle / s) at 1 │ │ │ │ │ -112 // (2 + 2 * (1-qw) / 3) * q.vec(); │ │ │ │ │ -113 omega = ( 8. / 3. - 2. / 3. * qw) * q.vec(); │ │ │ │ │ -114 } else if (qw < NearlyNegativeOne) { │ │ │ │ │ -115 // Taylor expansion of (angle / s) at -1 │ │ │ │ │ -116 // (-2 - 2 * (1 + qw) / 3) * q.vec(); │ │ │ │ │ -117 omega = (-8. / 3. - 2. / 3. * qw) * q.vec(); │ │ │ │ │ -118 } else { │ │ │ │ │ -119 // Normal, away from zero case │ │ │ │ │ -120 if (qw > 0) { │ │ │ │ │ -121 _Scalar angle = 2 * acos(qw), s = sqrt(1 - qw * qw); │ │ │ │ │ -122 // Important: convert to [-pi,pi] to keep error continuous │ │ │ │ │ -123 if (angle > M_PI) │ │ │ │ │ -124 angle -= twoPi; │ │ │ │ │ -125 else if (angle < -M_PI) │ │ │ │ │ -126 angle += twoPi; │ │ │ │ │ -127 omega = (angle / s) * q.vec(); │ │ │ │ │ -128 } else { │ │ │ │ │ -129 // Make sure that we are using a canonical quaternion with w > 0 │ │ │ │ │ -130 _Scalar angle = 2 * acos(-qw), s = sqrt(1 - qw * qw); │ │ │ │ │ -131 if (angle > M_PI) │ │ │ │ │ -132 angle -= twoPi; │ │ │ │ │ -133 else if (angle < -M_PI) │ │ │ │ │ -134 angle += twoPi; │ │ │ │ │ -135 omega = (angle / s) * -q.vec(); │ │ │ │ │ -136 } │ │ │ │ │ -137 } │ │ │ │ │ -138 │ │ │ │ │ -139 if(H) *H = _S_O_3_:_:_L_o_g_m_a_p_D_e_r_i_v_a_t_i_v_e(omega.template cast()); │ │ │ │ │ -140 return omega; │ │ │ │ │ -141 } │ │ │ │ │ -142 │ │ │ │ │ -146 │ │ │ │ │ -147 static TangentVector Local(const Q& g, const Q& h, │ │ │ │ │ -148 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { │ │ │ │ │ -149 Q b = Between(g, h, H1, H2); │ │ │ │ │ -150 Matrix3 D_v_b; │ │ │ │ │ -151 TangentVector v = Logmap(b, (H1 || H2) ? &D_v_b : 0); │ │ │ │ │ -152 if (H1) *H1 = D_v_b * (*H1); │ │ │ │ │ -153 if (H2) *H2 = D_v_b * (*H2); │ │ │ │ │ -154 return v; │ │ │ │ │ -155 } │ │ │ │ │ -156 │ │ │ │ │ -157 static Q Retract(const Q& g, const TangentVector& v, │ │ │ │ │ -158 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { │ │ │ │ │ -159 Matrix3 D_h_v; │ │ │ │ │ -160 Q b = Expmap(v,H2 ? &D_h_v : 0); │ │ │ │ │ -161 Q h = Compose(g, b, H1, H2); │ │ │ │ │ -162 if (H2) *H2 = (*H2) * D_h_v; │ │ │ │ │ -163 return h; │ │ │ │ │ -164 } │ │ │ │ │ -165 │ │ │ │ │ -169 static void Print(const Q& q, const std::string& str = "") { │ │ │ │ │ -170 if (str.size() == 0) │ │ │ │ │ -171 std::cout << "Eigen::Quaternion: "; │ │ │ │ │ -172 else │ │ │ │ │ -173 std::cout << str << " "; │ │ │ │ │ -174 std::cout << q.vec().transpose() << std::endl; │ │ │ │ │ -175 } │ │ │ │ │ -176 static bool Equals(const Q& q1, const Q& q2, double tol = 1e-8) { │ │ │ │ │ -177 return Between(q1, q2).vec().array().abs().maxCoeff() < tol; │ │ │ │ │ -178 } │ │ │ │ │ -180}; │ │ │ │ │ -181 │ │ │ │ │ -182typedef Eigen::Quaternion Quaternion; │ │ │ │ │ -183 │ │ │ │ │ -184} // \namespace gtsam │ │ │ │ │ -185 │ │ │ │ │ -_L_i_e_._h │ │ │ │ │ -Base class and basic functions for Lie types. │ │ │ │ │ -_S_O_3_._h │ │ │ │ │ -3*3 matrix representation of SO(3) │ │ │ │ │ +54 _~_C_a_l_3_D_S_2() override {} │ │ │ │ │ +55 │ │ │ │ │ +59 │ │ │ │ │ +60 Cal3DS2(const Vector9& v) : Base(v) {} │ │ │ │ │ +61 │ │ │ │ │ +65 │ │ │ │ │ +67 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, │ │ │ │ │ +68 const Cal3DS2& cal); │ │ │ │ │ +69 │ │ │ │ │ +71 void _p_r_i_n_t(const std::string& s = "") const override; │ │ │ │ │ +72 │ │ │ │ │ +74 bool equals(const Cal3DS2& K, double tol = 10e-9) const; │ │ │ │ │ +75 │ │ │ │ │ +79 │ │ │ │ │ +81 Cal3DS2 retract(const Vector& d) const; │ │ │ │ │ +82 │ │ │ │ │ +84 Vector localCoordinates(const Cal3DS2& T2) const; │ │ │ │ │ +85 │ │ │ │ │ +_8_7 size_t _d_i_m() const override { return Dim(); } │ │ │ │ │ +88 │ │ │ │ │ +_9_0 inline static size_t _D_i_m() { return dimension; } │ │ │ │ │ +91 │ │ │ │ │ +95 │ │ │ │ │ +_9_7 boost::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +98 return boost::shared_ptr(new _C_a_l_3_D_S_2(*this)); │ │ │ │ │ +99 } │ │ │ │ │ +100 │ │ │ │ │ +102 │ │ │ │ │ +103 private: │ │ │ │ │ +106 │ │ │ │ │ +_1_0_8 friend class boost::serialization::access; │ │ │ │ │ +109 template │ │ │ │ │ +110 void serialize(Archive& ar, const unsigned int /*version*/) { │ │ │ │ │ +111 ar& boost::serialization::make_nvp( │ │ │ │ │ +112 "Cal3DS2", boost::serialization::base_object(*this)); │ │ │ │ │ +113 } │ │ │ │ │ +114 │ │ │ │ │ +116}; │ │ │ │ │ +117 │ │ │ │ │ +118template <> │ │ │ │ │ +_1_1_9struct _t_r_a_i_t_s<_C_a_l_3_D_S_2> : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d {}; │ │ │ │ │ +120 │ │ │ │ │ +121template <> │ │ │ │ │ +_1_2_2struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d {}; │ │ │ │ │ +123} │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_m_u_l_t_i_p_l_i_c_a_t_i_v_e___g_r_o_u_p___t_a_g │ │ │ │ │ -Group operator syntax flavors. │ │ │ │ │ -DDeeffiinniittiioonn Group.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_l_i_e___g_r_o_u_p___t_a_g │ │ │ │ │ -tag to assert a type is a Lie group │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:164 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ -either a fixed size o... │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _Q_U_A_T_E_R_N_I_O_N___T_Y_P_E_ _>_:_:_E_x_p_m_a_p │ │ │ │ │ -static Q Expmap(const Eigen::Ref< const TangentVector > &omega, ChartJacobian │ │ │ │ │ -H=boost::none) │ │ │ │ │ -Exponential map, using the inlined code from Eigen's conversion from axis/ │ │ │ │ │ -angle. │ │ │ │ │ -DDeeffiinniittiioonn Quaternion.h:79 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _Q_U_A_T_E_R_N_I_O_N___T_Y_P_E_ _>_:_:_L_o_g_m_a_p │ │ │ │ │ -static TangentVector Logmap(const Q &q, ChartJacobian H=boost::none) │ │ │ │ │ -We use our own Logmap, as there is a slight bug in Eigen. │ │ │ │ │ -DDeeffiinniittiioonn Quaternion.h:98 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_<_ _3_ _>_:_:_E_x_p_m_a_p_D_e_r_i_v_a_t_i_v_e │ │ │ │ │ -static MatrixDD ExpmapDerivative(const TangentVector &omega) │ │ │ │ │ -Derivative of Expmap, currently only defined for SO3. │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:72 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_<_ _3_ _>_:_:_L_o_g_m_a_p_D_e_r_i_v_a_t_i_v_e │ │ │ │ │ -static MatrixDD LogmapDerivative(const TangentVector &omega) │ │ │ │ │ -Derivative of Logmap, currently only defined for SO3. │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:82 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ +Both ManifoldTraits and Testable. │ │ │ │ │ +DDeeffiinniittiioonn Manifold.h:120 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_D_S_2 │ │ │ │ │ +Calibration of a camera with radial distortion that also supports Lie-group │ │ │ │ │ +behaviors for optimizatio... │ │ │ │ │ +DDeeffiinniittiioonn Cal3DS2.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_D_S_2_:_:_d_i_m │ │ │ │ │ +size_t dim() const override │ │ │ │ │ +Return dimensions of calibration manifold object. │ │ │ │ │ +DDeeffiinniittiioonn Cal3DS2.h:87 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_D_S_2_:_:_c_l_o_n_e │ │ │ │ │ +boost::shared_ptr< Base > clone() const override │ │ │ │ │ +DDeeffiinniittiioonn Cal3DS2.h:97 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_D_S_2_:_:_D_i_m │ │ │ │ │ +static size_t Dim() │ │ │ │ │ +Return dimensions of calibration manifold object. │ │ │ │ │ +DDeeffiinniittiioonn Cal3DS2.h:90 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_D_S_2_:_:_C_a_l_3_D_S_2 │ │ │ │ │ +Cal3DS2()=default │ │ │ │ │ +Default Constructor with only unit focal length. │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_D_S_2___B_a_s_e │ │ │ │ │ +Calibration of a camera with radial distortion. │ │ │ │ │ +DDeeffiinniittiioonn Cal3DS2_Base.h:42 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _Q_u_a_t_e_r_n_i_o_n_._h │ │ │ │ │ + * _C_a_l_3_D_S_2_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00296.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/CameraSet.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Pose3.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,48 +95,75 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
CameraSet.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ +Functions
│ │ │ │ +
Pose3.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Base class to create smart factors on poses or cameras. │ │ │ │ +

3D Pose │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::CameraSet< CAMERA >
 A set of cameras, all with their own calibration. More...
class  gtsam::Pose3
 A 3D pose (R,t) : (Rot3,Point3) More...
 
struct  gtsam::traits< CameraSet< CAMERA > >
struct  gtsam::Pose3::ChartAtOrigin
 
struct  gtsam::traits< const CameraSet< CAMERA > >
struct  gtsam::traits< Pose3 >
 
struct  gtsam::traits< const Pose3 >
 
struct  gtsam::Bearing< Pose3, Point3 >
 
struct  gtsam::Bearing< Pose3, Pose3 >
 
struct  gtsam::Range< Pose3, T >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Typedefs

│ │ │ │ +using gtsam::Pose3Pair = std::pair< Pose3, Pose3 >
 
│ │ │ │ +using gtsam::Pose3Pairs = std::vector< std::pair< Pose3, Pose3 > >
 
│ │ │ │ +typedef std::vector< Pose3 > gtsam::Pose3Vector
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

template<>
Matrix gtsam::wedge< Pose3 > (const Vector &xi)
 wedge for Pose3:
 
│ │ │ │

Detailed Description

│ │ │ │ -

Base class to create smart factors on poses or cameras.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
Date
Feb 19, 2015
│ │ │ │ +

3D Pose

│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,31 +1,48 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -CameraSet.h File Reference │ │ │ │ │ -Base class to create smart factors on poses or cameras. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Pose3.h File Reference │ │ │ │ │ +3D Pose _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_<_ _C_A_M_E_R_A_ _> │ │ │ │ │ -  A set of cameras, all with their own calibration. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ +  A 3D pose (R,t) : (_R_o_t_3,Point3) _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _C_a_m_e_r_a_S_e_t_<_ _C_A_M_E_R_A_ _>_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_P_o_s_e_3_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _C_a_m_e_r_a_S_e_t_<_ _C_A_M_E_R_A_ _>_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _P_o_s_e_3_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _P_o_s_e_3_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_B_e_a_r_i_n_g_<_ _P_o_s_e_3_,_ _P_o_i_n_t_3_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_B_e_a_r_i_n_g_<_ _P_o_s_e_3_,_ _P_o_s_e_3_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_R_a_n_g_e_<_ _P_o_s_e_3_,_ _T_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ + using  ggttssaamm::::PPoossee33PPaaiirr = std::pair< _P_o_s_e_3, _P_o_s_e_3 > │ │ │ │ │ +  │ │ │ │ │ + using  ggttssaamm::::PPoossee33PPaaiirrss = std::vector< std::pair< │ │ │ │ │ + _P_o_s_e_3, _P_o_s_e_3 > > │ │ │ │ │ +  │ │ │ │ │ +typedef std::vector< _P_o_s_e_3 >  ggttssaamm::::PPoossee33VVeeccttoorr │ │ │ │ │ +  │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +template<> │ │ │ │ │ + Matrix  _g_t_s_a_m_:_:_w_e_d_g_e_<_ _P_o_s_e_3_ _> (const Vector &xi) │ │ │ │ │ +  wedge for _P_o_s_e_3: │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Base class to create smart factors on poses or cameras. │ │ │ │ │ - Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - Feb 19, 2015 │ │ │ │ │ +3D Pose │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_m_e_r_a_S_e_t_._h │ │ │ │ │ + * _P_o_s_e_3_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00296.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,5 +1,9 @@ │ │ │ │ │ var a00296 = [ │ │ │ │ │ - ["gtsam::CameraSet< CAMERA >", "a03044.html", "a03044"], │ │ │ │ │ - ["gtsam::traits< CameraSet< CAMERA > >", "a03048.html", null], │ │ │ │ │ - ["gtsam::traits< const CameraSet< CAMERA > >", "a03052.html", null] │ │ │ │ │ + ["gtsam::Pose3::ChartAtOrigin", "a03184.html", null], │ │ │ │ │ + ["gtsam::traits< Pose3 >", "a03188.html", null], │ │ │ │ │ + ["gtsam::traits< const Pose3 >", "a03192.html", null], │ │ │ │ │ + ["gtsam::Bearing< Pose3, Point3 >", "a03196.html", null], │ │ │ │ │ + ["gtsam::Bearing< Pose3, Pose3 >", "a03200.html", null], │ │ │ │ │ + ["gtsam::Range< Pose3, T >", "a03204.html", null], │ │ │ │ │ + ["wedge< Pose3 >", "a00296.html#aefb606eb24889c0f418acf1515a2c723", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00296_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/CameraSet.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Pose3.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,494 +98,334 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
CameraSet.h
│ │ │ │ +
Pose3.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/base/FastMap.h>
│ │ │ │ -
22#include <gtsam/base/SymmetricBlockMatrix.h>
│ │ │ │ -
23#include <gtsam/base/Testable.h>
│ │ │ │ -
24#include <gtsam/geometry/CalibratedCamera.h> // for Cheirality exception
│ │ │ │ -
25#include <gtsam/geometry/Point3.h>
│ │ │ │ -
26#include <gtsam/inference/Key.h>
│ │ │ │ -
27
│ │ │ │ -
28#include <vector>
│ │ │ │ -
29
│ │ │ │ -
30namespace gtsam {
│ │ │ │ +
17// \callgraph
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/config.h>
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/geometry/BearingRange.h>
│ │ │ │ +
23#include <gtsam/geometry/Point3.h>
│ │ │ │ +
24#include <gtsam/geometry/Rot3.h>
│ │ │ │ +
25#include <gtsam/base/Lie.h>
│ │ │ │ +
26
│ │ │ │ +
27namespace gtsam {
│ │ │ │ +
28
│ │ │ │ +
29class Pose2;
│ │ │ │ +
30// forward declare
│ │ │ │
31
│ │ │ │ -
35template <class CAMERA>
│ │ │ │ -
│ │ │ │ -
36class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA>> {
│ │ │ │ -
37 protected:
│ │ │ │ -
38 using Base = std::vector<CAMERA, typename Eigen::aligned_allocator<CAMERA>>;
│ │ │ │ +
│ │ │ │ +
37class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
│ │ │ │ +
38public:
│ │ │ │
39
│ │ │ │ -
44 typedef typename CAMERA::Measurement Z;
│ │ │ │ -
45 typedef typename CAMERA::MeasurementVector ZVector;
│ │ │ │ -
46
│ │ │ │ -
47 static const int D = traits<CAMERA>::dimension;
│ │ │ │ -
48 static const int ZDim = traits<Z>::dimension;
│ │ │ │ -
49
│ │ │ │ -
│ │ │ │ -
51 static Vector ErrorVector(const ZVector& predicted, const ZVector& measured) {
│ │ │ │ -
52 // Check size
│ │ │ │ -
53 size_t m = predicted.size();
│ │ │ │ -
54 if (measured.size() != m)
│ │ │ │ -
55 throw std::runtime_error("CameraSet::errors: size mismatch");
│ │ │ │ +
41 typedef Rot3 Rotation;
│ │ │ │ +
42 typedef Point3 Translation;
│ │ │ │ +
43
│ │ │ │ +
44private:
│ │ │ │ +
45
│ │ │ │ +
46 Rot3 R_;
│ │ │ │ +
47 Point3 t_;
│ │ │ │ +
48
│ │ │ │ +
49public:
│ │ │ │ +
50
│ │ │ │ +
53
│ │ │ │ +
55 Pose3() : R_(traits<Rot3>::Identity()), t_(traits<Point3>::Identity()) {}
│ │ │ │
56
│ │ │ │ -
57 // Project and fill error vector
│ │ │ │ -
58 Vector b(ZDim * m);
│ │ │ │ -
59 for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
│ │ │ │ -
60 Vector bi = traits<Z>::Local(measured[i], predicted[i]);
│ │ │ │ -
61 if (ZDim == 3 && std::isnan(bi(1))) { // if it is a stereo point and the
│ │ │ │ -
62 // right pixel is missing (nan)
│ │ │ │ -
63 bi(1) = 0;
│ │ │ │ -
64 }
│ │ │ │ -
65 b.segment<ZDim>(row) = bi;
│ │ │ │ -
66 }
│ │ │ │ -
67 return b;
│ │ │ │ -
68 }
│ │ │ │ +
│ │ │ │ +
58 Pose3(const Pose3& pose) :
│ │ │ │ +
59 R_(pose.R_), t_(pose.t_) {
│ │ │ │ +
60 }
│ │ │ │ +
│ │ │ │ +
61
│ │ │ │ +
│ │ │ │ +
63 Pose3(const Rot3& R, const Point3& t) :
│ │ │ │ +
64 R_(R), t_(t) {
│ │ │ │ +
65 }
│ │ │ │
│ │ │ │ +
66
│ │ │ │ +
68 explicit Pose3(const Pose2& pose2);
│ │ │ │
69
│ │ │ │ -
70 public:
│ │ │ │ -
71 using Base::Base; // Inherit the vector constructors
│ │ │ │ -
72
│ │ │ │ -
74 virtual ~CameraSet() = default;
│ │ │ │ -
75
│ │ │ │ -
77 using MatrixZD = Eigen::Matrix<double, ZDim, D>;
│ │ │ │ -
78 using FBlocks = std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>>;
│ │ │ │ -
79
│ │ │ │ -
│ │ │ │ -
85 virtual void print(const std::string& s = "") const {
│ │ │ │ -
86 std::cout << s << "CameraSet, cameras = \n";
│ │ │ │ -
87 for (size_t k = 0; k < this->size(); ++k) this->at(k).print(s);
│ │ │ │ -
88 }
│ │ │ │ -
│ │ │ │ -
89
│ │ │ │ -
│ │ │ │ -
91 bool equals(const CameraSet& p, double tol = 1e-9) const {
│ │ │ │ -
92 if (this->size() != p.size()) return false;
│ │ │ │ -
93 bool camerasAreEqual = true;
│ │ │ │ -
94 for (size_t i = 0; i < this->size(); i++) {
│ │ │ │ -
95 if (this->at(i).equals(p.at(i), tol) == false) camerasAreEqual = false;
│ │ │ │ -
96 break;
│ │ │ │ -
97 }
│ │ │ │ -
98 return camerasAreEqual;
│ │ │ │ -
99 }
│ │ │ │ +
│ │ │ │ +
71 Pose3(const Matrix &T) :
│ │ │ │ +
72 R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), T(2, 1),
│ │ │ │ +
73 T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {
│ │ │ │ +
74 }
│ │ │ │
│ │ │ │ +
75
│ │ │ │ +
77 static Pose3 Create(const Rot3& R, const Point3& t,
│ │ │ │ +
78 OptionalJacobian<6, 3> HR = boost::none,
│ │ │ │ +
79 OptionalJacobian<6, 3> Ht = boost::none);
│ │ │ │ +
80
│ │ │ │ +
86 static boost::optional<Pose3> Align(const Point3Pairs& abPointPairs);
│ │ │ │ +
87
│ │ │ │ +
88 // Version of Pose3::Align that takes 2 matrices.
│ │ │ │ +
89 static boost::optional<Pose3> Align(const Matrix& a, const Matrix& b);
│ │ │ │ +
90
│ │ │ │ +
94
│ │ │ │ +
96 void print(const std::string& s = "") const;
│ │ │ │ +
97
│ │ │ │ +
99 bool equals(const Pose3& pose, double tol = 1e-9) const;
│ │ │ │
100
│ │ │ │ -
107 template <class POINT>
│ │ │ │ -
│ │ │ │ -
108 ZVector project2(const POINT& point, //
│ │ │ │ -
109 boost::optional<FBlocks&> Fs = boost::none, //
│ │ │ │ -
110 boost::optional<Matrix&> E = boost::none) const {
│ │ │ │ -
111 static const int N = FixedDimension<POINT>::value;
│ │ │ │ +
104
│ │ │ │ +
│ │ │ │ +
106 static Pose3 Identity() {
│ │ │ │ +
107 return Pose3();
│ │ │ │ +
108 }
│ │ │ │ +
│ │ │ │ +
109
│ │ │ │ +
111 Pose3 inverse() const;
│ │ │ │
112
│ │ │ │ -
113 // Allocate result
│ │ │ │ -
114 size_t m = this->size();
│ │ │ │ -
115 ZVector z;
│ │ │ │ -
116 z.reserve(m);
│ │ │ │ -
117
│ │ │ │ -
118 // Allocate derivatives
│ │ │ │ -
119 if (E) E->resize(ZDim * m, N);
│ │ │ │ -
120 if (Fs) Fs->resize(m);
│ │ │ │ -
121
│ │ │ │ -
122 // Project and fill derivatives
│ │ │ │ -
123 for (size_t i = 0; i < m; i++) {
│ │ │ │ -
124 MatrixZD Fi;
│ │ │ │ -
125 Eigen::Matrix<double, ZDim, N> Ei;
│ │ │ │ -
126 z.emplace_back(this->at(i).project2(point, Fs ? &Fi : 0, E ? &Ei : 0));
│ │ │ │ -
127 if (Fs) (*Fs)[i] = Fi;
│ │ │ │ -
128 if (E) E->block<ZDim, N>(ZDim * i, 0) = Ei;
│ │ │ │ -
129 }
│ │ │ │ -
130
│ │ │ │ -
131 return z;
│ │ │ │ -
132 }
│ │ │ │ +
│ │ │ │ +
114 Pose3 operator*(const Pose3& T) const {
│ │ │ │ +
115 return Pose3(R_ * T.R_, t_ + R_ * T.t_);
│ │ │ │ +
116 }
│ │ │ │
│ │ │ │ +
117
│ │ │ │ +
132 Pose3 interpolateRt(const Pose3& T, double t) const;
│ │ │ │
133
│ │ │ │ -
135 template <class POINT>
│ │ │ │ -
│ │ │ │ -
136 Vector reprojectionError(const POINT& point, const ZVector& measured,
│ │ │ │ -
137 boost::optional<FBlocks&> Fs = boost::none, //
│ │ │ │ -
138 boost::optional<Matrix&> E = boost::none) const {
│ │ │ │ -
139 return ErrorVector(project2(point, Fs, E), measured);
│ │ │ │ -
140 }
│ │ │ │ -
│ │ │ │ -
141
│ │ │ │ -
148 template <int N,
│ │ │ │ -
149 int ND> // N = 2 or 3 (point dimension), ND is the camera dimension
│ │ │ │ -
│ │ │ │ -
150 static SymmetricBlockMatrix SchurComplement(
│ │ │ │ -
151 const std::vector<
│ │ │ │ -
152 Eigen::Matrix<double, ZDim, ND>,
│ │ │ │ -
153 Eigen::aligned_allocator<Eigen::Matrix<double, ZDim, ND>>>& Fs,
│ │ │ │ -
154 const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
│ │ │ │ -
155 // a single point is observed in m cameras
│ │ │ │ -
156 size_t m = Fs.size();
│ │ │ │ -
157
│ │ │ │ -
158 // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column
│ │ │ │ -
159 // with info vector)
│ │ │ │ -
160 size_t M1 = ND * m + 1;
│ │ │ │ -
161 std::vector<DenseIndex> dims(m + 1); // this also includes the b term
│ │ │ │ -
162 std::fill(dims.begin(), dims.end() - 1, ND);
│ │ │ │ -
163 dims.back() = 1;
│ │ │ │ -
164 SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
│ │ │ │ -
165
│ │ │ │ -
166 // Blockwise Schur complement
│ │ │ │ -
167 for (size_t i = 0; i < m; i++) { // for each camera
│ │ │ │ -
168
│ │ │ │ -
169 const Eigen::Matrix<double, ZDim, ND>& Fi = Fs[i];
│ │ │ │ -
170 const auto FiT = Fi.transpose();
│ │ │ │ -
171 const Eigen::Matrix<double, ZDim, N> Ei_P = //
│ │ │ │ -
172 E.block(ZDim * i, 0, ZDim, N) * P;
│ │ │ │ -
173
│ │ │ │ -
174 // D = (Dx2) * ZDim
│ │ │ │ -
175 augmentedHessian.setOffDiagonalBlock(
│ │ │ │ -
176 i, m,
│ │ │ │ -
177 FiT * b.segment<ZDim>(ZDim * i) // F' * b
│ │ │ │ -
178 -
│ │ │ │ -
179 FiT *
│ │ │ │ -
180 (Ei_P *
│ │ │ │ -
181 (E.transpose() *
│ │ │ │ -
182 b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
│ │ │ │ -
183
│ │ │ │ -
184 // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
│ │ │ │ -
185 augmentedHessian.setDiagonalBlock(
│ │ │ │ -
186 i,
│ │ │ │ -
187 FiT * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi));
│ │ │ │ +
137
│ │ │ │ +
139 static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = boost::none);
│ │ │ │ +
140
│ │ │ │ +
142 static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = boost::none);
│ │ │ │ +
143
│ │ │ │ +
148 Matrix6 AdjointMap() const;
│ │ │ │ +
149
│ │ │ │ +
156 Vector6 Adjoint(const Vector6& xi_b,
│ │ │ │ +
157 OptionalJacobian<6, 6> H_this = boost::none,
│ │ │ │ +
158 OptionalJacobian<6, 6> H_xib = boost::none) const;
│ │ │ │ +
159
│ │ │ │ +
161 Vector6 AdjointTranspose(const Vector6& x,
│ │ │ │ +
162 OptionalJacobian<6, 6> H_this = boost::none,
│ │ │ │ +
163 OptionalJacobian<6, 6> H_x = boost::none) const;
│ │ │ │ +
164
│ │ │ │ +
180 static Matrix6 adjointMap(const Vector6& xi);
│ │ │ │ +
181
│ │ │ │ +
185 static Vector6 adjoint(const Vector6& xi, const Vector6& y,
│ │ │ │ +
186 OptionalJacobian<6, 6> Hxi = boost::none,
│ │ │ │ +
187 OptionalJacobian<6, 6> H_y = boost::none);
│ │ │ │
188
│ │ │ │ -
189 // upper triangular part of the hessian
│ │ │ │ -
190 for (size_t j = i + 1; j < m; j++) { // for each camera
│ │ │ │ -
191 const Eigen::Matrix<double, ZDim, ND>& Fj = Fs[j];
│ │ │ │ +
189 // temporary fix for wrappers until case issue is resolved
│ │ │ │ +
190 static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);}
│ │ │ │ +
191 static Vector6 adjoint_(const Vector6 &xi, const Vector6 &y) { return adjoint(xi, y);}
│ │ │ │
192
│ │ │ │ -
193 // (DxD) = (Dx2) * ( (2x2) * (2xD) )
│ │ │ │ -
194 augmentedHessian.setOffDiagonalBlock(
│ │ │ │ -
195 i, j,
│ │ │ │ -
196 -FiT * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj));
│ │ │ │ -
197 }
│ │ │ │ -
198 } // end of for over cameras
│ │ │ │ +
196 static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y,
│ │ │ │ +
197 OptionalJacobian<6, 6> Hxi = boost::none,
│ │ │ │ +
198 OptionalJacobian<6, 6> H_y = boost::none);
│ │ │ │
199
│ │ │ │ -
200 augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
│ │ │ │ -
201 return augmentedHessian;
│ │ │ │ -
202 }
│ │ │ │ -
│ │ │ │ -
203
│ │ │ │ -
217 template <int N, int ND, int NDD>
│ │ │ │ -
│ │ │ │ -
218 static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks(
│ │ │ │ -
219 const std::vector<
│ │ │ │ -
220 Eigen::Matrix<double, ZDim, ND>,
│ │ │ │ -
221 Eigen::aligned_allocator<Eigen::Matrix<double, ZDim, ND>>>& Fs,
│ │ │ │ -
222 const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b,
│ │ │ │ -
223 const KeyVector& jacobianKeys, const KeyVector& hessianKeys) {
│ │ │ │ -
224 size_t nrNonuniqueKeys = jacobianKeys.size();
│ │ │ │ -
225 size_t nrUniqueKeys = hessianKeys.size();
│ │ │ │ -
226
│ │ │ │ -
227 // Marginalize point: note - we reuse the standard SchurComplement function.
│ │ │ │ -
228 SymmetricBlockMatrix augmentedHessian = SchurComplement<N, ND>(Fs, E, P, b);
│ │ │ │ -
229
│ │ │ │ -
230 // Pack into an Hessian factor, allow space for b term.
│ │ │ │ -
231 std::vector<DenseIndex> dims(nrUniqueKeys + 1);
│ │ │ │ -
232 std::fill(dims.begin(), dims.end() - 1, NDD);
│ │ │ │ -
233 dims.back() = 1;
│ │ │ │ -
234 SymmetricBlockMatrix augmentedHessianUniqueKeys;
│ │ │ │ -
235
│ │ │ │ -
236 // Deal with the fact that some blocks may share the same keys.
│ │ │ │ -
237 if (nrUniqueKeys == nrNonuniqueKeys) {
│ │ │ │ -
238 // Case when there is 1 calibration key per camera:
│ │ │ │ -
239 augmentedHessianUniqueKeys = SymmetricBlockMatrix(
│ │ │ │ -
240 dims, Matrix(augmentedHessian.selfadjointView()));
│ │ │ │ -
241 } else {
│ │ │ │ -
242 // When multiple cameras share a calibration we have to rearrange
│ │ │ │ -
243 // the results of the Schur complement matrix.
│ │ │ │ -
244 std::vector<DenseIndex> nonuniqueDims(nrNonuniqueKeys + 1); // includes b
│ │ │ │ -
245 std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, NDD);
│ │ │ │ -
246 nonuniqueDims.back() = 1;
│ │ │ │ -
247 augmentedHessian = SymmetricBlockMatrix(
│ │ │ │ -
248 nonuniqueDims, Matrix(augmentedHessian.selfadjointView()));
│ │ │ │ -
249
│ │ │ │ -
250 // Get map from key to location in the new augmented Hessian matrix (the
│ │ │ │ -
251 // one including only unique keys).
│ │ │ │ -
252 std::map<Key, size_t> keyToSlotMap;
│ │ │ │ -
253 for (size_t k = 0; k < nrUniqueKeys; k++) {
│ │ │ │ -
254 keyToSlotMap[hessianKeys[k]] = k;
│ │ │ │ -
255 }
│ │ │ │ -
256
│ │ │ │ -
257 // Initialize matrix to zero.
│ │ │ │ -
258 augmentedHessianUniqueKeys = SymmetricBlockMatrix(
│ │ │ │ -
259 dims, Matrix::Zero(NDD * nrUniqueKeys + 1, NDD * nrUniqueKeys + 1));
│ │ │ │ -
260
│ │ │ │ -
261 // Add contributions for each key: note this loops over the hessian with
│ │ │ │ -
262 // nonUnique keys (augmentedHessian) and populates an Hessian that only
│ │ │ │ -
263 // includes the unique keys (that is what we want to return).
│ │ │ │ -
264 for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows
│ │ │ │ -
265 Key key_i = jacobianKeys.at(i);
│ │ │ │ -
266
│ │ │ │ -
267 // Update information vector.
│ │ │ │ -
268 augmentedHessianUniqueKeys.updateOffDiagonalBlock(
│ │ │ │ -
269 keyToSlotMap[key_i], nrUniqueKeys,
│ │ │ │ -
270 augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys));
│ │ │ │ -
271
│ │ │ │ -
272 // Update blocks.
│ │ │ │ -
273 for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols
│ │ │ │ -
274 Key key_j = jacobianKeys.at(j);
│ │ │ │ -
275 if (i == j) {
│ │ │ │ -
276 augmentedHessianUniqueKeys.updateDiagonalBlock(
│ │ │ │ -
277 keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i));
│ │ │ │ -
278 } else { // (i < j)
│ │ │ │ -
279 if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) {
│ │ │ │ -
280 augmentedHessianUniqueKeys.updateOffDiagonalBlock(
│ │ │ │ -
281 keyToSlotMap[key_i], keyToSlotMap[key_j],
│ │ │ │ -
282 augmentedHessian.aboveDiagonalBlock(i, j));
│ │ │ │ -
283 } else {
│ │ │ │ -
284 augmentedHessianUniqueKeys.updateDiagonalBlock(
│ │ │ │ -
285 keyToSlotMap[key_i],
│ │ │ │ -
286 augmentedHessian.aboveDiagonalBlock(i, j) +
│ │ │ │ -
287 augmentedHessian.aboveDiagonalBlock(i, j).transpose());
│ │ │ │ -
288 }
│ │ │ │ -
289 }
│ │ │ │ -
290 }
│ │ │ │ -
291 }
│ │ │ │ -
292
│ │ │ │ -
293 // Update bottom right element of the matrix.
│ │ │ │ -
294 augmentedHessianUniqueKeys.updateDiagonalBlock(
│ │ │ │ -
295 nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys));
│ │ │ │ -
296 }
│ │ │ │ -
297 return augmentedHessianUniqueKeys;
│ │ │ │ -
298 }
│ │ │ │ -
│ │ │ │ -
299
│ │ │ │ -
306 template <int N> // N = 2 or 3
│ │ │ │ -
│ │ │ │ -
307 static SymmetricBlockMatrix SchurComplement(
│ │ │ │ -
308 const FBlocks& Fs, const Matrix& E, const Eigen::Matrix<double, N, N>& P,
│ │ │ │ -
309 const Vector& b) {
│ │ │ │ -
310 return SchurComplement<N, D>(Fs, E, P, b);
│ │ │ │ -
311 }
│ │ │ │ -
│ │ │ │ -
312
│ │ │ │ -
314 template <int N> // N = 2 or 3 (point dimension)
│ │ │ │ -
│ │ │ │ -
315 static void ComputePointCovariance(Eigen::Matrix<double, N, N>& P,
│ │ │ │ -
316 const Matrix& E, double lambda,
│ │ │ │ -
317 bool diagonalDamping = false) {
│ │ │ │ -
318 Matrix EtE = E.transpose() * E;
│ │ │ │ -
319
│ │ │ │ -
320 if (diagonalDamping) { // diagonal of the hessian
│ │ │ │ -
321 EtE.diagonal() += lambda * EtE.diagonal();
│ │ │ │ -
322 } else {
│ │ │ │ -
323 DenseIndex n = E.cols();
│ │ │ │ -
324 EtE += lambda * Eigen::MatrixXd::Identity(n, n);
│ │ │ │ -
325 }
│ │ │ │ -
326
│ │ │ │ -
327 P = (EtE).inverse();
│ │ │ │ -
328 }
│ │ │ │ -
│ │ │ │ -
329
│ │ │ │ -
│ │ │ │ -
331 static Matrix PointCov(const Matrix& E, const double lambda = 0.0,
│ │ │ │ -
332 bool diagonalDamping = false) {
│ │ │ │ -
333 if (E.cols() == 2) {
│ │ │ │ -
334 Matrix2 P2;
│ │ │ │ -
335 ComputePointCovariance<2>(P2, E, lambda, diagonalDamping);
│ │ │ │ -
336 return P2;
│ │ │ │ -
337 } else {
│ │ │ │ -
338 Matrix3 P3;
│ │ │ │ -
339 ComputePointCovariance<3>(P3, E, lambda, diagonalDamping);
│ │ │ │ -
340 return P3;
│ │ │ │ -
341 }
│ │ │ │ -
342 }
│ │ │ │ -
│ │ │ │ -
343
│ │ │ │ -
│ │ │ │ -
348 static SymmetricBlockMatrix SchurComplement(const FBlocks& Fblocks,
│ │ │ │ -
349 const Matrix& E, const Vector& b,
│ │ │ │ -
350 const double lambda = 0.0,
│ │ │ │ -
351 bool diagonalDamping = false) {
│ │ │ │ -
352 if (E.cols() == 2) {
│ │ │ │ -
353 Matrix2 P;
│ │ │ │ -
354 ComputePointCovariance<2>(P, E, lambda, diagonalDamping);
│ │ │ │ -
355 return SchurComplement<2>(Fblocks, E, P, b);
│ │ │ │ -
356 } else {
│ │ │ │ -
357 Matrix3 P;
│ │ │ │ -
358 ComputePointCovariance<3>(P, E, lambda, diagonalDamping);
│ │ │ │ -
359 return SchurComplement<3>(Fblocks, E, P, b);
│ │ │ │ -
360 }
│ │ │ │ -
361 }
│ │ │ │ -
│ │ │ │ -
362
│ │ │ │ -
368 template <int N> // N = 2 or 3 (point dimension)
│ │ │ │ -
│ │ │ │ -
369 static void UpdateSchurComplement(
│ │ │ │ -
370 const FBlocks& Fs, const Matrix& E, const Eigen::Matrix<double, N, N>& P,
│ │ │ │ -
371 const Vector& b, const KeyVector& allKeys, const KeyVector& keys,
│ │ │ │ -
372 /*output ->*/ SymmetricBlockMatrix& augmentedHessian) {
│ │ │ │ -
373 assert(keys.size() == Fs.size());
│ │ │ │ -
374 assert(keys.size() <= allKeys.size());
│ │ │ │ -
375
│ │ │ │ -
376 FastMap<Key, size_t> KeySlotMap;
│ │ │ │ -
377 for (size_t slot = 0; slot < allKeys.size(); slot++)
│ │ │ │ -
378 KeySlotMap.insert(std::make_pair(allKeys[slot], slot));
│ │ │ │ -
379
│ │ │ │ -
380 // Schur complement trick
│ │ │ │ -
381 // G = F' * F - F' * E * P * E' * F
│ │ │ │ -
382 // g = F' * (b - E * P * E' * b)
│ │ │ │ -
383
│ │ │ │ -
384 // a single point is observed in m cameras
│ │ │ │ -
385 size_t m = Fs.size(); // cameras observing current point
│ │ │ │ -
386 size_t M = (augmentedHessian.rows() - 1) / D; // all cameras in the group
│ │ │ │ -
387 assert(allKeys.size() == M);
│ │ │ │ -
388
│ │ │ │ -
389 // Blockwise Schur complement
│ │ │ │ -
390 for (size_t i = 0; i < m; i++) { // for each camera in the current factor
│ │ │ │ -
391
│ │ │ │ -
392 const MatrixZD& Fi = Fs[i];
│ │ │ │ -
393 const auto FiT = Fi.transpose();
│ │ │ │ -
394 const Eigen::Matrix<double, 2, N> Ei_P =
│ │ │ │ -
395 E.template block<ZDim, N>(ZDim * i, 0) * P;
│ │ │ │ -
396
│ │ │ │ -
397 // D = (DxZDim) * (ZDim)
│ │ │ │ -
398 // allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7)
│ │ │ │ -
399 // we should map those to a slot in the local (grouped) hessian
│ │ │ │ -
400 // (0,1,2,3,4) Key cameraKey_i = this->keys_[i];
│ │ │ │ -
401 DenseIndex aug_i = KeySlotMap.at(keys[i]);
│ │ │ │ -
402
│ │ │ │ -
403 // information vector - store previous vector
│ │ │ │ -
404 // vectorBlock = augmentedHessian(aug_i, aug_m).knownOffDiagonal();
│ │ │ │ -
405 // add contribution of current factor
│ │ │ │ -
406 augmentedHessian.updateOffDiagonalBlock(
│ │ │ │ -
407 aug_i, M,
│ │ │ │ -
408 FiT * b.segment<ZDim>(ZDim * i) // F' * b
│ │ │ │ -
409 -
│ │ │ │ -
410 FiT *
│ │ │ │ -
411 (Ei_P *
│ │ │ │ -
412 (E.transpose() *
│ │ │ │ -
413 b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
│ │ │ │ -
414
│ │ │ │ -
415 // (DxD) += (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
│ │ │ │ -
416 // add contribution of current factor
│ │ │ │ -
417 // TODO(gareth): Eigen doesn't let us pass the expression. Call eval() for
│ │ │ │ -
418 // now...
│ │ │ │ -
419 augmentedHessian.updateDiagonalBlock(
│ │ │ │ -
420 aug_i,
│ │ │ │ -
421 ((FiT *
│ │ │ │ -
422 (Fi -
│ │ │ │ -
423 Ei_P * E.template block<ZDim, N>(ZDim * i, 0).transpose() * Fi)))
│ │ │ │ -
424 .eval());
│ │ │ │ -
425
│ │ │ │ -
426 // upper triangular part of the hessian
│ │ │ │ -
427 for (size_t j = i + 1; j < m; j++) { // for each camera
│ │ │ │ -
428 const MatrixZD& Fj = Fs[j];
│ │ │ │ -
429
│ │ │ │ -
430 DenseIndex aug_j = KeySlotMap.at(keys[j]);
│ │ │ │ -
431
│ │ │ │ -
432 // (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) )
│ │ │ │ -
433 // off diagonal block - store previous block
│ │ │ │ -
434 // matrixBlock = augmentedHessian(aug_i, aug_j).knownOffDiagonal();
│ │ │ │ -
435 // add contribution of current factor
│ │ │ │ -
436 augmentedHessian.updateOffDiagonalBlock(
│ │ │ │ -
437 aug_i, aug_j,
│ │ │ │ -
438 -FiT * (Ei_P * E.template block<ZDim, N>(ZDim * j, 0).transpose() *
│ │ │ │ -
439 Fj));
│ │ │ │ -
440 }
│ │ │ │ -
441 } // end of for over cameras
│ │ │ │ -
442
│ │ │ │ -
443 augmentedHessian.diagonalBlock(M)(0, 0) += b.squaredNorm();
│ │ │ │ -
444 }
│ │ │ │ -
│ │ │ │ -
445
│ │ │ │ -
446 private:
│ │ │ │ -
448 friend class boost::serialization::access;
│ │ │ │ -
449 template <class ARCHIVE>
│ │ │ │ -
450 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ -
451 ar&(*this);
│ │ │ │ -
452 }
│ │ │ │ -
453
│ │ │ │ -
454 public:
│ │ │ │ -
455 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
456};
│ │ │ │ -
│ │ │ │ -
457
│ │ │ │ -
458template <class CAMERA>
│ │ │ │ -
459const int CameraSet<CAMERA>::D;
│ │ │ │ -
460
│ │ │ │ -
461template <class CAMERA>
│ │ │ │ -
462const int CameraSet<CAMERA>::ZDim;
│ │ │ │ -
463
│ │ │ │ -
464template <class CAMERA>
│ │ │ │ -
465struct traits<CameraSet<CAMERA>> : public Testable<CameraSet<CAMERA>> {};
│ │ │ │ -
466
│ │ │ │ -
467template <class CAMERA>
│ │ │ │ -
468struct traits<const CameraSet<CAMERA>> : public Testable<CameraSet<CAMERA>> {};
│ │ │ │ -
469
│ │ │ │ -
470} // namespace gtsam
│ │ │ │ -
FastMap.h
A thin wrapper around std::map that uses boost's fast_pool_allocator.
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ -
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ -
SymmetricBlockMatrix.h
Access to matrices via blocks of pre-defined sizes.
│ │ │ │ -
Point3.h
3D Point
│ │ │ │ -
CalibratedCamera.h
Calibrated camera for which only pose is unknown.
│ │ │ │ - │ │ │ │ +
201 static Matrix6 ExpmapDerivative(const Vector6& xi);
│ │ │ │ +
202
│ │ │ │ +
204 static Matrix6 LogmapDerivative(const Pose3& xi);
│ │ │ │ +
205
│ │ │ │ +
206 // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
│ │ │ │ +
│ │ │ │ +
207 struct ChartAtOrigin {
│ │ │ │ +
208 static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = boost::none);
│ │ │ │ +
209 static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none);
│ │ │ │ +
210 };
│ │ │ │ +
│ │ │ │ +
211
│ │ │ │ +
221 static Matrix3 ComputeQforExpmapDerivative(
│ │ │ │ +
222 const Vector6& xi, double nearZeroThreshold = 1e-5);
│ │ │ │ +
223
│ │ │ │ +
224 using LieGroup<Pose3, 6>::inverse; // version with derivative
│ │ │ │ +
225
│ │ │ │ +
│ │ │ │ +
233 static Matrix wedge(double wx, double wy, double wz, double vx, double vy,
│ │ │ │ +
234 double vz) {
│ │ │ │ +
235 return (Matrix(4, 4) << 0., -wz, wy, vx, wz, 0., -wx, vy, -wy, wx, 0., vz, 0., 0., 0., 0.).finished();
│ │ │ │ +
236 }
│ │ │ │ +
│ │ │ │ +
237
│ │ │ │ +
241
│ │ │ │ +
249 Point3 transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself =
│ │ │ │ +
250 boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
│ │ │ │ +
251
│ │ │ │ +
257 Matrix transformFrom(const Matrix& points) const;
│ │ │ │ +
258
│ │ │ │ +
│ │ │ │ +
260 inline Point3 operator*(const Point3& point) const {
│ │ │ │ +
261 return transformFrom(point);
│ │ │ │ +
262 }
│ │ │ │ +
│ │ │ │ +
263
│ │ │ │ +
271 Point3 transformTo(const Point3& point, OptionalJacobian<3, 6> Hself =
│ │ │ │ +
272 boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
│ │ │ │ +
273
│ │ │ │ +
279 Matrix transformTo(const Matrix& points) const;
│ │ │ │ +
280
│ │ │ │ +
284
│ │ │ │ +
286 const Rot3& rotation(OptionalJacobian<3, 6> Hself = boost::none) const;
│ │ │ │ +
287
│ │ │ │ +
289 const Point3& translation(OptionalJacobian<3, 6> Hself = boost::none) const;
│ │ │ │ +
290
│ │ │ │ +
│ │ │ │ +
292 double x() const {
│ │ │ │ +
293 return t_.x();
│ │ │ │ +
294 }
│ │ │ │ +
│ │ │ │ +
295
│ │ │ │ +
│ │ │ │ +
297 double y() const {
│ │ │ │ +
298 return t_.y();
│ │ │ │ +
299 }
│ │ │ │ +
│ │ │ │ +
300
│ │ │ │ +
│ │ │ │ +
302 double z() const {
│ │ │ │ +
303 return t_.z();
│ │ │ │ +
304 }
│ │ │ │ +
│ │ │ │ +
305
│ │ │ │ +
307 Matrix4 matrix() const;
│ │ │ │ +
308
│ │ │ │ +
314 Pose3 transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself = boost::none,
│ │ │ │ +
315 OptionalJacobian<6, 6> HaTb = boost::none) const;
│ │ │ │ +
316
│ │ │ │ +
321 Pose3 transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself = boost::none,
│ │ │ │ +
322 OptionalJacobian<6, 6> HwTb = boost::none) const;
│ │ │ │ +
323
│ │ │ │ +
329 double range(const Point3& point, OptionalJacobian<1, 6> Hself = boost::none,
│ │ │ │ +
330 OptionalJacobian<1, 3> Hpoint = boost::none) const;
│ │ │ │ +
331
│ │ │ │ +
337 double range(const Pose3& pose, OptionalJacobian<1, 6> Hself = boost::none,
│ │ │ │ +
338 OptionalJacobian<1, 6> Hpose = boost::none) const;
│ │ │ │ +
339
│ │ │ │ +
345 Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> Hself = boost::none,
│ │ │ │ +
346 OptionalJacobian<2, 3> Hpoint = boost::none) const;
│ │ │ │ +
347
│ │ │ │ +
354 Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself = boost::none,
│ │ │ │ +
355 OptionalJacobian<2, 6> Hpose = boost::none) const;
│ │ │ │ +
356
│ │ │ │ +
360
│ │ │ │ +
│ │ │ │ +
366 inline static std::pair<size_t, size_t> translationInterval() {
│ │ │ │ +
367 return std::make_pair(3, 5);
│ │ │ │ +
368 }
│ │ │ │ +
│ │ │ │ +
369
│ │ │ │ +
│ │ │ │ +
375 static std::pair<size_t, size_t> rotationInterval() {
│ │ │ │ +
376 return std::make_pair(0, 2);
│ │ │ │ +
377 }
│ │ │ │ +
│ │ │ │ +
378
│ │ │ │ +
384 Pose3 slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none,
│ │ │ │ +
385 OptionalJacobian<6, 6> Hy = boost::none) const;
│ │ │ │ +
386
│ │ │ │ +
388 GTSAM_EXPORT
│ │ │ │ +
389 friend std::ostream &operator<<(std::ostream &os, const Pose3& p);
│ │ │ │ +
390
│ │ │ │ +
391 private:
│ │ │ │ +
393 friend class boost::serialization::access;
│ │ │ │ +
394 template<class Archive>
│ │ │ │ +
395 void serialize(Archive & ar, const unsigned int /*version*/) {
│ │ │ │ +
396 ar & BOOST_SERIALIZATION_NVP(R_);
│ │ │ │ +
397 ar & BOOST_SERIALIZATION_NVP(t_);
│ │ │ │ +
398 }
│ │ │ │ +
400
│ │ │ │ +
401#ifdef GTSAM_USE_QUATERNIONS
│ │ │ │ +
402 // Align if we are using Quaternions
│ │ │ │ +
403 public:
│ │ │ │ +
404 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
405#endif
│ │ │ │ +
406};
│ │ │ │ +
│ │ │ │ +
407// Pose3 class
│ │ │ │ +
408
│ │ │ │ +
416template<>
│ │ │ │ +
│ │ │ │ +
417inline Matrix wedge<Pose3>(const Vector& xi) {
│ │ │ │ +
418 return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5));
│ │ │ │ +
419}
│ │ │ │ +
│ │ │ │ +
420
│ │ │ │ +
421// Convenience typedef
│ │ │ │ +
422using Pose3Pair = std::pair<Pose3, Pose3>;
│ │ │ │ +
423using Pose3Pairs = std::vector<std::pair<Pose3, Pose3> >;
│ │ │ │ +
424
│ │ │ │ +
425// For MATLAB wrapper
│ │ │ │ +
426typedef std::vector<Pose3> Pose3Vector;
│ │ │ │ +
427
│ │ │ │ +
428template <>
│ │ │ │ +
429struct traits<Pose3> : public internal::LieGroup<Pose3> {};
│ │ │ │ +
430
│ │ │ │ +
431template <>
│ │ │ │ +
432struct traits<const Pose3> : public internal::LieGroup<Pose3> {};
│ │ │ │ +
433
│ │ │ │ +
434// bearing and range traits, used in RangeFactor
│ │ │ │ +
435template <>
│ │ │ │ +
436struct Bearing<Pose3, Point3> : HasBearing<Pose3, Point3, Unit3> {};
│ │ │ │ +
437
│ │ │ │ +
438template<>
│ │ │ │ +
439struct Bearing<Pose3, Pose3> : HasBearing<Pose3, Pose3, Unit3> {};
│ │ │ │ +
440
│ │ │ │ +
441template <typename T>
│ │ │ │ +
442struct Range<Pose3, T> : HasRange<Pose3, T, double> {};
│ │ │ │ +
443
│ │ │ │ +
444} // namespace gtsam
│ │ │ │ +
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ +
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ +
Rot3.h
3D rotation represented as a rotation matrix or quaternion
│ │ │ │ +
Point3.h
3D Point
│ │ │ │ +
BearingRange.h
Bearing-Range product.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │ -
gtsam::row
const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j)
Extracts a row view from a matrix that avoids a copy.
Definition Matrix.h:222
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ +
gtsam::transformTo
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Transform a line from world to camera frame.
Definition Line3.cpp:94
│ │ │ │ +
gtsam::wedge< Pose3 >
Matrix wedge< Pose3 >(const Vector &xi)
wedge for Pose3:
Definition Pose3.h:417
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ -
gtsam::FixedDimension
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition Manifold.h:164
│ │ │ │ -
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
│ │ │ │ -
gtsam::SymmetricBlockMatrix::setDiagonalBlock
void setDiagonalBlock(DenseIndex I, const XprType &xpr)
Set a diagonal block. Only the upper triangular portion of xpr is evaluated.
Definition SymmetricBlockMatrix.h:195
│ │ │ │ -
gtsam::SymmetricBlockMatrix::setOffDiagonalBlock
void setOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
Set an off-diagonal block. Only the upper triangular portion of xpr is evaluated.
Definition SymmetricBlockMatrix.h:201
│ │ │ │ -
gtsam::SymmetricBlockMatrix::aboveDiagonalBlock
constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
Get block above the diagonal (I, J).
Definition SymmetricBlockMatrix.h:150
│ │ │ │ -
gtsam::SymmetricBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition SymmetricBlockMatrix.h:114
│ │ │ │ -
gtsam::SymmetricBlockMatrix::updateOffDiagonalBlock
void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
Update an off diagonal block.
Definition SymmetricBlockMatrix.h:228
│ │ │ │ -
gtsam::SymmetricBlockMatrix::diagonalBlock
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J'th diagonal block as a self adjoint view.
Definition SymmetricBlockMatrix.h:135
│ │ │ │ -
gtsam::SymmetricBlockMatrix::updateDiagonalBlock
void updateDiagonalBlock(DenseIndex I, const XprType &xpr)
Increment the diagonal block by the values in xpr. Only reads the upper triangular part of xpr.
Definition SymmetricBlockMatrix.h:212
│ │ │ │ -
gtsam::SymmetricBlockMatrix::selfadjointView
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j).
Definition SymmetricBlockMatrix.h:156
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition CameraSet.h:36
│ │ │ │ -
gtsam::CameraSet::reprojectionError
Vector reprojectionError(const POINT &point, const ZVector &measured, boost::optional< FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const
Calculate vector [project2(point)-z] of re-projection errors.
Definition CameraSet.h:136
│ │ │ │ -
gtsam::CameraSet::print
virtual void print(const std::string &s="") const
print
Definition CameraSet.h:85
│ │ │ │ -
gtsam::CameraSet::ComputePointCovariance
static void ComputePointCovariance(Eigen::Matrix< double, N, N > &P, const Matrix &E, double lambda, bool diagonalDamping=false)
Computes Point Covariance P, with lambda parameter.
Definition CameraSet.h:315
│ │ │ │ -
gtsam::CameraSet::SchurComplement
static SymmetricBlockMatrix SchurComplement(const FBlocks &Fblocks, const Matrix &E, const Vector &b, const double lambda=0.0, bool diagonalDamping=false)
Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix Dynamic version.
Definition CameraSet.h:348
│ │ │ │ -
gtsam::CameraSet::Z
CAMERA::Measurement Z
2D measurement and noise model for each of the m views The order is kept the same as the keys that we...
Definition CameraSet.h:44
│ │ │ │ -
gtsam::CameraSet::~CameraSet
virtual ~CameraSet()=default
Destructor.
│ │ │ │ -
gtsam::CameraSet::equals
bool equals(const CameraSet &p, double tol=1e-9) const
equals
Definition CameraSet.h:91
│ │ │ │ -
gtsam::CameraSet::PointCov
static Matrix PointCov(const Matrix &E, const double lambda=0.0, bool diagonalDamping=false)
Computes Point Covariance P, with lambda parameter, dynamic version.
Definition CameraSet.h:331
│ │ │ │ -
gtsam::CameraSet::ErrorVector
static Vector ErrorVector(const ZVector &predicted, const ZVector &measured)
Make a vector of re-projection errors.
Definition CameraSet.h:51
│ │ │ │ -
gtsam::CameraSet::UpdateSchurComplement
static void UpdateSchurComplement(const FBlocks &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b, const KeyVector &allKeys, const KeyVector &keys, SymmetricBlockMatrix &augmentedHessian)
Applies Schur complement (exploiting block structure) to get a smart factor on cameras,...
Definition CameraSet.h:369
│ │ │ │ -
gtsam::CameraSet::SchurComplementAndRearrangeBlocks
static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks(const std::vector< Eigen::Matrix< double, ZDim, ND >, Eigen::aligned_allocator< Eigen::Matrix< double, ZDim, ND > > > &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b, const KeyVector &jacobianKeys, const KeyVector &hessianKeys)
Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = F' * F - F' * E * P * ...
Definition CameraSet.h:218
│ │ │ │ -
gtsam::CameraSet::SchurComplement
static SymmetricBlockMatrix SchurComplement(const std::vector< Eigen::Matrix< double, ZDim, ND >, Eigen::aligned_allocator< Eigen::Matrix< double, ZDim, ND > > > &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b)
Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = F' * F - F' * E * P * ...
Definition CameraSet.h:150
│ │ │ │ -
gtsam::CameraSet::ZDim
static const int ZDim
Measurement dimension.
Definition CameraSet.h:48
│ │ │ │ -
gtsam::CameraSet::SchurComplement
static SymmetricBlockMatrix SchurComplement(const FBlocks &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b)
Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = F' * F - F' * E * P * ...
Definition CameraSet.h:307
│ │ │ │ -
gtsam::CameraSet::D
static const int D
Camera dimension.
Definition CameraSet.h:47
│ │ │ │ -
gtsam::CameraSet::access
friend class boost::serialization::access
Serialization function.
Definition CameraSet.h:448
│ │ │ │ -
gtsam::CameraSet::MatrixZD
Eigen::Matrix< double, ZDim, D > MatrixZD
Definitions for blocks of F.
Definition CameraSet.h:77
│ │ │ │ -
gtsam::CameraSet::project2
ZVector project2(const POINT &point, boost::optional< FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const
Project a point (possibly Unit3 at infinity), with derivatives Note that F is a sparse block-diagonal...
Definition CameraSet.h:108
│ │ │ │ +
gtsam::LieGroup
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition Lie.h:37
│ │ │ │ +
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition Lie.h:229
│ │ │ │ +
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::Bearing
Definition BearingRange.h:34
│ │ │ │ +
gtsam::Range
Definition BearingRange.h:40
│ │ │ │ +
gtsam::HasBearing
Definition BearingRange.h:180
│ │ │ │ +
gtsam::HasRange
Definition BearingRange.h:194
│ │ │ │ +
gtsam::Pose2
A 2D pose (Point2,Rot2)
Definition Pose2.h:36
│ │ │ │ +
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ +
gtsam::Pose3::Identity
static Pose3 Identity()
identity for group operation
Definition Pose3.h:106
│ │ │ │ +
gtsam::Pose3::Pose3
Pose3(const Pose3 &pose)
Copy constructor.
Definition Pose3.h:58
│ │ │ │ +
gtsam::Pose3::Pose3
Pose3()
Default constructor is origin.
Definition Pose3.h:55
│ │ │ │ +
gtsam::Pose3::wedge
static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz)
wedge for Pose3:
Definition Pose3.h:233
│ │ │ │ +
gtsam::Pose3::Pose3
Pose3(const Rot3 &R, const Point3 &t)
Construct from R,t.
Definition Pose3.h:63
│ │ │ │ +
gtsam::Pose3::Pose3
Pose3(const Matrix &T)
Constructor from 4*4 matrix.
Definition Pose3.h:71
│ │ │ │ +
gtsam::Pose3::z
double z() const
get z
Definition Pose3.h:302
│ │ │ │ +
gtsam::Pose3::rotationInterval
static std::pair< size_t, size_t > rotationInterval()
Return the start and end indices (inclusive) of the rotation component of the exponential map paramet...
Definition Pose3.h:375
│ │ │ │ +
gtsam::Pose3::operator*
Pose3 operator*(const Pose3 &T) const
compose syntactic sugar
Definition Pose3.h:114
│ │ │ │ +
gtsam::Pose3::Rotation
Rot3 Rotation
Pose Concept requirements.
Definition Pose3.h:41
│ │ │ │ +
gtsam::Pose3::y
double y() const
get y
Definition Pose3.h:297
│ │ │ │ +
gtsam::Pose3::translationInterval
static std::pair< size_t, size_t > translationInterval()
Return the start and end indices (inclusive) of the translation component of the exponential map para...
Definition Pose3.h:366
│ │ │ │ +
gtsam::Pose3::operator*
Point3 operator*(const Point3 &point) const
syntactic sugar for transformFrom
Definition Pose3.h:260
│ │ │ │ +
gtsam::Pose3::x
double x() const
get x
Definition Pose3.h:292
│ │ │ │ +
gtsam::Pose3::ChartAtOrigin
Definition Pose3.h:207
│ │ │ │ +
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
│ │ │ │ +
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,606 +1,402 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -CameraSet.h │ │ │ │ │ +Pose3.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_M_a_p_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_._h> // for Cheirality exception │ │ │ │ │ -25#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_3_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ -27 │ │ │ │ │ -28#include │ │ │ │ │ -29 │ │ │ │ │ -30namespace _g_t_s_a_m { │ │ │ │ │ +17// \callgraph │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include │ │ │ │ │ +21 │ │ │ │ │ +22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_B_e_a_r_i_n_g_R_a_n_g_e_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_3_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_3_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_b_a_s_e_/_L_i_e_._h> │ │ │ │ │ +26 │ │ │ │ │ +27namespace _g_t_s_a_m { │ │ │ │ │ +28 │ │ │ │ │ +29class Pose2; │ │ │ │ │ +30// forward declare │ │ │ │ │ 31 │ │ │ │ │ -35template │ │ │ │ │ -_3_6class _C_a_m_e_r_a_S_e_t : public std::vector> { │ │ │ │ │ -37 protected: │ │ │ │ │ -38 using Base = std::vector>; │ │ │ │ │ +_3_7class GTSAM_EXPORT _P_o_s_e_3: public _L_i_e_G_r_o_u_p { │ │ │ │ │ +38public: │ │ │ │ │ 39 │ │ │ │ │ -_4_4 typedef typename CAMERA::Measurement _Z; │ │ │ │ │ -45 typedef typename CAMERA::MeasurementVector ZVector; │ │ │ │ │ -46 │ │ │ │ │ -_4_7 static const int _D = _t_r_a_i_t_s_<_C_A_M_E_R_A_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ -_4_8 static const int _Z_D_i_m = _t_r_a_i_t_s_<_Z_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ -49 │ │ │ │ │ -_5_1 static Vector _E_r_r_o_r_V_e_c_t_o_r(const ZVector& predicted, const ZVector& measured) │ │ │ │ │ -{ │ │ │ │ │ -52 // Check size │ │ │ │ │ -53 size_t m = predicted.size(); │ │ │ │ │ -54 if (measured.size() != m) │ │ │ │ │ -55 throw std::runtime_error("CameraSet::errors: size mismatch"); │ │ │ │ │ +_4_1 typedef _R_o_t_3 _R_o_t_a_t_i_o_n; │ │ │ │ │ +42 typedef _P_o_i_n_t_3 Translation; │ │ │ │ │ +43 │ │ │ │ │ +44private: │ │ │ │ │ +45 │ │ │ │ │ +46 _R_o_t_3 R_; │ │ │ │ │ +47 _P_o_i_n_t_3 t_; │ │ │ │ │ +48 │ │ │ │ │ +49public: │ │ │ │ │ +50 │ │ │ │ │ +53 │ │ │ │ │ +_5_5 _P_o_s_e_3() : R_(_t_r_a_i_t_s<_R_o_t_3>::Identity()), t_(_t_r_a_i_t_s<_P_o_i_n_t_3>::Identity()) {} │ │ │ │ │ 56 │ │ │ │ │ -57 // Project and fill error vector │ │ │ │ │ -58 Vector b(_Z_D_i_m * m); │ │ │ │ │ -59 for (size_t i = 0, _r_o_w = 0; i < m; i++, _r_o_w += _Z_D_i_m) { │ │ │ │ │ -60 Vector bi = _t_r_a_i_t_s_<_Z_>_:_:_L_o_c_a_l(measured[i], predicted[i]); │ │ │ │ │ -61 if (_Z_D_i_m == 3 && std::isnan(bi(1))) { // if it is a stereo point and the │ │ │ │ │ -62 // right pixel is missing (nan) │ │ │ │ │ -63 bi(1) = 0; │ │ │ │ │ -64 } │ │ │ │ │ -65 b.segment<_Z_D_i_m>(_r_o_w) = bi; │ │ │ │ │ -66 } │ │ │ │ │ -67 return b; │ │ │ │ │ -68 } │ │ │ │ │ +_5_8 _P_o_s_e_3(const _P_o_s_e_3& pose) : │ │ │ │ │ +59 R_(pose.R_), t_(pose.t_) { │ │ │ │ │ +60 } │ │ │ │ │ +61 │ │ │ │ │ +_6_3 _P_o_s_e_3(const _R_o_t_3& R, const _P_o_i_n_t_3& t) : │ │ │ │ │ +64 R_(R), t_(t) { │ │ │ │ │ +65 } │ │ │ │ │ +66 │ │ │ │ │ +68 explicit _P_o_s_e_3(const _P_o_s_e_2& pose2); │ │ │ │ │ 69 │ │ │ │ │ -70 public: │ │ │ │ │ -71 using Base::Base; // Inherit the vector constructors │ │ │ │ │ -72 │ │ │ │ │ -_7_4 virtual _~_C_a_m_e_r_a_S_e_t() = default; │ │ │ │ │ +_7_1 _P_o_s_e_3(const Matrix &T) : │ │ │ │ │ +72 R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), T(2, 1), │ │ │ │ │ +73 T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) { │ │ │ │ │ +74 } │ │ │ │ │ 75 │ │ │ │ │ -_7_7 using _M_a_t_r_i_x_Z_D = Eigen::Matrix; │ │ │ │ │ -78 using FBlocks = std::vector>; │ │ │ │ │ -79 │ │ │ │ │ -_8_5 virtual void _p_r_i_n_t(const std::string& s = "") const { │ │ │ │ │ -86 std::cout << s << "CameraSet, cameras = \n"; │ │ │ │ │ -87 for (size_t k = 0; k < this->size(); ++k) this->at(k).print(s); │ │ │ │ │ -88 } │ │ │ │ │ -89 │ │ │ │ │ -_9_1 bool _e_q_u_a_l_s(const _C_a_m_e_r_a_S_e_t& p, double tol = 1e-9) const { │ │ │ │ │ -92 if (this->size() != p.size()) return false; │ │ │ │ │ -93 bool camerasAreEqual = true; │ │ │ │ │ -94 for (size_t i = 0; i < this->size(); i++) { │ │ │ │ │ -95 if (this->at(i).equals(p.at(i), tol) == false) camerasAreEqual = false; │ │ │ │ │ -96 break; │ │ │ │ │ -97 } │ │ │ │ │ -98 return camerasAreEqual; │ │ │ │ │ -99 } │ │ │ │ │ +77 static _P_o_s_e_3 Create(const _R_o_t_3& R, const _P_o_i_n_t_3& t, │ │ │ │ │ +78 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _3_> HR = boost::none, │ │ │ │ │ +79 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _3_> Ht = boost::none); │ │ │ │ │ +80 │ │ │ │ │ +86 static boost::optional Align(const Point3Pairs& abPointPairs); │ │ │ │ │ +87 │ │ │ │ │ +88 // Version of Pose3::Align that takes 2 matrices. │ │ │ │ │ +89 static boost::optional Align(const Matrix& a, const Matrix& b); │ │ │ │ │ +90 │ │ │ │ │ +94 │ │ │ │ │ +96 void _p_r_i_n_t(const std::string& s = "") const; │ │ │ │ │ +97 │ │ │ │ │ +99 bool _e_q_u_a_l_s(const _P_o_s_e_3& pose, double tol = 1e-9) const; │ │ │ │ │ 100 │ │ │ │ │ -107 template │ │ │ │ │ -_1_0_8 ZVector _p_r_o_j_e_c_t_2(const POINT& point, // │ │ │ │ │ -109 boost::optional Fs = boost::none, // │ │ │ │ │ -110 boost::optional E = boost::none) const { │ │ │ │ │ -111 static const int N = _F_i_x_e_d_D_i_m_e_n_s_i_o_n_<_P_O_I_N_T_>_:_:_v_a_l_u_e; │ │ │ │ │ +104 │ │ │ │ │ +_1_0_6 static _P_o_s_e_3 _I_d_e_n_t_i_t_y() { │ │ │ │ │ +107 return _P_o_s_e_3(); │ │ │ │ │ +108 } │ │ │ │ │ +109 │ │ │ │ │ +111 _P_o_s_e_3 inverse() const; │ │ │ │ │ 112 │ │ │ │ │ -113 // Allocate result │ │ │ │ │ -114 size_t m = this->size(); │ │ │ │ │ -115 ZVector z; │ │ │ │ │ -116 z.reserve(m); │ │ │ │ │ +_1_1_4 _P_o_s_e_3 _o_p_e_r_a_t_o_r_*(const _P_o_s_e_3& T) const { │ │ │ │ │ +115 return _P_o_s_e_3(R_ * T.R_, t_ + R_ * T.t_); │ │ │ │ │ +116 } │ │ │ │ │ 117 │ │ │ │ │ -118 // Allocate derivatives │ │ │ │ │ -119 if (E) E->resize(_Z_D_i_m * m, N); │ │ │ │ │ -120 if (Fs) Fs->resize(m); │ │ │ │ │ -121 │ │ │ │ │ -122 // Project and fill derivatives │ │ │ │ │ -123 for (size_t i = 0; i < m; i++) { │ │ │ │ │ -124 _M_a_t_r_i_x_Z_D Fi; │ │ │ │ │ -125 Eigen::Matrix Ei; │ │ │ │ │ -126 z.emplace_back(this->at(i)._p_r_o_j_e_c_t_2(point, Fs ? &Fi : 0, E ? &Ei : 0)); │ │ │ │ │ -127 if (Fs) (*Fs)[i] = Fi; │ │ │ │ │ -128 if (E) E->block<_Z_D_i_m, N>(_Z_D_i_m * i, 0) = Ei; │ │ │ │ │ -129 } │ │ │ │ │ -130 │ │ │ │ │ -131 return z; │ │ │ │ │ -132 } │ │ │ │ │ +132 _P_o_s_e_3 interpolateRt(const _P_o_s_e_3& T, double t) const; │ │ │ │ │ 133 │ │ │ │ │ -135 template │ │ │ │ │ -_1_3_6 Vector _r_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r(const POINT& point, const ZVector& measured, │ │ │ │ │ -137 boost::optional Fs = boost::none, // │ │ │ │ │ -138 boost::optional E = boost::none) const { │ │ │ │ │ -139 return _E_r_r_o_r_V_e_c_t_o_r(_p_r_o_j_e_c_t_2(point, Fs, E), measured); │ │ │ │ │ -140 } │ │ │ │ │ -141 │ │ │ │ │ -148 template // N = 2 or 3 (point dimension), ND is the camera dimension │ │ │ │ │ -_1_5_0 static _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x _S_c_h_u_r_C_o_m_p_l_e_m_e_n_t( │ │ │ │ │ -151 const std::vector< │ │ │ │ │ -152 Eigen::Matrix, │ │ │ │ │ -153 Eigen::aligned_allocator>>& Fs, │ │ │ │ │ -154 const Matrix& E, const Eigen::Matrix& P, const Vector& b) { │ │ │ │ │ -155 // a single point is observed in m cameras │ │ │ │ │ -156 size_t m = Fs.size(); │ │ │ │ │ -157 │ │ │ │ │ -158 // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column │ │ │ │ │ -159 // with info vector) │ │ │ │ │ -160 size_t M1 = ND * m + 1; │ │ │ │ │ -161 std::vector dims(m + 1); // this also includes the b term │ │ │ │ │ -162 std::fill(dims.begin(), dims.end() - 1, ND); │ │ │ │ │ -163 dims.back() = 1; │ │ │ │ │ -164 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x augmentedHessian(dims, Matrix::Zero(M1, M1)); │ │ │ │ │ -165 │ │ │ │ │ -166 // Blockwise Schur complement │ │ │ │ │ -167 for (size_t i = 0; i < m; i++) { // for each camera │ │ │ │ │ -168 │ │ │ │ │ -169 const Eigen::Matrix& Fi = Fs[i]; │ │ │ │ │ -170 const auto FiT = Fi.transpose(); │ │ │ │ │ -171 const Eigen::Matrix Ei_P = // │ │ │ │ │ -172 E.block(_Z_D_i_m * i, 0, _Z_D_i_m, N) * P; │ │ │ │ │ -173 │ │ │ │ │ -174 // D = (Dx2) * ZDim │ │ │ │ │ -175 augmentedHessian._s_e_t_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k( │ │ │ │ │ -176 i, m, │ │ │ │ │ -177 FiT * b.segment<_Z_D_i_m>(_Z_D_i_m * i) // F' * b │ │ │ │ │ -178 - │ │ │ │ │ -179 FiT * │ │ │ │ │ -180 (Ei_P * │ │ │ │ │ -181 (E.transpose() * │ │ │ │ │ -182 b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) │ │ │ │ │ -183 │ │ │ │ │ -184 // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) │ │ │ │ │ -185 augmentedHessian._s_e_t_D_i_a_g_o_n_a_l_B_l_o_c_k( │ │ │ │ │ -186 i, │ │ │ │ │ -187 FiT * (Fi - Ei_P * E.block(_Z_D_i_m * i, 0, _Z_D_i_m, N).transpose() * Fi)); │ │ │ │ │ +137 │ │ │ │ │ +139 static _P_o_s_e_3 Expmap(const Vector6& xi, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> Hxi = boost:: │ │ │ │ │ +none); │ │ │ │ │ +140 │ │ │ │ │ +142 static Vector6 Logmap(const _P_o_s_e_3& pose, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> Hpose = │ │ │ │ │ +boost::none); │ │ │ │ │ +143 │ │ │ │ │ +148 Matrix6 AdjointMap() const; │ │ │ │ │ +149 │ │ │ │ │ +156 Vector6 Adjoint(const Vector6& xi_b, │ │ │ │ │ +157 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> H_this = boost::none, │ │ │ │ │ +158 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> H_xib = boost::none) const; │ │ │ │ │ +159 │ │ │ │ │ +161 Vector6 AdjointTranspose(const Vector6& x, │ │ │ │ │ +162 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> H_this = boost::none, │ │ │ │ │ +163 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> H_x = boost::none) const; │ │ │ │ │ +164 │ │ │ │ │ +180 static Matrix6 adjointMap(const Vector6& xi); │ │ │ │ │ +181 │ │ │ │ │ +185 static Vector6 adjoint(const Vector6& xi, const Vector6& y, │ │ │ │ │ +186 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> Hxi = boost::none, │ │ │ │ │ +187 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> H_y = boost::none); │ │ │ │ │ 188 │ │ │ │ │ -189 // upper triangular part of the hessian │ │ │ │ │ -190 for (size_t j = i + 1; j < m; j++) { // for each camera │ │ │ │ │ -191 const Eigen::Matrix& Fj = Fs[j]; │ │ │ │ │ +189 // temporary fix for wrappers until case issue is resolved │ │ │ │ │ +190 static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);} │ │ │ │ │ +191 static Vector6 adjoint_(const Vector6 &xi, const Vector6 &y) { return │ │ │ │ │ +adjoint(xi, y);} │ │ │ │ │ 192 │ │ │ │ │ -193 // (DxD) = (Dx2) * ( (2x2) * (2xD) ) │ │ │ │ │ -194 augmentedHessian._s_e_t_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k( │ │ │ │ │ -195 i, j, │ │ │ │ │ -196 -FiT * (Ei_P * E.block(_Z_D_i_m * j, 0, _Z_D_i_m, N).transpose() * Fj)); │ │ │ │ │ -197 } │ │ │ │ │ -198 } // end of for over cameras │ │ │ │ │ +196 static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, │ │ │ │ │ +197 OptionalJacobian<6, 6> Hxi = boost::none, │ │ │ │ │ +198 OptionalJacobian<6, 6> H_y = boost::none); │ │ │ │ │ 199 │ │ │ │ │ -200 augmentedHessian._d_i_a_g_o_n_a_l_B_l_o_c_k(m)(0, 0) += b.squaredNorm(); │ │ │ │ │ -201 return augmentedHessian; │ │ │ │ │ -202 } │ │ │ │ │ -203 │ │ │ │ │ -217 template │ │ │ │ │ -_2_1_8 static _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x _S_c_h_u_r_C_o_m_p_l_e_m_e_n_t_A_n_d_R_e_a_r_r_a_n_g_e_B_l_o_c_k_s( │ │ │ │ │ -219 const std::vector< │ │ │ │ │ -220 Eigen::Matrix, │ │ │ │ │ -221 Eigen::aligned_allocator>>& Fs, │ │ │ │ │ -222 const Matrix& E, const Eigen::Matrix& P, const Vector& b, │ │ │ │ │ -223 const _K_e_y_V_e_c_t_o_r& jacobianKeys, const _K_e_y_V_e_c_t_o_r& hessianKeys) { │ │ │ │ │ -224 size_t nrNonuniqueKeys = jacobianKeys.size(); │ │ │ │ │ -225 size_t nrUniqueKeys = hessianKeys.size(); │ │ │ │ │ -226 │ │ │ │ │ -227 // Marginalize point: note - we reuse the standard SchurComplement │ │ │ │ │ -function. │ │ │ │ │ -228 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x augmentedHessian = SchurComplement(Fs, E, P, │ │ │ │ │ -b); │ │ │ │ │ -229 │ │ │ │ │ -230 // Pack into an Hessian factor, allow space for b term. │ │ │ │ │ -231 std::vector dims(nrUniqueKeys + 1); │ │ │ │ │ -232 std::fill(dims.begin(), dims.end() - 1, NDD); │ │ │ │ │ -233 dims.back() = 1; │ │ │ │ │ -234 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x augmentedHessianUniqueKeys; │ │ │ │ │ -235 │ │ │ │ │ -236 // Deal with the fact that some blocks may share the same keys. │ │ │ │ │ -237 if (nrUniqueKeys == nrNonuniqueKeys) { │ │ │ │ │ -238 // Case when there is 1 calibration key per camera: │ │ │ │ │ -239 augmentedHessianUniqueKeys = _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x( │ │ │ │ │ -240 dims, Matrix(augmentedHessian._s_e_l_f_a_d_j_o_i_n_t_V_i_e_w())); │ │ │ │ │ -241 } else { │ │ │ │ │ -242 // When multiple cameras share a calibration we have to rearrange │ │ │ │ │ -243 // the results of the Schur complement matrix. │ │ │ │ │ -244 std::vector nonuniqueDims(nrNonuniqueKeys + 1); // includes b │ │ │ │ │ -245 std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, NDD); │ │ │ │ │ -246 nonuniqueDims.back() = 1; │ │ │ │ │ -247 augmentedHessian = _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x( │ │ │ │ │ -248 nonuniqueDims, Matrix(augmentedHessian._s_e_l_f_a_d_j_o_i_n_t_V_i_e_w())); │ │ │ │ │ -249 │ │ │ │ │ -250 // Get map from key to location in the new augmented Hessian matrix (the │ │ │ │ │ -251 // one including only unique keys). │ │ │ │ │ -252 std::map keyToSlotMap; │ │ │ │ │ -253 for (size_t k = 0; k < nrUniqueKeys; k++) { │ │ │ │ │ -254 keyToSlotMap[hessianKeys[k]] = k; │ │ │ │ │ -255 } │ │ │ │ │ -256 │ │ │ │ │ -257 // Initialize matrix to zero. │ │ │ │ │ -258 augmentedHessianUniqueKeys = _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x( │ │ │ │ │ -259 dims, Matrix::Zero(NDD * nrUniqueKeys + 1, NDD * nrUniqueKeys + 1)); │ │ │ │ │ -260 │ │ │ │ │ -261 // Add contributions for each key: note this loops over the hessian with │ │ │ │ │ -262 // nonUnique keys (augmentedHessian) and populates an Hessian that only │ │ │ │ │ -263 // includes the unique keys (that is what we want to return). │ │ │ │ │ -264 for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows │ │ │ │ │ -265 _K_e_y key_i = jacobianKeys.at(i); │ │ │ │ │ -266 │ │ │ │ │ -267 // Update information vector. │ │ │ │ │ -268 augmentedHessianUniqueKeys._u_p_d_a_t_e_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k( │ │ │ │ │ -269 keyToSlotMap[key_i], nrUniqueKeys, │ │ │ │ │ -270 augmentedHessian._a_b_o_v_e_D_i_a_g_o_n_a_l_B_l_o_c_k(i, nrNonuniqueKeys)); │ │ │ │ │ -271 │ │ │ │ │ -272 // Update blocks. │ │ │ │ │ -273 for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols │ │ │ │ │ -274 _K_e_y key_j = jacobianKeys.at(j); │ │ │ │ │ -275 if (i == j) { │ │ │ │ │ -276 augmentedHessianUniqueKeys._u_p_d_a_t_e_D_i_a_g_o_n_a_l_B_l_o_c_k( │ │ │ │ │ -277 keyToSlotMap[key_i], augmentedHessian._d_i_a_g_o_n_a_l_B_l_o_c_k(i)); │ │ │ │ │ -278 } else { // (i < j) │ │ │ │ │ -279 if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) { │ │ │ │ │ -280 augmentedHessianUniqueKeys._u_p_d_a_t_e_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k( │ │ │ │ │ -281 keyToSlotMap[key_i], keyToSlotMap[key_j], │ │ │ │ │ -282 augmentedHessian._a_b_o_v_e_D_i_a_g_o_n_a_l_B_l_o_c_k(i, j)); │ │ │ │ │ -283 } else { │ │ │ │ │ -284 augmentedHessianUniqueKeys._u_p_d_a_t_e_D_i_a_g_o_n_a_l_B_l_o_c_k( │ │ │ │ │ -285 keyToSlotMap[key_i], │ │ │ │ │ -286 augmentedHessian._a_b_o_v_e_D_i_a_g_o_n_a_l_B_l_o_c_k(i, j) + │ │ │ │ │ -287 augmentedHessian._a_b_o_v_e_D_i_a_g_o_n_a_l_B_l_o_c_k(i, j).transpose()); │ │ │ │ │ -288 } │ │ │ │ │ -289 } │ │ │ │ │ -290 } │ │ │ │ │ -291 } │ │ │ │ │ -292 │ │ │ │ │ -293 // Update bottom right element of the matrix. │ │ │ │ │ -294 augmentedHessianUniqueKeys._u_p_d_a_t_e_D_i_a_g_o_n_a_l_B_l_o_c_k( │ │ │ │ │ -295 nrUniqueKeys, augmentedHessian._d_i_a_g_o_n_a_l_B_l_o_c_k(nrNonuniqueKeys)); │ │ │ │ │ -296 } │ │ │ │ │ -297 return augmentedHessianUniqueKeys; │ │ │ │ │ -298 } │ │ │ │ │ -299 │ │ │ │ │ -306 template // N = 2 or 3 │ │ │ │ │ -_3_0_7 static _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x _S_c_h_u_r_C_o_m_p_l_e_m_e_n_t( │ │ │ │ │ -308 const FBlocks& Fs, const Matrix& E, const Eigen::Matrix& P, │ │ │ │ │ -309 const Vector& b) { │ │ │ │ │ -310 return SchurComplement(Fs, E, P, b); │ │ │ │ │ -311 } │ │ │ │ │ -312 │ │ │ │ │ -314 template // N = 2 or 3 (point dimension) │ │ │ │ │ -_3_1_5 static void _C_o_m_p_u_t_e_P_o_i_n_t_C_o_v_a_r_i_a_n_c_e(Eigen::Matrix& P, │ │ │ │ │ -316 const Matrix& E, double lambda, │ │ │ │ │ -317 bool diagonalDamping = false) { │ │ │ │ │ -318 Matrix EtE = E.transpose() * E; │ │ │ │ │ -319 │ │ │ │ │ -320 if (diagonalDamping) { // diagonal of the hessian │ │ │ │ │ -321 EtE.diagonal() += lambda * EtE.diagonal(); │ │ │ │ │ -322 } else { │ │ │ │ │ -323 _D_e_n_s_e_I_n_d_e_x n = E.cols(); │ │ │ │ │ -324 EtE += lambda * Eigen::MatrixXd::Identity(n, n); │ │ │ │ │ -325 } │ │ │ │ │ -326 │ │ │ │ │ -327 P = (EtE).inverse(); │ │ │ │ │ -328 } │ │ │ │ │ -329 │ │ │ │ │ -_3_3_1 static Matrix _P_o_i_n_t_C_o_v(const Matrix& E, const double lambda = 0.0, │ │ │ │ │ -332 bool diagonalDamping = false) { │ │ │ │ │ -333 if (E.cols() == 2) { │ │ │ │ │ -334 Matrix2 P2; │ │ │ │ │ -335 ComputePointCovariance<2>(P2, E, lambda, diagonalDamping); │ │ │ │ │ -336 return P2; │ │ │ │ │ -337 } else { │ │ │ │ │ -338 Matrix3 P3; │ │ │ │ │ -339 ComputePointCovariance<3>(P3, E, lambda, diagonalDamping); │ │ │ │ │ -340 return P3; │ │ │ │ │ -341 } │ │ │ │ │ -342 } │ │ │ │ │ -343 │ │ │ │ │ -_3_4_8 static _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x _S_c_h_u_r_C_o_m_p_l_e_m_e_n_t(const FBlocks& Fblocks, │ │ │ │ │ -349 const Matrix& E, const Vector& b, │ │ │ │ │ -350 const double lambda = 0.0, │ │ │ │ │ -351 bool diagonalDamping = false) { │ │ │ │ │ -352 if (E.cols() == 2) { │ │ │ │ │ -353 Matrix2 P; │ │ │ │ │ -354 ComputePointCovariance<2>(P, E, lambda, diagonalDamping); │ │ │ │ │ -355 return SchurComplement<2>(Fblocks, E, P, b); │ │ │ │ │ -356 } else { │ │ │ │ │ -357 Matrix3 P; │ │ │ │ │ -358 ComputePointCovariance<3>(P, E, lambda, diagonalDamping); │ │ │ │ │ -359 return SchurComplement<3>(Fblocks, E, P, b); │ │ │ │ │ -360 } │ │ │ │ │ -361 } │ │ │ │ │ -362 │ │ │ │ │ -368 template // N = 2 or 3 (point dimension) │ │ │ │ │ -_3_6_9 static void _U_p_d_a_t_e_S_c_h_u_r_C_o_m_p_l_e_m_e_n_t( │ │ │ │ │ -370 const FBlocks& Fs, const Matrix& E, const Eigen::Matrix& P, │ │ │ │ │ -371 const Vector& b, const _K_e_y_V_e_c_t_o_r& allKeys, const _K_e_y_V_e_c_t_o_r& keys, │ │ │ │ │ -372 /*output ->*/ _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x& augmentedHessian) { │ │ │ │ │ -373 assert(keys.size() == Fs.size()); │ │ │ │ │ -374 assert(keys.size() <= allKeys.size()); │ │ │ │ │ -375 │ │ │ │ │ -376 _F_a_s_t_M_a_p_<_K_e_y_,_ _s_i_z_e___t_> KeySlotMap; │ │ │ │ │ -377 for (size_t slot = 0; slot < allKeys.size(); slot++) │ │ │ │ │ -378 KeySlotMap.insert(std::make_pair(allKeys[slot], slot)); │ │ │ │ │ -379 │ │ │ │ │ -380 // Schur complement trick │ │ │ │ │ -381 // G = F' * F - F' * E * P * E' * F │ │ │ │ │ -382 // g = F' * (b - E * P * E' * b) │ │ │ │ │ -383 │ │ │ │ │ -384 // a single point is observed in m cameras │ │ │ │ │ -385 size_t m = Fs.size(); // cameras observing current point │ │ │ │ │ -386 size_t M = (augmentedHessian._r_o_w_s() - 1) / _D; // all cameras in the group │ │ │ │ │ -387 assert(allKeys.size() == M); │ │ │ │ │ -388 │ │ │ │ │ -389 // Blockwise Schur complement │ │ │ │ │ -390 for (size_t i = 0; i < m; i++) { // for each camera in the current factor │ │ │ │ │ -391 │ │ │ │ │ -392 const _M_a_t_r_i_x_Z_D& Fi = Fs[i]; │ │ │ │ │ -393 const auto FiT = Fi.transpose(); │ │ │ │ │ -394 const Eigen::Matrix Ei_P = │ │ │ │ │ -395 E.template block(_Z_D_i_m * i, 0) * P; │ │ │ │ │ -396 │ │ │ │ │ -397 // D = (DxZDim) * (ZDim) │ │ │ │ │ -398 // allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7) │ │ │ │ │ -399 // we should map those to a slot in the local (grouped) hessian │ │ │ │ │ -400 // (0,1,2,3,4) Key cameraKey_i = this->keys_[i]; │ │ │ │ │ -401 _D_e_n_s_e_I_n_d_e_x aug_i = KeySlotMap.at(keys[i]); │ │ │ │ │ -402 │ │ │ │ │ -403 // information vector - store previous vector │ │ │ │ │ -404 // vectorBlock = augmentedHessian(aug_i, aug_m).knownOffDiagonal(); │ │ │ │ │ -405 // add contribution of current factor │ │ │ │ │ -406 augmentedHessian._u_p_d_a_t_e_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k( │ │ │ │ │ -407 aug_i, M, │ │ │ │ │ -408 FiT * b.segment<_Z_D_i_m>(_Z_D_i_m * i) // F' * b │ │ │ │ │ -409 - │ │ │ │ │ -410 FiT * │ │ │ │ │ -411 (Ei_P * │ │ │ │ │ -412 (E.transpose() * │ │ │ │ │ -413 b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) │ │ │ │ │ -414 │ │ │ │ │ -415 // (DxD) += (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) │ │ │ │ │ -416 // add contribution of current factor │ │ │ │ │ -417 // TODO(gareth): Eigen doesn't let us pass the expression. Call eval() for │ │ │ │ │ -418 // now... │ │ │ │ │ -419 augmentedHessian._u_p_d_a_t_e_D_i_a_g_o_n_a_l_B_l_o_c_k( │ │ │ │ │ -420 aug_i, │ │ │ │ │ -421 ((FiT * │ │ │ │ │ -422 (Fi - │ │ │ │ │ -423 Ei_P * E.template block(_Z_D_i_m * i, 0).transpose() * Fi))) │ │ │ │ │ -424 .eval()); │ │ │ │ │ -425 │ │ │ │ │ -426 // upper triangular part of the hessian │ │ │ │ │ -427 for (size_t j = i + 1; j < m; j++) { // for each camera │ │ │ │ │ -428 const _M_a_t_r_i_x_Z_D& Fj = Fs[j]; │ │ │ │ │ -429 │ │ │ │ │ -430 _D_e_n_s_e_I_n_d_e_x aug_j = KeySlotMap.at(keys[j]); │ │ │ │ │ -431 │ │ │ │ │ -432 // (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) ) │ │ │ │ │ -433 // off diagonal block - store previous block │ │ │ │ │ -434 // matrixBlock = augmentedHessian(aug_i, aug_j).knownOffDiagonal(); │ │ │ │ │ -435 // add contribution of current factor │ │ │ │ │ -436 augmentedHessian._u_p_d_a_t_e_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k( │ │ │ │ │ -437 aug_i, aug_j, │ │ │ │ │ -438 -FiT * (Ei_P * E.template block(_Z_D_i_m * j, 0).transpose() * │ │ │ │ │ -439 Fj)); │ │ │ │ │ -440 } │ │ │ │ │ -441 } // end of for over cameras │ │ │ │ │ -442 │ │ │ │ │ -443 augmentedHessian._d_i_a_g_o_n_a_l_B_l_o_c_k(M)(0, 0) += b.squaredNorm(); │ │ │ │ │ -444 } │ │ │ │ │ -445 │ │ │ │ │ -446 private: │ │ │ │ │ -_4_4_8 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -449 template │ │ │ │ │ -450 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ -451 ar&(*this); │ │ │ │ │ -452 } │ │ │ │ │ -453 │ │ │ │ │ -454 public: │ │ │ │ │ -455 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -456}; │ │ │ │ │ -457 │ │ │ │ │ -458template │ │ │ │ │ -459const int _C_a_m_e_r_a_S_e_t_<_C_A_M_E_R_A_>_:_:_D; │ │ │ │ │ -460 │ │ │ │ │ -461template │ │ │ │ │ -462const int _C_a_m_e_r_a_S_e_t_<_C_A_M_E_R_A_>_:_:_Z_D_i_m; │ │ │ │ │ -463 │ │ │ │ │ -464template │ │ │ │ │ -_4_6_5struct _t_r_a_i_t_s<_C_a_m_e_r_a_S_e_t> : public _T_e_s_t_a_b_l_e> {}; │ │ │ │ │ -466 │ │ │ │ │ -467template │ │ │ │ │ -_4_6_8struct _t_r_a_i_t_s> : public _T_e_s_t_a_b_l_e> │ │ │ │ │ -{}; │ │ │ │ │ -469 │ │ │ │ │ -470} // namespace gtsam │ │ │ │ │ -_F_a_s_t_M_a_p_._h │ │ │ │ │ -A thin wrapper around std::map that uses boost's fast_pool_allocator. │ │ │ │ │ -_T_e_s_t_a_b_l_e_._h │ │ │ │ │ -Concept check for values that can be used in unit tests. │ │ │ │ │ +201 static Matrix6 ExpmapDerivative(const Vector6& xi); │ │ │ │ │ +202 │ │ │ │ │ +204 static Matrix6 LogmapDerivative(const Pose3& xi); │ │ │ │ │ +205 │ │ │ │ │ +206 // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP │ │ │ │ │ +_2_0_7 struct _C_h_a_r_t_A_t_O_r_i_g_i_n { │ │ │ │ │ +208 static _P_o_s_e_3 Retract(const Vector6& xi, ChartJacobian Hxi = boost::none); │ │ │ │ │ +209 static Vector6 Local(const _P_o_s_e_3& pose, ChartJacobian Hpose = boost::none); │ │ │ │ │ +210 }; │ │ │ │ │ +211 │ │ │ │ │ +221 static Matrix3 ComputeQforExpmapDerivative( │ │ │ │ │ +222 const Vector6& xi, double nearZeroThreshold = 1e-5); │ │ │ │ │ +223 │ │ │ │ │ +224 using _L_i_e_G_r_o_u_p<_P_o_s_e_3, 6>::inverse; // version with derivative │ │ │ │ │ +225 │ │ │ │ │ +_2_3_3 static Matrix _w_e_d_g_e(double wx, double wy, double wz, double vx, double vy, │ │ │ │ │ +234 double vz) { │ │ │ │ │ +235 return (Matrix(4, 4) << 0., -wz, wy, vx, wz, 0., -wx, vy, -wy, wx, 0., vz, │ │ │ │ │ +0., 0., 0., 0.).finished(); │ │ │ │ │ +236 } │ │ │ │ │ +237 │ │ │ │ │ +241 │ │ │ │ │ +249 _P_o_i_n_t_3 transformFrom(const _P_o_i_n_t_3& point, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _6_> Hself = │ │ │ │ │ +250 boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> Hpoint = boost::none) const; │ │ │ │ │ +251 │ │ │ │ │ +257 Matrix transformFrom(const Matrix& points) const; │ │ │ │ │ +258 │ │ │ │ │ +_2_6_0 inline _P_o_i_n_t_3 _o_p_e_r_a_t_o_r_*(const _P_o_i_n_t_3& point) const { │ │ │ │ │ +261 return transformFrom(point); │ │ │ │ │ +262 } │ │ │ │ │ +263 │ │ │ │ │ +271 _P_o_i_n_t_3 _t_r_a_n_s_f_o_r_m_T_o(const _P_o_i_n_t_3& point, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _6_> Hself = │ │ │ │ │ +272 boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> Hpoint = boost::none) const; │ │ │ │ │ +273 │ │ │ │ │ +279 Matrix _t_r_a_n_s_f_o_r_m_T_o(const Matrix& points) const; │ │ │ │ │ +280 │ │ │ │ │ +284 │ │ │ │ │ +286 const _R_o_t_3& rotation(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _6_> Hself = boost::none) const; │ │ │ │ │ +287 │ │ │ │ │ +289 const _P_o_i_n_t_3& translation(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _6_> Hself = boost::none) │ │ │ │ │ +const; │ │ │ │ │ +290 │ │ │ │ │ +_2_9_2 double _x() const { │ │ │ │ │ +293 return t_.x(); │ │ │ │ │ +294 } │ │ │ │ │ +295 │ │ │ │ │ +_2_9_7 double _y() const { │ │ │ │ │ +298 return t_.y(); │ │ │ │ │ +299 } │ │ │ │ │ +300 │ │ │ │ │ +_3_0_2 double _z() const { │ │ │ │ │ +303 return t_.z(); │ │ │ │ │ +304 } │ │ │ │ │ +305 │ │ │ │ │ +307 Matrix4 matrix() const; │ │ │ │ │ +308 │ │ │ │ │ +314 _P_o_s_e_3 transformPoseFrom(const _P_o_s_e_3& aTb, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> Hself = │ │ │ │ │ +boost::none, │ │ │ │ │ +315 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> HaTb = boost::none) const; │ │ │ │ │ +316 │ │ │ │ │ +321 _P_o_s_e_3 transformPoseTo(const _P_o_s_e_3& wTb, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> Hself = │ │ │ │ │ +boost::none, │ │ │ │ │ +322 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> HwTb = boost::none) const; │ │ │ │ │ +323 │ │ │ │ │ +329 double range(const _P_o_i_n_t_3& point, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> Hself = boost:: │ │ │ │ │ +none, │ │ │ │ │ +330 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> Hpoint = boost::none) const; │ │ │ │ │ +331 │ │ │ │ │ +337 double range(const _P_o_s_e_3& pose, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> Hself = boost::none, │ │ │ │ │ +338 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> Hpose = boost::none) const; │ │ │ │ │ +339 │ │ │ │ │ +345 _U_n_i_t_3 bearing(const _P_o_i_n_t_3& point, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> Hself = boost:: │ │ │ │ │ +none, │ │ │ │ │ +346 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> Hpoint = boost::none) const; │ │ │ │ │ +347 │ │ │ │ │ +354 _U_n_i_t_3 bearing(const _P_o_s_e_3& pose, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> Hself = boost:: │ │ │ │ │ +none, │ │ │ │ │ +355 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> Hpose = boost::none) const; │ │ │ │ │ +356 │ │ │ │ │ +360 │ │ │ │ │ +_3_6_6 inline static std::pair _t_r_a_n_s_l_a_t_i_o_n_I_n_t_e_r_v_a_l() { │ │ │ │ │ +367 return std::make_pair(3, 5); │ │ │ │ │ +368 } │ │ │ │ │ +369 │ │ │ │ │ +_3_7_5 static std::pair _r_o_t_a_t_i_o_n_I_n_t_e_r_v_a_l() { │ │ │ │ │ +376 return std::make_pair(0, 2); │ │ │ │ │ +377 } │ │ │ │ │ +378 │ │ │ │ │ +384 _P_o_s_e_3 slerp(double t, const _P_o_s_e_3& other, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> Hx = │ │ │ │ │ +boost::none, │ │ │ │ │ +385 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> Hy = boost::none) const; │ │ │ │ │ +386 │ │ │ │ │ +388 GTSAM_EXPORT │ │ │ │ │ +389 friend std::ostream &operator<<(std::ostream &os, const _P_o_s_e_3& p); │ │ │ │ │ +390 │ │ │ │ │ +391 private: │ │ │ │ │ +_3_9_3 friend class boost::serialization::access; │ │ │ │ │ +394 template │ │ │ │ │ +395 void serialize(Archive & ar, const unsigned int /*version*/) { │ │ │ │ │ +396 ar & BOOST_SERIALIZATION_NVP(R_); │ │ │ │ │ +397 ar & BOOST_SERIALIZATION_NVP(t_); │ │ │ │ │ +398 } │ │ │ │ │ +400 │ │ │ │ │ +401#ifdef GTSAM_USE_QUATERNIONS │ │ │ │ │ +402 // Align if we are using Quaternions │ │ │ │ │ +403 public: │ │ │ │ │ +404 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +405#endif │ │ │ │ │ +406}; │ │ │ │ │ +407// Pose3 class │ │ │ │ │ +408 │ │ │ │ │ +416template<> │ │ │ │ │ +_4_1_7inline Matrix _w_e_d_g_e_<_P_o_s_e_3_>(const Vector& xi) { │ │ │ │ │ +418 return _P_o_s_e_3_:_:_w_e_d_g_e(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5)); │ │ │ │ │ +419} │ │ │ │ │ +420 │ │ │ │ │ +421// Convenience typedef │ │ │ │ │ +422using Pose3Pair = std::pair; │ │ │ │ │ +423using Pose3Pairs = std::vector >; │ │ │ │ │ +424 │ │ │ │ │ +425// For MATLAB wrapper │ │ │ │ │ +426typedef std::vector Pose3Vector; │ │ │ │ │ +427 │ │ │ │ │ +428template <> │ │ │ │ │ +_4_2_9struct _t_r_a_i_t_s<_P_o_s_e_3> : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ +430 │ │ │ │ │ +431template <> │ │ │ │ │ +_4_3_2struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ +433 │ │ │ │ │ +434// bearing and range traits, used in RangeFactor │ │ │ │ │ +435template <> │ │ │ │ │ +_4_3_6struct _B_e_a_r_i_n_g<_P_o_s_e_3, _P_o_i_n_t_3> : _H_a_s_B_e_a_r_i_n_g {}; │ │ │ │ │ +437 │ │ │ │ │ +438template<> │ │ │ │ │ +_4_3_9struct _B_e_a_r_i_n_g<_P_o_s_e_3, _P_o_s_e_3> : _H_a_s_B_e_a_r_i_n_g {}; │ │ │ │ │ +440 │ │ │ │ │ +441template │ │ │ │ │ +_4_4_2struct _R_a_n_g_e<_P_o_s_e_3, T> : _H_a_s_R_a_n_g_e {}; │ │ │ │ │ +443 │ │ │ │ │ +444} // namespace gtsam │ │ │ │ │ _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ This marks a GTSAM object to require alignment. │ │ │ │ │ DDeeffiinniittiioonn types.h:308 │ │ │ │ │ -_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_._h │ │ │ │ │ -Access to matrices via blocks of pre-defined sizes. │ │ │ │ │ +_L_i_e_._h │ │ │ │ │ +Base class and basic functions for Lie types. │ │ │ │ │ +_R_o_t_3_._h │ │ │ │ │ +3D rotation represented as a rotation matrix or quaternion │ │ │ │ │ _P_o_i_n_t_3_._h │ │ │ │ │ 3D Point │ │ │ │ │ -_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_._h │ │ │ │ │ -Calibrated camera for which only pose is unknown. │ │ │ │ │ -_K_e_y_._h │ │ │ │ │ +_B_e_a_r_i_n_g_R_a_n_g_e_._h │ │ │ │ │ +Bearing-Range product. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_n_s_e_I_n_d_e_x │ │ │ │ │ -ptrdiff_t DenseIndex │ │ │ │ │ -The index type for Eigen objects. │ │ │ │ │ -DDeeffiinniittiioonn types.h:106 │ │ │ │ │ -_g_t_s_a_m_:_:_r_o_w │ │ │ │ │ -const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j) │ │ │ │ │ -Extracts a row view from a matrix that avoids a copy. │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:222 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ +Vector3 Point3 │ │ │ │ │ +As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ +typedef Point3 to Vector3... │ │ │ │ │ +DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_n_s_f_o_r_m_T_o │ │ │ │ │ +Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > │ │ │ │ │ +Dpose, OptionalJacobian< 4, 4 > Dline) │ │ │ │ │ +Transform a line from world to camera frame. │ │ │ │ │ +DDeeffiinniittiioonn Line3.cpp:94 │ │ │ │ │ +_g_t_s_a_m_:_:_w_e_d_g_e_<_ _P_o_s_e_3_ _> │ │ │ │ │ +Matrix wedge< Pose3 >(const Vector &xi) │ │ │ │ │ +wedge for Pose3: │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:417 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ -FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ -fast_pool_allocator instead of the defa... │ │ │ │ │ -DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_F_i_x_e_d_D_i_m_e_n_s_i_o_n │ │ │ │ │ -Give fixed size dimension of a type, fails at compile time if dynamic. │ │ │ │ │ -DDeeffiinniittiioonn Manifold.h:164 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ -of blocks. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_s_e_t_D_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ -void setDiagonalBlock(DenseIndex I, const XprType &xpr) │ │ │ │ │ -Set a diagonal block. Only the upper triangular portion of xpr is evaluated. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:195 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_s_e_t_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ -void setOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr) │ │ │ │ │ -Set an off-diagonal block. Only the upper triangular portion of xpr is │ │ │ │ │ -evaluated. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:201 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_a_b_o_v_e_D_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ -constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const │ │ │ │ │ -Get block above the diagonal (I, J). │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:150 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_r_o_w_s │ │ │ │ │ -DenseIndex rows() const │ │ │ │ │ -Row size. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:114 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_u_p_d_a_t_e_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ -void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr) │ │ │ │ │ -Update an off diagonal block. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:228 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_d_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ -Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J) │ │ │ │ │ -Return the J'th diagonal block as a self adjoint view. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:135 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_u_p_d_a_t_e_D_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ -void updateDiagonalBlock(DenseIndex I, const XprType &xpr) │ │ │ │ │ -Increment the diagonal block by the values in xpr. Only reads the upper │ │ │ │ │ -triangular part of xpr. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:212 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_s_e_l_f_a_d_j_o_i_n_t_V_i_e_w │ │ │ │ │ -Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex │ │ │ │ │ -I, DenseIndex J) const │ │ │ │ │ -Return the square sub-matrix that contains blocks(i:j, i:j). │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:156 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t │ │ │ │ │ -A set of cameras, all with their own calibration. │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_r_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r │ │ │ │ │ -Vector reprojectionError(const POINT &point, const ZVector &measured, boost:: │ │ │ │ │ -optional< FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost:: │ │ │ │ │ -none) const │ │ │ │ │ -Calculate vector [project2(point)-z] of re-projection errors. │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:136 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_p_r_i_n_t │ │ │ │ │ -virtual void print(const std::string &s="") const │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_C_o_m_p_u_t_e_P_o_i_n_t_C_o_v_a_r_i_a_n_c_e │ │ │ │ │ -static void ComputePointCovariance(Eigen::Matrix< double, N, N > &P, const │ │ │ │ │ -Matrix &E, double lambda, bool diagonalDamping=false) │ │ │ │ │ -Computes Point Covariance P, with lambda parameter. │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:315 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_S_c_h_u_r_C_o_m_p_l_e_m_e_n_t │ │ │ │ │ -static SymmetricBlockMatrix SchurComplement(const FBlocks &Fblocks, const │ │ │ │ │ -Matrix &E, const Vector &b, const double lambda=0.0, bool │ │ │ │ │ -diagonalDamping=false) │ │ │ │ │ -Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix │ │ │ │ │ -Dynamic version. │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:348 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_Z │ │ │ │ │ -CAMERA::Measurement Z │ │ │ │ │ -2D measurement and noise model for each of the m views The order is kept the │ │ │ │ │ -same as the keys that we... │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_~_C_a_m_e_r_a_S_e_t │ │ │ │ │ -virtual ~CameraSet()=default │ │ │ │ │ -Destructor. │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const CameraSet &p, double tol=1e-9) const │ │ │ │ │ -equals │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:91 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_P_o_i_n_t_C_o_v │ │ │ │ │ -static Matrix PointCov(const Matrix &E, const double lambda=0.0, bool │ │ │ │ │ -diagonalDamping=false) │ │ │ │ │ -Computes Point Covariance P, with lambda parameter, dynamic version. │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:331 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_E_r_r_o_r_V_e_c_t_o_r │ │ │ │ │ -static Vector ErrorVector(const ZVector &predicted, const ZVector &measured) │ │ │ │ │ -Make a vector of re-projection errors. │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:51 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_U_p_d_a_t_e_S_c_h_u_r_C_o_m_p_l_e_m_e_n_t │ │ │ │ │ -static void UpdateSchurComplement(const FBlocks &Fs, const Matrix &E, const │ │ │ │ │ -Eigen::Matrix< double, N, N > &P, const Vector &b, const KeyVector &allKeys, │ │ │ │ │ -const KeyVector &keys, SymmetricBlockMatrix &augmentedHessian) │ │ │ │ │ -Applies Schur complement (exploiting block structure) to get a smart factor on │ │ │ │ │ -cameras,... │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:369 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_S_c_h_u_r_C_o_m_p_l_e_m_e_n_t_A_n_d_R_e_a_r_r_a_n_g_e_B_l_o_c_k_s │ │ │ │ │ -static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks(const std:: │ │ │ │ │ -vector< Eigen::Matrix< double, ZDim, ND >, Eigen::aligned_allocator< Eigen:: │ │ │ │ │ -Matrix< double, ZDim, ND > > > &Fs, const Matrix &E, const Eigen::Matrix< │ │ │ │ │ -double, N, N > &P, const Vector &b, const KeyVector &jacobianKeys, const │ │ │ │ │ -KeyVector &hessianKeys) │ │ │ │ │ -Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = │ │ │ │ │ -F' * F - F' * E * P * ... │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:218 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_S_c_h_u_r_C_o_m_p_l_e_m_e_n_t │ │ │ │ │ -static SymmetricBlockMatrix SchurComplement(const std::vector< Eigen::Matrix< │ │ │ │ │ -double, ZDim, ND >, Eigen::aligned_allocator< Eigen::Matrix< double, ZDim, ND > │ │ │ │ │ -> > &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector │ │ │ │ │ -&b) │ │ │ │ │ -Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = │ │ │ │ │ -F' * F - F' * E * P * ... │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:150 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_Z_D_i_m │ │ │ │ │ -static const int ZDim │ │ │ │ │ -Measurement dimension. │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_S_c_h_u_r_C_o_m_p_l_e_m_e_n_t │ │ │ │ │ -static SymmetricBlockMatrix SchurComplement(const FBlocks &Fs, const Matrix &E, │ │ │ │ │ -const Eigen::Matrix< double, N, N > &P, const Vector &b) │ │ │ │ │ -Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = │ │ │ │ │ -F' * F - F' * E * P * ... │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:307 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_D │ │ │ │ │ -static const int D │ │ │ │ │ -Camera dimension. │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:448 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_M_a_t_r_i_x_Z_D │ │ │ │ │ -Eigen::Matrix< double, ZDim, D > MatrixZD │ │ │ │ │ -Definitions for blocks of F. │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_p_r_o_j_e_c_t_2 │ │ │ │ │ -ZVector project2(const POINT &point, boost::optional< FBlocks & > Fs=boost:: │ │ │ │ │ -none, boost::optional< Matrix & > E=boost::none) const │ │ │ │ │ -Project a point (possibly Unit3 at infinity), with derivatives Note that F is a │ │ │ │ │ -sparse block-diagonal... │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:108 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ +A CRTP helper class that implements Lie group methods Prerequisites: methods │ │ │ │ │ +operator*,... │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ +Both LieGroupTraits and Testable. │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:229 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ +either a fixed size o... │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_B_e_a_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn BearingRange.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_R_a_n_g_e │ │ │ │ │ +DDeeffiinniittiioonn BearingRange.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_H_a_s_B_e_a_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn BearingRange.h:180 │ │ │ │ │ +_g_t_s_a_m_:_:_H_a_s_R_a_n_g_e │ │ │ │ │ +DDeeffiinniittiioonn BearingRange.h:194 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2 │ │ │ │ │ +A 2D pose (Point2,Rot2) │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ +A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_I_d_e_n_t_i_t_y │ │ │ │ │ +static Pose3 Identity() │ │ │ │ │ +identity for group operation │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:106 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_P_o_s_e_3 │ │ │ │ │ +Pose3(const Pose3 &pose) │ │ │ │ │ +Copy constructor. │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_P_o_s_e_3 │ │ │ │ │ +Pose3() │ │ │ │ │ +Default constructor is origin. │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_w_e_d_g_e │ │ │ │ │ +static Matrix wedge(double wx, double wy, double wz, double vx, double vy, │ │ │ │ │ +double vz) │ │ │ │ │ +wedge for Pose3: │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:233 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_P_o_s_e_3 │ │ │ │ │ +Pose3(const Rot3 &R, const Point3 &t) │ │ │ │ │ +Construct from R,t. │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:63 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_P_o_s_e_3 │ │ │ │ │ +Pose3(const Matrix &T) │ │ │ │ │ +Constructor from 4*4 matrix. │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:71 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_z │ │ │ │ │ +double z() const │ │ │ │ │ +get z │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:302 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_r_o_t_a_t_i_o_n_I_n_t_e_r_v_a_l │ │ │ │ │ +static std::pair< size_t, size_t > rotationInterval() │ │ │ │ │ +Return the start and end indices (inclusive) of the rotation component of the │ │ │ │ │ +exponential map paramet... │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:375 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ +Pose3 operator*(const Pose3 &T) const │ │ │ │ │ +compose syntactic sugar │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:114 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_R_o_t_a_t_i_o_n │ │ │ │ │ +Rot3 Rotation │ │ │ │ │ +Pose Concept requirements. │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_y │ │ │ │ │ +double y() const │ │ │ │ │ +get y │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:297 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_t_r_a_n_s_l_a_t_i_o_n_I_n_t_e_r_v_a_l │ │ │ │ │ +static std::pair< size_t, size_t > translationInterval() │ │ │ │ │ +Return the start and end indices (inclusive) of the translation component of │ │ │ │ │ +the exponential map para... │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:366 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ +Point3 operator*(const Point3 &point) const │ │ │ │ │ +syntactic sugar for transformFrom │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:260 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_x │ │ │ │ │ +double x() const │ │ │ │ │ +get x │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:292 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:207 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ +Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor │ │ │ │ │ +symbol GTSAM_USE_QUATERNIO... │ │ │ │ │ +DDeeffiinniittiioonn Rot3.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_U_n_i_t_3 │ │ │ │ │ +Represents a 3D point on a unit sphere. │ │ │ │ │ +DDeeffiinniittiioonn Unit3.h:43 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_m_e_r_a_S_e_t_._h │ │ │ │ │ + * _P_o_s_e_3_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00302.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3_S2Stereo.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SphericalCamera.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,46 +96,50 @@ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
Cal3_S2Stereo.h File Reference
│ │ │ │ +
SphericalCamera.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

The most common 5DOF 3D->2D calibration + Stereo baseline. │ │ │ │ +

Calibrated camera with spherical projection. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::Cal3_S2Stereo
 The most common 5DOF 3D->2D calibration, stereo version. More...
class  gtsam::EmptyCal
 Empty calibration. More...
 
struct  gtsam::traits< Cal3_S2Stereo >
class  gtsam::SphericalCamera
 A spherical camera class that has a Pose3 and measures bearing vectors. More...
 
struct  gtsam::traits< const Cal3_S2Stereo >
struct  gtsam::traits< SphericalCamera >
 
struct  gtsam::traits< const SphericalCamera >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

The most common 5DOF 3D->2D calibration + Stereo baseline.

│ │ │ │ -
Author
Chris Beall
│ │ │ │ +

Calibrated camera with spherical projection.

│ │ │ │ +
Date
Aug 26, 2021
│ │ │ │ +
Author
Luca Carlone
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,29 +1,35 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Cal3_S2Stereo.h File Reference │ │ │ │ │ -The most common 5DOF 3D->2D calibration + Stereo baseline. _M_o_r_e_._._. │ │ │ │ │ +SphericalCamera.h File Reference │ │ │ │ │ +Calibrated camera with spherical projection. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o │ │ │ │ │ -  The most common 5DOF 3D->2D calibration, stereo version. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_E_m_p_t_y_C_a_l │ │ │ │ │ +  Empty calibration. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _C_a_l_3___S_2_S_t_e_r_e_o_ _> │ │ │ │ │ + class   _g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a │ │ │ │ │ +  A spherical camera class that has a _P_o_s_e_3 and measures bearing │ │ │ │ │ + vectors. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _C_a_l_3___S_2_S_t_e_r_e_o_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -The most common 5DOF 3D->2D calibration + Stereo baseline. │ │ │ │ │ +Calibrated camera with spherical projection. │ │ │ │ │ + Date │ │ │ │ │ + Aug 26, 2021 │ │ │ │ │ Author │ │ │ │ │ - Chris Beall │ │ │ │ │ + Luca Carlone │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3___S_2_S_t_e_r_e_o_._h │ │ │ │ │ + * _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00302.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a00302 = [ │ │ │ │ │ - ["gtsam::traits< Cal3_S2Stereo >", "a02960.html", null], │ │ │ │ │ - ["gtsam::traits< const Cal3_S2Stereo >", "a02964.html", null] │ │ │ │ │ + ["gtsam::traits< SphericalCamera >", "a03328.html", null], │ │ │ │ │ + ["gtsam::traits< const SphericalCamera >", "a03332.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00302_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3_S2Stereo.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SphericalCamera.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,163 +98,245 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Cal3_S2Stereo.h
│ │ │ │ +
SphericalCamera.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/geometry/Cal3_S2.h>
│ │ │ │ -
21#include <iosfwd>
│ │ │ │ -
22
│ │ │ │ -
23namespace gtsam {
│ │ │ │ -
24
│ │ │ │ -
│ │ │ │ -
30class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
│ │ │ │ -
31 private:
│ │ │ │ -
32 double b_ = 1.0f;
│ │ │ │ -
33
│ │ │ │ -
34 public:
│ │ │ │ -
35 enum { dimension = 6 };
│ │ │ │ -
36
│ │ │ │ -
38 using shared_ptr = boost::shared_ptr<Cal3_S2Stereo>;
│ │ │ │ -
39
│ │ │ │ -
42
│ │ │ │ -
44 Cal3_S2Stereo() = default;
│ │ │ │ -
45
│ │ │ │ -
│ │ │ │ -
47 Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b)
│ │ │ │ -
48 : Cal3_S2(fx, fy, s, u0, v0), b_(b) {}
│ │ │ │ -
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/base/Manifold.h>
│ │ │ │ +
22#include <gtsam/base/ThreadsafeException.h>
│ │ │ │ +
23#include <gtsam/base/concepts.h>
│ │ │ │ +
24#include <gtsam/dllexport.h>
│ │ │ │ +
25#include <gtsam/geometry/BearingRange.h>
│ │ │ │ +
26#include <gtsam/geometry/Pose3.h>
│ │ │ │ +
27#include <gtsam/geometry/Unit3.h>
│ │ │ │ +
28
│ │ │ │ +
29#include <boost/serialization/nvp.hpp>
│ │ │ │ +
30
│ │ │ │ +
31namespace gtsam {
│ │ │ │ +
32
│ │ │ │ +
│ │ │ │ +
40class GTSAM_EXPORT EmptyCal {
│ │ │ │ +
41 public:
│ │ │ │ +
42 enum { dimension = 0 };
│ │ │ │ +
43 EmptyCal() {}
│ │ │ │ +
44 virtual ~EmptyCal() = default;
│ │ │ │ +
45 using shared_ptr = boost::shared_ptr<EmptyCal>;
│ │ │ │ +
46
│ │ │ │ +
48 inline static size_t Dim() { return dimension; }
│ │ │ │
49
│ │ │ │ -
│ │ │ │ -
51 Cal3_S2Stereo(const Vector6& d)
│ │ │ │ -
52 : Cal3_S2(d(0), d(1), d(2), d(3), d(4)), b_(d(5)) {}
│ │ │ │ -
│ │ │ │ +
50 void print(const std::string& s) const {
│ │ │ │ +
51 std::cout << "empty calibration: " << s << std::endl;
│ │ │ │ +
52 }
│ │ │ │
53
│ │ │ │ -
│ │ │ │ -
55 Cal3_S2Stereo(double fov, int w, int h, double b)
│ │ │ │ -
56 : Cal3_S2(fov, w, h), b_(b) {}
│ │ │ │ -
│ │ │ │ -
57
│ │ │ │ -
59
│ │ │ │ -
67 Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none,
│ │ │ │ -
68 OptionalJacobian<2, 2> Dp = boost::none) const;
│ │ │ │ -
69
│ │ │ │ -
77 Point2 calibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none,
│ │ │ │ -
78 OptionalJacobian<2, 2> Dp = boost::none) const;
│ │ │ │ -
79
│ │ │ │ -
85 Vector3 calibrate(const Vector3& p) const { return Cal3_S2::calibrate(p); }
│ │ │ │ -
86
│ │ │ │ -
89
│ │ │ │ -
91 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
│ │ │ │ -
92 const Cal3_S2Stereo& cal);
│ │ │ │ -
93
│ │ │ │ -
95 void print(const std::string& s = "") const override;
│ │ │ │ -
96
│ │ │ │ -
98 bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const;
│ │ │ │ -
99
│ │ │ │ -
103
│ │ │ │ -
105 const Cal3_S2& calibration() const { return *this; }
│ │ │ │ -
106
│ │ │ │ -
108 Matrix3 K() const override { return Cal3_S2::K(); }
│ │ │ │ -
109
│ │ │ │ -
111 inline double baseline() const { return b_; }
│ │ │ │ -
112
│ │ │ │ -
│ │ │ │ -
114 Vector6 vector() const {
│ │ │ │ -
115 Vector6 v;
│ │ │ │ -
116 v << Cal3_S2::vector(), b_;
│ │ │ │ -
117 return v;
│ │ │ │ -
118 }
│ │ │ │ -
│ │ │ │ -
119
│ │ │ │ +
54 private:
│ │ │ │ +
56 friend class boost::serialization::access;
│ │ │ │ +
57 template <class Archive>
│ │ │ │ +
58 void serialize(Archive& ar, const unsigned int /*version*/) {
│ │ │ │ +
59 ar& boost::serialization::make_nvp(
│ │ │ │ +
60 "EmptyCal", boost::serialization::base_object<EmptyCal>(*this));
│ │ │ │ +
61 }
│ │ │ │ +
62};
│ │ │ │ +
│ │ │ │ +
63
│ │ │ │ +
│ │ │ │ +
70class GTSAM_EXPORT SphericalCamera {
│ │ │ │ +
71 public:
│ │ │ │ +
72 enum { dimension = 6 };
│ │ │ │ +
73
│ │ │ │ +
74 using Measurement = Unit3;
│ │ │ │ +
75 using MeasurementVector = std::vector<Unit3>;
│ │ │ │ +
76 using CalibrationType = EmptyCal;
│ │ │ │ +
77
│ │ │ │ +
78 private:
│ │ │ │ +
79 Pose3 pose_;
│ │ │ │ +
80
│ │ │ │ +
81 protected:
│ │ │ │ +
82 EmptyCal::shared_ptr emptyCal_;
│ │ │ │ +
83
│ │ │ │ +
84 public:
│ │ │ │ +
87
│ │ │ │ +
│ │ │ │ +
89 SphericalCamera()
│ │ │ │ +
90 : pose_(Pose3()), emptyCal_(boost::make_shared<EmptyCal>()) {}
│ │ │ │ +
│ │ │ │ +
91
│ │ │ │ +
│ │ │ │ +
93 explicit SphericalCamera(const Pose3& pose)
│ │ │ │ +
94 : pose_(pose), emptyCal_(boost::make_shared<EmptyCal>()) {}
│ │ │ │ +
│ │ │ │ +
95
│ │ │ │ +
│ │ │ │ +
97 explicit SphericalCamera(const Pose3& pose,
│ │ │ │ +
98 const EmptyCal::shared_ptr& cal)
│ │ │ │ +
99 : pose_(pose), emptyCal_(cal) {}
│ │ │ │ +
│ │ │ │ +
100
│ │ │ │ +
104 explicit SphericalCamera(const Vector& v) : pose_(Pose3::Expmap(v)) {}
│ │ │ │ +
105
│ │ │ │ +
107 virtual ~SphericalCamera() = default;
│ │ │ │ +
108
│ │ │ │ +
│ │ │ │ +
110 const EmptyCal::shared_ptr& sharedCalibration() const {
│ │ │ │ +
111 return emptyCal_;
│ │ │ │ +
112 }
│ │ │ │ +
│ │ │ │ +
113
│ │ │ │ +
115 const EmptyCal& calibration() const { return *emptyCal_; }
│ │ │ │ +
116
│ │ │ │ +
120
│ │ │ │ +
122 bool equals(const SphericalCamera& camera, double tol = 1e-9) const;
│ │ │ │
123
│ │ │ │ -
125 inline size_t dim() const override { return Dim(); }
│ │ │ │ +
125 virtual void print(const std::string& s = "SphericalCamera") const;
│ │ │ │
126
│ │ │ │ -
128 inline static size_t Dim() { return dimension; }
│ │ │ │ -
129
│ │ │ │ -
│ │ │ │ -
131 inline Cal3_S2Stereo retract(const Vector& d) const {
│ │ │ │ -
132 return Cal3_S2Stereo(fx() + d(0), fy() + d(1), skew() + d(2), px() + d(3),
│ │ │ │ -
133 py() + d(4), b_ + d(5));
│ │ │ │ -
134 }
│ │ │ │ -
│ │ │ │ -
135
│ │ │ │ -
│ │ │ │ -
137 Vector6 localCoordinates(const Cal3_S2Stereo& T2) const {
│ │ │ │ -
138 return T2.vector() - vector();
│ │ │ │ -
139 }
│ │ │ │ -
│ │ │ │ -
140
│ │ │ │ -
144
│ │ │ │ -
145 private:
│ │ │ │ -
147 friend class boost::serialization::access;
│ │ │ │ -
148 template <class Archive>
│ │ │ │ -
149 void serialize(Archive& ar, const unsigned int /*version*/) {
│ │ │ │ -
150 ar& boost::serialization::make_nvp(
│ │ │ │ -
151 "Cal3_S2", boost::serialization::base_object<Cal3_S2>(*this));
│ │ │ │ -
152 ar& BOOST_SERIALIZATION_NVP(b_);
│ │ │ │ -
153 }
│ │ │ │ -
155};
│ │ │ │ -
│ │ │ │ -
156
│ │ │ │ -
157// Define GTSAM traits
│ │ │ │ -
158template <>
│ │ │ │ -
159struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {};
│ │ │ │ -
160
│ │ │ │ -
161template <>
│ │ │ │ -
│ │ │ │ -
162struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
│ │ │ │ -
163};
│ │ │ │ -
│ │ │ │ -
164
│ │ │ │ -
165} // \ namespace gtsam
│ │ │ │ -
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
│ │ │ │ +
130
│ │ │ │ +
132 const Pose3& pose() const { return pose_; }
│ │ │ │ +
133
│ │ │ │ +
135 const Rot3& rotation() const { return pose_.rotation(); }
│ │ │ │ +
136
│ │ │ │ +
138 const Point3& translation() const { return pose_.translation(); }
│ │ │ │ +
139
│ │ │ │ +
140 // /// return pose, with derivative
│ │ │ │ +
141 // const Pose3& getPose(OptionalJacobian<6, 6> H) const;
│ │ │ │ +
142
│ │ │ │ +
146
│ │ │ │ +
148 std::pair<Unit3, bool> projectSafe(const Point3& pw) const;
│ │ │ │ +
149
│ │ │ │ +
155 Unit3 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
│ │ │ │ +
156 OptionalJacobian<2, 3> Dpoint = boost::none) const;
│ │ │ │ +
157
│ │ │ │ +
163 Unit3 project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose = boost::none,
│ │ │ │ +
164 OptionalJacobian<2, 2> Dpoint = boost::none) const;
│ │ │ │ +
165
│ │ │ │ +
167 Point3 backproject(const Unit3& p, const double depth) const;
│ │ │ │ +
168
│ │ │ │ +
170 Unit3 backprojectPointAtInfinity(const Unit3& p) const;
│ │ │ │ +
171
│ │ │ │ +
177 Unit3 project(const Point3& point, OptionalJacobian<2, 6> Dpose = boost::none,
│ │ │ │ +
178 OptionalJacobian<2, 3> Dpoint = boost::none) const;
│ │ │ │ +
179
│ │ │ │ +
184 Vector2 reprojectionError(const Point3& point, const Unit3& measured,
│ │ │ │ +
185 OptionalJacobian<2, 6> Dpose = boost::none,
│ │ │ │ +
186 OptionalJacobian<2, 3> Dpoint = boost::none) const;
│ │ │ │ +
188
│ │ │ │ +
│ │ │ │ +
190 SphericalCamera retract(const Vector6& d) const {
│ │ │ │ +
191 return SphericalCamera(pose().retract(d));
│ │ │ │ +
192 }
│ │ │ │ +
│ │ │ │ +
193
│ │ │ │ +
│ │ │ │ +
195 Vector6 localCoordinates(const SphericalCamera& p) const {
│ │ │ │ +
196 return pose().localCoordinates(p.pose());
│ │ │ │ +
197 }
│ │ │ │ +
│ │ │ │ +
198
│ │ │ │ +
│ │ │ │ +
200 static SphericalCamera Identity() {
│ │ │ │ +
201 return SphericalCamera(
│ │ │ │ +
202 Pose3::Identity()); // assumes that the default constructor is valid
│ │ │ │ +
203 }
│ │ │ │ +
│ │ │ │ +
204
│ │ │ │ +
│ │ │ │ +
206 Matrix34 cameraProjectionMatrix() const {
│ │ │ │ +
207 return Matrix34(pose_.inverse().matrix().block(0, 0, 3, 4));
│ │ │ │ +
208 }
│ │ │ │ +
│ │ │ │ +
209
│ │ │ │ +
│ │ │ │ +
211 Vector defaultErrorWhenTriangulatingBehindCamera() const {
│ │ │ │ +
212 return Eigen::Matrix<double, traits<Point2>::dimension, 1>::Constant(0.0);
│ │ │ │ +
213 }
│ │ │ │ +
│ │ │ │ +
214
│ │ │ │ +
216 size_t dim() const { return 6; }
│ │ │ │ +
217
│ │ │ │ +
219 static size_t Dim() { return 6; }
│ │ │ │ +
220
│ │ │ │ +
221 private:
│ │ │ │ +
223 friend class boost::serialization::access;
│ │ │ │ +
224 template <class Archive>
│ │ │ │ +
225 void serialize(Archive& ar, const unsigned int /*version*/) {
│ │ │ │ +
226 ar& BOOST_SERIALIZATION_NVP(pose_);
│ │ │ │ +
227 }
│ │ │ │ +
228
│ │ │ │ +
229 public:
│ │ │ │ +
230 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
231};
│ │ │ │ +
│ │ │ │ +
232// end of class SphericalCamera
│ │ │ │ +
233
│ │ │ │ +
234template <>
│ │ │ │ +
235struct traits<SphericalCamera> : public internal::LieGroup<Pose3> {};
│ │ │ │ +
236
│ │ │ │ +
237template <>
│ │ │ │ +
238struct traits<const SphericalCamera> : public internal::LieGroup<Pose3> {};
│ │ │ │ +
239
│ │ │ │ +
240} // namespace gtsam
│ │ │ │ +
ThreadsafeException.h
Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
│ │ │ │ +
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ +
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ +
Pose3.h
3D Pose
│ │ │ │ +
BearingRange.h
Bearing-Range product.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │ +
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ +
gtsam::make_shared
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost::shared_ptr< T > > make_shared(Args &&... args)
Add our own make_shared as a layer of wrapping on boost::make_shared This solves the problem with the...
Definition make_shared.h:57
│ │ │ │ +
gtsam::project
Point2_ project(const Point3_ &p_cam)
Expression version of PinholeBase::Project.
Definition expressions.h:131
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
│ │ │ │ +
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition Lie.h:229
│ │ │ │
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition Cal3_S2.h:34
│ │ │ │ -
gtsam::Cal3_S2Stereo
The most common 5DOF 3D->2D calibration, stereo version.
Definition Cal3_S2Stereo.h:30
│ │ │ │ -
gtsam::Cal3_S2Stereo::vector
Vector6 vector() const
vectorized form (column-wise)
Definition Cal3_S2Stereo.h:114
│ │ │ │ -
gtsam::Cal3_S2Stereo::Cal3_S2Stereo
Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b)
constructor from doubles
Definition Cal3_S2Stereo.h:47
│ │ │ │ -
gtsam::Cal3_S2Stereo::localCoordinates
Vector6 localCoordinates(const Cal3_S2Stereo &T2) const
Unretraction for the calibration.
Definition Cal3_S2Stereo.h:137
│ │ │ │ -
gtsam::Cal3_S2Stereo::calibration
const Cal3_S2 & calibration() const
return calibration, same for left and right
Definition Cal3_S2Stereo.h:105
│ │ │ │ -
gtsam::Cal3_S2Stereo::dim
size_t dim() const override
return DOF, dimensionality of tangent space
Definition Cal3_S2Stereo.h:125
│ │ │ │ -
gtsam::Cal3_S2Stereo::calibrate
Vector3 calibrate(const Vector3 &p) const
Convert homogeneous image coordinates to intrinsic coordinates.
Definition Cal3_S2Stereo.h:85
│ │ │ │ -
gtsam::Cal3_S2Stereo::retract
Cal3_S2Stereo retract(const Vector &d) const
Given 6-dim tangent vector, create new calibration.
Definition Cal3_S2Stereo.h:131
│ │ │ │ -
gtsam::Cal3_S2Stereo::K
Matrix3 K() const override
return calibration matrix K, same for left and right
Definition Cal3_S2Stereo.h:108
│ │ │ │ -
gtsam::Cal3_S2Stereo::Dim
static size_t Dim()
return DOF, dimensionality of tangent space
Definition Cal3_S2Stereo.h:128
│ │ │ │ -
gtsam::Cal3_S2Stereo::Cal3_S2Stereo
Cal3_S2Stereo(double fov, int w, int h, double b)
easy constructor; field-of-view in degrees, assumes zero skew
Definition Cal3_S2Stereo.h:55
│ │ │ │ -
gtsam::Cal3_S2Stereo::baseline
double baseline() const
return baseline
Definition Cal3_S2Stereo.h:111
│ │ │ │ -
gtsam::Cal3_S2Stereo::Cal3_S2Stereo
Cal3_S2Stereo(const Vector6 &d)
constructor from vector
Definition Cal3_S2Stereo.h:51
│ │ │ │ -
gtsam::Cal3_S2Stereo::Cal3_S2Stereo
Cal3_S2Stereo()=default
default calibration leaves coordinates unchanged
│ │ │ │ +
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ +
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get translation
Definition Pose3.cpp:308
│ │ │ │ +
gtsam::Pose3::matrix
Matrix4 matrix() const
convert to 4*4 matrix
Definition Pose3.cpp:323
│ │ │ │ +
gtsam::Pose3::inverse
Pose3 inverse() const
inverse transformation with derivatives
Definition Pose3.cpp:49
│ │ │ │ +
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get rotation
Definition Pose3.cpp:315
│ │ │ │ +
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
│ │ │ │ +
gtsam::EmptyCal
Empty calibration.
Definition SphericalCamera.h:40
│ │ │ │ +
gtsam::EmptyCal::Dim
static size_t Dim()
return DOF, dimensionality of tangent space
Definition SphericalCamera.h:48
│ │ │ │ +
gtsam::SphericalCamera
A spherical camera class that has a Pose3 and measures bearing vectors.
Definition SphericalCamera.h:70
│ │ │ │ +
gtsam::SphericalCamera::sharedCalibration
const EmptyCal::shared_ptr & sharedCalibration() const
return shared pointer to calibration
Definition SphericalCamera.h:110
│ │ │ │ +
gtsam::SphericalCamera::Dim
static size_t Dim()
Definition SphericalCamera.h:219
│ │ │ │ +
gtsam::SphericalCamera::SphericalCamera
SphericalCamera(const Pose3 &pose)
Constructor with pose.
Definition SphericalCamera.h:93
│ │ │ │ +
gtsam::SphericalCamera::~SphericalCamera
virtual ~SphericalCamera()=default
Default destructor.
│ │ │ │ +
gtsam::SphericalCamera::cameraProjectionMatrix
Matrix34 cameraProjectionMatrix() const
for Linear Triangulation
Definition SphericalCamera.h:206
│ │ │ │ +
gtsam::SphericalCamera::localCoordinates
Vector6 localCoordinates(const SphericalCamera &p) const
return canonical coordinate
Definition SphericalCamera.h:195
│ │ │ │ +
gtsam::SphericalCamera::rotation
const Rot3 & rotation() const
get rotation
Definition SphericalCamera.h:135
│ │ │ │ +
gtsam::SphericalCamera::pose
const Pose3 & pose() const
return pose, constant version
Definition SphericalCamera.h:132
│ │ │ │ +
gtsam::SphericalCamera::translation
const Point3 & translation() const
get translation
Definition SphericalCamera.h:138
│ │ │ │ +
gtsam::SphericalCamera::retract
SphericalCamera retract(const Vector6 &d) const
move a cameras according to d
Definition SphericalCamera.h:190
│ │ │ │ +
gtsam::SphericalCamera::calibration
const EmptyCal & calibration() const
return calibration
Definition SphericalCamera.h:115
│ │ │ │ +
gtsam::SphericalCamera::dim
size_t dim() const
Definition SphericalCamera.h:216
│ │ │ │ +
gtsam::SphericalCamera::Identity
static SphericalCamera Identity()
for Canonical
Definition SphericalCamera.h:200
│ │ │ │ +
gtsam::SphericalCamera::SphericalCamera
SphericalCamera()
Default constructor.
Definition SphericalCamera.h:89
│ │ │ │ +
gtsam::SphericalCamera::defaultErrorWhenTriangulatingBehindCamera
Vector defaultErrorWhenTriangulatingBehindCamera() const
for Nonlinear Triangulation
Definition SphericalCamera.h:211
│ │ │ │ +
gtsam::SphericalCamera::SphericalCamera
SphericalCamera(const Pose3 &pose, const EmptyCal::shared_ptr &cal)
Constructor with empty intrinsics (needed for smart factors)
Definition SphericalCamera.h:97
│ │ │ │ +
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,204 +1,323 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Cal3_S2Stereo.h │ │ │ │ │ +SphericalCamera.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3___S_2_._h> │ │ │ │ │ -21#include │ │ │ │ │ -22 │ │ │ │ │ -23namespace _g_t_s_a_m { │ │ │ │ │ -24 │ │ │ │ │ -_3_0class GTSAM_EXPORT _C_a_l_3___S_2_S_t_e_r_e_o : public _C_a_l_3___S_2 { │ │ │ │ │ -31 private: │ │ │ │ │ -32 double b_ = 1.0f; │ │ │ │ │ -33 │ │ │ │ │ -34 public: │ │ │ │ │ -35 enum { dimension = 6 }; │ │ │ │ │ -36 │ │ │ │ │ -38 using shared_ptr = boost::shared_ptr; │ │ │ │ │ -39 │ │ │ │ │ -42 │ │ │ │ │ -_4_4 _C_a_l_3___S_2_S_t_e_r_e_o() = default; │ │ │ │ │ -45 │ │ │ │ │ -_4_7 _C_a_l_3___S_2_S_t_e_r_e_o(double fx, double fy, double s, double u0, double v0, double │ │ │ │ │ -b) │ │ │ │ │ -48 : _C_a_l_3___S_2(fx, fy, s, u0, v0), b_(b) {} │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_._h> │ │ │ │ │ +23#include │ │ │ │ │ +24#include │ │ │ │ │ +25#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_B_e_a_r_i_n_g_R_a_n_g_e_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_3_._h> │ │ │ │ │ +27#include │ │ │ │ │ +28 │ │ │ │ │ +29#include │ │ │ │ │ +30 │ │ │ │ │ +31namespace _g_t_s_a_m { │ │ │ │ │ +32 │ │ │ │ │ +_4_0class GTSAM_EXPORT _E_m_p_t_y_C_a_l { │ │ │ │ │ +41 public: │ │ │ │ │ +42 enum { dimension = 0 }; │ │ │ │ │ +43 _E_m_p_t_y_C_a_l() {} │ │ │ │ │ +44 virtual _~_E_m_p_t_y_C_a_l() = default; │ │ │ │ │ +45 using shared_ptr = boost::shared_ptr; │ │ │ │ │ +46 │ │ │ │ │ +_4_8 inline static size_t _D_i_m() { return dimension; } │ │ │ │ │ 49 │ │ │ │ │ -_5_1 _C_a_l_3___S_2_S_t_e_r_e_o(const Vector6& d) │ │ │ │ │ -52 : _C_a_l_3___S_2(d(0), d(1), d(2), d(3), d(4)), b_(d(5)) {} │ │ │ │ │ +50 void _p_r_i_n_t(const std::string& s) const { │ │ │ │ │ +51 std::cout << "empty calibration: " << s << std::endl; │ │ │ │ │ +52 } │ │ │ │ │ 53 │ │ │ │ │ -_5_5 _C_a_l_3___S_2_S_t_e_r_e_o(double fov, int w, int h, double b) │ │ │ │ │ -56 : _C_a_l_3___S_2(fov, w, h), b_(b) {} │ │ │ │ │ -57 │ │ │ │ │ -59 │ │ │ │ │ -67 _P_o_i_n_t_2 uncalibrate(const _P_o_i_n_t_2& p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> Dcal = boost:: │ │ │ │ │ -none, │ │ │ │ │ -68 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dp = boost::none) const; │ │ │ │ │ -69 │ │ │ │ │ -77 _P_o_i_n_t_2 calibrate(const _P_o_i_n_t_2& p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> Dcal = boost::none, │ │ │ │ │ -78 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dp = boost::none) const; │ │ │ │ │ -79 │ │ │ │ │ -_8_5 Vector3 _c_a_l_i_b_r_a_t_e(const Vector3& p) const { return Cal3_S2::calibrate(p); } │ │ │ │ │ -86 │ │ │ │ │ -89 │ │ │ │ │ -91 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, │ │ │ │ │ -92 const _C_a_l_3___S_2_S_t_e_r_e_o& cal); │ │ │ │ │ -93 │ │ │ │ │ -95 void _p_r_i_n_t(const std::string& s = "") const override; │ │ │ │ │ -96 │ │ │ │ │ -98 bool _e_q_u_a_l_s(const _C_a_l_3___S_2_S_t_e_r_e_o& other, double tol = 10e-9) const; │ │ │ │ │ -99 │ │ │ │ │ -103 │ │ │ │ │ -_1_0_5 const _C_a_l_3___S_2& _c_a_l_i_b_r_a_t_i_o_n() const { return *this; } │ │ │ │ │ -106 │ │ │ │ │ -_1_0_8 Matrix3 _K() const override { return Cal3_S2::K(); } │ │ │ │ │ -109 │ │ │ │ │ -_1_1_1 inline double _b_a_s_e_l_i_n_e() const { return b_; } │ │ │ │ │ -112 │ │ │ │ │ -_1_1_4 Vector6 _v_e_c_t_o_r() const { │ │ │ │ │ -115 Vector6 v; │ │ │ │ │ -116 v << Cal3_S2::vector(), b_; │ │ │ │ │ -117 return v; │ │ │ │ │ -118 } │ │ │ │ │ -119 │ │ │ │ │ +54 private: │ │ │ │ │ +_5_6 friend class boost::serialization::access; │ │ │ │ │ +57 template │ │ │ │ │ +58 void serialize(Archive& ar, const unsigned int /*version*/) { │ │ │ │ │ +59 ar& boost::serialization::make_nvp( │ │ │ │ │ +60 "EmptyCal", boost::serialization::base_object(*this)); │ │ │ │ │ +61 } │ │ │ │ │ +62}; │ │ │ │ │ +63 │ │ │ │ │ +_7_0class GTSAM_EXPORT _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a { │ │ │ │ │ +71 public: │ │ │ │ │ +72 enum { dimension = 6 }; │ │ │ │ │ +73 │ │ │ │ │ +74 using _M_e_a_s_u_r_e_m_e_n_t = _U_n_i_t_3; │ │ │ │ │ +75 using MeasurementVector = std::vector; │ │ │ │ │ +76 using _C_a_l_i_b_r_a_t_i_o_n_T_y_p_e = _E_m_p_t_y_C_a_l; │ │ │ │ │ +77 │ │ │ │ │ +78 private: │ │ │ │ │ +79 _P_o_s_e_3 pose_; │ │ │ │ │ +80 │ │ │ │ │ +81 protected: │ │ │ │ │ +82 EmptyCal::shared_ptr emptyCal_; │ │ │ │ │ +83 │ │ │ │ │ +84 public: │ │ │ │ │ +87 │ │ │ │ │ +_8_9 _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a() │ │ │ │ │ +90 : pose_(_P_o_s_e_3()), emptyCal_(boost::_m_a_k_e___s_h_a_r_e_d<_E_m_p_t_y_C_a_l>()) {} │ │ │ │ │ +91 │ │ │ │ │ +_9_3 explicit _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a(const _P_o_s_e_3& pose) │ │ │ │ │ +94 : pose_(pose), emptyCal_(boost::_m_a_k_e___s_h_a_r_e_d<_E_m_p_t_y_C_a_l>()) {} │ │ │ │ │ +95 │ │ │ │ │ +_9_7 explicit _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a(const _P_o_s_e_3& pose, │ │ │ │ │ +98 const EmptyCal::shared_ptr& cal) │ │ │ │ │ +99 : pose_(pose), emptyCal_(cal) {} │ │ │ │ │ +100 │ │ │ │ │ +104 explicit _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a(const Vector& v) : pose_(_P_o_s_e_3::Expmap(v)) {} │ │ │ │ │ +105 │ │ │ │ │ +_1_0_7 virtual _~_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a() = default; │ │ │ │ │ +108 │ │ │ │ │ +_1_1_0 const EmptyCal::shared_ptr& _s_h_a_r_e_d_C_a_l_i_b_r_a_t_i_o_n() const { │ │ │ │ │ +111 return emptyCal_; │ │ │ │ │ +112 } │ │ │ │ │ +113 │ │ │ │ │ +_1_1_5 const _E_m_p_t_y_C_a_l& _c_a_l_i_b_r_a_t_i_o_n() const { return *emptyCal_; } │ │ │ │ │ +116 │ │ │ │ │ +120 │ │ │ │ │ +122 bool _e_q_u_a_l_s(const _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a& camera, double tol = 1e-9) const; │ │ │ │ │ 123 │ │ │ │ │ -_1_2_5 inline size_t _d_i_m() const override { return Dim(); } │ │ │ │ │ +125 virtual void _p_r_i_n_t(const std::string& s = "SphericalCamera") const; │ │ │ │ │ 126 │ │ │ │ │ -_1_2_8 inline static size_t _D_i_m() { return dimension; } │ │ │ │ │ -129 │ │ │ │ │ -_1_3_1 inline _C_a_l_3___S_2_S_t_e_r_e_o _r_e_t_r_a_c_t(const Vector& d) const { │ │ │ │ │ -132 return _C_a_l_3___S_2_S_t_e_r_e_o(fx() + d(0), fy() + d(1), skew() + d(2), px() + d(3), │ │ │ │ │ -133 py() + d(4), b_ + d(5)); │ │ │ │ │ -134 } │ │ │ │ │ -135 │ │ │ │ │ -_1_3_7 Vector6 _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(const _C_a_l_3___S_2_S_t_e_r_e_o& T2) const { │ │ │ │ │ -138 return T2._v_e_c_t_o_r() - vector(); │ │ │ │ │ -139 } │ │ │ │ │ -140 │ │ │ │ │ -144 │ │ │ │ │ -145 private: │ │ │ │ │ -_1_4_7 friend class boost::serialization::access; │ │ │ │ │ -148 template │ │ │ │ │ -149 void serialize(Archive& ar, const unsigned int /*version*/) { │ │ │ │ │ -150 ar& boost::serialization::make_nvp( │ │ │ │ │ -151 "Cal3_S2", boost::serialization::base_object(*this)); │ │ │ │ │ -152 ar& BOOST_SERIALIZATION_NVP(b_); │ │ │ │ │ -153 } │ │ │ │ │ -155}; │ │ │ │ │ -156 │ │ │ │ │ -157// Define GTSAM traits │ │ │ │ │ -158template <> │ │ │ │ │ -_1_5_9struct _t_r_a_i_t_s<_C_a_l_3___S_2_S_t_e_r_e_o> : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d {}; │ │ │ │ │ -160 │ │ │ │ │ -161template <> │ │ │ │ │ -_1_6_2struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_: │ │ │ │ │ -_M_a_n_i_f_o_l_d { │ │ │ │ │ -163}; │ │ │ │ │ -164 │ │ │ │ │ -165} // \ namespace gtsam │ │ │ │ │ -_C_a_l_3___S_2_._h │ │ │ │ │ -The most common 5DOF 3D->2D calibration. │ │ │ │ │ +130 │ │ │ │ │ +_1_3_2 const _P_o_s_e_3& _p_o_s_e() const { return pose_; } │ │ │ │ │ +133 │ │ │ │ │ +_1_3_5 const _R_o_t_3& _r_o_t_a_t_i_o_n() const { return pose_._r_o_t_a_t_i_o_n(); } │ │ │ │ │ +136 │ │ │ │ │ +_1_3_8 const _P_o_i_n_t_3& _t_r_a_n_s_l_a_t_i_o_n() const { return pose_._t_r_a_n_s_l_a_t_i_o_n(); } │ │ │ │ │ +139 │ │ │ │ │ +140 // /// return pose, with derivative │ │ │ │ │ +141 // const Pose3& getPose(OptionalJacobian<6, 6> H) const; │ │ │ │ │ +142 │ │ │ │ │ +146 │ │ │ │ │ +148 std::pair projectSafe(const _P_o_i_n_t_3& pw) const; │ │ │ │ │ +149 │ │ │ │ │ +155 _U_n_i_t_3 project2(const _P_o_i_n_t_3& pw, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> Dpose = boost:: │ │ │ │ │ +none, │ │ │ │ │ +156 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> Dpoint = boost::none) const; │ │ │ │ │ +157 │ │ │ │ │ +163 _U_n_i_t_3 project2(const _U_n_i_t_3& pwu, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> Dpose = boost:: │ │ │ │ │ +none, │ │ │ │ │ +164 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dpoint = boost::none) const; │ │ │ │ │ +165 │ │ │ │ │ +167 _P_o_i_n_t_3 backproject(const _U_n_i_t_3& p, const double depth) const; │ │ │ │ │ +168 │ │ │ │ │ +170 _U_n_i_t_3 backprojectPointAtInfinity(const _U_n_i_t_3& p) const; │ │ │ │ │ +171 │ │ │ │ │ +177 _U_n_i_t_3 _p_r_o_j_e_c_t(const _P_o_i_n_t_3& point, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> Dpose = boost:: │ │ │ │ │ +none, │ │ │ │ │ +178 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> Dpoint = boost::none) const; │ │ │ │ │ +179 │ │ │ │ │ +184 Vector2 reprojectionError(const _P_o_i_n_t_3& point, const _U_n_i_t_3& measured, │ │ │ │ │ +185 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> Dpose = boost::none, │ │ │ │ │ +186 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> Dpoint = boost::none) const; │ │ │ │ │ +188 │ │ │ │ │ +_1_9_0 _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a _r_e_t_r_a_c_t(const Vector6& d) const { │ │ │ │ │ +191 return _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a(pose().retract(d)); │ │ │ │ │ +192 } │ │ │ │ │ +193 │ │ │ │ │ +_1_9_5 Vector6 _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(const _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a& p) const { │ │ │ │ │ +196 return pose().localCoordinates(p._p_o_s_e()); │ │ │ │ │ +197 } │ │ │ │ │ +198 │ │ │ │ │ +_2_0_0 static _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a _I_d_e_n_t_i_t_y() { │ │ │ │ │ +201 return _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a( │ │ │ │ │ +202 Pose3::Identity()); // assumes that the default constructor is valid │ │ │ │ │ +203 } │ │ │ │ │ +204 │ │ │ │ │ +_2_0_6 Matrix34 _c_a_m_e_r_a_P_r_o_j_e_c_t_i_o_n_M_a_t_r_i_x() const { │ │ │ │ │ +207 return Matrix34(pose_._i_n_v_e_r_s_e()._m_a_t_r_i_x().block(0, 0, 3, 4)); │ │ │ │ │ +208 } │ │ │ │ │ +209 │ │ │ │ │ +_2_1_1 Vector _d_e_f_a_u_l_t_E_r_r_o_r_W_h_e_n_T_r_i_a_n_g_u_l_a_t_i_n_g_B_e_h_i_n_d_C_a_m_e_r_a() const { │ │ │ │ │ +212 return Eigen::Matrix::dimension, 1>::Constant(0.0); │ │ │ │ │ +213 } │ │ │ │ │ +214 │ │ │ │ │ +_2_1_6 size_t _d_i_m() const { return 6; } │ │ │ │ │ +217 │ │ │ │ │ +_2_1_9 static size_t _D_i_m() { return 6; } │ │ │ │ │ +220 │ │ │ │ │ +221 private: │ │ │ │ │ +_2_2_3 friend class boost::serialization::access; │ │ │ │ │ +224 template │ │ │ │ │ +225 void serialize(Archive& ar, const unsigned int /*version*/) { │ │ │ │ │ +226 ar& BOOST_SERIALIZATION_NVP(pose_); │ │ │ │ │ +227 } │ │ │ │ │ +228 │ │ │ │ │ +229 public: │ │ │ │ │ +230 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +231}; │ │ │ │ │ +232// end of class SphericalCamera │ │ │ │ │ +233 │ │ │ │ │ +234template <> │ │ │ │ │ +_2_3_5struct _t_r_a_i_t_s<_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a> : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ +236 │ │ │ │ │ +237template <> │ │ │ │ │ +_2_3_8struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ +239 │ │ │ │ │ +240} // namespace gtsam │ │ │ │ │ +_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_._h │ │ │ │ │ +Base exception type that uses tbb_allocator if GTSAM is compiled with TBB. │ │ │ │ │ +_M_a_n_i_f_o_l_d_._h │ │ │ │ │ +Base class and basic functions for Manifold types. │ │ │ │ │ +_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ +This marks a GTSAM object to require alignment. │ │ │ │ │ +DDeeffiinniittiioonn types.h:308 │ │ │ │ │ +_P_o_s_e_3_._h │ │ │ │ │ +3D Pose │ │ │ │ │ +_B_e_a_r_i_n_g_R_a_n_g_e_._h │ │ │ │ │ +Bearing-Range product. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ print without optional string, must specify cout yourself │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ -Vector2 Point2 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ +Vector3 Point3 │ │ │ │ │ As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point2 to Vector2... │ │ │ │ │ -DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ +typedef Point3 to Vector3... │ │ │ │ │ +DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_m_a_k_e___s_h_a_r_e_d │ │ │ │ │ +gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost:: │ │ │ │ │ +shared_ptr< T > > make_shared(Args &&... args) │ │ │ │ │ +Add our own make_shared as a layer of wrapping on boost::make_shared This │ │ │ │ │ +solves the problem with the... │ │ │ │ │ +DDeeffiinniittiioonn make_shared.h:57 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_o_j_e_c_t │ │ │ │ │ +Point2_ project(const Point3_ &p_cam) │ │ │ │ │ +Expression version of PinholeBase::Project. │ │ │ │ │ +DDeeffiinniittiioonn expressions.h:131 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ -Both ManifoldTraits and Testable. │ │ │ │ │ -DDeeffiinniittiioonn Manifold.h:120 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ +Both LieGroupTraits and Testable. │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:229 │ │ │ │ │ _g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ either a fixed size o... │ │ │ │ │ DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ _g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ Template to create a binary predicate. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3___S_2 │ │ │ │ │ -The most common 5DOF 3D->2D calibration. │ │ │ │ │ -DDeeffiinniittiioonn Cal3_S2.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o │ │ │ │ │ -The most common 5DOF 3D->2D calibration, stereo version. │ │ │ │ │ -DDeeffiinniittiioonn Cal3_S2Stereo.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o_:_:_v_e_c_t_o_r │ │ │ │ │ -Vector6 vector() const │ │ │ │ │ -vectorized form (column-wise) │ │ │ │ │ -DDeeffiinniittiioonn Cal3_S2Stereo.h:114 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o_:_:_C_a_l_3___S_2_S_t_e_r_e_o │ │ │ │ │ -Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b) │ │ │ │ │ -constructor from doubles │ │ │ │ │ -DDeeffiinniittiioonn Cal3_S2Stereo.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ -Vector6 localCoordinates(const Cal3_S2Stereo &T2) const │ │ │ │ │ -Unretraction for the calibration. │ │ │ │ │ -DDeeffiinniittiioonn Cal3_S2Stereo.h:137 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o_:_:_c_a_l_i_b_r_a_t_i_o_n │ │ │ │ │ -const Cal3_S2 & calibration() const │ │ │ │ │ -return calibration, same for left and right │ │ │ │ │ -DDeeffiinniittiioonn Cal3_S2Stereo.h:105 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o_:_:_d_i_m │ │ │ │ │ -size_t dim() const override │ │ │ │ │ -return DOF, dimensionality of tangent space │ │ │ │ │ -DDeeffiinniittiioonn Cal3_S2Stereo.h:125 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o_:_:_c_a_l_i_b_r_a_t_e │ │ │ │ │ -Vector3 calibrate(const Vector3 &p) const │ │ │ │ │ -Convert homogeneous image coordinates to intrinsic coordinates. │ │ │ │ │ -DDeeffiinniittiioonn Cal3_S2Stereo.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o_:_:_r_e_t_r_a_c_t │ │ │ │ │ -Cal3_S2Stereo retract(const Vector &d) const │ │ │ │ │ -Given 6-dim tangent vector, create new calibration. │ │ │ │ │ -DDeeffiinniittiioonn Cal3_S2Stereo.h:131 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o_:_:_K │ │ │ │ │ -Matrix3 K() const override │ │ │ │ │ -return calibration matrix K, same for left and right │ │ │ │ │ -DDeeffiinniittiioonn Cal3_S2Stereo.h:108 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o_:_:_D_i_m │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ +A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_t_r_a_n_s_l_a_t_i_o_n │ │ │ │ │ +const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const │ │ │ │ │ +get translation │ │ │ │ │ +DDeeffiinniittiioonn Pose3.cpp:308 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_m_a_t_r_i_x │ │ │ │ │ +Matrix4 matrix() const │ │ │ │ │ +convert to 4*4 matrix │ │ │ │ │ +DDeeffiinniittiioonn Pose3.cpp:323 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_i_n_v_e_r_s_e │ │ │ │ │ +Pose3 inverse() const │ │ │ │ │ +inverse transformation with derivatives │ │ │ │ │ +DDeeffiinniittiioonn Pose3.cpp:49 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_r_o_t_a_t_i_o_n │ │ │ │ │ +const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const │ │ │ │ │ +get rotation │ │ │ │ │ +DDeeffiinniittiioonn Pose3.cpp:315 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ +Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor │ │ │ │ │ +symbol GTSAM_USE_QUATERNIO... │ │ │ │ │ +DDeeffiinniittiioonn Rot3.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_E_m_p_t_y_C_a_l │ │ │ │ │ +Empty calibration. │ │ │ │ │ +DDeeffiinniittiioonn SphericalCamera.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_E_m_p_t_y_C_a_l_:_:_D_i_m │ │ │ │ │ static size_t Dim() │ │ │ │ │ return DOF, dimensionality of tangent space │ │ │ │ │ -DDeeffiinniittiioonn Cal3_S2Stereo.h:128 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o_:_:_C_a_l_3___S_2_S_t_e_r_e_o │ │ │ │ │ -Cal3_S2Stereo(double fov, int w, int h, double b) │ │ │ │ │ -easy constructor; field-of-view in degrees, assumes zero skew │ │ │ │ │ -DDeeffiinniittiioonn Cal3_S2Stereo.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o_:_:_b_a_s_e_l_i_n_e │ │ │ │ │ -double baseline() const │ │ │ │ │ -return baseline │ │ │ │ │ -DDeeffiinniittiioonn Cal3_S2Stereo.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o_:_:_C_a_l_3___S_2_S_t_e_r_e_o │ │ │ │ │ -Cal3_S2Stereo(const Vector6 &d) │ │ │ │ │ -constructor from vector │ │ │ │ │ -DDeeffiinniittiioonn Cal3_S2Stereo.h:51 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o_:_:_C_a_l_3___S_2_S_t_e_r_e_o │ │ │ │ │ -Cal3_S2Stereo()=default │ │ │ │ │ -default calibration leaves coordinates unchanged │ │ │ │ │ +DDeeffiinniittiioonn SphericalCamera.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a │ │ │ │ │ +A spherical camera class that has a Pose3 and measures bearing vectors. │ │ │ │ │ +DDeeffiinniittiioonn SphericalCamera.h:70 │ │ │ │ │ +_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_s_h_a_r_e_d_C_a_l_i_b_r_a_t_i_o_n │ │ │ │ │ +const EmptyCal::shared_ptr & sharedCalibration() const │ │ │ │ │ +return shared pointer to calibration │ │ │ │ │ +DDeeffiinniittiioonn SphericalCamera.h:110 │ │ │ │ │ +_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_D_i_m │ │ │ │ │ +static size_t Dim() │ │ │ │ │ +DDeeffiinniittiioonn SphericalCamera.h:219 │ │ │ │ │ +_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a │ │ │ │ │ +SphericalCamera(const Pose3 &pose) │ │ │ │ │ +Constructor with pose. │ │ │ │ │ +DDeeffiinniittiioonn SphericalCamera.h:93 │ │ │ │ │ +_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_~_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a │ │ │ │ │ +virtual ~SphericalCamera()=default │ │ │ │ │ +Default destructor. │ │ │ │ │ +_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_c_a_m_e_r_a_P_r_o_j_e_c_t_i_o_n_M_a_t_r_i_x │ │ │ │ │ +Matrix34 cameraProjectionMatrix() const │ │ │ │ │ +for Linear Triangulation │ │ │ │ │ +DDeeffiinniittiioonn SphericalCamera.h:206 │ │ │ │ │ +_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ +Vector6 localCoordinates(const SphericalCamera &p) const │ │ │ │ │ +return canonical coordinate │ │ │ │ │ +DDeeffiinniittiioonn SphericalCamera.h:195 │ │ │ │ │ +_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_r_o_t_a_t_i_o_n │ │ │ │ │ +const Rot3 & rotation() const │ │ │ │ │ +get rotation │ │ │ │ │ +DDeeffiinniittiioonn SphericalCamera.h:135 │ │ │ │ │ +_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_p_o_s_e │ │ │ │ │ +const Pose3 & pose() const │ │ │ │ │ +return pose, constant version │ │ │ │ │ +DDeeffiinniittiioonn SphericalCamera.h:132 │ │ │ │ │ +_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_t_r_a_n_s_l_a_t_i_o_n │ │ │ │ │ +const Point3 & translation() const │ │ │ │ │ +get translation │ │ │ │ │ +DDeeffiinniittiioonn SphericalCamera.h:138 │ │ │ │ │ +_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_r_e_t_r_a_c_t │ │ │ │ │ +SphericalCamera retract(const Vector6 &d) const │ │ │ │ │ +move a cameras according to d │ │ │ │ │ +DDeeffiinniittiioonn SphericalCamera.h:190 │ │ │ │ │ +_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_c_a_l_i_b_r_a_t_i_o_n │ │ │ │ │ +const EmptyCal & calibration() const │ │ │ │ │ +return calibration │ │ │ │ │ +DDeeffiinniittiioonn SphericalCamera.h:115 │ │ │ │ │ +_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_d_i_m │ │ │ │ │ +size_t dim() const │ │ │ │ │ +DDeeffiinniittiioonn SphericalCamera.h:216 │ │ │ │ │ +_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_I_d_e_n_t_i_t_y │ │ │ │ │ +static SphericalCamera Identity() │ │ │ │ │ +for Canonical │ │ │ │ │ +DDeeffiinniittiioonn SphericalCamera.h:200 │ │ │ │ │ +_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a │ │ │ │ │ +SphericalCamera() │ │ │ │ │ +Default constructor. │ │ │ │ │ +DDeeffiinniittiioonn SphericalCamera.h:89 │ │ │ │ │ +_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_d_e_f_a_u_l_t_E_r_r_o_r_W_h_e_n_T_r_i_a_n_g_u_l_a_t_i_n_g_B_e_h_i_n_d_C_a_m_e_r_a │ │ │ │ │ +Vector defaultErrorWhenTriangulatingBehindCamera() const │ │ │ │ │ +for Nonlinear Triangulation │ │ │ │ │ +DDeeffiinniittiioonn SphericalCamera.h:211 │ │ │ │ │ +_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a │ │ │ │ │ +SphericalCamera(const Pose3 &pose, const EmptyCal::shared_ptr &cal) │ │ │ │ │ +Constructor with empty intrinsics (needed for smart factors) │ │ │ │ │ +DDeeffiinniittiioonn SphericalCamera.h:97 │ │ │ │ │ +_g_t_s_a_m_:_:_U_n_i_t_3 │ │ │ │ │ +Represents a 3D point on a unit sphere. │ │ │ │ │ +DDeeffiinniittiioonn Unit3.h:43 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3___S_2_S_t_e_r_e_o_._h │ │ │ │ │ + * _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00305.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Unified.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SO4.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,51 +94,48 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
Cal3Unified.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
SO4.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Unified Calibration Model, see Mei07icra for details. │ │ │ │ +

4*4 matrix representation of SO(4) │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::Cal3Unified
 Calibration of a omni-directional camera with mirror + lens radial distortion. More...
 
struct  gtsam::traits< Cal3Unified >
 
struct  gtsam::traits< const Cal3Unified >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

GTSAM_EXPORT Matrix3 gtsam::topLeft (const SO4 &Q, OptionalJacobian< 9, 6 > H=boost::none)
 Project to top-left 3*3 matrix.
 
│ │ │ │ +GTSAM_EXPORT Matrix43 gtsam::stiefel (const SO4 &Q, OptionalJacobian< 12, 6 > H=boost::none)
 Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q) -> \( S \in St(3,4) \).
 
│ │ │ │

Detailed Description

│ │ │ │ -

Unified Calibration Model, see Mei07icra for details.

│ │ │ │ -
Date
Mar 8, 2014
│ │ │ │ -
Author
Jing Dong
│ │ │ │ +

4*4 matrix representation of SO(4)

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │ -Varun Agrawal
│ │ │ │ +Luca Carlone
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,33 +1,31 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Cal3Unified.h File Reference │ │ │ │ │ -Unified Calibration Model, see Mei07icra for details. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_C_a_l_3_U_n_i_f_i_e_d │ │ │ │ │ -  Calibration of a omni-directional camera with mirror + lens radial │ │ │ │ │ - distortion. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _C_a_l_3_U_n_i_f_i_e_d_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _C_a_l_3_U_n_i_f_i_e_d_ _> │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +SO4.cpp File Reference │ │ │ │ │ +4*4 matrix representation of SO(4) _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ + GTSAM_EXPORT Matrix3  _g_t_s_a_m_:_:_t_o_p_L_e_f_t (const _S_O_4 &Q, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 9, 6 > │ │ │ │ │ + H=boost::none) │ │ │ │ │ +  Project to top-left 3*3 matrix. │ │ │ │ │ +  │ │ │ │ │ +GTSAM_EXPORT Matrix43  ggttssaamm::::ssttiieeffeell (const _S_O_4 &Q, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 12, 6 > │ │ │ │ │ + H=boost::none) │ │ │ │ │ +  Project to Stiefel manifold of 4*3 orthonormal 3-frames │ │ │ │ │ + in R^4, i.e., pi(Q) -> \( S \in St(3,4) \). │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Unified Calibration Model, see Mei07icra for details. │ │ │ │ │ - Date │ │ │ │ │ - Mar 8, 2014 │ │ │ │ │ +4*4 matrix representation of SO(4) │ │ │ │ │ Author │ │ │ │ │ - Jing Dong │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Luca Carlone │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3_U_n_i_f_i_e_d_._h │ │ │ │ │ + * _S_O_4_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00305.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a00305 = [ │ │ │ │ │ - ["gtsam::traits< Cal3Unified >", "a03012.html", null], │ │ │ │ │ - ["gtsam::traits< const Cal3Unified >", "a03016.html", null] │ │ │ │ │ + ["stiefel", "a00305.html#a3e57e4771f13855a495ec2c6454c9121", null], │ │ │ │ │ + ["topLeft", "a00305.html#a8d21728e3da4cf1a96e1b6f87a97da47", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00308.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Rot3Q.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SimpleCamera.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -93,27 +93,37 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Rot3Q.cpp File Reference
│ │ │ │ +
│ │ │ │ +Namespaces
│ │ │ │ +
SimpleCamera.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Rotation (internal: quaternion representation*) │ │ │ │ +

A simple camera class with a Cal3_S2 calibration. │ │ │ │ More...

│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Rotation (internal: quaternion representation*)

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ +

A simple camera class with a Cal3_S2 calibration.

│ │ │ │ +
Date
June 30, 2012
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,15 +1,22 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Rot3Q.cpp File Reference │ │ │ │ │ -Rotation (internal: quaternion representation*) _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +SimpleCamera.cpp File Reference │ │ │ │ │ +A simple camera class with a Cal3_S2 calibration. _M_o_r_e_._._. │ │ │ │ │ +NNaammeessppaacceess │ │ │ │ │ +namespace   _g_t_s_a_m │ │ │ │ │ +  Global functions in a separate testing namespace. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Rotation (internal: quaternion representation*) │ │ │ │ │ +A simple camera class with a Cal3_S2 calibration. │ │ │ │ │ + Date │ │ │ │ │ + June 30, 2012 │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _R_o_t_3_Q_._c_p_p │ │ │ │ │ + * _S_i_m_p_l_e_C_a_m_e_r_a_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00311.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SOn.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Similarity3.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,75 +96,59 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs | │ │ │ │ Functions
│ │ │ │ -
SOn.h File Reference
│ │ │ │ +
Similarity3.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

N*N matrix representation of SO(N). │ │ │ │ +

Implementation of Similarity3 transform. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::SO< N >
 Manifold of special orthogonal rotation matrices SO<N>. More...
class  gtsam::Similarity3
 3D similarity transform More...
 
struct  gtsam::SO< N >::ChartAtOrigin
struct  gtsam::Similarity3::ChartAtOrigin
 Chart at the origin. More...
 
struct  gtsam::traits< SO< N > >
struct  gtsam::traits< Similarity3 >
 
struct  gtsam::traits< const SO< N > >
struct  gtsam::traits< const Similarity3 >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Typedefs

│ │ │ │ -using gtsam::SOn = SO< Eigen::Dynamic >
 
│ │ │ │ -using gtsam::DynamicJacobian = OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

│ │ │ │ -constexpr int gtsam::internal::DimensionSO (int N)
 Calculate dimensionality of SO<N> manifold, or return Dynamic if so.
 
│ │ │ │ -constexpr int gtsam::internal::NSquaredSO (int N)
 
│ │ │ │ -template<class Archive >
void gtsam::serialize (Archive &ar, SOn &Q, const unsigned int file_version)
 Serialization function.
 
│ │ │ │ +template<>
Matrix gtsam::wedge< Similarity3 > (const Vector &xi)
 
│ │ │ │

Detailed Description

│ │ │ │ -

N*N matrix representation of SO(N).

│ │ │ │ -

N can be Eigen::Dynamic

Author
Frank Dellaert
│ │ │ │ -
Date
March 2019
│ │ │ │ +

Implementation of Similarity3 transform.

│ │ │ │ +
Author
Paul Drews
│ │ │ │ +
│ │ │ │ +John Lambert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,52 +1,37 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -SOn.h File Reference │ │ │ │ │ -N*N matrix representation of SO(N). _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Similarity3.h File Reference │ │ │ │ │ +Implementation of Similarity3 transform. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_S_O_<_ _N_ _> │ │ │ │ │ -  Manifold of special orthogonal rotation matrices SO. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_3 │ │ │ │ │ +  3D similarity transform _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_S_O_<_ _N_ _>_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n │ │ │ │ │ +struct   _g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_3_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n │ │ │ │ │ +  Chart at the origin. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_O_<_ _N_ _>_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_i_m_i_l_a_r_i_t_y_3_ _> │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _S_O_<_ _N_ _>_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _S_i_m_i_l_a_r_i_t_y_3_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -using  ggttssaamm::::SSOOnn = _S_O< Eigen::Dynamic > │ │ │ │ │ -  │ │ │ │ │ -using  ggttssaamm::::DDyynnaammiiccJJaaccoobbiiaann = _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< Eigen::Dynamic, Eigen:: │ │ │ │ │ - Dynamic > │ │ │ │ │ -  │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -constexpr int  ggttssaamm::::iinntteerrnnaall::::DDiimmeennssiioonnSSOO (int N) │ │ │ │ │ -  Calculate dimensionality of SO manifold, or return Dynamic if │ │ │ │ │ - so. │ │ │ │ │ -  │ │ │ │ │ -constexpr int  ggttssaamm::::iinntteerrnnaall::::NNSSqquuaarreeddSSOO (int N) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - void  ggttssaamm::::sseerriiaalliizzee (Archive &ar, _S_O_n &Q, const unsigned int │ │ │ │ │ - file_version) │ │ │ │ │ -  Serialization function. │ │ │ │ │ +template<> │ │ │ │ │ + Matrix  ggttssaamm::::wweeddggee<< SSiimmiillaarriittyy33 >> (const Vector &xi) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -N*N matrix representation of SO(N). │ │ │ │ │ -N can be Eigen::Dynamic │ │ │ │ │ +Implementation of Similarity3 transform. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - March 2019 │ │ │ │ │ + Paul Drews │ │ │ │ │ + John Lambert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_O_n_._h │ │ │ │ │ + * _S_i_m_i_l_a_r_i_t_y_3_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00311.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,8 +1,6 @@ │ │ │ │ │ var a00311 = [ │ │ │ │ │ - ["gtsam::SO< N >", "a03304.html", "a03304"], │ │ │ │ │ - ["gtsam::SO< N >::ChartAtOrigin", "a03308.html", null], │ │ │ │ │ - ["gtsam::traits< SO< N > >", "a03312.html", null], │ │ │ │ │ - ["gtsam::traits< const SO< N > >", "a03316.html", null], │ │ │ │ │ - ["DimensionSO", "a00311.html#afdaebcf25a03a9a9986d35d2831605dd", null], │ │ │ │ │ - ["serialize", "a00311.html#ac1829b506847127f406c0d6182fc0cdd", null] │ │ │ │ │ + ["gtsam::Similarity3", "a03264.html", "a03264"], │ │ │ │ │ + ["gtsam::Similarity3::ChartAtOrigin", "a03268.html", null], │ │ │ │ │ + ["gtsam::traits< Similarity3 >", "a03272.html", null], │ │ │ │ │ + ["gtsam::traits< const Similarity3 >", "a03276.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00311_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SOn.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Similarity3.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,400 +98,181 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
SOn.h
│ │ │ │ +
Similarity3.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/base/Lie.h>
│ │ │ │ -
22#include <gtsam/base/Manifold.h>
│ │ │ │ -
23#include <gtsam/base/make_shared.h>
│ │ │ │ -
24#include <gtsam/dllexport.h>
│ │ │ │ -
25#include <Eigen/Core>
│ │ │ │ -
26
│ │ │ │ -
27#include <boost/serialization/nvp.hpp>
│ │ │ │ -
28
│ │ │ │ -
29#include <iostream> // TODO(frank): how to avoid?
│ │ │ │ -
30#include <string>
│ │ │ │ -
31#include <type_traits>
│ │ │ │ -
32#include <vector>
│ │ │ │ -
33#include <random>
│ │ │ │ -
34
│ │ │ │ -
35namespace gtsam {
│ │ │ │ -
36
│ │ │ │ -
37namespace internal {
│ │ │ │ -
│ │ │ │ -
39constexpr int DimensionSO(int N) {
│ │ │ │ -
40 return (N < 0) ? Eigen::Dynamic : N * (N - 1) / 2;
│ │ │ │ -
41}
│ │ │ │ -
│ │ │ │ +
21#include <gtsam/base/Lie.h>
│ │ │ │ +
22#include <gtsam/base/Manifold.h>
│ │ │ │ +
23#include <gtsam/dllexport.h>
│ │ │ │ +
24#include <gtsam/geometry/Point3.h>
│ │ │ │ +
25#include <gtsam/geometry/Pose3.h>
│ │ │ │ +
26#include <gtsam/geometry/Rot3.h>
│ │ │ │ +
27
│ │ │ │ +
28namespace gtsam {
│ │ │ │ +
29
│ │ │ │ +
30// Forward declarations
│ │ │ │ +
31class Pose3;
│ │ │ │ +
32
│ │ │ │ +
│ │ │ │ +
36class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
│ │ │ │ +
39 typedef Rot3 Rotation;
│ │ │ │ +
40 typedef Point3 Translation;
│ │ │ │
42
│ │ │ │ -
43// Calculate N^2 at compile time, or return Dynamic if so
│ │ │ │ -
44constexpr int NSquaredSO(int N) { return (N < 0) ? Eigen::Dynamic : N * N; }
│ │ │ │ -
45} // namespace internal
│ │ │ │ -
46
│ │ │ │ -
51template <int N>
│ │ │ │ -
│ │ │ │ -
52class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
│ │ │ │ -
53 public:
│ │ │ │ -
54 enum { dimension = internal::DimensionSO(N) };
│ │ │ │ -
55 using MatrixNN = Eigen::Matrix<double, N, N>;
│ │ │ │ -
56 using VectorN2 = Eigen::Matrix<double, internal::NSquaredSO(N), 1>;
│ │ │ │ -
57 using MatrixDD = Eigen::Matrix<double, dimension, dimension>;
│ │ │ │ -
58
│ │ │ │ -
59 GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(true)
│ │ │ │ +
43 private:
│ │ │ │ +
44 Rot3 R_;
│ │ │ │ +
45 Point3 t_;
│ │ │ │ +
46 double s_;
│ │ │ │ +
47
│ │ │ │ +
48 public:
│ │ │ │ +
51
│ │ │ │ +
53 Similarity3();
│ │ │ │ +
54
│ │ │ │ +
56 Similarity3(double s);
│ │ │ │ +
57
│ │ │ │ +
59 Similarity3(const Rot3& R, const Point3& t, double s);
│ │ │ │
60
│ │ │ │ -
61 protected:
│ │ │ │ -
62 MatrixNN matrix_;
│ │ │ │ +
62 Similarity3(const Matrix3& R, const Vector3& t, double s);
│ │ │ │
63
│ │ │ │ -
64 // enable_if_t aliases, used to specialize constructors/methods, see
│ │ │ │ -
65 // https://www.fluentcpp.com/2018/05/18/make-sfinae-pretty-2-hidden-beauty-sfinae/
│ │ │ │ -
66 template <int N_>
│ │ │ │ -
67 using IsDynamic = typename std::enable_if<N_ == Eigen::Dynamic, void>::type;
│ │ │ │ -
68 template <int N_>
│ │ │ │ -
69 using IsFixed = typename std::enable_if<N_ >= 2, void>::type;
│ │ │ │ -
70 template <int N_>
│ │ │ │ -
71 using IsSO3 = typename std::enable_if<N_ == 3, void>::type;
│ │ │ │ -
72
│ │ │ │ -
73 public:
│ │ │ │ +
65 Similarity3(const Matrix4& T);
│ │ │ │ +
66
│ │ │ │ +
70
│ │ │ │ +
72 bool equals(const Similarity3& sim, double tol) const;
│ │ │ │ +
73
│ │ │ │ +
75 bool operator==(const Similarity3& other) const;
│ │ │ │
76
│ │ │ │ -
78 template <int N_ = N, typename = IsFixed<N_>>
│ │ │ │ -
79 SO() : matrix_(MatrixNN::Identity()) {}
│ │ │ │ -
80
│ │ │ │ -
82 template <int N_ = N, typename = IsDynamic<N_>>
│ │ │ │ -
│ │ │ │ -
83 explicit SO(size_t n = 0) {
│ │ │ │ -
84 // We allow for n=0 as the default constructor, needed for serialization,
│ │ │ │ -
85 // wrappers etc.
│ │ │ │ -
86 matrix_ = Eigen::MatrixXd::Identity(n, n);
│ │ │ │ -
87 }
│ │ │ │ -
│ │ │ │ +
78 void print(const std::string& s) const;
│ │ │ │ +
79
│ │ │ │ +
80 friend std::ostream& operator<<(std::ostream& os, const Similarity3& p);
│ │ │ │ +
81
│ │ │ │ +
85
│ │ │ │ +
87 static Similarity3 Identity();
│ │ │ │
88
│ │ │ │ -
90 template <typename Derived>
│ │ │ │ -
91 explicit SO(const Eigen::MatrixBase<Derived>& R) : matrix_(R.eval()) {}
│ │ │ │ -
92
│ │ │ │ -
94 template <typename Derived>
│ │ │ │ -
│ │ │ │ -
95 static SO FromMatrix(const Eigen::MatrixBase<Derived>& R) {
│ │ │ │ -
96 return SO(R);
│ │ │ │ -
97 }
│ │ │ │ -
│ │ │ │ +
90 Similarity3 operator*(const Similarity3& S) const;
│ │ │ │ +
91
│ │ │ │ +
93 Similarity3 inverse() const;
│ │ │ │ +
94
│ │ │ │
98
│ │ │ │ -
100 template <typename Derived, int N_ = N, typename = IsDynamic<N_>>
│ │ │ │ -
│ │ │ │ -
101 static SO Lift(size_t n, const Eigen::MatrixBase<Derived> &R) {
│ │ │ │ -
102 Matrix Q = Matrix::Identity(n, n);
│ │ │ │ -
103 const int p = R.rows();
│ │ │ │ -
104 assert(p >= 0 && p <= static_cast<int>(n) && R.cols() == p);
│ │ │ │ -
105 Q.topLeftCorner(p, p) = R;
│ │ │ │ -
106 return SO(Q);
│ │ │ │ -
107 }
│ │ │ │ -
│ │ │ │ -
108
│ │ │ │ -
110 template <int M, int N_ = N, typename = IsDynamic<N_>>
│ │ │ │ -
111 explicit SO(const SO<M>& R) : matrix_(R.matrix()) {}
│ │ │ │ -
112
│ │ │ │ -
114 template <int N_ = N, typename = IsSO3<N_>>
│ │ │ │ -
115 explicit SO(const Eigen::AngleAxisd& angleAxis) : matrix_(angleAxis) {}
│ │ │ │ +
100 Point3 transformFrom(const Point3& p, //
│ │ │ │ +
101 OptionalJacobian<3, 7> H1 = boost::none, //
│ │ │ │ +
102 OptionalJacobian<3, 3> H2 = boost::none) const;
│ │ │ │ +
103
│ │ │ │ +
115 Pose3 transformFrom(const Pose3& T) const;
│ │ │ │
116
│ │ │ │ -
118 static SO AxisAngle(const Vector3& axis, double theta);
│ │ │ │ +
118 Point3 operator*(const Point3& p) const;
│ │ │ │
119
│ │ │ │ -
122 static SO ClosestTo(const MatrixNN& M);
│ │ │ │ -
123
│ │ │ │ -
127 static SO ChordalMean(const std::vector<SO>& rotations);
│ │ │ │ -
128
│ │ │ │ -
130 template <int N_ = N, typename = IsDynamic<N_>>
│ │ │ │ -
│ │ │ │ -
131 static SO Random(std::mt19937& rng, size_t n = 0) {
│ │ │ │ -
132 if (n == 0) throw std::runtime_error("SO: Dimensionality not known.");
│ │ │ │ -
133 // TODO(frank): this might need to be re-thought
│ │ │ │ -
134 static std::uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
│ │ │ │ -
135 const size_t d = SO::Dimension(n);
│ │ │ │ -
136 Vector xi(d);
│ │ │ │ -
137 for (size_t j = 0; j < d; j++) {
│ │ │ │ -
138 xi(j) = randomAngle(rng);
│ │ │ │ -
139 }
│ │ │ │ -
140 return SO::Retract(xi);
│ │ │ │ -
141 }
│ │ │ │ +
123 static Similarity3 Align(const Point3Pairs& abPointPairs);
│ │ │ │ +
124
│ │ │ │ +
135 static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);
│ │ │ │ +
136
│ │ │ │ +
140
│ │ │ │ +
144 static Vector7 Logmap(const Similarity3& s, //
│ │ │ │ +
145 OptionalJacobian<7, 7> Hm = boost::none);
│ │ │ │ +
146
│ │ │ │ +
149 static Similarity3 Expmap(const Vector7& v, //
│ │ │ │ +
150 OptionalJacobian<7, 7> Hm = boost::none);
│ │ │ │ +
151
│ │ │ │ +
│ │ │ │ +
153 struct ChartAtOrigin {
│ │ │ │ +
154 static Similarity3 Retract(const Vector7& v,
│ │ │ │ +
155 ChartJacobian H = boost::none) {
│ │ │ │ +
156 return Similarity3::Expmap(v, H);
│ │ │ │ +
157 }
│ │ │ │ +
158 static Vector7 Local(const Similarity3& other,
│ │ │ │ +
159 ChartJacobian H = boost::none) {
│ │ │ │ +
160 return Similarity3::Logmap(other, H);
│ │ │ │ +
161 }
│ │ │ │ +
162 };
│ │ │ │
│ │ │ │ -
142
│ │ │ │ -
144 template <int N_ = N, typename = IsFixed<N_>>
│ │ │ │ -
│ │ │ │ -
145 static SO Random(std::mt19937& rng) {
│ │ │ │ -
146 // By default, use dynamic implementation above. Specialized for SO(3).
│ │ │ │ -
147 return SO(SO<Eigen::Dynamic>::Random(rng, N).matrix());
│ │ │ │ -
148 }
│ │ │ │ -
│ │ │ │ -
149
│ │ │ │ -
153
│ │ │ │ -
155 const MatrixNN& matrix() const { return matrix_; }
│ │ │ │ -
156
│ │ │ │ -
157 size_t rows() const { return matrix_.rows(); }
│ │ │ │ -
158 size_t cols() const { return matrix_.cols(); }
│ │ │ │ -
159
│ │ │ │
163
│ │ │ │ -
164 void print(const std::string& s = std::string()) const;
│ │ │ │ +
164 using LieGroup<Similarity3, 7>::inverse;
│ │ │ │
165
│ │ │ │ -
166 bool equals(const SO& other, double tol) const {
│ │ │ │ -
167 return equal_with_abs_tol(matrix_, other.matrix_, tol);
│ │ │ │ -
168 }
│ │ │ │ -
169
│ │ │ │ +
172 static Matrix4 wedge(const Vector7& xi);
│ │ │ │
173
│ │ │ │ -
│ │ │ │ -
175 SO operator*(const SO& other) const {
│ │ │ │ -
176 assert(dim() == other.dim());
│ │ │ │ -
177 return SO(matrix_ * other.matrix_);
│ │ │ │ -
178 }
│ │ │ │ -
│ │ │ │ -
179
│ │ │ │ -
181 template <int N_ = N, typename = IsFixed<N_>>
│ │ │ │ -
│ │ │ │ -
182 static SO Identity() {
│ │ │ │ -
183 return SO();
│ │ │ │ -
184 }
│ │ │ │ -
│ │ │ │ -
185
│ │ │ │ -
187 template <int N_ = N, typename = IsDynamic<N_>>
│ │ │ │ -
│ │ │ │ -
188 static SO Identity(size_t n = 0) {
│ │ │ │ -
189 return SO(n);
│ │ │ │ -
190 }
│ │ │ │ -
│ │ │ │ -
191
│ │ │ │ -
193 SO inverse() const { return SO(matrix_.transpose()); }
│ │ │ │ -
194
│ │ │ │ +
175 Matrix7 AdjointMap() const;
│ │ │ │ +
176
│ │ │ │ +
180
│ │ │ │ +
182 Matrix4 matrix() const;
│ │ │ │ +
183
│ │ │ │ +
185 Rot3 rotation() const { return R_; }
│ │ │ │ +
186
│ │ │ │ +
188 Point3 translation() const { return t_; }
│ │ │ │ +
189
│ │ │ │ +
191 double scale() const { return s_; }
│ │ │ │ +
192
│ │ │ │ +
194 inline static size_t Dim() { return 7; }
│ │ │ │ +
195
│ │ │ │ +
197 inline size_t dim() const { return 7; }
│ │ │ │
198
│ │ │ │ -
199 using TangentVector = Eigen::Matrix<double, dimension, 1>;
│ │ │ │ -
200 using ChartJacobian = OptionalJacobian<dimension, dimension>;
│ │ │ │ -
201
│ │ │ │ -
203 static int Dim() { return dimension; }
│ │ │ │ -
204
│ │ │ │ -
205 // Calculate manifold dimensionality for SO(n).
│ │ │ │ -
206 // Available as dimension or Dim() for fixed N.
│ │ │ │ -
207 static size_t Dimension(size_t n) { return n * (n - 1) / 2; }
│ │ │ │ -
208
│ │ │ │ -
209 // Calculate ambient dimension n from manifold dimensionality d.
│ │ │ │ -
210 static size_t AmbientDim(size_t d) { return (1 + std::sqrt(1 + 8 * d)) / 2; }
│ │ │ │ -
211
│ │ │ │ -
212 // Calculate run-time dimensionality of manifold.
│ │ │ │ -
213 // Available as dimension or Dim() for fixed N.
│ │ │ │ -
214 size_t dim() const { return Dimension(static_cast<size_t>(matrix_.rows())); }
│ │ │ │ -
215
│ │ │ │ -
231 static MatrixNN Hat(const TangentVector& xi);
│ │ │ │ -
232
│ │ │ │ -
234 static void Hat(const Vector &xi, Eigen::Ref<MatrixNN> X);
│ │ │ │ -
235
│ │ │ │ -
237 static TangentVector Vee(const MatrixNN& X);
│ │ │ │ -
238
│ │ │ │ -
239 // Chart at origin
│ │ │ │ -
│ │ │ │ -
240 struct ChartAtOrigin {
│ │ │ │ -
245 static SO Retract(const TangentVector& xi, ChartJacobian H = boost::none);
│ │ │ │ -
246
│ │ │ │ -
250 static TangentVector Local(const SO& R, ChartJacobian H = boost::none);
│ │ │ │ -
251 };
│ │ │ │ -
│ │ │ │ -
252
│ │ │ │ -
253 // Return dynamic identity DxD Jacobian for given SO(n)
│ │ │ │ -
254 template <int N_ = N, typename = IsDynamic<N_>>
│ │ │ │ -
255 static MatrixDD IdentityJacobian(size_t n) {
│ │ │ │ -
256 const size_t d = Dimension(n);
│ │ │ │ -
257 return MatrixDD::Identity(d, d);
│ │ │ │ -
258 }
│ │ │ │ -
259
│ │ │ │ -
263
│ │ │ │ -
265 MatrixDD AdjointMap() const;
│ │ │ │ -
266
│ │ │ │ -
270 static SO Expmap(const TangentVector& omega, ChartJacobian H = boost::none);
│ │ │ │ -
271
│ │ │ │ -
273 static MatrixDD ExpmapDerivative(const TangentVector& omega);
│ │ │ │ -
274
│ │ │ │ -
278 static TangentVector Logmap(const SO& R, ChartJacobian H = boost::none);
│ │ │ │ -
279
│ │ │ │ -
281 static MatrixDD LogmapDerivative(const TangentVector& omega);
│ │ │ │ -
282
│ │ │ │ -
283 // inverse with optional derivative
│ │ │ │ -
284 using LieGroup<SO<N>, internal::DimensionSO(N)>::inverse;
│ │ │ │ -
285
│ │ │ │ -
289
│ │ │ │ -
295 VectorN2 vec(OptionalJacobian<internal::NSquaredSO(N), dimension> H =
│ │ │ │ -
296 boost::none) const;
│ │ │ │ -
297
│ │ │ │ -
299 template <int N_ = N, typename = IsFixed<N_>>
│ │ │ │ -
│ │ │ │ -
300 static Matrix VectorizedGenerators() {
│ │ │ │ -
301 constexpr size_t N2 = static_cast<size_t>(N * N);
│ │ │ │ -
302 Eigen::Matrix<double, N2, dimension> G;
│ │ │ │ -
303 for (size_t j = 0; j < dimension; j++) {
│ │ │ │ -
304 const auto X = Hat(Vector::Unit(dimension, j));
│ │ │ │ -
305 G.col(j) = Eigen::Map<const VectorN2>(X.data());
│ │ │ │ -
306 }
│ │ │ │ -
307 return G;
│ │ │ │ -
308 }
│ │ │ │ -
│ │ │ │ -
309
│ │ │ │ -
311 template <int N_ = N, typename = IsDynamic<N_>>
│ │ │ │ -
│ │ │ │ -
312 static Matrix VectorizedGenerators(size_t n = 0) {
│ │ │ │ -
313 const size_t n2 = n * n, dim = Dimension(n);
│ │ │ │ -
314 Matrix G(n2, dim);
│ │ │ │ -
315 for (size_t j = 0; j < dim; j++) {
│ │ │ │ -
316 const auto X = Hat(Vector::Unit(dim, j));
│ │ │ │ -
317 G.col(j) = Eigen::Map<const Matrix>(X.data(), n2, 1);
│ │ │ │ -
318 }
│ │ │ │ -
319 return G;
│ │ │ │ -
320 }
│ │ │ │ -
│ │ │ │ -
321
│ │ │ │ -
325
│ │ │ │ -
326 template <class Archive>
│ │ │ │ -
327 friend void save(Archive&, SO&, const unsigned int);
│ │ │ │ -
328 template <class Archive>
│ │ │ │ -
329 friend void load(Archive&, SO&, const unsigned int);
│ │ │ │ -
330 template <class Archive>
│ │ │ │ -
331 friend void serialize(Archive&, SO&, const unsigned int);
│ │ │ │ -
332 friend class boost::serialization::access;
│ │ │ │ -
333 friend class Rot3; // for serialize
│ │ │ │ -
334
│ │ │ │ -
336};
│ │ │ │ -
│ │ │ │ -
337
│ │ │ │ -
338using SOn = SO<Eigen::Dynamic>;
│ │ │ │ -
339
│ │ │ │ -
340/*
│ │ │ │ -
341 * Specialize dynamic Hat and Vee, because recursion depends on dynamic nature.
│ │ │ │ -
342 * The definition is in SOn.cpp. Fixed-size SO3 and SO4 have their own version,
│ │ │ │ -
343 * and implementation for other fixed N is in SOn-inl.h.
│ │ │ │ -
344 */
│ │ │ │ -
345
│ │ │ │ -
346template <>
│ │ │ │ -
347GTSAM_EXPORT
│ │ │ │ -
348Matrix SOn::Hat(const Vector& xi);
│ │ │ │ -
349
│ │ │ │ -
350template <>
│ │ │ │ -
351GTSAM_EXPORT
│ │ │ │ -
352Vector SOn::Vee(const Matrix& X);
│ │ │ │ -
353
│ │ │ │ -
354/*
│ │ │ │ -
355 * Specialize dynamic compose and between, because the derivative is unknowable
│ │ │ │ -
356 * by the LieGroup implementations, who return a fixed-size matrix for H2.
│ │ │ │ -
357 */
│ │ │ │ -
358
│ │ │ │ -
359using DynamicJacobian = OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic>;
│ │ │ │ -
360
│ │ │ │ -
361template <>
│ │ │ │ -
362GTSAM_EXPORT
│ │ │ │ -
363SOn LieGroup<SOn, Eigen::Dynamic>::compose(const SOn& g, DynamicJacobian H1,
│ │ │ │ -
364 DynamicJacobian H2) const;
│ │ │ │ -
365
│ │ │ │ -
366template <>
│ │ │ │ -
367GTSAM_EXPORT
│ │ │ │ -
368SOn LieGroup<SOn, Eigen::Dynamic>::between(const SOn& g, DynamicJacobian H1,
│ │ │ │ -
369 DynamicJacobian H2) const;
│ │ │ │ -
370
│ │ │ │ -
371/*
│ │ │ │ -
372 * Specialize dynamic vec.
│ │ │ │ -
373 */
│ │ │ │ -
374template <>
│ │ │ │ -
375GTSAM_EXPORT
│ │ │ │ -
376typename SOn::VectorN2 SOn::vec(DynamicJacobian H) const;
│ │ │ │ -
377
│ │ │ │ -
379template<class Archive>
│ │ │ │ -
│ │ │ │ -
380void serialize(
│ │ │ │ -
381 Archive& ar, SOn& Q,
│ │ │ │ -
382 const unsigned int file_version
│ │ │ │ -
383) {
│ │ │ │ -
384 Matrix& M = Q.matrix_;
│ │ │ │ -
385 ar& BOOST_SERIALIZATION_NVP(M);
│ │ │ │ -
386}
│ │ │ │ -
│ │ │ │ -
387
│ │ │ │ -
388/*
│ │ │ │ -
389 * Define the traits. internal::LieGroup provides both Lie group and Testable
│ │ │ │ -
390 */
│ │ │ │ -
391
│ │ │ │ -
392template <int N>
│ │ │ │ -
393struct traits<SO<N>> : public internal::LieGroup<SO<N>> {};
│ │ │ │ -
394
│ │ │ │ -
395template <int N>
│ │ │ │ -
396struct traits<const SO<N>> : public internal::LieGroup<SO<N>> {};
│ │ │ │ -
397
│ │ │ │ -
398} // namespace gtsam
│ │ │ │ -
399
│ │ │ │ -
400#include "SOn-inl.h"
│ │ │ │ -
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ -
make_shared.h
make_shared trampoline function to ensure proper alignment
│ │ │ │ -
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ -
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
This marks a GTSAM object to require alignment.
Definition types.h:317
│ │ │ │ -
SOn-inl.h
Template implementations for SO(n)
│ │ │ │ -
gtsam::internal::DimensionSO
constexpr int DimensionSO(int N)
Calculate dimensionality of SO<N> manifold, or return Dynamic if so.
Definition SOn.h:39
│ │ │ │ +
202
│ │ │ │ +
203 private:
│ │ │ │ +
205 static Matrix3 GetV(Vector3 w, double lambda);
│ │ │ │ +
206
│ │ │ │ +
208};
│ │ │ │ +
│ │ │ │ +
209
│ │ │ │ +
210template <>
│ │ │ │ +
211inline Matrix wedge<Similarity3>(const Vector& xi) {
│ │ │ │ +
212 return Similarity3::wedge(xi);
│ │ │ │ +
213}
│ │ │ │ +
214
│ │ │ │ +
215template <>
│ │ │ │ +
216struct traits<Similarity3> : public internal::LieGroup<Similarity3> {};
│ │ │ │ +
217
│ │ │ │ +
218template <>
│ │ │ │ +
219struct traits<const Similarity3> : public internal::LieGroup<Similarity3> {};
│ │ │ │ +
220
│ │ │ │ +
221} // namespace gtsam
│ │ │ │ +
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ +
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ +
Rot3.h
3D rotation represented as a rotation matrix or quaternion
│ │ │ │ +
Pose3.h
3D Pose
│ │ │ │ +
Point3.h
3D Point
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition serialization.h:113
│ │ │ │ -
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition Matrix.h:81
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
│ │ │ │ +
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ +
gtsam::operator==
bool operator==(const Matrix &A, const Matrix &B)
equality is just equal_with_abs_tol 1e-9
Definition Matrix.h:100
│ │ │ │ +
gtsam::wedge
Matrix wedge(const Vector &x)
Declaration of wedge (see Murray94book) used to convert from n exponential coordinates to n*n element...
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │
gtsam::LieGroup
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition Lie.h:37
│ │ │ │ -
gtsam::LieGroup< SO< N >, internal::DimensionSO(N)>::Retract
static SO< N > Retract(const TangentVector &v)
Retract at origin: possible in Lie group because it has an identity.
Definition Lie.h:111
│ │ │ │
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition Lie.h:229
│ │ │ │
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
│ │ │ │ -
gtsam::SO
Manifold of special orthogonal rotation matrices SO<N>.
Definition SOn.h:52
│ │ │ │ -
gtsam::SO::FromMatrix
static SO FromMatrix(const Eigen::MatrixBase< Derived > &R)
Named constructor from Eigen Matrix.
Definition SOn.h:95
│ │ │ │ -
gtsam::SO::Expmap
static SO Expmap(const TangentVector &omega, ChartJacobian H=boost::none)
Exponential map at identity - create a rotation from canonical coordinates.
Definition SOn-inl.h:67
│ │ │ │ -
gtsam::SO::VectorizedGenerators
static Matrix VectorizedGenerators()
Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N)
Definition SOn.h:300
│ │ │ │ -
gtsam::SO::inverse
SO inverse() const
inverse of a rotation = transpose
Definition SOn.h:193
│ │ │ │ -
gtsam::SO::vec
VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost::none) const
Return vectorized rotation matrix in column order.
Definition SOn-inl.h:88
│ │ │ │ -
gtsam::SO::ChordalMean
static SO ChordalMean(const std::vector< SO > &rotations)
Named constructor that finds chordal mean , currently only defined for SO3.
│ │ │ │ -
gtsam::SO::operator*
SO operator*(const SO &other) const
Multiplication.
Definition SOn.h:175
│ │ │ │ -
gtsam::SO::Vee
static TangentVector Vee(const MatrixNN &X)
Inverse of Hat. See note about xi element order in Hat.
Definition SOn-inl.h:35
│ │ │ │ -
gtsam::SO::matrix_
MatrixNN matrix_
Rotation matrix.
Definition SOn.h:62
│ │ │ │ -
gtsam::SO::Logmap
static TangentVector Logmap(const SO &R, ChartJacobian H=boost::none)
Log map at identity - returns the canonical coordinates of this rotation.
Definition SOn-inl.h:77
│ │ │ │ -
gtsam::SO::AxisAngle
static SO AxisAngle(const Vector3 &axis, double theta)
Constructor from axis and angle. Only defined for SO3.
│ │ │ │ -
gtsam::SO::AdjointMap
MatrixDD AdjointMap() const
Adjoint map.
Definition SO4.cpp:159
│ │ │ │ -
gtsam::SO::Hat
static void Hat(const Vector &xi, Eigen::Ref< MatrixNN > X)
In-place version of Hat (see details there), implements recursion.
│ │ │ │ -
gtsam::SO::Identity
static SO Identity()
SO<N> identity for N >= 2.
Definition SOn.h:182
│ │ │ │ -
gtsam::SO::Dim
static int Dim()
Return compile-time dimensionality: fixed size N or Eigen::Dynamic.
Definition SOn.h:203
│ │ │ │ -
gtsam::SO::Hat
static MatrixNN Hat(const TangentVector &xi)
Hat operator creates Lie algebra element corresponding to d-vector, where d is the dimensionality of ...
Definition SOn-inl.h:29
│ │ │ │ -
gtsam::SO::ExpmapDerivative
static MatrixDD ExpmapDerivative(const TangentVector &omega)
Derivative of Expmap, currently only defined for SO3.
Definition SOn-inl.h:72
│ │ │ │ -
gtsam::SO::LogmapDerivative
static MatrixDD LogmapDerivative(const TangentVector &omega)
Derivative of Logmap, currently only defined for SO3.
Definition SOn-inl.h:82
│ │ │ │ -
gtsam::SO::SO
SO(const SO< M > &R)
Construct dynamic SO(n) from Fixed SO<M>
Definition SOn.h:111
│ │ │ │ -
gtsam::SO::SO
SO(size_t n=0)
Construct SO<N> identity for N == Eigen::Dynamic.
Definition SOn.h:83
│ │ │ │ -
gtsam::SO::SO
SO()
Construct SO<N> identity for N >= 2.
Definition SOn.h:79
│ │ │ │ -
gtsam::SO::Lift
static SO Lift(size_t n, const Eigen::MatrixBase< Derived > &R)
Named constructor from lower dimensional matrix.
Definition SOn.h:101
│ │ │ │ -
gtsam::SO::Random
static SO Random(std::mt19937 &rng, size_t n=0)
Random SO(n) element (no big claims about uniformity). SO(3) is specialized in SO3....
Definition SOn.h:131
│ │ │ │ -
gtsam::SO::Identity
static SO Identity(size_t n=0)
SO<N> identity for N == Eigen::Dynamic.
Definition SOn.h:188
│ │ │ │ -
gtsam::SO::matrix
const MatrixNN & matrix() const
Return matrix.
Definition SOn.h:155
│ │ │ │ -
gtsam::SO::Random
static SO Random(std::mt19937 &rng)
Random SO(N) element (no big claims about uniformity)
Definition SOn.h:145
│ │ │ │ -
gtsam::SO::ClosestTo
static SO ClosestTo(const MatrixNN &M)
Named constructor that finds SO(n) matrix closest to M in Frobenius norm, currently only defined for ...
│ │ │ │ -
gtsam::SO::SO
SO(const Eigen::AngleAxisd &angleAxis)
Constructor from AngleAxisd.
Definition SOn.h:115
│ │ │ │ -
gtsam::SO::VectorizedGenerators
static Matrix VectorizedGenerators(size_t n=0)
Calculate n^2 x dim matrix of vectorized Lie algebra generators for SO(n)
Definition SOn.h:312
│ │ │ │ -
gtsam::SO::SO
SO(const Eigen::MatrixBase< Derived > &R)
Constructor from Eigen Matrix, dynamic version.
Definition SOn.h:91
│ │ │ │ -
gtsam::SO::ChartAtOrigin
Definition SOn.h:240
│ │ │ │ -
gtsam::SO::ChartAtOrigin::Local
static TangentVector Local(const SO &R, ChartJacobian H=boost::none)
Inverse of Retract.
Definition SOn-inl.h:50
│ │ │ │ -
gtsam::SO::ChartAtOrigin::Retract
static SO Retract(const TangentVector &xi, ChartJacobian H=boost::none)
Retract uses Cayley map.
Definition SOn-inl.h:40
│ │ │ │ +
gtsam::Similarity3
3D similarity transform
Definition Similarity3.h:36
│ │ │ │ +
gtsam::Similarity3::Dim
static size_t Dim()
Dimensionality of tangent space = 7 DOF - used to autodetect sizes.
Definition Similarity3.h:194
│ │ │ │ +
gtsam::Similarity3::translation
Point3 translation() const
Return a GTSAM translation.
Definition Similarity3.h:188
│ │ │ │ +
gtsam::Similarity3::rotation
Rot3 rotation() const
Return a GTSAM rotation.
Definition Similarity3.h:185
│ │ │ │ +
gtsam::Similarity3::dim
size_t dim() const
Dimensionality of tangent space = 7 DOF.
Definition Similarity3.h:197
│ │ │ │ +
gtsam::Similarity3::wedge
static Matrix4 wedge(const Vector7 &xi)
wedge for Similarity3:
Definition Similarity3.cpp:198
│ │ │ │ +
gtsam::Similarity3::scale
double scale() const
Return the scale.
Definition Similarity3.h:191
│ │ │ │ +
gtsam::Similarity3::ChartAtOrigin
Chart at the origin.
Definition Similarity3.h:153
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,507 +1,233 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SOn.h │ │ │ │ │ +Similarity3.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 19#pragma once │ │ │ │ │ 20 │ │ │ │ │ 21#include <_g_t_s_a_m_/_b_a_s_e_/_L_i_e_._h> │ │ │ │ │ 22#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_e_/_m_a_k_e___s_h_a_r_e_d_._h> │ │ │ │ │ -24#include │ │ │ │ │ -25#include │ │ │ │ │ -26 │ │ │ │ │ -27#include │ │ │ │ │ -28 │ │ │ │ │ -29#include // TODO(frank): how to avoid? │ │ │ │ │ -30#include │ │ │ │ │ -31#include │ │ │ │ │ -32#include │ │ │ │ │ -33#include │ │ │ │ │ -34 │ │ │ │ │ -35namespace _g_t_s_a_m { │ │ │ │ │ -36 │ │ │ │ │ -37namespace internal { │ │ │ │ │ -_3_9constexpr int _D_i_m_e_n_s_i_o_n_S_O(int N) { │ │ │ │ │ -40 return (N < 0) ? Eigen::Dynamic : N * (N - 1) / 2; │ │ │ │ │ -41} │ │ │ │ │ +23#include │ │ │ │ │ +24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_3_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_3_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_3_._h> │ │ │ │ │ +27 │ │ │ │ │ +28namespace _g_t_s_a_m { │ │ │ │ │ +29 │ │ │ │ │ +30// Forward declarations │ │ │ │ │ +31class Pose3; │ │ │ │ │ +32 │ │ │ │ │ +_3_6class GTSAM_EXPORT _S_i_m_i_l_a_r_i_t_y_3 : public _L_i_e_G_r_o_u_p { │ │ │ │ │ +39 typedef _R_o_t_3 _R_o_t_a_t_i_o_n; │ │ │ │ │ +40 typedef _P_o_i_n_t_3 Translation; │ │ │ │ │ 42 │ │ │ │ │ -43// Calculate N^2 at compile time, or return Dynamic if so │ │ │ │ │ -44constexpr int NSquaredSO(int N) { return (N < 0) ? Eigen::Dynamic : N * N; } │ │ │ │ │ -45} // namespace internal │ │ │ │ │ -46 │ │ │ │ │ -51template │ │ │ │ │ -_5_2class _S_O : public _L_i_e_G_r_o_u_p, internal::DimensionSO(N)> { │ │ │ │ │ -53 public: │ │ │ │ │ -54 enum { dimension = internal::DimensionSO(N) }; │ │ │ │ │ -55 using MatrixNN = Eigen::Matrix; │ │ │ │ │ -56 using VectorN2 = Eigen::Matrix; │ │ │ │ │ -57 using MatrixDD = Eigen::Matrix; │ │ │ │ │ -58 │ │ │ │ │ -59 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W___I_F(true) │ │ │ │ │ +43 private: │ │ │ │ │ +44 _R_o_t_3 R_; │ │ │ │ │ +45 _P_o_i_n_t_3 t_; │ │ │ │ │ +46 double s_; │ │ │ │ │ +47 │ │ │ │ │ +48 public: │ │ │ │ │ +51 │ │ │ │ │ +53 _S_i_m_i_l_a_r_i_t_y_3(); │ │ │ │ │ +54 │ │ │ │ │ +56 _S_i_m_i_l_a_r_i_t_y_3(double s); │ │ │ │ │ +57 │ │ │ │ │ +59 _S_i_m_i_l_a_r_i_t_y_3(const _R_o_t_3& R, const _P_o_i_n_t_3& t, double s); │ │ │ │ │ 60 │ │ │ │ │ -61 protected: │ │ │ │ │ -_6_2 MatrixNN _m_a_t_r_i_x__; │ │ │ │ │ +62 _S_i_m_i_l_a_r_i_t_y_3(const Matrix3& R, const Vector3& t, double s); │ │ │ │ │ 63 │ │ │ │ │ -64 // enable_if_t aliases, used to specialize constructors/methods, see │ │ │ │ │ -65 // https://www.fluentcpp.com/2018/05/18/make-sfinae-pretty-2-hidden-beauty- │ │ │ │ │ -sfinae/ │ │ │ │ │ -66 template │ │ │ │ │ -67 using IsDynamic = typename std::enable_if::type; │ │ │ │ │ -68 template │ │ │ │ │ -69 using IsFixed = typename std::enable_if= 2, void>::type; │ │ │ │ │ -70 template │ │ │ │ │ -71 using IsSO3 = typename std::enable_if::type; │ │ │ │ │ -72 │ │ │ │ │ -73 public: │ │ │ │ │ +65 _S_i_m_i_l_a_r_i_t_y_3(const Matrix4& T); │ │ │ │ │ +66 │ │ │ │ │ +70 │ │ │ │ │ +72 bool _e_q_u_a_l_s(const _S_i_m_i_l_a_r_i_t_y_3& sim, double tol) const; │ │ │ │ │ +73 │ │ │ │ │ +75 bool _o_p_e_r_a_t_o_r_=_=(const _S_i_m_i_l_a_r_i_t_y_3& other) const; │ │ │ │ │ 76 │ │ │ │ │ -78 template > │ │ │ │ │ -_7_9 _S_O() : _m_a_t_r_i_x__(MatrixNN::_I_d_e_n_t_i_t_y()) {} │ │ │ │ │ -80 │ │ │ │ │ -82 template > │ │ │ │ │ -_8_3 explicit _S_O(size_t n = 0) { │ │ │ │ │ -84 // We allow for n=0 as the default constructor, needed for serialization, │ │ │ │ │ -85 // wrappers etc. │ │ │ │ │ -86 _m_a_t_r_i_x__ = Eigen::MatrixXd::Identity(n, n); │ │ │ │ │ -87 } │ │ │ │ │ +78 void _p_r_i_n_t(const std::string& s) const; │ │ │ │ │ +79 │ │ │ │ │ +80 friend std::ostream& operator<<(std::ostream& os, const _S_i_m_i_l_a_r_i_t_y_3& p); │ │ │ │ │ +81 │ │ │ │ │ +85 │ │ │ │ │ +87 static _S_i_m_i_l_a_r_i_t_y_3 Identity(); │ │ │ │ │ 88 │ │ │ │ │ -90 template │ │ │ │ │ -_9_1 explicit _S_O(const Eigen::MatrixBase& R) : _m_a_t_r_i_x__(R.eval()) {} │ │ │ │ │ -92 │ │ │ │ │ -94 template │ │ │ │ │ -_9_5 static _S_O _F_r_o_m_M_a_t_r_i_x(const Eigen::MatrixBase& R) { │ │ │ │ │ -96 return _S_O(R); │ │ │ │ │ -97 } │ │ │ │ │ +90 _S_i_m_i_l_a_r_i_t_y_3 _o_p_e_r_a_t_o_r_*(const _S_i_m_i_l_a_r_i_t_y_3& S) const; │ │ │ │ │ +91 │ │ │ │ │ +93 _S_i_m_i_l_a_r_i_t_y_3 inverse() const; │ │ │ │ │ +94 │ │ │ │ │ 98 │ │ │ │ │ -100 template > │ │ │ │ │ -_1_0_1 static _S_O _L_i_f_t(size_t n, const Eigen::MatrixBase &R) { │ │ │ │ │ -102 Matrix Q = Matrix::Identity(n, n); │ │ │ │ │ -103 const int p = R.rows(); │ │ │ │ │ -104 assert(p >= 0 && p <= static_cast(n) && R.cols() == p); │ │ │ │ │ -105 Q.topLeftCorner(p, p) = R; │ │ │ │ │ -106 return _S_O(Q); │ │ │ │ │ -107 } │ │ │ │ │ -108 │ │ │ │ │ -110 template > │ │ │ │ │ -_1_1_1 explicit _S_O(const _S_O_<_M_>& R) : _m_a_t_r_i_x__(R._m_a_t_r_i_x()) {} │ │ │ │ │ -112 │ │ │ │ │ -114 template > │ │ │ │ │ -_1_1_5 explicit _S_O(const Eigen::AngleAxisd& angleAxis) : _m_a_t_r_i_x__(angleAxis) {} │ │ │ │ │ +100 _P_o_i_n_t_3 transformFrom(const _P_o_i_n_t_3& p, // │ │ │ │ │ +101 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _7_> H1 = boost::none, // │ │ │ │ │ +102 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H2 = boost::none) const; │ │ │ │ │ +103 │ │ │ │ │ +115 _P_o_s_e_3 transformFrom(const _P_o_s_e_3& T) const; │ │ │ │ │ 116 │ │ │ │ │ -_1_1_8 static _S_O _A_x_i_s_A_n_g_l_e(const Vector3& axis, double theta); │ │ │ │ │ +118 _P_o_i_n_t_3 _o_p_e_r_a_t_o_r_*(const _P_o_i_n_t_3& p) const; │ │ │ │ │ 119 │ │ │ │ │ -_1_2_2 static _S_O _C_l_o_s_e_s_t_T_o(const MatrixNN& M); │ │ │ │ │ -123 │ │ │ │ │ -_1_2_7 static _S_O _C_h_o_r_d_a_l_M_e_a_n(const std::vector& rotations); │ │ │ │ │ -128 │ │ │ │ │ -130 template > │ │ │ │ │ -_1_3_1 static _S_O _R_a_n_d_o_m(std::mt19937& rng, size_t n = 0) { │ │ │ │ │ -132 if (n == 0) throw std::runtime_error("SO: Dimensionality not known."); │ │ │ │ │ -133 // TODO(frank): this might need to be re-thought │ │ │ │ │ -134 static std::uniform_real_distribution randomAngle(-M_PI, M_PI); │ │ │ │ │ -135 const size_t d = SO::Dimension(n); │ │ │ │ │ -136 Vector xi(d); │ │ │ │ │ -137 for (size_t j = 0; j < d; j++) { │ │ │ │ │ -138 xi(j) = randomAngle(rng); │ │ │ │ │ -139 } │ │ │ │ │ -140 return _S_O_:_:_R_e_t_r_a_c_t(xi); │ │ │ │ │ -141 } │ │ │ │ │ -142 │ │ │ │ │ -144 template > │ │ │ │ │ -_1_4_5 static _S_O _R_a_n_d_o_m(std::mt19937& rng) { │ │ │ │ │ -146 // By default, use dynamic implementation above. Specialized for SO(3). │ │ │ │ │ -147 return _S_O(_S_O_<_E_i_g_e_n_:_:_D_y_n_a_m_i_c_>_:_:_R_a_n_d_o_m(rng, N)._m_a_t_r_i_x()); │ │ │ │ │ -148 } │ │ │ │ │ -149 │ │ │ │ │ -153 │ │ │ │ │ -_1_5_5 const MatrixNN& _m_a_t_r_i_x() const { return _m_a_t_r_i_x__; } │ │ │ │ │ -156 │ │ │ │ │ -157 size_t rows() const { return _m_a_t_r_i_x__.rows(); } │ │ │ │ │ -158 size_t cols() const { return _m_a_t_r_i_x__.cols(); } │ │ │ │ │ -159 │ │ │ │ │ +123 static _S_i_m_i_l_a_r_i_t_y_3 Align(const Point3Pairs& abPointPairs); │ │ │ │ │ +124 │ │ │ │ │ +135 static _S_i_m_i_l_a_r_i_t_y_3 Align(const std::vector& abPosePairs); │ │ │ │ │ +136 │ │ │ │ │ +140 │ │ │ │ │ +144 static Vector7 Logmap(const _S_i_m_i_l_a_r_i_t_y_3& s, // │ │ │ │ │ +145 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_7_,_ _7_> Hm = boost::none); │ │ │ │ │ +146 │ │ │ │ │ +149 static _S_i_m_i_l_a_r_i_t_y_3 Expmap(const Vector7& v, // │ │ │ │ │ +150 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_7_,_ _7_> Hm = boost::none); │ │ │ │ │ +151 │ │ │ │ │ +_1_5_3 struct _C_h_a_r_t_A_t_O_r_i_g_i_n { │ │ │ │ │ +154 static _S_i_m_i_l_a_r_i_t_y_3 Retract(const Vector7& v, │ │ │ │ │ +155 ChartJacobian H = boost::none) { │ │ │ │ │ +156 return Similarity3::Expmap(v, H); │ │ │ │ │ +157 } │ │ │ │ │ +158 static Vector7 Local(const _S_i_m_i_l_a_r_i_t_y_3& other, │ │ │ │ │ +159 ChartJacobian H = boost::none) { │ │ │ │ │ +160 return Similarity3::Logmap(other, H); │ │ │ │ │ +161 } │ │ │ │ │ +162 }; │ │ │ │ │ 163 │ │ │ │ │ -164 void print(const std::string& s = std::string()) const; │ │ │ │ │ +164 using _L_i_e_G_r_o_u_p<_S_i_m_i_l_a_r_i_t_y_3, 7>::inverse; │ │ │ │ │ 165 │ │ │ │ │ -166 bool equals(const _S_O& other, double tol) const { │ │ │ │ │ -167 return _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(_m_a_t_r_i_x__, other.matrix_, tol); │ │ │ │ │ -168 } │ │ │ │ │ -169 │ │ │ │ │ +172 static Matrix4 _w_e_d_g_e(const Vector7& xi); │ │ │ │ │ 173 │ │ │ │ │ -_1_7_5 _S_O _o_p_e_r_a_t_o_r_*(const _S_O& other) const { │ │ │ │ │ -176 assert(dim() == other.dim()); │ │ │ │ │ -177 return _S_O(_m_a_t_r_i_x__ * other._m_a_t_r_i_x__); │ │ │ │ │ -178 } │ │ │ │ │ -179 │ │ │ │ │ -181 template > │ │ │ │ │ -_1_8_2 static _S_O _I_d_e_n_t_i_t_y() { │ │ │ │ │ -183 return _S_O(); │ │ │ │ │ -184 } │ │ │ │ │ -185 │ │ │ │ │ -187 template > │ │ │ │ │ -_1_8_8 static _S_O _I_d_e_n_t_i_t_y(size_t n = 0) { │ │ │ │ │ -189 return _S_O(n); │ │ │ │ │ -190 } │ │ │ │ │ -191 │ │ │ │ │ -_1_9_3 _S_O _i_n_v_e_r_s_e() const { return _S_O(_m_a_t_r_i_x__.transpose()); } │ │ │ │ │ -194 │ │ │ │ │ +175 Matrix7 AdjointMap() const; │ │ │ │ │ +176 │ │ │ │ │ +180 │ │ │ │ │ +182 Matrix4 matrix() const; │ │ │ │ │ +183 │ │ │ │ │ +_1_8_5 _R_o_t_3 _r_o_t_a_t_i_o_n() const { return R_; } │ │ │ │ │ +186 │ │ │ │ │ +_1_8_8 _P_o_i_n_t_3 _t_r_a_n_s_l_a_t_i_o_n() const { return t_; } │ │ │ │ │ +189 │ │ │ │ │ +_1_9_1 double _s_c_a_l_e() const { return s_; } │ │ │ │ │ +192 │ │ │ │ │ +_1_9_4 inline static size_t _D_i_m() { return 7; } │ │ │ │ │ +195 │ │ │ │ │ +_1_9_7 inline size_t _d_i_m() const { return 7; } │ │ │ │ │ 198 │ │ │ │ │ -199 using TangentVector = Eigen::Matrix; │ │ │ │ │ -200 using ChartJacobian = _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_d_i_m_e_n_s_i_o_n_,_ _d_i_m_e_n_s_i_o_n_>; │ │ │ │ │ -201 │ │ │ │ │ -_2_0_3 static int _D_i_m() { return dimension; } │ │ │ │ │ -204 │ │ │ │ │ -205 // Calculate manifold dimensionality for SO(n). │ │ │ │ │ -206 // Available as dimension or Dim() for fixed N. │ │ │ │ │ -207 static size_t Dimension(size_t n) { return n * (n - 1) / 2; } │ │ │ │ │ -208 │ │ │ │ │ -209 // Calculate ambient dimension n from manifold dimensionality d. │ │ │ │ │ -210 static size_t AmbientDim(size_t d) { return (1 + std::sqrt(1 + 8 * d)) / 2; │ │ │ │ │ -} │ │ │ │ │ -211 │ │ │ │ │ -212 // Calculate run-time dimensionality of manifold. │ │ │ │ │ -213 // Available as dimension or Dim() for fixed N. │ │ │ │ │ -214 size_t dim() const { return Dimension(static_cast(_m_a_t_r_i_x__.rows())); │ │ │ │ │ -} │ │ │ │ │ -215 │ │ │ │ │ -231 static MatrixNN _H_a_t(const TangentVector& xi); │ │ │ │ │ -232 │ │ │ │ │ -_2_3_4 static void _H_a_t(const Vector &xi, Eigen::Ref X); │ │ │ │ │ -235 │ │ │ │ │ -237 static TangentVector _V_e_e(const MatrixNN& X); │ │ │ │ │ -238 │ │ │ │ │ -239 // Chart at origin │ │ │ │ │ -_2_4_0 struct _C_h_a_r_t_A_t_O_r_i_g_i_n { │ │ │ │ │ -245 static _S_O _R_e_t_r_a_c_t(const TangentVector& xi, _C_h_a_r_t_J_a_c_o_b_i_a_n H = boost::none); │ │ │ │ │ -246 │ │ │ │ │ -250 static TangentVector _L_o_c_a_l(const _S_O& R, _C_h_a_r_t_J_a_c_o_b_i_a_n H = boost::none); │ │ │ │ │ -251 }; │ │ │ │ │ -252 │ │ │ │ │ -253 // Return dynamic identity DxD Jacobian for given SO(n) │ │ │ │ │ -254 template > │ │ │ │ │ -255 static MatrixDD IdentityJacobian(size_t n) { │ │ │ │ │ -256 const size_t d = Dimension(n); │ │ │ │ │ -257 return MatrixDD::Identity(d, d); │ │ │ │ │ -258 } │ │ │ │ │ -259 │ │ │ │ │ -263 │ │ │ │ │ -265 MatrixDD _A_d_j_o_i_n_t_M_a_p() const; │ │ │ │ │ -266 │ │ │ │ │ -270 static _S_O _E_x_p_m_a_p(const TangentVector& omega, ChartJacobian H = boost:: │ │ │ │ │ -none); │ │ │ │ │ -271 │ │ │ │ │ -273 static MatrixDD _E_x_p_m_a_p_D_e_r_i_v_a_t_i_v_e(const TangentVector& omega); │ │ │ │ │ -274 │ │ │ │ │ -278 static TangentVector _L_o_g_m_a_p(const _S_O& R, ChartJacobian H = boost::none); │ │ │ │ │ -279 │ │ │ │ │ -281 static MatrixDD _L_o_g_m_a_p_D_e_r_i_v_a_t_i_v_e(const TangentVector& omega); │ │ │ │ │ -282 │ │ │ │ │ -283 // inverse with optional derivative │ │ │ │ │ -284 using _L_i_e_G_r_o_u_p<_S_O_<_N_>, internal::DimensionSO(N)>_:_:_i_n_v_e_r_s_e; │ │ │ │ │ -285 │ │ │ │ │ -289 │ │ │ │ │ -295 VectorN2 _v_e_c(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n H = │ │ │ │ │ -296 boost::none) const; │ │ │ │ │ -297 │ │ │ │ │ -299 template > │ │ │ │ │ -_3_0_0 static Matrix _V_e_c_t_o_r_i_z_e_d_G_e_n_e_r_a_t_o_r_s() { │ │ │ │ │ -301 constexpr size_t N2 = static_cast(N * N); │ │ │ │ │ -302 Eigen::Matrix G; │ │ │ │ │ -303 for (size_t j = 0; j < dimension; j++) { │ │ │ │ │ -304 const auto X = _H_a_t(Vector::Unit(dimension, j)); │ │ │ │ │ -305 G.col(j) = Eigen::Map(X.data()); │ │ │ │ │ -306 } │ │ │ │ │ -307 return G; │ │ │ │ │ -308 } │ │ │ │ │ -309 │ │ │ │ │ -311 template > │ │ │ │ │ -_3_1_2 static Matrix _V_e_c_t_o_r_i_z_e_d_G_e_n_e_r_a_t_o_r_s(size_t n = 0) { │ │ │ │ │ -313 const size_t n2 = n * n, dim = Dimension(n); │ │ │ │ │ -314 Matrix G(n2, dim); │ │ │ │ │ -315 for (size_t j = 0; j < dim; j++) { │ │ │ │ │ -316 const auto X = _H_a_t(Vector::Unit(dim, j)); │ │ │ │ │ -317 G.col(j) = Eigen::Map(X.data(), n2, 1); │ │ │ │ │ -318 } │ │ │ │ │ -319 return G; │ │ │ │ │ -320 } │ │ │ │ │ -321 │ │ │ │ │ -325 │ │ │ │ │ -326 template │ │ │ │ │ -327 friend void save(Archive&, _S_O&, const unsigned int); │ │ │ │ │ -328 template │ │ │ │ │ -329 friend void load(Archive&, _S_O&, const unsigned int); │ │ │ │ │ -330 template │ │ │ │ │ -331 friend void serialize(Archive&, _S_O&, const unsigned int); │ │ │ │ │ -332 friend class boost::serialization::access; │ │ │ │ │ -333 friend class _R_o_t_3; // for serialize │ │ │ │ │ -334 │ │ │ │ │ -336}; │ │ │ │ │ -337 │ │ │ │ │ -338using SOn = SO; │ │ │ │ │ -339 │ │ │ │ │ -340/* │ │ │ │ │ -341 * Specialize dynamic Hat and Vee, because recursion depends on dynamic │ │ │ │ │ -nature. │ │ │ │ │ -342 * The definition is in SOn.cpp. Fixed-size SO3 and SO4 have their own │ │ │ │ │ -version, │ │ │ │ │ -343 * and implementation for other fixed N is in SOn-inl.h. │ │ │ │ │ -344 */ │ │ │ │ │ -345 │ │ │ │ │ -346template <> │ │ │ │ │ -347GTSAM_EXPORT │ │ │ │ │ -348Matrix _S_O_n_:_:_H_a_t(const Vector& xi); │ │ │ │ │ -349 │ │ │ │ │ -350template <> │ │ │ │ │ -351GTSAM_EXPORT │ │ │ │ │ -352Vector _S_O_n_:_:_V_e_e(const Matrix& X); │ │ │ │ │ -353 │ │ │ │ │ -354/* │ │ │ │ │ -355 * Specialize dynamic compose and between, because the derivative is │ │ │ │ │ -unknowable │ │ │ │ │ -356 * by the LieGroup implementations, who return a fixed-size matrix for H2. │ │ │ │ │ -357 */ │ │ │ │ │ -358 │ │ │ │ │ -359using DynamicJacobian = OptionalJacobian; │ │ │ │ │ -360 │ │ │ │ │ -361template <> │ │ │ │ │ -362GTSAM_EXPORT │ │ │ │ │ -363SOn LieGroup::compose(const SOn& g, DynamicJacobian H1, │ │ │ │ │ -364 DynamicJacobian H2) const; │ │ │ │ │ -365 │ │ │ │ │ -366template <> │ │ │ │ │ -367GTSAM_EXPORT │ │ │ │ │ -368SOn LieGroup::between(const SOn& g, DynamicJacobian H1, │ │ │ │ │ -369 DynamicJacobian H2) const; │ │ │ │ │ -370 │ │ │ │ │ -371/* │ │ │ │ │ -372 * Specialize dynamic vec. │ │ │ │ │ -373 */ │ │ │ │ │ -374template <> │ │ │ │ │ -375GTSAM_EXPORT │ │ │ │ │ -376typename SOn::VectorN2 _S_O_n_:_:_v_e_c(DynamicJacobian H) const; │ │ │ │ │ -377 │ │ │ │ │ -379template │ │ │ │ │ -_3_8_0void _s_e_r_i_a_l_i_z_e( │ │ │ │ │ -381 Archive& ar, _S_O_n& Q, │ │ │ │ │ -382 const unsigned int file_version │ │ │ │ │ -383) { │ │ │ │ │ -384 Matrix& M = Q.matrix_; │ │ │ │ │ -385 ar& BOOST_SERIALIZATION_NVP(M); │ │ │ │ │ -386} │ │ │ │ │ -387 │ │ │ │ │ -388/* │ │ │ │ │ -389 * Define the traits. internal::LieGroup provides both Lie group and │ │ │ │ │ -Testable │ │ │ │ │ -390 */ │ │ │ │ │ -391 │ │ │ │ │ -392template │ │ │ │ │ -_3_9_3struct _t_r_a_i_t_s<_S_O> : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p> {}; │ │ │ │ │ -394 │ │ │ │ │ -395template │ │ │ │ │ -_3_9_6struct _t_r_a_i_t_s> : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p> {}; │ │ │ │ │ -397 │ │ │ │ │ -398} // namespace gtsam │ │ │ │ │ -399 │ │ │ │ │ -400#include "_S_O_n_-_i_n_l_._h" │ │ │ │ │ -_L_i_e_._h │ │ │ │ │ -Base class and basic functions for Lie types. │ │ │ │ │ -_m_a_k_e___s_h_a_r_e_d_._h │ │ │ │ │ -make_shared trampoline function to ensure proper alignment │ │ │ │ │ +202 │ │ │ │ │ +203 private: │ │ │ │ │ +205 static Matrix3 GetV(Vector3 w, double lambda); │ │ │ │ │ +206 │ │ │ │ │ +208}; │ │ │ │ │ +209 │ │ │ │ │ +210template <> │ │ │ │ │ +211inline Matrix wedge(const Vector& xi) { │ │ │ │ │ +212 return _S_i_m_i_l_a_r_i_t_y_3_:_:_w_e_d_g_e(xi); │ │ │ │ │ +213} │ │ │ │ │ +214 │ │ │ │ │ +215template <> │ │ │ │ │ +_2_1_6struct _t_r_a_i_t_s<_S_i_m_i_l_a_r_i_t_y_3> : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ +217 │ │ │ │ │ +218template <> │ │ │ │ │ +_2_1_9struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ +{}; │ │ │ │ │ +220 │ │ │ │ │ +221} // namespace gtsam │ │ │ │ │ _M_a_n_i_f_o_l_d_._h │ │ │ │ │ Base class and basic functions for Manifold types. │ │ │ │ │ -_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W___I_F │ │ │ │ │ -#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) │ │ │ │ │ -This marks a GTSAM object to require alignment. │ │ │ │ │ -DDeeffiinniittiioonn types.h:317 │ │ │ │ │ -_S_O_n_-_i_n_l_._h │ │ │ │ │ -Template implementations for SO(n) │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_D_i_m_e_n_s_i_o_n_S_O │ │ │ │ │ -constexpr int DimensionSO(int N) │ │ │ │ │ -Calculate dimensionality of SO manifold, or return Dynamic if so. │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:39 │ │ │ │ │ +_L_i_e_._h │ │ │ │ │ +Base class and basic functions for Lie types. │ │ │ │ │ +_R_o_t_3_._h │ │ │ │ │ +3D rotation represented as a rotation matrix or quaternion │ │ │ │ │ +_P_o_s_e_3_._h │ │ │ │ │ +3D Pose │ │ │ │ │ +_P_o_i_n_t_3_._h │ │ │ │ │ +3D Point │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e │ │ │ │ │ -std::string serialize(const T &input) │ │ │ │ │ -serializes to a string │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:113 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l___w_i_t_h___a_b_s___t_o_l │ │ │ │ │ -bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen:: │ │ │ │ │ -DenseBase< MATRIX > &B, double tol=1e-9) │ │ │ │ │ -equals with a tolerance │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:81 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ +Point2 operator*(double s, const Point2 &p) │ │ │ │ │ +multiply with scalar │ │ │ │ │ +DDeeffiinniittiioonn Point2.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ +Vector3 Point3 │ │ │ │ │ +As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ +typedef Point3 to Vector3... │ │ │ │ │ +DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_=_= │ │ │ │ │ +bool operator==(const Matrix &A, const Matrix &B) │ │ │ │ │ +equality is just equal_with_abs_tol 1e-9 │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_w_e_d_g_e │ │ │ │ │ +Matrix wedge(const Vector &x) │ │ │ │ │ +Declaration of wedge (see Murray94book) used to convert from n exponential │ │ │ │ │ +coordinates to n*n element... │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ _g_t_s_a_m_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ A CRTP helper class that implements Lie group methods Prerequisites: methods │ │ │ │ │ operator*,... │ │ │ │ │ DDeeffiinniittiioonn Lie.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_<_ _S_O_<_ _N_ _>_,_ _i_n_t_e_r_n_a_l_:_:_D_i_m_e_n_s_i_o_n_S_O_(_N_)_>_:_:_R_e_t_r_a_c_t │ │ │ │ │ -static SO< N > Retract(const TangentVector &v) │ │ │ │ │ -Retract at origin: possible in Lie group because it has an identity. │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:111 │ │ │ │ │ _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ Both LieGroupTraits and Testable. │ │ │ │ │ DDeeffiinniittiioonn Lie.h:229 │ │ │ │ │ _g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ either a fixed size o... │ │ │ │ │ DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ +A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ _g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor │ │ │ │ │ symbol GTSAM_USE_QUATERNIO... │ │ │ │ │ DDeeffiinniittiioonn Rot3.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O │ │ │ │ │ -Manifold of special orthogonal rotation matrices SO. │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_F_r_o_m_M_a_t_r_i_x │ │ │ │ │ -static SO FromMatrix(const Eigen::MatrixBase< Derived > &R) │ │ │ │ │ -Named constructor from Eigen Matrix. │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:95 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_E_x_p_m_a_p │ │ │ │ │ -static SO Expmap(const TangentVector &omega, ChartJacobian H=boost::none) │ │ │ │ │ -Exponential map at identity - create a rotation from canonical coordinates. │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_V_e_c_t_o_r_i_z_e_d_G_e_n_e_r_a_t_o_r_s │ │ │ │ │ -static Matrix VectorizedGenerators() │ │ │ │ │ -Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N) │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:300 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_i_n_v_e_r_s_e │ │ │ │ │ -SO inverse() const │ │ │ │ │ -inverse of a rotation = transpose │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:193 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_v_e_c │ │ │ │ │ -VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost:: │ │ │ │ │ -none) const │ │ │ │ │ -Return vectorized rotation matrix in column order. │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:88 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_C_h_o_r_d_a_l_M_e_a_n │ │ │ │ │ -static SO ChordalMean(const std::vector< SO > &rotations) │ │ │ │ │ -Named constructor that finds chordal mean , currently only defined for SO3. │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ -SO operator*(const SO &other) const │ │ │ │ │ -Multiplication. │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:175 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_V_e_e │ │ │ │ │ -static TangentVector Vee(const MatrixNN &X) │ │ │ │ │ -Inverse of Hat. See note about xi element order in Hat. │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_m_a_t_r_i_x__ │ │ │ │ │ -MatrixNN matrix_ │ │ │ │ │ -Rotation matrix. │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:62 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_L_o_g_m_a_p │ │ │ │ │ -static TangentVector Logmap(const SO &R, ChartJacobian H=boost::none) │ │ │ │ │ -Log map at identity - returns the canonical coordinates of this rotation. │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_A_x_i_s_A_n_g_l_e │ │ │ │ │ -static SO AxisAngle(const Vector3 &axis, double theta) │ │ │ │ │ -Constructor from axis and angle. Only defined for SO3. │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_A_d_j_o_i_n_t_M_a_p │ │ │ │ │ -MatrixDD AdjointMap() const │ │ │ │ │ -Adjoint map. │ │ │ │ │ -DDeeffiinniittiioonn SO4.cpp:159 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_H_a_t │ │ │ │ │ -static void Hat(const Vector &xi, Eigen::Ref< MatrixNN > X) │ │ │ │ │ -In-place version of Hat (see details there), implements recursion. │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_I_d_e_n_t_i_t_y │ │ │ │ │ -static SO Identity() │ │ │ │ │ -SO identity for N >= 2. │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:182 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_D_i_m │ │ │ │ │ -static int Dim() │ │ │ │ │ -Return compile-time dimensionality: fixed size N or Eigen::Dynamic. │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:203 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_H_a_t │ │ │ │ │ -static MatrixNN Hat(const TangentVector &xi) │ │ │ │ │ -Hat operator creates Lie algebra element corresponding to d-vector, where d is │ │ │ │ │ -the dimensionality of ... │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:29 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_E_x_p_m_a_p_D_e_r_i_v_a_t_i_v_e │ │ │ │ │ -static MatrixDD ExpmapDerivative(const TangentVector &omega) │ │ │ │ │ -Derivative of Expmap, currently only defined for SO3. │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:72 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_L_o_g_m_a_p_D_e_r_i_v_a_t_i_v_e │ │ │ │ │ -static MatrixDD LogmapDerivative(const TangentVector &omega) │ │ │ │ │ -Derivative of Logmap, currently only defined for SO3. │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:82 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_S_O │ │ │ │ │ -SO(const SO< M > &R) │ │ │ │ │ -Construct dynamic SO(n) from Fixed SO │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_S_O │ │ │ │ │ -SO(size_t n=0) │ │ │ │ │ -Construct SO identity for N == Eigen::Dynamic. │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:83 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_S_O │ │ │ │ │ -SO() │ │ │ │ │ -Construct SO identity for N >= 2. │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:79 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_L_i_f_t │ │ │ │ │ -static SO Lift(size_t n, const Eigen::MatrixBase< Derived > &R) │ │ │ │ │ -Named constructor from lower dimensional matrix. │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:101 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_R_a_n_d_o_m │ │ │ │ │ -static SO Random(std::mt19937 &rng, size_t n=0) │ │ │ │ │ -Random SO(n) element (no big claims about uniformity). SO(3) is specialized in │ │ │ │ │ -SO3.... │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:131 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_I_d_e_n_t_i_t_y │ │ │ │ │ -static SO Identity(size_t n=0) │ │ │ │ │ -SO identity for N == Eigen::Dynamic. │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:188 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_m_a_t_r_i_x │ │ │ │ │ -const MatrixNN & matrix() const │ │ │ │ │ -Return matrix. │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:155 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_R_a_n_d_o_m │ │ │ │ │ -static SO Random(std::mt19937 &rng) │ │ │ │ │ -Random SO(N) element (no big claims about uniformity) │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:145 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_C_l_o_s_e_s_t_T_o │ │ │ │ │ -static SO ClosestTo(const MatrixNN &M) │ │ │ │ │ -Named constructor that finds SO(n) matrix closest to M in Frobenius norm, │ │ │ │ │ -currently only defined for ... │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_S_O │ │ │ │ │ -SO(const Eigen::AngleAxisd &angleAxis) │ │ │ │ │ -Constructor from AngleAxisd. │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:115 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_V_e_c_t_o_r_i_z_e_d_G_e_n_e_r_a_t_o_r_s │ │ │ │ │ -static Matrix VectorizedGenerators(size_t n=0) │ │ │ │ │ -Calculate n^2 x dim matrix of vectorized Lie algebra generators for SO(n) │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:312 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_S_O │ │ │ │ │ -SO(const Eigen::MatrixBase< Derived > &R) │ │ │ │ │ -Constructor from Eigen Matrix, dynamic version. │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:91 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:240 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n_:_:_L_o_c_a_l │ │ │ │ │ -static TangentVector Local(const SO &R, ChartJacobian H=boost::none) │ │ │ │ │ -Inverse of Retract. │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n_:_:_R_e_t_r_a_c_t │ │ │ │ │ -static SO Retract(const TangentVector &xi, ChartJacobian H=boost::none) │ │ │ │ │ -Retract uses Cayley map. │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_3 │ │ │ │ │ +3D similarity transform │ │ │ │ │ +DDeeffiinniittiioonn Similarity3.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_3_:_:_D_i_m │ │ │ │ │ +static size_t Dim() │ │ │ │ │ +Dimensionality of tangent space = 7 DOF - used to autodetect sizes. │ │ │ │ │ +DDeeffiinniittiioonn Similarity3.h:194 │ │ │ │ │ +_g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_3_:_:_t_r_a_n_s_l_a_t_i_o_n │ │ │ │ │ +Point3 translation() const │ │ │ │ │ +Return a GTSAM translation. │ │ │ │ │ +DDeeffiinniittiioonn Similarity3.h:188 │ │ │ │ │ +_g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_3_:_:_r_o_t_a_t_i_o_n │ │ │ │ │ +Rot3 rotation() const │ │ │ │ │ +Return a GTSAM rotation. │ │ │ │ │ +DDeeffiinniittiioonn Similarity3.h:185 │ │ │ │ │ +_g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_3_:_:_d_i_m │ │ │ │ │ +size_t dim() const │ │ │ │ │ +Dimensionality of tangent space = 7 DOF. │ │ │ │ │ +DDeeffiinniittiioonn Similarity3.h:197 │ │ │ │ │ +_g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_3_:_:_w_e_d_g_e │ │ │ │ │ +static Matrix4 wedge(const Vector7 &xi) │ │ │ │ │ +wedge for Similarity3: │ │ │ │ │ +DDeeffiinniittiioonn Similarity3.cpp:198 │ │ │ │ │ +_g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_3_:_:_s_c_a_l_e │ │ │ │ │ +double scale() const │ │ │ │ │ +Return the scale. │ │ │ │ │ +DDeeffiinniittiioonn Similarity3.h:191 │ │ │ │ │ +_g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_3_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n │ │ │ │ │ +Chart at the origin. │ │ │ │ │ +DDeeffiinniittiioonn Similarity3.h:153 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_O_n_._h │ │ │ │ │ + * _S_i_m_i_l_a_r_i_t_y_3_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00314.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Bundler.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,50 +95,51 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
Cal3Bundler.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
Cal3.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Calibration used by Bundler. │ │ │ │ +

Common code for all Calibration models. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::Cal3Bundler
 Calibration used by Bundler. More...
 
struct  gtsam::traits< Cal3Bundler >
 
struct  gtsam::traits< const Cal3Bundler >
class  gtsam::Cal3
 Common base class for all calibration models. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

template<typename Cal , size_t Dim>
void gtsam::calibrateJacobians (const Cal &calibration, const Point2 &pn, OptionalJacobian< 2, Dim > Dcal=boost::none, OptionalJacobian< 2, 2 > Dp=boost::none)
 Function which makes use of the Implicit Function Theorem to compute the Jacobians of calibrate using uncalibrate.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Calibration used by Bundler.

│ │ │ │ -
Date
Sep 25, 2010
│ │ │ │ -
Author
Yong Dian Jian
│ │ │ │ -
│ │ │ │ -Varun Agrawal
│ │ │ │ +

Common code for all Calibration models.

│ │ │ │ +
Author
Varun Agrawal
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,32 +1,33 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Cal3Bundler.h File Reference │ │ │ │ │ -Calibration used by Bundler. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Cal3.h File Reference │ │ │ │ │ +Common code for all Calibration models. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_C_a_l_3_B_u_n_d_l_e_r │ │ │ │ │ -  Calibration used by Bundler. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _C_a_l_3_B_u_n_d_l_e_r_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _C_a_l_3_B_u_n_d_l_e_r_ _> │ │ │ │ │ +class   _g_t_s_a_m_:_:_C_a_l_3 │ │ │ │ │ +  Common base class for all calibration models. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +template │ │ │ │ │ +void  _g_t_s_a_m_:_:_c_a_l_i_b_r_a_t_e_J_a_c_o_b_i_a_n_s (const Cal &calibration, const _P_o_i_n_t_2 &pn, │ │ │ │ │ + _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 2, Dim > Dcal=boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 2, 2 > │ │ │ │ │ + Dp=boost::none) │ │ │ │ │ +  Function which makes use of the Implicit Function Theorem to compute the │ │ │ │ │ + Jacobians of calibrate using uncalibrate. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Calibration used by Bundler. │ │ │ │ │ - Date │ │ │ │ │ - Sep 25, 2010 │ │ │ │ │ +Common code for all Calibration models. │ │ │ │ │ Author │ │ │ │ │ - Yong Dian Jian │ │ │ │ │ Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3_B_u_n_d_l_e_r_._h │ │ │ │ │ + * _C_a_l_3_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00314.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,3 @@ │ │ │ │ │ var a00314 = [ │ │ │ │ │ - ["gtsam::traits< Cal3Bundler >", "a02972.html", null], │ │ │ │ │ - ["gtsam::traits< const Cal3Bundler >", "a02976.html", null] │ │ │ │ │ + ["calibrateJacobians", "a00314.html#a9ac814fcc4484bce32e4616d69afb225", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00314_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Bundler.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,162 +98,177 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Cal3Bundler.h
│ │ │ │ +
Cal3.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/geometry/Cal3.h>
│ │ │ │ -
23#include <gtsam/geometry/Point2.h>
│ │ │ │ -
24
│ │ │ │ -
25namespace gtsam {
│ │ │ │ -
26
│ │ │ │ -
│ │ │ │ -
32class GTSAM_EXPORT Cal3Bundler : public Cal3 {
│ │ │ │ -
33 private:
│ │ │ │ -
34 double k1_ = 0.0f, k2_ = 0.0f;
│ │ │ │ -
35 double tol_ = 1e-5;
│ │ │ │ -
36
│ │ │ │ -
37 // NOTE: We use the base class fx to represent the common focal length.
│ │ │ │ -
38 // Also, image center parameters (u0, v0) are not optimized
│ │ │ │ -
39 // but are treated as constants.
│ │ │ │ -
40
│ │ │ │ -
41 public:
│ │ │ │ -
42 enum { dimension = 3 };
│ │ │ │ -
43
│ │ │ │ -
45 using shared_ptr = boost::shared_ptr<Cal3Bundler>;
│ │ │ │ -
46
│ │ │ │ -
49
│ │ │ │ -
51 Cal3Bundler() = default;
│ │ │ │ -
52
│ │ │ │ -
│ │ │ │ -
62 Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0,
│ │ │ │ -
63 double tol = 1e-5)
│ │ │ │ -
64 : Cal3(f, f, 0, u0, v0), k1_(k1), k2_(k2), tol_(tol) {}
│ │ │ │ -
│ │ │ │ -
65
│ │ │ │ -
66 ~Cal3Bundler() override {}
│ │ │ │ -
67
│ │ │ │ -
71
│ │ │ │ -
73 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
│ │ │ │ -
74 const Cal3Bundler& cal);
│ │ │ │ -
75
│ │ │ │ -
77 void print(const std::string& s = "") const override;
│ │ │ │ -
78
│ │ │ │ -
80 bool equals(const Cal3Bundler& K, double tol = 10e-9) const;
│ │ │ │ -
81
│ │ │ │ +
22#pragma once
│ │ │ │ +
23
│ │ │ │ +
24#include <gtsam/geometry/Point2.h>
│ │ │ │ +
25
│ │ │ │ +
26namespace gtsam {
│ │ │ │ +
27
│ │ │ │ +
46template <typename Cal, size_t Dim>
│ │ │ │ +
│ │ │ │ +
47void calibrateJacobians(const Cal& calibration, const Point2& pn,
│ │ │ │ +
48 OptionalJacobian<2, Dim> Dcal = boost::none,
│ │ │ │ +
49 OptionalJacobian<2, 2> Dp = boost::none) {
│ │ │ │ +
50 if (Dcal || Dp) {
│ │ │ │ +
51 Eigen::Matrix<double, 2, Dim> H_uncal_K;
│ │ │ │ +
52 Matrix22 H_uncal_pn, H_uncal_pn_inv;
│ │ │ │ +
53
│ │ │ │ +
54 // Compute uncalibrate Jacobians
│ │ │ │ +
55 calibration.uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
│ │ │ │ +
56
│ │ │ │ +
57 H_uncal_pn_inv = H_uncal_pn.inverse();
│ │ │ │ +
58
│ │ │ │ +
59 if (Dp) *Dp = H_uncal_pn_inv;
│ │ │ │ +
60 if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
│ │ │ │ +
61 }
│ │ │ │ +
62}
│ │ │ │ +
│ │ │ │ +
63
│ │ │ │ +
│ │ │ │ +
69class GTSAM_EXPORT Cal3 {
│ │ │ │ +
70 protected:
│ │ │ │ +
71 double fx_ = 1.0f, fy_ = 1.0f;
│ │ │ │ +
72 double s_ = 0.0f;
│ │ │ │ +
73 double u0_ = 0.0f, v0_ = 0.0f;
│ │ │ │ +
74
│ │ │ │ +
75 public:
│ │ │ │ +
76 enum { dimension = 5 };
│ │ │ │ +
78 using shared_ptr = boost::shared_ptr<Cal3>;
│ │ │ │ +
79
│ │ │ │ +
82
│ │ │ │ +
84 Cal3() = default;
│ │ │ │
85
│ │ │ │ -
87 inline double k1() const { return k1_; }
│ │ │ │ -
88
│ │ │ │ -
90 inline double k2() const { return k2_; }
│ │ │ │ -
91
│ │ │ │ -
93 inline double px() const { return u0_; }
│ │ │ │ -
94
│ │ │ │ -
96 inline double py() const { return v0_; }
│ │ │ │ -
97
│ │ │ │ -
98 Matrix3 K() const override;
│ │ │ │ -
99 Vector4 k() const;
│ │ │ │ -
100
│ │ │ │ -
101 Vector3 vector() const;
│ │ │ │ -
102
│ │ │ │ -
103#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ -
105 inline double GTSAM_DEPRECATED u0() const { return u0_; }
│ │ │ │ -
106
│ │ │ │ -
108 inline double GTSAM_DEPRECATED v0() const { return v0_; }
│ │ │ │ -
109#endif
│ │ │ │ -
110
│ │ │ │ -
119 Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none,
│ │ │ │ -
120 OptionalJacobian<2, 2> Dp = boost::none) const;
│ │ │ │ -
121
│ │ │ │ -
130 Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = boost::none,
│ │ │ │ -
131 OptionalJacobian<2, 2> Dp = boost::none) const;
│ │ │ │ -
132
│ │ │ │ -
134 Matrix2 D2d_intrinsic(const Point2& p) const;
│ │ │ │ -
135
│ │ │ │ -
137 Matrix23 D2d_calibration(const Point2& p) const;
│ │ │ │ -
138
│ │ │ │ -
140 Matrix25 D2d_intrinsic_calibration(const Point2& p) const;
│ │ │ │ -
141
│ │ │ │ -
145
│ │ │ │ -
147 size_t dim() const override { return Dim(); }
│ │ │ │ -
148
│ │ │ │ -
150 inline static size_t Dim() { return dimension; }
│ │ │ │ -
151
│ │ │ │ -
│ │ │ │ -
153 inline Cal3Bundler retract(const Vector& d) const {
│ │ │ │ -
154 return Cal3Bundler(fx_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_);
│ │ │ │ -
155 }
│ │ │ │ -
│ │ │ │ -
156
│ │ │ │ -
│ │ │ │ -
158 Vector3 localCoordinates(const Cal3Bundler& T2) const {
│ │ │ │ -
159 return T2.vector() - vector();
│ │ │ │ -
160 }
│ │ │ │ -
│ │ │ │ -
161
│ │ │ │ -
162 private:
│ │ │ │ -
166
│ │ │ │ -
168 friend class boost::serialization::access;
│ │ │ │ -
169 template <class Archive>
│ │ │ │ -
170 void serialize(Archive& ar, const unsigned int /*version*/) {
│ │ │ │ -
171 ar& boost::serialization::make_nvp(
│ │ │ │ -
172 "Cal3Bundler", boost::serialization::base_object<Cal3>(*this));
│ │ │ │ -
173 ar& BOOST_SERIALIZATION_NVP(k1_);
│ │ │ │ -
174 ar& BOOST_SERIALIZATION_NVP(k2_);
│ │ │ │ -
175 ar& BOOST_SERIALIZATION_NVP(tol_);
│ │ │ │ -
176 }
│ │ │ │ +
│ │ │ │ +
87 Cal3(double fx, double fy, double s, double u0, double v0)
│ │ │ │ +
88 : fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {}
│ │ │ │ +
│ │ │ │ +
89
│ │ │ │ +
│ │ │ │ +
91 Cal3(const Vector5& d)
│ │ │ │ +
92 : fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) {}
│ │ │ │ +
│ │ │ │ +
93
│ │ │ │ +
100 Cal3(double fov, int w, int h);
│ │ │ │ +
101
│ │ │ │ +
103 virtual ~Cal3() {}
│ │ │ │ +
104
│ │ │ │ +
108
│ │ │ │ +
118 Cal3(const std::string& path);
│ │ │ │ +
119
│ │ │ │ +
123
│ │ │ │ +
125 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
│ │ │ │ +
126 const Cal3& cal);
│ │ │ │ +
127
│ │ │ │ +
129 virtual void print(const std::string& s = "") const;
│ │ │ │ +
130
│ │ │ │ +
132 bool equals(const Cal3& K, double tol = 10e-9) const;
│ │ │ │ +
133
│ │ │ │ +
137
│ │ │ │ +
139 inline double fx() const { return fx_; }
│ │ │ │ +
140
│ │ │ │ +
142 inline double fy() const { return fy_; }
│ │ │ │ +
143
│ │ │ │ +
145 inline double aspectRatio() const { return fx_ / fy_; }
│ │ │ │ +
146
│ │ │ │ +
148 inline double skew() const { return s_; }
│ │ │ │ +
149
│ │ │ │ +
151 inline double px() const { return u0_; }
│ │ │ │ +
152
│ │ │ │ +
154 inline double py() const { return v0_; }
│ │ │ │ +
155
│ │ │ │ +
157 Point2 principalPoint() const { return Point2(u0_, v0_); }
│ │ │ │ +
158
│ │ │ │ +
│ │ │ │ +
160 Vector5 vector() const {
│ │ │ │ +
161 Vector5 v;
│ │ │ │ +
162 v << fx_, fy_, s_, u0_, v0_;
│ │ │ │ +
163 return v;
│ │ │ │ +
164 }
│ │ │ │ +
│ │ │ │ +
165
│ │ │ │ +
│ │ │ │ +
167 virtual Matrix3 K() const {
│ │ │ │ +
168 Matrix3 K;
│ │ │ │ +
169 K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
│ │ │ │ +
170 return K;
│ │ │ │ +
171 }
│ │ │ │ +
│ │ │ │ +
172
│ │ │ │ +
173#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ +
175 Matrix3 GTSAM_DEPRECATED matrix() const { return K(); }
│ │ │ │ +
176#endif
│ │ │ │
177
│ │ │ │ -
179};
│ │ │ │ -
│ │ │ │ +
179 Matrix3 inverse() const;
│ │ │ │
180
│ │ │ │ -
181template <>
│ │ │ │ -
182struct traits<Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
│ │ │ │ +
182 inline virtual size_t dim() const { return Dim(); }
│ │ │ │
183
│ │ │ │ -
184template <>
│ │ │ │ -
185struct traits<const Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
│ │ │ │ +
185 inline static size_t Dim() { return dimension; }
│ │ │ │
186
│ │ │ │ -
187} // namespace gtsam
│ │ │ │ -
Cal3.h
Common code for all Calibration models.
│ │ │ │ -
Point2.h
2D Point
│ │ │ │ +
190
│ │ │ │ +
191 private:
│ │ │ │ +
193 friend class boost::serialization::access;
│ │ │ │ +
194 template <class Archive>
│ │ │ │ +
195 void serialize(Archive& ar, const unsigned int /*version*/) {
│ │ │ │ +
196 ar& BOOST_SERIALIZATION_NVP(fx_);
│ │ │ │ +
197 ar& BOOST_SERIALIZATION_NVP(fy_);
│ │ │ │ +
198 ar& BOOST_SERIALIZATION_NVP(s_);
│ │ │ │ +
199 ar& BOOST_SERIALIZATION_NVP(u0_);
│ │ │ │ +
200 ar& BOOST_SERIALIZATION_NVP(v0_);
│ │ │ │ +
201 }
│ │ │ │ +
202
│ │ │ │ +
204};
│ │ │ │ +
│ │ │ │ +
205
│ │ │ │ +
206} // \ namespace gtsam
│ │ │ │ +
Point2.h
2D Point
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
│ │ │ │ +
gtsam::calibrateJacobians
void calibrateJacobians(const Cal &calibration, const Point2 &pn, OptionalJacobian< 2, Dim > Dcal=boost::none, OptionalJacobian< 2, 2 > Dp=boost::none)
Function which makes use of the Implicit Function Theorem to compute the Jacobians of calibrate using...
Definition Cal3.h:47
│ │ │ │ +
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │
gtsam::Cal3
Common base class for all calibration models.
Definition Cal3.h:69
│ │ │ │ -
gtsam::Cal3Bundler
Calibration used by Bundler.
Definition Cal3Bundler.h:32
│ │ │ │ -
gtsam::Cal3Bundler::k1
double k1() const
distorsion parameter k1
Definition Cal3Bundler.h:87
│ │ │ │ -
gtsam::Cal3Bundler::k2
double k2() const
distorsion parameter k2
Definition Cal3Bundler.h:90
│ │ │ │ -
gtsam::Cal3Bundler::Cal3Bundler
Cal3Bundler()=default
Default constructor.
│ │ │ │ -
gtsam::Cal3Bundler::retract
Cal3Bundler retract(const Vector &d) const
Update calibration with tangent space delta.
Definition Cal3Bundler.h:153
│ │ │ │ -
gtsam::Cal3Bundler::py
double py() const
image center in y
Definition Cal3Bundler.h:96
│ │ │ │ -
gtsam::Cal3Bundler::Cal3Bundler
Cal3Bundler(double f, double k1, double k2, double u0=0, double v0=0, double tol=1e-5)
Constructor.
Definition Cal3Bundler.h:62
│ │ │ │ -
gtsam::Cal3Bundler::localCoordinates
Vector3 localCoordinates(const Cal3Bundler &T2) const
Calculate local coordinates to another calibration.
Definition Cal3Bundler.h:158
│ │ │ │ -
gtsam::Cal3Bundler::px
double px() const
image center in x
Definition Cal3Bundler.h:93
│ │ │ │ -
gtsam::Cal3Bundler::dim
size_t dim() const override
return DOF, dimensionality of tangent space
Definition Cal3Bundler.h:147
│ │ │ │ -
gtsam::Cal3Bundler::Dim
static size_t Dim()
return DOF, dimensionality of tangent space
Definition Cal3Bundler.h:150
│ │ │ │ +
gtsam::Cal3::Cal3
Cal3(double fx, double fy, double s, double u0, double v0)
constructor from doubles
Definition Cal3.h:87
│ │ │ │ +
gtsam::Cal3::K
virtual Matrix3 K() const
return calibration matrix K
Definition Cal3.h:167
│ │ │ │ +
gtsam::Cal3::Cal3
Cal3(const Vector5 &d)
constructor from vector
Definition Cal3.h:91
│ │ │ │ +
gtsam::Cal3::~Cal3
virtual ~Cal3()
Virtual destructor.
Definition Cal3.h:103
│ │ │ │ +
gtsam::Cal3::vector
Vector5 vector() const
vectorized form (column-wise)
Definition Cal3.h:160
│ │ │ │ +
gtsam::Cal3::Cal3
Cal3()=default
Create a default calibration that leaves coordinates unchanged.
│ │ │ │ +
gtsam::Cal3::px
double px() const
image center in x
Definition Cal3.h:151
│ │ │ │ +
gtsam::Cal3::Dim
static size_t Dim()
return DOF, dimensionality of tangent space
Definition Cal3.h:185
│ │ │ │ +
gtsam::Cal3::principalPoint
Point2 principalPoint() const
return the principal point
Definition Cal3.h:157
│ │ │ │ +
gtsam::Cal3::fx
double fx() const
focal length x
Definition Cal3.h:139
│ │ │ │ +
gtsam::Cal3::py
double py() const
image center in y
Definition Cal3.h:154
│ │ │ │ +
gtsam::Cal3::skew
double skew() const
skew
Definition Cal3.h:148
│ │ │ │ +
gtsam::Cal3::dim
virtual size_t dim() const
return DOF, dimensionality of tangent space
Definition Cal3.h:182
│ │ │ │ +
gtsam::Cal3::aspectRatio
double aspectRatio() const
aspect ratio
Definition Cal3.h:145
│ │ │ │ +
gtsam::Cal3::fy
double fy() const
focal length y
Definition Cal3.h:142
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,196 +1,224 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Cal3Bundler.h │ │ │ │ │ +Cal3.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ -24 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ -26 │ │ │ │ │ -_3_2class GTSAM_EXPORT _C_a_l_3_B_u_n_d_l_e_r : public _C_a_l_3 { │ │ │ │ │ -33 private: │ │ │ │ │ -34 double k1_ = 0.0f, k2_ = 0.0f; │ │ │ │ │ -35 double tol_ = 1e-5; │ │ │ │ │ -36 │ │ │ │ │ -37 // NOTE: We use the base class fx to represent the common focal length. │ │ │ │ │ -38 // Also, image center parameters (u0, v0) are not optimized │ │ │ │ │ -39 // but are treated as constants. │ │ │ │ │ -40 │ │ │ │ │ -41 public: │ │ │ │ │ -42 enum { dimension = 3 }; │ │ │ │ │ -43 │ │ │ │ │ -45 using shared_ptr = boost::shared_ptr; │ │ │ │ │ -46 │ │ │ │ │ -49 │ │ │ │ │ -_5_1 _C_a_l_3_B_u_n_d_l_e_r() = default; │ │ │ │ │ -52 │ │ │ │ │ -_6_2 _C_a_l_3_B_u_n_d_l_e_r(double f, double k1, double k2, double u0 = 0, double v0 = 0, │ │ │ │ │ -63 double tol = 1e-5) │ │ │ │ │ -64 : _C_a_l_3(f, f, 0, u0, v0), k1_(k1), k2_(k2), tol_(tol) {} │ │ │ │ │ -65 │ │ │ │ │ -66 _~_C_a_l_3_B_u_n_d_l_e_r() override {} │ │ │ │ │ -67 │ │ │ │ │ -71 │ │ │ │ │ -73 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, │ │ │ │ │ -74 const Cal3Bundler& cal); │ │ │ │ │ -75 │ │ │ │ │ -77 void print(const std::string& s = "") const override; │ │ │ │ │ -78 │ │ │ │ │ -80 bool equals(const Cal3Bundler& K, double tol = 10e-9) const; │ │ │ │ │ -81 │ │ │ │ │ +22#pragma once │ │ │ │ │ +23 │ │ │ │ │ +24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ +25 │ │ │ │ │ +26namespace _g_t_s_a_m { │ │ │ │ │ +27 │ │ │ │ │ +46template │ │ │ │ │ +_4_7void _c_a_l_i_b_r_a_t_e_J_a_c_o_b_i_a_n_s(const Cal& calibration, const _P_o_i_n_t_2& pn, │ │ │ │ │ +48 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _D_i_m_> Dcal = boost::none, │ │ │ │ │ +49 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dp = boost::none) { │ │ │ │ │ +50 if (Dcal || Dp) { │ │ │ │ │ +51 Eigen::Matrix H_uncal_K; │ │ │ │ │ +52 Matrix22 H_uncal_pn, H_uncal_pn_inv; │ │ │ │ │ +53 │ │ │ │ │ +54 // Compute uncalibrate Jacobians │ │ │ │ │ +55 calibration.uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn); │ │ │ │ │ +56 │ │ │ │ │ +57 H_uncal_pn_inv = H_uncal_pn.inverse(); │ │ │ │ │ +58 │ │ │ │ │ +59 if (Dp) *Dp = H_uncal_pn_inv; │ │ │ │ │ +60 if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K; │ │ │ │ │ +61 } │ │ │ │ │ +62} │ │ │ │ │ +63 │ │ │ │ │ +_6_9class GTSAM_EXPORT _C_a_l_3 { │ │ │ │ │ +70 protected: │ │ │ │ │ +_7_1 double fx_ = 1.0f, fy_ = 1.0f; │ │ │ │ │ +_7_2 double s_ = 0.0f; │ │ │ │ │ +_7_3 double u0_ = 0.0f, v0_ = 0.0f; │ │ │ │ │ +74 │ │ │ │ │ +75 public: │ │ │ │ │ +76 enum { dimension = 5 }; │ │ │ │ │ +78 using shared_ptr = boost::shared_ptr; │ │ │ │ │ +79 │ │ │ │ │ +82 │ │ │ │ │ +_8_4 _C_a_l_3() = default; │ │ │ │ │ 85 │ │ │ │ │ -_8_7 inline double _k_1() const { return k1_; } │ │ │ │ │ -88 │ │ │ │ │ -_9_0 inline double _k_2() const { return k2_; } │ │ │ │ │ -91 │ │ │ │ │ -_9_3 inline double _p_x() const { return u0_; } │ │ │ │ │ -94 │ │ │ │ │ -_9_6 inline double _p_y() const { return v0_; } │ │ │ │ │ -97 │ │ │ │ │ -98 Matrix3 K() const override; │ │ │ │ │ -99 Vector4 k() const; │ │ │ │ │ -100 │ │ │ │ │ -101 Vector3 vector() const; │ │ │ │ │ -102 │ │ │ │ │ -103#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ -105 inline double GTSAM_DEPRECATED u0() const { return u0_; } │ │ │ │ │ -106 │ │ │ │ │ -108 inline double GTSAM_DEPRECATED v0() const { return v0_; } │ │ │ │ │ -109#endif │ │ │ │ │ -110 │ │ │ │ │ -119 _P_o_i_n_t_2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost:: │ │ │ │ │ -none, │ │ │ │ │ -120 OptionalJacobian<2, 2> Dp = boost::none) const; │ │ │ │ │ -121 │ │ │ │ │ -130 _P_o_i_n_t_2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = boost:: │ │ │ │ │ -none, │ │ │ │ │ -131 OptionalJacobian<2, 2> Dp = boost::none) const; │ │ │ │ │ -132 │ │ │ │ │ -134 Matrix2 D2d_intrinsic(const Point2& p) const; │ │ │ │ │ -135 │ │ │ │ │ -137 Matrix23 D2d_calibration(const Point2& p) const; │ │ │ │ │ -138 │ │ │ │ │ -140 Matrix25 D2d_intrinsic_calibration(const Point2& p) const; │ │ │ │ │ -141 │ │ │ │ │ -145 │ │ │ │ │ -_1_4_7 size_t _d_i_m() const override { return Dim(); } │ │ │ │ │ -148 │ │ │ │ │ -_1_5_0 inline static size_t _D_i_m() { return dimension; } │ │ │ │ │ -151 │ │ │ │ │ -_1_5_3 inline _C_a_l_3_B_u_n_d_l_e_r _r_e_t_r_a_c_t(const Vector& d) const { │ │ │ │ │ -154 return _C_a_l_3_B_u_n_d_l_e_r(fx_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_); │ │ │ │ │ -155 } │ │ │ │ │ -156 │ │ │ │ │ -_1_5_8 Vector3 _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(const _C_a_l_3_B_u_n_d_l_e_r& T2) const { │ │ │ │ │ -159 return T2.vector() - vector(); │ │ │ │ │ -160 } │ │ │ │ │ -161 │ │ │ │ │ -162 private: │ │ │ │ │ -166 │ │ │ │ │ -_1_6_8 friend class boost::serialization::access; │ │ │ │ │ -169 template │ │ │ │ │ -170 void serialize(Archive& ar, const unsigned int /*version*/) { │ │ │ │ │ -171 ar& boost::serialization::make_nvp( │ │ │ │ │ -172 "Cal3Bundler", boost::serialization::base_object(*this)); │ │ │ │ │ -173 ar& BOOST_SERIALIZATION_NVP(k1_); │ │ │ │ │ -174 ar& BOOST_SERIALIZATION_NVP(k2_); │ │ │ │ │ -175 ar& BOOST_SERIALIZATION_NVP(tol_); │ │ │ │ │ -176 } │ │ │ │ │ +_8_7 _C_a_l_3(double fx, double fy, double s, double u0, double v0) │ │ │ │ │ +88 : fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {} │ │ │ │ │ +89 │ │ │ │ │ +_9_1 _C_a_l_3(const Vector5& d) │ │ │ │ │ +92 : fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) {} │ │ │ │ │ +93 │ │ │ │ │ +100 _C_a_l_3(double fov, int w, int h); │ │ │ │ │ +101 │ │ │ │ │ +_1_0_3 virtual _~_C_a_l_3() {} │ │ │ │ │ +104 │ │ │ │ │ +108 │ │ │ │ │ +118 _C_a_l_3(const std::string& path); │ │ │ │ │ +119 │ │ │ │ │ +123 │ │ │ │ │ +125 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, │ │ │ │ │ +126 const _C_a_l_3& cal); │ │ │ │ │ +127 │ │ │ │ │ +129 virtual void _p_r_i_n_t(const std::string& s = "") const; │ │ │ │ │ +130 │ │ │ │ │ +132 bool _e_q_u_a_l_s(const _C_a_l_3& K, double tol = 10e-9) const; │ │ │ │ │ +133 │ │ │ │ │ +137 │ │ │ │ │ +_1_3_9 inline double _f_x() const { return fx_; } │ │ │ │ │ +140 │ │ │ │ │ +_1_4_2 inline double _f_y() const { return fy_; } │ │ │ │ │ +143 │ │ │ │ │ +_1_4_5 inline double _a_s_p_e_c_t_R_a_t_i_o() const { return fx_ / fy_; } │ │ │ │ │ +146 │ │ │ │ │ +_1_4_8 inline double _s_k_e_w() const { return s_; } │ │ │ │ │ +149 │ │ │ │ │ +_1_5_1 inline double _p_x() const { return u0_; } │ │ │ │ │ +152 │ │ │ │ │ +_1_5_4 inline double _p_y() const { return v0_; } │ │ │ │ │ +155 │ │ │ │ │ +_1_5_7 _P_o_i_n_t_2 _p_r_i_n_c_i_p_a_l_P_o_i_n_t() const { return _P_o_i_n_t_2(u0_, v0_); } │ │ │ │ │ +158 │ │ │ │ │ +_1_6_0 Vector5 _v_e_c_t_o_r() const { │ │ │ │ │ +161 Vector5 v; │ │ │ │ │ +162 v << fx_, fy_, s_, u0_, v0_; │ │ │ │ │ +163 return v; │ │ │ │ │ +164 } │ │ │ │ │ +165 │ │ │ │ │ +_1_6_7 virtual Matrix3 _K() const { │ │ │ │ │ +168 Matrix3 K; │ │ │ │ │ +169 K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0; │ │ │ │ │ +170 return K; │ │ │ │ │ +171 } │ │ │ │ │ +172 │ │ │ │ │ +173#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ +175 Matrix3 GTSAM_DEPRECATED matrix() const { return K(); } │ │ │ │ │ +176#endif │ │ │ │ │ 177 │ │ │ │ │ -179}; │ │ │ │ │ +179 Matrix3 inverse() const; │ │ │ │ │ 180 │ │ │ │ │ -181template <> │ │ │ │ │ -_1_8_2struct _t_r_a_i_t_s<_C_a_l_3_B_u_n_d_l_e_r> : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d {}; │ │ │ │ │ +_1_8_2 inline virtual size_t _d_i_m() const { return Dim(); } │ │ │ │ │ 183 │ │ │ │ │ -184template <> │ │ │ │ │ -_1_8_5struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ -{}; │ │ │ │ │ +_1_8_5 inline static size_t _D_i_m() { return dimension; } │ │ │ │ │ 186 │ │ │ │ │ -187} // namespace gtsam │ │ │ │ │ -_C_a_l_3_._h │ │ │ │ │ -Common code for all Calibration models. │ │ │ │ │ +190 │ │ │ │ │ +191 private: │ │ │ │ │ +_1_9_3 friend class boost::serialization::access; │ │ │ │ │ +194 template │ │ │ │ │ +195 void serialize(Archive& ar, const unsigned int /*version*/) { │ │ │ │ │ +196 ar& BOOST_SERIALIZATION_NVP(fx_); │ │ │ │ │ +197 ar& BOOST_SERIALIZATION_NVP(fy_); │ │ │ │ │ +198 ar& BOOST_SERIALIZATION_NVP(s_); │ │ │ │ │ +199 ar& BOOST_SERIALIZATION_NVP(u0_); │ │ │ │ │ +200 ar& BOOST_SERIALIZATION_NVP(v0_); │ │ │ │ │ +201 } │ │ │ │ │ +202 │ │ │ │ │ +204}; │ │ │ │ │ +205 │ │ │ │ │ +206} // \ namespace gtsam │ │ │ │ │ _P_o_i_n_t_2_._h │ │ │ │ │ 2D Point │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ _g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ Vector2 Point2 │ │ │ │ │ As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ typedef Point2 to Vector2... │ │ │ │ │ DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ -Both ManifoldTraits and Testable. │ │ │ │ │ -DDeeffiinniittiioonn Manifold.h:120 │ │ │ │ │ +_g_t_s_a_m_:_:_c_a_l_i_b_r_a_t_e_J_a_c_o_b_i_a_n_s │ │ │ │ │ +void calibrateJacobians(const Cal &calibration, const Point2 &pn, │ │ │ │ │ +OptionalJacobian< 2, Dim > Dcal=boost::none, OptionalJacobian< 2, 2 > │ │ │ │ │ +Dp=boost::none) │ │ │ │ │ +Function which makes use of the Implicit Function Theorem to compute the │ │ │ │ │ +Jacobians of calibrate using... │ │ │ │ │ +DDeeffiinniittiioonn Cal3.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ +either a fixed size o... │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ _g_t_s_a_m_:_:_C_a_l_3 │ │ │ │ │ Common base class for all calibration models. │ │ │ │ │ DDeeffiinniittiioonn Cal3.h:69 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_B_u_n_d_l_e_r │ │ │ │ │ -Calibration used by Bundler. │ │ │ │ │ -DDeeffiinniittiioonn Cal3Bundler.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_B_u_n_d_l_e_r_:_:_k_1 │ │ │ │ │ -double k1() const │ │ │ │ │ -distorsion parameter k1 │ │ │ │ │ -DDeeffiinniittiioonn Cal3Bundler.h:87 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_B_u_n_d_l_e_r_:_:_k_2 │ │ │ │ │ -double k2() const │ │ │ │ │ -distorsion parameter k2 │ │ │ │ │ -DDeeffiinniittiioonn Cal3Bundler.h:90 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_B_u_n_d_l_e_r_:_:_C_a_l_3_B_u_n_d_l_e_r │ │ │ │ │ -Cal3Bundler()=default │ │ │ │ │ -Default constructor. │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_B_u_n_d_l_e_r_:_:_r_e_t_r_a_c_t │ │ │ │ │ -Cal3Bundler retract(const Vector &d) const │ │ │ │ │ -Update calibration with tangent space delta. │ │ │ │ │ -DDeeffiinniittiioonn Cal3Bundler.h:153 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_B_u_n_d_l_e_r_:_:_p_y │ │ │ │ │ -double py() const │ │ │ │ │ -image center in y │ │ │ │ │ -DDeeffiinniittiioonn Cal3Bundler.h:96 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_B_u_n_d_l_e_r_:_:_C_a_l_3_B_u_n_d_l_e_r │ │ │ │ │ -Cal3Bundler(double f, double k1, double k2, double u0=0, double v0=0, double │ │ │ │ │ -tol=1e-5) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn Cal3Bundler.h:62 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_B_u_n_d_l_e_r_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ -Vector3 localCoordinates(const Cal3Bundler &T2) const │ │ │ │ │ -Calculate local coordinates to another calibration. │ │ │ │ │ -DDeeffiinniittiioonn Cal3Bundler.h:158 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_B_u_n_d_l_e_r_:_:_p_x │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_:_:_C_a_l_3 │ │ │ │ │ +Cal3(double fx, double fy, double s, double u0, double v0) │ │ │ │ │ +constructor from doubles │ │ │ │ │ +DDeeffiinniittiioonn Cal3.h:87 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_:_:_K │ │ │ │ │ +virtual Matrix3 K() const │ │ │ │ │ +return calibration matrix K │ │ │ │ │ +DDeeffiinniittiioonn Cal3.h:167 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_:_:_C_a_l_3 │ │ │ │ │ +Cal3(const Vector5 &d) │ │ │ │ │ +constructor from vector │ │ │ │ │ +DDeeffiinniittiioonn Cal3.h:91 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_:_:_~_C_a_l_3 │ │ │ │ │ +virtual ~Cal3() │ │ │ │ │ +Virtual destructor. │ │ │ │ │ +DDeeffiinniittiioonn Cal3.h:103 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_:_:_v_e_c_t_o_r │ │ │ │ │ +Vector5 vector() const │ │ │ │ │ +vectorized form (column-wise) │ │ │ │ │ +DDeeffiinniittiioonn Cal3.h:160 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_:_:_C_a_l_3 │ │ │ │ │ +Cal3()=default │ │ │ │ │ +Create a default calibration that leaves coordinates unchanged. │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_:_:_p_x │ │ │ │ │ double px() const │ │ │ │ │ image center in x │ │ │ │ │ -DDeeffiinniittiioonn Cal3Bundler.h:93 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_B_u_n_d_l_e_r_:_:_d_i_m │ │ │ │ │ -size_t dim() const override │ │ │ │ │ -return DOF, dimensionality of tangent space │ │ │ │ │ -DDeeffiinniittiioonn Cal3Bundler.h:147 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_B_u_n_d_l_e_r_:_:_D_i_m │ │ │ │ │ +DDeeffiinniittiioonn Cal3.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_:_:_D_i_m │ │ │ │ │ static size_t Dim() │ │ │ │ │ return DOF, dimensionality of tangent space │ │ │ │ │ -DDeeffiinniittiioonn Cal3Bundler.h:150 │ │ │ │ │ +DDeeffiinniittiioonn Cal3.h:185 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_:_:_p_r_i_n_c_i_p_a_l_P_o_i_n_t │ │ │ │ │ +Point2 principalPoint() const │ │ │ │ │ +return the principal point │ │ │ │ │ +DDeeffiinniittiioonn Cal3.h:157 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_:_:_f_x │ │ │ │ │ +double fx() const │ │ │ │ │ +focal length x │ │ │ │ │ +DDeeffiinniittiioonn Cal3.h:139 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_:_:_p_y │ │ │ │ │ +double py() const │ │ │ │ │ +image center in y │ │ │ │ │ +DDeeffiinniittiioonn Cal3.h:154 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_:_:_s_k_e_w │ │ │ │ │ +double skew() const │ │ │ │ │ +skew │ │ │ │ │ +DDeeffiinniittiioonn Cal3.h:148 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_:_:_d_i_m │ │ │ │ │ +virtual size_t dim() const │ │ │ │ │ +return DOF, dimensionality of tangent space │ │ │ │ │ +DDeeffiinniittiioonn Cal3.h:182 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_:_:_a_s_p_e_c_t_R_a_t_i_o │ │ │ │ │ +double aspectRatio() const │ │ │ │ │ +aspect ratio │ │ │ │ │ +DDeeffiinniittiioonn Cal3.h:145 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_:_:_f_y │ │ │ │ │ +double fy() const │ │ │ │ │ +focal length y │ │ │ │ │ +DDeeffiinniittiioonn Cal3.h:142 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3_B_u_n_d_l_e_r_._h │ │ │ │ │ + * _C_a_l_3_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00317.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cyclic.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SO4.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,46 +95,70 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
Cyclic.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ +Functions
│ │ │ │ +
SO4.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Cyclic group, i.e., the integers modulo N. │ │ │ │ +

4*4 matrix representation of SO(4) │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::Cyclic< N >
 Cyclic group of order N. More...
struct  gtsam::traits< SO4 >
 
struct  gtsam::traits< Cyclic< N > >
 Define cyclic group to be a model of the Additive Group concept. More...
struct  gtsam::traits< const SO4 >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Typedefs

│ │ │ │ +using gtsam::SO4 = SO< 4 >
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

GTSAM_EXPORT Matrix3 gtsam::topLeft (const SO4 &Q, OptionalJacobian< 9, 6 > H=boost::none)
 Project to top-left 3*3 matrix.
 
│ │ │ │ +GTSAM_EXPORT Matrix43 gtsam::stiefel (const SO4 &Q, OptionalJacobian< 12, 6 > H=boost::none)
 Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q) -> \( S \in St(3,4) \).
 
│ │ │ │ +template<class Archive >
void gtsam::serialize (Archive &ar, SO4 &Q, const unsigned int)
 Serialization function.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Cyclic group, i.e., the integers modulo N.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +

4*4 matrix representation of SO(4)

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ +Luca Carlone
│ │ │ │ +
Date
March 2019
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,29 +1,47 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Cyclic.h File Reference │ │ │ │ │ -Cyclic group, i.e., the integers modulo N. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +SO4.h File Reference │ │ │ │ │ +4*4 matrix representation of SO(4) _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_C_y_c_l_i_c_<_ _N_ _> │ │ │ │ │ -  _C_y_c_l_i_c group of order N. _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_O_4_ _> │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _C_y_c_l_i_c_<_ _N_ _>_ _> │ │ │ │ │ -  Define cyclic group to be a model of the Additive Group concept. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _S_O_4_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +using  ggttssaamm::::SSOO44 = _S_O< 4 > │ │ │ │ │ +  │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ + GTSAM_EXPORT Matrix3  _g_t_s_a_m_:_:_t_o_p_L_e_f_t (const _S_O_4 &Q, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 9, 6 > │ │ │ │ │ + H=boost::none) │ │ │ │ │ +  Project to top-left 3*3 matrix. │ │ │ │ │ +  │ │ │ │ │ +GTSAM_EXPORT Matrix43  ggttssaamm::::ssttiieeffeell (const _S_O_4 &Q, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 12, 6 > │ │ │ │ │ + H=boost::none) │ │ │ │ │ +  Project to Stiefel manifold of 4*3 orthonormal 3-frames │ │ │ │ │ + in R^4, i.e., pi(Q) -> \( S \in St(3,4) \). │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + void  ggttssaamm::::sseerriiaalliizzee (Archive &ar, _S_O_4 &Q, const unsigned │ │ │ │ │ + int) │ │ │ │ │ +  Serialization function. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Cyclic group, i.e., the integers modulo N. │ │ │ │ │ +4*4 matrix representation of SO(4) │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Date │ │ │ │ │ + March 2019 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_y_c_l_i_c_._h │ │ │ │ │ + * _S_O_4_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00317.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,7 @@ │ │ │ │ │ var a00317 = [ │ │ │ │ │ - ["gtsam::Cyclic< N >", "a03060.html", "a03060"], │ │ │ │ │ - ["gtsam::traits< Cyclic< N > >", "a03064.html", null] │ │ │ │ │ + ["gtsam::traits< SO4 >", "a03296.html", null], │ │ │ │ │ + ["gtsam::traits< const SO4 >", "a03300.html", null], │ │ │ │ │ + ["serialize", "a00317.html#a76855a1214bf05a8704ee3deaece6339", null], │ │ │ │ │ + ["stiefel", "a00317.html#a3e57e4771f13855a495ec2c6454c9121", null], │ │ │ │ │ + ["topLeft", "a00317.html#a8d21728e3da4cf1a96e1b6f87a97da47", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00317_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cyclic.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SO4.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,114 +98,141 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Cyclic.h
│ │ │ │ +
SO4.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/base/Group.h>
│ │ │ │ -
21#include <gtsam/base/Testable.h>
│ │ │ │ -
22
│ │ │ │ -
23#include <cassert>
│ │ │ │ -
24#include <iostream> // for cout :-(
│ │ │ │ -
25
│ │ │ │ -
26namespace gtsam {
│ │ │ │ -
27
│ │ │ │ -
29template<size_t N>
│ │ │ │ -
│ │ │ │ -
30class Cyclic {
│ │ │ │ -
31 size_t i_;
│ │ │ │ -
32public:
│ │ │ │ -
│ │ │ │ -
34 Cyclic(size_t i) :
│ │ │ │ -
35 i_(i) {
│ │ │ │ -
36 assert(i < N);
│ │ │ │ -
37 }
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
39 Cyclic():i_(0) {
│ │ │ │ -
40 }
│ │ │ │ -
│ │ │ │ -
41 static Cyclic Identity() { return Cyclic();}
│ │ │ │ -
42
│ │ │ │ -
│ │ │ │ -
44 operator size_t() const {
│ │ │ │ -
45 return i_;
│ │ │ │ -
46 }
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
48 Cyclic operator+(const Cyclic& h) const {
│ │ │ │ -
49 return (i_ + h.i_) % N;
│ │ │ │ -
50 }
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
52 Cyclic operator-(const Cyclic& h) const {
│ │ │ │ -
53 return (N + i_ - h.i_) % N;
│ │ │ │ -
54 }
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
56 Cyclic operator-() const {
│ │ │ │ -
57 return (N - i_) % N;
│ │ │ │ -
58 }
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
60 void print(const std::string& s = "") const {
│ │ │ │ -
61 std::cout << s << i_ << std::endl;
│ │ │ │ -
62 }
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
64 bool equals(const Cyclic& other, double tol = 1e-9) const {
│ │ │ │ -
65 return other.i_ == i_;
│ │ │ │ -
66 }
│ │ │ │ -
│ │ │ │ -
67};
│ │ │ │ -
│ │ │ │ -
68
│ │ │ │ -
70template<size_t N>
│ │ │ │ -
│ │ │ │ -
71struct traits<Cyclic<N> > : internal::AdditiveGroupTraits<Cyclic<N> >, //
│ │ │ │ -
72 Testable<Cyclic<N> > {
│ │ │ │ -
73};
│ │ │ │ -
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/geometry/SOn.h>
│ │ │ │ +
23
│ │ │ │ +
24#include <gtsam/base/Group.h>
│ │ │ │ +
25#include <gtsam/base/Lie.h>
│ │ │ │ +
26#include <gtsam/base/Manifold.h>
│ │ │ │ +
27#include <gtsam/base/Matrix.h>
│ │ │ │ +
28#include <gtsam/dllexport.h>
│ │ │ │ +
29
│ │ │ │ +
30#include <string>
│ │ │ │ +
31
│ │ │ │ +
32namespace gtsam {
│ │ │ │ +
33
│ │ │ │ +
34using SO4 = SO<4>;
│ │ │ │ +
35
│ │ │ │ +
36// /// Random SO(4) element (no big claims about uniformity)
│ │ │ │ +
37// static SO4 Random(std::mt19937 &rng);
│ │ │ │ +
38
│ │ │ │ +
39// Below are all declarations of SO<4> specializations.
│ │ │ │ +
40// They are *defined* in SO4.cpp.
│ │ │ │ +
41
│ │ │ │ +
42template <>
│ │ │ │ +
43GTSAM_EXPORT
│ │ │ │ +
44Matrix4 SO4::Hat(const TangentVector &xi);
│ │ │ │ +
45
│ │ │ │ +
46template <>
│ │ │ │ +
47GTSAM_EXPORT
│ │ │ │ +
48Vector6 SO4::Vee(const Matrix4 &X);
│ │ │ │ +
49
│ │ │ │ +
50template <>
│ │ │ │ +
51GTSAM_EXPORT
│ │ │ │ +
52SO4 SO4::Expmap(const Vector6 &xi, ChartJacobian H);
│ │ │ │ +
53
│ │ │ │ +
54template <>
│ │ │ │ +
55GTSAM_EXPORT
│ │ │ │ +
56Matrix6 SO4::AdjointMap() const;
│ │ │ │ +
57
│ │ │ │ +
58template <>
│ │ │ │ +
59GTSAM_EXPORT
│ │ │ │ +
60SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const;
│ │ │ │ +
61
│ │ │ │ +
62template <>
│ │ │ │ +
63GTSAM_EXPORT
│ │ │ │ +
64SO4 SO4::ChartAtOrigin::Retract(const Vector6 &omega, ChartJacobian H);
│ │ │ │ +
65
│ │ │ │ +
66template <>
│ │ │ │ +
67GTSAM_EXPORT
│ │ │ │ +
68Vector6 SO4::ChartAtOrigin::Local(const SO4 &Q, ChartJacobian H);
│ │ │ │ +
69
│ │ │ │ +
73GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian<9, 6> H = boost::none);
│ │ │ │
74
│ │ │ │ -
75} // \namespace gtsam
│ │ │ │ -
76
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ -
Group.h
Concept check class for variable types with Group properties.
│ │ │ │ +
79GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian<12, 6> H = boost::none);
│ │ │ │ +
80
│ │ │ │ +
82template <class Archive>
│ │ │ │ +
│ │ │ │ +
83void serialize(Archive &ar, SO4 &Q, const unsigned int /*version*/) {
│ │ │ │ +
84 Matrix4 &M = Q.matrix_;
│ │ │ │ +
85 ar &boost::serialization::make_nvp("Q11", M(0, 0));
│ │ │ │ +
86 ar &boost::serialization::make_nvp("Q12", M(0, 1));
│ │ │ │ +
87 ar &boost::serialization::make_nvp("Q13", M(0, 2));
│ │ │ │ +
88 ar &boost::serialization::make_nvp("Q14", M(0, 3));
│ │ │ │ +
89
│ │ │ │ +
90 ar &boost::serialization::make_nvp("Q21", M(1, 0));
│ │ │ │ +
91 ar &boost::serialization::make_nvp("Q22", M(1, 1));
│ │ │ │ +
92 ar &boost::serialization::make_nvp("Q23", M(1, 2));
│ │ │ │ +
93 ar &boost::serialization::make_nvp("Q24", M(1, 3));
│ │ │ │ +
94
│ │ │ │ +
95 ar &boost::serialization::make_nvp("Q31", M(2, 0));
│ │ │ │ +
96 ar &boost::serialization::make_nvp("Q32", M(2, 1));
│ │ │ │ +
97 ar &boost::serialization::make_nvp("Q33", M(2, 2));
│ │ │ │ +
98 ar &boost::serialization::make_nvp("Q34", M(2, 3));
│ │ │ │ +
99
│ │ │ │ +
100 ar &boost::serialization::make_nvp("Q41", M(3, 0));
│ │ │ │ +
101 ar &boost::serialization::make_nvp("Q42", M(3, 1));
│ │ │ │ +
102 ar &boost::serialization::make_nvp("Q43", M(3, 2));
│ │ │ │ +
103 ar &boost::serialization::make_nvp("Q44", M(3, 3));
│ │ │ │ +
104}
│ │ │ │ +
│ │ │ │ +
105
│ │ │ │ +
106/*
│ │ │ │ +
107 * Define the traits. internal::LieGroup provides both Lie group and Testable
│ │ │ │ +
108 */
│ │ │ │ +
109
│ │ │ │ +
110template <>
│ │ │ │ +
111struct traits<SO4> : public internal::LieGroup<SO4> {};
│ │ │ │ +
112
│ │ │ │ +
113template <>
│ │ │ │ +
114struct traits<const SO4> : public internal::LieGroup<SO4> {};
│ │ │ │ +
115
│ │ │ │ +
116} // end namespace gtsam
│ │ │ │ +
Group.h
Concept check class for variable types with Group properties.
│ │ │ │ +
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ +
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ +
SOn.h
N*N matrix representation of SO(N).
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition serialization.h:113
│ │ │ │ +
gtsam::stiefel
GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian< 12, 6 > H)
Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q) -> .
Definition SO4.cpp:220
│ │ │ │ +
gtsam::topLeft
GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian< 9, 6 > H)
Project to top-left 3*3 matrix.
Definition SO4.cpp:206
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::Cyclic
Cyclic group of order N.
Definition Cyclic.h:30
│ │ │ │ -
gtsam::Cyclic::equals
bool equals(const Cyclic &other, double tol=1e-9) const
equals with an tolerance, prints out message if unequal
Definition Cyclic.h:64
│ │ │ │ -
gtsam::Cyclic::print
void print(const std::string &s="") const
print with optional string
Definition Cyclic.h:60
│ │ │ │ -
gtsam::Cyclic::Cyclic
Cyclic()
Default constructor yields identity.
Definition Cyclic.h:39
│ │ │ │ -
gtsam::Cyclic::Cyclic
Cyclic(size_t i)
Constructor.
Definition Cyclic.h:34
│ │ │ │ -
gtsam::Cyclic::operator-
Cyclic operator-(const Cyclic &h) const
Subtraction modulo N.
Definition Cyclic.h:52
│ │ │ │ -
gtsam::Cyclic::operator+
Cyclic operator+(const Cyclic &h) const
Addition modulo N.
Definition Cyclic.h:48
│ │ │ │ -
gtsam::Cyclic::operator-
Cyclic operator-() const
Inverse.
Definition Cyclic.h:56
│ │ │ │ +
gtsam::LieGroup< SO< N >, internal::DimensionSO(N)>::Retract
static SO< N > Retract(const TangentVector &v)
Retract at origin: possible in Lie group because it has an identity.
Definition Lie.h:111
│ │ │ │ +
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition Lie.h:229
│ │ │ │ +
gtsam::SO
Manifold of special orthogonal rotation matrices SO<N>.
Definition SOn.h:52
│ │ │ │ +
gtsam::SO::Expmap
static SO Expmap(const TangentVector &omega, ChartJacobian H=boost::none)
Exponential map at identity - create a rotation from canonical coordinates.
Definition SOn-inl.h:67
│ │ │ │ +
gtsam::SO::vec
VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost::none) const
Return vectorized rotation matrix in column order.
Definition SOn-inl.h:88
│ │ │ │ +
gtsam::SO::Vee
static TangentVector Vee(const MatrixNN &X)
Inverse of Hat. See note about xi element order in Hat.
Definition SOn-inl.h:35
│ │ │ │ +
gtsam::SO::AdjointMap
MatrixDD AdjointMap() const
Adjoint map.
Definition SO4.cpp:159
│ │ │ │ +
gtsam::SO::Hat
static MatrixNN Hat(const TangentVector &xi)
Hat operator creates Lie algebra element corresponding to d-vector, where d is the dimensionality of ...
Definition SOn-inl.h:29
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,118 +1,178 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Cyclic.h │ │ │ │ │ +SO4.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_b_a_s_e_/_G_r_o_u_p_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ -22 │ │ │ │ │ -23#include │ │ │ │ │ -24#include // for cout :-( │ │ │ │ │ -25 │ │ │ │ │ -26namespace _g_t_s_a_m { │ │ │ │ │ -27 │ │ │ │ │ -29template │ │ │ │ │ -_3_0class _C_y_c_l_i_c { │ │ │ │ │ -31 size_t i_; │ │ │ │ │ -32public: │ │ │ │ │ -_3_4 _C_y_c_l_i_c(size_t i) : │ │ │ │ │ -35 i_(i) { │ │ │ │ │ -36 assert(i < N); │ │ │ │ │ -37 } │ │ │ │ │ -_3_9 _C_y_c_l_i_c():i_(0) { │ │ │ │ │ -40 } │ │ │ │ │ -41 static _C_y_c_l_i_c Identity() { return _C_y_c_l_i_c();} │ │ │ │ │ -42 │ │ │ │ │ -_4_4 operator size_t() const { │ │ │ │ │ -45 return i_; │ │ │ │ │ -46 } │ │ │ │ │ -_4_8 _C_y_c_l_i_c _o_p_e_r_a_t_o_r_+(const _C_y_c_l_i_c& h) const { │ │ │ │ │ -49 return (i_ + h.i_) % N; │ │ │ │ │ -50 } │ │ │ │ │ -_5_2 _C_y_c_l_i_c _o_p_e_r_a_t_o_r_-(const _C_y_c_l_i_c& h) const { │ │ │ │ │ -53 return (N + i_ - h.i_) % N; │ │ │ │ │ -54 } │ │ │ │ │ -_5_6 _C_y_c_l_i_c _o_p_e_r_a_t_o_r_-() const { │ │ │ │ │ -57 return (N - i_) % N; │ │ │ │ │ -58 } │ │ │ │ │ -_6_0 void _p_r_i_n_t(const std::string& s = "") const { │ │ │ │ │ -61 std::cout << s << i_ << std::endl; │ │ │ │ │ -62 } │ │ │ │ │ -_6_4 bool _e_q_u_a_l_s(const _C_y_c_l_i_c& other, double tol = 1e-9) const { │ │ │ │ │ -65 return other.i_ == i_; │ │ │ │ │ -66 } │ │ │ │ │ -67}; │ │ │ │ │ -68 │ │ │ │ │ -70template │ │ │ │ │ -_7_1struct _t_r_a_i_t_s<_C_y_c_l_i_c > : internal::AdditiveGroupTraits >, // │ │ │ │ │ -72 _T_e_s_t_a_b_l_e > { │ │ │ │ │ -73}; │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_S_O_n_._h> │ │ │ │ │ +23 │ │ │ │ │ +24#include <_g_t_s_a_m_/_b_a_s_e_/_G_r_o_u_p_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_b_a_s_e_/_L_i_e_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ +27#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ +28#include │ │ │ │ │ +29 │ │ │ │ │ +30#include │ │ │ │ │ +31 │ │ │ │ │ +32namespace _g_t_s_a_m { │ │ │ │ │ +33 │ │ │ │ │ +34using SO4 = SO<4>; │ │ │ │ │ +35 │ │ │ │ │ +36// /// Random SO(4) element (no big claims about uniformity) │ │ │ │ │ +37// static SO4 Random(std::mt19937 &rng); │ │ │ │ │ +38 │ │ │ │ │ +39// Below are all declarations of SO<4> specializations. │ │ │ │ │ +40// They are *defined* in SO4.cpp. │ │ │ │ │ +41 │ │ │ │ │ +42template <> │ │ │ │ │ +43GTSAM_EXPORT │ │ │ │ │ +44Matrix4 _S_O_4_:_:_H_a_t(const TangentVector &xi); │ │ │ │ │ +45 │ │ │ │ │ +46template <> │ │ │ │ │ +47GTSAM_EXPORT │ │ │ │ │ +48Vector6 _S_O_4_:_:_V_e_e(const Matrix4 &X); │ │ │ │ │ +49 │ │ │ │ │ +50template <> │ │ │ │ │ +51GTSAM_EXPORT │ │ │ │ │ +52SO4 _S_O_4_:_:_E_x_p_m_a_p(const Vector6 &xi, ChartJacobian H); │ │ │ │ │ +53 │ │ │ │ │ +54template <> │ │ │ │ │ +55GTSAM_EXPORT │ │ │ │ │ +56Matrix6 _S_O_4_:_:_A_d_j_o_i_n_t_M_a_p() const; │ │ │ │ │ +57 │ │ │ │ │ +58template <> │ │ │ │ │ +59GTSAM_EXPORT │ │ │ │ │ +60SO4::VectorN2 _S_O_4_:_:_v_e_c(OptionalJacobian<16, 6> H) const; │ │ │ │ │ +61 │ │ │ │ │ +62template <> │ │ │ │ │ +63GTSAM_EXPORT │ │ │ │ │ +64SO4 _S_O_4_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n_:_:_R_e_t_r_a_c_t(const Vector6 &omega, ChartJacobian H); │ │ │ │ │ +65 │ │ │ │ │ +66template <> │ │ │ │ │ +67GTSAM_EXPORT │ │ │ │ │ +68Vector6 SO4::ChartAtOrigin::Local(const SO4 &Q, ChartJacobian H); │ │ │ │ │ +69 │ │ │ │ │ +73GTSAM_EXPORT Matrix3 _t_o_p_L_e_f_t(const SO4 &Q, OptionalJacobian<9, 6> H = boost:: │ │ │ │ │ +none); │ │ │ │ │ 74 │ │ │ │ │ -75} // \namespace gtsam │ │ │ │ │ -76 │ │ │ │ │ -_T_e_s_t_a_b_l_e_._h │ │ │ │ │ -Concept check for values that can be used in unit tests. │ │ │ │ │ +79GTSAM_EXPORT Matrix43 _s_t_i_e_f_e_l(const SO4 &Q, OptionalJacobian<12, 6> H = │ │ │ │ │ +boost::none); │ │ │ │ │ +80 │ │ │ │ │ +82template │ │ │ │ │ +_8_3void _s_e_r_i_a_l_i_z_e(Archive &ar, _S_O_4 &Q, const unsigned int /*version*/) { │ │ │ │ │ +84 Matrix4 &M = Q.matrix_; │ │ │ │ │ +85 ar &boost::serialization::make_nvp("Q11", M(0, 0)); │ │ │ │ │ +86 ar &boost::serialization::make_nvp("Q12", M(0, 1)); │ │ │ │ │ +87 ar &boost::serialization::make_nvp("Q13", M(0, 2)); │ │ │ │ │ +88 ar &boost::serialization::make_nvp("Q14", M(0, 3)); │ │ │ │ │ +89 │ │ │ │ │ +90 ar &boost::serialization::make_nvp("Q21", M(1, 0)); │ │ │ │ │ +91 ar &boost::serialization::make_nvp("Q22", M(1, 1)); │ │ │ │ │ +92 ar &boost::serialization::make_nvp("Q23", M(1, 2)); │ │ │ │ │ +93 ar &boost::serialization::make_nvp("Q24", M(1, 3)); │ │ │ │ │ +94 │ │ │ │ │ +95 ar &boost::serialization::make_nvp("Q31", M(2, 0)); │ │ │ │ │ +96 ar &boost::serialization::make_nvp("Q32", M(2, 1)); │ │ │ │ │ +97 ar &boost::serialization::make_nvp("Q33", M(2, 2)); │ │ │ │ │ +98 ar &boost::serialization::make_nvp("Q34", M(2, 3)); │ │ │ │ │ +99 │ │ │ │ │ +100 ar &boost::serialization::make_nvp("Q41", M(3, 0)); │ │ │ │ │ +101 ar &boost::serialization::make_nvp("Q42", M(3, 1)); │ │ │ │ │ +102 ar &boost::serialization::make_nvp("Q43", M(3, 2)); │ │ │ │ │ +103 ar &boost::serialization::make_nvp("Q44", M(3, 3)); │ │ │ │ │ +104} │ │ │ │ │ +105 │ │ │ │ │ +106/* │ │ │ │ │ +107 * Define the traits. internal::LieGroup provides both Lie group and │ │ │ │ │ +Testable │ │ │ │ │ +108 */ │ │ │ │ │ +109 │ │ │ │ │ +110template <> │ │ │ │ │ +_1_1_1struct _t_r_a_i_t_s<_S_O_4> : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ +112 │ │ │ │ │ +113template <> │ │ │ │ │ +_1_1_4struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ +115 │ │ │ │ │ +116} // end namespace gtsam │ │ │ │ │ _G_r_o_u_p_._h │ │ │ │ │ Concept check class for variable types with Group properties. │ │ │ │ │ +_M_a_t_r_i_x_._h │ │ │ │ │ +typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +_M_a_n_i_f_o_l_d_._h │ │ │ │ │ +Base class and basic functions for Manifold types. │ │ │ │ │ +_L_i_e_._h │ │ │ │ │ +Base class and basic functions for Lie types. │ │ │ │ │ +_S_O_n_._h │ │ │ │ │ +N*N matrix representation of SO(N). │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e │ │ │ │ │ +std::string serialize(const T &input) │ │ │ │ │ +serializes to a string │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:113 │ │ │ │ │ +_g_t_s_a_m_:_:_s_t_i_e_f_e_l │ │ │ │ │ +GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian< 12, 6 > H) │ │ │ │ │ +Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q) - │ │ │ │ │ +> . │ │ │ │ │ +DDeeffiinniittiioonn SO4.cpp:220 │ │ │ │ │ +_g_t_s_a_m_:_:_t_o_p_L_e_f_t │ │ │ │ │ +GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian< 9, 6 > H) │ │ │ │ │ +Project to top-left 3*3 matrix. │ │ │ │ │ +DDeeffiinniittiioonn SO4.cpp:206 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_C_y_c_l_i_c │ │ │ │ │ -Cyclic group of order N. │ │ │ │ │ -DDeeffiinniittiioonn Cyclic.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_C_y_c_l_i_c_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const Cyclic &other, double tol=1e-9) const │ │ │ │ │ -equals with an tolerance, prints out message if unequal │ │ │ │ │ -DDeeffiinniittiioonn Cyclic.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_C_y_c_l_i_c_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="") const │ │ │ │ │ -print with optional string │ │ │ │ │ -DDeeffiinniittiioonn Cyclic.h:60 │ │ │ │ │ -_g_t_s_a_m_:_:_C_y_c_l_i_c_:_:_C_y_c_l_i_c │ │ │ │ │ -Cyclic() │ │ │ │ │ -Default constructor yields identity. │ │ │ │ │ -DDeeffiinniittiioonn Cyclic.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_C_y_c_l_i_c_:_:_C_y_c_l_i_c │ │ │ │ │ -Cyclic(size_t i) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn Cyclic.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_C_y_c_l_i_c_:_:_o_p_e_r_a_t_o_r_- │ │ │ │ │ -Cyclic operator-(const Cyclic &h) const │ │ │ │ │ -Subtraction modulo N. │ │ │ │ │ -DDeeffiinniittiioonn Cyclic.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_C_y_c_l_i_c_:_:_o_p_e_r_a_t_o_r_+ │ │ │ │ │ -Cyclic operator+(const Cyclic &h) const │ │ │ │ │ -Addition modulo N. │ │ │ │ │ -DDeeffiinniittiioonn Cyclic.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_C_y_c_l_i_c_:_:_o_p_e_r_a_t_o_r_- │ │ │ │ │ -Cyclic operator-() const │ │ │ │ │ -Inverse. │ │ │ │ │ -DDeeffiinniittiioonn Cyclic.h:56 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_<_ _S_O_<_ _N_ _>_,_ _i_n_t_e_r_n_a_l_:_:_D_i_m_e_n_s_i_o_n_S_O_(_N_)_>_:_:_R_e_t_r_a_c_t │ │ │ │ │ +static SO< N > Retract(const TangentVector &v) │ │ │ │ │ +Retract at origin: possible in Lie group because it has an identity. │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ +Both LieGroupTraits and Testable. │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:229 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O │ │ │ │ │ +Manifold of special orthogonal rotation matrices SO. │ │ │ │ │ +DDeeffiinniittiioonn SOn.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_E_x_p_m_a_p │ │ │ │ │ +static SO Expmap(const TangentVector &omega, ChartJacobian H=boost::none) │ │ │ │ │ +Exponential map at identity - create a rotation from canonical coordinates. │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_v_e_c │ │ │ │ │ +VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost:: │ │ │ │ │ +none) const │ │ │ │ │ +Return vectorized rotation matrix in column order. │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:88 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_V_e_e │ │ │ │ │ +static TangentVector Vee(const MatrixNN &X) │ │ │ │ │ +Inverse of Hat. See note about xi element order in Hat. │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_A_d_j_o_i_n_t_M_a_p │ │ │ │ │ +MatrixDD AdjointMap() const │ │ │ │ │ +Adjoint map. │ │ │ │ │ +DDeeffiinniittiioonn SO4.cpp:159 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_H_a_t │ │ │ │ │ +static MatrixNN Hat(const TangentVector &xi) │ │ │ │ │ +Hat operator creates Lie algebra element corresponding to d-vector, where d is │ │ │ │ │ +the dimensionality of ... │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:29 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_y_c_l_i_c_._h │ │ │ │ │ + * _S_O_4_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00320.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3DS2.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Pose2.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,39 +96,39 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces | │ │ │ │ Functions
│ │ │ │ -
Cal3DS2.cpp File Reference
│ │ │ │ +
Pose2.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ + │ │ │ │ +

2D Pose │ │ │ │ +More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

│ │ │ │ -std::ostream & gtsam::operator<< (std::ostream &os, const Cal3DS2 &cal)
 
│ │ │ │ +std::ostream & gtsam::operator<< (std::ostream &os, const Pose2 &pose)
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Feb 28, 2010
│ │ │ │ -
Author
ydjian
│ │ │ │ -
│ │ │ │ -Varun Agrawal
│ │ │ │ +

2D Pose

│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,24 +1,21 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Cal3DS2.cpp File Reference │ │ │ │ │ +Pose2.cpp File Reference │ │ │ │ │ +2D Pose _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _C_a_l_3_D_S_2 &cal) │ │ │ │ │ +std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _P_o_s_e_2 &pose) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Feb 28, 2010 │ │ │ │ │ - Author │ │ │ │ │ - ydjian │ │ │ │ │ - Varun Agrawal │ │ │ │ │ +2D Pose │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3_D_S_2_._c_p_p │ │ │ │ │ + * _P_o_s_e_2_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00323.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Bundler.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3_S2.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,37 +96,40 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces | │ │ │ │ Functions
│ │ │ │ -
Cal3Bundler.cpp File Reference
│ │ │ │ +
Cal3_S2.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ + │ │ │ │ +

The most common 5DOF 3D->2D calibration. │ │ │ │ +More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

│ │ │ │ -std::ostream & gtsam::operator<< (std::ostream &os, const Cal3Bundler &cal)
 
│ │ │ │ +std::ostream & gtsam::operator<< (std::ostream &os, const Cal3_S2 &cal)
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Sep 25, 2010
│ │ │ │ -
Author
ydjian
│ │ │ │ +

The most common 5DOF 3D->2D calibration.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,23 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Cal3Bundler.cpp File Reference │ │ │ │ │ +Cal3_S2.cpp File Reference │ │ │ │ │ +The most common 5DOF 3D->2D calibration. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _C_a_l_3_B_u_n_d_l_e_r &cal) │ │ │ │ │ +std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _C_a_l_3___S_2 &cal) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Sep 25, 2010 │ │ │ │ │ +The most common 5DOF 3D->2D calibration. │ │ │ │ │ Author │ │ │ │ │ - ydjian │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3_B_u_n_d_l_e_r_._c_p_p │ │ │ │ │ + * _C_a_l_3___S_2_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00326.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Pose2.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/StereoPoint2.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,41 +94,56 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces | │ │ │ │ -Functions
│ │ │ │ -
Pose2.cpp File Reference
│ │ │ │ +Typedefs
│ │ │ │ +
StereoPoint2.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

2D Pose │ │ │ │ +

A 2D stereo point (uL,uR,v) │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::StereoPoint2
 A 2D stereo point, v will be same for rectified images. More...
 
struct  gtsam::traits< StereoPoint2 >
 
struct  gtsam::traits< const StereoPoint2 >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -std::ostream & gtsam::operator<< (std::ostream &os, const Pose2 &pose)
 

│ │ │ │ +Typedefs

│ │ │ │ +typedef std::vector< StereoPoint2 > gtsam::StereoPoint2Vector
 
│ │ │ │

Detailed Description

│ │ │ │ -

2D Pose

│ │ │ │ +

A 2D stereo point (uL,uR,v)

│ │ │ │ +
Date
Jan 26, 2010
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,34 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Pose2.cpp File Reference │ │ │ │ │ -2D Pose _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ +StereoPoint2.h File Reference │ │ │ │ │ +A 2D stereo point (uL,uR,v) _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2 │ │ │ │ │ +  A 2D stereo point, v will be same for rectified images. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_t_e_r_e_o_P_o_i_n_t_2_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _S_t_e_r_e_o_P_o_i_n_t_2_ _> │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _P_o_s_e_2 &pose) │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +typedef std::vector< _S_t_e_r_e_o_P_o_i_n_t_2 >  ggttssaamm::::SStteerreeooPPooiinntt22VVeeccttoorr │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -2D Pose │ │ │ │ │ +A 2D stereo point (uL,uR,v) │ │ │ │ │ + Date │ │ │ │ │ + Jan 26, 2010 │ │ │ │ │ + Author │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _P_o_s_e_2_._c_p_p │ │ │ │ │ + * _S_t_e_r_e_o_P_o_i_n_t_2_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00329.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SphericalCamera.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Pose2.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,51 +95,74 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
SphericalCamera.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ +Functions
│ │ │ │ +
Pose2.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Calibrated camera with spherical projection. │ │ │ │ +

2D Pose │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::EmptyCal
 Empty calibration. More...
class  gtsam::Pose2
 A 2D pose (Point2,Rot2) More...
 
class  gtsam::SphericalCamera
 A spherical camera class that has a Pose3 and measures bearing vectors. More...
struct  gtsam::Pose2::ChartAtOrigin
 
struct  gtsam::traits< SphericalCamera >
struct  gtsam::traits< Pose2 >
 
struct  gtsam::traits< const SphericalCamera >
struct  gtsam::traits< const Pose2 >
 
struct  gtsam::Bearing< Pose2, T >
 
struct  gtsam::Range< Pose2, T >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Typedefs

│ │ │ │ +using gtsam::Pose2Pair = std::pair< Pose2, Pose2 >
 
│ │ │ │ +using gtsam::Pose2Pairs = std::vector< Pose2Pair >
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +template<>
Matrix gtsam::wedge< Pose2 > (const Vector &xi)
 specialization for pose2 wedge function (generic template in Lie.h)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Calibrated camera with spherical projection.

│ │ │ │ -
Date
Aug 26, 2021
│ │ │ │ -
Author
Luca Carlone
│ │ │ │ +

2D Pose

│ │ │ │ +
Author
: Frank Dellaert
│ │ │ │ +
│ │ │ │ +: Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,35 +1,46 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SphericalCamera.h File Reference │ │ │ │ │ -Calibrated camera with spherical projection. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Pose2.h File Reference │ │ │ │ │ +2D Pose _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_E_m_p_t_y_C_a_l │ │ │ │ │ -  Empty calibration. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_P_o_s_e_2 │ │ │ │ │ +  A 2D pose (Point2,_R_o_t_2) _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ - class   _g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a │ │ │ │ │ -  A spherical camera class that has a _P_o_s_e_3 and measures bearing │ │ │ │ │ - vectors. _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_P_o_s_e_2_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _P_o_s_e_2_ _> │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _P_o_s_e_2_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_B_e_a_r_i_n_g_<_ _P_o_s_e_2_,_ _T_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_R_a_n_g_e_<_ _P_o_s_e_2_,_ _T_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +using  ggttssaamm::::PPoossee22PPaaiirr = std::pair< _P_o_s_e_2, _P_o_s_e_2 > │ │ │ │ │ +  │ │ │ │ │ +using  ggttssaamm::::PPoossee22PPaaiirrss = std::vector< Pose2Pair > │ │ │ │ │ +  │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +template<> │ │ │ │ │ + Matrix  ggttssaamm::::wweeddggee<< PPoossee22 >> (const Vector &xi) │ │ │ │ │ +  specialization for pose2 wedge function (generic template in _L_i_e_._h) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Calibrated camera with spherical projection. │ │ │ │ │ - Date │ │ │ │ │ - Aug 26, 2021 │ │ │ │ │ +2D Pose │ │ │ │ │ Author │ │ │ │ │ - Luca Carlone │ │ │ │ │ + : Frank Dellaert │ │ │ │ │ + : Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_._h │ │ │ │ │ + * _P_o_s_e_2_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00329.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,8 @@ │ │ │ │ │ var a00329 = [ │ │ │ │ │ - ["gtsam::traits< SphericalCamera >", "a03328.html", null], │ │ │ │ │ - ["gtsam::traits< const SphericalCamera >", "a03332.html", null] │ │ │ │ │ + ["gtsam::Pose2::ChartAtOrigin", "a03160.html", null], │ │ │ │ │ + ["gtsam::traits< Pose2 >", "a03164.html", null], │ │ │ │ │ + ["gtsam::traits< const Pose2 >", "a03168.html", null], │ │ │ │ │ + ["gtsam::Bearing< Pose2, T >", "a03172.html", null], │ │ │ │ │ + ["gtsam::Range< Pose2, T >", "a03176.html", null], │ │ │ │ │ + ["wedge< Pose2 >", "a00329.html#a4b78f4db0e05f3ca838ee4da6187710e", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00329_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SphericalCamera.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Pose2.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,245 +98,337 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
SphericalCamera.h
│ │ │ │ +
Pose2.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ +
19// \callgraph
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/base/Manifold.h>
│ │ │ │ -
22#include <gtsam/base/ThreadsafeException.h>
│ │ │ │ -
23#include <gtsam/base/concepts.h>
│ │ │ │ -
24#include <gtsam/dllexport.h>
│ │ │ │ -
25#include <gtsam/geometry/BearingRange.h>
│ │ │ │ -
26#include <gtsam/geometry/Pose3.h>
│ │ │ │ -
27#include <gtsam/geometry/Unit3.h>
│ │ │ │ +
21#pragma once
│ │ │ │ +
22
│ │ │ │ +
23#include <gtsam/geometry/BearingRange.h>
│ │ │ │ +
24#include <gtsam/geometry/Point2.h>
│ │ │ │ +
25#include <gtsam/geometry/Rot2.h>
│ │ │ │ +
26#include <gtsam/base/Lie.h>
│ │ │ │ +
27#include <gtsam/dllexport.h>
│ │ │ │
28
│ │ │ │ -
29#include <boost/serialization/nvp.hpp>
│ │ │ │ +
29namespace gtsam {
│ │ │ │
30
│ │ │ │ -
31namespace gtsam {
│ │ │ │ -
32
│ │ │ │ -
│ │ │ │ -
40class GTSAM_EXPORT EmptyCal {
│ │ │ │ -
41 public:
│ │ │ │ -
42 enum { dimension = 0 };
│ │ │ │ -
43 EmptyCal() {}
│ │ │ │ -
44 virtual ~EmptyCal() = default;
│ │ │ │ -
45 using shared_ptr = boost::shared_ptr<EmptyCal>;
│ │ │ │ -
46
│ │ │ │ -
48 inline static size_t Dim() { return dimension; }
│ │ │ │ -
49
│ │ │ │ -
50 void print(const std::string& s) const {
│ │ │ │ -
51 std::cout << "empty calibration: " << s << std::endl;
│ │ │ │ -
52 }
│ │ │ │ +
│ │ │ │ +
36class Pose2: public LieGroup<Pose2, 3> {
│ │ │ │ +
37
│ │ │ │ +
38public:
│ │ │ │ +
39
│ │ │ │ +
41 typedef Rot2 Rotation;
│ │ │ │ +
42 typedef Point2 Translation;
│ │ │ │ +
43
│ │ │ │ +
44private:
│ │ │ │ +
45
│ │ │ │ +
46 Rot2 r_;
│ │ │ │ +
47 Point2 t_;
│ │ │ │ +
48
│ │ │ │ +
49public:
│ │ │ │ +
50
│ │ │ │
53
│ │ │ │ -
54 private:
│ │ │ │ -
56 friend class boost::serialization::access;
│ │ │ │ -
57 template <class Archive>
│ │ │ │ -
58 void serialize(Archive& ar, const unsigned int /*version*/) {
│ │ │ │ -
59 ar& boost::serialization::make_nvp(
│ │ │ │ -
60 "EmptyCal", boost::serialization::base_object<EmptyCal>(*this));
│ │ │ │ -
61 }
│ │ │ │ -
62};
│ │ │ │ -
│ │ │ │ -
63
│ │ │ │ -
│ │ │ │ -
70class GTSAM_EXPORT SphericalCamera {
│ │ │ │ -
71 public:
│ │ │ │ -
72 enum { dimension = 6 };
│ │ │ │ -
73
│ │ │ │ -
74 using Measurement = Unit3;
│ │ │ │ -
75 using MeasurementVector = std::vector<Unit3>;
│ │ │ │ -
76 using CalibrationType = EmptyCal;
│ │ │ │ -
77
│ │ │ │ -
78 private:
│ │ │ │ -
79 Pose3 pose_;
│ │ │ │ -
80
│ │ │ │ -
81 protected:
│ │ │ │ -
82 EmptyCal::shared_ptr emptyCal_;
│ │ │ │ -
83
│ │ │ │ -
84 public:
│ │ │ │ -
87
│ │ │ │ -
│ │ │ │ -
89 SphericalCamera()
│ │ │ │ -
90 : pose_(Pose3()), emptyCal_(boost::make_shared<EmptyCal>()) {}
│ │ │ │ -
│ │ │ │ -
91
│ │ │ │ -
│ │ │ │ -
93 explicit SphericalCamera(const Pose3& pose)
│ │ │ │ -
94 : pose_(pose), emptyCal_(boost::make_shared<EmptyCal>()) {}
│ │ │ │ -
│ │ │ │ -
95
│ │ │ │ -
│ │ │ │ -
97 explicit SphericalCamera(const Pose3& pose,
│ │ │ │ -
98 const EmptyCal::shared_ptr& cal)
│ │ │ │ -
99 : pose_(pose), emptyCal_(cal) {}
│ │ │ │ -
│ │ │ │ -
100
│ │ │ │ -
104 explicit SphericalCamera(const Vector& v) : pose_(Pose3::Expmap(v)) {}
│ │ │ │ -
105
│ │ │ │ -
107 virtual ~SphericalCamera() = default;
│ │ │ │ -
108
│ │ │ │ -
│ │ │ │ -
110 const EmptyCal::shared_ptr& sharedCalibration() const {
│ │ │ │ -
111 return emptyCal_;
│ │ │ │ -
112 }
│ │ │ │ -
│ │ │ │ +
│ │ │ │ +
55 Pose2() :
│ │ │ │ +
56 r_(traits<Rot2>::Identity()), t_(traits<Point2>::Identity()) {
│ │ │ │ +
57 }
│ │ │ │ +
│ │ │ │ +
58
│ │ │ │ +
60 Pose2(const Pose2& pose) : r_(pose.r_), t_(pose.t_) {}
│ │ │ │ +
61
│ │ │ │ +
│ │ │ │ +
68 Pose2(double x, double y, double theta) :
│ │ │ │ +
69 r_(Rot2::fromAngle(theta)), t_(x, y) {
│ │ │ │ +
70 }
│ │ │ │ +
│ │ │ │ +
71
│ │ │ │ +
│ │ │ │ +
73 Pose2(double theta, const Point2& t) :
│ │ │ │ +
74 r_(Rot2::fromAngle(theta)), t_(t) {
│ │ │ │ +
75 }
│ │ │ │ +
│ │ │ │ +
76
│ │ │ │ +
78 Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {}
│ │ │ │ +
79
│ │ │ │ +
│ │ │ │ +
81 Pose2(const Matrix &T) :
│ │ │ │ +
82 r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) {
│ │ │ │ +
83 assert(T.rows() == 3 && T.cols() == 3);
│ │ │ │ +
84 }
│ │ │ │ +
│ │ │ │ +
85
│ │ │ │ +
89
│ │ │ │ +
│ │ │ │ +
91 Pose2(const Vector& v) : Pose2() {
│ │ │ │ +
92 *this = Expmap(v);
│ │ │ │ +
93 }
│ │ │ │ +
│ │ │ │ +
94
│ │ │ │ +
102 static boost::optional<Pose2> Align(const Point2Pairs& abPointPairs);
│ │ │ │ +
103
│ │ │ │ +
104 // Version of Pose2::Align that takes 2 matrices.
│ │ │ │ +
105 static boost::optional<Pose2> Align(const Matrix& a, const Matrix& b);
│ │ │ │ +
106
│ │ │ │ +
110
│ │ │ │ +
112 GTSAM_EXPORT void print(const std::string& s = "") const;
│ │ │ │
113
│ │ │ │ -
115 const EmptyCal& calibration() const { return *emptyCal_; }
│ │ │ │ +
115 GTSAM_EXPORT bool equals(const Pose2& pose, double tol = 1e-9) const;
│ │ │ │
116
│ │ │ │
120
│ │ │ │ -
122 bool equals(const SphericalCamera& camera, double tol = 1e-9) const;
│ │ │ │ +
122 inline static Pose2 Identity() { return Pose2(); }
│ │ │ │
123
│ │ │ │ -
125 virtual void print(const std::string& s = "SphericalCamera") const;
│ │ │ │ +
125 GTSAM_EXPORT Pose2 inverse() const;
│ │ │ │
126
│ │ │ │ -
130
│ │ │ │ -
132 const Pose3& pose() const { return pose_; }
│ │ │ │ -
133
│ │ │ │ -
135 const Rot3& rotation() const { return pose_.rotation(); }
│ │ │ │ -
136
│ │ │ │ -
138 const Point3& translation() const { return pose_.translation(); }
│ │ │ │ -
139
│ │ │ │ -
140 // /// return pose, with derivative
│ │ │ │ -
141 // const Pose3& getPose(OptionalJacobian<6, 6> H) const;
│ │ │ │ -
142
│ │ │ │ -
146
│ │ │ │ -
148 std::pair<Unit3, bool> projectSafe(const Point3& pw) const;
│ │ │ │ -
149
│ │ │ │ -
155 Unit3 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
│ │ │ │ -
156 OptionalJacobian<2, 3> Dpoint = boost::none) const;
│ │ │ │ +
│ │ │ │ +
128 inline Pose2 operator*(const Pose2& p2) const {
│ │ │ │ +
129 return Pose2(r_*p2.r(), t_ + r_*p2.t());
│ │ │ │ +
130 }
│ │ │ │ +
│ │ │ │ +
131
│ │ │ │ +
135
│ │ │ │ +
137 GTSAM_EXPORT static Pose2 Expmap(const Vector3& xi, ChartJacobian H = boost::none);
│ │ │ │ +
138
│ │ │ │ +
140 GTSAM_EXPORT static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none);
│ │ │ │ +
141
│ │ │ │ +
146 GTSAM_EXPORT Matrix3 AdjointMap() const;
│ │ │ │ +
147
│ │ │ │ +
│ │ │ │ +
149 inline Vector3 Adjoint(const Vector3& xi) const {
│ │ │ │ +
150 return AdjointMap()*xi;
│ │ │ │ +
151 }
│ │ │ │ +
│ │ │ │ +
152
│ │ │ │ +
156 GTSAM_EXPORT static Matrix3 adjointMap(const Vector3& v);
│ │ │ │
157
│ │ │ │ -
163 Unit3 project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose = boost::none,
│ │ │ │ -
164 OptionalJacobian<2, 2> Dpoint = boost::none) const;
│ │ │ │ -
165
│ │ │ │ -
167 Point3 backproject(const Unit3& p, const double depth) const;
│ │ │ │ -
168
│ │ │ │ -
170 Unit3 backprojectPointAtInfinity(const Unit3& p) const;
│ │ │ │ +
│ │ │ │ +
161 static Vector3 adjoint(const Vector3& xi, const Vector3& y) {
│ │ │ │ +
162 return adjointMap(xi) * y;
│ │ │ │ +
163 }
│ │ │ │ +
│ │ │ │ +
164
│ │ │ │ +
│ │ │ │ +
168 static Vector3 adjointTranspose(const Vector3& xi, const Vector3& y) {
│ │ │ │ +
169 return adjointMap(xi).transpose() * y;
│ │ │ │ +
170 }
│ │ │ │ +
│ │ │ │
171
│ │ │ │ -
177 Unit3 project(const Point3& point, OptionalJacobian<2, 6> Dpose = boost::none,
│ │ │ │ -
178 OptionalJacobian<2, 3> Dpoint = boost::none) const;
│ │ │ │ -
179
│ │ │ │ -
184 Vector2 reprojectionError(const Point3& point, const Unit3& measured,
│ │ │ │ -
185 OptionalJacobian<2, 6> Dpose = boost::none,
│ │ │ │ -
186 OptionalJacobian<2, 3> Dpoint = boost::none) const;
│ │ │ │ -
188
│ │ │ │ -
│ │ │ │ -
190 SphericalCamera retract(const Vector6& d) const {
│ │ │ │ -
191 return SphericalCamera(pose().retract(d));
│ │ │ │ -
192 }
│ │ │ │ +
172 // temporary fix for wrappers until case issue is resolved
│ │ │ │ +
173 static Matrix3 adjointMap_(const Vector3 &xi) { return adjointMap(xi);}
│ │ │ │ +
174 static Vector3 adjoint_(const Vector3 &xi, const Vector3 &y) { return adjoint(xi, y);}
│ │ │ │ +
175
│ │ │ │ +
│ │ │ │ +
183 static inline Matrix3 wedge(double vx, double vy, double w) {
│ │ │ │ +
184 Matrix3 m;
│ │ │ │ +
185 m << 0.,-w, vx,
│ │ │ │ +
186 w, 0., vy,
│ │ │ │ +
187 0., 0., 0.;
│ │ │ │ +
188 return m;
│ │ │ │ +
189 }
│ │ │ │
│ │ │ │ +
190
│ │ │ │ +
192 GTSAM_EXPORT static Matrix3 ExpmapDerivative(const Vector3& v);
│ │ │ │
193
│ │ │ │ -
│ │ │ │ -
195 Vector6 localCoordinates(const SphericalCamera& p) const {
│ │ │ │ -
196 return pose().localCoordinates(p.pose());
│ │ │ │ -
197 }
│ │ │ │ -
│ │ │ │ -
198
│ │ │ │ -
│ │ │ │ -
200 static SphericalCamera Identity() {
│ │ │ │ -
201 return SphericalCamera(
│ │ │ │ -
202 Pose3::Identity()); // assumes that the default constructor is valid
│ │ │ │ -
203 }
│ │ │ │ +
195 GTSAM_EXPORT static Matrix3 LogmapDerivative(const Pose2& v);
│ │ │ │ +
196
│ │ │ │ +
197 // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP
│ │ │ │ +
│ │ │ │ +
198 struct ChartAtOrigin {
│ │ │ │ +
199 GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = boost::none);
│ │ │ │ +
200 GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = boost::none);
│ │ │ │ +
201 };
│ │ │ │
│ │ │ │ +
202
│ │ │ │ +
203 using LieGroup<Pose2, 3>::inverse; // version with derivative
│ │ │ │
204
│ │ │ │ -
│ │ │ │ -
206 Matrix34 cameraProjectionMatrix() const {
│ │ │ │ -
207 return Matrix34(pose_.inverse().matrix().block(0, 0, 3, 4));
│ │ │ │ -
208 }
│ │ │ │ -
│ │ │ │ -
209
│ │ │ │ -
│ │ │ │ -
211 Vector defaultErrorWhenTriangulatingBehindCamera() const {
│ │ │ │ -
212 return Eigen::Matrix<double, traits<Point2>::dimension, 1>::Constant(0.0);
│ │ │ │ -
213 }
│ │ │ │ -
│ │ │ │ -
214
│ │ │ │ -
216 size_t dim() const { return 6; }
│ │ │ │ -
217
│ │ │ │ -
219 static size_t Dim() { return 6; }
│ │ │ │ +
208
│ │ │ │ +
210 GTSAM_EXPORT Point2 transformTo(const Point2& point,
│ │ │ │ +
211 OptionalJacobian<2, 3> Dpose = boost::none,
│ │ │ │ +
212 OptionalJacobian<2, 2> Dpoint = boost::none) const;
│ │ │ │ +
213
│ │ │ │ +
219 Matrix transformTo(const Matrix& points) const;
│ │ │ │
220
│ │ │ │ -
221 private:
│ │ │ │ -
223 friend class boost::serialization::access;
│ │ │ │ -
224 template <class Archive>
│ │ │ │ -
225 void serialize(Archive& ar, const unsigned int /*version*/) {
│ │ │ │ -
226 ar& BOOST_SERIALIZATION_NVP(pose_);
│ │ │ │ -
227 }
│ │ │ │ -
228
│ │ │ │ -
229 public:
│ │ │ │ -
230 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
231};
│ │ │ │ -
│ │ │ │ -
232// end of class SphericalCamera
│ │ │ │ -
233
│ │ │ │ -
234template <>
│ │ │ │ -
235struct traits<SphericalCamera> : public internal::LieGroup<Pose3> {};
│ │ │ │ -
236
│ │ │ │ -
237template <>
│ │ │ │ -
238struct traits<const SphericalCamera> : public internal::LieGroup<Pose3> {};
│ │ │ │ -
239
│ │ │ │ -
240} // namespace gtsam
│ │ │ │ -
ThreadsafeException.h
Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
│ │ │ │ -
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ -
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ -
Pose3.h
3D Pose
│ │ │ │ -
BearingRange.h
Bearing-Range product.
│ │ │ │ +
222 GTSAM_EXPORT Point2 transformFrom(const Point2& point,
│ │ │ │ +
223 OptionalJacobian<2, 3> Dpose = boost::none,
│ │ │ │ +
224 OptionalJacobian<2, 2> Dpoint = boost::none) const;
│ │ │ │ +
225
│ │ │ │ +
231 Matrix transformFrom(const Matrix& points) const;
│ │ │ │ +
232
│ │ │ │ +
│ │ │ │ +
234 inline Point2 operator*(const Point2& point) const {
│ │ │ │ +
235 return transformFrom(point);
│ │ │ │ +
236 }
│ │ │ │ +
│ │ │ │ +
237
│ │ │ │ +
241
│ │ │ │ +
243 inline double x() const { return t_.x(); }
│ │ │ │ +
244
│ │ │ │ +
246 inline double y() const { return t_.y(); }
│ │ │ │ +
247
│ │ │ │ +
249 inline double theta() const { return r_.theta(); }
│ │ │ │ +
250
│ │ │ │ +
252 inline const Point2& t() const { return t_; }
│ │ │ │ +
253
│ │ │ │ +
255 inline const Rot2& r() const { return r_; }
│ │ │ │ +
256
│ │ │ │ +
258 inline const Point2& translation() const { return t_; }
│ │ │ │ +
259
│ │ │ │ +
261 inline const Rot2& rotation() const { return r_; }
│ │ │ │ +
262
│ │ │ │ +
264 GTSAM_EXPORT Matrix3 matrix() const;
│ │ │ │ +
265
│ │ │ │ +
271 GTSAM_EXPORT Rot2 bearing(const Point2& point,
│ │ │ │ +
272 OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 2> H2=boost::none) const;
│ │ │ │ +
273
│ │ │ │ +
279 GTSAM_EXPORT Rot2 bearing(const Pose2& pose,
│ │ │ │ +
280 OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 3> H2=boost::none) const;
│ │ │ │ +
281
│ │ │ │ +
287 GTSAM_EXPORT double range(const Point2& point,
│ │ │ │ +
288 OptionalJacobian<1, 3> H1=boost::none,
│ │ │ │ +
289 OptionalJacobian<1, 2> H2=boost::none) const;
│ │ │ │ +
290
│ │ │ │ +
296 GTSAM_EXPORT double range(const Pose2& point,
│ │ │ │ +
297 OptionalJacobian<1, 3> H1=boost::none,
│ │ │ │ +
298 OptionalJacobian<1, 3> H2=boost::none) const;
│ │ │ │ +
299
│ │ │ │ +
303
│ │ │ │ +
309 inline static std::pair<size_t, size_t> translationInterval() { return std::make_pair(0, 1); }
│ │ │ │ +
310
│ │ │ │ +
316 static std::pair<size_t, size_t> rotationInterval() { return std::make_pair(2, 2); }
│ │ │ │ +
317
│ │ │ │ +
319 GTSAM_EXPORT
│ │ │ │ +
320 friend std::ostream &operator<<(std::ostream &os, const Pose2& p);
│ │ │ │ +
321
│ │ │ │ +
323
│ │ │ │ +
324 private:
│ │ │ │ +
325
│ │ │ │ +
326 // Serialization function
│ │ │ │ +
327 friend class boost::serialization::access;
│ │ │ │ +
328 template<class Archive>
│ │ │ │ +
329 void serialize(Archive & ar, const unsigned int /*version*/) {
│ │ │ │ +
330 ar & BOOST_SERIALIZATION_NVP(t_);
│ │ │ │ +
331 ar & BOOST_SERIALIZATION_NVP(r_);
│ │ │ │ +
332 }
│ │ │ │ +
333
│ │ │ │ +
334public:
│ │ │ │ +
335 // Align for Point2, which is either derived from, or is typedef, of Vector2
│ │ │ │ +
336 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
337}; // Pose2
│ │ │ │ +
│ │ │ │ +
338
│ │ │ │ +
340template <>
│ │ │ │ +
│ │ │ │ +
341inline Matrix wedge<Pose2>(const Vector& xi) {
│ │ │ │ +
342 // NOTE(chris): Need eval() as workaround for Apple clang + avx2.
│ │ │ │ +
343 return Matrix(Pose2::wedge(xi(0),xi(1),xi(2))).eval();
│ │ │ │ +
344}
│ │ │ │ +
│ │ │ │ +
345
│ │ │ │ +
346#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ +
352GTSAM_EXPORT boost::optional<Pose2>
│ │ │ │ +
353GTSAM_DEPRECATED align(const Point2Pairs& pairs);
│ │ │ │ +
354#endif
│ │ │ │ +
355
│ │ │ │ +
356// Convenience typedef
│ │ │ │ +
357using Pose2Pair = std::pair<Pose2, Pose2>;
│ │ │ │ +
358using Pose2Pairs = std::vector<Pose2Pair>;
│ │ │ │ +
359
│ │ │ │ +
360template <>
│ │ │ │ +
361struct traits<Pose2> : public internal::LieGroup<Pose2> {};
│ │ │ │ +
362
│ │ │ │ +
363template <>
│ │ │ │ +
364struct traits<const Pose2> : public internal::LieGroup<Pose2> {};
│ │ │ │ +
365
│ │ │ │ +
366// bearing and range traits, used in RangeFactor
│ │ │ │ +
367template <typename T>
│ │ │ │ +
368struct Bearing<Pose2, T> : HasBearing<Pose2, T, Rot2> {};
│ │ │ │ +
369
│ │ │ │ +
370template <typename T>
│ │ │ │ +
371struct Range<Pose2, T> : HasRange<Pose2, T, double> {};
│ │ │ │ +
372
│ │ │ │ +
373} // namespace gtsam
│ │ │ │ +
374
│ │ │ │ +
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ +
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ +
Point2.h
2D Point
│ │ │ │ +
Rot2.h
2D rotation
│ │ │ │ +
BearingRange.h
Bearing-Range product.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ -
gtsam::make_shared
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost::shared_ptr< T > > make_shared(Args &&... args)
Add our own make_shared as a layer of wrapping on boost::make_shared This solves the problem with the...
Definition make_shared.h:57
│ │ │ │ -
gtsam::project
Point2_ project(const Point3_ &p_cam)
Expression version of PinholeBase::Project.
Definition expressions.h:131
│ │ │ │ +
gtsam::wedge< Pose2 >
Matrix wedge< Pose2 >(const Vector &xi)
specialization for pose2 wedge function (generic template in Lie.h)
Definition Pose2.h:341
│ │ │ │ +
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::LieGroup
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition Lie.h:37
│ │ │ │
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition Lie.h:229
│ │ │ │
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ -
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get translation
Definition Pose3.cpp:308
│ │ │ │ -
gtsam::Pose3::matrix
Matrix4 matrix() const
convert to 4*4 matrix
Definition Pose3.cpp:323
│ │ │ │ -
gtsam::Pose3::inverse
Pose3 inverse() const
inverse transformation with derivatives
Definition Pose3.cpp:49
│ │ │ │ -
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get rotation
Definition Pose3.cpp:315
│ │ │ │ -
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
│ │ │ │ -
gtsam::EmptyCal
Empty calibration.
Definition SphericalCamera.h:40
│ │ │ │ -
gtsam::EmptyCal::Dim
static size_t Dim()
return DOF, dimensionality of tangent space
Definition SphericalCamera.h:48
│ │ │ │ -
gtsam::SphericalCamera
A spherical camera class that has a Pose3 and measures bearing vectors.
Definition SphericalCamera.h:70
│ │ │ │ -
gtsam::SphericalCamera::sharedCalibration
const EmptyCal::shared_ptr & sharedCalibration() const
return shared pointer to calibration
Definition SphericalCamera.h:110
│ │ │ │ -
gtsam::SphericalCamera::Dim
static size_t Dim()
Definition SphericalCamera.h:219
│ │ │ │ -
gtsam::SphericalCamera::SphericalCamera
SphericalCamera(const Pose3 &pose)
Constructor with pose.
Definition SphericalCamera.h:93
│ │ │ │ -
gtsam::SphericalCamera::~SphericalCamera
virtual ~SphericalCamera()=default
Default destructor.
│ │ │ │ -
gtsam::SphericalCamera::cameraProjectionMatrix
Matrix34 cameraProjectionMatrix() const
for Linear Triangulation
Definition SphericalCamera.h:206
│ │ │ │ -
gtsam::SphericalCamera::localCoordinates
Vector6 localCoordinates(const SphericalCamera &p) const
return canonical coordinate
Definition SphericalCamera.h:195
│ │ │ │ -
gtsam::SphericalCamera::rotation
const Rot3 & rotation() const
get rotation
Definition SphericalCamera.h:135
│ │ │ │ -
gtsam::SphericalCamera::pose
const Pose3 & pose() const
return pose, constant version
Definition SphericalCamera.h:132
│ │ │ │ -
gtsam::SphericalCamera::translation
const Point3 & translation() const
get translation
Definition SphericalCamera.h:138
│ │ │ │ -
gtsam::SphericalCamera::retract
SphericalCamera retract(const Vector6 &d) const
move a cameras according to d
Definition SphericalCamera.h:190
│ │ │ │ -
gtsam::SphericalCamera::calibration
const EmptyCal & calibration() const
return calibration
Definition SphericalCamera.h:115
│ │ │ │ -
gtsam::SphericalCamera::dim
size_t dim() const
Definition SphericalCamera.h:216
│ │ │ │ -
gtsam::SphericalCamera::Identity
static SphericalCamera Identity()
for Canonical
Definition SphericalCamera.h:200
│ │ │ │ -
gtsam::SphericalCamera::SphericalCamera
SphericalCamera()
Default constructor.
Definition SphericalCamera.h:89
│ │ │ │ -
gtsam::SphericalCamera::defaultErrorWhenTriangulatingBehindCamera
Vector defaultErrorWhenTriangulatingBehindCamera() const
for Nonlinear Triangulation
Definition SphericalCamera.h:211
│ │ │ │ -
gtsam::SphericalCamera::SphericalCamera
SphericalCamera(const Pose3 &pose, const EmptyCal::shared_ptr &cal)
Constructor with empty intrinsics (needed for smart factors)
Definition SphericalCamera.h:97
│ │ │ │ -
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
│ │ │ │ +
gtsam::Bearing
Definition BearingRange.h:34
│ │ │ │ +
gtsam::Range
Definition BearingRange.h:40
│ │ │ │ +
gtsam::HasBearing
Definition BearingRange.h:180
│ │ │ │ +
gtsam::HasRange
Definition BearingRange.h:194
│ │ │ │ +
gtsam::Pose2
A 2D pose (Point2,Rot2)
Definition Pose2.h:36
│ │ │ │ +
gtsam::Pose2::bearing
GTSAM_EXPORT Rot2 bearing(const Point2 &point, OptionalJacobian< 1, 3 > H1=boost::none, OptionalJacobian< 1, 2 > H2=boost::none) const
Calculate bearing to a landmark.
Definition Pose2.cpp:245
│ │ │ │ +
gtsam::Pose2::operator*
Pose2 operator*(const Pose2 &p2) const
compose syntactic sugar
Definition Pose2.h:128
│ │ │ │ +
gtsam::Pose2::print
GTSAM_EXPORT void print(const std::string &s="") const
print with optional string
Definition Pose2.cpp:50
│ │ │ │ +
gtsam::Pose2::rotation
const Rot2 & rotation() const
rotation
Definition Pose2.h:261
│ │ │ │ +
gtsam::Pose2::AdjointMap
GTSAM_EXPORT Matrix3 AdjointMap() const
Calculate Adjoint map Ad_pose is 3*3 matrix that when applied to twist xi , returns Ad_pose(xi)
Definition Pose2.cpp:126
│ │ │ │ +
gtsam::Pose2::LogmapDerivative
static GTSAM_EXPORT Matrix3 LogmapDerivative(const Pose2 &v)
Derivative of Logmap.
Definition Pose2.cpp:179
│ │ │ │ +
gtsam::Pose2::y
double y() const
get y
Definition Pose2.h:246
│ │ │ │ +
gtsam::Pose2::Logmap
static GTSAM_EXPORT Vector3 Logmap(const Pose2 &p, ChartJacobian H=boost::none)
Log map at identity - return the canonical coordinates of this rotation.
Definition Pose2.cpp:82
│ │ │ │ +
gtsam::Pose2::inverse
GTSAM_EXPORT Pose2 inverse() const
inverse
Definition Pose2.cpp:201
│ │ │ │ +
gtsam::Pose2::operator<<
GTSAM_EXPORT friend std::ostream & operator<<(std::ostream &os, const Pose2 &p)
Output stream operator.
Definition Pose2.cpp:55
│ │ │ │ +
gtsam::Pose2::operator*
Point2 operator*(const Point2 &point) const
syntactic sugar for transformFrom
Definition Pose2.h:234
│ │ │ │ +
gtsam::Pose2::Pose2
Pose2(double x, double y, double theta)
construct from (x,y,theta)
Definition Pose2.h:68
│ │ │ │ +
gtsam::Pose2::t
const Point2 & t() const
translation
Definition Pose2.h:252
│ │ │ │ +
gtsam::Pose2::translationInterval
static std::pair< size_t, size_t > translationInterval()
Return the start and end indices (inclusive) of the translation component of the exponential map para...
Definition Pose2.h:309
│ │ │ │ +
gtsam::Pose2::Pose2
Pose2(double theta, const Point2 &t)
construct from rotation and translation
Definition Pose2.h:73
│ │ │ │ +
gtsam::Pose2::x
double x() const
get x
Definition Pose2.h:243
│ │ │ │ +
gtsam::Pose2::r
const Rot2 & r() const
rotation
Definition Pose2.h:255
│ │ │ │ +
gtsam::Pose2::Pose2
Pose2(const Vector &v)
Construct from canonical coordinates (Lie algebra)
Definition Pose2.h:91
│ │ │ │ +
gtsam::Pose2::Adjoint
Vector3 Adjoint(const Vector3 &xi) const
Apply AdjointMap to twist xi.
Definition Pose2.h:149
│ │ │ │ +
gtsam::Pose2::wedge
static Matrix3 wedge(double vx, double vy, double w)
wedge for SE(2):
Definition Pose2.h:183
│ │ │ │ +
gtsam::Pose2::rotationInterval
static std::pair< size_t, size_t > rotationInterval()
Return the start and end indices (inclusive) of the rotation component of the exponential map paramet...
Definition Pose2.h:316
│ │ │ │ +
gtsam::Pose2::adjointMap
static GTSAM_EXPORT Matrix3 adjointMap(const Vector3 &v)
Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19.
Definition Pose2.cpp:137
│ │ │ │ +
gtsam::Pose2::transformTo
GTSAM_EXPORT Point2 transformTo(const Point2 &point, OptionalJacobian< 2, 3 > Dpose=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const
Return point coordinates in pose coordinate frame.
Definition Pose2.cpp:207
│ │ │ │ +
gtsam::Pose2::theta
double theta() const
get theta
Definition Pose2.h:249
│ │ │ │ +
gtsam::Pose2::ExpmapDerivative
static GTSAM_EXPORT Matrix3 ExpmapDerivative(const Vector3 &v)
Derivative of Expmap.
Definition Pose2.cpp:148
│ │ │ │ +
gtsam::Pose2::Rotation
Rot2 Rotation
Pose Concept requirements.
Definition Pose2.h:41
│ │ │ │ +
gtsam::Pose2::Pose2
Pose2()
default constructor = origin
Definition Pose2.h:55
│ │ │ │ +
gtsam::Pose2::transformFrom
GTSAM_EXPORT Point2 transformFrom(const Point2 &point, OptionalJacobian< 2, 3 > Dpose=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const
Return point coordinates in global frame.
Definition Pose2.cpp:226
│ │ │ │ +
gtsam::Pose2::adjointTranspose
static Vector3 adjointTranspose(const Vector3 &xi, const Vector3 &y)
The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
Definition Pose2.h:168
│ │ │ │ +
gtsam::Pose2::Expmap
static GTSAM_EXPORT Pose2 Expmap(const Vector3 &xi, ChartJacobian H=boost::none)
Exponential map at identity - create a rotation from canonical coordinates .
Definition Pose2.cpp:66
│ │ │ │ +
gtsam::Pose2::Identity
static Pose2 Identity()
identity for group operation
Definition Pose2.h:122
│ │ │ │ +
gtsam::Pose2::Align
static boost::optional< Pose2 > Align(const Point2Pairs &abPointPairs)
Create Pose2 by aligning two point pairs A pose aTb is estimated between pairs (a_point,...
Definition Pose2.cpp:330
│ │ │ │ +
gtsam::Pose2::Pose2
Pose2(const Rot2 &r, const Point2 &t)
construct from r,t
Definition Pose2.h:78
│ │ │ │ +
gtsam::Pose2::adjoint
static Vector3 adjoint(const Vector3 &xi, const Vector3 &y)
Action of the adjointMap on a Lie-algebra vector y, with optional derivatives.
Definition Pose2.h:161
│ │ │ │ +
gtsam::Pose2::Pose2
Pose2(const Pose2 &pose)
copy constructor
Definition Pose2.h:60
│ │ │ │ +
gtsam::Pose2::translation
const Point2 & translation() const
translation
Definition Pose2.h:258
│ │ │ │ +
gtsam::Pose2::Pose2
Pose2(const Matrix &T)
Constructor from 3*3 matrix.
Definition Pose2.h:81
│ │ │ │ +
gtsam::Pose2::ChartAtOrigin
Definition Pose2.h:198
│ │ │ │ +
gtsam::Rot2
Rotation matrix NOTE: the angle theta is in radians unless explicitly stated.
Definition Rot2.h:36
│ │ │ │ +
gtsam::Rot2::theta
double theta() const
return angle (RADIANS)
Definition Rot2.h:187
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,323 +1,472 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SphericalCamera.h │ │ │ │ │ +Pose2.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ +19// \callgraph │ │ │ │ │ 20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_._h> │ │ │ │ │ -23#include │ │ │ │ │ -24#include │ │ │ │ │ -25#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_B_e_a_r_i_n_g_R_a_n_g_e_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_3_._h> │ │ │ │ │ -27#include │ │ │ │ │ +21#pragma once │ │ │ │ │ +22 │ │ │ │ │ +23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_B_e_a_r_i_n_g_R_a_n_g_e_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_2_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_b_a_s_e_/_L_i_e_._h> │ │ │ │ │ +27#include │ │ │ │ │ 28 │ │ │ │ │ -29#include │ │ │ │ │ +29namespace _g_t_s_a_m { │ │ │ │ │ 30 │ │ │ │ │ -31namespace _g_t_s_a_m { │ │ │ │ │ -32 │ │ │ │ │ -_4_0class GTSAM_EXPORT _E_m_p_t_y_C_a_l { │ │ │ │ │ -41 public: │ │ │ │ │ -42 enum { dimension = 0 }; │ │ │ │ │ -43 _E_m_p_t_y_C_a_l() {} │ │ │ │ │ -44 virtual _~_E_m_p_t_y_C_a_l() = default; │ │ │ │ │ -45 using shared_ptr = boost::shared_ptr; │ │ │ │ │ -46 │ │ │ │ │ -_4_8 inline static size_t _D_i_m() { return dimension; } │ │ │ │ │ -49 │ │ │ │ │ -50 void _p_r_i_n_t(const std::string& s) const { │ │ │ │ │ -51 std::cout << "empty calibration: " << s << std::endl; │ │ │ │ │ -52 } │ │ │ │ │ +_3_6class _P_o_s_e_2: public _L_i_e_G_r_o_u_p { │ │ │ │ │ +37 │ │ │ │ │ +38public: │ │ │ │ │ +39 │ │ │ │ │ +_4_1 typedef _R_o_t_2 _R_o_t_a_t_i_o_n; │ │ │ │ │ +42 typedef _P_o_i_n_t_2 Translation; │ │ │ │ │ +43 │ │ │ │ │ +44private: │ │ │ │ │ +45 │ │ │ │ │ +46 _R_o_t_2 r_; │ │ │ │ │ +47 _P_o_i_n_t_2 t_; │ │ │ │ │ +48 │ │ │ │ │ +49public: │ │ │ │ │ +50 │ │ │ │ │ 53 │ │ │ │ │ -54 private: │ │ │ │ │ -_5_6 friend class boost::serialization::access; │ │ │ │ │ -57 template │ │ │ │ │ -58 void serialize(Archive& ar, const unsigned int /*version*/) { │ │ │ │ │ -59 ar& boost::serialization::make_nvp( │ │ │ │ │ -60 "EmptyCal", boost::serialization::base_object(*this)); │ │ │ │ │ -61 } │ │ │ │ │ -62}; │ │ │ │ │ -63 │ │ │ │ │ -_7_0class GTSAM_EXPORT _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a { │ │ │ │ │ -71 public: │ │ │ │ │ -72 enum { dimension = 6 }; │ │ │ │ │ -73 │ │ │ │ │ -74 using _M_e_a_s_u_r_e_m_e_n_t = _U_n_i_t_3; │ │ │ │ │ -75 using MeasurementVector = std::vector; │ │ │ │ │ -76 using _C_a_l_i_b_r_a_t_i_o_n_T_y_p_e = _E_m_p_t_y_C_a_l; │ │ │ │ │ -77 │ │ │ │ │ -78 private: │ │ │ │ │ -79 _P_o_s_e_3 pose_; │ │ │ │ │ -80 │ │ │ │ │ -81 protected: │ │ │ │ │ -82 EmptyCal::shared_ptr emptyCal_; │ │ │ │ │ -83 │ │ │ │ │ -84 public: │ │ │ │ │ -87 │ │ │ │ │ -_8_9 _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a() │ │ │ │ │ -90 : pose_(_P_o_s_e_3()), emptyCal_(boost::_m_a_k_e___s_h_a_r_e_d<_E_m_p_t_y_C_a_l>()) {} │ │ │ │ │ -91 │ │ │ │ │ -_9_3 explicit _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a(const _P_o_s_e_3& pose) │ │ │ │ │ -94 : pose_(pose), emptyCal_(boost::_m_a_k_e___s_h_a_r_e_d<_E_m_p_t_y_C_a_l>()) {} │ │ │ │ │ -95 │ │ │ │ │ -_9_7 explicit _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a(const _P_o_s_e_3& pose, │ │ │ │ │ -98 const EmptyCal::shared_ptr& cal) │ │ │ │ │ -99 : pose_(pose), emptyCal_(cal) {} │ │ │ │ │ -100 │ │ │ │ │ -104 explicit _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a(const Vector& v) : pose_(_P_o_s_e_3::Expmap(v)) {} │ │ │ │ │ -105 │ │ │ │ │ -_1_0_7 virtual _~_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a() = default; │ │ │ │ │ -108 │ │ │ │ │ -_1_1_0 const EmptyCal::shared_ptr& _s_h_a_r_e_d_C_a_l_i_b_r_a_t_i_o_n() const { │ │ │ │ │ -111 return emptyCal_; │ │ │ │ │ -112 } │ │ │ │ │ +_5_5 _P_o_s_e_2() : │ │ │ │ │ +56 r_(_t_r_a_i_t_s<_R_o_t_2>::_I_d_e_n_t_i_t_y()), t_(_t_r_a_i_t_s<_P_o_i_n_t_2>::_I_d_e_n_t_i_t_y()) { │ │ │ │ │ +57 } │ │ │ │ │ +58 │ │ │ │ │ +_6_0 _P_o_s_e_2(const _P_o_s_e_2& pose) : r_(pose.r_), t_(pose.t_) {} │ │ │ │ │ +61 │ │ │ │ │ +_6_8 _P_o_s_e_2(double _x, double _y, double _t_h_e_t_a) : │ │ │ │ │ +69 r_(_R_o_t_2::fromAngle(_t_h_e_t_a)), t_(_x, _y) { │ │ │ │ │ +70 } │ │ │ │ │ +71 │ │ │ │ │ +_7_3 _P_o_s_e_2(double _t_h_e_t_a, const _P_o_i_n_t_2& _t) : │ │ │ │ │ +74 r_(_R_o_t_2::fromAngle(_t_h_e_t_a)), t_(_t) { │ │ │ │ │ +75 } │ │ │ │ │ +76 │ │ │ │ │ +_7_8 _P_o_s_e_2(const _R_o_t_2& _r, const _P_o_i_n_t_2& _t) : r_(_r), t_(_t) {} │ │ │ │ │ +79 │ │ │ │ │ +_8_1 _P_o_s_e_2(const Matrix &T) : │ │ │ │ │ +82 r_(_R_o_t_2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) { │ │ │ │ │ +83 assert(T.rows() == 3 && T.cols() == 3); │ │ │ │ │ +84 } │ │ │ │ │ +85 │ │ │ │ │ +89 │ │ │ │ │ +_9_1 _P_o_s_e_2(const Vector& v) : _P_o_s_e_2() { │ │ │ │ │ +92 *this = _E_x_p_m_a_p(v); │ │ │ │ │ +93 } │ │ │ │ │ +94 │ │ │ │ │ +102 static boost::optional _A_l_i_g_n(const Point2Pairs& abPointPairs); │ │ │ │ │ +103 │ │ │ │ │ +104 // Version of Pose2::Align that takes 2 matrices. │ │ │ │ │ +105 static boost::optional _A_l_i_g_n(const Matrix& a, const Matrix& b); │ │ │ │ │ +106 │ │ │ │ │ +110 │ │ │ │ │ +112 GTSAM_EXPORT void _p_r_i_n_t(const std::string& s = "") const; │ │ │ │ │ 113 │ │ │ │ │ -_1_1_5 const _E_m_p_t_y_C_a_l& _c_a_l_i_b_r_a_t_i_o_n() const { return *emptyCal_; } │ │ │ │ │ +115 GTSAM_EXPORT bool _e_q_u_a_l_s(const _P_o_s_e_2& pose, double tol = 1e-9) const; │ │ │ │ │ 116 │ │ │ │ │ 120 │ │ │ │ │ -122 bool _e_q_u_a_l_s(const _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a& camera, double tol = 1e-9) const; │ │ │ │ │ +_1_2_2 inline static _P_o_s_e_2 _I_d_e_n_t_i_t_y() { return _P_o_s_e_2(); } │ │ │ │ │ 123 │ │ │ │ │ -125 virtual void _p_r_i_n_t(const std::string& s = "SphericalCamera") const; │ │ │ │ │ +125 GTSAM_EXPORT _P_o_s_e_2 _i_n_v_e_r_s_e() const; │ │ │ │ │ 126 │ │ │ │ │ -130 │ │ │ │ │ -_1_3_2 const _P_o_s_e_3& _p_o_s_e() const { return pose_; } │ │ │ │ │ -133 │ │ │ │ │ -_1_3_5 const _R_o_t_3& _r_o_t_a_t_i_o_n() const { return pose_._r_o_t_a_t_i_o_n(); } │ │ │ │ │ -136 │ │ │ │ │ -_1_3_8 const _P_o_i_n_t_3& _t_r_a_n_s_l_a_t_i_o_n() const { return pose_._t_r_a_n_s_l_a_t_i_o_n(); } │ │ │ │ │ -139 │ │ │ │ │ -140 // /// return pose, with derivative │ │ │ │ │ -141 // const Pose3& getPose(OptionalJacobian<6, 6> H) const; │ │ │ │ │ -142 │ │ │ │ │ -146 │ │ │ │ │ -148 std::pair projectSafe(const _P_o_i_n_t_3& pw) const; │ │ │ │ │ -149 │ │ │ │ │ -155 _U_n_i_t_3 project2(const _P_o_i_n_t_3& pw, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> Dpose = boost:: │ │ │ │ │ -none, │ │ │ │ │ -156 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> Dpoint = boost::none) const; │ │ │ │ │ +_1_2_8 inline _P_o_s_e_2 _o_p_e_r_a_t_o_r_*(const _P_o_s_e_2& p2) const { │ │ │ │ │ +129 return _P_o_s_e_2(r_*p2._r(), t_ + r_*p2._t()); │ │ │ │ │ +130 } │ │ │ │ │ +131 │ │ │ │ │ +135 │ │ │ │ │ +137 GTSAM_EXPORT static _P_o_s_e_2 _E_x_p_m_a_p(const Vector3& xi, ChartJacobian H = │ │ │ │ │ +boost::none); │ │ │ │ │ +138 │ │ │ │ │ +140 GTSAM_EXPORT static Vector3 _L_o_g_m_a_p(const _P_o_s_e_2& p, ChartJacobian H = │ │ │ │ │ +boost::none); │ │ │ │ │ +141 │ │ │ │ │ +146 GTSAM_EXPORT Matrix3 _A_d_j_o_i_n_t_M_a_p() const; │ │ │ │ │ +147 │ │ │ │ │ +_1_4_9 inline Vector3 _A_d_j_o_i_n_t(const Vector3& xi) const { │ │ │ │ │ +150 return _A_d_j_o_i_n_t_M_a_p()*xi; │ │ │ │ │ +151 } │ │ │ │ │ +152 │ │ │ │ │ +156 GTSAM_EXPORT static Matrix3 _a_d_j_o_i_n_t_M_a_p(const Vector3& v); │ │ │ │ │ 157 │ │ │ │ │ -163 _U_n_i_t_3 project2(const _U_n_i_t_3& pwu, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> Dpose = boost:: │ │ │ │ │ -none, │ │ │ │ │ -164 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dpoint = boost::none) const; │ │ │ │ │ -165 │ │ │ │ │ -167 _P_o_i_n_t_3 backproject(const _U_n_i_t_3& p, const double depth) const; │ │ │ │ │ -168 │ │ │ │ │ -170 _U_n_i_t_3 backprojectPointAtInfinity(const _U_n_i_t_3& p) const; │ │ │ │ │ +_1_6_1 static Vector3 _a_d_j_o_i_n_t(const Vector3& xi, const Vector3& _y) { │ │ │ │ │ +162 return _a_d_j_o_i_n_t_M_a_p(xi) * _y; │ │ │ │ │ +163 } │ │ │ │ │ +164 │ │ │ │ │ +_1_6_8 static Vector3 _a_d_j_o_i_n_t_T_r_a_n_s_p_o_s_e(const Vector3& xi, const Vector3& _y) { │ │ │ │ │ +169 return _a_d_j_o_i_n_t_M_a_p(xi).transpose() * _y; │ │ │ │ │ +170 } │ │ │ │ │ 171 │ │ │ │ │ -177 _U_n_i_t_3 _p_r_o_j_e_c_t(const _P_o_i_n_t_3& point, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> Dpose = boost:: │ │ │ │ │ -none, │ │ │ │ │ -178 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> Dpoint = boost::none) const; │ │ │ │ │ -179 │ │ │ │ │ -184 Vector2 reprojectionError(const _P_o_i_n_t_3& point, const _U_n_i_t_3& measured, │ │ │ │ │ -185 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> Dpose = boost::none, │ │ │ │ │ -186 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> Dpoint = boost::none) const; │ │ │ │ │ -188 │ │ │ │ │ -_1_9_0 _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a _r_e_t_r_a_c_t(const Vector6& d) const { │ │ │ │ │ -191 return _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a(pose().retract(d)); │ │ │ │ │ -192 } │ │ │ │ │ +172 // temporary fix for wrappers until case issue is resolved │ │ │ │ │ +173 static Matrix3 adjointMap_(const Vector3 &xi) { return _a_d_j_o_i_n_t_M_a_p(xi);} │ │ │ │ │ +174 static Vector3 adjoint_(const Vector3 &xi, const Vector3 &_y) { return │ │ │ │ │ +_a_d_j_o_i_n_t(xi, _y);} │ │ │ │ │ +175 │ │ │ │ │ +_1_8_3 static inline Matrix3 _w_e_d_g_e(double vx, double vy, double w) { │ │ │ │ │ +184 Matrix3 m; │ │ │ │ │ +185 m << 0.,-w, vx, │ │ │ │ │ +186 w, 0., vy, │ │ │ │ │ +187 0., 0., 0.; │ │ │ │ │ +188 return m; │ │ │ │ │ +189 } │ │ │ │ │ +190 │ │ │ │ │ +192 GTSAM_EXPORT static Matrix3 _E_x_p_m_a_p_D_e_r_i_v_a_t_i_v_e(const Vector3& v); │ │ │ │ │ 193 │ │ │ │ │ -_1_9_5 Vector6 _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(const _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a& p) const { │ │ │ │ │ -196 return pose().localCoordinates(p._p_o_s_e()); │ │ │ │ │ -197 } │ │ │ │ │ -198 │ │ │ │ │ -_2_0_0 static _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a _I_d_e_n_t_i_t_y() { │ │ │ │ │ -201 return _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a( │ │ │ │ │ -202 Pose3::Identity()); // assumes that the default constructor is valid │ │ │ │ │ -203 } │ │ │ │ │ +195 GTSAM_EXPORT static Matrix3 _L_o_g_m_a_p_D_e_r_i_v_a_t_i_v_e(const _P_o_s_e_2& v); │ │ │ │ │ +196 │ │ │ │ │ +197 // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP │ │ │ │ │ +_1_9_8 struct _C_h_a_r_t_A_t_O_r_i_g_i_n { │ │ │ │ │ +199 GTSAM_EXPORT static _P_o_s_e_2 Retract(const Vector3& v, ChartJacobian H = │ │ │ │ │ +boost::none); │ │ │ │ │ +200 GTSAM_EXPORT static Vector3 Local(const _P_o_s_e_2& _r, ChartJacobian H = boost:: │ │ │ │ │ +none); │ │ │ │ │ +201 }; │ │ │ │ │ +202 │ │ │ │ │ +203 using _L_i_e_G_r_o_u_p<_P_o_s_e_2, 3>_:_:_i_n_v_e_r_s_e; // version with derivative │ │ │ │ │ 204 │ │ │ │ │ -_2_0_6 Matrix34 _c_a_m_e_r_a_P_r_o_j_e_c_t_i_o_n_M_a_t_r_i_x() const { │ │ │ │ │ -207 return Matrix34(pose_._i_n_v_e_r_s_e()._m_a_t_r_i_x().block(0, 0, 3, 4)); │ │ │ │ │ -208 } │ │ │ │ │ -209 │ │ │ │ │ -_2_1_1 Vector _d_e_f_a_u_l_t_E_r_r_o_r_W_h_e_n_T_r_i_a_n_g_u_l_a_t_i_n_g_B_e_h_i_n_d_C_a_m_e_r_a() const { │ │ │ │ │ -212 return Eigen::Matrix::dimension, 1>::Constant(0.0); │ │ │ │ │ -213 } │ │ │ │ │ -214 │ │ │ │ │ -_2_1_6 size_t _d_i_m() const { return 6; } │ │ │ │ │ -217 │ │ │ │ │ -_2_1_9 static size_t _D_i_m() { return 6; } │ │ │ │ │ +208 │ │ │ │ │ +210 GTSAM_EXPORT _P_o_i_n_t_2 _t_r_a_n_s_f_o_r_m_T_o(const _P_o_i_n_t_2& point, │ │ │ │ │ +211 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> Dpose = boost::none, │ │ │ │ │ +212 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dpoint = boost::none) const; │ │ │ │ │ +213 │ │ │ │ │ +219 Matrix _t_r_a_n_s_f_o_r_m_T_o(const Matrix& points) const; │ │ │ │ │ 220 │ │ │ │ │ -221 private: │ │ │ │ │ -_2_2_3 friend class boost::serialization::access; │ │ │ │ │ -224 template │ │ │ │ │ -225 void serialize(Archive& ar, const unsigned int /*version*/) { │ │ │ │ │ -226 ar& BOOST_SERIALIZATION_NVP(pose_); │ │ │ │ │ -227 } │ │ │ │ │ -228 │ │ │ │ │ -229 public: │ │ │ │ │ -230 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -231}; │ │ │ │ │ -232// end of class SphericalCamera │ │ │ │ │ -233 │ │ │ │ │ -234template <> │ │ │ │ │ -_2_3_5struct _t_r_a_i_t_s<_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a> : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ -236 │ │ │ │ │ -237template <> │ │ │ │ │ -_2_3_8struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ -239 │ │ │ │ │ -240} // namespace gtsam │ │ │ │ │ -_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_._h │ │ │ │ │ -Base exception type that uses tbb_allocator if GTSAM is compiled with TBB. │ │ │ │ │ -_M_a_n_i_f_o_l_d_._h │ │ │ │ │ -Base class and basic functions for Manifold types. │ │ │ │ │ +222 GTSAM_EXPORT _P_o_i_n_t_2 _t_r_a_n_s_f_o_r_m_F_r_o_m(const _P_o_i_n_t_2& point, │ │ │ │ │ +223 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> Dpose = boost::none, │ │ │ │ │ +224 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dpoint = boost::none) const; │ │ │ │ │ +225 │ │ │ │ │ +231 Matrix _t_r_a_n_s_f_o_r_m_F_r_o_m(const Matrix& points) const; │ │ │ │ │ +232 │ │ │ │ │ +_2_3_4 inline _P_o_i_n_t_2 _o_p_e_r_a_t_o_r_*(const _P_o_i_n_t_2& point) const { │ │ │ │ │ +235 return _t_r_a_n_s_f_o_r_m_F_r_o_m(point); │ │ │ │ │ +236 } │ │ │ │ │ +237 │ │ │ │ │ +241 │ │ │ │ │ +_2_4_3 inline double _x() const { return t_.x(); } │ │ │ │ │ +244 │ │ │ │ │ +_2_4_6 inline double _y() const { return t_.y(); } │ │ │ │ │ +247 │ │ │ │ │ +_2_4_9 inline double _t_h_e_t_a() const { return r_._t_h_e_t_a(); } │ │ │ │ │ +250 │ │ │ │ │ +_2_5_2 inline const _P_o_i_n_t_2& _t() const { return t_; } │ │ │ │ │ +253 │ │ │ │ │ +_2_5_5 inline const _R_o_t_2& _r() const { return r_; } │ │ │ │ │ +256 │ │ │ │ │ +_2_5_8 inline const _P_o_i_n_t_2& _t_r_a_n_s_l_a_t_i_o_n() const { return t_; } │ │ │ │ │ +259 │ │ │ │ │ +_2_6_1 inline const _R_o_t_2& _r_o_t_a_t_i_o_n() const { return r_; } │ │ │ │ │ +262 │ │ │ │ │ +264 GTSAM_EXPORT Matrix3 matrix() const; │ │ │ │ │ +265 │ │ │ │ │ +271 GTSAM_EXPORT _R_o_t_2 _b_e_a_r_i_n_g(const _P_o_i_n_t_2& point, │ │ │ │ │ +272 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> H1=boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _2_> H2=boost:: │ │ │ │ │ +none) const; │ │ │ │ │ +273 │ │ │ │ │ +279 GTSAM_EXPORT _R_o_t_2 _b_e_a_r_i_n_g(const _P_o_s_e_2& pose, │ │ │ │ │ +280 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> H1=boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> H2=boost:: │ │ │ │ │ +none) const; │ │ │ │ │ +281 │ │ │ │ │ +287 GTSAM_EXPORT double range(const _P_o_i_n_t_2& point, │ │ │ │ │ +288 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> H1=boost::none, │ │ │ │ │ +289 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _2_> H2=boost::none) const; │ │ │ │ │ +290 │ │ │ │ │ +296 GTSAM_EXPORT double range(const _P_o_s_e_2& point, │ │ │ │ │ +297 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> H1=boost::none, │ │ │ │ │ +298 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> H2=boost::none) const; │ │ │ │ │ +299 │ │ │ │ │ +303 │ │ │ │ │ +_3_0_9 inline static std::pair _t_r_a_n_s_l_a_t_i_o_n_I_n_t_e_r_v_a_l() { return │ │ │ │ │ +std::make_pair(0, 1); } │ │ │ │ │ +310 │ │ │ │ │ +_3_1_6 static std::pair _r_o_t_a_t_i_o_n_I_n_t_e_r_v_a_l() { return std::make_pair │ │ │ │ │ +(2, 2); } │ │ │ │ │ +317 │ │ │ │ │ +319 GTSAM_EXPORT │ │ │ │ │ +320 friend std::ostream &_o_p_e_r_a_t_o_r_<_<(std::ostream &os, const _P_o_s_e_2& p); │ │ │ │ │ +321 │ │ │ │ │ +323 │ │ │ │ │ +324 private: │ │ │ │ │ +325 │ │ │ │ │ +326 // Serialization function │ │ │ │ │ +327 friend class boost::serialization::access; │ │ │ │ │ +328 template │ │ │ │ │ +329 void serialize(Archive & ar, const unsigned int /*version*/) { │ │ │ │ │ +330 ar & BOOST_SERIALIZATION_NVP(t_); │ │ │ │ │ +331 ar & BOOST_SERIALIZATION_NVP(r_); │ │ │ │ │ +332 } │ │ │ │ │ +333 │ │ │ │ │ +334public: │ │ │ │ │ +335 // Align for Point2, which is either derived from, or is typedef, of │ │ │ │ │ +Vector2 │ │ │ │ │ +336 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +337}; // Pose2 │ │ │ │ │ +338 │ │ │ │ │ +340template <> │ │ │ │ │ +_3_4_1inline Matrix _w_e_d_g_e_<_P_o_s_e_2_>(const Vector& xi) { │ │ │ │ │ +342 // NOTE(chris): Need eval() as workaround for Apple clang + avx2. │ │ │ │ │ +343 return Matrix(_P_o_s_e_2_:_:_w_e_d_g_e(xi(0),xi(1),xi(2))).eval(); │ │ │ │ │ +344} │ │ │ │ │ +345 │ │ │ │ │ +346#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ +352GTSAM_EXPORT boost::optional │ │ │ │ │ +353GTSAM_DEPRECATED align(const Point2Pairs& pairs); │ │ │ │ │ +354#endif │ │ │ │ │ +355 │ │ │ │ │ +356// Convenience typedef │ │ │ │ │ +357using Pose2Pair = std::pair; │ │ │ │ │ +358using Pose2Pairs = std::vector; │ │ │ │ │ +359 │ │ │ │ │ +360template <> │ │ │ │ │ +_3_6_1struct _t_r_a_i_t_s<_P_o_s_e_2> : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ +362 │ │ │ │ │ +363template <> │ │ │ │ │ +_3_6_4struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ +365 │ │ │ │ │ +366// bearing and range traits, used in RangeFactor │ │ │ │ │ +367template │ │ │ │ │ +_3_6_8struct _B_e_a_r_i_n_g<_P_o_s_e_2, T> : _H_a_s_B_e_a_r_i_n_g {}; │ │ │ │ │ +369 │ │ │ │ │ +370template │ │ │ │ │ +_3_7_1struct _R_a_n_g_e<_P_o_s_e_2, T> : _H_a_s_R_a_n_g_e {}; │ │ │ │ │ +372 │ │ │ │ │ +373} // namespace gtsam │ │ │ │ │ +374 │ │ │ │ │ _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ This marks a GTSAM object to require alignment. │ │ │ │ │ DDeeffiinniittiioonn types.h:308 │ │ │ │ │ -_P_o_s_e_3_._h │ │ │ │ │ -3D Pose │ │ │ │ │ +_L_i_e_._h │ │ │ │ │ +Base class and basic functions for Lie types. │ │ │ │ │ +_P_o_i_n_t_2_._h │ │ │ │ │ +2D Point │ │ │ │ │ +_R_o_t_2_._h │ │ │ │ │ +2D rotation │ │ │ │ │ _B_e_a_r_i_n_g_R_a_n_g_e_._h │ │ │ │ │ Bearing-Range product. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ -Vector3 Point3 │ │ │ │ │ +_g_t_s_a_m_:_:_w_e_d_g_e_<_ _P_o_s_e_2_ _> │ │ │ │ │ +Matrix wedge< Pose2 >(const Vector &xi) │ │ │ │ │ +specialization for pose2 wedge function (generic template in Lie.h) │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:341 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ +Vector2 Point2 │ │ │ │ │ As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point3 to Vector3... │ │ │ │ │ -DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_m_a_k_e___s_h_a_r_e_d │ │ │ │ │ -gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost:: │ │ │ │ │ -shared_ptr< T > > make_shared(Args &&... args) │ │ │ │ │ -Add our own make_shared as a layer of wrapping on boost::make_shared This │ │ │ │ │ -solves the problem with the... │ │ │ │ │ -DDeeffiinniittiioonn make_shared.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_o_j_e_c_t │ │ │ │ │ -Point2_ project(const Point3_ &p_cam) │ │ │ │ │ -Expression version of PinholeBase::Project. │ │ │ │ │ -DDeeffiinniittiioonn expressions.h:131 │ │ │ │ │ +typedef Point2 to Vector2... │ │ │ │ │ +DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ +A CRTP helper class that implements Lie group methods Prerequisites: methods │ │ │ │ │ +operator*,... │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:37 │ │ │ │ │ _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ Both LieGroupTraits and Testable. │ │ │ │ │ DDeeffiinniittiioonn Lie.h:229 │ │ │ │ │ _g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ either a fixed size o... │ │ │ │ │ DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ _g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ Template to create a binary predicate. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ -A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_t_r_a_n_s_l_a_t_i_o_n │ │ │ │ │ -const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const │ │ │ │ │ -get translation │ │ │ │ │ -DDeeffiinniittiioonn Pose3.cpp:308 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_m_a_t_r_i_x │ │ │ │ │ -Matrix4 matrix() const │ │ │ │ │ -convert to 4*4 matrix │ │ │ │ │ -DDeeffiinniittiioonn Pose3.cpp:323 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_i_n_v_e_r_s_e │ │ │ │ │ -Pose3 inverse() const │ │ │ │ │ -inverse transformation with derivatives │ │ │ │ │ -DDeeffiinniittiioonn Pose3.cpp:49 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_r_o_t_a_t_i_o_n │ │ │ │ │ -const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const │ │ │ │ │ -get rotation │ │ │ │ │ -DDeeffiinniittiioonn Pose3.cpp:315 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ -Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor │ │ │ │ │ -symbol GTSAM_USE_QUATERNIO... │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_E_m_p_t_y_C_a_l │ │ │ │ │ -Empty calibration. │ │ │ │ │ -DDeeffiinniittiioonn SphericalCamera.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_E_m_p_t_y_C_a_l_:_:_D_i_m │ │ │ │ │ -static size_t Dim() │ │ │ │ │ -return DOF, dimensionality of tangent space │ │ │ │ │ -DDeeffiinniittiioonn SphericalCamera.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a │ │ │ │ │ -A spherical camera class that has a Pose3 and measures bearing vectors. │ │ │ │ │ -DDeeffiinniittiioonn SphericalCamera.h:70 │ │ │ │ │ -_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_s_h_a_r_e_d_C_a_l_i_b_r_a_t_i_o_n │ │ │ │ │ -const EmptyCal::shared_ptr & sharedCalibration() const │ │ │ │ │ -return shared pointer to calibration │ │ │ │ │ -DDeeffiinniittiioonn SphericalCamera.h:110 │ │ │ │ │ -_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_D_i_m │ │ │ │ │ -static size_t Dim() │ │ │ │ │ -DDeeffiinniittiioonn SphericalCamera.h:219 │ │ │ │ │ -_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a │ │ │ │ │ -SphericalCamera(const Pose3 &pose) │ │ │ │ │ -Constructor with pose. │ │ │ │ │ -DDeeffiinniittiioonn SphericalCamera.h:93 │ │ │ │ │ -_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_~_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a │ │ │ │ │ -virtual ~SphericalCamera()=default │ │ │ │ │ -Default destructor. │ │ │ │ │ -_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_c_a_m_e_r_a_P_r_o_j_e_c_t_i_o_n_M_a_t_r_i_x │ │ │ │ │ -Matrix34 cameraProjectionMatrix() const │ │ │ │ │ -for Linear Triangulation │ │ │ │ │ -DDeeffiinniittiioonn SphericalCamera.h:206 │ │ │ │ │ -_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ -Vector6 localCoordinates(const SphericalCamera &p) const │ │ │ │ │ -return canonical coordinate │ │ │ │ │ -DDeeffiinniittiioonn SphericalCamera.h:195 │ │ │ │ │ -_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_r_o_t_a_t_i_o_n │ │ │ │ │ -const Rot3 & rotation() const │ │ │ │ │ -get rotation │ │ │ │ │ -DDeeffiinniittiioonn SphericalCamera.h:135 │ │ │ │ │ -_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_p_o_s_e │ │ │ │ │ -const Pose3 & pose() const │ │ │ │ │ -return pose, constant version │ │ │ │ │ -DDeeffiinniittiioonn SphericalCamera.h:132 │ │ │ │ │ -_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_t_r_a_n_s_l_a_t_i_o_n │ │ │ │ │ -const Point3 & translation() const │ │ │ │ │ -get translation │ │ │ │ │ -DDeeffiinniittiioonn SphericalCamera.h:138 │ │ │ │ │ -_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_r_e_t_r_a_c_t │ │ │ │ │ -SphericalCamera retract(const Vector6 &d) const │ │ │ │ │ -move a cameras according to d │ │ │ │ │ -DDeeffiinniittiioonn SphericalCamera.h:190 │ │ │ │ │ -_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_c_a_l_i_b_r_a_t_i_o_n │ │ │ │ │ -const EmptyCal & calibration() const │ │ │ │ │ -return calibration │ │ │ │ │ -DDeeffiinniittiioonn SphericalCamera.h:115 │ │ │ │ │ -_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_d_i_m │ │ │ │ │ -size_t dim() const │ │ │ │ │ -DDeeffiinniittiioonn SphericalCamera.h:216 │ │ │ │ │ -_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_I_d_e_n_t_i_t_y │ │ │ │ │ -static SphericalCamera Identity() │ │ │ │ │ -for Canonical │ │ │ │ │ -DDeeffiinniittiioonn SphericalCamera.h:200 │ │ │ │ │ -_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a │ │ │ │ │ -SphericalCamera() │ │ │ │ │ -Default constructor. │ │ │ │ │ -DDeeffiinniittiioonn SphericalCamera.h:89 │ │ │ │ │ -_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_d_e_f_a_u_l_t_E_r_r_o_r_W_h_e_n_T_r_i_a_n_g_u_l_a_t_i_n_g_B_e_h_i_n_d_C_a_m_e_r_a │ │ │ │ │ -Vector defaultErrorWhenTriangulatingBehindCamera() const │ │ │ │ │ -for Nonlinear Triangulation │ │ │ │ │ -DDeeffiinniittiioonn SphericalCamera.h:211 │ │ │ │ │ -_g_t_s_a_m_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_:_:_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a │ │ │ │ │ -SphericalCamera(const Pose3 &pose, const EmptyCal::shared_ptr &cal) │ │ │ │ │ -Constructor with empty intrinsics (needed for smart factors) │ │ │ │ │ -DDeeffiinniittiioonn SphericalCamera.h:97 │ │ │ │ │ -_g_t_s_a_m_:_:_U_n_i_t_3 │ │ │ │ │ -Represents a 3D point on a unit sphere. │ │ │ │ │ -DDeeffiinniittiioonn Unit3.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_B_e_a_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn BearingRange.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_R_a_n_g_e │ │ │ │ │ +DDeeffiinniittiioonn BearingRange.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_H_a_s_B_e_a_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn BearingRange.h:180 │ │ │ │ │ +_g_t_s_a_m_:_:_H_a_s_R_a_n_g_e │ │ │ │ │ +DDeeffiinniittiioonn BearingRange.h:194 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2 │ │ │ │ │ +A 2D pose (Point2,Rot2) │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_b_e_a_r_i_n_g │ │ │ │ │ +GTSAM_EXPORT Rot2 bearing(const Point2 &point, OptionalJacobian< 1, 3 > │ │ │ │ │ +H1=boost::none, OptionalJacobian< 1, 2 > H2=boost::none) const │ │ │ │ │ +Calculate bearing to a landmark. │ │ │ │ │ +DDeeffiinniittiioonn Pose2.cpp:245 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ +Pose2 operator*(const Pose2 &p2) const │ │ │ │ │ +compose syntactic sugar │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:128 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_p_r_i_n_t │ │ │ │ │ +GTSAM_EXPORT void print(const std::string &s="") const │ │ │ │ │ +print with optional string │ │ │ │ │ +DDeeffiinniittiioonn Pose2.cpp:50 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_r_o_t_a_t_i_o_n │ │ │ │ │ +const Rot2 & rotation() const │ │ │ │ │ +rotation │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:261 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_A_d_j_o_i_n_t_M_a_p │ │ │ │ │ +GTSAM_EXPORT Matrix3 AdjointMap() const │ │ │ │ │ +Calculate Adjoint map Ad_pose is 3*3 matrix that when applied to twist xi , │ │ │ │ │ +returns Ad_pose(xi) │ │ │ │ │ +DDeeffiinniittiioonn Pose2.cpp:126 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_L_o_g_m_a_p_D_e_r_i_v_a_t_i_v_e │ │ │ │ │ +static GTSAM_EXPORT Matrix3 LogmapDerivative(const Pose2 &v) │ │ │ │ │ +Derivative of Logmap. │ │ │ │ │ +DDeeffiinniittiioonn Pose2.cpp:179 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_y │ │ │ │ │ +double y() const │ │ │ │ │ +get y │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:246 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_L_o_g_m_a_p │ │ │ │ │ +static GTSAM_EXPORT Vector3 Logmap(const Pose2 &p, ChartJacobian H=boost::none) │ │ │ │ │ +Log map at identity - return the canonical coordinates of this rotation. │ │ │ │ │ +DDeeffiinniittiioonn Pose2.cpp:82 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_i_n_v_e_r_s_e │ │ │ │ │ +GTSAM_EXPORT Pose2 inverse() const │ │ │ │ │ +inverse │ │ │ │ │ +DDeeffiinniittiioonn Pose2.cpp:201 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_o_p_e_r_a_t_o_r_<_< │ │ │ │ │ +GTSAM_EXPORT friend std::ostream & operator<<(std::ostream &os, const Pose2 &p) │ │ │ │ │ +Output stream operator. │ │ │ │ │ +DDeeffiinniittiioonn Pose2.cpp:55 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ +Point2 operator*(const Point2 &point) const │ │ │ │ │ +syntactic sugar for transformFrom │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:234 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_P_o_s_e_2 │ │ │ │ │ +Pose2(double x, double y, double theta) │ │ │ │ │ +construct from (x,y,theta) │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:68 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_t │ │ │ │ │ +const Point2 & t() const │ │ │ │ │ +translation │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:252 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_t_r_a_n_s_l_a_t_i_o_n_I_n_t_e_r_v_a_l │ │ │ │ │ +static std::pair< size_t, size_t > translationInterval() │ │ │ │ │ +Return the start and end indices (inclusive) of the translation component of │ │ │ │ │ +the exponential map para... │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:309 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_P_o_s_e_2 │ │ │ │ │ +Pose2(double theta, const Point2 &t) │ │ │ │ │ +construct from rotation and translation │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:73 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_x │ │ │ │ │ +double x() const │ │ │ │ │ +get x │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:243 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_r │ │ │ │ │ +const Rot2 & r() const │ │ │ │ │ +rotation │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:255 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_P_o_s_e_2 │ │ │ │ │ +Pose2(const Vector &v) │ │ │ │ │ +Construct from canonical coordinates (Lie algebra) │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:91 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_A_d_j_o_i_n_t │ │ │ │ │ +Vector3 Adjoint(const Vector3 &xi) const │ │ │ │ │ +Apply AdjointMap to twist xi. │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:149 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_w_e_d_g_e │ │ │ │ │ +static Matrix3 wedge(double vx, double vy, double w) │ │ │ │ │ +wedge for SE(2): │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:183 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_r_o_t_a_t_i_o_n_I_n_t_e_r_v_a_l │ │ │ │ │ +static std::pair< size_t, size_t > rotationInterval() │ │ │ │ │ +Return the start and end indices (inclusive) of the rotation component of the │ │ │ │ │ +exponential map paramet... │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:316 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_a_d_j_o_i_n_t_M_a_p │ │ │ │ │ +static GTSAM_EXPORT Matrix3 adjointMap(const Vector3 &v) │ │ │ │ │ +Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19. │ │ │ │ │ +DDeeffiinniittiioonn Pose2.cpp:137 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_t_r_a_n_s_f_o_r_m_T_o │ │ │ │ │ +GTSAM_EXPORT Point2 transformTo(const Point2 &point, OptionalJacobian< 2, 3 > │ │ │ │ │ +Dpose=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const │ │ │ │ │ +Return point coordinates in pose coordinate frame. │ │ │ │ │ +DDeeffiinniittiioonn Pose2.cpp:207 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_t_h_e_t_a │ │ │ │ │ +double theta() const │ │ │ │ │ +get theta │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:249 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_E_x_p_m_a_p_D_e_r_i_v_a_t_i_v_e │ │ │ │ │ +static GTSAM_EXPORT Matrix3 ExpmapDerivative(const Vector3 &v) │ │ │ │ │ +Derivative of Expmap. │ │ │ │ │ +DDeeffiinniittiioonn Pose2.cpp:148 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_R_o_t_a_t_i_o_n │ │ │ │ │ +Rot2 Rotation │ │ │ │ │ +Pose Concept requirements. │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_P_o_s_e_2 │ │ │ │ │ +Pose2() │ │ │ │ │ +default constructor = origin │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_t_r_a_n_s_f_o_r_m_F_r_o_m │ │ │ │ │ +GTSAM_EXPORT Point2 transformFrom(const Point2 &point, OptionalJacobian< 2, 3 > │ │ │ │ │ +Dpose=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const │ │ │ │ │ +Return point coordinates in global frame. │ │ │ │ │ +DDeeffiinniittiioonn Pose2.cpp:226 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_a_d_j_o_i_n_t_T_r_a_n_s_p_o_s_e │ │ │ │ │ +static Vector3 adjointTranspose(const Vector3 &xi, const Vector3 &y) │ │ │ │ │ +The dual version of adjoint action, acting on the dual space of the Lie-algebra │ │ │ │ │ +vector space. │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:168 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_E_x_p_m_a_p │ │ │ │ │ +static GTSAM_EXPORT Pose2 Expmap(const Vector3 &xi, ChartJacobian H=boost:: │ │ │ │ │ +none) │ │ │ │ │ +Exponential map at identity - create a rotation from canonical coordinates . │ │ │ │ │ +DDeeffiinniittiioonn Pose2.cpp:66 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_I_d_e_n_t_i_t_y │ │ │ │ │ +static Pose2 Identity() │ │ │ │ │ +identity for group operation │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:122 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_A_l_i_g_n │ │ │ │ │ +static boost::optional< Pose2 > Align(const Point2Pairs &abPointPairs) │ │ │ │ │ +Create Pose2 by aligning two point pairs A pose aTb is estimated between pairs │ │ │ │ │ +(a_point,... │ │ │ │ │ +DDeeffiinniittiioonn Pose2.cpp:330 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_P_o_s_e_2 │ │ │ │ │ +Pose2(const Rot2 &r, const Point2 &t) │ │ │ │ │ +construct from r,t │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:78 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_a_d_j_o_i_n_t │ │ │ │ │ +static Vector3 adjoint(const Vector3 &xi, const Vector3 &y) │ │ │ │ │ +Action of the adjointMap on a Lie-algebra vector y, with optional derivatives. │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:161 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_P_o_s_e_2 │ │ │ │ │ +Pose2(const Pose2 &pose) │ │ │ │ │ +copy constructor │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:60 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_t_r_a_n_s_l_a_t_i_o_n │ │ │ │ │ +const Point2 & translation() const │ │ │ │ │ +translation │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:258 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_P_o_s_e_2 │ │ │ │ │ +Pose2(const Matrix &T) │ │ │ │ │ +Constructor from 3*3 matrix. │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:81 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:198 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_2 │ │ │ │ │ +Rotation matrix NOTE: the angle theta is in radians unless explicitly stated. │ │ │ │ │ +DDeeffiinniittiioonn Rot2.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_2_:_:_t_h_e_t_a │ │ │ │ │ +double theta() const │ │ │ │ │ +return angle (RADIANS) │ │ │ │ │ +DDeeffiinniittiioonn Rot2.h:187 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_._h │ │ │ │ │ + * _P_o_s_e_2_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00335.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/PinholeCamera.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,51 +94,42 @@ │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
PinholeCamera.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
Cal3.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Base class for all pinhole cameras. │ │ │ │ +

Common code for all calibration models. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::PinholeCamera< Calibration >
 A pinhole camera class that has a Pose3 and a Calibration. More...
 
struct  gtsam::traits< PinholeCamera< Calibration > >
 
struct  gtsam::traits< const PinholeCamera< Calibration > >
 
struct  gtsam::Range< PinholeCamera< Calibration >, T >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +std::ostream & gtsam::operator<< (std::ostream &os, const Cal3 &cal)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Base class for all pinhole cameras.

│ │ │ │ -
Author
Yong-Dian Jian
│ │ │ │ -
Date
Jan 27, 2012
│ │ │ │ +

Common code for all calibration models.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,33 +1,23 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -PinholeCamera.h File Reference │ │ │ │ │ -Base class for all pinhole cameras. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_<_ _C_a_l_i_b_r_a_t_i_o_n_ _> │ │ │ │ │ -  A pinhole camera class that has a _P_o_s_e_3 and a Calibration. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _P_i_n_h_o_l_e_C_a_m_e_r_a_<_ _C_a_l_i_b_r_a_t_i_o_n_ _>_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _P_i_n_h_o_l_e_C_a_m_e_r_a_<_ _C_a_l_i_b_r_a_t_i_o_n_ _>_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_R_a_n_g_e_<_ _P_i_n_h_o_l_e_C_a_m_e_r_a_<_ _C_a_l_i_b_r_a_t_i_o_n_ _>_,_ _T_ _> │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Cal3.cpp File Reference │ │ │ │ │ +Common code for all calibration models. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _C_a_l_3 &cal) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Base class for all pinhole cameras. │ │ │ │ │ +Common code for all calibration models. │ │ │ │ │ Author │ │ │ │ │ - Yong-Dian Jian │ │ │ │ │ - Date │ │ │ │ │ - Jan 27, 2012 │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _P_i_n_h_o_l_e_C_a_m_e_r_a_._h │ │ │ │ │ + * _C_a_l_3_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00338.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SOn.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SOn.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,38 +94,77 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces
│ │ │ │ -
SOn.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ +Functions
│ │ │ │ +
SOn.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Definitions of dynamic specializations of SO(n) │ │ │ │ +

N*N matrix representation of SO(N). │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::SO< N >
 Manifold of special orthogonal rotation matrices SO<N>. More...
 
struct  gtsam::SO< N >::ChartAtOrigin
 
struct  gtsam::traits< SO< N > >
 
struct  gtsam::traits< const SO< N > >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Typedefs

│ │ │ │ +using gtsam::SOn = SO< Eigen::Dynamic >
 
│ │ │ │ +using gtsam::DynamicJacobian = OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +constexpr int gtsam::internal::DimensionSO (int N)
 Calculate dimensionality of SO<N> manifold, or return Dynamic if so.
 
│ │ │ │ +constexpr int gtsam::internal::NSquaredSO (int N)
 
│ │ │ │ +template<class Archive >
void gtsam::serialize (Archive &ar, SOn &Q, const unsigned int file_version)
 Serialization function.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Definitions of dynamic specializations of SO(n)

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ -Varun Agrawal
│ │ │ │ +

N*N matrix representation of SO(N).

│ │ │ │ +

N can be Eigen::Dynamic

Author
Frank Dellaert
│ │ │ │
Date
March 2019
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,52 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SOn.cpp File Reference │ │ │ │ │ -Definitions of dynamic specializations of SO(n) _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +SOn.h File Reference │ │ │ │ │ +N*N matrix representation of SO(N). _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_S_O_<_ _N_ _> │ │ │ │ │ +  Manifold of special orthogonal rotation matrices SO. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_S_O_<_ _N_ _>_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_O_<_ _N_ _>_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _S_O_<_ _N_ _>_ _> │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +using  ggttssaamm::::SSOOnn = _S_O< Eigen::Dynamic > │ │ │ │ │ +  │ │ │ │ │ +using  ggttssaamm::::DDyynnaammiiccJJaaccoobbiiaann = _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< Eigen::Dynamic, Eigen:: │ │ │ │ │ + Dynamic > │ │ │ │ │ +  │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +constexpr int  ggttssaamm::::iinntteerrnnaall::::DDiimmeennssiioonnSSOO (int N) │ │ │ │ │ +  Calculate dimensionality of SO manifold, or return Dynamic if │ │ │ │ │ + so. │ │ │ │ │ +  │ │ │ │ │ +constexpr int  ggttssaamm::::iinntteerrnnaall::::NNSSqquuaarreeddSSOO (int N) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + void  ggttssaamm::::sseerriiaalliizzee (Archive &ar, _S_O_n &Q, const unsigned int │ │ │ │ │ + file_version) │ │ │ │ │ +  Serialization function. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Definitions of dynamic specializations of SO(n) │ │ │ │ │ +N*N matrix representation of SO(N). │ │ │ │ │ +N can be Eigen::Dynamic │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Varun Agrawal │ │ │ │ │ Date │ │ │ │ │ March 2019 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_O_n_._c_p_p │ │ │ │ │ + * _S_O_n_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00341.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Pose3.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Rot3Q.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -93,77 +93,27 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
│ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
│ │ │ │ -
Pose3.h File Reference
│ │ │ │ +
Rot3Q.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

3D Pose │ │ │ │ +

Rotation (internal: quaternion representation*) │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::Pose3
 A 3D pose (R,t) : (Rot3,Point3) More...
 
struct  gtsam::Pose3::ChartAtOrigin
 
struct  gtsam::traits< Pose3 >
 
struct  gtsam::traits< const Pose3 >
 
struct  gtsam::Bearing< Pose3, Point3 >
 
struct  gtsam::Bearing< Pose3, Pose3 >
 
struct  gtsam::Range< Pose3, T >
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Typedefs

│ │ │ │ -using gtsam::Pose3Pair = std::pair< Pose3, Pose3 >
 
│ │ │ │ -using gtsam::Pose3Pairs = std::vector< std::pair< Pose3, Pose3 > >
 
│ │ │ │ -typedef std::vector< Pose3 > gtsam::Pose3Vector
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Functions

template<>
Matrix gtsam::wedge< Pose3 > (const Vector &xi)
 wedge for Pose3:
 
│ │ │ │

Detailed Description

│ │ │ │ -

3D Pose

│ │ │ │ +

Rotation (internal: quaternion representation*)

│ │ │ │ +
Author
Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,48 +1,15 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Pose3.h File Reference │ │ │ │ │ -3D Pose _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ -  A 3D pose (R,t) : (_R_o_t_3,Point3) _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_P_o_s_e_3_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _P_o_s_e_3_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _P_o_s_e_3_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_B_e_a_r_i_n_g_<_ _P_o_s_e_3_,_ _P_o_i_n_t_3_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_B_e_a_r_i_n_g_<_ _P_o_s_e_3_,_ _P_o_s_e_3_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_R_a_n_g_e_<_ _P_o_s_e_3_,_ _T_ _> │ │ │ │ │ -  │ │ │ │ │ -NNaammeessppaacceess │ │ │ │ │ -namespace   _g_t_s_a_m │ │ │ │ │ -  Global functions in a separate testing namespace. │ │ │ │ │ -  │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ - using  ggttssaamm::::PPoossee33PPaaiirr = std::pair< _P_o_s_e_3, _P_o_s_e_3 > │ │ │ │ │ -  │ │ │ │ │ - using  ggttssaamm::::PPoossee33PPaaiirrss = std::vector< std::pair< │ │ │ │ │ - _P_o_s_e_3, _P_o_s_e_3 > > │ │ │ │ │ -  │ │ │ │ │ -typedef std::vector< _P_o_s_e_3 >  ggttssaamm::::PPoossee33VVeeccttoorr │ │ │ │ │ -  │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -template<> │ │ │ │ │ - Matrix  _g_t_s_a_m_:_:_w_e_d_g_e_<_ _P_o_s_e_3_ _> (const Vector &xi) │ │ │ │ │ -  wedge for _P_o_s_e_3: │ │ │ │ │ -  │ │ │ │ │ +Rot3Q.cpp File Reference │ │ │ │ │ +Rotation (internal: quaternion representation*) _M_o_r_e_._._. │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -3D Pose │ │ │ │ │ +Rotation (internal: quaternion representation*) │ │ │ │ │ + Author │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _P_o_s_e_3_._h │ │ │ │ │ + * _R_o_t_3_Q_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00344.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Point3.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Similarity3.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,99 +94,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs | │ │ │ │ Functions
│ │ │ │ -
Point3.h File Reference
│ │ │ │ +
Similarity3.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

3D Point │ │ │ │ +

Implementation of Similarity3 transform. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

struct  gtsam::Range< Point3, Point3 >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Typedefs

│ │ │ │ -typedef Vector3 gtsam::Point3
 As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3.
 
│ │ │ │ -typedef std::vector< Point3, Eigen::aligned_allocator< Point3 > > gtsam::Point3Vector
 
│ │ │ │ -using gtsam::Point3Pair = std::pair< Point3, Point3 >
 
│ │ │ │ -using gtsam::Point3Pairs = std::vector< Point3Pair >
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

│ │ │ │ -ostream & gtsam::operator<< (ostream &os, const gtsam::Point3Pair &p)
 
│ │ │ │ -double gtsam::distance3 (const Point3 &p1, const Point3 &q, OptionalJacobian< 1, 3 > H1=boost::none, OptionalJacobian< 1, 3 > H2=boost::none)
 distance between two points
 
│ │ │ │ -double gtsam::norm3 (const Point3 &p, OptionalJacobian< 1, 3 > H=boost::none)
 Distance of the point from the origin, with Jacobian.
 
│ │ │ │ -Point3 gtsam::normalize (const Point3 &p, OptionalJacobian< 3, 3 > H=boost::none)
 normalize, with optional Jacobian
 
Point3 gtsam::cross (const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H_p=boost::none, OptionalJacobian< 3, 3 > H_q=boost::none)
 cross product
 
│ │ │ │ -double gtsam::dot (const Point3 &p, const Point3 &q, OptionalJacobian< 1, 3 > H_p=boost::none, OptionalJacobian< 1, 3 > H_q=boost::none)
 dot product
 
│ │ │ │ -template<class CONTAINER >
Point3 gtsam::mean (const CONTAINER &points)
 mean
 
│ │ │ │ -Point3Pair gtsam::means (const std::vector< Point3Pair > &abPointPairs)
 Calculate the two means of a set of Point3 pairs.
 
│ │ │ │ +std::ostream & gtsam::operator<< (std::ostream &os, const Similarity3 &p)
 
│ │ │ │

Detailed Description

│ │ │ │ -

3D Point

│ │ │ │ -
Author
Alireza Fathi
│ │ │ │ -
│ │ │ │ -Christian Potthast
│ │ │ │ +

Implementation of Similarity3 transform.

│ │ │ │ +
Author
Paul Drews
│ │ │ │
│ │ │ │ -Frank Dellaert
│ │ │ │ +John Lambert
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,72 +1,24 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Point3.h File Reference │ │ │ │ │ -3D Point _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_R_a_n_g_e_<_ _P_o_i_n_t_3_,_ _P_o_i_n_t_3_ _> │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Similarity3.cpp File Reference │ │ │ │ │ +Implementation of Similarity3 transform. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ - typedef Vector3  ggttssaamm::::PPooiinntt33 │ │ │ │ │ - As of GTSAM 4, in order to make GTSAM more │ │ │ │ │ -  lean, it is now possible to just typedef │ │ │ │ │ - Point3 to Vector3. │ │ │ │ │ -  │ │ │ │ │ -typedef std::vector< _P_o_i_n_t_3, Eigen:: │ │ │ │ │ - aligned_allocator< _P_o_i_n_t_3 > >  ggttssaamm::::PPooiinntt33VVeeccttoorr │ │ │ │ │ -  │ │ │ │ │ - using  ggttssaamm::::PPooiinntt33PPaaiirr = std::pair< _P_o_i_n_t_3, │ │ │ │ │ - _P_o_i_n_t_3 > │ │ │ │ │ -  │ │ │ │ │ - using  ggttssaamm::::PPooiinntt33PPaaiirrss = std::vector< │ │ │ │ │ - Point3Pair > │ │ │ │ │ -  │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ - ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const gtsam::Point3Pair &p) │ │ │ │ │ -  │ │ │ │ │ - double  ggttssaamm::::ddiissttaannccee33 (const _P_o_i_n_t_3 &p1, const _P_o_i_n_t_3 &q, │ │ │ │ │ - _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 1, 3 > H1=boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 1, 3 > │ │ │ │ │ - H2=boost::none) │ │ │ │ │ -  distance between two points │ │ │ │ │ -  │ │ │ │ │ - double  ggttssaamm::::nnoorrmm33 (const _P_o_i_n_t_3 &p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 1, 3 > H=boost:: │ │ │ │ │ - none) │ │ │ │ │ -  Distance of the point from the origin, with Jacobian. │ │ │ │ │ -  │ │ │ │ │ - _P_o_i_n_t_3  ggttssaamm::::nnoorrmmaalliizzee (const _P_o_i_n_t_3 &p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 3, 3 > │ │ │ │ │ - H=boost::none) │ │ │ │ │ -  normalize, with optional Jacobian │ │ │ │ │ -  │ │ │ │ │ - _P_o_i_n_t_3  _g_t_s_a_m_:_:_c_r_o_s_s (const _P_o_i_n_t_3 &p, const _P_o_i_n_t_3 &q, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< │ │ │ │ │ - 3, 3 > H_p=boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 3, 3 > H_q=boost::none) │ │ │ │ │ -  cross product │ │ │ │ │ -  │ │ │ │ │ - double  ggttssaamm::::ddoott (const _P_o_i_n_t_3 &p, const _P_o_i_n_t_3 &q, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 1, │ │ │ │ │ - 3 > H_p=boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 1, 3 > H_q=boost::none) │ │ │ │ │ -  dot product │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - _P_o_i_n_t_3  ggttssaamm::::mmeeaann (const CONTAINER &points) │ │ │ │ │ -  mean │ │ │ │ │ -  │ │ │ │ │ -Point3Pair  ggttssaamm::::mmeeaannss (const std::vector< Point3Pair > &abPointPairs) │ │ │ │ │ -  Calculate the two means of a set of Point3 pairs. │ │ │ │ │ +std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _S_i_m_i_l_a_r_i_t_y_3 &p) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -3D Point │ │ │ │ │ +Implementation of Similarity3 transform. │ │ │ │ │ Author │ │ │ │ │ - Alireza Fathi │ │ │ │ │ - Christian Potthast │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Paul Drews │ │ │ │ │ + John Lambert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _P_o_i_n_t_3_._h │ │ │ │ │ + * _S_i_m_i_l_a_r_i_t_y_3_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00347.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Rot3.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Fisheye.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,51 +96,39 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces | │ │ │ │ Functions
│ │ │ │ -
Rot3.cpp File Reference
│ │ │ │ +
Cal3Fisheye.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -

Rotation, common code between Rotation matrix and Quaternion. │ │ │ │ -More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

pair< Matrix3, Vector3 > gtsam::RQ (const Matrix3 &A, OptionalJacobian< 3, 9 > H=boost::none)
 [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx' such that A = R*Q = R*Qz'*Qy'*Qx'.
 
│ │ │ │ -ostream & gtsam::operator<< (ostream &os, const Rot3 &R)
 
│ │ │ │ +std::ostream & gtsam::operator<< (std::ostream &os, const Cal3Fisheye &cal)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Rotation, common code between Rotation matrix and Quaternion.

│ │ │ │ -
Author
Alireza Fathi
│ │ │ │ -
│ │ │ │ -Christian Potthast
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ +
Date
Apr 8, 2020
│ │ │ │ +
Author
ghaggin
│ │ │ │
│ │ │ │ Varun Agrawal
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,34 +1,24 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Rot3.cpp File Reference │ │ │ │ │ -Rotation, common code between Rotation matrix and Quaternion. _M_o_r_e_._._. │ │ │ │ │ +Cal3Fisheye.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -pair< Matrix3, Vector3 >  _g_t_s_a_m_:_:_R_Q (const Matrix3 &A, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 3, 9 > │ │ │ │ │ - H=boost::none) │ │ │ │ │ - [RQ] receives a 3 by 3 matrix and returns an upper │ │ │ │ │ -  triangular matrix R and 3 rotation angles │ │ │ │ │ - corresponding to the rotation matrix Q=Qz'*Qy'*Qx' │ │ │ │ │ - such that A = R*Q = R*Qz'*Qy'*Qx'. │ │ │ │ │ -  │ │ │ │ │ - ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const _R_o_t_3 &R) │ │ │ │ │ +std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _C_a_l_3_F_i_s_h_e_y_e &cal) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Rotation, common code between Rotation matrix and Quaternion. │ │ │ │ │ + Date │ │ │ │ │ + Apr 8, 2020 │ │ │ │ │ Author │ │ │ │ │ - Alireza Fathi │ │ │ │ │ - Christian Potthast │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Richard Roberts │ │ │ │ │ + ghaggin │ │ │ │ │ Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _R_o_t_3_._c_p_p │ │ │ │ │ + * _C_a_l_3_F_i_s_h_e_y_e_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00353.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Rot3.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,52 +94,53 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ Functions
│ │ │ │ -
Cal3.h File Reference
│ │ │ │ +
Rot3.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Common code for all Calibration models. │ │ │ │ +

Rotation, common code between Rotation matrix and Quaternion. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::Cal3
 Common base class for all calibration models. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

template<typename Cal , size_t Dim>
void gtsam::calibrateJacobians (const Cal &calibration, const Point2 &pn, OptionalJacobian< 2, Dim > Dcal=boost::none, OptionalJacobian< 2, 2 > Dp=boost::none)
 Function which makes use of the Implicit Function Theorem to compute the Jacobians of calibrate using uncalibrate.
 
pair< Matrix3, Vector3 > gtsam::RQ (const Matrix3 &A, OptionalJacobian< 3, 9 > H=boost::none)
 [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx' such that A = R*Q = R*Qz'*Qy'*Qx'.
 
│ │ │ │ +ostream & gtsam::operator<< (ostream &os, const Rot3 &R)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Common code for all Calibration models.

│ │ │ │ -
Author
Varun Agrawal
│ │ │ │ +

Rotation, common code between Rotation matrix and Quaternion.

│ │ │ │ +
Author
Alireza Fathi
│ │ │ │ +
│ │ │ │ +Christian Potthast
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │ +
│ │ │ │ +Varun Agrawal
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,33 +1,34 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Cal3.h File Reference │ │ │ │ │ -Common code for all Calibration models. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_C_a_l_3 │ │ │ │ │ -  Common base class for all calibration models. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Rot3.cpp File Reference │ │ │ │ │ +Rotation, common code between Rotation matrix and Quaternion. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ -void  _g_t_s_a_m_:_:_c_a_l_i_b_r_a_t_e_J_a_c_o_b_i_a_n_s (const Cal &calibration, const _P_o_i_n_t_2 &pn, │ │ │ │ │ - _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 2, Dim > Dcal=boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 2, 2 > │ │ │ │ │ - Dp=boost::none) │ │ │ │ │ -  Function which makes use of the Implicit Function Theorem to compute the │ │ │ │ │ - Jacobians of calibrate using uncalibrate. │ │ │ │ │ +pair< Matrix3, Vector3 >  _g_t_s_a_m_:_:_R_Q (const Matrix3 &A, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 3, 9 > │ │ │ │ │ + H=boost::none) │ │ │ │ │ + [RQ] receives a 3 by 3 matrix and returns an upper │ │ │ │ │ +  triangular matrix R and 3 rotation angles │ │ │ │ │ + corresponding to the rotation matrix Q=Qz'*Qy'*Qx' │ │ │ │ │ + such that A = R*Q = R*Qz'*Qy'*Qx'. │ │ │ │ │ +  │ │ │ │ │ + ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const _R_o_t_3 &R) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Common code for all Calibration models. │ │ │ │ │ +Rotation, common code between Rotation matrix and Quaternion. │ │ │ │ │ Author │ │ │ │ │ + Alireza Fathi │ │ │ │ │ + Christian Potthast │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3_._h │ │ │ │ │ + * _R_o_t_3_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00353.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a00353 = [ │ │ │ │ │ - ["calibrateJacobians", "a00353.html#a9ac814fcc4484bce32e4616d69afb225", null] │ │ │ │ │ + ["RQ", "a00353.html#a5172b5b6d51bd5348c7e551e1376f60a", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00359.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Fisheye.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/StereoCamera.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,49 +96,49 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
Cal3Fisheye.h File Reference
│ │ │ │ +
StereoCamera.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Calibration of a fisheye camera. │ │ │ │ +

A Stereo Camera based on two Simple Cameras. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::Cal3Fisheye
 Calibration of a fisheye camera. More...
class  gtsam::StereoCheiralityException
 
struct  gtsam::traits< Cal3Fisheye >
class  gtsam::StereoCamera
 A stereo camera class, parameterize by left camera pose and stereo calibration. More...
 
struct  gtsam::traits< const Cal3Fisheye >
struct  gtsam::traits< StereoCamera >
 
struct  gtsam::traits< const StereoCamera >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Calibration of a fisheye camera.

│ │ │ │ -
Date
Apr 8, 2020
│ │ │ │ -
Author
ghaggin
│ │ │ │ -
│ │ │ │ -Varun Agrawal
│ │ │ │ +

A Stereo Camera based on two Simple Cameras.

│ │ │ │ +

A Rectified Stereo Camera.

│ │ │ │ +
Author
Chris Beall
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,32 +1,33 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Cal3Fisheye.h File Reference │ │ │ │ │ -Calibration of a fisheye camera. _M_o_r_e_._._. │ │ │ │ │ +StereoCamera.h File Reference │ │ │ │ │ +A Stereo Camera based on two Simple Cameras. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_C_a_l_3_F_i_s_h_e_y_e │ │ │ │ │ -  Calibration of a fisheye camera. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_S_t_e_r_e_o_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _C_a_l_3_F_i_s_h_e_y_e_ _> │ │ │ │ │ + class   _g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a │ │ │ │ │ +  A stereo camera class, parameterize by left camera pose and stereo │ │ │ │ │ + calibration. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _C_a_l_3_F_i_s_h_e_y_e_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_t_e_r_e_o_C_a_m_e_r_a_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _S_t_e_r_e_o_C_a_m_e_r_a_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Calibration of a fisheye camera. │ │ │ │ │ - Date │ │ │ │ │ - Apr 8, 2020 │ │ │ │ │ +A Stereo Camera based on two Simple Cameras. │ │ │ │ │ +A Rectified Stereo Camera. │ │ │ │ │ Author │ │ │ │ │ - ghaggin │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + Chris Beall │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3_F_i_s_h_e_y_e_._h │ │ │ │ │ + * _S_t_e_r_e_o_C_a_m_e_r_a_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00359.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,5 @@ │ │ │ │ │ var a00359 = [ │ │ │ │ │ - ["gtsam::traits< Cal3Fisheye >", "a03000.html", null], │ │ │ │ │ - ["gtsam::traits< const Cal3Fisheye >", "a03004.html", null] │ │ │ │ │ + ["gtsam::StereoCheiralityException", "a03336.html", null], │ │ │ │ │ + ["gtsam::traits< StereoCamera >", "a03344.html", null], │ │ │ │ │ + ["gtsam::traits< const StereoCamera >", "a03348.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00359_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Fisheye.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/StereoCamera.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,177 +98,222 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Cal3Fisheye.h
│ │ │ │ +
StereoCamera.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/geometry/Cal3.h>
│ │ │ │ -
23#include <gtsam/geometry/Point2.h>
│ │ │ │ -
24
│ │ │ │ -
25#include <boost/shared_ptr.hpp>
│ │ │ │ -
26
│ │ │ │ -
27#include <string>
│ │ │ │ -
28
│ │ │ │ -
29namespace gtsam {
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/geometry/Cal3_S2Stereo.h>
│ │ │ │ +
21#include <gtsam/geometry/Pose3.h>
│ │ │ │ +
22#include <gtsam/geometry/StereoPoint2.h>
│ │ │ │ +
23
│ │ │ │ +
24namespace gtsam {
│ │ │ │ +
25
│ │ │ │ +
│ │ │ │ +
26class GTSAM_EXPORT StereoCheiralityException: public std::runtime_error {
│ │ │ │ +
27public:
│ │ │ │ +
28 StereoCheiralityException()
│ │ │ │ +
29 : StereoCheiralityException(std::numeric_limits<Key>::max()) {}
│ │ │ │
30
│ │ │ │ -
│ │ │ │ -
51class GTSAM_EXPORT Cal3Fisheye : public Cal3 {
│ │ │ │ -
52 private:
│ │ │ │ -
53 double k1_ = 0.0f, k2_ = 0.0f;
│ │ │ │ -
54 double k3_ = 0.0f, k4_ = 0.0f;
│ │ │ │ -
55 double tol_ = 1e-5;
│ │ │ │ -
56
│ │ │ │ -
57 public:
│ │ │ │ -
58 enum { dimension = 9 };
│ │ │ │ -
60 using shared_ptr = boost::shared_ptr<Cal3Fisheye>;
│ │ │ │ +
31 StereoCheiralityException(Key j)
│ │ │ │ +
32 : std::runtime_error("Stereo Cheirality Exception"),
│ │ │ │ +
33 j_(j) {}
│ │ │ │ +
34
│ │ │ │ +
35 Key nearbyVariable() const {
│ │ │ │ +
36 return j_;
│ │ │ │ +
37 }
│ │ │ │ +
38
│ │ │ │ +
39private:
│ │ │ │ +
40 Key j_;
│ │ │ │ +
41};
│ │ │ │ +
│ │ │ │ +
42
│ │ │ │ +
│ │ │ │ +
47class GTSAM_EXPORT StereoCamera {
│ │ │ │ +
48
│ │ │ │ +
49public:
│ │ │ │ +
50
│ │ │ │ +
55 typedef StereoPoint2 Measurement;
│ │ │ │ +
56 typedef StereoPoint2Vector MeasurementVector;
│ │ │ │ +
57
│ │ │ │ +
58private:
│ │ │ │ +
59 Pose3 leftCamPose_;
│ │ │ │ +
60 Cal3_S2Stereo::shared_ptr K_;
│ │ │ │
61
│ │ │ │ -
64
│ │ │ │ -
66 Cal3Fisheye() = default;
│ │ │ │ +
62public:
│ │ │ │ +
63
│ │ │ │ +
64 enum {
│ │ │ │ +
65 dimension = 6
│ │ │ │ +
66 };
│ │ │ │
67
│ │ │ │ -
68 Cal3Fisheye(const double fx, const double fy, const double s, const double u0,
│ │ │ │ -
69 const double v0, const double k1, const double k2,
│ │ │ │ -
70 const double k3, const double k4, double tol = 1e-5)
│ │ │ │ -
71 : Cal3(fx, fy, s, u0, v0),
│ │ │ │ -
72 k1_(k1),
│ │ │ │ -
73 k2_(k2),
│ │ │ │ -
74 k3_(k3),
│ │ │ │ -
75 k4_(k4),
│ │ │ │ -
76 tol_(tol) {}
│ │ │ │ -
77
│ │ │ │ -
78 ~Cal3Fisheye() override {}
│ │ │ │ -
79
│ │ │ │ +
70
│ │ │ │ +
│ │ │ │ +
72 StereoCamera() :
│ │ │ │ +
73 K_(new Cal3_S2Stereo()) {
│ │ │ │ +
74 }
│ │ │ │ +
│ │ │ │ +
75
│ │ │ │ +
77 StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K);
│ │ │ │ +
78
│ │ │ │ +
│ │ │ │ +
80 const Cal3_S2Stereo& calibration() const {
│ │ │ │ +
81 return *K_;
│ │ │ │ +
82 }
│ │ │ │ +
│ │ │ │
83
│ │ │ │ -
84 explicit Cal3Fisheye(const Vector9& v)
│ │ │ │ -
85 : Cal3(v(0), v(1), v(2), v(3), v(4)),
│ │ │ │ -
86 k1_(v(5)),
│ │ │ │ -
87 k2_(v(6)),
│ │ │ │ -
88 k3_(v(7)),
│ │ │ │ -
89 k4_(v(8)) {}
│ │ │ │ -
90
│ │ │ │ -
94
│ │ │ │ -
96 inline double k1() const { return k1_; }
│ │ │ │ -
97
│ │ │ │ -
99 inline double k2() const { return k2_; }
│ │ │ │ -
100
│ │ │ │ -
102 inline double k3() const { return k3_; }
│ │ │ │ +
87
│ │ │ │ +
│ │ │ │ +
89 void print(const std::string& s = "") const {
│ │ │ │ +
90 leftCamPose_.print(s + ".camera.");
│ │ │ │ +
91 K_->print(s + ".calibration.");
│ │ │ │ +
92 }
│ │ │ │ +
│ │ │ │ +
93
│ │ │ │ +
│ │ │ │ +
95 bool equals(const StereoCamera &camera, double tol = 1e-9) const {
│ │ │ │ +
96 return leftCamPose_.equals(camera.leftCamPose_, tol)
│ │ │ │ +
97 && K_->equals(*camera.K_, tol);
│ │ │ │ +
98 }
│ │ │ │ +
│ │ │ │ +
99
│ │ │ │
103
│ │ │ │ -
105 inline double k4() const { return k4_; }
│ │ │ │ -
106
│ │ │ │ -
108 Vector4 k() const { return Vector4(k1_, k2_, k3_, k4_); }
│ │ │ │ -
109
│ │ │ │ -
111 Vector9 vector() const;
│ │ │ │ -
112
│ │ │ │ -
114 static double Scaling(double r);
│ │ │ │ -
115
│ │ │ │ -
124 Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
│ │ │ │ -
125 OptionalJacobian<2, 2> Dp = boost::none) const;
│ │ │ │ -
126
│ │ │ │ -
135 Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
│ │ │ │ -
136 OptionalJacobian<2, 2> Dp = boost::none) const;
│ │ │ │ +
│ │ │ │ +
105 inline size_t dim() const {
│ │ │ │ +
106 return 6;
│ │ │ │ +
107 }
│ │ │ │ +
│ │ │ │ +
108
│ │ │ │ +
│ │ │ │ +
110 static inline size_t Dim() {
│ │ │ │ +
111 return 6;
│ │ │ │ +
112 }
│ │ │ │ +
│ │ │ │ +
113
│ │ │ │ +
│ │ │ │ +
115 inline StereoCamera retract(const Vector& v) const {
│ │ │ │ +
116 return StereoCamera(pose().retract(v), K_);
│ │ │ │ +
117 }
│ │ │ │ +
│ │ │ │ +
118
│ │ │ │ +
│ │ │ │ +
120 inline Vector6 localCoordinates(const StereoCamera& t2) const {
│ │ │ │ +
121 return leftCamPose_.localCoordinates(t2.leftCamPose_);
│ │ │ │ +
122 }
│ │ │ │ +
│ │ │ │ +
123
│ │ │ │ +
127
│ │ │ │ +
│ │ │ │ +
129 const Pose3& pose() const {
│ │ │ │ +
130 return leftCamPose_;
│ │ │ │ +
131 }
│ │ │ │ +
│ │ │ │ +
132
│ │ │ │ +
│ │ │ │ +
134 double baseline() const {
│ │ │ │ +
135 return K_->baseline();
│ │ │ │ +
136 }
│ │ │ │ +
│ │ │ │
137
│ │ │ │ -
141
│ │ │ │ -
143 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
│ │ │ │ -
144 const Cal3Fisheye& cal);
│ │ │ │ -
145
│ │ │ │ -
147 void print(const std::string& s = "") const override;
│ │ │ │ -
148
│ │ │ │ -
150 bool equals(const Cal3Fisheye& K, double tol = 10e-9) const;
│ │ │ │ -
151
│ │ │ │ -
155
│ │ │ │ -
157 size_t dim() const override { return Dim(); }
│ │ │ │ +
139 StereoPoint2 project(const Point3& point) const;
│ │ │ │ +
140
│ │ │ │ +
145 StereoPoint2 project2(const Point3& point, OptionalJacobian<3, 6> H1 =
│ │ │ │ +
146 boost::none, OptionalJacobian<3, 3> H2 = boost::none) const;
│ │ │ │ +
147
│ │ │ │ +
149 Point3 backproject(const StereoPoint2& z) const;
│ │ │ │ +
150
│ │ │ │ +
155 Point3 backproject2(const StereoPoint2& z,
│ │ │ │ +
156 OptionalJacobian<3, 6> H1 = boost::none,
│ │ │ │ +
157 OptionalJacobian<3, 3> H2 = boost::none) const;
│ │ │ │
158
│ │ │ │ -
160 inline static size_t Dim() { return dimension; }
│ │ │ │ -
161
│ │ │ │ -
│ │ │ │ -
163 inline Cal3Fisheye retract(const Vector& d) const {
│ │ │ │ -
164 return Cal3Fisheye(vector() + d);
│ │ │ │ -
165 }
│ │ │ │ -
│ │ │ │ -
166
│ │ │ │ -
│ │ │ │ -
168 Vector localCoordinates(const Cal3Fisheye& T2) const {
│ │ │ │ -
169 return T2.vector() - vector();
│ │ │ │ -
170 }
│ │ │ │ -
│ │ │ │ -
171
│ │ │ │ -
175
│ │ │ │ -
│ │ │ │ -
177 virtual boost::shared_ptr<Cal3Fisheye> clone() const {
│ │ │ │ -
178 return boost::shared_ptr<Cal3Fisheye>(new Cal3Fisheye(*this));
│ │ │ │ -
179 }
│ │ │ │ -
│ │ │ │ -
180
│ │ │ │ -
182
│ │ │ │ -
183 private:
│ │ │ │ -
186
│ │ │ │ -
188 friend class boost::serialization::access;
│ │ │ │ -
189 template <class Archive>
│ │ │ │ -
190 void serialize(Archive& ar, const unsigned int /*version*/) {
│ │ │ │ -
191 ar& boost::serialization::make_nvp(
│ │ │ │ -
192 "Cal3Fisheye", boost::serialization::base_object<Cal3>(*this));
│ │ │ │ -
193 ar& BOOST_SERIALIZATION_NVP(k1_);
│ │ │ │ -
194 ar& BOOST_SERIALIZATION_NVP(k2_);
│ │ │ │ -
195 ar& BOOST_SERIALIZATION_NVP(k3_);
│ │ │ │ -
196 ar& BOOST_SERIALIZATION_NVP(k4_);
│ │ │ │ -
197 }
│ │ │ │ -
198
│ │ │ │ -
200};
│ │ │ │ -
│ │ │ │ -
201
│ │ │ │ -
202template <>
│ │ │ │ -
203struct traits<Cal3Fisheye> : public internal::Manifold<Cal3Fisheye> {};
│ │ │ │ -
204
│ │ │ │ -
205template <>
│ │ │ │ -
206struct traits<const Cal3Fisheye> : public internal::Manifold<Cal3Fisheye> {};
│ │ │ │ -
207
│ │ │ │ -
208} // namespace gtsam
│ │ │ │ -
Cal3.h
Common code for all Calibration models.
│ │ │ │ -
Point2.h
2D Point
│ │ │ │ +
162
│ │ │ │ +
169 StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1,
│ │ │ │ +
170 OptionalJacobian<3, 3> H2 = boost::none, OptionalJacobian<3, 0> H3 =
│ │ │ │ +
171 boost::none) const;
│ │ │ │ +
172
│ │ │ │ +
│ │ │ │ +
174 Vector defaultErrorWhenTriangulatingBehindCamera() const {
│ │ │ │ +
175 return Eigen::Matrix<double,traits<Measurement>::dimension,1>::Constant(2.0 * K_->fx());;
│ │ │ │ +
176 }
│ │ │ │ +
│ │ │ │ +
177
│ │ │ │ +
179
│ │ │ │ +
180private:
│ │ │ │ +
181
│ │ │ │ +
182 friend class boost::serialization::access;
│ │ │ │ +
183 template<class Archive>
│ │ │ │ +
184 void serialize(Archive & ar, const unsigned int /*version*/) {
│ │ │ │ +
185 ar & BOOST_SERIALIZATION_NVP(leftCamPose_);
│ │ │ │ +
186 ar & BOOST_SERIALIZATION_NVP(K_);
│ │ │ │ +
187 }
│ │ │ │ +
188
│ │ │ │ +
189};
│ │ │ │ +
│ │ │ │ +
190
│ │ │ │ +
191template<>
│ │ │ │ +
│ │ │ │ +
192struct traits<StereoCamera> : public internal::Manifold<StereoCamera> {
│ │ │ │ +
193};
│ │ │ │ +
│ │ │ │ +
194
│ │ │ │ +
195template<>
│ │ │ │ +
│ │ │ │ +
196struct traits<const StereoCamera> : public internal::Manifold<StereoCamera> {
│ │ │ │ +
197};
│ │ │ │ +
│ │ │ │ +
198}
│ │ │ │ +
Pose3.h
3D Pose
│ │ │ │ +
StereoPoint2.h
A 2D stereo point (uL,uR,v)
│ │ │ │ +
Cal3_S2Stereo.h
The most common 5DOF 3D->2D calibration + Stereo baseline.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │ +
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ +
gtsam::project
Point2_ project(const Point3_ &p_cam)
Expression version of PinholeBase::Project.
Definition expressions.h:131
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::LieGroup::localCoordinates
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g
Definition Lie.h:136
│ │ │ │
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
│ │ │ │
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Cal3
Common base class for all calibration models.
Definition Cal3.h:69
│ │ │ │ -
gtsam::Cal3Fisheye
Calibration of a fisheye camera.
Definition Cal3Fisheye.h:51
│ │ │ │ -
gtsam::Cal3Fisheye::k4
double k4() const
Second tangential distortion coefficient.
Definition Cal3Fisheye.h:105
│ │ │ │ -
gtsam::Cal3Fisheye::k1
double k1() const
First distortion coefficient.
Definition Cal3Fisheye.h:96
│ │ │ │ -
gtsam::Cal3Fisheye::k3
double k3() const
First tangential distortion coefficient.
Definition Cal3Fisheye.h:102
│ │ │ │ -
gtsam::Cal3Fisheye::localCoordinates
Vector localCoordinates(const Cal3Fisheye &T2) const
Given a different calibration, calculate update to obtain it.
Definition Cal3Fisheye.h:168
│ │ │ │ -
gtsam::Cal3Fisheye::Dim
static size_t Dim()
Return dimensions of calibration manifold object.
Definition Cal3Fisheye.h:160
│ │ │ │ -
gtsam::Cal3Fisheye::k2
double k2() const
Second distortion coefficient.
Definition Cal3Fisheye.h:99
│ │ │ │ -
gtsam::Cal3Fisheye::dim
size_t dim() const override
Return dimensions of calibration manifold object.
Definition Cal3Fisheye.h:157
│ │ │ │ -
gtsam::Cal3Fisheye::vector
Vector9 vector() const
Return all parameters as a vector.
Definition Cal3Fisheye.cpp:28
│ │ │ │ -
gtsam::Cal3Fisheye::clone
virtual boost::shared_ptr< Cal3Fisheye > clone() const
Definition Cal3Fisheye.h:177
│ │ │ │ -
gtsam::Cal3Fisheye::k
Vector4 k() const
return distortion parameter vector
Definition Cal3Fisheye.h:108
│ │ │ │ -
gtsam::Cal3Fisheye::Cal3Fisheye
Cal3Fisheye()=default
Default Constructor with only unit focal length.
│ │ │ │ -
gtsam::Cal3Fisheye::retract
Cal3Fisheye retract(const Vector &d) const
Given delta vector, update calibration.
Definition Cal3Fisheye.h:163
│ │ │ │ +
gtsam::Cal3_S2Stereo
The most common 5DOF 3D->2D calibration, stereo version.
Definition Cal3_S2Stereo.h:30
│ │ │ │ +
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ +
gtsam::Pose3::equals
bool equals(const Pose3 &pose, double tol=1e-9) const
assert equality up to a tolerance
Definition Pose3.cpp:157
│ │ │ │ +
gtsam::Pose3::print
void print(const std::string &s="") const
print with optional string
Definition Pose3.cpp:152
│ │ │ │ +
gtsam::StereoCheiralityException
Definition StereoCamera.h:26
│ │ │ │ +
gtsam::StereoCamera
A stereo camera class, parameterize by left camera pose and stereo calibration.
Definition StereoCamera.h:47
│ │ │ │ +
gtsam::StereoCamera::StereoCamera
StereoCamera()
Default constructor allocates a calibration!
Definition StereoCamera.h:72
│ │ │ │ +
gtsam::StereoCamera::Dim
static size_t Dim()
Dimensionality of the tangent space.
Definition StereoCamera.h:110
│ │ │ │ +
gtsam::StereoCamera::print
void print(const std::string &s="") const
print
Definition StereoCamera.h:89
│ │ │ │ +
gtsam::StereoCamera::localCoordinates
Vector6 localCoordinates(const StereoCamera &t2) const
Local coordinates of manifold neighborhood around current value.
Definition StereoCamera.h:120
│ │ │ │ +
gtsam::StereoCamera::equals
bool equals(const StereoCamera &camera, double tol=1e-9) const
equals
Definition StereoCamera.h:95
│ │ │ │ +
gtsam::StereoCamera::dim
size_t dim() const
Dimensionality of the tangent space.
Definition StereoCamera.h:105
│ │ │ │ +
gtsam::StereoCamera::retract
StereoCamera retract(const Vector &v) const
Updates a with tangent space delta.
Definition StereoCamera.h:115
│ │ │ │ +
gtsam::StereoCamera::Measurement
StereoPoint2 Measurement
Some classes template on either PinholeCamera or StereoCamera, and this typedef informs those classes...
Definition StereoCamera.h:55
│ │ │ │ +
gtsam::StereoCamera::calibration
const Cal3_S2Stereo & calibration() const
Return shared pointer to calibration.
Definition StereoCamera.h:80
│ │ │ │ +
gtsam::StereoCamera::baseline
double baseline() const
baseline
Definition StereoCamera.h:134
│ │ │ │ +
gtsam::StereoCamera::pose
const Pose3 & pose() const
pose
Definition StereoCamera.h:129
│ │ │ │ +
gtsam::StereoCamera::defaultErrorWhenTriangulatingBehindCamera
Vector defaultErrorWhenTriangulatingBehindCamera() const
for Nonlinear Triangulation
Definition StereoCamera.h:174
│ │ │ │ +
gtsam::StereoPoint2
A 2D stereo point, v will be same for rectified images.
Definition StereoPoint2.h:32
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,224 +1,265 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Cal3Fisheye.h │ │ │ │ │ +StereoCamera.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ -24 │ │ │ │ │ -25#include │ │ │ │ │ -26 │ │ │ │ │ -27#include │ │ │ │ │ -28 │ │ │ │ │ -29namespace _g_t_s_a_m { │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3___S_2_S_t_e_r_e_o_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_3_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_S_t_e_r_e_o_P_o_i_n_t_2_._h> │ │ │ │ │ +23 │ │ │ │ │ +24namespace _g_t_s_a_m { │ │ │ │ │ +25 │ │ │ │ │ +_2_6class GTSAM_EXPORT _S_t_e_r_e_o_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n: public std::runtime_error { │ │ │ │ │ +27public: │ │ │ │ │ +28 _S_t_e_r_e_o_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n() │ │ │ │ │ +29 : _S_t_e_r_e_o_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n(std::numeric_limits::max()) {} │ │ │ │ │ 30 │ │ │ │ │ -_5_1class GTSAM_EXPORT _C_a_l_3_F_i_s_h_e_y_e : public _C_a_l_3 { │ │ │ │ │ -52 private: │ │ │ │ │ -53 double k1_ = 0.0f, k2_ = 0.0f; │ │ │ │ │ -54 double k3_ = 0.0f, k4_ = 0.0f; │ │ │ │ │ -55 double tol_ = 1e-5; │ │ │ │ │ -56 │ │ │ │ │ -57 public: │ │ │ │ │ -58 enum { dimension = 9 }; │ │ │ │ │ -60 using shared_ptr = boost::shared_ptr; │ │ │ │ │ +31 _S_t_e_r_e_o_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n(_K_e_y j) │ │ │ │ │ +32 : std::runtime_error("Stereo Cheirality Exception"), │ │ │ │ │ +33 j_(j) {} │ │ │ │ │ +34 │ │ │ │ │ +35 _K_e_y nearbyVariable() const { │ │ │ │ │ +36 return j_; │ │ │ │ │ +37 } │ │ │ │ │ +38 │ │ │ │ │ +39private: │ │ │ │ │ +40 _K_e_y j_; │ │ │ │ │ +41}; │ │ │ │ │ +42 │ │ │ │ │ +_4_7class GTSAM_EXPORT _S_t_e_r_e_o_C_a_m_e_r_a { │ │ │ │ │ +48 │ │ │ │ │ +49public: │ │ │ │ │ +50 │ │ │ │ │ +_5_5 typedef _S_t_e_r_e_o_P_o_i_n_t_2 _M_e_a_s_u_r_e_m_e_n_t; │ │ │ │ │ +56 typedef StereoPoint2Vector MeasurementVector; │ │ │ │ │ +57 │ │ │ │ │ +58private: │ │ │ │ │ +59 _P_o_s_e_3 leftCamPose_; │ │ │ │ │ +60 Cal3_S2Stereo::shared_ptr K_; │ │ │ │ │ 61 │ │ │ │ │ -64 │ │ │ │ │ -_6_6 _C_a_l_3_F_i_s_h_e_y_e() = default; │ │ │ │ │ +62public: │ │ │ │ │ +63 │ │ │ │ │ +64 enum { │ │ │ │ │ +65 dimension = 6 │ │ │ │ │ +66 }; │ │ │ │ │ 67 │ │ │ │ │ -68 _C_a_l_3_F_i_s_h_e_y_e(const double fx, const double fy, const double s, const double │ │ │ │ │ -u0, │ │ │ │ │ -69 const double v0, const double k1, const double k2, │ │ │ │ │ -70 const double k3, const double k4, double tol = 1e-5) │ │ │ │ │ -71 : _C_a_l_3(fx, fy, s, u0, v0), │ │ │ │ │ -72 k1_(k1), │ │ │ │ │ -73 k2_(k2), │ │ │ │ │ -74 k3_(k3), │ │ │ │ │ -75 k4_(k4), │ │ │ │ │ -76 tol_(tol) {} │ │ │ │ │ -77 │ │ │ │ │ -78 _~_C_a_l_3_F_i_s_h_e_y_e() override {} │ │ │ │ │ -79 │ │ │ │ │ +70 │ │ │ │ │ +_7_2 _S_t_e_r_e_o_C_a_m_e_r_a() : │ │ │ │ │ +73 K_(new _C_a_l_3___S_2_S_t_e_r_e_o()) { │ │ │ │ │ +74 } │ │ │ │ │ +75 │ │ │ │ │ +77 _S_t_e_r_e_o_C_a_m_e_r_a(const _P_o_s_e_3& leftCamPose, const Cal3_S2Stereo::shared_ptr K); │ │ │ │ │ +78 │ │ │ │ │ +_8_0 const _C_a_l_3___S_2_S_t_e_r_e_o& _c_a_l_i_b_r_a_t_i_o_n() const { │ │ │ │ │ +81 return *K_; │ │ │ │ │ +82 } │ │ │ │ │ 83 │ │ │ │ │ -84 explicit Cal3Fisheye(const Vector9& v) │ │ │ │ │ -85 : Cal3(v(0), v(1), v(2), v(3), v(4)), │ │ │ │ │ -86 k1_(v(5)), │ │ │ │ │ -87 k2_(v(6)), │ │ │ │ │ -88 k3_(v(7)), │ │ │ │ │ -89 k4_(v(8)) {} │ │ │ │ │ -90 │ │ │ │ │ -94 │ │ │ │ │ -_9_6 inline double _k_1() const { return k1_; } │ │ │ │ │ -97 │ │ │ │ │ -_9_9 inline double _k_2() const { return k2_; } │ │ │ │ │ -100 │ │ │ │ │ -_1_0_2 inline double _k_3() const { return k3_; } │ │ │ │ │ +87 │ │ │ │ │ +_8_9 void _p_r_i_n_t(const std::string& s = "") const { │ │ │ │ │ +90 leftCamPose_._p_r_i_n_t(s + ".camera."); │ │ │ │ │ +91 K_->print(s + ".calibration."); │ │ │ │ │ +92 } │ │ │ │ │ +93 │ │ │ │ │ +_9_5 bool _e_q_u_a_l_s(const _S_t_e_r_e_o_C_a_m_e_r_a &camera, double tol = 1e-9) const { │ │ │ │ │ +96 return leftCamPose_._e_q_u_a_l_s(camera.leftCamPose_, tol) │ │ │ │ │ +97 && K_->equals(*camera.K_, tol); │ │ │ │ │ +98 } │ │ │ │ │ +99 │ │ │ │ │ 103 │ │ │ │ │ -_1_0_5 inline double _k_4() const { return k4_; } │ │ │ │ │ -106 │ │ │ │ │ -_1_0_8 Vector4 _k() const { return Vector4(k1_, k2_, k3_, k4_); } │ │ │ │ │ -109 │ │ │ │ │ -111 Vector9 vector() const; │ │ │ │ │ -112 │ │ │ │ │ -114 static double Scaling(double r); │ │ │ │ │ -115 │ │ │ │ │ -124 _P_o_i_n_t_2 uncalibrate(const _P_o_i_n_t_2& p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _9_> Dcal = boost:: │ │ │ │ │ -none, │ │ │ │ │ -125 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dp = boost::none) const; │ │ │ │ │ -126 │ │ │ │ │ -135 _P_o_i_n_t_2 calibrate(const _P_o_i_n_t_2& p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _9_> Dcal = boost:: │ │ │ │ │ -none, │ │ │ │ │ -136 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dp = boost::none) const; │ │ │ │ │ +_1_0_5 inline size_t _d_i_m() const { │ │ │ │ │ +106 return 6; │ │ │ │ │ +107 } │ │ │ │ │ +108 │ │ │ │ │ +_1_1_0 static inline size_t _D_i_m() { │ │ │ │ │ +111 return 6; │ │ │ │ │ +112 } │ │ │ │ │ +113 │ │ │ │ │ +_1_1_5 inline _S_t_e_r_e_o_C_a_m_e_r_a _r_e_t_r_a_c_t(const Vector& v) const { │ │ │ │ │ +116 return _S_t_e_r_e_o_C_a_m_e_r_a(pose().retract(v), K_); │ │ │ │ │ +117 } │ │ │ │ │ +118 │ │ │ │ │ +_1_2_0 inline Vector6 _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(const _S_t_e_r_e_o_C_a_m_e_r_a& t2) const { │ │ │ │ │ +121 return leftCamPose_._l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(t2.leftCamPose_); │ │ │ │ │ +122 } │ │ │ │ │ +123 │ │ │ │ │ +127 │ │ │ │ │ +_1_2_9 const _P_o_s_e_3& _p_o_s_e() const { │ │ │ │ │ +130 return leftCamPose_; │ │ │ │ │ +131 } │ │ │ │ │ +132 │ │ │ │ │ +_1_3_4 double _b_a_s_e_l_i_n_e() const { │ │ │ │ │ +135 return K_->baseline(); │ │ │ │ │ +136 } │ │ │ │ │ 137 │ │ │ │ │ -141 │ │ │ │ │ -143 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, │ │ │ │ │ -144 const _C_a_l_3_F_i_s_h_e_y_e& cal); │ │ │ │ │ -145 │ │ │ │ │ -147 void _p_r_i_n_t(const std::string& s = "") const override; │ │ │ │ │ -148 │ │ │ │ │ -150 bool _e_q_u_a_l_s(const _C_a_l_3_F_i_s_h_e_y_e& K, double tol = 10e-9) const; │ │ │ │ │ -151 │ │ │ │ │ -155 │ │ │ │ │ -_1_5_7 size_t _d_i_m() const override { return Dim(); } │ │ │ │ │ +139 _S_t_e_r_e_o_P_o_i_n_t_2 _p_r_o_j_e_c_t(const _P_o_i_n_t_3& point) const; │ │ │ │ │ +140 │ │ │ │ │ +145 _S_t_e_r_e_o_P_o_i_n_t_2 project2(const _P_o_i_n_t_3& point, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _6_> H1 = │ │ │ │ │ +146 boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H2 = boost::none) const; │ │ │ │ │ +147 │ │ │ │ │ +149 _P_o_i_n_t_3 backproject(const _S_t_e_r_e_o_P_o_i_n_t_2& z) const; │ │ │ │ │ +150 │ │ │ │ │ +155 _P_o_i_n_t_3 backproject2(const _S_t_e_r_e_o_P_o_i_n_t_2& z, │ │ │ │ │ +156 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _6_> H1 = boost::none, │ │ │ │ │ +157 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H2 = boost::none) const; │ │ │ │ │ 158 │ │ │ │ │ -_1_6_0 inline static size_t _D_i_m() { return dimension; } │ │ │ │ │ -161 │ │ │ │ │ -_1_6_3 inline _C_a_l_3_F_i_s_h_e_y_e _r_e_t_r_a_c_t(const Vector& d) const { │ │ │ │ │ -164 return _C_a_l_3_F_i_s_h_e_y_e(vector() + d); │ │ │ │ │ -165 } │ │ │ │ │ -166 │ │ │ │ │ -_1_6_8 Vector _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(const _C_a_l_3_F_i_s_h_e_y_e& T2) const { │ │ │ │ │ -169 return T2._v_e_c_t_o_r() - vector(); │ │ │ │ │ -170 } │ │ │ │ │ -171 │ │ │ │ │ -175 │ │ │ │ │ -_1_7_7 virtual boost::shared_ptr _c_l_o_n_e() const { │ │ │ │ │ -178 return boost::shared_ptr(new _C_a_l_3_F_i_s_h_e_y_e(*this)); │ │ │ │ │ -179 } │ │ │ │ │ -180 │ │ │ │ │ -182 │ │ │ │ │ -183 private: │ │ │ │ │ -186 │ │ │ │ │ -_1_8_8 friend class boost::serialization::access; │ │ │ │ │ -189 template │ │ │ │ │ -190 void serialize(Archive& ar, const unsigned int /*version*/) { │ │ │ │ │ -191 ar& boost::serialization::make_nvp( │ │ │ │ │ -192 "Cal3Fisheye", boost::serialization::base_object(*this)); │ │ │ │ │ -193 ar& BOOST_SERIALIZATION_NVP(k1_); │ │ │ │ │ -194 ar& BOOST_SERIALIZATION_NVP(k2_); │ │ │ │ │ -195 ar& BOOST_SERIALIZATION_NVP(k3_); │ │ │ │ │ -196 ar& BOOST_SERIALIZATION_NVP(k4_); │ │ │ │ │ -197 } │ │ │ │ │ -198 │ │ │ │ │ -200}; │ │ │ │ │ -201 │ │ │ │ │ -202template <> │ │ │ │ │ -_2_0_3struct _t_r_a_i_t_s<_C_a_l_3_F_i_s_h_e_y_e> : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d {}; │ │ │ │ │ -204 │ │ │ │ │ -205template <> │ │ │ │ │ -_2_0_6struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ -{}; │ │ │ │ │ -207 │ │ │ │ │ -208} // namespace gtsam │ │ │ │ │ -_C_a_l_3_._h │ │ │ │ │ -Common code for all Calibration models. │ │ │ │ │ -_P_o_i_n_t_2_._h │ │ │ │ │ -2D Point │ │ │ │ │ +162 │ │ │ │ │ +169 _S_t_e_r_e_o_P_o_i_n_t_2 _p_r_o_j_e_c_t(const _P_o_i_n_t_3& point, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _6_> H1, │ │ │ │ │ +170 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H2 = boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _0_> H3 = │ │ │ │ │ +171 boost::none) const; │ │ │ │ │ +172 │ │ │ │ │ +_1_7_4 Vector _d_e_f_a_u_l_t_E_r_r_o_r_W_h_e_n_T_r_i_a_n_g_u_l_a_t_i_n_g_B_e_h_i_n_d_C_a_m_e_r_a() const { │ │ │ │ │ +175 return Eigen::Matrix::dimension,1>::Constant(2.0 │ │ │ │ │ +* K_->fx());; │ │ │ │ │ +176 } │ │ │ │ │ +177 │ │ │ │ │ +179 │ │ │ │ │ +180private: │ │ │ │ │ +181 │ │ │ │ │ +182 friend class boost::serialization::access; │ │ │ │ │ +183 template │ │ │ │ │ +184 void serialize(Archive & ar, const unsigned int /*version*/) { │ │ │ │ │ +185 ar & BOOST_SERIALIZATION_NVP(leftCamPose_); │ │ │ │ │ +186 ar & BOOST_SERIALIZATION_NVP(K_); │ │ │ │ │ +187 } │ │ │ │ │ +188 │ │ │ │ │ +189}; │ │ │ │ │ +190 │ │ │ │ │ +191template<> │ │ │ │ │ +_1_9_2struct _t_r_a_i_t_s<_S_t_e_r_e_o_C_a_m_e_r_a> : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d { │ │ │ │ │ +193}; │ │ │ │ │ +194 │ │ │ │ │ +195template<> │ │ │ │ │ +_1_9_6struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ +{ │ │ │ │ │ +197}; │ │ │ │ │ +198} │ │ │ │ │ +_P_o_s_e_3_._h │ │ │ │ │ +3D Pose │ │ │ │ │ +_S_t_e_r_e_o_P_o_i_n_t_2_._h │ │ │ │ │ +A 2D stereo point (uL,uR,v) │ │ │ │ │ +_C_a_l_3___S_2_S_t_e_r_e_o_._h │ │ │ │ │ +The most common 5DOF 3D->2D calibration + Stereo baseline. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ -Vector2 Point2 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ +Vector3 Point3 │ │ │ │ │ As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point2 to Vector2... │ │ │ │ │ -DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ +typedef Point3 to Vector3... │ │ │ │ │ +DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_o_j_e_c_t │ │ │ │ │ +Point2_ project(const Point3_ &p_cam) │ │ │ │ │ +Expression version of PinholeBase::Project. │ │ │ │ │ +DDeeffiinniittiioonn expressions.h:131 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ +TangentVector localCoordinates(const Class &g) const │ │ │ │ │ +localCoordinates as required by manifold concept: finds tangent vector between │ │ │ │ │ +*this and g │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:136 │ │ │ │ │ _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ Both ManifoldTraits and Testable. │ │ │ │ │ DDeeffiinniittiioonn Manifold.h:120 │ │ │ │ │ _g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ either a fixed size o... │ │ │ │ │ DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3 │ │ │ │ │ -Common base class for all calibration models. │ │ │ │ │ -DDeeffiinniittiioonn Cal3.h:69 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_F_i_s_h_e_y_e │ │ │ │ │ -Calibration of a fisheye camera. │ │ │ │ │ -DDeeffiinniittiioonn Cal3Fisheye.h:51 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_F_i_s_h_e_y_e_:_:_k_4 │ │ │ │ │ -double k4() const │ │ │ │ │ -Second tangential distortion coefficient. │ │ │ │ │ -DDeeffiinniittiioonn Cal3Fisheye.h:105 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_F_i_s_h_e_y_e_:_:_k_1 │ │ │ │ │ -double k1() const │ │ │ │ │ -First distortion coefficient. │ │ │ │ │ -DDeeffiinniittiioonn Cal3Fisheye.h:96 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_F_i_s_h_e_y_e_:_:_k_3 │ │ │ │ │ -double k3() const │ │ │ │ │ -First tangential distortion coefficient. │ │ │ │ │ -DDeeffiinniittiioonn Cal3Fisheye.h:102 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_F_i_s_h_e_y_e_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ -Vector localCoordinates(const Cal3Fisheye &T2) const │ │ │ │ │ -Given a different calibration, calculate update to obtain it. │ │ │ │ │ -DDeeffiinniittiioonn Cal3Fisheye.h:168 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_F_i_s_h_e_y_e_:_:_D_i_m │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o │ │ │ │ │ +The most common 5DOF 3D->2D calibration, stereo version. │ │ │ │ │ +DDeeffiinniittiioonn Cal3_S2Stereo.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ +A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const Pose3 &pose, double tol=1e-9) const │ │ │ │ │ +assert equality up to a tolerance │ │ │ │ │ +DDeeffiinniittiioonn Pose3.cpp:157 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="") const │ │ │ │ │ +print with optional string │ │ │ │ │ +DDeeffiinniittiioonn Pose3.cpp:152 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n │ │ │ │ │ +DDeeffiinniittiioonn StereoCamera.h:26 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a │ │ │ │ │ +A stereo camera class, parameterize by left camera pose and stereo calibration. │ │ │ │ │ +DDeeffiinniittiioonn StereoCamera.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a_:_:_S_t_e_r_e_o_C_a_m_e_r_a │ │ │ │ │ +StereoCamera() │ │ │ │ │ +Default constructor allocates a calibration! │ │ │ │ │ +DDeeffiinniittiioonn StereoCamera.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a_:_:_D_i_m │ │ │ │ │ static size_t Dim() │ │ │ │ │ -Return dimensions of calibration manifold object. │ │ │ │ │ -DDeeffiinniittiioonn Cal3Fisheye.h:160 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_F_i_s_h_e_y_e_:_:_k_2 │ │ │ │ │ -double k2() const │ │ │ │ │ -Second distortion coefficient. │ │ │ │ │ -DDeeffiinniittiioonn Cal3Fisheye.h:99 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_F_i_s_h_e_y_e_:_:_d_i_m │ │ │ │ │ -size_t dim() const override │ │ │ │ │ -Return dimensions of calibration manifold object. │ │ │ │ │ -DDeeffiinniittiioonn Cal3Fisheye.h:157 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_F_i_s_h_e_y_e_:_:_v_e_c_t_o_r │ │ │ │ │ -Vector9 vector() const │ │ │ │ │ -Return all parameters as a vector. │ │ │ │ │ -DDeeffiinniittiioonn Cal3Fisheye.cpp:28 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_F_i_s_h_e_y_e_:_:_c_l_o_n_e │ │ │ │ │ -virtual boost::shared_ptr< Cal3Fisheye > clone() const │ │ │ │ │ -DDeeffiinniittiioonn Cal3Fisheye.h:177 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_F_i_s_h_e_y_e_:_:_k │ │ │ │ │ -Vector4 k() const │ │ │ │ │ -return distortion parameter vector │ │ │ │ │ -DDeeffiinniittiioonn Cal3Fisheye.h:108 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_F_i_s_h_e_y_e_:_:_C_a_l_3_F_i_s_h_e_y_e │ │ │ │ │ -Cal3Fisheye()=default │ │ │ │ │ -Default Constructor with only unit focal length. │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_F_i_s_h_e_y_e_:_:_r_e_t_r_a_c_t │ │ │ │ │ -Cal3Fisheye retract(const Vector &d) const │ │ │ │ │ -Given delta vector, update calibration. │ │ │ │ │ -DDeeffiinniittiioonn Cal3Fisheye.h:163 │ │ │ │ │ +Dimensionality of the tangent space. │ │ │ │ │ +DDeeffiinniittiioonn StereoCamera.h:110 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="") const │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn StereoCamera.h:89 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ +Vector6 localCoordinates(const StereoCamera &t2) const │ │ │ │ │ +Local coordinates of manifold neighborhood around current value. │ │ │ │ │ +DDeeffiinniittiioonn StereoCamera.h:120 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const StereoCamera &camera, double tol=1e-9) const │ │ │ │ │ +equals │ │ │ │ │ +DDeeffiinniittiioonn StereoCamera.h:95 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a_:_:_d_i_m │ │ │ │ │ +size_t dim() const │ │ │ │ │ +Dimensionality of the tangent space. │ │ │ │ │ +DDeeffiinniittiioonn StereoCamera.h:105 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a_:_:_r_e_t_r_a_c_t │ │ │ │ │ +StereoCamera retract(const Vector &v) const │ │ │ │ │ +Updates a with tangent space delta. │ │ │ │ │ +DDeeffiinniittiioonn StereoCamera.h:115 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a_:_:_M_e_a_s_u_r_e_m_e_n_t │ │ │ │ │ +StereoPoint2 Measurement │ │ │ │ │ +Some classes template on either PinholeCamera or StereoCamera, and this typedef │ │ │ │ │ +informs those classes... │ │ │ │ │ +DDeeffiinniittiioonn StereoCamera.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a_:_:_c_a_l_i_b_r_a_t_i_o_n │ │ │ │ │ +const Cal3_S2Stereo & calibration() const │ │ │ │ │ +Return shared pointer to calibration. │ │ │ │ │ +DDeeffiinniittiioonn StereoCamera.h:80 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a_:_:_b_a_s_e_l_i_n_e │ │ │ │ │ +double baseline() const │ │ │ │ │ +baseline │ │ │ │ │ +DDeeffiinniittiioonn StereoCamera.h:134 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a_:_:_p_o_s_e │ │ │ │ │ +const Pose3 & pose() const │ │ │ │ │ +pose │ │ │ │ │ +DDeeffiinniittiioonn StereoCamera.h:129 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a_:_:_d_e_f_a_u_l_t_E_r_r_o_r_W_h_e_n_T_r_i_a_n_g_u_l_a_t_i_n_g_B_e_h_i_n_d_C_a_m_e_r_a │ │ │ │ │ +Vector defaultErrorWhenTriangulatingBehindCamera() const │ │ │ │ │ +for Nonlinear Triangulation │ │ │ │ │ +DDeeffiinniittiioonn StereoCamera.h:174 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2 │ │ │ │ │ +A 2D stereo point, v will be same for rectified images. │ │ │ │ │ +DDeeffiinniittiioonn StereoPoint2.h:32 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3_F_i_s_h_e_y_e_._h │ │ │ │ │ + * _S_t_e_r_e_o_C_a_m_e_r_a_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00365.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SO3.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Unified.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,89 +94,51 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
SO3.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
Cal3Unified.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

3*3 matrix representation of SO(3) │ │ │ │ +

Unified Calibration Model, see Mei07icra for details. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::Cal3Unified
 Calibration of a omni-directional camera with mirror + lens radial distortion. More...
 
struct  gtsam::traits< Cal3Unified >
 
struct  gtsam::traits< const Cal3Unified >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -GTSAM_EXPORT Matrix99 gtsam::so3::Dcompose (const SO3 &R)
 (constant) Jacobian of compose wrpt M
 
GTSAM_EXPORT Matrix3 gtsam::so3::compose (const Matrix3 &M, const SO3 &R, OptionalJacobian< 9, 9 > H=boost::none)
 Compose general matrix with an SO(3) element.
 
│ │ │ │

Detailed Description

│ │ │ │ -

3*3 matrix representation of SO(3)

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +

Unified Calibration Model, see Mei07icra for details.

│ │ │ │ +
Date
Mar 8, 2014
│ │ │ │ +
Author
Jing Dong
│ │ │ │
│ │ │ │ -Luca Carlone
│ │ │ │ -
│ │ │ │ -Duy Nguyen Ta
│ │ │ │ -
Date
December 2014
│ │ │ │ -

Function Documentation

│ │ │ │ - │ │ │ │ -

◆ compose()

│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
GTSAM_EXPORT Matrix3 gtsam::so3::compose (const Matrix3 & M,
const SO3 & R,
OptionalJacobian< 9, 9 > H = boost::none 
)
│ │ │ │ -
│ │ │ │ - │ │ │ │ -

Compose general matrix with an SO(3) element.

│ │ │ │ -

We only provide the 9*9 derivative in the first argument M.

│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ +Varun Agrawal │ │ │ │ +
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,41 +1,33 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -SO3.cpp File Reference │ │ │ │ │ -3*3 matrix representation of SO(3) _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +Cal3Unified.h File Reference │ │ │ │ │ +Unified Calibration Model, see Mei07icra for details. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_C_a_l_3_U_n_i_f_i_e_d │ │ │ │ │ +  Calibration of a omni-directional camera with mirror + lens radial │ │ │ │ │ + distortion. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _C_a_l_3_U_n_i_f_i_e_d_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _C_a_l_3_U_n_i_f_i_e_d_ _> │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -GTSAM_EXPORT Matrix99  ggttssaamm::::ssoo33::::DDccoommppoossee (const _S_O_3 &R) │ │ │ │ │ -  (constant) Jacobian of compose wrpt M │ │ │ │ │ -  │ │ │ │ │ - GTSAM_EXPORT Matrix3  _g_t_s_a_m_:_:_s_o_3_:_:_c_o_m_p_o_s_e (const Matrix3 &M, const _S_O_3 &R, │ │ │ │ │ - _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 9, 9 > H=boost::none) │ │ │ │ │ -  Compose general matrix with an SO(3) element. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -3*3 matrix representation of SO(3) │ │ │ │ │ - Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Luca Carlone │ │ │ │ │ - Duy Nguyen Ta │ │ │ │ │ +Unified Calibration Model, see Mei07icra for details. │ │ │ │ │ Date │ │ │ │ │ - December 2014 │ │ │ │ │ -********** FFuunnccttiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ -********** _?◆_? ccoommppoossee(()) ********** │ │ │ │ │ -GTSAM_EXPORT Matrix3 gtsam::so3:: ( const Matrix3 &  MM, │ │ │ │ │ -compose │ │ │ │ │ - const _S_O_3 &  RR, │ │ │ │ │ - _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 9, 9 >  HH = boost::none  │ │ │ │ │ - ) │ │ │ │ │ -Compose general matrix with an SO(3) element. │ │ │ │ │ -We only provide the 9*9 derivative in the first argument M. │ │ │ │ │ + Mar 8, 2014 │ │ │ │ │ + Author │ │ │ │ │ + Jing Dong │ │ │ │ │ + Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_O_3_._c_p_p │ │ │ │ │ + * _C_a_l_3_U_n_i_f_i_e_d_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00365.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a00365 = [ │ │ │ │ │ - ["compose", "a00365.html#a72fc8b10f24d82aa8a4c99f99403fe5e", null], │ │ │ │ │ - ["Dcompose", "a00365.html#a7c7e26b6d07ec0f16363c196247b294d", null] │ │ │ │ │ + ["gtsam::traits< Cal3Unified >", "a03012.html", null], │ │ │ │ │ + ["gtsam::traits< const Cal3Unified >", "a03016.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00368.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/BearingRange.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cyclic.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,51 +94,35 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
BearingRange.h File Reference
│ │ │ │ +
Cyclic.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Bearing-Range product. │ │ │ │ +

Cyclic group implementation. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

struct  gtsam::BearingRange< A1, A2, B, R >
 Bearing-Range product for a particular A1,A2 combination will use the functors above to create a similar functor of type A1*A2 -> pair<Bearing::return_type,Range::return_type> For example BearingRange<Pose2,Point2>(pose,point) will return pair<Rot2,double> and BearingRange<Pose3,Point3>(pose,point) will return pair<Unit3,double> More...
 
struct  gtsam::traits< BearingRange< A1, A2 > >
 
struct  gtsam::HasBearing< A1, A2, RT >
 
struct  gtsam::HasRange< A1, A2, RT >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Bearing-Range product.

│ │ │ │ -
Date
July, 2015
│ │ │ │ +

Cyclic group implementation.

│ │ │ │
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,38 +1,20 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -BearingRange.h File Reference │ │ │ │ │ -Bearing-Range product. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_B_e_a_r_i_n_g_R_a_n_g_e_<_ _A_1_,_ _A_2_,_ _B_,_ _R_ _> │ │ │ │ │ - Bearing-Range product for a particular A1,A2 combination will use the │ │ │ │ │ - functors above to create a similar functor of type A1*A2 - │ │ │ │ │ -  > pair For example │ │ │ │ │ - BearingRange(pose,point) will return pair │ │ │ │ │ - and BearingRange(pose,point) will return │ │ │ │ │ - pair _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _B_e_a_r_i_n_g_R_a_n_g_e_<_ _A_1_,_ _A_2_ _>_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_H_a_s_B_e_a_r_i_n_g_<_ _A_1_,_ _A_2_,_ _R_T_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_H_a_s_R_a_n_g_e_<_ _A_1_,_ _A_2_,_ _R_T_ _> │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +Cyclic.cpp File Reference │ │ │ │ │ +Cyclic group implementation. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Bearing-Range product. │ │ │ │ │ - Date │ │ │ │ │ - July, 2015 │ │ │ │ │ +Cyclic group implementation. │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _B_e_a_r_i_n_g_R_a_n_g_e_._h │ │ │ │ │ + * _C_y_c_l_i_c_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00371.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Rot2.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,42 +94,53 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
Cal3.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
Rot2.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Common code for all calibration models. │ │ │ │ +

2D rotation │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::Rot2
 Rotation matrix NOTE: the angle theta is in radians unless explicitly stated. More...
 
struct  gtsam::Rot2::ChartAtOrigin
 
struct  gtsam::traits< Rot2 >
 
struct  gtsam::traits< const Rot2 >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -std::ostream & gtsam::operator<< (std::ostream &os, const Cal3 &cal)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Common code for all calibration models.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +

2D rotation

│ │ │ │ +
Date
Dec 9, 2009
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ +John Lambert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,35 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Cal3.cpp File Reference │ │ │ │ │ -Common code for all calibration models. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +Rot2.h File Reference │ │ │ │ │ +2D rotation _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_R_o_t_2 │ │ │ │ │ +  Rotation matrix NOTE: the angle theta is in radians unless explicitly │ │ │ │ │ + stated. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_R_o_t_2_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _R_o_t_2_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _R_o_t_2_ _> │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _C_a_l_3 &cal) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Common code for all calibration models. │ │ │ │ │ +2D rotation │ │ │ │ │ + Date │ │ │ │ │ + Dec 9, 2009 │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ + John Lambert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3_._c_p_p │ │ │ │ │ + * _R_o_t_2_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00374.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Fisheye.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3_S2Stereo.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,41 +94,48 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
Cal3Fisheye.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
Cal3_S2Stereo.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ + │ │ │ │ +

The most common 5DOF 3D->2D calibration + Stereo baseline. │ │ │ │ +More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::Cal3_S2Stereo
 The most common 5DOF 3D->2D calibration, stereo version. More...
 
struct  gtsam::traits< Cal3_S2Stereo >
 
struct  gtsam::traits< const Cal3_S2Stereo >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -std::ostream & gtsam::operator<< (std::ostream &os, const Cal3Fisheye &cal)
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Apr 8, 2020
│ │ │ │ -
Author
ghaggin
│ │ │ │ -
│ │ │ │ -Varun Agrawal
│ │ │ │ +

The most common 5DOF 3D->2D calibration + Stereo baseline.

│ │ │ │ +
Author
Chris Beall
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,24 +1,29 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Cal3Fisheye.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +Cal3_S2Stereo.h File Reference │ │ │ │ │ +The most common 5DOF 3D->2D calibration + Stereo baseline. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o │ │ │ │ │ +  The most common 5DOF 3D->2D calibration, stereo version. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _C_a_l_3___S_2_S_t_e_r_e_o_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _C_a_l_3___S_2_S_t_e_r_e_o_ _> │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _C_a_l_3_F_i_s_h_e_y_e &cal) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Apr 8, 2020 │ │ │ │ │ +The most common 5DOF 3D->2D calibration + Stereo baseline. │ │ │ │ │ Author │ │ │ │ │ - ghaggin │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + Chris Beall │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3_F_i_s_h_e_y_e_._c_p_p │ │ │ │ │ + * _C_a_l_3___S_2_S_t_e_r_e_o_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00377.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/StereoCamera.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Similarity2.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,51 +94,42 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
StereoCamera.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
Similarity2.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

A Stereo Camera based on two Simple Cameras. │ │ │ │ +

Implementation of Similarity2 transform. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::StereoCheiralityException
 
class  gtsam::StereoCamera
 A stereo camera class, parameterize by left camera pose and stereo calibration. More...
 
struct  gtsam::traits< StereoCamera >
 
struct  gtsam::traits< const StereoCamera >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +std::ostream & gtsam::operator<< (std::ostream &os, const Similarity2 &p)
 
│ │ │ │

Detailed Description

│ │ │ │ -

A Stereo Camera based on two Simple Cameras.

│ │ │ │ -

A Rectified Stereo Camera.

│ │ │ │ -
Author
Chris Beall
│ │ │ │ +

Implementation of Similarity2 transform.

│ │ │ │ +
Author
John Lambert, Varun Agrawal
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,33 +1,23 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -StereoCamera.h File Reference │ │ │ │ │ -A Stereo Camera based on two Simple Cameras. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_S_t_e_r_e_o_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a │ │ │ │ │ -  A stereo camera class, parameterize by left camera pose and stereo │ │ │ │ │ - calibration. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_t_e_r_e_o_C_a_m_e_r_a_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _S_t_e_r_e_o_C_a_m_e_r_a_ _> │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Similarity2.cpp File Reference │ │ │ │ │ +Implementation of Similarity2 transform. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _S_i_m_i_l_a_r_i_t_y_2 &p) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A Stereo Camera based on two Simple Cameras. │ │ │ │ │ -A Rectified Stereo Camera. │ │ │ │ │ +Implementation of Similarity2 transform. │ │ │ │ │ Author │ │ │ │ │ - Chris Beall │ │ │ │ │ + John Lambert, Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_t_e_r_e_o_C_a_m_e_r_a_._h │ │ │ │ │ + * _S_i_m_i_l_a_r_i_t_y_2_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00380.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Rot3.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Bundler.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,83 +95,50 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Macros | │ │ │ │ -Typedefs | │ │ │ │ -Functions
│ │ │ │ -
Rot3.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
Cal3Bundler.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

3D rotation represented as a rotation matrix or quaternion │ │ │ │ +

Calibration used by Bundler. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::Rot3
 Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion if it is defined. More...
class  gtsam::Cal3Bundler
 Calibration used by Bundler. More...
 
struct  gtsam::Rot3::CayleyChart
struct  gtsam::traits< Cal3Bundler >
 
struct  gtsam::Rot3::ChartAtOrigin
 
struct  gtsam::traits< Rot3 >
 
struct  gtsam::traits< const Rot3 >
struct  gtsam::traits< const Cal3Bundler >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Macros

│ │ │ │ -#define ROT3_DEFAULT_COORDINATES_MODE   Rot3::CAYLEY
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Typedefs

│ │ │ │ -using gtsam::Rot3Vector = std::vector< Rot3, Eigen::aligned_allocator< Rot3 > >
 std::vector of Rot3s, mainly for wrapper
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

pair< Matrix3, Vector3 > gtsam::RQ (const Matrix3 &A, OptionalJacobian< 3, 9 > H=boost::none)
 [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx' such that A = R*Q = R*Qz'*Qy'*Qx'.
 
│ │ │ │

Detailed Description

│ │ │ │ -

3D rotation represented as a rotation matrix or quaternion

│ │ │ │ -
Author
Alireza Fathi
│ │ │ │ -
│ │ │ │ -Christian Potthast
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ -
│ │ │ │ -Luca Carlone
│ │ │ │ +

Calibration used by Bundler.

│ │ │ │ +
Date
Sep 25, 2010
│ │ │ │ +
Author
Yong Dian Jian
│ │ │ │
│ │ │ │ Varun Agrawal
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,56 +1,32 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Rot3.h File Reference │ │ │ │ │ -3D rotation represented as a rotation matrix or quaternion _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +Cal3Bundler.h File Reference │ │ │ │ │ +Calibration used by Bundler. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ - _R_o_t_3 is a 3D rotation represented as a rotation matrix if the │ │ │ │ │ -  preprocessor symbol GTSAM_USE_QUATERNIONS is not defined, or as a │ │ │ │ │ - quaternion if it is defined. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_C_a_l_3_B_u_n_d_l_e_r │ │ │ │ │ +  Calibration used by Bundler. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_R_o_t_3_:_:_C_a_y_l_e_y_C_h_a_r_t │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _C_a_l_3_B_u_n_d_l_e_r_ _> │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_R_o_t_3_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _R_o_t_3_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _R_o_t_3_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _C_a_l_3_B_u_n_d_l_e_r_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -MMaaccrrooss │ │ │ │ │ -#define  RROOTT33__DDEEFFAAUULLTT__CCOOOORRDDIINNAATTEESS__MMOODDEE   Rot3::CAYLEY │ │ │ │ │ -  │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -using  ggttssaamm::::RRoott33VVeeccttoorr = std::vector< _R_o_t_3, Eigen::aligned_allocator< _R_o_t_3 > │ │ │ │ │ - > │ │ │ │ │ -  std::vector of Rot3s, mainly for wrapper │ │ │ │ │ -  │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -pair< Matrix3, Vector3 >  _g_t_s_a_m_:_:_R_Q (const Matrix3 &A, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 3, 9 > │ │ │ │ │ - H=boost::none) │ │ │ │ │ - [RQ] receives a 3 by 3 matrix and returns an upper │ │ │ │ │ -  triangular matrix R and 3 rotation angles │ │ │ │ │ - corresponding to the rotation matrix Q=Qz'*Qy'*Qx' │ │ │ │ │ - such that A = R*Q = R*Qz'*Qy'*Qx'. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -3D rotation represented as a rotation matrix or quaternion │ │ │ │ │ +Calibration used by Bundler. │ │ │ │ │ + Date │ │ │ │ │ + Sep 25, 2010 │ │ │ │ │ Author │ │ │ │ │ - Alireza Fathi │ │ │ │ │ - Christian Potthast │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Luca Carlone │ │ │ │ │ + Yong Dian Jian │ │ │ │ │ Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _R_o_t_3_._h │ │ │ │ │ + * _C_a_l_3_B_u_n_d_l_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00380.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,8 +1,4 @@ │ │ │ │ │ var a00380 = [ │ │ │ │ │ - ["gtsam::Rot3::CayleyChart", "a03232.html", null], │ │ │ │ │ - ["gtsam::Rot3::ChartAtOrigin", "a03236.html", null], │ │ │ │ │ - ["gtsam::traits< Rot3 >", "a03240.html", null], │ │ │ │ │ - ["gtsam::traits< const Rot3 >", "a03244.html", null], │ │ │ │ │ - ["Rot3Vector", "a00380.html#a52e26554234edf7de94a5e43dd0bcbf9", null], │ │ │ │ │ - ["RQ", "a00380.html#a5172b5b6d51bd5348c7e551e1376f60a", null] │ │ │ │ │ + ["gtsam::traits< Cal3Bundler >", "a02972.html", null], │ │ │ │ │ + ["gtsam::traits< const Cal3Bundler >", "a02976.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00380_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Rot3.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Bundler.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,443 +98,162 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Rot3.h
│ │ │ │ +
Cal3Bundler.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
22// \callgraph
│ │ │ │ -
23
│ │ │ │ -
24#pragma once
│ │ │ │ -
25
│ │ │ │ -
26#include <gtsam/geometry/Unit3.h>
│ │ │ │ -
27#include <gtsam/geometry/Quaternion.h>
│ │ │ │ -
28#include <gtsam/geometry/SO3.h>
│ │ │ │ -
29#include <gtsam/base/concepts.h>
│ │ │ │ -
30#include <gtsam/config.h> // Get GTSAM_USE_QUATERNIONS macro
│ │ │ │ -
31
│ │ │ │ -
32#include <random>
│ │ │ │ -
33
│ │ │ │ -
34// You can override the default coordinate mode using this flag
│ │ │ │ -
35#ifndef ROT3_DEFAULT_COORDINATES_MODE
│ │ │ │ -
36 #ifdef GTSAM_USE_QUATERNIONS
│ │ │ │ -
37 // Exponential map is very cheap for quaternions
│ │ │ │ -
38 #define ROT3_DEFAULT_COORDINATES_MODE Rot3::EXPMAP
│ │ │ │ -
39 #else
│ │ │ │ -
40 // If user doesn't require GTSAM_ROT3_EXPMAP in cmake when building
│ │ │ │ -
41 #ifndef GTSAM_ROT3_EXPMAP
│ │ │ │ -
42 // For rotation matrices, the Cayley transform is a fast retract alternative
│ │ │ │ -
43 #define ROT3_DEFAULT_COORDINATES_MODE Rot3::CAYLEY
│ │ │ │ -
44 #else
│ │ │ │ -
45 #define ROT3_DEFAULT_COORDINATES_MODE Rot3::EXPMAP
│ │ │ │ -
46 #endif
│ │ │ │ -
47 #endif
│ │ │ │ -
48#endif
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/geometry/Cal3.h>
│ │ │ │ +
23#include <gtsam/geometry/Point2.h>
│ │ │ │ +
24
│ │ │ │ +
25namespace gtsam {
│ │ │ │ +
26
│ │ │ │ +
│ │ │ │ +
32class GTSAM_EXPORT Cal3Bundler : public Cal3 {
│ │ │ │ +
33 private:
│ │ │ │ +
34 double k1_ = 0.0f, k2_ = 0.0f;
│ │ │ │ +
35 double tol_ = 1e-5;
│ │ │ │ +
36
│ │ │ │ +
37 // NOTE: We use the base class fx to represent the common focal length.
│ │ │ │ +
38 // Also, image center parameters (u0, v0) are not optimized
│ │ │ │ +
39 // but are treated as constants.
│ │ │ │ +
40
│ │ │ │ +
41 public:
│ │ │ │ +
42 enum { dimension = 3 };
│ │ │ │ +
43
│ │ │ │ +
45 using shared_ptr = boost::shared_ptr<Cal3Bundler>;
│ │ │ │ +
46
│ │ │ │
49
│ │ │ │ -
50namespace gtsam {
│ │ │ │ -
51
│ │ │ │ -
│ │ │ │ -
58class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
│ │ │ │ -
59 private:
│ │ │ │ -
60
│ │ │ │ -
61#ifdef GTSAM_USE_QUATERNIONS
│ │ │ │ -
63 gtsam::Quaternion quaternion_;
│ │ │ │ -
64#else
│ │ │ │ -
65 SO3 rot_;
│ │ │ │ -
66#endif
│ │ │ │ +
51 Cal3Bundler() = default;
│ │ │ │ +
52
│ │ │ │ +
│ │ │ │ +
62 Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0,
│ │ │ │ +
63 double tol = 1e-5)
│ │ │ │ +
64 : Cal3(f, f, 0, u0, v0), k1_(k1), k2_(k2), tol_(tol) {}
│ │ │ │ +
│ │ │ │ +
65
│ │ │ │ +
66 ~Cal3Bundler() override {}
│ │ │ │
67
│ │ │ │ -
68 public:
│ │ │ │
71
│ │ │ │ -
73 Rot3();
│ │ │ │ -
74
│ │ │ │ -
81 Rot3(const Point3& col1, const Point3& col2, const Point3& col3);
│ │ │ │ -
82
│ │ │ │ -
84 Rot3(double R11, double R12, double R13,
│ │ │ │ -
85 double R21, double R22, double R23,
│ │ │ │ -
86 double R31, double R32, double R33);
│ │ │ │ -
87
│ │ │ │ -
95 template <typename Derived>
│ │ │ │ -
96#ifdef GTSAM_USE_QUATERNIONS
│ │ │ │ -
97 explicit Rot3(const Eigen::MatrixBase<Derived>& R) {
│ │ │ │ -
98 quaternion_ = Matrix3(R);
│ │ │ │ -
99 }
│ │ │ │ -
100#else
│ │ │ │ -
│ │ │ │ -
101 explicit Rot3(const Eigen::MatrixBase<Derived>& R) : rot_(R) {
│ │ │ │ -
102 }
│ │ │ │ -
│ │ │ │ -
103#endif
│ │ │ │ -
104
│ │ │ │ -
109#ifdef GTSAM_USE_QUATERNIONS
│ │ │ │ -
110 explicit Rot3(const Matrix3& R) : quaternion_(R) {}
│ │ │ │ -
111#else
│ │ │ │ -
112 explicit Rot3(const Matrix3& R) : rot_(R) {}
│ │ │ │ -
113#endif
│ │ │ │ -
114
│ │ │ │ -
118#ifdef GTSAM_USE_QUATERNIONS
│ │ │ │ -
119 explicit Rot3(const SO3& R) : quaternion_(R.matrix()) {}
│ │ │ │ -
120#else
│ │ │ │ -
121 explicit Rot3(const SO3& R) : rot_(R) {}
│ │ │ │ -
122#endif
│ │ │ │ -
123
│ │ │ │ -
128 Rot3(const Quaternion& q);
│ │ │ │ -
129 Rot3(double w, double x, double y, double z) : Rot3(Quaternion(w, x, y, z)) {}
│ │ │ │ -
130
│ │ │ │ -
137 static Rot3 Random(std::mt19937 & rng);
│ │ │ │ +
73 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
│ │ │ │ +
74 const Cal3Bundler& cal);
│ │ │ │ +
75
│ │ │ │ +
77 void print(const std::string& s = "") const override;
│ │ │ │ +
78
│ │ │ │ +
80 bool equals(const Cal3Bundler& K, double tol = 10e-9) const;
│ │ │ │ +
81
│ │ │ │ +
85
│ │ │ │ +
87 inline double k1() const { return k1_; }
│ │ │ │ +
88
│ │ │ │ +
90 inline double k2() const { return k2_; }
│ │ │ │ +
91
│ │ │ │ +
93 inline double px() const { return u0_; }
│ │ │ │ +
94
│ │ │ │ +
96 inline double py() const { return v0_; }
│ │ │ │ +
97
│ │ │ │ +
98 Matrix3 K() const override;
│ │ │ │ +
99 Vector4 k() const;
│ │ │ │ +
100
│ │ │ │ +
101 Vector3 vector() const;
│ │ │ │ +
102
│ │ │ │ +
103#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ +
105 inline double GTSAM_DEPRECATED u0() const { return u0_; }
│ │ │ │ +
106
│ │ │ │ +
108 inline double GTSAM_DEPRECATED v0() const { return v0_; }
│ │ │ │ +
109#endif
│ │ │ │ +
110
│ │ │ │ +
119 Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none,
│ │ │ │ +
120 OptionalJacobian<2, 2> Dp = boost::none) const;
│ │ │ │ +
121
│ │ │ │ +
130 Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = boost::none,
│ │ │ │ +
131 OptionalJacobian<2, 2> Dp = boost::none) const;
│ │ │ │ +
132
│ │ │ │ +
134 Matrix2 D2d_intrinsic(const Point2& p) const;
│ │ │ │ +
135
│ │ │ │ +
137 Matrix23 D2d_calibration(const Point2& p) const;
│ │ │ │
138
│ │ │ │ -
140 virtual ~Rot3() {}
│ │ │ │ +
140 Matrix25 D2d_intrinsic_calibration(const Point2& p) const;
│ │ │ │
141
│ │ │ │ -
142 /* Static member function to generate some well known rotations */
│ │ │ │ -
143
│ │ │ │ -
145 static Rot3 Rx(double t);
│ │ │ │ -
146
│ │ │ │ -
148 static Rot3 Ry(double t);
│ │ │ │ -
149
│ │ │ │ -
151 static Rot3 Rz(double t);
│ │ │ │ -
152
│ │ │ │ -
154 static Rot3 RzRyRx(double x, double y, double z,
│ │ │ │ -
155 OptionalJacobian<3, 1> Hx = boost::none,
│ │ │ │ -
156 OptionalJacobian<3, 1> Hy = boost::none,
│ │ │ │ -
157 OptionalJacobian<3, 1> Hz = boost::none);
│ │ │ │ -
158
│ │ │ │ -
│ │ │ │ -
160 inline static Rot3 RzRyRx(const Vector& xyz,
│ │ │ │ -
161 OptionalJacobian<3, 3> H = boost::none) {
│ │ │ │ -
162 assert(xyz.size() == 3);
│ │ │ │ -
163 Rot3 out;
│ │ │ │ -
164 if (H) {
│ │ │ │ -
165 Vector3 Hx, Hy, Hz;
│ │ │ │ -
166 out = RzRyRx(xyz(0), xyz(1), xyz(2), Hx, Hy, Hz);
│ │ │ │ -
167 (*H) << Hx, Hy, Hz;
│ │ │ │ -
168 } else
│ │ │ │ -
169 out = RzRyRx(xyz(0), xyz(1), xyz(2));
│ │ │ │ -
170 return out;
│ │ │ │ -
171 }
│ │ │ │ -
│ │ │ │ -
172
│ │ │ │ -
174 static Rot3 Yaw (double t) { return Rz(t); }
│ │ │ │ -
175
│ │ │ │ -
177 static Rot3 Pitch(double t) { return Ry(t); }
│ │ │ │ -
178
│ │ │ │ -
180 static Rot3 Roll (double t) { return Rx(t); }
│ │ │ │ -
181
│ │ │ │ -
│ │ │ │ -
196 static Rot3 Ypr(double y, double p, double r,
│ │ │ │ -
197 OptionalJacobian<3, 1> Hy = boost::none,
│ │ │ │ -
198 OptionalJacobian<3, 1> Hp = boost::none,
│ │ │ │ -
199 OptionalJacobian<3, 1> Hr = boost::none) {
│ │ │ │ -
200 return RzRyRx(r, p, y, Hr, Hp, Hy);
│ │ │ │ -
201 }
│ │ │ │ -
│ │ │ │ -
202
│ │ │ │ -
│ │ │ │ -
204 static Rot3 Quaternion(double w, double x, double y, double z) {
│ │ │ │ -
205 gtsam::Quaternion q(w, x, y, z);
│ │ │ │ -
206 return Rot3(q);
│ │ │ │ -
207 }
│ │ │ │ -
│ │ │ │ -
208
│ │ │ │ -
│ │ │ │ -
215 static Rot3 AxisAngle(const Point3& axis, double angle) {
│ │ │ │ -
216 // Convert to unit vector.
│ │ │ │ -
217 Vector3 unitAxis = Unit3(axis).unitVector();
│ │ │ │ -
218#ifdef GTSAM_USE_QUATERNIONS
│ │ │ │ -
219 return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, unitAxis));
│ │ │ │ -
220#else
│ │ │ │ -
221 return Rot3(SO3::AxisAngle(unitAxis,angle));
│ │ │ │ -
222#endif
│ │ │ │ -
223 }
│ │ │ │ -
│ │ │ │ -
224
│ │ │ │ -
│ │ │ │ -
231 static Rot3 AxisAngle(const Unit3& axis, double angle) {
│ │ │ │ -
232 return AxisAngle(axis.unitVector(),angle);
│ │ │ │ -
233 }
│ │ │ │ -
│ │ │ │ -
234
│ │ │ │ -
│ │ │ │ -
240 static Rot3 Rodrigues(const Vector3& w) {
│ │ │ │ -
241 return Rot3::Expmap(w);
│ │ │ │ -
242 }
│ │ │ │ -
│ │ │ │ -
243
│ │ │ │ -
│ │ │ │ -
251 static Rot3 Rodrigues(double wx, double wy, double wz) {
│ │ │ │ -
252 return Rodrigues(Vector3(wx, wy, wz));
│ │ │ │ -
253 }
│ │ │ │ -
│ │ │ │ -
254
│ │ │ │ -
256 static Rot3 AlignPair(const Unit3& axis, const Unit3& a_p, const Unit3& b_p);
│ │ │ │ -
257
│ │ │ │ -
259 static Rot3 AlignTwoPairs(const Unit3& a_p, const Unit3& b_p, //
│ │ │ │ -
260 const Unit3& a_q, const Unit3& b_q);
│ │ │ │ -
261
│ │ │ │ -
271 static Rot3 ClosestTo(const Matrix3& M) { return Rot3(SO3::ClosestTo(M)); }
│ │ │ │ -
272
│ │ │ │ -
283 Rot3 normalized() const;
│ │ │ │ -
284
│ │ │ │ -
288
│ │ │ │ -
290 void print(const std::string& s="") const;
│ │ │ │ -
291
│ │ │ │ -
293 bool equals(const Rot3& p, double tol = 1e-9) const;
│ │ │ │ -
294
│ │ │ │ -
298
│ │ │ │ -
│ │ │ │ -
300 inline static Rot3 Identity() {
│ │ │ │ -
301 return Rot3();
│ │ │ │ -
302 }
│ │ │ │ -
│ │ │ │ -
303
│ │ │ │ -
305 Rot3 operator*(const Rot3& R2) const;
│ │ │ │ -
306
│ │ │ │ -
│ │ │ │ -
308 Rot3 inverse() const {
│ │ │ │ -
309#ifdef GTSAM_USE_QUATERNIONS
│ │ │ │ -
310 return Rot3(quaternion_.inverse());
│ │ │ │ -
311#else
│ │ │ │ -
312 return Rot3(rot_.matrix().transpose());
│ │ │ │ -
313#endif
│ │ │ │ -
314 }
│ │ │ │ -
│ │ │ │ -
315
│ │ │ │ -
│ │ │ │ -
321 Rot3 conjugate(const Rot3& cRb) const {
│ │ │ │ -
322 // TODO: do more efficiently by using Eigen or quaternion properties
│ │ │ │ -
323 return cRb * (*this) * cRb.inverse();
│ │ │ │ -
324 }
│ │ │ │ -
│ │ │ │ -
325
│ │ │ │ -
329
│ │ │ │ -
│ │ │ │ -
339 enum CoordinatesMode {
│ │ │ │ -
340 EXPMAP,
│ │ │ │ -
341#ifndef GTSAM_USE_QUATERNIONS
│ │ │ │ -
342 CAYLEY,
│ │ │ │ -
343#endif
│ │ │ │ -
344 };
│ │ │ │ -
│ │ │ │ -
345
│ │ │ │ -
346#ifndef GTSAM_USE_QUATERNIONS
│ │ │ │ -
347
│ │ │ │ -
348 // Cayley chart around origin
│ │ │ │ -
│ │ │ │ -
349 struct CayleyChart {
│ │ │ │ -
350 static Rot3 Retract(const Vector3& v, OptionalJacobian<3, 3> H = boost::none);
│ │ │ │ -
351 static Vector3 Local(const Rot3& r, OptionalJacobian<3, 3> H = boost::none);
│ │ │ │ -
352 };
│ │ │ │ -
│ │ │ │ -
353
│ │ │ │ -
│ │ │ │ -
355 Rot3 retractCayley(const Vector& omega) const {
│ │ │ │ -
356 return compose(CayleyChart::Retract(omega));
│ │ │ │ -
357 }
│ │ │ │ -
│ │ │ │ -
358
│ │ │ │ -
│ │ │ │ -
360 Vector3 localCayley(const Rot3& other) const {
│ │ │ │ -
361 return CayleyChart::Local(between(other));
│ │ │ │ -
362 }
│ │ │ │ -
│ │ │ │ -
363
│ │ │ │ -
364#endif
│ │ │ │ -
365
│ │ │ │ -
369
│ │ │ │ -
│ │ │ │ -
374 static Rot3 Expmap(const Vector3& v, OptionalJacobian<3,3> H = boost::none) {
│ │ │ │ -
375 if(H) *H = Rot3::ExpmapDerivative(v);
│ │ │ │ -
376#ifdef GTSAM_USE_QUATERNIONS
│ │ │ │ -
377 return traits<gtsam::Quaternion>::Expmap(v);
│ │ │ │ -
378#else
│ │ │ │ -
379 return Rot3(traits<SO3>::Expmap(v));
│ │ │ │ -
380#endif
│ │ │ │ -
381 }
│ │ │ │ -
│ │ │ │ -
382
│ │ │ │ -
387 static Vector3 Logmap(const Rot3& R, OptionalJacobian<3,3> H = boost::none);
│ │ │ │ -
388
│ │ │ │ -
390 static Matrix3 ExpmapDerivative(const Vector3& x);
│ │ │ │ -
391
│ │ │ │ -
393 static Matrix3 LogmapDerivative(const Vector3& x);
│ │ │ │ -
394
│ │ │ │ -
396 Matrix3 AdjointMap() const { return matrix(); }
│ │ │ │ -
397
│ │ │ │ -
398 // Chart at origin, depends on compile-time flag ROT3_DEFAULT_COORDINATES_MODE
│ │ │ │ -
│ │ │ │ -
399 struct ChartAtOrigin {
│ │ │ │ -
400 static Rot3 Retract(const Vector3& v, ChartJacobian H = boost::none);
│ │ │ │ -
401 static Vector3 Local(const Rot3& r, ChartJacobian H = boost::none);
│ │ │ │ -
402 };
│ │ │ │ -
│ │ │ │ -
403
│ │ │ │ -
404 using LieGroup<Rot3, 3>::inverse; // version with derivative
│ │ │ │ -
405
│ │ │ │ -
409
│ │ │ │ -
413 Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
│ │ │ │ -
414 OptionalJacobian<3,3> H2 = boost::none) const;
│ │ │ │ -
415
│ │ │ │ -
417 Point3 operator*(const Point3& p) const;
│ │ │ │ -
418
│ │ │ │ -
420 Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
│ │ │ │ -
421 OptionalJacobian<3,3> H2=boost::none) const;
│ │ │ │ -
422
│ │ │ │ -
426
│ │ │ │ -
428 Unit3 rotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none,
│ │ │ │ -
429 OptionalJacobian<2,2> Hp = boost::none) const;
│ │ │ │ -
430
│ │ │ │ -
432 Unit3 unrotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none,
│ │ │ │ -
433 OptionalJacobian<2,2> Hp = boost::none) const;
│ │ │ │ -
434
│ │ │ │ -
436 Unit3 operator*(const Unit3& p) const;
│ │ │ │ -
437
│ │ │ │ -
441
│ │ │ │ -
443 Matrix3 matrix() const;
│ │ │ │ -
444
│ │ │ │ -
448 Matrix3 transpose() const;
│ │ │ │ -
449
│ │ │ │ -
451 Point3 column(int index) const;
│ │ │ │ -
452
│ │ │ │ -
453 Point3 r1() const;
│ │ │ │ -
454 Point3 r2() const;
│ │ │ │ -
455 Point3 r3() const;
│ │ │ │ -
456
│ │ │ │ -
461 Vector3 xyz(OptionalJacobian<3, 3> H = boost::none) const;
│ │ │ │ -
462
│ │ │ │ -
467 Vector3 ypr(OptionalJacobian<3, 3> H = boost::none) const;
│ │ │ │ -
468
│ │ │ │ -
473 Vector3 rpy(OptionalJacobian<3, 3> H = boost::none) const;
│ │ │ │ -
474
│ │ │ │ -
481 double roll(OptionalJacobian<1, 3> H = boost::none) const;
│ │ │ │ -
482
│ │ │ │ -
489 double pitch(OptionalJacobian<1, 3> H = boost::none) const;
│ │ │ │ -
490
│ │ │ │ -
497 double yaw(OptionalJacobian<1, 3> H = boost::none) const;
│ │ │ │ -
498
│ │ │ │ -
502
│ │ │ │ -
511 std::pair<Unit3, double> axisAngle() const;
│ │ │ │ -
512
│ │ │ │ -
516 gtsam::Quaternion toQuaternion() const;
│ │ │ │ -
517
│ │ │ │ -
518#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ -
526 Vector GTSAM_DEPRECATED quaternion() const;
│ │ │ │ -
527#endif
│ │ │ │ -
528
│ │ │ │ -
534 Rot3 slerp(double t, const Rot3& other) const;
│ │ │ │ -
535
│ │ │ │ -
537 GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p);
│ │ │ │ -
538
│ │ │ │ -
540
│ │ │ │ -
541 private:
│ │ │ │ -
543 friend class boost::serialization::access;
│ │ │ │ -
544 template <class ARCHIVE>
│ │ │ │ -
545 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ -
546#ifndef GTSAM_USE_QUATERNIONS
│ │ │ │ -
547 Matrix3& M = rot_.matrix_;
│ │ │ │ -
548 ar& boost::serialization::make_nvp("rot11", M(0, 0));
│ │ │ │ -
549 ar& boost::serialization::make_nvp("rot12", M(0, 1));
│ │ │ │ -
550 ar& boost::serialization::make_nvp("rot13", M(0, 2));
│ │ │ │ -
551 ar& boost::serialization::make_nvp("rot21", M(1, 0));
│ │ │ │ -
552 ar& boost::serialization::make_nvp("rot22", M(1, 1));
│ │ │ │ -
553 ar& boost::serialization::make_nvp("rot23", M(1, 2));
│ │ │ │ -
554 ar& boost::serialization::make_nvp("rot31", M(2, 0));
│ │ │ │ -
555 ar& boost::serialization::make_nvp("rot32", M(2, 1));
│ │ │ │ -
556 ar& boost::serialization::make_nvp("rot33", M(2, 2));
│ │ │ │ -
557#else
│ │ │ │ -
558 ar& boost::serialization::make_nvp("w", quaternion_.w());
│ │ │ │ -
559 ar& boost::serialization::make_nvp("x", quaternion_.x());
│ │ │ │ -
560 ar& boost::serialization::make_nvp("y", quaternion_.y());
│ │ │ │ -
561 ar& boost::serialization::make_nvp("z", quaternion_.z());
│ │ │ │ -
562#endif
│ │ │ │ -
563 }
│ │ │ │ -
564
│ │ │ │ -
565#ifdef GTSAM_USE_QUATERNIONS
│ │ │ │ -
566 // only align if quaternion, Matrix3 has no alignment requirements
│ │ │ │ -
567 public:
│ │ │ │ -
568 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
569#endif
│ │ │ │ -
570 };
│ │ │ │ -
│ │ │ │ -
571
│ │ │ │ -
573 using Rot3Vector = std::vector<Rot3, Eigen::aligned_allocator<Rot3> >;
│ │ │ │ -
574
│ │ │ │ -
585 GTSAM_EXPORT std::pair<Matrix3, Vector3> RQ(
│ │ │ │ -
586 const Matrix3& A, OptionalJacobian<3, 9> H = boost::none);
│ │ │ │ -
587
│ │ │ │ -
588 template<>
│ │ │ │ -
589 struct traits<Rot3> : public internal::LieGroup<Rot3> {};
│ │ │ │ -
590
│ │ │ │ -
591 template<>
│ │ │ │ -
592 struct traits<const Rot3> : public internal::LieGroup<Rot3> {};
│ │ │ │ -
593
│ │ │ │ -
594} // namespace gtsam
│ │ │ │ -
595
│ │ │ │ -
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ -
Quaternion.h
Lie Group wrapper for Eigen Quaternions.
│ │ │ │ -
SO3.h
3*3 matrix representation of SO(3)
│ │ │ │ +
145
│ │ │ │ +
147 size_t dim() const override { return Dim(); }
│ │ │ │ +
148
│ │ │ │ +
150 inline static size_t Dim() { return dimension; }
│ │ │ │ +
151
│ │ │ │ +
│ │ │ │ +
153 inline Cal3Bundler retract(const Vector& d) const {
│ │ │ │ +
154 return Cal3Bundler(fx_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_);
│ │ │ │ +
155 }
│ │ │ │ +
│ │ │ │ +
156
│ │ │ │ +
│ │ │ │ +
158 Vector3 localCoordinates(const Cal3Bundler& T2) const {
│ │ │ │ +
159 return T2.vector() - vector();
│ │ │ │ +
160 }
│ │ │ │ +
│ │ │ │ +
161
│ │ │ │ +
162 private:
│ │ │ │ +
166
│ │ │ │ +
168 friend class boost::serialization::access;
│ │ │ │ +
169 template <class Archive>
│ │ │ │ +
170 void serialize(Archive& ar, const unsigned int /*version*/) {
│ │ │ │ +
171 ar& boost::serialization::make_nvp(
│ │ │ │ +
172 "Cal3Bundler", boost::serialization::base_object<Cal3>(*this));
│ │ │ │ +
173 ar& BOOST_SERIALIZATION_NVP(k1_);
│ │ │ │ +
174 ar& BOOST_SERIALIZATION_NVP(k2_);
│ │ │ │ +
175 ar& BOOST_SERIALIZATION_NVP(tol_);
│ │ │ │ +
176 }
│ │ │ │ +
177
│ │ │ │ +
179};
│ │ │ │ +
│ │ │ │ +
180
│ │ │ │ +
181template <>
│ │ │ │ +
182struct traits<Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
│ │ │ │ +
183
│ │ │ │ +
184template <>
│ │ │ │ +
185struct traits<const Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
│ │ │ │ +
186
│ │ │ │ +
187} // namespace gtsam
│ │ │ │ +
Point2.h
2D Point
│ │ │ │ +
Cal3.h
Common code for all Calibration models.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::RQ
pair< Matrix3, Vector3 > RQ(const Matrix3 &A, OptionalJacobian< 3, 9 > H)
[RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R and 3 rotation angles correspo...
Definition Rot3.cpp:260
│ │ │ │ -
gtsam::Rot3Vector
std::vector< Rot3, Eigen::aligned_allocator< Rot3 > > Rot3Vector
std::vector of Rot3s, mainly for wrapper
Definition Rot3.h:573
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::column
const MATRIX::ConstColXpr column(const MATRIX &A, size_t j)
Extracts a column view from a matrix that avoids a copy.
Definition Matrix.h:211
│ │ │ │ -
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
│ │ │ │ -
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ +
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::LieGroup
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition Lie.h:37
│ │ │ │ -
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition Lie.h:229
│ │ │ │ -
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
│ │ │ │ -
gtsam::Rot3::Identity
static Rot3 Identity()
identity rotation for group operation
Definition Rot3.h:300
│ │ │ │ -
gtsam::Rot3::retractCayley
Rot3 retractCayley(const Vector &omega) const
Retraction from R^3 to Rot3 manifold using the Cayley transform.
Definition Rot3.h:355
│ │ │ │ -
gtsam::Rot3::AdjointMap
Matrix3 AdjointMap() const
Calculate Adjoint map.
Definition Rot3.h:396
│ │ │ │ -
gtsam::Rot3::ClosestTo
static Rot3 ClosestTo(const Matrix3 &M)
Static, named constructor that finds Rot3 element closest to M in Frobenius norm.
Definition Rot3.h:271
│ │ │ │ -
gtsam::Rot3::~Rot3
virtual ~Rot3()
Virtual destructor.
Definition Rot3.h:140
│ │ │ │ -
gtsam::Rot3::Yaw
static Rot3 Yaw(double t)
Positive yaw is to right (as in aircraft heading). See ypr.
Definition Rot3.h:174
│ │ │ │ -
gtsam::Rot3::AxisAngle
static Rot3 AxisAngle(const Unit3 &axis, double angle)
Convert from axis/angle representation.
Definition Rot3.h:231
│ │ │ │ -
gtsam::Rot3::Rot3
Rot3(const Matrix3 &R)
Constructor from a rotation matrix Overload version for Matrix3 to avoid casting in quaternion mode.
Definition Rot3.h:112
│ │ │ │ -
gtsam::Rot3::Rodrigues
static Rot3 Rodrigues(const Vector3 &w)
Rodrigues' formula to compute an incremental rotation.
Definition Rot3.h:240
│ │ │ │ -
gtsam::Rot3::AxisAngle
static Rot3 AxisAngle(const Point3 &axis, double angle)
Convert from axis/angle representation.
Definition Rot3.h:215
│ │ │ │ -
gtsam::Rot3::Pitch
static Rot3 Pitch(double t)
Positive pitch is up (increasing aircraft altitude).See ypr.
Definition Rot3.h:177
│ │ │ │ -
gtsam::Rot3::RzRyRx
static Rot3 RzRyRx(const Vector &xyz, OptionalJacobian< 3, 3 > H=boost::none)
Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix,...
Definition Rot3.h:160
│ │ │ │ -
gtsam::Rot3::localCayley
Vector3 localCayley(const Rot3 &other) const
Inverse of retractCayley.
Definition Rot3.h:360
│ │ │ │ -
gtsam::Rot3::Quaternion
static Rot3 Quaternion(double w, double x, double y, double z)
Create from Quaternion coefficients.
Definition Rot3.h:204
│ │ │ │ -
gtsam::Rot3::conjugate
Rot3 conjugate(const Rot3 &cRb) const
Conjugation: given a rotation acting in frame B, compute rotation c1Rc2 acting in a frame C.
Definition Rot3.h:321
│ │ │ │ -
gtsam::Rot3::Rodrigues
static Rot3 Rodrigues(double wx, double wy, double wz)
Rodrigues' formula to compute an incremental rotation.
Definition Rot3.h:251
│ │ │ │ -
gtsam::Rot3::inverse
Rot3 inverse() const
inverse of a rotation
Definition Rot3.h:308
│ │ │ │ -
gtsam::Rot3::Rot3
Rot3(const SO3 &R)
Constructor from an SO3 instance.
Definition Rot3.h:121
│ │ │ │ -
gtsam::Rot3::Expmap
static Rot3 Expmap(const Vector3 &v, OptionalJacobian< 3, 3 > H=boost::none)
Exponential map at identity - create a rotation from canonical coordinates using Rodrigues' formula.
Definition Rot3.h:374
│ │ │ │ -
gtsam::Rot3::Ypr
static Rot3 Ypr(double y, double p, double r, OptionalJacobian< 3, 1 > Hy=boost::none, OptionalJacobian< 3, 1 > Hp=boost::none, OptionalJacobian< 3, 1 > Hr=boost::none)
Returns rotation nRb from body to nav frame.
Definition Rot3.h:196
│ │ │ │ -
gtsam::Rot3::CoordinatesMode
CoordinatesMode
The method retract() is used to map from the tangent space back to the manifold.
Definition Rot3.h:339
│ │ │ │ -
gtsam::Rot3::CAYLEY
@ CAYLEY
Retract and localCoordinates using the Cayley transform.
Definition Rot3.h:342
│ │ │ │ -
gtsam::Rot3::EXPMAP
@ EXPMAP
Use the Lie group exponential map to retract.
Definition Rot3.h:340
│ │ │ │ -
gtsam::Rot3::Rot3
Rot3(const Eigen::MatrixBase< Derived > &R)
Constructor from a rotation matrix Version for generic matrices.
Definition Rot3.h:101
│ │ │ │ -
gtsam::Rot3::CayleyChart
Definition Rot3.h:349
│ │ │ │ -
gtsam::Rot3::ChartAtOrigin
Definition Rot3.h:399
│ │ │ │ - │ │ │ │ -
gtsam::SO::matrix_
MatrixNN matrix_
Rotation matrix.
Definition SOn.h:62
│ │ │ │ -
gtsam::SO::matrix
const MatrixNN & matrix() const
Return matrix.
Definition SOn.h:155
│ │ │ │ -
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
│ │ │ │ -
gtsam::Unit3::unitVector
Vector3 unitVector(OptionalJacobian< 3, 2 > H=boost::none) const
Return unit-norm Vector.
Definition Unit3.cpp:151
│ │ │ │ +
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
│ │ │ │ +
gtsam::Cal3
Common base class for all calibration models.
Definition Cal3.h:69
│ │ │ │ +
gtsam::Cal3Bundler
Calibration used by Bundler.
Definition Cal3Bundler.h:32
│ │ │ │ +
gtsam::Cal3Bundler::k1
double k1() const
distorsion parameter k1
Definition Cal3Bundler.h:87
│ │ │ │ +
gtsam::Cal3Bundler::k2
double k2() const
distorsion parameter k2
Definition Cal3Bundler.h:90
│ │ │ │ +
gtsam::Cal3Bundler::Cal3Bundler
Cal3Bundler()=default
Default constructor.
│ │ │ │ +
gtsam::Cal3Bundler::retract
Cal3Bundler retract(const Vector &d) const
Update calibration with tangent space delta.
Definition Cal3Bundler.h:153
│ │ │ │ +
gtsam::Cal3Bundler::py
double py() const
image center in y
Definition Cal3Bundler.h:96
│ │ │ │ +
gtsam::Cal3Bundler::Cal3Bundler
Cal3Bundler(double f, double k1, double k2, double u0=0, double v0=0, double tol=1e-5)
Constructor.
Definition Cal3Bundler.h:62
│ │ │ │ +
gtsam::Cal3Bundler::localCoordinates
Vector3 localCoordinates(const Cal3Bundler &T2) const
Calculate local coordinates to another calibration.
Definition Cal3Bundler.h:158
│ │ │ │ +
gtsam::Cal3Bundler::px
double px() const
image center in x
Definition Cal3Bundler.h:93
│ │ │ │ +
gtsam::Cal3Bundler::dim
size_t dim() const override
return DOF, dimensionality of tangent space
Definition Cal3Bundler.h:147
│ │ │ │ +
gtsam::Cal3Bundler::Dim
static size_t Dim()
return DOF, dimensionality of tangent space
Definition Cal3Bundler.h:150
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,543 +1,196 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Rot3.h │ │ │ │ │ +Cal3Bundler.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -22// \callgraph │ │ │ │ │ -23 │ │ │ │ │ -24#pragma once │ │ │ │ │ -25 │ │ │ │ │ -26#include │ │ │ │ │ -27#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_Q_u_a_t_e_r_n_i_o_n_._h> │ │ │ │ │ -28#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_S_O_3_._h> │ │ │ │ │ -29#include │ │ │ │ │ -30#include // Get GTSAM_USE_QUATERNIONS macro │ │ │ │ │ -31 │ │ │ │ │ -32#include │ │ │ │ │ -33 │ │ │ │ │ -34// You can override the default coordinate mode using this flag │ │ │ │ │ -35#ifndef ROT3_DEFAULT_COORDINATES_MODE │ │ │ │ │ -36 #ifdef GTSAM_USE_QUATERNIONS │ │ │ │ │ -37 // Exponential map is very cheap for quaternions │ │ │ │ │ -38 #define ROT3_DEFAULT_COORDINATES_MODE Rot3::EXPMAP │ │ │ │ │ -39 #else │ │ │ │ │ -40 // If user doesn't require GTSAM_ROT3_EXPMAP in cmake when building │ │ │ │ │ -41 #ifndef GTSAM_ROT3_EXPMAP │ │ │ │ │ -42 // For rotation matrices, the Cayley transform is a fast retract alternative │ │ │ │ │ -43 #define ROT3_DEFAULT_COORDINATES_MODE Rot3::CAYLEY │ │ │ │ │ -44 #else │ │ │ │ │ -45 #define ROT3_DEFAULT_COORDINATES_MODE Rot3::EXPMAP │ │ │ │ │ -46 #endif │ │ │ │ │ -47 #endif │ │ │ │ │ -48#endif │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ +24 │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ +26 │ │ │ │ │ +_3_2class GTSAM_EXPORT _C_a_l_3_B_u_n_d_l_e_r : public _C_a_l_3 { │ │ │ │ │ +33 private: │ │ │ │ │ +34 double k1_ = 0.0f, k2_ = 0.0f; │ │ │ │ │ +35 double tol_ = 1e-5; │ │ │ │ │ +36 │ │ │ │ │ +37 // NOTE: We use the base class fx to represent the common focal length. │ │ │ │ │ +38 // Also, image center parameters (u0, v0) are not optimized │ │ │ │ │ +39 // but are treated as constants. │ │ │ │ │ +40 │ │ │ │ │ +41 public: │ │ │ │ │ +42 enum { dimension = 3 }; │ │ │ │ │ +43 │ │ │ │ │ +45 using shared_ptr = boost::shared_ptr; │ │ │ │ │ +46 │ │ │ │ │ 49 │ │ │ │ │ -50namespace _g_t_s_a_m { │ │ │ │ │ -51 │ │ │ │ │ -_5_8class GTSAM_EXPORT _R_o_t_3 : public _L_i_e_G_r_o_u_p { │ │ │ │ │ -59 private: │ │ │ │ │ -60 │ │ │ │ │ -61#ifdef GTSAM_USE_QUATERNIONS │ │ │ │ │ -63 gtsam::Quaternion quaternion_; │ │ │ │ │ -64#else │ │ │ │ │ -65 _S_O_3 rot_; │ │ │ │ │ -66#endif │ │ │ │ │ +_5_1 _C_a_l_3_B_u_n_d_l_e_r() = default; │ │ │ │ │ +52 │ │ │ │ │ +_6_2 _C_a_l_3_B_u_n_d_l_e_r(double f, double k1, double k2, double u0 = 0, double v0 = 0, │ │ │ │ │ +63 double tol = 1e-5) │ │ │ │ │ +64 : _C_a_l_3(f, f, 0, u0, v0), k1_(k1), k2_(k2), tol_(tol) {} │ │ │ │ │ +65 │ │ │ │ │ +66 _~_C_a_l_3_B_u_n_d_l_e_r() override {} │ │ │ │ │ 67 │ │ │ │ │ -68 public: │ │ │ │ │ 71 │ │ │ │ │ -73 _R_o_t_3(); │ │ │ │ │ -74 │ │ │ │ │ -81 _R_o_t_3(const _P_o_i_n_t_3& col1, const _P_o_i_n_t_3& col2, const _P_o_i_n_t_3& col3); │ │ │ │ │ -82 │ │ │ │ │ -84 _R_o_t_3(double R11, double R12, double R13, │ │ │ │ │ -85 double R21, double R22, double R23, │ │ │ │ │ -86 double R31, double R32, double R33); │ │ │ │ │ -87 │ │ │ │ │ -95 template │ │ │ │ │ -96#ifdef GTSAM_USE_QUATERNIONS │ │ │ │ │ -97 explicit _R_o_t_3(const Eigen::MatrixBase& R) { │ │ │ │ │ -98 quaternion_ = Matrix3(R); │ │ │ │ │ -99 } │ │ │ │ │ -100#else │ │ │ │ │ -_1_0_1 explicit _R_o_t_3(const Eigen::MatrixBase& R) : rot_(R) { │ │ │ │ │ -102 } │ │ │ │ │ -103#endif │ │ │ │ │ -104 │ │ │ │ │ -109#ifdef GTSAM_USE_QUATERNIONS │ │ │ │ │ -110 explicit _R_o_t_3(const Matrix3& R) : quaternion_(R) {} │ │ │ │ │ -111#else │ │ │ │ │ -_1_1_2 explicit _R_o_t_3(const Matrix3& R) : rot_(R) {} │ │ │ │ │ -113#endif │ │ │ │ │ -114 │ │ │ │ │ -118#ifdef GTSAM_USE_QUATERNIONS │ │ │ │ │ -119 explicit _R_o_t_3(const _S_O_3& R) : quaternion_(R.matrix()) {} │ │ │ │ │ -120#else │ │ │ │ │ -_1_2_1 explicit _R_o_t_3(const _S_O_3& R) : rot_(R) {} │ │ │ │ │ -122#endif │ │ │ │ │ -123 │ │ │ │ │ -128 _R_o_t_3(const Quaternion& q); │ │ │ │ │ -129 _R_o_t_3(double w, double x, double y, double z) : _R_o_t_3(Quaternion(w, x, y, z)) │ │ │ │ │ -{} │ │ │ │ │ -130 │ │ │ │ │ -137 static Rot3 Random(std::mt19937 & rng); │ │ │ │ │ +73 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, │ │ │ │ │ +74 const Cal3Bundler& cal); │ │ │ │ │ +75 │ │ │ │ │ +77 void print(const std::string& s = "") const override; │ │ │ │ │ +78 │ │ │ │ │ +80 bool equals(const Cal3Bundler& K, double tol = 10e-9) const; │ │ │ │ │ +81 │ │ │ │ │ +85 │ │ │ │ │ +_8_7 inline double _k_1() const { return k1_; } │ │ │ │ │ +88 │ │ │ │ │ +_9_0 inline double _k_2() const { return k2_; } │ │ │ │ │ +91 │ │ │ │ │ +_9_3 inline double _p_x() const { return u0_; } │ │ │ │ │ +94 │ │ │ │ │ +_9_6 inline double _p_y() const { return v0_; } │ │ │ │ │ +97 │ │ │ │ │ +98 Matrix3 K() const override; │ │ │ │ │ +99 Vector4 k() const; │ │ │ │ │ +100 │ │ │ │ │ +101 Vector3 vector() const; │ │ │ │ │ +102 │ │ │ │ │ +103#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ +105 inline double GTSAM_DEPRECATED u0() const { return u0_; } │ │ │ │ │ +106 │ │ │ │ │ +108 inline double GTSAM_DEPRECATED v0() const { return v0_; } │ │ │ │ │ +109#endif │ │ │ │ │ +110 │ │ │ │ │ +119 _P_o_i_n_t_2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost:: │ │ │ │ │ +none, │ │ │ │ │ +120 OptionalJacobian<2, 2> Dp = boost::none) const; │ │ │ │ │ +121 │ │ │ │ │ +130 _P_o_i_n_t_2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = boost:: │ │ │ │ │ +none, │ │ │ │ │ +131 OptionalJacobian<2, 2> Dp = boost::none) const; │ │ │ │ │ +132 │ │ │ │ │ +134 Matrix2 D2d_intrinsic(const Point2& p) const; │ │ │ │ │ +135 │ │ │ │ │ +137 Matrix23 D2d_calibration(const Point2& p) const; │ │ │ │ │ 138 │ │ │ │ │ -_1_4_0 virtual _~_R_o_t_3() {} │ │ │ │ │ +140 Matrix25 D2d_intrinsic_calibration(const Point2& p) const; │ │ │ │ │ 141 │ │ │ │ │ -142 /* Static member function to generate some well known rotations */ │ │ │ │ │ -143 │ │ │ │ │ -145 static _R_o_t_3 Rx(double t); │ │ │ │ │ -146 │ │ │ │ │ -148 static _R_o_t_3 Ry(double t); │ │ │ │ │ -149 │ │ │ │ │ -151 static _R_o_t_3 Rz(double t); │ │ │ │ │ -152 │ │ │ │ │ -154 static _R_o_t_3 RzRyRx(double x, double y, double z, │ │ │ │ │ -155 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _1_> Hx = boost::none, │ │ │ │ │ -156 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _1_> Hy = boost::none, │ │ │ │ │ -157 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _1_> Hz = boost::none); │ │ │ │ │ -158 │ │ │ │ │ -_1_6_0 inline static _R_o_t_3 _R_z_R_y_R_x(const Vector& xyz, │ │ │ │ │ -161 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H = boost::none) { │ │ │ │ │ -162 assert(xyz.size() == 3); │ │ │ │ │ -163 _R_o_t_3 out; │ │ │ │ │ -164 if (H) { │ │ │ │ │ -165 Vector3 Hx, Hy, Hz; │ │ │ │ │ -166 out = RzRyRx(xyz(0), xyz(1), xyz(2), Hx, Hy, Hz); │ │ │ │ │ -167 (*H) << Hx, Hy, Hz; │ │ │ │ │ -168 } else │ │ │ │ │ -169 out = RzRyRx(xyz(0), xyz(1), xyz(2)); │ │ │ │ │ -170 return out; │ │ │ │ │ -171 } │ │ │ │ │ -172 │ │ │ │ │ -_1_7_4 static _R_o_t_3 _Y_a_w (double t) { return Rz(t); } │ │ │ │ │ -175 │ │ │ │ │ -_1_7_7 static _R_o_t_3 _P_i_t_c_h(double t) { return Ry(t); } │ │ │ │ │ -178 │ │ │ │ │ -180 static _R_o_t_3 Roll (double t) { return Rx(t); } │ │ │ │ │ -181 │ │ │ │ │ -_1_9_6 static _R_o_t_3 _Y_p_r(double y, double p, double r, │ │ │ │ │ -197 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _1_> Hy = boost::none, │ │ │ │ │ -198 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _1_> Hp = boost::none, │ │ │ │ │ -199 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _1_> Hr = boost::none) { │ │ │ │ │ -200 return RzRyRx(r, p, y, Hr, Hp, Hy); │ │ │ │ │ -201 } │ │ │ │ │ -202 │ │ │ │ │ -_2_0_4 static _R_o_t_3 _Q_u_a_t_e_r_n_i_o_n(double w, double x, double y, double z) { │ │ │ │ │ -205 gtsam::Quaternion q(w, x, y, z); │ │ │ │ │ -206 return _R_o_t_3(q); │ │ │ │ │ -207 } │ │ │ │ │ -208 │ │ │ │ │ -_2_1_5 static _R_o_t_3 _A_x_i_s_A_n_g_l_e(const _P_o_i_n_t_3& axis, double angle) { │ │ │ │ │ -216 // Convert to unit vector. │ │ │ │ │ -217 Vector3 unitAxis = _U_n_i_t_3(axis)._u_n_i_t_V_e_c_t_o_r(); │ │ │ │ │ -218#ifdef GTSAM_USE_QUATERNIONS │ │ │ │ │ -219 return gtsam::Quaternion(Eigen::AngleAxis(angle, unitAxis)); │ │ │ │ │ -220#else │ │ │ │ │ -221 return _R_o_t_3(SO3::AxisAngle(unitAxis,angle)); │ │ │ │ │ -222#endif │ │ │ │ │ -223 } │ │ │ │ │ -224 │ │ │ │ │ -_2_3_1 static _R_o_t_3 _A_x_i_s_A_n_g_l_e(const _U_n_i_t_3& axis, double angle) { │ │ │ │ │ -232 return AxisAngle(axis._u_n_i_t_V_e_c_t_o_r(),angle); │ │ │ │ │ -233 } │ │ │ │ │ -234 │ │ │ │ │ -_2_4_0 static _R_o_t_3 _R_o_d_r_i_g_u_e_s(const Vector3& w) { │ │ │ │ │ -241 return Rot3::Expmap(w); │ │ │ │ │ -242 } │ │ │ │ │ -243 │ │ │ │ │ -_2_5_1 static _R_o_t_3 _R_o_d_r_i_g_u_e_s(double wx, double wy, double wz) { │ │ │ │ │ -252 return Rodrigues(Vector3(wx, wy, wz)); │ │ │ │ │ -253 } │ │ │ │ │ -254 │ │ │ │ │ -256 static _R_o_t_3 AlignPair(const _U_n_i_t_3& axis, const _U_n_i_t_3& a_p, const _U_n_i_t_3& │ │ │ │ │ -b_p); │ │ │ │ │ -257 │ │ │ │ │ -259 static _R_o_t_3 AlignTwoPairs(const _U_n_i_t_3& a_p, const _U_n_i_t_3& b_p, // │ │ │ │ │ -260 const _U_n_i_t_3& a_q, const _U_n_i_t_3& b_q); │ │ │ │ │ -261 │ │ │ │ │ -_2_7_1 static _R_o_t_3 _C_l_o_s_e_s_t_T_o(const Matrix3& M) { return _R_o_t_3(SO3::ClosestTo(M)); } │ │ │ │ │ -272 │ │ │ │ │ -283 _R_o_t_3 normalized() const; │ │ │ │ │ -284 │ │ │ │ │ -288 │ │ │ │ │ -290 void _p_r_i_n_t(const std::string& s="") const; │ │ │ │ │ -291 │ │ │ │ │ -293 bool _e_q_u_a_l_s(const _R_o_t_3& p, double tol = 1e-9) const; │ │ │ │ │ -294 │ │ │ │ │ -298 │ │ │ │ │ -_3_0_0 inline static _R_o_t_3 _I_d_e_n_t_i_t_y() { │ │ │ │ │ -301 return _R_o_t_3(); │ │ │ │ │ -302 } │ │ │ │ │ -303 │ │ │ │ │ -305 _R_o_t_3 _o_p_e_r_a_t_o_r_*(const _R_o_t_3& R2) const; │ │ │ │ │ -306 │ │ │ │ │ -_3_0_8 _R_o_t_3 _i_n_v_e_r_s_e() const { │ │ │ │ │ -309#ifdef GTSAM_USE_QUATERNIONS │ │ │ │ │ -310 return _R_o_t_3(quaternion_.inverse()); │ │ │ │ │ -311#else │ │ │ │ │ -312 return _R_o_t_3(rot_._m_a_t_r_i_x().transpose()); │ │ │ │ │ -313#endif │ │ │ │ │ -314 } │ │ │ │ │ -315 │ │ │ │ │ -_3_2_1 _R_o_t_3 _c_o_n_j_u_g_a_t_e(const _R_o_t_3& cRb) const { │ │ │ │ │ -322 // TODO: do more efficiently by using Eigen or quaternion properties │ │ │ │ │ -323 return cRb * (*this) * cRb._i_n_v_e_r_s_e(); │ │ │ │ │ -324 } │ │ │ │ │ -325 │ │ │ │ │ -329 │ │ │ │ │ -_3_3_9 enum _C_o_o_r_d_i_n_a_t_e_s_M_o_d_e { │ │ │ │ │ -_3_4_0 _E_X_P_M_A_P, │ │ │ │ │ -341#ifndef GTSAM_USE_QUATERNIONS │ │ │ │ │ -_3_4_2 _C_A_Y_L_E_Y, │ │ │ │ │ -343#endif │ │ │ │ │ -344 }; │ │ │ │ │ -345 │ │ │ │ │ -346#ifndef GTSAM_USE_QUATERNIONS │ │ │ │ │ -347 │ │ │ │ │ -348 // Cayley chart around origin │ │ │ │ │ -_3_4_9 struct _C_a_y_l_e_y_C_h_a_r_t { │ │ │ │ │ -350 static _R_o_t_3 Retract(const Vector3& v, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H = boost:: │ │ │ │ │ -none); │ │ │ │ │ -351 static Vector3 Local(const _R_o_t_3& r, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H = boost:: │ │ │ │ │ -none); │ │ │ │ │ -352 }; │ │ │ │ │ -353 │ │ │ │ │ -_3_5_5 _R_o_t_3 _r_e_t_r_a_c_t_C_a_y_l_e_y(const Vector& omega) const { │ │ │ │ │ -356 return compose(CayleyChart::Retract(omega)); │ │ │ │ │ -357 } │ │ │ │ │ -358 │ │ │ │ │ -_3_6_0 Vector3 _l_o_c_a_l_C_a_y_l_e_y(const _R_o_t_3& other) const { │ │ │ │ │ -361 return CayleyChart::Local(between(other)); │ │ │ │ │ -362 } │ │ │ │ │ -363 │ │ │ │ │ -364#endif │ │ │ │ │ -365 │ │ │ │ │ -369 │ │ │ │ │ -_3_7_4 static _R_o_t_3 _E_x_p_m_a_p(const Vector3& v, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_3_> H = boost::none) │ │ │ │ │ -{ │ │ │ │ │ -375 if(H) *H = Rot3::ExpmapDerivative(v); │ │ │ │ │ -376#ifdef GTSAM_USE_QUATERNIONS │ │ │ │ │ -377 return _t_r_a_i_t_s_<_g_t_s_a_m_:_:_Q_u_a_t_e_r_n_i_o_n_>_:_:_E_x_p_m_a_p(v); │ │ │ │ │ -378#else │ │ │ │ │ -379 return _R_o_t_3(_t_r_a_i_t_s_<_S_O_3_>_:_:_E_x_p_m_a_p(v)); │ │ │ │ │ -380#endif │ │ │ │ │ -381 } │ │ │ │ │ -382 │ │ │ │ │ -387 static Vector3 Logmap(const _R_o_t_3& R, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_3_> H = boost:: │ │ │ │ │ -none); │ │ │ │ │ -388 │ │ │ │ │ -390 static Matrix3 ExpmapDerivative(const Vector3& x); │ │ │ │ │ -391 │ │ │ │ │ -393 static Matrix3 LogmapDerivative(const Vector3& x); │ │ │ │ │ -394 │ │ │ │ │ -_3_9_6 Matrix3 _A_d_j_o_i_n_t_M_a_p() const { return matrix(); } │ │ │ │ │ -397 │ │ │ │ │ -398 // Chart at origin, depends on compile-time flag │ │ │ │ │ -ROT3_DEFAULT_COORDINATES_MODE │ │ │ │ │ -_3_9_9 struct _C_h_a_r_t_A_t_O_r_i_g_i_n { │ │ │ │ │ -400 static _R_o_t_3 Retract(const Vector3& v, ChartJacobian H = boost::none); │ │ │ │ │ -401 static Vector3 Local(const _R_o_t_3& r, ChartJacobian H = boost::none); │ │ │ │ │ -402 }; │ │ │ │ │ -403 │ │ │ │ │ -404 using _L_i_e_G_r_o_u_p<_R_o_t_3, 3>::inverse; // version with derivative │ │ │ │ │ -405 │ │ │ │ │ -409 │ │ │ │ │ -413 _P_o_i_n_t_3 rotate(const _P_o_i_n_t_3& p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_3_> H1 = boost::none, │ │ │ │ │ -414 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_3_> H2 = boost::none) const; │ │ │ │ │ -415 │ │ │ │ │ -417 _P_o_i_n_t_3 _o_p_e_r_a_t_o_r_*(const _P_o_i_n_t_3& p) const; │ │ │ │ │ -418 │ │ │ │ │ -420 _P_o_i_n_t_3 unrotate(const _P_o_i_n_t_3& p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_3_> H1 = boost::none, │ │ │ │ │ -421 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_3_> H2=boost::none) const; │ │ │ │ │ -422 │ │ │ │ │ -426 │ │ │ │ │ -428 _U_n_i_t_3 rotate(const _U_n_i_t_3& p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_3_> HR = boost::none, │ │ │ │ │ -429 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_2_> Hp = boost::none) const; │ │ │ │ │ -430 │ │ │ │ │ -432 _U_n_i_t_3 unrotate(const _U_n_i_t_3& p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_3_> HR = boost::none, │ │ │ │ │ -433 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_2_> Hp = boost::none) const; │ │ │ │ │ -434 │ │ │ │ │ -436 _U_n_i_t_3 _o_p_e_r_a_t_o_r_*(const _U_n_i_t_3& p) const; │ │ │ │ │ -437 │ │ │ │ │ -441 │ │ │ │ │ -443 Matrix3 matrix() const; │ │ │ │ │ -444 │ │ │ │ │ -448 Matrix3 transpose() const; │ │ │ │ │ -449 │ │ │ │ │ -451 _P_o_i_n_t_3 _c_o_l_u_m_n(int index) const; │ │ │ │ │ -452 │ │ │ │ │ -453 _P_o_i_n_t_3 r1() const; │ │ │ │ │ -454 _P_o_i_n_t_3 r2() const; │ │ │ │ │ -455 _P_o_i_n_t_3 r3() const; │ │ │ │ │ -456 │ │ │ │ │ -461 Vector3 xyz(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H = boost::none) const; │ │ │ │ │ -462 │ │ │ │ │ -467 Vector3 ypr(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H = boost::none) const; │ │ │ │ │ -468 │ │ │ │ │ -473 Vector3 rpy(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H = boost::none) const; │ │ │ │ │ -474 │ │ │ │ │ -481 double roll(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> H = boost::none) const; │ │ │ │ │ -482 │ │ │ │ │ -489 double pitch(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> H = boost::none) const; │ │ │ │ │ -490 │ │ │ │ │ -497 double yaw(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> H = boost::none) const; │ │ │ │ │ -498 │ │ │ │ │ -502 │ │ │ │ │ -511 std::pair axisAngle() const; │ │ │ │ │ -512 │ │ │ │ │ -516 gtsam::Quaternion toQuaternion() const; │ │ │ │ │ -517 │ │ │ │ │ -518#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ -526 Vector GTSAM_DEPRECATED quaternion() const; │ │ │ │ │ -527#endif │ │ │ │ │ -528 │ │ │ │ │ -534 _R_o_t_3 slerp(double t, const _R_o_t_3& other) const; │ │ │ │ │ -535 │ │ │ │ │ -537 GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const _R_o_t_3& │ │ │ │ │ -p); │ │ │ │ │ -538 │ │ │ │ │ -540 │ │ │ │ │ -541 private: │ │ │ │ │ -_5_4_3 friend class boost::serialization::access; │ │ │ │ │ -544 template │ │ │ │ │ -545 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ -546#ifndef GTSAM_USE_QUATERNIONS │ │ │ │ │ -547 Matrix3& M = rot_._m_a_t_r_i_x__; │ │ │ │ │ -548 ar& boost::serialization::make_nvp("rot11", M(0, 0)); │ │ │ │ │ -549 ar& boost::serialization::make_nvp("rot12", M(0, 1)); │ │ │ │ │ -550 ar& boost::serialization::make_nvp("rot13", M(0, 2)); │ │ │ │ │ -551 ar& boost::serialization::make_nvp("rot21", M(1, 0)); │ │ │ │ │ -552 ar& boost::serialization::make_nvp("rot22", M(1, 1)); │ │ │ │ │ -553 ar& boost::serialization::make_nvp("rot23", M(1, 2)); │ │ │ │ │ -554 ar& boost::serialization::make_nvp("rot31", M(2, 0)); │ │ │ │ │ -555 ar& boost::serialization::make_nvp("rot32", M(2, 1)); │ │ │ │ │ -556 ar& boost::serialization::make_nvp("rot33", M(2, 2)); │ │ │ │ │ -557#else │ │ │ │ │ -558 ar& boost::serialization::make_nvp("w", quaternion_.w()); │ │ │ │ │ -559 ar& boost::serialization::make_nvp("x", quaternion_.x()); │ │ │ │ │ -560 ar& boost::serialization::make_nvp("y", quaternion_.y()); │ │ │ │ │ -561 ar& boost::serialization::make_nvp("z", quaternion_.z()); │ │ │ │ │ -562#endif │ │ │ │ │ -563 } │ │ │ │ │ -564 │ │ │ │ │ -565#ifdef GTSAM_USE_QUATERNIONS │ │ │ │ │ -566 // only align if quaternion, Matrix3 has no alignment requirements │ │ │ │ │ -567 public: │ │ │ │ │ -568 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -569#endif │ │ │ │ │ -570 }; │ │ │ │ │ -571 │ │ │ │ │ -_5_7_3 using _R_o_t_3_V_e_c_t_o_r = std::vector >; │ │ │ │ │ -574 │ │ │ │ │ -585 GTSAM_EXPORT std::pair _R_Q( │ │ │ │ │ -586 const Matrix3& A, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _9_> H = boost::none); │ │ │ │ │ -587 │ │ │ │ │ -588 template<> │ │ │ │ │ -_5_8_9 struct _t_r_a_i_t_s<_R_o_t_3> : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ -590 │ │ │ │ │ -591 template<> │ │ │ │ │ -_5_9_2 struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ -593 │ │ │ │ │ -594} // namespace gtsam │ │ │ │ │ -595 │ │ │ │ │ -_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ -This marks a GTSAM object to require alignment. │ │ │ │ │ -DDeeffiinniittiioonn types.h:308 │ │ │ │ │ -_Q_u_a_t_e_r_n_i_o_n_._h │ │ │ │ │ -Lie Group wrapper for Eigen Quaternions. │ │ │ │ │ -_S_O_3_._h │ │ │ │ │ -3*3 matrix representation of SO(3) │ │ │ │ │ +145 │ │ │ │ │ +_1_4_7 size_t _d_i_m() const override { return Dim(); } │ │ │ │ │ +148 │ │ │ │ │ +_1_5_0 inline static size_t _D_i_m() { return dimension; } │ │ │ │ │ +151 │ │ │ │ │ +_1_5_3 inline _C_a_l_3_B_u_n_d_l_e_r _r_e_t_r_a_c_t(const Vector& d) const { │ │ │ │ │ +154 return _C_a_l_3_B_u_n_d_l_e_r(fx_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_); │ │ │ │ │ +155 } │ │ │ │ │ +156 │ │ │ │ │ +_1_5_8 Vector3 _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(const _C_a_l_3_B_u_n_d_l_e_r& T2) const { │ │ │ │ │ +159 return T2.vector() - vector(); │ │ │ │ │ +160 } │ │ │ │ │ +161 │ │ │ │ │ +162 private: │ │ │ │ │ +166 │ │ │ │ │ +_1_6_8 friend class boost::serialization::access; │ │ │ │ │ +169 template │ │ │ │ │ +170 void serialize(Archive& ar, const unsigned int /*version*/) { │ │ │ │ │ +171 ar& boost::serialization::make_nvp( │ │ │ │ │ +172 "Cal3Bundler", boost::serialization::base_object(*this)); │ │ │ │ │ +173 ar& BOOST_SERIALIZATION_NVP(k1_); │ │ │ │ │ +174 ar& BOOST_SERIALIZATION_NVP(k2_); │ │ │ │ │ +175 ar& BOOST_SERIALIZATION_NVP(tol_); │ │ │ │ │ +176 } │ │ │ │ │ +177 │ │ │ │ │ +179}; │ │ │ │ │ +180 │ │ │ │ │ +181template <> │ │ │ │ │ +_1_8_2struct _t_r_a_i_t_s<_C_a_l_3_B_u_n_d_l_e_r> : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d {}; │ │ │ │ │ +183 │ │ │ │ │ +184template <> │ │ │ │ │ +_1_8_5struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ +{}; │ │ │ │ │ +186 │ │ │ │ │ +187} // namespace gtsam │ │ │ │ │ +_P_o_i_n_t_2_._h │ │ │ │ │ +2D Point │ │ │ │ │ +_C_a_l_3_._h │ │ │ │ │ +Common code for all Calibration models. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_R_Q │ │ │ │ │ -pair< Matrix3, Vector3 > RQ(const Matrix3 &A, OptionalJacobian< 3, 9 > H) │ │ │ │ │ -[RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R and 3 │ │ │ │ │ -rotation angles correspo... │ │ │ │ │ -DDeeffiinniittiioonn Rot3.cpp:260 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_V_e_c_t_o_r │ │ │ │ │ -std::vector< Rot3, Eigen::aligned_allocator< Rot3 > > Rot3Vector │ │ │ │ │ -std::vector of Rot3s, mainly for wrapper │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:573 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_c_o_l_u_m_n │ │ │ │ │ -const MATRIX::ConstColXpr column(const MATRIX &A, size_t j) │ │ │ │ │ -Extracts a column view from a matrix that avoids a copy. │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:211 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ -Point2 operator*(double s, const Point2 &p) │ │ │ │ │ -multiply with scalar │ │ │ │ │ -DDeeffiinniittiioonn Point2.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ -Vector3 Point3 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ +Vector2 Point2 │ │ │ │ │ As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point3 to Vector3... │ │ │ │ │ -DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ +typedef Point2 to Vector2... │ │ │ │ │ +DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ -A CRTP helper class that implements Lie group methods Prerequisites: methods │ │ │ │ │ -operator*,... │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ -Both LieGroupTraits and Testable. │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:229 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ -either a fixed size o... │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ -Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor │ │ │ │ │ -symbol GTSAM_USE_QUATERNIO... │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_I_d_e_n_t_i_t_y │ │ │ │ │ -static Rot3 Identity() │ │ │ │ │ -identity rotation for group operation │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:300 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_r_e_t_r_a_c_t_C_a_y_l_e_y │ │ │ │ │ -Rot3 retractCayley(const Vector &omega) const │ │ │ │ │ -Retraction from R^3 to Rot3 manifold using the Cayley transform. │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:355 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_A_d_j_o_i_n_t_M_a_p │ │ │ │ │ -Matrix3 AdjointMap() const │ │ │ │ │ -Calculate Adjoint map. │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:396 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_C_l_o_s_e_s_t_T_o │ │ │ │ │ -static Rot3 ClosestTo(const Matrix3 &M) │ │ │ │ │ -Static, named constructor that finds Rot3 element closest to M in Frobenius │ │ │ │ │ -norm. │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:271 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_~_R_o_t_3 │ │ │ │ │ -virtual ~Rot3() │ │ │ │ │ -Virtual destructor. │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:140 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_Y_a_w │ │ │ │ │ -static Rot3 Yaw(double t) │ │ │ │ │ -Positive yaw is to right (as in aircraft heading). See ypr. │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:174 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_A_x_i_s_A_n_g_l_e │ │ │ │ │ -static Rot3 AxisAngle(const Unit3 &axis, double angle) │ │ │ │ │ -Convert from axis/angle representation. │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:231 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_R_o_t_3 │ │ │ │ │ -Rot3(const Matrix3 &R) │ │ │ │ │ -Constructor from a rotation matrix Overload version for Matrix3 to avoid │ │ │ │ │ -casting in quaternion mode. │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:112 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_R_o_d_r_i_g_u_e_s │ │ │ │ │ -static Rot3 Rodrigues(const Vector3 &w) │ │ │ │ │ -Rodrigues' formula to compute an incremental rotation. │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:240 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_A_x_i_s_A_n_g_l_e │ │ │ │ │ -static Rot3 AxisAngle(const Point3 &axis, double angle) │ │ │ │ │ -Convert from axis/angle representation. │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:215 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_P_i_t_c_h │ │ │ │ │ -static Rot3 Pitch(double t) │ │ │ │ │ -Positive pitch is up (increasing aircraft altitude).See ypr. │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:177 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_R_z_R_y_R_x │ │ │ │ │ -static Rot3 RzRyRx(const Vector &xyz, OptionalJacobian< 3, 3 > H=boost::none) │ │ │ │ │ -Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/ │ │ │ │ │ -Rotation_matrix,... │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:160 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_l_o_c_a_l_C_a_y_l_e_y │ │ │ │ │ -Vector3 localCayley(const Rot3 &other) const │ │ │ │ │ -Inverse of retractCayley. │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:360 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_Q_u_a_t_e_r_n_i_o_n │ │ │ │ │ -static Rot3 Quaternion(double w, double x, double y, double z) │ │ │ │ │ -Create from Quaternion coefficients. │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:204 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_c_o_n_j_u_g_a_t_e │ │ │ │ │ -Rot3 conjugate(const Rot3 &cRb) const │ │ │ │ │ -Conjugation: given a rotation acting in frame B, compute rotation c1Rc2 acting │ │ │ │ │ -in a frame C. │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:321 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_R_o_d_r_i_g_u_e_s │ │ │ │ │ -static Rot3 Rodrigues(double wx, double wy, double wz) │ │ │ │ │ -Rodrigues' formula to compute an incremental rotation. │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:251 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_i_n_v_e_r_s_e │ │ │ │ │ -Rot3 inverse() const │ │ │ │ │ -inverse of a rotation │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:308 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_R_o_t_3 │ │ │ │ │ -Rot3(const SO3 &R) │ │ │ │ │ -Constructor from an SO3 instance. │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:121 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_E_x_p_m_a_p │ │ │ │ │ -static Rot3 Expmap(const Vector3 &v, OptionalJacobian< 3, 3 > H=boost::none) │ │ │ │ │ -Exponential map at identity - create a rotation from canonical coordinates │ │ │ │ │ -using Rodrigues' formula. │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:374 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_Y_p_r │ │ │ │ │ -static Rot3 Ypr(double y, double p, double r, OptionalJacobian< 3, 1 > │ │ │ │ │ -Hy=boost::none, OptionalJacobian< 3, 1 > Hp=boost::none, OptionalJacobian< 3, 1 │ │ │ │ │ -> Hr=boost::none) │ │ │ │ │ -Returns rotation nRb from body to nav frame. │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:196 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_C_o_o_r_d_i_n_a_t_e_s_M_o_d_e │ │ │ │ │ -CoordinatesMode │ │ │ │ │ -The method retract() is used to map from the tangent space back to the │ │ │ │ │ -manifold. │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:339 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_C_A_Y_L_E_Y │ │ │ │ │ -@ CAYLEY │ │ │ │ │ -Retract and localCoordinates using the Cayley transform. │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:342 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_E_X_P_M_A_P │ │ │ │ │ -@ EXPMAP │ │ │ │ │ -Use the Lie group exponential map to retract. │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:340 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_R_o_t_3 │ │ │ │ │ -Rot3(const Eigen::MatrixBase< Derived > &R) │ │ │ │ │ -Constructor from a rotation matrix Version for generic matrices. │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:101 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_C_a_y_l_e_y_C_h_a_r_t │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:349 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:399 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_<_ _3_ _> │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_m_a_t_r_i_x__ │ │ │ │ │ -MatrixNN matrix_ │ │ │ │ │ -Rotation matrix. │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:62 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_m_a_t_r_i_x │ │ │ │ │ -const MatrixNN & matrix() const │ │ │ │ │ -Return matrix. │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:155 │ │ │ │ │ -_g_t_s_a_m_:_:_U_n_i_t_3 │ │ │ │ │ -Represents a 3D point on a unit sphere. │ │ │ │ │ -DDeeffiinniittiioonn Unit3.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_U_n_i_t_3_:_:_u_n_i_t_V_e_c_t_o_r │ │ │ │ │ -Vector3 unitVector(OptionalJacobian< 3, 2 > H=boost::none) const │ │ │ │ │ -Return unit-norm Vector. │ │ │ │ │ -DDeeffiinniittiioonn Unit3.cpp:151 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ +Both ManifoldTraits and Testable. │ │ │ │ │ +DDeeffiinniittiioonn Manifold.h:120 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3 │ │ │ │ │ +Common base class for all calibration models. │ │ │ │ │ +DDeeffiinniittiioonn Cal3.h:69 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_B_u_n_d_l_e_r │ │ │ │ │ +Calibration used by Bundler. │ │ │ │ │ +DDeeffiinniittiioonn Cal3Bundler.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_B_u_n_d_l_e_r_:_:_k_1 │ │ │ │ │ +double k1() const │ │ │ │ │ +distorsion parameter k1 │ │ │ │ │ +DDeeffiinniittiioonn Cal3Bundler.h:87 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_B_u_n_d_l_e_r_:_:_k_2 │ │ │ │ │ +double k2() const │ │ │ │ │ +distorsion parameter k2 │ │ │ │ │ +DDeeffiinniittiioonn Cal3Bundler.h:90 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_B_u_n_d_l_e_r_:_:_C_a_l_3_B_u_n_d_l_e_r │ │ │ │ │ +Cal3Bundler()=default │ │ │ │ │ +Default constructor. │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_B_u_n_d_l_e_r_:_:_r_e_t_r_a_c_t │ │ │ │ │ +Cal3Bundler retract(const Vector &d) const │ │ │ │ │ +Update calibration with tangent space delta. │ │ │ │ │ +DDeeffiinniittiioonn Cal3Bundler.h:153 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_B_u_n_d_l_e_r_:_:_p_y │ │ │ │ │ +double py() const │ │ │ │ │ +image center in y │ │ │ │ │ +DDeeffiinniittiioonn Cal3Bundler.h:96 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_B_u_n_d_l_e_r_:_:_C_a_l_3_B_u_n_d_l_e_r │ │ │ │ │ +Cal3Bundler(double f, double k1, double k2, double u0=0, double v0=0, double │ │ │ │ │ +tol=1e-5) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn Cal3Bundler.h:62 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_B_u_n_d_l_e_r_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ +Vector3 localCoordinates(const Cal3Bundler &T2) const │ │ │ │ │ +Calculate local coordinates to another calibration. │ │ │ │ │ +DDeeffiinniittiioonn Cal3Bundler.h:158 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_B_u_n_d_l_e_r_:_:_p_x │ │ │ │ │ +double px() const │ │ │ │ │ +image center in x │ │ │ │ │ +DDeeffiinniittiioonn Cal3Bundler.h:93 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_B_u_n_d_l_e_r_:_:_d_i_m │ │ │ │ │ +size_t dim() const override │ │ │ │ │ +return DOF, dimensionality of tangent space │ │ │ │ │ +DDeeffiinniittiioonn Cal3Bundler.h:147 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_B_u_n_d_l_e_r_:_:_D_i_m │ │ │ │ │ +static size_t Dim() │ │ │ │ │ +return DOF, dimensionality of tangent space │ │ │ │ │ +DDeeffiinniittiioonn Cal3Bundler.h:150 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _R_o_t_3_._h │ │ │ │ │ + * _C_a_l_3_B_u_n_d_l_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00383.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/PinholeSet.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Point3.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,48 +94,64 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
PinholeSet.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
Point3.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

A CameraSet of either CalibratedCamera, PinholePose, or PinholeCamera. │ │ │ │ +

3D Point │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::PinholeSet< CAMERA >
 PinholeSet: triangulates point and keeps an estimate of it around. More...
 
struct  gtsam::traits< PinholeSet< CAMERA > >
 
struct  gtsam::traits< const PinholeSet< CAMERA > >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +double gtsam::distance3 (const Point3 &p1, const Point3 &q, OptionalJacobian< 1, 3 > H1=boost::none, OptionalJacobian< 1, 3 > H2=boost::none)
 distance between two points
 
│ │ │ │ +double gtsam::norm3 (const Point3 &p, OptionalJacobian< 1, 3 > H=boost::none)
 Distance of the point from the origin, with Jacobian.
 
│ │ │ │ +Point3 gtsam::normalize (const Point3 &p, OptionalJacobian< 3, 3 > H=boost::none)
 normalize, with optional Jacobian
 
Point3 gtsam::cross (const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H_p=boost::none, OptionalJacobian< 3, 3 > H_q=boost::none)
 cross product
 
│ │ │ │ +double gtsam::dot (const Point3 &p, const Point3 &q, OptionalJacobian< 1, 3 > H_p=boost::none, OptionalJacobian< 1, 3 > H_q=boost::none)
 dot product
 
│ │ │ │ +Point3Pair gtsam::means (const std::vector< Point3Pair > &abPointPairs)
 Calculate the two means of a set of Point3 pairs.
 
│ │ │ │ +ostream & gtsam::operator<< (ostream &os, const gtsam::Point3Pair &p)
 
│ │ │ │

Detailed Description

│ │ │ │ -

A CameraSet of either CalibratedCamera, PinholePose, or PinholeCamera.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +

3D Point

│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,30 +1,45 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -PinholeSet.h File Reference │ │ │ │ │ -A CameraSet of either CalibratedCamera, PinholePose, or PinholeCamera. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_P_i_n_h_o_l_e_S_e_t_<_ _C_A_M_E_R_A_ _> │ │ │ │ │ -  _P_i_n_h_o_l_e_S_e_t: triangulates point and keeps an estimate of it around. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _P_i_n_h_o_l_e_S_e_t_<_ _C_A_M_E_R_A_ _>_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _P_i_n_h_o_l_e_S_e_t_<_ _C_A_M_E_R_A_ _>_ _> │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Point3.cpp File Reference │ │ │ │ │ +3D Point _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ + double  ggttssaamm::::ddiissttaannccee33 (const _P_o_i_n_t_3 &p1, const _P_o_i_n_t_3 &q, │ │ │ │ │ + _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 1, 3 > H1=boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 1, 3 > │ │ │ │ │ + H2=boost::none) │ │ │ │ │ +  distance between two points │ │ │ │ │ +  │ │ │ │ │ + double  ggttssaamm::::nnoorrmm33 (const _P_o_i_n_t_3 &p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 1, 3 > H=boost:: │ │ │ │ │ + none) │ │ │ │ │ +  Distance of the point from the origin, with Jacobian. │ │ │ │ │ +  │ │ │ │ │ + _P_o_i_n_t_3  ggttssaamm::::nnoorrmmaalliizzee (const _P_o_i_n_t_3 &p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 3, 3 > │ │ │ │ │ + H=boost::none) │ │ │ │ │ +  normalize, with optional Jacobian │ │ │ │ │ +  │ │ │ │ │ + _P_o_i_n_t_3  _g_t_s_a_m_:_:_c_r_o_s_s (const _P_o_i_n_t_3 &p, const _P_o_i_n_t_3 &q, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< │ │ │ │ │ + 3, 3 > H_p=boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 3, 3 > H_q=boost::none) │ │ │ │ │ +  cross product │ │ │ │ │ +  │ │ │ │ │ + double  ggttssaamm::::ddoott (const _P_o_i_n_t_3 &p, const _P_o_i_n_t_3 &q, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 1, │ │ │ │ │ + 3 > H_p=boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 1, 3 > H_q=boost::none) │ │ │ │ │ +  dot product │ │ │ │ │ +  │ │ │ │ │ +Point3Pair  ggttssaamm::::mmeeaannss (const std::vector< Point3Pair > &abPointPairs) │ │ │ │ │ +  Calculate the two means of a set of Point3 pairs. │ │ │ │ │ +  │ │ │ │ │ + ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const gtsam::Point3Pair &p) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A CameraSet of either CalibratedCamera, PinholePose, or PinholeCamera. │ │ │ │ │ - Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ +3D Point │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _P_i_n_h_o_l_e_S_e_t_._h │ │ │ │ │ + * _P_o_i_n_t_3_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00383.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,5 +1,8 @@ │ │ │ │ │ var a00383 = [ │ │ │ │ │ - ["gtsam::PinholeSet< CAMERA >", "a03136.html", "a03136"], │ │ │ │ │ - ["gtsam::traits< PinholeSet< CAMERA > >", "a03140.html", null], │ │ │ │ │ - ["gtsam::traits< const PinholeSet< CAMERA > >", "a03144.html", null] │ │ │ │ │ + ["cross", "a00383.html#a86bf6ed2ffb0ecc42a7deb8922f4ca4f", null], │ │ │ │ │ + ["distance3", "a00383.html#aa42f7ec660b7353de39f9c86a26711cc", null], │ │ │ │ │ + ["dot", "a00383.html#a7dc820e4e47218768f104b43a184a1bd", null], │ │ │ │ │ + ["means", "a00383.html#a57f778b84bc6106471006421d289b832", null], │ │ │ │ │ + ["norm3", "a00383.html#ac37b6f807985ffd25217e33f6136fe58", null], │ │ │ │ │ + ["normalize", "a00383.html#abb52bb00c68909fc1147e5d112e8c2ae", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00386.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SO4.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SOn-inl.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,48 +94,38 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
SO4.cpp File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
SOn-inl.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

4*4 matrix representation of SO(4) │ │ │ │ +

Template implementations for SO(n) │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

GTSAM_EXPORT Matrix3 gtsam::topLeft (const SO4 &Q, OptionalJacobian< 9, 6 > H=boost::none)
 Project to top-left 3*3 matrix.
 
│ │ │ │ -GTSAM_EXPORT Matrix43 gtsam::stiefel (const SO4 &Q, OptionalJacobian< 12, 6 > H=boost::none)
 Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q) -> \( S \in St(3,4) \).
 
│ │ │ │

Detailed Description

│ │ │ │ -

4*4 matrix representation of SO(4)

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ -Luca Carlone
│ │ │ │ +

Template implementations for SO(n)

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
Date
March 2019
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,31 +1,23 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -SO4.cpp File Reference │ │ │ │ │ -4*4 matrix representation of SO(4) _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +SOn-inl.h File Reference │ │ │ │ │ +Template implementations for SO(n) _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ - GTSAM_EXPORT Matrix3  _g_t_s_a_m_:_:_t_o_p_L_e_f_t (const _S_O_4 &Q, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 9, 6 > │ │ │ │ │ - H=boost::none) │ │ │ │ │ -  Project to top-left 3*3 matrix. │ │ │ │ │ -  │ │ │ │ │ -GTSAM_EXPORT Matrix43  ggttssaamm::::ssttiieeffeell (const _S_O_4 &Q, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 12, 6 > │ │ │ │ │ - H=boost::none) │ │ │ │ │ -  Project to Stiefel manifold of 4*3 orthonormal 3-frames │ │ │ │ │ - in R^4, i.e., pi(Q) -> \( S \in St(3,4) \). │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -4*4 matrix representation of SO(4) │ │ │ │ │ +Template implementations for SO(n) │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Luca Carlone │ │ │ │ │ + Date │ │ │ │ │ + March 2019 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_O_4_._c_p_p │ │ │ │ │ + * _S_O_n_-_i_n_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00398.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Pose2.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3_S2.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,74 +95,47 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
│ │ │ │ -
Pose2.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
Cal3_S2.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

2D Pose │ │ │ │ +

The most common 5DOF 3D->2D calibration. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::Pose2
 A 2D pose (Point2,Rot2) More...
class  gtsam::Cal3_S2
 The most common 5DOF 3D->2D calibration. More...
 
struct  gtsam::Pose2::ChartAtOrigin
struct  gtsam::traits< Cal3_S2 >
 
struct  gtsam::traits< Pose2 >
 
struct  gtsam::traits< const Pose2 >
 
struct  gtsam::Bearing< Pose2, T >
 
struct  gtsam::Range< Pose2, T >
struct  gtsam::traits< const Cal3_S2 >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Typedefs

│ │ │ │ -using gtsam::Pose2Pair = std::pair< Pose2, Pose2 >
 
│ │ │ │ -using gtsam::Pose2Pairs = std::vector< Pose2Pair >
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -template<>
Matrix gtsam::wedge< Pose2 > (const Vector &xi)
 specialization for pose2 wedge function (generic template in Lie.h)
 
│ │ │ │

Detailed Description

│ │ │ │ -

2D Pose

│ │ │ │ -
Author
: Frank Dellaert
│ │ │ │ -
│ │ │ │ -: Richard Roberts
│ │ │ │ +

The most common 5DOF 3D->2D calibration.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,46 +1,29 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Pose2.h File Reference │ │ │ │ │ -2D Pose _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +Cal3_S2.h File Reference │ │ │ │ │ +The most common 5DOF 3D->2D calibration. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_P_o_s_e_2 │ │ │ │ │ -  A 2D pose (Point2,_R_o_t_2) _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_C_a_l_3___S_2 │ │ │ │ │ +  The most common 5DOF 3D->2D calibration. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_P_o_s_e_2_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _C_a_l_3___S_2_ _> │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _P_o_s_e_2_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _P_o_s_e_2_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_B_e_a_r_i_n_g_<_ _P_o_s_e_2_,_ _T_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_R_a_n_g_e_<_ _P_o_s_e_2_,_ _T_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _C_a_l_3___S_2_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -using  ggttssaamm::::PPoossee22PPaaiirr = std::pair< _P_o_s_e_2, _P_o_s_e_2 > │ │ │ │ │ -  │ │ │ │ │ -using  ggttssaamm::::PPoossee22PPaaiirrss = std::vector< Pose2Pair > │ │ │ │ │ -  │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -template<> │ │ │ │ │ - Matrix  ggttssaamm::::wweeddggee<< PPoossee22 >> (const Vector &xi) │ │ │ │ │ -  specialization for pose2 wedge function (generic template in _L_i_e_._h) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -2D Pose │ │ │ │ │ +The most common 5DOF 3D->2D calibration. │ │ │ │ │ Author │ │ │ │ │ - : Frank Dellaert │ │ │ │ │ - : Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _P_o_s_e_2_._h │ │ │ │ │ + * _C_a_l_3___S_2_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00398.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,8 +1,4 @@ │ │ │ │ │ var a00398 = [ │ │ │ │ │ - ["gtsam::Pose2::ChartAtOrigin", "a03160.html", null], │ │ │ │ │ - ["gtsam::traits< Pose2 >", "a03164.html", null], │ │ │ │ │ - ["gtsam::traits< const Pose2 >", "a03168.html", null], │ │ │ │ │ - ["gtsam::Bearing< Pose2, T >", "a03172.html", null], │ │ │ │ │ - ["gtsam::Range< Pose2, T >", "a03176.html", null], │ │ │ │ │ - ["wedge< Pose2 >", "a00398.html#a4b78f4db0e05f3ca838ee4da6187710e", null] │ │ │ │ │ + ["gtsam::traits< Cal3_S2 >", "a02948.html", null], │ │ │ │ │ + ["gtsam::traits< const Cal3_S2 >", "a02952.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00398_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Pose2.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3_S2.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,337 +98,142 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Pose2.h
│ │ │ │ +
Cal3_S2.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19// \callgraph
│ │ │ │ -
20
│ │ │ │ -
21#pragma once
│ │ │ │ -
22
│ │ │ │ -
23#include <gtsam/geometry/BearingRange.h>
│ │ │ │ -
24#include <gtsam/geometry/Point2.h>
│ │ │ │ -
25#include <gtsam/geometry/Rot2.h>
│ │ │ │ -
26#include <gtsam/base/Lie.h>
│ │ │ │ -
27#include <gtsam/dllexport.h>
│ │ │ │ +
22#pragma once
│ │ │ │ +
23
│ │ │ │ +
24#include <gtsam/geometry/Cal3.h>
│ │ │ │ +
25#include <gtsam/geometry/Point2.h>
│ │ │ │ +
26
│ │ │ │ +
27namespace gtsam {
│ │ │ │
28
│ │ │ │ -
29namespace gtsam {
│ │ │ │ -
30
│ │ │ │ -
│ │ │ │ -
36class Pose2: public LieGroup<Pose2, 3> {
│ │ │ │ +
│ │ │ │ +
34class GTSAM_EXPORT Cal3_S2 : public Cal3 {
│ │ │ │ +
35 public:
│ │ │ │ +
36 enum { dimension = 5 };
│ │ │ │
37
│ │ │ │ -
38public:
│ │ │ │ -
39
│ │ │ │ -
41 typedef Rot2 Rotation;
│ │ │ │ -
42 typedef Point2 Translation;
│ │ │ │ +
39 using shared_ptr = boost::shared_ptr<Cal3_S2>;
│ │ │ │ +
40
│ │ │ │
43
│ │ │ │ -
44private:
│ │ │ │ -
45
│ │ │ │ -
46 Rot2 r_;
│ │ │ │ -
47 Point2 t_;
│ │ │ │ -
48
│ │ │ │ -
49public:
│ │ │ │ +
45 Cal3_S2() = default;
│ │ │ │ +
46
│ │ │ │ +
│ │ │ │ +
48 Cal3_S2(double fx, double fy, double s, double u0, double v0)
│ │ │ │ +
49 : Cal3(fx, fy, s, u0, v0) {}
│ │ │ │ +
│ │ │ │
50
│ │ │ │ +
52 Cal3_S2(const Vector5& d) : Cal3(d) {}
│ │ │ │
53
│ │ │ │ -
│ │ │ │ -
55 Pose2() :
│ │ │ │ -
56 r_(traits<Rot2>::Identity()), t_(traits<Point2>::Identity()) {
│ │ │ │ -
57 }
│ │ │ │ -
│ │ │ │ -
58
│ │ │ │ -
60 Pose2(const Pose2& pose) : r_(pose.r_), t_(pose.t_) {}
│ │ │ │ +
60 Cal3_S2(double fov, int w, int h) : Cal3(fov, w, h) {}
│ │ │ │
61
│ │ │ │ -
│ │ │ │ -
68 Pose2(double x, double y, double theta) :
│ │ │ │ -
69 r_(Rot2::fromAngle(theta)), t_(x, y) {
│ │ │ │ -
70 }
│ │ │ │ -
│ │ │ │ +
69 Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = boost::none,
│ │ │ │ +
70 OptionalJacobian<2, 2> Dp = boost::none) const;
│ │ │ │
71
│ │ │ │ -
│ │ │ │ -
73 Pose2(double theta, const Point2& t) :
│ │ │ │ -
74 r_(Rot2::fromAngle(theta)), t_(t) {
│ │ │ │ -
75 }
│ │ │ │ -
│ │ │ │ -
76
│ │ │ │ -
78 Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {}
│ │ │ │ -
79
│ │ │ │ -
│ │ │ │ -
81 Pose2(const Matrix &T) :
│ │ │ │ -
82 r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) {
│ │ │ │ -
83 assert(T.rows() == 3 && T.cols() == 3);
│ │ │ │ -
84 }
│ │ │ │ -
│ │ │ │ -
85
│ │ │ │ -
89
│ │ │ │ -
│ │ │ │ -
91 Pose2(const Vector& v) : Pose2() {
│ │ │ │ -
92 *this = Expmap(v);
│ │ │ │ -
93 }
│ │ │ │ -
│ │ │ │ -
94
│ │ │ │ -
102 static boost::optional<Pose2> Align(const Point2Pairs& abPointPairs);
│ │ │ │ -
103
│ │ │ │ -
104 // Version of Pose2::Align that takes 2 matrices.
│ │ │ │ -
105 static boost::optional<Pose2> Align(const Matrix& a, const Matrix& b);
│ │ │ │ -
106
│ │ │ │ -
110
│ │ │ │ -
112 GTSAM_EXPORT void print(const std::string& s = "") const;
│ │ │ │ -
113
│ │ │ │ -
115 GTSAM_EXPORT bool equals(const Pose2& pose, double tol = 1e-9) const;
│ │ │ │ +
79 Point2 calibrate(const Point2& p, OptionalJacobian<2, 5> Dcal = boost::none,
│ │ │ │ +
80 OptionalJacobian<2, 2> Dp = boost::none) const;
│ │ │ │ +
81
│ │ │ │ +
87 Vector3 calibrate(const Vector3& p) const;
│ │ │ │ +
88
│ │ │ │ +
92
│ │ │ │ +
94 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
│ │ │ │ +
95 const Cal3_S2& cal);
│ │ │ │ +
96
│ │ │ │ +
98 void print(const std::string& s = "Cal3_S2") const override;
│ │ │ │ +
99
│ │ │ │ +
101 bool equals(const Cal3_S2& K, double tol = 10e-9) const;
│ │ │ │ +
102
│ │ │ │ +
│ │ │ │ +
104 inline Cal3_S2 between(const Cal3_S2& q,
│ │ │ │ +
105 OptionalJacobian<5, 5> H1 = boost::none,
│ │ │ │ +
106 OptionalJacobian<5, 5> H2 = boost::none) const {
│ │ │ │ +
107 if (H1) *H1 = -I_5x5;
│ │ │ │ +
108 if (H2) *H2 = I_5x5;
│ │ │ │ +
109 return Cal3_S2(q.fx_ - fx_, q.fy_ - fy_, q.s_ - s_, q.u0_ - u0_,
│ │ │ │ +
110 q.v0_ - v0_);
│ │ │ │ +
111 }
│ │ │ │ +
│ │ │ │ +
112
│ │ │ │
116
│ │ │ │ -
120
│ │ │ │ -
122 inline static Pose2 Identity() { return Pose2(); }
│ │ │ │ -
123
│ │ │ │ -
125 GTSAM_EXPORT Pose2 inverse() const;
│ │ │ │ -
126
│ │ │ │ -
│ │ │ │ -
128 inline Pose2 operator*(const Pose2& p2) const {
│ │ │ │ -
129 return Pose2(r_*p2.r(), t_ + r_*p2.t());
│ │ │ │ -
130 }
│ │ │ │ -
│ │ │ │ -
131
│ │ │ │ -
135
│ │ │ │ -
137 GTSAM_EXPORT static Pose2 Expmap(const Vector3& xi, ChartJacobian H = boost::none);
│ │ │ │ -
138
│ │ │ │ -
140 GTSAM_EXPORT static Vector3 Logmap(const Pose2& p, ChartJacobian H = boost::none);
│ │ │ │ -
141
│ │ │ │ -
146 GTSAM_EXPORT Matrix3 AdjointMap() const;
│ │ │ │ -
147
│ │ │ │ -
│ │ │ │ -
149 inline Vector3 Adjoint(const Vector3& xi) const {
│ │ │ │ -
150 return AdjointMap()*xi;
│ │ │ │ -
151 }
│ │ │ │ -
│ │ │ │ -
152
│ │ │ │ -
156 GTSAM_EXPORT static Matrix3 adjointMap(const Vector3& v);
│ │ │ │ -
157
│ │ │ │ -
│ │ │ │ -
161 static Vector3 adjoint(const Vector3& xi, const Vector3& y) {
│ │ │ │ -
162 return adjointMap(xi) * y;
│ │ │ │ -
163 }
│ │ │ │ -
│ │ │ │ -
164
│ │ │ │ -
│ │ │ │ -
168 static Vector3 adjointTranspose(const Vector3& xi, const Vector3& y) {
│ │ │ │ -
169 return adjointMap(xi).transpose() * y;
│ │ │ │ -
170 }
│ │ │ │ -
│ │ │ │ -
171
│ │ │ │ -
172 // temporary fix for wrappers until case issue is resolved
│ │ │ │ -
173 static Matrix3 adjointMap_(const Vector3 &xi) { return adjointMap(xi);}
│ │ │ │ -
174 static Vector3 adjoint_(const Vector3 &xi, const Vector3 &y) { return adjoint(xi, y);}
│ │ │ │ -
175
│ │ │ │ -
│ │ │ │ -
183 static inline Matrix3 wedge(double vx, double vy, double w) {
│ │ │ │ -
184 Matrix3 m;
│ │ │ │ -
185 m << 0.,-w, vx,
│ │ │ │ -
186 w, 0., vy,
│ │ │ │ -
187 0., 0., 0.;
│ │ │ │ -
188 return m;
│ │ │ │ -
189 }
│ │ │ │ -
│ │ │ │ -
190
│ │ │ │ -
192 GTSAM_EXPORT static Matrix3 ExpmapDerivative(const Vector3& v);
│ │ │ │ -
193
│ │ │ │ -
195 GTSAM_EXPORT static Matrix3 LogmapDerivative(const Pose2& v);
│ │ │ │ -
196
│ │ │ │ -
197 // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP
│ │ │ │ -
│ │ │ │ -
198 struct ChartAtOrigin {
│ │ │ │ -
199 GTSAM_EXPORT static Pose2 Retract(const Vector3& v, ChartJacobian H = boost::none);
│ │ │ │ -
200 GTSAM_EXPORT static Vector3 Local(const Pose2& r, ChartJacobian H = boost::none);
│ │ │ │ -
201 };
│ │ │ │ -
│ │ │ │ -
202
│ │ │ │ -
203 using LieGroup<Pose2, 3>::inverse; // version with derivative
│ │ │ │ -
204
│ │ │ │ -
208
│ │ │ │ -
210 GTSAM_EXPORT Point2 transformTo(const Point2& point,
│ │ │ │ -
211 OptionalJacobian<2, 3> Dpose = boost::none,
│ │ │ │ -
212 OptionalJacobian<2, 2> Dpoint = boost::none) const;
│ │ │ │ -
213
│ │ │ │ -
219 Matrix transformTo(const Matrix& points) const;
│ │ │ │ -
220
│ │ │ │ -
222 GTSAM_EXPORT Point2 transformFrom(const Point2& point,
│ │ │ │ -
223 OptionalJacobian<2, 3> Dpose = boost::none,
│ │ │ │ -
224 OptionalJacobian<2, 2> Dpoint = boost::none) const;
│ │ │ │ -
225
│ │ │ │ -
231 Matrix transformFrom(const Matrix& points) const;
│ │ │ │ -
232
│ │ │ │ -
│ │ │ │ -
234 inline Point2 operator*(const Point2& point) const {
│ │ │ │ -
235 return transformFrom(point);
│ │ │ │ -
236 }
│ │ │ │ -
│ │ │ │ -
237
│ │ │ │ -
241
│ │ │ │ -
243 inline double x() const { return t_.x(); }
│ │ │ │ -
244
│ │ │ │ -
246 inline double y() const { return t_.y(); }
│ │ │ │ -
247
│ │ │ │ -
249 inline double theta() const { return r_.theta(); }
│ │ │ │ -
250
│ │ │ │ -
252 inline const Point2& t() const { return t_; }
│ │ │ │ -
253
│ │ │ │ -
255 inline const Rot2& r() const { return r_; }
│ │ │ │ -
256
│ │ │ │ -
258 inline const Point2& translation() const { return t_; }
│ │ │ │ -
259
│ │ │ │ -
261 inline const Rot2& rotation() const { return r_; }
│ │ │ │ -
262
│ │ │ │ -
264 GTSAM_EXPORT Matrix3 matrix() const;
│ │ │ │ -
265
│ │ │ │ -
271 GTSAM_EXPORT Rot2 bearing(const Point2& point,
│ │ │ │ -
272 OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 2> H2=boost::none) const;
│ │ │ │ -
273
│ │ │ │ -
279 GTSAM_EXPORT Rot2 bearing(const Pose2& pose,
│ │ │ │ -
280 OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 3> H2=boost::none) const;
│ │ │ │ -
281
│ │ │ │ -
287 GTSAM_EXPORT double range(const Point2& point,
│ │ │ │ -
288 OptionalJacobian<1, 3> H1=boost::none,
│ │ │ │ -
289 OptionalJacobian<1, 2> H2=boost::none) const;
│ │ │ │ -
290
│ │ │ │ -
296 GTSAM_EXPORT double range(const Pose2& point,
│ │ │ │ -
297 OptionalJacobian<1, 3> H1=boost::none,
│ │ │ │ -
298 OptionalJacobian<1, 3> H2=boost::none) const;
│ │ │ │ -
299
│ │ │ │ -
303
│ │ │ │ -
309 inline static std::pair<size_t, size_t> translationInterval() { return std::make_pair(0, 1); }
│ │ │ │ -
310
│ │ │ │ -
316 static std::pair<size_t, size_t> rotationInterval() { return std::make_pair(2, 2); }
│ │ │ │ -
317
│ │ │ │ -
319 GTSAM_EXPORT
│ │ │ │ -
320 friend std::ostream &operator<<(std::ostream &os, const Pose2& p);
│ │ │ │ -
321
│ │ │ │ -
323
│ │ │ │ -
324 private:
│ │ │ │ -
325
│ │ │ │ -
326 // Serialization function
│ │ │ │ -
327 friend class boost::serialization::access;
│ │ │ │ -
328 template<class Archive>
│ │ │ │ -
329 void serialize(Archive & ar, const unsigned int /*version*/) {
│ │ │ │ -
330 ar & BOOST_SERIALIZATION_NVP(t_);
│ │ │ │ -
331 ar & BOOST_SERIALIZATION_NVP(r_);
│ │ │ │ -
332 }
│ │ │ │ -
333
│ │ │ │ -
334public:
│ │ │ │ -
335 // Align for Point2, which is either derived from, or is typedef, of Vector2
│ │ │ │ -
336 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
337}; // Pose2
│ │ │ │ -
│ │ │ │ -
338
│ │ │ │ -
340template <>
│ │ │ │ -
│ │ │ │ -
341inline Matrix wedge<Pose2>(const Vector& xi) {
│ │ │ │ -
342 // NOTE(chris): Need eval() as workaround for Apple clang + avx2.
│ │ │ │ -
343 return Matrix(Pose2::wedge(xi(0),xi(1),xi(2))).eval();
│ │ │ │ -
344}
│ │ │ │ -
│ │ │ │ -
345
│ │ │ │ -
346#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ -
352GTSAM_EXPORT boost::optional<Pose2>
│ │ │ │ -
353GTSAM_DEPRECATED align(const Point2Pairs& pairs);
│ │ │ │ -
354#endif
│ │ │ │ -
355
│ │ │ │ -
356// Convenience typedef
│ │ │ │ -
357using Pose2Pair = std::pair<Pose2, Pose2>;
│ │ │ │ -
358using Pose2Pairs = std::vector<Pose2Pair>;
│ │ │ │ -
359
│ │ │ │ -
360template <>
│ │ │ │ -
361struct traits<Pose2> : public internal::LieGroup<Pose2> {};
│ │ │ │ -
362
│ │ │ │ -
363template <>
│ │ │ │ -
364struct traits<const Pose2> : public internal::LieGroup<Pose2> {};
│ │ │ │ -
365
│ │ │ │ -
366// bearing and range traits, used in RangeFactor
│ │ │ │ -
367template <typename T>
│ │ │ │ -
368struct Bearing<Pose2, T> : HasBearing<Pose2, T, Rot2> {};
│ │ │ │ -
369
│ │ │ │ -
370template <typename T>
│ │ │ │ -
371struct Range<Pose2, T> : HasRange<Pose2, T, double> {};
│ │ │ │ -
372
│ │ │ │ -
373} // namespace gtsam
│ │ │ │ -
374
│ │ │ │ -
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ -
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ -
BearingRange.h
Bearing-Range product.
│ │ │ │ -
Rot2.h
2D rotation
│ │ │ │ -
Point2.h
2D Point
│ │ │ │ +
118 inline static size_t Dim() { return dimension; }
│ │ │ │ +
119
│ │ │ │ +
│ │ │ │ +
121 inline Cal3_S2 retract(const Vector& d) const {
│ │ │ │ +
122 return Cal3_S2(fx_ + d(0), fy_ + d(1), s_ + d(2), u0_ + d(3), v0_ + d(4));
│ │ │ │ +
123 }
│ │ │ │ +
│ │ │ │ +
124
│ │ │ │ +
│ │ │ │ +
126 Vector5 localCoordinates(const Cal3_S2& T2) const {
│ │ │ │ +
127 return T2.vector() - vector();
│ │ │ │ +
128 }
│ │ │ │ +
│ │ │ │ +
129
│ │ │ │ +
133
│ │ │ │ +
134 private:
│ │ │ │ +
136 friend class boost::serialization::access;
│ │ │ │ +
137 template <class Archive>
│ │ │ │ +
138 void serialize(Archive& ar, const unsigned int /*version*/) {
│ │ │ │ +
139 ar& boost::serialization::make_nvp(
│ │ │ │ +
140 "Cal3_S2", boost::serialization::base_object<Cal3>(*this));
│ │ │ │ +
141 }
│ │ │ │ +
142
│ │ │ │ +
144};
│ │ │ │ +
│ │ │ │ +
145
│ │ │ │ +
146template <>
│ │ │ │ +
147struct traits<Cal3_S2> : public internal::Manifold<Cal3_S2> {};
│ │ │ │ +
148
│ │ │ │ +
149template <>
│ │ │ │ +
150struct traits<const Cal3_S2> : public internal::Manifold<Cal3_S2> {};
│ │ │ │ +
151
│ │ │ │ +
152} // \ namespace gtsam
│ │ │ │ +
Point2.h
2D Point
│ │ │ │ +
Cal3.h
Common code for all Calibration models.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::wedge< Pose2 >
Matrix wedge< Pose2 >(const Vector &xi)
specialization for pose2 wedge function (generic template in Lie.h)
Definition Pose2.h:341
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::LieGroup
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition Lie.h:37
│ │ │ │ -
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition Lie.h:229
│ │ │ │ +
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
│ │ │ │
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Bearing
Definition BearingRange.h:34
│ │ │ │ -
gtsam::Range
Definition BearingRange.h:40
│ │ │ │ -
gtsam::HasBearing
Definition BearingRange.h:180
│ │ │ │ -
gtsam::HasRange
Definition BearingRange.h:194
│ │ │ │ -
gtsam::Pose2
A 2D pose (Point2,Rot2)
Definition Pose2.h:36
│ │ │ │ -
gtsam::Pose2::bearing
GTSAM_EXPORT Rot2 bearing(const Point2 &point, OptionalJacobian< 1, 3 > H1=boost::none, OptionalJacobian< 1, 2 > H2=boost::none) const
Calculate bearing to a landmark.
Definition Pose2.cpp:245
│ │ │ │ -
gtsam::Pose2::operator*
Pose2 operator*(const Pose2 &p2) const
compose syntactic sugar
Definition Pose2.h:128
│ │ │ │ -
gtsam::Pose2::print
GTSAM_EXPORT void print(const std::string &s="") const
print with optional string
Definition Pose2.cpp:50
│ │ │ │ -
gtsam::Pose2::rotation
const Rot2 & rotation() const
rotation
Definition Pose2.h:261
│ │ │ │ -
gtsam::Pose2::AdjointMap
GTSAM_EXPORT Matrix3 AdjointMap() const
Calculate Adjoint map Ad_pose is 3*3 matrix that when applied to twist xi , returns Ad_pose(xi)
Definition Pose2.cpp:126
│ │ │ │ -
gtsam::Pose2::LogmapDerivative
static GTSAM_EXPORT Matrix3 LogmapDerivative(const Pose2 &v)
Derivative of Logmap.
Definition Pose2.cpp:179
│ │ │ │ -
gtsam::Pose2::y
double y() const
get y
Definition Pose2.h:246
│ │ │ │ -
gtsam::Pose2::Logmap
static GTSAM_EXPORT Vector3 Logmap(const Pose2 &p, ChartJacobian H=boost::none)
Log map at identity - return the canonical coordinates of this rotation.
Definition Pose2.cpp:82
│ │ │ │ -
gtsam::Pose2::inverse
GTSAM_EXPORT Pose2 inverse() const
inverse
Definition Pose2.cpp:201
│ │ │ │ -
gtsam::Pose2::operator<<
GTSAM_EXPORT friend std::ostream & operator<<(std::ostream &os, const Pose2 &p)
Output stream operator.
Definition Pose2.cpp:55
│ │ │ │ -
gtsam::Pose2::operator*
Point2 operator*(const Point2 &point) const
syntactic sugar for transformFrom
Definition Pose2.h:234
│ │ │ │ -
gtsam::Pose2::Pose2
Pose2(double x, double y, double theta)
construct from (x,y,theta)
Definition Pose2.h:68
│ │ │ │ -
gtsam::Pose2::t
const Point2 & t() const
translation
Definition Pose2.h:252
│ │ │ │ -
gtsam::Pose2::translationInterval
static std::pair< size_t, size_t > translationInterval()
Return the start and end indices (inclusive) of the translation component of the exponential map para...
Definition Pose2.h:309
│ │ │ │ -
gtsam::Pose2::Pose2
Pose2(double theta, const Point2 &t)
construct from rotation and translation
Definition Pose2.h:73
│ │ │ │ -
gtsam::Pose2::x
double x() const
get x
Definition Pose2.h:243
│ │ │ │ -
gtsam::Pose2::r
const Rot2 & r() const
rotation
Definition Pose2.h:255
│ │ │ │ -
gtsam::Pose2::Pose2
Pose2(const Vector &v)
Construct from canonical coordinates (Lie algebra)
Definition Pose2.h:91
│ │ │ │ -
gtsam::Pose2::Adjoint
Vector3 Adjoint(const Vector3 &xi) const
Apply AdjointMap to twist xi.
Definition Pose2.h:149
│ │ │ │ -
gtsam::Pose2::wedge
static Matrix3 wedge(double vx, double vy, double w)
wedge for SE(2):
Definition Pose2.h:183
│ │ │ │ -
gtsam::Pose2::rotationInterval
static std::pair< size_t, size_t > rotationInterval()
Return the start and end indices (inclusive) of the rotation component of the exponential map paramet...
Definition Pose2.h:316
│ │ │ │ -
gtsam::Pose2::adjointMap
static GTSAM_EXPORT Matrix3 adjointMap(const Vector3 &v)
Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19.
Definition Pose2.cpp:137
│ │ │ │ -
gtsam::Pose2::transformTo
GTSAM_EXPORT Point2 transformTo(const Point2 &point, OptionalJacobian< 2, 3 > Dpose=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const
Return point coordinates in pose coordinate frame.
Definition Pose2.cpp:207
│ │ │ │ -
gtsam::Pose2::theta
double theta() const
get theta
Definition Pose2.h:249
│ │ │ │ -
gtsam::Pose2::ExpmapDerivative
static GTSAM_EXPORT Matrix3 ExpmapDerivative(const Vector3 &v)
Derivative of Expmap.
Definition Pose2.cpp:148
│ │ │ │ -
gtsam::Pose2::Rotation
Rot2 Rotation
Pose Concept requirements.
Definition Pose2.h:41
│ │ │ │ -
gtsam::Pose2::Pose2
Pose2()
default constructor = origin
Definition Pose2.h:55
│ │ │ │ -
gtsam::Pose2::transformFrom
GTSAM_EXPORT Point2 transformFrom(const Point2 &point, OptionalJacobian< 2, 3 > Dpose=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const
Return point coordinates in global frame.
Definition Pose2.cpp:226
│ │ │ │ -
gtsam::Pose2::adjointTranspose
static Vector3 adjointTranspose(const Vector3 &xi, const Vector3 &y)
The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
Definition Pose2.h:168
│ │ │ │ -
gtsam::Pose2::Expmap
static GTSAM_EXPORT Pose2 Expmap(const Vector3 &xi, ChartJacobian H=boost::none)
Exponential map at identity - create a rotation from canonical coordinates .
Definition Pose2.cpp:66
│ │ │ │ -
gtsam::Pose2::Identity
static Pose2 Identity()
identity for group operation
Definition Pose2.h:122
│ │ │ │ -
gtsam::Pose2::Align
static boost::optional< Pose2 > Align(const Point2Pairs &abPointPairs)
Create Pose2 by aligning two point pairs A pose aTb is estimated between pairs (a_point,...
Definition Pose2.cpp:330
│ │ │ │ -
gtsam::Pose2::Pose2
Pose2(const Rot2 &r, const Point2 &t)
construct from r,t
Definition Pose2.h:78
│ │ │ │ -
gtsam::Pose2::adjoint
static Vector3 adjoint(const Vector3 &xi, const Vector3 &y)
Action of the adjointMap on a Lie-algebra vector y, with optional derivatives.
Definition Pose2.h:161
│ │ │ │ -
gtsam::Pose2::Pose2
Pose2(const Pose2 &pose)
copy constructor
Definition Pose2.h:60
│ │ │ │ -
gtsam::Pose2::translation
const Point2 & translation() const
translation
Definition Pose2.h:258
│ │ │ │ -
gtsam::Pose2::Pose2
Pose2(const Matrix &T)
Constructor from 3*3 matrix.
Definition Pose2.h:81
│ │ │ │ -
gtsam::Pose2::ChartAtOrigin
Definition Pose2.h:198
│ │ │ │ -
gtsam::Rot2
Rotation matrix NOTE: the angle theta is in radians unless explicitly stated.
Definition Rot2.h:36
│ │ │ │ -
gtsam::Rot2::theta
double theta() const
return angle (RADIANS)
Definition Rot2.h:187
│ │ │ │ +
gtsam::Cal3
Common base class for all calibration models.
Definition Cal3.h:69
│ │ │ │ +
gtsam::Cal3::vector
Vector5 vector() const
vectorized form (column-wise)
Definition Cal3.h:160
│ │ │ │ +
gtsam::Cal3::fy_
double fy_
focal length
Definition Cal3.h:71
│ │ │ │ +
gtsam::Cal3::s_
double s_
skew
Definition Cal3.h:72
│ │ │ │ +
gtsam::Cal3::v0_
double v0_
principal point
Definition Cal3.h:73
│ │ │ │ +
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition Cal3_S2.h:34
│ │ │ │ +
gtsam::Cal3_S2::Cal3_S2
Cal3_S2()=default
Create a default calibration that leaves coordinates unchanged.
│ │ │ │ +
gtsam::Cal3_S2::Cal3_S2
Cal3_S2(double fx, double fy, double s, double u0, double v0)
constructor from doubles
Definition Cal3_S2.h:48
│ │ │ │ +
gtsam::Cal3_S2::between
Cal3_S2 between(const Cal3_S2 &q, OptionalJacobian< 5, 5 > H1=boost::none, OptionalJacobian< 5, 5 > H2=boost::none) const
"Between", subtracts calibrations. between(p,q) == compose(inverse(p),q)
Definition Cal3_S2.h:104
│ │ │ │ +
gtsam::Cal3_S2::Dim
static size_t Dim()
return DOF, dimensionality of tangent space
Definition Cal3_S2.h:118
│ │ │ │ +
gtsam::Cal3_S2::Cal3_S2
Cal3_S2(const Vector5 &d)
constructor from vector
Definition Cal3_S2.h:52
│ │ │ │ +
gtsam::Cal3_S2::localCoordinates
Vector5 localCoordinates(const Cal3_S2 &T2) const
Unretraction for the calibration.
Definition Cal3_S2.h:126
│ │ │ │ +
gtsam::Cal3_S2::Cal3_S2
Cal3_S2(double fov, int w, int h)
Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect.
Definition Cal3_S2.h:60
│ │ │ │ +
gtsam::Cal3_S2::retract
Cal3_S2 retract(const Vector &d) const
Given 5-dim tangent vector, create new calibration.
Definition Cal3_S2.h:121
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,472 +1,186 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Pose2.h │ │ │ │ │ +Cal3_S2.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19// \callgraph │ │ │ │ │ -20 │ │ │ │ │ -21#pragma once │ │ │ │ │ -22 │ │ │ │ │ -23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_B_e_a_r_i_n_g_R_a_n_g_e_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_2_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_b_a_s_e_/_L_i_e_._h> │ │ │ │ │ -27#include │ │ │ │ │ +22#pragma once │ │ │ │ │ +23 │ │ │ │ │ +24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ +26 │ │ │ │ │ +27namespace _g_t_s_a_m { │ │ │ │ │ 28 │ │ │ │ │ -29namespace _g_t_s_a_m { │ │ │ │ │ -30 │ │ │ │ │ -_3_6class _P_o_s_e_2: public _L_i_e_G_r_o_u_p { │ │ │ │ │ +_3_4class GTSAM_EXPORT _C_a_l_3___S_2 : public _C_a_l_3 { │ │ │ │ │ +35 public: │ │ │ │ │ +36 enum { dimension = 5 }; │ │ │ │ │ 37 │ │ │ │ │ -38public: │ │ │ │ │ -39 │ │ │ │ │ -_4_1 typedef _R_o_t_2 _R_o_t_a_t_i_o_n; │ │ │ │ │ -42 typedef _P_o_i_n_t_2 Translation; │ │ │ │ │ +39 using shared_ptr = boost::shared_ptr; │ │ │ │ │ +40 │ │ │ │ │ 43 │ │ │ │ │ -44private: │ │ │ │ │ -45 │ │ │ │ │ -46 _R_o_t_2 r_; │ │ │ │ │ -47 _P_o_i_n_t_2 t_; │ │ │ │ │ -48 │ │ │ │ │ -49public: │ │ │ │ │ +_4_5 _C_a_l_3___S_2() = default; │ │ │ │ │ +46 │ │ │ │ │ +_4_8 _C_a_l_3___S_2(double fx, double fy, double s, double u0, double v0) │ │ │ │ │ +49 : _C_a_l_3(fx, fy, s, u0, v0) {} │ │ │ │ │ 50 │ │ │ │ │ +_5_2 _C_a_l_3___S_2(const Vector5& d) : _C_a_l_3(d) {} │ │ │ │ │ 53 │ │ │ │ │ -_5_5 _P_o_s_e_2() : │ │ │ │ │ -56 r_(_t_r_a_i_t_s<_R_o_t_2>::_I_d_e_n_t_i_t_y()), t_(_t_r_a_i_t_s<_P_o_i_n_t_2>::_I_d_e_n_t_i_t_y()) { │ │ │ │ │ -57 } │ │ │ │ │ -58 │ │ │ │ │ -_6_0 _P_o_s_e_2(const _P_o_s_e_2& pose) : r_(pose.r_), t_(pose.t_) {} │ │ │ │ │ +_6_0 _C_a_l_3___S_2(double fov, int w, int h) : _C_a_l_3(fov, w, h) {} │ │ │ │ │ 61 │ │ │ │ │ -_6_8 _P_o_s_e_2(double _x, double _y, double _t_h_e_t_a) : │ │ │ │ │ -69 r_(_R_o_t_2::fromAngle(_t_h_e_t_a)), t_(_x, _y) { │ │ │ │ │ -70 } │ │ │ │ │ +69 _P_o_i_n_t_2 uncalibrate(const _P_o_i_n_t_2& p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _5_> Dcal = boost:: │ │ │ │ │ +none, │ │ │ │ │ +70 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dp = boost::none) const; │ │ │ │ │ 71 │ │ │ │ │ -_7_3 _P_o_s_e_2(double _t_h_e_t_a, const _P_o_i_n_t_2& _t) : │ │ │ │ │ -74 r_(_R_o_t_2::fromAngle(_t_h_e_t_a)), t_(_t) { │ │ │ │ │ -75 } │ │ │ │ │ -76 │ │ │ │ │ -_7_8 _P_o_s_e_2(const _R_o_t_2& _r, const _P_o_i_n_t_2& _t) : r_(_r), t_(_t) {} │ │ │ │ │ -79 │ │ │ │ │ -_8_1 _P_o_s_e_2(const Matrix &T) : │ │ │ │ │ -82 r_(_R_o_t_2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) { │ │ │ │ │ -83 assert(T.rows() == 3 && T.cols() == 3); │ │ │ │ │ -84 } │ │ │ │ │ -85 │ │ │ │ │ -89 │ │ │ │ │ -_9_1 _P_o_s_e_2(const Vector& v) : _P_o_s_e_2() { │ │ │ │ │ -92 *this = _E_x_p_m_a_p(v); │ │ │ │ │ -93 } │ │ │ │ │ -94 │ │ │ │ │ -102 static boost::optional _A_l_i_g_n(const Point2Pairs& abPointPairs); │ │ │ │ │ -103 │ │ │ │ │ -104 // Version of Pose2::Align that takes 2 matrices. │ │ │ │ │ -105 static boost::optional _A_l_i_g_n(const Matrix& a, const Matrix& b); │ │ │ │ │ -106 │ │ │ │ │ -110 │ │ │ │ │ -112 GTSAM_EXPORT void _p_r_i_n_t(const std::string& s = "") const; │ │ │ │ │ -113 │ │ │ │ │ -115 GTSAM_EXPORT bool _e_q_u_a_l_s(const _P_o_s_e_2& pose, double tol = 1e-9) const; │ │ │ │ │ +79 _P_o_i_n_t_2 calibrate(const _P_o_i_n_t_2& p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _5_> Dcal = boost::none, │ │ │ │ │ +80 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dp = boost::none) const; │ │ │ │ │ +81 │ │ │ │ │ +87 Vector3 calibrate(const Vector3& p) const; │ │ │ │ │ +88 │ │ │ │ │ +92 │ │ │ │ │ +94 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, │ │ │ │ │ +95 const _C_a_l_3___S_2& cal); │ │ │ │ │ +96 │ │ │ │ │ +98 void _p_r_i_n_t(const std::string& s = "Cal3_S2") const override; │ │ │ │ │ +99 │ │ │ │ │ +101 bool _e_q_u_a_l_s(const _C_a_l_3___S_2& K, double tol = 10e-9) const; │ │ │ │ │ +102 │ │ │ │ │ +_1_0_4 inline _C_a_l_3___S_2 _b_e_t_w_e_e_n(const _C_a_l_3___S_2& q, │ │ │ │ │ +105 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_5_,_ _5_> H1 = boost::none, │ │ │ │ │ +106 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_5_,_ _5_> H2 = boost::none) const { │ │ │ │ │ +107 if (H1) *H1 = -I_5x5; │ │ │ │ │ +108 if (H2) *H2 = I_5x5; │ │ │ │ │ +109 return _C_a_l_3___S_2(q.fx_ - fx_, q._f_y__ - fy_, q._s__ - s_, q.u0_ - u0_, │ │ │ │ │ +110 q._v_0__ - v0_); │ │ │ │ │ +111 } │ │ │ │ │ +112 │ │ │ │ │ 116 │ │ │ │ │ -120 │ │ │ │ │ -_1_2_2 inline static _P_o_s_e_2 _I_d_e_n_t_i_t_y() { return _P_o_s_e_2(); } │ │ │ │ │ -123 │ │ │ │ │ -125 GTSAM_EXPORT _P_o_s_e_2 _i_n_v_e_r_s_e() const; │ │ │ │ │ -126 │ │ │ │ │ -_1_2_8 inline _P_o_s_e_2 _o_p_e_r_a_t_o_r_*(const _P_o_s_e_2& p2) const { │ │ │ │ │ -129 return _P_o_s_e_2(r_*p2._r(), t_ + r_*p2._t()); │ │ │ │ │ -130 } │ │ │ │ │ -131 │ │ │ │ │ -135 │ │ │ │ │ -137 GTSAM_EXPORT static _P_o_s_e_2 _E_x_p_m_a_p(const Vector3& xi, ChartJacobian H = │ │ │ │ │ -boost::none); │ │ │ │ │ -138 │ │ │ │ │ -140 GTSAM_EXPORT static Vector3 _L_o_g_m_a_p(const _P_o_s_e_2& p, ChartJacobian H = │ │ │ │ │ -boost::none); │ │ │ │ │ -141 │ │ │ │ │ -146 GTSAM_EXPORT Matrix3 _A_d_j_o_i_n_t_M_a_p() const; │ │ │ │ │ -147 │ │ │ │ │ -_1_4_9 inline Vector3 _A_d_j_o_i_n_t(const Vector3& xi) const { │ │ │ │ │ -150 return _A_d_j_o_i_n_t_M_a_p()*xi; │ │ │ │ │ -151 } │ │ │ │ │ -152 │ │ │ │ │ -156 GTSAM_EXPORT static Matrix3 _a_d_j_o_i_n_t_M_a_p(const Vector3& v); │ │ │ │ │ -157 │ │ │ │ │ -_1_6_1 static Vector3 _a_d_j_o_i_n_t(const Vector3& xi, const Vector3& _y) { │ │ │ │ │ -162 return _a_d_j_o_i_n_t_M_a_p(xi) * _y; │ │ │ │ │ -163 } │ │ │ │ │ -164 │ │ │ │ │ -_1_6_8 static Vector3 _a_d_j_o_i_n_t_T_r_a_n_s_p_o_s_e(const Vector3& xi, const Vector3& _y) { │ │ │ │ │ -169 return _a_d_j_o_i_n_t_M_a_p(xi).transpose() * _y; │ │ │ │ │ -170 } │ │ │ │ │ -171 │ │ │ │ │ -172 // temporary fix for wrappers until case issue is resolved │ │ │ │ │ -173 static Matrix3 adjointMap_(const Vector3 &xi) { return _a_d_j_o_i_n_t_M_a_p(xi);} │ │ │ │ │ -174 static Vector3 adjoint_(const Vector3 &xi, const Vector3 &_y) { return │ │ │ │ │ -_a_d_j_o_i_n_t(xi, _y);} │ │ │ │ │ -175 │ │ │ │ │ -_1_8_3 static inline Matrix3 _w_e_d_g_e(double vx, double vy, double w) { │ │ │ │ │ -184 Matrix3 m; │ │ │ │ │ -185 m << 0.,-w, vx, │ │ │ │ │ -186 w, 0., vy, │ │ │ │ │ -187 0., 0., 0.; │ │ │ │ │ -188 return m; │ │ │ │ │ -189 } │ │ │ │ │ -190 │ │ │ │ │ -192 GTSAM_EXPORT static Matrix3 _E_x_p_m_a_p_D_e_r_i_v_a_t_i_v_e(const Vector3& v); │ │ │ │ │ -193 │ │ │ │ │ -195 GTSAM_EXPORT static Matrix3 _L_o_g_m_a_p_D_e_r_i_v_a_t_i_v_e(const _P_o_s_e_2& v); │ │ │ │ │ -196 │ │ │ │ │ -197 // Chart at origin, depends on compile-time flag SLOW_BUT_CORRECT_EXPMAP │ │ │ │ │ -_1_9_8 struct _C_h_a_r_t_A_t_O_r_i_g_i_n { │ │ │ │ │ -199 GTSAM_EXPORT static _P_o_s_e_2 Retract(const Vector3& v, ChartJacobian H = │ │ │ │ │ -boost::none); │ │ │ │ │ -200 GTSAM_EXPORT static Vector3 Local(const _P_o_s_e_2& _r, ChartJacobian H = boost:: │ │ │ │ │ -none); │ │ │ │ │ -201 }; │ │ │ │ │ -202 │ │ │ │ │ -203 using _L_i_e_G_r_o_u_p<_P_o_s_e_2, 3>_:_:_i_n_v_e_r_s_e; // version with derivative │ │ │ │ │ -204 │ │ │ │ │ -208 │ │ │ │ │ -210 GTSAM_EXPORT _P_o_i_n_t_2 _t_r_a_n_s_f_o_r_m_T_o(const _P_o_i_n_t_2& point, │ │ │ │ │ -211 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> Dpose = boost::none, │ │ │ │ │ -212 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dpoint = boost::none) const; │ │ │ │ │ -213 │ │ │ │ │ -219 Matrix _t_r_a_n_s_f_o_r_m_T_o(const Matrix& points) const; │ │ │ │ │ -220 │ │ │ │ │ -222 GTSAM_EXPORT _P_o_i_n_t_2 _t_r_a_n_s_f_o_r_m_F_r_o_m(const _P_o_i_n_t_2& point, │ │ │ │ │ -223 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> Dpose = boost::none, │ │ │ │ │ -224 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dpoint = boost::none) const; │ │ │ │ │ -225 │ │ │ │ │ -231 Matrix _t_r_a_n_s_f_o_r_m_F_r_o_m(const Matrix& points) const; │ │ │ │ │ -232 │ │ │ │ │ -_2_3_4 inline _P_o_i_n_t_2 _o_p_e_r_a_t_o_r_*(const _P_o_i_n_t_2& point) const { │ │ │ │ │ -235 return _t_r_a_n_s_f_o_r_m_F_r_o_m(point); │ │ │ │ │ -236 } │ │ │ │ │ -237 │ │ │ │ │ -241 │ │ │ │ │ -_2_4_3 inline double _x() const { return t_.x(); } │ │ │ │ │ -244 │ │ │ │ │ -_2_4_6 inline double _y() const { return t_.y(); } │ │ │ │ │ -247 │ │ │ │ │ -_2_4_9 inline double _t_h_e_t_a() const { return r_._t_h_e_t_a(); } │ │ │ │ │ -250 │ │ │ │ │ -_2_5_2 inline const _P_o_i_n_t_2& _t() const { return t_; } │ │ │ │ │ -253 │ │ │ │ │ -_2_5_5 inline const _R_o_t_2& _r() const { return r_; } │ │ │ │ │ -256 │ │ │ │ │ -_2_5_8 inline const _P_o_i_n_t_2& _t_r_a_n_s_l_a_t_i_o_n() const { return t_; } │ │ │ │ │ -259 │ │ │ │ │ -_2_6_1 inline const _R_o_t_2& _r_o_t_a_t_i_o_n() const { return r_; } │ │ │ │ │ -262 │ │ │ │ │ -264 GTSAM_EXPORT Matrix3 matrix() const; │ │ │ │ │ -265 │ │ │ │ │ -271 GTSAM_EXPORT _R_o_t_2 _b_e_a_r_i_n_g(const _P_o_i_n_t_2& point, │ │ │ │ │ -272 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> H1=boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _2_> H2=boost:: │ │ │ │ │ -none) const; │ │ │ │ │ -273 │ │ │ │ │ -279 GTSAM_EXPORT _R_o_t_2 _b_e_a_r_i_n_g(const _P_o_s_e_2& pose, │ │ │ │ │ -280 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> H1=boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> H2=boost:: │ │ │ │ │ -none) const; │ │ │ │ │ -281 │ │ │ │ │ -287 GTSAM_EXPORT double range(const _P_o_i_n_t_2& point, │ │ │ │ │ -288 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> H1=boost::none, │ │ │ │ │ -289 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _2_> H2=boost::none) const; │ │ │ │ │ -290 │ │ │ │ │ -296 GTSAM_EXPORT double range(const _P_o_s_e_2& point, │ │ │ │ │ -297 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> H1=boost::none, │ │ │ │ │ -298 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> H2=boost::none) const; │ │ │ │ │ -299 │ │ │ │ │ -303 │ │ │ │ │ -_3_0_9 inline static std::pair _t_r_a_n_s_l_a_t_i_o_n_I_n_t_e_r_v_a_l() { return │ │ │ │ │ -std::make_pair(0, 1); } │ │ │ │ │ -310 │ │ │ │ │ -_3_1_6 static std::pair _r_o_t_a_t_i_o_n_I_n_t_e_r_v_a_l() { return std::make_pair │ │ │ │ │ -(2, 2); } │ │ │ │ │ -317 │ │ │ │ │ -319 GTSAM_EXPORT │ │ │ │ │ -320 friend std::ostream &_o_p_e_r_a_t_o_r_<_<(std::ostream &os, const _P_o_s_e_2& p); │ │ │ │ │ -321 │ │ │ │ │ -323 │ │ │ │ │ -324 private: │ │ │ │ │ -325 │ │ │ │ │ -326 // Serialization function │ │ │ │ │ -327 friend class boost::serialization::access; │ │ │ │ │ -328 template │ │ │ │ │ -329 void serialize(Archive & ar, const unsigned int /*version*/) { │ │ │ │ │ -330 ar & BOOST_SERIALIZATION_NVP(t_); │ │ │ │ │ -331 ar & BOOST_SERIALIZATION_NVP(r_); │ │ │ │ │ -332 } │ │ │ │ │ -333 │ │ │ │ │ -334public: │ │ │ │ │ -335 // Align for Point2, which is either derived from, or is typedef, of │ │ │ │ │ -Vector2 │ │ │ │ │ -336 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -337}; // Pose2 │ │ │ │ │ -338 │ │ │ │ │ -340template <> │ │ │ │ │ -_3_4_1inline Matrix _w_e_d_g_e_<_P_o_s_e_2_>(const Vector& xi) { │ │ │ │ │ -342 // NOTE(chris): Need eval() as workaround for Apple clang + avx2. │ │ │ │ │ -343 return Matrix(_P_o_s_e_2_:_:_w_e_d_g_e(xi(0),xi(1),xi(2))).eval(); │ │ │ │ │ -344} │ │ │ │ │ -345 │ │ │ │ │ -346#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ -352GTSAM_EXPORT boost::optional │ │ │ │ │ -353GTSAM_DEPRECATED align(const Point2Pairs& pairs); │ │ │ │ │ -354#endif │ │ │ │ │ -355 │ │ │ │ │ -356// Convenience typedef │ │ │ │ │ -357using Pose2Pair = std::pair; │ │ │ │ │ -358using Pose2Pairs = std::vector; │ │ │ │ │ -359 │ │ │ │ │ -360template <> │ │ │ │ │ -_3_6_1struct _t_r_a_i_t_s<_P_o_s_e_2> : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ -362 │ │ │ │ │ -363template <> │ │ │ │ │ -_3_6_4struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ -365 │ │ │ │ │ -366// bearing and range traits, used in RangeFactor │ │ │ │ │ -367template │ │ │ │ │ -_3_6_8struct _B_e_a_r_i_n_g<_P_o_s_e_2, T> : _H_a_s_B_e_a_r_i_n_g {}; │ │ │ │ │ -369 │ │ │ │ │ -370template │ │ │ │ │ -_3_7_1struct _R_a_n_g_e<_P_o_s_e_2, T> : _H_a_s_R_a_n_g_e {}; │ │ │ │ │ -372 │ │ │ │ │ -373} // namespace gtsam │ │ │ │ │ -374 │ │ │ │ │ -_L_i_e_._h │ │ │ │ │ -Base class and basic functions for Lie types. │ │ │ │ │ -_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ -This marks a GTSAM object to require alignment. │ │ │ │ │ -DDeeffiinniittiioonn types.h:308 │ │ │ │ │ -_B_e_a_r_i_n_g_R_a_n_g_e_._h │ │ │ │ │ -Bearing-Range product. │ │ │ │ │ -_R_o_t_2_._h │ │ │ │ │ -2D rotation │ │ │ │ │ +_1_1_8 inline static size_t _D_i_m() { return dimension; } │ │ │ │ │ +119 │ │ │ │ │ +_1_2_1 inline _C_a_l_3___S_2 _r_e_t_r_a_c_t(const Vector& d) const { │ │ │ │ │ +122 return _C_a_l_3___S_2(fx_ + d(0), fy_ + d(1), s_ + d(2), u0_ + d(3), v0_ + d(4)); │ │ │ │ │ +123 } │ │ │ │ │ +124 │ │ │ │ │ +_1_2_6 Vector5 _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(const _C_a_l_3___S_2& T2) const { │ │ │ │ │ +127 return T2._v_e_c_t_o_r() - vector(); │ │ │ │ │ +128 } │ │ │ │ │ +129 │ │ │ │ │ +133 │ │ │ │ │ +134 private: │ │ │ │ │ +_1_3_6 friend class boost::serialization::access; │ │ │ │ │ +137 template │ │ │ │ │ +138 void serialize(Archive& ar, const unsigned int /*version*/) { │ │ │ │ │ +139 ar& boost::serialization::make_nvp( │ │ │ │ │ +140 "Cal3_S2", boost::serialization::base_object(*this)); │ │ │ │ │ +141 } │ │ │ │ │ +142 │ │ │ │ │ +144}; │ │ │ │ │ +145 │ │ │ │ │ +146template <> │ │ │ │ │ +_1_4_7struct _t_r_a_i_t_s<_C_a_l_3___S_2> : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d {}; │ │ │ │ │ +148 │ │ │ │ │ +149template <> │ │ │ │ │ +_1_5_0struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d {}; │ │ │ │ │ +151 │ │ │ │ │ +152} // \ namespace gtsam │ │ │ │ │ _P_o_i_n_t_2_._h │ │ │ │ │ 2D Point │ │ │ │ │ +_C_a_l_3_._h │ │ │ │ │ +Common code for all Calibration models. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_w_e_d_g_e_<_ _P_o_s_e_2_ _> │ │ │ │ │ -Matrix wedge< Pose2 >(const Vector &xi) │ │ │ │ │ -specialization for pose2 wedge function (generic template in Lie.h) │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:341 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ _g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ Vector2 Point2 │ │ │ │ │ As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ typedef Point2 to Vector2... │ │ │ │ │ DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ -A CRTP helper class that implements Lie group methods Prerequisites: methods │ │ │ │ │ -operator*,... │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ -Both LieGroupTraits and Testable. │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:229 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ +Both ManifoldTraits and Testable. │ │ │ │ │ +DDeeffiinniittiioonn Manifold.h:120 │ │ │ │ │ _g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ either a fixed size o... │ │ │ │ │ DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ _g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ Template to create a binary predicate. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_B_e_a_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn BearingRange.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_R_a_n_g_e │ │ │ │ │ -DDeeffiinniittiioonn BearingRange.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_H_a_s_B_e_a_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn BearingRange.h:180 │ │ │ │ │ -_g_t_s_a_m_:_:_H_a_s_R_a_n_g_e │ │ │ │ │ -DDeeffiinniittiioonn BearingRange.h:194 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2 │ │ │ │ │ -A 2D pose (Point2,Rot2) │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_b_e_a_r_i_n_g │ │ │ │ │ -GTSAM_EXPORT Rot2 bearing(const Point2 &point, OptionalJacobian< 1, 3 > │ │ │ │ │ -H1=boost::none, OptionalJacobian< 1, 2 > H2=boost::none) const │ │ │ │ │ -Calculate bearing to a landmark. │ │ │ │ │ -DDeeffiinniittiioonn Pose2.cpp:245 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ -Pose2 operator*(const Pose2 &p2) const │ │ │ │ │ -compose syntactic sugar │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:128 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_p_r_i_n_t │ │ │ │ │ -GTSAM_EXPORT void print(const std::string &s="") const │ │ │ │ │ -print with optional string │ │ │ │ │ -DDeeffiinniittiioonn Pose2.cpp:50 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_r_o_t_a_t_i_o_n │ │ │ │ │ -const Rot2 & rotation() const │ │ │ │ │ -rotation │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:261 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_A_d_j_o_i_n_t_M_a_p │ │ │ │ │ -GTSAM_EXPORT Matrix3 AdjointMap() const │ │ │ │ │ -Calculate Adjoint map Ad_pose is 3*3 matrix that when applied to twist xi , │ │ │ │ │ -returns Ad_pose(xi) │ │ │ │ │ -DDeeffiinniittiioonn Pose2.cpp:126 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_L_o_g_m_a_p_D_e_r_i_v_a_t_i_v_e │ │ │ │ │ -static GTSAM_EXPORT Matrix3 LogmapDerivative(const Pose2 &v) │ │ │ │ │ -Derivative of Logmap. │ │ │ │ │ -DDeeffiinniittiioonn Pose2.cpp:179 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_y │ │ │ │ │ -double y() const │ │ │ │ │ -get y │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:246 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_L_o_g_m_a_p │ │ │ │ │ -static GTSAM_EXPORT Vector3 Logmap(const Pose2 &p, ChartJacobian H=boost::none) │ │ │ │ │ -Log map at identity - return the canonical coordinates of this rotation. │ │ │ │ │ -DDeeffiinniittiioonn Pose2.cpp:82 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_i_n_v_e_r_s_e │ │ │ │ │ -GTSAM_EXPORT Pose2 inverse() const │ │ │ │ │ -inverse │ │ │ │ │ -DDeeffiinniittiioonn Pose2.cpp:201 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_o_p_e_r_a_t_o_r_<_< │ │ │ │ │ -GTSAM_EXPORT friend std::ostream & operator<<(std::ostream &os, const Pose2 &p) │ │ │ │ │ -Output stream operator. │ │ │ │ │ -DDeeffiinniittiioonn Pose2.cpp:55 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ -Point2 operator*(const Point2 &point) const │ │ │ │ │ -syntactic sugar for transformFrom │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:234 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_P_o_s_e_2 │ │ │ │ │ -Pose2(double x, double y, double theta) │ │ │ │ │ -construct from (x,y,theta) │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:68 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_t │ │ │ │ │ -const Point2 & t() const │ │ │ │ │ -translation │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:252 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_t_r_a_n_s_l_a_t_i_o_n_I_n_t_e_r_v_a_l │ │ │ │ │ -static std::pair< size_t, size_t > translationInterval() │ │ │ │ │ -Return the start and end indices (inclusive) of the translation component of │ │ │ │ │ -the exponential map para... │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:309 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_P_o_s_e_2 │ │ │ │ │ -Pose2(double theta, const Point2 &t) │ │ │ │ │ -construct from rotation and translation │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:73 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_x │ │ │ │ │ -double x() const │ │ │ │ │ -get x │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:243 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_r │ │ │ │ │ -const Rot2 & r() const │ │ │ │ │ -rotation │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:255 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_P_o_s_e_2 │ │ │ │ │ -Pose2(const Vector &v) │ │ │ │ │ -Construct from canonical coordinates (Lie algebra) │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:91 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_A_d_j_o_i_n_t │ │ │ │ │ -Vector3 Adjoint(const Vector3 &xi) const │ │ │ │ │ -Apply AdjointMap to twist xi. │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:149 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_w_e_d_g_e │ │ │ │ │ -static Matrix3 wedge(double vx, double vy, double w) │ │ │ │ │ -wedge for SE(2): │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:183 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_r_o_t_a_t_i_o_n_I_n_t_e_r_v_a_l │ │ │ │ │ -static std::pair< size_t, size_t > rotationInterval() │ │ │ │ │ -Return the start and end indices (inclusive) of the rotation component of the │ │ │ │ │ -exponential map paramet... │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:316 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_a_d_j_o_i_n_t_M_a_p │ │ │ │ │ -static GTSAM_EXPORT Matrix3 adjointMap(const Vector3 &v) │ │ │ │ │ -Compute the [ad(w,v)] operator for SE2 as in [Kobilarov09siggraph], pg 19. │ │ │ │ │ -DDeeffiinniittiioonn Pose2.cpp:137 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_t_r_a_n_s_f_o_r_m_T_o │ │ │ │ │ -GTSAM_EXPORT Point2 transformTo(const Point2 &point, OptionalJacobian< 2, 3 > │ │ │ │ │ -Dpose=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const │ │ │ │ │ -Return point coordinates in pose coordinate frame. │ │ │ │ │ -DDeeffiinniittiioonn Pose2.cpp:207 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_t_h_e_t_a │ │ │ │ │ -double theta() const │ │ │ │ │ -get theta │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:249 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_E_x_p_m_a_p_D_e_r_i_v_a_t_i_v_e │ │ │ │ │ -static GTSAM_EXPORT Matrix3 ExpmapDerivative(const Vector3 &v) │ │ │ │ │ -Derivative of Expmap. │ │ │ │ │ -DDeeffiinniittiioonn Pose2.cpp:148 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_R_o_t_a_t_i_o_n │ │ │ │ │ -Rot2 Rotation │ │ │ │ │ -Pose Concept requirements. │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_P_o_s_e_2 │ │ │ │ │ -Pose2() │ │ │ │ │ -default constructor = origin │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_t_r_a_n_s_f_o_r_m_F_r_o_m │ │ │ │ │ -GTSAM_EXPORT Point2 transformFrom(const Point2 &point, OptionalJacobian< 2, 3 > │ │ │ │ │ -Dpose=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const │ │ │ │ │ -Return point coordinates in global frame. │ │ │ │ │ -DDeeffiinniittiioonn Pose2.cpp:226 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_a_d_j_o_i_n_t_T_r_a_n_s_p_o_s_e │ │ │ │ │ -static Vector3 adjointTranspose(const Vector3 &xi, const Vector3 &y) │ │ │ │ │ -The dual version of adjoint action, acting on the dual space of the Lie-algebra │ │ │ │ │ -vector space. │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:168 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_E_x_p_m_a_p │ │ │ │ │ -static GTSAM_EXPORT Pose2 Expmap(const Vector3 &xi, ChartJacobian H=boost:: │ │ │ │ │ -none) │ │ │ │ │ -Exponential map at identity - create a rotation from canonical coordinates . │ │ │ │ │ -DDeeffiinniittiioonn Pose2.cpp:66 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_I_d_e_n_t_i_t_y │ │ │ │ │ -static Pose2 Identity() │ │ │ │ │ -identity for group operation │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:122 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_A_l_i_g_n │ │ │ │ │ -static boost::optional< Pose2 > Align(const Point2Pairs &abPointPairs) │ │ │ │ │ -Create Pose2 by aligning two point pairs A pose aTb is estimated between pairs │ │ │ │ │ -(a_point,... │ │ │ │ │ -DDeeffiinniittiioonn Pose2.cpp:330 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_P_o_s_e_2 │ │ │ │ │ -Pose2(const Rot2 &r, const Point2 &t) │ │ │ │ │ -construct from r,t │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:78 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_a_d_j_o_i_n_t │ │ │ │ │ -static Vector3 adjoint(const Vector3 &xi, const Vector3 &y) │ │ │ │ │ -Action of the adjointMap on a Lie-algebra vector y, with optional derivatives. │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:161 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_P_o_s_e_2 │ │ │ │ │ -Pose2(const Pose2 &pose) │ │ │ │ │ -copy constructor │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:60 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_t_r_a_n_s_l_a_t_i_o_n │ │ │ │ │ -const Point2 & translation() const │ │ │ │ │ -translation │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:258 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_P_o_s_e_2 │ │ │ │ │ -Pose2(const Matrix &T) │ │ │ │ │ -Constructor from 3*3 matrix. │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:81 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:198 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_2 │ │ │ │ │ -Rotation matrix NOTE: the angle theta is in radians unless explicitly stated. │ │ │ │ │ -DDeeffiinniittiioonn Rot2.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_2_:_:_t_h_e_t_a │ │ │ │ │ -double theta() const │ │ │ │ │ -return angle (RADIANS) │ │ │ │ │ -DDeeffiinniittiioonn Rot2.h:187 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3 │ │ │ │ │ +Common base class for all calibration models. │ │ │ │ │ +DDeeffiinniittiioonn Cal3.h:69 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_:_:_v_e_c_t_o_r │ │ │ │ │ +Vector5 vector() const │ │ │ │ │ +vectorized form (column-wise) │ │ │ │ │ +DDeeffiinniittiioonn Cal3.h:160 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_:_:_f_y__ │ │ │ │ │ +double fy_ │ │ │ │ │ +focal length │ │ │ │ │ +DDeeffiinniittiioonn Cal3.h:71 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_:_:_s__ │ │ │ │ │ +double s_ │ │ │ │ │ +skew │ │ │ │ │ +DDeeffiinniittiioonn Cal3.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_:_:_v_0__ │ │ │ │ │ +double v0_ │ │ │ │ │ +principal point │ │ │ │ │ +DDeeffiinniittiioonn Cal3.h:73 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3___S_2 │ │ │ │ │ +The most common 5DOF 3D->2D calibration. │ │ │ │ │ +DDeeffiinniittiioonn Cal3_S2.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3___S_2_:_:_C_a_l_3___S_2 │ │ │ │ │ +Cal3_S2()=default │ │ │ │ │ +Create a default calibration that leaves coordinates unchanged. │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3___S_2_:_:_C_a_l_3___S_2 │ │ │ │ │ +Cal3_S2(double fx, double fy, double s, double u0, double v0) │ │ │ │ │ +constructor from doubles │ │ │ │ │ +DDeeffiinniittiioonn Cal3_S2.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3___S_2_:_:_b_e_t_w_e_e_n │ │ │ │ │ +Cal3_S2 between(const Cal3_S2 &q, OptionalJacobian< 5, 5 > H1=boost::none, │ │ │ │ │ +OptionalJacobian< 5, 5 > H2=boost::none) const │ │ │ │ │ +"Between", subtracts calibrations. between(p,q) == compose(inverse(p),q) │ │ │ │ │ +DDeeffiinniittiioonn Cal3_S2.h:104 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3___S_2_:_:_D_i_m │ │ │ │ │ +static size_t Dim() │ │ │ │ │ +return DOF, dimensionality of tangent space │ │ │ │ │ +DDeeffiinniittiioonn Cal3_S2.h:118 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3___S_2_:_:_C_a_l_3___S_2 │ │ │ │ │ +Cal3_S2(const Vector5 &d) │ │ │ │ │ +constructor from vector │ │ │ │ │ +DDeeffiinniittiioonn Cal3_S2.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3___S_2_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ +Vector5 localCoordinates(const Cal3_S2 &T2) const │ │ │ │ │ +Unretraction for the calibration. │ │ │ │ │ +DDeeffiinniittiioonn Cal3_S2.h:126 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3___S_2_:_:_C_a_l_3___S_2 │ │ │ │ │ +Cal3_S2(double fov, int w, int h) │ │ │ │ │ +Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect. │ │ │ │ │ +DDeeffiinniittiioonn Cal3_S2.h:60 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3___S_2_:_:_r_e_t_r_a_c_t │ │ │ │ │ +Cal3_S2 retract(const Vector &d) const │ │ │ │ │ +Given 5-dim tangent vector, create new calibration. │ │ │ │ │ +DDeeffiinniittiioonn Cal3_S2.h:121 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _P_o_s_e_2_._h │ │ │ │ │ + * _C_a_l_3___S_2_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00401.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/StereoPoint2.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Fisheye.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,55 +95,50 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs
│ │ │ │ -
StereoPoint2.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
Cal3Fisheye.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

A 2D stereo point (uL,uR,v) │ │ │ │ +

Calibration of a fisheye camera. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::StereoPoint2
 A 2D stereo point, v will be same for rectified images. More...
class  gtsam::Cal3Fisheye
 Calibration of a fisheye camera. More...
 
struct  gtsam::traits< StereoPoint2 >
struct  gtsam::traits< Cal3Fisheye >
 
struct  gtsam::traits< const StereoPoint2 >
struct  gtsam::traits< const Cal3Fisheye >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Typedefs

│ │ │ │ -typedef std::vector< StereoPoint2 > gtsam::StereoPoint2Vector
 
│ │ │ │

Detailed Description

│ │ │ │ -

A 2D stereo point (uL,uR,v)

│ │ │ │ -
Date
Jan 26, 2010
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +

Calibration of a fisheye camera.

│ │ │ │ +
Date
Apr 8, 2020
│ │ │ │ +
Author
ghaggin
│ │ │ │ +
│ │ │ │ +Varun Agrawal
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,34 +1,32 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ -StereoPoint2.h File Reference │ │ │ │ │ -A 2D stereo point (uL,uR,v) _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +Cal3Fisheye.h File Reference │ │ │ │ │ +Calibration of a fisheye camera. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2 │ │ │ │ │ -  A 2D stereo point, v will be same for rectified images. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_C_a_l_3_F_i_s_h_e_y_e │ │ │ │ │ +  Calibration of a fisheye camera. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_t_e_r_e_o_P_o_i_n_t_2_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _C_a_l_3_F_i_s_h_e_y_e_ _> │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _S_t_e_r_e_o_P_o_i_n_t_2_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _C_a_l_3_F_i_s_h_e_y_e_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -typedef std::vector< _S_t_e_r_e_o_P_o_i_n_t_2 >  ggttssaamm::::SStteerreeooPPooiinntt22VVeeccttoorr │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A 2D stereo point (uL,uR,v) │ │ │ │ │ +Calibration of a fisheye camera. │ │ │ │ │ Date │ │ │ │ │ - Jan 26, 2010 │ │ │ │ │ + Apr 8, 2020 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + ghaggin │ │ │ │ │ + Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_t_e_r_e_o_P_o_i_n_t_2_._h │ │ │ │ │ + * _C_a_l_3_F_i_s_h_e_y_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00401.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a00401 = [ │ │ │ │ │ - ["gtsam::traits< StereoPoint2 >", "a03356.html", null], │ │ │ │ │ - ["gtsam::traits< const StereoPoint2 >", "a03360.html", null] │ │ │ │ │ + ["gtsam::traits< Cal3Fisheye >", "a03000.html", null], │ │ │ │ │ + ["gtsam::traits< const Cal3Fisheye >", "a03004.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00401_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/StereoPoint2.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Fisheye.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,187 +98,177 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
StereoPoint2.h
│ │ │ │ +
Cal3Fisheye.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/geometry/Point2.h>
│ │ │ │ -
22#include <gtsam/base/VectorSpace.h>
│ │ │ │ -
23#include <boost/serialization/nvp.hpp>
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/geometry/Cal3.h>
│ │ │ │ +
23#include <gtsam/geometry/Point2.h>
│ │ │ │
24
│ │ │ │ -
25namespace gtsam {
│ │ │ │ +
25#include <boost/shared_ptr.hpp>
│ │ │ │
26
│ │ │ │ -
│ │ │ │ -
32class GTSAM_EXPORT StereoPoint2 {
│ │ │ │ -
33private:
│ │ │ │ -
34
│ │ │ │ -
35 double uL_, uR_, v_;
│ │ │ │ -
36
│ │ │ │ -
37public:
│ │ │ │ -
38 enum { dimension = 3 };
│ │ │ │ -
41
│ │ │ │ -
│ │ │ │ -
43 StereoPoint2() :
│ │ │ │ -
44 uL_(0), uR_(0), v_(0) {
│ │ │ │ -
45 }
│ │ │ │ -
│ │ │ │ -
46
│ │ │ │ -
│ │ │ │ -
48 StereoPoint2(double uL, double uR, double v) :
│ │ │ │ -
49 uL_(uL), uR_(uR), v_(v) {
│ │ │ │ -
50 }
│ │ │ │ -
│ │ │ │ -
51
│ │ │ │ -
│ │ │ │ -
53 explicit StereoPoint2(const Vector3& v) :
│ │ │ │ -
54 uL_(v(0)), uR_(v(1)), v_(v(2)) {}
│ │ │ │ -
│ │ │ │ -
55
│ │ │ │ -
59
│ │ │ │ -
61 void print(const std::string& s = "") const;
│ │ │ │ -
62
│ │ │ │ -
│ │ │ │ -
64 bool equals(const StereoPoint2& q, double tol = 1e-9) const {
│ │ │ │ -
65 return (std::abs(uL_ - q.uL_) < tol && std::abs(uR_ - q.uR_) < tol
│ │ │ │ -
66 && std::abs(v_ - q.v_) < tol);
│ │ │ │ -
67 }
│ │ │ │ -
│ │ │ │ -
68
│ │ │ │ -
72
│ │ │ │ -
│ │ │ │ -
74 inline static StereoPoint2 Identity() {
│ │ │ │ -
75 return StereoPoint2();
│ │ │ │ -
76 }
│ │ │ │ -
│ │ │ │ +
27#include <string>
│ │ │ │ +
28
│ │ │ │ +
29namespace gtsam {
│ │ │ │ +
30
│ │ │ │ +
│ │ │ │ +
51class GTSAM_EXPORT Cal3Fisheye : public Cal3 {
│ │ │ │ +
52 private:
│ │ │ │ +
53 double k1_ = 0.0f, k2_ = 0.0f;
│ │ │ │ +
54 double k3_ = 0.0f, k4_ = 0.0f;
│ │ │ │ +
55 double tol_ = 1e-5;
│ │ │ │ +
56
│ │ │ │ +
57 public:
│ │ │ │ +
58 enum { dimension = 9 };
│ │ │ │ +
60 using shared_ptr = boost::shared_ptr<Cal3Fisheye>;
│ │ │ │ +
61
│ │ │ │ +
64
│ │ │ │ +
66 Cal3Fisheye() = default;
│ │ │ │ +
67
│ │ │ │ +
68 Cal3Fisheye(const double fx, const double fy, const double s, const double u0,
│ │ │ │ +
69 const double v0, const double k1, const double k2,
│ │ │ │ +
70 const double k3, const double k4, double tol = 1e-5)
│ │ │ │ +
71 : Cal3(fx, fy, s, u0, v0),
│ │ │ │ +
72 k1_(k1),
│ │ │ │ +
73 k2_(k2),
│ │ │ │ +
74 k3_(k3),
│ │ │ │ +
75 k4_(k4),
│ │ │ │ +
76 tol_(tol) {}
│ │ │ │
77
│ │ │ │ -
│ │ │ │ -
79 StereoPoint2 operator-() const {
│ │ │ │ -
80 return StereoPoint2(-uL_, -uR_, -v_);
│ │ │ │ -
81 }
│ │ │ │ -
│ │ │ │ -
82
│ │ │ │ -
│ │ │ │ -
84 inline StereoPoint2 operator +(const Vector3& v) const {
│ │ │ │ -
85 return StereoPoint2(uL_ + v[0], uR_ + v[1], v_ + v[2]);
│ │ │ │ -
86 }
│ │ │ │ -
│ │ │ │ -
87
│ │ │ │ -
│ │ │ │ -
89 inline StereoPoint2 operator +(const StereoPoint2& b) const {
│ │ │ │ -
90 return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_);
│ │ │ │ -
91 }
│ │ │ │ -
│ │ │ │ -
92
│ │ │ │ -
│ │ │ │ -
94 inline StereoPoint2 operator -(const StereoPoint2& b) const {
│ │ │ │ -
95 return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_);
│ │ │ │ -
96 }
│ │ │ │ -
│ │ │ │ +
78 ~Cal3Fisheye() override {}
│ │ │ │ +
79
│ │ │ │ +
83
│ │ │ │ +
84 explicit Cal3Fisheye(const Vector9& v)
│ │ │ │ +
85 : Cal3(v(0), v(1), v(2), v(3), v(4)),
│ │ │ │ +
86 k1_(v(5)),
│ │ │ │ +
87 k2_(v(6)),
│ │ │ │ +
88 k3_(v(7)),
│ │ │ │ +
89 k4_(v(8)) {}
│ │ │ │ +
90
│ │ │ │ +
94
│ │ │ │ +
96 inline double k1() const { return k1_; }
│ │ │ │
97
│ │ │ │ -
101
│ │ │ │ -
103 inline bool operator ==(const StereoPoint2& q) const {return uL_== q.uL_ && uR_==q.uR_ && v_ == q.v_;}
│ │ │ │ -
104
│ │ │ │ -
106 inline double uL() const {return uL_;}
│ │ │ │ -
107
│ │ │ │ -
109 inline double uR() const {return uR_;}
│ │ │ │ -
110
│ │ │ │ -
112 inline double v() const {return v_;}
│ │ │ │ -
113
│ │ │ │ -
│ │ │ │ -
115 Vector3 vector() const {
│ │ │ │ -
116 return Vector3(uL_, uR_, v_);
│ │ │ │ -
117 }
│ │ │ │ -
│ │ │ │ -
118
│ │ │ │ -
│ │ │ │ -
120 Point2 point2() const {
│ │ │ │ -
121 return Point2(uL_, v_);
│ │ │ │ -
122 }
│ │ │ │ -
│ │ │ │ -
123
│ │ │ │ -
│ │ │ │ -
125 Point2 right() const {
│ │ │ │ -
126 return Point2(uR_, v_);
│ │ │ │ -
127 }
│ │ │ │ -
│ │ │ │ -
128
│ │ │ │ -
131 inline StereoPoint2 inverse() const { return StereoPoint2()- (*this);}
│ │ │ │ -
132 inline StereoPoint2 compose(const StereoPoint2& p1) const { return *this + p1;}
│ │ │ │ -
133 inline StereoPoint2 between(const StereoPoint2& p2) const { return p2 - *this; }
│ │ │ │ -
134 inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); }
│ │ │ │ -
135 inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); }
│ │ │ │ -
136 static inline Vector Logmap(const StereoPoint2& p) { return p.vector(); }
│ │ │ │ -
137 static inline StereoPoint2 Expmap(const Vector& d) { return StereoPoint2(d(0), d(1), d(2)); }
│ │ │ │ -
139
│ │ │ │ -
141 GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const StereoPoint2& p);
│ │ │ │ -
142
│ │ │ │ -
143private:
│ │ │ │ -
144
│ │ │ │ +
99 inline double k2() const { return k2_; }
│ │ │ │ +
100
│ │ │ │ +
102 inline double k3() const { return k3_; }
│ │ │ │ +
103
│ │ │ │ +
105 inline double k4() const { return k4_; }
│ │ │ │ +
106
│ │ │ │ +
108 Vector4 k() const { return Vector4(k1_, k2_, k3_, k4_); }
│ │ │ │ +
109
│ │ │ │ +
111 Vector9 vector() const;
│ │ │ │ +
112
│ │ │ │ +
114 static double Scaling(double r);
│ │ │ │ +
115
│ │ │ │ +
124 Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
│ │ │ │ +
125 OptionalJacobian<2, 2> Dp = boost::none) const;
│ │ │ │ +
126
│ │ │ │ +
135 Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
│ │ │ │ +
136 OptionalJacobian<2, 2> Dp = boost::none) const;
│ │ │ │ +
137
│ │ │ │ +
141
│ │ │ │ +
143 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
│ │ │ │ +
144 const Cal3Fisheye& cal);
│ │ │ │ +
145
│ │ │ │ +
147 void print(const std::string& s = "") const override;
│ │ │ │
148
│ │ │ │ -
150 friend class boost::serialization::access;
│ │ │ │ -
151 template<class ARCHIVE>
│ │ │ │ -
152 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
153 ar & BOOST_SERIALIZATION_NVP(uL_);
│ │ │ │ -
154 ar & BOOST_SERIALIZATION_NVP(uR_);
│ │ │ │ -
155 ar & BOOST_SERIALIZATION_NVP(v_);
│ │ │ │ -
156 }
│ │ │ │ -
157
│ │ │ │ -
159
│ │ │ │ -
160};
│ │ │ │ -
│ │ │ │ +
150 bool equals(const Cal3Fisheye& K, double tol = 10e-9) const;
│ │ │ │ +
151
│ │ │ │ +
155
│ │ │ │ +
157 size_t dim() const override { return Dim(); }
│ │ │ │ +
158
│ │ │ │ +
160 inline static size_t Dim() { return dimension; }
│ │ │ │
161
│ │ │ │ -
162typedef std::vector<StereoPoint2> StereoPoint2Vector;
│ │ │ │ -
163
│ │ │ │ -
164template<>
│ │ │ │ -
165struct traits<StereoPoint2> : public internal::VectorSpace<StereoPoint2> {};
│ │ │ │ +
│ │ │ │ +
163 inline Cal3Fisheye retract(const Vector& d) const {
│ │ │ │ +
164 return Cal3Fisheye(vector() + d);
│ │ │ │ +
165 }
│ │ │ │ +
│ │ │ │
166
│ │ │ │ -
167template<>
│ │ │ │ -
168struct traits<const StereoPoint2> : public internal::VectorSpace<StereoPoint2> {};
│ │ │ │ -
169}
│ │ │ │ -
gtsam::testing::between
T between(const T &t1, const T &t2)
binary functions
Definition lieProxies.h:36
│ │ │ │ -
Point2.h
2D Point
│ │ │ │ +
│ │ │ │ +
168 Vector localCoordinates(const Cal3Fisheye& T2) const {
│ │ │ │ +
169 return T2.vector() - vector();
│ │ │ │ +
170 }
│ │ │ │ +
│ │ │ │ +
171
│ │ │ │ +
175
│ │ │ │ +
│ │ │ │ +
177 virtual boost::shared_ptr<Cal3Fisheye> clone() const {
│ │ │ │ +
178 return boost::shared_ptr<Cal3Fisheye>(new Cal3Fisheye(*this));
│ │ │ │ +
179 }
│ │ │ │ +
│ │ │ │ +
180
│ │ │ │ +
182
│ │ │ │ +
183 private:
│ │ │ │ +
186
│ │ │ │ +
188 friend class boost::serialization::access;
│ │ │ │ +
189 template <class Archive>
│ │ │ │ +
190 void serialize(Archive& ar, const unsigned int /*version*/) {
│ │ │ │ +
191 ar& boost::serialization::make_nvp(
│ │ │ │ +
192 "Cal3Fisheye", boost::serialization::base_object<Cal3>(*this));
│ │ │ │ +
193 ar& BOOST_SERIALIZATION_NVP(k1_);
│ │ │ │ +
194 ar& BOOST_SERIALIZATION_NVP(k2_);
│ │ │ │ +
195 ar& BOOST_SERIALIZATION_NVP(k3_);
│ │ │ │ +
196 ar& BOOST_SERIALIZATION_NVP(k4_);
│ │ │ │ +
197 }
│ │ │ │ +
198
│ │ │ │ +
200};
│ │ │ │ +
│ │ │ │ +
201
│ │ │ │ +
202template <>
│ │ │ │ +
203struct traits<Cal3Fisheye> : public internal::Manifold<Cal3Fisheye> {};
│ │ │ │ +
204
│ │ │ │ +
205template <>
│ │ │ │ +
206struct traits<const Cal3Fisheye> : public internal::Manifold<Cal3Fisheye> {};
│ │ │ │ +
207
│ │ │ │ +
208} // namespace gtsam
│ │ │ │ +
Point2.h
2D Point
│ │ │ │ +
Cal3.h
Common code for all Calibration models.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::internal::VectorSpace
VectorSpace provides both Testable and VectorSpaceTraits.
Definition VectorSpace.h:207
│ │ │ │ -
gtsam::StereoPoint2
A 2D stereo point, v will be same for rectified images.
Definition StereoPoint2.h:32
│ │ │ │ -
gtsam::StereoPoint2::uR
double uR() const
get uR
Definition StereoPoint2.h:109
│ │ │ │ -
gtsam::StereoPoint2::StereoPoint2
StereoPoint2(const Vector3 &v)
construct from 3D vector
Definition StereoPoint2.h:53
│ │ │ │ -
gtsam::StereoPoint2::right
Point2 right() const
convenient function to get a Point2 from the right image
Definition StereoPoint2.h:125
│ │ │ │ -
gtsam::StereoPoint2::vector
Vector3 vector() const
convert to vector
Definition StereoPoint2.h:115
│ │ │ │ -
gtsam::StereoPoint2::uL
double uL() const
get uL
Definition StereoPoint2.h:106
│ │ │ │ -
gtsam::StereoPoint2::StereoPoint2
StereoPoint2(double uL, double uR, double v)
constructor
Definition StereoPoint2.h:48
│ │ │ │ -
gtsam::StereoPoint2::equals
bool equals(const StereoPoint2 &q, double tol=1e-9) const
equals
Definition StereoPoint2.h:64
│ │ │ │ -
gtsam::StereoPoint2::operator-
StereoPoint2 operator-() const
inverse
Definition StereoPoint2.h:79
│ │ │ │ -
gtsam::StereoPoint2::v
double v() const
get v
Definition StereoPoint2.h:112
│ │ │ │ -
gtsam::StereoPoint2::point2
Point2 point2() const
convenient function to get a Point2 from the left image
Definition StereoPoint2.h:120
│ │ │ │ -
gtsam::StereoPoint2::Identity
static StereoPoint2 Identity()
identity
Definition StereoPoint2.h:74
│ │ │ │ -
gtsam::StereoPoint2::StereoPoint2
StereoPoint2()
default constructor
Definition StereoPoint2.h:43
│ │ │ │ +
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
│ │ │ │ +
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::Cal3
Common base class for all calibration models.
Definition Cal3.h:69
│ │ │ │ +
gtsam::Cal3Fisheye
Calibration of a fisheye camera.
Definition Cal3Fisheye.h:51
│ │ │ │ +
gtsam::Cal3Fisheye::k4
double k4() const
Second tangential distortion coefficient.
Definition Cal3Fisheye.h:105
│ │ │ │ +
gtsam::Cal3Fisheye::k1
double k1() const
First distortion coefficient.
Definition Cal3Fisheye.h:96
│ │ │ │ +
gtsam::Cal3Fisheye::k3
double k3() const
First tangential distortion coefficient.
Definition Cal3Fisheye.h:102
│ │ │ │ +
gtsam::Cal3Fisheye::localCoordinates
Vector localCoordinates(const Cal3Fisheye &T2) const
Given a different calibration, calculate update to obtain it.
Definition Cal3Fisheye.h:168
│ │ │ │ +
gtsam::Cal3Fisheye::Dim
static size_t Dim()
Return dimensions of calibration manifold object.
Definition Cal3Fisheye.h:160
│ │ │ │ +
gtsam::Cal3Fisheye::k2
double k2() const
Second distortion coefficient.
Definition Cal3Fisheye.h:99
│ │ │ │ +
gtsam::Cal3Fisheye::dim
size_t dim() const override
Return dimensions of calibration manifold object.
Definition Cal3Fisheye.h:157
│ │ │ │ +
gtsam::Cal3Fisheye::vector
Vector9 vector() const
Return all parameters as a vector.
Definition Cal3Fisheye.cpp:28
│ │ │ │ +
gtsam::Cal3Fisheye::clone
virtual boost::shared_ptr< Cal3Fisheye > clone() const
Definition Cal3Fisheye.h:177
│ │ │ │ +
gtsam::Cal3Fisheye::k
Vector4 k() const
return distortion parameter vector
Definition Cal3Fisheye.h:108
│ │ │ │ +
gtsam::Cal3Fisheye::Cal3Fisheye
Cal3Fisheye()=default
Default Constructor with only unit focal length.
│ │ │ │ +
gtsam::Cal3Fisheye::retract
Cal3Fisheye retract(const Vector &d) const
Given delta vector, update calibration.
Definition Cal3Fisheye.h:163
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,150 +1,149 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -StereoPoint2.h │ │ │ │ │ +Cal3Fisheye.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ -22#include │ │ │ │ │ -23#include │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ 24 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ +25#include │ │ │ │ │ 26 │ │ │ │ │ -_3_2class GTSAM_EXPORT _S_t_e_r_e_o_P_o_i_n_t_2 { │ │ │ │ │ -33private: │ │ │ │ │ -34 │ │ │ │ │ -35 double uL_, uR_, v_; │ │ │ │ │ -36 │ │ │ │ │ -37public: │ │ │ │ │ -38 enum { dimension = 3 }; │ │ │ │ │ -41 │ │ │ │ │ -_4_3 _S_t_e_r_e_o_P_o_i_n_t_2() : │ │ │ │ │ -44 uL_(0), uR_(0), v_(0) { │ │ │ │ │ -45 } │ │ │ │ │ -46 │ │ │ │ │ -_4_8 _S_t_e_r_e_o_P_o_i_n_t_2(double uL, double uR, double v) : │ │ │ │ │ -49 uL_(uL), uR_(uR), v_(v) { │ │ │ │ │ -50 } │ │ │ │ │ -51 │ │ │ │ │ -_5_3 explicit _S_t_e_r_e_o_P_o_i_n_t_2(const Vector3& v) : │ │ │ │ │ -54 uL_(v(0)), uR_(v(1)), v_(v(2)) {} │ │ │ │ │ -55 │ │ │ │ │ -59 │ │ │ │ │ -61 void _p_r_i_n_t(const std::string& s = "") const; │ │ │ │ │ -62 │ │ │ │ │ -_6_4 bool _e_q_u_a_l_s(const _S_t_e_r_e_o_P_o_i_n_t_2& q, double tol = 1e-9) const { │ │ │ │ │ -65 return (std::abs(uL_ - q.uL_) < tol && std::abs(uR_ - q.uR_) < tol │ │ │ │ │ -66 && std::abs(v_ - q.v_) < tol); │ │ │ │ │ -67 } │ │ │ │ │ -68 │ │ │ │ │ -72 │ │ │ │ │ -_7_4 inline static _S_t_e_r_e_o_P_o_i_n_t_2 _I_d_e_n_t_i_t_y() { │ │ │ │ │ -75 return _S_t_e_r_e_o_P_o_i_n_t_2(); │ │ │ │ │ -76 } │ │ │ │ │ +27#include │ │ │ │ │ +28 │ │ │ │ │ +29namespace _g_t_s_a_m { │ │ │ │ │ +30 │ │ │ │ │ +_5_1class GTSAM_EXPORT _C_a_l_3_F_i_s_h_e_y_e : public _C_a_l_3 { │ │ │ │ │ +52 private: │ │ │ │ │ +53 double k1_ = 0.0f, k2_ = 0.0f; │ │ │ │ │ +54 double k3_ = 0.0f, k4_ = 0.0f; │ │ │ │ │ +55 double tol_ = 1e-5; │ │ │ │ │ +56 │ │ │ │ │ +57 public: │ │ │ │ │ +58 enum { dimension = 9 }; │ │ │ │ │ +60 using shared_ptr = boost::shared_ptr; │ │ │ │ │ +61 │ │ │ │ │ +64 │ │ │ │ │ +_6_6 _C_a_l_3_F_i_s_h_e_y_e() = default; │ │ │ │ │ +67 │ │ │ │ │ +68 _C_a_l_3_F_i_s_h_e_y_e(const double fx, const double fy, const double s, const double │ │ │ │ │ +u0, │ │ │ │ │ +69 const double v0, const double k1, const double k2, │ │ │ │ │ +70 const double k3, const double k4, double tol = 1e-5) │ │ │ │ │ +71 : _C_a_l_3(fx, fy, s, u0, v0), │ │ │ │ │ +72 k1_(k1), │ │ │ │ │ +73 k2_(k2), │ │ │ │ │ +74 k3_(k3), │ │ │ │ │ +75 k4_(k4), │ │ │ │ │ +76 tol_(tol) {} │ │ │ │ │ 77 │ │ │ │ │ -_7_9 _S_t_e_r_e_o_P_o_i_n_t_2 _o_p_e_r_a_t_o_r_-() const { │ │ │ │ │ -80 return _S_t_e_r_e_o_P_o_i_n_t_2(-uL_, -uR_, -v_); │ │ │ │ │ -81 } │ │ │ │ │ -82 │ │ │ │ │ -_8_4 inline _S_t_e_r_e_o_P_o_i_n_t_2 operator +(const Vector3& v) const { │ │ │ │ │ -85 return _S_t_e_r_e_o_P_o_i_n_t_2(uL_ + v[0], uR_ + v[1], v_ + v[2]); │ │ │ │ │ -86 } │ │ │ │ │ -87 │ │ │ │ │ -_8_9 inline _S_t_e_r_e_o_P_o_i_n_t_2 operator +(const _S_t_e_r_e_o_P_o_i_n_t_2& b) const { │ │ │ │ │ -90 return _S_t_e_r_e_o_P_o_i_n_t_2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_); │ │ │ │ │ -91 } │ │ │ │ │ -92 │ │ │ │ │ -_9_4 inline _S_t_e_r_e_o_P_o_i_n_t_2 operator -(const _S_t_e_r_e_o_P_o_i_n_t_2& b) const { │ │ │ │ │ -95 return _S_t_e_r_e_o_P_o_i_n_t_2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_); │ │ │ │ │ -96 } │ │ │ │ │ +78 _~_C_a_l_3_F_i_s_h_e_y_e() override {} │ │ │ │ │ +79 │ │ │ │ │ +83 │ │ │ │ │ +84 explicit Cal3Fisheye(const Vector9& v) │ │ │ │ │ +85 : Cal3(v(0), v(1), v(2), v(3), v(4)), │ │ │ │ │ +86 k1_(v(5)), │ │ │ │ │ +87 k2_(v(6)), │ │ │ │ │ +88 k3_(v(7)), │ │ │ │ │ +89 k4_(v(8)) {} │ │ │ │ │ +90 │ │ │ │ │ +94 │ │ │ │ │ +_9_6 inline double _k_1() const { return k1_; } │ │ │ │ │ 97 │ │ │ │ │ -101 │ │ │ │ │ -_1_0_3 inline bool operator ==(const _S_t_e_r_e_o_P_o_i_n_t_2& q) const {return uL_== q.uL_ && │ │ │ │ │ -uR_==q.uR_ && v_ == q.v_;} │ │ │ │ │ -104 │ │ │ │ │ -_1_0_6 inline double _u_L() const {return uL_;} │ │ │ │ │ -107 │ │ │ │ │ -_1_0_9 inline double _u_R() const {return uR_;} │ │ │ │ │ -110 │ │ │ │ │ -_1_1_2 inline double _v() const {return v_;} │ │ │ │ │ -113 │ │ │ │ │ -_1_1_5 Vector3 _v_e_c_t_o_r() const { │ │ │ │ │ -116 return Vector3(uL_, uR_, v_); │ │ │ │ │ -117 } │ │ │ │ │ -118 │ │ │ │ │ -_1_2_0 _P_o_i_n_t_2 _p_o_i_n_t_2() const { │ │ │ │ │ -121 return _P_o_i_n_t_2(uL_, v_); │ │ │ │ │ -122 } │ │ │ │ │ -123 │ │ │ │ │ -_1_2_5 _P_o_i_n_t_2 _r_i_g_h_t() const { │ │ │ │ │ -126 return _P_o_i_n_t_2(uR_, v_); │ │ │ │ │ -127 } │ │ │ │ │ -128 │ │ │ │ │ -131 inline _S_t_e_r_e_o_P_o_i_n_t_2 inverse() const { return _S_t_e_r_e_o_P_o_i_n_t_2()- (*this);} │ │ │ │ │ -132 inline StereoPoint2 compose(const StereoPoint2& p1) const { return *this + │ │ │ │ │ -p1;} │ │ │ │ │ -133 inline StereoPoint2 _b_e_t_w_e_e_n(const StereoPoint2& p2) const { return p2 - │ │ │ │ │ -*this; } │ │ │ │ │ -134 inline Vector localCoordinates(const StereoPoint2& t2) const { return │ │ │ │ │ -Logmap(_b_e_t_w_e_e_n(t2)); } │ │ │ │ │ -135 inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap │ │ │ │ │ -(v)); } │ │ │ │ │ -136 static inline Vector Logmap(const StereoPoint2& p) { return p.vector(); } │ │ │ │ │ -137 static inline StereoPoint2 Expmap(const Vector& d) { return StereoPoint2(d │ │ │ │ │ -(0), d(1), d(2)); } │ │ │ │ │ -139 │ │ │ │ │ -141 GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const │ │ │ │ │ -StereoPoint2& p); │ │ │ │ │ -142 │ │ │ │ │ -143private: │ │ │ │ │ -144 │ │ │ │ │ +_9_9 inline double _k_2() const { return k2_; } │ │ │ │ │ +100 │ │ │ │ │ +_1_0_2 inline double _k_3() const { return k3_; } │ │ │ │ │ +103 │ │ │ │ │ +_1_0_5 inline double _k_4() const { return k4_; } │ │ │ │ │ +106 │ │ │ │ │ +_1_0_8 Vector4 _k() const { return Vector4(k1_, k2_, k3_, k4_); } │ │ │ │ │ +109 │ │ │ │ │ +111 Vector9 vector() const; │ │ │ │ │ +112 │ │ │ │ │ +114 static double Scaling(double r); │ │ │ │ │ +115 │ │ │ │ │ +124 _P_o_i_n_t_2 uncalibrate(const _P_o_i_n_t_2& p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _9_> Dcal = boost:: │ │ │ │ │ +none, │ │ │ │ │ +125 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dp = boost::none) const; │ │ │ │ │ +126 │ │ │ │ │ +135 _P_o_i_n_t_2 calibrate(const _P_o_i_n_t_2& p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _9_> Dcal = boost:: │ │ │ │ │ +none, │ │ │ │ │ +136 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dp = boost::none) const; │ │ │ │ │ +137 │ │ │ │ │ +141 │ │ │ │ │ +143 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, │ │ │ │ │ +144 const _C_a_l_3_F_i_s_h_e_y_e& cal); │ │ │ │ │ +145 │ │ │ │ │ +147 void _p_r_i_n_t(const std::string& s = "") const override; │ │ │ │ │ 148 │ │ │ │ │ -_1_5_0 friend class boost::serialization::access; │ │ │ │ │ -151 template │ │ │ │ │ -152 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -153 ar & BOOST_SERIALIZATION_NVP(uL_); │ │ │ │ │ -154 ar & BOOST_SERIALIZATION_NVP(uR_); │ │ │ │ │ -155 ar & BOOST_SERIALIZATION_NVP(v_); │ │ │ │ │ -156 } │ │ │ │ │ -157 │ │ │ │ │ -159 │ │ │ │ │ -160}; │ │ │ │ │ +150 bool _e_q_u_a_l_s(const _C_a_l_3_F_i_s_h_e_y_e& K, double tol = 10e-9) const; │ │ │ │ │ +151 │ │ │ │ │ +155 │ │ │ │ │ +_1_5_7 size_t _d_i_m() const override { return Dim(); } │ │ │ │ │ +158 │ │ │ │ │ +_1_6_0 inline static size_t _D_i_m() { return dimension; } │ │ │ │ │ 161 │ │ │ │ │ -162typedef std::vector StereoPoint2Vector; │ │ │ │ │ -163 │ │ │ │ │ -164template<> │ │ │ │ │ -_1_6_5struct _t_r_a_i_t_s<_S_t_e_r_e_o_P_o_i_n_t_2> : public _i_n_t_e_r_n_a_l_:_:_V_e_c_t_o_r_S_p_a_c_e {}; │ │ │ │ │ +_1_6_3 inline _C_a_l_3_F_i_s_h_e_y_e _r_e_t_r_a_c_t(const Vector& d) const { │ │ │ │ │ +164 return _C_a_l_3_F_i_s_h_e_y_e(vector() + d); │ │ │ │ │ +165 } │ │ │ │ │ 166 │ │ │ │ │ -167template<> │ │ │ │ │ -_1_6_8struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_: │ │ │ │ │ -_V_e_c_t_o_r_S_p_a_c_e {}; │ │ │ │ │ -169} │ │ │ │ │ -_g_t_s_a_m_:_:_t_e_s_t_i_n_g_:_:_b_e_t_w_e_e_n │ │ │ │ │ -T between(const T &t1, const T &t2) │ │ │ │ │ -binary functions │ │ │ │ │ -DDeeffiinniittiioonn lieProxies.h:36 │ │ │ │ │ +_1_6_8 Vector _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(const _C_a_l_3_F_i_s_h_e_y_e& T2) const { │ │ │ │ │ +169 return T2._v_e_c_t_o_r() - vector(); │ │ │ │ │ +170 } │ │ │ │ │ +171 │ │ │ │ │ +175 │ │ │ │ │ +_1_7_7 virtual boost::shared_ptr _c_l_o_n_e() const { │ │ │ │ │ +178 return boost::shared_ptr(new _C_a_l_3_F_i_s_h_e_y_e(*this)); │ │ │ │ │ +179 } │ │ │ │ │ +180 │ │ │ │ │ +182 │ │ │ │ │ +183 private: │ │ │ │ │ +186 │ │ │ │ │ +_1_8_8 friend class boost::serialization::access; │ │ │ │ │ +189 template │ │ │ │ │ +190 void serialize(Archive& ar, const unsigned int /*version*/) { │ │ │ │ │ +191 ar& boost::serialization::make_nvp( │ │ │ │ │ +192 "Cal3Fisheye", boost::serialization::base_object(*this)); │ │ │ │ │ +193 ar& BOOST_SERIALIZATION_NVP(k1_); │ │ │ │ │ +194 ar& BOOST_SERIALIZATION_NVP(k2_); │ │ │ │ │ +195 ar& BOOST_SERIALIZATION_NVP(k3_); │ │ │ │ │ +196 ar& BOOST_SERIALIZATION_NVP(k4_); │ │ │ │ │ +197 } │ │ │ │ │ +198 │ │ │ │ │ +200}; │ │ │ │ │ +201 │ │ │ │ │ +202template <> │ │ │ │ │ +_2_0_3struct _t_r_a_i_t_s<_C_a_l_3_F_i_s_h_e_y_e> : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d {}; │ │ │ │ │ +204 │ │ │ │ │ +205template <> │ │ │ │ │ +_2_0_6struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ +{}; │ │ │ │ │ +207 │ │ │ │ │ +208} // namespace gtsam │ │ │ │ │ _P_o_i_n_t_2_._h │ │ │ │ │ 2D Point │ │ │ │ │ +_C_a_l_3_._h │ │ │ │ │ +Common code for all Calibration models. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ print without optional string, must specify cout yourself │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ @@ -153,65 +152,73 @@ │ │ │ │ │ As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ typedef Point2 to Vector2... │ │ │ │ │ DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_V_e_c_t_o_r_S_p_a_c_e │ │ │ │ │ -VectorSpace provides both Testable and VectorSpaceTraits. │ │ │ │ │ -DDeeffiinniittiioonn VectorSpace.h:207 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2 │ │ │ │ │ -A 2D stereo point, v will be same for rectified images. │ │ │ │ │ -DDeeffiinniittiioonn StereoPoint2.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_u_R │ │ │ │ │ -double uR() const │ │ │ │ │ -get uR │ │ │ │ │ -DDeeffiinniittiioonn StereoPoint2.h:109 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_S_t_e_r_e_o_P_o_i_n_t_2 │ │ │ │ │ -StereoPoint2(const Vector3 &v) │ │ │ │ │ -construct from 3D vector │ │ │ │ │ -DDeeffiinniittiioonn StereoPoint2.h:53 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_r_i_g_h_t │ │ │ │ │ -Point2 right() const │ │ │ │ │ -convenient function to get a Point2 from the right image │ │ │ │ │ -DDeeffiinniittiioonn StereoPoint2.h:125 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_v_e_c_t_o_r │ │ │ │ │ -Vector3 vector() const │ │ │ │ │ -convert to vector │ │ │ │ │ -DDeeffiinniittiioonn StereoPoint2.h:115 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_u_L │ │ │ │ │ -double uL() const │ │ │ │ │ -get uL │ │ │ │ │ -DDeeffiinniittiioonn StereoPoint2.h:106 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_S_t_e_r_e_o_P_o_i_n_t_2 │ │ │ │ │ -StereoPoint2(double uL, double uR, double v) │ │ │ │ │ -constructor │ │ │ │ │ -DDeeffiinniittiioonn StereoPoint2.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const StereoPoint2 &q, double tol=1e-9) const │ │ │ │ │ -equals │ │ │ │ │ -DDeeffiinniittiioonn StereoPoint2.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_o_p_e_r_a_t_o_r_- │ │ │ │ │ -StereoPoint2 operator-() const │ │ │ │ │ -inverse │ │ │ │ │ -DDeeffiinniittiioonn StereoPoint2.h:79 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_v │ │ │ │ │ -double v() const │ │ │ │ │ -get v │ │ │ │ │ -DDeeffiinniittiioonn StereoPoint2.h:112 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_p_o_i_n_t_2 │ │ │ │ │ -Point2 point2() const │ │ │ │ │ -convenient function to get a Point2 from the left image │ │ │ │ │ -DDeeffiinniittiioonn StereoPoint2.h:120 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_I_d_e_n_t_i_t_y │ │ │ │ │ -static StereoPoint2 Identity() │ │ │ │ │ -identity │ │ │ │ │ -DDeeffiinniittiioonn StereoPoint2.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_S_t_e_r_e_o_P_o_i_n_t_2 │ │ │ │ │ -StereoPoint2() │ │ │ │ │ -default constructor │ │ │ │ │ -DDeeffiinniittiioonn StereoPoint2.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ +Both ManifoldTraits and Testable. │ │ │ │ │ +DDeeffiinniittiioonn Manifold.h:120 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ +either a fixed size o... │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3 │ │ │ │ │ +Common base class for all calibration models. │ │ │ │ │ +DDeeffiinniittiioonn Cal3.h:69 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_F_i_s_h_e_y_e │ │ │ │ │ +Calibration of a fisheye camera. │ │ │ │ │ +DDeeffiinniittiioonn Cal3Fisheye.h:51 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_F_i_s_h_e_y_e_:_:_k_4 │ │ │ │ │ +double k4() const │ │ │ │ │ +Second tangential distortion coefficient. │ │ │ │ │ +DDeeffiinniittiioonn Cal3Fisheye.h:105 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_F_i_s_h_e_y_e_:_:_k_1 │ │ │ │ │ +double k1() const │ │ │ │ │ +First distortion coefficient. │ │ │ │ │ +DDeeffiinniittiioonn Cal3Fisheye.h:96 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_F_i_s_h_e_y_e_:_:_k_3 │ │ │ │ │ +double k3() const │ │ │ │ │ +First tangential distortion coefficient. │ │ │ │ │ +DDeeffiinniittiioonn Cal3Fisheye.h:102 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_F_i_s_h_e_y_e_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ +Vector localCoordinates(const Cal3Fisheye &T2) const │ │ │ │ │ +Given a different calibration, calculate update to obtain it. │ │ │ │ │ +DDeeffiinniittiioonn Cal3Fisheye.h:168 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_F_i_s_h_e_y_e_:_:_D_i_m │ │ │ │ │ +static size_t Dim() │ │ │ │ │ +Return dimensions of calibration manifold object. │ │ │ │ │ +DDeeffiinniittiioonn Cal3Fisheye.h:160 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_F_i_s_h_e_y_e_:_:_k_2 │ │ │ │ │ +double k2() const │ │ │ │ │ +Second distortion coefficient. │ │ │ │ │ +DDeeffiinniittiioonn Cal3Fisheye.h:99 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_F_i_s_h_e_y_e_:_:_d_i_m │ │ │ │ │ +size_t dim() const override │ │ │ │ │ +Return dimensions of calibration manifold object. │ │ │ │ │ +DDeeffiinniittiioonn Cal3Fisheye.h:157 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_F_i_s_h_e_y_e_:_:_v_e_c_t_o_r │ │ │ │ │ +Vector9 vector() const │ │ │ │ │ +Return all parameters as a vector. │ │ │ │ │ +DDeeffiinniittiioonn Cal3Fisheye.cpp:28 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_F_i_s_h_e_y_e_:_:_c_l_o_n_e │ │ │ │ │ +virtual boost::shared_ptr< Cal3Fisheye > clone() const │ │ │ │ │ +DDeeffiinniittiioonn Cal3Fisheye.h:177 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_F_i_s_h_e_y_e_:_:_k │ │ │ │ │ +Vector4 k() const │ │ │ │ │ +return distortion parameter vector │ │ │ │ │ +DDeeffiinniittiioonn Cal3Fisheye.h:108 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_F_i_s_h_e_y_e_:_:_C_a_l_3_F_i_s_h_e_y_e │ │ │ │ │ +Cal3Fisheye()=default │ │ │ │ │ +Default Constructor with only unit focal length. │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_F_i_s_h_e_y_e_:_:_r_e_t_r_a_c_t │ │ │ │ │ +Cal3Fisheye retract(const Vector &d) const │ │ │ │ │ +Given delta vector, update calibration. │ │ │ │ │ +DDeeffiinniittiioonn Cal3Fisheye.h:163 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_t_e_r_e_o_P_o_i_n_t_2_._h │ │ │ │ │ + * _C_a_l_3_F_i_s_h_e_y_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00404.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3DS2.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/PinholePose.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,52 +96,52 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
Cal3DS2.h File Reference
│ │ │ │ +
PinholePose.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base. │ │ │ │ +

Pinhole camera with known calibration. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::Cal3DS2
 Calibration of a camera with radial distortion that also supports Lie-group behaviors for optimization. More...
class  gtsam::PinholeBaseK< CALIBRATION >
 A pinhole camera class that has a Pose3 and a fixed Calibration. More...
 
struct  gtsam::traits< Cal3DS2 >
class  gtsam::PinholePose< CALIBRATION >
 A pinhole camera class that has a Pose3 and a fixed Calibration. More...
 
struct  gtsam::traits< const Cal3DS2 >
struct  gtsam::traits< PinholePose< CALIBRATION > >
 
struct  gtsam::traits< const PinholePose< CALIBRATION > >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base.

│ │ │ │ -

Calibration of a camera with radial distortion.

│ │ │ │ -
Date
Feb 28, 2010
│ │ │ │ -
Author
ydjian @autho Varun Agrawal
│ │ │ │ -
Date
Feb 28, 2010
│ │ │ │ -
Author
ydjian
│ │ │ │ +

Pinhole camera with known calibration.

│ │ │ │ +
Author
Yong-Dian Jian
│ │ │ │
│ │ │ │ -Varun Agrawal
│ │ │ │ +Frank Dellaert
│ │ │ │ +
Date
Feb 20, 2015
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,40 +1,37 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Cal3DS2.h File Reference │ │ │ │ │ -Calibration of a camera with radial distortion, calculations in base class │ │ │ │ │ -Cal3DS2_Base. _M_o_r_e_._._. │ │ │ │ │ +PinholePose.h File Reference │ │ │ │ │ +Pinhole camera with known calibration. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_C_a_l_3_D_S_2 │ │ │ │ │ -  Calibration of a camera with radial distortion that also supports Lie- │ │ │ │ │ - group behaviors for optimization. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_<_ _C_A_L_I_B_R_A_T_I_O_N_ _> │ │ │ │ │ +  A pinhole camera class that has a _P_o_s_e_3 and a ffiixxeedd Calibration. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _C_a_l_3_D_S_2_ _> │ │ │ │ │ + class   _g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_<_ _C_A_L_I_B_R_A_T_I_O_N_ _> │ │ │ │ │ +  A pinhole camera class that has a _P_o_s_e_3 and a ffiixxeedd Calibration. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _C_a_l_3_D_S_2_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _P_i_n_h_o_l_e_P_o_s_e_<_ _C_A_L_I_B_R_A_T_I_O_N_ _>_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _P_i_n_h_o_l_e_P_o_s_e_<_ _C_A_L_I_B_R_A_T_I_O_N_ _>_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Calibration of a camera with radial distortion, calculations in base class │ │ │ │ │ -Cal3DS2_Base. │ │ │ │ │ -Calibration of a camera with radial distortion. │ │ │ │ │ - Date │ │ │ │ │ - Feb 28, 2010 │ │ │ │ │ +Pinhole camera with known calibration. │ │ │ │ │ Author │ │ │ │ │ - ydjian @autho Varun Agrawal │ │ │ │ │ + Yong-Dian Jian │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - Feb 28, 2010 │ │ │ │ │ - Author │ │ │ │ │ - ydjian │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + Feb 20, 2015 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3_D_S_2_._h │ │ │ │ │ + * _P_i_n_h_o_l_e_P_o_s_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00404.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a00404 = [ │ │ │ │ │ - ["gtsam::traits< Cal3DS2 >", "a02984.html", null], │ │ │ │ │ - ["gtsam::traits< const Cal3DS2 >", "a02988.html", null] │ │ │ │ │ + ["gtsam::traits< PinholePose< CALIBRATION > >", "a03128.html", null], │ │ │ │ │ + ["gtsam::traits< const PinholePose< CALIBRATION > >", "a03132.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00404_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3DS2.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/PinholePose.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,113 +98,503 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Cal3DS2.h
│ │ │ │ +
PinholePose.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
21#pragma once
│ │ │ │ -
22
│ │ │ │ -
23#include <gtsam/geometry/Cal3DS2_Base.h>
│ │ │ │ -
24#include <boost/shared_ptr.hpp>
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/geometry/CalibratedCamera.h>
│ │ │ │ +
23#include <gtsam/geometry/Point2.h>
│ │ │ │ +
24#include <boost/make_shared.hpp>
│ │ │ │
25
│ │ │ │
26namespace gtsam {
│ │ │ │
27
│ │ │ │ -
│ │ │ │ -
35class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
│ │ │ │ -
36 using Base = Cal3DS2_Base;
│ │ │ │ +
33template<typename CALIBRATION>
│ │ │ │ +
│ │ │ │ +
34class PinholeBaseK: public PinholeBase {
│ │ │ │ +
35
│ │ │ │ +
36private:
│ │ │ │
37
│ │ │ │ -
38 public:
│ │ │ │ -
39 enum { dimension = 9 };
│ │ │ │ -
40
│ │ │ │ -
42 using shared_ptr = boost::shared_ptr<Cal3DS2>;
│ │ │ │ -
43
│ │ │ │ +
38 GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
│ │ │ │ +
39
│ │ │ │ +
40 // Get dimensions of calibration type at compile time
│ │ │ │ +
41 static const int DimK = FixedDimension<CALIBRATION>::value;
│ │ │ │ +
42
│ │ │ │ +
43public:
│ │ │ │ +
44
│ │ │ │ +
45 typedef CALIBRATION CalibrationType;
│ │ │ │
46
│ │ │ │ -
48 Cal3DS2() = default;
│ │ │ │
49
│ │ │ │ -
50 Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1,
│ │ │ │ -
51 double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5)
│ │ │ │ -
52 : Base(fx, fy, s, u0, v0, k1, k2, p1, p2, tol) {}
│ │ │ │ +
│ │ │ │ +
51 PinholeBaseK() {
│ │ │ │ +
52 }
│ │ │ │ +
│ │ │ │
53
│ │ │ │ -
54 ~Cal3DS2() override {}
│ │ │ │ -
55
│ │ │ │ -
59
│ │ │ │ -
60 Cal3DS2(const Vector9& v) : Base(v) {}
│ │ │ │ -
61
│ │ │ │ -
65
│ │ │ │ -
67 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
│ │ │ │ -
68 const Cal3DS2& cal);
│ │ │ │ -
69
│ │ │ │ -
71 void print(const std::string& s = "") const override;
│ │ │ │ -
72
│ │ │ │ -
74 bool equals(const Cal3DS2& K, double tol = 10e-9) const;
│ │ │ │ -
75
│ │ │ │ -
79
│ │ │ │ -
81 Cal3DS2 retract(const Vector& d) const;
│ │ │ │ -
82
│ │ │ │ -
84 Vector localCoordinates(const Cal3DS2& T2) const;
│ │ │ │ -
85
│ │ │ │ -
87 size_t dim() const override { return Dim(); }
│ │ │ │ -
88
│ │ │ │ -
90 inline static size_t Dim() { return dimension; }
│ │ │ │ -
91
│ │ │ │ -
95
│ │ │ │ -
│ │ │ │ -
97 boost::shared_ptr<Base> clone() const override {
│ │ │ │ -
98 return boost::shared_ptr<Base>(new Cal3DS2(*this));
│ │ │ │ -
99 }
│ │ │ │ +
│ │ │ │ +
55 explicit PinholeBaseK(const Pose3& pose) :
│ │ │ │ +
56 PinholeBase(pose) {
│ │ │ │ +
57 }
│ │ │ │ +
│ │ │ │ +
58
│ │ │ │ +
62
│ │ │ │ +
63 explicit PinholeBaseK(const Vector &v) :
│ │ │ │ +
64 PinholeBase(v) {
│ │ │ │ +
65 }
│ │ │ │ +
66
│ │ │ │ +
70
│ │ │ │ +
71 virtual ~PinholeBaseK() {
│ │ │ │ +
72 }
│ │ │ │ +
73
│ │ │ │ +
75 virtual const CALIBRATION& calibration() const = 0;
│ │ │ │ +
76
│ │ │ │ +
80
│ │ │ │ +
│ │ │ │ +
82 std::pair<Point2, bool> projectSafe(const Point3& pw) const {
│ │ │ │ +
83 std::pair<Point2, bool> pn = PinholeBase::projectSafe(pw);
│ │ │ │ +
84 pn.first = calibration().uncalibrate(pn.first);
│ │ │ │ +
85 return pn;
│ │ │ │ +
86 }
│ │ │ │
│ │ │ │ -
100
│ │ │ │ +
87
│ │ │ │ +
88
│ │ │ │ +
95 template <class POINT>
│ │ │ │ +
│ │ │ │ +
96 Point2 _project(const POINT& pw, OptionalJacobian<2, 6> Dpose,
│ │ │ │ +
97 OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint,
│ │ │ │ +
98 OptionalJacobian<2, DimK> Dcal) const {
│ │ │ │ +
99
│ │ │ │ +
100 // project to normalized coordinates
│ │ │ │ +
101 const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
│ │ │ │
102
│ │ │ │ -
103 private:
│ │ │ │ -
106
│ │ │ │ -
108 friend class boost::serialization::access;
│ │ │ │ -
109 template <class Archive>
│ │ │ │ -
110 void serialize(Archive& ar, const unsigned int /*version*/) {
│ │ │ │ -
111 ar& boost::serialization::make_nvp(
│ │ │ │ -
112 "Cal3DS2", boost::serialization::base_object<Cal3DS2_Base>(*this));
│ │ │ │ -
113 }
│ │ │ │ -
114
│ │ │ │ -
116};
│ │ │ │ -
│ │ │ │ -
117
│ │ │ │ -
118template <>
│ │ │ │ -
119struct traits<Cal3DS2> : public internal::Manifold<Cal3DS2> {};
│ │ │ │ -
120
│ │ │ │ -
121template <>
│ │ │ │ -
122struct traits<const Cal3DS2> : public internal::Manifold<Cal3DS2> {};
│ │ │ │ -
123}
│ │ │ │ +
103 // uncalibrate to pixel coordinates
│ │ │ │ +
104 Matrix2 Dpi_pn;
│ │ │ │ +
105 const Point2 pi = calibration().uncalibrate(pn, Dcal,
│ │ │ │ +
106 Dpose || Dpoint ? &Dpi_pn : 0);
│ │ │ │ +
107
│ │ │ │ +
108 // If needed, apply chain rule
│ │ │ │ +
109 if (Dpose)
│ │ │ │ +
110 *Dpose = Dpi_pn * *Dpose;
│ │ │ │ +
111 if (Dpoint)
│ │ │ │ +
112 *Dpoint = Dpi_pn * *Dpoint;
│ │ │ │ +
113
│ │ │ │ +
114 return pi;
│ │ │ │ +
115 }
│ │ │ │ +
│ │ │ │ +
116
│ │ │ │ +
│ │ │ │ +
118 Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
│ │ │ │ +
119 OptionalJacobian<2, 3> Dpoint = boost::none,
│ │ │ │ +
120 OptionalJacobian<2, DimK> Dcal = boost::none) const {
│ │ │ │ +
121 return _project(pw, Dpose, Dpoint, Dcal);
│ │ │ │ +
122 }
│ │ │ │ +
│ │ │ │ +
123
│ │ │ │ +
│ │ │ │ +
125 Point2 reprojectionError(const Point3& pw, const Point2& measured, OptionalJacobian<2, 6> Dpose = boost::none,
│ │ │ │ +
126 OptionalJacobian<2, 3> Dpoint = boost::none,
│ │ │ │ +
127 OptionalJacobian<2, DimK> Dcal = boost::none) const {
│ │ │ │ +
128 return Point2(_project(pw, Dpose, Dpoint, Dcal) - measured);
│ │ │ │ +
129 }
│ │ │ │ +
│ │ │ │ +
130
│ │ │ │ +
│ │ │ │ +
132 Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
│ │ │ │ +
133 OptionalJacobian<2, 2> Dpoint = boost::none,
│ │ │ │ +
134 OptionalJacobian<2, DimK> Dcal = boost::none) const {
│ │ │ │ +
135 return _project(pw, Dpose, Dpoint, Dcal);
│ │ │ │ +
136 }
│ │ │ │ +
│ │ │ │ +
137
│ │ │ │ +
│ │ │ │ +
139 Point3 backproject(const Point2& p, double depth,
│ │ │ │ +
140 OptionalJacobian<3, 6> Dresult_dpose = boost::none,
│ │ │ │ +
141 OptionalJacobian<3, 2> Dresult_dp = boost::none,
│ │ │ │ +
142 OptionalJacobian<3, 1> Dresult_ddepth = boost::none,
│ │ │ │ +
143 OptionalJacobian<3, DimK> Dresult_dcal = boost::none) const {
│ │ │ │ +
144 typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
│ │ │ │ +
145 Matrix2K Dpn_dcal;
│ │ │ │ +
146 Matrix22 Dpn_dp;
│ │ │ │ +
147 const Point2 pn = calibration().calibrate(p, Dresult_dcal ? &Dpn_dcal : 0,
│ │ │ │ +
148 Dresult_dp ? &Dpn_dp : 0);
│ │ │ │ +
149 Matrix32 Dpoint_dpn;
│ │ │ │ +
150 Matrix31 Dpoint_ddepth;
│ │ │ │ +
151 const Point3 point = BackprojectFromCamera(pn, depth,
│ │ │ │ +
152 (Dresult_dp || Dresult_dcal) ? &Dpoint_dpn : 0,
│ │ │ │ +
153 Dresult_ddepth ? &Dpoint_ddepth : 0);
│ │ │ │ +
154 Matrix33 Dresult_dpoint;
│ │ │ │ +
155 const Point3 result = pose().transformFrom(point, Dresult_dpose,
│ │ │ │ +
156 (Dresult_ddepth ||
│ │ │ │ +
157 Dresult_dp ||
│ │ │ │ +
158 Dresult_dcal) ? &Dresult_dpoint : 0);
│ │ │ │ +
159 if (Dresult_dcal)
│ │ │ │ +
160 *Dresult_dcal = Dresult_dpoint * Dpoint_dpn * Dpn_dcal; // (3x3)*(3x2)*(2xDimK)
│ │ │ │ +
161 if (Dresult_dp)
│ │ │ │ +
162 *Dresult_dp = Dresult_dpoint * Dpoint_dpn * Dpn_dp; // (3x3)*(3x2)*(2x2)
│ │ │ │ +
163 if (Dresult_ddepth)
│ │ │ │ +
164 *Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth; // (3x3)*(3x1)
│ │ │ │ +
165
│ │ │ │ +
166 return result;
│ │ │ │ +
167 }
│ │ │ │ +
│ │ │ │ +
168
│ │ │ │ +
│ │ │ │ +
170 Unit3 backprojectPointAtInfinity(const Point2& p) const {
│ │ │ │ +
171 const Point2 pn = calibration().calibrate(p);
│ │ │ │ +
172 const Unit3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1
│ │ │ │ +
173 return pose().rotation().rotate(pc);
│ │ │ │ +
174 }
│ │ │ │ +
│ │ │ │ +
175
│ │ │ │ +
│ │ │ │ +
181 double range(const Point3& point,
│ │ │ │ +
182 OptionalJacobian<1, 6> Dcamera = boost::none,
│ │ │ │ +
183 OptionalJacobian<1, 3> Dpoint = boost::none) const {
│ │ │ │ +
184 return pose().range(point, Dcamera, Dpoint);
│ │ │ │ +
185 }
│ │ │ │ +
│ │ │ │ +
186
│ │ │ │ +
│ │ │ │ +
192 double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = boost::none,
│ │ │ │ +
193 OptionalJacobian<1, 6> Dpose = boost::none) const {
│ │ │ │ +
194 return this->pose().range(pose, Dcamera, Dpose);
│ │ │ │ +
195 }
│ │ │ │ +
│ │ │ │ +
196
│ │ │ │ +
│ │ │ │ +
202 double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> Dcamera =
│ │ │ │ +
203 boost::none, OptionalJacobian<1, 6> Dother = boost::none) const {
│ │ │ │ +
204 return pose().range(camera.pose(), Dcamera, Dother);
│ │ │ │ +
205 }
│ │ │ │ +
│ │ │ │ +
206
│ │ │ │ +
212 template<class CalibrationB>
│ │ │ │ +
│ │ │ │ +
213 double range(const PinholeBaseK<CalibrationB>& camera,
│ │ │ │ +
214 OptionalJacobian<1, 6> Dcamera = boost::none,
│ │ │ │ +
215 OptionalJacobian<1, 6> Dother = boost::none) const {
│ │ │ │ +
216 return pose().range(camera.pose(), Dcamera, Dother);
│ │ │ │ +
217 }
│ │ │ │ +
│ │ │ │ +
218
│ │ │ │ +
219private:
│ │ │ │ +
220
│ │ │ │ +
222 friend class boost::serialization::access;
│ │ │ │ +
223 template<class Archive>
│ │ │ │ +
224 void serialize(Archive & ar, const unsigned int /*version*/) {
│ │ │ │ +
225 ar
│ │ │ │ +
226 & boost::serialization::make_nvp("PinholeBase",
│ │ │ │ +
227 boost::serialization::base_object<PinholeBase>(*this));
│ │ │ │ +
228 }
│ │ │ │ +
229
│ │ │ │ +
230public:
│ │ │ │ +
231 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
232};
│ │ │ │ +
│ │ │ │ +
233// end of class PinholeBaseK
│ │ │ │ +
234
│ │ │ │ +
242template<typename CALIBRATION>
│ │ │ │ +
│ │ │ │ +
243class PinholePose: public PinholeBaseK<CALIBRATION> {
│ │ │ │ +
244
│ │ │ │ +
245private:
│ │ │ │ +
246
│ │ │ │ +
247 typedef PinholeBaseK<CALIBRATION> Base;
│ │ │ │ +
248 boost::shared_ptr<CALIBRATION> K_;
│ │ │ │ +
249
│ │ │ │ +
250public:
│ │ │ │ +
251
│ │ │ │ +
252 enum {
│ │ │ │ +
253 dimension = 6
│ │ │ │ +
254 };
│ │ │ │ +
255
│ │ │ │ +
258
│ │ │ │ +
│ │ │ │ +
260 PinholePose() {
│ │ │ │ +
261 }
│ │ │ │ +
│ │ │ │ +
262
│ │ │ │ +
│ │ │ │ +
264 explicit PinholePose(const Pose3& pose) :
│ │ │ │ +
265 Base(pose), K_(new CALIBRATION()) {
│ │ │ │ +
266 }
│ │ │ │ +
│ │ │ │ +
267
│ │ │ │ +
│ │ │ │ +
269 PinholePose(const Pose3& pose, const boost::shared_ptr<CALIBRATION>& K) :
│ │ │ │ +
270 Base(pose), K_(K) {
│ │ │ │ +
271 }
│ │ │ │ +
│ │ │ │ +
272
│ │ │ │ +
276
│ │ │ │ +
│ │ │ │ +
284 static PinholePose Level(const boost::shared_ptr<CALIBRATION>& K,
│ │ │ │ +
285 const Pose2& pose2, double height) {
│ │ │ │ +
286 return PinholePose(Base::LevelPose(pose2, height), K);
│ │ │ │ +
287 }
│ │ │ │ +
│ │ │ │ +
288
│ │ │ │ +
│ │ │ │ +
290 static PinholePose Level(const Pose2& pose2, double height) {
│ │ │ │ +
291 return PinholePose::Level(boost::make_shared<CALIBRATION>(), pose2, height);
│ │ │ │ +
292 }
│ │ │ │ +
│ │ │ │ +
293
│ │ │ │ +
│ │ │ │ +
303 static PinholePose Lookat(const Point3& eye, const Point3& target,
│ │ │ │ +
304 const Point3& upVector, const boost::shared_ptr<CALIBRATION>& K =
│ │ │ │ +
305 boost::make_shared<CALIBRATION>()) {
│ │ │ │ +
306 return PinholePose(Base::LookatPose(eye, target, upVector), K);
│ │ │ │ +
307 }
│ │ │ │ +
│ │ │ │ +
308
│ │ │ │ +
312
│ │ │ │ +
│ │ │ │ +
314 explicit PinholePose(const Vector &v) :
│ │ │ │ +
315 Base(v), K_(new CALIBRATION()) {
│ │ │ │ +
316 }
│ │ │ │ +
│ │ │ │ +
317
│ │ │ │ +
│ │ │ │ +
319 PinholePose(const Vector &v, const Vector &K) :
│ │ │ │ +
320 Base(v), K_(new CALIBRATION(K)) {
│ │ │ │ +
321 }
│ │ │ │ +
│ │ │ │ +
322
│ │ │ │ +
323 // Init from Pose3 and calibration
│ │ │ │ +
324 PinholePose(const Pose3 &pose, const Vector &K) :
│ │ │ │ +
325 Base(pose), K_(new CALIBRATION(K)) {
│ │ │ │ +
326 }
│ │ │ │ +
327
│ │ │ │ +
331
│ │ │ │ +
│ │ │ │ +
333 bool equals(const Base &camera, double tol = 1e-9) const {
│ │ │ │ +
334 const PinholePose* e = dynamic_cast<const PinholePose*>(&camera);
│ │ │ │ +
335 return Base::equals(camera, tol) && K_->equals(e->calibration(), tol);
│ │ │ │ +
336 }
│ │ │ │ +
│ │ │ │ +
337
│ │ │ │ +
│ │ │ │ +
339 friend std::ostream& operator<<(std::ostream &os, const PinholePose& camera) {
│ │ │ │ +
340 os << "{R: " << camera.pose().rotation().rpy().transpose();
│ │ │ │ +
341 os << ", t: " << camera.pose().translation().transpose();
│ │ │ │ +
342 if (!camera.K_) os << ", K: none";
│ │ │ │ +
343 else os << ", K: " << *camera.K_;
│ │ │ │ +
344 os << "}";
│ │ │ │ +
345 return os;
│ │ │ │ +
346 }
│ │ │ │ +
│ │ │ │ +
347
│ │ │ │ +
│ │ │ │ +
349 void print(const std::string& s = "PinholePose") const override {
│ │ │ │ +
350 Base::print(s);
│ │ │ │ +
351 if (!K_)
│ │ │ │ +
352 std::cout << "s No calibration given" << std::endl;
│ │ │ │ +
353 else
│ │ │ │ +
354 K_->print(s + ".calibration");
│ │ │ │ +
355 }
│ │ │ │ +
│ │ │ │ +
356
│ │ │ │ +
360
│ │ │ │ +
361 ~PinholePose() override {
│ │ │ │ +
362 }
│ │ │ │ +
363
│ │ │ │ +
│ │ │ │ +
365 const boost::shared_ptr<CALIBRATION>& sharedCalibration() const {
│ │ │ │ +
366 return K_;
│ │ │ │ +
367 }
│ │ │ │ +
│ │ │ │ +
368
│ │ │ │ +
│ │ │ │ +
370 const CALIBRATION& calibration() const override {
│ │ │ │ +
371 return *K_;
│ │ │ │ +
372 }
│ │ │ │ +
│ │ │ │ +
373
│ │ │ │ +
│ │ │ │ +
379 Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
│ │ │ │ +
380 OptionalJacobian<2, 3> Dpoint = boost::none) const {
│ │ │ │ +
381 return Base::project(pw, Dpose, Dpoint);
│ │ │ │ +
382 }
│ │ │ │ +
│ │ │ │ +
383
│ │ │ │ +
│ │ │ │ +
385 Point2 project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
│ │ │ │ +
386 OptionalJacobian<2, 2> Dpoint = boost::none) const {
│ │ │ │ +
387 return Base::project(pw, Dpose, Dpoint);
│ │ │ │ +
388 }
│ │ │ │ +
│ │ │ │ +
389
│ │ │ │ +
393
│ │ │ │ +
│ │ │ │ +
395 size_t dim() const {
│ │ │ │ +
396 return 6;
│ │ │ │ +
397 }
│ │ │ │ +
│ │ │ │ +
398
│ │ │ │ +
│ │ │ │ +
400 static size_t Dim() {
│ │ │ │ +
401 return 6;
│ │ │ │ +
402 }
│ │ │ │ +
│ │ │ │ +
403
│ │ │ │ +
│ │ │ │ +
405 PinholePose retract(const Vector6& d) const {
│ │ │ │ +
406 return PinholePose(Base::pose().retract(d), K_);
│ │ │ │ +
407 }
│ │ │ │ +
│ │ │ │ +
408
│ │ │ │ +
│ │ │ │ +
410 Vector6 localCoordinates(const PinholePose& p) const {
│ │ │ │ +
411 return Base::pose().localCoordinates(p.Base::pose());
│ │ │ │ +
412 }
│ │ │ │ +
│ │ │ │ +
413
│ │ │ │ +
│ │ │ │ +
415 static PinholePose Identity() {
│ │ │ │ +
416 return PinholePose(); // assumes that the default constructor is valid
│ │ │ │ +
417 }
│ │ │ │ +
│ │ │ │ +
418
│ │ │ │ +
│ │ │ │ +
420 Matrix34 cameraProjectionMatrix() const {
│ │ │ │ +
421 Matrix34 P = Matrix34(PinholeBase::pose().inverse().matrix().block(0, 0, 3, 4));
│ │ │ │ +
422 return K_->K() * P;
│ │ │ │ +
423 }
│ │ │ │ +
│ │ │ │ +
424
│ │ │ │ +
│ │ │ │ +
426 Vector defaultErrorWhenTriangulatingBehindCamera() const {
│ │ │ │ +
427 return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_->fx());;
│ │ │ │ +
428 }
│ │ │ │ +
│ │ │ │ +
430
│ │ │ │ +
431private:
│ │ │ │ +
432
│ │ │ │ +
434 friend class boost::serialization::access;
│ │ │ │ +
435 template<class Archive>
│ │ │ │ +
436 void serialize(Archive & ar, const unsigned int /*version*/) {
│ │ │ │ +
437 ar
│ │ │ │ +
438 & boost::serialization::make_nvp("PinholeBaseK",
│ │ │ │ +
439 boost::serialization::base_object<Base>(*this));
│ │ │ │ +
440 ar & BOOST_SERIALIZATION_NVP(K_);
│ │ │ │ +
441 }
│ │ │ │ +
442
│ │ │ │ +
443public:
│ │ │ │ +
444 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
445};
│ │ │ │ +
│ │ │ │ +
446// end of class PinholePose
│ │ │ │ +
447
│ │ │ │ +
448template<typename CALIBRATION>
│ │ │ │ +
│ │ │ │ +
449struct traits<PinholePose<CALIBRATION> > : public internal::Manifold<
│ │ │ │ +
450 PinholePose<CALIBRATION> > {
│ │ │ │ +
451};
│ │ │ │ +
│ │ │ │ +
452
│ │ │ │ +
453template<typename CALIBRATION>
│ │ │ │ +
│ │ │ │ +
454struct traits<const PinholePose<CALIBRATION> > : public internal::Manifold<
│ │ │ │ +
455 PinholePose<CALIBRATION> > {
│ │ │ │ +
456};
│ │ │ │ +
│ │ │ │ +
457
│ │ │ │ +
458} // \ gtsam
│ │ │ │ +
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ +
Point2.h
2D Point
│ │ │ │ +
CalibratedCamera.h
Calibrated camera for which only pose is unknown.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │ +
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::LieGroup::localCoordinates
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g
Definition Lie.h:136
│ │ │ │
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
│ │ │ │ -
gtsam::Cal3DS2
Calibration of a camera with radial distortion that also supports Lie-group behaviors for optimizatio...
Definition Cal3DS2.h:35
│ │ │ │ -
gtsam::Cal3DS2::dim
size_t dim() const override
Return dimensions of calibration manifold object.
Definition Cal3DS2.h:87
│ │ │ │ -
gtsam::Cal3DS2::clone
boost::shared_ptr< Base > clone() const override
Definition Cal3DS2.h:97
│ │ │ │ -
gtsam::Cal3DS2::Dim
static size_t Dim()
Return dimensions of calibration manifold object.
Definition Cal3DS2.h:90
│ │ │ │ -
gtsam::Cal3DS2::Cal3DS2
Cal3DS2()=default
Default Constructor with only unit focal length.
│ │ │ │ -
gtsam::Cal3DS2_Base
Calibration of a camera with radial distortion.
Definition Cal3DS2_Base.h:42
│ │ │ │ +
gtsam::FixedDimension
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition Manifold.h:164
│ │ │ │ +
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ +
gtsam::PinholeBase
A pinhole camera class that has a Pose3, functions as base class for all pinhole cameras.
Definition CalibratedCamera.h:52
│ │ │ │ +
gtsam::PinholeBase::Dpose
static Matrix26 Dpose(const Point2 &pn, double d)
Calculate Jacobian with respect to pose.
Definition CalibratedCamera.cpp:27
│ │ │ │ +
gtsam::PinholeBase::print
virtual void print(const std::string &s="PinholeBase") const
print
Definition CalibratedCamera.cpp:74
│ │ │ │ +
gtsam::PinholeBase::projectSafe
std::pair< Point2, bool > projectSafe(const Point3 &pw) const
Project a point into the image and check depth.
Definition CalibratedCamera.cpp:109
│ │ │ │ +
gtsam::PinholeBase::pose
const Pose3 & pose() const
return pose, constant version
Definition CalibratedCamera.h:152
│ │ │ │ +
gtsam::PinholeBase::LevelPose
static Pose3 LevelPose(const Pose2 &pose2, double height)
Create a level pose at the given 2D pose and height.
Definition CalibratedCamera.cpp:49
│ │ │ │ +
gtsam::PinholeBase::equals
bool equals(const PinholeBase &camera, double tol=1e-9) const
assert equality up to a tolerance
Definition CalibratedCamera.cpp:69
│ │ │ │ +
gtsam::PinholeBase::Dpoint
static Matrix23 Dpoint(const Point2 &pn, double d, const Matrix3 &Rt)
Calculate Jacobian with respect to point.
Definition CalibratedCamera.cpp:37
│ │ │ │ +
gtsam::PinholeBase::BackprojectFromCamera
static Point3 BackprojectFromCamera(const Point2 &p, const double depth, OptionalJacobian< 3, 2 > Dpoint=boost::none, OptionalJacobian< 3, 1 > Ddepth=boost::none)
backproject a 2-dimensional point to a 3-dimensional point at given depth
Definition CalibratedCamera.cpp:167
│ │ │ │ +
gtsam::PinholeBase::LookatPose
static Pose3 LookatPose(const Point3 &eye, const Point3 &target, const Point3 &upVector)
Create a camera pose at the given eye position looking at a target point in the scene with the specif...
Definition CalibratedCamera.cpp:58
│ │ │ │ +
gtsam::PinholeBase::project2
Point2 project2(const Point3 &point, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none) const
Project point into the image Throws a CheiralityException if point behind image plane iff GTSAM_THROW...
Definition CalibratedCamera.cpp:116
│ │ │ │ +
gtsam::CalibratedCamera
A Calibrated camera class [R|-R't], calibration K=I.
Definition CalibratedCamera.h:247
│ │ │ │ +
gtsam::PinholeBaseK
A pinhole camera class that has a Pose3 and a fixed Calibration.
Definition PinholePose.h:34
│ │ │ │ +
gtsam::PinholeBaseK::range
double range(const CalibratedCamera &camera, OptionalJacobian< 1, 6 > Dcamera=boost::none, OptionalJacobian< 1, 6 > Dother=boost::none) const
Calculate range to a CalibratedCamera.
Definition PinholePose.h:202
│ │ │ │ +
gtsam::PinholeBaseK::project
Point2 project(const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none, OptionalJacobian< 2, DimK > Dcal=boost::none) const
project a point at infinity from world coordinates into the image
Definition PinholePose.h:132
│ │ │ │ +
gtsam::PinholeBaseK::project
Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > Dcal=boost::none) const
project a 3D point from world coordinates into the image
Definition PinholePose.h:118
│ │ │ │ +
gtsam::PinholeBaseK::projectSafe
std::pair< Point2, bool > projectSafe(const Point3 &pw) const
Project a point into the image and check depth.
Definition PinholePose.h:82
│ │ │ │ +
gtsam::PinholeBaseK::PinholeBaseK
PinholeBaseK()
default constructor
Definition PinholePose.h:51
│ │ │ │ +
gtsam::PinholeBaseK::range
double range(const Point3 &point, OptionalJacobian< 1, 6 > Dcamera=boost::none, OptionalJacobian< 1, 3 > Dpoint=boost::none) const
Calculate range to a landmark.
Definition PinholePose.h:181
│ │ │ │ +
gtsam::PinholeBaseK::calibration
virtual const CALIBRATION & calibration() const =0
return calibration
│ │ │ │ +
gtsam::PinholeBaseK::reprojectionError
Point2 reprojectionError(const Point3 &pw, const Point2 &measured, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > Dcal=boost::none) const
project a 3D point from world coordinates into the image
Definition PinholePose.h:125
│ │ │ │ +
gtsam::PinholeBaseK::PinholeBaseK
PinholeBaseK(const Pose3 &pose)
constructor with pose
Definition PinholePose.h:55
│ │ │ │ +
gtsam::PinholeBaseK::backproject
Point3 backproject(const Point2 &p, double depth, OptionalJacobian< 3, 6 > Dresult_dpose=boost::none, OptionalJacobian< 3, 2 > Dresult_dp=boost::none, OptionalJacobian< 3, 1 > Dresult_ddepth=boost::none, OptionalJacobian< 3, DimK > Dresult_dcal=boost::none) const
backproject a 2-dimensional point to a 3-dimensional point at given depth
Definition PinholePose.h:139
│ │ │ │ +
gtsam::PinholeBaseK::range
double range(const PinholeBaseK< CalibrationB > &camera, OptionalJacobian< 1, 6 > Dcamera=boost::none, OptionalJacobian< 1, 6 > Dother=boost::none) const
Calculate range to a PinholePoseK derived class.
Definition PinholePose.h:213
│ │ │ │ +
gtsam::PinholeBaseK::backprojectPointAtInfinity
Unit3 backprojectPointAtInfinity(const Point2 &p) const
backproject a 2-dimensional point to a 3-dimensional point at infinity
Definition PinholePose.h:170
│ │ │ │ +
gtsam::PinholeBaseK::access
friend class boost::serialization::access
Serialization function.
Definition PinholePose.h:222
│ │ │ │ +
gtsam::PinholeBaseK::_project
Point2 _project(const POINT &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, FixedDimension< POINT >::value > Dpoint, OptionalJacobian< 2, DimK > Dcal) const
Templated projection of a point (possibly at infinity) from world coordinate to the image.
Definition PinholePose.h:96
│ │ │ │ +
gtsam::PinholeBaseK::range
double range(const Pose3 &pose, OptionalJacobian< 1, 6 > Dcamera=boost::none, OptionalJacobian< 1, 6 > Dpose=boost::none) const
Calculate range to another pose.
Definition PinholePose.h:192
│ │ │ │ +
gtsam::PinholePose
A pinhole camera class that has a Pose3 and a fixed Calibration.
Definition PinholePose.h:243
│ │ │ │ +
gtsam::PinholePose::Lookat
static PinholePose Lookat(const Point3 &eye, const Point3 &target, const Point3 &upVector, const boost::shared_ptr< CALIBRATION > &K=boost::make_shared< CALIBRATION >())
Create a camera at the given eye position looking at a target point in the scene with the specified u...
Definition PinholePose.h:303
│ │ │ │ +
gtsam::PinholePose::PinholePose
PinholePose()
default constructor
Definition PinholePose.h:260
│ │ │ │ +
gtsam::PinholePose::cameraProjectionMatrix
Matrix34 cameraProjectionMatrix() const
for Linear Triangulation
Definition PinholePose.h:420
│ │ │ │ +
gtsam::PinholePose::project2
Point2 project2(const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const
project2 version for point at infinity
Definition PinholePose.h:385
│ │ │ │ +
gtsam::PinholePose::calibration
const CALIBRATION & calibration() const override
return calibration
Definition PinholePose.h:370
│ │ │ │ +
gtsam::PinholePose::defaultErrorWhenTriangulatingBehindCamera
Vector defaultErrorWhenTriangulatingBehindCamera() const
for Nonlinear Triangulation
Definition PinholePose.h:426
│ │ │ │ +
gtsam::PinholePose::operator<<
friend std::ostream & operator<<(std::ostream &os, const PinholePose &camera)
stream operator
Definition PinholePose.h:339
│ │ │ │ +
gtsam::PinholePose::Identity
static PinholePose Identity()
for Canonical
Definition PinholePose.h:415
│ │ │ │ +
gtsam::PinholePose::PinholePose
PinholePose(const Vector &v, const Vector &K)
Init from Vector and calibration.
Definition PinholePose.h:319
│ │ │ │ +
gtsam::PinholePose::sharedCalibration
const boost::shared_ptr< CALIBRATION > & sharedCalibration() const
return shared pointer to calibration
Definition PinholePose.h:365
│ │ │ │ +
gtsam::PinholePose::dim
size_t dim() const
Definition PinholePose.h:395
│ │ │ │ +
gtsam::PinholePose::project2
Point2 project2(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none) const
project a point from world coordinate to the image, 2 derivatives only
Definition PinholePose.h:379
│ │ │ │ +
gtsam::PinholePose::retract
PinholePose retract(const Vector6 &d) const
move a cameras according to d
Definition PinholePose.h:405
│ │ │ │ +
gtsam::PinholePose::PinholePose
PinholePose(const Vector &v)
Init from 6D vector.
Definition PinholePose.h:314
│ │ │ │ +
gtsam::PinholePose::PinholePose
PinholePose(const Pose3 &pose, const boost::shared_ptr< CALIBRATION > &K)
constructor with pose and calibration
Definition PinholePose.h:269
│ │ │ │ +
gtsam::PinholePose::Dim
static size_t Dim()
Definition PinholePose.h:400
│ │ │ │ +
gtsam::PinholePose::access
friend class boost::serialization::access
Serialization function.
Definition PinholePose.h:434
│ │ │ │ +
gtsam::PinholePose::equals
bool equals(const Base &camera, double tol=1e-9) const
assert equality up to a tolerance
Definition PinholePose.h:333
│ │ │ │ +
gtsam::PinholePose::print
void print(const std::string &s="PinholePose") const override
print
Definition PinholePose.h:349
│ │ │ │ +
gtsam::PinholePose::Level
static PinholePose Level(const boost::shared_ptr< CALIBRATION > &K, const Pose2 &pose2, double height)
Create a level camera at the given 2D pose and height.
Definition PinholePose.h:284
│ │ │ │ +
gtsam::PinholePose::PinholePose
PinholePose(const Pose3 &pose)
constructor with pose, uses default calibration
Definition PinholePose.h:264
│ │ │ │ +
gtsam::PinholePose::Level
static PinholePose Level(const Pose2 &pose2, double height)
PinholePose::level with default calibration.
Definition PinholePose.h:290
│ │ │ │ +
gtsam::PinholePose::localCoordinates
Vector6 localCoordinates(const PinholePose &p) const
return canonical coordinate
Definition PinholePose.h:410
│ │ │ │ +
gtsam::Pose2
A 2D pose (Point2,Rot2)
Definition Pose2.h:36
│ │ │ │ +
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ +
gtsam::Pose3::transformFrom
Point3 transformFrom(const Point3 &point, OptionalJacobian< 3, 6 > Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const
takes point in Pose coordinates and transforms it to world coordinates
Definition Pose3.cpp:347
│ │ │ │ +
gtsam::Pose3::range
double range(const Point3 &point, OptionalJacobian< 1, 6 > Hself=boost::none, OptionalJacobian< 1, 3 > Hpoint=boost::none) const
Calculate range to a landmark.
Definition Pose3.cpp:399
│ │ │ │ +
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get translation
Definition Pose3.cpp:308
│ │ │ │ +
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get rotation
Definition Pose3.cpp:315
│ │ │ │ +
gtsam::Rot3::rotate
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
rotate point from rotated coordinate frame to world
Definition Rot3M.cpp:149
│ │ │ │ +
gtsam::Rot3::rpy
Vector3 rpy(OptionalJacobian< 3, 3 > H=boost::none) const
Use RQ to calculate roll-pitch-yaw angle representation.
Definition Rot3.cpp:192
│ │ │ │ +
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,126 +1,662 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Cal3DS2.h │ │ │ │ │ +PinholePose.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -21#pragma once │ │ │ │ │ -22 │ │ │ │ │ -23#include │ │ │ │ │ -24#include │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ +24#include │ │ │ │ │ 25 │ │ │ │ │ 26namespace _g_t_s_a_m { │ │ │ │ │ 27 │ │ │ │ │ -_3_5class GTSAM_EXPORT _C_a_l_3_D_S_2 : public _C_a_l_3_D_S_2___B_a_s_e { │ │ │ │ │ -36 using _B_a_s_e = _C_a_l_3_D_S_2___B_a_s_e; │ │ │ │ │ +33template │ │ │ │ │ +_3_4class _P_i_n_h_o_l_e_B_a_s_e_K: public _P_i_n_h_o_l_e_B_a_s_e { │ │ │ │ │ +35 │ │ │ │ │ +36private: │ │ │ │ │ 37 │ │ │ │ │ -38 public: │ │ │ │ │ -39 enum { dimension = 9 }; │ │ │ │ │ -40 │ │ │ │ │ -42 using shared_ptr = boost::shared_ptr; │ │ │ │ │ -43 │ │ │ │ │ +38 GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) │ │ │ │ │ +39 │ │ │ │ │ +40 // Get dimensions of calibration type at compile time │ │ │ │ │ +41 static const int DimK = _F_i_x_e_d_D_i_m_e_n_s_i_o_n_<_C_A_L_I_B_R_A_T_I_O_N_>_:_:_v_a_l_u_e; │ │ │ │ │ +42 │ │ │ │ │ +43public: │ │ │ │ │ +44 │ │ │ │ │ +45 typedef CALIBRATION CalibrationType; │ │ │ │ │ 46 │ │ │ │ │ -_4_8 _C_a_l_3_D_S_2() = default; │ │ │ │ │ 49 │ │ │ │ │ -50 _C_a_l_3_D_S_2(double fx, double fy, double s, double u0, double v0, double k1, │ │ │ │ │ -51 double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5) │ │ │ │ │ -52 : _B_a_s_e(fx, fy, s, u0, v0, k1, k2, p1, p2, tol) {} │ │ │ │ │ +_5_1 _P_i_n_h_o_l_e_B_a_s_e_K() { │ │ │ │ │ +52 } │ │ │ │ │ 53 │ │ │ │ │ -54 _~_C_a_l_3_D_S_2() override {} │ │ │ │ │ -55 │ │ │ │ │ -59 │ │ │ │ │ -60 Cal3DS2(const Vector9& v) : Base(v) {} │ │ │ │ │ -61 │ │ │ │ │ -65 │ │ │ │ │ -67 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, │ │ │ │ │ -68 const Cal3DS2& cal); │ │ │ │ │ -69 │ │ │ │ │ -71 void _p_r_i_n_t(const std::string& s = "") const override; │ │ │ │ │ -72 │ │ │ │ │ -74 bool equals(const Cal3DS2& K, double tol = 10e-9) const; │ │ │ │ │ -75 │ │ │ │ │ -79 │ │ │ │ │ -81 Cal3DS2 retract(const Vector& d) const; │ │ │ │ │ -82 │ │ │ │ │ -84 Vector localCoordinates(const Cal3DS2& T2) const; │ │ │ │ │ -85 │ │ │ │ │ -_8_7 size_t _d_i_m() const override { return Dim(); } │ │ │ │ │ +_5_5 explicit _P_i_n_h_o_l_e_B_a_s_e_K(const _P_o_s_e_3& _p_o_s_e) : │ │ │ │ │ +56 _P_i_n_h_o_l_e_B_a_s_e(_p_o_s_e) { │ │ │ │ │ +57 } │ │ │ │ │ +58 │ │ │ │ │ +62 │ │ │ │ │ +63 explicit _P_i_n_h_o_l_e_B_a_s_e_K(const Vector &v) : │ │ │ │ │ +64 _P_i_n_h_o_l_e_B_a_s_e(v) { │ │ │ │ │ +65 } │ │ │ │ │ +66 │ │ │ │ │ +70 │ │ │ │ │ +71 virtual ~PinholeBaseK() { │ │ │ │ │ +72 } │ │ │ │ │ +73 │ │ │ │ │ +_7_5 virtual const CALIBRATION& _c_a_l_i_b_r_a_t_i_o_n() const = 0; │ │ │ │ │ +76 │ │ │ │ │ +80 │ │ │ │ │ +_8_2 std::pair _p_r_o_j_e_c_t_S_a_f_e(const _P_o_i_n_t_3& pw) const { │ │ │ │ │ +83 std::pair pn = _P_i_n_h_o_l_e_B_a_s_e_:_:_p_r_o_j_e_c_t_S_a_f_e(pw); │ │ │ │ │ +84 pn.first = _c_a_l_i_b_r_a_t_i_o_n().uncalibrate(pn.first); │ │ │ │ │ +85 return pn; │ │ │ │ │ +86 } │ │ │ │ │ +87 │ │ │ │ │ 88 │ │ │ │ │ -_9_0 inline static size_t _D_i_m() { return dimension; } │ │ │ │ │ -91 │ │ │ │ │ -95 │ │ │ │ │ -_9_7 boost::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -98 return boost::shared_ptr(new _C_a_l_3_D_S_2(*this)); │ │ │ │ │ -99 } │ │ │ │ │ -100 │ │ │ │ │ +95 template │ │ │ │ │ +_9_6 _P_o_i_n_t_2 ___p_r_o_j_e_c_t(const POINT& pw, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> _D_p_o_s_e, │ │ │ │ │ +97 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n<2, _F_i_x_e_d_D_i_m_e_n_s_i_o_n_<_P_O_I_N_T_>_:_:_v_a_l_u_e> _D_p_o_i_n_t, │ │ │ │ │ +98 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _D_i_m_K_> Dcal) const { │ │ │ │ │ +99 │ │ │ │ │ +100 // project to normalized coordinates │ │ │ │ │ +101 const _P_o_i_n_t_2 pn = _P_i_n_h_o_l_e_B_a_s_e_:_:_p_r_o_j_e_c_t_2(pw, _D_p_o_s_e, _D_p_o_i_n_t); │ │ │ │ │ 102 │ │ │ │ │ -103 private: │ │ │ │ │ -106 │ │ │ │ │ -_1_0_8 friend class boost::serialization::access; │ │ │ │ │ -109 template │ │ │ │ │ -110 void serialize(Archive& ar, const unsigned int /*version*/) { │ │ │ │ │ -111 ar& boost::serialization::make_nvp( │ │ │ │ │ -112 "Cal3DS2", boost::serialization::base_object(*this)); │ │ │ │ │ -113 } │ │ │ │ │ -114 │ │ │ │ │ -116}; │ │ │ │ │ -117 │ │ │ │ │ -118template <> │ │ │ │ │ -_1_1_9struct _t_r_a_i_t_s<_C_a_l_3_D_S_2> : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d {}; │ │ │ │ │ -120 │ │ │ │ │ -121template <> │ │ │ │ │ -_1_2_2struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d {}; │ │ │ │ │ -123} │ │ │ │ │ +103 // uncalibrate to pixel coordinates │ │ │ │ │ +104 Matrix2 Dpi_pn; │ │ │ │ │ +105 const _P_o_i_n_t_2 pi = _c_a_l_i_b_r_a_t_i_o_n().uncalibrate(pn, Dcal, │ │ │ │ │ +106 _D_p_o_s_e || _D_p_o_i_n_t ? &Dpi_pn : 0); │ │ │ │ │ +107 │ │ │ │ │ +108 // If needed, apply chain rule │ │ │ │ │ +109 if (_D_p_o_s_e) │ │ │ │ │ +110 *_D_p_o_s_e = Dpi_pn * *_D_p_o_s_e; │ │ │ │ │ +111 if (_D_p_o_i_n_t) │ │ │ │ │ +112 *_D_p_o_i_n_t = Dpi_pn * *_D_p_o_i_n_t; │ │ │ │ │ +113 │ │ │ │ │ +114 return pi; │ │ │ │ │ +115 } │ │ │ │ │ +116 │ │ │ │ │ +_1_1_8 _P_o_i_n_t_2 _p_r_o_j_e_c_t(const _P_o_i_n_t_3& pw, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> _D_p_o_s_e = boost:: │ │ │ │ │ +none, │ │ │ │ │ +119 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> _D_p_o_i_n_t = boost::none, │ │ │ │ │ +120 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _D_i_m_K_> Dcal = boost::none) const { │ │ │ │ │ +121 return ___p_r_o_j_e_c_t(pw, _D_p_o_s_e, _D_p_o_i_n_t, Dcal); │ │ │ │ │ +122 } │ │ │ │ │ +123 │ │ │ │ │ +_1_2_5 _P_o_i_n_t_2 _r_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r(const _P_o_i_n_t_3& pw, const _P_o_i_n_t_2& measured, │ │ │ │ │ +_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> _D_p_o_s_e = boost::none, │ │ │ │ │ +126 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> _D_p_o_i_n_t = boost::none, │ │ │ │ │ +127 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _D_i_m_K_> Dcal = boost::none) const { │ │ │ │ │ +128 return _P_o_i_n_t_2(___p_r_o_j_e_c_t(pw, _D_p_o_s_e, _D_p_o_i_n_t, Dcal) - measured); │ │ │ │ │ +129 } │ │ │ │ │ +130 │ │ │ │ │ +_1_3_2 _P_o_i_n_t_2 _p_r_o_j_e_c_t(const _U_n_i_t_3& pw, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> _D_p_o_s_e = boost::none, │ │ │ │ │ +133 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> _D_p_o_i_n_t = boost::none, │ │ │ │ │ +134 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _D_i_m_K_> Dcal = boost::none) const { │ │ │ │ │ +135 return ___p_r_o_j_e_c_t(pw, _D_p_o_s_e, _D_p_o_i_n_t, Dcal); │ │ │ │ │ +136 } │ │ │ │ │ +137 │ │ │ │ │ +_1_3_9 _P_o_i_n_t_3 _b_a_c_k_p_r_o_j_e_c_t(const _P_o_i_n_t_2& p, double depth, │ │ │ │ │ +140 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _6_> Dresult_dpose = boost::none, │ │ │ │ │ +141 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _2_> Dresult_dp = boost::none, │ │ │ │ │ +142 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _1_> Dresult_ddepth = boost::none, │ │ │ │ │ +143 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _D_i_m_K_> Dresult_dcal = boost::none) const { │ │ │ │ │ +144 typedef Eigen::Matrix Matrix2K; │ │ │ │ │ +145 Matrix2K Dpn_dcal; │ │ │ │ │ +146 Matrix22 Dpn_dp; │ │ │ │ │ +147 const _P_o_i_n_t_2 pn = _c_a_l_i_b_r_a_t_i_o_n().calibrate(p, Dresult_dcal ? &Dpn_dcal : 0, │ │ │ │ │ +148 Dresult_dp ? &Dpn_dp : 0); │ │ │ │ │ +149 Matrix32 Dpoint_dpn; │ │ │ │ │ +150 Matrix31 Dpoint_ddepth; │ │ │ │ │ +151 const _P_o_i_n_t_3 point = _B_a_c_k_p_r_o_j_e_c_t_F_r_o_m_C_a_m_e_r_a(pn, depth, │ │ │ │ │ +152 (Dresult_dp || Dresult_dcal) ? &Dpoint_dpn : 0, │ │ │ │ │ +153 Dresult_ddepth ? &Dpoint_ddepth : 0); │ │ │ │ │ +154 Matrix33 Dresult_dpoint; │ │ │ │ │ +155 const _P_o_i_n_t_3 result = _p_o_s_e()._t_r_a_n_s_f_o_r_m_F_r_o_m(point, Dresult_dpose, │ │ │ │ │ +156 (Dresult_ddepth || │ │ │ │ │ +157 Dresult_dp || │ │ │ │ │ +158 Dresult_dcal) ? &Dresult_dpoint : 0); │ │ │ │ │ +159 if (Dresult_dcal) │ │ │ │ │ +160 *Dresult_dcal = Dresult_dpoint * Dpoint_dpn * Dpn_dcal; // (3x3)*(3x2)* │ │ │ │ │ +(2xDimK) │ │ │ │ │ +161 if (Dresult_dp) │ │ │ │ │ +162 *Dresult_dp = Dresult_dpoint * Dpoint_dpn * Dpn_dp; // (3x3)*(3x2)*(2x2) │ │ │ │ │ +163 if (Dresult_ddepth) │ │ │ │ │ +164 *Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth; // (3x3)*(3x1) │ │ │ │ │ +165 │ │ │ │ │ +166 return result; │ │ │ │ │ +167 } │ │ │ │ │ +168 │ │ │ │ │ +_1_7_0 _U_n_i_t_3 _b_a_c_k_p_r_o_j_e_c_t_P_o_i_n_t_A_t_I_n_f_i_n_i_t_y(const _P_o_i_n_t_2& p) const { │ │ │ │ │ +171 const _P_o_i_n_t_2 pn = _c_a_l_i_b_r_a_t_i_o_n().calibrate(p); │ │ │ │ │ +172 const _U_n_i_t_3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1 │ │ │ │ │ +173 return _p_o_s_e()._r_o_t_a_t_i_o_n()._r_o_t_a_t_e(pc); │ │ │ │ │ +174 } │ │ │ │ │ +175 │ │ │ │ │ +_1_8_1 double _r_a_n_g_e(const _P_o_i_n_t_3& point, │ │ │ │ │ +182 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> Dcamera = boost::none, │ │ │ │ │ +183 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> _D_p_o_i_n_t = boost::none) const { │ │ │ │ │ +184 return _p_o_s_e()._r_a_n_g_e(point, Dcamera, _D_p_o_i_n_t); │ │ │ │ │ +185 } │ │ │ │ │ +186 │ │ │ │ │ +_1_9_2 double _r_a_n_g_e(const _P_o_s_e_3& _p_o_s_e, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> Dcamera = boost:: │ │ │ │ │ +none, │ │ │ │ │ +193 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> _D_p_o_s_e = boost::none) const { │ │ │ │ │ +194 return this->_p_o_s_e()._r_a_n_g_e(pose, Dcamera, _D_p_o_s_e); │ │ │ │ │ +195 } │ │ │ │ │ +196 │ │ │ │ │ +_2_0_2 double _r_a_n_g_e(const _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a& camera, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> Dcamera │ │ │ │ │ += │ │ │ │ │ +203 boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> Dother = boost::none) const { │ │ │ │ │ +204 return _p_o_s_e()._r_a_n_g_e(camera._p_o_s_e(), Dcamera, Dother); │ │ │ │ │ +205 } │ │ │ │ │ +206 │ │ │ │ │ +212 template │ │ │ │ │ +_2_1_3 double _r_a_n_g_e(const _P_i_n_h_o_l_e_B_a_s_e_K_<_C_a_l_i_b_r_a_t_i_o_n_B_>& camera, │ │ │ │ │ +214 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> Dcamera = boost::none, │ │ │ │ │ +215 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> Dother = boost::none) const { │ │ │ │ │ +216 return _p_o_s_e()._r_a_n_g_e(camera._p_o_s_e(), Dcamera, Dother); │ │ │ │ │ +217 } │ │ │ │ │ +218 │ │ │ │ │ +219private: │ │ │ │ │ +220 │ │ │ │ │ +_2_2_2 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +223 template │ │ │ │ │ +224 void serialize(Archive & ar, const unsigned int /*version*/) { │ │ │ │ │ +225 ar │ │ │ │ │ +226 & boost::serialization::make_nvp("PinholeBase", │ │ │ │ │ +227 boost::serialization::base_object(*this)); │ │ │ │ │ +228 } │ │ │ │ │ +229 │ │ │ │ │ +230public: │ │ │ │ │ +231 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +232}; │ │ │ │ │ +233// end of class PinholeBaseK │ │ │ │ │ +234 │ │ │ │ │ +242template │ │ │ │ │ +_2_4_3class _P_i_n_h_o_l_e_P_o_s_e: public _P_i_n_h_o_l_e_B_a_s_e_K { │ │ │ │ │ +244 │ │ │ │ │ +245private: │ │ │ │ │ +246 │ │ │ │ │ +247 typedef _P_i_n_h_o_l_e_B_a_s_e_K_<_C_A_L_I_B_R_A_T_I_O_N_> _B_a_s_e; │ │ │ │ │ +248 boost::shared_ptr K_; │ │ │ │ │ +249 │ │ │ │ │ +250public: │ │ │ │ │ +251 │ │ │ │ │ +252 enum { │ │ │ │ │ +253 dimension = 6 │ │ │ │ │ +254 }; │ │ │ │ │ +255 │ │ │ │ │ +258 │ │ │ │ │ +_2_6_0 _P_i_n_h_o_l_e_P_o_s_e() { │ │ │ │ │ +261 } │ │ │ │ │ +262 │ │ │ │ │ +_2_6_4 explicit _P_i_n_h_o_l_e_P_o_s_e(const _P_o_s_e_3& _p_o_s_e) : │ │ │ │ │ +265 _B_a_s_e(_p_o_s_e), K_(new CALIBRATION()) { │ │ │ │ │ +266 } │ │ │ │ │ +267 │ │ │ │ │ +_2_6_9 _P_i_n_h_o_l_e_P_o_s_e(const _P_o_s_e_3& _p_o_s_e, const boost::shared_ptr& K) : │ │ │ │ │ +270 _B_a_s_e(_p_o_s_e), K_(K) { │ │ │ │ │ +271 } │ │ │ │ │ +272 │ │ │ │ │ +276 │ │ │ │ │ +_2_8_4 static _P_i_n_h_o_l_e_P_o_s_e _L_e_v_e_l(const boost::shared_ptr& K, │ │ │ │ │ +285 const _P_o_s_e_2& pose2, double height) { │ │ │ │ │ +286 return _P_i_n_h_o_l_e_P_o_s_e(_B_a_s_e_:_:_L_e_v_e_l_P_o_s_e(pose2, height), K); │ │ │ │ │ +287 } │ │ │ │ │ +288 │ │ │ │ │ +_2_9_0 static _P_i_n_h_o_l_e_P_o_s_e _L_e_v_e_l(const _P_o_s_e_2& pose2, double height) { │ │ │ │ │ +291 return _P_i_n_h_o_l_e_P_o_s_e_:_:_L_e_v_e_l(boost::make_shared(), pose2, │ │ │ │ │ +height); │ │ │ │ │ +292 } │ │ │ │ │ +293 │ │ │ │ │ +_3_0_3 static _P_i_n_h_o_l_e_P_o_s_e _L_o_o_k_a_t(const _P_o_i_n_t_3& eye, const _P_o_i_n_t_3& target, │ │ │ │ │ +304 const _P_o_i_n_t_3& upVector, const boost::shared_ptr& K = │ │ │ │ │ +305 boost::make_shared()) { │ │ │ │ │ +306 return _P_i_n_h_o_l_e_P_o_s_e(_B_a_s_e_:_:_L_o_o_k_a_t_P_o_s_e(eye, target, upVector), K); │ │ │ │ │ +307 } │ │ │ │ │ +308 │ │ │ │ │ +312 │ │ │ │ │ +_3_1_4 explicit _P_i_n_h_o_l_e_P_o_s_e(const Vector &v) : │ │ │ │ │ +315 _B_a_s_e(v), K_(new CALIBRATION()) { │ │ │ │ │ +316 } │ │ │ │ │ +317 │ │ │ │ │ +_3_1_9 _P_i_n_h_o_l_e_P_o_s_e(const Vector &v, const Vector &K) : │ │ │ │ │ +320 _B_a_s_e(v), K_(new CALIBRATION(K)) { │ │ │ │ │ +321 } │ │ │ │ │ +322 │ │ │ │ │ +323 // Init from Pose3 and calibration │ │ │ │ │ +324 _P_i_n_h_o_l_e_P_o_s_e(const _P_o_s_e_3 &_p_o_s_e, const Vector &K) : │ │ │ │ │ +325 Base(_p_o_s_e), K_(new CALIBRATION(K)) { │ │ │ │ │ +326 } │ │ │ │ │ +327 │ │ │ │ │ +331 │ │ │ │ │ +_3_3_3 bool _e_q_u_a_l_s(const _B_a_s_e &camera, double tol = 1e-9) const { │ │ │ │ │ +334 const _P_i_n_h_o_l_e_P_o_s_e* e = dynamic_cast(&camera); │ │ │ │ │ +335 return _B_a_s_e_:_:_e_q_u_a_l_s(camera, tol) && K_->_e_q_u_a_l_s(e->_c_a_l_i_b_r_a_t_i_o_n(), tol); │ │ │ │ │ +336 } │ │ │ │ │ +337 │ │ │ │ │ +_3_3_9 friend std::ostream& _o_p_e_r_a_t_o_r_<_<(std::ostream &os, const _P_i_n_h_o_l_e_P_o_s_e& │ │ │ │ │ +camera) { │ │ │ │ │ +340 os << "{R: " << camera._p_o_s_e()._r_o_t_a_t_i_o_n()._r_p_y().transpose(); │ │ │ │ │ +341 os << ", t: " << camera._p_o_s_e()._t_r_a_n_s_l_a_t_i_o_n().transpose(); │ │ │ │ │ +342 if (!camera.K_) os << ", K: none"; │ │ │ │ │ +343 else os << ", K: " << *camera.K_; │ │ │ │ │ +344 os << "}"; │ │ │ │ │ +345 return os; │ │ │ │ │ +346 } │ │ │ │ │ +347 │ │ │ │ │ +_3_4_9 void _p_r_i_n_t(const std::string& s = "PinholePose") const override { │ │ │ │ │ +350 _B_a_s_e_:_:_p_r_i_n_t(s); │ │ │ │ │ +351 if (!K_) │ │ │ │ │ +352 std::cout << "s No calibration given" << std::endl; │ │ │ │ │ +353 else │ │ │ │ │ +354 K_->print(s + ".calibration"); │ │ │ │ │ +355 } │ │ │ │ │ +356 │ │ │ │ │ +360 │ │ │ │ │ +361 _~_P_i_n_h_o_l_e_P_o_s_e() override { │ │ │ │ │ +362 } │ │ │ │ │ +363 │ │ │ │ │ +_3_6_5 const boost::shared_ptr& _s_h_a_r_e_d_C_a_l_i_b_r_a_t_i_o_n() const { │ │ │ │ │ +366 return K_; │ │ │ │ │ +367 } │ │ │ │ │ +368 │ │ │ │ │ +_3_7_0 const CALIBRATION& _c_a_l_i_b_r_a_t_i_o_n() const override { │ │ │ │ │ +371 return *K_; │ │ │ │ │ +372 } │ │ │ │ │ +373 │ │ │ │ │ +_3_7_9 _P_o_i_n_t_2 _p_r_o_j_e_c_t_2(const _P_o_i_n_t_3& pw, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> _D_p_o_s_e = boost:: │ │ │ │ │ +none, │ │ │ │ │ +380 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> _D_p_o_i_n_t = boost::none) const { │ │ │ │ │ +381 return _B_a_s_e_:_:_p_r_o_j_e_c_t(pw, _D_p_o_s_e, _D_p_o_i_n_t); │ │ │ │ │ +382 } │ │ │ │ │ +383 │ │ │ │ │ +_3_8_5 _P_o_i_n_t_2 _p_r_o_j_e_c_t_2(const _U_n_i_t_3& pw, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> _D_p_o_s_e = boost:: │ │ │ │ │ +none, │ │ │ │ │ +386 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> _D_p_o_i_n_t = boost::none) const { │ │ │ │ │ +387 return _B_a_s_e_:_:_p_r_o_j_e_c_t(pw, _D_p_o_s_e, _D_p_o_i_n_t); │ │ │ │ │ +388 } │ │ │ │ │ +389 │ │ │ │ │ +393 │ │ │ │ │ +_3_9_5 size_t _d_i_m() const { │ │ │ │ │ +396 return 6; │ │ │ │ │ +397 } │ │ │ │ │ +398 │ │ │ │ │ +_4_0_0 static size_t _D_i_m() { │ │ │ │ │ +401 return 6; │ │ │ │ │ +402 } │ │ │ │ │ +403 │ │ │ │ │ +_4_0_5 _P_i_n_h_o_l_e_P_o_s_e _r_e_t_r_a_c_t(const Vector6& d) const { │ │ │ │ │ +406 return _P_i_n_h_o_l_e_P_o_s_e(_B_a_s_e_:_:_p_o_s_e()._r_e_t_r_a_c_t(d), K_); │ │ │ │ │ +407 } │ │ │ │ │ +408 │ │ │ │ │ +_4_1_0 Vector6 _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(const _P_i_n_h_o_l_e_P_o_s_e& p) const { │ │ │ │ │ +411 return _B_a_s_e_:_:_p_o_s_e()._l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(p.Base::pose()); │ │ │ │ │ +412 } │ │ │ │ │ +413 │ │ │ │ │ +_4_1_5 static _P_i_n_h_o_l_e_P_o_s_e _I_d_e_n_t_i_t_y() { │ │ │ │ │ +416 return _P_i_n_h_o_l_e_P_o_s_e(); // assumes that the default constructor is valid │ │ │ │ │ +417 } │ │ │ │ │ +418 │ │ │ │ │ +_4_2_0 Matrix34 _c_a_m_e_r_a_P_r_o_j_e_c_t_i_o_n_M_a_t_r_i_x() const { │ │ │ │ │ +421 Matrix34 P = Matrix34(_P_i_n_h_o_l_e_B_a_s_e_:_:_p_o_s_e().inverse().matrix().block(0, 0, 3, │ │ │ │ │ +4)); │ │ │ │ │ +422 return K_->K() * P; │ │ │ │ │ +423 } │ │ │ │ │ +424 │ │ │ │ │ +_4_2_6 Vector _d_e_f_a_u_l_t_E_r_r_o_r_W_h_e_n_T_r_i_a_n_g_u_l_a_t_i_n_g_B_e_h_i_n_d_C_a_m_e_r_a() const { │ │ │ │ │ +427 return Eigen::Matrix::dimension,1>::Constant(2.0 * │ │ │ │ │ +K_->fx());; │ │ │ │ │ +428 } │ │ │ │ │ +430 │ │ │ │ │ +431private: │ │ │ │ │ +432 │ │ │ │ │ +_4_3_4 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +435 template │ │ │ │ │ +436 void serialize(Archive & ar, const unsigned int /*version*/) { │ │ │ │ │ +437 ar │ │ │ │ │ +438 & boost::serialization::make_nvp("PinholeBaseK", │ │ │ │ │ +439 boost::serialization::base_object(*this)); │ │ │ │ │ +440 ar & BOOST_SERIALIZATION_NVP(K_); │ │ │ │ │ +441 } │ │ │ │ │ +442 │ │ │ │ │ +443public: │ │ │ │ │ +444 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +445}; │ │ │ │ │ +446// end of class PinholePose │ │ │ │ │ +447 │ │ │ │ │ +448template │ │ │ │ │ +_4_4_9struct _t_r_a_i_t_s<_P_i_n_h_o_l_e_P_o_s_e > : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d< │ │ │ │ │ +450 PinholePose > { │ │ │ │ │ +451}; │ │ │ │ │ +452 │ │ │ │ │ +453template │ │ │ │ │ +_4_5_4struct _t_r_a_i_t_s > : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d< │ │ │ │ │ +455 PinholePose > { │ │ │ │ │ +456}; │ │ │ │ │ +457 │ │ │ │ │ +458} // \ gtsam │ │ │ │ │ +_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ +This marks a GTSAM object to require alignment. │ │ │ │ │ +DDeeffiinniittiioonn types.h:308 │ │ │ │ │ +_P_o_i_n_t_2_._h │ │ │ │ │ +2D Point │ │ │ │ │ +_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_._h │ │ │ │ │ +Calibrated camera for which only pose is unknown. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ +Vector2 Point2 │ │ │ │ │ +As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ +typedef Point2 to Vector2... │ │ │ │ │ +DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ +Vector3 Point3 │ │ │ │ │ +As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ +typedef Point3 to Vector3... │ │ │ │ │ +DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ +TangentVector localCoordinates(const Class &g) const │ │ │ │ │ +localCoordinates as required by manifold concept: finds tangent vector between │ │ │ │ │ +*this and g │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:136 │ │ │ │ │ _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ Both ManifoldTraits and Testable. │ │ │ │ │ DDeeffiinniittiioonn Manifold.h:120 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_D_S_2 │ │ │ │ │ -Calibration of a camera with radial distortion that also supports Lie-group │ │ │ │ │ -behaviors for optimizatio... │ │ │ │ │ -DDeeffiinniittiioonn Cal3DS2.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_D_S_2_:_:_d_i_m │ │ │ │ │ -size_t dim() const override │ │ │ │ │ -Return dimensions of calibration manifold object. │ │ │ │ │ -DDeeffiinniittiioonn Cal3DS2.h:87 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_D_S_2_:_:_c_l_o_n_e │ │ │ │ │ -boost::shared_ptr< Base > clone() const override │ │ │ │ │ -DDeeffiinniittiioonn Cal3DS2.h:97 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_D_S_2_:_:_D_i_m │ │ │ │ │ +_g_t_s_a_m_:_:_F_i_x_e_d_D_i_m_e_n_s_i_o_n │ │ │ │ │ +Give fixed size dimension of a type, fails at compile time if dynamic. │ │ │ │ │ +DDeeffiinniittiioonn Manifold.h:164 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ +either a fixed size o... │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e │ │ │ │ │ +A pinhole camera class that has a Pose3, functions as base class for all │ │ │ │ │ +pinhole cameras. │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_D_p_o_s_e │ │ │ │ │ +static Matrix26 Dpose(const Point2 &pn, double d) │ │ │ │ │ +Calculate Jacobian with respect to pose. │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.cpp:27 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_p_r_i_n_t │ │ │ │ │ +virtual void print(const std::string &s="PinholeBase") const │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.cpp:74 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_p_r_o_j_e_c_t_S_a_f_e │ │ │ │ │ +std::pair< Point2, bool > projectSafe(const Point3 &pw) const │ │ │ │ │ +Project a point into the image and check depth. │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.cpp:109 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_p_o_s_e │ │ │ │ │ +const Pose3 & pose() const │ │ │ │ │ +return pose, constant version │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:152 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_L_e_v_e_l_P_o_s_e │ │ │ │ │ +static Pose3 LevelPose(const Pose2 &pose2, double height) │ │ │ │ │ +Create a level pose at the given 2D pose and height. │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.cpp:49 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const PinholeBase &camera, double tol=1e-9) const │ │ │ │ │ +assert equality up to a tolerance │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.cpp:69 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_D_p_o_i_n_t │ │ │ │ │ +static Matrix23 Dpoint(const Point2 &pn, double d, const Matrix3 &Rt) │ │ │ │ │ +Calculate Jacobian with respect to point. │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.cpp:37 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_B_a_c_k_p_r_o_j_e_c_t_F_r_o_m_C_a_m_e_r_a │ │ │ │ │ +static Point3 BackprojectFromCamera(const Point2 &p, const double depth, │ │ │ │ │ +OptionalJacobian< 3, 2 > Dpoint=boost::none, OptionalJacobian< 3, 1 > │ │ │ │ │ +Ddepth=boost::none) │ │ │ │ │ +backproject a 2-dimensional point to a 3-dimensional point at given depth │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.cpp:167 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_L_o_o_k_a_t_P_o_s_e │ │ │ │ │ +static Pose3 LookatPose(const Point3 &eye, const Point3 &target, const Point3 │ │ │ │ │ +&upVector) │ │ │ │ │ +Create a camera pose at the given eye position looking at a target point in the │ │ │ │ │ +scene with the specif... │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.cpp:58 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_p_r_o_j_e_c_t_2 │ │ │ │ │ +Point2 project2(const Point3 &point, OptionalJacobian< 2, 6 > Dpose=boost:: │ │ │ │ │ +none, OptionalJacobian< 2, 3 > Dpoint=boost::none) const │ │ │ │ │ +Project point into the image Throws a CheiralityException if point behind image │ │ │ │ │ +plane iff GTSAM_THROW... │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.cpp:116 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a │ │ │ │ │ +A Calibrated camera class [R|-R't], calibration K=I. │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:247 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K │ │ │ │ │ +A pinhole camera class that has a Pose3 and a fixed Calibration. │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_r_a_n_g_e │ │ │ │ │ +double range(const CalibratedCamera &camera, OptionalJacobian< 1, 6 > │ │ │ │ │ +Dcamera=boost::none, OptionalJacobian< 1, 6 > Dother=boost::none) const │ │ │ │ │ +Calculate range to a CalibratedCamera. │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:202 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_p_r_o_j_e_c_t │ │ │ │ │ +Point2 project(const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, │ │ │ │ │ +OptionalJacobian< 2, 2 > Dpoint=boost::none, OptionalJacobian< 2, DimK > │ │ │ │ │ +Dcal=boost::none) const │ │ │ │ │ +project a point at infinity from world coordinates into the image │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:132 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_p_r_o_j_e_c_t │ │ │ │ │ +Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, │ │ │ │ │ +OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > │ │ │ │ │ +Dcal=boost::none) const │ │ │ │ │ +project a 3D point from world coordinates into the image │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:118 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_p_r_o_j_e_c_t_S_a_f_e │ │ │ │ │ +std::pair< Point2, bool > projectSafe(const Point3 &pw) const │ │ │ │ │ +Project a point into the image and check depth. │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:82 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_P_i_n_h_o_l_e_B_a_s_e_K │ │ │ │ │ +PinholeBaseK() │ │ │ │ │ +default constructor │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:51 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_r_a_n_g_e │ │ │ │ │ +double range(const Point3 &point, OptionalJacobian< 1, 6 > Dcamera=boost::none, │ │ │ │ │ +OptionalJacobian< 1, 3 > Dpoint=boost::none) const │ │ │ │ │ +Calculate range to a landmark. │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:181 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_c_a_l_i_b_r_a_t_i_o_n │ │ │ │ │ +virtual const CALIBRATION & calibration() const =0 │ │ │ │ │ +return calibration │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_r_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r │ │ │ │ │ +Point2 reprojectionError(const Point3 &pw, const Point2 &measured, │ │ │ │ │ +OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > │ │ │ │ │ +Dpoint=boost::none, OptionalJacobian< 2, DimK > Dcal=boost::none) const │ │ │ │ │ +project a 3D point from world coordinates into the image │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:125 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_P_i_n_h_o_l_e_B_a_s_e_K │ │ │ │ │ +PinholeBaseK(const Pose3 &pose) │ │ │ │ │ +constructor with pose │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_b_a_c_k_p_r_o_j_e_c_t │ │ │ │ │ +Point3 backproject(const Point2 &p, double depth, OptionalJacobian< 3, 6 > │ │ │ │ │ +Dresult_dpose=boost::none, OptionalJacobian< 3, 2 > Dresult_dp=boost::none, │ │ │ │ │ +OptionalJacobian< 3, 1 > Dresult_ddepth=boost::none, OptionalJacobian< 3, DimK │ │ │ │ │ +> Dresult_dcal=boost::none) const │ │ │ │ │ +backproject a 2-dimensional point to a 3-dimensional point at given depth │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:139 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_r_a_n_g_e │ │ │ │ │ +double range(const PinholeBaseK< CalibrationB > &camera, OptionalJacobian< 1, 6 │ │ │ │ │ +> Dcamera=boost::none, OptionalJacobian< 1, 6 > Dother=boost::none) const │ │ │ │ │ +Calculate range to a PinholePoseK derived class. │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:213 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_b_a_c_k_p_r_o_j_e_c_t_P_o_i_n_t_A_t_I_n_f_i_n_i_t_y │ │ │ │ │ +Unit3 backprojectPointAtInfinity(const Point2 &p) const │ │ │ │ │ +backproject a 2-dimensional point to a 3-dimensional point at infinity │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:170 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:222 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:___p_r_o_j_e_c_t │ │ │ │ │ +Point2 _project(const POINT &pw, OptionalJacobian< 2, 6 > Dpose, │ │ │ │ │ +OptionalJacobian< 2, FixedDimension< POINT >::value > Dpoint, OptionalJacobian< │ │ │ │ │ +2, DimK > Dcal) const │ │ │ │ │ +Templated projection of a point (possibly at infinity) from world coordinate to │ │ │ │ │ +the image. │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:96 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_r_a_n_g_e │ │ │ │ │ +double range(const Pose3 &pose, OptionalJacobian< 1, 6 > Dcamera=boost::none, │ │ │ │ │ +OptionalJacobian< 1, 6 > Dpose=boost::none) const │ │ │ │ │ +Calculate range to another pose. │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:192 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e │ │ │ │ │ +A pinhole camera class that has a Pose3 and a fixed Calibration. │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:243 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_L_o_o_k_a_t │ │ │ │ │ +static PinholePose Lookat(const Point3 &eye, const Point3 &target, const Point3 │ │ │ │ │ +&upVector, const boost::shared_ptr< CALIBRATION > &K=boost::make_shared< │ │ │ │ │ +CALIBRATION >()) │ │ │ │ │ +Create a camera at the given eye position looking at a target point in the │ │ │ │ │ +scene with the specified u... │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:303 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_P_i_n_h_o_l_e_P_o_s_e │ │ │ │ │ +PinholePose() │ │ │ │ │ +default constructor │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:260 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_c_a_m_e_r_a_P_r_o_j_e_c_t_i_o_n_M_a_t_r_i_x │ │ │ │ │ +Matrix34 cameraProjectionMatrix() const │ │ │ │ │ +for Linear Triangulation │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:420 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_p_r_o_j_e_c_t_2 │ │ │ │ │ +Point2 project2(const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, │ │ │ │ │ +OptionalJacobian< 2, 2 > Dpoint=boost::none) const │ │ │ │ │ +project2 version for point at infinity │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:385 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_c_a_l_i_b_r_a_t_i_o_n │ │ │ │ │ +const CALIBRATION & calibration() const override │ │ │ │ │ +return calibration │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:370 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_d_e_f_a_u_l_t_E_r_r_o_r_W_h_e_n_T_r_i_a_n_g_u_l_a_t_i_n_g_B_e_h_i_n_d_C_a_m_e_r_a │ │ │ │ │ +Vector defaultErrorWhenTriangulatingBehindCamera() const │ │ │ │ │ +for Nonlinear Triangulation │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:426 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_o_p_e_r_a_t_o_r_<_< │ │ │ │ │ +friend std::ostream & operator<<(std::ostream &os, const PinholePose &camera) │ │ │ │ │ +stream operator │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:339 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_I_d_e_n_t_i_t_y │ │ │ │ │ +static PinholePose Identity() │ │ │ │ │ +for Canonical │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:415 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_P_i_n_h_o_l_e_P_o_s_e │ │ │ │ │ +PinholePose(const Vector &v, const Vector &K) │ │ │ │ │ +Init from Vector and calibration. │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:319 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_s_h_a_r_e_d_C_a_l_i_b_r_a_t_i_o_n │ │ │ │ │ +const boost::shared_ptr< CALIBRATION > & sharedCalibration() const │ │ │ │ │ +return shared pointer to calibration │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:365 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_d_i_m │ │ │ │ │ +size_t dim() const │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:395 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_p_r_o_j_e_c_t_2 │ │ │ │ │ +Point2 project2(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, │ │ │ │ │ +OptionalJacobian< 2, 3 > Dpoint=boost::none) const │ │ │ │ │ +project a point from world coordinate to the image, 2 derivatives only │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:379 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_r_e_t_r_a_c_t │ │ │ │ │ +PinholePose retract(const Vector6 &d) const │ │ │ │ │ +move a cameras according to d │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:405 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_P_i_n_h_o_l_e_P_o_s_e │ │ │ │ │ +PinholePose(const Vector &v) │ │ │ │ │ +Init from 6D vector. │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:314 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_P_i_n_h_o_l_e_P_o_s_e │ │ │ │ │ +PinholePose(const Pose3 &pose, const boost::shared_ptr< CALIBRATION > &K) │ │ │ │ │ +constructor with pose and calibration │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:269 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_D_i_m │ │ │ │ │ static size_t Dim() │ │ │ │ │ -Return dimensions of calibration manifold object. │ │ │ │ │ -DDeeffiinniittiioonn Cal3DS2.h:90 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_D_S_2_:_:_C_a_l_3_D_S_2 │ │ │ │ │ -Cal3DS2()=default │ │ │ │ │ -Default Constructor with only unit focal length. │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_D_S_2___B_a_s_e │ │ │ │ │ -Calibration of a camera with radial distortion. │ │ │ │ │ -DDeeffiinniittiioonn Cal3DS2_Base.h:42 │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:400 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:434 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const Base &camera, double tol=1e-9) const │ │ │ │ │ +assert equality up to a tolerance │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:333 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="PinholePose") const override │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:349 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_L_e_v_e_l │ │ │ │ │ +static PinholePose Level(const boost::shared_ptr< CALIBRATION > &K, const Pose2 │ │ │ │ │ +&pose2, double height) │ │ │ │ │ +Create a level camera at the given 2D pose and height. │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:284 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_P_i_n_h_o_l_e_P_o_s_e │ │ │ │ │ +PinholePose(const Pose3 &pose) │ │ │ │ │ +constructor with pose, uses default calibration │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:264 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_L_e_v_e_l │ │ │ │ │ +static PinholePose Level(const Pose2 &pose2, double height) │ │ │ │ │ +PinholePose::level with default calibration. │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:290 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ +Vector6 localCoordinates(const PinholePose &p) const │ │ │ │ │ +return canonical coordinate │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:410 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2 │ │ │ │ │ +A 2D pose (Point2,Rot2) │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ +A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_t_r_a_n_s_f_o_r_m_F_r_o_m │ │ │ │ │ +Point3 transformFrom(const Point3 &point, OptionalJacobian< 3, 6 > │ │ │ │ │ +Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const │ │ │ │ │ +takes point in Pose coordinates and transforms it to world coordinates │ │ │ │ │ +DDeeffiinniittiioonn Pose3.cpp:347 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_r_a_n_g_e │ │ │ │ │ +double range(const Point3 &point, OptionalJacobian< 1, 6 > Hself=boost::none, │ │ │ │ │ +OptionalJacobian< 1, 3 > Hpoint=boost::none) const │ │ │ │ │ +Calculate range to a landmark. │ │ │ │ │ +DDeeffiinniittiioonn Pose3.cpp:399 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_t_r_a_n_s_l_a_t_i_o_n │ │ │ │ │ +const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const │ │ │ │ │ +get translation │ │ │ │ │ +DDeeffiinniittiioonn Pose3.cpp:308 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_r_o_t_a_t_i_o_n │ │ │ │ │ +const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const │ │ │ │ │ +get rotation │ │ │ │ │ +DDeeffiinniittiioonn Pose3.cpp:315 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_3_:_:_r_o_t_a_t_e │ │ │ │ │ +Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, │ │ │ │ │ +OptionalJacobian< 3, 3 > H2=boost::none) const │ │ │ │ │ +rotate point from rotated coordinate frame to world │ │ │ │ │ +DDeeffiinniittiioonn Rot3M.cpp:149 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_3_:_:_r_p_y │ │ │ │ │ +Vector3 rpy(OptionalJacobian< 3, 3 > H=boost::none) const │ │ │ │ │ +Use RQ to calculate roll-pitch-yaw angle representation. │ │ │ │ │ +DDeeffiinniittiioonn Rot3.cpp:192 │ │ │ │ │ +_g_t_s_a_m_:_:_U_n_i_t_3 │ │ │ │ │ +Represents a 3D point on a unit sphere. │ │ │ │ │ +DDeeffiinniittiioonn Unit3.h:43 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3_D_S_2_._h │ │ │ │ │ + * _P_i_n_h_o_l_e_P_o_s_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00407.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Rot2.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/triangulation.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,52 +95,192 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
Rot2.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ +Functions
│ │ │ │ +
triangulation.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

2D rotation │ │ │ │ +

Functions for triangulation. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::Rot2
 Rotation matrix NOTE: the angle theta is in radians unless explicitly stated. More...
class  gtsam::TriangulationUnderconstrainedException
 Exception thrown by triangulateDLT when SVD returns rank < 3. More...
 
struct  gtsam::Rot2::ChartAtOrigin
class  gtsam::TriangulationCheiralityException
 Exception thrown by triangulateDLT when landmark is behind one or more of the cameras. More...
 
struct  gtsam::traits< Rot2 >
struct  gtsam::TriangulationParameters
 
struct  gtsam::traits< const Rot2 >
class  gtsam::TriangulationResult
 TriangulationResult is an optional point, along with the reasons why it is invalid. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Typedefs

│ │ │ │ +using gtsam::CameraSetCal3Bundler = CameraSet< PinholeCamera< Cal3Bundler > >
 
│ │ │ │ +using gtsam::CameraSetCal3_S2 = CameraSet< PinholeCamera< Cal3_S2 > >
 
│ │ │ │ +using gtsam::CameraSetCal3DS2 = CameraSet< PinholeCamera< Cal3DS2 > >
 
│ │ │ │ +using gtsam::CameraSetCal3Fisheye = CameraSet< PinholeCamera< Cal3Fisheye > >
 
│ │ │ │ +using gtsam::CameraSetCal3Unified = CameraSet< PinholeCamera< Cal3Unified > >
 
│ │ │ │ +using gtsam::CameraSetSpherical = CameraSet< SphericalCamera >
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

Vector4 gtsam::triangulateHomogeneousDLT (const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > &projection_matrices, const Point2Vector &measurements, double rank_tol=1e-9)
 DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312.
 
Vector4 gtsam::triangulateHomogeneousDLT (const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > &projection_matrices, const std::vector< Unit3 > &measurements, double rank_tol=1e-9)
 Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and Zisserman)
 
Point3 gtsam::triangulateDLT (const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > &projection_matrices, const Point2Vector &measurements, double rank_tol=1e-9)
 DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312.
 
│ │ │ │ +Point3 gtsam::triangulateDLT (const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > &projection_matrices, const std::vector< Unit3 > &measurements, double rank_tol=1e-9)
 overload of previous function to work with Unit3 (projected to canonical camera)
 
Point3 gtsam::triangulateLOST (const std::vector< Pose3 > &poses, const Point3Vector &calibratedMeasurements, const SharedIsotropic &measurementNoise)
 Triangulation using the LOST (Linear Optimal Sine Triangulation) algorithm proposed in https://arxiv.org/pdf/2205.12197.pdf by Sebastien Henry and John Christian.
 
template<class CALIBRATION >
std::pair< NonlinearFactorGraph, Values > gtsam::triangulationGraph (const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, Key landmarkKey, const Point3 &initialEstimate, const SharedNoiseModel &model=noiseModel::Unit::Create(2))
 Create a factor graph with projection factors from poses and one calibration.
 
template<class CAMERA >
std::pair< NonlinearFactorGraph, Values > gtsam::triangulationGraph (const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements, Key landmarkKey, const Point3 &initialEstimate, const SharedNoiseModel &model=nullptr)
 Create a factor graph with projection factors from pinhole cameras (each camera has a pose and calibration)
 
Point3 gtsam::optimize (const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
 Optimize for triangulation.
 
template<class CALIBRATION >
Point3 gtsam::triangulateNonlinear (const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, const Point3 &initialEstimate, const SharedNoiseModel &model=nullptr)
 Given an initial estimate , refine a point using measurements in several cameras.
 
template<class CAMERA >
Point3 gtsam::triangulateNonlinear (const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements, const Point3 &initialEstimate, const SharedNoiseModel &model=nullptr)
 Given an initial estimate , refine a point using measurements in several cameras.
 
│ │ │ │ +template<class CAMERA >
std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > gtsam::projectionMatricesFromCameras (const CameraSet< CAMERA > &cameras)
 
│ │ │ │ +template<class CALIBRATION >
std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > gtsam::projectionMatricesFromPoses (const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal)
 
template<class CALIBRATION >
Cal3_S2 gtsam::createPinholeCalibration (const CALIBRATION &cal)
 Create a pinhole calibration from a different Cal3 object, removing distortion.
 
│ │ │ │ +template<class CALIBRATION , class MEASUREMENT >
MEASUREMENT gtsam::undistortMeasurementInternal (const CALIBRATION &cal, const MEASUREMENT &measurement, boost::optional< Cal3_S2 > pinholeCal=boost::none)
 Internal undistortMeasurement to be used by undistortMeasurement and undistortMeasurements.
 
template<class CALIBRATION >
Point2Vector gtsam::undistortMeasurements (const CALIBRATION &cal, const Point2Vector &measurements)
 Remove distortion for measurements so as if the measurements came from a pinhole camera.
 
│ │ │ │ +template<>
Point2Vector gtsam::undistortMeasurements (const Cal3_S2 &cal, const Point2Vector &measurements)
 Specialization for Cal3_S2 as it doesn't need to be undistorted.
 
template<class CAMERA >
CAMERA::MeasurementVector gtsam::undistortMeasurements (const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements)
 Remove distortion for measurements so as if the measurements came from a pinhole camera.
 
│ │ │ │ +template<class CAMERA = PinholeCamera<Cal3_S2>>
PinholeCamera< Cal3_S2 >::MeasurementVector gtsam::undistortMeasurements (const CameraSet< PinholeCamera< Cal3_S2 > > &cameras, const PinholeCamera< Cal3_S2 >::MeasurementVector &measurements)
 Specialize for Cal3_S2 to do nothing.
 
│ │ │ │ +template<class CAMERA = SphericalCamera>
SphericalCamera::MeasurementVector gtsam::undistortMeasurements (const CameraSet< SphericalCamera > &cameras, const SphericalCamera::MeasurementVector &measurements)
 Specialize for SphericalCamera to do nothing.
 
template<class CALIBRATION >
Point3Vector gtsam::calibrateMeasurementsShared (const CALIBRATION &cal, const Point2Vector &measurements)
 Convert pixel measurements in image to homogeneous measurements in the image plane using shared camera intrinsics.
 
template<class CAMERA >
Point3Vector gtsam::calibrateMeasurements (const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements)
 Convert pixel measurements in image to homogeneous measurements in the image plane using camera intrinsics of each measurement.
 
│ │ │ │ +template<class CAMERA = SphericalCamera>
Point3Vector gtsam::calibrateMeasurements (const CameraSet< SphericalCamera > &cameras, const SphericalCamera::MeasurementVector &measurements)
 Specialize for SphericalCamera to do nothing.
 
template<class CALIBRATION >
Point3 gtsam::triangulatePoint3 (const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, double rank_tol=1e-9, bool optimize=false, const SharedNoiseModel &model=nullptr, const bool useLOST=false)
 Function to triangulate 3D landmark point from an arbitrary number of poses (at least 2) using the DLT.
 
template<class CAMERA >
Point3 gtsam::triangulatePoint3 (const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements, double rank_tol=1e-9, bool optimize=false, const SharedNoiseModel &model=nullptr, const bool useLOST=false)
 Function to triangulate 3D landmark point from an arbitrary number of poses (at least 2) using the DLT.
 
│ │ │ │ +template<class CALIBRATION >
Point3 gtsam::triangulatePoint3 (const CameraSet< PinholeCamera< CALIBRATION > > &cameras, const Point2Vector &measurements, double rank_tol=1e-9, bool optimize=false, const SharedNoiseModel &model=nullptr, const bool useLOST=false)
 Pinhole-specific version.
 
│ │ │ │ +template<class CAMERA >
TriangulationResult gtsam::triangulateSafe (const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters &params)
 triangulateSafe: extensive checking of the outcome
 
│ │ │ │

Detailed Description

│ │ │ │ -

2D rotation

│ │ │ │ -
Date
Dec 9, 2009
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +

Functions for triangulation.

│ │ │ │ +
Date
July 31, 2013
│ │ │ │ +
Author
Chris Beall
│ │ │ │ +
│ │ │ │ +Akshay Krishnan
│ │ │ │ +
Date
July 31, 2013
│ │ │ │ +
Author
Chris Beall
│ │ │ │ +
│ │ │ │ +Luca Carlone
│ │ │ │
│ │ │ │ -John Lambert
│ │ │ │ +Akshay Krishnan
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,35 +1,295 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Rot2.h File Reference │ │ │ │ │ -2D rotation _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +triangulation.h File Reference │ │ │ │ │ +Functions for triangulation. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_R_o_t_2 │ │ │ │ │ -  Rotation matrix NOTE: the angle theta is in radians unless explicitly │ │ │ │ │ - stated. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_U_n_d_e_r_c_o_n_s_t_r_a_i_n_e_d_E_x_c_e_p_t_i_o_n │ │ │ │ │ +  Exception thrown by triangulateDLT when SVD returns rank < 3. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_R_o_t_2_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n │ │ │ │ │ + class   _g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n │ │ │ │ │ +  Exception thrown by triangulateDLT when landmark is behind one or more │ │ │ │ │ + of the cameras. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _R_o_t_2_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _R_o_t_2_ _> │ │ │ │ │ + class   _g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t │ │ │ │ │ +  _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t is an optional point, along with the reasons why │ │ │ │ │ + it is invalid. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +using  ggttssaamm::::CCaammeerraaSSeettCCaall33BBuunnddlleerr = _C_a_m_e_r_a_S_e_t< _P_i_n_h_o_l_e_C_a_m_e_r_a< _C_a_l_3_B_u_n_d_l_e_r > > │ │ │ │ │ +  │ │ │ │ │ +using  ggttssaamm::::CCaammeerraaSSeettCCaall33__SS22 = _C_a_m_e_r_a_S_e_t< _P_i_n_h_o_l_e_C_a_m_e_r_a< _C_a_l_3___S_2 > > │ │ │ │ │ +  │ │ │ │ │ +using  ggttssaamm::::CCaammeerraaSSeettCCaall33DDSS22 = _C_a_m_e_r_a_S_e_t< _P_i_n_h_o_l_e_C_a_m_e_r_a< _C_a_l_3_D_S_2 > > │ │ │ │ │ +  │ │ │ │ │ +using  ggttssaamm::::CCaammeerraaSSeettCCaall33FFiisshheeyyee = _C_a_m_e_r_a_S_e_t< _P_i_n_h_o_l_e_C_a_m_e_r_a< _C_a_l_3_F_i_s_h_e_y_e > > │ │ │ │ │ +  │ │ │ │ │ +using  ggttssaamm::::CCaammeerraaSSeettCCaall33UUnniiffiieedd = _C_a_m_e_r_a_S_e_t< _P_i_n_h_o_l_e_C_a_m_e_r_a< _C_a_l_3_U_n_i_f_i_e_d > > │ │ │ │ │ +  │ │ │ │ │ +using  ggttssaamm::::CCaammeerraaSSeettSSpphheerriiccaall = _C_a_m_e_r_a_S_e_t< _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a > │ │ │ │ │ +  │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ + Vector4  _g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_H_o_m_o_g_e_n_e_o_u_s_D_L_T (const │ │ │ │ │ + std::vector< Matrix34, Eigen:: │ │ │ │ │ + aligned_allocator< Matrix34 > > │ │ │ │ │ + &projection_matrices, const │ │ │ │ │ + Point2Vector &measurements, double │ │ │ │ │ + rank_tol=1e-9) │ │ │ │ │ +  DLT triangulation: See Hartley and │ │ │ │ │ + Zisserman, 2nd Ed., page 312. │ │ │ │ │ +  │ │ │ │ │ + Vector4  _g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_H_o_m_o_g_e_n_e_o_u_s_D_L_T (const │ │ │ │ │ + std::vector< Matrix34, Eigen:: │ │ │ │ │ + aligned_allocator< Matrix34 > > │ │ │ │ │ + &projection_matrices, const std:: │ │ │ │ │ + vector< _U_n_i_t_3 > &measurements, double │ │ │ │ │ + rank_tol=1e-9) │ │ │ │ │ + Same math as Hartley and Zisserman, 2nd │ │ │ │ │ + Ed., page 312, but with unit-norm │ │ │ │ │ +  bearing vectors (contrarily to pinhole │ │ │ │ │ + projection, the z entry is not assumed │ │ │ │ │ + to be 1 as in Hartley and Zisserman) │ │ │ │ │ +  │ │ │ │ │ + _P_o_i_n_t_3  _g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_D_L_T (const std:: │ │ │ │ │ + vector< Matrix34, Eigen:: │ │ │ │ │ + aligned_allocator< Matrix34 > > │ │ │ │ │ + &projection_matrices, const │ │ │ │ │ + Point2Vector &measurements, double │ │ │ │ │ + rank_tol=1e-9) │ │ │ │ │ +  DLT triangulation: See Hartley and │ │ │ │ │ + Zisserman, 2nd Ed., page 312. │ │ │ │ │ +  │ │ │ │ │ + _P_o_i_n_t_3  ggttssaamm::::ttrriiaanngguullaatteeDDLLTT (const std:: │ │ │ │ │ + vector< Matrix34, Eigen:: │ │ │ │ │ + aligned_allocator< Matrix34 > > │ │ │ │ │ + &projection_matrices, const std:: │ │ │ │ │ + vector< _U_n_i_t_3 > &measurements, double │ │ │ │ │ + rank_tol=1e-9) │ │ │ │ │ + overload of previous function to work │ │ │ │ │ +  with _U_n_i_t_3 (projected to canonical │ │ │ │ │ + camera) │ │ │ │ │ +  │ │ │ │ │ + _P_o_i_n_t_3  _g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_L_O_S_T (const std:: │ │ │ │ │ + vector< _P_o_s_e_3 > &poses, const │ │ │ │ │ + Point3Vector &calibratedMeasurements, │ │ │ │ │ + const SharedIsotropic │ │ │ │ │ + &measurementNoise) │ │ │ │ │ + Triangulation using the LOST (Linear │ │ │ │ │ + Optimal Sine Triangulation) algorithm │ │ │ │ │ +  proposed in _h_t_t_p_s_:_/_/_a_r_x_i_v_._o_r_g_/_p_d_f_/ │ │ │ │ │ + _2_2_0_5_._1_2_1_9_7_._p_d_f by Sebastien Henry and │ │ │ │ │ + John Christian. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +std::pair< _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h, _V_a_l_u_e_s _g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_i_o_n_G_r_a_p_h (const std:: │ │ │ │ │ + >  vector< _P_o_s_e_3 > &poses, boost:: │ │ │ │ │ + shared_ptr< CALIBRATION > sharedCal, │ │ │ │ │ + const Point2Vector &measurements, _K_e_y │ │ │ │ │ + landmarkKey, const _P_o_i_n_t_3 │ │ │ │ │ + &initialEstimate, const │ │ │ │ │ + _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model=_n_o_i_s_e_M_o_d_e_l_:_: │ │ │ │ │ + _U_n_i_t_:_:_C_r_e_a_t_e(2)) │ │ │ │ │ +  Create a factor graph with projection │ │ │ │ │ + factors from poses and one calibration. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +std::pair< _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h, _V_a_l_u_e_s _g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_i_o_n_G_r_a_p_h (const │ │ │ │ │ + >  _C_a_m_e_r_a_S_e_t< CAMERA > &cameras, const │ │ │ │ │ + typename CAMERA::MeasurementVector │ │ │ │ │ + &measurements, _K_e_y landmarkKey, const │ │ │ │ │ + _P_o_i_n_t_3 &initialEstimate, const │ │ │ │ │ + _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model=nullptr) │ │ │ │ │ + Create a factor graph with projection │ │ │ │ │ +  factors from pinhole cameras (each │ │ │ │ │ + camera has a pose and calibration) │ │ │ │ │ +  │ │ │ │ │ + _P_o_i_n_t_3  _g_t_s_a_m_:_:_o_p_t_i_m_i_z_e (const │ │ │ │ │ + _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h &graph, const │ │ │ │ │ + _V_a_l_u_e_s &values, _K_e_y landmarkKey) │ │ │ │ │ +  Optimize for triangulation. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _P_o_i_n_t_3  _g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_N_o_n_l_i_n_e_a_r (const │ │ │ │ │ + std::vector< _P_o_s_e_3 > &poses, boost:: │ │ │ │ │ + shared_ptr< CALIBRATION > sharedCal, │ │ │ │ │ + const Point2Vector &measurements, const │ │ │ │ │ + _P_o_i_n_t_3 &initialEstimate, const │ │ │ │ │ + _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model=nullptr) │ │ │ │ │ + Given an initial estimate , refine a │ │ │ │ │ +  point using measurements in several │ │ │ │ │ + cameras. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _P_o_i_n_t_3  _g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_N_o_n_l_i_n_e_a_r (const │ │ │ │ │ + _C_a_m_e_r_a_S_e_t< CAMERA > &cameras, const │ │ │ │ │ + typename CAMERA::MeasurementVector │ │ │ │ │ + &measurements, const _P_o_i_n_t_3 │ │ │ │ │ + &initialEstimate, const │ │ │ │ │ + _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model=nullptr) │ │ │ │ │ + Given an initial estimate , refine a │ │ │ │ │ +  point using measurements in several │ │ │ │ │ + cameras. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + std::vector< Matrix34, Eigen:: ggttssaamm::::pprroojjeeccttiioonnMMaattrriicceessFFrroommCCaammeerraass │ │ │ │ │ + aligned_allocator< Matrix34 > >  (const _C_a_m_e_r_a_S_e_t< CAMERA > &cameras) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + std::vector< Matrix34, Eigen:: ggttssaamm::::pprroojjeeccttiioonnMMaattrriicceessFFrroommPPoosseess │ │ │ │ │ + aligned_allocator< Matrix34 > >  (const std::vector< _P_o_s_e_3 > &poses, │ │ │ │ │ + boost::shared_ptr< CALIBRATION > │ │ │ │ │ + sharedCal) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _C_a_l_3___S_2  _g_t_s_a_m_:_:_c_r_e_a_t_e_P_i_n_h_o_l_e_C_a_l_i_b_r_a_t_i_o_n (const │ │ │ │ │ + CALIBRATION &cal) │ │ │ │ │ + Create a pinhole calibration from a │ │ │ │ │ +  different _C_a_l_3 object, removing │ │ │ │ │ + distortion. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + MEASUREMENT  ggttssaamm::::uunnddiissttoorrttMMeeaassuurreemmeennttIInntteerrnnaall │ │ │ │ │ + (const CALIBRATION &cal, const │ │ │ │ │ + MEASUREMENT &measurement, boost:: │ │ │ │ │ + optional< _C_a_l_3___S_2 > pinholeCal=boost:: │ │ │ │ │ + none) │ │ │ │ │ + Internal undistortMeasurement to be │ │ │ │ │ +  used by undistortMeasurement and │ │ │ │ │ + undistortMeasurements. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + Point2Vector  _g_t_s_a_m_:_:_u_n_d_i_s_t_o_r_t_M_e_a_s_u_r_e_m_e_n_t_s (const │ │ │ │ │ + CALIBRATION &cal, const Point2Vector │ │ │ │ │ + &measurements) │ │ │ │ │ + Remove distortion for measurements so │ │ │ │ │ +  as if the measurements came from a │ │ │ │ │ + pinhole camera. │ │ │ │ │ +  │ │ │ │ │ + template<> │ │ │ │ │ + Point2Vector  ggttssaamm::::uunnddiissttoorrttMMeeaassuurreemmeennttss (const │ │ │ │ │ + _C_a_l_3___S_2 &cal, const Point2Vector │ │ │ │ │ + &measurements) │ │ │ │ │ +  Specialization for _C_a_l_3___S_2 as it │ │ │ │ │ + doesn't need to be undistorted. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + CAMERA::MeasurementVector  _g_t_s_a_m_:_:_u_n_d_i_s_t_o_r_t_M_e_a_s_u_r_e_m_e_n_t_s (const │ │ │ │ │ + _C_a_m_e_r_a_S_e_t< CAMERA > &cameras, const │ │ │ │ │ + typename CAMERA::MeasurementVector │ │ │ │ │ + &measurements) │ │ │ │ │ + Remove distortion for measurements so │ │ │ │ │ +  as if the measurements came from a │ │ │ │ │ + pinhole camera. │ │ │ │ │ +  │ │ │ │ │ +template> │ │ │ │ │ + _P_i_n_h_o_l_e_C_a_m_e_r_a< _C_a_l_3___S_2 >:: ggttssaamm::::uunnddiissttoorrttMMeeaassuurreemmeennttss (const │ │ │ │ │ + MeasurementVector  _C_a_m_e_r_a_S_e_t< _P_i_n_h_o_l_e_C_a_m_e_r_a< _C_a_l_3___S_2 > > │ │ │ │ │ + &cameras, const _P_i_n_h_o_l_e_C_a_m_e_r_a< _C_a_l_3___S_2 │ │ │ │ │ + >::MeasurementVector &measurements) │ │ │ │ │ +  Specialize for _C_a_l_3___S_2 to do nothing. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + SphericalCamera::MeasurementVector  ggttssaamm::::uunnddiissttoorrttMMeeaassuurreemmeennttss (const │ │ │ │ │ + _C_a_m_e_r_a_S_e_t< _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a > &cameras, │ │ │ │ │ + const SphericalCamera:: │ │ │ │ │ + MeasurementVector &measurements) │ │ │ │ │ +  Specialize for _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a to do │ │ │ │ │ + nothing. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + Point3Vector  _g_t_s_a_m_:_:_c_a_l_i_b_r_a_t_e_M_e_a_s_u_r_e_m_e_n_t_s_S_h_a_r_e_d │ │ │ │ │ + (const CALIBRATION &cal, const │ │ │ │ │ + Point2Vector &measurements) │ │ │ │ │ + Convert pixel measurements in image to │ │ │ │ │ +  homogeneous measurements in the image │ │ │ │ │ + plane using shared camera intrinsics. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + Point3Vector  _g_t_s_a_m_:_:_c_a_l_i_b_r_a_t_e_M_e_a_s_u_r_e_m_e_n_t_s (const │ │ │ │ │ + _C_a_m_e_r_a_S_e_t< CAMERA > &cameras, const │ │ │ │ │ + typename CAMERA::MeasurementVector │ │ │ │ │ + &measurements) │ │ │ │ │ + Convert pixel measurements in image to │ │ │ │ │ +  homogeneous measurements in the image │ │ │ │ │ + plane using camera intrinsics of each │ │ │ │ │ + measurement. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + Point3Vector  ggttssaamm::::ccaalliibbrraatteeMMeeaassuurreemmeennttss (const │ │ │ │ │ + _C_a_m_e_r_a_S_e_t< _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a > &cameras, │ │ │ │ │ + const SphericalCamera:: │ │ │ │ │ + MeasurementVector &measurements) │ │ │ │ │ +  Specialize for _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a to do │ │ │ │ │ + nothing. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _P_o_i_n_t_3  _g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_P_o_i_n_t_3 (const std:: │ │ │ │ │ + vector< _P_o_s_e_3 > &poses, boost:: │ │ │ │ │ + shared_ptr< CALIBRATION > sharedCal, │ │ │ │ │ + const Point2Vector &measurements, │ │ │ │ │ + double rank_tol=1e-9, bool │ │ │ │ │ + _o_p_t_i_m_i_z_e=false, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ + &model=nullptr, const bool │ │ │ │ │ + useLOST=false) │ │ │ │ │ + Function to triangulate 3D landmark │ │ │ │ │ +  point from an arbitrary number of poses │ │ │ │ │ + (at least 2) using the DLT. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _P_o_i_n_t_3  _g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_P_o_i_n_t_3 (const │ │ │ │ │ + _C_a_m_e_r_a_S_e_t< CAMERA > &cameras, const │ │ │ │ │ + typename CAMERA::MeasurementVector │ │ │ │ │ + &measurements, double rank_tol=1e-9, │ │ │ │ │ + bool _o_p_t_i_m_i_z_e=false, const │ │ │ │ │ + _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model=nullptr, const │ │ │ │ │ + bool useLOST=false) │ │ │ │ │ + Function to triangulate 3D landmark │ │ │ │ │ +  point from an arbitrary number of poses │ │ │ │ │ + (at least 2) using the DLT. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _P_o_i_n_t_3  ggttssaamm::::ttrriiaanngguullaatteePPooiinntt33 (const │ │ │ │ │ + _C_a_m_e_r_a_S_e_t< _P_i_n_h_o_l_e_C_a_m_e_r_a< CALIBRATION > │ │ │ │ │ + > &cameras, const Point2Vector │ │ │ │ │ + &measurements, double rank_tol=1e-9, │ │ │ │ │ + bool _o_p_t_i_m_i_z_e=false, const │ │ │ │ │ + _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model=nullptr, const │ │ │ │ │ + bool useLOST=false) │ │ │ │ │ +  Pinhole-specific version. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t  ggttssaamm::::ttrriiaanngguullaatteeSSaaffee (const │ │ │ │ │ + _C_a_m_e_r_a_S_e_t< CAMERA > &cameras, const │ │ │ │ │ + typename CAMERA::MeasurementVector │ │ │ │ │ + &measured, const │ │ │ │ │ + _T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s ¶ms) │ │ │ │ │ +  triangulateSafe: extensive checking of │ │ │ │ │ + the outcome │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -2D rotation │ │ │ │ │ +Functions for triangulation. │ │ │ │ │ + Date │ │ │ │ │ + July 31, 2013 │ │ │ │ │ + Author │ │ │ │ │ + Chris Beall │ │ │ │ │ + Akshay Krishnan │ │ │ │ │ Date │ │ │ │ │ - Dec 9, 2009 │ │ │ │ │ + July 31, 2013 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - John Lambert │ │ │ │ │ + Chris Beall │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Akshay Krishnan │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _R_o_t_2_._h │ │ │ │ │ + * _t_r_i_a_n_g_u_l_a_t_i_o_n_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00407.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,5 +1,30 @@ │ │ │ │ │ var a00407 = [ │ │ │ │ │ - ["gtsam::Rot2::ChartAtOrigin", "a03216.html", null], │ │ │ │ │ - ["gtsam::traits< Rot2 >", "a03220.html", null], │ │ │ │ │ - ["gtsam::traits< const Rot2 >", "a03224.html", null] │ │ │ │ │ + ["gtsam::TriangulationUnderconstrainedException", "a03364.html", null], │ │ │ │ │ + ["gtsam::TriangulationCheiralityException", "a03368.html", null], │ │ │ │ │ + ["gtsam::TriangulationParameters", "a03372.html", "a03372"], │ │ │ │ │ + ["gtsam::TriangulationResult", "a03376.html", "a03376"], │ │ │ │ │ + ["calibrateMeasurements", "a00407.html#aebe53b43e5151ce9c355b4aa4ba57c13", null], │ │ │ │ │ + ["calibrateMeasurements", "a00407.html#a48ad2c1085fcae3881fa956f4cd81890", null], │ │ │ │ │ + ["calibrateMeasurementsShared", "a00407.html#a0a386184e9ed9cc4ec844d48d91ba759", null], │ │ │ │ │ + ["createPinholeCalibration", "a00407.html#a3d070e707c4b2d69d0f024a30501f06c", null], │ │ │ │ │ + ["optimize", "a00407.html#a4b0057879b0c5f323185452b0d1a4232", null], │ │ │ │ │ + ["triangulateDLT", "a00407.html#aed2918ac0049316bf09f96879efca3b6", null], │ │ │ │ │ + ["triangulateDLT", "a00407.html#a523a0528f883f1c569a8a4c9a0e514a0", null], │ │ │ │ │ + ["triangulateHomogeneousDLT", "a00407.html#af378e231b519e8bb1f09f4aa13a38dca", null], │ │ │ │ │ + ["triangulateHomogeneousDLT", "a00407.html#a8d16887f8890aaf7ceb5e3cbb181a191", null], │ │ │ │ │ + ["triangulateLOST", "a00407.html#a0f9e5111ffc481b34f0123418b0ecd13", null], │ │ │ │ │ + ["triangulateNonlinear", "a00407.html#a13ac0858b6f6600f5a4242aeb797692f", null], │ │ │ │ │ + ["triangulateNonlinear", "a00407.html#a9dab6b5829b51511735b6b841bb36a36", null], │ │ │ │ │ + ["triangulatePoint3", "a00407.html#a58f4009e3a8872d0ac6780d654cccc2e", null], │ │ │ │ │ + ["triangulatePoint3", "a00407.html#aac6df5bbfb7131cbb05e2a9691e59ba3", null], │ │ │ │ │ + ["triangulatePoint3", "a00407.html#ab79854d3cb3bcc4f562fc88a0731a447", null], │ │ │ │ │ + ["triangulateSafe", "a00407.html#a8d52d7d8a5e5c8d7318ce95ee57e9cfa", null], │ │ │ │ │ + ["triangulationGraph", "a00407.html#a2b96bc32f3c5c436db3fe60e5139319e", null], │ │ │ │ │ + ["triangulationGraph", "a00407.html#ad167fac94fa72c0d3b8db36e5fa2becb", null], │ │ │ │ │ + ["undistortMeasurementInternal", "a00407.html#a46a14c34d729a626825e4eff8a7de8dd", null], │ │ │ │ │ + ["undistortMeasurements", "a00407.html#af01b2988c1bb89268ad572ea7d7ba293", null], │ │ │ │ │ + ["undistortMeasurements", "a00407.html#a31e52d463db397f0dda1a13352ab217c", null], │ │ │ │ │ + ["undistortMeasurements", "a00407.html#a1a0e4ac1b773cc295daae7849c833876", null], │ │ │ │ │ + ["undistortMeasurements", "a00407.html#a7f3d15de99fffcc537089fa1440d8ca2", null], │ │ │ │ │ + ["undistortMeasurements", "a00407.html#a6a1849035b3acd163d6de715e7683ebf", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00407_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Rot2.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/triangulation.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,230 +98,675 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Rot2.h
│ │ │ │ +
triangulation.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/geometry/Point2.h>
│ │ │ │ -
23#include <gtsam/base/Lie.h>
│ │ │ │ -
24#include <boost/optional.hpp>
│ │ │ │ -
25
│ │ │ │ -
26#include <random>
│ │ │ │ -
27
│ │ │ │ -
28namespace gtsam {
│ │ │ │ -
29
│ │ │ │ -
│ │ │ │ -
36 class GTSAM_EXPORT Rot2 : public LieGroup<Rot2, 1> {
│ │ │ │ +
21#pragma once
│ │ │ │ +
22
│ │ │ │ +
23#include <gtsam/geometry/Cal3Bundler.h>
│ │ │ │ +
24#include <gtsam/geometry/Cal3Fisheye.h>
│ │ │ │ +
25#include <gtsam/geometry/Cal3Unified.h>
│ │ │ │ +
26#include <gtsam/geometry/Cal3_S2.h>
│ │ │ │ +
27#include <gtsam/geometry/Cal3DS2.h>
│ │ │ │ +
28#include <gtsam/geometry/CameraSet.h>
│ │ │ │ +
29#include <gtsam/geometry/PinholeCamera.h>
│ │ │ │ +
30#include <gtsam/geometry/SphericalCamera.h>
│ │ │ │ +
31#include <gtsam/geometry/Pose2.h>
│ │ │ │ +
32#include <gtsam/inference/Symbol.h>
│ │ │ │ +
33#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ +
34#include <gtsam/slam/TriangulationFactor.h>
│ │ │ │ +
35
│ │ │ │ +
36namespace gtsam {
│ │ │ │
37
│ │ │ │ -
39 double c_, s_;
│ │ │ │ -
40
│ │ │ │ -
42 Rot2& normalize();
│ │ │ │ -
43
│ │ │ │ -
45 inline Rot2(double c, double s) : c_(c), s_(s) {}
│ │ │ │ -
46
│ │ │ │ -
47 public:
│ │ │ │ -
48
│ │ │ │ -
51
│ │ │ │ -
53 Rot2() : c_(1.0), s_(0.0) {}
│ │ │ │ -
54
│ │ │ │ -
56 Rot2(const Rot2& r) : Rot2(r.c_, r.s_) {}
│ │ │ │ -
57
│ │ │ │ -
59 Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}
│ │ │ │ -
60
│ │ │ │ -
│ │ │ │ -
62 static Rot2 fromAngle(double theta) {
│ │ │ │ -
63 return Rot2(theta);
│ │ │ │ -
64 }
│ │ │ │ -
│ │ │ │ +
│ │ │ │ +
39class GTSAM_EXPORT TriangulationUnderconstrainedException: public std::runtime_error {
│ │ │ │ +
40public:
│ │ │ │ +
41 TriangulationUnderconstrainedException() :
│ │ │ │ +
42 std::runtime_error("Triangulation Underconstrained Exception.") {
│ │ │ │ +
43 }
│ │ │ │ +
44};
│ │ │ │ +
│ │ │ │ +
45
│ │ │ │ +
│ │ │ │ +
47class GTSAM_EXPORT TriangulationCheiralityException: public std::runtime_error {
│ │ │ │ +
48public:
│ │ │ │ +
49 TriangulationCheiralityException() :
│ │ │ │ +
50 std::runtime_error(
│ │ │ │ +
51 "Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") {
│ │ │ │ +
52 }
│ │ │ │ +
53};
│ │ │ │ +
│ │ │ │ +
54
│ │ │ │ +
62GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
│ │ │ │ +
63 const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
│ │ │ │ +
64 const Point2Vector& measurements, double rank_tol = 1e-9);
│ │ │ │
65
│ │ │ │ -
│ │ │ │ -
67 static Rot2 fromDegrees(double theta) {
│ │ │ │ -
68 static const double degree = M_PI / 180;
│ │ │ │ -
69 return fromAngle(theta * degree);
│ │ │ │ -
70 }
│ │ │ │ -
│ │ │ │ -
71
│ │ │ │ -
73 static Rot2 fromCosSin(double c, double s);
│ │ │ │ -
74
│ │ │ │ -
82 static Rot2 relativeBearing(const Point2& d, OptionalJacobian<1,2> H =
│ │ │ │ -
83 boost::none);
│ │ │ │ -
84
│ │ │ │ -
86 static Rot2 atan2(double y, double x);
│ │ │ │ -
87
│ │ │ │ -
94 static Rot2 Random(std::mt19937 & rng);
│ │ │ │ -
95
│ │ │ │ -
99
│ │ │ │ -
101 void print(const std::string& s = "theta") const;
│ │ │ │ -
102
│ │ │ │ -
104 bool equals(const Rot2& R, double tol = 1e-9) const;
│ │ │ │ -
105
│ │ │ │ -
109
│ │ │ │ -
111 inline static Rot2 Identity() { return Rot2(); }
│ │ │ │ -
112
│ │ │ │ -
114 Rot2 inverse() const { return Rot2(c_, -s_);}
│ │ │ │ -
115
│ │ │ │ -
│ │ │ │ -
117 Rot2 operator*(const Rot2& R) const {
│ │ │ │ -
118 return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
│ │ │ │ -
119 }
│ │ │ │ -
│ │ │ │ -
120
│ │ │ │ -
124
│ │ │ │ -
126 static Rot2 Expmap(const Vector1& v, ChartJacobian H = boost::none);
│ │ │ │ -
127
│ │ │ │ -
129 static Vector1 Logmap(const Rot2& r, ChartJacobian H = boost::none);
│ │ │ │ -
130
│ │ │ │ -
132 Matrix1 AdjointMap() const { return I_1x1; }
│ │ │ │ -
133
│ │ │ │ -
│ │ │ │ -
135 static Matrix ExpmapDerivative(const Vector& /*v*/) {
│ │ │ │ -
136 return I_1x1;
│ │ │ │ -
137 }
│ │ │ │ -
│ │ │ │ -
138
│ │ │ │ -
│ │ │ │ -
140 static Matrix LogmapDerivative(const Vector& /*v*/) {
│ │ │ │ -
141 return I_1x1;
│ │ │ │ -
142 }
│ │ │ │ -
│ │ │ │ -
143
│ │ │ │ -
144 // Chart at origin simply uses exponential map and its inverse
│ │ │ │ -
│ │ │ │ -
145 struct ChartAtOrigin {
│ │ │ │ -
146 static Rot2 Retract(const Vector1& v, ChartJacobian H = boost::none) {
│ │ │ │ -
147 return Expmap(v, H);
│ │ │ │ -
148 }
│ │ │ │ -
149 static Vector1 Local(const Rot2& r, ChartJacobian H = boost::none) {
│ │ │ │ -
150 return Logmap(r, H);
│ │ │ │ -
151 }
│ │ │ │ -
152 };
│ │ │ │ -
│ │ │ │ -
153
│ │ │ │ -
154 using LieGroup<Rot2, 1>::inverse; // version with derivative
│ │ │ │ -
155
│ │ │ │ -
159
│ │ │ │ -
163 Point2 rotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none,
│ │ │ │ -
164 OptionalJacobian<2, 2> H2 = boost::none) const;
│ │ │ │ -
165
│ │ │ │ -
│ │ │ │ -
167 inline Point2 operator*(const Point2& p) const {
│ │ │ │ -
168 return rotate(p);
│ │ │ │ -
169 }
│ │ │ │ -
│ │ │ │ -
170
│ │ │ │ -
174 Point2 unrotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none,
│ │ │ │ -
175 OptionalJacobian<2, 2> H2 = boost::none) const;
│ │ │ │ -
176
│ │ │ │ -
180
│ │ │ │ -
│ │ │ │ -
182 inline Point2 unit() const {
│ │ │ │ -
183 return Point2(c_, s_);
│ │ │ │ -
184 }
│ │ │ │ -
│ │ │ │ -
185
│ │ │ │ +
74GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
│ │ │ │ +
75 const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
│ │ │ │ +
76 const std::vector<Unit3>& measurements, double rank_tol = 1e-9);
│ │ │ │ +
77
│ │ │ │ +
85GTSAM_EXPORT Point3 triangulateDLT(
│ │ │ │ +
86 const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
│ │ │ │ +
87 const Point2Vector& measurements,
│ │ │ │ +
88 double rank_tol = 1e-9);
│ │ │ │ +
89
│ │ │ │ +
93GTSAM_EXPORT Point3 triangulateDLT(
│ │ │ │ +
94 const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
│ │ │ │ +
95 const std::vector<Unit3>& measurements,
│ │ │ │ +
96 double rank_tol = 1e-9);
│ │ │ │ +
97
│ │ │ │ +
108GTSAM_EXPORT Point3 triangulateLOST(const std::vector<Pose3>& poses,
│ │ │ │ +
109 const Point3Vector& calibratedMeasurements,
│ │ │ │ +
110 const SharedIsotropic& measurementNoise);
│ │ │ │ +
111
│ │ │ │ +
121template<class CALIBRATION>
│ │ │ │ +
│ │ │ │ +
122std::pair<NonlinearFactorGraph, Values> triangulationGraph(
│ │ │ │ +
123 const std::vector<Pose3>& poses, boost::shared_ptr<CALIBRATION> sharedCal,
│ │ │ │ +
124 const Point2Vector& measurements, Key landmarkKey,
│ │ │ │ +
125 const Point3& initialEstimate,
│ │ │ │ +
126 const SharedNoiseModel& model = noiseModel::Unit::Create(2)) {
│ │ │ │ +
127 Values values;
│ │ │ │ +
128 values.insert(landmarkKey, initialEstimate); // Initial landmark value
│ │ │ │ +
129 NonlinearFactorGraph graph;
│ │ │ │ +
130 for (size_t i = 0; i < measurements.size(); i++) {
│ │ │ │ +
131 const Pose3& pose_i = poses[i];
│ │ │ │ +
132 typedef PinholePose<CALIBRATION> Camera;
│ │ │ │ +
133 Camera camera_i(pose_i, sharedCal);
│ │ │ │ +
134 graph.emplace_shared<TriangulationFactor<Camera> > //
│ │ │ │ +
135 (camera_i, measurements[i], model, landmarkKey);
│ │ │ │ +
136 }
│ │ │ │ +
137 return std::make_pair(graph, values);
│ │ │ │ +
138}
│ │ │ │ +
│ │ │ │ +
139
│ │ │ │ +
149template<class CAMERA>
│ │ │ │ +
│ │ │ │ +
150std::pair<NonlinearFactorGraph, Values> triangulationGraph(
│ │ │ │ +
151 const CameraSet<CAMERA>& cameras,
│ │ │ │ +
152 const typename CAMERA::MeasurementVector& measurements, Key landmarkKey,
│ │ │ │ +
153 const Point3& initialEstimate,
│ │ │ │ +
154 const SharedNoiseModel& model = nullptr) {
│ │ │ │ +
155 Values values;
│ │ │ │ +
156 values.insert(landmarkKey, initialEstimate); // Initial landmark value
│ │ │ │ +
157 NonlinearFactorGraph graph;
│ │ │ │ +
158 static SharedNoiseModel unit(noiseModel::Unit::Create(
│ │ │ │ +
159 traits<typename CAMERA::Measurement>::dimension));
│ │ │ │ +
160 for (size_t i = 0; i < measurements.size(); i++) {
│ │ │ │ +
161 const CAMERA& camera_i = cameras[i];
│ │ │ │ +
162 graph.emplace_shared<TriangulationFactor<CAMERA> > //
│ │ │ │ +
163 (camera_i, measurements[i], model? model : unit, landmarkKey);
│ │ │ │ +
164 }
│ │ │ │ +
165 return std::make_pair(graph, values);
│ │ │ │ +
166}
│ │ │ │ +
│ │ │ │ +
167
│ │ │ │ +
175GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph,
│ │ │ │ +
176 const Values& values, Key landmarkKey);
│ │ │ │ +
177
│ │ │ │ +
186template<class CALIBRATION>
│ │ │ │
│ │ │ │ -
187 double theta() const {
│ │ │ │ -
188 return ::atan2(s_, c_);
│ │ │ │ -
189 }
│ │ │ │ -
│ │ │ │ -
190
│ │ │ │ -
│ │ │ │ -
192 double degrees() const {
│ │ │ │ -
193 const double degree = M_PI / 180;
│ │ │ │ -
194 return theta() / degree;
│ │ │ │ -
195 }
│ │ │ │ -
│ │ │ │ -
196
│ │ │ │ -
│ │ │ │ -
198 inline double c() const {
│ │ │ │ -
199 return c_;
│ │ │ │ -
200 }
│ │ │ │ -
│ │ │ │ -
201
│ │ │ │ -
│ │ │ │ -
203 inline double s() const {
│ │ │ │ -
204 return s_;
│ │ │ │ -
205 }
│ │ │ │ -
│ │ │ │ -
206
│ │ │ │ -
208 Matrix2 matrix() const;
│ │ │ │ -
209
│ │ │ │ -
211 Matrix2 transpose() const;
│ │ │ │ -
212
│ │ │ │ -
214 static Rot2 ClosestTo(const Matrix2& M);
│ │ │ │ -
215
│ │ │ │ -
216 private:
│ │ │ │ -
218 friend class boost::serialization::access;
│ │ │ │ -
219 template<class ARCHIVE>
│ │ │ │ -
220 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
221 ar & BOOST_SERIALIZATION_NVP(c_);
│ │ │ │ -
222 ar & BOOST_SERIALIZATION_NVP(s_);
│ │ │ │ -
223 }
│ │ │ │ -
224
│ │ │ │ -
225 };
│ │ │ │ -
│ │ │ │ -
226
│ │ │ │ -
227 template<>
│ │ │ │ -
228 struct traits<Rot2> : public internal::LieGroup<Rot2> {};
│ │ │ │ -
229
│ │ │ │ -
230 template<>
│ │ │ │ -
231 struct traits<const Rot2> : public internal::LieGroup<Rot2> {};
│ │ │ │ +
187Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
│ │ │ │ +
188 boost::shared_ptr<CALIBRATION> sharedCal,
│ │ │ │ +
189 const Point2Vector& measurements, const Point3& initialEstimate,
│ │ │ │ +
190 const SharedNoiseModel& model = nullptr) {
│ │ │ │ +
191
│ │ │ │ +
192 // Create a factor graph and initial values
│ │ │ │ +
193 Values values;
│ │ │ │ +
194 NonlinearFactorGraph graph;
│ │ │ │ +
195 boost::tie(graph, values) = triangulationGraph<CALIBRATION> //
│ │ │ │ +
196 (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate, model);
│ │ │ │ +
197
│ │ │ │ +
198 return optimize(graph, values, Symbol('p', 0));
│ │ │ │ +
199}
│ │ │ │ +
│ │ │ │ +
200
│ │ │ │ +
208template<class CAMERA>
│ │ │ │ +
│ │ │ │ +
209Point3 triangulateNonlinear(
│ │ │ │ +
210 const CameraSet<CAMERA>& cameras,
│ │ │ │ +
211 const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate,
│ │ │ │ +
212 const SharedNoiseModel& model = nullptr) {
│ │ │ │ +
213
│ │ │ │ +
214 // Create a factor graph and initial values
│ │ │ │ +
215 Values values;
│ │ │ │ +
216 NonlinearFactorGraph graph;
│ │ │ │ +
217 boost::tie(graph, values) = triangulationGraph<CAMERA> //
│ │ │ │ +
218 (cameras, measurements, Symbol('p', 0), initialEstimate, model);
│ │ │ │ +
219
│ │ │ │ +
220 return optimize(graph, values, Symbol('p', 0));
│ │ │ │ +
221}
│ │ │ │ +
│ │ │ │ +
222
│ │ │ │ +
223template<class CAMERA>
│ │ │ │ +
224std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>
│ │ │ │ +
225projectionMatricesFromCameras(const CameraSet<CAMERA> &cameras) {
│ │ │ │ +
226 std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
│ │ │ │ +
227 for (const CAMERA &camera: cameras) {
│ │ │ │ +
228 projection_matrices.push_back(camera.cameraProjectionMatrix());
│ │ │ │ +
229 }
│ │ │ │ +
230 return projection_matrices;
│ │ │ │ +
231}
│ │ │ │
232
│ │ │ │ -
233} // gtsam
│ │ │ │ -
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ -
Point2.h
2D Point
│ │ │ │ +
233// overload, assuming pinholePose
│ │ │ │ +
234template<class CALIBRATION>
│ │ │ │ +
235std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projectionMatricesFromPoses(
│ │ │ │ +
236 const std::vector<Pose3> &poses, boost::shared_ptr<CALIBRATION> sharedCal) {
│ │ │ │ +
237 std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
│ │ │ │ +
238 for (size_t i = 0; i < poses.size(); i++) {
│ │ │ │ +
239 PinholePose<CALIBRATION> camera(poses.at(i), sharedCal);
│ │ │ │ +
240 projection_matrices.push_back(camera.cameraProjectionMatrix());
│ │ │ │ +
241 }
│ │ │ │ +
242 return projection_matrices;
│ │ │ │ +
243}
│ │ │ │ +
244
│ │ │ │ +
252template <class CALIBRATION>
│ │ │ │ +
│ │ │ │ +
253Cal3_S2 createPinholeCalibration(const CALIBRATION& cal) {
│ │ │ │ +
254 const auto& K = cal.K();
│ │ │ │ +
255 return Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2));
│ │ │ │ +
256}
│ │ │ │ +
│ │ │ │ +
257
│ │ │ │ +
260template <class CALIBRATION, class MEASUREMENT>
│ │ │ │ +
│ │ │ │ +
261MEASUREMENT undistortMeasurementInternal(
│ │ │ │ +
262 const CALIBRATION& cal, const MEASUREMENT& measurement,
│ │ │ │ +
263 boost::optional<Cal3_S2> pinholeCal = boost::none) {
│ │ │ │ +
264 if (!pinholeCal) {
│ │ │ │ +
265 pinholeCal = createPinholeCalibration(cal);
│ │ │ │ +
266 }
│ │ │ │ +
267 return pinholeCal->uncalibrate(cal.calibrate(measurement));
│ │ │ │ +
268}
│ │ │ │ +
│ │ │ │ +
269
│ │ │ │ +
281template <class CALIBRATION>
│ │ │ │ +
│ │ │ │ +
282Point2Vector undistortMeasurements(const CALIBRATION& cal,
│ │ │ │ +
283 const Point2Vector& measurements) {
│ │ │ │ +
284 Cal3_S2 pinholeCalibration = createPinholeCalibration(cal);
│ │ │ │ +
285 Point2Vector undistortedMeasurements;
│ │ │ │ +
286 // Calibrate with cal and uncalibrate with pinhole version of cal so that
│ │ │ │ +
287 // measurements are undistorted.
│ │ │ │ +
288 std::transform(measurements.begin(), measurements.end(),
│ │ │ │ +
289 std::back_inserter(undistortedMeasurements),
│ │ │ │ +
290 [&cal, &pinholeCalibration](const Point2& measurement) {
│ │ │ │ +
291 return undistortMeasurementInternal<CALIBRATION>(
│ │ │ │ +
292 cal, measurement, pinholeCalibration);
│ │ │ │ +
293 });
│ │ │ │ +
294 return undistortedMeasurements;
│ │ │ │ +
295}
│ │ │ │ +
│ │ │ │ +
296
│ │ │ │ +
298template <>
│ │ │ │ +
│ │ │ │ +
299inline Point2Vector undistortMeasurements(const Cal3_S2& cal,
│ │ │ │ +
300 const Point2Vector& measurements) {
│ │ │ │ +
301 return measurements;
│ │ │ │ +
302}
│ │ │ │ +
│ │ │ │ +
303
│ │ │ │ +
315template <class CAMERA>
│ │ │ │ +
│ │ │ │ +
316typename CAMERA::MeasurementVector undistortMeasurements(
│ │ │ │ +
317 const CameraSet<CAMERA>& cameras,
│ │ │ │ +
318 const typename CAMERA::MeasurementVector& measurements) {
│ │ │ │ +
319 const size_t nrMeasurements = measurements.size();
│ │ │ │ +
320 assert(nrMeasurements == cameras.size());
│ │ │ │ +
321 typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements);
│ │ │ │ +
322 for (size_t ii = 0; ii < nrMeasurements; ++ii) {
│ │ │ │ +
323 // Calibrate with cal and uncalibrate with pinhole version of cal so that
│ │ │ │ +
324 // measurements are undistorted.
│ │ │ │ +
325 undistortedMeasurements[ii] =
│ │ │ │ +
326 undistortMeasurementInternal<typename CAMERA::CalibrationType>(
│ │ │ │ +
327 cameras[ii].calibration(), measurements[ii]);
│ │ │ │ +
328 }
│ │ │ │ +
329 return undistortedMeasurements;
│ │ │ │ +
330}
│ │ │ │ +
│ │ │ │ +
331
│ │ │ │ +
333template <class CAMERA = PinholeCamera<Cal3_S2>>
│ │ │ │ +
│ │ │ │ +
334inline PinholeCamera<Cal3_S2>::MeasurementVector undistortMeasurements(
│ │ │ │ +
335 const CameraSet<PinholeCamera<Cal3_S2>>& cameras,
│ │ │ │ +
336 const PinholeCamera<Cal3_S2>::MeasurementVector& measurements) {
│ │ │ │ +
337 return measurements;
│ │ │ │ +
338}
│ │ │ │ +
│ │ │ │ +
339
│ │ │ │ +
341template <class CAMERA = SphericalCamera>
│ │ │ │ +
│ │ │ │ +
342inline SphericalCamera::MeasurementVector undistortMeasurements(
│ │ │ │ +
343 const CameraSet<SphericalCamera>& cameras,
│ │ │ │ +
344 const SphericalCamera::MeasurementVector& measurements) {
│ │ │ │ +
345 return measurements;
│ │ │ │ +
346}
│ │ │ │ +
│ │ │ │ +
347
│ │ │ │ +
356template <class CALIBRATION>
│ │ │ │ +
│ │ │ │ +
357inline Point3Vector calibrateMeasurementsShared(
│ │ │ │ +
358 const CALIBRATION& cal, const Point2Vector& measurements) {
│ │ │ │ +
359 Point3Vector calibratedMeasurements;
│ │ │ │ +
360 // Calibrate with cal and uncalibrate with pinhole version of cal so that
│ │ │ │ +
361 // measurements are undistorted.
│ │ │ │ +
362 std::transform(measurements.begin(), measurements.end(),
│ │ │ │ +
363 std::back_inserter(calibratedMeasurements),
│ │ │ │ +
364 [&cal](const Point2& measurement) {
│ │ │ │ +
365 Point3 p;
│ │ │ │ +
366 p << cal.calibrate(measurement), 1.0;
│ │ │ │ +
367 return p;
│ │ │ │ +
368 });
│ │ │ │ +
369 return calibratedMeasurements;
│ │ │ │ +
370}
│ │ │ │ +
│ │ │ │ +
371
│ │ │ │ +
380template <class CAMERA>
│ │ │ │ +
│ │ │ │ +
381inline Point3Vector calibrateMeasurements(
│ │ │ │ +
382 const CameraSet<CAMERA>& cameras,
│ │ │ │ +
383 const typename CAMERA::MeasurementVector& measurements) {
│ │ │ │ +
384 const size_t nrMeasurements = measurements.size();
│ │ │ │ +
385 assert(nrMeasurements == cameras.size());
│ │ │ │ +
386 Point3Vector calibratedMeasurements(nrMeasurements);
│ │ │ │ +
387 for (size_t ii = 0; ii < nrMeasurements; ++ii) {
│ │ │ │ +
388 calibratedMeasurements[ii]
│ │ │ │ +
389 << cameras[ii].calibration().calibrate(measurements[ii]),
│ │ │ │ +
390 1.0;
│ │ │ │ +
391 }
│ │ │ │ +
392 return calibratedMeasurements;
│ │ │ │ +
393}
│ │ │ │ +
│ │ │ │ +
394
│ │ │ │ +
396template <class CAMERA = SphericalCamera>
│ │ │ │ +
│ │ │ │ +
397inline Point3Vector calibrateMeasurements(
│ │ │ │ +
398 const CameraSet<SphericalCamera>& cameras,
│ │ │ │ +
399 const SphericalCamera::MeasurementVector& measurements) {
│ │ │ │ +
400 Point3Vector calibratedMeasurements(measurements.size());
│ │ │ │ +
401 for (size_t ii = 0; ii < measurements.size(); ++ii) {
│ │ │ │ +
402 calibratedMeasurements[ii] << measurements[ii].point3();
│ │ │ │ +
403 }
│ │ │ │ +
404 return calibratedMeasurements;
│ │ │ │ +
405}
│ │ │ │ +
│ │ │ │ +
406
│ │ │ │ +
420template <class CALIBRATION>
│ │ │ │ +
│ │ │ │ +
421Point3 triangulatePoint3(const std::vector<Pose3>& poses,
│ │ │ │ +
422 boost::shared_ptr<CALIBRATION> sharedCal,
│ │ │ │ +
423 const Point2Vector& measurements,
│ │ │ │ +
424 double rank_tol = 1e-9, bool optimize = false,
│ │ │ │ +
425 const SharedNoiseModel& model = nullptr,
│ │ │ │ +
426 const bool useLOST = false) {
│ │ │ │ +
427 assert(poses.size() == measurements.size());
│ │ │ │ +
428 if (poses.size() < 2) throw(TriangulationUnderconstrainedException());
│ │ │ │ +
429
│ │ │ │ +
430 // Triangulate linearly
│ │ │ │ +
431 Point3 point;
│ │ │ │ +
432 if (useLOST) {
│ │ │ │ +
433 // Reduce input noise model to an isotropic noise model using the mean of
│ │ │ │ +
434 // the diagonal.
│ │ │ │ +
435 const double measurementSigma = model ? model->sigmas().mean() : 1e-4;
│ │ │ │ +
436 SharedIsotropic measurementNoise =
│ │ │ │ +
437 noiseModel::Isotropic::Sigma(2, measurementSigma);
│ │ │ │ +
438 // calibrate the measurements to obtain homogenous coordinates in image
│ │ │ │ +
439 // plane.
│ │ │ │ +
440 auto calibratedMeasurements =
│ │ │ │ +
441 calibrateMeasurementsShared<CALIBRATION>(*sharedCal, measurements);
│ │ │ │ +
442
│ │ │ │ +
443 point = triangulateLOST(poses, calibratedMeasurements, measurementNoise);
│ │ │ │ +
444 } else {
│ │ │ │ +
445 // construct projection matrices from poses & calibration
│ │ │ │ +
446 auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal);
│ │ │ │ +
447
│ │ │ │ +
448 // Undistort the measurements, leaving only the pinhole elements in effect.
│ │ │ │ +
449 auto undistortedMeasurements =
│ │ │ │ +
450 undistortMeasurements<CALIBRATION>(*sharedCal, measurements);
│ │ │ │ +
451
│ │ │ │ +
452 point =
│ │ │ │ +
453 triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
│ │ │ │ +
454 }
│ │ │ │ +
455
│ │ │ │ +
456 // Then refine using non-linear optimization
│ │ │ │ +
457 if (optimize)
│ │ │ │ +
458 point = triangulateNonlinear<CALIBRATION> //
│ │ │ │ +
459 (poses, sharedCal, measurements, point, model);
│ │ │ │ +
460
│ │ │ │ +
461#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
│ │ │ │ +
462 // verify that the triangulated point lies in front of all cameras
│ │ │ │ +
463 for (const Pose3& pose : poses) {
│ │ │ │ +
464 const Point3& p_local = pose.transformTo(point);
│ │ │ │ +
465 if (p_local.z() <= 0) throw(TriangulationCheiralityException());
│ │ │ │ +
466 }
│ │ │ │ +
467#endif
│ │ │ │ +
468
│ │ │ │ +
469 return point;
│ │ │ │ +
470}
│ │ │ │ +
│ │ │ │ +
471
│ │ │ │ +
486template <class CAMERA>
│ │ │ │ +
│ │ │ │ +
487Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
│ │ │ │ +
488 const typename CAMERA::MeasurementVector& measurements,
│ │ │ │ +
489 double rank_tol = 1e-9, bool optimize = false,
│ │ │ │ +
490 const SharedNoiseModel& model = nullptr,
│ │ │ │ +
491 const bool useLOST = false) {
│ │ │ │ +
492 size_t m = cameras.size();
│ │ │ │ +
493 assert(measurements.size() == m);
│ │ │ │ +
494
│ │ │ │ +
495 if (m < 2) throw(TriangulationUnderconstrainedException());
│ │ │ │ +
496
│ │ │ │ +
497 // Triangulate linearly
│ │ │ │ +
498 Point3 point;
│ │ │ │ +
499 if (useLOST) {
│ │ │ │ +
500 // Reduce input noise model to an isotropic noise model using the mean of
│ │ │ │ +
501 // the diagonal.
│ │ │ │ +
502 const double measurementSigma = model ? model->sigmas().mean() : 1e-4;
│ │ │ │ +
503 SharedIsotropic measurementNoise =
│ │ │ │ +
504 noiseModel::Isotropic::Sigma(2, measurementSigma);
│ │ │ │ +
505
│ │ │ │ +
506 // construct poses from cameras.
│ │ │ │ +
507 std::vector<Pose3> poses;
│ │ │ │ +
508 poses.reserve(cameras.size());
│ │ │ │ +
509 for (const auto& camera : cameras) poses.push_back(camera.pose());
│ │ │ │ +
510
│ │ │ │ +
511 // calibrate the measurements to obtain homogenous coordinates in image
│ │ │ │ +
512 // plane.
│ │ │ │ +
513 auto calibratedMeasurements =
│ │ │ │ +
514 calibrateMeasurements<CAMERA>(cameras, measurements);
│ │ │ │ +
515
│ │ │ │ +
516 point = triangulateLOST(poses, calibratedMeasurements, measurementNoise);
│ │ │ │ +
517 } else {
│ │ │ │ +
518 // construct projection matrices from poses & calibration
│ │ │ │ +
519 auto projection_matrices = projectionMatricesFromCameras(cameras);
│ │ │ │ +
520
│ │ │ │ +
521 // Undistort the measurements, leaving only the pinhole elements in effect.
│ │ │ │ +
522 auto undistortedMeasurements =
│ │ │ │ +
523 undistortMeasurements<CAMERA>(cameras, measurements);
│ │ │ │ +
524
│ │ │ │ +
525 point =
│ │ │ │ +
526 triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
│ │ │ │ +
527 }
│ │ │ │ +
528
│ │ │ │ +
529 // Then refine using non-linear optimization
│ │ │ │ +
530 if (optimize) {
│ │ │ │ +
531 point = triangulateNonlinear<CAMERA>(cameras, measurements, point, model);
│ │ │ │ +
532 }
│ │ │ │ +
533
│ │ │ │ +
534#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
│ │ │ │ +
535 // verify that the triangulated point lies in front of all cameras
│ │ │ │ +
536 for (const CAMERA& camera : cameras) {
│ │ │ │ +
537 const Point3& p_local = camera.pose().transformTo(point);
│ │ │ │ +
538 if (p_local.z() <= 0) throw(TriangulationCheiralityException());
│ │ │ │ +
539 }
│ │ │ │ +
540#endif
│ │ │ │ +
541
│ │ │ │ +
542 return point;
│ │ │ │ +
543}
│ │ │ │ +
│ │ │ │ +
544
│ │ │ │ +
546template <class CALIBRATION>
│ │ │ │ +
│ │ │ │ +
547Point3 triangulatePoint3(const CameraSet<PinholeCamera<CALIBRATION>>& cameras,
│ │ │ │ +
548 const Point2Vector& measurements,
│ │ │ │ +
549 double rank_tol = 1e-9, bool optimize = false,
│ │ │ │ +
550 const SharedNoiseModel& model = nullptr,
│ │ │ │ +
551 const bool useLOST = false) {
│ │ │ │ +
552 return triangulatePoint3<PinholeCamera<CALIBRATION>> //
│ │ │ │ +
553 (cameras, measurements, rank_tol, optimize, model, useLOST);
│ │ │ │ +
554}
│ │ │ │ +
│ │ │ │ +
555
│ │ │ │ +
│ │ │ │ +
556struct GTSAM_EXPORT TriangulationParameters {
│ │ │ │ +
557
│ │ │ │ +
558 double rankTolerance;
│ │ │ │ +
560 bool enableEPI;
│ │ │ │ +
561
│ │ │ │ +
566 double landmarkDistanceThreshold; //
│ │ │ │ +
567
│ │ │ │ +
573 double dynamicOutlierRejectionThreshold;
│ │ │ │ +
574
│ │ │ │ +
575 SharedNoiseModel noiseModel;
│ │ │ │ +
576
│ │ │ │ +
│ │ │ │ +
586 TriangulationParameters(const double _rankTolerance = 1.0,
│ │ │ │ +
587 const bool _enableEPI = false, double _landmarkDistanceThreshold = -1,
│ │ │ │ +
588 double _dynamicOutlierRejectionThreshold = -1,
│ │ │ │ +
589 const SharedNoiseModel& _noiseModel = nullptr) :
│ │ │ │ +
590 rankTolerance(_rankTolerance), enableEPI(_enableEPI), //
│ │ │ │ +
591 landmarkDistanceThreshold(_landmarkDistanceThreshold), //
│ │ │ │ +
592 dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold),
│ │ │ │ +
593 noiseModel(_noiseModel){
│ │ │ │ +
594 }
│ │ │ │ +
│ │ │ │ +
595
│ │ │ │ +
596 // stream to output
│ │ │ │ +
597 friend std::ostream &operator<<(std::ostream &os,
│ │ │ │ +
598 const TriangulationParameters& p) {
│ │ │ │ +
599 os << "rankTolerance = " << p.rankTolerance << std::endl;
│ │ │ │ +
600 os << "enableEPI = " << p.enableEPI << std::endl;
│ │ │ │ +
601 os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold
│ │ │ │ +
602 << std::endl;
│ │ │ │ +
603 os << "dynamicOutlierRejectionThreshold = "
│ │ │ │ +
604 << p.dynamicOutlierRejectionThreshold << std::endl;
│ │ │ │ +
605 os << "noise model" << std::endl;
│ │ │ │ +
606 return os;
│ │ │ │ +
607 }
│ │ │ │ +
608
│ │ │ │ +
609private:
│ │ │ │ +
610
│ │ │ │ +
612 friend class boost::serialization::access;
│ │ │ │ +
613 template<class ARCHIVE>
│ │ │ │ +
614 void serialize(ARCHIVE & ar, const unsigned int version) {
│ │ │ │ +
615 ar & BOOST_SERIALIZATION_NVP(rankTolerance);
│ │ │ │ +
616 ar & BOOST_SERIALIZATION_NVP(enableEPI);
│ │ │ │ +
617 ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold);
│ │ │ │ +
618 ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold);
│ │ │ │ +
619 }
│ │ │ │ +
620};
│ │ │ │ +
│ │ │ │ +
621
│ │ │ │ +
│ │ │ │ +
626class TriangulationResult : public boost::optional<Point3> {
│ │ │ │ +
627 public:
│ │ │ │ +
628 enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT };
│ │ │ │ +
629 Status status;
│ │ │ │ +
630
│ │ │ │ +
631 private:
│ │ │ │ +
632 TriangulationResult(Status s) : status(s) {}
│ │ │ │ +
633
│ │ │ │ +
634 public:
│ │ │ │ +
638 TriangulationResult() {}
│ │ │ │ +
639
│ │ │ │ +
643 TriangulationResult(const Point3& p) : status(VALID) { reset(p); }
│ │ │ │ +
644 static TriangulationResult Degenerate() {
│ │ │ │ +
645 return TriangulationResult(DEGENERATE);
│ │ │ │ +
646 }
│ │ │ │ +
647 static TriangulationResult Outlier() { return TriangulationResult(OUTLIER); }
│ │ │ │ +
648 static TriangulationResult FarPoint() {
│ │ │ │ +
649 return TriangulationResult(FAR_POINT);
│ │ │ │ +
650 }
│ │ │ │ +
651 static TriangulationResult BehindCamera() {
│ │ │ │ +
652 return TriangulationResult(BEHIND_CAMERA);
│ │ │ │ +
653 }
│ │ │ │ +
654 bool valid() const { return status == VALID; }
│ │ │ │ +
655 bool degenerate() const { return status == DEGENERATE; }
│ │ │ │ +
656 bool outlier() const { return status == OUTLIER; }
│ │ │ │ +
657 bool farPoint() const { return status == FAR_POINT; }
│ │ │ │ +
658 bool behindCamera() const { return status == BEHIND_CAMERA; }
│ │ │ │ +
659 // stream to output
│ │ │ │ +
660 friend std::ostream& operator<<(std::ostream& os,
│ │ │ │ +
661 const TriangulationResult& result) {
│ │ │ │ +
662 if (result)
│ │ │ │ +
663 os << "point = " << *result << std::endl;
│ │ │ │ +
664 else
│ │ │ │ +
665 os << "no point, status = " << result.status << std::endl;
│ │ │ │ +
666 return os;
│ │ │ │ +
667 }
│ │ │ │ +
668
│ │ │ │ +
669 private:
│ │ │ │ +
671 friend class boost::serialization::access;
│ │ │ │ +
672 template <class ARCHIVE>
│ │ │ │ +
673 void serialize(ARCHIVE& ar, const unsigned int version) {
│ │ │ │ +
674 ar& BOOST_SERIALIZATION_NVP(status);
│ │ │ │ +
675 }
│ │ │ │ +
676};
│ │ │ │ +
│ │ │ │ +
677
│ │ │ │ +
679template<class CAMERA>
│ │ │ │ +
│ │ │ │ +
680TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
│ │ │ │ +
681 const typename CAMERA::MeasurementVector& measured,
│ │ │ │ +
682 const TriangulationParameters& params) {
│ │ │ │ +
683
│ │ │ │ +
684 size_t m = cameras.size();
│ │ │ │ +
685
│ │ │ │ +
686 // if we have a single pose the corresponding factor is uninformative
│ │ │ │ +
687 if (m < 2)
│ │ │ │ +
688 return TriangulationResult::Degenerate();
│ │ │ │ +
689 else
│ │ │ │ +
690 // We triangulate the 3D position of the landmark
│ │ │ │ +
691 try {
│ │ │ │ +
692 Point3 point =
│ │ │ │ +
693 triangulatePoint3<CAMERA>(cameras, measured, params.rankTolerance,
│ │ │ │ +
694 params.enableEPI, params.noiseModel);
│ │ │ │ +
695
│ │ │ │ +
696 // Check landmark distance and re-projection errors to avoid outliers
│ │ │ │ +
697 size_t i = 0;
│ │ │ │ +
698 double maxReprojError = 0.0;
│ │ │ │ +
699 for(const CAMERA& camera: cameras) {
│ │ │ │ +
700 const Pose3& pose = camera.pose();
│ │ │ │ +
701 if (params.landmarkDistanceThreshold > 0
│ │ │ │ +
702 && distance3(pose.translation(), point)
│ │ │ │ +
703 > params.landmarkDistanceThreshold)
│ │ │ │ +
704 return TriangulationResult::FarPoint();
│ │ │ │ +
705#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
│ │ │ │ +
706 // verify that the triangulated point lies in front of all cameras
│ │ │ │ +
707 // Only needed if this was not yet handled by exception
│ │ │ │ +
708 const Point3& p_local = pose.transformTo(point);
│ │ │ │ +
709 if (p_local.z() <= 0)
│ │ │ │ +
710 return TriangulationResult::BehindCamera();
│ │ │ │ +
711#endif
│ │ │ │ +
712 // Check reprojection error
│ │ │ │ +
713 if (params.dynamicOutlierRejectionThreshold > 0) {
│ │ │ │ +
714 const typename CAMERA::Measurement& zi = measured.at(i);
│ │ │ │ +
715 Point2 reprojectionError = camera.reprojectionError(point, zi);
│ │ │ │ +
716 maxReprojError = std::max(maxReprojError, reprojectionError.norm());
│ │ │ │ +
717 }
│ │ │ │ +
718 i += 1;
│ │ │ │ +
719 }
│ │ │ │ +
720 // Flag as degenerate if average reprojection error is too large
│ │ │ │ +
721 if (params.dynamicOutlierRejectionThreshold > 0
│ │ │ │ +
722 && maxReprojError > params.dynamicOutlierRejectionThreshold)
│ │ │ │ +
723 return TriangulationResult::Outlier();
│ │ │ │ +
724
│ │ │ │ +
725 // all good!
│ │ │ │ +
726 return TriangulationResult(point);
│ │ │ │ +
727 } catch (TriangulationUnderconstrainedException&) {
│ │ │ │ +
728 // This exception is thrown if
│ │ │ │ +
729 // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
│ │ │ │ +
730 // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
│ │ │ │ +
731 return TriangulationResult::Degenerate();
│ │ │ │ +
732 } catch (TriangulationCheiralityException&) {
│ │ │ │ +
733 // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
│ │ │ │ +
734 return TriangulationResult::BehindCamera();
│ │ │ │ +
735 }
│ │ │ │ +
736}
│ │ │ │ +
│ │ │ │ +
737
│ │ │ │ +
738// Vector of Cameras - used by the Python/MATLAB wrapper
│ │ │ │ +
739using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
│ │ │ │ +
740using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
│ │ │ │ +
741using CameraSetCal3DS2 = CameraSet<PinholeCamera<Cal3DS2>>;
│ │ │ │ +
742using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
│ │ │ │ +
743using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
│ │ │ │ +
744using CameraSetSpherical = CameraSet<SphericalCamera>;
│ │ │ │ +
745} // \namespace gtsam
│ │ │ │ +
746
│ │ │ │ +
Cal3DS2.h
Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base.
│ │ │ │ +
SphericalCamera.h
Calibrated camera with spherical projection.
│ │ │ │ +
Pose2.h
2D Pose
│ │ │ │ +
Cal3Unified.h
Unified Calibration Model, see Mei07icra for details.
│ │ │ │ +
Cal3Bundler.h
Calibration used by Bundler.
│ │ │ │ +
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
│ │ │ │ +
Cal3Fisheye.h
Calibration of a fisheye camera.
│ │ │ │ +
PinholeCamera.h
Base class for all pinhole cameras.
│ │ │ │ +
CameraSet.h
Base class to create smart factors on poses or cameras.
│ │ │ │ +
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::calibrateMeasurementsShared
Point3Vector calibrateMeasurementsShared(const CALIBRATION &cal, const Point2Vector &measurements)
Convert pixel measurements in image to homogeneous measurements in the image plane using shared camer...
Definition triangulation.h:357
│ │ │ │ +
gtsam::triangulateLOST
Point3 triangulateLOST(const std::vector< Pose3 > &poses, const Point3Vector &calibratedMeasurements, const SharedIsotropic &measurementNoise)
Triangulation using the LOST (Linear Optimal Sine Triangulation) algorithm proposed in https://arxiv....
Definition triangulation.cpp:92
│ │ │ │ +
gtsam::undistortMeasurements
Point2Vector undistortMeasurements(const CALIBRATION &cal, const Point2Vector &measurements)
Remove distortion for measurements so as if the measurements came from a pinhole camera.
Definition triangulation.h:282
│ │ │ │ +
gtsam::createPinholeCalibration
Cal3_S2 createPinholeCalibration(const CALIBRATION &cal)
Create a pinhole calibration from a different Cal3 object, removing distortion.
Definition triangulation.h:253
│ │ │ │ +
gtsam::undistortMeasurementInternal
MEASUREMENT undistortMeasurementInternal(const CALIBRATION &cal, const MEASUREMENT &measurement, boost::optional< Cal3_S2 > pinholeCal=boost::none)
Internal undistortMeasurement to be used by undistortMeasurement and undistortMeasurements.
Definition triangulation.h:261
│ │ │ │ +
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
│ │ │ │
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │ -
gtsam::normalize
Point3 normalize(const Point3 &p, OptionalJacobian< 3, 3 > H)
normalize, with optional Jacobian
Definition Point3.cpp:52
│ │ │ │ +
gtsam::triangulateSafe
TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters &params)
triangulateSafe: extensive checking of the outcome
Definition triangulation.h:680
│ │ │ │ +
gtsam::triangulateNonlinear
Point3 triangulateNonlinear(const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, const Point3 &initialEstimate, const SharedNoiseModel &model=nullptr)
Given an initial estimate , refine a point using measurements in several cameras.
Definition triangulation.h:187
│ │ │ │ +
gtsam::distance3
double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian< 1, 3 > H1, OptionalJacobian< 1, 3 > H2)
distance between two points
Definition Point3.cpp:27
│ │ │ │ +
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ +
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ +
gtsam::triangulatePoint3
Point3 triangulatePoint3(const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, double rank_tol=1e-9, bool optimize=false, const SharedNoiseModel &model=nullptr, const bool useLOST=false)
Function to triangulate 3D landmark point from an arbitrary number of poses (at least 2) using the DL...
Definition triangulation.h:421
│ │ │ │ +
gtsam::triangulationGraph
std::pair< NonlinearFactorGraph, Values > triangulationGraph(const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, Key landmarkKey, const Point3 &initialEstimate, const SharedNoiseModel &model=noiseModel::Unit::Create(2))
Create a factor graph with projection factors from poses and one calibration.
Definition triangulation.h:122
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::calibrateMeasurements
Point3Vector calibrateMeasurements(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements)
Convert pixel measurements in image to homogeneous measurements in the image plane using camera intri...
Definition triangulation.h:381
│ │ │ │ +
gtsam::triangulateDLT
Point3 triangulateDLT(const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > &projection_matrices, const Point2Vector &measurements, double rank_tol)
DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312.
Definition triangulation.cpp:127
│ │ │ │ +
gtsam::triangulateHomogeneousDLT
Vector4 triangulateHomogeneousDLT(const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > &projection_matrices, const Point2Vector &measurements, double rank_tol)
DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312.
Definition triangulation.cpp:27
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::LieGroup
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition Lie.h:37
│ │ │ │ -
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition Lie.h:229
│ │ │ │ -
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Rot2
Rotation matrix NOTE: the angle theta is in radians unless explicitly stated.
Definition Rot2.h:36
│ │ │ │ -
gtsam::Rot2::operator*
Rot2 operator*(const Rot2 &R) const
Compose - make a new rotation by adding angles.
Definition Rot2.h:117
│ │ │ │ -
gtsam::Rot2::c
double c() const
return cos
Definition Rot2.h:198
│ │ │ │ -
gtsam::Rot2::ExpmapDerivative
static Matrix ExpmapDerivative(const Vector &)
Left-trivialized derivative of the exponential map.
Definition Rot2.h:135
│ │ │ │ -
gtsam::Rot2::unit
Point2 unit() const
Creates a unit vector as a Point2.
Definition Rot2.h:182
│ │ │ │ -
gtsam::Rot2::theta
double theta() const
return angle (RADIANS)
Definition Rot2.h:187
│ │ │ │ -
gtsam::Rot2::inverse
Rot2 inverse() const
The inverse rotation - negative angle.
Definition Rot2.h:114
│ │ │ │ -
gtsam::Rot2::operator*
Point2 operator*(const Point2 &p) const
syntactic sugar for rotate
Definition Rot2.h:167
│ │ │ │ -
gtsam::Rot2::s
double s() const
return sin
Definition Rot2.h:203
│ │ │ │ -
gtsam::Rot2::Identity
static Rot2 Identity()
Identity.
Definition Rot2.h:111
│ │ │ │ -
gtsam::Rot2::degrees
double degrees() const
return angle (DEGREES)
Definition Rot2.h:192
│ │ │ │ -
gtsam::Rot2::LogmapDerivative
static Matrix LogmapDerivative(const Vector &)
Left-trivialized derivative inverse of the exponential map.
Definition Rot2.h:140
│ │ │ │ -
gtsam::Rot2::AdjointMap
Matrix1 AdjointMap() const
Calculate Adjoint map.
Definition Rot2.h:132
│ │ │ │ -
gtsam::Rot2::Rot2
Rot2(double theta)
Constructor from angle in radians == exponential map at identity.
Definition Rot2.h:59
│ │ │ │ -
gtsam::Rot2::fromDegrees
static Rot2 fromDegrees(double theta)
Named constructor from angle in degrees.
Definition Rot2.h:67
│ │ │ │ -
gtsam::Rot2::Rot2
Rot2()
default constructor, zero rotation
Definition Rot2.h:53
│ │ │ │ -
gtsam::Rot2::Rot2
Rot2(const Rot2 &r)
copy constructor
Definition Rot2.h:56
│ │ │ │ -
gtsam::Rot2::fromAngle
static Rot2 fromAngle(double theta)
Named constructor from angle in radians.
Definition Rot2.h:62
│ │ │ │ -
gtsam::Rot2::ChartAtOrigin
Definition Rot2.h:145
│ │ │ │ +
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition Cal3_S2.h:34
│ │ │ │ +
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition CameraSet.h:36
│ │ │ │ +
gtsam::PinholeCamera
A pinhole camera class that has a Pose3 and a Calibration.
Definition PinholeCamera.h:33
│ │ │ │ +
gtsam::PinholePose
A pinhole camera class that has a Pose3 and a fixed Calibration.
Definition PinholePose.h:243
│ │ │ │ +
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ +
gtsam::Pose3::transformTo
Point3 transformTo(const Point3 &point, OptionalJacobian< 3, 6 > Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const
takes point in world coordinates and transforms it to Pose coordinates
Definition Pose3.cpp:371
│ │ │ │ +
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get translation
Definition Pose3.cpp:308
│ │ │ │ +
gtsam::TriangulationUnderconstrainedException
Exception thrown by triangulateDLT when SVD returns rank < 3.
Definition triangulation.h:39
│ │ │ │ +
gtsam::TriangulationCheiralityException
Exception thrown by triangulateDLT when landmark is behind one or more of the cameras.
Definition triangulation.h:47
│ │ │ │ +
gtsam::TriangulationParameters
Definition triangulation.h:556
│ │ │ │ +
gtsam::TriangulationParameters::TriangulationParameters
TriangulationParameters(const double _rankTolerance=1.0, const bool _enableEPI=false, double _landmarkDistanceThreshold=-1, double _dynamicOutlierRejectionThreshold=-1, const SharedNoiseModel &_noiseModel=nullptr)
Constructor.
Definition triangulation.h:586
│ │ │ │ +
gtsam::TriangulationParameters::dynamicOutlierRejectionThreshold
double dynamicOutlierRejectionThreshold
If this is nonnegative the we will check if the average reprojection error is smaller than this thres...
Definition triangulation.h:573
│ │ │ │ +
gtsam::TriangulationParameters::rankTolerance
double rankTolerance
threshold to decide whether triangulation is result.degenerate
Definition triangulation.h:558
│ │ │ │ +
gtsam::TriangulationParameters::landmarkDistanceThreshold
double landmarkDistanceThreshold
if the landmark is triangulated at distance larger than this, result is flagged as degenerate.
Definition triangulation.h:566
│ │ │ │ +
gtsam::TriangulationParameters::enableEPI
bool enableEPI
if set to true, will refine triangulation using LM
Definition triangulation.h:560
│ │ │ │ +
gtsam::TriangulationParameters::noiseModel
SharedNoiseModel noiseModel
used in the nonlinear triangulation
Definition triangulation.h:575
│ │ │ │ +
gtsam::TriangulationResult
TriangulationResult is an optional point, along with the reasons why it is invalid.
Definition triangulation.h:626
│ │ │ │ +
gtsam::TriangulationResult::TriangulationResult
TriangulationResult()
Default constructor, only for serialization.
Definition triangulation.h:638
│ │ │ │ +
gtsam::TriangulationResult::TriangulationResult
TriangulationResult(const Point3 &p)
Constructor.
Definition triangulation.h:643
│ │ │ │ +
gtsam::TriangulationResult::access
friend class boost::serialization::access
Serialization function.
Definition triangulation.h:671
│ │ │ │ +
gtsam::Symbol
Character and index key used to refer to variables.
Definition Symbol.h:35
│ │ │ │ +
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
An isotropic noise model created by specifying a standard devation sigma.
Definition NoiseModel.cpp:597
│ │ │ │ +
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Create a unit covariance noise model.
Definition NoiseModel.h:597
│ │ │ │ +
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │ +
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
gtsam::Values::insert
void insert(Key j, const Value &val)
Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present.
Definition Values.cpp:157
│ │ │ │ +
gtsam::TriangulationFactor
Non-linear factor for a constraint derived from a 2D measurement.
Definition TriangulationFactor.h:33
│ │ │ │ +
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │ + │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,277 +1,815 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Rot2.h │ │ │ │ │ +triangulation.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_e_/_L_i_e_._h> │ │ │ │ │ -24#include │ │ │ │ │ -25 │ │ │ │ │ -26#include │ │ │ │ │ -27 │ │ │ │ │ -28namespace _g_t_s_a_m { │ │ │ │ │ -29 │ │ │ │ │ -_3_6 class GTSAM_EXPORT _R_o_t_2 : public _L_i_e_G_r_o_u_p { │ │ │ │ │ +21#pragma once │ │ │ │ │ +22 │ │ │ │ │ +23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3_B_u_n_d_l_e_r_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3_F_i_s_h_e_y_e_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3_U_n_i_f_i_e_d_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3___S_2_._h> │ │ │ │ │ +27#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3_D_S_2_._h> │ │ │ │ │ +28#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_m_e_r_a_S_e_t_._h> │ │ │ │ │ +29#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_i_n_h_o_l_e_C_a_m_e_r_a_._h> │ │ │ │ │ +30#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_._h> │ │ │ │ │ +31#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_2_._h> │ │ │ │ │ +32#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_S_y_m_b_o_l_._h> │ │ │ │ │ +33#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +34#include │ │ │ │ │ +35 │ │ │ │ │ +36namespace _g_t_s_a_m { │ │ │ │ │ 37 │ │ │ │ │ -39 double c_, s_; │ │ │ │ │ -40 │ │ │ │ │ -42 _R_o_t_2& _n_o_r_m_a_l_i_z_e(); │ │ │ │ │ -43 │ │ │ │ │ -45 inline _R_o_t_2(double c, double s) : c_(c), s_(s) {} │ │ │ │ │ -46 │ │ │ │ │ -47 public: │ │ │ │ │ -48 │ │ │ │ │ -51 │ │ │ │ │ -_5_3 _R_o_t_2() : c_(1.0), s_(0.0) {} │ │ │ │ │ +_3_9class GTSAM_EXPORT _T_r_i_a_n_g_u_l_a_t_i_o_n_U_n_d_e_r_c_o_n_s_t_r_a_i_n_e_d_E_x_c_e_p_t_i_o_n: public std:: │ │ │ │ │ +runtime_error { │ │ │ │ │ +40public: │ │ │ │ │ +41 _T_r_i_a_n_g_u_l_a_t_i_o_n_U_n_d_e_r_c_o_n_s_t_r_a_i_n_e_d_E_x_c_e_p_t_i_o_n() : │ │ │ │ │ +42 std::runtime_error("Triangulation Underconstrained Exception.") { │ │ │ │ │ +43 } │ │ │ │ │ +44}; │ │ │ │ │ +45 │ │ │ │ │ +_4_7class GTSAM_EXPORT _T_r_i_a_n_g_u_l_a_t_i_o_n_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n: public std:: │ │ │ │ │ +runtime_error { │ │ │ │ │ +48public: │ │ │ │ │ +49 _T_r_i_a_n_g_u_l_a_t_i_o_n_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n() : │ │ │ │ │ +50 std::runtime_error( │ │ │ │ │ +51 "Triangulation Cheirality Exception: The resulting landmark is behind one or │ │ │ │ │ +more cameras.") { │ │ │ │ │ +52 } │ │ │ │ │ +53}; │ │ │ │ │ 54 │ │ │ │ │ -_5_6 _R_o_t_2(const _R_o_t_2& r) : _R_o_t_2(r.c_, r.s_) {} │ │ │ │ │ -57 │ │ │ │ │ -_5_9 _R_o_t_2(double theta) : c_(cos(theta)), s_(sin(theta)) {} │ │ │ │ │ -60 │ │ │ │ │ -_6_2 static _R_o_t_2 _f_r_o_m_A_n_g_l_e(double theta) { │ │ │ │ │ -63 return _R_o_t_2(theta); │ │ │ │ │ -64 } │ │ │ │ │ +62GTSAM_EXPORT Vector4 _t_r_i_a_n_g_u_l_a_t_e_H_o_m_o_g_e_n_e_o_u_s_D_L_T( │ │ │ │ │ +63 const std::vector>& │ │ │ │ │ +projection_matrices, │ │ │ │ │ +64 const Point2Vector& measurements, double rank_tol = 1e-9); │ │ │ │ │ 65 │ │ │ │ │ -_6_7 static _R_o_t_2 _f_r_o_m_D_e_g_r_e_e_s(double theta) { │ │ │ │ │ -68 static const double degree = M_PI / 180; │ │ │ │ │ -69 return fromAngle(theta * degree); │ │ │ │ │ -70 } │ │ │ │ │ -71 │ │ │ │ │ -73 static _R_o_t_2 fromCosSin(double c, double s); │ │ │ │ │ -74 │ │ │ │ │ -82 static _R_o_t_2 relativeBearing(const _P_o_i_n_t_2& d, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_2_> H = │ │ │ │ │ -83 boost::none); │ │ │ │ │ -84 │ │ │ │ │ -86 static _R_o_t_2 atan2(double y, double x); │ │ │ │ │ -87 │ │ │ │ │ -94 static _R_o_t_2 Random(std::mt19937 & rng); │ │ │ │ │ -95 │ │ │ │ │ -99 │ │ │ │ │ -101 void _p_r_i_n_t(const std::string& s = "theta") const; │ │ │ │ │ -102 │ │ │ │ │ -104 bool _e_q_u_a_l_s(const _R_o_t_2& R, double tol = 1e-9) const; │ │ │ │ │ -105 │ │ │ │ │ -109 │ │ │ │ │ -_1_1_1 inline static _R_o_t_2 _I_d_e_n_t_i_t_y() { return _R_o_t_2(); } │ │ │ │ │ -112 │ │ │ │ │ -_1_1_4 _R_o_t_2 _i_n_v_e_r_s_e() const { return _R_o_t_2(c_, -s_);} │ │ │ │ │ -115 │ │ │ │ │ -_1_1_7 _R_o_t_2 _o_p_e_r_a_t_o_r_*(const _R_o_t_2& R) const { │ │ │ │ │ -118 return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); │ │ │ │ │ -119 } │ │ │ │ │ -120 │ │ │ │ │ -124 │ │ │ │ │ -126 static _R_o_t_2 Expmap(const Vector1& v, ChartJacobian H = boost::none); │ │ │ │ │ -127 │ │ │ │ │ -129 static Vector1 Logmap(const _R_o_t_2& r, ChartJacobian H = boost::none); │ │ │ │ │ -130 │ │ │ │ │ -_1_3_2 Matrix1 _A_d_j_o_i_n_t_M_a_p() const { return I_1x1; } │ │ │ │ │ -133 │ │ │ │ │ -_1_3_5 static Matrix _E_x_p_m_a_p_D_e_r_i_v_a_t_i_v_e(const Vector& /*v*/) { │ │ │ │ │ -136 return I_1x1; │ │ │ │ │ -137 } │ │ │ │ │ -138 │ │ │ │ │ -_1_4_0 static Matrix _L_o_g_m_a_p_D_e_r_i_v_a_t_i_v_e(const Vector& /*v*/) { │ │ │ │ │ -141 return I_1x1; │ │ │ │ │ -142 } │ │ │ │ │ -143 │ │ │ │ │ -144 // Chart at origin simply uses exponential map and its inverse │ │ │ │ │ -_1_4_5 struct _C_h_a_r_t_A_t_O_r_i_g_i_n { │ │ │ │ │ -146 static _R_o_t_2 Retract(const Vector1& v, ChartJacobian H = boost::none) { │ │ │ │ │ -147 return Expmap(v, H); │ │ │ │ │ -148 } │ │ │ │ │ -149 static Vector1 Local(const _R_o_t_2& r, ChartJacobian H = boost::none) { │ │ │ │ │ -150 return Logmap(r, H); │ │ │ │ │ -151 } │ │ │ │ │ -152 }; │ │ │ │ │ -153 │ │ │ │ │ -154 using _L_i_e_G_r_o_u_p<_R_o_t_2, 1>::inverse; // version with derivative │ │ │ │ │ -155 │ │ │ │ │ -159 │ │ │ │ │ -163 _P_o_i_n_t_2 rotate(const _P_o_i_n_t_2& p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _1_> H1 = boost::none, │ │ │ │ │ -164 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> H2 = boost::none) const; │ │ │ │ │ -165 │ │ │ │ │ -_1_6_7 inline _P_o_i_n_t_2 _o_p_e_r_a_t_o_r_*(const _P_o_i_n_t_2& p) const { │ │ │ │ │ -168 return rotate(p); │ │ │ │ │ -169 } │ │ │ │ │ -170 │ │ │ │ │ -174 _P_o_i_n_t_2 unrotate(const _P_o_i_n_t_2& p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _1_> H1 = boost::none, │ │ │ │ │ -175 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> H2 = boost::none) const; │ │ │ │ │ -176 │ │ │ │ │ -180 │ │ │ │ │ -_1_8_2 inline _P_o_i_n_t_2 _u_n_i_t() const { │ │ │ │ │ -183 return _P_o_i_n_t_2(c_, s_); │ │ │ │ │ -184 } │ │ │ │ │ -185 │ │ │ │ │ -_1_8_7 double _t_h_e_t_a() const { │ │ │ │ │ -188 return ::atan2(s_, c_); │ │ │ │ │ -189 } │ │ │ │ │ -190 │ │ │ │ │ -_1_9_2 double _d_e_g_r_e_e_s() const { │ │ │ │ │ -193 const double degree = M_PI / 180; │ │ │ │ │ -194 return theta() / degree; │ │ │ │ │ -195 } │ │ │ │ │ -196 │ │ │ │ │ -_1_9_8 inline double _c() const { │ │ │ │ │ -199 return c_; │ │ │ │ │ -200 } │ │ │ │ │ -201 │ │ │ │ │ -_2_0_3 inline double _s() const { │ │ │ │ │ -204 return s_; │ │ │ │ │ -205 } │ │ │ │ │ -206 │ │ │ │ │ -208 Matrix2 matrix() const; │ │ │ │ │ -209 │ │ │ │ │ -211 Matrix2 transpose() const; │ │ │ │ │ -212 │ │ │ │ │ -214 static _R_o_t_2 ClosestTo(const Matrix2& M); │ │ │ │ │ -215 │ │ │ │ │ -216 private: │ │ │ │ │ -_2_1_8 friend class boost::serialization::access; │ │ │ │ │ -219 template │ │ │ │ │ -220 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -221 ar & BOOST_SERIALIZATION_NVP(c_); │ │ │ │ │ -222 ar & BOOST_SERIALIZATION_NVP(s_); │ │ │ │ │ -223 } │ │ │ │ │ -224 │ │ │ │ │ -225 }; │ │ │ │ │ -226 │ │ │ │ │ -227 template<> │ │ │ │ │ -_2_2_8 struct _t_r_a_i_t_s<_R_o_t_2> : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ -229 │ │ │ │ │ -230 template<> │ │ │ │ │ -_2_3_1 struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ +74GTSAM_EXPORT Vector4 _t_r_i_a_n_g_u_l_a_t_e_H_o_m_o_g_e_n_e_o_u_s_D_L_T( │ │ │ │ │ +75 const std::vector>& │ │ │ │ │ +projection_matrices, │ │ │ │ │ +76 const std::vector& measurements, double rank_tol = 1e-9); │ │ │ │ │ +77 │ │ │ │ │ +85GTSAM_EXPORT _P_o_i_n_t_3 _t_r_i_a_n_g_u_l_a_t_e_D_L_T( │ │ │ │ │ +86 const std::vector>& │ │ │ │ │ +projection_matrices, │ │ │ │ │ +87 const Point2Vector& measurements, │ │ │ │ │ +88 double rank_tol = 1e-9); │ │ │ │ │ +89 │ │ │ │ │ +93GTSAM_EXPORT _P_o_i_n_t_3 _t_r_i_a_n_g_u_l_a_t_e_D_L_T( │ │ │ │ │ +94 const std::vector>& │ │ │ │ │ +projection_matrices, │ │ │ │ │ +95 const std::vector& measurements, │ │ │ │ │ +96 double rank_tol = 1e-9); │ │ │ │ │ +97 │ │ │ │ │ +108GTSAM_EXPORT _P_o_i_n_t_3 _t_r_i_a_n_g_u_l_a_t_e_L_O_S_T(const std::vector& poses, │ │ │ │ │ +109 const Point3Vector& calibratedMeasurements, │ │ │ │ │ +110 const SharedIsotropic& measurementNoise); │ │ │ │ │ +111 │ │ │ │ │ +121template │ │ │ │ │ +_1_2_2std::pair _t_r_i_a_n_g_u_l_a_t_i_o_n_G_r_a_p_h( │ │ │ │ │ +123 const std::vector& poses, boost::shared_ptr sharedCal, │ │ │ │ │ +124 const Point2Vector& measurements, _K_e_y landmarkKey, │ │ │ │ │ +125 const _P_o_i_n_t_3& initialEstimate, │ │ │ │ │ +126 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model = _n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_C_r_e_a_t_e(2)) { │ │ │ │ │ +127 _V_a_l_u_e_s values; │ │ │ │ │ +128 values._i_n_s_e_r_t(landmarkKey, initialEstimate); // Initial landmark value │ │ │ │ │ +129 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h graph; │ │ │ │ │ +130 for (size_t i = 0; i < measurements.size(); i++) { │ │ │ │ │ +131 const _P_o_s_e_3& pose_i = poses[i]; │ │ │ │ │ +132 typedef _P_i_n_h_o_l_e_P_o_s_e_<_C_A_L_I_B_R_A_T_I_O_N_> Camera; │ │ │ │ │ +133 Camera camera_i(pose_i, sharedCal); │ │ │ │ │ +134 graph.emplace_shared<_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_<_C_a_m_e_r_a_> > // │ │ │ │ │ +135 (camera_i, measurements[i], model, landmarkKey); │ │ │ │ │ +136 } │ │ │ │ │ +137 return std::make_pair(graph, values); │ │ │ │ │ +138} │ │ │ │ │ +139 │ │ │ │ │ +149template │ │ │ │ │ +_1_5_0std::pair _t_r_i_a_n_g_u_l_a_t_i_o_n_G_r_a_p_h( │ │ │ │ │ +151 const _C_a_m_e_r_a_S_e_t_<_C_A_M_E_R_A_>& cameras, │ │ │ │ │ +152 const typename CAMERA::MeasurementVector& measurements, _K_e_y landmarkKey, │ │ │ │ │ +153 const _P_o_i_n_t_3& initialEstimate, │ │ │ │ │ +154 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model = nullptr) { │ │ │ │ │ +155 _V_a_l_u_e_s values; │ │ │ │ │ +156 values._i_n_s_e_r_t(landmarkKey, initialEstimate); // Initial landmark value │ │ │ │ │ +157 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h graph; │ │ │ │ │ +158 static _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l unit(_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_C_r_e_a_t_e( │ │ │ │ │ +159 _t_r_a_i_t_s_<_t_y_p_e_n_a_m_e_ _C_A_M_E_R_A_:_:_M_e_a_s_u_r_e_m_e_n_t_>_:_:_d_i_m_e_n_s_i_o_n)); │ │ │ │ │ +160 for (size_t i = 0; i < measurements.size(); i++) { │ │ │ │ │ +161 const CAMERA& camera_i = cameras[i]; │ │ │ │ │ +162 graph.emplace_shared<_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_<_C_A_M_E_R_A_> > // │ │ │ │ │ +163 (camera_i, measurements[i], model? model : unit, landmarkKey); │ │ │ │ │ +164 } │ │ │ │ │ +165 return std::make_pair(graph, values); │ │ │ │ │ +166} │ │ │ │ │ +167 │ │ │ │ │ +175GTSAM_EXPORT _P_o_i_n_t_3 _o_p_t_i_m_i_z_e(const NonlinearFactorGraph& graph, │ │ │ │ │ +176 const _V_a_l_u_e_s& values, _K_e_y landmarkKey); │ │ │ │ │ +177 │ │ │ │ │ +186template │ │ │ │ │ +_1_8_7_P_o_i_n_t_3 _t_r_i_a_n_g_u_l_a_t_e_N_o_n_l_i_n_e_a_r(const std::vector& poses, │ │ │ │ │ +188 boost::shared_ptr sharedCal, │ │ │ │ │ +189 const Point2Vector& measurements, const _P_o_i_n_t_3& initialEstimate, │ │ │ │ │ +190 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model = nullptr) { │ │ │ │ │ +191 │ │ │ │ │ +192 // Create a factor graph and initial values │ │ │ │ │ +193 _V_a_l_u_e_s values; │ │ │ │ │ +194 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h graph; │ │ │ │ │ +195 boost::tie(graph, values) = triangulationGraph // │ │ │ │ │ +196 (poses, sharedCal, measurements, _S_y_m_b_o_l('p', 0), initialEstimate, model); │ │ │ │ │ +197 │ │ │ │ │ +198 return _o_p_t_i_m_i_z_e(graph, values, _S_y_m_b_o_l('p', 0)); │ │ │ │ │ +199} │ │ │ │ │ +200 │ │ │ │ │ +208template │ │ │ │ │ +_2_0_9_P_o_i_n_t_3 _t_r_i_a_n_g_u_l_a_t_e_N_o_n_l_i_n_e_a_r( │ │ │ │ │ +210 const _C_a_m_e_r_a_S_e_t_<_C_A_M_E_R_A_>& cameras, │ │ │ │ │ +211 const typename CAMERA::MeasurementVector& measurements, const _P_o_i_n_t_3& │ │ │ │ │ +initialEstimate, │ │ │ │ │ +212 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model = nullptr) { │ │ │ │ │ +213 │ │ │ │ │ +214 // Create a factor graph and initial values │ │ │ │ │ +215 _V_a_l_u_e_s values; │ │ │ │ │ +216 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h graph; │ │ │ │ │ +217 boost::tie(graph, values) = triangulationGraph // │ │ │ │ │ +218 (cameras, measurements, _S_y_m_b_o_l('p', 0), initialEstimate, model); │ │ │ │ │ +219 │ │ │ │ │ +220 return _o_p_t_i_m_i_z_e(graph, values, _S_y_m_b_o_l('p', 0)); │ │ │ │ │ +221} │ │ │ │ │ +222 │ │ │ │ │ +223template │ │ │ │ │ +224std::vector> │ │ │ │ │ +225projectionMatricesFromCameras(const CameraSet &cameras) { │ │ │ │ │ +226 std::vector> │ │ │ │ │ +projection_matrices; │ │ │ │ │ +227 for (const CAMERA &camera: cameras) { │ │ │ │ │ +228 projection_matrices.push_back(camera.cameraProjectionMatrix()); │ │ │ │ │ +229 } │ │ │ │ │ +230 return projection_matrices; │ │ │ │ │ +231} │ │ │ │ │ 232 │ │ │ │ │ -233} // gtsam │ │ │ │ │ -_L_i_e_._h │ │ │ │ │ -Base class and basic functions for Lie types. │ │ │ │ │ -_P_o_i_n_t_2_._h │ │ │ │ │ -2D Point │ │ │ │ │ +233// overload, assuming pinholePose │ │ │ │ │ +234template │ │ │ │ │ +235std::vector> │ │ │ │ │ +projectionMatricesFromPoses( │ │ │ │ │ +236 const std::vector &poses, boost::shared_ptr sharedCal) │ │ │ │ │ +{ │ │ │ │ │ +237 std::vector> │ │ │ │ │ +projection_matrices; │ │ │ │ │ +238 for (size_t i = 0; i < poses.size(); i++) { │ │ │ │ │ +239 PinholePose camera(poses.at(i), sharedCal); │ │ │ │ │ +240 projection_matrices.push_back(camera.cameraProjectionMatrix()); │ │ │ │ │ +241 } │ │ │ │ │ +242 return projection_matrices; │ │ │ │ │ +243} │ │ │ │ │ +244 │ │ │ │ │ +252template │ │ │ │ │ +_2_5_3_C_a_l_3___S_2 _c_r_e_a_t_e_P_i_n_h_o_l_e_C_a_l_i_b_r_a_t_i_o_n(const CALIBRATION& cal) { │ │ │ │ │ +254 const auto& K = cal.K(); │ │ │ │ │ +255 return _C_a_l_3___S_2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2)); │ │ │ │ │ +256} │ │ │ │ │ +257 │ │ │ │ │ +260template │ │ │ │ │ +_2_6_1MEASUREMENT _u_n_d_i_s_t_o_r_t_M_e_a_s_u_r_e_m_e_n_t_I_n_t_e_r_n_a_l( │ │ │ │ │ +262 const CALIBRATION& cal, const MEASUREMENT& measurement, │ │ │ │ │ +263 boost::optional pinholeCal = boost::none) { │ │ │ │ │ +264 if (!pinholeCal) { │ │ │ │ │ +265 pinholeCal = _c_r_e_a_t_e_P_i_n_h_o_l_e_C_a_l_i_b_r_a_t_i_o_n(cal); │ │ │ │ │ +266 } │ │ │ │ │ +267 return pinholeCal->uncalibrate(cal.calibrate(measurement)); │ │ │ │ │ +268} │ │ │ │ │ +269 │ │ │ │ │ +281template │ │ │ │ │ +_2_8_2Point2Vector _u_n_d_i_s_t_o_r_t_M_e_a_s_u_r_e_m_e_n_t_s(const CALIBRATION& cal, │ │ │ │ │ +283 const Point2Vector& measurements) { │ │ │ │ │ +284 _C_a_l_3___S_2 pinholeCalibration = _c_r_e_a_t_e_P_i_n_h_o_l_e_C_a_l_i_b_r_a_t_i_o_n(cal); │ │ │ │ │ +285 Point2Vector undistortedMeasurements; │ │ │ │ │ +286 // Calibrate with cal and uncalibrate with pinhole version of cal so that │ │ │ │ │ +287 // measurements are undistorted. │ │ │ │ │ +288 std::transform(measurements.begin(), measurements.end(), │ │ │ │ │ +289 std::back_inserter(undistortedMeasurements), │ │ │ │ │ +290 [&cal, &pinholeCalibration](const _P_o_i_n_t_2& measurement) { │ │ │ │ │ +291 return undistortMeasurementInternal( │ │ │ │ │ +292 cal, measurement, pinholeCalibration); │ │ │ │ │ +293 }); │ │ │ │ │ +294 return undistortedMeasurements; │ │ │ │ │ +295} │ │ │ │ │ +296 │ │ │ │ │ +298template <> │ │ │ │ │ +_2_9_9inline Point2Vector _u_n_d_i_s_t_o_r_t_M_e_a_s_u_r_e_m_e_n_t_s(const _C_a_l_3___S_2& cal, │ │ │ │ │ +300 const Point2Vector& measurements) { │ │ │ │ │ +301 return measurements; │ │ │ │ │ +302} │ │ │ │ │ +303 │ │ │ │ │ +315template │ │ │ │ │ +_3_1_6typename CAMERA::MeasurementVector _u_n_d_i_s_t_o_r_t_M_e_a_s_u_r_e_m_e_n_t_s( │ │ │ │ │ +317 const _C_a_m_e_r_a_S_e_t_<_C_A_M_E_R_A_>& cameras, │ │ │ │ │ +318 const typename CAMERA::MeasurementVector& measurements) { │ │ │ │ │ +319 const size_t nrMeasurements = measurements.size(); │ │ │ │ │ +320 assert(nrMeasurements == cameras.size()); │ │ │ │ │ +321 typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements); │ │ │ │ │ +322 for (size_t ii = 0; ii < nrMeasurements; ++ii) { │ │ │ │ │ +323 // Calibrate with cal and uncalibrate with pinhole version of cal so that │ │ │ │ │ +324 // measurements are undistorted. │ │ │ │ │ +325 undistortedMeasurements[ii] = │ │ │ │ │ +326 undistortMeasurementInternal( │ │ │ │ │ +327 cameras[ii].calibration(), measurements[ii]); │ │ │ │ │ +328 } │ │ │ │ │ +329 return undistortedMeasurements; │ │ │ │ │ +330} │ │ │ │ │ +331 │ │ │ │ │ +333template > │ │ │ │ │ +_3_3_4inline PinholeCamera::MeasurementVector _u_n_d_i_s_t_o_r_t_M_e_a_s_u_r_e_m_e_n_t_s( │ │ │ │ │ +335 const _C_a_m_e_r_a_S_e_t<_P_i_n_h_o_l_e_C_a_m_e_r_a_<_C_a_l_3___S_2_>>& cameras, │ │ │ │ │ +336 const PinholeCamera::MeasurementVector& measurements) { │ │ │ │ │ +337 return measurements; │ │ │ │ │ +338} │ │ │ │ │ +339 │ │ │ │ │ +341template │ │ │ │ │ +_3_4_2inline SphericalCamera::MeasurementVector _u_n_d_i_s_t_o_r_t_M_e_a_s_u_r_e_m_e_n_t_s( │ │ │ │ │ +343 const _C_a_m_e_r_a_S_e_t_<_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_>& cameras, │ │ │ │ │ +344 const SphericalCamera::MeasurementVector& measurements) { │ │ │ │ │ +345 return measurements; │ │ │ │ │ +346} │ │ │ │ │ +347 │ │ │ │ │ +356template │ │ │ │ │ +_3_5_7inline Point3Vector _c_a_l_i_b_r_a_t_e_M_e_a_s_u_r_e_m_e_n_t_s_S_h_a_r_e_d( │ │ │ │ │ +358 const CALIBRATION& cal, const Point2Vector& measurements) { │ │ │ │ │ +359 Point3Vector calibratedMeasurements; │ │ │ │ │ +360 // Calibrate with cal and uncalibrate with pinhole version of cal so that │ │ │ │ │ +361 // measurements are undistorted. │ │ │ │ │ +362 std::transform(measurements.begin(), measurements.end(), │ │ │ │ │ +363 std::back_inserter(calibratedMeasurements), │ │ │ │ │ +364 [&cal](const _P_o_i_n_t_2& measurement) { │ │ │ │ │ +365 Point3 p; │ │ │ │ │ +366 p << cal.calibrate(measurement), 1.0; │ │ │ │ │ +367 return p; │ │ │ │ │ +368 }); │ │ │ │ │ +369 return calibratedMeasurements; │ │ │ │ │ +370} │ │ │ │ │ +371 │ │ │ │ │ +380template │ │ │ │ │ +_3_8_1inline Point3Vector _c_a_l_i_b_r_a_t_e_M_e_a_s_u_r_e_m_e_n_t_s( │ │ │ │ │ +382 const _C_a_m_e_r_a_S_e_t_<_C_A_M_E_R_A_>& cameras, │ │ │ │ │ +383 const typename CAMERA::MeasurementVector& measurements) { │ │ │ │ │ +384 const size_t nrMeasurements = measurements.size(); │ │ │ │ │ +385 assert(nrMeasurements == cameras.size()); │ │ │ │ │ +386 Point3Vector calibratedMeasurements(nrMeasurements); │ │ │ │ │ +387 for (size_t ii = 0; ii < nrMeasurements; ++ii) { │ │ │ │ │ +388 calibratedMeasurements[ii] │ │ │ │ │ +389 << cameras[ii].calibration().calibrate(measurements[ii]), │ │ │ │ │ +390 1.0; │ │ │ │ │ +391 } │ │ │ │ │ +392 return calibratedMeasurements; │ │ │ │ │ +393} │ │ │ │ │ +394 │ │ │ │ │ +396template │ │ │ │ │ +_3_9_7inline Point3Vector _c_a_l_i_b_r_a_t_e_M_e_a_s_u_r_e_m_e_n_t_s( │ │ │ │ │ +398 const _C_a_m_e_r_a_S_e_t_<_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_>& cameras, │ │ │ │ │ +399 const SphericalCamera::MeasurementVector& measurements) { │ │ │ │ │ +400 Point3Vector calibratedMeasurements(measurements.size()); │ │ │ │ │ +401 for (size_t ii = 0; ii < measurements.size(); ++ii) { │ │ │ │ │ +402 calibratedMeasurements[ii] << measurements[ii].point3(); │ │ │ │ │ +403 } │ │ │ │ │ +404 return calibratedMeasurements; │ │ │ │ │ +405} │ │ │ │ │ +406 │ │ │ │ │ +420template │ │ │ │ │ +_4_2_1_P_o_i_n_t_3 _t_r_i_a_n_g_u_l_a_t_e_P_o_i_n_t_3(const std::vector& poses, │ │ │ │ │ +422 boost::shared_ptr sharedCal, │ │ │ │ │ +423 const Point2Vector& measurements, │ │ │ │ │ +424 double rank_tol = 1e-9, bool _o_p_t_i_m_i_z_e = false, │ │ │ │ │ +425 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model = nullptr, │ │ │ │ │ +426 const bool useLOST = false) { │ │ │ │ │ +427 assert(poses.size() == measurements.size()); │ │ │ │ │ +428 if (poses.size() < 2) throw(_T_r_i_a_n_g_u_l_a_t_i_o_n_U_n_d_e_r_c_o_n_s_t_r_a_i_n_e_d_E_x_c_e_p_t_i_o_n()); │ │ │ │ │ +429 │ │ │ │ │ +430 // Triangulate linearly │ │ │ │ │ +431 _P_o_i_n_t_3 point; │ │ │ │ │ +432 if (useLOST) { │ │ │ │ │ +433 // Reduce input noise model to an isotropic noise model using the mean of │ │ │ │ │ +434 // the diagonal. │ │ │ │ │ +435 const double measurementSigma = model ? model->sigmas().mean() : 1e-4; │ │ │ │ │ +436 SharedIsotropic measurementNoise = │ │ │ │ │ +437 _n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c_:_:_S_i_g_m_a(2, measurementSigma); │ │ │ │ │ +438 // calibrate the measurements to obtain homogenous coordinates in image │ │ │ │ │ +439 // plane. │ │ │ │ │ +440 auto calibratedMeasurements = │ │ │ │ │ +441 calibrateMeasurementsShared(*sharedCal, measurements); │ │ │ │ │ +442 │ │ │ │ │ +443 point = _t_r_i_a_n_g_u_l_a_t_e_L_O_S_T(poses, calibratedMeasurements, measurementNoise); │ │ │ │ │ +444 } else { │ │ │ │ │ +445 // construct projection matrices from poses & calibration │ │ │ │ │ +446 auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal); │ │ │ │ │ +447 │ │ │ │ │ +448 // Undistort the measurements, leaving only the pinhole elements in effect. │ │ │ │ │ +449 auto undistortedMeasurements = │ │ │ │ │ +450 undistortMeasurements(*sharedCal, measurements); │ │ │ │ │ +451 │ │ │ │ │ +452 point = │ │ │ │ │ +453 _t_r_i_a_n_g_u_l_a_t_e_D_L_T(projection_matrices, undistortedMeasurements, rank_tol); │ │ │ │ │ +454 } │ │ │ │ │ +455 │ │ │ │ │ +456 // Then refine using non-linear optimization │ │ │ │ │ +457 if (_o_p_t_i_m_i_z_e) │ │ │ │ │ +458 point = triangulateNonlinear // │ │ │ │ │ +459 (poses, sharedCal, measurements, point, model); │ │ │ │ │ +460 │ │ │ │ │ +461#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION │ │ │ │ │ +462 // verify that the triangulated point lies in front of all cameras │ │ │ │ │ +463 for (const _P_o_s_e_3& pose : poses) { │ │ │ │ │ +464 const _P_o_i_n_t_3& p_local = pose.transformTo(point); │ │ │ │ │ +465 if (p_local.z() <= 0) throw(_T_r_i_a_n_g_u_l_a_t_i_o_n_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n()); │ │ │ │ │ +466 } │ │ │ │ │ +467#endif │ │ │ │ │ +468 │ │ │ │ │ +469 return point; │ │ │ │ │ +470} │ │ │ │ │ +471 │ │ │ │ │ +486template │ │ │ │ │ +_4_8_7_P_o_i_n_t_3 _t_r_i_a_n_g_u_l_a_t_e_P_o_i_n_t_3(const _C_a_m_e_r_a_S_e_t_<_C_A_M_E_R_A_>& cameras, │ │ │ │ │ +488 const typename CAMERA::MeasurementVector& measurements, │ │ │ │ │ +489 double rank_tol = 1e-9, bool _o_p_t_i_m_i_z_e = false, │ │ │ │ │ +490 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model = nullptr, │ │ │ │ │ +491 const bool useLOST = false) { │ │ │ │ │ +492 size_t m = cameras.size(); │ │ │ │ │ +493 assert(measurements.size() == m); │ │ │ │ │ +494 │ │ │ │ │ +495 if (m < 2) throw(_T_r_i_a_n_g_u_l_a_t_i_o_n_U_n_d_e_r_c_o_n_s_t_r_a_i_n_e_d_E_x_c_e_p_t_i_o_n()); │ │ │ │ │ +496 │ │ │ │ │ +497 // Triangulate linearly │ │ │ │ │ +498 _P_o_i_n_t_3 point; │ │ │ │ │ +499 if (useLOST) { │ │ │ │ │ +500 // Reduce input noise model to an isotropic noise model using the mean of │ │ │ │ │ +501 // the diagonal. │ │ │ │ │ +502 const double measurementSigma = model ? model->sigmas().mean() : 1e-4; │ │ │ │ │ +503 SharedIsotropic measurementNoise = │ │ │ │ │ +504 _n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c_:_:_S_i_g_m_a(2, measurementSigma); │ │ │ │ │ +505 │ │ │ │ │ +506 // construct poses from cameras. │ │ │ │ │ +507 std::vector poses; │ │ │ │ │ +508 poses.reserve(cameras.size()); │ │ │ │ │ +509 for (const auto& camera : cameras) poses.push_back(camera.pose()); │ │ │ │ │ +510 │ │ │ │ │ +511 // calibrate the measurements to obtain homogenous coordinates in image │ │ │ │ │ +512 // plane. │ │ │ │ │ +513 auto calibratedMeasurements = │ │ │ │ │ +514 calibrateMeasurements(cameras, measurements); │ │ │ │ │ +515 │ │ │ │ │ +516 point = _t_r_i_a_n_g_u_l_a_t_e_L_O_S_T(poses, calibratedMeasurements, measurementNoise); │ │ │ │ │ +517 } else { │ │ │ │ │ +518 // construct projection matrices from poses & calibration │ │ │ │ │ +519 auto projection_matrices = projectionMatricesFromCameras(cameras); │ │ │ │ │ +520 │ │ │ │ │ +521 // Undistort the measurements, leaving only the pinhole elements in effect. │ │ │ │ │ +522 auto undistortedMeasurements = │ │ │ │ │ +523 undistortMeasurements(cameras, measurements); │ │ │ │ │ +524 │ │ │ │ │ +525 point = │ │ │ │ │ +526 _t_r_i_a_n_g_u_l_a_t_e_D_L_T(projection_matrices, undistortedMeasurements, rank_tol); │ │ │ │ │ +527 } │ │ │ │ │ +528 │ │ │ │ │ +529 // Then refine using non-linear optimization │ │ │ │ │ +530 if (_o_p_t_i_m_i_z_e) { │ │ │ │ │ +531 point = triangulateNonlinear(cameras, measurements, point, model); │ │ │ │ │ +532 } │ │ │ │ │ +533 │ │ │ │ │ +534#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION │ │ │ │ │ +535 // verify that the triangulated point lies in front of all cameras │ │ │ │ │ +536 for (const CAMERA& camera : cameras) { │ │ │ │ │ +537 const _P_o_i_n_t_3& p_local = camera.pose().transformTo(point); │ │ │ │ │ +538 if (p_local.z() <= 0) throw(_T_r_i_a_n_g_u_l_a_t_i_o_n_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n()); │ │ │ │ │ +539 } │ │ │ │ │ +540#endif │ │ │ │ │ +541 │ │ │ │ │ +542 return point; │ │ │ │ │ +543} │ │ │ │ │ +544 │ │ │ │ │ +546template │ │ │ │ │ +_5_4_7_P_o_i_n_t_3 _t_r_i_a_n_g_u_l_a_t_e_P_o_i_n_t_3(const _C_a_m_e_r_a_S_e_t<_P_i_n_h_o_l_e_C_a_m_e_r_a_<_C_A_L_I_B_R_A_T_I_O_N_>>& │ │ │ │ │ +cameras, │ │ │ │ │ +548 const Point2Vector& measurements, │ │ │ │ │ +549 double rank_tol = 1e-9, bool _o_p_t_i_m_i_z_e = false, │ │ │ │ │ +550 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model = nullptr, │ │ │ │ │ +551 const bool useLOST = false) { │ │ │ │ │ +552 return triangulatePoint3> // │ │ │ │ │ +553 (cameras, measurements, rank_tol, _o_p_t_i_m_i_z_e, model, useLOST); │ │ │ │ │ +554} │ │ │ │ │ +555 │ │ │ │ │ +_5_5_6struct GTSAM_EXPORT _T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s { │ │ │ │ │ +557 │ │ │ │ │ +_5_5_8 double _r_a_n_k_T_o_l_e_r_a_n_c_e; │ │ │ │ │ +_5_6_0 bool _e_n_a_b_l_e_E_P_I; │ │ │ │ │ +561 │ │ │ │ │ +_5_6_6 double _l_a_n_d_m_a_r_k_D_i_s_t_a_n_c_e_T_h_r_e_s_h_o_l_d; // │ │ │ │ │ +567 │ │ │ │ │ +_5_7_3 double _d_y_n_a_m_i_c_O_u_t_l_i_e_r_R_e_j_e_c_t_i_o_n_T_h_r_e_s_h_o_l_d; │ │ │ │ │ +574 │ │ │ │ │ +_5_7_5 _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l _n_o_i_s_e_M_o_d_e_l; │ │ │ │ │ +576 │ │ │ │ │ +_5_8_6 _T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s(const double _rankTolerance = 1.0, │ │ │ │ │ +587 const bool _enableEPI = false, double _landmarkDistanceThreshold = -1, │ │ │ │ │ +588 double _dynamicOutlierRejectionThreshold = -1, │ │ │ │ │ +589 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& _noiseModel = nullptr) : │ │ │ │ │ +590 rankTolerance(_rankTolerance), enableEPI(_enableEPI), // │ │ │ │ │ +591 landmarkDistanceThreshold(_landmarkDistanceThreshold), // │ │ │ │ │ +592 dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold), │ │ │ │ │ +593 noiseModel(_noiseModel){ │ │ │ │ │ +594 } │ │ │ │ │ +595 │ │ │ │ │ +596 // stream to output │ │ │ │ │ +597 friend std::ostream &operator<<(std::ostream &os, │ │ │ │ │ +598 const _T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s& p) { │ │ │ │ │ +599 os << "rankTolerance = " << p._r_a_n_k_T_o_l_e_r_a_n_c_e << std::endl; │ │ │ │ │ +600 os << "enableEPI = " << p._e_n_a_b_l_e_E_P_I << std::endl; │ │ │ │ │ +601 os << "landmarkDistanceThreshold = " << p._l_a_n_d_m_a_r_k_D_i_s_t_a_n_c_e_T_h_r_e_s_h_o_l_d │ │ │ │ │ +602 << std::endl; │ │ │ │ │ +603 os << "dynamicOutlierRejectionThreshold = " │ │ │ │ │ +604 << p._d_y_n_a_m_i_c_O_u_t_l_i_e_r_R_e_j_e_c_t_i_o_n_T_h_r_e_s_h_o_l_d << std::endl; │ │ │ │ │ +605 os << "noise model" << std::endl; │ │ │ │ │ +606 return os; │ │ │ │ │ +607 } │ │ │ │ │ +608 │ │ │ │ │ +609private: │ │ │ │ │ +610 │ │ │ │ │ +_6_1_2 friend class boost::serialization::access; │ │ │ │ │ +613 template │ │ │ │ │ +614 void serialize(ARCHIVE & ar, const unsigned int version) { │ │ │ │ │ +615 ar & BOOST_SERIALIZATION_NVP(rankTolerance); │ │ │ │ │ +616 ar & BOOST_SERIALIZATION_NVP(enableEPI); │ │ │ │ │ +617 ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold); │ │ │ │ │ +618 ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold); │ │ │ │ │ +619 } │ │ │ │ │ +620}; │ │ │ │ │ +621 │ │ │ │ │ +_6_2_6class _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t : public boost::optional { │ │ │ │ │ +627 public: │ │ │ │ │ +628 enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT }; │ │ │ │ │ +629 Status status; │ │ │ │ │ +630 │ │ │ │ │ +631 private: │ │ │ │ │ +632 _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t(Status s) : status(s) {} │ │ │ │ │ +633 │ │ │ │ │ +634 public: │ │ │ │ │ +_6_3_8 _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t() {} │ │ │ │ │ +639 │ │ │ │ │ +_6_4_3 _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t(const _P_o_i_n_t_3& p) : status(VALID) { reset(p); } │ │ │ │ │ +644 static _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t Degenerate() { │ │ │ │ │ +645 return _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t(DEGENERATE); │ │ │ │ │ +646 } │ │ │ │ │ +647 static _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t Outlier() { return _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t(OUTLIER); │ │ │ │ │ +} │ │ │ │ │ +648 static _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t FarPoint() { │ │ │ │ │ +649 return _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t(FAR_POINT); │ │ │ │ │ +650 } │ │ │ │ │ +651 static _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t BehindCamera() { │ │ │ │ │ +652 return _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t(BEHIND_CAMERA); │ │ │ │ │ +653 } │ │ │ │ │ +654 bool valid() const { return status == VALID; } │ │ │ │ │ +655 bool degenerate() const { return status == DEGENERATE; } │ │ │ │ │ +656 bool outlier() const { return status == OUTLIER; } │ │ │ │ │ +657 bool farPoint() const { return status == FAR_POINT; } │ │ │ │ │ +658 bool behindCamera() const { return status == BEHIND_CAMERA; } │ │ │ │ │ +659 // stream to output │ │ │ │ │ +660 friend std::ostream& operator<<(std::ostream& os, │ │ │ │ │ +661 const _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t& result) { │ │ │ │ │ +662 if (result) │ │ │ │ │ +663 os << "point = " << *result << std::endl; │ │ │ │ │ +664 else │ │ │ │ │ +665 os << "no point, status = " << result.status << std::endl; │ │ │ │ │ +666 return os; │ │ │ │ │ +667 } │ │ │ │ │ +668 │ │ │ │ │ +669 private: │ │ │ │ │ +_6_7_1 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +672 template │ │ │ │ │ +673 void serialize(ARCHIVE& ar, const unsigned int version) { │ │ │ │ │ +674 ar& BOOST_SERIALIZATION_NVP(status); │ │ │ │ │ +675 } │ │ │ │ │ +676}; │ │ │ │ │ +677 │ │ │ │ │ +679template │ │ │ │ │ +_6_8_0_T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t _t_r_i_a_n_g_u_l_a_t_e_S_a_f_e(const _C_a_m_e_r_a_S_e_t_<_C_A_M_E_R_A_>& cameras, │ │ │ │ │ +681 const typename CAMERA::MeasurementVector& measured, │ │ │ │ │ +682 const _T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s& params) { │ │ │ │ │ +683 │ │ │ │ │ +684 size_t m = cameras.size(); │ │ │ │ │ +685 │ │ │ │ │ +686 // if we have a single pose the corresponding factor is uninformative │ │ │ │ │ +687 if (m < 2) │ │ │ │ │ +688 return TriangulationResult::Degenerate(); │ │ │ │ │ +689 else │ │ │ │ │ +690 // We triangulate the 3D position of the landmark │ │ │ │ │ +691 try { │ │ │ │ │ +692 _P_o_i_n_t_3 point = │ │ │ │ │ +693 triangulatePoint3(cameras, measured, params._r_a_n_k_T_o_l_e_r_a_n_c_e, │ │ │ │ │ +694 params._e_n_a_b_l_e_E_P_I, params._n_o_i_s_e_M_o_d_e_l); │ │ │ │ │ +695 │ │ │ │ │ +696 // Check landmark distance and re-projection errors to avoid outliers │ │ │ │ │ +697 size_t i = 0; │ │ │ │ │ +698 double maxReprojError = 0.0; │ │ │ │ │ +699 for(const CAMERA& camera: cameras) { │ │ │ │ │ +700 const _P_o_s_e_3& pose = camera.pose(); │ │ │ │ │ +701 if (params._l_a_n_d_m_a_r_k_D_i_s_t_a_n_c_e_T_h_r_e_s_h_o_l_d > 0 │ │ │ │ │ +702 && _d_i_s_t_a_n_c_e_3(pose._t_r_a_n_s_l_a_t_i_o_n(), point) │ │ │ │ │ +703 > params._l_a_n_d_m_a_r_k_D_i_s_t_a_n_c_e_T_h_r_e_s_h_o_l_d) │ │ │ │ │ +704 return TriangulationResult::FarPoint(); │ │ │ │ │ +705#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION │ │ │ │ │ +706 // verify that the triangulated point lies in front of all cameras │ │ │ │ │ +707 // Only needed if this was not yet handled by exception │ │ │ │ │ +708 const _P_o_i_n_t_3& p_local = pose._t_r_a_n_s_f_o_r_m_T_o(point); │ │ │ │ │ +709 if (p_local.z() <= 0) │ │ │ │ │ +710 return TriangulationResult::BehindCamera(); │ │ │ │ │ +711#endif │ │ │ │ │ +712 // Check reprojection error │ │ │ │ │ +713 if (params._d_y_n_a_m_i_c_O_u_t_l_i_e_r_R_e_j_e_c_t_i_o_n_T_h_r_e_s_h_o_l_d > 0) { │ │ │ │ │ +714 const typename CAMERA::Measurement& zi = measured.at(i); │ │ │ │ │ +715 _P_o_i_n_t_2 reprojectionError = camera.reprojectionError(point, zi); │ │ │ │ │ +716 maxReprojError = std::max(maxReprojError, reprojectionError.norm()); │ │ │ │ │ +717 } │ │ │ │ │ +718 i += 1; │ │ │ │ │ +719 } │ │ │ │ │ +720 // Flag as degenerate if average reprojection error is too large │ │ │ │ │ +721 if (params._d_y_n_a_m_i_c_O_u_t_l_i_e_r_R_e_j_e_c_t_i_o_n_T_h_r_e_s_h_o_l_d > 0 │ │ │ │ │ +722 && maxReprojError > params._d_y_n_a_m_i_c_O_u_t_l_i_e_r_R_e_j_e_c_t_i_o_n_T_h_r_e_s_h_o_l_d) │ │ │ │ │ +723 return TriangulationResult::Outlier(); │ │ │ │ │ +724 │ │ │ │ │ +725 // all good! │ │ │ │ │ +726 return _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t(point); │ │ │ │ │ +727 } catch (_T_r_i_a_n_g_u_l_a_t_i_o_n_U_n_d_e_r_c_o_n_s_t_r_a_i_n_e_d_E_x_c_e_p_t_i_o_n&) { │ │ │ │ │ +728 // This exception is thrown if │ │ │ │ │ +729 // 1) There is a single pose for triangulation - this should not happen │ │ │ │ │ +because we checked the number of poses before │ │ │ │ │ +730 // 2) The rank of the matrix used for triangulation is < 3: rotation-only, │ │ │ │ │ +parallel cameras (or motion towards the landmark) │ │ │ │ │ +731 return TriangulationResult::Degenerate(); │ │ │ │ │ +732 } catch (_T_r_i_a_n_g_u_l_a_t_i_o_n_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n&) { │ │ │ │ │ +733 // point is behind one of the cameras: can be the case of close-to-parallel │ │ │ │ │ +cameras or may depend on outliers │ │ │ │ │ +734 return TriangulationResult::BehindCamera(); │ │ │ │ │ +735 } │ │ │ │ │ +736} │ │ │ │ │ +737 │ │ │ │ │ +738// Vector of Cameras - used by the Python/MATLAB wrapper │ │ │ │ │ +739using CameraSetCal3Bundler = CameraSet>; │ │ │ │ │ +740using CameraSetCal3_S2 = CameraSet>; │ │ │ │ │ +741using CameraSetCal3DS2 = CameraSet>; │ │ │ │ │ +742using CameraSetCal3Fisheye = CameraSet>; │ │ │ │ │ +743using CameraSetCal3Unified = CameraSet>; │ │ │ │ │ +744using CameraSetSpherical = CameraSet; │ │ │ │ │ +745} // \namespace gtsam │ │ │ │ │ +746 │ │ │ │ │ +_C_a_l_3_D_S_2_._h │ │ │ │ │ +Calibration of a camera with radial distortion, calculations in base class │ │ │ │ │ +Cal3DS2_Base. │ │ │ │ │ +_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_._h │ │ │ │ │ +Calibrated camera with spherical projection. │ │ │ │ │ +_P_o_s_e_2_._h │ │ │ │ │ +2D Pose │ │ │ │ │ +_C_a_l_3_U_n_i_f_i_e_d_._h │ │ │ │ │ +Unified Calibration Model, see Mei07icra for details. │ │ │ │ │ +_C_a_l_3_B_u_n_d_l_e_r_._h │ │ │ │ │ +Calibration used by Bundler. │ │ │ │ │ +_C_a_l_3___S_2_._h │ │ │ │ │ +The most common 5DOF 3D->2D calibration. │ │ │ │ │ +_C_a_l_3_F_i_s_h_e_y_e_._h │ │ │ │ │ +Calibration of a fisheye camera. │ │ │ │ │ +_P_i_n_h_o_l_e_C_a_m_e_r_a_._h │ │ │ │ │ +Base class for all pinhole cameras. │ │ │ │ │ +_C_a_m_e_r_a_S_e_t_._h │ │ │ │ │ +Base class to create smart factors on poses or cameras. │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Factor Graph consisting of non-linear factors. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_c_a_l_i_b_r_a_t_e_M_e_a_s_u_r_e_m_e_n_t_s_S_h_a_r_e_d │ │ │ │ │ +Point3Vector calibrateMeasurementsShared(const CALIBRATION &cal, const │ │ │ │ │ +Point2Vector &measurements) │ │ │ │ │ +Convert pixel measurements in image to homogeneous measurements in the image │ │ │ │ │ +plane using shared camer... │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:357 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_L_O_S_T │ │ │ │ │ +Point3 triangulateLOST(const std::vector< Pose3 > &poses, const Point3Vector │ │ │ │ │ +&calibratedMeasurements, const SharedIsotropic &measurementNoise) │ │ │ │ │ +Triangulation using the LOST (Linear Optimal Sine Triangulation) algorithm │ │ │ │ │ +proposed in https://arxiv.... │ │ │ │ │ +DDeeffiinniittiioonn triangulation.cpp:92 │ │ │ │ │ +_g_t_s_a_m_:_:_u_n_d_i_s_t_o_r_t_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ +Point2Vector undistortMeasurements(const CALIBRATION &cal, const Point2Vector │ │ │ │ │ +&measurements) │ │ │ │ │ +Remove distortion for measurements so as if the measurements came from a │ │ │ │ │ +pinhole camera. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:282 │ │ │ │ │ +_g_t_s_a_m_:_:_c_r_e_a_t_e_P_i_n_h_o_l_e_C_a_l_i_b_r_a_t_i_o_n │ │ │ │ │ +Cal3_S2 createPinholeCalibration(const CALIBRATION &cal) │ │ │ │ │ +Create a pinhole calibration from a different Cal3 object, removing distortion. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:253 │ │ │ │ │ +_g_t_s_a_m_:_:_u_n_d_i_s_t_o_r_t_M_e_a_s_u_r_e_m_e_n_t_I_n_t_e_r_n_a_l │ │ │ │ │ +MEASUREMENT undistortMeasurementInternal(const CALIBRATION &cal, const │ │ │ │ │ +MEASUREMENT &measurement, boost::optional< Cal3_S2 > pinholeCal=boost::none) │ │ │ │ │ +Internal undistortMeasurement to be used by undistortMeasurement and │ │ │ │ │ +undistortMeasurements. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:261 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ +Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key │ │ │ │ │ +landmarkKey) │ │ │ │ │ +Optimize for triangulation. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.cpp:155 │ │ │ │ │ _g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ Vector2 Point2 │ │ │ │ │ As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ typedef Point2 to Vector2... │ │ │ │ │ DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_r_m_a_l_i_z_e │ │ │ │ │ -Point3 normalize(const Point3 &p, OptionalJacobian< 3, 3 > H) │ │ │ │ │ -normalize, with optional Jacobian │ │ │ │ │ -DDeeffiinniittiioonn Point3.cpp:52 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_S_a_f_e │ │ │ │ │ +TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const │ │ │ │ │ +typename CAMERA::MeasurementVector &measured, const TriangulationParameters │ │ │ │ │ +¶ms) │ │ │ │ │ +triangulateSafe: extensive checking of the outcome │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:680 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_N_o_n_l_i_n_e_a_r │ │ │ │ │ +Point3 triangulateNonlinear(const std::vector< Pose3 > &poses, boost:: │ │ │ │ │ +shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, const │ │ │ │ │ +Point3 &initialEstimate, const SharedNoiseModel &model=nullptr) │ │ │ │ │ +Given an initial estimate , refine a point using measurements in several │ │ │ │ │ +cameras. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:187 │ │ │ │ │ +_g_t_s_a_m_:_:_d_i_s_t_a_n_c_e_3 │ │ │ │ │ +double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian< 1, 3 > │ │ │ │ │ +H1, OptionalJacobian< 1, 3 > H2) │ │ │ │ │ +distance between two points │ │ │ │ │ +DDeeffiinniittiioonn Point3.cpp:27 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ +Vector3 Point3 │ │ │ │ │ +As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ +typedef Point3 to Vector3... │ │ │ │ │ +DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ +Aliases. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_P_o_i_n_t_3 │ │ │ │ │ +Point3 triangulatePoint3(const std::vector< Pose3 > &poses, boost::shared_ptr< │ │ │ │ │ +CALIBRATION > sharedCal, const Point2Vector &measurements, double rank_tol=1e- │ │ │ │ │ +9, bool optimize=false, const SharedNoiseModel &model=nullptr, const bool │ │ │ │ │ +useLOST=false) │ │ │ │ │ +Function to triangulate 3D landmark point from an arbitrary number of poses (at │ │ │ │ │ +least 2) using the DL... │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:421 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_i_o_n_G_r_a_p_h │ │ │ │ │ +std::pair< NonlinearFactorGraph, Values > triangulationGraph(const std::vector< │ │ │ │ │ +Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector │ │ │ │ │ +&measurements, Key landmarkKey, const Point3 &initialEstimate, const │ │ │ │ │ +SharedNoiseModel &model=noiseModel::Unit::Create(2)) │ │ │ │ │ +Create a factor graph with projection factors from poses and one calibration. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:122 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_c_a_l_i_b_r_a_t_e_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ +Point3Vector calibrateMeasurements(const CameraSet< CAMERA > &cameras, const │ │ │ │ │ +typename CAMERA::MeasurementVector &measurements) │ │ │ │ │ +Convert pixel measurements in image to homogeneous measurements in the image │ │ │ │ │ +plane using camera intri... │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:381 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_D_L_T │ │ │ │ │ +Point3 triangulateDLT(const std::vector< Matrix34, Eigen::aligned_allocator< │ │ │ │ │ +Matrix34 > > &projection_matrices, const Point2Vector &measurements, double │ │ │ │ │ +rank_tol) │ │ │ │ │ +DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.cpp:127 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_H_o_m_o_g_e_n_e_o_u_s_D_L_T │ │ │ │ │ +Vector4 triangulateHomogeneousDLT(const std::vector< Matrix34, Eigen:: │ │ │ │ │ +aligned_allocator< Matrix34 > > &projection_matrices, const Point2Vector │ │ │ │ │ +&measurements, double rank_tol) │ │ │ │ │ +DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.cpp:27 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ -A CRTP helper class that implements Lie group methods Prerequisites: methods │ │ │ │ │ -operator*,... │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ -Both LieGroupTraits and Testable. │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:229 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ -either a fixed size o... │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_2 │ │ │ │ │ -Rotation matrix NOTE: the angle theta is in radians unless explicitly stated. │ │ │ │ │ -DDeeffiinniittiioonn Rot2.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_2_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ -Rot2 operator*(const Rot2 &R) const │ │ │ │ │ -Compose - make a new rotation by adding angles. │ │ │ │ │ -DDeeffiinniittiioonn Rot2.h:117 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_2_:_:_c │ │ │ │ │ -double c() const │ │ │ │ │ -return cos │ │ │ │ │ -DDeeffiinniittiioonn Rot2.h:198 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_2_:_:_E_x_p_m_a_p_D_e_r_i_v_a_t_i_v_e │ │ │ │ │ -static Matrix ExpmapDerivative(const Vector &) │ │ │ │ │ -Left-trivialized derivative of the exponential map. │ │ │ │ │ -DDeeffiinniittiioonn Rot2.h:135 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_2_:_:_u_n_i_t │ │ │ │ │ -Point2 unit() const │ │ │ │ │ -Creates a unit vector as a Point2. │ │ │ │ │ -DDeeffiinniittiioonn Rot2.h:182 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_2_:_:_t_h_e_t_a │ │ │ │ │ -double theta() const │ │ │ │ │ -return angle (RADIANS) │ │ │ │ │ -DDeeffiinniittiioonn Rot2.h:187 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_2_:_:_i_n_v_e_r_s_e │ │ │ │ │ -Rot2 inverse() const │ │ │ │ │ -The inverse rotation - negative angle. │ │ │ │ │ -DDeeffiinniittiioonn Rot2.h:114 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_2_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ -Point2 operator*(const Point2 &p) const │ │ │ │ │ -syntactic sugar for rotate │ │ │ │ │ -DDeeffiinniittiioonn Rot2.h:167 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_2_:_:_s │ │ │ │ │ -double s() const │ │ │ │ │ -return sin │ │ │ │ │ -DDeeffiinniittiioonn Rot2.h:203 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_2_:_:_I_d_e_n_t_i_t_y │ │ │ │ │ -static Rot2 Identity() │ │ │ │ │ -Identity. │ │ │ │ │ -DDeeffiinniittiioonn Rot2.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_2_:_:_d_e_g_r_e_e_s │ │ │ │ │ -double degrees() const │ │ │ │ │ -return angle (DEGREES) │ │ │ │ │ -DDeeffiinniittiioonn Rot2.h:192 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_2_:_:_L_o_g_m_a_p_D_e_r_i_v_a_t_i_v_e │ │ │ │ │ -static Matrix LogmapDerivative(const Vector &) │ │ │ │ │ -Left-trivialized derivative inverse of the exponential map. │ │ │ │ │ -DDeeffiinniittiioonn Rot2.h:140 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_2_:_:_A_d_j_o_i_n_t_M_a_p │ │ │ │ │ -Matrix1 AdjointMap() const │ │ │ │ │ -Calculate Adjoint map. │ │ │ │ │ -DDeeffiinniittiioonn Rot2.h:132 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_2_:_:_R_o_t_2 │ │ │ │ │ -Rot2(double theta) │ │ │ │ │ -Constructor from angle in radians == exponential map at identity. │ │ │ │ │ -DDeeffiinniittiioonn Rot2.h:59 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_2_:_:_f_r_o_m_D_e_g_r_e_e_s │ │ │ │ │ -static Rot2 fromDegrees(double theta) │ │ │ │ │ -Named constructor from angle in degrees. │ │ │ │ │ -DDeeffiinniittiioonn Rot2.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_2_:_:_R_o_t_2 │ │ │ │ │ -Rot2() │ │ │ │ │ -default constructor, zero rotation │ │ │ │ │ -DDeeffiinniittiioonn Rot2.h:53 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_2_:_:_R_o_t_2 │ │ │ │ │ -Rot2(const Rot2 &r) │ │ │ │ │ -copy constructor │ │ │ │ │ -DDeeffiinniittiioonn Rot2.h:56 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_2_:_:_f_r_o_m_A_n_g_l_e │ │ │ │ │ -static Rot2 fromAngle(double theta) │ │ │ │ │ -Named constructor from angle in radians. │ │ │ │ │ -DDeeffiinniittiioonn Rot2.h:62 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_2_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n │ │ │ │ │ -DDeeffiinniittiioonn Rot2.h:145 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3___S_2 │ │ │ │ │ +The most common 5DOF 3D->2D calibration. │ │ │ │ │ +DDeeffiinniittiioonn Cal3_S2.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t │ │ │ │ │ +A set of cameras, all with their own calibration. │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a │ │ │ │ │ +A pinhole camera class that has a Pose3 and a Calibration. │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:33 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e │ │ │ │ │ +A pinhole camera class that has a Pose3 and a fixed Calibration. │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:243 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ +A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_t_r_a_n_s_f_o_r_m_T_o │ │ │ │ │ +Point3 transformTo(const Point3 &point, OptionalJacobian< 3, 6 > Hself=boost:: │ │ │ │ │ +none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const │ │ │ │ │ +takes point in world coordinates and transforms it to Pose coordinates │ │ │ │ │ +DDeeffiinniittiioonn Pose3.cpp:371 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_t_r_a_n_s_l_a_t_i_o_n │ │ │ │ │ +const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const │ │ │ │ │ +get translation │ │ │ │ │ +DDeeffiinniittiioonn Pose3.cpp:308 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_U_n_d_e_r_c_o_n_s_t_r_a_i_n_e_d_E_x_c_e_p_t_i_o_n │ │ │ │ │ +Exception thrown by triangulateDLT when SVD returns rank < 3. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n │ │ │ │ │ +Exception thrown by triangulateDLT when landmark is behind one or more of the │ │ │ │ │ +cameras. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:556 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ +TriangulationParameters(const double _rankTolerance=1.0, const bool │ │ │ │ │ +_enableEPI=false, double _landmarkDistanceThreshold=-1, double │ │ │ │ │ +_dynamicOutlierRejectionThreshold=-1, const SharedNoiseModel │ │ │ │ │ +&_noiseModel=nullptr) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:586 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s_:_:_d_y_n_a_m_i_c_O_u_t_l_i_e_r_R_e_j_e_c_t_i_o_n_T_h_r_e_s_h_o_l_d │ │ │ │ │ +double dynamicOutlierRejectionThreshold │ │ │ │ │ +If this is nonnegative the we will check if the average reprojection error is │ │ │ │ │ +smaller than this thres... │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:573 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s_:_:_r_a_n_k_T_o_l_e_r_a_n_c_e │ │ │ │ │ +double rankTolerance │ │ │ │ │ +threshold to decide whether triangulation is result.degenerate │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:558 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s_:_:_l_a_n_d_m_a_r_k_D_i_s_t_a_n_c_e_T_h_r_e_s_h_o_l_d │ │ │ │ │ +double landmarkDistanceThreshold │ │ │ │ │ +if the landmark is triangulated at distance larger than this, result is flagged │ │ │ │ │ +as degenerate. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:566 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s_:_:_e_n_a_b_l_e_E_P_I │ │ │ │ │ +bool enableEPI │ │ │ │ │ +if set to true, will refine triangulation using LM │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:560 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s_:_:_n_o_i_s_e_M_o_d_e_l │ │ │ │ │ +SharedNoiseModel noiseModel │ │ │ │ │ +used in the nonlinear triangulation │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:575 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t │ │ │ │ │ +TriangulationResult is an optional point, along with the reasons why it is │ │ │ │ │ +invalid. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:626 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t │ │ │ │ │ +TriangulationResult() │ │ │ │ │ +Default constructor, only for serialization. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:638 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t │ │ │ │ │ +TriangulationResult(const Point3 &p) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:643 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:671 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l │ │ │ │ │ +Character and index key used to refer to variables. │ │ │ │ │ +DDeeffiinniittiioonn Symbol.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c_:_:_S_i_g_m_a │ │ │ │ │ +static shared_ptr Sigma(size_t dim, double sigma, bool smart=true) │ │ │ │ │ +An isotropic noise model created by specifying a standard devation sigma. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.cpp:597 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_C_r_e_a_t_e │ │ │ │ │ +static shared_ptr Create(size_t dim) │ │ │ │ │ +Create a unit covariance noise model. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:597 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_i_n_s_e_r_t │ │ │ │ │ +void insert(Key j, const Value &val) │ │ │ │ │ +Add a variable with the given j, throws KeyAlreadyExists if j is already │ │ │ │ │ +present. │ │ │ │ │ +DDeeffiinniittiioonn Values.cpp:157 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +Non-linear factor for a constraint derived from a 2D measurement. │ │ │ │ │ +DDeeffiinniittiioonn TriangulationFactor.h:33 │ │ │ │ │ +_V_a_l_u_e_s │ │ │ │ │ +In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ +a non-linear function... │ │ │ │ │ +_S_y_m_b_o_l_._h │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _R_o_t_2_._h │ │ │ │ │ + * _t_r_i_a_n_g_u_l_a_t_i_o_n_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00410.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3_S2.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cyclic.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,42 +94,47 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
Cal3_S2.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
Cyclic.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

The most common 5DOF 3D->2D calibration. │ │ │ │ +

Cyclic group, i.e., the integers modulo N. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::Cyclic< N >
 Cyclic group of order N. More...
 
struct  gtsam::traits< Cyclic< N > >
 Define cyclic group to be a model of the Additive Group concept. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -std::ostream & gtsam::operator<< (std::ostream &os, const Cal3_S2 &cal)
 
│ │ │ │

Detailed Description

│ │ │ │ -

The most common 5DOF 3D->2D calibration.

│ │ │ │ +

Cyclic group, i.e., the integers modulo N.

│ │ │ │
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,29 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Cal3_S2.cpp File Reference │ │ │ │ │ -The most common 5DOF 3D->2D calibration. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +Cyclic.h File Reference │ │ │ │ │ +Cyclic group, i.e., the integers modulo N. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_C_y_c_l_i_c_<_ _N_ _> │ │ │ │ │ +  _C_y_c_l_i_c group of order N. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _C_y_c_l_i_c_<_ _N_ _>_ _> │ │ │ │ │ +  Define cyclic group to be a model of the Additive Group concept. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _C_a_l_3___S_2 &cal) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -The most common 5DOF 3D->2D calibration. │ │ │ │ │ +Cyclic group, i.e., the integers modulo N. │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3___S_2_._c_p_p │ │ │ │ │ + * _C_y_c_l_i_c_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00413.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3DS2_Base.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3DS2.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,39 +96,39 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces | │ │ │ │ Functions
│ │ │ │ -
Cal3DS2_Base.cpp File Reference
│ │ │ │ +
Cal3DS2.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

│ │ │ │ -std::ostream & gtsam::operator<< (std::ostream &os, const Cal3DS2_Base &cal)
 
│ │ │ │ +std::ostream & gtsam::operator<< (std::ostream &os, const Cal3DS2 &cal)
 
│ │ │ │

Detailed Description

│ │ │ │
Date
Feb 28, 2010
│ │ │ │
Author
ydjian
│ │ │ │
│ │ │ │ Varun Agrawal
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,24 +1,24 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Cal3DS2_Base.cpp File Reference │ │ │ │ │ +Cal3DS2.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _C_a_l_3_D_S_2___B_a_s_e &cal) │ │ │ │ │ +std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _C_a_l_3_D_S_2 &cal) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Date │ │ │ │ │ Feb 28, 2010 │ │ │ │ │ Author │ │ │ │ │ ydjian │ │ │ │ │ Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3_D_S_2___B_a_s_e_._c_p_p │ │ │ │ │ + * _C_a_l_3_D_S_2_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00422.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Line3.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/PinholeCamera.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,56 +95,50 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
Line3.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
PinholeCamera.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

4 dimensional manifold of 3D lines │ │ │ │ +

Base class for all pinhole cameras. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::Line3
 A 3D line (R,a,b) : (Rot3,Scalar,Scalar) More...
class  gtsam::PinholeCamera< Calibration >
 A pinhole camera class that has a Pose3 and a Calibration. More...
 
struct  gtsam::traits< Line3 >
struct  gtsam::traits< PinholeCamera< Calibration > >
 
struct  gtsam::traits< const Line3 >
struct  gtsam::traits< const PinholeCamera< Calibration > >
 
struct  gtsam::Range< PinholeCamera< Calibration >, T >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

Line3 gtsam::transformTo (const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose=boost::none, OptionalJacobian< 4, 4 > Dline=boost::none)
 Transform a line from world to camera frame.
 
│ │ │ │

Detailed Description

│ │ │ │ -

4 dimensional manifold of 3D lines

│ │ │ │ -
Author
Akshay Krishnan
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ +

Base class for all pinhole cameras.

│ │ │ │ +
Author
Yong-Dian Jian
│ │ │ │ +
Date
Jan 27, 2012
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,35 +1,33 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Line3.h File Reference │ │ │ │ │ -4 dimensional manifold of 3D lines _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +PinholeCamera.h File Reference │ │ │ │ │ +Base class for all pinhole cameras. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_L_i_n_e_3 │ │ │ │ │ -  A 3D line (R,a,b) : (_R_o_t_3,Scalar,Scalar) _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_<_ _C_a_l_i_b_r_a_t_i_o_n_ _> │ │ │ │ │ +  A pinhole camera class that has a _P_o_s_e_3 and a Calibration. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _L_i_n_e_3_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _P_i_n_h_o_l_e_C_a_m_e_r_a_<_ _C_a_l_i_b_r_a_t_i_o_n_ _>_ _> │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _L_i_n_e_3_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _P_i_n_h_o_l_e_C_a_m_e_r_a_<_ _C_a_l_i_b_r_a_t_i_o_n_ _>_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_R_a_n_g_e_<_ _P_i_n_h_o_l_e_C_a_m_e_r_a_<_ _C_a_l_i_b_r_a_t_i_o_n_ _>_,_ _T_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -_L_i_n_e_3  _g_t_s_a_m_:_:_t_r_a_n_s_f_o_r_m_T_o (const _P_o_s_e_3 &wTc, const _L_i_n_e_3 &wL, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< │ │ │ │ │ - 4, 6 > Dpose=boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 4, 4 > Dline=boost::none) │ │ │ │ │ -  Transform a line from world to camera frame. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -4 dimensional manifold of 3D lines │ │ │ │ │ +Base class for all pinhole cameras. │ │ │ │ │ Author │ │ │ │ │ - Akshay Krishnan │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Yong-Dian Jian │ │ │ │ │ + Date │ │ │ │ │ + Jan 27, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _L_i_n_e_3_._h │ │ │ │ │ + * _P_i_n_h_o_l_e_C_a_m_e_r_a_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00422.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,5 +1,5 @@ │ │ │ │ │ var a00422 = [ │ │ │ │ │ - ["gtsam::traits< Line3 >", "a03084.html", null], │ │ │ │ │ - ["gtsam::traits< const Line3 >", "a03088.html", null], │ │ │ │ │ - ["transformTo", "a00422.html#aae9294b064e306ac7993dea546c0d8fb", null] │ │ │ │ │ + ["gtsam::traits< PinholeCamera< Calibration > >", "a03108.html", null], │ │ │ │ │ + ["gtsam::traits< const PinholeCamera< Calibration > >", "a03112.html", null], │ │ │ │ │ + ["gtsam::Range< PinholeCamera< Calibration >, T >", "a03116.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00422_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Line3.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/PinholeCamera.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,130 +98,400 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Line3.h
│ │ │ │ +
PinholeCamera.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ -
2 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ -
3 * Atlanta, Georgia 30332-0415
│ │ │ │ -
4 * All Rights Reserved
│ │ │ │ -
5 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ -
6 * See LICENSE for the license information
│ │ │ │ -
7 * -------------------------------------------------------------------------- */
│ │ │ │ -
8
│ │ │ │ -
15// \callgraph
│ │ │ │ -
16
│ │ │ │ -
17#pragma once
│ │ │ │ -
18
│ │ │ │ -
19#include <gtsam/geometry/Rot3.h>
│ │ │ │ -
20#include <gtsam/geometry/Pose3.h>
│ │ │ │ -
21
│ │ │ │ -
22namespace gtsam {
│ │ │ │ +
2
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
4 * Atlanta, Georgia 30332-0415
│ │ │ │ +
5 * All Rights Reserved
│ │ │ │ +
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
7
│ │ │ │ +
8 * See LICENSE for the license information
│ │ │ │ +
9
│ │ │ │ +
10 * -------------------------------------------------------------------------- */
│ │ │ │ +
11
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/geometry/PinholePose.h>
│ │ │ │ +
22#include <gtsam/geometry/BearingRange.h>
│ │ │ │
23
│ │ │ │ -
24class Line3;
│ │ │ │ +
24namespace gtsam {
│ │ │ │
25
│ │ │ │ -
34GTSAM_EXPORT Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
│ │ │ │ -
35 OptionalJacobian<4, 6> Dpose = boost::none,
│ │ │ │ -
36 OptionalJacobian<4, 4> Dline = boost::none);
│ │ │ │ -
37
│ │ │ │ -
38
│ │ │ │ -
│ │ │ │ -
44class GTSAM_EXPORT Line3 {
│ │ │ │ -
45 private:
│ │ │ │ -
46 Rot3 R_; // Rotation of line about x and y in world frame
│ │ │ │ -
47 double a_, b_; // Intersection of line with the world x-y plane rotated by R_
│ │ │ │ -
48 // Also the closest point on line to origin
│ │ │ │ -
49 public:
│ │ │ │ -
50 enum { dimension = 4 };
│ │ │ │ +
32template<typename Calibration>
│ │ │ │ +
│ │ │ │ +
33class PinholeCamera: public PinholeBaseK<Calibration> {
│ │ │ │ +
34
│ │ │ │ +
35public:
│ │ │ │ +
36
│ │ │ │ +
41 typedef Point2 Measurement;
│ │ │ │ +
42 typedef Point2Vector MeasurementVector;
│ │ │ │ +
43
│ │ │ │ +
44private:
│ │ │ │ +
45
│ │ │ │ +
46 typedef PinholeBaseK<Calibration> Base;
│ │ │ │ +
47 Calibration K_;
│ │ │ │ +
48
│ │ │ │ +
49 // Get dimensions of calibration type at compile time
│ │ │ │ +
50 static const int DimK = FixedDimension<Calibration>::value;
│ │ │ │
51
│ │ │ │ -
│ │ │ │ -
53 Line3() :
│ │ │ │ -
54 a_(0), b_(0) {}
│ │ │ │ -
│ │ │ │ -
55
│ │ │ │ -
│ │ │ │ -
57 Line3(const Rot3 &R, const double a, const double b) :
│ │ │ │ -
58 R_(R), a_(a), b_(b) {}
│ │ │ │ -
│ │ │ │ -
59
│ │ │ │ -
71 Line3 retract(const Vector4 &v,
│ │ │ │ -
72 OptionalJacobian<4, 4> Dp = boost::none,
│ │ │ │ -
73 OptionalJacobian<4, 4> Dv = boost::none) const;
│ │ │ │ +
52public:
│ │ │ │ +
53
│ │ │ │ +
54 enum {
│ │ │ │ +
55 dimension = 6 + DimK
│ │ │ │ +
56 };
│ │ │ │ +
57
│ │ │ │ +
60
│ │ │ │ +
│ │ │ │ +
62 PinholeCamera() {
│ │ │ │ +
63 }
│ │ │ │ +
│ │ │ │ +
64
│ │ │ │ +
│ │ │ │ +
66 explicit PinholeCamera(const Pose3& pose) :
│ │ │ │ +
67 Base(pose) {
│ │ │ │ +
68 }
│ │ │ │ +
│ │ │ │ +
69
│ │ │ │ +
│ │ │ │ +
71 PinholeCamera(const Pose3& pose, const Calibration& K) :
│ │ │ │ +
72 Base(pose), K_(K) {
│ │ │ │ +
73 }
│ │ │ │ +
│ │ │ │
74
│ │ │ │ -
84 Vector4 localCoordinates(const Line3 &q,
│ │ │ │ -
85 OptionalJacobian<4, 4> Dp = boost::none,
│ │ │ │ -
86 OptionalJacobian<4, 4> Dq = boost::none) const;
│ │ │ │ -
87
│ │ │ │ -
92 void print(const std::string &s = "") const;
│ │ │ │ -
93
│ │ │ │ -
100 bool equals(const Line3 &l2, double tol = 10e-9) const;
│ │ │ │ -
101
│ │ │ │ -
108 Unit3 project(OptionalJacobian<2, 4> Dline = boost::none) const;
│ │ │ │ +
78
│ │ │ │ +
│ │ │ │ +
86 static PinholeCamera Level(const Calibration &K, const Pose2& pose2,
│ │ │ │ +
87 double height) {
│ │ │ │ +
88 return PinholeCamera(Base::LevelPose(pose2, height), K);
│ │ │ │ +
89 }
│ │ │ │ +
│ │ │ │ +
90
│ │ │ │ +
│ │ │ │ +
92 static PinholeCamera Level(const Pose2& pose2, double height) {
│ │ │ │ +
93 return PinholeCamera::Level(Calibration(), pose2, height);
│ │ │ │ +
94 }
│ │ │ │ +
│ │ │ │ +
95
│ │ │ │ +
│ │ │ │ +
105 static PinholeCamera Lookat(const Point3& eye, const Point3& target,
│ │ │ │ +
106 const Point3& upVector, const Calibration& K = Calibration()) {
│ │ │ │ +
107 return PinholeCamera(Base::LookatPose(eye, target, upVector), K);
│ │ │ │ +
108 }
│ │ │ │ +
│ │ │ │
109
│ │ │ │ -
118 Point3 point(double distance = 0) const;
│ │ │ │ -
119
│ │ │ │ -
│ │ │ │ -
123 inline Rot3 R() const {
│ │ │ │ -
124 return R_;
│ │ │ │ -
125 }
│ │ │ │ -
│ │ │ │ -
126
│ │ │ │ -
│ │ │ │ -
130 inline double a() const {
│ │ │ │ -
131 return a_;
│ │ │ │ -
132 }
│ │ │ │ -
│ │ │ │ -
133
│ │ │ │ -
│ │ │ │ -
137 inline double b() const {
│ │ │ │ -
138 return b_;
│ │ │ │ -
139 }
│ │ │ │ -
│ │ │ │ -
140
│ │ │ │ -
149 friend Line3 transformTo(const Pose3 &wTc, const Line3 &wL,
│ │ │ │ -
150 OptionalJacobian<4, 6> Dpose,
│ │ │ │ -
151 OptionalJacobian<4, 4> Dline);
│ │ │ │ -
152};
│ │ │ │ -
│ │ │ │ -
153
│ │ │ │ -
154template<>
│ │ │ │ -
155struct traits<Line3> : public internal::Manifold<Line3> {};
│ │ │ │ -
156
│ │ │ │ -
157template<>
│ │ │ │ -
158struct traits<const Line3> : public internal::Manifold<Line3> {};
│ │ │ │ -
159}
│ │ │ │ -
Pose3.h
3D Pose
│ │ │ │ -
Rot3.h
3D rotation represented as a rotation matrix or quaternion
│ │ │ │ +
110 // Create PinholeCamera, with derivatives
│ │ │ │ +
111 static PinholeCamera Create(const Pose3& pose, const Calibration &K,
│ │ │ │ +
112 OptionalJacobian<dimension, 6> H1 = boost::none, //
│ │ │ │ +
113 OptionalJacobian<dimension, DimK> H2 = boost::none) {
│ │ │ │ +
114 typedef Eigen::Matrix<double, DimK, 6> MatrixK6;
│ │ │ │ +
115 if (H1)
│ │ │ │ +
116 *H1 << I_6x6, MatrixK6::Zero();
│ │ │ │ +
117 typedef Eigen::Matrix<double, 6, DimK> Matrix6K;
│ │ │ │ +
118 typedef Eigen::Matrix<double, DimK, DimK> MatrixK;
│ │ │ │ +
119 if (H2)
│ │ │ │ +
120 *H2 << Matrix6K::Zero(), MatrixK::Identity();
│ │ │ │ +
121 return PinholeCamera(pose,K);
│ │ │ │ +
122 }
│ │ │ │ +
123
│ │ │ │ +
127
│ │ │ │ +
│ │ │ │ +
129 explicit PinholeCamera(const Vector &v) :
│ │ │ │ +
130 Base(v.head<6>()) {
│ │ │ │ +
131 if (v.size() > 6)
│ │ │ │ +
132 K_ = Calibration(v.tail<DimK>());
│ │ │ │ +
133 }
│ │ │ │ +
│ │ │ │ +
134
│ │ │ │ +
│ │ │ │ +
136 PinholeCamera(const Vector &v, const Vector &K) :
│ │ │ │ +
137 Base(v), K_(K) {
│ │ │ │ +
138 }
│ │ │ │ +
│ │ │ │ +
139
│ │ │ │ +
143
│ │ │ │ +
│ │ │ │ +
145 bool equals(const Base &camera, double tol = 1e-9) const {
│ │ │ │ +
146 const PinholeCamera* e = dynamic_cast<const PinholeCamera*>(&camera);
│ │ │ │ +
147 return Base::equals(camera, tol) && K_.equals(e->calibration(), tol);
│ │ │ │ +
148 }
│ │ │ │ +
│ │ │ │ +
149
│ │ │ │ +
│ │ │ │ +
151 void print(const std::string& s = "PinholeCamera") const override {
│ │ │ │ +
152 Base::print(s);
│ │ │ │ +
153 K_.print(s + ".calibration");
│ │ │ │ +
154 }
│ │ │ │ +
│ │ │ │ +
155
│ │ │ │ +
159
│ │ │ │ +
160 ~PinholeCamera() override {
│ │ │ │ +
161 }
│ │ │ │ +
162
│ │ │ │ +
│ │ │ │ +
164 const Pose3& pose() const {
│ │ │ │ +
165 return Base::pose();
│ │ │ │ +
166 }
│ │ │ │ +
│ │ │ │ +
167
│ │ │ │ +
│ │ │ │ +
169 const Pose3& getPose(OptionalJacobian<6, dimension> H) const {
│ │ │ │ +
170 if (H) {
│ │ │ │ +
171 H->setZero();
│ │ │ │ +
172 H->template block<6, 6>(0, 0) = I_6x6;
│ │ │ │ +
173 }
│ │ │ │ +
174 return Base::pose();
│ │ │ │ +
175 }
│ │ │ │ +
│ │ │ │ +
176
│ │ │ │ +
│ │ │ │ +
178 const Calibration& calibration() const override {
│ │ │ │ +
179 return K_;
│ │ │ │ +
180 }
│ │ │ │ +
│ │ │ │ +
181
│ │ │ │ +
185
│ │ │ │ +
│ │ │ │ +
187 size_t dim() const {
│ │ │ │ +
188 return dimension;
│ │ │ │ +
189 }
│ │ │ │ +
│ │ │ │ +
190
│ │ │ │ +
│ │ │ │ +
192 static size_t Dim() {
│ │ │ │ +
193 return dimension;
│ │ │ │ +
194 }
│ │ │ │ +
│ │ │ │ +
195
│ │ │ │ +
196 typedef Eigen::Matrix<double, dimension, 1> VectorK6;
│ │ │ │ +
197
│ │ │ │ +
│ │ │ │ +
199 PinholeCamera retract(const Vector& d) const {
│ │ │ │ +
200 if ((size_t) d.size() == 6)
│ │ │ │ +
201 return PinholeCamera(this->pose().retract(d), calibration());
│ │ │ │ +
202 else
│ │ │ │ +
203 return PinholeCamera(this->pose().retract(d.head<6>()),
│ │ │ │ +
204 calibration().retract(d.tail(calibration().dim())));
│ │ │ │ +
205 }
│ │ │ │ +
│ │ │ │ +
206
│ │ │ │ +
│ │ │ │ +
208 VectorK6 localCoordinates(const PinholeCamera& T2) const {
│ │ │ │ +
209 VectorK6 d;
│ │ │ │ +
210 d.template head<6>() = this->pose().localCoordinates(T2.pose());
│ │ │ │ +
211 d.template tail<DimK>() = calibration().localCoordinates(T2.calibration());
│ │ │ │ +
212 return d;
│ │ │ │ +
213 }
│ │ │ │ +
│ │ │ │ +
214
│ │ │ │ +
│ │ │ │ +
216 static PinholeCamera Identity() {
│ │ │ │ +
217 return PinholeCamera(); // assumes that the default constructor is valid
│ │ │ │ +
218 }
│ │ │ │ +
│ │ │ │ +
219
│ │ │ │ +
223
│ │ │ │ +
224 typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
│ │ │ │ +
225
│ │ │ │ +
229 template<class POINT>
│ │ │ │ +
│ │ │ │ +
230 Point2 _project2(const POINT& pw, OptionalJacobian<2, dimension> Dcamera,
│ │ │ │ +
231 OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint) const {
│ │ │ │ +
232 // We just call 3-derivative version in Base
│ │ │ │ +
233 if (Dcamera){
│ │ │ │ +
234 Matrix26 Dpose;
│ │ │ │ +
235 Eigen::Matrix<double, 2, DimK> Dcal;
│ │ │ │ +
236 const Point2 pi = Base::project(pw, Dpose, Dpoint, Dcal);
│ │ │ │ +
237 *Dcamera << Dpose, Dcal;
│ │ │ │ +
238 return pi;
│ │ │ │ +
239 } else {
│ │ │ │ +
240 return Base::project(pw, boost::none, Dpoint, boost::none);
│ │ │ │ +
241 }
│ │ │ │ +
242 }
│ │ │ │ +
│ │ │ │ +
243
│ │ │ │ +
│ │ │ │ +
245 Point2 project2(const Point3& pw, OptionalJacobian<2, dimension> Dcamera =
│ │ │ │ +
246 boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const {
│ │ │ │ +
247 return _project2(pw, Dcamera, Dpoint);
│ │ │ │ +
248 }
│ │ │ │ +
│ │ │ │ +
249
│ │ │ │ +
│ │ │ │ +
251 Point2 project2(const Unit3& pw, OptionalJacobian<2, dimension> Dcamera =
│ │ │ │ +
252 boost::none, OptionalJacobian<2, 2> Dpoint = boost::none) const {
│ │ │ │ +
253 return _project2(pw, Dcamera, Dpoint);
│ │ │ │ +
254 }
│ │ │ │ +
│ │ │ │ +
255
│ │ │ │ +
│ │ │ │ +
261 double range(const Point3& point, OptionalJacobian<1, dimension> Dcamera =
│ │ │ │ +
262 boost::none, OptionalJacobian<1, 3> Dpoint = boost::none) const {
│ │ │ │ +
263 Matrix16 Dpose_;
│ │ │ │ +
264 double result = this->pose().range(point, Dcamera ? &Dpose_ : 0, Dpoint);
│ │ │ │ +
265 if (Dcamera)
│ │ │ │ +
266 *Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
│ │ │ │ +
267 return result;
│ │ │ │ +
268 }
│ │ │ │ +
│ │ │ │ +
269
│ │ │ │ +
│ │ │ │ +
275 double range(const Pose3& pose, OptionalJacobian<1, dimension> Dcamera =
│ │ │ │ +
276 boost::none, OptionalJacobian<1, 6> Dpose = boost::none) const {
│ │ │ │ +
277 Matrix16 Dpose_;
│ │ │ │ +
278 double result = this->pose().range(pose, Dcamera ? &Dpose_ : 0, Dpose);
│ │ │ │ +
279 if (Dcamera)
│ │ │ │ +
280 *Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
│ │ │ │ +
281 return result;
│ │ │ │ +
282 }
│ │ │ │ +
│ │ │ │ +
283
│ │ │ │ +
289 template<class CalibrationB>
│ │ │ │ +
│ │ │ │ +
290 double range(const PinholeCamera<CalibrationB>& camera,
│ │ │ │ +
291 OptionalJacobian<1, dimension> Dcamera = boost::none,
│ │ │ │ +
292 OptionalJacobian<1, 6 + CalibrationB::dimension> Dother = boost::none) const {
│ │ │ │ +
293 Matrix16 Dcamera_, Dother_;
│ │ │ │ +
294 double result = this->pose().range(camera.pose(), Dcamera ? &Dcamera_ : 0,
│ │ │ │ +
295 Dother ? &Dother_ : 0);
│ │ │ │ +
296 if (Dcamera) {
│ │ │ │ +
297 *Dcamera << Dcamera_, Eigen::Matrix<double, 1, DimK>::Zero();
│ │ │ │ +
298 }
│ │ │ │ +
299 if (Dother) {
│ │ │ │ +
300 Dother->setZero();
│ │ │ │ +
301 Dother->template block<1, 6>(0, 0) = Dother_;
│ │ │ │ +
302 }
│ │ │ │ +
303 return result;
│ │ │ │ +
304 }
│ │ │ │ +
│ │ │ │ +
305
│ │ │ │ +
│ │ │ │ +
311 double range(const CalibratedCamera& camera,
│ │ │ │ +
312 OptionalJacobian<1, dimension> Dcamera = boost::none,
│ │ │ │ +
313 OptionalJacobian<1, 6> Dother = boost::none) const {
│ │ │ │ +
314 return range(camera.pose(), Dcamera, Dother);
│ │ │ │ +
315 }
│ │ │ │ +
│ │ │ │ +
316
│ │ │ │ +
│ │ │ │ +
318 Matrix34 cameraProjectionMatrix() const {
│ │ │ │ +
319 return K_.K() * PinholeBase::pose().inverse().matrix().block(0, 0, 3, 4);
│ │ │ │ +
320 }
│ │ │ │ +
│ │ │ │ +
321
│ │ │ │ +
│ │ │ │ +
323 Vector defaultErrorWhenTriangulatingBehindCamera() const {
│ │ │ │ +
324 return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_.fx());;
│ │ │ │ +
325 }
│ │ │ │ +
│ │ │ │ +
326
│ │ │ │ +
327private:
│ │ │ │ +
328
│ │ │ │ +
330 friend class boost::serialization::access;
│ │ │ │ +
331 template<class Archive>
│ │ │ │ +
332 void serialize(Archive & ar, const unsigned int /*version*/) {
│ │ │ │ +
333 ar
│ │ │ │ +
334 & boost::serialization::make_nvp("PinholeBaseK",
│ │ │ │ +
335 boost::serialization::base_object<Base>(*this));
│ │ │ │ +
336 ar & BOOST_SERIALIZATION_NVP(K_);
│ │ │ │ +
337 }
│ │ │ │ +
338
│ │ │ │ +
339public:
│ │ │ │ +
340 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
341};
│ │ │ │ +
│ │ │ │ +
342
│ │ │ │ +
343// manifold traits
│ │ │ │ +
344
│ │ │ │ +
345template <typename Calibration>
│ │ │ │ +
│ │ │ │ +
346struct traits<PinholeCamera<Calibration> >
│ │ │ │ +
347 : public internal::Manifold<PinholeCamera<Calibration> > {};
│ │ │ │ +
│ │ │ │ +
348
│ │ │ │ +
349template <typename Calibration>
│ │ │ │ +
│ │ │ │ +
350struct traits<const PinholeCamera<Calibration> >
│ │ │ │ +
351 : public internal::Manifold<PinholeCamera<Calibration> > {};
│ │ │ │ +
│ │ │ │ +
352
│ │ │ │ +
353// range traits, used in RangeFactor
│ │ │ │ +
354template <typename Calibration, typename T>
│ │ │ │ +
355struct Range<PinholeCamera<Calibration>, T> : HasRange<PinholeCamera<Calibration>, T, double> {};
│ │ │ │ +
356
│ │ │ │ +
357} // \ gtsam
│ │ │ │ +
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ +
PinholePose.h
Pinhole camera with known calibration.
│ │ │ │ +
BearingRange.h
Bearing-Range product.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ -
gtsam::transformTo
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Transform a line from world to camera frame.
Definition Line3.cpp:94
│ │ │ │ -
gtsam::project
Point2_ project(const Point3_ &p_cam)
Expression version of PinholeBase::Project.
Definition expressions.h:131
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::LieGroup::localCoordinates
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g
Definition Lie.h:136
│ │ │ │
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
│ │ │ │ +
gtsam::FixedDimension
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition Manifold.h:164
│ │ │ │
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Line3
A 3D line (R,a,b) : (Rot3,Scalar,Scalar)
Definition Line3.h:44
│ │ │ │ -
gtsam::Line3::R
Rot3 R() const
Return the rotation of the line.
Definition Line3.h:123
│ │ │ │ -
gtsam::Line3::Line3
Line3()
Default constructor is the Z axis.
Definition Line3.h:53
│ │ │ │ -
gtsam::Line3::Line3
Line3(const Rot3 &R, const double a, const double b)
Constructor for general line from (R, a, b)
Definition Line3.h:57
│ │ │ │ -
gtsam::Line3::b
double b() const
Return the y-coordinate of the intersection of the line with the xy plane.
Definition Line3.h:137
│ │ │ │ -
gtsam::Line3::a
double a() const
Return the x-coordinate of the intersection of the line with the xy plane.
Definition Line3.h:130
│ │ │ │ +
gtsam::Range
Definition BearingRange.h:40
│ │ │ │ +
gtsam::HasRange
Definition BearingRange.h:194
│ │ │ │ +
gtsam::PinholeBase::Dpose
static Matrix26 Dpose(const Point2 &pn, double d)
Calculate Jacobian with respect to pose.
Definition CalibratedCamera.cpp:27
│ │ │ │ +
gtsam::PinholeBase::print
virtual void print(const std::string &s="PinholeBase") const
print
Definition CalibratedCamera.cpp:74
│ │ │ │ +
gtsam::PinholeBase::pose
const Pose3 & pose() const
return pose, constant version
Definition CalibratedCamera.h:152
│ │ │ │ +
gtsam::PinholeBase::LevelPose
static Pose3 LevelPose(const Pose2 &pose2, double height)
Create a level pose at the given 2D pose and height.
Definition CalibratedCamera.cpp:49
│ │ │ │ +
gtsam::PinholeBase::equals
bool equals(const PinholeBase &camera, double tol=1e-9) const
assert equality up to a tolerance
Definition CalibratedCamera.cpp:69
│ │ │ │ +
gtsam::PinholeBase::Dpoint
static Matrix23 Dpoint(const Point2 &pn, double d, const Matrix3 &Rt)
Calculate Jacobian with respect to point.
Definition CalibratedCamera.cpp:37
│ │ │ │ +
gtsam::PinholeBase::LookatPose
static Pose3 LookatPose(const Point3 &eye, const Point3 &target, const Point3 &upVector)
Create a camera pose at the given eye position looking at a target point in the scene with the specif...
Definition CalibratedCamera.cpp:58
│ │ │ │ +
gtsam::CalibratedCamera
A Calibrated camera class [R|-R't], calibration K=I.
Definition CalibratedCamera.h:247
│ │ │ │ +
gtsam::PinholeCamera
A pinhole camera class that has a Pose3 and a Calibration.
Definition PinholeCamera.h:33
│ │ │ │ +
gtsam::PinholeCamera::print
void print(const std::string &s="PinholeCamera") const override
print
Definition PinholeCamera.h:151
│ │ │ │ +
gtsam::PinholeCamera::_project2
Point2 _project2(const POINT &pw, OptionalJacobian< 2, dimension > Dcamera, OptionalJacobian< 2, FixedDimension< POINT >::value > Dpoint) const
Templated projection of a 3D point or a point at infinity into the image.
Definition PinholeCamera.h:230
│ │ │ │ +
gtsam::PinholeCamera::defaultErrorWhenTriangulatingBehindCamera
Vector defaultErrorWhenTriangulatingBehindCamera() const
for Nonlinear Triangulation
Definition PinholeCamera.h:323
│ │ │ │ +
gtsam::PinholeCamera::calibration
const Calibration & calibration() const override
return calibration
Definition PinholeCamera.h:178
│ │ │ │ +
gtsam::PinholeCamera::getPose
const Pose3 & getPose(OptionalJacobian< 6, dimension > H) const
return pose, with derivative
Definition PinholeCamera.h:169
│ │ │ │ +
gtsam::PinholeCamera::project2
Point2 project2(const Point3 &pw, OptionalJacobian< 2, dimension > Dcamera=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none) const
project a 3D point from world coordinates into the image
Definition PinholeCamera.h:245
│ │ │ │ +
gtsam::PinholeCamera::PinholeCamera
PinholeCamera(const Pose3 &pose)
constructor with pose
Definition PinholeCamera.h:66
│ │ │ │ +
gtsam::PinholeCamera::Level
static PinholeCamera Level(const Pose2 &pose2, double height)
PinholeCamera::level with default calibration.
Definition PinholeCamera.h:92
│ │ │ │ +
gtsam::PinholeCamera::PinholeCamera
PinholeCamera(const Pose3 &pose, const Calibration &K)
constructor with pose and calibration
Definition PinholeCamera.h:71
│ │ │ │ +
gtsam::PinholeCamera::Level
static PinholeCamera Level(const Calibration &K, const Pose2 &pose2, double height)
Create a level camera at the given 2D pose and height.
Definition PinholeCamera.h:86
│ │ │ │ +
gtsam::PinholeCamera::dim
size_t dim() const
Definition PinholeCamera.h:187
│ │ │ │ +
gtsam::PinholeCamera::Identity
static PinholeCamera Identity()
for Canonical
Definition PinholeCamera.h:216
│ │ │ │ +
gtsam::PinholeCamera::Dim
static size_t Dim()
Definition PinholeCamera.h:192
│ │ │ │ +
gtsam::PinholeCamera::equals
bool equals(const Base &camera, double tol=1e-9) const
assert equality up to a tolerance
Definition PinholeCamera.h:145
│ │ │ │ +
gtsam::PinholeCamera::range
double range(const Pose3 &pose, OptionalJacobian< 1, dimension > Dcamera=boost::none, OptionalJacobian< 1, 6 > Dpose=boost::none) const
Calculate range to another pose.
Definition PinholeCamera.h:275
│ │ │ │ +
gtsam::PinholeCamera::range
double range(const PinholeCamera< CalibrationB > &camera, OptionalJacobian< 1, dimension > Dcamera=boost::none, OptionalJacobian< 1, 6+CalibrationB::dimension > Dother=boost::none) const
Calculate range to another camera.
Definition PinholeCamera.h:290
│ │ │ │ +
gtsam::PinholeCamera::PinholeCamera
PinholeCamera(const Vector &v, const Vector &K)
Init from Vector and calibration.
Definition PinholeCamera.h:136
│ │ │ │ +
gtsam::PinholeCamera::Lookat
static PinholeCamera Lookat(const Point3 &eye, const Point3 &target, const Point3 &upVector, const Calibration &K=Calibration())
Create a camera at the given eye position looking at a target point in the scene with the specified u...
Definition PinholeCamera.h:105
│ │ │ │ +
gtsam::PinholeCamera::cameraProjectionMatrix
Matrix34 cameraProjectionMatrix() const
for Linear Triangulation
Definition PinholeCamera.h:318
│ │ │ │ +
gtsam::PinholeCamera::localCoordinates
VectorK6 localCoordinates(const PinholeCamera &T2) const
return canonical coordinate
Definition PinholeCamera.h:208
│ │ │ │ +
gtsam::PinholeCamera::retract
PinholeCamera retract(const Vector &d) const
move a cameras according to d
Definition PinholeCamera.h:199
│ │ │ │ +
gtsam::PinholeCamera::access
friend class boost::serialization::access
Serialization function.
Definition PinholeCamera.h:330
│ │ │ │ +
gtsam::PinholeCamera::project2
Point2 project2(const Unit3 &pw, OptionalJacobian< 2, dimension > Dcamera=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const
project a point at infinity from world coordinates into the image
Definition PinholeCamera.h:251
│ │ │ │ +
gtsam::PinholeCamera::pose
const Pose3 & pose() const
return pose
Definition PinholeCamera.h:164
│ │ │ │ +
gtsam::PinholeCamera::PinholeCamera
PinholeCamera(const Vector &v)
Init from vector, can be 6D (default calibration) or dim.
Definition PinholeCamera.h:129
│ │ │ │ +
gtsam::PinholeCamera::PinholeCamera
PinholeCamera()
default constructor
Definition PinholeCamera.h:62
│ │ │ │ +
gtsam::PinholeCamera::Measurement
Point2 Measurement
Some classes template on either PinholeCamera or StereoCamera, and this typedef informs those classes...
Definition PinholeCamera.h:41
│ │ │ │ +
gtsam::PinholeCamera::range
double range(const CalibratedCamera &camera, OptionalJacobian< 1, dimension > Dcamera=boost::none, OptionalJacobian< 1, 6 > Dother=boost::none) const
Calculate range to a calibrated camera.
Definition PinholeCamera.h:311
│ │ │ │ +
gtsam::PinholeCamera::range
double range(const Point3 &point, OptionalJacobian< 1, dimension > Dcamera=boost::none, OptionalJacobian< 1, 3 > Dpoint=boost::none) const
Calculate range to a landmark.
Definition PinholeCamera.h:261
│ │ │ │ +
gtsam::PinholeBaseK
A pinhole camera class that has a Pose3 and a fixed Calibration.
Definition PinholePose.h:34
│ │ │ │ +
gtsam::PinholeBaseK< Calibration >::project
Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > Dcal=boost::none) const
project a 3D point from world coordinates into the image
Definition PinholePose.h:118
│ │ │ │ +
gtsam::Pose2
A 2D pose (Point2,Rot2)
Definition Pose2.h:36
│ │ │ │
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ -
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
│ │ │ │ +
gtsam::Pose3::range
double range(const Point3 &point, OptionalJacobian< 1, 6 > Hself=boost::none, OptionalJacobian< 1, 3 > Hpoint=boost::none) const
Calculate range to a landmark.
Definition Pose3.cpp:399
│ │ │ │ +
gtsam::Pose3::matrix
Matrix4 matrix() const
convert to 4*4 matrix
Definition Pose3.cpp:323
│ │ │ │ +
gtsam::Pose3::inverse
Pose3 inverse() const
inverse transformation with derivatives
Definition Pose3.cpp:49
│ │ │ │
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,163 +1,513 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Line3.h │ │ │ │ │ +PinholeCamera.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ -2 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ -3 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -4 * All Rights Reserved │ │ │ │ │ -5 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ -6 * See LICENSE for the license information │ │ │ │ │ -7 * ------------------------------------------------------------------------- │ │ │ │ │ +2 │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +5 * All Rights Reserved │ │ │ │ │ +6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +7 │ │ │ │ │ +8 * See LICENSE for the license information │ │ │ │ │ +9 │ │ │ │ │ +10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ -8 │ │ │ │ │ -15// \callgraph │ │ │ │ │ -16 │ │ │ │ │ -17#pragma once │ │ │ │ │ -18 │ │ │ │ │ -19#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_3_._h> │ │ │ │ │ -20#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_3_._h> │ │ │ │ │ -21 │ │ │ │ │ -22namespace _g_t_s_a_m { │ │ │ │ │ +11 │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_i_n_h_o_l_e_P_o_s_e_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_B_e_a_r_i_n_g_R_a_n_g_e_._h> │ │ │ │ │ 23 │ │ │ │ │ -24class Line3; │ │ │ │ │ +24namespace _g_t_s_a_m { │ │ │ │ │ 25 │ │ │ │ │ -34GTSAM_EXPORT Line3 _t_r_a_n_s_f_o_r_m_T_o(const Pose3 &wTc, const Line3 &wL, │ │ │ │ │ -35 OptionalJacobian<4, 6> Dpose = boost::none, │ │ │ │ │ -36 OptionalJacobian<4, 4> Dline = boost::none); │ │ │ │ │ -37 │ │ │ │ │ -38 │ │ │ │ │ -_4_4class GTSAM_EXPORT _L_i_n_e_3 { │ │ │ │ │ -45 private: │ │ │ │ │ -46 _R_o_t_3 R_; // Rotation of line about x and y in world frame │ │ │ │ │ -47 double a_, b_; // Intersection of line with the world x-y plane rotated by │ │ │ │ │ -R_ │ │ │ │ │ -48 // Also the closest point on line to origin │ │ │ │ │ -49 public: │ │ │ │ │ -50 enum { dimension = 4 }; │ │ │ │ │ +32template │ │ │ │ │ +_3_3class _P_i_n_h_o_l_e_C_a_m_e_r_a: public _P_i_n_h_o_l_e_B_a_s_e_K { │ │ │ │ │ +34 │ │ │ │ │ +35public: │ │ │ │ │ +36 │ │ │ │ │ +_4_1 typedef _P_o_i_n_t_2 _M_e_a_s_u_r_e_m_e_n_t; │ │ │ │ │ +42 typedef Point2Vector MeasurementVector; │ │ │ │ │ +43 │ │ │ │ │ +44private: │ │ │ │ │ +45 │ │ │ │ │ +46 typedef _P_i_n_h_o_l_e_B_a_s_e_K_<_C_a_l_i_b_r_a_t_i_o_n_> _B_a_s_e; │ │ │ │ │ +47 Calibration K_; │ │ │ │ │ +48 │ │ │ │ │ +49 // Get dimensions of calibration type at compile time │ │ │ │ │ +50 static const int DimK = _F_i_x_e_d_D_i_m_e_n_s_i_o_n_<_C_a_l_i_b_r_a_t_i_o_n_>_:_:_v_a_l_u_e; │ │ │ │ │ 51 │ │ │ │ │ -_5_3 _L_i_n_e_3() : │ │ │ │ │ -54 a_(0), b_(0) {} │ │ │ │ │ -55 │ │ │ │ │ -_5_7 _L_i_n_e_3(const _R_o_t_3 &R, const double a, const double b) : │ │ │ │ │ -58 R_(R), a_(a), b_(b) {} │ │ │ │ │ -59 │ │ │ │ │ -71 _L_i_n_e_3 retract(const Vector4 &v, │ │ │ │ │ -72 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_4_,_ _4_> Dp = boost::none, │ │ │ │ │ -73 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_4_,_ _4_> Dv = boost::none) const; │ │ │ │ │ +52public: │ │ │ │ │ +53 │ │ │ │ │ +54 enum { │ │ │ │ │ +55 dimension = 6 + DimK │ │ │ │ │ +56 }; │ │ │ │ │ +57 │ │ │ │ │ +60 │ │ │ │ │ +_6_2 _P_i_n_h_o_l_e_C_a_m_e_r_a() { │ │ │ │ │ +63 } │ │ │ │ │ +64 │ │ │ │ │ +_6_6 explicit _P_i_n_h_o_l_e_C_a_m_e_r_a(const _P_o_s_e_3& _p_o_s_e) : │ │ │ │ │ +67 _B_a_s_e(_p_o_s_e) { │ │ │ │ │ +68 } │ │ │ │ │ +69 │ │ │ │ │ +_7_1 _P_i_n_h_o_l_e_C_a_m_e_r_a(const _P_o_s_e_3& _p_o_s_e, const Calibration& K) : │ │ │ │ │ +72 _B_a_s_e(_p_o_s_e), K_(K) { │ │ │ │ │ +73 } │ │ │ │ │ 74 │ │ │ │ │ -84 Vector4 localCoordinates(const _L_i_n_e_3 &q, │ │ │ │ │ -85 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_4_,_ _4_> Dp = boost::none, │ │ │ │ │ -86 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_4_,_ _4_> Dq = boost::none) const; │ │ │ │ │ -87 │ │ │ │ │ -92 void _p_r_i_n_t(const std::string &s = "") const; │ │ │ │ │ -93 │ │ │ │ │ -100 bool _e_q_u_a_l_s(const _L_i_n_e_3 &l2, double tol = 10e-9) const; │ │ │ │ │ -101 │ │ │ │ │ -108 _U_n_i_t_3 _p_r_o_j_e_c_t(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _4_> Dline = boost::none) const; │ │ │ │ │ +78 │ │ │ │ │ +_8_6 static _P_i_n_h_o_l_e_C_a_m_e_r_a _L_e_v_e_l(const Calibration &K, const _P_o_s_e_2& pose2, │ │ │ │ │ +87 double height) { │ │ │ │ │ +88 return _P_i_n_h_o_l_e_C_a_m_e_r_a(_B_a_s_e_:_:_L_e_v_e_l_P_o_s_e(pose2, height), K); │ │ │ │ │ +89 } │ │ │ │ │ +90 │ │ │ │ │ +_9_2 static _P_i_n_h_o_l_e_C_a_m_e_r_a _L_e_v_e_l(const _P_o_s_e_2& pose2, double height) { │ │ │ │ │ +93 return _P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_L_e_v_e_l(Calibration(), pose2, height); │ │ │ │ │ +94 } │ │ │ │ │ +95 │ │ │ │ │ +_1_0_5 static _P_i_n_h_o_l_e_C_a_m_e_r_a _L_o_o_k_a_t(const _P_o_i_n_t_3& eye, const _P_o_i_n_t_3& target, │ │ │ │ │ +106 const _P_o_i_n_t_3& upVector, const Calibration& K = Calibration()) { │ │ │ │ │ +107 return _P_i_n_h_o_l_e_C_a_m_e_r_a(_B_a_s_e_:_:_L_o_o_k_a_t_P_o_s_e(eye, target, upVector), K); │ │ │ │ │ +108 } │ │ │ │ │ 109 │ │ │ │ │ -118 _P_o_i_n_t_3 point(double distance = 0) const; │ │ │ │ │ -119 │ │ │ │ │ -_1_2_3 inline _R_o_t_3 _R() const { │ │ │ │ │ -124 return R_; │ │ │ │ │ -125 } │ │ │ │ │ -126 │ │ │ │ │ -_1_3_0 inline double _a() const { │ │ │ │ │ -131 return a_; │ │ │ │ │ -132 } │ │ │ │ │ -133 │ │ │ │ │ -_1_3_7 inline double _b() const { │ │ │ │ │ -138 return b_; │ │ │ │ │ -139 } │ │ │ │ │ -140 │ │ │ │ │ -149 friend _L_i_n_e_3 _t_r_a_n_s_f_o_r_m_T_o(const _P_o_s_e_3 &wTc, const _L_i_n_e_3 &wL, │ │ │ │ │ -150 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_4_,_ _6_> Dpose, │ │ │ │ │ -151 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_4_,_ _4_> Dline); │ │ │ │ │ -152}; │ │ │ │ │ -153 │ │ │ │ │ -154template<> │ │ │ │ │ -_1_5_5struct _t_r_a_i_t_s<_L_i_n_e_3> : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d {}; │ │ │ │ │ -156 │ │ │ │ │ -157template<> │ │ │ │ │ -_1_5_8struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d {}; │ │ │ │ │ -159} │ │ │ │ │ -_P_o_s_e_3_._h │ │ │ │ │ -3D Pose │ │ │ │ │ -_R_o_t_3_._h │ │ │ │ │ -3D rotation represented as a rotation matrix or quaternion │ │ │ │ │ +110 // Create PinholeCamera, with derivatives │ │ │ │ │ +111 static _P_i_n_h_o_l_e_C_a_m_e_r_a Create(const _P_o_s_e_3& _p_o_s_e, const Calibration &K, │ │ │ │ │ +112 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_d_i_m_e_n_s_i_o_n_,_ _6_> H1 = boost::none, // │ │ │ │ │ +113 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_d_i_m_e_n_s_i_o_n_,_ _D_i_m_K_> H2 = boost::none) { │ │ │ │ │ +114 typedef Eigen::Matrix MatrixK6; │ │ │ │ │ +115 if (H1) │ │ │ │ │ +116 *H1 << I_6x6, MatrixK6::Zero(); │ │ │ │ │ +117 typedef Eigen::Matrix Matrix6K; │ │ │ │ │ +118 typedef Eigen::Matrix MatrixK; │ │ │ │ │ +119 if (H2) │ │ │ │ │ +120 *H2 << Matrix6K::Zero(), MatrixK::Identity(); │ │ │ │ │ +121 return _P_i_n_h_o_l_e_C_a_m_e_r_a(_p_o_s_e,K); │ │ │ │ │ +122 } │ │ │ │ │ +123 │ │ │ │ │ +127 │ │ │ │ │ +_1_2_9 explicit _P_i_n_h_o_l_e_C_a_m_e_r_a(const Vector &v) : │ │ │ │ │ +130 _B_a_s_e(v.head<6>()) { │ │ │ │ │ +131 if (v.size() > 6) │ │ │ │ │ +132 K_ = Calibration(v.tail()); │ │ │ │ │ +133 } │ │ │ │ │ +134 │ │ │ │ │ +_1_3_6 _P_i_n_h_o_l_e_C_a_m_e_r_a(const Vector &v, const Vector &K) : │ │ │ │ │ +137 _B_a_s_e(v), K_(K) { │ │ │ │ │ +138 } │ │ │ │ │ +139 │ │ │ │ │ +143 │ │ │ │ │ +_1_4_5 bool _e_q_u_a_l_s(const _B_a_s_e &camera, double tol = 1e-9) const { │ │ │ │ │ +146 const _P_i_n_h_o_l_e_C_a_m_e_r_a* e = dynamic_cast(&camera); │ │ │ │ │ +147 return _B_a_s_e_:_:_e_q_u_a_l_s(camera, tol) && K_._e_q_u_a_l_s(e->_c_a_l_i_b_r_a_t_i_o_n(), tol); │ │ │ │ │ +148 } │ │ │ │ │ +149 │ │ │ │ │ +_1_5_1 void _p_r_i_n_t(const std::string& s = "PinholeCamera") const override { │ │ │ │ │ +152 _B_a_s_e_:_:_p_r_i_n_t(s); │ │ │ │ │ +153 K_.print(s + ".calibration"); │ │ │ │ │ +154 } │ │ │ │ │ +155 │ │ │ │ │ +159 │ │ │ │ │ +160 _~_P_i_n_h_o_l_e_C_a_m_e_r_a() override { │ │ │ │ │ +161 } │ │ │ │ │ +162 │ │ │ │ │ +_1_6_4 const _P_o_s_e_3& _p_o_s_e() const { │ │ │ │ │ +165 return _B_a_s_e_:_:_p_o_s_e(); │ │ │ │ │ +166 } │ │ │ │ │ +167 │ │ │ │ │ +_1_6_9 const _P_o_s_e_3& _g_e_t_P_o_s_e(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _d_i_m_e_n_s_i_o_n_> H) const { │ │ │ │ │ +170 if (H) { │ │ │ │ │ +171 H->setZero(); │ │ │ │ │ +172 H->template block<6, 6>(0, 0) = I_6x6; │ │ │ │ │ +173 } │ │ │ │ │ +174 return _B_a_s_e_:_:_p_o_s_e(); │ │ │ │ │ +175 } │ │ │ │ │ +176 │ │ │ │ │ +_1_7_8 const Calibration& _c_a_l_i_b_r_a_t_i_o_n() const override { │ │ │ │ │ +179 return K_; │ │ │ │ │ +180 } │ │ │ │ │ +181 │ │ │ │ │ +185 │ │ │ │ │ +_1_8_7 size_t _d_i_m() const { │ │ │ │ │ +188 return dimension; │ │ │ │ │ +189 } │ │ │ │ │ +190 │ │ │ │ │ +_1_9_2 static size_t _D_i_m() { │ │ │ │ │ +193 return dimension; │ │ │ │ │ +194 } │ │ │ │ │ +195 │ │ │ │ │ +196 typedef Eigen::Matrix VectorK6; │ │ │ │ │ +197 │ │ │ │ │ +_1_9_9 _P_i_n_h_o_l_e_C_a_m_e_r_a _r_e_t_r_a_c_t(const Vector& d) const { │ │ │ │ │ +200 if ((size_t) d.size() == 6) │ │ │ │ │ +201 return _P_i_n_h_o_l_e_C_a_m_e_r_a(this->_p_o_s_e()._r_e_t_r_a_c_t(d), _c_a_l_i_b_r_a_t_i_o_n()); │ │ │ │ │ +202 else │ │ │ │ │ +203 return _P_i_n_h_o_l_e_C_a_m_e_r_a(this->_p_o_s_e()._r_e_t_r_a_c_t(d.head<6>()), │ │ │ │ │ +204 _c_a_l_i_b_r_a_t_i_o_n()._r_e_t_r_a_c_t(d.tail(_c_a_l_i_b_r_a_t_i_o_n()._d_i_m()))); │ │ │ │ │ +205 } │ │ │ │ │ +206 │ │ │ │ │ +_2_0_8 VectorK6 _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(const _P_i_n_h_o_l_e_C_a_m_e_r_a& T2) const { │ │ │ │ │ +209 VectorK6 d; │ │ │ │ │ +210 d.template head<6>() = this->_p_o_s_e()._l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(T2._p_o_s_e()); │ │ │ │ │ +211 d.template tail() = _c_a_l_i_b_r_a_t_i_o_n().localCoordinates(T2._c_a_l_i_b_r_a_t_i_o_n()); │ │ │ │ │ +212 return d; │ │ │ │ │ +213 } │ │ │ │ │ +214 │ │ │ │ │ +_2_1_6 static _P_i_n_h_o_l_e_C_a_m_e_r_a _I_d_e_n_t_i_t_y() { │ │ │ │ │ +217 return _P_i_n_h_o_l_e_C_a_m_e_r_a(); // assumes that the default constructor is valid │ │ │ │ │ +218 } │ │ │ │ │ +219 │ │ │ │ │ +223 │ │ │ │ │ +224 typedef Eigen::Matrix Matrix2K; │ │ │ │ │ +225 │ │ │ │ │ +229 template │ │ │ │ │ +_2_3_0 _P_o_i_n_t_2 ___p_r_o_j_e_c_t_2(const POINT& pw, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _d_i_m_e_n_s_i_o_n_> Dcamera, │ │ │ │ │ +231 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n<2, _F_i_x_e_d_D_i_m_e_n_s_i_o_n_<_P_O_I_N_T_>_:_:_v_a_l_u_e> _D_p_o_i_n_t) const { │ │ │ │ │ +232 // We just call 3-derivative version in Base │ │ │ │ │ +233 if (Dcamera){ │ │ │ │ │ +234 Matrix26 _D_p_o_s_e; │ │ │ │ │ +235 Eigen::Matrix Dcal; │ │ │ │ │ +236 const _P_o_i_n_t_2 pi = _B_a_s_e_:_:_p_r_o_j_e_c_t(pw, _D_p_o_s_e, _D_p_o_i_n_t, Dcal); │ │ │ │ │ +237 *Dcamera << _D_p_o_s_e, Dcal; │ │ │ │ │ +238 return pi; │ │ │ │ │ +239 } else { │ │ │ │ │ +240 return _B_a_s_e_:_:_p_r_o_j_e_c_t(pw, boost::none, _D_p_o_i_n_t, boost::none); │ │ │ │ │ +241 } │ │ │ │ │ +242 } │ │ │ │ │ +243 │ │ │ │ │ +_2_4_5 _P_o_i_n_t_2 _p_r_o_j_e_c_t_2(const _P_o_i_n_t_3& pw, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _d_i_m_e_n_s_i_o_n_> Dcamera = │ │ │ │ │ +246 boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> _D_p_o_i_n_t = boost::none) const { │ │ │ │ │ +247 return ___p_r_o_j_e_c_t_2(pw, Dcamera, _D_p_o_i_n_t); │ │ │ │ │ +248 } │ │ │ │ │ +249 │ │ │ │ │ +_2_5_1 _P_o_i_n_t_2 _p_r_o_j_e_c_t_2(const _U_n_i_t_3& pw, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _d_i_m_e_n_s_i_o_n_> Dcamera = │ │ │ │ │ +252 boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> _D_p_o_i_n_t = boost::none) const { │ │ │ │ │ +253 return ___p_r_o_j_e_c_t_2(pw, Dcamera, _D_p_o_i_n_t); │ │ │ │ │ +254 } │ │ │ │ │ +255 │ │ │ │ │ +_2_6_1 double _r_a_n_g_e(const _P_o_i_n_t_3& point, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _d_i_m_e_n_s_i_o_n_> Dcamera = │ │ │ │ │ +262 boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> _D_p_o_i_n_t = boost::none) const { │ │ │ │ │ +263 Matrix16 Dpose_; │ │ │ │ │ +264 double result = this->_p_o_s_e()._r_a_n_g_e(point, Dcamera ? &Dpose_ : 0, _D_p_o_i_n_t); │ │ │ │ │ +265 if (Dcamera) │ │ │ │ │ +266 *Dcamera << Dpose_, Eigen::Matrix::Zero(); │ │ │ │ │ +267 return result; │ │ │ │ │ +268 } │ │ │ │ │ +269 │ │ │ │ │ +_2_7_5 double _r_a_n_g_e(const _P_o_s_e_3& _p_o_s_e, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _d_i_m_e_n_s_i_o_n_> Dcamera = │ │ │ │ │ +276 boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> _D_p_o_s_e = boost::none) const { │ │ │ │ │ +277 Matrix16 Dpose_; │ │ │ │ │ +278 double result = this->_p_o_s_e()._r_a_n_g_e(pose, Dcamera ? &Dpose_ : 0, _D_p_o_s_e); │ │ │ │ │ +279 if (Dcamera) │ │ │ │ │ +280 *Dcamera << Dpose_, Eigen::Matrix::Zero(); │ │ │ │ │ +281 return result; │ │ │ │ │ +282 } │ │ │ │ │ +283 │ │ │ │ │ +289 template │ │ │ │ │ +_2_9_0 double _r_a_n_g_e(const _P_i_n_h_o_l_e_C_a_m_e_r_a_<_C_a_l_i_b_r_a_t_i_o_n_B_>& camera, │ │ │ │ │ +291 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _d_i_m_e_n_s_i_o_n_> Dcamera = boost::none, │ │ │ │ │ +292 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_ _+_ _C_a_l_i_b_r_a_t_i_o_n_B_:_:_d_i_m_e_n_s_i_o_n_> Dother = boost::none) │ │ │ │ │ +const { │ │ │ │ │ +293 Matrix16 Dcamera_, Dother_; │ │ │ │ │ +294 double result = this->_p_o_s_e()._r_a_n_g_e(camera._p_o_s_e(), Dcamera ? &Dcamera_ : 0, │ │ │ │ │ +295 Dother ? &Dother_ : 0); │ │ │ │ │ +296 if (Dcamera) { │ │ │ │ │ +297 *Dcamera << Dcamera_, Eigen::Matrix::Zero(); │ │ │ │ │ +298 } │ │ │ │ │ +299 if (Dother) { │ │ │ │ │ +300 Dother->setZero(); │ │ │ │ │ +301 Dother->template block<1, 6>(0, 0) = Dother_; │ │ │ │ │ +302 } │ │ │ │ │ +303 return result; │ │ │ │ │ +304 } │ │ │ │ │ +305 │ │ │ │ │ +_3_1_1 double _r_a_n_g_e(const _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a& camera, │ │ │ │ │ +312 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _d_i_m_e_n_s_i_o_n_> Dcamera = boost::none, │ │ │ │ │ +313 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> Dother = boost::none) const { │ │ │ │ │ +314 return _r_a_n_g_e(camera._p_o_s_e(), Dcamera, Dother); │ │ │ │ │ +315 } │ │ │ │ │ +316 │ │ │ │ │ +_3_1_8 Matrix34 _c_a_m_e_r_a_P_r_o_j_e_c_t_i_o_n_M_a_t_r_i_x() const { │ │ │ │ │ +319 return K_.K() * _P_i_n_h_o_l_e_B_a_s_e_:_:_p_o_s_e()._i_n_v_e_r_s_e()._m_a_t_r_i_x().block(0, 0, 3, 4); │ │ │ │ │ +320 } │ │ │ │ │ +321 │ │ │ │ │ +_3_2_3 Vector _d_e_f_a_u_l_t_E_r_r_o_r_W_h_e_n_T_r_i_a_n_g_u_l_a_t_i_n_g_B_e_h_i_n_d_C_a_m_e_r_a() const { │ │ │ │ │ +324 return Eigen::Matrix::dimension,1>::Constant(2.0 * │ │ │ │ │ +K_.fx());; │ │ │ │ │ +325 } │ │ │ │ │ +326 │ │ │ │ │ +327private: │ │ │ │ │ +328 │ │ │ │ │ +_3_3_0 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +331 template │ │ │ │ │ +332 void serialize(Archive & ar, const unsigned int /*version*/) { │ │ │ │ │ +333 ar │ │ │ │ │ +334 & boost::serialization::make_nvp("PinholeBaseK", │ │ │ │ │ +335 boost::serialization::base_object(*this)); │ │ │ │ │ +336 ar & BOOST_SERIALIZATION_NVP(K_); │ │ │ │ │ +337 } │ │ │ │ │ +338 │ │ │ │ │ +339public: │ │ │ │ │ +340 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +341}; │ │ │ │ │ +342 │ │ │ │ │ +343// manifold traits │ │ │ │ │ +344 │ │ │ │ │ +345template │ │ │ │ │ +_3_4_6struct _t_r_a_i_t_s<_P_i_n_h_o_l_e_C_a_m_e_r_a > │ │ │ │ │ +347 : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d > {}; │ │ │ │ │ +348 │ │ │ │ │ +349template │ │ │ │ │ +_3_5_0struct _t_r_a_i_t_s > │ │ │ │ │ +351 : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d > {}; │ │ │ │ │ +352 │ │ │ │ │ +353// range traits, used in RangeFactor │ │ │ │ │ +354template │ │ │ │ │ +_3_5_5struct _R_a_n_g_e<_P_i_n_h_o_l_e_C_a_m_e_r_a, T> : │ │ │ │ │ +_H_a_s_R_a_n_g_e, T, double> {}; │ │ │ │ │ +356 │ │ │ │ │ +357} // \ gtsam │ │ │ │ │ +_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ +This marks a GTSAM object to require alignment. │ │ │ │ │ +DDeeffiinniittiioonn types.h:308 │ │ │ │ │ +_P_i_n_h_o_l_e_P_o_s_e_._h │ │ │ │ │ +Pinhole camera with known calibration. │ │ │ │ │ +_B_e_a_r_i_n_g_R_a_n_g_e_._h │ │ │ │ │ +Bearing-Range product. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ +Vector2 Point2 │ │ │ │ │ +As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ +typedef Point2 to Vector2... │ │ │ │ │ +DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ _g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ Vector3 Point3 │ │ │ │ │ As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ typedef Point3 to Vector3... │ │ │ │ │ DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_n_s_f_o_r_m_T_o │ │ │ │ │ -Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > │ │ │ │ │ -Dpose, OptionalJacobian< 4, 4 > Dline) │ │ │ │ │ -Transform a line from world to camera frame. │ │ │ │ │ -DDeeffiinniittiioonn Line3.cpp:94 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_o_j_e_c_t │ │ │ │ │ -Point2_ project(const Point3_ &p_cam) │ │ │ │ │ -Expression version of PinholeBase::Project. │ │ │ │ │ -DDeeffiinniittiioonn expressions.h:131 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ +TangentVector localCoordinates(const Class &g) const │ │ │ │ │ +localCoordinates as required by manifold concept: finds tangent vector between │ │ │ │ │ +*this and g │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:136 │ │ │ │ │ _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ Both ManifoldTraits and Testable. │ │ │ │ │ DDeeffiinniittiioonn Manifold.h:120 │ │ │ │ │ +_g_t_s_a_m_:_:_F_i_x_e_d_D_i_m_e_n_s_i_o_n │ │ │ │ │ +Give fixed size dimension of a type, fails at compile time if dynamic. │ │ │ │ │ +DDeeffiinniittiioonn Manifold.h:164 │ │ │ │ │ _g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ either a fixed size o... │ │ │ │ │ DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_n_e_3 │ │ │ │ │ -A 3D line (R,a,b) : (Rot3,Scalar,Scalar) │ │ │ │ │ -DDeeffiinniittiioonn Line3.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_n_e_3_:_:_R │ │ │ │ │ -Rot3 R() const │ │ │ │ │ -Return the rotation of the line. │ │ │ │ │ -DDeeffiinniittiioonn Line3.h:123 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_n_e_3_:_:_L_i_n_e_3 │ │ │ │ │ -Line3() │ │ │ │ │ -Default constructor is the Z axis. │ │ │ │ │ -DDeeffiinniittiioonn Line3.h:53 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_n_e_3_:_:_L_i_n_e_3 │ │ │ │ │ -Line3(const Rot3 &R, const double a, const double b) │ │ │ │ │ -Constructor for general line from (R, a, b) │ │ │ │ │ -DDeeffiinniittiioonn Line3.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_n_e_3_:_:_b │ │ │ │ │ -double b() const │ │ │ │ │ -Return the y-coordinate of the intersection of the line with the xy plane. │ │ │ │ │ -DDeeffiinniittiioonn Line3.h:137 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_n_e_3_:_:_a │ │ │ │ │ -double a() const │ │ │ │ │ -Return the x-coordinate of the intersection of the line with the xy plane. │ │ │ │ │ -DDeeffiinniittiioonn Line3.h:130 │ │ │ │ │ +_g_t_s_a_m_:_:_R_a_n_g_e │ │ │ │ │ +DDeeffiinniittiioonn BearingRange.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_H_a_s_R_a_n_g_e │ │ │ │ │ +DDeeffiinniittiioonn BearingRange.h:194 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_D_p_o_s_e │ │ │ │ │ +static Matrix26 Dpose(const Point2 &pn, double d) │ │ │ │ │ +Calculate Jacobian with respect to pose. │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.cpp:27 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_p_r_i_n_t │ │ │ │ │ +virtual void print(const std::string &s="PinholeBase") const │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.cpp:74 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_p_o_s_e │ │ │ │ │ +const Pose3 & pose() const │ │ │ │ │ +return pose, constant version │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:152 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_L_e_v_e_l_P_o_s_e │ │ │ │ │ +static Pose3 LevelPose(const Pose2 &pose2, double height) │ │ │ │ │ +Create a level pose at the given 2D pose and height. │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.cpp:49 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const PinholeBase &camera, double tol=1e-9) const │ │ │ │ │ +assert equality up to a tolerance │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.cpp:69 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_D_p_o_i_n_t │ │ │ │ │ +static Matrix23 Dpoint(const Point2 &pn, double d, const Matrix3 &Rt) │ │ │ │ │ +Calculate Jacobian with respect to point. │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.cpp:37 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_L_o_o_k_a_t_P_o_s_e │ │ │ │ │ +static Pose3 LookatPose(const Point3 &eye, const Point3 &target, const Point3 │ │ │ │ │ +&upVector) │ │ │ │ │ +Create a camera pose at the given eye position looking at a target point in the │ │ │ │ │ +scene with the specif... │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.cpp:58 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a │ │ │ │ │ +A Calibrated camera class [R|-R't], calibration K=I. │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:247 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a │ │ │ │ │ +A pinhole camera class that has a Pose3 and a Calibration. │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:33 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="PinholeCamera") const override │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:___p_r_o_j_e_c_t_2 │ │ │ │ │ +Point2 _project2(const POINT &pw, OptionalJacobian< 2, dimension > Dcamera, │ │ │ │ │ +OptionalJacobian< 2, FixedDimension< POINT >::value > Dpoint) const │ │ │ │ │ +Templated projection of a 3D point or a point at infinity into the image. │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:230 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_d_e_f_a_u_l_t_E_r_r_o_r_W_h_e_n_T_r_i_a_n_g_u_l_a_t_i_n_g_B_e_h_i_n_d_C_a_m_e_r_a │ │ │ │ │ +Vector defaultErrorWhenTriangulatingBehindCamera() const │ │ │ │ │ +for Nonlinear Triangulation │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:323 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_c_a_l_i_b_r_a_t_i_o_n │ │ │ │ │ +const Calibration & calibration() const override │ │ │ │ │ +return calibration │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:178 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_g_e_t_P_o_s_e │ │ │ │ │ +const Pose3 & getPose(OptionalJacobian< 6, dimension > H) const │ │ │ │ │ +return pose, with derivative │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:169 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_p_r_o_j_e_c_t_2 │ │ │ │ │ +Point2 project2(const Point3 &pw, OptionalJacobian< 2, dimension > │ │ │ │ │ +Dcamera=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none) const │ │ │ │ │ +project a 3D point from world coordinates into the image │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:245 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a │ │ │ │ │ +PinholeCamera(const Pose3 &pose) │ │ │ │ │ +constructor with pose │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:66 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_L_e_v_e_l │ │ │ │ │ +static PinholeCamera Level(const Pose2 &pose2, double height) │ │ │ │ │ +PinholeCamera::level with default calibration. │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:92 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a │ │ │ │ │ +PinholeCamera(const Pose3 &pose, const Calibration &K) │ │ │ │ │ +constructor with pose and calibration │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:71 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_L_e_v_e_l │ │ │ │ │ +static PinholeCamera Level(const Calibration &K, const Pose2 &pose2, double │ │ │ │ │ +height) │ │ │ │ │ +Create a level camera at the given 2D pose and height. │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_d_i_m │ │ │ │ │ +size_t dim() const │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:187 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_I_d_e_n_t_i_t_y │ │ │ │ │ +static PinholeCamera Identity() │ │ │ │ │ +for Canonical │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:216 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_D_i_m │ │ │ │ │ +static size_t Dim() │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:192 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const Base &camera, double tol=1e-9) const │ │ │ │ │ +assert equality up to a tolerance │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:145 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_r_a_n_g_e │ │ │ │ │ +double range(const Pose3 &pose, OptionalJacobian< 1, dimension > │ │ │ │ │ +Dcamera=boost::none, OptionalJacobian< 1, 6 > Dpose=boost::none) const │ │ │ │ │ +Calculate range to another pose. │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:275 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_r_a_n_g_e │ │ │ │ │ +double range(const PinholeCamera< CalibrationB > &camera, OptionalJacobian< 1, │ │ │ │ │ +dimension > Dcamera=boost::none, OptionalJacobian< 1, 6+CalibrationB::dimension │ │ │ │ │ +> Dother=boost::none) const │ │ │ │ │ +Calculate range to another camera. │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:290 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a │ │ │ │ │ +PinholeCamera(const Vector &v, const Vector &K) │ │ │ │ │ +Init from Vector and calibration. │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:136 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_L_o_o_k_a_t │ │ │ │ │ +static PinholeCamera Lookat(const Point3 &eye, const Point3 &target, const │ │ │ │ │ +Point3 &upVector, const Calibration &K=Calibration()) │ │ │ │ │ +Create a camera at the given eye position looking at a target point in the │ │ │ │ │ +scene with the specified u... │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:105 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_c_a_m_e_r_a_P_r_o_j_e_c_t_i_o_n_M_a_t_r_i_x │ │ │ │ │ +Matrix34 cameraProjectionMatrix() const │ │ │ │ │ +for Linear Triangulation │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:318 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ +VectorK6 localCoordinates(const PinholeCamera &T2) const │ │ │ │ │ +return canonical coordinate │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:208 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_r_e_t_r_a_c_t │ │ │ │ │ +PinholeCamera retract(const Vector &d) const │ │ │ │ │ +move a cameras according to d │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:199 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:330 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_p_r_o_j_e_c_t_2 │ │ │ │ │ +Point2 project2(const Unit3 &pw, OptionalJacobian< 2, dimension > │ │ │ │ │ +Dcamera=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const │ │ │ │ │ +project a point at infinity from world coordinates into the image │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:251 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_p_o_s_e │ │ │ │ │ +const Pose3 & pose() const │ │ │ │ │ +return pose │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:164 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a │ │ │ │ │ +PinholeCamera(const Vector &v) │ │ │ │ │ +Init from vector, can be 6D (default calibration) or dim. │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:129 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a │ │ │ │ │ +PinholeCamera() │ │ │ │ │ +default constructor │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:62 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_M_e_a_s_u_r_e_m_e_n_t │ │ │ │ │ +Point2 Measurement │ │ │ │ │ +Some classes template on either PinholeCamera or StereoCamera, and this typedef │ │ │ │ │ +informs those classes... │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_r_a_n_g_e │ │ │ │ │ +double range(const CalibratedCamera &camera, OptionalJacobian< 1, dimension > │ │ │ │ │ +Dcamera=boost::none, OptionalJacobian< 1, 6 > Dother=boost::none) const │ │ │ │ │ +Calculate range to a calibrated camera. │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:311 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_r_a_n_g_e │ │ │ │ │ +double range(const Point3 &point, OptionalJacobian< 1, dimension > │ │ │ │ │ +Dcamera=boost::none, OptionalJacobian< 1, 3 > Dpoint=boost::none) const │ │ │ │ │ +Calculate range to a landmark. │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:261 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K │ │ │ │ │ +A pinhole camera class that has a Pose3 and a fixed Calibration. │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_<_ _C_a_l_i_b_r_a_t_i_o_n_ _>_:_:_p_r_o_j_e_c_t │ │ │ │ │ +Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, │ │ │ │ │ +OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > │ │ │ │ │ +Dcal=boost::none) const │ │ │ │ │ +project a 3D point from world coordinates into the image │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:118 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2 │ │ │ │ │ +A 2D pose (Point2,Rot2) │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:36 │ │ │ │ │ _g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ -Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor │ │ │ │ │ -symbol GTSAM_USE_QUATERNIO... │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_r_a_n_g_e │ │ │ │ │ +double range(const Point3 &point, OptionalJacobian< 1, 6 > Hself=boost::none, │ │ │ │ │ +OptionalJacobian< 1, 3 > Hpoint=boost::none) const │ │ │ │ │ +Calculate range to a landmark. │ │ │ │ │ +DDeeffiinniittiioonn Pose3.cpp:399 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_m_a_t_r_i_x │ │ │ │ │ +Matrix4 matrix() const │ │ │ │ │ +convert to 4*4 matrix │ │ │ │ │ +DDeeffiinniittiioonn Pose3.cpp:323 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_i_n_v_e_r_s_e │ │ │ │ │ +Pose3 inverse() const │ │ │ │ │ +inverse transformation with derivatives │ │ │ │ │ +DDeeffiinniittiioonn Pose3.cpp:49 │ │ │ │ │ _g_t_s_a_m_:_:_U_n_i_t_3 │ │ │ │ │ Represents a 3D point on a unit sphere. │ │ │ │ │ DDeeffiinniittiioonn Unit3.h:43 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _L_i_n_e_3_._h │ │ │ │ │ + * _P_i_n_h_o_l_e_C_a_m_e_r_a_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00425.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Similarity3.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/BearingRange.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,44 +94,51 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
Similarity3.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
BearingRange.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Implementation of Similarity3 transform. │ │ │ │ +

Bearing-Range product. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

struct  gtsam::BearingRange< A1, A2, B, R >
 Bearing-Range product for a particular A1,A2 combination will use the functors above to create a similar functor of type A1*A2 -> pair<Bearing::return_type,Range::return_type> For example BearingRange<Pose2,Point2>(pose,point) will return pair<Rot2,double> and BearingRange<Pose3,Point3>(pose,point) will return pair<Unit3,double> More...
 
struct  gtsam::traits< BearingRange< A1, A2 > >
 
struct  gtsam::HasBearing< A1, A2, RT >
 
struct  gtsam::HasRange< A1, A2, RT >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -std::ostream & gtsam::operator<< (std::ostream &os, const Similarity3 &p)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Implementation of Similarity3 transform.

│ │ │ │ -
Author
Paul Drews
│ │ │ │ -
│ │ │ │ -John Lambert
│ │ │ │ +

Bearing-Range product.

│ │ │ │ +
Date
July, 2015
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,24 +1,38 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Similarity3.cpp File Reference │ │ │ │ │ -Implementation of Similarity3 transform. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +BearingRange.h File Reference │ │ │ │ │ +Bearing-Range product. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +struct   _g_t_s_a_m_:_:_B_e_a_r_i_n_g_R_a_n_g_e_<_ _A_1_,_ _A_2_,_ _B_,_ _R_ _> │ │ │ │ │ + Bearing-Range product for a particular A1,A2 combination will use the │ │ │ │ │ + functors above to create a similar functor of type A1*A2 - │ │ │ │ │ +  > pair For example │ │ │ │ │ + BearingRange(pose,point) will return pair │ │ │ │ │ + and BearingRange(pose,point) will return │ │ │ │ │ + pair _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _B_e_a_r_i_n_g_R_a_n_g_e_<_ _A_1_,_ _A_2_ _>_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_H_a_s_B_e_a_r_i_n_g_<_ _A_1_,_ _A_2_,_ _R_T_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_H_a_s_R_a_n_g_e_<_ _A_1_,_ _A_2_,_ _R_T_ _> │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _S_i_m_i_l_a_r_i_t_y_3 &p) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Implementation of Similarity3 transform. │ │ │ │ │ +Bearing-Range product. │ │ │ │ │ + Date │ │ │ │ │ + July, 2015 │ │ │ │ │ Author │ │ │ │ │ - Paul Drews │ │ │ │ │ - John Lambert │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_i_m_i_l_a_r_i_t_y_3_._c_p_p │ │ │ │ │ + * _B_e_a_r_i_n_g_R_a_n_g_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00431_source.html │ │ │ │ @@ -114,20 +114,20 @@ │ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
18#pragma once
│ │ │ │
19
│ │ │ │ -
20#include <gtsam/base/Lie.h>
│ │ │ │ -
21#include <gtsam/base/Manifold.h>
│ │ │ │ +
20#include <gtsam/base/Lie.h>
│ │ │ │ +
21#include <gtsam/base/Manifold.h>
│ │ │ │
22#include <gtsam/dllexport.h>
│ │ │ │ -
23#include <gtsam/geometry/Point2.h>
│ │ │ │ -
24#include <gtsam/geometry/Pose2.h>
│ │ │ │ -
25#include <gtsam/geometry/Rot2.h>
│ │ │ │ +
23#include <gtsam/geometry/Point2.h>
│ │ │ │ +
24#include <gtsam/geometry/Pose2.h>
│ │ │ │ +
25#include <gtsam/geometry/Rot2.h>
│ │ │ │
26
│ │ │ │
27namespace gtsam {
│ │ │ │
28
│ │ │ │
29// Forward declarations
│ │ │ │
30class Pose2;
│ │ │ │
31
│ │ │ │
│ │ │ │ @@ -223,19 +223,19 @@ │ │ │ │
194template <>
│ │ │ │
195struct traits<Similarity2> : public internal::LieGroup<Similarity2> {};
│ │ │ │
196
│ │ │ │
197template <>
│ │ │ │
198struct traits<const Similarity2> : public internal::LieGroup<Similarity2> {};
│ │ │ │
199
│ │ │ │
200} // namespace gtsam
│ │ │ │ -
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ -
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ -
Pose2.h
2D Pose
│ │ │ │ -
Rot2.h
2D rotation
│ │ │ │ -
Point2.h
2D Point
│ │ │ │ +
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ +
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ +
Point2.h
2D Point
│ │ │ │ +
Pose2.h
2D Pose
│ │ │ │ +
Rot2.h
2D rotation
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
│ │ │ │
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │
gtsam::operator==
bool operator==(const Matrix &A, const Matrix &B)
equality is just equal_with_abs_tol 1e-9
Definition Matrix.h:100
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │
gtsam::LieGroup
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition Lie.h:37
│ │ │ │ ├── html2text {} │ │ │ │ │ @@ -122,24 +122,24 @@ │ │ │ │ │ _1_9_5struct _t_r_a_i_t_s<_S_i_m_i_l_a_r_i_t_y_2> : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ 196 │ │ │ │ │ 197template <> │ │ │ │ │ _1_9_8struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ {}; │ │ │ │ │ 199 │ │ │ │ │ 200} // namespace gtsam │ │ │ │ │ -_L_i_e_._h │ │ │ │ │ -Base class and basic functions for Lie types. │ │ │ │ │ _M_a_n_i_f_o_l_d_._h │ │ │ │ │ Base class and basic functions for Manifold types. │ │ │ │ │ +_L_i_e_._h │ │ │ │ │ +Base class and basic functions for Lie types. │ │ │ │ │ +_P_o_i_n_t_2_._h │ │ │ │ │ +2D Point │ │ │ │ │ _P_o_s_e_2_._h │ │ │ │ │ 2D Pose │ │ │ │ │ _R_o_t_2_._h │ │ │ │ │ 2D rotation │ │ │ │ │ -_P_o_i_n_t_2_._h │ │ │ │ │ -2D Point │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ print without optional string, must specify cout yourself │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00434.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Rot2.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SimpleCamera.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,36 +94,57 @@ │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces
│ │ │ │ -
Rot2.cpp File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Typedefs
│ │ │ │ +
SimpleCamera.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

2D Rotations │ │ │ │ +

A simple camera class with a Cal3_S2 calibration. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Typedefs

using gtsam::PinholeCameraCal3_S2 = gtsam::PinholeCamera< gtsam::Cal3_S2 >
 Convenient aliases for Pinhole camera classes with different calibrations.
 
│ │ │ │ +using gtsam::PinholeCameraCal3Bundler = gtsam::PinholeCamera< gtsam::Cal3Bundler >
 
│ │ │ │ +using gtsam::PinholeCameraCal3DS2 = gtsam::PinholeCamera< gtsam::Cal3DS2 >
 
│ │ │ │ +using gtsam::PinholeCameraCal3Unified = gtsam::PinholeCamera< gtsam::Cal3Unified >
 
│ │ │ │ +using gtsam::PinholeCameraCal3Fisheye = gtsam::PinholeCamera< gtsam::Cal3Fisheye >
 
│ │ │ │

Detailed Description

│ │ │ │ -

2D Rotations

│ │ │ │ -
Date
Dec 9, 2009
│ │ │ │ +

A simple camera class with a Cal3_S2 calibration.

│ │ │ │ +
Date
Aug 16, 2009
│ │ │ │
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,39 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Rot2.cpp File Reference │ │ │ │ │ -2D Rotations _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ +SimpleCamera.h File Reference │ │ │ │ │ +A simple camera class with a Cal3_S2 calibration. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +using  _g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_C_a_l_3___S_2 = _g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a< _g_t_s_a_m_:_:_C_a_l_3___S_2 > │ │ │ │ │ +  Convenient aliases for Pinhole camera classes with different │ │ │ │ │ + calibrations. │ │ │ │ │ +  │ │ │ │ │ +using  ggttssaamm::::PPiinnhhoolleeCCaammeerraaCCaall33BBuunnddlleerr = _g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a< _g_t_s_a_m_:_: │ │ │ │ │ + _C_a_l_3_B_u_n_d_l_e_r > │ │ │ │ │ +  │ │ │ │ │ +using  ggttssaamm::::PPiinnhhoolleeCCaammeerraaCCaall33DDSS22 = _g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a< _g_t_s_a_m_:_:_C_a_l_3_D_S_2 > │ │ │ │ │ +  │ │ │ │ │ +using  ggttssaamm::::PPiinnhhoolleeCCaammeerraaCCaall33UUnniiffiieedd = _g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a< _g_t_s_a_m_:_: │ │ │ │ │ + _C_a_l_3_U_n_i_f_i_e_d > │ │ │ │ │ +  │ │ │ │ │ +using  ggttssaamm::::PPiinnhhoolleeCCaammeerraaCCaall33FFiisshheeyyee = _g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a< _g_t_s_a_m_:_: │ │ │ │ │ + _C_a_l_3_F_i_s_h_e_y_e > │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -2D Rotations │ │ │ │ │ +A simple camera class with a Cal3_S2 calibration. │ │ │ │ │ Date │ │ │ │ │ - Dec 9, 2009 │ │ │ │ │ + Aug 16, 2009 │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _R_o_t_2_._c_p_p │ │ │ │ │ + * _S_i_m_p_l_e_C_a_m_e_r_a_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00437.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SO3.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SOn.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,117 +94,38 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
│ │ │ │ -
SO3.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
SOn.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

3*3 matrix representation of SO(3) │ │ │ │ +

Definitions of dynamic specializations of SO(n) │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::so3::ExpmapFunctor
 Functor implementing Exponential map. More...
 
class  gtsam::so3::DexpFunctor
 Functor that implements Exponential map and its derivatives. More...
 
struct  gtsam::traits< SO3 >
 
struct  gtsam::traits< const SO3 >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Typedefs

│ │ │ │ -using gtsam::SO3 = SO< 3 >
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -template<class Archive >
void gtsam::serialize (Archive &ar, SO3 &R, const unsigned int)
 Serialization function.
 
GTSAM_EXPORT Matrix3 gtsam::so3::compose (const Matrix3 &M, const SO3 &R, OptionalJacobian< 9, 9 > H=boost::none)
 Compose general matrix with an SO(3) element.
 
│ │ │ │ -GTSAM_EXPORT Matrix99 gtsam::so3::Dcompose (const SO3 &R)
 (constant) Jacobian of compose wrpt M
 
│ │ │ │

Detailed Description

│ │ │ │ -

3*3 matrix representation of SO(3)

│ │ │ │ +

Definitions of dynamic specializations of SO(n)

│ │ │ │
Author
Frank Dellaert
│ │ │ │
│ │ │ │ -Luca Carlone
│ │ │ │ -
│ │ │ │ -Duy Nguyen Ta
│ │ │ │ -
Date
December 2014
│ │ │ │ -

Function Documentation

│ │ │ │ - │ │ │ │ -

◆ compose()

│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
GTSAM_EXPORT Matrix3 gtsam::so3::compose (const Matrix3 & M,
const SO3 & R,
OptionalJacobian< 9, 9 > H = boost::none 
)
│ │ │ │ -
│ │ │ │ - │ │ │ │ -

Compose general matrix with an SO(3) element.

│ │ │ │ -

We only provide the 9*9 derivative in the first argument M.

│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ +Varun Agrawal │ │ │ │ +
Date
March 2019
│ │ │ │ +
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,61 +1,23 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -SO3.h File Reference │ │ │ │ │ -3*3 matrix representation of SO(3) _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_s_o_3_:_:_E_x_p_m_a_p_F_u_n_c_t_o_r │ │ │ │ │ -  Functor implementing Exponential map. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_s_o_3_:_:_D_e_x_p_F_u_n_c_t_o_r │ │ │ │ │ -  Functor that implements Exponential map aanndd its derivatives. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_O_3_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _S_O_3_ _> │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +SOn.cpp File Reference │ │ │ │ │ +Definitions of dynamic specializations of SO(n) _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -using  ggttssaamm::::SSOO33 = _S_O< 3 > │ │ │ │ │ -  │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ - void  ggttssaamm::::sseerriiaalliizzee (Archive &ar, _S_O_3 &R, const unsigned │ │ │ │ │ - int) │ │ │ │ │ -  Serialization function. │ │ │ │ │ -  │ │ │ │ │ - GTSAM_EXPORT Matrix3  _g_t_s_a_m_:_:_s_o_3_:_:_c_o_m_p_o_s_e (const Matrix3 &M, const _S_O_3 &R, │ │ │ │ │ - _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 9, 9 > H=boost::none) │ │ │ │ │ -  Compose general matrix with an SO(3) element. │ │ │ │ │ -  │ │ │ │ │ -GTSAM_EXPORT Matrix99  ggttssaamm::::ssoo33::::DDccoommppoossee (const _S_O_3 &R) │ │ │ │ │ -  (constant) Jacobian of compose wrpt M │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -3*3 matrix representation of SO(3) │ │ │ │ │ +Definitions of dynamic specializations of SO(n) │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Luca Carlone │ │ │ │ │ - Duy Nguyen Ta │ │ │ │ │ + Varun Agrawal │ │ │ │ │ Date │ │ │ │ │ - December 2014 │ │ │ │ │ -********** FFuunnccttiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ -********** _?◆_? ccoommppoossee(()) ********** │ │ │ │ │ -GTSAM_EXPORT Matrix3 gtsam::so3:: ( const Matrix3 &  MM, │ │ │ │ │ -compose │ │ │ │ │ - const _S_O_3 &  RR, │ │ │ │ │ - _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 9, 9 >  HH = boost::none  │ │ │ │ │ - ) │ │ │ │ │ -Compose general matrix with an SO(3) element. │ │ │ │ │ -We only provide the 9*9 derivative in the first argument M. │ │ │ │ │ + March 2019 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_O_3_._h │ │ │ │ │ + * _S_O_n_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00443.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cyclic.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/CalibratedCamera.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,35 +94,56 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
Cyclic.cpp File Reference
│ │ │ │ +
CalibratedCamera.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Cyclic group implementation. │ │ │ │ +

Calibrated camera for which only pose is unknown. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::CheiralityException
 
class  gtsam::PinholeBase
 A pinhole camera class that has a Pose3, functions as base class for all pinhole cameras. More...
 
class  gtsam::CalibratedCamera
 A Calibrated camera class [R|-R't], calibration K=I. More...
 
struct  gtsam::traits< CalibratedCamera >
 
struct  gtsam::traits< const CalibratedCamera >
 
struct  gtsam::Range< CalibratedCamera, T >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Cyclic group implementation.

│ │ │ │ +

Calibrated camera for which only pose is unknown.

│ │ │ │ +
Date
Aug 17, 2009
│ │ │ │
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,39 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Cyclic.cpp File Reference │ │ │ │ │ -Cyclic group implementation. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +CalibratedCamera.h File Reference │ │ │ │ │ +Calibrated camera for which only pose is unknown. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e │ │ │ │ │ +  A pinhole camera class that has a _P_o_s_e_3, functions as base class for │ │ │ │ │ + all pinhole cameras. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a │ │ │ │ │ +  A Calibrated camera class [R|-R't], calibration K=I. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_R_a_n_g_e_<_ _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_,_ _T_ _> │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Cyclic group implementation. │ │ │ │ │ +Calibrated camera for which only pose is unknown. │ │ │ │ │ + Date │ │ │ │ │ + Aug 17, 2009 │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_y_c_l_i_c_._c_p_p │ │ │ │ │ + * _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00446.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/CalibratedCamera.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SO3.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,55 +95,116 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
CalibratedCamera.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ +Functions
│ │ │ │ +
SO3.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Calibrated camera for which only pose is unknown. │ │ │ │ +

3*3 matrix representation of SO(3) │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::CheiralityException
class  gtsam::so3::ExpmapFunctor
 Functor implementing Exponential map. More...
 
class  gtsam::PinholeBase
 A pinhole camera class that has a Pose3, functions as base class for all pinhole cameras. More...
class  gtsam::so3::DexpFunctor
 Functor that implements Exponential map and its derivatives. More...
 
class  gtsam::CalibratedCamera
 A Calibrated camera class [R|-R't], calibration K=I. More...
struct  gtsam::traits< SO3 >
 
struct  gtsam::traits< CalibratedCamera >
 
struct  gtsam::traits< const CalibratedCamera >
 
struct  gtsam::Range< CalibratedCamera, T >
struct  gtsam::traits< const SO3 >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Typedefs

│ │ │ │ +using gtsam::SO3 = SO< 3 >
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +template<class Archive >
void gtsam::serialize (Archive &ar, SO3 &R, const unsigned int)
 Serialization function.
 
GTSAM_EXPORT Matrix3 gtsam::so3::compose (const Matrix3 &M, const SO3 &R, OptionalJacobian< 9, 9 > H=boost::none)
 Compose general matrix with an SO(3) element.
 
│ │ │ │ +GTSAM_EXPORT Matrix99 gtsam::so3::Dcompose (const SO3 &R)
 (constant) Jacobian of compose wrpt M
 
│ │ │ │

Detailed Description

│ │ │ │ -

Calibrated camera for which only pose is unknown.

│ │ │ │ -
Date
Aug 17, 2009
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ +

3*3 matrix representation of SO(3)

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ +Luca Carlone
│ │ │ │ +
│ │ │ │ +Duy Nguyen Ta
│ │ │ │ +
Date
December 2014
│ │ │ │ +

Function Documentation

│ │ │ │ + │ │ │ │ +

◆ compose()

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
GTSAM_EXPORT Matrix3 gtsam::so3::compose (const Matrix3 & M,
const SO3 & R,
OptionalJacobian< 9, 9 > H = boost::none 
)
│ │ │ │ +
│ │ │ │ + │ │ │ │ +

Compose general matrix with an SO(3) element.

│ │ │ │ +

We only provide the 9*9 derivative in the first argument M.

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,39 +1,61 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -CalibratedCamera.h File Reference │ │ │ │ │ -Calibrated camera for which only pose is unknown. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +SO3.h File Reference │ │ │ │ │ +3*3 matrix representation of SO(3) _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n │ │ │ │ │ + class   _g_t_s_a_m_:_:_s_o_3_:_:_E_x_p_m_a_p_F_u_n_c_t_o_r │ │ │ │ │ +  Functor implementing Exponential map. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ - class   _g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e │ │ │ │ │ -  A pinhole camera class that has a _P_o_s_e_3, functions as base class for │ │ │ │ │ - all pinhole cameras. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_s_o_3_:_:_D_e_x_p_F_u_n_c_t_o_r │ │ │ │ │ +  Functor that implements Exponential map aanndd its derivatives. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ - class   _g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a │ │ │ │ │ -  A Calibrated camera class [R|-R't], calibration K=I. _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_O_3_ _> │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_R_a_n_g_e_<_ _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_,_ _T_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _S_O_3_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +using  ggttssaamm::::SSOO33 = _S_O< 3 > │ │ │ │ │ +  │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +template │ │ │ │ │ + void  ggttssaamm::::sseerriiaalliizzee (Archive &ar, _S_O_3 &R, const unsigned │ │ │ │ │ + int) │ │ │ │ │ +  Serialization function. │ │ │ │ │ +  │ │ │ │ │ + GTSAM_EXPORT Matrix3  _g_t_s_a_m_:_:_s_o_3_:_:_c_o_m_p_o_s_e (const Matrix3 &M, const _S_O_3 &R, │ │ │ │ │ + _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 9, 9 > H=boost::none) │ │ │ │ │ +  Compose general matrix with an SO(3) element. │ │ │ │ │ +  │ │ │ │ │ +GTSAM_EXPORT Matrix99  ggttssaamm::::ssoo33::::DDccoommppoossee (const _S_O_3 &R) │ │ │ │ │ +  (constant) Jacobian of compose wrpt M │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Calibrated camera for which only pose is unknown. │ │ │ │ │ - Date │ │ │ │ │ - Aug 17, 2009 │ │ │ │ │ +3*3 matrix representation of SO(3) │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Duy Nguyen Ta │ │ │ │ │ + Date │ │ │ │ │ + December 2014 │ │ │ │ │ +********** FFuunnccttiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ +********** _?◆_? ccoommppoossee(()) ********** │ │ │ │ │ +GTSAM_EXPORT Matrix3 gtsam::so3:: ( const Matrix3 &  MM, │ │ │ │ │ +compose │ │ │ │ │ + const _S_O_3 &  RR, │ │ │ │ │ + _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 9, 9 >  HH = boost::none  │ │ │ │ │ + ) │ │ │ │ │ +Compose general matrix with an SO(3) element. │ │ │ │ │ +We only provide the 9*9 derivative in the first argument M. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_._h │ │ │ │ │ + * _S_O_3_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00446.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,6 +1,9 @@ │ │ │ │ │ var a00446 = [ │ │ │ │ │ - ["gtsam::CheiralityException", "a03020.html", null], │ │ │ │ │ - ["gtsam::traits< CalibratedCamera >", "a03032.html", null], │ │ │ │ │ - ["gtsam::traits< const CalibratedCamera >", "a03036.html", null], │ │ │ │ │ - ["gtsam::Range< CalibratedCamera, T >", "a03040.html", null] │ │ │ │ │ + ["gtsam::so3::ExpmapFunctor", "a03280.html", "a03280"], │ │ │ │ │ + ["gtsam::so3::DexpFunctor", "a03284.html", "a03284"], │ │ │ │ │ + ["gtsam::traits< SO3 >", "a03288.html", null], │ │ │ │ │ + ["gtsam::traits< const SO3 >", "a03292.html", null], │ │ │ │ │ + ["compose", "a00446.html#a72fc8b10f24d82aa8a4c99f99403fe5e", null], │ │ │ │ │ + ["Dcompose", "a00446.html#a7c7e26b6d07ec0f16363c196247b294d", null], │ │ │ │ │ + ["serialize", "a00446.html#a2e0b14126ed42ee95ca5f5f092c5bc84", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00446_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/CalibratedCamera.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SO3.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,368 +98,211 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
CalibratedCamera.h
│ │ │ │ +
SO3.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/geometry/BearingRange.h>
│ │ │ │ -
22#include <gtsam/geometry/Point2.h>
│ │ │ │ -
23#include <gtsam/geometry/Pose3.h>
│ │ │ │ -
24#include <gtsam/base/concepts.h>
│ │ │ │ -
25#include <gtsam/base/Manifold.h>
│ │ │ │ -
26#include <gtsam/base/ThreadsafeException.h>
│ │ │ │ +
21#pragma once
│ │ │ │ +
22
│ │ │ │ +
23#include <gtsam/geometry/SOn.h>
│ │ │ │ +
24
│ │ │ │ +
25#include <gtsam/base/Lie.h>
│ │ │ │ +
26#include <gtsam/base/Matrix.h>
│ │ │ │
27#include <gtsam/dllexport.h>
│ │ │ │ -
28#include <boost/serialization/nvp.hpp>
│ │ │ │ -
29
│ │ │ │ -
30namespace gtsam {
│ │ │ │ +
28
│ │ │ │ +
29#include <cmath>
│ │ │ │ +
30#include <vector>
│ │ │ │
31
│ │ │ │ -
│ │ │ │ -
32class GTSAM_EXPORT CheiralityException: public ThreadsafeException<CheiralityException> {
│ │ │ │ -
33public:
│ │ │ │ -
34 CheiralityException()
│ │ │ │ -
35 : CheiralityException(std::numeric_limits<Key>::max()) {}
│ │ │ │ -
36
│ │ │ │ -
37 CheiralityException(Key j)
│ │ │ │ -
38 : ThreadsafeException<CheiralityException>("CheiralityException"),
│ │ │ │ -
39 j_(j) {}
│ │ │ │ -
40
│ │ │ │ -
41 Key nearbyVariable() const {return j_;}
│ │ │ │ +
32namespace gtsam {
│ │ │ │ +
33
│ │ │ │ +
34using SO3 = SO<3>;
│ │ │ │ +
35
│ │ │ │ +
36// Below are all declarations of SO<3> specializations.
│ │ │ │ +
37// They are *defined* in SO3.cpp.
│ │ │ │ +
38
│ │ │ │ +
39template <>
│ │ │ │ +
40GTSAM_EXPORT
│ │ │ │ +
41SO3 SO3::AxisAngle(const Vector3& axis, double theta);
│ │ │ │
42
│ │ │ │ -
43private:
│ │ │ │ -
44 Key j_;
│ │ │ │ -
45};
│ │ │ │ -
│ │ │ │ +
43template <>
│ │ │ │ +
44GTSAM_EXPORT
│ │ │ │ +
45SO3 SO3::ClosestTo(const Matrix3& M);
│ │ │ │
46
│ │ │ │ -
│ │ │ │ -
52class GTSAM_EXPORT PinholeBase {
│ │ │ │ -
53
│ │ │ │ -
54public:
│ │ │ │ -
55
│ │ │ │ -
57 typedef Rot3 Rotation;
│ │ │ │ -
58 typedef Point3 Translation;
│ │ │ │ -
59
│ │ │ │ -
64 typedef Point2 Measurement;
│ │ │ │ -
65 typedef Point2Vector MeasurementVector;
│ │ │ │ -
66
│ │ │ │ -
67private:
│ │ │ │ -
68
│ │ │ │ -
69 Pose3 pose_;
│ │ │ │ +
47template <>
│ │ │ │ +
48GTSAM_EXPORT
│ │ │ │ +
49SO3 SO3::ChordalMean(const std::vector<SO3>& rotations);
│ │ │ │ +
50
│ │ │ │ +
51template <>
│ │ │ │ +
52GTSAM_EXPORT
│ │ │ │ +
53Matrix3 SO3::Hat(const Vector3& xi);
│ │ │ │ +
54
│ │ │ │ +
55template <>
│ │ │ │ +
56GTSAM_EXPORT
│ │ │ │ +
57Vector3 SO3::Vee(const Matrix3& X);
│ │ │ │ +
58
│ │ │ │ +
60template <>
│ │ │ │ +
61Matrix3 SO3::AdjointMap() const;
│ │ │ │ +
62
│ │ │ │ +
67template <>
│ │ │ │ +
68GTSAM_EXPORT
│ │ │ │ +
69SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H);
│ │ │ │
70
│ │ │ │ -
71protected:
│ │ │ │ -
72
│ │ │ │ +
72template <>
│ │ │ │ +
73GTSAM_EXPORT
│ │ │ │ +
74Matrix3 SO3::ExpmapDerivative(const Vector3& omega);
│ │ │ │
75
│ │ │ │ -
81 static Matrix26 Dpose(const Point2& pn, double d);
│ │ │ │ -
82
│ │ │ │ -
89 static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt);
│ │ │ │ -
90
│ │ │ │ -
92
│ │ │ │ -
93public:
│ │ │ │ -
94
│ │ │ │ +
80template <>
│ │ │ │ +
81GTSAM_EXPORT
│ │ │ │ +
82Vector3 SO3::Logmap(const SO3& R, ChartJacobian H);
│ │ │ │ +
83
│ │ │ │ +
85template <>
│ │ │ │ +
86GTSAM_EXPORT
│ │ │ │ +
87Matrix3 SO3::LogmapDerivative(const Vector3& omega);
│ │ │ │ +
88
│ │ │ │ +
89// Chart at origin for SO3 is *not* Cayley but actual Expmap/Logmap
│ │ │ │ +
90template <>
│ │ │ │ +
91GTSAM_EXPORT
│ │ │ │ +
92SO3 SO3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H);
│ │ │ │ +
93
│ │ │ │ +
94template <>
│ │ │ │ +
95GTSAM_EXPORT
│ │ │ │ +
96Vector3 SO3::ChartAtOrigin::Local(const SO3& R, ChartJacobian H);
│ │ │ │
97
│ │ │ │ -
105 static Pose3 LevelPose(const Pose2& pose2, double height);
│ │ │ │ -
106
│ │ │ │ -
115 static Pose3 LookatPose(const Point3& eye, const Point3& target,
│ │ │ │ -
116 const Point3& upVector);
│ │ │ │ -
117
│ │ │ │ -
121
│ │ │ │ -
123 PinholeBase() {}
│ │ │ │ -
124
│ │ │ │ -
126 explicit PinholeBase(const Pose3& pose) : pose_(pose) {}
│ │ │ │ -
127
│ │ │ │ -
131
│ │ │ │ -
132 explicit PinholeBase(const Vector& v) : pose_(Pose3::Expmap(v)) {}
│ │ │ │ -
133
│ │ │ │ -
135 virtual ~PinholeBase() = default;
│ │ │ │ -
136
│ │ │ │ +
98template <>
│ │ │ │ +
99GTSAM_EXPORT
│ │ │ │ +
100Vector9 SO3::vec(OptionalJacobian<9, 3> H) const;
│ │ │ │ +
101
│ │ │ │ +
103template <class Archive>
│ │ │ │ +
│ │ │ │ +
104void serialize(Archive& ar, SO3& R, const unsigned int /*version*/) {
│ │ │ │ +
105 Matrix3& M = R.matrix_;
│ │ │ │ +
106 ar& boost::serialization::make_nvp("R11", M(0, 0));
│ │ │ │ +
107 ar& boost::serialization::make_nvp("R12", M(0, 1));
│ │ │ │ +
108 ar& boost::serialization::make_nvp("R13", M(0, 2));
│ │ │ │ +
109 ar& boost::serialization::make_nvp("R21", M(1, 0));
│ │ │ │ +
110 ar& boost::serialization::make_nvp("R22", M(1, 1));
│ │ │ │ +
111 ar& boost::serialization::make_nvp("R23", M(1, 2));
│ │ │ │ +
112 ar& boost::serialization::make_nvp("R31", M(2, 0));
│ │ │ │ +
113 ar& boost::serialization::make_nvp("R32", M(2, 1));
│ │ │ │ +
114 ar& boost::serialization::make_nvp("R33", M(2, 2));
│ │ │ │ +
115}
│ │ │ │ +
│ │ │ │ +
116
│ │ │ │ +
117namespace so3 {
│ │ │ │ +
118
│ │ │ │ +
123GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R,
│ │ │ │ +
124 OptionalJacobian<9, 9> H = boost::none);
│ │ │ │ +
125
│ │ │ │ +
127GTSAM_EXPORT Matrix99 Dcompose(const SO3& R);
│ │ │ │ +
128
│ │ │ │ +
129// Below are two functors that allow for saving computation when exponential map
│ │ │ │ +
130// and its derivatives are needed at the same location in so<3>. The second
│ │ │ │ +
131// functor also implements dedicated methods to apply dexp and/or inv(dexp).
│ │ │ │ +
132
│ │ │ │ +
│ │ │ │ +
134class GTSAM_EXPORT ExpmapFunctor {
│ │ │ │ +
135 protected:
│ │ │ │ +
136 const double theta2;
│ │ │ │ +
137 Matrix3 W, K, KK;
│ │ │ │ +
138 bool nearZero;
│ │ │ │ +
139 double theta, sin_theta, one_minus_cos; // only defined if !nearZero
│ │ │ │
140
│ │ │ │ -
142 bool equals(const PinholeBase &camera, double tol = 1e-9) const;
│ │ │ │ -
143
│ │ │ │ -
145 virtual void print(const std::string& s = "PinholeBase") const;
│ │ │ │ +
141 void init(bool nearZeroApprox = false);
│ │ │ │ +
142
│ │ │ │ +
143 public:
│ │ │ │ +
145 explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
│ │ │ │
146
│ │ │ │ -
150
│ │ │ │ -
│ │ │ │ -
152 const Pose3& pose() const {
│ │ │ │ -
153 return pose_;
│ │ │ │ -
154 }
│ │ │ │ -
│ │ │ │ -
155
│ │ │ │ -
│ │ │ │ -
157 const Rot3& rotation() const {
│ │ │ │ -
158 return pose_.rotation();
│ │ │ │ -
159 }
│ │ │ │ -
│ │ │ │ -
160
│ │ │ │ -
│ │ │ │ -
162 const Point3& translation() const {
│ │ │ │ -
163 return pose_.translation();
│ │ │ │ -
164 }
│ │ │ │ -
│ │ │ │ -
165
│ │ │ │ -
167 const Pose3& getPose(OptionalJacobian<6, 6> H) const;
│ │ │ │ -
168
│ │ │ │ -
172
│ │ │ │ -
178 static Point2 Project(const Point3& pc, //
│ │ │ │ -
179 OptionalJacobian<2, 3> Dpoint = boost::none);
│ │ │ │ -
180
│ │ │ │ -
186 static Point2 Project(const Unit3& pc, //
│ │ │ │ -
187 OptionalJacobian<2, 2> Dpoint = boost::none);
│ │ │ │ -
188
│ │ │ │ -
190 std::pair<Point2, bool> projectSafe(const Point3& pw) const;
│ │ │ │ -
191
│ │ │ │ -
197 Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose =
│ │ │ │ -
198 boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
│ │ │ │ -
199
│ │ │ │ -
205 Point2 project2(const Unit3& point,
│ │ │ │ -
206 OptionalJacobian<2, 6> Dpose = boost::none,
│ │ │ │ -
207 OptionalJacobian<2, 2> Dpoint = boost::none) const;
│ │ │ │ -
208
│ │ │ │ -
210 static Point3 BackprojectFromCamera(const Point2& p, const double depth,
│ │ │ │ -
211 OptionalJacobian<3, 2> Dpoint = boost::none,
│ │ │ │ -
212 OptionalJacobian<3, 1> Ddepth = boost::none);
│ │ │ │ -
213
│ │ │ │ -
217
│ │ │ │ -
│ │ │ │ -
223 inline static std::pair<size_t, size_t> translationInterval() {
│ │ │ │ -
224 return std::make_pair(3, 5);
│ │ │ │ -
225 }
│ │ │ │ -
│ │ │ │ -
226
│ │ │ │ -
228
│ │ │ │ -
229private:
│ │ │ │ -
230
│ │ │ │ -
232 friend class boost::serialization::access;
│ │ │ │ -
233 template<class Archive>
│ │ │ │ -
234 void serialize(Archive & ar, const unsigned int /*version*/) {
│ │ │ │ -
235 ar & BOOST_SERIALIZATION_NVP(pose_);
│ │ │ │ -
236 }
│ │ │ │ -
237};
│ │ │ │ -
│ │ │ │ -
238// end of class PinholeBase
│ │ │ │ -
239
│ │ │ │ -
│ │ │ │ -
247class GTSAM_EXPORT CalibratedCamera: public PinholeBase {
│ │ │ │ -
248
│ │ │ │ -
249public:
│ │ │ │ -
250
│ │ │ │ -
251 enum {
│ │ │ │ -
252 dimension = 6
│ │ │ │ -
253 };
│ │ │ │ -
254
│ │ │ │ -
257
│ │ │ │ -
│ │ │ │ -
259 CalibratedCamera() {
│ │ │ │ -
260 }
│ │ │ │ -
│ │ │ │ -
261
│ │ │ │ -
│ │ │ │ -
263 explicit CalibratedCamera(const Pose3& pose) :
│ │ │ │ -
264 PinholeBase(pose) {
│ │ │ │ -
265 }
│ │ │ │ -
│ │ │ │ -
266
│ │ │ │ -
270
│ │ │ │ -
271 // Create CalibratedCamera, with derivatives
│ │ │ │ -
272 static CalibratedCamera Create(const Pose3& pose,
│ │ │ │ -
273 OptionalJacobian<dimension, 6> H1 = boost::none) {
│ │ │ │ -
274 if (H1)
│ │ │ │ -
275 *H1 << I_6x6;
│ │ │ │ -
276 return CalibratedCamera(pose);
│ │ │ │ -
277 }
│ │ │ │ -
278
│ │ │ │ -
285 static CalibratedCamera Level(const Pose2& pose2, double height);
│ │ │ │ -
286
│ │ │ │ -
295 static CalibratedCamera Lookat(const Point3& eye, const Point3& target,
│ │ │ │ -
296 const Point3& upVector);
│ │ │ │ -
297
│ │ │ │ -
301
│ │ │ │ -
│ │ │ │ -
303 explicit CalibratedCamera(const Vector &v) :
│ │ │ │ -
304 PinholeBase(v) {
│ │ │ │ -
305 }
│ │ │ │ -
│ │ │ │ -
306
│ │ │ │ -
310
│ │ │ │ -
│ │ │ │ -
312 virtual ~CalibratedCamera() {
│ │ │ │ -
313 }
│ │ │ │ -
│ │ │ │ -
314
│ │ │ │ -
318
│ │ │ │ -
320 CalibratedCamera retract(const Vector& d) const;
│ │ │ │ -
321
│ │ │ │ -
323 Vector localCoordinates(const CalibratedCamera& T2) const;
│ │ │ │ -
324
│ │ │ │ -
│ │ │ │ -
326 void print(const std::string& s = "CalibratedCamera") const override {
│ │ │ │ -
327 PinholeBase::print(s);
│ │ │ │ -
328 }
│ │ │ │ -
│ │ │ │ -
329
│ │ │ │ -
│ │ │ │ -
331 inline size_t dim() const {
│ │ │ │ -
332 return dimension;
│ │ │ │ -
333 }
│ │ │ │ -
│ │ │ │ -
334
│ │ │ │ -
│ │ │ │ -
336 inline static size_t Dim() {
│ │ │ │ -
337 return dimension;
│ │ │ │ -
338 }
│ │ │ │ -
│ │ │ │ -
339
│ │ │ │ -
343
│ │ │ │ -
348 Point2 project(const Point3& point, OptionalJacobian<2, 6> Dcamera =
│ │ │ │ -
349 boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
│ │ │ │ -
350
│ │ │ │ -
│ │ │ │ -
352 Point3 backproject(const Point2& pn, double depth,
│ │ │ │ -
353 OptionalJacobian<3, 6> Dresult_dpose = boost::none,
│ │ │ │ -
354 OptionalJacobian<3, 2> Dresult_dp = boost::none,
│ │ │ │ -
355 OptionalJacobian<3, 1> Dresult_ddepth = boost::none) const {
│ │ │ │ -
356
│ │ │ │ -
357 Matrix32 Dpoint_dpn;
│ │ │ │ -
358 Matrix31 Dpoint_ddepth;
│ │ │ │ -
359 const Point3 point = BackprojectFromCamera(pn, depth,
│ │ │ │ -
360 Dresult_dp ? &Dpoint_dpn : 0,
│ │ │ │ -
361 Dresult_ddepth ? &Dpoint_ddepth : 0);
│ │ │ │ -
362
│ │ │ │ -
363 Matrix33 Dresult_dpoint;
│ │ │ │ -
364 const Point3 result = pose().transformFrom(point, Dresult_dpose,
│ │ │ │ -
365 (Dresult_ddepth ||
│ │ │ │ -
366 Dresult_dp) ? &Dresult_dpoint : 0);
│ │ │ │ -
367
│ │ │ │ -
368 if (Dresult_dp)
│ │ │ │ -
369 *Dresult_dp = Dresult_dpoint * Dpoint_dpn;
│ │ │ │ -
370 if (Dresult_ddepth)
│ │ │ │ -
371 *Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth;
│ │ │ │ -
372
│ │ │ │ -
373 return result;
│ │ │ │ -
374 }
│ │ │ │ -
│ │ │ │ -
375
│ │ │ │ -
│ │ │ │ -
381 double range(const Point3& point,
│ │ │ │ -
382 OptionalJacobian<1, 6> Dcamera = boost::none,
│ │ │ │ -
383 OptionalJacobian<1, 3> Dpoint = boost::none) const {
│ │ │ │ -
384 return pose().range(point, Dcamera, Dpoint);
│ │ │ │ -
385 }
│ │ │ │ -
│ │ │ │ -
386
│ │ │ │ -
│ │ │ │ -
392 double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = boost::none,
│ │ │ │ -
393 OptionalJacobian<1, 6> Dpose = boost::none) const {
│ │ │ │ -
394 return this->pose().range(pose, Dcamera, Dpose);
│ │ │ │ -
395 }
│ │ │ │ -
│ │ │ │ -
396
│ │ │ │ -
│ │ │ │ -
402 double range(const CalibratedCamera& camera, //
│ │ │ │ -
403 OptionalJacobian<1, 6> H1 = boost::none, //
│ │ │ │ -
404 OptionalJacobian<1, 6> H2 = boost::none) const {
│ │ │ │ -
405 return pose().range(camera.pose(), H1, H2);
│ │ │ │ -
406 }
│ │ │ │ -
│ │ │ │ -
407
│ │ │ │ -
409
│ │ │ │ -
410private:
│ │ │ │ -
411
│ │ │ │ -
414
│ │ │ │ -
416 friend class boost::serialization::access;
│ │ │ │ -
417 template<class Archive>
│ │ │ │ -
418 void serialize(Archive & ar, const unsigned int /*version*/) {
│ │ │ │ -
419 ar
│ │ │ │ -
420 & boost::serialization::make_nvp("PinholeBase",
│ │ │ │ -
421 boost::serialization::base_object<PinholeBase>(*this));
│ │ │ │ -
422 }
│ │ │ │ -
423
│ │ │ │ -
425};
│ │ │ │ -
│ │ │ │ -
426
│ │ │ │ -
427// manifold traits
│ │ │ │ -
428template <>
│ │ │ │ -
429struct traits<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
│ │ │ │ -
430
│ │ │ │ -
431template <>
│ │ │ │ -
432struct traits<const CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
│ │ │ │ -
433
│ │ │ │ -
434// range traits, used in RangeFactor
│ │ │ │ -
435template <typename T>
│ │ │ │ -
436struct Range<CalibratedCamera, T> : HasRange<CalibratedCamera, T, double> {};
│ │ │ │ -
437
│ │ │ │ -
438} // namespace gtsam
│ │ │ │ -
ThreadsafeException.h
Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
│ │ │ │ -
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ -
Pose3.h
3D Pose
│ │ │ │ -
BearingRange.h
Bearing-Range product.
│ │ │ │ -
Point2.h
2D Point
│ │ │ │ +
148 ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false);
│ │ │ │ +
149
│ │ │ │ +
151 SO3 expmap() const;
│ │ │ │ +
152};
│ │ │ │ +
│ │ │ │ +
153
│ │ │ │ +
│ │ │ │ +
155class DexpFunctor : public ExpmapFunctor {
│ │ │ │ +
156 const Vector3 omega;
│ │ │ │ +
157 double a, b;
│ │ │ │ +
158 Matrix3 dexp_;
│ │ │ │ +
159
│ │ │ │ +
160 public:
│ │ │ │ +
162 GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
│ │ │ │ +
163
│ │ │ │ +
164 // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
│ │ │ │ +
165 // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models,
│ │ │ │ +
166 // Information Theory, and Lie Groups", Volume 2, 2008.
│ │ │ │ +
167 // expmap(omega + v) \approx expmap(omega) * expmap(dexp * v)
│ │ │ │ +
168 // This maps a perturbation v in the tangent space to
│ │ │ │ +
169 // a perturbation on the manifold Expmap(dexp * v) */
│ │ │ │ +
170 const Matrix3& dexp() const { return dexp_; }
│ │ │ │ +
171
│ │ │ │ +
173 GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none,
│ │ │ │ +
174 OptionalJacobian<3, 3> H2 = boost::none) const;
│ │ │ │ +
175
│ │ │ │ +
177 GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v,
│ │ │ │ +
178 OptionalJacobian<3, 3> H1 = boost::none,
│ │ │ │ +
179 OptionalJacobian<3, 3> H2 = boost::none) const;
│ │ │ │ +
180};
│ │ │ │ +
│ │ │ │ +
181} // namespace so3
│ │ │ │ +
182
│ │ │ │ +
183/*
│ │ │ │ +
184 * Define the traits. internal::LieGroup provides both Lie group and Testable
│ │ │ │ +
185 */
│ │ │ │ +
186
│ │ │ │ +
187template <>
│ │ │ │ +
188struct traits<SO3> : public internal::LieGroup<SO3> {};
│ │ │ │ +
189
│ │ │ │ +
190template <>
│ │ │ │ +
191struct traits<const SO3> : public internal::LieGroup<SO3> {};
│ │ │ │ +
192
│ │ │ │ +
193} // end namespace gtsam
│ │ │ │ +
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ +
SOn.h
N*N matrix representation of SO(N).
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │ -
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ -
gtsam::project
Point2_ project(const Point3_ &p_cam)
Expression version of PinholeBase::Project.
Definition expressions.h:131
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition serialization.h:113
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
│ │ │ │ +
gtsam::LieGroup< SO< N >, internal::DimensionSO(N)>::Retract
static SO< N > Retract(const TangentVector &v)
Retract at origin: possible in Lie group because it has an identity.
Definition Lie.h:111
│ │ │ │ +
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition Lie.h:229
│ │ │ │
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::ThreadsafeException
Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
Definition ThreadsafeException.h:42
│ │ │ │ -
gtsam::Range
Definition BearingRange.h:40
│ │ │ │ -
gtsam::HasRange
Definition BearingRange.h:194
│ │ │ │ -
gtsam::CheiralityException
Definition CalibratedCamera.h:32
│ │ │ │ -
gtsam::PinholeBase
A pinhole camera class that has a Pose3, functions as base class for all pinhole cameras.
Definition CalibratedCamera.h:52
│ │ │ │ -
gtsam::PinholeBase::PinholeBase
PinholeBase()
Default constructor.
Definition CalibratedCamera.h:123
│ │ │ │ -
gtsam::PinholeBase::PinholeBase
PinholeBase(const Pose3 &pose)
Constructor with pose.
Definition CalibratedCamera.h:126
│ │ │ │ -
gtsam::PinholeBase::translation
const Point3 & translation() const
get translation
Definition CalibratedCamera.h:162
│ │ │ │ -
gtsam::PinholeBase::rotation
const Rot3 & rotation() const
get rotation
Definition CalibratedCamera.h:157
│ │ │ │ -
gtsam::PinholeBase::Measurement
Point2 Measurement
Some classes template on either PinholeCamera or StereoCamera, and this typedef informs those classes...
Definition CalibratedCamera.h:64
│ │ │ │ -
gtsam::PinholeBase::pose
const Pose3 & pose() const
return pose, constant version
Definition CalibratedCamera.h:152
│ │ │ │ -
gtsam::PinholeBase::translationInterval
static std::pair< size_t, size_t > translationInterval()
Return the start and end indices (inclusive) of the translation component of the exponential map para...
Definition CalibratedCamera.h:223
│ │ │ │ -
gtsam::PinholeBase::Rotation
Rot3 Rotation
Pose Concept requirements.
Definition CalibratedCamera.h:57
│ │ │ │ -
gtsam::PinholeBase::~PinholeBase
virtual ~PinholeBase()=default
Default destructor.
│ │ │ │ -
gtsam::CalibratedCamera
A Calibrated camera class [R|-R't], calibration K=I.
Definition CalibratedCamera.h:247
│ │ │ │ -
gtsam::CalibratedCamera::backproject
Point3 backproject(const Point2 &pn, double depth, OptionalJacobian< 3, 6 > Dresult_dpose=boost::none, OptionalJacobian< 3, 2 > Dresult_dp=boost::none, OptionalJacobian< 3, 1 > Dresult_ddepth=boost::none) const
backproject a 2-dimensional point to a 3-dimensional point at given depth
Definition CalibratedCamera.h:352
│ │ │ │ -
gtsam::CalibratedCamera::CalibratedCamera
CalibratedCamera()
default constructor
Definition CalibratedCamera.h:259
│ │ │ │ -
gtsam::CalibratedCamera::range
double range(const Pose3 &pose, OptionalJacobian< 1, 6 > Dcamera=boost::none, OptionalJacobian< 1, 6 > Dpose=boost::none) const
Calculate range to another pose.
Definition CalibratedCamera.h:392
│ │ │ │ -
gtsam::CalibratedCamera::dim
size_t dim() const
Definition CalibratedCamera.h:331
│ │ │ │ -
gtsam::CalibratedCamera::CalibratedCamera
CalibratedCamera(const Vector &v)
construct from vector
Definition CalibratedCamera.h:303
│ │ │ │ -
gtsam::CalibratedCamera::~CalibratedCamera
virtual ~CalibratedCamera()
destructor
Definition CalibratedCamera.h:312
│ │ │ │ -
gtsam::CalibratedCamera::Dim
static size_t Dim()
Definition CalibratedCamera.h:336
│ │ │ │ -
gtsam::CalibratedCamera::range
double range(const CalibratedCamera &camera, OptionalJacobian< 1, 6 > H1=boost::none, OptionalJacobian< 1, 6 > H2=boost::none) const
Calculate range to another camera.
Definition CalibratedCamera.h:402
│ │ │ │ -
gtsam::CalibratedCamera::print
void print(const std::string &s="CalibratedCamera") const override
print
Definition CalibratedCamera.h:326
│ │ │ │ -
gtsam::CalibratedCamera::range
double range(const Point3 &point, OptionalJacobian< 1, 6 > Dcamera=boost::none, OptionalJacobian< 1, 3 > Dpoint=boost::none) const
Calculate range to a landmark.
Definition CalibratedCamera.h:381
│ │ │ │ -
gtsam::CalibratedCamera::CalibratedCamera
CalibratedCamera(const Pose3 &pose)
construct with pose
Definition CalibratedCamera.h:263
│ │ │ │ -
gtsam::Pose2
A 2D pose (Point2,Rot2)
Definition Pose2.h:36
│ │ │ │ -
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ -
gtsam::Pose3::transformFrom
Point3 transformFrom(const Point3 &point, OptionalJacobian< 3, 6 > Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const
takes point in Pose coordinates and transforms it to world coordinates
Definition Pose3.cpp:347
│ │ │ │ -
gtsam::Pose3::range
double range(const Point3 &point, OptionalJacobian< 1, 6 > Hself=boost::none, OptionalJacobian< 1, 3 > Hpoint=boost::none) const
Calculate range to a landmark.
Definition Pose3.cpp:399
│ │ │ │ -
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get translation
Definition Pose3.cpp:308
│ │ │ │ -
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get rotation
Definition Pose3.cpp:315
│ │ │ │ -
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
│ │ │ │ -
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
│ │ │ │ +
gtsam::so3::ExpmapFunctor
Functor implementing Exponential map.
Definition SO3.h:134
│ │ │ │ +
gtsam::so3::DexpFunctor
Functor that implements Exponential map and its derivatives.
Definition SO3.h:155
│ │ │ │ +
gtsam::so3::DexpFunctor::applyDexp
GTSAM_EXPORT Vector3 applyDexp(const Vector3 &v, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
Multiplies with dexp(), with optional derivatives.
Definition SO3.cpp:101
│ │ │ │ +
gtsam::so3::DexpFunctor::applyInvDexp
GTSAM_EXPORT Vector3 applyInvDexp(const Vector3 &v, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
Multiplies with dexp().inverse(), with optional derivatives.
Definition SO3.cpp:120
│ │ │ │ + │ │ │ │ +
gtsam::SO< 3 >::Expmap
static SO Expmap(const TangentVector &omega, ChartJacobian H=boost::none)
Exponential map at identity - create a rotation from canonical coordinates.
Definition SOn-inl.h:67
│ │ │ │ +
gtsam::SO< 3 >::vec
VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost::none) const
Return vectorized rotation matrix in column order.
Definition SOn-inl.h:88
│ │ │ │ +
gtsam::SO< 3 >::ChordalMean
static SO ChordalMean(const std::vector< SO > &rotations)
Named constructor that finds chordal mean , currently only defined for SO3.
│ │ │ │ +
gtsam::SO< 3 >::Vee
static TangentVector Vee(const MatrixNN &X)
Inverse of Hat. See note about xi element order in Hat.
Definition SOn-inl.h:35
│ │ │ │ +
gtsam::SO< 3 >::Logmap
static TangentVector Logmap(const SO &R, ChartJacobian H=boost::none)
Log map at identity - returns the canonical coordinates of this rotation.
Definition SOn-inl.h:77
│ │ │ │ +
gtsam::SO< 3 >::AxisAngle
static SO AxisAngle(const Vector3 &axis, double theta)
Constructor from axis and angle. Only defined for SO3.
│ │ │ │ +
gtsam::SO< 3 >::AdjointMap
MatrixDD AdjointMap() const
Adjoint map.
Definition SO4.cpp:159
│ │ │ │ +
gtsam::SO< 3 >::Hat
static MatrixNN Hat(const TangentVector &xi)
Hat operator creates Lie algebra element corresponding to d-vector, where d is the dimensionality of ...
Definition SOn-inl.h:29
│ │ │ │ +
gtsam::SO< 3 >::ExpmapDerivative
static MatrixDD ExpmapDerivative(const TangentVector &omega)
Derivative of Expmap, currently only defined for SO3.
Definition SOn-inl.h:72
│ │ │ │ +
gtsam::SO< 3 >::LogmapDerivative
static MatrixDD LogmapDerivative(const TangentVector &omega)
Derivative of Logmap, currently only defined for SO3.
Definition SOn-inl.h:82
│ │ │ │ +
gtsam::SO< 3 >::ClosestTo
static SO ClosestTo(const MatrixNN &M)
Named constructor that finds SO(n) matrix closest to M in Frobenius norm, currently only defined for ...
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,460 +1,266 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -CalibratedCamera.h │ │ │ │ │ +SO3.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_B_e_a_r_i_n_g_R_a_n_g_e_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_3_._h> │ │ │ │ │ -24#include │ │ │ │ │ -25#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_b_a_s_e_/_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_._h> │ │ │ │ │ +21#pragma once │ │ │ │ │ +22 │ │ │ │ │ +23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_S_O_n_._h> │ │ │ │ │ +24 │ │ │ │ │ +25#include <_g_t_s_a_m_/_b_a_s_e_/_L_i_e_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ 27#include │ │ │ │ │ -28#include │ │ │ │ │ -29 │ │ │ │ │ -30namespace _g_t_s_a_m { │ │ │ │ │ +28 │ │ │ │ │ +29#include │ │ │ │ │ +30#include │ │ │ │ │ 31 │ │ │ │ │ -_3_2class GTSAM_EXPORT _C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n: public │ │ │ │ │ -_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n { │ │ │ │ │ -33public: │ │ │ │ │ -34 _C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n() │ │ │ │ │ -35 : _C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n(std::numeric_limits::max()) {} │ │ │ │ │ -36 │ │ │ │ │ -37 _C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n(_K_e_y j) │ │ │ │ │ -38 : _T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_<_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n_>("CheiralityException"), │ │ │ │ │ -39 j_(j) {} │ │ │ │ │ -40 │ │ │ │ │ -41 _K_e_y nearbyVariable() const {return j_;} │ │ │ │ │ +32namespace _g_t_s_a_m { │ │ │ │ │ +33 │ │ │ │ │ +34using SO3 = SO<3>; │ │ │ │ │ +35 │ │ │ │ │ +36// Below are all declarations of SO<3> specializations. │ │ │ │ │ +37// They are *defined* in SO3.cpp. │ │ │ │ │ +38 │ │ │ │ │ +39template <> │ │ │ │ │ +40GTSAM_EXPORT │ │ │ │ │ +41SO3 _S_O_3_:_:_A_x_i_s_A_n_g_l_e(const Vector3& axis, double theta); │ │ │ │ │ 42 │ │ │ │ │ -43private: │ │ │ │ │ -44 _K_e_y j_; │ │ │ │ │ -45}; │ │ │ │ │ +43template <> │ │ │ │ │ +44GTSAM_EXPORT │ │ │ │ │ +45SO3 _S_O_3_:_:_C_l_o_s_e_s_t_T_o(const Matrix3& M); │ │ │ │ │ 46 │ │ │ │ │ -_5_2class GTSAM_EXPORT _P_i_n_h_o_l_e_B_a_s_e { │ │ │ │ │ -53 │ │ │ │ │ -54public: │ │ │ │ │ -55 │ │ │ │ │ -_5_7 typedef _R_o_t_3 _R_o_t_a_t_i_o_n; │ │ │ │ │ -58 typedef _P_o_i_n_t_3 Translation; │ │ │ │ │ -59 │ │ │ │ │ -_6_4 typedef _P_o_i_n_t_2 _M_e_a_s_u_r_e_m_e_n_t; │ │ │ │ │ -65 typedef Point2Vector MeasurementVector; │ │ │ │ │ -66 │ │ │ │ │ -67private: │ │ │ │ │ -68 │ │ │ │ │ -69 _P_o_s_e_3 pose_; │ │ │ │ │ +47template <> │ │ │ │ │ +48GTSAM_EXPORT │ │ │ │ │ +49SO3 _S_O_3_:_:_C_h_o_r_d_a_l_M_e_a_n(const std::vector& rotations); │ │ │ │ │ +50 │ │ │ │ │ +51template <> │ │ │ │ │ +52GTSAM_EXPORT │ │ │ │ │ +53Matrix3 _S_O_3_:_:_H_a_t(const Vector3& xi); │ │ │ │ │ +54 │ │ │ │ │ +55template <> │ │ │ │ │ +56GTSAM_EXPORT │ │ │ │ │ +57Vector3 _S_O_3_:_:_V_e_e(const Matrix3& X); │ │ │ │ │ +58 │ │ │ │ │ +60template <> │ │ │ │ │ +61Matrix3 _S_O_3_:_:_A_d_j_o_i_n_t_M_a_p() const; │ │ │ │ │ +62 │ │ │ │ │ +67template <> │ │ │ │ │ +68GTSAM_EXPORT │ │ │ │ │ +69SO3 _S_O_3_:_:_E_x_p_m_a_p(const Vector3& omega, ChartJacobian H); │ │ │ │ │ 70 │ │ │ │ │ -71protected: │ │ │ │ │ -72 │ │ │ │ │ +72template <> │ │ │ │ │ +73GTSAM_EXPORT │ │ │ │ │ +74Matrix3 _S_O_3_:_:_E_x_p_m_a_p_D_e_r_i_v_a_t_i_v_e(const Vector3& omega); │ │ │ │ │ 75 │ │ │ │ │ -81 static Matrix26 Dpose(const _P_o_i_n_t_2& pn, double d); │ │ │ │ │ -82 │ │ │ │ │ -89 static Matrix23 Dpoint(const _P_o_i_n_t_2& pn, double d, const Matrix3& Rt); │ │ │ │ │ -90 │ │ │ │ │ -92 │ │ │ │ │ -93public: │ │ │ │ │ -94 │ │ │ │ │ +80template <> │ │ │ │ │ +81GTSAM_EXPORT │ │ │ │ │ +82Vector3 _S_O_3_:_:_L_o_g_m_a_p(const SO3& R, ChartJacobian H); │ │ │ │ │ +83 │ │ │ │ │ +85template <> │ │ │ │ │ +86GTSAM_EXPORT │ │ │ │ │ +87Matrix3 _S_O_3_:_:_L_o_g_m_a_p_D_e_r_i_v_a_t_i_v_e(const Vector3& omega); │ │ │ │ │ +88 │ │ │ │ │ +89// Chart at origin for SO3 is *not* Cayley but actual Expmap/Logmap │ │ │ │ │ +90template <> │ │ │ │ │ +91GTSAM_EXPORT │ │ │ │ │ +92SO3 _S_O_3_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n_:_:_R_e_t_r_a_c_t(const Vector3& omega, ChartJacobian H); │ │ │ │ │ +93 │ │ │ │ │ +94template <> │ │ │ │ │ +95GTSAM_EXPORT │ │ │ │ │ +96Vector3 SO3::ChartAtOrigin::Local(const SO3& R, ChartJacobian H); │ │ │ │ │ 97 │ │ │ │ │ -105 static _P_o_s_e_3 LevelPose(const _P_o_s_e_2& pose2, double height); │ │ │ │ │ -106 │ │ │ │ │ -115 static _P_o_s_e_3 LookatPose(const _P_o_i_n_t_3& eye, const _P_o_i_n_t_3& target, │ │ │ │ │ -116 const _P_o_i_n_t_3& upVector); │ │ │ │ │ -117 │ │ │ │ │ -121 │ │ │ │ │ -_1_2_3 _P_i_n_h_o_l_e_B_a_s_e() {} │ │ │ │ │ -124 │ │ │ │ │ -_1_2_6 explicit _P_i_n_h_o_l_e_B_a_s_e(const _P_o_s_e_3& pose) : pose_(pose) {} │ │ │ │ │ -127 │ │ │ │ │ -131 │ │ │ │ │ -132 explicit _P_i_n_h_o_l_e_B_a_s_e(const Vector& v) : pose_(_P_o_s_e_3::Expmap(v)) {} │ │ │ │ │ -133 │ │ │ │ │ -_1_3_5 virtual _~_P_i_n_h_o_l_e_B_a_s_e() = default; │ │ │ │ │ -136 │ │ │ │ │ +98template <> │ │ │ │ │ +99GTSAM_EXPORT │ │ │ │ │ +100Vector9 _S_O_3_:_:_v_e_c(OptionalJacobian<9, 3> H) const; │ │ │ │ │ +101 │ │ │ │ │ +103template │ │ │ │ │ +_1_0_4void _s_e_r_i_a_l_i_z_e(Archive& ar, _S_O_3& R, const unsigned int /*version*/) { │ │ │ │ │ +105 Matrix3& M = R.matrix_; │ │ │ │ │ +106 ar& boost::serialization::make_nvp("R11", M(0, 0)); │ │ │ │ │ +107 ar& boost::serialization::make_nvp("R12", M(0, 1)); │ │ │ │ │ +108 ar& boost::serialization::make_nvp("R13", M(0, 2)); │ │ │ │ │ +109 ar& boost::serialization::make_nvp("R21", M(1, 0)); │ │ │ │ │ +110 ar& boost::serialization::make_nvp("R22", M(1, 1)); │ │ │ │ │ +111 ar& boost::serialization::make_nvp("R23", M(1, 2)); │ │ │ │ │ +112 ar& boost::serialization::make_nvp("R31", M(2, 0)); │ │ │ │ │ +113 ar& boost::serialization::make_nvp("R32", M(2, 1)); │ │ │ │ │ +114 ar& boost::serialization::make_nvp("R33", M(2, 2)); │ │ │ │ │ +115} │ │ │ │ │ +116 │ │ │ │ │ +117namespace so3 { │ │ │ │ │ +118 │ │ │ │ │ +123GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, │ │ │ │ │ +124 OptionalJacobian<9, 9> H = boost::none); │ │ │ │ │ +125 │ │ │ │ │ +127GTSAM_EXPORT Matrix99 Dcompose(const SO3& R); │ │ │ │ │ +128 │ │ │ │ │ +129// Below are two functors that allow for saving computation when exponential │ │ │ │ │ +map │ │ │ │ │ +130// and its derivatives are needed at the same location in so<3>. The second │ │ │ │ │ +131// functor also implements dedicated methods to apply dexp and/or inv(dexp). │ │ │ │ │ +132 │ │ │ │ │ +_1_3_4class GTSAM_EXPORT _E_x_p_m_a_p_F_u_n_c_t_o_r { │ │ │ │ │ +135 protected: │ │ │ │ │ +136 const double theta2; │ │ │ │ │ +137 Matrix3 W, K, KK; │ │ │ │ │ +138 bool nearZero; │ │ │ │ │ +139 double theta, sin_theta, one_minus_cos; // only defined if !nearZero │ │ │ │ │ 140 │ │ │ │ │ -142 bool _e_q_u_a_l_s(const _P_i_n_h_o_l_e_B_a_s_e &camera, double tol = 1e-9) const; │ │ │ │ │ -143 │ │ │ │ │ -145 virtual void _p_r_i_n_t(const std::string& s = "PinholeBase") const; │ │ │ │ │ +141 void init(bool nearZeroApprox = false); │ │ │ │ │ +142 │ │ │ │ │ +143 public: │ │ │ │ │ +145 explicit _E_x_p_m_a_p_F_u_n_c_t_o_r(const Vector3& omega, bool nearZeroApprox = false); │ │ │ │ │ 146 │ │ │ │ │ -150 │ │ │ │ │ -_1_5_2 const _P_o_s_e_3& _p_o_s_e() const { │ │ │ │ │ -153 return pose_; │ │ │ │ │ -154 } │ │ │ │ │ -155 │ │ │ │ │ -_1_5_7 const _R_o_t_3& _r_o_t_a_t_i_o_n() const { │ │ │ │ │ -158 return pose_._r_o_t_a_t_i_o_n(); │ │ │ │ │ -159 } │ │ │ │ │ -160 │ │ │ │ │ -_1_6_2 const _P_o_i_n_t_3& _t_r_a_n_s_l_a_t_i_o_n() const { │ │ │ │ │ -163 return pose_._t_r_a_n_s_l_a_t_i_o_n(); │ │ │ │ │ -164 } │ │ │ │ │ -165 │ │ │ │ │ -167 const _P_o_s_e_3& getPose(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> H) const; │ │ │ │ │ -168 │ │ │ │ │ -172 │ │ │ │ │ -178 static _P_o_i_n_t_2 Project(const _P_o_i_n_t_3& pc, // │ │ │ │ │ -179 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> Dpoint = boost::none); │ │ │ │ │ -180 │ │ │ │ │ -186 static _P_o_i_n_t_2 Project(const _U_n_i_t_3& pc, // │ │ │ │ │ -187 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dpoint = boost::none); │ │ │ │ │ -188 │ │ │ │ │ -190 std::pair projectSafe(const _P_o_i_n_t_3& pw) const; │ │ │ │ │ -191 │ │ │ │ │ -197 _P_o_i_n_t_2 project2(const _P_o_i_n_t_3& point, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> Dpose = │ │ │ │ │ -198 boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> Dpoint = boost::none) const; │ │ │ │ │ -199 │ │ │ │ │ -205 _P_o_i_n_t_2 project2(const _U_n_i_t_3& point, │ │ │ │ │ -206 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> Dpose = boost::none, │ │ │ │ │ -207 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dpoint = boost::none) const; │ │ │ │ │ -208 │ │ │ │ │ -210 static _P_o_i_n_t_3 BackprojectFromCamera(const _P_o_i_n_t_2& p, const double depth, │ │ │ │ │ -211 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _2_> Dpoint = boost::none, │ │ │ │ │ -212 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _1_> Ddepth = boost::none); │ │ │ │ │ -213 │ │ │ │ │ -217 │ │ │ │ │ -_2_2_3 inline static std::pair _t_r_a_n_s_l_a_t_i_o_n_I_n_t_e_r_v_a_l() { │ │ │ │ │ -224 return std::make_pair(3, 5); │ │ │ │ │ -225 } │ │ │ │ │ -226 │ │ │ │ │ -228 │ │ │ │ │ -229private: │ │ │ │ │ -230 │ │ │ │ │ -_2_3_2 friend class boost::serialization::access; │ │ │ │ │ -233 template │ │ │ │ │ -234 void serialize(Archive & ar, const unsigned int /*version*/) { │ │ │ │ │ -235 ar & BOOST_SERIALIZATION_NVP(pose_); │ │ │ │ │ -236 } │ │ │ │ │ -237}; │ │ │ │ │ -238// end of class PinholeBase │ │ │ │ │ -239 │ │ │ │ │ -_2_4_7class GTSAM_EXPORT _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a: public _P_i_n_h_o_l_e_B_a_s_e { │ │ │ │ │ -248 │ │ │ │ │ -249public: │ │ │ │ │ -250 │ │ │ │ │ -251 enum { │ │ │ │ │ -252 dimension = 6 │ │ │ │ │ -253 }; │ │ │ │ │ -254 │ │ │ │ │ -257 │ │ │ │ │ -_2_5_9 _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a() { │ │ │ │ │ -260 } │ │ │ │ │ -261 │ │ │ │ │ -_2_6_3 explicit _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a(const _P_o_s_e_3& pose) : │ │ │ │ │ -264 _P_i_n_h_o_l_e_B_a_s_e(pose) { │ │ │ │ │ -265 } │ │ │ │ │ -266 │ │ │ │ │ -270 │ │ │ │ │ -271 // Create CalibratedCamera, with derivatives │ │ │ │ │ -272 static _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a Create(const _P_o_s_e_3& pose, │ │ │ │ │ -273 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_d_i_m_e_n_s_i_o_n_,_ _6_> H1 = boost::none) { │ │ │ │ │ -274 if (H1) │ │ │ │ │ -275 *H1 << I_6x6; │ │ │ │ │ -276 return _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a(pose); │ │ │ │ │ -277 } │ │ │ │ │ -278 │ │ │ │ │ -285 static CalibratedCamera Level(const Pose2& pose2, double height); │ │ │ │ │ -286 │ │ │ │ │ -295 static CalibratedCamera Lookat(const Point3& eye, const Point3& target, │ │ │ │ │ -296 const Point3& upVector); │ │ │ │ │ -297 │ │ │ │ │ -301 │ │ │ │ │ -_3_0_3 explicit _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a(const Vector &v) : │ │ │ │ │ -304 _P_i_n_h_o_l_e_B_a_s_e(v) { │ │ │ │ │ -305 } │ │ │ │ │ -306 │ │ │ │ │ -310 │ │ │ │ │ -_3_1_2 virtual _~_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a() { │ │ │ │ │ -313 } │ │ │ │ │ -314 │ │ │ │ │ -318 │ │ │ │ │ -320 _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a retract(const Vector& d) const; │ │ │ │ │ -321 │ │ │ │ │ -323 Vector localCoordinates(const _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a& T2) const; │ │ │ │ │ -324 │ │ │ │ │ -_3_2_6 void _p_r_i_n_t(const std::string& s = "CalibratedCamera") const override { │ │ │ │ │ -327 PinholeBase::print(s); │ │ │ │ │ -328 } │ │ │ │ │ -329 │ │ │ │ │ -_3_3_1 inline size_t _d_i_m() const { │ │ │ │ │ -332 return dimension; │ │ │ │ │ -333 } │ │ │ │ │ -334 │ │ │ │ │ -_3_3_6 inline static size_t _D_i_m() { │ │ │ │ │ -337 return dimension; │ │ │ │ │ -338 } │ │ │ │ │ -339 │ │ │ │ │ -343 │ │ │ │ │ -348 _P_o_i_n_t_2 _p_r_o_j_e_c_t(const _P_o_i_n_t_3& point, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> Dcamera = │ │ │ │ │ -349 boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> Dpoint = boost::none) const; │ │ │ │ │ -350 │ │ │ │ │ -_3_5_2 _P_o_i_n_t_3 _b_a_c_k_p_r_o_j_e_c_t(const _P_o_i_n_t_2& pn, double depth, │ │ │ │ │ -353 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _6_> Dresult_dpose = boost::none, │ │ │ │ │ -354 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _2_> Dresult_dp = boost::none, │ │ │ │ │ -355 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _1_> Dresult_ddepth = boost::none) const { │ │ │ │ │ -356 │ │ │ │ │ -357 Matrix32 Dpoint_dpn; │ │ │ │ │ -358 Matrix31 Dpoint_ddepth; │ │ │ │ │ -359 const _P_o_i_n_t_3 point = BackprojectFromCamera(pn, depth, │ │ │ │ │ -360 Dresult_dp ? &Dpoint_dpn : 0, │ │ │ │ │ -361 Dresult_ddepth ? &Dpoint_ddepth : 0); │ │ │ │ │ -362 │ │ │ │ │ -363 Matrix33 Dresult_dpoint; │ │ │ │ │ -364 const _P_o_i_n_t_3 result = pose()._t_r_a_n_s_f_o_r_m_F_r_o_m(point, Dresult_dpose, │ │ │ │ │ -365 (Dresult_ddepth || │ │ │ │ │ -366 Dresult_dp) ? &Dresult_dpoint : 0); │ │ │ │ │ -367 │ │ │ │ │ -368 if (Dresult_dp) │ │ │ │ │ -369 *Dresult_dp = Dresult_dpoint * Dpoint_dpn; │ │ │ │ │ -370 if (Dresult_ddepth) │ │ │ │ │ -371 *Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth; │ │ │ │ │ -372 │ │ │ │ │ -373 return result; │ │ │ │ │ -374 } │ │ │ │ │ -375 │ │ │ │ │ -_3_8_1 double _r_a_n_g_e(const _P_o_i_n_t_3& point, │ │ │ │ │ -382 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> Dcamera = boost::none, │ │ │ │ │ -383 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> Dpoint = boost::none) const { │ │ │ │ │ -384 return pose()._r_a_n_g_e(point, Dcamera, Dpoint); │ │ │ │ │ -385 } │ │ │ │ │ -386 │ │ │ │ │ -_3_9_2 double _r_a_n_g_e(const _P_o_s_e_3& pose, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> Dcamera = boost:: │ │ │ │ │ -none, │ │ │ │ │ -393 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> Dpose = boost::none) const { │ │ │ │ │ -394 return this->pose()._r_a_n_g_e(pose, Dcamera, Dpose); │ │ │ │ │ -395 } │ │ │ │ │ -396 │ │ │ │ │ -_4_0_2 double _r_a_n_g_e(const _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a& camera, // │ │ │ │ │ -403 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> H1 = boost::none, // │ │ │ │ │ -404 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> H2 = boost::none) const { │ │ │ │ │ -405 return pose()._r_a_n_g_e(camera._p_o_s_e(), H1, H2); │ │ │ │ │ -406 } │ │ │ │ │ -407 │ │ │ │ │ -409 │ │ │ │ │ -410private: │ │ │ │ │ -411 │ │ │ │ │ -414 │ │ │ │ │ -_4_1_6 friend class boost::serialization::access; │ │ │ │ │ -417 template │ │ │ │ │ -418 void serialize(Archive & ar, const unsigned int /*version*/) { │ │ │ │ │ -419 ar │ │ │ │ │ -420 & boost::serialization::make_nvp("PinholeBase", │ │ │ │ │ -421 boost::serialization::base_object(*this)); │ │ │ │ │ -422 } │ │ │ │ │ -423 │ │ │ │ │ -425}; │ │ │ │ │ -426 │ │ │ │ │ -427// manifold traits │ │ │ │ │ -428template <> │ │ │ │ │ -_4_2_9struct _t_r_a_i_t_s<_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a> : public _i_n_t_e_r_n_a_l_:_: │ │ │ │ │ -_M_a_n_i_f_o_l_d {}; │ │ │ │ │ -430 │ │ │ │ │ -431template <> │ │ │ │ │ -_4_3_2struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_: │ │ │ │ │ -_M_a_n_i_f_o_l_d {}; │ │ │ │ │ -433 │ │ │ │ │ -434// range traits, used in RangeFactor │ │ │ │ │ -435template │ │ │ │ │ -_4_3_6struct _R_a_n_g_e<_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a, T> : _H_a_s_R_a_n_g_e │ │ │ │ │ -{}; │ │ │ │ │ -437 │ │ │ │ │ -438} // namespace gtsam │ │ │ │ │ -_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_._h │ │ │ │ │ -Base exception type that uses tbb_allocator if GTSAM is compiled with TBB. │ │ │ │ │ -_M_a_n_i_f_o_l_d_._h │ │ │ │ │ -Base class and basic functions for Manifold types. │ │ │ │ │ -_P_o_s_e_3_._h │ │ │ │ │ -3D Pose │ │ │ │ │ -_B_e_a_r_i_n_g_R_a_n_g_e_._h │ │ │ │ │ -Bearing-Range product. │ │ │ │ │ -_P_o_i_n_t_2_._h │ │ │ │ │ -2D Point │ │ │ │ │ +148 _E_x_p_m_a_p_F_u_n_c_t_o_r(const Vector3& axis, double angle, bool nearZeroApprox = │ │ │ │ │ +false); │ │ │ │ │ +149 │ │ │ │ │ +151 _S_O_3 expmap() const; │ │ │ │ │ +152}; │ │ │ │ │ +153 │ │ │ │ │ +_1_5_5class _D_e_x_p_F_u_n_c_t_o_r : public _E_x_p_m_a_p_F_u_n_c_t_o_r { │ │ │ │ │ +156 const Vector3 omega; │ │ │ │ │ +157 double a, b; │ │ │ │ │ +158 Matrix3 dexp_; │ │ │ │ │ +159 │ │ │ │ │ +160 public: │ │ │ │ │ +162 GTSAM_EXPORT explicit _D_e_x_p_F_u_n_c_t_o_r(const Vector3& omega, bool nearZeroApprox │ │ │ │ │ += false); │ │ │ │ │ +163 │ │ │ │ │ +164 // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation │ │ │ │ │ +165 // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models, │ │ │ │ │ +166 // Information Theory, and Lie Groups", Volume 2, 2008. │ │ │ │ │ +167 // expmap(omega + v) \approx expmap(omega) * expmap(dexp * v) │ │ │ │ │ +168 // This maps a perturbation v in the tangent space to │ │ │ │ │ +169 // a perturbation on the manifold Expmap(dexp * v) */ │ │ │ │ │ +170 const Matrix3& dexp() const { return dexp_; } │ │ │ │ │ +171 │ │ │ │ │ +173 GTSAM_EXPORT Vector3 _a_p_p_l_y_D_e_x_p(const Vector3& v, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H1 │ │ │ │ │ += boost::none, │ │ │ │ │ +174 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H2 = boost::none) const; │ │ │ │ │ +175 │ │ │ │ │ +177 GTSAM_EXPORT Vector3 _a_p_p_l_y_I_n_v_D_e_x_p(const Vector3& v, │ │ │ │ │ +178 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H1 = boost::none, │ │ │ │ │ +179 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H2 = boost::none) const; │ │ │ │ │ +180}; │ │ │ │ │ +181} // namespace so3 │ │ │ │ │ +182 │ │ │ │ │ +183/* │ │ │ │ │ +184 * Define the traits. internal::LieGroup provides both Lie group and │ │ │ │ │ +Testable │ │ │ │ │ +185 */ │ │ │ │ │ +186 │ │ │ │ │ +187template <> │ │ │ │ │ +_1_8_8struct _t_r_a_i_t_s<_S_O_3> : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ +189 │ │ │ │ │ +190template <> │ │ │ │ │ +_1_9_1struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ +192 │ │ │ │ │ +193} // end namespace gtsam │ │ │ │ │ +_M_a_t_r_i_x_._h │ │ │ │ │ +typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +_L_i_e_._h │ │ │ │ │ +Base class and basic functions for Lie types. │ │ │ │ │ +_S_O_n_._h │ │ │ │ │ +N*N matrix representation of SO(N). │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ -Vector2 Point2 │ │ │ │ │ -As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point2 to Vector2... │ │ │ │ │ -DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ -Vector3 Point3 │ │ │ │ │ -As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point3 to Vector3... │ │ │ │ │ -DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_o_j_e_c_t │ │ │ │ │ -Point2_ project(const Point3_ &p_cam) │ │ │ │ │ -Expression version of PinholeBase::Project. │ │ │ │ │ -DDeeffiinniittiioonn expressions.h:131 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e │ │ │ │ │ +std::string serialize(const T &input) │ │ │ │ │ +serializes to a string │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:113 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ -Both ManifoldTraits and Testable. │ │ │ │ │ -DDeeffiinniittiioonn Manifold.h:120 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_<_ _S_O_<_ _N_ _>_,_ _i_n_t_e_r_n_a_l_:_:_D_i_m_e_n_s_i_o_n_S_O_(_N_)_>_:_:_R_e_t_r_a_c_t │ │ │ │ │ +static SO< N > Retract(const TangentVector &v) │ │ │ │ │ +Retract at origin: possible in Lie group because it has an identity. │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ +Both LieGroupTraits and Testable. │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:229 │ │ │ │ │ _g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ either a fixed size o... │ │ │ │ │ DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n │ │ │ │ │ -Base exception type that uses tbb_allocator if GTSAM is compiled with TBB. │ │ │ │ │ -DDeeffiinniittiioonn ThreadsafeException.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_R_a_n_g_e │ │ │ │ │ -DDeeffiinniittiioonn BearingRange.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_H_a_s_R_a_n_g_e │ │ │ │ │ -DDeeffiinniittiioonn BearingRange.h:194 │ │ │ │ │ -_g_t_s_a_m_:_:_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e │ │ │ │ │ -A pinhole camera class that has a Pose3, functions as base class for all │ │ │ │ │ -pinhole cameras. │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_P_i_n_h_o_l_e_B_a_s_e │ │ │ │ │ -PinholeBase() │ │ │ │ │ -Default constructor. │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:123 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_P_i_n_h_o_l_e_B_a_s_e │ │ │ │ │ -PinholeBase(const Pose3 &pose) │ │ │ │ │ -Constructor with pose. │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:126 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_t_r_a_n_s_l_a_t_i_o_n │ │ │ │ │ -const Point3 & translation() const │ │ │ │ │ -get translation │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:162 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_r_o_t_a_t_i_o_n │ │ │ │ │ -const Rot3 & rotation() const │ │ │ │ │ -get rotation │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:157 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_M_e_a_s_u_r_e_m_e_n_t │ │ │ │ │ -Point2 Measurement │ │ │ │ │ -Some classes template on either PinholeCamera or StereoCamera, and this typedef │ │ │ │ │ -informs those classes... │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_p_o_s_e │ │ │ │ │ -const Pose3 & pose() const │ │ │ │ │ -return pose, constant version │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:152 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_t_r_a_n_s_l_a_t_i_o_n_I_n_t_e_r_v_a_l │ │ │ │ │ -static std::pair< size_t, size_t > translationInterval() │ │ │ │ │ -Return the start and end indices (inclusive) of the translation component of │ │ │ │ │ -the exponential map para... │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:223 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_R_o_t_a_t_i_o_n │ │ │ │ │ -Rot3 Rotation │ │ │ │ │ -Pose Concept requirements. │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_~_P_i_n_h_o_l_e_B_a_s_e │ │ │ │ │ -virtual ~PinholeBase()=default │ │ │ │ │ -Default destructor. │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a │ │ │ │ │ -A Calibrated camera class [R|-R't], calibration K=I. │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:247 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_:_:_b_a_c_k_p_r_o_j_e_c_t │ │ │ │ │ -Point3 backproject(const Point2 &pn, double depth, OptionalJacobian< 3, 6 > │ │ │ │ │ -Dresult_dpose=boost::none, OptionalJacobian< 3, 2 > Dresult_dp=boost::none, │ │ │ │ │ -OptionalJacobian< 3, 1 > Dresult_ddepth=boost::none) const │ │ │ │ │ -backproject a 2-dimensional point to a 3-dimensional point at given depth │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:352 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a │ │ │ │ │ -CalibratedCamera() │ │ │ │ │ -default constructor │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:259 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_:_:_r_a_n_g_e │ │ │ │ │ -double range(const Pose3 &pose, OptionalJacobian< 1, 6 > Dcamera=boost::none, │ │ │ │ │ -OptionalJacobian< 1, 6 > Dpose=boost::none) const │ │ │ │ │ -Calculate range to another pose. │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:392 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_:_:_d_i_m │ │ │ │ │ -size_t dim() const │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:331 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a │ │ │ │ │ -CalibratedCamera(const Vector &v) │ │ │ │ │ -construct from vector │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:303 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_:_:_~_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a │ │ │ │ │ -virtual ~CalibratedCamera() │ │ │ │ │ -destructor │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:312 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_:_:_D_i_m │ │ │ │ │ -static size_t Dim() │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:336 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_:_:_r_a_n_g_e │ │ │ │ │ -double range(const CalibratedCamera &camera, OptionalJacobian< 1, 6 > │ │ │ │ │ -H1=boost::none, OptionalJacobian< 1, 6 > H2=boost::none) const │ │ │ │ │ -Calculate range to another camera. │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:402 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="CalibratedCamera") const override │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:326 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_:_:_r_a_n_g_e │ │ │ │ │ -double range(const Point3 &point, OptionalJacobian< 1, 6 > Dcamera=boost::none, │ │ │ │ │ -OptionalJacobian< 1, 3 > Dpoint=boost::none) const │ │ │ │ │ -Calculate range to a landmark. │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:381 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a │ │ │ │ │ -CalibratedCamera(const Pose3 &pose) │ │ │ │ │ -construct with pose │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:263 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2 │ │ │ │ │ -A 2D pose (Point2,Rot2) │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ -A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_t_r_a_n_s_f_o_r_m_F_r_o_m │ │ │ │ │ -Point3 transformFrom(const Point3 &point, OptionalJacobian< 3, 6 > │ │ │ │ │ -Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const │ │ │ │ │ -takes point in Pose coordinates and transforms it to world coordinates │ │ │ │ │ -DDeeffiinniittiioonn Pose3.cpp:347 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_r_a_n_g_e │ │ │ │ │ -double range(const Point3 &point, OptionalJacobian< 1, 6 > Hself=boost::none, │ │ │ │ │ -OptionalJacobian< 1, 3 > Hpoint=boost::none) const │ │ │ │ │ -Calculate range to a landmark. │ │ │ │ │ -DDeeffiinniittiioonn Pose3.cpp:399 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_t_r_a_n_s_l_a_t_i_o_n │ │ │ │ │ -const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const │ │ │ │ │ -get translation │ │ │ │ │ -DDeeffiinniittiioonn Pose3.cpp:308 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_r_o_t_a_t_i_o_n │ │ │ │ │ -const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const │ │ │ │ │ -get rotation │ │ │ │ │ -DDeeffiinniittiioonn Pose3.cpp:315 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ -Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor │ │ │ │ │ -symbol GTSAM_USE_QUATERNIO... │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_U_n_i_t_3 │ │ │ │ │ -Represents a 3D point on a unit sphere. │ │ │ │ │ -DDeeffiinniittiioonn Unit3.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_s_o_3_:_:_E_x_p_m_a_p_F_u_n_c_t_o_r │ │ │ │ │ +Functor implementing Exponential map. │ │ │ │ │ +DDeeffiinniittiioonn SO3.h:134 │ │ │ │ │ +_g_t_s_a_m_:_:_s_o_3_:_:_D_e_x_p_F_u_n_c_t_o_r │ │ │ │ │ +Functor that implements Exponential map and its derivatives. │ │ │ │ │ +DDeeffiinniittiioonn SO3.h:155 │ │ │ │ │ +_g_t_s_a_m_:_:_s_o_3_:_:_D_e_x_p_F_u_n_c_t_o_r_:_:_a_p_p_l_y_D_e_x_p │ │ │ │ │ +GTSAM_EXPORT Vector3 applyDexp(const Vector3 &v, OptionalJacobian< 3, 3 > │ │ │ │ │ +H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const │ │ │ │ │ +Multiplies with dexp(), with optional derivatives. │ │ │ │ │ +DDeeffiinniittiioonn SO3.cpp:101 │ │ │ │ │ +_g_t_s_a_m_:_:_s_o_3_:_:_D_e_x_p_F_u_n_c_t_o_r_:_:_a_p_p_l_y_I_n_v_D_e_x_p │ │ │ │ │ +GTSAM_EXPORT Vector3 applyInvDexp(const Vector3 &v, OptionalJacobian< 3, 3 > │ │ │ │ │ +H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const │ │ │ │ │ +Multiplies with dexp().inverse(), with optional derivatives. │ │ │ │ │ +DDeeffiinniittiioonn SO3.cpp:120 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_<_ _3_ _> │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_<_ _3_ _>_:_:_E_x_p_m_a_p │ │ │ │ │ +static SO Expmap(const TangentVector &omega, ChartJacobian H=boost::none) │ │ │ │ │ +Exponential map at identity - create a rotation from canonical coordinates. │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_<_ _3_ _>_:_:_v_e_c │ │ │ │ │ +VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost:: │ │ │ │ │ +none) const │ │ │ │ │ +Return vectorized rotation matrix in column order. │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:88 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_<_ _3_ _>_:_:_C_h_o_r_d_a_l_M_e_a_n │ │ │ │ │ +static SO ChordalMean(const std::vector< SO > &rotations) │ │ │ │ │ +Named constructor that finds chordal mean , currently only defined for SO3. │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_<_ _3_ _>_:_:_V_e_e │ │ │ │ │ +static TangentVector Vee(const MatrixNN &X) │ │ │ │ │ +Inverse of Hat. See note about xi element order in Hat. │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_<_ _3_ _>_:_:_L_o_g_m_a_p │ │ │ │ │ +static TangentVector Logmap(const SO &R, ChartJacobian H=boost::none) │ │ │ │ │ +Log map at identity - returns the canonical coordinates of this rotation. │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_<_ _3_ _>_:_:_A_x_i_s_A_n_g_l_e │ │ │ │ │ +static SO AxisAngle(const Vector3 &axis, double theta) │ │ │ │ │ +Constructor from axis and angle. Only defined for SO3. │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_<_ _3_ _>_:_:_A_d_j_o_i_n_t_M_a_p │ │ │ │ │ +MatrixDD AdjointMap() const │ │ │ │ │ +Adjoint map. │ │ │ │ │ +DDeeffiinniittiioonn SO4.cpp:159 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_<_ _3_ _>_:_:_H_a_t │ │ │ │ │ +static MatrixNN Hat(const TangentVector &xi) │ │ │ │ │ +Hat operator creates Lie algebra element corresponding to d-vector, where d is │ │ │ │ │ +the dimensionality of ... │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:29 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_<_ _3_ _>_:_:_E_x_p_m_a_p_D_e_r_i_v_a_t_i_v_e │ │ │ │ │ +static MatrixDD ExpmapDerivative(const TangentVector &omega) │ │ │ │ │ +Derivative of Expmap, currently only defined for SO3. │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_<_ _3_ _>_:_:_L_o_g_m_a_p_D_e_r_i_v_a_t_i_v_e │ │ │ │ │ +static MatrixDD LogmapDerivative(const TangentVector &omega) │ │ │ │ │ +Derivative of Logmap, currently only defined for SO3. │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:82 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_<_ _3_ _>_:_:_C_l_o_s_e_s_t_T_o │ │ │ │ │ +static SO ClosestTo(const MatrixNN &M) │ │ │ │ │ +Named constructor that finds SO(n) matrix closest to M in Frobenius norm, │ │ │ │ │ +currently only defined for ... │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_._h │ │ │ │ │ + * _S_O_3_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00449.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Point2.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Quaternion.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,90 +96,55 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
│ │ │ │ -
Point2.h File Reference
│ │ │ │ +Macros | │ │ │ │ +Typedefs
│ │ │ │ +
Quaternion.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

2D Point │ │ │ │ +

Lie Group wrapper for Eigen Quaternions. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

struct  gtsam::Range< Point2, Point2 >
struct  gtsam::traits< QUATERNION_TYPE >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Macros

│ │ │ │ +#define QUATERNION_TYPE   Eigen::Quaternion<_Scalar,_Options>
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ Typedefs

│ │ │ │ -typedef Vector2 gtsam::Point2
 As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2.
 
│ │ │ │ -using gtsam::Point2Pair = std::pair< Point2, Point2 >
 
│ │ │ │ -using gtsam::Point2Pairs = std::vector< Point2Pair >
 
│ │ │ │ -typedef std::vector< Point2, Eigen::aligned_allocator< Point2 > > gtsam::Point2Vector
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -ostream & gtsam::operator<< (ostream &os, const gtsam::Point2Pair &p)
 
│ │ │ │ -double gtsam::norm2 (const Point2 &p, OptionalJacobian< 1, 2 > H=boost::none)
 Distance of the point from the origin, with Jacobian.
 
│ │ │ │ -double gtsam::distance2 (const Point2 &p1, const Point2 &q, OptionalJacobian< 1, 2 > H1=boost::none, OptionalJacobian< 1, 2 > H2=boost::none)
 distance between two points
 
│ │ │ │ -Point2 gtsam::operator* (double s, const Point2 &p)
 multiply with scalar
 
│ │ │ │ -boost::optional< Point2 > gtsam::circleCircleIntersection (double R_d, double r_d, double tol)
 
│ │ │ │ -list< Point2 > gtsam::circleCircleIntersection (Point2 c1, Point2 c2, boost::optional< Point2 > fh)
 
│ │ │ │ -Point2Pair gtsam::means (const std::vector< Point2Pair > &abPointPairs)
 Calculate the two means of a set of Point2 pairs.
 
list< Point2 > gtsam::circleCircleIntersection (Point2 c1, double r1, Point2 c2, double r2, double tol=1e-9)
 Intersect 2 circles.
 
│ │ │ │ +typedef Eigen::Quaternion< double, Eigen::DontAlign > gtsam::Quaternion
 
│ │ │ │

Detailed Description

│ │ │ │ -

2D Point

│ │ │ │ +

Lie Group wrapper for Eigen Quaternions.

│ │ │ │
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,70 +1,30 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Point2.h File Reference │ │ │ │ │ -2D Point _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _T_y_p_e_d_e_f_s │ │ │ │ │ +Quaternion.h File Reference │ │ │ │ │ +Lie Group wrapper for Eigen Quaternions. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_R_a_n_g_e_<_ _P_o_i_n_t_2_,_ _P_o_i_n_t_2_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _Q_U_A_T_E_R_N_I_O_N___T_Y_P_E_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ - typedef Vector2  ggttssaamm::::PPooiinntt22 │ │ │ │ │ - As of GTSAM 4, in order to make GTSAM more │ │ │ │ │ -  lean, it is now possible to just typedef │ │ │ │ │ - Point2 to Vector2. │ │ │ │ │ -  │ │ │ │ │ - using  ggttssaamm::::PPooiinntt22PPaaiirr = std::pair< _P_o_i_n_t_2, │ │ │ │ │ - _P_o_i_n_t_2 > │ │ │ │ │ -  │ │ │ │ │ - using  ggttssaamm::::PPooiinntt22PPaaiirrss = std::vector< │ │ │ │ │ - Point2Pair > │ │ │ │ │ -  │ │ │ │ │ -typedef std::vector< _P_o_i_n_t_2, Eigen:: │ │ │ │ │ - aligned_allocator< _P_o_i_n_t_2 > >  ggttssaamm::::PPooiinntt22VVeeccttoorr │ │ │ │ │ -  │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ - ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const gtsam:: │ │ │ │ │ - Point2Pair &p) │ │ │ │ │ -  │ │ │ │ │ - double  ggttssaamm::::nnoorrmm22 (const _P_o_i_n_t_2 &p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 1, │ │ │ │ │ - 2 > H=boost::none) │ │ │ │ │ -  Distance of the point from the origin, with │ │ │ │ │ - Jacobian. │ │ │ │ │ +MMaaccrrooss │ │ │ │ │ +#define  QQUUAATTEERRNNIIOONN__TTYYPPEE   Eigen::Quaternion<_Scalar,_Options> │ │ │ │ │   │ │ │ │ │ - double  ggttssaamm::::ddiissttaannccee22 (const _P_o_i_n_t_2 &p1, const _P_o_i_n_t_2 &q, │ │ │ │ │ - _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 1, 2 > H1=boost::none, │ │ │ │ │ - _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 1, 2 > H2=boost::none) │ │ │ │ │ -  distance between two points │ │ │ │ │ -  │ │ │ │ │ - _P_o_i_n_t_2  ggttssaamm::::ooppeerraattoorr** (double s, const _P_o_i_n_t_2 &p) │ │ │ │ │ -  multiply with scalar │ │ │ │ │ -  │ │ │ │ │ -boost::optional< _P_o_i_n_t_2 >  ggttssaamm::::cciirrcclleeCCiirrcclleeIInntteerrsseeccttiioonn (double R_d, double │ │ │ │ │ - r_d, double tol) │ │ │ │ │ -  │ │ │ │ │ - list< _P_o_i_n_t_2 >  ggttssaamm::::cciirrcclleeCCiirrcclleeIInntteerrsseeccttiioonn (_P_o_i_n_t_2 c1, _P_o_i_n_t_2 │ │ │ │ │ - c2, boost::optional< _P_o_i_n_t_2 > fh) │ │ │ │ │ -  │ │ │ │ │ - Point2Pair  ggttssaamm::::mmeeaannss (const std::vector< Point2Pair > │ │ │ │ │ - &abPointPairs) │ │ │ │ │ -  Calculate the two means of a set of Point2 pairs. │ │ │ │ │ -  │ │ │ │ │ - list< _P_o_i_n_t_2 >  _g_t_s_a_m_:_:_c_i_r_c_l_e_C_i_r_c_l_e_I_n_t_e_r_s_e_c_t_i_o_n (_P_o_i_n_t_2 c1, double │ │ │ │ │ - r1, _P_o_i_n_t_2 c2, double r2, double tol=1e-9) │ │ │ │ │ -  Intersect 2 circles. │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +typedef Eigen::Quaternion< double, Eigen::DontAlign >  ggttssaamm::::QQuuaatteerrnniioonn │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -2D Point │ │ │ │ │ +Lie Group wrapper for Eigen Quaternions. │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _P_o_i_n_t_2_._h │ │ │ │ │ + * _Q_u_a_t_e_r_n_i_o_n_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00449.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,9 +1,3 @@ │ │ │ │ │ var a00449 = [ │ │ │ │ │ - ["gtsam::Range< Point2, Point2 >", "a03148.html", null], │ │ │ │ │ - ["Point2", "a00449.html#a6ede8384dee0353a0ce5fb54ea50c21d", null], │ │ │ │ │ - ["circleCircleIntersection", "a00449.html#a93d4e38a582b6f32fc4f301df10721d5", null], │ │ │ │ │ - ["distance2", "a00449.html#afd1282dfb080cb393ae60188b4582cb2", null], │ │ │ │ │ - ["means", "a00449.html#a9e357cda5287fae1438f86bc4df27a80", null], │ │ │ │ │ - ["norm2", "a00449.html#afee4e6aa4aba9b6a6b421ddd75b52dfc", null], │ │ │ │ │ - ["operator*", "a00449.html#a63d86e99c211c6daeac2b7b4dd9d928e", null] │ │ │ │ │ + ["gtsam::traits< QUATERNION_TYPE >", "a03208.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00449_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Point2.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Quaternion.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,15 +98,15 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Point2.h
│ │ │ │ +
Quaternion.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │ @@ -114,100 +114,184 @@ │ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
18#pragma once
│ │ │ │
19
│ │ │ │ -
20#include <gtsam/base/VectorSpace.h>
│ │ │ │ -
21#include <boost/serialization/nvp.hpp>
│ │ │ │ -
22
│ │ │ │ -
23namespace gtsam {
│ │ │ │ -
24
│ │ │ │ -
27typedef Vector2 Point2;
│ │ │ │ -
28
│ │ │ │ -
29// Convenience typedef
│ │ │ │ -
30using Point2Pair = std::pair<Point2, Point2>;
│ │ │ │ -
31GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p);
│ │ │ │ -
32
│ │ │ │ -
33using Point2Pairs = std::vector<Point2Pair>;
│ │ │ │ -
34
│ │ │ │ -
36GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none);
│ │ │ │ -
37
│ │ │ │ -
39GTSAM_EXPORT double distance2(const Point2& p1, const Point2& q,
│ │ │ │ -
40 OptionalJacobian<1, 2> H1 = boost::none,
│ │ │ │ -
41 OptionalJacobian<1, 2> H2 = boost::none);
│ │ │ │ -
42
│ │ │ │ -
43// For MATLAB wrapper
│ │ │ │ -
44typedef std::vector<Point2, Eigen::aligned_allocator<Point2> > Point2Vector;
│ │ │ │ -
45
│ │ │ │ -
│ │ │ │ -
47inline Point2 operator*(double s, const Point2& p) {
│ │ │ │ -
48 return Point2(s * p.x(), s * p.y());
│ │ │ │ -
49}
│ │ │ │ -
│ │ │ │ -
50
│ │ │ │ -
51/*
│ │ │ │ -
52 * @brief Circle-circle intersection, given normalized radii.
│ │ │ │ -
53 * Calculate f and h, respectively the parallel and perpendicular distance of
│ │ │ │ -
54 * the intersections of two circles along and from the line connecting the centers.
│ │ │ │ -
55 * Both are dimensionless fractions of the distance d between the circle centers.
│ │ │ │ -
56 * If the circles do not intersect or they are identical, returns boost::none.
│ │ │ │ -
57 * If one solution (touching circles, as determined by tol), h will be exactly zero.
│ │ │ │ -
58 * h is a good measure for how accurate the intersection will be, as when circles touch
│ │ │ │ -
59 * or nearly touch, the intersection is ill-defined with noisy radius measurements.
│ │ │ │ -
60 * @param R_d : R/d, ratio of radius of first circle to distance between centers
│ │ │ │ -
61 * @param r_d : r/d, ratio of radius of second circle to distance between centers
│ │ │ │ -
62 * @param tol: absolute tolerance below which we consider touching circles
│ │ │ │ -
63 * @return optional Point2 with f and h, boost::none if no solution.
│ │ │ │ -
64 */
│ │ │ │ -
65GTSAM_EXPORT boost::optional<Point2> circleCircleIntersection(double R_d, double r_d, double tol = 1e-9);
│ │ │ │ -
66
│ │ │ │ -
67/*
│ │ │ │ -
68 * @brief Circle-circle intersection, from the normalized radii solution.
│ │ │ │ -
69 * @param c1 center of first circle
│ │ │ │ -
70 * @param c2 center of second circle
│ │ │ │ -
71 * @return list of solutions (0,1, or 2). Identical circles will return empty list, as well.
│ │ │ │ -
72 */
│ │ │ │ -
73GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh);
│ │ │ │ -
74
│ │ │ │ -
76GTSAM_EXPORT Point2Pair means(const std::vector<Point2Pair> &abPointPairs);
│ │ │ │ +
20#include <gtsam/base/Lie.h>
│ │ │ │ +
21#include <gtsam/base/concepts.h>
│ │ │ │ +
22#include <gtsam/geometry/SO3.h> // Logmap/Expmap derivatives
│ │ │ │ +
23#include <limits>
│ │ │ │ +
24#include <iostream>
│ │ │ │ +
25
│ │ │ │ +
26#define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options>
│ │ │ │ +
27
│ │ │ │ +
28namespace gtsam {
│ │ │ │ +
29
│ │ │ │ +
30// Define traits
│ │ │ │ +
31template<typename _Scalar, int _Options>
│ │ │ │ +
│ │ │ │ +
32struct traits<QUATERNION_TYPE> {
│ │ │ │ +
33 typedef QUATERNION_TYPE ManifoldType;
│ │ │ │ +
34 typedef QUATERNION_TYPE Q;
│ │ │ │ +
35
│ │ │ │ +
36 typedef lie_group_tag structure_category;
│ │ │ │ +
37 typedef multiplicative_group_tag group_flavor;
│ │ │ │ +
38
│ │ │ │ +
41 static Q Identity() {
│ │ │ │ +
42 return Q::Identity();
│ │ │ │ +
43 }
│ │ │ │ +
44
│ │ │ │ +
48 enum {
│ │ │ │ +
49 dimension = 3
│ │ │ │ +
50 };
│ │ │ │ +
51 typedef OptionalJacobian<3, 3> ChartJacobian;
│ │ │ │ +
52 typedef Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> TangentVector;
│ │ │ │ +
53
│ │ │ │ +
57 static Q Compose(const Q &g, const Q & h,
│ │ │ │ +
58 ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) {
│ │ │ │ +
59 if (Hg) *Hg = h.toRotationMatrix().transpose();
│ │ │ │ +
60 if (Hh) *Hh = I_3x3;
│ │ │ │ +
61 return g * h;
│ │ │ │ +
62 }
│ │ │ │ +
63
│ │ │ │ +
64 static Q Between(const Q &g, const Q & h,
│ │ │ │ +
65 ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) {
│ │ │ │ +
66 Q d = g.inverse() * h;
│ │ │ │ +
67 if (Hg) *Hg = -d.toRotationMatrix().transpose();
│ │ │ │ +
68 if (Hh) *Hh = I_3x3;
│ │ │ │ +
69 return d;
│ │ │ │ +
70 }
│ │ │ │ +
71
│ │ │ │ +
72 static Q Inverse(const Q &g,
│ │ │ │ +
73 ChartJacobian H = boost::none) {
│ │ │ │ +
74 if (H) *H = -g.toRotationMatrix();
│ │ │ │ +
75 return g.inverse();
│ │ │ │ +
76 }
│ │ │ │
77
│ │ │ │ -
87GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, double r1,
│ │ │ │ -
88 Point2 c2, double r2, double tol = 1e-9);
│ │ │ │ -
89
│ │ │ │ -
90template <typename A1, typename A2>
│ │ │ │ -
91struct Range;
│ │ │ │ -
92
│ │ │ │ -
93template <>
│ │ │ │ -
│ │ │ │ -
94struct Range<Point2, Point2> {
│ │ │ │ -
95 typedef double result_type;
│ │ │ │ -
96 double operator()(const Point2& p, const Point2& q,
│ │ │ │ -
97 OptionalJacobian<1, 2> H1 = boost::none,
│ │ │ │ -
98 OptionalJacobian<1, 2> H2 = boost::none) {
│ │ │ │ -
99 return distance2(p, q, H1, H2);
│ │ │ │ -
100 }
│ │ │ │ -
101};
│ │ │ │ -
│ │ │ │ -
102
│ │ │ │ -
103} // \ namespace gtsam
│ │ │ │ -
104
│ │ │ │ +
│ │ │ │ +
79 static Q Expmap(const Eigen::Ref<const TangentVector>& omega,
│ │ │ │ +
80 ChartJacobian H = boost::none) {
│ │ │ │ +
81 using std::cos;
│ │ │ │ +
82 using std::sin;
│ │ │ │ +
83 if (H) *H = SO3::ExpmapDerivative(omega.template cast<double>());
│ │ │ │ +
84 _Scalar theta2 = omega.dot(omega);
│ │ │ │ +
85 if (theta2 > std::numeric_limits<_Scalar>::epsilon()) {
│ │ │ │ +
86 _Scalar theta = std::sqrt(theta2);
│ │ │ │ +
87 _Scalar ha = _Scalar(0.5) * theta;
│ │ │ │ +
88 Vector3 vec = (sin(ha) / theta) * omega;
│ │ │ │ +
89 return Q(cos(ha), vec.x(), vec.y(), vec.z());
│ │ │ │ +
90 } else {
│ │ │ │ +
91 // first order approximation sin(theta/2)/theta = 0.5
│ │ │ │ +
92 Vector3 vec = _Scalar(0.5) * omega;
│ │ │ │ +
93 return Q(1.0, vec.x(), vec.y(), vec.z());
│ │ │ │ +
94 }
│ │ │ │ +
95 }
│ │ │ │ +
│ │ │ │ +
96
│ │ │ │ +
│ │ │ │ +
98 static TangentVector Logmap(const Q& q, ChartJacobian H = boost::none) {
│ │ │ │ +
99 using std::acos;
│ │ │ │ +
100 using std::sqrt;
│ │ │ │ +
101
│ │ │ │ +
102 // define these compile time constants to avoid std::abs:
│ │ │ │ +
103 static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10,
│ │ │ │ +
104 NearlyNegativeOne = -1.0 + 1e-10;
│ │ │ │ +
105
│ │ │ │ +
106 TangentVector omega;
│ │ │ │ +
107
│ │ │ │ +
108 const _Scalar qw = q.w();
│ │ │ │ +
109 // See Quaternion-Logmap.nb in doc for Taylor expansions
│ │ │ │ +
110 if (qw > NearlyOne) {
│ │ │ │ +
111 // Taylor expansion of (angle / s) at 1
│ │ │ │ +
112 // (2 + 2 * (1-qw) / 3) * q.vec();
│ │ │ │ +
113 omega = ( 8. / 3. - 2. / 3. * qw) * q.vec();
│ │ │ │ +
114 } else if (qw < NearlyNegativeOne) {
│ │ │ │ +
115 // Taylor expansion of (angle / s) at -1
│ │ │ │ +
116 // (-2 - 2 * (1 + qw) / 3) * q.vec();
│ │ │ │ +
117 omega = (-8. / 3. - 2. / 3. * qw) * q.vec();
│ │ │ │ +
118 } else {
│ │ │ │ +
119 // Normal, away from zero case
│ │ │ │ +
120 if (qw > 0) {
│ │ │ │ +
121 _Scalar angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
│ │ │ │ +
122 // Important: convert to [-pi,pi] to keep error continuous
│ │ │ │ +
123 if (angle > M_PI)
│ │ │ │ +
124 angle -= twoPi;
│ │ │ │ +
125 else if (angle < -M_PI)
│ │ │ │ +
126 angle += twoPi;
│ │ │ │ +
127 omega = (angle / s) * q.vec();
│ │ │ │ +
128 } else {
│ │ │ │ +
129 // Make sure that we are using a canonical quaternion with w > 0
│ │ │ │ +
130 _Scalar angle = 2 * acos(-qw), s = sqrt(1 - qw * qw);
│ │ │ │ +
131 if (angle > M_PI)
│ │ │ │ +
132 angle -= twoPi;
│ │ │ │ +
133 else if (angle < -M_PI)
│ │ │ │ +
134 angle += twoPi;
│ │ │ │ +
135 omega = (angle / s) * -q.vec();
│ │ │ │ +
136 }
│ │ │ │ +
137 }
│ │ │ │ +
138
│ │ │ │ +
139 if(H) *H = SO3::LogmapDerivative(omega.template cast<double>());
│ │ │ │ +
140 return omega;
│ │ │ │ +
141 }
│ │ │ │ +
│ │ │ │ +
142
│ │ │ │ +
146
│ │ │ │ +
147 static TangentVector Local(const Q& g, const Q& h,
│ │ │ │ +
148 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ +
149 Q b = Between(g, h, H1, H2);
│ │ │ │ +
150 Matrix3 D_v_b;
│ │ │ │ +
151 TangentVector v = Logmap(b, (H1 || H2) ? &D_v_b : 0);
│ │ │ │ +
152 if (H1) *H1 = D_v_b * (*H1);
│ │ │ │ +
153 if (H2) *H2 = D_v_b * (*H2);
│ │ │ │ +
154 return v;
│ │ │ │ +
155 }
│ │ │ │ +
156
│ │ │ │ +
157 static Q Retract(const Q& g, const TangentVector& v,
│ │ │ │ +
158 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ +
159 Matrix3 D_h_v;
│ │ │ │ +
160 Q b = Expmap(v,H2 ? &D_h_v : 0);
│ │ │ │ +
161 Q h = Compose(g, b, H1, H2);
│ │ │ │ +
162 if (H2) *H2 = (*H2) * D_h_v;
│ │ │ │ +
163 return h;
│ │ │ │ +
164 }
│ │ │ │ +
165
│ │ │ │ +
169 static void Print(const Q& q, const std::string& str = "") {
│ │ │ │ +
170 if (str.size() == 0)
│ │ │ │ +
171 std::cout << "Eigen::Quaternion: ";
│ │ │ │ +
172 else
│ │ │ │ +
173 std::cout << str << " ";
│ │ │ │ +
174 std::cout << q.vec().transpose() << std::endl;
│ │ │ │ +
175 }
│ │ │ │ +
176 static bool Equals(const Q& q1, const Q& q2, double tol = 1e-8) {
│ │ │ │ +
177 return Between(q1, q2).vec().array().abs().maxCoeff() < tol;
│ │ │ │ +
178 }
│ │ │ │ +
180};
│ │ │ │ +
│ │ │ │ +
181
│ │ │ │ +
182typedef Eigen::Quaternion<double, Eigen::DontAlign> Quaternion;
│ │ │ │ +
183
│ │ │ │ +
184} // \namespace gtsam
│ │ │ │ +
185
│ │ │ │ +
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ +
SO3.h
3*3 matrix representation of SO(3)
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
│ │ │ │ -
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │ -
gtsam::means
Point2Pair means(const std::vector< Point2Pair > &abPointPairs)
Calculate the two means of a set of Point2 pairs.
Definition Point2.cpp:116
│ │ │ │ -
gtsam::distance2
double distance2(const Point2 &p, const Point2 &q, OptionalJacobian< 1, 2 > H1, OptionalJacobian< 1, 2 > H2)
distance between two points
Definition Point2.cpp:39
│ │ │ │ -
gtsam::norm2
double norm2(const Point2 &p, OptionalJacobian< 1, 2 > H)
Distance of the point from the origin, with Jacobian.
Definition Point2.cpp:27
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::multiplicative_group_tag
Group operator syntax flavors.
Definition Group.h:37
│ │ │ │ +
gtsam::lie_group_tag
tag to assert a type is a Lie group
Definition Lie.h:164
│ │ │ │
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
gtsam::Range
Definition BearingRange.h:40
│ │ │ │ +
gtsam::traits< QUATERNION_TYPE >::Expmap
static Q Expmap(const Eigen::Ref< const TangentVector > &omega, ChartJacobian H=boost::none)
Exponential map, using the inlined code from Eigen's conversion from axis/angle.
Definition Quaternion.h:79
│ │ │ │ +
gtsam::traits< QUATERNION_TYPE >::Logmap
static TangentVector Logmap(const Q &q, ChartJacobian H=boost::none)
We use our own Logmap, as there is a slight bug in Eigen.
Definition Quaternion.h:98
│ │ │ │ +
gtsam::SO< 3 >::ExpmapDerivative
static MatrixDD ExpmapDerivative(const TangentVector &omega)
Derivative of Expmap, currently only defined for SO3.
Definition SOn-inl.h:72
│ │ │ │ +
gtsam::SO< 3 >::LogmapDerivative
static MatrixDD LogmapDerivative(const TangentVector &omega)
Derivative of Logmap, currently only defined for SO3.
Definition SOn-inl.h:82
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Point2.h │ │ │ │ │ +Quaternion.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ @@ -16,125 +16,199 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 18#pragma once │ │ │ │ │ 19 │ │ │ │ │ -20#include │ │ │ │ │ -21#include │ │ │ │ │ -22 │ │ │ │ │ -23namespace _g_t_s_a_m { │ │ │ │ │ -24 │ │ │ │ │ -_2_7typedef Vector2 _P_o_i_n_t_2; │ │ │ │ │ -28 │ │ │ │ │ -29// Convenience typedef │ │ │ │ │ -30using Point2Pair = std::pair; │ │ │ │ │ -31GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam:: │ │ │ │ │ -Point2Pair &p); │ │ │ │ │ -32 │ │ │ │ │ -33using Point2Pairs = std::vector; │ │ │ │ │ -34 │ │ │ │ │ -36GTSAM_EXPORT double _n_o_r_m_2(const _P_o_i_n_t_2& p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _2_> H = boost:: │ │ │ │ │ -none); │ │ │ │ │ -37 │ │ │ │ │ -39GTSAM_EXPORT double _d_i_s_t_a_n_c_e_2(const _P_o_i_n_t_2& p1, const _P_o_i_n_t_2& q, │ │ │ │ │ -40 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _2_> H1 = boost::none, │ │ │ │ │ -41 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _2_> H2 = boost::none); │ │ │ │ │ -42 │ │ │ │ │ -43// For MATLAB wrapper │ │ │ │ │ -44typedef std::vector > Point2Vector; │ │ │ │ │ -45 │ │ │ │ │ -_4_7inline _P_o_i_n_t_2 _o_p_e_r_a_t_o_r_*(double s, const _P_o_i_n_t_2& p) { │ │ │ │ │ -48 return _P_o_i_n_t_2(s * p.x(), s * p.y()); │ │ │ │ │ -49} │ │ │ │ │ -50 │ │ │ │ │ -51/* │ │ │ │ │ -52 * @brief Circle-circle intersection, given normalized radii. │ │ │ │ │ -53 * Calculate f and h, respectively the parallel and perpendicular distance of │ │ │ │ │ -54 * the intersections of two circles along and from the line connecting the │ │ │ │ │ -centers. │ │ │ │ │ -55 * Both are dimensionless fractions of the distance d between the circle │ │ │ │ │ -centers. │ │ │ │ │ -56 * If the circles do not intersect or they are identical, returns boost:: │ │ │ │ │ -none. │ │ │ │ │ -57 * If one solution (touching circles, as determined by tol), h will be │ │ │ │ │ -exactly zero. │ │ │ │ │ -58 * h is a good measure for how accurate the intersection will be, as when │ │ │ │ │ -circles touch │ │ │ │ │ -59 * or nearly touch, the intersection is ill-defined with noisy radius │ │ │ │ │ -measurements. │ │ │ │ │ -60 * @param R_d : R/d, ratio of radius of first circle to distance between │ │ │ │ │ -centers │ │ │ │ │ -61 * @param r_d : r/d, ratio of radius of second circle to distance between │ │ │ │ │ -centers │ │ │ │ │ -62 * @param tol: absolute tolerance below which we consider touching circles │ │ │ │ │ -63 * @return optional Point2 with f and h, boost::none if no solution. │ │ │ │ │ -64 */ │ │ │ │ │ -65GTSAM_EXPORT boost::optional circleCircleIntersection(double R_d, │ │ │ │ │ -double r_d, double tol = 1e-9); │ │ │ │ │ -66 │ │ │ │ │ -67/* │ │ │ │ │ -68 * @brief Circle-circle intersection, from the normalized radii solution. │ │ │ │ │ -69 * @param c1 center of first circle │ │ │ │ │ -70 * @param c2 center of second circle │ │ │ │ │ -71 * @return list of solutions (0,1, or 2). Identical circles will return empty │ │ │ │ │ -list, as well. │ │ │ │ │ -72 */ │ │ │ │ │ -73GTSAM_EXPORT std::list circleCircleIntersection(_P_o_i_n_t_2 c1, _P_o_i_n_t_2 c2, │ │ │ │ │ -boost::optional fh); │ │ │ │ │ -74 │ │ │ │ │ -76GTSAM_EXPORT Point2Pair _m_e_a_n_s(const std::vector &abPointPairs); │ │ │ │ │ +20#include <_g_t_s_a_m_/_b_a_s_e_/_L_i_e_._h> │ │ │ │ │ +21#include │ │ │ │ │ +22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_S_O_3_._h> // Logmap/Expmap derivatives │ │ │ │ │ +23#include │ │ │ │ │ +24#include │ │ │ │ │ +25 │ │ │ │ │ +26#define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options> │ │ │ │ │ +27 │ │ │ │ │ +28namespace _g_t_s_a_m { │ │ │ │ │ +29 │ │ │ │ │ +30// Define traits │ │ │ │ │ +31template │ │ │ │ │ +_3_2struct _t_r_a_i_t_s { │ │ │ │ │ +33 typedef QUATERNION_TYPE ManifoldType; │ │ │ │ │ +34 typedef QUATERNION_TYPE Q; │ │ │ │ │ +35 │ │ │ │ │ +36 typedef _l_i_e___g_r_o_u_p___t_a_g _s_t_r_u_c_t_u_r_e___c_a_t_e_g_o_r_y; │ │ │ │ │ +37 typedef _m_u_l_t_i_p_l_i_c_a_t_i_v_e___g_r_o_u_p___t_a_g _g_r_o_u_p___f_l_a_v_o_r; │ │ │ │ │ +38 │ │ │ │ │ +41 static Q Identity() { │ │ │ │ │ +42 return Q::Identity(); │ │ │ │ │ +43 } │ │ │ │ │ +44 │ │ │ │ │ +48 enum { │ │ │ │ │ +49 dimension = 3 │ │ │ │ │ +50 }; │ │ │ │ │ +51 typedef _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> ChartJacobian; │ │ │ │ │ +52 typedef Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> TangentVector; │ │ │ │ │ +53 │ │ │ │ │ +57 static Q Compose(const Q &g, const Q & h, │ │ │ │ │ +58 ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) { │ │ │ │ │ +59 if (Hg) *Hg = h.toRotationMatrix().transpose(); │ │ │ │ │ +60 if (Hh) *Hh = I_3x3; │ │ │ │ │ +61 return g * h; │ │ │ │ │ +62 } │ │ │ │ │ +63 │ │ │ │ │ +64 static Q Between(const Q &g, const Q & h, │ │ │ │ │ +65 ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) { │ │ │ │ │ +66 Q d = g.inverse() * h; │ │ │ │ │ +67 if (Hg) *Hg = -d.toRotationMatrix().transpose(); │ │ │ │ │ +68 if (Hh) *Hh = I_3x3; │ │ │ │ │ +69 return d; │ │ │ │ │ +70 } │ │ │ │ │ +71 │ │ │ │ │ +72 static Q Inverse(const Q &g, │ │ │ │ │ +73 ChartJacobian H = boost::none) { │ │ │ │ │ +74 if (H) *H = -g.toRotationMatrix(); │ │ │ │ │ +75 return g.inverse(); │ │ │ │ │ +76 } │ │ │ │ │ 77 │ │ │ │ │ -87GTSAM_EXPORT std::list circleCircleIntersection(_P_o_i_n_t_2 c1, double r1, │ │ │ │ │ -88 _P_o_i_n_t_2 c2, double r2, double tol = 1e-9); │ │ │ │ │ -89 │ │ │ │ │ -90template │ │ │ │ │ -91struct Range; │ │ │ │ │ -92 │ │ │ │ │ -93template <> │ │ │ │ │ -_9_4struct _R_a_n_g_e<_P_o_i_n_t_2, _P_o_i_n_t_2> { │ │ │ │ │ -95 typedef double result_type; │ │ │ │ │ -96 double operator()(const _P_o_i_n_t_2& p, const _P_o_i_n_t_2& q, │ │ │ │ │ -97 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _2_> H1 = boost::none, │ │ │ │ │ -98 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _2_> H2 = boost::none) { │ │ │ │ │ -99 return _d_i_s_t_a_n_c_e_2(p, q, H1, H2); │ │ │ │ │ -100 } │ │ │ │ │ -101}; │ │ │ │ │ -102 │ │ │ │ │ -103} // \ namespace gtsam │ │ │ │ │ -104 │ │ │ │ │ +_7_9 static Q _E_x_p_m_a_p(const Eigen::Ref& omega, │ │ │ │ │ +80 ChartJacobian H = boost::none) { │ │ │ │ │ +81 using std::cos; │ │ │ │ │ +82 using std::sin; │ │ │ │ │ +83 if (H) *H = _S_O_3_:_:_E_x_p_m_a_p_D_e_r_i_v_a_t_i_v_e(omega.template cast()); │ │ │ │ │ +84 _Scalar theta2 = omega.dot(omega); │ │ │ │ │ +85 if (theta2 > std::numeric_limits<_Scalar>::epsilon()) { │ │ │ │ │ +86 _Scalar theta = std::sqrt(theta2); │ │ │ │ │ +87 _Scalar ha = _Scalar(0.5) * theta; │ │ │ │ │ +88 Vector3 vec = (sin(ha) / theta) * omega; │ │ │ │ │ +89 return Q(cos(ha), vec.x(), vec.y(), vec.z()); │ │ │ │ │ +90 } else { │ │ │ │ │ +91 // first order approximation sin(theta/2)/theta = 0.5 │ │ │ │ │ +92 Vector3 vec = _Scalar(0.5) * omega; │ │ │ │ │ +93 return Q(1.0, vec.x(), vec.y(), vec.z()); │ │ │ │ │ +94 } │ │ │ │ │ +95 } │ │ │ │ │ +96 │ │ │ │ │ +_9_8 static TangentVector _L_o_g_m_a_p(const Q& q, ChartJacobian H = boost::none) { │ │ │ │ │ +99 using std::acos; │ │ │ │ │ +100 using std::sqrt; │ │ │ │ │ +101 │ │ │ │ │ +102 // define these compile time constants to avoid std::abs: │ │ │ │ │ +103 static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10, │ │ │ │ │ +104 NearlyNegativeOne = -1.0 + 1e-10; │ │ │ │ │ +105 │ │ │ │ │ +106 TangentVector omega; │ │ │ │ │ +107 │ │ │ │ │ +108 const _Scalar qw = q.w(); │ │ │ │ │ +109 // See Quaternion-Logmap.nb in doc for Taylor expansions │ │ │ │ │ +110 if (qw > NearlyOne) { │ │ │ │ │ +111 // Taylor expansion of (angle / s) at 1 │ │ │ │ │ +112 // (2 + 2 * (1-qw) / 3) * q.vec(); │ │ │ │ │ +113 omega = ( 8. / 3. - 2. / 3. * qw) * q.vec(); │ │ │ │ │ +114 } else if (qw < NearlyNegativeOne) { │ │ │ │ │ +115 // Taylor expansion of (angle / s) at -1 │ │ │ │ │ +116 // (-2 - 2 * (1 + qw) / 3) * q.vec(); │ │ │ │ │ +117 omega = (-8. / 3. - 2. / 3. * qw) * q.vec(); │ │ │ │ │ +118 } else { │ │ │ │ │ +119 // Normal, away from zero case │ │ │ │ │ +120 if (qw > 0) { │ │ │ │ │ +121 _Scalar angle = 2 * acos(qw), s = sqrt(1 - qw * qw); │ │ │ │ │ +122 // Important: convert to [-pi,pi] to keep error continuous │ │ │ │ │ +123 if (angle > M_PI) │ │ │ │ │ +124 angle -= twoPi; │ │ │ │ │ +125 else if (angle < -M_PI) │ │ │ │ │ +126 angle += twoPi; │ │ │ │ │ +127 omega = (angle / s) * q.vec(); │ │ │ │ │ +128 } else { │ │ │ │ │ +129 // Make sure that we are using a canonical quaternion with w > 0 │ │ │ │ │ +130 _Scalar angle = 2 * acos(-qw), s = sqrt(1 - qw * qw); │ │ │ │ │ +131 if (angle > M_PI) │ │ │ │ │ +132 angle -= twoPi; │ │ │ │ │ +133 else if (angle < -M_PI) │ │ │ │ │ +134 angle += twoPi; │ │ │ │ │ +135 omega = (angle / s) * -q.vec(); │ │ │ │ │ +136 } │ │ │ │ │ +137 } │ │ │ │ │ +138 │ │ │ │ │ +139 if(H) *H = _S_O_3_:_:_L_o_g_m_a_p_D_e_r_i_v_a_t_i_v_e(omega.template cast()); │ │ │ │ │ +140 return omega; │ │ │ │ │ +141 } │ │ │ │ │ +142 │ │ │ │ │ +146 │ │ │ │ │ +147 static TangentVector Local(const Q& g, const Q& h, │ │ │ │ │ +148 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { │ │ │ │ │ +149 Q b = Between(g, h, H1, H2); │ │ │ │ │ +150 Matrix3 D_v_b; │ │ │ │ │ +151 TangentVector v = Logmap(b, (H1 || H2) ? &D_v_b : 0); │ │ │ │ │ +152 if (H1) *H1 = D_v_b * (*H1); │ │ │ │ │ +153 if (H2) *H2 = D_v_b * (*H2); │ │ │ │ │ +154 return v; │ │ │ │ │ +155 } │ │ │ │ │ +156 │ │ │ │ │ +157 static Q Retract(const Q& g, const TangentVector& v, │ │ │ │ │ +158 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { │ │ │ │ │ +159 Matrix3 D_h_v; │ │ │ │ │ +160 Q b = Expmap(v,H2 ? &D_h_v : 0); │ │ │ │ │ +161 Q h = Compose(g, b, H1, H2); │ │ │ │ │ +162 if (H2) *H2 = (*H2) * D_h_v; │ │ │ │ │ +163 return h; │ │ │ │ │ +164 } │ │ │ │ │ +165 │ │ │ │ │ +169 static void Print(const Q& q, const std::string& str = "") { │ │ │ │ │ +170 if (str.size() == 0) │ │ │ │ │ +171 std::cout << "Eigen::Quaternion: "; │ │ │ │ │ +172 else │ │ │ │ │ +173 std::cout << str << " "; │ │ │ │ │ +174 std::cout << q.vec().transpose() << std::endl; │ │ │ │ │ +175 } │ │ │ │ │ +176 static bool Equals(const Q& q1, const Q& q2, double tol = 1e-8) { │ │ │ │ │ +177 return Between(q1, q2).vec().array().abs().maxCoeff() < tol; │ │ │ │ │ +178 } │ │ │ │ │ +180}; │ │ │ │ │ +181 │ │ │ │ │ +182typedef Eigen::Quaternion Quaternion; │ │ │ │ │ +183 │ │ │ │ │ +184} // \namespace gtsam │ │ │ │ │ +185 │ │ │ │ │ +_L_i_e_._h │ │ │ │ │ +Base class and basic functions for Lie types. │ │ │ │ │ +_S_O_3_._h │ │ │ │ │ +3*3 matrix representation of SO(3) │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ -Point2 operator*(double s, const Point2 &p) │ │ │ │ │ -multiply with scalar │ │ │ │ │ -DDeeffiinniittiioonn Point2.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ -Vector2 Point2 │ │ │ │ │ -As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point2 to Vector2... │ │ │ │ │ -DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ -_g_t_s_a_m_:_:_m_e_a_n_s │ │ │ │ │ -Point2Pair means(const std::vector< Point2Pair > &abPointPairs) │ │ │ │ │ -Calculate the two means of a set of Point2 pairs. │ │ │ │ │ -DDeeffiinniittiioonn Point2.cpp:116 │ │ │ │ │ -_g_t_s_a_m_:_:_d_i_s_t_a_n_c_e_2 │ │ │ │ │ -double distance2(const Point2 &p, const Point2 &q, OptionalJacobian< 1, 2 > H1, │ │ │ │ │ -OptionalJacobian< 1, 2 > H2) │ │ │ │ │ -distance between two points │ │ │ │ │ -DDeeffiinniittiioonn Point2.cpp:39 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_r_m_2 │ │ │ │ │ -double norm2(const Point2 &p, OptionalJacobian< 1, 2 > H) │ │ │ │ │ -Distance of the point from the origin, with Jacobian. │ │ │ │ │ -DDeeffiinniittiioonn Point2.cpp:27 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_m_u_l_t_i_p_l_i_c_a_t_i_v_e___g_r_o_u_p___t_a_g │ │ │ │ │ +Group operator syntax flavors. │ │ │ │ │ +DDeeffiinniittiioonn Group.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_l_i_e___g_r_o_u_p___t_a_g │ │ │ │ │ +tag to assert a type is a Lie group │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:164 │ │ │ │ │ _g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ either a fixed size o... │ │ │ │ │ DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_R_a_n_g_e │ │ │ │ │ -DDeeffiinniittiioonn BearingRange.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _Q_U_A_T_E_R_N_I_O_N___T_Y_P_E_ _>_:_:_E_x_p_m_a_p │ │ │ │ │ +static Q Expmap(const Eigen::Ref< const TangentVector > &omega, ChartJacobian │ │ │ │ │ +H=boost::none) │ │ │ │ │ +Exponential map, using the inlined code from Eigen's conversion from axis/ │ │ │ │ │ +angle. │ │ │ │ │ +DDeeffiinniittiioonn Quaternion.h:79 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _Q_U_A_T_E_R_N_I_O_N___T_Y_P_E_ _>_:_:_L_o_g_m_a_p │ │ │ │ │ +static TangentVector Logmap(const Q &q, ChartJacobian H=boost::none) │ │ │ │ │ +We use our own Logmap, as there is a slight bug in Eigen. │ │ │ │ │ +DDeeffiinniittiioonn Quaternion.h:98 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_<_ _3_ _>_:_:_E_x_p_m_a_p_D_e_r_i_v_a_t_i_v_e │ │ │ │ │ +static MatrixDD ExpmapDerivative(const TangentVector &omega) │ │ │ │ │ +Derivative of Expmap, currently only defined for SO3. │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_<_ _3_ _>_:_:_L_o_g_m_a_p_D_e_r_i_v_a_t_i_v_e │ │ │ │ │ +static MatrixDD LogmapDerivative(const TangentVector &omega) │ │ │ │ │ +Derivative of Logmap, currently only defined for SO3. │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:82 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _P_o_i_n_t_2_._h │ │ │ │ │ + * _Q_u_a_t_e_r_n_i_o_n_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00461.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/triangulation.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Bundler.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,193 +94,39 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs | │ │ │ │ Functions
│ │ │ │ -
triangulation.h File Reference
│ │ │ │ +
Cal3Bundler.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -

Functions for triangulation. │ │ │ │ -More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::TriangulationUnderconstrainedException
 Exception thrown by triangulateDLT when SVD returns rank < 3. More...
 
class  gtsam::TriangulationCheiralityException
 Exception thrown by triangulateDLT when landmark is behind one or more of the cameras. More...
 
struct  gtsam::TriangulationParameters
 
class  gtsam::TriangulationResult
 TriangulationResult is an optional point, along with the reasons why it is invalid. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Typedefs

│ │ │ │ -using gtsam::CameraSetCal3Bundler = CameraSet< PinholeCamera< Cal3Bundler > >
 
│ │ │ │ -using gtsam::CameraSetCal3_S2 = CameraSet< PinholeCamera< Cal3_S2 > >
 
│ │ │ │ -using gtsam::CameraSetCal3DS2 = CameraSet< PinholeCamera< Cal3DS2 > >
 
│ │ │ │ -using gtsam::CameraSetCal3Fisheye = CameraSet< PinholeCamera< Cal3Fisheye > >
 
│ │ │ │ -using gtsam::CameraSetCal3Unified = CameraSet< PinholeCamera< Cal3Unified > >
 
│ │ │ │ -using gtsam::CameraSetSpherical = CameraSet< SphericalCamera >
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

Vector4 gtsam::triangulateHomogeneousDLT (const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > &projection_matrices, const Point2Vector &measurements, double rank_tol=1e-9)
 DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312.
 
Vector4 gtsam::triangulateHomogeneousDLT (const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > &projection_matrices, const std::vector< Unit3 > &measurements, double rank_tol=1e-9)
 Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and Zisserman)
 
Point3 gtsam::triangulateDLT (const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > &projection_matrices, const Point2Vector &measurements, double rank_tol=1e-9)
 DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312.
 
│ │ │ │ -Point3 gtsam::triangulateDLT (const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > &projection_matrices, const std::vector< Unit3 > &measurements, double rank_tol=1e-9)
 overload of previous function to work with Unit3 (projected to canonical camera)
 
Point3 gtsam::triangulateLOST (const std::vector< Pose3 > &poses, const Point3Vector &calibratedMeasurements, const SharedIsotropic &measurementNoise)
 Triangulation using the LOST (Linear Optimal Sine Triangulation) algorithm proposed in https://arxiv.org/pdf/2205.12197.pdf by Sebastien Henry and John Christian.
 
template<class CALIBRATION >
std::pair< NonlinearFactorGraph, Values > gtsam::triangulationGraph (const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, Key landmarkKey, const Point3 &initialEstimate, const SharedNoiseModel &model=noiseModel::Unit::Create(2))
 Create a factor graph with projection factors from poses and one calibration.
 
template<class CAMERA >
std::pair< NonlinearFactorGraph, Values > gtsam::triangulationGraph (const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements, Key landmarkKey, const Point3 &initialEstimate, const SharedNoiseModel &model=nullptr)
 Create a factor graph with projection factors from pinhole cameras (each camera has a pose and calibration)
 
Point3 gtsam::optimize (const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
 Optimize for triangulation.
 
template<class CALIBRATION >
Point3 gtsam::triangulateNonlinear (const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, const Point3 &initialEstimate, const SharedNoiseModel &model=nullptr)
 Given an initial estimate , refine a point using measurements in several cameras.
 
template<class CAMERA >
Point3 gtsam::triangulateNonlinear (const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements, const Point3 &initialEstimate, const SharedNoiseModel &model=nullptr)
 Given an initial estimate , refine a point using measurements in several cameras.
 
│ │ │ │ -template<class CAMERA >
std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > gtsam::projectionMatricesFromCameras (const CameraSet< CAMERA > &cameras)
 
│ │ │ │ -template<class CALIBRATION >
std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > gtsam::projectionMatricesFromPoses (const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal)
 
template<class CALIBRATION >
Cal3_S2 gtsam::createPinholeCalibration (const CALIBRATION &cal)
 Create a pinhole calibration from a different Cal3 object, removing distortion.
 
│ │ │ │ -template<class CALIBRATION , class MEASUREMENT >
MEASUREMENT gtsam::undistortMeasurementInternal (const CALIBRATION &cal, const MEASUREMENT &measurement, boost::optional< Cal3_S2 > pinholeCal=boost::none)
 Internal undistortMeasurement to be used by undistortMeasurement and undistortMeasurements.
 
template<class CALIBRATION >
Point2Vector gtsam::undistortMeasurements (const CALIBRATION &cal, const Point2Vector &measurements)
 Remove distortion for measurements so as if the measurements came from a pinhole camera.
 
│ │ │ │ -template<>
Point2Vector gtsam::undistortMeasurements (const Cal3_S2 &cal, const Point2Vector &measurements)
 Specialization for Cal3_S2 as it doesn't need to be undistorted.
 
template<class CAMERA >
CAMERA::MeasurementVector gtsam::undistortMeasurements (const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements)
 Remove distortion for measurements so as if the measurements came from a pinhole camera.
 
│ │ │ │ -template<class CAMERA = PinholeCamera<Cal3_S2>>
PinholeCamera< Cal3_S2 >::MeasurementVector gtsam::undistortMeasurements (const CameraSet< PinholeCamera< Cal3_S2 > > &cameras, const PinholeCamera< Cal3_S2 >::MeasurementVector &measurements)
 Specialize for Cal3_S2 to do nothing.
 
│ │ │ │ -template<class CAMERA = SphericalCamera>
SphericalCamera::MeasurementVector gtsam::undistortMeasurements (const CameraSet< SphericalCamera > &cameras, const SphericalCamera::MeasurementVector &measurements)
 Specialize for SphericalCamera to do nothing.
 
template<class CALIBRATION >
Point3Vector gtsam::calibrateMeasurementsShared (const CALIBRATION &cal, const Point2Vector &measurements)
 Convert pixel measurements in image to homogeneous measurements in the image plane using shared camera intrinsics.
 
template<class CAMERA >
Point3Vector gtsam::calibrateMeasurements (const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements)
 Convert pixel measurements in image to homogeneous measurements in the image plane using camera intrinsics of each measurement.
 
│ │ │ │ -template<class CAMERA = SphericalCamera>
Point3Vector gtsam::calibrateMeasurements (const CameraSet< SphericalCamera > &cameras, const SphericalCamera::MeasurementVector &measurements)
 Specialize for SphericalCamera to do nothing.
 
template<class CALIBRATION >
Point3 gtsam::triangulatePoint3 (const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, double rank_tol=1e-9, bool optimize=false, const SharedNoiseModel &model=nullptr, const bool useLOST=false)
 Function to triangulate 3D landmark point from an arbitrary number of poses (at least 2) using the DLT.
 
template<class CAMERA >
Point3 gtsam::triangulatePoint3 (const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements, double rank_tol=1e-9, bool optimize=false, const SharedNoiseModel &model=nullptr, const bool useLOST=false)
 Function to triangulate 3D landmark point from an arbitrary number of poses (at least 2) using the DLT.
 
│ │ │ │ -template<class CALIBRATION >
Point3 gtsam::triangulatePoint3 (const CameraSet< PinholeCamera< CALIBRATION > > &cameras, const Point2Vector &measurements, double rank_tol=1e-9, bool optimize=false, const SharedNoiseModel &model=nullptr, const bool useLOST=false)
 Pinhole-specific version.
 
│ │ │ │ -template<class CAMERA >
TriangulationResult gtsam::triangulateSafe (const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters &params)
 triangulateSafe: extensive checking of the outcome
 
│ │ │ │ +std::ostream & gtsam::operator<< (std::ostream &os, const Cal3Bundler &cal)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Functions for triangulation.

│ │ │ │ -
Date
July 31, 2013
│ │ │ │ -
Author
Chris Beall
│ │ │ │ -
│ │ │ │ -Akshay Krishnan
│ │ │ │ -
Date
July 31, 2013
│ │ │ │ -
Author
Chris Beall
│ │ │ │ -
│ │ │ │ -Luca Carlone
│ │ │ │ -
│ │ │ │ -Akshay Krishnan
│ │ │ │ +
Date
Sep 25, 2010
│ │ │ │ +
Author
ydjian
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,295 +1,23 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -triangulation.h File Reference │ │ │ │ │ -Functions for triangulation. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_U_n_d_e_r_c_o_n_s_t_r_a_i_n_e_d_E_x_c_e_p_t_i_o_n │ │ │ │ │ -  Exception thrown by triangulateDLT when SVD returns rank < 3. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n │ │ │ │ │ -  Exception thrown by triangulateDLT when landmark is behind one or more │ │ │ │ │ - of the cameras. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t │ │ │ │ │ -  _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t is an optional point, along with the reasons why │ │ │ │ │ - it is invalid. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Cal3Bundler.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -using  ggttssaamm::::CCaammeerraaSSeettCCaall33BBuunnddlleerr = _C_a_m_e_r_a_S_e_t< _P_i_n_h_o_l_e_C_a_m_e_r_a< _C_a_l_3_B_u_n_d_l_e_r > > │ │ │ │ │ -  │ │ │ │ │ -using  ggttssaamm::::CCaammeerraaSSeettCCaall33__SS22 = _C_a_m_e_r_a_S_e_t< _P_i_n_h_o_l_e_C_a_m_e_r_a< _C_a_l_3___S_2 > > │ │ │ │ │ -  │ │ │ │ │ -using  ggttssaamm::::CCaammeerraaSSeettCCaall33DDSS22 = _C_a_m_e_r_a_S_e_t< _P_i_n_h_o_l_e_C_a_m_e_r_a< _C_a_l_3_D_S_2 > > │ │ │ │ │ -  │ │ │ │ │ -using  ggttssaamm::::CCaammeerraaSSeettCCaall33FFiisshheeyyee = _C_a_m_e_r_a_S_e_t< _P_i_n_h_o_l_e_C_a_m_e_r_a< _C_a_l_3_F_i_s_h_e_y_e > > │ │ │ │ │ -  │ │ │ │ │ -using  ggttssaamm::::CCaammeerraaSSeettCCaall33UUnniiffiieedd = _C_a_m_e_r_a_S_e_t< _P_i_n_h_o_l_e_C_a_m_e_r_a< _C_a_l_3_U_n_i_f_i_e_d > > │ │ │ │ │ -  │ │ │ │ │ -using  ggttssaamm::::CCaammeerraaSSeettSSpphheerriiccaall = _C_a_m_e_r_a_S_e_t< _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a > │ │ │ │ │ -  │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ - Vector4  _g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_H_o_m_o_g_e_n_e_o_u_s_D_L_T (const │ │ │ │ │ - std::vector< Matrix34, Eigen:: │ │ │ │ │ - aligned_allocator< Matrix34 > > │ │ │ │ │ - &projection_matrices, const │ │ │ │ │ - Point2Vector &measurements, double │ │ │ │ │ - rank_tol=1e-9) │ │ │ │ │ -  DLT triangulation: See Hartley and │ │ │ │ │ - Zisserman, 2nd Ed., page 312. │ │ │ │ │ -  │ │ │ │ │ - Vector4  _g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_H_o_m_o_g_e_n_e_o_u_s_D_L_T (const │ │ │ │ │ - std::vector< Matrix34, Eigen:: │ │ │ │ │ - aligned_allocator< Matrix34 > > │ │ │ │ │ - &projection_matrices, const std:: │ │ │ │ │ - vector< _U_n_i_t_3 > &measurements, double │ │ │ │ │ - rank_tol=1e-9) │ │ │ │ │ - Same math as Hartley and Zisserman, 2nd │ │ │ │ │ - Ed., page 312, but with unit-norm │ │ │ │ │ -  bearing vectors (contrarily to pinhole │ │ │ │ │ - projection, the z entry is not assumed │ │ │ │ │ - to be 1 as in Hartley and Zisserman) │ │ │ │ │ -  │ │ │ │ │ - _P_o_i_n_t_3  _g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_D_L_T (const std:: │ │ │ │ │ - vector< Matrix34, Eigen:: │ │ │ │ │ - aligned_allocator< Matrix34 > > │ │ │ │ │ - &projection_matrices, const │ │ │ │ │ - Point2Vector &measurements, double │ │ │ │ │ - rank_tol=1e-9) │ │ │ │ │ -  DLT triangulation: See Hartley and │ │ │ │ │ - Zisserman, 2nd Ed., page 312. │ │ │ │ │ -  │ │ │ │ │ - _P_o_i_n_t_3  ggttssaamm::::ttrriiaanngguullaatteeDDLLTT (const std:: │ │ │ │ │ - vector< Matrix34, Eigen:: │ │ │ │ │ - aligned_allocator< Matrix34 > > │ │ │ │ │ - &projection_matrices, const std:: │ │ │ │ │ - vector< _U_n_i_t_3 > &measurements, double │ │ │ │ │ - rank_tol=1e-9) │ │ │ │ │ - overload of previous function to work │ │ │ │ │ -  with _U_n_i_t_3 (projected to canonical │ │ │ │ │ - camera) │ │ │ │ │ -  │ │ │ │ │ - _P_o_i_n_t_3  _g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_L_O_S_T (const std:: │ │ │ │ │ - vector< _P_o_s_e_3 > &poses, const │ │ │ │ │ - Point3Vector &calibratedMeasurements, │ │ │ │ │ - const SharedIsotropic │ │ │ │ │ - &measurementNoise) │ │ │ │ │ - Triangulation using the LOST (Linear │ │ │ │ │ - Optimal Sine Triangulation) algorithm │ │ │ │ │ -  proposed in _h_t_t_p_s_:_/_/_a_r_x_i_v_._o_r_g_/_p_d_f_/ │ │ │ │ │ - _2_2_0_5_._1_2_1_9_7_._p_d_f by Sebastien Henry and │ │ │ │ │ - John Christian. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -std::pair< _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h, _V_a_l_u_e_s _g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_i_o_n_G_r_a_p_h (const std:: │ │ │ │ │ - >  vector< _P_o_s_e_3 > &poses, boost:: │ │ │ │ │ - shared_ptr< CALIBRATION > sharedCal, │ │ │ │ │ - const Point2Vector &measurements, _K_e_y │ │ │ │ │ - landmarkKey, const _P_o_i_n_t_3 │ │ │ │ │ - &initialEstimate, const │ │ │ │ │ - _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model=_n_o_i_s_e_M_o_d_e_l_:_: │ │ │ │ │ - _U_n_i_t_:_:_C_r_e_a_t_e(2)) │ │ │ │ │ -  Create a factor graph with projection │ │ │ │ │ - factors from poses and one calibration. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -std::pair< _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h, _V_a_l_u_e_s _g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_i_o_n_G_r_a_p_h (const │ │ │ │ │ - >  _C_a_m_e_r_a_S_e_t< CAMERA > &cameras, const │ │ │ │ │ - typename CAMERA::MeasurementVector │ │ │ │ │ - &measurements, _K_e_y landmarkKey, const │ │ │ │ │ - _P_o_i_n_t_3 &initialEstimate, const │ │ │ │ │ - _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model=nullptr) │ │ │ │ │ - Create a factor graph with projection │ │ │ │ │ -  factors from pinhole cameras (each │ │ │ │ │ - camera has a pose and calibration) │ │ │ │ │ -  │ │ │ │ │ - _P_o_i_n_t_3  _g_t_s_a_m_:_:_o_p_t_i_m_i_z_e (const │ │ │ │ │ - _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h &graph, const │ │ │ │ │ - _V_a_l_u_e_s &values, _K_e_y landmarkKey) │ │ │ │ │ -  Optimize for triangulation. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - _P_o_i_n_t_3  _g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_N_o_n_l_i_n_e_a_r (const │ │ │ │ │ - std::vector< _P_o_s_e_3 > &poses, boost:: │ │ │ │ │ - shared_ptr< CALIBRATION > sharedCal, │ │ │ │ │ - const Point2Vector &measurements, const │ │ │ │ │ - _P_o_i_n_t_3 &initialEstimate, const │ │ │ │ │ - _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model=nullptr) │ │ │ │ │ - Given an initial estimate , refine a │ │ │ │ │ -  point using measurements in several │ │ │ │ │ - cameras. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - _P_o_i_n_t_3  _g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_N_o_n_l_i_n_e_a_r (const │ │ │ │ │ - _C_a_m_e_r_a_S_e_t< CAMERA > &cameras, const │ │ │ │ │ - typename CAMERA::MeasurementVector │ │ │ │ │ - &measurements, const _P_o_i_n_t_3 │ │ │ │ │ - &initialEstimate, const │ │ │ │ │ - _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model=nullptr) │ │ │ │ │ - Given an initial estimate , refine a │ │ │ │ │ -  point using measurements in several │ │ │ │ │ - cameras. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - std::vector< Matrix34, Eigen:: ggttssaamm::::pprroojjeeccttiioonnMMaattrriicceessFFrroommCCaammeerraass │ │ │ │ │ - aligned_allocator< Matrix34 > >  (const _C_a_m_e_r_a_S_e_t< CAMERA > &cameras) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - std::vector< Matrix34, Eigen:: ggttssaamm::::pprroojjeeccttiioonnMMaattrriicceessFFrroommPPoosseess │ │ │ │ │ - aligned_allocator< Matrix34 > >  (const std::vector< _P_o_s_e_3 > &poses, │ │ │ │ │ - boost::shared_ptr< CALIBRATION > │ │ │ │ │ - sharedCal) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - _C_a_l_3___S_2  _g_t_s_a_m_:_:_c_r_e_a_t_e_P_i_n_h_o_l_e_C_a_l_i_b_r_a_t_i_o_n (const │ │ │ │ │ - CALIBRATION &cal) │ │ │ │ │ - Create a pinhole calibration from a │ │ │ │ │ -  different _C_a_l_3 object, removing │ │ │ │ │ - distortion. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - MEASUREMENT  ggttssaamm::::uunnddiissttoorrttMMeeaassuurreemmeennttIInntteerrnnaall │ │ │ │ │ - (const CALIBRATION &cal, const │ │ │ │ │ - MEASUREMENT &measurement, boost:: │ │ │ │ │ - optional< _C_a_l_3___S_2 > pinholeCal=boost:: │ │ │ │ │ - none) │ │ │ │ │ - Internal undistortMeasurement to be │ │ │ │ │ -  used by undistortMeasurement and │ │ │ │ │ - undistortMeasurements. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - Point2Vector  _g_t_s_a_m_:_:_u_n_d_i_s_t_o_r_t_M_e_a_s_u_r_e_m_e_n_t_s (const │ │ │ │ │ - CALIBRATION &cal, const Point2Vector │ │ │ │ │ - &measurements) │ │ │ │ │ - Remove distortion for measurements so │ │ │ │ │ -  as if the measurements came from a │ │ │ │ │ - pinhole camera. │ │ │ │ │ -  │ │ │ │ │ - template<> │ │ │ │ │ - Point2Vector  ggttssaamm::::uunnddiissttoorrttMMeeaassuurreemmeennttss (const │ │ │ │ │ - _C_a_l_3___S_2 &cal, const Point2Vector │ │ │ │ │ - &measurements) │ │ │ │ │ -  Specialization for _C_a_l_3___S_2 as it │ │ │ │ │ - doesn't need to be undistorted. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - CAMERA::MeasurementVector  _g_t_s_a_m_:_:_u_n_d_i_s_t_o_r_t_M_e_a_s_u_r_e_m_e_n_t_s (const │ │ │ │ │ - _C_a_m_e_r_a_S_e_t< CAMERA > &cameras, const │ │ │ │ │ - typename CAMERA::MeasurementVector │ │ │ │ │ - &measurements) │ │ │ │ │ - Remove distortion for measurements so │ │ │ │ │ -  as if the measurements came from a │ │ │ │ │ - pinhole camera. │ │ │ │ │ -  │ │ │ │ │ -template> │ │ │ │ │ - _P_i_n_h_o_l_e_C_a_m_e_r_a< _C_a_l_3___S_2 >:: ggttssaamm::::uunnddiissttoorrttMMeeaassuurreemmeennttss (const │ │ │ │ │ - MeasurementVector  _C_a_m_e_r_a_S_e_t< _P_i_n_h_o_l_e_C_a_m_e_r_a< _C_a_l_3___S_2 > > │ │ │ │ │ - &cameras, const _P_i_n_h_o_l_e_C_a_m_e_r_a< _C_a_l_3___S_2 │ │ │ │ │ - >::MeasurementVector &measurements) │ │ │ │ │ -  Specialize for _C_a_l_3___S_2 to do nothing. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - SphericalCamera::MeasurementVector  ggttssaamm::::uunnddiissttoorrttMMeeaassuurreemmeennttss (const │ │ │ │ │ - _C_a_m_e_r_a_S_e_t< _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a > &cameras, │ │ │ │ │ - const SphericalCamera:: │ │ │ │ │ - MeasurementVector &measurements) │ │ │ │ │ -  Specialize for _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a to do │ │ │ │ │ - nothing. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - Point3Vector  _g_t_s_a_m_:_:_c_a_l_i_b_r_a_t_e_M_e_a_s_u_r_e_m_e_n_t_s_S_h_a_r_e_d │ │ │ │ │ - (const CALIBRATION &cal, const │ │ │ │ │ - Point2Vector &measurements) │ │ │ │ │ - Convert pixel measurements in image to │ │ │ │ │ -  homogeneous measurements in the image │ │ │ │ │ - plane using shared camera intrinsics. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - Point3Vector  _g_t_s_a_m_:_:_c_a_l_i_b_r_a_t_e_M_e_a_s_u_r_e_m_e_n_t_s (const │ │ │ │ │ - _C_a_m_e_r_a_S_e_t< CAMERA > &cameras, const │ │ │ │ │ - typename CAMERA::MeasurementVector │ │ │ │ │ - &measurements) │ │ │ │ │ - Convert pixel measurements in image to │ │ │ │ │ -  homogeneous measurements in the image │ │ │ │ │ - plane using camera intrinsics of each │ │ │ │ │ - measurement. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - Point3Vector  ggttssaamm::::ccaalliibbrraatteeMMeeaassuurreemmeennttss (const │ │ │ │ │ - _C_a_m_e_r_a_S_e_t< _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a > &cameras, │ │ │ │ │ - const SphericalCamera:: │ │ │ │ │ - MeasurementVector &measurements) │ │ │ │ │ -  Specialize for _S_p_h_e_r_i_c_a_l_C_a_m_e_r_a to do │ │ │ │ │ - nothing. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - _P_o_i_n_t_3  _g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_P_o_i_n_t_3 (const std:: │ │ │ │ │ - vector< _P_o_s_e_3 > &poses, boost:: │ │ │ │ │ - shared_ptr< CALIBRATION > sharedCal, │ │ │ │ │ - const Point2Vector &measurements, │ │ │ │ │ - double rank_tol=1e-9, bool │ │ │ │ │ - _o_p_t_i_m_i_z_e=false, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ - &model=nullptr, const bool │ │ │ │ │ - useLOST=false) │ │ │ │ │ - Function to triangulate 3D landmark │ │ │ │ │ -  point from an arbitrary number of poses │ │ │ │ │ - (at least 2) using the DLT. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - _P_o_i_n_t_3  _g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_P_o_i_n_t_3 (const │ │ │ │ │ - _C_a_m_e_r_a_S_e_t< CAMERA > &cameras, const │ │ │ │ │ - typename CAMERA::MeasurementVector │ │ │ │ │ - &measurements, double rank_tol=1e-9, │ │ │ │ │ - bool _o_p_t_i_m_i_z_e=false, const │ │ │ │ │ - _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model=nullptr, const │ │ │ │ │ - bool useLOST=false) │ │ │ │ │ - Function to triangulate 3D landmark │ │ │ │ │ -  point from an arbitrary number of poses │ │ │ │ │ - (at least 2) using the DLT. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - _P_o_i_n_t_3  ggttssaamm::::ttrriiaanngguullaatteePPooiinntt33 (const │ │ │ │ │ - _C_a_m_e_r_a_S_e_t< _P_i_n_h_o_l_e_C_a_m_e_r_a< CALIBRATION > │ │ │ │ │ - > &cameras, const Point2Vector │ │ │ │ │ - &measurements, double rank_tol=1e-9, │ │ │ │ │ - bool _o_p_t_i_m_i_z_e=false, const │ │ │ │ │ - _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model=nullptr, const │ │ │ │ │ - bool useLOST=false) │ │ │ │ │ -  Pinhole-specific version. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t  ggttssaamm::::ttrriiaanngguullaatteeSSaaffee (const │ │ │ │ │ - _C_a_m_e_r_a_S_e_t< CAMERA > &cameras, const │ │ │ │ │ - typename CAMERA::MeasurementVector │ │ │ │ │ - &measured, const │ │ │ │ │ - _T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s ¶ms) │ │ │ │ │ -  triangulateSafe: extensive checking of │ │ │ │ │ - the outcome │ │ │ │ │ +std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _C_a_l_3_B_u_n_d_l_e_r &cal) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Functions for triangulation. │ │ │ │ │ - Date │ │ │ │ │ - July 31, 2013 │ │ │ │ │ - Author │ │ │ │ │ - Chris Beall │ │ │ │ │ - Akshay Krishnan │ │ │ │ │ Date │ │ │ │ │ - July 31, 2013 │ │ │ │ │ + Sep 25, 2010 │ │ │ │ │ Author │ │ │ │ │ - Chris Beall │ │ │ │ │ - Luca Carlone │ │ │ │ │ - Akshay Krishnan │ │ │ │ │ + ydjian │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _t_r_i_a_n_g_u_l_a_t_i_o_n_._h │ │ │ │ │ + * _C_a_l_3_B_u_n_d_l_e_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00464_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/OrientedPlane3.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/CameraSet.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,178 +98,494 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
OrientedPlane3.h
│ │ │ │ +
CameraSet.h
│ │ │ │
│ │ │ │
│ │ │ │ -
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * Atlanta, Georgia 30332-0415
│ │ │ │ -
4 * All Rights Reserved
│ │ │ │ -
5 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
4 * Atlanta, Georgia 30332-0415
│ │ │ │ +
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
12/*
│ │ │ │ -
13 * @file OrientedPlane3.h
│ │ │ │ -
14 * @date Dec 19, 2013
│ │ │ │ -
15 * @author Alex Trevor
│ │ │ │ -
16 * @author Frank Dellaert
│ │ │ │ -
17 * @author Zhaoyang Lv
│ │ │ │ -
18 * @brief An infinite plane, represented by a normal direction and perpendicular distance
│ │ │ │ -
19 */
│ │ │ │ +
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#pragma once
│ │ │ │ -
22
│ │ │ │ -
23#include <gtsam/geometry/Unit3.h>
│ │ │ │ -
24#include <gtsam/geometry/Pose3.h>
│ │ │ │ -
25#include <string>
│ │ │ │ -
26
│ │ │ │ -
27namespace gtsam {
│ │ │ │ -
28
│ │ │ │ +
21#include <gtsam/base/FastMap.h>
│ │ │ │ +
22#include <gtsam/base/SymmetricBlockMatrix.h>
│ │ │ │ +
23#include <gtsam/base/Testable.h>
│ │ │ │ +
24#include <gtsam/geometry/CalibratedCamera.h> // for Cheirality exception
│ │ │ │ +
25#include <gtsam/geometry/Point3.h>
│ │ │ │ +
26#include <gtsam/inference/Key.h>
│ │ │ │ +
27
│ │ │ │ +
28#include <vector>
│ │ │ │ +
29
│ │ │ │ +
30namespace gtsam {
│ │ │ │ +
31
│ │ │ │ +
35template <class CAMERA>
│ │ │ │
│ │ │ │ -
36class GTSAM_EXPORT OrientedPlane3 {
│ │ │ │ -
37private:
│ │ │ │ -
38 Unit3 n_;
│ │ │ │ -
39 double d_;
│ │ │ │ -
40
│ │ │ │ -
41public:
│ │ │ │ -
42 enum {
│ │ │ │ -
43 dimension = 3
│ │ │ │ -
44 };
│ │ │ │ -
45
│ │ │ │ -
48
│ │ │ │ -
│ │ │ │ -
50 OrientedPlane3() :
│ │ │ │ -
51 n_(), d_(0.0) {
│ │ │ │ -
52 }
│ │ │ │ -
│ │ │ │ -
53
│ │ │ │ -
│ │ │ │ -
55 OrientedPlane3(const Unit3& n, double d) :
│ │ │ │ -
56 n_(n), d_(d) {
│ │ │ │ -
57 }
│ │ │ │ -
│ │ │ │ -
58
│ │ │ │ -
│ │ │ │ -
60 explicit OrientedPlane3(const Vector4& vec)
│ │ │ │ -
61 : n_(vec(0), vec(1), vec(2)), d_(vec(3)) {}
│ │ │ │ -
│ │ │ │ -
62
│ │ │ │ -
│ │ │ │ -
64 OrientedPlane3(double a, double b, double c, double d) {
│ │ │ │ -
65 n_ = Unit3(a, b, c);
│ │ │ │ -
66 d_ = d;
│ │ │ │ -
67 }
│ │ │ │ -
│ │ │ │ -
68
│ │ │ │ +
36class CameraSet : public std::vector<CAMERA, Eigen::aligned_allocator<CAMERA>> {
│ │ │ │ +
37 protected:
│ │ │ │ +
38 using Base = std::vector<CAMERA, typename Eigen::aligned_allocator<CAMERA>>;
│ │ │ │ +
39
│ │ │ │ +
44 typedef typename CAMERA::Measurement Z;
│ │ │ │ +
45 typedef typename CAMERA::MeasurementVector ZVector;
│ │ │ │ +
46
│ │ │ │ +
47 static const int D = traits<CAMERA>::dimension;
│ │ │ │ +
48 static const int ZDim = traits<Z>::dimension;
│ │ │ │ +
49
│ │ │ │ +
│ │ │ │ +
51 static Vector ErrorVector(const ZVector& predicted, const ZVector& measured) {
│ │ │ │ +
52 // Check size
│ │ │ │ +
53 size_t m = predicted.size();
│ │ │ │ +
54 if (measured.size() != m)
│ │ │ │ +
55 throw std::runtime_error("CameraSet::errors: size mismatch");
│ │ │ │ +
56
│ │ │ │ +
57 // Project and fill error vector
│ │ │ │ +
58 Vector b(ZDim * m);
│ │ │ │ +
59 for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
│ │ │ │ +
60 Vector bi = traits<Z>::Local(measured[i], predicted[i]);
│ │ │ │ +
61 if (ZDim == 3 && std::isnan(bi(1))) { // if it is a stereo point and the
│ │ │ │ +
62 // right pixel is missing (nan)
│ │ │ │ +
63 bi(1) = 0;
│ │ │ │ +
64 }
│ │ │ │ +
65 b.segment<ZDim>(row) = bi;
│ │ │ │ +
66 }
│ │ │ │ +
67 return b;
│ │ │ │ +
68 }
│ │ │ │ +
│ │ │ │ +
69
│ │ │ │ +
70 public:
│ │ │ │ +
71 using Base::Base; // Inherit the vector constructors
│ │ │ │
72
│ │ │ │ -
74 void print(const std::string& s = std::string()) const;
│ │ │ │ +
74 virtual ~CameraSet() = default;
│ │ │ │
75
│ │ │ │ -
│ │ │ │ -
77 bool equals(const OrientedPlane3& s, double tol = 1e-9) const {
│ │ │ │ -
78 return (n_.equals(s.n_, tol) && (std::abs(d_ - s.d_) < tol));
│ │ │ │ -
79 }
│ │ │ │ -
│ │ │ │ -
80
│ │ │ │ -
82
│ │ │ │ -
89 OrientedPlane3 transform(const Pose3& xr,
│ │ │ │ -
90 OptionalJacobian<3, 3> Hp = boost::none,
│ │ │ │ -
91 OptionalJacobian<3, 6> Hr = boost::none) const;
│ │ │ │ -
92
│ │ │ │ -
100 Vector3 errorVector(const OrientedPlane3& other,
│ │ │ │ -
101 OptionalJacobian<3, 3> H1 = boost::none,
│ │ │ │ -
102 OptionalJacobian<3, 3> H2 = boost::none) const;
│ │ │ │ -
103
│ │ │ │ -
│ │ │ │ -
105 inline static size_t Dim() {
│ │ │ │ -
106 return 3;
│ │ │ │ -
107 }
│ │ │ │ -
│ │ │ │ -
108
│ │ │ │ -
│ │ │ │ -
110 inline size_t dim() const {
│ │ │ │ -
111 return 3;
│ │ │ │ -
112 }
│ │ │ │ -
│ │ │ │ -
113
│ │ │ │ -
115 OrientedPlane3 retract(const Vector3& v,
│ │ │ │ -
116 OptionalJacobian<3, 3> H = boost::none) const;
│ │ │ │ +
77 using MatrixZD = Eigen::Matrix<double, ZDim, D>;
│ │ │ │ +
78 using FBlocks = std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD>>;
│ │ │ │ +
79
│ │ │ │ +
│ │ │ │ +
85 virtual void print(const std::string& s = "") const {
│ │ │ │ +
86 std::cout << s << "CameraSet, cameras = \n";
│ │ │ │ +
87 for (size_t k = 0; k < this->size(); ++k) this->at(k).print(s);
│ │ │ │ +
88 }
│ │ │ │ +
│ │ │ │ +
89
│ │ │ │ +
│ │ │ │ +
91 bool equals(const CameraSet& p, double tol = 1e-9) const {
│ │ │ │ +
92 if (this->size() != p.size()) return false;
│ │ │ │ +
93 bool camerasAreEqual = true;
│ │ │ │ +
94 for (size_t i = 0; i < this->size(); i++) {
│ │ │ │ +
95 if (this->at(i).equals(p.at(i), tol) == false) camerasAreEqual = false;
│ │ │ │ +
96 break;
│ │ │ │ +
97 }
│ │ │ │ +
98 return camerasAreEqual;
│ │ │ │ +
99 }
│ │ │ │ +
│ │ │ │ +
100
│ │ │ │ +
107 template <class POINT>
│ │ │ │ +
│ │ │ │ +
108 ZVector project2(const POINT& point, //
│ │ │ │ +
109 boost::optional<FBlocks&> Fs = boost::none, //
│ │ │ │ +
110 boost::optional<Matrix&> E = boost::none) const {
│ │ │ │ +
111 static const int N = FixedDimension<POINT>::value;
│ │ │ │ +
112
│ │ │ │ +
113 // Allocate result
│ │ │ │ +
114 size_t m = this->size();
│ │ │ │ +
115 ZVector z;
│ │ │ │ +
116 z.reserve(m);
│ │ │ │
117
│ │ │ │ -
119 Vector3 localCoordinates(const OrientedPlane3& s) const;
│ │ │ │ -
120
│ │ │ │ -
│ │ │ │ -
122 inline Vector4 planeCoefficients() const {
│ │ │ │ -
123 Vector3 unit_vec = n_.unitVector();
│ │ │ │ -
124 return Vector4(unit_vec[0], unit_vec[1], unit_vec[2], d_);
│ │ │ │ -
125 }
│ │ │ │ -
│ │ │ │ -
126
│ │ │ │ -
│ │ │ │ -
128 inline Unit3 normal(OptionalJacobian<2, 3> H = boost::none) const {
│ │ │ │ -
129 if (H) *H << I_2x2, Z_2x1;
│ │ │ │ -
130 return n_;
│ │ │ │ -
131 }
│ │ │ │ -
│ │ │ │ -
132
│ │ │ │ -
│ │ │ │ -
134 inline double distance(OptionalJacobian<1, 3> H = boost::none) const {
│ │ │ │ -
135 if (H) *H << 0,0,1;
│ │ │ │ -
136 return d_;
│ │ │ │ -
137 }
│ │ │ │ -
│ │ │ │ -
138};
│ │ │ │ -
│ │ │ │ -
139
│ │ │ │ -
│ │ │ │ -
140template<> struct traits<OrientedPlane3> : public internal::Manifold<
│ │ │ │ -
141OrientedPlane3> {
│ │ │ │ -
142};
│ │ │ │ -
│ │ │ │ -
143
│ │ │ │ -
│ │ │ │ -
144template<> struct traits<const OrientedPlane3> : public internal::Manifold<
│ │ │ │ -
145OrientedPlane3> {
│ │ │ │ -
146};
│ │ │ │ -
│ │ │ │ -
147
│ │ │ │ -
148} // namespace gtsam
│ │ │ │ -
149
│ │ │ │ -
Pose3.h
3D Pose
│ │ │ │ +
118 // Allocate derivatives
│ │ │ │ +
119 if (E) E->resize(ZDim * m, N);
│ │ │ │ +
120 if (Fs) Fs->resize(m);
│ │ │ │ +
121
│ │ │ │ +
122 // Project and fill derivatives
│ │ │ │ +
123 for (size_t i = 0; i < m; i++) {
│ │ │ │ +
124 MatrixZD Fi;
│ │ │ │ +
125 Eigen::Matrix<double, ZDim, N> Ei;
│ │ │ │ +
126 z.emplace_back(this->at(i).project2(point, Fs ? &Fi : 0, E ? &Ei : 0));
│ │ │ │ +
127 if (Fs) (*Fs)[i] = Fi;
│ │ │ │ +
128 if (E) E->block<ZDim, N>(ZDim * i, 0) = Ei;
│ │ │ │ +
129 }
│ │ │ │ +
130
│ │ │ │ +
131 return z;
│ │ │ │ +
132 }
│ │ │ │ +
│ │ │ │ +
133
│ │ │ │ +
135 template <class POINT>
│ │ │ │ +
│ │ │ │ +
136 Vector reprojectionError(const POINT& point, const ZVector& measured,
│ │ │ │ +
137 boost::optional<FBlocks&> Fs = boost::none, //
│ │ │ │ +
138 boost::optional<Matrix&> E = boost::none) const {
│ │ │ │ +
139 return ErrorVector(project2(point, Fs, E), measured);
│ │ │ │ +
140 }
│ │ │ │ +
│ │ │ │ +
141
│ │ │ │ +
148 template <int N,
│ │ │ │ +
149 int ND> // N = 2 or 3 (point dimension), ND is the camera dimension
│ │ │ │ +
│ │ │ │ +
150 static SymmetricBlockMatrix SchurComplement(
│ │ │ │ +
151 const std::vector<
│ │ │ │ +
152 Eigen::Matrix<double, ZDim, ND>,
│ │ │ │ +
153 Eigen::aligned_allocator<Eigen::Matrix<double, ZDim, ND>>>& Fs,
│ │ │ │ +
154 const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) {
│ │ │ │ +
155 // a single point is observed in m cameras
│ │ │ │ +
156 size_t m = Fs.size();
│ │ │ │ +
157
│ │ │ │ +
158 // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column
│ │ │ │ +
159 // with info vector)
│ │ │ │ +
160 size_t M1 = ND * m + 1;
│ │ │ │ +
161 std::vector<DenseIndex> dims(m + 1); // this also includes the b term
│ │ │ │ +
162 std::fill(dims.begin(), dims.end() - 1, ND);
│ │ │ │ +
163 dims.back() = 1;
│ │ │ │ +
164 SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1));
│ │ │ │ +
165
│ │ │ │ +
166 // Blockwise Schur complement
│ │ │ │ +
167 for (size_t i = 0; i < m; i++) { // for each camera
│ │ │ │ +
168
│ │ │ │ +
169 const Eigen::Matrix<double, ZDim, ND>& Fi = Fs[i];
│ │ │ │ +
170 const auto FiT = Fi.transpose();
│ │ │ │ +
171 const Eigen::Matrix<double, ZDim, N> Ei_P = //
│ │ │ │ +
172 E.block(ZDim * i, 0, ZDim, N) * P;
│ │ │ │ +
173
│ │ │ │ +
174 // D = (Dx2) * ZDim
│ │ │ │ +
175 augmentedHessian.setOffDiagonalBlock(
│ │ │ │ +
176 i, m,
│ │ │ │ +
177 FiT * b.segment<ZDim>(ZDim * i) // F' * b
│ │ │ │ +
178 -
│ │ │ │ +
179 FiT *
│ │ │ │ +
180 (Ei_P *
│ │ │ │ +
181 (E.transpose() *
│ │ │ │ +
182 b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
│ │ │ │ +
183
│ │ │ │ +
184 // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
│ │ │ │ +
185 augmentedHessian.setDiagonalBlock(
│ │ │ │ +
186 i,
│ │ │ │ +
187 FiT * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi));
│ │ │ │ +
188
│ │ │ │ +
189 // upper triangular part of the hessian
│ │ │ │ +
190 for (size_t j = i + 1; j < m; j++) { // for each camera
│ │ │ │ +
191 const Eigen::Matrix<double, ZDim, ND>& Fj = Fs[j];
│ │ │ │ +
192
│ │ │ │ +
193 // (DxD) = (Dx2) * ( (2x2) * (2xD) )
│ │ │ │ +
194 augmentedHessian.setOffDiagonalBlock(
│ │ │ │ +
195 i, j,
│ │ │ │ +
196 -FiT * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj));
│ │ │ │ +
197 }
│ │ │ │ +
198 } // end of for over cameras
│ │ │ │ +
199
│ │ │ │ +
200 augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm();
│ │ │ │ +
201 return augmentedHessian;
│ │ │ │ +
202 }
│ │ │ │ +
│ │ │ │ +
203
│ │ │ │ +
217 template <int N, int ND, int NDD>
│ │ │ │ +
│ │ │ │ +
218 static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks(
│ │ │ │ +
219 const std::vector<
│ │ │ │ +
220 Eigen::Matrix<double, ZDim, ND>,
│ │ │ │ +
221 Eigen::aligned_allocator<Eigen::Matrix<double, ZDim, ND>>>& Fs,
│ │ │ │ +
222 const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b,
│ │ │ │ +
223 const KeyVector& jacobianKeys, const KeyVector& hessianKeys) {
│ │ │ │ +
224 size_t nrNonuniqueKeys = jacobianKeys.size();
│ │ │ │ +
225 size_t nrUniqueKeys = hessianKeys.size();
│ │ │ │ +
226
│ │ │ │ +
227 // Marginalize point: note - we reuse the standard SchurComplement function.
│ │ │ │ +
228 SymmetricBlockMatrix augmentedHessian = SchurComplement<N, ND>(Fs, E, P, b);
│ │ │ │ +
229
│ │ │ │ +
230 // Pack into an Hessian factor, allow space for b term.
│ │ │ │ +
231 std::vector<DenseIndex> dims(nrUniqueKeys + 1);
│ │ │ │ +
232 std::fill(dims.begin(), dims.end() - 1, NDD);
│ │ │ │ +
233 dims.back() = 1;
│ │ │ │ +
234 SymmetricBlockMatrix augmentedHessianUniqueKeys;
│ │ │ │ +
235
│ │ │ │ +
236 // Deal with the fact that some blocks may share the same keys.
│ │ │ │ +
237 if (nrUniqueKeys == nrNonuniqueKeys) {
│ │ │ │ +
238 // Case when there is 1 calibration key per camera:
│ │ │ │ +
239 augmentedHessianUniqueKeys = SymmetricBlockMatrix(
│ │ │ │ +
240 dims, Matrix(augmentedHessian.selfadjointView()));
│ │ │ │ +
241 } else {
│ │ │ │ +
242 // When multiple cameras share a calibration we have to rearrange
│ │ │ │ +
243 // the results of the Schur complement matrix.
│ │ │ │ +
244 std::vector<DenseIndex> nonuniqueDims(nrNonuniqueKeys + 1); // includes b
│ │ │ │ +
245 std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, NDD);
│ │ │ │ +
246 nonuniqueDims.back() = 1;
│ │ │ │ +
247 augmentedHessian = SymmetricBlockMatrix(
│ │ │ │ +
248 nonuniqueDims, Matrix(augmentedHessian.selfadjointView()));
│ │ │ │ +
249
│ │ │ │ +
250 // Get map from key to location in the new augmented Hessian matrix (the
│ │ │ │ +
251 // one including only unique keys).
│ │ │ │ +
252 std::map<Key, size_t> keyToSlotMap;
│ │ │ │ +
253 for (size_t k = 0; k < nrUniqueKeys; k++) {
│ │ │ │ +
254 keyToSlotMap[hessianKeys[k]] = k;
│ │ │ │ +
255 }
│ │ │ │ +
256
│ │ │ │ +
257 // Initialize matrix to zero.
│ │ │ │ +
258 augmentedHessianUniqueKeys = SymmetricBlockMatrix(
│ │ │ │ +
259 dims, Matrix::Zero(NDD * nrUniqueKeys + 1, NDD * nrUniqueKeys + 1));
│ │ │ │ +
260
│ │ │ │ +
261 // Add contributions for each key: note this loops over the hessian with
│ │ │ │ +
262 // nonUnique keys (augmentedHessian) and populates an Hessian that only
│ │ │ │ +
263 // includes the unique keys (that is what we want to return).
│ │ │ │ +
264 for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows
│ │ │ │ +
265 Key key_i = jacobianKeys.at(i);
│ │ │ │ +
266
│ │ │ │ +
267 // Update information vector.
│ │ │ │ +
268 augmentedHessianUniqueKeys.updateOffDiagonalBlock(
│ │ │ │ +
269 keyToSlotMap[key_i], nrUniqueKeys,
│ │ │ │ +
270 augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys));
│ │ │ │ +
271
│ │ │ │ +
272 // Update blocks.
│ │ │ │ +
273 for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols
│ │ │ │ +
274 Key key_j = jacobianKeys.at(j);
│ │ │ │ +
275 if (i == j) {
│ │ │ │ +
276 augmentedHessianUniqueKeys.updateDiagonalBlock(
│ │ │ │ +
277 keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i));
│ │ │ │ +
278 } else { // (i < j)
│ │ │ │ +
279 if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) {
│ │ │ │ +
280 augmentedHessianUniqueKeys.updateOffDiagonalBlock(
│ │ │ │ +
281 keyToSlotMap[key_i], keyToSlotMap[key_j],
│ │ │ │ +
282 augmentedHessian.aboveDiagonalBlock(i, j));
│ │ │ │ +
283 } else {
│ │ │ │ +
284 augmentedHessianUniqueKeys.updateDiagonalBlock(
│ │ │ │ +
285 keyToSlotMap[key_i],
│ │ │ │ +
286 augmentedHessian.aboveDiagonalBlock(i, j) +
│ │ │ │ +
287 augmentedHessian.aboveDiagonalBlock(i, j).transpose());
│ │ │ │ +
288 }
│ │ │ │ +
289 }
│ │ │ │ +
290 }
│ │ │ │ +
291 }
│ │ │ │ +
292
│ │ │ │ +
293 // Update bottom right element of the matrix.
│ │ │ │ +
294 augmentedHessianUniqueKeys.updateDiagonalBlock(
│ │ │ │ +
295 nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys));
│ │ │ │ +
296 }
│ │ │ │ +
297 return augmentedHessianUniqueKeys;
│ │ │ │ +
298 }
│ │ │ │ +
│ │ │ │ +
299
│ │ │ │ +
306 template <int N> // N = 2 or 3
│ │ │ │ +
│ │ │ │ +
307 static SymmetricBlockMatrix SchurComplement(
│ │ │ │ +
308 const FBlocks& Fs, const Matrix& E, const Eigen::Matrix<double, N, N>& P,
│ │ │ │ +
309 const Vector& b) {
│ │ │ │ +
310 return SchurComplement<N, D>(Fs, E, P, b);
│ │ │ │ +
311 }
│ │ │ │ +
│ │ │ │ +
312
│ │ │ │ +
314 template <int N> // N = 2 or 3 (point dimension)
│ │ │ │ +
│ │ │ │ +
315 static void ComputePointCovariance(Eigen::Matrix<double, N, N>& P,
│ │ │ │ +
316 const Matrix& E, double lambda,
│ │ │ │ +
317 bool diagonalDamping = false) {
│ │ │ │ +
318 Matrix EtE = E.transpose() * E;
│ │ │ │ +
319
│ │ │ │ +
320 if (diagonalDamping) { // diagonal of the hessian
│ │ │ │ +
321 EtE.diagonal() += lambda * EtE.diagonal();
│ │ │ │ +
322 } else {
│ │ │ │ +
323 DenseIndex n = E.cols();
│ │ │ │ +
324 EtE += lambda * Eigen::MatrixXd::Identity(n, n);
│ │ │ │ +
325 }
│ │ │ │ +
326
│ │ │ │ +
327 P = (EtE).inverse();
│ │ │ │ +
328 }
│ │ │ │ +
│ │ │ │ +
329
│ │ │ │ +
│ │ │ │ +
331 static Matrix PointCov(const Matrix& E, const double lambda = 0.0,
│ │ │ │ +
332 bool diagonalDamping = false) {
│ │ │ │ +
333 if (E.cols() == 2) {
│ │ │ │ +
334 Matrix2 P2;
│ │ │ │ +
335 ComputePointCovariance<2>(P2, E, lambda, diagonalDamping);
│ │ │ │ +
336 return P2;
│ │ │ │ +
337 } else {
│ │ │ │ +
338 Matrix3 P3;
│ │ │ │ +
339 ComputePointCovariance<3>(P3, E, lambda, diagonalDamping);
│ │ │ │ +
340 return P3;
│ │ │ │ +
341 }
│ │ │ │ +
342 }
│ │ │ │ +
│ │ │ │ +
343
│ │ │ │ +
│ │ │ │ +
348 static SymmetricBlockMatrix SchurComplement(const FBlocks& Fblocks,
│ │ │ │ +
349 const Matrix& E, const Vector& b,
│ │ │ │ +
350 const double lambda = 0.0,
│ │ │ │ +
351 bool diagonalDamping = false) {
│ │ │ │ +
352 if (E.cols() == 2) {
│ │ │ │ +
353 Matrix2 P;
│ │ │ │ +
354 ComputePointCovariance<2>(P, E, lambda, diagonalDamping);
│ │ │ │ +
355 return SchurComplement<2>(Fblocks, E, P, b);
│ │ │ │ +
356 } else {
│ │ │ │ +
357 Matrix3 P;
│ │ │ │ +
358 ComputePointCovariance<3>(P, E, lambda, diagonalDamping);
│ │ │ │ +
359 return SchurComplement<3>(Fblocks, E, P, b);
│ │ │ │ +
360 }
│ │ │ │ +
361 }
│ │ │ │ +
│ │ │ │ +
362
│ │ │ │ +
368 template <int N> // N = 2 or 3 (point dimension)
│ │ │ │ +
│ │ │ │ +
369 static void UpdateSchurComplement(
│ │ │ │ +
370 const FBlocks& Fs, const Matrix& E, const Eigen::Matrix<double, N, N>& P,
│ │ │ │ +
371 const Vector& b, const KeyVector& allKeys, const KeyVector& keys,
│ │ │ │ +
372 /*output ->*/ SymmetricBlockMatrix& augmentedHessian) {
│ │ │ │ +
373 assert(keys.size() == Fs.size());
│ │ │ │ +
374 assert(keys.size() <= allKeys.size());
│ │ │ │ +
375
│ │ │ │ +
376 FastMap<Key, size_t> KeySlotMap;
│ │ │ │ +
377 for (size_t slot = 0; slot < allKeys.size(); slot++)
│ │ │ │ +
378 KeySlotMap.insert(std::make_pair(allKeys[slot], slot));
│ │ │ │ +
379
│ │ │ │ +
380 // Schur complement trick
│ │ │ │ +
381 // G = F' * F - F' * E * P * E' * F
│ │ │ │ +
382 // g = F' * (b - E * P * E' * b)
│ │ │ │ +
383
│ │ │ │ +
384 // a single point is observed in m cameras
│ │ │ │ +
385 size_t m = Fs.size(); // cameras observing current point
│ │ │ │ +
386 size_t M = (augmentedHessian.rows() - 1) / D; // all cameras in the group
│ │ │ │ +
387 assert(allKeys.size() == M);
│ │ │ │ +
388
│ │ │ │ +
389 // Blockwise Schur complement
│ │ │ │ +
390 for (size_t i = 0; i < m; i++) { // for each camera in the current factor
│ │ │ │ +
391
│ │ │ │ +
392 const MatrixZD& Fi = Fs[i];
│ │ │ │ +
393 const auto FiT = Fi.transpose();
│ │ │ │ +
394 const Eigen::Matrix<double, 2, N> Ei_P =
│ │ │ │ +
395 E.template block<ZDim, N>(ZDim * i, 0) * P;
│ │ │ │ +
396
│ │ │ │ +
397 // D = (DxZDim) * (ZDim)
│ │ │ │ +
398 // allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7)
│ │ │ │ +
399 // we should map those to a slot in the local (grouped) hessian
│ │ │ │ +
400 // (0,1,2,3,4) Key cameraKey_i = this->keys_[i];
│ │ │ │ +
401 DenseIndex aug_i = KeySlotMap.at(keys[i]);
│ │ │ │ +
402
│ │ │ │ +
403 // information vector - store previous vector
│ │ │ │ +
404 // vectorBlock = augmentedHessian(aug_i, aug_m).knownOffDiagonal();
│ │ │ │ +
405 // add contribution of current factor
│ │ │ │ +
406 augmentedHessian.updateOffDiagonalBlock(
│ │ │ │ +
407 aug_i, M,
│ │ │ │ +
408 FiT * b.segment<ZDim>(ZDim * i) // F' * b
│ │ │ │ +
409 -
│ │ │ │ +
410 FiT *
│ │ │ │ +
411 (Ei_P *
│ │ │ │ +
412 (E.transpose() *
│ │ │ │ +
413 b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
│ │ │ │ +
414
│ │ │ │ +
415 // (DxD) += (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
│ │ │ │ +
416 // add contribution of current factor
│ │ │ │ +
417 // TODO(gareth): Eigen doesn't let us pass the expression. Call eval() for
│ │ │ │ +
418 // now...
│ │ │ │ +
419 augmentedHessian.updateDiagonalBlock(
│ │ │ │ +
420 aug_i,
│ │ │ │ +
421 ((FiT *
│ │ │ │ +
422 (Fi -
│ │ │ │ +
423 Ei_P * E.template block<ZDim, N>(ZDim * i, 0).transpose() * Fi)))
│ │ │ │ +
424 .eval());
│ │ │ │ +
425
│ │ │ │ +
426 // upper triangular part of the hessian
│ │ │ │ +
427 for (size_t j = i + 1; j < m; j++) { // for each camera
│ │ │ │ +
428 const MatrixZD& Fj = Fs[j];
│ │ │ │ +
429
│ │ │ │ +
430 DenseIndex aug_j = KeySlotMap.at(keys[j]);
│ │ │ │ +
431
│ │ │ │ +
432 // (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) )
│ │ │ │ +
433 // off diagonal block - store previous block
│ │ │ │ +
434 // matrixBlock = augmentedHessian(aug_i, aug_j).knownOffDiagonal();
│ │ │ │ +
435 // add contribution of current factor
│ │ │ │ +
436 augmentedHessian.updateOffDiagonalBlock(
│ │ │ │ +
437 aug_i, aug_j,
│ │ │ │ +
438 -FiT * (Ei_P * E.template block<ZDim, N>(ZDim * j, 0).transpose() *
│ │ │ │ +
439 Fj));
│ │ │ │ +
440 }
│ │ │ │ +
441 } // end of for over cameras
│ │ │ │ +
442
│ │ │ │ +
443 augmentedHessian.diagonalBlock(M)(0, 0) += b.squaredNorm();
│ │ │ │ +
444 }
│ │ │ │ +
│ │ │ │ +
445
│ │ │ │ +
446 private:
│ │ │ │ +
448 friend class boost::serialization::access;
│ │ │ │ +
449 template <class ARCHIVE>
│ │ │ │ +
450 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ +
451 ar&(*this);
│ │ │ │ +
452 }
│ │ │ │ +
453
│ │ │ │ +
454 public:
│ │ │ │ +
455 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
456};
│ │ │ │ +
│ │ │ │ +
457
│ │ │ │ +
458template <class CAMERA>
│ │ │ │ +
459const int CameraSet<CAMERA>::D;
│ │ │ │ +
460
│ │ │ │ +
461template <class CAMERA>
│ │ │ │ +
462const int CameraSet<CAMERA>::ZDim;
│ │ │ │ +
463
│ │ │ │ +
464template <class CAMERA>
│ │ │ │ +
465struct traits<CameraSet<CAMERA>> : public Testable<CameraSet<CAMERA>> {};
│ │ │ │ +
466
│ │ │ │ +
467template <class CAMERA>
│ │ │ │ +
468struct traits<const CameraSet<CAMERA>> : public Testable<CameraSet<CAMERA>> {};
│ │ │ │ +
469
│ │ │ │ +
470} // namespace gtsam
│ │ │ │ +
FastMap.h
A thin wrapper around std::map that uses boost's fast_pool_allocator.
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
SymmetricBlockMatrix.h
Access to matrices via blocks of pre-defined sizes.
│ │ │ │ +
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ +
Point3.h
3D Point
│ │ │ │ +
CalibratedCamera.h
Calibrated camera for which only pose is unknown.
│ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │ +
gtsam::row
const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j)
Extracts a row view from a matrix that avoids a copy.
Definition Matrix.h:222
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
│ │ │ │ -
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
gtsam::OrientedPlane3
Represents an infinite plane in 3D, which is composed of a planar normal and its perpendicular distan...
Definition OrientedPlane3.h:36
│ │ │ │ -
gtsam::OrientedPlane3::OrientedPlane3
OrientedPlane3(double a, double b, double c, double d)
Construct from four numbers of plane coeffcients (a, b, c, d)
Definition OrientedPlane3.h:64
│ │ │ │ -
gtsam::OrientedPlane3::planeCoefficients
Vector4 planeCoefficients() const
Returns the plane coefficients.
Definition OrientedPlane3.h:122
│ │ │ │ -
gtsam::OrientedPlane3::OrientedPlane3
OrientedPlane3(const Unit3 &n, double d)
Construct from a Unit3 and a distance.
Definition OrientedPlane3.h:55
│ │ │ │ -
gtsam::OrientedPlane3::dim
size_t dim() const
Dimensionality of tangent space = 3 DOF.
Definition OrientedPlane3.h:110
│ │ │ │ -
gtsam::OrientedPlane3::equals
bool equals(const OrientedPlane3 &s, double tol=1e-9) const
The equals function with tolerance.
Definition OrientedPlane3.h:77
│ │ │ │ -
gtsam::OrientedPlane3::OrientedPlane3
OrientedPlane3(const Vector4 &vec)
Construct from a vector of plane coefficients.
Definition OrientedPlane3.h:60
│ │ │ │ -
gtsam::OrientedPlane3::Dim
static size_t Dim()
Dimensionality of tangent space = 3 DOF.
Definition OrientedPlane3.h:105
│ │ │ │ -
gtsam::OrientedPlane3::OrientedPlane3
OrientedPlane3()
Default constructor.
Definition OrientedPlane3.h:50
│ │ │ │ -
gtsam::OrientedPlane3::distance
double distance(OptionalJacobian< 1, 3 > H=boost::none) const
Return the perpendicular distance to the origin.
Definition OrientedPlane3.h:134
│ │ │ │ -
gtsam::OrientedPlane3::normal
Unit3 normal(OptionalJacobian< 2, 3 > H=boost::none) const
Return the normal.
Definition OrientedPlane3.h:128
│ │ │ │ -
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ -
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
│ │ │ │ -
gtsam::Unit3::unitVector
Vector3 unitVector(OptionalJacobian< 3, 2 > H=boost::none) const
Return unit-norm Vector.
Definition Unit3.cpp:151
│ │ │ │ -
gtsam::Unit3::equals
bool equals(const Unit3 &s, double tol=1e-9) const
The equals function with tolerance.
Definition Unit3.h:115
│ │ │ │ +
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ +
gtsam::FixedDimension
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition Manifold.h:164
│ │ │ │ +
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
│ │ │ │ +
gtsam::SymmetricBlockMatrix::setDiagonalBlock
void setDiagonalBlock(DenseIndex I, const XprType &xpr)
Set a diagonal block. Only the upper triangular portion of xpr is evaluated.
Definition SymmetricBlockMatrix.h:195
│ │ │ │ +
gtsam::SymmetricBlockMatrix::setOffDiagonalBlock
void setOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
Set an off-diagonal block. Only the upper triangular portion of xpr is evaluated.
Definition SymmetricBlockMatrix.h:201
│ │ │ │ +
gtsam::SymmetricBlockMatrix::aboveDiagonalBlock
constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
Get block above the diagonal (I, J).
Definition SymmetricBlockMatrix.h:150
│ │ │ │ +
gtsam::SymmetricBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition SymmetricBlockMatrix.h:114
│ │ │ │ +
gtsam::SymmetricBlockMatrix::updateOffDiagonalBlock
void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
Update an off diagonal block.
Definition SymmetricBlockMatrix.h:228
│ │ │ │ +
gtsam::SymmetricBlockMatrix::diagonalBlock
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J'th diagonal block as a self adjoint view.
Definition SymmetricBlockMatrix.h:135
│ │ │ │ +
gtsam::SymmetricBlockMatrix::updateDiagonalBlock
void updateDiagonalBlock(DenseIndex I, const XprType &xpr)
Increment the diagonal block by the values in xpr. Only reads the upper triangular part of xpr.
Definition SymmetricBlockMatrix.h:212
│ │ │ │ +
gtsam::SymmetricBlockMatrix::selfadjointView
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j).
Definition SymmetricBlockMatrix.h:156
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition CameraSet.h:36
│ │ │ │ +
gtsam::CameraSet::reprojectionError
Vector reprojectionError(const POINT &point, const ZVector &measured, boost::optional< FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const
Calculate vector [project2(point)-z] of re-projection errors.
Definition CameraSet.h:136
│ │ │ │ +
gtsam::CameraSet::print
virtual void print(const std::string &s="") const
print
Definition CameraSet.h:85
│ │ │ │ +
gtsam::CameraSet::ComputePointCovariance
static void ComputePointCovariance(Eigen::Matrix< double, N, N > &P, const Matrix &E, double lambda, bool diagonalDamping=false)
Computes Point Covariance P, with lambda parameter.
Definition CameraSet.h:315
│ │ │ │ +
gtsam::CameraSet::SchurComplement
static SymmetricBlockMatrix SchurComplement(const FBlocks &Fblocks, const Matrix &E, const Vector &b, const double lambda=0.0, bool diagonalDamping=false)
Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix Dynamic version.
Definition CameraSet.h:348
│ │ │ │ +
gtsam::CameraSet::Z
CAMERA::Measurement Z
2D measurement and noise model for each of the m views The order is kept the same as the keys that we...
Definition CameraSet.h:44
│ │ │ │ +
gtsam::CameraSet::~CameraSet
virtual ~CameraSet()=default
Destructor.
│ │ │ │ +
gtsam::CameraSet::equals
bool equals(const CameraSet &p, double tol=1e-9) const
equals
Definition CameraSet.h:91
│ │ │ │ +
gtsam::CameraSet::PointCov
static Matrix PointCov(const Matrix &E, const double lambda=0.0, bool diagonalDamping=false)
Computes Point Covariance P, with lambda parameter, dynamic version.
Definition CameraSet.h:331
│ │ │ │ +
gtsam::CameraSet::ErrorVector
static Vector ErrorVector(const ZVector &predicted, const ZVector &measured)
Make a vector of re-projection errors.
Definition CameraSet.h:51
│ │ │ │ +
gtsam::CameraSet::UpdateSchurComplement
static void UpdateSchurComplement(const FBlocks &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b, const KeyVector &allKeys, const KeyVector &keys, SymmetricBlockMatrix &augmentedHessian)
Applies Schur complement (exploiting block structure) to get a smart factor on cameras,...
Definition CameraSet.h:369
│ │ │ │ +
gtsam::CameraSet::SchurComplementAndRearrangeBlocks
static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks(const std::vector< Eigen::Matrix< double, ZDim, ND >, Eigen::aligned_allocator< Eigen::Matrix< double, ZDim, ND > > > &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b, const KeyVector &jacobianKeys, const KeyVector &hessianKeys)
Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = F' * F - F' * E * P * ...
Definition CameraSet.h:218
│ │ │ │ +
gtsam::CameraSet::SchurComplement
static SymmetricBlockMatrix SchurComplement(const std::vector< Eigen::Matrix< double, ZDim, ND >, Eigen::aligned_allocator< Eigen::Matrix< double, ZDim, ND > > > &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b)
Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = F' * F - F' * E * P * ...
Definition CameraSet.h:150
│ │ │ │ +
gtsam::CameraSet::ZDim
static const int ZDim
Measurement dimension.
Definition CameraSet.h:48
│ │ │ │ +
gtsam::CameraSet::SchurComplement
static SymmetricBlockMatrix SchurComplement(const FBlocks &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b)
Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = F' * F - F' * E * P * ...
Definition CameraSet.h:307
│ │ │ │ +
gtsam::CameraSet::D
static const int D
Camera dimension.
Definition CameraSet.h:47
│ │ │ │ +
gtsam::CameraSet::access
friend class boost::serialization::access
Serialization function.
Definition CameraSet.h:448
│ │ │ │ +
gtsam::CameraSet::MatrixZD
Eigen::Matrix< double, ZDim, D > MatrixZD
Definitions for blocks of F.
Definition CameraSet.h:77
│ │ │ │ +
gtsam::CameraSet::project2
ZVector project2(const POINT &point, boost::optional< FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const
Project a point (possibly Unit3 at infinity), with derivatives Note that F is a sparse block-diagonal...
Definition CameraSet.h:108
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,201 +1,606 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -OrientedPlane3.h │ │ │ │ │ +CameraSet.h │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -4 * All Rights Reserved │ │ │ │ │ -5 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -12/* │ │ │ │ │ -13 * @file OrientedPlane3.h │ │ │ │ │ -14 * @date Dec 19, 2013 │ │ │ │ │ -15 * @author Alex Trevor │ │ │ │ │ -16 * @author Frank Dellaert │ │ │ │ │ -17 * @author Zhaoyang Lv │ │ │ │ │ -18 * @brief An infinite plane, represented by a normal direction and │ │ │ │ │ -perpendicular distance │ │ │ │ │ -19 */ │ │ │ │ │ +19#pragma once │ │ │ │ │ 20 │ │ │ │ │ -21#pragma once │ │ │ │ │ -22 │ │ │ │ │ -23#include │ │ │ │ │ -24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_3_._h> │ │ │ │ │ -25#include │ │ │ │ │ -26 │ │ │ │ │ -27namespace _g_t_s_a_m { │ │ │ │ │ -28 │ │ │ │ │ -_3_6class GTSAM_EXPORT _O_r_i_e_n_t_e_d_P_l_a_n_e_3 { │ │ │ │ │ -37private: │ │ │ │ │ -38 _U_n_i_t_3 n_; │ │ │ │ │ -39 double d_; │ │ │ │ │ -40 │ │ │ │ │ -41public: │ │ │ │ │ -42 enum { │ │ │ │ │ -43 dimension = 3 │ │ │ │ │ -44 }; │ │ │ │ │ -45 │ │ │ │ │ -48 │ │ │ │ │ -_5_0 _O_r_i_e_n_t_e_d_P_l_a_n_e_3() : │ │ │ │ │ -51 n_(), d_(0.0) { │ │ │ │ │ -52 } │ │ │ │ │ -53 │ │ │ │ │ -_5_5 _O_r_i_e_n_t_e_d_P_l_a_n_e_3(const _U_n_i_t_3& n, double d) : │ │ │ │ │ -56 n_(n), d_(d) { │ │ │ │ │ -57 } │ │ │ │ │ -58 │ │ │ │ │ -_6_0 explicit _O_r_i_e_n_t_e_d_P_l_a_n_e_3(const Vector4& vec) │ │ │ │ │ -61 : n_(vec(0), vec(1), vec(2)), d_(vec(3)) {} │ │ │ │ │ -62 │ │ │ │ │ -_6_4 _O_r_i_e_n_t_e_d_P_l_a_n_e_3(double a, double b, double c, double d) { │ │ │ │ │ -65 n_ = _U_n_i_t_3(a, b, c); │ │ │ │ │ -66 d_ = d; │ │ │ │ │ -67 } │ │ │ │ │ -68 │ │ │ │ │ +21#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_M_a_p_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_._h> // for Cheirality exception │ │ │ │ │ +25#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_3_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ +27 │ │ │ │ │ +28#include │ │ │ │ │ +29 │ │ │ │ │ +30namespace _g_t_s_a_m { │ │ │ │ │ +31 │ │ │ │ │ +35template │ │ │ │ │ +_3_6class _C_a_m_e_r_a_S_e_t : public std::vector> { │ │ │ │ │ +37 protected: │ │ │ │ │ +38 using Base = std::vector>; │ │ │ │ │ +39 │ │ │ │ │ +_4_4 typedef typename CAMERA::Measurement _Z; │ │ │ │ │ +45 typedef typename CAMERA::MeasurementVector ZVector; │ │ │ │ │ +46 │ │ │ │ │ +_4_7 static const int _D = _t_r_a_i_t_s_<_C_A_M_E_R_A_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ +_4_8 static const int _Z_D_i_m = _t_r_a_i_t_s_<_Z_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ +49 │ │ │ │ │ +_5_1 static Vector _E_r_r_o_r_V_e_c_t_o_r(const ZVector& predicted, const ZVector& measured) │ │ │ │ │ +{ │ │ │ │ │ +52 // Check size │ │ │ │ │ +53 size_t m = predicted.size(); │ │ │ │ │ +54 if (measured.size() != m) │ │ │ │ │ +55 throw std::runtime_error("CameraSet::errors: size mismatch"); │ │ │ │ │ +56 │ │ │ │ │ +57 // Project and fill error vector │ │ │ │ │ +58 Vector b(_Z_D_i_m * m); │ │ │ │ │ +59 for (size_t i = 0, _r_o_w = 0; i < m; i++, _r_o_w += _Z_D_i_m) { │ │ │ │ │ +60 Vector bi = _t_r_a_i_t_s_<_Z_>_:_:_L_o_c_a_l(measured[i], predicted[i]); │ │ │ │ │ +61 if (_Z_D_i_m == 3 && std::isnan(bi(1))) { // if it is a stereo point and the │ │ │ │ │ +62 // right pixel is missing (nan) │ │ │ │ │ +63 bi(1) = 0; │ │ │ │ │ +64 } │ │ │ │ │ +65 b.segment<_Z_D_i_m>(_r_o_w) = bi; │ │ │ │ │ +66 } │ │ │ │ │ +67 return b; │ │ │ │ │ +68 } │ │ │ │ │ +69 │ │ │ │ │ +70 public: │ │ │ │ │ +71 using Base::Base; // Inherit the vector constructors │ │ │ │ │ 72 │ │ │ │ │ -74 void _p_r_i_n_t(const std::string& s = std::string()) const; │ │ │ │ │ +_7_4 virtual _~_C_a_m_e_r_a_S_e_t() = default; │ │ │ │ │ 75 │ │ │ │ │ -_7_7 bool _e_q_u_a_l_s(const _O_r_i_e_n_t_e_d_P_l_a_n_e_3& s, double tol = 1e-9) const { │ │ │ │ │ -78 return (n_._e_q_u_a_l_s(s.n_, tol) && (std::abs(d_ - s.d_) < tol)); │ │ │ │ │ -79 } │ │ │ │ │ -80 │ │ │ │ │ -82 │ │ │ │ │ -89 _O_r_i_e_n_t_e_d_P_l_a_n_e_3 transform(const _P_o_s_e_3& xr, │ │ │ │ │ -90 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> Hp = boost::none, │ │ │ │ │ -91 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _6_> Hr = boost::none) const; │ │ │ │ │ -92 │ │ │ │ │ -100 Vector3 errorVector(const _O_r_i_e_n_t_e_d_P_l_a_n_e_3& other, │ │ │ │ │ -101 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H1 = boost::none, │ │ │ │ │ -102 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H2 = boost::none) const; │ │ │ │ │ -103 │ │ │ │ │ -_1_0_5 inline static size_t _D_i_m() { │ │ │ │ │ -106 return 3; │ │ │ │ │ -107 } │ │ │ │ │ -108 │ │ │ │ │ -_1_1_0 inline size_t _d_i_m() const { │ │ │ │ │ -111 return 3; │ │ │ │ │ -112 } │ │ │ │ │ -113 │ │ │ │ │ -115 _O_r_i_e_n_t_e_d_P_l_a_n_e_3 retract(const Vector3& v, │ │ │ │ │ -116 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H = boost::none) const; │ │ │ │ │ +_7_7 using _M_a_t_r_i_x_Z_D = Eigen::Matrix; │ │ │ │ │ +78 using FBlocks = std::vector>; │ │ │ │ │ +79 │ │ │ │ │ +_8_5 virtual void _p_r_i_n_t(const std::string& s = "") const { │ │ │ │ │ +86 std::cout << s << "CameraSet, cameras = \n"; │ │ │ │ │ +87 for (size_t k = 0; k < this->size(); ++k) this->at(k).print(s); │ │ │ │ │ +88 } │ │ │ │ │ +89 │ │ │ │ │ +_9_1 bool _e_q_u_a_l_s(const _C_a_m_e_r_a_S_e_t& p, double tol = 1e-9) const { │ │ │ │ │ +92 if (this->size() != p.size()) return false; │ │ │ │ │ +93 bool camerasAreEqual = true; │ │ │ │ │ +94 for (size_t i = 0; i < this->size(); i++) { │ │ │ │ │ +95 if (this->at(i).equals(p.at(i), tol) == false) camerasAreEqual = false; │ │ │ │ │ +96 break; │ │ │ │ │ +97 } │ │ │ │ │ +98 return camerasAreEqual; │ │ │ │ │ +99 } │ │ │ │ │ +100 │ │ │ │ │ +107 template │ │ │ │ │ +_1_0_8 ZVector _p_r_o_j_e_c_t_2(const POINT& point, // │ │ │ │ │ +109 boost::optional Fs = boost::none, // │ │ │ │ │ +110 boost::optional E = boost::none) const { │ │ │ │ │ +111 static const int N = _F_i_x_e_d_D_i_m_e_n_s_i_o_n_<_P_O_I_N_T_>_:_:_v_a_l_u_e; │ │ │ │ │ +112 │ │ │ │ │ +113 // Allocate result │ │ │ │ │ +114 size_t m = this->size(); │ │ │ │ │ +115 ZVector z; │ │ │ │ │ +116 z.reserve(m); │ │ │ │ │ 117 │ │ │ │ │ -119 Vector3 localCoordinates(const _O_r_i_e_n_t_e_d_P_l_a_n_e_3& s) const; │ │ │ │ │ -120 │ │ │ │ │ -_1_2_2 inline Vector4 _p_l_a_n_e_C_o_e_f_f_i_c_i_e_n_t_s() const { │ │ │ │ │ -123 Vector3 unit_vec = n_._u_n_i_t_V_e_c_t_o_r(); │ │ │ │ │ -124 return Vector4(unit_vec[0], unit_vec[1], unit_vec[2], d_); │ │ │ │ │ -125 } │ │ │ │ │ -126 │ │ │ │ │ -_1_2_8 inline _U_n_i_t_3 _n_o_r_m_a_l(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> H = boost::none) const { │ │ │ │ │ -129 if (H) *H << I_2x2, Z_2x1; │ │ │ │ │ -130 return n_; │ │ │ │ │ -131 } │ │ │ │ │ -132 │ │ │ │ │ -_1_3_4 inline double _d_i_s_t_a_n_c_e(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> H = boost::none) const { │ │ │ │ │ -135 if (H) *H << 0,0,1; │ │ │ │ │ -136 return d_; │ │ │ │ │ -137 } │ │ │ │ │ -138}; │ │ │ │ │ -139 │ │ │ │ │ -_1_4_0template<> struct _t_r_a_i_t_s<_O_r_i_e_n_t_e_d_P_l_a_n_e_3> : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d< │ │ │ │ │ -141OrientedPlane3> { │ │ │ │ │ -142}; │ │ │ │ │ -143 │ │ │ │ │ -_1_4_4template<> struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d< │ │ │ │ │ -145OrientedPlane3> { │ │ │ │ │ -146}; │ │ │ │ │ -147 │ │ │ │ │ -148} // namespace gtsam │ │ │ │ │ -149 │ │ │ │ │ -_P_o_s_e_3_._h │ │ │ │ │ -3D Pose │ │ │ │ │ +118 // Allocate derivatives │ │ │ │ │ +119 if (E) E->resize(_Z_D_i_m * m, N); │ │ │ │ │ +120 if (Fs) Fs->resize(m); │ │ │ │ │ +121 │ │ │ │ │ +122 // Project and fill derivatives │ │ │ │ │ +123 for (size_t i = 0; i < m; i++) { │ │ │ │ │ +124 _M_a_t_r_i_x_Z_D Fi; │ │ │ │ │ +125 Eigen::Matrix Ei; │ │ │ │ │ +126 z.emplace_back(this->at(i)._p_r_o_j_e_c_t_2(point, Fs ? &Fi : 0, E ? &Ei : 0)); │ │ │ │ │ +127 if (Fs) (*Fs)[i] = Fi; │ │ │ │ │ +128 if (E) E->block<_Z_D_i_m, N>(_Z_D_i_m * i, 0) = Ei; │ │ │ │ │ +129 } │ │ │ │ │ +130 │ │ │ │ │ +131 return z; │ │ │ │ │ +132 } │ │ │ │ │ +133 │ │ │ │ │ +135 template │ │ │ │ │ +_1_3_6 Vector _r_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r(const POINT& point, const ZVector& measured, │ │ │ │ │ +137 boost::optional Fs = boost::none, // │ │ │ │ │ +138 boost::optional E = boost::none) const { │ │ │ │ │ +139 return _E_r_r_o_r_V_e_c_t_o_r(_p_r_o_j_e_c_t_2(point, Fs, E), measured); │ │ │ │ │ +140 } │ │ │ │ │ +141 │ │ │ │ │ +148 template // N = 2 or 3 (point dimension), ND is the camera dimension │ │ │ │ │ +_1_5_0 static _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x _S_c_h_u_r_C_o_m_p_l_e_m_e_n_t( │ │ │ │ │ +151 const std::vector< │ │ │ │ │ +152 Eigen::Matrix, │ │ │ │ │ +153 Eigen::aligned_allocator>>& Fs, │ │ │ │ │ +154 const Matrix& E, const Eigen::Matrix& P, const Vector& b) { │ │ │ │ │ +155 // a single point is observed in m cameras │ │ │ │ │ +156 size_t m = Fs.size(); │ │ │ │ │ +157 │ │ │ │ │ +158 // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column │ │ │ │ │ +159 // with info vector) │ │ │ │ │ +160 size_t M1 = ND * m + 1; │ │ │ │ │ +161 std::vector dims(m + 1); // this also includes the b term │ │ │ │ │ +162 std::fill(dims.begin(), dims.end() - 1, ND); │ │ │ │ │ +163 dims.back() = 1; │ │ │ │ │ +164 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x augmentedHessian(dims, Matrix::Zero(M1, M1)); │ │ │ │ │ +165 │ │ │ │ │ +166 // Blockwise Schur complement │ │ │ │ │ +167 for (size_t i = 0; i < m; i++) { // for each camera │ │ │ │ │ +168 │ │ │ │ │ +169 const Eigen::Matrix& Fi = Fs[i]; │ │ │ │ │ +170 const auto FiT = Fi.transpose(); │ │ │ │ │ +171 const Eigen::Matrix Ei_P = // │ │ │ │ │ +172 E.block(_Z_D_i_m * i, 0, _Z_D_i_m, N) * P; │ │ │ │ │ +173 │ │ │ │ │ +174 // D = (Dx2) * ZDim │ │ │ │ │ +175 augmentedHessian._s_e_t_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k( │ │ │ │ │ +176 i, m, │ │ │ │ │ +177 FiT * b.segment<_Z_D_i_m>(_Z_D_i_m * i) // F' * b │ │ │ │ │ +178 - │ │ │ │ │ +179 FiT * │ │ │ │ │ +180 (Ei_P * │ │ │ │ │ +181 (E.transpose() * │ │ │ │ │ +182 b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) │ │ │ │ │ +183 │ │ │ │ │ +184 // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) │ │ │ │ │ +185 augmentedHessian._s_e_t_D_i_a_g_o_n_a_l_B_l_o_c_k( │ │ │ │ │ +186 i, │ │ │ │ │ +187 FiT * (Fi - Ei_P * E.block(_Z_D_i_m * i, 0, _Z_D_i_m, N).transpose() * Fi)); │ │ │ │ │ +188 │ │ │ │ │ +189 // upper triangular part of the hessian │ │ │ │ │ +190 for (size_t j = i + 1; j < m; j++) { // for each camera │ │ │ │ │ +191 const Eigen::Matrix& Fj = Fs[j]; │ │ │ │ │ +192 │ │ │ │ │ +193 // (DxD) = (Dx2) * ( (2x2) * (2xD) ) │ │ │ │ │ +194 augmentedHessian._s_e_t_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k( │ │ │ │ │ +195 i, j, │ │ │ │ │ +196 -FiT * (Ei_P * E.block(_Z_D_i_m * j, 0, _Z_D_i_m, N).transpose() * Fj)); │ │ │ │ │ +197 } │ │ │ │ │ +198 } // end of for over cameras │ │ │ │ │ +199 │ │ │ │ │ +200 augmentedHessian._d_i_a_g_o_n_a_l_B_l_o_c_k(m)(0, 0) += b.squaredNorm(); │ │ │ │ │ +201 return augmentedHessian; │ │ │ │ │ +202 } │ │ │ │ │ +203 │ │ │ │ │ +217 template │ │ │ │ │ +_2_1_8 static _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x _S_c_h_u_r_C_o_m_p_l_e_m_e_n_t_A_n_d_R_e_a_r_r_a_n_g_e_B_l_o_c_k_s( │ │ │ │ │ +219 const std::vector< │ │ │ │ │ +220 Eigen::Matrix, │ │ │ │ │ +221 Eigen::aligned_allocator>>& Fs, │ │ │ │ │ +222 const Matrix& E, const Eigen::Matrix& P, const Vector& b, │ │ │ │ │ +223 const _K_e_y_V_e_c_t_o_r& jacobianKeys, const _K_e_y_V_e_c_t_o_r& hessianKeys) { │ │ │ │ │ +224 size_t nrNonuniqueKeys = jacobianKeys.size(); │ │ │ │ │ +225 size_t nrUniqueKeys = hessianKeys.size(); │ │ │ │ │ +226 │ │ │ │ │ +227 // Marginalize point: note - we reuse the standard SchurComplement │ │ │ │ │ +function. │ │ │ │ │ +228 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x augmentedHessian = SchurComplement(Fs, E, P, │ │ │ │ │ +b); │ │ │ │ │ +229 │ │ │ │ │ +230 // Pack into an Hessian factor, allow space for b term. │ │ │ │ │ +231 std::vector dims(nrUniqueKeys + 1); │ │ │ │ │ +232 std::fill(dims.begin(), dims.end() - 1, NDD); │ │ │ │ │ +233 dims.back() = 1; │ │ │ │ │ +234 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x augmentedHessianUniqueKeys; │ │ │ │ │ +235 │ │ │ │ │ +236 // Deal with the fact that some blocks may share the same keys. │ │ │ │ │ +237 if (nrUniqueKeys == nrNonuniqueKeys) { │ │ │ │ │ +238 // Case when there is 1 calibration key per camera: │ │ │ │ │ +239 augmentedHessianUniqueKeys = _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x( │ │ │ │ │ +240 dims, Matrix(augmentedHessian._s_e_l_f_a_d_j_o_i_n_t_V_i_e_w())); │ │ │ │ │ +241 } else { │ │ │ │ │ +242 // When multiple cameras share a calibration we have to rearrange │ │ │ │ │ +243 // the results of the Schur complement matrix. │ │ │ │ │ +244 std::vector nonuniqueDims(nrNonuniqueKeys + 1); // includes b │ │ │ │ │ +245 std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, NDD); │ │ │ │ │ +246 nonuniqueDims.back() = 1; │ │ │ │ │ +247 augmentedHessian = _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x( │ │ │ │ │ +248 nonuniqueDims, Matrix(augmentedHessian._s_e_l_f_a_d_j_o_i_n_t_V_i_e_w())); │ │ │ │ │ +249 │ │ │ │ │ +250 // Get map from key to location in the new augmented Hessian matrix (the │ │ │ │ │ +251 // one including only unique keys). │ │ │ │ │ +252 std::map keyToSlotMap; │ │ │ │ │ +253 for (size_t k = 0; k < nrUniqueKeys; k++) { │ │ │ │ │ +254 keyToSlotMap[hessianKeys[k]] = k; │ │ │ │ │ +255 } │ │ │ │ │ +256 │ │ │ │ │ +257 // Initialize matrix to zero. │ │ │ │ │ +258 augmentedHessianUniqueKeys = _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x( │ │ │ │ │ +259 dims, Matrix::Zero(NDD * nrUniqueKeys + 1, NDD * nrUniqueKeys + 1)); │ │ │ │ │ +260 │ │ │ │ │ +261 // Add contributions for each key: note this loops over the hessian with │ │ │ │ │ +262 // nonUnique keys (augmentedHessian) and populates an Hessian that only │ │ │ │ │ +263 // includes the unique keys (that is what we want to return). │ │ │ │ │ +264 for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows │ │ │ │ │ +265 _K_e_y key_i = jacobianKeys.at(i); │ │ │ │ │ +266 │ │ │ │ │ +267 // Update information vector. │ │ │ │ │ +268 augmentedHessianUniqueKeys._u_p_d_a_t_e_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k( │ │ │ │ │ +269 keyToSlotMap[key_i], nrUniqueKeys, │ │ │ │ │ +270 augmentedHessian._a_b_o_v_e_D_i_a_g_o_n_a_l_B_l_o_c_k(i, nrNonuniqueKeys)); │ │ │ │ │ +271 │ │ │ │ │ +272 // Update blocks. │ │ │ │ │ +273 for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols │ │ │ │ │ +274 _K_e_y key_j = jacobianKeys.at(j); │ │ │ │ │ +275 if (i == j) { │ │ │ │ │ +276 augmentedHessianUniqueKeys._u_p_d_a_t_e_D_i_a_g_o_n_a_l_B_l_o_c_k( │ │ │ │ │ +277 keyToSlotMap[key_i], augmentedHessian._d_i_a_g_o_n_a_l_B_l_o_c_k(i)); │ │ │ │ │ +278 } else { // (i < j) │ │ │ │ │ +279 if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) { │ │ │ │ │ +280 augmentedHessianUniqueKeys._u_p_d_a_t_e_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k( │ │ │ │ │ +281 keyToSlotMap[key_i], keyToSlotMap[key_j], │ │ │ │ │ +282 augmentedHessian._a_b_o_v_e_D_i_a_g_o_n_a_l_B_l_o_c_k(i, j)); │ │ │ │ │ +283 } else { │ │ │ │ │ +284 augmentedHessianUniqueKeys._u_p_d_a_t_e_D_i_a_g_o_n_a_l_B_l_o_c_k( │ │ │ │ │ +285 keyToSlotMap[key_i], │ │ │ │ │ +286 augmentedHessian._a_b_o_v_e_D_i_a_g_o_n_a_l_B_l_o_c_k(i, j) + │ │ │ │ │ +287 augmentedHessian._a_b_o_v_e_D_i_a_g_o_n_a_l_B_l_o_c_k(i, j).transpose()); │ │ │ │ │ +288 } │ │ │ │ │ +289 } │ │ │ │ │ +290 } │ │ │ │ │ +291 } │ │ │ │ │ +292 │ │ │ │ │ +293 // Update bottom right element of the matrix. │ │ │ │ │ +294 augmentedHessianUniqueKeys._u_p_d_a_t_e_D_i_a_g_o_n_a_l_B_l_o_c_k( │ │ │ │ │ +295 nrUniqueKeys, augmentedHessian._d_i_a_g_o_n_a_l_B_l_o_c_k(nrNonuniqueKeys)); │ │ │ │ │ +296 } │ │ │ │ │ +297 return augmentedHessianUniqueKeys; │ │ │ │ │ +298 } │ │ │ │ │ +299 │ │ │ │ │ +306 template // N = 2 or 3 │ │ │ │ │ +_3_0_7 static _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x _S_c_h_u_r_C_o_m_p_l_e_m_e_n_t( │ │ │ │ │ +308 const FBlocks& Fs, const Matrix& E, const Eigen::Matrix& P, │ │ │ │ │ +309 const Vector& b) { │ │ │ │ │ +310 return SchurComplement(Fs, E, P, b); │ │ │ │ │ +311 } │ │ │ │ │ +312 │ │ │ │ │ +314 template // N = 2 or 3 (point dimension) │ │ │ │ │ +_3_1_5 static void _C_o_m_p_u_t_e_P_o_i_n_t_C_o_v_a_r_i_a_n_c_e(Eigen::Matrix& P, │ │ │ │ │ +316 const Matrix& E, double lambda, │ │ │ │ │ +317 bool diagonalDamping = false) { │ │ │ │ │ +318 Matrix EtE = E.transpose() * E; │ │ │ │ │ +319 │ │ │ │ │ +320 if (diagonalDamping) { // diagonal of the hessian │ │ │ │ │ +321 EtE.diagonal() += lambda * EtE.diagonal(); │ │ │ │ │ +322 } else { │ │ │ │ │ +323 _D_e_n_s_e_I_n_d_e_x n = E.cols(); │ │ │ │ │ +324 EtE += lambda * Eigen::MatrixXd::Identity(n, n); │ │ │ │ │ +325 } │ │ │ │ │ +326 │ │ │ │ │ +327 P = (EtE).inverse(); │ │ │ │ │ +328 } │ │ │ │ │ +329 │ │ │ │ │ +_3_3_1 static Matrix _P_o_i_n_t_C_o_v(const Matrix& E, const double lambda = 0.0, │ │ │ │ │ +332 bool diagonalDamping = false) { │ │ │ │ │ +333 if (E.cols() == 2) { │ │ │ │ │ +334 Matrix2 P2; │ │ │ │ │ +335 ComputePointCovariance<2>(P2, E, lambda, diagonalDamping); │ │ │ │ │ +336 return P2; │ │ │ │ │ +337 } else { │ │ │ │ │ +338 Matrix3 P3; │ │ │ │ │ +339 ComputePointCovariance<3>(P3, E, lambda, diagonalDamping); │ │ │ │ │ +340 return P3; │ │ │ │ │ +341 } │ │ │ │ │ +342 } │ │ │ │ │ +343 │ │ │ │ │ +_3_4_8 static _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x _S_c_h_u_r_C_o_m_p_l_e_m_e_n_t(const FBlocks& Fblocks, │ │ │ │ │ +349 const Matrix& E, const Vector& b, │ │ │ │ │ +350 const double lambda = 0.0, │ │ │ │ │ +351 bool diagonalDamping = false) { │ │ │ │ │ +352 if (E.cols() == 2) { │ │ │ │ │ +353 Matrix2 P; │ │ │ │ │ +354 ComputePointCovariance<2>(P, E, lambda, diagonalDamping); │ │ │ │ │ +355 return SchurComplement<2>(Fblocks, E, P, b); │ │ │ │ │ +356 } else { │ │ │ │ │ +357 Matrix3 P; │ │ │ │ │ +358 ComputePointCovariance<3>(P, E, lambda, diagonalDamping); │ │ │ │ │ +359 return SchurComplement<3>(Fblocks, E, P, b); │ │ │ │ │ +360 } │ │ │ │ │ +361 } │ │ │ │ │ +362 │ │ │ │ │ +368 template // N = 2 or 3 (point dimension) │ │ │ │ │ +_3_6_9 static void _U_p_d_a_t_e_S_c_h_u_r_C_o_m_p_l_e_m_e_n_t( │ │ │ │ │ +370 const FBlocks& Fs, const Matrix& E, const Eigen::Matrix& P, │ │ │ │ │ +371 const Vector& b, const _K_e_y_V_e_c_t_o_r& allKeys, const _K_e_y_V_e_c_t_o_r& keys, │ │ │ │ │ +372 /*output ->*/ _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x& augmentedHessian) { │ │ │ │ │ +373 assert(keys.size() == Fs.size()); │ │ │ │ │ +374 assert(keys.size() <= allKeys.size()); │ │ │ │ │ +375 │ │ │ │ │ +376 _F_a_s_t_M_a_p_<_K_e_y_,_ _s_i_z_e___t_> KeySlotMap; │ │ │ │ │ +377 for (size_t slot = 0; slot < allKeys.size(); slot++) │ │ │ │ │ +378 KeySlotMap.insert(std::make_pair(allKeys[slot], slot)); │ │ │ │ │ +379 │ │ │ │ │ +380 // Schur complement trick │ │ │ │ │ +381 // G = F' * F - F' * E * P * E' * F │ │ │ │ │ +382 // g = F' * (b - E * P * E' * b) │ │ │ │ │ +383 │ │ │ │ │ +384 // a single point is observed in m cameras │ │ │ │ │ +385 size_t m = Fs.size(); // cameras observing current point │ │ │ │ │ +386 size_t M = (augmentedHessian._r_o_w_s() - 1) / _D; // all cameras in the group │ │ │ │ │ +387 assert(allKeys.size() == M); │ │ │ │ │ +388 │ │ │ │ │ +389 // Blockwise Schur complement │ │ │ │ │ +390 for (size_t i = 0; i < m; i++) { // for each camera in the current factor │ │ │ │ │ +391 │ │ │ │ │ +392 const _M_a_t_r_i_x_Z_D& Fi = Fs[i]; │ │ │ │ │ +393 const auto FiT = Fi.transpose(); │ │ │ │ │ +394 const Eigen::Matrix Ei_P = │ │ │ │ │ +395 E.template block(_Z_D_i_m * i, 0) * P; │ │ │ │ │ +396 │ │ │ │ │ +397 // D = (DxZDim) * (ZDim) │ │ │ │ │ +398 // allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7) │ │ │ │ │ +399 // we should map those to a slot in the local (grouped) hessian │ │ │ │ │ +400 // (0,1,2,3,4) Key cameraKey_i = this->keys_[i]; │ │ │ │ │ +401 _D_e_n_s_e_I_n_d_e_x aug_i = KeySlotMap.at(keys[i]); │ │ │ │ │ +402 │ │ │ │ │ +403 // information vector - store previous vector │ │ │ │ │ +404 // vectorBlock = augmentedHessian(aug_i, aug_m).knownOffDiagonal(); │ │ │ │ │ +405 // add contribution of current factor │ │ │ │ │ +406 augmentedHessian._u_p_d_a_t_e_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k( │ │ │ │ │ +407 aug_i, M, │ │ │ │ │ +408 FiT * b.segment<_Z_D_i_m>(_Z_D_i_m * i) // F' * b │ │ │ │ │ +409 - │ │ │ │ │ +410 FiT * │ │ │ │ │ +411 (Ei_P * │ │ │ │ │ +412 (E.transpose() * │ │ │ │ │ +413 b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) │ │ │ │ │ +414 │ │ │ │ │ +415 // (DxD) += (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) │ │ │ │ │ +416 // add contribution of current factor │ │ │ │ │ +417 // TODO(gareth): Eigen doesn't let us pass the expression. Call eval() for │ │ │ │ │ +418 // now... │ │ │ │ │ +419 augmentedHessian._u_p_d_a_t_e_D_i_a_g_o_n_a_l_B_l_o_c_k( │ │ │ │ │ +420 aug_i, │ │ │ │ │ +421 ((FiT * │ │ │ │ │ +422 (Fi - │ │ │ │ │ +423 Ei_P * E.template block(_Z_D_i_m * i, 0).transpose() * Fi))) │ │ │ │ │ +424 .eval()); │ │ │ │ │ +425 │ │ │ │ │ +426 // upper triangular part of the hessian │ │ │ │ │ +427 for (size_t j = i + 1; j < m; j++) { // for each camera │ │ │ │ │ +428 const _M_a_t_r_i_x_Z_D& Fj = Fs[j]; │ │ │ │ │ +429 │ │ │ │ │ +430 _D_e_n_s_e_I_n_d_e_x aug_j = KeySlotMap.at(keys[j]); │ │ │ │ │ +431 │ │ │ │ │ +432 // (DxD) = (DxZDim) * ( (ZDimxZDim) * (ZDimxD) ) │ │ │ │ │ +433 // off diagonal block - store previous block │ │ │ │ │ +434 // matrixBlock = augmentedHessian(aug_i, aug_j).knownOffDiagonal(); │ │ │ │ │ +435 // add contribution of current factor │ │ │ │ │ +436 augmentedHessian._u_p_d_a_t_e_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k( │ │ │ │ │ +437 aug_i, aug_j, │ │ │ │ │ +438 -FiT * (Ei_P * E.template block(_Z_D_i_m * j, 0).transpose() * │ │ │ │ │ +439 Fj)); │ │ │ │ │ +440 } │ │ │ │ │ +441 } // end of for over cameras │ │ │ │ │ +442 │ │ │ │ │ +443 augmentedHessian._d_i_a_g_o_n_a_l_B_l_o_c_k(M)(0, 0) += b.squaredNorm(); │ │ │ │ │ +444 } │ │ │ │ │ +445 │ │ │ │ │ +446 private: │ │ │ │ │ +_4_4_8 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +449 template │ │ │ │ │ +450 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ +451 ar&(*this); │ │ │ │ │ +452 } │ │ │ │ │ +453 │ │ │ │ │ +454 public: │ │ │ │ │ +455 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +456}; │ │ │ │ │ +457 │ │ │ │ │ +458template │ │ │ │ │ +459const int _C_a_m_e_r_a_S_e_t_<_C_A_M_E_R_A_>_:_:_D; │ │ │ │ │ +460 │ │ │ │ │ +461template │ │ │ │ │ +462const int _C_a_m_e_r_a_S_e_t_<_C_A_M_E_R_A_>_:_:_Z_D_i_m; │ │ │ │ │ +463 │ │ │ │ │ +464template │ │ │ │ │ +_4_6_5struct _t_r_a_i_t_s<_C_a_m_e_r_a_S_e_t> : public _T_e_s_t_a_b_l_e> {}; │ │ │ │ │ +466 │ │ │ │ │ +467template │ │ │ │ │ +_4_6_8struct _t_r_a_i_t_s> : public _T_e_s_t_a_b_l_e> │ │ │ │ │ +{}; │ │ │ │ │ +469 │ │ │ │ │ +470} // namespace gtsam │ │ │ │ │ +_F_a_s_t_M_a_p_._h │ │ │ │ │ +A thin wrapper around std::map that uses boost's fast_pool_allocator. │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ +_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_._h │ │ │ │ │ +Access to matrices via blocks of pre-defined sizes. │ │ │ │ │ +_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ +This marks a GTSAM object to require alignment. │ │ │ │ │ +DDeeffiinniittiioonn types.h:308 │ │ │ │ │ +_P_o_i_n_t_3_._h │ │ │ │ │ +3D Point │ │ │ │ │ +_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_._h │ │ │ │ │ +Calibrated camera for which only pose is unknown. │ │ │ │ │ +_K_e_y_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_n_s_e_I_n_d_e_x │ │ │ │ │ +ptrdiff_t DenseIndex │ │ │ │ │ +The index type for Eigen objects. │ │ │ │ │ +DDeeffiinniittiioonn types.h:106 │ │ │ │ │ +_g_t_s_a_m_:_:_r_o_w │ │ │ │ │ +const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j) │ │ │ │ │ +Extracts a row view from a matrix that avoids a copy. │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:222 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ -Both ManifoldTraits and Testable. │ │ │ │ │ -DDeeffiinniittiioonn Manifold.h:120 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ -either a fixed size o... │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3 │ │ │ │ │ -Represents an infinite plane in 3D, which is composed of a planar normal and │ │ │ │ │ -its perpendicular distan... │ │ │ │ │ -DDeeffiinniittiioonn OrientedPlane3.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3 │ │ │ │ │ -OrientedPlane3(double a, double b, double c, double d) │ │ │ │ │ -Construct from four numbers of plane coeffcients (a, b, c, d) │ │ │ │ │ -DDeeffiinniittiioonn OrientedPlane3.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_:_:_p_l_a_n_e_C_o_e_f_f_i_c_i_e_n_t_s │ │ │ │ │ -Vector4 planeCoefficients() const │ │ │ │ │ -Returns the plane coefficients. │ │ │ │ │ -DDeeffiinniittiioonn OrientedPlane3.h:122 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3 │ │ │ │ │ -OrientedPlane3(const Unit3 &n, double d) │ │ │ │ │ -Construct from a Unit3 and a distance. │ │ │ │ │ -DDeeffiinniittiioonn OrientedPlane3.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_:_:_d_i_m │ │ │ │ │ -size_t dim() const │ │ │ │ │ -Dimensionality of tangent space = 3 DOF. │ │ │ │ │ -DDeeffiinniittiioonn OrientedPlane3.h:110 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const OrientedPlane3 &s, double tol=1e-9) const │ │ │ │ │ -The equals function with tolerance. │ │ │ │ │ -DDeeffiinniittiioonn OrientedPlane3.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3 │ │ │ │ │ -OrientedPlane3(const Vector4 &vec) │ │ │ │ │ -Construct from a vector of plane coefficients. │ │ │ │ │ -DDeeffiinniittiioonn OrientedPlane3.h:60 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_:_:_D_i_m │ │ │ │ │ -static size_t Dim() │ │ │ │ │ -Dimensionality of tangent space = 3 DOF. │ │ │ │ │ -DDeeffiinniittiioonn OrientedPlane3.h:105 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3 │ │ │ │ │ -OrientedPlane3() │ │ │ │ │ -Default constructor. │ │ │ │ │ -DDeeffiinniittiioonn OrientedPlane3.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_:_:_d_i_s_t_a_n_c_e │ │ │ │ │ -double distance(OptionalJacobian< 1, 3 > H=boost::none) const │ │ │ │ │ -Return the perpendicular distance to the origin. │ │ │ │ │ -DDeeffiinniittiioonn OrientedPlane3.h:134 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_:_:_n_o_r_m_a_l │ │ │ │ │ -Unit3 normal(OptionalJacobian< 2, 3 > H=boost::none) const │ │ │ │ │ -Return the normal. │ │ │ │ │ -DDeeffiinniittiioonn OrientedPlane3.h:128 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ -A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_U_n_i_t_3 │ │ │ │ │ -Represents a 3D point on a unit sphere. │ │ │ │ │ -DDeeffiinniittiioonn Unit3.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_U_n_i_t_3_:_:_u_n_i_t_V_e_c_t_o_r │ │ │ │ │ -Vector3 unitVector(OptionalJacobian< 3, 2 > H=boost::none) const │ │ │ │ │ -Return unit-norm Vector. │ │ │ │ │ -DDeeffiinniittiioonn Unit3.cpp:151 │ │ │ │ │ -_g_t_s_a_m_:_:_U_n_i_t_3_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const Unit3 &s, double tol=1e-9) const │ │ │ │ │ -The equals function with tolerance. │ │ │ │ │ -DDeeffiinniittiioonn Unit3.h:115 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ +FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ +fast_pool_allocator instead of the defa... │ │ │ │ │ +DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_F_i_x_e_d_D_i_m_e_n_s_i_o_n │ │ │ │ │ +Give fixed size dimension of a type, fails at compile time if dynamic. │ │ │ │ │ +DDeeffiinniittiioonn Manifold.h:164 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ +of blocks. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_s_e_t_D_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ +void setDiagonalBlock(DenseIndex I, const XprType &xpr) │ │ │ │ │ +Set a diagonal block. Only the upper triangular portion of xpr is evaluated. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:195 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_s_e_t_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ +void setOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr) │ │ │ │ │ +Set an off-diagonal block. Only the upper triangular portion of xpr is │ │ │ │ │ +evaluated. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:201 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_a_b_o_v_e_D_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ +constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const │ │ │ │ │ +Get block above the diagonal (I, J). │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:150 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_r_o_w_s │ │ │ │ │ +DenseIndex rows() const │ │ │ │ │ +Row size. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:114 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_u_p_d_a_t_e_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ +void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr) │ │ │ │ │ +Update an off diagonal block. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:228 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_d_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ +Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J) │ │ │ │ │ +Return the J'th diagonal block as a self adjoint view. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:135 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_u_p_d_a_t_e_D_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ +void updateDiagonalBlock(DenseIndex I, const XprType &xpr) │ │ │ │ │ +Increment the diagonal block by the values in xpr. Only reads the upper │ │ │ │ │ +triangular part of xpr. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:212 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_s_e_l_f_a_d_j_o_i_n_t_V_i_e_w │ │ │ │ │ +Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex │ │ │ │ │ +I, DenseIndex J) const │ │ │ │ │ +Return the square sub-matrix that contains blocks(i:j, i:j). │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:156 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t │ │ │ │ │ +A set of cameras, all with their own calibration. │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_r_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r │ │ │ │ │ +Vector reprojectionError(const POINT &point, const ZVector &measured, boost:: │ │ │ │ │ +optional< FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost:: │ │ │ │ │ +none) const │ │ │ │ │ +Calculate vector [project2(point)-z] of re-projection errors. │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:136 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_p_r_i_n_t │ │ │ │ │ +virtual void print(const std::string &s="") const │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_C_o_m_p_u_t_e_P_o_i_n_t_C_o_v_a_r_i_a_n_c_e │ │ │ │ │ +static void ComputePointCovariance(Eigen::Matrix< double, N, N > &P, const │ │ │ │ │ +Matrix &E, double lambda, bool diagonalDamping=false) │ │ │ │ │ +Computes Point Covariance P, with lambda parameter. │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:315 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_S_c_h_u_r_C_o_m_p_l_e_m_e_n_t │ │ │ │ │ +static SymmetricBlockMatrix SchurComplement(const FBlocks &Fblocks, const │ │ │ │ │ +Matrix &E, const Vector &b, const double lambda=0.0, bool │ │ │ │ │ +diagonalDamping=false) │ │ │ │ │ +Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix │ │ │ │ │ +Dynamic version. │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:348 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_Z │ │ │ │ │ +CAMERA::Measurement Z │ │ │ │ │ +2D measurement and noise model for each of the m views The order is kept the │ │ │ │ │ +same as the keys that we... │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_~_C_a_m_e_r_a_S_e_t │ │ │ │ │ +virtual ~CameraSet()=default │ │ │ │ │ +Destructor. │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const CameraSet &p, double tol=1e-9) const │ │ │ │ │ +equals │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:91 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_P_o_i_n_t_C_o_v │ │ │ │ │ +static Matrix PointCov(const Matrix &E, const double lambda=0.0, bool │ │ │ │ │ +diagonalDamping=false) │ │ │ │ │ +Computes Point Covariance P, with lambda parameter, dynamic version. │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:331 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_E_r_r_o_r_V_e_c_t_o_r │ │ │ │ │ +static Vector ErrorVector(const ZVector &predicted, const ZVector &measured) │ │ │ │ │ +Make a vector of re-projection errors. │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:51 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_U_p_d_a_t_e_S_c_h_u_r_C_o_m_p_l_e_m_e_n_t │ │ │ │ │ +static void UpdateSchurComplement(const FBlocks &Fs, const Matrix &E, const │ │ │ │ │ +Eigen::Matrix< double, N, N > &P, const Vector &b, const KeyVector &allKeys, │ │ │ │ │ +const KeyVector &keys, SymmetricBlockMatrix &augmentedHessian) │ │ │ │ │ +Applies Schur complement (exploiting block structure) to get a smart factor on │ │ │ │ │ +cameras,... │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:369 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_S_c_h_u_r_C_o_m_p_l_e_m_e_n_t_A_n_d_R_e_a_r_r_a_n_g_e_B_l_o_c_k_s │ │ │ │ │ +static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks(const std:: │ │ │ │ │ +vector< Eigen::Matrix< double, ZDim, ND >, Eigen::aligned_allocator< Eigen:: │ │ │ │ │ +Matrix< double, ZDim, ND > > > &Fs, const Matrix &E, const Eigen::Matrix< │ │ │ │ │ +double, N, N > &P, const Vector &b, const KeyVector &jacobianKeys, const │ │ │ │ │ +KeyVector &hessianKeys) │ │ │ │ │ +Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = │ │ │ │ │ +F' * F - F' * E * P * ... │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:218 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_S_c_h_u_r_C_o_m_p_l_e_m_e_n_t │ │ │ │ │ +static SymmetricBlockMatrix SchurComplement(const std::vector< Eigen::Matrix< │ │ │ │ │ +double, ZDim, ND >, Eigen::aligned_allocator< Eigen::Matrix< double, ZDim, ND > │ │ │ │ │ +> > &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector │ │ │ │ │ +&b) │ │ │ │ │ +Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = │ │ │ │ │ +F' * F - F' * E * P * ... │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:150 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_Z_D_i_m │ │ │ │ │ +static const int ZDim │ │ │ │ │ +Measurement dimension. │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_S_c_h_u_r_C_o_m_p_l_e_m_e_n_t │ │ │ │ │ +static SymmetricBlockMatrix SchurComplement(const FBlocks &Fs, const Matrix &E, │ │ │ │ │ +const Eigen::Matrix< double, N, N > &P, const Vector &b) │ │ │ │ │ +Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = │ │ │ │ │ +F' * F - F' * E * P * ... │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:307 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_D │ │ │ │ │ +static const int D │ │ │ │ │ +Camera dimension. │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:448 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_M_a_t_r_i_x_Z_D │ │ │ │ │ +Eigen::Matrix< double, ZDim, D > MatrixZD │ │ │ │ │ +Definitions for blocks of F. │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_p_r_o_j_e_c_t_2 │ │ │ │ │ +ZVector project2(const POINT &point, boost::optional< FBlocks & > Fs=boost:: │ │ │ │ │ +none, boost::optional< Matrix & > E=boost::none) const │ │ │ │ │ +Project a point (possibly Unit3 at infinity), with derivatives Note that F is a │ │ │ │ │ +sparse block-diagonal... │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:108 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * OOrriieenntteeddPPllaannee33..hh │ │ │ │ │ + * _C_a_m_e_r_a_S_e_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00467.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/PinholePose.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3_S2Stereo.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,54 +94,42 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
PinholePose.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
Cal3_S2Stereo.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Pinhole camera with known calibration. │ │ │ │ +

The most common 5DOF 3D->2D calibration + Stereo baseline. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::PinholeBaseK< CALIBRATION >
 A pinhole camera class that has a Pose3 and a fixed Calibration. More...
 
class  gtsam::PinholePose< CALIBRATION >
 A pinhole camera class that has a Pose3 and a fixed Calibration. More...
 
struct  gtsam::traits< PinholePose< CALIBRATION > >
 
struct  gtsam::traits< const PinholePose< CALIBRATION > >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +std::ostream & gtsam::operator<< (std::ostream &os, const Cal3_S2Stereo &cal)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Pinhole camera with known calibration.

│ │ │ │ -
Author
Yong-Dian Jian
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ -
Date
Feb 20, 2015
│ │ │ │ +

The most common 5DOF 3D->2D calibration + Stereo baseline.

│ │ │ │ +
Author
Chris Beall
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,37 +1,23 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -PinholePose.h File Reference │ │ │ │ │ -Pinhole camera with known calibration. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_<_ _C_A_L_I_B_R_A_T_I_O_N_ _> │ │ │ │ │ -  A pinhole camera class that has a _P_o_s_e_3 and a ffiixxeedd Calibration. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_<_ _C_A_L_I_B_R_A_T_I_O_N_ _> │ │ │ │ │ -  A pinhole camera class that has a _P_o_s_e_3 and a ffiixxeedd Calibration. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _P_i_n_h_o_l_e_P_o_s_e_<_ _C_A_L_I_B_R_A_T_I_O_N_ _>_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _P_i_n_h_o_l_e_P_o_s_e_<_ _C_A_L_I_B_R_A_T_I_O_N_ _>_ _> │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Cal3_S2Stereo.cpp File Reference │ │ │ │ │ +The most common 5DOF 3D->2D calibration + Stereo baseline. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _C_a_l_3___S_2_S_t_e_r_e_o &cal) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Pinhole camera with known calibration. │ │ │ │ │ +The most common 5DOF 3D->2D calibration + Stereo baseline. │ │ │ │ │ Author │ │ │ │ │ - Yong-Dian Jian │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - Feb 20, 2015 │ │ │ │ │ + Chris Beall │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _P_i_n_h_o_l_e_P_o_s_e_._h │ │ │ │ │ + * _C_a_l_3___S_2_S_t_e_r_e_o_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00473.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SimpleCamera.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Unified.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,36 +94,41 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces
│ │ │ │ -
SimpleCamera.cpp File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
Cal3Unified.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ - │ │ │ │ -

A simple camera class with a Cal3_S2 calibration. │ │ │ │ -More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +std::ostream & gtsam::operator<< (std::ostream &os, const Cal3Unified &cal)
 
│ │ │ │

Detailed Description

│ │ │ │ -

A simple camera class with a Cal3_S2 calibration.

│ │ │ │ -
Date
June 30, 2012
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +
Date
Mar 8, 2014
│ │ │ │ +
Author
Jing Dong
│ │ │ │ +
│ │ │ │ +Varun Agrawal
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,24 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SimpleCamera.cpp File Reference │ │ │ │ │ -A simple camera class with a Cal3_S2 calibration. _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Cal3Unified.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _C_a_l_3_U_n_i_f_i_e_d &cal) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A simple camera class with a Cal3_S2 calibration. │ │ │ │ │ Date │ │ │ │ │ - June 30, 2012 │ │ │ │ │ + Mar 8, 2014 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Jing Dong │ │ │ │ │ + Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_i_m_p_l_e_C_a_m_e_r_a_._c_p_p │ │ │ │ │ + * _C_a_l_3_U_n_i_f_i_e_d_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00479.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Similarity2.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Rot3M.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,42 +94,41 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
Similarity2.cpp File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
Rot3M.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Implementation of Similarity2 transform. │ │ │ │ +

Rotation (internal: 3*3 matrix representation*) │ │ │ │ More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -std::ostream & gtsam::operator<< (std::ostream &os, const Similarity2 &p)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Implementation of Similarity2 transform.

│ │ │ │ -
Author
John Lambert, Varun Agrawal
│ │ │ │ +

Rotation (internal: 3*3 matrix representation*)

│ │ │ │ +
Author
Alireza Fathi
│ │ │ │ +
│ │ │ │ +Christian Potthast
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,23 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Similarity2.cpp File Reference │ │ │ │ │ -Implementation of Similarity2 transform. _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +Rot3M.cpp File Reference │ │ │ │ │ +Rotation (internal: 3*3 matrix representation*) _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _S_i_m_i_l_a_r_i_t_y_2 &p) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Implementation of Similarity2 transform. │ │ │ │ │ +Rotation (internal: 3*3 matrix representation*) │ │ │ │ │ Author │ │ │ │ │ - John Lambert, Varun Agrawal │ │ │ │ │ + Alireza Fathi │ │ │ │ │ + Christian Potthast │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_i_m_i_l_a_r_i_t_y_2_._c_p_p │ │ │ │ │ + * _R_o_t_3_M_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00482.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/GaussianMixture.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,55 +96,62 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ +Typedefs | │ │ │ │ Functions
│ │ │ │ -
GaussianMixture.h File Reference
│ │ │ │ +
HybridFactor.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

A hybrid conditional in the Conditional Linear Gaussian scheme. │ │ │ │ -More...

│ │ │ │ - │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::GaussianMixture
 A conditional of gaussian mixtures indexed by discrete variables, as part of a Bayes Network. More...
class  gtsam::HybridFactor
 Base class for truly hybrid probabilistic factors. More...
 
struct  gtsam::traits< GaussianMixture >
struct  gtsam::traits< HybridFactor >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Typedefs

│ │ │ │ +using gtsam::GaussianFactorGraphTree = DecisionTree< Key, GaussianFactorGraph >
 Alias for DecisionTree of GaussianFactorGraphs.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

│ │ │ │ -std::set< DiscreteKey > gtsam::DiscreteKeysAsSet (const DiscreteKeys &discreteKeys)
 Return the DiscreteKey vector as a set.
 
│ │ │ │ +KeyVector gtsam::CollectKeys (const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys)
 
│ │ │ │ +KeyVector gtsam::CollectKeys (const KeyVector &keys1, const KeyVector &keys2)
 
│ │ │ │ +DiscreteKeys gtsam::CollectDiscreteKeys (const DiscreteKeys &key1, const DiscreteKeys &key2)
 
│ │ │ │

Detailed Description

│ │ │ │ -

A hybrid conditional in the Conditional Linear Gaussian scheme.

│ │ │ │ -
Author
Fan Jiang
│ │ │ │ -
│ │ │ │ -Varun Agrawal
│ │ │ │ -
Date
Mar 12, 2022
│ │ │ │ +
Date
Mar 11, 2022
│ │ │ │ +
Author
Fan Jiang
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,36 +1,42 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -GaussianMixture.h File Reference │ │ │ │ │ -A hybrid conditional in the Conditional Linear Gaussian scheme. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +HybridFactor.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e │ │ │ │ │ -  A conditional of gaussian mixtures indexed by discrete variables, as │ │ │ │ │ - part of a Bayes Network. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r │ │ │ │ │ +  Base class for ttrruullyy hybrid probabilistic factors. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _H_y_b_r_i_d_F_a_c_t_o_r_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +using  ggttssaamm::::GGaauussssiiaannFFaaccttoorrGGrraapphhTTrreeee = _D_e_c_i_s_i_o_n_T_r_e_e< _K_e_y, _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ + > │ │ │ │ │ +  Alias for _D_e_c_i_s_i_o_n_T_r_e_e of GaussianFactorGraphs. │ │ │ │ │ +  │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -std::set< _D_i_s_c_r_e_t_e_K_e_y >  ggttssaamm::::DDiissccrreetteeKKeeyyssAAssSSeett (const _D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ - &discreteKeys) │ │ │ │ │ -  Return the DiscreteKey vector as a set. │ │ │ │ │ + _K_e_y_V_e_c_t_o_r  ggttssaamm::::CCoolllleeccttKKeeyyss (const _K_e_y_V_e_c_t_o_r &continuousKeys, const │ │ │ │ │ + _D_i_s_c_r_e_t_e_K_e_y_s &discreteKeys) │ │ │ │ │ +  │ │ │ │ │ + _K_e_y_V_e_c_t_o_r  ggttssaamm::::CCoolllleeccttKKeeyyss (const _K_e_y_V_e_c_t_o_r &keys1, const _K_e_y_V_e_c_t_o_r │ │ │ │ │ + &keys2) │ │ │ │ │ +  │ │ │ │ │ +_D_i_s_c_r_e_t_e_K_e_y_s  ggttssaamm::::CCoolllleeccttDDiissccrreetteeKKeeyyss (const _D_i_s_c_r_e_t_e_K_e_y_s &key1, const │ │ │ │ │ + _D_i_s_c_r_e_t_e_K_e_y_s &key2) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A hybrid conditional in the Conditional Linear Gaussian scheme. │ │ │ │ │ + Date │ │ │ │ │ + Mar 11, 2022 │ │ │ │ │ Author │ │ │ │ │ Fan Jiang │ │ │ │ │ - Varun Agrawal │ │ │ │ │ - Date │ │ │ │ │ - Mar 12, 2022 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_._h │ │ │ │ │ + * _H_y_b_r_i_d_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00482.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a00482 = [ │ │ │ │ │ - ["gtsam::traits< GaussianMixture >", "a03396.html", null], │ │ │ │ │ - ["DiscreteKeysAsSet", "a00482.html#ad92a1966f54e02a7c2ce061c1228c7b3", null] │ │ │ │ │ + ["gtsam::traits< HybridFactor >", "a03452.html", null], │ │ │ │ │ + ["GaussianFactorGraphTree", "a00482.html#a8b4a8e0d32b81b0fa2fa15c437b89e7c", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00482_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/GaussianMixture.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,182 +98,157 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
GaussianMixture.h
│ │ │ │ +
HybridFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/discrete/DecisionTree-inl.h>
│ │ │ │ -
23#include <gtsam/discrete/DecisionTree.h>
│ │ │ │ -
24#include <gtsam/discrete/DecisionTreeFactor.h>
│ │ │ │ -
25#include <gtsam/discrete/DiscreteKey.h>
│ │ │ │ -
26#include <gtsam/hybrid/GaussianMixtureFactor.h>
│ │ │ │ -
27#include <gtsam/hybrid/HybridFactor.h>
│ │ │ │ -
28#include <gtsam/inference/Conditional.h>
│ │ │ │ -
29#include <gtsam/linear/GaussianConditional.h>
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/base/Testable.h>
│ │ │ │ +
21#include <gtsam/discrete/DecisionTree.h>
│ │ │ │ +
22#include <gtsam/discrete/DiscreteKey.h>
│ │ │ │ +
23#include <gtsam/inference/Factor.h>
│ │ │ │ +
24#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ +
25#include <gtsam/nonlinear/Values.h>
│ │ │ │ +
26
│ │ │ │ +
27#include <cstddef>
│ │ │ │ +
28#include <string>
│ │ │ │ +
29namespace gtsam {
│ │ │ │
30
│ │ │ │ -
31namespace gtsam {
│ │ │ │ +
31class HybridValues;
│ │ │ │
32
│ │ │ │ -
33class HybridValues;
│ │ │ │ -
34
│ │ │ │ -
│ │ │ │ -
53class GTSAM_EXPORT GaussianMixture
│ │ │ │ -
54 : public HybridFactor,
│ │ │ │ -
55 public Conditional<HybridFactor, GaussianMixture> {
│ │ │ │ -
56 public:
│ │ │ │ -
57 using This = GaussianMixture;
│ │ │ │ -
58 using shared_ptr = boost::shared_ptr<GaussianMixture>;
│ │ │ │ -
59 using BaseFactor = HybridFactor;
│ │ │ │ -
60 using BaseConditional = Conditional<HybridFactor, GaussianMixture>;
│ │ │ │ -
61
│ │ │ │ -
63 using Conditionals = DecisionTree<Key, GaussianConditional::shared_ptr>;
│ │ │ │ -
64
│ │ │ │ -
65 private:
│ │ │ │ -
66 Conditionals conditionals_;
│ │ │ │ -
67 double logConstant_;
│ │ │ │ -
68
│ │ │ │ -
72 GaussianFactorGraphTree asGaussianFactorGraphTree() const;
│ │ │ │ +
34using GaussianFactorGraphTree = DecisionTree<Key, GaussianFactorGraph>;
│ │ │ │ +
35
│ │ │ │ +
36KeyVector CollectKeys(const KeyVector &continuousKeys,
│ │ │ │ +
37 const DiscreteKeys &discreteKeys);
│ │ │ │ +
38KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2);
│ │ │ │ +
39DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1,
│ │ │ │ +
40 const DiscreteKeys &key2);
│ │ │ │ +
41
│ │ │ │ +
│ │ │ │ +
52class GTSAM_EXPORT HybridFactor : public Factor {
│ │ │ │ +
53 private:
│ │ │ │ +
54 bool isDiscrete_ = false;
│ │ │ │ +
55 bool isContinuous_ = false;
│ │ │ │ +
56 bool isHybrid_ = false;
│ │ │ │ +
57
│ │ │ │ +
58 protected:
│ │ │ │ +
59 // Set of DiscreteKeys for this factor.
│ │ │ │ +
60 DiscreteKeys discreteKeys_;
│ │ │ │ +
62 KeyVector continuousKeys_;
│ │ │ │ +
63
│ │ │ │ +
64 public:
│ │ │ │ +
65 // typedefs needed to play nice with gtsam
│ │ │ │ +
66 typedef HybridFactor This;
│ │ │ │ +
67 typedef boost::shared_ptr<HybridFactor>
│ │ │ │ +
68 shared_ptr;
│ │ │ │ +
69 typedef Factor Base;
│ │ │ │ +
70
│ │ │ │
73
│ │ │ │ -
81 std::function<GaussianConditional::shared_ptr(
│ │ │ │ -
82 const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
│ │ │ │ -
83 prunerFunc(const DecisionTreeFactor &decisionTree);
│ │ │ │ -
84
│ │ │ │ -
85 public:
│ │ │ │ -
88
│ │ │ │ -
90 GaussianMixture() = default;
│ │ │ │ -
91
│ │ │ │ -
103 GaussianMixture(const KeyVector &continuousFrontals,
│ │ │ │ -
104 const KeyVector &continuousParents,
│ │ │ │ -
105 const DiscreteKeys &discreteParents,
│ │ │ │ -
106 const Conditionals &conditionals);
│ │ │ │ -
107
│ │ │ │ -
116 GaussianMixture(KeyVector &&continuousFrontals, KeyVector &&continuousParents,
│ │ │ │ -
117 DiscreteKeys &&discreteParents,
│ │ │ │ -
118 std::vector<GaussianConditional::shared_ptr> &&conditionals);
│ │ │ │ -
119
│ │ │ │ -
128 GaussianMixture(
│ │ │ │ -
129 const KeyVector &continuousFrontals, const KeyVector &continuousParents,
│ │ │ │ -
130 const DiscreteKeys &discreteParents,
│ │ │ │ -
131 const std::vector<GaussianConditional::shared_ptr> &conditionals);
│ │ │ │ -
132
│ │ │ │ +
75 HybridFactor() = default;
│ │ │ │ +
76
│ │ │ │ +
82 explicit HybridFactor(const KeyVector &keys);
│ │ │ │ +
83
│ │ │ │ +
89 explicit HybridFactor(const DiscreteKeys &discreteKeys);
│ │ │ │ +
90
│ │ │ │ +
97 HybridFactor(const KeyVector &continuousKeys,
│ │ │ │ +
98 const DiscreteKeys &discreteKeys);
│ │ │ │ +
99
│ │ │ │ +
101 virtual ~HybridFactor() = default;
│ │ │ │ +
102
│ │ │ │ +
106
│ │ │ │ +
108 virtual bool equals(const HybridFactor &lf, double tol = 1e-9) const;
│ │ │ │ +
109
│ │ │ │ +
111 void print(
│ │ │ │ +
112 const std::string &s = "HybridFactor\n",
│ │ │ │ +
113 const KeyFormatter &formatter = DefaultKeyFormatter) const override;
│ │ │ │ +
114
│ │ │ │ +
118
│ │ │ │ +
120 bool isDiscrete() const { return isDiscrete_; }
│ │ │ │ +
121
│ │ │ │ +
123 bool isContinuous() const { return isContinuous_; }
│ │ │ │ +
124
│ │ │ │ +
126 bool isHybrid() const { return isHybrid_; }
│ │ │ │ +
127
│ │ │ │ +
129 size_t nrContinuous() const { return continuousKeys_.size(); }
│ │ │ │ +
130
│ │ │ │ +
132 const DiscreteKeys &discreteKeys() const { return discreteKeys_; }
│ │ │ │ +
133
│ │ │ │ +
135 const KeyVector &continuousKeys() const { return continuousKeys_; }
│ │ │ │
136
│ │ │ │ -
138 bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
│ │ │ │ -
139
│ │ │ │ -
141 void print(
│ │ │ │ -
142 const std::string &s = "GaussianMixture\n",
│ │ │ │ -
143 const KeyFormatter &formatter = DefaultKeyFormatter) const override;
│ │ │ │ -
144
│ │ │ │ -
148
│ │ │ │ -
150 GaussianConditional::shared_ptr operator()(
│ │ │ │ -
151 const DiscreteValues &discreteValues) const;
│ │ │ │ -
152
│ │ │ │ -
154 size_t nrComponents() const;
│ │ │ │ -
155
│ │ │ │ -
157 KeyVector continuousParents() const;
│ │ │ │ -
158
│ │ │ │ -
161 double logNormalizationConstant() const override { return logConstant_; }
│ │ │ │ -
162
│ │ │ │ -
167 boost::shared_ptr<GaussianMixtureFactor> likelihood(
│ │ │ │ -
168 const VectorValues &given) const;
│ │ │ │ -
169
│ │ │ │ -
171 const Conditionals &conditionals() const;
│ │ │ │ -
172
│ │ │ │ -
180 AlgebraicDecisionTree<Key> logProbability(
│ │ │ │ -
181 const VectorValues &continuousValues) const;
│ │ │ │ -
182
│ │ │ │ -
208 double error(const HybridValues &values) const override;
│ │ │ │ -
209
│ │ │ │ -
217 AlgebraicDecisionTree<Key> error(const VectorValues &continuousValues) const;
│ │ │ │ -
218
│ │ │ │ -
225 double logProbability(const HybridValues &values) const override;
│ │ │ │ -
226
│ │ │ │ -
228 double evaluate(const HybridValues &values) const override;
│ │ │ │ -
229
│ │ │ │ -
│ │ │ │ -
231 double operator()(const HybridValues &values) const {
│ │ │ │ -
232 return evaluate(values);
│ │ │ │ -
233 }
│ │ │ │ -
│ │ │ │ -
234
│ │ │ │ -
242 void prune(const DecisionTreeFactor &decisionTree);
│ │ │ │ -
243
│ │ │ │ -
251 GaussianFactorGraphTree add(const GaussianFactorGraphTree &sum) const;
│ │ │ │ -
253
│ │ │ │ -
254 private:
│ │ │ │ -
256 bool allFrontalsGiven(const VectorValues &given) const;
│ │ │ │ -
257
│ │ │ │ -
259 friend class boost::serialization::access;
│ │ │ │ -
260 template <class Archive>
│ │ │ │ -
261 void serialize(Archive &ar, const unsigned int /*version*/) {
│ │ │ │ -
262 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseFactor);
│ │ │ │ -
263 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseConditional);
│ │ │ │ -
264 ar &BOOST_SERIALIZATION_NVP(conditionals_);
│ │ │ │ -
265 }
│ │ │ │ -
266};
│ │ │ │ -
│ │ │ │ -
267
│ │ │ │ -
269std::set<DiscreteKey> DiscreteKeysAsSet(const DiscreteKeys &discreteKeys);
│ │ │ │ -
270
│ │ │ │ -
271// traits
│ │ │ │ -
272template <>
│ │ │ │ -
273struct traits<GaussianMixture> : public Testable<GaussianMixture> {};
│ │ │ │ -
274
│ │ │ │ -
275} // namespace gtsam
│ │ │ │ -
DecisionTree.h
Decision Tree for use in DiscreteFactors.
│ │ │ │ -
DiscreteKey.h
specialized key for discrete variables
│ │ │ │ - │ │ │ │ - │ │ │ │ -
GaussianMixtureFactor.h
A set of GaussianFactors, indexed by a set of discrete keys.
│ │ │ │ -
Conditional.h
Base class for conditional densities.
│ │ │ │ -
GaussianConditional.h
Conditional Gaussian Base class.
│ │ │ │ +
138
│ │ │ │ +
139 private:
│ │ │ │ +
141 friend class boost::serialization::access;
│ │ │ │ +
142 template <class ARCHIVE>
│ │ │ │ +
143 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ +
144 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
145 ar &BOOST_SERIALIZATION_NVP(isDiscrete_);
│ │ │ │ +
146 ar &BOOST_SERIALIZATION_NVP(isContinuous_);
│ │ │ │ +
147 ar &BOOST_SERIALIZATION_NVP(isHybrid_);
│ │ │ │ +
148 ar &BOOST_SERIALIZATION_NVP(discreteKeys_);
│ │ │ │ +
149 ar &BOOST_SERIALIZATION_NVP(continuousKeys_);
│ │ │ │ +
150 }
│ │ │ │ +
151};
│ │ │ │ +
│ │ │ │ +
152// HybridFactor
│ │ │ │ +
153
│ │ │ │ +
154// traits
│ │ │ │ +
155template <>
│ │ │ │ +
156struct traits<HybridFactor> : public Testable<HybridFactor> {};
│ │ │ │ +
157
│ │ │ │ +
158} // namespace gtsam
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
DecisionTree.h
Decision Tree for use in DiscreteFactors.
│ │ │ │ +
DiscreteKey.h
specialized key for discrete variables
│ │ │ │ +
Factor.h
The base class for all factors.
│ │ │ │ +
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ +
Values.h
A non-templated config holding any types of Manifold-group elements.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::DiscreteKeysAsSet
std::set< DiscreteKey > DiscreteKeysAsSet(const DiscreteKeys &discreteKeys)
Return the DiscreteKey vector as a set.
Definition GaussianMixture.cpp:221
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::prunerFunc
std::function< double(const Assignment< Key > &, double)> prunerFunc(const DecisionTreeFactor &prunedDecisionTree, const HybridConditional &conditional)
Helper function to get the pruner functional.
Definition HybridBayesNet.cpp:66
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::AlgebraicDecisionTree
An algebraic decision tree fixes the range of a DecisionTree to double.
Definition AlgebraicDecisionTree.h:38
│ │ │ │ -
gtsam::Assignment
An assignment from labels to value index (size_t).
Definition Assignment.h:37
│ │ │ │ - │ │ │ │ -
gtsam::DecisionTreeFactor
A discrete probabilistic factor.
Definition DecisionTreeFactor.h:45
│ │ │ │ +
gtsam::DecisionTree
a decision tree is a function from assignments to values.
Definition DecisionTree.h:61
│ │ │ │
gtsam::DiscreteKeys
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition DiscreteKey.h:39
│ │ │ │ -
gtsam::DiscreteValues
A map from keys to values.
Definition DiscreteValues.h:34
│ │ │ │ -
gtsam::GaussianMixture
A conditional of gaussian mixtures indexed by discrete variables, as part of a Bayes Network.
Definition GaussianMixture.h:55
│ │ │ │ -
gtsam::GaussianMixture::logNormalizationConstant
double logNormalizationConstant() const override
The log normalization constant is max of the the individual log-normalization constants.
Definition GaussianMixture.h:161
│ │ │ │ -
gtsam::GaussianMixture::operator()
double operator()(const HybridValues &values) const
Evaluate probability density, sugar.
Definition GaussianMixture.h:231
│ │ │ │ -
gtsam::GaussianMixture::GaussianMixture
GaussianMixture()=default
Default constructor, mainly for serialization.
│ │ │ │
gtsam::HybridFactor
Base class for truly hybrid probabilistic factors.
Definition HybridFactor.h:52
│ │ │ │ -
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ -
gtsam::Conditional
Definition Conditional.h:64
│ │ │ │ -
gtsam::GaussianConditional::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianConditional.h:46
│ │ │ │ -
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
gtsam::HybridFactor::nrContinuous
size_t nrContinuous() const
Return the number of continuous variables in this factor.
Definition HybridFactor.h:129
│ │ │ │ +
gtsam::HybridFactor::This
HybridFactor This
This class.
Definition HybridFactor.h:66
│ │ │ │ +
gtsam::HybridFactor::shared_ptr
boost::shared_ptr< HybridFactor > shared_ptr
shared_ptr to this class
Definition HybridFactor.h:68
│ │ │ │ +
gtsam::HybridFactor::isDiscrete
bool isDiscrete() const
True if this is a factor of discrete variables only.
Definition HybridFactor.h:120
│ │ │ │ +
gtsam::HybridFactor::isHybrid
bool isHybrid() const
True is this is a Discrete-Continuous factor.
Definition HybridFactor.h:126
│ │ │ │ +
gtsam::HybridFactor::HybridFactor
HybridFactor()=default
Default constructor creates empty factor.
│ │ │ │ +
gtsam::HybridFactor::Base
Factor Base
Our base class.
Definition HybridFactor.h:69
│ │ │ │ +
gtsam::HybridFactor::continuousKeys
const KeyVector & continuousKeys() const
Return only the continuous keys for this factor.
Definition HybridFactor.h:135
│ │ │ │ +
gtsam::HybridFactor::isContinuous
bool isContinuous() const
True if this is a factor of continuous variables only.
Definition HybridFactor.h:123
│ │ │ │ +
gtsam::HybridFactor::continuousKeys_
KeyVector continuousKeys_
Record continuous keys for book-keeping.
Definition HybridFactor.h:62
│ │ │ │ +
gtsam::HybridFactor::~HybridFactor
virtual ~HybridFactor()=default
Virtual destructor.
│ │ │ │ +
gtsam::HybridFactor::discreteKeys
const DiscreteKeys & discreteKeys() const
Return the discrete keys for this factor.
Definition HybridFactor.h:132
│ │ │ │ +
gtsam::Factor
Definition Factor.h:68
│ │ │ │
HybridValues
the error.
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,236 +1,213 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GaussianMixture.h │ │ │ │ │ +HybridFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include │ │ │ │ │ -23#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_e_c_i_s_i_o_n_T_r_e_e_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_K_e_y_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r_._h> │ │ │ │ │ -27#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_F_a_c_t_o_r_._h> │ │ │ │ │ -28#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ -29#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_e_c_i_s_i_o_n_T_r_e_e_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_K_e_y_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_V_a_l_u_e_s_._h> │ │ │ │ │ +26 │ │ │ │ │ +27#include │ │ │ │ │ +28#include │ │ │ │ │ +29namespace _g_t_s_a_m { │ │ │ │ │ 30 │ │ │ │ │ -31namespace _g_t_s_a_m { │ │ │ │ │ +31class _H_y_b_r_i_d_V_a_l_u_e_s; │ │ │ │ │ 32 │ │ │ │ │ -33class _H_y_b_r_i_d_V_a_l_u_e_s; │ │ │ │ │ -34 │ │ │ │ │ -_5_3class GTSAM_EXPORT _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e │ │ │ │ │ -54 : public _H_y_b_r_i_d_F_a_c_t_o_r, │ │ │ │ │ -55 public _C_o_n_d_i_t_i_o_n_a_l { │ │ │ │ │ -56 public: │ │ │ │ │ -57 using _T_h_i_s = _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e; │ │ │ │ │ -58 using shared_ptr = boost::shared_ptr; │ │ │ │ │ -59 using _B_a_s_e_F_a_c_t_o_r = _H_y_b_r_i_d_F_a_c_t_o_r; │ │ │ │ │ -60 using _B_a_s_e_C_o_n_d_i_t_i_o_n_a_l = _C_o_n_d_i_t_i_o_n_a_l_<_H_y_b_r_i_d_F_a_c_t_o_r_,_ _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_>; │ │ │ │ │ -61 │ │ │ │ │ -_6_3 using _C_o_n_d_i_t_i_o_n_a_l_s = _D_e_c_i_s_i_o_n_T_r_e_e_<_K_e_y_,_ _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r_>; │ │ │ │ │ -64 │ │ │ │ │ -65 private: │ │ │ │ │ -66 _C_o_n_d_i_t_i_o_n_a_l_s conditionals_; │ │ │ │ │ -67 double logConstant_; │ │ │ │ │ -68 │ │ │ │ │ -72 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_T_r_e_e asGaussianFactorGraphTree() const; │ │ │ │ │ +_3_4using _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_T_r_e_e = _D_e_c_i_s_i_o_n_T_r_e_e_<_K_e_y_,_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_>; │ │ │ │ │ +35 │ │ │ │ │ +36_K_e_y_V_e_c_t_o_r CollectKeys(const _K_e_y_V_e_c_t_o_r &continuousKeys, │ │ │ │ │ +37 const _D_i_s_c_r_e_t_e_K_e_y_s &discreteKeys); │ │ │ │ │ +38_K_e_y_V_e_c_t_o_r CollectKeys(const _K_e_y_V_e_c_t_o_r &keys1, const _K_e_y_V_e_c_t_o_r &keys2); │ │ │ │ │ +39_D_i_s_c_r_e_t_e_K_e_y_s CollectDiscreteKeys(const _D_i_s_c_r_e_t_e_K_e_y_s &key1, │ │ │ │ │ +40 const _D_i_s_c_r_e_t_e_K_e_y_s &key2); │ │ │ │ │ +41 │ │ │ │ │ +_5_2class GTSAM_EXPORT _H_y_b_r_i_d_F_a_c_t_o_r : public _F_a_c_t_o_r { │ │ │ │ │ +53 private: │ │ │ │ │ +54 bool isDiscrete_ = false; │ │ │ │ │ +55 bool isContinuous_ = false; │ │ │ │ │ +56 bool isHybrid_ = false; │ │ │ │ │ +57 │ │ │ │ │ +58 protected: │ │ │ │ │ +59 // Set of DiscreteKeys for this factor. │ │ │ │ │ +60 _D_i_s_c_r_e_t_e_K_e_y_s discreteKeys_; │ │ │ │ │ +_6_2 _K_e_y_V_e_c_t_o_r _c_o_n_t_i_n_u_o_u_s_K_e_y_s__; │ │ │ │ │ +63 │ │ │ │ │ +64 public: │ │ │ │ │ +65 // typedefs needed to play nice with gtsam │ │ │ │ │ +_6_6 typedef _H_y_b_r_i_d_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ +67 typedef boost::shared_ptr │ │ │ │ │ +_6_8 _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +_6_9 typedef _F_a_c_t_o_r _B_a_s_e; │ │ │ │ │ +70 │ │ │ │ │ 73 │ │ │ │ │ -81 std::function<_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r( │ │ │ │ │ -82 const _A_s_s_i_g_n_m_e_n_t_<_K_e_y_> &, const _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r &)> │ │ │ │ │ -83 _p_r_u_n_e_r_F_u_n_c(const _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r &decisionTree); │ │ │ │ │ -84 │ │ │ │ │ -85 public: │ │ │ │ │ -88 │ │ │ │ │ -_9_0 _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e() = default; │ │ │ │ │ -91 │ │ │ │ │ -103 _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e(const _K_e_y_V_e_c_t_o_r &continuousFrontals, │ │ │ │ │ -104 const _K_e_y_V_e_c_t_o_r &continuousParents, │ │ │ │ │ -105 const _D_i_s_c_r_e_t_e_K_e_y_s &discreteParents, │ │ │ │ │ -106 const _C_o_n_d_i_t_i_o_n_a_l_s &conditionals); │ │ │ │ │ -107 │ │ │ │ │ -116 _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e(_K_e_y_V_e_c_t_o_r &&continuousFrontals, _K_e_y_V_e_c_t_o_r │ │ │ │ │ -&&continuousParents, │ │ │ │ │ -117 _D_i_s_c_r_e_t_e_K_e_y_s &&discreteParents, │ │ │ │ │ -118 std::vector &&conditionals); │ │ │ │ │ -119 │ │ │ │ │ -128 _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e( │ │ │ │ │ -129 const _K_e_y_V_e_c_t_o_r &continuousFrontals, const _K_e_y_V_e_c_t_o_r &continuousParents, │ │ │ │ │ -130 const _D_i_s_c_r_e_t_e_K_e_y_s &discreteParents, │ │ │ │ │ -131 const std::vector &conditionals); │ │ │ │ │ -132 │ │ │ │ │ +_7_5 _H_y_b_r_i_d_F_a_c_t_o_r() = default; │ │ │ │ │ +76 │ │ │ │ │ +82 explicit _H_y_b_r_i_d_F_a_c_t_o_r(const _K_e_y_V_e_c_t_o_r &keys); │ │ │ │ │ +83 │ │ │ │ │ +89 explicit _H_y_b_r_i_d_F_a_c_t_o_r(const _D_i_s_c_r_e_t_e_K_e_y_s &discreteKeys); │ │ │ │ │ +90 │ │ │ │ │ +97 _H_y_b_r_i_d_F_a_c_t_o_r(const _K_e_y_V_e_c_t_o_r &continuousKeys, │ │ │ │ │ +98 const _D_i_s_c_r_e_t_e_K_e_y_s &discreteKeys); │ │ │ │ │ +99 │ │ │ │ │ +_1_0_1 virtual _~_H_y_b_r_i_d_F_a_c_t_o_r() = default; │ │ │ │ │ +102 │ │ │ │ │ +106 │ │ │ │ │ +108 virtual bool _e_q_u_a_l_s(const _H_y_b_r_i_d_F_a_c_t_o_r &lf, double tol = 1e-9) const; │ │ │ │ │ +109 │ │ │ │ │ +111 void _p_r_i_n_t( │ │ │ │ │ +112 const std::string &s = "HybridFactor\n", │ │ │ │ │ +113 const _K_e_y_F_o_r_m_a_t_t_e_r &formatter = DefaultKeyFormatter) const override; │ │ │ │ │ +114 │ │ │ │ │ +118 │ │ │ │ │ +_1_2_0 bool _i_s_D_i_s_c_r_e_t_e() const { return isDiscrete_; } │ │ │ │ │ +121 │ │ │ │ │ +_1_2_3 bool _i_s_C_o_n_t_i_n_u_o_u_s() const { return isContinuous_; } │ │ │ │ │ +124 │ │ │ │ │ +_1_2_6 bool _i_s_H_y_b_r_i_d() const { return isHybrid_; } │ │ │ │ │ +127 │ │ │ │ │ +_1_2_9 size_t _n_r_C_o_n_t_i_n_u_o_u_s() const { return continuousKeys_.size(); } │ │ │ │ │ +130 │ │ │ │ │ +_1_3_2 const _D_i_s_c_r_e_t_e_K_e_y_s &_d_i_s_c_r_e_t_e_K_e_y_s() const { return discreteKeys_; } │ │ │ │ │ +133 │ │ │ │ │ +_1_3_5 const _K_e_y_V_e_c_t_o_r &_c_o_n_t_i_n_u_o_u_s_K_e_y_s() const { return continuousKeys_; } │ │ │ │ │ 136 │ │ │ │ │ -138 bool _e_q_u_a_l_s(const _H_y_b_r_i_d_F_a_c_t_o_r &lf, double tol = 1e-9) const override; │ │ │ │ │ -139 │ │ │ │ │ -141 void _p_r_i_n_t( │ │ │ │ │ -142 const std::string &s = "GaussianMixture\n", │ │ │ │ │ -143 const _K_e_y_F_o_r_m_a_t_t_e_r &formatter = DefaultKeyFormatter) const override; │ │ │ │ │ -144 │ │ │ │ │ -148 │ │ │ │ │ -150 _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r operator()( │ │ │ │ │ -151 const _D_i_s_c_r_e_t_e_V_a_l_u_e_s &discreteValues) const; │ │ │ │ │ -152 │ │ │ │ │ -154 size_t nrComponents() const; │ │ │ │ │ -155 │ │ │ │ │ -157 _K_e_y_V_e_c_t_o_r continuousParents() const; │ │ │ │ │ -158 │ │ │ │ │ -_1_6_1 double _l_o_g_N_o_r_m_a_l_i_z_a_t_i_o_n_C_o_n_s_t_a_n_t() const override { return logConstant_; } │ │ │ │ │ -162 │ │ │ │ │ -167 boost::shared_ptr likelihood( │ │ │ │ │ -168 const _V_e_c_t_o_r_V_a_l_u_e_s &given) const; │ │ │ │ │ -169 │ │ │ │ │ -171 const Conditionals &conditionals() const; │ │ │ │ │ -172 │ │ │ │ │ -180 _A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_<_K_e_y_> logProbability( │ │ │ │ │ -181 const _V_e_c_t_o_r_V_a_l_u_e_s &continuousValues) const; │ │ │ │ │ -182 │ │ │ │ │ -208 double error(const _H_y_b_r_i_d_V_a_l_u_e_s &values) const override; │ │ │ │ │ -209 │ │ │ │ │ -217 _A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_<_K_e_y_> error(const _V_e_c_t_o_r_V_a_l_u_e_s &continuousValues) │ │ │ │ │ -const; │ │ │ │ │ -218 │ │ │ │ │ -225 double logProbability(const _H_y_b_r_i_d_V_a_l_u_e_s &values) const override; │ │ │ │ │ -226 │ │ │ │ │ -228 double evaluate(const _H_y_b_r_i_d_V_a_l_u_e_s &values) const override; │ │ │ │ │ -229 │ │ │ │ │ -_2_3_1 double _o_p_e_r_a_t_o_r_(_)(const _H_y_b_r_i_d_V_a_l_u_e_s &values) const { │ │ │ │ │ -232 return evaluate(values); │ │ │ │ │ -233 } │ │ │ │ │ -234 │ │ │ │ │ -242 void prune(const _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r &decisionTree); │ │ │ │ │ -243 │ │ │ │ │ -251 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_T_r_e_e add(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_T_r_e_e &sum) const; │ │ │ │ │ -253 │ │ │ │ │ -254 private: │ │ │ │ │ -256 bool allFrontalsGiven(const _V_e_c_t_o_r_V_a_l_u_e_s &given) const; │ │ │ │ │ -257 │ │ │ │ │ -_2_5_9 friend class boost::serialization::access; │ │ │ │ │ -260 template │ │ │ │ │ -261 void serialize(Archive &ar, const unsigned int /*version*/) { │ │ │ │ │ -262 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e_F_a_c_t_o_r); │ │ │ │ │ -263 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e_C_o_n_d_i_t_i_o_n_a_l); │ │ │ │ │ -264 ar &BOOST_SERIALIZATION_NVP(conditionals_); │ │ │ │ │ -265 } │ │ │ │ │ -266}; │ │ │ │ │ -267 │ │ │ │ │ -269std::set _D_i_s_c_r_e_t_e_K_e_y_s_A_s_S_e_t(const DiscreteKeys &discreteKeys); │ │ │ │ │ -270 │ │ │ │ │ -271// traits │ │ │ │ │ -272template <> │ │ │ │ │ -_2_7_3struct _t_r_a_i_t_s<_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ -274 │ │ │ │ │ -275} // namespace gtsam │ │ │ │ │ +138 │ │ │ │ │ +139 private: │ │ │ │ │ +_1_4_1 friend class boost::serialization::access; │ │ │ │ │ +142 template │ │ │ │ │ +143 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ +144 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +145 ar &BOOST_SERIALIZATION_NVP(isDiscrete_); │ │ │ │ │ +146 ar &BOOST_SERIALIZATION_NVP(isContinuous_); │ │ │ │ │ +147 ar &BOOST_SERIALIZATION_NVP(isHybrid_); │ │ │ │ │ +148 ar &BOOST_SERIALIZATION_NVP(discreteKeys_); │ │ │ │ │ +149 ar &BOOST_SERIALIZATION_NVP(continuousKeys_); │ │ │ │ │ +150 } │ │ │ │ │ +151}; │ │ │ │ │ +152// HybridFactor │ │ │ │ │ +153 │ │ │ │ │ +154// traits │ │ │ │ │ +155template <> │ │ │ │ │ +_1_5_6struct _t_r_a_i_t_s<_H_y_b_r_i_d_F_a_c_t_o_r> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ +157 │ │ │ │ │ +158} // namespace gtsam │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ _D_e_c_i_s_i_o_n_T_r_e_e_._h │ │ │ │ │ Decision Tree for use in DiscreteFactors. │ │ │ │ │ _D_i_s_c_r_e_t_e_K_e_y_._h │ │ │ │ │ specialized key for discrete variables │ │ │ │ │ -_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_._h │ │ │ │ │ -_H_y_b_r_i_d_F_a_c_t_o_r_._h │ │ │ │ │ -_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r_._h │ │ │ │ │ -A set of GaussianFactors, indexed by a set of discrete keys. │ │ │ │ │ -_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ -Base class for conditional densities. │ │ │ │ │ -_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ -Conditional Gaussian Base class. │ │ │ │ │ +_F_a_c_t_o_r_._h │ │ │ │ │ +The base class for all factors. │ │ │ │ │ +_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ +_V_a_l_u_e_s_._h │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ FastVector< Key > KeyVector │ │ │ │ │ Define collection type once and for all - also used in wrappers. │ │ │ │ │ DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ _g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ print without optional string, must specify cout yourself │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s_A_s_S_e_t │ │ │ │ │ -std::set< DiscreteKey > DiscreteKeysAsSet(const DiscreteKeys &discreteKeys) │ │ │ │ │ -Return the DiscreteKey vector as a set. │ │ │ │ │ -DDeeffiinniittiioonn GaussianMixture.cpp:221 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_u_n_e_r_F_u_n_c │ │ │ │ │ -std::function< double(const Assignment< Key > &, double)> prunerFunc(const │ │ │ │ │ -DecisionTreeFactor &prunedDecisionTree, const HybridConditional &conditional) │ │ │ │ │ -Helper function to get the pruner functional. │ │ │ │ │ -DDeeffiinniittiioonn HybridBayesNet.cpp:66 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ _g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ Template to create a binary predicate. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ _g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ A helper that implements the traits interface for GTSAM types. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e │ │ │ │ │ -An algebraic decision tree fixes the range of a DecisionTree to double. │ │ │ │ │ -DDeeffiinniittiioonn AlgebraicDecisionTree.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_A_s_s_i_g_n_m_e_n_t │ │ │ │ │ -An assignment from labels to value index (size_t). │ │ │ │ │ -DDeeffiinniittiioonn Assignment.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_<_ _K_e_y_,_ _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r_ _> │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r │ │ │ │ │ -A discrete probabilistic factor. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTreeFactor.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e │ │ │ │ │ +a decision tree is a function from assignments to values. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree.h:61 │ │ │ │ │ _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ DiscreteKeys is a set of keys that can be assembled using the & operator. │ │ │ │ │ DDeeffiinniittiioonn DiscreteKey.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ -A map from keys to values. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteValues.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e │ │ │ │ │ -A conditional of gaussian mixtures indexed by discrete variables, as part of a │ │ │ │ │ -Bayes Network. │ │ │ │ │ -DDeeffiinniittiioonn GaussianMixture.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_:_:_l_o_g_N_o_r_m_a_l_i_z_a_t_i_o_n_C_o_n_s_t_a_n_t │ │ │ │ │ -double logNormalizationConstant() const override │ │ │ │ │ -The log normalization constant is max of the the individual log-normalization │ │ │ │ │ -constants. │ │ │ │ │ -DDeeffiinniittiioonn GaussianMixture.h:161 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ -double operator()(const HybridValues &values) const │ │ │ │ │ -Evaluate probability density, sugar. │ │ │ │ │ -DDeeffiinniittiioonn GaussianMixture.h:231 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_:_:_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e │ │ │ │ │ -GaussianMixture()=default │ │ │ │ │ -Default constructor, mainly for serialization. │ │ │ │ │ _g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r │ │ │ │ │ Base class for truly hybrid probabilistic factors. │ │ │ │ │ DDeeffiinniittiioonn HybridFactor.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_:_:_n_r_C_o_n_t_i_n_u_o_u_s │ │ │ │ │ +size_t nrContinuous() const │ │ │ │ │ +Return the number of continuous variables in this factor. │ │ │ │ │ +DDeeffiinniittiioonn HybridFactor.h:129 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_:_:_T_h_i_s │ │ │ │ │ +HybridFactor This │ │ │ │ │ +This class. │ │ │ │ │ +DDeeffiinniittiioonn HybridFactor.h:66 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< HybridFactor > shared_ptr │ │ │ │ │ shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn GaussianConditional.h:46 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ -with a unique integer... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +DDeeffiinniittiioonn HybridFactor.h:68 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_:_:_i_s_D_i_s_c_r_e_t_e │ │ │ │ │ +bool isDiscrete() const │ │ │ │ │ +True if this is a factor of discrete variables only. │ │ │ │ │ +DDeeffiinniittiioonn HybridFactor.h:120 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_:_:_i_s_H_y_b_r_i_d │ │ │ │ │ +bool isHybrid() const │ │ │ │ │ +True is this is a Discrete-Continuous factor. │ │ │ │ │ +DDeeffiinniittiioonn HybridFactor.h:126 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_:_:_H_y_b_r_i_d_F_a_c_t_o_r │ │ │ │ │ +HybridFactor()=default │ │ │ │ │ +Default constructor creates empty factor. │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_:_:_B_a_s_e │ │ │ │ │ +Factor Base │ │ │ │ │ +Our base class. │ │ │ │ │ +DDeeffiinniittiioonn HybridFactor.h:69 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_:_:_c_o_n_t_i_n_u_o_u_s_K_e_y_s │ │ │ │ │ +const KeyVector & continuousKeys() const │ │ │ │ │ +Return only the continuous keys for this factor. │ │ │ │ │ +DDeeffiinniittiioonn HybridFactor.h:135 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_:_:_i_s_C_o_n_t_i_n_u_o_u_s │ │ │ │ │ +bool isContinuous() const │ │ │ │ │ +True if this is a factor of continuous variables only. │ │ │ │ │ +DDeeffiinniittiioonn HybridFactor.h:123 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_:_:_c_o_n_t_i_n_u_o_u_s_K_e_y_s__ │ │ │ │ │ +KeyVector continuousKeys_ │ │ │ │ │ +Record continuous keys for book-keeping. │ │ │ │ │ +DDeeffiinniittiioonn HybridFactor.h:62 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_:_:_~_H_y_b_r_i_d_F_a_c_t_o_r │ │ │ │ │ +virtual ~HybridFactor()=default │ │ │ │ │ +Virtual destructor. │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_:_:_d_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ +const DiscreteKeys & discreteKeys() const │ │ │ │ │ +Return the discrete keys for this factor. │ │ │ │ │ +DDeeffiinniittiioonn HybridFactor.h:132 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:68 │ │ │ │ │ _H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ the error. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_._h │ │ │ │ │ + * _H_y_b_r_i_d_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00485.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridGaussianFactorGraph.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridBayesTree.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,60 +94,36 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
│ │ │ │ -
HybridGaussianFactorGraph.cpp File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
HybridBayesTree.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Hybrid factor graph that uses type erasure. │ │ │ │ +

Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree. │ │ │ │ More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Typedefs

│ │ │ │ -using gtsam::OrphanWrapper = BayesTreeOrphanWrapper< HybridBayesTree::Clique >
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

const Ordering gtsam::HybridOrdering (const HybridGaussianFactorGraph &graph)
 Return a Colamd constrained ordering where the discrete keys are eliminated after the continuous keys.
 
│ │ │ │ -GaussianFactorGraphTree gtsam::removeEmpty (const GaussianFactorGraphTree &sum)
 
std::pair< HybridConditional::shared_ptr, boost::shared_ptr< Factor > > gtsam::EliminateHybrid (const HybridGaussianFactorGraph &factors, const Ordering &keys)
 Main elimination function for HybridGaussianFactorGraph.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Hybrid factor graph that uses type erasure.

│ │ │ │ -
Author
Fan Jiang
│ │ │ │ -
│ │ │ │ -Varun Agrawal
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ +

Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree.

│ │ │ │
Date
Mar 11, 2022
│ │ │ │ +
Author
Fan Jiang, Varun Agrawal
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,43 +1,22 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -HybridGaussianFactorGraph.cpp File Reference │ │ │ │ │ -Hybrid factor graph that uses type erasure. _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +HybridBayesTree.cpp File Reference │ │ │ │ │ +Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -using  ggttssaamm::::OOrrpphhaannWWrraappppeerr = _B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r< _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_:_:_C_l_i_q_u_e > │ │ │ │ │ -  │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ - const _O_r_d_e_r_i_n_g  _g_t_s_a_m_:_:_H_y_b_r_i_d_O_r_d_e_r_i_n_g (const │ │ │ │ │ - _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &graph) │ │ │ │ │ - Return a Colamd constrained ordering │ │ │ │ │ -  where the discrete keys are eliminated │ │ │ │ │ - after the continuous keys. │ │ │ │ │ -  │ │ │ │ │ - _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_T_r_e_e  ggttssaamm::::rreemmoovveeEEmmppttyy (const │ │ │ │ │ - _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_T_r_e_e &sum) │ │ │ │ │ -  │ │ │ │ │ - std::pair< _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_:_: _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_H_y_b_r_i_d (const │ │ │ │ │ -_s_h_a_r_e_d___p_t_r, boost::shared_ptr< _F_a_c_t_o_r > _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &factors, │ │ │ │ │ - >  const _O_r_d_e_r_i_n_g &keys) │ │ │ │ │ -  Main elimination function for │ │ │ │ │ - _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Hybrid factor graph that uses type erasure. │ │ │ │ │ - Author │ │ │ │ │ - Fan Jiang │ │ │ │ │ - Varun Agrawal │ │ │ │ │ - Frank Dellaert │ │ │ │ │ +Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree. │ │ │ │ │ Date │ │ │ │ │ Mar 11, 2022 │ │ │ │ │ + Author │ │ │ │ │ + Fan Jiang, Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._c_p_p │ │ │ │ │ + * _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00488.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridJunctionTree.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridConditional.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,24 +96,26 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
HybridJunctionTree.h File Reference
│ │ │ │ +
HybridConditional.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::HybridJunctionTree
 An EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree, with the additional property that it represents the clique tree associated with a Bayes net. More...
class  gtsam::HybridConditional
 Hybrid Conditional Density. More...
 
struct  gtsam::traits< HybridConditional >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -122,13 +124,13 @@ │ │ │ │
Date
Mar 11, 2022
│ │ │ │
Author
Fan Jiang
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,27 +1,27 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -HybridJunctionTree.h File Reference │ │ │ │ │ +HybridConditional.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_H_y_b_r_i_d_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ - An _E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e, i.e., a set of variable clusters with │ │ │ │ │ -  factors, arranged in a tree, with the additional property that it │ │ │ │ │ - represents the clique tree associated with a Bayes net. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +  Hybrid _C_o_n_d_i_t_i_o_n_a_l Density. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Date │ │ │ │ │ Mar 11, 2022 │ │ │ │ │ Author │ │ │ │ │ Fan Jiang │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_J_u_n_c_t_i_o_n_T_r_e_e_._h │ │ │ │ │ + * _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00488_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridJunctionTree.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridConditional.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,15 +98,15 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
HybridJunctionTree.h
│ │ │ │ +
HybridConditional.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │ @@ -114,52 +114,190 @@ │ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
18#pragma once
│ │ │ │
19
│ │ │ │ -
20#include <gtsam/hybrid/HybridBayesTree.h>
│ │ │ │ -
21#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
│ │ │ │ -
22#include <gtsam/inference/JunctionTree.h>
│ │ │ │ -
23
│ │ │ │ -
24namespace gtsam {
│ │ │ │ -
25
│ │ │ │ -
26// Forward declarations
│ │ │ │ -
27class HybridEliminationTree;
│ │ │ │ -
28
│ │ │ │ -
│ │ │ │ -
52class GTSAM_EXPORT HybridJunctionTree
│ │ │ │ -
53 : public JunctionTree<HybridBayesTree, HybridGaussianFactorGraph> {
│ │ │ │ -
54
│ │ │ │ -
55 public:
│ │ │ │ -
56 typedef JunctionTree<HybridBayesTree, HybridGaussianFactorGraph>
│ │ │ │ -
57 Base;
│ │ │ │ -
58 typedef HybridJunctionTree This;
│ │ │ │ -
59 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
60
│ │ │ │ -
70 HybridJunctionTree(const HybridEliminationTree& eliminationTree);
│ │ │ │ -
71};
│ │ │ │ -
│ │ │ │ -
72
│ │ │ │ -
73} // namespace gtsam
│ │ │ │ -
HybridBayesTree.h
Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree.
│ │ │ │ -
HybridGaussianFactorGraph.h
Linearized Hybrid factor graph that uses type erasure.
│ │ │ │ -
JunctionTree.h
The junction tree.
│ │ │ │ +
20#include <gtsam/discrete/DiscreteConditional.h>
│ │ │ │ +
21#include <gtsam/hybrid/GaussianMixture.h>
│ │ │ │ +
22#include <gtsam/hybrid/HybridFactor.h>
│ │ │ │ +
23#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
│ │ │ │ +
24#include <gtsam/inference/Conditional.h>
│ │ │ │ +
25#include <gtsam/inference/Key.h>
│ │ │ │ +
26#include <gtsam/linear/GaussianConditional.h>
│ │ │ │ +
27
│ │ │ │ +
28#include <boost/make_shared.hpp>
│ │ │ │ +
29#include <boost/shared_ptr.hpp>
│ │ │ │ +
30#include <stdexcept>
│ │ │ │ +
31#include <string>
│ │ │ │ +
32#include <typeinfo>
│ │ │ │ +
33#include <vector>
│ │ │ │ +
34
│ │ │ │ +
35namespace gtsam {
│ │ │ │ +
36
│ │ │ │ +
│ │ │ │ +
60class GTSAM_EXPORT HybridConditional
│ │ │ │ +
61 : public HybridFactor,
│ │ │ │ +
62 public Conditional<HybridFactor, HybridConditional> {
│ │ │ │ +
63 public:
│ │ │ │ +
64 // typedefs needed to play nice with gtsam
│ │ │ │ +
65 typedef HybridConditional This;
│ │ │ │ +
66 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
67 typedef HybridFactor BaseFactor;
│ │ │ │ +
68 typedef Conditional<BaseFactor, This>
│ │ │ │ +
69 BaseConditional;
│ │ │ │ +
70
│ │ │ │ +
71 protected:
│ │ │ │ +
73 boost::shared_ptr<Factor> inner_;
│ │ │ │ +
74
│ │ │ │ +
75 public:
│ │ │ │ +
78
│ │ │ │ +
80 HybridConditional() = default;
│ │ │ │ +
81
│ │ │ │ +
│ │ │ │ +
89 HybridConditional(const KeyVector& continuousKeys,
│ │ │ │ +
90 const DiscreteKeys& discreteKeys, size_t nFrontals)
│ │ │ │ +
91 : BaseFactor(continuousKeys, discreteKeys), BaseConditional(nFrontals) {}
│ │ │ │ +
│ │ │ │ +
92
│ │ │ │ +
102 HybridConditional(const KeyVector& continuousFrontals,
│ │ │ │ +
103 const DiscreteKeys& discreteFrontals,
│ │ │ │ +
104 const KeyVector& continuousParents,
│ │ │ │ +
105 const DiscreteKeys& discreteParents);
│ │ │ │ +
106
│ │ │ │ +
113 HybridConditional(
│ │ │ │ +
114 const boost::shared_ptr<GaussianConditional>& continuousConditional);
│ │ │ │ +
115
│ │ │ │ +
122 HybridConditional(
│ │ │ │ +
123 const boost::shared_ptr<DiscreteConditional>& discreteConditional);
│ │ │ │ +
124
│ │ │ │ +
131 HybridConditional(const boost::shared_ptr<GaussianMixture>& gaussianMixture);
│ │ │ │ +
132
│ │ │ │ +
136
│ │ │ │ +
138 void print(
│ │ │ │ +
139 const std::string& s = "Hybrid Conditional: ",
│ │ │ │ +
140 const KeyFormatter& formatter = DefaultKeyFormatter) const override;
│ │ │ │ +
141
│ │ │ │ +
143 bool equals(const HybridFactor& other, double tol = 1e-9) const override;
│ │ │ │ +
144
│ │ │ │ +
148
│ │ │ │ +
│ │ │ │ +
154 GaussianMixture::shared_ptr asMixture() const {
│ │ │ │ +
155 return boost::dynamic_pointer_cast<GaussianMixture>(inner_);
│ │ │ │ +
156 }
│ │ │ │ +
│ │ │ │ +
157
│ │ │ │ +
│ │ │ │ +
163 GaussianConditional::shared_ptr asGaussian() const {
│ │ │ │ +
164 return boost::dynamic_pointer_cast<GaussianConditional>(inner_);
│ │ │ │ +
165 }
│ │ │ │ +
│ │ │ │ +
166
│ │ │ │ +
│ │ │ │ +
172 DiscreteConditional::shared_ptr asDiscrete() const {
│ │ │ │ +
173 return boost::dynamic_pointer_cast<DiscreteConditional>(inner_);
│ │ │ │ +
174 }
│ │ │ │ +
│ │ │ │ +
175
│ │ │ │ +
177 boost::shared_ptr<Factor> inner() const { return inner_; }
│ │ │ │ +
178
│ │ │ │ +
180 double error(const HybridValues& values) const override;
│ │ │ │ +
181
│ │ │ │ +
183 double logProbability(const HybridValues& values) const override;
│ │ │ │ +
184
│ │ │ │ +
190 double logNormalizationConstant() const override;
│ │ │ │ +
191
│ │ │ │ +
193 double evaluate(const HybridValues& values) const override;
│ │ │ │ +
194
│ │ │ │ +
│ │ │ │ +
196 bool frontalsIn(const VectorValues& measurements) const {
│ │ │ │ +
197 for (Key key : frontals()) {
│ │ │ │ +
198 if (!measurements.exists(key)) {
│ │ │ │ +
199 return false;
│ │ │ │ +
200 }
│ │ │ │ +
201 }
│ │ │ │ +
202 return true;
│ │ │ │ +
203 }
│ │ │ │ +
│ │ │ │ +
204
│ │ │ │ +
206
│ │ │ │ +
207 private:
│ │ │ │ +
209 friend class boost::serialization::access;
│ │ │ │ +
210 template <class Archive>
│ │ │ │ +
211 void serialize(Archive& ar, const unsigned int /*version*/) {
│ │ │ │ +
212 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseFactor);
│ │ │ │ +
213 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseConditional);
│ │ │ │ +
214 ar& BOOST_SERIALIZATION_NVP(inner_);
│ │ │ │ +
215
│ │ │ │ +
216 // register the various casts based on the type of inner_
│ │ │ │ +
217 // https://www.boost.org/doc/libs/1_80_0/libs/serialization/doc/serialization.html#runtimecasting
│ │ │ │ +
218 if (isDiscrete()) {
│ │ │ │ +
219 boost::serialization::void_cast_register<DiscreteConditional, Factor>(
│ │ │ │ +
220 static_cast<DiscreteConditional*>(NULL), static_cast<Factor*>(NULL));
│ │ │ │ +
221 } else if (isContinuous()) {
│ │ │ │ +
222 boost::serialization::void_cast_register<GaussianConditional, Factor>(
│ │ │ │ +
223 static_cast<GaussianConditional*>(NULL), static_cast<Factor*>(NULL));
│ │ │ │ +
224 } else {
│ │ │ │ +
225 boost::serialization::void_cast_register<GaussianMixture, Factor>(
│ │ │ │ +
226 static_cast<GaussianMixture*>(NULL), static_cast<Factor*>(NULL));
│ │ │ │ +
227 }
│ │ │ │ +
228 }
│ │ │ │ +
229
│ │ │ │ +
230}; // HybridConditional
│ │ │ │ +
│ │ │ │ +
231
│ │ │ │ +
232// traits
│ │ │ │ +
233template <>
│ │ │ │ +
234struct traits<HybridConditional> : public Testable<HybridConditional> {};
│ │ │ │ +
235
│ │ │ │ +
236} // namespace gtsam
│ │ │ │ + │ │ │ │ + │ │ │ │ +
GaussianMixture.h
A hybrid conditional in the Conditional Linear Gaussian scheme.
│ │ │ │ +
HybridGaussianFactorGraph.h
Linearized Hybrid factor graph that uses type erasure.
│ │ │ │ +
Conditional.h
Base class for conditional densities.
│ │ │ │ + │ │ │ │ +
GaussianConditional.h
Conditional Gaussian Base class.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::HybridEliminationTree
Elimination Tree type for Hybrid Factor Graphs.
Definition HybridEliminationTree.h:32
│ │ │ │ -
gtsam::HybridJunctionTree
An EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree,...
Definition HybridJunctionTree.h:53
│ │ │ │ -
gtsam::HybridJunctionTree::shared_ptr
boost::shared_ptr< This > shared_ptr
Shared pointer to this class.
Definition HybridJunctionTree.h:59
│ │ │ │ -
gtsam::HybridJunctionTree::Base
JunctionTree< HybridBayesTree, HybridGaussianFactorGraph > Base
Base class.
Definition HybridJunctionTree.h:57
│ │ │ │ -
gtsam::HybridJunctionTree::This
HybridJunctionTree This
This class.
Definition HybridJunctionTree.h:58
│ │ │ │ -
gtsam::JunctionTree
A JunctionTree is a cluster tree, a set of variable clusters with factors, arranged in a tree,...
Definition JunctionTree.h:50
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::DiscreteConditional
Discrete Conditional Density Derives from DecisionTreeFactor.
Definition DiscreteConditional.h:40
│ │ │ │ +
gtsam::DiscreteConditional::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition DiscreteConditional.h:44
│ │ │ │ +
gtsam::DiscreteKeys
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition DiscreteKey.h:39
│ │ │ │ +
gtsam::HybridConditional
Hybrid Conditional Density.
Definition HybridConditional.h:62
│ │ │ │ +
gtsam::HybridConditional::BaseConditional
Conditional< BaseFactor, This > BaseConditional
Typedef to our conditional base class.
Definition HybridConditional.h:69
│ │ │ │ +
gtsam::HybridConditional::BaseFactor
HybridFactor BaseFactor
Typedef to our factor base class.
Definition HybridConditional.h:67
│ │ │ │ +
gtsam::HybridConditional::inner_
boost::shared_ptr< Factor > inner_
Type-erased pointer to the inner type.
Definition HybridConditional.h:73
│ │ │ │ +
gtsam::HybridConditional::asDiscrete
DiscreteConditional::shared_ptr asDiscrete() const
Return conditional as a DiscreteConditional.
Definition HybridConditional.h:172
│ │ │ │ +
gtsam::HybridConditional::asMixture
GaussianMixture::shared_ptr asMixture() const
Return HybridConditional as a GaussianMixture.
Definition HybridConditional.h:154
│ │ │ │ +
gtsam::HybridConditional::inner
boost::shared_ptr< Factor > inner() const
Get the type-erased pointer to the inner type.
Definition HybridConditional.h:177
│ │ │ │ +
gtsam::HybridConditional::frontalsIn
bool frontalsIn(const VectorValues &measurements) const
Check if VectorValues measurements contains all frontal keys.
Definition HybridConditional.h:196
│ │ │ │ +
gtsam::HybridConditional::HybridConditional
HybridConditional(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, size_t nFrontals)
Construct a new Hybrid Conditional object.
Definition HybridConditional.h:89
│ │ │ │ +
gtsam::HybridConditional::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition HybridConditional.h:66
│ │ │ │ +
gtsam::HybridConditional::asGaussian
GaussianConditional::shared_ptr asGaussian() const
Return HybridConditional as a GaussianConditional.
Definition HybridConditional.h:163
│ │ │ │ +
gtsam::HybridConditional::This
HybridConditional This
Typedef to this class.
Definition HybridConditional.h:65
│ │ │ │ +
gtsam::HybridConditional::HybridConditional
HybridConditional()=default
Default constructor needed for serialization.
│ │ │ │ +
gtsam::HybridFactor
Base class for truly hybrid probabilistic factors.
Definition HybridFactor.h:52
│ │ │ │ +
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ +
gtsam::Conditional
Definition Conditional.h:64
│ │ │ │ +
gtsam::Factor
Definition Factor.h:68
│ │ │ │ +
gtsam::GaussianConditional
A GaussianConditional functions as the node in a Bayes network.
Definition GaussianConditional.h:43
│ │ │ │ +
gtsam::GaussianConditional::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianConditional.h:46
│ │ │ │ +
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
gtsam::VectorValues::exists
bool exists(Key j) const
Check whether a variable with key j exists.
Definition VectorValues.h:133
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -HybridJunctionTree.h │ │ │ │ │ +HybridConditional.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ @@ -16,65 +16,260 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 18#pragma once │ │ │ │ │ 19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_J_u_n_c_t_i_o_n_T_r_e_e_._h> │ │ │ │ │ -23 │ │ │ │ │ -24namespace _g_t_s_a_m { │ │ │ │ │ -25 │ │ │ │ │ -26// Forward declarations │ │ │ │ │ -27class HybridEliminationTree; │ │ │ │ │ -28 │ │ │ │ │ -_5_2class GTSAM_EXPORT _H_y_b_r_i_d_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ -53 : public _J_u_n_c_t_i_o_n_T_r_e_e { │ │ │ │ │ -54 │ │ │ │ │ -55 public: │ │ │ │ │ -56 typedef _J_u_n_c_t_i_o_n_T_r_e_e_<_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_,_ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_> │ │ │ │ │ -_5_7 _B_a_s_e; │ │ │ │ │ -_5_8 typedef _H_y_b_r_i_d_J_u_n_c_t_i_o_n_T_r_e_e _T_h_i_s; │ │ │ │ │ -_5_9 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -60 │ │ │ │ │ -70 _H_y_b_r_i_d_J_u_n_c_t_i_o_n_T_r_e_e(const _H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e& eliminationTree); │ │ │ │ │ -71}; │ │ │ │ │ -72 │ │ │ │ │ -73} // namespace gtsam │ │ │ │ │ -_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ -Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree. │ │ │ │ │ +20#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_F_a_c_t_o_r_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ +27 │ │ │ │ │ +28#include │ │ │ │ │ +29#include │ │ │ │ │ +30#include │ │ │ │ │ +31#include │ │ │ │ │ +32#include │ │ │ │ │ +33#include │ │ │ │ │ +34 │ │ │ │ │ +35namespace _g_t_s_a_m { │ │ │ │ │ +36 │ │ │ │ │ +_6_0class GTSAM_EXPORT _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +61 : public _H_y_b_r_i_d_F_a_c_t_o_r, │ │ │ │ │ +62 public _C_o_n_d_i_t_i_o_n_a_l { │ │ │ │ │ +63 public: │ │ │ │ │ +64 // typedefs needed to play nice with gtsam │ │ │ │ │ +_6_5 typedef _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l _T_h_i_s; │ │ │ │ │ +_6_6 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +_6_7 typedef _H_y_b_r_i_d_F_a_c_t_o_r _B_a_s_e_F_a_c_t_o_r; │ │ │ │ │ +68 typedef _C_o_n_d_i_t_i_o_n_a_l_<_B_a_s_e_F_a_c_t_o_r_,_ _T_h_i_s_> │ │ │ │ │ +_6_9 _B_a_s_e_C_o_n_d_i_t_i_o_n_a_l; │ │ │ │ │ +70 │ │ │ │ │ +71 protected: │ │ │ │ │ +_7_3 boost::shared_ptr _i_n_n_e_r__; │ │ │ │ │ +74 │ │ │ │ │ +75 public: │ │ │ │ │ +78 │ │ │ │ │ +_8_0 _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l() = default; │ │ │ │ │ +81 │ │ │ │ │ +_8_9 _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l(const _K_e_y_V_e_c_t_o_r& continuousKeys, │ │ │ │ │ +90 const _D_i_s_c_r_e_t_e_K_e_y_s& discreteKeys, size_t nFrontals) │ │ │ │ │ +91 : _B_a_s_e_F_a_c_t_o_r(continuousKeys, discreteKeys), _B_a_s_e_C_o_n_d_i_t_i_o_n_a_l(nFrontals) {} │ │ │ │ │ +92 │ │ │ │ │ +102 _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l(const _K_e_y_V_e_c_t_o_r& continuousFrontals, │ │ │ │ │ +103 const _D_i_s_c_r_e_t_e_K_e_y_s& discreteFrontals, │ │ │ │ │ +104 const _K_e_y_V_e_c_t_o_r& continuousParents, │ │ │ │ │ +105 const _D_i_s_c_r_e_t_e_K_e_y_s& discreteParents); │ │ │ │ │ +106 │ │ │ │ │ +113 _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l( │ │ │ │ │ +114 const boost::shared_ptr& continuousConditional); │ │ │ │ │ +115 │ │ │ │ │ +122 _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l( │ │ │ │ │ +123 const boost::shared_ptr& discreteConditional); │ │ │ │ │ +124 │ │ │ │ │ +131 _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l(const boost::shared_ptr& │ │ │ │ │ +gaussianMixture); │ │ │ │ │ +132 │ │ │ │ │ +136 │ │ │ │ │ +138 void _p_r_i_n_t( │ │ │ │ │ +139 const std::string& s = "Hybrid Conditional: ", │ │ │ │ │ +140 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override; │ │ │ │ │ +141 │ │ │ │ │ +143 bool _e_q_u_a_l_s(const _H_y_b_r_i_d_F_a_c_t_o_r& other, double tol = 1e-9) const override; │ │ │ │ │ +144 │ │ │ │ │ +148 │ │ │ │ │ +_1_5_4 GaussianMixture::shared_ptr _a_s_M_i_x_t_u_r_e() const { │ │ │ │ │ +155 return boost::dynamic_pointer_cast(inner_); │ │ │ │ │ +156 } │ │ │ │ │ +157 │ │ │ │ │ +_1_6_3 _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r _a_s_G_a_u_s_s_i_a_n() const { │ │ │ │ │ +164 return boost::dynamic_pointer_cast(inner_); │ │ │ │ │ +165 } │ │ │ │ │ +166 │ │ │ │ │ +_1_7_2 _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r _a_s_D_i_s_c_r_e_t_e() const { │ │ │ │ │ +173 return boost::dynamic_pointer_cast(inner_); │ │ │ │ │ +174 } │ │ │ │ │ +175 │ │ │ │ │ +_1_7_7 boost::shared_ptr _i_n_n_e_r() const { return inner_; } │ │ │ │ │ +178 │ │ │ │ │ +180 double error(const _H_y_b_r_i_d_V_a_l_u_e_s& values) const override; │ │ │ │ │ +181 │ │ │ │ │ +183 double logProbability(const _H_y_b_r_i_d_V_a_l_u_e_s& values) const override; │ │ │ │ │ +184 │ │ │ │ │ +190 double logNormalizationConstant() const override; │ │ │ │ │ +191 │ │ │ │ │ +193 double evaluate(const _H_y_b_r_i_d_V_a_l_u_e_s& values) const override; │ │ │ │ │ +194 │ │ │ │ │ +_1_9_6 bool _f_r_o_n_t_a_l_s_I_n(const _V_e_c_t_o_r_V_a_l_u_e_s& measurements) const { │ │ │ │ │ +197 for (_K_e_y key : frontals()) { │ │ │ │ │ +198 if (!measurements._e_x_i_s_t_s(key)) { │ │ │ │ │ +199 return false; │ │ │ │ │ +200 } │ │ │ │ │ +201 } │ │ │ │ │ +202 return true; │ │ │ │ │ +203 } │ │ │ │ │ +204 │ │ │ │ │ +206 │ │ │ │ │ +207 private: │ │ │ │ │ +_2_0_9 friend class boost::serialization::access; │ │ │ │ │ +210 template │ │ │ │ │ +211 void serialize(Archive& ar, const unsigned int /*version*/) { │ │ │ │ │ +212 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e_F_a_c_t_o_r); │ │ │ │ │ +213 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e_C_o_n_d_i_t_i_o_n_a_l); │ │ │ │ │ +214 ar& BOOST_SERIALIZATION_NVP(inner_); │ │ │ │ │ +215 │ │ │ │ │ +216 // register the various casts based on the type of inner_ │ │ │ │ │ +217 // https://www.boost.org/doc/libs/1_80_0/libs/serialization/doc/ │ │ │ │ │ +serialization.html#runtimecasting │ │ │ │ │ +218 if (isDiscrete()) { │ │ │ │ │ +219 boost::serialization::void_cast_register( │ │ │ │ │ +220 static_cast<_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l*>(NULL), static_cast<_F_a_c_t_o_r*>(NULL)); │ │ │ │ │ +221 } else if (isContinuous()) { │ │ │ │ │ +222 boost::serialization::void_cast_register( │ │ │ │ │ +223 static_cast<_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l*>(NULL), static_cast<_F_a_c_t_o_r*>(NULL)); │ │ │ │ │ +224 } else { │ │ │ │ │ +225 boost::serialization::void_cast_register( │ │ │ │ │ +226 static_cast(NULL), static_cast(NULL)); │ │ │ │ │ +227 } │ │ │ │ │ +228 } │ │ │ │ │ +229 │ │ │ │ │ +230}; // HybridConditional │ │ │ │ │ +231 │ │ │ │ │ +232// traits │ │ │ │ │ +233template <> │ │ │ │ │ +_2_3_4struct _t_r_a_i_t_s<_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ +235 │ │ │ │ │ +236} // namespace gtsam │ │ │ │ │ +_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ +_H_y_b_r_i_d_F_a_c_t_o_r_._h │ │ │ │ │ +_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_._h │ │ │ │ │ +A hybrid conditional in the Conditional Linear Gaussian scheme. │ │ │ │ │ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ Linearized Hybrid factor graph that uses type erasure. │ │ │ │ │ -_J_u_n_c_t_i_o_n_T_r_e_e_._h │ │ │ │ │ -The junction tree. │ │ │ │ │ +_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ +Base class for conditional densities. │ │ │ │ │ +_K_e_y_._h │ │ │ │ │ +_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ +Conditional Gaussian Base class. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ -Elimination Tree type for Hybrid Factor Graphs. │ │ │ │ │ -DDeeffiinniittiioonn HybridEliminationTree.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ -An EliminatableClusterTree, i.e., a set of variable clusters with factors, │ │ │ │ │ -arranged in a tree,... │ │ │ │ │ -DDeeffiinniittiioonn HybridJunctionTree.h:53 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_J_u_n_c_t_i_o_n_T_r_e_e_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +Discrete Conditional Density Derives from DecisionTreeFactor. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteConditional.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ boost::shared_ptr< This > shared_ptr │ │ │ │ │ -Shared pointer to this class. │ │ │ │ │ -DDeeffiinniittiioonn HybridJunctionTree.h:59 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_J_u_n_c_t_i_o_n_T_r_e_e_:_:_B_a_s_e │ │ │ │ │ -JunctionTree< HybridBayesTree, HybridGaussianFactorGraph > Base │ │ │ │ │ -Base class. │ │ │ │ │ -DDeeffiinniittiioonn HybridJunctionTree.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_J_u_n_c_t_i_o_n_T_r_e_e_:_:_T_h_i_s │ │ │ │ │ -HybridJunctionTree This │ │ │ │ │ -This class. │ │ │ │ │ -DDeeffiinniittiioonn HybridJunctionTree.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ -A JunctionTree is a cluster tree, a set of variable clusters with factors, │ │ │ │ │ -arranged in a tree,... │ │ │ │ │ -DDeeffiinniittiioonn JunctionTree.h:50 │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn DiscreteConditional.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ +DiscreteKeys is a set of keys that can be assembled using the & operator. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteKey.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +Hybrid Conditional Density. │ │ │ │ │ +DDeeffiinniittiioonn HybridConditional.h:62 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_:_:_B_a_s_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +Conditional< BaseFactor, This > BaseConditional │ │ │ │ │ +Typedef to our conditional base class. │ │ │ │ │ +DDeeffiinniittiioonn HybridConditional.h:69 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_:_:_B_a_s_e_F_a_c_t_o_r │ │ │ │ │ +HybridFactor BaseFactor │ │ │ │ │ +Typedef to our factor base class. │ │ │ │ │ +DDeeffiinniittiioonn HybridConditional.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_:_:_i_n_n_e_r__ │ │ │ │ │ +boost::shared_ptr< Factor > inner_ │ │ │ │ │ +Type-erased pointer to the inner type. │ │ │ │ │ +DDeeffiinniittiioonn HybridConditional.h:73 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_:_:_a_s_D_i_s_c_r_e_t_e │ │ │ │ │ +DiscreteConditional::shared_ptr asDiscrete() const │ │ │ │ │ +Return conditional as a DiscreteConditional. │ │ │ │ │ +DDeeffiinniittiioonn HybridConditional.h:172 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_:_:_a_s_M_i_x_t_u_r_e │ │ │ │ │ +GaussianMixture::shared_ptr asMixture() const │ │ │ │ │ +Return HybridConditional as a GaussianMixture. │ │ │ │ │ +DDeeffiinniittiioonn HybridConditional.h:154 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_:_:_i_n_n_e_r │ │ │ │ │ +boost::shared_ptr< Factor > inner() const │ │ │ │ │ +Get the type-erased pointer to the inner type. │ │ │ │ │ +DDeeffiinniittiioonn HybridConditional.h:177 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_:_:_f_r_o_n_t_a_l_s_I_n │ │ │ │ │ +bool frontalsIn(const VectorValues &measurements) const │ │ │ │ │ +Check if VectorValues measurements contains all frontal keys. │ │ │ │ │ +DDeeffiinniittiioonn HybridConditional.h:196 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +HybridConditional(const KeyVector &continuousKeys, const DiscreteKeys │ │ │ │ │ +&discreteKeys, size_t nFrontals) │ │ │ │ │ +Construct a new Hybrid Conditional object. │ │ │ │ │ +DDeeffiinniittiioonn HybridConditional.h:89 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn HybridConditional.h:66 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_:_:_a_s_G_a_u_s_s_i_a_n │ │ │ │ │ +GaussianConditional::shared_ptr asGaussian() const │ │ │ │ │ +Return HybridConditional as a GaussianConditional. │ │ │ │ │ +DDeeffiinniittiioonn HybridConditional.h:163 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_:_:_T_h_i_s │ │ │ │ │ +HybridConditional This │ │ │ │ │ +Typedef to this class. │ │ │ │ │ +DDeeffiinniittiioonn HybridConditional.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +HybridConditional()=default │ │ │ │ │ +Default constructor needed for serialization. │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r │ │ │ │ │ +Base class for truly hybrid probabilistic factors. │ │ │ │ │ +DDeeffiinniittiioonn HybridFactor.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:68 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +A GaussianConditional functions as the node in a Bayes network. │ │ │ │ │ +DDeeffiinniittiioonn GaussianConditional.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn GaussianConditional.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ +with a unique integer... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_e_x_i_s_t_s │ │ │ │ │ +bool exists(Key j) const │ │ │ │ │ +Check whether a variable with key j exists. │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:133 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_J_u_n_c_t_i_o_n_T_r_e_e_._h │ │ │ │ │ + * _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00491.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridBayesNet.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridConditional.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,49 +94,32 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
HybridBayesNet.cpp File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
HybridConditional.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ - │ │ │ │ -

A Bayes net of Gaussian Conditionals indexed by discrete keys. │ │ │ │ -More...

│ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

std::function< double(const Assignment< Key > &, double)> gtsam::prunerFunc (const DecisionTreeFactor &prunedDecisionTree, const HybridConditional &conditional)
 Helper function to get the pruner functional.
 
│ │ │ │

Detailed Description

│ │ │ │ -

A Bayes net of Gaussian Conditionals indexed by discrete keys.

│ │ │ │ -
Author
Fan Jiang
│ │ │ │ -
│ │ │ │ -Varun Agrawal
│ │ │ │ -
│ │ │ │ -Shangjie Xue
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ -
Date
January 2022
│ │ │ │ +
Date
Mar 11, 2022
│ │ │ │ +
Author
Fan Jiang
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,32 +1,20 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -HybridBayesNet.cpp File Reference │ │ │ │ │ -A Bayes net of Gaussian Conditionals indexed by discrete keys. _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +HybridConditional.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -std::function< double(const _A_s_s_i_g_n_m_e_n_t< _g_t_s_a_m_:_:_p_r_u_n_e_r_F_u_n_c (const │ │ │ │ │ - _K_e_y > &, double)>  _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r &prunedDecisionTree, │ │ │ │ │ - const _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l &conditional) │ │ │ │ │ -  Helper function to get the pruner │ │ │ │ │ - functional. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A Bayes net of Gaussian Conditionals indexed by discrete keys. │ │ │ │ │ + Date │ │ │ │ │ + Mar 11, 2022 │ │ │ │ │ Author │ │ │ │ │ Fan Jiang │ │ │ │ │ - Varun Agrawal │ │ │ │ │ - Shangjie Xue │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - January 2022 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_B_a_y_e_s_N_e_t_._c_p_p │ │ │ │ │ + * _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00494.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridBayesTree.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridEliminationTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,52 +96,39 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
HybridBayesTree.h File Reference
│ │ │ │ +
HybridEliminationTree.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree. │ │ │ │ -More...

│ │ │ │ - │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::HybridBayesTreeClique
 A clique in a HybridBayesTree which is a HybridConditional internally. More...
 
class  gtsam::HybridBayesTree
 A Bayes tree representing a Hybrid density. More...
 
struct  gtsam::traits< HybridBayesTree >
 traits More...
 
class  gtsam::BayesTreeOrphanWrapper< HybridBayesTreeClique >
 Class for Hybrid Bayes tree orphan subtrees. More...
class  gtsam::HybridEliminationTree
 Elimination Tree type for Hybrid Factor Graphs. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree.

│ │ │ │ -
Date
Mar 11, 2022
│ │ │ │ +
Date
Mar 11, 2022
│ │ │ │
Author
Fan Jiang
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,37 +1,25 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -HybridBayesTree.h File Reference │ │ │ │ │ -Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree. _M_o_r_e_._._. │ │ │ │ │ +HybridEliminationTree.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e │ │ │ │ │ -  A clique in a _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e which is a _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l internally. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e │ │ │ │ │ -  A Bayes tree representing a Hybrid density. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r_<_ _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_ _> │ │ │ │ │ -  Class for Hybrid Bayes tree orphan subtrees. _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ +  Elimination Tree type for Hybrid _F_a_c_t_o_r Graphs. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree. │ │ │ │ │ Date │ │ │ │ │ Mar 11, 2022 │ │ │ │ │ Author │ │ │ │ │ Fan Jiang │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ + * _H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00494_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridBayesTree.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridEliminationTree.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,164 +98,78 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
HybridBayesTree.h
│ │ │ │ +
HybridEliminationTree.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/hybrid/HybridBayesNet.h>
│ │ │ │ -
23#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
│ │ │ │ -
24#include <gtsam/inference/BayesTree.h>
│ │ │ │ -
25#include <gtsam/inference/BayesTreeCliqueBase.h>
│ │ │ │ -
26#include <gtsam/inference/Conditional.h>
│ │ │ │ -
27#include <gtsam/linear/GaussianBayesTree.h>
│ │ │ │ -
28
│ │ │ │ -
29#include <string>
│ │ │ │ -
30
│ │ │ │ -
31namespace gtsam {
│ │ │ │ -
32
│ │ │ │ -
33// Forward declarations
│ │ │ │ -
34class HybridConditional;
│ │ │ │ -
35class VectorValues;
│ │ │ │ -
36
│ │ │ │ -
37/* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
44class GTSAM_EXPORT HybridBayesTreeClique
│ │ │ │ -
45 : public BayesTreeCliqueBase<HybridBayesTreeClique,
│ │ │ │ -
46 HybridGaussianFactorGraph> {
│ │ │ │ -
47 public:
│ │ │ │ -
48 typedef HybridBayesTreeClique This;
│ │ │ │ -
49 typedef BayesTreeCliqueBase<HybridBayesTreeClique, HybridGaussianFactorGraph>
│ │ │ │ -
50 Base;
│ │ │ │ -
51 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
52 typedef boost::weak_ptr<This> weak_ptr;
│ │ │ │ -
53 HybridBayesTreeClique() {}
│ │ │ │ -
54 HybridBayesTreeClique(const boost::shared_ptr<HybridConditional>& conditional)
│ │ │ │ -
55 : Base(conditional) {}
│ │ │ │ -
57 HybridBayesTreeClique(const HybridBayesTreeClique& clique) : Base(clique) {}
│ │ │ │ -
58
│ │ │ │ -
59 virtual ~HybridBayesTreeClique() {}
│ │ │ │ -
60};
│ │ │ │ -
│ │ │ │ -
61
│ │ │ │ -
62/* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
64class GTSAM_EXPORT HybridBayesTree : public BayesTree<HybridBayesTreeClique> {
│ │ │ │ -
65 private:
│ │ │ │ -
66 typedef BayesTree<HybridBayesTreeClique> Base;
│ │ │ │ -
67
│ │ │ │ -
68 public:
│ │ │ │ -
69 typedef HybridBayesTree This;
│ │ │ │ -
70 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
71
│ │ │ │ -
74
│ │ │ │ -
75 HybridBayesTree() = default;
│ │ │ │ -
76
│ │ │ │ -
78 bool equals(const This& other, double tol = 1e-9) const;
│ │ │ │ -
79
│ │ │ │ -
87 GaussianBayesTree choose(const DiscreteValues& assignment) const;
│ │ │ │ -
88
│ │ │ │ -
96 HybridValues optimize() const;
│ │ │ │ -
97
│ │ │ │ -
105 VectorValues optimize(const DiscreteValues& assignment) const;
│ │ │ │ -
106
│ │ │ │ -
112 void prune(const size_t maxNumberLeaves);
│ │ │ │ -
113
│ │ │ │ -
115
│ │ │ │ -
116 private:
│ │ │ │ -
118 friend class boost::serialization::access;
│ │ │ │ -
119 template <class ARCHIVE>
│ │ │ │ -
120 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ -
121 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
122 }
│ │ │ │ -
123};
│ │ │ │ -
│ │ │ │ -
124
│ │ │ │ -
126template <>
│ │ │ │ -
127struct traits<HybridBayesTree> : public Testable<HybridBayesTree> {};
│ │ │ │ -
128
│ │ │ │ -
139template <>
│ │ │ │ -
│ │ │ │ -
140class BayesTreeOrphanWrapper<HybridBayesTreeClique> : public HybridConditional {
│ │ │ │ -
141 public:
│ │ │ │ -
142 typedef HybridBayesTreeClique CliqueType;
│ │ │ │ -
143 typedef HybridConditional Base;
│ │ │ │ -
144
│ │ │ │ -
145 boost::shared_ptr<CliqueType> clique;
│ │ │ │ -
146
│ │ │ │ -
│ │ │ │ -
152 BayesTreeOrphanWrapper(const boost::shared_ptr<CliqueType>& clique)
│ │ │ │ -
153 : clique(clique) {
│ │ │ │ -
154 // Store parent keys in our base type factor so that eliminating those
│ │ │ │ -
155 // parent keys will pull this subtree into the elimination.
│ │ │ │ -
156 this->keys_.assign(clique->conditional()->beginParents(),
│ │ │ │ -
157 clique->conditional()->endParents());
│ │ │ │ -
158 this->discreteKeys_.assign(clique->conditional()->discreteKeys().begin(),
│ │ │ │ -
159 clique->conditional()->discreteKeys().end());
│ │ │ │ -
160 }
│ │ │ │ -
│ │ │ │ -
161
│ │ │ │ -
│ │ │ │ -
163 void print(
│ │ │ │ -
164 const std::string& s = "",
│ │ │ │ -
165 const KeyFormatter& formatter = DefaultKeyFormatter) const override {
│ │ │ │ -
166 clique->print(s + "stored clique", formatter);
│ │ │ │ -
167 }
│ │ │ │ -
│ │ │ │ -
168};
│ │ │ │ -
│ │ │ │ -
169
│ │ │ │ -
170} // namespace gtsam
│ │ │ │ -
HybridBayesNet.h
A Bayes net of Gaussian Conditionals indexed by discrete keys.
│ │ │ │ -
HybridGaussianFactorGraph.h
Linearized Hybrid factor graph that uses type erasure.
│ │ │ │ -
BayesTree.h
Bayes Tree is a tree of cliques of a Bayes Chain.
│ │ │ │ -
Conditional.h
Base class for conditional densities.
│ │ │ │ -
BayesTreeCliqueBase.h
Base class for cliques of a BayesTree.
│ │ │ │ -
GaussianBayesTree.h
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/hybrid/HybridBayesNet.h>
│ │ │ │ +
21#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
│ │ │ │ +
22#include <gtsam/inference/EliminationTree.h>
│ │ │ │ +
23
│ │ │ │ +
24namespace gtsam {
│ │ │ │ +
25
│ │ │ │ +
│ │ │ │ +
31class GTSAM_EXPORT HybridEliminationTree
│ │ │ │ +
32 : public EliminationTree<HybridBayesNet, HybridGaussianFactorGraph> {
│ │ │ │ +
33 private:
│ │ │ │ +
34 friend class ::EliminationTreeTester;
│ │ │ │ +
35
│ │ │ │ +
36 public:
│ │ │ │ +
37 typedef EliminationTree<HybridBayesNet, HybridGaussianFactorGraph>
│ │ │ │ +
38 Base;
│ │ │ │ +
39 typedef HybridEliminationTree This;
│ │ │ │ +
40 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
41
│ │ │ │ +
44
│ │ │ │ +
54 HybridEliminationTree(const HybridGaussianFactorGraph& factorGraph,
│ │ │ │ +
55 const VariableIndex& structure, const Ordering& order);
│ │ │ │ +
56
│ │ │ │ +
62 HybridEliminationTree(const HybridGaussianFactorGraph& factorGraph,
│ │ │ │ +
63 const Ordering& order);
│ │ │ │ +
64
│ │ │ │ +
66
│ │ │ │ +
68 bool equals(const This& other, double tol = 1e-9) const;
│ │ │ │ +
69};
│ │ │ │ +
│ │ │ │ +
70
│ │ │ │ +
71} // namespace gtsam
│ │ │ │ +
HybridGaussianFactorGraph.h
Linearized Hybrid factor graph that uses type erasure.
│ │ │ │ +
HybridBayesNet.h
A Bayes net of Gaussian Conditionals indexed by discrete keys.
│ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::DiscreteValues
A map from keys to values.
Definition DiscreteValues.h:34
│ │ │ │ -
gtsam::HybridBayesTreeClique
A clique in a HybridBayesTree which is a HybridConditional internally.
Definition HybridBayesTree.h:46
│ │ │ │ -
gtsam::HybridBayesTreeClique::HybridBayesTreeClique
HybridBayesTreeClique(const boost::shared_ptr< HybridConditional > &conditional)
Copy constructor.
Definition HybridBayesTree.h:54
│ │ │ │ -
gtsam::HybridBayesTree
A Bayes tree representing a Hybrid density.
Definition HybridBayesTree.h:64
│ │ │ │ -
gtsam::HybridBayesTree::HybridBayesTree
HybridBayesTree()=default
Default constructor, creates an empty Bayes tree.
│ │ │ │ -
gtsam::BayesTreeOrphanWrapper< HybridBayesTreeClique >::BayesTreeOrphanWrapper
BayesTreeOrphanWrapper(const boost::shared_ptr< CliqueType > &clique)
Construct a new Bayes Tree Orphan Wrapper object.
Definition HybridBayesTree.h:152
│ │ │ │ -
gtsam::BayesTreeOrphanWrapper< HybridBayesTreeClique >::print
void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print utility
Definition HybridBayesTree.h:163
│ │ │ │ -
gtsam::HybridConditional
Hybrid Conditional Density.
Definition HybridConditional.h:62
│ │ │ │ -
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ -
gtsam::BayesTree
Bayes tree.
Definition BayesTree.h:67
│ │ │ │ -
gtsam::BayesTreeOrphanWrapper
Definition BayesTree.h:276
│ │ │ │ -
gtsam::BayesTreeCliqueBase
This is the base class for BayesTree cliques.
Definition BayesTreeCliqueBase.h:50
│ │ │ │ -
gtsam::GaussianBayesTree
A Bayes tree representing a Gaussian density.
Definition GaussianBayesTree.h:52
│ │ │ │ -
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ -
VectorValues
The Factor::error simply extracts the.
│ │ │ │ +
gtsam::HybridEliminationTree
Elimination Tree type for Hybrid Factor Graphs.
Definition HybridEliminationTree.h:32
│ │ │ │ +
gtsam::HybridEliminationTree::This
HybridEliminationTree This
This class.
Definition HybridEliminationTree.h:39
│ │ │ │ +
gtsam::HybridEliminationTree::shared_ptr
boost::shared_ptr< This > shared_ptr
Shared pointer to this class.
Definition HybridEliminationTree.h:40
│ │ │ │ +
gtsam::HybridEliminationTree::Base
EliminationTree< HybridBayesNet, HybridGaussianFactorGraph > Base
Base class.
Definition HybridEliminationTree.h:38
│ │ │ │ +
gtsam::HybridGaussianFactorGraph
Definition HybridGaussianFactorGraph.h:102
│ │ │ │ +
gtsam::EliminationTree
An elimination tree is a data structure used intermediately during elimination.
Definition EliminationTree.h:52
│ │ │ │ +
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition VariableIndex.h:43
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,208 +1,92 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -HybridBayesTree.h │ │ │ │ │ +HybridEliminationTree.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ -27#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ -28 │ │ │ │ │ -29#include │ │ │ │ │ -30 │ │ │ │ │ -31namespace _g_t_s_a_m { │ │ │ │ │ -32 │ │ │ │ │ -33// Forward declarations │ │ │ │ │ -34class HybridConditional; │ │ │ │ │ -35class _V_e_c_t_o_r_V_a_l_u_e_s; │ │ │ │ │ -36 │ │ │ │ │ -37/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -_4_4class GTSAM_EXPORT _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e │ │ │ │ │ -45 : public _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e { │ │ │ │ │ -47 public: │ │ │ │ │ -48 typedef _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e _T_h_i_s; │ │ │ │ │ -49 typedef _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_, │ │ │ │ │ -_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_> │ │ │ │ │ -50 _B_a_s_e; │ │ │ │ │ -51 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -52 typedef boost::weak_ptr weak_ptr; │ │ │ │ │ -53 _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e() {} │ │ │ │ │ -_5_4 _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e(const boost::shared_ptr& │ │ │ │ │ -conditional) │ │ │ │ │ -55 : _B_a_s_e(conditional) {} │ │ │ │ │ -57 _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e(const _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e& clique) : Base(clique) {} │ │ │ │ │ -58 │ │ │ │ │ -59 virtual ~HybridBayesTreeClique() {} │ │ │ │ │ -60}; │ │ │ │ │ -61 │ │ │ │ │ -62/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -_6_4class GTSAM_EXPORT _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e : public _B_a_y_e_s_T_r_e_e │ │ │ │ │ -{ │ │ │ │ │ -65 private: │ │ │ │ │ -66 typedef _B_a_y_e_s_T_r_e_e_<_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_> _B_a_s_e; │ │ │ │ │ -67 │ │ │ │ │ -68 public: │ │ │ │ │ -69 typedef _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e _T_h_i_s; │ │ │ │ │ -70 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -71 │ │ │ │ │ -74 │ │ │ │ │ -_7_5 _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e() = default; │ │ │ │ │ -76 │ │ │ │ │ -78 bool _e_q_u_a_l_s(const _T_h_i_s& other, double tol = 1e-9) const; │ │ │ │ │ -79 │ │ │ │ │ -87 _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e choose(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& assignment) const; │ │ │ │ │ -88 │ │ │ │ │ -96 _H_y_b_r_i_d_V_a_l_u_e_s _o_p_t_i_m_i_z_e() const; │ │ │ │ │ -97 │ │ │ │ │ -105 _V_e_c_t_o_r_V_a_l_u_e_s _o_p_t_i_m_i_z_e(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& assignment) const; │ │ │ │ │ -106 │ │ │ │ │ -112 void prune(const size_t maxNumberLeaves); │ │ │ │ │ -113 │ │ │ │ │ -115 │ │ │ │ │ -116 private: │ │ │ │ │ -_1_1_8 friend class boost::serialization::access; │ │ │ │ │ -119 template │ │ │ │ │ -120 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ -121 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -122 } │ │ │ │ │ -123}; │ │ │ │ │ -124 │ │ │ │ │ -126template <> │ │ │ │ │ -_1_2_7struct _t_r_a_i_t_s<_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ -128 │ │ │ │ │ -139template <> │ │ │ │ │ -_1_4_0class _B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r<_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e> : public │ │ │ │ │ -_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l { │ │ │ │ │ -141 public: │ │ │ │ │ -142 typedef _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e _C_l_i_q_u_e_T_y_p_e; │ │ │ │ │ -143 typedef _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l _B_a_s_e; │ │ │ │ │ -144 │ │ │ │ │ -145 boost::shared_ptr clique; │ │ │ │ │ -146 │ │ │ │ │ -_1_5_2 _B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r(const boost::shared_ptr& clique) │ │ │ │ │ -153 : clique(clique) { │ │ │ │ │ -154 // Store parent keys in our base type factor so that eliminating those │ │ │ │ │ -155 // parent keys will pull this subtree into the elimination. │ │ │ │ │ -156 this->keys_.assign(clique->conditional()->beginParents(), │ │ │ │ │ -157 clique->conditional()->endParents()); │ │ │ │ │ -158 this->discreteKeys_.assign(clique->conditional()->discreteKeys().begin(), │ │ │ │ │ -159 clique->conditional()->discreteKeys().end()); │ │ │ │ │ -160 } │ │ │ │ │ -161 │ │ │ │ │ -_1_6_3 void _p_r_i_n_t( │ │ │ │ │ -164 const std::string& s = "", │ │ │ │ │ -165 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override { │ │ │ │ │ -166 clique->print(s + "stored clique", formatter); │ │ │ │ │ -167 } │ │ │ │ │ -168}; │ │ │ │ │ -169 │ │ │ │ │ -170} // namespace gtsam │ │ │ │ │ -_H_y_b_r_i_d_B_a_y_e_s_N_e_t_._h │ │ │ │ │ -A Bayes net of Gaussian Conditionals indexed by discrete keys. │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h> │ │ │ │ │ +23 │ │ │ │ │ +24namespace _g_t_s_a_m { │ │ │ │ │ +25 │ │ │ │ │ +_3_1class GTSAM_EXPORT _H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ +32 : public _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e { │ │ │ │ │ +33 private: │ │ │ │ │ +34 friend class ::EliminationTreeTester; │ │ │ │ │ +35 │ │ │ │ │ +36 public: │ │ │ │ │ +37 typedef _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_H_y_b_r_i_d_B_a_y_e_s_N_e_t_,_ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_> │ │ │ │ │ +_3_8 _B_a_s_e; │ │ │ │ │ +_3_9 typedef _H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e _T_h_i_s; │ │ │ │ │ +_4_0 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +41 │ │ │ │ │ +44 │ │ │ │ │ +54 _H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e(const _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& factorGraph, │ │ │ │ │ +55 const _V_a_r_i_a_b_l_e_I_n_d_e_x& structure, const _O_r_d_e_r_i_n_g& order); │ │ │ │ │ +56 │ │ │ │ │ +62 _H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e(const _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& factorGraph, │ │ │ │ │ +63 const _O_r_d_e_r_i_n_g& order); │ │ │ │ │ +64 │ │ │ │ │ +66 │ │ │ │ │ +68 bool _e_q_u_a_l_s(const _T_h_i_s& other, double tol = 1e-9) const; │ │ │ │ │ +69}; │ │ │ │ │ +70 │ │ │ │ │ +71} // namespace gtsam │ │ │ │ │ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ Linearized Hybrid factor graph that uses type erasure. │ │ │ │ │ -_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ -Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ │ -_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ -Base class for conditional densities. │ │ │ │ │ -_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._h │ │ │ │ │ -Base class for cliques of a BayesTree. │ │ │ │ │ -_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ -Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ │ +_H_y_b_r_i_d_B_a_y_e_s_N_e_t_._h │ │ │ │ │ +A Bayes net of Gaussian Conditionals indexed by discrete keys. │ │ │ │ │ +_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ -Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key │ │ │ │ │ -landmarkKey) │ │ │ │ │ -Optimize for triangulation. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.cpp:155 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ _g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ Template to create a binary predicate. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ -A map from keys to values. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteValues.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e │ │ │ │ │ -A clique in a HybridBayesTree which is a HybridConditional internally. │ │ │ │ │ -DDeeffiinniittiioonn HybridBayesTree.h:46 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_:_:_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e │ │ │ │ │ -HybridBayesTreeClique(const boost::shared_ptr< HybridConditional > │ │ │ │ │ -&conditional) │ │ │ │ │ -Copy constructor. │ │ │ │ │ -DDeeffiinniittiioonn HybridBayesTree.h:54 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e │ │ │ │ │ -A Bayes tree representing a Hybrid density. │ │ │ │ │ -DDeeffiinniittiioonn HybridBayesTree.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_:_:_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e │ │ │ │ │ -HybridBayesTree()=default │ │ │ │ │ -Default constructor, creates an empty Bayes tree. │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r_<_ _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_ _>_:_:_B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r │ │ │ │ │ -BayesTreeOrphanWrapper(const boost::shared_ptr< CliqueType > &clique) │ │ │ │ │ -Construct a new Bayes Tree Orphan Wrapper object. │ │ │ │ │ -DDeeffiinniittiioonn HybridBayesTree.h:152 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r_<_ _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_ _>_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&formatter=DefaultKeyFormatter) const override │ │ │ │ │ -print utility │ │ │ │ │ -DDeeffiinniittiioonn HybridBayesTree.h:163 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -Hybrid Conditional Density. │ │ │ │ │ -DDeeffiinniittiioonn HybridConditional.h:62 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e │ │ │ │ │ -Bayes tree. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:276 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e │ │ │ │ │ -This is the base class for BayesTree cliques. │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e │ │ │ │ │ -A Bayes tree representing a Gaussian density. │ │ │ │ │ -DDeeffiinniittiioonn GaussianBayesTree.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ -with a unique integer... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -The Factor::error simply extracts the. │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ +Elimination Tree type for Hybrid Factor Graphs. │ │ │ │ │ +DDeeffiinniittiioonn HybridEliminationTree.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_T_h_i_s │ │ │ │ │ +HybridEliminationTree This │ │ │ │ │ +This class. │ │ │ │ │ +DDeeffiinniittiioonn HybridEliminationTree.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +Shared pointer to this class. │ │ │ │ │ +DDeeffiinniittiioonn HybridEliminationTree.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_B_a_s_e │ │ │ │ │ +EliminationTree< HybridBayesNet, HybridGaussianFactorGraph > Base │ │ │ │ │ +Base class. │ │ │ │ │ +DDeeffiinniittiioonn HybridEliminationTree.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +DDeeffiinniittiioonn HybridGaussianFactorGraph.h:102 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ +An elimination tree is a data structure used intermediately during elimination. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ +The VariableIndex class computes and stores the block column structure of a │ │ │ │ │ +factor graph. │ │ │ │ │ +DDeeffiinniittiioonn VariableIndex.h:43 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ + * _H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00497.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridValues.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridBayesTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,41 +96,52 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
HybridValues.h File Reference
│ │ │ │ +
HybridBayesTree.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ +

Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree. │ │ │ │ +More...

│ │ │ │ + │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::HybridValues
 HybridValues represents a collection of DiscreteValues and VectorValues. More...
class  gtsam::HybridBayesTreeClique
 A clique in a HybridBayesTree which is a HybridConditional internally. More...
 
class  gtsam::HybridBayesTree
 A Bayes tree representing a Hybrid density. More...
 
struct  gtsam::traits< HybridBayesTree >
 traits More...
 
struct  gtsam::traits< HybridValues >
class  gtsam::BayesTreeOrphanWrapper< HybridBayesTreeClique >
 Class for Hybrid Bayes tree orphan subtrees. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Jul 28, 2022
│ │ │ │ -
Author
Shangjie Xue
│ │ │ │ +

Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree.

│ │ │ │ +
Date
Mar 11, 2022
│ │ │ │ +
Author
Fan Jiang
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,37 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -HybridValues.h File Reference │ │ │ │ │ +HybridBayesTree.h File Reference │ │ │ │ │ +Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -  _H_y_b_r_i_d_V_a_l_u_e_s represents a collection of _D_i_s_c_r_e_t_e_V_a_l_u_e_s and │ │ │ │ │ - _V_e_c_t_o_r_V_a_l_u_e_s. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e │ │ │ │ │ +  A clique in a _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e which is a _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l internally. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _H_y_b_r_i_d_V_a_l_u_e_s_ _> │ │ │ │ │ + class   _g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e │ │ │ │ │ +  A Bayes tree representing a Hybrid density. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r_<_ _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_ _> │ │ │ │ │ +  Class for Hybrid Bayes tree orphan subtrees. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree. │ │ │ │ │ Date │ │ │ │ │ - Jul 28, 2022 │ │ │ │ │ + Mar 11, 2022 │ │ │ │ │ Author │ │ │ │ │ - Shangjie Xue │ │ │ │ │ + Fan Jiang │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_V_a_l_u_e_s_._h │ │ │ │ │ + * _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00497.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,5 @@ │ │ │ │ │ var a00497 = [ │ │ │ │ │ - ["gtsam::HybridValues", "a03504.html", "a03504"], │ │ │ │ │ - ["gtsam::traits< HybridValues >", "a03508.html", null] │ │ │ │ │ + ["gtsam::HybridBayesTree", "a03424.html", "a03424"], │ │ │ │ │ + ["gtsam::traits< HybridBayesTree >", "a03428.html", null], │ │ │ │ │ + ["gtsam::BayesTreeOrphanWrapper< HybridBayesTreeClique >", "a03432.html", "a03432"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00497_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridValues.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridBayesTree.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,270 +98,164 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
HybridValues.h
│ │ │ │ +
HybridBayesTree.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/discrete/Assignment.h>
│ │ │ │ -
21#include <gtsam/discrete/DiscreteKey.h>
│ │ │ │ -
22#include <gtsam/discrete/DiscreteValues.h>
│ │ │ │ -
23#include <gtsam/inference/Key.h>
│ │ │ │ -
24#include <gtsam/linear/VectorValues.h>
│ │ │ │ -
25#include <gtsam/nonlinear/Values.h>
│ │ │ │ -
26
│ │ │ │ -
27#include <map>
│ │ │ │ -
28#include <string>
│ │ │ │ -
29#include <vector>
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/hybrid/HybridBayesNet.h>
│ │ │ │ +
23#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
│ │ │ │ +
24#include <gtsam/inference/BayesTree.h>
│ │ │ │ +
25#include <gtsam/inference/BayesTreeCliqueBase.h>
│ │ │ │ +
26#include <gtsam/inference/Conditional.h>
│ │ │ │ +
27#include <gtsam/linear/GaussianBayesTree.h>
│ │ │ │ +
28
│ │ │ │ +
29#include <string>
│ │ │ │
30
│ │ │ │
31namespace gtsam {
│ │ │ │
32
│ │ │ │ -
│ │ │ │ -
38class GTSAM_EXPORT HybridValues {
│ │ │ │ -
39 private:
│ │ │ │ -
41 VectorValues continuous_;
│ │ │ │ -
42
│ │ │ │ -
44 DiscreteValues discrete_;
│ │ │ │ -
45
│ │ │ │ -
47 Values nonlinear_;
│ │ │ │ -
48
│ │ │ │ -
49 public:
│ │ │ │ -
52
│ │ │ │ -
54 HybridValues() = default;
│ │ │ │ -
55
│ │ │ │ -
│ │ │ │ -
57 HybridValues(const VectorValues& cv, const DiscreteValues& dv)
│ │ │ │ -
58 : continuous_(cv), discrete_(dv){};
│ │ │ │ -
│ │ │ │ -
59
│ │ │ │ -
│ │ │ │ -
61 HybridValues(const VectorValues& cv, const DiscreteValues& dv,
│ │ │ │ -
62 const Values& v)
│ │ │ │ -
63 : continuous_(cv), discrete_(dv), nonlinear_(v){};
│ │ │ │ -
│ │ │ │ -
64
│ │ │ │ -
68
│ │ │ │ -
│ │ │ │ -
70 void print(const std::string& s = "HybridValues",
│ │ │ │ -
71 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
│ │ │ │ -
72 std::cout << s << ": \n";
│ │ │ │ -
73 continuous_.print(" Continuous",
│ │ │ │ -
74 keyFormatter); // print continuous components
│ │ │ │ -
75 discrete_.print(" Discrete", keyFormatter); // print discrete components
│ │ │ │ -
76 };
│ │ │ │ -
│ │ │ │ -
77
│ │ │ │ -
│ │ │ │ -
79 bool equals(const HybridValues& other, double tol = 1e-9) const {
│ │ │ │ -
80 return continuous_.equals(other.continuous_, tol) &&
│ │ │ │ -
81 discrete_.equals(other.discrete_, tol);
│ │ │ │ -
82 }
│ │ │ │ -
│ │ │ │ -
83
│ │ │ │ -
87
│ │ │ │ -
89 const VectorValues& continuous() const { return continuous_; }
│ │ │ │ -
90
│ │ │ │ -
92 const DiscreteValues& discrete() const { return discrete_; }
│ │ │ │ -
93
│ │ │ │ -
95 const Values& nonlinear() const { return nonlinear_; }
│ │ │ │ -
96
│ │ │ │ -
98 bool existsVector(Key j) { return continuous_.exists(j); };
│ │ │ │ -
99
│ │ │ │ -
101 bool existsDiscrete(Key j) { return (discrete_.find(j) != discrete_.end()); };
│ │ │ │ -
102
│ │ │ │ -
│ │ │ │ -
104 bool existsNonlinear(Key j) {
│ │ │ │ -
105 return nonlinear_.exists(j);
│ │ │ │ -
106 };
│ │ │ │ -
│ │ │ │ -
107
│ │ │ │ -
│ │ │ │ -
109 bool exists(Key j) {
│ │ │ │ -
110 return existsVector(j) || existsDiscrete(j) || existsNonlinear(j);
│ │ │ │ -
111 };
│ │ │ │ -
│ │ │ │ -
112
│ │ │ │ -
117 void insert(Key j, const Vector& value) { continuous_.insert(j, value); }
│ │ │ │ -
118
│ │ │ │ -
123 void insert(Key j, size_t value) { discrete_[j] = value; };
│ │ │ │ +
33// Forward declarations
│ │ │ │ +
34class HybridConditional;
│ │ │ │ +
35class VectorValues;
│ │ │ │ +
36
│ │ │ │ +
37/* ************************************************************************* */
│ │ │ │ +
│ │ │ │ +
44class GTSAM_EXPORT HybridBayesTreeClique
│ │ │ │ +
45 : public BayesTreeCliqueBase<HybridBayesTreeClique,
│ │ │ │ +
46 HybridGaussianFactorGraph> {
│ │ │ │ +
47 public:
│ │ │ │ +
48 typedef HybridBayesTreeClique This;
│ │ │ │ +
49 typedef BayesTreeCliqueBase<HybridBayesTreeClique, HybridGaussianFactorGraph>
│ │ │ │ +
50 Base;
│ │ │ │ +
51 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
52 typedef boost::weak_ptr<This> weak_ptr;
│ │ │ │ +
53 HybridBayesTreeClique() {}
│ │ │ │ +
54 HybridBayesTreeClique(const boost::shared_ptr<HybridConditional>& conditional)
│ │ │ │ +
55 : Base(conditional) {}
│ │ │ │ +
57 HybridBayesTreeClique(const HybridBayesTreeClique& clique) : Base(clique) {}
│ │ │ │ +
58
│ │ │ │ +
59 virtual ~HybridBayesTreeClique() {}
│ │ │ │ +
60};
│ │ │ │ +
│ │ │ │ +
61
│ │ │ │ +
62/* ************************************************************************* */
│ │ │ │ +
│ │ │ │ +
64class GTSAM_EXPORT HybridBayesTree : public BayesTree<HybridBayesTreeClique> {
│ │ │ │ +
65 private:
│ │ │ │ +
66 typedef BayesTree<HybridBayesTreeClique> Base;
│ │ │ │ +
67
│ │ │ │ +
68 public:
│ │ │ │ +
69 typedef HybridBayesTree This;
│ │ │ │ +
70 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
71
│ │ │ │ +
74
│ │ │ │ +
75 HybridBayesTree() = default;
│ │ │ │ +
76
│ │ │ │ +
78 bool equals(const This& other, double tol = 1e-9) const;
│ │ │ │ +
79
│ │ │ │ +
87 GaussianBayesTree choose(const DiscreteValues& assignment) const;
│ │ │ │ +
88
│ │ │ │ +
96 HybridValues optimize() const;
│ │ │ │ +
97
│ │ │ │ +
105 VectorValues optimize(const DiscreteValues& assignment) const;
│ │ │ │ +
106
│ │ │ │ +
112 void prune(const size_t maxNumberLeaves);
│ │ │ │ +
113
│ │ │ │ +
115
│ │ │ │ +
116 private:
│ │ │ │ +
118 friend class boost::serialization::access;
│ │ │ │ +
119 template <class ARCHIVE>
│ │ │ │ +
120 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ +
121 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
122 }
│ │ │ │ +
123};
│ │ │ │ +
│ │ │ │
124
│ │ │ │ -
│ │ │ │ -
126 void insert_or_assign(Key j, const Vector& value) {
│ │ │ │ -
127 continuous_.insert_or_assign(j, value);
│ │ │ │ -
128 }
│ │ │ │ -
│ │ │ │ -
129
│ │ │ │ -
│ │ │ │ -
131 void insert_or_assign(Key j, size_t value) {
│ │ │ │ -
132 discrete_[j] = value;
│ │ │ │ -
133 }
│ │ │ │ -
│ │ │ │ -
134
│ │ │ │ -
│ │ │ │ -
137 HybridValues& insert(const VectorValues& values) {
│ │ │ │ -
138 continuous_.insert(values);
│ │ │ │ -
139 return *this;
│ │ │ │ -
140 }
│ │ │ │ -
│ │ │ │ -
141
│ │ │ │ -
│ │ │ │ -
144 HybridValues& insert(const DiscreteValues& values) {
│ │ │ │ -
145 discrete_.insert(values);
│ │ │ │ -
146 return *this;
│ │ │ │ -
147 }
│ │ │ │ -
│ │ │ │ -
148
│ │ │ │ -
│ │ │ │ -
151 HybridValues& insert(const Values& values) {
│ │ │ │ -
152 nonlinear_.insert(values);
│ │ │ │ -
153 return *this;
│ │ │ │ -
154 }
│ │ │ │ -
│ │ │ │ -
155
│ │ │ │ -
│ │ │ │ -
158 HybridValues& insert(const HybridValues& values) {
│ │ │ │ -
159 continuous_.insert(values.continuous());
│ │ │ │ -
160 discrete_.insert(values.discrete());
│ │ │ │ -
161 nonlinear_.insert(values.nonlinear());
│ │ │ │ -
162 return *this;
│ │ │ │ -
163 }
│ │ │ │ -
│ │ │ │ -
164
│ │ │ │ -
169 Vector& at(Key j) { return continuous_.at(j); };
│ │ │ │ -
170
│ │ │ │ -
175 size_t& atDiscrete(Key j) { return discrete_.at(j); };
│ │ │ │ -
176
│ │ │ │ -
│ │ │ │ -
181 HybridValues& update(const VectorValues& values) {
│ │ │ │ -
182 continuous_.update(values);
│ │ │ │ -
183 return *this;
│ │ │ │ -
184 }
│ │ │ │ -
│ │ │ │ -
185
│ │ │ │ -
│ │ │ │ -
190 HybridValues& update(const DiscreteValues& values) {
│ │ │ │ -
191 discrete_.update(values);
│ │ │ │ -
192 return *this;
│ │ │ │ -
193 }
│ │ │ │ -
│ │ │ │ -
194
│ │ │ │ -
│ │ │ │ -
199 HybridValues& update(const HybridValues& values) {
│ │ │ │ -
200 continuous_.update(values.continuous());
│ │ │ │ -
201 discrete_.update(values.discrete());
│ │ │ │ -
202 return *this;
│ │ │ │ -
203 }
│ │ │ │ -
│ │ │ │ -
204
│ │ │ │ -
│ │ │ │ -
206 VectorValues continuousSubset(const KeyVector& keys) const {
│ │ │ │ -
207 VectorValues measurements;
│ │ │ │ -
208 for (const auto& key : keys) {
│ │ │ │ -
209 measurements.insert(key, continuous_.at(key));
│ │ │ │ -
210 }
│ │ │ │ -
211 return measurements;
│ │ │ │ -
212 }
│ │ │ │ -
│ │ │ │ -
213
│ │ │ │ -
217
│ │ │ │ -
│ │ │ │ -
224 std::string html(
│ │ │ │ -
225 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
│ │ │ │ -
226 std::stringstream ss;
│ │ │ │ -
227 ss << this->continuous_.html(keyFormatter);
│ │ │ │ -
228 ss << this->discrete_.html(keyFormatter);
│ │ │ │ -
229 return ss.str();
│ │ │ │ -
230 };
│ │ │ │ -
│ │ │ │ -
231
│ │ │ │ -
233};
│ │ │ │ -
│ │ │ │ -
234
│ │ │ │ -
235// traits
│ │ │ │ -
236template <>
│ │ │ │ -
237struct traits<HybridValues> : public Testable<HybridValues> {};
│ │ │ │ -
238
│ │ │ │ -
239} // namespace gtsam
│ │ │ │ - │ │ │ │ -
DiscreteKey.h
specialized key for discrete variables
│ │ │ │ -
Assignment.h
An assignment from labels to a discrete value index (size_t)
│ │ │ │ - │ │ │ │ -
VectorValues.h
Factor Graph Values.
│ │ │ │ -
Values.h
A non-templated config holding any types of Manifold-group elements.
│ │ │ │ +
126template <>
│ │ │ │ +
127struct traits<HybridBayesTree> : public Testable<HybridBayesTree> {};
│ │ │ │ +
128
│ │ │ │ +
139template <>
│ │ │ │ +
│ │ │ │ +
140class BayesTreeOrphanWrapper<HybridBayesTreeClique> : public HybridConditional {
│ │ │ │ +
141 public:
│ │ │ │ +
142 typedef HybridBayesTreeClique CliqueType;
│ │ │ │ +
143 typedef HybridConditional Base;
│ │ │ │ +
144
│ │ │ │ +
145 boost::shared_ptr<CliqueType> clique;
│ │ │ │ +
146
│ │ │ │ +
│ │ │ │ +
152 BayesTreeOrphanWrapper(const boost::shared_ptr<CliqueType>& clique)
│ │ │ │ +
153 : clique(clique) {
│ │ │ │ +
154 // Store parent keys in our base type factor so that eliminating those
│ │ │ │ +
155 // parent keys will pull this subtree into the elimination.
│ │ │ │ +
156 this->keys_.assign(clique->conditional()->beginParents(),
│ │ │ │ +
157 clique->conditional()->endParents());
│ │ │ │ +
158 this->discreteKeys_.assign(clique->conditional()->discreteKeys().begin(),
│ │ │ │ +
159 clique->conditional()->discreteKeys().end());
│ │ │ │ +
160 }
│ │ │ │ +
│ │ │ │ +
161
│ │ │ │ +
│ │ │ │ +
163 void print(
│ │ │ │ +
164 const std::string& s = "",
│ │ │ │ +
165 const KeyFormatter& formatter = DefaultKeyFormatter) const override {
│ │ │ │ +
166 clique->print(s + "stored clique", formatter);
│ │ │ │ +
167 }
│ │ │ │ +
│ │ │ │ +
168};
│ │ │ │ +
│ │ │ │ +
169
│ │ │ │ +
170} // namespace gtsam
│ │ │ │ +
HybridGaussianFactorGraph.h
Linearized Hybrid factor graph that uses type erasure.
│ │ │ │ +
HybridBayesNet.h
A Bayes net of Gaussian Conditionals indexed by discrete keys.
│ │ │ │ +
BayesTreeCliqueBase.h
Base class for cliques of a BayesTree.
│ │ │ │ +
Conditional.h
Base class for conditional densities.
│ │ │ │ +
BayesTree.h
Bayes Tree is a tree of cliques of a Bayes Chain.
│ │ │ │ +
GaussianBayesTree.h
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │
gtsam::DiscreteValues
A map from keys to values.
Definition DiscreteValues.h:34
│ │ │ │ -
gtsam::DiscreteValues::html
std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter, const Names &names={}) const
Output as a html table.
Definition DiscreteValues.cpp:104
│ │ │ │ -
gtsam::DiscreteValues::update
DiscreteValues & update(const DiscreteValues &values)
For all key/value pairs in values, replace values with corresponding keys in this object with those i...
Definition DiscreteValues.cpp:63
│ │ │ │ -
gtsam::DiscreteValues::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print required by Testable.
Definition DiscreteValues.cpp:31
│ │ │ │ -
gtsam::DiscreteValues::equals
bool equals(const DiscreteValues &x, double tol=1e-9) const
equals required by Testable for unit testing.
Definition DiscreteValues.cpp:40
│ │ │ │ +
gtsam::HybridBayesTreeClique
A clique in a HybridBayesTree which is a HybridConditional internally.
Definition HybridBayesTree.h:46
│ │ │ │ +
gtsam::HybridBayesTreeClique::HybridBayesTreeClique
HybridBayesTreeClique(const boost::shared_ptr< HybridConditional > &conditional)
Copy constructor.
Definition HybridBayesTree.h:54
│ │ │ │ +
gtsam::HybridBayesTree
A Bayes tree representing a Hybrid density.
Definition HybridBayesTree.h:64
│ │ │ │ +
gtsam::HybridBayesTree::HybridBayesTree
HybridBayesTree()=default
Default constructor, creates an empty Bayes tree.
│ │ │ │ +
gtsam::BayesTreeOrphanWrapper< HybridBayesTreeClique >::BayesTreeOrphanWrapper
BayesTreeOrphanWrapper(const boost::shared_ptr< CliqueType > &clique)
Construct a new Bayes Tree Orphan Wrapper object.
Definition HybridBayesTree.h:152
│ │ │ │ +
gtsam::BayesTreeOrphanWrapper< HybridBayesTreeClique >::print
void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print utility
Definition HybridBayesTree.h:163
│ │ │ │ +
gtsam::HybridConditional
Hybrid Conditional Density.
Definition HybridConditional.h:62
│ │ │ │
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ -
gtsam::HybridValues::print
void print(const std::string &s="HybridValues", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print required by Testable for unit testing
Definition HybridValues.h:70
│ │ │ │ -
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Insert a vector value with key j.
Definition HybridValues.h:117
│ │ │ │ -
gtsam::HybridValues::insert
HybridValues & insert(const DiscreteValues &values)
Insert all discrete values from values.
Definition HybridValues.h:144
│ │ │ │ -
gtsam::HybridValues::HybridValues
HybridValues(const VectorValues &cv, const DiscreteValues &dv, const Values &v)
Construct from all values types.
Definition HybridValues.h:61
│ │ │ │ -
gtsam::HybridValues::html
std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Output as a html table.
Definition HybridValues.h:224
│ │ │ │ -
gtsam::HybridValues::continuousSubset
VectorValues continuousSubset(const KeyVector &keys) const
Extract continuous values with given keys.
Definition HybridValues.h:206
│ │ │ │ -
gtsam::HybridValues::insert_or_assign
void insert_or_assign(Key j, const Vector &value)
insert_or_assign() , similar to Values.h
Definition HybridValues.h:126
│ │ │ │ -
gtsam::HybridValues::insert
HybridValues & insert(const Values &values)
Insert all values from values.
Definition HybridValues.h:151
│ │ │ │ -
gtsam::HybridValues::equals
bool equals(const HybridValues &other, double tol=1e-9) const
equals required by Testable for unit testing
Definition HybridValues.h:79
│ │ │ │ -
gtsam::HybridValues::update
HybridValues & update(const VectorValues &values)
For all key/value pairs in values, replace continuous values with corresponding keys in this object w...
Definition HybridValues.h:181
│ │ │ │ -
gtsam::HybridValues::discrete
const DiscreteValues & discrete() const
Return the discrete values.
Definition HybridValues.h:92
│ │ │ │ -
gtsam::HybridValues::existsDiscrete
bool existsDiscrete(Key j)
Check whether a variable with key j exists in DiscreteValues.
Definition HybridValues.h:101
│ │ │ │ -
gtsam::HybridValues::continuous
const VectorValues & continuous() const
Return the multi-dimensional vector values.
Definition HybridValues.h:89
│ │ │ │ -
gtsam::HybridValues::at
Vector & at(Key j)
Read/write access to the vector value with key j, throws std::out_of_range if j does not exist.
Definition HybridValues.h:169
│ │ │ │ -
gtsam::HybridValues::exists
bool exists(Key j)
Check whether a variable with key j exists.
Definition HybridValues.h:109
│ │ │ │ -
gtsam::HybridValues::update
HybridValues & update(const DiscreteValues &values)
For all key/value pairs in values, replace discrete values with corresponding keys in this object wit...
Definition HybridValues.h:190
│ │ │ │ -
gtsam::HybridValues::insert
HybridValues & insert(const HybridValues &values)
Insert all values from values.
Definition HybridValues.h:158
│ │ │ │ -
gtsam::HybridValues::insert
HybridValues & insert(const VectorValues &values)
Insert all continuous values from values.
Definition HybridValues.h:137
│ │ │ │ -
gtsam::HybridValues::insert
void insert(Key j, size_t value)
Insert a discrete value with key j.
Definition HybridValues.h:123
│ │ │ │ -
gtsam::HybridValues::nonlinear
const Values & nonlinear() const
Return the nonlinear values.
Definition HybridValues.h:95
│ │ │ │ -
gtsam::HybridValues::existsNonlinear
bool existsNonlinear(Key j)
Check whether a variable with key j exists in values.
Definition HybridValues.h:104
│ │ │ │ -
gtsam::HybridValues::update
HybridValues & update(const HybridValues &values)
For all key/value pairs in values, replace all values with corresponding keys in this object with tho...
Definition HybridValues.h:199
│ │ │ │ -
gtsam::HybridValues::HybridValues
HybridValues()=default
Default constructor creates an empty HybridValues.
│ │ │ │ -
gtsam::HybridValues::existsVector
bool existsVector(Key j)
Check whether a variable with key j exists in VectorValues.
Definition HybridValues.h:98
│ │ │ │ -
gtsam::HybridValues::HybridValues
HybridValues(const VectorValues &cv, const DiscreteValues &dv)
Construct from DiscreteValues and VectorValues.
Definition HybridValues.h:57
│ │ │ │ -
gtsam::HybridValues::atDiscrete
size_t & atDiscrete(Key j)
Read/write access to the discrete value with key j, throws std::out_of_range if j does not exist.
Definition HybridValues.h:175
│ │ │ │ -
gtsam::HybridValues::insert_or_assign
void insert_or_assign(Key j, size_t value)
insert_or_assign() , similar to Values.h
Definition HybridValues.h:131
│ │ │ │ +
gtsam::BayesTree
Bayes tree.
Definition BayesTree.h:67
│ │ │ │ +
gtsam::BayesTreeOrphanWrapper
Definition BayesTree.h:276
│ │ │ │ +
gtsam::BayesTreeCliqueBase
This is the base class for BayesTree cliques.
Definition BayesTreeCliqueBase.h:50
│ │ │ │ +
gtsam::GaussianBayesTree
A Bayes tree representing a Gaussian density.
Definition GaussianBayesTree.h:52
│ │ │ │
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ -
gtsam::VectorValues::insert
iterator insert(const std::pair< Key, Vector > &key_value)
Insert a vector value with key j.
Definition VectorValues.cpp:91
│ │ │ │ -
gtsam::VectorValues::print
void print(const std::string &str="VectorValues", const KeyFormatter &formatter=DefaultKeyFormatter) const
print required by Testable for unit testing
Definition VectorValues.cpp:158
│ │ │ │ -
gtsam::VectorValues::equals
bool equals(const VectorValues &x, double tol=1e-9) const
equals required by Testable for unit testing
Definition VectorValues.cpp:166
│ │ │ │ -
gtsam::VectorValues::exists
bool exists(Key j) const
Check whether a variable with key j exists.
Definition VectorValues.h:133
│ │ │ │ -
gtsam::VectorValues::html
std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Output as a html table.
Definition VectorValues.cpp:387
│ │ │ │ -
gtsam::VectorValues::insert_or_assign
void insert_or_assign(Key j, const Vector &value)
insert_or_assign that mimics the STL map insert_or_assign - if the value already exists,...
Definition VectorValues.h:219
│ │ │ │ -
gtsam::VectorValues::at
Vector & at(Key j)
Read/write access to the vector value with key j, throws std::out_of_range if j does not exist,...
Definition VectorValues.h:139
│ │ │ │ -
gtsam::VectorValues::update
VectorValues & update(const VectorValues &values)
For all key/value pairs in values, replace values with corresponding keys in this class with those in...
Definition VectorValues.cpp:101
│ │ │ │ -
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
gtsam::Values::insert
void insert(Key j, const Value &val)
Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present.
Definition Values.cpp:157
│ │ │ │ -
gtsam::Values::exists
bool exists(Key j) const
Check if a value exists with key j.
Definition Values.cpp:94
│ │ │ │ +
VectorValues
The Factor::error simply extracts the.
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,392 +1,208 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -HybridValues.h │ │ │ │ │ +HybridBayesTree.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_A_s_s_i_g_n_m_e_n_t_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_K_e_y_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_V_a_l_u_e_s_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_V_a_l_u_e_s_._h> │ │ │ │ │ -26 │ │ │ │ │ -27#include │ │ │ │ │ -28#include │ │ │ │ │ -29#include │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ +27#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ +28 │ │ │ │ │ +29#include │ │ │ │ │ 30 │ │ │ │ │ 31namespace _g_t_s_a_m { │ │ │ │ │ 32 │ │ │ │ │ -_3_8class GTSAM_EXPORT _H_y_b_r_i_d_V_a_l_u_e_s { │ │ │ │ │ -39 private: │ │ │ │ │ -41 _V_e_c_t_o_r_V_a_l_u_e_s continuous_; │ │ │ │ │ -42 │ │ │ │ │ -44 _D_i_s_c_r_e_t_e_V_a_l_u_e_s discrete_; │ │ │ │ │ -45 │ │ │ │ │ -47 _V_a_l_u_e_s nonlinear_; │ │ │ │ │ -48 │ │ │ │ │ -49 public: │ │ │ │ │ -52 │ │ │ │ │ -_5_4 _H_y_b_r_i_d_V_a_l_u_e_s() = default; │ │ │ │ │ -55 │ │ │ │ │ -_5_7 _H_y_b_r_i_d_V_a_l_u_e_s(const _V_e_c_t_o_r_V_a_l_u_e_s& cv, const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& dv) │ │ │ │ │ -58 : continuous_(cv), discrete_(dv){}; │ │ │ │ │ -59 │ │ │ │ │ -_6_1 _H_y_b_r_i_d_V_a_l_u_e_s(const _V_e_c_t_o_r_V_a_l_u_e_s& cv, const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& dv, │ │ │ │ │ -62 const _V_a_l_u_e_s& v) │ │ │ │ │ -63 : continuous_(cv), discrete_(dv), nonlinear_(v){}; │ │ │ │ │ -64 │ │ │ │ │ -68 │ │ │ │ │ -_7_0 void _p_r_i_n_t(const std::string& s = "HybridValues", │ │ │ │ │ -71 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const { │ │ │ │ │ -72 std::cout << s << ": \n"; │ │ │ │ │ -73 continuous_._p_r_i_n_t(" Continuous", │ │ │ │ │ -74 keyFormatter); // print continuous components │ │ │ │ │ -75 discrete_._p_r_i_n_t(" Discrete", keyFormatter); // print discrete components │ │ │ │ │ -76 }; │ │ │ │ │ -77 │ │ │ │ │ -_7_9 bool _e_q_u_a_l_s(const _H_y_b_r_i_d_V_a_l_u_e_s& other, double tol = 1e-9) const { │ │ │ │ │ -80 return continuous_._e_q_u_a_l_s(other.continuous_, tol) && │ │ │ │ │ -81 discrete_._e_q_u_a_l_s(other.discrete_, tol); │ │ │ │ │ -82 } │ │ │ │ │ -83 │ │ │ │ │ -87 │ │ │ │ │ -_8_9 const _V_e_c_t_o_r_V_a_l_u_e_s& _c_o_n_t_i_n_u_o_u_s() const { return continuous_; } │ │ │ │ │ -90 │ │ │ │ │ -_9_2 const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& _d_i_s_c_r_e_t_e() const { return discrete_; } │ │ │ │ │ -93 │ │ │ │ │ -_9_5 const _V_a_l_u_e_s& _n_o_n_l_i_n_e_a_r() const { return nonlinear_; } │ │ │ │ │ -96 │ │ │ │ │ -_9_8 bool _e_x_i_s_t_s_V_e_c_t_o_r(_K_e_y j) { return continuous_._e_x_i_s_t_s(j); }; │ │ │ │ │ -99 │ │ │ │ │ -_1_0_1 bool _e_x_i_s_t_s_D_i_s_c_r_e_t_e(_K_e_y j) { return (discrete_.find(j) != discrete_.end()); │ │ │ │ │ -}; │ │ │ │ │ -102 │ │ │ │ │ -_1_0_4 bool _e_x_i_s_t_s_N_o_n_l_i_n_e_a_r(_K_e_y j) { │ │ │ │ │ -105 return nonlinear_._e_x_i_s_t_s(j); │ │ │ │ │ -106 }; │ │ │ │ │ -107 │ │ │ │ │ -_1_0_9 bool _e_x_i_s_t_s(_K_e_y j) { │ │ │ │ │ -110 return existsVector(j) || existsDiscrete(j) || existsNonlinear(j); │ │ │ │ │ -111 }; │ │ │ │ │ -112 │ │ │ │ │ -_1_1_7 void _i_n_s_e_r_t(_K_e_y j, const Vector& value) { continuous_._i_n_s_e_r_t(j, value); } │ │ │ │ │ -118 │ │ │ │ │ -_1_2_3 void _i_n_s_e_r_t(_K_e_y j, size_t value) { discrete_[j] = value; }; │ │ │ │ │ +33// Forward declarations │ │ │ │ │ +34class HybridConditional; │ │ │ │ │ +35class _V_e_c_t_o_r_V_a_l_u_e_s; │ │ │ │ │ +36 │ │ │ │ │ +37/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +_4_4class GTSAM_EXPORT _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e │ │ │ │ │ +45 : public _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e { │ │ │ │ │ +47 public: │ │ │ │ │ +48 typedef _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e _T_h_i_s; │ │ │ │ │ +49 typedef _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_, │ │ │ │ │ +_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_> │ │ │ │ │ +50 _B_a_s_e; │ │ │ │ │ +51 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +52 typedef boost::weak_ptr weak_ptr; │ │ │ │ │ +53 _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e() {} │ │ │ │ │ +_5_4 _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e(const boost::shared_ptr& │ │ │ │ │ +conditional) │ │ │ │ │ +55 : _B_a_s_e(conditional) {} │ │ │ │ │ +57 _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e(const _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e& clique) : Base(clique) {} │ │ │ │ │ +58 │ │ │ │ │ +59 virtual ~HybridBayesTreeClique() {} │ │ │ │ │ +60}; │ │ │ │ │ +61 │ │ │ │ │ +62/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +_6_4class GTSAM_EXPORT _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e : public _B_a_y_e_s_T_r_e_e │ │ │ │ │ +{ │ │ │ │ │ +65 private: │ │ │ │ │ +66 typedef _B_a_y_e_s_T_r_e_e_<_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_> _B_a_s_e; │ │ │ │ │ +67 │ │ │ │ │ +68 public: │ │ │ │ │ +69 typedef _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e _T_h_i_s; │ │ │ │ │ +70 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +71 │ │ │ │ │ +74 │ │ │ │ │ +_7_5 _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e() = default; │ │ │ │ │ +76 │ │ │ │ │ +78 bool _e_q_u_a_l_s(const _T_h_i_s& other, double tol = 1e-9) const; │ │ │ │ │ +79 │ │ │ │ │ +87 _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e choose(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& assignment) const; │ │ │ │ │ +88 │ │ │ │ │ +96 _H_y_b_r_i_d_V_a_l_u_e_s _o_p_t_i_m_i_z_e() const; │ │ │ │ │ +97 │ │ │ │ │ +105 _V_e_c_t_o_r_V_a_l_u_e_s _o_p_t_i_m_i_z_e(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& assignment) const; │ │ │ │ │ +106 │ │ │ │ │ +112 void prune(const size_t maxNumberLeaves); │ │ │ │ │ +113 │ │ │ │ │ +115 │ │ │ │ │ +116 private: │ │ │ │ │ +_1_1_8 friend class boost::serialization::access; │ │ │ │ │ +119 template │ │ │ │ │ +120 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ +121 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +122 } │ │ │ │ │ +123}; │ │ │ │ │ 124 │ │ │ │ │ -_1_2_6 void _i_n_s_e_r_t___o_r___a_s_s_i_g_n(_K_e_y j, const Vector& value) { │ │ │ │ │ -127 continuous_._i_n_s_e_r_t___o_r___a_s_s_i_g_n(j, value); │ │ │ │ │ -128 } │ │ │ │ │ -129 │ │ │ │ │ -_1_3_1 void _i_n_s_e_r_t___o_r___a_s_s_i_g_n(_K_e_y j, size_t value) { │ │ │ │ │ -132 discrete_[j] = value; │ │ │ │ │ -133 } │ │ │ │ │ -134 │ │ │ │ │ -_1_3_7 _H_y_b_r_i_d_V_a_l_u_e_s& _i_n_s_e_r_t(const _V_e_c_t_o_r_V_a_l_u_e_s& values) { │ │ │ │ │ -138 continuous_._i_n_s_e_r_t(values); │ │ │ │ │ -139 return *this; │ │ │ │ │ -140 } │ │ │ │ │ -141 │ │ │ │ │ -_1_4_4 _H_y_b_r_i_d_V_a_l_u_e_s& _i_n_s_e_r_t(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values) { │ │ │ │ │ -145 discrete_.insert(values); │ │ │ │ │ -146 return *this; │ │ │ │ │ -147 } │ │ │ │ │ -148 │ │ │ │ │ -_1_5_1 _H_y_b_r_i_d_V_a_l_u_e_s& _i_n_s_e_r_t(const _V_a_l_u_e_s& values) { │ │ │ │ │ -152 nonlinear_._i_n_s_e_r_t(values); │ │ │ │ │ -153 return *this; │ │ │ │ │ -154 } │ │ │ │ │ -155 │ │ │ │ │ -_1_5_8 _H_y_b_r_i_d_V_a_l_u_e_s& _i_n_s_e_r_t(const _H_y_b_r_i_d_V_a_l_u_e_s& values) { │ │ │ │ │ -159 continuous_._i_n_s_e_r_t(values._c_o_n_t_i_n_u_o_u_s()); │ │ │ │ │ -160 discrete_.insert(values._d_i_s_c_r_e_t_e()); │ │ │ │ │ -161 nonlinear_._i_n_s_e_r_t(values._n_o_n_l_i_n_e_a_r()); │ │ │ │ │ -162 return *this; │ │ │ │ │ -163 } │ │ │ │ │ -164 │ │ │ │ │ -_1_6_9 Vector& _a_t(_K_e_y j) { return continuous_._a_t(j); }; │ │ │ │ │ -170 │ │ │ │ │ -_1_7_5 size_t& _a_t_D_i_s_c_r_e_t_e(_K_e_y j) { return discrete_.at(j); }; │ │ │ │ │ -176 │ │ │ │ │ -_1_8_1 _H_y_b_r_i_d_V_a_l_u_e_s& _u_p_d_a_t_e(const _V_e_c_t_o_r_V_a_l_u_e_s& values) { │ │ │ │ │ -182 continuous_._u_p_d_a_t_e(values); │ │ │ │ │ -183 return *this; │ │ │ │ │ -184 } │ │ │ │ │ -185 │ │ │ │ │ -_1_9_0 _H_y_b_r_i_d_V_a_l_u_e_s& _u_p_d_a_t_e(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values) { │ │ │ │ │ -191 discrete_._u_p_d_a_t_e(values); │ │ │ │ │ -192 return *this; │ │ │ │ │ -193 } │ │ │ │ │ -194 │ │ │ │ │ -_1_9_9 _H_y_b_r_i_d_V_a_l_u_e_s& _u_p_d_a_t_e(const _H_y_b_r_i_d_V_a_l_u_e_s& values) { │ │ │ │ │ -200 continuous_._u_p_d_a_t_e(values._c_o_n_t_i_n_u_o_u_s()); │ │ │ │ │ -201 discrete_._u_p_d_a_t_e(values._d_i_s_c_r_e_t_e()); │ │ │ │ │ -202 return *this; │ │ │ │ │ -203 } │ │ │ │ │ -204 │ │ │ │ │ -_2_0_6 _V_e_c_t_o_r_V_a_l_u_e_s _c_o_n_t_i_n_u_o_u_s_S_u_b_s_e_t(const _K_e_y_V_e_c_t_o_r& keys) const { │ │ │ │ │ -207 _V_e_c_t_o_r_V_a_l_u_e_s measurements; │ │ │ │ │ -208 for (const auto& key : keys) { │ │ │ │ │ -209 measurements._i_n_s_e_r_t(key, continuous_._a_t(key)); │ │ │ │ │ -210 } │ │ │ │ │ -211 return measurements; │ │ │ │ │ -212 } │ │ │ │ │ -213 │ │ │ │ │ -217 │ │ │ │ │ -_2_2_4 std::string _h_t_m_l( │ │ │ │ │ -225 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const { │ │ │ │ │ -226 std::stringstream ss; │ │ │ │ │ -227 ss << this->continuous_._h_t_m_l(keyFormatter); │ │ │ │ │ -228 ss << this->discrete_._h_t_m_l(keyFormatter); │ │ │ │ │ -229 return ss.str(); │ │ │ │ │ -230 }; │ │ │ │ │ -231 │ │ │ │ │ -233}; │ │ │ │ │ -234 │ │ │ │ │ -235// traits │ │ │ │ │ -236template <> │ │ │ │ │ -_2_3_7struct _t_r_a_i_t_s<_H_y_b_r_i_d_V_a_l_u_e_s> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ -238 │ │ │ │ │ -239} // namespace gtsam │ │ │ │ │ -_D_i_s_c_r_e_t_e_V_a_l_u_e_s_._h │ │ │ │ │ -_D_i_s_c_r_e_t_e_K_e_y_._h │ │ │ │ │ -specialized key for discrete variables │ │ │ │ │ -_A_s_s_i_g_n_m_e_n_t_._h │ │ │ │ │ -An assignment from labels to a discrete value index (size_t) │ │ │ │ │ -_K_e_y_._h │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ -Factor Graph Values. │ │ │ │ │ -_V_a_l_u_e_s_._h │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +126template <> │ │ │ │ │ +_1_2_7struct _t_r_a_i_t_s<_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ +128 │ │ │ │ │ +139template <> │ │ │ │ │ +_1_4_0class _B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r<_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e> : public │ │ │ │ │ +_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l { │ │ │ │ │ +141 public: │ │ │ │ │ +142 typedef _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e _C_l_i_q_u_e_T_y_p_e; │ │ │ │ │ +143 typedef _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l _B_a_s_e; │ │ │ │ │ +144 │ │ │ │ │ +145 boost::shared_ptr clique; │ │ │ │ │ +146 │ │ │ │ │ +_1_5_2 _B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r(const boost::shared_ptr& clique) │ │ │ │ │ +153 : clique(clique) { │ │ │ │ │ +154 // Store parent keys in our base type factor so that eliminating those │ │ │ │ │ +155 // parent keys will pull this subtree into the elimination. │ │ │ │ │ +156 this->keys_.assign(clique->conditional()->beginParents(), │ │ │ │ │ +157 clique->conditional()->endParents()); │ │ │ │ │ +158 this->discreteKeys_.assign(clique->conditional()->discreteKeys().begin(), │ │ │ │ │ +159 clique->conditional()->discreteKeys().end()); │ │ │ │ │ +160 } │ │ │ │ │ +161 │ │ │ │ │ +_1_6_3 void _p_r_i_n_t( │ │ │ │ │ +164 const std::string& s = "", │ │ │ │ │ +165 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override { │ │ │ │ │ +166 clique->print(s + "stored clique", formatter); │ │ │ │ │ +167 } │ │ │ │ │ +168}; │ │ │ │ │ +169 │ │ │ │ │ +170} // namespace gtsam │ │ │ │ │ +_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Linearized Hybrid factor graph that uses type erasure. │ │ │ │ │ +_H_y_b_r_i_d_B_a_y_e_s_N_e_t_._h │ │ │ │ │ +A Bayes net of Gaussian Conditionals indexed by discrete keys. │ │ │ │ │ +_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._h │ │ │ │ │ +Base class for cliques of a BayesTree. │ │ │ │ │ +_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ +Base class for conditional densities. │ │ │ │ │ +_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ +Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ │ +_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ +Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ +Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key │ │ │ │ │ +landmarkKey) │ │ │ │ │ +Optimize for triangulation. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.cpp:155 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ _g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ A helper that implements the traits interface for GTSAM types. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ A map from keys to values. │ │ │ │ │ DDeeffiinniittiioonn DiscreteValues.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_h_t_m_l │ │ │ │ │ -std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter, const │ │ │ │ │ -Names &names={}) const │ │ │ │ │ -Output as a html table. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteValues.cpp:104 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_u_p_d_a_t_e │ │ │ │ │ -DiscreteValues & update(const DiscreteValues &values) │ │ │ │ │ -For all key/value pairs in values, replace values with corresponding keys in │ │ │ │ │ -this object with those i... │ │ │ │ │ -DDeeffiinniittiioonn DiscreteValues.cpp:63 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_p_r_i_n_t │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e │ │ │ │ │ +A clique in a HybridBayesTree which is a HybridConditional internally. │ │ │ │ │ +DDeeffiinniittiioonn HybridBayesTree.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_:_:_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e │ │ │ │ │ +HybridBayesTreeClique(const boost::shared_ptr< HybridConditional > │ │ │ │ │ +&conditional) │ │ │ │ │ +Copy constructor. │ │ │ │ │ +DDeeffiinniittiioonn HybridBayesTree.h:54 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e │ │ │ │ │ +A Bayes tree representing a Hybrid density. │ │ │ │ │ +DDeeffiinniittiioonn HybridBayesTree.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_:_:_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e │ │ │ │ │ +HybridBayesTree()=default │ │ │ │ │ +Default constructor, creates an empty Bayes tree. │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r_<_ _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_ _>_:_:_B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r │ │ │ │ │ +BayesTreeOrphanWrapper(const boost::shared_ptr< CliqueType > &clique) │ │ │ │ │ +Construct a new Bayes Tree Orphan Wrapper object. │ │ │ │ │ +DDeeffiinniittiioonn HybridBayesTree.h:152 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r_<_ _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_ _>_:_:_p_r_i_n_t │ │ │ │ │ void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ -print required by Testable. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteValues.cpp:31 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const DiscreteValues &x, double tol=1e-9) const │ │ │ │ │ -equals required by Testable for unit testing. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteValues.cpp:40 │ │ │ │ │ +&formatter=DefaultKeyFormatter) const override │ │ │ │ │ +print utility │ │ │ │ │ +DDeeffiinniittiioonn HybridBayesTree.h:163 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +Hybrid Conditional Density. │ │ │ │ │ +DDeeffiinniittiioonn HybridConditional.h:62 │ │ │ │ │ _g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="HybridValues", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ -print required by Testable for unit testing │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:70 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_i_n_s_e_r_t │ │ │ │ │ -void insert(Key j, const Vector &value) │ │ │ │ │ -Insert a vector value with key j. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:117 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_i_n_s_e_r_t │ │ │ │ │ -HybridValues & insert(const DiscreteValues &values) │ │ │ │ │ -Insert all discrete values from values. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:144 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -HybridValues(const VectorValues &cv, const DiscreteValues &dv, const Values &v) │ │ │ │ │ -Construct from all values types. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:61 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_h_t_m_l │ │ │ │ │ -std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ -Output as a html table. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:224 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_c_o_n_t_i_n_u_o_u_s_S_u_b_s_e_t │ │ │ │ │ -VectorValues continuousSubset(const KeyVector &keys) const │ │ │ │ │ -Extract continuous values with given keys. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:206 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_i_n_s_e_r_t___o_r___a_s_s_i_g_n │ │ │ │ │ -void insert_or_assign(Key j, const Vector &value) │ │ │ │ │ -insert_or_assign() , similar to Values.h │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:126 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_i_n_s_e_r_t │ │ │ │ │ -HybridValues & insert(const Values &values) │ │ │ │ │ -Insert all values from values. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const HybridValues &other, double tol=1e-9) const │ │ │ │ │ -equals required by Testable for unit testing │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:79 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_u_p_d_a_t_e │ │ │ │ │ -HybridValues & update(const VectorValues &values) │ │ │ │ │ -For all key/value pairs in values, replace continuous values with corresponding │ │ │ │ │ -keys in this object w... │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:181 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_d_i_s_c_r_e_t_e │ │ │ │ │ -const DiscreteValues & discrete() const │ │ │ │ │ -Return the discrete values. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:92 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_e_x_i_s_t_s_D_i_s_c_r_e_t_e │ │ │ │ │ -bool existsDiscrete(Key j) │ │ │ │ │ -Check whether a variable with key j exists in DiscreteValues. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:101 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_c_o_n_t_i_n_u_o_u_s │ │ │ │ │ -const VectorValues & continuous() const │ │ │ │ │ -Return the multi-dimensional vector values. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:89 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_a_t │ │ │ │ │ -Vector & at(Key j) │ │ │ │ │ -Read/write access to the vector value with key j, throws std::out_of_range if j │ │ │ │ │ -does not exist. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:169 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_e_x_i_s_t_s │ │ │ │ │ -bool exists(Key j) │ │ │ │ │ -Check whether a variable with key j exists. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:109 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_u_p_d_a_t_e │ │ │ │ │ -HybridValues & update(const DiscreteValues &values) │ │ │ │ │ -For all key/value pairs in values, replace discrete values with corresponding │ │ │ │ │ -keys in this object wit... │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:190 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_i_n_s_e_r_t │ │ │ │ │ -HybridValues & insert(const HybridValues &values) │ │ │ │ │ -Insert all values from values. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:158 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_i_n_s_e_r_t │ │ │ │ │ -HybridValues & insert(const VectorValues &values) │ │ │ │ │ -Insert all continuous values from values. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:137 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_i_n_s_e_r_t │ │ │ │ │ -void insert(Key j, size_t value) │ │ │ │ │ -Insert a discrete value with key j. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:123 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_n_o_n_l_i_n_e_a_r │ │ │ │ │ -const Values & nonlinear() const │ │ │ │ │ -Return the nonlinear values. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:95 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_e_x_i_s_t_s_N_o_n_l_i_n_e_a_r │ │ │ │ │ -bool existsNonlinear(Key j) │ │ │ │ │ -Check whether a variable with key j exists in values. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:104 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_u_p_d_a_t_e │ │ │ │ │ -HybridValues & update(const HybridValues &values) │ │ │ │ │ -For all key/value pairs in values, replace all values with corresponding keys │ │ │ │ │ -in this object with tho... │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:199 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -HybridValues()=default │ │ │ │ │ -Default constructor creates an empty HybridValues. │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_e_x_i_s_t_s_V_e_c_t_o_r │ │ │ │ │ -bool existsVector(Key j) │ │ │ │ │ -Check whether a variable with key j exists in VectorValues. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:98 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -HybridValues(const VectorValues &cv, const DiscreteValues &dv) │ │ │ │ │ -Construct from DiscreteValues and VectorValues. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_a_t_D_i_s_c_r_e_t_e │ │ │ │ │ -size_t & atDiscrete(Key j) │ │ │ │ │ -Read/write access to the discrete value with key j, throws std::out_of_range if │ │ │ │ │ -j does not exist. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:175 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_i_n_s_e_r_t___o_r___a_s_s_i_g_n │ │ │ │ │ -void insert_or_assign(Key j, size_t value) │ │ │ │ │ -insert_or_assign() , similar to Values.h │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:131 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e │ │ │ │ │ +Bayes tree. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:276 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e │ │ │ │ │ +This is the base class for BayesTree cliques. │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e │ │ │ │ │ +A Bayes tree representing a Gaussian density. │ │ │ │ │ +DDeeffiinniittiioonn GaussianBayesTree.h:52 │ │ │ │ │ _g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ with a unique integer... │ │ │ │ │ DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_i_n_s_e_r_t │ │ │ │ │ -iterator insert(const std::pair< Key, Vector > &key_value) │ │ │ │ │ -Insert a vector value with key j. │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.cpp:91 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &str="VectorValues", const KeyFormatter │ │ │ │ │ -&formatter=DefaultKeyFormatter) const │ │ │ │ │ -print required by Testable for unit testing │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.cpp:158 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const VectorValues &x, double tol=1e-9) const │ │ │ │ │ -equals required by Testable for unit testing │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.cpp:166 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_e_x_i_s_t_s │ │ │ │ │ -bool exists(Key j) const │ │ │ │ │ -Check whether a variable with key j exists. │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:133 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_h_t_m_l │ │ │ │ │ -std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ -Output as a html table. │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.cpp:387 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_i_n_s_e_r_t___o_r___a_s_s_i_g_n │ │ │ │ │ -void insert_or_assign(Key j, const Vector &value) │ │ │ │ │ -insert_or_assign that mimics the STL map insert_or_assign - if the value │ │ │ │ │ -already exists,... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:219 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_a_t │ │ │ │ │ -Vector & at(Key j) │ │ │ │ │ -Read/write access to the vector value with key j, throws std::out_of_range if j │ │ │ │ │ -does not exist,... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:139 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_u_p_d_a_t_e │ │ │ │ │ -VectorValues & update(const VectorValues &values) │ │ │ │ │ -For all key/value pairs in values, replace values with corresponding keys in │ │ │ │ │ -this class with those in... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.cpp:101 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_i_n_s_e_r_t │ │ │ │ │ -void insert(Key j, const Value &val) │ │ │ │ │ -Add a variable with the given j, throws KeyAlreadyExists if j is already │ │ │ │ │ -present. │ │ │ │ │ -DDeeffiinniittiioonn Values.cpp:157 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_e_x_i_s_t_s │ │ │ │ │ -bool exists(Key j) const │ │ │ │ │ -Check if a value exists with key j. │ │ │ │ │ -DDeeffiinniittiioonn Values.cpp:94 │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +The Factor::error simply extracts the. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_V_a_l_u_e_s_._h │ │ │ │ │ + * _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00500.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridJunctionTree.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridGaussianISAM.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,32 +94,53 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
HybridJunctionTree.cpp File Reference
│ │ │ │ +
HybridGaussianISAM.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::HybridGaussianISAM
 
struct  gtsam::traits< HybridGaussianISAM >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Mar 11, 2022
│ │ │ │ -
Author
Fan Jiang
│ │ │ │ +
Date
March 31, 2022
│ │ │ │ +
Author
Fan Jiang
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +
│ │ │ │ +Varun Agrawal
│ │ │ │ +
Date
March 31, 2022
│ │ │ │ +
Author
Fan Jiang
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,35 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -HybridJunctionTree.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +HybridGaussianISAM.h File Reference │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Date │ │ │ │ │ - Mar 11, 2022 │ │ │ │ │ + March 31, 2022 │ │ │ │ │ + Author │ │ │ │ │ + Fan Jiang │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Varun Agrawal │ │ │ │ │ + Date │ │ │ │ │ + March 31, 2022 │ │ │ │ │ Author │ │ │ │ │ Fan Jiang │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_J_u_n_c_t_i_o_n_T_r_e_e_._c_p_p │ │ │ │ │ + * _H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00503.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridNonlinearISAM.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,41 +94,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
HybridNonlinearISAM.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
HybridFactor.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::HybridNonlinearISAM
 Wrapper class to manage ISAM in a nonlinear context. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +KeyVector gtsam::CollectKeys (const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys)
 
│ │ │ │ +KeyVector gtsam::CollectKeys (const KeyVector &keys1, const KeyVector &keys2)
 
│ │ │ │ +DiscreteKeys gtsam::CollectDiscreteKeys (const DiscreteKeys &key1, const DiscreteKeys &key2)
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Sep 12, 2022
│ │ │ │ -
Author
Varun Agrawal
│ │ │ │ +
Date
Mar 11, 2022
│ │ │ │ +
Author
Fan Jiang
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,25 +1,30 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -HybridNonlinearISAM.h File Reference │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M │ │ │ │ │ -  Wrapper class to manage _I_S_A_M in a nonlinear context. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +HybridFactor.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ + _K_e_y_V_e_c_t_o_r  ggttssaamm::::CCoolllleeccttKKeeyyss (const _K_e_y_V_e_c_t_o_r &continuousKeys, const │ │ │ │ │ + _D_i_s_c_r_e_t_e_K_e_y_s &discreteKeys) │ │ │ │ │ +  │ │ │ │ │ + _K_e_y_V_e_c_t_o_r  ggttssaamm::::CCoolllleeccttKKeeyyss (const _K_e_y_V_e_c_t_o_r &keys1, const _K_e_y_V_e_c_t_o_r │ │ │ │ │ + &keys2) │ │ │ │ │ +  │ │ │ │ │ +_D_i_s_c_r_e_t_e_K_e_y_s  ggttssaamm::::CCoolllleeccttDDiissccrreetteeKKeeyyss (const _D_i_s_c_r_e_t_e_K_e_y_s &key1, const │ │ │ │ │ + _D_i_s_c_r_e_t_e_K_e_y_s &key2) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Date │ │ │ │ │ - Sep 12, 2022 │ │ │ │ │ + Mar 11, 2022 │ │ │ │ │ Author │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + Fan Jiang │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_._h │ │ │ │ │ + * _H_y_b_r_i_d_F_a_c_t_o_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00506.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridNonlinearISAM.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridNonlinearFactorGraph.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,31 +95,35 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces
│ │ │ │ -
HybridNonlinearISAM.cpp File Reference
│ │ │ │ +
HybridNonlinearFactorGraph.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ + │ │ │ │ +

Nonlinear hybrid factor graph that uses type erasure. │ │ │ │ +More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Sep 12, 2022
│ │ │ │ +

Nonlinear hybrid factor graph that uses type erasure.

│ │ │ │
Author
Varun Agrawal
│ │ │ │ +
Date
May 28, 2022
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,22 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -HybridNonlinearISAM.cpp File Reference │ │ │ │ │ +HybridNonlinearFactorGraph.cpp File Reference │ │ │ │ │ +Nonlinear hybrid factor graph that uses type erasure. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Sep 12, 2022 │ │ │ │ │ +Nonlinear hybrid factor graph that uses type erasure. │ │ │ │ │ Author │ │ │ │ │ Varun Agrawal │ │ │ │ │ + Date │ │ │ │ │ + May 28, 2022 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_._c_p_p │ │ │ │ │ + * _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00512.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridBayesTree.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridSmoother.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,36 +94,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
HybridBayesTree.cpp File Reference
│ │ │ │ +
HybridSmoother.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree. │ │ │ │ +

An incremental smoother for hybrid factor graphs. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::HybridSmoother
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree.

│ │ │ │ -
Date
Mar 11, 2022
│ │ │ │ -
Author
Fan Jiang, Varun Agrawal
│ │ │ │ +

An incremental smoother for hybrid factor graphs.

│ │ │ │ +
Author
Varun Agrawal
│ │ │ │ +
Date
October 2022
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,26 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -HybridBayesTree.cpp File Reference │ │ │ │ │ -Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +HybridSmoother.h File Reference │ │ │ │ │ +An incremental smoother for hybrid factor graphs. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_H_y_b_r_i_d_S_m_o_o_t_h_e_r │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree. │ │ │ │ │ - Date │ │ │ │ │ - Mar 11, 2022 │ │ │ │ │ +An incremental smoother for hybrid factor graphs. │ │ │ │ │ Author │ │ │ │ │ - Fan Jiang, Varun Agrawal │ │ │ │ │ + Varun Agrawal │ │ │ │ │ + Date │ │ │ │ │ + October 2022 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_._c_p_p │ │ │ │ │ + * _H_y_b_r_i_d_S_m_o_o_t_h_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00515.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridEliminationTree.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/GaussianMixture.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,32 +94,48 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces
│ │ │ │ -
HybridEliminationTree.cpp File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
GaussianMixture.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ + │ │ │ │ +

A hybrid conditional in the Conditional Linear Gaussian scheme. │ │ │ │ +More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +std::set< DiscreteKey > gtsam::DiscreteKeysAsSet (const DiscreteKeys &discreteKeys)
 Return the DiscreteKey vector as a set.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Mar 11, 2022
│ │ │ │ -
Author
Fan Jiang
│ │ │ │ +

A hybrid conditional in the Conditional Linear Gaussian scheme.

│ │ │ │ +
Author
Fan Jiang
│ │ │ │ +
│ │ │ │ +Varun Agrawal
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +
Date
Mar 12, 2022
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,29 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -HybridEliminationTree.cpp File Reference │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +GaussianMixture.cpp File Reference │ │ │ │ │ +A hybrid conditional in the Conditional Linear Gaussian scheme. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +std::set< _D_i_s_c_r_e_t_e_K_e_y >  ggttssaamm::::DDiissccrreetteeKKeeyyssAAssSSeett (const _D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ + &discreteKeys) │ │ │ │ │ +  Return the DiscreteKey vector as a set. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Mar 11, 2022 │ │ │ │ │ +A hybrid conditional in the Conditional Linear Gaussian scheme. │ │ │ │ │ Author │ │ │ │ │ Fan Jiang │ │ │ │ │ + Varun Agrawal │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + Mar 12, 2022 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._c_p_p │ │ │ │ │ + * _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00518.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridFactorGraph.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridBayesNet.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,54 +94,49 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs
│ │ │ │ -
HybridFactorGraph.h File Reference
│ │ │ │ +Functions
│ │ │ │ +
HybridBayesNet.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Factor graph with utilities for hybrid factors. │ │ │ │ +

A Bayes net of Gaussian Conditionals indexed by discrete keys. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::HybridFactorGraph
 Hybrid Factor Graph Factor graph with utilities for hybrid factors. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ -Typedefs

│ │ │ │ -using gtsam::SharedFactor = boost::shared_ptr< Factor >
 

│ │ │ │ +Functions

std::function< double(const Assignment< Key > &, double)> gtsam::prunerFunc (const DecisionTreeFactor &prunedDecisionTree, const HybridConditional &conditional)
 Helper function to get the pruner functional.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Factor graph with utilities for hybrid factors.

│ │ │ │ -
Author
Varun Agrawal
│ │ │ │ +

A Bayes net of Gaussian Conditionals indexed by discrete keys.

│ │ │ │ +
Author
Fan Jiang
│ │ │ │ +
│ │ │ │ +Varun Agrawal
│ │ │ │ +
│ │ │ │ +Shangjie Xue
│ │ │ │
│ │ │ │ Frank Dellaert
│ │ │ │ -
Date
May 28, 2022
│ │ │ │ +
Date
January 2022
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,32 +1,32 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ -HybridFactorGraph.h File Reference │ │ │ │ │ -Factor graph with utilities for hybrid factors. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -  Hybrid _F_a_c_t_o_r Graph _F_a_c_t_o_r graph with utilities for hybrid factors. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +HybridBayesNet.cpp File Reference │ │ │ │ │ +A Bayes net of Gaussian Conditionals indexed by discrete keys. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -using  ggttssaamm::::SShhaarreeddFFaaccttoorr = boost::shared_ptr< _F_a_c_t_o_r > │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +std::function< double(const _A_s_s_i_g_n_m_e_n_t< _g_t_s_a_m_:_:_p_r_u_n_e_r_F_u_n_c (const │ │ │ │ │ + _K_e_y > &, double)>  _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r &prunedDecisionTree, │ │ │ │ │ + const _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l &conditional) │ │ │ │ │ +  Helper function to get the pruner │ │ │ │ │ + functional. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Factor graph with utilities for hybrid factors. │ │ │ │ │ +A Bayes net of Gaussian Conditionals indexed by discrete keys. │ │ │ │ │ Author │ │ │ │ │ + Fan Jiang │ │ │ │ │ Varun Agrawal │ │ │ │ │ + Shangjie Xue │ │ │ │ │ Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - May 28, 2022 │ │ │ │ │ + January 2022 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ + * _H_y_b_r_i_d_B_a_y_e_s_N_e_t_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00518.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a00518 = [ │ │ │ │ │ - ["gtsam::HybridFactorGraph", "a03456.html", "a03456"] │ │ │ │ │ + ["prunerFunc", "a00518.html#ae6415c63d70d1b582a0162652b16b868", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00521.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridFactorGraph.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridJunctionTree.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,37 +95,31 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces
│ │ │ │ -
HybridFactorGraph.cpp File Reference
│ │ │ │ +
HybridJunctionTree.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -

Factor graph with utilities for hybrid factors. │ │ │ │ -More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Factor graph with utilities for hybrid factors.

│ │ │ │ -
Author
Varun Agrawal
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ -
Date
January, 2023
│ │ │ │ +
Date
Mar 11, 2022
│ │ │ │ +
Author
Fan Jiang
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,20 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -HybridFactorGraph.cpp File Reference │ │ │ │ │ -Factor graph with utilities for hybrid factors. _M_o_r_e_._._. │ │ │ │ │ +HybridJunctionTree.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Factor graph with utilities for hybrid factors. │ │ │ │ │ - Author │ │ │ │ │ - Varun Agrawal │ │ │ │ │ - Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - January, 2023 │ │ │ │ │ + Mar 11, 2022 │ │ │ │ │ + Author │ │ │ │ │ + Fan Jiang │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h_._c_p_p │ │ │ │ │ + * _H_y_b_r_i_d_J_u_n_c_t_i_o_n_T_r_e_e_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00524.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridBayesNet.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/GaussianMixtureFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,52 +94,40 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
HybridBayesNet.h File Reference
│ │ │ │ +
GaussianMixtureFactor.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

A Bayes net of Gaussian Conditionals indexed by discrete keys. │ │ │ │ +

A set of Gaussian factors indexed by a set of discrete keys. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::HybridBayesNet
 A hybrid Bayes net is a collection of HybridConditionals, which can have discrete conditionals, Gaussian mixtures, or pure Gaussian conditionals. More...
 
struct  gtsam::traits< HybridBayesNet >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

A Bayes net of Gaussian Conditionals indexed by discrete keys.

│ │ │ │ -
Author
Varun Agrawal
│ │ │ │ +

A set of Gaussian factors indexed by a set of discrete keys.

│ │ │ │ +
Author
Fan Jiang
│ │ │ │
│ │ │ │ -Fan Jiang
│ │ │ │ +Varun Agrawal │ │ │ │
│ │ │ │ Frank Dellaert
│ │ │ │ -
Date
December 2021
│ │ │ │ +
Date
Mar 12, 2022
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,34 +1,24 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -HybridBayesNet.h File Reference │ │ │ │ │ -A Bayes net of Gaussian Conditionals indexed by discrete keys. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_N_e_t │ │ │ │ │ - A hybrid Bayes net is a collection of HybridConditionals, which can │ │ │ │ │ -  have discrete conditionals, Gaussian mixtures, or pure Gaussian │ │ │ │ │ - conditionals. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _H_y_b_r_i_d_B_a_y_e_s_N_e_t_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +GaussianMixtureFactor.cpp File Reference │ │ │ │ │ +A set of Gaussian factors indexed by a set of discrete keys. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A Bayes net of Gaussian Conditionals indexed by discrete keys. │ │ │ │ │ +A set of Gaussian factors indexed by a set of discrete keys. │ │ │ │ │ Author │ │ │ │ │ - Varun Agrawal │ │ │ │ │ Fan Jiang │ │ │ │ │ + Varun Agrawal │ │ │ │ │ Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - December 2021 │ │ │ │ │ + Mar 12, 2022 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_B_a_y_e_s_N_e_t_._h │ │ │ │ │ + * _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00527.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/MixtureFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridGaussianFactorGraph.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,47 +94,60 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
MixtureFactor.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ +Functions
│ │ │ │ +
HybridGaussianFactorGraph.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Nonlinear Mixture factor of continuous and discrete. │ │ │ │ +

Hybrid factor graph that uses type erasure. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::MixtureFactor
 Implementation of a discrete conditional mixture factor. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Typedefs

│ │ │ │ +using gtsam::OrphanWrapper = BayesTreeOrphanWrapper< HybridBayesTree::Clique >
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

const Ordering gtsam::HybridOrdering (const HybridGaussianFactorGraph &graph)
 Return a Colamd constrained ordering where the discrete keys are eliminated after the continuous keys.
 
│ │ │ │ +GaussianFactorGraphTree gtsam::removeEmpty (const GaussianFactorGraphTree &sum)
 
std::pair< HybridConditional::shared_ptr, boost::shared_ptr< Factor > > gtsam::EliminateHybrid (const HybridGaussianFactorGraph &factors, const Ordering &keys)
 Main elimination function for HybridGaussianFactorGraph.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Nonlinear Mixture factor of continuous and discrete.

│ │ │ │ -
Author
Kevin Doherty, kdohe.nosp@m.rty@.nosp@m.mit.e.nosp@m.du
│ │ │ │ +

Hybrid factor graph that uses type erasure.

│ │ │ │ +
Author
Fan Jiang
│ │ │ │ +
│ │ │ │ +Varun Agrawal
│ │ │ │
│ │ │ │ -Varun Agrawal
│ │ │ │ -
Date
December 2021
│ │ │ │ +Frank Dellaert
│ │ │ │ +
Date
Mar 11, 2022
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,43 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -MixtureFactor.h File Reference │ │ │ │ │ -Nonlinear Mixture factor of continuous and discrete. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_M_i_x_t_u_r_e_F_a_c_t_o_r │ │ │ │ │ -  Implementation of a discrete conditional mixture factor. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +HybridGaussianFactorGraph.cpp File Reference │ │ │ │ │ +Hybrid factor graph that uses type erasure. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +using  ggttssaamm::::OOrrpphhaannWWrraappppeerr = _B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r< _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_:_:_C_l_i_q_u_e > │ │ │ │ │ +  │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ + const _O_r_d_e_r_i_n_g  _g_t_s_a_m_:_:_H_y_b_r_i_d_O_r_d_e_r_i_n_g (const │ │ │ │ │ + _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &graph) │ │ │ │ │ + Return a Colamd constrained ordering │ │ │ │ │ +  where the discrete keys are eliminated │ │ │ │ │ + after the continuous keys. │ │ │ │ │ +  │ │ │ │ │ + _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_T_r_e_e  ggttssaamm::::rreemmoovveeEEmmppttyy (const │ │ │ │ │ + _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_T_r_e_e &sum) │ │ │ │ │ +  │ │ │ │ │ + std::pair< _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_:_: _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_H_y_b_r_i_d (const │ │ │ │ │ +_s_h_a_r_e_d___p_t_r, boost::shared_ptr< _F_a_c_t_o_r > _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &factors, │ │ │ │ │ + >  const _O_r_d_e_r_i_n_g &keys) │ │ │ │ │ +  Main elimination function for │ │ │ │ │ + _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Nonlinear Mixture factor of continuous and discrete. │ │ │ │ │ +Hybrid factor graph that uses type erasure. │ │ │ │ │ Author │ │ │ │ │ - Kevin Doherty, _k_d_o_h_e_._n_o_s_p_@_m_._r_t_y_@_._n_o_s_p_@_m_._m_i_t_._e_._n_o_s_p_@_m_._d_u │ │ │ │ │ + Fan Jiang │ │ │ │ │ Varun Agrawal │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - December 2021 │ │ │ │ │ + Mar 11, 2022 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _M_i_x_t_u_r_e_F_a_c_t_o_r_._h │ │ │ │ │ + * _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00527.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,4 @@ │ │ │ │ │ var a00527 = [ │ │ │ │ │ - ["gtsam::MixtureFactor", "a03512.html", "a03512"] │ │ │ │ │ + ["EliminateHybrid", "a00527.html#gacbc9f97e48e6aa3abb5f789af8c3bfe5", null], │ │ │ │ │ + ["HybridOrdering", "a00527.html#a584e4e05d52a617c1d313e1f53c8ce58", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00530.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridSmoother.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridFactorGraph.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,35 +95,37 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces
│ │ │ │ -
HybridSmoother.cpp File Reference
│ │ │ │ +
HybridFactorGraph.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

An incremental smoother for hybrid factor graphs. │ │ │ │ +

Factor graph with utilities for hybrid factors. │ │ │ │ More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

An incremental smoother for hybrid factor graphs.

│ │ │ │ -
Author
Varun Agrawal
│ │ │ │ -
Date
October 2022
│ │ │ │ +

Factor graph with utilities for hybrid factors.

│ │ │ │ +
Author
Varun Agrawal
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +
Date
January, 2023
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,23 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -HybridSmoother.cpp File Reference │ │ │ │ │ -An incremental smoother for hybrid factor graphs. _M_o_r_e_._._. │ │ │ │ │ +HybridFactorGraph.cpp File Reference │ │ │ │ │ +Factor graph with utilities for hybrid factors. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -An incremental smoother for hybrid factor graphs. │ │ │ │ │ +Factor graph with utilities for hybrid factors. │ │ │ │ │ Author │ │ │ │ │ Varun Agrawal │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - October 2022 │ │ │ │ │ + January, 2023 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_S_m_o_o_t_h_e_r_._c_p_p │ │ │ │ │ + * _H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00533.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/GaussianMixtureFactor.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/GaussianMixtureFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,40 +94,51 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
GaussianMixtureFactor.cpp File Reference
│ │ │ │ +
GaussianMixtureFactor.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

A set of Gaussian factors indexed by a set of discrete keys. │ │ │ │ +

A set of GaussianFactors, indexed by a set of discrete keys. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::GaussianMixtureFactor
 Implementation of a discrete conditional mixture factor. More...
 
struct  gtsam::traits< GaussianMixtureFactor >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

A set of Gaussian factors indexed by a set of discrete keys.

│ │ │ │ +

A set of GaussianFactors, indexed by a set of discrete keys.

│ │ │ │
Author
Fan Jiang
│ │ │ │
│ │ │ │ Varun Agrawal
│ │ │ │
│ │ │ │ Frank Dellaert
│ │ │ │
Date
Mar 12, 2022
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,24 +1,31 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -GaussianMixtureFactor.cpp File Reference │ │ │ │ │ -A set of Gaussian factors indexed by a set of discrete keys. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +GaussianMixtureFactor.h File Reference │ │ │ │ │ +A set of GaussianFactors, indexed by a set of discrete keys. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r │ │ │ │ │ +  Implementation of a discrete conditional mixture factor. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r_ _> │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A set of Gaussian factors indexed by a set of discrete keys. │ │ │ │ │ +A set of GaussianFactors, indexed by a set of discrete keys. │ │ │ │ │ Author │ │ │ │ │ Fan Jiang │ │ │ │ │ Varun Agrawal │ │ │ │ │ Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ Mar 12, 2022 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r_._c_p_p │ │ │ │ │ + * _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00536.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridGaussianISAM.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/GaussianMixture.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,52 +95,56 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
HybridGaussianISAM.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
GaussianMixture.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ +

A hybrid conditional in the Conditional Linear Gaussian scheme. │ │ │ │ +More...

│ │ │ │ + │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::HybridGaussianISAM
class  gtsam::GaussianMixture
 A conditional of gaussian mixtures indexed by discrete variables, as part of a Bayes Network. More...
 
struct  gtsam::traits< HybridGaussianISAM >
 traits More...
struct  gtsam::traits< GaussianMixture >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +std::set< DiscreteKey > gtsam::DiscreteKeysAsSet (const DiscreteKeys &discreteKeys)
 Return the DiscreteKey vector as a set.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
March 31, 2022
│ │ │ │ -
Author
Fan Jiang
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ -
│ │ │ │ -Varun Agrawal
│ │ │ │ -
Date
March 31, 2022
│ │ │ │ +

A hybrid conditional in the Conditional Linear Gaussian scheme.

│ │ │ │
Author
Fan Jiang
│ │ │ │
│ │ │ │ -Frank Dellaert
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ +Varun Agrawal │ │ │ │ +
Date
Mar 12, 2022
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,35 +1,36 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -HybridGaussianISAM.h File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +GaussianMixture.h File Reference │ │ │ │ │ +A hybrid conditional in the Conditional Linear Gaussian scheme. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M │ │ │ │ │ + class   _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e │ │ │ │ │ +  A conditional of gaussian mixtures indexed by discrete variables, as │ │ │ │ │ + part of a Bayes Network. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +std::set< _D_i_s_c_r_e_t_e_K_e_y >  ggttssaamm::::DDiissccrreetteeKKeeyyssAAssSSeett (const _D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ + &discreteKeys) │ │ │ │ │ +  Return the DiscreteKey vector as a set. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - March 31, 2022 │ │ │ │ │ +A hybrid conditional in the Conditional Linear Gaussian scheme. │ │ │ │ │ Author │ │ │ │ │ Fan Jiang │ │ │ │ │ - Frank Dellaert │ │ │ │ │ Varun Agrawal │ │ │ │ │ Date │ │ │ │ │ - March 31, 2022 │ │ │ │ │ - Author │ │ │ │ │ - Fan Jiang │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Mar 12, 2022 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M_._h │ │ │ │ │ + * _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00536.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,4 @@ │ │ │ │ │ var a00536 = [ │ │ │ │ │ - ["gtsam::traits< HybridGaussianISAM >", "a03472.html", null] │ │ │ │ │ + ["gtsam::traits< GaussianMixture >", "a03396.html", null], │ │ │ │ │ + ["DiscreteKeysAsSet", "a00536.html#ad92a1966f54e02a7c2ce061c1228c7b3", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00536_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridGaussianISAM.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/GaussianMixture.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,15 +98,15 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
HybridGaussianISAM.h
│ │ │ │ +
GaussianMixture.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │ @@ -114,75 +114,166 @@ │ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
20#pragma once
│ │ │ │
21
│ │ │ │ -
22#include <gtsam/base/Testable.h>
│ │ │ │ -
23#include <gtsam/hybrid/HybridBayesTree.h>
│ │ │ │ -
24#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
│ │ │ │ -
25#include <gtsam/inference/ISAM.h>
│ │ │ │ -
26
│ │ │ │ -
27namespace gtsam {
│ │ │ │ -
28
│ │ │ │ -
│ │ │ │ -
34class GTSAM_EXPORT HybridGaussianISAM : public ISAM<HybridBayesTree> {
│ │ │ │ -
35 public:
│ │ │ │ -
36 typedef ISAM<HybridBayesTree> Base;
│ │ │ │ -
37 typedef HybridGaussianISAM This;
│ │ │ │ -
38 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
39
│ │ │ │ -
42
│ │ │ │ -
44 HybridGaussianISAM();
│ │ │ │ -
45
│ │ │ │ -
47 HybridGaussianISAM(const HybridBayesTree& bayesTree);
│ │ │ │ -
48
│ │ │ │ -
50
│ │ │ │ -
51 private:
│ │ │ │ -
53 void updateInternal(
│ │ │ │ -
54 const HybridGaussianFactorGraph& newFactors,
│ │ │ │ -
55 HybridBayesTree::Cliques* orphans,
│ │ │ │ -
56 const boost::optional<size_t>& maxNrLeaves = boost::none,
│ │ │ │ -
57 const boost::optional<Ordering>& ordering = boost::none,
│ │ │ │ -
58 const HybridBayesTree::Eliminate& function =
│ │ │ │ -
59 HybridBayesTree::EliminationTraitsType::DefaultEliminate);
│ │ │ │ -
60
│ │ │ │ -
61 public:
│ │ │ │ -
70 void update(const HybridGaussianFactorGraph& newFactors,
│ │ │ │ -
71 const boost::optional<size_t>& maxNrLeaves = boost::none,
│ │ │ │ -
72 const boost::optional<Ordering>& ordering = boost::none,
│ │ │ │ -
73 const HybridBayesTree::Eliminate& function =
│ │ │ │ -
74 HybridBayesTree::EliminationTraitsType::DefaultEliminate);
│ │ │ │ -
75
│ │ │ │ -
84 static Ordering GetOrdering(HybridGaussianFactorGraph& factors,
│ │ │ │ -
85 const HybridGaussianFactorGraph& newFactors);
│ │ │ │ -
86};
│ │ │ │ -
│ │ │ │ -
87
│ │ │ │ -
89template <>
│ │ │ │ -
90struct traits<HybridGaussianISAM> : public Testable<HybridGaussianISAM> {};
│ │ │ │ +
22#include <gtsam/discrete/DecisionTree-inl.h>
│ │ │ │ +
23#include <gtsam/discrete/DecisionTree.h>
│ │ │ │ +
24#include <gtsam/discrete/DecisionTreeFactor.h>
│ │ │ │ +
25#include <gtsam/discrete/DiscreteKey.h>
│ │ │ │ +
26#include <gtsam/hybrid/GaussianMixtureFactor.h>
│ │ │ │ +
27#include <gtsam/hybrid/HybridFactor.h>
│ │ │ │ +
28#include <gtsam/inference/Conditional.h>
│ │ │ │ +
29#include <gtsam/linear/GaussianConditional.h>
│ │ │ │ +
30
│ │ │ │ +
31namespace gtsam {
│ │ │ │ +
32
│ │ │ │ +
33class HybridValues;
│ │ │ │ +
34
│ │ │ │ +
│ │ │ │ +
53class GTSAM_EXPORT GaussianMixture
│ │ │ │ +
54 : public HybridFactor,
│ │ │ │ +
55 public Conditional<HybridFactor, GaussianMixture> {
│ │ │ │ +
56 public:
│ │ │ │ +
57 using This = GaussianMixture;
│ │ │ │ +
58 using shared_ptr = boost::shared_ptr<GaussianMixture>;
│ │ │ │ +
59 using BaseFactor = HybridFactor;
│ │ │ │ +
60 using BaseConditional = Conditional<HybridFactor, GaussianMixture>;
│ │ │ │ +
61
│ │ │ │ +
63 using Conditionals = DecisionTree<Key, GaussianConditional::shared_ptr>;
│ │ │ │ +
64
│ │ │ │ +
65 private:
│ │ │ │ +
66 Conditionals conditionals_;
│ │ │ │ +
67 double logConstant_;
│ │ │ │ +
68
│ │ │ │ +
72 GaussianFactorGraphTree asGaussianFactorGraphTree() const;
│ │ │ │ +
73
│ │ │ │ +
81 std::function<GaussianConditional::shared_ptr(
│ │ │ │ +
82 const Assignment<Key> &, const GaussianConditional::shared_ptr &)>
│ │ │ │ +
83 prunerFunc(const DecisionTreeFactor &decisionTree);
│ │ │ │ +
84
│ │ │ │ +
85 public:
│ │ │ │ +
88
│ │ │ │ +
90 GaussianMixture() = default;
│ │ │ │
91
│ │ │ │ -
92} // namespace gtsam
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ -
HybridBayesTree.h
Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree.
│ │ │ │ -
HybridGaussianFactorGraph.h
Linearized Hybrid factor graph that uses type erasure.
│ │ │ │ -
ISAM.h
Incremental update functionality (iSAM) for BayesTree.
│ │ │ │ +
103 GaussianMixture(const KeyVector &continuousFrontals,
│ │ │ │ +
104 const KeyVector &continuousParents,
│ │ │ │ +
105 const DiscreteKeys &discreteParents,
│ │ │ │ +
106 const Conditionals &conditionals);
│ │ │ │ +
107
│ │ │ │ +
116 GaussianMixture(KeyVector &&continuousFrontals, KeyVector &&continuousParents,
│ │ │ │ +
117 DiscreteKeys &&discreteParents,
│ │ │ │ +
118 std::vector<GaussianConditional::shared_ptr> &&conditionals);
│ │ │ │ +
119
│ │ │ │ +
128 GaussianMixture(
│ │ │ │ +
129 const KeyVector &continuousFrontals, const KeyVector &continuousParents,
│ │ │ │ +
130 const DiscreteKeys &discreteParents,
│ │ │ │ +
131 const std::vector<GaussianConditional::shared_ptr> &conditionals);
│ │ │ │ +
132
│ │ │ │ +
136
│ │ │ │ +
138 bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
│ │ │ │ +
139
│ │ │ │ +
141 void print(
│ │ │ │ +
142 const std::string &s = "GaussianMixture\n",
│ │ │ │ +
143 const KeyFormatter &formatter = DefaultKeyFormatter) const override;
│ │ │ │ +
144
│ │ │ │ +
148
│ │ │ │ +
150 GaussianConditional::shared_ptr operator()(
│ │ │ │ +
151 const DiscreteValues &discreteValues) const;
│ │ │ │ +
152
│ │ │ │ +
154 size_t nrComponents() const;
│ │ │ │ +
155
│ │ │ │ +
157 KeyVector continuousParents() const;
│ │ │ │ +
158
│ │ │ │ +
161 double logNormalizationConstant() const override { return logConstant_; }
│ │ │ │ +
162
│ │ │ │ +
167 boost::shared_ptr<GaussianMixtureFactor> likelihood(
│ │ │ │ +
168 const VectorValues &given) const;
│ │ │ │ +
169
│ │ │ │ +
171 const Conditionals &conditionals() const;
│ │ │ │ +
172
│ │ │ │ +
180 AlgebraicDecisionTree<Key> logProbability(
│ │ │ │ +
181 const VectorValues &continuousValues) const;
│ │ │ │ +
182
│ │ │ │ +
208 double error(const HybridValues &values) const override;
│ │ │ │ +
209
│ │ │ │ +
217 AlgebraicDecisionTree<Key> error(const VectorValues &continuousValues) const;
│ │ │ │ +
218
│ │ │ │ +
225 double logProbability(const HybridValues &values) const override;
│ │ │ │ +
226
│ │ │ │ +
228 double evaluate(const HybridValues &values) const override;
│ │ │ │ +
229
│ │ │ │ +
│ │ │ │ +
231 double operator()(const HybridValues &values) const {
│ │ │ │ +
232 return evaluate(values);
│ │ │ │ +
233 }
│ │ │ │ +
│ │ │ │ +
234
│ │ │ │ +
242 void prune(const DecisionTreeFactor &decisionTree);
│ │ │ │ +
243
│ │ │ │ +
251 GaussianFactorGraphTree add(const GaussianFactorGraphTree &sum) const;
│ │ │ │ +
253
│ │ │ │ +
254 private:
│ │ │ │ +
256 bool allFrontalsGiven(const VectorValues &given) const;
│ │ │ │ +
257
│ │ │ │ +
259 friend class boost::serialization::access;
│ │ │ │ +
260 template <class Archive>
│ │ │ │ +
261 void serialize(Archive &ar, const unsigned int /*version*/) {
│ │ │ │ +
262 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseFactor);
│ │ │ │ +
263 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseConditional);
│ │ │ │ +
264 ar &BOOST_SERIALIZATION_NVP(conditionals_);
│ │ │ │ +
265 }
│ │ │ │ +
266};
│ │ │ │ +
│ │ │ │ +
267
│ │ │ │ +
269std::set<DiscreteKey> DiscreteKeysAsSet(const DiscreteKeys &discreteKeys);
│ │ │ │ +
270
│ │ │ │ +
271// traits
│ │ │ │ +
272template <>
│ │ │ │ +
273struct traits<GaussianMixture> : public Testable<GaussianMixture> {};
│ │ │ │ +
274
│ │ │ │ +
275} // namespace gtsam
│ │ │ │ + │ │ │ │ +
DecisionTree.h
Decision Tree for use in DiscreteFactors.
│ │ │ │ +
DiscreteKey.h
specialized key for discrete variables
│ │ │ │ + │ │ │ │ +
GaussianMixtureFactor.h
A set of GaussianFactors, indexed by a set of discrete keys.
│ │ │ │ +
Conditional.h
Base class for conditional densities.
│ │ │ │ +
GaussianConditional.h
Conditional Gaussian Base class.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::DiscreteKeysAsSet
std::set< DiscreteKey > DiscreteKeysAsSet(const DiscreteKeys &discreteKeys)
Return the DiscreteKey vector as a set.
Definition GaussianMixture.cpp:221
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::prunerFunc
std::function< double(const Assignment< Key > &, double)> prunerFunc(const DecisionTreeFactor &prunedDecisionTree, const HybridConditional &conditional)
Helper function to get the pruner functional.
Definition HybridBayesNet.cpp:66
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::HybridBayesTree
A Bayes tree representing a Hybrid density.
Definition HybridBayesTree.h:64
│ │ │ │ -
gtsam::HybridGaussianFactorGraph
Definition HybridGaussianFactorGraph.h:102
│ │ │ │ -
gtsam::HybridGaussianISAM
Definition HybridGaussianISAM.h:34
│ │ │ │ -
gtsam::ISAM
A Bayes tree with an update methods that implements the iSAM algorithm.
Definition ISAM.h:31
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
gtsam::AlgebraicDecisionTree
An algebraic decision tree fixes the range of a DecisionTree to double.
Definition AlgebraicDecisionTree.h:38
│ │ │ │ +
gtsam::Assignment
An assignment from labels to value index (size_t).
Definition Assignment.h:37
│ │ │ │ + │ │ │ │ +
gtsam::DecisionTreeFactor
A discrete probabilistic factor.
Definition DecisionTreeFactor.h:45
│ │ │ │ +
gtsam::DiscreteKeys
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition DiscreteKey.h:39
│ │ │ │ +
gtsam::DiscreteValues
A map from keys to values.
Definition DiscreteValues.h:34
│ │ │ │ +
gtsam::GaussianMixture
A conditional of gaussian mixtures indexed by discrete variables, as part of a Bayes Network.
Definition GaussianMixture.h:55
│ │ │ │ +
gtsam::GaussianMixture::logNormalizationConstant
double logNormalizationConstant() const override
The log normalization constant is max of the the individual log-normalization constants.
Definition GaussianMixture.h:161
│ │ │ │ +
gtsam::GaussianMixture::operator()
double operator()(const HybridValues &values) const
Evaluate probability density, sugar.
Definition GaussianMixture.h:231
│ │ │ │ +
gtsam::GaussianMixture::GaussianMixture
GaussianMixture()=default
Default constructor, mainly for serialization.
│ │ │ │ +
gtsam::HybridFactor
Base class for truly hybrid probabilistic factors.
Definition HybridFactor.h:52
│ │ │ │ +
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ +
gtsam::Conditional
Definition Conditional.h:64
│ │ │ │ +
gtsam::GaussianConditional::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianConditional.h:46
│ │ │ │ +
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
HybridValues
the error.
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -HybridGaussianISAM.h │ │ │ │ │ +GaussianMixture.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ @@ -16,84 +16,221 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 20#pragma once │ │ │ │ │ 21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_I_S_A_M_._h> │ │ │ │ │ -26 │ │ │ │ │ -27namespace _g_t_s_a_m { │ │ │ │ │ -28 │ │ │ │ │ -_3_4class GTSAM_EXPORT _H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M : public _I_S_A_M { │ │ │ │ │ -35 public: │ │ │ │ │ -36 typedef _I_S_A_M_<_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_> _B_a_s_e; │ │ │ │ │ -37 typedef _H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M _T_h_i_s; │ │ │ │ │ -38 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -39 │ │ │ │ │ -42 │ │ │ │ │ -44 _H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M(); │ │ │ │ │ -45 │ │ │ │ │ -47 _H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M(const _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e& bayesTree); │ │ │ │ │ -48 │ │ │ │ │ -50 │ │ │ │ │ -51 private: │ │ │ │ │ -53 void updateInternal( │ │ │ │ │ -54 const _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& newFactors, │ │ │ │ │ -55 HybridBayesTree::Cliques* orphans, │ │ │ │ │ -56 const boost::optional& maxNrLeaves = boost::none, │ │ │ │ │ -57 const boost::optional& ordering = boost::none, │ │ │ │ │ -58 const HybridBayesTree::Eliminate& function = │ │ │ │ │ -59 HybridBayesTree::EliminationTraitsType::DefaultEliminate); │ │ │ │ │ -60 │ │ │ │ │ -61 public: │ │ │ │ │ -70 void update(const _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& newFactors, │ │ │ │ │ -71 const boost::optional& maxNrLeaves = boost::none, │ │ │ │ │ -72 const boost::optional& ordering = boost::none, │ │ │ │ │ -73 const HybridBayesTree::Eliminate& function = │ │ │ │ │ -74 HybridBayesTree::EliminationTraitsType::DefaultEliminate); │ │ │ │ │ -75 │ │ │ │ │ -84 static _O_r_d_e_r_i_n_g GetOrdering(_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& factors, │ │ │ │ │ -85 const _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& newFactors); │ │ │ │ │ -86}; │ │ │ │ │ -87 │ │ │ │ │ -89template <> │ │ │ │ │ -_9_0struct _t_r_a_i_t_s<_H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ +22#include │ │ │ │ │ +23#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_e_c_i_s_i_o_n_T_r_e_e_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_K_e_y_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r_._h> │ │ │ │ │ +27#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_F_a_c_t_o_r_._h> │ │ │ │ │ +28#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ +29#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ +30 │ │ │ │ │ +31namespace _g_t_s_a_m { │ │ │ │ │ +32 │ │ │ │ │ +33class _H_y_b_r_i_d_V_a_l_u_e_s; │ │ │ │ │ +34 │ │ │ │ │ +_5_3class GTSAM_EXPORT _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e │ │ │ │ │ +54 : public _H_y_b_r_i_d_F_a_c_t_o_r, │ │ │ │ │ +55 public _C_o_n_d_i_t_i_o_n_a_l { │ │ │ │ │ +56 public: │ │ │ │ │ +57 using _T_h_i_s = _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e; │ │ │ │ │ +58 using shared_ptr = boost::shared_ptr; │ │ │ │ │ +59 using _B_a_s_e_F_a_c_t_o_r = _H_y_b_r_i_d_F_a_c_t_o_r; │ │ │ │ │ +60 using _B_a_s_e_C_o_n_d_i_t_i_o_n_a_l = _C_o_n_d_i_t_i_o_n_a_l_<_H_y_b_r_i_d_F_a_c_t_o_r_,_ _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_>; │ │ │ │ │ +61 │ │ │ │ │ +_6_3 using _C_o_n_d_i_t_i_o_n_a_l_s = _D_e_c_i_s_i_o_n_T_r_e_e_<_K_e_y_,_ _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r_>; │ │ │ │ │ +64 │ │ │ │ │ +65 private: │ │ │ │ │ +66 _C_o_n_d_i_t_i_o_n_a_l_s conditionals_; │ │ │ │ │ +67 double logConstant_; │ │ │ │ │ +68 │ │ │ │ │ +72 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_T_r_e_e asGaussianFactorGraphTree() const; │ │ │ │ │ +73 │ │ │ │ │ +81 std::function<_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r( │ │ │ │ │ +82 const _A_s_s_i_g_n_m_e_n_t_<_K_e_y_> &, const _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r &)> │ │ │ │ │ +83 _p_r_u_n_e_r_F_u_n_c(const _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r &decisionTree); │ │ │ │ │ +84 │ │ │ │ │ +85 public: │ │ │ │ │ +88 │ │ │ │ │ +_9_0 _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e() = default; │ │ │ │ │ 91 │ │ │ │ │ -92} // namespace gtsam │ │ │ │ │ -_T_e_s_t_a_b_l_e_._h │ │ │ │ │ -Concept check for values that can be used in unit tests. │ │ │ │ │ -_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ -Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree. │ │ │ │ │ -_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Linearized Hybrid factor graph that uses type erasure. │ │ │ │ │ -_I_S_A_M_._h │ │ │ │ │ -Incremental update functionality (iSAM) for BayesTree. │ │ │ │ │ +103 _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e(const _K_e_y_V_e_c_t_o_r &continuousFrontals, │ │ │ │ │ +104 const _K_e_y_V_e_c_t_o_r &continuousParents, │ │ │ │ │ +105 const _D_i_s_c_r_e_t_e_K_e_y_s &discreteParents, │ │ │ │ │ +106 const _C_o_n_d_i_t_i_o_n_a_l_s &conditionals); │ │ │ │ │ +107 │ │ │ │ │ +116 _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e(_K_e_y_V_e_c_t_o_r &&continuousFrontals, _K_e_y_V_e_c_t_o_r │ │ │ │ │ +&&continuousParents, │ │ │ │ │ +117 _D_i_s_c_r_e_t_e_K_e_y_s &&discreteParents, │ │ │ │ │ +118 std::vector &&conditionals); │ │ │ │ │ +119 │ │ │ │ │ +128 _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e( │ │ │ │ │ +129 const _K_e_y_V_e_c_t_o_r &continuousFrontals, const _K_e_y_V_e_c_t_o_r &continuousParents, │ │ │ │ │ +130 const _D_i_s_c_r_e_t_e_K_e_y_s &discreteParents, │ │ │ │ │ +131 const std::vector &conditionals); │ │ │ │ │ +132 │ │ │ │ │ +136 │ │ │ │ │ +138 bool _e_q_u_a_l_s(const _H_y_b_r_i_d_F_a_c_t_o_r &lf, double tol = 1e-9) const override; │ │ │ │ │ +139 │ │ │ │ │ +141 void _p_r_i_n_t( │ │ │ │ │ +142 const std::string &s = "GaussianMixture\n", │ │ │ │ │ +143 const _K_e_y_F_o_r_m_a_t_t_e_r &formatter = DefaultKeyFormatter) const override; │ │ │ │ │ +144 │ │ │ │ │ +148 │ │ │ │ │ +150 _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r operator()( │ │ │ │ │ +151 const _D_i_s_c_r_e_t_e_V_a_l_u_e_s &discreteValues) const; │ │ │ │ │ +152 │ │ │ │ │ +154 size_t nrComponents() const; │ │ │ │ │ +155 │ │ │ │ │ +157 _K_e_y_V_e_c_t_o_r continuousParents() const; │ │ │ │ │ +158 │ │ │ │ │ +_1_6_1 double _l_o_g_N_o_r_m_a_l_i_z_a_t_i_o_n_C_o_n_s_t_a_n_t() const override { return logConstant_; } │ │ │ │ │ +162 │ │ │ │ │ +167 boost::shared_ptr likelihood( │ │ │ │ │ +168 const _V_e_c_t_o_r_V_a_l_u_e_s &given) const; │ │ │ │ │ +169 │ │ │ │ │ +171 const Conditionals &conditionals() const; │ │ │ │ │ +172 │ │ │ │ │ +180 _A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_<_K_e_y_> logProbability( │ │ │ │ │ +181 const _V_e_c_t_o_r_V_a_l_u_e_s &continuousValues) const; │ │ │ │ │ +182 │ │ │ │ │ +208 double error(const _H_y_b_r_i_d_V_a_l_u_e_s &values) const override; │ │ │ │ │ +209 │ │ │ │ │ +217 _A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_<_K_e_y_> error(const _V_e_c_t_o_r_V_a_l_u_e_s &continuousValues) │ │ │ │ │ +const; │ │ │ │ │ +218 │ │ │ │ │ +225 double logProbability(const _H_y_b_r_i_d_V_a_l_u_e_s &values) const override; │ │ │ │ │ +226 │ │ │ │ │ +228 double evaluate(const _H_y_b_r_i_d_V_a_l_u_e_s &values) const override; │ │ │ │ │ +229 │ │ │ │ │ +_2_3_1 double _o_p_e_r_a_t_o_r_(_)(const _H_y_b_r_i_d_V_a_l_u_e_s &values) const { │ │ │ │ │ +232 return evaluate(values); │ │ │ │ │ +233 } │ │ │ │ │ +234 │ │ │ │ │ +242 void prune(const _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r &decisionTree); │ │ │ │ │ +243 │ │ │ │ │ +251 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_T_r_e_e add(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_T_r_e_e &sum) const; │ │ │ │ │ +253 │ │ │ │ │ +254 private: │ │ │ │ │ +256 bool allFrontalsGiven(const _V_e_c_t_o_r_V_a_l_u_e_s &given) const; │ │ │ │ │ +257 │ │ │ │ │ +_2_5_9 friend class boost::serialization::access; │ │ │ │ │ +260 template │ │ │ │ │ +261 void serialize(Archive &ar, const unsigned int /*version*/) { │ │ │ │ │ +262 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e_F_a_c_t_o_r); │ │ │ │ │ +263 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e_C_o_n_d_i_t_i_o_n_a_l); │ │ │ │ │ +264 ar &BOOST_SERIALIZATION_NVP(conditionals_); │ │ │ │ │ +265 } │ │ │ │ │ +266}; │ │ │ │ │ +267 │ │ │ │ │ +269std::set _D_i_s_c_r_e_t_e_K_e_y_s_A_s_S_e_t(const DiscreteKeys &discreteKeys); │ │ │ │ │ +270 │ │ │ │ │ +271// traits │ │ │ │ │ +272template <> │ │ │ │ │ +_2_7_3struct _t_r_a_i_t_s<_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ +274 │ │ │ │ │ +275} // namespace gtsam │ │ │ │ │ +_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_._h │ │ │ │ │ +_D_e_c_i_s_i_o_n_T_r_e_e_._h │ │ │ │ │ +Decision Tree for use in DiscreteFactors. │ │ │ │ │ +_D_i_s_c_r_e_t_e_K_e_y_._h │ │ │ │ │ +specialized key for discrete variables │ │ │ │ │ +_H_y_b_r_i_d_F_a_c_t_o_r_._h │ │ │ │ │ +_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r_._h │ │ │ │ │ +A set of GaussianFactors, indexed by a set of discrete keys. │ │ │ │ │ +_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ +Base class for conditional densities. │ │ │ │ │ +_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ +Conditional Gaussian Base class. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s_A_s_S_e_t │ │ │ │ │ +std::set< DiscreteKey > DiscreteKeysAsSet(const DiscreteKeys &discreteKeys) │ │ │ │ │ +Return the DiscreteKey vector as a set. │ │ │ │ │ +DDeeffiinniittiioonn GaussianMixture.cpp:221 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_u_n_e_r_F_u_n_c │ │ │ │ │ +std::function< double(const Assignment< Key > &, double)> prunerFunc(const │ │ │ │ │ +DecisionTreeFactor &prunedDecisionTree, const HybridConditional &conditional) │ │ │ │ │ +Helper function to get the pruner functional. │ │ │ │ │ +DDeeffiinniittiioonn HybridBayesNet.cpp:66 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ _g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ A helper that implements the traits interface for GTSAM types. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e │ │ │ │ │ -A Bayes tree representing a Hybrid density. │ │ │ │ │ -DDeeffiinniittiioonn HybridBayesTree.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -DDeeffiinniittiioonn HybridGaussianFactorGraph.h:102 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M │ │ │ │ │ -DDeeffiinniittiioonn HybridGaussianISAM.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M │ │ │ │ │ -A Bayes tree with an update methods that implements the iSAM algorithm. │ │ │ │ │ -DDeeffiinniittiioonn ISAM.h:31 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e │ │ │ │ │ +An algebraic decision tree fixes the range of a DecisionTree to double. │ │ │ │ │ +DDeeffiinniittiioonn AlgebraicDecisionTree.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_A_s_s_i_g_n_m_e_n_t │ │ │ │ │ +An assignment from labels to value index (size_t). │ │ │ │ │ +DDeeffiinniittiioonn Assignment.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_<_ _K_e_y_,_ _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r_ _> │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r │ │ │ │ │ +A discrete probabilistic factor. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTreeFactor.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ +DiscreteKeys is a set of keys that can be assembled using the & operator. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteKey.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ +A map from keys to values. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteValues.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e │ │ │ │ │ +A conditional of gaussian mixtures indexed by discrete variables, as part of a │ │ │ │ │ +Bayes Network. │ │ │ │ │ +DDeeffiinniittiioonn GaussianMixture.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_:_:_l_o_g_N_o_r_m_a_l_i_z_a_t_i_o_n_C_o_n_s_t_a_n_t │ │ │ │ │ +double logNormalizationConstant() const override │ │ │ │ │ +The log normalization constant is max of the the individual log-normalization │ │ │ │ │ +constants. │ │ │ │ │ +DDeeffiinniittiioonn GaussianMixture.h:161 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ +double operator()(const HybridValues &values) const │ │ │ │ │ +Evaluate probability density, sugar. │ │ │ │ │ +DDeeffiinniittiioonn GaussianMixture.h:231 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_:_:_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e │ │ │ │ │ +GaussianMixture()=default │ │ │ │ │ +Default constructor, mainly for serialization. │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r │ │ │ │ │ +Base class for truly hybrid probabilistic factors. │ │ │ │ │ +DDeeffiinniittiioonn HybridFactor.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn GaussianConditional.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ +with a unique integer... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +the error. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M_._h │ │ │ │ │ + * _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00539.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridSmoother.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridNonlinearISAM.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,42 +96,39 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
HybridSmoother.h File Reference
│ │ │ │ +
HybridNonlinearISAM.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

An incremental smoother for hybrid factor graphs. │ │ │ │ -More...

│ │ │ │ - │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::HybridSmoother
class  gtsam::HybridNonlinearISAM
 Wrapper class to manage ISAM in a nonlinear context. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

An incremental smoother for hybrid factor graphs.

│ │ │ │ +
Date
Sep 12, 2022
│ │ │ │
Author
Varun Agrawal
│ │ │ │ -
Date
October 2022
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,26 +1,25 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -HybridSmoother.h File Reference │ │ │ │ │ -An incremental smoother for hybrid factor graphs. _M_o_r_e_._._. │ │ │ │ │ +HybridNonlinearISAM.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_H_y_b_r_i_d_S_m_o_o_t_h_e_r │ │ │ │ │ +class   _g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M │ │ │ │ │ +  Wrapper class to manage _I_S_A_M in a nonlinear context. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -An incremental smoother for hybrid factor graphs. │ │ │ │ │ + Date │ │ │ │ │ + Sep 12, 2022 │ │ │ │ │ Author │ │ │ │ │ Varun Agrawal │ │ │ │ │ - Date │ │ │ │ │ - October 2022 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_S_m_o_o_t_h_e_r_._h │ │ │ │ │ + * _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00539.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a00539 = [ │ │ │ │ │ - ["gtsam::HybridSmoother", "a03500.html", "a03500"] │ │ │ │ │ + ["gtsam::HybridNonlinearISAM", "a03496.html", "a03496"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00539_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridSmoother.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridNonlinearISAM.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,73 +98,139 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
HybridSmoother.h
│ │ │ │ +
HybridNonlinearISAM.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#include <gtsam/discrete/DiscreteFactorGraph.h>
│ │ │ │ -
20#include <gtsam/hybrid/HybridBayesNet.h>
│ │ │ │ -
21#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/hybrid/HybridGaussianISAM.h>
│ │ │ │ +
21#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
│ │ │ │
22
│ │ │ │
23namespace gtsam {
│ │ │ │ -
24
│ │ │ │ -
│ │ │ │ -
25class HybridSmoother {
│ │ │ │ -
26 private:
│ │ │ │ -
27 HybridBayesNet hybridBayesNet_;
│ │ │ │ -
28 HybridGaussianFactorGraph remainingFactorGraph_;
│ │ │ │ -
29
│ │ │ │ -
30 public:
│ │ │ │ -
50 void update(HybridGaussianFactorGraph graph, const Ordering& ordering,
│ │ │ │ -
51 boost::optional<size_t> maxNrLeaves = boost::none);
│ │ │ │ -
52
│ │ │ │ -
53 Ordering getOrdering(const HybridGaussianFactorGraph& newFactors);
│ │ │ │ -
54
│ │ │ │ -
64 std::pair<HybridGaussianFactorGraph, HybridBayesNet> addConditionals(
│ │ │ │ -
65 const HybridGaussianFactorGraph& graph,
│ │ │ │ -
66 const HybridBayesNet& hybridBayesNet, const Ordering& ordering) const;
│ │ │ │ -
67
│ │ │ │ -
69 GaussianMixture::shared_ptr gaussianMixture(size_t index) const;
│ │ │ │ +
│ │ │ │ +
27class GTSAM_EXPORT HybridNonlinearISAM {
│ │ │ │ +
28 protected:
│ │ │ │ +
30 gtsam::HybridGaussianISAM isam_;
│ │ │ │ +
31
│ │ │ │ +
33 Values linPoint_;
│ │ │ │ +
34
│ │ │ │ +
36 DiscreteValues assignment_;
│ │ │ │ +
37
│ │ │ │ +
39 HybridNonlinearFactorGraph factors_;
│ │ │ │ +
40
│ │ │ │ +
42 int reorderInterval_;
│ │ │ │ +
43 int reorderCounter_;
│ │ │ │ +
44
│ │ │ │ +
46 HybridGaussianFactorGraph::Eliminate eliminationFunction_;
│ │ │ │ +
47
│ │ │ │ +
48 public:
│ │ │ │ +
51
│ │ │ │ +
│ │ │ │ +
59 HybridNonlinearISAM(
│ │ │ │ +
60 int reorderInterval = 1,
│ │ │ │ +
61 const HybridGaussianFactorGraph::Eliminate& eliminationFunction =
│ │ │ │ +
62 HybridGaussianFactorGraph::EliminationTraitsType::DefaultEliminate)
│ │ │ │ +
63 : reorderInterval_(reorderInterval),
│ │ │ │ +
64 reorderCounter_(0),
│ │ │ │ +
65 eliminationFunction_(eliminationFunction) {}
│ │ │ │ +
│ │ │ │ +
66
│ │ │ │
70
│ │ │ │ -
72 const HybridBayesNet& hybridBayesNet() const;
│ │ │ │ -
73};
│ │ │ │ +
72 Values estimate();
│ │ │ │ +
73
│ │ │ │ +
74 // /** find the marginal covariance for a single variable */
│ │ │ │ +
75 // Matrix marginalCovariance(Key key) const;
│ │ │ │ +
76
│ │ │ │ +
77 // access
│ │ │ │ +
78
│ │ │ │ +
80 const HybridGaussianISAM& bayesTree() const { return isam_; }
│ │ │ │ +
81
│ │ │ │ +
87 void prune(const size_t maxNumberLeaves) { isam_.prune(maxNumberLeaves); }
│ │ │ │ +
88
│ │ │ │ +
90 const Values& getLinearizationPoint() const { return linPoint_; }
│ │ │ │ +
91
│ │ │ │ +
93 const DiscreteValues& assignment() const { return assignment_; }
│ │ │ │ +
94
│ │ │ │ +
│ │ │ │ +
96 const HybridNonlinearFactorGraph& getFactorsUnsafe() const {
│ │ │ │ +
97 return factors_;
│ │ │ │ +
98 }
│ │ │ │ +
│ │ │ │ +
99
│ │ │ │ +
101 int reorderInterval() const { return reorderInterval_; }
│ │ │ │ +
│ │ │ │ +
102 int reorderCounter() const { return reorderCounter_; }
│ │ │ │ +
103
│ │ │ │ +
105 void print(const std::string& s = "",
│ │ │ │ +
106 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ +
107
│ │ │ │ +
109 void printStats() const;
│ │ │ │ +
110
│ │ │ │ +
112 void saveGraph(const std::string& s,
│ │ │ │ +
113 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ +
114
│ │ │ │ +
118
│ │ │ │ +
120 void update(const HybridNonlinearFactorGraph& newFactors,
│ │ │ │ +
121 const Values& initialValues,
│ │ │ │ +
122 const boost::optional<size_t>& maxNrLeaves = boost::none,
│ │ │ │ +
123 const boost::optional<Ordering>& ordering = boost::none);
│ │ │ │ +
124
│ │ │ │ +
126 void reorder_relinearize();
│ │ │ │ +
127
│ │ │ │ +
129};
│ │ │ │ +
130
│ │ │ │ +
131} // namespace gtsam
│ │ │ │ +
│ │ │ │
│ │ │ │ -
74
│ │ │ │ -
75}; // namespace gtsam
│ │ │ │ - │ │ │ │ -
HybridBayesNet.h
A Bayes net of Gaussian Conditionals indexed by discrete keys.
│ │ │ │ -
HybridGaussianFactorGraph.h
Linearized Hybrid factor graph that uses type erasure.
│ │ │ │ + │ │ │ │ +
HybridNonlinearFactorGraph.h
Nonlinear hybrid factor graph that uses type erasure.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::HybridBayesNet
A hybrid Bayes net is a collection of HybridConditionals, which can have discrete conditionals,...
Definition HybridBayesNet.h:35
│ │ │ │ -
gtsam::HybridGaussianFactorGraph
Definition HybridGaussianFactorGraph.h:102
│ │ │ │ -
gtsam::HybridSmoother
Definition HybridSmoother.h:25
│ │ │ │ -
gtsam::HybridSmoother::gaussianMixture
GaussianMixture::shared_ptr gaussianMixture(size_t index) const
Get the Gaussian Mixture from the Bayes Net posterior at index.
Definition HybridSmoother.cpp:131
│ │ │ │ -
gtsam::HybridSmoother::update
void update(HybridGaussianFactorGraph graph, const Ordering &ordering, boost::optional< size_t > maxNrLeaves=boost::none)
Given new factors, perform an incremental update.
Definition HybridSmoother.cpp:58
│ │ │ │ -
gtsam::HybridSmoother::addConditionals
std::pair< HybridGaussianFactorGraph, HybridBayesNet > addConditionals(const HybridGaussianFactorGraph &graph, const HybridBayesNet &hybridBayesNet, const Ordering &ordering) const
Add conditionals from previous timestep as part of liquefication.
Definition HybridSmoother.cpp:85
│ │ │ │ -
gtsam::HybridSmoother::hybridBayesNet
const HybridBayesNet & hybridBayesNet() const
Return the Bayes Net posterior.
Definition HybridSmoother.cpp:137
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::DiscreteValues
A map from keys to values.
Definition DiscreteValues.h:34
│ │ │ │ +
gtsam::HybridBayesTree::prune
void prune(const size_t maxNumberLeaves)
Prune the underlying Bayes tree.
Definition HybridBayesTree.cpp:175
│ │ │ │ +
gtsam::HybridGaussianISAM
Definition HybridGaussianISAM.h:34
│ │ │ │ +
gtsam::HybridNonlinearFactorGraph
Definition HybridNonlinearFactorGraph.h:33
│ │ │ │ +
gtsam::HybridNonlinearISAM
Wrapper class to manage ISAM in a nonlinear context.
Definition HybridNonlinearISAM.h:27
│ │ │ │ +
gtsam::HybridNonlinearISAM::getFactorsUnsafe
const HybridNonlinearFactorGraph & getFactorsUnsafe() const
get underlying nonlinear graph
Definition HybridNonlinearISAM.h:96
│ │ │ │ +
gtsam::HybridNonlinearISAM::assignment_
DiscreteValues assignment_
The discrete assignment.
Definition HybridNonlinearISAM.h:36
│ │ │ │ +
gtsam::HybridNonlinearISAM::getLinearizationPoint
const Values & getLinearizationPoint() const
Return the current linearization point.
Definition HybridNonlinearISAM.h:90
│ │ │ │ +
gtsam::HybridNonlinearISAM::HybridNonlinearISAM
HybridNonlinearISAM(int reorderInterval=1, const HybridGaussianFactorGraph::Eliminate &eliminationFunction=HybridGaussianFactorGraph::EliminationTraitsType::DefaultEliminate)
Periodically reorder and relinearize.
Definition HybridNonlinearISAM.h:59
│ │ │ │ +
gtsam::HybridNonlinearISAM::factors_
HybridNonlinearFactorGraph factors_
The original factors, used when relinearizing.
Definition HybridNonlinearISAM.h:39
│ │ │ │ +
gtsam::HybridNonlinearISAM::assignment
const DiscreteValues & assignment() const
Return the current discrete assignment.
Definition HybridNonlinearISAM.h:93
│ │ │ │ +
gtsam::HybridNonlinearISAM::reorderCounter
int reorderCounter() const
TODO: comment.
Definition HybridNonlinearISAM.h:102
│ │ │ │ +
gtsam::HybridNonlinearISAM::prune
void prune(const size_t maxNumberLeaves)
Prune the underlying Bayes tree.
Definition HybridNonlinearISAM.h:87
│ │ │ │ +
gtsam::HybridNonlinearISAM::reorderInterval_
int reorderInterval_
The reordering interval and counter.
Definition HybridNonlinearISAM.h:42
│ │ │ │ +
gtsam::HybridNonlinearISAM::reorderInterval
int reorderInterval() const
get counters
Definition HybridNonlinearISAM.h:101
│ │ │ │ +
gtsam::HybridNonlinearISAM::linPoint_
Values linPoint_
The current linearization point.
Definition HybridNonlinearISAM.h:33
│ │ │ │ +
gtsam::HybridNonlinearISAM::isam_
gtsam::HybridGaussianISAM isam_
The internal iSAM object.
Definition HybridNonlinearISAM.h:30
│ │ │ │ +
gtsam::HybridNonlinearISAM::eliminationFunction_
HybridGaussianFactorGraph::Eliminate eliminationFunction_
The elimination function.
Definition HybridNonlinearISAM.h:46
│ │ │ │ +
gtsam::HybridNonlinearISAM::bayesTree
const HybridGaussianISAM & bayesTree() const
access the underlying bayes tree
Definition HybridNonlinearISAM.h:80
│ │ │ │ +
gtsam::EliminateableFactorGraph< HybridGaussianFactorGraph >::Eliminate
std::function< EliminationResult(const FactorGraphType &, const Ordering &)> Eliminate
The function type that does a single dense elimination step on a subgraph.
Definition EliminateableFactorGraph.h:89
│ │ │ │ +
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,88 +1,191 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -HybridSmoother.h │ │ │ │ │ +HybridNonlinearISAM.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -20#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ 22 │ │ │ │ │ 23namespace _g_t_s_a_m { │ │ │ │ │ -24 │ │ │ │ │ -_2_5class _H_y_b_r_i_d_S_m_o_o_t_h_e_r { │ │ │ │ │ -26 private: │ │ │ │ │ -27 _H_y_b_r_i_d_B_a_y_e_s_N_e_t hybridBayesNet_; │ │ │ │ │ -28 _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h remainingFactorGraph_; │ │ │ │ │ -29 │ │ │ │ │ -30 public: │ │ │ │ │ -50 void _u_p_d_a_t_e(_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h graph, const _O_r_d_e_r_i_n_g& ordering, │ │ │ │ │ -51 boost::optional maxNrLeaves = boost::none); │ │ │ │ │ -52 │ │ │ │ │ -53 _O_r_d_e_r_i_n_g getOrdering(const _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& newFactors); │ │ │ │ │ -54 │ │ │ │ │ -64 std::pair _a_d_d_C_o_n_d_i_t_i_o_n_a_l_s( │ │ │ │ │ -65 const _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& graph, │ │ │ │ │ -66 const _H_y_b_r_i_d_B_a_y_e_s_N_e_t& _h_y_b_r_i_d_B_a_y_e_s_N_e_t, const _O_r_d_e_r_i_n_g& ordering) const; │ │ │ │ │ -67 │ │ │ │ │ -69 GaussianMixture::shared_ptr _g_a_u_s_s_i_a_n_M_i_x_t_u_r_e(size_t index) const; │ │ │ │ │ +_2_7class GTSAM_EXPORT _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M { │ │ │ │ │ +28 protected: │ │ │ │ │ +_3_0 _g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M _i_s_a_m__; │ │ │ │ │ +31 │ │ │ │ │ +_3_3 _V_a_l_u_e_s _l_i_n_P_o_i_n_t__; │ │ │ │ │ +34 │ │ │ │ │ +_3_6 _D_i_s_c_r_e_t_e_V_a_l_u_e_s _a_s_s_i_g_n_m_e_n_t__; │ │ │ │ │ +37 │ │ │ │ │ +_3_9 _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h _f_a_c_t_o_r_s__; │ │ │ │ │ +40 │ │ │ │ │ +_4_2 int _r_e_o_r_d_e_r_I_n_t_e_r_v_a_l__; │ │ │ │ │ +43 int reorderCounter_; │ │ │ │ │ +44 │ │ │ │ │ +_4_6 _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_E_l_i_m_i_n_a_t_e _e_l_i_m_i_n_a_t_i_o_n_F_u_n_c_t_i_o_n__; │ │ │ │ │ +47 │ │ │ │ │ +48 public: │ │ │ │ │ +51 │ │ │ │ │ +_5_9 _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M( │ │ │ │ │ +60 int reorderInterval = 1, │ │ │ │ │ +61 const _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_E_l_i_m_i_n_a_t_e& eliminationFunction = │ │ │ │ │ +62 HybridGaussianFactorGraph::EliminationTraitsType::DefaultEliminate) │ │ │ │ │ +63 : reorderInterval_(reorderInterval), │ │ │ │ │ +64 reorderCounter_(0), │ │ │ │ │ +65 eliminationFunction_(eliminationFunction) {} │ │ │ │ │ +66 │ │ │ │ │ 70 │ │ │ │ │ -72 const _H_y_b_r_i_d_B_a_y_e_s_N_e_t& _h_y_b_r_i_d_B_a_y_e_s_N_e_t() const; │ │ │ │ │ -73}; │ │ │ │ │ -74 │ │ │ │ │ -75}; // namespace gtsam │ │ │ │ │ -_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -_H_y_b_r_i_d_B_a_y_e_s_N_e_t_._h │ │ │ │ │ -A Bayes net of Gaussian Conditionals indexed by discrete keys. │ │ │ │ │ -_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Linearized Hybrid factor graph that uses type erasure. │ │ │ │ │ +72 _V_a_l_u_e_s estimate(); │ │ │ │ │ +73 │ │ │ │ │ +74 // /** find the marginal covariance for a single variable */ │ │ │ │ │ +75 // Matrix marginalCovariance(Key key) const; │ │ │ │ │ +76 │ │ │ │ │ +77 // access │ │ │ │ │ +78 │ │ │ │ │ +_8_0 const _H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M& _b_a_y_e_s_T_r_e_e() const { return isam_; } │ │ │ │ │ +81 │ │ │ │ │ +_8_7 void _p_r_u_n_e(const size_t maxNumberLeaves) { isam_._p_r_u_n_e(maxNumberLeaves); } │ │ │ │ │ +88 │ │ │ │ │ +_9_0 const _V_a_l_u_e_s& _g_e_t_L_i_n_e_a_r_i_z_a_t_i_o_n_P_o_i_n_t() const { return linPoint_; } │ │ │ │ │ +91 │ │ │ │ │ +_9_3 const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& _a_s_s_i_g_n_m_e_n_t() const { return assignment_; } │ │ │ │ │ +94 │ │ │ │ │ +_9_6 const _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& _g_e_t_F_a_c_t_o_r_s_U_n_s_a_f_e() const { │ │ │ │ │ +97 return factors_; │ │ │ │ │ +98 } │ │ │ │ │ +99 │ │ │ │ │ +_1_0_1 int _r_e_o_r_d_e_r_I_n_t_e_r_v_a_l() const { return reorderInterval_; } │ │ │ │ │ +_1_0_2 int _r_e_o_r_d_e_r_C_o_u_n_t_e_r() const { return reorderCounter_; } │ │ │ │ │ +103 │ │ │ │ │ +105 void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ +106 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const; │ │ │ │ │ +107 │ │ │ │ │ +109 void printStats() const; │ │ │ │ │ +110 │ │ │ │ │ +112 void saveGraph(const std::string& s, │ │ │ │ │ +113 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const; │ │ │ │ │ +114 │ │ │ │ │ +118 │ │ │ │ │ +120 void update(const _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& newFactors, │ │ │ │ │ +121 const _V_a_l_u_e_s& initialValues, │ │ │ │ │ +122 const boost::optional& maxNrLeaves = boost::none, │ │ │ │ │ +123 const boost::optional& ordering = boost::none); │ │ │ │ │ +124 │ │ │ │ │ +126 void reorder_relinearize(); │ │ │ │ │ +127 │ │ │ │ │ +129}; │ │ │ │ │ +130 │ │ │ │ │ +131} // namespace gtsam │ │ │ │ │ +_H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M_._h │ │ │ │ │ +_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Nonlinear hybrid factor graph that uses type erasure. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_N_e_t │ │ │ │ │ -A hybrid Bayes net is a collection of HybridConditionals, which can have │ │ │ │ │ -discrete conditionals,... │ │ │ │ │ -DDeeffiinniittiioonn HybridBayesNet.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -DDeeffiinniittiioonn HybridGaussianFactorGraph.h:102 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_S_m_o_o_t_h_e_r │ │ │ │ │ -DDeeffiinniittiioonn HybridSmoother.h:25 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_S_m_o_o_t_h_e_r_:_:_g_a_u_s_s_i_a_n_M_i_x_t_u_r_e │ │ │ │ │ -GaussianMixture::shared_ptr gaussianMixture(size_t index) const │ │ │ │ │ -Get the Gaussian Mixture from the Bayes Net posterior at index. │ │ │ │ │ -DDeeffiinniittiioonn HybridSmoother.cpp:131 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_S_m_o_o_t_h_e_r_:_:_u_p_d_a_t_e │ │ │ │ │ -void update(HybridGaussianFactorGraph graph, const Ordering &ordering, boost:: │ │ │ │ │ -optional< size_t > maxNrLeaves=boost::none) │ │ │ │ │ -Given new factors, perform an incremental update. │ │ │ │ │ -DDeeffiinniittiioonn HybridSmoother.cpp:58 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_S_m_o_o_t_h_e_r_:_:_a_d_d_C_o_n_d_i_t_i_o_n_a_l_s │ │ │ │ │ -std::pair< HybridGaussianFactorGraph, HybridBayesNet > addConditionals(const │ │ │ │ │ -HybridGaussianFactorGraph &graph, const HybridBayesNet &hybridBayesNet, const │ │ │ │ │ -Ordering &ordering) const │ │ │ │ │ -Add conditionals from previous timestep as part of liquefication. │ │ │ │ │ -DDeeffiinniittiioonn HybridSmoother.cpp:85 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_S_m_o_o_t_h_e_r_:_:_h_y_b_r_i_d_B_a_y_e_s_N_e_t │ │ │ │ │ -const HybridBayesNet & hybridBayesNet() const │ │ │ │ │ -Return the Bayes Net posterior. │ │ │ │ │ -DDeeffiinniittiioonn HybridSmoother.cpp:137 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ +A map from keys to values. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteValues.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_:_:_p_r_u_n_e │ │ │ │ │ +void prune(const size_t maxNumberLeaves) │ │ │ │ │ +Prune the underlying Bayes tree. │ │ │ │ │ +DDeeffiinniittiioonn HybridBayesTree.cpp:175 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M │ │ │ │ │ +DDeeffiinniittiioonn HybridGaussianISAM.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +DDeeffiinniittiioonn HybridNonlinearFactorGraph.h:33 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M │ │ │ │ │ +Wrapper class to manage ISAM in a nonlinear context. │ │ │ │ │ +DDeeffiinniittiioonn HybridNonlinearISAM.h:27 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_g_e_t_F_a_c_t_o_r_s_U_n_s_a_f_e │ │ │ │ │ +const HybridNonlinearFactorGraph & getFactorsUnsafe() const │ │ │ │ │ +get underlying nonlinear graph │ │ │ │ │ +DDeeffiinniittiioonn HybridNonlinearISAM.h:96 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_a_s_s_i_g_n_m_e_n_t__ │ │ │ │ │ +DiscreteValues assignment_ │ │ │ │ │ +The discrete assignment. │ │ │ │ │ +DDeeffiinniittiioonn HybridNonlinearISAM.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_g_e_t_L_i_n_e_a_r_i_z_a_t_i_o_n_P_o_i_n_t │ │ │ │ │ +const Values & getLinearizationPoint() const │ │ │ │ │ +Return the current linearization point. │ │ │ │ │ +DDeeffiinniittiioonn HybridNonlinearISAM.h:90 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M │ │ │ │ │ +HybridNonlinearISAM(int reorderInterval=1, const HybridGaussianFactorGraph:: │ │ │ │ │ +Eliminate &eliminationFunction=HybridGaussianFactorGraph:: │ │ │ │ │ +EliminationTraitsType::DefaultEliminate) │ │ │ │ │ +Periodically reorder and relinearize. │ │ │ │ │ +DDeeffiinniittiioonn HybridNonlinearISAM.h:59 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_f_a_c_t_o_r_s__ │ │ │ │ │ +HybridNonlinearFactorGraph factors_ │ │ │ │ │ +The original factors, used when relinearizing. │ │ │ │ │ +DDeeffiinniittiioonn HybridNonlinearISAM.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_a_s_s_i_g_n_m_e_n_t │ │ │ │ │ +const DiscreteValues & assignment() const │ │ │ │ │ +Return the current discrete assignment. │ │ │ │ │ +DDeeffiinniittiioonn HybridNonlinearISAM.h:93 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_r_e_o_r_d_e_r_C_o_u_n_t_e_r │ │ │ │ │ +int reorderCounter() const │ │ │ │ │ +TODO: comment. │ │ │ │ │ +DDeeffiinniittiioonn HybridNonlinearISAM.h:102 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_p_r_u_n_e │ │ │ │ │ +void prune(const size_t maxNumberLeaves) │ │ │ │ │ +Prune the underlying Bayes tree. │ │ │ │ │ +DDeeffiinniittiioonn HybridNonlinearISAM.h:87 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_r_e_o_r_d_e_r_I_n_t_e_r_v_a_l__ │ │ │ │ │ +int reorderInterval_ │ │ │ │ │ +The reordering interval and counter. │ │ │ │ │ +DDeeffiinniittiioonn HybridNonlinearISAM.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_r_e_o_r_d_e_r_I_n_t_e_r_v_a_l │ │ │ │ │ +int reorderInterval() const │ │ │ │ │ +get counters │ │ │ │ │ +DDeeffiinniittiioonn HybridNonlinearISAM.h:101 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_l_i_n_P_o_i_n_t__ │ │ │ │ │ +Values linPoint_ │ │ │ │ │ +The current linearization point. │ │ │ │ │ +DDeeffiinniittiioonn HybridNonlinearISAM.h:33 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_i_s_a_m__ │ │ │ │ │ +gtsam::HybridGaussianISAM isam_ │ │ │ │ │ +The internal iSAM object. │ │ │ │ │ +DDeeffiinniittiioonn HybridNonlinearISAM.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_e_l_i_m_i_n_a_t_i_o_n_F_u_n_c_t_i_o_n__ │ │ │ │ │ +HybridGaussianFactorGraph::Eliminate eliminationFunction_ │ │ │ │ │ +The elimination function. │ │ │ │ │ +DDeeffiinniittiioonn HybridNonlinearISAM.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_b_a_y_e_s_T_r_e_e │ │ │ │ │ +const HybridGaussianISAM & bayesTree() const │ │ │ │ │ +access the underlying bayes tree │ │ │ │ │ +DDeeffiinniittiioonn HybridNonlinearISAM.h:80 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_E_l_i_m_i_n_a_t_e │ │ │ │ │ +std::function< EliminationResult(const FactorGraphType &, const Ordering &)> │ │ │ │ │ +Eliminate │ │ │ │ │ +The function type that does a single dense elimination step on a subgraph. │ │ │ │ │ +DDeeffiinniittiioonn EliminateableFactorGraph.h:89 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_S_m_o_o_t_h_e_r_._h │ │ │ │ │ + * _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00542.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridEliminationTree.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridSmoother.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,41 +94,36 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
HybridEliminationTree.h File Reference
│ │ │ │ +
HybridSmoother.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

An incremental smoother for hybrid factor graphs. │ │ │ │ +More...

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::HybridEliminationTree
 Elimination Tree type for Hybrid Factor Graphs. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Mar 11, 2022
│ │ │ │ -
Author
Fan Jiang
│ │ │ │ +

An incremental smoother for hybrid factor graphs.

│ │ │ │ +
Author
Varun Agrawal
│ │ │ │ +
Date
October 2022
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,25 +1,22 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -HybridEliminationTree.h File Reference │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ -  Elimination Tree type for Hybrid _F_a_c_t_o_r Graphs. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +HybridSmoother.cpp File Reference │ │ │ │ │ +An incremental smoother for hybrid factor graphs. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Mar 11, 2022 │ │ │ │ │ +An incremental smoother for hybrid factor graphs. │ │ │ │ │ Author │ │ │ │ │ - Fan Jiang │ │ │ │ │ + Varun Agrawal │ │ │ │ │ + Date │ │ │ │ │ + October 2022 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h │ │ │ │ │ + * _H_y_b_r_i_d_S_m_o_o_t_h_e_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00548.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridGaussianFactorGraph.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridEliminationTree.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,56 +94,32 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
HybridGaussianFactorGraph.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
HybridEliminationTree.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ - │ │ │ │ -

Linearized Hybrid factor graph that uses type erasure. │ │ │ │ -More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

struct  gtsam::EliminationTraits< HybridGaussianFactorGraph >
 
class  gtsam::HybridGaussianFactorGraph
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

std::pair< HybridConditional::shared_ptr, boost::shared_ptr< Factor > > gtsam::EliminateHybrid (const HybridGaussianFactorGraph &factors, const Ordering &keys)
 Main elimination function for HybridGaussianFactorGraph.
 
const Ordering gtsam::HybridOrdering (const HybridGaussianFactorGraph &graph)
 Return a Colamd constrained ordering where the discrete keys are eliminated after the continuous keys.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Linearized Hybrid factor graph that uses type erasure.

│ │ │ │ -
Author
Fan Jiang, Varun Agrawal, Frank Dellaert
│ │ │ │ -
Date
Mar 11, 2022
│ │ │ │ +
Date
Mar 11, 2022
│ │ │ │ +
Author
Fan Jiang
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,41 +1,20 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -HybridGaussianFactorGraph.h File Reference │ │ │ │ │ -Linearized Hybrid factor graph that uses type erasure. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _> │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +HybridEliminationTree.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ - std::pair< _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_:_: _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_H_y_b_r_i_d (const │ │ │ │ │ -_s_h_a_r_e_d___p_t_r, boost::shared_ptr< _F_a_c_t_o_r > _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &factors, │ │ │ │ │ - >  const _O_r_d_e_r_i_n_g &keys) │ │ │ │ │ -  Main elimination function for │ │ │ │ │ - _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h. │ │ │ │ │ -  │ │ │ │ │ - const _O_r_d_e_r_i_n_g  _g_t_s_a_m_:_:_H_y_b_r_i_d_O_r_d_e_r_i_n_g (const │ │ │ │ │ - _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &graph) │ │ │ │ │ - Return a Colamd constrained ordering │ │ │ │ │ -  where the discrete keys are eliminated │ │ │ │ │ - after the continuous keys. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Linearized Hybrid factor graph that uses type erasure. │ │ │ │ │ - Author │ │ │ │ │ - Fan Jiang, Varun Agrawal, Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ Mar 11, 2022 │ │ │ │ │ + Author │ │ │ │ │ + Fan Jiang │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ + * _H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00551.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/GaussianMixture.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/MixtureFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,48 +94,47 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
GaussianMixture.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
MixtureFactor.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

A hybrid conditional in the Conditional Linear Gaussian scheme. │ │ │ │ +

Nonlinear Mixture factor of continuous and discrete. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::MixtureFactor
 Implementation of a discrete conditional mixture factor. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -std::set< DiscreteKey > gtsam::DiscreteKeysAsSet (const DiscreteKeys &discreteKeys)
 Return the DiscreteKey vector as a set.
 
│ │ │ │

Detailed Description

│ │ │ │ -

A hybrid conditional in the Conditional Linear Gaussian scheme.

│ │ │ │ -
Author
Fan Jiang
│ │ │ │ -
│ │ │ │ -Varun Agrawal
│ │ │ │ +

Nonlinear Mixture factor of continuous and discrete.

│ │ │ │ +
Author
Kevin Doherty, kdohe.nosp@m.rty@.nosp@m.mit.e.nosp@m.du
│ │ │ │
│ │ │ │ -Frank Dellaert
│ │ │ │ -
Date
Mar 12, 2022
│ │ │ │ +Varun Agrawal
│ │ │ │ +
Date
December 2021
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,29 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -GaussianMixture.cpp File Reference │ │ │ │ │ -A hybrid conditional in the Conditional Linear Gaussian scheme. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +MixtureFactor.h File Reference │ │ │ │ │ +Nonlinear Mixture factor of continuous and discrete. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_M_i_x_t_u_r_e_F_a_c_t_o_r │ │ │ │ │ +  Implementation of a discrete conditional mixture factor. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -std::set< _D_i_s_c_r_e_t_e_K_e_y >  ggttssaamm::::DDiissccrreetteeKKeeyyssAAssSSeett (const _D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ - &discreteKeys) │ │ │ │ │ -  Return the DiscreteKey vector as a set. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A hybrid conditional in the Conditional Linear Gaussian scheme. │ │ │ │ │ +Nonlinear Mixture factor of continuous and discrete. │ │ │ │ │ Author │ │ │ │ │ - Fan Jiang │ │ │ │ │ + Kevin Doherty, _k_d_o_h_e_._n_o_s_p_@_m_._r_t_y_@_._n_o_s_p_@_m_._m_i_t_._e_._n_o_s_p_@_m_._d_u │ │ │ │ │ Varun Agrawal │ │ │ │ │ - Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - Mar 12, 2022 │ │ │ │ │ + December 2021 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_._c_p_p │ │ │ │ │ + * _M_i_x_t_u_r_e_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00551.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a00551 = [ │ │ │ │ │ - ["DiscreteKeysAsSet", "a00551.html#ad92a1966f54e02a7c2ce061c1228c7b3", null] │ │ │ │ │ + ["gtsam::MixtureFactor", "a03512.html", "a03512"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00554.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridNonlinearFactorGraph.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridGaussianFactorGraph.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,45 +95,55 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
HybridNonlinearFactorGraph.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
HybridGaussianFactorGraph.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Nonlinear hybrid factor graph that uses type erasure. │ │ │ │ +

Linearized Hybrid factor graph that uses type erasure. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::HybridNonlinearFactorGraph
struct  gtsam::EliminationTraits< HybridGaussianFactorGraph >
 
struct  gtsam::traits< HybridNonlinearFactorGraph >
class  gtsam::HybridGaussianFactorGraph
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

std::pair< HybridConditional::shared_ptr, boost::shared_ptr< Factor > > gtsam::EliminateHybrid (const HybridGaussianFactorGraph &factors, const Ordering &keys)
 Main elimination function for HybridGaussianFactorGraph.
 
const Ordering gtsam::HybridOrdering (const HybridGaussianFactorGraph &graph)
 Return a Colamd constrained ordering where the discrete keys are eliminated after the continuous keys.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Nonlinear hybrid factor graph that uses type erasure.

│ │ │ │ -
Author
Varun Agrawal
│ │ │ │ -
Date
May 28, 2022
│ │ │ │ +

Linearized Hybrid factor graph that uses type erasure.

│ │ │ │ +
Author
Fan Jiang, Varun Agrawal, Frank Dellaert
│ │ │ │ +
Date
Mar 11, 2022
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,41 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -HybridNonlinearFactorGraph.h File Reference │ │ │ │ │ -Nonlinear hybrid factor graph that uses type erasure. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +HybridGaussianFactorGraph.h File Reference │ │ │ │ │ +Linearized Hybrid factor graph that uses type erasure. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +struct   _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _> │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_ _> │ │ │ │ │ + class   _g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ + std::pair< _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_:_: _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_H_y_b_r_i_d (const │ │ │ │ │ +_s_h_a_r_e_d___p_t_r, boost::shared_ptr< _F_a_c_t_o_r > _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &factors, │ │ │ │ │ + >  const _O_r_d_e_r_i_n_g &keys) │ │ │ │ │ +  Main elimination function for │ │ │ │ │ + _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h. │ │ │ │ │ +  │ │ │ │ │ + const _O_r_d_e_r_i_n_g  _g_t_s_a_m_:_:_H_y_b_r_i_d_O_r_d_e_r_i_n_g (const │ │ │ │ │ + _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &graph) │ │ │ │ │ + Return a Colamd constrained ordering │ │ │ │ │ +  where the discrete keys are eliminated │ │ │ │ │ + after the continuous keys. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Nonlinear hybrid factor graph that uses type erasure. │ │ │ │ │ +Linearized Hybrid factor graph that uses type erasure. │ │ │ │ │ Author │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + Fan Jiang, Varun Agrawal, Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - May 28, 2022 │ │ │ │ │ + Mar 11, 2022 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ + * _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00554.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,5 @@ │ │ │ │ │ var a00554 = [ │ │ │ │ │ - ["gtsam::HybridNonlinearFactorGraph", "a03488.html", "a03488"], │ │ │ │ │ - ["gtsam::traits< HybridNonlinearFactorGraph >", "a03492.html", null] │ │ │ │ │ + ["gtsam::EliminationTraits< HybridGaussianFactorGraph >", "a03460.html", "a03460"], │ │ │ │ │ + ["EliminateHybrid", "a00554.html#gacbc9f97e48e6aa3abb5f789af8c3bfe5", null], │ │ │ │ │ + ["HybridOrdering", "a00554.html#a584e4e05d52a617c1d313e1f53c8ce58", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00554_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridNonlinearFactorGraph.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridGaussianFactorGraph.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,15 +98,15 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
HybridNonlinearFactorGraph.h
│ │ │ │ +
HybridGaussianFactorGraph.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │ @@ -114,75 +114,176 @@ │ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/hybrid/HybridFactorGraph.h>
│ │ │ │ -
22
│ │ │ │ -
23namespace gtsam {
│ │ │ │ -
24
│ │ │ │ -
25class HybridGaussianFactorGraph;
│ │ │ │ -
26
│ │ │ │ -
│ │ │ │ -
33class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph {
│ │ │ │ -
34 protected:
│ │ │ │ -
35 public:
│ │ │ │ -
36 using Base = HybridFactorGraph;
│ │ │ │ -
37 using This = HybridNonlinearFactorGraph;
│ │ │ │ -
38 using shared_ptr = boost::shared_ptr<This>;
│ │ │ │ -
39
│ │ │ │ -
40 using Values = gtsam::Values;
│ │ │ │ -
41 using Indices = KeyVector;
│ │ │ │ +
21#include <gtsam/hybrid/GaussianMixtureFactor.h>
│ │ │ │ +
22#include <gtsam/hybrid/HybridFactor.h>
│ │ │ │ +
23#include <gtsam/hybrid/HybridFactorGraph.h>
│ │ │ │ +
24#include <gtsam/inference/EliminateableFactorGraph.h>
│ │ │ │ +
25#include <gtsam/inference/FactorGraph.h>
│ │ │ │ +
26#include <gtsam/inference/Ordering.h>
│ │ │ │ +
27#include <gtsam/linear/GaussianFactor.h>
│ │ │ │ +
28#include <gtsam/linear/VectorValues.h>
│ │ │ │ +
29
│ │ │ │ +
30namespace gtsam {
│ │ │ │ +
31
│ │ │ │ +
32// Forward declarations
│ │ │ │ +
33class HybridGaussianFactorGraph;
│ │ │ │ +
34class HybridConditional;
│ │ │ │ +
35class HybridBayesNet;
│ │ │ │ +
36class HybridEliminationTree;
│ │ │ │ +
37class HybridBayesTree;
│ │ │ │ +
38class HybridJunctionTree;
│ │ │ │ +
39class DecisionTreeFactor;
│ │ │ │ +
40class JacobianFactor;
│ │ │ │ +
41class HybridValues;
│ │ │ │
42
│ │ │ │ -
45
│ │ │ │ -
46 HybridNonlinearFactorGraph() = default;
│ │ │ │ -
47
│ │ │ │ -
53 template <class DERIVEDFACTOR>
│ │ │ │ -
│ │ │ │ -
54 HybridNonlinearFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph)
│ │ │ │ -
55 : Base(graph) {}
│ │ │ │ -
│ │ │ │ -
56
│ │ │ │ -
60
│ │ │ │ -
62 void print(
│ │ │ │ -
63 const std::string& s = "HybridNonlinearFactorGraph",
│ │ │ │ -
64 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
│ │ │ │ -
65
│ │ │ │ -
69
│ │ │ │ -
77 boost::shared_ptr<HybridGaussianFactorGraph> linearize(
│ │ │ │ -
78 const Values& continuousValues) const;
│ │ │ │ -
80};
│ │ │ │ -
│ │ │ │ -
81
│ │ │ │ -
82template <>
│ │ │ │ -
│ │ │ │ -
83struct traits<HybridNonlinearFactorGraph>
│ │ │ │ -
84 : public Testable<HybridNonlinearFactorGraph> {};
│ │ │ │ -
│ │ │ │ -
85
│ │ │ │ -
86} // namespace gtsam
│ │ │ │ -
HybridFactorGraph.h
Factor graph with utilities for hybrid factors.
│ │ │ │ +
51GTSAM_EXPORT
│ │ │ │ +
52std::pair<boost::shared_ptr<HybridConditional>, boost::shared_ptr<Factor>>
│ │ │ │ +
53EliminateHybrid(const HybridGaussianFactorGraph& factors, const Ordering& keys);
│ │ │ │ +
54
│ │ │ │ +
61GTSAM_EXPORT const Ordering
│ │ │ │ +
62HybridOrdering(const HybridGaussianFactorGraph& graph);
│ │ │ │ +
63
│ │ │ │ +
64/* ************************************************************************* */
│ │ │ │ +
65template <>
│ │ │ │ +
│ │ │ │ +
66struct EliminationTraits<HybridGaussianFactorGraph> {
│ │ │ │ +
67 typedef Factor FactorType;
│ │ │ │ +
68 typedef HybridGaussianFactorGraph
│ │ │ │ +
69 FactorGraphType;
│ │ │ │ +
71 typedef HybridConditional
│ │ │ │ +
72 ConditionalType;
│ │ │ │ +
73 typedef HybridBayesNet
│ │ │ │ +
74 BayesNetType;
│ │ │ │ +
75 typedef HybridEliminationTree
│ │ │ │ +
76 EliminationTreeType;
│ │ │ │ +
77 typedef HybridBayesTree BayesTreeType;
│ │ │ │ +
78 typedef HybridJunctionTree JunctionTreeType;
│ │ │ │ +
80 static std::pair<boost::shared_ptr<ConditionalType>,
│ │ │ │ +
81 boost::shared_ptr<FactorType>>
│ │ │ │ +
│ │ │ │ +
82 DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) {
│ │ │ │ +
83 return EliminateHybrid(factors, keys);
│ │ │ │ +
84 }
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
86 static Ordering DefaultOrderingFunc(
│ │ │ │ +
87 const FactorGraphType& graph,
│ │ │ │ +
88 boost::optional<const VariableIndex&> variableIndex) {
│ │ │ │ +
89 return HybridOrdering(graph);
│ │ │ │ +
90 }
│ │ │ │ +
│ │ │ │ +
91};
│ │ │ │ +
│ │ │ │ +
92
│ │ │ │ +
│ │ │ │ +
100class GTSAM_EXPORT HybridGaussianFactorGraph
│ │ │ │ +
101 : public HybridFactorGraph,
│ │ │ │ +
102 public EliminateableFactorGraph<HybridGaussianFactorGraph> {
│ │ │ │ +
103 protected:
│ │ │ │ +
105 template <typename FACTOR>
│ │ │ │ +
106 using IsGaussian = typename std::enable_if<
│ │ │ │ +
107 std::is_base_of<GaussianFactor, FACTOR>::value>::type;
│ │ │ │ +
108
│ │ │ │ +
109 public:
│ │ │ │ +
110 using Base = HybridFactorGraph;
│ │ │ │ +
111 using This = HybridGaussianFactorGraph;
│ │ │ │ +
112 using BaseEliminateable =
│ │ │ │ +
113 EliminateableFactorGraph<This>;
│ │ │ │ +
114 using shared_ptr = boost::shared_ptr<This>;
│ │ │ │ +
115
│ │ │ │ +
116 using Values = gtsam::Values;
│ │ │ │ +
117 using Indices = KeyVector;
│ │ │ │ +
118
│ │ │ │ +
121
│ │ │ │ +
123 HybridGaussianFactorGraph() = default;
│ │ │ │ +
124
│ │ │ │ +
130 template <class DERIVEDFACTOR>
│ │ │ │ +
│ │ │ │ +
131 HybridGaussianFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph)
│ │ │ │ +
132 : Base(graph) {}
│ │ │ │ +
│ │ │ │ +
133
│ │ │ │ +
137
│ │ │ │ +
138 // TODO(dellaert): customize print and equals.
│ │ │ │ +
139 // void print(const std::string& s = "HybridGaussianFactorGraph",
│ │ │ │ +
140 // const KeyFormatter& keyFormatter = DefaultKeyFormatter) const
│ │ │ │ +
141 // override;
│ │ │ │ +
142 // bool equals(const This& fg, double tol = 1e-9) const override;
│ │ │ │ +
143
│ │ │ │ +
147
│ │ │ │ +
148 using Base::error; // Expose error(const HybridValues&) method..
│ │ │ │ +
149
│ │ │ │ +
159 AlgebraicDecisionTree<Key> error(const VectorValues& continuousValues) const;
│ │ │ │ +
160
│ │ │ │ +
169 AlgebraicDecisionTree<Key> probPrime(
│ │ │ │ +
170 const VectorValues& continuousValues) const;
│ │ │ │ +
171
│ │ │ │ +
178 double probPrime(const HybridValues& values) const;
│ │ │ │ +
179
│ │ │ │ +
189 GaussianFactorGraphTree assembleGraphTree() const;
│ │ │ │ +
190
│ │ │ │ +
192};
│ │ │ │ +
│ │ │ │ +
193
│ │ │ │ +
194} // namespace gtsam
│ │ │ │ + │ │ │ │ +
GaussianMixtureFactor.h
A set of GaussianFactors, indexed by a set of discrete keys.
│ │ │ │ +
HybridFactorGraph.h
Factor graph with utilities for hybrid factors.
│ │ │ │ +
Ordering.h
Variable ordering for the elimination algorithm.
│ │ │ │ +
FactorGraph.h
Factor Graph Base Class.
│ │ │ │ +
EliminateableFactorGraph.h
Variable elimination algorithms for factor graphs.
│ │ │ │ +
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
│ │ │ │ +
VectorValues.h
Factor Graph Values.
│ │ │ │ +
gtsam::EliminateHybrid
std::pair< HybridConditional::shared_ptr, boost::shared_ptr< Factor > > EliminateHybrid(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys)
Main elimination function for HybridGaussianFactorGraph.
Definition HybridGaussianFactorGraph.cpp:320
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::HybridOrdering
const Ordering HybridOrdering(const HybridGaussianFactorGraph &graph)
Return a Colamd constrained ordering where the discrete keys are eliminated after the continuous keys...
Definition HybridGaussianFactorGraph.cpp:72
│ │ │ │ +
gtsam::AlgebraicDecisionTree
An algebraic decision tree fixes the range of a DecisionTree to double.
Definition AlgebraicDecisionTree.h:38
│ │ │ │ +
gtsam::DecisionTree
a decision tree is a function from assignments to values.
Definition DecisionTree.h:61
│ │ │ │ +
gtsam::HybridBayesNet
A hybrid Bayes net is a collection of HybridConditionals, which can have discrete conditionals,...
Definition HybridBayesNet.h:35
│ │ │ │ +
gtsam::HybridBayesTree
A Bayes tree representing a Hybrid density.
Definition HybridBayesTree.h:64
│ │ │ │ +
gtsam::HybridConditional
Hybrid Conditional Density.
Definition HybridConditional.h:62
│ │ │ │ +
gtsam::HybridEliminationTree
Elimination Tree type for Hybrid Factor Graphs.
Definition HybridEliminationTree.h:32
│ │ │ │
gtsam::HybridFactorGraph
Hybrid Factor Graph Factor graph with utilities for hybrid factors.
Definition HybridFactorGraph.h:39
│ │ │ │ -
gtsam::HybridNonlinearFactorGraph
Definition HybridNonlinearFactorGraph.h:33
│ │ │ │ -
gtsam::HybridNonlinearFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to This
Definition HybridNonlinearFactorGraph.h:38
│ │ │ │ -
gtsam::HybridNonlinearFactorGraph::HybridNonlinearFactorGraph
HybridNonlinearFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
Implicit copy/downcast constructor to override explicit template container constructor.
Definition HybridNonlinearFactorGraph.h:54
│ │ │ │ +
gtsam::EliminationTraits< HybridGaussianFactorGraph >::DefaultEliminate
static std::pair< boost::shared_ptr< ConditionalType >, boost::shared_ptr< FactorType > > DefaultEliminate(const FactorGraphType &factors, const Ordering &keys)
The default dense elimination function.
Definition HybridGaussianFactorGraph.h:82
│ │ │ │ +
gtsam::EliminationTraits< HybridGaussianFactorGraph >::ConditionalType
HybridConditional ConditionalType
Type of conditionals from elimination.
Definition HybridGaussianFactorGraph.h:72
│ │ │ │ +
gtsam::EliminationTraits< HybridGaussianFactorGraph >::FactorGraphType
HybridGaussianFactorGraph FactorGraphType
Type of the factor graph (e.g.
Definition HybridGaussianFactorGraph.h:69
│ │ │ │ +
gtsam::EliminationTraits< HybridGaussianFactorGraph >::JunctionTreeType
HybridJunctionTree JunctionTreeType
Type of Junction tree.
Definition HybridGaussianFactorGraph.h:78
│ │ │ │ +
gtsam::EliminationTraits< HybridGaussianFactorGraph >::EliminationTreeType
HybridEliminationTree EliminationTreeType
Type of elimination tree.
Definition HybridGaussianFactorGraph.h:76
│ │ │ │ +
gtsam::EliminationTraits< HybridGaussianFactorGraph >::DefaultOrderingFunc
static Ordering DefaultOrderingFunc(const FactorGraphType &graph, boost::optional< const VariableIndex & > variableIndex)
The default ordering generation function.
Definition HybridGaussianFactorGraph.h:86
│ │ │ │ +
gtsam::EliminationTraits< HybridGaussianFactorGraph >::FactorType
Factor FactorType
Type of factors in factor graph.
Definition HybridGaussianFactorGraph.h:67
│ │ │ │ +
gtsam::EliminationTraits< HybridGaussianFactorGraph >::BayesTreeType
HybridBayesTree BayesTreeType
Type of Bayes tree.
Definition HybridGaussianFactorGraph.h:77
│ │ │ │ +
gtsam::EliminationTraits< HybridGaussianFactorGraph >::BayesNetType
HybridBayesNet BayesNetType
Type of Bayes net from sequential elimination.
Definition HybridGaussianFactorGraph.h:74
│ │ │ │ +
gtsam::HybridGaussianFactorGraph
Definition HybridGaussianFactorGraph.h:102
│ │ │ │ +
gtsam::HybridGaussianFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to This
Definition HybridGaussianFactorGraph.h:114
│ │ │ │ +
gtsam::HybridGaussianFactorGraph::IsGaussian
typename std::enable_if< std::is_base_of< GaussianFactor, FACTOR >::value >::type IsGaussian
Check if FACTOR type is derived from GaussianFactor.
Definition HybridGaussianFactorGraph.h:107
│ │ │ │ +
gtsam::HybridGaussianFactorGraph::Indices
KeyVector Indices
map from keys to values
Definition HybridGaussianFactorGraph.h:117
│ │ │ │ +
gtsam::HybridGaussianFactorGraph::BaseEliminateable
EliminateableFactorGraph< This > BaseEliminateable
for elimination
Definition HybridGaussianFactorGraph.h:113
│ │ │ │ +
gtsam::HybridGaussianFactorGraph::HybridGaussianFactorGraph
HybridGaussianFactorGraph()=default
Default constructor.
│ │ │ │ +
gtsam::HybridGaussianFactorGraph::HybridGaussianFactorGraph
HybridGaussianFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
Implicit copy/downcast constructor to override explicit template container constructor.
Definition HybridGaussianFactorGraph.h:131
│ │ │ │ +
gtsam::HybridJunctionTree
An EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree,...
Definition HybridJunctionTree.h:53
│ │ │ │ +
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ +
gtsam::EliminationTraits
Traits class for eliminateable factor graphs, specifies the types that result from elimination,...
Definition EliminateableFactorGraph.h:36
│ │ │ │ +
gtsam::EliminateableFactorGraph
EliminateableFactorGraph is a base class for factor graphs that contains elimination algorithms.
Definition EliminateableFactorGraph.h:57
│ │ │ │ +
gtsam::Factor
Definition Factor.h:68
│ │ │ │ +
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
HybridValues
the error.
│ │ │ │ +
JacobianFactor
In Gaussian factors, the error function returns either the negative log-likelihood,...
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -HybridNonlinearFactorGraph.h │ │ │ │ │ +HybridGaussianFactorGraph.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ @@ -16,94 +16,272 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 19#pragma once │ │ │ │ │ 20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -22 │ │ │ │ │ -23namespace _g_t_s_a_m { │ │ │ │ │ -24 │ │ │ │ │ -25class HybridGaussianFactorGraph; │ │ │ │ │ -26 │ │ │ │ │ -_3_3class GTSAM_EXPORT _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h : public _H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h { │ │ │ │ │ -34 protected: │ │ │ │ │ -35 public: │ │ │ │ │ -36 using _B_a_s_e = _H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h; │ │ │ │ │ -_3_7 using _T_h_i_s = _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h; │ │ │ │ │ -_3_8 using _s_h_a_r_e_d___p_t_r = boost::shared_ptr; │ │ │ │ │ -39 │ │ │ │ │ -_4_0 using _V_a_l_u_e_s = _g_t_s_a_m_:_:_V_a_l_u_e_s; │ │ │ │ │ -41 using Indices = _K_e_y_V_e_c_t_o_r; │ │ │ │ │ +21#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_F_a_c_t_o_r_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_O_r_d_e_r_i_n_g_._h> │ │ │ │ │ +27#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ +28#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ +29 │ │ │ │ │ +30namespace _g_t_s_a_m { │ │ │ │ │ +31 │ │ │ │ │ +32// Forward declarations │ │ │ │ │ +33class HybridGaussianFactorGraph; │ │ │ │ │ +34class HybridConditional; │ │ │ │ │ +35class HybridBayesNet; │ │ │ │ │ +36class HybridEliminationTree; │ │ │ │ │ +37class HybridBayesTree; │ │ │ │ │ +38class HybridJunctionTree; │ │ │ │ │ +39class DecisionTreeFactor; │ │ │ │ │ +40class _J_a_c_o_b_i_a_n_F_a_c_t_o_r; │ │ │ │ │ +41class _H_y_b_r_i_d_V_a_l_u_e_s; │ │ │ │ │ 42 │ │ │ │ │ -45 │ │ │ │ │ -46 _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h() = default; │ │ │ │ │ -47 │ │ │ │ │ -53 template │ │ │ │ │ -_5_4 _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h(const _F_a_c_t_o_r_G_r_a_p_h_<_D_E_R_I_V_E_D_F_A_C_T_O_R_>& graph) │ │ │ │ │ -55 : _B_a_s_e(graph) {} │ │ │ │ │ -56 │ │ │ │ │ -60 │ │ │ │ │ -62 void _p_r_i_n_t( │ │ │ │ │ -63 const std::string& s = "HybridNonlinearFactorGraph", │ │ │ │ │ -64 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override; │ │ │ │ │ -65 │ │ │ │ │ -69 │ │ │ │ │ -77 boost::shared_ptr linearize( │ │ │ │ │ -78 const _V_a_l_u_e_s& continuousValues) const; │ │ │ │ │ -80}; │ │ │ │ │ -81 │ │ │ │ │ -82template <> │ │ │ │ │ -_8_3struct _t_r_a_i_t_s<_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h> │ │ │ │ │ -84 : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ -85 │ │ │ │ │ -86} // namespace gtsam │ │ │ │ │ +51GTSAM_EXPORT │ │ │ │ │ +52std::pair, boost::shared_ptr> │ │ │ │ │ +53_E_l_i_m_i_n_a_t_e_H_y_b_r_i_d(const HybridGaussianFactorGraph& factors, const Ordering& │ │ │ │ │ +keys); │ │ │ │ │ +54 │ │ │ │ │ +61GTSAM_EXPORT const Ordering │ │ │ │ │ +62_H_y_b_r_i_d_O_r_d_e_r_i_n_g(const HybridGaussianFactorGraph& graph); │ │ │ │ │ +63 │ │ │ │ │ +64/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +65template <> │ │ │ │ │ +_6_6struct _E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s<_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h> { │ │ │ │ │ +_6_7 typedef _F_a_c_t_o_r _F_a_c_t_o_r_T_y_p_e; │ │ │ │ │ +68 typedef _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +_6_9 _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e; │ │ │ │ │ +71 typedef _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +_7_2 _C_o_n_d_i_t_i_o_n_a_l_T_y_p_e; │ │ │ │ │ +73 typedef _H_y_b_r_i_d_B_a_y_e_s_N_e_t │ │ │ │ │ +_7_4 _B_a_y_e_s_N_e_t_T_y_p_e; │ │ │ │ │ +75 typedef _H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ +_7_6 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_T_y_p_e; │ │ │ │ │ +_7_7 typedef _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e _B_a_y_e_s_T_r_e_e_T_y_p_e; │ │ │ │ │ +_7_8 typedef _H_y_b_r_i_d_J_u_n_c_t_i_o_n_T_r_e_e _J_u_n_c_t_i_o_n_T_r_e_e_T_y_p_e; │ │ │ │ │ +80 static std::pair, │ │ │ │ │ +81 boost::shared_ptr> │ │ │ │ │ +_8_2 _D_e_f_a_u_l_t_E_l_i_m_i_n_a_t_e(const _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e& factors, const _O_r_d_e_r_i_n_g& keys) { │ │ │ │ │ +83 return _E_l_i_m_i_n_a_t_e_H_y_b_r_i_d(factors, keys); │ │ │ │ │ +84 } │ │ │ │ │ +_8_6 static _O_r_d_e_r_i_n_g _D_e_f_a_u_l_t_O_r_d_e_r_i_n_g_F_u_n_c( │ │ │ │ │ +87 const _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e& graph, │ │ │ │ │ +88 boost::optional variableIndex) { │ │ │ │ │ +89 return _H_y_b_r_i_d_O_r_d_e_r_i_n_g(graph); │ │ │ │ │ +90 } │ │ │ │ │ +91}; │ │ │ │ │ +92 │ │ │ │ │ +_1_0_0class GTSAM_EXPORT _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +101 : public _H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h, │ │ │ │ │ +102 public _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h { │ │ │ │ │ +103 protected: │ │ │ │ │ +105 template │ │ │ │ │ +_1_0_6 using _I_s_G_a_u_s_s_i_a_n = typename std::enable_if< │ │ │ │ │ +107 std::is_base_of::value>::type; │ │ │ │ │ +108 │ │ │ │ │ +109 public: │ │ │ │ │ +110 using _B_a_s_e = _H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h; │ │ │ │ │ +_1_1_1 using _T_h_i_s = _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h; │ │ │ │ │ +_1_1_2 using _B_a_s_e_E_l_i_m_i_n_a_t_e_a_b_l_e = │ │ │ │ │ +113 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_T_h_i_s_>; │ │ │ │ │ +_1_1_4 using _s_h_a_r_e_d___p_t_r = boost::shared_ptr; │ │ │ │ │ +115 │ │ │ │ │ +_1_1_6 using _V_a_l_u_e_s = _g_t_s_a_m_:_:_V_a_l_u_e_s; │ │ │ │ │ +_1_1_7 using _I_n_d_i_c_e_s = _K_e_y_V_e_c_t_o_r; │ │ │ │ │ +118 │ │ │ │ │ +121 │ │ │ │ │ +_1_2_3 _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h() = default; │ │ │ │ │ +124 │ │ │ │ │ +130 template │ │ │ │ │ +_1_3_1 _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h(const _F_a_c_t_o_r_G_r_a_p_h_<_D_E_R_I_V_E_D_F_A_C_T_O_R_>& graph) │ │ │ │ │ +132 : _B_a_s_e(graph) {} │ │ │ │ │ +133 │ │ │ │ │ +137 │ │ │ │ │ +138 // TODO(dellaert): customize print and equals. │ │ │ │ │ +139 // void print(const std::string& s = "HybridGaussianFactorGraph", │ │ │ │ │ +140 // const KeyFormatter& keyFormatter = DefaultKeyFormatter) const │ │ │ │ │ +141 // override; │ │ │ │ │ +142 // bool equals(const This& fg, double tol = 1e-9) const override; │ │ │ │ │ +143 │ │ │ │ │ +147 │ │ │ │ │ +148 using Base::error; // Expose error(const HybridValues&) method.. │ │ │ │ │ +149 │ │ │ │ │ +159 _A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_<_K_e_y_> error(const _V_e_c_t_o_r_V_a_l_u_e_s& continuousValues) │ │ │ │ │ +const; │ │ │ │ │ +160 │ │ │ │ │ +169 _A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_<_K_e_y_> probPrime( │ │ │ │ │ +170 const _V_e_c_t_o_r_V_a_l_u_e_s& continuousValues) const; │ │ │ │ │ +171 │ │ │ │ │ +178 double probPrime(const _H_y_b_r_i_d_V_a_l_u_e_s& values) const; │ │ │ │ │ +179 │ │ │ │ │ +189 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_T_r_e_e assembleGraphTree() const; │ │ │ │ │ +190 │ │ │ │ │ +192}; │ │ │ │ │ +193 │ │ │ │ │ +194} // namespace gtsam │ │ │ │ │ +_H_y_b_r_i_d_F_a_c_t_o_r_._h │ │ │ │ │ +_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r_._h │ │ │ │ │ +A set of GaussianFactors, indexed by a set of discrete keys. │ │ │ │ │ _H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ Factor graph with utilities for hybrid factors. │ │ │ │ │ +_O_r_d_e_r_i_n_g_._h │ │ │ │ │ +Variable ordering for the elimination algorithm. │ │ │ │ │ +_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Factor Graph Base Class. │ │ │ │ │ +_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Variable elimination algorithms for factor graphs. │ │ │ │ │ +_G_a_u_s_s_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ +A factor with a quadratic error function - a Gaussian. │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ +Factor Graph Values. │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_H_y_b_r_i_d │ │ │ │ │ +std::pair< HybridConditional::shared_ptr, boost::shared_ptr< Factor > > │ │ │ │ │ +EliminateHybrid(const HybridGaussianFactorGraph &factors, const Ordering │ │ │ │ │ +&frontalKeys) │ │ │ │ │ +Main elimination function for HybridGaussianFactorGraph. │ │ │ │ │ +DDeeffiinniittiioonn HybridGaussianFactorGraph.cpp:320 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ FastVector< Key > KeyVector │ │ │ │ │ Define collection type once and for all - also used in wrappers. │ │ │ │ │ DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_O_r_d_e_r_i_n_g │ │ │ │ │ +const Ordering HybridOrdering(const HybridGaussianFactorGraph &graph) │ │ │ │ │ +Return a Colamd constrained ordering where the discrete keys are eliminated │ │ │ │ │ +after the continuous keys... │ │ │ │ │ +DDeeffiinniittiioonn HybridGaussianFactorGraph.cpp:72 │ │ │ │ │ +_g_t_s_a_m_:_:_A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e │ │ │ │ │ +An algebraic decision tree fixes the range of a DecisionTree to double. │ │ │ │ │ +DDeeffiinniittiioonn AlgebraicDecisionTree.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e │ │ │ │ │ +a decision tree is a function from assignments to values. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree.h:61 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_N_e_t │ │ │ │ │ +A hybrid Bayes net is a collection of HybridConditionals, which can have │ │ │ │ │ +discrete conditionals,... │ │ │ │ │ +DDeeffiinniittiioonn HybridBayesNet.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e │ │ │ │ │ +A Bayes tree representing a Hybrid density. │ │ │ │ │ +DDeeffiinniittiioonn HybridBayesTree.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +Hybrid Conditional Density. │ │ │ │ │ +DDeeffiinniittiioonn HybridConditional.h:62 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ +Elimination Tree type for Hybrid Factor Graphs. │ │ │ │ │ +DDeeffiinniittiioonn HybridEliminationTree.h:32 │ │ │ │ │ _g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ Hybrid Factor Graph Factor graph with utilities for hybrid factors. │ │ │ │ │ DDeeffiinniittiioonn HybridFactorGraph.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -DDeeffiinniittiioonn HybridNonlinearFactorGraph.h:33 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_D_e_f_a_u_l_t_E_l_i_m_i_n_a_t_e │ │ │ │ │ +static std::pair< boost::shared_ptr< ConditionalType >, boost::shared_ptr< │ │ │ │ │ +FactorType > > DefaultEliminate(const FactorGraphType &factors, const Ordering │ │ │ │ │ +&keys) │ │ │ │ │ +The default dense elimination function. │ │ │ │ │ +DDeeffiinniittiioonn HybridGaussianFactorGraph.h:82 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_C_o_n_d_i_t_i_o_n_a_l_T_y_p_e │ │ │ │ │ +HybridConditional ConditionalType │ │ │ │ │ +Type of conditionals from elimination. │ │ │ │ │ +DDeeffiinniittiioonn HybridGaussianFactorGraph.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_F_a_c_t_o_r_G_r_a_p_h_T_y_p_e │ │ │ │ │ +HybridGaussianFactorGraph FactorGraphType │ │ │ │ │ +Type of the factor graph (e.g. │ │ │ │ │ +DDeeffiinniittiioonn HybridGaussianFactorGraph.h:69 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_J_u_n_c_t_i_o_n_T_r_e_e_T_y_p_e │ │ │ │ │ +HybridJunctionTree JunctionTreeType │ │ │ │ │ +Type of Junction tree. │ │ │ │ │ +DDeeffiinniittiioonn HybridGaussianFactorGraph.h:78 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_T_y_p_e │ │ │ │ │ +HybridEliminationTree EliminationTreeType │ │ │ │ │ +Type of elimination tree. │ │ │ │ │ +DDeeffiinniittiioonn HybridGaussianFactorGraph.h:76 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_D_e_f_a_u_l_t_O_r_d_e_r_i_n_g_F_u_n_c │ │ │ │ │ +static Ordering DefaultOrderingFunc(const FactorGraphType &graph, boost:: │ │ │ │ │ +optional< const VariableIndex & > variableIndex) │ │ │ │ │ +The default ordering generation function. │ │ │ │ │ +DDeeffiinniittiioonn HybridGaussianFactorGraph.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_F_a_c_t_o_r_T_y_p_e │ │ │ │ │ +Factor FactorType │ │ │ │ │ +Type of factors in factor graph. │ │ │ │ │ +DDeeffiinniittiioonn HybridGaussianFactorGraph.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_B_a_y_e_s_T_r_e_e_T_y_p_e │ │ │ │ │ +HybridBayesTree BayesTreeType │ │ │ │ │ +Type of Bayes tree. │ │ │ │ │ +DDeeffiinniittiioonn HybridGaussianFactorGraph.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_B_a_y_e_s_N_e_t_T_y_p_e │ │ │ │ │ +HybridBayesNet BayesNetType │ │ │ │ │ +Type of Bayes net from sequential elimination. │ │ │ │ │ +DDeeffiinniittiioonn HybridGaussianFactorGraph.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +DDeeffiinniittiioonn HybridGaussianFactorGraph.h:102 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ boost::shared_ptr< This > shared_ptr │ │ │ │ │ shared_ptr to This │ │ │ │ │ -DDeeffiinniittiioonn HybridNonlinearFactorGraph.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -HybridNonlinearFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph) │ │ │ │ │ +DDeeffiinniittiioonn HybridGaussianFactorGraph.h:114 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_I_s_G_a_u_s_s_i_a_n │ │ │ │ │ +typename std::enable_if< std::is_base_of< GaussianFactor, FACTOR >::value >:: │ │ │ │ │ +type IsGaussian │ │ │ │ │ +Check if FACTOR type is derived from GaussianFactor. │ │ │ │ │ +DDeeffiinniittiioonn HybridGaussianFactorGraph.h:107 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_I_n_d_i_c_e_s │ │ │ │ │ +KeyVector Indices │ │ │ │ │ +map from keys to values │ │ │ │ │ +DDeeffiinniittiioonn HybridGaussianFactorGraph.h:117 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_B_a_s_e_E_l_i_m_i_n_a_t_e_a_b_l_e │ │ │ │ │ +EliminateableFactorGraph< This > BaseEliminateable │ │ │ │ │ +for elimination │ │ │ │ │ +DDeeffiinniittiioonn HybridGaussianFactorGraph.h:113 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +HybridGaussianFactorGraph()=default │ │ │ │ │ +Default constructor. │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +HybridGaussianFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph) │ │ │ │ │ Implicit copy/downcast constructor to override explicit template container │ │ │ │ │ constructor. │ │ │ │ │ -DDeeffiinniittiioonn HybridNonlinearFactorGraph.h:54 │ │ │ │ │ +DDeeffiinniittiioonn HybridGaussianFactorGraph.h:131 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ +An EliminatableClusterTree, i.e., a set of variable clusters with factors, │ │ │ │ │ +arranged in a tree,... │ │ │ │ │ +DDeeffiinniittiioonn HybridJunctionTree.h:53 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ _g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ nodes. │ │ │ │ │ DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s │ │ │ │ │ +Traits class for eliminateable factor graphs, specifies the types that result │ │ │ │ │ +from elimination,... │ │ │ │ │ +DDeeffiinniittiioonn EliminateableFactorGraph.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +EliminateableFactorGraph is a base class for factor graphs that contains │ │ │ │ │ +elimination algorithms. │ │ │ │ │ +DDeeffiinniittiioonn EliminateableFactorGraph.h:57 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:68 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ +with a unique integer... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ _g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +the error. │ │ │ │ │ +_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ +In Gaussian factors, the error function returns either the negative log- │ │ │ │ │ +likelihood,... │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ + * _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00557.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridNonlinearFactorGraph.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,63 +95,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
│ │ │ │ -
HybridFactor.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
HybridNonlinearFactorGraph.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ +

Nonlinear hybrid factor graph that uses type erasure. │ │ │ │ +More...

│ │ │ │ + │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::HybridFactor
 Base class for truly hybrid probabilistic factors. More...
class  gtsam::HybridNonlinearFactorGraph
 
struct  gtsam::traits< HybridFactor >
struct  gtsam::traits< HybridNonlinearFactorGraph >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Typedefs

│ │ │ │ -using gtsam::GaussianFactorGraphTree = DecisionTree< Key, GaussianFactorGraph >
 Alias for DecisionTree of GaussianFactorGraphs.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -KeyVector gtsam::CollectKeys (const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys)
 
│ │ │ │ -KeyVector gtsam::CollectKeys (const KeyVector &keys1, const KeyVector &keys2)
 
│ │ │ │ -DiscreteKeys gtsam::CollectDiscreteKeys (const DiscreteKeys &key1, const DiscreteKeys &key2)
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Mar 11, 2022
│ │ │ │ -
Author
Fan Jiang
│ │ │ │ +

Nonlinear hybrid factor graph that uses type erasure.

│ │ │ │ +
Author
Varun Agrawal
│ │ │ │ +
Date
May 28, 2022
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,42 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -HybridFactor.h File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +HybridNonlinearFactorGraph.h File Reference │ │ │ │ │ +Nonlinear hybrid factor graph that uses type erasure. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r │ │ │ │ │ -  Base class for ttrruullyy hybrid probabilistic factors. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _H_y_b_r_i_d_F_a_c_t_o_r_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -using  ggttssaamm::::GGaauussssiiaannFFaaccttoorrGGrraapphhTTrreeee = _D_e_c_i_s_i_o_n_T_r_e_e< _K_e_y, _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ - > │ │ │ │ │ -  Alias for _D_e_c_i_s_i_o_n_T_r_e_e of GaussianFactorGraphs. │ │ │ │ │ -  │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ - _K_e_y_V_e_c_t_o_r  ggttssaamm::::CCoolllleeccttKKeeyyss (const _K_e_y_V_e_c_t_o_r &continuousKeys, const │ │ │ │ │ - _D_i_s_c_r_e_t_e_K_e_y_s &discreteKeys) │ │ │ │ │ -  │ │ │ │ │ - _K_e_y_V_e_c_t_o_r  ggttssaamm::::CCoolllleeccttKKeeyyss (const _K_e_y_V_e_c_t_o_r &keys1, const _K_e_y_V_e_c_t_o_r │ │ │ │ │ - &keys2) │ │ │ │ │ -  │ │ │ │ │ -_D_i_s_c_r_e_t_e_K_e_y_s  ggttssaamm::::CCoolllleeccttDDiissccrreetteeKKeeyyss (const _D_i_s_c_r_e_t_e_K_e_y_s &key1, const │ │ │ │ │ - _D_i_s_c_r_e_t_e_K_e_y_s &key2) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Mar 11, 2022 │ │ │ │ │ +Nonlinear hybrid factor graph that uses type erasure. │ │ │ │ │ Author │ │ │ │ │ - Fan Jiang │ │ │ │ │ + Varun Agrawal │ │ │ │ │ + Date │ │ │ │ │ + May 28, 2022 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_F_a_c_t_o_r_._h │ │ │ │ │ + * _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00557.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a00557 = [ │ │ │ │ │ - ["gtsam::traits< HybridFactor >", "a03452.html", null], │ │ │ │ │ - ["GaussianFactorGraphTree", "a00557.html#a8b4a8e0d32b81b0fa2fa15c437b89e7c", null] │ │ │ │ │ + ["gtsam::HybridNonlinearFactorGraph", "a03488.html", "a03488"], │ │ │ │ │ + ["gtsam::traits< HybridNonlinearFactorGraph >", "a03492.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00557_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridNonlinearFactorGraph.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,157 +98,91 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
HybridFactor.h
│ │ │ │ +
HybridNonlinearFactorGraph.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/base/Testable.h>
│ │ │ │ -
21#include <gtsam/discrete/DecisionTree.h>
│ │ │ │ -
22#include <gtsam/discrete/DiscreteKey.h>
│ │ │ │ -
23#include <gtsam/inference/Factor.h>
│ │ │ │ -
24#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ -
25#include <gtsam/nonlinear/Values.h>
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/hybrid/HybridFactorGraph.h>
│ │ │ │ +
22
│ │ │ │ +
23namespace gtsam {
│ │ │ │ +
24
│ │ │ │ +
25class HybridGaussianFactorGraph;
│ │ │ │
26
│ │ │ │ -
27#include <cstddef>
│ │ │ │ -
28#include <string>
│ │ │ │ -
29namespace gtsam {
│ │ │ │ -
30
│ │ │ │ -
31class HybridValues;
│ │ │ │ -
32
│ │ │ │ -
34using GaussianFactorGraphTree = DecisionTree<Key, GaussianFactorGraph>;
│ │ │ │ -
35
│ │ │ │ -
36KeyVector CollectKeys(const KeyVector &continuousKeys,
│ │ │ │ -
37 const DiscreteKeys &discreteKeys);
│ │ │ │ -
38KeyVector CollectKeys(const KeyVector &keys1, const KeyVector &keys2);
│ │ │ │ -
39DiscreteKeys CollectDiscreteKeys(const DiscreteKeys &key1,
│ │ │ │ -
40 const DiscreteKeys &key2);
│ │ │ │ -
41
│ │ │ │ -
│ │ │ │ -
52class GTSAM_EXPORT HybridFactor : public Factor {
│ │ │ │ -
53 private:
│ │ │ │ -
54 bool isDiscrete_ = false;
│ │ │ │ -
55 bool isContinuous_ = false;
│ │ │ │ -
56 bool isHybrid_ = false;
│ │ │ │ -
57
│ │ │ │ -
58 protected:
│ │ │ │ -
59 // Set of DiscreteKeys for this factor.
│ │ │ │ -
60 DiscreteKeys discreteKeys_;
│ │ │ │ -
62 KeyVector continuousKeys_;
│ │ │ │ -
63
│ │ │ │ -
64 public:
│ │ │ │ -
65 // typedefs needed to play nice with gtsam
│ │ │ │ -
66 typedef HybridFactor This;
│ │ │ │ -
67 typedef boost::shared_ptr<HybridFactor>
│ │ │ │ -
68 shared_ptr;
│ │ │ │ -
69 typedef Factor Base;
│ │ │ │ -
70
│ │ │ │ -
73
│ │ │ │ -
75 HybridFactor() = default;
│ │ │ │ -
76
│ │ │ │ -
82 explicit HybridFactor(const KeyVector &keys);
│ │ │ │ -
83
│ │ │ │ -
89 explicit HybridFactor(const DiscreteKeys &discreteKeys);
│ │ │ │ -
90
│ │ │ │ -
97 HybridFactor(const KeyVector &continuousKeys,
│ │ │ │ -
98 const DiscreteKeys &discreteKeys);
│ │ │ │ -
99
│ │ │ │ -
101 virtual ~HybridFactor() = default;
│ │ │ │ -
102
│ │ │ │ -
106
│ │ │ │ -
108 virtual bool equals(const HybridFactor &lf, double tol = 1e-9) const;
│ │ │ │ -
109
│ │ │ │ -
111 void print(
│ │ │ │ -
112 const std::string &s = "HybridFactor\n",
│ │ │ │ -
113 const KeyFormatter &formatter = DefaultKeyFormatter) const override;
│ │ │ │ -
114
│ │ │ │ -
118
│ │ │ │ -
120 bool isDiscrete() const { return isDiscrete_; }
│ │ │ │ -
121
│ │ │ │ -
123 bool isContinuous() const { return isContinuous_; }
│ │ │ │ -
124
│ │ │ │ -
126 bool isHybrid() const { return isHybrid_; }
│ │ │ │ -
127
│ │ │ │ -
129 size_t nrContinuous() const { return continuousKeys_.size(); }
│ │ │ │ -
130
│ │ │ │ -
132 const DiscreteKeys &discreteKeys() const { return discreteKeys_; }
│ │ │ │ -
133
│ │ │ │ -
135 const KeyVector &continuousKeys() const { return continuousKeys_; }
│ │ │ │ -
136
│ │ │ │ -
138
│ │ │ │ -
139 private:
│ │ │ │ -
141 friend class boost::serialization::access;
│ │ │ │ -
142 template <class ARCHIVE>
│ │ │ │ -
143 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ -
144 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
145 ar &BOOST_SERIALIZATION_NVP(isDiscrete_);
│ │ │ │ -
146 ar &BOOST_SERIALIZATION_NVP(isContinuous_);
│ │ │ │ -
147 ar &BOOST_SERIALIZATION_NVP(isHybrid_);
│ │ │ │ -
148 ar &BOOST_SERIALIZATION_NVP(discreteKeys_);
│ │ │ │ -
149 ar &BOOST_SERIALIZATION_NVP(continuousKeys_);
│ │ │ │ -
150 }
│ │ │ │ -
151};
│ │ │ │ -
│ │ │ │ -
152// HybridFactor
│ │ │ │ -
153
│ │ │ │ -
154// traits
│ │ │ │ -
155template <>
│ │ │ │ -
156struct traits<HybridFactor> : public Testable<HybridFactor> {};
│ │ │ │ -
157
│ │ │ │ -
158} // namespace gtsam
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ -
DecisionTree.h
Decision Tree for use in DiscreteFactors.
│ │ │ │ -
DiscreteKey.h
specialized key for discrete variables
│ │ │ │ -
Factor.h
The base class for all factors.
│ │ │ │ -
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ -
Values.h
A non-templated config holding any types of Manifold-group elements.
│ │ │ │ +
│ │ │ │ +
33class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph {
│ │ │ │ +
34 protected:
│ │ │ │ +
35 public:
│ │ │ │ +
36 using Base = HybridFactorGraph;
│ │ │ │ +
37 using This = HybridNonlinearFactorGraph;
│ │ │ │ +
38 using shared_ptr = boost::shared_ptr<This>;
│ │ │ │ +
39
│ │ │ │ +
40 using Values = gtsam::Values;
│ │ │ │ +
41 using Indices = KeyVector;
│ │ │ │ +
42
│ │ │ │ +
45
│ │ │ │ +
46 HybridNonlinearFactorGraph() = default;
│ │ │ │ +
47
│ │ │ │ +
53 template <class DERIVEDFACTOR>
│ │ │ │ +
│ │ │ │ +
54 HybridNonlinearFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph)
│ │ │ │ +
55 : Base(graph) {}
│ │ │ │ +
│ │ │ │ +
56
│ │ │ │ +
60
│ │ │ │ +
62 void print(
│ │ │ │ +
63 const std::string& s = "HybridNonlinearFactorGraph",
│ │ │ │ +
64 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
│ │ │ │ +
65
│ │ │ │ +
69
│ │ │ │ +
77 boost::shared_ptr<HybridGaussianFactorGraph> linearize(
│ │ │ │ +
78 const Values& continuousValues) const;
│ │ │ │ +
80};
│ │ │ │ +
│ │ │ │ +
81
│ │ │ │ +
82template <>
│ │ │ │ +
│ │ │ │ +
83struct traits<HybridNonlinearFactorGraph>
│ │ │ │ +
84 : public Testable<HybridNonlinearFactorGraph> {};
│ │ │ │ +
│ │ │ │ +
85
│ │ │ │ +
86} // namespace gtsam
│ │ │ │ +
HybridFactorGraph.h
Factor graph with utilities for hybrid factors.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::DecisionTree
a decision tree is a function from assignments to values.
Definition DecisionTree.h:61
│ │ │ │ -
gtsam::DiscreteKeys
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition DiscreteKey.h:39
│ │ │ │ -
gtsam::HybridFactor
Base class for truly hybrid probabilistic factors.
Definition HybridFactor.h:52
│ │ │ │ -
gtsam::HybridFactor::nrContinuous
size_t nrContinuous() const
Return the number of continuous variables in this factor.
Definition HybridFactor.h:129
│ │ │ │ -
gtsam::HybridFactor::This
HybridFactor This
This class.
Definition HybridFactor.h:66
│ │ │ │ -
gtsam::HybridFactor::shared_ptr
boost::shared_ptr< HybridFactor > shared_ptr
shared_ptr to this class
Definition HybridFactor.h:68
│ │ │ │ -
gtsam::HybridFactor::isDiscrete
bool isDiscrete() const
True if this is a factor of discrete variables only.
Definition HybridFactor.h:120
│ │ │ │ -
gtsam::HybridFactor::isHybrid
bool isHybrid() const
True is this is a Discrete-Continuous factor.
Definition HybridFactor.h:126
│ │ │ │ -
gtsam::HybridFactor::HybridFactor
HybridFactor()=default
Default constructor creates empty factor.
│ │ │ │ -
gtsam::HybridFactor::Base
Factor Base
Our base class.
Definition HybridFactor.h:69
│ │ │ │ -
gtsam::HybridFactor::continuousKeys
const KeyVector & continuousKeys() const
Return only the continuous keys for this factor.
Definition HybridFactor.h:135
│ │ │ │ -
gtsam::HybridFactor::isContinuous
bool isContinuous() const
True if this is a factor of continuous variables only.
Definition HybridFactor.h:123
│ │ │ │ -
gtsam::HybridFactor::continuousKeys_
KeyVector continuousKeys_
Record continuous keys for book-keeping.
Definition HybridFactor.h:62
│ │ │ │ -
gtsam::HybridFactor::~HybridFactor
virtual ~HybridFactor()=default
Virtual destructor.
│ │ │ │ -
gtsam::HybridFactor::discreteKeys
const DiscreteKeys & discreteKeys() const
Return the discrete keys for this factor.
Definition HybridFactor.h:132
│ │ │ │ -
gtsam::Factor
Definition Factor.h:68
│ │ │ │ -
HybridValues
the error.
│ │ │ │ +
gtsam::HybridFactorGraph
Hybrid Factor Graph Factor graph with utilities for hybrid factors.
Definition HybridFactorGraph.h:39
│ │ │ │ +
gtsam::HybridNonlinearFactorGraph
Definition HybridNonlinearFactorGraph.h:33
│ │ │ │ +
gtsam::HybridNonlinearFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to This
Definition HybridNonlinearFactorGraph.h:38
│ │ │ │ +
gtsam::HybridNonlinearFactorGraph::HybridNonlinearFactorGraph
HybridNonlinearFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
Implicit copy/downcast constructor to override explicit template container constructor.
Definition HybridNonlinearFactorGraph.h:54
│ │ │ │ +
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ +
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,132 +1,69 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -HybridFactor.h │ │ │ │ │ +HybridNonlinearFactorGraph.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_e_c_i_s_i_o_n_T_r_e_e_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_K_e_y_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_V_a_l_u_e_s_._h> │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +22 │ │ │ │ │ +23namespace _g_t_s_a_m { │ │ │ │ │ +24 │ │ │ │ │ +25class HybridGaussianFactorGraph; │ │ │ │ │ 26 │ │ │ │ │ -27#include │ │ │ │ │ -28#include │ │ │ │ │ -29namespace _g_t_s_a_m { │ │ │ │ │ -30 │ │ │ │ │ -31class _H_y_b_r_i_d_V_a_l_u_e_s; │ │ │ │ │ -32 │ │ │ │ │ -_3_4using _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_T_r_e_e = _D_e_c_i_s_i_o_n_T_r_e_e_<_K_e_y_,_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_>; │ │ │ │ │ -35 │ │ │ │ │ -36_K_e_y_V_e_c_t_o_r CollectKeys(const _K_e_y_V_e_c_t_o_r &continuousKeys, │ │ │ │ │ -37 const _D_i_s_c_r_e_t_e_K_e_y_s &discreteKeys); │ │ │ │ │ -38_K_e_y_V_e_c_t_o_r CollectKeys(const _K_e_y_V_e_c_t_o_r &keys1, const _K_e_y_V_e_c_t_o_r &keys2); │ │ │ │ │ -39_D_i_s_c_r_e_t_e_K_e_y_s CollectDiscreteKeys(const _D_i_s_c_r_e_t_e_K_e_y_s &key1, │ │ │ │ │ -40 const _D_i_s_c_r_e_t_e_K_e_y_s &key2); │ │ │ │ │ -41 │ │ │ │ │ -_5_2class GTSAM_EXPORT _H_y_b_r_i_d_F_a_c_t_o_r : public _F_a_c_t_o_r { │ │ │ │ │ -53 private: │ │ │ │ │ -54 bool isDiscrete_ = false; │ │ │ │ │ -55 bool isContinuous_ = false; │ │ │ │ │ -56 bool isHybrid_ = false; │ │ │ │ │ -57 │ │ │ │ │ -58 protected: │ │ │ │ │ -59 // Set of DiscreteKeys for this factor. │ │ │ │ │ -60 _D_i_s_c_r_e_t_e_K_e_y_s discreteKeys_; │ │ │ │ │ -_6_2 _K_e_y_V_e_c_t_o_r _c_o_n_t_i_n_u_o_u_s_K_e_y_s__; │ │ │ │ │ -63 │ │ │ │ │ -64 public: │ │ │ │ │ -65 // typedefs needed to play nice with gtsam │ │ │ │ │ -_6_6 typedef _H_y_b_r_i_d_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ -67 typedef boost::shared_ptr │ │ │ │ │ -_6_8 _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -_6_9 typedef _F_a_c_t_o_r _B_a_s_e; │ │ │ │ │ -70 │ │ │ │ │ -73 │ │ │ │ │ -_7_5 _H_y_b_r_i_d_F_a_c_t_o_r() = default; │ │ │ │ │ -76 │ │ │ │ │ -82 explicit _H_y_b_r_i_d_F_a_c_t_o_r(const _K_e_y_V_e_c_t_o_r &keys); │ │ │ │ │ -83 │ │ │ │ │ -89 explicit _H_y_b_r_i_d_F_a_c_t_o_r(const _D_i_s_c_r_e_t_e_K_e_y_s &discreteKeys); │ │ │ │ │ -90 │ │ │ │ │ -97 _H_y_b_r_i_d_F_a_c_t_o_r(const _K_e_y_V_e_c_t_o_r &continuousKeys, │ │ │ │ │ -98 const _D_i_s_c_r_e_t_e_K_e_y_s &discreteKeys); │ │ │ │ │ -99 │ │ │ │ │ -_1_0_1 virtual _~_H_y_b_r_i_d_F_a_c_t_o_r() = default; │ │ │ │ │ -102 │ │ │ │ │ -106 │ │ │ │ │ -108 virtual bool _e_q_u_a_l_s(const _H_y_b_r_i_d_F_a_c_t_o_r &lf, double tol = 1e-9) const; │ │ │ │ │ -109 │ │ │ │ │ -111 void _p_r_i_n_t( │ │ │ │ │ -112 const std::string &s = "HybridFactor\n", │ │ │ │ │ -113 const _K_e_y_F_o_r_m_a_t_t_e_r &formatter = DefaultKeyFormatter) const override; │ │ │ │ │ -114 │ │ │ │ │ -118 │ │ │ │ │ -_1_2_0 bool _i_s_D_i_s_c_r_e_t_e() const { return isDiscrete_; } │ │ │ │ │ -121 │ │ │ │ │ -_1_2_3 bool _i_s_C_o_n_t_i_n_u_o_u_s() const { return isContinuous_; } │ │ │ │ │ -124 │ │ │ │ │ -_1_2_6 bool _i_s_H_y_b_r_i_d() const { return isHybrid_; } │ │ │ │ │ -127 │ │ │ │ │ -_1_2_9 size_t _n_r_C_o_n_t_i_n_u_o_u_s() const { return continuousKeys_.size(); } │ │ │ │ │ -130 │ │ │ │ │ -_1_3_2 const _D_i_s_c_r_e_t_e_K_e_y_s &_d_i_s_c_r_e_t_e_K_e_y_s() const { return discreteKeys_; } │ │ │ │ │ -133 │ │ │ │ │ -_1_3_5 const _K_e_y_V_e_c_t_o_r &_c_o_n_t_i_n_u_o_u_s_K_e_y_s() const { return continuousKeys_; } │ │ │ │ │ -136 │ │ │ │ │ -138 │ │ │ │ │ -139 private: │ │ │ │ │ -_1_4_1 friend class boost::serialization::access; │ │ │ │ │ -142 template │ │ │ │ │ -143 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ -144 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -145 ar &BOOST_SERIALIZATION_NVP(isDiscrete_); │ │ │ │ │ -146 ar &BOOST_SERIALIZATION_NVP(isContinuous_); │ │ │ │ │ -147 ar &BOOST_SERIALIZATION_NVP(isHybrid_); │ │ │ │ │ -148 ar &BOOST_SERIALIZATION_NVP(discreteKeys_); │ │ │ │ │ -149 ar &BOOST_SERIALIZATION_NVP(continuousKeys_); │ │ │ │ │ -150 } │ │ │ │ │ -151}; │ │ │ │ │ -152// HybridFactor │ │ │ │ │ -153 │ │ │ │ │ -154// traits │ │ │ │ │ -155template <> │ │ │ │ │ -_1_5_6struct _t_r_a_i_t_s<_H_y_b_r_i_d_F_a_c_t_o_r> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ -157 │ │ │ │ │ -158} // namespace gtsam │ │ │ │ │ -_T_e_s_t_a_b_l_e_._h │ │ │ │ │ -Concept check for values that can be used in unit tests. │ │ │ │ │ -_D_e_c_i_s_i_o_n_T_r_e_e_._h │ │ │ │ │ -Decision Tree for use in DiscreteFactors. │ │ │ │ │ -_D_i_s_c_r_e_t_e_K_e_y_._h │ │ │ │ │ -specialized key for discrete variables │ │ │ │ │ -_F_a_c_t_o_r_._h │ │ │ │ │ -The base class for all factors. │ │ │ │ │ -_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ -_V_a_l_u_e_s_._h │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +_3_3class GTSAM_EXPORT _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h : public _H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h { │ │ │ │ │ +34 protected: │ │ │ │ │ +35 public: │ │ │ │ │ +36 using _B_a_s_e = _H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h; │ │ │ │ │ +_3_7 using _T_h_i_s = _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h; │ │ │ │ │ +_3_8 using _s_h_a_r_e_d___p_t_r = boost::shared_ptr; │ │ │ │ │ +39 │ │ │ │ │ +_4_0 using _V_a_l_u_e_s = _g_t_s_a_m_:_:_V_a_l_u_e_s; │ │ │ │ │ +41 using Indices = _K_e_y_V_e_c_t_o_r; │ │ │ │ │ +42 │ │ │ │ │ +45 │ │ │ │ │ +46 _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h() = default; │ │ │ │ │ +47 │ │ │ │ │ +53 template │ │ │ │ │ +_5_4 _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h(const _F_a_c_t_o_r_G_r_a_p_h_<_D_E_R_I_V_E_D_F_A_C_T_O_R_>& graph) │ │ │ │ │ +55 : _B_a_s_e(graph) {} │ │ │ │ │ +56 │ │ │ │ │ +60 │ │ │ │ │ +62 void _p_r_i_n_t( │ │ │ │ │ +63 const std::string& s = "HybridNonlinearFactorGraph", │ │ │ │ │ +64 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override; │ │ │ │ │ +65 │ │ │ │ │ +69 │ │ │ │ │ +77 boost::shared_ptr linearize( │ │ │ │ │ +78 const _V_a_l_u_e_s& continuousValues) const; │ │ │ │ │ +80}; │ │ │ │ │ +81 │ │ │ │ │ +82template <> │ │ │ │ │ +_8_3struct _t_r_a_i_t_s<_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h> │ │ │ │ │ +84 : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ +85 │ │ │ │ │ +86} // namespace gtsam │ │ │ │ │ +_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Factor graph with utilities for hybrid factors. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ FastVector< Key > KeyVector │ │ │ │ │ Define collection type once and for all - also used in wrappers. │ │ │ │ │ DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ @@ -138,76 +75,35 @@ │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ _g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ A helper that implements the traits interface for GTSAM types. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e │ │ │ │ │ -a decision tree is a function from assignments to values. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree.h:61 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ -DiscreteKeys is a set of keys that can be assembled using the & operator. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteKey.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r │ │ │ │ │ -Base class for truly hybrid probabilistic factors. │ │ │ │ │ -DDeeffiinniittiioonn HybridFactor.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_:_:_n_r_C_o_n_t_i_n_u_o_u_s │ │ │ │ │ -size_t nrContinuous() const │ │ │ │ │ -Return the number of continuous variables in this factor. │ │ │ │ │ -DDeeffiinniittiioonn HybridFactor.h:129 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_:_:_T_h_i_s │ │ │ │ │ -HybridFactor This │ │ │ │ │ -This class. │ │ │ │ │ -DDeeffiinniittiioonn HybridFactor.h:66 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< HybridFactor > shared_ptr │ │ │ │ │ -shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn HybridFactor.h:68 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_:_:_i_s_D_i_s_c_r_e_t_e │ │ │ │ │ -bool isDiscrete() const │ │ │ │ │ -True if this is a factor of discrete variables only. │ │ │ │ │ -DDeeffiinniittiioonn HybridFactor.h:120 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_:_:_i_s_H_y_b_r_i_d │ │ │ │ │ -bool isHybrid() const │ │ │ │ │ -True is this is a Discrete-Continuous factor. │ │ │ │ │ -DDeeffiinniittiioonn HybridFactor.h:126 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_:_:_H_y_b_r_i_d_F_a_c_t_o_r │ │ │ │ │ -HybridFactor()=default │ │ │ │ │ -Default constructor creates empty factor. │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_:_:_B_a_s_e │ │ │ │ │ -Factor Base │ │ │ │ │ -Our base class. │ │ │ │ │ -DDeeffiinniittiioonn HybridFactor.h:69 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_:_:_c_o_n_t_i_n_u_o_u_s_K_e_y_s │ │ │ │ │ -const KeyVector & continuousKeys() const │ │ │ │ │ -Return only the continuous keys for this factor. │ │ │ │ │ -DDeeffiinniittiioonn HybridFactor.h:135 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_:_:_i_s_C_o_n_t_i_n_u_o_u_s │ │ │ │ │ -bool isContinuous() const │ │ │ │ │ -True if this is a factor of continuous variables only. │ │ │ │ │ -DDeeffiinniittiioonn HybridFactor.h:123 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_:_:_c_o_n_t_i_n_u_o_u_s_K_e_y_s__ │ │ │ │ │ -KeyVector continuousKeys_ │ │ │ │ │ -Record continuous keys for book-keeping. │ │ │ │ │ -DDeeffiinniittiioonn HybridFactor.h:62 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_:_:_~_H_y_b_r_i_d_F_a_c_t_o_r │ │ │ │ │ -virtual ~HybridFactor()=default │ │ │ │ │ -Virtual destructor. │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_:_:_d_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ -const DiscreteKeys & discreteKeys() const │ │ │ │ │ -Return the discrete keys for this factor. │ │ │ │ │ -DDeeffiinniittiioonn HybridFactor.h:132 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:68 │ │ │ │ │ -_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -the error. │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +Hybrid Factor Graph Factor graph with utilities for hybrid factors. │ │ │ │ │ +DDeeffiinniittiioonn HybridFactorGraph.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +DDeeffiinniittiioonn HybridNonlinearFactorGraph.h:33 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to This │ │ │ │ │ +DDeeffiinniittiioonn HybridNonlinearFactorGraph.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +HybridNonlinearFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph) │ │ │ │ │ +Implicit copy/downcast constructor to override explicit template container │ │ │ │ │ +constructor. │ │ │ │ │ +DDeeffiinniittiioonn HybridNonlinearFactorGraph.h:54 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ +nodes. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_F_a_c_t_o_r_._h │ │ │ │ │ + * _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00560.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridConditional.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridNonlinearISAM.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,43 +94,32 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
HybridConditional.h File Reference
│ │ │ │ +
HybridNonlinearISAM.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::HybridConditional
 Hybrid Conditional Density. More...
 
struct  gtsam::traits< HybridConditional >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Mar 11, 2022
│ │ │ │ -
Author
Fan Jiang
│ │ │ │ +
Date
Sep 12, 2022
│ │ │ │ +
Author
Varun Agrawal
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,27 +1,20 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -HybridConditional.h File Reference │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -  Hybrid _C_o_n_d_i_t_i_o_n_a_l Density. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_ _> │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +HybridNonlinearISAM.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Date │ │ │ │ │ - Mar 11, 2022 │ │ │ │ │ + Sep 12, 2022 │ │ │ │ │ Author │ │ │ │ │ - Fan Jiang │ │ │ │ │ + Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ + * _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00563.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/GaussianMixtureFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridFactorGraph.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,50 +95,53 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
GaussianMixtureFactor.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Typedefs
│ │ │ │ +
HybridFactorGraph.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

A set of GaussianFactors, indexed by a set of discrete keys. │ │ │ │ +

Factor graph with utilities for hybrid factors. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::GaussianMixtureFactor
 Implementation of a discrete conditional mixture factor. More...
 
struct  gtsam::traits< GaussianMixtureFactor >
class  gtsam::HybridFactorGraph
 Hybrid Factor Graph Factor graph with utilities for hybrid factors. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Typedefs

│ │ │ │ +using gtsam::SharedFactor = boost::shared_ptr< Factor >
 
│ │ │ │

Detailed Description

│ │ │ │ -

A set of GaussianFactors, indexed by a set of discrete keys.

│ │ │ │ -
Author
Fan Jiang
│ │ │ │ -
│ │ │ │ -Varun Agrawal
│ │ │ │ +

Factor graph with utilities for hybrid factors.

│ │ │ │ +
Author
Varun Agrawal
│ │ │ │
│ │ │ │ Frank Dellaert
│ │ │ │ -
Date
Mar 12, 2022
│ │ │ │ +
Date
May 28, 2022
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,31 +1,32 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -GaussianMixtureFactor.h File Reference │ │ │ │ │ -A set of GaussianFactors, indexed by a set of discrete keys. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ +HybridFactorGraph.h File Reference │ │ │ │ │ +Factor graph with utilities for hybrid factors. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r │ │ │ │ │ -  Implementation of a discrete conditional mixture factor. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r_ _> │ │ │ │ │ +class   _g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +  Hybrid _F_a_c_t_o_r Graph _F_a_c_t_o_r graph with utilities for hybrid factors. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +using  ggttssaamm::::SShhaarreeddFFaaccttoorr = boost::shared_ptr< _F_a_c_t_o_r > │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A set of GaussianFactors, indexed by a set of discrete keys. │ │ │ │ │ +Factor graph with utilities for hybrid factors. │ │ │ │ │ Author │ │ │ │ │ - Fan Jiang │ │ │ │ │ Varun Agrawal │ │ │ │ │ Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - Mar 12, 2022 │ │ │ │ │ + May 28, 2022 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r_._h │ │ │ │ │ + * _H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00563.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a00563 = [ │ │ │ │ │ - ["gtsam::traits< GaussianMixtureFactor >", "a03404.html", null] │ │ │ │ │ + ["gtsam::HybridFactorGraph", "a03456.html", "a03456"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00563_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/GaussianMixtureFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridFactorGraph.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,149 +98,92 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
GaussianMixtureFactor.h
│ │ │ │ +
HybridFactorGraph.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
21#pragma once
│ │ │ │ -
22
│ │ │ │ -
23#include <gtsam/discrete/AlgebraicDecisionTree.h>
│ │ │ │ -
24#include <gtsam/discrete/DecisionTree.h>
│ │ │ │ -
25#include <gtsam/discrete/DiscreteKey.h>
│ │ │ │ -
26#include <gtsam/hybrid/HybridFactor.h>
│ │ │ │ -
27#include <gtsam/linear/GaussianFactor.h>
│ │ │ │ -
28#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/hybrid/HybridFactor.h>
│ │ │ │ +
23#include <gtsam/inference/FactorGraph.h>
│ │ │ │ +
24
│ │ │ │ +
25#include <boost/format.hpp>
│ │ │ │ +
26#include <unordered_map>
│ │ │ │ +
27
│ │ │ │ +
28namespace gtsam {
│ │ │ │
29
│ │ │ │ -
30namespace gtsam {
│ │ │ │ -
31
│ │ │ │ -
32class HybridValues;
│ │ │ │ -
33class DiscreteValues;
│ │ │ │ -
34class VectorValues;
│ │ │ │ -
35
│ │ │ │ -
│ │ │ │ -
47class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
│ │ │ │ +
30class DiscreteFactor;
│ │ │ │ +
31class Ordering;
│ │ │ │ +
32
│ │ │ │ +
33using SharedFactor = boost::shared_ptr<Factor>;
│ │ │ │ +
34
│ │ │ │ +
│ │ │ │ +
39class HybridFactorGraph : public FactorGraph<Factor> {
│ │ │ │ +
40 public:
│ │ │ │ +
41 using Base = FactorGraph<Factor>;
│ │ │ │ +
42 using This = HybridFactorGraph;
│ │ │ │ +
43 using shared_ptr = boost::shared_ptr<This>;
│ │ │ │ +
44
│ │ │ │ +
45 using Values = gtsam::Values;
│ │ │ │ +
46 using Indices = KeyVector;
│ │ │ │ +
47
│ │ │ │
48 public:
│ │ │ │ -
49 using Base = HybridFactor;
│ │ │ │ -
50 using This = GaussianMixtureFactor;
│ │ │ │ -
51 using shared_ptr = boost::shared_ptr<This>;
│ │ │ │ -
52
│ │ │ │ -
53 using sharedFactor = boost::shared_ptr<GaussianFactor>;
│ │ │ │ +
51
│ │ │ │ +
53 HybridFactorGraph() = default;
│ │ │ │
54
│ │ │ │ -
56 using Factors = DecisionTree<Key, sharedFactor>;
│ │ │ │ -
57
│ │ │ │ -
58 private:
│ │ │ │ -
60 Factors factors_;
│ │ │ │ -
61
│ │ │ │ -
68 GaussianFactorGraphTree asGaussianFactorGraphTree() const;
│ │ │ │ +
60 template <class DERIVEDFACTOR>
│ │ │ │ +
61 HybridFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
│ │ │ │ +
62
│ │ │ │ +
66
│ │ │ │ +
68 std::set<DiscreteKey> discreteKeys() const;
│ │ │ │
69
│ │ │ │ -
70 public:
│ │ │ │ -
73
│ │ │ │ -
75 GaussianMixtureFactor() = default;
│ │ │ │ -
76
│ │ │ │ -
86 GaussianMixtureFactor(const KeyVector &continuousKeys,
│ │ │ │ -
87 const DiscreteKeys &discreteKeys,
│ │ │ │ -
88 const Factors &factors);
│ │ │ │ -
89
│ │ │ │ -
│ │ │ │ -
98 GaussianMixtureFactor(const KeyVector &continuousKeys,
│ │ │ │ -
99 const DiscreteKeys &discreteKeys,
│ │ │ │ -
100 const std::vector<sharedFactor> &factors)
│ │ │ │ -
101 : GaussianMixtureFactor(continuousKeys, discreteKeys,
│ │ │ │ -
102 Factors(discreteKeys, factors)) {}
│ │ │ │ -
│ │ │ │ -
103
│ │ │ │ -
107
│ │ │ │ -
108 bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
│ │ │ │ -
109
│ │ │ │ -
110 void print(
│ │ │ │ -
111 const std::string &s = "GaussianMixtureFactor\n",
│ │ │ │ -
112 const KeyFormatter &formatter = DefaultKeyFormatter) const override;
│ │ │ │ -
113
│ │ │ │ -
117
│ │ │ │ -
119 sharedFactor operator()(const DiscreteValues &assignment) const;
│ │ │ │ -
120
│ │ │ │ -
129 GaussianFactorGraphTree add(const GaussianFactorGraphTree &sum) const;
│ │ │ │ -
130
│ │ │ │ -
138 AlgebraicDecisionTree<Key> error(const VectorValues &continuousValues) const;
│ │ │ │ -
139
│ │ │ │ -
144 double error(const HybridValues &values) const override;
│ │ │ │ -
145
│ │ │ │ -
│ │ │ │ -
147 friend GaussianFactorGraphTree &operator+=(
│ │ │ │ -
148 GaussianFactorGraphTree &sum, const GaussianMixtureFactor &factor) {
│ │ │ │ -
149 sum = factor.add(sum);
│ │ │ │ -
150 return sum;
│ │ │ │ -
151 }
│ │ │ │ -
│ │ │ │ -
153
│ │ │ │ -
154 private:
│ │ │ │ -
156 friend class boost::serialization::access;
│ │ │ │ -
157 template <class ARCHIVE>
│ │ │ │ -
158 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ -
159 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
160 ar &BOOST_SERIALIZATION_NVP(factors_);
│ │ │ │ -
161 }
│ │ │ │ -
162};
│ │ │ │ -
│ │ │ │ -
163
│ │ │ │ -
164// traits
│ │ │ │ -
165template <>
│ │ │ │ -
│ │ │ │ -
166struct traits<GaussianMixtureFactor> : public Testable<GaussianMixtureFactor> {
│ │ │ │ -
167};
│ │ │ │ -
│ │ │ │ -
168
│ │ │ │ -
169} // namespace gtsam
│ │ │ │ -
DecisionTree.h
Decision Tree for use in DiscreteFactors.
│ │ │ │ -
AlgebraicDecisionTree.h
Algebraic Decision Trees.
│ │ │ │ -
DiscreteKey.h
specialized key for discrete variables
│ │ │ │ - │ │ │ │ -
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
│ │ │ │ -
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ +
71 KeySet discreteKeySet() const;
│ │ │ │ +
72
│ │ │ │ +
74 std::unordered_map<Key, DiscreteKey> discreteKeyMap() const;
│ │ │ │ +
75
│ │ │ │ +
77 const KeySet continuousKeySet() const;
│ │ │ │ +
78
│ │ │ │ +
80};
│ │ │ │ +
│ │ │ │ +
81
│ │ │ │ +
82} // namespace gtsam
│ │ │ │ + │ │ │ │ +
FactorGraph.h
Factor Graph Base Class.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::AlgebraicDecisionTree
An algebraic decision tree fixes the range of a DecisionTree to double.
Definition AlgebraicDecisionTree.h:38
│ │ │ │ - │ │ │ │ -
gtsam::DiscreteKeys
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition DiscreteKey.h:39
│ │ │ │ -
gtsam::DiscreteValues
A map from keys to values.
Definition DiscreteValues.h:34
│ │ │ │ -
gtsam::GaussianMixtureFactor
Implementation of a discrete conditional mixture factor.
Definition GaussianMixtureFactor.h:47
│ │ │ │ -
gtsam::GaussianMixtureFactor::GaussianMixtureFactor
GaussianMixtureFactor()=default
Default constructor, mainly for serialization.
│ │ │ │ -
gtsam::GaussianMixtureFactor::GaussianMixtureFactor
GaussianMixtureFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, const std::vector< sharedFactor > &factors)
Construct a new GaussianMixtureFactor object using a vector of GaussianFactor shared pointers.
Definition GaussianMixtureFactor.h:98
│ │ │ │ -
gtsam::GaussianMixtureFactor::add
GaussianFactorGraphTree add(const GaussianFactorGraphTree &sum) const
Combine the Gaussian Factor Graphs in sum and this while maintaining the original tree structure.
Definition GaussianMixtureFactor.cpp:85
│ │ │ │ -
gtsam::GaussianMixtureFactor::operator+=
friend GaussianFactorGraphTree & operator+=(GaussianFactorGraphTree &sum, const GaussianMixtureFactor &factor)
Add MixtureFactor to a Sum, syntactic sugar.
Definition GaussianMixtureFactor.h:147
│ │ │ │ -
gtsam::HybridFactor
Base class for truly hybrid probabilistic factors.
Definition HybridFactor.h:52
│ │ │ │ -
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ -
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ -
DiscreteValues
The Factor::error simply extracts the.
│ │ │ │ -
HybridValues
the error.
│ │ │ │ -
VectorValues
The Factor::error simply extracts the.
│ │ │ │ + │ │ │ │ +
gtsam::HybridFactorGraph
Hybrid Factor Graph Factor graph with utilities for hybrid factors.
Definition HybridFactorGraph.h:39
│ │ │ │ +
gtsam::HybridFactorGraph::HybridFactorGraph
HybridFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
Implicit copy/downcast constructor to override explicit template container constructor.
Definition HybridFactorGraph.h:61
│ │ │ │ +
gtsam::HybridFactorGraph::discreteKeySet
KeySet discreteKeySet() const
Get all the discrete keys in the factor graph, as a set.
Definition HybridFactorGraph.cpp:46
│ │ │ │ +
gtsam::HybridFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to This
Definition HybridFactorGraph.h:43
│ │ │ │ +
gtsam::HybridFactorGraph::discreteKeyMap
std::unordered_map< Key, DiscreteKey > discreteKeyMap() const
Get a map from Key to corresponding DiscreteKey.
Definition HybridFactorGraph.cpp:56
│ │ │ │ +
gtsam::HybridFactorGraph::continuousKeySet
const KeySet continuousKeySet() const
Get all the continuous keys in the factor graph.
Definition HybridFactorGraph.cpp:65
│ │ │ │ +
gtsam::HybridFactorGraph::HybridFactorGraph
HybridFactorGraph()=default
Default constructor.
│ │ │ │ +
gtsam::HybridFactorGraph::discreteKeys
std::set< DiscreteKey > discreteKeys() const
Get all the discrete keys in the factor graph.
Definition HybridFactorGraph.cpp:28
│ │ │ │ +
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ +
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,191 +1,116 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GaussianMixtureFactor.h │ │ │ │ │ +HybridFactorGraph.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -21#pragma once │ │ │ │ │ -22 │ │ │ │ │ -23#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_e_c_i_s_i_o_n_T_r_e_e_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_K_e_y_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_F_a_c_t_o_r_._h> │ │ │ │ │ -27#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ -28#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_F_a_c_t_o_r_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +24 │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ +27 │ │ │ │ │ +28namespace _g_t_s_a_m { │ │ │ │ │ 29 │ │ │ │ │ -30namespace _g_t_s_a_m { │ │ │ │ │ -31 │ │ │ │ │ -32class _H_y_b_r_i_d_V_a_l_u_e_s; │ │ │ │ │ -33class _D_i_s_c_r_e_t_e_V_a_l_u_e_s; │ │ │ │ │ -34class _V_e_c_t_o_r_V_a_l_u_e_s; │ │ │ │ │ -35 │ │ │ │ │ -_4_7class GTSAM_EXPORT _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r : public _H_y_b_r_i_d_F_a_c_t_o_r { │ │ │ │ │ +30class DiscreteFactor; │ │ │ │ │ +31class Ordering; │ │ │ │ │ +32 │ │ │ │ │ +33using SharedFactor = boost::shared_ptr; │ │ │ │ │ +34 │ │ │ │ │ +_3_9class _H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h : public _F_a_c_t_o_r_G_r_a_p_h { │ │ │ │ │ +40 public: │ │ │ │ │ +41 using _B_a_s_e = _F_a_c_t_o_r_G_r_a_p_h_<_F_a_c_t_o_r_>; │ │ │ │ │ +_4_2 using _T_h_i_s = _H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h; │ │ │ │ │ +_4_3 using _s_h_a_r_e_d___p_t_r = boost::shared_ptr; │ │ │ │ │ +44 │ │ │ │ │ +_4_5 using _V_a_l_u_e_s = _g_t_s_a_m_:_:_V_a_l_u_e_s; │ │ │ │ │ +46 using Indices = _K_e_y_V_e_c_t_o_r; │ │ │ │ │ +47 │ │ │ │ │ 48 public: │ │ │ │ │ -49 using _B_a_s_e = _H_y_b_r_i_d_F_a_c_t_o_r; │ │ │ │ │ -50 using _T_h_i_s = _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r; │ │ │ │ │ -51 using shared_ptr = boost::shared_ptr; │ │ │ │ │ -52 │ │ │ │ │ -53 using sharedFactor = boost::shared_ptr; │ │ │ │ │ +51 │ │ │ │ │ +_5_3 _H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h() = default; │ │ │ │ │ 54 │ │ │ │ │ -_5_6 using _F_a_c_t_o_r_s = _D_e_c_i_s_i_o_n_T_r_e_e_<_K_e_y_,_ _s_h_a_r_e_d_F_a_c_t_o_r_>; │ │ │ │ │ -57 │ │ │ │ │ -58 private: │ │ │ │ │ -60 _F_a_c_t_o_r_s factors_; │ │ │ │ │ -61 │ │ │ │ │ -68 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_T_r_e_e asGaussianFactorGraphTree() const; │ │ │ │ │ +60 template │ │ │ │ │ +_6_1 _H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h(const _F_a_c_t_o_r_G_r_a_p_h_<_D_E_R_I_V_E_D_F_A_C_T_O_R_>& graph) : _B_a_s_e(graph) {} │ │ │ │ │ +62 │ │ │ │ │ +66 │ │ │ │ │ +68 std::set _d_i_s_c_r_e_t_e_K_e_y_s() const; │ │ │ │ │ 69 │ │ │ │ │ -70 public: │ │ │ │ │ -73 │ │ │ │ │ -_7_5 _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r() = default; │ │ │ │ │ -76 │ │ │ │ │ -86 _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r(const _K_e_y_V_e_c_t_o_r &continuousKeys, │ │ │ │ │ -87 const _D_i_s_c_r_e_t_e_K_e_y_s &discreteKeys, │ │ │ │ │ -88 const _F_a_c_t_o_r_s &factors); │ │ │ │ │ -89 │ │ │ │ │ -_9_8 _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r(const _K_e_y_V_e_c_t_o_r &continuousKeys, │ │ │ │ │ -99 const _D_i_s_c_r_e_t_e_K_e_y_s &discreteKeys, │ │ │ │ │ -100 const std::vector &factors) │ │ │ │ │ -101 : _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r(continuousKeys, discreteKeys, │ │ │ │ │ -102 _F_a_c_t_o_r_s(discreteKeys, factors)) {} │ │ │ │ │ -103 │ │ │ │ │ -107 │ │ │ │ │ -108 bool _e_q_u_a_l_s(const _H_y_b_r_i_d_F_a_c_t_o_r &lf, double tol = 1e-9) const override; │ │ │ │ │ -109 │ │ │ │ │ -110 void _p_r_i_n_t( │ │ │ │ │ -111 const std::string &s = "GaussianMixtureFactor\n", │ │ │ │ │ -112 const _K_e_y_F_o_r_m_a_t_t_e_r &formatter = DefaultKeyFormatter) const override; │ │ │ │ │ -113 │ │ │ │ │ -117 │ │ │ │ │ -119 sharedFactor operator()(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s &assignment) const; │ │ │ │ │ -120 │ │ │ │ │ -129 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_T_r_e_e add(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_T_r_e_e &sum) const; │ │ │ │ │ -130 │ │ │ │ │ -138 _A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_<_K_e_y_> error(const _V_e_c_t_o_r_V_a_l_u_e_s &continuousValues) │ │ │ │ │ -const; │ │ │ │ │ -139 │ │ │ │ │ -144 double error(const _H_y_b_r_i_d_V_a_l_u_e_s &values) const override; │ │ │ │ │ -145 │ │ │ │ │ -_1_4_7 friend _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_T_r_e_e &_o_p_e_r_a_t_o_r_+_=( │ │ │ │ │ -148 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_T_r_e_e &sum, const _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r &factor) { │ │ │ │ │ -149 sum = factor._a_d_d(sum); │ │ │ │ │ -150 return sum; │ │ │ │ │ -151 } │ │ │ │ │ -153 │ │ │ │ │ -154 private: │ │ │ │ │ -_1_5_6 friend class boost::serialization::access; │ │ │ │ │ -157 template │ │ │ │ │ -158 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ -159 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -160 ar &BOOST_SERIALIZATION_NVP(factors_); │ │ │ │ │ -161 } │ │ │ │ │ -162}; │ │ │ │ │ -163 │ │ │ │ │ -164// traits │ │ │ │ │ -165template <> │ │ │ │ │ -_1_6_6struct _t_r_a_i_t_s<_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r> : public │ │ │ │ │ -_T_e_s_t_a_b_l_e { │ │ │ │ │ -167}; │ │ │ │ │ -168 │ │ │ │ │ -169} // namespace gtsam │ │ │ │ │ -_D_e_c_i_s_i_o_n_T_r_e_e_._h │ │ │ │ │ -Decision Tree for use in DiscreteFactors. │ │ │ │ │ -_A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_._h │ │ │ │ │ -Algebraic Decision Trees. │ │ │ │ │ -_D_i_s_c_r_e_t_e_K_e_y_._h │ │ │ │ │ -specialized key for discrete variables │ │ │ │ │ +71 _K_e_y_S_e_t _d_i_s_c_r_e_t_e_K_e_y_S_e_t() const; │ │ │ │ │ +72 │ │ │ │ │ +74 std::unordered_map _d_i_s_c_r_e_t_e_K_e_y_M_a_p() const; │ │ │ │ │ +75 │ │ │ │ │ +77 const _K_e_y_S_e_t _c_o_n_t_i_n_u_o_u_s_K_e_y_S_e_t() const; │ │ │ │ │ +78 │ │ │ │ │ +80}; │ │ │ │ │ +81 │ │ │ │ │ +82} // namespace gtsam │ │ │ │ │ _H_y_b_r_i_d_F_a_c_t_o_r_._h │ │ │ │ │ -_G_a_u_s_s_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ -A factor with a quadratic error function - a Gaussian. │ │ │ │ │ -_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ +_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Factor Graph Base Class. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ FastVector< Key > KeyVector │ │ │ │ │ Define collection type once and for all - also used in wrappers. │ │ │ │ │ DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e │ │ │ │ │ -An algebraic decision tree fixes the range of a DecisionTree to double. │ │ │ │ │ -DDeeffiinniittiioonn AlgebraicDecisionTree.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_<_ _K_e_y_,_ _s_h_a_r_e_d_F_a_c_t_o_r_ _> │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ -DiscreteKeys is a set of keys that can be assembled using the & operator. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteKey.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ -A map from keys to values. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteValues.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r │ │ │ │ │ -Implementation of a discrete conditional mixture factor. │ │ │ │ │ -DDeeffiinniittiioonn GaussianMixtureFactor.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r_:_:_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r │ │ │ │ │ -GaussianMixtureFactor()=default │ │ │ │ │ -Default constructor, mainly for serialization. │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r_:_:_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r │ │ │ │ │ -GaussianMixtureFactor(const KeyVector &continuousKeys, const DiscreteKeys │ │ │ │ │ -&discreteKeys, const std::vector< sharedFactor > &factors) │ │ │ │ │ -Construct a new GaussianMixtureFactor object using a vector of GaussianFactor │ │ │ │ │ -shared pointers. │ │ │ │ │ -DDeeffiinniittiioonn GaussianMixtureFactor.h:98 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r_:_:_a_d_d │ │ │ │ │ -GaussianFactorGraphTree add(const GaussianFactorGraphTree &sum) const │ │ │ │ │ -Combine the Gaussian Factor Graphs in sum and this while maintaining the │ │ │ │ │ -original tree structure. │ │ │ │ │ -DDeeffiinniittiioonn GaussianMixtureFactor.cpp:85 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r_:_:_o_p_e_r_a_t_o_r_+_= │ │ │ │ │ -friend GaussianFactorGraphTree & operator+=(GaussianFactorGraphTree &sum, const │ │ │ │ │ -GaussianMixtureFactor &factor) │ │ │ │ │ -Add MixtureFactor to a Sum, syntactic sugar. │ │ │ │ │ -DDeeffiinniittiioonn GaussianMixtureFactor.h:147 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r │ │ │ │ │ -Base class for truly hybrid probabilistic factors. │ │ │ │ │ -DDeeffiinniittiioonn HybridFactor.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ -with a unique integer... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ -_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ -The Factor::error simply extracts the. │ │ │ │ │ -_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -the error. │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -The Factor::error simply extracts the. │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +Hybrid Factor Graph Factor graph with utilities for hybrid factors. │ │ │ │ │ +DDeeffiinniittiioonn HybridFactorGraph.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h_:_:_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +HybridFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph) │ │ │ │ │ +Implicit copy/downcast constructor to override explicit template container │ │ │ │ │ +constructor. │ │ │ │ │ +DDeeffiinniittiioonn HybridFactorGraph.h:61 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h_:_:_d_i_s_c_r_e_t_e_K_e_y_S_e_t │ │ │ │ │ +KeySet discreteKeySet() const │ │ │ │ │ +Get all the discrete keys in the factor graph, as a set. │ │ │ │ │ +DDeeffiinniittiioonn HybridFactorGraph.cpp:46 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to This │ │ │ │ │ +DDeeffiinniittiioonn HybridFactorGraph.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h_:_:_d_i_s_c_r_e_t_e_K_e_y_M_a_p │ │ │ │ │ +std::unordered_map< Key, DiscreteKey > discreteKeyMap() const │ │ │ │ │ +Get a map from Key to corresponding DiscreteKey. │ │ │ │ │ +DDeeffiinniittiioonn HybridFactorGraph.cpp:56 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h_:_:_c_o_n_t_i_n_u_o_u_s_K_e_y_S_e_t │ │ │ │ │ +const KeySet continuousKeySet() const │ │ │ │ │ +Get all the continuous keys in the factor graph. │ │ │ │ │ +DDeeffiinniittiioonn HybridFactorGraph.cpp:65 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h_:_:_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +HybridFactorGraph()=default │ │ │ │ │ +Default constructor. │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h_:_:_d_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ +std::set< DiscreteKey > discreteKeys() const │ │ │ │ │ +Get all the discrete keys in the factor graph. │ │ │ │ │ +DDeeffiinniittiioonn HybridFactorGraph.cpp:28 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ +nodes. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r_._h │ │ │ │ │ + * _H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00566.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridFactor.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridValues.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,45 +94,43 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
HybridFactor.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
HybridValues.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::HybridValues
 HybridValues represents a collection of DiscreteValues and VectorValues. More...
 
struct  gtsam::traits< HybridValues >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -KeyVector gtsam::CollectKeys (const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys)
 
│ │ │ │ -KeyVector gtsam::CollectKeys (const KeyVector &keys1, const KeyVector &keys2)
 
│ │ │ │ -DiscreteKeys gtsam::CollectDiscreteKeys (const DiscreteKeys &key1, const DiscreteKeys &key2)
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Mar 11, 2022
│ │ │ │ -
Author
Fan Jiang
│ │ │ │ +
Date
Jul 28, 2022
│ │ │ │ +
Author
Shangjie Xue
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,30 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -HybridFactor.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +HybridValues.h File Reference │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +  _H_y_b_r_i_d_V_a_l_u_e_s represents a collection of _D_i_s_c_r_e_t_e_V_a_l_u_e_s and │ │ │ │ │ + _V_e_c_t_o_r_V_a_l_u_e_s. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _H_y_b_r_i_d_V_a_l_u_e_s_ _> │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ - _K_e_y_V_e_c_t_o_r  ggttssaamm::::CCoolllleeccttKKeeyyss (const _K_e_y_V_e_c_t_o_r &continuousKeys, const │ │ │ │ │ - _D_i_s_c_r_e_t_e_K_e_y_s &discreteKeys) │ │ │ │ │ -  │ │ │ │ │ - _K_e_y_V_e_c_t_o_r  ggttssaamm::::CCoolllleeccttKKeeyyss (const _K_e_y_V_e_c_t_o_r &keys1, const _K_e_y_V_e_c_t_o_r │ │ │ │ │ - &keys2) │ │ │ │ │ -  │ │ │ │ │ -_D_i_s_c_r_e_t_e_K_e_y_s  ggttssaamm::::CCoolllleeccttDDiissccrreetteeKKeeyyss (const _D_i_s_c_r_e_t_e_K_e_y_s &key1, const │ │ │ │ │ - _D_i_s_c_r_e_t_e_K_e_y_s &key2) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Date │ │ │ │ │ - Mar 11, 2022 │ │ │ │ │ + Jul 28, 2022 │ │ │ │ │ Author │ │ │ │ │ - Fan Jiang │ │ │ │ │ + Shangjie Xue │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_F_a_c_t_o_r_._c_p_p │ │ │ │ │ + * _H_y_b_r_i_d_V_a_l_u_e_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00569.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridNonlinearFactorGraph.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridBayesNet.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,36 +94,52 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
HybridNonlinearFactorGraph.cpp File Reference
│ │ │ │ +
HybridBayesNet.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Nonlinear hybrid factor graph that uses type erasure. │ │ │ │ +

A Bayes net of Gaussian Conditionals indexed by discrete keys. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::HybridBayesNet
 A hybrid Bayes net is a collection of HybridConditionals, which can have discrete conditionals, Gaussian mixtures, or pure Gaussian conditionals. More...
 
struct  gtsam::traits< HybridBayesNet >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Nonlinear hybrid factor graph that uses type erasure.

│ │ │ │ -
Author
Varun Agrawal
│ │ │ │ -
Date
May 28, 2022
│ │ │ │ +

A Bayes net of Gaussian Conditionals indexed by discrete keys.

│ │ │ │ +
Author
Varun Agrawal
│ │ │ │ +
│ │ │ │ +Fan Jiang
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +
Date
December 2021
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,34 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -HybridNonlinearFactorGraph.cpp File Reference │ │ │ │ │ -Nonlinear hybrid factor graph that uses type erasure. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +HybridBayesNet.h File Reference │ │ │ │ │ +A Bayes net of Gaussian Conditionals indexed by discrete keys. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_N_e_t │ │ │ │ │ + A hybrid Bayes net is a collection of HybridConditionals, which can │ │ │ │ │ +  have discrete conditionals, Gaussian mixtures, or pure Gaussian │ │ │ │ │ + conditionals. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _H_y_b_r_i_d_B_a_y_e_s_N_e_t_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Nonlinear hybrid factor graph that uses type erasure. │ │ │ │ │ +A Bayes net of Gaussian Conditionals indexed by discrete keys. │ │ │ │ │ Author │ │ │ │ │ Varun Agrawal │ │ │ │ │ + Fan Jiang │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - May 28, 2022 │ │ │ │ │ + December 2021 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._c_p_p │ │ │ │ │ + * _H_y_b_r_i_d_B_a_y_e_s_N_e_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00572_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/graph.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesNet-inst.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,115 +98,151 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
graph.h
│ │ │ │ +
BayesNet-inst.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ -
4 * Atlanta, Georgia 30332-0415
│ │ │ │ -
5 * All Rights Reserved
│ │ │ │ -
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
3* GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
4* Atlanta, Georgia 30332-0415
│ │ │ │ +
5* All Rights Reserved
│ │ │ │ +
6* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │ -
8 * See LICENSE for the license information
│ │ │ │ +
8* See LICENSE for the license information
│ │ │ │
9
│ │ │ │ -
10 * -------------------------------------------------------------------------- */
│ │ │ │ +
10* -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <map>
│ │ │ │ -
22
│ │ │ │ -
23#define BOOST_NO_HASH // to pacify the warnings about depricated headers in boost.graph
│ │ │ │ -
24
│ │ │ │ -
25#include <boost/graph/graph_traits.hpp>
│ │ │ │ -
26#include <boost/graph/adjacency_list.hpp>
│ │ │ │ -
27#include <boost/shared_ptr.hpp>
│ │ │ │ -
28#include <gtsam/nonlinear/Values.h>
│ │ │ │ +
21#include <gtsam/inference/BayesNet.h>
│ │ │ │ +
22#include <gtsam/inference/FactorGraph-inst.h>
│ │ │ │ +
23
│ │ │ │ +
24#include <boost/range/adaptor/reversed.hpp>
│ │ │ │ +
25#include <fstream>
│ │ │ │ +
26#include <string>
│ │ │ │ +
27
│ │ │ │ +
28namespace gtsam {
│ │ │ │
29
│ │ │ │ -
30namespace gtsam {
│ │ │ │ -
31
│ │ │ │ -
32 // type definitions :
│ │ │ │ -
33
│ │ │ │ -
37 template<class KEY>
│ │ │ │ -
│ │ │ │ -
38 class SDGraph: public boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
│ │ │ │ -
39 boost::property<boost::vertex_name_t, KEY>, boost::property<
│ │ │ │ -
40 boost::edge_weight_t, double> > {
│ │ │ │ -
41 public:
│ │ │ │ -
42 typedef typename boost::graph_traits<SDGraph<KEY> >::vertex_descriptor Vertex;
│ │ │ │ -
43 };
│ │ │ │ -
│ │ │ │ -
44
│ │ │ │ -
45 template<class KEY>
│ │ │ │ -
│ │ │ │ -
46 class SGraph : public boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS,
│ │ │ │ -
47 boost::property<boost::vertex_name_t, KEY> > {
│ │ │ │ -
48 public:
│ │ │ │ -
49 typedef typename boost::graph_traits<SGraph<KEY> >::vertex_descriptor Vertex;
│ │ │ │ -
50 };
│ │ │ │ -
│ │ │ │ -
51
│ │ │ │ -
52 //typedef boost::graph_traits<SGraph>::vertex_descriptor SVertex;
│ │ │ │ -
53
│ │ │ │ -
57 template<class KEY>
│ │ │ │ -
│ │ │ │ -
58 class PredecessorMap: public std::map<KEY, KEY> {
│ │ │ │ -
59 public:
│ │ │ │ -
│ │ │ │ -
61 inline void insert(const KEY& key, const KEY& parent) {
│ │ │ │ -
62 std::map<KEY, KEY>::insert(std::make_pair(key, parent));
│ │ │ │ -
63 }
│ │ │ │ -
│ │ │ │ -
64 };
│ │ │ │ -
│ │ │ │ -
65
│ │ │ │ -
69 template<class KEY>
│ │ │ │ -
70 std::list<KEY> predecessorMap2Keys(const PredecessorMap<KEY>& p_map);
│ │ │ │ +
30/* ************************************************************************* */
│ │ │ │ +
31template <class CONDITIONAL>
│ │ │ │ +
│ │ │ │ +
32void BayesNet<CONDITIONAL>::print(const std::string& s,
│ │ │ │ +
33 const KeyFormatter& formatter) const {
│ │ │ │ +
34 std::cout << (s.empty() ? "" : s + " ") << std::endl;
│ │ │ │ +
35 std::cout << "size: " << this->size() << std::endl;
│ │ │ │ +
36 for (size_t i = 0; i < this->size(); i++) {
│ │ │ │ +
37 const auto& conditional = this->at(i);
│ │ │ │ +
38 std::stringstream ss;
│ │ │ │ +
39 ss << "conditional " << i << ": ";
│ │ │ │ +
40 if (conditional) conditional->print(ss.str(), formatter);
│ │ │ │ +
41 }
│ │ │ │ +
42}
│ │ │ │ +
│ │ │ │ +
43
│ │ │ │ +
44/* ************************************************************************* */
│ │ │ │ +
45template <class CONDITIONAL>
│ │ │ │ +
│ │ │ │ +
46void BayesNet<CONDITIONAL>::dot(std::ostream& os,
│ │ │ │ +
47 const KeyFormatter& keyFormatter,
│ │ │ │ +
48 const DotWriter& writer) const {
│ │ │ │ +
49 writer.digraphPreamble(&os);
│ │ │ │ +
50
│ │ │ │ +
51 // Create nodes for each variable in the graph
│ │ │ │ +
52 for (Key key : this->keys()) {
│ │ │ │ +
53 auto position = writer.variablePos(key);
│ │ │ │ +
54 writer.drawVariable(key, keyFormatter, position, &os);
│ │ │ │ +
55 }
│ │ │ │ +
56 os << "\n";
│ │ │ │ +
57
│ │ │ │ +
58 // Reverse order as typically Bayes nets stored in reverse topological sort.
│ │ │ │ +
59 for (auto conditional : boost::adaptors::reverse(*this)) {
│ │ │ │ +
60 auto frontals = conditional->frontals();
│ │ │ │ +
61 const Key me = frontals.front();
│ │ │ │ +
62 auto parents = conditional->parents();
│ │ │ │ +
63 for (const Key& p : parents) {
│ │ │ │ +
64 os << " var" << p << "->var" << me << "\n";
│ │ │ │ +
65 }
│ │ │ │ +
66 }
│ │ │ │ +
67
│ │ │ │ +
68 os << "}";
│ │ │ │ +
69 std::flush(os);
│ │ │ │ +
70}
│ │ │ │ +
│ │ │ │
71
│ │ │ │ -
78 template<class G, class F, class KEY> SDGraph<KEY> toBoostGraph(const G& graph);
│ │ │ │ -
79
│ │ │ │ -
85 template<class G, class V, class KEY>
│ │ │ │ -
86 boost::tuple<G, V, std::map<KEY,V> > predecessorMap2Graph(const PredecessorMap<KEY>& p_map);
│ │ │ │ -
87
│ │ │ │ -
91 template<class G, class Factor, class POSE, class KEY>
│ │ │ │ -
92 boost::shared_ptr<Values>
│ │ │ │ -
93 composePoses(const G& graph, const PredecessorMap<KEY>& tree, const POSE& rootPose);
│ │ │ │ -
94
│ │ │ │ -
95
│ │ │ │ -
99 template<class G, class KEY, class FACTOR2>
│ │ │ │ -
100 PredecessorMap<KEY> findMinimumSpanningTree(const G& g) ;
│ │ │ │ -
101
│ │ │ │ -
106 template<class G, class KEY, class FACTOR2>
│ │ │ │ -
107 void split(const G& g, const PredecessorMap<KEY>& tree, G& Ab1, G& Ab2) ;
│ │ │ │ +
72/* ************************************************************************* */
│ │ │ │ +
73template <class CONDITIONAL>
│ │ │ │ +
│ │ │ │ +
74std::string BayesNet<CONDITIONAL>::dot(const KeyFormatter& keyFormatter,
│ │ │ │ +
75 const DotWriter& writer) const {
│ │ │ │ +
76 std::stringstream ss;
│ │ │ │ +
77 dot(ss, keyFormatter, writer);
│ │ │ │ +
78 return ss.str();
│ │ │ │ +
79}
│ │ │ │ +
│ │ │ │ +
80
│ │ │ │ +
81/* ************************************************************************* */
│ │ │ │ +
82template <class CONDITIONAL>
│ │ │ │ +
│ │ │ │ +
83void BayesNet<CONDITIONAL>::saveGraph(const std::string& filename,
│ │ │ │ +
│ │ │ │ +
84 const KeyFormatter& keyFormatter,
│ │ │ │ +
85 const DotWriter& writer) const {
│ │ │ │ +
86 std::ofstream of(filename.c_str());
│ │ │ │ +
87 dot(of, keyFormatter, writer);
│ │ │ │ +
│ │ │ │ +
88 of.close();
│ │ │ │ +
89}
│ │ │ │ +
│ │ │ │ +
90
│ │ │ │ +
91/* ************************************************************************* */
│ │ │ │ +
92template <class CONDITIONAL>
│ │ │ │ +
93double BayesNet<CONDITIONAL>::logProbability(const HybridValues& x) const {
│ │ │ │ +
94 double sum = 0.;
│ │ │ │ +
95 for (const auto& gc : *this) {
│ │ │ │ +
96 if (gc) sum += gc->logProbability(x);
│ │ │ │ +
97 }
│ │ │ │ +
98 return sum;
│ │ │ │ +
99}
│ │ │ │ +
100
│ │ │ │ +
101/* ************************************************************************* */
│ │ │ │ +
102template <class CONDITIONAL>
│ │ │ │ +
103double BayesNet<CONDITIONAL>::evaluate(const HybridValues& x) const {
│ │ │ │ +
104 return exp(-logProbability(x));
│ │ │ │ +
105}
│ │ │ │ +
106
│ │ │ │ +
107/* ************************************************************************* */
│ │ │ │
108
│ │ │ │ -
109
│ │ │ │ -
110} // namespace gtsam
│ │ │ │ -
111
│ │ │ │ -
112#include <gtsam/inference/graph-inl.h>
│ │ │ │ -
Values.h
A non-templated config holding any types of Manifold-group elements.
│ │ │ │ +
109} // namespace gtsam
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
FactorGraph-inst.h
Factor Graph Base Class.
│ │ │ │ +
BayesNet.h
Bayes network.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::predecessorMap2Keys
std::list< KEY > predecessorMap2Keys(const PredecessorMap< KEY > &p_map)
Generate a list of keys from a spanning tree represented by its predecessor map.
Definition graph-inl.h:50
│ │ │ │ -
gtsam::split
void split(const G &g, const PredecessorMap< KEY > &tree, G &Ab1, G &Ab2)
Split the graph into two parts: one corresponds to the given spanning tree, and the other corresponds...
Definition graph-inl.h:255
│ │ │ │ -
gtsam::composePoses
boost::shared_ptr< Values > composePoses(const G &graph, const PredecessorMap< KEY > &tree, const POSE &rootPose)
Compose the poses by following the chain specified by the spanning tree.
Definition graph-inl.h:174
│ │ │ │ -
gtsam::toBoostGraph
SDGraph< KEY > toBoostGraph(const G &graph)
Convert the factor graph to an SDGraph G = Graph type F = Factor type Key = Key type.
Definition graph-inl.h:68
│ │ │ │ -
gtsam::findMinimumSpanningTree
PredecessorMap< KEY > findMinimumSpanningTree(const G &fg)
find the minimum spanning tree using boost graph library
Definition graph-inl.h:232
│ │ │ │ -
gtsam::predecessorMap2Graph
boost::tuple< G, V, std::map< KEY, V > > predecessorMap2Graph(const PredecessorMap< KEY > &p_map)
Build takes a predecessor map, and builds a directed graph corresponding to the tree.
Definition graph-inl.h:118
│ │ │ │ -
gtsam::SDGraph
SDGraph is undirected graph with variable keys and double edge weights.
Definition graph.h:40
│ │ │ │ -
gtsam::SGraph
Definition graph.h:47
│ │ │ │ -
gtsam::PredecessorMap
Map from variable key to parent key.
Definition graph.h:58
│ │ │ │ -
gtsam::PredecessorMap::insert
void insert(const KEY &key, const KEY &parent)
convenience insert so we can pass ints for TypedSymbol keys
Definition graph.h:61
│ │ │ │ +
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ +
gtsam::BayesNet
A BayesNet is a tree of conditionals, stored in elimination order.
Definition BayesNet.h:35
│ │ │ │ +
gtsam::BayesNet::print
void print(const std::string &s="BayesNet", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print out graph
Definition BayesNet-inst.h:32
│ │ │ │ +
gtsam::BayesNet::dot
void dot(std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
Output to graphviz format, stream version.
Definition BayesNet-inst.h:46
│ │ │ │ +
gtsam::BayesNet::saveGraph
void saveGraph(const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
output to file with graphviz format.
Definition BayesNet-inst.h:83
│ │ │ │ +
gtsam::DotWriter
DotWriter is a helper class for writing graphviz .dot files.
Definition DotWriter.h:35
│ │ │ │ +
gtsam::DotWriter::drawVariable
void drawVariable(Key key, const KeyFormatter &keyFormatter, const boost::optional< Vector2 > &position, std::ostream *os) const
Create a variable dot fragment.
Definition DotWriter.cpp:42
│ │ │ │ +
gtsam::DotWriter::digraphPreamble
void digraphPreamble(std::ostream *os) const
Write out preamble for digraph, including size.
Definition DotWriter.cpp:36
│ │ │ │ +
gtsam::DotWriter::variablePos
boost::optional< Vector2 > variablePos(Key key) const
Return variable position or none.
Definition DotWriter.cpp:79
│ │ │ │ +
HybridValues
the error.
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,142 +1,179 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -graph.h │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +BayesNet-inst.h │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ -4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -5 * All Rights Reserved │ │ │ │ │ -6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +3* GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4* Atlanta, Georgia 30332-0415 │ │ │ │ │ +5* All Rights Reserved │ │ │ │ │ +6* Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ -8 * See LICENSE for the license information │ │ │ │ │ +8* See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ -10 * ------------------------------------------------------------------------- │ │ │ │ │ +10* ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 19#pragma once │ │ │ │ │ 20 │ │ │ │ │ -21#include │ │ │ │ │ -22 │ │ │ │ │ -23#define BOOST_NO_HASH // to pacify the warnings about depricated headers in │ │ │ │ │ -boost.graph │ │ │ │ │ -24 │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ -28#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_V_a_l_u_e_s_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_-_i_n_s_t_._h> │ │ │ │ │ +23 │ │ │ │ │ +24#include │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ +27 │ │ │ │ │ +28namespace _g_t_s_a_m { │ │ │ │ │ 29 │ │ │ │ │ -30namespace _g_t_s_a_m { │ │ │ │ │ -31 │ │ │ │ │ -32 // type definitions : │ │ │ │ │ -33 │ │ │ │ │ -37 template │ │ │ │ │ -_3_8 class _S_D_G_r_a_p_h: public boost::adjacency_list, boost::property< │ │ │ │ │ -40 boost::edge_weight_t, double> > { │ │ │ │ │ -41 public: │ │ │ │ │ -42 typedef typename boost::graph_traits >::vertex_descriptor │ │ │ │ │ -Vertex; │ │ │ │ │ -43 }; │ │ │ │ │ -44 │ │ │ │ │ -45 template │ │ │ │ │ -_4_6 class _S_G_r_a_p_h : public boost::adjacency_list > { │ │ │ │ │ -48 public: │ │ │ │ │ -49 typedef typename boost::graph_traits >::vertex_descriptor │ │ │ │ │ -Vertex; │ │ │ │ │ -50 }; │ │ │ │ │ -51 │ │ │ │ │ -52 //typedef boost::graph_traits::vertex_descriptor SVertex; │ │ │ │ │ -53 │ │ │ │ │ -57 template │ │ │ │ │ -_5_8 class _P_r_e_d_e_c_e_s_s_o_r_M_a_p: public std::map { │ │ │ │ │ -59 public: │ │ │ │ │ -_6_1 inline void _i_n_s_e_r_t(const KEY& key, const KEY& parent) { │ │ │ │ │ -62 std::map::insert(std::make_pair(key, parent)); │ │ │ │ │ -63 } │ │ │ │ │ -64 }; │ │ │ │ │ -65 │ │ │ │ │ -69 template │ │ │ │ │ -70 std::list _p_r_e_d_e_c_e_s_s_o_r_M_a_p_2_K_e_y_s(const PredecessorMap& p_map); │ │ │ │ │ +30/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +31template │ │ │ │ │ +_3_2void _B_a_y_e_s_N_e_t_<_C_O_N_D_I_T_I_O_N_A_L_>_:_:_p_r_i_n_t(const std::string& s, │ │ │ │ │ +33 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter) const { │ │ │ │ │ +34 std::cout << (s.empty() ? "" : s + " ") << std::endl; │ │ │ │ │ +35 std::cout << "size: " << this->size() << std::endl; │ │ │ │ │ +36 for (size_t i = 0; i < this->size(); i++) { │ │ │ │ │ +37 const auto& conditional = this->at(i); │ │ │ │ │ +38 std::stringstream ss; │ │ │ │ │ +39 ss << "conditional " << i << ": "; │ │ │ │ │ +40 if (conditional) conditional->print(ss.str(), formatter); │ │ │ │ │ +41 } │ │ │ │ │ +42} │ │ │ │ │ +43 │ │ │ │ │ +44/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +45template │ │ │ │ │ +_4_6void _B_a_y_e_s_N_e_t_<_C_O_N_D_I_T_I_O_N_A_L_>_:_:_d_o_t(std::ostream& os, │ │ │ │ │ +47 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter, │ │ │ │ │ +48 const _D_o_t_W_r_i_t_e_r& writer) const { │ │ │ │ │ +49 writer._d_i_g_r_a_p_h_P_r_e_a_m_b_l_e(&os); │ │ │ │ │ +50 │ │ │ │ │ +51 // Create nodes for each variable in the graph │ │ │ │ │ +52 for (_K_e_y key : this->keys()) { │ │ │ │ │ +53 auto position = writer._v_a_r_i_a_b_l_e_P_o_s(key); │ │ │ │ │ +54 writer._d_r_a_w_V_a_r_i_a_b_l_e(key, keyFormatter, position, &os); │ │ │ │ │ +55 } │ │ │ │ │ +56 os << "\n"; │ │ │ │ │ +57 │ │ │ │ │ +58 // Reverse order as typically Bayes nets stored in reverse topological sort. │ │ │ │ │ +59 for (auto conditional : boost::adaptors::reverse(*this)) { │ │ │ │ │ +60 auto frontals = conditional->frontals(); │ │ │ │ │ +61 const _K_e_y me = frontals.front(); │ │ │ │ │ +62 auto parents = conditional->parents(); │ │ │ │ │ +63 for (const _K_e_y& p : parents) { │ │ │ │ │ +64 os << " var" << p << "->var" << me << "\n"; │ │ │ │ │ +65 } │ │ │ │ │ +66 } │ │ │ │ │ +67 │ │ │ │ │ +68 os << "}"; │ │ │ │ │ +69 std::flush(os); │ │ │ │ │ +_7_0} │ │ │ │ │ 71 │ │ │ │ │ -78 template SDGraph _t_o_B_o_o_s_t_G_r_a_p_h(const G& │ │ │ │ │ -graph); │ │ │ │ │ -79 │ │ │ │ │ -85 template │ │ │ │ │ -86 boost::tuple > _p_r_e_d_e_c_e_s_s_o_r_M_a_p_2_G_r_a_p_h(const │ │ │ │ │ -PredecessorMap& p_map); │ │ │ │ │ -87 │ │ │ │ │ -91 template │ │ │ │ │ -92 boost::shared_ptr │ │ │ │ │ -93 _c_o_m_p_o_s_e_P_o_s_e_s(const G& graph, const PredecessorMap& tree, const POSE& │ │ │ │ │ -rootPose); │ │ │ │ │ -94 │ │ │ │ │ -95 │ │ │ │ │ -99 template │ │ │ │ │ -100 PredecessorMap _f_i_n_d_M_i_n_i_m_u_m_S_p_a_n_n_i_n_g_T_r_e_e(const G& g) ; │ │ │ │ │ -101 │ │ │ │ │ -106 template │ │ │ │ │ -107 void _s_p_l_i_t(const G& g, const PredecessorMap& tree, G& Ab1, G& Ab2) ; │ │ │ │ │ +72/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +73template │ │ │ │ │ +_7_4std::string _B_a_y_e_s_N_e_t_<_C_O_N_D_I_T_I_O_N_A_L_>_:_:_d_o_t(const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter, │ │ │ │ │ +75 const _D_o_t_W_r_i_t_e_r& writer) const { │ │ │ │ │ +76 std::stringstream ss; │ │ │ │ │ +77 _d_o_t(ss, keyFormatter, writer); │ │ │ │ │ +78 return ss.str(); │ │ │ │ │ +_7_9} │ │ │ │ │ +80 │ │ │ │ │ +81/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +82template │ │ │ │ │ +_8_3void _B_a_y_e_s_N_e_t_<_C_O_N_D_I_T_I_O_N_A_L_>_:_:_s_a_v_e_G_r_a_p_h(const std::string& filename, │ │ │ │ │ +_8_4 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter, │ │ │ │ │ +85 const _D_o_t_W_r_i_t_e_r& writer) const { │ │ │ │ │ +86 std::ofstream of(filename.c_str()); │ │ │ │ │ +87 _d_o_t(of, keyFormatter, writer); │ │ │ │ │ +_8_8 of.close(); │ │ │ │ │ +89} │ │ │ │ │ +90 │ │ │ │ │ +91/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +92template │ │ │ │ │ +93double _B_a_y_e_s_N_e_t_<_C_O_N_D_I_T_I_O_N_A_L_>_:_:_l_o_g_P_r_o_b_a_b_i_l_i_t_y(const _H_y_b_r_i_d_V_a_l_u_e_s& x) const { │ │ │ │ │ +94 double sum = 0.; │ │ │ │ │ +95 for (const auto& gc : *this) { │ │ │ │ │ +96 if (gc) sum += gc->logProbability(x); │ │ │ │ │ +97 } │ │ │ │ │ +98 return sum; │ │ │ │ │ +99} │ │ │ │ │ +100 │ │ │ │ │ +101/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +102template │ │ │ │ │ +103double BayesNet::evaluate(const _H_y_b_r_i_d_V_a_l_u_e_s& x) const { │ │ │ │ │ +104 return exp(-logProbability(x)); │ │ │ │ │ +105} │ │ │ │ │ +106 │ │ │ │ │ +107/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ 108 │ │ │ │ │ -109 │ │ │ │ │ -110} // namespace gtsam │ │ │ │ │ -111 │ │ │ │ │ -112#include │ │ │ │ │ -_V_a_l_u_e_s_._h │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +109} // namespace gtsam │ │ │ │ │ +_F_a_c_t_o_r_G_r_a_p_h_-_i_n_s_t_._h │ │ │ │ │ +Factor Graph Base Class. │ │ │ │ │ +_B_a_y_e_s_N_e_t_._h │ │ │ │ │ +Bayes network. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_e_d_e_c_e_s_s_o_r_M_a_p_2_K_e_y_s │ │ │ │ │ -std::list< KEY > predecessorMap2Keys(const PredecessorMap< KEY > &p_map) │ │ │ │ │ -Generate a list of keys from a spanning tree represented by its predecessor │ │ │ │ │ -map. │ │ │ │ │ -DDeeffiinniittiioonn graph-inl.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_s_p_l_i_t │ │ │ │ │ -void split(const G &g, const PredecessorMap< KEY > &tree, G &Ab1, G &Ab2) │ │ │ │ │ -Split the graph into two parts: one corresponds to the given spanning tree, and │ │ │ │ │ -the other corresponds... │ │ │ │ │ -DDeeffiinniittiioonn graph-inl.h:255 │ │ │ │ │ -_g_t_s_a_m_:_:_c_o_m_p_o_s_e_P_o_s_e_s │ │ │ │ │ -boost::shared_ptr< Values > composePoses(const G &graph, const PredecessorMap< │ │ │ │ │ -KEY > &tree, const POSE &rootPose) │ │ │ │ │ -Compose the poses by following the chain specified by the spanning tree. │ │ │ │ │ -DDeeffiinniittiioonn graph-inl.h:174 │ │ │ │ │ -_g_t_s_a_m_:_:_t_o_B_o_o_s_t_G_r_a_p_h │ │ │ │ │ -SDGraph< KEY > toBoostGraph(const G &graph) │ │ │ │ │ -Convert the factor graph to an SDGraph G = Graph type F = Factor type Key = Key │ │ │ │ │ -type. │ │ │ │ │ -DDeeffiinniittiioonn graph-inl.h:68 │ │ │ │ │ -_g_t_s_a_m_:_:_f_i_n_d_M_i_n_i_m_u_m_S_p_a_n_n_i_n_g_T_r_e_e │ │ │ │ │ -PredecessorMap< KEY > findMinimumSpanningTree(const G &fg) │ │ │ │ │ -find the minimum spanning tree using boost graph library │ │ │ │ │ -DDeeffiinniittiioonn graph-inl.h:232 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_e_d_e_c_e_s_s_o_r_M_a_p_2_G_r_a_p_h │ │ │ │ │ -boost::tuple< G, V, std::map< KEY, V > > predecessorMap2Graph(const │ │ │ │ │ -PredecessorMap< KEY > &p_map) │ │ │ │ │ -Build takes a predecessor map, and builds a directed graph corresponding to the │ │ │ │ │ -tree. │ │ │ │ │ -DDeeffiinniittiioonn graph-inl.h:118 │ │ │ │ │ -_g_t_s_a_m_:_:_S_D_G_r_a_p_h │ │ │ │ │ -SDGraph is undirected graph with variable keys and double edge weights. │ │ │ │ │ -DDeeffiinniittiioonn graph.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_S_G_r_a_p_h │ │ │ │ │ -DDeeffiinniittiioonn graph.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_d_e_c_e_s_s_o_r_M_a_p │ │ │ │ │ -Map from variable key to parent key. │ │ │ │ │ -DDeeffiinniittiioonn graph.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_d_e_c_e_s_s_o_r_M_a_p_:_:_i_n_s_e_r_t │ │ │ │ │ -void insert(const KEY &key, const KEY &parent) │ │ │ │ │ -convenience insert so we can pass ints for TypedSymbol keys │ │ │ │ │ -DDeeffiinniittiioonn graph.h:61 │ │ │ │ │ +_g_t_s_a_m_:_:_d_o_t │ │ │ │ │ +double dot(const V1 &a, const V2 &b) │ │ │ │ │ +Dot product. │ │ │ │ │ +DDeeffiinniittiioonn Vector.h:195 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t │ │ │ │ │ +A BayesNet is a tree of conditionals, stored in elimination order. │ │ │ │ │ +DDeeffiinniittiioonn BayesNet.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="BayesNet", const KeyFormatter │ │ │ │ │ +&formatter=DefaultKeyFormatter) const override │ │ │ │ │ +print out graph │ │ │ │ │ +DDeeffiinniittiioonn BayesNet-inst.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t_:_:_d_o_t │ │ │ │ │ +void dot(std::ostream &os, const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const │ │ │ │ │ +Output to graphviz format, stream version. │ │ │ │ │ +DDeeffiinniittiioonn BayesNet-inst.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t_:_:_s_a_v_e_G_r_a_p_h │ │ │ │ │ +void saveGraph(const std::string &filename, const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const │ │ │ │ │ +output to file with graphviz format. │ │ │ │ │ +DDeeffiinniittiioonn BayesNet-inst.h:83 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r │ │ │ │ │ +DotWriter is a helper class for writing graphviz .dot files. │ │ │ │ │ +DDeeffiinniittiioonn DotWriter.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_d_r_a_w_V_a_r_i_a_b_l_e │ │ │ │ │ +void drawVariable(Key key, const KeyFormatter &keyFormatter, const boost:: │ │ │ │ │ +optional< Vector2 > &position, std::ostream *os) const │ │ │ │ │ +Create a variable dot fragment. │ │ │ │ │ +DDeeffiinniittiioonn DotWriter.cpp:42 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_d_i_g_r_a_p_h_P_r_e_a_m_b_l_e │ │ │ │ │ +void digraphPreamble(std::ostream *os) const │ │ │ │ │ +Write out preamble for digraph, including size. │ │ │ │ │ +DDeeffiinniittiioonn DotWriter.cpp:36 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_v_a_r_i_a_b_l_e_P_o_s │ │ │ │ │ +boost::optional< Vector2 > variablePos(Key key) const │ │ │ │ │ +Return variable position or none. │ │ │ │ │ +DDeeffiinniittiioonn DotWriter.cpp:79 │ │ │ │ │ +_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +the error. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _g_r_a_p_h_._h │ │ │ │ │ + * BBaayyeessNNeett--iinnsstt..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00575.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTree.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/VariableSlots.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,51 +96,46 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
BayesTree.h File Reference
│ │ │ │ +
VariableSlots.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ +

VariableSlots describes the structure of a combined factor in terms of where each block comes from in the source factors. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

struct  gtsam::BayesTreeCliqueStats
 clique statistics More...
class  gtsam::VariableSlots
 A combined factor is assembled as one block of rows for each component factor. More...
 
struct  gtsam::BayesTreeCliqueData
 store all the sizes
│ │ │ │ - More...
 
class  gtsam::BayesTree< CLIQUE >
 Bayes tree. More...
 
class  gtsam::BayesTreeOrphanWrapper< CLIQUE, typename >
struct  gtsam::traits< VariableSlots >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Bayes Tree is a tree of cliques of a Bayes Chain.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +

VariableSlots describes the structure of a combined factor in terms of where each block comes from in the source factors.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Oct 4, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,34 +1,33 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -BayesTree.h File Reference │ │ │ │ │ -Bayes Tree is a tree of cliques of a Bayes Chain. _M_o_r_e_._._. │ │ │ │ │ +VariableSlots.h File Reference │ │ │ │ │ +VariableSlots describes the structure of a combined factor in terms of where │ │ │ │ │ +each block comes from in the source factors. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_S_t_a_t_s │ │ │ │ │ -  clique statistics _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_S_l_o_t_s │ │ │ │ │ +  A combined factor is assembled as one block of rows for each component │ │ │ │ │ + factor. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_D_a_t_a │ │ │ │ │ -  store all the sizes │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_<_ _C_L_I_Q_U_E_ _> │ │ │ │ │ -  Bayes tree. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r_<_ _C_L_I_Q_U_E_,_ _t_y_p_e_n_a_m_e_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _V_a_r_i_a_b_l_e_S_l_o_t_s_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ │ +VariableSlots describes the structure of a combined factor in terms of where │ │ │ │ │ +each block comes from in the source factors. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Oct 4, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ + * _V_a_r_i_a_b_l_e_S_l_o_t_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00575.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,5 +1,4 @@ │ │ │ │ │ var a00575 = [ │ │ │ │ │ - ["gtsam::BayesTreeCliqueStats", "a03532.html", null], │ │ │ │ │ - ["gtsam::BayesTreeCliqueData", "a03536.html", null], │ │ │ │ │ - ["gtsam::BayesTreeOrphanWrapper< CLIQUE, typename >", "a03544.html", "a03544"] │ │ │ │ │ + ["gtsam::VariableSlots", "a03708.html", "a03708"], │ │ │ │ │ + ["gtsam::traits< VariableSlots >", "a03712.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00575_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTree.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/VariableSlots.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,295 +98,126 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
BayesTree.h
│ │ │ │ +
VariableSlots.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18// \callgraph
│ │ │ │ -
19
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <boost/shared_ptr.hpp>
│ │ │ │ -
23
│ │ │ │ -
24#include <gtsam/inference/Key.h>
│ │ │ │ -
25#include <gtsam/base/FastList.h>
│ │ │ │ -
26#include <gtsam/base/ConcurrentMap.h>
│ │ │ │ -
27#include <gtsam/base/FastVector.h>
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/global_includes.h>
│ │ │ │ +
22#include <gtsam/base/FastMap.h>
│ │ │ │ +
23#include <gtsam/base/FastVector.h>
│ │ │ │ +
24#include <gtsam/base/timing.h>
│ │ │ │ +
25#include <gtsam/base/Testable.h>
│ │ │ │ +
26
│ │ │ │ +
27#include <boost/tuple/tuple.hpp>
│ │ │ │
28
│ │ │ │ -
29#include <string>
│ │ │ │ -
30
│ │ │ │ -
31namespace gtsam {
│ │ │ │ -
32
│ │ │ │ -
33 // Forward declarations
│ │ │ │ -
34 template<class FACTOR> class FactorGraph;
│ │ │ │ -
35 template<class BAYESTREE, class GRAPH> class EliminatableClusterTree;
│ │ │ │ -
36
│ │ │ │ -
37 /* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
39 struct GTSAM_EXPORT BayesTreeCliqueStats {
│ │ │ │ -
40 double avgConditionalSize;
│ │ │ │ -
41 std::size_t maxConditionalSize;
│ │ │ │ -
42 double avgSeparatorSize;
│ │ │ │ -
43 std::size_t maxSeparatorSize;
│ │ │ │ -
44 void print(const std::string& s = "") const ;
│ │ │ │ -
45 };
│ │ │ │ -
│ │ │ │ -
46
│ │ │ │ -
│ │ │ │ -
48 struct GTSAM_EXPORT BayesTreeCliqueData {
│ │ │ │ -
49 FastVector<std::size_t> conditionalSizes;
│ │ │ │ -
50 FastVector<std::size_t> separatorSizes;
│ │ │ │ -
51 BayesTreeCliqueStats getStats() const;
│ │ │ │ -
52 };
│ │ │ │ -
│ │ │ │ +
29#include <iostream>
│ │ │ │ +
30#include <string>
│ │ │ │ +
31
│ │ │ │ +
32namespace gtsam {
│ │ │ │ +
33
│ │ │ │ +
│ │ │ │ +
52class VariableSlots : public FastMap<Key, FastVector<size_t> > {
│ │ │ │
53
│ │ │ │ -
54 /* ************************************************************************* */
│ │ │ │ -
65 template<class CLIQUE>
│ │ │ │ -
│ │ │ │ -
66 class BayesTree
│ │ │ │ -
67 {
│ │ │ │ -
68 protected:
│ │ │ │ -
69 typedef BayesTree<CLIQUE> This;
│ │ │ │ -
70 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
54public:
│ │ │ │ +
55
│ │ │ │ +
56 typedef FastMap<Key, FastVector<size_t> > Base;
│ │ │ │ +
57 GTSAM_EXPORT static const size_t Empty;
│ │ │ │ +
58
│ │ │ │ +
61
│ │ │ │ +
67 template<class FG>
│ │ │ │ +
68 VariableSlots(const FG& factorGraph);
│ │ │ │ +
69
│ │ │ │
71
│ │ │ │ -
72 public:
│ │ │ │ -
73 typedef CLIQUE Clique;
│ │ │ │ -
74 typedef boost::shared_ptr<Clique> sharedClique;
│ │ │ │ -
75 typedef Clique Node;
│ │ │ │ -
76 typedef sharedClique sharedNode;
│ │ │ │ -
77 typedef typename CLIQUE::ConditionalType ConditionalType;
│ │ │ │ -
78 typedef boost::shared_ptr<ConditionalType> sharedConditional;
│ │ │ │ -
79 typedef typename CLIQUE::BayesNetType BayesNetType;
│ │ │ │ -
80 typedef boost::shared_ptr<BayesNetType> sharedBayesNet;
│ │ │ │ -
81 typedef typename CLIQUE::FactorType FactorType;
│ │ │ │ -
82 typedef boost::shared_ptr<FactorType> sharedFactor;
│ │ │ │ -
83 typedef typename CLIQUE::FactorGraphType FactorGraphType;
│ │ │ │ -
84 typedef boost::shared_ptr<FactorGraphType> sharedFactorGraph;
│ │ │ │ -
85 typedef typename FactorGraphType::Eliminate Eliminate;
│ │ │ │ -
86 typedef typename CLIQUE::EliminationTraitsType EliminationTraitsType;
│ │ │ │ -
87
│ │ │ │ -
89 typedef FastList<sharedClique> Cliques;
│ │ │ │ -
90
│ │ │ │ -
92 typedef ConcurrentMap<Key, sharedClique> Nodes;
│ │ │ │ +
74
│ │ │ │ +
76 GTSAM_EXPORT void print(const std::string& str = "VariableSlots: ") const;
│ │ │ │ +
77
│ │ │ │ +
79 GTSAM_EXPORT bool equals(const VariableSlots& rhs, double tol = 0.0) const;
│ │ │ │ +
80
│ │ │ │ +
82};
│ │ │ │ +
│ │ │ │ +
83
│ │ │ │ +
85template<> struct traits<VariableSlots> : public Testable<VariableSlots> {};
│ │ │ │ +
86
│ │ │ │ +
87/* ************************************************************************* */
│ │ │ │ +
88template<class FG>
│ │ │ │ +
│ │ │ │ +
89VariableSlots::VariableSlots(const FG& factorGraph)
│ │ │ │ +
90{
│ │ │ │ +
91 gttic(VariableSlots_constructor);
│ │ │ │ +
92 static const bool debug = false;
│ │ │ │
93
│ │ │ │ -
95 typedef FastVector<sharedClique> Roots;
│ │ │ │ -
96
│ │ │ │ -
97 protected:
│ │ │ │ -
98
│ │ │ │ -
100 Nodes nodes_;
│ │ │ │ -
101
│ │ │ │ -
103 Roots roots_;
│ │ │ │ -
104
│ │ │ │ -
107
│ │ │ │ -
109 BayesTree() {}
│ │ │ │ -
110
│ │ │ │ -
112 BayesTree(const This& other);
│ │ │ │ -
113
│ │ │ │ -
115
│ │ │ │ -
117 This& operator=(const This& other);
│ │ │ │ -
118
│ │ │ │ -
121
│ │ │ │ -
123 bool equals(const This& other, double tol = 1e-9) const;
│ │ │ │ -
124
│ │ │ │ -
125 public:
│ │ │ │ -
127 void print(const std::string& s = "",
│ │ │ │ -
128 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ -
130
│ │ │ │ -
133
│ │ │ │ -
135 size_t size() const;
│ │ │ │ -
136
│ │ │ │ -
│ │ │ │ -
138 inline bool empty() const {
│ │ │ │ -
139 return nodes_.empty();
│ │ │ │ -
140 }
│ │ │ │ -
│ │ │ │ -
141
│ │ │ │ -
143 const Nodes& nodes() const { return nodes_; }
│ │ │ │ -
144
│ │ │ │ -
146 sharedClique operator[](Key j) const { return nodes_.at(j); }
│ │ │ │ -
147
│ │ │ │ -
149 const Roots& roots() const { return roots_; }
│ │ │ │ -
150
│ │ │ │ -
│ │ │ │ -
152 const sharedClique& clique(Key j) const {
│ │ │ │ -
153 typename Nodes::const_iterator c = nodes_.find(j);
│ │ │ │ -
154 if(c == nodes_.end())
│ │ │ │ -
155 throw std::out_of_range("Requested the BayesTree clique for a key that is not in the BayesTree");
│ │ │ │ -
156 else
│ │ │ │ -
157 return c->second;
│ │ │ │ -
158 }
│ │ │ │ +
94 // Compute a mapping (called variableSlots) *from* each involved
│ │ │ │ +
95 // variable that will be in the new joint factor *to* the slot in each
│ │ │ │ +
96 // removed factor in which that variable appears. For each variable,
│ │ │ │ +
97 // this is stored as a vector of slot numbers, stored in order of the
│ │ │ │ +
98 // removed factors. The slot number is the max integer value if the
│ │ │ │ +
99 // factor does not involve that variable.
│ │ │ │ +
100 size_t jointFactorPos = 0;
│ │ │ │ +
101 for(const typename FG::sharedFactor& factor: factorGraph) {
│ │ │ │ +
102 if (!factor) {
│ │ │ │ +
103 continue;
│ │ │ │ +
104 }
│ │ │ │ +
105 size_t factorVarSlot = 0;
│ │ │ │ +
106 for(const Key involvedVariable: *factor) {
│ │ │ │ +
107 // Set the slot in this factor for this variable. If the
│ │ │ │ +
108 // variable was not already discovered, create an array for it
│ │ │ │ +
109 // that we'll fill with the slot indices for each factor that
│ │ │ │ +
110 // we're combining. Initially we put the max integer value in
│ │ │ │ +
111 // the array entry for each factor that will indicate the factor
│ │ │ │ +
112 // does not involve the variable.
│ │ │ │ +
113 iterator thisVarSlots; bool inserted;
│ │ │ │ +
114 boost::tie(thisVarSlots, inserted) = this->insert(std::make_pair(involvedVariable, FastVector<size_t>()));
│ │ │ │ +
115 if(inserted)
│ │ │ │ +
116 thisVarSlots->second.resize(factorGraph.nrFactors(), Empty);
│ │ │ │ +
117 thisVarSlots->second[jointFactorPos] = factorVarSlot;
│ │ │ │ +
118 if(debug) std::cout << " var " << involvedVariable << " rowblock " << jointFactorPos << " comes from factor's slot " << factorVarSlot << std::endl;
│ │ │ │ +
119 ++ factorVarSlot;
│ │ │ │ +
120 }
│ │ │ │ +
121 ++ jointFactorPos;
│ │ │ │ +
122 }
│ │ │ │ +
123}
│ │ │ │
│ │ │ │ -
159
│ │ │ │ -
161 BayesTreeCliqueData getCliqueData() const;
│ │ │ │ -
162
│ │ │ │ -
164 size_t numCachedSeparatorMarginals() const;
│ │ │ │ -
165
│ │ │ │ -
171 sharedConditional marginalFactor(Key j, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
│ │ │ │ -
172
│ │ │ │ -
177 sharedFactorGraph joint(Key j1, Key j2, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
│ │ │ │ -
178
│ │ │ │ -
183 sharedBayesNet jointBayesNet(Key j1, Key j2, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
│ │ │ │ -
184
│ │ │ │ -
187
│ │ │ │ -
189 void dot(std::ostream& os, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ -
190
│ │ │ │ -
192 std::string dot(
│ │ │ │ -
193 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ -
194
│ │ │ │ -
196 void saveGraph(const std::string& filename,
│ │ │ │ -
197 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ -
198
│ │ │ │ -
202
│ │ │ │ -
207 template<class CONTAINER>
│ │ │ │ -
208 Key findParentClique(const CONTAINER& parents) const;
│ │ │ │ -
209
│ │ │ │ -
211 void clear();
│ │ │ │ -
212
│ │ │ │ -
214 void deleteCachedShortcuts();
│ │ │ │ -
215
│ │ │ │ -
220 void removePath(sharedClique clique, BayesNetType* bn, Cliques* orphans);
│ │ │ │ -
221
│ │ │ │ -
226 void removeTop(const KeyVector& keys, BayesNetType* bn, Cliques* orphans);
│ │ │ │ -
227
│ │ │ │ -
230 Cliques removeSubtree(const sharedClique& subtree);
│ │ │ │ -
231
│ │ │ │ -
235 void insertRoot(const sharedClique& subtree);
│ │ │ │ -
236
│ │ │ │ -
238 void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique());
│ │ │ │ -
239
│ │ │ │ -
241 void addFactorsToGraph(FactorGraph<FactorType>* graph) const;
│ │ │ │ -
242
│ │ │ │ -
243 protected:
│ │ │ │ -
244
│ │ │ │ -
246 void dot(std::ostream &s, sharedClique clique, const KeyFormatter& keyFormatter,
│ │ │ │ -
247 int parentnum = 0) const;
│ │ │ │ -
248
│ │ │ │ -
250 void getCliqueData(sharedClique clique, BayesTreeCliqueData* stats) const;
│ │ │ │ -
251
│ │ │ │ -
253 void removeClique(sharedClique clique);
│ │ │ │ -
254
│ │ │ │ -
256 void fillNodesIndex(const sharedClique& subtree);
│ │ │ │ -
257
│ │ │ │ -
258 // Friend JunctionTree because it directly fills roots and nodes index.
│ │ │ │ -
259 template<class BAYESTREE, class GRAPH> friend class EliminatableClusterTree;
│ │ │ │ -
260
│ │ │ │ -
261 private:
│ │ │ │ -
263 friend class boost::serialization::access;
│ │ │ │ -
264 template<class ARCHIVE>
│ │ │ │ -
265 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
266 ar & BOOST_SERIALIZATION_NVP(nodes_);
│ │ │ │ -
267 ar & BOOST_SERIALIZATION_NVP(roots_);
│ │ │ │ -
268 }
│ │ │ │ -
269
│ │ │ │ -
271
│ │ │ │ -
272 }; // BayesTree
│ │ │ │ -
│ │ │ │ -
273
│ │ │ │ -
274 /* ************************************************************************* */
│ │ │ │ -
275 template <class CLIQUE, typename = void>
│ │ │ │ -
│ │ │ │ -
276 class BayesTreeOrphanWrapper : public CLIQUE::ConditionalType {
│ │ │ │ -
277 public:
│ │ │ │ -
278 typedef CLIQUE CliqueType;
│ │ │ │ -
279 typedef typename CLIQUE::ConditionalType Base;
│ │ │ │ -
280
│ │ │ │ -
281 boost::shared_ptr<CliqueType> clique;
│ │ │ │ -
282
│ │ │ │ -
│ │ │ │ -
293 BayesTreeOrphanWrapper(const boost::shared_ptr<CliqueType>& clique)
│ │ │ │ -
294 : clique(clique) {
│ │ │ │ -
295 this->keys_.assign(clique->conditional()->beginParents(),
│ │ │ │ -
296 clique->conditional()->endParents());
│ │ │ │ -
297 }
│ │ │ │ -
│ │ │ │ -
298
│ │ │ │ -
299 void print(
│ │ │ │ -
300 const std::string& s = "",
│ │ │ │ -
301 const KeyFormatter& formatter = DefaultKeyFormatter) const override {
│ │ │ │ -
302 clique->print(s + "stored clique", formatter);
│ │ │ │ -
303 }
│ │ │ │ -
304 };
│ │ │ │ -
│ │ │ │ -
305
│ │ │ │ -
306}
│ │ │ │ -
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ -
FastList.h
A thin wrapper around std::list that uses boost's fast_pool_allocator.
│ │ │ │ - │ │ │ │ +
124
│ │ │ │ +
125}
│ │ │ │ +
FastMap.h
A thin wrapper around std::map that uses boost's fast_pool_allocator.
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ +
timing.h
Timing utilities.
│ │ │ │ +
global_includes.h
Included from all GTSAM files.
│ │ │ │
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::ConcurrentMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition ConcurrentMap.h:68
│ │ │ │ -
gtsam::FastList
FastList is a thin wrapper around std::list that uses the boost fast_pool_allocator instead of the de...
Definition FastList.h:40
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ -
gtsam::EliminatableClusterTree
A cluster-tree that eliminates to a Bayes tree.
Definition ClusterTree.h:184
│ │ │ │ -
gtsam::BayesTreeCliqueStats
clique statistics
Definition BayesTree.h:39
│ │ │ │ -
gtsam::BayesTreeCliqueData
store all the sizes
Definition BayesTree.h:48
│ │ │ │ -
gtsam::BayesTree
Bayes tree.
Definition BayesTree.h:67
│ │ │ │ -
gtsam::BayesTree::nodes_
Nodes nodes_
Map from indices to Clique.
Definition BayesTree.h:100
│ │ │ │ -
gtsam::BayesTree::removeClique
void removeClique(sharedClique clique)
remove a clique: warning, can result in a forest
Definition BayesTree-inst.h:424
│ │ │ │ -
gtsam::BayesTree::joint
sharedFactorGraph joint(Key j1, Key j2, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
return joint on two variables Limitation: can only calculate joint if cliques are disjoint or one of ...
Definition BayesTree-inst.h:293
│ │ │ │ -
gtsam::BayesTree::fillNodesIndex
void fillNodesIndex(const sharedClique &subtree)
Fill the nodes index for a subtree.
Definition BayesTree-inst.h:246
│ │ │ │ -
gtsam::BayesTree::dot
void dot(std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Output to graphviz format, stream version.
Definition BayesTree-inst.h:64
│ │ │ │ -
gtsam::BayesTree::addFactorsToGraph
void addFactorsToGraph(FactorGraph< FactorType > *graph) const
Add all cliques in this BayesTree to the specified factor graph.
Definition BayesTree-inst.h:168
│ │ │ │ -
gtsam::BayesTree::operator[]
sharedClique operator[](Key j) const
Access node by variable.
Definition BayesTree.h:146
│ │ │ │ -
gtsam::BayesTree::operator=
This & operator=(const This &other)
Assignment operator.
Definition BayesTree-inst.h:199
│ │ │ │ -
gtsam::BayesTree::sharedClique
boost::shared_ptr< Clique > sharedClique
Shared pointer to a clique.
Definition BayesTree.h:74
│ │ │ │ -
gtsam::BayesTree::BayesTree
BayesTree()
Create an empty Bayes Tree.
Definition BayesTree.h:109
│ │ │ │ -
gtsam::BayesTree::Node
Clique Node
Synonym for Clique (TODO: remove)
Definition BayesTree.h:75
│ │ │ │ -
gtsam::BayesTree::clear
void clear()
Remove all nodes.
Definition BayesTree-inst.h:408
│ │ │ │ -
gtsam::BayesTree::roots_
Roots roots_
Root cliques.
Definition BayesTree.h:103
│ │ │ │ -
gtsam::BayesTree::addClique
void addClique(const sharedClique &clique, const sharedClique &parent_clique=sharedClique())
add a clique (top down)
Definition BayesTree-inst.h:142
│ │ │ │ -
gtsam::BayesTree::jointBayesNet
sharedBayesNet jointBayesNet(Key j1, Key j2, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
return joint on two variables as a BayesNet Limitation: can only calculate joint if cliques are disjo...
Definition BayesTree-inst.h:302
│ │ │ │ -
gtsam::BayesTree::sharedNode
sharedClique sharedNode
Synonym for sharedClique (TODO: remove)
Definition BayesTree.h:76
│ │ │ │ -
gtsam::BayesTree::findParentClique
Key findParentClique(const CONTAINER &parents) const
Find parent clique of a conditional.
Definition BayesTree-inst.h:238
│ │ │ │ -
gtsam::BayesTree::size
size_t size() const
number of cliques
Definition BayesTree-inst.h:133
│ │ │ │ -
gtsam::BayesTree::deleteCachedShortcuts
void deleteCachedShortcuts()
Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data.
Definition BayesTree-inst.h:416
│ │ │ │ -
gtsam::BayesTree::removePath
void removePath(sharedClique clique, BayesNetType *bn, Cliques *orphans)
Remove path from clique to root and return that path as factors plus a list of orphaned subtree roots...
Definition BayesTree-inst.h:448
│ │ │ │ -
gtsam::BayesTree::nodes
const Nodes & nodes() const
Return nodes.
Definition BayesTree.h:143
│ │ │ │ -
gtsam::BayesTree::Cliques
FastList< sharedClique > Cliques
A convenience class for a list of shared cliques.
Definition BayesTree.h:89
│ │ │ │ -
gtsam::BayesTree::Clique
CLIQUE Clique
The clique type, normally BayesTreeClique.
Definition BayesTree.h:73
│ │ │ │ -
gtsam::BayesTree::marginalFactor
sharedConditional marginalFactor(Key j, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
Return marginal on any variable.
Definition BayesTree-inst.h:270
│ │ │ │ -
gtsam::BayesTree::roots
const Roots & roots() const
return root cliques
Definition BayesTree.h:149
│ │ │ │ -
gtsam::BayesTree::clique
const sharedClique & clique(Key j) const
alternate syntax for matlab: find the clique that contains the variable with Key j
Definition BayesTree.h:152
│ │ │ │ -
gtsam::BayesTree::Nodes
ConcurrentMap< Key, sharedClique > Nodes
Map from keys to Clique.
Definition BayesTree.h:92
│ │ │ │ -
gtsam::BayesTree::access
friend class boost::serialization::access
Serialization function.
Definition BayesTree.h:263
│ │ │ │ -
gtsam::BayesTree::numCachedSeparatorMarginals
size_t numCachedSeparatorMarginals() const
Collect number of cliques with cached separator marginals.
Definition BayesTree-inst.h:55
│ │ │ │ -
gtsam::BayesTree::getCliqueData
BayesTreeCliqueData getCliqueData() const
Gather data on all cliques.
Definition BayesTree-inst.h:35
│ │ │ │ -
gtsam::BayesTree::removeSubtree
Cliques removeSubtree(const sharedClique &subtree)
Remove the requested subtree.
Definition BayesTree-inst.h:496
│ │ │ │ -
gtsam::BayesTree::empty
bool empty() const
Check if there are any cliques in the tree.
Definition BayesTree.h:138
│ │ │ │ -
gtsam::BayesTree::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print
Definition BayesTree-inst.h:212
│ │ │ │ -
gtsam::BayesTree::insertRoot
void insertRoot(const sharedClique &subtree)
Insert a new subtree with known parent clique.
Definition BayesTree-inst.h:260
│ │ │ │ -
gtsam::BayesTree::saveGraph
void saveGraph(const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
output to file with graphviz format.
Definition BayesTree-inst.h:85
│ │ │ │ -
gtsam::BayesTree::removeTop
void removeTop(const KeyVector &keys, BayesNetType *bn, Cliques *orphans)
Given a list of indices, turn "contaminated" part of the tree back into a factor graph.
Definition BayesTree-inst.h:475
│ │ │ │ -
gtsam::BayesTree::Roots
FastVector< sharedClique > Roots
Root cliques.
Definition BayesTree.h:95
│ │ │ │ -
gtsam::BayesTreeOrphanWrapper
Definition BayesTree.h:276
│ │ │ │ -
gtsam::BayesTreeOrphanWrapper::BayesTreeOrphanWrapper
BayesTreeOrphanWrapper(const boost::shared_ptr< CliqueType > &clique)
Construct a new Bayes Tree Orphan Wrapper object.
Definition BayesTree.h:293
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::VariableSlots
A combined factor is assembled as one block of rows for each component factor.
Definition VariableSlots.h:52
│ │ │ │ +
gtsam::VariableSlots::print
GTSAM_EXPORT void print(const std::string &str="VariableSlots: ") const
print
Definition VariableSlots.cpp:29
│ │ │ │ +
gtsam::VariableSlots::VariableSlots
VariableSlots(const FG &factorGraph)
Constructor from a set of factors to be combined.
Definition VariableSlots.h:89
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,451 +1,149 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -BayesTree.h │ │ │ │ │ +VariableSlots.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18// \callgraph │ │ │ │ │ -19 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include │ │ │ │ │ -23 │ │ │ │ │ -24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_L_i_s_t_._h> │ │ │ │ │ -26#include │ │ │ │ │ -27#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_M_a_p_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_b_a_s_e_/_t_i_m_i_n_g_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +26 │ │ │ │ │ +27#include │ │ │ │ │ 28 │ │ │ │ │ -29#include │ │ │ │ │ -30 │ │ │ │ │ -31namespace _g_t_s_a_m { │ │ │ │ │ -32 │ │ │ │ │ -33 // Forward declarations │ │ │ │ │ -34 template class FactorGraph; │ │ │ │ │ -35 template class EliminatableClusterTree; │ │ │ │ │ -36 │ │ │ │ │ -37 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -_3_9 struct GTSAM_EXPORT _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_S_t_a_t_s { │ │ │ │ │ -40 double avgConditionalSize; │ │ │ │ │ -41 std::size_t maxConditionalSize; │ │ │ │ │ -42 double avgSeparatorSize; │ │ │ │ │ -43 std::size_t maxSeparatorSize; │ │ │ │ │ -44 void _p_r_i_n_t(const std::string& s = "") const ; │ │ │ │ │ -45 }; │ │ │ │ │ -46 │ │ │ │ │ -_4_8 struct GTSAM_EXPORT _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_D_a_t_a { │ │ │ │ │ -49 _F_a_s_t_V_e_c_t_o_r_<_s_t_d_:_:_s_i_z_e___t_> conditionalSizes; │ │ │ │ │ -50 _F_a_s_t_V_e_c_t_o_r_<_s_t_d_:_:_s_i_z_e___t_> separatorSizes; │ │ │ │ │ -51 _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_S_t_a_t_s getStats() const; │ │ │ │ │ -52 }; │ │ │ │ │ +29#include │ │ │ │ │ +30#include │ │ │ │ │ +31 │ │ │ │ │ +32namespace _g_t_s_a_m { │ │ │ │ │ +33 │ │ │ │ │ +_5_2class _V_a_r_i_a_b_l_e_S_l_o_t_s : public _F_a_s_t_M_a_p > { │ │ │ │ │ 53 │ │ │ │ │ -54 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -65 template │ │ │ │ │ -_6_6 class _B_a_y_e_s_T_r_e_e │ │ │ │ │ -67 { │ │ │ │ │ -68 protected: │ │ │ │ │ -69 typedef _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_> _T_h_i_s; │ │ │ │ │ -70 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +54public: │ │ │ │ │ +55 │ │ │ │ │ +56 typedef _F_a_s_t_M_a_p_<_K_e_y_,_ _F_a_s_t_V_e_c_t_o_r_<_s_i_z_e___t_> > _B_a_s_e; │ │ │ │ │ +57 GTSAM_EXPORT static const size_t Empty; │ │ │ │ │ +58 │ │ │ │ │ +61 │ │ │ │ │ +67 template │ │ │ │ │ +68 _V_a_r_i_a_b_l_e_S_l_o_t_s(const FG& factorGraph); │ │ │ │ │ +69 │ │ │ │ │ 71 │ │ │ │ │ -72 public: │ │ │ │ │ -_7_3 typedef CLIQUE _C_l_i_q_u_e; │ │ │ │ │ -_7_4 typedef boost::shared_ptr _s_h_a_r_e_d_C_l_i_q_u_e; │ │ │ │ │ -_7_5 typedef _C_l_i_q_u_e _N_o_d_e; │ │ │ │ │ -_7_6 typedef _s_h_a_r_e_d_C_l_i_q_u_e _s_h_a_r_e_d_N_o_d_e; │ │ │ │ │ -77 typedef typename CLIQUE::ConditionalType ConditionalType; │ │ │ │ │ -78 typedef boost::shared_ptr sharedConditional; │ │ │ │ │ -79 typedef typename CLIQUE::BayesNetType BayesNetType; │ │ │ │ │ -80 typedef boost::shared_ptr sharedBayesNet; │ │ │ │ │ -81 typedef typename CLIQUE::FactorType FactorType; │ │ │ │ │ -82 typedef boost::shared_ptr sharedFactor; │ │ │ │ │ -83 typedef typename CLIQUE::FactorGraphType FactorGraphType; │ │ │ │ │ -84 typedef boost::shared_ptr sharedFactorGraph; │ │ │ │ │ -85 typedef typename FactorGraphType::Eliminate Eliminate; │ │ │ │ │ -86 typedef typename CLIQUE::EliminationTraitsType EliminationTraitsType; │ │ │ │ │ -87 │ │ │ │ │ -_8_9 typedef _F_a_s_t_L_i_s_t_<_s_h_a_r_e_d_C_l_i_q_u_e_> _C_l_i_q_u_e_s; │ │ │ │ │ -90 │ │ │ │ │ -_9_2 typedef _C_o_n_c_u_r_r_e_n_t_M_a_p_<_K_e_y_,_ _s_h_a_r_e_d_C_l_i_q_u_e_> _N_o_d_e_s; │ │ │ │ │ +74 │ │ │ │ │ +76 GTSAM_EXPORT void _p_r_i_n_t(const std::string& str = "VariableSlots: ") const; │ │ │ │ │ +77 │ │ │ │ │ +79 GTSAM_EXPORT bool _e_q_u_a_l_s(const _V_a_r_i_a_b_l_e_S_l_o_t_s& rhs, double tol = 0.0) const; │ │ │ │ │ +80 │ │ │ │ │ +82}; │ │ │ │ │ +83 │ │ │ │ │ +_8_5template<> struct _t_r_a_i_t_s<_V_a_r_i_a_b_l_e_S_l_o_t_s> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ +86 │ │ │ │ │ +87/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +88template │ │ │ │ │ +_8_9_V_a_r_i_a_b_l_e_S_l_o_t_s_:_:_V_a_r_i_a_b_l_e_S_l_o_t_s(const FG& factorGraph) │ │ │ │ │ +90{ │ │ │ │ │ +91 gttic(VariableSlots_constructor); │ │ │ │ │ +92 static const bool debug = false; │ │ │ │ │ 93 │ │ │ │ │ -_9_5 typedef _F_a_s_t_V_e_c_t_o_r_<_s_h_a_r_e_d_C_l_i_q_u_e_> _R_o_o_t_s; │ │ │ │ │ -96 │ │ │ │ │ -97 protected: │ │ │ │ │ -98 │ │ │ │ │ -_1_0_0 _N_o_d_e_s _n_o_d_e_s__; │ │ │ │ │ -101 │ │ │ │ │ -_1_0_3 _R_o_o_t_s _r_o_o_t_s__; │ │ │ │ │ -104 │ │ │ │ │ -107 │ │ │ │ │ -_1_0_9 _B_a_y_e_s_T_r_e_e() {} │ │ │ │ │ -110 │ │ │ │ │ -112 _B_a_y_e_s_T_r_e_e(const This& other); │ │ │ │ │ -113 │ │ │ │ │ -115 │ │ │ │ │ -117 This& _o_p_e_r_a_t_o_r_=(const This& other); │ │ │ │ │ -118 │ │ │ │ │ -121 │ │ │ │ │ -123 bool _e_q_u_a_l_s(const This& other, double tol = 1e-9) const; │ │ │ │ │ +94 // Compute a mapping (called variableSlots) *from* each involved │ │ │ │ │ +95 // variable that will be in the new joint factor *to* the slot in each │ │ │ │ │ +96 // removed factor in which that variable appears. For each variable, │ │ │ │ │ +97 // this is stored as a vector of slot numbers, stored in order of the │ │ │ │ │ +98 // removed factors. The slot number is the max integer value if the │ │ │ │ │ +99 // factor does not involve that variable. │ │ │ │ │ +100 size_t jointFactorPos = 0; │ │ │ │ │ +101 for(const typename FG::sharedFactor& factor: factorGraph) { │ │ │ │ │ +102 if (!factor) { │ │ │ │ │ +103 continue; │ │ │ │ │ +104 } │ │ │ │ │ +105 size_t factorVarSlot = 0; │ │ │ │ │ +106 for(const _K_e_y involvedVariable: *factor) { │ │ │ │ │ +107 // Set the slot in this factor for this variable. If the │ │ │ │ │ +108 // variable was not already discovered, create an array for it │ │ │ │ │ +109 // that we'll fill with the slot indices for each factor that │ │ │ │ │ +110 // we're combining. Initially we put the max integer value in │ │ │ │ │ +111 // the array entry for each factor that will indicate the factor │ │ │ │ │ +112 // does not involve the variable. │ │ │ │ │ +113 iterator thisVarSlots; bool inserted; │ │ │ │ │ +114 boost::tie(thisVarSlots, inserted) = this->insert(std::make_pair │ │ │ │ │ +(involvedVariable, _F_a_s_t_V_e_c_t_o_r_<_s_i_z_e___t_>())); │ │ │ │ │ +115 if(inserted) │ │ │ │ │ +116 thisVarSlots->second.resize(factorGraph.nrFactors(), Empty); │ │ │ │ │ +117 thisVarSlots->second[jointFactorPos] = factorVarSlot; │ │ │ │ │ +118 if(debug) std::cout << " var " << involvedVariable << " rowblock " << │ │ │ │ │ +jointFactorPos << " comes from factor's slot " << factorVarSlot << std::endl; │ │ │ │ │ +119 ++ factorVarSlot; │ │ │ │ │ +120 } │ │ │ │ │ +121 ++ jointFactorPos; │ │ │ │ │ +122 } │ │ │ │ │ +123} │ │ │ │ │ 124 │ │ │ │ │ -125 public: │ │ │ │ │ -127 void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ -128 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const; │ │ │ │ │ -130 │ │ │ │ │ -133 │ │ │ │ │ -135 size_t _s_i_z_e() const; │ │ │ │ │ -136 │ │ │ │ │ -_1_3_8 inline bool _e_m_p_t_y() const { │ │ │ │ │ -139 return _n_o_d_e_s__.empty(); │ │ │ │ │ -140 } │ │ │ │ │ -141 │ │ │ │ │ -_1_4_3 const _N_o_d_e_s& _n_o_d_e_s() const { return _n_o_d_e_s__; } │ │ │ │ │ -144 │ │ │ │ │ -_1_4_6 _s_h_a_r_e_d_C_l_i_q_u_e _o_p_e_r_a_t_o_r_[_](_K_e_y j) const { return _n_o_d_e_s__.at(j); } │ │ │ │ │ -147 │ │ │ │ │ -_1_4_9 const _R_o_o_t_s& _r_o_o_t_s() const { return _r_o_o_t_s__; } │ │ │ │ │ -150 │ │ │ │ │ -_1_5_2 const _s_h_a_r_e_d_C_l_i_q_u_e& _c_l_i_q_u_e(_K_e_y j) const { │ │ │ │ │ -153 typename Nodes::const_iterator c = _n_o_d_e_s__.find(j); │ │ │ │ │ -154 if(c == _n_o_d_e_s__.end()) │ │ │ │ │ -155 throw std::out_of_range("Requested the BayesTree clique for a key that is │ │ │ │ │ -not in the BayesTree"); │ │ │ │ │ -156 else │ │ │ │ │ -157 return c->second; │ │ │ │ │ -158 } │ │ │ │ │ -159 │ │ │ │ │ -161 _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_D_a_t_a _g_e_t_C_l_i_q_u_e_D_a_t_a() const; │ │ │ │ │ -162 │ │ │ │ │ -164 size_t _n_u_m_C_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l_s() const; │ │ │ │ │ -165 │ │ │ │ │ -171 sharedConditional _m_a_r_g_i_n_a_l_F_a_c_t_o_r(_K_e_y j, const Eliminate& function = │ │ │ │ │ -EliminationTraitsType::DefaultEliminate) const; │ │ │ │ │ -172 │ │ │ │ │ -177 sharedFactorGraph _j_o_i_n_t(_K_e_y j1, _K_e_y j2, const Eliminate& function = │ │ │ │ │ -EliminationTraitsType::DefaultEliminate) const; │ │ │ │ │ -178 │ │ │ │ │ -183 sharedBayesNet _j_o_i_n_t_B_a_y_e_s_N_e_t(_K_e_y j1, _K_e_y j2, const Eliminate& function = │ │ │ │ │ -EliminationTraitsType::DefaultEliminate) const; │ │ │ │ │ -184 │ │ │ │ │ -187 │ │ │ │ │ -189 void _d_o_t(std::ostream& os, const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ -DefaultKeyFormatter) const; │ │ │ │ │ -190 │ │ │ │ │ -192 std::string _d_o_t( │ │ │ │ │ -193 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const; │ │ │ │ │ -194 │ │ │ │ │ -196 void _s_a_v_e_G_r_a_p_h(const std::string& filename, │ │ │ │ │ -197 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const; │ │ │ │ │ -198 │ │ │ │ │ -202 │ │ │ │ │ -207 template │ │ │ │ │ -208 _K_e_y _f_i_n_d_P_a_r_e_n_t_C_l_i_q_u_e(const CONTAINER& parents) const; │ │ │ │ │ -209 │ │ │ │ │ -211 void _c_l_e_a_r(); │ │ │ │ │ -212 │ │ │ │ │ -214 void _d_e_l_e_t_e_C_a_c_h_e_d_S_h_o_r_t_c_u_t_s(); │ │ │ │ │ -215 │ │ │ │ │ -220 void _r_e_m_o_v_e_P_a_t_h(_s_h_a_r_e_d_C_l_i_q_u_e _c_l_i_q_u_e, BayesNetType* bn, _C_l_i_q_u_e_s* orphans); │ │ │ │ │ -221 │ │ │ │ │ -226 void _r_e_m_o_v_e_T_o_p(const _K_e_y_V_e_c_t_o_r& keys, BayesNetType* bn, _C_l_i_q_u_e_s* orphans); │ │ │ │ │ -227 │ │ │ │ │ -230 _C_l_i_q_u_e_s _r_e_m_o_v_e_S_u_b_t_r_e_e(const _s_h_a_r_e_d_C_l_i_q_u_e& subtree); │ │ │ │ │ -231 │ │ │ │ │ -235 void _i_n_s_e_r_t_R_o_o_t(const _s_h_a_r_e_d_C_l_i_q_u_e& subtree); │ │ │ │ │ -236 │ │ │ │ │ -238 void _a_d_d_C_l_i_q_u_e(const _s_h_a_r_e_d_C_l_i_q_u_e& _c_l_i_q_u_e, const _s_h_a_r_e_d_C_l_i_q_u_e& │ │ │ │ │ -parent_clique = _s_h_a_r_e_d_C_l_i_q_u_e()); │ │ │ │ │ -239 │ │ │ │ │ -241 void _a_d_d_F_a_c_t_o_r_s_T_o_G_r_a_p_h(_F_a_c_t_o_r_G_r_a_p_h_<_F_a_c_t_o_r_T_y_p_e_>* graph) const; │ │ │ │ │ -242 │ │ │ │ │ -243 protected: │ │ │ │ │ -244 │ │ │ │ │ -246 void _d_o_t(std::ostream &s, _s_h_a_r_e_d_C_l_i_q_u_e _c_l_i_q_u_e, const _K_e_y_F_o_r_m_a_t_t_e_r& │ │ │ │ │ -keyFormatter, │ │ │ │ │ -247 int parentnum = 0) const; │ │ │ │ │ -248 │ │ │ │ │ -250 void _g_e_t_C_l_i_q_u_e_D_a_t_a(_s_h_a_r_e_d_C_l_i_q_u_e _c_l_i_q_u_e, _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_D_a_t_a* stats) const; │ │ │ │ │ -251 │ │ │ │ │ -253 void _r_e_m_o_v_e_C_l_i_q_u_e(_s_h_a_r_e_d_C_l_i_q_u_e _c_l_i_q_u_e); │ │ │ │ │ -254 │ │ │ │ │ -256 void _f_i_l_l_N_o_d_e_s_I_n_d_e_x(const _s_h_a_r_e_d_C_l_i_q_u_e& subtree); │ │ │ │ │ -257 │ │ │ │ │ -258 // Friend JunctionTree because it directly fills roots and nodes index. │ │ │ │ │ -259 template friend class │ │ │ │ │ -_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e; │ │ │ │ │ -260 │ │ │ │ │ -261 private: │ │ │ │ │ -_2_6_3 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -264 template │ │ │ │ │ -265 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -266 ar & BOOST_SERIALIZATION_NVP(_n_o_d_e_s__); │ │ │ │ │ -267 ar & BOOST_SERIALIZATION_NVP(_r_o_o_t_s__); │ │ │ │ │ -268 } │ │ │ │ │ -269 │ │ │ │ │ -271 │ │ │ │ │ -272 }; // BayesTree │ │ │ │ │ -273 │ │ │ │ │ -274 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -275 template │ │ │ │ │ -_2_7_6 class _B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r : public CLIQUE::ConditionalType { │ │ │ │ │ -277 public: │ │ │ │ │ -278 typedef CLIQUE CliqueType; │ │ │ │ │ -279 typedef typename CLIQUE::ConditionalType Base; │ │ │ │ │ -280 │ │ │ │ │ -281 boost::shared_ptr clique; │ │ │ │ │ -282 │ │ │ │ │ -_2_9_3 _B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r(const boost::shared_ptr& clique) │ │ │ │ │ -294 : clique(clique) { │ │ │ │ │ -295 this->keys_.assign(clique->conditional()->beginParents(), │ │ │ │ │ -296 clique->conditional()->endParents()); │ │ │ │ │ -297 } │ │ │ │ │ -298 │ │ │ │ │ -299 void print( │ │ │ │ │ -300 const std::string& s = "", │ │ │ │ │ -301 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override { │ │ │ │ │ -302 clique->print(s + "stored clique", formatter); │ │ │ │ │ -303 } │ │ │ │ │ -304 }; │ │ │ │ │ -305 │ │ │ │ │ -306} │ │ │ │ │ +125} │ │ │ │ │ +_F_a_s_t_M_a_p_._h │ │ │ │ │ +A thin wrapper around std::map that uses boost's fast_pool_allocator. │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ _F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ -_F_a_s_t_L_i_s_t_._h │ │ │ │ │ -A thin wrapper around std::list that uses boost's fast_pool_allocator. │ │ │ │ │ -_K_e_y_._h │ │ │ │ │ +_t_i_m_i_n_g_._h │ │ │ │ │ +Timing utilities. │ │ │ │ │ +_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ +Included from all GTSAM files. │ │ │ │ │ _g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ FastVector │ │ │ │ │ FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_c_u_r_r_e_n_t_M_a_p │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ fast_pool_allocator instead of the defa... │ │ │ │ │ -DDeeffiinniittiioonn ConcurrentMap.h:68 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t │ │ │ │ │ -FastList is a thin wrapper around std::list that uses the boost │ │ │ │ │ -fast_pool_allocator instead of the de... │ │ │ │ │ -DDeeffiinniittiioonn FastList.h:40 │ │ │ │ │ +DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ _g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ Template to create a binary predicate. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ -nodes. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e │ │ │ │ │ -A cluster-tree that eliminates to a Bayes tree. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:184 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_S_t_a_t_s │ │ │ │ │ -clique statistics │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_D_a_t_a │ │ │ │ │ -store all the sizes │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e │ │ │ │ │ -Bayes tree. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_n_o_d_e_s__ │ │ │ │ │ -Nodes nodes_ │ │ │ │ │ -Map from indices to Clique. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_r_e_m_o_v_e_C_l_i_q_u_e │ │ │ │ │ -void removeClique(sharedClique clique) │ │ │ │ │ -remove a clique: warning, can result in a forest │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:424 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_j_o_i_n_t │ │ │ │ │ -sharedFactorGraph joint(Key j1, Key j2, const Eliminate │ │ │ │ │ -&function=EliminationTraitsType::DefaultEliminate) const │ │ │ │ │ -return joint on two variables Limitation: can only calculate joint if cliques │ │ │ │ │ -are disjoint or one of ... │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:293 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_f_i_l_l_N_o_d_e_s_I_n_d_e_x │ │ │ │ │ -void fillNodesIndex(const sharedClique &subtree) │ │ │ │ │ -Fill the nodes index for a subtree. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:246 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_d_o_t │ │ │ │ │ -void dot(std::ostream &os, const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ -Output to graphviz format, stream version. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_a_d_d_F_a_c_t_o_r_s_T_o_G_r_a_p_h │ │ │ │ │ -void addFactorsToGraph(FactorGraph< FactorType > *graph) const │ │ │ │ │ -Add all cliques in this BayesTree to the specified factor graph. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:168 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_o_p_e_r_a_t_o_r_[_] │ │ │ │ │ -sharedClique operator[](Key j) const │ │ │ │ │ -Access node by variable. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:146 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ -This & operator=(const This &other) │ │ │ │ │ -Assignment operator. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:199 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_s_h_a_r_e_d_C_l_i_q_u_e │ │ │ │ │ -boost::shared_ptr< Clique > sharedClique │ │ │ │ │ -Shared pointer to a clique. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_B_a_y_e_s_T_r_e_e │ │ │ │ │ -BayesTree() │ │ │ │ │ -Create an empty Bayes Tree. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:109 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_N_o_d_e │ │ │ │ │ -Clique Node │ │ │ │ │ -Synonym for Clique (TODO: remove) │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_c_l_e_a_r │ │ │ │ │ -void clear() │ │ │ │ │ -Remove all nodes. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:408 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_r_o_o_t_s__ │ │ │ │ │ -Roots roots_ │ │ │ │ │ -Root cliques. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:103 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_a_d_d_C_l_i_q_u_e │ │ │ │ │ -void addClique(const sharedClique &clique, const sharedClique │ │ │ │ │ -&parent_clique=sharedClique()) │ │ │ │ │ -add a clique (top down) │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:142 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_j_o_i_n_t_B_a_y_e_s_N_e_t │ │ │ │ │ -sharedBayesNet jointBayesNet(Key j1, Key j2, const Eliminate │ │ │ │ │ -&function=EliminationTraitsType::DefaultEliminate) const │ │ │ │ │ -return joint on two variables as a BayesNet Limitation: can only calculate │ │ │ │ │ -joint if cliques are disjo... │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:302 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_s_h_a_r_e_d_N_o_d_e │ │ │ │ │ -sharedClique sharedNode │ │ │ │ │ -Synonym for sharedClique (TODO: remove) │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:76 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_f_i_n_d_P_a_r_e_n_t_C_l_i_q_u_e │ │ │ │ │ -Key findParentClique(const CONTAINER &parents) const │ │ │ │ │ -Find parent clique of a conditional. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:238 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_s_i_z_e │ │ │ │ │ -size_t size() const │ │ │ │ │ -number of cliques │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:133 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_d_e_l_e_t_e_C_a_c_h_e_d_S_h_o_r_t_c_u_t_s │ │ │ │ │ -void deleteCachedShortcuts() │ │ │ │ │ -Clear all shortcut caches - use before timing on marginal calculation to avoid │ │ │ │ │ -residual cache data. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:416 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_r_e_m_o_v_e_P_a_t_h │ │ │ │ │ -void removePath(sharedClique clique, BayesNetType *bn, Cliques *orphans) │ │ │ │ │ -Remove path from clique to root and return that path as factors plus a list of │ │ │ │ │ -orphaned subtree roots... │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:448 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_n_o_d_e_s │ │ │ │ │ -const Nodes & nodes() const │ │ │ │ │ -Return nodes. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:143 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_C_l_i_q_u_e_s │ │ │ │ │ -FastList< sharedClique > Cliques │ │ │ │ │ -A convenience class for a list of shared cliques. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:89 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_C_l_i_q_u_e │ │ │ │ │ -CLIQUE Clique │ │ │ │ │ -The clique type, normally BayesTreeClique. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:73 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_m_a_r_g_i_n_a_l_F_a_c_t_o_r │ │ │ │ │ -sharedConditional marginalFactor(Key j, const Eliminate │ │ │ │ │ -&function=EliminationTraitsType::DefaultEliminate) const │ │ │ │ │ -Return marginal on any variable. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:270 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_r_o_o_t_s │ │ │ │ │ -const Roots & roots() const │ │ │ │ │ -return root cliques │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:149 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_c_l_i_q_u_e │ │ │ │ │ -const sharedClique & clique(Key j) const │ │ │ │ │ -alternate syntax for matlab: find the clique that contains the variable with │ │ │ │ │ -Key j │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:152 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_N_o_d_e_s │ │ │ │ │ -ConcurrentMap< Key, sharedClique > Nodes │ │ │ │ │ -Map from keys to Clique. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:92 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:263 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_n_u_m_C_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l_s │ │ │ │ │ -size_t numCachedSeparatorMarginals() const │ │ │ │ │ -Collect number of cliques with cached separator marginals. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_g_e_t_C_l_i_q_u_e_D_a_t_a │ │ │ │ │ -BayesTreeCliqueData getCliqueData() const │ │ │ │ │ -Gather data on all cliques. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_r_e_m_o_v_e_S_u_b_t_r_e_e │ │ │ │ │ -Cliques removeSubtree(const sharedClique &subtree) │ │ │ │ │ -Remove the requested subtree. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:496 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_e_m_p_t_y │ │ │ │ │ -bool empty() const │ │ │ │ │ -Check if there are any cliques in the tree. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:138 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_S_l_o_t_s │ │ │ │ │ +A combined factor is assembled as one block of rows for each component factor. │ │ │ │ │ +DDeeffiinniittiioonn VariableSlots.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_S_l_o_t_s_:_:_p_r_i_n_t │ │ │ │ │ +GTSAM_EXPORT void print(const std::string &str="VariableSlots: ") const │ │ │ │ │ print │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:212 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_i_n_s_e_r_t_R_o_o_t │ │ │ │ │ -void insertRoot(const sharedClique &subtree) │ │ │ │ │ -Insert a new subtree with known parent clique. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:260 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_s_a_v_e_G_r_a_p_h │ │ │ │ │ -void saveGraph(const std::string &filename, const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ -output to file with graphviz format. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_r_e_m_o_v_e_T_o_p │ │ │ │ │ -void removeTop(const KeyVector &keys, BayesNetType *bn, Cliques *orphans) │ │ │ │ │ -Given a list of indices, turn "contaminated" part of the tree back into a │ │ │ │ │ -factor graph. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:475 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_R_o_o_t_s │ │ │ │ │ -FastVector< sharedClique > Roots │ │ │ │ │ -Root cliques. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:95 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:276 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r_:_:_B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r │ │ │ │ │ -BayesTreeOrphanWrapper(const boost::shared_ptr< CliqueType > &clique) │ │ │ │ │ -Construct a new Bayes Tree Orphan Wrapper object. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:293 │ │ │ │ │ +DDeeffiinniittiioonn VariableSlots.cpp:29 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_S_l_o_t_s_:_:_V_a_r_i_a_b_l_e_S_l_o_t_s │ │ │ │ │ +VariableSlots(const FG &factorGraph) │ │ │ │ │ +Constructor from a set of factors to be combined. │ │ │ │ │ +DDeeffiinniittiioonn VariableSlots.h:89 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ + * _V_a_r_i_a_b_l_e_S_l_o_t_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00578_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Conditional-inst.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/LabeledSymbol.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,143 +98,146 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Conditional-inst.h
│ │ │ │ +
LabeledSymbol.h
│ │ │ │
│ │ │ │
│ │ │ │ -
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18// \callgraph
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/inference/Conditional.h>
│ │ │ │ -
22
│ │ │ │ -
23#include <cmath>
│ │ │ │ -
24#include <iostream>
│ │ │ │ -
25
│ │ │ │ -
26namespace gtsam {
│ │ │ │ -
27
│ │ │ │ -
28/* ************************************************************************* */
│ │ │ │ -
29template <class FACTOR, class DERIVEDCONDITIONAL>
│ │ │ │ -
│ │ │ │ -
30void Conditional<FACTOR, DERIVEDCONDITIONAL>::print(
│ │ │ │ -
31 const std::string& s, const KeyFormatter& formatter) const {
│ │ │ │ -
32 std::cout << s << " P(";
│ │ │ │ -
33 for (Key key : frontals()) std::cout << " " << formatter(key);
│ │ │ │ -
34 if (nrParents() > 0) std::cout << " |";
│ │ │ │ -
35 for (Key parent : parents()) std::cout << " " << formatter(parent);
│ │ │ │ -
36 std::cout << ")" << std::endl;
│ │ │ │ -
37}
│ │ │ │ -
│ │ │ │ -
38
│ │ │ │ -
39/* ************************************************************************* */
│ │ │ │ -
40template <class FACTOR, class DERIVEDCONDITIONAL>
│ │ │ │ -
│ │ │ │ -
41bool Conditional<FACTOR, DERIVEDCONDITIONAL>::equals(const This& c,
│ │ │ │ -
42 double tol) const {
│ │ │ │ -
43 return nrFrontals_ == c.nrFrontals_;
│ │ │ │ -
44}
│ │ │ │ -
│ │ │ │ -
45
│ │ │ │ -
46/* ************************************************************************* */
│ │ │ │ -
47template <class FACTOR, class DERIVEDCONDITIONAL>
│ │ │ │ -
│ │ │ │ -
48double Conditional<FACTOR, DERIVEDCONDITIONAL>::logProbability(
│ │ │ │ -
49 const HybridValues& c) const {
│ │ │ │ -
50 throw std::runtime_error("Conditional::logProbability is not implemented");
│ │ │ │ -
51}
│ │ │ │ -
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <functional>
│ │ │ │ +
23#include <gtsam/inference/Symbol.h>
│ │ │ │ +
24
│ │ │ │ +
25namespace gtsam {
│ │ │ │ +
26
│ │ │ │ +
│ │ │ │ +
35class GTSAM_EXPORT LabeledSymbol {
│ │ │ │ +
36protected:
│ │ │ │ +
37 unsigned char c_, label_;
│ │ │ │ +
38 std::uint64_t j_;
│ │ │ │ +
39
│ │ │ │ +
40public:
│ │ │ │ +
42 LabeledSymbol();
│ │ │ │ +
43
│ │ │ │ +
45 LabeledSymbol(const LabeledSymbol& key);
│ │ │ │ +
46
│ │ │ │ +
48 LabeledSymbol(unsigned char c, unsigned char label, std::uint64_t j);
│ │ │ │ +
49
│ │ │ │ +
51 LabeledSymbol(gtsam::Key key);
│ │ │ │
52
│ │ │ │ -
53/* ************************************************************************* */
│ │ │ │ -
54template <class FACTOR, class DERIVEDCONDITIONAL>
│ │ │ │ -
│ │ │ │ -
55double Conditional<FACTOR, DERIVEDCONDITIONAL>::evaluate(
│ │ │ │ -
56 const HybridValues& c) const {
│ │ │ │ -
57 throw std::runtime_error("Conditional::evaluate is not implemented");
│ │ │ │ -
58}
│ │ │ │ -
│ │ │ │ -
59
│ │ │ │ -
60/* ************************************************************************* */
│ │ │ │ -
61template <class FACTOR, class DERIVEDCONDITIONAL>
│ │ │ │ -
│ │ │ │ -
62double Conditional<FACTOR, DERIVEDCONDITIONAL>::logNormalizationConstant()
│ │ │ │ -
63 const {
│ │ │ │ -
64 throw std::runtime_error(
│ │ │ │ -
65 "Conditional::logNormalizationConstant is not implemented");
│ │ │ │ -
66}
│ │ │ │ -
│ │ │ │ -
67
│ │ │ │ -
68/* ************************************************************************* */
│ │ │ │ -
69template <class FACTOR, class DERIVEDCONDITIONAL>
│ │ │ │ -
│ │ │ │ -
70double Conditional<FACTOR, DERIVEDCONDITIONAL>::normalizationConstant() const {
│ │ │ │ -
71 return std::exp(logNormalizationConstant());
│ │ │ │ -
72}
│ │ │ │ -
│ │ │ │ -
73
│ │ │ │ -
74/* ************************************************************************* */
│ │ │ │ -
75template <class FACTOR, class DERIVEDCONDITIONAL>
│ │ │ │ -
76template <class VALUES>
│ │ │ │ -
│ │ │ │ -
77bool Conditional<FACTOR, DERIVEDCONDITIONAL>::CheckInvariants(
│ │ │ │ -
78 const DERIVEDCONDITIONAL& conditional, const VALUES& values) {
│ │ │ │ -
79 const double prob_or_density = conditional.evaluate(values);
│ │ │ │ -
80 if (prob_or_density < 0.0) return false; // prob_or_density is negative.
│ │ │ │ -
81 if (std::abs(prob_or_density - conditional(values)) > 1e-9)
│ │ │ │ -
82 return false; // operator and evaluate differ
│ │ │ │ -
83 const double logProb = conditional.logProbability(values);
│ │ │ │ -
84 if (std::abs(prob_or_density - std::exp(logProb)) > 1e-9)
│ │ │ │ -
85 return false; // logProb is not consistent with prob_or_density
│ │ │ │ -
86 if (std::abs(conditional.logNormalizationConstant() -
│ │ │ │ -
87 std::log(conditional.normalizationConstant())) > 1e-9)
│ │ │ │ -
88 return false; // log normalization constant is not consistent with
│ │ │ │ -
89 // normalization constant
│ │ │ │ -
90 const double error = conditional.error(values);
│ │ │ │ -
91 if (error < 0.0) return false; // prob_or_density is negative.
│ │ │ │ -
92 const double expected = conditional.logNormalizationConstant() - error;
│ │ │ │ -
93 if (std::abs(logProb - expected) > 1e-9)
│ │ │ │ -
94 return false; // logProb is not consistent with error
│ │ │ │ -
95 return true;
│ │ │ │ -
96}
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
97
│ │ │ │ -
98} // namespace gtsam
│ │ │ │ -
│ │ │ │ -
Conditional.h
Base class for conditional densities.
│ │ │ │ +
54 operator gtsam::Key() const;
│ │ │ │ +
55
│ │ │ │ +
56 // Testable Requirements
│ │ │ │ +
57 void print(const std::string& s = "") const;
│ │ │ │ +
58
│ │ │ │ +
59 bool equals(const LabeledSymbol& expected, double tol = 0.0) const {
│ │ │ │ +
60 return (*this) == expected;
│ │ │ │ +
61 }
│ │ │ │ +
62
│ │ │ │ +
64 gtsam::Key key() const { return (gtsam::Key) *this; }
│ │ │ │ +
65
│ │ │ │ +
67 inline unsigned char label() const { return label_; }
│ │ │ │ +
68
│ │ │ │ +
70 inline unsigned char chr() const { return c_; }
│ │ │ │ +
71
│ │ │ │ +
73 inline size_t index() const { return j_; }
│ │ │ │ +
74
│ │ │ │ +
76 operator std::string() const;
│ │ │ │ +
77
│ │ │ │ +
79 bool operator<(const LabeledSymbol& comp) const;
│ │ │ │ +
80 bool operator==(const LabeledSymbol& comp) const;
│ │ │ │ +
81 bool operator==(gtsam::Key comp) const;
│ │ │ │ +
82 bool operator!=(const LabeledSymbol& comp) const;
│ │ │ │ +
83 bool operator!=(gtsam::Key comp) const;
│ │ │ │ +
84
│ │ │ │ +
91 // Checks only the type
│ │ │ │ +
92 static std::function<bool(gtsam::Key)> TypeTest(unsigned char c);
│ │ │ │ +
93
│ │ │ │ +
94 // Checks only the robot ID (label_)
│ │ │ │ +
95 static std::function<bool(gtsam::Key)> LabelTest(unsigned char label);
│ │ │ │ +
96
│ │ │ │ +
97 // Checks both type and the robot ID
│ │ │ │ +
98 static std::function<bool(gtsam::Key)> TypeLabelTest(unsigned char c, unsigned char label);
│ │ │ │ +
99
│ │ │ │ +
100 // Converts to upper/lower versions of labels
│ │ │ │ +
101 LabeledSymbol upper() const { return LabeledSymbol(c_, toupper(label_), j_); }
│ │ │ │ +
102 LabeledSymbol lower() const { return LabeledSymbol(c_, tolower(label_), j_); }
│ │ │ │ +
103
│ │ │ │ +
104 // Create a new symbol with a different character.
│ │ │ │ +
105 LabeledSymbol newChr(unsigned char c) const { return LabeledSymbol(c, label_, j_); }
│ │ │ │ +
106
│ │ │ │ +
107 // Create a new symbol with a different label.
│ │ │ │ +
108 LabeledSymbol newLabel(unsigned char label) const { return LabeledSymbol(c_, label, j_); }
│ │ │ │ +
109
│ │ │ │ +
111 friend GTSAM_EXPORT std::ostream &operator<<(std::ostream &, const LabeledSymbol &);
│ │ │ │ +
112
│ │ │ │ +
113private:
│ │ │ │ +
114
│ │ │ │ +
116 friend class boost::serialization::access;
│ │ │ │ +
117 template<class ARCHIVE>
│ │ │ │ +
118 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
119 ar & BOOST_SERIALIZATION_NVP(c_);
│ │ │ │ +
120 ar & BOOST_SERIALIZATION_NVP(label_);
│ │ │ │ +
121 ar & BOOST_SERIALIZATION_NVP(j_);
│ │ │ │ +
122 }
│ │ │ │ +
123}; // \class LabeledSymbol
│ │ │ │ +
│ │ │ │ +
124
│ │ │ │ +
│ │ │ │ +
126inline Key mrsymbol(unsigned char c, unsigned char label, size_t j) {
│ │ │ │ +
127 return (Key)LabeledSymbol(c,label,j);
│ │ │ │ +
128}
│ │ │ │ +
│ │ │ │ +
129
│ │ │ │ +
131inline unsigned char mrsymbolChr(Key key) { return LabeledSymbol(key).chr(); }
│ │ │ │ +
132
│ │ │ │ +
134inline unsigned char mrsymbolLabel(Key key) { return LabeledSymbol(key).label(); }
│ │ │ │ +
135
│ │ │ │ +
137inline size_t mrsymbolIndex(Key key) { return LabeledSymbol(key).index(); }
│ │ │ │ +
138
│ │ │ │ +
140template<> struct traits<LabeledSymbol> : public Testable<LabeledSymbol> {};
│ │ │ │ +
141
│ │ │ │ +
142} // \namespace gtsam
│ │ │ │ +
143
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::mrsymbolChr
unsigned char mrsymbolChr(Key key)
Return the character portion of a symbol key.
Definition LabeledSymbol.h:131
│ │ │ │ +
gtsam::mrsymbolIndex
size_t mrsymbolIndex(Key key)
Return the index portion of a symbol key.
Definition LabeledSymbol.h:137
│ │ │ │ +
gtsam::mrsymbolLabel
unsigned char mrsymbolLabel(Key key)
Return the label portion of a symbol key.
Definition LabeledSymbol.h:134
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::operator!=
bool operator!=(const Matrix &A, const Matrix &B)
inequality
Definition Matrix.h:107
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ -
gtsam::Conditional
Definition Conditional.h:64
│ │ │ │ -
gtsam::Conditional::CheckInvariants
static bool CheckInvariants(const DERIVEDCONDITIONAL &conditional, const VALUES &x)
Check invariants of this conditional, given the values x.
Definition Conditional-inst.h:77
│ │ │ │ -
gtsam::Conditional::nrFrontals_
size_t nrFrontals_
The first nrFrontal variables are frontal and the rest are parents.
Definition Conditional.h:67
│ │ │ │ -
gtsam::Conditional::logNormalizationConstant
virtual double logNormalizationConstant() const
All conditional types need to implement a log normalization constant to make it such that error>=0.
Definition Conditional-inst.h:62
│ │ │ │ -
gtsam::Conditional::evaluate
virtual double evaluate(const HybridValues &c) const
All conditional types need to implement an evaluate function, that yields a true probability.
Definition Conditional-inst.h:55
│ │ │ │ -
gtsam::Conditional::equals
bool equals(const This &c, double tol=1e-9) const
check equality
Definition Conditional-inst.h:41
│ │ │ │ -
gtsam::Conditional::logProbability
virtual double logProbability(const HybridValues &c) const
All conditional types need to implement a logProbability function, for which exp(logProbability(x)) =...
Definition Conditional-inst.h:48
│ │ │ │ -
gtsam::Conditional::normalizationConstant
double normalizationConstant() const
Non-virtual, exponentiate logNormalizationConstant.
Definition Conditional-inst.h:70
│ │ │ │ -
gtsam::Conditional::print
void print(const std::string &s="Conditional", const KeyFormatter &formatter=DefaultKeyFormatter) const
print with optional formatter
Definition Conditional-inst.h:30
│ │ │ │ +
gtsam::mrsymbol
Key mrsymbol(unsigned char c, unsigned char label, size_t j)
Create a symbol key from a character, label and index, i.e.
Definition LabeledSymbol.h:126
│ │ │ │ +
gtsam::operator==
bool operator==(const Matrix &A, const Matrix &B)
equality is just equal_with_abs_tol 1e-9
Definition Matrix.h:100
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::LabeledSymbol
Customized version of gtsam::Symbol for multi-robot use.
Definition LabeledSymbol.h:35
│ │ │ │ +
gtsam::LabeledSymbol::chr
unsigned char chr() const
Retrieve key character.
Definition LabeledSymbol.h:70
│ │ │ │ +
gtsam::LabeledSymbol::key
gtsam::Key key() const
return the integer version
Definition LabeledSymbol.h:64
│ │ │ │ +
gtsam::LabeledSymbol::index
size_t index() const
Retrieve key index.
Definition LabeledSymbol.h:73
│ │ │ │ +
gtsam::LabeledSymbol::label
unsigned char label() const
Retrieve label character.
Definition LabeledSymbol.h:67
│ │ │ │ + │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,167 +1,189 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Conditional-inst.h │ │ │ │ │ +LabeledSymbol.h │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18// \callgraph │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ -22 │ │ │ │ │ -23#include │ │ │ │ │ -24#include │ │ │ │ │ -25 │ │ │ │ │ -26namespace _g_t_s_a_m { │ │ │ │ │ -27 │ │ │ │ │ -28/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -29template │ │ │ │ │ -_3_0void _C_o_n_d_i_t_i_o_n_a_l_<_F_A_C_T_O_R_,_ _D_E_R_I_V_E_D_C_O_N_D_I_T_I_O_N_A_L_>_:_:_p_r_i_n_t( │ │ │ │ │ -31 const std::string& s, const _K_e_y_F_o_r_m_a_t_t_e_r& formatter) const { │ │ │ │ │ -32 std::cout << s << " P("; │ │ │ │ │ -33 for (_K_e_y key : frontals()) std::cout << " " << formatter(key); │ │ │ │ │ -34 if (nrParents() > 0) std::cout << " |"; │ │ │ │ │ -35 for (_K_e_y parent : parents()) std::cout << " " << formatter(parent); │ │ │ │ │ -36 std::cout << ")" << std::endl; │ │ │ │ │ -37} │ │ │ │ │ -38 │ │ │ │ │ -39/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -40template │ │ │ │ │ -_4_1bool _C_o_n_d_i_t_i_o_n_a_l_<_F_A_C_T_O_R_,_ _D_E_R_I_V_E_D_C_O_N_D_I_T_I_O_N_A_L_>_:_:_e_q_u_a_l_s(const _T_h_i_s& c, │ │ │ │ │ -42 double tol) const { │ │ │ │ │ -43 return nrFrontals_ == c._n_r_F_r_o_n_t_a_l_s__; │ │ │ │ │ -44} │ │ │ │ │ -45 │ │ │ │ │ -46/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -47template │ │ │ │ │ -_4_8double _C_o_n_d_i_t_i_o_n_a_l_<_F_A_C_T_O_R_,_ _D_E_R_I_V_E_D_C_O_N_D_I_T_I_O_N_A_L_>_:_:_l_o_g_P_r_o_b_a_b_i_l_i_t_y( │ │ │ │ │ -49 const _H_y_b_r_i_d_V_a_l_u_e_s& c) const { │ │ │ │ │ -50 throw std::runtime_error("Conditional::logProbability is not implemented"); │ │ │ │ │ -51} │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include │ │ │ │ │ +23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_S_y_m_b_o_l_._h> │ │ │ │ │ +24 │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ +26 │ │ │ │ │ +_3_5class GTSAM_EXPORT _L_a_b_e_l_e_d_S_y_m_b_o_l { │ │ │ │ │ +36protected: │ │ │ │ │ +37 unsigned char c_, label_; │ │ │ │ │ +38 std::uint64_t j_; │ │ │ │ │ +39 │ │ │ │ │ +40public: │ │ │ │ │ +42 _L_a_b_e_l_e_d_S_y_m_b_o_l(); │ │ │ │ │ +43 │ │ │ │ │ +45 _L_a_b_e_l_e_d_S_y_m_b_o_l(const _L_a_b_e_l_e_d_S_y_m_b_o_l& key); │ │ │ │ │ +46 │ │ │ │ │ +48 _L_a_b_e_l_e_d_S_y_m_b_o_l(unsigned char c, unsigned char label, std::uint64_t j); │ │ │ │ │ +49 │ │ │ │ │ +51 _L_a_b_e_l_e_d_S_y_m_b_o_l(_g_t_s_a_m_:_:_K_e_y key); │ │ │ │ │ 52 │ │ │ │ │ -53/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -54template │ │ │ │ │ -_5_5double _C_o_n_d_i_t_i_o_n_a_l_<_F_A_C_T_O_R_,_ _D_E_R_I_V_E_D_C_O_N_D_I_T_I_O_N_A_L_>_:_:_e_v_a_l_u_a_t_e( │ │ │ │ │ -56 const _H_y_b_r_i_d_V_a_l_u_e_s& c) const { │ │ │ │ │ -57 throw std::runtime_error("Conditional::evaluate is not implemented"); │ │ │ │ │ -58} │ │ │ │ │ -59 │ │ │ │ │ -60/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -61template │ │ │ │ │ -_6_2double _C_o_n_d_i_t_i_o_n_a_l_<_F_A_C_T_O_R_,_ _D_E_R_I_V_E_D_C_O_N_D_I_T_I_O_N_A_L_>_:_:_l_o_g_N_o_r_m_a_l_i_z_a_t_i_o_n_C_o_n_s_t_a_n_t() │ │ │ │ │ -63 const { │ │ │ │ │ -64 throw std::runtime_error( │ │ │ │ │ -65 "Conditional::logNormalizationConstant is not implemented"); │ │ │ │ │ -66} │ │ │ │ │ -67 │ │ │ │ │ -68/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -69template │ │ │ │ │ -_7_0double _C_o_n_d_i_t_i_o_n_a_l_<_F_A_C_T_O_R_,_ _D_E_R_I_V_E_D_C_O_N_D_I_T_I_O_N_A_L_>_:_:_n_o_r_m_a_l_i_z_a_t_i_o_n_C_o_n_s_t_a_n_t() const │ │ │ │ │ -{ │ │ │ │ │ -71 return std::exp(logNormalizationConstant()); │ │ │ │ │ -72} │ │ │ │ │ -73 │ │ │ │ │ -74/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -75template │ │ │ │ │ -76template │ │ │ │ │ -_7_7bool _C_o_n_d_i_t_i_o_n_a_l_<_F_A_C_T_O_R_,_ _D_E_R_I_V_E_D_C_O_N_D_I_T_I_O_N_A_L_>_:_:_C_h_e_c_k_I_n_v_a_r_i_a_n_t_s( │ │ │ │ │ -78 const DERIVEDCONDITIONAL& conditional, const VALUES& values) { │ │ │ │ │ -79 const double prob_or_density = conditional.evaluate(values); │ │ │ │ │ -80 if (prob_or_density < 0.0) return false; // prob_or_density is negative. │ │ │ │ │ -81 if (std::abs(prob_or_density - conditional(values)) > 1e-9) │ │ │ │ │ -82 return false; // operator and evaluate differ │ │ │ │ │ -83 const double logProb = conditional.logProbability(values); │ │ │ │ │ -84 if (std::abs(prob_or_density - std::exp(logProb)) > 1e-9) │ │ │ │ │ -85 return false; // logProb is not consistent with prob_or_density │ │ │ │ │ -86 if (std::abs(conditional.logNormalizationConstant() - │ │ │ │ │ -87 std::log(conditional.normalizationConstant())) > 1e-9) │ │ │ │ │ -88 return false; // log normalization constant is not consistent with │ │ │ │ │ -89 // normalization constant │ │ │ │ │ -90 const double error = conditional.error(values); │ │ │ │ │ -91 if (error < 0.0) return false; // prob_or_density is negative. │ │ │ │ │ -92 const double expected = conditional.logNormalizationConstant() - error; │ │ │ │ │ -93 if (std::abs(logProb - expected) > 1e-9) │ │ │ │ │ -94 return false; // logProb is not consistent with error │ │ │ │ │ -95 return true; │ │ │ │ │ -96} │ │ │ │ │ -_9_7 │ │ │ │ │ -98} // namespace gtsam │ │ │ │ │ -_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ -Base class for conditional densities. │ │ │ │ │ +54 operator _g_t_s_a_m_:_:_K_e_y() const; │ │ │ │ │ +55 │ │ │ │ │ +56 // Testable Requirements │ │ │ │ │ +57 void _p_r_i_n_t(const std::string& s = "") const; │ │ │ │ │ +58 │ │ │ │ │ +59 bool _e_q_u_a_l_s(const _L_a_b_e_l_e_d_S_y_m_b_o_l& expected, double tol = 0.0) const { │ │ │ │ │ +60 return (*this) == expected; │ │ │ │ │ +61 } │ │ │ │ │ +62 │ │ │ │ │ +_6_4 _g_t_s_a_m_:_:_K_e_y _k_e_y() const { return (_g_t_s_a_m_:_:_K_e_y) *this; } │ │ │ │ │ +65 │ │ │ │ │ +_6_7 inline unsigned char _l_a_b_e_l() const { return label_; } │ │ │ │ │ +68 │ │ │ │ │ +_7_0 inline unsigned char _c_h_r() const { return c_; } │ │ │ │ │ +71 │ │ │ │ │ +_7_3 inline size_t _i_n_d_e_x() const { return j_; } │ │ │ │ │ +74 │ │ │ │ │ +76 operator std::string() const; │ │ │ │ │ +77 │ │ │ │ │ +79 bool operator<(const _L_a_b_e_l_e_d_S_y_m_b_o_l& comp) const; │ │ │ │ │ +80 bool _o_p_e_r_a_t_o_r_=_=(const _L_a_b_e_l_e_d_S_y_m_b_o_l& comp) const; │ │ │ │ │ +81 bool _o_p_e_r_a_t_o_r_=_=(_g_t_s_a_m_:_:_K_e_y comp) const; │ │ │ │ │ +82 bool _o_p_e_r_a_t_o_r_!_=(const _L_a_b_e_l_e_d_S_y_m_b_o_l& comp) const; │ │ │ │ │ +83 bool _o_p_e_r_a_t_o_r_!_=(_g_t_s_a_m_:_:_K_e_y comp) const; │ │ │ │ │ +84 │ │ │ │ │ +91 // Checks only the type │ │ │ │ │ +92 static std::function TypeTest(unsigned char c); │ │ │ │ │ +93 │ │ │ │ │ +94 // Checks only the robot ID (label_) │ │ │ │ │ +95 static std::function LabelTest(unsigned char label); │ │ │ │ │ +96 │ │ │ │ │ +97 // Checks both type and the robot ID │ │ │ │ │ +98 static std::function TypeLabelTest(unsigned char c, │ │ │ │ │ +unsigned char label); │ │ │ │ │ +99 │ │ │ │ │ +100 // Converts to upper/lower versions of labels │ │ │ │ │ +101 _L_a_b_e_l_e_d_S_y_m_b_o_l upper() const { return _L_a_b_e_l_e_d_S_y_m_b_o_l(c_, toupper(label_), │ │ │ │ │ +j_); } │ │ │ │ │ +102 LabeledSymbol lower() const { return LabeledSymbol(c_, tolower(label_), │ │ │ │ │ +j_); } │ │ │ │ │ +103 │ │ │ │ │ +104 // Create a new symbol with a different character. │ │ │ │ │ +105 LabeledSymbol newChr(unsigned char c) const { return LabeledSymbol(c, │ │ │ │ │ +label_, j_); } │ │ │ │ │ +106 │ │ │ │ │ +107 // Create a new symbol with a different label. │ │ │ │ │ +108 LabeledSymbol newLabel(unsigned char label) const { return LabeledSymbol │ │ │ │ │ +(c_, label, j_); } │ │ │ │ │ +109 │ │ │ │ │ +111 friend GTSAM_EXPORT std::ostream &operator<<(std::ostream &, const │ │ │ │ │ +LabeledSymbol &); │ │ │ │ │ +112 │ │ │ │ │ +113private: │ │ │ │ │ +114 │ │ │ │ │ +_1_1_6 friend class boost::serialization::access; │ │ │ │ │ +117 template │ │ │ │ │ +118 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +119 ar & BOOST_SERIALIZATION_NVP(c_); │ │ │ │ │ +120 ar & BOOST_SERIALIZATION_NVP(label_); │ │ │ │ │ +121 ar & BOOST_SERIALIZATION_NVP(j_); │ │ │ │ │ +122 } │ │ │ │ │ +123}; // \class LabeledSymbol │ │ │ │ │ +124 │ │ │ │ │ +_1_2_6inline _K_e_y _m_r_s_y_m_b_o_l(unsigned char c, unsigned char label, size_t j) { │ │ │ │ │ +127 return (_K_e_y)_L_a_b_e_l_e_d_S_y_m_b_o_l(c,label,j); │ │ │ │ │ +128} │ │ │ │ │ +129 │ │ │ │ │ +_1_3_1inline unsigned char _m_r_s_y_m_b_o_l_C_h_r(_K_e_y key) { return _L_a_b_e_l_e_d_S_y_m_b_o_l(key)._c_h_r(); │ │ │ │ │ +} │ │ │ │ │ +132 │ │ │ │ │ +_1_3_4inline unsigned char _m_r_s_y_m_b_o_l_L_a_b_e_l(_K_e_y key) { return _L_a_b_e_l_e_d_S_y_m_b_o_l │ │ │ │ │ +(key)._l_a_b_e_l(); } │ │ │ │ │ +135 │ │ │ │ │ +_1_3_7inline size_t _m_r_s_y_m_b_o_l_I_n_d_e_x(_K_e_y key) { return _L_a_b_e_l_e_d_S_y_m_b_o_l(key)._i_n_d_e_x(); } │ │ │ │ │ +138 │ │ │ │ │ +_1_4_0template<> struct _t_r_a_i_t_s<_L_a_b_e_l_e_d_S_y_m_b_o_l> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ +141 │ │ │ │ │ +142} // \namespace gtsam │ │ │ │ │ +143 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_m_r_s_y_m_b_o_l_C_h_r │ │ │ │ │ +unsigned char mrsymbolChr(Key key) │ │ │ │ │ +Return the character portion of a symbol key. │ │ │ │ │ +DDeeffiinniittiioonn LabeledSymbol.h:131 │ │ │ │ │ +_g_t_s_a_m_:_:_m_r_s_y_m_b_o_l_I_n_d_e_x │ │ │ │ │ +size_t mrsymbolIndex(Key key) │ │ │ │ │ +Return the index portion of a symbol key. │ │ │ │ │ +DDeeffiinniittiioonn LabeledSymbol.h:137 │ │ │ │ │ +_g_t_s_a_m_:_:_m_r_s_y_m_b_o_l_L_a_b_e_l │ │ │ │ │ +unsigned char mrsymbolLabel(Key key) │ │ │ │ │ +Return the label portion of a symbol key. │ │ │ │ │ +DDeeffiinniittiioonn LabeledSymbol.h:134 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_!_= │ │ │ │ │ +bool operator!=(const Matrix &A, const Matrix &B) │ │ │ │ │ +inequality │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:107 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_C_h_e_c_k_I_n_v_a_r_i_a_n_t_s │ │ │ │ │ -static bool CheckInvariants(const DERIVEDCONDITIONAL &conditional, const VALUES │ │ │ │ │ -&x) │ │ │ │ │ -Check invariants of this conditional, given the values x. │ │ │ │ │ -DDeeffiinniittiioonn Conditional-inst.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_n_r_F_r_o_n_t_a_l_s__ │ │ │ │ │ -size_t nrFrontals_ │ │ │ │ │ -The first nrFrontal variables are frontal and the rest are parents. │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_l_o_g_N_o_r_m_a_l_i_z_a_t_i_o_n_C_o_n_s_t_a_n_t │ │ │ │ │ -virtual double logNormalizationConstant() const │ │ │ │ │ -All conditional types need to implement a log normalization constant to make it │ │ │ │ │ -such that error>=0. │ │ │ │ │ -DDeeffiinniittiioonn Conditional-inst.h:62 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_e_v_a_l_u_a_t_e │ │ │ │ │ -virtual double evaluate(const HybridValues &c) const │ │ │ │ │ -All conditional types need to implement an evaluate function, that yields a │ │ │ │ │ -true probability. │ │ │ │ │ -DDeeffiinniittiioonn Conditional-inst.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const This &c, double tol=1e-9) const │ │ │ │ │ -check equality │ │ │ │ │ -DDeeffiinniittiioonn Conditional-inst.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_l_o_g_P_r_o_b_a_b_i_l_i_t_y │ │ │ │ │ -virtual double logProbability(const HybridValues &c) const │ │ │ │ │ -All conditional types need to implement a logProbability function, for which │ │ │ │ │ -exp(logProbability(x)) =... │ │ │ │ │ -DDeeffiinniittiioonn Conditional-inst.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_n_o_r_m_a_l_i_z_a_t_i_o_n_C_o_n_s_t_a_n_t │ │ │ │ │ -double normalizationConstant() const │ │ │ │ │ -Non-virtual, exponentiate logNormalizationConstant. │ │ │ │ │ -DDeeffiinniittiioonn Conditional-inst.h:70 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="Conditional", const KeyFormatter │ │ │ │ │ -&formatter=DefaultKeyFormatter) const │ │ │ │ │ -print with optional formatter │ │ │ │ │ -DDeeffiinniittiioonn Conditional-inst.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_m_r_s_y_m_b_o_l │ │ │ │ │ +Key mrsymbol(unsigned char c, unsigned char label, size_t j) │ │ │ │ │ +Create a symbol key from a character, label and index, i.e. │ │ │ │ │ +DDeeffiinniittiioonn LabeledSymbol.h:126 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_=_= │ │ │ │ │ +bool operator==(const Matrix &A, const Matrix &B) │ │ │ │ │ +equality is just equal_with_abs_tol 1e-9 │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_L_a_b_e_l_e_d_S_y_m_b_o_l │ │ │ │ │ +Customized version of gtsam::Symbol for multi-robot use. │ │ │ │ │ +DDeeffiinniittiioonn LabeledSymbol.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_L_a_b_e_l_e_d_S_y_m_b_o_l_:_:_c_h_r │ │ │ │ │ +unsigned char chr() const │ │ │ │ │ +Retrieve key character. │ │ │ │ │ +DDeeffiinniittiioonn LabeledSymbol.h:70 │ │ │ │ │ +_g_t_s_a_m_:_:_L_a_b_e_l_e_d_S_y_m_b_o_l_:_:_k_e_y │ │ │ │ │ +gtsam::Key key() const │ │ │ │ │ +return the integer version │ │ │ │ │ +DDeeffiinniittiioonn LabeledSymbol.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_L_a_b_e_l_e_d_S_y_m_b_o_l_:_:_i_n_d_e_x │ │ │ │ │ +size_t index() const │ │ │ │ │ +Retrieve key index. │ │ │ │ │ +DDeeffiinniittiioonn LabeledSymbol.h:73 │ │ │ │ │ +_g_t_s_a_m_:_:_L_a_b_e_l_e_d_S_y_m_b_o_l_:_:_l_a_b_e_l │ │ │ │ │ +unsigned char label() const │ │ │ │ │ +Retrieve label character. │ │ │ │ │ +DDeeffiinniittiioonn LabeledSymbol.h:67 │ │ │ │ │ +_S_y_m_b_o_l_._h │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * CCoonnddiittiioonnaall--iinnsstt..hh │ │ │ │ │ + * _L_a_b_e_l_e_d_S_y_m_b_o_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00581.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Key.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTreeCliqueBase-inst.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,97 +94,37 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
│ │ │ │ -
Key.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
BayesTreeCliqueBase-inst.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ +

Base class for cliques of a BayesTree. │ │ │ │ +More...

│ │ │ │ + │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

struct  gtsam::StreamedKey
 To use the key_formatter on Keys, they must be wrapped in a StreamedKey. More...
 
class  gtsam::key_formatter
 Output stream manipulator that will format gtsam::Keys according to the given KeyFormatter, as long as Key values are wrapped in a gtsam::StreamedKey. More...
 
struct  gtsam::traits< Key >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Typedefs

│ │ │ │ -using gtsam::KeyFormatter = std::function< std::string(Key)>
 Typedef for a function to format a key, i.e. to convert it to a string.
 
│ │ │ │ -using gtsam::KeyVector = FastVector< Key >
 Define collection type once and for all - also used in wrappers.
 
│ │ │ │ -using gtsam::KeyList = FastList< Key >
 
│ │ │ │ -using gtsam::KeySet = FastSet< Key >
 
│ │ │ │ -using gtsam::KeyGroupMap = FastMap< Key, int >
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -string gtsam::_defaultKeyFormatter (Key key)
 
│ │ │ │ -string gtsam::_multirobotKeyFormatter (Key key)
 
│ │ │ │ -void gtsam::PrintKey (Key key, const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter)
 Utility function to print one key with optional prefix.
 
│ │ │ │ -void gtsam::PrintKeyList (const KeyList &keys, const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter)
 Utility function to print sets of keys with optional prefix.
 
│ │ │ │ -void gtsam::PrintKeyVector (const KeyVector &keys, const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter)
 Utility function to print sets of keys with optional prefix.
 
│ │ │ │ -void gtsam::PrintKeySet (const KeySet &keys, const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter)
 Utility function to print sets of keys with optional prefix.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
│ │ │ │ -Alex Cunningham
│ │ │ │ -
Date
Feb 20, 2012
│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Feb 20, 2012
│ │ │ │ +

Base class for cliques of a BayesTree.

│ │ │ │ +
Author
Richard Roberts and Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,72 +1,21 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Key.h File Reference │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +BayesTreeCliqueBase-inst.h File Reference │ │ │ │ │ +Base class for cliques of a BayesTree. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_S_t_r_e_a_m_e_d_K_e_y │ │ │ │ │ -  To use the _k_e_y___f_o_r_m_a_t_t_e_r on Keys, they must be wrapped in a │ │ │ │ │ - _S_t_r_e_a_m_e_d_K_e_y. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_k_e_y___f_o_r_m_a_t_t_e_r │ │ │ │ │ - Output stream manipulator that will format gtsam::Keys according to │ │ │ │ │ -  the given KeyFormatter, as long as Key values are wrapped in a _g_t_s_a_m_:_: │ │ │ │ │ - _S_t_r_e_a_m_e_d_K_e_y. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _K_e_y_ _> │ │ │ │ │ -  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -using  ggttssaamm::::KKeeyyFFoorrmmaatttteerr = std::function< std::string(_K_e_y)> │ │ │ │ │ -  Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -  │ │ │ │ │ -using  ggttssaamm::::KKeeyyVVeeccttoorr = _F_a_s_t_V_e_c_t_o_r< _K_e_y > │ │ │ │ │ -  Define collection type once and for all - also used in wrappers. │ │ │ │ │ -  │ │ │ │ │ -using  ggttssaamm::::KKeeyyLLiisstt = _F_a_s_t_L_i_s_t< _K_e_y > │ │ │ │ │ -  │ │ │ │ │ -using  ggttssaamm::::KKeeyySSeett = _F_a_s_t_S_e_t< _K_e_y > │ │ │ │ │ -  │ │ │ │ │ -using  ggttssaamm::::KKeeyyGGrroouuppMMaapp = _F_a_s_t_M_a_p< _K_e_y, int > │ │ │ │ │ -  │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -string  ggttssaamm::::__ddeeffaauullttKKeeyyFFoorrmmaatttteerr (_K_e_y key) │ │ │ │ │ -  │ │ │ │ │ -string  ggttssaamm::::__mmuullttiirroobboottKKeeyyFFoorrmmaatttteerr (_K_e_y key) │ │ │ │ │ -  │ │ │ │ │ - void  ggttssaamm::::PPrriinnttKKeeyy (_K_e_y key, const std::string &s="", const _K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ - &keyFormatter=DefaultKeyFormatter) │ │ │ │ │ -  Utility function to print one key with optional prefix. │ │ │ │ │ -  │ │ │ │ │ - void  ggttssaamm::::PPrriinnttKKeeyyLLiisstt (const _K_e_y_L_i_s_t &keys, const std::string &s="", │ │ │ │ │ - const _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter=DefaultKeyFormatter) │ │ │ │ │ -  Utility function to print sets of keys with optional prefix. │ │ │ │ │ -  │ │ │ │ │ - void  ggttssaamm::::PPrriinnttKKeeyyVVeeccttoorr (const _K_e_y_V_e_c_t_o_r &keys, const std::string &s="", │ │ │ │ │ - const _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter=DefaultKeyFormatter) │ │ │ │ │ -  Utility function to print sets of keys with optional prefix. │ │ │ │ │ -  │ │ │ │ │ - void  ggttssaamm::::PPrriinnttKKeeyySSeett (const _K_e_y_S_e_t &keys, const std::string &s="", const │ │ │ │ │ - _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter=DefaultKeyFormatter) │ │ │ │ │ -  Utility function to print sets of keys with optional prefix. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Base class for cliques of a BayesTree. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Alex Cunningham │ │ │ │ │ - Date │ │ │ │ │ - Feb 20, 2012 │ │ │ │ │ - Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Date │ │ │ │ │ - Feb 20, 2012 │ │ │ │ │ + Richard Roberts and Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _K_e_y_._h │ │ │ │ │ + * _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_-_i_n_s_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00581_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Key.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTreeCliqueBase-inst.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,148 +98,305 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Key.h
│ │ │ │ +
BayesTreeCliqueBase-inst.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │ -
9
│ │ │ │ -
10 * -------------------------------------------------------------------------- */
│ │ │ │ -
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/base/FastList.h>
│ │ │ │ -
21#include <gtsam/base/FastMap.h>
│ │ │ │ -
22#include <gtsam/base/FastSet.h>
│ │ │ │ -
23#include <gtsam/base/FastVector.h>
│ │ │ │ -
24#include <gtsam/base/Testable.h>
│ │ │ │ -
25#include <gtsam/base/types.h>
│ │ │ │ -
26#include <gtsam/dllexport.h>
│ │ │ │ -
27
│ │ │ │ -
28#include <functional>
│ │ │ │ -
29
│ │ │ │ -
30#include <iosfwd>
│ │ │ │ -
31
│ │ │ │ -
32namespace gtsam {
│ │ │ │ -
33
│ │ │ │ -
35using KeyFormatter = std::function<std::string(Key)>;
│ │ │ │ -
36
│ │ │ │ -
37// Helper function for DefaultKeyFormatter
│ │ │ │ -
38GTSAM_EXPORT std::string _defaultKeyFormatter(Key key);
│ │ │ │ -
39
│ │ │ │ -
43static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter;
│ │ │ │ -
44
│ │ │ │ -
45// Helper function for Multi-robot Key Formatter
│ │ │ │ -
46GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key);
│ │ │ │ -
47
│ │ │ │ -
54static const gtsam::KeyFormatter MultiRobotKeyFormatter =
│ │ │ │ -
55 &_multirobotKeyFormatter;
│ │ │ │ -
56
│ │ │ │ -
│ │ │ │ -
58struct StreamedKey {
│ │ │ │ -
59 const Key &key_;
│ │ │ │ -
60 explicit StreamedKey(const Key &key) : key_(key) {}
│ │ │ │ -
61 GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &, const StreamedKey &);
│ │ │ │ -
62};
│ │ │ │ -
│ │ │ │ -
63
│ │ │ │ -
│ │ │ │ -
72class key_formatter {
│ │ │ │ -
73 public:
│ │ │ │ -
74 explicit key_formatter(KeyFormatter v) : formatter_(v) {}
│ │ │ │ -
75 GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &, const key_formatter &);
│ │ │ │ -
76 GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &, const StreamedKey &);
│ │ │ │ -
77
│ │ │ │ -
78 private:
│ │ │ │ -
79 KeyFormatter formatter_;
│ │ │ │ -
80 static void *&property(std::ios_base &s);
│ │ │ │ -
81 static void set_property(std::ios_base &s, const KeyFormatter &f);
│ │ │ │ -
82 static KeyFormatter *get_property(std::ios_base &s);
│ │ │ │ -
83};
│ │ │ │ -
│ │ │ │ -
84
│ │ │ │ -
86using KeyVector = FastVector<Key>;
│ │ │ │ -
87
│ │ │ │ -
88// TODO(frank): Nothing fast about these :-(
│ │ │ │ -
89using KeyList = FastList<Key>;
│ │ │ │ -
90using KeySet = FastSet<Key>;
│ │ │ │ -
91using KeyGroupMap = FastMap<Key, int>;
│ │ │ │ -
92
│ │ │ │ -
94GTSAM_EXPORT void PrintKey(
│ │ │ │ -
95 Key key, const std::string &s = "",
│ │ │ │ -
96 const KeyFormatter &keyFormatter = DefaultKeyFormatter);
│ │ │ │ -
97
│ │ │ │ -
99GTSAM_EXPORT void PrintKeyList(
│ │ │ │ -
100 const KeyList &keys, const std::string &s = "",
│ │ │ │ -
101 const KeyFormatter &keyFormatter = DefaultKeyFormatter);
│ │ │ │ +
9 * -------------------------------------------------------------------------- */
│ │ │ │ +
10
│ │ │ │ +
17#pragma once
│ │ │ │ +
18
│ │ │ │ +
19#include <gtsam/inference/BayesTreeCliqueBase.h>
│ │ │ │ +
20#include <gtsam/inference/FactorGraph-inst.h>
│ │ │ │ +
21#include <gtsam/base/timing.h>
│ │ │ │ +
22
│ │ │ │ +
23namespace gtsam {
│ │ │ │ +
24
│ │ │ │ +
25 /* ************************************************************************* */
│ │ │ │ +
26 template<class DERIVED, class FACTORGRAPH>
│ │ │ │ +
│ │ │ │ +
27 void BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::setEliminationResult(
│ │ │ │ +
28 const typename FactorGraphType::EliminationResult& eliminationResult)
│ │ │ │ +
29 {
│ │ │ │ +
30 conditional_ = eliminationResult.first;
│ │ │ │ +
31 }
│ │ │ │ +
│ │ │ │ +
32
│ │ │ │ +
33 /* ************************************************************************* */
│ │ │ │ +
34 template<class DERIVED, class FACTORGRAPH>
│ │ │ │ +
│ │ │ │ +
35 bool BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::equals(
│ │ │ │ +
36 const DERIVED& other, double tol) const
│ │ │ │ +
37 {
│ │ │ │ +
38 return (!conditional_ && !other.conditional())
│ │ │ │ +
39 || conditional_->equals(*other.conditional(), tol);
│ │ │ │ +
40 }
│ │ │ │ +
│ │ │ │ +
41
│ │ │ │ +
42 /* ************************************************************************* */
│ │ │ │ +
43 template<class DERIVED, class FACTORGRAPH>
│ │ │ │ +
44 KeyVector
│ │ │ │ +
│ │ │ │ +
45 BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separator_setminus_B(const derived_ptr& B) const
│ │ │ │ +
46 {
│ │ │ │ +
47 KeySet p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents());
│ │ │ │ +
48 KeySet indicesB(B->conditional()->begin(), B->conditional()->end());
│ │ │ │ +
49 KeyVector S_setminus_B;
│ │ │ │ +
50 std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(),
│ │ │ │ +
51 indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B));
│ │ │ │ +
52 return S_setminus_B;
│ │ │ │ +
53 }
│ │ │ │ +
│ │ │ │ +
54
│ │ │ │ +
55 /* ************************************************************************* */
│ │ │ │ +
56 template<class DERIVED, class FACTORGRAPH>
│ │ │ │ +
│ │ │ │ +
57 KeyVector BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::shortcut_indices(
│ │ │ │ +
58 const derived_ptr& B, const FactorGraphType& p_Cp_B) const
│ │ │ │ +
59 {
│ │ │ │ +
60 gttic(shortcut_indices);
│ │ │ │ +
61 KeySet allKeys = p_Cp_B.keys();
│ │ │ │ +
62 KeySet indicesB(B->conditional()->begin(), B->conditional()->end());
│ │ │ │ +
63 KeyVector S_setminus_B = separator_setminus_B(B);
│ │ │ │ +
64 KeyVector keep;
│ │ │ │ +
65 // keep = S\B intersect allKeys (S_setminus_B is already sorted)
│ │ │ │ +
66 std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), //
│ │ │ │ +
67 allKeys.begin(), allKeys.end(), back_inserter(keep));
│ │ │ │ +
68 // keep += B intersect allKeys
│ │ │ │ +
69 std::set_intersection(indicesB.begin(), indicesB.end(), //
│ │ │ │ +
70 allKeys.begin(), allKeys.end(), back_inserter(keep));
│ │ │ │ +
71 return keep;
│ │ │ │ +
72 }
│ │ │ │ +
│ │ │ │ +
73
│ │ │ │ +
74 /* ************************************************************************* */
│ │ │ │ +
75 template<class DERIVED, class FACTORGRAPH>
│ │ │ │ +
│ │ │ │ +
76 void BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::print(
│ │ │ │ +
77 const std::string& s, const KeyFormatter& keyFormatter) const
│ │ │ │ +
78 {
│ │ │ │ +
79 conditional_->print(s, keyFormatter);
│ │ │ │ +
80 }
│ │ │ │ +
│ │ │ │ +
81
│ │ │ │ +
82 /* ************************************************************************* */
│ │ │ │ +
83 template<class DERIVED, class FACTORGRAPH>
│ │ │ │ +
│ │ │ │ +
84 size_t BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::treeSize() const {
│ │ │ │ +
85 size_t size = 1;
│ │ │ │ +
86 for(const derived_ptr& child: children)
│ │ │ │ +
87 size += child->treeSize();
│ │ │ │ +
88 return size;
│ │ │ │ +
89 }
│ │ │ │ +
│ │ │ │ +
90
│ │ │ │ +
91 /* ************************************************************************* */
│ │ │ │ +
92 template<class DERIVED, class FACTORGRAPH>
│ │ │ │ +
│ │ │ │ +
93 size_t BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::numCachedSeparatorMarginals() const
│ │ │ │ +
94 {
│ │ │ │ +
95 std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
│ │ │ │ +
96 if (!cachedSeparatorMarginal_)
│ │ │ │ +
97 return 0;
│ │ │ │ +
98
│ │ │ │ +
99 size_t subtree_count = 1;
│ │ │ │ +
100 for(const derived_ptr& child: children)
│ │ │ │ +
101 subtree_count += child->numCachedSeparatorMarginals();
│ │ │ │
102
│ │ │ │ -
104GTSAM_EXPORT void PrintKeyVector(
│ │ │ │ -
105 const KeyVector &keys, const std::string &s = "",
│ │ │ │ -
106 const KeyFormatter &keyFormatter = DefaultKeyFormatter);
│ │ │ │ -
107
│ │ │ │ -
109GTSAM_EXPORT void PrintKeySet(
│ │ │ │ -
110 const KeySet &keys, const std::string &s = "",
│ │ │ │ -
111 const KeyFormatter &keyFormatter = DefaultKeyFormatter);
│ │ │ │ -
112
│ │ │ │ -
113// Define Key to be Testable by specializing gtsam::traits
│ │ │ │ -
114template<typename T> struct traits;
│ │ │ │ -
115
│ │ │ │ -
116template <>
│ │ │ │ -
│ │ │ │ -
117struct traits<Key> {
│ │ │ │ -
118 static void Print(const Key& val, const std::string& str = "") {
│ │ │ │ -
119 PrintKey(val, str);
│ │ │ │ -
120 }
│ │ │ │ -
121 static bool Equals(const Key& val1, const Key& val2, double tol = 1e-8) {
│ │ │ │ -
122 return val1 == val2;
│ │ │ │ -
123 }
│ │ │ │ -
124};
│ │ │ │ +
103 return subtree_count;
│ │ │ │ +
104 }
│ │ │ │
│ │ │ │ -
125
│ │ │ │ -
126} // namespace gtsam
│ │ │ │ +
105
│ │ │ │ +
106 /* ************************************************************************* */
│ │ │ │ +
107 // The shortcut density is a conditional P(S|R) of the separator of this
│ │ │ │ +
108 // clique on the root. We can compute it recursively from the parent shortcut
│ │ │ │ +
109 // P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p
│ │ │ │ +
110 /* ************************************************************************* */
│ │ │ │ +
111 template<class DERIVED, class FACTORGRAPH>
│ │ │ │ +
112 typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::BayesNetType
│ │ │ │ +
│ │ │ │ +
113 BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::shortcut(const derived_ptr& B, Eliminate function) const
│ │ │ │ +
114 {
│ │ │ │ +
115 gttic(BayesTreeCliqueBase_shortcut);
│ │ │ │ +
116 // We only calculate the shortcut when this clique is not B
│ │ │ │ +
117 // and when the S\B is not empty
│ │ │ │ +
118 KeyVector S_setminus_B = separator_setminus_B(B);
│ │ │ │ +
119 if (!parent_.expired() /*(if we're not the root)*/ && !S_setminus_B.empty())
│ │ │ │ +
120 {
│ │ │ │ +
121 // Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph
│ │ │ │ +
122 derived_ptr parent(parent_.lock());
│ │ │ │ +
│ │ │ │ +
123 gttoc(BayesTreeCliqueBase_shortcut);
│ │ │ │ +
124 FactorGraphType p_Cp_B(parent->shortcut(B, function)); // P(Sp||B)
│ │ │ │ +
125 gttic(BayesTreeCliqueBase_shortcut);
│ │ │ │ +
126 p_Cp_B += parent->conditional_; // P(Fp|Sp)
│ │ │ │
127
│ │ │ │ -
128
│ │ │ │ -
129
│ │ │ │ +
128 // Determine the variables we want to keepSet, S union B
│ │ │ │ +
│ │ │ │ +
129 KeyVector keep = shortcut_indices(B, p_Cp_B);
│ │ │ │
130
│ │ │ │ -
FastMap.h
A thin wrapper around std::map that uses boost's fast_pool_allocator.
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ -
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ -
FastList.h
A thin wrapper around std::list that uses boost's fast_pool_allocator.
│ │ │ │ -
FastSet.h
A thin wrapper around std::set that uses boost's fast_pool_allocator.
│ │ │ │ -
types.h
Typedefs for easier changing of types.
│ │ │ │ -
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │ +
131 // Marginalize out everything except S union B
│ │ │ │ +
│ │ │ │ +
132 boost::shared_ptr<FactorGraphType> p_S_B = p_Cp_B.marginal(keep, function);
│ │ │ │ +
133 return *p_S_B->eliminatePartialSequential(S_setminus_B, function).first;
│ │ │ │ +
134 }
│ │ │ │ +
135 else
│ │ │ │ +
136 {
│ │ │ │ +
137 return BayesNetType();
│ │ │ │ +
138 }
│ │ │ │ +
139 }
│ │ │ │ +
140
│ │ │ │ +
141 /* *********************************************************************** */
│ │ │ │ +
142 // separator marginal, uses separator marginal of parent recursively
│ │ │ │ +
143 // P(C) = P(F|S) P(S)
│ │ │ │ +
144 /* *********************************************************************** */
│ │ │ │ +
145 template <class DERIVED, class FACTORGRAPH>
│ │ │ │ +
146 typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::FactorGraphType
│ │ │ │ +
│ │ │ │ +
147 BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separatorMarginal(
│ │ │ │ +
148 Eliminate function) const {
│ │ │ │ +
149 std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
│ │ │ │ +
150 gttic(BayesTreeCliqueBase_separatorMarginal);
│ │ │ │ +
151 // Check if the Separator marginal was already calculated
│ │ │ │ +
152 if (!cachedSeparatorMarginal_) {
│ │ │ │ +
│ │ │ │ +
153 gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
│ │ │ │ +
154
│ │ │ │ +
155 // If this is the root, there is no separator
│ │ │ │ +
│ │ │ │ +
156 if (parent_.expired() /*(if we're the root)*/) {
│ │ │ │ +
157 // we are root, return empty
│ │ │ │ +
158 FactorGraphType empty;
│ │ │ │ +
159 cachedSeparatorMarginal_ = empty;
│ │ │ │ +
160 } else {
│ │ │ │ +
161 // Flatten recursion in timing outline
│ │ │ │ +
162 gttoc(BayesTreeCliqueBase_separatorMarginal_cachemiss);
│ │ │ │ +
163 gttoc(BayesTreeCliqueBase_separatorMarginal);
│ │ │ │ +
164
│ │ │ │ +
165 // Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp)
│ │ │ │ +
166 // initialize P(Cp) with the parent separator marginal
│ │ │ │ +
167 derived_ptr parent(parent_.lock());
│ │ │ │ +
168 FactorGraphType p_Cp(parent->separatorMarginal(function)); // P(Sp)
│ │ │ │ +
│ │ │ │ +
169
│ │ │ │ +
170 gttic(BayesTreeCliqueBase_separatorMarginal);
│ │ │ │ +
171 gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
│ │ │ │ +
│ │ │ │ +
172
│ │ │ │ +
173 // now add the parent conditional
│ │ │ │ +
174 p_Cp += parent->conditional_; // P(Fp|Sp)
│ │ │ │ +
│ │ │ │ +
175
│ │ │ │ +
176 // The variables we want to keepSet are exactly the ones in S
│ │ │ │ +
177 KeyVector indicesS(this->conditional()->beginParents(),
│ │ │ │ +
178 this->conditional()->endParents());
│ │ │ │ +
179 auto separatorMarginal =
│ │ │ │ +
180 p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), function);
│ │ │ │ +
│ │ │ │ +
181 cachedSeparatorMarginal_.reset(*separatorMarginal);
│ │ │ │ +
182 }
│ │ │ │ +
183 }
│ │ │ │ +
184
│ │ │ │ +
185 // return the shortcut P(S||B)
│ │ │ │ +
186 return *cachedSeparatorMarginal_; // return the cached version
│ │ │ │ +
187 }
│ │ │ │ +
188
│ │ │ │ +
189 /* *********************************************************************** */
│ │ │ │ +
190 // marginal2, uses separator marginal of parent
│ │ │ │ +
191 // P(C) = P(F|S) P(S)
│ │ │ │ +
192 /* *********************************************************************** */
│ │ │ │ +
│ │ │ │ +
193 template <class DERIVED, class FACTORGRAPH>
│ │ │ │ +
194 typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::FactorGraphType
│ │ │ │ +
│ │ │ │ +
195 BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::marginal2(
│ │ │ │ +
196 Eliminate function) const {
│ │ │ │ +
197 gttic(BayesTreeCliqueBase_marginal2);
│ │ │ │ +
│ │ │ │ +
198 // initialize with separator marginal P(S)
│ │ │ │ +
199 FactorGraphType p_C = this->separatorMarginal(function);
│ │ │ │ +
200 // add the conditional P(F|S)
│ │ │ │ +
201 p_C += boost::shared_ptr<FactorType>(this->conditional_);
│ │ │ │ +
202 return p_C;
│ │ │ │ +
203 }
│ │ │ │ +
204
│ │ │ │ +
205 /* ************************************************************************* */
│ │ │ │ +
206 template<class DERIVED, class FACTORGRAPH>
│ │ │ │ +
│ │ │ │ +
207 void BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::deleteCachedShortcuts() {
│ │ │ │ +
208
│ │ │ │ +
209 // When a shortcut is requested, all of the shortcuts between it and the
│ │ │ │ +
210 // root are also generated. So, if this clique's cached shortcut is set,
│ │ │ │ +
211 // recursively call over all child cliques. Otherwise, it is unnecessary.
│ │ │ │ +
212
│ │ │ │ +
213 std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
│ │ │ │ +
214 if (cachedSeparatorMarginal_) {
│ │ │ │ +
215 for(derived_ptr& child: children) {
│ │ │ │ +
216 child->deleteCachedShortcuts();
│ │ │ │ +
217 }
│ │ │ │ +
218
│ │ │ │ +
219 //Delete CachedShortcut for this clique
│ │ │ │ +
220 cachedSeparatorMarginal_ = boost::none;
│ │ │ │ +
221 }
│ │ │ │ +
222
│ │ │ │ +
223 }
│ │ │ │ +
│ │ │ │ +
224
│ │ │ │ +
225}
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
timing.h
Timing utilities.
│ │ │ │ +
FactorGraph-inst.h
Factor Graph Base Class.
│ │ │ │ +
BayesTreeCliqueBase.h
Base class for cliques of a BayesTree.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::PrintKeyVector
void PrintKeyVector(const KeyVector &keys, const string &s, const KeyFormatter &keyFormatter)
Utility function to print sets of keys with optional prefix.
Definition Key.cpp:77
│ │ │ │ -
gtsam::PrintKey
void PrintKey(Key key, const string &s, const KeyFormatter &keyFormatter)
Utility function to print one key with optional prefix.
Definition Key.cpp:40
│ │ │ │ -
gtsam::PrintKeyList
void PrintKeyList(const KeyList &keys, const string &s, const KeyFormatter &keyFormatter)
Utility function to print sets of keys with optional prefix.
Definition Key.cpp:72
│ │ │ │ -
gtsam::PrintKeySet
void PrintKeySet(const KeySet &keys, const string &s, const KeyFormatter &keyFormatter)
Utility function to print sets of keys with optional prefix.
Definition Key.cpp:82
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::FastList
FastList is a thin wrapper around std::list that uses the boost fast_pool_allocator instead of the de...
Definition FastList.h:40
│ │ │ │ -
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ │ │ │ │ -
gtsam::StreamedKey
To use the key_formatter on Keys, they must be wrapped in a StreamedKey.
Definition Key.h:58
│ │ │ │ -
gtsam::key_formatter
Output stream manipulator that will format gtsam::Keys according to the given KeyFormatter,...
Definition Key.h:72
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::DiscreteFactorGraph
A Discrete Factor Graph is a factor graph where all factors are Discrete, i.e.
Definition DiscreteFactorGraph.h:101
│ │ │ │ +
gtsam::BayesTreeCliqueBase::treeSize
size_t treeSize() const
The size of subtree rooted at this clique, i.e., nr of Cliques.
Definition BayesTreeCliqueBase-inst.h:84
│ │ │ │ +
gtsam::BayesTreeCliqueBase::marginal2
FactorGraphType marginal2(Eliminate function=EliminationTraitsType::DefaultEliminate) const
return the marginal P(C) of the clique, using marginal caching
Definition BayesTreeCliqueBase-inst.h:195
│ │ │ │ +
gtsam::BayesTreeCliqueBase::equals
bool equals(const DERIVED &other, double tol=1e-9) const
check equality
Definition BayesTreeCliqueBase-inst.h:35
│ │ │ │ +
gtsam::BayesTreeCliqueBase::separatorMarginal
FactorGraphType separatorMarginal(Eliminate function=EliminationTraitsType::DefaultEliminate) const
return the marginal P(S) on the separator
Definition BayesTreeCliqueBase-inst.h:147
│ │ │ │ +
gtsam::BayesTreeCliqueBase::shortcut
BayesNetType shortcut(const derived_ptr &root, Eliminate function=EliminationTraitsType::DefaultEliminate) const
return the conditional P(S|Root) on the separator given the root
Definition BayesTreeCliqueBase-inst.h:113
│ │ │ │ +
gtsam::BayesTreeCliqueBase::deleteCachedShortcuts
void deleteCachedShortcuts()
This deletes the cached shortcuts of all cliques (subtree) below this clique.
Definition BayesTreeCliqueBase-inst.h:207
│ │ │ │ +
gtsam::BayesTreeCliqueBase::shortcut_indices
KeyVector shortcut_indices(const derived_ptr &B, const FactorGraphType &p_Cp_B) const
Determine variable indices to keep in recursive separator shortcut calculation The factor graph p_Cp_...
Definition BayesTreeCliqueBase-inst.h:57
│ │ │ │ +
gtsam::BayesTreeCliqueBase::separator_setminus_B
KeyVector separator_setminus_B(const derived_ptr &B) const
Calculate set for shortcut calculations.
Definition BayesTreeCliqueBase-inst.h:45
│ │ │ │ +
gtsam::BayesTreeCliqueBase::print
virtual void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print this node
Definition BayesTreeCliqueBase-inst.h:76
│ │ │ │ +
gtsam::BayesTreeCliqueBase::numCachedSeparatorMarginals
size_t numCachedSeparatorMarginals() const
Collect number of cliques with cached separator marginals.
Definition BayesTreeCliqueBase-inst.h:93
│ │ │ │ +
gtsam::BayesTreeCliqueBase::setEliminationResult
void setEliminationResult(const typename FactorGraphType::EliminationResult &eliminationResult)
Fill the elimination result produced during elimination.
Definition BayesTreeCliqueBase-inst.h:27
│ │ │ │ +
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,187 +1,329 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Key.h │ │ │ │ │ +BayesTreeCliqueBase-inst.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ -9 │ │ │ │ │ -10 * ------------------------------------------------------------------------- │ │ │ │ │ +9 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ -11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_L_i_s_t_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_M_a_p_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_S_e_t_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ -26#include │ │ │ │ │ -27 │ │ │ │ │ -28#include │ │ │ │ │ -29 │ │ │ │ │ -30#include │ │ │ │ │ -31 │ │ │ │ │ -32namespace _g_t_s_a_m { │ │ │ │ │ -33 │ │ │ │ │ -_3_5using _K_e_y_F_o_r_m_a_t_t_e_r = std::function; │ │ │ │ │ -36 │ │ │ │ │ -37// Helper function for DefaultKeyFormatter │ │ │ │ │ -38GTSAM_EXPORT std::string _defaultKeyFormatter(_K_e_y key); │ │ │ │ │ -39 │ │ │ │ │ -43static const _K_e_y_F_o_r_m_a_t_t_e_r DefaultKeyFormatter = &_defaultKeyFormatter; │ │ │ │ │ -44 │ │ │ │ │ -45// Helper function for Multi-robot Key Formatter │ │ │ │ │ -46GTSAM_EXPORT std::string _multirobotKeyFormatter(_g_t_s_a_m_:_:_K_e_y key); │ │ │ │ │ -47 │ │ │ │ │ -54static const _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r MultiRobotKeyFormatter = │ │ │ │ │ -55 &_multirobotKeyFormatter; │ │ │ │ │ -56 │ │ │ │ │ -_5_8struct _S_t_r_e_a_m_e_d_K_e_y { │ │ │ │ │ -59 const _K_e_y &key_; │ │ │ │ │ -60 explicit _S_t_r_e_a_m_e_d_K_e_y(const _K_e_y &key) : key_(key) {} │ │ │ │ │ -61 GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &, const │ │ │ │ │ -_S_t_r_e_a_m_e_d_K_e_y &); │ │ │ │ │ -62}; │ │ │ │ │ -63 │ │ │ │ │ -_7_2class _k_e_y___f_o_r_m_a_t_t_e_r { │ │ │ │ │ -73 public: │ │ │ │ │ -74 explicit _k_e_y___f_o_r_m_a_t_t_e_r(_K_e_y_F_o_r_m_a_t_t_e_r v) : formatter_(v) {} │ │ │ │ │ -75 GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &, const │ │ │ │ │ -_k_e_y___f_o_r_m_a_t_t_e_r &); │ │ │ │ │ -76 GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &, const │ │ │ │ │ -_S_t_r_e_a_m_e_d_K_e_y &); │ │ │ │ │ -77 │ │ │ │ │ -78 private: │ │ │ │ │ -79 _K_e_y_F_o_r_m_a_t_t_e_r formatter_; │ │ │ │ │ -80 static void *&property(std::ios_base &s); │ │ │ │ │ -81 static void set_property(std::ios_base &s, const _K_e_y_F_o_r_m_a_t_t_e_r &f); │ │ │ │ │ -82 static _K_e_y_F_o_r_m_a_t_t_e_r *get_property(std::ios_base &s); │ │ │ │ │ -83}; │ │ │ │ │ -84 │ │ │ │ │ -_8_6using _K_e_y_V_e_c_t_o_r = _F_a_s_t_V_e_c_t_o_r_<_K_e_y_>; │ │ │ │ │ -87 │ │ │ │ │ -88// TODO(frank): Nothing fast about these :-( │ │ │ │ │ -89using _K_e_y_L_i_s_t = _F_a_s_t_L_i_s_t_<_K_e_y_>; │ │ │ │ │ -90using _K_e_y_S_e_t = _F_a_s_t_S_e_t_<_K_e_y_>; │ │ │ │ │ -91using KeyGroupMap = _F_a_s_t_M_a_p_<_K_e_y_,_ _i_n_t_>; │ │ │ │ │ -92 │ │ │ │ │ -94GTSAM_EXPORT void _P_r_i_n_t_K_e_y( │ │ │ │ │ -95 _K_e_y key, const std::string &s = "", │ │ │ │ │ -96 const _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter = DefaultKeyFormatter); │ │ │ │ │ -97 │ │ │ │ │ -99GTSAM_EXPORT void _P_r_i_n_t_K_e_y_L_i_s_t( │ │ │ │ │ -100 const _K_e_y_L_i_s_t &keys, const std::string &s = "", │ │ │ │ │ -101 const _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter = DefaultKeyFormatter); │ │ │ │ │ +10 │ │ │ │ │ +17#pragma once │ │ │ │ │ +18 │ │ │ │ │ +19#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._h> │ │ │ │ │ +20#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_-_i_n_s_t_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_b_a_s_e_/_t_i_m_i_n_g_._h> │ │ │ │ │ +22 │ │ │ │ │ +23namespace _g_t_s_a_m { │ │ │ │ │ +24 │ │ │ │ │ +25 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +26 template │ │ │ │ │ +_2_7 void _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_D_E_R_I_V_E_D_,_ _F_A_C_T_O_R_G_R_A_P_H_>_:_:_s_e_t_E_l_i_m_i_n_a_t_i_o_n_R_e_s_u_l_t( │ │ │ │ │ +28 const typename FactorGraphType::EliminationResult& eliminationResult) │ │ │ │ │ +29 { │ │ │ │ │ +30 conditional_ = eliminationResult.first; │ │ │ │ │ +31 } │ │ │ │ │ +32 │ │ │ │ │ +33 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +34 template │ │ │ │ │ +_3_5 bool _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_D_E_R_I_V_E_D_,_ _F_A_C_T_O_R_G_R_A_P_H_>_:_:_e_q_u_a_l_s( │ │ │ │ │ +36 const DERIVED& other, double tol) const │ │ │ │ │ +37 { │ │ │ │ │ +38 return (!conditional_ && !other.conditional()) │ │ │ │ │ +39 || conditional_->_e_q_u_a_l_s(*other.conditional(), tol); │ │ │ │ │ +40 } │ │ │ │ │ +41 │ │ │ │ │ +42 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +43 template │ │ │ │ │ +44 _K_e_y_V_e_c_t_o_r │ │ │ │ │ +_4_5 _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_D_E_R_I_V_E_D_,_ _F_A_C_T_O_R_G_R_A_P_H_>_:_:_s_e_p_a_r_a_t_o_r___s_e_t_m_i_n_u_s___B(const │ │ │ │ │ +derived_ptr& B) const │ │ │ │ │ +46 { │ │ │ │ │ +47 _K_e_y_S_e_t p_F_S_parents(this->conditional()->beginParents(), this->conditional │ │ │ │ │ +()->endParents()); │ │ │ │ │ +48 _K_e_y_S_e_t indicesB(B->conditional()->begin(), B->conditional()->end()); │ │ │ │ │ +49 _K_e_y_V_e_c_t_o_r S_setminus_B; │ │ │ │ │ +50 std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(), │ │ │ │ │ +51 indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B)); │ │ │ │ │ +52 return S_setminus_B; │ │ │ │ │ +53 } │ │ │ │ │ +54 │ │ │ │ │ +55 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +56 template │ │ │ │ │ +_5_7 _K_e_y_V_e_c_t_o_r _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_D_E_R_I_V_E_D_,_ _F_A_C_T_O_R_G_R_A_P_H_>_:_:_s_h_o_r_t_c_u_t___i_n_d_i_c_e_s( │ │ │ │ │ +58 const derived_ptr& B, const FactorGraphType& p_Cp_B) const │ │ │ │ │ +59 { │ │ │ │ │ +60 gttic(shortcut_indices); │ │ │ │ │ +61 _K_e_y_S_e_t allKeys = p_Cp_B.keys(); │ │ │ │ │ +62 _K_e_y_S_e_t indicesB(B->conditional()->begin(), B->conditional()->end()); │ │ │ │ │ +63 _K_e_y_V_e_c_t_o_r S_setminus_B = separator_setminus_B(B); │ │ │ │ │ +64 _K_e_y_V_e_c_t_o_r keep; │ │ │ │ │ +65 // keep = S\B intersect allKeys (S_setminus_B is already sorted) │ │ │ │ │ +66 std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), // │ │ │ │ │ +67 allKeys.begin(), allKeys.end(), back_inserter(keep)); │ │ │ │ │ +68 // keep += B intersect allKeys │ │ │ │ │ +69 std::set_intersection(indicesB.begin(), indicesB.end(), // │ │ │ │ │ +70 allKeys.begin(), allKeys.end(), back_inserter(keep)); │ │ │ │ │ +71 return keep; │ │ │ │ │ +72 } │ │ │ │ │ +73 │ │ │ │ │ +74 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +75 template │ │ │ │ │ +_7_6 void _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_D_E_R_I_V_E_D_,_ _F_A_C_T_O_R_G_R_A_P_H_>_:_:_p_r_i_n_t( │ │ │ │ │ +77 const std::string& s, const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter) const │ │ │ │ │ +78 { │ │ │ │ │ +79 conditional_->print(s, keyFormatter); │ │ │ │ │ +80 } │ │ │ │ │ +81 │ │ │ │ │ +82 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +83 template │ │ │ │ │ +_8_4 size_t _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_D_E_R_I_V_E_D_,_ _F_A_C_T_O_R_G_R_A_P_H_>_:_:_t_r_e_e_S_i_z_e() const { │ │ │ │ │ +85 size_t size = 1; │ │ │ │ │ +86 for(const derived_ptr& child: children) │ │ │ │ │ +87 size += child->treeSize(); │ │ │ │ │ +88 return size; │ │ │ │ │ +89 } │ │ │ │ │ +90 │ │ │ │ │ +91 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +92 template │ │ │ │ │ +_9_3 size_t _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_D_E_R_I_V_E_D_,_ _F_A_C_T_O_R_G_R_A_P_H_>_:_: │ │ │ │ │ +_n_u_m_C_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l_s() const │ │ │ │ │ +94 { │ │ │ │ │ +95 std::lock_guard marginalLock(cachedSeparatorMarginalMutex_); │ │ │ │ │ +96 if (!cachedSeparatorMarginal_) │ │ │ │ │ +97 return 0; │ │ │ │ │ +98 │ │ │ │ │ +99 size_t subtree_count = 1; │ │ │ │ │ +100 for(const derived_ptr& child: children) │ │ │ │ │ +101 subtree_count += child->numCachedSeparatorMarginals(); │ │ │ │ │ 102 │ │ │ │ │ -104GTSAM_EXPORT void _P_r_i_n_t_K_e_y_V_e_c_t_o_r( │ │ │ │ │ -105 const _K_e_y_V_e_c_t_o_r &keys, const std::string &s = "", │ │ │ │ │ -106 const _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter = DefaultKeyFormatter); │ │ │ │ │ -107 │ │ │ │ │ -109GTSAM_EXPORT void _P_r_i_n_t_K_e_y_S_e_t( │ │ │ │ │ -110 const _K_e_y_S_e_t &keys, const std::string &s = "", │ │ │ │ │ -111 const _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter = DefaultKeyFormatter); │ │ │ │ │ -112 │ │ │ │ │ -113// Define Key to be Testable by specializing gtsam::traits │ │ │ │ │ -114template struct _t_r_a_i_t_s; │ │ │ │ │ -115 │ │ │ │ │ -116template <> │ │ │ │ │ -_1_1_7struct _t_r_a_i_t_s<_K_e_y> { │ │ │ │ │ -118 static void Print(const _K_e_y& val, const std::string& str = "") { │ │ │ │ │ -119 _P_r_i_n_t_K_e_y(val, str); │ │ │ │ │ -120 } │ │ │ │ │ -121 static bool Equals(const _K_e_y& val1, const _K_e_y& val2, double tol = 1e-8) { │ │ │ │ │ -122 return val1 == val2; │ │ │ │ │ -123 } │ │ │ │ │ -124}; │ │ │ │ │ -125 │ │ │ │ │ -126} // namespace gtsam │ │ │ │ │ +103 return subtree_count; │ │ │ │ │ +104 } │ │ │ │ │ +105 │ │ │ │ │ +106 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +107 // The shortcut density is a conditional P(S|R) of the separator of this │ │ │ │ │ +108 // clique on the root. We can compute it recursively from the parent │ │ │ │ │ +shortcut │ │ │ │ │ +109 // P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p │ │ │ │ │ +110 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +111 template │ │ │ │ │ +112 typename BayesTreeCliqueBase::BayesNetType │ │ │ │ │ +_1_1_3 _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_D_E_R_I_V_E_D_,_ _F_A_C_T_O_R_G_R_A_P_H_>_:_:_s_h_o_r_t_c_u_t(const derived_ptr& B, │ │ │ │ │ +Eliminate function) const │ │ │ │ │ +114 { │ │ │ │ │ +115 gttic(BayesTreeCliqueBase_shortcut); │ │ │ │ │ +116 // We only calculate the shortcut when this clique is not B │ │ │ │ │ +117 // and when the S\B is not empty │ │ │ │ │ +118 _K_e_y_V_e_c_t_o_r S_setminus_B = separator_setminus_B(B); │ │ │ │ │ +119 if (!parent_.expired() /*(if we're not the root)*/ && !S_setminus_B.empty │ │ │ │ │ +()) │ │ │ │ │ +120 { │ │ │ │ │ +121 // Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph │ │ │ │ │ +122 derived_ptr parent(parent_.lock()); │ │ │ │ │ +_1_2_3 gttoc(BayesTreeCliqueBase_shortcut); │ │ │ │ │ +124 _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e p_Cp_B(parent->shortcut(B, function)); // P(Sp||B) │ │ │ │ │ +125 gttic(BayesTreeCliqueBase_shortcut); │ │ │ │ │ +126 p_Cp_B += parent->conditional_; // P(Fp|Sp) │ │ │ │ │ 127 │ │ │ │ │ -128 │ │ │ │ │ -129 │ │ │ │ │ +128 // Determine the variables we want to keepSet, S union B │ │ │ │ │ +_1_2_9 _K_e_y_V_e_c_t_o_r keep = shortcut_indices(B, p_Cp_B); │ │ │ │ │ 130 │ │ │ │ │ -_F_a_s_t_M_a_p_._h │ │ │ │ │ -A thin wrapper around std::map that uses boost's fast_pool_allocator. │ │ │ │ │ -_T_e_s_t_a_b_l_e_._h │ │ │ │ │ -Concept check for values that can be used in unit tests. │ │ │ │ │ -_F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ -A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ -_F_a_s_t_L_i_s_t_._h │ │ │ │ │ -A thin wrapper around std::list that uses boost's fast_pool_allocator. │ │ │ │ │ -_F_a_s_t_S_e_t_._h │ │ │ │ │ -A thin wrapper around std::set that uses boost's fast_pool_allocator. │ │ │ │ │ -_t_y_p_e_s_._h │ │ │ │ │ -Typedefs for easier changing of types. │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ -std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ -FastVector │ │ │ │ │ -FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ -DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ +131 // Marginalize out everything except S union B │ │ │ │ │ +_1_3_2 boost::shared_ptr p_S_B = p_Cp_B.marginal(keep, function); │ │ │ │ │ +133 return *p_S_B->eliminatePartialSequential(S_setminus_B, function).first; │ │ │ │ │ +134 } │ │ │ │ │ +135 else │ │ │ │ │ +136 { │ │ │ │ │ +137 return BayesNetType(); │ │ │ │ │ +138 } │ │ │ │ │ +139 } │ │ │ │ │ +140 │ │ │ │ │ +141 /* *********************************************************************** │ │ │ │ │ +*/ │ │ │ │ │ +142 // separator marginal, uses separator marginal of parent recursively │ │ │ │ │ +143 // P(C) = P(F|S) P(S) │ │ │ │ │ +144 /* *********************************************************************** │ │ │ │ │ +*/ │ │ │ │ │ +145 template │ │ │ │ │ +146 typename BayesTreeCliqueBase::FactorGraphType │ │ │ │ │ +_1_4_7 _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_D_E_R_I_V_E_D_,_ _F_A_C_T_O_R_G_R_A_P_H_>_:_:_s_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l( │ │ │ │ │ +148 Eliminate function) const { │ │ │ │ │ +149 std::lock_guard marginalLock(cachedSeparatorMarginalMutex_); │ │ │ │ │ +150 gttic(BayesTreeCliqueBase_separatorMarginal); │ │ │ │ │ +151 // Check if the Separator marginal was already calculated │ │ │ │ │ +152 if (!cachedSeparatorMarginal_) { │ │ │ │ │ +_1_5_3 gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss); │ │ │ │ │ +154 │ │ │ │ │ +155 // If this is the root, there is no separator │ │ │ │ │ +_1_5_6 if (parent_.expired() /*(if we're the root)*/) { │ │ │ │ │ +157 // we are root, return empty │ │ │ │ │ +158 _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e empty; │ │ │ │ │ +159 cachedSeparatorMarginal_ = empty; │ │ │ │ │ +160 } else { │ │ │ │ │ +161 // Flatten recursion in timing outline │ │ │ │ │ +162 gttoc(BayesTreeCliqueBase_separatorMarginal_cachemiss); │ │ │ │ │ +163 gttoc(BayesTreeCliqueBase_separatorMarginal); │ │ │ │ │ +164 │ │ │ │ │ +165 // Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp) │ │ │ │ │ +166 // initialize P(Cp) with the parent separator marginal │ │ │ │ │ +167 derived_ptr parent(parent_.lock()); │ │ │ │ │ +168 FactorGraphType p_Cp(parent->separatorMarginal(function)); // P(Sp) │ │ │ │ │ +_1_6_9 │ │ │ │ │ +170 gttic(BayesTreeCliqueBase_separatorMarginal); │ │ │ │ │ +171 gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss); │ │ │ │ │ +_1_7_2 │ │ │ │ │ +173 // now add the parent conditional │ │ │ │ │ +174 p_Cp += parent->conditional_; // P(Fp|Sp) │ │ │ │ │ +_1_7_5 │ │ │ │ │ +176 // The variables we want to keepSet are exactly the ones in S │ │ │ │ │ +177 _K_e_y_V_e_c_t_o_r indicesS(this->conditional()->beginParents(), │ │ │ │ │ +178 this->conditional()->endParents()); │ │ │ │ │ +179 auto separatorMarginal = │ │ │ │ │ +180 p_Cp.marginalMultifrontalBayesNet(_O_r_d_e_r_i_n_g(indicesS), function); │ │ │ │ │ +_1_8_1 cachedSeparatorMarginal_.reset(*separatorMarginal); │ │ │ │ │ +182 } │ │ │ │ │ +183 } │ │ │ │ │ +184 │ │ │ │ │ +185 // return the shortcut P(S||B) │ │ │ │ │ +186 return *cachedSeparatorMarginal_; // return the cached version │ │ │ │ │ +187 } │ │ │ │ │ +188 │ │ │ │ │ +189 /* *********************************************************************** │ │ │ │ │ +*/ │ │ │ │ │ +190 // marginal2, uses separator marginal of parent │ │ │ │ │ +191 // P(C) = P(F|S) P(S) │ │ │ │ │ +192 /* *********************************************************************** │ │ │ │ │ +*/ │ │ │ │ │ +_1_9_3 template │ │ │ │ │ +194 typename BayesTreeCliqueBase::FactorGraphType │ │ │ │ │ +_1_9_5 _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_D_E_R_I_V_E_D_,_ _F_A_C_T_O_R_G_R_A_P_H_>_:_:_m_a_r_g_i_n_a_l_2( │ │ │ │ │ +196 Eliminate function) const { │ │ │ │ │ +197 gttic(BayesTreeCliqueBase_marginal2); │ │ │ │ │ +_1_9_8 // initialize with separator marginal P(S) │ │ │ │ │ +199 _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e p_C = this->separatorMarginal(function); │ │ │ │ │ +200 // add the conditional P(F|S) │ │ │ │ │ +201 p_C += boost::shared_ptr(this->conditional_); │ │ │ │ │ +202 return p_C; │ │ │ │ │ +203 } │ │ │ │ │ +204 │ │ │ │ │ +205 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +206 template │ │ │ │ │ +_2_0_7 void _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_D_E_R_I_V_E_D_,_ _F_A_C_T_O_R_G_R_A_P_H_>_:_:_d_e_l_e_t_e_C_a_c_h_e_d_S_h_o_r_t_c_u_t_s() { │ │ │ │ │ +208 │ │ │ │ │ +209 // When a shortcut is requested, all of the shortcuts between it and the │ │ │ │ │ +210 // root are also generated. So, if this clique's cached shortcut is set, │ │ │ │ │ +211 // recursively call over all child cliques. Otherwise, it is unnecessary. │ │ │ │ │ +212 │ │ │ │ │ +213 std::lock_guard marginalLock(cachedSeparatorMarginalMutex_); │ │ │ │ │ +214 if (cachedSeparatorMarginal_) { │ │ │ │ │ +215 for(derived_ptr& child: children) { │ │ │ │ │ +216 child->deleteCachedShortcuts(); │ │ │ │ │ +217 } │ │ │ │ │ +218 │ │ │ │ │ +219 //Delete CachedShortcut for this clique │ │ │ │ │ +220 cachedSeparatorMarginal_ = boost::none; │ │ │ │ │ +221 } │ │ │ │ │ +222 │ │ │ │ │ +223 } │ │ │ │ │ +224 │ │ │ │ │ +225} │ │ │ │ │ +_t_i_m_i_n_g_._h │ │ │ │ │ +Timing utilities. │ │ │ │ │ +_F_a_c_t_o_r_G_r_a_p_h_-_i_n_s_t_._h │ │ │ │ │ +Factor Graph Base Class. │ │ │ │ │ +_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._h │ │ │ │ │ +Base class for cliques of a BayesTree. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ FastVector< Key > KeyVector │ │ │ │ │ Define collection type once and for all - also used in wrappers. │ │ │ │ │ DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_i_n_t_K_e_y_V_e_c_t_o_r │ │ │ │ │ -void PrintKeyVector(const KeyVector &keys, const string &s, const KeyFormatter │ │ │ │ │ -&keyFormatter) │ │ │ │ │ -Utility function to print sets of keys with optional prefix. │ │ │ │ │ -DDeeffiinniittiioonn Key.cpp:77 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_i_n_t_K_e_y │ │ │ │ │ -void PrintKey(Key key, const string &s, const KeyFormatter &keyFormatter) │ │ │ │ │ -Utility function to print one key with optional prefix. │ │ │ │ │ -DDeeffiinniittiioonn Key.cpp:40 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_i_n_t_K_e_y_L_i_s_t │ │ │ │ │ -void PrintKeyList(const KeyList &keys, const string &s, const KeyFormatter │ │ │ │ │ -&keyFormatter) │ │ │ │ │ -Utility function to print sets of keys with optional prefix. │ │ │ │ │ -DDeeffiinniittiioonn Key.cpp:72 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_i_n_t_K_e_y_S_e_t │ │ │ │ │ -void PrintKeySet(const KeySet &keys, const string &s, const KeyFormatter │ │ │ │ │ -&keyFormatter) │ │ │ │ │ -Utility function to print sets of keys with optional prefix. │ │ │ │ │ -DDeeffiinniittiioonn Key.cpp:82 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t │ │ │ │ │ -FastList is a thin wrapper around std::list that uses the boost │ │ │ │ │ -fast_pool_allocator instead of the de... │ │ │ │ │ -DDeeffiinniittiioonn FastList.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ -FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ -fast_pool_allocator instead of the defa... │ │ │ │ │ -DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ _g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_r_e_a_m_e_d_K_e_y │ │ │ │ │ -To use the key_formatter on Keys, they must be wrapped in a StreamedKey. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_k_e_y___f_o_r_m_a_t_t_e_r │ │ │ │ │ -Output stream manipulator that will format gtsam::Keys according to the given │ │ │ │ │ -KeyFormatter,... │ │ │ │ │ -DDeeffiinniittiioonn Key.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A Discrete Factor Graph is a factor graph where all factors are Discrete, i.e. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteFactorGraph.h:101 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_t_r_e_e_S_i_z_e │ │ │ │ │ +size_t treeSize() const │ │ │ │ │ +The size of subtree rooted at this clique, i.e., nr of Cliques. │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:84 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_m_a_r_g_i_n_a_l_2 │ │ │ │ │ +FactorGraphType marginal2(Eliminate function=EliminationTraitsType:: │ │ │ │ │ +DefaultEliminate) const │ │ │ │ │ +return the marginal P(C) of the clique, using marginal caching │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:195 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const DERIVED &other, double tol=1e-9) const │ │ │ │ │ +check equality │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_s_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l │ │ │ │ │ +FactorGraphType separatorMarginal(Eliminate function=EliminationTraitsType:: │ │ │ │ │ +DefaultEliminate) const │ │ │ │ │ +return the marginal P(S) on the separator │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:147 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_s_h_o_r_t_c_u_t │ │ │ │ │ +BayesNetType shortcut(const derived_ptr &root, Eliminate │ │ │ │ │ +function=EliminationTraitsType::DefaultEliminate) const │ │ │ │ │ +return the conditional P(S|Root) on the separator given the root │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:113 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_d_e_l_e_t_e_C_a_c_h_e_d_S_h_o_r_t_c_u_t_s │ │ │ │ │ +void deleteCachedShortcuts() │ │ │ │ │ +This deletes the cached shortcuts of all cliques (subtree) below this clique. │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:207 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_s_h_o_r_t_c_u_t___i_n_d_i_c_e_s │ │ │ │ │ +KeyVector shortcut_indices(const derived_ptr &B, const FactorGraphType &p_Cp_B) │ │ │ │ │ +const │ │ │ │ │ +Determine variable indices to keep in recursive separator shortcut calculation │ │ │ │ │ +The factor graph p_Cp_... │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:57 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_s_e_p_a_r_a_t_o_r___s_e_t_m_i_n_u_s___B │ │ │ │ │ +KeyVector separator_setminus_B(const derived_ptr &B) const │ │ │ │ │ +Calculate set for shortcut calculations. │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_p_r_i_n_t │ │ │ │ │ +virtual void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ +print this node │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:76 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_n_u_m_C_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l_s │ │ │ │ │ +size_t numCachedSeparatorMarginals() const │ │ │ │ │ +Collect number of cliques with cached separator marginals. │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:93 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_s_e_t_E_l_i_m_i_n_a_t_i_o_n_R_e_s_u_l_t │ │ │ │ │ +void setEliminationResult(const typename FactorGraphType::EliminationResult │ │ │ │ │ +&eliminationResult) │ │ │ │ │ +Fill the elimination result produced during elimination. │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:27 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _K_e_y_._h │ │ │ │ │ + * _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_-_i_n_s_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00584.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Ordering.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/MetisIndex.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,34 +94,41 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
Ordering.cpp File Reference
│ │ │ │ +
MetisIndex.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::MetisIndex
 The MetisIndex class converts a factor graph into the Compressed Sparse Row format for use in METIS algorithms. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
│ │ │ │ -Andrew Melim
│ │ │ │ -
Date
Sep 2, 2010
│ │ │ │ +
Author
Andrew Melim
│ │ │ │ +
Date
Oct. 10, 2014
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,26 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Ordering.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +MetisIndex.h File Reference │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_M_e_t_i_s_I_n_d_e_x │ │ │ │ │ +  The _M_e_t_i_s_I_n_d_e_x class converts a factor graph into the Compressed Sparse │ │ │ │ │ + Row format for use in METIS algorithms. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ Andrew Melim │ │ │ │ │ Date │ │ │ │ │ - Sep 2, 2010 │ │ │ │ │ + Oct. 10, 2014 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _O_r_d_e_r_i_n_g_._c_p_p │ │ │ │ │ + * _M_e_t_i_s_I_n_d_e_x_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00587.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Conditional.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Factor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,42 +95,56 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
Conditional.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Typedefs
│ │ │ │ +
Factor.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Base class for conditional densities. │ │ │ │ +

The base class for all factors. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::Conditional< FACTOR, DERIVEDCONDITIONAL >
class  gtsam::Factor
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Typedefs

typedef FastVector< FactorIndex > gtsam::FactorIndices
 Define collection types:
 
│ │ │ │ +typedef FastSet< FactorIndex > gtsam::FactorIndexSet
 
│ │ │ │

Detailed Description

│ │ │ │ -

Base class for conditional densities.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +

The base class for all factors.

│ │ │ │ +
Author
Kai Ni
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,24 +1,32 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Conditional.h File Reference │ │ │ │ │ -Base class for conditional densities. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ +Factor.h File Reference │ │ │ │ │ +The base class for all factors. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_<_ _F_A_C_T_O_R_,_ _D_E_R_I_V_E_D_C_O_N_D_I_T_I_O_N_A_L_ _> │ │ │ │ │ +class   _g_t_s_a_m_:_:_F_a_c_t_o_r │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +typedef _F_a_s_t_V_e_c_t_o_r< _F_a_c_t_o_r_I_n_d_e_x >  _g_t_s_a_m_:_:_F_a_c_t_o_r_I_n_d_i_c_e_s │ │ │ │ │ +  Define collection types: │ │ │ │ │ +  │ │ │ │ │ + typedef _F_a_s_t_S_e_t< _F_a_c_t_o_r_I_n_d_e_x >  ggttssaamm::::FFaaccttoorrIInnddeexxSSeett │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Base class for conditional densities. │ │ │ │ │ +The base class for all factors. │ │ │ │ │ Author │ │ │ │ │ + Kai Ni │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ + * _F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00587.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,4 @@ │ │ │ │ │ var a00587 = [ │ │ │ │ │ - ["gtsam::Conditional< FACTOR, DERIVEDCONDITIONAL >", "a03572.html", "a03572"] │ │ │ │ │ + ["gtsam::Factor", "a03592.html", "a03592"], │ │ │ │ │ + ["FactorIndices", "a00587.html#a99e63c90a8ac740d99716e73fe97e054", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00587_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Conditional.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Factor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,185 +98,183 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Conditional.h
│ │ │ │ +
Factor.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18// \callgraph
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <boost/range.hpp>
│ │ │ │ -
22
│ │ │ │ -
23#include <gtsam/inference/Key.h>
│ │ │ │ -
24
│ │ │ │ -
25namespace gtsam {
│ │ │ │ +
20// \callgraph
│ │ │ │ +
21
│ │ │ │ +
22#pragma once
│ │ │ │ +
23
│ │ │ │ +
24#include <boost/serialization/nvp.hpp>
│ │ │ │ +
25#include <boost/shared_ptr.hpp>
│ │ │ │
26
│ │ │ │ -
27 class HybridValues; // forward declaration.
│ │ │ │ -
28
│ │ │ │ -
62 template<class FACTOR, class DERIVEDCONDITIONAL>
│ │ │ │ -
│ │ │ │ -
63 class Conditional
│ │ │ │ -
64 {
│ │ │ │ -
65 protected:
│ │ │ │ -
67 size_t nrFrontals_;
│ │ │ │ -
68
│ │ │ │ -
69 private:
│ │ │ │ -
71 typedef Conditional<FACTOR,DERIVEDCONDITIONAL> This;
│ │ │ │ -
72
│ │ │ │ -
73 public:
│ │ │ │ -
75 typedef boost::iterator_range<typename FACTOR::const_iterator> Frontals;
│ │ │ │ -
76
│ │ │ │ -
78 typedef boost::iterator_range<typename FACTOR::const_iterator> Parents;
│ │ │ │ -
79
│ │ │ │ -
80 protected:
│ │ │ │ +
27#include <gtsam/base/types.h>
│ │ │ │ +
28#include <gtsam/base/FastVector.h>
│ │ │ │ +
29#include <gtsam/inference/Key.h>
│ │ │ │ +
30
│ │ │ │ +
31namespace gtsam {
│ │ │ │ +
32
│ │ │ │ +
34 typedef FastVector<FactorIndex> FactorIndices;
│ │ │ │ +
35 typedef FastSet<FactorIndex> FactorIndexSet;
│ │ │ │ +
36
│ │ │ │ +
37 class HybridValues; // forward declaration of a Value type for error.
│ │ │ │ +
38
│ │ │ │ +
│ │ │ │ +
67 class GTSAM_EXPORT Factor
│ │ │ │ +
68 {
│ │ │ │ +
69
│ │ │ │ +
70 private:
│ │ │ │ +
71 // These typedefs are private because they must be overridden in derived classes.
│ │ │ │ +
72 typedef Factor This;
│ │ │ │ +
73 typedef boost::shared_ptr<Factor> shared_ptr;
│ │ │ │ +
74
│ │ │ │ +
75 public:
│ │ │ │ +
77 typedef KeyVector::iterator iterator;
│ │ │ │ +
78
│ │ │ │ +
80 typedef KeyVector::const_iterator const_iterator;
│ │ │ │ +
81
│ │ │ │ +
82 protected:
│ │ │ │
83
│ │ │ │ -
85 Conditional() : nrFrontals_(0) {}
│ │ │ │ +
85 KeyVector keys_;
│ │ │ │
86
│ │ │ │ -
88 Conditional(size_t nrFrontals) : nrFrontals_(nrFrontals) {}
│ │ │ │
89
│ │ │ │ -
91
│ │ │ │ -
92 public:
│ │ │ │ -
95
│ │ │ │ -
97 void print(const std::string& s = "Conditional", const KeyFormatter& formatter = DefaultKeyFormatter) const;
│ │ │ │ -
98
│ │ │ │ -
100 bool equals(const This& c, double tol = 1e-9) const;
│ │ │ │ -
101
│ │ │ │ -
103
│ │ │ │ -
106
│ │ │ │ -
107 virtual ~Conditional() {}
│ │ │ │ +
91 Factor() {}
│ │ │ │ +
92
│ │ │ │ +
95 template<typename CONTAINER>
│ │ │ │ +
96 explicit Factor(const CONTAINER& keys) : keys_(keys.begin(), keys.end()) {}
│ │ │ │ +
97
│ │ │ │ +
100 template<typename ITERATOR>
│ │ │ │ +
101 Factor(ITERATOR first, ITERATOR last) : keys_(first, last) {}
│ │ │ │ +
102
│ │ │ │ +
105 template<typename CONTAINER>
│ │ │ │ +
│ │ │ │ +
106 static Factor FromKeys(const CONTAINER& keys) {
│ │ │ │ +
107 return Factor(keys.begin(), keys.end()); }
│ │ │ │ +
│ │ │ │
108
│ │ │ │ -
110 size_t nrFrontals() const { return nrFrontals_; }
│ │ │ │ -
111
│ │ │ │ -
113 size_t nrParents() const { return asFactor().size() - nrFrontals_; }
│ │ │ │ +
111 template<typename ITERATOR>
│ │ │ │ +
│ │ │ │ +
112 static Factor FromIterators(ITERATOR first, ITERATOR last) {
│ │ │ │ +
113 return Factor(first, last); }
│ │ │ │ +
│ │ │ │
114
│ │ │ │ -
│ │ │ │ -
116 Key firstFrontalKey() const {
│ │ │ │ -
117 if(nrFrontals_ > 0)
│ │ │ │ -
118 return asFactor().front();
│ │ │ │ -
119 else
│ │ │ │ -
120 throw std::invalid_argument("Requested Conditional::firstFrontalKey from a conditional with zero frontal keys");
│ │ │ │ -
121 }
│ │ │ │ -
│ │ │ │ -
122
│ │ │ │ -
124 Frontals frontals() const { return boost::make_iterator_range(beginFrontals(), endFrontals()); }
│ │ │ │ -
125
│ │ │ │ -
127 Parents parents() const { return boost::make_iterator_range(beginParents(), endParents()); }
│ │ │ │ -
128
│ │ │ │ -
133 virtual double logProbability(const HybridValues& c) const;
│ │ │ │ -
134
│ │ │ │ -
139 virtual double evaluate(const HybridValues& c) const;
│ │ │ │ -
140
│ │ │ │ -
│ │ │ │ -
142 double operator()(const HybridValues& x) const {
│ │ │ │ -
143 return evaluate(x);
│ │ │ │ -
144 }
│ │ │ │ -
│ │ │ │ -
145
│ │ │ │ -
150 virtual double logNormalizationConstant() const;
│ │ │ │ -
151
│ │ │ │ -
153 double normalizationConstant() const;
│ │ │ │ -
154
│ │ │ │ +
116
│ │ │ │ +
117 public:
│ │ │ │ +
119 // public since it is required for boost serialization and static methods.
│ │ │ │ +
120 // virtual since it is public.
│ │ │ │ +
121 // http://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#Rc-dtor-virtual
│ │ │ │ +
122 virtual ~Factor() = default;
│ │ │ │ +
123
│ │ │ │ +
126
│ │ │ │ +
128 bool empty() const { return keys_.empty(); }
│ │ │ │ +
129
│ │ │ │ +
131 Key front() const { return keys_.front(); }
│ │ │ │ +
132
│ │ │ │ +
134 Key back() const { return keys_.back(); }
│ │ │ │ +
135
│ │ │ │ +
137 const_iterator find(Key key) const { return std::find(begin(), end(), key); }
│ │ │ │ +
138
│ │ │ │ +
140 const KeyVector& keys() const { return keys_; }
│ │ │ │ +
141
│ │ │ │ +
143 const_iterator begin() const { return keys_.begin(); }
│ │ │ │ +
144
│ │ │ │ +
146 const_iterator end() const { return keys_.end(); }
│ │ │ │ +
147
│ │ │ │ +
152 virtual double error(const HybridValues& c) const;
│ │ │ │ +
153
│ │ │ │ +
157 size_t size() const { return keys_.size(); }
│ │ │ │
158
│ │ │ │ -
160 typename FACTOR::const_iterator beginFrontals() const { return asFactor().begin(); }
│ │ │ │ -
161
│ │ │ │ -
163 typename FACTOR::const_iterator endFrontals() const { return asFactor().begin() + nrFrontals_; }
│ │ │ │ -
164
│ │ │ │ -
166 typename FACTOR::const_iterator beginParents() const { return endFrontals(); }
│ │ │ │ -
167
│ │ │ │ -
169 typename FACTOR::const_iterator endParents() const { return asFactor().end(); }
│ │ │ │ -
170
│ │ │ │ -
172 size_t& nrFrontals() { return nrFrontals_; }
│ │ │ │ +
160
│ │ │ │ +
163
│ │ │ │ +
165 virtual void print(
│ │ │ │ +
166 const std::string& s = "Factor",
│ │ │ │ +
167 const KeyFormatter& formatter = DefaultKeyFormatter) const;
│ │ │ │ +
168
│ │ │ │ +
170 virtual void printKeys(
│ │ │ │ +
171 const std::string& s = "Factor",
│ │ │ │ +
172 const KeyFormatter& formatter = DefaultKeyFormatter) const;
│ │ │ │
173
│ │ │ │ -
175 typename FACTOR::iterator beginFrontals() { return asFactor().begin(); }
│ │ │ │ +
175 bool equals(const This& other, double tol = 1e-9) const;
│ │ │ │
176
│ │ │ │ -
178 typename FACTOR::iterator endFrontals() { return asFactor().begin() + nrFrontals_; }
│ │ │ │ -
179
│ │ │ │ -
181 typename FACTOR::iterator beginParents() { return asFactor().begin() + nrFrontals_; }
│ │ │ │ -
182
│ │ │ │ -
184 typename FACTOR::iterator endParents() { return asFactor().end(); }
│ │ │ │ -
185
│ │ │ │ -
199 template <class VALUES>
│ │ │ │ -
200 static bool CheckInvariants(const DERIVEDCONDITIONAL& conditional,
│ │ │ │ -
201 const VALUES& x);
│ │ │ │ -
202
│ │ │ │ -
204
│ │ │ │ -
205 private:
│ │ │ │ -
206
│ │ │ │ -
209
│ │ │ │ -
210 // Cast to factor type (non-const) (casts down to derived conditional type, then up to factor type)
│ │ │ │ -
211 FACTOR& asFactor() { return static_cast<FACTOR&>(static_cast<DERIVEDCONDITIONAL&>(*this)); }
│ │ │ │ -
212
│ │ │ │ -
213 // Cast to derived type (const) (casts down to derived conditional type, then up to factor type)
│ │ │ │ -
214 const FACTOR& asFactor() const { return static_cast<const FACTOR&>(static_cast<const DERIVEDCONDITIONAL&>(*this)); }
│ │ │ │ -
215
│ │ │ │ -
217 friend class boost::serialization::access;
│ │ │ │ -
218 template<class ARCHIVE>
│ │ │ │ -
219 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
220 ar & BOOST_SERIALIZATION_NVP(nrFrontals_);
│ │ │ │ -
221 }
│ │ │ │ -
222
│ │ │ │ -
224
│ │ │ │ -
225 };
│ │ │ │ -
│ │ │ │ -
226
│ │ │ │ -
227} // gtsam
│ │ │ │ - │ │ │ │ +
180
│ │ │ │ +
182 KeyVector& keys() { return keys_; }
│ │ │ │ +
183
│ │ │ │ +
185 iterator begin() { return keys_.begin(); }
│ │ │ │ +
186
│ │ │ │ +
188 iterator end() { return keys_.end(); }
│ │ │ │ +
189
│ │ │ │ +
191
│ │ │ │ +
192 private:
│ │ │ │ +
193
│ │ │ │ +
196
│ │ │ │ +
198 friend class boost::serialization::access;
│ │ │ │ +
199 template<class Archive>
│ │ │ │ +
200 void serialize(Archive & ar, const unsigned int /*version*/) {
│ │ │ │ +
201 ar & BOOST_SERIALIZATION_NVP(keys_);
│ │ │ │ +
202 }
│ │ │ │ +
203
│ │ │ │ +
205
│ │ │ │ +
206 };
│ │ │ │ +
│ │ │ │ +
207
│ │ │ │ +
208} // \namespace gtsam
│ │ │ │ +
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ +
types.h
Typedefs for easier changing of types.
│ │ │ │ + │ │ │ │ +
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition Factor.h:34
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::FastSet
FastSet is a thin wrapper around std::set that uses the boost fast_pool_allocator instead of the defa...
Definition FastSet.h:50
│ │ │ │
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ -
gtsam::Conditional
Definition Conditional.h:64
│ │ │ │ -
gtsam::Conditional::CheckInvariants
static bool CheckInvariants(const DERIVEDCONDITIONAL &conditional, const VALUES &x)
Check invariants of this conditional, given the values x.
Definition Conditional-inst.h:77
│ │ │ │ -
gtsam::Conditional::nrFrontals_
size_t nrFrontals_
The first nrFrontal variables are frontal and the rest are parents.
Definition Conditional.h:67
│ │ │ │ -
gtsam::Conditional::logNormalizationConstant
virtual double logNormalizationConstant() const
All conditional types need to implement a log normalization constant to make it such that error>=0.
Definition Conditional-inst.h:62
│ │ │ │ -
gtsam::Conditional::firstFrontalKey
Key firstFrontalKey() const
Convenience function to get the first frontal key.
Definition Conditional.h:116
│ │ │ │ -
gtsam::Conditional::endFrontals
FACTOR::const_iterator endFrontals() const
Iterator pointing past the last frontal key.
Definition Conditional.h:163
│ │ │ │ -
gtsam::Conditional::endParents
FACTOR::iterator endParents()
Mutable iterator pointing past the last parent key.
Definition Conditional.h:184
│ │ │ │ -
gtsam::Conditional::endFrontals
FACTOR::iterator endFrontals()
Mutable iterator pointing past the last frontal key.
Definition Conditional.h:178
│ │ │ │ -
gtsam::Conditional::evaluate
virtual double evaluate(const HybridValues &c) const
All conditional types need to implement an evaluate function, that yields a true probability.
Definition Conditional-inst.h:55
│ │ │ │ -
gtsam::Conditional::parents
Parents parents() const
return a view of the parent keys
Definition Conditional.h:127
│ │ │ │ -
gtsam::Conditional::operator()
double operator()(const HybridValues &x) const
Evaluate probability density, sugar.
Definition Conditional.h:142
│ │ │ │ -
gtsam::Conditional::Parents
boost::iterator_range< typename FACTOR::const_iterator > Parents
View of the separator keys (call parents())
Definition Conditional.h:78
│ │ │ │ -
gtsam::Conditional::logProbability
virtual double logProbability(const HybridValues &c) const
All conditional types need to implement a logProbability function, for which exp(logProbability(x)) =...
Definition Conditional-inst.h:48
│ │ │ │ -
gtsam::Conditional::nrFrontals
size_t nrFrontals() const
return the number of frontals
Definition Conditional.h:110
│ │ │ │ -
gtsam::Conditional::Conditional
Conditional()
Empty Constructor to make serialization possible.
Definition Conditional.h:85
│ │ │ │ -
gtsam::Conditional::Conditional
Conditional(size_t nrFrontals)
Constructor.
Definition Conditional.h:88
│ │ │ │ -
gtsam::Conditional::beginParents
FACTOR::iterator beginParents()
Mutable iterator pointing to the first parent key.
Definition Conditional.h:181
│ │ │ │ -
gtsam::Conditional::nrFrontals
size_t & nrFrontals()
Mutable version of nrFrontals.
Definition Conditional.h:172
│ │ │ │ -
gtsam::Conditional::beginFrontals
FACTOR::iterator beginFrontals()
Mutable iterator pointing to first frontal key.
Definition Conditional.h:175
│ │ │ │ -
gtsam::Conditional::Frontals
boost::iterator_range< typename FACTOR::const_iterator > Frontals
View of the frontal keys (call frontals())
Definition Conditional.h:75
│ │ │ │ -
gtsam::Conditional::frontals
Frontals frontals() const
return a view of the frontal keys
Definition Conditional.h:124
│ │ │ │ -
gtsam::Conditional::access
friend class boost::serialization::access
Serialization function.
Definition Conditional.h:217
│ │ │ │ -
gtsam::Conditional::beginFrontals
FACTOR::const_iterator beginFrontals() const
Iterator pointing to first frontal key.
Definition Conditional.h:160
│ │ │ │ -
gtsam::Conditional::normalizationConstant
double normalizationConstant() const
Non-virtual, exponentiate logNormalizationConstant.
Definition Conditional-inst.h:70
│ │ │ │ -
gtsam::Conditional::nrParents
size_t nrParents() const
return the number of parents
Definition Conditional.h:113
│ │ │ │ -
gtsam::Conditional::print
void print(const std::string &s="Conditional", const KeyFormatter &formatter=DefaultKeyFormatter) const
print with optional formatter
Definition Conditional-inst.h:30
│ │ │ │ -
gtsam::Conditional::endParents
FACTOR::const_iterator endParents() const
Iterator pointing past the last parent key.
Definition Conditional.h:169
│ │ │ │ -
gtsam::Conditional::beginParents
FACTOR::const_iterator beginParents() const
Iterator pointing to the first parent key.
Definition Conditional.h:166
│ │ │ │ -
HybridValues
the error.
│ │ │ │ +
gtsam::Factor
Definition Factor.h:68
│ │ │ │ +
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
│ │ │ │ +
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
│ │ │ │ +
gtsam::Factor::find
const_iterator find(Key key) const
find
Definition Factor.h:137
│ │ │ │ +
gtsam::Factor::begin
const_iterator begin() const
Iterator at beginning of involved variable keys.
Definition Factor.h:143
│ │ │ │ +
gtsam::Factor::Factor
Factor()
Default constructor for I/O.
Definition Factor.h:91
│ │ │ │ +
gtsam::Factor::end
iterator end()
Iterator at end of involved variable keys.
Definition Factor.h:188
│ │ │ │ +
gtsam::Factor::empty
bool empty() const
Whether the factor is empty (involves zero variables).
Definition Factor.h:128
│ │ │ │ +
gtsam::Factor::keys
KeyVector & keys()
Definition Factor.h:182
│ │ │ │ +
gtsam::Factor::FromIterators
static Factor FromIterators(ITERATOR first, ITERATOR last)
Construct factor from iterator keys.
Definition Factor.h:112
│ │ │ │ +
gtsam::Factor::FromKeys
static Factor FromKeys(const CONTAINER &keys)
Construct factor from container of keys.
Definition Factor.h:106
│ │ │ │ +
gtsam::Factor::begin
iterator begin()
Iterator at beginning of involved variable keys.
Definition Factor.h:185
│ │ │ │ +
gtsam::Factor::~Factor
virtual ~Factor()=default
Default destructor.
│ │ │ │ +
gtsam::Factor::Factor
Factor(ITERATOR first, ITERATOR last)
Construct factor from iterator keys.
Definition Factor.h:101
│ │ │ │ +
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition Factor.h:80
│ │ │ │ +
gtsam::Factor::end
const_iterator end() const
Iterator at end of involved variable keys.
Definition Factor.h:146
│ │ │ │ +
gtsam::Factor::Factor
Factor(const CONTAINER &keys)
Construct factor from container of keys.
Definition Factor.h:96
│ │ │ │ +
gtsam::Factor::iterator
KeyVector::iterator iterator
Iterator over keys.
Definition Factor.h:77
│ │ │ │ +
gtsam::Factor::back
Key back() const
Last key.
Definition Factor.h:134
│ │ │ │ +
gtsam::Factor::front
Key front() const
First key.
Definition Factor.h:131
│ │ │ │ +
gtsam::Factor::size
size_t size() const
Definition Factor.h:157
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,285 +1,260 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Conditional.h │ │ │ │ │ +Factor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18// \callgraph │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include │ │ │ │ │ -22 │ │ │ │ │ -23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ -24 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ +20// \callgraph │ │ │ │ │ +21 │ │ │ │ │ +22#pragma once │ │ │ │ │ +23 │ │ │ │ │ +24#include │ │ │ │ │ +25#include │ │ │ │ │ 26 │ │ │ │ │ -27 class _H_y_b_r_i_d_V_a_l_u_e_s; // forward declaration. │ │ │ │ │ -28 │ │ │ │ │ -62 template │ │ │ │ │ -_6_3 class _C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -64 { │ │ │ │ │ -65 protected: │ │ │ │ │ -_6_7 size_t _n_r_F_r_o_n_t_a_l_s__; │ │ │ │ │ -68 │ │ │ │ │ -69 private: │ │ │ │ │ -71 typedef _C_o_n_d_i_t_i_o_n_a_l_<_F_A_C_T_O_R_,_D_E_R_I_V_E_D_C_O_N_D_I_T_I_O_N_A_L_> _T_h_i_s; │ │ │ │ │ -72 │ │ │ │ │ -73 public: │ │ │ │ │ -_7_5 typedef boost::iterator_range _F_r_o_n_t_a_l_s; │ │ │ │ │ -76 │ │ │ │ │ -_7_8 typedef boost::iterator_range _P_a_r_e_n_t_s; │ │ │ │ │ -79 │ │ │ │ │ -80 protected: │ │ │ │ │ +27#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ +28#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ +29#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ +30 │ │ │ │ │ +31namespace _g_t_s_a_m { │ │ │ │ │ +32 │ │ │ │ │ +_3_4 typedef _F_a_s_t_V_e_c_t_o_r_<_F_a_c_t_o_r_I_n_d_e_x_> _F_a_c_t_o_r_I_n_d_i_c_e_s; │ │ │ │ │ +35 typedef _F_a_s_t_S_e_t_<_F_a_c_t_o_r_I_n_d_e_x_> FactorIndexSet; │ │ │ │ │ +36 │ │ │ │ │ +37 class _H_y_b_r_i_d_V_a_l_u_e_s; // forward declaration of a Value type for error. │ │ │ │ │ +38 │ │ │ │ │ +_6_7 class GTSAM_EXPORT _F_a_c_t_o_r │ │ │ │ │ +68 { │ │ │ │ │ +69 │ │ │ │ │ +70 private: │ │ │ │ │ +71 // These typedefs are private because they must be overridden in derived │ │ │ │ │ +classes. │ │ │ │ │ +72 typedef _F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ +73 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +74 │ │ │ │ │ +75 public: │ │ │ │ │ +_7_7 typedef KeyVector::iterator _i_t_e_r_a_t_o_r; │ │ │ │ │ +78 │ │ │ │ │ +_8_0 typedef KeyVector::const_iterator _c_o_n_s_t___i_t_e_r_a_t_o_r; │ │ │ │ │ +81 │ │ │ │ │ +82 protected: │ │ │ │ │ 83 │ │ │ │ │ -_8_5 _C_o_n_d_i_t_i_o_n_a_l() : _n_r_F_r_o_n_t_a_l_s__(0) {} │ │ │ │ │ +_8_5 _K_e_y_V_e_c_t_o_r _k_e_y_s__; │ │ │ │ │ 86 │ │ │ │ │ -_8_8 _C_o_n_d_i_t_i_o_n_a_l(size_t _n_r_F_r_o_n_t_a_l_s) : _n_r_F_r_o_n_t_a_l_s__(_n_r_F_r_o_n_t_a_l_s) {} │ │ │ │ │ 89 │ │ │ │ │ -91 │ │ │ │ │ -92 public: │ │ │ │ │ -95 │ │ │ │ │ -97 void _p_r_i_n_t(const std::string& s = "Conditional", const _K_e_y_F_o_r_m_a_t_t_e_r& │ │ │ │ │ -formatter = DefaultKeyFormatter) const; │ │ │ │ │ -98 │ │ │ │ │ -100 bool _e_q_u_a_l_s(const This& c, double tol = 1e-9) const; │ │ │ │ │ -101 │ │ │ │ │ -103 │ │ │ │ │ -106 │ │ │ │ │ -107 virtual _~_C_o_n_d_i_t_i_o_n_a_l() {} │ │ │ │ │ +_9_1 _F_a_c_t_o_r() {} │ │ │ │ │ +92 │ │ │ │ │ +95 template │ │ │ │ │ +_9_6 explicit _F_a_c_t_o_r(const CONTAINER& keys) : keys_(keys.begin(), keys.end()) {} │ │ │ │ │ +97 │ │ │ │ │ +100 template │ │ │ │ │ +_1_0_1 _F_a_c_t_o_r(ITERATOR first, ITERATOR last) : keys_(first, last) {} │ │ │ │ │ +102 │ │ │ │ │ +105 template │ │ │ │ │ +_1_0_6 static _F_a_c_t_o_r _F_r_o_m_K_e_y_s(const CONTAINER& keys) { │ │ │ │ │ +107 return _F_a_c_t_o_r(keys.begin(), keys.end()); } │ │ │ │ │ 108 │ │ │ │ │ -_1_1_0 size_t _n_r_F_r_o_n_t_a_l_s() const { return _n_r_F_r_o_n_t_a_l_s__; } │ │ │ │ │ -111 │ │ │ │ │ -_1_1_3 size_t _n_r_P_a_r_e_n_t_s() const { return asFactor().size() - _n_r_F_r_o_n_t_a_l_s__; } │ │ │ │ │ +111 template │ │ │ │ │ +_1_1_2 static _F_a_c_t_o_r _F_r_o_m_I_t_e_r_a_t_o_r_s(ITERATOR first, ITERATOR last) { │ │ │ │ │ +113 return _F_a_c_t_o_r(first, last); } │ │ │ │ │ 114 │ │ │ │ │ -_1_1_6 _K_e_y _f_i_r_s_t_F_r_o_n_t_a_l_K_e_y() const { │ │ │ │ │ -117 if(_n_r_F_r_o_n_t_a_l_s__ > 0) │ │ │ │ │ -118 return asFactor().front(); │ │ │ │ │ -119 else │ │ │ │ │ -120 throw std::invalid_argument("Requested Conditional::firstFrontalKey from a │ │ │ │ │ -conditional with zero frontal keys"); │ │ │ │ │ -121 } │ │ │ │ │ -122 │ │ │ │ │ -_1_2_4 _F_r_o_n_t_a_l_s _f_r_o_n_t_a_l_s() const { return boost::make_iterator_range(_b_e_g_i_n_F_r_o_n_t_a_l_s │ │ │ │ │ -(), _e_n_d_F_r_o_n_t_a_l_s()); } │ │ │ │ │ -125 │ │ │ │ │ -_1_2_7 _P_a_r_e_n_t_s _p_a_r_e_n_t_s() const { return boost::make_iterator_range(_b_e_g_i_n_P_a_r_e_n_t_s(), │ │ │ │ │ -_e_n_d_P_a_r_e_n_t_s()); } │ │ │ │ │ -128 │ │ │ │ │ -133 virtual double _l_o_g_P_r_o_b_a_b_i_l_i_t_y(const _H_y_b_r_i_d_V_a_l_u_e_s& c) const; │ │ │ │ │ -134 │ │ │ │ │ -139 virtual double _e_v_a_l_u_a_t_e(const _H_y_b_r_i_d_V_a_l_u_e_s& c) const; │ │ │ │ │ -140 │ │ │ │ │ -_1_4_2 double _o_p_e_r_a_t_o_r_(_)(const _H_y_b_r_i_d_V_a_l_u_e_s& x) const { │ │ │ │ │ -143 return _e_v_a_l_u_a_t_e(x); │ │ │ │ │ -144 } │ │ │ │ │ -145 │ │ │ │ │ -150 virtual double _l_o_g_N_o_r_m_a_l_i_z_a_t_i_o_n_C_o_n_s_t_a_n_t() const; │ │ │ │ │ -151 │ │ │ │ │ -153 double _n_o_r_m_a_l_i_z_a_t_i_o_n_C_o_n_s_t_a_n_t() const; │ │ │ │ │ -154 │ │ │ │ │ +116 │ │ │ │ │ +117 public: │ │ │ │ │ +119 // public since it is required for boost serialization and static methods. │ │ │ │ │ +120 // virtual since it is public. │ │ │ │ │ +121 // http://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#Rc-dtor- │ │ │ │ │ +virtual │ │ │ │ │ +_1_2_2 virtual _~_F_a_c_t_o_r() = default; │ │ │ │ │ +123 │ │ │ │ │ +126 │ │ │ │ │ +_1_2_8 bool _e_m_p_t_y() const { return keys_.empty(); } │ │ │ │ │ +129 │ │ │ │ │ +_1_3_1 _K_e_y _f_r_o_n_t() const { return keys_.front(); } │ │ │ │ │ +132 │ │ │ │ │ +_1_3_4 _K_e_y _b_a_c_k() const { return keys_.back(); } │ │ │ │ │ +135 │ │ │ │ │ +_1_3_7 _c_o_n_s_t___i_t_e_r_a_t_o_r _f_i_n_d(_K_e_y key) const { return std::find(begin(), end(), key); │ │ │ │ │ +} │ │ │ │ │ +138 │ │ │ │ │ +_1_4_0 const _K_e_y_V_e_c_t_o_r& _k_e_y_s() const { return keys_; } │ │ │ │ │ +141 │ │ │ │ │ +_1_4_3 _c_o_n_s_t___i_t_e_r_a_t_o_r _b_e_g_i_n() const { return keys_.begin(); } │ │ │ │ │ +144 │ │ │ │ │ +_1_4_6 _c_o_n_s_t___i_t_e_r_a_t_o_r _e_n_d() const { return keys_.end(); } │ │ │ │ │ +147 │ │ │ │ │ +152 virtual double error(const _H_y_b_r_i_d_V_a_l_u_e_s& c) const; │ │ │ │ │ +153 │ │ │ │ │ +_1_5_7 size_t _s_i_z_e() const { return keys_.size(); } │ │ │ │ │ 158 │ │ │ │ │ -_1_6_0 typename FACTOR::const_iterator _b_e_g_i_n_F_r_o_n_t_a_l_s() const { return asFactor │ │ │ │ │ -().begin(); } │ │ │ │ │ -161 │ │ │ │ │ -_1_6_3 typename FACTOR::const_iterator _e_n_d_F_r_o_n_t_a_l_s() const { return asFactor │ │ │ │ │ -().begin() + _n_r_F_r_o_n_t_a_l_s__; } │ │ │ │ │ -164 │ │ │ │ │ -_1_6_6 typename FACTOR::const_iterator _b_e_g_i_n_P_a_r_e_n_t_s() const { return _e_n_d_F_r_o_n_t_a_l_s │ │ │ │ │ -(); } │ │ │ │ │ -167 │ │ │ │ │ -_1_6_9 typename FACTOR::const_iterator _e_n_d_P_a_r_e_n_t_s() const { return asFactor().end │ │ │ │ │ -(); } │ │ │ │ │ -170 │ │ │ │ │ -_1_7_2 size_t& _n_r_F_r_o_n_t_a_l_s() { return _n_r_F_r_o_n_t_a_l_s__; } │ │ │ │ │ +160 │ │ │ │ │ +163 │ │ │ │ │ +165 virtual void _p_r_i_n_t( │ │ │ │ │ +166 const std::string& s = "Factor", │ │ │ │ │ +167 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const; │ │ │ │ │ +168 │ │ │ │ │ +170 virtual void printKeys( │ │ │ │ │ +171 const std::string& s = "Factor", │ │ │ │ │ +172 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const; │ │ │ │ │ 173 │ │ │ │ │ -_1_7_5 typename FACTOR::iterator _b_e_g_i_n_F_r_o_n_t_a_l_s() { return asFactor().begin(); } │ │ │ │ │ +175 bool _e_q_u_a_l_s(const This& other, double tol = 1e-9) const; │ │ │ │ │ 176 │ │ │ │ │ -_1_7_8 typename FACTOR::iterator _e_n_d_F_r_o_n_t_a_l_s() { return asFactor().begin() + │ │ │ │ │ -_n_r_F_r_o_n_t_a_l_s__; } │ │ │ │ │ -179 │ │ │ │ │ -_1_8_1 typename FACTOR::iterator _b_e_g_i_n_P_a_r_e_n_t_s() { return asFactor().begin() + │ │ │ │ │ -_n_r_F_r_o_n_t_a_l_s__; } │ │ │ │ │ -182 │ │ │ │ │ -_1_8_4 typename FACTOR::iterator _e_n_d_P_a_r_e_n_t_s() { return asFactor().end(); } │ │ │ │ │ -185 │ │ │ │ │ -199 template │ │ │ │ │ -200 static bool _C_h_e_c_k_I_n_v_a_r_i_a_n_t_s(const DERIVEDCONDITIONAL& conditional, │ │ │ │ │ -201 const VALUES& x); │ │ │ │ │ -202 │ │ │ │ │ -204 │ │ │ │ │ -205 private: │ │ │ │ │ -206 │ │ │ │ │ -209 │ │ │ │ │ -210 // Cast to factor type (non-const) (casts down to derived conditional type, │ │ │ │ │ -then up to factor type) │ │ │ │ │ -211 FACTOR& asFactor() { return static_cast │ │ │ │ │ -(static_cast(*this)); } │ │ │ │ │ -212 │ │ │ │ │ -213 // Cast to derived type (const) (casts down to derived conditional type, │ │ │ │ │ -then up to factor type) │ │ │ │ │ -214 const FACTOR& asFactor() const { return static_cast │ │ │ │ │ -(static_cast(*this)); } │ │ │ │ │ -215 │ │ │ │ │ -_2_1_7 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -218 template │ │ │ │ │ -219 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -220 ar & BOOST_SERIALIZATION_NVP(_n_r_F_r_o_n_t_a_l_s__); │ │ │ │ │ -221 } │ │ │ │ │ -222 │ │ │ │ │ -224 │ │ │ │ │ -225 }; │ │ │ │ │ -226 │ │ │ │ │ -227} // gtsam │ │ │ │ │ +180 │ │ │ │ │ +_1_8_2 _K_e_y_V_e_c_t_o_r& _k_e_y_s() { return keys_; } │ │ │ │ │ +183 │ │ │ │ │ +_1_8_5 _i_t_e_r_a_t_o_r _b_e_g_i_n() { return keys_.begin(); } │ │ │ │ │ +186 │ │ │ │ │ +_1_8_8 _i_t_e_r_a_t_o_r _e_n_d() { return keys_.end(); } │ │ │ │ │ +189 │ │ │ │ │ +191 │ │ │ │ │ +192 private: │ │ │ │ │ +193 │ │ │ │ │ +196 │ │ │ │ │ +_1_9_8 friend class boost::serialization::access; │ │ │ │ │ +199 template │ │ │ │ │ +200 void serialize(Archive & ar, const unsigned int /*version*/) { │ │ │ │ │ +201 ar & BOOST_SERIALIZATION_NVP(keys_); │ │ │ │ │ +202 } │ │ │ │ │ +203 │ │ │ │ │ +205 │ │ │ │ │ +206 }; │ │ │ │ │ +207 │ │ │ │ │ +208} // \namespace gtsam │ │ │ │ │ +_F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ +A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ +_t_y_p_e_s_._h │ │ │ │ │ +Typedefs for easier changing of types. │ │ │ │ │ _K_e_y_._h │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ +std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ +FastVector │ │ │ │ │ +FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ +DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_I_n_d_i_c_e_s │ │ │ │ │ +FastVector< FactorIndex > FactorIndices │ │ │ │ │ +Define collection types: │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:34 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_S_e_t │ │ │ │ │ +FastSet is a thin wrapper around std::set that uses the boost │ │ │ │ │ +fast_pool_allocator instead of the defa... │ │ │ │ │ +DDeeffiinniittiioonn FastSet.h:50 │ │ │ │ │ _g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ Template to create a binary predicate. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ _g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_C_h_e_c_k_I_n_v_a_r_i_a_n_t_s │ │ │ │ │ -static bool CheckInvariants(const DERIVEDCONDITIONAL &conditional, const VALUES │ │ │ │ │ -&x) │ │ │ │ │ -Check invariants of this conditional, given the values x. │ │ │ │ │ -DDeeffiinniittiioonn Conditional-inst.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_n_r_F_r_o_n_t_a_l_s__ │ │ │ │ │ -size_t nrFrontals_ │ │ │ │ │ -The first nrFrontal variables are frontal and the rest are parents. │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_l_o_g_N_o_r_m_a_l_i_z_a_t_i_o_n_C_o_n_s_t_a_n_t │ │ │ │ │ -virtual double logNormalizationConstant() const │ │ │ │ │ -All conditional types need to implement a log normalization constant to make it │ │ │ │ │ -such that error>=0. │ │ │ │ │ -DDeeffiinniittiioonn Conditional-inst.h:62 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_f_i_r_s_t_F_r_o_n_t_a_l_K_e_y │ │ │ │ │ -Key firstFrontalKey() const │ │ │ │ │ -Convenience function to get the first frontal key. │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:116 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_e_n_d_F_r_o_n_t_a_l_s │ │ │ │ │ -FACTOR::const_iterator endFrontals() const │ │ │ │ │ -Iterator pointing past the last frontal key. │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:163 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_e_n_d_P_a_r_e_n_t_s │ │ │ │ │ -FACTOR::iterator endParents() │ │ │ │ │ -Mutable iterator pointing past the last parent key. │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:184 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_e_n_d_F_r_o_n_t_a_l_s │ │ │ │ │ -FACTOR::iterator endFrontals() │ │ │ │ │ -Mutable iterator pointing past the last frontal key. │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:178 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_e_v_a_l_u_a_t_e │ │ │ │ │ -virtual double evaluate(const HybridValues &c) const │ │ │ │ │ -All conditional types need to implement an evaluate function, that yields a │ │ │ │ │ -true probability. │ │ │ │ │ -DDeeffiinniittiioonn Conditional-inst.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_p_a_r_e_n_t_s │ │ │ │ │ -Parents parents() const │ │ │ │ │ -return a view of the parent keys │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:127 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ -double operator()(const HybridValues &x) const │ │ │ │ │ -Evaluate probability density, sugar. │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:142 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_P_a_r_e_n_t_s │ │ │ │ │ -boost::iterator_range< typename FACTOR::const_iterator > Parents │ │ │ │ │ -View of the separator keys (call parents()) │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:78 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_l_o_g_P_r_o_b_a_b_i_l_i_t_y │ │ │ │ │ -virtual double logProbability(const HybridValues &c) const │ │ │ │ │ -All conditional types need to implement a logProbability function, for which │ │ │ │ │ -exp(logProbability(x)) =... │ │ │ │ │ -DDeeffiinniittiioonn Conditional-inst.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_n_r_F_r_o_n_t_a_l_s │ │ │ │ │ -size_t nrFrontals() const │ │ │ │ │ -return the number of frontals │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:110 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -Conditional() │ │ │ │ │ -Empty Constructor to make serialization possible. │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -Conditional(size_t nrFrontals) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:88 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_b_e_g_i_n_P_a_r_e_n_t_s │ │ │ │ │ -FACTOR::iterator beginParents() │ │ │ │ │ -Mutable iterator pointing to the first parent key. │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:181 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_n_r_F_r_o_n_t_a_l_s │ │ │ │ │ -size_t & nrFrontals() │ │ │ │ │ -Mutable version of nrFrontals. │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:172 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_b_e_g_i_n_F_r_o_n_t_a_l_s │ │ │ │ │ -FACTOR::iterator beginFrontals() │ │ │ │ │ -Mutable iterator pointing to first frontal key. │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:175 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_F_r_o_n_t_a_l_s │ │ │ │ │ -boost::iterator_range< typename FACTOR::const_iterator > Frontals │ │ │ │ │ -View of the frontal keys (call frontals()) │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_f_r_o_n_t_a_l_s │ │ │ │ │ -Frontals frontals() const │ │ │ │ │ -return a view of the frontal keys │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:124 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:217 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_b_e_g_i_n_F_r_o_n_t_a_l_s │ │ │ │ │ -FACTOR::const_iterator beginFrontals() const │ │ │ │ │ -Iterator pointing to first frontal key. │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:160 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_n_o_r_m_a_l_i_z_a_t_i_o_n_C_o_n_s_t_a_n_t │ │ │ │ │ -double normalizationConstant() const │ │ │ │ │ -Non-virtual, exponentiate logNormalizationConstant. │ │ │ │ │ -DDeeffiinniittiioonn Conditional-inst.h:70 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_n_r_P_a_r_e_n_t_s │ │ │ │ │ -size_t nrParents() const │ │ │ │ │ -return the number of parents │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:113 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="Conditional", const KeyFormatter │ │ │ │ │ -&formatter=DefaultKeyFormatter) const │ │ │ │ │ -print with optional formatter │ │ │ │ │ -DDeeffiinniittiioonn Conditional-inst.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_e_n_d_P_a_r_e_n_t_s │ │ │ │ │ -FACTOR::const_iterator endParents() const │ │ │ │ │ -Iterator pointing past the last parent key. │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:169 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_b_e_g_i_n_P_a_r_e_n_t_s │ │ │ │ │ -FACTOR::const_iterator beginParents() const │ │ │ │ │ -Iterator pointing to the first parent key. │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:166 │ │ │ │ │ -_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -the error. │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:68 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s │ │ │ │ │ +const KeyVector & keys() const │ │ │ │ │ +Access the factor's involved variable keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:140 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s__ │ │ │ │ │ +KeyVector keys_ │ │ │ │ │ +The keys involved in this factor. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_f_i_n_d │ │ │ │ │ +const_iterator find(Key key) const │ │ │ │ │ +find │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:137 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_b_e_g_i_n │ │ │ │ │ +const_iterator begin() const │ │ │ │ │ +Iterator at beginning of involved variable keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:143 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_F_a_c_t_o_r │ │ │ │ │ +Factor() │ │ │ │ │ +Default constructor for I/O. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:91 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_n_d │ │ │ │ │ +iterator end() │ │ │ │ │ +Iterator at end of involved variable keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:188 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_m_p_t_y │ │ │ │ │ +bool empty() const │ │ │ │ │ +Whether the factor is empty (involves zero variables). │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:128 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s │ │ │ │ │ +KeyVector & keys() │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:182 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_F_r_o_m_I_t_e_r_a_t_o_r_s │ │ │ │ │ +static Factor FromIterators(ITERATOR first, ITERATOR last) │ │ │ │ │ +Construct factor from iterator keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:112 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_F_r_o_m_K_e_y_s │ │ │ │ │ +static Factor FromKeys(const CONTAINER &keys) │ │ │ │ │ +Construct factor from container of keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:106 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_b_e_g_i_n │ │ │ │ │ +iterator begin() │ │ │ │ │ +Iterator at beginning of involved variable keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:185 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_~_F_a_c_t_o_r │ │ │ │ │ +virtual ~Factor()=default │ │ │ │ │ +Default destructor. │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_F_a_c_t_o_r │ │ │ │ │ +Factor(ITERATOR first, ITERATOR last) │ │ │ │ │ +Construct factor from iterator keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:101 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_c_o_n_s_t___i_t_e_r_a_t_o_r │ │ │ │ │ +KeyVector::const_iterator const_iterator │ │ │ │ │ +Const iterator over keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:80 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_n_d │ │ │ │ │ +const_iterator end() const │ │ │ │ │ +Iterator at end of involved variable keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:146 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_F_a_c_t_o_r │ │ │ │ │ +Factor(const CONTAINER &keys) │ │ │ │ │ +Construct factor from container of keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:96 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_i_t_e_r_a_t_o_r │ │ │ │ │ +KeyVector::iterator iterator │ │ │ │ │ +Iterator over keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_b_a_c_k │ │ │ │ │ +Key back() const │ │ │ │ │ +Last key. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:134 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_f_r_o_n_t │ │ │ │ │ +Key front() const │ │ │ │ │ +First key. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:131 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_s_i_z_e │ │ │ │ │ +size_t size() const │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:157 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ + * _F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00590.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/VariableSlots.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/EliminationTree-inst.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,48 +94,36 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
VariableSlots.h File Reference
│ │ │ │ +
EliminationTree-inst.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

VariableSlots describes the structure of a combined factor in terms of where each block comes from in the source factors. │ │ │ │ -More...

│ │ │ │ - │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::VariableSlots
 A combined factor is assembled as one block of rows for each component factor. More...
 
struct  gtsam::traits< VariableSlots >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

VariableSlots describes the structure of a combined factor in terms of where each block comes from in the source factors.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Oct 4, 2010
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │ +
Date
Oct 13, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,33 +1,22 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -VariableSlots.h File Reference │ │ │ │ │ -VariableSlots describes the structure of a combined factor in terms of where │ │ │ │ │ -each block comes from in the source factors. _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +EliminationTree-inst.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_S_l_o_t_s │ │ │ │ │ -  A combined factor is assembled as one block of rows for each component │ │ │ │ │ - factor. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _V_a_r_i_a_b_l_e_S_l_o_t_s_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -VariableSlots describes the structure of a combined factor in terms of where │ │ │ │ │ -each block comes from in the source factors. │ │ │ │ │ Author │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ Date │ │ │ │ │ - Oct 4, 2010 │ │ │ │ │ + Oct 13, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _V_a_r_i_a_b_l_e_S_l_o_t_s_._h │ │ │ │ │ + * _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_-_i_n_s_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00590_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/VariableSlots.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/EliminationTree-inst.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,126 +98,363 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
VariableSlots.h
│ │ │ │ +
EliminationTree-inst.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ -
4 * Atlanta, Georgia 30332-0415
│ │ │ │ -
5 * All Rights Reserved
│ │ │ │ -
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
3* GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
4* Atlanta, Georgia 30332-0415
│ │ │ │ +
5* All Rights Reserved
│ │ │ │ +
6* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │ -
8 * See LICENSE for the license information
│ │ │ │ +
8* See LICENSE for the license information
│ │ │ │
9
│ │ │ │ -
10 * -------------------------------------------------------------------------- */
│ │ │ │ +
10* -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/global_includes.h>
│ │ │ │ -
22#include <gtsam/base/FastMap.h>
│ │ │ │ -
23#include <gtsam/base/FastVector.h>
│ │ │ │ -
24#include <gtsam/base/timing.h>
│ │ │ │ -
25#include <gtsam/base/Testable.h>
│ │ │ │ -
26
│ │ │ │ -
27#include <boost/tuple/tuple.hpp>
│ │ │ │ -
28
│ │ │ │ -
29#include <iostream>
│ │ │ │ -
30#include <string>
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <boost/make_shared.hpp>
│ │ │ │ +
21#include <stack>
│ │ │ │ +
22
│ │ │ │ +
23#include <gtsam/base/timing.h>
│ │ │ │ +
24#include <gtsam/base/treeTraversal-inst.h>
│ │ │ │ +
25#include <gtsam/inference/EliminationTree.h>
│ │ │ │ +
26#include <gtsam/inference/VariableIndex.h>
│ │ │ │ +
27#include <gtsam/inference/Ordering.h>
│ │ │ │ +
28#include <gtsam/inference/inference-inst.h>
│ │ │ │ +
29
│ │ │ │ +
30namespace gtsam {
│ │ │ │
31
│ │ │ │ -
32namespace gtsam {
│ │ │ │ -
33
│ │ │ │ -
│ │ │ │ -
52class VariableSlots : public FastMap<Key, FastVector<size_t> > {
│ │ │ │ -
53
│ │ │ │ -
54public:
│ │ │ │ +
32 /* ************************************************************************* */
│ │ │ │ +
33 template<class BAYESNET, class GRAPH>
│ │ │ │ +
34 typename EliminationTree<BAYESNET,GRAPH>::sharedFactor
│ │ │ │ +
35 EliminationTree<BAYESNET,GRAPH>::Node::eliminate(
│ │ │ │ +
36 const boost::shared_ptr<BayesNetType>& output,
│ │ │ │ +
37 const Eliminate& function, const FastVector<sharedFactor>& childrenResults) const
│ │ │ │ +
38 {
│ │ │ │ +
39 // This function eliminates one node (Node::eliminate) - see below eliminate for the whole tree.
│ │ │ │ +
40
│ │ │ │ +
41 assert(childrenResults.size() == children.size());
│ │ │ │ +
42
│ │ │ │ +
43 // Gather factors
│ │ │ │ +
44 FactorGraphType gatheredFactors;
│ │ │ │ +
45 gatheredFactors.reserve(factors.size() + children.size());
│ │ │ │ +
46 gatheredFactors.push_back(factors.begin(), factors.end());
│ │ │ │ +
47 gatheredFactors.push_back(childrenResults.begin(), childrenResults.end());
│ │ │ │ +
48
│ │ │ │ +
49 // Do dense elimination step
│ │ │ │ +
50 KeyVector keyAsVector(1); keyAsVector[0] = key;
│ │ │ │ +
51 auto eliminationResult = function(gatheredFactors, Ordering(keyAsVector));
│ │ │ │ +
52
│ │ │ │ +
53 // Add conditional to BayesNet
│ │ │ │ +
54 output->push_back(eliminationResult.first);
│ │ │ │
55
│ │ │ │ -
56 typedef FastMap<Key, FastVector<size_t> > Base;
│ │ │ │ -
57 GTSAM_EXPORT static const size_t Empty;
│ │ │ │ -
58
│ │ │ │ -
61
│ │ │ │ -
67 template<class FG>
│ │ │ │ -
68 VariableSlots(const FG& factorGraph);
│ │ │ │ -
69
│ │ │ │ -
71
│ │ │ │ +
56 // Return result
│ │ │ │ +
57 return eliminationResult.second;
│ │ │ │ +
58 }
│ │ │ │ +
59
│ │ │ │ +
60 /* ************************************************************************* */
│ │ │ │ +
61 template<class BAYESNET, class GRAPH>
│ │ │ │ +
62 void EliminationTree<BAYESNET,GRAPH>::Node::print(
│ │ │ │ +
63 const std::string& str, const KeyFormatter& keyFormatter) const
│ │ │ │ +
64 {
│ │ │ │ +
65 std::cout << str << "(" << keyFormatter(key) << ")\n";
│ │ │ │ +
66 for(const sharedFactor& factor: factors) {
│ │ │ │ +
67 if(factor)
│ │ │ │ +
68 factor->print(str);
│ │ │ │ +
69 else
│ │ │ │ +
70 std::cout << str << "null factor\n";
│ │ │ │ +
71 }
│ │ │ │ +
72 }
│ │ │ │ +
73
│ │ │ │
74
│ │ │ │ -
76 GTSAM_EXPORT void print(const std::string& str = "VariableSlots: ") const;
│ │ │ │ -
77
│ │ │ │ -
79 GTSAM_EXPORT bool equals(const VariableSlots& rhs, double tol = 0.0) const;
│ │ │ │ -
80
│ │ │ │ -
82};
│ │ │ │ -
│ │ │ │ -
83
│ │ │ │ -
85template<> struct traits<VariableSlots> : public Testable<VariableSlots> {};
│ │ │ │ +
75 /* ************************************************************************* */
│ │ │ │ +
76 template<class BAYESNET, class GRAPH>
│ │ │ │ +
│ │ │ │ +
77 EliminationTree<BAYESNET,GRAPH>::EliminationTree(const FactorGraphType& graph,
│ │ │ │ +
78 const VariableIndex& structure, const Ordering& order)
│ │ │ │ +
79 {
│ │ │ │ +
80 gttic(EliminationTree_Contructor);
│ │ │ │ +
81
│ │ │ │ +
82 // Number of factors and variables - NOTE in the case of partial elimination, n here may
│ │ │ │ +
83 // be fewer variables than are actually present in the graph.
│ │ │ │ +
84 const size_t m = graph.size();
│ │ │ │ +
85 const size_t n = order.size();
│ │ │ │
86
│ │ │ │ -
87/* ************************************************************************* */
│ │ │ │ -
88template<class FG>
│ │ │ │ -
│ │ │ │ -
89VariableSlots::VariableSlots(const FG& factorGraph)
│ │ │ │ -
90{
│ │ │ │ -
91 gttic(VariableSlots_constructor);
│ │ │ │ -
92 static const bool debug = false;
│ │ │ │ -
93
│ │ │ │ -
94 // Compute a mapping (called variableSlots) *from* each involved
│ │ │ │ -
95 // variable that will be in the new joint factor *to* the slot in each
│ │ │ │ -
96 // removed factor in which that variable appears. For each variable,
│ │ │ │ -
97 // this is stored as a vector of slot numbers, stored in order of the
│ │ │ │ -
98 // removed factors. The slot number is the max integer value if the
│ │ │ │ -
99 // factor does not involve that variable.
│ │ │ │ -
100 size_t jointFactorPos = 0;
│ │ │ │ -
101 for(const typename FG::sharedFactor& factor: factorGraph) {
│ │ │ │ -
102 if (!factor) {
│ │ │ │ -
103 continue;
│ │ │ │ -
104 }
│ │ │ │ -
105 size_t factorVarSlot = 0;
│ │ │ │ -
106 for(const Key involvedVariable: *factor) {
│ │ │ │ -
107 // Set the slot in this factor for this variable. If the
│ │ │ │ -
108 // variable was not already discovered, create an array for it
│ │ │ │ -
109 // that we'll fill with the slot indices for each factor that
│ │ │ │ -
110 // we're combining. Initially we put the max integer value in
│ │ │ │ -
111 // the array entry for each factor that will indicate the factor
│ │ │ │ -
112 // does not involve the variable.
│ │ │ │ -
113 iterator thisVarSlots; bool inserted;
│ │ │ │ -
114 boost::tie(thisVarSlots, inserted) = this->insert(std::make_pair(involvedVariable, FastVector<size_t>()));
│ │ │ │ -
115 if(inserted)
│ │ │ │ -
116 thisVarSlots->second.resize(factorGraph.nrFactors(), Empty);
│ │ │ │ -
117 thisVarSlots->second[jointFactorPos] = factorVarSlot;
│ │ │ │ -
118 if(debug) std::cout << " var " << involvedVariable << " rowblock " << jointFactorPos << " comes from factor's slot " << factorVarSlot << std::endl;
│ │ │ │ -
119 ++ factorVarSlot;
│ │ │ │ -
120 }
│ │ │ │ -
121 ++ jointFactorPos;
│ │ │ │ -
122 }
│ │ │ │ -
123}
│ │ │ │ -
│ │ │ │ -
124
│ │ │ │ -
125}
│ │ │ │ -
FastMap.h
A thin wrapper around std::map that uses boost's fast_pool_allocator.
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ -
timing.h
Timing utilities.
│ │ │ │ -
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ -
global_includes.h
Included from all GTSAM files.
│ │ │ │ +
87 static const size_t none = std::numeric_limits<size_t>::max();
│ │ │ │ +
88
│ │ │ │ +
89 // Allocate result parent vector and vector of last factor columns
│ │ │ │ +
90 FastVector<sharedNode> nodes(n);
│ │ │ │ +
91 FastVector<size_t> parents(n, none);
│ │ │ │ +
92 FastVector<size_t> prevCol(m, none);
│ │ │ │ +
93 FastVector<bool> factorUsed(m, false);
│ │ │ │ +
94
│ │ │ │ +
95 try {
│ │ │ │ +
96 // for column j \in 1 to n do
│ │ │ │ +
97 for (size_t j = 0; j < n; j++)
│ │ │ │ +
98 {
│ │ │ │ +
99 // Retrieve the factors involving this variable and create the current node
│ │ │ │ +
│ │ │ │ +
100 const FactorIndices& factors = structure[order[j]];
│ │ │ │ +
101 const sharedNode node = boost::make_shared<Node>();
│ │ │ │ +
102 node->key = order[j];
│ │ │ │ +
103
│ │ │ │ +
104 // for row i \in Struct[A*j] do
│ │ │ │ +
105 node->children.reserve(factors.size());
│ │ │ │ +
106 node->factors.reserve(factors.size());
│ │ │ │ +
107 for(const size_t i: factors) {
│ │ │ │ +
│ │ │ │ +
108 // If we already hit a variable in this factor, make the subtree containing the previous
│ │ │ │ +
109 // variable in this factor a child of the current node. This means that the variables
│ │ │ │ +
110 // eliminated earlier in the factor depend on the later variables in the factor. If we
│ │ │ │ +
111 // haven't yet hit a variable in this factor, we add the factor to the current node.
│ │ │ │ +
112 // TODO: Store root shortcuts instead of parents.
│ │ │ │ +
113 if (prevCol[i] != none) {
│ │ │ │ +
114 size_t k = prevCol[i];
│ │ │ │ +
115 // Find root r of the current tree that contains k. Use raw pointers in computing the
│ │ │ │ +
│ │ │ │ +
116 // parents to avoid changing the reference counts while traversing up the tree.
│ │ │ │ +
117 size_t r = k;
│ │ │ │ +
118 while (parents[r] != none)
│ │ │ │ +
119 r = parents[r];
│ │ │ │ +
120 // If the root of the subtree involving this node is actually the current node,
│ │ │ │ +
121 // TODO: what does this mean? forest?
│ │ │ │ +
122 if (r != j) {
│ │ │ │ +
123 // Now that we found the root, hook up parent and child pointers in the nodes.
│ │ │ │ +
124 parents[r] = j;
│ │ │ │ +
125 node->children.push_back(nodes[r]);
│ │ │ │ +
126 }
│ │ │ │ +
127 } else {
│ │ │ │ +
128 // Add the factor to the current node since we are at the first variable in this factor.
│ │ │ │ +
129 node->factors.push_back(graph[i]);
│ │ │ │ +
│ │ │ │ +
130 factorUsed[i] = true;
│ │ │ │ +
131 }
│ │ │ │ +
132 prevCol[i] = j;
│ │ │ │ +
133 }
│ │ │ │ +
134 nodes[j] = node;
│ │ │ │ +
135 }
│ │ │ │ +
136 } catch(std::invalid_argument& e) {
│ │ │ │ +
│ │ │ │ +
137 // If this is thrown from structure[order[j]] above, it means that it was requested to
│ │ │ │ +
138 // eliminate a variable not present in the graph, so throw a more informative error message.
│ │ │ │ +
139 (void)e; // Prevent unused variable warning
│ │ │ │ +
140 throw std::invalid_argument("EliminationTree: given ordering contains variables that are not involved in the factor graph");
│ │ │ │ +
141 } catch(...) {
│ │ │ │ +
│ │ │ │ +
142 throw;
│ │ │ │ +
143 }
│ │ │ │ +
144
│ │ │ │ +
145 // Find roots
│ │ │ │ +
146 assert(parents.empty() || parents.back() == none); // We expect the last-eliminated node to be a root no matter what
│ │ │ │ +
147 for(size_t j = 0; j < n; ++j)
│ │ │ │ +
148 if(parents[j] == none)
│ │ │ │ +
149 roots_.push_back(nodes[j]);
│ │ │ │ +
150
│ │ │ │ +
151 // Gather remaining factors (exclude null factors)
│ │ │ │ +
152 for(size_t i = 0; i < m; ++i)
│ │ │ │ +
153 if(!factorUsed[i] && graph[i])
│ │ │ │ +
154 remainingFactors_.push_back(graph[i]);
│ │ │ │ +
155 }
│ │ │ │ +
156
│ │ │ │ +
│ │ │ │ +
157 /* ************************************************************************* */
│ │ │ │ +
158 template<class BAYESNET, class GRAPH>
│ │ │ │ +
│ │ │ │ +
159 EliminationTree<BAYESNET,GRAPH>::EliminationTree(
│ │ │ │ +
160 const FactorGraphType& factorGraph, const Ordering& order)
│ │ │ │ +
161 {
│ │ │ │ +
162 gttic(ET_Create2);
│ │ │ │ +
163 // Build variable index first
│ │ │ │ +
164 const VariableIndex variableIndex(factorGraph);
│ │ │ │ +
165 This temp(factorGraph, variableIndex, order);
│ │ │ │ +
166 this->swap(temp); // Swap in the tree, and temp will be deleted
│ │ │ │ +
167 }
│ │ │ │ +
│ │ │ │ +
168
│ │ │ │ +
169 /* ************************************************************************* */
│ │ │ │ +
170 template<class BAYESNET, class GRAPH>
│ │ │ │ +
171 EliminationTree<BAYESNET,GRAPH>&
│ │ │ │ +
│ │ │ │ +
172 EliminationTree<BAYESNET,GRAPH>::operator=(const EliminationTree<BAYESNET,GRAPH>& other)
│ │ │ │ +
173 {
│ │ │ │ +
174 // Start by duplicating the tree.
│ │ │ │ +
175 roots_ = treeTraversal::CloneForest(other);
│ │ │ │ +
176
│ │ │ │ +
177 // Assign the remaining factors - these are pointers to factors in the original factor graph and
│ │ │ │ +
178 // we do not clone them.
│ │ │ │ +
179 remainingFactors_ = other.remainingFactors_;
│ │ │ │ +
180
│ │ │ │ +
181 return *this;
│ │ │ │ +
182 }
│ │ │ │ +
│ │ │ │ +
183
│ │ │ │ +
184 /* ************************************************************************* */
│ │ │ │ +
185 template<class BAYESNET, class GRAPH>
│ │ │ │ +
186 std::pair<boost::shared_ptr<BAYESNET>, boost::shared_ptr<GRAPH> >
│ │ │ │ +
│ │ │ │ +
187 EliminationTree<BAYESNET,GRAPH>::eliminate(Eliminate function) const
│ │ │ │ +
188 {
│ │ │ │ +
189 gttic(EliminationTree_eliminate);
│ │ │ │ +
190 // Allocate result
│ │ │ │ +
191 auto result = boost::make_shared<BayesNetType>();
│ │ │ │ +
192
│ │ │ │ +
193 // Run tree elimination algorithm
│ │ │ │ +
194 FastVector<sharedFactor> remainingFactors = inference::EliminateTree(result, *this, function);
│ │ │ │ +
195
│ │ │ │ +
196 // Add remaining factors that were not involved with eliminated variables
│ │ │ │ +
197 auto allRemainingFactors = boost::make_shared<FactorGraphType>();
│ │ │ │ +
198 allRemainingFactors->push_back(remainingFactors_.begin(), remainingFactors_.end());
│ │ │ │ +
199 allRemainingFactors->push_back(remainingFactors.begin(), remainingFactors.end());
│ │ │ │ +
200
│ │ │ │ +
201 // Return result
│ │ │ │ +
202 return std::make_pair(result, allRemainingFactors);
│ │ │ │ +
203 }
│ │ │ │ +
│ │ │ │ +
204
│ │ │ │ +
205 /* ************************************************************************* */
│ │ │ │ +
206 template<class BAYESNET, class GRAPH>
│ │ │ │ +
│ │ │ │ +
207 void EliminationTree<BAYESNET,GRAPH>::print(const std::string& name, const KeyFormatter& formatter) const
│ │ │ │ +
208 {
│ │ │ │ +
209 treeTraversal::PrintForest(*this, name, formatter);
│ │ │ │ +
210 }
│ │ │ │ +
│ │ │ │ +
211
│ │ │ │ +
212 /* ************************************************************************* */
│ │ │ │ +
213 template<class BAYESNET, class GRAPH>
│ │ │ │ +
│ │ │ │ +
214 bool EliminationTree<BAYESNET,GRAPH>::equals(const This& expected, double tol) const
│ │ │ │ +
215 {
│ │ │ │ +
216 // Depth-first-traversal stacks
│ │ │ │ +
217 std::stack<sharedNode, FastVector<sharedNode> > stack1, stack2;
│ │ │ │ +
218
│ │ │ │ +
219 // Add roots in sorted order
│ │ │ │ +
220 {
│ │ │ │ +
221 FastMap<Key,sharedNode> keys;
│ │ │ │ +
222 for(const sharedNode& root: this->roots_) { keys.insert(std::make_pair(root->key, root)); }
│ │ │ │ +
223 typedef typename FastMap<Key,sharedNode>::value_type Key_Node;
│ │ │ │ +
224 for(const Key_Node& key_node: keys) { stack1.push(key_node.second); }
│ │ │ │ +
225 }
│ │ │ │ +
226 {
│ │ │ │ +
227 FastMap<Key,sharedNode> keys;
│ │ │ │ +
228 for(const sharedNode& root: expected.roots_) { keys.insert(std::make_pair(root->key, root)); }
│ │ │ │ +
229 typedef typename FastMap<Key,sharedNode>::value_type Key_Node;
│ │ │ │ +
230 for(const Key_Node& key_node: keys) { stack2.push(key_node.second); }
│ │ │ │ +
231 }
│ │ │ │ +
232
│ │ │ │ +
233 // Traverse, adding children in sorted order
│ │ │ │ +
234 while(!stack1.empty() && !stack2.empty()) {
│ │ │ │ +
235 // Pop nodes
│ │ │ │ +
236 sharedNode node1 = stack1.top();
│ │ │ │ +
237 stack1.pop();
│ │ │ │ +
238 sharedNode node2 = stack2.top();
│ │ │ │ +
239 stack2.pop();
│ │ │ │ +
240
│ │ │ │ +
241 // Compare nodes
│ │ │ │ +
242 if(node1->key != node2->key)
│ │ │ │ +
243 return false;
│ │ │ │ +
244 if(node1->factors.size() != node2->factors.size()) {
│ │ │ │ +
245 return false;
│ │ │ │ +
246 } else {
│ │ │ │ +
247 for(typename Node::Factors::const_iterator it1 = node1->factors.begin(), it2 = node2->factors.begin();
│ │ │ │ +
248 it1 != node1->factors.end(); ++it1, ++it2) // Only check it1 == end because we already returned false for different counts
│ │ │ │ +
249 {
│ │ │ │ +
250 if(*it1 && *it2) {
│ │ │ │ +
251 if(!(*it1)->equals(**it2, tol))
│ │ │ │ +
252 return false;
│ │ │ │ +
253 } else if((*it1 && !*it2) || (*it2 && !*it1)) {
│ │ │ │ +
254 return false;
│ │ │ │ +
255 }
│ │ │ │ +
256 }
│ │ │ │ +
257 }
│ │ │ │ +
258
│ │ │ │ +
259 // Add children in sorted order
│ │ │ │ +
260 {
│ │ │ │ +
261 FastMap<Key,sharedNode> keys;
│ │ │ │ +
262 for(const sharedNode& node: node1->children) { keys.insert(std::make_pair(node->key, node)); }
│ │ │ │ +
263 typedef typename FastMap<Key,sharedNode>::value_type Key_Node;
│ │ │ │ +
264 for(const Key_Node& key_node: keys) { stack1.push(key_node.second); }
│ │ │ │ +
265 }
│ │ │ │ +
266 {
│ │ │ │ +
267 FastMap<Key,sharedNode> keys;
│ │ │ │ +
268 for(const sharedNode& node: node2->children) { keys.insert(std::make_pair(node->key, node)); }
│ │ │ │ +
269 typedef typename FastMap<Key,sharedNode>::value_type Key_Node;
│ │ │ │ +
270 for(const Key_Node& key_node: keys) { stack2.push(key_node.second); }
│ │ │ │ +
271 }
│ │ │ │ +
272 }
│ │ │ │ +
273
│ │ │ │ +
274 // If either stack is not empty, the number of nodes differed
│ │ │ │ +
275 if(!stack1.empty() || !stack2.empty())
│ │ │ │ +
276 return false;
│ │ │ │ +
277
│ │ │ │ +
278 return true;
│ │ │ │ +
279 }
│ │ │ │ +
│ │ │ │ +
280
│ │ │ │ +
281 /* ************************************************************************* */
│ │ │ │ +
282 template<class BAYESNET, class GRAPH>
│ │ │ │ +
│ │ │ │ +
283 void EliminationTree<BAYESNET,GRAPH>::swap(This& other) {
│ │ │ │ +
284 roots_.swap(other.roots_);
│ │ │ │ +
285 remainingFactors_.swap(other.remainingFactors_);
│ │ │ │ +
286 }
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
287
│ │ │ │ +
288
│ │ │ │ +
289}
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
timing.h
Timing utilities.
│ │ │ │ +
Ordering.h
Variable ordering for the elimination algorithm.
│ │ │ │ + │ │ │ │ +
inference-inst.h
Contains generic inference algorithms that convert between templated graphical models,...
│ │ │ │ + │ │ │ │
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition Factor.h:34
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::treeTraversal::CloneForest
FastVector< boost::shared_ptr< typename FOREST::Node > > CloneForest(const FOREST &forest)
Clone a tree, copy-constructing new nodes (calling boost::make_shared) and setting up child pointers ...
Definition treeTraversal-inst.h:189
│ │ │ │ +
gtsam::treeTraversal::PrintForest
void PrintForest(const FOREST &forest, std::string str, const KeyFormatter &keyFormatter)
Print a tree, prefixing each line with str, and formatting keys using keyFormatter.
Definition treeTraversal-inst.h:219
│ │ │ │
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::VariableSlots
A combined factor is assembled as one block of rows for each component factor.
Definition VariableSlots.h:52
│ │ │ │ -
gtsam::VariableSlots::print
GTSAM_EXPORT void print(const std::string &str="VariableSlots: ") const
print
Definition VariableSlots.cpp:29
│ │ │ │ -
gtsam::VariableSlots::VariableSlots
VariableSlots(const FG &factorGraph)
Constructor from a set of factors to be combined.
Definition VariableSlots.h:89
│ │ │ │ +
gtsam::EliminationTree
An elimination tree is a data structure used intermediately during elimination.
Definition EliminationTree.h:52
│ │ │ │ +
gtsam::EliminationTree::equals
bool equals(const This &other, double tol=1e-9) const
Test whether the tree is equal to another.
Definition EliminationTree-inst.h:214
│ │ │ │ +
gtsam::EliminationTree::print
void print(const std::string &name="EliminationTree: ", const KeyFormatter &formatter=DefaultKeyFormatter) const
Print the tree to cout.
Definition EliminationTree-inst.h:207
│ │ │ │ +
gtsam::EliminationTree::eliminate
std::pair< boost::shared_ptr< BayesNetType >, boost::shared_ptr< FactorGraphType > > eliminate(Eliminate function) const
Eliminate the factors to a Bayes net and remaining factor graph.
Definition EliminationTree-inst.h:187
│ │ │ │ +
gtsam::EliminationTree::operator=
This & operator=(const This &other)
Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are copie...
Definition EliminationTree-inst.h:172
│ │ │ │ +
gtsam::EliminationTree::roots_
FastVector< sharedNode > roots_
concept check
Definition EliminationTree.h:86
│ │ │ │ +
gtsam::EliminationTree::sharedFactor
boost::shared_ptr< FactorType > sharedFactor
Shared pointer to a factor.
Definition EliminationTree.h:60
│ │ │ │ +
gtsam::EliminationTree::FactorGraphType
GRAPH FactorGraphType
The factor graph type.
Definition EliminationTree.h:58
│ │ │ │ +
gtsam::EliminationTree::swap
void swap(This &other)
Swap the data of this tree with another one, this operation is very fast.
Definition EliminationTree-inst.h:283
│ │ │ │ +
gtsam::EliminationTree::EliminationTree
EliminationTree()
Protected default constructor.
Definition EliminationTree.h:161
│ │ │ │ +
gtsam::EliminationTree< DiscreteBayesNet, DiscreteFactorGraph >::sharedNode
boost::shared_ptr< Node > sharedNode
Shared pointer to Node.
Definition EliminationTree.h:80
│ │ │ │ +
gtsam::EliminationTree::remainingFactors
const FastVector< sharedFactor > & remainingFactors() const
Return the remaining factors that are not pulled into elimination.
Definition EliminationTree.h:154
│ │ │ │ +
gtsam::EliminationTree::Node::key
Key key
key associated with root
Definition EliminationTree.h:70
│ │ │ │ +
gtsam::EliminationTree::Node::children
Children children
sub-trees
Definition EliminationTree.h:72
│ │ │ │ +
gtsam::EliminationTree::Node::factors
Factors factors
factors associated with root
Definition EliminationTree.h:71
│ │ │ │ +
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition VariableIndex.h:43
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,149 +1,449 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -VariableSlots.h │ │ │ │ │ +EliminationTree-inst.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ -4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -5 * All Rights Reserved │ │ │ │ │ -6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +3* GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4* Atlanta, Georgia 30332-0415 │ │ │ │ │ +5* All Rights Reserved │ │ │ │ │ +6* Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ -8 * See LICENSE for the license information │ │ │ │ │ +8* See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ -10 * ------------------------------------------------------------------------- │ │ │ │ │ +10* ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_M_a_p_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_b_a_s_e_/_t_i_m_i_n_g_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ -26 │ │ │ │ │ -27#include │ │ │ │ │ -28 │ │ │ │ │ -29#include │ │ │ │ │ -30#include │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include │ │ │ │ │ +21#include │ │ │ │ │ +22 │ │ │ │ │ +23#include <_g_t_s_a_m_/_b_a_s_e_/_t_i_m_i_n_g_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_b_a_s_e_/_t_r_e_e_T_r_a_v_e_r_s_a_l_-_i_n_s_t_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_V_a_r_i_a_b_l_e_I_n_d_e_x_._h> │ │ │ │ │ +27#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_O_r_d_e_r_i_n_g_._h> │ │ │ │ │ +28#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_i_n_f_e_r_e_n_c_e_-_i_n_s_t_._h> │ │ │ │ │ +29 │ │ │ │ │ +30namespace _g_t_s_a_m { │ │ │ │ │ 31 │ │ │ │ │ -32namespace _g_t_s_a_m { │ │ │ │ │ -33 │ │ │ │ │ -_5_2class _V_a_r_i_a_b_l_e_S_l_o_t_s : public _F_a_s_t_M_a_p > { │ │ │ │ │ -53 │ │ │ │ │ -54public: │ │ │ │ │ +32 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +33 template │ │ │ │ │ +34 typename _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_B_A_Y_E_S_N_E_T_,_G_R_A_P_H_>_:_:_s_h_a_r_e_d_F_a_c_t_o_r │ │ │ │ │ +35 EliminationTree::Node::eliminate( │ │ │ │ │ +36 const boost::shared_ptr& output, │ │ │ │ │ +37 const Eliminate& function, const FastVector& childrenResults) │ │ │ │ │ +const │ │ │ │ │ +38 { │ │ │ │ │ +39 // This function eliminates one node (Node::eliminate) - see below eliminate │ │ │ │ │ +for the whole tree. │ │ │ │ │ +40 │ │ │ │ │ +41 assert(childrenResults.size() == _c_h_i_l_d_r_e_n.size()); │ │ │ │ │ +42 │ │ │ │ │ +43 // Gather factors │ │ │ │ │ +44 _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e gatheredFactors; │ │ │ │ │ +45 gatheredFactors.reserve(_f_a_c_t_o_r_s.size() + _c_h_i_l_d_r_e_n.size()); │ │ │ │ │ +46 gatheredFactors.push_back(_f_a_c_t_o_r_s.begin(), _f_a_c_t_o_r_s.end()); │ │ │ │ │ +47 gatheredFactors.push_back(childrenResults.begin(), childrenResults.end()); │ │ │ │ │ +48 │ │ │ │ │ +49 // Do dense elimination step │ │ │ │ │ +50 _K_e_y_V_e_c_t_o_r keyAsVector(1); keyAsVector[0] = _k_e_y; │ │ │ │ │ +51 auto eliminationResult = function(gatheredFactors, Ordering(keyAsVector)); │ │ │ │ │ +52 │ │ │ │ │ +53 // Add conditional to BayesNet │ │ │ │ │ +54 output->push_back(eliminationResult.first); │ │ │ │ │ 55 │ │ │ │ │ -56 typedef _F_a_s_t_M_a_p_<_K_e_y_,_ _F_a_s_t_V_e_c_t_o_r_<_s_i_z_e___t_> > _B_a_s_e; │ │ │ │ │ -57 GTSAM_EXPORT static const size_t Empty; │ │ │ │ │ -58 │ │ │ │ │ -61 │ │ │ │ │ -67 template │ │ │ │ │ -68 _V_a_r_i_a_b_l_e_S_l_o_t_s(const FG& factorGraph); │ │ │ │ │ -69 │ │ │ │ │ -71 │ │ │ │ │ +56 // Return result │ │ │ │ │ +57 return eliminationResult.second; │ │ │ │ │ +58 } │ │ │ │ │ +59 │ │ │ │ │ +60 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +61 template │ │ │ │ │ +62 void EliminationTree::Node::print( │ │ │ │ │ +63 const std::string& str, const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter) const │ │ │ │ │ +64 { │ │ │ │ │ +65 std::cout << str << "(" << keyFormatter(key) << ")\n"; │ │ │ │ │ +66 for(const _s_h_a_r_e_d_F_a_c_t_o_r& factor: factors) { │ │ │ │ │ +67 if(factor) │ │ │ │ │ +68 factor->print(str); │ │ │ │ │ +69 else │ │ │ │ │ +70 std::cout << str << "null factor\n"; │ │ │ │ │ +71 } │ │ │ │ │ +72 } │ │ │ │ │ +73 │ │ │ │ │ 74 │ │ │ │ │ -76 GTSAM_EXPORT void _p_r_i_n_t(const std::string& str = "VariableSlots: ") const; │ │ │ │ │ -77 │ │ │ │ │ -79 GTSAM_EXPORT bool _e_q_u_a_l_s(const _V_a_r_i_a_b_l_e_S_l_o_t_s& rhs, double tol = 0.0) const; │ │ │ │ │ -80 │ │ │ │ │ -82}; │ │ │ │ │ -83 │ │ │ │ │ -_8_5template<> struct _t_r_a_i_t_s<_V_a_r_i_a_b_l_e_S_l_o_t_s> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ -86 │ │ │ │ │ -87/* ************************************************************************* │ │ │ │ │ +75 /* ************************************************************************* │ │ │ │ │ */ │ │ │ │ │ -88template │ │ │ │ │ -_8_9_V_a_r_i_a_b_l_e_S_l_o_t_s_:_:_V_a_r_i_a_b_l_e_S_l_o_t_s(const FG& factorGraph) │ │ │ │ │ -90{ │ │ │ │ │ -91 gttic(VariableSlots_constructor); │ │ │ │ │ -92 static const bool debug = false; │ │ │ │ │ -93 │ │ │ │ │ -94 // Compute a mapping (called variableSlots) *from* each involved │ │ │ │ │ -95 // variable that will be in the new joint factor *to* the slot in each │ │ │ │ │ -96 // removed factor in which that variable appears. For each variable, │ │ │ │ │ -97 // this is stored as a vector of slot numbers, stored in order of the │ │ │ │ │ -98 // removed factors. The slot number is the max integer value if the │ │ │ │ │ -99 // factor does not involve that variable. │ │ │ │ │ -100 size_t jointFactorPos = 0; │ │ │ │ │ -101 for(const typename FG::sharedFactor& factor: factorGraph) { │ │ │ │ │ -102 if (!factor) { │ │ │ │ │ -103 continue; │ │ │ │ │ -104 } │ │ │ │ │ -105 size_t factorVarSlot = 0; │ │ │ │ │ -106 for(const _K_e_y involvedVariable: *factor) { │ │ │ │ │ -107 // Set the slot in this factor for this variable. If the │ │ │ │ │ -108 // variable was not already discovered, create an array for it │ │ │ │ │ -109 // that we'll fill with the slot indices for each factor that │ │ │ │ │ -110 // we're combining. Initially we put the max integer value in │ │ │ │ │ -111 // the array entry for each factor that will indicate the factor │ │ │ │ │ -112 // does not involve the variable. │ │ │ │ │ -113 iterator thisVarSlots; bool inserted; │ │ │ │ │ -114 boost::tie(thisVarSlots, inserted) = this->insert(std::make_pair │ │ │ │ │ -(involvedVariable, _F_a_s_t_V_e_c_t_o_r_<_s_i_z_e___t_>())); │ │ │ │ │ -115 if(inserted) │ │ │ │ │ -116 thisVarSlots->second.resize(factorGraph.nrFactors(), Empty); │ │ │ │ │ -117 thisVarSlots->second[jointFactorPos] = factorVarSlot; │ │ │ │ │ -118 if(debug) std::cout << " var " << involvedVariable << " rowblock " << │ │ │ │ │ -jointFactorPos << " comes from factor's slot " << factorVarSlot << std::endl; │ │ │ │ │ -119 ++ factorVarSlot; │ │ │ │ │ -120 } │ │ │ │ │ -121 ++ jointFactorPos; │ │ │ │ │ -122 } │ │ │ │ │ -123} │ │ │ │ │ -124 │ │ │ │ │ -125} │ │ │ │ │ -_F_a_s_t_M_a_p_._h │ │ │ │ │ -A thin wrapper around std::map that uses boost's fast_pool_allocator. │ │ │ │ │ -_T_e_s_t_a_b_l_e_._h │ │ │ │ │ -Concept check for values that can be used in unit tests. │ │ │ │ │ +76 template │ │ │ │ │ +_7_7 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_B_A_Y_E_S_N_E_T_,_G_R_A_P_H_>_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e(const _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e& │ │ │ │ │ +graph, │ │ │ │ │ +78 const _V_a_r_i_a_b_l_e_I_n_d_e_x& structure, const _O_r_d_e_r_i_n_g& order) │ │ │ │ │ +79 { │ │ │ │ │ +80 gttic(EliminationTree_Contructor); │ │ │ │ │ +81 │ │ │ │ │ +82 // Number of factors and variables - NOTE in the case of partial │ │ │ │ │ +elimination, n here may │ │ │ │ │ +83 // be fewer variables than are actually present in the graph. │ │ │ │ │ +84 const size_t m = graph.size(); │ │ │ │ │ +85 const size_t n = order.size(); │ │ │ │ │ +86 │ │ │ │ │ +87 static const size_t none = std::numeric_limits::max(); │ │ │ │ │ +88 │ │ │ │ │ +89 // Allocate result parent vector and vector of last factor columns │ │ │ │ │ +90 _F_a_s_t_V_e_c_t_o_r_<_s_h_a_r_e_d_N_o_d_e_> nodes(n); │ │ │ │ │ +91 _F_a_s_t_V_e_c_t_o_r_<_s_i_z_e___t_> parents(n, none); │ │ │ │ │ +92 _F_a_s_t_V_e_c_t_o_r_<_s_i_z_e___t_> prevCol(m, none); │ │ │ │ │ +93 _F_a_s_t_V_e_c_t_o_r_<_b_o_o_l_> factorUsed(m, false); │ │ │ │ │ +94 │ │ │ │ │ +95 try { │ │ │ │ │ +96 // for column j \in 1 to n do │ │ │ │ │ +97 for (size_t j = 0; j < n; j++) │ │ │ │ │ +98 { │ │ │ │ │ +99 // Retrieve the factors involving this variable and create the current node │ │ │ │ │ +_1_0_0 const _F_a_c_t_o_r_I_n_d_i_c_e_s& factors = structure[order[j]]; │ │ │ │ │ +101 const _s_h_a_r_e_d_N_o_d_e node = boost::make_shared(); │ │ │ │ │ +102 node->key = order[j]; │ │ │ │ │ +103 │ │ │ │ │ +104 // for row i \in Struct[A*j] do │ │ │ │ │ +105 node->children.reserve(factors.size()); │ │ │ │ │ +106 node->factors.reserve(factors.size()); │ │ │ │ │ +107 for(const size_t i: factors) { │ │ │ │ │ +_1_0_8 // If we already hit a variable in this factor, make the subtree containing │ │ │ │ │ +the previous │ │ │ │ │ +109 // variable in this factor a child of the current node. This means that the │ │ │ │ │ +variables │ │ │ │ │ +110 // eliminated earlier in the factor depend on the later variables in the │ │ │ │ │ +factor. If we │ │ │ │ │ +111 // haven't yet hit a variable in this factor, we add the factor to the │ │ │ │ │ +current node. │ │ │ │ │ +112 // TODO: Store root shortcuts instead of parents. │ │ │ │ │ +113 if (prevCol[i] != none) { │ │ │ │ │ +114 size_t k = prevCol[i]; │ │ │ │ │ +115 // Find root r of the current tree that contains k. Use raw pointers in │ │ │ │ │ +computing the │ │ │ │ │ +_1_1_6 // parents to avoid changing the reference counts while traversing up the │ │ │ │ │ +tree. │ │ │ │ │ +117 size_t r = k; │ │ │ │ │ +118 while (parents[r] != none) │ │ │ │ │ +119 r = parents[r]; │ │ │ │ │ +120 // If the root of the subtree involving this node is actually the current │ │ │ │ │ +node, │ │ │ │ │ +121 // TODO: what does this mean? forest? │ │ │ │ │ +122 if (r != j) { │ │ │ │ │ +123 // Now that we found the root, hook up parent and child pointers in the │ │ │ │ │ +nodes. │ │ │ │ │ +124 parents[r] = j; │ │ │ │ │ +125 node->children.push_back(nodes[r]); │ │ │ │ │ +126 } │ │ │ │ │ +127 } else { │ │ │ │ │ +128 // Add the factor to the current node since we are at the first variable in │ │ │ │ │ +this factor. │ │ │ │ │ +129 node->factors.push_back(graph[i]); │ │ │ │ │ +_1_3_0 factorUsed[i] = true; │ │ │ │ │ +131 } │ │ │ │ │ +132 prevCol[i] = j; │ │ │ │ │ +133 } │ │ │ │ │ +134 nodes[j] = node; │ │ │ │ │ +135 } │ │ │ │ │ +136 } catch(std::invalid_argument& e) { │ │ │ │ │ +_1_3_7 // If this is thrown from structure[order[j]] above, it means that it was │ │ │ │ │ +requested to │ │ │ │ │ +138 // eliminate a variable not present in the graph, so throw a more │ │ │ │ │ +informative error message. │ │ │ │ │ +139 (void)e; // Prevent unused variable warning │ │ │ │ │ +140 throw std::invalid_argument("EliminationTree: given ordering contains │ │ │ │ │ +variables that are not involved in the factor graph"); │ │ │ │ │ +141 } catch(...) { │ │ │ │ │ +_1_4_2 throw; │ │ │ │ │ +143 } │ │ │ │ │ +144 │ │ │ │ │ +145 // Find roots │ │ │ │ │ +146 assert(parents.empty() || parents.back() == none); // We expect the last- │ │ │ │ │ +eliminated node to be a root no matter what │ │ │ │ │ +147 for(size_t j = 0; j < n; ++j) │ │ │ │ │ +148 if(parents[j] == none) │ │ │ │ │ +149 _r_o_o_t_s__.push_back(nodes[j]); │ │ │ │ │ +150 │ │ │ │ │ +151 // Gather remaining factors (exclude null factors) │ │ │ │ │ +152 for(size_t i = 0; i < m; ++i) │ │ │ │ │ +153 if(!factorUsed[i] && graph[i]) │ │ │ │ │ +154 remainingFactors_.push_back(graph[i]); │ │ │ │ │ +155 } │ │ │ │ │ +156 │ │ │ │ │ +_1_5_7 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +158 template │ │ │ │ │ +_1_5_9 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_B_A_Y_E_S_N_E_T_,_G_R_A_P_H_>_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e( │ │ │ │ │ +160 const _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e& factorGraph, const _O_r_d_e_r_i_n_g& order) │ │ │ │ │ +161 { │ │ │ │ │ +162 gttic(ET_Create2); │ │ │ │ │ +163 // Build variable index first │ │ │ │ │ +164 const _V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex(factorGraph); │ │ │ │ │ +165 _T_h_i_s temp(factorGraph, variableIndex, order); │ │ │ │ │ +166 this->_s_w_a_p(temp); // Swap in the tree, and temp will be deleted │ │ │ │ │ +167 } │ │ │ │ │ +168 │ │ │ │ │ +169 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +170 template │ │ │ │ │ +171 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_B_A_Y_E_S_N_E_T_,_G_R_A_P_H_>& │ │ │ │ │ +_1_7_2 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_B_A_Y_E_S_N_E_T_,_G_R_A_P_H_>_:_:_o_p_e_r_a_t_o_r_=(const │ │ │ │ │ +_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_B_A_Y_E_S_N_E_T_,_G_R_A_P_H_>& other) │ │ │ │ │ +173 { │ │ │ │ │ +174 // Start by duplicating the tree. │ │ │ │ │ +175 _r_o_o_t_s__ = _t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_C_l_o_n_e_F_o_r_e_s_t(other); │ │ │ │ │ +176 │ │ │ │ │ +177 // Assign the remaining factors - these are pointers to factors in the │ │ │ │ │ +original factor graph and │ │ │ │ │ +178 // we do not clone them. │ │ │ │ │ +179 remainingFactors_ = other.remainingFactors_; │ │ │ │ │ +180 │ │ │ │ │ +181 return *this; │ │ │ │ │ +182 } │ │ │ │ │ +183 │ │ │ │ │ +184 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +185 template │ │ │ │ │ +186 std::pair, boost::shared_ptr > │ │ │ │ │ +_1_8_7 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_B_A_Y_E_S_N_E_T_,_G_R_A_P_H_>_:_:_e_l_i_m_i_n_a_t_e(Eliminate function) const │ │ │ │ │ +188 { │ │ │ │ │ +189 gttic(EliminationTree_eliminate); │ │ │ │ │ +190 // Allocate result │ │ │ │ │ +191 auto result = boost::make_shared(); │ │ │ │ │ +192 │ │ │ │ │ +193 // Run tree elimination algorithm │ │ │ │ │ +194 _F_a_s_t_V_e_c_t_o_r_<_s_h_a_r_e_d_F_a_c_t_o_r_> _r_e_m_a_i_n_i_n_g_F_a_c_t_o_r_s = inference::EliminateTree │ │ │ │ │ +(result, *this, function); │ │ │ │ │ +195 │ │ │ │ │ +196 // Add remaining factors that were not involved with eliminated variables │ │ │ │ │ +197 auto allRemainingFactors = boost::make_shared(); │ │ │ │ │ +198 allRemainingFactors->push_back(remainingFactors_.begin(), │ │ │ │ │ +remainingFactors_.end()); │ │ │ │ │ +199 allRemainingFactors->push_back(_r_e_m_a_i_n_i_n_g_F_a_c_t_o_r_s.begin(), │ │ │ │ │ +_r_e_m_a_i_n_i_n_g_F_a_c_t_o_r_s.end()); │ │ │ │ │ +200 │ │ │ │ │ +201 // Return result │ │ │ │ │ +202 return std::make_pair(result, allRemainingFactors); │ │ │ │ │ +203 } │ │ │ │ │ +204 │ │ │ │ │ +205 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +206 template │ │ │ │ │ +_2_0_7 void _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_B_A_Y_E_S_N_E_T_,_G_R_A_P_H_>_:_:_p_r_i_n_t(const std::string& name, const │ │ │ │ │ +_K_e_y_F_o_r_m_a_t_t_e_r& formatter) const │ │ │ │ │ +208 { │ │ │ │ │ +209 _t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_P_r_i_n_t_F_o_r_e_s_t(*this, name, formatter); │ │ │ │ │ +210 } │ │ │ │ │ +211 │ │ │ │ │ +212 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +213 template │ │ │ │ │ +_2_1_4 bool _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_B_A_Y_E_S_N_E_T_,_G_R_A_P_H_>_:_:_e_q_u_a_l_s(const _T_h_i_s& expected, double │ │ │ │ │ +tol) const │ │ │ │ │ +215 { │ │ │ │ │ +216 // Depth-first-traversal stacks │ │ │ │ │ +217 std::stack > stack1, stack2; │ │ │ │ │ +218 │ │ │ │ │ +219 // Add roots in sorted order │ │ │ │ │ +220 { │ │ │ │ │ +221 _F_a_s_t_M_a_p_<_K_e_y_,_s_h_a_r_e_d_N_o_d_e_> keys; │ │ │ │ │ +222 for(const _s_h_a_r_e_d_N_o_d_e& root: this->_r_o_o_t_s__) { keys.insert(std::make_pair │ │ │ │ │ +(root->key, root)); } │ │ │ │ │ +223 typedef typename _F_a_s_t_M_a_p_<_K_e_y_,_s_h_a_r_e_d_N_o_d_e_>_:_:_v_a_l_u_e___t_y_p_e Key_Node; │ │ │ │ │ +224 for(const Key_Node& key_node: keys) { stack1.push(key_node.second); } │ │ │ │ │ +225 } │ │ │ │ │ +226 { │ │ │ │ │ +227 _F_a_s_t_M_a_p_<_K_e_y_,_s_h_a_r_e_d_N_o_d_e_> keys; │ │ │ │ │ +228 for(const _s_h_a_r_e_d_N_o_d_e& root: expected._r_o_o_t_s__) { keys.insert(std::make_pair │ │ │ │ │ +(root->key, root)); } │ │ │ │ │ +229 typedef typename _F_a_s_t_M_a_p_<_K_e_y_,_s_h_a_r_e_d_N_o_d_e_>_:_:_v_a_l_u_e___t_y_p_e Key_Node; │ │ │ │ │ +230 for(const Key_Node& key_node: keys) { stack2.push(key_node.second); } │ │ │ │ │ +231 } │ │ │ │ │ +232 │ │ │ │ │ +233 // Traverse, adding children in sorted order │ │ │ │ │ +234 while(!stack1.empty() && !stack2.empty()) { │ │ │ │ │ +235 // Pop nodes │ │ │ │ │ +236 _s_h_a_r_e_d_N_o_d_e node1 = stack1.top(); │ │ │ │ │ +237 stack1.pop(); │ │ │ │ │ +238 _s_h_a_r_e_d_N_o_d_e node2 = stack2.top(); │ │ │ │ │ +239 stack2.pop(); │ │ │ │ │ +240 │ │ │ │ │ +241 // Compare nodes │ │ │ │ │ +242 if(node1->key != node2->key) │ │ │ │ │ +243 return false; │ │ │ │ │ +244 if(node1->factors.size() != node2->factors.size()) { │ │ │ │ │ +245 return false; │ │ │ │ │ +246 } else { │ │ │ │ │ +247 for(typename Node::Factors::const_iterator it1 = node1->factors.begin(), │ │ │ │ │ +it2 = node2->factors.begin(); │ │ │ │ │ +248 it1 != node1->factors.end(); ++it1, ++it2) // Only check it1 == end because │ │ │ │ │ +we already returned false for different counts │ │ │ │ │ +249 { │ │ │ │ │ +250 if(*it1 && *it2) { │ │ │ │ │ +251 if(!(*it1)->equals(**it2, tol)) │ │ │ │ │ +252 return false; │ │ │ │ │ +253 } else if((*it1 && !*it2) || (*it2 && !*it1)) { │ │ │ │ │ +254 return false; │ │ │ │ │ +255 } │ │ │ │ │ +256 } │ │ │ │ │ +257 } │ │ │ │ │ +258 │ │ │ │ │ +259 // Add children in sorted order │ │ │ │ │ +260 { │ │ │ │ │ +261 _F_a_s_t_M_a_p_<_K_e_y_,_s_h_a_r_e_d_N_o_d_e_> keys; │ │ │ │ │ +262 for(const _s_h_a_r_e_d_N_o_d_e& node: node1->children) { keys.insert(std::make_pair │ │ │ │ │ +(node->key, node)); } │ │ │ │ │ +263 typedef typename _F_a_s_t_M_a_p_<_K_e_y_,_s_h_a_r_e_d_N_o_d_e_>_:_:_v_a_l_u_e___t_y_p_e Key_Node; │ │ │ │ │ +264 for(const Key_Node& key_node: keys) { stack1.push(key_node.second); } │ │ │ │ │ +265 } │ │ │ │ │ +266 { │ │ │ │ │ +267 _F_a_s_t_M_a_p_<_K_e_y_,_s_h_a_r_e_d_N_o_d_e_> keys; │ │ │ │ │ +268 for(const _s_h_a_r_e_d_N_o_d_e& node: node2->children) { keys.insert(std::make_pair │ │ │ │ │ +(node->key, node)); } │ │ │ │ │ +269 typedef typename _F_a_s_t_M_a_p_<_K_e_y_,_s_h_a_r_e_d_N_o_d_e_>_:_:_v_a_l_u_e___t_y_p_e Key_Node; │ │ │ │ │ +270 for(const Key_Node& key_node: keys) { stack2.push(key_node.second); } │ │ │ │ │ +271 } │ │ │ │ │ +272 } │ │ │ │ │ +273 │ │ │ │ │ +274 // If either stack is not empty, the number of nodes differed │ │ │ │ │ +275 if(!stack1.empty() || !stack2.empty()) │ │ │ │ │ +276 return false; │ │ │ │ │ +277 │ │ │ │ │ +278 return true; │ │ │ │ │ +279 } │ │ │ │ │ +280 │ │ │ │ │ +281 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +282 template │ │ │ │ │ +_2_8_3 void _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_B_A_Y_E_S_N_E_T_,_G_R_A_P_H_>_:_:_s_w_a_p(_T_h_i_s& other) { │ │ │ │ │ +284 _r_o_o_t_s__.swap(other._r_o_o_t_s__); │ │ │ │ │ +285 remainingFactors_.swap(other.remainingFactors_); │ │ │ │ │ +286 } │ │ │ │ │ +287 │ │ │ │ │ +288 │ │ │ │ │ +289} │ │ │ │ │ +_t_r_e_e_T_r_a_v_e_r_s_a_l_-_i_n_s_t_._h │ │ │ │ │ _t_i_m_i_n_g_._h │ │ │ │ │ Timing utilities. │ │ │ │ │ -_F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ -A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ -_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ -Included from all GTSAM files. │ │ │ │ │ +_O_r_d_e_r_i_n_g_._h │ │ │ │ │ +Variable ordering for the elimination algorithm. │ │ │ │ │ +_V_a_r_i_a_b_l_e_I_n_d_e_x_._h │ │ │ │ │ +_i_n_f_e_r_e_n_c_e_-_i_n_s_t_._h │ │ │ │ │ +Contains generic inference algorithms that convert between templated graphical │ │ │ │ │ +models,... │ │ │ │ │ +_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h │ │ │ │ │ _g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ FastVector │ │ │ │ │ FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_I_n_d_i_c_e_s │ │ │ │ │ +FastVector< FactorIndex > FactorIndices │ │ │ │ │ +Define collection types: │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_C_l_o_n_e_F_o_r_e_s_t │ │ │ │ │ +FastVector< boost::shared_ptr< typename FOREST::Node > > CloneForest(const │ │ │ │ │ +FOREST &forest) │ │ │ │ │ +Clone a tree, copy-constructing new nodes (calling boost::make_shared) and │ │ │ │ │ +setting up child pointers ... │ │ │ │ │ +DDeeffiinniittiioonn treeTraversal-inst.h:189 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_P_r_i_n_t_F_o_r_e_s_t │ │ │ │ │ +void PrintForest(const FOREST &forest, std::string str, const KeyFormatter │ │ │ │ │ +&keyFormatter) │ │ │ │ │ +Print a tree, prefixing each line with str, and formatting keys using │ │ │ │ │ +keyFormatter. │ │ │ │ │ +DDeeffiinniittiioonn treeTraversal-inst.h:219 │ │ │ │ │ _g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ fast_pool_allocator instead of the defa... │ │ │ │ │ DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_S_l_o_t_s │ │ │ │ │ -A combined factor is assembled as one block of rows for each component factor. │ │ │ │ │ -DDeeffiinniittiioonn VariableSlots.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_S_l_o_t_s_:_:_p_r_i_n_t │ │ │ │ │ -GTSAM_EXPORT void print(const std::string &str="VariableSlots: ") const │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn VariableSlots.cpp:29 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_S_l_o_t_s_:_:_V_a_r_i_a_b_l_e_S_l_o_t_s │ │ │ │ │ -VariableSlots(const FG &factorGraph) │ │ │ │ │ -Constructor from a set of factors to be combined. │ │ │ │ │ -DDeeffiinniittiioonn VariableSlots.h:89 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ +An elimination tree is a data structure used intermediately during elimination. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const This &other, double tol=1e-9) const │ │ │ │ │ +Test whether the tree is equal to another. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree-inst.h:214 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &name="EliminationTree: ", const KeyFormatter │ │ │ │ │ +&formatter=DefaultKeyFormatter) const │ │ │ │ │ +Print the tree to cout. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree-inst.h:207 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_e_l_i_m_i_n_a_t_e │ │ │ │ │ +std::pair< boost::shared_ptr< BayesNetType >, boost::shared_ptr< │ │ │ │ │ +FactorGraphType > > eliminate(Eliminate function) const │ │ │ │ │ +Eliminate the factors to a Bayes net and remaining factor graph. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree-inst.h:187 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ +This & operator=(const This &other) │ │ │ │ │ +Assignment operator - makes a deep copy of the tree structure, but only │ │ │ │ │ +pointers to factors are copie... │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree-inst.h:172 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_r_o_o_t_s__ │ │ │ │ │ +FastVector< sharedNode > roots_ │ │ │ │ │ +concept check │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_s_h_a_r_e_d_F_a_c_t_o_r │ │ │ │ │ +boost::shared_ptr< FactorType > sharedFactor │ │ │ │ │ +Shared pointer to a factor. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:60 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_F_a_c_t_o_r_G_r_a_p_h_T_y_p_e │ │ │ │ │ +GRAPH FactorGraphType │ │ │ │ │ +The factor graph type. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_s_w_a_p │ │ │ │ │ +void swap(This &other) │ │ │ │ │ +Swap the data of this tree with another one, this operation is very fast. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree-inst.h:283 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ +EliminationTree() │ │ │ │ │ +Protected default constructor. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:161 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_ _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_,_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_s_h_a_r_e_d_N_o_d_e │ │ │ │ │ +boost::shared_ptr< Node > sharedNode │ │ │ │ │ +Shared pointer to Node. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:80 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_r_e_m_a_i_n_i_n_g_F_a_c_t_o_r_s │ │ │ │ │ +const FastVector< sharedFactor > & remainingFactors() const │ │ │ │ │ +Return the remaining factors that are not pulled into elimination. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:154 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_N_o_d_e_:_:_k_e_y │ │ │ │ │ +Key key │ │ │ │ │ +key associated with root │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:70 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_N_o_d_e_:_:_c_h_i_l_d_r_e_n │ │ │ │ │ +Children children │ │ │ │ │ +sub-trees │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_N_o_d_e_:_:_f_a_c_t_o_r_s │ │ │ │ │ +Factors factors │ │ │ │ │ +factors associated with root │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:71 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ +The VariableIndex class computes and stores the block column structure of a │ │ │ │ │ +factor graph. │ │ │ │ │ +DDeeffiinniittiioonn VariableIndex.h:43 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _V_a_r_i_a_b_l_e_S_l_o_t_s_._h │ │ │ │ │ + * _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_-_i_n_s_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00593.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/FactorGraph-inst.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/VariableSlots.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,44 +95,31 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces
│ │ │ │ -
FactorGraph-inst.h File Reference
│ │ │ │ +
VariableSlots.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -

Factor Graph Base Class. │ │ │ │ -More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Factor Graph Base Class.

│ │ │ │ -
Author
Carlos Nieto
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ -
│ │ │ │ -Alireza Fathi
│ │ │ │ -
│ │ │ │ -Michael Kaess
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Oct 5, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,25 +1,20 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -FactorGraph-inst.h File Reference │ │ │ │ │ -Factor Graph Base Class. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +VariableSlots.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Factor Graph Base Class. │ │ │ │ │ Author │ │ │ │ │ - Carlos Nieto │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Alireza Fathi │ │ │ │ │ - Michael Kaess │ │ │ │ │ Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Oct 5, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _F_a_c_t_o_r_G_r_a_p_h_-_i_n_s_t_._h │ │ │ │ │ + * _V_a_r_i_a_b_l_e_S_l_o_t_s_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00596.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/DotWriter.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Ordering.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,43 +96,49 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
DotWriter.h File Reference
│ │ │ │ +
Ordering.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Graphviz formatter. │ │ │ │ +

Variable ordering for the elimination algorithm. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

struct  gtsam::DotWriter
 DotWriter is a helper class for writing graphviz .dot files. More...
class  gtsam::Ordering
 
struct  gtsam::traits< Ordering >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Graphviz formatter.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
Date
December, 2021
│ │ │ │ +

Variable ordering for the elimination algorithm.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
│ │ │ │ +Andrew Melim
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +
Date
Sep 2, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,27 +1,31 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -DotWriter.h File Reference │ │ │ │ │ -Graphviz formatter. _M_o_r_e_._._. │ │ │ │ │ +Ordering.h File Reference │ │ │ │ │ +Variable ordering for the elimination algorithm. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r │ │ │ │ │ -  _D_o_t_W_r_i_t_e_r is a helper class for writing graphviz .dot files. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _O_r_d_e_r_i_n_g_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Graphviz formatter. │ │ │ │ │ +Variable ordering for the elimination algorithm. │ │ │ │ │ Author │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Andrew Melim │ │ │ │ │ Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - December, 2021 │ │ │ │ │ + Sep 2, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _D_o_t_W_r_i_t_e_r_._h │ │ │ │ │ + * _O_r_d_e_r_i_n_g_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00596_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/DotWriter.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Ordering.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,108 +98,255 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
DotWriter.h
│ │ │ │ +
Ordering.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010-2021, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/base/FastVector.h>
│ │ │ │ -
22#include <gtsam/base/Vector.h>
│ │ │ │ -
23#include <gtsam/inference/Key.h>
│ │ │ │ -
24
│ │ │ │ -
25#include <iosfwd>
│ │ │ │ -
26#include <map>
│ │ │ │ -
27#include <set>
│ │ │ │ -
28
│ │ │ │ -
29namespace gtsam {
│ │ │ │ -
30
│ │ │ │ -
│ │ │ │ -
35struct GTSAM_EXPORT DotWriter {
│ │ │ │ -
36 double figureWidthInches;
│ │ │ │ -
37 double figureHeightInches;
│ │ │ │ -
38 bool plotFactorPoints;
│ │ │ │ -
39 bool connectKeysToFactor;
│ │ │ │ -
41 bool binaryEdges;
│ │ │ │ -
42
│ │ │ │ -
47 std::map<Key, Vector2> variablePositions;
│ │ │ │ -
48
│ │ │ │ -
54 std::map<char, double> positionHints;
│ │ │ │ -
55
│ │ │ │ -
57 std::set<Key> boxes;
│ │ │ │ -
58
│ │ │ │ -
63 std::map<size_t, Vector2> factorPositions;
│ │ │ │ -
64
│ │ │ │ -
65 explicit DotWriter(double figureWidthInches = 5,
│ │ │ │ -
66 double figureHeightInches = 5,
│ │ │ │ -
67 bool plotFactorPoints = true,
│ │ │ │ -
68 bool connectKeysToFactor = true, bool binaryEdges = false)
│ │ │ │ -
69 : figureWidthInches(figureWidthInches),
│ │ │ │ -
70 figureHeightInches(figureHeightInches),
│ │ │ │ -
71 plotFactorPoints(plotFactorPoints),
│ │ │ │ -
72 connectKeysToFactor(connectKeysToFactor),
│ │ │ │ -
73 binaryEdges(binaryEdges) {}
│ │ │ │ -
74
│ │ │ │ -
76 void graphPreamble(std::ostream* os) const;
│ │ │ │ -
77
│ │ │ │ -
79 void digraphPreamble(std::ostream* os) const;
│ │ │ │ -
80
│ │ │ │ -
82 void drawVariable(Key key, const KeyFormatter& keyFormatter,
│ │ │ │ -
83 const boost::optional<Vector2>& position,
│ │ │ │ -
84 std::ostream* os) const;
│ │ │ │ -
85
│ │ │ │ -
87 static void DrawFactor(size_t i, const boost::optional<Vector2>& position,
│ │ │ │ -
88 std::ostream* os);
│ │ │ │ -
89
│ │ │ │ -
91 boost::optional<Vector2> variablePos(Key key) const;
│ │ │ │ -
92
│ │ │ │ -
94 void processFactor(size_t i, const KeyVector& keys,
│ │ │ │ -
95 const KeyFormatter& keyFormatter,
│ │ │ │ -
96 const boost::optional<Vector2>& position,
│ │ │ │ -
97 std::ostream* os) const;
│ │ │ │ -
98};
│ │ │ │ -
│ │ │ │ -
99
│ │ │ │ -
100} // namespace gtsam
│ │ │ │ -
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ -
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ - │ │ │ │ +
21#pragma once
│ │ │ │ +
22
│ │ │ │ +
23#include <gtsam/inference/Key.h>
│ │ │ │ +
24#include <gtsam/inference/VariableIndex.h>
│ │ │ │ +
25#include <gtsam/inference/MetisIndex.h>
│ │ │ │ +
26#include <gtsam/base/FastSet.h>
│ │ │ │ +
27
│ │ │ │ +
28#include <boost/assign/list_inserter.hpp>
│ │ │ │ +
29#include <algorithm>
│ │ │ │ +
30#include <vector>
│ │ │ │ +
31
│ │ │ │ +
32namespace gtsam {
│ │ │ │ +
33
│ │ │ │ +
│ │ │ │ +
34class Ordering: public KeyVector {
│ │ │ │ +
35protected:
│ │ │ │ +
36 typedef KeyVector Base;
│ │ │ │ +
37
│ │ │ │ +
38public:
│ │ │ │ +
39
│ │ │ │ +
│ │ │ │ +
41 enum OrderingType {
│ │ │ │ +
42 COLAMD, METIS, NATURAL, CUSTOM
│ │ │ │ +
43 };
│ │ │ │ +
│ │ │ │ +
44
│ │ │ │ +
45 typedef Ordering This;
│ │ │ │ +
46 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
47
│ │ │ │ +
49 GTSAM_EXPORT
│ │ │ │ +
│ │ │ │ +
50 Ordering() {
│ │ │ │ +
51 }
│ │ │ │ +
│ │ │ │ +
52
│ │ │ │ +
53 using KeyVector::KeyVector; // Inherit the KeyVector's constructors
│ │ │ │ +
54
│ │ │ │ +
56 template<typename KEYS>
│ │ │ │ +
│ │ │ │ +
57 explicit Ordering(const KEYS& keys) :
│ │ │ │ +
58 Base(keys.begin(), keys.end()) {
│ │ │ │ +
59 }
│ │ │ │ +
│ │ │ │ +
60
│ │ │ │ +
│ │ │ │ +
63 boost::assign::list_inserter<boost::assign_detail::call_push_back<This> > operator+=(
│ │ │ │ +
64 Key key) {
│ │ │ │ +
65 return boost::assign::make_list_inserter(
│ │ │ │ +
66 boost::assign_detail::call_push_back<This>(*this))(key);
│ │ │ │ +
67 }
│ │ │ │ +
│ │ │ │ +
68
│ │ │ │ +
75 This& operator+=(KeyVector& keys);
│ │ │ │ +
76
│ │ │ │ +
78 bool contains(const Key& key) const;
│ │ │ │ +
79
│ │ │ │ +
86 FastMap<Key, size_t> invert() const;
│ │ │ │ +
87
│ │ │ │ +
90
│ │ │ │ +
94 template<class FACTOR_GRAPH>
│ │ │ │ +
│ │ │ │ +
95 static Ordering Colamd(const FACTOR_GRAPH& graph) {
│ │ │ │ +
96 if (graph.empty())
│ │ │ │ +
97 return Ordering();
│ │ │ │ +
98 else
│ │ │ │ +
99 return Colamd(VariableIndex(graph));
│ │ │ │ +
100 }
│ │ │ │ +
│ │ │ │ +
101
│ │ │ │ +
103 static GTSAM_EXPORT Ordering Colamd(const VariableIndex& variableIndex);
│ │ │ │ +
104
│ │ │ │ +
113 template<class FACTOR_GRAPH>
│ │ │ │ +
│ │ │ │ +
114 static Ordering ColamdConstrainedLast(const FACTOR_GRAPH& graph,
│ │ │ │ +
115 const KeyVector& constrainLast, bool forceOrder = false) {
│ │ │ │ +
116 if (graph.empty())
│ │ │ │ +
117 return Ordering();
│ │ │ │ +
118 else
│ │ │ │ +
119 return ColamdConstrainedLast(VariableIndex(graph), constrainLast, forceOrder);
│ │ │ │ +
120 }
│ │ │ │ +
│ │ │ │ +
121
│ │ │ │ +
128 static GTSAM_EXPORT Ordering ColamdConstrainedLast(
│ │ │ │ +
129 const VariableIndex& variableIndex, const KeyVector& constrainLast,
│ │ │ │ +
130 bool forceOrder = false);
│ │ │ │ +
131
│ │ │ │ +
140 template<class FACTOR_GRAPH>
│ │ │ │ +
│ │ │ │ +
141 static Ordering ColamdConstrainedFirst(const FACTOR_GRAPH& graph,
│ │ │ │ +
142 const KeyVector& constrainFirst, bool forceOrder = false) {
│ │ │ │ +
143 if (graph.empty())
│ │ │ │ +
144 return Ordering();
│ │ │ │ +
145 else
│ │ │ │ +
146 return ColamdConstrainedFirst(VariableIndex(graph), constrainFirst, forceOrder);
│ │ │ │ +
147 }
│ │ │ │ +
│ │ │ │ +
148
│ │ │ │ +
156 static GTSAM_EXPORT Ordering ColamdConstrainedFirst(
│ │ │ │ +
157 const VariableIndex& variableIndex,
│ │ │ │ +
158 const KeyVector& constrainFirst, bool forceOrder = false);
│ │ │ │ +
159
│ │ │ │ +
169 template<class FACTOR_GRAPH>
│ │ │ │ +
│ │ │ │ +
170 static Ordering ColamdConstrained(const FACTOR_GRAPH& graph,
│ │ │ │ +
171 const FastMap<Key, int>& groups) {
│ │ │ │ +
172 if (graph.empty())
│ │ │ │ +
173 return Ordering();
│ │ │ │ +
174 else
│ │ │ │ +
175 return ColamdConstrained(VariableIndex(graph), groups);
│ │ │ │ +
176 }
│ │ │ │ +
│ │ │ │ +
177
│ │ │ │ +
185 static GTSAM_EXPORT Ordering ColamdConstrained(
│ │ │ │ +
186 const VariableIndex& variableIndex, const FastMap<Key, int>& groups);
│ │ │ │ +
187
│ │ │ │ +
189 template<class FACTOR_GRAPH>
│ │ │ │ +
│ │ │ │ +
190 static Ordering Natural(const FACTOR_GRAPH &fg) {
│ │ │ │ +
191 KeySet src = fg.keys();
│ │ │ │ +
192 KeyVector keys(src.begin(), src.end());
│ │ │ │ +
193 std::stable_sort(keys.begin(), keys.end());
│ │ │ │ +
194 return Ordering(keys.begin(), keys.end());
│ │ │ │ +
195 }
│ │ │ │ +
│ │ │ │ +
196
│ │ │ │ +
198 template<class FACTOR_GRAPH>
│ │ │ │ +
199 static GTSAM_EXPORT void CSRFormat(std::vector<int>& xadj,
│ │ │ │ +
200 std::vector<int>& adj, const FACTOR_GRAPH& graph);
│ │ │ │ +
201
│ │ │ │ +
203 static GTSAM_EXPORT Ordering Metis(const MetisIndex& met);
│ │ │ │ +
204
│ │ │ │ +
205 template<class FACTOR_GRAPH>
│ │ │ │ +
206 static Ordering Metis(const FACTOR_GRAPH& graph) {
│ │ │ │ +
207 if (graph.empty())
│ │ │ │ +
208 return Ordering();
│ │ │ │ +
209 else
│ │ │ │ +
210 return Metis(MetisIndex(graph));
│ │ │ │ +
211 }
│ │ │ │ +
212
│ │ │ │ +
214
│ │ │ │ +
217
│ │ │ │ +
218 template<class FACTOR_GRAPH>
│ │ │ │ +
219 static Ordering Create(OrderingType orderingType,
│ │ │ │ +
220 const FACTOR_GRAPH& graph) {
│ │ │ │ +
221 if (graph.empty())
│ │ │ │ +
222 return Ordering();
│ │ │ │ +
223
│ │ │ │ +
224 switch (orderingType) {
│ │ │ │ +
225 case COLAMD:
│ │ │ │ +
226 return Colamd(graph);
│ │ │ │ +
227 case METIS:
│ │ │ │ +
228 return Metis(graph);
│ │ │ │ +
229 case NATURAL:
│ │ │ │ +
230 return Natural(graph);
│ │ │ │ +
231 case CUSTOM:
│ │ │ │ +
232 throw std::runtime_error(
│ │ │ │ +
233 "Ordering::Create error: called with CUSTOM ordering type.");
│ │ │ │ +
234 default:
│ │ │ │ +
235 throw std::runtime_error(
│ │ │ │ +
236 "Ordering::Create error: called with unknown ordering type.");
│ │ │ │ +
237 }
│ │ │ │ +
238 }
│ │ │ │ +
239
│ │ │ │ +
241
│ │ │ │ +
244
│ │ │ │ +
245 GTSAM_EXPORT
│ │ │ │ +
246 void print(const std::string& str = "", const KeyFormatter& keyFormatter =
│ │ │ │ +
247 DefaultKeyFormatter) const;
│ │ │ │ +
248
│ │ │ │ +
249 GTSAM_EXPORT
│ │ │ │ +
250 bool equals(const Ordering& other, double tol = 1e-9) const;
│ │ │ │ +
251
│ │ │ │ +
253
│ │ │ │ +
254private:
│ │ │ │ +
256 static GTSAM_EXPORT Ordering ColamdConstrained(
│ │ │ │ +
257 const VariableIndex& variableIndex, std::vector<int>& cmember);
│ │ │ │ +
258
│ │ │ │ +
260 friend class boost::serialization::access;
│ │ │ │ +
261 template<class ARCHIVE>
│ │ │ │ +
262 void serialize(ARCHIVE & ar, const unsigned int version) {
│ │ │ │ +
263 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
264 }
│ │ │ │ +
265};
│ │ │ │ +
│ │ │ │ +
266
│ │ │ │ +
│ │ │ │ +
268template<> struct traits<Ordering> : public Testable<Ordering> {
│ │ │ │ +
269};
│ │ │ │ +
│ │ │ │ +
270
│ │ │ │ +
271}
│ │ │ │ +
272
│ │ │ │ +
FastSet.h
A thin wrapper around std::set that uses boost's fast_pool_allocator.
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::DotWriter
DotWriter is a helper class for writing graphviz .dot files.
Definition DotWriter.h:35
│ │ │ │ -
gtsam::DotWriter::figureWidthInches
double figureWidthInches
The figure width on paper in inches.
Definition DotWriter.h:36
│ │ │ │ -
gtsam::DotWriter::plotFactorPoints
bool plotFactorPoints
Plots each factor as a dot between the variables.
Definition DotWriter.h:38
│ │ │ │ -
gtsam::DotWriter::connectKeysToFactor
bool connectKeysToFactor
Draw a line from each key within a factor to the dot of the factor.
Definition DotWriter.h:39
│ │ │ │ -
gtsam::DotWriter::variablePositions
std::map< Key, Vector2 > variablePositions
Variable positions can be optionally specified and will be included in the dot file with a "!...
Definition DotWriter.h:47
│ │ │ │ -
gtsam::DotWriter::binaryEdges
bool binaryEdges
just use non-dotted edges for binary factors
Definition DotWriter.h:41
│ │ │ │ -
gtsam::DotWriter::positionHints
std::map< char, double > positionHints
The position hints allow one to use symbol character and index to specify position.
Definition DotWriter.h:54
│ │ │ │ -
gtsam::DotWriter::boxes
std::set< Key > boxes
A set of keys that will be displayed as a box.
Definition DotWriter.h:57
│ │ │ │ -
gtsam::DotWriter::factorPositions
std::map< size_t, Vector2 > factorPositions
Factor positions can be optionally specified and will be included in the dot file with a "!...
Definition DotWriter.h:63
│ │ │ │ -
gtsam::DotWriter::figureHeightInches
double figureHeightInches
The figure height on paper in inches.
Definition DotWriter.h:37
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ + │ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::MetisIndex
The MetisIndex class converts a factor graph into the Compressed Sparse Row format for use in METIS a...
Definition MetisIndex.h:45
│ │ │ │ +
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
gtsam::Ordering::Natural
static Ordering Natural(const FACTOR_GRAPH &fg)
Return a natural Ordering. Typically used by iterative solvers.
Definition Ordering.h:190
│ │ │ │ +
gtsam::Ordering::ColamdConstrained
static Ordering ColamdConstrained(const FACTOR_GRAPH &graph, const FastMap< Key, int > &groups)
Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details for note o...
Definition Ordering.h:170
│ │ │ │ +
gtsam::Ordering::Ordering
Ordering(const KEYS &keys)
Create from a container.
Definition Ordering.h:57
│ │ │ │ +
gtsam::Ordering::Colamd
static Ordering Colamd(const FACTOR_GRAPH &graph)
Compute a fill-reducing ordering using COLAMD from a factor graph (see details for note on performanc...
Definition Ordering.h:95
│ │ │ │ +
gtsam::Ordering::OrderingType
OrderingType
Type of ordering to use.
Definition Ordering.h:41
│ │ │ │ +
gtsam::Ordering::ColamdConstrainedLast
static Ordering ColamdConstrainedLast(const FACTOR_GRAPH &graph, const KeyVector &constrainLast, bool forceOrder=false)
Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details for note o...
Definition Ordering.h:114
│ │ │ │ +
gtsam::Ordering::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition Ordering.h:46
│ │ │ │ +
gtsam::Ordering::operator+=
boost::assign::list_inserter< boost::assign_detail::call_push_back< This > > operator+=(Key key)
Add new variables to the ordering as ordering += key1, key2, ... Equivalent to calling push_back.
Definition Ordering.h:63
│ │ │ │ +
gtsam::Ordering::ColamdConstrainedFirst
static Ordering ColamdConstrainedFirst(const FACTOR_GRAPH &graph, const KeyVector &constrainFirst, bool forceOrder=false)
Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details for note o...
Definition Ordering.h:141
│ │ │ │ +
gtsam::Ordering::Ordering
GTSAM_EXPORT Ordering()
Create an empty ordering.
Definition Ordering.h:50
│ │ │ │ +
gtsam::Ordering::Metis
static GTSAM_EXPORT Ordering Metis(const MetisIndex &met)
Compute an ordering determined by METIS from a VariableIndex.
Definition Ordering.cpp:212
│ │ │ │ +
gtsam::Ordering::invert
FastMap< Key, size_t > invert() const
Invert (not reverse) the ordering - returns a map from key to order position.
Definition Ordering.cpp:36
│ │ │ │ +
gtsam::Ordering::access
friend class boost::serialization::access
Serialization function.
Definition Ordering.h:260
│ │ │ │ +
gtsam::Ordering::This
Ordering This
Typedef to this class.
Definition Ordering.h:45
│ │ │ │ +
gtsam::Ordering::contains
bool contains(const Key &key) const
Check if key exists in ordering.
Definition Ordering.cpp:293
│ │ │ │ +
gtsam::Ordering::CSRFormat
static GTSAM_EXPORT void CSRFormat(std::vector< int > &xadj, std::vector< int > &adj, const FACTOR_GRAPH &graph)
METIS Formatting function.
│ │ │ │ +
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition VariableIndex.h:43
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,89 +1,205 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DotWriter.h │ │ │ │ │ +Ordering.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010-2021, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_c_t_o_r_._h> │ │ │ │ │ +21#pragma once │ │ │ │ │ +22 │ │ │ │ │ 23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ -24 │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ -28 │ │ │ │ │ -29namespace _g_t_s_a_m { │ │ │ │ │ -30 │ │ │ │ │ -_3_5struct GTSAM_EXPORT _D_o_t_W_r_i_t_e_r { │ │ │ │ │ -_3_6 double _f_i_g_u_r_e_W_i_d_t_h_I_n_c_h_e_s; │ │ │ │ │ -_3_7 double _f_i_g_u_r_e_H_e_i_g_h_t_I_n_c_h_e_s; │ │ │ │ │ -_3_8 bool _p_l_o_t_F_a_c_t_o_r_P_o_i_n_t_s; │ │ │ │ │ -_3_9 bool _c_o_n_n_e_c_t_K_e_y_s_T_o_F_a_c_t_o_r; │ │ │ │ │ -_4_1 bool _b_i_n_a_r_y_E_d_g_e_s; │ │ │ │ │ -42 │ │ │ │ │ -_4_7 std::map _v_a_r_i_a_b_l_e_P_o_s_i_t_i_o_n_s; │ │ │ │ │ -48 │ │ │ │ │ -_5_4 std::map _p_o_s_i_t_i_o_n_H_i_n_t_s; │ │ │ │ │ -55 │ │ │ │ │ -_5_7 std::set _b_o_x_e_s; │ │ │ │ │ -58 │ │ │ │ │ -_6_3 std::map _f_a_c_t_o_r_P_o_s_i_t_i_o_n_s; │ │ │ │ │ -64 │ │ │ │ │ -65 explicit _D_o_t_W_r_i_t_e_r(double figureWidthInches = 5, │ │ │ │ │ -66 double figureHeightInches = 5, │ │ │ │ │ -67 bool plotFactorPoints = true, │ │ │ │ │ -68 bool connectKeysToFactor = true, bool binaryEdges = false) │ │ │ │ │ -69 : figureWidthInches(figureWidthInches), │ │ │ │ │ -70 figureHeightInches(figureHeightInches), │ │ │ │ │ -71 plotFactorPoints(plotFactorPoints), │ │ │ │ │ -72 connectKeysToFactor(connectKeysToFactor), │ │ │ │ │ -73 binaryEdges(binaryEdges) {} │ │ │ │ │ -74 │ │ │ │ │ -76 void graphPreamble(std::ostream* os) const; │ │ │ │ │ -77 │ │ │ │ │ -79 void digraphPreamble(std::ostream* os) const; │ │ │ │ │ -80 │ │ │ │ │ -82 void drawVariable(_K_e_y key, const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter, │ │ │ │ │ -83 const boost::optional& position, │ │ │ │ │ -84 std::ostream* os) const; │ │ │ │ │ -85 │ │ │ │ │ -87 static void DrawFactor(size_t i, const boost::optional& position, │ │ │ │ │ -88 std::ostream* os); │ │ │ │ │ -89 │ │ │ │ │ -91 boost::optional variablePos(_K_e_y key) const; │ │ │ │ │ -92 │ │ │ │ │ -94 void processFactor(size_t i, const _K_e_y_V_e_c_t_o_r& keys, │ │ │ │ │ -95 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter, │ │ │ │ │ -96 const boost::optional& position, │ │ │ │ │ -97 std::ostream* os) const; │ │ │ │ │ -98}; │ │ │ │ │ -99 │ │ │ │ │ -100} // namespace gtsam │ │ │ │ │ -_V_e_c_t_o_r_._h │ │ │ │ │ -typedef and functions to augment Eigen's VectorXd │ │ │ │ │ -_F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ -A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ +24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_V_a_r_i_a_b_l_e_I_n_d_e_x_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_M_e_t_i_s_I_n_d_e_x_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_S_e_t_._h> │ │ │ │ │ +27 │ │ │ │ │ +28#include │ │ │ │ │ +29#include │ │ │ │ │ +30#include │ │ │ │ │ +31 │ │ │ │ │ +32namespace _g_t_s_a_m { │ │ │ │ │ +33 │ │ │ │ │ +_3_4class _O_r_d_e_r_i_n_g: public _K_e_y_V_e_c_t_o_r { │ │ │ │ │ +35protected: │ │ │ │ │ +36 typedef _K_e_y_V_e_c_t_o_r Base; │ │ │ │ │ +37 │ │ │ │ │ +38public: │ │ │ │ │ +39 │ │ │ │ │ +_4_1 enum _O_r_d_e_r_i_n_g_T_y_p_e { │ │ │ │ │ +42 COLAMD, METIS, NATURAL, CUSTOM │ │ │ │ │ +43 }; │ │ │ │ │ +44 │ │ │ │ │ +_4_5 typedef _O_r_d_e_r_i_n_g _T_h_i_s; │ │ │ │ │ +_4_6 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +47 │ │ │ │ │ +49 GTSAM_EXPORT │ │ │ │ │ +_5_0 _O_r_d_e_r_i_n_g() { │ │ │ │ │ +51 } │ │ │ │ │ +52 │ │ │ │ │ +53 using KeyVector::KeyVector; // Inherit the KeyVector's constructors │ │ │ │ │ +54 │ │ │ │ │ +56 template │ │ │ │ │ +_5_7 explicit _O_r_d_e_r_i_n_g(const KEYS& keys) : │ │ │ │ │ +58 Base(keys.begin(), keys.end()) { │ │ │ │ │ +59 } │ │ │ │ │ +60 │ │ │ │ │ +_6_3 boost::assign::list_inserter > │ │ │ │ │ +_o_p_e_r_a_t_o_r_+_=( │ │ │ │ │ +64 _K_e_y key) { │ │ │ │ │ +65 return boost::assign::make_list_inserter( │ │ │ │ │ +66 boost::assign_detail::call_push_back(*this))(key); │ │ │ │ │ +67 } │ │ │ │ │ +68 │ │ │ │ │ +75 _T_h_i_s& _o_p_e_r_a_t_o_r_+_=(_K_e_y_V_e_c_t_o_r& keys); │ │ │ │ │ +76 │ │ │ │ │ +78 bool _c_o_n_t_a_i_n_s(const _K_e_y& key) const; │ │ │ │ │ +79 │ │ │ │ │ +86 _F_a_s_t_M_a_p_<_K_e_y_,_ _s_i_z_e___t_> _i_n_v_e_r_t() const; │ │ │ │ │ +87 │ │ │ │ │ +90 │ │ │ │ │ +94 template │ │ │ │ │ +_9_5 static _O_r_d_e_r_i_n_g _C_o_l_a_m_d(const FACTOR_GRAPH& graph) { │ │ │ │ │ +96 if (graph.empty()) │ │ │ │ │ +97 return _O_r_d_e_r_i_n_g(); │ │ │ │ │ +98 else │ │ │ │ │ +99 return _C_o_l_a_m_d(_V_a_r_i_a_b_l_e_I_n_d_e_x(graph)); │ │ │ │ │ +100 } │ │ │ │ │ +101 │ │ │ │ │ +103 static GTSAM_EXPORT _O_r_d_e_r_i_n_g _C_o_l_a_m_d(const _V_a_r_i_a_b_l_e_I_n_d_e_x& variableIndex); │ │ │ │ │ +104 │ │ │ │ │ +113 template │ │ │ │ │ +_1_1_4 static _O_r_d_e_r_i_n_g _C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_L_a_s_t(const FACTOR_GRAPH& graph, │ │ │ │ │ +115 const _K_e_y_V_e_c_t_o_r& constrainLast, bool forceOrder = false) { │ │ │ │ │ +116 if (graph.empty()) │ │ │ │ │ +117 return _O_r_d_e_r_i_n_g(); │ │ │ │ │ +118 else │ │ │ │ │ +119 return _C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_L_a_s_t(_V_a_r_i_a_b_l_e_I_n_d_e_x(graph), constrainLast, │ │ │ │ │ +forceOrder); │ │ │ │ │ +120 } │ │ │ │ │ +121 │ │ │ │ │ +128 static GTSAM_EXPORT _O_r_d_e_r_i_n_g _C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_L_a_s_t( │ │ │ │ │ +129 const _V_a_r_i_a_b_l_e_I_n_d_e_x& variableIndex, const _K_e_y_V_e_c_t_o_r& constrainLast, │ │ │ │ │ +130 bool forceOrder = false); │ │ │ │ │ +131 │ │ │ │ │ +140 template │ │ │ │ │ +_1_4_1 static _O_r_d_e_r_i_n_g _C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_F_i_r_s_t(const FACTOR_GRAPH& graph, │ │ │ │ │ +142 const _K_e_y_V_e_c_t_o_r& constrainFirst, bool forceOrder = false) { │ │ │ │ │ +143 if (graph.empty()) │ │ │ │ │ +144 return _O_r_d_e_r_i_n_g(); │ │ │ │ │ +145 else │ │ │ │ │ +146 return _C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_F_i_r_s_t(_V_a_r_i_a_b_l_e_I_n_d_e_x(graph), constrainFirst, │ │ │ │ │ +forceOrder); │ │ │ │ │ +147 } │ │ │ │ │ +148 │ │ │ │ │ +156 static GTSAM_EXPORT _O_r_d_e_r_i_n_g _C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_F_i_r_s_t( │ │ │ │ │ +157 const _V_a_r_i_a_b_l_e_I_n_d_e_x& variableIndex, │ │ │ │ │ +158 const _K_e_y_V_e_c_t_o_r& constrainFirst, bool forceOrder = false); │ │ │ │ │ +159 │ │ │ │ │ +169 template │ │ │ │ │ +_1_7_0 static _O_r_d_e_r_i_n_g _C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d(const FACTOR_GRAPH& graph, │ │ │ │ │ +171 const _F_a_s_t_M_a_p_<_K_e_y_,_ _i_n_t_>& groups) { │ │ │ │ │ +172 if (graph.empty()) │ │ │ │ │ +173 return _O_r_d_e_r_i_n_g(); │ │ │ │ │ +174 else │ │ │ │ │ +175 return _C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d(_V_a_r_i_a_b_l_e_I_n_d_e_x(graph), groups); │ │ │ │ │ +176 } │ │ │ │ │ +177 │ │ │ │ │ +185 static GTSAM_EXPORT _O_r_d_e_r_i_n_g _C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d( │ │ │ │ │ +186 const _V_a_r_i_a_b_l_e_I_n_d_e_x& variableIndex, const _F_a_s_t_M_a_p_<_K_e_y_,_ _i_n_t_>& groups); │ │ │ │ │ +187 │ │ │ │ │ +189 template │ │ │ │ │ +_1_9_0 static _O_r_d_e_r_i_n_g _N_a_t_u_r_a_l(const FACTOR_GRAPH &fg) { │ │ │ │ │ +191 _K_e_y_S_e_t src = fg.keys(); │ │ │ │ │ +192 _K_e_y_V_e_c_t_o_r keys(src.begin(), src.end()); │ │ │ │ │ +193 std::stable_sort(keys.begin(), keys.end()); │ │ │ │ │ +194 return _O_r_d_e_r_i_n_g(keys.begin(), keys.end()); │ │ │ │ │ +195 } │ │ │ │ │ +196 │ │ │ │ │ +198 template │ │ │ │ │ +_1_9_9 static GTSAM_EXPORT void _C_S_R_F_o_r_m_a_t(std::vector& xadj, │ │ │ │ │ +200 std::vector& adj, const FACTOR_GRAPH& graph); │ │ │ │ │ +201 │ │ │ │ │ +203 static GTSAM_EXPORT _O_r_d_e_r_i_n_g _M_e_t_i_s(const _M_e_t_i_s_I_n_d_e_x& met); │ │ │ │ │ +204 │ │ │ │ │ +205 template │ │ │ │ │ +206 static _O_r_d_e_r_i_n_g _M_e_t_i_s(const FACTOR_GRAPH& graph) { │ │ │ │ │ +207 if (graph.empty()) │ │ │ │ │ +208 return _O_r_d_e_r_i_n_g(); │ │ │ │ │ +209 else │ │ │ │ │ +210 return _M_e_t_i_s(_M_e_t_i_s_I_n_d_e_x(graph)); │ │ │ │ │ +211 } │ │ │ │ │ +212 │ │ │ │ │ +214 │ │ │ │ │ +217 │ │ │ │ │ +218 template │ │ │ │ │ +219 static _O_r_d_e_r_i_n_g Create(_O_r_d_e_r_i_n_g_T_y_p_e orderingType, │ │ │ │ │ +220 const FACTOR_GRAPH& graph) { │ │ │ │ │ +221 if (graph.empty()) │ │ │ │ │ +222 return _O_r_d_e_r_i_n_g(); │ │ │ │ │ +223 │ │ │ │ │ +224 switch (orderingType) { │ │ │ │ │ +225 case COLAMD: │ │ │ │ │ +226 return _C_o_l_a_m_d(graph); │ │ │ │ │ +227 case METIS: │ │ │ │ │ +228 return _M_e_t_i_s(graph); │ │ │ │ │ +229 case NATURAL: │ │ │ │ │ +230 return _N_a_t_u_r_a_l(graph); │ │ │ │ │ +231 case CUSTOM: │ │ │ │ │ +232 throw std::runtime_error( │ │ │ │ │ +233 "Ordering::Create error: called with CUSTOM ordering type."); │ │ │ │ │ +234 default: │ │ │ │ │ +235 throw std::runtime_error( │ │ │ │ │ +236 "Ordering::Create error: called with unknown ordering type."); │ │ │ │ │ +237 } │ │ │ │ │ +238 } │ │ │ │ │ +239 │ │ │ │ │ +241 │ │ │ │ │ +244 │ │ │ │ │ +245 GTSAM_EXPORT │ │ │ │ │ +246 void print(const std::string& str = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ +247 DefaultKeyFormatter) const; │ │ │ │ │ +248 │ │ │ │ │ +249 GTSAM_EXPORT │ │ │ │ │ +250 bool equals(const _O_r_d_e_r_i_n_g& other, double tol = 1e-9) const; │ │ │ │ │ +251 │ │ │ │ │ +253 │ │ │ │ │ +254private: │ │ │ │ │ +256 static GTSAM_EXPORT _O_r_d_e_r_i_n_g _C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d( │ │ │ │ │ +257 const VariableIndex& variableIndex, std::vector& cmember); │ │ │ │ │ +258 │ │ │ │ │ +_2_6_0 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +261 template │ │ │ │ │ +262 void serialize(ARCHIVE & ar, const unsigned int version) { │ │ │ │ │ +263 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); │ │ │ │ │ +264 } │ │ │ │ │ +265}; │ │ │ │ │ +266 │ │ │ │ │ +_2_6_8template<> struct _t_r_a_i_t_s<_O_r_d_e_r_i_n_g> : public _T_e_s_t_a_b_l_e { │ │ │ │ │ +269}; │ │ │ │ │ +270 │ │ │ │ │ +271} │ │ │ │ │ +272 │ │ │ │ │ +_F_a_s_t_S_e_t_._h │ │ │ │ │ +A thin wrapper around std::set that uses boost's fast_pool_allocator. │ │ │ │ │ +_M_e_t_i_s_I_n_d_e_x_._h │ │ │ │ │ _K_e_y_._h │ │ │ │ │ +_V_a_r_i_a_b_l_e_I_n_d_e_x_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ FastVector< Key > KeyVector │ │ │ │ │ Define collection type once and for all - also used in wrappers. │ │ │ │ │ DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ @@ -91,53 +207,106 @@ │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r │ │ │ │ │ -DotWriter is a helper class for writing graphviz .dot files. │ │ │ │ │ -DDeeffiinniittiioonn DotWriter.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_f_i_g_u_r_e_W_i_d_t_h_I_n_c_h_e_s │ │ │ │ │ -double figureWidthInches │ │ │ │ │ -The figure width on paper in inches. │ │ │ │ │ -DDeeffiinniittiioonn DotWriter.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_p_l_o_t_F_a_c_t_o_r_P_o_i_n_t_s │ │ │ │ │ -bool plotFactorPoints │ │ │ │ │ -Plots each factor as a dot between the variables. │ │ │ │ │ -DDeeffiinniittiioonn DotWriter.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_c_o_n_n_e_c_t_K_e_y_s_T_o_F_a_c_t_o_r │ │ │ │ │ -bool connectKeysToFactor │ │ │ │ │ -Draw a line from each key within a factor to the dot of the factor. │ │ │ │ │ -DDeeffiinniittiioonn DotWriter.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_v_a_r_i_a_b_l_e_P_o_s_i_t_i_o_n_s │ │ │ │ │ -std::map< Key, Vector2 > variablePositions │ │ │ │ │ -Variable positions can be optionally specified and will be included in the dot │ │ │ │ │ -file with a "!... │ │ │ │ │ -DDeeffiinniittiioonn DotWriter.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_b_i_n_a_r_y_E_d_g_e_s │ │ │ │ │ -bool binaryEdges │ │ │ │ │ -just use non-dotted edges for binary factors │ │ │ │ │ -DDeeffiinniittiioonn DotWriter.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_p_o_s_i_t_i_o_n_H_i_n_t_s │ │ │ │ │ -std::map< char, double > positionHints │ │ │ │ │ -The position hints allow one to use symbol character and index to specify │ │ │ │ │ -position. │ │ │ │ │ -DDeeffiinniittiioonn DotWriter.h:54 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_b_o_x_e_s │ │ │ │ │ -std::set< Key > boxes │ │ │ │ │ -A set of keys that will be displayed as a box. │ │ │ │ │ -DDeeffiinniittiioonn DotWriter.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_f_a_c_t_o_r_P_o_s_i_t_i_o_n_s │ │ │ │ │ -std::map< size_t, Vector2 > factorPositions │ │ │ │ │ -Factor positions can be optionally specified and will be included in the dot │ │ │ │ │ -file with a "!... │ │ │ │ │ -DDeeffiinniittiioonn DotWriter.h:63 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_f_i_g_u_r_e_H_e_i_g_h_t_I_n_c_h_e_s │ │ │ │ │ -double figureHeightInches │ │ │ │ │ -The figure height on paper in inches. │ │ │ │ │ -DDeeffiinniittiioonn DotWriter.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ +FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ +fast_pool_allocator instead of the defa... │ │ │ │ │ +DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_M_e_t_i_s_I_n_d_e_x │ │ │ │ │ +The MetisIndex class converts a factor graph into the Compressed Sparse Row │ │ │ │ │ +format for use in METIS a... │ │ │ │ │ +DDeeffiinniittiioonn MetisIndex.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_N_a_t_u_r_a_l │ │ │ │ │ +static Ordering Natural(const FACTOR_GRAPH &fg) │ │ │ │ │ +Return a natural Ordering. Typically used by iterative solvers. │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:190 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d │ │ │ │ │ +static Ordering ColamdConstrained(const FACTOR_GRAPH &graph, const FastMap< │ │ │ │ │ +Key, int > &groups) │ │ │ │ │ +Compute a fill-reducing ordering using constrained COLAMD from a factor graph │ │ │ │ │ +(see details for note o... │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:170 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +Ordering(const KEYS &keys) │ │ │ │ │ +Create from a container. │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:57 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d │ │ │ │ │ +static Ordering Colamd(const FACTOR_GRAPH &graph) │ │ │ │ │ +Compute a fill-reducing ordering using COLAMD from a factor graph (see details │ │ │ │ │ +for note on performanc... │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:95 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_O_r_d_e_r_i_n_g_T_y_p_e │ │ │ │ │ +OrderingType │ │ │ │ │ +Type of ordering to use. │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_L_a_s_t │ │ │ │ │ +static Ordering ColamdConstrainedLast(const FACTOR_GRAPH &graph, const │ │ │ │ │ +KeyVector &constrainLast, bool forceOrder=false) │ │ │ │ │ +Compute a fill-reducing ordering using constrained COLAMD from a factor graph │ │ │ │ │ +(see details for note o... │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:114 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_o_p_e_r_a_t_o_r_+_= │ │ │ │ │ +boost::assign::list_inserter< boost::assign_detail::call_push_back< This > > │ │ │ │ │ +operator+=(Key key) │ │ │ │ │ +Add new variables to the ordering as ordering += key1, key2, ... Equivalent to │ │ │ │ │ +calling push_back. │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:63 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_F_i_r_s_t │ │ │ │ │ +static Ordering ColamdConstrainedFirst(const FACTOR_GRAPH &graph, const │ │ │ │ │ +KeyVector &constrainFirst, bool forceOrder=false) │ │ │ │ │ +Compute a fill-reducing ordering using constrained COLAMD from a factor graph │ │ │ │ │ +(see details for note o... │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:141 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +GTSAM_EXPORT Ordering() │ │ │ │ │ +Create an empty ordering. │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_M_e_t_i_s │ │ │ │ │ +static GTSAM_EXPORT Ordering Metis(const MetisIndex &met) │ │ │ │ │ +Compute an ordering determined by METIS from a VariableIndex. │ │ │ │ │ +DDeeffiinniittiioonn Ordering.cpp:212 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_i_n_v_e_r_t │ │ │ │ │ +FastMap< Key, size_t > invert() const │ │ │ │ │ +Invert (not reverse) the ordering - returns a map from key to order position. │ │ │ │ │ +DDeeffiinniittiioonn Ordering.cpp:36 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:260 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_T_h_i_s │ │ │ │ │ +Ordering This │ │ │ │ │ +Typedef to this class. │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_c_o_n_t_a_i_n_s │ │ │ │ │ +bool contains(const Key &key) const │ │ │ │ │ +Check if key exists in ordering. │ │ │ │ │ +DDeeffiinniittiioonn Ordering.cpp:293 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_C_S_R_F_o_r_m_a_t │ │ │ │ │ +static GTSAM_EXPORT void CSRFormat(std::vector< int > &xadj, std::vector< int > │ │ │ │ │ +&adj, const FACTOR_GRAPH &graph) │ │ │ │ │ +METIS Formatting function. │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ +The VariableIndex class computes and stores the block column structure of a │ │ │ │ │ +factor graph. │ │ │ │ │ +DDeeffiinniittiioonn VariableIndex.h:43 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _D_o_t_W_r_i_t_e_r_._h │ │ │ │ │ + * _O_r_d_e_r_i_n_g_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00599.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/MetisIndex.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/FactorGraph-inst.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,41 +94,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
MetisIndex.h File Reference
│ │ │ │ +
FactorGraph-inst.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ +

Factor Graph Base Class. │ │ │ │ +More...

│ │ │ │ + │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::MetisIndex
 The MetisIndex class converts a factor graph into the Compressed Sparse Row format for use in METIS algorithms. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Andrew Melim
│ │ │ │ -
Date
Oct. 10, 2014
│ │ │ │ +

Factor Graph Base Class.

│ │ │ │ +
Author
Carlos Nieto
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +
│ │ │ │ +Alireza Fathi
│ │ │ │ +
│ │ │ │ +Michael Kaess
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,26 +1,25 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -MetisIndex.h File Reference │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +FactorGraph-inst.h File Reference │ │ │ │ │ +Factor Graph Base Class. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_M_e_t_i_s_I_n_d_e_x │ │ │ │ │ -  The _M_e_t_i_s_I_n_d_e_x class converts a factor graph into the Compressed Sparse │ │ │ │ │ - Row format for use in METIS algorithms. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Factor Graph Base Class. │ │ │ │ │ Author │ │ │ │ │ - Andrew Melim │ │ │ │ │ - Date │ │ │ │ │ - Oct. 10, 2014 │ │ │ │ │ + Carlos Nieto │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Alireza Fathi │ │ │ │ │ + Michael Kaess │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _M_e_t_i_s_I_n_d_e_x_._h │ │ │ │ │ + * _F_a_c_t_o_r_G_r_a_p_h_-_i_n_s_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00599_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/MetisIndex.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/FactorGraph-inst.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,113 +98,243 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
MetisIndex.h
│ │ │ │ +
FactorGraph-inst.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │ -
9 * -------------------------------------------------------------------------- */
│ │ │ │ -
10
│ │ │ │ -
17#pragma once
│ │ │ │ -
18
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/inference/Key.h>
│ │ │ │ -
21#include <gtsam/inference/FactorGraph.h>
│ │ │ │ -
22#include <gtsam/base/types.h>
│ │ │ │ -
23#include <gtsam/base/timing.h>
│ │ │ │ -
24
│ │ │ │ -
25// Boost bimap generates many ugly warnings in CLANG
│ │ │ │ -
26#ifdef __clang__
│ │ │ │ -
27# pragma clang diagnostic push
│ │ │ │ -
28# pragma clang diagnostic ignored "-Wredeclared-class-member"
│ │ │ │ -
29#endif
│ │ │ │ -
30#include <boost/bimap.hpp>
│ │ │ │ -
31#ifdef __clang__
│ │ │ │ -
32# pragma clang diagnostic pop
│ │ │ │ -
33#endif
│ │ │ │ +
9
│ │ │ │ +
10 * -------------------------------------------------------------------------- */
│ │ │ │ +
11
│ │ │ │ +
22#pragma once
│ │ │ │ +
23
│ │ │ │ +
24#include <gtsam/inference/FactorGraph.h>
│ │ │ │ +
25
│ │ │ │ +
26#include <stdio.h>
│ │ │ │ +
27#include <algorithm>
│ │ │ │ +
28#include <iostream> // for cout :-(
│ │ │ │ +
29#include <fstream>
│ │ │ │ +
30#include <sstream>
│ │ │ │ +
31#include <string>
│ │ │ │ +
32
│ │ │ │ +
33namespace gtsam {
│ │ │ │
34
│ │ │ │ -
35#include <vector>
│ │ │ │ -
36
│ │ │ │ -
37namespace gtsam {
│ │ │ │ -
│ │ │ │ -
45class GTSAM_EXPORT MetisIndex {
│ │ │ │ -
46public:
│ │ │ │ -
47 typedef boost::shared_ptr<MetisIndex> shared_ptr;
│ │ │ │ -
48 typedef boost::bimap<Key, int32_t> bm_type;
│ │ │ │ -
49
│ │ │ │ -
50private:
│ │ │ │ -
51 std::vector<int32_t> xadj_; // Index of node's adjacency list in adj
│ │ │ │ -
52 std::vector<int32_t> adj_; // Stores ajacency lists of all nodes, appended into a single vector
│ │ │ │ -
53 boost::bimap<Key, int32_t> intKeyBMap_; // Stores Key <-> integer value relationship
│ │ │ │ -
54 size_t nKeys_;
│ │ │ │ -
55
│ │ │ │ -
56public:
│ │ │ │ -
59
│ │ │ │ -
│ │ │ │ -
61 MetisIndex() :
│ │ │ │ -
62 nKeys_(0) {
│ │ │ │ -
63 }
│ │ │ │ -
│ │ │ │ -
64
│ │ │ │ -
65 template<class FACTORGRAPH>
│ │ │ │ -
66 MetisIndex(const FACTORGRAPH& factorGraph) :
│ │ │ │ -
67 nKeys_(0) {
│ │ │ │ -
68 augment(factorGraph);
│ │ │ │ -
69 }
│ │ │ │ -
70
│ │ │ │ -
71 ~MetisIndex() {
│ │ │ │ -
72 }
│ │ │ │ -
76
│ │ │ │ -
81 template<class FACTORGRAPH>
│ │ │ │ -
82 void augment(const FACTORGRAPH& factors);
│ │ │ │ -
83
│ │ │ │ -
84 const std::vector<int32_t>& xadj() const {
│ │ │ │ -
85 return xadj_;
│ │ │ │ -
86 }
│ │ │ │ -
87 const std::vector<int32_t>& adj() const {
│ │ │ │ -
88 return adj_;
│ │ │ │ +
35/* ************************************************************************* */
│ │ │ │ +
36template <class FACTOR>
│ │ │ │ +
│ │ │ │ +
37void FactorGraph<FACTOR>::print(const std::string& s,
│ │ │ │ +
38 const KeyFormatter& formatter) const {
│ │ │ │ +
39 std::cout << (s.empty() ? "" : s + " ") << std::endl;
│ │ │ │ +
40 std::cout << "size: " << size() << std::endl;
│ │ │ │ +
41 for (size_t i = 0; i < factors_.size(); i++) {
│ │ │ │ +
42 std::stringstream ss;
│ │ │ │ +
43 ss << "factor " << i << ": ";
│ │ │ │ +
44 if (factors_[i]) factors_[i]->print(ss.str(), formatter);
│ │ │ │ +
45 }
│ │ │ │ +
46}
│ │ │ │ +
│ │ │ │ +
47
│ │ │ │ +
48/* ************************************************************************* */
│ │ │ │ +
49template <class FACTOR>
│ │ │ │ +
│ │ │ │ +
50bool FactorGraph<FACTOR>::equals(const This& fg, double tol) const {
│ │ │ │ +
51 // check whether the two factor graphs have the same number of factors.
│ │ │ │ +
52 if (factors_.size() != fg.size()) return false;
│ │ │ │ +
53
│ │ │ │ +
54 // check whether the factors are the same, in same order.
│ │ │ │ +
55 for (size_t i = 0; i < factors_.size(); i++) {
│ │ │ │ +
56 sharedFactor f1 = factors_[i], f2 = fg.factors_[i];
│ │ │ │ +
57 if (f1 == nullptr && f2 == nullptr) continue;
│ │ │ │ +
58 if (f1 == nullptr || f2 == nullptr) return false;
│ │ │ │ +
59 if (!f1->equals(*f2, tol)) return false;
│ │ │ │ +
60 }
│ │ │ │ +
61 return true;
│ │ │ │ +
62}
│ │ │ │ +
│ │ │ │ +
63
│ │ │ │ +
64/* ************************************************************************ */
│ │ │ │ +
65template <class FACTOR>
│ │ │ │ +
│ │ │ │ +
66double FactorGraph<FACTOR>::error(const HybridValues &values) const {
│ │ │ │ +
67 double error = 0.0;
│ │ │ │ +
68 for (auto &f : factors_) {
│ │ │ │ +
69 error += f->error(values);
│ │ │ │ +
70 }
│ │ │ │ +
71 return error;
│ │ │ │ +
72}
│ │ │ │ +
│ │ │ │ +
73
│ │ │ │ +
74/* ************************************************************************* */
│ │ │ │ +
75template <class FACTOR>
│ │ │ │ +
│ │ │ │ +
76size_t FactorGraph<FACTOR>::nrFactors() const {
│ │ │ │ +
77 size_t size_ = 0;
│ │ │ │ +
78 for (const sharedFactor& factor : factors_)
│ │ │ │ +
79 if (factor) size_++;
│ │ │ │ +
80 return size_;
│ │ │ │ +
81}
│ │ │ │ +
│ │ │ │ +
82
│ │ │ │ +
83/* ************************************************************************* */
│ │ │ │ +
84template <class FACTOR>
│ │ │ │ +
│ │ │ │ +
85KeySet FactorGraph<FACTOR>::keys() const {
│ │ │ │ +
86 KeySet keys;
│ │ │ │ +
87 for (const sharedFactor& factor : this->factors_) {
│ │ │ │ +
88 if (factor) keys.insert(factor->begin(), factor->end());
│ │ │ │
89 }
│ │ │ │ -
90 size_t nValues() const {
│ │ │ │ -
91 return nKeys_;
│ │ │ │ -
92 }
│ │ │ │ -
93 Key intToKey(int32_t value) const {
│ │ │ │ -
94 assert(value >= 0);
│ │ │ │ -
95 return intKeyBMap_.right.find(value)->second;
│ │ │ │ -
96 }
│ │ │ │ -
97
│ │ │ │ -
99};
│ │ │ │ -
│ │ │ │ -
100
│ │ │ │ -
101} // \ namesace gtsam
│ │ │ │ -
102
│ │ │ │ -
103#include <gtsam/inference/MetisIndex-inl.h>
│ │ │ │ -
timing.h
Timing utilities.
│ │ │ │ -
types.h
Typedefs for easier changing of types.
│ │ │ │ - │ │ │ │ -
FactorGraph.h
Factor Graph Base Class.
│ │ │ │ - │ │ │ │ +
90 return keys;
│ │ │ │ +
91}
│ │ │ │ +
│ │ │ │ +
92
│ │ │ │ +
93/* ************************************************************************* */
│ │ │ │ +
94template <class FACTOR>
│ │ │ │ +
│ │ │ │ +
95KeyVector FactorGraph<FACTOR>::keyVector() const {
│ │ │ │ +
96 KeyVector keys;
│ │ │ │ +
97 keys.reserve(2 * size()); // guess at size
│ │ │ │ +
98 for (const sharedFactor& factor : factors_)
│ │ │ │ +
99 if (factor) keys.insert(keys.end(), factor->begin(), factor->end());
│ │ │ │ +
100 std::sort(keys.begin(), keys.end());
│ │ │ │ +
101 auto last = std::unique(keys.begin(), keys.end());
│ │ │ │ +
102 keys.erase(last, keys.end());
│ │ │ │ +
103 return keys;
│ │ │ │ +
104}
│ │ │ │ +
│ │ │ │ +
105
│ │ │ │ +
106/* ************************************************************************* */
│ │ │ │ +
107template <class FACTOR>
│ │ │ │ +
108template <typename CONTAINER, typename>
│ │ │ │ +
│ │ │ │ +
109FactorIndices FactorGraph<FACTOR>::add_factors(const CONTAINER& factors,
│ │ │ │ +
110 bool useEmptySlots) {
│ │ │ │ +
111 const size_t num_factors = factors.size();
│ │ │ │ +
112 FactorIndices newFactorIndices(num_factors);
│ │ │ │ +
113 if (useEmptySlots) {
│ │ │ │ +
114 size_t i = 0;
│ │ │ │ +
115 for (size_t j = 0; j < num_factors; ++j) {
│ │ │ │ +
116 // Loop to find the next available factor slot
│ │ │ │ +
117 do {
│ │ │ │ +
118 if (i >= size())
│ │ │ │ +
119 // Make room for remaining factors, happens only once.
│ │ │ │ +
120 resize(size() + num_factors - j);
│ │ │ │ +
121 else if (at(i))
│ │ │ │ +
122 ++i; // Move on to the next slot or past end.
│ │ │ │ +
123 else
│ │ │ │ +
124 break; // We found an empty slot, break to fill it.
│ │ │ │ +
125 } while (true);
│ │ │ │ +
126
│ │ │ │ +
127 // Use the current slot, updating graph and newFactorSlots.
│ │ │ │ +
128 at(i) = factors[j];
│ │ │ │ +
129 newFactorIndices[j] = i;
│ │ │ │ +
130 }
│ │ │ │ +
131 } else {
│ │ │ │ +
132 // We're not looking for unused slots, so just add the factors at the end.
│ │ │ │ +
133 for (size_t i = 0; i < num_factors; ++i) newFactorIndices[i] = i + size();
│ │ │ │ +
134 push_back(factors);
│ │ │ │ +
135 }
│ │ │ │ +
136 return newFactorIndices;
│ │ │ │ +
137}
│ │ │ │ +
│ │ │ │ +
138
│ │ │ │ +
139/* ************************************************************************* */
│ │ │ │ +
140template <class FACTOR>
│ │ │ │ +
│ │ │ │ +
141void FactorGraph<FACTOR>::dot(std::ostream& os,
│ │ │ │ +
142 const KeyFormatter& keyFormatter,
│ │ │ │ +
143 const DotWriter& writer) const {
│ │ │ │ +
144 writer.graphPreamble(&os);
│ │ │ │ +
145
│ │ │ │ +
146 // Create nodes for each variable in the graph
│ │ │ │ +
147 for (Key key : keys()) {
│ │ │ │ +
148 auto position = writer.variablePos(key);
│ │ │ │ +
149 writer.drawVariable(key, keyFormatter, position, &os);
│ │ │ │ +
150 }
│ │ │ │ +
151 os << "\n";
│ │ │ │ +
152
│ │ │ │ +
153 // Create factors and variable connections
│ │ │ │ +
154 for (size_t i = 0; i < size(); ++i) {
│ │ │ │ +
155 const auto& factor = at(i);
│ │ │ │ +
156 if (factor) {
│ │ │ │ +
157 const KeyVector& factorKeys = factor->keys();
│ │ │ │ +
158 writer.processFactor(i, factorKeys, keyFormatter, boost::none, &os);
│ │ │ │ +
159 }
│ │ │ │ +
160 }
│ │ │ │ +
161
│ │ │ │ +
162 os << "}\n";
│ │ │ │ +
163 std::flush(os);
│ │ │ │ +
164}
│ │ │ │ +
│ │ │ │ +
165
│ │ │ │ +
166/* ************************************************************************* */
│ │ │ │ +
167template <class FACTOR>
│ │ │ │ +
│ │ │ │ +
168std::string FactorGraph<FACTOR>::dot(const KeyFormatter& keyFormatter,
│ │ │ │ +
169 const DotWriter& writer) const {
│ │ │ │ +
170 std::stringstream ss;
│ │ │ │ +
171 dot(ss, keyFormatter, writer);
│ │ │ │ +
172 return ss.str();
│ │ │ │ +
173}
│ │ │ │ +
│ │ │ │ +
174
│ │ │ │ +
175/* ************************************************************************* */
│ │ │ │ +
176template <class FACTOR>
│ │ │ │ +
│ │ │ │ +
177void FactorGraph<FACTOR>::saveGraph(const std::string& filename,
│ │ │ │ +
178 const KeyFormatter& keyFormatter,
│ │ │ │ +
179 const DotWriter& writer) const {
│ │ │ │ +
180 std::ofstream of(filename.c_str());
│ │ │ │ +
181 dot(of, keyFormatter, writer);
│ │ │ │ +
182 of.close();
│ │ │ │ +
183}
│ │ │ │ +
│ │ │ │ +
184
│ │ │ │ +
185} // namespace gtsam
│ │ │ │ +
FactorGraph.h
Factor Graph Base Class.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition Factor.h:34
│ │ │ │ +
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::MetisIndex
The MetisIndex class converts a factor graph into the Compressed Sparse Row format for use in METIS a...
Definition MetisIndex.h:45
│ │ │ │ -
gtsam::MetisIndex::MetisIndex
MetisIndex()
Default constructor, creates empty MetisIndex.
Definition MetisIndex.h:61
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ + │ │ │ │ +
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ +
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ +
gtsam::FactorGraph::print
virtual void print(const std::string &s="FactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const
Print out graph to std::cout, with optional key formatter.
Definition FactorGraph-inst.h:37
│ │ │ │ +
gtsam::FactorGraph::keys
KeySet keys() const
Potentially slow function to return all keys involved, sorted, as a set.
Definition FactorGraph-inst.h:85
│ │ │ │ +
gtsam::FactorGraph::add_factors
FactorIndices add_factors(const CONTAINER &factors, bool useEmptySlots=false)
Add new factors to a factor graph and returns a list of new factor indices, optionally finding and re...
Definition FactorGraph-inst.h:109
│ │ │ │ +
gtsam::FactorGraph::dot
void dot(std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
Output to graphviz format, stream version.
Definition FactorGraph-inst.h:141
│ │ │ │ +
gtsam::FactorGraph::keyVector
KeyVector keyVector() const
Potentially slow function to return all keys involved, sorted, as a vector.
Definition FactorGraph-inst.h:95
│ │ │ │ +
gtsam::FactorGraph::error
double error(const HybridValues &values) const
Add error for all factors.
Definition FactorGraph-inst.h:66
│ │ │ │ +
gtsam::FactorGraph::nrFactors
size_t nrFactors() const
return the number of non-null factors
Definition FactorGraph-inst.h:76
│ │ │ │ +
gtsam::FactorGraph::size
size_t size() const
return the number of factors (including any null factors set by remove() ).
Definition FactorGraph.h:326
│ │ │ │ +
gtsam::FactorGraph::equals
bool equals(const This &fg, double tol=1e-9) const
Check equality up to tolerance.
Definition FactorGraph-inst.h:50
│ │ │ │ +
gtsam::FactorGraph::sharedFactor
boost::shared_ptr< FACTOR > sharedFactor
Shared pointer to a factor.
Definition FactorGraph.h:101
│ │ │ │ +
gtsam::FactorGraph::factors_
FastVector< sharedFactor > factors_
concept check, makes sure FACTOR defines print and equals
Definition FactorGraph.h:131
│ │ │ │ +
gtsam::FactorGraph::saveGraph
void saveGraph(const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
output to file with graphviz format.
Definition FactorGraph-inst.h:177
│ │ │ │ +
gtsam::DotWriter
DotWriter is a helper class for writing graphviz .dot files.
Definition DotWriter.h:35
│ │ │ │ +
gtsam::DotWriter::drawVariable
void drawVariable(Key key, const KeyFormatter &keyFormatter, const boost::optional< Vector2 > &position, std::ostream *os) const
Create a variable dot fragment.
Definition DotWriter.cpp:42
│ │ │ │ +
gtsam::DotWriter::processFactor
void processFactor(size_t i, const KeyVector &keys, const KeyFormatter &keyFormatter, const boost::optional< Vector2 > &position, std::ostream *os) const
Draw a single factor, specified by its index i and its variable keys.
Definition DotWriter.cpp:96
│ │ │ │ +
gtsam::DotWriter::variablePos
boost::optional< Vector2 > variablePos(Key key) const
Return variable position or none.
Definition DotWriter.cpp:79
│ │ │ │ +
gtsam::DotWriter::graphPreamble
void graphPreamble(std::ostream *os) const
Write out preamble for graph, including size.
Definition DotWriter.cpp:30
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,117 +1,305 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -MetisIndex.h │ │ │ │ │ +FactorGraph-inst.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ -9 * ------------------------------------------------------------------------- │ │ │ │ │ +9 │ │ │ │ │ +10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ -10 │ │ │ │ │ -17#pragma once │ │ │ │ │ -18 │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_e_/_t_i_m_i_n_g_._h> │ │ │ │ │ -24 │ │ │ │ │ -25// Boost bimap generates many ugly warnings in CLANG │ │ │ │ │ -26#ifdef __clang__ │ │ │ │ │ -27# pragma clang diagnostic push │ │ │ │ │ -28# pragma clang diagnostic ignored "-Wredeclared-class-member" │ │ │ │ │ -29#endif │ │ │ │ │ -30#include │ │ │ │ │ -31#ifdef __clang__ │ │ │ │ │ -32# pragma clang diagnostic pop │ │ │ │ │ -33#endif │ │ │ │ │ +11 │ │ │ │ │ +22#pragma once │ │ │ │ │ +23 │ │ │ │ │ +24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +25 │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28#include // for cout :-( │ │ │ │ │ +29#include │ │ │ │ │ +30#include │ │ │ │ │ +31#include │ │ │ │ │ +32 │ │ │ │ │ +33namespace _g_t_s_a_m { │ │ │ │ │ 34 │ │ │ │ │ -35#include │ │ │ │ │ -36 │ │ │ │ │ -37namespace _g_t_s_a_m { │ │ │ │ │ -_4_5class GTSAM_EXPORT _M_e_t_i_s_I_n_d_e_x { │ │ │ │ │ -46public: │ │ │ │ │ -47 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -48 typedef boost::bimap bm_type; │ │ │ │ │ -49 │ │ │ │ │ -50private: │ │ │ │ │ -51 std::vector xadj_; // Index of node's adjacency list in adj │ │ │ │ │ -52 std::vector adj_; // Stores ajacency lists of all nodes, appended │ │ │ │ │ -into a single vector │ │ │ │ │ -53 boost::bimap intKeyBMap_; // Stores Key <-> integer value │ │ │ │ │ -relationship │ │ │ │ │ -54 size_t nKeys_; │ │ │ │ │ -55 │ │ │ │ │ -56public: │ │ │ │ │ -59 │ │ │ │ │ -_6_1 _M_e_t_i_s_I_n_d_e_x() : │ │ │ │ │ -62 nKeys_(0) { │ │ │ │ │ -63 } │ │ │ │ │ -64 │ │ │ │ │ -65 template │ │ │ │ │ -66 _M_e_t_i_s_I_n_d_e_x(const FACTORGRAPH& factorGraph) : │ │ │ │ │ -67 nKeys_(0) { │ │ │ │ │ -68 augment(factorGraph); │ │ │ │ │ -69 } │ │ │ │ │ -70 │ │ │ │ │ -71 ~MetisIndex() { │ │ │ │ │ -72 } │ │ │ │ │ -76 │ │ │ │ │ -81 template │ │ │ │ │ -82 void augment(const FACTORGRAPH& factors); │ │ │ │ │ -83 │ │ │ │ │ -84 const std::vector& xadj() const { │ │ │ │ │ -85 return xadj_; │ │ │ │ │ -86 } │ │ │ │ │ -87 const std::vector& adj() const { │ │ │ │ │ -88 return adj_; │ │ │ │ │ +35/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +36template │ │ │ │ │ +_3_7void _F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_>_:_:_p_r_i_n_t(const std::string& s, │ │ │ │ │ +38 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter) const { │ │ │ │ │ +39 std::cout << (s.empty() ? "" : s + " ") << std::endl; │ │ │ │ │ +40 std::cout << "size: " << size() << std::endl; │ │ │ │ │ +41 for (size_t i = 0; i < factors_.size(); i++) { │ │ │ │ │ +42 std::stringstream ss; │ │ │ │ │ +43 ss << "factor " << i << ": "; │ │ │ │ │ +44 if (factors_[i]) factors_[i]->print(ss.str(), formatter); │ │ │ │ │ +45 } │ │ │ │ │ +46} │ │ │ │ │ +47 │ │ │ │ │ +48/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +49template │ │ │ │ │ +_5_0bool _F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_>_:_:_e_q_u_a_l_s(const _T_h_i_s& fg, double tol) const { │ │ │ │ │ +51 // check whether the two factor graphs have the same number of factors. │ │ │ │ │ +52 if (factors_.size() != fg._s_i_z_e()) return false; │ │ │ │ │ +53 │ │ │ │ │ +54 // check whether the factors are the same, in same order. │ │ │ │ │ +55 for (size_t i = 0; i < factors_.size(); i++) { │ │ │ │ │ +56 _s_h_a_r_e_d_F_a_c_t_o_r f1 = factors_[i], f2 = fg._f_a_c_t_o_r_s__[i]; │ │ │ │ │ +57 if (f1 == nullptr && f2 == nullptr) continue; │ │ │ │ │ +58 if (f1 == nullptr || f2 == nullptr) return false; │ │ │ │ │ +59 if (!f1->equals(*f2, tol)) return false; │ │ │ │ │ +60 } │ │ │ │ │ +61 return true; │ │ │ │ │ +62} │ │ │ │ │ +63 │ │ │ │ │ +64/* ************************************************************************ │ │ │ │ │ +*/ │ │ │ │ │ +65template │ │ │ │ │ +_6_6double _F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_>_:_:_e_r_r_o_r(const _H_y_b_r_i_d_V_a_l_u_e_s &values) const { │ │ │ │ │ +67 double error = 0.0; │ │ │ │ │ +68 for (auto &f : factors_) { │ │ │ │ │ +69 error += f->error(values); │ │ │ │ │ +70 } │ │ │ │ │ +71 return error; │ │ │ │ │ +72} │ │ │ │ │ +73 │ │ │ │ │ +74/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +75template │ │ │ │ │ +_7_6size_t _F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_>_:_:_n_r_F_a_c_t_o_r_s() const { │ │ │ │ │ +77 size_t size_ = 0; │ │ │ │ │ +78 for (const _s_h_a_r_e_d_F_a_c_t_o_r& factor : factors_) │ │ │ │ │ +79 if (factor) size_++; │ │ │ │ │ +80 return size_; │ │ │ │ │ +81} │ │ │ │ │ +82 │ │ │ │ │ +83/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +84template │ │ │ │ │ +_8_5_K_e_y_S_e_t _F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_>_:_:_k_e_y_s() const { │ │ │ │ │ +86 _K_e_y_S_e_t keys; │ │ │ │ │ +87 for (const _s_h_a_r_e_d_F_a_c_t_o_r& factor : this->factors_) { │ │ │ │ │ +88 if (factor) keys.insert(factor->begin(), factor->end()); │ │ │ │ │ 89 } │ │ │ │ │ -90 size_t nValues() const { │ │ │ │ │ -91 return nKeys_; │ │ │ │ │ -92 } │ │ │ │ │ -93 _K_e_y intToKey(int32_t value) const { │ │ │ │ │ -94 assert(value >= 0); │ │ │ │ │ -95 return intKeyBMap_.right.find(value)->second; │ │ │ │ │ -96 } │ │ │ │ │ -97 │ │ │ │ │ -99}; │ │ │ │ │ -100 │ │ │ │ │ -101} // \ namesace gtsam │ │ │ │ │ -102 │ │ │ │ │ -103#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_M_e_t_i_s_I_n_d_e_x_-_i_n_l_._h> │ │ │ │ │ -_t_i_m_i_n_g_._h │ │ │ │ │ -Timing utilities. │ │ │ │ │ -_t_y_p_e_s_._h │ │ │ │ │ -Typedefs for easier changing of types. │ │ │ │ │ -_K_e_y_._h │ │ │ │ │ +90 return keys; │ │ │ │ │ +91} │ │ │ │ │ +92 │ │ │ │ │ +93/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +94template │ │ │ │ │ +_9_5_K_e_y_V_e_c_t_o_r _F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_>_:_:_k_e_y_V_e_c_t_o_r() const { │ │ │ │ │ +96 _K_e_y_V_e_c_t_o_r keys; │ │ │ │ │ +97 keys.reserve(2 * size()); // guess at size │ │ │ │ │ +98 for (const _s_h_a_r_e_d_F_a_c_t_o_r& factor : factors_) │ │ │ │ │ +99 if (factor) keys.insert(keys.end(), factor->begin(), factor->end()); │ │ │ │ │ +100 std::sort(keys.begin(), keys.end()); │ │ │ │ │ +101 auto last = std::unique(keys.begin(), keys.end()); │ │ │ │ │ +102 keys.erase(last, keys.end()); │ │ │ │ │ +103 return keys; │ │ │ │ │ +104} │ │ │ │ │ +105 │ │ │ │ │ +106/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +107template │ │ │ │ │ +108template │ │ │ │ │ +_1_0_9_F_a_c_t_o_r_I_n_d_i_c_e_s _F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_>_:_:_a_d_d___f_a_c_t_o_r_s(const CONTAINER& factors, │ │ │ │ │ +110 bool useEmptySlots) { │ │ │ │ │ +111 const size_t num_factors = factors.size(); │ │ │ │ │ +112 _F_a_c_t_o_r_I_n_d_i_c_e_s newFactorIndices(num_factors); │ │ │ │ │ +113 if (useEmptySlots) { │ │ │ │ │ +114 size_t i = 0; │ │ │ │ │ +115 for (size_t j = 0; j < num_factors; ++j) { │ │ │ │ │ +116 // Loop to find the next available factor slot │ │ │ │ │ +117 do { │ │ │ │ │ +118 if (i >= size()) │ │ │ │ │ +119 // Make room for remaining factors, happens only once. │ │ │ │ │ +120 resize(size() + num_factors - j); │ │ │ │ │ +121 else if (at(i)) │ │ │ │ │ +122 ++i; // Move on to the next slot or past end. │ │ │ │ │ +123 else │ │ │ │ │ +124 break; // We found an empty slot, break to fill it. │ │ │ │ │ +125 } while (true); │ │ │ │ │ +126 │ │ │ │ │ +127 // Use the current slot, updating graph and newFactorSlots. │ │ │ │ │ +128 at(i) = factors[j]; │ │ │ │ │ +129 newFactorIndices[j] = i; │ │ │ │ │ +130 } │ │ │ │ │ +131 } else { │ │ │ │ │ +132 // We're not looking for unused slots, so just add the factors at the end. │ │ │ │ │ +133 for (size_t i = 0; i < num_factors; ++i) newFactorIndices[i] = i + size(); │ │ │ │ │ +134 push_back(factors); │ │ │ │ │ +135 } │ │ │ │ │ +136 return newFactorIndices; │ │ │ │ │ +137} │ │ │ │ │ +138 │ │ │ │ │ +139/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +140template │ │ │ │ │ +_1_4_1void _F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_>_:_:_d_o_t(std::ostream& os, │ │ │ │ │ +142 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter, │ │ │ │ │ +143 const _D_o_t_W_r_i_t_e_r& writer) const { │ │ │ │ │ +144 writer._g_r_a_p_h_P_r_e_a_m_b_l_e(&os); │ │ │ │ │ +145 │ │ │ │ │ +146 // Create nodes for each variable in the graph │ │ │ │ │ +147 for (_K_e_y key : keys()) { │ │ │ │ │ +148 auto position = writer._v_a_r_i_a_b_l_e_P_o_s(key); │ │ │ │ │ +149 writer._d_r_a_w_V_a_r_i_a_b_l_e(key, keyFormatter, position, &os); │ │ │ │ │ +150 } │ │ │ │ │ +151 os << "\n"; │ │ │ │ │ +152 │ │ │ │ │ +153 // Create factors and variable connections │ │ │ │ │ +154 for (size_t i = 0; i < size(); ++i) { │ │ │ │ │ +155 const auto& factor = at(i); │ │ │ │ │ +156 if (factor) { │ │ │ │ │ +157 const _K_e_y_V_e_c_t_o_r& factorKeys = factor->keys(); │ │ │ │ │ +158 writer._p_r_o_c_e_s_s_F_a_c_t_o_r(i, factorKeys, keyFormatter, boost::none, &os); │ │ │ │ │ +159 } │ │ │ │ │ +160 } │ │ │ │ │ +161 │ │ │ │ │ +162 os << "}\n"; │ │ │ │ │ +163 std::flush(os); │ │ │ │ │ +164} │ │ │ │ │ +165 │ │ │ │ │ +166/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +167template │ │ │ │ │ +_1_6_8std::string _F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_>_:_:_d_o_t(const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter, │ │ │ │ │ +169 const _D_o_t_W_r_i_t_e_r& writer) const { │ │ │ │ │ +170 std::stringstream ss; │ │ │ │ │ +171 _d_o_t(ss, keyFormatter, writer); │ │ │ │ │ +172 return ss.str(); │ │ │ │ │ +173} │ │ │ │ │ +174 │ │ │ │ │ +175/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +176template │ │ │ │ │ +_1_7_7void _F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_>_:_:_s_a_v_e_G_r_a_p_h(const std::string& filename, │ │ │ │ │ +178 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter, │ │ │ │ │ +179 const _D_o_t_W_r_i_t_e_r& writer) const { │ │ │ │ │ +180 std::ofstream of(filename.c_str()); │ │ │ │ │ +181 _d_o_t(of, keyFormatter, writer); │ │ │ │ │ +182 of.close(); │ │ │ │ │ +183} │ │ │ │ │ +184 │ │ │ │ │ +185} // namespace gtsam │ │ │ │ │ _F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ Factor Graph Base Class. │ │ │ │ │ -_M_e_t_i_s_I_n_d_e_x_-_i_n_l_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_I_n_d_i_c_e_s │ │ │ │ │ +FastVector< FactorIndex > FactorIndices │ │ │ │ │ +Define collection types: │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_d_o_t │ │ │ │ │ +double dot(const V1 &a, const V2 &b) │ │ │ │ │ +Dot product. │ │ │ │ │ +DDeeffiinniittiioonn Vector.h:195 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_M_e_t_i_s_I_n_d_e_x │ │ │ │ │ -The MetisIndex class converts a factor graph into the Compressed Sparse Row │ │ │ │ │ -format for use in METIS a... │ │ │ │ │ -DDeeffiinniittiioonn MetisIndex.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_M_e_t_i_s_I_n_d_e_x_:_:_M_e_t_i_s_I_n_d_e_x │ │ │ │ │ -MetisIndex() │ │ │ │ │ -Default constructor, creates empty MetisIndex. │ │ │ │ │ -DDeeffiinniittiioonn MetisIndex.h:61 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ +nodes. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_p_r_i_n_t │ │ │ │ │ +virtual void print(const std::string &s="FactorGraph", const KeyFormatter │ │ │ │ │ +&formatter=DefaultKeyFormatter) const │ │ │ │ │ +Print out graph to std::cout, with optional key formatter. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph-inst.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_k_e_y_s │ │ │ │ │ +KeySet keys() const │ │ │ │ │ +Potentially slow function to return all keys involved, sorted, as a set. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph-inst.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d___f_a_c_t_o_r_s │ │ │ │ │ +FactorIndices add_factors(const CONTAINER &factors, bool useEmptySlots=false) │ │ │ │ │ +Add new factors to a factor graph and returns a list of new factor indices, │ │ │ │ │ +optionally finding and re... │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph-inst.h:109 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_d_o_t │ │ │ │ │ +void dot(std::ostream &os, const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const │ │ │ │ │ +Output to graphviz format, stream version. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph-inst.h:141 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_k_e_y_V_e_c_t_o_r │ │ │ │ │ +KeyVector keyVector() const │ │ │ │ │ +Potentially slow function to return all keys involved, sorted, as a vector. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph-inst.h:95 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_e_r_r_o_r │ │ │ │ │ +double error(const HybridValues &values) const │ │ │ │ │ +Add error for all factors. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph-inst.h:66 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_n_r_F_a_c_t_o_r_s │ │ │ │ │ +size_t nrFactors() const │ │ │ │ │ +return the number of non-null factors │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph-inst.h:76 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_s_i_z_e │ │ │ │ │ +size_t size() const │ │ │ │ │ +return the number of factors (including any null factors set by remove() ). │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:326 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const This &fg, double tol=1e-9) const │ │ │ │ │ +Check equality up to tolerance. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph-inst.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d_F_a_c_t_o_r │ │ │ │ │ +boost::shared_ptr< FACTOR > sharedFactor │ │ │ │ │ +Shared pointer to a factor. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:101 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_f_a_c_t_o_r_s__ │ │ │ │ │ +FastVector< sharedFactor > factors_ │ │ │ │ │ +concept check, makes sure FACTOR defines print and equals │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:131 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_s_a_v_e_G_r_a_p_h │ │ │ │ │ +void saveGraph(const std::string &filename, const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const │ │ │ │ │ +output to file with graphviz format. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph-inst.h:177 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r │ │ │ │ │ +DotWriter is a helper class for writing graphviz .dot files. │ │ │ │ │ +DDeeffiinniittiioonn DotWriter.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_d_r_a_w_V_a_r_i_a_b_l_e │ │ │ │ │ +void drawVariable(Key key, const KeyFormatter &keyFormatter, const boost:: │ │ │ │ │ +optional< Vector2 > &position, std::ostream *os) const │ │ │ │ │ +Create a variable dot fragment. │ │ │ │ │ +DDeeffiinniittiioonn DotWriter.cpp:42 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_p_r_o_c_e_s_s_F_a_c_t_o_r │ │ │ │ │ +void processFactor(size_t i, const KeyVector &keys, const KeyFormatter │ │ │ │ │ +&keyFormatter, const boost::optional< Vector2 > &position, std::ostream *os) │ │ │ │ │ +const │ │ │ │ │ +Draw a single factor, specified by its index i and its variable keys. │ │ │ │ │ +DDeeffiinniittiioonn DotWriter.cpp:96 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_v_a_r_i_a_b_l_e_P_o_s │ │ │ │ │ +boost::optional< Vector2 > variablePos(Key key) const │ │ │ │ │ +Return variable position or none. │ │ │ │ │ +DDeeffiinniittiioonn DotWriter.cpp:79 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_g_r_a_p_h_P_r_e_a_m_b_l_e │ │ │ │ │ +void graphPreamble(std::ostream *os) const │ │ │ │ │ +Write out preamble for graph, including size. │ │ │ │ │ +DDeeffiinniittiioonn DotWriter.cpp:30 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _M_e_t_i_s_I_n_d_e_x_._h │ │ │ │ │ + * _F_a_c_t_o_r_G_r_a_p_h_-_i_n_s_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00602.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/EliminateableFactorGraph.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTreeCliqueBase.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,46 +96,42 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
EliminateableFactorGraph.h File Reference
│ │ │ │ +
BayesTreeCliqueBase.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Variable elimination algorithms for factor graphs. │ │ │ │ +

Base class for cliques of a BayesTree. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

struct  gtsam::EliminationTraits< GRAPH >
 Traits class for eliminateable factor graphs, specifies the types that result from elimination, etc. More...
 
class  gtsam::EliminateableFactorGraph< FACTOR_GRAPH >
 EliminateableFactorGraph is a base class for factor graphs that contains elimination algorithms. More...
class  gtsam::BayesTreeCliqueBase< DERIVED, FACTORGRAPH >
 This is the base class for BayesTree cliques. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Variable elimination algorithms for factor graphs.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Apr 21, 2013
│ │ │ │ +

Base class for cliques of a BayesTree.

│ │ │ │ +
Author
Richard Roberts and Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,32 +1,25 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -EliminateableFactorGraph.h File Reference │ │ │ │ │ -Variable elimination algorithms for factor graphs. _M_o_r_e_._._. │ │ │ │ │ +BayesTreeCliqueBase.h File Reference │ │ │ │ │ +Base class for cliques of a BayesTree. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _G_R_A_P_H_ _> │ │ │ │ │ -  Traits class for eliminateable factor graphs, specifies the types that │ │ │ │ │ - result from elimination, etc. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_ _F_A_C_T_O_R___G_R_A_P_H_ _> │ │ │ │ │ -  _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h is a base class for factor graphs that │ │ │ │ │ - contains elimination algorithms. _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_ _D_E_R_I_V_E_D_,_ _F_A_C_T_O_R_G_R_A_P_H_ _> │ │ │ │ │ +  This is the base class for _B_a_y_e_s_T_r_e_e cliques. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Variable elimination algorithms for factor graphs. │ │ │ │ │ +Base class for cliques of a BayesTree. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Date │ │ │ │ │ - Apr 21, 2013 │ │ │ │ │ + Richard Roberts and Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ + * _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00602.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,3 @@ │ │ │ │ │ var a00602 = [ │ │ │ │ │ - ["gtsam::EliminationTraits< GRAPH >", "a03548.html", null], │ │ │ │ │ - ["gtsam::EliminateableFactorGraph< FACTOR_GRAPH >", "a03580.html", "a03580"] │ │ │ │ │ + ["gtsam::BayesTreeCliqueBase< DERIVED, FACTORGRAPH >", "a03552.html", "a03552"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00602_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/EliminateableFactorGraph.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTreeCliqueBase.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,250 +98,226 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
EliminateableFactorGraph.h
│ │ │ │ +
BayesTreeCliqueBase.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <boost/shared_ptr.hpp>
│ │ │ │ -
22#include <functional>
│ │ │ │ -
23#include <boost/variant.hpp>
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/inference/Key.h>
│ │ │ │ +
21#include <gtsam/inference/Ordering.h>
│ │ │ │ +
22#include <gtsam/base/types.h>
│ │ │ │ +
23#include <gtsam/base/FastVector.h>
│ │ │ │
24#include <boost/optional.hpp>
│ │ │ │
25
│ │ │ │ -
26#include <gtsam/inference/Ordering.h>
│ │ │ │ -
27#include <gtsam/inference/VariableIndex.h>
│ │ │ │ +
26#include <string>
│ │ │ │ +
27#include <mutex>
│ │ │ │
28
│ │ │ │
29namespace gtsam {
│ │ │ │
30
│ │ │ │ -
34 template<class GRAPH>
│ │ │ │ -
│ │ │ │ -
35 struct EliminationTraits
│ │ │ │ -
36 {
│ │ │ │ -
37 // Template for deriving:
│ │ │ │ -
38 // typedef MyFactor FactorType; ///< Type of factors in factor graph (e.g. GaussianFactor)
│ │ │ │ -
39 // typedef MyFactorGraphType FactorGraphType; ///< Type of the factor graph (e.g. GaussianFactorGraph)
│ │ │ │ -
40 // typedef MyConditional ConditionalType; ///< Type of conditionals from elimination (e.g. GaussianConditional)
│ │ │ │ -
41 // typedef MyBayesNet BayesNetType; ///< Type of Bayes net from sequential elimination (e.g. GaussianBayesNet)
│ │ │ │ -
42 // typedef MyEliminationTree EliminationTreeType; ///< Type of elimination tree (e.g. GaussianEliminationTree)
│ │ │ │ -
43 // typedef MyBayesTree BayesTreeType; ///< Type of Bayes tree (e.g. GaussianBayesTree)
│ │ │ │ -
44 // typedef MyJunctionTree JunctionTreeType; ///< Type of Junction tree (e.g. GaussianJunctionTree)
│ │ │ │ -
45 // static pair<shared_ptr<ConditionalType>, shared_ptr<FactorType>
│ │ │ │ -
46 // DefaultEliminate(
│ │ │ │ -
47 // const MyFactorGraph& factors, const Ordering& keys); ///< The default dense elimination function
│ │ │ │ -
48 };
│ │ │ │ -
│ │ │ │ -
49
│ │ │ │ -
50
│ │ │ │ -
55 template<class FACTOR_GRAPH>
│ │ │ │ -
│ │ │ │ -
56 class EliminateableFactorGraph
│ │ │ │ -
57 {
│ │ │ │ -
58 private:
│ │ │ │ -
59 typedef EliminateableFactorGraph<FACTOR_GRAPH> This;
│ │ │ │ -
60 typedef FACTOR_GRAPH FactorGraphType;
│ │ │ │ -
61 // Base factor type stored in this graph (private because derived classes will get this from
│ │ │ │ -
62 // their FactorGraph base class)
│ │ │ │ -
63 typedef typename EliminationTraits<FactorGraphType>::FactorType _FactorType;
│ │ │ │ -
64
│ │ │ │ -
65 public:
│ │ │ │ -
67 typedef EliminationTraits<FactorGraphType> EliminationTraitsType;
│ │ │ │ -
68
│ │ │ │ -
70 typedef typename EliminationTraitsType::ConditionalType ConditionalType;
│ │ │ │ -
71
│ │ │ │ -
73 typedef typename EliminationTraitsType::BayesNetType BayesNetType;
│ │ │ │ -
74
│ │ │ │ -
76 typedef typename EliminationTraitsType::EliminationTreeType EliminationTreeType;
│ │ │ │ -
77
│ │ │ │ -
79 typedef typename EliminationTraitsType::BayesTreeType BayesTreeType;
│ │ │ │ +
31 // Forward declarations
│ │ │ │ +
32 template<class CLIQUE> class BayesTree;
│ │ │ │ +
33 template<class GRAPH> struct EliminationTraits;
│ │ │ │ +
34
│ │ │ │ +
48 template<class DERIVED, class FACTORGRAPH>
│ │ │ │ +
│ │ │ │ +
49 class BayesTreeCliqueBase
│ │ │ │ +
50 {
│ │ │ │ +
51 private:
│ │ │ │ +
52 typedef BayesTreeCliqueBase<DERIVED, FACTORGRAPH> This;
│ │ │ │ +
53 typedef DERIVED DerivedType;
│ │ │ │ +
54 typedef EliminationTraits<FACTORGRAPH> EliminationTraitsType;
│ │ │ │ +
55 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
56 typedef boost::weak_ptr<This> weak_ptr;
│ │ │ │ +
57 typedef boost::shared_ptr<DerivedType> derived_ptr;
│ │ │ │ +
58 typedef boost::weak_ptr<DerivedType> derived_weak_ptr;
│ │ │ │ +
59
│ │ │ │ +
60 public:
│ │ │ │ +
61 typedef FACTORGRAPH FactorGraphType;
│ │ │ │ +
62 typedef typename EliminationTraitsType::BayesNetType BayesNetType;
│ │ │ │ +
63 typedef typename BayesNetType::ConditionalType ConditionalType;
│ │ │ │ +
64 typedef boost::shared_ptr<ConditionalType> sharedConditional;
│ │ │ │ +
65 typedef typename FactorGraphType::FactorType FactorType;
│ │ │ │ +
66 typedef typename FactorGraphType::Eliminate Eliminate;
│ │ │ │ +
67
│ │ │ │ +
68 protected:
│ │ │ │ +
69
│ │ │ │ +
72
│ │ │ │ +
74 BayesTreeCliqueBase() : problemSize_(1) {}
│ │ │ │ +
75
│ │ │ │ +
│ │ │ │ +
78 BayesTreeCliqueBase(const sharedConditional& conditional)
│ │ │ │ +
79 : conditional_(conditional), problemSize_(1) {}
│ │ │ │ +
│ │ │ │
80
│ │ │ │ -
82 typedef typename EliminationTraitsType::JunctionTreeType JunctionTreeType;
│ │ │ │ -
83
│ │ │ │ -
86 typedef std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<_FactorType> > EliminationResult;
│ │ │ │ -
87
│ │ │ │ -
89 typedef std::function<EliminationResult(const FactorGraphType&, const Ordering&)> Eliminate;
│ │ │ │ -
90
│ │ │ │ -
92 typedef boost::optional<const VariableIndex&> OptionalVariableIndex;
│ │ │ │ -
93
│ │ │ │ -
95 typedef boost::optional<Ordering::OrderingType> OptionalOrderingType;
│ │ │ │ -
96
│ │ │ │ -
117 boost::shared_ptr<BayesNetType> eliminateSequential(
│ │ │ │ -
118 OptionalOrderingType orderingType = boost::none,
│ │ │ │ -
119 const Eliminate& function = EliminationTraitsType::DefaultEliminate,
│ │ │ │ -
120 OptionalVariableIndex variableIndex = boost::none) const;
│ │ │ │ -
121
│ │ │ │ -
136 boost::shared_ptr<BayesNetType> eliminateSequential(
│ │ │ │ -
137 const Ordering& ordering,
│ │ │ │ -
138 const Eliminate& function = EliminationTraitsType::DefaultEliminate,
│ │ │ │ -
139 OptionalVariableIndex variableIndex = boost::none) const;
│ │ │ │ -
140
│ │ │ │ -
157 boost::shared_ptr<BayesTreeType> eliminateMultifrontal(
│ │ │ │ -
158 OptionalOrderingType orderingType = boost::none,
│ │ │ │ -
159 const Eliminate& function = EliminationTraitsType::DefaultEliminate,
│ │ │ │ -
160 OptionalVariableIndex variableIndex = boost::none) const;
│ │ │ │ -
161
│ │ │ │ -
171 boost::shared_ptr<BayesTreeType> eliminateMultifrontal(
│ │ │ │ -
172 const Ordering& ordering,
│ │ │ │ -
173 const Eliminate& function = EliminationTraitsType::DefaultEliminate,
│ │ │ │ -
174 OptionalVariableIndex variableIndex = boost::none) const;
│ │ │ │ -
175
│ │ │ │ -
180 std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> >
│ │ │ │ -
181 eliminatePartialSequential(
│ │ │ │ -
182 const Ordering& ordering,
│ │ │ │ -
183 const Eliminate& function = EliminationTraitsType::DefaultEliminate,
│ │ │ │ -
184 OptionalVariableIndex variableIndex = boost::none) const;
│ │ │ │ -
185
│ │ │ │ -
190 std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> >
│ │ │ │ -
191 eliminatePartialSequential(
│ │ │ │ -
192 const KeyVector& variables,
│ │ │ │ -
193 const Eliminate& function = EliminationTraitsType::DefaultEliminate,
│ │ │ │ -
194 OptionalVariableIndex variableIndex = boost::none) const;
│ │ │ │ -
195
│ │ │ │ -
200 std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> >
│ │ │ │ -
201 eliminatePartialMultifrontal(
│ │ │ │ -
202 const Ordering& ordering,
│ │ │ │ -
203 const Eliminate& function = EliminationTraitsType::DefaultEliminate,
│ │ │ │ -
204 OptionalVariableIndex variableIndex = boost::none) const;
│ │ │ │ +
│ │ │ │ +
82 BayesTreeCliqueBase(const BayesTreeCliqueBase& c)
│ │ │ │ +
83 : conditional_(c.conditional_),
│ │ │ │ +
84 parent_(c.parent_),
│ │ │ │ +
85 children(c.children),
│ │ │ │ +
86 problemSize_(c.problemSize_),
│ │ │ │ +
87 is_root(c.is_root) {}
│ │ │ │ +
│ │ │ │ +
88
│ │ │ │ +
│ │ │ │ +
90 BayesTreeCliqueBase& operator=(const BayesTreeCliqueBase& c) {
│ │ │ │ +
91 conditional_ = c.conditional_;
│ │ │ │ +
92 parent_ = c.parent_;
│ │ │ │ +
93 children = c.children;
│ │ │ │ +
94 problemSize_ = c.problemSize_;
│ │ │ │ +
95 is_root = c.is_root;
│ │ │ │ +
96 return *this;
│ │ │ │ +
97 }
│ │ │ │ +
│ │ │ │ +
98
│ │ │ │ +
99 // Virtual destructor.
│ │ │ │ +
100 virtual ~BayesTreeCliqueBase() {}
│ │ │ │ +
101
│ │ │ │ +
103
│ │ │ │ +
105 mutable boost::optional<FactorGraphType> cachedSeparatorMarginal_;
│ │ │ │ +
110 mutable std::mutex cachedSeparatorMarginalMutex_;
│ │ │ │ +
111
│ │ │ │ +
112 public:
│ │ │ │ +
113 sharedConditional conditional_;
│ │ │ │ +
114 derived_weak_ptr parent_;
│ │ │ │ +
115 FastVector<derived_ptr> children;
│ │ │ │ +
116 int problemSize_;
│ │ │ │ +
117
│ │ │ │ +
118 bool is_root = false;
│ │ │ │ +
119
│ │ │ │ +
123 void setEliminationResult(const typename FactorGraphType::EliminationResult& eliminationResult);
│ │ │ │ +
124
│ │ │ │ +
127
│ │ │ │ +
129 bool equals(const DERIVED& other, double tol = 1e-9) const;
│ │ │ │ +
130
│ │ │ │ +
132 virtual void print(
│ │ │ │ +
133 const std::string& s = "",
│ │ │ │ +
134 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ +
135
│ │ │ │ +
139
│ │ │ │ +
141 const sharedConditional& conditional() const { return conditional_; }
│ │ │ │ +
142
│ │ │ │ +
144 inline bool isRoot() const { return parent_.expired(); }
│ │ │ │ +
145
│ │ │ │ +
147 size_t nrChildren() const { return children.size(); }
│ │ │ │ +
148
│ │ │ │ +
150 const derived_ptr operator[](size_t i) const { return children[i]; }
│ │ │ │ +
151
│ │ │ │ +
153 size_t treeSize() const;
│ │ │ │ +
154
│ │ │ │ +
156 size_t numCachedSeparatorMarginals() const;
│ │ │ │ +
157
│ │ │ │ +
159 derived_ptr parent() const { return parent_.lock(); }
│ │ │ │ +
160
│ │ │ │ +
162 int problemSize() const { return problemSize_; }
│ │ │ │ +
163
│ │ │ │ +
167
│ │ │ │ +
169 BayesNetType shortcut(const derived_ptr& root, Eliminate function = EliminationTraitsType::DefaultEliminate) const;
│ │ │ │ +
170
│ │ │ │ +
172 FactorGraphType separatorMarginal(Eliminate function = EliminationTraitsType::DefaultEliminate) const;
│ │ │ │ +
173
│ │ │ │ +
175 FactorGraphType marginal2(Eliminate function = EliminationTraitsType::DefaultEliminate) const;
│ │ │ │ +
176
│ │ │ │ +
181 void deleteCachedShortcuts();
│ │ │ │ +
182
│ │ │ │ +
183 const boost::optional<FactorGraphType>& cachedSeparatorMarginal() const {
│ │ │ │ +
184 std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
│ │ │ │ +
185 return cachedSeparatorMarginal_;
│ │ │ │ +
186 }
│ │ │ │ +
187
│ │ │ │ +
188 friend class BayesTree<DerivedType>;
│ │ │ │ +
189
│ │ │ │ +
190 protected:
│ │ │ │ +
191
│ │ │ │ +
193 KeyVector separator_setminus_B(const derived_ptr& B) const;
│ │ │ │ +
194
│ │ │ │ +
198 KeyVector shortcut_indices(const derived_ptr& B, const FactorGraphType& p_Cp_B) const;
│ │ │ │ +
199
│ │ │ │ +
│ │ │ │ +
201 void deleteCachedShortcutsNonRecursive() {
│ │ │ │ +
202 std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
│ │ │ │ +
203 cachedSeparatorMarginal_ = boost::none;
│ │ │ │ +
204 }
│ │ │ │ +
│ │ │ │
205
│ │ │ │ -
210 std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> >
│ │ │ │ -
211 eliminatePartialMultifrontal(
│ │ │ │ -
212 const KeyVector& variables,
│ │ │ │ -
213 const Eliminate& function = EliminationTraitsType::DefaultEliminate,
│ │ │ │ -
214 OptionalVariableIndex variableIndex = boost::none) const;
│ │ │ │ -
215
│ │ │ │ -
225 boost::shared_ptr<BayesNetType> marginalMultifrontalBayesNet(
│ │ │ │ -
226 boost::variant<const Ordering&, const KeyVector&> variables,
│ │ │ │ -
227 const Eliminate& function = EliminationTraitsType::DefaultEliminate,
│ │ │ │ -
228 OptionalVariableIndex variableIndex = boost::none) const;
│ │ │ │ -
229
│ │ │ │ -
240 boost::shared_ptr<BayesNetType> marginalMultifrontalBayesNet(
│ │ │ │ -
241 boost::variant<const Ordering&, const KeyVector&> variables,
│ │ │ │ -
242 const Ordering& marginalizedVariableOrdering,
│ │ │ │ -
243 const Eliminate& function = EliminationTraitsType::DefaultEliminate,
│ │ │ │ -
244 OptionalVariableIndex variableIndex = boost::none) const;
│ │ │ │ -
245
│ │ │ │ -
255 boost::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
│ │ │ │ -
256 boost::variant<const Ordering&, const KeyVector&> variables,
│ │ │ │ -
257 const Eliminate& function = EliminationTraitsType::DefaultEliminate,
│ │ │ │ -
258 OptionalVariableIndex variableIndex = boost::none) const;
│ │ │ │ -
259
│ │ │ │ -
270 boost::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree(
│ │ │ │ -
271 boost::variant<const Ordering&, const KeyVector&> variables,
│ │ │ │ -
272 const Ordering& marginalizedVariableOrdering,
│ │ │ │ -
273 const Eliminate& function = EliminationTraitsType::DefaultEliminate,
│ │ │ │ -
274 OptionalVariableIndex variableIndex = boost::none) const;
│ │ │ │ -
275
│ │ │ │ -
277 boost::shared_ptr<FactorGraphType> marginal(
│ │ │ │ -
278 const KeyVector& variables,
│ │ │ │ -
279 const Eliminate& function = EliminationTraitsType::DefaultEliminate,
│ │ │ │ -
280 OptionalVariableIndex variableIndex = boost::none) const;
│ │ │ │ -
281
│ │ │ │ -
282 private:
│ │ │ │ -
283
│ │ │ │ -
284 // Access the derived factor graph class
│ │ │ │ -
285 const FactorGraphType& asDerived() const { return static_cast<const FactorGraphType&>(*this); }
│ │ │ │ -
286
│ │ │ │ -
287 // Access the derived factor graph class
│ │ │ │ -
288 FactorGraphType& asDerived() { return static_cast<FactorGraphType&>(*this); }
│ │ │ │ -
289
│ │ │ │ -
290 public:
│ │ │ │ -
291 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ -
293 boost::shared_ptr<BayesNetType> GTSAM_DEPRECATED eliminateSequential(
│ │ │ │ -
294 const Ordering& ordering,
│ │ │ │ -
295 const Eliminate& function,
│ │ │ │ -
296 OptionalVariableIndex variableIndex,
│ │ │ │ -
297 OptionalOrderingType orderingType) const {
│ │ │ │ -
298 return eliminateSequential(ordering, function, variableIndex);
│ │ │ │ -
299 }
│ │ │ │ -
300
│ │ │ │ -
302 boost::shared_ptr<BayesNetType> GTSAM_DEPRECATED eliminateSequential(
│ │ │ │ -
303 const Eliminate& function,
│ │ │ │ -
304 OptionalVariableIndex variableIndex = boost::none,
│ │ │ │ -
305 OptionalOrderingType orderingType = boost::none) const {
│ │ │ │ -
306 return eliminateSequential(orderingType, function, variableIndex);
│ │ │ │ -
307 }
│ │ │ │ -
308
│ │ │ │ -
310 boost::shared_ptr<BayesTreeType> GTSAM_DEPRECATED eliminateMultifrontal(
│ │ │ │ -
311 const Ordering& ordering,
│ │ │ │ -
312 const Eliminate& function,
│ │ │ │ -
313 OptionalVariableIndex variableIndex,
│ │ │ │ -
314 OptionalOrderingType orderingType) const {
│ │ │ │ -
315 return eliminateMultifrontal(ordering, function, variableIndex);
│ │ │ │ -
316 }
│ │ │ │ -
317
│ │ │ │ -
319 boost::shared_ptr<BayesTreeType> GTSAM_DEPRECATED eliminateMultifrontal(
│ │ │ │ -
320 const Eliminate& function,
│ │ │ │ -
321 OptionalVariableIndex variableIndex = boost::none,
│ │ │ │ -
322 OptionalOrderingType orderingType = boost::none) const {
│ │ │ │ -
323 return eliminateMultifrontal(orderingType, function, variableIndex);
│ │ │ │ -
324 }
│ │ │ │ -
325
│ │ │ │ -
327 boost::shared_ptr<BayesNetType> GTSAM_DEPRECATED marginalMultifrontalBayesNet(
│ │ │ │ -
328 boost::variant<const Ordering&, const KeyVector&> variables,
│ │ │ │ -
329 boost::none_t,
│ │ │ │ -
330 const Eliminate& function = EliminationTraitsType::DefaultEliminate,
│ │ │ │ -
331 OptionalVariableIndex variableIndex = boost::none) const {
│ │ │ │ -
332 return marginalMultifrontalBayesNet(variables, function, variableIndex);
│ │ │ │ -
333 }
│ │ │ │ -
334
│ │ │ │ -
336 boost::shared_ptr<BayesTreeType> GTSAM_DEPRECATED marginalMultifrontalBayesTree(
│ │ │ │ -
337 boost::variant<const Ordering&, const KeyVector&> variables,
│ │ │ │ -
338 boost::none_t,
│ │ │ │ -
339 const Eliminate& function = EliminationTraitsType::DefaultEliminate,
│ │ │ │ -
340 OptionalVariableIndex variableIndex = boost::none) const {
│ │ │ │ -
341 return marginalMultifrontalBayesTree(variables, function, variableIndex);
│ │ │ │ -
342 }
│ │ │ │ -
343 #endif
│ │ │ │ -
344 };
│ │ │ │ -
│ │ │ │ -
345
│ │ │ │ -
346}
│ │ │ │ -
Ordering.h
Variable ordering for the elimination algorithm.
│ │ │ │ - │ │ │ │ +
206 private:
│ │ │ │ +
207
│ │ │ │ +
209 friend class boost::serialization::access;
│ │ │ │ +
210 template<class ARCHIVE>
│ │ │ │ +
211 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
212 if(!parent_.lock()) {
│ │ │ │ +
213 is_root = true;
│ │ │ │ +
214 }
│ │ │ │ +
215 ar & BOOST_SERIALIZATION_NVP(is_root);
│ │ │ │ +
216 ar & BOOST_SERIALIZATION_NVP(conditional_);
│ │ │ │ +
217 if (!is_root) { // TODO(fan): Workaround for boost/serialization #119
│ │ │ │ +
218 ar & BOOST_SERIALIZATION_NVP(parent_);
│ │ │ │ +
219 }
│ │ │ │ +
220 ar & BOOST_SERIALIZATION_NVP(children);
│ │ │ │ +
221 }
│ │ │ │ +
222
│ │ │ │ +
224
│ │ │ │ +
225 };
│ │ │ │ +
│ │ │ │ +
226
│ │ │ │ +
227}
│ │ │ │ +
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ +
types.h
Typedefs for easier changing of types.
│ │ │ │ +
Ordering.h
Variable ordering for the elimination algorithm.
│ │ │ │ + │ │ │ │ +
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │
gtsam::EliminationTraits
Traits class for eliminateable factor graphs, specifies the types that result from elimination,...
Definition EliminateableFactorGraph.h:36
│ │ │ │ -
gtsam::EliminateableFactorGraph
EliminateableFactorGraph is a base class for factor graphs that contains elimination algorithms.
Definition EliminateableFactorGraph.h:57
│ │ │ │ -
gtsam::EliminateableFactorGraph::marginalMultifrontalBayesTree
boost::shared_ptr< BayesTreeType > marginalMultifrontalBayesTree(boost::variant< const Ordering &, const KeyVector & > variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Compute the marginal of the requested variables and return the result as a Bayes tree.
Definition EliminateableFactorGraph-inst.h:300
│ │ │ │ -
gtsam::EliminateableFactorGraph::JunctionTreeType
EliminationTraitsType::JunctionTreeType JunctionTreeType
Junction tree type that can do multifrontal elimination of this graph.
Definition EliminateableFactorGraph.h:82
│ │ │ │ -
gtsam::EliminateableFactorGraph::marginal
boost::shared_ptr< FactorGraphType > marginal(const KeyVector &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Compute the marginal factor graph of the requested variables.
Definition EliminateableFactorGraph-inst.h:367
│ │ │ │ -
gtsam::EliminateableFactorGraph::Eliminate
std::function< EliminationResult(const FactorGraphType &, const Ordering &)> Eliminate
The function type that does a single dense elimination step on a subgraph.
Definition EliminateableFactorGraph.h:89
│ │ │ │ -
gtsam::EliminateableFactorGraph::BayesTreeType
EliminationTraitsType::BayesTreeType BayesTreeType
Bayes tree type produced by multifrontal elimination.
Definition EliminateableFactorGraph.h:79
│ │ │ │ -
gtsam::EliminateableFactorGraph::BayesNetType
EliminationTraitsType::BayesNetType BayesNetType
Bayes net type produced by sequential elimination.
Definition EliminateableFactorGraph.h:73
│ │ │ │ -
gtsam::EliminateableFactorGraph::EliminationResult
std::pair< boost::shared_ptr< ConditionalType >, boost::shared_ptr< _FactorType > > EliminationResult
The pair of conditional and remaining factor produced by a single dense elimination step on a subgrap...
Definition EliminateableFactorGraph.h:86
│ │ │ │ -
gtsam::EliminateableFactorGraph::eliminateSequential
boost::shared_ptr< BayesNetType > eliminateSequential(OptionalOrderingType orderingType=boost::none, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Do sequential elimination of all variables to produce a Bayes net.
Definition EliminateableFactorGraph-inst.h:30
│ │ │ │ -
gtsam::EliminateableFactorGraph::eliminateMultifrontal
boost::shared_ptr< BayesTreeType > eliminateMultifrontal(OptionalOrderingType orderingType=boost::none, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Do multifrontal elimination of all variables to produce a Bayes tree.
Definition EliminateableFactorGraph-inst.h:91
│ │ │ │ -
gtsam::EliminateableFactorGraph::ConditionalType
EliminationTraitsType::ConditionalType ConditionalType
Conditional type stored in the Bayes net produced by elimination.
Definition EliminateableFactorGraph.h:70
│ │ │ │ -
gtsam::EliminateableFactorGraph::EliminationTreeType
EliminationTraitsType::EliminationTreeType EliminationTreeType
Elimination tree type that can do sequential elimination of this graph.
Definition EliminateableFactorGraph.h:76
│ │ │ │ -
gtsam::EliminateableFactorGraph::OptionalVariableIndex
boost::optional< const VariableIndex & > OptionalVariableIndex
Typedef for an optional variable index as an argument to elimination functions.
Definition EliminateableFactorGraph.h:92
│ │ │ │ -
gtsam::EliminateableFactorGraph::eliminatePartialSequential
std::pair< boost::shared_ptr< BayesNetType >, boost::shared_ptr< FactorGraphType > > eliminatePartialSequential(const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Do sequential elimination of some variables, in ordering provided, to produce a Bayes net and a remai...
Definition EliminateableFactorGraph-inst.h:154
│ │ │ │ -
gtsam::EliminateableFactorGraph::marginalMultifrontalBayesNet
boost::shared_ptr< BayesNetType > marginalMultifrontalBayesNet(boost::variant< const Ordering &, const KeyVector & > variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Compute the marginal of the requested variables and return the result as a Bayes net.
Definition EliminateableFactorGraph-inst.h:233
│ │ │ │ -
gtsam::EliminateableFactorGraph::eliminatePartialMultifrontal
std::pair< boost::shared_ptr< BayesTreeType >, boost::shared_ptr< FactorGraphType > > eliminatePartialMultifrontal(const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Do multifrontal elimination of some variables, in ordering provided, to produce a Bayes tree and a re...
Definition EliminateableFactorGraph-inst.h:193
│ │ │ │ -
gtsam::EliminateableFactorGraph::EliminationTraitsType
EliminationTraits< FactorGraphType > EliminationTraitsType
Typedef to the specific EliminationTraits for this graph.
Definition EliminateableFactorGraph.h:67
│ │ │ │ -
gtsam::EliminateableFactorGraph::OptionalOrderingType
boost::optional< Ordering::OrderingType > OptionalOrderingType
Typedef for an optional ordering type.
Definition EliminateableFactorGraph.h:95
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
gtsam::BayesTreeCliqueBase
This is the base class for BayesTree cliques.
Definition BayesTreeCliqueBase.h:50
│ │ │ │ +
gtsam::BayesTreeCliqueBase::operator[]
const derived_ptr operator[](size_t i) const
Return the child at index i.
Definition BayesTreeCliqueBase.h:150
│ │ │ │ +
gtsam::BayesTreeCliqueBase::operator=
BayesTreeCliqueBase & operator=(const BayesTreeCliqueBase &c)
Shallow copy assignment constructor.
Definition BayesTreeCliqueBase.h:90
│ │ │ │ +
gtsam::BayesTreeCliqueBase::deleteCachedShortcutsNonRecursive
void deleteCachedShortcutsNonRecursive()
Non-recursive delete cached shortcuts and marginals - internal only.
Definition BayesTreeCliqueBase.h:201
│ │ │ │ +
gtsam::BayesTreeCliqueBase::nrChildren
size_t nrChildren() const
Return the number of children.
Definition BayesTreeCliqueBase.h:147
│ │ │ │ +
gtsam::BayesTreeCliqueBase::problemSize
int problemSize() const
Problem size (used for parallel traversal)
Definition BayesTreeCliqueBase.h:162
│ │ │ │ +
gtsam::BayesTreeCliqueBase::BayesTreeCliqueBase
BayesTreeCliqueBase()
Default constructor.
Definition BayesTreeCliqueBase.h:74
│ │ │ │ +
gtsam::BayesTreeCliqueBase::parent
derived_ptr parent() const
return a shared_ptr to the parent clique
Definition BayesTreeCliqueBase.h:159
│ │ │ │ +
gtsam::BayesTreeCliqueBase::treeSize
size_t treeSize() const
The size of subtree rooted at this clique, i.e., nr of Cliques.
Definition BayesTreeCliqueBase-inst.h:84
│ │ │ │ +
gtsam::BayesTreeCliqueBase::BayesTreeCliqueBase
BayesTreeCliqueBase(const sharedConditional &conditional)
Construct from a conditional, leaving parent and child pointers uninitialized.
Definition BayesTreeCliqueBase.h:78
│ │ │ │ +
gtsam::BayesTreeCliqueBase::cachedSeparatorMarginalMutex_
std::mutex cachedSeparatorMarginalMutex_
This protects Cached seperator marginal P(S) from concurrent read/writes as many the functions which ...
Definition BayesTreeCliqueBase.h:110
│ │ │ │ +
gtsam::BayesTreeCliqueBase::isRoot
bool isRoot() const
Return true if this clique is the root of a Bayes tree.
Definition BayesTreeCliqueBase.h:144
│ │ │ │ +
gtsam::BayesTreeCliqueBase::BayesTreeCliqueBase
BayesTreeCliqueBase(const BayesTreeCliqueBase &c)
Shallow copy constructor.
Definition BayesTreeCliqueBase.h:82
│ │ │ │ +
gtsam::BayesTreeCliqueBase::marginal2
FactorGraphType marginal2(Eliminate function=EliminationTraitsType::DefaultEliminate) const
return the marginal P(C) of the clique, using marginal caching
Definition BayesTreeCliqueBase-inst.h:195
│ │ │ │ +
gtsam::BayesTreeCliqueBase::separatorMarginal
FactorGraphType separatorMarginal(Eliminate function=EliminationTraitsType::DefaultEliminate) const
return the marginal P(S) on the separator
Definition BayesTreeCliqueBase-inst.h:147
│ │ │ │ +
gtsam::BayesTreeCliqueBase::shortcut
BayesNetType shortcut(const derived_ptr &root, Eliminate function=EliminationTraitsType::DefaultEliminate) const
return the conditional P(S|Root) on the separator given the root
Definition BayesTreeCliqueBase-inst.h:113
│ │ │ │ +
gtsam::BayesTreeCliqueBase::deleteCachedShortcuts
void deleteCachedShortcuts()
This deletes the cached shortcuts of all cliques (subtree) below this clique.
Definition BayesTreeCliqueBase-inst.h:207
│ │ │ │ +
gtsam::BayesTreeCliqueBase::shortcut_indices
KeyVector shortcut_indices(const derived_ptr &B, const FactorGraphType &p_Cp_B) const
Determine variable indices to keep in recursive separator shortcut calculation The factor graph p_Cp_...
Definition BayesTreeCliqueBase-inst.h:57
│ │ │ │ +
gtsam::BayesTreeCliqueBase::conditional
const sharedConditional & conditional() const
Access the conditional.
Definition BayesTreeCliqueBase.h:141
│ │ │ │ +
gtsam::BayesTreeCliqueBase::access
friend class boost::serialization::access
Serialization function.
Definition BayesTreeCliqueBase.h:209
│ │ │ │ +
gtsam::BayesTreeCliqueBase::separator_setminus_B
KeyVector separator_setminus_B(const derived_ptr &B) const
Calculate set for shortcut calculations.
Definition BayesTreeCliqueBase-inst.h:45
│ │ │ │ +
gtsam::BayesTreeCliqueBase::print
virtual void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print this node
Definition BayesTreeCliqueBase-inst.h:76
│ │ │ │ +
gtsam::BayesTreeCliqueBase::numCachedSeparatorMarginals
size_t numCachedSeparatorMarginals() const
Collect number of cliques with cached separator marginals.
Definition BayesTreeCliqueBase-inst.h:93
│ │ │ │ +
gtsam::BayesTreeCliqueBase::cachedSeparatorMarginal_
boost::optional< FactorGraphType > cachedSeparatorMarginal_
This stores the Cached separator marginal P(S)
Definition BayesTreeCliqueBase.h:105
│ │ │ │ +
gtsam::BayesTreeCliqueBase::setEliminationResult
void setEliminationResult(const typename FactorGraphType::EliminationResult &eliminationResult)
Fill the elimination result produced during elimination.
Definition BayesTreeCliqueBase-inst.h:27
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,347 +1,315 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -EliminateableFactorGraph.h │ │ │ │ │ +BayesTreeCliqueBase.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include │ │ │ │ │ -22#include │ │ │ │ │ -23#include │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_O_r_d_e_r_i_n_g_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ 24#include │ │ │ │ │ 25 │ │ │ │ │ -26#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_O_r_d_e_r_i_n_g_._h> │ │ │ │ │ -27#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_V_a_r_i_a_b_l_e_I_n_d_e_x_._h> │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ 28 │ │ │ │ │ 29namespace _g_t_s_a_m { │ │ │ │ │ 30 │ │ │ │ │ -34 template │ │ │ │ │ -_3_5 struct _E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s │ │ │ │ │ -36 { │ │ │ │ │ -37 // Template for deriving: │ │ │ │ │ -38 // typedef MyFactor FactorType; ///< Type of factors in factor graph (e.g. │ │ │ │ │ -GaussianFactor) │ │ │ │ │ -39 // typedef MyFactorGraphType FactorGraphType; ///< Type of the factor graph │ │ │ │ │ -(e.g. GaussianFactorGraph) │ │ │ │ │ -40 // typedef MyConditional ConditionalType; ///< Type of conditionals from │ │ │ │ │ -elimination (e.g. GaussianConditional) │ │ │ │ │ -41 // typedef MyBayesNet BayesNetType; ///< Type of Bayes net from sequential │ │ │ │ │ -elimination (e.g. GaussianBayesNet) │ │ │ │ │ -42 // typedef MyEliminationTree EliminationTreeType; ///< Type of elimination │ │ │ │ │ -tree (e.g. GaussianEliminationTree) │ │ │ │ │ -43 // typedef MyBayesTree BayesTreeType; ///< Type of Bayes tree (e.g. │ │ │ │ │ -GaussianBayesTree) │ │ │ │ │ -44 // typedef MyJunctionTree JunctionTreeType; ///< Type of Junction tree (e.g. │ │ │ │ │ -GaussianJunctionTree) │ │ │ │ │ -45 // static pair, shared_ptr │ │ │ │ │ -46 // DefaultEliminate( │ │ │ │ │ -47 // const MyFactorGraph& factors, const Ordering& keys); ///< The default │ │ │ │ │ -dense elimination function │ │ │ │ │ -48 }; │ │ │ │ │ -49 │ │ │ │ │ -50 │ │ │ │ │ -55 template │ │ │ │ │ -_5_6 class _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -57 { │ │ │ │ │ -58 private: │ │ │ │ │ -59 typedef _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R___G_R_A_P_H_> This; │ │ │ │ │ -60 typedef FACTOR_GRAPH FactorGraphType; │ │ │ │ │ -61 // Base factor type stored in this graph (private because derived classes │ │ │ │ │ -will get this from │ │ │ │ │ -62 // their FactorGraph base class) │ │ │ │ │ -63 typedef typename _E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_F_a_c_t_o_r_G_r_a_p_h_T_y_p_e_>_:_:_F_a_c_t_o_r_T_y_p_e _FactorType; │ │ │ │ │ -64 │ │ │ │ │ -65 public: │ │ │ │ │ -_6_7 typedef _E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_F_a_c_t_o_r_G_r_a_p_h_T_y_p_e_> _E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_T_y_p_e; │ │ │ │ │ -68 │ │ │ │ │ -_7_0 typedef typename EliminationTraitsType::ConditionalType _C_o_n_d_i_t_i_o_n_a_l_T_y_p_e; │ │ │ │ │ -71 │ │ │ │ │ -_7_3 typedef typename EliminationTraitsType::BayesNetType _B_a_y_e_s_N_e_t_T_y_p_e; │ │ │ │ │ -74 │ │ │ │ │ -_7_6 typedef typename EliminationTraitsType::EliminationTreeType │ │ │ │ │ -_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_T_y_p_e; │ │ │ │ │ -77 │ │ │ │ │ -_7_9 typedef typename EliminationTraitsType::BayesTreeType _B_a_y_e_s_T_r_e_e_T_y_p_e; │ │ │ │ │ +31 // Forward declarations │ │ │ │ │ +32 template class BayesTree; │ │ │ │ │ +33 template struct EliminationTraits; │ │ │ │ │ +34 │ │ │ │ │ +48 template │ │ │ │ │ +_4_9 class _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e │ │ │ │ │ +50 { │ │ │ │ │ +51 private: │ │ │ │ │ +52 typedef _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_D_E_R_I_V_E_D_,_ _F_A_C_T_O_R_G_R_A_P_H_> This; │ │ │ │ │ +53 typedef DERIVED DerivedType; │ │ │ │ │ +54 typedef _E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_F_A_C_T_O_R_G_R_A_P_H_> EliminationTraitsType; │ │ │ │ │ +55 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +56 typedef boost::weak_ptr weak_ptr; │ │ │ │ │ +57 typedef boost::shared_ptr derived_ptr; │ │ │ │ │ +58 typedef boost::weak_ptr derived_weak_ptr; │ │ │ │ │ +59 │ │ │ │ │ +60 public: │ │ │ │ │ +61 typedef FACTORGRAPH FactorGraphType; │ │ │ │ │ +62 typedef typename EliminationTraitsType::BayesNetType BayesNetType; │ │ │ │ │ +63 typedef typename BayesNetType::ConditionalType ConditionalType; │ │ │ │ │ +64 typedef boost::shared_ptr sharedConditional; │ │ │ │ │ +65 typedef typename FactorGraphType::FactorType FactorType; │ │ │ │ │ +66 typedef typename FactorGraphType::Eliminate Eliminate; │ │ │ │ │ +67 │ │ │ │ │ +68 protected: │ │ │ │ │ +69 │ │ │ │ │ +72 │ │ │ │ │ +_7_4 _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e() : problemSize_(1) {} │ │ │ │ │ +75 │ │ │ │ │ +_7_8 _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e(const sharedConditional& _c_o_n_d_i_t_i_o_n_a_l) │ │ │ │ │ +79 : conditional_(_c_o_n_d_i_t_i_o_n_a_l), problemSize_(1) {} │ │ │ │ │ 80 │ │ │ │ │ -_8_2 typedef typename EliminationTraitsType::JunctionTreeType _J_u_n_c_t_i_o_n_T_r_e_e_T_y_p_e; │ │ │ │ │ -83 │ │ │ │ │ -_8_6 typedef std::pair, boost:: │ │ │ │ │ -shared_ptr<_FactorType> > _E_l_i_m_i_n_a_t_i_o_n_R_e_s_u_l_t; │ │ │ │ │ -87 │ │ │ │ │ -_8_9 typedef std::function<_E_l_i_m_i_n_a_t_i_o_n_R_e_s_u_l_t(const FactorGraphType&, const │ │ │ │ │ -_O_r_d_e_r_i_n_g&)> _E_l_i_m_i_n_a_t_e; │ │ │ │ │ -90 │ │ │ │ │ -_9_2 typedef boost::optional _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x; │ │ │ │ │ -93 │ │ │ │ │ -_9_5 typedef boost::optional _O_p_t_i_o_n_a_l_O_r_d_e_r_i_n_g_T_y_p_e; │ │ │ │ │ -96 │ │ │ │ │ -117 boost::shared_ptr _e_l_i_m_i_n_a_t_e_S_e_q_u_e_n_t_i_a_l( │ │ │ │ │ -118 _O_p_t_i_o_n_a_l_O_r_d_e_r_i_n_g_T_y_p_e orderingType = boost::none, │ │ │ │ │ -119 const _E_l_i_m_i_n_a_t_e& function = EliminationTraitsType::DefaultEliminate, │ │ │ │ │ -120 _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex = boost::none) const; │ │ │ │ │ -121 │ │ │ │ │ -136 boost::shared_ptr _e_l_i_m_i_n_a_t_e_S_e_q_u_e_n_t_i_a_l( │ │ │ │ │ -137 const _O_r_d_e_r_i_n_g& ordering, │ │ │ │ │ -138 const _E_l_i_m_i_n_a_t_e& function = EliminationTraitsType::DefaultEliminate, │ │ │ │ │ -139 _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex = boost::none) const; │ │ │ │ │ -140 │ │ │ │ │ -157 boost::shared_ptr _e_l_i_m_i_n_a_t_e_M_u_l_t_i_f_r_o_n_t_a_l( │ │ │ │ │ -158 _O_p_t_i_o_n_a_l_O_r_d_e_r_i_n_g_T_y_p_e orderingType = boost::none, │ │ │ │ │ -159 const _E_l_i_m_i_n_a_t_e& function = EliminationTraitsType::DefaultEliminate, │ │ │ │ │ -160 _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex = boost::none) const; │ │ │ │ │ -161 │ │ │ │ │ -171 boost::shared_ptr _e_l_i_m_i_n_a_t_e_M_u_l_t_i_f_r_o_n_t_a_l( │ │ │ │ │ -172 const _O_r_d_e_r_i_n_g& ordering, │ │ │ │ │ -173 const _E_l_i_m_i_n_a_t_e& function = EliminationTraitsType::DefaultEliminate, │ │ │ │ │ -174 _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex = boost::none) const; │ │ │ │ │ -175 │ │ │ │ │ -180 std::pair, boost:: │ │ │ │ │ -shared_ptr > │ │ │ │ │ -181 _e_l_i_m_i_n_a_t_e_P_a_r_t_i_a_l_S_e_q_u_e_n_t_i_a_l( │ │ │ │ │ -182 const _O_r_d_e_r_i_n_g& ordering, │ │ │ │ │ -183 const _E_l_i_m_i_n_a_t_e& function = EliminationTraitsType::DefaultEliminate, │ │ │ │ │ -184 _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex = boost::none) const; │ │ │ │ │ -185 │ │ │ │ │ -190 std::pair, boost:: │ │ │ │ │ -shared_ptr > │ │ │ │ │ -191 _e_l_i_m_i_n_a_t_e_P_a_r_t_i_a_l_S_e_q_u_e_n_t_i_a_l( │ │ │ │ │ -192 const _K_e_y_V_e_c_t_o_r& variables, │ │ │ │ │ -193 const _E_l_i_m_i_n_a_t_e& function = EliminationTraitsType::DefaultEliminate, │ │ │ │ │ -194 _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex = boost::none) const; │ │ │ │ │ -195 │ │ │ │ │ -200 std::pair, boost:: │ │ │ │ │ -shared_ptr > │ │ │ │ │ -201 _e_l_i_m_i_n_a_t_e_P_a_r_t_i_a_l_M_u_l_t_i_f_r_o_n_t_a_l( │ │ │ │ │ -202 const _O_r_d_e_r_i_n_g& ordering, │ │ │ │ │ -203 const _E_l_i_m_i_n_a_t_e& function = EliminationTraitsType::DefaultEliminate, │ │ │ │ │ -204 _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex = boost::none) const; │ │ │ │ │ +_8_2 _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e(const _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e& c) │ │ │ │ │ +83 : conditional_(c.conditional_), │ │ │ │ │ +84 parent_(c.parent_), │ │ │ │ │ +85 children(c.children), │ │ │ │ │ +86 problemSize_(c.problemSize_), │ │ │ │ │ +87 is_root(c.is_root) {} │ │ │ │ │ +88 │ │ │ │ │ +_9_0 _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e& _o_p_e_r_a_t_o_r_=(const _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e& c) { │ │ │ │ │ +91 conditional_ = c.conditional_; │ │ │ │ │ +92 parent_ = c.parent_; │ │ │ │ │ +93 children = c.children; │ │ │ │ │ +94 problemSize_ = c.problemSize_; │ │ │ │ │ +95 is_root = c.is_root; │ │ │ │ │ +96 return *this; │ │ │ │ │ +97 } │ │ │ │ │ +98 │ │ │ │ │ +99 // Virtual destructor. │ │ │ │ │ +100 virtual _~_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e() {} │ │ │ │ │ +101 │ │ │ │ │ +103 │ │ │ │ │ +_1_0_5 mutable boost::optional _c_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l__; │ │ │ │ │ +_1_1_0 mutable std::mutex _c_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l_M_u_t_e_x__; │ │ │ │ │ +111 │ │ │ │ │ +112 public: │ │ │ │ │ +113 sharedConditional conditional_; │ │ │ │ │ +114 derived_weak_ptr parent_; │ │ │ │ │ +115 _F_a_s_t_V_e_c_t_o_r_<_d_e_r_i_v_e_d___p_t_r_> children; │ │ │ │ │ +116 int problemSize_; │ │ │ │ │ +117 │ │ │ │ │ +118 bool is_root = false; │ │ │ │ │ +119 │ │ │ │ │ +123 void _s_e_t_E_l_i_m_i_n_a_t_i_o_n_R_e_s_u_l_t(const typename FactorGraphType:: │ │ │ │ │ +EliminationResult& eliminationResult); │ │ │ │ │ +124 │ │ │ │ │ +127 │ │ │ │ │ +129 bool _e_q_u_a_l_s(const DERIVED& other, double tol = 1e-9) const; │ │ │ │ │ +130 │ │ │ │ │ +132 virtual void _p_r_i_n_t( │ │ │ │ │ +133 const std::string& s = "", │ │ │ │ │ +134 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const; │ │ │ │ │ +135 │ │ │ │ │ +139 │ │ │ │ │ +_1_4_1 const sharedConditional& _c_o_n_d_i_t_i_o_n_a_l() const { return conditional_; } │ │ │ │ │ +142 │ │ │ │ │ +_1_4_4 inline bool _i_s_R_o_o_t() const { return parent_.expired(); } │ │ │ │ │ +145 │ │ │ │ │ +_1_4_7 size_t _n_r_C_h_i_l_d_r_e_n() const { return children.size(); } │ │ │ │ │ +148 │ │ │ │ │ +_1_5_0 const derived_ptr _o_p_e_r_a_t_o_r_[_](size_t i) const { return children[i]; } │ │ │ │ │ +151 │ │ │ │ │ +153 size_t _t_r_e_e_S_i_z_e() const; │ │ │ │ │ +154 │ │ │ │ │ +156 size_t _n_u_m_C_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l_s() const; │ │ │ │ │ +157 │ │ │ │ │ +_1_5_9 derived_ptr _p_a_r_e_n_t() const { return parent_.lock(); } │ │ │ │ │ +160 │ │ │ │ │ +_1_6_2 int _p_r_o_b_l_e_m_S_i_z_e() const { return problemSize_; } │ │ │ │ │ +163 │ │ │ │ │ +167 │ │ │ │ │ +169 BayesNetType _s_h_o_r_t_c_u_t(const derived_ptr& root, Eliminate function = │ │ │ │ │ +EliminationTraitsType::DefaultEliminate) const; │ │ │ │ │ +170 │ │ │ │ │ +172 FactorGraphType _s_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l(Eliminate function = │ │ │ │ │ +EliminationTraitsType::DefaultEliminate) const; │ │ │ │ │ +173 │ │ │ │ │ +175 FactorGraphType _m_a_r_g_i_n_a_l_2(Eliminate function = EliminationTraitsType:: │ │ │ │ │ +DefaultEliminate) const; │ │ │ │ │ +176 │ │ │ │ │ +181 void _d_e_l_e_t_e_C_a_c_h_e_d_S_h_o_r_t_c_u_t_s(); │ │ │ │ │ +182 │ │ │ │ │ +183 const boost::optional& cachedSeparatorMarginal() const { │ │ │ │ │ +184 std::lock_guard marginalLock(_c_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l_M_u_t_e_x__); │ │ │ │ │ +185 return _c_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l__; │ │ │ │ │ +186 } │ │ │ │ │ +187 │ │ │ │ │ +188 friend class BayesTree; │ │ │ │ │ +189 │ │ │ │ │ +190 protected: │ │ │ │ │ +191 │ │ │ │ │ +193 _K_e_y_V_e_c_t_o_r _s_e_p_a_r_a_t_o_r___s_e_t_m_i_n_u_s___B(const derived_ptr& B) const; │ │ │ │ │ +194 │ │ │ │ │ +198 _K_e_y_V_e_c_t_o_r _s_h_o_r_t_c_u_t___i_n_d_i_c_e_s(const derived_ptr& B, const FactorGraphType& │ │ │ │ │ +p_Cp_B) const; │ │ │ │ │ +199 │ │ │ │ │ +_2_0_1 void _d_e_l_e_t_e_C_a_c_h_e_d_S_h_o_r_t_c_u_t_s_N_o_n_R_e_c_u_r_s_i_v_e() { │ │ │ │ │ +202 std::lock_guard marginalLock(_c_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l_M_u_t_e_x__); │ │ │ │ │ +203 _c_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l__ = boost::none; │ │ │ │ │ +204 } │ │ │ │ │ 205 │ │ │ │ │ -210 std::pair, boost:: │ │ │ │ │ -shared_ptr > │ │ │ │ │ -211 _e_l_i_m_i_n_a_t_e_P_a_r_t_i_a_l_M_u_l_t_i_f_r_o_n_t_a_l( │ │ │ │ │ -212 const _K_e_y_V_e_c_t_o_r& variables, │ │ │ │ │ -213 const _E_l_i_m_i_n_a_t_e& function = EliminationTraitsType::DefaultEliminate, │ │ │ │ │ -214 _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex = boost::none) const; │ │ │ │ │ -215 │ │ │ │ │ -225 boost::shared_ptr _m_a_r_g_i_n_a_l_M_u_l_t_i_f_r_o_n_t_a_l_B_a_y_e_s_N_e_t( │ │ │ │ │ -226 boost::variant variables, │ │ │ │ │ -227 const _E_l_i_m_i_n_a_t_e& function = EliminationTraitsType::DefaultEliminate, │ │ │ │ │ -228 _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex = boost::none) const; │ │ │ │ │ -229 │ │ │ │ │ -240 boost::shared_ptr _m_a_r_g_i_n_a_l_M_u_l_t_i_f_r_o_n_t_a_l_B_a_y_e_s_N_e_t( │ │ │ │ │ -241 boost::variant variables, │ │ │ │ │ -242 const _O_r_d_e_r_i_n_g& marginalizedVariableOrdering, │ │ │ │ │ -243 const _E_l_i_m_i_n_a_t_e& function = EliminationTraitsType::DefaultEliminate, │ │ │ │ │ -244 _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex = boost::none) const; │ │ │ │ │ -245 │ │ │ │ │ -255 boost::shared_ptr _m_a_r_g_i_n_a_l_M_u_l_t_i_f_r_o_n_t_a_l_B_a_y_e_s_T_r_e_e( │ │ │ │ │ -256 boost::variant variables, │ │ │ │ │ -257 const _E_l_i_m_i_n_a_t_e& function = EliminationTraitsType::DefaultEliminate, │ │ │ │ │ -258 _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex = boost::none) const; │ │ │ │ │ -259 │ │ │ │ │ -270 boost::shared_ptr _m_a_r_g_i_n_a_l_M_u_l_t_i_f_r_o_n_t_a_l_B_a_y_e_s_T_r_e_e( │ │ │ │ │ -271 boost::variant variables, │ │ │ │ │ -272 const _O_r_d_e_r_i_n_g& marginalizedVariableOrdering, │ │ │ │ │ -273 const _E_l_i_m_i_n_a_t_e& function = EliminationTraitsType::DefaultEliminate, │ │ │ │ │ -274 _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex = boost::none) const; │ │ │ │ │ -275 │ │ │ │ │ -277 boost::shared_ptr _m_a_r_g_i_n_a_l( │ │ │ │ │ -278 const _K_e_y_V_e_c_t_o_r& variables, │ │ │ │ │ -279 const _E_l_i_m_i_n_a_t_e& function = EliminationTraitsType::DefaultEliminate, │ │ │ │ │ -280 _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex = boost::none) const; │ │ │ │ │ -281 │ │ │ │ │ -282 private: │ │ │ │ │ -283 │ │ │ │ │ -284 // Access the derived factor graph class │ │ │ │ │ -285 const FactorGraphType& asDerived() const { return static_cast(*this); } │ │ │ │ │ -286 │ │ │ │ │ -287 // Access the derived factor graph class │ │ │ │ │ -288 FactorGraphType& asDerived() { return static_cast(*this); │ │ │ │ │ -} │ │ │ │ │ -289 │ │ │ │ │ -290 public: │ │ │ │ │ -291 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ -293 boost::shared_ptr GTSAM_DEPRECATED _e_l_i_m_i_n_a_t_e_S_e_q_u_e_n_t_i_a_l( │ │ │ │ │ -294 const Ordering& ordering, │ │ │ │ │ -295 const _E_l_i_m_i_n_a_t_e& function, │ │ │ │ │ -296 _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex, │ │ │ │ │ -297 _O_p_t_i_o_n_a_l_O_r_d_e_r_i_n_g_T_y_p_e orderingType) const { │ │ │ │ │ -298 return _e_l_i_m_i_n_a_t_e_S_e_q_u_e_n_t_i_a_l(ordering, function, variableIndex); │ │ │ │ │ -299 } │ │ │ │ │ -300 │ │ │ │ │ -302 boost::shared_ptr GTSAM_DEPRECATED _e_l_i_m_i_n_a_t_e_S_e_q_u_e_n_t_i_a_l( │ │ │ │ │ -303 const _E_l_i_m_i_n_a_t_e& function, │ │ │ │ │ -304 _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex = boost::none, │ │ │ │ │ -305 _O_p_t_i_o_n_a_l_O_r_d_e_r_i_n_g_T_y_p_e orderingType = boost::none) const { │ │ │ │ │ -306 return _e_l_i_m_i_n_a_t_e_S_e_q_u_e_n_t_i_a_l(orderingType, function, variableIndex); │ │ │ │ │ -307 } │ │ │ │ │ -308 │ │ │ │ │ -310 boost::shared_ptr GTSAM_DEPRECATED _e_l_i_m_i_n_a_t_e_M_u_l_t_i_f_r_o_n_t_a_l( │ │ │ │ │ -311 const Ordering& ordering, │ │ │ │ │ -312 const _E_l_i_m_i_n_a_t_e& function, │ │ │ │ │ -313 _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex, │ │ │ │ │ -314 _O_p_t_i_o_n_a_l_O_r_d_e_r_i_n_g_T_y_p_e orderingType) const { │ │ │ │ │ -315 return _e_l_i_m_i_n_a_t_e_M_u_l_t_i_f_r_o_n_t_a_l(ordering, function, variableIndex); │ │ │ │ │ -316 } │ │ │ │ │ -317 │ │ │ │ │ -319 boost::shared_ptr GTSAM_DEPRECATED _e_l_i_m_i_n_a_t_e_M_u_l_t_i_f_r_o_n_t_a_l( │ │ │ │ │ -320 const _E_l_i_m_i_n_a_t_e& function, │ │ │ │ │ -321 _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex = boost::none, │ │ │ │ │ -322 _O_p_t_i_o_n_a_l_O_r_d_e_r_i_n_g_T_y_p_e orderingType = boost::none) const { │ │ │ │ │ -323 return _e_l_i_m_i_n_a_t_e_M_u_l_t_i_f_r_o_n_t_a_l(orderingType, function, variableIndex); │ │ │ │ │ -324 } │ │ │ │ │ -325 │ │ │ │ │ -327 boost::shared_ptr GTSAM_DEPRECATED │ │ │ │ │ -_m_a_r_g_i_n_a_l_M_u_l_t_i_f_r_o_n_t_a_l_B_a_y_e_s_N_e_t( │ │ │ │ │ -328 boost::variant variables, │ │ │ │ │ -329 boost::none_t, │ │ │ │ │ -330 const _E_l_i_m_i_n_a_t_e& function = EliminationTraitsType::DefaultEliminate, │ │ │ │ │ -331 _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex = boost::none) const { │ │ │ │ │ -332 return _m_a_r_g_i_n_a_l_M_u_l_t_i_f_r_o_n_t_a_l_B_a_y_e_s_N_e_t(variables, function, variableIndex); │ │ │ │ │ -333 } │ │ │ │ │ -334 │ │ │ │ │ -336 boost::shared_ptr GTSAM_DEPRECATED │ │ │ │ │ -_m_a_r_g_i_n_a_l_M_u_l_t_i_f_r_o_n_t_a_l_B_a_y_e_s_T_r_e_e( │ │ │ │ │ -337 boost::variant variables, │ │ │ │ │ -338 boost::none_t, │ │ │ │ │ -339 const _E_l_i_m_i_n_a_t_e& function = EliminationTraitsType::DefaultEliminate, │ │ │ │ │ -340 _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex = boost::none) const { │ │ │ │ │ -341 return _m_a_r_g_i_n_a_l_M_u_l_t_i_f_r_o_n_t_a_l_B_a_y_e_s_T_r_e_e(variables, function, variableIndex); │ │ │ │ │ -342 } │ │ │ │ │ -343 #endif │ │ │ │ │ -344 }; │ │ │ │ │ -345 │ │ │ │ │ -346} │ │ │ │ │ +206 private: │ │ │ │ │ +207 │ │ │ │ │ +_2_0_9 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +210 template │ │ │ │ │ +211 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +212 if(!parent_.lock()) { │ │ │ │ │ +213 is_root = true; │ │ │ │ │ +214 } │ │ │ │ │ +215 ar & BOOST_SERIALIZATION_NVP(is_root); │ │ │ │ │ +216 ar & BOOST_SERIALIZATION_NVP(conditional_); │ │ │ │ │ +217 if (!is_root) { // TODO(fan): Workaround for boost/serialization #119 │ │ │ │ │ +218 ar & BOOST_SERIALIZATION_NVP(parent_); │ │ │ │ │ +219 } │ │ │ │ │ +220 ar & BOOST_SERIALIZATION_NVP(children); │ │ │ │ │ +221 } │ │ │ │ │ +222 │ │ │ │ │ +224 │ │ │ │ │ +225 }; │ │ │ │ │ +226 │ │ │ │ │ +227} │ │ │ │ │ +_F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ +A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ +_t_y_p_e_s_._h │ │ │ │ │ +Typedefs for easier changing of types. │ │ │ │ │ _O_r_d_e_r_i_n_g_._h │ │ │ │ │ Variable ordering for the elimination algorithm. │ │ │ │ │ -_V_a_r_i_a_b_l_e_I_n_d_e_x_._h │ │ │ │ │ +_K_e_y_._h │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ +std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ +FastVector │ │ │ │ │ +FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ +DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ FastVector< Key > KeyVector │ │ │ │ │ Define collection type once and for all - also used in wrappers. │ │ │ │ │ DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s │ │ │ │ │ Traits class for eliminateable factor graphs, specifies the types that result │ │ │ │ │ from elimination,... │ │ │ │ │ DDeeffiinniittiioonn EliminateableFactorGraph.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -EliminateableFactorGraph is a base class for factor graphs that contains │ │ │ │ │ -elimination algorithms. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_m_a_r_g_i_n_a_l_M_u_l_t_i_f_r_o_n_t_a_l_B_a_y_e_s_T_r_e_e │ │ │ │ │ -boost::shared_ptr< BayesTreeType > marginalMultifrontalBayesTree(boost:: │ │ │ │ │ -variant< const Ordering &, const KeyVector & > variables, const Eliminate │ │ │ │ │ -&function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex │ │ │ │ │ -variableIndex=boost::none) const │ │ │ │ │ -Compute the marginal of the requested variables and return the result as a │ │ │ │ │ -Bayes tree. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph-inst.h:300 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_J_u_n_c_t_i_o_n_T_r_e_e_T_y_p_e │ │ │ │ │ -EliminationTraitsType::JunctionTreeType JunctionTreeType │ │ │ │ │ -Junction tree type that can do multifrontal elimination of this graph. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph.h:82 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_m_a_r_g_i_n_a_l │ │ │ │ │ -boost::shared_ptr< FactorGraphType > marginal(const KeyVector &variables, const │ │ │ │ │ -Eliminate &function=EliminationTraitsType::DefaultEliminate, │ │ │ │ │ -OptionalVariableIndex variableIndex=boost::none) const │ │ │ │ │ -Compute the marginal factor graph of the requested variables. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph-inst.h:367 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_E_l_i_m_i_n_a_t_e │ │ │ │ │ -std::function< EliminationResult(const FactorGraphType &, const Ordering &)> │ │ │ │ │ -Eliminate │ │ │ │ │ -The function type that does a single dense elimination step on a subgraph. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph.h:89 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_B_a_y_e_s_T_r_e_e_T_y_p_e │ │ │ │ │ -EliminationTraitsType::BayesTreeType BayesTreeType │ │ │ │ │ -Bayes tree type produced by multifrontal elimination. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph.h:79 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_B_a_y_e_s_N_e_t_T_y_p_e │ │ │ │ │ -EliminationTraitsType::BayesNetType BayesNetType │ │ │ │ │ -Bayes net type produced by sequential elimination. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph.h:73 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_E_l_i_m_i_n_a_t_i_o_n_R_e_s_u_l_t │ │ │ │ │ -std::pair< boost::shared_ptr< ConditionalType >, boost::shared_ptr< _FactorType │ │ │ │ │ -> > EliminationResult │ │ │ │ │ -The pair of conditional and remaining factor produced by a single dense │ │ │ │ │ -elimination step on a subgrap... │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_e_l_i_m_i_n_a_t_e_S_e_q_u_e_n_t_i_a_l │ │ │ │ │ -boost::shared_ptr< BayesNetType > eliminateSequential(OptionalOrderingType │ │ │ │ │ -orderingType=boost::none, const Eliminate &function=EliminationTraitsType:: │ │ │ │ │ -DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const │ │ │ │ │ -Do sequential elimination of all variables to produce a Bayes net. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph-inst.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_e_l_i_m_i_n_a_t_e_M_u_l_t_i_f_r_o_n_t_a_l │ │ │ │ │ -boost::shared_ptr< BayesTreeType > eliminateMultifrontal(OptionalOrderingType │ │ │ │ │ -orderingType=boost::none, const Eliminate &function=EliminationTraitsType:: │ │ │ │ │ -DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const │ │ │ │ │ -Do multifrontal elimination of all variables to produce a Bayes tree. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph-inst.h:91 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_C_o_n_d_i_t_i_o_n_a_l_T_y_p_e │ │ │ │ │ -EliminationTraitsType::ConditionalType ConditionalType │ │ │ │ │ -Conditional type stored in the Bayes net produced by elimination. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph.h:70 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_T_y_p_e │ │ │ │ │ -EliminationTraitsType::EliminationTreeType EliminationTreeType │ │ │ │ │ -Elimination tree type that can do sequential elimination of this graph. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph.h:76 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ -boost::optional< const VariableIndex & > OptionalVariableIndex │ │ │ │ │ -Typedef for an optional variable index as an argument to elimination functions. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph.h:92 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_e_l_i_m_i_n_a_t_e_P_a_r_t_i_a_l_S_e_q_u_e_n_t_i_a_l │ │ │ │ │ -std::pair< boost::shared_ptr< BayesNetType >, boost::shared_ptr< │ │ │ │ │ -FactorGraphType > > eliminatePartialSequential(const Ordering &ordering, const │ │ │ │ │ -Eliminate &function=EliminationTraitsType::DefaultEliminate, │ │ │ │ │ -OptionalVariableIndex variableIndex=boost::none) const │ │ │ │ │ -Do sequential elimination of some variables, in ordering provided, to produce a │ │ │ │ │ -Bayes net and a remai... │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph-inst.h:154 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_m_a_r_g_i_n_a_l_M_u_l_t_i_f_r_o_n_t_a_l_B_a_y_e_s_N_e_t │ │ │ │ │ -boost::shared_ptr< BayesNetType > marginalMultifrontalBayesNet(boost::variant< │ │ │ │ │ -const Ordering &, const KeyVector & > variables, const Eliminate │ │ │ │ │ -&function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex │ │ │ │ │ -variableIndex=boost::none) const │ │ │ │ │ -Compute the marginal of the requested variables and return the result as a │ │ │ │ │ -Bayes net. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph-inst.h:233 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_e_l_i_m_i_n_a_t_e_P_a_r_t_i_a_l_M_u_l_t_i_f_r_o_n_t_a_l │ │ │ │ │ -std::pair< boost::shared_ptr< BayesTreeType >, boost::shared_ptr< │ │ │ │ │ -FactorGraphType > > eliminatePartialMultifrontal(const Ordering &ordering, │ │ │ │ │ -const Eliminate &function=EliminationTraitsType::DefaultEliminate, │ │ │ │ │ -OptionalVariableIndex variableIndex=boost::none) const │ │ │ │ │ -Do multifrontal elimination of some variables, in ordering provided, to produce │ │ │ │ │ -a Bayes tree and a re... │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph-inst.h:193 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_T_y_p_e │ │ │ │ │ -EliminationTraits< FactorGraphType > EliminationTraitsType │ │ │ │ │ -Typedef to the specific EliminationTraits for this graph. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_O_p_t_i_o_n_a_l_O_r_d_e_r_i_n_g_T_y_p_e │ │ │ │ │ -boost::optional< Ordering::OrderingType > OptionalOrderingType │ │ │ │ │ -Typedef for an optional ordering type. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph.h:95 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e │ │ │ │ │ +This is the base class for BayesTree cliques. │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_o_p_e_r_a_t_o_r_[_] │ │ │ │ │ +const derived_ptr operator[](size_t i) const │ │ │ │ │ +Return the child at index i. │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase.h:150 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ +BayesTreeCliqueBase & operator=(const BayesTreeCliqueBase &c) │ │ │ │ │ +Shallow copy assignment constructor. │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase.h:90 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_d_e_l_e_t_e_C_a_c_h_e_d_S_h_o_r_t_c_u_t_s_N_o_n_R_e_c_u_r_s_i_v_e │ │ │ │ │ +void deleteCachedShortcutsNonRecursive() │ │ │ │ │ +Non-recursive delete cached shortcuts and marginals - internal only. │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase.h:201 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_n_r_C_h_i_l_d_r_e_n │ │ │ │ │ +size_t nrChildren() const │ │ │ │ │ +Return the number of children. │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase.h:147 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_p_r_o_b_l_e_m_S_i_z_e │ │ │ │ │ +int problemSize() const │ │ │ │ │ +Problem size (used for parallel traversal) │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase.h:162 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e │ │ │ │ │ +BayesTreeCliqueBase() │ │ │ │ │ +Default constructor. │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_p_a_r_e_n_t │ │ │ │ │ +derived_ptr parent() const │ │ │ │ │ +return a shared_ptr to the parent clique │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase.h:159 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_t_r_e_e_S_i_z_e │ │ │ │ │ +size_t treeSize() const │ │ │ │ │ +The size of subtree rooted at this clique, i.e., nr of Cliques. │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:84 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e │ │ │ │ │ +BayesTreeCliqueBase(const sharedConditional &conditional) │ │ │ │ │ +Construct from a conditional, leaving parent and child pointers uninitialized. │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase.h:78 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_c_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l_M_u_t_e_x__ │ │ │ │ │ +std::mutex cachedSeparatorMarginalMutex_ │ │ │ │ │ +This protects Cached seperator marginal P(S) from concurrent read/writes as │ │ │ │ │ +many the functions which ... │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase.h:110 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_i_s_R_o_o_t │ │ │ │ │ +bool isRoot() const │ │ │ │ │ +Return true if this clique is the root of a Bayes tree. │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase.h:144 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e │ │ │ │ │ +BayesTreeCliqueBase(const BayesTreeCliqueBase &c) │ │ │ │ │ +Shallow copy constructor. │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase.h:82 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_m_a_r_g_i_n_a_l_2 │ │ │ │ │ +FactorGraphType marginal2(Eliminate function=EliminationTraitsType:: │ │ │ │ │ +DefaultEliminate) const │ │ │ │ │ +return the marginal P(C) of the clique, using marginal caching │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:195 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_s_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l │ │ │ │ │ +FactorGraphType separatorMarginal(Eliminate function=EliminationTraitsType:: │ │ │ │ │ +DefaultEliminate) const │ │ │ │ │ +return the marginal P(S) on the separator │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:147 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_s_h_o_r_t_c_u_t │ │ │ │ │ +BayesNetType shortcut(const derived_ptr &root, Eliminate │ │ │ │ │ +function=EliminationTraitsType::DefaultEliminate) const │ │ │ │ │ +return the conditional P(S|Root) on the separator given the root │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:113 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_d_e_l_e_t_e_C_a_c_h_e_d_S_h_o_r_t_c_u_t_s │ │ │ │ │ +void deleteCachedShortcuts() │ │ │ │ │ +This deletes the cached shortcuts of all cliques (subtree) below this clique. │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:207 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_s_h_o_r_t_c_u_t___i_n_d_i_c_e_s │ │ │ │ │ +KeyVector shortcut_indices(const derived_ptr &B, const FactorGraphType &p_Cp_B) │ │ │ │ │ +const │ │ │ │ │ +Determine variable indices to keep in recursive separator shortcut calculation │ │ │ │ │ +The factor graph p_Cp_... │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:57 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_c_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +const sharedConditional & conditional() const │ │ │ │ │ +Access the conditional. │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase.h:141 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase.h:209 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_s_e_p_a_r_a_t_o_r___s_e_t_m_i_n_u_s___B │ │ │ │ │ +KeyVector separator_setminus_B(const derived_ptr &B) const │ │ │ │ │ +Calculate set for shortcut calculations. │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_p_r_i_n_t │ │ │ │ │ +virtual void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ +print this node │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:76 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_n_u_m_C_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l_s │ │ │ │ │ +size_t numCachedSeparatorMarginals() const │ │ │ │ │ +Collect number of cliques with cached separator marginals. │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:93 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_c_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l__ │ │ │ │ │ +boost::optional< FactorGraphType > cachedSeparatorMarginal_ │ │ │ │ │ +This stores the Cached separator marginal P(S) │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase.h:105 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_s_e_t_E_l_i_m_i_n_a_t_i_o_n_R_e_s_u_l_t │ │ │ │ │ +void setEliminationResult(const typename FactorGraphType::EliminationResult │ │ │ │ │ +&eliminationResult) │ │ │ │ │ +Fill the elimination result produced during elimination. │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:27 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ + * _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00605_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/EliminateableFactorGraph-inst.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Conditional.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,483 +98,185 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
EliminateableFactorGraph-inst.h
│ │ │ │ +
Conditional.h
│ │ │ │
│ │ │ │
│ │ │ │ -
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ +
18// \callgraph
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/inference/EliminateableFactorGraph.h>
│ │ │ │ -
22#include <gtsam/inference/inferenceExceptions.h>
│ │ │ │ -
23#include <boost/tuple/tuple.hpp>
│ │ │ │ +
21#include <boost/range.hpp>
│ │ │ │ +
22
│ │ │ │ +
23#include <gtsam/inference/Key.h>
│ │ │ │
24
│ │ │ │
25namespace gtsam {
│ │ │ │
26
│ │ │ │ -
27 /* ************************************************************************* */
│ │ │ │ -
28 template<class FACTORGRAPH>
│ │ │ │ -
29 boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
│ │ │ │ -
│ │ │ │ -
30 EliminateableFactorGraph<FACTORGRAPH>::eliminateSequential(
│ │ │ │ -
31 OptionalOrderingType orderingType, const Eliminate& function,
│ │ │ │ -
32 OptionalVariableIndex variableIndex) const {
│ │ │ │ -
33 if(!variableIndex) {
│ │ │ │ -
34 // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check
│ │ │ │ -
35 // for no variable index first so that it's always computed if we need to call COLAMD because
│ │ │ │ -
36 // no Ordering is provided. When removing optional from VariableIndex, create VariableIndex
│ │ │ │ -
37 // before creating ordering.
│ │ │ │ -
38 VariableIndex computedVariableIndex(asDerived());
│ │ │ │ -
39 return eliminateSequential(orderingType, function, computedVariableIndex);
│ │ │ │ -
40 }
│ │ │ │ -
41 else {
│ │ │ │ -
42 // Compute an ordering and call this function again. We are guaranteed to have a
│ │ │ │ -
43 // VariableIndex already here because we computed one if needed in the previous 'if' block.
│ │ │ │ -
44 if (orderingType == Ordering::METIS) {
│ │ │ │ -
45 Ordering computedOrdering = Ordering::Metis(asDerived());
│ │ │ │ -
46 return eliminateSequential(computedOrdering, function, variableIndex);
│ │ │ │ -
47 } else if (orderingType == Ordering::COLAMD) {
│ │ │ │ -
48 Ordering computedOrdering = Ordering::Colamd(*variableIndex);
│ │ │ │ -
49 return eliminateSequential(computedOrdering, function, variableIndex);
│ │ │ │ -
50 } else if (orderingType == Ordering::NATURAL) {
│ │ │ │ -
51 Ordering computedOrdering = Ordering::Natural(asDerived());
│ │ │ │ -
52 return eliminateSequential(computedOrdering, function, variableIndex);
│ │ │ │ -
53 } else {
│ │ │ │ -
54 Ordering computedOrdering = EliminationTraitsType::DefaultOrderingFunc(
│ │ │ │ -
55 asDerived(), variableIndex);
│ │ │ │ -
56 return eliminateSequential(computedOrdering, function, variableIndex);
│ │ │ │ -
57 }
│ │ │ │ -
58 }
│ │ │ │ -
59 }
│ │ │ │ -
│ │ │ │ -
60
│ │ │ │ -
61 /* ************************************************************************* */
│ │ │ │ -
62 template<class FACTORGRAPH>
│ │ │ │ -
63 boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
│ │ │ │ -
│ │ │ │ -
64 EliminateableFactorGraph<FACTORGRAPH>::eliminateSequential(
│ │ │ │ -
65 const Ordering& ordering, const Eliminate& function,
│ │ │ │ -
66 OptionalVariableIndex variableIndex) const
│ │ │ │ -
67 {
│ │ │ │ -
68 if(!variableIndex) {
│ │ │ │ -
69 // If no VariableIndex provided, compute one and call this function again
│ │ │ │ -
70 VariableIndex computedVariableIndex(asDerived());
│ │ │ │ -
71 return eliminateSequential(ordering, function, computedVariableIndex);
│ │ │ │ -
72 } else {
│ │ │ │ -
73 gttic(eliminateSequential);
│ │ │ │ -
74 // Do elimination
│ │ │ │ -
75 EliminationTreeType etree(asDerived(), *variableIndex, ordering);
│ │ │ │ -
76 boost::shared_ptr<BayesNetType> bayesNet;
│ │ │ │ -
77 boost::shared_ptr<FactorGraphType> factorGraph;
│ │ │ │ -
78 boost::tie(bayesNet,factorGraph) = etree.eliminate(function);
│ │ │ │ -
79 // If any factors are remaining, the ordering was incomplete
│ │ │ │ -
80 if(!factorGraph->empty())
│ │ │ │ -
81 throw InconsistentEliminationRequested();
│ │ │ │ -
82 // Return the Bayes net
│ │ │ │ -
83 return bayesNet;
│ │ │ │ -
84 }
│ │ │ │ -
85 }
│ │ │ │ -
│ │ │ │ +
27 class HybridValues; // forward declaration.
│ │ │ │ +
28
│ │ │ │ +
62 template<class FACTOR, class DERIVEDCONDITIONAL>
│ │ │ │ +
│ │ │ │ +
63 class Conditional
│ │ │ │ +
64 {
│ │ │ │ +
65 protected:
│ │ │ │ +
67 size_t nrFrontals_;
│ │ │ │ +
68
│ │ │ │ +
69 private:
│ │ │ │ +
71 typedef Conditional<FACTOR,DERIVEDCONDITIONAL> This;
│ │ │ │ +
72
│ │ │ │ +
73 public:
│ │ │ │ +
75 typedef boost::iterator_range<typename FACTOR::const_iterator> Frontals;
│ │ │ │ +
76
│ │ │ │ +
78 typedef boost::iterator_range<typename FACTOR::const_iterator> Parents;
│ │ │ │ +
79
│ │ │ │ +
80 protected:
│ │ │ │ +
83
│ │ │ │ +
85 Conditional() : nrFrontals_(0) {}
│ │ │ │
86
│ │ │ │ -
87 /* ************************************************************************* */
│ │ │ │ -
88 template <class FACTORGRAPH>
│ │ │ │ -
89 boost::shared_ptr<
│ │ │ │ -
90 typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
│ │ │ │ -
│ │ │ │ -
91 EliminateableFactorGraph<FACTORGRAPH>::eliminateMultifrontal(
│ │ │ │ -
92 OptionalOrderingType orderingType, const Eliminate& function,
│ │ │ │ -
93 OptionalVariableIndex variableIndex) const {
│ │ │ │ -
94 if (!variableIndex) {
│ │ │ │ -
95 // If no VariableIndex provided, compute one and call this function again
│ │ │ │ -
96 // IMPORTANT: we check for no variable index first so that it's always
│ │ │ │ -
97 // computed if we need to call COLAMD because no Ordering is provided.
│ │ │ │ -
98 // When removing optional from VariableIndex, create VariableIndex before
│ │ │ │ -
99 // creating ordering.
│ │ │ │ -
100 VariableIndex computedVariableIndex(asDerived());
│ │ │ │ -
101 return eliminateMultifrontal(orderingType, function,
│ │ │ │ -
102 computedVariableIndex);
│ │ │ │ -
103 } else {
│ │ │ │ -
104 // Compute an ordering and call this function again. We are guaranteed to
│ │ │ │ -
105 // have a VariableIndex already here because we computed one if needed in
│ │ │ │ -
106 // the previous 'if' block.
│ │ │ │ -
107 if (orderingType == Ordering::METIS) {
│ │ │ │ -
108 Ordering computedOrdering = Ordering::Metis(asDerived());
│ │ │ │ -
109 return eliminateMultifrontal(computedOrdering, function, variableIndex);
│ │ │ │ -
110 } else if (orderingType == Ordering::COLAMD) {
│ │ │ │ -
111 Ordering computedOrdering = Ordering::Colamd(*variableIndex);
│ │ │ │ -
112 return eliminateMultifrontal(computedOrdering, function, variableIndex);
│ │ │ │ -
113 } else if (orderingType == Ordering::NATURAL) {
│ │ │ │ -
114 Ordering computedOrdering = Ordering::Natural(asDerived());
│ │ │ │ -
115 return eliminateMultifrontal(computedOrdering, function, variableIndex);
│ │ │ │ -
116 } else {
│ │ │ │ -
│ │ │ │ -
117 Ordering computedOrdering = EliminationTraitsType::DefaultOrderingFunc(
│ │ │ │ -
118 asDerived(), variableIndex);
│ │ │ │ -
119 return eliminateMultifrontal(computedOrdering, function, variableIndex);
│ │ │ │ -
120 }
│ │ │ │ +
88 Conditional(size_t nrFrontals) : nrFrontals_(nrFrontals) {}
│ │ │ │ +
89
│ │ │ │ +
91
│ │ │ │ +
92 public:
│ │ │ │ +
95
│ │ │ │ +
97 void print(const std::string& s = "Conditional", const KeyFormatter& formatter = DefaultKeyFormatter) const;
│ │ │ │ +
98
│ │ │ │ +
100 bool equals(const This& c, double tol = 1e-9) const;
│ │ │ │ +
101
│ │ │ │ +
103
│ │ │ │ +
106
│ │ │ │ +
107 virtual ~Conditional() {}
│ │ │ │ +
108
│ │ │ │ +
110 size_t nrFrontals() const { return nrFrontals_; }
│ │ │ │ +
111
│ │ │ │ +
113 size_t nrParents() const { return asFactor().size() - nrFrontals_; }
│ │ │ │ +
114
│ │ │ │ +
│ │ │ │ +
116 Key firstFrontalKey() const {
│ │ │ │ +
117 if(nrFrontals_ > 0)
│ │ │ │ +
118 return asFactor().front();
│ │ │ │ +
119 else
│ │ │ │ +
120 throw std::invalid_argument("Requested Conditional::firstFrontalKey from a conditional with zero frontal keys");
│ │ │ │
121 }
│ │ │ │ -
122 }
│ │ │ │ -
123
│ │ │ │ -
124 /* ************************************************************************* */
│ │ │ │ -
125 template<class FACTORGRAPH>
│ │ │ │ -
126 boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
│ │ │ │ -
│ │ │ │ -
127 EliminateableFactorGraph<FACTORGRAPH>::eliminateMultifrontal(
│ │ │ │ -
128 const Ordering& ordering, const Eliminate& function,
│ │ │ │ -
129 OptionalVariableIndex variableIndex) const
│ │ │ │ -
130 {
│ │ │ │ -
131 if(!variableIndex) {
│ │ │ │ -
132 // If no VariableIndex provided, compute one and call this function again
│ │ │ │ -
133 VariableIndex computedVariableIndex(asDerived());
│ │ │ │ -
134 return eliminateMultifrontal(ordering, function, computedVariableIndex);
│ │ │ │ -
135 } else {
│ │ │ │ -
│ │ │ │ -
136 gttic(eliminateMultifrontal);
│ │ │ │ -
137 // Do elimination with given ordering
│ │ │ │ -
138 EliminationTreeType etree(asDerived(), *variableIndex, ordering);
│ │ │ │ -
139 JunctionTreeType junctionTree(etree);
│ │ │ │ -
140 boost::shared_ptr<BayesTreeType> bayesTree;
│ │ │ │ -
141 boost::shared_ptr<FactorGraphType> factorGraph;
│ │ │ │ -
142 boost::tie(bayesTree,factorGraph) = junctionTree.eliminate(function);
│ │ │ │ -
143 // If any factors are remaining, the ordering was incomplete
│ │ │ │ -
144 if(!factorGraph->empty())
│ │ │ │ -
145 throw InconsistentEliminationRequested();
│ │ │ │ -
146 // Return the Bayes tree
│ │ │ │ -
147 return bayesTree;
│ │ │ │ -
148 }
│ │ │ │ -
149 }
│ │ │ │ -
150
│ │ │ │ -
151 /* ************************************************************************* */
│ │ │ │ -
152 template<class FACTORGRAPH>
│ │ │ │ -
153 std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>, boost::shared_ptr<FACTORGRAPH> >
│ │ │ │ -
│ │ │ │ -
154 EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialSequential(
│ │ │ │ -
155 const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
│ │ │ │ -
156 {
│ │ │ │ -
│ │ │ │ -
157 if(variableIndex) {
│ │ │ │ -
158 gttic(eliminatePartialSequential);
│ │ │ │ -
159 // Do elimination
│ │ │ │ -
160 EliminationTreeType etree(asDerived(), *variableIndex, ordering);
│ │ │ │ -
161 return etree.eliminate(function);
│ │ │ │ -
162 } else {
│ │ │ │ -
163 // If no variable index is provided, compute one and call this function again
│ │ │ │ -
164 VariableIndex computedVariableIndex(asDerived());
│ │ │ │ -
165 return eliminatePartialSequential(ordering, function, computedVariableIndex);
│ │ │ │ -
166 }
│ │ │ │ -
167 }
│ │ │ │ -
168
│ │ │ │ -
169 /* ************************************************************************* */
│ │ │ │ -
170 template<class FACTORGRAPH>
│ │ │ │ -
│ │ │ │ -
171 std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>, boost::shared_ptr<FACTORGRAPH> >
│ │ │ │ -
│ │ │ │ -
172 EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialSequential(
│ │ │ │ -
173 const KeyVector& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const
│ │ │ │ -
174 {
│ │ │ │ -
175 if(variableIndex) {
│ │ │ │ -
176 gttic(eliminatePartialSequential);
│ │ │ │ -
177 // Compute full ordering
│ │ │ │ -
178 Ordering fullOrdering = Ordering::ColamdConstrainedFirst(*variableIndex, variables);
│ │ │ │ -
179
│ │ │ │ -
180 // Split off the part of the ordering for the variables being eliminated
│ │ │ │ -
│ │ │ │ -
181 Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size());
│ │ │ │ -
182 return eliminatePartialSequential(ordering, function, variableIndex);
│ │ │ │ -
183 } else {
│ │ │ │ -
184 // If no variable index is provided, compute one and call this function again
│ │ │ │ -
185 VariableIndex computedVariableIndex(asDerived());
│ │ │ │ -
186 return eliminatePartialSequential(variables, function, computedVariableIndex);
│ │ │ │ -
187 }
│ │ │ │ -
188 }
│ │ │ │ -
189
│ │ │ │ -
190 /* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
191 template<class FACTORGRAPH>
│ │ │ │ -
192 std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>, boost::shared_ptr<FACTORGRAPH> >
│ │ │ │ -
│ │ │ │ -
193 EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialMultifrontal(
│ │ │ │ -
194 const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
│ │ │ │ -
195 {
│ │ │ │ -
196 if(variableIndex) {
│ │ │ │ -
197 gttic(eliminatePartialMultifrontal);
│ │ │ │ -
198 // Do elimination
│ │ │ │ -
199 EliminationTreeType etree(asDerived(), *variableIndex, ordering);
│ │ │ │ -
200 JunctionTreeType junctionTree(etree);
│ │ │ │ -
│ │ │ │ -
201 return junctionTree.eliminate(function);
│ │ │ │ -
202 } else {
│ │ │ │ -
203 // If no variable index is provided, compute one and call this function again
│ │ │ │ -
204 VariableIndex computedVariableIndex(asDerived());
│ │ │ │ -
205 return eliminatePartialMultifrontal(ordering, function, computedVariableIndex);
│ │ │ │ -
206 }
│ │ │ │ -
207 }
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
208
│ │ │ │ -
209 /* ************************************************************************* */
│ │ │ │ -
210 template<class FACTORGRAPH>
│ │ │ │ -
│ │ │ │ -
211 std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>, boost::shared_ptr<FACTORGRAPH> >
│ │ │ │ -
│ │ │ │ -
212 EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialMultifrontal(
│ │ │ │ -
213 const KeyVector& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const
│ │ │ │ -
214 {
│ │ │ │ -
215 if(variableIndex) {
│ │ │ │ -
216 gttic(eliminatePartialMultifrontal);
│ │ │ │ -
217 // Compute full ordering
│ │ │ │ -
218 Ordering fullOrdering = Ordering::ColamdConstrainedFirst(*variableIndex, variables);
│ │ │ │ -
219
│ │ │ │ -
220 // Split off the part of the ordering for the variables being eliminated
│ │ │ │ -
221 Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size());
│ │ │ │ -
222 return eliminatePartialMultifrontal(ordering, function, variableIndex);
│ │ │ │ -
223 } else {
│ │ │ │ -
224 // If no variable index is provided, compute one and call this function again
│ │ │ │ -
│ │ │ │ -
225 VariableIndex computedVariableIndex(asDerived());
│ │ │ │ -
226 return eliminatePartialMultifrontal(variables, function, computedVariableIndex);
│ │ │ │ -
227 }
│ │ │ │ -
228 }
│ │ │ │ -
229
│ │ │ │ -
230 /* ************************************************************************* */
│ │ │ │ -
231 template<class FACTORGRAPH>
│ │ │ │ -
232 boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
│ │ │ │ -
│ │ │ │ -
233 EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet(
│ │ │ │ -
234 boost::variant<const Ordering&, const KeyVector&> variables,
│ │ │ │ -
235 const Eliminate& function, OptionalVariableIndex variableIndex) const
│ │ │ │ -
236 {
│ │ │ │ -
237 if(!variableIndex) {
│ │ │ │ -
238 // If no variable index is provided, compute one and call this function again
│ │ │ │ -
239 VariableIndex index(asDerived());
│ │ │ │ -
│ │ │ │ -
240 return marginalMultifrontalBayesNet(variables, function, index);
│ │ │ │ -
241 } else {
│ │ │ │ -
242 // No ordering was provided for the marginalized variables, so order them using constrained
│ │ │ │ -
243 // COLAMD.
│ │ │ │ -
244 bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0);
│ │ │ │ -
245 const KeyVector* variablesOrOrdering =
│ │ │ │ -
246 unmarginalizedAreOrdered ?
│ │ │ │ -
247 boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&variables);
│ │ │ │ -
248
│ │ │ │ -
249 Ordering totalOrdering =
│ │ │ │ -
250 Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
│ │ │ │ -
251
│ │ │ │ -
252 // Split up ordering
│ │ │ │ -
253 const size_t nVars = variablesOrOrdering->size();
│ │ │ │ -
254 Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars);
│ │ │ │ -
│ │ │ │ -
255 Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
│ │ │ │ -
256
│ │ │ │ -
257 // Call this function again with the computed orderings
│ │ │ │ -
258 return marginalMultifrontalBayesNet(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex);
│ │ │ │ -
259 }
│ │ │ │ -
260 }
│ │ │ │ -
261
│ │ │ │ -
262 /* ************************************************************************* */
│ │ │ │ -
263 template<class FACTORGRAPH>
│ │ │ │ -
264 boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
│ │ │ │ -
│ │ │ │ -
265 EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet(
│ │ │ │ -
266 boost::variant<const Ordering&, const KeyVector&> variables,
│ │ │ │ -
267 const Ordering& marginalizedVariableOrdering,
│ │ │ │ -
268 const Eliminate& function, OptionalVariableIndex variableIndex) const
│ │ │ │ -
269 {
│ │ │ │ -
│ │ │ │ -
270 if(!variableIndex) {
│ │ │ │ -
271 // If no variable index is provided, compute one and call this function again
│ │ │ │ -
272 VariableIndex index(asDerived());
│ │ │ │ -
273 return marginalMultifrontalBayesNet(variables, marginalizedVariableOrdering, function, index);
│ │ │ │ -
274 } else {
│ │ │ │ -
275 gttic(marginalMultifrontalBayesNet);
│ │ │ │ -
276 // An ordering was provided for the marginalized variables, so we can first eliminate them
│ │ │ │ -
│ │ │ │ -
277 // in the order requested.
│ │ │ │ -
278 boost::shared_ptr<BayesTreeType> bayesTree;
│ │ │ │ -
279 boost::shared_ptr<FactorGraphType> factorGraph;
│ │ │ │ -
280 boost::tie(bayesTree,factorGraph) =
│ │ │ │ -
281 eliminatePartialMultifrontal(marginalizedVariableOrdering, function, *variableIndex);
│ │ │ │ -
282
│ │ │ │ -
283 if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables))
│ │ │ │ -
284 {
│ │ │ │ -
285 // An ordering was also provided for the unmarginalized variables, so we can also
│ │ │ │ -
286 // eliminate them in the order requested.
│ │ │ │ -
287 return factorGraph->eliminateSequential(*varsAsOrdering, function);
│ │ │ │ -
288 }
│ │ │ │ -
289 else
│ │ │ │ -
290 {
│ │ │ │ -
291 // No ordering was provided for the unmarginalized variables, so order them with COLAMD.
│ │ │ │ -
292 return factorGraph->eliminateSequential(Ordering::COLAMD, function);
│ │ │ │ -
293 }
│ │ │ │ -
294 }
│ │ │ │ -
295 }
│ │ │ │ -
296
│ │ │ │ -
297 /* ************************************************************************* */
│ │ │ │ -
298 template<class FACTORGRAPH>
│ │ │ │ -
299 boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
│ │ │ │ -
│ │ │ │ -
300 EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree(
│ │ │ │ -
301 boost::variant<const Ordering&, const KeyVector&> variables,
│ │ │ │ -
302 const Eliminate& function, OptionalVariableIndex variableIndex) const
│ │ │ │ -
303 {
│ │ │ │ -
304 if(!variableIndex) {
│ │ │ │ -
305 // If no variable index is provided, compute one and call this function again
│ │ │ │ -
306 VariableIndex computedVariableIndex(asDerived());
│ │ │ │ -
307 return marginalMultifrontalBayesTree(variables, function, computedVariableIndex);
│ │ │ │ -
308 } else {
│ │ │ │ -
309 // No ordering was provided for the marginalized variables, so order them using constrained
│ │ │ │ -
310 // COLAMD.
│ │ │ │ -
311 bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0);
│ │ │ │ -
312 const KeyVector* variablesOrOrdering =
│ │ │ │ -
313 unmarginalizedAreOrdered ?
│ │ │ │ -
314 boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&variables);
│ │ │ │ -
315
│ │ │ │ -
316 Ordering totalOrdering =
│ │ │ │ -
317 Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
│ │ │ │ -
318
│ │ │ │ -
319 // Split up ordering
│ │ │ │ -
320 const size_t nVars = variablesOrOrdering->size();
│ │ │ │ -
321 Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars);
│ │ │ │ -
322 Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
│ │ │ │ -
323
│ │ │ │ -
324 // Call this function again with the computed orderings
│ │ │ │ -
325 return marginalMultifrontalBayesTree(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex);
│ │ │ │ -
326 }
│ │ │ │ -
327 }
│ │ │ │ -
│ │ │ │ -
328
│ │ │ │ -
329 /* ************************************************************************* */
│ │ │ │ -
330 template<class FACTORGRAPH>
│ │ │ │ -
331 boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
│ │ │ │ -
│ │ │ │ -
332 EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree(
│ │ │ │ -
333 boost::variant<const Ordering&, const KeyVector&> variables,
│ │ │ │ -
334 const Ordering& marginalizedVariableOrdering,
│ │ │ │ -
335 const Eliminate& function, OptionalVariableIndex variableIndex) const
│ │ │ │ -
336 {
│ │ │ │ -
337 if(!variableIndex) {
│ │ │ │ -
338 // If no variable index is provided, compute one and call this function again
│ │ │ │ -
339 VariableIndex computedVariableIndex(asDerived());
│ │ │ │ -
340 return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, computedVariableIndex);
│ │ │ │ -
341 } else {
│ │ │ │ -
342 gttic(marginalMultifrontalBayesTree);
│ │ │ │ -
343 // An ordering was provided for the marginalized variables, so we can first eliminate them
│ │ │ │ -
344 // in the order requested.
│ │ │ │ -
345 boost::shared_ptr<BayesTreeType> bayesTree;
│ │ │ │ -
346 boost::shared_ptr<FactorGraphType> factorGraph;
│ │ │ │ -
347 boost::tie(bayesTree,factorGraph) =
│ │ │ │ -
348 eliminatePartialMultifrontal(marginalizedVariableOrdering, function, *variableIndex);
│ │ │ │ -
349
│ │ │ │ -
350 if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables))
│ │ │ │ -
351 {
│ │ │ │ -
352 // An ordering was also provided for the unmarginalized variables, so we can also
│ │ │ │ -
353 // eliminate them in the order requested.
│ │ │ │ -
354 return factorGraph->eliminateMultifrontal(*varsAsOrdering, function);
│ │ │ │ -
355 }
│ │ │ │ -
356 else
│ │ │ │ -
357 {
│ │ │ │ -
358 // No ordering was provided for the unmarginalized variables, so order them with COLAMD.
│ │ │ │ -
359 return factorGraph->eliminateMultifrontal(Ordering::COLAMD, function);
│ │ │ │ -
360 }
│ │ │ │ -
361 }
│ │ │ │ -
362 }
│ │ │ │ -
│ │ │ │ -
363
│ │ │ │ -
364 /* ************************************************************************* */
│ │ │ │ -
365 template<class FACTORGRAPH>
│ │ │ │ -
366 boost::shared_ptr<FACTORGRAPH>
│ │ │ │ -
│ │ │ │ -
367 EliminateableFactorGraph<FACTORGRAPH>::marginal(
│ │ │ │ -
368 const KeyVector& variables,
│ │ │ │ -
369 const Eliminate& function, OptionalVariableIndex variableIndex) const
│ │ │ │ -
370 {
│ │ │ │ -
371 if(variableIndex)
│ │ │ │ -
372 {
│ │ │ │ -
373 // Compute a total ordering for all variables
│ │ │ │ -
374 Ordering totalOrdering = Ordering::ColamdConstrainedLast(*variableIndex, variables);
│ │ │ │ -
375
│ │ │ │ -
376 // Split out the part for the marginalized variables
│ │ │ │ -
377 Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - variables.size());
│ │ │ │ -
378
│ │ │ │ -
379 // Eliminate and return the remaining factor graph
│ │ │ │ -
380 return eliminatePartialMultifrontal(marginalizationOrdering, function, *variableIndex).second;
│ │ │ │ -
381 }
│ │ │ │ -
382 else
│ │ │ │ -
383 {
│ │ │ │ -
384 // If no variable index is provided, compute one and call this function again
│ │ │ │ -
385 VariableIndex computedVariableIndex(asDerived());
│ │ │ │ -
386 return marginal(variables, function, computedVariableIndex);
│ │ │ │ -
387 }
│ │ │ │ -
388 }
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
389
│ │ │ │ -
390
│ │ │ │ -
391}
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
EliminateableFactorGraph.h
Variable elimination algorithms for factor graphs.
│ │ │ │ -
inferenceExceptions.h
Exceptions that may be thrown by inference algorithms.
│ │ │ │ +
122
│ │ │ │ +
124 Frontals frontals() const { return boost::make_iterator_range(beginFrontals(), endFrontals()); }
│ │ │ │ +
125
│ │ │ │ +
127 Parents parents() const { return boost::make_iterator_range(beginParents(), endParents()); }
│ │ │ │ +
128
│ │ │ │ +
133 virtual double logProbability(const HybridValues& c) const;
│ │ │ │ +
134
│ │ │ │ +
139 virtual double evaluate(const HybridValues& c) const;
│ │ │ │ +
140
│ │ │ │ +
│ │ │ │ +
142 double operator()(const HybridValues& x) const {
│ │ │ │ +
143 return evaluate(x);
│ │ │ │ +
144 }
│ │ │ │ +
│ │ │ │ +
145
│ │ │ │ +
150 virtual double logNormalizationConstant() const;
│ │ │ │ +
151
│ │ │ │ +
153 double normalizationConstant() const;
│ │ │ │ +
154
│ │ │ │ +
158
│ │ │ │ +
160 typename FACTOR::const_iterator beginFrontals() const { return asFactor().begin(); }
│ │ │ │ +
161
│ │ │ │ +
163 typename FACTOR::const_iterator endFrontals() const { return asFactor().begin() + nrFrontals_; }
│ │ │ │ +
164
│ │ │ │ +
166 typename FACTOR::const_iterator beginParents() const { return endFrontals(); }
│ │ │ │ +
167
│ │ │ │ +
169 typename FACTOR::const_iterator endParents() const { return asFactor().end(); }
│ │ │ │ +
170
│ │ │ │ +
172 size_t& nrFrontals() { return nrFrontals_; }
│ │ │ │ +
173
│ │ │ │ +
175 typename FACTOR::iterator beginFrontals() { return asFactor().begin(); }
│ │ │ │ +
176
│ │ │ │ +
178 typename FACTOR::iterator endFrontals() { return asFactor().begin() + nrFrontals_; }
│ │ │ │ +
179
│ │ │ │ +
181 typename FACTOR::iterator beginParents() { return asFactor().begin() + nrFrontals_; }
│ │ │ │ +
182
│ │ │ │ +
184 typename FACTOR::iterator endParents() { return asFactor().end(); }
│ │ │ │ +
185
│ │ │ │ +
199 template <class VALUES>
│ │ │ │ +
200 static bool CheckInvariants(const DERIVEDCONDITIONAL& conditional,
│ │ │ │ +
201 const VALUES& x);
│ │ │ │ +
202
│ │ │ │ +
204
│ │ │ │ +
205 private:
│ │ │ │ +
206
│ │ │ │ +
209
│ │ │ │ +
210 // Cast to factor type (non-const) (casts down to derived conditional type, then up to factor type)
│ │ │ │ +
211 FACTOR& asFactor() { return static_cast<FACTOR&>(static_cast<DERIVEDCONDITIONAL&>(*this)); }
│ │ │ │ +
212
│ │ │ │ +
213 // Cast to derived type (const) (casts down to derived conditional type, then up to factor type)
│ │ │ │ +
214 const FACTOR& asFactor() const { return static_cast<const FACTOR&>(static_cast<const DERIVEDCONDITIONAL&>(*this)); }
│ │ │ │ +
215
│ │ │ │ +
217 friend class boost::serialization::access;
│ │ │ │ +
218 template<class ARCHIVE>
│ │ │ │ +
219 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
220 ar & BOOST_SERIALIZATION_NVP(nrFrontals_);
│ │ │ │ +
221 }
│ │ │ │ +
222
│ │ │ │ +
224
│ │ │ │ +
225 };
│ │ │ │ +
│ │ │ │ +
226
│ │ │ │ +
227} // gtsam
│ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::EliminateableFactorGraph::marginalMultifrontalBayesTree
boost::shared_ptr< BayesTreeType > marginalMultifrontalBayesTree(boost::variant< const Ordering &, const KeyVector & > variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Compute the marginal of the requested variables and return the result as a Bayes tree.
Definition EliminateableFactorGraph-inst.h:300
│ │ │ │ -
gtsam::EliminateableFactorGraph< DiscreteFactorGraph >::JunctionTreeType
EliminationTraitsType::JunctionTreeType JunctionTreeType
Junction tree type that can do multifrontal elimination of this graph.
Definition EliminateableFactorGraph.h:82
│ │ │ │ -
gtsam::EliminateableFactorGraph::marginal
boost::shared_ptr< FactorGraphType > marginal(const KeyVector &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Compute the marginal factor graph of the requested variables.
Definition EliminateableFactorGraph-inst.h:367
│ │ │ │ -
gtsam::EliminateableFactorGraph::marginalMultifrontalBayesNet
boost::shared_ptr< BayesNetType > marginalMultifrontalBayesNet(boost::variant< const Ordering &, const KeyVector & > variables, const Ordering &marginalizedVariableOrdering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Compute the marginal of the requested variables and return the result as a Bayes net.
Definition EliminateableFactorGraph-inst.h:265
│ │ │ │ -
gtsam::EliminateableFactorGraph::Eliminate
std::function< EliminationResult(const FactorGraphType &, const Ordering &)> Eliminate
The function type that does a single dense elimination step on a subgraph.
Definition EliminateableFactorGraph.h:89
│ │ │ │ -
gtsam::EliminateableFactorGraph::BayesTreeType
EliminationTraitsType::BayesTreeType BayesTreeType
Bayes tree type produced by multifrontal elimination.
Definition EliminateableFactorGraph.h:79
│ │ │ │ -
gtsam::EliminateableFactorGraph::eliminateSequential
boost::shared_ptr< BayesNetType > eliminateSequential(OptionalOrderingType orderingType=boost::none, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Do sequential elimination of all variables to produce a Bayes net.
Definition EliminateableFactorGraph-inst.h:30
│ │ │ │ -
gtsam::EliminateableFactorGraph::eliminateMultifrontal
boost::shared_ptr< BayesTreeType > eliminateMultifrontal(OptionalOrderingType orderingType=boost::none, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Do multifrontal elimination of all variables to produce a Bayes tree.
Definition EliminateableFactorGraph-inst.h:91
│ │ │ │ -
gtsam::EliminateableFactorGraph::EliminationTreeType
EliminationTraitsType::EliminationTreeType EliminationTreeType
Elimination tree type that can do sequential elimination of this graph.
Definition EliminateableFactorGraph.h:76
│ │ │ │ -
gtsam::EliminateableFactorGraph::OptionalVariableIndex
boost::optional< const VariableIndex & > OptionalVariableIndex
Typedef for an optional variable index as an argument to elimination functions.
Definition EliminateableFactorGraph.h:92
│ │ │ │ -
gtsam::EliminateableFactorGraph::eliminatePartialSequential
std::pair< boost::shared_ptr< BayesNetType >, boost::shared_ptr< FactorGraphType > > eliminatePartialSequential(const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Do sequential elimination of some variables, in ordering provided, to produce a Bayes net and a remai...
Definition EliminateableFactorGraph-inst.h:154
│ │ │ │ -
gtsam::EliminateableFactorGraph::marginalMultifrontalBayesNet
boost::shared_ptr< BayesNetType > marginalMultifrontalBayesNet(boost::variant< const Ordering &, const KeyVector & > variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Compute the marginal of the requested variables and return the result as a Bayes net.
Definition EliminateableFactorGraph-inst.h:233
│ │ │ │ -
gtsam::EliminateableFactorGraph::eliminatePartialMultifrontal
std::pair< boost::shared_ptr< BayesTreeType >, boost::shared_ptr< FactorGraphType > > eliminatePartialMultifrontal(const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Do multifrontal elimination of some variables, in ordering provided, to produce a Bayes tree and a re...
Definition EliminateableFactorGraph-inst.h:193
│ │ │ │ -
gtsam::EliminateableFactorGraph::OptionalOrderingType
boost::optional< Ordering::OrderingType > OptionalOrderingType
Typedef for an optional ordering type.
Definition EliminateableFactorGraph.h:95
│ │ │ │ -
gtsam::InconsistentEliminationRequested
An inference algorithm was called with inconsistent arguments.
Definition inferenceExceptions.h:29
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ -
gtsam::Ordering::Natural
static Ordering Natural(const FACTOR_GRAPH &fg)
Return a natural Ordering. Typically used by iterative solvers.
Definition Ordering.h:190
│ │ │ │ -
gtsam::Ordering::Colamd
static Ordering Colamd(const FACTOR_GRAPH &graph)
Compute a fill-reducing ordering using COLAMD from a factor graph (see details for note on performanc...
Definition Ordering.h:95
│ │ │ │ -
gtsam::Ordering::ColamdConstrainedLast
static Ordering ColamdConstrainedLast(const FACTOR_GRAPH &graph, const KeyVector &constrainLast, bool forceOrder=false)
Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details for note o...
Definition Ordering.h:114
│ │ │ │ -
gtsam::Ordering::ColamdConstrainedFirst
static Ordering ColamdConstrainedFirst(const FACTOR_GRAPH &graph, const KeyVector &constrainFirst, bool forceOrder=false)
Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details for note o...
Definition Ordering.h:141
│ │ │ │ -
gtsam::Ordering::Metis
static GTSAM_EXPORT Ordering Metis(const MetisIndex &met)
Compute an ordering determined by METIS from a VariableIndex.
Definition Ordering.cpp:212
│ │ │ │ -
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition VariableIndex.h:43
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ +
gtsam::Conditional
Definition Conditional.h:64
│ │ │ │ +
gtsam::Conditional::CheckInvariants
static bool CheckInvariants(const DERIVEDCONDITIONAL &conditional, const VALUES &x)
Check invariants of this conditional, given the values x.
Definition Conditional-inst.h:77
│ │ │ │ +
gtsam::Conditional::nrFrontals_
size_t nrFrontals_
The first nrFrontal variables are frontal and the rest are parents.
Definition Conditional.h:67
│ │ │ │ +
gtsam::Conditional::logNormalizationConstant
virtual double logNormalizationConstant() const
All conditional types need to implement a log normalization constant to make it such that error>=0.
Definition Conditional-inst.h:62
│ │ │ │ +
gtsam::Conditional::firstFrontalKey
Key firstFrontalKey() const
Convenience function to get the first frontal key.
Definition Conditional.h:116
│ │ │ │ +
gtsam::Conditional::endFrontals
FACTOR::const_iterator endFrontals() const
Iterator pointing past the last frontal key.
Definition Conditional.h:163
│ │ │ │ +
gtsam::Conditional::endParents
FACTOR::iterator endParents()
Mutable iterator pointing past the last parent key.
Definition Conditional.h:184
│ │ │ │ +
gtsam::Conditional::endFrontals
FACTOR::iterator endFrontals()
Mutable iterator pointing past the last frontal key.
Definition Conditional.h:178
│ │ │ │ +
gtsam::Conditional::evaluate
virtual double evaluate(const HybridValues &c) const
All conditional types need to implement an evaluate function, that yields a true probability.
Definition Conditional-inst.h:55
│ │ │ │ +
gtsam::Conditional::parents
Parents parents() const
return a view of the parent keys
Definition Conditional.h:127
│ │ │ │ +
gtsam::Conditional::operator()
double operator()(const HybridValues &x) const
Evaluate probability density, sugar.
Definition Conditional.h:142
│ │ │ │ +
gtsam::Conditional::Parents
boost::iterator_range< typename FACTOR::const_iterator > Parents
View of the separator keys (call parents())
Definition Conditional.h:78
│ │ │ │ +
gtsam::Conditional::logProbability
virtual double logProbability(const HybridValues &c) const
All conditional types need to implement a logProbability function, for which exp(logProbability(x)) =...
Definition Conditional-inst.h:48
│ │ │ │ +
gtsam::Conditional::nrFrontals
size_t nrFrontals() const
return the number of frontals
Definition Conditional.h:110
│ │ │ │ +
gtsam::Conditional::Conditional
Conditional()
Empty Constructor to make serialization possible.
Definition Conditional.h:85
│ │ │ │ +
gtsam::Conditional::Conditional
Conditional(size_t nrFrontals)
Constructor.
Definition Conditional.h:88
│ │ │ │ +
gtsam::Conditional::beginParents
FACTOR::iterator beginParents()
Mutable iterator pointing to the first parent key.
Definition Conditional.h:181
│ │ │ │ +
gtsam::Conditional::nrFrontals
size_t & nrFrontals()
Mutable version of nrFrontals.
Definition Conditional.h:172
│ │ │ │ +
gtsam::Conditional::beginFrontals
FACTOR::iterator beginFrontals()
Mutable iterator pointing to first frontal key.
Definition Conditional.h:175
│ │ │ │ +
gtsam::Conditional::Frontals
boost::iterator_range< typename FACTOR::const_iterator > Frontals
View of the frontal keys (call frontals())
Definition Conditional.h:75
│ │ │ │ +
gtsam::Conditional::frontals
Frontals frontals() const
return a view of the frontal keys
Definition Conditional.h:124
│ │ │ │ +
gtsam::Conditional::access
friend class boost::serialization::access
Serialization function.
Definition Conditional.h:217
│ │ │ │ +
gtsam::Conditional::beginFrontals
FACTOR::const_iterator beginFrontals() const
Iterator pointing to first frontal key.
Definition Conditional.h:160
│ │ │ │ +
gtsam::Conditional::normalizationConstant
double normalizationConstant() const
Non-virtual, exponentiate logNormalizationConstant.
Definition Conditional-inst.h:70
│ │ │ │ +
gtsam::Conditional::nrParents
size_t nrParents() const
return the number of parents
Definition Conditional.h:113
│ │ │ │ +
gtsam::Conditional::print
void print(const std::string &s="Conditional", const KeyFormatter &formatter=DefaultKeyFormatter) const
print with optional formatter
Definition Conditional-inst.h:30
│ │ │ │ +
gtsam::Conditional::endParents
FACTOR::const_iterator endParents() const
Iterator pointing past the last parent key.
Definition Conditional.h:169
│ │ │ │ +
gtsam::Conditional::beginParents
FACTOR::const_iterator beginParents() const
Iterator pointing to the first parent key.
Definition Conditional.h:166
│ │ │ │ +
HybridValues
the error.
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,609 +1,285 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -EliminateableFactorGraph-inst.h │ │ │ │ │ +Conditional.h │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ +18// \callgraph │ │ │ │ │ 19#pragma once │ │ │ │ │ 20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_i_n_f_e_r_e_n_c_e_E_x_c_e_p_t_i_o_n_s_._h> │ │ │ │ │ -23#include │ │ │ │ │ +21#include │ │ │ │ │ +22 │ │ │ │ │ +23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ 24 │ │ │ │ │ 25namespace _g_t_s_a_m { │ │ │ │ │ 26 │ │ │ │ │ -27 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -28 template │ │ │ │ │ -29 boost::shared_ptr:: │ │ │ │ │ -BayesNetType> │ │ │ │ │ -_3_0 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_G_R_A_P_H_>_:_:_e_l_i_m_i_n_a_t_e_S_e_q_u_e_n_t_i_a_l( │ │ │ │ │ -31 _O_p_t_i_o_n_a_l_O_r_d_e_r_i_n_g_T_y_p_e orderingType, const _E_l_i_m_i_n_a_t_e& function, │ │ │ │ │ -32 _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex) const { │ │ │ │ │ -33 if(!variableIndex) { │ │ │ │ │ -34 // If no VariableIndex provided, compute one and call this function again │ │ │ │ │ -IMPORTANT: we check │ │ │ │ │ -35 // for no variable index first so that it's always computed if we need to │ │ │ │ │ -call COLAMD because │ │ │ │ │ -36 // no Ordering is provided. When removing optional from VariableIndex, │ │ │ │ │ -create VariableIndex │ │ │ │ │ -37 // before creating ordering. │ │ │ │ │ -38 _V_a_r_i_a_b_l_e_I_n_d_e_x computedVariableIndex(asDerived()); │ │ │ │ │ -39 return eliminateSequential(orderingType, function, computedVariableIndex); │ │ │ │ │ -40 } │ │ │ │ │ -41 else { │ │ │ │ │ -42 // Compute an ordering and call this function again. We are guaranteed to │ │ │ │ │ -have a │ │ │ │ │ -43 // VariableIndex already here because we computed one if needed in the │ │ │ │ │ -previous 'if' block. │ │ │ │ │ -44 if (orderingType == Ordering::METIS) { │ │ │ │ │ -45 _O_r_d_e_r_i_n_g computedOrdering = _O_r_d_e_r_i_n_g_:_:_M_e_t_i_s(asDerived()); │ │ │ │ │ -46 return eliminateSequential(computedOrdering, function, variableIndex); │ │ │ │ │ -47 } else if (orderingType == Ordering::COLAMD) { │ │ │ │ │ -48 _O_r_d_e_r_i_n_g computedOrdering = _O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d(*variableIndex); │ │ │ │ │ -49 return eliminateSequential(computedOrdering, function, variableIndex); │ │ │ │ │ -50 } else if (orderingType == Ordering::NATURAL) { │ │ │ │ │ -51 _O_r_d_e_r_i_n_g computedOrdering = _O_r_d_e_r_i_n_g_:_:_N_a_t_u_r_a_l(asDerived()); │ │ │ │ │ -52 return eliminateSequential(computedOrdering, function, variableIndex); │ │ │ │ │ -53 } else { │ │ │ │ │ -54 _O_r_d_e_r_i_n_g computedOrdering = EliminationTraitsType::DefaultOrderingFunc( │ │ │ │ │ -55 asDerived(), variableIndex); │ │ │ │ │ -56 return eliminateSequential(computedOrdering, function, variableIndex); │ │ │ │ │ -57 } │ │ │ │ │ -58 } │ │ │ │ │ -59 } │ │ │ │ │ -60 │ │ │ │ │ -61 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -62 template │ │ │ │ │ -63 boost::shared_ptr:: │ │ │ │ │ -BayesNetType> │ │ │ │ │ -_6_4 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_G_R_A_P_H_>_:_:_e_l_i_m_i_n_a_t_e_S_e_q_u_e_n_t_i_a_l( │ │ │ │ │ -65 const _O_r_d_e_r_i_n_g& ordering, const _E_l_i_m_i_n_a_t_e& function, │ │ │ │ │ -66 _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex) const │ │ │ │ │ -67 { │ │ │ │ │ -68 if(!variableIndex) { │ │ │ │ │ -69 // If no VariableIndex provided, compute one and call this function again │ │ │ │ │ -70 _V_a_r_i_a_b_l_e_I_n_d_e_x computedVariableIndex(asDerived()); │ │ │ │ │ -71 return eliminateSequential(ordering, function, computedVariableIndex); │ │ │ │ │ -72 } else { │ │ │ │ │ -73 gttic(eliminateSequential); │ │ │ │ │ -74 // Do elimination │ │ │ │ │ -75 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_T_y_p_e etree(asDerived(), *variableIndex, ordering); │ │ │ │ │ -76 boost::shared_ptr bayesNet; │ │ │ │ │ -77 boost::shared_ptr factorGraph; │ │ │ │ │ -78 boost::tie(bayesNet,factorGraph) = etree.eliminate(function); │ │ │ │ │ -79 // If any factors are remaining, the ordering was incomplete │ │ │ │ │ -80 if(!factorGraph->empty()) │ │ │ │ │ -81 throw _I_n_c_o_n_s_i_s_t_e_n_t_E_l_i_m_i_n_a_t_i_o_n_R_e_q_u_e_s_t_e_d(); │ │ │ │ │ -82 // Return the Bayes net │ │ │ │ │ -83 return bayesNet; │ │ │ │ │ -84 } │ │ │ │ │ -85 } │ │ │ │ │ +27 class _H_y_b_r_i_d_V_a_l_u_e_s; // forward declaration. │ │ │ │ │ +28 │ │ │ │ │ +62 template │ │ │ │ │ +_6_3 class _C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +64 { │ │ │ │ │ +65 protected: │ │ │ │ │ +_6_7 size_t _n_r_F_r_o_n_t_a_l_s__; │ │ │ │ │ +68 │ │ │ │ │ +69 private: │ │ │ │ │ +71 typedef _C_o_n_d_i_t_i_o_n_a_l_<_F_A_C_T_O_R_,_D_E_R_I_V_E_D_C_O_N_D_I_T_I_O_N_A_L_> _T_h_i_s; │ │ │ │ │ +72 │ │ │ │ │ +73 public: │ │ │ │ │ +_7_5 typedef boost::iterator_range _F_r_o_n_t_a_l_s; │ │ │ │ │ +76 │ │ │ │ │ +_7_8 typedef boost::iterator_range _P_a_r_e_n_t_s; │ │ │ │ │ +79 │ │ │ │ │ +80 protected: │ │ │ │ │ +83 │ │ │ │ │ +_8_5 _C_o_n_d_i_t_i_o_n_a_l() : _n_r_F_r_o_n_t_a_l_s__(0) {} │ │ │ │ │ 86 │ │ │ │ │ -87 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -88 template │ │ │ │ │ -89 boost::shared_ptr< │ │ │ │ │ -90 typename _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_G_R_A_P_H_>_:_:_B_a_y_e_s_T_r_e_e_T_y_p_e> │ │ │ │ │ -_9_1 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_G_R_A_P_H_>_:_:_e_l_i_m_i_n_a_t_e_M_u_l_t_i_f_r_o_n_t_a_l( │ │ │ │ │ -92 _O_p_t_i_o_n_a_l_O_r_d_e_r_i_n_g_T_y_p_e orderingType, const _E_l_i_m_i_n_a_t_e& function, │ │ │ │ │ -93 _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex) const { │ │ │ │ │ -94 if (!variableIndex) { │ │ │ │ │ -95 // If no VariableIndex provided, compute one and call this function again │ │ │ │ │ -96 // IMPORTANT: we check for no variable index first so that it's always │ │ │ │ │ -97 // computed if we need to call COLAMD because no Ordering is provided. │ │ │ │ │ -98 // When removing optional from VariableIndex, create VariableIndex before │ │ │ │ │ -99 // creating ordering. │ │ │ │ │ -100 _V_a_r_i_a_b_l_e_I_n_d_e_x computedVariableIndex(asDerived()); │ │ │ │ │ -101 return eliminateMultifrontal(orderingType, function, │ │ │ │ │ -102 computedVariableIndex); │ │ │ │ │ -103 } else { │ │ │ │ │ -104 // Compute an ordering and call this function again. We are guaranteed to │ │ │ │ │ -105 // have a VariableIndex already here because we computed one if needed in │ │ │ │ │ -106 // the previous 'if' block. │ │ │ │ │ -107 if (orderingType == Ordering::METIS) { │ │ │ │ │ -108 _O_r_d_e_r_i_n_g computedOrdering = _O_r_d_e_r_i_n_g_:_:_M_e_t_i_s(asDerived()); │ │ │ │ │ -109 return eliminateMultifrontal(computedOrdering, function, variableIndex); │ │ │ │ │ -110 } else if (orderingType == Ordering::COLAMD) { │ │ │ │ │ -111 _O_r_d_e_r_i_n_g computedOrdering = _O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d(*variableIndex); │ │ │ │ │ -112 return eliminateMultifrontal(computedOrdering, function, variableIndex); │ │ │ │ │ -113 } else if (orderingType == Ordering::NATURAL) { │ │ │ │ │ -114 _O_r_d_e_r_i_n_g computedOrdering = _O_r_d_e_r_i_n_g_:_:_N_a_t_u_r_a_l(asDerived()); │ │ │ │ │ -115 return eliminateMultifrontal(computedOrdering, function, variableIndex); │ │ │ │ │ -116 } else { │ │ │ │ │ -_1_1_7 _O_r_d_e_r_i_n_g computedOrdering = EliminationTraitsType::DefaultOrderingFunc( │ │ │ │ │ -118 asDerived(), variableIndex); │ │ │ │ │ -119 return eliminateMultifrontal(computedOrdering, function, variableIndex); │ │ │ │ │ -120 } │ │ │ │ │ +_8_8 _C_o_n_d_i_t_i_o_n_a_l(size_t _n_r_F_r_o_n_t_a_l_s) : _n_r_F_r_o_n_t_a_l_s__(_n_r_F_r_o_n_t_a_l_s) {} │ │ │ │ │ +89 │ │ │ │ │ +91 │ │ │ │ │ +92 public: │ │ │ │ │ +95 │ │ │ │ │ +97 void _p_r_i_n_t(const std::string& s = "Conditional", const _K_e_y_F_o_r_m_a_t_t_e_r& │ │ │ │ │ +formatter = DefaultKeyFormatter) const; │ │ │ │ │ +98 │ │ │ │ │ +100 bool _e_q_u_a_l_s(const This& c, double tol = 1e-9) const; │ │ │ │ │ +101 │ │ │ │ │ +103 │ │ │ │ │ +106 │ │ │ │ │ +107 virtual _~_C_o_n_d_i_t_i_o_n_a_l() {} │ │ │ │ │ +108 │ │ │ │ │ +_1_1_0 size_t _n_r_F_r_o_n_t_a_l_s() const { return _n_r_F_r_o_n_t_a_l_s__; } │ │ │ │ │ +111 │ │ │ │ │ +_1_1_3 size_t _n_r_P_a_r_e_n_t_s() const { return asFactor().size() - _n_r_F_r_o_n_t_a_l_s__; } │ │ │ │ │ +114 │ │ │ │ │ +_1_1_6 _K_e_y _f_i_r_s_t_F_r_o_n_t_a_l_K_e_y() const { │ │ │ │ │ +117 if(_n_r_F_r_o_n_t_a_l_s__ > 0) │ │ │ │ │ +118 return asFactor().front(); │ │ │ │ │ +119 else │ │ │ │ │ +120 throw std::invalid_argument("Requested Conditional::firstFrontalKey from a │ │ │ │ │ +conditional with zero frontal keys"); │ │ │ │ │ 121 } │ │ │ │ │ -122 } │ │ │ │ │ -123 │ │ │ │ │ -124 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -125 template │ │ │ │ │ -126 boost::shared_ptr:: │ │ │ │ │ -BayesTreeType> │ │ │ │ │ -_1_2_7 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_G_R_A_P_H_>_:_:_e_l_i_m_i_n_a_t_e_M_u_l_t_i_f_r_o_n_t_a_l( │ │ │ │ │ -128 const _O_r_d_e_r_i_n_g& ordering, const _E_l_i_m_i_n_a_t_e& function, │ │ │ │ │ -129 _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex) const │ │ │ │ │ -130 { │ │ │ │ │ -131 if(!variableIndex) { │ │ │ │ │ -132 // If no VariableIndex provided, compute one and call this function again │ │ │ │ │ -133 _V_a_r_i_a_b_l_e_I_n_d_e_x computedVariableIndex(asDerived()); │ │ │ │ │ -134 return eliminateMultifrontal(ordering, function, computedVariableIndex); │ │ │ │ │ -135 } else { │ │ │ │ │ -_1_3_6 gttic(eliminateMultifrontal); │ │ │ │ │ -137 // Do elimination with given ordering │ │ │ │ │ -138 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_T_y_p_e etree(asDerived(), *variableIndex, ordering); │ │ │ │ │ -139 _J_u_n_c_t_i_o_n_T_r_e_e_T_y_p_e junctionTree(etree); │ │ │ │ │ -140 boost::shared_ptr bayesTree; │ │ │ │ │ -141 boost::shared_ptr factorGraph; │ │ │ │ │ -142 boost::tie(bayesTree,factorGraph) = junctionTree.eliminate(function); │ │ │ │ │ -143 // If any factors are remaining, the ordering was incomplete │ │ │ │ │ -144 if(!factorGraph->empty()) │ │ │ │ │ -145 throw _I_n_c_o_n_s_i_s_t_e_n_t_E_l_i_m_i_n_a_t_i_o_n_R_e_q_u_e_s_t_e_d(); │ │ │ │ │ -146 // Return the Bayes tree │ │ │ │ │ -147 return bayesTree; │ │ │ │ │ -148 } │ │ │ │ │ -149 } │ │ │ │ │ -150 │ │ │ │ │ -151 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -152 template │ │ │ │ │ -153 std::pair::BayesNetType>, boost:: │ │ │ │ │ -shared_ptr > │ │ │ │ │ -_1_5_4 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_G_R_A_P_H_>_:_:_e_l_i_m_i_n_a_t_e_P_a_r_t_i_a_l_S_e_q_u_e_n_t_i_a_l( │ │ │ │ │ -155 const _O_r_d_e_r_i_n_g& ordering, const _E_l_i_m_i_n_a_t_e& function, _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ -variableIndex) const │ │ │ │ │ -156 { │ │ │ │ │ -_1_5_7 if(variableIndex) { │ │ │ │ │ -158 gttic(eliminatePartialSequential); │ │ │ │ │ -159 // Do elimination │ │ │ │ │ -160 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_T_y_p_e etree(asDerived(), *variableIndex, ordering); │ │ │ │ │ -161 return etree.eliminate(function); │ │ │ │ │ -162 } else { │ │ │ │ │ -163 // If no variable index is provided, compute one and call this function │ │ │ │ │ -again │ │ │ │ │ -164 _V_a_r_i_a_b_l_e_I_n_d_e_x computedVariableIndex(asDerived()); │ │ │ │ │ -165 return eliminatePartialSequential(ordering, function, │ │ │ │ │ -computedVariableIndex); │ │ │ │ │ -166 } │ │ │ │ │ -167 } │ │ │ │ │ -168 │ │ │ │ │ -169 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -170 template │ │ │ │ │ -_1_7_1 std::pair::BayesNetType>, boost:: │ │ │ │ │ -shared_ptr > │ │ │ │ │ -_1_7_2 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_G_R_A_P_H_>_:_:_e_l_i_m_i_n_a_t_e_P_a_r_t_i_a_l_S_e_q_u_e_n_t_i_a_l( │ │ │ │ │ -173 const _K_e_y_V_e_c_t_o_r& variables, const _E_l_i_m_i_n_a_t_e& function, │ │ │ │ │ -_O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex) const │ │ │ │ │ -174 { │ │ │ │ │ -175 if(variableIndex) { │ │ │ │ │ -176 gttic(eliminatePartialSequential); │ │ │ │ │ -177 // Compute full ordering │ │ │ │ │ -178 _O_r_d_e_r_i_n_g fullOrdering = _O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_F_i_r_s_t(*variableIndex, │ │ │ │ │ -variables); │ │ │ │ │ +122 │ │ │ │ │ +_1_2_4 _F_r_o_n_t_a_l_s _f_r_o_n_t_a_l_s() const { return boost::make_iterator_range(_b_e_g_i_n_F_r_o_n_t_a_l_s │ │ │ │ │ +(), _e_n_d_F_r_o_n_t_a_l_s()); } │ │ │ │ │ +125 │ │ │ │ │ +_1_2_7 _P_a_r_e_n_t_s _p_a_r_e_n_t_s() const { return boost::make_iterator_range(_b_e_g_i_n_P_a_r_e_n_t_s(), │ │ │ │ │ +_e_n_d_P_a_r_e_n_t_s()); } │ │ │ │ │ +128 │ │ │ │ │ +133 virtual double _l_o_g_P_r_o_b_a_b_i_l_i_t_y(const _H_y_b_r_i_d_V_a_l_u_e_s& c) const; │ │ │ │ │ +134 │ │ │ │ │ +139 virtual double _e_v_a_l_u_a_t_e(const _H_y_b_r_i_d_V_a_l_u_e_s& c) const; │ │ │ │ │ +140 │ │ │ │ │ +_1_4_2 double _o_p_e_r_a_t_o_r_(_)(const _H_y_b_r_i_d_V_a_l_u_e_s& x) const { │ │ │ │ │ +143 return _e_v_a_l_u_a_t_e(x); │ │ │ │ │ +144 } │ │ │ │ │ +145 │ │ │ │ │ +150 virtual double _l_o_g_N_o_r_m_a_l_i_z_a_t_i_o_n_C_o_n_s_t_a_n_t() const; │ │ │ │ │ +151 │ │ │ │ │ +153 double _n_o_r_m_a_l_i_z_a_t_i_o_n_C_o_n_s_t_a_n_t() const; │ │ │ │ │ +154 │ │ │ │ │ +158 │ │ │ │ │ +_1_6_0 typename FACTOR::const_iterator _b_e_g_i_n_F_r_o_n_t_a_l_s() const { return asFactor │ │ │ │ │ +().begin(); } │ │ │ │ │ +161 │ │ │ │ │ +_1_6_3 typename FACTOR::const_iterator _e_n_d_F_r_o_n_t_a_l_s() const { return asFactor │ │ │ │ │ +().begin() + _n_r_F_r_o_n_t_a_l_s__; } │ │ │ │ │ +164 │ │ │ │ │ +_1_6_6 typename FACTOR::const_iterator _b_e_g_i_n_P_a_r_e_n_t_s() const { return _e_n_d_F_r_o_n_t_a_l_s │ │ │ │ │ +(); } │ │ │ │ │ +167 │ │ │ │ │ +_1_6_9 typename FACTOR::const_iterator _e_n_d_P_a_r_e_n_t_s() const { return asFactor().end │ │ │ │ │ +(); } │ │ │ │ │ +170 │ │ │ │ │ +_1_7_2 size_t& _n_r_F_r_o_n_t_a_l_s() { return _n_r_F_r_o_n_t_a_l_s__; } │ │ │ │ │ +173 │ │ │ │ │ +_1_7_5 typename FACTOR::iterator _b_e_g_i_n_F_r_o_n_t_a_l_s() { return asFactor().begin(); } │ │ │ │ │ +176 │ │ │ │ │ +_1_7_8 typename FACTOR::iterator _e_n_d_F_r_o_n_t_a_l_s() { return asFactor().begin() + │ │ │ │ │ +_n_r_F_r_o_n_t_a_l_s__; } │ │ │ │ │ 179 │ │ │ │ │ -180 // Split off the part of the ordering for the variables being eliminated │ │ │ │ │ -_1_8_1 _O_r_d_e_r_i_n_g ordering(fullOrdering.begin(), fullOrdering.begin() + │ │ │ │ │ -variables.size()); │ │ │ │ │ -182 return eliminatePartialSequential(ordering, function, variableIndex); │ │ │ │ │ -183 } else { │ │ │ │ │ -184 // If no variable index is provided, compute one and call this function │ │ │ │ │ -again │ │ │ │ │ -185 _V_a_r_i_a_b_l_e_I_n_d_e_x computedVariableIndex(asDerived()); │ │ │ │ │ -186 return eliminatePartialSequential(variables, function, │ │ │ │ │ -computedVariableIndex); │ │ │ │ │ -187 } │ │ │ │ │ -188 } │ │ │ │ │ -189 │ │ │ │ │ -190 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -_1_9_1 template │ │ │ │ │ -192 std::pair::BayesTreeType>, boost:: │ │ │ │ │ -shared_ptr > │ │ │ │ │ -_1_9_3 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_G_R_A_P_H_>_:_:_e_l_i_m_i_n_a_t_e_P_a_r_t_i_a_l_M_u_l_t_i_f_r_o_n_t_a_l( │ │ │ │ │ -194 const _O_r_d_e_r_i_n_g& ordering, const _E_l_i_m_i_n_a_t_e& function, _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ -variableIndex) const │ │ │ │ │ -195 { │ │ │ │ │ -196 if(variableIndex) { │ │ │ │ │ -197 gttic(eliminatePartialMultifrontal); │ │ │ │ │ -198 // Do elimination │ │ │ │ │ -199 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_T_y_p_e etree(asDerived(), *variableIndex, ordering); │ │ │ │ │ -200 _J_u_n_c_t_i_o_n_T_r_e_e_T_y_p_e junctionTree(etree); │ │ │ │ │ -_2_0_1 return junctionTree.eliminate(function); │ │ │ │ │ -202 } else { │ │ │ │ │ -203 // If no variable index is provided, compute one and call this function │ │ │ │ │ -again │ │ │ │ │ -204 _V_a_r_i_a_b_l_e_I_n_d_e_x computedVariableIndex(asDerived()); │ │ │ │ │ -205 return eliminatePartialMultifrontal(ordering, function, │ │ │ │ │ -computedVariableIndex); │ │ │ │ │ -206 } │ │ │ │ │ -207 } │ │ │ │ │ -208 │ │ │ │ │ -209 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -210 template │ │ │ │ │ -_2_1_1 std::pair::BayesTreeType>, boost:: │ │ │ │ │ -shared_ptr > │ │ │ │ │ -_2_1_2 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_G_R_A_P_H_>_:_:_e_l_i_m_i_n_a_t_e_P_a_r_t_i_a_l_M_u_l_t_i_f_r_o_n_t_a_l( │ │ │ │ │ -213 const _K_e_y_V_e_c_t_o_r& variables, const _E_l_i_m_i_n_a_t_e& function, │ │ │ │ │ -_O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex) const │ │ │ │ │ -214 { │ │ │ │ │ -215 if(variableIndex) { │ │ │ │ │ -216 gttic(eliminatePartialMultifrontal); │ │ │ │ │ -217 // Compute full ordering │ │ │ │ │ -218 _O_r_d_e_r_i_n_g fullOrdering = _O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_F_i_r_s_t(*variableIndex, │ │ │ │ │ -variables); │ │ │ │ │ -219 │ │ │ │ │ -220 // Split off the part of the ordering for the variables being eliminated │ │ │ │ │ -221 _O_r_d_e_r_i_n_g ordering(fullOrdering.begin(), fullOrdering.begin() + │ │ │ │ │ -variables.size()); │ │ │ │ │ -222 return eliminatePartialMultifrontal(ordering, function, variableIndex); │ │ │ │ │ -223 } else { │ │ │ │ │ -224 // If no variable index is provided, compute one and call this function │ │ │ │ │ -again │ │ │ │ │ -_2_2_5 _V_a_r_i_a_b_l_e_I_n_d_e_x computedVariableIndex(asDerived()); │ │ │ │ │ -226 return eliminatePartialMultifrontal(variables, function, │ │ │ │ │ -computedVariableIndex); │ │ │ │ │ -227 } │ │ │ │ │ -228 } │ │ │ │ │ -229 │ │ │ │ │ -230 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -231 template │ │ │ │ │ -232 boost::shared_ptr:: │ │ │ │ │ -BayesNetType> │ │ │ │ │ -_2_3_3 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_G_R_A_P_H_>_:_:_m_a_r_g_i_n_a_l_M_u_l_t_i_f_r_o_n_t_a_l_B_a_y_e_s_N_e_t( │ │ │ │ │ -234 boost::variant variables, │ │ │ │ │ -235 const _E_l_i_m_i_n_a_t_e& function, _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex) const │ │ │ │ │ -236 { │ │ │ │ │ -237 if(!variableIndex) { │ │ │ │ │ -238 // If no variable index is provided, compute one and call this function │ │ │ │ │ -again │ │ │ │ │ -239 _V_a_r_i_a_b_l_e_I_n_d_e_x index(asDerived()); │ │ │ │ │ -_2_4_0 return _m_a_r_g_i_n_a_l_M_u_l_t_i_f_r_o_n_t_a_l_B_a_y_e_s_N_e_t(variables, function, index); │ │ │ │ │ -241 } else { │ │ │ │ │ -242 // No ordering was provided for the marginalized variables, so order them │ │ │ │ │ -using constrained │ │ │ │ │ -243 // COLAMD. │ │ │ │ │ -244 bool unmarginalizedAreOrdered = (boost::get(&variables) != │ │ │ │ │ -0); │ │ │ │ │ -245 const _K_e_y_V_e_c_t_o_r* variablesOrOrdering = │ │ │ │ │ -246 unmarginalizedAreOrdered ? │ │ │ │ │ -247 boost::get(&variables) : boost::get │ │ │ │ │ -(&variables); │ │ │ │ │ -248 │ │ │ │ │ -249 _O_r_d_e_r_i_n_g totalOrdering = │ │ │ │ │ -250 _O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_L_a_s_t(*variableIndex, *variablesOrOrdering, │ │ │ │ │ -unmarginalizedAreOrdered); │ │ │ │ │ -251 │ │ │ │ │ -252 // Split up ordering │ │ │ │ │ -253 const size_t nVars = variablesOrOrdering->size(); │ │ │ │ │ -254 _O_r_d_e_r_i_n_g marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() │ │ │ │ │ -- nVars); │ │ │ │ │ -_2_5_5 _O_r_d_e_r_i_n_g marginalVarsOrdering(totalOrdering.end() - nVars, │ │ │ │ │ -totalOrdering.end()); │ │ │ │ │ -256 │ │ │ │ │ -257 // Call this function again with the computed orderings │ │ │ │ │ -258 return marginalMultifrontalBayesNet(marginalVarsOrdering, │ │ │ │ │ -marginalizationOrdering, function, *variableIndex); │ │ │ │ │ -259 } │ │ │ │ │ -260 } │ │ │ │ │ -261 │ │ │ │ │ -262 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -263 template │ │ │ │ │ -264 boost::shared_ptr:: │ │ │ │ │ -BayesNetType> │ │ │ │ │ -_2_6_5 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_G_R_A_P_H_>_:_:_m_a_r_g_i_n_a_l_M_u_l_t_i_f_r_o_n_t_a_l_B_a_y_e_s_N_e_t( │ │ │ │ │ -266 boost::variant variables, │ │ │ │ │ -267 const _O_r_d_e_r_i_n_g& marginalizedVariableOrdering, │ │ │ │ │ -268 const _E_l_i_m_i_n_a_t_e& function, _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex) const │ │ │ │ │ -269 { │ │ │ │ │ -_2_7_0 if(!variableIndex) { │ │ │ │ │ -271 // If no variable index is provided, compute one and call this function │ │ │ │ │ -again │ │ │ │ │ -272 _V_a_r_i_a_b_l_e_I_n_d_e_x index(asDerived()); │ │ │ │ │ -273 return marginalMultifrontalBayesNet(variables, │ │ │ │ │ -marginalizedVariableOrdering, function, index); │ │ │ │ │ -274 } else { │ │ │ │ │ -275 gttic(marginalMultifrontalBayesNet); │ │ │ │ │ -276 // An ordering was provided for the marginalized variables, so we can first │ │ │ │ │ -eliminate them │ │ │ │ │ -_2_7_7 // in the order requested. │ │ │ │ │ -278 boost::shared_ptr bayesTree; │ │ │ │ │ -279 boost::shared_ptr factorGraph; │ │ │ │ │ -280 boost::tie(bayesTree,factorGraph) = │ │ │ │ │ -281 eliminatePartialMultifrontal(marginalizedVariableOrdering, function, │ │ │ │ │ -*variableIndex); │ │ │ │ │ -282 │ │ │ │ │ -283 if(const _O_r_d_e_r_i_n_g* varsAsOrdering = boost::get │ │ │ │ │ -(&variables)) │ │ │ │ │ -284 { │ │ │ │ │ -285 // An ordering was also provided for the unmarginalized variables, so we │ │ │ │ │ -can also │ │ │ │ │ -286 // eliminate them in the order requested. │ │ │ │ │ -287 return factorGraph->eliminateSequential(*varsAsOrdering, function); │ │ │ │ │ -288 } │ │ │ │ │ -289 else │ │ │ │ │ -290 { │ │ │ │ │ -291 // No ordering was provided for the unmarginalized variables, so order them │ │ │ │ │ -with COLAMD. │ │ │ │ │ -292 return factorGraph->eliminateSequential(Ordering::COLAMD, function); │ │ │ │ │ -293 } │ │ │ │ │ -294 } │ │ │ │ │ -295 } │ │ │ │ │ -296 │ │ │ │ │ -297 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -298 template │ │ │ │ │ -299 boost::shared_ptr:: │ │ │ │ │ -BayesTreeType> │ │ │ │ │ -_3_0_0 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_G_R_A_P_H_>_:_:_m_a_r_g_i_n_a_l_M_u_l_t_i_f_r_o_n_t_a_l_B_a_y_e_s_T_r_e_e( │ │ │ │ │ -301 boost::variant variables, │ │ │ │ │ -302 const _E_l_i_m_i_n_a_t_e& function, _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex) const │ │ │ │ │ -303 { │ │ │ │ │ -304 if(!variableIndex) { │ │ │ │ │ -305 // If no variable index is provided, compute one and call this function │ │ │ │ │ -again │ │ │ │ │ -306 _V_a_r_i_a_b_l_e_I_n_d_e_x computedVariableIndex(asDerived()); │ │ │ │ │ -307 return marginalMultifrontalBayesTree(variables, function, │ │ │ │ │ -computedVariableIndex); │ │ │ │ │ -308 } else { │ │ │ │ │ -309 // No ordering was provided for the marginalized variables, so order them │ │ │ │ │ -using constrained │ │ │ │ │ -310 // COLAMD. │ │ │ │ │ -311 bool unmarginalizedAreOrdered = (boost::get(&variables) != │ │ │ │ │ -0); │ │ │ │ │ -312 const _K_e_y_V_e_c_t_o_r* variablesOrOrdering = │ │ │ │ │ -313 unmarginalizedAreOrdered ? │ │ │ │ │ -314 boost::get(&variables) : boost::get │ │ │ │ │ -(&variables); │ │ │ │ │ -315 │ │ │ │ │ -316 _O_r_d_e_r_i_n_g totalOrdering = │ │ │ │ │ -317 _O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_L_a_s_t(*variableIndex, *variablesOrOrdering, │ │ │ │ │ -unmarginalizedAreOrdered); │ │ │ │ │ -318 │ │ │ │ │ -319 // Split up ordering │ │ │ │ │ -320 const size_t nVars = variablesOrOrdering->size(); │ │ │ │ │ -321 _O_r_d_e_r_i_n_g marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() │ │ │ │ │ -- nVars); │ │ │ │ │ -322 _O_r_d_e_r_i_n_g marginalVarsOrdering(totalOrdering.end() - nVars, │ │ │ │ │ -totalOrdering.end()); │ │ │ │ │ -323 │ │ │ │ │ -324 // Call this function again with the computed orderings │ │ │ │ │ -325 return marginalMultifrontalBayesTree(marginalVarsOrdering, │ │ │ │ │ -marginalizationOrdering, function, *variableIndex); │ │ │ │ │ -326 } │ │ │ │ │ -327 } │ │ │ │ │ -328 │ │ │ │ │ -329 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -330 template │ │ │ │ │ -331 boost::shared_ptr:: │ │ │ │ │ -BayesTreeType> │ │ │ │ │ -_3_3_2 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_G_R_A_P_H_>_:_:_m_a_r_g_i_n_a_l_M_u_l_t_i_f_r_o_n_t_a_l_B_a_y_e_s_T_r_e_e( │ │ │ │ │ -333 boost::variant variables, │ │ │ │ │ -334 const _O_r_d_e_r_i_n_g& marginalizedVariableOrdering, │ │ │ │ │ -335 const _E_l_i_m_i_n_a_t_e& function, _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex) const │ │ │ │ │ -336 { │ │ │ │ │ -337 if(!variableIndex) { │ │ │ │ │ -338 // If no variable index is provided, compute one and call this function │ │ │ │ │ -again │ │ │ │ │ -339 _V_a_r_i_a_b_l_e_I_n_d_e_x computedVariableIndex(asDerived()); │ │ │ │ │ -340 return marginalMultifrontalBayesTree(variables, │ │ │ │ │ -marginalizedVariableOrdering, function, computedVariableIndex); │ │ │ │ │ -341 } else { │ │ │ │ │ -342 gttic(marginalMultifrontalBayesTree); │ │ │ │ │ -343 // An ordering was provided for the marginalized variables, so we can first │ │ │ │ │ -eliminate them │ │ │ │ │ -344 // in the order requested. │ │ │ │ │ -345 boost::shared_ptr bayesTree; │ │ │ │ │ -346 boost::shared_ptr factorGraph; │ │ │ │ │ -347 boost::tie(bayesTree,factorGraph) = │ │ │ │ │ -348 eliminatePartialMultifrontal(marginalizedVariableOrdering, function, │ │ │ │ │ -*variableIndex); │ │ │ │ │ -349 │ │ │ │ │ -350 if(const _O_r_d_e_r_i_n_g* varsAsOrdering = boost::get │ │ │ │ │ -(&variables)) │ │ │ │ │ -351 { │ │ │ │ │ -352 // An ordering was also provided for the unmarginalized variables, so we │ │ │ │ │ -can also │ │ │ │ │ -353 // eliminate them in the order requested. │ │ │ │ │ -354 return factorGraph->eliminateMultifrontal(*varsAsOrdering, function); │ │ │ │ │ -355 } │ │ │ │ │ -356 else │ │ │ │ │ -357 { │ │ │ │ │ -358 // No ordering was provided for the unmarginalized variables, so order them │ │ │ │ │ -with COLAMD. │ │ │ │ │ -359 return factorGraph->eliminateMultifrontal(Ordering::COLAMD, function); │ │ │ │ │ -360 } │ │ │ │ │ -361 } │ │ │ │ │ -362 } │ │ │ │ │ -363 │ │ │ │ │ -364 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -365 template │ │ │ │ │ -366 boost::shared_ptr │ │ │ │ │ -_3_6_7 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_G_R_A_P_H_>_:_:_m_a_r_g_i_n_a_l( │ │ │ │ │ -368 const _K_e_y_V_e_c_t_o_r& variables, │ │ │ │ │ -369 const _E_l_i_m_i_n_a_t_e& function, _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex) const │ │ │ │ │ -370 { │ │ │ │ │ -371 if(variableIndex) │ │ │ │ │ -372 { │ │ │ │ │ -373 // Compute a total ordering for all variables │ │ │ │ │ -374 _O_r_d_e_r_i_n_g totalOrdering = _O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_L_a_s_t(*variableIndex, │ │ │ │ │ -variables); │ │ │ │ │ -375 │ │ │ │ │ -376 // Split out the part for the marginalized variables │ │ │ │ │ -377 _O_r_d_e_r_i_n_g marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() │ │ │ │ │ -- variables.size()); │ │ │ │ │ -378 │ │ │ │ │ -379 // Eliminate and return the remaining factor graph │ │ │ │ │ -380 return eliminatePartialMultifrontal(marginalizationOrdering, function, │ │ │ │ │ -*variableIndex).second; │ │ │ │ │ -381 } │ │ │ │ │ -382 else │ │ │ │ │ -383 { │ │ │ │ │ -384 // If no variable index is provided, compute one and call this function │ │ │ │ │ -again │ │ │ │ │ -385 _V_a_r_i_a_b_l_e_I_n_d_e_x computedVariableIndex(asDerived()); │ │ │ │ │ -386 return marginal(variables, function, computedVariableIndex); │ │ │ │ │ -387 } │ │ │ │ │ -388 } │ │ │ │ │ -389 │ │ │ │ │ -390 │ │ │ │ │ -391} │ │ │ │ │ -_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Variable elimination algorithms for factor graphs. │ │ │ │ │ -_i_n_f_e_r_e_n_c_e_E_x_c_e_p_t_i_o_n_s_._h │ │ │ │ │ -Exceptions that may be thrown by inference algorithms. │ │ │ │ │ +_1_8_1 typename FACTOR::iterator _b_e_g_i_n_P_a_r_e_n_t_s() { return asFactor().begin() + │ │ │ │ │ +_n_r_F_r_o_n_t_a_l_s__; } │ │ │ │ │ +182 │ │ │ │ │ +_1_8_4 typename FACTOR::iterator _e_n_d_P_a_r_e_n_t_s() { return asFactor().end(); } │ │ │ │ │ +185 │ │ │ │ │ +199 template │ │ │ │ │ +200 static bool _C_h_e_c_k_I_n_v_a_r_i_a_n_t_s(const DERIVEDCONDITIONAL& conditional, │ │ │ │ │ +201 const VALUES& x); │ │ │ │ │ +202 │ │ │ │ │ +204 │ │ │ │ │ +205 private: │ │ │ │ │ +206 │ │ │ │ │ +209 │ │ │ │ │ +210 // Cast to factor type (non-const) (casts down to derived conditional type, │ │ │ │ │ +then up to factor type) │ │ │ │ │ +211 FACTOR& asFactor() { return static_cast │ │ │ │ │ +(static_cast(*this)); } │ │ │ │ │ +212 │ │ │ │ │ +213 // Cast to derived type (const) (casts down to derived conditional type, │ │ │ │ │ +then up to factor type) │ │ │ │ │ +214 const FACTOR& asFactor() const { return static_cast │ │ │ │ │ +(static_cast(*this)); } │ │ │ │ │ +215 │ │ │ │ │ +_2_1_7 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +218 template │ │ │ │ │ +219 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +220 ar & BOOST_SERIALIZATION_NVP(_n_r_F_r_o_n_t_a_l_s__); │ │ │ │ │ +221 } │ │ │ │ │ +222 │ │ │ │ │ +224 │ │ │ │ │ +225 }; │ │ │ │ │ +226 │ │ │ │ │ +227} // gtsam │ │ │ │ │ +_K_e_y_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_m_a_r_g_i_n_a_l_M_u_l_t_i_f_r_o_n_t_a_l_B_a_y_e_s_T_r_e_e │ │ │ │ │ -boost::shared_ptr< BayesTreeType > marginalMultifrontalBayesTree(boost:: │ │ │ │ │ -variant< const Ordering &, const KeyVector & > variables, const Eliminate │ │ │ │ │ -&function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex │ │ │ │ │ -variableIndex=boost::none) const │ │ │ │ │ -Compute the marginal of the requested variables and return the result as a │ │ │ │ │ -Bayes tree. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph-inst.h:300 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_J_u_n_c_t_i_o_n_T_r_e_e_T_y_p_e │ │ │ │ │ -EliminationTraitsType::JunctionTreeType JunctionTreeType │ │ │ │ │ -Junction tree type that can do multifrontal elimination of this graph. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph.h:82 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_m_a_r_g_i_n_a_l │ │ │ │ │ -boost::shared_ptr< FactorGraphType > marginal(const KeyVector &variables, const │ │ │ │ │ -Eliminate &function=EliminationTraitsType::DefaultEliminate, │ │ │ │ │ -OptionalVariableIndex variableIndex=boost::none) const │ │ │ │ │ -Compute the marginal factor graph of the requested variables. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph-inst.h:367 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_m_a_r_g_i_n_a_l_M_u_l_t_i_f_r_o_n_t_a_l_B_a_y_e_s_N_e_t │ │ │ │ │ -boost::shared_ptr< BayesNetType > marginalMultifrontalBayesNet(boost::variant< │ │ │ │ │ -const Ordering &, const KeyVector & > variables, const Ordering │ │ │ │ │ -&marginalizedVariableOrdering, const Eliminate │ │ │ │ │ -&function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex │ │ │ │ │ -variableIndex=boost::none) const │ │ │ │ │ -Compute the marginal of the requested variables and return the result as a │ │ │ │ │ -Bayes net. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph-inst.h:265 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_E_l_i_m_i_n_a_t_e │ │ │ │ │ -std::function< EliminationResult(const FactorGraphType &, const Ordering &)> │ │ │ │ │ -Eliminate │ │ │ │ │ -The function type that does a single dense elimination step on a subgraph. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph.h:89 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_B_a_y_e_s_T_r_e_e_T_y_p_e │ │ │ │ │ -EliminationTraitsType::BayesTreeType BayesTreeType │ │ │ │ │ -Bayes tree type produced by multifrontal elimination. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph.h:79 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_e_l_i_m_i_n_a_t_e_S_e_q_u_e_n_t_i_a_l │ │ │ │ │ -boost::shared_ptr< BayesNetType > eliminateSequential(OptionalOrderingType │ │ │ │ │ -orderingType=boost::none, const Eliminate &function=EliminationTraitsType:: │ │ │ │ │ -DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const │ │ │ │ │ -Do sequential elimination of all variables to produce a Bayes net. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph-inst.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_e_l_i_m_i_n_a_t_e_M_u_l_t_i_f_r_o_n_t_a_l │ │ │ │ │ -boost::shared_ptr< BayesTreeType > eliminateMultifrontal(OptionalOrderingType │ │ │ │ │ -orderingType=boost::none, const Eliminate &function=EliminationTraitsType:: │ │ │ │ │ -DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const │ │ │ │ │ -Do multifrontal elimination of all variables to produce a Bayes tree. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph-inst.h:91 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_T_y_p_e │ │ │ │ │ -EliminationTraitsType::EliminationTreeType EliminationTreeType │ │ │ │ │ -Elimination tree type that can do sequential elimination of this graph. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph.h:76 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ -boost::optional< const VariableIndex & > OptionalVariableIndex │ │ │ │ │ -Typedef for an optional variable index as an argument to elimination functions. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph.h:92 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_e_l_i_m_i_n_a_t_e_P_a_r_t_i_a_l_S_e_q_u_e_n_t_i_a_l │ │ │ │ │ -std::pair< boost::shared_ptr< BayesNetType >, boost::shared_ptr< │ │ │ │ │ -FactorGraphType > > eliminatePartialSequential(const Ordering &ordering, const │ │ │ │ │ -Eliminate &function=EliminationTraitsType::DefaultEliminate, │ │ │ │ │ -OptionalVariableIndex variableIndex=boost::none) const │ │ │ │ │ -Do sequential elimination of some variables, in ordering provided, to produce a │ │ │ │ │ -Bayes net and a remai... │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph-inst.h:154 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_m_a_r_g_i_n_a_l_M_u_l_t_i_f_r_o_n_t_a_l_B_a_y_e_s_N_e_t │ │ │ │ │ -boost::shared_ptr< BayesNetType > marginalMultifrontalBayesNet(boost::variant< │ │ │ │ │ -const Ordering &, const KeyVector & > variables, const Eliminate │ │ │ │ │ -&function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex │ │ │ │ │ -variableIndex=boost::none) const │ │ │ │ │ -Compute the marginal of the requested variables and return the result as a │ │ │ │ │ -Bayes net. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph-inst.h:233 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_e_l_i_m_i_n_a_t_e_P_a_r_t_i_a_l_M_u_l_t_i_f_r_o_n_t_a_l │ │ │ │ │ -std::pair< boost::shared_ptr< BayesTreeType >, boost::shared_ptr< │ │ │ │ │ -FactorGraphType > > eliminatePartialMultifrontal(const Ordering &ordering, │ │ │ │ │ -const Eliminate &function=EliminationTraitsType::DefaultEliminate, │ │ │ │ │ -OptionalVariableIndex variableIndex=boost::none) const │ │ │ │ │ -Do multifrontal elimination of some variables, in ordering provided, to produce │ │ │ │ │ -a Bayes tree and a re... │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph-inst.h:193 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_O_p_t_i_o_n_a_l_O_r_d_e_r_i_n_g_T_y_p_e │ │ │ │ │ -boost::optional< Ordering::OrderingType > OptionalOrderingType │ │ │ │ │ -Typedef for an optional ordering type. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph.h:95 │ │ │ │ │ -_g_t_s_a_m_:_:_I_n_c_o_n_s_i_s_t_e_n_t_E_l_i_m_i_n_a_t_i_o_n_R_e_q_u_e_s_t_e_d │ │ │ │ │ -An inference algorithm was called with inconsistent arguments. │ │ │ │ │ -DDeeffiinniittiioonn inferenceExceptions.h:29 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_N_a_t_u_r_a_l │ │ │ │ │ -static Ordering Natural(const FACTOR_GRAPH &fg) │ │ │ │ │ -Return a natural Ordering. Typically used by iterative solvers. │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:190 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d │ │ │ │ │ -static Ordering Colamd(const FACTOR_GRAPH &graph) │ │ │ │ │ -Compute a fill-reducing ordering using COLAMD from a factor graph (see details │ │ │ │ │ -for note on performanc... │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:95 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_L_a_s_t │ │ │ │ │ -static Ordering ColamdConstrainedLast(const FACTOR_GRAPH &graph, const │ │ │ │ │ -KeyVector &constrainLast, bool forceOrder=false) │ │ │ │ │ -Compute a fill-reducing ordering using constrained COLAMD from a factor graph │ │ │ │ │ -(see details for note o... │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:114 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_F_i_r_s_t │ │ │ │ │ -static Ordering ColamdConstrainedFirst(const FACTOR_GRAPH &graph, const │ │ │ │ │ -KeyVector &constrainFirst, bool forceOrder=false) │ │ │ │ │ -Compute a fill-reducing ordering using constrained COLAMD from a factor graph │ │ │ │ │ -(see details for note o... │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:141 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_M_e_t_i_s │ │ │ │ │ -static GTSAM_EXPORT Ordering Metis(const MetisIndex &met) │ │ │ │ │ -Compute an ordering determined by METIS from a VariableIndex. │ │ │ │ │ -DDeeffiinniittiioonn Ordering.cpp:212 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ -The VariableIndex class computes and stores the block column structure of a │ │ │ │ │ -factor graph. │ │ │ │ │ -DDeeffiinniittiioonn VariableIndex.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_C_h_e_c_k_I_n_v_a_r_i_a_n_t_s │ │ │ │ │ +static bool CheckInvariants(const DERIVEDCONDITIONAL &conditional, const VALUES │ │ │ │ │ +&x) │ │ │ │ │ +Check invariants of this conditional, given the values x. │ │ │ │ │ +DDeeffiinniittiioonn Conditional-inst.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_n_r_F_r_o_n_t_a_l_s__ │ │ │ │ │ +size_t nrFrontals_ │ │ │ │ │ +The first nrFrontal variables are frontal and the rest are parents. │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_l_o_g_N_o_r_m_a_l_i_z_a_t_i_o_n_C_o_n_s_t_a_n_t │ │ │ │ │ +virtual double logNormalizationConstant() const │ │ │ │ │ +All conditional types need to implement a log normalization constant to make it │ │ │ │ │ +such that error>=0. │ │ │ │ │ +DDeeffiinniittiioonn Conditional-inst.h:62 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_f_i_r_s_t_F_r_o_n_t_a_l_K_e_y │ │ │ │ │ +Key firstFrontalKey() const │ │ │ │ │ +Convenience function to get the first frontal key. │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:116 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_e_n_d_F_r_o_n_t_a_l_s │ │ │ │ │ +FACTOR::const_iterator endFrontals() const │ │ │ │ │ +Iterator pointing past the last frontal key. │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:163 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_e_n_d_P_a_r_e_n_t_s │ │ │ │ │ +FACTOR::iterator endParents() │ │ │ │ │ +Mutable iterator pointing past the last parent key. │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:184 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_e_n_d_F_r_o_n_t_a_l_s │ │ │ │ │ +FACTOR::iterator endFrontals() │ │ │ │ │ +Mutable iterator pointing past the last frontal key. │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:178 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_e_v_a_l_u_a_t_e │ │ │ │ │ +virtual double evaluate(const HybridValues &c) const │ │ │ │ │ +All conditional types need to implement an evaluate function, that yields a │ │ │ │ │ +true probability. │ │ │ │ │ +DDeeffiinniittiioonn Conditional-inst.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_p_a_r_e_n_t_s │ │ │ │ │ +Parents parents() const │ │ │ │ │ +return a view of the parent keys │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:127 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ +double operator()(const HybridValues &x) const │ │ │ │ │ +Evaluate probability density, sugar. │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:142 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_P_a_r_e_n_t_s │ │ │ │ │ +boost::iterator_range< typename FACTOR::const_iterator > Parents │ │ │ │ │ +View of the separator keys (call parents()) │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:78 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_l_o_g_P_r_o_b_a_b_i_l_i_t_y │ │ │ │ │ +virtual double logProbability(const HybridValues &c) const │ │ │ │ │ +All conditional types need to implement a logProbability function, for which │ │ │ │ │ +exp(logProbability(x)) =... │ │ │ │ │ +DDeeffiinniittiioonn Conditional-inst.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_n_r_F_r_o_n_t_a_l_s │ │ │ │ │ +size_t nrFrontals() const │ │ │ │ │ +return the number of frontals │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:110 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +Conditional() │ │ │ │ │ +Empty Constructor to make serialization possible. │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +Conditional(size_t nrFrontals) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:88 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_b_e_g_i_n_P_a_r_e_n_t_s │ │ │ │ │ +FACTOR::iterator beginParents() │ │ │ │ │ +Mutable iterator pointing to the first parent key. │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:181 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_n_r_F_r_o_n_t_a_l_s │ │ │ │ │ +size_t & nrFrontals() │ │ │ │ │ +Mutable version of nrFrontals. │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:172 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_b_e_g_i_n_F_r_o_n_t_a_l_s │ │ │ │ │ +FACTOR::iterator beginFrontals() │ │ │ │ │ +Mutable iterator pointing to first frontal key. │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:175 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_F_r_o_n_t_a_l_s │ │ │ │ │ +boost::iterator_range< typename FACTOR::const_iterator > Frontals │ │ │ │ │ +View of the frontal keys (call frontals()) │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_f_r_o_n_t_a_l_s │ │ │ │ │ +Frontals frontals() const │ │ │ │ │ +return a view of the frontal keys │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:124 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:217 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_b_e_g_i_n_F_r_o_n_t_a_l_s │ │ │ │ │ +FACTOR::const_iterator beginFrontals() const │ │ │ │ │ +Iterator pointing to first frontal key. │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:160 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_n_o_r_m_a_l_i_z_a_t_i_o_n_C_o_n_s_t_a_n_t │ │ │ │ │ +double normalizationConstant() const │ │ │ │ │ +Non-virtual, exponentiate logNormalizationConstant. │ │ │ │ │ +DDeeffiinniittiioonn Conditional-inst.h:70 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_n_r_P_a_r_e_n_t_s │ │ │ │ │ +size_t nrParents() const │ │ │ │ │ +return the number of parents │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:113 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="Conditional", const KeyFormatter │ │ │ │ │ +&formatter=DefaultKeyFormatter) const │ │ │ │ │ +print with optional formatter │ │ │ │ │ +DDeeffiinniittiioonn Conditional-inst.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_e_n_d_P_a_r_e_n_t_s │ │ │ │ │ +FACTOR::const_iterator endParents() const │ │ │ │ │ +Iterator pointing past the last parent key. │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:169 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_b_e_g_i_n_P_a_r_e_n_t_s │ │ │ │ │ +FACTOR::const_iterator beginParents() const │ │ │ │ │ +Iterator pointing to the first parent key. │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:166 │ │ │ │ │ +_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +the error. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * EElliimmiinnaatteeaabblleeFFaaccttoorrGGrraapphh--iinnsstt..hh │ │ │ │ │ + * _C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00608_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/graph-inl.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Conditional-inst.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,338 +98,143 @@ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
graph-inl.h
│ │ │ │ +
Conditional-inst.h
│ │ │ │
│ │ │ │
│ │ │ │
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
12/*
│ │ │ │ -
13 * @file graph-inl.h
│ │ │ │ -
14 * @brief Graph algorithm using boost library
│ │ │ │ -
15 * @author Kai Ni
│ │ │ │ -
16 */
│ │ │ │ -
17
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <stdexcept>
│ │ │ │ -
21#ifdef __GNUC__
│ │ │ │ -
22#pragma GCC diagnostic push
│ │ │ │ -
23#pragma GCC diagnostic ignored "-Wunused-variable"
│ │ │ │ -
24//#pragma GCC diagnostic ignored "-Wunneeded-internal-declaration"
│ │ │ │ -
25#endif
│ │ │ │ -
26#include <boost/graph/breadth_first_search.hpp>
│ │ │ │ -
27#ifdef __GNUC__
│ │ │ │ -
28#pragma GCC diagnostic pop
│ │ │ │ -
29#endif
│ │ │ │ -
30#include <boost/graph/prim_minimum_spanning_tree.hpp>
│ │ │ │ -
31
│ │ │ │ -
32#include <gtsam/inference/graph.h>
│ │ │ │ -
33
│ │ │ │ -
34namespace gtsam {
│ │ │ │ -
35
│ │ │ │ -
36/* ************************************************************************* */
│ │ │ │ -
37template <class KEY>
│ │ │ │ -
│ │ │ │ -
38class ordering_key_visitor : public boost::default_bfs_visitor {
│ │ │ │ -
39public:
│ │ │ │ -
40 ordering_key_visitor(std::list<KEY>& ordering_in) : ordering_(ordering_in) {}
│ │ │ │ -
41 template <typename Vertex, typename Graph> void discover_vertex(Vertex v, const Graph& g) const {
│ │ │ │ -
42 KEY key = boost::get(boost::vertex_name, g, v);
│ │ │ │ -
43 ordering_.push_front(key);
│ │ │ │ -
44 }
│ │ │ │ -
45 std::list<KEY>& ordering_;
│ │ │ │ -
46};
│ │ │ │ -
│ │ │ │ -
47
│ │ │ │ -
48/* ************************************************************************* */
│ │ │ │ -
49template<class KEY>
│ │ │ │ -
│ │ │ │ -
50std::list<KEY> predecessorMap2Keys(const PredecessorMap<KEY>& p_map) {
│ │ │ │ -
51
│ │ │ │ -
52 typedef typename SGraph<KEY>::Vertex SVertex;
│ │ │ │ -
53
│ │ │ │ -
54 SGraph<KEY> g;
│ │ │ │ -
55 SVertex root;
│ │ │ │ -
56 std::map<KEY, SVertex> key2vertex;
│ │ │ │ -
57 boost::tie(g, root, key2vertex) = gtsam::predecessorMap2Graph<SGraph<KEY>, SVertex, KEY>(p_map);
│ │ │ │ -
58
│ │ │ │ -
59 // breadth first visit on the graph
│ │ │ │ -
60 std::list<KEY> keys;
│ │ │ │ -
61 ordering_key_visitor<KEY> vis(keys);
│ │ │ │ -
62 boost::breadth_first_search(g, root, boost::visitor(vis));
│ │ │ │ -
63 return keys;
│ │ │ │ -
64}
│ │ │ │ -
│ │ │ │ -
65
│ │ │ │ -
66/* ************************************************************************* */
│ │ │ │ -
67template<class G, class F, class KEY>
│ │ │ │ -
│ │ │ │ -
68SDGraph<KEY> toBoostGraph(const G& graph) {
│ │ │ │ -
69 // convert the factor graph to boost graph
│ │ │ │ -
70 SDGraph<KEY> g;
│ │ │ │ -
71 typedef typename boost::graph_traits<SDGraph<KEY> >::vertex_descriptor BoostVertex;
│ │ │ │ -
72 std::map<KEY, BoostVertex> key2vertex;
│ │ │ │ -
73 typename G::const_iterator itFactor;
│ │ │ │ -
74
│ │ │ │ -
75 // Loop over the factors
│ │ │ │ -
76 for(itFactor=graph.begin(); itFactor!=graph.end(); itFactor++) {
│ │ │ │ -
77
│ │ │ │ -
78 // Ignore factors that are not binary
│ │ │ │ -
79 if ((*itFactor)->keys().size() != 2)
│ │ │ │ -
80 continue;
│ │ │ │ -
81
│ │ │ │ -
82 // Cast the factor to the user-specified factor type F
│ │ │ │ -
83 boost::shared_ptr<F> factor = boost::dynamic_pointer_cast<F>(*itFactor);
│ │ │ │ -
84 // Ignore factors that are not of type F
│ │ │ │ -
85 if (!factor) continue;
│ │ │ │ -
86
│ │ │ │ -
87 // Retrieve the 2 keys (nodes) the factor (edge) is incident on
│ │ │ │ -
88 KEY key1 = factor->keys()[0];
│ │ │ │ -
89 KEY key2 = factor->keys()[1];
│ │ │ │ -
90
│ │ │ │ -
91 BoostVertex v1, v2;
│ │ │ │ -
92
│ │ │ │ -
93 // If key1 is a new key, add it to the key2vertex map, else get the corresponding vertex id
│ │ │ │ -
94 if (key2vertex.find(key1) == key2vertex.end()) {
│ │ │ │ -
95 v1 = add_vertex(key1, g);
│ │ │ │ -
96 key2vertex.insert(std::pair<KEY,KEY>(key1, v1));
│ │ │ │ -
97 } else
│ │ │ │ -
98 v1 = key2vertex[key1];
│ │ │ │ -
99
│ │ │ │ -
100 // If key2 is a new key, add it to the key2vertex map, else get the corresponding vertex id
│ │ │ │ -
101 if (key2vertex.find(key2) == key2vertex.end()) {
│ │ │ │ -
102 v2 = add_vertex(key2, g);
│ │ │ │ -
103 key2vertex.insert(std::pair<KEY,KEY>(key2, v2));
│ │ │ │ -
104 } else
│ │ │ │ -
105 v2 = key2vertex[key2];
│ │ │ │ -
106
│ │ │ │ -
107 // Add an edge with weight 1.0
│ │ │ │ -
108 boost::property<boost::edge_weight_t, double> edge_property(1.0); // assume constant edge weight here
│ │ │ │ -
109 boost::add_edge(v1, v2, edge_property, g);
│ │ │ │ -
110 }
│ │ │ │ -
111
│ │ │ │ -
112 return g;
│ │ │ │ -
113}
│ │ │ │ -
│ │ │ │ -
114
│ │ │ │ -
115/* ************************************************************************* */
│ │ │ │ -
116template<class G, class V, class KEY>
│ │ │ │ -
117boost::tuple<G, V, std::map<KEY,V> >
│ │ │ │ -
│ │ │ │ -
118predecessorMap2Graph(const PredecessorMap<KEY>& p_map) {
│ │ │ │ -
119
│ │ │ │ -
120 G g;
│ │ │ │ -
121 std::map<KEY, V> key2vertex;
│ │ │ │ -
122 V v1, v2, root;
│ │ │ │ -
123 bool foundRoot = false;
│ │ │ │ -
124 for(auto child_parent: p_map) {
│ │ │ │ -
125 KEY child, parent;
│ │ │ │ -
126 std::tie(child,parent) = child_parent;
│ │ │ │ -
127 if (key2vertex.find(child) == key2vertex.end()) {
│ │ │ │ -
128 v1 = add_vertex(child, g);
│ │ │ │ -
129 key2vertex.insert(std::make_pair(child, v1));
│ │ │ │ -
130 } else
│ │ │ │ -
131 v1 = key2vertex[child];
│ │ │ │ -
132
│ │ │ │ -
133 if (key2vertex.find(parent) == key2vertex.end()) {
│ │ │ │ -
134 v2 = add_vertex(parent, g);
│ │ │ │ -
135 key2vertex.insert(std::make_pair(parent, v2));
│ │ │ │ -
136 } else
│ │ │ │ -
137 v2 = key2vertex[parent];
│ │ │ │ -
138
│ │ │ │ -
139 if (child==parent) {
│ │ │ │ -
140 root = v1;
│ │ │ │ -
141 foundRoot = true;
│ │ │ │ -
142 } else
│ │ │ │ -
143 boost::add_edge(v2, v1, g); // edge is from parent to child
│ │ │ │ -
144 }
│ │ │ │ -
145
│ │ │ │ -
146 if (!foundRoot)
│ │ │ │ -
147 throw std::invalid_argument("predecessorMap2Graph: invalid predecessor map!");
│ │ │ │ -
148 else
│ │ │ │ -
149 return boost::tuple<G, V, std::map<KEY, V> >(g, root, key2vertex);
│ │ │ │ -
150}
│ │ │ │ -
│ │ │ │ -
151
│ │ │ │ -
152/* ************************************************************************* */
│ │ │ │ -
153template <class V, class POSE, class KEY>
│ │ │ │ -
│ │ │ │ -
154class compose_key_visitor : public boost::default_bfs_visitor {
│ │ │ │ -
155
│ │ │ │ -
156private:
│ │ │ │ -
157 boost::shared_ptr<Values> config_;
│ │ │ │ -
158
│ │ │ │ -
159public:
│ │ │ │ -
160
│ │ │ │ -
161 compose_key_visitor(boost::shared_ptr<Values> config_in) {config_ = config_in;}
│ │ │ │ -
162
│ │ │ │ -
163 template <typename Edge, typename Graph> void tree_edge(Edge edge, const Graph& g) const {
│ │ │ │ -
164 KEY key_from = boost::get(boost::vertex_name, g, boost::source(edge, g));
│ │ │ │ -
165 KEY key_to = boost::get(boost::vertex_name, g, boost::target(edge, g));
│ │ │ │ -
166 POSE relativePose = boost::get(boost::edge_weight, g, edge);
│ │ │ │ -
167 config_->insert(key_to, config_->at<POSE>(key_from).compose(relativePose));
│ │ │ │ -
168 }
│ │ │ │ -
169
│ │ │ │ -
170};
│ │ │ │ -
│ │ │ │ -
171
│ │ │ │ -
172/* ************************************************************************* */
│ │ │ │ -
173template<class G, class Factor, class POSE, class KEY>
│ │ │ │ -
│ │ │ │ -
174boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<KEY>& tree,
│ │ │ │ -
175 const POSE& rootPose) {
│ │ │ │ -
176
│ │ │ │ -
177 //TODO: change edge_weight_t to edge_pose_t
│ │ │ │ -
178 typedef typename boost::adjacency_list<
│ │ │ │ -
179 boost::vecS, boost::vecS, boost::directedS,
│ │ │ │ -
180 boost::property<boost::vertex_name_t, KEY>,
│ │ │ │ -
181 boost::property<boost::edge_weight_t, POSE> > PoseGraph;
│ │ │ │ -
182 typedef typename boost::graph_traits<PoseGraph>::vertex_descriptor PoseVertex;
│ │ │ │ -
183 typedef typename boost::graph_traits<PoseGraph>::edge_descriptor PoseEdge;
│ │ │ │ -
184
│ │ │ │ -
185 PoseGraph g;
│ │ │ │ -
186 PoseVertex root;
│ │ │ │ -
187 std::map<KEY, PoseVertex> key2vertex;
│ │ │ │ -
188 boost::tie(g, root, key2vertex) =
│ │ │ │ -
189 predecessorMap2Graph<PoseGraph, PoseVertex, KEY>(tree);
│ │ │ │ -
190
│ │ │ │ -
191 // attach the relative poses to the edges
│ │ │ │ -
192 PoseEdge edge12, edge21;
│ │ │ │ -
193 bool found1, found2;
│ │ │ │ -
194 for(typename G::sharedFactor nl_factor: graph) {
│ │ │ │ -
195
│ │ │ │ -
196 if (nl_factor->keys().size() > 2)
│ │ │ │ -
197 throw std::invalid_argument("composePoses: only support factors with at most two keys");
│ │ │ │ -
198
│ │ │ │ -
199 // e.g. in pose2graph, nonlinear factor needs to be converted to pose2factor
│ │ │ │ -
200 boost::shared_ptr<Factor> factor = boost::dynamic_pointer_cast<Factor>(nl_factor);
│ │ │ │ -
201 if (!factor) continue;
│ │ │ │ -
202
│ │ │ │ -
203 KEY key1 = factor->key1();
│ │ │ │ -
204 KEY key2 = factor->key2();
│ │ │ │ -
205
│ │ │ │ -
206 PoseVertex v1 = key2vertex.find(key1)->second;
│ │ │ │ -
207 PoseVertex v2 = key2vertex.find(key2)->second;
│ │ │ │ -
208
│ │ │ │ -
209 POSE l1Xl2 = factor->measured();
│ │ │ │ -
210 boost::tie(edge12, found1) = boost::edge(v1, v2, g);
│ │ │ │ -
211 boost::tie(edge21, found2) = boost::edge(v2, v1, g);
│ │ │ │ -
212 if (found1 && found2) throw std::invalid_argument ("composePoses: invalid spanning tree");
│ │ │ │ -
213 if (!found1 && !found2) continue;
│ │ │ │ -
214 if (found1)
│ │ │ │ -
215 boost::put(boost::edge_weight, g, edge12, l1Xl2);
│ │ │ │ -
216 else if (found2)
│ │ │ │ -
217 boost::put(boost::edge_weight, g, edge21, l1Xl2.inverse());
│ │ │ │ -
218 }
│ │ │ │ -
219
│ │ │ │ -
220 // compose poses
│ │ │ │ -
221 boost::shared_ptr<Values> config(new Values);
│ │ │ │ -
222 KEY rootKey = boost::get(boost::vertex_name, g, root);
│ │ │ │ -
223 config->insert(rootKey, rootPose);
│ │ │ │ -
224 compose_key_visitor<PoseVertex, POSE, KEY> vis(config);
│ │ │ │ -
225 boost::breadth_first_search(g, root, boost::visitor(vis));
│ │ │ │ -
226
│ │ │ │ -
227 return config;
│ │ │ │ -
228}
│ │ │ │ -
│ │ │ │ -
229
│ │ │ │ -
230/* ************************************************************************* */
│ │ │ │ -
231template<class G, class KEY, class FACTOR2>
│ │ │ │ -
│ │ │ │ -
232PredecessorMap<KEY> findMinimumSpanningTree(const G& fg) {
│ │ │ │ -
233
│ │ │ │ -
234 // Convert to a graph that boost understands
│ │ │ │ -
235 SDGraph<KEY> g = gtsam::toBoostGraph<G, FACTOR2, KEY>(fg);
│ │ │ │ -
236
│ │ │ │ -
237 // find minimum spanning tree
│ │ │ │ -
238 std::vector<typename SDGraph<KEY>::Vertex> p_map(boost::num_vertices(g));
│ │ │ │ -
239 prim_minimum_spanning_tree(g, &p_map[0]);
│ │ │ │ -
240
│ │ │ │ -
241 // convert edge to string pairs
│ │ │ │ -
242 PredecessorMap<KEY> tree;
│ │ │ │ -
243 typename SDGraph<KEY>::vertex_iterator itVertex = boost::vertices(g).first;
│ │ │ │ -
244 for(const typename SDGraph<KEY>::Vertex& vi: p_map){
│ │ │ │ -
245 KEY key = boost::get(boost::vertex_name, g, *itVertex);
│ │ │ │ -
246 KEY parent = boost::get(boost::vertex_name, g, vi);
│ │ │ │ -
247 tree.insert(key, parent);
│ │ │ │ -
248 itVertex++;
│ │ │ │ -
249 }
│ │ │ │ -
250 return tree;
│ │ │ │ -
251}
│ │ │ │ -
│ │ │ │ -
252
│ │ │ │ -
253/* ************************************************************************* */
│ │ │ │ -
254template<class G, class KEY, class FACTOR2>
│ │ │ │ -
│ │ │ │ -
255void split(const G& g, const PredecessorMap<KEY>& tree, G& Ab1, G& Ab2) {
│ │ │ │ -
256
│ │ │ │ -
257 typedef typename G::sharedFactor F ;
│ │ │ │ -
258
│ │ │ │ -
259 for(const F& factor: g)
│ │ │ │ -
260 {
│ │ │ │ -
261 if (factor->keys().size() > 2)
│ │ │ │ -
262 throw(std::invalid_argument("split: only support factors with at most two keys"));
│ │ │ │ -
263
│ │ │ │ -
264 if (factor->keys().size() == 1) {
│ │ │ │ -
265 Ab1.push_back(factor);
│ │ │ │ -
266 continue;
│ │ │ │ -
267 }
│ │ │ │ -
268
│ │ │ │ -
269 boost::shared_ptr<FACTOR2> factor2 = boost::dynamic_pointer_cast<
│ │ │ │ -
270 FACTOR2>(factor);
│ │ │ │ -
271 if (!factor2) continue;
│ │ │ │ -
272
│ │ │ │ -
273 KEY key1 = factor2->key1();
│ │ │ │ -
274 KEY key2 = factor2->key2();
│ │ │ │ -
275 // if the tree contains the key
│ │ │ │ -
276 if ((tree.find(key1) != tree.end() &&
│ │ │ │ -
277 tree.find(key1)->second.compare(key2) == 0) ||
│ │ │ │ -
278 (tree.find(key2) != tree.end() &&
│ │ │ │ -
279 tree.find(key2)->second.compare(key1)== 0) )
│ │ │ │ -
280 Ab1.push_back(factor2);
│ │ │ │ -
281 else
│ │ │ │ -
282 Ab2.push_back(factor2);
│ │ │ │ -
283 }
│ │ │ │ -
284}
│ │ │ │ -
│ │ │ │ -
285
│ │ │ │ -
286}
│ │ │ │ -
graph.h
Graph algorithm using boost library.
│ │ │ │ +
18// \callgraph
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/inference/Conditional.h>
│ │ │ │ +
22
│ │ │ │ +
23#include <cmath>
│ │ │ │ +
24#include <iostream>
│ │ │ │ +
25
│ │ │ │ +
26namespace gtsam {
│ │ │ │ +
27
│ │ │ │ +
28/* ************************************************************************* */
│ │ │ │ +
29template <class FACTOR, class DERIVEDCONDITIONAL>
│ │ │ │ +
│ │ │ │ +
30void Conditional<FACTOR, DERIVEDCONDITIONAL>::print(
│ │ │ │ +
31 const std::string& s, const KeyFormatter& formatter) const {
│ │ │ │ +
32 std::cout << s << " P(";
│ │ │ │ +
33 for (Key key : frontals()) std::cout << " " << formatter(key);
│ │ │ │ +
34 if (nrParents() > 0) std::cout << " |";
│ │ │ │ +
35 for (Key parent : parents()) std::cout << " " << formatter(parent);
│ │ │ │ +
36 std::cout << ")" << std::endl;
│ │ │ │ +
37}
│ │ │ │ +
│ │ │ │ +
38
│ │ │ │ +
39/* ************************************************************************* */
│ │ │ │ +
40template <class FACTOR, class DERIVEDCONDITIONAL>
│ │ │ │ +
│ │ │ │ +
41bool Conditional<FACTOR, DERIVEDCONDITIONAL>::equals(const This& c,
│ │ │ │ +
42 double tol) const {
│ │ │ │ +
43 return nrFrontals_ == c.nrFrontals_;
│ │ │ │ +
44}
│ │ │ │ +
│ │ │ │ +
45
│ │ │ │ +
46/* ************************************************************************* */
│ │ │ │ +
47template <class FACTOR, class DERIVEDCONDITIONAL>
│ │ │ │ +
│ │ │ │ +
48double Conditional<FACTOR, DERIVEDCONDITIONAL>::logProbability(
│ │ │ │ +
49 const HybridValues& c) const {
│ │ │ │ +
50 throw std::runtime_error("Conditional::logProbability is not implemented");
│ │ │ │ +
51}
│ │ │ │ +
│ │ │ │ +
52
│ │ │ │ +
53/* ************************************************************************* */
│ │ │ │ +
54template <class FACTOR, class DERIVEDCONDITIONAL>
│ │ │ │ +
│ │ │ │ +
55double Conditional<FACTOR, DERIVEDCONDITIONAL>::evaluate(
│ │ │ │ +
56 const HybridValues& c) const {
│ │ │ │ +
57 throw std::runtime_error("Conditional::evaluate is not implemented");
│ │ │ │ +
58}
│ │ │ │ +
│ │ │ │ +
59
│ │ │ │ +
60/* ************************************************************************* */
│ │ │ │ +
61template <class FACTOR, class DERIVEDCONDITIONAL>
│ │ │ │ +
│ │ │ │ +
62double Conditional<FACTOR, DERIVEDCONDITIONAL>::logNormalizationConstant()
│ │ │ │ +
63 const {
│ │ │ │ +
64 throw std::runtime_error(
│ │ │ │ +
65 "Conditional::logNormalizationConstant is not implemented");
│ │ │ │ +
66}
│ │ │ │ +
│ │ │ │ +
67
│ │ │ │ +
68/* ************************************************************************* */
│ │ │ │ +
69template <class FACTOR, class DERIVEDCONDITIONAL>
│ │ │ │ +
│ │ │ │ +
70double Conditional<FACTOR, DERIVEDCONDITIONAL>::normalizationConstant() const {
│ │ │ │ +
71 return std::exp(logNormalizationConstant());
│ │ │ │ +
72}
│ │ │ │ +
│ │ │ │ +
73
│ │ │ │ +
74/* ************************************************************************* */
│ │ │ │ +
75template <class FACTOR, class DERIVEDCONDITIONAL>
│ │ │ │ +
76template <class VALUES>
│ │ │ │ +
│ │ │ │ +
77bool Conditional<FACTOR, DERIVEDCONDITIONAL>::CheckInvariants(
│ │ │ │ +
78 const DERIVEDCONDITIONAL& conditional, const VALUES& values) {
│ │ │ │ +
79 const double prob_or_density = conditional.evaluate(values);
│ │ │ │ +
80 if (prob_or_density < 0.0) return false; // prob_or_density is negative.
│ │ │ │ +
81 if (std::abs(prob_or_density - conditional(values)) > 1e-9)
│ │ │ │ +
82 return false; // operator and evaluate differ
│ │ │ │ +
83 const double logProb = conditional.logProbability(values);
│ │ │ │ +
84 if (std::abs(prob_or_density - std::exp(logProb)) > 1e-9)
│ │ │ │ +
85 return false; // logProb is not consistent with prob_or_density
│ │ │ │ +
86 if (std::abs(conditional.logNormalizationConstant() -
│ │ │ │ +
87 std::log(conditional.normalizationConstant())) > 1e-9)
│ │ │ │ +
88 return false; // log normalization constant is not consistent with
│ │ │ │ +
89 // normalization constant
│ │ │ │ +
90 const double error = conditional.error(values);
│ │ │ │ +
91 if (error < 0.0) return false; // prob_or_density is negative.
│ │ │ │ +
92 const double expected = conditional.logNormalizationConstant() - error;
│ │ │ │ +
93 if (std::abs(logProb - expected) > 1e-9)
│ │ │ │ +
94 return false; // logProb is not consistent with error
│ │ │ │ +
95 return true;
│ │ │ │ +
96}
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
97
│ │ │ │ +
98} // namespace gtsam
│ │ │ │ +
│ │ │ │ +
Conditional.h
Base class for conditional densities.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::predecessorMap2Keys
std::list< KEY > predecessorMap2Keys(const PredecessorMap< KEY > &p_map)
Generate a list of keys from a spanning tree represented by its predecessor map.
Definition graph-inl.h:50
│ │ │ │ -
gtsam::split
void split(const G &g, const PredecessorMap< KEY > &tree, G &Ab1, G &Ab2)
Split the graph into two parts: one corresponds to the given spanning tree, and the other corresponds...
Definition graph-inl.h:255
│ │ │ │ -
gtsam::composePoses
boost::shared_ptr< Values > composePoses(const G &graph, const PredecessorMap< KEY > &tree, const POSE &rootPose)
Compose the poses by following the chain specified by the spanning tree.
Definition graph-inl.h:174
│ │ │ │ -
gtsam::toBoostGraph
SDGraph< KEY > toBoostGraph(const G &graph)
Convert the factor graph to an SDGraph G = Graph type F = Factor type Key = Key type.
Definition graph-inl.h:68
│ │ │ │ -
gtsam::findMinimumSpanningTree
PredecessorMap< KEY > findMinimumSpanningTree(const G &fg)
find the minimum spanning tree using boost graph library
Definition graph-inl.h:232
│ │ │ │ -
gtsam::predecessorMap2Graph
boost::tuple< G, V, std::map< KEY, V > > predecessorMap2Graph(const PredecessorMap< KEY > &p_map)
Build takes a predecessor map, and builds a directed graph corresponding to the tree.
Definition graph-inl.h:118
│ │ │ │ -
gtsam::ordering_key_visitor
Definition graph-inl.h:38
│ │ │ │ -
gtsam::compose_key_visitor
Definition graph-inl.h:154
│ │ │ │ -
gtsam::SDGraph
SDGraph is undirected graph with variable keys and double edge weights.
Definition graph.h:40
│ │ │ │ -
gtsam::SGraph
Definition graph.h:47
│ │ │ │ -
gtsam::PredecessorMap
Map from variable key to parent key.
Definition graph.h:58
│ │ │ │ -
gtsam::PredecessorMap::insert
void insert(const KEY &key, const KEY &parent)
convenience insert so we can pass ints for TypedSymbol keys
Definition graph.h:61
│ │ │ │ -
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ +
gtsam::Conditional
Definition Conditional.h:64
│ │ │ │ +
gtsam::Conditional::CheckInvariants
static bool CheckInvariants(const DERIVEDCONDITIONAL &conditional, const VALUES &x)
Check invariants of this conditional, given the values x.
Definition Conditional-inst.h:77
│ │ │ │ +
gtsam::Conditional::nrFrontals_
size_t nrFrontals_
The first nrFrontal variables are frontal and the rest are parents.
Definition Conditional.h:67
│ │ │ │ +
gtsam::Conditional::logNormalizationConstant
virtual double logNormalizationConstant() const
All conditional types need to implement a log normalization constant to make it such that error>=0.
Definition Conditional-inst.h:62
│ │ │ │ +
gtsam::Conditional::evaluate
virtual double evaluate(const HybridValues &c) const
All conditional types need to implement an evaluate function, that yields a true probability.
Definition Conditional-inst.h:55
│ │ │ │ +
gtsam::Conditional::equals
bool equals(const This &c, double tol=1e-9) const
check equality
Definition Conditional-inst.h:41
│ │ │ │ +
gtsam::Conditional::logProbability
virtual double logProbability(const HybridValues &c) const
All conditional types need to implement a logProbability function, for which exp(logProbability(x)) =...
Definition Conditional-inst.h:48
│ │ │ │ +
gtsam::Conditional::normalizationConstant
double normalizationConstant() const
Non-virtual, exponentiate logNormalizationConstant.
Definition Conditional-inst.h:70
│ │ │ │ +
gtsam::Conditional::print
void print(const std::string &s="Conditional", const KeyFormatter &formatter=DefaultKeyFormatter) const
print with optional formatter
Definition Conditional-inst.h:30
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,377 +1,167 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -graph-inl.h │ │ │ │ │ +Conditional-inst.h │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -12/* │ │ │ │ │ -13 * @file graph-inl.h │ │ │ │ │ -14 * @brief Graph algorithm using boost library │ │ │ │ │ -15 * @author Kai Ni │ │ │ │ │ -16 */ │ │ │ │ │ -17 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include │ │ │ │ │ -21#ifdef __GNUC__ │ │ │ │ │ -22#pragma GCC diagnostic push │ │ │ │ │ -23#pragma GCC diagnostic ignored "-Wunused-variable" │ │ │ │ │ -24//#pragma GCC diagnostic ignored "-Wunneeded-internal-declaration" │ │ │ │ │ -25#endif │ │ │ │ │ -26#include │ │ │ │ │ -27#ifdef __GNUC__ │ │ │ │ │ -28#pragma GCC diagnostic pop │ │ │ │ │ -29#endif │ │ │ │ │ -30#include │ │ │ │ │ -31 │ │ │ │ │ -32#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_g_r_a_p_h_._h> │ │ │ │ │ -33 │ │ │ │ │ -34namespace _g_t_s_a_m { │ │ │ │ │ -35 │ │ │ │ │ -36/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -37template │ │ │ │ │ -_3_8class _o_r_d_e_r_i_n_g___k_e_y___v_i_s_i_t_o_r : public boost::default_bfs_visitor { │ │ │ │ │ -39public: │ │ │ │ │ -40 _o_r_d_e_r_i_n_g___k_e_y___v_i_s_i_t_o_r(std::list& ordering_in) : ordering_(ordering_in) │ │ │ │ │ -{} │ │ │ │ │ -41 template void discover_vertex(Vertex v, │ │ │ │ │ -const Graph& g) const { │ │ │ │ │ -42 KEY key = boost::get(boost::vertex_name, g, v); │ │ │ │ │ -43 ordering_.push_front(key); │ │ │ │ │ -44 } │ │ │ │ │ -45 std::list& ordering_; │ │ │ │ │ -46}; │ │ │ │ │ -47 │ │ │ │ │ -48/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -49template │ │ │ │ │ -_5_0std::list _p_r_e_d_e_c_e_s_s_o_r_M_a_p_2_K_e_y_s(const _P_r_e_d_e_c_e_s_s_o_r_M_a_p_<_K_E_Y_>& p_map) { │ │ │ │ │ -51 │ │ │ │ │ -52 typedef typename SGraph::Vertex SVertex; │ │ │ │ │ -53 │ │ │ │ │ -54 _S_G_r_a_p_h_<_K_E_Y_> g; │ │ │ │ │ -55 SVertex root; │ │ │ │ │ -56 std::map key2vertex; │ │ │ │ │ -57 boost::tie(g, root, key2vertex) = gtsam::predecessorMap2Graph, │ │ │ │ │ -SVertex, KEY>(p_map); │ │ │ │ │ -58 │ │ │ │ │ -59 // breadth first visit on the graph │ │ │ │ │ -60 std::list keys; │ │ │ │ │ -61 _o_r_d_e_r_i_n_g___k_e_y___v_i_s_i_t_o_r_<_K_E_Y_> vis(keys); │ │ │ │ │ -62 boost::breadth_first_search(g, root, boost::visitor(vis)); │ │ │ │ │ -63 return keys; │ │ │ │ │ -64} │ │ │ │ │ -65 │ │ │ │ │ -66/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -67template │ │ │ │ │ -_6_8_S_D_G_r_a_p_h_<_K_E_Y_> _t_o_B_o_o_s_t_G_r_a_p_h(const G& graph) { │ │ │ │ │ -69 // convert the factor graph to boost graph │ │ │ │ │ -70 _S_D_G_r_a_p_h_<_K_E_Y_> g; │ │ │ │ │ -71 typedef typename boost::graph_traits >::vertex_descriptor │ │ │ │ │ -BoostVertex; │ │ │ │ │ -72 std::map key2vertex; │ │ │ │ │ -73 typename G::const_iterator itFactor; │ │ │ │ │ -74 │ │ │ │ │ -75 // Loop over the factors │ │ │ │ │ -76 for(itFactor=graph.begin(); itFactor!=graph.end(); itFactor++) { │ │ │ │ │ -77 │ │ │ │ │ -78 // Ignore factors that are not binary │ │ │ │ │ -79 if ((*itFactor)->keys().size() != 2) │ │ │ │ │ -80 continue; │ │ │ │ │ -81 │ │ │ │ │ -82 // Cast the factor to the user-specified factor type F │ │ │ │ │ -83 boost::shared_ptr factor = boost::dynamic_pointer_cast(*itFactor); │ │ │ │ │ -84 // Ignore factors that are not of type F │ │ │ │ │ -85 if (!factor) continue; │ │ │ │ │ -86 │ │ │ │ │ -87 // Retrieve the 2 keys (nodes) the factor (edge) is incident on │ │ │ │ │ -88 KEY key1 = factor->keys()[0]; │ │ │ │ │ -89 KEY key2 = factor->keys()[1]; │ │ │ │ │ -90 │ │ │ │ │ -91 BoostVertex v1, v2; │ │ │ │ │ -92 │ │ │ │ │ -93 // If key1 is a new key, add it to the key2vertex map, else get the │ │ │ │ │ -corresponding vertex id │ │ │ │ │ -94 if (key2vertex.find(key1) == key2vertex.end()) { │ │ │ │ │ -95 v1 = add_vertex(key1, g); │ │ │ │ │ -96 key2vertex.insert(std::pair(key1, v1)); │ │ │ │ │ -97 } else │ │ │ │ │ -98 v1 = key2vertex[key1]; │ │ │ │ │ -99 │ │ │ │ │ -100 // If key2 is a new key, add it to the key2vertex map, else get the │ │ │ │ │ -corresponding vertex id │ │ │ │ │ -101 if (key2vertex.find(key2) == key2vertex.end()) { │ │ │ │ │ -102 v2 = add_vertex(key2, g); │ │ │ │ │ -103 key2vertex.insert(std::pair(key2, v2)); │ │ │ │ │ -104 } else │ │ │ │ │ -105 v2 = key2vertex[key2]; │ │ │ │ │ -106 │ │ │ │ │ -107 // Add an edge with weight 1.0 │ │ │ │ │ -108 boost::property edge_property(1.0); // assume │ │ │ │ │ -constant edge weight here │ │ │ │ │ -109 boost::add_edge(v1, v2, edge_property, g); │ │ │ │ │ -110 } │ │ │ │ │ -111 │ │ │ │ │ -112 return g; │ │ │ │ │ -113} │ │ │ │ │ -114 │ │ │ │ │ -115/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -116template │ │ │ │ │ -117boost::tuple > │ │ │ │ │ -_1_1_8_p_r_e_d_e_c_e_s_s_o_r_M_a_p_2_G_r_a_p_h(const _P_r_e_d_e_c_e_s_s_o_r_M_a_p_<_K_E_Y_>& p_map) { │ │ │ │ │ -119 │ │ │ │ │ -120 G g; │ │ │ │ │ -121 std::map key2vertex; │ │ │ │ │ -122 V v1, v2, root; │ │ │ │ │ -123 bool foundRoot = false; │ │ │ │ │ -124 for(auto child_parent: p_map) { │ │ │ │ │ -125 KEY child, parent; │ │ │ │ │ -126 std::tie(child,parent) = child_parent; │ │ │ │ │ -127 if (key2vertex.find(child) == key2vertex.end()) { │ │ │ │ │ -128 v1 = add_vertex(child, g); │ │ │ │ │ -129 key2vertex.insert(std::make_pair(child, v1)); │ │ │ │ │ -130 } else │ │ │ │ │ -131 v1 = key2vertex[child]; │ │ │ │ │ -132 │ │ │ │ │ -133 if (key2vertex.find(parent) == key2vertex.end()) { │ │ │ │ │ -134 v2 = add_vertex(parent, g); │ │ │ │ │ -135 key2vertex.insert(std::make_pair(parent, v2)); │ │ │ │ │ -136 } else │ │ │ │ │ -137 v2 = key2vertex[parent]; │ │ │ │ │ -138 │ │ │ │ │ -139 if (child==parent) { │ │ │ │ │ -140 root = v1; │ │ │ │ │ -141 foundRoot = true; │ │ │ │ │ -142 } else │ │ │ │ │ -143 boost::add_edge(v2, v1, g); // edge is from parent to child │ │ │ │ │ -144 } │ │ │ │ │ -145 │ │ │ │ │ -146 if (!foundRoot) │ │ │ │ │ -147 throw std::invalid_argument("predecessorMap2Graph: invalid predecessor │ │ │ │ │ -map!"); │ │ │ │ │ -148 else │ │ │ │ │ -149 return boost::tuple >(g, root, key2vertex); │ │ │ │ │ -150} │ │ │ │ │ -151 │ │ │ │ │ -152/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -153template │ │ │ │ │ -_1_5_4class _c_o_m_p_o_s_e___k_e_y___v_i_s_i_t_o_r : public boost::default_bfs_visitor { │ │ │ │ │ -155 │ │ │ │ │ -156private: │ │ │ │ │ -157 boost::shared_ptr config_; │ │ │ │ │ -158 │ │ │ │ │ -159public: │ │ │ │ │ -160 │ │ │ │ │ -161 _c_o_m_p_o_s_e___k_e_y___v_i_s_i_t_o_r(boost::shared_ptr config_in) {config_ = │ │ │ │ │ -config_in;} │ │ │ │ │ -162 │ │ │ │ │ -163 template void tree_edge(Edge edge, const │ │ │ │ │ -Graph& g) const { │ │ │ │ │ -164 KEY key_from = boost::get(boost::vertex_name, g, boost::source(edge, g)); │ │ │ │ │ -165 KEY key_to = boost::get(boost::vertex_name, g, boost::target(edge, g)); │ │ │ │ │ -166 POSE relativePose = boost::get(boost::edge_weight, g, edge); │ │ │ │ │ -167 config_->insert(key_to, config_->at(key_from).compose(relativePose)); │ │ │ │ │ -168 } │ │ │ │ │ -169 │ │ │ │ │ -170}; │ │ │ │ │ -171 │ │ │ │ │ -172/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -173template │ │ │ │ │ -_1_7_4boost::shared_ptr _c_o_m_p_o_s_e_P_o_s_e_s(const G& graph, const │ │ │ │ │ -_P_r_e_d_e_c_e_s_s_o_r_M_a_p_<_K_E_Y_>& tree, │ │ │ │ │ -175 const POSE& rootPose) { │ │ │ │ │ -176 │ │ │ │ │ -177 //TODO: change edge_weight_t to edge_pose_t │ │ │ │ │ -178 typedef typename boost::adjacency_list< │ │ │ │ │ -179 boost::vecS, boost::vecS, boost::directedS, │ │ │ │ │ -180 boost::property, │ │ │ │ │ -181 boost::property > PoseGraph; │ │ │ │ │ -182 typedef typename boost::graph_traits::vertex_descriptor │ │ │ │ │ -PoseVertex; │ │ │ │ │ -183 typedef typename boost::graph_traits::edge_descriptor PoseEdge; │ │ │ │ │ -184 │ │ │ │ │ -185 PoseGraph g; │ │ │ │ │ -186 PoseVertex root; │ │ │ │ │ -187 std::map key2vertex; │ │ │ │ │ -188 boost::tie(g, root, key2vertex) = │ │ │ │ │ -189 predecessorMap2Graph(tree); │ │ │ │ │ -190 │ │ │ │ │ -191 // attach the relative poses to the edges │ │ │ │ │ -192 PoseEdge edge12, edge21; │ │ │ │ │ -193 bool found1, found2; │ │ │ │ │ -194 for(typename G::sharedFactor nl_factor: graph) { │ │ │ │ │ -195 │ │ │ │ │ -196 if (nl_factor->keys().size() > 2) │ │ │ │ │ -197 throw std::invalid_argument("composePoses: only support factors with at │ │ │ │ │ -most two keys"); │ │ │ │ │ -198 │ │ │ │ │ -199 // e.g. in pose2graph, nonlinear factor needs to be converted to │ │ │ │ │ -pose2factor │ │ │ │ │ -200 boost::shared_ptr factor = boost::dynamic_pointer_cast │ │ │ │ │ -(nl_factor); │ │ │ │ │ -201 if (!factor) continue; │ │ │ │ │ -202 │ │ │ │ │ -203 KEY key1 = factor->key1(); │ │ │ │ │ -204 KEY key2 = factor->key2(); │ │ │ │ │ -205 │ │ │ │ │ -206 PoseVertex v1 = key2vertex.find(key1)->second; │ │ │ │ │ -207 PoseVertex v2 = key2vertex.find(key2)->second; │ │ │ │ │ -208 │ │ │ │ │ -209 POSE l1Xl2 = factor->measured(); │ │ │ │ │ -210 boost::tie(edge12, found1) = boost::edge(v1, v2, g); │ │ │ │ │ -211 boost::tie(edge21, found2) = boost::edge(v2, v1, g); │ │ │ │ │ -212 if (found1 && found2) throw std::invalid_argument ("composePoses: invalid │ │ │ │ │ -spanning tree"); │ │ │ │ │ -213 if (!found1 && !found2) continue; │ │ │ │ │ -214 if (found1) │ │ │ │ │ -215 boost::put(boost::edge_weight, g, edge12, l1Xl2); │ │ │ │ │ -216 else if (found2) │ │ │ │ │ -217 boost::put(boost::edge_weight, g, edge21, l1Xl2.inverse()); │ │ │ │ │ -218 } │ │ │ │ │ -219 │ │ │ │ │ -220 // compose poses │ │ │ │ │ -221 boost::shared_ptr config(new _V_a_l_u_e_s); │ │ │ │ │ -222 KEY rootKey = boost::get(boost::vertex_name, g, root); │ │ │ │ │ -223 config->insert(rootKey, rootPose); │ │ │ │ │ -224 _c_o_m_p_o_s_e___k_e_y___v_i_s_i_t_o_r_<_P_o_s_e_V_e_r_t_e_x_,_ _P_O_S_E_,_ _K_E_Y_> vis(config); │ │ │ │ │ -225 boost::breadth_first_search(g, root, boost::visitor(vis)); │ │ │ │ │ -226 │ │ │ │ │ -227 return config; │ │ │ │ │ -228} │ │ │ │ │ -229 │ │ │ │ │ -230/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -231template │ │ │ │ │ -_2_3_2_P_r_e_d_e_c_e_s_s_o_r_M_a_p_<_K_E_Y_> _f_i_n_d_M_i_n_i_m_u_m_S_p_a_n_n_i_n_g_T_r_e_e(const G& fg) { │ │ │ │ │ -233 │ │ │ │ │ -234 // Convert to a graph that boost understands │ │ │ │ │ -235 _S_D_G_r_a_p_h_<_K_E_Y_> g = gtsam::toBoostGraph(fg); │ │ │ │ │ -236 │ │ │ │ │ -237 // find minimum spanning tree │ │ │ │ │ -238 std::vector::Vertex> p_map(boost::num_vertices(g)); │ │ │ │ │ -239 prim_minimum_spanning_tree(g, &p_map[0]); │ │ │ │ │ -240 │ │ │ │ │ -241 // convert edge to string pairs │ │ │ │ │ -242 _P_r_e_d_e_c_e_s_s_o_r_M_a_p_<_K_E_Y_> tree; │ │ │ │ │ -243 typename _S_D_G_r_a_p_h_<_K_E_Y_>_:_:_v_e_r_t_e_x___i_t_e_r_a_t_o_r itVertex = boost::vertices(g).first; │ │ │ │ │ -244 for(const typename SDGraph::Vertex& vi: p_map){ │ │ │ │ │ -245 KEY key = boost::get(boost::vertex_name, g, *itVertex); │ │ │ │ │ -246 KEY parent = boost::get(boost::vertex_name, g, vi); │ │ │ │ │ -247 tree._i_n_s_e_r_t(key, parent); │ │ │ │ │ -248 itVertex++; │ │ │ │ │ -249 } │ │ │ │ │ -250 return tree; │ │ │ │ │ -251} │ │ │ │ │ -252 │ │ │ │ │ -253/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -254template │ │ │ │ │ -_2_5_5void _s_p_l_i_t(const G& g, const _P_r_e_d_e_c_e_s_s_o_r_M_a_p_<_K_E_Y_>& tree, G& Ab1, G& Ab2) { │ │ │ │ │ -256 │ │ │ │ │ -257 typedef typename G::sharedFactor F ; │ │ │ │ │ -258 │ │ │ │ │ -259 for(const F& factor: g) │ │ │ │ │ -260 { │ │ │ │ │ -261 if (factor->keys().size() > 2) │ │ │ │ │ -262 throw(std::invalid_argument("split: only support factors with at most two │ │ │ │ │ -keys")); │ │ │ │ │ -263 │ │ │ │ │ -264 if (factor->keys().size() == 1) { │ │ │ │ │ -265 Ab1.push_back(factor); │ │ │ │ │ -266 continue; │ │ │ │ │ -267 } │ │ │ │ │ -268 │ │ │ │ │ -269 boost::shared_ptr factor2 = boost::dynamic_pointer_cast< │ │ │ │ │ -270 FACTOR2>(factor); │ │ │ │ │ -271 if (!factor2) continue; │ │ │ │ │ -272 │ │ │ │ │ -273 KEY key1 = factor2->key1(); │ │ │ │ │ -274 KEY key2 = factor2->key2(); │ │ │ │ │ -275 // if the tree contains the key │ │ │ │ │ -276 if ((tree.find(key1) != tree.end() && │ │ │ │ │ -277 tree.find(key1)->second.compare(key2) == 0) || │ │ │ │ │ -278 (tree.find(key2) != tree.end() && │ │ │ │ │ -279 tree.find(key2)->second.compare(key1)== 0) ) │ │ │ │ │ -280 Ab1.push_back(factor2); │ │ │ │ │ -281 else │ │ │ │ │ -282 Ab2.push_back(factor2); │ │ │ │ │ -283 } │ │ │ │ │ -284} │ │ │ │ │ -285 │ │ │ │ │ -286} │ │ │ │ │ -_g_r_a_p_h_._h │ │ │ │ │ -Graph algorithm using boost library. │ │ │ │ │ +18// \callgraph │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ +22 │ │ │ │ │ +23#include │ │ │ │ │ +24#include │ │ │ │ │ +25 │ │ │ │ │ +26namespace _g_t_s_a_m { │ │ │ │ │ +27 │ │ │ │ │ +28/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +29template │ │ │ │ │ +_3_0void _C_o_n_d_i_t_i_o_n_a_l_<_F_A_C_T_O_R_,_ _D_E_R_I_V_E_D_C_O_N_D_I_T_I_O_N_A_L_>_:_:_p_r_i_n_t( │ │ │ │ │ +31 const std::string& s, const _K_e_y_F_o_r_m_a_t_t_e_r& formatter) const { │ │ │ │ │ +32 std::cout << s << " P("; │ │ │ │ │ +33 for (_K_e_y key : frontals()) std::cout << " " << formatter(key); │ │ │ │ │ +34 if (nrParents() > 0) std::cout << " |"; │ │ │ │ │ +35 for (_K_e_y parent : parents()) std::cout << " " << formatter(parent); │ │ │ │ │ +36 std::cout << ")" << std::endl; │ │ │ │ │ +37} │ │ │ │ │ +38 │ │ │ │ │ +39/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +40template │ │ │ │ │ +_4_1bool _C_o_n_d_i_t_i_o_n_a_l_<_F_A_C_T_O_R_,_ _D_E_R_I_V_E_D_C_O_N_D_I_T_I_O_N_A_L_>_:_:_e_q_u_a_l_s(const _T_h_i_s& c, │ │ │ │ │ +42 double tol) const { │ │ │ │ │ +43 return nrFrontals_ == c._n_r_F_r_o_n_t_a_l_s__; │ │ │ │ │ +44} │ │ │ │ │ +45 │ │ │ │ │ +46/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +47template │ │ │ │ │ +_4_8double _C_o_n_d_i_t_i_o_n_a_l_<_F_A_C_T_O_R_,_ _D_E_R_I_V_E_D_C_O_N_D_I_T_I_O_N_A_L_>_:_:_l_o_g_P_r_o_b_a_b_i_l_i_t_y( │ │ │ │ │ +49 const _H_y_b_r_i_d_V_a_l_u_e_s& c) const { │ │ │ │ │ +50 throw std::runtime_error("Conditional::logProbability is not implemented"); │ │ │ │ │ +51} │ │ │ │ │ +52 │ │ │ │ │ +53/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +54template │ │ │ │ │ +_5_5double _C_o_n_d_i_t_i_o_n_a_l_<_F_A_C_T_O_R_,_ _D_E_R_I_V_E_D_C_O_N_D_I_T_I_O_N_A_L_>_:_:_e_v_a_l_u_a_t_e( │ │ │ │ │ +56 const _H_y_b_r_i_d_V_a_l_u_e_s& c) const { │ │ │ │ │ +57 throw std::runtime_error("Conditional::evaluate is not implemented"); │ │ │ │ │ +58} │ │ │ │ │ +59 │ │ │ │ │ +60/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +61template │ │ │ │ │ +_6_2double _C_o_n_d_i_t_i_o_n_a_l_<_F_A_C_T_O_R_,_ _D_E_R_I_V_E_D_C_O_N_D_I_T_I_O_N_A_L_>_:_:_l_o_g_N_o_r_m_a_l_i_z_a_t_i_o_n_C_o_n_s_t_a_n_t() │ │ │ │ │ +63 const { │ │ │ │ │ +64 throw std::runtime_error( │ │ │ │ │ +65 "Conditional::logNormalizationConstant is not implemented"); │ │ │ │ │ +66} │ │ │ │ │ +67 │ │ │ │ │ +68/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +69template │ │ │ │ │ +_7_0double _C_o_n_d_i_t_i_o_n_a_l_<_F_A_C_T_O_R_,_ _D_E_R_I_V_E_D_C_O_N_D_I_T_I_O_N_A_L_>_:_:_n_o_r_m_a_l_i_z_a_t_i_o_n_C_o_n_s_t_a_n_t() const │ │ │ │ │ +{ │ │ │ │ │ +71 return std::exp(logNormalizationConstant()); │ │ │ │ │ +72} │ │ │ │ │ +73 │ │ │ │ │ +74/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +75template │ │ │ │ │ +76template │ │ │ │ │ +_7_7bool _C_o_n_d_i_t_i_o_n_a_l_<_F_A_C_T_O_R_,_ _D_E_R_I_V_E_D_C_O_N_D_I_T_I_O_N_A_L_>_:_:_C_h_e_c_k_I_n_v_a_r_i_a_n_t_s( │ │ │ │ │ +78 const DERIVEDCONDITIONAL& conditional, const VALUES& values) { │ │ │ │ │ +79 const double prob_or_density = conditional.evaluate(values); │ │ │ │ │ +80 if (prob_or_density < 0.0) return false; // prob_or_density is negative. │ │ │ │ │ +81 if (std::abs(prob_or_density - conditional(values)) > 1e-9) │ │ │ │ │ +82 return false; // operator and evaluate differ │ │ │ │ │ +83 const double logProb = conditional.logProbability(values); │ │ │ │ │ +84 if (std::abs(prob_or_density - std::exp(logProb)) > 1e-9) │ │ │ │ │ +85 return false; // logProb is not consistent with prob_or_density │ │ │ │ │ +86 if (std::abs(conditional.logNormalizationConstant() - │ │ │ │ │ +87 std::log(conditional.normalizationConstant())) > 1e-9) │ │ │ │ │ +88 return false; // log normalization constant is not consistent with │ │ │ │ │ +89 // normalization constant │ │ │ │ │ +90 const double error = conditional.error(values); │ │ │ │ │ +91 if (error < 0.0) return false; // prob_or_density is negative. │ │ │ │ │ +92 const double expected = conditional.logNormalizationConstant() - error; │ │ │ │ │ +93 if (std::abs(logProb - expected) > 1e-9) │ │ │ │ │ +94 return false; // logProb is not consistent with error │ │ │ │ │ +95 return true; │ │ │ │ │ +96} │ │ │ │ │ +_9_7 │ │ │ │ │ +98} // namespace gtsam │ │ │ │ │ +_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ +Base class for conditional densities. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_e_d_e_c_e_s_s_o_r_M_a_p_2_K_e_y_s │ │ │ │ │ -std::list< KEY > predecessorMap2Keys(const PredecessorMap< KEY > &p_map) │ │ │ │ │ -Generate a list of keys from a spanning tree represented by its predecessor │ │ │ │ │ -map. │ │ │ │ │ -DDeeffiinniittiioonn graph-inl.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_s_p_l_i_t │ │ │ │ │ -void split(const G &g, const PredecessorMap< KEY > &tree, G &Ab1, G &Ab2) │ │ │ │ │ -Split the graph into two parts: one corresponds to the given spanning tree, and │ │ │ │ │ -the other corresponds... │ │ │ │ │ -DDeeffiinniittiioonn graph-inl.h:255 │ │ │ │ │ -_g_t_s_a_m_:_:_c_o_m_p_o_s_e_P_o_s_e_s │ │ │ │ │ -boost::shared_ptr< Values > composePoses(const G &graph, const PredecessorMap< │ │ │ │ │ -KEY > &tree, const POSE &rootPose) │ │ │ │ │ -Compose the poses by following the chain specified by the spanning tree. │ │ │ │ │ -DDeeffiinniittiioonn graph-inl.h:174 │ │ │ │ │ -_g_t_s_a_m_:_:_t_o_B_o_o_s_t_G_r_a_p_h │ │ │ │ │ -SDGraph< KEY > toBoostGraph(const G &graph) │ │ │ │ │ -Convert the factor graph to an SDGraph G = Graph type F = Factor type Key = Key │ │ │ │ │ -type. │ │ │ │ │ -DDeeffiinniittiioonn graph-inl.h:68 │ │ │ │ │ -_g_t_s_a_m_:_:_f_i_n_d_M_i_n_i_m_u_m_S_p_a_n_n_i_n_g_T_r_e_e │ │ │ │ │ -PredecessorMap< KEY > findMinimumSpanningTree(const G &fg) │ │ │ │ │ -find the minimum spanning tree using boost graph library │ │ │ │ │ -DDeeffiinniittiioonn graph-inl.h:232 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_e_d_e_c_e_s_s_o_r_M_a_p_2_G_r_a_p_h │ │ │ │ │ -boost::tuple< G, V, std::map< KEY, V > > predecessorMap2Graph(const │ │ │ │ │ -PredecessorMap< KEY > &p_map) │ │ │ │ │ -Build takes a predecessor map, and builds a directed graph corresponding to the │ │ │ │ │ -tree. │ │ │ │ │ -DDeeffiinniittiioonn graph-inl.h:118 │ │ │ │ │ -_g_t_s_a_m_:_:_o_r_d_e_r_i_n_g___k_e_y___v_i_s_i_t_o_r │ │ │ │ │ -DDeeffiinniittiioonn graph-inl.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_c_o_m_p_o_s_e___k_e_y___v_i_s_i_t_o_r │ │ │ │ │ -DDeeffiinniittiioonn graph-inl.h:154 │ │ │ │ │ -_g_t_s_a_m_:_:_S_D_G_r_a_p_h │ │ │ │ │ -SDGraph is undirected graph with variable keys and double edge weights. │ │ │ │ │ -DDeeffiinniittiioonn graph.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_S_G_r_a_p_h │ │ │ │ │ -DDeeffiinniittiioonn graph.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_d_e_c_e_s_s_o_r_M_a_p │ │ │ │ │ -Map from variable key to parent key. │ │ │ │ │ -DDeeffiinniittiioonn graph.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_d_e_c_e_s_s_o_r_M_a_p_:_:_i_n_s_e_r_t │ │ │ │ │ -void insert(const KEY &key, const KEY &parent) │ │ │ │ │ -convenience insert so we can pass ints for TypedSymbol keys │ │ │ │ │ -DDeeffiinniittiioonn graph.h:61 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_C_h_e_c_k_I_n_v_a_r_i_a_n_t_s │ │ │ │ │ +static bool CheckInvariants(const DERIVEDCONDITIONAL &conditional, const VALUES │ │ │ │ │ +&x) │ │ │ │ │ +Check invariants of this conditional, given the values x. │ │ │ │ │ +DDeeffiinniittiioonn Conditional-inst.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_n_r_F_r_o_n_t_a_l_s__ │ │ │ │ │ +size_t nrFrontals_ │ │ │ │ │ +The first nrFrontal variables are frontal and the rest are parents. │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_l_o_g_N_o_r_m_a_l_i_z_a_t_i_o_n_C_o_n_s_t_a_n_t │ │ │ │ │ +virtual double logNormalizationConstant() const │ │ │ │ │ +All conditional types need to implement a log normalization constant to make it │ │ │ │ │ +such that error>=0. │ │ │ │ │ +DDeeffiinniittiioonn Conditional-inst.h:62 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_e_v_a_l_u_a_t_e │ │ │ │ │ +virtual double evaluate(const HybridValues &c) const │ │ │ │ │ +All conditional types need to implement an evaluate function, that yields a │ │ │ │ │ +true probability. │ │ │ │ │ +DDeeffiinniittiioonn Conditional-inst.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const This &c, double tol=1e-9) const │ │ │ │ │ +check equality │ │ │ │ │ +DDeeffiinniittiioonn Conditional-inst.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_l_o_g_P_r_o_b_a_b_i_l_i_t_y │ │ │ │ │ +virtual double logProbability(const HybridValues &c) const │ │ │ │ │ +All conditional types need to implement a logProbability function, for which │ │ │ │ │ +exp(logProbability(x)) =... │ │ │ │ │ +DDeeffiinniittiioonn Conditional-inst.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_n_o_r_m_a_l_i_z_a_t_i_o_n_C_o_n_s_t_a_n_t │ │ │ │ │ +double normalizationConstant() const │ │ │ │ │ +Non-virtual, exponentiate logNormalizationConstant. │ │ │ │ │ +DDeeffiinniittiioonn Conditional-inst.h:70 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="Conditional", const KeyFormatter │ │ │ │ │ +&formatter=DefaultKeyFormatter) const │ │ │ │ │ +print with optional formatter │ │ │ │ │ +DDeeffiinniittiioonn Conditional-inst.h:30 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * ggrraapphh--iinnll..hh │ │ │ │ │ + * CCoonnddiittiioonnaall--iinnsstt..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00611.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/FactorGraph.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Key.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,58 +95,96 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
FactorGraph.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ +Functions
│ │ │ │ +
Key.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Factor Graph Base Class. │ │ │ │ -More...

│ │ │ │ - │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::CRefCallPushBack< C >
 Helper. More...
 
class  gtsam::RefCallPushBack< C >
 Helper. More...
struct  gtsam::StreamedKey
 To use the key_formatter on Keys, they must be wrapped in a StreamedKey. More...
 
class  gtsam::CRefCallAddCopy< C >
 Helper. More...
class  gtsam::key_formatter
 Output stream manipulator that will format gtsam::Keys according to the given KeyFormatter, as long as Key values are wrapped in a gtsam::StreamedKey. More...
 
class  gtsam::FactorGraph< FACTOR >
 A factor graph is a bipartite graph with factor nodes connected to variable nodes. More...
struct  gtsam::traits< Key >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Typedefs

│ │ │ │ +using gtsam::KeyFormatter = std::function< std::string(Key)>
 Typedef for a function to format a key, i.e. to convert it to a string.
 
│ │ │ │ +using gtsam::KeyVector = FastVector< Key >
 Define collection type once and for all - also used in wrappers.
 
│ │ │ │ +using gtsam::KeyList = FastList< Key >
 
│ │ │ │ +using gtsam::KeySet = FastSet< Key >
 
│ │ │ │ +using gtsam::KeyGroupMap = FastMap< Key, int >
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +string gtsam::_defaultKeyFormatter (Key key)
 
│ │ │ │ +string gtsam::_multirobotKeyFormatter (Key key)
 
│ │ │ │ +void gtsam::PrintKey (Key key, const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter)
 Utility function to print one key with optional prefix.
 
│ │ │ │ +void gtsam::PrintKeyList (const KeyList &keys, const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter)
 Utility function to print sets of keys with optional prefix.
 
│ │ │ │ +void gtsam::PrintKeyVector (const KeyVector &keys, const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter)
 Utility function to print sets of keys with optional prefix.
 
│ │ │ │ +void gtsam::PrintKeySet (const KeySet &keys, const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter)
 Utility function to print sets of keys with optional prefix.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Factor Graph Base Class.

│ │ │ │ -
Author
Carlos Nieto
│ │ │ │ -
│ │ │ │ -Christian Potthast
│ │ │ │ -
│ │ │ │ -Michael Kaess
│ │ │ │ +
Author
Richard Roberts
│ │ │ │
│ │ │ │ -Richard Roberts
│ │ │ │ +Alex Cunningham
│ │ │ │ +
Date
Feb 20, 2012
│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Feb 20, 2012
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,38 +1,72 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -FactorGraph.h File Reference │ │ │ │ │ -Factor Graph Base Class. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Key.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_C_R_e_f_C_a_l_l_P_u_s_h_B_a_c_k_<_ _C_ _> │ │ │ │ │ -  Helper. _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_S_t_r_e_a_m_e_d_K_e_y │ │ │ │ │ +  To use the _k_e_y___f_o_r_m_a_t_t_e_r on Keys, they must be wrapped in a │ │ │ │ │ + _S_t_r_e_a_m_e_d_K_e_y. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -class   _g_t_s_a_m_:_:_R_e_f_C_a_l_l_P_u_s_h_B_a_c_k_<_ _C_ _> │ │ │ │ │ -  Helper. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_k_e_y___f_o_r_m_a_t_t_e_r │ │ │ │ │ + Output stream manipulator that will format gtsam::Keys according to │ │ │ │ │ +  the given KeyFormatter, as long as Key values are wrapped in a _g_t_s_a_m_:_: │ │ │ │ │ + _S_t_r_e_a_m_e_d_K_e_y. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -class   _g_t_s_a_m_:_:_C_R_e_f_C_a_l_l_A_d_d_C_o_p_y_<_ _C_ _> │ │ │ │ │ -  Helper. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_<_ _F_A_C_T_O_R_ _> │ │ │ │ │ -  A factor graph is a bipartite graph with factor nodes connected to │ │ │ │ │ - variable nodes. _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _K_e_y_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +using  ggttssaamm::::KKeeyyFFoorrmmaatttteerr = std::function< std::string(_K_e_y)> │ │ │ │ │ +  Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +  │ │ │ │ │ +using  ggttssaamm::::KKeeyyVVeeccttoorr = _F_a_s_t_V_e_c_t_o_r< _K_e_y > │ │ │ │ │ +  Define collection type once and for all - also used in wrappers. │ │ │ │ │ +  │ │ │ │ │ +using  ggttssaamm::::KKeeyyLLiisstt = _F_a_s_t_L_i_s_t< _K_e_y > │ │ │ │ │ +  │ │ │ │ │ +using  ggttssaamm::::KKeeyySSeett = _F_a_s_t_S_e_t< _K_e_y > │ │ │ │ │ +  │ │ │ │ │ +using  ggttssaamm::::KKeeyyGGrroouuppMMaapp = _F_a_s_t_M_a_p< _K_e_y, int > │ │ │ │ │ +  │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +string  ggttssaamm::::__ddeeffaauullttKKeeyyFFoorrmmaatttteerr (_K_e_y key) │ │ │ │ │ +  │ │ │ │ │ +string  ggttssaamm::::__mmuullttiirroobboottKKeeyyFFoorrmmaatttteerr (_K_e_y key) │ │ │ │ │ +  │ │ │ │ │ + void  ggttssaamm::::PPrriinnttKKeeyy (_K_e_y key, const std::string &s="", const _K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ + &keyFormatter=DefaultKeyFormatter) │ │ │ │ │ +  Utility function to print one key with optional prefix. │ │ │ │ │ +  │ │ │ │ │ + void  ggttssaamm::::PPrriinnttKKeeyyLLiisstt (const _K_e_y_L_i_s_t &keys, const std::string &s="", │ │ │ │ │ + const _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter=DefaultKeyFormatter) │ │ │ │ │ +  Utility function to print sets of keys with optional prefix. │ │ │ │ │ +  │ │ │ │ │ + void  ggttssaamm::::PPrriinnttKKeeyyVVeeccttoorr (const _K_e_y_V_e_c_t_o_r &keys, const std::string &s="", │ │ │ │ │ + const _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter=DefaultKeyFormatter) │ │ │ │ │ +  Utility function to print sets of keys with optional prefix. │ │ │ │ │ +  │ │ │ │ │ + void  ggttssaamm::::PPrriinnttKKeeyySSeett (const _K_e_y_S_e_t &keys, const std::string &s="", const │ │ │ │ │ + _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter=DefaultKeyFormatter) │ │ │ │ │ +  Utility function to print sets of keys with optional prefix. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Factor Graph Base Class. │ │ │ │ │ Author │ │ │ │ │ - Carlos Nieto │ │ │ │ │ - Christian Potthast │ │ │ │ │ - Michael Kaess │ │ │ │ │ Richard Roberts │ │ │ │ │ + Alex Cunningham │ │ │ │ │ + Date │ │ │ │ │ + Feb 20, 2012 │ │ │ │ │ + Author │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Feb 20, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ + * _K_e_y_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00611.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,6 +1,11 @@ │ │ │ │ │ var a00611 = [ │ │ │ │ │ - ["gtsam::CRefCallPushBack< C >", "a03596.html", null], │ │ │ │ │ - ["gtsam::RefCallPushBack< C >", "a03600.html", null], │ │ │ │ │ - ["gtsam::CRefCallAddCopy< C >", "a03604.html", null], │ │ │ │ │ - ["gtsam::FactorGraph< FACTOR >", "a03524.html", "a03524"] │ │ │ │ │ + ["gtsam::StreamedKey", "a03656.html", null], │ │ │ │ │ + ["gtsam::key_formatter", "a03660.html", null], │ │ │ │ │ + ["gtsam::traits< Key >", "a03664.html", null], │ │ │ │ │ + ["KeyFormatter", "a00611.html#ae4b4e8e7f0d745882c6a02b507d5bffe", null], │ │ │ │ │ + ["KeyVector", "a00611.html#a1c320e42b033352fbbd8266e3bd5faad", null], │ │ │ │ │ + ["PrintKey", "a00611.html#a4f9e295c4f7086ce05cbe37aeeca47fe", null], │ │ │ │ │ + ["PrintKeyList", "a00611.html#aab7ddea9b0bc36f296327e0ec08c4ef0", null], │ │ │ │ │ + ["PrintKeySet", "a00611.html#aac25d6c8afb07054416ba6727685bf55", null], │ │ │ │ │ + ["PrintKeyVector", "a00611.html#a4c14be406d50f65bd404429a7b1d3af0", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00611_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/FactorGraph.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Key.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,431 +98,148 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
FactorGraph.h
│ │ │ │ +
Key.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
21// \callgraph
│ │ │ │ -
22
│ │ │ │ -
23#pragma once
│ │ │ │ -
24
│ │ │ │ -
25#include <gtsam/inference/DotWriter.h>
│ │ │ │ -
26#include <gtsam/inference/Key.h>
│ │ │ │ -
27#include <gtsam/base/FastVector.h>
│ │ │ │ -
28#include <gtsam/base/Testable.h>
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/base/FastList.h>
│ │ │ │ +
21#include <gtsam/base/FastMap.h>
│ │ │ │ +
22#include <gtsam/base/FastSet.h>
│ │ │ │ +
23#include <gtsam/base/FastVector.h>
│ │ │ │ +
24#include <gtsam/base/Testable.h>
│ │ │ │ +
25#include <gtsam/base/types.h>
│ │ │ │ +
26#include <gtsam/dllexport.h>
│ │ │ │ +
27
│ │ │ │ +
28#include <functional>
│ │ │ │
29
│ │ │ │ -
30#include <Eigen/Core> // for Eigen::aligned_allocator
│ │ │ │ +
30#include <iosfwd>
│ │ │ │
31
│ │ │ │ -
32#include <boost/assign/list_inserter.hpp>
│ │ │ │ -
33#include <boost/make_shared.hpp>
│ │ │ │ -
34#include <boost/serialization/nvp.hpp>
│ │ │ │ -
35#include <boost/serialization/vector.hpp>
│ │ │ │ +
32namespace gtsam {
│ │ │ │ +
33
│ │ │ │ +
35using KeyFormatter = std::function<std::string(Key)>;
│ │ │ │
36
│ │ │ │ -
37#include <string>
│ │ │ │ -
38#include <type_traits>
│ │ │ │ -
39#include <utility>
│ │ │ │ -
40#include <iosfwd>
│ │ │ │ -
41
│ │ │ │ -
42namespace gtsam {
│ │ │ │ -
44typedef FastVector<FactorIndex> FactorIndices;
│ │ │ │ -
45
│ │ │ │ -
46// Forward declarations
│ │ │ │ -
47template <class CLIQUE>
│ │ │ │ -
48class BayesTree;
│ │ │ │ -
49
│ │ │ │ -
50class HybridValues;
│ │ │ │ -
51
│ │ │ │ -
53template <class C>
│ │ │ │ -
│ │ │ │ -
54class CRefCallPushBack {
│ │ │ │ -
55 C& obj;
│ │ │ │ +
37// Helper function for DefaultKeyFormatter
│ │ │ │ +
38GTSAM_EXPORT std::string _defaultKeyFormatter(Key key);
│ │ │ │ +
39
│ │ │ │ +
43static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter;
│ │ │ │ +
44
│ │ │ │ +
45// Helper function for Multi-robot Key Formatter
│ │ │ │ +
46GTSAM_EXPORT std::string _multirobotKeyFormatter(gtsam::Key key);
│ │ │ │ +
47
│ │ │ │ +
54static const gtsam::KeyFormatter MultiRobotKeyFormatter =
│ │ │ │ +
55 &_multirobotKeyFormatter;
│ │ │ │
56
│ │ │ │ -
57 public:
│ │ │ │ -
58 explicit CRefCallPushBack(C& obj) : obj(obj) {}
│ │ │ │ -
59 template <typename A>
│ │ │ │ -
60 void operator()(const A& a) {
│ │ │ │ -
61 obj.push_back(a);
│ │ │ │ -
62 }
│ │ │ │ -
63};
│ │ │ │ -
│ │ │ │ -
64
│ │ │ │ -
66template <class C>
│ │ │ │ -
│ │ │ │ -
67class RefCallPushBack {
│ │ │ │ -
68 C& obj;
│ │ │ │ -
69
│ │ │ │ -
70 public:
│ │ │ │ -
71 explicit RefCallPushBack(C& obj) : obj(obj) {}
│ │ │ │ -
72 template <typename A>
│ │ │ │ -
73 void operator()(A& a) {
│ │ │ │ -
74 obj.push_back(a);
│ │ │ │ -
75 }
│ │ │ │ -
76};
│ │ │ │ -
│ │ │ │ +
│ │ │ │ +
58struct StreamedKey {
│ │ │ │ +
59 const Key &key_;
│ │ │ │ +
60 explicit StreamedKey(const Key &key) : key_(key) {}
│ │ │ │ +
61 GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &, const StreamedKey &);
│ │ │ │ +
62};
│ │ │ │ +
│ │ │ │ +
63
│ │ │ │ +
│ │ │ │ +
72class key_formatter {
│ │ │ │ +
73 public:
│ │ │ │ +
74 explicit key_formatter(KeyFormatter v) : formatter_(v) {}
│ │ │ │ +
75 GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &, const key_formatter &);
│ │ │ │ +
76 GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &, const StreamedKey &);
│ │ │ │
77
│ │ │ │ -
79template <class C>
│ │ │ │ -
│ │ │ │ -
80class CRefCallAddCopy {
│ │ │ │ -
81 C& obj;
│ │ │ │ -
82
│ │ │ │ -
83 public:
│ │ │ │ -
84 explicit CRefCallAddCopy(C& obj) : obj(obj) {}
│ │ │ │ -
85 template <typename A>
│ │ │ │ -
86 void operator()(const A& a) {
│ │ │ │ -
87 obj.addCopy(a);
│ │ │ │ -
88 }
│ │ │ │ -
89};
│ │ │ │ -
│ │ │ │ -
90
│ │ │ │ -
96template <class FACTOR>
│ │ │ │ -
│ │ │ │ -
97class FactorGraph {
│ │ │ │ -
98 public:
│ │ │ │ -
99 typedef FACTOR FactorType;
│ │ │ │ -
100 typedef boost::shared_ptr<FACTOR>
│ │ │ │ -
101 sharedFactor;
│ │ │ │ -
102 typedef sharedFactor value_type;
│ │ │ │ -
103 typedef typename FastVector<sharedFactor>::iterator iterator;
│ │ │ │ -
104 typedef typename FastVector<sharedFactor>::const_iterator const_iterator;
│ │ │ │ -
105
│ │ │ │ -
106 private:
│ │ │ │ -
107 typedef FactorGraph<FACTOR> This;
│ │ │ │ -
108 typedef boost::shared_ptr<This>
│ │ │ │ -
109 shared_ptr;
│ │ │ │ -
110
│ │ │ │ -
112 template <typename DERIVEDFACTOR>
│ │ │ │ -
113 using IsDerived = typename std::enable_if<
│ │ │ │ -
114 std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type;
│ │ │ │ +
78 private:
│ │ │ │ +
79 KeyFormatter formatter_;
│ │ │ │ +
80 static void *&property(std::ios_base &s);
│ │ │ │ +
81 static void set_property(std::ios_base &s, const KeyFormatter &f);
│ │ │ │ +
82 static KeyFormatter *get_property(std::ios_base &s);
│ │ │ │ +
83};
│ │ │ │ +
│ │ │ │ +
84
│ │ │ │ +
86using KeyVector = FastVector<Key>;
│ │ │ │ +
87
│ │ │ │ +
88// TODO(frank): Nothing fast about these :-(
│ │ │ │ +
89using KeyList = FastList<Key>;
│ │ │ │ +
90using KeySet = FastSet<Key>;
│ │ │ │ +
91using KeyGroupMap = FastMap<Key, int>;
│ │ │ │ +
92
│ │ │ │ +
94GTSAM_EXPORT void PrintKey(
│ │ │ │ +
95 Key key, const std::string &s = "",
│ │ │ │ +
96 const KeyFormatter &keyFormatter = DefaultKeyFormatter);
│ │ │ │ +
97
│ │ │ │ +
99GTSAM_EXPORT void PrintKeyList(
│ │ │ │ +
100 const KeyList &keys, const std::string &s = "",
│ │ │ │ +
101 const KeyFormatter &keyFormatter = DefaultKeyFormatter);
│ │ │ │ +
102
│ │ │ │ +
104GTSAM_EXPORT void PrintKeyVector(
│ │ │ │ +
105 const KeyVector &keys, const std::string &s = "",
│ │ │ │ +
106 const KeyFormatter &keyFormatter = DefaultKeyFormatter);
│ │ │ │ +
107
│ │ │ │ +
109GTSAM_EXPORT void PrintKeySet(
│ │ │ │ +
110 const KeySet &keys, const std::string &s = "",
│ │ │ │ +
111 const KeyFormatter &keyFormatter = DefaultKeyFormatter);
│ │ │ │ +
112
│ │ │ │ +
113// Define Key to be Testable by specializing gtsam::traits
│ │ │ │ +
114template<typename T> struct traits;
│ │ │ │
115
│ │ │ │ -
117 template <typename T>
│ │ │ │ -
118 using HasDerivedValueType = typename std::enable_if<
│ │ │ │ -
119 std::is_base_of<FactorType, typename T::value_type>::value>::type;
│ │ │ │ -
120
│ │ │ │ -
122 template <typename T>
│ │ │ │ -
123 using HasDerivedElementType = typename std::enable_if<std::is_base_of<
│ │ │ │ -
124 FactorType, typename T::value_type::element_type>::value>::type;
│ │ │ │ +
116template <>
│ │ │ │ +
│ │ │ │ +
117struct traits<Key> {
│ │ │ │ +
118 static void Print(const Key& val, const std::string& str = "") {
│ │ │ │ +
119 PrintKey(val, str);
│ │ │ │ +
120 }
│ │ │ │ +
121 static bool Equals(const Key& val1, const Key& val2, double tol = 1e-8) {
│ │ │ │ +
122 return val1 == val2;
│ │ │ │ +
123 }
│ │ │ │ +
124};
│ │ │ │ +
│ │ │ │
125
│ │ │ │ -
126 protected:
│ │ │ │ -
128 GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
│ │ │ │ +
126} // namespace gtsam
│ │ │ │ +
127
│ │ │ │ +
128
│ │ │ │
129
│ │ │ │ -
130
│ │ │ │ -
131 FastVector<sharedFactor> factors_;
│ │ │ │ -
132
│ │ │ │ -
│ │ │ │ -
134 bool isEqual(const FactorGraph& other) const {
│ │ │ │ -
135 return factors_ == other.factors_;
│ │ │ │ -
136 }
│ │ │ │ -
│ │ │ │ -
137
│ │ │ │ -
140
│ │ │ │ -
142 FactorGraph() {}
│ │ │ │ -
143
│ │ │ │ -
145 template <typename ITERATOR>
│ │ │ │ -
│ │ │ │ -
146 FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) {
│ │ │ │ -
147 push_back(firstFactor, lastFactor);
│ │ │ │ -
148 }
│ │ │ │ -
│ │ │ │ -
149
│ │ │ │ -
151 template <class CONTAINER>
│ │ │ │ -
│ │ │ │ -
152 explicit FactorGraph(const CONTAINER& factors) {
│ │ │ │ -
153 push_back(factors);
│ │ │ │ -
154 }
│ │ │ │ -
│ │ │ │ -
155
│ │ │ │ -
157
│ │ │ │ -
158 public:
│ │ │ │ -
161
│ │ │ │ -
164 virtual ~FactorGraph() = default;
│ │ │ │ -
165
│ │ │ │ -
170 template <class DERIVEDFACTOR, typename = IsDerived<DERIVEDFACTOR>>
│ │ │ │ -
│ │ │ │ -
171 FactorGraph(std::initializer_list<boost::shared_ptr<DERIVEDFACTOR>> sharedFactors)
│ │ │ │ -
172 : factors_(sharedFactors) {}
│ │ │ │ -
│ │ │ │ -
173
│ │ │ │ -
177
│ │ │ │ -
182 void reserve(size_t size) { factors_.reserve(size); }
│ │ │ │ -
183
│ │ │ │ -
185 template <class DERIVEDFACTOR>
│ │ │ │ -
│ │ │ │ -
186 IsDerived<DERIVEDFACTOR> push_back(boost::shared_ptr<DERIVEDFACTOR> factor) {
│ │ │ │ -
187 factors_.push_back(boost::shared_ptr<FACTOR>(factor));
│ │ │ │ -
188 }
│ │ │ │ -
│ │ │ │ -
189
│ │ │ │ -
191 template <class DERIVEDFACTOR, class... Args>
│ │ │ │ -
│ │ │ │ -
192 IsDerived<DERIVEDFACTOR> emplace_shared(Args&&... args) {
│ │ │ │ -
193 factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(
│ │ │ │ -
194 Eigen::aligned_allocator<DERIVEDFACTOR>(),
│ │ │ │ -
195 std::forward<Args>(args)...));
│ │ │ │ -
196 }
│ │ │ │ -
│ │ │ │ -
197
│ │ │ │ -
202 template <class DERIVEDFACTOR>
│ │ │ │ -
│ │ │ │ -
203 IsDerived<DERIVEDFACTOR> push_back(const DERIVEDFACTOR& factor) {
│ │ │ │ -
204 factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(
│ │ │ │ -
205 Eigen::aligned_allocator<DERIVEDFACTOR>(), factor));
│ │ │ │ -
206 }
│ │ │ │ -
│ │ │ │ -
207
│ │ │ │ -
209 template <class DERIVEDFACTOR>
│ │ │ │ -
│ │ │ │ -
210 IsDerived<DERIVEDFACTOR> add(boost::shared_ptr<DERIVEDFACTOR> factor) {
│ │ │ │ -
211 push_back(factor);
│ │ │ │ -
212 }
│ │ │ │ -
│ │ │ │ -
213
│ │ │ │ -
215 template <class DERIVEDFACTOR>
│ │ │ │ -
216 typename std::enable_if<
│ │ │ │ -
217 std::is_base_of<FactorType, DERIVEDFACTOR>::value,
│ │ │ │ -
218 boost::assign::list_inserter<RefCallPushBack<This>>>::type
│ │ │ │ -
│ │ │ │ -
219 operator+=(boost::shared_ptr<DERIVEDFACTOR> factor) {
│ │ │ │ -
220 return boost::assign::make_list_inserter(RefCallPushBack<This>(*this))(
│ │ │ │ -
221 factor);
│ │ │ │ -
222 }
│ │ │ │ -
│ │ │ │ -
223
│ │ │ │ -
227
│ │ │ │ -
232 template <typename ITERATOR>
│ │ │ │ -
│ │ │ │ -
233 HasDerivedElementType<ITERATOR> push_back(ITERATOR firstFactor,
│ │ │ │ -
234 ITERATOR lastFactor) {
│ │ │ │ -
235 factors_.insert(end(), firstFactor, lastFactor);
│ │ │ │ -
236 }
│ │ │ │ -
│ │ │ │ -
237
│ │ │ │ -
239 template <typename ITERATOR>
│ │ │ │ -
│ │ │ │ -
240 HasDerivedValueType<ITERATOR> push_back(ITERATOR firstFactor,
│ │ │ │ -
241 ITERATOR lastFactor) {
│ │ │ │ -
242 for (ITERATOR f = firstFactor; f != lastFactor; ++f) push_back(*f);
│ │ │ │ -
243 }
│ │ │ │ -
│ │ │ │ -
244
│ │ │ │ -
248
│ │ │ │ -
253 template <typename CONTAINER>
│ │ │ │ -
│ │ │ │ -
254 HasDerivedElementType<CONTAINER> push_back(const CONTAINER& container) {
│ │ │ │ -
255 push_back(container.begin(), container.end());
│ │ │ │ -
256 }
│ │ │ │ -
│ │ │ │ -
257
│ │ │ │ -
259 template <typename CONTAINER>
│ │ │ │ -
│ │ │ │ -
260 HasDerivedValueType<CONTAINER> push_back(const CONTAINER& container) {
│ │ │ │ -
261 push_back(container.begin(), container.end());
│ │ │ │ -
262 }
│ │ │ │ -
│ │ │ │ -
263
│ │ │ │ -
268 template <class FACTOR_OR_CONTAINER>
│ │ │ │ -
│ │ │ │ -
269 void add(const FACTOR_OR_CONTAINER& factorOrContainer) {
│ │ │ │ -
270 push_back(factorOrContainer);
│ │ │ │ -
271 }
│ │ │ │ -
│ │ │ │ -
272
│ │ │ │ -
277 template <class FACTOR_OR_CONTAINER>
│ │ │ │ -
│ │ │ │ -
278 boost::assign::list_inserter<CRefCallPushBack<This>> operator+=(
│ │ │ │ -
279 const FACTOR_OR_CONTAINER& factorOrContainer) {
│ │ │ │ -
280 return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this))(
│ │ │ │ -
281 factorOrContainer);
│ │ │ │ -
282 }
│ │ │ │ -
│ │ │ │ -
283
│ │ │ │ -
287
│ │ │ │ -
293 template <class CLIQUE>
│ │ │ │ -
294 typename std::enable_if<
│ │ │ │ -
295 std::is_base_of<This, typename CLIQUE::FactorGraphType>::value>::type
│ │ │ │ -
│ │ │ │ -
296 push_back(const BayesTree<CLIQUE>& bayesTree) {
│ │ │ │ -
297 bayesTree.addFactorsToGraph(this);
│ │ │ │ -
298 }
│ │ │ │ -
│ │ │ │ -
299
│ │ │ │ -
304 template <typename CONTAINER, typename = HasDerivedElementType<CONTAINER>>
│ │ │ │ -
305 FactorIndices add_factors(const CONTAINER& factors,
│ │ │ │ -
306 bool useEmptySlots = false);
│ │ │ │ -
307
│ │ │ │ -
311
│ │ │ │ -
313 virtual void print(const std::string& s = "FactorGraph",
│ │ │ │ -
314 const KeyFormatter& formatter = DefaultKeyFormatter) const;
│ │ │ │ -
315
│ │ │ │ -
317 bool equals(const This& fg, double tol = 1e-9) const;
│ │ │ │ -
319
│ │ │ │ -
320 public:
│ │ │ │ -
323
│ │ │ │ -
326 size_t size() const { return factors_.size(); }
│ │ │ │ -
327
│ │ │ │ -
330 bool empty() const { return factors_.empty(); }
│ │ │ │ -
331
│ │ │ │ -
335 const sharedFactor at(size_t i) const { return factors_.at(i); }
│ │ │ │ -
336
│ │ │ │ -
340 sharedFactor& at(size_t i) { return factors_.at(i); }
│ │ │ │ -
341
│ │ │ │ -
345 const sharedFactor operator[](size_t i) const { return at(i); }
│ │ │ │ -
346
│ │ │ │ -
350 sharedFactor& operator[](size_t i) { return at(i); }
│ │ │ │ -
351
│ │ │ │ -
353 const_iterator begin() const { return factors_.begin(); }
│ │ │ │ -
354
│ │ │ │ -
356 const_iterator end() const { return factors_.end(); }
│ │ │ │ -
357
│ │ │ │ -
359 sharedFactor front() const { return factors_.front(); }
│ │ │ │ -
360
│ │ │ │ -
362 sharedFactor back() const { return factors_.back(); }
│ │ │ │ -
363
│ │ │ │ -
365 double error(const HybridValues &values) const;
│ │ │ │ -
366
│ │ │ │ -
370
│ │ │ │ -
372 iterator begin() { return factors_.begin(); }
│ │ │ │ -
373
│ │ │ │ -
375 iterator end() { return factors_.end(); }
│ │ │ │ -
376
│ │ │ │ -
381 virtual void resize(size_t size) { factors_.resize(size); }
│ │ │ │ -
382
│ │ │ │ -
385 void remove(size_t i) { factors_.at(i).reset(); }
│ │ │ │ -
386
│ │ │ │ -
388 void replace(size_t index, sharedFactor factor) { at(index) = factor; }
│ │ │ │ -
389
│ │ │ │ -
391 iterator erase(iterator item) { return factors_.erase(item); }
│ │ │ │ -
392
│ │ │ │ -
│ │ │ │ -
394 iterator erase(iterator first, iterator last) {
│ │ │ │ -
395 return factors_.erase(first, last);
│ │ │ │ -
396 }
│ │ │ │ -
│ │ │ │ -
397
│ │ │ │ -
401
│ │ │ │ -
403 void dot(std::ostream& os,
│ │ │ │ -
404 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
405 const DotWriter& writer = DotWriter()) const;
│ │ │ │ -
406
│ │ │ │ -
408 std::string dot(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
409 const DotWriter& writer = DotWriter()) const;
│ │ │ │ -
410
│ │ │ │ -
412 void saveGraph(const std::string& filename,
│ │ │ │ -
413 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
414 const DotWriter& writer = DotWriter()) const;
│ │ │ │ -
415
│ │ │ │ -
419
│ │ │ │ -
421 size_t nrFactors() const;
│ │ │ │ -
422
│ │ │ │ -
425 KeySet keys() const;
│ │ │ │ -
426
│ │ │ │ -
430 KeyVector keyVector() const;
│ │ │ │ -
431
│ │ │ │ -
434 inline bool exists(size_t idx) const { return idx < size() && at(idx); }
│ │ │ │ -
435
│ │ │ │ -
436 private:
│ │ │ │ -
438 friend class boost::serialization::access;
│ │ │ │ -
439 template <class ARCHIVE>
│ │ │ │ -
440 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ -
441 ar& BOOST_SERIALIZATION_NVP(factors_);
│ │ │ │ -
442 }
│ │ │ │ -
443
│ │ │ │ -
445}; // FactorGraph
│ │ │ │ -
│ │ │ │ -
446} // namespace gtsam
│ │ │ │ -
447
│ │ │ │ -
448#include <gtsam/inference/FactorGraph-inst.h>
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ -
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ - │ │ │ │ -
FactorGraph-inst.h
Factor Graph Base Class.
│ │ │ │ -
DotWriter.h
Graphviz formatter.
│ │ │ │ +
130
│ │ │ │ +
FastMap.h
A thin wrapper around std::map that uses boost's fast_pool_allocator.
│ │ │ │ +
FastSet.h
A thin wrapper around std::set that uses boost's fast_pool_allocator.
│ │ │ │ +
FastList.h
A thin wrapper around std::list that uses boost's fast_pool_allocator.
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ +
types.h
Typedefs for easier changing of types.
│ │ │ │
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition Factor.h:34
│ │ │ │ +
gtsam::PrintKeyVector
void PrintKeyVector(const KeyVector &keys, const string &s, const KeyFormatter &keyFormatter)
Utility function to print sets of keys with optional prefix.
Definition Key.cpp:77
│ │ │ │ +
gtsam::PrintKey
void PrintKey(Key key, const string &s, const KeyFormatter &keyFormatter)
Utility function to print one key with optional prefix.
Definition Key.cpp:40
│ │ │ │ +
gtsam::PrintKeyList
void PrintKeyList(const KeyList &keys, const string &s, const KeyFormatter &keyFormatter)
Utility function to print sets of keys with optional prefix.
Definition Key.cpp:72
│ │ │ │ +
gtsam::PrintKeySet
void PrintKeySet(const KeySet &keys, const string &s, const KeyFormatter &keyFormatter)
Utility function to print sets of keys with optional prefix.
Definition Key.cpp:82
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::FastList
FastList is a thin wrapper around std::list that uses the boost fast_pool_allocator instead of the de...
Definition FastList.h:40
│ │ │ │ +
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ │ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ -
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ -
gtsam::FactorGraph::print
virtual void print(const std::string &s="FactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const
Print out graph to std::cout, with optional key formatter.
Definition FactorGraph-inst.h:37
│ │ │ │ -
gtsam::FactorGraph::isEqual
bool isEqual(const FactorGraph &other) const
Check exact equality of the factor pointers. Useful for derived ==.
Definition FactorGraph.h:134
│ │ │ │ -
gtsam::FactorGraph::keys
KeySet keys() const
Potentially slow function to return all keys involved, sorted, as a set.
Definition FactorGraph-inst.h:85
│ │ │ │ -
gtsam::FactorGraph::empty
bool empty() const
Check if the graph is empty (null factors set by remove() will cause this to return false).
Definition FactorGraph.h:330
│ │ │ │ -
gtsam::FactorGraph::add_factors
FactorIndices add_factors(const CONTAINER &factors, bool useEmptySlots=false)
Add new factors to a factor graph and returns a list of new factor indices, optionally finding and re...
Definition FactorGraph-inst.h:109
│ │ │ │ -
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition FactorGraph.h:186
│ │ │ │ -
gtsam::FactorGraph::dot
void dot(std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
Output to graphviz format, stream version.
Definition FactorGraph-inst.h:141
│ │ │ │ -
gtsam::FactorGraph::erase
iterator erase(iterator item)
Erase factor and rearrange other factors to take up the empty space.
Definition FactorGraph.h:391
│ │ │ │ -
gtsam::FactorGraph::add
void add(const FACTOR_OR_CONTAINER &factorOrContainer)
Add a factor or container of factors, including STL collections, BayesTrees, etc.
Definition FactorGraph.h:269
│ │ │ │ -
gtsam::FactorGraph::erase
iterator erase(iterator first, iterator last)
Erase factors and rearrange other factors to take up the empty space.
Definition FactorGraph.h:394
│ │ │ │ -
gtsam::FactorGraph::back
sharedFactor back() const
Get the last factor.
Definition FactorGraph.h:362
│ │ │ │ -
gtsam::FactorGraph::FactorGraph
FactorGraph(const CONTAINER &factors)
Construct from container of factors (shared_ptr or plain objects)
Definition FactorGraph.h:152
│ │ │ │ -
gtsam::FactorGraph::remove
void remove(size_t i)
delete factor without re-arranging indexes by inserting a nullptr pointer
Definition FactorGraph.h:385
│ │ │ │ -
gtsam::FactorGraph::keyVector
KeyVector keyVector() const
Potentially slow function to return all keys involved, sorted, as a vector.
Definition FactorGraph-inst.h:95
│ │ │ │ -
gtsam::FactorGraph::push_back
HasDerivedValueType< CONTAINER > push_back(const CONTAINER &container)
Push back non-pointer objects in a container (factors are copied).
Definition FactorGraph.h:260
│ │ │ │ -
gtsam::FactorGraph::FactorGraph
FactorGraph()
Default constructor.
Definition FactorGraph.h:142
│ │ │ │ -
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition FactorGraph.h:192
│ │ │ │ -
gtsam::FactorGraph::operator[]
sharedFactor & operator[](size_t i)
Get a specific factor by index (this does not check array bounds, as opposed to at() which does).
Definition FactorGraph.h:350
│ │ │ │ -
gtsam::FactorGraph::at
sharedFactor & at(size_t i)
Get a specific factor by index (this checks array bounds and may throw an exception,...
Definition FactorGraph.h:340
│ │ │ │ -
gtsam::FactorGraph::push_back
std::enable_if< std::is_base_of< This, typenameCLIQUE::FactorGraphType >::value >::type push_back(const BayesTree< CLIQUE > &bayesTree)
Push back a BayesTree as a collection of factors.
Definition FactorGraph.h:296
│ │ │ │ -
gtsam::FactorGraph::error
double error(const HybridValues &values) const
Add error for all factors.
Definition FactorGraph-inst.h:66
│ │ │ │ -
gtsam::FactorGraph::FactorGraph
FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
Constructor from iterator over factors (shared_ptr or plain objects)
Definition FactorGraph.h:146
│ │ │ │ -
gtsam::FactorGraph::resize
virtual void resize(size_t size)
Directly resize the number of factors in the graph.
Definition FactorGraph.h:381
│ │ │ │ -
gtsam::FactorGraph::nrFactors
size_t nrFactors() const
return the number of non-null factors
Definition FactorGraph-inst.h:76
│ │ │ │ -
gtsam::FactorGraph::size
size_t size() const
return the number of factors (including any null factors set by remove() ).
Definition FactorGraph.h:326
│ │ │ │ -
gtsam::FactorGraph::push_back
HasDerivedValueType< ITERATOR > push_back(ITERATOR firstFactor, ITERATOR lastFactor)
Push back many factors with an iterator (factors are copied)
Definition FactorGraph.h:240
│ │ │ │ -
gtsam::FactorGraph::front
sharedFactor front() const
Get the first factor.
Definition FactorGraph.h:359
│ │ │ │ -
gtsam::FactorGraph::end
const_iterator end() const
Iterator to end of factors.
Definition FactorGraph.h:356
│ │ │ │ -
gtsam::FactorGraph::push_back
HasDerivedElementType< CONTAINER > push_back(const CONTAINER &container)
Push back many factors as shared_ptr's in a container (factors are not copied)
Definition FactorGraph.h:254
│ │ │ │ -
gtsam::FactorGraph::FactorType
FACTOR FactorType
factor type
Definition FactorGraph.h:99
│ │ │ │ -
gtsam::FactorGraph::replace
void replace(size_t index, sharedFactor factor)
replace a factor by index
Definition FactorGraph.h:388
│ │ │ │ -
gtsam::FactorGraph::operator+=
boost::assign::list_inserter< CRefCallPushBack< This > > operator+=(const FACTOR_OR_CONTAINER &factorOrContainer)
Add a factor or container of factors, including STL collections, BayesTrees, etc.
Definition FactorGraph.h:278
│ │ │ │ -
gtsam::FactorGraph::operator+=
std::enable_if< std::is_base_of< FactorType, DERIVEDFACTOR >::value, boost::assign::list_inserter< RefCallPushBack< This > > >::type operator+=(boost::shared_ptr< DERIVEDFACTOR > factor)
+= works well with boost::assign list inserter.
Definition FactorGraph.h:219
│ │ │ │ -
gtsam::FactorGraph::begin
iterator begin()
non-const STL-style begin()
Definition FactorGraph.h:372
│ │ │ │ -
gtsam::FactorGraph::begin
const_iterator begin() const
Iterator to beginning of factors.
Definition FactorGraph.h:353
│ │ │ │ -
gtsam::FactorGraph::push_back
HasDerivedElementType< ITERATOR > push_back(ITERATOR firstFactor, ITERATOR lastFactor)
Push back many factors with an iterator over shared_ptr (factors are not copied)
Definition FactorGraph.h:233
│ │ │ │ -
gtsam::FactorGraph::add
IsDerived< DERIVEDFACTOR > add(boost::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition FactorGraph.h:210
│ │ │ │ -
gtsam::FactorGraph::access
friend class boost::serialization::access
Serialization function.
Definition FactorGraph.h:438
│ │ │ │ -
gtsam::FactorGraph::sharedFactor
boost::shared_ptr< FACTOR > sharedFactor
Shared pointer to a factor.
Definition FactorGraph.h:101
│ │ │ │ -
gtsam::FactorGraph::exists
bool exists(size_t idx) const
MATLAB interface utility: Checks whether a factor index idx exists in the graph and is a live pointer...
Definition FactorGraph.h:434
│ │ │ │ -
gtsam::FactorGraph::operator[]
const sharedFactor operator[](size_t i) const
Get a specific factor by index (this does not check array bounds, as opposed to at() which does).
Definition FactorGraph.h:345
│ │ │ │ -
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(const DERIVEDFACTOR &factor)
Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid the copy).
Definition FactorGraph.h:203
│ │ │ │ -
gtsam::FactorGraph::FactorGraph
FactorGraph(std::initializer_list< boost::shared_ptr< DERIVEDFACTOR > > sharedFactors)
Constructor that takes an initializer list of shared pointers.
Definition FactorGraph.h:171
│ │ │ │ -
gtsam::FactorGraph::end
iterator end()
non-const STL-style end()
Definition FactorGraph.h:375
│ │ │ │ -
gtsam::FactorGraph::factors_
FastVector< sharedFactor > factors_
concept check, makes sure FACTOR defines print and equals
Definition FactorGraph.h:131
│ │ │ │ -
gtsam::FactorGraph::at
const sharedFactor at(size_t i) const
Get a specific factor by index (this checks array bounds and may throw an exception,...
Definition FactorGraph.h:335
│ │ │ │ -
gtsam::FactorGraph::saveGraph
void saveGraph(const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
output to file with graphviz format.
Definition FactorGraph-inst.h:177
│ │ │ │ -
gtsam::FactorGraph::~FactorGraph
virtual ~FactorGraph()=default
Default destructor Public and virtual so boost serialization can call it.
│ │ │ │ -
gtsam::FactorGraph::reserve
void reserve(size_t size)
Reserve space for the specified number of factors if you know in advance how many there will be (work...
Definition FactorGraph.h:182
│ │ │ │ -
gtsam::BayesTree
Bayes tree.
Definition BayesTree.h:67
│ │ │ │ -
gtsam::BayesTree::addFactorsToGraph
void addFactorsToGraph(FactorGraph< FactorType > *graph) const
Add all cliques in this BayesTree to the specified factor graph.
Definition BayesTree-inst.h:168
│ │ │ │ -
gtsam::DotWriter
DotWriter is a helper class for writing graphviz .dot files.
Definition DotWriter.h:35
│ │ │ │ -
gtsam::CRefCallPushBack
Helper.
Definition FactorGraph.h:54
│ │ │ │ -
gtsam::RefCallPushBack
Helper.
Definition FactorGraph.h:67
│ │ │ │ -
gtsam::CRefCallAddCopy
Helper.
Definition FactorGraph.h:80
│ │ │ │ -
HybridValues
the error.
│ │ │ │ +
gtsam::StreamedKey
To use the key_formatter on Keys, they must be wrapped in a StreamedKey.
Definition Key.h:58
│ │ │ │ +
gtsam::key_formatter
Output stream manipulator that will format gtsam::Keys according to the given KeyFormatter,...
Definition Key.h:72
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,591 +1,187 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -FactorGraph.h │ │ │ │ │ +Key.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -21// \callgraph │ │ │ │ │ -22 │ │ │ │ │ -23#pragma once │ │ │ │ │ -24 │ │ │ │ │ -25#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_D_o_t_W_r_i_t_e_r_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ -27#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ -28#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_L_i_s_t_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_M_a_p_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_S_e_t_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ +26#include │ │ │ │ │ +27 │ │ │ │ │ +28#include │ │ │ │ │ 29 │ │ │ │ │ -30#include // for Eigen::aligned_allocator │ │ │ │ │ +30#include │ │ │ │ │ 31 │ │ │ │ │ -32#include │ │ │ │ │ -33#include │ │ │ │ │ -34#include │ │ │ │ │ -35#include │ │ │ │ │ +32namespace _g_t_s_a_m { │ │ │ │ │ +33 │ │ │ │ │ +_3_5using _K_e_y_F_o_r_m_a_t_t_e_r = std::function; │ │ │ │ │ 36 │ │ │ │ │ -37#include │ │ │ │ │ -38#include │ │ │ │ │ -39#include │ │ │ │ │ -40#include │ │ │ │ │ -41 │ │ │ │ │ -42namespace _g_t_s_a_m { │ │ │ │ │ -44typedef FastVector _F_a_c_t_o_r_I_n_d_i_c_e_s; │ │ │ │ │ -45 │ │ │ │ │ -46// Forward declarations │ │ │ │ │ -47template │ │ │ │ │ -48class BayesTree; │ │ │ │ │ -49 │ │ │ │ │ -50class _H_y_b_r_i_d_V_a_l_u_e_s; │ │ │ │ │ -51 │ │ │ │ │ -53template │ │ │ │ │ -_5_4class _C_R_e_f_C_a_l_l_P_u_s_h_B_a_c_k { │ │ │ │ │ -55 C& obj; │ │ │ │ │ +37// Helper function for DefaultKeyFormatter │ │ │ │ │ +38GTSAM_EXPORT std::string _defaultKeyFormatter(_K_e_y key); │ │ │ │ │ +39 │ │ │ │ │ +43static const _K_e_y_F_o_r_m_a_t_t_e_r DefaultKeyFormatter = &_defaultKeyFormatter; │ │ │ │ │ +44 │ │ │ │ │ +45// Helper function for Multi-robot Key Formatter │ │ │ │ │ +46GTSAM_EXPORT std::string _multirobotKeyFormatter(_g_t_s_a_m_:_:_K_e_y key); │ │ │ │ │ +47 │ │ │ │ │ +54static const _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r MultiRobotKeyFormatter = │ │ │ │ │ +55 &_multirobotKeyFormatter; │ │ │ │ │ 56 │ │ │ │ │ -57 public: │ │ │ │ │ -58 explicit _C_R_e_f_C_a_l_l_P_u_s_h_B_a_c_k(C& obj) : obj(obj) {} │ │ │ │ │ -59 template │ │ │ │ │ -60 void operator()(const A& a) { │ │ │ │ │ -61 obj.push_back(a); │ │ │ │ │ -62 } │ │ │ │ │ -63}; │ │ │ │ │ -64 │ │ │ │ │ -66template │ │ │ │ │ -_6_7class _R_e_f_C_a_l_l_P_u_s_h_B_a_c_k { │ │ │ │ │ -68 C& obj; │ │ │ │ │ -69 │ │ │ │ │ -70 public: │ │ │ │ │ -71 explicit _R_e_f_C_a_l_l_P_u_s_h_B_a_c_k(C& obj) : obj(obj) {} │ │ │ │ │ -72 template │ │ │ │ │ -73 void operator()(A& a) { │ │ │ │ │ -74 obj.push_back(a); │ │ │ │ │ -75 } │ │ │ │ │ -76}; │ │ │ │ │ +_5_8struct _S_t_r_e_a_m_e_d_K_e_y { │ │ │ │ │ +59 const _K_e_y &key_; │ │ │ │ │ +60 explicit _S_t_r_e_a_m_e_d_K_e_y(const _K_e_y &key) : key_(key) {} │ │ │ │ │ +61 GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &, const │ │ │ │ │ +_S_t_r_e_a_m_e_d_K_e_y &); │ │ │ │ │ +62}; │ │ │ │ │ +63 │ │ │ │ │ +_7_2class _k_e_y___f_o_r_m_a_t_t_e_r { │ │ │ │ │ +73 public: │ │ │ │ │ +74 explicit _k_e_y___f_o_r_m_a_t_t_e_r(_K_e_y_F_o_r_m_a_t_t_e_r v) : formatter_(v) {} │ │ │ │ │ +75 GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &, const │ │ │ │ │ +_k_e_y___f_o_r_m_a_t_t_e_r &); │ │ │ │ │ +76 GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &, const │ │ │ │ │ +_S_t_r_e_a_m_e_d_K_e_y &); │ │ │ │ │ 77 │ │ │ │ │ -79template │ │ │ │ │ -_8_0class _C_R_e_f_C_a_l_l_A_d_d_C_o_p_y { │ │ │ │ │ -81 C& obj; │ │ │ │ │ -82 │ │ │ │ │ -83 public: │ │ │ │ │ -84 explicit _C_R_e_f_C_a_l_l_A_d_d_C_o_p_y(C& obj) : obj(obj) {} │ │ │ │ │ -85 template │ │ │ │ │ -86 void operator()(const A& a) { │ │ │ │ │ -87 obj.addCopy(a); │ │ │ │ │ -88 } │ │ │ │ │ -89}; │ │ │ │ │ -90 │ │ │ │ │ -96template │ │ │ │ │ -_9_7class _F_a_c_t_o_r_G_r_a_p_h { │ │ │ │ │ -98 public: │ │ │ │ │ -_9_9 typedef FACTOR _F_a_c_t_o_r_T_y_p_e; │ │ │ │ │ -100 typedef boost::shared_ptr │ │ │ │ │ -_1_0_1 _s_h_a_r_e_d_F_a_c_t_o_r; │ │ │ │ │ -102 typedef _s_h_a_r_e_d_F_a_c_t_o_r value_type; │ │ │ │ │ -103 typedef typename _F_a_s_t_V_e_c_t_o_r_<_s_h_a_r_e_d_F_a_c_t_o_r_>_:_:_i_t_e_r_a_t_o_r iterator; │ │ │ │ │ -104 typedef typename _F_a_s_t_V_e_c_t_o_r_<_s_h_a_r_e_d_F_a_c_t_o_r_>_:_:_c_o_n_s_t___i_t_e_r_a_t_o_r const_iterator; │ │ │ │ │ -105 │ │ │ │ │ -106 private: │ │ │ │ │ -107 typedef _F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_> _T_h_i_s; │ │ │ │ │ -108 typedef boost::shared_ptr │ │ │ │ │ -109 shared_ptr; │ │ │ │ │ -110 │ │ │ │ │ -112 template │ │ │ │ │ -113 using IsDerived = typename std::enable_if< │ │ │ │ │ -114 std::is_base_of::value>::type; │ │ │ │ │ +78 private: │ │ │ │ │ +79 _K_e_y_F_o_r_m_a_t_t_e_r formatter_; │ │ │ │ │ +80 static void *&property(std::ios_base &s); │ │ │ │ │ +81 static void set_property(std::ios_base &s, const _K_e_y_F_o_r_m_a_t_t_e_r &f); │ │ │ │ │ +82 static _K_e_y_F_o_r_m_a_t_t_e_r *get_property(std::ios_base &s); │ │ │ │ │ +83}; │ │ │ │ │ +84 │ │ │ │ │ +_8_6using _K_e_y_V_e_c_t_o_r = _F_a_s_t_V_e_c_t_o_r_<_K_e_y_>; │ │ │ │ │ +87 │ │ │ │ │ +88// TODO(frank): Nothing fast about these :-( │ │ │ │ │ +89using _K_e_y_L_i_s_t = _F_a_s_t_L_i_s_t_<_K_e_y_>; │ │ │ │ │ +90using _K_e_y_S_e_t = _F_a_s_t_S_e_t_<_K_e_y_>; │ │ │ │ │ +91using KeyGroupMap = _F_a_s_t_M_a_p_<_K_e_y_,_ _i_n_t_>; │ │ │ │ │ +92 │ │ │ │ │ +94GTSAM_EXPORT void _P_r_i_n_t_K_e_y( │ │ │ │ │ +95 _K_e_y key, const std::string &s = "", │ │ │ │ │ +96 const _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter = DefaultKeyFormatter); │ │ │ │ │ +97 │ │ │ │ │ +99GTSAM_EXPORT void _P_r_i_n_t_K_e_y_L_i_s_t( │ │ │ │ │ +100 const _K_e_y_L_i_s_t &keys, const std::string &s = "", │ │ │ │ │ +101 const _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter = DefaultKeyFormatter); │ │ │ │ │ +102 │ │ │ │ │ +104GTSAM_EXPORT void _P_r_i_n_t_K_e_y_V_e_c_t_o_r( │ │ │ │ │ +105 const _K_e_y_V_e_c_t_o_r &keys, const std::string &s = "", │ │ │ │ │ +106 const _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter = DefaultKeyFormatter); │ │ │ │ │ +107 │ │ │ │ │ +109GTSAM_EXPORT void _P_r_i_n_t_K_e_y_S_e_t( │ │ │ │ │ +110 const _K_e_y_S_e_t &keys, const std::string &s = "", │ │ │ │ │ +111 const _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter = DefaultKeyFormatter); │ │ │ │ │ +112 │ │ │ │ │ +113// Define Key to be Testable by specializing gtsam::traits │ │ │ │ │ +114template struct _t_r_a_i_t_s; │ │ │ │ │ 115 │ │ │ │ │ -117 template │ │ │ │ │ -118 using HasDerivedValueType = typename std::enable_if< │ │ │ │ │ -119 std::is_base_of::value>::type; │ │ │ │ │ -120 │ │ │ │ │ -122 template │ │ │ │ │ -123 using HasDerivedElementType = typename std::enable_if::value>::type; │ │ │ │ │ +116template <> │ │ │ │ │ +_1_1_7struct _t_r_a_i_t_s<_K_e_y> { │ │ │ │ │ +118 static void Print(const _K_e_y& val, const std::string& str = "") { │ │ │ │ │ +119 _P_r_i_n_t_K_e_y(val, str); │ │ │ │ │ +120 } │ │ │ │ │ +121 static bool Equals(const _K_e_y& val1, const _K_e_y& val2, double tol = 1e-8) { │ │ │ │ │ +122 return val1 == val2; │ │ │ │ │ +123 } │ │ │ │ │ +124}; │ │ │ │ │ 125 │ │ │ │ │ -126 protected: │ │ │ │ │ -128 GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) │ │ │ │ │ +126} // namespace gtsam │ │ │ │ │ +127 │ │ │ │ │ +128 │ │ │ │ │ 129 │ │ │ │ │ 130 │ │ │ │ │ -_1_3_1 _F_a_s_t_V_e_c_t_o_r<_s_h_a_r_e_d_F_a_c_t_o_r> _f_a_c_t_o_r_s__; │ │ │ │ │ -132 │ │ │ │ │ -_1_3_4 bool _i_s_E_q_u_a_l(const _F_a_c_t_o_r_G_r_a_p_h& other) const { │ │ │ │ │ -135 return _f_a_c_t_o_r_s__ == other.factors_; │ │ │ │ │ -136 } │ │ │ │ │ -137 │ │ │ │ │ -140 │ │ │ │ │ -_1_4_2 _F_a_c_t_o_r_G_r_a_p_h() {} │ │ │ │ │ -143 │ │ │ │ │ -145 template │ │ │ │ │ -_1_4_6 _F_a_c_t_o_r_G_r_a_p_h(ITERATOR firstFactor, ITERATOR lastFactor) { │ │ │ │ │ -147 _p_u_s_h___b_a_c_k(firstFactor, lastFactor); │ │ │ │ │ -148 } │ │ │ │ │ -149 │ │ │ │ │ -151 template │ │ │ │ │ -_1_5_2 explicit _F_a_c_t_o_r_G_r_a_p_h(const CONTAINER& factors) { │ │ │ │ │ -153 _p_u_s_h___b_a_c_k(factors); │ │ │ │ │ -154 } │ │ │ │ │ -155 │ │ │ │ │ -157 │ │ │ │ │ -158 public: │ │ │ │ │ -161 │ │ │ │ │ -_1_6_4 virtual _~_F_a_c_t_o_r_G_r_a_p_h() = default; │ │ │ │ │ -165 │ │ │ │ │ -170 template > │ │ │ │ │ -_1_7_1 _F_a_c_t_o_r_G_r_a_p_h(std::initializer_list> │ │ │ │ │ -sharedFactors) │ │ │ │ │ -172 : _f_a_c_t_o_r_s__(sharedFactors) {} │ │ │ │ │ -173 │ │ │ │ │ -177 │ │ │ │ │ -_1_8_2 void _r_e_s_e_r_v_e(size_t _s_i_z_e) { _f_a_c_t_o_r_s__.reserve(_s_i_z_e); } │ │ │ │ │ -183 │ │ │ │ │ -185 template │ │ │ │ │ -_1_8_6 IsDerived _p_u_s_h___b_a_c_k(boost::shared_ptr factor) │ │ │ │ │ -{ │ │ │ │ │ -187 _f_a_c_t_o_r_s__.push_back(boost::shared_ptr(factor)); │ │ │ │ │ -188 } │ │ │ │ │ -189 │ │ │ │ │ -191 template │ │ │ │ │ -_1_9_2 IsDerived _e_m_p_l_a_c_e___s_h_a_r_e_d(Args&&... args) { │ │ │ │ │ -193 _f_a_c_t_o_r_s__.push_back(boost::allocate_shared( │ │ │ │ │ -194 Eigen::aligned_allocator(), │ │ │ │ │ -195 std::forward(args)...)); │ │ │ │ │ -196 } │ │ │ │ │ -197 │ │ │ │ │ -202 template │ │ │ │ │ -_2_0_3 IsDerived _p_u_s_h___b_a_c_k(const DERIVEDFACTOR& factor) { │ │ │ │ │ -204 _f_a_c_t_o_r_s__.push_back(boost::allocate_shared( │ │ │ │ │ -205 Eigen::aligned_allocator(), factor)); │ │ │ │ │ -206 } │ │ │ │ │ -207 │ │ │ │ │ -209 template │ │ │ │ │ -_2_1_0 IsDerived _a_d_d(boost::shared_ptr factor) { │ │ │ │ │ -211 _p_u_s_h___b_a_c_k(factor); │ │ │ │ │ -212 } │ │ │ │ │ -213 │ │ │ │ │ -215 template │ │ │ │ │ -216 typename std::enable_if< │ │ │ │ │ -217 std::is_base_of::value, │ │ │ │ │ -218 boost::assign::list_inserter>>::type │ │ │ │ │ -_2_1_9 _o_p_e_r_a_t_o_r_+_=(boost::shared_ptr factor) { │ │ │ │ │ -220 return boost::assign::make_list_inserter(_R_e_f_C_a_l_l_P_u_s_h_B_a_c_k_<_T_h_i_s_>(*this))( │ │ │ │ │ -221 factor); │ │ │ │ │ -222 } │ │ │ │ │ -223 │ │ │ │ │ -227 │ │ │ │ │ -232 template │ │ │ │ │ -_2_3_3 HasDerivedElementType _p_u_s_h___b_a_c_k(ITERATOR firstFactor, │ │ │ │ │ -234 ITERATOR lastFactor) { │ │ │ │ │ -235 _f_a_c_t_o_r_s__.insert(_e_n_d(), firstFactor, lastFactor); │ │ │ │ │ -236 } │ │ │ │ │ -237 │ │ │ │ │ -239 template │ │ │ │ │ -_2_4_0 HasDerivedValueType _p_u_s_h___b_a_c_k(ITERATOR firstFactor, │ │ │ │ │ -241 ITERATOR lastFactor) { │ │ │ │ │ -242 for (ITERATOR f = firstFactor; f != lastFactor; ++f) _p_u_s_h___b_a_c_k(*f); │ │ │ │ │ -243 } │ │ │ │ │ -244 │ │ │ │ │ -248 │ │ │ │ │ -253 template │ │ │ │ │ -_2_5_4 HasDerivedElementType _p_u_s_h___b_a_c_k(const CONTAINER& container) { │ │ │ │ │ -255 _p_u_s_h___b_a_c_k(container.begin(), container.end()); │ │ │ │ │ -256 } │ │ │ │ │ -257 │ │ │ │ │ -259 template │ │ │ │ │ -_2_6_0 HasDerivedValueType _p_u_s_h___b_a_c_k(const CONTAINER& container) { │ │ │ │ │ -261 _p_u_s_h___b_a_c_k(container.begin(), container.end()); │ │ │ │ │ -262 } │ │ │ │ │ -263 │ │ │ │ │ -268 template │ │ │ │ │ -_2_6_9 void _a_d_d(const FACTOR_OR_CONTAINER& factorOrContainer) { │ │ │ │ │ -270 _p_u_s_h___b_a_c_k(factorOrContainer); │ │ │ │ │ -271 } │ │ │ │ │ -272 │ │ │ │ │ -277 template │ │ │ │ │ -_2_7_8 boost::assign::list_inserter> _o_p_e_r_a_t_o_r_+_=( │ │ │ │ │ -279 const FACTOR_OR_CONTAINER& factorOrContainer) { │ │ │ │ │ -280 return boost::assign::make_list_inserter(_C_R_e_f_C_a_l_l_P_u_s_h_B_a_c_k_<_T_h_i_s_>(*this))( │ │ │ │ │ -281 factorOrContainer); │ │ │ │ │ -282 } │ │ │ │ │ -283 │ │ │ │ │ -287 │ │ │ │ │ -293 template │ │ │ │ │ -294 typename std::enable_if< │ │ │ │ │ -295 std::is_base_of::value>::type │ │ │ │ │ -_2_9_6 _p_u_s_h___b_a_c_k(const _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>& bayesTree) { │ │ │ │ │ -297 bayesTree._a_d_d_F_a_c_t_o_r_s_T_o_G_r_a_p_h(this); │ │ │ │ │ -298 } │ │ │ │ │ -299 │ │ │ │ │ -304 template > │ │ │ │ │ -305 _F_a_c_t_o_r_I_n_d_i_c_e_s _a_d_d___f_a_c_t_o_r_s(const CONTAINER& factors, │ │ │ │ │ -306 bool useEmptySlots = false); │ │ │ │ │ -307 │ │ │ │ │ -311 │ │ │ │ │ -313 virtual void _p_r_i_n_t(const std::string& s = "FactorGraph", │ │ │ │ │ -314 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const; │ │ │ │ │ -315 │ │ │ │ │ -317 bool _e_q_u_a_l_s(const This& fg, double tol = 1e-9) const; │ │ │ │ │ -319 │ │ │ │ │ -320 public: │ │ │ │ │ -323 │ │ │ │ │ -_3_2_6 size_t _s_i_z_e() const { return _f_a_c_t_o_r_s__.size(); } │ │ │ │ │ -327 │ │ │ │ │ -_3_3_0 bool _e_m_p_t_y() const { return _f_a_c_t_o_r_s__.empty(); } │ │ │ │ │ -331 │ │ │ │ │ -_3_3_5 const _s_h_a_r_e_d_F_a_c_t_o_r _a_t(size_t i) const { return _f_a_c_t_o_r_s__.at(i); } │ │ │ │ │ -336 │ │ │ │ │ -_3_4_0 _s_h_a_r_e_d_F_a_c_t_o_r& _a_t(size_t i) { return _f_a_c_t_o_r_s__.at(i); } │ │ │ │ │ -341 │ │ │ │ │ -_3_4_5 const _s_h_a_r_e_d_F_a_c_t_o_r _o_p_e_r_a_t_o_r_[_](size_t i) const { return _a_t(i); } │ │ │ │ │ -346 │ │ │ │ │ -_3_5_0 _s_h_a_r_e_d_F_a_c_t_o_r& _o_p_e_r_a_t_o_r_[_](size_t i) { return _a_t(i); } │ │ │ │ │ -351 │ │ │ │ │ -_3_5_3 const_iterator _b_e_g_i_n() const { return _f_a_c_t_o_r_s__.begin(); } │ │ │ │ │ -354 │ │ │ │ │ -_3_5_6 const_iterator _e_n_d() const { return _f_a_c_t_o_r_s__.end(); } │ │ │ │ │ -357 │ │ │ │ │ -_3_5_9 _s_h_a_r_e_d_F_a_c_t_o_r _f_r_o_n_t() const { return _f_a_c_t_o_r_s__.front(); } │ │ │ │ │ -360 │ │ │ │ │ -_3_6_2 _s_h_a_r_e_d_F_a_c_t_o_r _b_a_c_k() const { return _f_a_c_t_o_r_s__.back(); } │ │ │ │ │ -363 │ │ │ │ │ -365 double _e_r_r_o_r(const _H_y_b_r_i_d_V_a_l_u_e_s &values) const; │ │ │ │ │ -366 │ │ │ │ │ -370 │ │ │ │ │ -_3_7_2 iterator _b_e_g_i_n() { return _f_a_c_t_o_r_s__.begin(); } │ │ │ │ │ -373 │ │ │ │ │ -_3_7_5 iterator _e_n_d() { return _f_a_c_t_o_r_s__.end(); } │ │ │ │ │ -376 │ │ │ │ │ -_3_8_1 virtual void _r_e_s_i_z_e(size_t _s_i_z_e) { _f_a_c_t_o_r_s__.resize(_s_i_z_e); } │ │ │ │ │ -382 │ │ │ │ │ -_3_8_5 void _r_e_m_o_v_e(size_t i) { _f_a_c_t_o_r_s__.at(i).reset(); } │ │ │ │ │ -386 │ │ │ │ │ -_3_8_8 void _r_e_p_l_a_c_e(size_t index, _s_h_a_r_e_d_F_a_c_t_o_r factor) { _a_t(index) = factor; } │ │ │ │ │ -389 │ │ │ │ │ -_3_9_1 iterator _e_r_a_s_e(iterator item) { return _f_a_c_t_o_r_s__.erase(item); } │ │ │ │ │ -392 │ │ │ │ │ -_3_9_4 iterator _e_r_a_s_e(iterator first, iterator last) { │ │ │ │ │ -395 return _f_a_c_t_o_r_s__.erase(first, last); │ │ │ │ │ -396 } │ │ │ │ │ -397 │ │ │ │ │ -401 │ │ │ │ │ -403 void _d_o_t(std::ostream& os, │ │ │ │ │ -404 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ -405 const _D_o_t_W_r_i_t_e_r& writer = _D_o_t_W_r_i_t_e_r()) const; │ │ │ │ │ -406 │ │ │ │ │ -408 std::string _d_o_t(const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ -409 const _D_o_t_W_r_i_t_e_r& writer = _D_o_t_W_r_i_t_e_r()) const; │ │ │ │ │ -410 │ │ │ │ │ -412 void _s_a_v_e_G_r_a_p_h(const std::string& filename, │ │ │ │ │ -413 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ -414 const _D_o_t_W_r_i_t_e_r& writer = _D_o_t_W_r_i_t_e_r()) const; │ │ │ │ │ -415 │ │ │ │ │ -419 │ │ │ │ │ -421 size_t _n_r_F_a_c_t_o_r_s() const; │ │ │ │ │ -422 │ │ │ │ │ -425 _K_e_y_S_e_t _k_e_y_s() const; │ │ │ │ │ -426 │ │ │ │ │ -430 _K_e_y_V_e_c_t_o_r _k_e_y_V_e_c_t_o_r() const; │ │ │ │ │ -431 │ │ │ │ │ -_4_3_4 inline bool _e_x_i_s_t_s(size_t idx) const { return idx < _s_i_z_e() && _a_t(idx); } │ │ │ │ │ -435 │ │ │ │ │ -436 private: │ │ │ │ │ -_4_3_8 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -439 template │ │ │ │ │ -440 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ -441 ar& BOOST_SERIALIZATION_NVP(_f_a_c_t_o_r_s__); │ │ │ │ │ -442 } │ │ │ │ │ -443 │ │ │ │ │ -445}; // FactorGraph │ │ │ │ │ -446} // namespace gtsam │ │ │ │ │ -447 │ │ │ │ │ -448#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_-_i_n_s_t_._h> │ │ │ │ │ +_F_a_s_t_M_a_p_._h │ │ │ │ │ +A thin wrapper around std::map that uses boost's fast_pool_allocator. │ │ │ │ │ +_F_a_s_t_S_e_t_._h │ │ │ │ │ +A thin wrapper around std::set that uses boost's fast_pool_allocator. │ │ │ │ │ +_F_a_s_t_L_i_s_t_._h │ │ │ │ │ +A thin wrapper around std::list that uses boost's fast_pool_allocator. │ │ │ │ │ _T_e_s_t_a_b_l_e_._h │ │ │ │ │ Concept check for values that can be used in unit tests. │ │ │ │ │ _F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ -_K_e_y_._h │ │ │ │ │ -_F_a_c_t_o_r_G_r_a_p_h_-_i_n_s_t_._h │ │ │ │ │ -Factor Graph Base Class. │ │ │ │ │ -_D_o_t_W_r_i_t_e_r_._h │ │ │ │ │ -Graphviz formatter. │ │ │ │ │ +_t_y_p_e_s_._h │ │ │ │ │ +Typedefs for easier changing of types. │ │ │ │ │ _g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ FastVector │ │ │ │ │ FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ FastVector< Key > KeyVector │ │ │ │ │ Define collection type once and for all - also used in wrappers. │ │ │ │ │ DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_I_n_d_i_c_e_s │ │ │ │ │ -FastVector< FactorIndex > FactorIndices │ │ │ │ │ -Define collection types: │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_i_n_t_K_e_y_V_e_c_t_o_r │ │ │ │ │ +void PrintKeyVector(const KeyVector &keys, const string &s, const KeyFormatter │ │ │ │ │ +&keyFormatter) │ │ │ │ │ +Utility function to print sets of keys with optional prefix. │ │ │ │ │ +DDeeffiinniittiioonn Key.cpp:77 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_i_n_t_K_e_y │ │ │ │ │ +void PrintKey(Key key, const string &s, const KeyFormatter &keyFormatter) │ │ │ │ │ +Utility function to print one key with optional prefix. │ │ │ │ │ +DDeeffiinniittiioonn Key.cpp:40 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_i_n_t_K_e_y_L_i_s_t │ │ │ │ │ +void PrintKeyList(const KeyList &keys, const string &s, const KeyFormatter │ │ │ │ │ +&keyFormatter) │ │ │ │ │ +Utility function to print sets of keys with optional prefix. │ │ │ │ │ +DDeeffiinniittiioonn Key.cpp:72 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_i_n_t_K_e_y_S_e_t │ │ │ │ │ +void PrintKeySet(const KeySet &keys, const string &s, const KeyFormatter │ │ │ │ │ +&keyFormatter) │ │ │ │ │ +Utility function to print sets of keys with optional prefix. │ │ │ │ │ +DDeeffiinniittiioonn Key.cpp:82 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t │ │ │ │ │ +FastList is a thin wrapper around std::list that uses the boost │ │ │ │ │ +fast_pool_allocator instead of the de... │ │ │ │ │ +DDeeffiinniittiioonn FastList.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ +FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ +fast_pool_allocator instead of the defa... │ │ │ │ │ +DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ _g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ -nodes. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_p_r_i_n_t │ │ │ │ │ -virtual void print(const std::string &s="FactorGraph", const KeyFormatter │ │ │ │ │ -&formatter=DefaultKeyFormatter) const │ │ │ │ │ -Print out graph to std::cout, with optional key formatter. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph-inst.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_i_s_E_q_u_a_l │ │ │ │ │ -bool isEqual(const FactorGraph &other) const │ │ │ │ │ -Check exact equality of the factor pointers. Useful for derived ==. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:134 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_k_e_y_s │ │ │ │ │ -KeySet keys() const │ │ │ │ │ -Potentially slow function to return all keys involved, sorted, as a set. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph-inst.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_e_m_p_t_y │ │ │ │ │ -bool empty() const │ │ │ │ │ -Check if the graph is empty (null factors set by remove() will cause this to │ │ │ │ │ -return false). │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:330 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d___f_a_c_t_o_r_s │ │ │ │ │ -FactorIndices add_factors(const CONTAINER &factors, bool useEmptySlots=false) │ │ │ │ │ -Add new factors to a factor graph and returns a list of new factor indices, │ │ │ │ │ -optionally finding and re... │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph-inst.h:109 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_p_u_s_h___b_a_c_k │ │ │ │ │ -IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor) │ │ │ │ │ -Add a factor directly using a shared_ptr. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:186 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_d_o_t │ │ │ │ │ -void dot(std::ostream &os, const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const │ │ │ │ │ -Output to graphviz format, stream version. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph-inst.h:141 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_e_r_a_s_e │ │ │ │ │ -iterator erase(iterator item) │ │ │ │ │ -Erase factor and rearrange other factors to take up the empty space. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:391 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d │ │ │ │ │ -void add(const FACTOR_OR_CONTAINER &factorOrContainer) │ │ │ │ │ -Add a factor or container of factors, including STL collections, BayesTrees, │ │ │ │ │ -etc. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:269 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_e_r_a_s_e │ │ │ │ │ -iterator erase(iterator first, iterator last) │ │ │ │ │ -Erase factors and rearrange other factors to take up the empty space. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:394 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_b_a_c_k │ │ │ │ │ -sharedFactor back() const │ │ │ │ │ -Get the last factor. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:362 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -FactorGraph(const CONTAINER &factors) │ │ │ │ │ -Construct from container of factors (shared_ptr or plain objects) │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:152 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_r_e_m_o_v_e │ │ │ │ │ -void remove(size_t i) │ │ │ │ │ -delete factor without re-arranging indexes by inserting a nullptr pointer │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:385 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_k_e_y_V_e_c_t_o_r │ │ │ │ │ -KeyVector keyVector() const │ │ │ │ │ -Potentially slow function to return all keys involved, sorted, as a vector. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph-inst.h:95 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_p_u_s_h___b_a_c_k │ │ │ │ │ -HasDerivedValueType< CONTAINER > push_back(const CONTAINER &container) │ │ │ │ │ -Push back non-pointer objects in a container (factors are copied). │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:260 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -FactorGraph() │ │ │ │ │ -Default constructor. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:142 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_e_m_p_l_a_c_e___s_h_a_r_e_d │ │ │ │ │ -IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args) │ │ │ │ │ -Emplace a shared pointer to factor of given type. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:192 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_o_p_e_r_a_t_o_r_[_] │ │ │ │ │ -sharedFactor & operator[](size_t i) │ │ │ │ │ -Get a specific factor by index (this does not check array bounds, as opposed to │ │ │ │ │ -at() which does). │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:350 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_a_t │ │ │ │ │ -sharedFactor & at(size_t i) │ │ │ │ │ -Get a specific factor by index (this checks array bounds and may throw an │ │ │ │ │ -exception,... │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:340 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_p_u_s_h___b_a_c_k │ │ │ │ │ -std::enable_if< std::is_base_of< This, typenameCLIQUE::FactorGraphType >::value │ │ │ │ │ ->::type push_back(const BayesTree< CLIQUE > &bayesTree) │ │ │ │ │ -Push back a BayesTree as a collection of factors. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:296 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_e_r_r_o_r │ │ │ │ │ -double error(const HybridValues &values) const │ │ │ │ │ -Add error for all factors. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph-inst.h:66 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) │ │ │ │ │ -Constructor from iterator over factors (shared_ptr or plain objects) │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:146 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_r_e_s_i_z_e │ │ │ │ │ -virtual void resize(size_t size) │ │ │ │ │ -Directly resize the number of factors in the graph. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:381 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_n_r_F_a_c_t_o_r_s │ │ │ │ │ -size_t nrFactors() const │ │ │ │ │ -return the number of non-null factors │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph-inst.h:76 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_s_i_z_e │ │ │ │ │ -size_t size() const │ │ │ │ │ -return the number of factors (including any null factors set by remove() ). │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:326 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_p_u_s_h___b_a_c_k │ │ │ │ │ -HasDerivedValueType< ITERATOR > push_back(ITERATOR firstFactor, ITERATOR │ │ │ │ │ -lastFactor) │ │ │ │ │ -Push back many factors with an iterator (factors are copied) │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:240 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_f_r_o_n_t │ │ │ │ │ -sharedFactor front() const │ │ │ │ │ -Get the first factor. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:359 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_e_n_d │ │ │ │ │ -const_iterator end() const │ │ │ │ │ -Iterator to end of factors. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:356 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_p_u_s_h___b_a_c_k │ │ │ │ │ -HasDerivedElementType< CONTAINER > push_back(const CONTAINER &container) │ │ │ │ │ -Push back many factors as shared_ptr's in a container (factors are not copied) │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:254 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_F_a_c_t_o_r_T_y_p_e │ │ │ │ │ -FACTOR FactorType │ │ │ │ │ -factor type │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:99 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_r_e_p_l_a_c_e │ │ │ │ │ -void replace(size_t index, sharedFactor factor) │ │ │ │ │ -replace a factor by index │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:388 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_o_p_e_r_a_t_o_r_+_= │ │ │ │ │ -boost::assign::list_inserter< CRefCallPushBack< This > > operator+=(const │ │ │ │ │ -FACTOR_OR_CONTAINER &factorOrContainer) │ │ │ │ │ -Add a factor or container of factors, including STL collections, BayesTrees, │ │ │ │ │ -etc. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:278 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_o_p_e_r_a_t_o_r_+_= │ │ │ │ │ -std::enable_if< std::is_base_of< FactorType, DERIVEDFACTOR >::value, boost:: │ │ │ │ │ -assign::list_inserter< RefCallPushBack< This > > >::type operator+=(boost:: │ │ │ │ │ -shared_ptr< DERIVEDFACTOR > factor) │ │ │ │ │ -+= works well with boost::assign list inserter. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:219 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_b_e_g_i_n │ │ │ │ │ -iterator begin() │ │ │ │ │ -non-const STL-style begin() │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:372 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_b_e_g_i_n │ │ │ │ │ -const_iterator begin() const │ │ │ │ │ -Iterator to beginning of factors. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:353 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_p_u_s_h___b_a_c_k │ │ │ │ │ -HasDerivedElementType< ITERATOR > push_back(ITERATOR firstFactor, ITERATOR │ │ │ │ │ -lastFactor) │ │ │ │ │ -Push back many factors with an iterator over shared_ptr (factors are not │ │ │ │ │ -copied) │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:233 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d │ │ │ │ │ -IsDerived< DERIVEDFACTOR > add(boost::shared_ptr< DERIVEDFACTOR > factor) │ │ │ │ │ -add is a synonym for push_back. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:210 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:438 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d_F_a_c_t_o_r │ │ │ │ │ -boost::shared_ptr< FACTOR > sharedFactor │ │ │ │ │ -Shared pointer to a factor. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:101 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_e_x_i_s_t_s │ │ │ │ │ -bool exists(size_t idx) const │ │ │ │ │ -MATLAB interface utility: Checks whether a factor index idx exists in the graph │ │ │ │ │ -and is a live pointer... │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:434 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_o_p_e_r_a_t_o_r_[_] │ │ │ │ │ -const sharedFactor operator[](size_t i) const │ │ │ │ │ -Get a specific factor by index (this does not check array bounds, as opposed to │ │ │ │ │ -at() which does). │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:345 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_p_u_s_h___b_a_c_k │ │ │ │ │ -IsDerived< DERIVEDFACTOR > push_back(const DERIVEDFACTOR &factor) │ │ │ │ │ -Add a factor by value, will be copy-constructed (use push_back with a │ │ │ │ │ -shared_ptr to avoid the copy). │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:203 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -FactorGraph(std::initializer_list< boost::shared_ptr< DERIVEDFACTOR > > │ │ │ │ │ -sharedFactors) │ │ │ │ │ -Constructor that takes an initializer list of shared pointers. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:171 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_e_n_d │ │ │ │ │ -iterator end() │ │ │ │ │ -non-const STL-style end() │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:375 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_f_a_c_t_o_r_s__ │ │ │ │ │ -FastVector< sharedFactor > factors_ │ │ │ │ │ -concept check, makes sure FACTOR defines print and equals │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:131 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_a_t │ │ │ │ │ -const sharedFactor at(size_t i) const │ │ │ │ │ -Get a specific factor by index (this checks array bounds and may throw an │ │ │ │ │ -exception,... │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:335 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_s_a_v_e_G_r_a_p_h │ │ │ │ │ -void saveGraph(const std::string &filename, const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const │ │ │ │ │ -output to file with graphviz format. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph-inst.h:177 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_~_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -virtual ~FactorGraph()=default │ │ │ │ │ -Default destructor Public and virtual so boost serialization can call it. │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_r_e_s_e_r_v_e │ │ │ │ │ -void reserve(size_t size) │ │ │ │ │ -Reserve space for the specified number of factors if you know in advance how │ │ │ │ │ -many there will be (work... │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:182 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e │ │ │ │ │ -Bayes tree. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_a_d_d_F_a_c_t_o_r_s_T_o_G_r_a_p_h │ │ │ │ │ -void addFactorsToGraph(FactorGraph< FactorType > *graph) const │ │ │ │ │ -Add all cliques in this BayesTree to the specified factor graph. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:168 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r │ │ │ │ │ -DotWriter is a helper class for writing graphviz .dot files. │ │ │ │ │ -DDeeffiinniittiioonn DotWriter.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_C_R_e_f_C_a_l_l_P_u_s_h_B_a_c_k │ │ │ │ │ -Helper. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:54 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_f_C_a_l_l_P_u_s_h_B_a_c_k │ │ │ │ │ -Helper. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_C_R_e_f_C_a_l_l_A_d_d_C_o_p_y │ │ │ │ │ -Helper. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:80 │ │ │ │ │ -_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -the error. │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_r_e_a_m_e_d_K_e_y │ │ │ │ │ +To use the key_formatter on Keys, they must be wrapped in a StreamedKey. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_k_e_y___f_o_r_m_a_t_t_e_r │ │ │ │ │ +Output stream manipulator that will format gtsam::Keys according to the given │ │ │ │ │ +KeyFormatter,... │ │ │ │ │ +DDeeffiinniittiioonn Key.h:72 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ + * _K_e_y_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00614_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/MetisIndex-inl.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/graph-inl.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,104 +98,338 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
MetisIndex-inl.h
│ │ │ │ +
graph-inl.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ +
12/*
│ │ │ │ +
13 * @file graph-inl.h
│ │ │ │ +
14 * @brief Graph algorithm using boost library
│ │ │ │ +
15 * @author Kai Ni
│ │ │ │ +
16 */
│ │ │ │ +
17
│ │ │ │
18#pragma once
│ │ │ │
19
│ │ │ │ -
20#include <map>
│ │ │ │ -
21#include <vector>
│ │ │ │ -
22
│ │ │ │ -
23namespace gtsam {
│ │ │ │ -
24
│ │ │ │ -
25/* ************************************************************************* */
│ │ │ │ -
26template<class FACTORGRAPH>
│ │ │ │ -
│ │ │ │ -
27void MetisIndex::augment(const FACTORGRAPH& factors) {
│ │ │ │ -
28 std::map<int32_t, std::set<int32_t> > iAdjMap; // Stores a set of keys that are adjacent to key x, with adjMap.first
│ │ │ │ -
29 std::map<int32_t, std::set<int32_t> >::iterator iAdjMapIt;
│ │ │ │ -
30 std::set<Key> keySet;
│ │ │ │ +
20#include <stdexcept>
│ │ │ │ +
21#ifdef __GNUC__
│ │ │ │ +
22#pragma GCC diagnostic push
│ │ │ │ +
23#pragma GCC diagnostic ignored "-Wunused-variable"
│ │ │ │ +
24//#pragma GCC diagnostic ignored "-Wunneeded-internal-declaration"
│ │ │ │ +
25#endif
│ │ │ │ +
26#include <boost/graph/breadth_first_search.hpp>
│ │ │ │ +
27#ifdef __GNUC__
│ │ │ │ +
28#pragma GCC diagnostic pop
│ │ │ │ +
29#endif
│ │ │ │ +
30#include <boost/graph/prim_minimum_spanning_tree.hpp>
│ │ │ │
31
│ │ │ │ -
32 /* ********** Convert to CSR format ********** */
│ │ │ │ -
33 // Assuming that vertex numbering starts from 0 (C style),
│ │ │ │ -
34 // then the adjacency list of vertex i is stored in array adjncy
│ │ │ │ -
35 // starting at index xadj[i] and ending at(but not including)
│ │ │ │ -
36 // index xadj[i + 1](i.e., adjncy[xadj[i]] through
│ │ │ │ -
37 // and including adjncy[xadj[i + 1] - 1]).
│ │ │ │ -
38 int32_t keyCounter = 0;
│ │ │ │ -
39
│ │ │ │ -
40 // First: Record a copy of each key inside the factorgraph and create a
│ │ │ │ -
41 // key to integer mapping. This is referenced during the adjaceny step
│ │ │ │ -
42 for (size_t i = 0; i < factors.size(); i++) {
│ │ │ │ -
43 if (factors[i]) {
│ │ │ │ -
44 for(const Key& key: *factors[i]) {
│ │ │ │ -
45 keySet.insert(keySet.end(), key); // Keep a track of all unique keys
│ │ │ │ -
46 if (intKeyBMap_.left.find(key) == intKeyBMap_.left.end()) {
│ │ │ │ -
47 intKeyBMap_.insert(bm_type::value_type(key, keyCounter));
│ │ │ │ -
48 keyCounter++;
│ │ │ │ -
49 }
│ │ │ │ -
50 }
│ │ │ │ -
51 }
│ │ │ │ -
52 }
│ │ │ │ +
32#include <gtsam/inference/graph.h>
│ │ │ │ +
33
│ │ │ │ +
34namespace gtsam {
│ │ │ │ +
35
│ │ │ │ +
36/* ************************************************************************* */
│ │ │ │ +
37template <class KEY>
│ │ │ │ +
│ │ │ │ +
38class ordering_key_visitor : public boost::default_bfs_visitor {
│ │ │ │ +
39public:
│ │ │ │ +
40 ordering_key_visitor(std::list<KEY>& ordering_in) : ordering_(ordering_in) {}
│ │ │ │ +
41 template <typename Vertex, typename Graph> void discover_vertex(Vertex v, const Graph& g) const {
│ │ │ │ +
42 KEY key = boost::get(boost::vertex_name, g, v);
│ │ │ │ +
43 ordering_.push_front(key);
│ │ │ │ +
44 }
│ │ │ │ +
45 std::list<KEY>& ordering_;
│ │ │ │ +
46};
│ │ │ │ +
│ │ │ │ +
47
│ │ │ │ +
48/* ************************************************************************* */
│ │ │ │ +
49template<class KEY>
│ │ │ │ +
│ │ │ │ +
50std::list<KEY> predecessorMap2Keys(const PredecessorMap<KEY>& p_map) {
│ │ │ │ +
51
│ │ │ │ +
52 typedef typename SGraph<KEY>::Vertex SVertex;
│ │ │ │
53
│ │ │ │ -
54 // Create an adjacency mapping that stores the set of all adjacent keys for every key
│ │ │ │ -
55 for (size_t i = 0; i < factors.size(); i++) {
│ │ │ │ -
56 if (factors[i]) {
│ │ │ │ -
57 for(const Key& k1: *factors[i])
│ │ │ │ -
58 for(const Key& k2: *factors[i])
│ │ │ │ -
59 if (k1 != k2) {
│ │ │ │ -
60 // Store both in Key and int32_t format
│ │ │ │ -
61 int i = intKeyBMap_.left.at(k1);
│ │ │ │ -
62 int j = intKeyBMap_.left.at(k2);
│ │ │ │ -
63 iAdjMap[i].insert(iAdjMap[i].end(), j);
│ │ │ │ -
64 }
│ │ │ │ -
65 }
│ │ │ │ -
66 }
│ │ │ │ -
67
│ │ │ │ -
68 // Number of keys referenced in this factor graph
│ │ │ │ -
69 nKeys_ = keySet.size();
│ │ │ │ -
70
│ │ │ │ -
71 xadj_.push_back(0); // Always set the first index to zero
│ │ │ │ -
72 for (iAdjMapIt = iAdjMap.begin(); iAdjMapIt != iAdjMap.end(); ++iAdjMapIt) {
│ │ │ │ -
73 std::vector<int32_t> temp;
│ │ │ │ -
74 // Copy from the FastSet into a temporary vector
│ │ │ │ -
75 std::copy(iAdjMapIt->second.begin(), iAdjMapIt->second.end(),
│ │ │ │ -
76 std::back_inserter(temp));
│ │ │ │ -
77 // Insert each index's set in order by appending them to the end of adj_
│ │ │ │ -
78 adj_.insert(adj_.end(), temp.begin(), temp.end());
│ │ │ │ -
79 //adj_.push_back(temp);
│ │ │ │ -
80 xadj_.push_back((int32_t) adj_.size());
│ │ │ │ -
81 }
│ │ │ │ -
82}
│ │ │ │ +
54 SGraph<KEY> g;
│ │ │ │ +
55 SVertex root;
│ │ │ │ +
56 std::map<KEY, SVertex> key2vertex;
│ │ │ │ +
57 boost::tie(g, root, key2vertex) = gtsam::predecessorMap2Graph<SGraph<KEY>, SVertex, KEY>(p_map);
│ │ │ │ +
58
│ │ │ │ +
59 // breadth first visit on the graph
│ │ │ │ +
60 std::list<KEY> keys;
│ │ │ │ +
61 ordering_key_visitor<KEY> vis(keys);
│ │ │ │ +
62 boost::breadth_first_search(g, root, boost::visitor(vis));
│ │ │ │ +
63 return keys;
│ │ │ │ +
64}
│ │ │ │ +
│ │ │ │ +
65
│ │ │ │ +
66/* ************************************************************************* */
│ │ │ │ +
67template<class G, class F, class KEY>
│ │ │ │ +
│ │ │ │ +
68SDGraph<KEY> toBoostGraph(const G& graph) {
│ │ │ │ +
69 // convert the factor graph to boost graph
│ │ │ │ +
70 SDGraph<KEY> g;
│ │ │ │ +
71 typedef typename boost::graph_traits<SDGraph<KEY> >::vertex_descriptor BoostVertex;
│ │ │ │ +
72 std::map<KEY, BoostVertex> key2vertex;
│ │ │ │ +
73 typename G::const_iterator itFactor;
│ │ │ │ +
74
│ │ │ │ +
75 // Loop over the factors
│ │ │ │ +
76 for(itFactor=graph.begin(); itFactor!=graph.end(); itFactor++) {
│ │ │ │ +
77
│ │ │ │ +
78 // Ignore factors that are not binary
│ │ │ │ +
79 if ((*itFactor)->keys().size() != 2)
│ │ │ │ +
80 continue;
│ │ │ │ +
81
│ │ │ │ +
82 // Cast the factor to the user-specified factor type F
│ │ │ │ +
83 boost::shared_ptr<F> factor = boost::dynamic_pointer_cast<F>(*itFactor);
│ │ │ │ +
84 // Ignore factors that are not of type F
│ │ │ │ +
85 if (!factor) continue;
│ │ │ │ +
86
│ │ │ │ +
87 // Retrieve the 2 keys (nodes) the factor (edge) is incident on
│ │ │ │ +
88 KEY key1 = factor->keys()[0];
│ │ │ │ +
89 KEY key2 = factor->keys()[1];
│ │ │ │ +
90
│ │ │ │ +
91 BoostVertex v1, v2;
│ │ │ │ +
92
│ │ │ │ +
93 // If key1 is a new key, add it to the key2vertex map, else get the corresponding vertex id
│ │ │ │ +
94 if (key2vertex.find(key1) == key2vertex.end()) {
│ │ │ │ +
95 v1 = add_vertex(key1, g);
│ │ │ │ +
96 key2vertex.insert(std::pair<KEY,KEY>(key1, v1));
│ │ │ │ +
97 } else
│ │ │ │ +
98 v1 = key2vertex[key1];
│ │ │ │ +
99
│ │ │ │ +
100 // If key2 is a new key, add it to the key2vertex map, else get the corresponding vertex id
│ │ │ │ +
101 if (key2vertex.find(key2) == key2vertex.end()) {
│ │ │ │ +
102 v2 = add_vertex(key2, g);
│ │ │ │ +
103 key2vertex.insert(std::pair<KEY,KEY>(key2, v2));
│ │ │ │ +
104 } else
│ │ │ │ +
105 v2 = key2vertex[key2];
│ │ │ │ +
106
│ │ │ │ +
107 // Add an edge with weight 1.0
│ │ │ │ +
108 boost::property<boost::edge_weight_t, double> edge_property(1.0); // assume constant edge weight here
│ │ │ │ +
109 boost::add_edge(v1, v2, edge_property, g);
│ │ │ │ +
110 }
│ │ │ │ +
111
│ │ │ │ +
112 return g;
│ │ │ │ +
113}
│ │ │ │ +
│ │ │ │ +
114
│ │ │ │ +
115/* ************************************************************************* */
│ │ │ │ +
116template<class G, class V, class KEY>
│ │ │ │ +
117boost::tuple<G, V, std::map<KEY,V> >
│ │ │ │ +
│ │ │ │ +
118predecessorMap2Graph(const PredecessorMap<KEY>& p_map) {
│ │ │ │ +
119
│ │ │ │ +
120 G g;
│ │ │ │ +
121 std::map<KEY, V> key2vertex;
│ │ │ │ +
122 V v1, v2, root;
│ │ │ │ +
123 bool foundRoot = false;
│ │ │ │ +
124 for(auto child_parent: p_map) {
│ │ │ │ +
125 KEY child, parent;
│ │ │ │ +
126 std::tie(child,parent) = child_parent;
│ │ │ │ +
127 if (key2vertex.find(child) == key2vertex.end()) {
│ │ │ │ +
128 v1 = add_vertex(child, g);
│ │ │ │ +
129 key2vertex.insert(std::make_pair(child, v1));
│ │ │ │ +
130 } else
│ │ │ │ +
131 v1 = key2vertex[child];
│ │ │ │ +
132
│ │ │ │ +
133 if (key2vertex.find(parent) == key2vertex.end()) {
│ │ │ │ +
134 v2 = add_vertex(parent, g);
│ │ │ │ +
135 key2vertex.insert(std::make_pair(parent, v2));
│ │ │ │ +
136 } else
│ │ │ │ +
137 v2 = key2vertex[parent];
│ │ │ │ +
138
│ │ │ │ +
139 if (child==parent) {
│ │ │ │ +
140 root = v1;
│ │ │ │ +
141 foundRoot = true;
│ │ │ │ +
142 } else
│ │ │ │ +
143 boost::add_edge(v2, v1, g); // edge is from parent to child
│ │ │ │ +
144 }
│ │ │ │ +
145
│ │ │ │ +
146 if (!foundRoot)
│ │ │ │ +
147 throw std::invalid_argument("predecessorMap2Graph: invalid predecessor map!");
│ │ │ │ +
148 else
│ │ │ │ +
149 return boost::tuple<G, V, std::map<KEY, V> >(g, root, key2vertex);
│ │ │ │ +
150}
│ │ │ │ +
│ │ │ │ +
151
│ │ │ │ +
152/* ************************************************************************* */
│ │ │ │ +
153template <class V, class POSE, class KEY>
│ │ │ │ +
│ │ │ │ +
154class compose_key_visitor : public boost::default_bfs_visitor {
│ │ │ │ +
155
│ │ │ │ +
156private:
│ │ │ │ +
157 boost::shared_ptr<Values> config_;
│ │ │ │ +
158
│ │ │ │ +
159public:
│ │ │ │ +
160
│ │ │ │ +
161 compose_key_visitor(boost::shared_ptr<Values> config_in) {config_ = config_in;}
│ │ │ │ +
162
│ │ │ │ +
163 template <typename Edge, typename Graph> void tree_edge(Edge edge, const Graph& g) const {
│ │ │ │ +
164 KEY key_from = boost::get(boost::vertex_name, g, boost::source(edge, g));
│ │ │ │ +
165 KEY key_to = boost::get(boost::vertex_name, g, boost::target(edge, g));
│ │ │ │ +
166 POSE relativePose = boost::get(boost::edge_weight, g, edge);
│ │ │ │ +
167 config_->insert(key_to, config_->at<POSE>(key_from).compose(relativePose));
│ │ │ │ +
168 }
│ │ │ │ +
169
│ │ │ │ +
170};
│ │ │ │ +
│ │ │ │ +
171
│ │ │ │ +
172/* ************************************************************************* */
│ │ │ │ +
173template<class G, class Factor, class POSE, class KEY>
│ │ │ │ +
│ │ │ │ +
174boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<KEY>& tree,
│ │ │ │ +
175 const POSE& rootPose) {
│ │ │ │ +
176
│ │ │ │ +
177 //TODO: change edge_weight_t to edge_pose_t
│ │ │ │ +
178 typedef typename boost::adjacency_list<
│ │ │ │ +
179 boost::vecS, boost::vecS, boost::directedS,
│ │ │ │ +
180 boost::property<boost::vertex_name_t, KEY>,
│ │ │ │ +
181 boost::property<boost::edge_weight_t, POSE> > PoseGraph;
│ │ │ │ +
182 typedef typename boost::graph_traits<PoseGraph>::vertex_descriptor PoseVertex;
│ │ │ │ +
183 typedef typename boost::graph_traits<PoseGraph>::edge_descriptor PoseEdge;
│ │ │ │ +
184
│ │ │ │ +
185 PoseGraph g;
│ │ │ │ +
186 PoseVertex root;
│ │ │ │ +
187 std::map<KEY, PoseVertex> key2vertex;
│ │ │ │ +
188 boost::tie(g, root, key2vertex) =
│ │ │ │ +
189 predecessorMap2Graph<PoseGraph, PoseVertex, KEY>(tree);
│ │ │ │ +
190
│ │ │ │ +
191 // attach the relative poses to the edges
│ │ │ │ +
192 PoseEdge edge12, edge21;
│ │ │ │ +
193 bool found1, found2;
│ │ │ │ +
194 for(typename G::sharedFactor nl_factor: graph) {
│ │ │ │ +
195
│ │ │ │ +
196 if (nl_factor->keys().size() > 2)
│ │ │ │ +
197 throw std::invalid_argument("composePoses: only support factors with at most two keys");
│ │ │ │ +
198
│ │ │ │ +
199 // e.g. in pose2graph, nonlinear factor needs to be converted to pose2factor
│ │ │ │ +
200 boost::shared_ptr<Factor> factor = boost::dynamic_pointer_cast<Factor>(nl_factor);
│ │ │ │ +
201 if (!factor) continue;
│ │ │ │ +
202
│ │ │ │ +
203 KEY key1 = factor->key1();
│ │ │ │ +
204 KEY key2 = factor->key2();
│ │ │ │ +
205
│ │ │ │ +
206 PoseVertex v1 = key2vertex.find(key1)->second;
│ │ │ │ +
207 PoseVertex v2 = key2vertex.find(key2)->second;
│ │ │ │ +
208
│ │ │ │ +
209 POSE l1Xl2 = factor->measured();
│ │ │ │ +
210 boost::tie(edge12, found1) = boost::edge(v1, v2, g);
│ │ │ │ +
211 boost::tie(edge21, found2) = boost::edge(v2, v1, g);
│ │ │ │ +
212 if (found1 && found2) throw std::invalid_argument ("composePoses: invalid spanning tree");
│ │ │ │ +
213 if (!found1 && !found2) continue;
│ │ │ │ +
214 if (found1)
│ │ │ │ +
215 boost::put(boost::edge_weight, g, edge12, l1Xl2);
│ │ │ │ +
216 else if (found2)
│ │ │ │ +
217 boost::put(boost::edge_weight, g, edge21, l1Xl2.inverse());
│ │ │ │ +
218 }
│ │ │ │ +
219
│ │ │ │ +
220 // compose poses
│ │ │ │ +
221 boost::shared_ptr<Values> config(new Values);
│ │ │ │ +
222 KEY rootKey = boost::get(boost::vertex_name, g, root);
│ │ │ │ +
223 config->insert(rootKey, rootPose);
│ │ │ │ +
224 compose_key_visitor<PoseVertex, POSE, KEY> vis(config);
│ │ │ │ +
225 boost::breadth_first_search(g, root, boost::visitor(vis));
│ │ │ │ +
226
│ │ │ │ +
227 return config;
│ │ │ │ +
228}
│ │ │ │ +
│ │ │ │ +
229
│ │ │ │ +
230/* ************************************************************************* */
│ │ │ │ +
231template<class G, class KEY, class FACTOR2>
│ │ │ │ +
│ │ │ │ +
232PredecessorMap<KEY> findMinimumSpanningTree(const G& fg) {
│ │ │ │ +
233
│ │ │ │ +
234 // Convert to a graph that boost understands
│ │ │ │ +
235 SDGraph<KEY> g = gtsam::toBoostGraph<G, FACTOR2, KEY>(fg);
│ │ │ │ +
236
│ │ │ │ +
237 // find minimum spanning tree
│ │ │ │ +
238 std::vector<typename SDGraph<KEY>::Vertex> p_map(boost::num_vertices(g));
│ │ │ │ +
239 prim_minimum_spanning_tree(g, &p_map[0]);
│ │ │ │ +
240
│ │ │ │ +
241 // convert edge to string pairs
│ │ │ │ +
242 PredecessorMap<KEY> tree;
│ │ │ │ +
243 typename SDGraph<KEY>::vertex_iterator itVertex = boost::vertices(g).first;
│ │ │ │ +
244 for(const typename SDGraph<KEY>::Vertex& vi: p_map){
│ │ │ │ +
245 KEY key = boost::get(boost::vertex_name, g, *itVertex);
│ │ │ │ +
246 KEY parent = boost::get(boost::vertex_name, g, vi);
│ │ │ │ +
247 tree.insert(key, parent);
│ │ │ │ +
248 itVertex++;
│ │ │ │ +
249 }
│ │ │ │ +
250 return tree;
│ │ │ │ +
251}
│ │ │ │ +
│ │ │ │ +
252
│ │ │ │ +
253/* ************************************************************************* */
│ │ │ │ +
254template<class G, class KEY, class FACTOR2>
│ │ │ │ +
│ │ │ │ +
255void split(const G& g, const PredecessorMap<KEY>& tree, G& Ab1, G& Ab2) {
│ │ │ │ +
256
│ │ │ │ +
257 typedef typename G::sharedFactor F ;
│ │ │ │ +
258
│ │ │ │ +
259 for(const F& factor: g)
│ │ │ │ +
260 {
│ │ │ │ +
261 if (factor->keys().size() > 2)
│ │ │ │ +
262 throw(std::invalid_argument("split: only support factors with at most two keys"));
│ │ │ │ +
263
│ │ │ │ +
264 if (factor->keys().size() == 1) {
│ │ │ │ +
265 Ab1.push_back(factor);
│ │ │ │ +
266 continue;
│ │ │ │ +
267 }
│ │ │ │ +
268
│ │ │ │ +
269 boost::shared_ptr<FACTOR2> factor2 = boost::dynamic_pointer_cast<
│ │ │ │ +
270 FACTOR2>(factor);
│ │ │ │ +
271 if (!factor2) continue;
│ │ │ │ +
272
│ │ │ │ +
273 KEY key1 = factor2->key1();
│ │ │ │ +
274 KEY key2 = factor2->key2();
│ │ │ │ +
275 // if the tree contains the key
│ │ │ │ +
276 if ((tree.find(key1) != tree.end() &&
│ │ │ │ +
277 tree.find(key1)->second.compare(key2) == 0) ||
│ │ │ │ +
278 (tree.find(key2) != tree.end() &&
│ │ │ │ +
279 tree.find(key2)->second.compare(key1)== 0) )
│ │ │ │ +
280 Ab1.push_back(factor2);
│ │ │ │ +
281 else
│ │ │ │ +
282 Ab2.push_back(factor2);
│ │ │ │ +
283 }
│ │ │ │ +
284}
│ │ │ │
│ │ │ │ -
83
│ │ │ │ -
84} // \ gtsam
│ │ │ │ +
285
│ │ │ │ +
286}
│ │ │ │ +
graph.h
Graph algorithm using boost library.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::MetisIndex::augment
void augment(const FACTORGRAPH &factors)
Augment the variable index with new factors.
Definition MetisIndex-inl.h:27
│ │ │ │ +
gtsam::predecessorMap2Keys
std::list< KEY > predecessorMap2Keys(const PredecessorMap< KEY > &p_map)
Generate a list of keys from a spanning tree represented by its predecessor map.
Definition graph-inl.h:50
│ │ │ │ +
gtsam::split
void split(const G &g, const PredecessorMap< KEY > &tree, G &Ab1, G &Ab2)
Split the graph into two parts: one corresponds to the given spanning tree, and the other corresponds...
Definition graph-inl.h:255
│ │ │ │ +
gtsam::composePoses
boost::shared_ptr< Values > composePoses(const G &graph, const PredecessorMap< KEY > &tree, const POSE &rootPose)
Compose the poses by following the chain specified by the spanning tree.
Definition graph-inl.h:174
│ │ │ │ +
gtsam::toBoostGraph
SDGraph< KEY > toBoostGraph(const G &graph)
Convert the factor graph to an SDGraph G = Graph type F = Factor type Key = Key type.
Definition graph-inl.h:68
│ │ │ │ +
gtsam::findMinimumSpanningTree
PredecessorMap< KEY > findMinimumSpanningTree(const G &fg)
find the minimum spanning tree using boost graph library
Definition graph-inl.h:232
│ │ │ │ +
gtsam::predecessorMap2Graph
boost::tuple< G, V, std::map< KEY, V > > predecessorMap2Graph(const PredecessorMap< KEY > &p_map)
Build takes a predecessor map, and builds a directed graph corresponding to the tree.
Definition graph-inl.h:118
│ │ │ │ +
gtsam::ordering_key_visitor
Definition graph-inl.h:38
│ │ │ │ +
gtsam::compose_key_visitor
Definition graph-inl.h:154
│ │ │ │ +
gtsam::SDGraph
SDGraph is undirected graph with variable keys and double edge weights.
Definition graph.h:40
│ │ │ │ +
gtsam::SGraph
Definition graph.h:47
│ │ │ │ +
gtsam::PredecessorMap
Map from variable key to parent key.
Definition graph.h:58
│ │ │ │ +
gtsam::PredecessorMap::insert
void insert(const KEY &key, const KEY &parent)
convenience insert so we can pass ints for TypedSymbol keys
Definition graph.h:61
│ │ │ │ +
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,105 +1,377 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -MetisIndex-inl.h │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +graph-inl.h │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ +12/* │ │ │ │ │ +13 * @file graph-inl.h │ │ │ │ │ +14 * @brief Graph algorithm using boost library │ │ │ │ │ +15 * @author Kai Ni │ │ │ │ │ +16 */ │ │ │ │ │ +17 │ │ │ │ │ 18#pragma once │ │ │ │ │ 19 │ │ │ │ │ -20#include │ │ │ │ │ -21#include │ │ │ │ │ -22 │ │ │ │ │ -23namespace _g_t_s_a_m { │ │ │ │ │ -24 │ │ │ │ │ -25/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -26template │ │ │ │ │ -_2_7void _M_e_t_i_s_I_n_d_e_x_:_:_a_u_g_m_e_n_t(const FACTORGRAPH& factors) { │ │ │ │ │ -28 std::map > iAdjMap; // Stores a set of keys that │ │ │ │ │ -are adjacent to key x, with adjMap.first │ │ │ │ │ -29 std::map >::iterator iAdjMapIt; │ │ │ │ │ -30 std::set keySet; │ │ │ │ │ +20#include │ │ │ │ │ +21#ifdef __GNUC__ │ │ │ │ │ +22#pragma GCC diagnostic push │ │ │ │ │ +23#pragma GCC diagnostic ignored "-Wunused-variable" │ │ │ │ │ +24//#pragma GCC diagnostic ignored "-Wunneeded-internal-declaration" │ │ │ │ │ +25#endif │ │ │ │ │ +26#include │ │ │ │ │ +27#ifdef __GNUC__ │ │ │ │ │ +28#pragma GCC diagnostic pop │ │ │ │ │ +29#endif │ │ │ │ │ +30#include │ │ │ │ │ 31 │ │ │ │ │ -32 /* ********** Convert to CSR format ********** */ │ │ │ │ │ -33 // Assuming that vertex numbering starts from 0 (C style), │ │ │ │ │ -34 // then the adjacency list of vertex i is stored in array adjncy │ │ │ │ │ -35 // starting at index xadj[i] and ending at(but not including) │ │ │ │ │ -36 // index xadj[i + 1](i.e., adjncy[xadj[i]] through │ │ │ │ │ -37 // and including adjncy[xadj[i + 1] - 1]). │ │ │ │ │ -38 int32_t keyCounter = 0; │ │ │ │ │ -39 │ │ │ │ │ -40 // First: Record a copy of each key inside the factorgraph and create a │ │ │ │ │ -41 // key to integer mapping. This is referenced during the adjaceny step │ │ │ │ │ -42 for (size_t i = 0; i < factors.size(); i++) { │ │ │ │ │ -43 if (factors[i]) { │ │ │ │ │ -44 for(const _K_e_y& key: *factors[i]) { │ │ │ │ │ -45 keySet.insert(keySet.end(), key); // Keep a track of all unique keys │ │ │ │ │ -46 if (intKeyBMap_.left.find(key) == intKeyBMap_.left.end()) { │ │ │ │ │ -47 intKeyBMap_.insert(bm_type::value_type(key, keyCounter)); │ │ │ │ │ -48 keyCounter++; │ │ │ │ │ -49 } │ │ │ │ │ -50 } │ │ │ │ │ -51 } │ │ │ │ │ -52 } │ │ │ │ │ +32#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_g_r_a_p_h_._h> │ │ │ │ │ +33 │ │ │ │ │ +34namespace _g_t_s_a_m { │ │ │ │ │ +35 │ │ │ │ │ +36/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +37template │ │ │ │ │ +_3_8class _o_r_d_e_r_i_n_g___k_e_y___v_i_s_i_t_o_r : public boost::default_bfs_visitor { │ │ │ │ │ +39public: │ │ │ │ │ +40 _o_r_d_e_r_i_n_g___k_e_y___v_i_s_i_t_o_r(std::list& ordering_in) : ordering_(ordering_in) │ │ │ │ │ +{} │ │ │ │ │ +41 template void discover_vertex(Vertex v, │ │ │ │ │ +const Graph& g) const { │ │ │ │ │ +42 KEY key = boost::get(boost::vertex_name, g, v); │ │ │ │ │ +43 ordering_.push_front(key); │ │ │ │ │ +44 } │ │ │ │ │ +45 std::list& ordering_; │ │ │ │ │ +46}; │ │ │ │ │ +47 │ │ │ │ │ +48/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +49template │ │ │ │ │ +_5_0std::list _p_r_e_d_e_c_e_s_s_o_r_M_a_p_2_K_e_y_s(const _P_r_e_d_e_c_e_s_s_o_r_M_a_p_<_K_E_Y_>& p_map) { │ │ │ │ │ +51 │ │ │ │ │ +52 typedef typename SGraph::Vertex SVertex; │ │ │ │ │ 53 │ │ │ │ │ -54 // Create an adjacency mapping that stores the set of all adjacent keys for │ │ │ │ │ -every key │ │ │ │ │ -55 for (size_t i = 0; i < factors.size(); i++) { │ │ │ │ │ -56 if (factors[i]) { │ │ │ │ │ -57 for(const _K_e_y& k1: *factors[i]) │ │ │ │ │ -58 for(const _K_e_y& k2: *factors[i]) │ │ │ │ │ -59 if (k1 != k2) { │ │ │ │ │ -60 // Store both in Key and int32_t format │ │ │ │ │ -61 int i = intKeyBMap_.left.at(k1); │ │ │ │ │ -62 int j = intKeyBMap_.left.at(k2); │ │ │ │ │ -63 iAdjMap[i].insert(iAdjMap[i].end(), j); │ │ │ │ │ -64 } │ │ │ │ │ -65 } │ │ │ │ │ -66 } │ │ │ │ │ -67 │ │ │ │ │ -68 // Number of keys referenced in this factor graph │ │ │ │ │ -69 nKeys_ = keySet.size(); │ │ │ │ │ -70 │ │ │ │ │ -71 xadj_.push_back(0); // Always set the first index to zero │ │ │ │ │ -72 for (iAdjMapIt = iAdjMap.begin(); iAdjMapIt != iAdjMap.end(); ++iAdjMapIt) { │ │ │ │ │ -73 std::vector temp; │ │ │ │ │ -74 // Copy from the FastSet into a temporary vector │ │ │ │ │ -75 std::copy(iAdjMapIt->second.begin(), iAdjMapIt->second.end(), │ │ │ │ │ -76 std::back_inserter(temp)); │ │ │ │ │ -77 // Insert each index's set in order by appending them to the end of adj_ │ │ │ │ │ -78 adj_.insert(adj_.end(), temp.begin(), temp.end()); │ │ │ │ │ -79 //adj_.push_back(temp); │ │ │ │ │ -80 xadj_.push_back((int32_t) adj_.size()); │ │ │ │ │ -81 } │ │ │ │ │ -82} │ │ │ │ │ -83 │ │ │ │ │ -84} // \ gtsam │ │ │ │ │ +54 _S_G_r_a_p_h_<_K_E_Y_> g; │ │ │ │ │ +55 SVertex root; │ │ │ │ │ +56 std::map key2vertex; │ │ │ │ │ +57 boost::tie(g, root, key2vertex) = gtsam::predecessorMap2Graph, │ │ │ │ │ +SVertex, KEY>(p_map); │ │ │ │ │ +58 │ │ │ │ │ +59 // breadth first visit on the graph │ │ │ │ │ +60 std::list keys; │ │ │ │ │ +61 _o_r_d_e_r_i_n_g___k_e_y___v_i_s_i_t_o_r_<_K_E_Y_> vis(keys); │ │ │ │ │ +62 boost::breadth_first_search(g, root, boost::visitor(vis)); │ │ │ │ │ +63 return keys; │ │ │ │ │ +64} │ │ │ │ │ +65 │ │ │ │ │ +66/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +67template │ │ │ │ │ +_6_8_S_D_G_r_a_p_h_<_K_E_Y_> _t_o_B_o_o_s_t_G_r_a_p_h(const G& graph) { │ │ │ │ │ +69 // convert the factor graph to boost graph │ │ │ │ │ +70 _S_D_G_r_a_p_h_<_K_E_Y_> g; │ │ │ │ │ +71 typedef typename boost::graph_traits >::vertex_descriptor │ │ │ │ │ +BoostVertex; │ │ │ │ │ +72 std::map key2vertex; │ │ │ │ │ +73 typename G::const_iterator itFactor; │ │ │ │ │ +74 │ │ │ │ │ +75 // Loop over the factors │ │ │ │ │ +76 for(itFactor=graph.begin(); itFactor!=graph.end(); itFactor++) { │ │ │ │ │ +77 │ │ │ │ │ +78 // Ignore factors that are not binary │ │ │ │ │ +79 if ((*itFactor)->keys().size() != 2) │ │ │ │ │ +80 continue; │ │ │ │ │ +81 │ │ │ │ │ +82 // Cast the factor to the user-specified factor type F │ │ │ │ │ +83 boost::shared_ptr factor = boost::dynamic_pointer_cast(*itFactor); │ │ │ │ │ +84 // Ignore factors that are not of type F │ │ │ │ │ +85 if (!factor) continue; │ │ │ │ │ +86 │ │ │ │ │ +87 // Retrieve the 2 keys (nodes) the factor (edge) is incident on │ │ │ │ │ +88 KEY key1 = factor->keys()[0]; │ │ │ │ │ +89 KEY key2 = factor->keys()[1]; │ │ │ │ │ +90 │ │ │ │ │ +91 BoostVertex v1, v2; │ │ │ │ │ +92 │ │ │ │ │ +93 // If key1 is a new key, add it to the key2vertex map, else get the │ │ │ │ │ +corresponding vertex id │ │ │ │ │ +94 if (key2vertex.find(key1) == key2vertex.end()) { │ │ │ │ │ +95 v1 = add_vertex(key1, g); │ │ │ │ │ +96 key2vertex.insert(std::pair(key1, v1)); │ │ │ │ │ +97 } else │ │ │ │ │ +98 v1 = key2vertex[key1]; │ │ │ │ │ +99 │ │ │ │ │ +100 // If key2 is a new key, add it to the key2vertex map, else get the │ │ │ │ │ +corresponding vertex id │ │ │ │ │ +101 if (key2vertex.find(key2) == key2vertex.end()) { │ │ │ │ │ +102 v2 = add_vertex(key2, g); │ │ │ │ │ +103 key2vertex.insert(std::pair(key2, v2)); │ │ │ │ │ +104 } else │ │ │ │ │ +105 v2 = key2vertex[key2]; │ │ │ │ │ +106 │ │ │ │ │ +107 // Add an edge with weight 1.0 │ │ │ │ │ +108 boost::property edge_property(1.0); // assume │ │ │ │ │ +constant edge weight here │ │ │ │ │ +109 boost::add_edge(v1, v2, edge_property, g); │ │ │ │ │ +110 } │ │ │ │ │ +111 │ │ │ │ │ +112 return g; │ │ │ │ │ +113} │ │ │ │ │ +114 │ │ │ │ │ +115/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +116template │ │ │ │ │ +117boost::tuple > │ │ │ │ │ +_1_1_8_p_r_e_d_e_c_e_s_s_o_r_M_a_p_2_G_r_a_p_h(const _P_r_e_d_e_c_e_s_s_o_r_M_a_p_<_K_E_Y_>& p_map) { │ │ │ │ │ +119 │ │ │ │ │ +120 G g; │ │ │ │ │ +121 std::map key2vertex; │ │ │ │ │ +122 V v1, v2, root; │ │ │ │ │ +123 bool foundRoot = false; │ │ │ │ │ +124 for(auto child_parent: p_map) { │ │ │ │ │ +125 KEY child, parent; │ │ │ │ │ +126 std::tie(child,parent) = child_parent; │ │ │ │ │ +127 if (key2vertex.find(child) == key2vertex.end()) { │ │ │ │ │ +128 v1 = add_vertex(child, g); │ │ │ │ │ +129 key2vertex.insert(std::make_pair(child, v1)); │ │ │ │ │ +130 } else │ │ │ │ │ +131 v1 = key2vertex[child]; │ │ │ │ │ +132 │ │ │ │ │ +133 if (key2vertex.find(parent) == key2vertex.end()) { │ │ │ │ │ +134 v2 = add_vertex(parent, g); │ │ │ │ │ +135 key2vertex.insert(std::make_pair(parent, v2)); │ │ │ │ │ +136 } else │ │ │ │ │ +137 v2 = key2vertex[parent]; │ │ │ │ │ +138 │ │ │ │ │ +139 if (child==parent) { │ │ │ │ │ +140 root = v1; │ │ │ │ │ +141 foundRoot = true; │ │ │ │ │ +142 } else │ │ │ │ │ +143 boost::add_edge(v2, v1, g); // edge is from parent to child │ │ │ │ │ +144 } │ │ │ │ │ +145 │ │ │ │ │ +146 if (!foundRoot) │ │ │ │ │ +147 throw std::invalid_argument("predecessorMap2Graph: invalid predecessor │ │ │ │ │ +map!"); │ │ │ │ │ +148 else │ │ │ │ │ +149 return boost::tuple >(g, root, key2vertex); │ │ │ │ │ +150} │ │ │ │ │ +151 │ │ │ │ │ +152/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +153template │ │ │ │ │ +_1_5_4class _c_o_m_p_o_s_e___k_e_y___v_i_s_i_t_o_r : public boost::default_bfs_visitor { │ │ │ │ │ +155 │ │ │ │ │ +156private: │ │ │ │ │ +157 boost::shared_ptr config_; │ │ │ │ │ +158 │ │ │ │ │ +159public: │ │ │ │ │ +160 │ │ │ │ │ +161 _c_o_m_p_o_s_e___k_e_y___v_i_s_i_t_o_r(boost::shared_ptr config_in) {config_ = │ │ │ │ │ +config_in;} │ │ │ │ │ +162 │ │ │ │ │ +163 template void tree_edge(Edge edge, const │ │ │ │ │ +Graph& g) const { │ │ │ │ │ +164 KEY key_from = boost::get(boost::vertex_name, g, boost::source(edge, g)); │ │ │ │ │ +165 KEY key_to = boost::get(boost::vertex_name, g, boost::target(edge, g)); │ │ │ │ │ +166 POSE relativePose = boost::get(boost::edge_weight, g, edge); │ │ │ │ │ +167 config_->insert(key_to, config_->at(key_from).compose(relativePose)); │ │ │ │ │ +168 } │ │ │ │ │ +169 │ │ │ │ │ +170}; │ │ │ │ │ +171 │ │ │ │ │ +172/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +173template │ │ │ │ │ +_1_7_4boost::shared_ptr _c_o_m_p_o_s_e_P_o_s_e_s(const G& graph, const │ │ │ │ │ +_P_r_e_d_e_c_e_s_s_o_r_M_a_p_<_K_E_Y_>& tree, │ │ │ │ │ +175 const POSE& rootPose) { │ │ │ │ │ +176 │ │ │ │ │ +177 //TODO: change edge_weight_t to edge_pose_t │ │ │ │ │ +178 typedef typename boost::adjacency_list< │ │ │ │ │ +179 boost::vecS, boost::vecS, boost::directedS, │ │ │ │ │ +180 boost::property, │ │ │ │ │ +181 boost::property > PoseGraph; │ │ │ │ │ +182 typedef typename boost::graph_traits::vertex_descriptor │ │ │ │ │ +PoseVertex; │ │ │ │ │ +183 typedef typename boost::graph_traits::edge_descriptor PoseEdge; │ │ │ │ │ +184 │ │ │ │ │ +185 PoseGraph g; │ │ │ │ │ +186 PoseVertex root; │ │ │ │ │ +187 std::map key2vertex; │ │ │ │ │ +188 boost::tie(g, root, key2vertex) = │ │ │ │ │ +189 predecessorMap2Graph(tree); │ │ │ │ │ +190 │ │ │ │ │ +191 // attach the relative poses to the edges │ │ │ │ │ +192 PoseEdge edge12, edge21; │ │ │ │ │ +193 bool found1, found2; │ │ │ │ │ +194 for(typename G::sharedFactor nl_factor: graph) { │ │ │ │ │ +195 │ │ │ │ │ +196 if (nl_factor->keys().size() > 2) │ │ │ │ │ +197 throw std::invalid_argument("composePoses: only support factors with at │ │ │ │ │ +most two keys"); │ │ │ │ │ +198 │ │ │ │ │ +199 // e.g. in pose2graph, nonlinear factor needs to be converted to │ │ │ │ │ +pose2factor │ │ │ │ │ +200 boost::shared_ptr factor = boost::dynamic_pointer_cast │ │ │ │ │ +(nl_factor); │ │ │ │ │ +201 if (!factor) continue; │ │ │ │ │ +202 │ │ │ │ │ +203 KEY key1 = factor->key1(); │ │ │ │ │ +204 KEY key2 = factor->key2(); │ │ │ │ │ +205 │ │ │ │ │ +206 PoseVertex v1 = key2vertex.find(key1)->second; │ │ │ │ │ +207 PoseVertex v2 = key2vertex.find(key2)->second; │ │ │ │ │ +208 │ │ │ │ │ +209 POSE l1Xl2 = factor->measured(); │ │ │ │ │ +210 boost::tie(edge12, found1) = boost::edge(v1, v2, g); │ │ │ │ │ +211 boost::tie(edge21, found2) = boost::edge(v2, v1, g); │ │ │ │ │ +212 if (found1 && found2) throw std::invalid_argument ("composePoses: invalid │ │ │ │ │ +spanning tree"); │ │ │ │ │ +213 if (!found1 && !found2) continue; │ │ │ │ │ +214 if (found1) │ │ │ │ │ +215 boost::put(boost::edge_weight, g, edge12, l1Xl2); │ │ │ │ │ +216 else if (found2) │ │ │ │ │ +217 boost::put(boost::edge_weight, g, edge21, l1Xl2.inverse()); │ │ │ │ │ +218 } │ │ │ │ │ +219 │ │ │ │ │ +220 // compose poses │ │ │ │ │ +221 boost::shared_ptr config(new _V_a_l_u_e_s); │ │ │ │ │ +222 KEY rootKey = boost::get(boost::vertex_name, g, root); │ │ │ │ │ +223 config->insert(rootKey, rootPose); │ │ │ │ │ +224 _c_o_m_p_o_s_e___k_e_y___v_i_s_i_t_o_r_<_P_o_s_e_V_e_r_t_e_x_,_ _P_O_S_E_,_ _K_E_Y_> vis(config); │ │ │ │ │ +225 boost::breadth_first_search(g, root, boost::visitor(vis)); │ │ │ │ │ +226 │ │ │ │ │ +227 return config; │ │ │ │ │ +228} │ │ │ │ │ +229 │ │ │ │ │ +230/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +231template │ │ │ │ │ +_2_3_2_P_r_e_d_e_c_e_s_s_o_r_M_a_p_<_K_E_Y_> _f_i_n_d_M_i_n_i_m_u_m_S_p_a_n_n_i_n_g_T_r_e_e(const G& fg) { │ │ │ │ │ +233 │ │ │ │ │ +234 // Convert to a graph that boost understands │ │ │ │ │ +235 _S_D_G_r_a_p_h_<_K_E_Y_> g = gtsam::toBoostGraph(fg); │ │ │ │ │ +236 │ │ │ │ │ +237 // find minimum spanning tree │ │ │ │ │ +238 std::vector::Vertex> p_map(boost::num_vertices(g)); │ │ │ │ │ +239 prim_minimum_spanning_tree(g, &p_map[0]); │ │ │ │ │ +240 │ │ │ │ │ +241 // convert edge to string pairs │ │ │ │ │ +242 _P_r_e_d_e_c_e_s_s_o_r_M_a_p_<_K_E_Y_> tree; │ │ │ │ │ +243 typename _S_D_G_r_a_p_h_<_K_E_Y_>_:_:_v_e_r_t_e_x___i_t_e_r_a_t_o_r itVertex = boost::vertices(g).first; │ │ │ │ │ +244 for(const typename SDGraph::Vertex& vi: p_map){ │ │ │ │ │ +245 KEY key = boost::get(boost::vertex_name, g, *itVertex); │ │ │ │ │ +246 KEY parent = boost::get(boost::vertex_name, g, vi); │ │ │ │ │ +247 tree._i_n_s_e_r_t(key, parent); │ │ │ │ │ +248 itVertex++; │ │ │ │ │ +249 } │ │ │ │ │ +250 return tree; │ │ │ │ │ +251} │ │ │ │ │ +252 │ │ │ │ │ +253/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +254template │ │ │ │ │ +_2_5_5void _s_p_l_i_t(const G& g, const _P_r_e_d_e_c_e_s_s_o_r_M_a_p_<_K_E_Y_>& tree, G& Ab1, G& Ab2) { │ │ │ │ │ +256 │ │ │ │ │ +257 typedef typename G::sharedFactor F ; │ │ │ │ │ +258 │ │ │ │ │ +259 for(const F& factor: g) │ │ │ │ │ +260 { │ │ │ │ │ +261 if (factor->keys().size() > 2) │ │ │ │ │ +262 throw(std::invalid_argument("split: only support factors with at most two │ │ │ │ │ +keys")); │ │ │ │ │ +263 │ │ │ │ │ +264 if (factor->keys().size() == 1) { │ │ │ │ │ +265 Ab1.push_back(factor); │ │ │ │ │ +266 continue; │ │ │ │ │ +267 } │ │ │ │ │ +268 │ │ │ │ │ +269 boost::shared_ptr factor2 = boost::dynamic_pointer_cast< │ │ │ │ │ +270 FACTOR2>(factor); │ │ │ │ │ +271 if (!factor2) continue; │ │ │ │ │ +272 │ │ │ │ │ +273 KEY key1 = factor2->key1(); │ │ │ │ │ +274 KEY key2 = factor2->key2(); │ │ │ │ │ +275 // if the tree contains the key │ │ │ │ │ +276 if ((tree.find(key1) != tree.end() && │ │ │ │ │ +277 tree.find(key1)->second.compare(key2) == 0) || │ │ │ │ │ +278 (tree.find(key2) != tree.end() && │ │ │ │ │ +279 tree.find(key2)->second.compare(key1)== 0) ) │ │ │ │ │ +280 Ab1.push_back(factor2); │ │ │ │ │ +281 else │ │ │ │ │ +282 Ab2.push_back(factor2); │ │ │ │ │ +283 } │ │ │ │ │ +284} │ │ │ │ │ +285 │ │ │ │ │ +286} │ │ │ │ │ +_g_r_a_p_h_._h │ │ │ │ │ +Graph algorithm using boost library. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_M_e_t_i_s_I_n_d_e_x_:_:_a_u_g_m_e_n_t │ │ │ │ │ -void augment(const FACTORGRAPH &factors) │ │ │ │ │ -Augment the variable index with new factors. │ │ │ │ │ -DDeeffiinniittiioonn MetisIndex-inl.h:27 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_e_d_e_c_e_s_s_o_r_M_a_p_2_K_e_y_s │ │ │ │ │ +std::list< KEY > predecessorMap2Keys(const PredecessorMap< KEY > &p_map) │ │ │ │ │ +Generate a list of keys from a spanning tree represented by its predecessor │ │ │ │ │ +map. │ │ │ │ │ +DDeeffiinniittiioonn graph-inl.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_s_p_l_i_t │ │ │ │ │ +void split(const G &g, const PredecessorMap< KEY > &tree, G &Ab1, G &Ab2) │ │ │ │ │ +Split the graph into two parts: one corresponds to the given spanning tree, and │ │ │ │ │ +the other corresponds... │ │ │ │ │ +DDeeffiinniittiioonn graph-inl.h:255 │ │ │ │ │ +_g_t_s_a_m_:_:_c_o_m_p_o_s_e_P_o_s_e_s │ │ │ │ │ +boost::shared_ptr< Values > composePoses(const G &graph, const PredecessorMap< │ │ │ │ │ +KEY > &tree, const POSE &rootPose) │ │ │ │ │ +Compose the poses by following the chain specified by the spanning tree. │ │ │ │ │ +DDeeffiinniittiioonn graph-inl.h:174 │ │ │ │ │ +_g_t_s_a_m_:_:_t_o_B_o_o_s_t_G_r_a_p_h │ │ │ │ │ +SDGraph< KEY > toBoostGraph(const G &graph) │ │ │ │ │ +Convert the factor graph to an SDGraph G = Graph type F = Factor type Key = Key │ │ │ │ │ +type. │ │ │ │ │ +DDeeffiinniittiioonn graph-inl.h:68 │ │ │ │ │ +_g_t_s_a_m_:_:_f_i_n_d_M_i_n_i_m_u_m_S_p_a_n_n_i_n_g_T_r_e_e │ │ │ │ │ +PredecessorMap< KEY > findMinimumSpanningTree(const G &fg) │ │ │ │ │ +find the minimum spanning tree using boost graph library │ │ │ │ │ +DDeeffiinniittiioonn graph-inl.h:232 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_e_d_e_c_e_s_s_o_r_M_a_p_2_G_r_a_p_h │ │ │ │ │ +boost::tuple< G, V, std::map< KEY, V > > predecessorMap2Graph(const │ │ │ │ │ +PredecessorMap< KEY > &p_map) │ │ │ │ │ +Build takes a predecessor map, and builds a directed graph corresponding to the │ │ │ │ │ +tree. │ │ │ │ │ +DDeeffiinniittiioonn graph-inl.h:118 │ │ │ │ │ +_g_t_s_a_m_:_:_o_r_d_e_r_i_n_g___k_e_y___v_i_s_i_t_o_r │ │ │ │ │ +DDeeffiinniittiioonn graph-inl.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_c_o_m_p_o_s_e___k_e_y___v_i_s_i_t_o_r │ │ │ │ │ +DDeeffiinniittiioonn graph-inl.h:154 │ │ │ │ │ +_g_t_s_a_m_:_:_S_D_G_r_a_p_h │ │ │ │ │ +SDGraph is undirected graph with variable keys and double edge weights. │ │ │ │ │ +DDeeffiinniittiioonn graph.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_S_G_r_a_p_h │ │ │ │ │ +DDeeffiinniittiioonn graph.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_d_e_c_e_s_s_o_r_M_a_p │ │ │ │ │ +Map from variable key to parent key. │ │ │ │ │ +DDeeffiinniittiioonn graph.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_d_e_c_e_s_s_o_r_M_a_p_:_:_i_n_s_e_r_t │ │ │ │ │ +void insert(const KEY &key, const KEY &parent) │ │ │ │ │ +convenience insert so we can pass ints for TypedSymbol keys │ │ │ │ │ +DDeeffiinniittiioonn graph.h:61 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _M_e_t_i_s_I_n_d_e_x_-_i_n_l_._h │ │ │ │ │ + * ggrraapphh--iinnll..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00620.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/JunctionTree-inst.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Ordering.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,50 +94,34 @@ │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
JunctionTree-inst.h File Reference
│ │ │ │ +
Ordering.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -

The junction tree, template bodies. │ │ │ │ -More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

struct  gtsam::ConstructorTraversalData< BAYESTREE, GRAPH, ETREE_NODE >
 
class  gtsam::ConstructorTraversalData< BAYESTREE, GRAPH, ETREE_NODE >::SymbolicFactors
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

The junction tree, template bodies.

│ │ │ │ -
Date
Feb 4, 2010
│ │ │ │ -
Author
Kai Ni
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ +
Author
Richard Roberts
│ │ │ │
│ │ │ │ -Richard Roberts
│ │ │ │ +Andrew Melim
│ │ │ │ +
Date
Sep 2, 2010
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,31 +1,21 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -JunctionTree-inst.h File Reference │ │ │ │ │ -The junction tree, template bodies. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_C_o_n_s_t_r_u_c_t_o_r_T_r_a_v_e_r_s_a_l_D_a_t_a_<_ _B_A_Y_E_S_T_R_E_E_,_ _G_R_A_P_H_,_ _E_T_R_E_E___N_O_D_E_ _> │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_C_o_n_s_t_r_u_c_t_o_r_T_r_a_v_e_r_s_a_l_D_a_t_a_<_ _B_A_Y_E_S_T_R_E_E_,_ _G_R_A_P_H_,_ _E_T_R_E_E___N_O_D_E_ _>_:_: │ │ │ │ │ - _S_y_m_b_o_l_i_c_F_a_c_t_o_r_s │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +Ordering.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -The junction tree, template bodies. │ │ │ │ │ - Date │ │ │ │ │ - Feb 4, 2010 │ │ │ │ │ Author │ │ │ │ │ - Kai Ni │ │ │ │ │ - Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ + Andrew Melim │ │ │ │ │ + Date │ │ │ │ │ + Sep 2, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _J_u_n_c_t_i_o_n_T_r_e_e_-_i_n_s_t_._h │ │ │ │ │ + * _O_r_d_e_r_i_n_g_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00623.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTreeCliqueBase-inst.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/VariableIndex.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,37 +94,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
BayesTreeCliqueBase-inst.h File Reference
│ │ │ │ +
VariableIndex.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Base class for cliques of a BayesTree. │ │ │ │ -More...

│ │ │ │ - │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::VariableIndex
 The VariableIndex class computes and stores the block column structure of a factor graph. More...
 
struct  gtsam::traits< VariableIndex >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Base class for cliques of a BayesTree.

│ │ │ │ -
Author
Richard Roberts and Frank Dellaert
│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
March 26, 2013
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,29 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -BayesTreeCliqueBase-inst.h File Reference │ │ │ │ │ -Base class for cliques of a BayesTree. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +VariableIndex.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ +  The _V_a_r_i_a_b_l_e_I_n_d_e_x class computes and stores the block column structure │ │ │ │ │ + of a factor graph. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _V_a_r_i_a_b_l_e_I_n_d_e_x_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Base class for cliques of a BayesTree. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts and Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + March 26, 2013 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_-_i_n_s_t_._h │ │ │ │ │ + * _V_a_r_i_a_b_l_e_I_n_d_e_x_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00623_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTreeCliqueBase-inst.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/VariableIndex.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,305 +98,199 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
BayesTreeCliqueBase-inst.h
│ │ │ │ +
VariableIndex.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │ -
9 * -------------------------------------------------------------------------- */
│ │ │ │ -
10
│ │ │ │ -
17#pragma once
│ │ │ │ -
18
│ │ │ │ -
19#include <gtsam/inference/BayesTreeCliqueBase.h>
│ │ │ │ -
20#include <gtsam/inference/FactorGraph-inst.h>
│ │ │ │ -
21#include <gtsam/base/timing.h>
│ │ │ │ -
22
│ │ │ │ -
23namespace gtsam {
│ │ │ │ -
24
│ │ │ │ -
25 /* ************************************************************************* */
│ │ │ │ -
26 template<class DERIVED, class FACTORGRAPH>
│ │ │ │ -
│ │ │ │ -
27 void BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::setEliminationResult(
│ │ │ │ -
28 const typename FactorGraphType::EliminationResult& eliminationResult)
│ │ │ │ -
29 {
│ │ │ │ -
30 conditional_ = eliminationResult.first;
│ │ │ │ -
31 }
│ │ │ │ -
│ │ │ │ -
32
│ │ │ │ -
33 /* ************************************************************************* */
│ │ │ │ -
34 template<class DERIVED, class FACTORGRAPH>
│ │ │ │ -
│ │ │ │ -
35 bool BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::equals(
│ │ │ │ -
36 const DERIVED& other, double tol) const
│ │ │ │ -
37 {
│ │ │ │ -
38 return (!conditional_ && !other.conditional())
│ │ │ │ -
39 || conditional_->equals(*other.conditional(), tol);
│ │ │ │ -
40 }
│ │ │ │ -
│ │ │ │ -
41
│ │ │ │ -
42 /* ************************************************************************* */
│ │ │ │ -
43 template<class DERIVED, class FACTORGRAPH>
│ │ │ │ -
44 KeyVector
│ │ │ │ -
│ │ │ │ -
45 BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separator_setminus_B(const derived_ptr& B) const
│ │ │ │ -
46 {
│ │ │ │ -
47 KeySet p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents());
│ │ │ │ -
48 KeySet indicesB(B->conditional()->begin(), B->conditional()->end());
│ │ │ │ -
49 KeyVector S_setminus_B;
│ │ │ │ -
50 std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(),
│ │ │ │ -
51 indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B));
│ │ │ │ -
52 return S_setminus_B;
│ │ │ │ -
53 }
│ │ │ │ -
│ │ │ │ +
9
│ │ │ │ +
10 * -------------------------------------------------------------------------- */
│ │ │ │ +
11
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/inference/Factor.h>
│ │ │ │ +
21#include <gtsam/inference/Key.h>
│ │ │ │ +
22#include <gtsam/base/FastMap.h>
│ │ │ │ +
23#include <gtsam/base/FastVector.h>
│ │ │ │ +
24#include <gtsam/dllexport.h>
│ │ │ │ +
25
│ │ │ │ +
26#include <boost/optional/optional.hpp>
│ │ │ │ +
27#include <boost/smart_ptr/shared_ptr.hpp>
│ │ │ │ +
28
│ │ │ │ +
29#include <cassert>
│ │ │ │ +
30#include <stdexcept>
│ │ │ │ +
31
│ │ │ │ +
32namespace gtsam {
│ │ │ │ +
33
│ │ │ │ +
│ │ │ │ +
43class GTSAM_EXPORT VariableIndex {
│ │ │ │ +
44 public:
│ │ │ │ +
45 typedef boost::shared_ptr<VariableIndex> shared_ptr;
│ │ │ │ +
46 typedef FactorIndices::iterator Factor_iterator;
│ │ │ │ +
47 typedef FactorIndices::const_iterator Factor_const_iterator;
│ │ │ │ +
48
│ │ │ │ +
49 protected:
│ │ │ │ +
50 typedef FastMap<Key, FactorIndices> KeyMap;
│ │ │ │ +
51 KeyMap index_;
│ │ │ │ +
52 size_t nFactors_; // Number of factors in the original factor graph.
│ │ │ │ +
53 size_t nEntries_; // Sum of involved variable counts of each factor.
│ │ │ │
54
│ │ │ │ -
55 /* ************************************************************************* */
│ │ │ │ -
56 template<class DERIVED, class FACTORGRAPH>
│ │ │ │ -
│ │ │ │ -
57 KeyVector BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::shortcut_indices(
│ │ │ │ -
58 const derived_ptr& B, const FactorGraphType& p_Cp_B) const
│ │ │ │ -
59 {
│ │ │ │ -
60 gttic(shortcut_indices);
│ │ │ │ -
61 KeySet allKeys = p_Cp_B.keys();
│ │ │ │ -
62 KeySet indicesB(B->conditional()->begin(), B->conditional()->end());
│ │ │ │ -
63 KeyVector S_setminus_B = separator_setminus_B(B);
│ │ │ │ -
64 KeyVector keep;
│ │ │ │ -
65 // keep = S\B intersect allKeys (S_setminus_B is already sorted)
│ │ │ │ -
66 std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), //
│ │ │ │ -
67 allKeys.begin(), allKeys.end(), back_inserter(keep));
│ │ │ │ -
68 // keep += B intersect allKeys
│ │ │ │ -
69 std::set_intersection(indicesB.begin(), indicesB.end(), //
│ │ │ │ -
70 allKeys.begin(), allKeys.end(), back_inserter(keep));
│ │ │ │ -
71 return keep;
│ │ │ │ -
72 }
│ │ │ │ -
│ │ │ │ -
73
│ │ │ │ -
74 /* ************************************************************************* */
│ │ │ │ -
75 template<class DERIVED, class FACTORGRAPH>
│ │ │ │ -
│ │ │ │ -
76 void BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::print(
│ │ │ │ -
77 const std::string& s, const KeyFormatter& keyFormatter) const
│ │ │ │ -
78 {
│ │ │ │ -
79 conditional_->print(s, keyFormatter);
│ │ │ │ -
80 }
│ │ │ │ -
│ │ │ │ +
55 public:
│ │ │ │ +
56 typedef KeyMap::const_iterator const_iterator;
│ │ │ │ +
57 typedef KeyMap::const_iterator iterator;
│ │ │ │ +
58 typedef KeyMap::value_type value_type;
│ │ │ │ +
59
│ │ │ │ +
62
│ │ │ │ +
64 VariableIndex() : nFactors_(0), nEntries_(0) {}
│ │ │ │ +
65
│ │ │ │ +
70 template <class FG>
│ │ │ │ +
│ │ │ │ +
71 explicit VariableIndex(const FG& factorGraph) : nFactors_(0), nEntries_(0) {
│ │ │ │ +
72 augment(factorGraph);
│ │ │ │ +
73 }
│ │ │ │ +
│ │ │ │ +
74
│ │ │ │ +
78
│ │ │ │ +
80 size_t size() const { return index_.size(); }
│ │ │ │
81
│ │ │ │ -
82 /* ************************************************************************* */
│ │ │ │ -
83 template<class DERIVED, class FACTORGRAPH>
│ │ │ │ -
│ │ │ │ -
84 size_t BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::treeSize() const {
│ │ │ │ -
85 size_t size = 1;
│ │ │ │ -
86 for(const derived_ptr& child: children)
│ │ │ │ -
87 size += child->treeSize();
│ │ │ │ -
88 return size;
│ │ │ │ -
89 }
│ │ │ │ -
│ │ │ │ -
90
│ │ │ │ -
91 /* ************************************************************************* */
│ │ │ │ -
92 template<class DERIVED, class FACTORGRAPH>
│ │ │ │ -
│ │ │ │ -
93 size_t BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::numCachedSeparatorMarginals() const
│ │ │ │ -
94 {
│ │ │ │ -
95 std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
│ │ │ │ -
96 if (!cachedSeparatorMarginal_)
│ │ │ │ -
97 return 0;
│ │ │ │ -
98
│ │ │ │ -
99 size_t subtree_count = 1;
│ │ │ │ -
100 for(const derived_ptr& child: children)
│ │ │ │ -
101 subtree_count += child->numCachedSeparatorMarginals();
│ │ │ │ -
102
│ │ │ │ -
103 return subtree_count;
│ │ │ │ -
104 }
│ │ │ │ +
83 size_t nFactors() const { return nFactors_; }
│ │ │ │ +
84
│ │ │ │ +
86 size_t nEntries() const { return nEntries_; }
│ │ │ │ +
87
│ │ │ │ +
│ │ │ │ +
89 const FactorIndices& operator[](Key variable) const {
│ │ │ │ +
90 KeyMap::const_iterator item = index_.find(variable);
│ │ │ │ +
91 if(item == index_.end())
│ │ │ │ +
92 throw std::invalid_argument("Requested non-existent variable from VariableIndex");
│ │ │ │ +
93 else
│ │ │ │ +
94 return item->second;
│ │ │ │ +
95 }
│ │ │ │ +
│ │ │ │ +
96
│ │ │ │ +
│ │ │ │ +
98 bool empty(Key variable) const {
│ │ │ │ +
99 return (*this)[variable].empty();
│ │ │ │ +
100 }
│ │ │ │
│ │ │ │ +
101
│ │ │ │
105
│ │ │ │ -
106 /* ************************************************************************* */
│ │ │ │ -
107 // The shortcut density is a conditional P(S|R) of the separator of this
│ │ │ │ -
108 // clique on the root. We can compute it recursively from the parent shortcut
│ │ │ │ -
109 // P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p
│ │ │ │ -
110 /* ************************************************************************* */
│ │ │ │ -
111 template<class DERIVED, class FACTORGRAPH>
│ │ │ │ -
112 typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::BayesNetType
│ │ │ │ -
│ │ │ │ -
113 BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::shortcut(const derived_ptr& B, Eliminate function) const
│ │ │ │ -
114 {
│ │ │ │ -
115 gttic(BayesTreeCliqueBase_shortcut);
│ │ │ │ -
116 // We only calculate the shortcut when this clique is not B
│ │ │ │ -
117 // and when the S\B is not empty
│ │ │ │ -
118 KeyVector S_setminus_B = separator_setminus_B(B);
│ │ │ │ -
119 if (!parent_.expired() /*(if we're not the root)*/ && !S_setminus_B.empty())
│ │ │ │ -
120 {
│ │ │ │ -
121 // Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph
│ │ │ │ -
122 derived_ptr parent(parent_.lock());
│ │ │ │ -
│ │ │ │ -
123 gttoc(BayesTreeCliqueBase_shortcut);
│ │ │ │ -
124 FactorGraphType p_Cp_B(parent->shortcut(B, function)); // P(Sp||B)
│ │ │ │ -
125 gttic(BayesTreeCliqueBase_shortcut);
│ │ │ │ -
126 p_Cp_B += parent->conditional_; // P(Fp|Sp)
│ │ │ │ -
127
│ │ │ │ -
128 // Determine the variables we want to keepSet, S union B
│ │ │ │ -
│ │ │ │ -
129 KeyVector keep = shortcut_indices(B, p_Cp_B);
│ │ │ │ +
107 bool equals(const VariableIndex& other, double tol=0.0) const;
│ │ │ │ +
108
│ │ │ │ +
110 void print(const std::string& str = "VariableIndex: ",
│ │ │ │ +
111 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ +
112
│ │ │ │ +
117 void outputMetisFormat(std::ostream& os) const;
│ │ │ │ +
118
│ │ │ │ +
119
│ │ │ │ +
123
│ │ │ │ +
128 template<class FG>
│ │ │ │ +
129 void augment(const FG& factors, boost::optional<const FactorIndices&> newFactorIndices = boost::none);
│ │ │ │
130
│ │ │ │ -
131 // Marginalize out everything except S union B
│ │ │ │ -
│ │ │ │ -
132 boost::shared_ptr<FactorGraphType> p_S_B = p_Cp_B.marginal(keep, function);
│ │ │ │ -
133 return *p_S_B->eliminatePartialSequential(S_setminus_B, function).first;
│ │ │ │ -
134 }
│ │ │ │ -
135 else
│ │ │ │ -
136 {
│ │ │ │ -
137 return BayesNetType();
│ │ │ │ -
138 }
│ │ │ │ -
139 }
│ │ │ │ -
140
│ │ │ │ -
141 /* *********************************************************************** */
│ │ │ │ -
142 // separator marginal, uses separator marginal of parent recursively
│ │ │ │ -
143 // P(C) = P(F|S) P(S)
│ │ │ │ -
144 /* *********************************************************************** */
│ │ │ │ -
145 template <class DERIVED, class FACTORGRAPH>
│ │ │ │ -
146 typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::FactorGraphType
│ │ │ │ -
│ │ │ │ -
147 BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::separatorMarginal(
│ │ │ │ -
148 Eliminate function) const {
│ │ │ │ -
149 std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
│ │ │ │ -
150 gttic(BayesTreeCliqueBase_separatorMarginal);
│ │ │ │ -
151 // Check if the Separator marginal was already calculated
│ │ │ │ -
152 if (!cachedSeparatorMarginal_) {
│ │ │ │ -
│ │ │ │ -
153 gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
│ │ │ │ +
136 void augmentExistingFactor(const FactorIndex factorIndex, const KeySet & newKeys);
│ │ │ │ +
137
│ │ │ │ +
148 template<typename ITERATOR, class FG>
│ │ │ │ +
149 void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors);
│ │ │ │ +
150
│ │ │ │ +
152 template<typename ITERATOR>
│ │ │ │ +
153 void removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey);
│ │ │ │
154
│ │ │ │ -
155 // If this is the root, there is no separator
│ │ │ │ -
│ │ │ │ -
156 if (parent_.expired() /*(if we're the root)*/) {
│ │ │ │ -
157 // we are root, return empty
│ │ │ │ -
158 FactorGraphType empty;
│ │ │ │ -
159 cachedSeparatorMarginal_ = empty;
│ │ │ │ -
160 } else {
│ │ │ │ -
161 // Flatten recursion in timing outline
│ │ │ │ -
162 gttoc(BayesTreeCliqueBase_separatorMarginal_cachemiss);
│ │ │ │ -
163 gttoc(BayesTreeCliqueBase_separatorMarginal);
│ │ │ │ -
164
│ │ │ │ -
165 // Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp)
│ │ │ │ -
166 // initialize P(Cp) with the parent separator marginal
│ │ │ │ -
167 derived_ptr parent(parent_.lock());
│ │ │ │ -
168 FactorGraphType p_Cp(parent->separatorMarginal(function)); // P(Sp)
│ │ │ │ -
│ │ │ │ -
169
│ │ │ │ -
170 gttic(BayesTreeCliqueBase_separatorMarginal);
│ │ │ │ -
171 gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss);
│ │ │ │ +
156 const_iterator begin() const { return index_.begin(); }
│ │ │ │ +
157
│ │ │ │ +
159 const_iterator end() const { return index_.end(); }
│ │ │ │ +
160
│ │ │ │ +
162 const_iterator find(Key key) const { return index_.find(key); }
│ │ │ │ +
163
│ │ │ │ +
164protected:
│ │ │ │ +
165 Factor_iterator factorsBegin(Key variable) { return internalAt(variable).begin(); }
│ │ │ │ +
166 Factor_iterator factorsEnd(Key variable) { return internalAt(variable).end(); }
│ │ │ │ +
167
│ │ │ │ +
168 Factor_const_iterator factorsBegin(Key variable) const { return internalAt(variable).begin(); }
│ │ │ │ +
169 Factor_const_iterator factorsEnd(Key variable) const { return internalAt(variable).end(); }
│ │ │ │ +
170
│ │ │ │
│ │ │ │ -
172
│ │ │ │ -
173 // now add the parent conditional
│ │ │ │ -
174 p_Cp += parent->conditional_; // P(Fp|Sp)
│ │ │ │ -
│ │ │ │ -
175
│ │ │ │ -
176 // The variables we want to keepSet are exactly the ones in S
│ │ │ │ -
177 KeyVector indicesS(this->conditional()->beginParents(),
│ │ │ │ -
178 this->conditional()->endParents());
│ │ │ │ -
179 auto separatorMarginal =
│ │ │ │ -
180 p_Cp.marginalMultifrontalBayesNet(Ordering(indicesS), function);
│ │ │ │ -
│ │ │ │ -
181 cachedSeparatorMarginal_.reset(*separatorMarginal);
│ │ │ │ -
182 }
│ │ │ │ -
183 }
│ │ │ │ -
184
│ │ │ │ -
185 // return the shortcut P(S||B)
│ │ │ │ -
186 return *cachedSeparatorMarginal_; // return the cached version
│ │ │ │ -
187 }
│ │ │ │ -
188
│ │ │ │ -
189 /* *********************************************************************** */
│ │ │ │ -
190 // marginal2, uses separator marginal of parent
│ │ │ │ -
191 // P(C) = P(F|S) P(S)
│ │ │ │ -
192 /* *********************************************************************** */
│ │ │ │ -
│ │ │ │ -
193 template <class DERIVED, class FACTORGRAPH>
│ │ │ │ -
194 typename BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::FactorGraphType
│ │ │ │ -
│ │ │ │ -
195 BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::marginal2(
│ │ │ │ -
196 Eliminate function) const {
│ │ │ │ -
197 gttic(BayesTreeCliqueBase_marginal2);
│ │ │ │ -
│ │ │ │ -
198 // initialize with separator marginal P(S)
│ │ │ │ -
199 FactorGraphType p_C = this->separatorMarginal(function);
│ │ │ │ -
200 // add the conditional P(F|S)
│ │ │ │ -
201 p_C += boost::shared_ptr<FactorType>(this->conditional_);
│ │ │ │ -
202 return p_C;
│ │ │ │ -
203 }
│ │ │ │ -
204
│ │ │ │ -
205 /* ************************************************************************* */
│ │ │ │ -
206 template<class DERIVED, class FACTORGRAPH>
│ │ │ │ -
│ │ │ │ -
207 void BayesTreeCliqueBase<DERIVED, FACTORGRAPH>::deleteCachedShortcuts() {
│ │ │ │ -
208
│ │ │ │ -
209 // When a shortcut is requested, all of the shortcuts between it and the
│ │ │ │ -
210 // root are also generated. So, if this clique's cached shortcut is set,
│ │ │ │ -
211 // recursively call over all child cliques. Otherwise, it is unnecessary.
│ │ │ │ -
212
│ │ │ │ -
213 std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
│ │ │ │ -
214 if (cachedSeparatorMarginal_) {
│ │ │ │ -
215 for(derived_ptr& child: children) {
│ │ │ │ -
216 child->deleteCachedShortcuts();
│ │ │ │ -
217 }
│ │ │ │ -
218
│ │ │ │ -
219 //Delete CachedShortcut for this clique
│ │ │ │ -
220 cachedSeparatorMarginal_ = boost::none;
│ │ │ │ -
221 }
│ │ │ │ -
222
│ │ │ │ -
223 }
│ │ │ │ -
│ │ │ │ -
224
│ │ │ │ -
225}
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ +
172 const FactorIndices& internalAt(Key variable) const {
│ │ │ │ +
173 const KeyMap::const_iterator item = index_.find(variable);
│ │ │ │ +
174 assert(item != index_.end());
│ │ │ │ +
175 return item->second;
│ │ │ │ +
176 }
│ │ │ │ +
│ │ │ │ +
177
│ │ │ │ +
│ │ │ │ +
179 FactorIndices& internalAt(Key variable) {
│ │ │ │ +
180 const KeyMap::iterator item = index_.find(variable);
│ │ │ │ +
181 assert(item != index_.end());
│ │ │ │ +
182 return item->second;
│ │ │ │ +
183 }
│ │ │ │
│ │ │ │ +
184
│ │ │ │ +
185private:
│ │ │ │ +
187 friend class boost::serialization::access;
│ │ │ │ +
188 template<class ARCHIVE>
│ │ │ │ +
189 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
190 ar & BOOST_SERIALIZATION_NVP(index_);
│ │ │ │ +
191 ar & BOOST_SERIALIZATION_NVP(nFactors_);
│ │ │ │ +
192 ar & BOOST_SERIALIZATION_NVP(nEntries_);
│ │ │ │ +
193 }
│ │ │ │ +
194
│ │ │ │ +
196};
│ │ │ │ +
│ │ │ │ +
197
│ │ │ │ +
199template<>
│ │ │ │ +
│ │ │ │ +
200struct traits<VariableIndex> : public Testable<VariableIndex> {
│ │ │ │ +
201};
│ │ │ │
│ │ │ │ -
timing.h
Timing utilities.
│ │ │ │ -
FactorGraph-inst.h
Factor Graph Base Class.
│ │ │ │ -
BayesTreeCliqueBase.h
Base class for cliques of a BayesTree.
│ │ │ │ +
202
│ │ │ │ +
203} //\ namespace gtsam
│ │ │ │ +
204
│ │ │ │ +
205#include <gtsam/inference/VariableIndex-inl.h>
│ │ │ │ +
FastMap.h
A thin wrapper around std::map that uses boost's fast_pool_allocator.
│ │ │ │ +
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ +
Factor.h
The base class for all factors.
│ │ │ │ + │ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::FactorIndex
std::uint64_t FactorIndex
Integer nonlinear factor index type.
Definition types.h:103
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition Factor.h:34
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ │ │ │ │
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::DiscreteFactorGraph
A Discrete Factor Graph is a factor graph where all factors are Discrete, i.e.
Definition DiscreteFactorGraph.h:101
│ │ │ │ -
gtsam::BayesTreeCliqueBase::treeSize
size_t treeSize() const
The size of subtree rooted at this clique, i.e., nr of Cliques.
Definition BayesTreeCliqueBase-inst.h:84
│ │ │ │ -
gtsam::BayesTreeCliqueBase::marginal2
FactorGraphType marginal2(Eliminate function=EliminationTraitsType::DefaultEliminate) const
return the marginal P(C) of the clique, using marginal caching
Definition BayesTreeCliqueBase-inst.h:195
│ │ │ │ -
gtsam::BayesTreeCliqueBase::equals
bool equals(const DERIVED &other, double tol=1e-9) const
check equality
Definition BayesTreeCliqueBase-inst.h:35
│ │ │ │ -
gtsam::BayesTreeCliqueBase::separatorMarginal
FactorGraphType separatorMarginal(Eliminate function=EliminationTraitsType::DefaultEliminate) const
return the marginal P(S) on the separator
Definition BayesTreeCliqueBase-inst.h:147
│ │ │ │ -
gtsam::BayesTreeCliqueBase::shortcut
BayesNetType shortcut(const derived_ptr &root, Eliminate function=EliminationTraitsType::DefaultEliminate) const
return the conditional P(S|Root) on the separator given the root
Definition BayesTreeCliqueBase-inst.h:113
│ │ │ │ -
gtsam::BayesTreeCliqueBase::deleteCachedShortcuts
void deleteCachedShortcuts()
This deletes the cached shortcuts of all cliques (subtree) below this clique.
Definition BayesTreeCliqueBase-inst.h:207
│ │ │ │ -
gtsam::BayesTreeCliqueBase::shortcut_indices
KeyVector shortcut_indices(const derived_ptr &B, const FactorGraphType &p_Cp_B) const
Determine variable indices to keep in recursive separator shortcut calculation The factor graph p_Cp_...
Definition BayesTreeCliqueBase-inst.h:57
│ │ │ │ -
gtsam::BayesTreeCliqueBase::separator_setminus_B
KeyVector separator_setminus_B(const derived_ptr &B) const
Calculate set for shortcut calculations.
Definition BayesTreeCliqueBase-inst.h:45
│ │ │ │ -
gtsam::BayesTreeCliqueBase::print
virtual void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print this node
Definition BayesTreeCliqueBase-inst.h:76
│ │ │ │ -
gtsam::BayesTreeCliqueBase::numCachedSeparatorMarginals
size_t numCachedSeparatorMarginals() const
Collect number of cliques with cached separator marginals.
Definition BayesTreeCliqueBase-inst.h:93
│ │ │ │ -
gtsam::BayesTreeCliqueBase::setEliminationResult
void setEliminationResult(const typename FactorGraphType::EliminationResult &eliminationResult)
Fill the elimination result produced during elimination.
Definition BayesTreeCliqueBase-inst.h:27
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition VariableIndex.h:43
│ │ │ │ +
gtsam::VariableIndex::internalAt
const FactorIndices & internalAt(Key variable) const
Internal version of 'at' that asserts existence.
Definition VariableIndex.h:172
│ │ │ │ +
gtsam::VariableIndex::nEntries
size_t nEntries() const
The number of nonzero blocks, i.e. the number of variable-factor entries.
Definition VariableIndex.h:86
│ │ │ │ +
gtsam::VariableIndex::begin
const_iterator begin() const
Iterator to the first variable entry.
Definition VariableIndex.h:156
│ │ │ │ +
gtsam::VariableIndex::find
const_iterator find(Key key) const
Find the iterator for the requested variable entry.
Definition VariableIndex.h:162
│ │ │ │ +
gtsam::VariableIndex::operator[]
const FactorIndices & operator[](Key variable) const
Access a list of factors by variable.
Definition VariableIndex.h:89
│ │ │ │ +
gtsam::VariableIndex::VariableIndex
VariableIndex(const FG &factorGraph)
Create a VariableIndex that computes and stores the block column structure of a factor graph.
Definition VariableIndex.h:71
│ │ │ │ +
gtsam::VariableIndex::empty
bool empty(Key variable) const
Return true if no factors associated with a variable.
Definition VariableIndex.h:98
│ │ │ │ +
gtsam::VariableIndex::size
size_t size() const
The number of variable entries. This is equal to the number of unique variable Keys.
Definition VariableIndex.h:80
│ │ │ │ +
gtsam::VariableIndex::internalAt
FactorIndices & internalAt(Key variable)
Internal version of 'at' that asserts existence.
Definition VariableIndex.h:179
│ │ │ │ +
gtsam::VariableIndex::end
const_iterator end() const
Iterator to the first variable entry.
Definition VariableIndex.h:159
│ │ │ │ +
gtsam::VariableIndex::VariableIndex
VariableIndex()
Default constructor, creates an empty VariableIndex.
Definition VariableIndex.h:64
│ │ │ │ +
gtsam::VariableIndex::nFactors
size_t nFactors() const
The number of factors in the original factor graph.
Definition VariableIndex.h:83
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,329 +1,255 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -BayesTreeCliqueBase-inst.h │ │ │ │ │ +VariableIndex.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ -9 * ------------------------------------------------------------------------- │ │ │ │ │ +9 │ │ │ │ │ +10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ -10 │ │ │ │ │ -17#pragma once │ │ │ │ │ -18 │ │ │ │ │ -19#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._h> │ │ │ │ │ -20#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_-_i_n_s_t_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_b_a_s_e_/_t_i_m_i_n_g_._h> │ │ │ │ │ -22 │ │ │ │ │ -23namespace _g_t_s_a_m { │ │ │ │ │ -24 │ │ │ │ │ -25 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -26 template │ │ │ │ │ -_2_7 void _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_D_E_R_I_V_E_D_,_ _F_A_C_T_O_R_G_R_A_P_H_>_:_:_s_e_t_E_l_i_m_i_n_a_t_i_o_n_R_e_s_u_l_t( │ │ │ │ │ -28 const typename FactorGraphType::EliminationResult& eliminationResult) │ │ │ │ │ -29 { │ │ │ │ │ -30 conditional_ = eliminationResult.first; │ │ │ │ │ -31 } │ │ │ │ │ -32 │ │ │ │ │ -33 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -34 template │ │ │ │ │ -_3_5 bool _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_D_E_R_I_V_E_D_,_ _F_A_C_T_O_R_G_R_A_P_H_>_:_:_e_q_u_a_l_s( │ │ │ │ │ -36 const DERIVED& other, double tol) const │ │ │ │ │ -37 { │ │ │ │ │ -38 return (!conditional_ && !other.conditional()) │ │ │ │ │ -39 || conditional_->_e_q_u_a_l_s(*other.conditional(), tol); │ │ │ │ │ -40 } │ │ │ │ │ -41 │ │ │ │ │ -42 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -43 template │ │ │ │ │ -44 _K_e_y_V_e_c_t_o_r │ │ │ │ │ -_4_5 _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_D_E_R_I_V_E_D_,_ _F_A_C_T_O_R_G_R_A_P_H_>_:_:_s_e_p_a_r_a_t_o_r___s_e_t_m_i_n_u_s___B(const │ │ │ │ │ -derived_ptr& B) const │ │ │ │ │ -46 { │ │ │ │ │ -47 _K_e_y_S_e_t p_F_S_parents(this->conditional()->beginParents(), this->conditional │ │ │ │ │ -()->endParents()); │ │ │ │ │ -48 _K_e_y_S_e_t indicesB(B->conditional()->begin(), B->conditional()->end()); │ │ │ │ │ -49 _K_e_y_V_e_c_t_o_r S_setminus_B; │ │ │ │ │ -50 std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(), │ │ │ │ │ -51 indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B)); │ │ │ │ │ -52 return S_setminus_B; │ │ │ │ │ -53 } │ │ │ │ │ +11 │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_M_a_p_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ +24#include │ │ │ │ │ +25 │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28 │ │ │ │ │ +29#include │ │ │ │ │ +30#include │ │ │ │ │ +31 │ │ │ │ │ +32namespace _g_t_s_a_m { │ │ │ │ │ +33 │ │ │ │ │ +_4_3class GTSAM_EXPORT _V_a_r_i_a_b_l_e_I_n_d_e_x { │ │ │ │ │ +44 public: │ │ │ │ │ +45 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +46 typedef FactorIndices::iterator Factor_iterator; │ │ │ │ │ +47 typedef FactorIndices::const_iterator Factor_const_iterator; │ │ │ │ │ +48 │ │ │ │ │ +49 protected: │ │ │ │ │ +50 typedef _F_a_s_t_M_a_p_<_K_e_y_,_ _F_a_c_t_o_r_I_n_d_i_c_e_s_> _K_e_y_M_a_p; │ │ │ │ │ +51 _K_e_y_M_a_p index_; │ │ │ │ │ +52 size_t nFactors_; // Number of factors in the original factor graph. │ │ │ │ │ +53 size_t nEntries_; // Sum of involved variable counts of each factor. │ │ │ │ │ 54 │ │ │ │ │ -55 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -56 template │ │ │ │ │ -_5_7 _K_e_y_V_e_c_t_o_r _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_D_E_R_I_V_E_D_,_ _F_A_C_T_O_R_G_R_A_P_H_>_:_:_s_h_o_r_t_c_u_t___i_n_d_i_c_e_s( │ │ │ │ │ -58 const derived_ptr& B, const FactorGraphType& p_Cp_B) const │ │ │ │ │ -59 { │ │ │ │ │ -60 gttic(shortcut_indices); │ │ │ │ │ -61 _K_e_y_S_e_t allKeys = p_Cp_B.keys(); │ │ │ │ │ -62 _K_e_y_S_e_t indicesB(B->conditional()->begin(), B->conditional()->end()); │ │ │ │ │ -63 _K_e_y_V_e_c_t_o_r S_setminus_B = separator_setminus_B(B); │ │ │ │ │ -64 _K_e_y_V_e_c_t_o_r keep; │ │ │ │ │ -65 // keep = S\B intersect allKeys (S_setminus_B is already sorted) │ │ │ │ │ -66 std::set_intersection(S_setminus_B.begin(), S_setminus_B.end(), // │ │ │ │ │ -67 allKeys.begin(), allKeys.end(), back_inserter(keep)); │ │ │ │ │ -68 // keep += B intersect allKeys │ │ │ │ │ -69 std::set_intersection(indicesB.begin(), indicesB.end(), // │ │ │ │ │ -70 allKeys.begin(), allKeys.end(), back_inserter(keep)); │ │ │ │ │ -71 return keep; │ │ │ │ │ -72 } │ │ │ │ │ -73 │ │ │ │ │ -74 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -75 template │ │ │ │ │ -_7_6 void _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_D_E_R_I_V_E_D_,_ _F_A_C_T_O_R_G_R_A_P_H_>_:_:_p_r_i_n_t( │ │ │ │ │ -77 const std::string& s, const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter) const │ │ │ │ │ -78 { │ │ │ │ │ -79 conditional_->print(s, keyFormatter); │ │ │ │ │ -80 } │ │ │ │ │ +55 public: │ │ │ │ │ +56 typedef KeyMap::const_iterator const_iterator; │ │ │ │ │ +57 typedef KeyMap::const_iterator iterator; │ │ │ │ │ +58 typedef KeyMap::value_type value_type; │ │ │ │ │ +59 │ │ │ │ │ +62 │ │ │ │ │ +_6_4 _V_a_r_i_a_b_l_e_I_n_d_e_x() : nFactors_(0), nEntries_(0) {} │ │ │ │ │ +65 │ │ │ │ │ +70 template │ │ │ │ │ +_7_1 explicit _V_a_r_i_a_b_l_e_I_n_d_e_x(const FG& factorGraph) : nFactors_(0), nEntries_(0) { │ │ │ │ │ +72 augment(factorGraph); │ │ │ │ │ +73 } │ │ │ │ │ +74 │ │ │ │ │ +78 │ │ │ │ │ +_8_0 size_t _s_i_z_e() const { return index_.size(); } │ │ │ │ │ 81 │ │ │ │ │ -82 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -83 template │ │ │ │ │ -_8_4 size_t _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_D_E_R_I_V_E_D_,_ _F_A_C_T_O_R_G_R_A_P_H_>_:_:_t_r_e_e_S_i_z_e() const { │ │ │ │ │ -85 size_t size = 1; │ │ │ │ │ -86 for(const derived_ptr& child: children) │ │ │ │ │ -87 size += child->treeSize(); │ │ │ │ │ -88 return size; │ │ │ │ │ -89 } │ │ │ │ │ -90 │ │ │ │ │ -91 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -92 template │ │ │ │ │ -_9_3 size_t _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_D_E_R_I_V_E_D_,_ _F_A_C_T_O_R_G_R_A_P_H_>_:_: │ │ │ │ │ -_n_u_m_C_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l_s() const │ │ │ │ │ -94 { │ │ │ │ │ -95 std::lock_guard marginalLock(cachedSeparatorMarginalMutex_); │ │ │ │ │ -96 if (!cachedSeparatorMarginal_) │ │ │ │ │ -97 return 0; │ │ │ │ │ -98 │ │ │ │ │ -99 size_t subtree_count = 1; │ │ │ │ │ -100 for(const derived_ptr& child: children) │ │ │ │ │ -101 subtree_count += child->numCachedSeparatorMarginals(); │ │ │ │ │ -102 │ │ │ │ │ -103 return subtree_count; │ │ │ │ │ -104 } │ │ │ │ │ +_8_3 size_t _n_F_a_c_t_o_r_s() const { return nFactors_; } │ │ │ │ │ +84 │ │ │ │ │ +_8_6 size_t _n_E_n_t_r_i_e_s() const { return nEntries_; } │ │ │ │ │ +87 │ │ │ │ │ +_8_9 const _F_a_c_t_o_r_I_n_d_i_c_e_s& _o_p_e_r_a_t_o_r_[_](_K_e_y variable) const { │ │ │ │ │ +90 KeyMap::const_iterator item = index_.find(variable); │ │ │ │ │ +91 if(item == index_.end()) │ │ │ │ │ +92 throw std::invalid_argument("Requested non-existent variable from │ │ │ │ │ +VariableIndex"); │ │ │ │ │ +93 else │ │ │ │ │ +94 return item->second; │ │ │ │ │ +95 } │ │ │ │ │ +96 │ │ │ │ │ +_9_8 bool _e_m_p_t_y(_K_e_y variable) const { │ │ │ │ │ +99 return (*this)[variable].empty(); │ │ │ │ │ +100 } │ │ │ │ │ +101 │ │ │ │ │ 105 │ │ │ │ │ -106 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -107 // The shortcut density is a conditional P(S|R) of the separator of this │ │ │ │ │ -108 // clique on the root. We can compute it recursively from the parent │ │ │ │ │ -shortcut │ │ │ │ │ -109 // P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p │ │ │ │ │ -110 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -111 template │ │ │ │ │ -112 typename BayesTreeCliqueBase::BayesNetType │ │ │ │ │ -_1_1_3 _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_D_E_R_I_V_E_D_,_ _F_A_C_T_O_R_G_R_A_P_H_>_:_:_s_h_o_r_t_c_u_t(const derived_ptr& B, │ │ │ │ │ -Eliminate function) const │ │ │ │ │ -114 { │ │ │ │ │ -115 gttic(BayesTreeCliqueBase_shortcut); │ │ │ │ │ -116 // We only calculate the shortcut when this clique is not B │ │ │ │ │ -117 // and when the S\B is not empty │ │ │ │ │ -118 _K_e_y_V_e_c_t_o_r S_setminus_B = separator_setminus_B(B); │ │ │ │ │ -119 if (!parent_.expired() /*(if we're not the root)*/ && !S_setminus_B.empty │ │ │ │ │ -()) │ │ │ │ │ -120 { │ │ │ │ │ -121 // Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph │ │ │ │ │ -122 derived_ptr parent(parent_.lock()); │ │ │ │ │ -_1_2_3 gttoc(BayesTreeCliqueBase_shortcut); │ │ │ │ │ -124 _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e p_Cp_B(parent->shortcut(B, function)); // P(Sp||B) │ │ │ │ │ -125 gttic(BayesTreeCliqueBase_shortcut); │ │ │ │ │ -126 p_Cp_B += parent->conditional_; // P(Fp|Sp) │ │ │ │ │ -127 │ │ │ │ │ -128 // Determine the variables we want to keepSet, S union B │ │ │ │ │ -_1_2_9 _K_e_y_V_e_c_t_o_r keep = shortcut_indices(B, p_Cp_B); │ │ │ │ │ +107 bool _e_q_u_a_l_s(const _V_a_r_i_a_b_l_e_I_n_d_e_x& other, double tol=0.0) const; │ │ │ │ │ +108 │ │ │ │ │ +110 void _p_r_i_n_t(const std::string& str = "VariableIndex: ", │ │ │ │ │ +111 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const; │ │ │ │ │ +112 │ │ │ │ │ +117 void outputMetisFormat(std::ostream& os) const; │ │ │ │ │ +118 │ │ │ │ │ +119 │ │ │ │ │ +123 │ │ │ │ │ +128 template │ │ │ │ │ +129 void augment(const FG& factors, boost::optional │ │ │ │ │ +newFactorIndices = boost::none); │ │ │ │ │ 130 │ │ │ │ │ -131 // Marginalize out everything except S union B │ │ │ │ │ -_1_3_2 boost::shared_ptr p_S_B = p_Cp_B.marginal(keep, function); │ │ │ │ │ -133 return *p_S_B->eliminatePartialSequential(S_setminus_B, function).first; │ │ │ │ │ -134 } │ │ │ │ │ -135 else │ │ │ │ │ -136 { │ │ │ │ │ -137 return BayesNetType(); │ │ │ │ │ -138 } │ │ │ │ │ -139 } │ │ │ │ │ -140 │ │ │ │ │ -141 /* *********************************************************************** │ │ │ │ │ -*/ │ │ │ │ │ -142 // separator marginal, uses separator marginal of parent recursively │ │ │ │ │ -143 // P(C) = P(F|S) P(S) │ │ │ │ │ -144 /* *********************************************************************** │ │ │ │ │ -*/ │ │ │ │ │ -145 template │ │ │ │ │ -146 typename BayesTreeCliqueBase::FactorGraphType │ │ │ │ │ -_1_4_7 _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_D_E_R_I_V_E_D_,_ _F_A_C_T_O_R_G_R_A_P_H_>_:_:_s_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l( │ │ │ │ │ -148 Eliminate function) const { │ │ │ │ │ -149 std::lock_guard marginalLock(cachedSeparatorMarginalMutex_); │ │ │ │ │ -150 gttic(BayesTreeCliqueBase_separatorMarginal); │ │ │ │ │ -151 // Check if the Separator marginal was already calculated │ │ │ │ │ -152 if (!cachedSeparatorMarginal_) { │ │ │ │ │ -_1_5_3 gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss); │ │ │ │ │ +136 void augmentExistingFactor(const _F_a_c_t_o_r_I_n_d_e_x factorIndex, const _K_e_y_S_e_t & │ │ │ │ │ +newKeys); │ │ │ │ │ +137 │ │ │ │ │ +148 template │ │ │ │ │ +149 void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors); │ │ │ │ │ +150 │ │ │ │ │ +152 template │ │ │ │ │ +153 void removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey); │ │ │ │ │ 154 │ │ │ │ │ -155 // If this is the root, there is no separator │ │ │ │ │ -_1_5_6 if (parent_.expired() /*(if we're the root)*/) { │ │ │ │ │ -157 // we are root, return empty │ │ │ │ │ -158 _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e empty; │ │ │ │ │ -159 cachedSeparatorMarginal_ = empty; │ │ │ │ │ -160 } else { │ │ │ │ │ -161 // Flatten recursion in timing outline │ │ │ │ │ -162 gttoc(BayesTreeCliqueBase_separatorMarginal_cachemiss); │ │ │ │ │ -163 gttoc(BayesTreeCliqueBase_separatorMarginal); │ │ │ │ │ -164 │ │ │ │ │ -165 // Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp) │ │ │ │ │ -166 // initialize P(Cp) with the parent separator marginal │ │ │ │ │ -167 derived_ptr parent(parent_.lock()); │ │ │ │ │ -168 FactorGraphType p_Cp(parent->separatorMarginal(function)); // P(Sp) │ │ │ │ │ -_1_6_9 │ │ │ │ │ -170 gttic(BayesTreeCliqueBase_separatorMarginal); │ │ │ │ │ -171 gttic(BayesTreeCliqueBase_separatorMarginal_cachemiss); │ │ │ │ │ -_1_7_2 │ │ │ │ │ -173 // now add the parent conditional │ │ │ │ │ -174 p_Cp += parent->conditional_; // P(Fp|Sp) │ │ │ │ │ -_1_7_5 │ │ │ │ │ -176 // The variables we want to keepSet are exactly the ones in S │ │ │ │ │ -177 _K_e_y_V_e_c_t_o_r indicesS(this->conditional()->beginParents(), │ │ │ │ │ -178 this->conditional()->endParents()); │ │ │ │ │ -179 auto separatorMarginal = │ │ │ │ │ -180 p_Cp.marginalMultifrontalBayesNet(_O_r_d_e_r_i_n_g(indicesS), function); │ │ │ │ │ -_1_8_1 cachedSeparatorMarginal_.reset(*separatorMarginal); │ │ │ │ │ -182 } │ │ │ │ │ +_1_5_6 const_iterator _b_e_g_i_n() const { return index_.begin(); } │ │ │ │ │ +157 │ │ │ │ │ +_1_5_9 const_iterator _e_n_d() const { return index_.end(); } │ │ │ │ │ +160 │ │ │ │ │ +_1_6_2 const_iterator _f_i_n_d(_K_e_y key) const { return index_.find(key); } │ │ │ │ │ +163 │ │ │ │ │ +164protected: │ │ │ │ │ +165 Factor_iterator factorsBegin(_K_e_y variable) { return internalAt │ │ │ │ │ +(variable).begin(); } │ │ │ │ │ +166 Factor_iterator factorsEnd(Key variable) { return internalAt(variable).end │ │ │ │ │ +(); } │ │ │ │ │ +167 │ │ │ │ │ +168 Factor_const_iterator factorsBegin(Key variable) const { return internalAt │ │ │ │ │ +(variable).begin(); } │ │ │ │ │ +169 Factor_const_iterator factorsEnd(Key variable) const { return internalAt │ │ │ │ │ +(variable).end(); } │ │ │ │ │ +170 │ │ │ │ │ +_1_7_2 const _F_a_c_t_o_r_I_n_d_i_c_e_s& _i_n_t_e_r_n_a_l_A_t(_K_e_y variable) const { │ │ │ │ │ +173 const KeyMap::const_iterator item = index_.find(variable); │ │ │ │ │ +174 assert(item != index_.end()); │ │ │ │ │ +175 return item->second; │ │ │ │ │ +176 } │ │ │ │ │ +177 │ │ │ │ │ +_1_7_9 _F_a_c_t_o_r_I_n_d_i_c_e_s& _i_n_t_e_r_n_a_l_A_t(_K_e_y variable) { │ │ │ │ │ +180 const KeyMap::iterator item = index_.find(variable); │ │ │ │ │ +181 assert(item != index_.end()); │ │ │ │ │ +182 return item->second; │ │ │ │ │ 183 } │ │ │ │ │ 184 │ │ │ │ │ -185 // return the shortcut P(S||B) │ │ │ │ │ -186 return *cachedSeparatorMarginal_; // return the cached version │ │ │ │ │ -187 } │ │ │ │ │ -188 │ │ │ │ │ -189 /* *********************************************************************** │ │ │ │ │ -*/ │ │ │ │ │ -190 // marginal2, uses separator marginal of parent │ │ │ │ │ -191 // P(C) = P(F|S) P(S) │ │ │ │ │ -192 /* *********************************************************************** │ │ │ │ │ -*/ │ │ │ │ │ -_1_9_3 template │ │ │ │ │ -194 typename BayesTreeCliqueBase::FactorGraphType │ │ │ │ │ -_1_9_5 _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_D_E_R_I_V_E_D_,_ _F_A_C_T_O_R_G_R_A_P_H_>_:_:_m_a_r_g_i_n_a_l_2( │ │ │ │ │ -196 Eliminate function) const { │ │ │ │ │ -197 gttic(BayesTreeCliqueBase_marginal2); │ │ │ │ │ -_1_9_8 // initialize with separator marginal P(S) │ │ │ │ │ -199 _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e p_C = this->separatorMarginal(function); │ │ │ │ │ -200 // add the conditional P(F|S) │ │ │ │ │ -201 p_C += boost::shared_ptr(this->conditional_); │ │ │ │ │ -202 return p_C; │ │ │ │ │ -203 } │ │ │ │ │ +185private: │ │ │ │ │ +_1_8_7 friend class boost::serialization::access; │ │ │ │ │ +188 template │ │ │ │ │ +189 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +190 ar & BOOST_SERIALIZATION_NVP(index_); │ │ │ │ │ +191 ar & BOOST_SERIALIZATION_NVP(nFactors_); │ │ │ │ │ +192 ar & BOOST_SERIALIZATION_NVP(nEntries_); │ │ │ │ │ +193 } │ │ │ │ │ +194 │ │ │ │ │ +196}; │ │ │ │ │ +197 │ │ │ │ │ +199template<> │ │ │ │ │ +_2_0_0struct _t_r_a_i_t_s<_V_a_r_i_a_b_l_e_I_n_d_e_x> : public _T_e_s_t_a_b_l_e { │ │ │ │ │ +201}; │ │ │ │ │ +202 │ │ │ │ │ +203} //\ namespace gtsam │ │ │ │ │ 204 │ │ │ │ │ -205 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -206 template │ │ │ │ │ -_2_0_7 void _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_D_E_R_I_V_E_D_,_ _F_A_C_T_O_R_G_R_A_P_H_>_:_:_d_e_l_e_t_e_C_a_c_h_e_d_S_h_o_r_t_c_u_t_s() { │ │ │ │ │ -208 │ │ │ │ │ -209 // When a shortcut is requested, all of the shortcuts between it and the │ │ │ │ │ -210 // root are also generated. So, if this clique's cached shortcut is set, │ │ │ │ │ -211 // recursively call over all child cliques. Otherwise, it is unnecessary. │ │ │ │ │ -212 │ │ │ │ │ -213 std::lock_guard marginalLock(cachedSeparatorMarginalMutex_); │ │ │ │ │ -214 if (cachedSeparatorMarginal_) { │ │ │ │ │ -215 for(derived_ptr& child: children) { │ │ │ │ │ -216 child->deleteCachedShortcuts(); │ │ │ │ │ -217 } │ │ │ │ │ -218 │ │ │ │ │ -219 //Delete CachedShortcut for this clique │ │ │ │ │ -220 cachedSeparatorMarginal_ = boost::none; │ │ │ │ │ -221 } │ │ │ │ │ -222 │ │ │ │ │ -223 } │ │ │ │ │ -224 │ │ │ │ │ -225} │ │ │ │ │ -_t_i_m_i_n_g_._h │ │ │ │ │ -Timing utilities. │ │ │ │ │ -_F_a_c_t_o_r_G_r_a_p_h_-_i_n_s_t_._h │ │ │ │ │ -Factor Graph Base Class. │ │ │ │ │ -_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._h │ │ │ │ │ -Base class for cliques of a BayesTree. │ │ │ │ │ +205#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_V_a_r_i_a_b_l_e_I_n_d_e_x_-_i_n_l_._h> │ │ │ │ │ +_F_a_s_t_M_a_p_._h │ │ │ │ │ +A thin wrapper around std::map that uses boost's fast_pool_allocator. │ │ │ │ │ +_F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ +A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ +_F_a_c_t_o_r_._h │ │ │ │ │ +The base class for all factors. │ │ │ │ │ +_K_e_y_._h │ │ │ │ │ +_V_a_r_i_a_b_l_e_I_n_d_e_x_-_i_n_l_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_I_n_d_e_x │ │ │ │ │ +std::uint64_t FactorIndex │ │ │ │ │ +Integer nonlinear factor index type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:103 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_I_n_d_i_c_e_s │ │ │ │ │ +FastVector< FactorIndex > FactorIndices │ │ │ │ │ +Define collection types: │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ +FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ +fast_pool_allocator instead of the defa... │ │ │ │ │ +DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ _g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ _g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ Template to create a binary predicate. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A Discrete Factor Graph is a factor graph where all factors are Discrete, i.e. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactorGraph.h:101 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_t_r_e_e_S_i_z_e │ │ │ │ │ -size_t treeSize() const │ │ │ │ │ -The size of subtree rooted at this clique, i.e., nr of Cliques. │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:84 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_m_a_r_g_i_n_a_l_2 │ │ │ │ │ -FactorGraphType marginal2(Eliminate function=EliminationTraitsType:: │ │ │ │ │ -DefaultEliminate) const │ │ │ │ │ -return the marginal P(C) of the clique, using marginal caching │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:195 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const DERIVED &other, double tol=1e-9) const │ │ │ │ │ -check equality │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_s_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l │ │ │ │ │ -FactorGraphType separatorMarginal(Eliminate function=EliminationTraitsType:: │ │ │ │ │ -DefaultEliminate) const │ │ │ │ │ -return the marginal P(S) on the separator │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:147 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_s_h_o_r_t_c_u_t │ │ │ │ │ -BayesNetType shortcut(const derived_ptr &root, Eliminate │ │ │ │ │ -function=EliminationTraitsType::DefaultEliminate) const │ │ │ │ │ -return the conditional P(S|Root) on the separator given the root │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:113 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_d_e_l_e_t_e_C_a_c_h_e_d_S_h_o_r_t_c_u_t_s │ │ │ │ │ -void deleteCachedShortcuts() │ │ │ │ │ -This deletes the cached shortcuts of all cliques (subtree) below this clique. │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:207 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_s_h_o_r_t_c_u_t___i_n_d_i_c_e_s │ │ │ │ │ -KeyVector shortcut_indices(const derived_ptr &B, const FactorGraphType &p_Cp_B) │ │ │ │ │ -const │ │ │ │ │ -Determine variable indices to keep in recursive separator shortcut calculation │ │ │ │ │ -The factor graph p_Cp_... │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_s_e_p_a_r_a_t_o_r___s_e_t_m_i_n_u_s___B │ │ │ │ │ -KeyVector separator_setminus_B(const derived_ptr &B) const │ │ │ │ │ -Calculate set for shortcut calculations. │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_p_r_i_n_t │ │ │ │ │ -virtual void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ -print this node │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:76 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_n_u_m_C_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l_s │ │ │ │ │ -size_t numCachedSeparatorMarginals() const │ │ │ │ │ -Collect number of cliques with cached separator marginals. │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:93 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_s_e_t_E_l_i_m_i_n_a_t_i_o_n_R_e_s_u_l_t │ │ │ │ │ -void setEliminationResult(const typename FactorGraphType::EliminationResult │ │ │ │ │ -&eliminationResult) │ │ │ │ │ -Fill the elimination result produced during elimination. │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:27 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ +The VariableIndex class computes and stores the block column structure of a │ │ │ │ │ +factor graph. │ │ │ │ │ +DDeeffiinniittiioonn VariableIndex.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_i_n_t_e_r_n_a_l_A_t │ │ │ │ │ +const FactorIndices & internalAt(Key variable) const │ │ │ │ │ +Internal version of 'at' that asserts existence. │ │ │ │ │ +DDeeffiinniittiioonn VariableIndex.h:172 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_n_E_n_t_r_i_e_s │ │ │ │ │ +size_t nEntries() const │ │ │ │ │ +The number of nonzero blocks, i.e. the number of variable-factor entries. │ │ │ │ │ +DDeeffiinniittiioonn VariableIndex.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_b_e_g_i_n │ │ │ │ │ +const_iterator begin() const │ │ │ │ │ +Iterator to the first variable entry. │ │ │ │ │ +DDeeffiinniittiioonn VariableIndex.h:156 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_f_i_n_d │ │ │ │ │ +const_iterator find(Key key) const │ │ │ │ │ +Find the iterator for the requested variable entry. │ │ │ │ │ +DDeeffiinniittiioonn VariableIndex.h:162 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_o_p_e_r_a_t_o_r_[_] │ │ │ │ │ +const FactorIndices & operator[](Key variable) const │ │ │ │ │ +Access a list of factors by variable. │ │ │ │ │ +DDeeffiinniittiioonn VariableIndex.h:89 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ +VariableIndex(const FG &factorGraph) │ │ │ │ │ +Create a VariableIndex that computes and stores the block column structure of a │ │ │ │ │ +factor graph. │ │ │ │ │ +DDeeffiinniittiioonn VariableIndex.h:71 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_e_m_p_t_y │ │ │ │ │ +bool empty(Key variable) const │ │ │ │ │ +Return true if no factors associated with a variable. │ │ │ │ │ +DDeeffiinniittiioonn VariableIndex.h:98 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_s_i_z_e │ │ │ │ │ +size_t size() const │ │ │ │ │ +The number of variable entries. This is equal to the number of unique variable │ │ │ │ │ +Keys. │ │ │ │ │ +DDeeffiinniittiioonn VariableIndex.h:80 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_i_n_t_e_r_n_a_l_A_t │ │ │ │ │ +FactorIndices & internalAt(Key variable) │ │ │ │ │ +Internal version of 'at' that asserts existence. │ │ │ │ │ +DDeeffiinniittiioonn VariableIndex.h:179 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_e_n_d │ │ │ │ │ +const_iterator end() const │ │ │ │ │ +Iterator to the first variable entry. │ │ │ │ │ +DDeeffiinniittiioonn VariableIndex.h:159 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ +VariableIndex() │ │ │ │ │ +Default constructor, creates an empty VariableIndex. │ │ │ │ │ +DDeeffiinniittiioonn VariableIndex.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_n_F_a_c_t_o_r_s │ │ │ │ │ +size_t nFactors() const │ │ │ │ │ +The number of factors in the original factor graph. │ │ │ │ │ +DDeeffiinniittiioonn VariableIndex.h:83 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_-_i_n_s_t_._h │ │ │ │ │ + * _V_a_r_i_a_b_l_e_I_n_d_e_x_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00626.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/inference-inst.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/ISAM-inst.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,87 +94,37 @@ │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
inference-inst.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
ISAM-inst.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Contains generic inference algorithms that convert between templated graphical models, i.e., factor graphs, Bayes nets, and Bayes trees. │ │ │ │ +

Incremental update functionality (iSAM) for BayesTree. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

template<class TREE , class RESULT >
FastVector< typename TREE::sharedFactor > gtsam::inference::EliminateTree (RESULT &result, const TREE &tree, const typename TREE::Eliminate &function)
 Eliminate an elimination tree or a Bayes tree (used internally).
 
│ │ │ │

Detailed Description

│ │ │ │ -

Contains generic inference algorithms that convert between templated graphical models, i.e., factor graphs, Bayes nets, and Bayes trees.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ -

Function Documentation

│ │ │ │ - │ │ │ │ -

◆ EliminateTree()

│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -template<class TREE , class RESULT >
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
FastVector< typename TREE::sharedFactor > gtsam::inference::EliminateTree (RESULT & result,
const TREE & tree,
const typename TREE::Eliminate & function 
)
│ │ │ │ -
│ │ │ │ - │ │ │ │ -

Eliminate an elimination tree or a Bayes tree (used internally).

│ │ │ │ -

Requires TREE::BayesNetType, TREE::FactorGraphType, TREE::sharedConditional, TREE::sharedFactor, TREE::Node, TREE::sharedNode, TREE::Node::factors, TREE::Node::children.

│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ +

Incremental update functionality (iSAM) for BayesTree.

│ │ │ │ +
Author
Michael Kaess
│ │ │ │ +
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,45 +1,21 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -inference-inst.h File Reference │ │ │ │ │ -Contains ggeenneerriicc inference algorithms that convert between templated graphical │ │ │ │ │ -models, i.e., factor graphs, Bayes nets, and Bayes trees. _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +ISAM-inst.h File Reference │ │ │ │ │ +Incremental update functionality (iSAM) for BayesTree. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ -_F_a_s_t_V_e_c_t_o_r< typename TREE::sharedFactor _g_t_s_a_m_:_:_i_n_f_e_r_e_n_c_e_:_:_E_l_i_m_i_n_a_t_e_T_r_e_e (RESULT │ │ │ │ │ - >  &result, const TREE &tree, const │ │ │ │ │ - typename TREE::Eliminate &function) │ │ │ │ │ -  Eliminate an elimination tree or a │ │ │ │ │ - Bayes tree (used internally). │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Contains ggeenneerriicc inference algorithms that convert between templated graphical │ │ │ │ │ -models, i.e., factor graphs, Bayes nets, and Bayes trees. │ │ │ │ │ +Incremental update functionality (iSAM) for BayesTree. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Richard Roberts │ │ │ │ │ -********** FFuunnccttiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ -********** _?◆_? EElliimmiinnaatteeTTrreeee(()) ********** │ │ │ │ │ -template │ │ │ │ │ -FastVector< typename TREE:: │ │ │ │ │ -sharedFactor > gtsam::inference:: ( RESULT &  rreessuulltt, │ │ │ │ │ -EliminateTree │ │ │ │ │ - const TREE &  ttrreeee, │ │ │ │ │ - const typename TREE::Eliminate &  ffuunnccttiioonn  │ │ │ │ │ - ) │ │ │ │ │ -Eliminate an elimination tree or a Bayes tree (used internally). │ │ │ │ │ -Requires TREE::BayesNetType, TREE::FactorGraphType, TREE::sharedConditional, │ │ │ │ │ -TREE::sharedFactor, TREE::Node, TREE::sharedNode, TREE::Node::factors, TREE:: │ │ │ │ │ -Node::children. │ │ │ │ │ + Michael Kaess │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _i_n_f_e_r_e_n_c_e_-_i_n_s_t_._h │ │ │ │ │ + * _I_S_A_M_-_i_n_s_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00626_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/inference-inst.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/ISAM-inst.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,109 +98,100 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
inference-inst.h
│ │ │ │ +
ISAM-inst.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3* GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ -
4* Atlanta, Georgia 30332-0415
│ │ │ │ -
5* All Rights Reserved
│ │ │ │ -
6* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
4 * Atlanta, Georgia 30332-0415
│ │ │ │ +
5 * All Rights Reserved
│ │ │ │ +
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │ -
8* See LICENSE for the license information
│ │ │ │ +
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │ -
10* -------------------------------------------------------------------------- */
│ │ │ │ +
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <boost/shared_ptr.hpp>
│ │ │ │ -
23#include <utility>
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/inference/ISAM.h>
│ │ │ │ +
21#include <gtsam/inference/VariableIndex.h>
│ │ │ │ +
22
│ │ │ │ +
23namespace gtsam {
│ │ │ │
24
│ │ │ │ -
25#include <gtsam/base/treeTraversal-inst.h>
│ │ │ │ -
26#include <gtsam/base/FastVector.h>
│ │ │ │ -
27
│ │ │ │ -
28namespace gtsam {
│ │ │ │ -
29 namespace inference {
│ │ │ │ -
30
│ │ │ │ -
31 namespace {
│ │ │ │ -
32 /* ************************************************************************* */
│ │ │ │ -
33 template<class TREE>
│ │ │ │ -
34 struct EliminationData {
│ │ │ │ -
35 EliminationData* const parentData;
│ │ │ │ -
36 FastVector<typename TREE::sharedFactor> childFactors;
│ │ │ │ -
37 EliminationData(EliminationData* _parentData, size_t nChildren) :
│ │ │ │ -
38 parentData(_parentData) { childFactors.reserve(nChildren); }
│ │ │ │ -
39 };
│ │ │ │ -
40
│ │ │ │ -
41 /* ************************************************************************* */
│ │ │ │ -
42 template<class TREE>
│ │ │ │ -
43 EliminationData<TREE> eliminationPreOrderVisitor(
│ │ │ │ -
44 const typename TREE::sharedNode& node, EliminationData<TREE>& parentData)
│ │ │ │ -
45 {
│ │ │ │ -
46 // This function is called before visiting the children. Here, we create this node's data,
│ │ │ │ -
47 // which includes a pointer to the parent data and space for the factors of the children.
│ │ │ │ -
48 return EliminationData<TREE>(&parentData, node->children.size());
│ │ │ │ -
49 }
│ │ │ │ +
25/* ************************************************************************* */
│ │ │ │ +
26template<class BAYESTREE>
│ │ │ │ +
│ │ │ │ +
27void ISAM<BAYESTREE>::updateInternal(const FactorGraphType& newFactors,
│ │ │ │ +
28 Cliques* orphans, const Eliminate& function) {
│ │ │ │ +
29 // Remove the contaminated part of the Bayes tree
│ │ │ │ +
30 BayesNetType bn;
│ │ │ │ +
31 const KeySet newFactorKeys = newFactors.keys();
│ │ │ │ +
32 if (!this->empty()) {
│ │ │ │ +
33 KeyVector keyVector(newFactorKeys.begin(), newFactorKeys.end());
│ │ │ │ +
34 this->removeTop(keyVector, &bn, orphans);
│ │ │ │ +
35 }
│ │ │ │ +
36
│ │ │ │ +
37 // Add the removed top and the new factors
│ │ │ │ +
38 FactorGraphType factors;
│ │ │ │ +
39 factors += bn;
│ │ │ │ +
40 factors += newFactors;
│ │ │ │ +
41
│ │ │ │ +
42 // Add the orphaned subtrees
│ │ │ │ +
43 for (const sharedClique& orphan : *orphans)
│ │ │ │ +
44 factors += boost::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan);
│ │ │ │ +
45
│ │ │ │ +
46 // Get an ordering where the new keys are eliminated last
│ │ │ │ +
47 const VariableIndex index(factors);
│ │ │ │ +
48 const Ordering ordering = Ordering::ColamdConstrainedLast(index,
│ │ │ │ +
49 KeyVector(newFactorKeys.begin(), newFactorKeys.end()));
│ │ │ │
50
│ │ │ │ -
51 /* ************************************************************************* */
│ │ │ │ -
52 template<class TREE, class RESULT>
│ │ │ │ -
53 struct EliminationPostOrderVisitor
│ │ │ │ -
54 {
│ │ │ │ -
55 RESULT& result;
│ │ │ │ -
56 const typename TREE::Eliminate& eliminationFunction;
│ │ │ │ -
57 EliminationPostOrderVisitor(RESULT& result, const typename TREE::Eliminate& eliminationFunction) :
│ │ │ │ -
58 result(result), eliminationFunction(eliminationFunction) {}
│ │ │ │ -
59 void operator()(const typename TREE::sharedNode& node, EliminationData<TREE>& myData)
│ │ │ │ -
60 {
│ │ │ │ -
61 // Call eliminate on the node and add the result to the parent's gathered factors
│ │ │ │ -
62 typename TREE::sharedFactor childFactor = node->eliminate(result, eliminationFunction, myData.childFactors);
│ │ │ │ -
63 if(childFactor && !childFactor->empty())
│ │ │ │ -
64 myData.parentData->childFactors.push_back(childFactor);
│ │ │ │ -
65 }
│ │ │ │ -
66 };
│ │ │ │ -
67 }
│ │ │ │ -
68
│ │ │ │ -
69 /* ************************************************************************* */
│ │ │ │ -
73 template<class TREE, class RESULT>
│ │ │ │ -
74 FastVector<typename TREE::sharedFactor>
│ │ │ │ -
│ │ │ │ -
75 EliminateTree(RESULT& result, const TREE& tree, const typename TREE::Eliminate& function)
│ │ │ │ -
76 {
│ │ │ │ -
77 // Do elimination using a depth-first traversal. During the pre-order visit (see
│ │ │ │ -
78 // eliminationPreOrderVisitor), we store a pointer to the parent data (where we'll put the
│ │ │ │ -
79 // remaining factor) and reserve a vector of factors to store the children elimination
│ │ │ │ -
80 // results. During the post-order visit (see eliminationPostOrderVisitor), we call dense
│ │ │ │ -
81 // elimination (using the gathered child factors) and store the result in the parent's
│ │ │ │ -
82 // gathered factors.
│ │ │ │ -
83 EliminationData<TREE> rootData(0, tree.roots().size());
│ │ │ │ -
84 EliminationPostOrderVisitor<TREE,RESULT> visitorPost(result, function);
│ │ │ │ -
85 treeTraversal::DepthFirstForest(tree, rootData, eliminationPreOrderVisitor<TREE>, visitorPost);
│ │ │ │ -
86
│ │ │ │ -
87 // Return remaining factors
│ │ │ │ -
88 return rootData.childFactors;
│ │ │ │ -
89 }
│ │ │ │ -
│ │ │ │ -
90
│ │ │ │ -
91 }
│ │ │ │ -
92}
│ │ │ │ - │ │ │ │ -
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ -
gtsam::inference::EliminateTree
FastVector< typename TREE::sharedFactor > EliminateTree(RESULT &result, const TREE &tree, const typename TREE::Eliminate &function)
Eliminate an elimination tree or a Bayes tree (used internally).
Definition inference-inst.h:75
│ │ │ │ +
51 // eliminate all factors (top, added, orphans) into a new Bayes tree
│ │ │ │ +
52 auto bayesTree = factors.eliminateMultifrontal(ordering, function, index);
│ │ │ │ +
53
│ │ │ │ +
54 // Re-add into Bayes tree data structures
│ │ │ │ +
55 this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(),
│ │ │ │ +
56 bayesTree->roots().end());
│ │ │ │ +
57 this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end());
│ │ │ │ +
58}
│ │ │ │ +
│ │ │ │ +
59
│ │ │ │ +
60/* ************************************************************************* */
│ │ │ │ +
61template<class BAYESTREE>
│ │ │ │ +
│ │ │ │ +
62void ISAM<BAYESTREE>::update(const FactorGraphType& newFactors,
│ │ │ │ +
63 const Eliminate& function) {
│ │ │ │ +
│ │ │ │ +
64 Cliques orphans;
│ │ │ │ +
65 this->updateInternal(newFactors, &orphans, function);
│ │ │ │ +
66}
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
67
│ │ │ │ +
68}
│ │ │ │ + │ │ │ │ +
ISAM.h
Incremental update functionality (iSAM) for BayesTree.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::treeTraversal::DepthFirstForest
void DepthFirstForest(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, VISITOR_POST &visitorPost)
Traverse a forest depth-first with pre-order and post-order visits.
Definition treeTraversal-inst.h:77
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ + │ │ │ │ +
gtsam::BayesTreeOrphanWrapper
Definition BayesTree.h:276
│ │ │ │ +
gtsam::ISAM::update
void update(const FactorGraphType &newFactors, const Eliminate &function=EliminationTraitsType::DefaultEliminate)
update the Bayes tree with a set of new factors, typically derived from measurements
Definition ISAM-inst.h:62
│ │ │ │ +
gtsam::ISAM::updateInternal
void updateInternal(const FactorGraphType &newFactors, Cliques *orphans, const Eliminate &function=EliminationTraitsType::DefaultEliminate)
updateInternal provides access to list of orphans for drawing purposes
Definition ISAM-inst.h:27
│ │ │ │ +
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
gtsam::Ordering::ColamdConstrainedLast
static Ordering ColamdConstrainedLast(const FACTOR_GRAPH &graph, const KeyVector &constrainLast, bool forceOrder=false)
Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details for note o...
Definition Ordering.h:114
│ │ │ │ +
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition VariableIndex.h:43
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,127 +1,113 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -inference-inst.h │ │ │ │ │ +ISAM-inst.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3* GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ -4* Atlanta, Georgia 30332-0415 │ │ │ │ │ -5* All Rights Reserved │ │ │ │ │ -6* Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +5 * All Rights Reserved │ │ │ │ │ +6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ -8* See LICENSE for the license information │ │ │ │ │ +8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ -10* ------------------------------------------------------------------------- │ │ │ │ │ +10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include │ │ │ │ │ -23#include │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_I_S_A_M_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_V_a_r_i_a_b_l_e_I_n_d_e_x_._h> │ │ │ │ │ +22 │ │ │ │ │ +23namespace _g_t_s_a_m { │ │ │ │ │ 24 │ │ │ │ │ -25#include <_g_t_s_a_m_/_b_a_s_e_/_t_r_e_e_T_r_a_v_e_r_s_a_l_-_i_n_s_t_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ -27 │ │ │ │ │ -28namespace _g_t_s_a_m { │ │ │ │ │ -29 namespace inference { │ │ │ │ │ -30 │ │ │ │ │ -31 namespace { │ │ │ │ │ -32 /* ************************************************************************* │ │ │ │ │ +25/* ************************************************************************* │ │ │ │ │ */ │ │ │ │ │ -33 template │ │ │ │ │ -34 struct EliminationData { │ │ │ │ │ -35 EliminationData* const parentData; │ │ │ │ │ -36 FastVector childFactors; │ │ │ │ │ -37 EliminationData(EliminationData* _parentData, size_t nChildren) : │ │ │ │ │ -38 parentData(_parentData) { childFactors.reserve(nChildren); } │ │ │ │ │ -39 }; │ │ │ │ │ -40 │ │ │ │ │ -41 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -42 template │ │ │ │ │ -43 EliminationData eliminationPreOrderVisitor( │ │ │ │ │ -44 const typename TREE::sharedNode& node, EliminationData& parentData) │ │ │ │ │ -45 { │ │ │ │ │ -46 // This function is called before visiting the children. Here, we create │ │ │ │ │ -this node's data, │ │ │ │ │ -47 // which includes a pointer to the parent data and space for the factors of │ │ │ │ │ -the children. │ │ │ │ │ -48 return EliminationData(&parentData, node->children.size()); │ │ │ │ │ -49 } │ │ │ │ │ +26template │ │ │ │ │ +_2_7void _I_S_A_M_<_B_A_Y_E_S_T_R_E_E_>_:_:_u_p_d_a_t_e_I_n_t_e_r_n_a_l(const FactorGraphType& newFactors, │ │ │ │ │ +28 Cliques* orphans, const Eliminate& function) { │ │ │ │ │ +29 // Remove the contaminated part of the Bayes tree │ │ │ │ │ +30 BayesNetType bn; │ │ │ │ │ +31 const _K_e_y_S_e_t newFactorKeys = newFactors.keys(); │ │ │ │ │ +32 if (!this->empty()) { │ │ │ │ │ +33 _K_e_y_V_e_c_t_o_r keyVector(newFactorKeys.begin(), newFactorKeys.end()); │ │ │ │ │ +34 this->removeTop(keyVector, &bn, orphans); │ │ │ │ │ +35 } │ │ │ │ │ +36 │ │ │ │ │ +37 // Add the removed top and the new factors │ │ │ │ │ +38 FactorGraphType factors; │ │ │ │ │ +39 factors += bn; │ │ │ │ │ +40 factors += newFactors; │ │ │ │ │ +41 │ │ │ │ │ +42 // Add the orphaned subtrees │ │ │ │ │ +43 for (const sharedClique& orphan : *orphans) │ │ │ │ │ +44 factors += boost::make_shared<_B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r_<_C_l_i_q_u_e_> >(orphan); │ │ │ │ │ +45 │ │ │ │ │ +46 // Get an ordering where the new keys are eliminated last │ │ │ │ │ +47 const _V_a_r_i_a_b_l_e_I_n_d_e_x index(factors); │ │ │ │ │ +48 const _O_r_d_e_r_i_n_g ordering = _O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_L_a_s_t(index, │ │ │ │ │ +49 _K_e_y_V_e_c_t_o_r(newFactorKeys.begin(), newFactorKeys.end())); │ │ │ │ │ 50 │ │ │ │ │ -51 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -52 template │ │ │ │ │ -53 struct EliminationPostOrderVisitor │ │ │ │ │ -54 { │ │ │ │ │ -55 RESULT& result; │ │ │ │ │ -56 const typename TREE::Eliminate& eliminationFunction; │ │ │ │ │ -57 EliminationPostOrderVisitor(RESULT& result, const typename TREE::Eliminate& │ │ │ │ │ -eliminationFunction) : │ │ │ │ │ -58 result(result), eliminationFunction(eliminationFunction) {} │ │ │ │ │ -59 void operator()(const typename TREE::sharedNode& node, │ │ │ │ │ -EliminationData& myData) │ │ │ │ │ -60 { │ │ │ │ │ -61 // Call eliminate on the node and add the result to the parent's gathered │ │ │ │ │ -factors │ │ │ │ │ -62 typename TREE::sharedFactor childFactor = node->eliminate(result, │ │ │ │ │ -eliminationFunction, myData.childFactors); │ │ │ │ │ -63 if(childFactor && !childFactor->empty()) │ │ │ │ │ -64 myData.parentData->childFactors.push_back(childFactor); │ │ │ │ │ -65 } │ │ │ │ │ -66 }; │ │ │ │ │ -67 } │ │ │ │ │ -68 │ │ │ │ │ -69 /* ************************************************************************* │ │ │ │ │ +51 // eliminate all factors (top, added, orphans) into a new Bayes tree │ │ │ │ │ +52 auto bayesTree = factors.eliminateMultifrontal(ordering, function, index); │ │ │ │ │ +53 │ │ │ │ │ +54 // Re-add into Bayes tree data structures │ │ │ │ │ +55 this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), │ │ │ │ │ +56 bayesTree->roots().end()); │ │ │ │ │ +57 this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); │ │ │ │ │ +58} │ │ │ │ │ +59 │ │ │ │ │ +60/* ************************************************************************* │ │ │ │ │ */ │ │ │ │ │ -73 template │ │ │ │ │ -74 FastVector │ │ │ │ │ -_7_5 _E_l_i_m_i_n_a_t_e_T_r_e_e(RESULT& result, const TREE& tree, const typename TREE:: │ │ │ │ │ -Eliminate& function) │ │ │ │ │ -76 { │ │ │ │ │ -77 // Do elimination using a depth-first traversal. During the pre-order visit │ │ │ │ │ -(see │ │ │ │ │ -78 // eliminationPreOrderVisitor), we store a pointer to the parent data (where │ │ │ │ │ -we'll put the │ │ │ │ │ -79 // remaining factor) and reserve a vector of factors to store the children │ │ │ │ │ -elimination │ │ │ │ │ -80 // results. During the post-order visit (see eliminationPostOrderVisitor), │ │ │ │ │ -we call dense │ │ │ │ │ -81 // elimination (using the gathered child factors) and store the result in │ │ │ │ │ -the parent's │ │ │ │ │ -82 // gathered factors. │ │ │ │ │ -83 EliminationData rootData(0, tree.roots().size()); │ │ │ │ │ -84 EliminationPostOrderVisitor visitorPost(result, function); │ │ │ │ │ -85 _t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t(tree, rootData, │ │ │ │ │ -eliminationPreOrderVisitor, visitorPost); │ │ │ │ │ -86 │ │ │ │ │ -87 // Return remaining factors │ │ │ │ │ -88 return rootData.childFactors; │ │ │ │ │ -89 } │ │ │ │ │ -90 │ │ │ │ │ -91 } │ │ │ │ │ -92} │ │ │ │ │ -_t_r_e_e_T_r_a_v_e_r_s_a_l_-_i_n_s_t_._h │ │ │ │ │ -_F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ -A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_f_e_r_e_n_c_e_:_:_E_l_i_m_i_n_a_t_e_T_r_e_e │ │ │ │ │ -FastVector< typename TREE::sharedFactor > EliminateTree(RESULT &result, const │ │ │ │ │ -TREE &tree, const typename TREE::Eliminate &function) │ │ │ │ │ -Eliminate an elimination tree or a Bayes tree (used internally). │ │ │ │ │ -DDeeffiinniittiioonn inference-inst.h:75 │ │ │ │ │ +61template │ │ │ │ │ +_6_2void _I_S_A_M_<_B_A_Y_E_S_T_R_E_E_>_:_:_u_p_d_a_t_e(const FactorGraphType& newFactors, │ │ │ │ │ +63 const Eliminate& function) { │ │ │ │ │ +_6_4 Cliques orphans; │ │ │ │ │ +65 this->updateInternal(newFactors, &orphans, function); │ │ │ │ │ +66} │ │ │ │ │ +67 │ │ │ │ │ +68} │ │ │ │ │ +_V_a_r_i_a_b_l_e_I_n_d_e_x_._h │ │ │ │ │ +_I_S_A_M_._h │ │ │ │ │ +Incremental update functionality (iSAM) for BayesTree. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t │ │ │ │ │ -void DepthFirstForest(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, │ │ │ │ │ -VISITOR_POST &visitorPost) │ │ │ │ │ -Traverse a forest depth-first with pre-order and post-order visits. │ │ │ │ │ -DDeeffiinniittiioonn treeTraversal-inst.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:276 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_:_:_u_p_d_a_t_e │ │ │ │ │ +void update(const FactorGraphType &newFactors, const Eliminate │ │ │ │ │ +&function=EliminationTraitsType::DefaultEliminate) │ │ │ │ │ +update the Bayes tree with a set of new factors, typically derived from │ │ │ │ │ +measurements │ │ │ │ │ +DDeeffiinniittiioonn ISAM-inst.h:62 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_:_:_u_p_d_a_t_e_I_n_t_e_r_n_a_l │ │ │ │ │ +void updateInternal(const FactorGraphType &newFactors, Cliques *orphans, const │ │ │ │ │ +Eliminate &function=EliminationTraitsType::DefaultEliminate) │ │ │ │ │ +updateInternal provides access to list of orphans for drawing purposes │ │ │ │ │ +DDeeffiinniittiioonn ISAM-inst.h:27 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_L_a_s_t │ │ │ │ │ +static Ordering ColamdConstrainedLast(const FACTOR_GRAPH &graph, const │ │ │ │ │ +KeyVector &constrainLast, bool forceOrder=false) │ │ │ │ │ +Compute a fill-reducing ordering using constrained COLAMD from a factor graph │ │ │ │ │ +(see details for note o... │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:114 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ +The VariableIndex class computes and stores the block column structure of a │ │ │ │ │ +factor graph. │ │ │ │ │ +DDeeffiinniittiioonn VariableIndex.h:43 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _i_n_f_e_r_e_n_c_e_-_i_n_s_t_._h │ │ │ │ │ + * _I_S_A_M_-_i_n_s_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00629.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/VariableSlots.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/VariableIndex.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,31 +95,31 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces
│ │ │ │ -
VariableSlots.cpp File Reference
│ │ │ │ +
VariableIndex.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │
Author
Richard Roberts
│ │ │ │ -
Date
Oct 5, 2010
│ │ │ │ +
Date
March 26, 2013
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,20 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -VariableSlots.cpp File Reference │ │ │ │ │ +VariableIndex.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Author │ │ │ │ │ Richard Roberts │ │ │ │ │ Date │ │ │ │ │ - Oct 5, 2010 │ │ │ │ │ + March 26, 2013 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _V_a_r_i_a_b_l_e_S_l_o_t_s_._c_p_p │ │ │ │ │ + * _V_a_r_i_a_b_l_e_I_n_d_e_x_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00632.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/JunctionTree.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/DotWriter.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,49 +94,36 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
JunctionTree.h File Reference
│ │ │ │ +
DotWriter.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

The junction tree. │ │ │ │ +

Graphviz formatting for factor graphs. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::JunctionTree< BAYESTREE, GRAPH >
 A JunctionTree is a cluster tree, a set of variable clusters with factors, arranged in a tree, with the additional property that it represents the clique tree associated with a Bayes Net. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

The junction tree.

│ │ │ │ -
Date
Feb 4, 2010
│ │ │ │ -
Author
Kai Ni
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ +

Graphviz formatting for factor graphs.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
Date
December, 2021
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,31 +1,22 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -JunctionTree.h File Reference │ │ │ │ │ -The junction tree. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_J_u_n_c_t_i_o_n_T_r_e_e_<_ _B_A_Y_E_S_T_R_E_E_,_ _G_R_A_P_H_ _> │ │ │ │ │ - A _J_u_n_c_t_i_o_n_T_r_e_e is a cluster tree, a set of variable clusters with │ │ │ │ │ -  factors, arranged in a tree, with the additional property that it │ │ │ │ │ - represents the clique tree associated with a Bayes Net. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +DotWriter.cpp File Reference │ │ │ │ │ +Graphviz formatting for factor graphs. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -The junction tree. │ │ │ │ │ - Date │ │ │ │ │ - Feb 4, 2010 │ │ │ │ │ +Graphviz formatting for factor graphs. │ │ │ │ │ Author │ │ │ │ │ - Kai Ni │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + December, 2021 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _J_u_n_c_t_i_o_n_T_r_e_e_._h │ │ │ │ │ + * _D_o_t_W_r_i_t_e_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00635.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/ClusterTree.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/graph.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,54 +95,83 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
ClusterTree.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Macros | │ │ │ │ +Functions
│ │ │ │ +
graph.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Collects factorgraph fragments defined on variable clusters, arranged in a tree. │ │ │ │ +

Graph algorithm using boost library. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::ClusterTree< GRAPH >
 A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman: each node k represents a subset \( C_k \sub X \), and the tree is family preserving, in that each factor \( f_i \) is associated with a single cluster and \( scope(f_i) \sub C_k \). More...
class  gtsam::SDGraph< KEY >
 SDGraph is undirected graph with variable keys and double edge weights. More...
 
struct  gtsam::ClusterTree< GRAPH >::Cluster
 A Cluster is just a collection of factors. More...
class  gtsam::SGraph< KEY >
 
class  gtsam::EliminatableClusterTree< BAYESTREE, GRAPH >
 A cluster-tree that eliminates to a Bayes tree. More...
class  gtsam::PredecessorMap< KEY >
 Map from variable key to parent key. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +template<class KEY >
std::list< KEY > gtsam::predecessorMap2Keys (const PredecessorMap< KEY > &p_map)
 Generate a list of keys from a spanning tree represented by its predecessor map.
 
│ │ │ │ +template<class G , class F , class KEY >
SDGraph< KEY > gtsam::toBoostGraph (const G &graph)
 Convert the factor graph to an SDGraph G = Graph type F = Factor type Key = Key type.
 
template<class G , class V , class KEY >
boost::tuple< G, V, std::map< KEY, V > > gtsam::predecessorMap2Graph (const PredecessorMap< KEY > &p_map)
 Build takes a predecessor map, and builds a directed graph corresponding to the tree.
 
│ │ │ │ +template<class G , class Factor , class POSE , class KEY >
boost::shared_ptr< Values > gtsam::composePoses (const G &graph, const PredecessorMap< KEY > &tree, const POSE &rootPose)
 Compose the poses by following the chain specified by the spanning tree.
 
│ │ │ │ +template<class G , class KEY , class FACTOR2 >
PredecessorMap< KEY > gtsam::findMinimumSpanningTree (const G &g)
 find the minimum spanning tree using boost graph library
 
│ │ │ │ +template<class G , class KEY , class FACTOR2 >
void gtsam::split (const G &g, const PredecessorMap< KEY > &tree, G &Ab1, G &Ab2)
 Split the graph into two parts: one corresponds to the given spanning tree, and the other corresponds to the rest of the factors.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Collects factorgraph fragments defined on variable clusters, arranged in a tree.

│ │ │ │ -
Date
Oct 8, 2013
│ │ │ │ -
Author
Kai Ni
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ +

Graph algorithm using boost library.

│ │ │ │ +
Author
: Kai Ni
│ │ │ │ +
Date
Jan 11, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,41 +1,75 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -ClusterTree.h File Reference │ │ │ │ │ -Collects factorgraph fragments defined on variable clusters, arranged in a │ │ │ │ │ -tree. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +graph.h File Reference │ │ │ │ │ +Graph algorithm using boost library. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_<_ _G_R_A_P_H_ _> │ │ │ │ │ - A cluster-tree is associated with a factor graph and is defined as in │ │ │ │ │ - Koller-Friedman: each node k represents a subset \( C_k \sub X \), and │ │ │ │ │ -  the tree is family preserving, in that each factor \( f_i \) is │ │ │ │ │ - associated with a single cluster and \( scope(f_i) \sub C_k \). │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_S_D_G_r_a_p_h_<_ _K_E_Y_ _> │ │ │ │ │ +  _S_D_G_r_a_p_h is undirected graph with variable keys and double edge weights. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_<_ _G_R_A_P_H_ _>_:_:_C_l_u_s_t_e_r │ │ │ │ │ -  A _C_l_u_s_t_e_r is just a collection of factors. _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_S_G_r_a_p_h_<_ _K_E_Y_ _> │ │ │ │ │   │ │ │ │ │ - class   _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_<_ _B_A_Y_E_S_T_R_E_E_,_ _G_R_A_P_H_ _> │ │ │ │ │ -  A cluster-tree that eliminates to a Bayes tree. _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_P_r_e_d_e_c_e_s_s_o_r_M_a_p_<_ _K_E_Y_ _> │ │ │ │ │ +  Map from variable key to parent key. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +template │ │ │ │ │ + std::list< KEY >  ggttssaamm::::pprreeddeecceessssoorrMMaapp22KKeeyyss (const │ │ │ │ │ + _P_r_e_d_e_c_e_s_s_o_r_M_a_p< KEY > &p_map) │ │ │ │ │ +  Generate a list of keys from a spanning │ │ │ │ │ + tree represented by its predecessor map. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _S_D_G_r_a_p_h< KEY >  ggttssaamm::::ttooBBoooossttGGrraapphh (const G &graph) │ │ │ │ │ + Convert the factor graph to an _S_D_G_r_a_p_h G │ │ │ │ │ +  = Graph type F = _F_a_c_t_o_r type Key = Key │ │ │ │ │ + type. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +boost::tuple< G, V, std::map< KEY, V > _g_t_s_a_m_:_:_p_r_e_d_e_c_e_s_s_o_r_M_a_p_2_G_r_a_p_h (const │ │ │ │ │ + >  _P_r_e_d_e_c_e_s_s_o_r_M_a_p< KEY > &p_map) │ │ │ │ │ + Build takes a predecessor map, and │ │ │ │ │ +  builds a directed graph corresponding to │ │ │ │ │ + the tree. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + boost::shared_ptr< _V_a_l_u_e_s >  ggttssaamm::::ccoommppoosseePPoosseess (const G &graph, │ │ │ │ │ + const _P_r_e_d_e_c_e_s_s_o_r_M_a_p< KEY > &tree, const │ │ │ │ │ + POSE &rootPose) │ │ │ │ │ +  Compose the poses by following the chain │ │ │ │ │ + specified by the spanning tree. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _P_r_e_d_e_c_e_s_s_o_r_M_a_p< KEY >  ggttssaamm::::ffiinnddMMiinniimmuummSSppaannnniinnggTTrreeee (const G │ │ │ │ │ + &g) │ │ │ │ │ +  find the minimum spanning tree using │ │ │ │ │ + boost graph library │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + void  ggttssaamm::::sspplliitt (const G &g, const │ │ │ │ │ + _P_r_e_d_e_c_e_s_s_o_r_M_a_p< KEY > &tree, G &Ab1, G │ │ │ │ │ + &Ab2) │ │ │ │ │ + Split the graph into two parts: one │ │ │ │ │ +  corresponds to the given spanning tree, │ │ │ │ │ + and the other corresponds to the rest of │ │ │ │ │ + the factors. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Collects factorgraph fragments defined on variable clusters, arranged in a │ │ │ │ │ -tree. │ │ │ │ │ - Date │ │ │ │ │ - Oct 8, 2013 │ │ │ │ │ +Graph algorithm using boost library. │ │ │ │ │ Author │ │ │ │ │ - Kai Ni │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + : Kai Ni │ │ │ │ │ + Date │ │ │ │ │ + Jan 11, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _C_l_u_s_t_e_r_T_r_e_e_._h │ │ │ │ │ + * _g_r_a_p_h_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00635.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,5 +1,11 @@ │ │ │ │ │ var a00635 = [ │ │ │ │ │ - ["gtsam::ClusterTree< GRAPH >", "a03564.html", "a03564"], │ │ │ │ │ - ["gtsam::ClusterTree< GRAPH >::Cluster", "a03568.html", "a03568"], │ │ │ │ │ - ["gtsam::EliminatableClusterTree< BAYESTREE, GRAPH >", "a03528.html", "a03528"] │ │ │ │ │ + ["gtsam::SDGraph< KEY >", "a03616.html", null], │ │ │ │ │ + ["gtsam::SGraph< KEY >", "a03620.html", null], │ │ │ │ │ + ["gtsam::PredecessorMap< KEY >", "a03624.html", "a03624"], │ │ │ │ │ + ["composePoses", "a00635.html#a62b34d6038ccdfa84d8a5bb0c15328b1", null], │ │ │ │ │ + ["findMinimumSpanningTree", "a00635.html#a7771a103b41708e88763ff67059ff01d", null], │ │ │ │ │ + ["predecessorMap2Graph", "a00635.html#ae67d0d86c4e55b175859967a70b36983", null], │ │ │ │ │ + ["predecessorMap2Keys", "a00635.html#a18a265afdea057ea54be10262396be12", null], │ │ │ │ │ + ["split", "a00635.html#a3453247aec1d8b4642947ffd4bca426b", null], │ │ │ │ │ + ["toBoostGraph", "a00635.html#a6c0319a6ebcc83d4a5cf9ff51729bb80", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00635_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/ClusterTree.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/graph.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,273 +98,115 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ClusterTree.h
│ │ │ │ +
graph.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1
│ │ │ │ -
10#pragma once
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +
2
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
4 * Atlanta, Georgia 30332-0415
│ │ │ │ +
5 * All Rights Reserved
│ │ │ │ +
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
7
│ │ │ │ +
8 * See LICENSE for the license information
│ │ │ │ +
9
│ │ │ │ +
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
12#include <gtsam/base/Testable.h>
│ │ │ │ -
13#include <gtsam/base/FastVector.h>
│ │ │ │ -
14#include <gtsam/inference/Ordering.h>
│ │ │ │ -
15
│ │ │ │ -
16namespace gtsam {
│ │ │ │ -
17
│ │ │ │ -
24template <class GRAPH>
│ │ │ │ -
│ │ │ │ -
25class ClusterTree {
│ │ │ │ -
26 public:
│ │ │ │ -
27 typedef GRAPH FactorGraphType;
│ │ │ │ -
28 typedef ClusterTree<GRAPH> This;
│ │ │ │ -
29 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
30
│ │ │ │ -
31 typedef typename GRAPH::FactorType FactorType;
│ │ │ │ -
32 typedef boost::shared_ptr<FactorType> sharedFactor;
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <map>
│ │ │ │ +
22
│ │ │ │ +
23#define BOOST_NO_HASH // to pacify the warnings about depricated headers in boost.graph
│ │ │ │ +
24
│ │ │ │ +
25#include <boost/graph/graph_traits.hpp>
│ │ │ │ +
26#include <boost/graph/adjacency_list.hpp>
│ │ │ │ +
27#include <boost/shared_ptr.hpp>
│ │ │ │ +
28#include <gtsam/nonlinear/Values.h>
│ │ │ │ +
29
│ │ │ │ +
30namespace gtsam {
│ │ │ │ +
31
│ │ │ │ +
32 // type definitions :
│ │ │ │
33
│ │ │ │ -
35 // TODO(frank): re-factor JunctionTree so we can make members private
│ │ │ │ -
│ │ │ │ -
36 struct Cluster {
│ │ │ │ -
37 typedef FastVector<boost::shared_ptr<Cluster> > Children;
│ │ │ │ -
38 Children children;
│ │ │ │ -
39
│ │ │ │ -
40 typedef Ordering Keys;
│ │ │ │ -
41 Keys orderedFrontalKeys;
│ │ │ │ -
42
│ │ │ │ -
43 FactorGraphType factors;
│ │ │ │ +
37 template<class KEY>
│ │ │ │ +
│ │ │ │ +
38 class SDGraph: public boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
│ │ │ │ +
39 boost::property<boost::vertex_name_t, KEY>, boost::property<
│ │ │ │ +
40 boost::edge_weight_t, double> > {
│ │ │ │ +
41 public:
│ │ │ │ +
42 typedef typename boost::graph_traits<SDGraph<KEY> >::vertex_descriptor Vertex;
│ │ │ │ +
43 };
│ │ │ │ +
│ │ │ │
44
│ │ │ │ -
45 int problemSize_;
│ │ │ │ -
46
│ │ │ │ -
47 Cluster() : problemSize_(0) {}
│ │ │ │ -
48
│ │ │ │ -
49 virtual ~Cluster() {}
│ │ │ │ -
50
│ │ │ │ -
51 const Cluster& operator[](size_t i) const {
│ │ │ │ -
52 return *(children.at(i));
│ │ │ │ -
53 }
│ │ │ │ -
54
│ │ │ │ -
56 template <class CONTAINER>
│ │ │ │ -
│ │ │ │ -
57 Cluster(Key key, const CONTAINER& factorsToAdd)
│ │ │ │ -
58 : problemSize_(0) {
│ │ │ │ -
59 addFactors(key, factorsToAdd);
│ │ │ │ -
60 }
│ │ │ │ -
│ │ │ │ -
61
│ │ │ │ -
63 template <class CONTAINER>
│ │ │ │ -
│ │ │ │ -
64 void addFactors(Key key, const CONTAINER& factorsToAdd) {
│ │ │ │ -
65 orderedFrontalKeys.push_back(key);
│ │ │ │ -
66 factors.push_back(factorsToAdd);
│ │ │ │ -
67 problemSize_ += factors.size();
│ │ │ │ -
68 }
│ │ │ │ -
│ │ │ │ -
69
│ │ │ │ -
│ │ │ │ -
71 void addChild(const boost::shared_ptr<Cluster>& cluster) {
│ │ │ │ -
72 children.push_back(cluster);
│ │ │ │ -
73 problemSize_ = std::max(problemSize_, cluster->problemSize_);
│ │ │ │ -
74 }
│ │ │ │ -
│ │ │ │ -
75
│ │ │ │ -
76 size_t nrChildren() const {
│ │ │ │ -
77 return children.size();
│ │ │ │ -
78 }
│ │ │ │ +
45 template<class KEY>
│ │ │ │ +
│ │ │ │ +
46 class SGraph : public boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS,
│ │ │ │ +
47 boost::property<boost::vertex_name_t, KEY> > {
│ │ │ │ +
48 public:
│ │ │ │ +
49 typedef typename boost::graph_traits<SGraph<KEY> >::vertex_descriptor Vertex;
│ │ │ │ +
50 };
│ │ │ │ +
│ │ │ │ +
51
│ │ │ │ +
52 //typedef boost::graph_traits<SGraph>::vertex_descriptor SVertex;
│ │ │ │ +
53
│ │ │ │ +
57 template<class KEY>
│ │ │ │ +
│ │ │ │ +
58 class PredecessorMap: public std::map<KEY, KEY> {
│ │ │ │ +
59 public:
│ │ │ │ +
│ │ │ │ +
61 inline void insert(const KEY& key, const KEY& parent) {
│ │ │ │ +
62 std::map<KEY, KEY>::insert(std::make_pair(key, parent));
│ │ │ │ +
63 }
│ │ │ │ +
│ │ │ │ +
64 };
│ │ │ │ +
│ │ │ │ +
65
│ │ │ │ +
69 template<class KEY>
│ │ │ │ +
70 std::list<KEY> predecessorMap2Keys(const PredecessorMap<KEY>& p_map);
│ │ │ │ +
71
│ │ │ │ +
78 template<class G, class F, class KEY> SDGraph<KEY> toBoostGraph(const G& graph);
│ │ │ │
79
│ │ │ │ -
80 size_t nrFactors() const {
│ │ │ │ -
81 return factors.size();
│ │ │ │ -
82 }
│ │ │ │ -
83
│ │ │ │ -
84 size_t nrFrontals() const {
│ │ │ │ -
85 return orderedFrontalKeys.size();
│ │ │ │ -
86 }
│ │ │ │ +
85 template<class G, class V, class KEY>
│ │ │ │ +
86 boost::tuple<G, V, std::map<KEY,V> > predecessorMap2Graph(const PredecessorMap<KEY>& p_map);
│ │ │ │
87
│ │ │ │ -
88 int problemSize() const {
│ │ │ │ -
89 return problemSize_;
│ │ │ │ -
90 }
│ │ │ │ -
91
│ │ │ │ -
93 virtual void print(const std::string& s = "",
│ │ │ │ -
94 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ +
91 template<class G, class Factor, class POSE, class KEY>
│ │ │ │ +
92 boost::shared_ptr<Values>
│ │ │ │ +
93 composePoses(const G& graph, const PredecessorMap<KEY>& tree, const POSE& rootPose);
│ │ │ │ +
94
│ │ │ │
95
│ │ │ │ -
97 std::vector<size_t> nrFrontalsOfChildren() const;
│ │ │ │ -
98
│ │ │ │ -
100 void merge(const boost::shared_ptr<Cluster>& cluster);
│ │ │ │ +
99 template<class G, class KEY, class FACTOR2>
│ │ │ │ +
100 PredecessorMap<KEY> findMinimumSpanningTree(const G& g) ;
│ │ │ │
101
│ │ │ │ -
103 void mergeChildren(const std::vector<bool>& merge);
│ │ │ │ -
104 };
│ │ │ │ -
│ │ │ │ -
105
│ │ │ │ -
106 typedef boost::shared_ptr<Cluster> sharedCluster;
│ │ │ │ -
107
│ │ │ │ -
108 // Define Node=Cluster for compatibility with tree traversal functions
│ │ │ │ -
109 typedef Cluster Node;
│ │ │ │ -
110 typedef sharedCluster sharedNode;
│ │ │ │ +
106 template<class G, class KEY, class FACTOR2>
│ │ │ │ +
107 void split(const G& g, const PredecessorMap<KEY>& tree, G& Ab1, G& Ab2) ;
│ │ │ │ +
108
│ │ │ │ +
109
│ │ │ │ +
110} // namespace gtsam
│ │ │ │
111
│ │ │ │ -
113 GTSAM_CONCEPT_TESTABLE_TYPE(FactorType)
│ │ │ │ -
114
│ │ │ │ -
115 protected:
│ │ │ │ -
116 FastVector<sharedNode> roots_;
│ │ │ │ -
117
│ │ │ │ -
120
│ │ │ │ -
│ │ │ │ -
123 ClusterTree(const This& other) {
│ │ │ │ -
124 *this = other;
│ │ │ │ -
125 }
│ │ │ │ -
│ │ │ │ -
126
│ │ │ │ -
128
│ │ │ │ -
129 public:
│ │ │ │ -
130
│ │ │ │ -
132 ClusterTree() {}
│ │ │ │ -
133
│ │ │ │ -
136
│ │ │ │ -
138 void print(const std::string& s = "",
│ │ │ │ -
139 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ -
140
│ │ │ │ -
144
│ │ │ │ -
145 void addRoot(const boost::shared_ptr<Cluster>& cluster) {
│ │ │ │ -
146 roots_.push_back(cluster);
│ │ │ │ -
147 }
│ │ │ │ -
148
│ │ │ │ -
149 void addChildrenAsRoots(const boost::shared_ptr<Cluster>& cluster) {
│ │ │ │ -
150 for (auto child : cluster->children)
│ │ │ │ -
151 this->addRoot(child);
│ │ │ │ -
152 }
│ │ │ │ -
153
│ │ │ │ -
154 size_t nrRoots() const {
│ │ │ │ -
155 return roots_.size();
│ │ │ │ -
156 }
│ │ │ │ -
157
│ │ │ │ -
│ │ │ │ -
159 const FastVector<sharedNode>& roots() const {
│ │ │ │ -
160 return roots_;
│ │ │ │ -
161 }
│ │ │ │ -
│ │ │ │ -
162
│ │ │ │ -
163 const Cluster& operator[](size_t i) const {
│ │ │ │ -
164 return *(roots_.at(i));
│ │ │ │ -
165 }
│ │ │ │ -
166
│ │ │ │ -
168
│ │ │ │ -
169 protected:
│ │ │ │ -
172
│ │ │ │ -
175 This& operator=(const This& other);
│ │ │ │ -
176
│ │ │ │ -
178};
│ │ │ │ -
│ │ │ │ -
179
│ │ │ │ -
183template <class BAYESTREE, class GRAPH>
│ │ │ │ -
│ │ │ │ -
184class EliminatableClusterTree : public ClusterTree<GRAPH> {
│ │ │ │ -
185 public:
│ │ │ │ -
186 typedef BAYESTREE BayesTreeType;
│ │ │ │ -
187 typedef GRAPH FactorGraphType;
│ │ │ │ -
188 typedef EliminatableClusterTree<BAYESTREE, GRAPH> This;
│ │ │ │ -
189 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
190
│ │ │ │ -
191 typedef typename BAYESTREE::ConditionalType ConditionalType;
│ │ │ │ -
192 typedef boost::shared_ptr<ConditionalType>
│ │ │ │ -
193 sharedConditional;
│ │ │ │ -
194
│ │ │ │ -
195 typedef typename GRAPH::Eliminate Eliminate;
│ │ │ │ -
196 typedef typename GRAPH::FactorType FactorType;
│ │ │ │ -
197 typedef boost::shared_ptr<FactorType> sharedFactor;
│ │ │ │ -
198
│ │ │ │ -
199 protected:
│ │ │ │ -
200 FastVector<sharedFactor> remainingFactors_;
│ │ │ │ -
201
│ │ │ │ -
204
│ │ │ │ -
│ │ │ │ -
207 EliminatableClusterTree(const This& other) : ClusterTree<GRAPH>(other) {
│ │ │ │ -
208 *this = other;
│ │ │ │ -
209 }
│ │ │ │ -
│ │ │ │ -
210
│ │ │ │ -
212
│ │ │ │ -
213 public:
│ │ │ │ -
216
│ │ │ │ -
222 std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> > eliminate(
│ │ │ │ -
223 const Eliminate& function) const;
│ │ │ │ -
224
│ │ │ │ -
226
│ │ │ │ -
229
│ │ │ │ -
│ │ │ │ -
231 const FastVector<sharedFactor>& remainingFactors() const {
│ │ │ │ -
232 return remainingFactors_;
│ │ │ │ -
233 }
│ │ │ │ -
│ │ │ │ -
234
│ │ │ │ -
236
│ │ │ │ -
237 protected:
│ │ │ │ -
240
│ │ │ │ -
243 This& operator=(const This& other);
│ │ │ │ -
244
│ │ │ │ -
246 EliminatableClusterTree() {}
│ │ │ │ -
247
│ │ │ │ -
249};
│ │ │ │ -
│ │ │ │ -
250}
│ │ │ │ -
251
│ │ │ │ -
252#include <gtsam/inference/ClusterTree-inst.h>
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ -
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ -
Ordering.h
Variable ordering for the elimination algorithm.
│ │ │ │ -
ClusterTree-inst.h
Collects factorgraph fragments defined on variable clusters, arranged in a tree.
│ │ │ │ -
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │ +
112#include <gtsam/inference/graph-inl.h>
│ │ │ │ +
Values.h
A non-templated config holding any types of Manifold-group elements.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::EliminatableClusterTree
A cluster-tree that eliminates to a Bayes tree.
Definition ClusterTree.h:184
│ │ │ │ -
gtsam::EliminatableClusterTree::This
EliminatableClusterTree< BAYESTREE, GRAPH > This
This class.
Definition ClusterTree.h:188
│ │ │ │ -
gtsam::EliminatableClusterTree::EliminatableClusterTree
EliminatableClusterTree()
Default constructor to be used in derived classes.
Definition ClusterTree.h:246
│ │ │ │ -
gtsam::EliminatableClusterTree::ConditionalType
BAYESTREE::ConditionalType ConditionalType
The type of conditionals.
Definition ClusterTree.h:191
│ │ │ │ -
gtsam::EliminatableClusterTree::operator=
This & operator=(const This &other)
Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are copie...
Definition ClusterTree-inst.h:231
│ │ │ │ -
gtsam::EliminatableClusterTree::eliminate
std::pair< boost::shared_ptr< BayesTreeType >, boost::shared_ptr< FactorGraphType > > eliminate(const Eliminate &function) const
Eliminate the factors to a Bayes tree and remaining factor graph.
Definition ClusterTree-inst.h:245
│ │ │ │ -
gtsam::EliminatableClusterTree::sharedConditional
boost::shared_ptr< ConditionalType > sharedConditional
Shared pointer to a conditional.
Definition ClusterTree.h:193
│ │ │ │ -
gtsam::EliminatableClusterTree::sharedFactor
boost::shared_ptr< FactorType > sharedFactor
Shared pointer to a factor.
Definition ClusterTree.h:197
│ │ │ │ -
gtsam::EliminatableClusterTree::remainingFactors
const FastVector< sharedFactor > & remainingFactors() const
Return the remaining factors that are not pulled into elimination.
Definition ClusterTree.h:231
│ │ │ │ -
gtsam::EliminatableClusterTree::EliminatableClusterTree
EliminatableClusterTree(const This &other)
Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are copied,...
Definition ClusterTree.h:207
│ │ │ │ -
gtsam::EliminatableClusterTree::BayesTreeType
BAYESTREE BayesTreeType
The BayesTree type produced by elimination.
Definition ClusterTree.h:186
│ │ │ │ -
gtsam::EliminatableClusterTree::FactorType
GRAPH::FactorType FactorType
The type of factors.
Definition ClusterTree.h:196
│ │ │ │ -
gtsam::EliminatableClusterTree::FactorGraphType
GRAPH FactorGraphType
The factor graph type.
Definition ClusterTree.h:187
│ │ │ │ -
gtsam::EliminatableClusterTree::shared_ptr
boost::shared_ptr< This > shared_ptr
Shared pointer to this class.
Definition ClusterTree.h:189
│ │ │ │ -
gtsam::EliminatableClusterTree::Eliminate
GRAPH::Eliminate Eliminate
Typedef for an eliminate subroutine.
Definition ClusterTree.h:195
│ │ │ │ -
gtsam::ClusterTree
A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman: each node k re...
Definition ClusterTree.h:25
│ │ │ │ -
gtsam::ClusterTree::operator=
This & operator=(const This &other)
Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are copie...
Definition ClusterTree-inst.h:104
│ │ │ │ -
gtsam::ClusterTree::This
ClusterTree< GRAPH > This
This class.
Definition ClusterTree.h:28
│ │ │ │ -
gtsam::ClusterTree::shared_ptr
boost::shared_ptr< This > shared_ptr
Shared pointer to this class.
Definition ClusterTree.h:29
│ │ │ │ -
gtsam::ClusterTree::roots_
FastVector< sharedNode > roots_
concept check
Definition ClusterTree.h:116
│ │ │ │ -
gtsam::ClusterTree::FactorType
GRAPH::FactorType FactorType
The type of factors.
Definition ClusterTree.h:31
│ │ │ │ -
gtsam::ClusterTree::FactorGraphType
GRAPH FactorGraphType
The factor graph type.
Definition ClusterTree.h:27
│ │ │ │ -
gtsam::ClusterTree::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Print the cluster tree.
Definition ClusterTree-inst.h:98
│ │ │ │ -
gtsam::ClusterTree::sharedFactor
boost::shared_ptr< FactorType > sharedFactor
Shared pointer to a factor.
Definition ClusterTree.h:32
│ │ │ │ -
gtsam::ClusterTree::roots
const FastVector< sharedNode > & roots() const
Return the set of roots (one for a tree, multiple for a forest)
Definition ClusterTree.h:159
│ │ │ │ -
gtsam::ClusterTree::ClusterTree
ClusterTree(const This &other)
Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are copied,...
Definition ClusterTree.h:123
│ │ │ │ -
gtsam::ClusterTree::ClusterTree
ClusterTree()
Default constructor.
Definition ClusterTree.h:132
│ │ │ │ -
gtsam::ClusterTree::sharedCluster
boost::shared_ptr< Cluster > sharedCluster
Shared pointer to Cluster.
Definition ClusterTree.h:106
│ │ │ │ -
gtsam::ClusterTree::Cluster
A Cluster is just a collection of factors.
Definition ClusterTree.h:36
│ │ │ │ -
gtsam::ClusterTree::Cluster::Cluster
Cluster(Key key, const CONTAINER &factorsToAdd)
Construct from factors associated with a single key.
Definition ClusterTree.h:57
│ │ │ │ -
gtsam::ClusterTree::Cluster::children
Children children
sub-trees
Definition ClusterTree.h:38
│ │ │ │ -
gtsam::ClusterTree::Cluster::print
virtual void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print this node
Definition ClusterTree-inst.h:26
│ │ │ │ -
gtsam::ClusterTree::Cluster::orderedFrontalKeys
Keys orderedFrontalKeys
Frontal keys of this node.
Definition ClusterTree.h:41
│ │ │ │ -
gtsam::ClusterTree::Cluster::mergeChildren
void mergeChildren(const std::vector< bool > &merge)
Merge all children for which bit is set into this node.
Definition ClusterTree-inst.h:56
│ │ │ │ -
gtsam::ClusterTree::Cluster::merge
void merge(const boost::shared_ptr< Cluster > &cluster)
Merge in given cluster.
Definition ClusterTree-inst.h:44
│ │ │ │ -
gtsam::ClusterTree::Cluster::nrFrontalsOfChildren
std::vector< size_t > nrFrontalsOfChildren() const
Return a vector with nrFrontal keys for each child.
Definition ClusterTree-inst.h:34
│ │ │ │ -
gtsam::ClusterTree::Cluster::factors
FactorGraphType factors
Factors associated with this node.
Definition ClusterTree.h:43
│ │ │ │ -
gtsam::ClusterTree::Cluster::addChild
void addChild(const boost::shared_ptr< Cluster > &cluster)
Add a child cluster.
Definition ClusterTree.h:71
│ │ │ │ -
gtsam::ClusterTree::Cluster::addFactors
void addFactors(Key key, const CONTAINER &factorsToAdd)
Add factors associated with a single key.
Definition ClusterTree.h:64
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
gtsam::predecessorMap2Keys
std::list< KEY > predecessorMap2Keys(const PredecessorMap< KEY > &p_map)
Generate a list of keys from a spanning tree represented by its predecessor map.
Definition graph-inl.h:50
│ │ │ │ +
gtsam::split
void split(const G &g, const PredecessorMap< KEY > &tree, G &Ab1, G &Ab2)
Split the graph into two parts: one corresponds to the given spanning tree, and the other corresponds...
Definition graph-inl.h:255
│ │ │ │ +
gtsam::composePoses
boost::shared_ptr< Values > composePoses(const G &graph, const PredecessorMap< KEY > &tree, const POSE &rootPose)
Compose the poses by following the chain specified by the spanning tree.
Definition graph-inl.h:174
│ │ │ │ +
gtsam::toBoostGraph
SDGraph< KEY > toBoostGraph(const G &graph)
Convert the factor graph to an SDGraph G = Graph type F = Factor type Key = Key type.
Definition graph-inl.h:68
│ │ │ │ +
gtsam::findMinimumSpanningTree
PredecessorMap< KEY > findMinimumSpanningTree(const G &fg)
find the minimum spanning tree using boost graph library
Definition graph-inl.h:232
│ │ │ │ +
gtsam::predecessorMap2Graph
boost::tuple< G, V, std::map< KEY, V > > predecessorMap2Graph(const PredecessorMap< KEY > &p_map)
Build takes a predecessor map, and builds a directed graph corresponding to the tree.
Definition graph-inl.h:118
│ │ │ │ +
gtsam::SDGraph
SDGraph is undirected graph with variable keys and double edge weights.
Definition graph.h:40
│ │ │ │ +
gtsam::SGraph
Definition graph.h:47
│ │ │ │ +
gtsam::PredecessorMap
Map from variable key to parent key.
Definition graph.h:58
│ │ │ │ +
gtsam::PredecessorMap::insert
void insert(const KEY &key, const KEY &parent)
convenience insert so we can pass ints for TypedSymbol keys
Definition graph.h:61
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,384 +1,142 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ClusterTree.h │ │ │ │ │ +graph.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -1 │ │ │ │ │ -10#pragma once │ │ │ │ │ +1/* --------------------------------------------------------------------------- │ │ │ │ │ +- │ │ │ │ │ +2 │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +5 * All Rights Reserved │ │ │ │ │ +6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +7 │ │ │ │ │ +8 * See LICENSE for the license information │ │ │ │ │ +9 │ │ │ │ │ +10 * ------------------------------------------------------------------------- │ │ │ │ │ +- */ │ │ │ │ │ 11 │ │ │ │ │ -12#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ -13#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ -14#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_O_r_d_e_r_i_n_g_._h> │ │ │ │ │ -15 │ │ │ │ │ -16namespace _g_t_s_a_m { │ │ │ │ │ -17 │ │ │ │ │ -24template │ │ │ │ │ -_2_5class _C_l_u_s_t_e_r_T_r_e_e { │ │ │ │ │ -26 public: │ │ │ │ │ -_2_7 typedef GRAPH _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e; │ │ │ │ │ -_2_8 typedef _C_l_u_s_t_e_r_T_r_e_e_<_G_R_A_P_H_> _T_h_i_s; │ │ │ │ │ -_2_9 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -30 │ │ │ │ │ -_3_1 typedef typename GRAPH::FactorType _F_a_c_t_o_r_T_y_p_e; │ │ │ │ │ -_3_2 typedef boost::shared_ptr _s_h_a_r_e_d_F_a_c_t_o_r; │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include │ │ │ │ │ +22 │ │ │ │ │ +23#define BOOST_NO_HASH // to pacify the warnings about depricated headers in │ │ │ │ │ +boost.graph │ │ │ │ │ +24 │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_V_a_l_u_e_s_._h> │ │ │ │ │ +29 │ │ │ │ │ +30namespace _g_t_s_a_m { │ │ │ │ │ +31 │ │ │ │ │ +32 // type definitions : │ │ │ │ │ 33 │ │ │ │ │ -35 // TODO(frank): re-factor JunctionTree so we can make members private │ │ │ │ │ -_3_6 struct _C_l_u_s_t_e_r { │ │ │ │ │ -37 typedef _F_a_s_t_V_e_c_t_o_r_<_b_o_o_s_t_:_:_s_h_a_r_e_d___p_t_r_<_C_l_u_s_t_e_r_> > Children; │ │ │ │ │ -_3_8 Children _c_h_i_l_d_r_e_n; │ │ │ │ │ -39 │ │ │ │ │ -40 typedef _O_r_d_e_r_i_n_g _K_e_y_s; │ │ │ │ │ -_4_1 _K_e_y_s _o_r_d_e_r_e_d_F_r_o_n_t_a_l_K_e_y_s; │ │ │ │ │ -42 │ │ │ │ │ -_4_3 _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e _f_a_c_t_o_r_s; │ │ │ │ │ +37 template │ │ │ │ │ +_3_8 class _S_D_G_r_a_p_h: public boost::adjacency_list, boost::property< │ │ │ │ │ +40 boost::edge_weight_t, double> > { │ │ │ │ │ +41 public: │ │ │ │ │ +42 typedef typename boost::graph_traits >::vertex_descriptor │ │ │ │ │ +Vertex; │ │ │ │ │ +43 }; │ │ │ │ │ 44 │ │ │ │ │ -45 int problemSize_; │ │ │ │ │ -46 │ │ │ │ │ -47 _C_l_u_s_t_e_r() : problemSize_(0) {} │ │ │ │ │ -48 │ │ │ │ │ -49 virtual _~_C_l_u_s_t_e_r() {} │ │ │ │ │ -50 │ │ │ │ │ -51 const Cluster& operator[](size_t i) const { │ │ │ │ │ -52 return *(_c_h_i_l_d_r_e_n.at(i)); │ │ │ │ │ -53 } │ │ │ │ │ -54 │ │ │ │ │ -56 template │ │ │ │ │ -_5_7 _C_l_u_s_t_e_r(_K_e_y key, const CONTAINER& factorsToAdd) │ │ │ │ │ -58 : problemSize_(0) { │ │ │ │ │ -59 _a_d_d_F_a_c_t_o_r_s(key, factorsToAdd); │ │ │ │ │ -60 } │ │ │ │ │ -61 │ │ │ │ │ -63 template │ │ │ │ │ -_6_4 void _a_d_d_F_a_c_t_o_r_s(_K_e_y key, const CONTAINER& factorsToAdd) { │ │ │ │ │ -65 _o_r_d_e_r_e_d_F_r_o_n_t_a_l_K_e_y_s.push_back(key); │ │ │ │ │ -66 _f_a_c_t_o_r_s.push_back(factorsToAdd); │ │ │ │ │ -67 problemSize_ += _f_a_c_t_o_r_s.size(); │ │ │ │ │ -68 } │ │ │ │ │ -69 │ │ │ │ │ -_7_1 void _a_d_d_C_h_i_l_d(const boost::shared_ptr& cluster) { │ │ │ │ │ -72 _c_h_i_l_d_r_e_n.push_back(cluster); │ │ │ │ │ -73 problemSize_ = std::max(problemSize_, cluster->problemSize_); │ │ │ │ │ -74 } │ │ │ │ │ -75 │ │ │ │ │ -76 size_t nrChildren() const { │ │ │ │ │ -77 return _c_h_i_l_d_r_e_n.size(); │ │ │ │ │ -78 } │ │ │ │ │ +45 template │ │ │ │ │ +_4_6 class _S_G_r_a_p_h : public boost::adjacency_list > { │ │ │ │ │ +48 public: │ │ │ │ │ +49 typedef typename boost::graph_traits >::vertex_descriptor │ │ │ │ │ +Vertex; │ │ │ │ │ +50 }; │ │ │ │ │ +51 │ │ │ │ │ +52 //typedef boost::graph_traits::vertex_descriptor SVertex; │ │ │ │ │ +53 │ │ │ │ │ +57 template │ │ │ │ │ +_5_8 class _P_r_e_d_e_c_e_s_s_o_r_M_a_p: public std::map { │ │ │ │ │ +59 public: │ │ │ │ │ +_6_1 inline void _i_n_s_e_r_t(const KEY& key, const KEY& parent) { │ │ │ │ │ +62 std::map::insert(std::make_pair(key, parent)); │ │ │ │ │ +63 } │ │ │ │ │ +64 }; │ │ │ │ │ +65 │ │ │ │ │ +69 template │ │ │ │ │ +70 std::list _p_r_e_d_e_c_e_s_s_o_r_M_a_p_2_K_e_y_s(const PredecessorMap& p_map); │ │ │ │ │ +71 │ │ │ │ │ +78 template SDGraph _t_o_B_o_o_s_t_G_r_a_p_h(const G& │ │ │ │ │ +graph); │ │ │ │ │ 79 │ │ │ │ │ -80 size_t nrFactors() const { │ │ │ │ │ -81 return _f_a_c_t_o_r_s.size(); │ │ │ │ │ -82 } │ │ │ │ │ -83 │ │ │ │ │ -84 size_t nrFrontals() const { │ │ │ │ │ -85 return _o_r_d_e_r_e_d_F_r_o_n_t_a_l_K_e_y_s.size(); │ │ │ │ │ -86 } │ │ │ │ │ +85 template │ │ │ │ │ +86 boost::tuple > _p_r_e_d_e_c_e_s_s_o_r_M_a_p_2_G_r_a_p_h(const │ │ │ │ │ +PredecessorMap& p_map); │ │ │ │ │ 87 │ │ │ │ │ -88 int problemSize() const { │ │ │ │ │ -89 return problemSize_; │ │ │ │ │ -90 } │ │ │ │ │ -91 │ │ │ │ │ -93 virtual void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ -94 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const; │ │ │ │ │ +91 template │ │ │ │ │ +92 boost::shared_ptr │ │ │ │ │ +93 _c_o_m_p_o_s_e_P_o_s_e_s(const G& graph, const PredecessorMap& tree, const POSE& │ │ │ │ │ +rootPose); │ │ │ │ │ +94 │ │ │ │ │ 95 │ │ │ │ │ -97 std::vector _n_r_F_r_o_n_t_a_l_s_O_f_C_h_i_l_d_r_e_n() const; │ │ │ │ │ -98 │ │ │ │ │ -100 void _m_e_r_g_e(const boost::shared_ptr& cluster); │ │ │ │ │ +99 template │ │ │ │ │ +100 PredecessorMap _f_i_n_d_M_i_n_i_m_u_m_S_p_a_n_n_i_n_g_T_r_e_e(const G& g) ; │ │ │ │ │ 101 │ │ │ │ │ -103 void _m_e_r_g_e_C_h_i_l_d_r_e_n(const std::vector& _m_e_r_g_e); │ │ │ │ │ -104 }; │ │ │ │ │ -105 │ │ │ │ │ -_1_0_6 typedef boost::shared_ptr _s_h_a_r_e_d_C_l_u_s_t_e_r; │ │ │ │ │ -107 │ │ │ │ │ -108 // Define Node=Cluster for compatibility with tree traversal functions │ │ │ │ │ -109 typedef _C_l_u_s_t_e_r _N_o_d_e; │ │ │ │ │ -110 typedef _s_h_a_r_e_d_C_l_u_s_t_e_r sharedNode; │ │ │ │ │ +106 template │ │ │ │ │ +107 void _s_p_l_i_t(const G& g, const PredecessorMap& tree, G& Ab1, G& Ab2) ; │ │ │ │ │ +108 │ │ │ │ │ +109 │ │ │ │ │ +110} // namespace gtsam │ │ │ │ │ 111 │ │ │ │ │ -113 GTSAM_CONCEPT_TESTABLE_TYPE(_F_a_c_t_o_r_T_y_p_e) │ │ │ │ │ -114 │ │ │ │ │ -115 protected: │ │ │ │ │ -_1_1_6 _F_a_s_t_V_e_c_t_o_r_<_s_h_a_r_e_d_N_o_d_e_> _r_o_o_t_s__; │ │ │ │ │ -117 │ │ │ │ │ -120 │ │ │ │ │ -_1_2_3 _C_l_u_s_t_e_r_T_r_e_e(const _T_h_i_s& other) { │ │ │ │ │ -124 *this = other; │ │ │ │ │ -125 } │ │ │ │ │ -126 │ │ │ │ │ -128 │ │ │ │ │ -129 public: │ │ │ │ │ -130 │ │ │ │ │ -_1_3_2 _C_l_u_s_t_e_r_T_r_e_e() {} │ │ │ │ │ -133 │ │ │ │ │ -136 │ │ │ │ │ -138 void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ -139 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const; │ │ │ │ │ -140 │ │ │ │ │ -144 │ │ │ │ │ -145 void addRoot(const boost::shared_ptr& cluster) { │ │ │ │ │ -146 _r_o_o_t_s__.push_back(cluster); │ │ │ │ │ -147 } │ │ │ │ │ -148 │ │ │ │ │ -149 void addChildrenAsRoots(const boost::shared_ptr& cluster) { │ │ │ │ │ -150 for (auto child : cluster->children) │ │ │ │ │ -151 this->addRoot(child); │ │ │ │ │ -152 } │ │ │ │ │ -153 │ │ │ │ │ -154 size_t nrRoots() const { │ │ │ │ │ -155 return _r_o_o_t_s__.size(); │ │ │ │ │ -156 } │ │ │ │ │ -157 │ │ │ │ │ -_1_5_9 const _F_a_s_t_V_e_c_t_o_r_<_s_h_a_r_e_d_N_o_d_e_>& _r_o_o_t_s() const { │ │ │ │ │ -160 return _r_o_o_t_s__; │ │ │ │ │ -161 } │ │ │ │ │ -162 │ │ │ │ │ -163 const Cluster& operator[](size_t i) const { │ │ │ │ │ -164 return *(_r_o_o_t_s__.at(i)); │ │ │ │ │ -165 } │ │ │ │ │ -166 │ │ │ │ │ -168 │ │ │ │ │ -169 protected: │ │ │ │ │ -172 │ │ │ │ │ -175 _T_h_i_s& _o_p_e_r_a_t_o_r_=(const _T_h_i_s& other); │ │ │ │ │ -176 │ │ │ │ │ -178}; │ │ │ │ │ -179 │ │ │ │ │ -183template │ │ │ │ │ -_1_8_4class _E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e : public _C_l_u_s_t_e_r_T_r_e_e { │ │ │ │ │ -185 public: │ │ │ │ │ -_1_8_6 typedef BAYESTREE _B_a_y_e_s_T_r_e_e_T_y_p_e; │ │ │ │ │ -_1_8_7 typedef GRAPH _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e; │ │ │ │ │ -_1_8_8 typedef _E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_<_B_A_Y_E_S_T_R_E_E_,_ _G_R_A_P_H_> _T_h_i_s; │ │ │ │ │ -_1_8_9 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -190 │ │ │ │ │ -_1_9_1 typedef typename BAYESTREE::ConditionalType _C_o_n_d_i_t_i_o_n_a_l_T_y_p_e; │ │ │ │ │ -192 typedef boost::shared_ptr │ │ │ │ │ -_1_9_3 _s_h_a_r_e_d_C_o_n_d_i_t_i_o_n_a_l; │ │ │ │ │ -194 │ │ │ │ │ -_1_9_5 typedef typename GRAPH::Eliminate _E_l_i_m_i_n_a_t_e; │ │ │ │ │ -_1_9_6 typedef typename GRAPH::FactorType _F_a_c_t_o_r_T_y_p_e; │ │ │ │ │ -_1_9_7 typedef boost::shared_ptr _s_h_a_r_e_d_F_a_c_t_o_r; │ │ │ │ │ -198 │ │ │ │ │ -199 protected: │ │ │ │ │ -200 _F_a_s_t_V_e_c_t_o_r_<_s_h_a_r_e_d_F_a_c_t_o_r_> remainingFactors_; │ │ │ │ │ -201 │ │ │ │ │ -204 │ │ │ │ │ -_2_0_7 _E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e(const _T_h_i_s& other) : _C_l_u_s_t_e_r_T_r_e_e(other) { │ │ │ │ │ -208 *this = other; │ │ │ │ │ -209 } │ │ │ │ │ -210 │ │ │ │ │ -212 │ │ │ │ │ -213 public: │ │ │ │ │ -216 │ │ │ │ │ -222 std::pair, boost:: │ │ │ │ │ -shared_ptr > _e_l_i_m_i_n_a_t_e( │ │ │ │ │ -223 const _E_l_i_m_i_n_a_t_e& function) const; │ │ │ │ │ -224 │ │ │ │ │ -226 │ │ │ │ │ -229 │ │ │ │ │ -_2_3_1 const _F_a_s_t_V_e_c_t_o_r_<_s_h_a_r_e_d_F_a_c_t_o_r_>& _r_e_m_a_i_n_i_n_g_F_a_c_t_o_r_s() const { │ │ │ │ │ -232 return remainingFactors_; │ │ │ │ │ -233 } │ │ │ │ │ -234 │ │ │ │ │ -236 │ │ │ │ │ -237 protected: │ │ │ │ │ -240 │ │ │ │ │ -243 _T_h_i_s& _o_p_e_r_a_t_o_r_=(const _T_h_i_s& other); │ │ │ │ │ -244 │ │ │ │ │ -_2_4_6 _E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e() {} │ │ │ │ │ -247 │ │ │ │ │ -249}; │ │ │ │ │ -250} │ │ │ │ │ -251 │ │ │ │ │ -252#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_C_l_u_s_t_e_r_T_r_e_e_-_i_n_s_t_._h> │ │ │ │ │ -_T_e_s_t_a_b_l_e_._h │ │ │ │ │ -Concept check for values that can be used in unit tests. │ │ │ │ │ -_F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ -A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ -_O_r_d_e_r_i_n_g_._h │ │ │ │ │ -Variable ordering for the elimination algorithm. │ │ │ │ │ -_C_l_u_s_t_e_r_T_r_e_e_-_i_n_s_t_._h │ │ │ │ │ -Collects factorgraph fragments defined on variable clusters, arranged in a │ │ │ │ │ -tree. │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ -std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ -FastVector │ │ │ │ │ -FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ -DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ +112#include │ │ │ │ │ +_V_a_l_u_e_s_._h │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e │ │ │ │ │ -A cluster-tree that eliminates to a Bayes tree. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:184 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_T_h_i_s │ │ │ │ │ -EliminatableClusterTree< BAYESTREE, GRAPH > This │ │ │ │ │ -This class. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:188 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e │ │ │ │ │ -EliminatableClusterTree() │ │ │ │ │ -Default constructor to be used in derived classes. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:246 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_C_o_n_d_i_t_i_o_n_a_l_T_y_p_e │ │ │ │ │ -BAYESTREE::ConditionalType ConditionalType │ │ │ │ │ -The type of conditionals. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:191 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ -This & operator=(const This &other) │ │ │ │ │ -Assignment operator - makes a deep copy of the tree structure, but only │ │ │ │ │ -pointers to factors are copie... │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree-inst.h:231 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_e_l_i_m_i_n_a_t_e │ │ │ │ │ -std::pair< boost::shared_ptr< BayesTreeType >, boost::shared_ptr< │ │ │ │ │ -FactorGraphType > > eliminate(const Eliminate &function) const │ │ │ │ │ -Eliminate the factors to a Bayes tree and remaining factor graph. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree-inst.h:245 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_s_h_a_r_e_d_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -boost::shared_ptr< ConditionalType > sharedConditional │ │ │ │ │ -Shared pointer to a conditional. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:193 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_s_h_a_r_e_d_F_a_c_t_o_r │ │ │ │ │ -boost::shared_ptr< FactorType > sharedFactor │ │ │ │ │ -Shared pointer to a factor. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:197 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_r_e_m_a_i_n_i_n_g_F_a_c_t_o_r_s │ │ │ │ │ -const FastVector< sharedFactor > & remainingFactors() const │ │ │ │ │ -Return the remaining factors that are not pulled into elimination. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:231 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e │ │ │ │ │ -EliminatableClusterTree(const This &other) │ │ │ │ │ -Copy constructor - makes a deep copy of the tree structure, but only pointers │ │ │ │ │ -to factors are copied,... │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:207 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_B_a_y_e_s_T_r_e_e_T_y_p_e │ │ │ │ │ -BAYESTREE BayesTreeType │ │ │ │ │ -The BayesTree type produced by elimination. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:186 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_F_a_c_t_o_r_T_y_p_e │ │ │ │ │ -GRAPH::FactorType FactorType │ │ │ │ │ -The type of factors. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:196 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_F_a_c_t_o_r_G_r_a_p_h_T_y_p_e │ │ │ │ │ -GRAPH FactorGraphType │ │ │ │ │ -The factor graph type. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:187 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -Shared pointer to this class. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:189 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_E_l_i_m_i_n_a_t_e │ │ │ │ │ -GRAPH::Eliminate Eliminate │ │ │ │ │ -Typedef for an eliminate subroutine. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:195 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e │ │ │ │ │ -A cluster-tree is associated with a factor graph and is defined as in Koller- │ │ │ │ │ -Friedman: each node k re... │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:25 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ -This & operator=(const This &other) │ │ │ │ │ -Assignment operator - makes a deep copy of the tree structure, but only │ │ │ │ │ -pointers to factors are copie... │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree-inst.h:104 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_T_h_i_s │ │ │ │ │ -ClusterTree< GRAPH > This │ │ │ │ │ -This class. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -Shared pointer to this class. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:29 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_r_o_o_t_s__ │ │ │ │ │ -FastVector< sharedNode > roots_ │ │ │ │ │ -concept check │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:116 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_F_a_c_t_o_r_T_y_p_e │ │ │ │ │ -GRAPH::FactorType FactorType │ │ │ │ │ -The type of factors. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:31 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_F_a_c_t_o_r_G_r_a_p_h_T_y_p_e │ │ │ │ │ -GRAPH FactorGraphType │ │ │ │ │ -The factor graph type. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:27 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ -Print the cluster tree. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree-inst.h:98 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_s_h_a_r_e_d_F_a_c_t_o_r │ │ │ │ │ -boost::shared_ptr< FactorType > sharedFactor │ │ │ │ │ -Shared pointer to a factor. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_r_o_o_t_s │ │ │ │ │ -const FastVector< sharedNode > & roots() const │ │ │ │ │ -Return the set of roots (one for a tree, multiple for a forest) │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:159 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_T_r_e_e │ │ │ │ │ -ClusterTree(const This &other) │ │ │ │ │ -Copy constructor - makes a deep copy of the tree structure, but only pointers │ │ │ │ │ -to factors are copied,... │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:123 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_T_r_e_e │ │ │ │ │ -ClusterTree() │ │ │ │ │ -Default constructor. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:132 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_s_h_a_r_e_d_C_l_u_s_t_e_r │ │ │ │ │ -boost::shared_ptr< Cluster > sharedCluster │ │ │ │ │ -Shared pointer to Cluster. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:106 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r │ │ │ │ │ -A Cluster is just a collection of factors. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_C_l_u_s_t_e_r │ │ │ │ │ -Cluster(Key key, const CONTAINER &factorsToAdd) │ │ │ │ │ -Construct from factors associated with a single key. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_c_h_i_l_d_r_e_n │ │ │ │ │ -Children children │ │ │ │ │ -sub-trees │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_p_r_i_n_t │ │ │ │ │ -virtual void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ -print this node │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree-inst.h:26 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_o_r_d_e_r_e_d_F_r_o_n_t_a_l_K_e_y_s │ │ │ │ │ -Keys orderedFrontalKeys │ │ │ │ │ -Frontal keys of this node. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_m_e_r_g_e_C_h_i_l_d_r_e_n │ │ │ │ │ -void mergeChildren(const std::vector< bool > &merge) │ │ │ │ │ -Merge all children for which bit is set into this node. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree-inst.h:56 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_m_e_r_g_e │ │ │ │ │ -void merge(const boost::shared_ptr< Cluster > &cluster) │ │ │ │ │ -Merge in given cluster. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree-inst.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_n_r_F_r_o_n_t_a_l_s_O_f_C_h_i_l_d_r_e_n │ │ │ │ │ -std::vector< size_t > nrFrontalsOfChildren() const │ │ │ │ │ -Return a vector with nrFrontal keys for each child. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree-inst.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_f_a_c_t_o_r_s │ │ │ │ │ -FactorGraphType factors │ │ │ │ │ -Factors associated with this node. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_a_d_d_C_h_i_l_d │ │ │ │ │ -void addChild(const boost::shared_ptr< Cluster > &cluster) │ │ │ │ │ -Add a child cluster. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:71 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_a_d_d_F_a_c_t_o_r_s │ │ │ │ │ -void addFactors(Key key, const CONTAINER &factorsToAdd) │ │ │ │ │ -Add factors associated with a single key. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_e_d_e_c_e_s_s_o_r_M_a_p_2_K_e_y_s │ │ │ │ │ +std::list< KEY > predecessorMap2Keys(const PredecessorMap< KEY > &p_map) │ │ │ │ │ +Generate a list of keys from a spanning tree represented by its predecessor │ │ │ │ │ +map. │ │ │ │ │ +DDeeffiinniittiioonn graph-inl.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_s_p_l_i_t │ │ │ │ │ +void split(const G &g, const PredecessorMap< KEY > &tree, G &Ab1, G &Ab2) │ │ │ │ │ +Split the graph into two parts: one corresponds to the given spanning tree, and │ │ │ │ │ +the other corresponds... │ │ │ │ │ +DDeeffiinniittiioonn graph-inl.h:255 │ │ │ │ │ +_g_t_s_a_m_:_:_c_o_m_p_o_s_e_P_o_s_e_s │ │ │ │ │ +boost::shared_ptr< Values > composePoses(const G &graph, const PredecessorMap< │ │ │ │ │ +KEY > &tree, const POSE &rootPose) │ │ │ │ │ +Compose the poses by following the chain specified by the spanning tree. │ │ │ │ │ +DDeeffiinniittiioonn graph-inl.h:174 │ │ │ │ │ +_g_t_s_a_m_:_:_t_o_B_o_o_s_t_G_r_a_p_h │ │ │ │ │ +SDGraph< KEY > toBoostGraph(const G &graph) │ │ │ │ │ +Convert the factor graph to an SDGraph G = Graph type F = Factor type Key = Key │ │ │ │ │ +type. │ │ │ │ │ +DDeeffiinniittiioonn graph-inl.h:68 │ │ │ │ │ +_g_t_s_a_m_:_:_f_i_n_d_M_i_n_i_m_u_m_S_p_a_n_n_i_n_g_T_r_e_e │ │ │ │ │ +PredecessorMap< KEY > findMinimumSpanningTree(const G &fg) │ │ │ │ │ +find the minimum spanning tree using boost graph library │ │ │ │ │ +DDeeffiinniittiioonn graph-inl.h:232 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_e_d_e_c_e_s_s_o_r_M_a_p_2_G_r_a_p_h │ │ │ │ │ +boost::tuple< G, V, std::map< KEY, V > > predecessorMap2Graph(const │ │ │ │ │ +PredecessorMap< KEY > &p_map) │ │ │ │ │ +Build takes a predecessor map, and builds a directed graph corresponding to the │ │ │ │ │ +tree. │ │ │ │ │ +DDeeffiinniittiioonn graph-inl.h:118 │ │ │ │ │ +_g_t_s_a_m_:_:_S_D_G_r_a_p_h │ │ │ │ │ +SDGraph is undirected graph with variable keys and double edge weights. │ │ │ │ │ +DDeeffiinniittiioonn graph.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_S_G_r_a_p_h │ │ │ │ │ +DDeeffiinniittiioonn graph.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_d_e_c_e_s_s_o_r_M_a_p │ │ │ │ │ +Map from variable key to parent key. │ │ │ │ │ +DDeeffiinniittiioonn graph.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_d_e_c_e_s_s_o_r_M_a_p_:_:_i_n_s_e_r_t │ │ │ │ │ +void insert(const KEY &key, const KEY &parent) │ │ │ │ │ +convenience insert so we can pass ints for TypedSymbol keys │ │ │ │ │ +DDeeffiinniittiioonn graph.h:61 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _C_l_u_s_t_e_r_T_r_e_e_._h │ │ │ │ │ + * _g_r_a_p_h_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00638.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Factor.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTree.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,38 +95,40 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces
│ │ │ │ -
Factor.cpp File Reference
│ │ │ │ +
BayesTree.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

The base class for all factors. │ │ │ │ +

Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

The base class for all factors.

│ │ │ │ -
Author
Kai Ni
│ │ │ │ +

Bayes Tree is a tree of cliques of a Bayes Chain.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │ -Frank Dellaert
│ │ │ │ +Michael Kaess │ │ │ │ +
│ │ │ │ +Viorela Ila
│ │ │ │
│ │ │ │ Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,23 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Factor.cpp File Reference │ │ │ │ │ -The base class for all factors. _M_o_r_e_._._. │ │ │ │ │ +BayesTree.cpp File Reference │ │ │ │ │ +Bayes Tree is a tree of cliques of a Bayes Chain. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -The base class for all factors. │ │ │ │ │ +Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ │ Author │ │ │ │ │ - Kai Ni │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Michael Kaess │ │ │ │ │ + Viorela Ila │ │ │ │ │ Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _F_a_c_t_o_r_._c_p_p │ │ │ │ │ + * _B_a_y_e_s_T_r_e_e_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00644.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesNet.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTree-inst.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,46 +94,51 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
BayesNet.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
BayesTree-inst.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Bayes network. │ │ │ │ +

Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::BayesNet< CONDITIONAL >
 A BayesNet is a tree of conditionals, stored in elimination order. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +template<class CLIQUE >
bool gtsam::check_sharedCliques (const std::pair< Key, typename BayesTree< CLIQUE >::sharedClique > &v1, const std::pair< Key, typename BayesTree< CLIQUE >::sharedClique > &v2)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Bayes network.

│ │ │ │ +

Bayes Tree is a tree of cliques of a Bayes Chain.

│ │ │ │
Author
Frank Dellaert
│ │ │ │
│ │ │ │ +Michael Kaess
│ │ │ │ +
│ │ │ │ +Viorela Ila
│ │ │ │ +
│ │ │ │ Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,27 +1,30 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -BayesNet.h File Reference │ │ │ │ │ -Bayes network. _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +BayesTree-inst.h File Reference │ │ │ │ │ +Bayes Tree is a tree of cliques of a Bayes Chain. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_B_a_y_e_s_N_e_t_<_ _C_O_N_D_I_T_I_O_N_A_L_ _> │ │ │ │ │ -  A _B_a_y_e_s_N_e_t is a tree of conditionals, stored in elimination order. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +template │ │ │ │ │ +bool  ggttssaamm::::cchheecckk__sshhaarreeddCClliiqquueess (const std::pair< _K_e_y, typename _B_a_y_e_s_T_r_e_e< │ │ │ │ │ + CLIQUE >::sharedClique > &v1, const std::pair< _K_e_y, typename _B_a_y_e_s_T_r_e_e< │ │ │ │ │ + CLIQUE >::sharedClique > &v2) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Bayes network. │ │ │ │ │ +Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Michael Kaess │ │ │ │ │ + Viorela Ila │ │ │ │ │ Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _B_a_y_e_s_N_e_t_._h │ │ │ │ │ + * _B_a_y_e_s_T_r_e_e_-_i_n_s_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00644_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesNet.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTree-inst.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,115 +98,680 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
BayesNet.h
│ │ │ │ +
BayesTree-inst.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3* GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ -
4* Atlanta, Georgia 30332-0415
│ │ │ │ -
5* All Rights Reserved
│ │ │ │ -
6* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
4 * Atlanta, Georgia 30332-0415
│ │ │ │ +
5 * All Rights Reserved
│ │ │ │ +
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │ -
8* See LICENSE for the license information
│ │ │ │ +
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │ -
10* -------------------------------------------------------------------------- */
│ │ │ │ +
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/inference/FactorGraph.h>
│ │ │ │ +
21#pragma once
│ │ │ │
22
│ │ │ │ -
23#include <boost/shared_ptr.hpp>
│ │ │ │ -
24#include <string>
│ │ │ │ -
25
│ │ │ │ -
26namespace gtsam {
│ │ │ │ +
23#include <gtsam/inference/Ordering.h>
│ │ │ │ +
24#include <gtsam/inference/BayesTree.h>
│ │ │ │ +
25#include <gtsam/base/treeTraversal-inst.h>
│ │ │ │ +
26#include <gtsam/base/timing.h>
│ │ │ │
27
│ │ │ │ -
28class HybridValues;
│ │ │ │ -
29
│ │ │ │ -
34template <class CONDITIONAL>
│ │ │ │ -
│ │ │ │ -
35class BayesNet : public FactorGraph<CONDITIONAL> {
│ │ │ │ -
36 private:
│ │ │ │ -
37 typedef FactorGraph<CONDITIONAL> Base;
│ │ │ │ -
38
│ │ │ │ -
39 public:
│ │ │ │ -
40 typedef typename boost::shared_ptr<CONDITIONAL>
│ │ │ │ -
41 sharedConditional;
│ │ │ │ -
42
│ │ │ │ -
43 protected:
│ │ │ │ -
46
│ │ │ │ -
48 BayesNet() {}
│ │ │ │ -
49
│ │ │ │ -
51 template <typename ITERATOR>
│ │ │ │ -
│ │ │ │ -
52 BayesNet(ITERATOR firstConditional, ITERATOR lastConditional)
│ │ │ │ -
53 : Base(firstConditional, lastConditional) {}
│ │ │ │ -
│ │ │ │ -
54
│ │ │ │ -
│ │ │ │ -
60 BayesNet(std::initializer_list<sharedConditional> conditionals)
│ │ │ │ -
61 : Base(conditionals) {}
│ │ │ │ -
│ │ │ │ -
62
│ │ │ │ -
64
│ │ │ │ -
65 public:
│ │ │ │ -
68
│ │ │ │ -
70 void print(
│ │ │ │ -
71 const std::string& s = "BayesNet",
│ │ │ │ -
72 const KeyFormatter& formatter = DefaultKeyFormatter) const override;
│ │ │ │ -
73
│ │ │ │ -
77
│ │ │ │ -
79 void dot(std::ostream& os,
│ │ │ │ -
80 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
81 const DotWriter& writer = DotWriter()) const;
│ │ │ │ +
28#include <boost/optional.hpp>
│ │ │ │ +
29#include <fstream>
│ │ │ │ +
30
│ │ │ │ +
31namespace gtsam {
│ │ │ │ +
32
│ │ │ │ +
33 /* ************************************************************************* */
│ │ │ │ +
34 template<class CLIQUE>
│ │ │ │ +
│ │ │ │ +
35 BayesTreeCliqueData BayesTree<CLIQUE>::getCliqueData() const {
│ │ │ │ +
36 BayesTreeCliqueData stats;
│ │ │ │ +
37 for (const sharedClique& root : roots_) getCliqueData(root, &stats);
│ │ │ │ +
38 return stats;
│ │ │ │ +
39 }
│ │ │ │ +
│ │ │ │ +
40
│ │ │ │ +
41 /* ************************************************************************* */
│ │ │ │ +
42 template <class CLIQUE>
│ │ │ │ +
│ │ │ │ +
43 void BayesTree<CLIQUE>::getCliqueData(sharedClique clique,
│ │ │ │ +
44 BayesTreeCliqueData* stats) const {
│ │ │ │ +
45 const auto conditional = clique->conditional();
│ │ │ │ +
46 stats->conditionalSizes.push_back(conditional->nrFrontals());
│ │ │ │ +
47 stats->separatorSizes.push_back(conditional->nrParents());
│ │ │ │ +
48 for (sharedClique c : clique->children) {
│ │ │ │ +
49 getCliqueData(c, stats);
│ │ │ │ +
50 }
│ │ │ │ +
51 }
│ │ │ │ +
│ │ │ │ +
52
│ │ │ │ +
53 /* ************************************************************************* */
│ │ │ │ +
54 template<class CLIQUE>
│ │ │ │ +
│ │ │ │ +
55 size_t BayesTree<CLIQUE>::numCachedSeparatorMarginals() const {
│ │ │ │ +
56 size_t count = 0;
│ │ │ │ +
57 for(const sharedClique& root: roots_)
│ │ │ │ +
58 count += root->numCachedSeparatorMarginals();
│ │ │ │ +
59 return count;
│ │ │ │ +
60 }
│ │ │ │ +
│ │ │ │ +
61
│ │ │ │ +
62 /* ************************************************************************* */
│ │ │ │ +
63 template <class CLIQUE>
│ │ │ │ +
│ │ │ │ +
64 void BayesTree<CLIQUE>::dot(std::ostream& os,
│ │ │ │ +
65 const KeyFormatter& keyFormatter) const {
│ │ │ │ +
66 if (roots_.empty())
│ │ │ │ +
67 throw std::invalid_argument(
│ │ │ │ +
68 "the root of Bayes tree has not been initialized!");
│ │ │ │ +
69 os << "digraph G{\n";
│ │ │ │ +
70 for (const sharedClique& root : roots_) dot(os, root, keyFormatter);
│ │ │ │ +
71 os << "}";
│ │ │ │ +
72 std::flush(os);
│ │ │ │ +
73 }
│ │ │ │ +
│ │ │ │ +
74
│ │ │ │ +
75 /* ************************************************************************* */
│ │ │ │ +
76 template <class CLIQUE>
│ │ │ │ +
│ │ │ │ +
77 std::string BayesTree<CLIQUE>::dot(const KeyFormatter& keyFormatter) const {
│ │ │ │ +
78 std::stringstream ss;
│ │ │ │ +
79 dot(ss, keyFormatter);
│ │ │ │ +
80 return ss.str();
│ │ │ │ +
81 }
│ │ │ │ +
│ │ │ │
82
│ │ │ │ -
84 std::string dot(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
85 const DotWriter& writer = DotWriter()) const;
│ │ │ │ -
86
│ │ │ │ -
88 void saveGraph(const std::string& filename,
│ │ │ │ -
89 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
90 const DotWriter& writer = DotWriter()) const;
│ │ │ │ +
83 /* ************************************************************************* */
│ │ │ │ +
84 template <class CLIQUE>
│ │ │ │ +
│ │ │ │ +
85 void BayesTree<CLIQUE>::saveGraph(const std::string& filename,
│ │ │ │ +
86 const KeyFormatter& keyFormatter) const {
│ │ │ │ +
87 std::ofstream of(filename.c_str());
│ │ │ │ +
88 dot(of, keyFormatter);
│ │ │ │ +
89 of.close();
│ │ │ │ +
90 }
│ │ │ │ +
│ │ │ │
91
│ │ │ │ -
95
│ │ │ │ -
96 // Expose HybridValues version of logProbability.
│ │ │ │ -
97 double logProbability(const HybridValues& x) const;
│ │ │ │ -
98
│ │ │ │ -
99 // Expose HybridValues version of evaluate.
│ │ │ │ -
100 double evaluate(const HybridValues& c) const;
│ │ │ │ -
101
│ │ │ │ -
103};
│ │ │ │ -
│ │ │ │ -
104
│ │ │ │ -
105} // namespace gtsam
│ │ │ │ -
106
│ │ │ │ -
107#include <gtsam/inference/BayesNet-inst.h>
│ │ │ │ -
FactorGraph.h
Factor Graph Base Class.
│ │ │ │ +
92 /* ************************************************************************* */
│ │ │ │ +
93 template <class CLIQUE>
│ │ │ │ +
│ │ │ │ +
94 void BayesTree<CLIQUE>::dot(std::ostream& s, sharedClique clique,
│ │ │ │ +
95 const KeyFormatter& keyFormatter,
│ │ │ │ +
96 int parentnum) const {
│ │ │ │ +
97 static int num = 0;
│ │ │ │ +
98 bool first = true;
│ │ │ │ +
99 std::stringstream out;
│ │ │ │ +
100 out << num;
│ │ │ │ +
101 std::string parent = out.str();
│ │ │ │ +
102 parent += "[label=\"";
│ │ │ │ +
103
│ │ │ │ +
104 for (Key key : clique->conditional_->frontals()) {
│ │ │ │ +
105 if (!first) parent += ", ";
│ │ │ │ +
106 first = false;
│ │ │ │ +
107 parent += keyFormatter(key);
│ │ │ │ +
108 }
│ │ │ │ +
109
│ │ │ │ +
110 if (clique->parent()) {
│ │ │ │ +
111 parent += " : ";
│ │ │ │ +
│ │ │ │ +
112 s << parentnum << "->" << num << "\n";
│ │ │ │ +
113 }
│ │ │ │ +
114
│ │ │ │ +
115 first = true;
│ │ │ │ +
116 for (Key parentKey : clique->conditional_->parents()) {
│ │ │ │ +
│ │ │ │ +
117 if (!first) parent += ", ";
│ │ │ │ +
118 first = false;
│ │ │ │ +
119 parent += keyFormatter(parentKey);
│ │ │ │ +
120 }
│ │ │ │ +
121 parent += "\"];\n";
│ │ │ │ +
122 s << parent;
│ │ │ │ +
│ │ │ │ +
123 parentnum = num;
│ │ │ │ +
124
│ │ │ │ +
125 for (sharedClique c : clique->children) {
│ │ │ │ +
126 num++;
│ │ │ │ +
│ │ │ │ +
127 dot(s, c, keyFormatter, parentnum);
│ │ │ │ +
128 }
│ │ │ │ +
129 }
│ │ │ │ +
130
│ │ │ │ +
131 /* ************************************************************************* */
│ │ │ │ +
132 template<class CLIQUE>
│ │ │ │ +
│ │ │ │ +
133 size_t BayesTree<CLIQUE>::size() const {
│ │ │ │ +
134 size_t size = 0;
│ │ │ │ +
│ │ │ │ +
135 for(const sharedClique& clique: roots_)
│ │ │ │ +
136 size += clique->treeSize();
│ │ │ │ +
137 return size;
│ │ │ │ +
138 }
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
139
│ │ │ │ +
140 /* ************************************************************************* */
│ │ │ │ +
141 template<class CLIQUE>
│ │ │ │ +
│ │ │ │ +
142 void BayesTree<CLIQUE>::addClique(const sharedClique& clique, const sharedClique& parent_clique) {
│ │ │ │ +
143 for(Key j: clique->conditional()->frontals())
│ │ │ │ +
144 nodes_[j] = clique;
│ │ │ │ +
145 if (parent_clique != nullptr) {
│ │ │ │ +
146 clique->parent_ = parent_clique;
│ │ │ │ +
147 parent_clique->children.push_back(clique);
│ │ │ │ +
148 } else {
│ │ │ │ +
149 roots_.push_back(clique);
│ │ │ │ +
150 }
│ │ │ │ +
151 }
│ │ │ │ +
│ │ │ │ +
152
│ │ │ │ +
153 /* ************************************************************************* */
│ │ │ │ +
154 namespace {
│ │ │ │ +
155 template <class FACTOR, class CLIQUE>
│ │ │ │ +
156 struct _pushCliqueFunctor {
│ │ │ │ +
157 _pushCliqueFunctor(FactorGraph<FACTOR>* graph_) : graph(graph_) {}
│ │ │ │ +
158 FactorGraph<FACTOR>* graph;
│ │ │ │ +
159 int operator()(const boost::shared_ptr<CLIQUE>& clique, int dummy) {
│ │ │ │ +
160 graph->push_back(clique->conditional_);
│ │ │ │ +
│ │ │ │ +
161 return 0;
│ │ │ │ +
162 }
│ │ │ │ +
163 };
│ │ │ │ +
│ │ │ │ +
164 } // namespace
│ │ │ │ +
165
│ │ │ │ +
166 /* ************************************************************************* */
│ │ │ │ +
167 template <class CLIQUE>
│ │ │ │ +
│ │ │ │ +
168 void BayesTree<CLIQUE>::addFactorsToGraph(
│ │ │ │ +
169 FactorGraph<FactorType>* graph) const {
│ │ │ │ +
170 // Traverse the BayesTree and add all conditionals to this graph
│ │ │ │ +
│ │ │ │ +
171 int data = 0; // Unused
│ │ │ │ +
172 _pushCliqueFunctor<FactorType, CLIQUE> functor(graph);
│ │ │ │ +
173 treeTraversal::DepthFirstForest(*this, data, functor);
│ │ │ │ +
174 }
│ │ │ │ +
175
│ │ │ │ +
176 /* ************************************************************************* */
│ │ │ │ +
│ │ │ │ +
177 template<class CLIQUE>
│ │ │ │ +
│ │ │ │ +
178 BayesTree<CLIQUE>::BayesTree(const This& other) {
│ │ │ │ +
179 *this = other;
│ │ │ │ +
180 }
│ │ │ │ +
│ │ │ │ +
181
│ │ │ │ +
182 /* ************************************************************************* */
│ │ │ │ +
│ │ │ │ +
183 namespace {
│ │ │ │ +
184 template<typename NODE>
│ │ │ │ +
185 boost::shared_ptr<NODE>
│ │ │ │ +
186 BayesTreeCloneForestVisitorPre(const boost::shared_ptr<NODE>& node, const boost::shared_ptr<NODE>& parentPointer)
│ │ │ │ +
187 {
│ │ │ │ +
188 // Clone the current node and add it to its cloned parent
│ │ │ │ +
│ │ │ │ +
189 boost::shared_ptr<NODE> clone = boost::make_shared<NODE>(*node);
│ │ │ │ +
190 clone->children.clear();
│ │ │ │ +
191 clone->parent_ = parentPointer;
│ │ │ │ +
│ │ │ │ +
192 parentPointer->children.push_back(clone);
│ │ │ │ +
193 return clone;
│ │ │ │ +
194 }
│ │ │ │ +
195 }
│ │ │ │ +
│ │ │ │ +
196
│ │ │ │ +
197 /* ************************************************************************* */
│ │ │ │ +
198 template<class CLIQUE>
│ │ │ │ +
│ │ │ │ +
199 BayesTree<CLIQUE>& BayesTree<CLIQUE>::operator=(const This& other) {
│ │ │ │ +
200 this->clear();
│ │ │ │ +
201 boost::shared_ptr<Clique> rootContainer = boost::make_shared<Clique>();
│ │ │ │ +
202 treeTraversal::DepthFirstForest(other, rootContainer, BayesTreeCloneForestVisitorPre<Clique>);
│ │ │ │ +
203 for(const sharedClique& root: rootContainer->children) {
│ │ │ │ +
204 root->parent_ = typename Clique::weak_ptr(); // Reset the parent since it's set to the dummy clique
│ │ │ │ +
205 insertRoot(root);
│ │ │ │ +
206 }
│ │ │ │ +
207 return *this;
│ │ │ │ +
208 }
│ │ │ │ +
│ │ │ │ +
209
│ │ │ │ +
210 /* ************************************************************************* */
│ │ │ │ +
│ │ │ │ +
211 template<class CLIQUE>
│ │ │ │ +
│ │ │ │ +
212 void BayesTree<CLIQUE>::print(const std::string& s, const KeyFormatter& keyFormatter) const {
│ │ │ │ +
213 std::cout << s << ": cliques: " << size() << ", variables: " << nodes_.size() << std::endl;
│ │ │ │ +
│ │ │ │ +
214 treeTraversal::PrintForest(*this, s, keyFormatter);
│ │ │ │ +
215 }
│ │ │ │ +
216
│ │ │ │ +
217 /* ************************************************************************* */
│ │ │ │ +
218 // binary predicate to test equality of a pair for use in equals
│ │ │ │ +
219 template<class CLIQUE>
│ │ │ │ +
│ │ │ │ +
220 bool check_sharedCliques(
│ │ │ │ +
221 const std::pair<Key, typename BayesTree<CLIQUE>::sharedClique>& v1,
│ │ │ │ +
222 const std::pair<Key, typename BayesTree<CLIQUE>::sharedClique>& v2
│ │ │ │ +
223 ) {
│ │ │ │ +
224 return v1.first == v2.first &&
│ │ │ │ +
225 ((!v1.second && !v2.second) || (v1.second && v2.second && v1.second->equals(*v2.second)));
│ │ │ │ +
│ │ │ │ +
226 }
│ │ │ │ +
227
│ │ │ │ +
228 /* ************************************************************************* */
│ │ │ │ +
229 template<class CLIQUE>
│ │ │ │ +
│ │ │ │ +
230 bool BayesTree<CLIQUE>::equals(const BayesTree<CLIQUE>& other, double tol) const {
│ │ │ │ +
231 return size()==other.size() &&
│ │ │ │ +
232 std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques<CLIQUE>);
│ │ │ │ +
233 }
│ │ │ │ +
│ │ │ │ +
234
│ │ │ │ +
│ │ │ │ +
235 /* ************************************************************************* */
│ │ │ │ +
236 template<class CLIQUE>
│ │ │ │ +
237 template<class CONTAINER>
│ │ │ │ +
│ │ │ │ +
238 Key BayesTree<CLIQUE>::findParentClique(const CONTAINER& parents) const {
│ │ │ │ +
239 typename CONTAINER::const_iterator lowestOrderedParent = min_element(parents.begin(), parents.end());
│ │ │ │ +
240 assert(lowestOrderedParent != parents.end());
│ │ │ │ +
│ │ │ │ +
241 return *lowestOrderedParent;
│ │ │ │ +
242 }
│ │ │ │ +
243
│ │ │ │ +
244 /* ************************************************************************* */
│ │ │ │ +
245 template<class CLIQUE>
│ │ │ │ +
│ │ │ │ +
246 void BayesTree<CLIQUE>::fillNodesIndex(const sharedClique& subtree) {
│ │ │ │ +
247 // Add each frontal variable of this root node
│ │ │ │ +
248 for(const Key& j: subtree->conditional()->frontals()) {
│ │ │ │ +
249 bool inserted = nodes_.insert(std::make_pair(j, subtree)).second;
│ │ │ │ +
│ │ │ │ +
250 assert(inserted); (void)inserted;
│ │ │ │ +
251 }
│ │ │ │ +
252 // Fill index for each child
│ │ │ │ +
│ │ │ │ +
253 typedef typename BayesTree<CLIQUE>::sharedClique sharedClique;
│ │ │ │ +
254 for(const sharedClique& child: subtree->children) {
│ │ │ │ +
255 fillNodesIndex(child); }
│ │ │ │ +
256 }
│ │ │ │ +
257
│ │ │ │ +
258 /* ************************************************************************* */
│ │ │ │ +
259 template<class CLIQUE>
│ │ │ │ +
│ │ │ │ +
260 void BayesTree<CLIQUE>::insertRoot(const sharedClique& subtree) {
│ │ │ │ +
261 roots_.push_back(subtree); // Add to roots
│ │ │ │ +
262 fillNodesIndex(subtree); // Populate nodes index
│ │ │ │ +
263 }
│ │ │ │ +
│ │ │ │ +
264
│ │ │ │ +
265 /* ************************************************************************* */
│ │ │ │ +
266 // First finds clique marginal then marginalizes that
│ │ │ │ +
267 /* ************************************************************************* */
│ │ │ │ +
268 template<class CLIQUE>
│ │ │ │ +
269 typename BayesTree<CLIQUE>::sharedConditional
│ │ │ │ +
│ │ │ │ +
270 BayesTree<CLIQUE>::marginalFactor(Key j, const Eliminate& function) const
│ │ │ │ +
271 {
│ │ │ │ +
272 gttic(BayesTree_marginalFactor);
│ │ │ │ +
273
│ │ │ │ +
274 // get clique containing Key j
│ │ │ │ +
275 sharedClique clique = this->clique(j);
│ │ │ │ +
276
│ │ │ │ +
277 // calculate or retrieve its marginal P(C) = P(F,S)
│ │ │ │ +
278 FactorGraphType cliqueMarginal = clique->marginal2(function);
│ │ │ │ +
279
│ │ │ │ +
280 // Now, marginalize out everything that is not variable j
│ │ │ │ +
281 BayesNetType marginalBN =
│ │ │ │ +
282 *cliqueMarginal.marginalMultifrontalBayesNet(Ordering{j}, function);
│ │ │ │ +
283
│ │ │ │ +
284 // The Bayes net should contain only one conditional for variable j, so return it
│ │ │ │ +
285 return marginalBN.front();
│ │ │ │ +
286 }
│ │ │ │ +
│ │ │ │ +
287
│ │ │ │ +
288 /* ************************************************************************* */
│ │ │ │ +
289 // Find two cliques, their joint, then marginalizes
│ │ │ │ +
290 /* ************************************************************************* */
│ │ │ │ +
291 template<class CLIQUE>
│ │ │ │ +
292 typename BayesTree<CLIQUE>::sharedFactorGraph
│ │ │ │ +
│ │ │ │ +
293 BayesTree<CLIQUE>::joint(Key j1, Key j2, const Eliminate& function) const
│ │ │ │ +
294 {
│ │ │ │ +
295 gttic(BayesTree_joint);
│ │ │ │ +
296 return boost::make_shared<FactorGraphType>(*jointBayesNet(j1, j2, function));
│ │ │ │ +
297 }
│ │ │ │ +
│ │ │ │ +
298
│ │ │ │ +
299 /* ************************************************************************* */
│ │ │ │ +
300 template<class CLIQUE>
│ │ │ │ +
301 typename BayesTree<CLIQUE>::sharedBayesNet
│ │ │ │ +
│ │ │ │ +
302 BayesTree<CLIQUE>::jointBayesNet(Key j1, Key j2, const Eliminate& function) const
│ │ │ │ +
303 {
│ │ │ │ +
304 gttic(BayesTree_jointBayesNet);
│ │ │ │ +
305 // get clique C1 and C2
│ │ │ │ +
306 sharedClique C1 = (*this)[j1], C2 = (*this)[j2];
│ │ │ │ +
307
│ │ │ │ +
308 gttic(Lowest_common_ancestor);
│ │ │ │ +
309 // Find lowest common ancestor clique
│ │ │ │ +
310 sharedClique B; {
│ │ │ │ +
311 // Build two paths to the root
│ │ │ │ +
312 FastList<sharedClique> path1, path2; {
│ │ │ │ +
313 sharedClique p = C1;
│ │ │ │ +
314 while(p) {
│ │ │ │ +
315 path1.push_front(p);
│ │ │ │ +
316 p = p->parent();
│ │ │ │ +
317 }
│ │ │ │ +
318 } {
│ │ │ │ +
319 sharedClique p = C2;
│ │ │ │ +
320 while(p) {
│ │ │ │ +
321 path2.push_front(p);
│ │ │ │ +
322 p = p->parent();
│ │ │ │ +
323 }
│ │ │ │ +
324 }
│ │ │ │ +
325 // Find the path intersection
│ │ │ │ +
326 typename FastList<sharedClique>::const_iterator p1 = path1.begin(), p2 = path2.begin();
│ │ │ │ +
327 if(*p1 == *p2)
│ │ │ │ +
328 B = *p1;
│ │ │ │ +
329 while(p1 != path1.end() && p2 != path2.end() && *p1 == *p2) {
│ │ │ │ +
330 B = *p1;
│ │ │ │ +
331 ++p1;
│ │ │ │ +
332 ++p2;
│ │ │ │ +
333 }
│ │ │ │ +
334 }
│ │ │ │ +
335 gttoc(Lowest_common_ancestor);
│ │ │ │ +
336
│ │ │ │ +
337 // Build joint on all involved variables
│ │ │ │ +
338 FactorGraphType p_BC1C2;
│ │ │ │ +
339
│ │ │ │ +
340 if(B)
│ │ │ │ +
341 {
│ │ │ │ +
342 // Compute marginal on lowest common ancestor clique
│ │ │ │ +
343 gttic(LCA_marginal);
│ │ │ │ +
344 FactorGraphType p_B = B->marginal2(function);
│ │ │ │ +
345 gttoc(LCA_marginal);
│ │ │ │ +
346
│ │ │ │ +
347 // Compute shortcuts of the requested cliques given the lowest common ancestor
│ │ │ │ +
348 gttic(Clique_shortcuts);
│ │ │ │ +
349 BayesNetType p_C1_Bred = C1->shortcut(B, function);
│ │ │ │ +
350 BayesNetType p_C2_Bred = C2->shortcut(B, function);
│ │ │ │ +
351 gttoc(Clique_shortcuts);
│ │ │ │ +
352
│ │ │ │ +
353 // Factor the shortcuts to be conditioned on the full root
│ │ │ │ +
354 // Get the set of variables to eliminate, which is C1\B.
│ │ │ │ +
355 gttic(Full_root_factoring);
│ │ │ │ +
356 boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C1_B; {
│ │ │ │ +
357 KeyVector C1_minus_B; {
│ │ │ │ +
358 KeySet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents());
│ │ │ │ +
359 for(const Key j: *B->conditional()) {
│ │ │ │ +
360 C1_minus_B_set.erase(j); }
│ │ │ │ +
361 C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end());
│ │ │ │ +
362 }
│ │ │ │ +
363 // Factor into C1\B | B.
│ │ │ │ +
364 sharedFactorGraph temp_remaining;
│ │ │ │ +
365 boost::tie(p_C1_B, temp_remaining) =
│ │ │ │ +
366 FactorGraphType(p_C1_Bred).eliminatePartialMultifrontal(Ordering(C1_minus_B), function);
│ │ │ │ +
367 }
│ │ │ │ +
368 boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C2_B; {
│ │ │ │ +
369 KeyVector C2_minus_B; {
│ │ │ │ +
370 KeySet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents());
│ │ │ │ +
371 for(const Key j: *B->conditional()) {
│ │ │ │ +
372 C2_minus_B_set.erase(j); }
│ │ │ │ +
373 C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end());
│ │ │ │ +
374 }
│ │ │ │ +
375 // Factor into C2\B | B.
│ │ │ │ +
376 sharedFactorGraph temp_remaining;
│ │ │ │ +
377 boost::tie(p_C2_B, temp_remaining) =
│ │ │ │ +
378 FactorGraphType(p_C2_Bred).eliminatePartialMultifrontal(Ordering(C2_minus_B), function);
│ │ │ │ +
379 }
│ │ │ │ +
380 gttoc(Full_root_factoring);
│ │ │ │ +
381
│ │ │ │ +
382 gttic(Variable_joint);
│ │ │ │ +
383 p_BC1C2 += p_B;
│ │ │ │ +
384 p_BC1C2 += *p_C1_B;
│ │ │ │ +
385 p_BC1C2 += *p_C2_B;
│ │ │ │ +
386 if(C1 != B)
│ │ │ │ +
387 p_BC1C2 += C1->conditional();
│ │ │ │ +
388 if(C2 != B)
│ │ │ │ +
389 p_BC1C2 += C2->conditional();
│ │ │ │ +
390 gttoc(Variable_joint);
│ │ │ │ +
391 }
│ │ │ │ +
392 else
│ │ │ │ +
393 {
│ │ │ │ +
394 // The nodes have no common ancestor, they're in different trees, so they're joint is just the
│ │ │ │ +
395 // product of their marginals.
│ │ │ │ +
396 gttic(Disjoint_marginals);
│ │ │ │ +
397 p_BC1C2 += C1->marginal2(function);
│ │ │ │ +
398 p_BC1C2 += C2->marginal2(function);
│ │ │ │ +
399 gttoc(Disjoint_marginals);
│ │ │ │ +
400 }
│ │ │ │ +
401
│ │ │ │ +
402 // now, marginalize out everything that is not variable j1 or j2
│ │ │ │ +
403 return p_BC1C2.marginalMultifrontalBayesNet(Ordering{j1, j2}, function);
│ │ │ │ +
404 }
│ │ │ │ +
│ │ │ │ +
405
│ │ │ │ +
406 /* ************************************************************************* */
│ │ │ │ +
407 template<class CLIQUE>
│ │ │ │ +
│ │ │ │ +
408 void BayesTree<CLIQUE>::clear() {
│ │ │ │ +
409 // Remove all nodes and clear the root pointer
│ │ │ │ +
410 nodes_.clear();
│ │ │ │ +
411 roots_.clear();
│ │ │ │ +
412 }
│ │ │ │ +
│ │ │ │ +
413
│ │ │ │ +
414 /* ************************************************************************* */
│ │ │ │ +
415 template<class CLIQUE>
│ │ │ │ +
│ │ │ │ +
416 void BayesTree<CLIQUE>::deleteCachedShortcuts() {
│ │ │ │ +
417 for(const sharedClique& root: roots_) {
│ │ │ │ +
418 root->deleteCachedShortcuts();
│ │ │ │ +
419 }
│ │ │ │ +
420 }
│ │ │ │ +
│ │ │ │ +
421
│ │ │ │ +
422 /* ************************************************************************* */
│ │ │ │ +
423 template<class CLIQUE>
│ │ │ │ +
│ │ │ │ +
424 void BayesTree<CLIQUE>::removeClique(sharedClique clique)
│ │ │ │ +
425 {
│ │ │ │ +
426 if (clique->isRoot()) {
│ │ │ │ +
427 typename Roots::iterator root = std::find(roots_.begin(), roots_.end(), clique);
│ │ │ │ +
428 if(root != roots_.end())
│ │ │ │ +
429 roots_.erase(root);
│ │ │ │ +
430 } else { // detach clique from parent
│ │ │ │ +
431 sharedClique parent = clique->parent_.lock();
│ │ │ │ +
432 typename Roots::iterator child = std::find(parent->children.begin(), parent->children.end(), clique);
│ │ │ │ +
433 assert(child != parent->children.end());
│ │ │ │ +
434 parent->children.erase(child);
│ │ │ │ +
435 }
│ │ │ │ +
436
│ │ │ │ +
437 // orphan my children
│ │ │ │ +
438 for(sharedClique child: clique->children)
│ │ │ │ +
439 child->parent_ = typename Clique::weak_ptr();
│ │ │ │ +
440
│ │ │ │ +
441 for(Key j: clique->conditional()->frontals()) {
│ │ │ │ +
442 nodes_.unsafe_erase(j);
│ │ │ │ +
443 }
│ │ │ │ +
444 }
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
445
│ │ │ │ +
446 /* ************************************************************************* */
│ │ │ │ +
447 template <class CLIQUE>
│ │ │ │ +
│ │ │ │ +
448 void BayesTree<CLIQUE>::removePath(sharedClique clique, BayesNetType* bn,
│ │ │ │ +
449 Cliques* orphans) {
│ │ │ │ +
450 // base case is nullptr, if so we do nothing and return empties above
│ │ │ │ +
451 if (clique) {
│ │ │ │ +
452 // remove the clique from orphans in case it has been added earlier
│ │ │ │ +
453 orphans->remove(clique);
│ │ │ │ +
454
│ │ │ │ +
455 // remove me
│ │ │ │ +
456 this->removeClique(clique);
│ │ │ │ +
457
│ │ │ │ +
458 // remove path above me
│ │ │ │ +
459 this->removePath(typename Clique::shared_ptr(clique->parent_.lock()), bn,
│ │ │ │ +
460 orphans);
│ │ │ │ +
461
│ │ │ │ +
462 // add children to list of orphans (splice also removed them from
│ │ │ │ +
463 // clique->children_)
│ │ │ │ +
464 orphans->insert(orphans->begin(), clique->children.begin(),
│ │ │ │ +
465 clique->children.end());
│ │ │ │ +
466 clique->children.clear();
│ │ │ │ +
467
│ │ │ │ +
468 bn->push_back(clique->conditional_);
│ │ │ │ +
469 }
│ │ │ │ +
470 }
│ │ │ │ +
│ │ │ │ +
471
│ │ │ │ +
472 /* *************************************************************************
│ │ │ │ +
473 */
│ │ │ │ +
474 template <class CLIQUE>
│ │ │ │ +
│ │ │ │ +
475 void BayesTree<CLIQUE>::removeTop(const KeyVector& keys, BayesNetType* bn,
│ │ │ │ +
476 Cliques* orphans) {
│ │ │ │ +
477 gttic(removetop);
│ │ │ │ +
478 // process each key of the new factor
│ │ │ │ +
479 for (const Key& j : keys) {
│ │ │ │ +
480 // get the clique
│ │ │ │ +
481 // TODO(frank): Nodes will be searched again in removeClique
│ │ │ │ +
482 typename Nodes::const_iterator node = nodes_.find(j);
│ │ │ │ +
483 if (node != nodes_.end()) {
│ │ │ │ +
484 // remove path from clique to root
│ │ │ │ +
485 this->removePath(node->second, bn, orphans);
│ │ │ │ +
486 }
│ │ │ │ +
487 }
│ │ │ │ +
488
│ │ │ │ +
489 // Delete cachedShortcuts for each orphan subtree
│ │ │ │ +
490 // TODO(frank): Consider Improving
│ │ │ │ +
491 for (sharedClique& orphan : *orphans) orphan->deleteCachedShortcuts();
│ │ │ │ +
492 }
│ │ │ │ +
│ │ │ │ +
493
│ │ │ │ +
494 /* ************************************************************************* */
│ │ │ │ +
495 template<class CLIQUE>
│ │ │ │ +
│ │ │ │ +
496 typename BayesTree<CLIQUE>::Cliques BayesTree<CLIQUE>::removeSubtree(
│ │ │ │ +
497 const sharedClique& subtree)
│ │ │ │ +
498 {
│ │ │ │ +
499 // Result clique list
│ │ │ │ +
500 Cliques cliques;
│ │ │ │ +
501 cliques.push_back(subtree);
│ │ │ │ +
502
│ │ │ │ +
503 // Remove the first clique from its parents
│ │ │ │ +
504 if(!subtree->isRoot())
│ │ │ │ +
505 subtree->parent()->children.erase(std::find(
│ │ │ │ +
506 subtree->parent()->children.begin(), subtree->parent()->children.end(), subtree));
│ │ │ │ +
507 else
│ │ │ │ +
508 roots_.erase(std::find(roots_.begin(), roots_.end(), subtree));
│ │ │ │ +
509
│ │ │ │ +
510 // Add all subtree cliques and erase the children and parent of each
│ │ │ │ +
511 for(typename Cliques::iterator clique = cliques.begin(); clique != cliques.end(); ++clique)
│ │ │ │ +
512 {
│ │ │ │ +
513 // Add children
│ │ │ │ +
514 for(const sharedClique& child: (*clique)->children) {
│ │ │ │ +
515 cliques.push_back(child); }
│ │ │ │ +
516
│ │ │ │ +
517 // Delete cached shortcuts
│ │ │ │ +
518 (*clique)->deleteCachedShortcutsNonRecursive();
│ │ │ │ +
519
│ │ │ │ +
520 // Remove this node from the nodes index
│ │ │ │ +
521 for(Key j: (*clique)->conditional()->frontals()) {
│ │ │ │ +
522 nodes_.unsafe_erase(j); }
│ │ │ │ +
523
│ │ │ │ +
524 // Erase the parent and children pointers
│ │ │ │ +
525 (*clique)->parent_.reset();
│ │ │ │ +
526 (*clique)->children.clear();
│ │ │ │ +
527 }
│ │ │ │ +
528
│ │ │ │ +
529 return cliques;
│ │ │ │ +
530 }
│ │ │ │ +
│ │ │ │ +
531
│ │ │ │ +
532}
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
timing.h
Timing utilities.
│ │ │ │ +
Ordering.h
Variable ordering for the elimination algorithm.
│ │ │ │ +
BayesTree.h
Bayes Tree is a tree of cliques of a Bayes Chain.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ -
gtsam::BayesNet
A BayesNet is a tree of conditionals, stored in elimination order.
Definition BayesNet.h:35
│ │ │ │ -
gtsam::BayesNet::BayesNet
BayesNet(std::initializer_list< sharedConditional > conditionals)
Constructor that takes an initializer list of shared pointers.
Definition BayesNet.h:60
│ │ │ │ -
gtsam::BayesNet::print
void print(const std::string &s="BayesNet", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print out graph
Definition BayesNet-inst.h:32
│ │ │ │ -
gtsam::BayesNet::dot
void dot(std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
Output to graphviz format, stream version.
Definition BayesNet-inst.h:46
│ │ │ │ -
gtsam::BayesNet::sharedConditional
boost::shared_ptr< CONDITIONAL > sharedConditional
A shared pointer to a conditional.
Definition BayesNet.h:41
│ │ │ │ -
gtsam::BayesNet::BayesNet
BayesNet(ITERATOR firstConditional, ITERATOR lastConditional)
Construct from iterator over conditionals.
Definition BayesNet.h:52
│ │ │ │ -
gtsam::BayesNet::saveGraph
void saveGraph(const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
output to file with graphviz format.
Definition BayesNet-inst.h:83
│ │ │ │ -
gtsam::BayesNet::BayesNet
BayesNet()
Default constructor as an empty BayesNet.
Definition BayesNet.h:48
│ │ │ │ +
gtsam::treeTraversal::DepthFirstForest
void DepthFirstForest(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, VISITOR_POST &visitorPost)
Traverse a forest depth-first with pre-order and post-order visits.
Definition treeTraversal-inst.h:77
│ │ │ │ +
gtsam::treeTraversal::PrintForest
void PrintForest(const FOREST &forest, std::string str, const KeyFormatter &keyFormatter)
Print a tree, prefixing each line with str, and formatting keys using keyFormatter.
Definition treeTraversal-inst.h:219
│ │ │ │ +
gtsam::FastList
FastList is a thin wrapper around std::list that uses the boost fast_pool_allocator instead of the de...
Definition FastList.h:40
│ │ │ │ + │ │ │ │
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ -
gtsam::DotWriter
DotWriter is a helper class for writing graphviz .dot files.
Definition DotWriter.h:35
│ │ │ │ -
HybridValues
the error.
│ │ │ │ +
gtsam::BayesTreeCliqueData
store all the sizes
Definition BayesTree.h:48
│ │ │ │ +
gtsam::BayesTree
Bayes tree.
Definition BayesTree.h:67
│ │ │ │ +
gtsam::BayesTree::nodes_
Nodes nodes_
Map from indices to Clique.
Definition BayesTree.h:100
│ │ │ │ +
gtsam::BayesTree::removeClique
void removeClique(sharedClique clique)
remove a clique: warning, can result in a forest
Definition BayesTree-inst.h:424
│ │ │ │ +
gtsam::BayesTree::joint
sharedFactorGraph joint(Key j1, Key j2, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
return joint on two variables Limitation: can only calculate joint if cliques are disjoint or one of ...
Definition BayesTree-inst.h:293
│ │ │ │ +
gtsam::BayesTree::fillNodesIndex
void fillNodesIndex(const sharedClique &subtree)
Fill the nodes index for a subtree.
Definition BayesTree-inst.h:246
│ │ │ │ +
gtsam::BayesTree::dot
void dot(std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Output to graphviz format, stream version.
Definition BayesTree-inst.h:64
│ │ │ │ +
gtsam::BayesTree::addFactorsToGraph
void addFactorsToGraph(FactorGraph< FactorType > *graph) const
Add all cliques in this BayesTree to the specified factor graph.
Definition BayesTree-inst.h:168
│ │ │ │ +
gtsam::BayesTree::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition BayesTree-inst.h:230
│ │ │ │ +
gtsam::BayesTree::operator=
This & operator=(const This &other)
Assignment operator.
Definition BayesTree-inst.h:199
│ │ │ │ +
gtsam::BayesTree::sharedClique
boost::shared_ptr< Clique > sharedClique
Shared pointer to a clique.
Definition BayesTree.h:74
│ │ │ │ +
gtsam::BayesTree::BayesTree
BayesTree()
Create an empty Bayes Tree.
Definition BayesTree.h:109
│ │ │ │ +
gtsam::BayesTree::clear
void clear()
Remove all nodes.
Definition BayesTree-inst.h:408
│ │ │ │ +
gtsam::BayesTree::addClique
void addClique(const sharedClique &clique, const sharedClique &parent_clique=sharedClique())
add a clique (top down)
Definition BayesTree-inst.h:142
│ │ │ │ +
gtsam::BayesTree::jointBayesNet
sharedBayesNet jointBayesNet(Key j1, Key j2, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
return joint on two variables as a BayesNet Limitation: can only calculate joint if cliques are disjo...
Definition BayesTree-inst.h:302
│ │ │ │ +
gtsam::BayesTree::findParentClique
Key findParentClique(const CONTAINER &parents) const
Find parent clique of a conditional.
Definition BayesTree-inst.h:238
│ │ │ │ +
gtsam::BayesTree::size
size_t size() const
number of cliques
Definition BayesTree-inst.h:133
│ │ │ │ +
gtsam::BayesTree::deleteCachedShortcuts
void deleteCachedShortcuts()
Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data.
Definition BayesTree-inst.h:416
│ │ │ │ +
gtsam::BayesTree::removePath
void removePath(sharedClique clique, BayesNetType *bn, Cliques *orphans)
Remove path from clique to root and return that path as factors plus a list of orphaned subtree roots...
Definition BayesTree-inst.h:448
│ │ │ │ +
gtsam::BayesTree::marginalFactor
sharedConditional marginalFactor(Key j, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
Return marginal on any variable.
Definition BayesTree-inst.h:270
│ │ │ │ +
gtsam::BayesTree::numCachedSeparatorMarginals
size_t numCachedSeparatorMarginals() const
Collect number of cliques with cached separator marginals.
Definition BayesTree-inst.h:55
│ │ │ │ +
gtsam::BayesTree::getCliqueData
BayesTreeCliqueData getCliqueData() const
Gather data on all cliques.
Definition BayesTree-inst.h:35
│ │ │ │ +
gtsam::BayesTree::removeSubtree
Cliques removeSubtree(const sharedClique &subtree)
Remove the requested subtree.
Definition BayesTree-inst.h:496
│ │ │ │ +
gtsam::BayesTree::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print
Definition BayesTree-inst.h:212
│ │ │ │ +
gtsam::BayesTree::insertRoot
void insertRoot(const sharedClique &subtree)
Insert a new subtree with known parent clique.
Definition BayesTree-inst.h:260
│ │ │ │ +
gtsam::BayesTree::saveGraph
void saveGraph(const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
output to file with graphviz format.
Definition BayesTree-inst.h:85
│ │ │ │ +
gtsam::BayesTree::removeTop
void removeTop(const KeyVector &keys, BayesNetType *bn, Cliques *orphans)
Given a list of indices, turn "contaminated" part of the tree back into a factor graph.
Definition BayesTree-inst.h:475
│ │ │ │ +
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,141 +1,757 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -BayesNet.h │ │ │ │ │ +BayesTree-inst.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3* GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ -4* Atlanta, Georgia 30332-0415 │ │ │ │ │ -5* All Rights Reserved │ │ │ │ │ -6* Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +5 * All Rights Reserved │ │ │ │ │ +6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ -8* See LICENSE for the license information │ │ │ │ │ +8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ -10* ------------------------------------------------------------------------- │ │ │ │ │ +10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +21#pragma once │ │ │ │ │ 22 │ │ │ │ │ -23#include │ │ │ │ │ -24#include │ │ │ │ │ -25 │ │ │ │ │ -26namespace _g_t_s_a_m { │ │ │ │ │ +23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_O_r_d_e_r_i_n_g_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_b_a_s_e_/_t_r_e_e_T_r_a_v_e_r_s_a_l_-_i_n_s_t_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_b_a_s_e_/_t_i_m_i_n_g_._h> │ │ │ │ │ 27 │ │ │ │ │ -28class _H_y_b_r_i_d_V_a_l_u_e_s; │ │ │ │ │ -29 │ │ │ │ │ -34template │ │ │ │ │ -_3_5class _B_a_y_e_s_N_e_t : public _F_a_c_t_o_r_G_r_a_p_h { │ │ │ │ │ -36 private: │ │ │ │ │ -37 typedef _F_a_c_t_o_r_G_r_a_p_h_<_C_O_N_D_I_T_I_O_N_A_L_> _B_a_s_e; │ │ │ │ │ -38 │ │ │ │ │ -39 public: │ │ │ │ │ -40 typedef typename boost::shared_ptr │ │ │ │ │ -_4_1 _s_h_a_r_e_d_C_o_n_d_i_t_i_o_n_a_l; │ │ │ │ │ -42 │ │ │ │ │ -43 protected: │ │ │ │ │ -46 │ │ │ │ │ -_4_8 _B_a_y_e_s_N_e_t() {} │ │ │ │ │ -49 │ │ │ │ │ -51 template │ │ │ │ │ -_5_2 _B_a_y_e_s_N_e_t(ITERATOR firstConditional, ITERATOR lastConditional) │ │ │ │ │ -53 : _B_a_s_e(firstConditional, lastConditional) {} │ │ │ │ │ -54 │ │ │ │ │ -_6_0 _B_a_y_e_s_N_e_t(std::initializer_list conditionals) │ │ │ │ │ -61 : _B_a_s_e(conditionals) {} │ │ │ │ │ -62 │ │ │ │ │ -64 │ │ │ │ │ -65 public: │ │ │ │ │ -68 │ │ │ │ │ -70 void _p_r_i_n_t( │ │ │ │ │ -71 const std::string& s = "BayesNet", │ │ │ │ │ -72 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override; │ │ │ │ │ -73 │ │ │ │ │ -77 │ │ │ │ │ -79 void _d_o_t(std::ostream& os, │ │ │ │ │ -80 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ -81 const _D_o_t_W_r_i_t_e_r& writer = _D_o_t_W_r_i_t_e_r()) const; │ │ │ │ │ +28#include │ │ │ │ │ +29#include │ │ │ │ │ +30 │ │ │ │ │ +31namespace _g_t_s_a_m { │ │ │ │ │ +32 │ │ │ │ │ +33 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +34 template │ │ │ │ │ +_3_5 _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_D_a_t_a _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_g_e_t_C_l_i_q_u_e_D_a_t_a() const { │ │ │ │ │ +36 _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_D_a_t_a stats; │ │ │ │ │ +37 for (const _s_h_a_r_e_d_C_l_i_q_u_e& root : roots_) getCliqueData(root, &stats); │ │ │ │ │ +38 return stats; │ │ │ │ │ +39 } │ │ │ │ │ +40 │ │ │ │ │ +41 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +42 template │ │ │ │ │ +_4_3 void _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_g_e_t_C_l_i_q_u_e_D_a_t_a(_s_h_a_r_e_d_C_l_i_q_u_e clique, │ │ │ │ │ +44 _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_D_a_t_a* stats) const { │ │ │ │ │ +45 const auto conditional = clique->conditional(); │ │ │ │ │ +46 stats->conditionalSizes.push_back(conditional->nrFrontals()); │ │ │ │ │ +47 stats->separatorSizes.push_back(conditional->nrParents()); │ │ │ │ │ +48 for (_s_h_a_r_e_d_C_l_i_q_u_e c : clique->children) { │ │ │ │ │ +49 getCliqueData(c, stats); │ │ │ │ │ +50 } │ │ │ │ │ +51 } │ │ │ │ │ +52 │ │ │ │ │ +53 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +54 template │ │ │ │ │ +_5_5 size_t _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_n_u_m_C_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l_s() const { │ │ │ │ │ +56 size_t count = 0; │ │ │ │ │ +57 for(const _s_h_a_r_e_d_C_l_i_q_u_e& root: roots_) │ │ │ │ │ +58 count += root->numCachedSeparatorMarginals(); │ │ │ │ │ +59 return count; │ │ │ │ │ +60 } │ │ │ │ │ +61 │ │ │ │ │ +62 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +63 template │ │ │ │ │ +_6_4 void _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_d_o_t(std::ostream& os, │ │ │ │ │ +65 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter) const { │ │ │ │ │ +66 if (roots_.empty()) │ │ │ │ │ +67 throw std::invalid_argument( │ │ │ │ │ +68 "the root of Bayes tree has not been initialized!"); │ │ │ │ │ +69 os << "digraph G{\n"; │ │ │ │ │ +70 for (const _s_h_a_r_e_d_C_l_i_q_u_e& root : roots_) _d_o_t(os, root, keyFormatter); │ │ │ │ │ +71 os << "}"; │ │ │ │ │ +72 std::flush(os); │ │ │ │ │ +73 } │ │ │ │ │ +74 │ │ │ │ │ +75 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +76 template │ │ │ │ │ +_7_7 std::string _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_d_o_t(const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter) const { │ │ │ │ │ +78 std::stringstream ss; │ │ │ │ │ +79 _d_o_t(ss, keyFormatter); │ │ │ │ │ +80 return ss.str(); │ │ │ │ │ +81 } │ │ │ │ │ 82 │ │ │ │ │ -84 std::string _d_o_t(const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ -85 const _D_o_t_W_r_i_t_e_r& writer = _D_o_t_W_r_i_t_e_r()) const; │ │ │ │ │ -86 │ │ │ │ │ -88 void _s_a_v_e_G_r_a_p_h(const std::string& filename, │ │ │ │ │ -89 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ -90 const _D_o_t_W_r_i_t_e_r& writer = _D_o_t_W_r_i_t_e_r()) const; │ │ │ │ │ +83 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +84 template │ │ │ │ │ +_8_5 void _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_s_a_v_e_G_r_a_p_h(const std::string& filename, │ │ │ │ │ +86 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter) const { │ │ │ │ │ +87 std::ofstream of(filename.c_str()); │ │ │ │ │ +88 _d_o_t(of, keyFormatter); │ │ │ │ │ +89 of.close(); │ │ │ │ │ +90 } │ │ │ │ │ 91 │ │ │ │ │ -95 │ │ │ │ │ -96 // Expose HybridValues version of logProbability. │ │ │ │ │ -97 double logProbability(const _H_y_b_r_i_d_V_a_l_u_e_s& x) const; │ │ │ │ │ -98 │ │ │ │ │ -99 // Expose HybridValues version of evaluate. │ │ │ │ │ -100 double evaluate(const _H_y_b_r_i_d_V_a_l_u_e_s& c) const; │ │ │ │ │ -101 │ │ │ │ │ -103}; │ │ │ │ │ -104 │ │ │ │ │ -105} // namespace gtsam │ │ │ │ │ -106 │ │ │ │ │ -107#include │ │ │ │ │ -_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Factor Graph Base Class. │ │ │ │ │ +92 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +93 template │ │ │ │ │ +_9_4 void _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_d_o_t(std::ostream& s, _s_h_a_r_e_d_C_l_i_q_u_e clique, │ │ │ │ │ +95 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter, │ │ │ │ │ +96 int parentnum) const { │ │ │ │ │ +97 static int num = 0; │ │ │ │ │ +98 bool first = true; │ │ │ │ │ +99 std::stringstream out; │ │ │ │ │ +100 out << num; │ │ │ │ │ +101 std::string parent = out.str(); │ │ │ │ │ +102 parent += "[label=\""; │ │ │ │ │ +103 │ │ │ │ │ +104 for (_K_e_y key : clique->conditional_->frontals()) { │ │ │ │ │ +105 if (!first) parent += ", "; │ │ │ │ │ +106 first = false; │ │ │ │ │ +107 parent += keyFormatter(key); │ │ │ │ │ +108 } │ │ │ │ │ +109 │ │ │ │ │ +110 if (clique->parent()) { │ │ │ │ │ +111 parent += " : "; │ │ │ │ │ +_1_1_2 s << parentnum << "->" << num << "\n"; │ │ │ │ │ +113 } │ │ │ │ │ +114 │ │ │ │ │ +115 first = true; │ │ │ │ │ +116 for (_K_e_y parentKey : clique->conditional_->parents()) { │ │ │ │ │ +_1_1_7 if (!first) parent += ", "; │ │ │ │ │ +118 first = false; │ │ │ │ │ +119 parent += keyFormatter(parentKey); │ │ │ │ │ +120 } │ │ │ │ │ +121 parent += "\"];\n"; │ │ │ │ │ +122 s << parent; │ │ │ │ │ +_1_2_3 parentnum = num; │ │ │ │ │ +124 │ │ │ │ │ +125 for (_s_h_a_r_e_d_C_l_i_q_u_e c : clique->children) { │ │ │ │ │ +126 num++; │ │ │ │ │ +_1_2_7 _d_o_t(s, c, keyFormatter, parentnum); │ │ │ │ │ +128 } │ │ │ │ │ +129 } │ │ │ │ │ +130 │ │ │ │ │ +131 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +132 template │ │ │ │ │ +_1_3_3 size_t _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_s_i_z_e() const { │ │ │ │ │ +134 size_t size = 0; │ │ │ │ │ +_1_3_5 for(const _s_h_a_r_e_d_C_l_i_q_u_e& clique: roots_) │ │ │ │ │ +136 size += clique->treeSize(); │ │ │ │ │ +137 return size; │ │ │ │ │ +138 } │ │ │ │ │ +139 │ │ │ │ │ +140 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +141 template │ │ │ │ │ +_1_4_2 void _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_a_d_d_C_l_i_q_u_e(const _s_h_a_r_e_d_C_l_i_q_u_e& clique, const │ │ │ │ │ +_s_h_a_r_e_d_C_l_i_q_u_e& parent_clique) { │ │ │ │ │ +143 for(_K_e_y j: clique->conditional()->frontals()) │ │ │ │ │ +144 nodes_[j] = clique; │ │ │ │ │ +145 if (parent_clique != nullptr) { │ │ │ │ │ +146 clique->parent_ = parent_clique; │ │ │ │ │ +147 parent_clique->children.push_back(clique); │ │ │ │ │ +148 } else { │ │ │ │ │ +149 roots_.push_back(clique); │ │ │ │ │ +150 } │ │ │ │ │ +151 } │ │ │ │ │ +152 │ │ │ │ │ +153 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +154 namespace { │ │ │ │ │ +155 template │ │ │ │ │ +156 struct _pushCliqueFunctor { │ │ │ │ │ +157 _pushCliqueFunctor(_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_>* graph_) : graph(graph_) {} │ │ │ │ │ +158 FactorGraph* graph; │ │ │ │ │ +159 int operator()(const boost::shared_ptr& clique, int dummy) { │ │ │ │ │ +160 graph->push_back(clique->conditional_); │ │ │ │ │ +_1_6_1 return 0; │ │ │ │ │ +162 } │ │ │ │ │ +163 }; │ │ │ │ │ +_1_6_4 } // namespace │ │ │ │ │ +165 │ │ │ │ │ +166 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +167 template │ │ │ │ │ +_1_6_8 void _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_a_d_d_F_a_c_t_o_r_s_T_o_G_r_a_p_h( │ │ │ │ │ +169 _F_a_c_t_o_r_G_r_a_p_h_<_F_a_c_t_o_r_T_y_p_e_>* graph) const { │ │ │ │ │ +170 // Traverse the BayesTree and add all conditionals to this graph │ │ │ │ │ +_1_7_1 int data = 0; // Unused │ │ │ │ │ +172 _pushCliqueFunctor functor(graph); │ │ │ │ │ +173 _t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t(*this, data, functor); │ │ │ │ │ +174 } │ │ │ │ │ +175 │ │ │ │ │ +176 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +_1_7_7 template │ │ │ │ │ +_1_7_8 _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_B_a_y_e_s_T_r_e_e(const _T_h_i_s& other) { │ │ │ │ │ +179 *this = other; │ │ │ │ │ +180 } │ │ │ │ │ +181 │ │ │ │ │ +182 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +_1_8_3 namespace { │ │ │ │ │ +184 template │ │ │ │ │ +185 boost::shared_ptr │ │ │ │ │ +186 BayesTreeCloneForestVisitorPre(const boost::shared_ptr& node, const │ │ │ │ │ +boost::shared_ptr& parentPointer) │ │ │ │ │ +187 { │ │ │ │ │ +188 // Clone the current node and add it to its cloned parent │ │ │ │ │ +_1_8_9 boost::shared_ptr clone = boost::make_shared(*node); │ │ │ │ │ +190 clone->children.clear(); │ │ │ │ │ +191 clone->parent_ = parentPointer; │ │ │ │ │ +_1_9_2 parentPointer->children.push_back(clone); │ │ │ │ │ +193 return clone; │ │ │ │ │ +194 } │ │ │ │ │ +195 } │ │ │ │ │ +_1_9_6 │ │ │ │ │ +197 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +198 template │ │ │ │ │ +_1_9_9 _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>& _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_o_p_e_r_a_t_o_r_=(const _T_h_i_s& other) { │ │ │ │ │ +200 this->clear(); │ │ │ │ │ +201 boost::shared_ptr rootContainer = boost::make_shared(); │ │ │ │ │ +202 _t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t(other, rootContainer, │ │ │ │ │ +BayesTreeCloneForestVisitorPre); │ │ │ │ │ +203 for(const _s_h_a_r_e_d_C_l_i_q_u_e& root: rootContainer->children) { │ │ │ │ │ +204 root->parent_ = typename Clique::weak_ptr(); // Reset the parent since it's │ │ │ │ │ +set to the dummy clique │ │ │ │ │ +205 insertRoot(root); │ │ │ │ │ +206 } │ │ │ │ │ +207 return *this; │ │ │ │ │ +_2_0_8 } │ │ │ │ │ +209 │ │ │ │ │ +210 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +_2_1_1 template │ │ │ │ │ +_2_1_2 void _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_p_r_i_n_t(const std::string& s, const _K_e_y_F_o_r_m_a_t_t_e_r& │ │ │ │ │ +keyFormatter) const { │ │ │ │ │ +213 std::cout << s << ": cliques: " << size() << ", variables: " << nodes_.size │ │ │ │ │ +() << std::endl; │ │ │ │ │ +_2_1_4 _t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_P_r_i_n_t_F_o_r_e_s_t(*this, s, keyFormatter); │ │ │ │ │ +215 } │ │ │ │ │ +216 │ │ │ │ │ +217 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +218 // binary predicate to test equality of a pair for use in equals │ │ │ │ │ +219 template │ │ │ │ │ +_2_2_0 bool check_sharedCliques( │ │ │ │ │ +221 const std::pair<_K_e_y, typename _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_s_h_a_r_e_d_C_l_i_q_u_e>& v1, │ │ │ │ │ +222 const std::pair<_K_e_y, typename _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_s_h_a_r_e_d_C_l_i_q_u_e>& v2 │ │ │ │ │ +223 ) { │ │ │ │ │ +224 return v1.first == v2.first && │ │ │ │ │ +225 ((!v1.second && !v2.second) || (v1.second && v2.second && v1.second->equals │ │ │ │ │ +(*v2.second))); │ │ │ │ │ +_2_2_6 } │ │ │ │ │ +227 │ │ │ │ │ +228 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +229 template │ │ │ │ │ +_2_3_0 bool _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_e_q_u_a_l_s(const _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>& other, double tol) │ │ │ │ │ +const { │ │ │ │ │ +231 return size()==other._s_i_z_e() && │ │ │ │ │ +232 std::equal(nodes_.begin(), nodes_.end(), other._n_o_d_e_s__.begin(), │ │ │ │ │ +&check_sharedCliques); │ │ │ │ │ +233 } │ │ │ │ │ +234 │ │ │ │ │ +_2_3_5 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +236 template │ │ │ │ │ +237 template │ │ │ │ │ +_2_3_8 _K_e_y _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_f_i_n_d_P_a_r_e_n_t_C_l_i_q_u_e(const CONTAINER& parents) const { │ │ │ │ │ +239 typename CONTAINER::const_iterator lowestOrderedParent = min_element │ │ │ │ │ +(parents.begin(), parents.end()); │ │ │ │ │ +240 assert(lowestOrderedParent != parents.end()); │ │ │ │ │ +_2_4_1 return *lowestOrderedParent; │ │ │ │ │ +242 } │ │ │ │ │ +243 │ │ │ │ │ +244 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +245 template │ │ │ │ │ +_2_4_6 void _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_f_i_l_l_N_o_d_e_s_I_n_d_e_x(const _s_h_a_r_e_d_C_l_i_q_u_e& subtree) { │ │ │ │ │ +247 // Add each frontal variable of this root node │ │ │ │ │ +248 for(const _K_e_y& j: subtree->conditional()->frontals()) { │ │ │ │ │ +249 bool inserted = nodes_.insert(std::make_pair(j, subtree)).second; │ │ │ │ │ +_2_5_0 assert(inserted); (void)inserted; │ │ │ │ │ +251 } │ │ │ │ │ +252 // Fill index for each child │ │ │ │ │ +_2_5_3 typedef typename _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_s_h_a_r_e_d_C_l_i_q_u_e _s_h_a_r_e_d_C_l_i_q_u_e; │ │ │ │ │ +254 for(const _s_h_a_r_e_d_C_l_i_q_u_e& child: subtree->children) { │ │ │ │ │ +255 fillNodesIndex(child); } │ │ │ │ │ +_2_5_6 } │ │ │ │ │ +257 │ │ │ │ │ +258 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +259 template │ │ │ │ │ +_2_6_0 void _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_i_n_s_e_r_t_R_o_o_t(const _s_h_a_r_e_d_C_l_i_q_u_e& subtree) { │ │ │ │ │ +261 roots_.push_back(subtree); // Add to roots │ │ │ │ │ +262 fillNodesIndex(subtree); // Populate nodes index │ │ │ │ │ +263 } │ │ │ │ │ +264 │ │ │ │ │ +265 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +266 // First finds clique marginal then marginalizes that │ │ │ │ │ +267 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +268 template │ │ │ │ │ +269 typename BayesTree::sharedConditional │ │ │ │ │ +_2_7_0 _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_m_a_r_g_i_n_a_l_F_a_c_t_o_r(_K_e_y j, const Eliminate& function) const │ │ │ │ │ +271 { │ │ │ │ │ +272 gttic(BayesTree_marginalFactor); │ │ │ │ │ +273 │ │ │ │ │ +274 // get clique containing Key j │ │ │ │ │ +275 _s_h_a_r_e_d_C_l_i_q_u_e clique = this->clique(j); │ │ │ │ │ +276 │ │ │ │ │ +277 // calculate or retrieve its marginal P(C) = P(F,S) │ │ │ │ │ +278 FactorGraphType cliqueMarginal = clique->marginal2(function); │ │ │ │ │ +279 │ │ │ │ │ +280 // Now, marginalize out everything that is not variable j │ │ │ │ │ +281 BayesNetType marginalBN = │ │ │ │ │ +282 *cliqueMarginal.marginalMultifrontalBayesNet(_O_r_d_e_r_i_n_g{j}, function); │ │ │ │ │ +283 │ │ │ │ │ +284 // The Bayes net should contain only one conditional for variable j, so │ │ │ │ │ +return it │ │ │ │ │ +285 return marginalBN.front(); │ │ │ │ │ +286 } │ │ │ │ │ +287 │ │ │ │ │ +288 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +289 // Find two cliques, their joint, then marginalizes │ │ │ │ │ +290 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +291 template │ │ │ │ │ +292 typename BayesTree::sharedFactorGraph │ │ │ │ │ +_2_9_3 _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_j_o_i_n_t(_K_e_y j1, _K_e_y j2, const Eliminate& function) const │ │ │ │ │ +294 { │ │ │ │ │ +295 gttic(BayesTree_joint); │ │ │ │ │ +296 return boost::make_shared(*jointBayesNet(j1, j2, │ │ │ │ │ +function)); │ │ │ │ │ +297 } │ │ │ │ │ +298 │ │ │ │ │ +299 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +300 template │ │ │ │ │ +301 typename BayesTree::sharedBayesNet │ │ │ │ │ +_3_0_2 _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_j_o_i_n_t_B_a_y_e_s_N_e_t(_K_e_y j1, _K_e_y j2, const Eliminate& function) │ │ │ │ │ +const │ │ │ │ │ +303 { │ │ │ │ │ +304 gttic(BayesTree_jointBayesNet); │ │ │ │ │ +305 // get clique C1 and C2 │ │ │ │ │ +306 _s_h_a_r_e_d_C_l_i_q_u_e C1 = (*this)[j1], C2 = (*this)[j2]; │ │ │ │ │ +307 │ │ │ │ │ +308 gttic(Lowest_common_ancestor); │ │ │ │ │ +309 // Find lowest common ancestor clique │ │ │ │ │ +310 _s_h_a_r_e_d_C_l_i_q_u_e B; { │ │ │ │ │ +311 // Build two paths to the root │ │ │ │ │ +312 _F_a_s_t_L_i_s_t_<_s_h_a_r_e_d_C_l_i_q_u_e_> path1, path2; { │ │ │ │ │ +313 _s_h_a_r_e_d_C_l_i_q_u_e p = C1; │ │ │ │ │ +314 while(p) { │ │ │ │ │ +315 path1.push_front(p); │ │ │ │ │ +316 p = p->parent(); │ │ │ │ │ +317 } │ │ │ │ │ +318 } { │ │ │ │ │ +319 _s_h_a_r_e_d_C_l_i_q_u_e p = C2; │ │ │ │ │ +320 while(p) { │ │ │ │ │ +321 path2.push_front(p); │ │ │ │ │ +322 p = p->parent(); │ │ │ │ │ +323 } │ │ │ │ │ +324 } │ │ │ │ │ +325 // Find the path intersection │ │ │ │ │ +326 typename _F_a_s_t_L_i_s_t_<_s_h_a_r_e_d_C_l_i_q_u_e_>_:_:_c_o_n_s_t___i_t_e_r_a_t_o_r p1 = path1.begin(), p2 = │ │ │ │ │ +path2.begin(); │ │ │ │ │ +327 if(*p1 == *p2) │ │ │ │ │ +328 B = *p1; │ │ │ │ │ +329 while(p1 != path1.end() && p2 != path2.end() && *p1 == *p2) { │ │ │ │ │ +330 B = *p1; │ │ │ │ │ +331 ++p1; │ │ │ │ │ +332 ++p2; │ │ │ │ │ +333 } │ │ │ │ │ +334 } │ │ │ │ │ +335 gttoc(Lowest_common_ancestor); │ │ │ │ │ +336 │ │ │ │ │ +337 // Build joint on all involved variables │ │ │ │ │ +338 FactorGraphType p_BC1C2; │ │ │ │ │ +339 │ │ │ │ │ +340 if(B) │ │ │ │ │ +341 { │ │ │ │ │ +342 // Compute marginal on lowest common ancestor clique │ │ │ │ │ +343 gttic(LCA_marginal); │ │ │ │ │ +344 FactorGraphType p_B = B->marginal2(function); │ │ │ │ │ +345 gttoc(LCA_marginal); │ │ │ │ │ +346 │ │ │ │ │ +347 // Compute shortcuts of the requested cliques given the lowest common │ │ │ │ │ +ancestor │ │ │ │ │ +348 gttic(Clique_shortcuts); │ │ │ │ │ +349 BayesNetType p_C1_Bred = C1->shortcut(B, function); │ │ │ │ │ +350 BayesNetType p_C2_Bred = C2->shortcut(B, function); │ │ │ │ │ +351 gttoc(Clique_shortcuts); │ │ │ │ │ +352 │ │ │ │ │ +353 // Factor the shortcuts to be conditioned on the full root │ │ │ │ │ +354 // Get the set of variables to eliminate, which is C1\B. │ │ │ │ │ +355 gttic(Full_root_factoring); │ │ │ │ │ +356 boost::shared_ptr p_C1_B; { │ │ │ │ │ +357 _K_e_y_V_e_c_t_o_r C1_minus_B; { │ │ │ │ │ +358 _K_e_y_S_e_t C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()- │ │ │ │ │ +>endParents()); │ │ │ │ │ +359 for(const _K_e_y j: *B->conditional()) { │ │ │ │ │ +360 C1_minus_B_set.erase(j); } │ │ │ │ │ +361 C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end()); │ │ │ │ │ +362 } │ │ │ │ │ +363 // Factor into C1\B | B. │ │ │ │ │ +364 sharedFactorGraph temp_remaining; │ │ │ │ │ +365 boost::tie(p_C1_B, temp_remaining) = │ │ │ │ │ +366 FactorGraphType(p_C1_Bred).eliminatePartialMultifrontal(_O_r_d_e_r_i_n_g │ │ │ │ │ +(C1_minus_B), function); │ │ │ │ │ +367 } │ │ │ │ │ +368 boost::shared_ptr p_C2_B; { │ │ │ │ │ +369 _K_e_y_V_e_c_t_o_r C2_minus_B; { │ │ │ │ │ +370 _K_e_y_S_e_t C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()- │ │ │ │ │ +>endParents()); │ │ │ │ │ +371 for(const _K_e_y j: *B->conditional()) { │ │ │ │ │ +372 C2_minus_B_set.erase(j); } │ │ │ │ │ +373 C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end()); │ │ │ │ │ +374 } │ │ │ │ │ +375 // Factor into C2\B | B. │ │ │ │ │ +376 sharedFactorGraph temp_remaining; │ │ │ │ │ +377 boost::tie(p_C2_B, temp_remaining) = │ │ │ │ │ +378 FactorGraphType(p_C2_Bred).eliminatePartialMultifrontal(_O_r_d_e_r_i_n_g │ │ │ │ │ +(C2_minus_B), function); │ │ │ │ │ +379 } │ │ │ │ │ +380 gttoc(Full_root_factoring); │ │ │ │ │ +381 │ │ │ │ │ +382 gttic(Variable_joint); │ │ │ │ │ +383 p_BC1C2 += p_B; │ │ │ │ │ +384 p_BC1C2 += *p_C1_B; │ │ │ │ │ +385 p_BC1C2 += *p_C2_B; │ │ │ │ │ +386 if(C1 != B) │ │ │ │ │ +387 p_BC1C2 += C1->conditional(); │ │ │ │ │ +388 if(C2 != B) │ │ │ │ │ +389 p_BC1C2 += C2->conditional(); │ │ │ │ │ +390 gttoc(Variable_joint); │ │ │ │ │ +391 } │ │ │ │ │ +392 else │ │ │ │ │ +393 { │ │ │ │ │ +394 // The nodes have no common ancestor, they're in different trees, so │ │ │ │ │ +they're joint is just the │ │ │ │ │ +395 // product of their marginals. │ │ │ │ │ +396 gttic(Disjoint_marginals); │ │ │ │ │ +397 p_BC1C2 += C1->marginal2(function); │ │ │ │ │ +398 p_BC1C2 += C2->marginal2(function); │ │ │ │ │ +399 gttoc(Disjoint_marginals); │ │ │ │ │ +400 } │ │ │ │ │ +401 │ │ │ │ │ +402 // now, marginalize out everything that is not variable j1 or j2 │ │ │ │ │ +403 return p_BC1C2.marginalMultifrontalBayesNet(_O_r_d_e_r_i_n_g{j1, j2}, function); │ │ │ │ │ +404 } │ │ │ │ │ +405 │ │ │ │ │ +406 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +407 template │ │ │ │ │ +_4_0_8 void _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_c_l_e_a_r() { │ │ │ │ │ +409 // Remove all nodes and clear the root pointer │ │ │ │ │ +410 nodes_.clear(); │ │ │ │ │ +411 roots_.clear(); │ │ │ │ │ +412 } │ │ │ │ │ +413 │ │ │ │ │ +414 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +415 template │ │ │ │ │ +_4_1_6 void _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_d_e_l_e_t_e_C_a_c_h_e_d_S_h_o_r_t_c_u_t_s() { │ │ │ │ │ +417 for(const _s_h_a_r_e_d_C_l_i_q_u_e& root: roots_) { │ │ │ │ │ +418 root->deleteCachedShortcuts(); │ │ │ │ │ +419 } │ │ │ │ │ +420 } │ │ │ │ │ +421 │ │ │ │ │ +422 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +423 template │ │ │ │ │ +_4_2_4 void _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_r_e_m_o_v_e_C_l_i_q_u_e(_s_h_a_r_e_d_C_l_i_q_u_e clique) │ │ │ │ │ +425 { │ │ │ │ │ +426 if (clique->isRoot()) { │ │ │ │ │ +427 typename Roots::iterator root = std::find(roots_.begin(), roots_.end(), │ │ │ │ │ +clique); │ │ │ │ │ +428 if(root != roots_.end()) │ │ │ │ │ +429 roots_.erase(root); │ │ │ │ │ +430 } else { // detach clique from parent │ │ │ │ │ +431 _s_h_a_r_e_d_C_l_i_q_u_e parent = clique->parent_.lock(); │ │ │ │ │ +432 typename Roots::iterator child = std::find(parent->children.begin(), │ │ │ │ │ +parent->children.end(), clique); │ │ │ │ │ +433 assert(child != parent->children.end()); │ │ │ │ │ +434 parent->children.erase(child); │ │ │ │ │ +435 } │ │ │ │ │ +436 │ │ │ │ │ +437 // orphan my children │ │ │ │ │ +438 for(_s_h_a_r_e_d_C_l_i_q_u_e child: clique->children) │ │ │ │ │ +439 child->parent_ = typename Clique::weak_ptr(); │ │ │ │ │ +440 │ │ │ │ │ +441 for(_K_e_y j: clique->conditional()->frontals()) { │ │ │ │ │ +442 nodes_.unsafe_erase(j); │ │ │ │ │ +443 } │ │ │ │ │ +444 } │ │ │ │ │ +445 │ │ │ │ │ +446 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +447 template │ │ │ │ │ +_4_4_8 void _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_r_e_m_o_v_e_P_a_t_h(_s_h_a_r_e_d_C_l_i_q_u_e clique, BayesNetType* bn, │ │ │ │ │ +449 _C_l_i_q_u_e_s* orphans) { │ │ │ │ │ +450 // base case is nullptr, if so we do nothing and return empties above │ │ │ │ │ +451 if (clique) { │ │ │ │ │ +452 // remove the clique from orphans in case it has been added earlier │ │ │ │ │ +453 orphans->remove(clique); │ │ │ │ │ +454 │ │ │ │ │ +455 // remove me │ │ │ │ │ +456 this->removeClique(clique); │ │ │ │ │ +457 │ │ │ │ │ +458 // remove path above me │ │ │ │ │ +459 this->removePath(typename Clique::shared_ptr(clique->parent_.lock()), bn, │ │ │ │ │ +460 orphans); │ │ │ │ │ +461 │ │ │ │ │ +462 // add children to list of orphans (splice also removed them from │ │ │ │ │ +463 // clique->children_) │ │ │ │ │ +464 orphans->insert(orphans->begin(), clique->children.begin(), │ │ │ │ │ +465 clique->children.end()); │ │ │ │ │ +466 clique->children.clear(); │ │ │ │ │ +467 │ │ │ │ │ +468 bn->push_back(clique->conditional_); │ │ │ │ │ +469 } │ │ │ │ │ +470 } │ │ │ │ │ +471 │ │ │ │ │ +472 / │ │ │ │ │ +* ************************************************************************* │ │ │ │ │ +473 */ │ │ │ │ │ +474 template │ │ │ │ │ +_4_7_5 void _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_r_e_m_o_v_e_T_o_p(const _K_e_y_V_e_c_t_o_r& keys, BayesNetType* bn, │ │ │ │ │ +476 _C_l_i_q_u_e_s* orphans) { │ │ │ │ │ +477 gttic(removetop); │ │ │ │ │ +478 // process each key of the new factor │ │ │ │ │ +479 for (const _K_e_y& j : keys) { │ │ │ │ │ +480 // get the clique │ │ │ │ │ +481 // TODO(frank): Nodes will be searched again in removeClique │ │ │ │ │ +482 typename Nodes::const_iterator node = nodes_.find(j); │ │ │ │ │ +483 if (node != nodes_.end()) { │ │ │ │ │ +484 // remove path from clique to root │ │ │ │ │ +485 this->removePath(node->second, bn, orphans); │ │ │ │ │ +486 } │ │ │ │ │ +487 } │ │ │ │ │ +488 │ │ │ │ │ +489 // Delete cachedShortcuts for each orphan subtree │ │ │ │ │ +490 // TODO(frank): Consider Improving │ │ │ │ │ +491 for (_s_h_a_r_e_d_C_l_i_q_u_e& orphan : *orphans) orphan->deleteCachedShortcuts(); │ │ │ │ │ +492 } │ │ │ │ │ +493 │ │ │ │ │ +494 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +495 template │ │ │ │ │ +_4_9_6 typename _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_C_l_i_q_u_e_s _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_r_e_m_o_v_e_S_u_b_t_r_e_e( │ │ │ │ │ +497 const _s_h_a_r_e_d_C_l_i_q_u_e& subtree) │ │ │ │ │ +498 { │ │ │ │ │ +499 // Result clique list │ │ │ │ │ +500 _C_l_i_q_u_e_s cliques; │ │ │ │ │ +501 cliques.push_back(subtree); │ │ │ │ │ +502 │ │ │ │ │ +503 // Remove the first clique from its parents │ │ │ │ │ +504 if(!subtree->isRoot()) │ │ │ │ │ +505 subtree->parent()->children.erase(std::find( │ │ │ │ │ +506 subtree->parent()->children.begin(), subtree->parent()->children.end(), │ │ │ │ │ +subtree)); │ │ │ │ │ +507 else │ │ │ │ │ +508 roots_.erase(std::find(roots_.begin(), roots_.end(), subtree)); │ │ │ │ │ +509 │ │ │ │ │ +510 // Add all subtree cliques and erase the children and parent of each │ │ │ │ │ +511 for(typename Cliques::iterator clique = cliques.begin(); clique != │ │ │ │ │ +cliques.end(); ++clique) │ │ │ │ │ +512 { │ │ │ │ │ +513 // Add children │ │ │ │ │ +514 for(const _s_h_a_r_e_d_C_l_i_q_u_e& child: (*clique)->children) { │ │ │ │ │ +515 cliques.push_back(child); } │ │ │ │ │ +516 │ │ │ │ │ +517 // Delete cached shortcuts │ │ │ │ │ +518 (*clique)->deleteCachedShortcutsNonRecursive(); │ │ │ │ │ +519 │ │ │ │ │ +520 // Remove this node from the nodes index │ │ │ │ │ +521 for(_K_e_y j: (*clique)->conditional()->frontals()) { │ │ │ │ │ +522 nodes_.unsafe_erase(j); } │ │ │ │ │ +523 │ │ │ │ │ +524 // Erase the parent and children pointers │ │ │ │ │ +525 (*clique)->parent_.reset(); │ │ │ │ │ +526 (*clique)->children.clear(); │ │ │ │ │ +527 } │ │ │ │ │ +528 │ │ │ │ │ +529 return cliques; │ │ │ │ │ +530 } │ │ │ │ │ +531 │ │ │ │ │ +532} │ │ │ │ │ +_t_r_e_e_T_r_a_v_e_r_s_a_l_-_i_n_s_t_._h │ │ │ │ │ +_t_i_m_i_n_g_._h │ │ │ │ │ +Timing utilities. │ │ │ │ │ +_O_r_d_e_r_i_n_g_._h │ │ │ │ │ +Variable ordering for the elimination algorithm. │ │ │ │ │ +_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ +Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_d_o_t │ │ │ │ │ +double dot(const V1 &a, const V2 &b) │ │ │ │ │ +Dot product. │ │ │ │ │ +DDeeffiinniittiioonn Vector.h:195 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t │ │ │ │ │ -A BayesNet is a tree of conditionals, stored in elimination order. │ │ │ │ │ -DDeeffiinniittiioonn BayesNet.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t_:_:_B_a_y_e_s_N_e_t │ │ │ │ │ -BayesNet(std::initializer_list< sharedConditional > conditionals) │ │ │ │ │ -Constructor that takes an initializer list of shared pointers. │ │ │ │ │ -DDeeffiinniittiioonn BayesNet.h:60 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="BayesNet", const KeyFormatter │ │ │ │ │ -&formatter=DefaultKeyFormatter) const override │ │ │ │ │ -print out graph │ │ │ │ │ -DDeeffiinniittiioonn BayesNet-inst.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t_:_:_d_o_t │ │ │ │ │ -void dot(std::ostream &os, const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const │ │ │ │ │ -Output to graphviz format, stream version. │ │ │ │ │ -DDeeffiinniittiioonn BayesNet-inst.h:46 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t_:_:_s_h_a_r_e_d_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -boost::shared_ptr< CONDITIONAL > sharedConditional │ │ │ │ │ -A shared pointer to a conditional. │ │ │ │ │ -DDeeffiinniittiioonn BayesNet.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t_:_:_B_a_y_e_s_N_e_t │ │ │ │ │ -BayesNet(ITERATOR firstConditional, ITERATOR lastConditional) │ │ │ │ │ -Construct from iterator over conditionals. │ │ │ │ │ -DDeeffiinniittiioonn BayesNet.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t_:_:_s_a_v_e_G_r_a_p_h │ │ │ │ │ -void saveGraph(const std::string &filename, const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const │ │ │ │ │ -output to file with graphviz format. │ │ │ │ │ -DDeeffiinniittiioonn BayesNet-inst.h:83 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t_:_:_B_a_y_e_s_N_e_t │ │ │ │ │ -BayesNet() │ │ │ │ │ -Default constructor as an empty BayesNet. │ │ │ │ │ -DDeeffiinniittiioonn BayesNet.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t │ │ │ │ │ +void DepthFirstForest(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, │ │ │ │ │ +VISITOR_POST &visitorPost) │ │ │ │ │ +Traverse a forest depth-first with pre-order and post-order visits. │ │ │ │ │ +DDeeffiinniittiioonn treeTraversal-inst.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_P_r_i_n_t_F_o_r_e_s_t │ │ │ │ │ +void PrintForest(const FOREST &forest, std::string str, const KeyFormatter │ │ │ │ │ +&keyFormatter) │ │ │ │ │ +Print a tree, prefixing each line with str, and formatting keys using │ │ │ │ │ +keyFormatter. │ │ │ │ │ +DDeeffiinniittiioonn treeTraversal-inst.h:219 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t │ │ │ │ │ +FastList is a thin wrapper around std::list that uses the boost │ │ │ │ │ +fast_pool_allocator instead of the de... │ │ │ │ │ +DDeeffiinniittiioonn FastList.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ _g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ nodes. │ │ │ │ │ DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r │ │ │ │ │ -DotWriter is a helper class for writing graphviz .dot files. │ │ │ │ │ -DDeeffiinniittiioonn DotWriter.h:35 │ │ │ │ │ -_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -the error. │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_D_a_t_a │ │ │ │ │ +store all the sizes │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e │ │ │ │ │ +Bayes tree. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_n_o_d_e_s__ │ │ │ │ │ +Nodes nodes_ │ │ │ │ │ +Map from indices to Clique. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_r_e_m_o_v_e_C_l_i_q_u_e │ │ │ │ │ +void removeClique(sharedClique clique) │ │ │ │ │ +remove a clique: warning, can result in a forest │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:424 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_j_o_i_n_t │ │ │ │ │ +sharedFactorGraph joint(Key j1, Key j2, const Eliminate │ │ │ │ │ +&function=EliminationTraitsType::DefaultEliminate) const │ │ │ │ │ +return joint on two variables Limitation: can only calculate joint if cliques │ │ │ │ │ +are disjoint or one of ... │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:293 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_f_i_l_l_N_o_d_e_s_I_n_d_e_x │ │ │ │ │ +void fillNodesIndex(const sharedClique &subtree) │ │ │ │ │ +Fill the nodes index for a subtree. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:246 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_d_o_t │ │ │ │ │ +void dot(std::ostream &os, const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ +Output to graphviz format, stream version. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_a_d_d_F_a_c_t_o_r_s_T_o_G_r_a_p_h │ │ │ │ │ +void addFactorsToGraph(FactorGraph< FactorType > *graph) const │ │ │ │ │ +Add all cliques in this BayesTree to the specified factor graph. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:168 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const This &other, double tol=1e-9) const │ │ │ │ │ +check equality │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:230 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ +This & operator=(const This &other) │ │ │ │ │ +Assignment operator. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:199 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_s_h_a_r_e_d_C_l_i_q_u_e │ │ │ │ │ +boost::shared_ptr< Clique > sharedClique │ │ │ │ │ +Shared pointer to a clique. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_B_a_y_e_s_T_r_e_e │ │ │ │ │ +BayesTree() │ │ │ │ │ +Create an empty Bayes Tree. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:109 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_c_l_e_a_r │ │ │ │ │ +void clear() │ │ │ │ │ +Remove all nodes. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:408 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_a_d_d_C_l_i_q_u_e │ │ │ │ │ +void addClique(const sharedClique &clique, const sharedClique │ │ │ │ │ +&parent_clique=sharedClique()) │ │ │ │ │ +add a clique (top down) │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:142 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_j_o_i_n_t_B_a_y_e_s_N_e_t │ │ │ │ │ +sharedBayesNet jointBayesNet(Key j1, Key j2, const Eliminate │ │ │ │ │ +&function=EliminationTraitsType::DefaultEliminate) const │ │ │ │ │ +return joint on two variables as a BayesNet Limitation: can only calculate │ │ │ │ │ +joint if cliques are disjo... │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:302 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_f_i_n_d_P_a_r_e_n_t_C_l_i_q_u_e │ │ │ │ │ +Key findParentClique(const CONTAINER &parents) const │ │ │ │ │ +Find parent clique of a conditional. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:238 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_s_i_z_e │ │ │ │ │ +size_t size() const │ │ │ │ │ +number of cliques │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:133 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_d_e_l_e_t_e_C_a_c_h_e_d_S_h_o_r_t_c_u_t_s │ │ │ │ │ +void deleteCachedShortcuts() │ │ │ │ │ +Clear all shortcut caches - use before timing on marginal calculation to avoid │ │ │ │ │ +residual cache data. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:416 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_r_e_m_o_v_e_P_a_t_h │ │ │ │ │ +void removePath(sharedClique clique, BayesNetType *bn, Cliques *orphans) │ │ │ │ │ +Remove path from clique to root and return that path as factors plus a list of │ │ │ │ │ +orphaned subtree roots... │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:448 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_m_a_r_g_i_n_a_l_F_a_c_t_o_r │ │ │ │ │ +sharedConditional marginalFactor(Key j, const Eliminate │ │ │ │ │ +&function=EliminationTraitsType::DefaultEliminate) const │ │ │ │ │ +Return marginal on any variable. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:270 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_n_u_m_C_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l_s │ │ │ │ │ +size_t numCachedSeparatorMarginals() const │ │ │ │ │ +Collect number of cliques with cached separator marginals. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_g_e_t_C_l_i_q_u_e_D_a_t_a │ │ │ │ │ +BayesTreeCliqueData getCliqueData() const │ │ │ │ │ +Gather data on all cliques. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_r_e_m_o_v_e_S_u_b_t_r_e_e │ │ │ │ │ +Cliques removeSubtree(const sharedClique &subtree) │ │ │ │ │ +Remove the requested subtree. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:496 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:212 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_i_n_s_e_r_t_R_o_o_t │ │ │ │ │ +void insertRoot(const sharedClique &subtree) │ │ │ │ │ +Insert a new subtree with known parent clique. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:260 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_s_a_v_e_G_r_a_p_h │ │ │ │ │ +void saveGraph(const std::string &filename, const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ +output to file with graphviz format. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_r_e_m_o_v_e_T_o_p │ │ │ │ │ +void removeTop(const KeyVector &keys, BayesNetType *bn, Cliques *orphans) │ │ │ │ │ +Given a list of indices, turn "contaminated" part of the tree back into a │ │ │ │ │ +factor graph. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:475 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _B_a_y_e_s_N_e_t_._h │ │ │ │ │ + * _B_a_y_e_s_T_r_e_e_-_i_n_s_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00650.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/VariableIndex.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/JunctionTree-inst.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,32 +94,50 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
VariableIndex.cpp File Reference
│ │ │ │ +
JunctionTree-inst.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ + │ │ │ │ +

The junction tree, template bodies. │ │ │ │ +More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

struct  gtsam::ConstructorTraversalData< BAYESTREE, GRAPH, ETREE_NODE >
 
class  gtsam::ConstructorTraversalData< BAYESTREE, GRAPH, ETREE_NODE >::SymbolicFactors
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
March 26, 2013
│ │ │ │ +

The junction tree, template bodies.

│ │ │ │ +
Date
Feb 4, 2010
│ │ │ │ +
Author
Kai Ni
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,31 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -VariableIndex.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +JunctionTree-inst.h File Reference │ │ │ │ │ +The junction tree, template bodies. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +struct   _g_t_s_a_m_:_:_C_o_n_s_t_r_u_c_t_o_r_T_r_a_v_e_r_s_a_l_D_a_t_a_<_ _B_A_Y_E_S_T_R_E_E_,_ _G_R_A_P_H_,_ _E_T_R_E_E___N_O_D_E_ _> │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_C_o_n_s_t_r_u_c_t_o_r_T_r_a_v_e_r_s_a_l_D_a_t_a_<_ _B_A_Y_E_S_T_R_E_E_,_ _G_R_A_P_H_,_ _E_T_R_E_E___N_O_D_E_ _>_:_: │ │ │ │ │ + _S_y_m_b_o_l_i_c_F_a_c_t_o_r_s │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +The junction tree, template bodies. │ │ │ │ │ + Date │ │ │ │ │ + Feb 4, 2010 │ │ │ │ │ Author │ │ │ │ │ + Kai Ni │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ - Date │ │ │ │ │ - March 26, 2013 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _V_a_r_i_a_b_l_e_I_n_d_e_x_._c_p_p │ │ │ │ │ + * _J_u_n_c_t_i_o_n_T_r_e_e_-_i_n_s_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00653.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/EliminationTree.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/ISAM.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,43 +96,42 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
EliminationTree.h File Reference
│ │ │ │ +
ISAM.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ +

Incremental update functionality (iSAM) for BayesTree. │ │ │ │ +More...

│ │ │ │ + │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::EliminationTree< BAYESNET, GRAPH >
 An elimination tree is a data structure used intermediately during elimination. More...
 
struct  gtsam::EliminationTree< BAYESNET, GRAPH >::Node
class  gtsam::ISAM< BAYESTREE >
 A Bayes tree with an update methods that implements the iSAM algorithm. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ -
Date
Oct 13, 2010
│ │ │ │ +

Incremental update functionality (iSAM) for BayesTree.

│ │ │ │ +
Author
Michael Kaess
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,29 +1,26 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -EliminationTree.h File Reference │ │ │ │ │ +ISAM.h File Reference │ │ │ │ │ +Incremental update functionality (iSAM) for BayesTree. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_ _B_A_Y_E_S_N_E_T_,_ _G_R_A_P_H_ _> │ │ │ │ │ -  An elimination tree is a data structure used intermediately during │ │ │ │ │ - elimination. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_ _B_A_Y_E_S_N_E_T_,_ _G_R_A_P_H_ _>_:_:_N_o_d_e │ │ │ │ │ +class   _g_t_s_a_m_:_:_I_S_A_M_<_ _B_A_Y_E_S_T_R_E_E_ _> │ │ │ │ │ +  A Bayes tree with an update methods that implements the iSAM algorithm. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Incremental update functionality (iSAM) for BayesTree. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Date │ │ │ │ │ - Oct 13, 2010 │ │ │ │ │ + Michael Kaess │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h │ │ │ │ │ + * _I_S_A_M_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00653.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,3 @@ │ │ │ │ │ var a00653 = [ │ │ │ │ │ - ["gtsam::EliminationTree< BAYESNET, GRAPH >", "a03584.html", "a03584"], │ │ │ │ │ - ["gtsam::EliminationTree< BAYESNET, GRAPH >::Node", "a03588.html", "a03588"] │ │ │ │ │ + ["gtsam::ISAM< BAYESTREE >", "a03640.html", "a03640"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00653_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/EliminationTree.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/ISAM.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,160 +98,80 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
EliminationTree.h
│ │ │ │ +
ISAM.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3* GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ -
4* Atlanta, Georgia 30332-0415
│ │ │ │ -
5* All Rights Reserved
│ │ │ │ -
6* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
4 * Atlanta, Georgia 30332-0415
│ │ │ │ +
5 * All Rights Reserved
│ │ │ │ +
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │ -
8* See LICENSE for the license information
│ │ │ │ +
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │ -
10* -------------------------------------------------------------------------- */
│ │ │ │ +
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <utility>
│ │ │ │ -
21#include <boost/shared_ptr.hpp>
│ │ │ │ +
18// \callgraph
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/global_includes.h>
│ │ │ │
22
│ │ │ │ -
23#include <gtsam/base/Testable.h>
│ │ │ │ -
24#include <gtsam/base/FastVector.h>
│ │ │ │ -
25
│ │ │ │ -
26class EliminationTreeTester; // for unit tests, see testEliminationTree
│ │ │ │ -
27
│ │ │ │ -
28namespace gtsam {
│ │ │ │ -
29
│ │ │ │ -
30 class VariableIndex;
│ │ │ │ -
31 class Ordering;
│ │ │ │ -
32
│ │ │ │ -
50 template<class BAYESNET, class GRAPH>
│ │ │ │ -
│ │ │ │ -
51 class EliminationTree
│ │ │ │ -
52 {
│ │ │ │ -
53 protected:
│ │ │ │ -
54 typedef EliminationTree<BAYESNET, GRAPH> This;
│ │ │ │ -
55 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
56
│ │ │ │ -
57 public:
│ │ │ │ -
58 typedef GRAPH FactorGraphType;
│ │ │ │ -
59 typedef typename GRAPH::FactorType FactorType;
│ │ │ │ -
60 typedef typename boost::shared_ptr<FactorType> sharedFactor;
│ │ │ │ -
61 typedef BAYESNET BayesNetType;
│ │ │ │ -
62 typedef typename BayesNetType::ConditionalType ConditionalType;
│ │ │ │ -
63 typedef typename boost::shared_ptr<ConditionalType> sharedConditional;
│ │ │ │ -
64 typedef typename GRAPH::Eliminate Eliminate;
│ │ │ │ -
65
│ │ │ │ -
│ │ │ │ -
66 struct Node {
│ │ │ │ -
67 typedef FastVector<sharedFactor> Factors;
│ │ │ │ -
68 typedef FastVector<boost::shared_ptr<Node> > Children;
│ │ │ │ -
69
│ │ │ │ -
70 Key key;
│ │ │ │ -
71 Factors factors;
│ │ │ │ -
72 Children children;
│ │ │ │ +
23namespace gtsam {
│ │ │ │ +
24
│ │ │ │ +
30template <class BAYESTREE>
│ │ │ │ +
│ │ │ │ +
31class ISAM : public BAYESTREE {
│ │ │ │ +
32 public:
│ │ │ │ +
33 typedef BAYESTREE Base;
│ │ │ │ +
34 typedef typename Base::BayesNetType BayesNetType;
│ │ │ │ +
35 typedef typename Base::FactorGraphType FactorGraphType;
│ │ │ │ +
36 typedef typename Base::Clique Clique;
│ │ │ │ +
37 typedef typename Base::sharedClique sharedClique;
│ │ │ │ +
38 typedef typename Base::Cliques Cliques;
│ │ │ │ +
39
│ │ │ │ +
40 private:
│ │ │ │ +
41 typedef typename Base::Eliminate Eliminate;
│ │ │ │ +
42 typedef typename Base::EliminationTraitsType EliminationTraitsType;
│ │ │ │ +
43
│ │ │ │ +
44 public:
│ │ │ │ +
47
│ │ │ │ +
49 ISAM() {}
│ │ │ │ +
50
│ │ │ │ +
52 explicit ISAM(const Base& bayesTree) : Base(bayesTree) {}
│ │ │ │ +
53
│ │ │ │ +
57
│ │ │ │ +
64 void update(
│ │ │ │ +
65 const FactorGraphType& newFactors,
│ │ │ │ +
66 const Eliminate& function = EliminationTraitsType::DefaultEliminate);
│ │ │ │ +
67
│ │ │ │ +
70 void updateInternal(
│ │ │ │ +
71 const FactorGraphType& newFactors, Cliques* orphans,
│ │ │ │ +
72 const Eliminate& function = EliminationTraitsType::DefaultEliminate);
│ │ │ │
73
│ │ │ │ -
74 sharedFactor eliminate(const boost::shared_ptr<BayesNetType>& output,
│ │ │ │ -
75 const Eliminate& function, const FastVector<sharedFactor>& childrenFactors) const;
│ │ │ │ -
76
│ │ │ │ -
77 void print(const std::string& str, const KeyFormatter& keyFormatter) const;
│ │ │ │ -
78 };
│ │ │ │ -
│ │ │ │ -
79
│ │ │ │ -
80 typedef boost::shared_ptr<Node> sharedNode;
│ │ │ │ -
81
│ │ │ │ -
82 protected:
│ │ │ │ -
84 GTSAM_CONCEPT_TESTABLE_TYPE(FactorType)
│ │ │ │ -
85
│ │ │ │ -
86 FastVector<sharedNode> roots_;
│ │ │ │ -
87 FastVector<sharedFactor> remainingFactors_;
│ │ │ │ -
88
│ │ │ │ -
91
│ │ │ │ -
100 EliminationTree(const FactorGraphType& factorGraph,
│ │ │ │ -
101 const VariableIndex& structure, const Ordering& order);
│ │ │ │ -
102
│ │ │ │ -
108 EliminationTree(const FactorGraphType& factorGraph, const Ordering& order);
│ │ │ │ -
109
│ │ │ │ -
112 EliminationTree(const This& other) { *this = other; }
│ │ │ │ -
113
│ │ │ │ -
116 This& operator=(const This& other);
│ │ │ │ -
117
│ │ │ │ -
119
│ │ │ │ -
120 public:
│ │ │ │ -
123
│ │ │ │ -
129 std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> >
│ │ │ │ -
130 eliminate(Eliminate function) const;
│ │ │ │ -
131
│ │ │ │ -
135
│ │ │ │ -
137 void print(const std::string& name = "EliminationTree: ",
│ │ │ │ -
138 const KeyFormatter& formatter = DefaultKeyFormatter) const;
│ │ │ │ -
139
│ │ │ │ -
140 protected:
│ │ │ │ -
142 bool equals(const This& other, double tol = 1e-9) const;
│ │ │ │ -
143
│ │ │ │ -
145
│ │ │ │ -
146 public:
│ │ │ │ -
149
│ │ │ │ -
151 const FastVector<sharedNode>& roots() const { return roots_; }
│ │ │ │ -
152
│ │ │ │ -
154 const FastVector<sharedFactor>& remainingFactors() const { return remainingFactors_; }
│ │ │ │ -
155
│ │ │ │ -
157 void swap(This& other);
│ │ │ │ -
158
│ │ │ │ -
159 protected:
│ │ │ │ -
161 EliminationTree() {}
│ │ │ │ -
162
│ │ │ │ -
163 private:
│ │ │ │ -
165 friend class ::EliminationTreeTester;
│ │ │ │ -
166 };
│ │ │ │ +
75};
│ │ │ │
│ │ │ │ -
167
│ │ │ │ -
168}
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ -
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ -
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │ +
76
│ │ │ │ +
77} // namespace gtsam
│ │ │ │ +
global_includes.h
Included from all GTSAM files.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::EliminationTree
An elimination tree is a data structure used intermediately during elimination.
Definition EliminationTree.h:52
│ │ │ │ -
gtsam::EliminationTree::print
void print(const std::string &name="EliminationTree: ", const KeyFormatter &formatter=DefaultKeyFormatter) const
Print the tree to cout.
Definition EliminationTree-inst.h:207
│ │ │ │ -
gtsam::EliminationTree::eliminate
std::pair< boost::shared_ptr< BayesNetType >, boost::shared_ptr< FactorGraphType > > eliminate(Eliminate function) const
Eliminate the factors to a Bayes net and remaining factor graph.
Definition EliminationTree-inst.h:187
│ │ │ │ -
gtsam::EliminationTree::operator=
This & operator=(const This &other)
Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are copie...
Definition EliminationTree-inst.h:172
│ │ │ │ -
gtsam::EliminationTree::This
EliminationTree< BAYESNET, GRAPH > This
This class.
Definition EliminationTree.h:54
│ │ │ │ -
gtsam::EliminationTree::roots_
FastVector< sharedNode > roots_
concept check
Definition EliminationTree.h:86
│ │ │ │ -
gtsam::EliminationTree::sharedFactor
boost::shared_ptr< FactorType > sharedFactor
Shared pointer to a factor.
Definition EliminationTree.h:60
│ │ │ │ -
gtsam::EliminationTree::BayesNetType
BAYESNET BayesNetType
The BayesNet corresponding to FACTOR.
Definition EliminationTree.h:61
│ │ │ │ -
gtsam::EliminationTree::sharedConditional
boost::shared_ptr< ConditionalType > sharedConditional
Shared pointer to a conditional.
Definition EliminationTree.h:63
│ │ │ │ -
gtsam::EliminationTree::roots
const FastVector< sharedNode > & roots() const
Return the set of roots (one for a tree, multiple for a forest)
Definition EliminationTree.h:151
│ │ │ │ -
gtsam::EliminationTree::FactorGraphType
GRAPH FactorGraphType
The factor graph type.
Definition EliminationTree.h:58
│ │ │ │ -
gtsam::EliminationTree::swap
void swap(This &other)
Swap the data of this tree with another one, this operation is very fast.
Definition EliminationTree-inst.h:283
│ │ │ │ -
gtsam::EliminationTree::EliminationTree
EliminationTree()
Protected default constructor.
Definition EliminationTree.h:161
│ │ │ │ -
gtsam::EliminationTree::ConditionalType
BayesNetType::ConditionalType ConditionalType
The type of conditionals.
Definition EliminationTree.h:62
│ │ │ │ -
gtsam::EliminationTree::shared_ptr
boost::shared_ptr< This > shared_ptr
Shared pointer to this class.
Definition EliminationTree.h:55
│ │ │ │ -
gtsam::EliminationTree::FactorType
GRAPH::FactorType FactorType
The type of factors.
Definition EliminationTree.h:59
│ │ │ │ -
gtsam::EliminationTree::sharedNode
boost::shared_ptr< Node > sharedNode
Shared pointer to Node.
Definition EliminationTree.h:80
│ │ │ │ -
gtsam::EliminationTree::remainingFactors
const FastVector< sharedFactor > & remainingFactors() const
Return the remaining factors that are not pulled into elimination.
Definition EliminationTree.h:154
│ │ │ │ -
gtsam::EliminationTree::Node
Definition EliminationTree.h:66
│ │ │ │ -
gtsam::EliminationTree::Node::key
Key key
key associated with root
Definition EliminationTree.h:70
│ │ │ │ -
gtsam::EliminationTree::Node::children
Children children
sub-trees
Definition EliminationTree.h:72
│ │ │ │ -
gtsam::EliminationTree::Node::factors
Factors factors
factors associated with root
Definition EliminationTree.h:71
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ -
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition VariableIndex.h:43
│ │ │ │ +
gtsam::ISAM
A Bayes tree with an update methods that implements the iSAM algorithm.
Definition ISAM.h:31
│ │ │ │ +
gtsam::ISAM::update
void update(const FactorGraphType &newFactors, const Eliminate &function=EliminationTraitsType::DefaultEliminate)
update the Bayes tree with a set of new factors, typically derived from measurements
Definition ISAM-inst.h:62
│ │ │ │ +
gtsam::ISAM::ISAM
ISAM(const Base &bayesTree)
Copy constructor.
Definition ISAM.h:52
│ │ │ │ +
gtsam::ISAM::ISAM
ISAM()
Create an empty Bayes Tree.
Definition ISAM.h:49
│ │ │ │ +
gtsam::ISAM::updateInternal
void updateInternal(const FactorGraphType &newFactors, Cliques *orphans, const Eliminate &function=EliminationTraitsType::DefaultEliminate)
updateInternal provides access to list of orphans for drawing purposes
Definition ISAM-inst.h:27
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,237 +1,90 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -EliminationTree.h │ │ │ │ │ +ISAM.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3* GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ -4* Atlanta, Georgia 30332-0415 │ │ │ │ │ -5* All Rights Reserved │ │ │ │ │ -6* Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +5 * All Rights Reserved │ │ │ │ │ +6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ -8* See LICENSE for the license information │ │ │ │ │ +8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ -10* ------------------------------------------------------------------------- │ │ │ │ │ +10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include │ │ │ │ │ -21#include │ │ │ │ │ +18// \callgraph │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ 22 │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ -25 │ │ │ │ │ -26class EliminationTreeTester; // for unit tests, see testEliminationTree │ │ │ │ │ -27 │ │ │ │ │ -28namespace _g_t_s_a_m { │ │ │ │ │ -29 │ │ │ │ │ -30 class VariableIndex; │ │ │ │ │ -31 class Ordering; │ │ │ │ │ -32 │ │ │ │ │ -50 template │ │ │ │ │ -_5_1 class _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ -52 { │ │ │ │ │ -53 protected: │ │ │ │ │ -_5_4 typedef _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_B_A_Y_E_S_N_E_T_,_ _G_R_A_P_H_> _T_h_i_s; │ │ │ │ │ -_5_5 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -56 │ │ │ │ │ -57 public: │ │ │ │ │ -_5_8 typedef GRAPH _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e; │ │ │ │ │ -_5_9 typedef typename GRAPH::FactorType _F_a_c_t_o_r_T_y_p_e; │ │ │ │ │ -_6_0 typedef typename boost::shared_ptr _s_h_a_r_e_d_F_a_c_t_o_r; │ │ │ │ │ -_6_1 typedef BAYESNET _B_a_y_e_s_N_e_t_T_y_p_e; │ │ │ │ │ -_6_2 typedef typename BayesNetType::ConditionalType _C_o_n_d_i_t_i_o_n_a_l_T_y_p_e; │ │ │ │ │ -_6_3 typedef typename boost::shared_ptr _s_h_a_r_e_d_C_o_n_d_i_t_i_o_n_a_l; │ │ │ │ │ -64 typedef typename GRAPH::Eliminate Eliminate; │ │ │ │ │ -65 │ │ │ │ │ -_6_6 struct _N_o_d_e { │ │ │ │ │ -67 typedef _F_a_s_t_V_e_c_t_o_r_<_s_h_a_r_e_d_F_a_c_t_o_r_> Factors; │ │ │ │ │ -68 typedef _F_a_s_t_V_e_c_t_o_r_<_b_o_o_s_t_:_:_s_h_a_r_e_d___p_t_r_<_N_o_d_e_> > Children; │ │ │ │ │ -69 │ │ │ │ │ -_7_0 _K_e_y _k_e_y; │ │ │ │ │ -_7_1 Factors _f_a_c_t_o_r_s; │ │ │ │ │ -_7_2 Children _c_h_i_l_d_r_e_n; │ │ │ │ │ +23namespace _g_t_s_a_m { │ │ │ │ │ +24 │ │ │ │ │ +30template │ │ │ │ │ +_3_1class _I_S_A_M : public BAYESTREE { │ │ │ │ │ +32 public: │ │ │ │ │ +33 typedef BAYESTREE Base; │ │ │ │ │ +34 typedef typename Base::BayesNetType BayesNetType; │ │ │ │ │ +35 typedef typename Base::FactorGraphType FactorGraphType; │ │ │ │ │ +36 typedef typename Base::Clique Clique; │ │ │ │ │ +37 typedef typename Base::sharedClique sharedClique; │ │ │ │ │ +38 typedef typename Base::Cliques Cliques; │ │ │ │ │ +39 │ │ │ │ │ +40 private: │ │ │ │ │ +41 typedef typename Base::Eliminate Eliminate; │ │ │ │ │ +42 typedef typename Base::EliminationTraitsType EliminationTraitsType; │ │ │ │ │ +43 │ │ │ │ │ +44 public: │ │ │ │ │ +47 │ │ │ │ │ +_4_9 _I_S_A_M() {} │ │ │ │ │ +50 │ │ │ │ │ +_5_2 explicit _I_S_A_M(const Base& bayesTree) : Base(bayesTree) {} │ │ │ │ │ +53 │ │ │ │ │ +57 │ │ │ │ │ +64 void _u_p_d_a_t_e( │ │ │ │ │ +65 const FactorGraphType& newFactors, │ │ │ │ │ +66 const Eliminate& function = EliminationTraitsType::DefaultEliminate); │ │ │ │ │ +67 │ │ │ │ │ +70 void _u_p_d_a_t_e_I_n_t_e_r_n_a_l( │ │ │ │ │ +71 const FactorGraphType& newFactors, Cliques* orphans, │ │ │ │ │ +72 const Eliminate& function = EliminationTraitsType::DefaultEliminate); │ │ │ │ │ 73 │ │ │ │ │ -74 _s_h_a_r_e_d_F_a_c_t_o_r eliminate(const boost::shared_ptr& output, │ │ │ │ │ -75 const Eliminate& function, const _F_a_s_t_V_e_c_t_o_r_<_s_h_a_r_e_d_F_a_c_t_o_r_>& childrenFactors) │ │ │ │ │ -const; │ │ │ │ │ +75}; │ │ │ │ │ 76 │ │ │ │ │ -77 void print(const std::string& str, const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter) const; │ │ │ │ │ -78 }; │ │ │ │ │ -79 │ │ │ │ │ -_8_0 typedef boost::shared_ptr _s_h_a_r_e_d_N_o_d_e; │ │ │ │ │ -81 │ │ │ │ │ -82 protected: │ │ │ │ │ -84 GTSAM_CONCEPT_TESTABLE_TYPE(_F_a_c_t_o_r_T_y_p_e) │ │ │ │ │ -85 │ │ │ │ │ -_8_6 _F_a_s_t_V_e_c_t_o_r<_s_h_a_r_e_d_N_o_d_e> _r_o_o_t_s__; │ │ │ │ │ -87 _F_a_s_t_V_e_c_t_o_r<_s_h_a_r_e_d_F_a_c_t_o_r> remainingFactors_; │ │ │ │ │ -88 │ │ │ │ │ -91 │ │ │ │ │ -100 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e(const _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e& factorGraph, │ │ │ │ │ -101 const _V_a_r_i_a_b_l_e_I_n_d_e_x& structure, const _O_r_d_e_r_i_n_g& order); │ │ │ │ │ -102 │ │ │ │ │ -108 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e(const _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e& factorGraph, const _O_r_d_e_r_i_n_g& order); │ │ │ │ │ -109 │ │ │ │ │ -_1_1_2 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e(const _T_h_i_s& other) { *this = other; } │ │ │ │ │ -113 │ │ │ │ │ -116 _T_h_i_s& _o_p_e_r_a_t_o_r_=(const _T_h_i_s& other); │ │ │ │ │ -117 │ │ │ │ │ -119 │ │ │ │ │ -120 public: │ │ │ │ │ -123 │ │ │ │ │ -129 std::pair, boost:: │ │ │ │ │ -shared_ptr > │ │ │ │ │ -130 _e_l_i_m_i_n_a_t_e(Eliminate function) const; │ │ │ │ │ -131 │ │ │ │ │ -135 │ │ │ │ │ -137 void _p_r_i_n_t(const std::string& name = "EliminationTree: ", │ │ │ │ │ -138 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const; │ │ │ │ │ -139 │ │ │ │ │ -140 protected: │ │ │ │ │ -142 bool _e_q_u_a_l_s(const _T_h_i_s& other, double tol = 1e-9) const; │ │ │ │ │ -143 │ │ │ │ │ -145 │ │ │ │ │ -146 public: │ │ │ │ │ -149 │ │ │ │ │ -_1_5_1 const _F_a_s_t_V_e_c_t_o_r_<_s_h_a_r_e_d_N_o_d_e_>& _r_o_o_t_s() const { return _r_o_o_t_s__; } │ │ │ │ │ -152 │ │ │ │ │ -_1_5_4 const _F_a_s_t_V_e_c_t_o_r_<_s_h_a_r_e_d_F_a_c_t_o_r_>& _r_e_m_a_i_n_i_n_g_F_a_c_t_o_r_s() const { return │ │ │ │ │ -remainingFactors_; } │ │ │ │ │ -155 │ │ │ │ │ -157 void _s_w_a_p(_T_h_i_s& other); │ │ │ │ │ -158 │ │ │ │ │ -159 protected: │ │ │ │ │ -_1_6_1 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e() {} │ │ │ │ │ -162 │ │ │ │ │ -163 private: │ │ │ │ │ -_1_6_5 friend class ::EliminationTreeTester; │ │ │ │ │ -166 }; │ │ │ │ │ -167 │ │ │ │ │ -168} │ │ │ │ │ -_T_e_s_t_a_b_l_e_._h │ │ │ │ │ -Concept check for values that can be used in unit tests. │ │ │ │ │ -_F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ -A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ -std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ -FastVector │ │ │ │ │ -FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ -DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ +77} // namespace gtsam │ │ │ │ │ +_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ +Included from all GTSAM files. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ -An elimination tree is a data structure used intermediately during elimination. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &name="EliminationTree: ", const KeyFormatter │ │ │ │ │ -&formatter=DefaultKeyFormatter) const │ │ │ │ │ -Print the tree to cout. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree-inst.h:207 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_e_l_i_m_i_n_a_t_e │ │ │ │ │ -std::pair< boost::shared_ptr< BayesNetType >, boost::shared_ptr< │ │ │ │ │ -FactorGraphType > > eliminate(Eliminate function) const │ │ │ │ │ -Eliminate the factors to a Bayes net and remaining factor graph. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree-inst.h:187 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ -This & operator=(const This &other) │ │ │ │ │ -Assignment operator - makes a deep copy of the tree structure, but only │ │ │ │ │ -pointers to factors are copie... │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree-inst.h:172 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_T_h_i_s │ │ │ │ │ -EliminationTree< BAYESNET, GRAPH > This │ │ │ │ │ -This class. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:54 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_r_o_o_t_s__ │ │ │ │ │ -FastVector< sharedNode > roots_ │ │ │ │ │ -concept check │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_s_h_a_r_e_d_F_a_c_t_o_r │ │ │ │ │ -boost::shared_ptr< FactorType > sharedFactor │ │ │ │ │ -Shared pointer to a factor. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:60 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_B_a_y_e_s_N_e_t_T_y_p_e │ │ │ │ │ -BAYESNET BayesNetType │ │ │ │ │ -The BayesNet corresponding to FACTOR. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:61 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_s_h_a_r_e_d_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -boost::shared_ptr< ConditionalType > sharedConditional │ │ │ │ │ -Shared pointer to a conditional. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:63 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_r_o_o_t_s │ │ │ │ │ -const FastVector< sharedNode > & roots() const │ │ │ │ │ -Return the set of roots (one for a tree, multiple for a forest) │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_F_a_c_t_o_r_G_r_a_p_h_T_y_p_e │ │ │ │ │ -GRAPH FactorGraphType │ │ │ │ │ -The factor graph type. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_s_w_a_p │ │ │ │ │ -void swap(This &other) │ │ │ │ │ -Swap the data of this tree with another one, this operation is very fast. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree-inst.h:283 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ -EliminationTree() │ │ │ │ │ -Protected default constructor. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:161 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_C_o_n_d_i_t_i_o_n_a_l_T_y_p_e │ │ │ │ │ -BayesNetType::ConditionalType ConditionalType │ │ │ │ │ -The type of conditionals. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:62 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -Shared pointer to this class. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_F_a_c_t_o_r_T_y_p_e │ │ │ │ │ -GRAPH::FactorType FactorType │ │ │ │ │ -The type of factors. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:59 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_s_h_a_r_e_d_N_o_d_e │ │ │ │ │ -boost::shared_ptr< Node > sharedNode │ │ │ │ │ -Shared pointer to Node. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:80 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_r_e_m_a_i_n_i_n_g_F_a_c_t_o_r_s │ │ │ │ │ -const FastVector< sharedFactor > & remainingFactors() const │ │ │ │ │ -Return the remaining factors that are not pulled into elimination. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:154 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_N_o_d_e │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:66 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_N_o_d_e_:_:_k_e_y │ │ │ │ │ -Key key │ │ │ │ │ -key associated with root │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:70 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_N_o_d_e_:_:_c_h_i_l_d_r_e_n │ │ │ │ │ -Children children │ │ │ │ │ -sub-trees │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:72 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_N_o_d_e_:_:_f_a_c_t_o_r_s │ │ │ │ │ -Factors factors │ │ │ │ │ -factors associated with root │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:71 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ -The VariableIndex class computes and stores the block column structure of a │ │ │ │ │ -factor graph. │ │ │ │ │ -DDeeffiinniittiioonn VariableIndex.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M │ │ │ │ │ +A Bayes tree with an update methods that implements the iSAM algorithm. │ │ │ │ │ +DDeeffiinniittiioonn ISAM.h:31 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_:_:_u_p_d_a_t_e │ │ │ │ │ +void update(const FactorGraphType &newFactors, const Eliminate │ │ │ │ │ +&function=EliminationTraitsType::DefaultEliminate) │ │ │ │ │ +update the Bayes tree with a set of new factors, typically derived from │ │ │ │ │ +measurements │ │ │ │ │ +DDeeffiinniittiioonn ISAM-inst.h:62 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_:_:_I_S_A_M │ │ │ │ │ +ISAM(const Base &bayesTree) │ │ │ │ │ +Copy constructor. │ │ │ │ │ +DDeeffiinniittiioonn ISAM.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_:_:_I_S_A_M │ │ │ │ │ +ISAM() │ │ │ │ │ +Create an empty Bayes Tree. │ │ │ │ │ +DDeeffiinniittiioonn ISAM.h:49 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_:_:_u_p_d_a_t_e_I_n_t_e_r_n_a_l │ │ │ │ │ +void updateInternal(const FactorGraphType &newFactors, Cliques *orphans, const │ │ │ │ │ +Eliminate &function=EliminationTraitsType::DefaultEliminate) │ │ │ │ │ +updateInternal provides access to list of orphans for drawing purposes │ │ │ │ │ +DDeeffiinniittiioonn ISAM-inst.h:27 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h │ │ │ │ │ + * _I_S_A_M_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00656.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/VariableIndex-inl.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/FactorGraph.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,34 +94,59 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
VariableIndex-inl.h File Reference
│ │ │ │ +
FactorGraph.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ +

Factor Graph Base Class. │ │ │ │ +More...

│ │ │ │ + │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::CRefCallPushBack< C >
 Helper. More...
 
class  gtsam::RefCallPushBack< C >
 Helper. More...
 
class  gtsam::CRefCallAddCopy< C >
 Helper. More...
 
class  gtsam::FactorGraph< FACTOR >
 A factor graph is a bipartite graph with factor nodes connected to variable nodes. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
March 26, 2013
│ │ │ │ +

Factor Graph Base Class.

│ │ │ │ +
Author
Carlos Nieto
│ │ │ │ +
│ │ │ │ +Christian Potthast
│ │ │ │ +
│ │ │ │ +Michael Kaess
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,38 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -VariableIndex-inl.h File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +FactorGraph.h File Reference │ │ │ │ │ +Factor Graph Base Class. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_C_R_e_f_C_a_l_l_P_u_s_h_B_a_c_k_<_ _C_ _> │ │ │ │ │ +  Helper. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_R_e_f_C_a_l_l_P_u_s_h_B_a_c_k_<_ _C_ _> │ │ │ │ │ +  Helper. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_C_R_e_f_C_a_l_l_A_d_d_C_o_p_y_<_ _C_ _> │ │ │ │ │ +  Helper. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_<_ _F_A_C_T_O_R_ _> │ │ │ │ │ +  A factor graph is a bipartite graph with factor nodes connected to │ │ │ │ │ + variable nodes. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Factor Graph Base Class. │ │ │ │ │ Author │ │ │ │ │ + Carlos Nieto │ │ │ │ │ + Christian Potthast │ │ │ │ │ + Michael Kaess │ │ │ │ │ Richard Roberts │ │ │ │ │ - Date │ │ │ │ │ - March 26, 2013 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _V_a_r_i_a_b_l_e_I_n_d_e_x_-_i_n_l_._h │ │ │ │ │ + * _F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00656_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/VariableIndex-inl.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/FactorGraph.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,125 +98,431 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
VariableIndex-inl.h
│ │ │ │ +
FactorGraph.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/inference/VariableIndex.h>
│ │ │ │ -
21#include <gtsam/base/timing.h>
│ │ │ │ +
21// \callgraph
│ │ │ │
22
│ │ │ │ -
23namespace gtsam {
│ │ │ │ +
23#pragma once
│ │ │ │
24
│ │ │ │ -
25/* ************************************************************************* */
│ │ │ │ -
26template<class FG>
│ │ │ │ -
│ │ │ │ -
27void VariableIndex::augment(const FG& factors,
│ │ │ │ -
28 boost::optional<const FactorIndices&> newFactorIndices) {
│ │ │ │ -
29 gttic(VariableIndex_augment);
│ │ │ │ -
30
│ │ │ │ -
31 // Augment index for each factor
│ │ │ │ -
32 for (size_t i = 0; i < factors.size(); ++i) {
│ │ │ │ -
33 if (factors[i]) {
│ │ │ │ -
34 const size_t globalI =
│ │ │ │ -
35 newFactorIndices ? (*newFactorIndices)[i] : nFactors_;
│ │ │ │ -
36 for(const Key key: *factors[i]) {
│ │ │ │ -
37 index_[key].push_back(globalI);
│ │ │ │ -
38 ++nEntries_;
│ │ │ │ -
39 }
│ │ │ │ -
40 }
│ │ │ │ +
25#include <gtsam/inference/DotWriter.h>
│ │ │ │ +
26#include <gtsam/inference/Key.h>
│ │ │ │ +
27#include <gtsam/base/FastVector.h>
│ │ │ │ +
28#include <gtsam/base/Testable.h>
│ │ │ │ +
29
│ │ │ │ +
30#include <Eigen/Core> // for Eigen::aligned_allocator
│ │ │ │ +
31
│ │ │ │ +
32#include <boost/assign/list_inserter.hpp>
│ │ │ │ +
33#include <boost/make_shared.hpp>
│ │ │ │ +
34#include <boost/serialization/nvp.hpp>
│ │ │ │ +
35#include <boost/serialization/vector.hpp>
│ │ │ │ +
36
│ │ │ │ +
37#include <string>
│ │ │ │ +
38#include <type_traits>
│ │ │ │ +
39#include <utility>
│ │ │ │ +
40#include <iosfwd>
│ │ │ │
41
│ │ │ │ -
42 // Increment factor count even if factors are null, to keep indices consistent
│ │ │ │ -
43 if (newFactorIndices) {
│ │ │ │ -
44 if ((*newFactorIndices)[i] >= nFactors_)
│ │ │ │ -
45 nFactors_ = (*newFactorIndices)[i] + 1;
│ │ │ │ -
46 } else {
│ │ │ │ -
47 ++nFactors_;
│ │ │ │ -
48 }
│ │ │ │ -
49 }
│ │ │ │ -
50}
│ │ │ │ -
│ │ │ │ +
42namespace gtsam {
│ │ │ │ +
44typedef FastVector<FactorIndex> FactorIndices;
│ │ │ │ +
45
│ │ │ │ +
46// Forward declarations
│ │ │ │ +
47template <class CLIQUE>
│ │ │ │ +
48class BayesTree;
│ │ │ │ +
49
│ │ │ │ +
50class HybridValues;
│ │ │ │
51
│ │ │ │ -
52/* ************************************************************************* */
│ │ │ │ -
53template<typename ITERATOR, class FG>
│ │ │ │ -
│ │ │ │ -
54void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor,
│ │ │ │ -
55 const FG& factors) {
│ │ │ │ -
56 gttic(VariableIndex_remove);
│ │ │ │ -
57
│ │ │ │ -
58 // NOTE: We intentionally do not decrement nFactors_ because the factor
│ │ │ │ -
59 // indices need to remain consistent. Removing factors from a factor graph
│ │ │ │ -
60 // does not shift the indices of other factors. Also, we keep nFactors_
│ │ │ │ -
61 // one greater than the highest-numbered factor referenced in a VariableIndex.
│ │ │ │ -
62 ITERATOR factorIndex = firstFactor;
│ │ │ │ -
63 size_t i = 0;
│ │ │ │ -
64 for (; factorIndex != lastFactor; ++factorIndex, ++i) {
│ │ │ │ -
65 if (i >= factors.size())
│ │ │ │ -
66 throw std::invalid_argument(
│ │ │ │ -
67 "Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove");
│ │ │ │ -
68 if (factors[i]) {
│ │ │ │ -
69 for(Key j: *factors[i]) {
│ │ │ │ -
70 FactorIndices& factorEntries = internalAt(j);
│ │ │ │ -
71 auto entry = std::find(factorEntries.begin(),
│ │ │ │ -
72 factorEntries.end(), *factorIndex);
│ │ │ │ -
73 if (entry == factorEntries.end())
│ │ │ │ -
74 throw std::invalid_argument(
│ │ │ │ -
75 "Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index");
│ │ │ │ -
76 factorEntries.erase(entry);
│ │ │ │ -
77 --nEntries_;
│ │ │ │ -
78 }
│ │ │ │ -
79 }
│ │ │ │ -
80 }
│ │ │ │ -
81}
│ │ │ │ -
│ │ │ │ +
53template <class C>
│ │ │ │ +
│ │ │ │ +
54class CRefCallPushBack {
│ │ │ │ +
55 C& obj;
│ │ │ │ +
56
│ │ │ │ +
57 public:
│ │ │ │ +
58 explicit CRefCallPushBack(C& obj) : obj(obj) {}
│ │ │ │ +
59 template <typename A>
│ │ │ │ +
60 void operator()(const A& a) {
│ │ │ │ +
61 obj.push_back(a);
│ │ │ │ +
62 }
│ │ │ │ +
63};
│ │ │ │ +
│ │ │ │ +
64
│ │ │ │ +
66template <class C>
│ │ │ │ +
│ │ │ │ +
67class RefCallPushBack {
│ │ │ │ +
68 C& obj;
│ │ │ │ +
69
│ │ │ │ +
70 public:
│ │ │ │ +
71 explicit RefCallPushBack(C& obj) : obj(obj) {}
│ │ │ │ +
72 template <typename A>
│ │ │ │ +
73 void operator()(A& a) {
│ │ │ │ +
74 obj.push_back(a);
│ │ │ │ +
75 }
│ │ │ │ +
76};
│ │ │ │ +
│ │ │ │ +
77
│ │ │ │ +
79template <class C>
│ │ │ │ +
│ │ │ │ +
80class CRefCallAddCopy {
│ │ │ │ +
81 C& obj;
│ │ │ │
82
│ │ │ │ -
83/* ************************************************************************* */
│ │ │ │ -
84template<typename ITERATOR>
│ │ │ │ -
│ │ │ │ -
85void VariableIndex::removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey) {
│ │ │ │ -
86 for (ITERATOR key = firstKey; key != lastKey; ++key) {
│ │ │ │ -
87 KeyMap::iterator entry = index_.find(*key);
│ │ │ │ -
88 if (!entry->second.empty())
│ │ │ │ -
89 throw std::invalid_argument(
│ │ │ │ -
90 "Asking to remove variables from the variable index that are not unused");
│ │ │ │ -
91 index_.erase(entry);
│ │ │ │ -
92 }
│ │ │ │ -
93}
│ │ │ │ -
│ │ │ │ -
94
│ │ │ │ -
95}
│ │ │ │ -
timing.h
Timing utilities.
│ │ │ │ - │ │ │ │ +
83 public:
│ │ │ │ +
84 explicit CRefCallAddCopy(C& obj) : obj(obj) {}
│ │ │ │ +
85 template <typename A>
│ │ │ │ +
86 void operator()(const A& a) {
│ │ │ │ +
87 obj.addCopy(a);
│ │ │ │ +
88 }
│ │ │ │ +
89};
│ │ │ │ +
│ │ │ │ +
90
│ │ │ │ +
96template <class FACTOR>
│ │ │ │ +
│ │ │ │ +
97class FactorGraph {
│ │ │ │ +
98 public:
│ │ │ │ +
99 typedef FACTOR FactorType;
│ │ │ │ +
100 typedef boost::shared_ptr<FACTOR>
│ │ │ │ +
101 sharedFactor;
│ │ │ │ +
102 typedef sharedFactor value_type;
│ │ │ │ +
103 typedef typename FastVector<sharedFactor>::iterator iterator;
│ │ │ │ +
104 typedef typename FastVector<sharedFactor>::const_iterator const_iterator;
│ │ │ │ +
105
│ │ │ │ +
106 private:
│ │ │ │ +
107 typedef FactorGraph<FACTOR> This;
│ │ │ │ +
108 typedef boost::shared_ptr<This>
│ │ │ │ +
109 shared_ptr;
│ │ │ │ +
110
│ │ │ │ +
112 template <typename DERIVEDFACTOR>
│ │ │ │ +
113 using IsDerived = typename std::enable_if<
│ │ │ │ +
114 std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type;
│ │ │ │ +
115
│ │ │ │ +
117 template <typename T>
│ │ │ │ +
118 using HasDerivedValueType = typename std::enable_if<
│ │ │ │ +
119 std::is_base_of<FactorType, typename T::value_type>::value>::type;
│ │ │ │ +
120
│ │ │ │ +
122 template <typename T>
│ │ │ │ +
123 using HasDerivedElementType = typename std::enable_if<std::is_base_of<
│ │ │ │ +
124 FactorType, typename T::value_type::element_type>::value>::type;
│ │ │ │ +
125
│ │ │ │ +
126 protected:
│ │ │ │ +
128 GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
│ │ │ │ +
129
│ │ │ │ +
130
│ │ │ │ +
131 FastVector<sharedFactor> factors_;
│ │ │ │ +
132
│ │ │ │ +
│ │ │ │ +
134 bool isEqual(const FactorGraph& other) const {
│ │ │ │ +
135 return factors_ == other.factors_;
│ │ │ │ +
136 }
│ │ │ │ +
│ │ │ │ +
137
│ │ │ │ +
140
│ │ │ │ +
142 FactorGraph() {}
│ │ │ │ +
143
│ │ │ │ +
145 template <typename ITERATOR>
│ │ │ │ +
│ │ │ │ +
146 FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) {
│ │ │ │ +
147 push_back(firstFactor, lastFactor);
│ │ │ │ +
148 }
│ │ │ │ +
│ │ │ │ +
149
│ │ │ │ +
151 template <class CONTAINER>
│ │ │ │ +
│ │ │ │ +
152 explicit FactorGraph(const CONTAINER& factors) {
│ │ │ │ +
153 push_back(factors);
│ │ │ │ +
154 }
│ │ │ │ +
│ │ │ │ +
155
│ │ │ │ +
157
│ │ │ │ +
158 public:
│ │ │ │ +
161
│ │ │ │ +
164 virtual ~FactorGraph() = default;
│ │ │ │ +
165
│ │ │ │ +
170 template <class DERIVEDFACTOR, typename = IsDerived<DERIVEDFACTOR>>
│ │ │ │ +
│ │ │ │ +
171 FactorGraph(std::initializer_list<boost::shared_ptr<DERIVEDFACTOR>> sharedFactors)
│ │ │ │ +
172 : factors_(sharedFactors) {}
│ │ │ │ +
│ │ │ │ +
173
│ │ │ │ +
177
│ │ │ │ +
182 void reserve(size_t size) { factors_.reserve(size); }
│ │ │ │ +
183
│ │ │ │ +
185 template <class DERIVEDFACTOR>
│ │ │ │ +
│ │ │ │ +
186 IsDerived<DERIVEDFACTOR> push_back(boost::shared_ptr<DERIVEDFACTOR> factor) {
│ │ │ │ +
187 factors_.push_back(boost::shared_ptr<FACTOR>(factor));
│ │ │ │ +
188 }
│ │ │ │ +
│ │ │ │ +
189
│ │ │ │ +
191 template <class DERIVEDFACTOR, class... Args>
│ │ │ │ +
│ │ │ │ +
192 IsDerived<DERIVEDFACTOR> emplace_shared(Args&&... args) {
│ │ │ │ +
193 factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(
│ │ │ │ +
194 Eigen::aligned_allocator<DERIVEDFACTOR>(),
│ │ │ │ +
195 std::forward<Args>(args)...));
│ │ │ │ +
196 }
│ │ │ │ +
│ │ │ │ +
197
│ │ │ │ +
202 template <class DERIVEDFACTOR>
│ │ │ │ +
│ │ │ │ +
203 IsDerived<DERIVEDFACTOR> push_back(const DERIVEDFACTOR& factor) {
│ │ │ │ +
204 factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(
│ │ │ │ +
205 Eigen::aligned_allocator<DERIVEDFACTOR>(), factor));
│ │ │ │ +
206 }
│ │ │ │ +
│ │ │ │ +
207
│ │ │ │ +
209 template <class DERIVEDFACTOR>
│ │ │ │ +
│ │ │ │ +
210 IsDerived<DERIVEDFACTOR> add(boost::shared_ptr<DERIVEDFACTOR> factor) {
│ │ │ │ +
211 push_back(factor);
│ │ │ │ +
212 }
│ │ │ │ +
│ │ │ │ +
213
│ │ │ │ +
215 template <class DERIVEDFACTOR>
│ │ │ │ +
216 typename std::enable_if<
│ │ │ │ +
217 std::is_base_of<FactorType, DERIVEDFACTOR>::value,
│ │ │ │ +
218 boost::assign::list_inserter<RefCallPushBack<This>>>::type
│ │ │ │ +
│ │ │ │ +
219 operator+=(boost::shared_ptr<DERIVEDFACTOR> factor) {
│ │ │ │ +
220 return boost::assign::make_list_inserter(RefCallPushBack<This>(*this))(
│ │ │ │ +
221 factor);
│ │ │ │ +
222 }
│ │ │ │ +
│ │ │ │ +
223
│ │ │ │ +
227
│ │ │ │ +
232 template <typename ITERATOR>
│ │ │ │ +
│ │ │ │ +
233 HasDerivedElementType<ITERATOR> push_back(ITERATOR firstFactor,
│ │ │ │ +
234 ITERATOR lastFactor) {
│ │ │ │ +
235 factors_.insert(end(), firstFactor, lastFactor);
│ │ │ │ +
236 }
│ │ │ │ +
│ │ │ │ +
237
│ │ │ │ +
239 template <typename ITERATOR>
│ │ │ │ +
│ │ │ │ +
240 HasDerivedValueType<ITERATOR> push_back(ITERATOR firstFactor,
│ │ │ │ +
241 ITERATOR lastFactor) {
│ │ │ │ +
242 for (ITERATOR f = firstFactor; f != lastFactor; ++f) push_back(*f);
│ │ │ │ +
243 }
│ │ │ │ +
│ │ │ │ +
244
│ │ │ │ +
248
│ │ │ │ +
253 template <typename CONTAINER>
│ │ │ │ +
│ │ │ │ +
254 HasDerivedElementType<CONTAINER> push_back(const CONTAINER& container) {
│ │ │ │ +
255 push_back(container.begin(), container.end());
│ │ │ │ +
256 }
│ │ │ │ +
│ │ │ │ +
257
│ │ │ │ +
259 template <typename CONTAINER>
│ │ │ │ +
│ │ │ │ +
260 HasDerivedValueType<CONTAINER> push_back(const CONTAINER& container) {
│ │ │ │ +
261 push_back(container.begin(), container.end());
│ │ │ │ +
262 }
│ │ │ │ +
│ │ │ │ +
263
│ │ │ │ +
268 template <class FACTOR_OR_CONTAINER>
│ │ │ │ +
│ │ │ │ +
269 void add(const FACTOR_OR_CONTAINER& factorOrContainer) {
│ │ │ │ +
270 push_back(factorOrContainer);
│ │ │ │ +
271 }
│ │ │ │ +
│ │ │ │ +
272
│ │ │ │ +
277 template <class FACTOR_OR_CONTAINER>
│ │ │ │ +
│ │ │ │ +
278 boost::assign::list_inserter<CRefCallPushBack<This>> operator+=(
│ │ │ │ +
279 const FACTOR_OR_CONTAINER& factorOrContainer) {
│ │ │ │ +
280 return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this))(
│ │ │ │ +
281 factorOrContainer);
│ │ │ │ +
282 }
│ │ │ │ +
│ │ │ │ +
283
│ │ │ │ +
287
│ │ │ │ +
293 template <class CLIQUE>
│ │ │ │ +
294 typename std::enable_if<
│ │ │ │ +
295 std::is_base_of<This, typename CLIQUE::FactorGraphType>::value>::type
│ │ │ │ +
│ │ │ │ +
296 push_back(const BayesTree<CLIQUE>& bayesTree) {
│ │ │ │ +
297 bayesTree.addFactorsToGraph(this);
│ │ │ │ +
298 }
│ │ │ │ +
│ │ │ │ +
299
│ │ │ │ +
304 template <typename CONTAINER, typename = HasDerivedElementType<CONTAINER>>
│ │ │ │ +
305 FactorIndices add_factors(const CONTAINER& factors,
│ │ │ │ +
306 bool useEmptySlots = false);
│ │ │ │ +
307
│ │ │ │ +
311
│ │ │ │ +
313 virtual void print(const std::string& s = "FactorGraph",
│ │ │ │ +
314 const KeyFormatter& formatter = DefaultKeyFormatter) const;
│ │ │ │ +
315
│ │ │ │ +
317 bool equals(const This& fg, double tol = 1e-9) const;
│ │ │ │ +
319
│ │ │ │ +
320 public:
│ │ │ │ +
323
│ │ │ │ +
326 size_t size() const { return factors_.size(); }
│ │ │ │ +
327
│ │ │ │ +
330 bool empty() const { return factors_.empty(); }
│ │ │ │ +
331
│ │ │ │ +
335 const sharedFactor at(size_t i) const { return factors_.at(i); }
│ │ │ │ +
336
│ │ │ │ +
340 sharedFactor& at(size_t i) { return factors_.at(i); }
│ │ │ │ +
341
│ │ │ │ +
345 const sharedFactor operator[](size_t i) const { return at(i); }
│ │ │ │ +
346
│ │ │ │ +
350 sharedFactor& operator[](size_t i) { return at(i); }
│ │ │ │ +
351
│ │ │ │ +
353 const_iterator begin() const { return factors_.begin(); }
│ │ │ │ +
354
│ │ │ │ +
356 const_iterator end() const { return factors_.end(); }
│ │ │ │ +
357
│ │ │ │ +
359 sharedFactor front() const { return factors_.front(); }
│ │ │ │ +
360
│ │ │ │ +
362 sharedFactor back() const { return factors_.back(); }
│ │ │ │ +
363
│ │ │ │ +
365 double error(const HybridValues &values) const;
│ │ │ │ +
366
│ │ │ │ +
370
│ │ │ │ +
372 iterator begin() { return factors_.begin(); }
│ │ │ │ +
373
│ │ │ │ +
375 iterator end() { return factors_.end(); }
│ │ │ │ +
376
│ │ │ │ +
381 virtual void resize(size_t size) { factors_.resize(size); }
│ │ │ │ +
382
│ │ │ │ +
385 void remove(size_t i) { factors_.at(i).reset(); }
│ │ │ │ +
386
│ │ │ │ +
388 void replace(size_t index, sharedFactor factor) { at(index) = factor; }
│ │ │ │ +
389
│ │ │ │ +
391 iterator erase(iterator item) { return factors_.erase(item); }
│ │ │ │ +
392
│ │ │ │ +
│ │ │ │ +
394 iterator erase(iterator first, iterator last) {
│ │ │ │ +
395 return factors_.erase(first, last);
│ │ │ │ +
396 }
│ │ │ │ +
│ │ │ │ +
397
│ │ │ │ +
401
│ │ │ │ +
403 void dot(std::ostream& os,
│ │ │ │ +
404 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
405 const DotWriter& writer = DotWriter()) const;
│ │ │ │ +
406
│ │ │ │ +
408 std::string dot(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
409 const DotWriter& writer = DotWriter()) const;
│ │ │ │ +
410
│ │ │ │ +
412 void saveGraph(const std::string& filename,
│ │ │ │ +
413 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
414 const DotWriter& writer = DotWriter()) const;
│ │ │ │ +
415
│ │ │ │ +
419
│ │ │ │ +
421 size_t nrFactors() const;
│ │ │ │ +
422
│ │ │ │ +
425 KeySet keys() const;
│ │ │ │ +
426
│ │ │ │ +
430 KeyVector keyVector() const;
│ │ │ │ +
431
│ │ │ │ +
434 inline bool exists(size_t idx) const { return idx < size() && at(idx); }
│ │ │ │ +
435
│ │ │ │ +
436 private:
│ │ │ │ +
438 friend class boost::serialization::access;
│ │ │ │ +
439 template <class ARCHIVE>
│ │ │ │ +
440 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ +
441 ar& BOOST_SERIALIZATION_NVP(factors_);
│ │ │ │ +
442 }
│ │ │ │ +
443
│ │ │ │ +
445}; // FactorGraph
│ │ │ │ +
│ │ │ │ +
446} // namespace gtsam
│ │ │ │ +
447
│ │ │ │ +
448#include <gtsam/inference/FactorGraph-inst.h>
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ +
FactorGraph-inst.h
Factor Graph Base Class.
│ │ │ │ + │ │ │ │ +
DotWriter.h
Graphviz formatter.
│ │ │ │ +
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition Factor.h:34
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::VariableIndex::internalAt
const FactorIndices & internalAt(Key variable) const
Internal version of 'at' that asserts existence.
Definition VariableIndex.h:172
│ │ │ │ -
gtsam::VariableIndex::remove
void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG &factors)
Remove entries corresponding to the specified factors.
Definition VariableIndex-inl.h:54
│ │ │ │ -
gtsam::VariableIndex::removeUnusedVariables
void removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey)
Remove unused empty variables (in debug mode verifies they are empty).
Definition VariableIndex-inl.h:85
│ │ │ │ -
gtsam::VariableIndex::augment
void augment(const FG &factors, boost::optional< const FactorIndices & > newFactorIndices=boost::none)
Augment the variable index with new factors.
Definition VariableIndex-inl.h:27
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ + │ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ +
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ +
gtsam::FactorGraph::print
virtual void print(const std::string &s="FactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const
Print out graph to std::cout, with optional key formatter.
Definition FactorGraph-inst.h:37
│ │ │ │ +
gtsam::FactorGraph::isEqual
bool isEqual(const FactorGraph &other) const
Check exact equality of the factor pointers. Useful for derived ==.
Definition FactorGraph.h:134
│ │ │ │ +
gtsam::FactorGraph::keys
KeySet keys() const
Potentially slow function to return all keys involved, sorted, as a set.
Definition FactorGraph-inst.h:85
│ │ │ │ +
gtsam::FactorGraph::empty
bool empty() const
Check if the graph is empty (null factors set by remove() will cause this to return false).
Definition FactorGraph.h:330
│ │ │ │ +
gtsam::FactorGraph::add_factors
FactorIndices add_factors(const CONTAINER &factors, bool useEmptySlots=false)
Add new factors to a factor graph and returns a list of new factor indices, optionally finding and re...
Definition FactorGraph-inst.h:109
│ │ │ │ +
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition FactorGraph.h:186
│ │ │ │ +
gtsam::FactorGraph::dot
void dot(std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
Output to graphviz format, stream version.
Definition FactorGraph-inst.h:141
│ │ │ │ +
gtsam::FactorGraph::erase
iterator erase(iterator item)
Erase factor and rearrange other factors to take up the empty space.
Definition FactorGraph.h:391
│ │ │ │ +
gtsam::FactorGraph::add
void add(const FACTOR_OR_CONTAINER &factorOrContainer)
Add a factor or container of factors, including STL collections, BayesTrees, etc.
Definition FactorGraph.h:269
│ │ │ │ +
gtsam::FactorGraph::erase
iterator erase(iterator first, iterator last)
Erase factors and rearrange other factors to take up the empty space.
Definition FactorGraph.h:394
│ │ │ │ +
gtsam::FactorGraph::back
sharedFactor back() const
Get the last factor.
Definition FactorGraph.h:362
│ │ │ │ +
gtsam::FactorGraph::FactorGraph
FactorGraph(const CONTAINER &factors)
Construct from container of factors (shared_ptr or plain objects)
Definition FactorGraph.h:152
│ │ │ │ +
gtsam::FactorGraph::remove
void remove(size_t i)
delete factor without re-arranging indexes by inserting a nullptr pointer
Definition FactorGraph.h:385
│ │ │ │ +
gtsam::FactorGraph::keyVector
KeyVector keyVector() const
Potentially slow function to return all keys involved, sorted, as a vector.
Definition FactorGraph-inst.h:95
│ │ │ │ +
gtsam::FactorGraph::push_back
HasDerivedValueType< CONTAINER > push_back(const CONTAINER &container)
Push back non-pointer objects in a container (factors are copied).
Definition FactorGraph.h:260
│ │ │ │ +
gtsam::FactorGraph::FactorGraph
FactorGraph()
Default constructor.
Definition FactorGraph.h:142
│ │ │ │ +
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition FactorGraph.h:192
│ │ │ │ +
gtsam::FactorGraph::operator[]
sharedFactor & operator[](size_t i)
Get a specific factor by index (this does not check array bounds, as opposed to at() which does).
Definition FactorGraph.h:350
│ │ │ │ +
gtsam::FactorGraph::at
sharedFactor & at(size_t i)
Get a specific factor by index (this checks array bounds and may throw an exception,...
Definition FactorGraph.h:340
│ │ │ │ +
gtsam::FactorGraph::push_back
std::enable_if< std::is_base_of< This, typenameCLIQUE::FactorGraphType >::value >::type push_back(const BayesTree< CLIQUE > &bayesTree)
Push back a BayesTree as a collection of factors.
Definition FactorGraph.h:296
│ │ │ │ +
gtsam::FactorGraph::error
double error(const HybridValues &values) const
Add error for all factors.
Definition FactorGraph-inst.h:66
│ │ │ │ +
gtsam::FactorGraph::FactorGraph
FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
Constructor from iterator over factors (shared_ptr or plain objects)
Definition FactorGraph.h:146
│ │ │ │ +
gtsam::FactorGraph::resize
virtual void resize(size_t size)
Directly resize the number of factors in the graph.
Definition FactorGraph.h:381
│ │ │ │ +
gtsam::FactorGraph::nrFactors
size_t nrFactors() const
return the number of non-null factors
Definition FactorGraph-inst.h:76
│ │ │ │ +
gtsam::FactorGraph::size
size_t size() const
return the number of factors (including any null factors set by remove() ).
Definition FactorGraph.h:326
│ │ │ │ +
gtsam::FactorGraph::push_back
HasDerivedValueType< ITERATOR > push_back(ITERATOR firstFactor, ITERATOR lastFactor)
Push back many factors with an iterator (factors are copied)
Definition FactorGraph.h:240
│ │ │ │ +
gtsam::FactorGraph::front
sharedFactor front() const
Get the first factor.
Definition FactorGraph.h:359
│ │ │ │ +
gtsam::FactorGraph::end
const_iterator end() const
Iterator to end of factors.
Definition FactorGraph.h:356
│ │ │ │ +
gtsam::FactorGraph::push_back
HasDerivedElementType< CONTAINER > push_back(const CONTAINER &container)
Push back many factors as shared_ptr's in a container (factors are not copied)
Definition FactorGraph.h:254
│ │ │ │ +
gtsam::FactorGraph::FactorType
FACTOR FactorType
factor type
Definition FactorGraph.h:99
│ │ │ │ +
gtsam::FactorGraph::replace
void replace(size_t index, sharedFactor factor)
replace a factor by index
Definition FactorGraph.h:388
│ │ │ │ +
gtsam::FactorGraph::operator+=
boost::assign::list_inserter< CRefCallPushBack< This > > operator+=(const FACTOR_OR_CONTAINER &factorOrContainer)
Add a factor or container of factors, including STL collections, BayesTrees, etc.
Definition FactorGraph.h:278
│ │ │ │ +
gtsam::FactorGraph::operator+=
std::enable_if< std::is_base_of< FactorType, DERIVEDFACTOR >::value, boost::assign::list_inserter< RefCallPushBack< This > > >::type operator+=(boost::shared_ptr< DERIVEDFACTOR > factor)
+= works well with boost::assign list inserter.
Definition FactorGraph.h:219
│ │ │ │ +
gtsam::FactorGraph::begin
iterator begin()
non-const STL-style begin()
Definition FactorGraph.h:372
│ │ │ │ +
gtsam::FactorGraph::begin
const_iterator begin() const
Iterator to beginning of factors.
Definition FactorGraph.h:353
│ │ │ │ +
gtsam::FactorGraph::push_back
HasDerivedElementType< ITERATOR > push_back(ITERATOR firstFactor, ITERATOR lastFactor)
Push back many factors with an iterator over shared_ptr (factors are not copied)
Definition FactorGraph.h:233
│ │ │ │ +
gtsam::FactorGraph::add
IsDerived< DERIVEDFACTOR > add(boost::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition FactorGraph.h:210
│ │ │ │ +
gtsam::FactorGraph::access
friend class boost::serialization::access
Serialization function.
Definition FactorGraph.h:438
│ │ │ │ +
gtsam::FactorGraph::sharedFactor
boost::shared_ptr< FACTOR > sharedFactor
Shared pointer to a factor.
Definition FactorGraph.h:101
│ │ │ │ +
gtsam::FactorGraph::exists
bool exists(size_t idx) const
MATLAB interface utility: Checks whether a factor index idx exists in the graph and is a live pointer...
Definition FactorGraph.h:434
│ │ │ │ +
gtsam::FactorGraph::operator[]
const sharedFactor operator[](size_t i) const
Get a specific factor by index (this does not check array bounds, as opposed to at() which does).
Definition FactorGraph.h:345
│ │ │ │ +
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(const DERIVEDFACTOR &factor)
Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid the copy).
Definition FactorGraph.h:203
│ │ │ │ +
gtsam::FactorGraph::FactorGraph
FactorGraph(std::initializer_list< boost::shared_ptr< DERIVEDFACTOR > > sharedFactors)
Constructor that takes an initializer list of shared pointers.
Definition FactorGraph.h:171
│ │ │ │ +
gtsam::FactorGraph::end
iterator end()
non-const STL-style end()
Definition FactorGraph.h:375
│ │ │ │ +
gtsam::FactorGraph::factors_
FastVector< sharedFactor > factors_
concept check, makes sure FACTOR defines print and equals
Definition FactorGraph.h:131
│ │ │ │ +
gtsam::FactorGraph::at
const sharedFactor at(size_t i) const
Get a specific factor by index (this checks array bounds and may throw an exception,...
Definition FactorGraph.h:335
│ │ │ │ +
gtsam::FactorGraph::saveGraph
void saveGraph(const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
output to file with graphviz format.
Definition FactorGraph-inst.h:177
│ │ │ │ +
gtsam::FactorGraph::~FactorGraph
virtual ~FactorGraph()=default
Default destructor Public and virtual so boost serialization can call it.
│ │ │ │ +
gtsam::FactorGraph::reserve
void reserve(size_t size)
Reserve space for the specified number of factors if you know in advance how many there will be (work...
Definition FactorGraph.h:182
│ │ │ │ +
gtsam::BayesTree
Bayes tree.
Definition BayesTree.h:67
│ │ │ │ +
gtsam::BayesTree::addFactorsToGraph
void addFactorsToGraph(FactorGraph< FactorType > *graph) const
Add all cliques in this BayesTree to the specified factor graph.
Definition BayesTree-inst.h:168
│ │ │ │ +
gtsam::DotWriter
DotWriter is a helper class for writing graphviz .dot files.
Definition DotWriter.h:35
│ │ │ │ +
gtsam::CRefCallPushBack
Helper.
Definition FactorGraph.h:54
│ │ │ │ +
gtsam::RefCallPushBack
Helper.
Definition FactorGraph.h:67
│ │ │ │ +
gtsam::CRefCallAddCopy
Helper.
Definition FactorGraph.h:80
│ │ │ │ +
HybridValues
the error.
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,141 +1,591 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -VariableIndex-inl.h │ │ │ │ │ +FactorGraph.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_V_a_r_i_a_b_l_e_I_n_d_e_x_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_b_a_s_e_/_t_i_m_i_n_g_._h> │ │ │ │ │ +21// \callgraph │ │ │ │ │ 22 │ │ │ │ │ -23namespace _g_t_s_a_m { │ │ │ │ │ +23#pragma once │ │ │ │ │ 24 │ │ │ │ │ -25/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -26template │ │ │ │ │ -_2_7void _V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_a_u_g_m_e_n_t(const FG& factors, │ │ │ │ │ -28 boost::optional newFactorIndices) { │ │ │ │ │ -29 gttic(VariableIndex_augment); │ │ │ │ │ -30 │ │ │ │ │ -31 // Augment index for each factor │ │ │ │ │ -32 for (size_t i = 0; i < factors.size(); ++i) { │ │ │ │ │ -33 if (factors[i]) { │ │ │ │ │ -34 const size_t globalI = │ │ │ │ │ -35 newFactorIndices ? (*newFactorIndices)[i] : nFactors_; │ │ │ │ │ -36 for(const _K_e_y key: *factors[i]) { │ │ │ │ │ -37 index_[key].push_back(globalI); │ │ │ │ │ -38 ++nEntries_; │ │ │ │ │ -39 } │ │ │ │ │ -40 } │ │ │ │ │ +25#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_D_o_t_W_r_i_t_e_r_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ +27#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ +28#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +29 │ │ │ │ │ +30#include // for Eigen::aligned_allocator │ │ │ │ │ +31 │ │ │ │ │ +32#include │ │ │ │ │ +33#include │ │ │ │ │ +34#include │ │ │ │ │ +35#include │ │ │ │ │ +36 │ │ │ │ │ +37#include │ │ │ │ │ +38#include │ │ │ │ │ +39#include │ │ │ │ │ +40#include │ │ │ │ │ 41 │ │ │ │ │ -42 // Increment factor count even if factors are null, to keep indices │ │ │ │ │ -consistent │ │ │ │ │ -43 if (newFactorIndices) { │ │ │ │ │ -44 if ((*newFactorIndices)[i] >= nFactors_) │ │ │ │ │ -45 nFactors_ = (*newFactorIndices)[i] + 1; │ │ │ │ │ -46 } else { │ │ │ │ │ -47 ++nFactors_; │ │ │ │ │ -48 } │ │ │ │ │ -49 } │ │ │ │ │ -50} │ │ │ │ │ +42namespace _g_t_s_a_m { │ │ │ │ │ +44typedef FastVector _F_a_c_t_o_r_I_n_d_i_c_e_s; │ │ │ │ │ +45 │ │ │ │ │ +46// Forward declarations │ │ │ │ │ +47template │ │ │ │ │ +48class BayesTree; │ │ │ │ │ +49 │ │ │ │ │ +50class _H_y_b_r_i_d_V_a_l_u_e_s; │ │ │ │ │ 51 │ │ │ │ │ -52/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -53template │ │ │ │ │ -_5_4void _V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_r_e_m_o_v_e(ITERATOR firstFactor, ITERATOR lastFactor, │ │ │ │ │ -55 const FG& factors) { │ │ │ │ │ -56 gttic(VariableIndex_remove); │ │ │ │ │ -57 │ │ │ │ │ -58 // NOTE: We intentionally do not decrement nFactors_ because the factor │ │ │ │ │ -59 // indices need to remain consistent. Removing factors from a factor graph │ │ │ │ │ -60 // does not shift the indices of other factors. Also, we keep nFactors_ │ │ │ │ │ -61 // one greater than the highest-numbered factor referenced in a │ │ │ │ │ -VariableIndex. │ │ │ │ │ -62 ITERATOR factorIndex = firstFactor; │ │ │ │ │ -63 size_t i = 0; │ │ │ │ │ -64 for (; factorIndex != lastFactor; ++factorIndex, ++i) { │ │ │ │ │ -65 if (i >= factors.size()) │ │ │ │ │ -66 throw std::invalid_argument( │ │ │ │ │ -67 "Internal error, requested inconsistent number of factor indices and factors │ │ │ │ │ -in VariableIndex::remove"); │ │ │ │ │ -68 if (factors[i]) { │ │ │ │ │ -69 for(_K_e_y j: *factors[i]) { │ │ │ │ │ -70 _F_a_c_t_o_r_I_n_d_i_c_e_s& factorEntries = _i_n_t_e_r_n_a_l_A_t(j); │ │ │ │ │ -71 auto entry = std::find(factorEntries.begin(), │ │ │ │ │ -72 factorEntries.end(), *factorIndex); │ │ │ │ │ -73 if (entry == factorEntries.end()) │ │ │ │ │ -74 throw std::invalid_argument( │ │ │ │ │ -75 "Internal error, indices and factors passed into VariableIndex::remove are │ │ │ │ │ -not consistent with the existing variable index"); │ │ │ │ │ -76 factorEntries.erase(entry); │ │ │ │ │ -77 --nEntries_; │ │ │ │ │ -78 } │ │ │ │ │ -79 } │ │ │ │ │ -80 } │ │ │ │ │ -81} │ │ │ │ │ +53template │ │ │ │ │ +_5_4class _C_R_e_f_C_a_l_l_P_u_s_h_B_a_c_k { │ │ │ │ │ +55 C& obj; │ │ │ │ │ +56 │ │ │ │ │ +57 public: │ │ │ │ │ +58 explicit _C_R_e_f_C_a_l_l_P_u_s_h_B_a_c_k(C& obj) : obj(obj) {} │ │ │ │ │ +59 template │ │ │ │ │ +60 void operator()(const A& a) { │ │ │ │ │ +61 obj.push_back(a); │ │ │ │ │ +62 } │ │ │ │ │ +63}; │ │ │ │ │ +64 │ │ │ │ │ +66template │ │ │ │ │ +_6_7class _R_e_f_C_a_l_l_P_u_s_h_B_a_c_k { │ │ │ │ │ +68 C& obj; │ │ │ │ │ +69 │ │ │ │ │ +70 public: │ │ │ │ │ +71 explicit _R_e_f_C_a_l_l_P_u_s_h_B_a_c_k(C& obj) : obj(obj) {} │ │ │ │ │ +72 template │ │ │ │ │ +73 void operator()(A& a) { │ │ │ │ │ +74 obj.push_back(a); │ │ │ │ │ +75 } │ │ │ │ │ +76}; │ │ │ │ │ +77 │ │ │ │ │ +79template │ │ │ │ │ +_8_0class _C_R_e_f_C_a_l_l_A_d_d_C_o_p_y { │ │ │ │ │ +81 C& obj; │ │ │ │ │ 82 │ │ │ │ │ -83/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -84template │ │ │ │ │ -_8_5void _V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_r_e_m_o_v_e_U_n_u_s_e_d_V_a_r_i_a_b_l_e_s(ITERATOR firstKey, ITERATOR │ │ │ │ │ -lastKey) { │ │ │ │ │ -86 for (ITERATOR key = firstKey; key != lastKey; ++key) { │ │ │ │ │ -87 KeyMap::iterator entry = index_.find(*key); │ │ │ │ │ -88 if (!entry->second.empty()) │ │ │ │ │ -89 throw std::invalid_argument( │ │ │ │ │ -90 "Asking to remove variables from the variable index that are not unused"); │ │ │ │ │ -91 index_.erase(entry); │ │ │ │ │ -92 } │ │ │ │ │ -93} │ │ │ │ │ -94 │ │ │ │ │ -95} │ │ │ │ │ -_t_i_m_i_n_g_._h │ │ │ │ │ -Timing utilities. │ │ │ │ │ -_V_a_r_i_a_b_l_e_I_n_d_e_x_._h │ │ │ │ │ +83 public: │ │ │ │ │ +84 explicit _C_R_e_f_C_a_l_l_A_d_d_C_o_p_y(C& obj) : obj(obj) {} │ │ │ │ │ +85 template │ │ │ │ │ +86 void operator()(const A& a) { │ │ │ │ │ +87 obj.addCopy(a); │ │ │ │ │ +88 } │ │ │ │ │ +89}; │ │ │ │ │ +90 │ │ │ │ │ +96template │ │ │ │ │ +_9_7class _F_a_c_t_o_r_G_r_a_p_h { │ │ │ │ │ +98 public: │ │ │ │ │ +_9_9 typedef FACTOR _F_a_c_t_o_r_T_y_p_e; │ │ │ │ │ +100 typedef boost::shared_ptr │ │ │ │ │ +_1_0_1 _s_h_a_r_e_d_F_a_c_t_o_r; │ │ │ │ │ +102 typedef _s_h_a_r_e_d_F_a_c_t_o_r value_type; │ │ │ │ │ +103 typedef typename _F_a_s_t_V_e_c_t_o_r_<_s_h_a_r_e_d_F_a_c_t_o_r_>_:_:_i_t_e_r_a_t_o_r iterator; │ │ │ │ │ +104 typedef typename _F_a_s_t_V_e_c_t_o_r_<_s_h_a_r_e_d_F_a_c_t_o_r_>_:_:_c_o_n_s_t___i_t_e_r_a_t_o_r const_iterator; │ │ │ │ │ +105 │ │ │ │ │ +106 private: │ │ │ │ │ +107 typedef _F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_> _T_h_i_s; │ │ │ │ │ +108 typedef boost::shared_ptr │ │ │ │ │ +109 shared_ptr; │ │ │ │ │ +110 │ │ │ │ │ +112 template │ │ │ │ │ +113 using IsDerived = typename std::enable_if< │ │ │ │ │ +114 std::is_base_of::value>::type; │ │ │ │ │ +115 │ │ │ │ │ +117 template │ │ │ │ │ +118 using HasDerivedValueType = typename std::enable_if< │ │ │ │ │ +119 std::is_base_of::value>::type; │ │ │ │ │ +120 │ │ │ │ │ +122 template │ │ │ │ │ +123 using HasDerivedElementType = typename std::enable_if::value>::type; │ │ │ │ │ +125 │ │ │ │ │ +126 protected: │ │ │ │ │ +128 GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) │ │ │ │ │ +129 │ │ │ │ │ +130 │ │ │ │ │ +_1_3_1 _F_a_s_t_V_e_c_t_o_r<_s_h_a_r_e_d_F_a_c_t_o_r> _f_a_c_t_o_r_s__; │ │ │ │ │ +132 │ │ │ │ │ +_1_3_4 bool _i_s_E_q_u_a_l(const _F_a_c_t_o_r_G_r_a_p_h& other) const { │ │ │ │ │ +135 return _f_a_c_t_o_r_s__ == other.factors_; │ │ │ │ │ +136 } │ │ │ │ │ +137 │ │ │ │ │ +140 │ │ │ │ │ +_1_4_2 _F_a_c_t_o_r_G_r_a_p_h() {} │ │ │ │ │ +143 │ │ │ │ │ +145 template │ │ │ │ │ +_1_4_6 _F_a_c_t_o_r_G_r_a_p_h(ITERATOR firstFactor, ITERATOR lastFactor) { │ │ │ │ │ +147 _p_u_s_h___b_a_c_k(firstFactor, lastFactor); │ │ │ │ │ +148 } │ │ │ │ │ +149 │ │ │ │ │ +151 template │ │ │ │ │ +_1_5_2 explicit _F_a_c_t_o_r_G_r_a_p_h(const CONTAINER& factors) { │ │ │ │ │ +153 _p_u_s_h___b_a_c_k(factors); │ │ │ │ │ +154 } │ │ │ │ │ +155 │ │ │ │ │ +157 │ │ │ │ │ +158 public: │ │ │ │ │ +161 │ │ │ │ │ +_1_6_4 virtual _~_F_a_c_t_o_r_G_r_a_p_h() = default; │ │ │ │ │ +165 │ │ │ │ │ +170 template > │ │ │ │ │ +_1_7_1 _F_a_c_t_o_r_G_r_a_p_h(std::initializer_list> │ │ │ │ │ +sharedFactors) │ │ │ │ │ +172 : _f_a_c_t_o_r_s__(sharedFactors) {} │ │ │ │ │ +173 │ │ │ │ │ +177 │ │ │ │ │ +_1_8_2 void _r_e_s_e_r_v_e(size_t _s_i_z_e) { _f_a_c_t_o_r_s__.reserve(_s_i_z_e); } │ │ │ │ │ +183 │ │ │ │ │ +185 template │ │ │ │ │ +_1_8_6 IsDerived _p_u_s_h___b_a_c_k(boost::shared_ptr factor) │ │ │ │ │ +{ │ │ │ │ │ +187 _f_a_c_t_o_r_s__.push_back(boost::shared_ptr(factor)); │ │ │ │ │ +188 } │ │ │ │ │ +189 │ │ │ │ │ +191 template │ │ │ │ │ +_1_9_2 IsDerived _e_m_p_l_a_c_e___s_h_a_r_e_d(Args&&... args) { │ │ │ │ │ +193 _f_a_c_t_o_r_s__.push_back(boost::allocate_shared( │ │ │ │ │ +194 Eigen::aligned_allocator(), │ │ │ │ │ +195 std::forward(args)...)); │ │ │ │ │ +196 } │ │ │ │ │ +197 │ │ │ │ │ +202 template │ │ │ │ │ +_2_0_3 IsDerived _p_u_s_h___b_a_c_k(const DERIVEDFACTOR& factor) { │ │ │ │ │ +204 _f_a_c_t_o_r_s__.push_back(boost::allocate_shared( │ │ │ │ │ +205 Eigen::aligned_allocator(), factor)); │ │ │ │ │ +206 } │ │ │ │ │ +207 │ │ │ │ │ +209 template │ │ │ │ │ +_2_1_0 IsDerived _a_d_d(boost::shared_ptr factor) { │ │ │ │ │ +211 _p_u_s_h___b_a_c_k(factor); │ │ │ │ │ +212 } │ │ │ │ │ +213 │ │ │ │ │ +215 template │ │ │ │ │ +216 typename std::enable_if< │ │ │ │ │ +217 std::is_base_of::value, │ │ │ │ │ +218 boost::assign::list_inserter>>::type │ │ │ │ │ +_2_1_9 _o_p_e_r_a_t_o_r_+_=(boost::shared_ptr factor) { │ │ │ │ │ +220 return boost::assign::make_list_inserter(_R_e_f_C_a_l_l_P_u_s_h_B_a_c_k_<_T_h_i_s_>(*this))( │ │ │ │ │ +221 factor); │ │ │ │ │ +222 } │ │ │ │ │ +223 │ │ │ │ │ +227 │ │ │ │ │ +232 template │ │ │ │ │ +_2_3_3 HasDerivedElementType _p_u_s_h___b_a_c_k(ITERATOR firstFactor, │ │ │ │ │ +234 ITERATOR lastFactor) { │ │ │ │ │ +235 _f_a_c_t_o_r_s__.insert(_e_n_d(), firstFactor, lastFactor); │ │ │ │ │ +236 } │ │ │ │ │ +237 │ │ │ │ │ +239 template │ │ │ │ │ +_2_4_0 HasDerivedValueType _p_u_s_h___b_a_c_k(ITERATOR firstFactor, │ │ │ │ │ +241 ITERATOR lastFactor) { │ │ │ │ │ +242 for (ITERATOR f = firstFactor; f != lastFactor; ++f) _p_u_s_h___b_a_c_k(*f); │ │ │ │ │ +243 } │ │ │ │ │ +244 │ │ │ │ │ +248 │ │ │ │ │ +253 template │ │ │ │ │ +_2_5_4 HasDerivedElementType _p_u_s_h___b_a_c_k(const CONTAINER& container) { │ │ │ │ │ +255 _p_u_s_h___b_a_c_k(container.begin(), container.end()); │ │ │ │ │ +256 } │ │ │ │ │ +257 │ │ │ │ │ +259 template │ │ │ │ │ +_2_6_0 HasDerivedValueType _p_u_s_h___b_a_c_k(const CONTAINER& container) { │ │ │ │ │ +261 _p_u_s_h___b_a_c_k(container.begin(), container.end()); │ │ │ │ │ +262 } │ │ │ │ │ +263 │ │ │ │ │ +268 template │ │ │ │ │ +_2_6_9 void _a_d_d(const FACTOR_OR_CONTAINER& factorOrContainer) { │ │ │ │ │ +270 _p_u_s_h___b_a_c_k(factorOrContainer); │ │ │ │ │ +271 } │ │ │ │ │ +272 │ │ │ │ │ +277 template │ │ │ │ │ +_2_7_8 boost::assign::list_inserter> _o_p_e_r_a_t_o_r_+_=( │ │ │ │ │ +279 const FACTOR_OR_CONTAINER& factorOrContainer) { │ │ │ │ │ +280 return boost::assign::make_list_inserter(_C_R_e_f_C_a_l_l_P_u_s_h_B_a_c_k_<_T_h_i_s_>(*this))( │ │ │ │ │ +281 factorOrContainer); │ │ │ │ │ +282 } │ │ │ │ │ +283 │ │ │ │ │ +287 │ │ │ │ │ +293 template │ │ │ │ │ +294 typename std::enable_if< │ │ │ │ │ +295 std::is_base_of::value>::type │ │ │ │ │ +_2_9_6 _p_u_s_h___b_a_c_k(const _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>& bayesTree) { │ │ │ │ │ +297 bayesTree._a_d_d_F_a_c_t_o_r_s_T_o_G_r_a_p_h(this); │ │ │ │ │ +298 } │ │ │ │ │ +299 │ │ │ │ │ +304 template > │ │ │ │ │ +305 _F_a_c_t_o_r_I_n_d_i_c_e_s _a_d_d___f_a_c_t_o_r_s(const CONTAINER& factors, │ │ │ │ │ +306 bool useEmptySlots = false); │ │ │ │ │ +307 │ │ │ │ │ +311 │ │ │ │ │ +313 virtual void _p_r_i_n_t(const std::string& s = "FactorGraph", │ │ │ │ │ +314 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const; │ │ │ │ │ +315 │ │ │ │ │ +317 bool _e_q_u_a_l_s(const This& fg, double tol = 1e-9) const; │ │ │ │ │ +319 │ │ │ │ │ +320 public: │ │ │ │ │ +323 │ │ │ │ │ +_3_2_6 size_t _s_i_z_e() const { return _f_a_c_t_o_r_s__.size(); } │ │ │ │ │ +327 │ │ │ │ │ +_3_3_0 bool _e_m_p_t_y() const { return _f_a_c_t_o_r_s__.empty(); } │ │ │ │ │ +331 │ │ │ │ │ +_3_3_5 const _s_h_a_r_e_d_F_a_c_t_o_r _a_t(size_t i) const { return _f_a_c_t_o_r_s__.at(i); } │ │ │ │ │ +336 │ │ │ │ │ +_3_4_0 _s_h_a_r_e_d_F_a_c_t_o_r& _a_t(size_t i) { return _f_a_c_t_o_r_s__.at(i); } │ │ │ │ │ +341 │ │ │ │ │ +_3_4_5 const _s_h_a_r_e_d_F_a_c_t_o_r _o_p_e_r_a_t_o_r_[_](size_t i) const { return _a_t(i); } │ │ │ │ │ +346 │ │ │ │ │ +_3_5_0 _s_h_a_r_e_d_F_a_c_t_o_r& _o_p_e_r_a_t_o_r_[_](size_t i) { return _a_t(i); } │ │ │ │ │ +351 │ │ │ │ │ +_3_5_3 const_iterator _b_e_g_i_n() const { return _f_a_c_t_o_r_s__.begin(); } │ │ │ │ │ +354 │ │ │ │ │ +_3_5_6 const_iterator _e_n_d() const { return _f_a_c_t_o_r_s__.end(); } │ │ │ │ │ +357 │ │ │ │ │ +_3_5_9 _s_h_a_r_e_d_F_a_c_t_o_r _f_r_o_n_t() const { return _f_a_c_t_o_r_s__.front(); } │ │ │ │ │ +360 │ │ │ │ │ +_3_6_2 _s_h_a_r_e_d_F_a_c_t_o_r _b_a_c_k() const { return _f_a_c_t_o_r_s__.back(); } │ │ │ │ │ +363 │ │ │ │ │ +365 double _e_r_r_o_r(const _H_y_b_r_i_d_V_a_l_u_e_s &values) const; │ │ │ │ │ +366 │ │ │ │ │ +370 │ │ │ │ │ +_3_7_2 iterator _b_e_g_i_n() { return _f_a_c_t_o_r_s__.begin(); } │ │ │ │ │ +373 │ │ │ │ │ +_3_7_5 iterator _e_n_d() { return _f_a_c_t_o_r_s__.end(); } │ │ │ │ │ +376 │ │ │ │ │ +_3_8_1 virtual void _r_e_s_i_z_e(size_t _s_i_z_e) { _f_a_c_t_o_r_s__.resize(_s_i_z_e); } │ │ │ │ │ +382 │ │ │ │ │ +_3_8_5 void _r_e_m_o_v_e(size_t i) { _f_a_c_t_o_r_s__.at(i).reset(); } │ │ │ │ │ +386 │ │ │ │ │ +_3_8_8 void _r_e_p_l_a_c_e(size_t index, _s_h_a_r_e_d_F_a_c_t_o_r factor) { _a_t(index) = factor; } │ │ │ │ │ +389 │ │ │ │ │ +_3_9_1 iterator _e_r_a_s_e(iterator item) { return _f_a_c_t_o_r_s__.erase(item); } │ │ │ │ │ +392 │ │ │ │ │ +_3_9_4 iterator _e_r_a_s_e(iterator first, iterator last) { │ │ │ │ │ +395 return _f_a_c_t_o_r_s__.erase(first, last); │ │ │ │ │ +396 } │ │ │ │ │ +397 │ │ │ │ │ +401 │ │ │ │ │ +403 void _d_o_t(std::ostream& os, │ │ │ │ │ +404 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ +405 const _D_o_t_W_r_i_t_e_r& writer = _D_o_t_W_r_i_t_e_r()) const; │ │ │ │ │ +406 │ │ │ │ │ +408 std::string _d_o_t(const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ +409 const _D_o_t_W_r_i_t_e_r& writer = _D_o_t_W_r_i_t_e_r()) const; │ │ │ │ │ +410 │ │ │ │ │ +412 void _s_a_v_e_G_r_a_p_h(const std::string& filename, │ │ │ │ │ +413 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ +414 const _D_o_t_W_r_i_t_e_r& writer = _D_o_t_W_r_i_t_e_r()) const; │ │ │ │ │ +415 │ │ │ │ │ +419 │ │ │ │ │ +421 size_t _n_r_F_a_c_t_o_r_s() const; │ │ │ │ │ +422 │ │ │ │ │ +425 _K_e_y_S_e_t _k_e_y_s() const; │ │ │ │ │ +426 │ │ │ │ │ +430 _K_e_y_V_e_c_t_o_r _k_e_y_V_e_c_t_o_r() const; │ │ │ │ │ +431 │ │ │ │ │ +_4_3_4 inline bool _e_x_i_s_t_s(size_t idx) const { return idx < _s_i_z_e() && _a_t(idx); } │ │ │ │ │ +435 │ │ │ │ │ +436 private: │ │ │ │ │ +_4_3_8 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +439 template │ │ │ │ │ +440 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ +441 ar& BOOST_SERIALIZATION_NVP(_f_a_c_t_o_r_s__); │ │ │ │ │ +442 } │ │ │ │ │ +443 │ │ │ │ │ +445}; // FactorGraph │ │ │ │ │ +446} // namespace gtsam │ │ │ │ │ +447 │ │ │ │ │ +448#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_-_i_n_s_t_._h> │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ +_F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ +A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ +_F_a_c_t_o_r_G_r_a_p_h_-_i_n_s_t_._h │ │ │ │ │ +Factor Graph Base Class. │ │ │ │ │ +_K_e_y_._h │ │ │ │ │ +_D_o_t_W_r_i_t_e_r_._h │ │ │ │ │ +Graphviz formatter. │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ +std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ +FastVector │ │ │ │ │ +FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ +DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ _g_t_s_a_m_:_:_F_a_c_t_o_r_I_n_d_i_c_e_s │ │ │ │ │ FastVector< FactorIndex > FactorIndices │ │ │ │ │ Define collection types: │ │ │ │ │ DDeeffiinniittiioonn Factor.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_i_n_t_e_r_n_a_l_A_t │ │ │ │ │ -const FactorIndices & internalAt(Key variable) const │ │ │ │ │ -Internal version of 'at' that asserts existence. │ │ │ │ │ -DDeeffiinniittiioonn VariableIndex.h:172 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_r_e_m_o_v_e │ │ │ │ │ -void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG &factors) │ │ │ │ │ -Remove entries corresponding to the specified factors. │ │ │ │ │ -DDeeffiinniittiioonn VariableIndex-inl.h:54 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_r_e_m_o_v_e_U_n_u_s_e_d_V_a_r_i_a_b_l_e_s │ │ │ │ │ -void removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey) │ │ │ │ │ -Remove unused empty variables (in debug mode verifies they are empty). │ │ │ │ │ -DDeeffiinniittiioonn VariableIndex-inl.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_a_u_g_m_e_n_t │ │ │ │ │ -void augment(const FG &factors, boost::optional< const FactorIndices & > │ │ │ │ │ -newFactorIndices=boost::none) │ │ │ │ │ -Augment the variable index with new factors. │ │ │ │ │ -DDeeffiinniittiioonn VariableIndex-inl.h:27 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ +nodes. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_p_r_i_n_t │ │ │ │ │ +virtual void print(const std::string &s="FactorGraph", const KeyFormatter │ │ │ │ │ +&formatter=DefaultKeyFormatter) const │ │ │ │ │ +Print out graph to std::cout, with optional key formatter. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph-inst.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_i_s_E_q_u_a_l │ │ │ │ │ +bool isEqual(const FactorGraph &other) const │ │ │ │ │ +Check exact equality of the factor pointers. Useful for derived ==. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:134 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_k_e_y_s │ │ │ │ │ +KeySet keys() const │ │ │ │ │ +Potentially slow function to return all keys involved, sorted, as a set. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph-inst.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_e_m_p_t_y │ │ │ │ │ +bool empty() const │ │ │ │ │ +Check if the graph is empty (null factors set by remove() will cause this to │ │ │ │ │ +return false). │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:330 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d___f_a_c_t_o_r_s │ │ │ │ │ +FactorIndices add_factors(const CONTAINER &factors, bool useEmptySlots=false) │ │ │ │ │ +Add new factors to a factor graph and returns a list of new factor indices, │ │ │ │ │ +optionally finding and re... │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph-inst.h:109 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_p_u_s_h___b_a_c_k │ │ │ │ │ +IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor) │ │ │ │ │ +Add a factor directly using a shared_ptr. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:186 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_d_o_t │ │ │ │ │ +void dot(std::ostream &os, const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const │ │ │ │ │ +Output to graphviz format, stream version. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph-inst.h:141 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_e_r_a_s_e │ │ │ │ │ +iterator erase(iterator item) │ │ │ │ │ +Erase factor and rearrange other factors to take up the empty space. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:391 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d │ │ │ │ │ +void add(const FACTOR_OR_CONTAINER &factorOrContainer) │ │ │ │ │ +Add a factor or container of factors, including STL collections, BayesTrees, │ │ │ │ │ +etc. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:269 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_e_r_a_s_e │ │ │ │ │ +iterator erase(iterator first, iterator last) │ │ │ │ │ +Erase factors and rearrange other factors to take up the empty space. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:394 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_b_a_c_k │ │ │ │ │ +sharedFactor back() const │ │ │ │ │ +Get the last factor. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:362 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +FactorGraph(const CONTAINER &factors) │ │ │ │ │ +Construct from container of factors (shared_ptr or plain objects) │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:152 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_r_e_m_o_v_e │ │ │ │ │ +void remove(size_t i) │ │ │ │ │ +delete factor without re-arranging indexes by inserting a nullptr pointer │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:385 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_k_e_y_V_e_c_t_o_r │ │ │ │ │ +KeyVector keyVector() const │ │ │ │ │ +Potentially slow function to return all keys involved, sorted, as a vector. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph-inst.h:95 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_p_u_s_h___b_a_c_k │ │ │ │ │ +HasDerivedValueType< CONTAINER > push_back(const CONTAINER &container) │ │ │ │ │ +Push back non-pointer objects in a container (factors are copied). │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:260 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +FactorGraph() │ │ │ │ │ +Default constructor. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:142 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_e_m_p_l_a_c_e___s_h_a_r_e_d │ │ │ │ │ +IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args) │ │ │ │ │ +Emplace a shared pointer to factor of given type. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:192 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_o_p_e_r_a_t_o_r_[_] │ │ │ │ │ +sharedFactor & operator[](size_t i) │ │ │ │ │ +Get a specific factor by index (this does not check array bounds, as opposed to │ │ │ │ │ +at() which does). │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:350 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_a_t │ │ │ │ │ +sharedFactor & at(size_t i) │ │ │ │ │ +Get a specific factor by index (this checks array bounds and may throw an │ │ │ │ │ +exception,... │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:340 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_p_u_s_h___b_a_c_k │ │ │ │ │ +std::enable_if< std::is_base_of< This, typenameCLIQUE::FactorGraphType >::value │ │ │ │ │ +>::type push_back(const BayesTree< CLIQUE > &bayesTree) │ │ │ │ │ +Push back a BayesTree as a collection of factors. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:296 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_e_r_r_o_r │ │ │ │ │ +double error(const HybridValues &values) const │ │ │ │ │ +Add error for all factors. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph-inst.h:66 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) │ │ │ │ │ +Constructor from iterator over factors (shared_ptr or plain objects) │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:146 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_r_e_s_i_z_e │ │ │ │ │ +virtual void resize(size_t size) │ │ │ │ │ +Directly resize the number of factors in the graph. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:381 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_n_r_F_a_c_t_o_r_s │ │ │ │ │ +size_t nrFactors() const │ │ │ │ │ +return the number of non-null factors │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph-inst.h:76 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_s_i_z_e │ │ │ │ │ +size_t size() const │ │ │ │ │ +return the number of factors (including any null factors set by remove() ). │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:326 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_p_u_s_h___b_a_c_k │ │ │ │ │ +HasDerivedValueType< ITERATOR > push_back(ITERATOR firstFactor, ITERATOR │ │ │ │ │ +lastFactor) │ │ │ │ │ +Push back many factors with an iterator (factors are copied) │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:240 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_f_r_o_n_t │ │ │ │ │ +sharedFactor front() const │ │ │ │ │ +Get the first factor. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:359 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_e_n_d │ │ │ │ │ +const_iterator end() const │ │ │ │ │ +Iterator to end of factors. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:356 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_p_u_s_h___b_a_c_k │ │ │ │ │ +HasDerivedElementType< CONTAINER > push_back(const CONTAINER &container) │ │ │ │ │ +Push back many factors as shared_ptr's in a container (factors are not copied) │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:254 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_F_a_c_t_o_r_T_y_p_e │ │ │ │ │ +FACTOR FactorType │ │ │ │ │ +factor type │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:99 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_r_e_p_l_a_c_e │ │ │ │ │ +void replace(size_t index, sharedFactor factor) │ │ │ │ │ +replace a factor by index │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:388 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_o_p_e_r_a_t_o_r_+_= │ │ │ │ │ +boost::assign::list_inserter< CRefCallPushBack< This > > operator+=(const │ │ │ │ │ +FACTOR_OR_CONTAINER &factorOrContainer) │ │ │ │ │ +Add a factor or container of factors, including STL collections, BayesTrees, │ │ │ │ │ +etc. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:278 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_o_p_e_r_a_t_o_r_+_= │ │ │ │ │ +std::enable_if< std::is_base_of< FactorType, DERIVEDFACTOR >::value, boost:: │ │ │ │ │ +assign::list_inserter< RefCallPushBack< This > > >::type operator+=(boost:: │ │ │ │ │ +shared_ptr< DERIVEDFACTOR > factor) │ │ │ │ │ ++= works well with boost::assign list inserter. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:219 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_b_e_g_i_n │ │ │ │ │ +iterator begin() │ │ │ │ │ +non-const STL-style begin() │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:372 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_b_e_g_i_n │ │ │ │ │ +const_iterator begin() const │ │ │ │ │ +Iterator to beginning of factors. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:353 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_p_u_s_h___b_a_c_k │ │ │ │ │ +HasDerivedElementType< ITERATOR > push_back(ITERATOR firstFactor, ITERATOR │ │ │ │ │ +lastFactor) │ │ │ │ │ +Push back many factors with an iterator over shared_ptr (factors are not │ │ │ │ │ +copied) │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:233 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d │ │ │ │ │ +IsDerived< DERIVEDFACTOR > add(boost::shared_ptr< DERIVEDFACTOR > factor) │ │ │ │ │ +add is a synonym for push_back. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:210 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:438 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d_F_a_c_t_o_r │ │ │ │ │ +boost::shared_ptr< FACTOR > sharedFactor │ │ │ │ │ +Shared pointer to a factor. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:101 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_e_x_i_s_t_s │ │ │ │ │ +bool exists(size_t idx) const │ │ │ │ │ +MATLAB interface utility: Checks whether a factor index idx exists in the graph │ │ │ │ │ +and is a live pointer... │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:434 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_o_p_e_r_a_t_o_r_[_] │ │ │ │ │ +const sharedFactor operator[](size_t i) const │ │ │ │ │ +Get a specific factor by index (this does not check array bounds, as opposed to │ │ │ │ │ +at() which does). │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:345 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_p_u_s_h___b_a_c_k │ │ │ │ │ +IsDerived< DERIVEDFACTOR > push_back(const DERIVEDFACTOR &factor) │ │ │ │ │ +Add a factor by value, will be copy-constructed (use push_back with a │ │ │ │ │ +shared_ptr to avoid the copy). │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:203 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +FactorGraph(std::initializer_list< boost::shared_ptr< DERIVEDFACTOR > > │ │ │ │ │ +sharedFactors) │ │ │ │ │ +Constructor that takes an initializer list of shared pointers. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:171 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_e_n_d │ │ │ │ │ +iterator end() │ │ │ │ │ +non-const STL-style end() │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:375 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_f_a_c_t_o_r_s__ │ │ │ │ │ +FastVector< sharedFactor > factors_ │ │ │ │ │ +concept check, makes sure FACTOR defines print and equals │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:131 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_a_t │ │ │ │ │ +const sharedFactor at(size_t i) const │ │ │ │ │ +Get a specific factor by index (this checks array bounds and may throw an │ │ │ │ │ +exception,... │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:335 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_s_a_v_e_G_r_a_p_h │ │ │ │ │ +void saveGraph(const std::string &filename, const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const │ │ │ │ │ +output to file with graphviz format. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph-inst.h:177 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_~_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +virtual ~FactorGraph()=default │ │ │ │ │ +Default destructor Public and virtual so boost serialization can call it. │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_r_e_s_e_r_v_e │ │ │ │ │ +void reserve(size_t size) │ │ │ │ │ +Reserve space for the specified number of factors if you know in advance how │ │ │ │ │ +many there will be (work... │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:182 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e │ │ │ │ │ +Bayes tree. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_a_d_d_F_a_c_t_o_r_s_T_o_G_r_a_p_h │ │ │ │ │ +void addFactorsToGraph(FactorGraph< FactorType > *graph) const │ │ │ │ │ +Add all cliques in this BayesTree to the specified factor graph. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:168 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r │ │ │ │ │ +DotWriter is a helper class for writing graphviz .dot files. │ │ │ │ │ +DDeeffiinniittiioonn DotWriter.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_C_R_e_f_C_a_l_l_P_u_s_h_B_a_c_k │ │ │ │ │ +Helper. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:54 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_f_C_a_l_l_P_u_s_h_B_a_c_k │ │ │ │ │ +Helper. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_C_R_e_f_C_a_l_l_A_d_d_C_o_p_y │ │ │ │ │ +Helper. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:80 │ │ │ │ │ +_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +the error. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _V_a_r_i_a_b_l_e_I_n_d_e_x_-_i_n_l_._h │ │ │ │ │ + * _F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00662.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/ISAM.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Symbol.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,44 +94,41 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
ISAM.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
Symbol.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ - │ │ │ │ -

Incremental update functionality (iSAM) for BayesTree. │ │ │ │ -More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::ISAM< BAYESTREE >
 A Bayes tree with an update methods that implements the iSAM algorithm. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +GTSAM_EXPORT std::ostream & gtsam::operator<< (std::ostream &os, const Symbol &symbol)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Incremental update functionality (iSAM) for BayesTree.

│ │ │ │ -
Author
Michael Kaess
│ │ │ │ +
Date
June 9, 2012
│ │ │ │ +
Author
: Frank Dellaert
│ │ │ │ +
│ │ │ │ +: Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,26 +1,25 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -ISAM.h File Reference │ │ │ │ │ -Incremental update functionality (iSAM) for BayesTree. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_I_S_A_M_<_ _B_A_Y_E_S_T_R_E_E_ _> │ │ │ │ │ -  A Bayes tree with an update methods that implements the iSAM algorithm. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Symbol.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +GTSAM_EXPORT std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _S_y_m_b_o_l │ │ │ │ │ + &_s_y_m_b_o_l) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Incremental update functionality (iSAM) for BayesTree. │ │ │ │ │ + Date │ │ │ │ │ + June 9, 2012 │ │ │ │ │ Author │ │ │ │ │ - Michael Kaess │ │ │ │ │ + : Frank Dellaert │ │ │ │ │ + : Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _I_S_A_M_._h │ │ │ │ │ + * _S_y_m_b_o_l_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00665.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/LabeledSymbol.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/inferenceExceptions.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,68 +95,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
LabeledSymbol.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
inferenceExceptions.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ +

Exceptions that may be thrown by inference algorithms. │ │ │ │ +More...

│ │ │ │ + │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::LabeledSymbol
 Customized version of gtsam::Symbol for multi-robot use. More...
 
struct  gtsam::traits< LabeledSymbol >
 traits More...
class  gtsam::InconsistentEliminationRequested
 An inference algorithm was called with inconsistent arguments. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

Key gtsam::mrsymbol (unsigned char c, unsigned char label, size_t j)
 Create a symbol key from a character, label and index, i.e.
 
│ │ │ │ -unsigned char gtsam::mrsymbolChr (Key key)
 Return the character portion of a symbol key.
 
│ │ │ │ -unsigned char gtsam::mrsymbolLabel (Key key)
 Return the label portion of a symbol key.
 
│ │ │ │ -size_t gtsam::mrsymbolIndex (Key key)
 Return the index portion of a symbol key.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Jan 12, 2010
│ │ │ │ -
Author
: Alex Cunningham
│ │ │ │ -
Date
Jan 12, 2010
│ │ │ │ -
Author
: Alex Cunningham
│ │ │ │ -
│ │ │ │ -: Frank Dellaert
│ │ │ │ -
│ │ │ │ -: Richard Roberts
│ │ │ │ +

Exceptions that may be thrown by inference algorithms.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Apr 25, 2013
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,47 +1,27 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -LabeledSymbol.h File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +inferenceExceptions.h File Reference │ │ │ │ │ +Exceptions that may be thrown by inference algorithms. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_L_a_b_e_l_e_d_S_y_m_b_o_l │ │ │ │ │ -  Customized version of _g_t_s_a_m_:_:_S_y_m_b_o_l for multi-robot use. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _L_a_b_e_l_e_d_S_y_m_b_o_l_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_I_n_c_o_n_s_i_s_t_e_n_t_E_l_i_m_i_n_a_t_i_o_n_R_e_q_u_e_s_t_e_d │ │ │ │ │ +  An inference algorithm was called with inconsistent arguments. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ - _K_e_y  _g_t_s_a_m_:_:_m_r_s_y_m_b_o_l (unsigned char c, unsigned char label, size_t j) │ │ │ │ │ -  Create a symbol key from a character, label and index, i.e. │ │ │ │ │ -  │ │ │ │ │ -unsigned char  ggttssaamm::::mmrrssyymmbboollCChhrr (_K_e_y key) │ │ │ │ │ -  Return the character portion of a symbol key. │ │ │ │ │ -  │ │ │ │ │ -unsigned char  ggttssaamm::::mmrrssyymmbboollLLaabbeell (_K_e_y key) │ │ │ │ │ -  Return the label portion of a symbol key. │ │ │ │ │ -  │ │ │ │ │ - size_t  ggttssaamm::::mmrrssyymmbboollIInnddeexx (_K_e_y key) │ │ │ │ │ -  Return the index portion of a symbol key. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Jan 12, 2010 │ │ │ │ │ +Exceptions that may be thrown by inference algorithms. │ │ │ │ │ Author │ │ │ │ │ - : Alex Cunningham │ │ │ │ │ + Richard Roberts │ │ │ │ │ Date │ │ │ │ │ - Jan 12, 2010 │ │ │ │ │ - Author │ │ │ │ │ - : Alex Cunningham │ │ │ │ │ - : Frank Dellaert │ │ │ │ │ - : Richard Roberts │ │ │ │ │ + Apr 25, 2013 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _L_a_b_e_l_e_d_S_y_m_b_o_l_._h │ │ │ │ │ + * _i_n_f_e_r_e_n_c_e_E_x_c_e_p_t_i_o_n_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00665.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,8 +1,3 @@ │ │ │ │ │ var a00665 = [ │ │ │ │ │ - ["gtsam::LabeledSymbol", "a03668.html", "a03668"], │ │ │ │ │ - ["gtsam::traits< LabeledSymbol >", "a03672.html", null], │ │ │ │ │ - ["mrsymbol", "a00665.html#ae9a7dceb0a85c356062b55f6939b1419", null], │ │ │ │ │ - ["mrsymbolChr", "a00665.html#a37995b0d0610e5fb29c6bd0715151d67", null], │ │ │ │ │ - ["mrsymbolIndex", "a00665.html#a4adc1bb606fa4a14d5dbd246f95cecd3", null], │ │ │ │ │ - ["mrsymbolLabel", "a00665.html#a4e4c133c4fd0d9d73593f601be2248a6", null] │ │ │ │ │ + ["gtsam::InconsistentEliminationRequested", "a03636.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00665_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/LabeledSymbol.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/inferenceExceptions.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,146 +98,59 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
LabeledSymbol.h
│ │ │ │ +
inferenceExceptions.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <functional>
│ │ │ │ -
23#include <gtsam/inference/Symbol.h>
│ │ │ │ -
24
│ │ │ │ -
25namespace gtsam {
│ │ │ │ -
26
│ │ │ │ -
│ │ │ │ -
35class GTSAM_EXPORT LabeledSymbol {
│ │ │ │ -
36protected:
│ │ │ │ -
37 unsigned char c_, label_;
│ │ │ │ -
38 std::uint64_t j_;
│ │ │ │ -
39
│ │ │ │ -
40public:
│ │ │ │ -
42 LabeledSymbol();
│ │ │ │ -
43
│ │ │ │ -
45 LabeledSymbol(const LabeledSymbol& key);
│ │ │ │ -
46
│ │ │ │ -
48 LabeledSymbol(unsigned char c, unsigned char label, std::uint64_t j);
│ │ │ │ -
49
│ │ │ │ -
51 LabeledSymbol(gtsam::Key key);
│ │ │ │ -
52
│ │ │ │ -
54 operator gtsam::Key() const;
│ │ │ │ -
55
│ │ │ │ -
56 // Testable Requirements
│ │ │ │ -
57 void print(const std::string& s = "") const;
│ │ │ │ -
58
│ │ │ │ -
59 bool equals(const LabeledSymbol& expected, double tol = 0.0) const {
│ │ │ │ -
60 return (*this) == expected;
│ │ │ │ -
61 }
│ │ │ │ -
62
│ │ │ │ -
64 gtsam::Key key() const { return (gtsam::Key) *this; }
│ │ │ │ -
65
│ │ │ │ -
67 inline unsigned char label() const { return label_; }
│ │ │ │ -
68
│ │ │ │ -
70 inline unsigned char chr() const { return c_; }
│ │ │ │ -
71
│ │ │ │ -
73 inline size_t index() const { return j_; }
│ │ │ │ -
74
│ │ │ │ -
76 operator std::string() const;
│ │ │ │ -
77
│ │ │ │ -
79 bool operator<(const LabeledSymbol& comp) const;
│ │ │ │ -
80 bool operator==(const LabeledSymbol& comp) const;
│ │ │ │ -
81 bool operator==(gtsam::Key comp) const;
│ │ │ │ -
82 bool operator!=(const LabeledSymbol& comp) const;
│ │ │ │ -
83 bool operator!=(gtsam::Key comp) const;
│ │ │ │ -
84
│ │ │ │ -
91 // Checks only the type
│ │ │ │ -
92 static std::function<bool(gtsam::Key)> TypeTest(unsigned char c);
│ │ │ │ -
93
│ │ │ │ -
94 // Checks only the robot ID (label_)
│ │ │ │ -
95 static std::function<bool(gtsam::Key)> LabelTest(unsigned char label);
│ │ │ │ -
96
│ │ │ │ -
97 // Checks both type and the robot ID
│ │ │ │ -
98 static std::function<bool(gtsam::Key)> TypeLabelTest(unsigned char c, unsigned char label);
│ │ │ │ -
99
│ │ │ │ -
100 // Converts to upper/lower versions of labels
│ │ │ │ -
101 LabeledSymbol upper() const { return LabeledSymbol(c_, toupper(label_), j_); }
│ │ │ │ -
102 LabeledSymbol lower() const { return LabeledSymbol(c_, tolower(label_), j_); }
│ │ │ │ -
103
│ │ │ │ -
104 // Create a new symbol with a different character.
│ │ │ │ -
105 LabeledSymbol newChr(unsigned char c) const { return LabeledSymbol(c, label_, j_); }
│ │ │ │ -
106
│ │ │ │ -
107 // Create a new symbol with a different label.
│ │ │ │ -
108 LabeledSymbol newLabel(unsigned char label) const { return LabeledSymbol(c_, label, j_); }
│ │ │ │ -
109
│ │ │ │ -
111 friend GTSAM_EXPORT std::ostream &operator<<(std::ostream &, const LabeledSymbol &);
│ │ │ │ -
112
│ │ │ │ -
113private:
│ │ │ │ -
114
│ │ │ │ -
116 friend class boost::serialization::access;
│ │ │ │ -
117 template<class ARCHIVE>
│ │ │ │ -
118 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
119 ar & BOOST_SERIALIZATION_NVP(c_);
│ │ │ │ -
120 ar & BOOST_SERIALIZATION_NVP(label_);
│ │ │ │ -
121 ar & BOOST_SERIALIZATION_NVP(j_);
│ │ │ │ -
122 }
│ │ │ │ -
123}; // \class LabeledSymbol
│ │ │ │ -
│ │ │ │ -
124
│ │ │ │ -
│ │ │ │ -
126inline Key mrsymbol(unsigned char c, unsigned char label, size_t j) {
│ │ │ │ -
127 return (Key)LabeledSymbol(c,label,j);
│ │ │ │ -
128}
│ │ │ │ -
│ │ │ │ -
129
│ │ │ │ -
131inline unsigned char mrsymbolChr(Key key) { return LabeledSymbol(key).chr(); }
│ │ │ │ -
132
│ │ │ │ -
134inline unsigned char mrsymbolLabel(Key key) { return LabeledSymbol(key).label(); }
│ │ │ │ -
135
│ │ │ │ -
137inline size_t mrsymbolIndex(Key key) { return LabeledSymbol(key).index(); }
│ │ │ │ -
138
│ │ │ │ -
140template<> struct traits<LabeledSymbol> : public Testable<LabeledSymbol> {};
│ │ │ │ -
141
│ │ │ │ -
142} // \namespace gtsam
│ │ │ │ -
143
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/global_includes.h>
│ │ │ │ +
21#include <boost/lexical_cast.hpp>
│ │ │ │ +
22#include <exception>
│ │ │ │ +
23
│ │ │ │ +
24namespace gtsam {
│ │ │ │ +
25
│ │ │ │ +
│ │ │ │ +
29 class InconsistentEliminationRequested : public std::exception {
│ │ │ │ +
30 public:
│ │ │ │ +
31 InconsistentEliminationRequested() noexcept {}
│ │ │ │ +
32 ~InconsistentEliminationRequested() noexcept override {}
│ │ │ │ +
33 const char* what() const noexcept override {
│ │ │ │ +
34 return
│ │ │ │ +
35 "An inference algorithm was called with inconsistent arguments. The\n"
│ │ │ │ +
36 "factor graph, ordering, or variable index were inconsistent with each\n"
│ │ │ │ +
37 "other, or a full elimination routine was called with an ordering that\n"
│ │ │ │ +
38 "does not include all of the variables.";
│ │ │ │ +
39 }
│ │ │ │ +
40 };
│ │ │ │ +
│ │ │ │ +
41
│ │ │ │ +
42}
│ │ │ │ +
global_includes.h
Included from all GTSAM files.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::mrsymbolChr
unsigned char mrsymbolChr(Key key)
Return the character portion of a symbol key.
Definition LabeledSymbol.h:131
│ │ │ │ -
gtsam::mrsymbolIndex
size_t mrsymbolIndex(Key key)
Return the index portion of a symbol key.
Definition LabeledSymbol.h:137
│ │ │ │ -
gtsam::mrsymbolLabel
unsigned char mrsymbolLabel(Key key)
Return the label portion of a symbol key.
Definition LabeledSymbol.h:134
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::operator!=
bool operator!=(const Matrix &A, const Matrix &B)
inequality
Definition Matrix.h:107
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::mrsymbol
Key mrsymbol(unsigned char c, unsigned char label, size_t j)
Create a symbol key from a character, label and index, i.e.
Definition LabeledSymbol.h:126
│ │ │ │ -
gtsam::operator==
bool operator==(const Matrix &A, const Matrix &B)
equality is just equal_with_abs_tol 1e-9
Definition Matrix.h:100
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::LabeledSymbol
Customized version of gtsam::Symbol for multi-robot use.
Definition LabeledSymbol.h:35
│ │ │ │ -
gtsam::LabeledSymbol::chr
unsigned char chr() const
Retrieve key character.
Definition LabeledSymbol.h:70
│ │ │ │ -
gtsam::LabeledSymbol::key
gtsam::Key key() const
return the integer version
Definition LabeledSymbol.h:64
│ │ │ │ -
gtsam::LabeledSymbol::index
size_t index() const
Retrieve key index.
Definition LabeledSymbol.h:73
│ │ │ │ -
gtsam::LabeledSymbol::label
unsigned char label() const
Retrieve label character.
Definition LabeledSymbol.h:67
│ │ │ │ - │ │ │ │ +
gtsam::InconsistentEliminationRequested
An inference algorithm was called with inconsistent arguments.
Definition inferenceExceptions.h:29
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,189 +1,54 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -LabeledSymbol.h │ │ │ │ │ +inferenceExceptions.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include │ │ │ │ │ -23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_S_y_m_b_o_l_._h> │ │ │ │ │ -24 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ -26 │ │ │ │ │ -_3_5class GTSAM_EXPORT _L_a_b_e_l_e_d_S_y_m_b_o_l { │ │ │ │ │ -36protected: │ │ │ │ │ -37 unsigned char c_, label_; │ │ │ │ │ -38 std::uint64_t j_; │ │ │ │ │ -39 │ │ │ │ │ -40public: │ │ │ │ │ -42 _L_a_b_e_l_e_d_S_y_m_b_o_l(); │ │ │ │ │ -43 │ │ │ │ │ -45 _L_a_b_e_l_e_d_S_y_m_b_o_l(const _L_a_b_e_l_e_d_S_y_m_b_o_l& key); │ │ │ │ │ -46 │ │ │ │ │ -48 _L_a_b_e_l_e_d_S_y_m_b_o_l(unsigned char c, unsigned char label, std::uint64_t j); │ │ │ │ │ -49 │ │ │ │ │ -51 _L_a_b_e_l_e_d_S_y_m_b_o_l(_g_t_s_a_m_:_:_K_e_y key); │ │ │ │ │ -52 │ │ │ │ │ -54 operator _g_t_s_a_m_:_:_K_e_y() const; │ │ │ │ │ -55 │ │ │ │ │ -56 // Testable Requirements │ │ │ │ │ -57 void _p_r_i_n_t(const std::string& s = "") const; │ │ │ │ │ -58 │ │ │ │ │ -59 bool _e_q_u_a_l_s(const _L_a_b_e_l_e_d_S_y_m_b_o_l& expected, double tol = 0.0) const { │ │ │ │ │ -60 return (*this) == expected; │ │ │ │ │ -61 } │ │ │ │ │ -62 │ │ │ │ │ -_6_4 _g_t_s_a_m_:_:_K_e_y _k_e_y() const { return (_g_t_s_a_m_:_:_K_e_y) *this; } │ │ │ │ │ -65 │ │ │ │ │ -_6_7 inline unsigned char _l_a_b_e_l() const { return label_; } │ │ │ │ │ -68 │ │ │ │ │ -_7_0 inline unsigned char _c_h_r() const { return c_; } │ │ │ │ │ -71 │ │ │ │ │ -_7_3 inline size_t _i_n_d_e_x() const { return j_; } │ │ │ │ │ -74 │ │ │ │ │ -76 operator std::string() const; │ │ │ │ │ -77 │ │ │ │ │ -79 bool operator<(const _L_a_b_e_l_e_d_S_y_m_b_o_l& comp) const; │ │ │ │ │ -80 bool _o_p_e_r_a_t_o_r_=_=(const _L_a_b_e_l_e_d_S_y_m_b_o_l& comp) const; │ │ │ │ │ -81 bool _o_p_e_r_a_t_o_r_=_=(_g_t_s_a_m_:_:_K_e_y comp) const; │ │ │ │ │ -82 bool _o_p_e_r_a_t_o_r_!_=(const _L_a_b_e_l_e_d_S_y_m_b_o_l& comp) const; │ │ │ │ │ -83 bool _o_p_e_r_a_t_o_r_!_=(_g_t_s_a_m_:_:_K_e_y comp) const; │ │ │ │ │ -84 │ │ │ │ │ -91 // Checks only the type │ │ │ │ │ -92 static std::function TypeTest(unsigned char c); │ │ │ │ │ -93 │ │ │ │ │ -94 // Checks only the robot ID (label_) │ │ │ │ │ -95 static std::function LabelTest(unsigned char label); │ │ │ │ │ -96 │ │ │ │ │ -97 // Checks both type and the robot ID │ │ │ │ │ -98 static std::function TypeLabelTest(unsigned char c, │ │ │ │ │ -unsigned char label); │ │ │ │ │ -99 │ │ │ │ │ -100 // Converts to upper/lower versions of labels │ │ │ │ │ -101 _L_a_b_e_l_e_d_S_y_m_b_o_l upper() const { return _L_a_b_e_l_e_d_S_y_m_b_o_l(c_, toupper(label_), │ │ │ │ │ -j_); } │ │ │ │ │ -102 LabeledSymbol lower() const { return LabeledSymbol(c_, tolower(label_), │ │ │ │ │ -j_); } │ │ │ │ │ -103 │ │ │ │ │ -104 // Create a new symbol with a different character. │ │ │ │ │ -105 LabeledSymbol newChr(unsigned char c) const { return LabeledSymbol(c, │ │ │ │ │ -label_, j_); } │ │ │ │ │ -106 │ │ │ │ │ -107 // Create a new symbol with a different label. │ │ │ │ │ -108 LabeledSymbol newLabel(unsigned char label) const { return LabeledSymbol │ │ │ │ │ -(c_, label, j_); } │ │ │ │ │ -109 │ │ │ │ │ -111 friend GTSAM_EXPORT std::ostream &operator<<(std::ostream &, const │ │ │ │ │ -LabeledSymbol &); │ │ │ │ │ -112 │ │ │ │ │ -113private: │ │ │ │ │ -114 │ │ │ │ │ -_1_1_6 friend class boost::serialization::access; │ │ │ │ │ -117 template │ │ │ │ │ -118 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -119 ar & BOOST_SERIALIZATION_NVP(c_); │ │ │ │ │ -120 ar & BOOST_SERIALIZATION_NVP(label_); │ │ │ │ │ -121 ar & BOOST_SERIALIZATION_NVP(j_); │ │ │ │ │ -122 } │ │ │ │ │ -123}; // \class LabeledSymbol │ │ │ │ │ -124 │ │ │ │ │ -_1_2_6inline _K_e_y _m_r_s_y_m_b_o_l(unsigned char c, unsigned char label, size_t j) { │ │ │ │ │ -127 return (_K_e_y)_L_a_b_e_l_e_d_S_y_m_b_o_l(c,label,j); │ │ │ │ │ -128} │ │ │ │ │ -129 │ │ │ │ │ -_1_3_1inline unsigned char _m_r_s_y_m_b_o_l_C_h_r(_K_e_y key) { return _L_a_b_e_l_e_d_S_y_m_b_o_l(key)._c_h_r(); │ │ │ │ │ -} │ │ │ │ │ -132 │ │ │ │ │ -_1_3_4inline unsigned char _m_r_s_y_m_b_o_l_L_a_b_e_l(_K_e_y key) { return _L_a_b_e_l_e_d_S_y_m_b_o_l │ │ │ │ │ -(key)._l_a_b_e_l(); } │ │ │ │ │ -135 │ │ │ │ │ -_1_3_7inline size_t _m_r_s_y_m_b_o_l_I_n_d_e_x(_K_e_y key) { return _L_a_b_e_l_e_d_S_y_m_b_o_l(key)._i_n_d_e_x(); } │ │ │ │ │ -138 │ │ │ │ │ -_1_4_0template<> struct _t_r_a_i_t_s<_L_a_b_e_l_e_d_S_y_m_b_o_l> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ -141 │ │ │ │ │ -142} // \namespace gtsam │ │ │ │ │ -143 │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ +21#include │ │ │ │ │ +22#include │ │ │ │ │ +23 │ │ │ │ │ +24namespace _g_t_s_a_m { │ │ │ │ │ +25 │ │ │ │ │ +_2_9 class _I_n_c_o_n_s_i_s_t_e_n_t_E_l_i_m_i_n_a_t_i_o_n_R_e_q_u_e_s_t_e_d : public std::exception { │ │ │ │ │ +30 public: │ │ │ │ │ +31 _I_n_c_o_n_s_i_s_t_e_n_t_E_l_i_m_i_n_a_t_i_o_n_R_e_q_u_e_s_t_e_d() noexcept {} │ │ │ │ │ +32 _~_I_n_c_o_n_s_i_s_t_e_n_t_E_l_i_m_i_n_a_t_i_o_n_R_e_q_u_e_s_t_e_d() noexcept override {} │ │ │ │ │ +33 const char* what() const noexcept override { │ │ │ │ │ +34 return │ │ │ │ │ +35 "An inference algorithm was called with inconsistent arguments. The\n" │ │ │ │ │ +36 "factor graph, ordering, or variable index were inconsistent with each\n" │ │ │ │ │ +37 "other, or a full elimination routine was called with an ordering that\n" │ │ │ │ │ +38 "does not include all of the variables."; │ │ │ │ │ +39 } │ │ │ │ │ +40 }; │ │ │ │ │ +41 │ │ │ │ │ +42} │ │ │ │ │ +_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ +Included from all GTSAM files. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_m_r_s_y_m_b_o_l_C_h_r │ │ │ │ │ -unsigned char mrsymbolChr(Key key) │ │ │ │ │ -Return the character portion of a symbol key. │ │ │ │ │ -DDeeffiinniittiioonn LabeledSymbol.h:131 │ │ │ │ │ -_g_t_s_a_m_:_:_m_r_s_y_m_b_o_l_I_n_d_e_x │ │ │ │ │ -size_t mrsymbolIndex(Key key) │ │ │ │ │ -Return the index portion of a symbol key. │ │ │ │ │ -DDeeffiinniittiioonn LabeledSymbol.h:137 │ │ │ │ │ -_g_t_s_a_m_:_:_m_r_s_y_m_b_o_l_L_a_b_e_l │ │ │ │ │ -unsigned char mrsymbolLabel(Key key) │ │ │ │ │ -Return the label portion of a symbol key. │ │ │ │ │ -DDeeffiinniittiioonn LabeledSymbol.h:134 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_!_= │ │ │ │ │ -bool operator!=(const Matrix &A, const Matrix &B) │ │ │ │ │ -inequality │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:107 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_m_r_s_y_m_b_o_l │ │ │ │ │ -Key mrsymbol(unsigned char c, unsigned char label, size_t j) │ │ │ │ │ -Create a symbol key from a character, label and index, i.e. │ │ │ │ │ -DDeeffiinniittiioonn LabeledSymbol.h:126 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_=_= │ │ │ │ │ -bool operator==(const Matrix &A, const Matrix &B) │ │ │ │ │ -equality is just equal_with_abs_tol 1e-9 │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_L_a_b_e_l_e_d_S_y_m_b_o_l │ │ │ │ │ -Customized version of gtsam::Symbol for multi-robot use. │ │ │ │ │ -DDeeffiinniittiioonn LabeledSymbol.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_L_a_b_e_l_e_d_S_y_m_b_o_l_:_:_c_h_r │ │ │ │ │ -unsigned char chr() const │ │ │ │ │ -Retrieve key character. │ │ │ │ │ -DDeeffiinniittiioonn LabeledSymbol.h:70 │ │ │ │ │ -_g_t_s_a_m_:_:_L_a_b_e_l_e_d_S_y_m_b_o_l_:_:_k_e_y │ │ │ │ │ -gtsam::Key key() const │ │ │ │ │ -return the integer version │ │ │ │ │ -DDeeffiinniittiioonn LabeledSymbol.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_L_a_b_e_l_e_d_S_y_m_b_o_l_:_:_i_n_d_e_x │ │ │ │ │ -size_t index() const │ │ │ │ │ -Retrieve key index. │ │ │ │ │ -DDeeffiinniittiioonn LabeledSymbol.h:73 │ │ │ │ │ -_g_t_s_a_m_:_:_L_a_b_e_l_e_d_S_y_m_b_o_l_:_:_l_a_b_e_l │ │ │ │ │ -unsigned char label() const │ │ │ │ │ -Retrieve label character. │ │ │ │ │ -DDeeffiinniittiioonn LabeledSymbol.h:67 │ │ │ │ │ -_S_y_m_b_o_l_._h │ │ │ │ │ +_g_t_s_a_m_:_:_I_n_c_o_n_s_i_s_t_e_n_t_E_l_i_m_i_n_a_t_i_o_n_R_e_q_u_e_s_t_e_d │ │ │ │ │ +An inference algorithm was called with inconsistent arguments. │ │ │ │ │ +DDeeffiinniittiioonn inferenceExceptions.h:29 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _L_a_b_e_l_e_d_S_y_m_b_o_l_._h │ │ │ │ │ + * _i_n_f_e_r_e_n_c_e_E_x_c_e_p_t_i_o_n_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00668.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Ordering.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/inference-inst.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,51 +94,87 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
Ordering.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
inference-inst.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Variable ordering for the elimination algorithm. │ │ │ │ +

Contains generic inference algorithms that convert between templated graphical models, i.e., factor graphs, Bayes nets, and Bayes trees. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::Ordering
 
struct  gtsam::traits< Ordering >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

template<class TREE , class RESULT >
FastVector< typename TREE::sharedFactor > gtsam::inference::EliminateTree (RESULT &result, const TREE &tree, const typename TREE::Eliminate &function)
 Eliminate an elimination tree or a Bayes tree (used internally).
 
│ │ │ │

Detailed Description

│ │ │ │ -

Variable ordering for the elimination algorithm.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
│ │ │ │ -Andrew Melim
│ │ │ │ +

Contains generic inference algorithms that convert between templated graphical models, i.e., factor graphs, Bayes nets, and Bayes trees.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │ -Frank Dellaert
│ │ │ │ -
Date
Sep 2, 2010
│ │ │ │ -
│ │ │ │ +Richard Roberts │ │ │ │ +

Function Documentation

│ │ │ │ + │ │ │ │ +

◆ EliminateTree()

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +template<class TREE , class RESULT >
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
FastVector< typename TREE::sharedFactor > gtsam::inference::EliminateTree (RESULT & result,
const TREE & tree,
const typename TREE::Eliminate & function 
)
│ │ │ │ +
│ │ │ │ + │ │ │ │ +

Eliminate an elimination tree or a Bayes tree (used internally).

│ │ │ │ +

Requires TREE::BayesNetType, TREE::FactorGraphType, TREE::sharedConditional, TREE::sharedFactor, TREE::Node, TREE::sharedNode, TREE::Node::factors, TREE::Node::children.

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,31 +1,45 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Ordering.h File Reference │ │ │ │ │ -Variable ordering for the elimination algorithm. _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +inference-inst.h File Reference │ │ │ │ │ +Contains ggeenneerriicc inference algorithms that convert between templated graphical │ │ │ │ │ +models, i.e., factor graphs, Bayes nets, and Bayes trees. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _O_r_d_e_r_i_n_g_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +template │ │ │ │ │ +_F_a_s_t_V_e_c_t_o_r< typename TREE::sharedFactor _g_t_s_a_m_:_:_i_n_f_e_r_e_n_c_e_:_:_E_l_i_m_i_n_a_t_e_T_r_e_e (RESULT │ │ │ │ │ + >  &result, const TREE &tree, const │ │ │ │ │ + typename TREE::Eliminate &function) │ │ │ │ │ +  Eliminate an elimination tree or a │ │ │ │ │ + Bayes tree (used internally). │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Variable ordering for the elimination algorithm. │ │ │ │ │ +Contains ggeenneerriicc inference algorithms that convert between templated graphical │ │ │ │ │ +models, i.e., factor graphs, Bayes nets, and Bayes trees. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Andrew Melim │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - Sep 2, 2010 │ │ │ │ │ + Richard Roberts │ │ │ │ │ +********** FFuunnccttiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ +********** _?◆_? EElliimmiinnaatteeTTrreeee(()) ********** │ │ │ │ │ +template │ │ │ │ │ +FastVector< typename TREE:: │ │ │ │ │ +sharedFactor > gtsam::inference:: ( RESULT &  rreessuulltt, │ │ │ │ │ +EliminateTree │ │ │ │ │ + const TREE &  ttrreeee, │ │ │ │ │ + const typename TREE::Eliminate &  ffuunnccttiioonn  │ │ │ │ │ + ) │ │ │ │ │ +Eliminate an elimination tree or a Bayes tree (used internally). │ │ │ │ │ +Requires TREE::BayesNetType, TREE::FactorGraphType, TREE::sharedConditional, │ │ │ │ │ +TREE::sharedFactor, TREE::Node, TREE::sharedNode, TREE::Node::factors, TREE:: │ │ │ │ │ +Node::children. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _O_r_d_e_r_i_n_g_._h │ │ │ │ │ + * _i_n_f_e_r_e_n_c_e_-_i_n_s_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00668.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,3 @@ │ │ │ │ │ var a00668 = [ │ │ │ │ │ - ["gtsam::Ordering", "a03680.html", "a03680"], │ │ │ │ │ - ["gtsam::traits< Ordering >", "a03684.html", null] │ │ │ │ │ + ["EliminateTree", "a00668.html#ae955942fde34b4cf32ac3573a1aa1e29", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00668_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Ordering.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/inference-inst.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,255 +98,109 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Ordering.h
│ │ │ │ +
inference-inst.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ -
4 * Atlanta, Georgia 30332-0415
│ │ │ │ -
5 * All Rights Reserved
│ │ │ │ -
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
3* GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
4* Atlanta, Georgia 30332-0415
│ │ │ │ +
5* All Rights Reserved
│ │ │ │ +
6* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │ -
8 * See LICENSE for the license information
│ │ │ │ +
8* See LICENSE for the license information
│ │ │ │
9
│ │ │ │ -
10 * -------------------------------------------------------------------------- */
│ │ │ │ +
10* -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
21#pragma once
│ │ │ │ -
22
│ │ │ │ -
23#include <gtsam/inference/Key.h>
│ │ │ │ -
24#include <gtsam/inference/VariableIndex.h>
│ │ │ │ -
25#include <gtsam/inference/MetisIndex.h>
│ │ │ │ -
26#include <gtsam/base/FastSet.h>
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <boost/shared_ptr.hpp>
│ │ │ │ +
23#include <utility>
│ │ │ │ +
24
│ │ │ │ +
25#include <gtsam/base/treeTraversal-inst.h>
│ │ │ │ +
26#include <gtsam/base/FastVector.h>
│ │ │ │
27
│ │ │ │ -
28#include <boost/assign/list_inserter.hpp>
│ │ │ │ -
29#include <algorithm>
│ │ │ │ -
30#include <vector>
│ │ │ │ -
31
│ │ │ │ -
32namespace gtsam {
│ │ │ │ -
33
│ │ │ │ -
│ │ │ │ -
34class Ordering: public KeyVector {
│ │ │ │ -
35protected:
│ │ │ │ -
36 typedef KeyVector Base;
│ │ │ │ -
37
│ │ │ │ -
38public:
│ │ │ │ -
39
│ │ │ │ -
│ │ │ │ -
41 enum OrderingType {
│ │ │ │ -
42 COLAMD, METIS, NATURAL, CUSTOM
│ │ │ │ -
43 };
│ │ │ │ -
│ │ │ │ -
44
│ │ │ │ -
45 typedef Ordering This;
│ │ │ │ -
46 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
47
│ │ │ │ -
49 GTSAM_EXPORT
│ │ │ │ -
│ │ │ │ -
50 Ordering() {
│ │ │ │ -
51 }
│ │ │ │ -
│ │ │ │ -
52
│ │ │ │ -
53 using KeyVector::KeyVector; // Inherit the KeyVector's constructors
│ │ │ │ -
54
│ │ │ │ -
56 template<typename KEYS>
│ │ │ │ -
│ │ │ │ -
57 explicit Ordering(const KEYS& keys) :
│ │ │ │ -
58 Base(keys.begin(), keys.end()) {
│ │ │ │ -
59 }
│ │ │ │ -
│ │ │ │ -
60
│ │ │ │ -
│ │ │ │ -
63 boost::assign::list_inserter<boost::assign_detail::call_push_back<This> > operator+=(
│ │ │ │ -
64 Key key) {
│ │ │ │ -
65 return boost::assign::make_list_inserter(
│ │ │ │ -
66 boost::assign_detail::call_push_back<This>(*this))(key);
│ │ │ │ -
67 }
│ │ │ │ -
│ │ │ │ +
28namespace gtsam {
│ │ │ │ +
29 namespace inference {
│ │ │ │ +
30
│ │ │ │ +
31 namespace {
│ │ │ │ +
32 /* ************************************************************************* */
│ │ │ │ +
33 template<class TREE>
│ │ │ │ +
34 struct EliminationData {
│ │ │ │ +
35 EliminationData* const parentData;
│ │ │ │ +
36 FastVector<typename TREE::sharedFactor> childFactors;
│ │ │ │ +
37 EliminationData(EliminationData* _parentData, size_t nChildren) :
│ │ │ │ +
38 parentData(_parentData) { childFactors.reserve(nChildren); }
│ │ │ │ +
39 };
│ │ │ │ +
40
│ │ │ │ +
41 /* ************************************************************************* */
│ │ │ │ +
42 template<class TREE>
│ │ │ │ +
43 EliminationData<TREE> eliminationPreOrderVisitor(
│ │ │ │ +
44 const typename TREE::sharedNode& node, EliminationData<TREE>& parentData)
│ │ │ │ +
45 {
│ │ │ │ +
46 // This function is called before visiting the children. Here, we create this node's data,
│ │ │ │ +
47 // which includes a pointer to the parent data and space for the factors of the children.
│ │ │ │ +
48 return EliminationData<TREE>(&parentData, node->children.size());
│ │ │ │ +
49 }
│ │ │ │ +
50
│ │ │ │ +
51 /* ************************************************************************* */
│ │ │ │ +
52 template<class TREE, class RESULT>
│ │ │ │ +
53 struct EliminationPostOrderVisitor
│ │ │ │ +
54 {
│ │ │ │ +
55 RESULT& result;
│ │ │ │ +
56 const typename TREE::Eliminate& eliminationFunction;
│ │ │ │ +
57 EliminationPostOrderVisitor(RESULT& result, const typename TREE::Eliminate& eliminationFunction) :
│ │ │ │ +
58 result(result), eliminationFunction(eliminationFunction) {}
│ │ │ │ +
59 void operator()(const typename TREE::sharedNode& node, EliminationData<TREE>& myData)
│ │ │ │ +
60 {
│ │ │ │ +
61 // Call eliminate on the node and add the result to the parent's gathered factors
│ │ │ │ +
62 typename TREE::sharedFactor childFactor = node->eliminate(result, eliminationFunction, myData.childFactors);
│ │ │ │ +
63 if(childFactor && !childFactor->empty())
│ │ │ │ +
64 myData.parentData->childFactors.push_back(childFactor);
│ │ │ │ +
65 }
│ │ │ │ +
66 };
│ │ │ │ +
67 }
│ │ │ │
68
│ │ │ │ -
75 This& operator+=(KeyVector& keys);
│ │ │ │ -
76
│ │ │ │ -
78 bool contains(const Key& key) const;
│ │ │ │ -
79
│ │ │ │ -
86 FastMap<Key, size_t> invert() const;
│ │ │ │ -
87
│ │ │ │ -
90
│ │ │ │ -
94 template<class FACTOR_GRAPH>
│ │ │ │ -
│ │ │ │ -
95 static Ordering Colamd(const FACTOR_GRAPH& graph) {
│ │ │ │ -
96 if (graph.empty())
│ │ │ │ -
97 return Ordering();
│ │ │ │ -
98 else
│ │ │ │ -
99 return Colamd(VariableIndex(graph));
│ │ │ │ -
100 }
│ │ │ │ -
│ │ │ │ -
101
│ │ │ │ -
103 static GTSAM_EXPORT Ordering Colamd(const VariableIndex& variableIndex);
│ │ │ │ -
104
│ │ │ │ -
113 template<class FACTOR_GRAPH>
│ │ │ │ -
│ │ │ │ -
114 static Ordering ColamdConstrainedLast(const FACTOR_GRAPH& graph,
│ │ │ │ -
115 const KeyVector& constrainLast, bool forceOrder = false) {
│ │ │ │ -
116 if (graph.empty())
│ │ │ │ -
117 return Ordering();
│ │ │ │ -
118 else
│ │ │ │ -
119 return ColamdConstrainedLast(VariableIndex(graph), constrainLast, forceOrder);
│ │ │ │ -
120 }
│ │ │ │ +
69 /* ************************************************************************* */
│ │ │ │ +
73 template<class TREE, class RESULT>
│ │ │ │ +
74 FastVector<typename TREE::sharedFactor>
│ │ │ │ +
│ │ │ │ +
75 EliminateTree(RESULT& result, const TREE& tree, const typename TREE::Eliminate& function)
│ │ │ │ +
76 {
│ │ │ │ +
77 // Do elimination using a depth-first traversal. During the pre-order visit (see
│ │ │ │ +
78 // eliminationPreOrderVisitor), we store a pointer to the parent data (where we'll put the
│ │ │ │ +
79 // remaining factor) and reserve a vector of factors to store the children elimination
│ │ │ │ +
80 // results. During the post-order visit (see eliminationPostOrderVisitor), we call dense
│ │ │ │ +
81 // elimination (using the gathered child factors) and store the result in the parent's
│ │ │ │ +
82 // gathered factors.
│ │ │ │ +
83 EliminationData<TREE> rootData(0, tree.roots().size());
│ │ │ │ +
84 EliminationPostOrderVisitor<TREE,RESULT> visitorPost(result, function);
│ │ │ │ +
85 treeTraversal::DepthFirstForest(tree, rootData, eliminationPreOrderVisitor<TREE>, visitorPost);
│ │ │ │ +
86
│ │ │ │ +
87 // Return remaining factors
│ │ │ │ +
88 return rootData.childFactors;
│ │ │ │ +
89 }
│ │ │ │
│ │ │ │ -
121
│ │ │ │ -
128 static GTSAM_EXPORT Ordering ColamdConstrainedLast(
│ │ │ │ -
129 const VariableIndex& variableIndex, const KeyVector& constrainLast,
│ │ │ │ -
130 bool forceOrder = false);
│ │ │ │ -
131
│ │ │ │ -
140 template<class FACTOR_GRAPH>
│ │ │ │ -
│ │ │ │ -
141 static Ordering ColamdConstrainedFirst(const FACTOR_GRAPH& graph,
│ │ │ │ -
142 const KeyVector& constrainFirst, bool forceOrder = false) {
│ │ │ │ -
143 if (graph.empty())
│ │ │ │ -
144 return Ordering();
│ │ │ │ -
145 else
│ │ │ │ -
146 return ColamdConstrainedFirst(VariableIndex(graph), constrainFirst, forceOrder);
│ │ │ │ -
147 }
│ │ │ │ -
│ │ │ │ -
148
│ │ │ │ -
156 static GTSAM_EXPORT Ordering ColamdConstrainedFirst(
│ │ │ │ -
157 const VariableIndex& variableIndex,
│ │ │ │ -
158 const KeyVector& constrainFirst, bool forceOrder = false);
│ │ │ │ -
159
│ │ │ │ -
169 template<class FACTOR_GRAPH>
│ │ │ │ -
│ │ │ │ -
170 static Ordering ColamdConstrained(const FACTOR_GRAPH& graph,
│ │ │ │ -
171 const FastMap<Key, int>& groups) {
│ │ │ │ -
172 if (graph.empty())
│ │ │ │ -
173 return Ordering();
│ │ │ │ -
174 else
│ │ │ │ -
175 return ColamdConstrained(VariableIndex(graph), groups);
│ │ │ │ -
176 }
│ │ │ │ -
│ │ │ │ -
177
│ │ │ │ -
185 static GTSAM_EXPORT Ordering ColamdConstrained(
│ │ │ │ -
186 const VariableIndex& variableIndex, const FastMap<Key, int>& groups);
│ │ │ │ -
187
│ │ │ │ -
189 template<class FACTOR_GRAPH>
│ │ │ │ -
│ │ │ │ -
190 static Ordering Natural(const FACTOR_GRAPH &fg) {
│ │ │ │ -
191 KeySet src = fg.keys();
│ │ │ │ -
192 KeyVector keys(src.begin(), src.end());
│ │ │ │ -
193 std::stable_sort(keys.begin(), keys.end());
│ │ │ │ -
194 return Ordering(keys.begin(), keys.end());
│ │ │ │ -
195 }
│ │ │ │ -
│ │ │ │ -
196
│ │ │ │ -
198 template<class FACTOR_GRAPH>
│ │ │ │ -
199 static GTSAM_EXPORT void CSRFormat(std::vector<int>& xadj,
│ │ │ │ -
200 std::vector<int>& adj, const FACTOR_GRAPH& graph);
│ │ │ │ -
201
│ │ │ │ -
203 static GTSAM_EXPORT Ordering Metis(const MetisIndex& met);
│ │ │ │ -
204
│ │ │ │ -
205 template<class FACTOR_GRAPH>
│ │ │ │ -
206 static Ordering Metis(const FACTOR_GRAPH& graph) {
│ │ │ │ -
207 if (graph.empty())
│ │ │ │ -
208 return Ordering();
│ │ │ │ -
209 else
│ │ │ │ -
210 return Metis(MetisIndex(graph));
│ │ │ │ -
211 }
│ │ │ │ -
212
│ │ │ │ -
214
│ │ │ │ -
217
│ │ │ │ -
218 template<class FACTOR_GRAPH>
│ │ │ │ -
219 static Ordering Create(OrderingType orderingType,
│ │ │ │ -
220 const FACTOR_GRAPH& graph) {
│ │ │ │ -
221 if (graph.empty())
│ │ │ │ -
222 return Ordering();
│ │ │ │ -
223
│ │ │ │ -
224 switch (orderingType) {
│ │ │ │ -
225 case COLAMD:
│ │ │ │ -
226 return Colamd(graph);
│ │ │ │ -
227 case METIS:
│ │ │ │ -
228 return Metis(graph);
│ │ │ │ -
229 case NATURAL:
│ │ │ │ -
230 return Natural(graph);
│ │ │ │ -
231 case CUSTOM:
│ │ │ │ -
232 throw std::runtime_error(
│ │ │ │ -
233 "Ordering::Create error: called with CUSTOM ordering type.");
│ │ │ │ -
234 default:
│ │ │ │ -
235 throw std::runtime_error(
│ │ │ │ -
236 "Ordering::Create error: called with unknown ordering type.");
│ │ │ │ -
237 }
│ │ │ │ -
238 }
│ │ │ │ -
239
│ │ │ │ -
241
│ │ │ │ -
244
│ │ │ │ -
245 GTSAM_EXPORT
│ │ │ │ -
246 void print(const std::string& str = "", const KeyFormatter& keyFormatter =
│ │ │ │ -
247 DefaultKeyFormatter) const;
│ │ │ │ -
248
│ │ │ │ -
249 GTSAM_EXPORT
│ │ │ │ -
250 bool equals(const Ordering& other, double tol = 1e-9) const;
│ │ │ │ -
251
│ │ │ │ -
253
│ │ │ │ -
254private:
│ │ │ │ -
256 static GTSAM_EXPORT Ordering ColamdConstrained(
│ │ │ │ -
257 const VariableIndex& variableIndex, std::vector<int>& cmember);
│ │ │ │ -
258
│ │ │ │ -
260 friend class boost::serialization::access;
│ │ │ │ -
261 template<class ARCHIVE>
│ │ │ │ -
262 void serialize(ARCHIVE & ar, const unsigned int version) {
│ │ │ │ -
263 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
264 }
│ │ │ │ -
265};
│ │ │ │ -
│ │ │ │ -
266
│ │ │ │ -
│ │ │ │ -
268template<> struct traits<Ordering> : public Testable<Ordering> {
│ │ │ │ -
269};
│ │ │ │ -
│ │ │ │ -
270
│ │ │ │ -
271}
│ │ │ │ -
272
│ │ │ │ -
FastSet.h
A thin wrapper around std::set that uses boost's fast_pool_allocator.
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ +
90
│ │ │ │ +
91 }
│ │ │ │ +
92}
│ │ │ │ +
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ + │ │ │ │ +
gtsam::inference::EliminateTree
FastVector< typename TREE::sharedFactor > EliminateTree(RESULT &result, const TREE &tree, const typename TREE::Eliminate &function)
Eliminate an elimination tree or a Bayes tree (used internally).
Definition inference-inst.h:75
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ - │ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::MetisIndex
The MetisIndex class converts a factor graph into the Compressed Sparse Row format for use in METIS a...
Definition MetisIndex.h:45
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ -
gtsam::Ordering::Natural
static Ordering Natural(const FACTOR_GRAPH &fg)
Return a natural Ordering. Typically used by iterative solvers.
Definition Ordering.h:190
│ │ │ │ -
gtsam::Ordering::ColamdConstrained
static Ordering ColamdConstrained(const FACTOR_GRAPH &graph, const FastMap< Key, int > &groups)
Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details for note o...
Definition Ordering.h:170
│ │ │ │ -
gtsam::Ordering::Ordering
Ordering(const KEYS &keys)
Create from a container.
Definition Ordering.h:57
│ │ │ │ -
gtsam::Ordering::Colamd
static Ordering Colamd(const FACTOR_GRAPH &graph)
Compute a fill-reducing ordering using COLAMD from a factor graph (see details for note on performanc...
Definition Ordering.h:95
│ │ │ │ -
gtsam::Ordering::OrderingType
OrderingType
Type of ordering to use.
Definition Ordering.h:41
│ │ │ │ -
gtsam::Ordering::ColamdConstrainedLast
static Ordering ColamdConstrainedLast(const FACTOR_GRAPH &graph, const KeyVector &constrainLast, bool forceOrder=false)
Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details for note o...
Definition Ordering.h:114
│ │ │ │ -
gtsam::Ordering::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition Ordering.h:46
│ │ │ │ -
gtsam::Ordering::operator+=
boost::assign::list_inserter< boost::assign_detail::call_push_back< This > > operator+=(Key key)
Add new variables to the ordering as ordering += key1, key2, ... Equivalent to calling push_back.
Definition Ordering.h:63
│ │ │ │ -
gtsam::Ordering::ColamdConstrainedFirst
static Ordering ColamdConstrainedFirst(const FACTOR_GRAPH &graph, const KeyVector &constrainFirst, bool forceOrder=false)
Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details for note o...
Definition Ordering.h:141
│ │ │ │ -
gtsam::Ordering::Ordering
GTSAM_EXPORT Ordering()
Create an empty ordering.
Definition Ordering.h:50
│ │ │ │ -
gtsam::Ordering::Metis
static GTSAM_EXPORT Ordering Metis(const MetisIndex &met)
Compute an ordering determined by METIS from a VariableIndex.
Definition Ordering.cpp:212
│ │ │ │ -
gtsam::Ordering::invert
FastMap< Key, size_t > invert() const
Invert (not reverse) the ordering - returns a map from key to order position.
Definition Ordering.cpp:36
│ │ │ │ -
gtsam::Ordering::access
friend class boost::serialization::access
Serialization function.
Definition Ordering.h:260
│ │ │ │ -
gtsam::Ordering::This
Ordering This
Typedef to this class.
Definition Ordering.h:45
│ │ │ │ -
gtsam::Ordering::contains
bool contains(const Key &key) const
Check if key exists in ordering.
Definition Ordering.cpp:293
│ │ │ │ -
gtsam::Ordering::CSRFormat
static GTSAM_EXPORT void CSRFormat(std::vector< int > &xadj, std::vector< int > &adj, const FACTOR_GRAPH &graph)
METIS Formatting function.
│ │ │ │ -
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition VariableIndex.h:43
│ │ │ │ +
gtsam::treeTraversal::DepthFirstForest
void DepthFirstForest(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, VISITOR_POST &visitorPost)
Traverse a forest depth-first with pre-order and post-order visits.
Definition treeTraversal-inst.h:77
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,312 +1,127 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Ordering.h │ │ │ │ │ +inference-inst.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ -4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -5 * All Rights Reserved │ │ │ │ │ -6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +3* GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4* Atlanta, Georgia 30332-0415 │ │ │ │ │ +5* All Rights Reserved │ │ │ │ │ +6* Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ -8 * See LICENSE for the license information │ │ │ │ │ +8* See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ -10 * ------------------------------------------------------------------------- │ │ │ │ │ +10* ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -21#pragma once │ │ │ │ │ -22 │ │ │ │ │ -23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_V_a_r_i_a_b_l_e_I_n_d_e_x_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_M_e_t_i_s_I_n_d_e_x_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_S_e_t_._h> │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include │ │ │ │ │ +23#include │ │ │ │ │ +24 │ │ │ │ │ +25#include <_g_t_s_a_m_/_b_a_s_e_/_t_r_e_e_T_r_a_v_e_r_s_a_l_-_i_n_s_t_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ 27 │ │ │ │ │ -28#include │ │ │ │ │ -29#include │ │ │ │ │ -30#include │ │ │ │ │ -31 │ │ │ │ │ -32namespace _g_t_s_a_m { │ │ │ │ │ -33 │ │ │ │ │ -_3_4class _O_r_d_e_r_i_n_g: public _K_e_y_V_e_c_t_o_r { │ │ │ │ │ -35protected: │ │ │ │ │ -36 typedef _K_e_y_V_e_c_t_o_r Base; │ │ │ │ │ -37 │ │ │ │ │ -38public: │ │ │ │ │ -39 │ │ │ │ │ -_4_1 enum _O_r_d_e_r_i_n_g_T_y_p_e { │ │ │ │ │ -42 COLAMD, METIS, NATURAL, CUSTOM │ │ │ │ │ -43 }; │ │ │ │ │ -44 │ │ │ │ │ -_4_5 typedef _O_r_d_e_r_i_n_g _T_h_i_s; │ │ │ │ │ -_4_6 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -47 │ │ │ │ │ -49 GTSAM_EXPORT │ │ │ │ │ -_5_0 _O_r_d_e_r_i_n_g() { │ │ │ │ │ -51 } │ │ │ │ │ -52 │ │ │ │ │ -53 using KeyVector::KeyVector; // Inherit the KeyVector's constructors │ │ │ │ │ -54 │ │ │ │ │ -56 template │ │ │ │ │ -_5_7 explicit _O_r_d_e_r_i_n_g(const KEYS& keys) : │ │ │ │ │ -58 Base(keys.begin(), keys.end()) { │ │ │ │ │ -59 } │ │ │ │ │ -60 │ │ │ │ │ -_6_3 boost::assign::list_inserter > │ │ │ │ │ -_o_p_e_r_a_t_o_r_+_=( │ │ │ │ │ -64 _K_e_y key) { │ │ │ │ │ -65 return boost::assign::make_list_inserter( │ │ │ │ │ -66 boost::assign_detail::call_push_back(*this))(key); │ │ │ │ │ +28namespace _g_t_s_a_m { │ │ │ │ │ +29 namespace inference { │ │ │ │ │ +30 │ │ │ │ │ +31 namespace { │ │ │ │ │ +32 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +33 template │ │ │ │ │ +34 struct EliminationData { │ │ │ │ │ +35 EliminationData* const parentData; │ │ │ │ │ +36 FastVector childFactors; │ │ │ │ │ +37 EliminationData(EliminationData* _parentData, size_t nChildren) : │ │ │ │ │ +38 parentData(_parentData) { childFactors.reserve(nChildren); } │ │ │ │ │ +39 }; │ │ │ │ │ +40 │ │ │ │ │ +41 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +42 template │ │ │ │ │ +43 EliminationData eliminationPreOrderVisitor( │ │ │ │ │ +44 const typename TREE::sharedNode& node, EliminationData& parentData) │ │ │ │ │ +45 { │ │ │ │ │ +46 // This function is called before visiting the children. Here, we create │ │ │ │ │ +this node's data, │ │ │ │ │ +47 // which includes a pointer to the parent data and space for the factors of │ │ │ │ │ +the children. │ │ │ │ │ +48 return EliminationData(&parentData, node->children.size()); │ │ │ │ │ +49 } │ │ │ │ │ +50 │ │ │ │ │ +51 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +52 template │ │ │ │ │ +53 struct EliminationPostOrderVisitor │ │ │ │ │ +54 { │ │ │ │ │ +55 RESULT& result; │ │ │ │ │ +56 const typename TREE::Eliminate& eliminationFunction; │ │ │ │ │ +57 EliminationPostOrderVisitor(RESULT& result, const typename TREE::Eliminate& │ │ │ │ │ +eliminationFunction) : │ │ │ │ │ +58 result(result), eliminationFunction(eliminationFunction) {} │ │ │ │ │ +59 void operator()(const typename TREE::sharedNode& node, │ │ │ │ │ +EliminationData& myData) │ │ │ │ │ +60 { │ │ │ │ │ +61 // Call eliminate on the node and add the result to the parent's gathered │ │ │ │ │ +factors │ │ │ │ │ +62 typename TREE::sharedFactor childFactor = node->eliminate(result, │ │ │ │ │ +eliminationFunction, myData.childFactors); │ │ │ │ │ +63 if(childFactor && !childFactor->empty()) │ │ │ │ │ +64 myData.parentData->childFactors.push_back(childFactor); │ │ │ │ │ +65 } │ │ │ │ │ +66 }; │ │ │ │ │ 67 } │ │ │ │ │ 68 │ │ │ │ │ -75 _T_h_i_s& _o_p_e_r_a_t_o_r_+_=(_K_e_y_V_e_c_t_o_r& keys); │ │ │ │ │ -76 │ │ │ │ │ -78 bool _c_o_n_t_a_i_n_s(const _K_e_y& key) const; │ │ │ │ │ -79 │ │ │ │ │ -86 _F_a_s_t_M_a_p_<_K_e_y_,_ _s_i_z_e___t_> _i_n_v_e_r_t() const; │ │ │ │ │ -87 │ │ │ │ │ +69 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +73 template │ │ │ │ │ +74 FastVector │ │ │ │ │ +_7_5 _E_l_i_m_i_n_a_t_e_T_r_e_e(RESULT& result, const TREE& tree, const typename TREE:: │ │ │ │ │ +Eliminate& function) │ │ │ │ │ +76 { │ │ │ │ │ +77 // Do elimination using a depth-first traversal. During the pre-order visit │ │ │ │ │ +(see │ │ │ │ │ +78 // eliminationPreOrderVisitor), we store a pointer to the parent data (where │ │ │ │ │ +we'll put the │ │ │ │ │ +79 // remaining factor) and reserve a vector of factors to store the children │ │ │ │ │ +elimination │ │ │ │ │ +80 // results. During the post-order visit (see eliminationPostOrderVisitor), │ │ │ │ │ +we call dense │ │ │ │ │ +81 // elimination (using the gathered child factors) and store the result in │ │ │ │ │ +the parent's │ │ │ │ │ +82 // gathered factors. │ │ │ │ │ +83 EliminationData rootData(0, tree.roots().size()); │ │ │ │ │ +84 EliminationPostOrderVisitor visitorPost(result, function); │ │ │ │ │ +85 _t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t(tree, rootData, │ │ │ │ │ +eliminationPreOrderVisitor, visitorPost); │ │ │ │ │ +86 │ │ │ │ │ +87 // Return remaining factors │ │ │ │ │ +88 return rootData.childFactors; │ │ │ │ │ +89 } │ │ │ │ │ 90 │ │ │ │ │ -94 template │ │ │ │ │ -_9_5 static _O_r_d_e_r_i_n_g _C_o_l_a_m_d(const FACTOR_GRAPH& graph) { │ │ │ │ │ -96 if (graph.empty()) │ │ │ │ │ -97 return _O_r_d_e_r_i_n_g(); │ │ │ │ │ -98 else │ │ │ │ │ -99 return _C_o_l_a_m_d(_V_a_r_i_a_b_l_e_I_n_d_e_x(graph)); │ │ │ │ │ -100 } │ │ │ │ │ -101 │ │ │ │ │ -103 static GTSAM_EXPORT _O_r_d_e_r_i_n_g _C_o_l_a_m_d(const _V_a_r_i_a_b_l_e_I_n_d_e_x& variableIndex); │ │ │ │ │ -104 │ │ │ │ │ -113 template │ │ │ │ │ -_1_1_4 static _O_r_d_e_r_i_n_g _C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_L_a_s_t(const FACTOR_GRAPH& graph, │ │ │ │ │ -115 const _K_e_y_V_e_c_t_o_r& constrainLast, bool forceOrder = false) { │ │ │ │ │ -116 if (graph.empty()) │ │ │ │ │ -117 return _O_r_d_e_r_i_n_g(); │ │ │ │ │ -118 else │ │ │ │ │ -119 return _C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_L_a_s_t(_V_a_r_i_a_b_l_e_I_n_d_e_x(graph), constrainLast, │ │ │ │ │ -forceOrder); │ │ │ │ │ -120 } │ │ │ │ │ -121 │ │ │ │ │ -128 static GTSAM_EXPORT _O_r_d_e_r_i_n_g _C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_L_a_s_t( │ │ │ │ │ -129 const _V_a_r_i_a_b_l_e_I_n_d_e_x& variableIndex, const _K_e_y_V_e_c_t_o_r& constrainLast, │ │ │ │ │ -130 bool forceOrder = false); │ │ │ │ │ -131 │ │ │ │ │ -140 template │ │ │ │ │ -_1_4_1 static _O_r_d_e_r_i_n_g _C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_F_i_r_s_t(const FACTOR_GRAPH& graph, │ │ │ │ │ -142 const _K_e_y_V_e_c_t_o_r& constrainFirst, bool forceOrder = false) { │ │ │ │ │ -143 if (graph.empty()) │ │ │ │ │ -144 return _O_r_d_e_r_i_n_g(); │ │ │ │ │ -145 else │ │ │ │ │ -146 return _C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_F_i_r_s_t(_V_a_r_i_a_b_l_e_I_n_d_e_x(graph), constrainFirst, │ │ │ │ │ -forceOrder); │ │ │ │ │ -147 } │ │ │ │ │ -148 │ │ │ │ │ -156 static GTSAM_EXPORT _O_r_d_e_r_i_n_g _C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_F_i_r_s_t( │ │ │ │ │ -157 const _V_a_r_i_a_b_l_e_I_n_d_e_x& variableIndex, │ │ │ │ │ -158 const _K_e_y_V_e_c_t_o_r& constrainFirst, bool forceOrder = false); │ │ │ │ │ -159 │ │ │ │ │ -169 template │ │ │ │ │ -_1_7_0 static _O_r_d_e_r_i_n_g _C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d(const FACTOR_GRAPH& graph, │ │ │ │ │ -171 const _F_a_s_t_M_a_p_<_K_e_y_,_ _i_n_t_>& groups) { │ │ │ │ │ -172 if (graph.empty()) │ │ │ │ │ -173 return _O_r_d_e_r_i_n_g(); │ │ │ │ │ -174 else │ │ │ │ │ -175 return _C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d(_V_a_r_i_a_b_l_e_I_n_d_e_x(graph), groups); │ │ │ │ │ -176 } │ │ │ │ │ -177 │ │ │ │ │ -185 static GTSAM_EXPORT _O_r_d_e_r_i_n_g _C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d( │ │ │ │ │ -186 const _V_a_r_i_a_b_l_e_I_n_d_e_x& variableIndex, const _F_a_s_t_M_a_p_<_K_e_y_,_ _i_n_t_>& groups); │ │ │ │ │ -187 │ │ │ │ │ -189 template │ │ │ │ │ -_1_9_0 static _O_r_d_e_r_i_n_g _N_a_t_u_r_a_l(const FACTOR_GRAPH &fg) { │ │ │ │ │ -191 _K_e_y_S_e_t src = fg.keys(); │ │ │ │ │ -192 _K_e_y_V_e_c_t_o_r keys(src.begin(), src.end()); │ │ │ │ │ -193 std::stable_sort(keys.begin(), keys.end()); │ │ │ │ │ -194 return _O_r_d_e_r_i_n_g(keys.begin(), keys.end()); │ │ │ │ │ -195 } │ │ │ │ │ -196 │ │ │ │ │ -198 template │ │ │ │ │ -_1_9_9 static GTSAM_EXPORT void _C_S_R_F_o_r_m_a_t(std::vector& xadj, │ │ │ │ │ -200 std::vector& adj, const FACTOR_GRAPH& graph); │ │ │ │ │ -201 │ │ │ │ │ -203 static GTSAM_EXPORT _O_r_d_e_r_i_n_g _M_e_t_i_s(const _M_e_t_i_s_I_n_d_e_x& met); │ │ │ │ │ -204 │ │ │ │ │ -205 template │ │ │ │ │ -206 static _O_r_d_e_r_i_n_g _M_e_t_i_s(const FACTOR_GRAPH& graph) { │ │ │ │ │ -207 if (graph.empty()) │ │ │ │ │ -208 return _O_r_d_e_r_i_n_g(); │ │ │ │ │ -209 else │ │ │ │ │ -210 return _M_e_t_i_s(_M_e_t_i_s_I_n_d_e_x(graph)); │ │ │ │ │ -211 } │ │ │ │ │ -212 │ │ │ │ │ -214 │ │ │ │ │ -217 │ │ │ │ │ -218 template │ │ │ │ │ -219 static _O_r_d_e_r_i_n_g Create(_O_r_d_e_r_i_n_g_T_y_p_e orderingType, │ │ │ │ │ -220 const FACTOR_GRAPH& graph) { │ │ │ │ │ -221 if (graph.empty()) │ │ │ │ │ -222 return _O_r_d_e_r_i_n_g(); │ │ │ │ │ -223 │ │ │ │ │ -224 switch (orderingType) { │ │ │ │ │ -225 case COLAMD: │ │ │ │ │ -226 return _C_o_l_a_m_d(graph); │ │ │ │ │ -227 case METIS: │ │ │ │ │ -228 return _M_e_t_i_s(graph); │ │ │ │ │ -229 case NATURAL: │ │ │ │ │ -230 return _N_a_t_u_r_a_l(graph); │ │ │ │ │ -231 case CUSTOM: │ │ │ │ │ -232 throw std::runtime_error( │ │ │ │ │ -233 "Ordering::Create error: called with CUSTOM ordering type."); │ │ │ │ │ -234 default: │ │ │ │ │ -235 throw std::runtime_error( │ │ │ │ │ -236 "Ordering::Create error: called with unknown ordering type."); │ │ │ │ │ -237 } │ │ │ │ │ -238 } │ │ │ │ │ -239 │ │ │ │ │ -241 │ │ │ │ │ -244 │ │ │ │ │ -245 GTSAM_EXPORT │ │ │ │ │ -246 void print(const std::string& str = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ -247 DefaultKeyFormatter) const; │ │ │ │ │ -248 │ │ │ │ │ -249 GTSAM_EXPORT │ │ │ │ │ -250 bool equals(const _O_r_d_e_r_i_n_g& other, double tol = 1e-9) const; │ │ │ │ │ -251 │ │ │ │ │ -253 │ │ │ │ │ -254private: │ │ │ │ │ -256 static GTSAM_EXPORT _O_r_d_e_r_i_n_g _C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d( │ │ │ │ │ -257 const VariableIndex& variableIndex, std::vector& cmember); │ │ │ │ │ -258 │ │ │ │ │ -_2_6_0 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -261 template │ │ │ │ │ -262 void serialize(ARCHIVE & ar, const unsigned int version) { │ │ │ │ │ -263 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); │ │ │ │ │ -264 } │ │ │ │ │ -265}; │ │ │ │ │ -266 │ │ │ │ │ -_2_6_8template<> struct _t_r_a_i_t_s<_O_r_d_e_r_i_n_g> : public _T_e_s_t_a_b_l_e { │ │ │ │ │ -269}; │ │ │ │ │ -270 │ │ │ │ │ -271} │ │ │ │ │ -272 │ │ │ │ │ -_F_a_s_t_S_e_t_._h │ │ │ │ │ -A thin wrapper around std::set that uses boost's fast_pool_allocator. │ │ │ │ │ -_K_e_y_._h │ │ │ │ │ -_M_e_t_i_s_I_n_d_e_x_._h │ │ │ │ │ -_V_a_r_i_a_b_l_e_I_n_d_e_x_._h │ │ │ │ │ +91 } │ │ │ │ │ +92} │ │ │ │ │ +_F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ +A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ +_t_r_e_e_T_r_a_v_e_r_s_a_l_-_i_n_s_t_._h │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_f_e_r_e_n_c_e_:_:_E_l_i_m_i_n_a_t_e_T_r_e_e │ │ │ │ │ +FastVector< typename TREE::sharedFactor > EliminateTree(RESULT &result, const │ │ │ │ │ +TREE &tree, const typename TREE::Eliminate &function) │ │ │ │ │ +Eliminate an elimination tree or a Bayes tree (used internally). │ │ │ │ │ +DDeeffiinniittiioonn inference-inst.h:75 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ -FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ -fast_pool_allocator instead of the defa... │ │ │ │ │ -DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_M_e_t_i_s_I_n_d_e_x │ │ │ │ │ -The MetisIndex class converts a factor graph into the Compressed Sparse Row │ │ │ │ │ -format for use in METIS a... │ │ │ │ │ -DDeeffiinniittiioonn MetisIndex.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_N_a_t_u_r_a_l │ │ │ │ │ -static Ordering Natural(const FACTOR_GRAPH &fg) │ │ │ │ │ -Return a natural Ordering. Typically used by iterative solvers. │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:190 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d │ │ │ │ │ -static Ordering ColamdConstrained(const FACTOR_GRAPH &graph, const FastMap< │ │ │ │ │ -Key, int > &groups) │ │ │ │ │ -Compute a fill-reducing ordering using constrained COLAMD from a factor graph │ │ │ │ │ -(see details for note o... │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:170 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -Ordering(const KEYS &keys) │ │ │ │ │ -Create from a container. │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d │ │ │ │ │ -static Ordering Colamd(const FACTOR_GRAPH &graph) │ │ │ │ │ -Compute a fill-reducing ordering using COLAMD from a factor graph (see details │ │ │ │ │ -for note on performanc... │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:95 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_O_r_d_e_r_i_n_g_T_y_p_e │ │ │ │ │ -OrderingType │ │ │ │ │ -Type of ordering to use. │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_L_a_s_t │ │ │ │ │ -static Ordering ColamdConstrainedLast(const FACTOR_GRAPH &graph, const │ │ │ │ │ -KeyVector &constrainLast, bool forceOrder=false) │ │ │ │ │ -Compute a fill-reducing ordering using constrained COLAMD from a factor graph │ │ │ │ │ -(see details for note o... │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:114 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:46 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_o_p_e_r_a_t_o_r_+_= │ │ │ │ │ -boost::assign::list_inserter< boost::assign_detail::call_push_back< This > > │ │ │ │ │ -operator+=(Key key) │ │ │ │ │ -Add new variables to the ordering as ordering += key1, key2, ... Equivalent to │ │ │ │ │ -calling push_back. │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:63 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_F_i_r_s_t │ │ │ │ │ -static Ordering ColamdConstrainedFirst(const FACTOR_GRAPH &graph, const │ │ │ │ │ -KeyVector &constrainFirst, bool forceOrder=false) │ │ │ │ │ -Compute a fill-reducing ordering using constrained COLAMD from a factor graph │ │ │ │ │ -(see details for note o... │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:141 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -GTSAM_EXPORT Ordering() │ │ │ │ │ -Create an empty ordering. │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_M_e_t_i_s │ │ │ │ │ -static GTSAM_EXPORT Ordering Metis(const MetisIndex &met) │ │ │ │ │ -Compute an ordering determined by METIS from a VariableIndex. │ │ │ │ │ -DDeeffiinniittiioonn Ordering.cpp:212 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_i_n_v_e_r_t │ │ │ │ │ -FastMap< Key, size_t > invert() const │ │ │ │ │ -Invert (not reverse) the ordering - returns a map from key to order position. │ │ │ │ │ -DDeeffiinniittiioonn Ordering.cpp:36 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:260 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_T_h_i_s │ │ │ │ │ -Ordering This │ │ │ │ │ -Typedef to this class. │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_c_o_n_t_a_i_n_s │ │ │ │ │ -bool contains(const Key &key) const │ │ │ │ │ -Check if key exists in ordering. │ │ │ │ │ -DDeeffiinniittiioonn Ordering.cpp:293 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_C_S_R_F_o_r_m_a_t │ │ │ │ │ -static GTSAM_EXPORT void CSRFormat(std::vector< int > &xadj, std::vector< int > │ │ │ │ │ -&adj, const FACTOR_GRAPH &graph) │ │ │ │ │ -METIS Formatting function. │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ -The VariableIndex class computes and stores the block column structure of a │ │ │ │ │ -factor graph. │ │ │ │ │ -DDeeffiinniittiioonn VariableIndex.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t │ │ │ │ │ +void DepthFirstForest(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, │ │ │ │ │ +VISITOR_POST &visitorPost) │ │ │ │ │ +Traverse a forest depth-first with pre-order and post-order visits. │ │ │ │ │ +DDeeffiinniittiioonn treeTraversal-inst.h:77 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _O_r_d_e_r_i_n_g_._h │ │ │ │ │ + * _i_n_f_e_r_e_n_c_e_-_i_n_s_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00671_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesNet-inst.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/EliminationTree.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,151 +98,160 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
BayesNet-inst.h
│ │ │ │ +
EliminationTree.h
│ │ │ │
│ │ │ │
│ │ │ │ -
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3* GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4* Atlanta, Georgia 30332-0415
│ │ │ │
5* All Rights Reserved
│ │ │ │
6* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8* See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10* -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/inference/BayesNet.h>
│ │ │ │ -
22#include <gtsam/inference/FactorGraph-inst.h>
│ │ │ │ -
23
│ │ │ │ -
24#include <boost/range/adaptor/reversed.hpp>
│ │ │ │ -
25#include <fstream>
│ │ │ │ -
26#include <string>
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <utility>
│ │ │ │ +
21#include <boost/shared_ptr.hpp>
│ │ │ │ +
22
│ │ │ │ +
23#include <gtsam/base/Testable.h>
│ │ │ │ +
24#include <gtsam/base/FastVector.h>
│ │ │ │ +
25
│ │ │ │ +
26class EliminationTreeTester; // for unit tests, see testEliminationTree
│ │ │ │
27
│ │ │ │
28namespace gtsam {
│ │ │ │
29
│ │ │ │ -
30/* ************************************************************************* */
│ │ │ │ -
31template <class CONDITIONAL>
│ │ │ │ -
│ │ │ │ -
32void BayesNet<CONDITIONAL>::print(const std::string& s,
│ │ │ │ -
33 const KeyFormatter& formatter) const {
│ │ │ │ -
34 std::cout << (s.empty() ? "" : s + " ") << std::endl;
│ │ │ │ -
35 std::cout << "size: " << this->size() << std::endl;
│ │ │ │ -
36 for (size_t i = 0; i < this->size(); i++) {
│ │ │ │ -
37 const auto& conditional = this->at(i);
│ │ │ │ -
38 std::stringstream ss;
│ │ │ │ -
39 ss << "conditional " << i << ": ";
│ │ │ │ -
40 if (conditional) conditional->print(ss.str(), formatter);
│ │ │ │ -
41 }
│ │ │ │ -
42}
│ │ │ │ -
│ │ │ │ -
43
│ │ │ │ -
44/* ************************************************************************* */
│ │ │ │ -
45template <class CONDITIONAL>
│ │ │ │ -
│ │ │ │ -
46void BayesNet<CONDITIONAL>::dot(std::ostream& os,
│ │ │ │ -
47 const KeyFormatter& keyFormatter,
│ │ │ │ -
48 const DotWriter& writer) const {
│ │ │ │ -
49 writer.digraphPreamble(&os);
│ │ │ │ -
50
│ │ │ │ -
51 // Create nodes for each variable in the graph
│ │ │ │ -
52 for (Key key : this->keys()) {
│ │ │ │ -
53 auto position = writer.variablePos(key);
│ │ │ │ -
54 writer.drawVariable(key, keyFormatter, position, &os);
│ │ │ │ -
55 }
│ │ │ │ -
56 os << "\n";
│ │ │ │ -
57
│ │ │ │ -
58 // Reverse order as typically Bayes nets stored in reverse topological sort.
│ │ │ │ -
59 for (auto conditional : boost::adaptors::reverse(*this)) {
│ │ │ │ -
60 auto frontals = conditional->frontals();
│ │ │ │ -
61 const Key me = frontals.front();
│ │ │ │ -
62 auto parents = conditional->parents();
│ │ │ │ -
63 for (const Key& p : parents) {
│ │ │ │ -
64 os << " var" << p << "->var" << me << "\n";
│ │ │ │ -
65 }
│ │ │ │ -
66 }
│ │ │ │ -
67
│ │ │ │ -
68 os << "}";
│ │ │ │ -
69 std::flush(os);
│ │ │ │ -
70}
│ │ │ │ -
│ │ │ │ -
71
│ │ │ │ -
72/* ************************************************************************* */
│ │ │ │ -
73template <class CONDITIONAL>
│ │ │ │ -
│ │ │ │ -
74std::string BayesNet<CONDITIONAL>::dot(const KeyFormatter& keyFormatter,
│ │ │ │ -
75 const DotWriter& writer) const {
│ │ │ │ -
76 std::stringstream ss;
│ │ │ │ -
77 dot(ss, keyFormatter, writer);
│ │ │ │ -
78 return ss.str();
│ │ │ │ -
79}
│ │ │ │ -
│ │ │ │ -
80
│ │ │ │ -
81/* ************************************************************************* */
│ │ │ │ -
82template <class CONDITIONAL>
│ │ │ │ -
│ │ │ │ -
83void BayesNet<CONDITIONAL>::saveGraph(const std::string& filename,
│ │ │ │ -
│ │ │ │ -
84 const KeyFormatter& keyFormatter,
│ │ │ │ -
85 const DotWriter& writer) const {
│ │ │ │ -
86 std::ofstream of(filename.c_str());
│ │ │ │ -
87 dot(of, keyFormatter, writer);
│ │ │ │ -
│ │ │ │ -
88 of.close();
│ │ │ │ -
89}
│ │ │ │ -
│ │ │ │ -
90
│ │ │ │ -
91/* ************************************************************************* */
│ │ │ │ -
92template <class CONDITIONAL>
│ │ │ │ -
93double BayesNet<CONDITIONAL>::logProbability(const HybridValues& x) const {
│ │ │ │ -
94 double sum = 0.;
│ │ │ │ -
95 for (const auto& gc : *this) {
│ │ │ │ -
96 if (gc) sum += gc->logProbability(x);
│ │ │ │ -
97 }
│ │ │ │ -
98 return sum;
│ │ │ │ -
99}
│ │ │ │ -
100
│ │ │ │ -
101/* ************************************************************************* */
│ │ │ │ -
102template <class CONDITIONAL>
│ │ │ │ -
103double BayesNet<CONDITIONAL>::evaluate(const HybridValues& x) const {
│ │ │ │ -
104 return exp(-logProbability(x));
│ │ │ │ -
105}
│ │ │ │ -
106
│ │ │ │ -
107/* ************************************************************************* */
│ │ │ │ -
108
│ │ │ │ -
109} // namespace gtsam
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
FactorGraph-inst.h
Factor Graph Base Class.
│ │ │ │ -
BayesNet.h
Bayes network.
│ │ │ │ +
30 class VariableIndex;
│ │ │ │ +
31 class Ordering;
│ │ │ │ +
32
│ │ │ │ +
50 template<class BAYESNET, class GRAPH>
│ │ │ │ +
│ │ │ │ +
51 class EliminationTree
│ │ │ │ +
52 {
│ │ │ │ +
53 protected:
│ │ │ │ +
54 typedef EliminationTree<BAYESNET, GRAPH> This;
│ │ │ │ +
55 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
56
│ │ │ │ +
57 public:
│ │ │ │ +
58 typedef GRAPH FactorGraphType;
│ │ │ │ +
59 typedef typename GRAPH::FactorType FactorType;
│ │ │ │ +
60 typedef typename boost::shared_ptr<FactorType> sharedFactor;
│ │ │ │ +
61 typedef BAYESNET BayesNetType;
│ │ │ │ +
62 typedef typename BayesNetType::ConditionalType ConditionalType;
│ │ │ │ +
63 typedef typename boost::shared_ptr<ConditionalType> sharedConditional;
│ │ │ │ +
64 typedef typename GRAPH::Eliminate Eliminate;
│ │ │ │ +
65
│ │ │ │ +
│ │ │ │ +
66 struct Node {
│ │ │ │ +
67 typedef FastVector<sharedFactor> Factors;
│ │ │ │ +
68 typedef FastVector<boost::shared_ptr<Node> > Children;
│ │ │ │ +
69
│ │ │ │ +
70 Key key;
│ │ │ │ +
71 Factors factors;
│ │ │ │ +
72 Children children;
│ │ │ │ +
73
│ │ │ │ +
74 sharedFactor eliminate(const boost::shared_ptr<BayesNetType>& output,
│ │ │ │ +
75 const Eliminate& function, const FastVector<sharedFactor>& childrenFactors) const;
│ │ │ │ +
76
│ │ │ │ +
77 void print(const std::string& str, const KeyFormatter& keyFormatter) const;
│ │ │ │ +
78 };
│ │ │ │ +
│ │ │ │ +
79
│ │ │ │ +
80 typedef boost::shared_ptr<Node> sharedNode;
│ │ │ │ +
81
│ │ │ │ +
82 protected:
│ │ │ │ +
84 GTSAM_CONCEPT_TESTABLE_TYPE(FactorType)
│ │ │ │ +
85
│ │ │ │ +
86 FastVector<sharedNode> roots_;
│ │ │ │ +
87 FastVector<sharedFactor> remainingFactors_;
│ │ │ │ +
88
│ │ │ │ +
91
│ │ │ │ +
100 EliminationTree(const FactorGraphType& factorGraph,
│ │ │ │ +
101 const VariableIndex& structure, const Ordering& order);
│ │ │ │ +
102
│ │ │ │ +
108 EliminationTree(const FactorGraphType& factorGraph, const Ordering& order);
│ │ │ │ +
109
│ │ │ │ +
112 EliminationTree(const This& other) { *this = other; }
│ │ │ │ +
113
│ │ │ │ +
116 This& operator=(const This& other);
│ │ │ │ +
117
│ │ │ │ +
119
│ │ │ │ +
120 public:
│ │ │ │ +
123
│ │ │ │ +
129 std::pair<boost::shared_ptr<BayesNetType>, boost::shared_ptr<FactorGraphType> >
│ │ │ │ +
130 eliminate(Eliminate function) const;
│ │ │ │ +
131
│ │ │ │ +
135
│ │ │ │ +
137 void print(const std::string& name = "EliminationTree: ",
│ │ │ │ +
138 const KeyFormatter& formatter = DefaultKeyFormatter) const;
│ │ │ │ +
139
│ │ │ │ +
140 protected:
│ │ │ │ +
142 bool equals(const This& other, double tol = 1e-9) const;
│ │ │ │ +
143
│ │ │ │ +
145
│ │ │ │ +
146 public:
│ │ │ │ +
149
│ │ │ │ +
151 const FastVector<sharedNode>& roots() const { return roots_; }
│ │ │ │ +
152
│ │ │ │ +
154 const FastVector<sharedFactor>& remainingFactors() const { return remainingFactors_; }
│ │ │ │ +
155
│ │ │ │ +
157 void swap(This& other);
│ │ │ │ +
158
│ │ │ │ +
159 protected:
│ │ │ │ +
161 EliminationTree() {}
│ │ │ │ +
162
│ │ │ │ +
163 private:
│ │ │ │ +
165 friend class ::EliminationTreeTester;
│ │ │ │ +
166 };
│ │ │ │ +
│ │ │ │ +
167
│ │ │ │ +
168}
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ +
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ -
gtsam::BayesNet
A BayesNet is a tree of conditionals, stored in elimination order.
Definition BayesNet.h:35
│ │ │ │ -
gtsam::BayesNet::print
void print(const std::string &s="BayesNet", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print out graph
Definition BayesNet-inst.h:32
│ │ │ │ -
gtsam::BayesNet::dot
void dot(std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
Output to graphviz format, stream version.
Definition BayesNet-inst.h:46
│ │ │ │ -
gtsam::BayesNet::saveGraph
void saveGraph(const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
output to file with graphviz format.
Definition BayesNet-inst.h:83
│ │ │ │ -
gtsam::DotWriter
DotWriter is a helper class for writing graphviz .dot files.
Definition DotWriter.h:35
│ │ │ │ -
gtsam::DotWriter::drawVariable
void drawVariable(Key key, const KeyFormatter &keyFormatter, const boost::optional< Vector2 > &position, std::ostream *os) const
Create a variable dot fragment.
Definition DotWriter.cpp:42
│ │ │ │ -
gtsam::DotWriter::digraphPreamble
void digraphPreamble(std::ostream *os) const
Write out preamble for digraph, including size.
Definition DotWriter.cpp:36
│ │ │ │ -
gtsam::DotWriter::variablePos
boost::optional< Vector2 > variablePos(Key key) const
Return variable position or none.
Definition DotWriter.cpp:79
│ │ │ │ -
HybridValues
the error.
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::EliminationTree
An elimination tree is a data structure used intermediately during elimination.
Definition EliminationTree.h:52
│ │ │ │ +
gtsam::EliminationTree::print
void print(const std::string &name="EliminationTree: ", const KeyFormatter &formatter=DefaultKeyFormatter) const
Print the tree to cout.
Definition EliminationTree-inst.h:207
│ │ │ │ +
gtsam::EliminationTree::eliminate
std::pair< boost::shared_ptr< BayesNetType >, boost::shared_ptr< FactorGraphType > > eliminate(Eliminate function) const
Eliminate the factors to a Bayes net and remaining factor graph.
Definition EliminationTree-inst.h:187
│ │ │ │ +
gtsam::EliminationTree::operator=
This & operator=(const This &other)
Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are copie...
Definition EliminationTree-inst.h:172
│ │ │ │ +
gtsam::EliminationTree::This
EliminationTree< BAYESNET, GRAPH > This
This class.
Definition EliminationTree.h:54
│ │ │ │ +
gtsam::EliminationTree::roots_
FastVector< sharedNode > roots_
concept check
Definition EliminationTree.h:86
│ │ │ │ +
gtsam::EliminationTree::sharedFactor
boost::shared_ptr< FactorType > sharedFactor
Shared pointer to a factor.
Definition EliminationTree.h:60
│ │ │ │ +
gtsam::EliminationTree::BayesNetType
BAYESNET BayesNetType
The BayesNet corresponding to FACTOR.
Definition EliminationTree.h:61
│ │ │ │ +
gtsam::EliminationTree::sharedConditional
boost::shared_ptr< ConditionalType > sharedConditional
Shared pointer to a conditional.
Definition EliminationTree.h:63
│ │ │ │ +
gtsam::EliminationTree::roots
const FastVector< sharedNode > & roots() const
Return the set of roots (one for a tree, multiple for a forest)
Definition EliminationTree.h:151
│ │ │ │ +
gtsam::EliminationTree::FactorGraphType
GRAPH FactorGraphType
The factor graph type.
Definition EliminationTree.h:58
│ │ │ │ +
gtsam::EliminationTree::swap
void swap(This &other)
Swap the data of this tree with another one, this operation is very fast.
Definition EliminationTree-inst.h:283
│ │ │ │ +
gtsam::EliminationTree::EliminationTree
EliminationTree()
Protected default constructor.
Definition EliminationTree.h:161
│ │ │ │ +
gtsam::EliminationTree::ConditionalType
BayesNetType::ConditionalType ConditionalType
The type of conditionals.
Definition EliminationTree.h:62
│ │ │ │ +
gtsam::EliminationTree::shared_ptr
boost::shared_ptr< This > shared_ptr
Shared pointer to this class.
Definition EliminationTree.h:55
│ │ │ │ +
gtsam::EliminationTree::FactorType
GRAPH::FactorType FactorType
The type of factors.
Definition EliminationTree.h:59
│ │ │ │ +
gtsam::EliminationTree::sharedNode
boost::shared_ptr< Node > sharedNode
Shared pointer to Node.
Definition EliminationTree.h:80
│ │ │ │ +
gtsam::EliminationTree::remainingFactors
const FastVector< sharedFactor > & remainingFactors() const
Return the remaining factors that are not pulled into elimination.
Definition EliminationTree.h:154
│ │ │ │ +
gtsam::EliminationTree::Node
Definition EliminationTree.h:66
│ │ │ │ +
gtsam::EliminationTree::Node::key
Key key
key associated with root
Definition EliminationTree.h:70
│ │ │ │ +
gtsam::EliminationTree::Node::children
Children children
sub-trees
Definition EliminationTree.h:72
│ │ │ │ +
gtsam::EliminationTree::Node::factors
Factors factors
factors associated with root
Definition EliminationTree.h:71
│ │ │ │ +
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition VariableIndex.h:43
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,179 +1,237 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -BayesNet-inst.h │ │ │ │ │ +EliminationTree.h │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3* GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4* Atlanta, Georgia 30332-0415 │ │ │ │ │ 5* All Rights Reserved │ │ │ │ │ 6* Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8* See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10* ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_-_i_n_s_t_._h> │ │ │ │ │ -23 │ │ │ │ │ -24#include │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include │ │ │ │ │ +21#include │ │ │ │ │ +22 │ │ │ │ │ +23#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ +25 │ │ │ │ │ +26class EliminationTreeTester; // for unit tests, see testEliminationTree │ │ │ │ │ 27 │ │ │ │ │ 28namespace _g_t_s_a_m { │ │ │ │ │ 29 │ │ │ │ │ -30/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -31template │ │ │ │ │ -_3_2void _B_a_y_e_s_N_e_t_<_C_O_N_D_I_T_I_O_N_A_L_>_:_:_p_r_i_n_t(const std::string& s, │ │ │ │ │ -33 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter) const { │ │ │ │ │ -34 std::cout << (s.empty() ? "" : s + " ") << std::endl; │ │ │ │ │ -35 std::cout << "size: " << this->size() << std::endl; │ │ │ │ │ -36 for (size_t i = 0; i < this->size(); i++) { │ │ │ │ │ -37 const auto& conditional = this->at(i); │ │ │ │ │ -38 std::stringstream ss; │ │ │ │ │ -39 ss << "conditional " << i << ": "; │ │ │ │ │ -40 if (conditional) conditional->print(ss.str(), formatter); │ │ │ │ │ -41 } │ │ │ │ │ -42} │ │ │ │ │ -43 │ │ │ │ │ -44/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -45template │ │ │ │ │ -_4_6void _B_a_y_e_s_N_e_t_<_C_O_N_D_I_T_I_O_N_A_L_>_:_:_d_o_t(std::ostream& os, │ │ │ │ │ -47 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter, │ │ │ │ │ -48 const _D_o_t_W_r_i_t_e_r& writer) const { │ │ │ │ │ -49 writer._d_i_g_r_a_p_h_P_r_e_a_m_b_l_e(&os); │ │ │ │ │ -50 │ │ │ │ │ -51 // Create nodes for each variable in the graph │ │ │ │ │ -52 for (_K_e_y key : this->keys()) { │ │ │ │ │ -53 auto position = writer._v_a_r_i_a_b_l_e_P_o_s(key); │ │ │ │ │ -54 writer._d_r_a_w_V_a_r_i_a_b_l_e(key, keyFormatter, position, &os); │ │ │ │ │ -55 } │ │ │ │ │ -56 os << "\n"; │ │ │ │ │ -57 │ │ │ │ │ -58 // Reverse order as typically Bayes nets stored in reverse topological sort. │ │ │ │ │ -59 for (auto conditional : boost::adaptors::reverse(*this)) { │ │ │ │ │ -60 auto frontals = conditional->frontals(); │ │ │ │ │ -61 const _K_e_y me = frontals.front(); │ │ │ │ │ -62 auto parents = conditional->parents(); │ │ │ │ │ -63 for (const _K_e_y& p : parents) { │ │ │ │ │ -64 os << " var" << p << "->var" << me << "\n"; │ │ │ │ │ -65 } │ │ │ │ │ -66 } │ │ │ │ │ -67 │ │ │ │ │ -68 os << "}"; │ │ │ │ │ -69 std::flush(os); │ │ │ │ │ -_7_0} │ │ │ │ │ -71 │ │ │ │ │ -72/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -73template │ │ │ │ │ -_7_4std::string _B_a_y_e_s_N_e_t_<_C_O_N_D_I_T_I_O_N_A_L_>_:_:_d_o_t(const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter, │ │ │ │ │ -75 const _D_o_t_W_r_i_t_e_r& writer) const { │ │ │ │ │ -76 std::stringstream ss; │ │ │ │ │ -77 _d_o_t(ss, keyFormatter, writer); │ │ │ │ │ -78 return ss.str(); │ │ │ │ │ -_7_9} │ │ │ │ │ -80 │ │ │ │ │ -81/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -82template │ │ │ │ │ -_8_3void _B_a_y_e_s_N_e_t_<_C_O_N_D_I_T_I_O_N_A_L_>_:_:_s_a_v_e_G_r_a_p_h(const std::string& filename, │ │ │ │ │ -_8_4 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter, │ │ │ │ │ -85 const _D_o_t_W_r_i_t_e_r& writer) const { │ │ │ │ │ -86 std::ofstream of(filename.c_str()); │ │ │ │ │ -87 _d_o_t(of, keyFormatter, writer); │ │ │ │ │ -_8_8 of.close(); │ │ │ │ │ -89} │ │ │ │ │ -90 │ │ │ │ │ -91/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -92template │ │ │ │ │ -93double _B_a_y_e_s_N_e_t_<_C_O_N_D_I_T_I_O_N_A_L_>_:_:_l_o_g_P_r_o_b_a_b_i_l_i_t_y(const _H_y_b_r_i_d_V_a_l_u_e_s& x) const { │ │ │ │ │ -94 double sum = 0.; │ │ │ │ │ -95 for (const auto& gc : *this) { │ │ │ │ │ -96 if (gc) sum += gc->logProbability(x); │ │ │ │ │ -97 } │ │ │ │ │ -98 return sum; │ │ │ │ │ -99} │ │ │ │ │ -100 │ │ │ │ │ -101/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -102template │ │ │ │ │ -103double BayesNet::evaluate(const _H_y_b_r_i_d_V_a_l_u_e_s& x) const { │ │ │ │ │ -104 return exp(-logProbability(x)); │ │ │ │ │ -105} │ │ │ │ │ -106 │ │ │ │ │ -107/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -108 │ │ │ │ │ -109} // namespace gtsam │ │ │ │ │ -_F_a_c_t_o_r_G_r_a_p_h_-_i_n_s_t_._h │ │ │ │ │ -Factor Graph Base Class. │ │ │ │ │ -_B_a_y_e_s_N_e_t_._h │ │ │ │ │ -Bayes network. │ │ │ │ │ +30 class VariableIndex; │ │ │ │ │ +31 class Ordering; │ │ │ │ │ +32 │ │ │ │ │ +50 template │ │ │ │ │ +_5_1 class _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ +52 { │ │ │ │ │ +53 protected: │ │ │ │ │ +_5_4 typedef _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_B_A_Y_E_S_N_E_T_,_ _G_R_A_P_H_> _T_h_i_s; │ │ │ │ │ +_5_5 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +56 │ │ │ │ │ +57 public: │ │ │ │ │ +_5_8 typedef GRAPH _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e; │ │ │ │ │ +_5_9 typedef typename GRAPH::FactorType _F_a_c_t_o_r_T_y_p_e; │ │ │ │ │ +_6_0 typedef typename boost::shared_ptr _s_h_a_r_e_d_F_a_c_t_o_r; │ │ │ │ │ +_6_1 typedef BAYESNET _B_a_y_e_s_N_e_t_T_y_p_e; │ │ │ │ │ +_6_2 typedef typename BayesNetType::ConditionalType _C_o_n_d_i_t_i_o_n_a_l_T_y_p_e; │ │ │ │ │ +_6_3 typedef typename boost::shared_ptr _s_h_a_r_e_d_C_o_n_d_i_t_i_o_n_a_l; │ │ │ │ │ +64 typedef typename GRAPH::Eliminate Eliminate; │ │ │ │ │ +65 │ │ │ │ │ +_6_6 struct _N_o_d_e { │ │ │ │ │ +67 typedef _F_a_s_t_V_e_c_t_o_r_<_s_h_a_r_e_d_F_a_c_t_o_r_> Factors; │ │ │ │ │ +68 typedef _F_a_s_t_V_e_c_t_o_r_<_b_o_o_s_t_:_:_s_h_a_r_e_d___p_t_r_<_N_o_d_e_> > Children; │ │ │ │ │ +69 │ │ │ │ │ +_7_0 _K_e_y _k_e_y; │ │ │ │ │ +_7_1 Factors _f_a_c_t_o_r_s; │ │ │ │ │ +_7_2 Children _c_h_i_l_d_r_e_n; │ │ │ │ │ +73 │ │ │ │ │ +74 _s_h_a_r_e_d_F_a_c_t_o_r eliminate(const boost::shared_ptr& output, │ │ │ │ │ +75 const Eliminate& function, const _F_a_s_t_V_e_c_t_o_r_<_s_h_a_r_e_d_F_a_c_t_o_r_>& childrenFactors) │ │ │ │ │ +const; │ │ │ │ │ +76 │ │ │ │ │ +77 void print(const std::string& str, const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter) const; │ │ │ │ │ +78 }; │ │ │ │ │ +79 │ │ │ │ │ +_8_0 typedef boost::shared_ptr _s_h_a_r_e_d_N_o_d_e; │ │ │ │ │ +81 │ │ │ │ │ +82 protected: │ │ │ │ │ +84 GTSAM_CONCEPT_TESTABLE_TYPE(_F_a_c_t_o_r_T_y_p_e) │ │ │ │ │ +85 │ │ │ │ │ +_8_6 _F_a_s_t_V_e_c_t_o_r<_s_h_a_r_e_d_N_o_d_e> _r_o_o_t_s__; │ │ │ │ │ +87 _F_a_s_t_V_e_c_t_o_r<_s_h_a_r_e_d_F_a_c_t_o_r> remainingFactors_; │ │ │ │ │ +88 │ │ │ │ │ +91 │ │ │ │ │ +100 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e(const _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e& factorGraph, │ │ │ │ │ +101 const _V_a_r_i_a_b_l_e_I_n_d_e_x& structure, const _O_r_d_e_r_i_n_g& order); │ │ │ │ │ +102 │ │ │ │ │ +108 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e(const _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e& factorGraph, const _O_r_d_e_r_i_n_g& order); │ │ │ │ │ +109 │ │ │ │ │ +_1_1_2 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e(const _T_h_i_s& other) { *this = other; } │ │ │ │ │ +113 │ │ │ │ │ +116 _T_h_i_s& _o_p_e_r_a_t_o_r_=(const _T_h_i_s& other); │ │ │ │ │ +117 │ │ │ │ │ +119 │ │ │ │ │ +120 public: │ │ │ │ │ +123 │ │ │ │ │ +129 std::pair, boost:: │ │ │ │ │ +shared_ptr > │ │ │ │ │ +130 _e_l_i_m_i_n_a_t_e(Eliminate function) const; │ │ │ │ │ +131 │ │ │ │ │ +135 │ │ │ │ │ +137 void _p_r_i_n_t(const std::string& name = "EliminationTree: ", │ │ │ │ │ +138 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const; │ │ │ │ │ +139 │ │ │ │ │ +140 protected: │ │ │ │ │ +142 bool _e_q_u_a_l_s(const _T_h_i_s& other, double tol = 1e-9) const; │ │ │ │ │ +143 │ │ │ │ │ +145 │ │ │ │ │ +146 public: │ │ │ │ │ +149 │ │ │ │ │ +_1_5_1 const _F_a_s_t_V_e_c_t_o_r_<_s_h_a_r_e_d_N_o_d_e_>& _r_o_o_t_s() const { return _r_o_o_t_s__; } │ │ │ │ │ +152 │ │ │ │ │ +_1_5_4 const _F_a_s_t_V_e_c_t_o_r_<_s_h_a_r_e_d_F_a_c_t_o_r_>& _r_e_m_a_i_n_i_n_g_F_a_c_t_o_r_s() const { return │ │ │ │ │ +remainingFactors_; } │ │ │ │ │ +155 │ │ │ │ │ +157 void _s_w_a_p(_T_h_i_s& other); │ │ │ │ │ +158 │ │ │ │ │ +159 protected: │ │ │ │ │ +_1_6_1 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e() {} │ │ │ │ │ +162 │ │ │ │ │ +163 private: │ │ │ │ │ +_1_6_5 friend class ::EliminationTreeTester; │ │ │ │ │ +166 }; │ │ │ │ │ +167 │ │ │ │ │ +168} │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ +_F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ +A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ +std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ +FastVector │ │ │ │ │ +FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ +DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_d_o_t │ │ │ │ │ -double dot(const V1 &a, const V2 &b) │ │ │ │ │ -Dot product. │ │ │ │ │ -DDeeffiinniittiioonn Vector.h:195 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t │ │ │ │ │ -A BayesNet is a tree of conditionals, stored in elimination order. │ │ │ │ │ -DDeeffiinniittiioonn BayesNet.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="BayesNet", const KeyFormatter │ │ │ │ │ -&formatter=DefaultKeyFormatter) const override │ │ │ │ │ -print out graph │ │ │ │ │ -DDeeffiinniittiioonn BayesNet-inst.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t_:_:_d_o_t │ │ │ │ │ -void dot(std::ostream &os, const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const │ │ │ │ │ -Output to graphviz format, stream version. │ │ │ │ │ -DDeeffiinniittiioonn BayesNet-inst.h:46 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t_:_:_s_a_v_e_G_r_a_p_h │ │ │ │ │ -void saveGraph(const std::string &filename, const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const │ │ │ │ │ -output to file with graphviz format. │ │ │ │ │ -DDeeffiinniittiioonn BayesNet-inst.h:83 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r │ │ │ │ │ -DotWriter is a helper class for writing graphviz .dot files. │ │ │ │ │ -DDeeffiinniittiioonn DotWriter.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_d_r_a_w_V_a_r_i_a_b_l_e │ │ │ │ │ -void drawVariable(Key key, const KeyFormatter &keyFormatter, const boost:: │ │ │ │ │ -optional< Vector2 > &position, std::ostream *os) const │ │ │ │ │ -Create a variable dot fragment. │ │ │ │ │ -DDeeffiinniittiioonn DotWriter.cpp:42 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_d_i_g_r_a_p_h_P_r_e_a_m_b_l_e │ │ │ │ │ -void digraphPreamble(std::ostream *os) const │ │ │ │ │ -Write out preamble for digraph, including size. │ │ │ │ │ -DDeeffiinniittiioonn DotWriter.cpp:36 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_v_a_r_i_a_b_l_e_P_o_s │ │ │ │ │ -boost::optional< Vector2 > variablePos(Key key) const │ │ │ │ │ -Return variable position or none. │ │ │ │ │ -DDeeffiinniittiioonn DotWriter.cpp:79 │ │ │ │ │ -_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -the error. │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ +An elimination tree is a data structure used intermediately during elimination. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &name="EliminationTree: ", const KeyFormatter │ │ │ │ │ +&formatter=DefaultKeyFormatter) const │ │ │ │ │ +Print the tree to cout. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree-inst.h:207 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_e_l_i_m_i_n_a_t_e │ │ │ │ │ +std::pair< boost::shared_ptr< BayesNetType >, boost::shared_ptr< │ │ │ │ │ +FactorGraphType > > eliminate(Eliminate function) const │ │ │ │ │ +Eliminate the factors to a Bayes net and remaining factor graph. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree-inst.h:187 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ +This & operator=(const This &other) │ │ │ │ │ +Assignment operator - makes a deep copy of the tree structure, but only │ │ │ │ │ +pointers to factors are copie... │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree-inst.h:172 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_T_h_i_s │ │ │ │ │ +EliminationTree< BAYESNET, GRAPH > This │ │ │ │ │ +This class. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:54 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_r_o_o_t_s__ │ │ │ │ │ +FastVector< sharedNode > roots_ │ │ │ │ │ +concept check │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_s_h_a_r_e_d_F_a_c_t_o_r │ │ │ │ │ +boost::shared_ptr< FactorType > sharedFactor │ │ │ │ │ +Shared pointer to a factor. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:60 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_B_a_y_e_s_N_e_t_T_y_p_e │ │ │ │ │ +BAYESNET BayesNetType │ │ │ │ │ +The BayesNet corresponding to FACTOR. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:61 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_s_h_a_r_e_d_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +boost::shared_ptr< ConditionalType > sharedConditional │ │ │ │ │ +Shared pointer to a conditional. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:63 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_r_o_o_t_s │ │ │ │ │ +const FastVector< sharedNode > & roots() const │ │ │ │ │ +Return the set of roots (one for a tree, multiple for a forest) │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_F_a_c_t_o_r_G_r_a_p_h_T_y_p_e │ │ │ │ │ +GRAPH FactorGraphType │ │ │ │ │ +The factor graph type. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_s_w_a_p │ │ │ │ │ +void swap(This &other) │ │ │ │ │ +Swap the data of this tree with another one, this operation is very fast. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree-inst.h:283 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ +EliminationTree() │ │ │ │ │ +Protected default constructor. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:161 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_C_o_n_d_i_t_i_o_n_a_l_T_y_p_e │ │ │ │ │ +BayesNetType::ConditionalType ConditionalType │ │ │ │ │ +The type of conditionals. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:62 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +Shared pointer to this class. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_F_a_c_t_o_r_T_y_p_e │ │ │ │ │ +GRAPH::FactorType FactorType │ │ │ │ │ +The type of factors. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:59 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_s_h_a_r_e_d_N_o_d_e │ │ │ │ │ +boost::shared_ptr< Node > sharedNode │ │ │ │ │ +Shared pointer to Node. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:80 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_r_e_m_a_i_n_i_n_g_F_a_c_t_o_r_s │ │ │ │ │ +const FastVector< sharedFactor > & remainingFactors() const │ │ │ │ │ +Return the remaining factors that are not pulled into elimination. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:154 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_N_o_d_e │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:66 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_N_o_d_e_:_:_k_e_y │ │ │ │ │ +Key key │ │ │ │ │ +key associated with root │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:70 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_N_o_d_e_:_:_c_h_i_l_d_r_e_n │ │ │ │ │ +Children children │ │ │ │ │ +sub-trees │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_N_o_d_e_:_:_f_a_c_t_o_r_s │ │ │ │ │ +Factors factors │ │ │ │ │ +factors associated with root │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:71 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ +The VariableIndex class computes and stores the block column structure of a │ │ │ │ │ +factor graph. │ │ │ │ │ +DDeeffiinniittiioonn VariableIndex.h:43 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * BBaayyeessNNeett--iinnsstt..hh │ │ │ │ │ + * _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00674.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/inferenceExceptions.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/ClusterTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,43 +96,53 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
inferenceExceptions.h File Reference
│ │ │ │ +
ClusterTree.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Exceptions that may be thrown by inference algorithms. │ │ │ │ +

Collects factorgraph fragments defined on variable clusters, arranged in a tree. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::InconsistentEliminationRequested
 An inference algorithm was called with inconsistent arguments. More...
class  gtsam::ClusterTree< GRAPH >
 A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman: each node k represents a subset \( C_k \sub X \), and the tree is family preserving, in that each factor \( f_i \) is associated with a single cluster and \( scope(f_i) \sub C_k \). More...
 
struct  gtsam::ClusterTree< GRAPH >::Cluster
 A Cluster is just a collection of factors. More...
 
class  gtsam::EliminatableClusterTree< BAYESTREE, GRAPH >
 A cluster-tree that eliminates to a Bayes tree. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Exceptions that may be thrown by inference algorithms.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Apr 25, 2013
│ │ │ │ +

Collects factorgraph fragments defined on variable clusters, arranged in a tree.

│ │ │ │ +
Date
Oct 8, 2013
│ │ │ │ +
Author
Kai Ni
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,27 +1,41 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -inferenceExceptions.h File Reference │ │ │ │ │ -Exceptions that may be thrown by inference algorithms. _M_o_r_e_._._. │ │ │ │ │ +ClusterTree.h File Reference │ │ │ │ │ +Collects factorgraph fragments defined on variable clusters, arranged in a │ │ │ │ │ +tree. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_I_n_c_o_n_s_i_s_t_e_n_t_E_l_i_m_i_n_a_t_i_o_n_R_e_q_u_e_s_t_e_d │ │ │ │ │ -  An inference algorithm was called with inconsistent arguments. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_<_ _G_R_A_P_H_ _> │ │ │ │ │ + A cluster-tree is associated with a factor graph and is defined as in │ │ │ │ │ + Koller-Friedman: each node k represents a subset \( C_k \sub X \), and │ │ │ │ │ +  the tree is family preserving, in that each factor \( f_i \) is │ │ │ │ │ + associated with a single cluster and \( scope(f_i) \sub C_k \). │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_<_ _G_R_A_P_H_ _>_:_:_C_l_u_s_t_e_r │ │ │ │ │ +  A _C_l_u_s_t_e_r is just a collection of factors. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_<_ _B_A_Y_E_S_T_R_E_E_,_ _G_R_A_P_H_ _> │ │ │ │ │ +  A cluster-tree that eliminates to a Bayes tree. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Exceptions that may be thrown by inference algorithms. │ │ │ │ │ +Collects factorgraph fragments defined on variable clusters, arranged in a │ │ │ │ │ +tree. │ │ │ │ │ + Date │ │ │ │ │ + Oct 8, 2013 │ │ │ │ │ Author │ │ │ │ │ + Kai Ni │ │ │ │ │ Richard Roberts │ │ │ │ │ - Date │ │ │ │ │ - Apr 25, 2013 │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _i_n_f_e_r_e_n_c_e_E_x_c_e_p_t_i_o_n_s_._h │ │ │ │ │ + * _C_l_u_s_t_e_r_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00674.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,5 @@ │ │ │ │ │ var a00674 = [ │ │ │ │ │ - ["gtsam::InconsistentEliminationRequested", "a03636.html", null] │ │ │ │ │ + ["gtsam::ClusterTree< GRAPH >", "a03564.html", "a03564"], │ │ │ │ │ + ["gtsam::ClusterTree< GRAPH >::Cluster", "a03568.html", "a03568"], │ │ │ │ │ + ["gtsam::EliminatableClusterTree< BAYESTREE, GRAPH >", "a03528.html", "a03528"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00674_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/inferenceExceptions.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/ClusterTree.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,59 +98,273 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
inferenceExceptions.h
│ │ │ │ +
ClusterTree.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ -
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ -
4 * Atlanta, Georgia 30332-0415
│ │ │ │ -
5 * All Rights Reserved
│ │ │ │ -
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ -
7
│ │ │ │ -
8 * See LICENSE for the license information
│ │ │ │ -
9
│ │ │ │ -
10 * -------------------------------------------------------------------------- */
│ │ │ │ +Go to the documentation of this file.
1
│ │ │ │ +
10#pragma once
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/global_includes.h>
│ │ │ │ -
21#include <boost/lexical_cast.hpp>
│ │ │ │ -
22#include <exception>
│ │ │ │ -
23
│ │ │ │ -
24namespace gtsam {
│ │ │ │ -
25
│ │ │ │ -
│ │ │ │ -
29 class InconsistentEliminationRequested : public std::exception {
│ │ │ │ -
30 public:
│ │ │ │ -
31 InconsistentEliminationRequested() noexcept {}
│ │ │ │ -
32 ~InconsistentEliminationRequested() noexcept override {}
│ │ │ │ -
33 const char* what() const noexcept override {
│ │ │ │ -
34 return
│ │ │ │ -
35 "An inference algorithm was called with inconsistent arguments. The\n"
│ │ │ │ -
36 "factor graph, ordering, or variable index were inconsistent with each\n"
│ │ │ │ -
37 "other, or a full elimination routine was called with an ordering that\n"
│ │ │ │ -
38 "does not include all of the variables.";
│ │ │ │ -
39 }
│ │ │ │ -
40 };
│ │ │ │ -
│ │ │ │ -
41
│ │ │ │ -
42}
│ │ │ │ -
global_includes.h
Included from all GTSAM files.
│ │ │ │ +
12#include <gtsam/base/Testable.h>
│ │ │ │ +
13#include <gtsam/base/FastVector.h>
│ │ │ │ +
14#include <gtsam/inference/Ordering.h>
│ │ │ │ +
15
│ │ │ │ +
16namespace gtsam {
│ │ │ │ +
17
│ │ │ │ +
24template <class GRAPH>
│ │ │ │ +
│ │ │ │ +
25class ClusterTree {
│ │ │ │ +
26 public:
│ │ │ │ +
27 typedef GRAPH FactorGraphType;
│ │ │ │ +
28 typedef ClusterTree<GRAPH> This;
│ │ │ │ +
29 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
30
│ │ │ │ +
31 typedef typename GRAPH::FactorType FactorType;
│ │ │ │ +
32 typedef boost::shared_ptr<FactorType> sharedFactor;
│ │ │ │ +
33
│ │ │ │ +
35 // TODO(frank): re-factor JunctionTree so we can make members private
│ │ │ │ +
│ │ │ │ +
36 struct Cluster {
│ │ │ │ +
37 typedef FastVector<boost::shared_ptr<Cluster> > Children;
│ │ │ │ +
38 Children children;
│ │ │ │ +
39
│ │ │ │ +
40 typedef Ordering Keys;
│ │ │ │ +
41 Keys orderedFrontalKeys;
│ │ │ │ +
42
│ │ │ │ +
43 FactorGraphType factors;
│ │ │ │ +
44
│ │ │ │ +
45 int problemSize_;
│ │ │ │ +
46
│ │ │ │ +
47 Cluster() : problemSize_(0) {}
│ │ │ │ +
48
│ │ │ │ +
49 virtual ~Cluster() {}
│ │ │ │ +
50
│ │ │ │ +
51 const Cluster& operator[](size_t i) const {
│ │ │ │ +
52 return *(children.at(i));
│ │ │ │ +
53 }
│ │ │ │ +
54
│ │ │ │ +
56 template <class CONTAINER>
│ │ │ │ +
│ │ │ │ +
57 Cluster(Key key, const CONTAINER& factorsToAdd)
│ │ │ │ +
58 : problemSize_(0) {
│ │ │ │ +
59 addFactors(key, factorsToAdd);
│ │ │ │ +
60 }
│ │ │ │ +
│ │ │ │ +
61
│ │ │ │ +
63 template <class CONTAINER>
│ │ │ │ +
│ │ │ │ +
64 void addFactors(Key key, const CONTAINER& factorsToAdd) {
│ │ │ │ +
65 orderedFrontalKeys.push_back(key);
│ │ │ │ +
66 factors.push_back(factorsToAdd);
│ │ │ │ +
67 problemSize_ += factors.size();
│ │ │ │ +
68 }
│ │ │ │ +
│ │ │ │ +
69
│ │ │ │ +
│ │ │ │ +
71 void addChild(const boost::shared_ptr<Cluster>& cluster) {
│ │ │ │ +
72 children.push_back(cluster);
│ │ │ │ +
73 problemSize_ = std::max(problemSize_, cluster->problemSize_);
│ │ │ │ +
74 }
│ │ │ │ +
│ │ │ │ +
75
│ │ │ │ +
76 size_t nrChildren() const {
│ │ │ │ +
77 return children.size();
│ │ │ │ +
78 }
│ │ │ │ +
79
│ │ │ │ +
80 size_t nrFactors() const {
│ │ │ │ +
81 return factors.size();
│ │ │ │ +
82 }
│ │ │ │ +
83
│ │ │ │ +
84 size_t nrFrontals() const {
│ │ │ │ +
85 return orderedFrontalKeys.size();
│ │ │ │ +
86 }
│ │ │ │ +
87
│ │ │ │ +
88 int problemSize() const {
│ │ │ │ +
89 return problemSize_;
│ │ │ │ +
90 }
│ │ │ │ +
91
│ │ │ │ +
93 virtual void print(const std::string& s = "",
│ │ │ │ +
94 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ +
95
│ │ │ │ +
97 std::vector<size_t> nrFrontalsOfChildren() const;
│ │ │ │ +
98
│ │ │ │ +
100 void merge(const boost::shared_ptr<Cluster>& cluster);
│ │ │ │ +
101
│ │ │ │ +
103 void mergeChildren(const std::vector<bool>& merge);
│ │ │ │ +
104 };
│ │ │ │ +
│ │ │ │ +
105
│ │ │ │ +
106 typedef boost::shared_ptr<Cluster> sharedCluster;
│ │ │ │ +
107
│ │ │ │ +
108 // Define Node=Cluster for compatibility with tree traversal functions
│ │ │ │ +
109 typedef Cluster Node;
│ │ │ │ +
110 typedef sharedCluster sharedNode;
│ │ │ │ +
111
│ │ │ │ +
113 GTSAM_CONCEPT_TESTABLE_TYPE(FactorType)
│ │ │ │ +
114
│ │ │ │ +
115 protected:
│ │ │ │ +
116 FastVector<sharedNode> roots_;
│ │ │ │ +
117
│ │ │ │ +
120
│ │ │ │ +
│ │ │ │ +
123 ClusterTree(const This& other) {
│ │ │ │ +
124 *this = other;
│ │ │ │ +
125 }
│ │ │ │ +
│ │ │ │ +
126
│ │ │ │ +
128
│ │ │ │ +
129 public:
│ │ │ │ +
130
│ │ │ │ +
132 ClusterTree() {}
│ │ │ │ +
133
│ │ │ │ +
136
│ │ │ │ +
138 void print(const std::string& s = "",
│ │ │ │ +
139 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ +
140
│ │ │ │ +
144
│ │ │ │ +
145 void addRoot(const boost::shared_ptr<Cluster>& cluster) {
│ │ │ │ +
146 roots_.push_back(cluster);
│ │ │ │ +
147 }
│ │ │ │ +
148
│ │ │ │ +
149 void addChildrenAsRoots(const boost::shared_ptr<Cluster>& cluster) {
│ │ │ │ +
150 for (auto child : cluster->children)
│ │ │ │ +
151 this->addRoot(child);
│ │ │ │ +
152 }
│ │ │ │ +
153
│ │ │ │ +
154 size_t nrRoots() const {
│ │ │ │ +
155 return roots_.size();
│ │ │ │ +
156 }
│ │ │ │ +
157
│ │ │ │ +
│ │ │ │ +
159 const FastVector<sharedNode>& roots() const {
│ │ │ │ +
160 return roots_;
│ │ │ │ +
161 }
│ │ │ │ +
│ │ │ │ +
162
│ │ │ │ +
163 const Cluster& operator[](size_t i) const {
│ │ │ │ +
164 return *(roots_.at(i));
│ │ │ │ +
165 }
│ │ │ │ +
166
│ │ │ │ +
168
│ │ │ │ +
169 protected:
│ │ │ │ +
172
│ │ │ │ +
175 This& operator=(const This& other);
│ │ │ │ +
176
│ │ │ │ +
178};
│ │ │ │ +
│ │ │ │ +
179
│ │ │ │ +
183template <class BAYESTREE, class GRAPH>
│ │ │ │ +
│ │ │ │ +
184class EliminatableClusterTree : public ClusterTree<GRAPH> {
│ │ │ │ +
185 public:
│ │ │ │ +
186 typedef BAYESTREE BayesTreeType;
│ │ │ │ +
187 typedef GRAPH FactorGraphType;
│ │ │ │ +
188 typedef EliminatableClusterTree<BAYESTREE, GRAPH> This;
│ │ │ │ +
189 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
190
│ │ │ │ +
191 typedef typename BAYESTREE::ConditionalType ConditionalType;
│ │ │ │ +
192 typedef boost::shared_ptr<ConditionalType>
│ │ │ │ +
193 sharedConditional;
│ │ │ │ +
194
│ │ │ │ +
195 typedef typename GRAPH::Eliminate Eliminate;
│ │ │ │ +
196 typedef typename GRAPH::FactorType FactorType;
│ │ │ │ +
197 typedef boost::shared_ptr<FactorType> sharedFactor;
│ │ │ │ +
198
│ │ │ │ +
199 protected:
│ │ │ │ +
200 FastVector<sharedFactor> remainingFactors_;
│ │ │ │ +
201
│ │ │ │ +
204
│ │ │ │ +
│ │ │ │ +
207 EliminatableClusterTree(const This& other) : ClusterTree<GRAPH>(other) {
│ │ │ │ +
208 *this = other;
│ │ │ │ +
209 }
│ │ │ │ +
│ │ │ │ +
210
│ │ │ │ +
212
│ │ │ │ +
213 public:
│ │ │ │ +
216
│ │ │ │ +
222 std::pair<boost::shared_ptr<BayesTreeType>, boost::shared_ptr<FactorGraphType> > eliminate(
│ │ │ │ +
223 const Eliminate& function) const;
│ │ │ │ +
224
│ │ │ │ +
226
│ │ │ │ +
229
│ │ │ │ +
│ │ │ │ +
231 const FastVector<sharedFactor>& remainingFactors() const {
│ │ │ │ +
232 return remainingFactors_;
│ │ │ │ +
233 }
│ │ │ │ +
│ │ │ │ +
234
│ │ │ │ +
236
│ │ │ │ +
237 protected:
│ │ │ │ +
240
│ │ │ │ +
243 This& operator=(const This& other);
│ │ │ │ +
244
│ │ │ │ +
246 EliminatableClusterTree() {}
│ │ │ │ +
247
│ │ │ │ +
249};
│ │ │ │ +
│ │ │ │ +
250}
│ │ │ │ +
251
│ │ │ │ +
252#include <gtsam/inference/ClusterTree-inst.h>
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ +
Ordering.h
Variable ordering for the elimination algorithm.
│ │ │ │ +
ClusterTree-inst.h
Collects factorgraph fragments defined on variable clusters, arranged in a tree.
│ │ │ │ +
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::InconsistentEliminationRequested
An inference algorithm was called with inconsistent arguments.
Definition inferenceExceptions.h:29
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::EliminatableClusterTree
A cluster-tree that eliminates to a Bayes tree.
Definition ClusterTree.h:184
│ │ │ │ +
gtsam::EliminatableClusterTree::This
EliminatableClusterTree< BAYESTREE, GRAPH > This
This class.
Definition ClusterTree.h:188
│ │ │ │ +
gtsam::EliminatableClusterTree::EliminatableClusterTree
EliminatableClusterTree()
Default constructor to be used in derived classes.
Definition ClusterTree.h:246
│ │ │ │ +
gtsam::EliminatableClusterTree::ConditionalType
BAYESTREE::ConditionalType ConditionalType
The type of conditionals.
Definition ClusterTree.h:191
│ │ │ │ +
gtsam::EliminatableClusterTree::operator=
This & operator=(const This &other)
Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are copie...
Definition ClusterTree-inst.h:231
│ │ │ │ +
gtsam::EliminatableClusterTree::eliminate
std::pair< boost::shared_ptr< BayesTreeType >, boost::shared_ptr< FactorGraphType > > eliminate(const Eliminate &function) const
Eliminate the factors to a Bayes tree and remaining factor graph.
Definition ClusterTree-inst.h:245
│ │ │ │ +
gtsam::EliminatableClusterTree::sharedConditional
boost::shared_ptr< ConditionalType > sharedConditional
Shared pointer to a conditional.
Definition ClusterTree.h:193
│ │ │ │ +
gtsam::EliminatableClusterTree::sharedFactor
boost::shared_ptr< FactorType > sharedFactor
Shared pointer to a factor.
Definition ClusterTree.h:197
│ │ │ │ +
gtsam::EliminatableClusterTree::remainingFactors
const FastVector< sharedFactor > & remainingFactors() const
Return the remaining factors that are not pulled into elimination.
Definition ClusterTree.h:231
│ │ │ │ +
gtsam::EliminatableClusterTree::EliminatableClusterTree
EliminatableClusterTree(const This &other)
Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are copied,...
Definition ClusterTree.h:207
│ │ │ │ +
gtsam::EliminatableClusterTree::BayesTreeType
BAYESTREE BayesTreeType
The BayesTree type produced by elimination.
Definition ClusterTree.h:186
│ │ │ │ +
gtsam::EliminatableClusterTree::FactorType
GRAPH::FactorType FactorType
The type of factors.
Definition ClusterTree.h:196
│ │ │ │ +
gtsam::EliminatableClusterTree::FactorGraphType
GRAPH FactorGraphType
The factor graph type.
Definition ClusterTree.h:187
│ │ │ │ +
gtsam::EliminatableClusterTree::shared_ptr
boost::shared_ptr< This > shared_ptr
Shared pointer to this class.
Definition ClusterTree.h:189
│ │ │ │ +
gtsam::EliminatableClusterTree::Eliminate
GRAPH::Eliminate Eliminate
Typedef for an eliminate subroutine.
Definition ClusterTree.h:195
│ │ │ │ +
gtsam::ClusterTree
A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman: each node k re...
Definition ClusterTree.h:25
│ │ │ │ +
gtsam::ClusterTree::operator=
This & operator=(const This &other)
Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are copie...
Definition ClusterTree-inst.h:104
│ │ │ │ +
gtsam::ClusterTree::This
ClusterTree< GRAPH > This
This class.
Definition ClusterTree.h:28
│ │ │ │ +
gtsam::ClusterTree::shared_ptr
boost::shared_ptr< This > shared_ptr
Shared pointer to this class.
Definition ClusterTree.h:29
│ │ │ │ +
gtsam::ClusterTree::roots_
FastVector< sharedNode > roots_
concept check
Definition ClusterTree.h:116
│ │ │ │ +
gtsam::ClusterTree::FactorType
GRAPH::FactorType FactorType
The type of factors.
Definition ClusterTree.h:31
│ │ │ │ +
gtsam::ClusterTree::FactorGraphType
GRAPH FactorGraphType
The factor graph type.
Definition ClusterTree.h:27
│ │ │ │ +
gtsam::ClusterTree::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Print the cluster tree.
Definition ClusterTree-inst.h:98
│ │ │ │ +
gtsam::ClusterTree::sharedFactor
boost::shared_ptr< FactorType > sharedFactor
Shared pointer to a factor.
Definition ClusterTree.h:32
│ │ │ │ +
gtsam::ClusterTree::roots
const FastVector< sharedNode > & roots() const
Return the set of roots (one for a tree, multiple for a forest)
Definition ClusterTree.h:159
│ │ │ │ +
gtsam::ClusterTree::ClusterTree
ClusterTree(const This &other)
Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are copied,...
Definition ClusterTree.h:123
│ │ │ │ +
gtsam::ClusterTree::ClusterTree
ClusterTree()
Default constructor.
Definition ClusterTree.h:132
│ │ │ │ +
gtsam::ClusterTree::sharedCluster
boost::shared_ptr< Cluster > sharedCluster
Shared pointer to Cluster.
Definition ClusterTree.h:106
│ │ │ │ +
gtsam::ClusterTree::Cluster
A Cluster is just a collection of factors.
Definition ClusterTree.h:36
│ │ │ │ +
gtsam::ClusterTree::Cluster::Cluster
Cluster(Key key, const CONTAINER &factorsToAdd)
Construct from factors associated with a single key.
Definition ClusterTree.h:57
│ │ │ │ +
gtsam::ClusterTree::Cluster::children
Children children
sub-trees
Definition ClusterTree.h:38
│ │ │ │ +
gtsam::ClusterTree::Cluster::print
virtual void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print this node
Definition ClusterTree-inst.h:26
│ │ │ │ +
gtsam::ClusterTree::Cluster::orderedFrontalKeys
Keys orderedFrontalKeys
Frontal keys of this node.
Definition ClusterTree.h:41
│ │ │ │ +
gtsam::ClusterTree::Cluster::mergeChildren
void mergeChildren(const std::vector< bool > &merge)
Merge all children for which bit is set into this node.
Definition ClusterTree-inst.h:56
│ │ │ │ +
gtsam::ClusterTree::Cluster::merge
void merge(const boost::shared_ptr< Cluster > &cluster)
Merge in given cluster.
Definition ClusterTree-inst.h:44
│ │ │ │ +
gtsam::ClusterTree::Cluster::nrFrontalsOfChildren
std::vector< size_t > nrFrontalsOfChildren() const
Return a vector with nrFrontal keys for each child.
Definition ClusterTree-inst.h:34
│ │ │ │ +
gtsam::ClusterTree::Cluster::factors
FactorGraphType factors
Factors associated with this node.
Definition ClusterTree.h:43
│ │ │ │ +
gtsam::ClusterTree::Cluster::addChild
void addChild(const boost::shared_ptr< Cluster > &cluster)
Add a child cluster.
Definition ClusterTree.h:71
│ │ │ │ +
gtsam::ClusterTree::Cluster::addFactors
void addFactors(Key key, const CONTAINER &factorsToAdd)
Add factors associated with a single key.
Definition ClusterTree.h:64
│ │ │ │ +
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,54 +1,384 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -inferenceExceptions.h │ │ │ │ │ +ClusterTree.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -1/* --------------------------------------------------------------------------- │ │ │ │ │ -- │ │ │ │ │ -2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ -4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -5 * All Rights Reserved │ │ │ │ │ -6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ -7 │ │ │ │ │ -8 * See LICENSE for the license information │ │ │ │ │ -9 │ │ │ │ │ -10 * ------------------------------------------------------------------------- │ │ │ │ │ -- */ │ │ │ │ │ +1 │ │ │ │ │ +10#pragma once │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ -21#include │ │ │ │ │ -22#include │ │ │ │ │ -23 │ │ │ │ │ -24namespace _g_t_s_a_m { │ │ │ │ │ -25 │ │ │ │ │ -_2_9 class _I_n_c_o_n_s_i_s_t_e_n_t_E_l_i_m_i_n_a_t_i_o_n_R_e_q_u_e_s_t_e_d : public std::exception { │ │ │ │ │ -30 public: │ │ │ │ │ -31 _I_n_c_o_n_s_i_s_t_e_n_t_E_l_i_m_i_n_a_t_i_o_n_R_e_q_u_e_s_t_e_d() noexcept {} │ │ │ │ │ -32 _~_I_n_c_o_n_s_i_s_t_e_n_t_E_l_i_m_i_n_a_t_i_o_n_R_e_q_u_e_s_t_e_d() noexcept override {} │ │ │ │ │ -33 const char* what() const noexcept override { │ │ │ │ │ -34 return │ │ │ │ │ -35 "An inference algorithm was called with inconsistent arguments. The\n" │ │ │ │ │ -36 "factor graph, ordering, or variable index were inconsistent with each\n" │ │ │ │ │ -37 "other, or a full elimination routine was called with an ordering that\n" │ │ │ │ │ -38 "does not include all of the variables."; │ │ │ │ │ -39 } │ │ │ │ │ -40 }; │ │ │ │ │ -41 │ │ │ │ │ -42} │ │ │ │ │ -_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ -Included from all GTSAM files. │ │ │ │ │ +12#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +13#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ +14#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_O_r_d_e_r_i_n_g_._h> │ │ │ │ │ +15 │ │ │ │ │ +16namespace _g_t_s_a_m { │ │ │ │ │ +17 │ │ │ │ │ +24template │ │ │ │ │ +_2_5class _C_l_u_s_t_e_r_T_r_e_e { │ │ │ │ │ +26 public: │ │ │ │ │ +_2_7 typedef GRAPH _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e; │ │ │ │ │ +_2_8 typedef _C_l_u_s_t_e_r_T_r_e_e_<_G_R_A_P_H_> _T_h_i_s; │ │ │ │ │ +_2_9 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +30 │ │ │ │ │ +_3_1 typedef typename GRAPH::FactorType _F_a_c_t_o_r_T_y_p_e; │ │ │ │ │ +_3_2 typedef boost::shared_ptr _s_h_a_r_e_d_F_a_c_t_o_r; │ │ │ │ │ +33 │ │ │ │ │ +35 // TODO(frank): re-factor JunctionTree so we can make members private │ │ │ │ │ +_3_6 struct _C_l_u_s_t_e_r { │ │ │ │ │ +37 typedef _F_a_s_t_V_e_c_t_o_r_<_b_o_o_s_t_:_:_s_h_a_r_e_d___p_t_r_<_C_l_u_s_t_e_r_> > Children; │ │ │ │ │ +_3_8 Children _c_h_i_l_d_r_e_n; │ │ │ │ │ +39 │ │ │ │ │ +40 typedef _O_r_d_e_r_i_n_g _K_e_y_s; │ │ │ │ │ +_4_1 _K_e_y_s _o_r_d_e_r_e_d_F_r_o_n_t_a_l_K_e_y_s; │ │ │ │ │ +42 │ │ │ │ │ +_4_3 _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e _f_a_c_t_o_r_s; │ │ │ │ │ +44 │ │ │ │ │ +45 int problemSize_; │ │ │ │ │ +46 │ │ │ │ │ +47 _C_l_u_s_t_e_r() : problemSize_(0) {} │ │ │ │ │ +48 │ │ │ │ │ +49 virtual _~_C_l_u_s_t_e_r() {} │ │ │ │ │ +50 │ │ │ │ │ +51 const Cluster& operator[](size_t i) const { │ │ │ │ │ +52 return *(_c_h_i_l_d_r_e_n.at(i)); │ │ │ │ │ +53 } │ │ │ │ │ +54 │ │ │ │ │ +56 template │ │ │ │ │ +_5_7 _C_l_u_s_t_e_r(_K_e_y key, const CONTAINER& factorsToAdd) │ │ │ │ │ +58 : problemSize_(0) { │ │ │ │ │ +59 _a_d_d_F_a_c_t_o_r_s(key, factorsToAdd); │ │ │ │ │ +60 } │ │ │ │ │ +61 │ │ │ │ │ +63 template │ │ │ │ │ +_6_4 void _a_d_d_F_a_c_t_o_r_s(_K_e_y key, const CONTAINER& factorsToAdd) { │ │ │ │ │ +65 _o_r_d_e_r_e_d_F_r_o_n_t_a_l_K_e_y_s.push_back(key); │ │ │ │ │ +66 _f_a_c_t_o_r_s.push_back(factorsToAdd); │ │ │ │ │ +67 problemSize_ += _f_a_c_t_o_r_s.size(); │ │ │ │ │ +68 } │ │ │ │ │ +69 │ │ │ │ │ +_7_1 void _a_d_d_C_h_i_l_d(const boost::shared_ptr& cluster) { │ │ │ │ │ +72 _c_h_i_l_d_r_e_n.push_back(cluster); │ │ │ │ │ +73 problemSize_ = std::max(problemSize_, cluster->problemSize_); │ │ │ │ │ +74 } │ │ │ │ │ +75 │ │ │ │ │ +76 size_t nrChildren() const { │ │ │ │ │ +77 return _c_h_i_l_d_r_e_n.size(); │ │ │ │ │ +78 } │ │ │ │ │ +79 │ │ │ │ │ +80 size_t nrFactors() const { │ │ │ │ │ +81 return _f_a_c_t_o_r_s.size(); │ │ │ │ │ +82 } │ │ │ │ │ +83 │ │ │ │ │ +84 size_t nrFrontals() const { │ │ │ │ │ +85 return _o_r_d_e_r_e_d_F_r_o_n_t_a_l_K_e_y_s.size(); │ │ │ │ │ +86 } │ │ │ │ │ +87 │ │ │ │ │ +88 int problemSize() const { │ │ │ │ │ +89 return problemSize_; │ │ │ │ │ +90 } │ │ │ │ │ +91 │ │ │ │ │ +93 virtual void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ +94 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const; │ │ │ │ │ +95 │ │ │ │ │ +97 std::vector _n_r_F_r_o_n_t_a_l_s_O_f_C_h_i_l_d_r_e_n() const; │ │ │ │ │ +98 │ │ │ │ │ +100 void _m_e_r_g_e(const boost::shared_ptr& cluster); │ │ │ │ │ +101 │ │ │ │ │ +103 void _m_e_r_g_e_C_h_i_l_d_r_e_n(const std::vector& _m_e_r_g_e); │ │ │ │ │ +104 }; │ │ │ │ │ +105 │ │ │ │ │ +_1_0_6 typedef boost::shared_ptr _s_h_a_r_e_d_C_l_u_s_t_e_r; │ │ │ │ │ +107 │ │ │ │ │ +108 // Define Node=Cluster for compatibility with tree traversal functions │ │ │ │ │ +109 typedef _C_l_u_s_t_e_r _N_o_d_e; │ │ │ │ │ +110 typedef _s_h_a_r_e_d_C_l_u_s_t_e_r sharedNode; │ │ │ │ │ +111 │ │ │ │ │ +113 GTSAM_CONCEPT_TESTABLE_TYPE(_F_a_c_t_o_r_T_y_p_e) │ │ │ │ │ +114 │ │ │ │ │ +115 protected: │ │ │ │ │ +_1_1_6 _F_a_s_t_V_e_c_t_o_r_<_s_h_a_r_e_d_N_o_d_e_> _r_o_o_t_s__; │ │ │ │ │ +117 │ │ │ │ │ +120 │ │ │ │ │ +_1_2_3 _C_l_u_s_t_e_r_T_r_e_e(const _T_h_i_s& other) { │ │ │ │ │ +124 *this = other; │ │ │ │ │ +125 } │ │ │ │ │ +126 │ │ │ │ │ +128 │ │ │ │ │ +129 public: │ │ │ │ │ +130 │ │ │ │ │ +_1_3_2 _C_l_u_s_t_e_r_T_r_e_e() {} │ │ │ │ │ +133 │ │ │ │ │ +136 │ │ │ │ │ +138 void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ +139 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const; │ │ │ │ │ +140 │ │ │ │ │ +144 │ │ │ │ │ +145 void addRoot(const boost::shared_ptr& cluster) { │ │ │ │ │ +146 _r_o_o_t_s__.push_back(cluster); │ │ │ │ │ +147 } │ │ │ │ │ +148 │ │ │ │ │ +149 void addChildrenAsRoots(const boost::shared_ptr& cluster) { │ │ │ │ │ +150 for (auto child : cluster->children) │ │ │ │ │ +151 this->addRoot(child); │ │ │ │ │ +152 } │ │ │ │ │ +153 │ │ │ │ │ +154 size_t nrRoots() const { │ │ │ │ │ +155 return _r_o_o_t_s__.size(); │ │ │ │ │ +156 } │ │ │ │ │ +157 │ │ │ │ │ +_1_5_9 const _F_a_s_t_V_e_c_t_o_r_<_s_h_a_r_e_d_N_o_d_e_>& _r_o_o_t_s() const { │ │ │ │ │ +160 return _r_o_o_t_s__; │ │ │ │ │ +161 } │ │ │ │ │ +162 │ │ │ │ │ +163 const Cluster& operator[](size_t i) const { │ │ │ │ │ +164 return *(_r_o_o_t_s__.at(i)); │ │ │ │ │ +165 } │ │ │ │ │ +166 │ │ │ │ │ +168 │ │ │ │ │ +169 protected: │ │ │ │ │ +172 │ │ │ │ │ +175 _T_h_i_s& _o_p_e_r_a_t_o_r_=(const _T_h_i_s& other); │ │ │ │ │ +176 │ │ │ │ │ +178}; │ │ │ │ │ +179 │ │ │ │ │ +183template │ │ │ │ │ +_1_8_4class _E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e : public _C_l_u_s_t_e_r_T_r_e_e { │ │ │ │ │ +185 public: │ │ │ │ │ +_1_8_6 typedef BAYESTREE _B_a_y_e_s_T_r_e_e_T_y_p_e; │ │ │ │ │ +_1_8_7 typedef GRAPH _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e; │ │ │ │ │ +_1_8_8 typedef _E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_<_B_A_Y_E_S_T_R_E_E_,_ _G_R_A_P_H_> _T_h_i_s; │ │ │ │ │ +_1_8_9 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +190 │ │ │ │ │ +_1_9_1 typedef typename BAYESTREE::ConditionalType _C_o_n_d_i_t_i_o_n_a_l_T_y_p_e; │ │ │ │ │ +192 typedef boost::shared_ptr │ │ │ │ │ +_1_9_3 _s_h_a_r_e_d_C_o_n_d_i_t_i_o_n_a_l; │ │ │ │ │ +194 │ │ │ │ │ +_1_9_5 typedef typename GRAPH::Eliminate _E_l_i_m_i_n_a_t_e; │ │ │ │ │ +_1_9_6 typedef typename GRAPH::FactorType _F_a_c_t_o_r_T_y_p_e; │ │ │ │ │ +_1_9_7 typedef boost::shared_ptr _s_h_a_r_e_d_F_a_c_t_o_r; │ │ │ │ │ +198 │ │ │ │ │ +199 protected: │ │ │ │ │ +200 _F_a_s_t_V_e_c_t_o_r_<_s_h_a_r_e_d_F_a_c_t_o_r_> remainingFactors_; │ │ │ │ │ +201 │ │ │ │ │ +204 │ │ │ │ │ +_2_0_7 _E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e(const _T_h_i_s& other) : _C_l_u_s_t_e_r_T_r_e_e(other) { │ │ │ │ │ +208 *this = other; │ │ │ │ │ +209 } │ │ │ │ │ +210 │ │ │ │ │ +212 │ │ │ │ │ +213 public: │ │ │ │ │ +216 │ │ │ │ │ +222 std::pair, boost:: │ │ │ │ │ +shared_ptr > _e_l_i_m_i_n_a_t_e( │ │ │ │ │ +223 const _E_l_i_m_i_n_a_t_e& function) const; │ │ │ │ │ +224 │ │ │ │ │ +226 │ │ │ │ │ +229 │ │ │ │ │ +_2_3_1 const _F_a_s_t_V_e_c_t_o_r_<_s_h_a_r_e_d_F_a_c_t_o_r_>& _r_e_m_a_i_n_i_n_g_F_a_c_t_o_r_s() const { │ │ │ │ │ +232 return remainingFactors_; │ │ │ │ │ +233 } │ │ │ │ │ +234 │ │ │ │ │ +236 │ │ │ │ │ +237 protected: │ │ │ │ │ +240 │ │ │ │ │ +243 _T_h_i_s& _o_p_e_r_a_t_o_r_=(const _T_h_i_s& other); │ │ │ │ │ +244 │ │ │ │ │ +_2_4_6 _E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e() {} │ │ │ │ │ +247 │ │ │ │ │ +249}; │ │ │ │ │ +250} │ │ │ │ │ +251 │ │ │ │ │ +252#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_C_l_u_s_t_e_r_T_r_e_e_-_i_n_s_t_._h> │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ +_F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ +A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ +_O_r_d_e_r_i_n_g_._h │ │ │ │ │ +Variable ordering for the elimination algorithm. │ │ │ │ │ +_C_l_u_s_t_e_r_T_r_e_e_-_i_n_s_t_._h │ │ │ │ │ +Collects factorgraph fragments defined on variable clusters, arranged in a │ │ │ │ │ +tree. │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ +std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ +FastVector │ │ │ │ │ +FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ +DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_I_n_c_o_n_s_i_s_t_e_n_t_E_l_i_m_i_n_a_t_i_o_n_R_e_q_u_e_s_t_e_d │ │ │ │ │ -An inference algorithm was called with inconsistent arguments. │ │ │ │ │ -DDeeffiinniittiioonn inferenceExceptions.h:29 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e │ │ │ │ │ +A cluster-tree that eliminates to a Bayes tree. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:184 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_T_h_i_s │ │ │ │ │ +EliminatableClusterTree< BAYESTREE, GRAPH > This │ │ │ │ │ +This class. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:188 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e │ │ │ │ │ +EliminatableClusterTree() │ │ │ │ │ +Default constructor to be used in derived classes. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:246 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_C_o_n_d_i_t_i_o_n_a_l_T_y_p_e │ │ │ │ │ +BAYESTREE::ConditionalType ConditionalType │ │ │ │ │ +The type of conditionals. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:191 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ +This & operator=(const This &other) │ │ │ │ │ +Assignment operator - makes a deep copy of the tree structure, but only │ │ │ │ │ +pointers to factors are copie... │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree-inst.h:231 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_e_l_i_m_i_n_a_t_e │ │ │ │ │ +std::pair< boost::shared_ptr< BayesTreeType >, boost::shared_ptr< │ │ │ │ │ +FactorGraphType > > eliminate(const Eliminate &function) const │ │ │ │ │ +Eliminate the factors to a Bayes tree and remaining factor graph. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree-inst.h:245 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_s_h_a_r_e_d_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +boost::shared_ptr< ConditionalType > sharedConditional │ │ │ │ │ +Shared pointer to a conditional. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:193 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_s_h_a_r_e_d_F_a_c_t_o_r │ │ │ │ │ +boost::shared_ptr< FactorType > sharedFactor │ │ │ │ │ +Shared pointer to a factor. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:197 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_r_e_m_a_i_n_i_n_g_F_a_c_t_o_r_s │ │ │ │ │ +const FastVector< sharedFactor > & remainingFactors() const │ │ │ │ │ +Return the remaining factors that are not pulled into elimination. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:231 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e │ │ │ │ │ +EliminatableClusterTree(const This &other) │ │ │ │ │ +Copy constructor - makes a deep copy of the tree structure, but only pointers │ │ │ │ │ +to factors are copied,... │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:207 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_B_a_y_e_s_T_r_e_e_T_y_p_e │ │ │ │ │ +BAYESTREE BayesTreeType │ │ │ │ │ +The BayesTree type produced by elimination. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:186 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_F_a_c_t_o_r_T_y_p_e │ │ │ │ │ +GRAPH::FactorType FactorType │ │ │ │ │ +The type of factors. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:196 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_F_a_c_t_o_r_G_r_a_p_h_T_y_p_e │ │ │ │ │ +GRAPH FactorGraphType │ │ │ │ │ +The factor graph type. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:187 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +Shared pointer to this class. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:189 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_E_l_i_m_i_n_a_t_e │ │ │ │ │ +GRAPH::Eliminate Eliminate │ │ │ │ │ +Typedef for an eliminate subroutine. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:195 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e │ │ │ │ │ +A cluster-tree is associated with a factor graph and is defined as in Koller- │ │ │ │ │ +Friedman: each node k re... │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:25 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ +This & operator=(const This &other) │ │ │ │ │ +Assignment operator - makes a deep copy of the tree structure, but only │ │ │ │ │ +pointers to factors are copie... │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree-inst.h:104 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_T_h_i_s │ │ │ │ │ +ClusterTree< GRAPH > This │ │ │ │ │ +This class. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +Shared pointer to this class. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:29 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_r_o_o_t_s__ │ │ │ │ │ +FastVector< sharedNode > roots_ │ │ │ │ │ +concept check │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:116 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_F_a_c_t_o_r_T_y_p_e │ │ │ │ │ +GRAPH::FactorType FactorType │ │ │ │ │ +The type of factors. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:31 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_F_a_c_t_o_r_G_r_a_p_h_T_y_p_e │ │ │ │ │ +GRAPH FactorGraphType │ │ │ │ │ +The factor graph type. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:27 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ +Print the cluster tree. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree-inst.h:98 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_s_h_a_r_e_d_F_a_c_t_o_r │ │ │ │ │ +boost::shared_ptr< FactorType > sharedFactor │ │ │ │ │ +Shared pointer to a factor. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_r_o_o_t_s │ │ │ │ │ +const FastVector< sharedNode > & roots() const │ │ │ │ │ +Return the set of roots (one for a tree, multiple for a forest) │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:159 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_T_r_e_e │ │ │ │ │ +ClusterTree(const This &other) │ │ │ │ │ +Copy constructor - makes a deep copy of the tree structure, but only pointers │ │ │ │ │ +to factors are copied,... │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:123 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_T_r_e_e │ │ │ │ │ +ClusterTree() │ │ │ │ │ +Default constructor. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:132 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_s_h_a_r_e_d_C_l_u_s_t_e_r │ │ │ │ │ +boost::shared_ptr< Cluster > sharedCluster │ │ │ │ │ +Shared pointer to Cluster. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:106 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r │ │ │ │ │ +A Cluster is just a collection of factors. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_C_l_u_s_t_e_r │ │ │ │ │ +Cluster(Key key, const CONTAINER &factorsToAdd) │ │ │ │ │ +Construct from factors associated with a single key. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:57 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_c_h_i_l_d_r_e_n │ │ │ │ │ +Children children │ │ │ │ │ +sub-trees │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_p_r_i_n_t │ │ │ │ │ +virtual void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ +print this node │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree-inst.h:26 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_o_r_d_e_r_e_d_F_r_o_n_t_a_l_K_e_y_s │ │ │ │ │ +Keys orderedFrontalKeys │ │ │ │ │ +Frontal keys of this node. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_m_e_r_g_e_C_h_i_l_d_r_e_n │ │ │ │ │ +void mergeChildren(const std::vector< bool > &merge) │ │ │ │ │ +Merge all children for which bit is set into this node. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree-inst.h:56 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_m_e_r_g_e │ │ │ │ │ +void merge(const boost::shared_ptr< Cluster > &cluster) │ │ │ │ │ +Merge in given cluster. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree-inst.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_n_r_F_r_o_n_t_a_l_s_O_f_C_h_i_l_d_r_e_n │ │ │ │ │ +std::vector< size_t > nrFrontalsOfChildren() const │ │ │ │ │ +Return a vector with nrFrontal keys for each child. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree-inst.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_f_a_c_t_o_r_s │ │ │ │ │ +FactorGraphType factors │ │ │ │ │ +Factors associated with this node. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_a_d_d_C_h_i_l_d │ │ │ │ │ +void addChild(const boost::shared_ptr< Cluster > &cluster) │ │ │ │ │ +Add a child cluster. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:71 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_a_d_d_F_a_c_t_o_r_s │ │ │ │ │ +void addFactors(Key key, const CONTAINER &factorsToAdd) │ │ │ │ │ +Add factors associated with a single key. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _i_n_f_e_r_e_n_c_e_E_x_c_e_p_t_i_o_n_s_._h │ │ │ │ │ + * _C_l_u_s_t_e_r_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00677.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Symbol.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/EliminateableFactorGraph.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,41 +94,48 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
Symbol.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
EliminateableFactorGraph.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ + │ │ │ │ +

Variable elimination algorithms for factor graphs. │ │ │ │ +More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

struct  gtsam::EliminationTraits< GRAPH >
 Traits class for eliminateable factor graphs, specifies the types that result from elimination, etc. More...
 
class  gtsam::EliminateableFactorGraph< FACTOR_GRAPH >
 EliminateableFactorGraph is a base class for factor graphs that contains elimination algorithms. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -GTSAM_EXPORT std::ostream & gtsam::operator<< (std::ostream &os, const Symbol &symbol)
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
June 9, 2012
│ │ │ │ -
Author
: Frank Dellaert
│ │ │ │ -
│ │ │ │ -: Richard Roberts
│ │ │ │ +

Variable elimination algorithms for factor graphs.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Apr 21, 2013
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,25 +1,32 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Symbol.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +EliminateableFactorGraph.h File Reference │ │ │ │ │ +Variable elimination algorithms for factor graphs. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +struct   _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _G_R_A_P_H_ _> │ │ │ │ │ +  Traits class for eliminateable factor graphs, specifies the types that │ │ │ │ │ + result from elimination, etc. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_ _F_A_C_T_O_R___G_R_A_P_H_ _> │ │ │ │ │ +  _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h is a base class for factor graphs that │ │ │ │ │ + contains elimination algorithms. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -GTSAM_EXPORT std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _S_y_m_b_o_l │ │ │ │ │ - &_s_y_m_b_o_l) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - June 9, 2012 │ │ │ │ │ +Variable elimination algorithms for factor graphs. │ │ │ │ │ Author │ │ │ │ │ - : Frank Dellaert │ │ │ │ │ - : Richard Roberts │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Apr 21, 2013 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _S_y_m_b_o_l_._c_p_p │ │ │ │ │ + * _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00680.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTreeCliqueBase.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,42 +96,51 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
BayesTreeCliqueBase.h File Reference
│ │ │ │ +
BayesTree.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Base class for cliques of a BayesTree. │ │ │ │ +

Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::BayesTreeCliqueBase< DERIVED, FACTORGRAPH >
 This is the base class for BayesTree cliques. More...
struct  gtsam::BayesTreeCliqueStats
 clique statistics More...
 
struct  gtsam::BayesTreeCliqueData
 store all the sizes
│ │ │ │ + More...
 
class  gtsam::BayesTree< CLIQUE >
 Bayes tree. More...
 
class  gtsam::BayesTreeOrphanWrapper< CLIQUE, typename >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Base class for cliques of a BayesTree.

│ │ │ │ -
Author
Richard Roberts and Frank Dellaert
│ │ │ │ +

Bayes Tree is a tree of cliques of a Bayes Chain.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,25 +1,34 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -BayesTreeCliqueBase.h File Reference │ │ │ │ │ -Base class for cliques of a BayesTree. _M_o_r_e_._._. │ │ │ │ │ +BayesTree.h File Reference │ │ │ │ │ +Bayes Tree is a tree of cliques of a Bayes Chain. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_ _D_E_R_I_V_E_D_,_ _F_A_C_T_O_R_G_R_A_P_H_ _> │ │ │ │ │ -  This is the base class for _B_a_y_e_s_T_r_e_e cliques. _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_S_t_a_t_s │ │ │ │ │ +  clique statistics _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_D_a_t_a │ │ │ │ │ +  store all the sizes │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_<_ _C_L_I_Q_U_E_ _> │ │ │ │ │ +  Bayes tree. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r_<_ _C_L_I_Q_U_E_,_ _t_y_p_e_n_a_m_e_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Base class for cliques of a BayesTree. │ │ │ │ │ +Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts and Frank Dellaert │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._h │ │ │ │ │ + * _B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00680.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,5 @@ │ │ │ │ │ var a00680 = [ │ │ │ │ │ - ["gtsam::BayesTreeCliqueBase< DERIVED, FACTORGRAPH >", "a03552.html", "a03552"] │ │ │ │ │ + ["gtsam::BayesTreeCliqueStats", "a03532.html", null], │ │ │ │ │ + ["gtsam::BayesTreeCliqueData", "a03536.html", null], │ │ │ │ │ + ["gtsam::BayesTreeOrphanWrapper< CLIQUE, typename >", "a03544.html", "a03544"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00680_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTreeCliqueBase.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTree.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,226 +98,295 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
BayesTreeCliqueBase.h
│ │ │ │ +
BayesTree.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ +
18// \callgraph
│ │ │ │
19
│ │ │ │ -
20#include <gtsam/inference/Key.h>
│ │ │ │ -
21#include <gtsam/inference/Ordering.h>
│ │ │ │ -
22#include <gtsam/base/types.h>
│ │ │ │ -
23#include <gtsam/base/FastVector.h>
│ │ │ │ -
24#include <boost/optional.hpp>
│ │ │ │ -
25
│ │ │ │ -
26#include <string>
│ │ │ │ -
27#include <mutex>
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <boost/shared_ptr.hpp>
│ │ │ │ +
23
│ │ │ │ +
24#include <gtsam/inference/Key.h>
│ │ │ │ +
25#include <gtsam/base/FastList.h>
│ │ │ │ +
26#include <gtsam/base/ConcurrentMap.h>
│ │ │ │ +
27#include <gtsam/base/FastVector.h>
│ │ │ │
28
│ │ │ │ -
29namespace gtsam {
│ │ │ │ +
29#include <string>
│ │ │ │
30
│ │ │ │ -
31 // Forward declarations
│ │ │ │ -
32 template<class CLIQUE> class BayesTree;
│ │ │ │ -
33 template<class GRAPH> struct EliminationTraits;
│ │ │ │ -
34
│ │ │ │ -
48 template<class DERIVED, class FACTORGRAPH>
│ │ │ │ -
│ │ │ │ -
49 class BayesTreeCliqueBase
│ │ │ │ -
50 {
│ │ │ │ -
51 private:
│ │ │ │ -
52 typedef BayesTreeCliqueBase<DERIVED, FACTORGRAPH> This;
│ │ │ │ -
53 typedef DERIVED DerivedType;
│ │ │ │ -
54 typedef EliminationTraits<FACTORGRAPH> EliminationTraitsType;
│ │ │ │ -
55 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
56 typedef boost::weak_ptr<This> weak_ptr;
│ │ │ │ -
57 typedef boost::shared_ptr<DerivedType> derived_ptr;
│ │ │ │ -
58 typedef boost::weak_ptr<DerivedType> derived_weak_ptr;
│ │ │ │ -
59
│ │ │ │ -
60 public:
│ │ │ │ -
61 typedef FACTORGRAPH FactorGraphType;
│ │ │ │ -
62 typedef typename EliminationTraitsType::BayesNetType BayesNetType;
│ │ │ │ -
63 typedef typename BayesNetType::ConditionalType ConditionalType;
│ │ │ │ -
64 typedef boost::shared_ptr<ConditionalType> sharedConditional;
│ │ │ │ -
65 typedef typename FactorGraphType::FactorType FactorType;
│ │ │ │ -
66 typedef typename FactorGraphType::Eliminate Eliminate;
│ │ │ │ -
67
│ │ │ │ +
31namespace gtsam {
│ │ │ │ +
32
│ │ │ │ +
33 // Forward declarations
│ │ │ │ +
34 template<class FACTOR> class FactorGraph;
│ │ │ │ +
35 template<class BAYESTREE, class GRAPH> class EliminatableClusterTree;
│ │ │ │ +
36
│ │ │ │ +
37 /* ************************************************************************* */
│ │ │ │ +
│ │ │ │ +
39 struct GTSAM_EXPORT BayesTreeCliqueStats {
│ │ │ │ +
40 double avgConditionalSize;
│ │ │ │ +
41 std::size_t maxConditionalSize;
│ │ │ │ +
42 double avgSeparatorSize;
│ │ │ │ +
43 std::size_t maxSeparatorSize;
│ │ │ │ +
44 void print(const std::string& s = "") const ;
│ │ │ │ +
45 };
│ │ │ │ +
│ │ │ │ +
46
│ │ │ │ +
│ │ │ │ +
48 struct GTSAM_EXPORT BayesTreeCliqueData {
│ │ │ │ +
49 FastVector<std::size_t> conditionalSizes;
│ │ │ │ +
50 FastVector<std::size_t> separatorSizes;
│ │ │ │ +
51 BayesTreeCliqueStats getStats() const;
│ │ │ │ +
52 };
│ │ │ │ +
│ │ │ │ +
53
│ │ │ │ +
54 /* ************************************************************************* */
│ │ │ │ +
65 template<class CLIQUE>
│ │ │ │ +
│ │ │ │ +
66 class BayesTree
│ │ │ │ +
67 {
│ │ │ │
68 protected:
│ │ │ │ -
69
│ │ │ │ -
72
│ │ │ │ -
74 BayesTreeCliqueBase() : problemSize_(1) {}
│ │ │ │ -
75
│ │ │ │ -
│ │ │ │ -
78 BayesTreeCliqueBase(const sharedConditional& conditional)
│ │ │ │ -
79 : conditional_(conditional), problemSize_(1) {}
│ │ │ │ -
│ │ │ │ -
80
│ │ │ │ -
│ │ │ │ -
82 BayesTreeCliqueBase(const BayesTreeCliqueBase& c)
│ │ │ │ -
83 : conditional_(c.conditional_),
│ │ │ │ -
84 parent_(c.parent_),
│ │ │ │ -
85 children(c.children),
│ │ │ │ -
86 problemSize_(c.problemSize_),
│ │ │ │ -
87 is_root(c.is_root) {}
│ │ │ │ -
│ │ │ │ -
88
│ │ │ │ -
│ │ │ │ -
90 BayesTreeCliqueBase& operator=(const BayesTreeCliqueBase& c) {
│ │ │ │ -
91 conditional_ = c.conditional_;
│ │ │ │ -
92 parent_ = c.parent_;
│ │ │ │ -
93 children = c.children;
│ │ │ │ -
94 problemSize_ = c.problemSize_;
│ │ │ │ -
95 is_root = c.is_root;
│ │ │ │ -
96 return *this;
│ │ │ │ -
97 }
│ │ │ │ -
│ │ │ │ +
69 typedef BayesTree<CLIQUE> This;
│ │ │ │ +
70 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
71
│ │ │ │ +
72 public:
│ │ │ │ +
73 typedef CLIQUE Clique;
│ │ │ │ +
74 typedef boost::shared_ptr<Clique> sharedClique;
│ │ │ │ +
75 typedef Clique Node;
│ │ │ │ +
76 typedef sharedClique sharedNode;
│ │ │ │ +
77 typedef typename CLIQUE::ConditionalType ConditionalType;
│ │ │ │ +
78 typedef boost::shared_ptr<ConditionalType> sharedConditional;
│ │ │ │ +
79 typedef typename CLIQUE::BayesNetType BayesNetType;
│ │ │ │ +
80 typedef boost::shared_ptr<BayesNetType> sharedBayesNet;
│ │ │ │ +
81 typedef typename CLIQUE::FactorType FactorType;
│ │ │ │ +
82 typedef boost::shared_ptr<FactorType> sharedFactor;
│ │ │ │ +
83 typedef typename CLIQUE::FactorGraphType FactorGraphType;
│ │ │ │ +
84 typedef boost::shared_ptr<FactorGraphType> sharedFactorGraph;
│ │ │ │ +
85 typedef typename FactorGraphType::Eliminate Eliminate;
│ │ │ │ +
86 typedef typename CLIQUE::EliminationTraitsType EliminationTraitsType;
│ │ │ │ +
87
│ │ │ │ +
89 typedef FastList<sharedClique> Cliques;
│ │ │ │ +
90
│ │ │ │ +
92 typedef ConcurrentMap<Key, sharedClique> Nodes;
│ │ │ │ +
93
│ │ │ │ +
95 typedef FastVector<sharedClique> Roots;
│ │ │ │ +
96
│ │ │ │ +
97 protected:
│ │ │ │
98
│ │ │ │ -
99 // Virtual destructor.
│ │ │ │ -
100 virtual ~BayesTreeCliqueBase() {}
│ │ │ │ +
100 Nodes nodes_;
│ │ │ │
101
│ │ │ │ -
103
│ │ │ │ -
105 mutable boost::optional<FactorGraphType> cachedSeparatorMarginal_;
│ │ │ │ -
110 mutable std::mutex cachedSeparatorMarginalMutex_;
│ │ │ │ -
111
│ │ │ │ -
112 public:
│ │ │ │ -
113 sharedConditional conditional_;
│ │ │ │ -
114 derived_weak_ptr parent_;
│ │ │ │ -
115 FastVector<derived_ptr> children;
│ │ │ │ -
116 int problemSize_;
│ │ │ │ -
117
│ │ │ │ -
118 bool is_root = false;
│ │ │ │ -
119
│ │ │ │ -
123 void setEliminationResult(const typename FactorGraphType::EliminationResult& eliminationResult);
│ │ │ │ +
103 Roots roots_;
│ │ │ │ +
104
│ │ │ │ +
107
│ │ │ │ +
109 BayesTree() {}
│ │ │ │ +
110
│ │ │ │ +
112 BayesTree(const This& other);
│ │ │ │ +
113
│ │ │ │ +
115
│ │ │ │ +
117 This& operator=(const This& other);
│ │ │ │ +
118
│ │ │ │ +
121
│ │ │ │ +
123 bool equals(const This& other, double tol = 1e-9) const;
│ │ │ │
124
│ │ │ │ -
127
│ │ │ │ -
129 bool equals(const DERIVED& other, double tol = 1e-9) const;
│ │ │ │ +
125 public:
│ │ │ │ +
127 void print(const std::string& s = "",
│ │ │ │ +
128 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │
130
│ │ │ │ -
132 virtual void print(
│ │ │ │ -
133 const std::string& s = "",
│ │ │ │ -
134 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ -
135
│ │ │ │ -
139
│ │ │ │ -
141 const sharedConditional& conditional() const { return conditional_; }
│ │ │ │ -
142
│ │ │ │ -
144 inline bool isRoot() const { return parent_.expired(); }
│ │ │ │ -
145
│ │ │ │ -
147 size_t nrChildren() const { return children.size(); }
│ │ │ │ -
148
│ │ │ │ -
150 const derived_ptr operator[](size_t i) const { return children[i]; }
│ │ │ │ -
151
│ │ │ │ -
153 size_t treeSize() const;
│ │ │ │ -
154
│ │ │ │ -
156 size_t numCachedSeparatorMarginals() const;
│ │ │ │ -
157
│ │ │ │ -
159 derived_ptr parent() const { return parent_.lock(); }
│ │ │ │ -
160
│ │ │ │ -
162 int problemSize() const { return problemSize_; }
│ │ │ │ -
163
│ │ │ │ -
167
│ │ │ │ -
169 BayesNetType shortcut(const derived_ptr& root, Eliminate function = EliminationTraitsType::DefaultEliminate) const;
│ │ │ │ -
170
│ │ │ │ -
172 FactorGraphType separatorMarginal(Eliminate function = EliminationTraitsType::DefaultEliminate) const;
│ │ │ │ -
173
│ │ │ │ -
175 FactorGraphType marginal2(Eliminate function = EliminationTraitsType::DefaultEliminate) const;
│ │ │ │ -
176
│ │ │ │ -
181 void deleteCachedShortcuts();
│ │ │ │ -
182
│ │ │ │ -
183 const boost::optional<FactorGraphType>& cachedSeparatorMarginal() const {
│ │ │ │ -
184 std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
│ │ │ │ -
185 return cachedSeparatorMarginal_;
│ │ │ │ -
186 }
│ │ │ │ +
133
│ │ │ │ +
135 size_t size() const;
│ │ │ │ +
136
│ │ │ │ +
│ │ │ │ +
138 inline bool empty() const {
│ │ │ │ +
139 return nodes_.empty();
│ │ │ │ +
140 }
│ │ │ │ +
│ │ │ │ +
141
│ │ │ │ +
143 const Nodes& nodes() const { return nodes_; }
│ │ │ │ +
144
│ │ │ │ +
146 sharedClique operator[](Key j) const { return nodes_.at(j); }
│ │ │ │ +
147
│ │ │ │ +
149 const Roots& roots() const { return roots_; }
│ │ │ │ +
150
│ │ │ │ +
│ │ │ │ +
152 const sharedClique& clique(Key j) const {
│ │ │ │ +
153 typename Nodes::const_iterator c = nodes_.find(j);
│ │ │ │ +
154 if(c == nodes_.end())
│ │ │ │ +
155 throw std::out_of_range("Requested the BayesTree clique for a key that is not in the BayesTree");
│ │ │ │ +
156 else
│ │ │ │ +
157 return c->second;
│ │ │ │ +
158 }
│ │ │ │ +
│ │ │ │ +
159
│ │ │ │ +
161 BayesTreeCliqueData getCliqueData() const;
│ │ │ │ +
162
│ │ │ │ +
164 size_t numCachedSeparatorMarginals() const;
│ │ │ │ +
165
│ │ │ │ +
171 sharedConditional marginalFactor(Key j, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
│ │ │ │ +
172
│ │ │ │ +
177 sharedFactorGraph joint(Key j1, Key j2, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
│ │ │ │ +
178
│ │ │ │ +
183 sharedBayesNet jointBayesNet(Key j1, Key j2, const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
│ │ │ │ +
184
│ │ │ │
187
│ │ │ │ -
188 friend class BayesTree<DerivedType>;
│ │ │ │ -
189
│ │ │ │ -
190 protected:
│ │ │ │ -
191
│ │ │ │ -
193 KeyVector separator_setminus_B(const derived_ptr& B) const;
│ │ │ │ +
189 void dot(std::ostream& os, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ +
190
│ │ │ │ +
192 std::string dot(
│ │ │ │ +
193 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │
194
│ │ │ │ -
198 KeyVector shortcut_indices(const derived_ptr& B, const FactorGraphType& p_Cp_B) const;
│ │ │ │ -
199
│ │ │ │ -
│ │ │ │ -
201 void deleteCachedShortcutsNonRecursive() {
│ │ │ │ -
202 std::lock_guard<std::mutex> marginalLock(cachedSeparatorMarginalMutex_);
│ │ │ │ -
203 cachedSeparatorMarginal_ = boost::none;
│ │ │ │ -
204 }
│ │ │ │ -
│ │ │ │ -
205
│ │ │ │ -
206 private:
│ │ │ │ -
207
│ │ │ │ -
209 friend class boost::serialization::access;
│ │ │ │ -
210 template<class ARCHIVE>
│ │ │ │ -
211 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
212 if(!parent_.lock()) {
│ │ │ │ -
213 is_root = true;
│ │ │ │ -
214 }
│ │ │ │ -
215 ar & BOOST_SERIALIZATION_NVP(is_root);
│ │ │ │ -
216 ar & BOOST_SERIALIZATION_NVP(conditional_);
│ │ │ │ -
217 if (!is_root) { // TODO(fan): Workaround for boost/serialization #119
│ │ │ │ -
218 ar & BOOST_SERIALIZATION_NVP(parent_);
│ │ │ │ -
219 }
│ │ │ │ -
220 ar & BOOST_SERIALIZATION_NVP(children);
│ │ │ │ -
221 }
│ │ │ │ -
222
│ │ │ │ -
224
│ │ │ │ -
225 };
│ │ │ │ -
│ │ │ │ -
226
│ │ │ │ -
227}
│ │ │ │ -
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ -
types.h
Typedefs for easier changing of types.
│ │ │ │ - │ │ │ │ -
Ordering.h
Variable ordering for the elimination algorithm.
│ │ │ │ +
196 void saveGraph(const std::string& filename,
│ │ │ │ +
197 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ +
198
│ │ │ │ +
202
│ │ │ │ +
207 template<class CONTAINER>
│ │ │ │ +
208 Key findParentClique(const CONTAINER& parents) const;
│ │ │ │ +
209
│ │ │ │ +
211 void clear();
│ │ │ │ +
212
│ │ │ │ +
214 void deleteCachedShortcuts();
│ │ │ │ +
215
│ │ │ │ +
220 void removePath(sharedClique clique, BayesNetType* bn, Cliques* orphans);
│ │ │ │ +
221
│ │ │ │ +
226 void removeTop(const KeyVector& keys, BayesNetType* bn, Cliques* orphans);
│ │ │ │ +
227
│ │ │ │ +
230 Cliques removeSubtree(const sharedClique& subtree);
│ │ │ │ +
231
│ │ │ │ +
235 void insertRoot(const sharedClique& subtree);
│ │ │ │ +
236
│ │ │ │ +
238 void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique());
│ │ │ │ +
239
│ │ │ │ +
241 void addFactorsToGraph(FactorGraph<FactorType>* graph) const;
│ │ │ │ +
242
│ │ │ │ +
243 protected:
│ │ │ │ +
244
│ │ │ │ +
246 void dot(std::ostream &s, sharedClique clique, const KeyFormatter& keyFormatter,
│ │ │ │ +
247 int parentnum = 0) const;
│ │ │ │ +
248
│ │ │ │ +
250 void getCliqueData(sharedClique clique, BayesTreeCliqueData* stats) const;
│ │ │ │ +
251
│ │ │ │ +
253 void removeClique(sharedClique clique);
│ │ │ │ +
254
│ │ │ │ +
256 void fillNodesIndex(const sharedClique& subtree);
│ │ │ │ +
257
│ │ │ │ +
258 // Friend JunctionTree because it directly fills roots and nodes index.
│ │ │ │ +
259 template<class BAYESTREE, class GRAPH> friend class EliminatableClusterTree;
│ │ │ │ +
260
│ │ │ │ +
261 private:
│ │ │ │ +
263 friend class boost::serialization::access;
│ │ │ │ +
264 template<class ARCHIVE>
│ │ │ │ +
265 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
266 ar & BOOST_SERIALIZATION_NVP(nodes_);
│ │ │ │ +
267 ar & BOOST_SERIALIZATION_NVP(roots_);
│ │ │ │ +
268 }
│ │ │ │ +
269
│ │ │ │ +
271
│ │ │ │ +
272 }; // BayesTree
│ │ │ │ +
│ │ │ │ +
273
│ │ │ │ +
274 /* ************************************************************************* */
│ │ │ │ +
275 template <class CLIQUE, typename = void>
│ │ │ │ +
│ │ │ │ +
276 class BayesTreeOrphanWrapper : public CLIQUE::ConditionalType {
│ │ │ │ +
277 public:
│ │ │ │ +
278 typedef CLIQUE CliqueType;
│ │ │ │ +
279 typedef typename CLIQUE::ConditionalType Base;
│ │ │ │ +
280
│ │ │ │ +
281 boost::shared_ptr<CliqueType> clique;
│ │ │ │ +
282
│ │ │ │ +
│ │ │ │ +
293 BayesTreeOrphanWrapper(const boost::shared_ptr<CliqueType>& clique)
│ │ │ │ +
294 : clique(clique) {
│ │ │ │ +
295 this->keys_.assign(clique->conditional()->beginParents(),
│ │ │ │ +
296 clique->conditional()->endParents());
│ │ │ │ +
297 }
│ │ │ │ +
│ │ │ │ +
298
│ │ │ │ +
299 void print(
│ │ │ │ +
300 const std::string& s = "",
│ │ │ │ +
301 const KeyFormatter& formatter = DefaultKeyFormatter) const override {
│ │ │ │ +
302 clique->print(s + "stored clique", formatter);
│ │ │ │ +
303 }
│ │ │ │ +
304 };
│ │ │ │ +
│ │ │ │ +
305
│ │ │ │ +
306}
│ │ │ │ +
FastList.h
A thin wrapper around std::list that uses boost's fast_pool_allocator.
│ │ │ │ +
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ + │ │ │ │
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::ConcurrentMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition ConcurrentMap.h:68
│ │ │ │ +
gtsam::FastList
FastList is a thin wrapper around std::list that uses the boost fast_pool_allocator instead of the de...
Definition FastList.h:40
│ │ │ │
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::EliminationTraits
Traits class for eliminateable factor graphs, specifies the types that result from elimination,...
Definition EliminateableFactorGraph.h:36
│ │ │ │ -
gtsam::BayesTreeCliqueBase
This is the base class for BayesTree cliques.
Definition BayesTreeCliqueBase.h:50
│ │ │ │ -
gtsam::BayesTreeCliqueBase::operator[]
const derived_ptr operator[](size_t i) const
Return the child at index i.
Definition BayesTreeCliqueBase.h:150
│ │ │ │ -
gtsam::BayesTreeCliqueBase::operator=
BayesTreeCliqueBase & operator=(const BayesTreeCliqueBase &c)
Shallow copy assignment constructor.
Definition BayesTreeCliqueBase.h:90
│ │ │ │ -
gtsam::BayesTreeCliqueBase::deleteCachedShortcutsNonRecursive
void deleteCachedShortcutsNonRecursive()
Non-recursive delete cached shortcuts and marginals - internal only.
Definition BayesTreeCliqueBase.h:201
│ │ │ │ -
gtsam::BayesTreeCliqueBase::nrChildren
size_t nrChildren() const
Return the number of children.
Definition BayesTreeCliqueBase.h:147
│ │ │ │ -
gtsam::BayesTreeCliqueBase::problemSize
int problemSize() const
Problem size (used for parallel traversal)
Definition BayesTreeCliqueBase.h:162
│ │ │ │ -
gtsam::BayesTreeCliqueBase::BayesTreeCliqueBase
BayesTreeCliqueBase()
Default constructor.
Definition BayesTreeCliqueBase.h:74
│ │ │ │ -
gtsam::BayesTreeCliqueBase::parent
derived_ptr parent() const
return a shared_ptr to the parent clique
Definition BayesTreeCliqueBase.h:159
│ │ │ │ -
gtsam::BayesTreeCliqueBase::treeSize
size_t treeSize() const
The size of subtree rooted at this clique, i.e., nr of Cliques.
Definition BayesTreeCliqueBase-inst.h:84
│ │ │ │ -
gtsam::BayesTreeCliqueBase::BayesTreeCliqueBase
BayesTreeCliqueBase(const sharedConditional &conditional)
Construct from a conditional, leaving parent and child pointers uninitialized.
Definition BayesTreeCliqueBase.h:78
│ │ │ │ -
gtsam::BayesTreeCliqueBase::cachedSeparatorMarginalMutex_
std::mutex cachedSeparatorMarginalMutex_
This protects Cached seperator marginal P(S) from concurrent read/writes as many the functions which ...
Definition BayesTreeCliqueBase.h:110
│ │ │ │ -
gtsam::BayesTreeCliqueBase::isRoot
bool isRoot() const
Return true if this clique is the root of a Bayes tree.
Definition BayesTreeCliqueBase.h:144
│ │ │ │ -
gtsam::BayesTreeCliqueBase::BayesTreeCliqueBase
BayesTreeCliqueBase(const BayesTreeCliqueBase &c)
Shallow copy constructor.
Definition BayesTreeCliqueBase.h:82
│ │ │ │ -
gtsam::BayesTreeCliqueBase::marginal2
FactorGraphType marginal2(Eliminate function=EliminationTraitsType::DefaultEliminate) const
return the marginal P(C) of the clique, using marginal caching
Definition BayesTreeCliqueBase-inst.h:195
│ │ │ │ -
gtsam::BayesTreeCliqueBase::separatorMarginal
FactorGraphType separatorMarginal(Eliminate function=EliminationTraitsType::DefaultEliminate) const
return the marginal P(S) on the separator
Definition BayesTreeCliqueBase-inst.h:147
│ │ │ │ -
gtsam::BayesTreeCliqueBase::shortcut
BayesNetType shortcut(const derived_ptr &root, Eliminate function=EliminationTraitsType::DefaultEliminate) const
return the conditional P(S|Root) on the separator given the root
Definition BayesTreeCliqueBase-inst.h:113
│ │ │ │ -
gtsam::BayesTreeCliqueBase::deleteCachedShortcuts
void deleteCachedShortcuts()
This deletes the cached shortcuts of all cliques (subtree) below this clique.
Definition BayesTreeCliqueBase-inst.h:207
│ │ │ │ -
gtsam::BayesTreeCliqueBase::shortcut_indices
KeyVector shortcut_indices(const derived_ptr &B, const FactorGraphType &p_Cp_B) const
Determine variable indices to keep in recursive separator shortcut calculation The factor graph p_Cp_...
Definition BayesTreeCliqueBase-inst.h:57
│ │ │ │ -
gtsam::BayesTreeCliqueBase::conditional
const sharedConditional & conditional() const
Access the conditional.
Definition BayesTreeCliqueBase.h:141
│ │ │ │ -
gtsam::BayesTreeCliqueBase::access
friend class boost::serialization::access
Serialization function.
Definition BayesTreeCliqueBase.h:209
│ │ │ │ -
gtsam::BayesTreeCliqueBase::separator_setminus_B
KeyVector separator_setminus_B(const derived_ptr &B) const
Calculate set for shortcut calculations.
Definition BayesTreeCliqueBase-inst.h:45
│ │ │ │ -
gtsam::BayesTreeCliqueBase::print
virtual void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print this node
Definition BayesTreeCliqueBase-inst.h:76
│ │ │ │ -
gtsam::BayesTreeCliqueBase::numCachedSeparatorMarginals
size_t numCachedSeparatorMarginals() const
Collect number of cliques with cached separator marginals.
Definition BayesTreeCliqueBase-inst.h:93
│ │ │ │ -
gtsam::BayesTreeCliqueBase::cachedSeparatorMarginal_
boost::optional< FactorGraphType > cachedSeparatorMarginal_
This stores the Cached separator marginal P(S)
Definition BayesTreeCliqueBase.h:105
│ │ │ │ -
gtsam::BayesTreeCliqueBase::setEliminationResult
void setEliminationResult(const typename FactorGraphType::EliminationResult &eliminationResult)
Fill the elimination result produced during elimination.
Definition BayesTreeCliqueBase-inst.h:27
│ │ │ │ +
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ +
gtsam::EliminatableClusterTree
A cluster-tree that eliminates to a Bayes tree.
Definition ClusterTree.h:184
│ │ │ │ +
gtsam::BayesTreeCliqueStats
clique statistics
Definition BayesTree.h:39
│ │ │ │ +
gtsam::BayesTreeCliqueData
store all the sizes
Definition BayesTree.h:48
│ │ │ │ +
gtsam::BayesTree
Bayes tree.
Definition BayesTree.h:67
│ │ │ │ +
gtsam::BayesTree::nodes_
Nodes nodes_
Map from indices to Clique.
Definition BayesTree.h:100
│ │ │ │ +
gtsam::BayesTree::removeClique
void removeClique(sharedClique clique)
remove a clique: warning, can result in a forest
Definition BayesTree-inst.h:424
│ │ │ │ +
gtsam::BayesTree::joint
sharedFactorGraph joint(Key j1, Key j2, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
return joint on two variables Limitation: can only calculate joint if cliques are disjoint or one of ...
Definition BayesTree-inst.h:293
│ │ │ │ +
gtsam::BayesTree::fillNodesIndex
void fillNodesIndex(const sharedClique &subtree)
Fill the nodes index for a subtree.
Definition BayesTree-inst.h:246
│ │ │ │ +
gtsam::BayesTree::dot
void dot(std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Output to graphviz format, stream version.
Definition BayesTree-inst.h:64
│ │ │ │ +
gtsam::BayesTree::addFactorsToGraph
void addFactorsToGraph(FactorGraph< FactorType > *graph) const
Add all cliques in this BayesTree to the specified factor graph.
Definition BayesTree-inst.h:168
│ │ │ │ +
gtsam::BayesTree::operator[]
sharedClique operator[](Key j) const
Access node by variable.
Definition BayesTree.h:146
│ │ │ │ +
gtsam::BayesTree::operator=
This & operator=(const This &other)
Assignment operator.
Definition BayesTree-inst.h:199
│ │ │ │ +
gtsam::BayesTree::sharedClique
boost::shared_ptr< Clique > sharedClique
Shared pointer to a clique.
Definition BayesTree.h:74
│ │ │ │ +
gtsam::BayesTree::BayesTree
BayesTree()
Create an empty Bayes Tree.
Definition BayesTree.h:109
│ │ │ │ +
gtsam::BayesTree::Node
Clique Node
Synonym for Clique (TODO: remove)
Definition BayesTree.h:75
│ │ │ │ +
gtsam::BayesTree::clear
void clear()
Remove all nodes.
Definition BayesTree-inst.h:408
│ │ │ │ +
gtsam::BayesTree::roots_
Roots roots_
Root cliques.
Definition BayesTree.h:103
│ │ │ │ +
gtsam::BayesTree::addClique
void addClique(const sharedClique &clique, const sharedClique &parent_clique=sharedClique())
add a clique (top down)
Definition BayesTree-inst.h:142
│ │ │ │ +
gtsam::BayesTree::jointBayesNet
sharedBayesNet jointBayesNet(Key j1, Key j2, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
return joint on two variables as a BayesNet Limitation: can only calculate joint if cliques are disjo...
Definition BayesTree-inst.h:302
│ │ │ │ +
gtsam::BayesTree::sharedNode
sharedClique sharedNode
Synonym for sharedClique (TODO: remove)
Definition BayesTree.h:76
│ │ │ │ +
gtsam::BayesTree::findParentClique
Key findParentClique(const CONTAINER &parents) const
Find parent clique of a conditional.
Definition BayesTree-inst.h:238
│ │ │ │ +
gtsam::BayesTree::size
size_t size() const
number of cliques
Definition BayesTree-inst.h:133
│ │ │ │ +
gtsam::BayesTree::deleteCachedShortcuts
void deleteCachedShortcuts()
Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data.
Definition BayesTree-inst.h:416
│ │ │ │ +
gtsam::BayesTree::removePath
void removePath(sharedClique clique, BayesNetType *bn, Cliques *orphans)
Remove path from clique to root and return that path as factors plus a list of orphaned subtree roots...
Definition BayesTree-inst.h:448
│ │ │ │ +
gtsam::BayesTree::nodes
const Nodes & nodes() const
Return nodes.
Definition BayesTree.h:143
│ │ │ │ +
gtsam::BayesTree::Cliques
FastList< sharedClique > Cliques
A convenience class for a list of shared cliques.
Definition BayesTree.h:89
│ │ │ │ +
gtsam::BayesTree::Clique
CLIQUE Clique
The clique type, normally BayesTreeClique.
Definition BayesTree.h:73
│ │ │ │ +
gtsam::BayesTree::marginalFactor
sharedConditional marginalFactor(Key j, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
Return marginal on any variable.
Definition BayesTree-inst.h:270
│ │ │ │ +
gtsam::BayesTree::roots
const Roots & roots() const
return root cliques
Definition BayesTree.h:149
│ │ │ │ +
gtsam::BayesTree::clique
const sharedClique & clique(Key j) const
alternate syntax for matlab: find the clique that contains the variable with Key j
Definition BayesTree.h:152
│ │ │ │ +
gtsam::BayesTree::Nodes
ConcurrentMap< Key, sharedClique > Nodes
Map from keys to Clique.
Definition BayesTree.h:92
│ │ │ │ +
gtsam::BayesTree::access
friend class boost::serialization::access
Serialization function.
Definition BayesTree.h:263
│ │ │ │ +
gtsam::BayesTree::numCachedSeparatorMarginals
size_t numCachedSeparatorMarginals() const
Collect number of cliques with cached separator marginals.
Definition BayesTree-inst.h:55
│ │ │ │ +
gtsam::BayesTree::getCliqueData
BayesTreeCliqueData getCliqueData() const
Gather data on all cliques.
Definition BayesTree-inst.h:35
│ │ │ │ +
gtsam::BayesTree::removeSubtree
Cliques removeSubtree(const sharedClique &subtree)
Remove the requested subtree.
Definition BayesTree-inst.h:496
│ │ │ │ +
gtsam::BayesTree::empty
bool empty() const
Check if there are any cliques in the tree.
Definition BayesTree.h:138
│ │ │ │ +
gtsam::BayesTree::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print
Definition BayesTree-inst.h:212
│ │ │ │ +
gtsam::BayesTree::insertRoot
void insertRoot(const sharedClique &subtree)
Insert a new subtree with known parent clique.
Definition BayesTree-inst.h:260
│ │ │ │ +
gtsam::BayesTree::saveGraph
void saveGraph(const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
output to file with graphviz format.
Definition BayesTree-inst.h:85
│ │ │ │ +
gtsam::BayesTree::removeTop
void removeTop(const KeyVector &keys, BayesNetType *bn, Cliques *orphans)
Given a list of indices, turn "contaminated" part of the tree back into a factor graph.
Definition BayesTree-inst.h:475
│ │ │ │ +
gtsam::BayesTree::Roots
FastVector< sharedClique > Roots
Root cliques.
Definition BayesTree.h:95
│ │ │ │ +
gtsam::BayesTreeOrphanWrapper
Definition BayesTree.h:276
│ │ │ │ +
gtsam::BayesTreeOrphanWrapper::BayesTreeOrphanWrapper
BayesTreeOrphanWrapper(const boost::shared_ptr< CliqueType > &clique)
Construct a new Bayes Tree Orphan Wrapper object.
Definition BayesTree.h:293
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,315 +1,451 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -BayesTreeCliqueBase.h │ │ │ │ │ +BayesTree.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ +18// \callgraph │ │ │ │ │ 19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_O_r_d_e_r_i_n_g_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ -24#include │ │ │ │ │ -25 │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include │ │ │ │ │ +23 │ │ │ │ │ +24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_L_i_s_t_._h> │ │ │ │ │ +26#include │ │ │ │ │ +27#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ 28 │ │ │ │ │ -29namespace _g_t_s_a_m { │ │ │ │ │ +29#include │ │ │ │ │ 30 │ │ │ │ │ -31 // Forward declarations │ │ │ │ │ -32 template class BayesTree; │ │ │ │ │ -33 template struct EliminationTraits; │ │ │ │ │ -34 │ │ │ │ │ -48 template │ │ │ │ │ -_4_9 class _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e │ │ │ │ │ -50 { │ │ │ │ │ -51 private: │ │ │ │ │ -52 typedef _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_D_E_R_I_V_E_D_,_ _F_A_C_T_O_R_G_R_A_P_H_> This; │ │ │ │ │ -53 typedef DERIVED DerivedType; │ │ │ │ │ -54 typedef _E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_F_A_C_T_O_R_G_R_A_P_H_> EliminationTraitsType; │ │ │ │ │ -55 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -56 typedef boost::weak_ptr weak_ptr; │ │ │ │ │ -57 typedef boost::shared_ptr derived_ptr; │ │ │ │ │ -58 typedef boost::weak_ptr derived_weak_ptr; │ │ │ │ │ -59 │ │ │ │ │ -60 public: │ │ │ │ │ -61 typedef FACTORGRAPH FactorGraphType; │ │ │ │ │ -62 typedef typename EliminationTraitsType::BayesNetType BayesNetType; │ │ │ │ │ -63 typedef typename BayesNetType::ConditionalType ConditionalType; │ │ │ │ │ -64 typedef boost::shared_ptr sharedConditional; │ │ │ │ │ -65 typedef typename FactorGraphType::FactorType FactorType; │ │ │ │ │ -66 typedef typename FactorGraphType::Eliminate Eliminate; │ │ │ │ │ -67 │ │ │ │ │ +31namespace _g_t_s_a_m { │ │ │ │ │ +32 │ │ │ │ │ +33 // Forward declarations │ │ │ │ │ +34 template class FactorGraph; │ │ │ │ │ +35 template class EliminatableClusterTree; │ │ │ │ │ +36 │ │ │ │ │ +37 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +_3_9 struct GTSAM_EXPORT _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_S_t_a_t_s { │ │ │ │ │ +40 double avgConditionalSize; │ │ │ │ │ +41 std::size_t maxConditionalSize; │ │ │ │ │ +42 double avgSeparatorSize; │ │ │ │ │ +43 std::size_t maxSeparatorSize; │ │ │ │ │ +44 void _p_r_i_n_t(const std::string& s = "") const ; │ │ │ │ │ +45 }; │ │ │ │ │ +46 │ │ │ │ │ +_4_8 struct GTSAM_EXPORT _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_D_a_t_a { │ │ │ │ │ +49 _F_a_s_t_V_e_c_t_o_r_<_s_t_d_:_:_s_i_z_e___t_> conditionalSizes; │ │ │ │ │ +50 _F_a_s_t_V_e_c_t_o_r_<_s_t_d_:_:_s_i_z_e___t_> separatorSizes; │ │ │ │ │ +51 _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_S_t_a_t_s getStats() const; │ │ │ │ │ +52 }; │ │ │ │ │ +53 │ │ │ │ │ +54 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +65 template │ │ │ │ │ +_6_6 class _B_a_y_e_s_T_r_e_e │ │ │ │ │ +67 { │ │ │ │ │ 68 protected: │ │ │ │ │ -69 │ │ │ │ │ -72 │ │ │ │ │ -_7_4 _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e() : problemSize_(1) {} │ │ │ │ │ -75 │ │ │ │ │ -_7_8 _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e(const sharedConditional& _c_o_n_d_i_t_i_o_n_a_l) │ │ │ │ │ -79 : conditional_(_c_o_n_d_i_t_i_o_n_a_l), problemSize_(1) {} │ │ │ │ │ -80 │ │ │ │ │ -_8_2 _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e(const _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e& c) │ │ │ │ │ -83 : conditional_(c.conditional_), │ │ │ │ │ -84 parent_(c.parent_), │ │ │ │ │ -85 children(c.children), │ │ │ │ │ -86 problemSize_(c.problemSize_), │ │ │ │ │ -87 is_root(c.is_root) {} │ │ │ │ │ -88 │ │ │ │ │ -_9_0 _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e& _o_p_e_r_a_t_o_r_=(const _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e& c) { │ │ │ │ │ -91 conditional_ = c.conditional_; │ │ │ │ │ -92 parent_ = c.parent_; │ │ │ │ │ -93 children = c.children; │ │ │ │ │ -94 problemSize_ = c.problemSize_; │ │ │ │ │ -95 is_root = c.is_root; │ │ │ │ │ -96 return *this; │ │ │ │ │ -97 } │ │ │ │ │ +69 typedef _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_> _T_h_i_s; │ │ │ │ │ +70 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +71 │ │ │ │ │ +72 public: │ │ │ │ │ +_7_3 typedef CLIQUE _C_l_i_q_u_e; │ │ │ │ │ +_7_4 typedef boost::shared_ptr _s_h_a_r_e_d_C_l_i_q_u_e; │ │ │ │ │ +_7_5 typedef _C_l_i_q_u_e _N_o_d_e; │ │ │ │ │ +_7_6 typedef _s_h_a_r_e_d_C_l_i_q_u_e _s_h_a_r_e_d_N_o_d_e; │ │ │ │ │ +77 typedef typename CLIQUE::ConditionalType ConditionalType; │ │ │ │ │ +78 typedef boost::shared_ptr sharedConditional; │ │ │ │ │ +79 typedef typename CLIQUE::BayesNetType BayesNetType; │ │ │ │ │ +80 typedef boost::shared_ptr sharedBayesNet; │ │ │ │ │ +81 typedef typename CLIQUE::FactorType FactorType; │ │ │ │ │ +82 typedef boost::shared_ptr sharedFactor; │ │ │ │ │ +83 typedef typename CLIQUE::FactorGraphType FactorGraphType; │ │ │ │ │ +84 typedef boost::shared_ptr sharedFactorGraph; │ │ │ │ │ +85 typedef typename FactorGraphType::Eliminate Eliminate; │ │ │ │ │ +86 typedef typename CLIQUE::EliminationTraitsType EliminationTraitsType; │ │ │ │ │ +87 │ │ │ │ │ +_8_9 typedef _F_a_s_t_L_i_s_t_<_s_h_a_r_e_d_C_l_i_q_u_e_> _C_l_i_q_u_e_s; │ │ │ │ │ +90 │ │ │ │ │ +_9_2 typedef _C_o_n_c_u_r_r_e_n_t_M_a_p_<_K_e_y_,_ _s_h_a_r_e_d_C_l_i_q_u_e_> _N_o_d_e_s; │ │ │ │ │ +93 │ │ │ │ │ +_9_5 typedef _F_a_s_t_V_e_c_t_o_r_<_s_h_a_r_e_d_C_l_i_q_u_e_> _R_o_o_t_s; │ │ │ │ │ +96 │ │ │ │ │ +97 protected: │ │ │ │ │ 98 │ │ │ │ │ -99 // Virtual destructor. │ │ │ │ │ -100 virtual _~_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e() {} │ │ │ │ │ +_1_0_0 _N_o_d_e_s _n_o_d_e_s__; │ │ │ │ │ 101 │ │ │ │ │ -103 │ │ │ │ │ -_1_0_5 mutable boost::optional _c_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l__; │ │ │ │ │ -_1_1_0 mutable std::mutex _c_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l_M_u_t_e_x__; │ │ │ │ │ -111 │ │ │ │ │ -112 public: │ │ │ │ │ -113 sharedConditional conditional_; │ │ │ │ │ -114 derived_weak_ptr parent_; │ │ │ │ │ -115 _F_a_s_t_V_e_c_t_o_r_<_d_e_r_i_v_e_d___p_t_r_> children; │ │ │ │ │ -116 int problemSize_; │ │ │ │ │ -117 │ │ │ │ │ -118 bool is_root = false; │ │ │ │ │ -119 │ │ │ │ │ -123 void _s_e_t_E_l_i_m_i_n_a_t_i_o_n_R_e_s_u_l_t(const typename FactorGraphType:: │ │ │ │ │ -EliminationResult& eliminationResult); │ │ │ │ │ +_1_0_3 _R_o_o_t_s _r_o_o_t_s__; │ │ │ │ │ +104 │ │ │ │ │ +107 │ │ │ │ │ +_1_0_9 _B_a_y_e_s_T_r_e_e() {} │ │ │ │ │ +110 │ │ │ │ │ +112 _B_a_y_e_s_T_r_e_e(const This& other); │ │ │ │ │ +113 │ │ │ │ │ +115 │ │ │ │ │ +117 This& _o_p_e_r_a_t_o_r_=(const This& other); │ │ │ │ │ +118 │ │ │ │ │ +121 │ │ │ │ │ +123 bool _e_q_u_a_l_s(const This& other, double tol = 1e-9) const; │ │ │ │ │ 124 │ │ │ │ │ -127 │ │ │ │ │ -129 bool _e_q_u_a_l_s(const DERIVED& other, double tol = 1e-9) const; │ │ │ │ │ +125 public: │ │ │ │ │ +127 void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ +128 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const; │ │ │ │ │ 130 │ │ │ │ │ -132 virtual void _p_r_i_n_t( │ │ │ │ │ -133 const std::string& s = "", │ │ │ │ │ -134 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const; │ │ │ │ │ -135 │ │ │ │ │ -139 │ │ │ │ │ -_1_4_1 const sharedConditional& _c_o_n_d_i_t_i_o_n_a_l() const { return conditional_; } │ │ │ │ │ -142 │ │ │ │ │ -_1_4_4 inline bool _i_s_R_o_o_t() const { return parent_.expired(); } │ │ │ │ │ -145 │ │ │ │ │ -_1_4_7 size_t _n_r_C_h_i_l_d_r_e_n() const { return children.size(); } │ │ │ │ │ -148 │ │ │ │ │ -_1_5_0 const derived_ptr _o_p_e_r_a_t_o_r_[_](size_t i) const { return children[i]; } │ │ │ │ │ -151 │ │ │ │ │ -153 size_t _t_r_e_e_S_i_z_e() const; │ │ │ │ │ -154 │ │ │ │ │ -156 size_t _n_u_m_C_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l_s() const; │ │ │ │ │ -157 │ │ │ │ │ -_1_5_9 derived_ptr _p_a_r_e_n_t() const { return parent_.lock(); } │ │ │ │ │ -160 │ │ │ │ │ -_1_6_2 int _p_r_o_b_l_e_m_S_i_z_e() const { return problemSize_; } │ │ │ │ │ -163 │ │ │ │ │ -167 │ │ │ │ │ -169 BayesNetType _s_h_o_r_t_c_u_t(const derived_ptr& root, Eliminate function = │ │ │ │ │ +133 │ │ │ │ │ +135 size_t _s_i_z_e() const; │ │ │ │ │ +136 │ │ │ │ │ +_1_3_8 inline bool _e_m_p_t_y() const { │ │ │ │ │ +139 return _n_o_d_e_s__.empty(); │ │ │ │ │ +140 } │ │ │ │ │ +141 │ │ │ │ │ +_1_4_3 const _N_o_d_e_s& _n_o_d_e_s() const { return _n_o_d_e_s__; } │ │ │ │ │ +144 │ │ │ │ │ +_1_4_6 _s_h_a_r_e_d_C_l_i_q_u_e _o_p_e_r_a_t_o_r_[_](_K_e_y j) const { return _n_o_d_e_s__.at(j); } │ │ │ │ │ +147 │ │ │ │ │ +_1_4_9 const _R_o_o_t_s& _r_o_o_t_s() const { return _r_o_o_t_s__; } │ │ │ │ │ +150 │ │ │ │ │ +_1_5_2 const _s_h_a_r_e_d_C_l_i_q_u_e& _c_l_i_q_u_e(_K_e_y j) const { │ │ │ │ │ +153 typename Nodes::const_iterator c = _n_o_d_e_s__.find(j); │ │ │ │ │ +154 if(c == _n_o_d_e_s__.end()) │ │ │ │ │ +155 throw std::out_of_range("Requested the BayesTree clique for a key that is │ │ │ │ │ +not in the BayesTree"); │ │ │ │ │ +156 else │ │ │ │ │ +157 return c->second; │ │ │ │ │ +158 } │ │ │ │ │ +159 │ │ │ │ │ +161 _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_D_a_t_a _g_e_t_C_l_i_q_u_e_D_a_t_a() const; │ │ │ │ │ +162 │ │ │ │ │ +164 size_t _n_u_m_C_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l_s() const; │ │ │ │ │ +165 │ │ │ │ │ +171 sharedConditional _m_a_r_g_i_n_a_l_F_a_c_t_o_r(_K_e_y j, const Eliminate& function = │ │ │ │ │ EliminationTraitsType::DefaultEliminate) const; │ │ │ │ │ -170 │ │ │ │ │ -172 FactorGraphType _s_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l(Eliminate function = │ │ │ │ │ +172 │ │ │ │ │ +177 sharedFactorGraph _j_o_i_n_t(_K_e_y j1, _K_e_y j2, const Eliminate& function = │ │ │ │ │ EliminationTraitsType::DefaultEliminate) const; │ │ │ │ │ -173 │ │ │ │ │ -175 FactorGraphType _m_a_r_g_i_n_a_l_2(Eliminate function = EliminationTraitsType:: │ │ │ │ │ -DefaultEliminate) const; │ │ │ │ │ -176 │ │ │ │ │ -181 void _d_e_l_e_t_e_C_a_c_h_e_d_S_h_o_r_t_c_u_t_s(); │ │ │ │ │ -182 │ │ │ │ │ -183 const boost::optional& cachedSeparatorMarginal() const { │ │ │ │ │ -184 std::lock_guard marginalLock(_c_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l_M_u_t_e_x__); │ │ │ │ │ -185 return _c_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l__; │ │ │ │ │ -186 } │ │ │ │ │ +178 │ │ │ │ │ +183 sharedBayesNet _j_o_i_n_t_B_a_y_e_s_N_e_t(_K_e_y j1, _K_e_y j2, const Eliminate& function = │ │ │ │ │ +EliminationTraitsType::DefaultEliminate) const; │ │ │ │ │ +184 │ │ │ │ │ 187 │ │ │ │ │ -188 friend class BayesTree; │ │ │ │ │ -189 │ │ │ │ │ -190 protected: │ │ │ │ │ -191 │ │ │ │ │ -193 _K_e_y_V_e_c_t_o_r _s_e_p_a_r_a_t_o_r___s_e_t_m_i_n_u_s___B(const derived_ptr& B) const; │ │ │ │ │ +189 void _d_o_t(std::ostream& os, const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ +DefaultKeyFormatter) const; │ │ │ │ │ +190 │ │ │ │ │ +192 std::string _d_o_t( │ │ │ │ │ +193 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const; │ │ │ │ │ 194 │ │ │ │ │ -198 _K_e_y_V_e_c_t_o_r _s_h_o_r_t_c_u_t___i_n_d_i_c_e_s(const derived_ptr& B, const FactorGraphType& │ │ │ │ │ -p_Cp_B) const; │ │ │ │ │ -199 │ │ │ │ │ -_2_0_1 void _d_e_l_e_t_e_C_a_c_h_e_d_S_h_o_r_t_c_u_t_s_N_o_n_R_e_c_u_r_s_i_v_e() { │ │ │ │ │ -202 std::lock_guard marginalLock(_c_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l_M_u_t_e_x__); │ │ │ │ │ -203 _c_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l__ = boost::none; │ │ │ │ │ -204 } │ │ │ │ │ -205 │ │ │ │ │ -206 private: │ │ │ │ │ -207 │ │ │ │ │ -_2_0_9 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -210 template │ │ │ │ │ -211 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -212 if(!parent_.lock()) { │ │ │ │ │ -213 is_root = true; │ │ │ │ │ -214 } │ │ │ │ │ -215 ar & BOOST_SERIALIZATION_NVP(is_root); │ │ │ │ │ -216 ar & BOOST_SERIALIZATION_NVP(conditional_); │ │ │ │ │ -217 if (!is_root) { // TODO(fan): Workaround for boost/serialization #119 │ │ │ │ │ -218 ar & BOOST_SERIALIZATION_NVP(parent_); │ │ │ │ │ -219 } │ │ │ │ │ -220 ar & BOOST_SERIALIZATION_NVP(children); │ │ │ │ │ -221 } │ │ │ │ │ -222 │ │ │ │ │ -224 │ │ │ │ │ -225 }; │ │ │ │ │ -226 │ │ │ │ │ -227} │ │ │ │ │ +196 void _s_a_v_e_G_r_a_p_h(const std::string& filename, │ │ │ │ │ +197 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const; │ │ │ │ │ +198 │ │ │ │ │ +202 │ │ │ │ │ +207 template │ │ │ │ │ +208 _K_e_y _f_i_n_d_P_a_r_e_n_t_C_l_i_q_u_e(const CONTAINER& parents) const; │ │ │ │ │ +209 │ │ │ │ │ +211 void _c_l_e_a_r(); │ │ │ │ │ +212 │ │ │ │ │ +214 void _d_e_l_e_t_e_C_a_c_h_e_d_S_h_o_r_t_c_u_t_s(); │ │ │ │ │ +215 │ │ │ │ │ +220 void _r_e_m_o_v_e_P_a_t_h(_s_h_a_r_e_d_C_l_i_q_u_e _c_l_i_q_u_e, BayesNetType* bn, _C_l_i_q_u_e_s* orphans); │ │ │ │ │ +221 │ │ │ │ │ +226 void _r_e_m_o_v_e_T_o_p(const _K_e_y_V_e_c_t_o_r& keys, BayesNetType* bn, _C_l_i_q_u_e_s* orphans); │ │ │ │ │ +227 │ │ │ │ │ +230 _C_l_i_q_u_e_s _r_e_m_o_v_e_S_u_b_t_r_e_e(const _s_h_a_r_e_d_C_l_i_q_u_e& subtree); │ │ │ │ │ +231 │ │ │ │ │ +235 void _i_n_s_e_r_t_R_o_o_t(const _s_h_a_r_e_d_C_l_i_q_u_e& subtree); │ │ │ │ │ +236 │ │ │ │ │ +238 void _a_d_d_C_l_i_q_u_e(const _s_h_a_r_e_d_C_l_i_q_u_e& _c_l_i_q_u_e, const _s_h_a_r_e_d_C_l_i_q_u_e& │ │ │ │ │ +parent_clique = _s_h_a_r_e_d_C_l_i_q_u_e()); │ │ │ │ │ +239 │ │ │ │ │ +241 void _a_d_d_F_a_c_t_o_r_s_T_o_G_r_a_p_h(_F_a_c_t_o_r_G_r_a_p_h_<_F_a_c_t_o_r_T_y_p_e_>* graph) const; │ │ │ │ │ +242 │ │ │ │ │ +243 protected: │ │ │ │ │ +244 │ │ │ │ │ +246 void _d_o_t(std::ostream &s, _s_h_a_r_e_d_C_l_i_q_u_e _c_l_i_q_u_e, const _K_e_y_F_o_r_m_a_t_t_e_r& │ │ │ │ │ +keyFormatter, │ │ │ │ │ +247 int parentnum = 0) const; │ │ │ │ │ +248 │ │ │ │ │ +250 void _g_e_t_C_l_i_q_u_e_D_a_t_a(_s_h_a_r_e_d_C_l_i_q_u_e _c_l_i_q_u_e, _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_D_a_t_a* stats) const; │ │ │ │ │ +251 │ │ │ │ │ +253 void _r_e_m_o_v_e_C_l_i_q_u_e(_s_h_a_r_e_d_C_l_i_q_u_e _c_l_i_q_u_e); │ │ │ │ │ +254 │ │ │ │ │ +256 void _f_i_l_l_N_o_d_e_s_I_n_d_e_x(const _s_h_a_r_e_d_C_l_i_q_u_e& subtree); │ │ │ │ │ +257 │ │ │ │ │ +258 // Friend JunctionTree because it directly fills roots and nodes index. │ │ │ │ │ +259 template friend class │ │ │ │ │ +_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e; │ │ │ │ │ +260 │ │ │ │ │ +261 private: │ │ │ │ │ +_2_6_3 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +264 template │ │ │ │ │ +265 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +266 ar & BOOST_SERIALIZATION_NVP(_n_o_d_e_s__); │ │ │ │ │ +267 ar & BOOST_SERIALIZATION_NVP(_r_o_o_t_s__); │ │ │ │ │ +268 } │ │ │ │ │ +269 │ │ │ │ │ +271 │ │ │ │ │ +272 }; // BayesTree │ │ │ │ │ +273 │ │ │ │ │ +274 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +275 template │ │ │ │ │ +_2_7_6 class _B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r : public CLIQUE::ConditionalType { │ │ │ │ │ +277 public: │ │ │ │ │ +278 typedef CLIQUE CliqueType; │ │ │ │ │ +279 typedef typename CLIQUE::ConditionalType Base; │ │ │ │ │ +280 │ │ │ │ │ +281 boost::shared_ptr clique; │ │ │ │ │ +282 │ │ │ │ │ +_2_9_3 _B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r(const boost::shared_ptr& clique) │ │ │ │ │ +294 : clique(clique) { │ │ │ │ │ +295 this->keys_.assign(clique->conditional()->beginParents(), │ │ │ │ │ +296 clique->conditional()->endParents()); │ │ │ │ │ +297 } │ │ │ │ │ +298 │ │ │ │ │ +299 void print( │ │ │ │ │ +300 const std::string& s = "", │ │ │ │ │ +301 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override { │ │ │ │ │ +302 clique->print(s + "stored clique", formatter); │ │ │ │ │ +303 } │ │ │ │ │ +304 }; │ │ │ │ │ +305 │ │ │ │ │ +306} │ │ │ │ │ +_F_a_s_t_L_i_s_t_._h │ │ │ │ │ +A thin wrapper around std::list that uses boost's fast_pool_allocator. │ │ │ │ │ _F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ -_t_y_p_e_s_._h │ │ │ │ │ -Typedefs for easier changing of types. │ │ │ │ │ _K_e_y_._h │ │ │ │ │ -_O_r_d_e_r_i_n_g_._h │ │ │ │ │ -Variable ordering for the elimination algorithm. │ │ │ │ │ _g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ FastVector │ │ │ │ │ FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ FastVector< Key > KeyVector │ │ │ │ │ Define collection type once and for all - also used in wrappers. │ │ │ │ │ DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_c_u_r_r_e_n_t_M_a_p │ │ │ │ │ +FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ +fast_pool_allocator instead of the defa... │ │ │ │ │ +DDeeffiinniittiioonn ConcurrentMap.h:68 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t │ │ │ │ │ +FastList is a thin wrapper around std::list that uses the boost │ │ │ │ │ +fast_pool_allocator instead of the de... │ │ │ │ │ +DDeeffiinniittiioonn FastList.h:40 │ │ │ │ │ _g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ Template to create a binary predicate. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s │ │ │ │ │ -Traits class for eliminateable factor graphs, specifies the types that result │ │ │ │ │ -from elimination,... │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e │ │ │ │ │ -This is the base class for BayesTree cliques. │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_o_p_e_r_a_t_o_r_[_] │ │ │ │ │ -const derived_ptr operator[](size_t i) const │ │ │ │ │ -Return the child at index i. │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase.h:150 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ -BayesTreeCliqueBase & operator=(const BayesTreeCliqueBase &c) │ │ │ │ │ -Shallow copy assignment constructor. │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase.h:90 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_d_e_l_e_t_e_C_a_c_h_e_d_S_h_o_r_t_c_u_t_s_N_o_n_R_e_c_u_r_s_i_v_e │ │ │ │ │ -void deleteCachedShortcutsNonRecursive() │ │ │ │ │ -Non-recursive delete cached shortcuts and marginals - internal only. │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase.h:201 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_n_r_C_h_i_l_d_r_e_n │ │ │ │ │ -size_t nrChildren() const │ │ │ │ │ -Return the number of children. │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase.h:147 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_p_r_o_b_l_e_m_S_i_z_e │ │ │ │ │ -int problemSize() const │ │ │ │ │ -Problem size (used for parallel traversal) │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase.h:162 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e │ │ │ │ │ -BayesTreeCliqueBase() │ │ │ │ │ -Default constructor. │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_p_a_r_e_n_t │ │ │ │ │ -derived_ptr parent() const │ │ │ │ │ -return a shared_ptr to the parent clique │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase.h:159 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_t_r_e_e_S_i_z_e │ │ │ │ │ -size_t treeSize() const │ │ │ │ │ -The size of subtree rooted at this clique, i.e., nr of Cliques. │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:84 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e │ │ │ │ │ -BayesTreeCliqueBase(const sharedConditional &conditional) │ │ │ │ │ -Construct from a conditional, leaving parent and child pointers uninitialized. │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase.h:78 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_c_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l_M_u_t_e_x__ │ │ │ │ │ -std::mutex cachedSeparatorMarginalMutex_ │ │ │ │ │ -This protects Cached seperator marginal P(S) from concurrent read/writes as │ │ │ │ │ -many the functions which ... │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase.h:110 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_i_s_R_o_o_t │ │ │ │ │ -bool isRoot() const │ │ │ │ │ -Return true if this clique is the root of a Bayes tree. │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase.h:144 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e │ │ │ │ │ -BayesTreeCliqueBase(const BayesTreeCliqueBase &c) │ │ │ │ │ -Shallow copy constructor. │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase.h:82 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_m_a_r_g_i_n_a_l_2 │ │ │ │ │ -FactorGraphType marginal2(Eliminate function=EliminationTraitsType:: │ │ │ │ │ -DefaultEliminate) const │ │ │ │ │ -return the marginal P(C) of the clique, using marginal caching │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:195 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_s_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l │ │ │ │ │ -FactorGraphType separatorMarginal(Eliminate function=EliminationTraitsType:: │ │ │ │ │ -DefaultEliminate) const │ │ │ │ │ -return the marginal P(S) on the separator │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:147 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_s_h_o_r_t_c_u_t │ │ │ │ │ -BayesNetType shortcut(const derived_ptr &root, Eliminate │ │ │ │ │ -function=EliminationTraitsType::DefaultEliminate) const │ │ │ │ │ -return the conditional P(S|Root) on the separator given the root │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:113 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_d_e_l_e_t_e_C_a_c_h_e_d_S_h_o_r_t_c_u_t_s │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ +nodes. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e │ │ │ │ │ +A cluster-tree that eliminates to a Bayes tree. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:184 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_S_t_a_t_s │ │ │ │ │ +clique statistics │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_D_a_t_a │ │ │ │ │ +store all the sizes │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e │ │ │ │ │ +Bayes tree. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_n_o_d_e_s__ │ │ │ │ │ +Nodes nodes_ │ │ │ │ │ +Map from indices to Clique. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_r_e_m_o_v_e_C_l_i_q_u_e │ │ │ │ │ +void removeClique(sharedClique clique) │ │ │ │ │ +remove a clique: warning, can result in a forest │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:424 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_j_o_i_n_t │ │ │ │ │ +sharedFactorGraph joint(Key j1, Key j2, const Eliminate │ │ │ │ │ +&function=EliminationTraitsType::DefaultEliminate) const │ │ │ │ │ +return joint on two variables Limitation: can only calculate joint if cliques │ │ │ │ │ +are disjoint or one of ... │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:293 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_f_i_l_l_N_o_d_e_s_I_n_d_e_x │ │ │ │ │ +void fillNodesIndex(const sharedClique &subtree) │ │ │ │ │ +Fill the nodes index for a subtree. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:246 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_d_o_t │ │ │ │ │ +void dot(std::ostream &os, const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ +Output to graphviz format, stream version. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_a_d_d_F_a_c_t_o_r_s_T_o_G_r_a_p_h │ │ │ │ │ +void addFactorsToGraph(FactorGraph< FactorType > *graph) const │ │ │ │ │ +Add all cliques in this BayesTree to the specified factor graph. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:168 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_o_p_e_r_a_t_o_r_[_] │ │ │ │ │ +sharedClique operator[](Key j) const │ │ │ │ │ +Access node by variable. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:146 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ +This & operator=(const This &other) │ │ │ │ │ +Assignment operator. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:199 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_s_h_a_r_e_d_C_l_i_q_u_e │ │ │ │ │ +boost::shared_ptr< Clique > sharedClique │ │ │ │ │ +Shared pointer to a clique. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_B_a_y_e_s_T_r_e_e │ │ │ │ │ +BayesTree() │ │ │ │ │ +Create an empty Bayes Tree. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:109 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_N_o_d_e │ │ │ │ │ +Clique Node │ │ │ │ │ +Synonym for Clique (TODO: remove) │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_c_l_e_a_r │ │ │ │ │ +void clear() │ │ │ │ │ +Remove all nodes. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:408 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_r_o_o_t_s__ │ │ │ │ │ +Roots roots_ │ │ │ │ │ +Root cliques. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:103 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_a_d_d_C_l_i_q_u_e │ │ │ │ │ +void addClique(const sharedClique &clique, const sharedClique │ │ │ │ │ +&parent_clique=sharedClique()) │ │ │ │ │ +add a clique (top down) │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:142 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_j_o_i_n_t_B_a_y_e_s_N_e_t │ │ │ │ │ +sharedBayesNet jointBayesNet(Key j1, Key j2, const Eliminate │ │ │ │ │ +&function=EliminationTraitsType::DefaultEliminate) const │ │ │ │ │ +return joint on two variables as a BayesNet Limitation: can only calculate │ │ │ │ │ +joint if cliques are disjo... │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:302 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_s_h_a_r_e_d_N_o_d_e │ │ │ │ │ +sharedClique sharedNode │ │ │ │ │ +Synonym for sharedClique (TODO: remove) │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:76 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_f_i_n_d_P_a_r_e_n_t_C_l_i_q_u_e │ │ │ │ │ +Key findParentClique(const CONTAINER &parents) const │ │ │ │ │ +Find parent clique of a conditional. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:238 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_s_i_z_e │ │ │ │ │ +size_t size() const │ │ │ │ │ +number of cliques │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:133 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_d_e_l_e_t_e_C_a_c_h_e_d_S_h_o_r_t_c_u_t_s │ │ │ │ │ void deleteCachedShortcuts() │ │ │ │ │ -This deletes the cached shortcuts of all cliques (subtree) below this clique. │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:207 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_s_h_o_r_t_c_u_t___i_n_d_i_c_e_s │ │ │ │ │ -KeyVector shortcut_indices(const derived_ptr &B, const FactorGraphType &p_Cp_B) │ │ │ │ │ -const │ │ │ │ │ -Determine variable indices to keep in recursive separator shortcut calculation │ │ │ │ │ -The factor graph p_Cp_... │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_c_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -const sharedConditional & conditional() const │ │ │ │ │ -Access the conditional. │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase.h:141 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_a_c_c_e_s_s │ │ │ │ │ +Clear all shortcut caches - use before timing on marginal calculation to avoid │ │ │ │ │ +residual cache data. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:416 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_r_e_m_o_v_e_P_a_t_h │ │ │ │ │ +void removePath(sharedClique clique, BayesNetType *bn, Cliques *orphans) │ │ │ │ │ +Remove path from clique to root and return that path as factors plus a list of │ │ │ │ │ +orphaned subtree roots... │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:448 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_n_o_d_e_s │ │ │ │ │ +const Nodes & nodes() const │ │ │ │ │ +Return nodes. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:143 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_C_l_i_q_u_e_s │ │ │ │ │ +FastList< sharedClique > Cliques │ │ │ │ │ +A convenience class for a list of shared cliques. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:89 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_C_l_i_q_u_e │ │ │ │ │ +CLIQUE Clique │ │ │ │ │ +The clique type, normally BayesTreeClique. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:73 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_m_a_r_g_i_n_a_l_F_a_c_t_o_r │ │ │ │ │ +sharedConditional marginalFactor(Key j, const Eliminate │ │ │ │ │ +&function=EliminationTraitsType::DefaultEliminate) const │ │ │ │ │ +Return marginal on any variable. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:270 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_r_o_o_t_s │ │ │ │ │ +const Roots & roots() const │ │ │ │ │ +return root cliques │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:149 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_c_l_i_q_u_e │ │ │ │ │ +const sharedClique & clique(Key j) const │ │ │ │ │ +alternate syntax for matlab: find the clique that contains the variable with │ │ │ │ │ +Key j │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:152 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_N_o_d_e_s │ │ │ │ │ +ConcurrentMap< Key, sharedClique > Nodes │ │ │ │ │ +Map from keys to Clique. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:92 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_a_c_c_e_s_s │ │ │ │ │ friend class boost::serialization::access │ │ │ │ │ Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase.h:209 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_s_e_p_a_r_a_t_o_r___s_e_t_m_i_n_u_s___B │ │ │ │ │ -KeyVector separator_setminus_B(const derived_ptr &B) const │ │ │ │ │ -Calculate set for shortcut calculations. │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_p_r_i_n_t │ │ │ │ │ -virtual void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ -print this node │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:76 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_n_u_m_C_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l_s │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:263 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_n_u_m_C_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l_s │ │ │ │ │ size_t numCachedSeparatorMarginals() const │ │ │ │ │ Collect number of cliques with cached separator marginals. │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:93 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_c_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l__ │ │ │ │ │ -boost::optional< FactorGraphType > cachedSeparatorMarginal_ │ │ │ │ │ -This stores the Cached separator marginal P(S) │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase.h:105 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_:_:_s_e_t_E_l_i_m_i_n_a_t_i_o_n_R_e_s_u_l_t │ │ │ │ │ -void setEliminationResult(const typename FactorGraphType::EliminationResult │ │ │ │ │ -&eliminationResult) │ │ │ │ │ -Fill the elimination result produced during elimination. │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase-inst.h:27 │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_g_e_t_C_l_i_q_u_e_D_a_t_a │ │ │ │ │ +BayesTreeCliqueData getCliqueData() const │ │ │ │ │ +Gather data on all cliques. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_r_e_m_o_v_e_S_u_b_t_r_e_e │ │ │ │ │ +Cliques removeSubtree(const sharedClique &subtree) │ │ │ │ │ +Remove the requested subtree. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:496 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_e_m_p_t_y │ │ │ │ │ +bool empty() const │ │ │ │ │ +Check if there are any cliques in the tree. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:138 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:212 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_i_n_s_e_r_t_R_o_o_t │ │ │ │ │ +void insertRoot(const sharedClique &subtree) │ │ │ │ │ +Insert a new subtree with known parent clique. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:260 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_s_a_v_e_G_r_a_p_h │ │ │ │ │ +void saveGraph(const std::string &filename, const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ +output to file with graphviz format. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_r_e_m_o_v_e_T_o_p │ │ │ │ │ +void removeTop(const KeyVector &keys, BayesNetType *bn, Cliques *orphans) │ │ │ │ │ +Given a list of indices, turn "contaminated" part of the tree back into a │ │ │ │ │ +factor graph. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:475 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_R_o_o_t_s │ │ │ │ │ +FastVector< sharedClique > Roots │ │ │ │ │ +Root cliques. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:95 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:276 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r_:_:_B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r │ │ │ │ │ +BayesTreeOrphanWrapper(const boost::shared_ptr< CliqueType > &clique) │ │ │ │ │ +Construct a new Bayes Tree Orphan Wrapper object. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:293 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._h │ │ │ │ │ + * _B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00683.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/VariableIndex.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/VariableIndex-inl.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,44 +94,34 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
VariableIndex.h File Reference
│ │ │ │ +
VariableIndex-inl.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::VariableIndex
 The VariableIndex class computes and stores the block column structure of a factor graph. More...
 
struct  gtsam::traits< VariableIndex >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │
Author
Richard Roberts
│ │ │ │
Date
March 26, 2013
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,29 +1,21 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -VariableIndex.h File Reference │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +VariableIndex-inl.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ -  The _V_a_r_i_a_b_l_e_I_n_d_e_x class computes and stores the block column structure │ │ │ │ │ - of a factor graph. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _V_a_r_i_a_b_l_e_I_n_d_e_x_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Author │ │ │ │ │ Richard Roberts │ │ │ │ │ Date │ │ │ │ │ March 26, 2013 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _V_a_r_i_a_b_l_e_I_n_d_e_x_._h │ │ │ │ │ + * _V_a_r_i_a_b_l_e_I_n_d_e_x_-_i_n_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00683_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/VariableIndex.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/VariableIndex-inl.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,15 +98,15 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
VariableIndex.h
│ │ │ │ +
VariableIndex-inl.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │ @@ -114,183 +114,109 @@ │ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
18#pragma once
│ │ │ │
19
│ │ │ │ -
20#include <gtsam/inference/Factor.h>
│ │ │ │ -
21#include <gtsam/inference/Key.h>
│ │ │ │ -
22#include <gtsam/base/FastMap.h>
│ │ │ │ -
23#include <gtsam/base/FastVector.h>
│ │ │ │ -
24#include <gtsam/dllexport.h>
│ │ │ │ -
25
│ │ │ │ -
26#include <boost/optional/optional.hpp>
│ │ │ │ -
27#include <boost/smart_ptr/shared_ptr.hpp>
│ │ │ │ -
28
│ │ │ │ -
29#include <cassert>
│ │ │ │ -
30#include <stdexcept>
│ │ │ │ -
31
│ │ │ │ -
32namespace gtsam {
│ │ │ │ -
33
│ │ │ │ -
│ │ │ │ -
43class GTSAM_EXPORT VariableIndex {
│ │ │ │ -
44 public:
│ │ │ │ -
45 typedef boost::shared_ptr<VariableIndex> shared_ptr;
│ │ │ │ -
46 typedef FactorIndices::iterator Factor_iterator;
│ │ │ │ -
47 typedef FactorIndices::const_iterator Factor_const_iterator;
│ │ │ │ -
48
│ │ │ │ -
49 protected:
│ │ │ │ -
50 typedef FastMap<Key, FactorIndices> KeyMap;
│ │ │ │ -
51 KeyMap index_;
│ │ │ │ -
52 size_t nFactors_; // Number of factors in the original factor graph.
│ │ │ │ -
53 size_t nEntries_; // Sum of involved variable counts of each factor.
│ │ │ │ -
54
│ │ │ │ -
55 public:
│ │ │ │ -
56 typedef KeyMap::const_iterator const_iterator;
│ │ │ │ -
57 typedef KeyMap::const_iterator iterator;
│ │ │ │ -
58 typedef KeyMap::value_type value_type;
│ │ │ │ -
59
│ │ │ │ -
62
│ │ │ │ -
64 VariableIndex() : nFactors_(0), nEntries_(0) {}
│ │ │ │ -
65
│ │ │ │ -
70 template <class FG>
│ │ │ │ -
│ │ │ │ -
71 explicit VariableIndex(const FG& factorGraph) : nFactors_(0), nEntries_(0) {
│ │ │ │ -
72 augment(factorGraph);
│ │ │ │ -
73 }
│ │ │ │ -
│ │ │ │ -
74
│ │ │ │ -
78
│ │ │ │ -
80 size_t size() const { return index_.size(); }
│ │ │ │ -
81
│ │ │ │ -
83 size_t nFactors() const { return nFactors_; }
│ │ │ │ -
84
│ │ │ │ -
86 size_t nEntries() const { return nEntries_; }
│ │ │ │ -
87
│ │ │ │ -
│ │ │ │ -
89 const FactorIndices& operator[](Key variable) const {
│ │ │ │ -
90 KeyMap::const_iterator item = index_.find(variable);
│ │ │ │ -
91 if(item == index_.end())
│ │ │ │ -
92 throw std::invalid_argument("Requested non-existent variable from VariableIndex");
│ │ │ │ -
93 else
│ │ │ │ -
94 return item->second;
│ │ │ │ -
95 }
│ │ │ │ -
│ │ │ │ -
96
│ │ │ │ -
│ │ │ │ -
98 bool empty(Key variable) const {
│ │ │ │ -
99 return (*this)[variable].empty();
│ │ │ │ -
100 }
│ │ │ │ -
│ │ │ │ -
101
│ │ │ │ -
105
│ │ │ │ -
107 bool equals(const VariableIndex& other, double tol=0.0) const;
│ │ │ │ -
108
│ │ │ │ -
110 void print(const std::string& str = "VariableIndex: ",
│ │ │ │ -
111 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ -
112
│ │ │ │ -
117 void outputMetisFormat(std::ostream& os) const;
│ │ │ │ -
118
│ │ │ │ -
119
│ │ │ │ -
123
│ │ │ │ -
128 template<class FG>
│ │ │ │ -
129 void augment(const FG& factors, boost::optional<const FactorIndices&> newFactorIndices = boost::none);
│ │ │ │ -
130
│ │ │ │ -
136 void augmentExistingFactor(const FactorIndex factorIndex, const KeySet & newKeys);
│ │ │ │ -
137
│ │ │ │ -
148 template<typename ITERATOR, class FG>
│ │ │ │ -
149 void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors);
│ │ │ │ -
150
│ │ │ │ -
152 template<typename ITERATOR>
│ │ │ │ -
153 void removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey);
│ │ │ │ -
154
│ │ │ │ -
156 const_iterator begin() const { return index_.begin(); }
│ │ │ │ -
157
│ │ │ │ -
159 const_iterator end() const { return index_.end(); }
│ │ │ │ -
160
│ │ │ │ -
162 const_iterator find(Key key) const { return index_.find(key); }
│ │ │ │ -
163
│ │ │ │ -
164protected:
│ │ │ │ -
165 Factor_iterator factorsBegin(Key variable) { return internalAt(variable).begin(); }
│ │ │ │ -
166 Factor_iterator factorsEnd(Key variable) { return internalAt(variable).end(); }
│ │ │ │ -
167
│ │ │ │ -
168 Factor_const_iterator factorsBegin(Key variable) const { return internalAt(variable).begin(); }
│ │ │ │ -
169 Factor_const_iterator factorsEnd(Key variable) const { return internalAt(variable).end(); }
│ │ │ │ -
170
│ │ │ │ -
│ │ │ │ -
172 const FactorIndices& internalAt(Key variable) const {
│ │ │ │ -
173 const KeyMap::const_iterator item = index_.find(variable);
│ │ │ │ -
174 assert(item != index_.end());
│ │ │ │ -
175 return item->second;
│ │ │ │ -
176 }
│ │ │ │ -
│ │ │ │ -
177
│ │ │ │ -
│ │ │ │ -
179 FactorIndices& internalAt(Key variable) {
│ │ │ │ -
180 const KeyMap::iterator item = index_.find(variable);
│ │ │ │ -
181 assert(item != index_.end());
│ │ │ │ -
182 return item->second;
│ │ │ │ -
183 }
│ │ │ │ -
│ │ │ │ -
184
│ │ │ │ -
185private:
│ │ │ │ -
187 friend class boost::serialization::access;
│ │ │ │ -
188 template<class ARCHIVE>
│ │ │ │ -
189 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
190 ar & BOOST_SERIALIZATION_NVP(index_);
│ │ │ │ -
191 ar & BOOST_SERIALIZATION_NVP(nFactors_);
│ │ │ │ -
192 ar & BOOST_SERIALIZATION_NVP(nEntries_);
│ │ │ │ -
193 }
│ │ │ │ -
194
│ │ │ │ -
196};
│ │ │ │ -
│ │ │ │ -
197
│ │ │ │ -
199template<>
│ │ │ │ -
│ │ │ │ -
200struct traits<VariableIndex> : public Testable<VariableIndex> {
│ │ │ │ -
201};
│ │ │ │ -
│ │ │ │ -
202
│ │ │ │ -
203} //\ namespace gtsam
│ │ │ │ -
204
│ │ │ │ -
205#include <gtsam/inference/VariableIndex-inl.h>
│ │ │ │ -
FastMap.h
A thin wrapper around std::map that uses boost's fast_pool_allocator.
│ │ │ │ -
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ - │ │ │ │ -
Factor.h
The base class for all factors.
│ │ │ │ - │ │ │ │ +
20#include <gtsam/inference/VariableIndex.h>
│ │ │ │ +
21#include <gtsam/base/timing.h>
│ │ │ │ +
22
│ │ │ │ +
23namespace gtsam {
│ │ │ │ +
24
│ │ │ │ +
25/* ************************************************************************* */
│ │ │ │ +
26template<class FG>
│ │ │ │ +
│ │ │ │ +
27void VariableIndex::augment(const FG& factors,
│ │ │ │ +
28 boost::optional<const FactorIndices&> newFactorIndices) {
│ │ │ │ +
29 gttic(VariableIndex_augment);
│ │ │ │ +
30
│ │ │ │ +
31 // Augment index for each factor
│ │ │ │ +
32 for (size_t i = 0; i < factors.size(); ++i) {
│ │ │ │ +
33 if (factors[i]) {
│ │ │ │ +
34 const size_t globalI =
│ │ │ │ +
35 newFactorIndices ? (*newFactorIndices)[i] : nFactors_;
│ │ │ │ +
36 for(const Key key: *factors[i]) {
│ │ │ │ +
37 index_[key].push_back(globalI);
│ │ │ │ +
38 ++nEntries_;
│ │ │ │ +
39 }
│ │ │ │ +
40 }
│ │ │ │ +
41
│ │ │ │ +
42 // Increment factor count even if factors are null, to keep indices consistent
│ │ │ │ +
43 if (newFactorIndices) {
│ │ │ │ +
44 if ((*newFactorIndices)[i] >= nFactors_)
│ │ │ │ +
45 nFactors_ = (*newFactorIndices)[i] + 1;
│ │ │ │ +
46 } else {
│ │ │ │ +
47 ++nFactors_;
│ │ │ │ +
48 }
│ │ │ │ +
49 }
│ │ │ │ +
50}
│ │ │ │ +
│ │ │ │ +
51
│ │ │ │ +
52/* ************************************************************************* */
│ │ │ │ +
53template<typename ITERATOR, class FG>
│ │ │ │ +
│ │ │ │ +
54void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor,
│ │ │ │ +
55 const FG& factors) {
│ │ │ │ +
56 gttic(VariableIndex_remove);
│ │ │ │ +
57
│ │ │ │ +
58 // NOTE: We intentionally do not decrement nFactors_ because the factor
│ │ │ │ +
59 // indices need to remain consistent. Removing factors from a factor graph
│ │ │ │ +
60 // does not shift the indices of other factors. Also, we keep nFactors_
│ │ │ │ +
61 // one greater than the highest-numbered factor referenced in a VariableIndex.
│ │ │ │ +
62 ITERATOR factorIndex = firstFactor;
│ │ │ │ +
63 size_t i = 0;
│ │ │ │ +
64 for (; factorIndex != lastFactor; ++factorIndex, ++i) {
│ │ │ │ +
65 if (i >= factors.size())
│ │ │ │ +
66 throw std::invalid_argument(
│ │ │ │ +
67 "Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove");
│ │ │ │ +
68 if (factors[i]) {
│ │ │ │ +
69 for(Key j: *factors[i]) {
│ │ │ │ +
70 FactorIndices& factorEntries = internalAt(j);
│ │ │ │ +
71 auto entry = std::find(factorEntries.begin(),
│ │ │ │ +
72 factorEntries.end(), *factorIndex);
│ │ │ │ +
73 if (entry == factorEntries.end())
│ │ │ │ +
74 throw std::invalid_argument(
│ │ │ │ +
75 "Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index");
│ │ │ │ +
76 factorEntries.erase(entry);
│ │ │ │ +
77 --nEntries_;
│ │ │ │ +
78 }
│ │ │ │ +
79 }
│ │ │ │ +
80 }
│ │ │ │ +
81}
│ │ │ │ +
│ │ │ │ +
82
│ │ │ │ +
83/* ************************************************************************* */
│ │ │ │ +
84template<typename ITERATOR>
│ │ │ │ +
│ │ │ │ +
85void VariableIndex::removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey) {
│ │ │ │ +
86 for (ITERATOR key = firstKey; key != lastKey; ++key) {
│ │ │ │ +
87 KeyMap::iterator entry = index_.find(*key);
│ │ │ │ +
88 if (!entry->second.empty())
│ │ │ │ +
89 throw std::invalid_argument(
│ │ │ │ +
90 "Asking to remove variables from the variable index that are not unused");
│ │ │ │ +
91 index_.erase(entry);
│ │ │ │ +
92 }
│ │ │ │ +
93}
│ │ │ │ +
│ │ │ │ +
94
│ │ │ │ +
95}
│ │ │ │ +
timing.h
Timing utilities.
│ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::FactorIndex
std::uint64_t FactorIndex
Integer nonlinear factor index type.
Definition types.h:103
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition Factor.h:34
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ - │ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition VariableIndex.h:43
│ │ │ │
gtsam::VariableIndex::internalAt
const FactorIndices & internalAt(Key variable) const
Internal version of 'at' that asserts existence.
Definition VariableIndex.h:172
│ │ │ │ -
gtsam::VariableIndex::nEntries
size_t nEntries() const
The number of nonzero blocks, i.e. the number of variable-factor entries.
Definition VariableIndex.h:86
│ │ │ │ -
gtsam::VariableIndex::begin
const_iterator begin() const
Iterator to the first variable entry.
Definition VariableIndex.h:156
│ │ │ │ -
gtsam::VariableIndex::find
const_iterator find(Key key) const
Find the iterator for the requested variable entry.
Definition VariableIndex.h:162
│ │ │ │ -
gtsam::VariableIndex::operator[]
const FactorIndices & operator[](Key variable) const
Access a list of factors by variable.
Definition VariableIndex.h:89
│ │ │ │ -
gtsam::VariableIndex::VariableIndex
VariableIndex(const FG &factorGraph)
Create a VariableIndex that computes and stores the block column structure of a factor graph.
Definition VariableIndex.h:71
│ │ │ │ -
gtsam::VariableIndex::empty
bool empty(Key variable) const
Return true if no factors associated with a variable.
Definition VariableIndex.h:98
│ │ │ │ -
gtsam::VariableIndex::size
size_t size() const
The number of variable entries. This is equal to the number of unique variable Keys.
Definition VariableIndex.h:80
│ │ │ │ -
gtsam::VariableIndex::internalAt
FactorIndices & internalAt(Key variable)
Internal version of 'at' that asserts existence.
Definition VariableIndex.h:179
│ │ │ │ -
gtsam::VariableIndex::end
const_iterator end() const
Iterator to the first variable entry.
Definition VariableIndex.h:159
│ │ │ │ -
gtsam::VariableIndex::VariableIndex
VariableIndex()
Default constructor, creates an empty VariableIndex.
Definition VariableIndex.h:64
│ │ │ │ -
gtsam::VariableIndex::nFactors
size_t nFactors() const
The number of factors in the original factor graph.
Definition VariableIndex.h:83
│ │ │ │ +
gtsam::VariableIndex::remove
void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG &factors)
Remove entries corresponding to the specified factors.
Definition VariableIndex-inl.h:54
│ │ │ │ +
gtsam::VariableIndex::removeUnusedVariables
void removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey)
Remove unused empty variables (in debug mode verifies they are empty).
Definition VariableIndex-inl.h:85
│ │ │ │ +
gtsam::VariableIndex::augment
void augment(const FG &factors, boost::optional< const FactorIndices & > newFactorIndices=boost::none)
Augment the variable index with new factors.
Definition VariableIndex-inl.h:27
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -VariableIndex.h │ │ │ │ │ +VariableIndex-inl.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ @@ -16,240 +16,126 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 18#pragma once │ │ │ │ │ 19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_M_a_p_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ -24#include │ │ │ │ │ -25 │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ -28 │ │ │ │ │ -29#include │ │ │ │ │ -30#include │ │ │ │ │ -31 │ │ │ │ │ -32namespace _g_t_s_a_m { │ │ │ │ │ -33 │ │ │ │ │ -_4_3class GTSAM_EXPORT _V_a_r_i_a_b_l_e_I_n_d_e_x { │ │ │ │ │ -44 public: │ │ │ │ │ -45 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -46 typedef FactorIndices::iterator Factor_iterator; │ │ │ │ │ -47 typedef FactorIndices::const_iterator Factor_const_iterator; │ │ │ │ │ -48 │ │ │ │ │ -49 protected: │ │ │ │ │ -50 typedef _F_a_s_t_M_a_p_<_K_e_y_,_ _F_a_c_t_o_r_I_n_d_i_c_e_s_> _K_e_y_M_a_p; │ │ │ │ │ -51 _K_e_y_M_a_p index_; │ │ │ │ │ -52 size_t nFactors_; // Number of factors in the original factor graph. │ │ │ │ │ -53 size_t nEntries_; // Sum of involved variable counts of each factor. │ │ │ │ │ -54 │ │ │ │ │ -55 public: │ │ │ │ │ -56 typedef KeyMap::const_iterator const_iterator; │ │ │ │ │ -57 typedef KeyMap::const_iterator iterator; │ │ │ │ │ -58 typedef KeyMap::value_type value_type; │ │ │ │ │ -59 │ │ │ │ │ -62 │ │ │ │ │ -_6_4 _V_a_r_i_a_b_l_e_I_n_d_e_x() : nFactors_(0), nEntries_(0) {} │ │ │ │ │ -65 │ │ │ │ │ -70 template │ │ │ │ │ -_7_1 explicit _V_a_r_i_a_b_l_e_I_n_d_e_x(const FG& factorGraph) : nFactors_(0), nEntries_(0) { │ │ │ │ │ -72 augment(factorGraph); │ │ │ │ │ -73 } │ │ │ │ │ -74 │ │ │ │ │ -78 │ │ │ │ │ -_8_0 size_t _s_i_z_e() const { return index_.size(); } │ │ │ │ │ -81 │ │ │ │ │ -_8_3 size_t _n_F_a_c_t_o_r_s() const { return nFactors_; } │ │ │ │ │ -84 │ │ │ │ │ -_8_6 size_t _n_E_n_t_r_i_e_s() const { return nEntries_; } │ │ │ │ │ -87 │ │ │ │ │ -_8_9 const _F_a_c_t_o_r_I_n_d_i_c_e_s& _o_p_e_r_a_t_o_r_[_](_K_e_y variable) const { │ │ │ │ │ -90 KeyMap::const_iterator item = index_.find(variable); │ │ │ │ │ -91 if(item == index_.end()) │ │ │ │ │ -92 throw std::invalid_argument("Requested non-existent variable from │ │ │ │ │ -VariableIndex"); │ │ │ │ │ -93 else │ │ │ │ │ -94 return item->second; │ │ │ │ │ -95 } │ │ │ │ │ -96 │ │ │ │ │ -_9_8 bool _e_m_p_t_y(_K_e_y variable) const { │ │ │ │ │ -99 return (*this)[variable].empty(); │ │ │ │ │ -100 } │ │ │ │ │ -101 │ │ │ │ │ -105 │ │ │ │ │ -107 bool _e_q_u_a_l_s(const _V_a_r_i_a_b_l_e_I_n_d_e_x& other, double tol=0.0) const; │ │ │ │ │ -108 │ │ │ │ │ -110 void _p_r_i_n_t(const std::string& str = "VariableIndex: ", │ │ │ │ │ -111 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const; │ │ │ │ │ -112 │ │ │ │ │ -117 void outputMetisFormat(std::ostream& os) const; │ │ │ │ │ -118 │ │ │ │ │ -119 │ │ │ │ │ -123 │ │ │ │ │ -128 template │ │ │ │ │ -129 void augment(const FG& factors, boost::optional │ │ │ │ │ -newFactorIndices = boost::none); │ │ │ │ │ -130 │ │ │ │ │ -136 void augmentExistingFactor(const _F_a_c_t_o_r_I_n_d_e_x factorIndex, const _K_e_y_S_e_t & │ │ │ │ │ -newKeys); │ │ │ │ │ -137 │ │ │ │ │ -148 template │ │ │ │ │ -149 void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors); │ │ │ │ │ -150 │ │ │ │ │ -152 template │ │ │ │ │ -153 void removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey); │ │ │ │ │ -154 │ │ │ │ │ -_1_5_6 const_iterator _b_e_g_i_n() const { return index_.begin(); } │ │ │ │ │ -157 │ │ │ │ │ -_1_5_9 const_iterator _e_n_d() const { return index_.end(); } │ │ │ │ │ -160 │ │ │ │ │ -_1_6_2 const_iterator _f_i_n_d(_K_e_y key) const { return index_.find(key); } │ │ │ │ │ -163 │ │ │ │ │ -164protected: │ │ │ │ │ -165 Factor_iterator factorsBegin(_K_e_y variable) { return internalAt │ │ │ │ │ -(variable).begin(); } │ │ │ │ │ -166 Factor_iterator factorsEnd(Key variable) { return internalAt(variable).end │ │ │ │ │ -(); } │ │ │ │ │ -167 │ │ │ │ │ -168 Factor_const_iterator factorsBegin(Key variable) const { return internalAt │ │ │ │ │ -(variable).begin(); } │ │ │ │ │ -169 Factor_const_iterator factorsEnd(Key variable) const { return internalAt │ │ │ │ │ -(variable).end(); } │ │ │ │ │ -170 │ │ │ │ │ -_1_7_2 const _F_a_c_t_o_r_I_n_d_i_c_e_s& _i_n_t_e_r_n_a_l_A_t(_K_e_y variable) const { │ │ │ │ │ -173 const KeyMap::const_iterator item = index_.find(variable); │ │ │ │ │ -174 assert(item != index_.end()); │ │ │ │ │ -175 return item->second; │ │ │ │ │ -176 } │ │ │ │ │ -177 │ │ │ │ │ -_1_7_9 _F_a_c_t_o_r_I_n_d_i_c_e_s& _i_n_t_e_r_n_a_l_A_t(_K_e_y variable) { │ │ │ │ │ -180 const KeyMap::iterator item = index_.find(variable); │ │ │ │ │ -181 assert(item != index_.end()); │ │ │ │ │ -182 return item->second; │ │ │ │ │ -183 } │ │ │ │ │ -184 │ │ │ │ │ -185private: │ │ │ │ │ -_1_8_7 friend class boost::serialization::access; │ │ │ │ │ -188 template │ │ │ │ │ -189 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -190 ar & BOOST_SERIALIZATION_NVP(index_); │ │ │ │ │ -191 ar & BOOST_SERIALIZATION_NVP(nFactors_); │ │ │ │ │ -192 ar & BOOST_SERIALIZATION_NVP(nEntries_); │ │ │ │ │ -193 } │ │ │ │ │ -194 │ │ │ │ │ -196}; │ │ │ │ │ -197 │ │ │ │ │ -199template<> │ │ │ │ │ -_2_0_0struct _t_r_a_i_t_s<_V_a_r_i_a_b_l_e_I_n_d_e_x> : public _T_e_s_t_a_b_l_e { │ │ │ │ │ -201}; │ │ │ │ │ -202 │ │ │ │ │ -203} //\ namespace gtsam │ │ │ │ │ -204 │ │ │ │ │ -205#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_V_a_r_i_a_b_l_e_I_n_d_e_x_-_i_n_l_._h> │ │ │ │ │ -_F_a_s_t_M_a_p_._h │ │ │ │ │ -A thin wrapper around std::map that uses boost's fast_pool_allocator. │ │ │ │ │ -_F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ -A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ -_K_e_y_._h │ │ │ │ │ -_F_a_c_t_o_r_._h │ │ │ │ │ -The base class for all factors. │ │ │ │ │ -_V_a_r_i_a_b_l_e_I_n_d_e_x_-_i_n_l_._h │ │ │ │ │ +20#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_V_a_r_i_a_b_l_e_I_n_d_e_x_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_b_a_s_e_/_t_i_m_i_n_g_._h> │ │ │ │ │ +22 │ │ │ │ │ +23namespace _g_t_s_a_m { │ │ │ │ │ +24 │ │ │ │ │ +25/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +26template │ │ │ │ │ +_2_7void _V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_a_u_g_m_e_n_t(const FG& factors, │ │ │ │ │ +28 boost::optional newFactorIndices) { │ │ │ │ │ +29 gttic(VariableIndex_augment); │ │ │ │ │ +30 │ │ │ │ │ +31 // Augment index for each factor │ │ │ │ │ +32 for (size_t i = 0; i < factors.size(); ++i) { │ │ │ │ │ +33 if (factors[i]) { │ │ │ │ │ +34 const size_t globalI = │ │ │ │ │ +35 newFactorIndices ? (*newFactorIndices)[i] : nFactors_; │ │ │ │ │ +36 for(const _K_e_y key: *factors[i]) { │ │ │ │ │ +37 index_[key].push_back(globalI); │ │ │ │ │ +38 ++nEntries_; │ │ │ │ │ +39 } │ │ │ │ │ +40 } │ │ │ │ │ +41 │ │ │ │ │ +42 // Increment factor count even if factors are null, to keep indices │ │ │ │ │ +consistent │ │ │ │ │ +43 if (newFactorIndices) { │ │ │ │ │ +44 if ((*newFactorIndices)[i] >= nFactors_) │ │ │ │ │ +45 nFactors_ = (*newFactorIndices)[i] + 1; │ │ │ │ │ +46 } else { │ │ │ │ │ +47 ++nFactors_; │ │ │ │ │ +48 } │ │ │ │ │ +49 } │ │ │ │ │ +50} │ │ │ │ │ +51 │ │ │ │ │ +52/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +53template │ │ │ │ │ +_5_4void _V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_r_e_m_o_v_e(ITERATOR firstFactor, ITERATOR lastFactor, │ │ │ │ │ +55 const FG& factors) { │ │ │ │ │ +56 gttic(VariableIndex_remove); │ │ │ │ │ +57 │ │ │ │ │ +58 // NOTE: We intentionally do not decrement nFactors_ because the factor │ │ │ │ │ +59 // indices need to remain consistent. Removing factors from a factor graph │ │ │ │ │ +60 // does not shift the indices of other factors. Also, we keep nFactors_ │ │ │ │ │ +61 // one greater than the highest-numbered factor referenced in a │ │ │ │ │ +VariableIndex. │ │ │ │ │ +62 ITERATOR factorIndex = firstFactor; │ │ │ │ │ +63 size_t i = 0; │ │ │ │ │ +64 for (; factorIndex != lastFactor; ++factorIndex, ++i) { │ │ │ │ │ +65 if (i >= factors.size()) │ │ │ │ │ +66 throw std::invalid_argument( │ │ │ │ │ +67 "Internal error, requested inconsistent number of factor indices and factors │ │ │ │ │ +in VariableIndex::remove"); │ │ │ │ │ +68 if (factors[i]) { │ │ │ │ │ +69 for(_K_e_y j: *factors[i]) { │ │ │ │ │ +70 _F_a_c_t_o_r_I_n_d_i_c_e_s& factorEntries = _i_n_t_e_r_n_a_l_A_t(j); │ │ │ │ │ +71 auto entry = std::find(factorEntries.begin(), │ │ │ │ │ +72 factorEntries.end(), *factorIndex); │ │ │ │ │ +73 if (entry == factorEntries.end()) │ │ │ │ │ +74 throw std::invalid_argument( │ │ │ │ │ +75 "Internal error, indices and factors passed into VariableIndex::remove are │ │ │ │ │ +not consistent with the existing variable index"); │ │ │ │ │ +76 factorEntries.erase(entry); │ │ │ │ │ +77 --nEntries_; │ │ │ │ │ +78 } │ │ │ │ │ +79 } │ │ │ │ │ +80 } │ │ │ │ │ +81} │ │ │ │ │ +82 │ │ │ │ │ +83/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +84template │ │ │ │ │ +_8_5void _V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_r_e_m_o_v_e_U_n_u_s_e_d_V_a_r_i_a_b_l_e_s(ITERATOR firstKey, ITERATOR │ │ │ │ │ +lastKey) { │ │ │ │ │ +86 for (ITERATOR key = firstKey; key != lastKey; ++key) { │ │ │ │ │ +87 KeyMap::iterator entry = index_.find(*key); │ │ │ │ │ +88 if (!entry->second.empty()) │ │ │ │ │ +89 throw std::invalid_argument( │ │ │ │ │ +90 "Asking to remove variables from the variable index that are not unused"); │ │ │ │ │ +91 index_.erase(entry); │ │ │ │ │ +92 } │ │ │ │ │ +93} │ │ │ │ │ +94 │ │ │ │ │ +95} │ │ │ │ │ +_t_i_m_i_n_g_._h │ │ │ │ │ +Timing utilities. │ │ │ │ │ +_V_a_r_i_a_b_l_e_I_n_d_e_x_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_I_n_d_e_x │ │ │ │ │ -std::uint64_t FactorIndex │ │ │ │ │ -Integer nonlinear factor index type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:103 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ _g_t_s_a_m_:_:_F_a_c_t_o_r_I_n_d_i_c_e_s │ │ │ │ │ FastVector< FactorIndex > FactorIndices │ │ │ │ │ Define collection types: │ │ │ │ │ DDeeffiinniittiioonn Factor.h:34 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ -FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ -fast_pool_allocator instead of the defa... │ │ │ │ │ -DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ -The VariableIndex class computes and stores the block column structure of a │ │ │ │ │ -factor graph. │ │ │ │ │ -DDeeffiinniittiioonn VariableIndex.h:43 │ │ │ │ │ _g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_i_n_t_e_r_n_a_l_A_t │ │ │ │ │ const FactorIndices & internalAt(Key variable) const │ │ │ │ │ Internal version of 'at' that asserts existence. │ │ │ │ │ DDeeffiinniittiioonn VariableIndex.h:172 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_n_E_n_t_r_i_e_s │ │ │ │ │ -size_t nEntries() const │ │ │ │ │ -The number of nonzero blocks, i.e. the number of variable-factor entries. │ │ │ │ │ -DDeeffiinniittiioonn VariableIndex.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_b_e_g_i_n │ │ │ │ │ -const_iterator begin() const │ │ │ │ │ -Iterator to the first variable entry. │ │ │ │ │ -DDeeffiinniittiioonn VariableIndex.h:156 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_f_i_n_d │ │ │ │ │ -const_iterator find(Key key) const │ │ │ │ │ -Find the iterator for the requested variable entry. │ │ │ │ │ -DDeeffiinniittiioonn VariableIndex.h:162 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_o_p_e_r_a_t_o_r_[_] │ │ │ │ │ -const FactorIndices & operator[](Key variable) const │ │ │ │ │ -Access a list of factors by variable. │ │ │ │ │ -DDeeffiinniittiioonn VariableIndex.h:89 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ -VariableIndex(const FG &factorGraph) │ │ │ │ │ -Create a VariableIndex that computes and stores the block column structure of a │ │ │ │ │ -factor graph. │ │ │ │ │ -DDeeffiinniittiioonn VariableIndex.h:71 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_e_m_p_t_y │ │ │ │ │ -bool empty(Key variable) const │ │ │ │ │ -Return true if no factors associated with a variable. │ │ │ │ │ -DDeeffiinniittiioonn VariableIndex.h:98 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_s_i_z_e │ │ │ │ │ -size_t size() const │ │ │ │ │ -The number of variable entries. This is equal to the number of unique variable │ │ │ │ │ -Keys. │ │ │ │ │ -DDeeffiinniittiioonn VariableIndex.h:80 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_i_n_t_e_r_n_a_l_A_t │ │ │ │ │ -FactorIndices & internalAt(Key variable) │ │ │ │ │ -Internal version of 'at' that asserts existence. │ │ │ │ │ -DDeeffiinniittiioonn VariableIndex.h:179 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_e_n_d │ │ │ │ │ -const_iterator end() const │ │ │ │ │ -Iterator to the first variable entry. │ │ │ │ │ -DDeeffiinniittiioonn VariableIndex.h:159 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ -VariableIndex() │ │ │ │ │ -Default constructor, creates an empty VariableIndex. │ │ │ │ │ -DDeeffiinniittiioonn VariableIndex.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_n_F_a_c_t_o_r_s │ │ │ │ │ -size_t nFactors() const │ │ │ │ │ -The number of factors in the original factor graph. │ │ │ │ │ -DDeeffiinniittiioonn VariableIndex.h:83 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_r_e_m_o_v_e │ │ │ │ │ +void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG &factors) │ │ │ │ │ +Remove entries corresponding to the specified factors. │ │ │ │ │ +DDeeffiinniittiioonn VariableIndex-inl.h:54 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_r_e_m_o_v_e_U_n_u_s_e_d_V_a_r_i_a_b_l_e_s │ │ │ │ │ +void removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey) │ │ │ │ │ +Remove unused empty variables (in debug mode verifies they are empty). │ │ │ │ │ +DDeeffiinniittiioonn VariableIndex-inl.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_a_u_g_m_e_n_t │ │ │ │ │ +void augment(const FG &factors, boost::optional< const FactorIndices & > │ │ │ │ │ +newFactorIndices=boost::none) │ │ │ │ │ +Augment the variable index with new factors. │ │ │ │ │ +DDeeffiinniittiioonn VariableIndex-inl.h:27 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _V_a_r_i_a_b_l_e_I_n_d_e_x_._h │ │ │ │ │ + * _V_a_r_i_a_b_l_e_I_n_d_e_x_-_i_n_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00692.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/DotWriter.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/JunctionTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,36 +94,49 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
DotWriter.cpp File Reference
│ │ │ │ +
JunctionTree.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Graphviz formatting for factor graphs. │ │ │ │ +

The junction tree. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::JunctionTree< BAYESTREE, GRAPH >
 A JunctionTree is a cluster tree, a set of variable clusters with factors, arranged in a tree, with the additional property that it represents the clique tree associated with a Bayes Net. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Graphviz formatting for factor graphs.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
Date
December, 2021
│ │ │ │ +

The junction tree.

│ │ │ │ +
Date
Feb 4, 2010
│ │ │ │ +
Author
Kai Ni
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,31 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -DotWriter.cpp File Reference │ │ │ │ │ -Graphviz formatting for factor graphs. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +JunctionTree.h File Reference │ │ │ │ │ +The junction tree. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_J_u_n_c_t_i_o_n_T_r_e_e_<_ _B_A_Y_E_S_T_R_E_E_,_ _G_R_A_P_H_ _> │ │ │ │ │ + A _J_u_n_c_t_i_o_n_T_r_e_e is a cluster tree, a set of variable clusters with │ │ │ │ │ +  factors, arranged in a tree, with the additional property that it │ │ │ │ │ + represents the clique tree associated with a Bayes Net. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Graphviz formatting for factor graphs. │ │ │ │ │ +The junction tree. │ │ │ │ │ + Date │ │ │ │ │ + Feb 4, 2010 │ │ │ │ │ Author │ │ │ │ │ + Kai Ni │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - December, 2021 │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _D_o_t_W_r_i_t_e_r_._c_p_p │ │ │ │ │ + * _J_u_n_c_t_i_o_n_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00695_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/ClusterTree-inst.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/EliminateableFactorGraph-inst.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,357 +98,483 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ClusterTree-inst.h
│ │ │ │ +
EliminateableFactorGraph-inst.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1
│ │ │ │ -
10#pragma once
│ │ │ │ +
1/* ----------------------------------------------------------------------------
│ │ │ │ +
2
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
4 * Atlanta, Georgia 30332-0415
│ │ │ │ +
5 * All Rights Reserved
│ │ │ │ +
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
7
│ │ │ │ +
8 * See LICENSE for the license information
│ │ │ │ +
9
│ │ │ │ +
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
12#include <gtsam/inference/ClusterTree.h>
│ │ │ │ -
13#include <gtsam/inference/BayesTree.h>
│ │ │ │ -
14#include <gtsam/inference/Ordering.h>
│ │ │ │ -
15#include <gtsam/base/timing.h>
│ │ │ │ -
16#include <gtsam/base/treeTraversal-inst.h>
│ │ │ │ -
17
│ │ │ │ -
18#ifdef GTSAM_USE_TBB
│ │ │ │ -
19#include <mutex>
│ │ │ │ -
20#endif
│ │ │ │ -
21
│ │ │ │ -
22namespace gtsam {
│ │ │ │ -
23
│ │ │ │ -
24/* ************************************************************************* */
│ │ │ │ -
25template<class GRAPH>
│ │ │ │ -
│ │ │ │ -
26void ClusterTree<GRAPH>::Cluster::print(const std::string& s,
│ │ │ │ -
27 const KeyFormatter& keyFormatter) const {
│ │ │ │ -
28 std::cout << s << " (" << problemSize_ << ")";
│ │ │ │ -
29 PrintKeyVector(orderedFrontalKeys);
│ │ │ │ -
30}
│ │ │ │ -
│ │ │ │ -
31
│ │ │ │ -
32/* ************************************************************************* */
│ │ │ │ -
33template <class GRAPH>
│ │ │ │ -
│ │ │ │ -
34std::vector<size_t> ClusterTree<GRAPH>::Cluster::nrFrontalsOfChildren() const {
│ │ │ │ -
35 std::vector<size_t> nrFrontals;
│ │ │ │ -
36 nrFrontals.reserve(nrChildren());
│ │ │ │ -
37 for (const sharedNode& child : children)
│ │ │ │ -
38 nrFrontals.push_back(child->nrFrontals());
│ │ │ │ -
39 return nrFrontals;
│ │ │ │ -
40}
│ │ │ │ -
│ │ │ │ -
41
│ │ │ │ -
42/* ************************************************************************* */
│ │ │ │ -
43template <class GRAPH>
│ │ │ │ -
│ │ │ │ -
44void ClusterTree<GRAPH>::Cluster::merge(const boost::shared_ptr<Cluster>& cluster) {
│ │ │ │ -
45 // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after..
│ │ │ │ -
46 orderedFrontalKeys.insert(orderedFrontalKeys.end(), cluster->orderedFrontalKeys.rbegin(),
│ │ │ │ -
47 cluster->orderedFrontalKeys.rend());
│ │ │ │ -
48 factors.push_back(cluster->factors);
│ │ │ │ -
49 children.insert(children.end(), cluster->children.begin(), cluster->children.end());
│ │ │ │ -
50 // Increment problem size
│ │ │ │ -
51 problemSize_ = std::max(problemSize_, cluster->problemSize_);
│ │ │ │ -
52}
│ │ │ │ -
│ │ │ │ -
53
│ │ │ │ -
54/* ************************************************************************* */
│ │ │ │ -
55template<class GRAPH>
│ │ │ │ -
│ │ │ │ -
56void ClusterTree<GRAPH>::Cluster::mergeChildren(
│ │ │ │ -
57 const std::vector<bool>& merge) {
│ │ │ │ -
58 gttic(Cluster_mergeChildren);
│ │ │ │ -
59 assert(merge.size() == this->children.size());
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/inference/EliminateableFactorGraph.h>
│ │ │ │ +
22#include <gtsam/inference/inferenceExceptions.h>
│ │ │ │ +
23#include <boost/tuple/tuple.hpp>
│ │ │ │ +
24
│ │ │ │ +
25namespace gtsam {
│ │ │ │ +
26
│ │ │ │ +
27 /* ************************************************************************* */
│ │ │ │ +
28 template<class FACTORGRAPH>
│ │ │ │ +
29 boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
│ │ │ │ +
│ │ │ │ +
30 EliminateableFactorGraph<FACTORGRAPH>::eliminateSequential(
│ │ │ │ +
31 OptionalOrderingType orderingType, const Eliminate& function,
│ │ │ │ +
32 OptionalVariableIndex variableIndex) const {
│ │ │ │ +
33 if(!variableIndex) {
│ │ │ │ +
34 // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check
│ │ │ │ +
35 // for no variable index first so that it's always computed if we need to call COLAMD because
│ │ │ │ +
36 // no Ordering is provided. When removing optional from VariableIndex, create VariableIndex
│ │ │ │ +
37 // before creating ordering.
│ │ │ │ +
38 VariableIndex computedVariableIndex(asDerived());
│ │ │ │ +
39 return eliminateSequential(orderingType, function, computedVariableIndex);
│ │ │ │ +
40 }
│ │ │ │ +
41 else {
│ │ │ │ +
42 // Compute an ordering and call this function again. We are guaranteed to have a
│ │ │ │ +
43 // VariableIndex already here because we computed one if needed in the previous 'if' block.
│ │ │ │ +
44 if (orderingType == Ordering::METIS) {
│ │ │ │ +
45 Ordering computedOrdering = Ordering::Metis(asDerived());
│ │ │ │ +
46 return eliminateSequential(computedOrdering, function, variableIndex);
│ │ │ │ +
47 } else if (orderingType == Ordering::COLAMD) {
│ │ │ │ +
48 Ordering computedOrdering = Ordering::Colamd(*variableIndex);
│ │ │ │ +
49 return eliminateSequential(computedOrdering, function, variableIndex);
│ │ │ │ +
50 } else if (orderingType == Ordering::NATURAL) {
│ │ │ │ +
51 Ordering computedOrdering = Ordering::Natural(asDerived());
│ │ │ │ +
52 return eliminateSequential(computedOrdering, function, variableIndex);
│ │ │ │ +
53 } else {
│ │ │ │ +
54 Ordering computedOrdering = EliminationTraitsType::DefaultOrderingFunc(
│ │ │ │ +
55 asDerived(), variableIndex);
│ │ │ │ +
56 return eliminateSequential(computedOrdering, function, variableIndex);
│ │ │ │ +
57 }
│ │ │ │ +
58 }
│ │ │ │ +
59 }
│ │ │ │ +
│ │ │ │
60
│ │ │ │ -
61 // Count how many keys, factors and children we'll end up with
│ │ │ │ -
62 size_t nrKeys = orderedFrontalKeys.size();
│ │ │ │ -
63 size_t nrFactors = factors.size();
│ │ │ │ -
64 size_t nrNewChildren = 0;
│ │ │ │ -
65 // Loop over children
│ │ │ │ -
66 size_t i = 0;
│ │ │ │ -
67 for(const sharedNode& child: this->children) {
│ │ │ │ -
68 if (merge[i]) {
│ │ │ │ -
69 nrKeys += child->orderedFrontalKeys.size();
│ │ │ │ -
70 nrFactors += child->factors.size();
│ │ │ │ -
71 nrNewChildren += child->nrChildren();
│ │ │ │ +
61 /* ************************************************************************* */
│ │ │ │ +
62 template<class FACTORGRAPH>
│ │ │ │ +
63 boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
│ │ │ │ +
│ │ │ │ +
64 EliminateableFactorGraph<FACTORGRAPH>::eliminateSequential(
│ │ │ │ +
65 const Ordering& ordering, const Eliminate& function,
│ │ │ │ +
66 OptionalVariableIndex variableIndex) const
│ │ │ │ +
67 {
│ │ │ │ +
68 if(!variableIndex) {
│ │ │ │ +
69 // If no VariableIndex provided, compute one and call this function again
│ │ │ │ +
70 VariableIndex computedVariableIndex(asDerived());
│ │ │ │ +
71 return eliminateSequential(ordering, function, computedVariableIndex);
│ │ │ │
72 } else {
│ │ │ │ -
73 nrNewChildren += 1; // we keep the child
│ │ │ │ -
74 }
│ │ │ │ -
75 ++i;
│ │ │ │ -
76 }
│ │ │ │ -
77
│ │ │ │ -
78 // now reserve space, and really merge
│ │ │ │ -
79 auto oldChildren = this->children;
│ │ │ │ -
80 this->children.clear();
│ │ │ │ -
81 this->children.reserve(nrNewChildren);
│ │ │ │ -
82 orderedFrontalKeys.reserve(nrKeys);
│ │ │ │ -
83 factors.reserve(nrFactors);
│ │ │ │ -
84 i = 0;
│ │ │ │ -
85 for (const sharedNode& child : oldChildren) {
│ │ │ │ -
86 if (merge[i]) {
│ │ │ │ -
87 this->merge(child);
│ │ │ │ -
88 } else {
│ │ │ │ -
89 this->addChild(child); // we keep the child
│ │ │ │ -
90 }
│ │ │ │ -
91 ++i;
│ │ │ │ -
92 }
│ │ │ │ -
│ │ │ │ -
93 std::reverse(orderedFrontalKeys.begin(), orderedFrontalKeys.end());
│ │ │ │ -
94}
│ │ │ │ -
95
│ │ │ │ -
96/* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
97template <class GRAPH>
│ │ │ │ -
│ │ │ │ -
98void ClusterTree<GRAPH>::print(const std::string& s, const KeyFormatter& keyFormatter) const {
│ │ │ │ -
99 treeTraversal::PrintForest(*this, s, keyFormatter);
│ │ │ │ -
100}
│ │ │ │ -
│ │ │ │ -
101
│ │ │ │ -
102/* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
103template <class GRAPH>
│ │ │ │ -
│ │ │ │ -
104ClusterTree<GRAPH>& ClusterTree<GRAPH>::operator=(const This& other) {
│ │ │ │ -
105 // Start by duplicating the tree.
│ │ │ │ -
106 roots_ = treeTraversal::CloneForest(other);
│ │ │ │ -
107 return *this;
│ │ │ │ -
108}
│ │ │ │ -
│ │ │ │ -
109
│ │ │ │ -
110/* ************************************************************************* */
│ │ │ │ -
111// Elimination traversal data - stores a pointer to the parent data and collects
│ │ │ │ -
112// the factors resulting from elimination of the children. Also sets up BayesTree
│ │ │ │ -
113// cliques with parent and child pointers.
│ │ │ │ -
114template<class CLUSTERTREE>
│ │ │ │ -
│ │ │ │ -
115struct EliminationData {
│ │ │ │ -
116 // Typedefs
│ │ │ │ -
117 typedef typename CLUSTERTREE::sharedFactor sharedFactor;
│ │ │ │ -
118 typedef typename CLUSTERTREE::FactorType FactorType;
│ │ │ │ -
119 typedef typename CLUSTERTREE::FactorGraphType FactorGraphType;
│ │ │ │ -
120 typedef typename CLUSTERTREE::ConditionalType ConditionalType;
│ │ │ │ -
121 typedef typename CLUSTERTREE::BayesTreeType::Node BTNode;
│ │ │ │ -
122
│ │ │ │ -
123 EliminationData* const parentData;
│ │ │ │ -
124 size_t myIndexInParent;
│ │ │ │ -
125 FastVector<sharedFactor> childFactors;
│ │ │ │ -
126 boost::shared_ptr<BTNode> bayesTreeNode;
│ │ │ │ -
127#ifdef GTSAM_USE_TBB
│ │ │ │ -
128 boost::shared_ptr<std::mutex> writeLock;
│ │ │ │ -
129#endif
│ │ │ │ -
130
│ │ │ │ -
131 EliminationData(EliminationData* _parentData, size_t nChildren) :
│ │ │ │ -
132 parentData(_parentData), bayesTreeNode(boost::make_shared<BTNode>())
│ │ │ │ -
133#ifdef GTSAM_USE_TBB
│ │ │ │ -
134 , writeLock(boost::make_shared<std::mutex>())
│ │ │ │ -
135#endif
│ │ │ │ -
136 {
│ │ │ │ -
137 if (parentData) {
│ │ │ │ -
│ │ │ │ -
138#ifdef GTSAM_USE_TBB
│ │ │ │ -
139 parentData->writeLock->lock();
│ │ │ │ -
140#endif
│ │ │ │ -
141 myIndexInParent = parentData->childFactors.size();
│ │ │ │ -
142 parentData->childFactors.push_back(sharedFactor());
│ │ │ │ -
143#ifdef GTSAM_USE_TBB
│ │ │ │ -
144 parentData->writeLock->unlock();
│ │ │ │ -
145#endif
│ │ │ │ -
146 } else {
│ │ │ │ -
147 myIndexInParent = 0;
│ │ │ │ +
73 gttic(eliminateSequential);
│ │ │ │ +
74 // Do elimination
│ │ │ │ +
75 EliminationTreeType etree(asDerived(), *variableIndex, ordering);
│ │ │ │ +
76 boost::shared_ptr<BayesNetType> bayesNet;
│ │ │ │ +
77 boost::shared_ptr<FactorGraphType> factorGraph;
│ │ │ │ +
78 boost::tie(bayesNet,factorGraph) = etree.eliminate(function);
│ │ │ │ +
79 // If any factors are remaining, the ordering was incomplete
│ │ │ │ +
80 if(!factorGraph->empty())
│ │ │ │ +
81 throw InconsistentEliminationRequested();
│ │ │ │ +
82 // Return the Bayes net
│ │ │ │ +
83 return bayesNet;
│ │ │ │ +
84 }
│ │ │ │ +
85 }
│ │ │ │ +
│ │ │ │ +
86
│ │ │ │ +
87 /* ************************************************************************* */
│ │ │ │ +
88 template <class FACTORGRAPH>
│ │ │ │ +
89 boost::shared_ptr<
│ │ │ │ +
90 typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
│ │ │ │ +
│ │ │ │ +
91 EliminateableFactorGraph<FACTORGRAPH>::eliminateMultifrontal(
│ │ │ │ +
92 OptionalOrderingType orderingType, const Eliminate& function,
│ │ │ │ +
93 OptionalVariableIndex variableIndex) const {
│ │ │ │ +
94 if (!variableIndex) {
│ │ │ │ +
95 // If no VariableIndex provided, compute one and call this function again
│ │ │ │ +
96 // IMPORTANT: we check for no variable index first so that it's always
│ │ │ │ +
97 // computed if we need to call COLAMD because no Ordering is provided.
│ │ │ │ +
98 // When removing optional from VariableIndex, create VariableIndex before
│ │ │ │ +
99 // creating ordering.
│ │ │ │ +
100 VariableIndex computedVariableIndex(asDerived());
│ │ │ │ +
101 return eliminateMultifrontal(orderingType, function,
│ │ │ │ +
102 computedVariableIndex);
│ │ │ │ +
103 } else {
│ │ │ │ +
104 // Compute an ordering and call this function again. We are guaranteed to
│ │ │ │ +
105 // have a VariableIndex already here because we computed one if needed in
│ │ │ │ +
106 // the previous 'if' block.
│ │ │ │ +
107 if (orderingType == Ordering::METIS) {
│ │ │ │ +
108 Ordering computedOrdering = Ordering::Metis(asDerived());
│ │ │ │ +
109 return eliminateMultifrontal(computedOrdering, function, variableIndex);
│ │ │ │ +
110 } else if (orderingType == Ordering::COLAMD) {
│ │ │ │ +
111 Ordering computedOrdering = Ordering::Colamd(*variableIndex);
│ │ │ │ +
112 return eliminateMultifrontal(computedOrdering, function, variableIndex);
│ │ │ │ +
113 } else if (orderingType == Ordering::NATURAL) {
│ │ │ │ +
114 Ordering computedOrdering = Ordering::Natural(asDerived());
│ │ │ │ +
115 return eliminateMultifrontal(computedOrdering, function, variableIndex);
│ │ │ │ +
116 } else {
│ │ │ │ +
│ │ │ │ +
117 Ordering computedOrdering = EliminationTraitsType::DefaultOrderingFunc(
│ │ │ │ +
118 asDerived(), variableIndex);
│ │ │ │ +
119 return eliminateMultifrontal(computedOrdering, function, variableIndex);
│ │ │ │ +
120 }
│ │ │ │ +
121 }
│ │ │ │ +
122 }
│ │ │ │ +
123
│ │ │ │ +
124 /* ************************************************************************* */
│ │ │ │ +
125 template<class FACTORGRAPH>
│ │ │ │ +
126 boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
│ │ │ │ +
│ │ │ │ +
127 EliminateableFactorGraph<FACTORGRAPH>::eliminateMultifrontal(
│ │ │ │ +
128 const Ordering& ordering, const Eliminate& function,
│ │ │ │ +
129 OptionalVariableIndex variableIndex) const
│ │ │ │ +
130 {
│ │ │ │ +
131 if(!variableIndex) {
│ │ │ │ +
132 // If no VariableIndex provided, compute one and call this function again
│ │ │ │ +
133 VariableIndex computedVariableIndex(asDerived());
│ │ │ │ +
134 return eliminateMultifrontal(ordering, function, computedVariableIndex);
│ │ │ │ +
135 } else {
│ │ │ │ +
│ │ │ │ +
136 gttic(eliminateMultifrontal);
│ │ │ │ +
137 // Do elimination with given ordering
│ │ │ │ +
138 EliminationTreeType etree(asDerived(), *variableIndex, ordering);
│ │ │ │ +
139 JunctionTreeType junctionTree(etree);
│ │ │ │ +
140 boost::shared_ptr<BayesTreeType> bayesTree;
│ │ │ │ +
141 boost::shared_ptr<FactorGraphType> factorGraph;
│ │ │ │ +
142 boost::tie(bayesTree,factorGraph) = junctionTree.eliminate(function);
│ │ │ │ +
143 // If any factors are remaining, the ordering was incomplete
│ │ │ │ +
144 if(!factorGraph->empty())
│ │ │ │ +
145 throw InconsistentEliminationRequested();
│ │ │ │ +
146 // Return the Bayes tree
│ │ │ │ +
147 return bayesTree;
│ │ │ │
148 }
│ │ │ │ -
149 // Set up BayesTree parent and child pointers
│ │ │ │ -
150 if (parentData) {
│ │ │ │ -
151 if (parentData->parentData) // If our parent is not the dummy node
│ │ │ │ -
152 bayesTreeNode->parent_ = parentData->bayesTreeNode;
│ │ │ │ -
153 parentData->bayesTreeNode->children.push_back(bayesTreeNode);
│ │ │ │ -
154 }
│ │ │ │ -
155 }
│ │ │ │ -
156
│ │ │ │ -
157 // Elimination pre-order visitor - creates the EliminationData structure for the visited node.
│ │ │ │ -
158 static EliminationData EliminationPreOrderVisitor(
│ │ │ │ -
159 const typename CLUSTERTREE::sharedNode& node,
│ │ │ │ -
160 EliminationData& parentData) {
│ │ │ │ -
161 assert(node);
│ │ │ │ -
162 EliminationData myData(&parentData, node->nrChildren());
│ │ │ │ -
163 myData.bayesTreeNode->problemSize_ = node->problemSize();
│ │ │ │ -
164 return myData;
│ │ │ │ -
165 }
│ │ │ │ -
166
│ │ │ │ -
167 // Elimination post-order visitor - combine the child factors with our own factors, add the
│ │ │ │ -
168 // resulting conditional to the BayesTree, and add the remaining factor to the parent.
│ │ │ │ -
│ │ │ │ -
169 class EliminationPostOrderVisitor {
│ │ │ │ -
170 const typename CLUSTERTREE::Eliminate& eliminationFunction_;
│ │ │ │ -
171 typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex_;
│ │ │ │ -
172
│ │ │ │ -
173 public:
│ │ │ │ -
174 // Construct functor
│ │ │ │ -
│ │ │ │ -
175 EliminationPostOrderVisitor(
│ │ │ │ -
176 const typename CLUSTERTREE::Eliminate& eliminationFunction,
│ │ │ │ -
177 typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex) :
│ │ │ │ -
178 eliminationFunction_(eliminationFunction), nodesIndex_(nodesIndex) {
│ │ │ │ -
179 }
│ │ │ │ -
180
│ │ │ │ -
181 // Function that does the HEAVY lifting
│ │ │ │ -
182 void operator()(const typename CLUSTERTREE::sharedNode& node, EliminationData& myData) {
│ │ │ │ -
183 assert(node);
│ │ │ │ -
184
│ │ │ │ -
185 // Gather factors
│ │ │ │ -
186 FactorGraphType gatheredFactors;
│ │ │ │ -
187 gatheredFactors.reserve(node->factors.size() + node->nrChildren());
│ │ │ │ -
188 gatheredFactors += node->factors;
│ │ │ │ -
189 gatheredFactors += myData.childFactors;
│ │ │ │ -
190
│ │ │ │ -
191 // Check for Bayes tree orphan subtrees, and add them to our children
│ │ │ │ -
192 // TODO(frank): should this really happen here?
│ │ │ │ -
193 for (const sharedFactor& factor: node->factors) {
│ │ │ │ -
194 auto asSubtree = dynamic_cast<const BayesTreeOrphanWrapper<BTNode>*>(factor.get());
│ │ │ │ -
195 if (asSubtree) {
│ │ │ │ -
196 myData.bayesTreeNode->children.push_back(asSubtree->clique);
│ │ │ │ -
197 asSubtree->clique->parent_ = myData.bayesTreeNode;
│ │ │ │ -
198 }
│ │ │ │ -
199 }
│ │ │ │ -
200
│ │ │ │ -
201 // >>>>>>>>>>>>>> Do dense elimination step >>>>>>>>>>>>>>>>>>>>>>>>>>>>>
│ │ │ │ -
202 auto eliminationResult = eliminationFunction_(gatheredFactors, node->orderedFrontalKeys);
│ │ │ │ -
203 // <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
│ │ │ │ -
204
│ │ │ │ -
205 // Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the
│ │ │ │ -
206 // remaining factor
│ │ │ │ -
207 myData.bayesTreeNode->setEliminationResult(eliminationResult);
│ │ │ │ +
149 }
│ │ │ │ +
150
│ │ │ │ +
151 /* ************************************************************************* */
│ │ │ │ +
152 template<class FACTORGRAPH>
│ │ │ │ +
153 std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>, boost::shared_ptr<FACTORGRAPH> >
│ │ │ │ +
│ │ │ │ +
154 EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialSequential(
│ │ │ │ +
155 const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
│ │ │ │ +
156 {
│ │ │ │ +
│ │ │ │ +
157 if(variableIndex) {
│ │ │ │ +
158 gttic(eliminatePartialSequential);
│ │ │ │ +
159 // Do elimination
│ │ │ │ +
160 EliminationTreeType etree(asDerived(), *variableIndex, ordering);
│ │ │ │ +
161 return etree.eliminate(function);
│ │ │ │ +
162 } else {
│ │ │ │ +
163 // If no variable index is provided, compute one and call this function again
│ │ │ │ +
164 VariableIndex computedVariableIndex(asDerived());
│ │ │ │ +
165 return eliminatePartialSequential(ordering, function, computedVariableIndex);
│ │ │ │ +
166 }
│ │ │ │ +
167 }
│ │ │ │ +
168
│ │ │ │ +
169 /* ************************************************************************* */
│ │ │ │ +
170 template<class FACTORGRAPH>
│ │ │ │ +
│ │ │ │ +
171 std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>, boost::shared_ptr<FACTORGRAPH> >
│ │ │ │ +
│ │ │ │ +
172 EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialSequential(
│ │ │ │ +
173 const KeyVector& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const
│ │ │ │ +
174 {
│ │ │ │ +
175 if(variableIndex) {
│ │ │ │ +
176 gttic(eliminatePartialSequential);
│ │ │ │ +
177 // Compute full ordering
│ │ │ │ +
178 Ordering fullOrdering = Ordering::ColamdConstrainedFirst(*variableIndex, variables);
│ │ │ │ +
179
│ │ │ │ +
180 // Split off the part of the ordering for the variables being eliminated
│ │ │ │ +
│ │ │ │ +
181 Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size());
│ │ │ │ +
182 return eliminatePartialSequential(ordering, function, variableIndex);
│ │ │ │ +
183 } else {
│ │ │ │ +
184 // If no variable index is provided, compute one and call this function again
│ │ │ │ +
185 VariableIndex computedVariableIndex(asDerived());
│ │ │ │ +
186 return eliminatePartialSequential(variables, function, computedVariableIndex);
│ │ │ │ +
187 }
│ │ │ │ +
188 }
│ │ │ │ +
189
│ │ │ │ +
190 /* ************************************************************************* */
│ │ │ │ +
│ │ │ │ +
191 template<class FACTORGRAPH>
│ │ │ │ +
192 std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>, boost::shared_ptr<FACTORGRAPH> >
│ │ │ │ +
│ │ │ │ +
193 EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialMultifrontal(
│ │ │ │ +
194 const Ordering& ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const
│ │ │ │ +
195 {
│ │ │ │ +
196 if(variableIndex) {
│ │ │ │ +
197 gttic(eliminatePartialMultifrontal);
│ │ │ │ +
198 // Do elimination
│ │ │ │ +
199 EliminationTreeType etree(asDerived(), *variableIndex, ordering);
│ │ │ │ +
200 JunctionTreeType junctionTree(etree);
│ │ │ │ +
│ │ │ │ +
201 return junctionTree.eliminate(function);
│ │ │ │ +
202 } else {
│ │ │ │ +
203 // If no variable index is provided, compute one and call this function again
│ │ │ │ +
204 VariableIndex computedVariableIndex(asDerived());
│ │ │ │ +
205 return eliminatePartialMultifrontal(ordering, function, computedVariableIndex);
│ │ │ │ +
206 }
│ │ │ │ +
207 }
│ │ │ │ +
│ │ │ │ +
│ │ │ │
208
│ │ │ │ -
209 // Fill nodes index - we do this here instead of calling insertRoot at the end to avoid
│ │ │ │ -
210 // putting orphan subtrees in the index - they'll already be in the index of the ISAM2
│ │ │ │ -
211 // object they're added to.
│ │ │ │ -
212 for (const Key& j: myData.bayesTreeNode->conditional()->frontals())
│ │ │ │ -
213 nodesIndex_.insert(std::make_pair(j, myData.bayesTreeNode));
│ │ │ │ -
214
│ │ │ │ -
215 // Store remaining factor in parent's gathered factors
│ │ │ │ -
216 if (!eliminationResult.second->empty()) {
│ │ │ │ -
217#ifdef GTSAM_USE_TBB
│ │ │ │ -
218 myData.parentData->writeLock->lock();
│ │ │ │ -
219#endif
│ │ │ │ -
220 myData.parentData->childFactors[myData.myIndexInParent] = eliminationResult.second;
│ │ │ │ -
221#ifdef GTSAM_USE_TBB
│ │ │ │ -
│ │ │ │ -
222 myData.parentData->writeLock->unlock();
│ │ │ │ -
223#endif
│ │ │ │ -
224 }
│ │ │ │ -
225 }
│ │ │ │ -
226 };
│ │ │ │ -
227};
│ │ │ │ -
228
│ │ │ │ -
229/* ************************************************************************* */
│ │ │ │ -
230template<class BAYESTREE, class GRAPH>
│ │ │ │ -
│ │ │ │ -
231EliminatableClusterTree<BAYESTREE, GRAPH>& EliminatableClusterTree<BAYESTREE, GRAPH>::operator=(
│ │ │ │ -
232 const This& other) {
│ │ │ │ -
233 ClusterTree<GRAPH>::operator=(other);
│ │ │ │ -
234
│ │ │ │ -
235 // Assign the remaining factors - these are pointers to factors in the original factor graph and
│ │ │ │ -
236 // we do not clone them.
│ │ │ │ -
237 remainingFactors_ = other.remainingFactors_;
│ │ │ │ -
238
│ │ │ │ -
239 return *this;
│ │ │ │ -
240}
│ │ │ │ -
│ │ │ │ -
241
│ │ │ │ -
242/* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
243template <class BAYESTREE, class GRAPH>
│ │ │ │ -
244std::pair<boost::shared_ptr<BAYESTREE>, boost::shared_ptr<GRAPH> >
│ │ │ │ -
│ │ │ │ -
245EliminatableClusterTree<BAYESTREE, GRAPH>::eliminate(const Eliminate& function) const {
│ │ │ │ -
246 gttic(ClusterTree_eliminate);
│ │ │ │ -
247 // Do elimination (depth-first traversal). The rootsContainer stores a 'dummy' BayesTree node
│ │ │ │ -
248 // that contains all of the roots as its children. rootsContainer also stores the remaining
│ │ │ │ -
249 // un-eliminated factors passed up from the roots.
│ │ │ │ -
250 boost::shared_ptr<BayesTreeType> result = boost::make_shared<BayesTreeType>();
│ │ │ │ +
209 /* ************************************************************************* */
│ │ │ │ +
210 template<class FACTORGRAPH>
│ │ │ │ +
│ │ │ │ +
211 std::pair<boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>, boost::shared_ptr<FACTORGRAPH> >
│ │ │ │ +
│ │ │ │ +
212 EliminateableFactorGraph<FACTORGRAPH>::eliminatePartialMultifrontal(
│ │ │ │ +
213 const KeyVector& variables, const Eliminate& function, OptionalVariableIndex variableIndex) const
│ │ │ │ +
214 {
│ │ │ │ +
215 if(variableIndex) {
│ │ │ │ +
216 gttic(eliminatePartialMultifrontal);
│ │ │ │ +
217 // Compute full ordering
│ │ │ │ +
218 Ordering fullOrdering = Ordering::ColamdConstrainedFirst(*variableIndex, variables);
│ │ │ │ +
219
│ │ │ │ +
220 // Split off the part of the ordering for the variables being eliminated
│ │ │ │ +
221 Ordering ordering(fullOrdering.begin(), fullOrdering.begin() + variables.size());
│ │ │ │ +
222 return eliminatePartialMultifrontal(ordering, function, variableIndex);
│ │ │ │ +
223 } else {
│ │ │ │ +
224 // If no variable index is provided, compute one and call this function again
│ │ │ │ +
│ │ │ │ +
225 VariableIndex computedVariableIndex(asDerived());
│ │ │ │ +
226 return eliminatePartialMultifrontal(variables, function, computedVariableIndex);
│ │ │ │ +
227 }
│ │ │ │ +
228 }
│ │ │ │ +
229
│ │ │ │ +
230 /* ************************************************************************* */
│ │ │ │ +
231 template<class FACTORGRAPH>
│ │ │ │ +
232 boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
│ │ │ │ +
│ │ │ │ +
233 EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet(
│ │ │ │ +
234 boost::variant<const Ordering&, const KeyVector&> variables,
│ │ │ │ +
235 const Eliminate& function, OptionalVariableIndex variableIndex) const
│ │ │ │ +
236 {
│ │ │ │ +
237 if(!variableIndex) {
│ │ │ │ +
238 // If no variable index is provided, compute one and call this function again
│ │ │ │ +
239 VariableIndex index(asDerived());
│ │ │ │ +
│ │ │ │ +
240 return marginalMultifrontalBayesNet(variables, function, index);
│ │ │ │ +
241 } else {
│ │ │ │ +
242 // No ordering was provided for the marginalized variables, so order them using constrained
│ │ │ │ +
243 // COLAMD.
│ │ │ │ +
244 bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0);
│ │ │ │ +
245 const KeyVector* variablesOrOrdering =
│ │ │ │ +
246 unmarginalizedAreOrdered ?
│ │ │ │ +
247 boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&variables);
│ │ │ │ +
248
│ │ │ │ +
249 Ordering totalOrdering =
│ │ │ │ +
250 Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
│ │ │ │
251
│ │ │ │ -
252 typedef EliminationData<This> Data;
│ │ │ │ -
253 Data rootsContainer(0, this->nrRoots());
│ │ │ │ -
254
│ │ │ │ -
255 typename Data::EliminationPostOrderVisitor visitorPost(function, result->nodes_);
│ │ │ │ -
256 {
│ │ │ │ -
257 TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP
│ │ │ │ -
258 treeTraversal::DepthFirstForestParallel(*this, rootsContainer, Data::EliminationPreOrderVisitor,
│ │ │ │ -
259 visitorPost, 10);
│ │ │ │ +
252 // Split up ordering
│ │ │ │ +
253 const size_t nVars = variablesOrOrdering->size();
│ │ │ │ +
254 Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars);
│ │ │ │ +
│ │ │ │ +
255 Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
│ │ │ │ +
256
│ │ │ │ +
257 // Call this function again with the computed orderings
│ │ │ │ +
258 return marginalMultifrontalBayesNet(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex);
│ │ │ │ +
259 }
│ │ │ │
260 }
│ │ │ │
261
│ │ │ │ -
262 // Create BayesTree from roots stored in the dummy BayesTree node.
│ │ │ │ -
263 result->roots_.insert(result->roots_.end(), rootsContainer.bayesTreeNode->children.begin(),
│ │ │ │ -
264 rootsContainer.bayesTreeNode->children.end());
│ │ │ │ -
265
│ │ │ │ -
266 // Add remaining factors that were not involved with eliminated variables
│ │ │ │ -
267 boost::shared_ptr<FactorGraphType> remaining = boost::make_shared<FactorGraphType>();
│ │ │ │ -
268 remaining->reserve(remainingFactors_.size() + rootsContainer.childFactors.size());
│ │ │ │ -
269 remaining->push_back(remainingFactors_.begin(), remainingFactors_.end());
│ │ │ │ -
270 for (const sharedFactor& factor : rootsContainer.childFactors) {
│ │ │ │ -
271 if (factor)
│ │ │ │ -
272 remaining->push_back(factor);
│ │ │ │ -
273 }
│ │ │ │ -
274
│ │ │ │ -
275 // Return result
│ │ │ │ -
276 return std::make_pair(result, remaining);
│ │ │ │ -
277}
│ │ │ │ +
262 /* ************************************************************************* */
│ │ │ │ +
263 template<class FACTORGRAPH>
│ │ │ │ +
264 boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesNetType>
│ │ │ │ +
│ │ │ │ +
265 EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesNet(
│ │ │ │ +
266 boost::variant<const Ordering&, const KeyVector&> variables,
│ │ │ │ +
267 const Ordering& marginalizedVariableOrdering,
│ │ │ │ +
268 const Eliminate& function, OptionalVariableIndex variableIndex) const
│ │ │ │ +
269 {
│ │ │ │ +
│ │ │ │ +
270 if(!variableIndex) {
│ │ │ │ +
271 // If no variable index is provided, compute one and call this function again
│ │ │ │ +
272 VariableIndex index(asDerived());
│ │ │ │ +
273 return marginalMultifrontalBayesNet(variables, marginalizedVariableOrdering, function, index);
│ │ │ │ +
274 } else {
│ │ │ │ +
275 gttic(marginalMultifrontalBayesNet);
│ │ │ │ +
276 // An ordering was provided for the marginalized variables, so we can first eliminate them
│ │ │ │ +
│ │ │ │ +
277 // in the order requested.
│ │ │ │ +
278 boost::shared_ptr<BayesTreeType> bayesTree;
│ │ │ │ +
279 boost::shared_ptr<FactorGraphType> factorGraph;
│ │ │ │ +
280 boost::tie(bayesTree,factorGraph) =
│ │ │ │ +
281 eliminatePartialMultifrontal(marginalizedVariableOrdering, function, *variableIndex);
│ │ │ │ +
282
│ │ │ │ +
283 if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables))
│ │ │ │ +
284 {
│ │ │ │ +
285 // An ordering was also provided for the unmarginalized variables, so we can also
│ │ │ │ +
286 // eliminate them in the order requested.
│ │ │ │ +
287 return factorGraph->eliminateSequential(*varsAsOrdering, function);
│ │ │ │ +
288 }
│ │ │ │ +
289 else
│ │ │ │ +
290 {
│ │ │ │ +
291 // No ordering was provided for the unmarginalized variables, so order them with COLAMD.
│ │ │ │ +
292 return factorGraph->eliminateSequential(Ordering::COLAMD, function);
│ │ │ │ +
293 }
│ │ │ │ +
294 }
│ │ │ │ +
295 }
│ │ │ │ +
296
│ │ │ │ +
297 /* ************************************************************************* */
│ │ │ │ +
298 template<class FACTORGRAPH>
│ │ │ │ +
299 boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
│ │ │ │ +
│ │ │ │ +
300 EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree(
│ │ │ │ +
301 boost::variant<const Ordering&, const KeyVector&> variables,
│ │ │ │ +
302 const Eliminate& function, OptionalVariableIndex variableIndex) const
│ │ │ │ +
303 {
│ │ │ │ +
304 if(!variableIndex) {
│ │ │ │ +
305 // If no variable index is provided, compute one and call this function again
│ │ │ │ +
306 VariableIndex computedVariableIndex(asDerived());
│ │ │ │ +
307 return marginalMultifrontalBayesTree(variables, function, computedVariableIndex);
│ │ │ │ +
308 } else {
│ │ │ │ +
309 // No ordering was provided for the marginalized variables, so order them using constrained
│ │ │ │ +
310 // COLAMD.
│ │ │ │ +
311 bool unmarginalizedAreOrdered = (boost::get<const Ordering&>(&variables) != 0);
│ │ │ │ +
312 const KeyVector* variablesOrOrdering =
│ │ │ │ +
313 unmarginalizedAreOrdered ?
│ │ │ │ +
314 boost::get<const Ordering&>(&variables) : boost::get<const KeyVector&>(&variables);
│ │ │ │ +
315
│ │ │ │ +
316 Ordering totalOrdering =
│ │ │ │ +
317 Ordering::ColamdConstrainedLast(*variableIndex, *variablesOrOrdering, unmarginalizedAreOrdered);
│ │ │ │ +
318
│ │ │ │ +
319 // Split up ordering
│ │ │ │ +
320 const size_t nVars = variablesOrOrdering->size();
│ │ │ │ +
321 Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - nVars);
│ │ │ │ +
322 Ordering marginalVarsOrdering(totalOrdering.end() - nVars, totalOrdering.end());
│ │ │ │ +
323
│ │ │ │ +
324 // Call this function again with the computed orderings
│ │ │ │ +
325 return marginalMultifrontalBayesTree(marginalVarsOrdering, marginalizationOrdering, function, *variableIndex);
│ │ │ │ +
326 }
│ │ │ │ +
327 }
│ │ │ │ +
│ │ │ │ +
328
│ │ │ │ +
329 /* ************************************************************************* */
│ │ │ │ +
330 template<class FACTORGRAPH>
│ │ │ │ +
331 boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
│ │ │ │ +
│ │ │ │ +
332 EliminateableFactorGraph<FACTORGRAPH>::marginalMultifrontalBayesTree(
│ │ │ │ +
333 boost::variant<const Ordering&, const KeyVector&> variables,
│ │ │ │ +
334 const Ordering& marginalizedVariableOrdering,
│ │ │ │ +
335 const Eliminate& function, OptionalVariableIndex variableIndex) const
│ │ │ │ +
336 {
│ │ │ │ +
337 if(!variableIndex) {
│ │ │ │ +
338 // If no variable index is provided, compute one and call this function again
│ │ │ │ +
339 VariableIndex computedVariableIndex(asDerived());
│ │ │ │ +
340 return marginalMultifrontalBayesTree(variables, marginalizedVariableOrdering, function, computedVariableIndex);
│ │ │ │ +
341 } else {
│ │ │ │ +
342 gttic(marginalMultifrontalBayesTree);
│ │ │ │ +
343 // An ordering was provided for the marginalized variables, so we can first eliminate them
│ │ │ │ +
344 // in the order requested.
│ │ │ │ +
345 boost::shared_ptr<BayesTreeType> bayesTree;
│ │ │ │ +
346 boost::shared_ptr<FactorGraphType> factorGraph;
│ │ │ │ +
347 boost::tie(bayesTree,factorGraph) =
│ │ │ │ +
348 eliminatePartialMultifrontal(marginalizedVariableOrdering, function, *variableIndex);
│ │ │ │ +
349
│ │ │ │ +
350 if(const Ordering* varsAsOrdering = boost::get<const Ordering&>(&variables))
│ │ │ │ +
351 {
│ │ │ │ +
352 // An ordering was also provided for the unmarginalized variables, so we can also
│ │ │ │ +
353 // eliminate them in the order requested.
│ │ │ │ +
354 return factorGraph->eliminateMultifrontal(*varsAsOrdering, function);
│ │ │ │ +
355 }
│ │ │ │ +
356 else
│ │ │ │ +
357 {
│ │ │ │ +
358 // No ordering was provided for the unmarginalized variables, so order them with COLAMD.
│ │ │ │ +
359 return factorGraph->eliminateMultifrontal(Ordering::COLAMD, function);
│ │ │ │ +
360 }
│ │ │ │ +
361 }
│ │ │ │ +
362 }
│ │ │ │ +
│ │ │ │ +
363
│ │ │ │ +
364 /* ************************************************************************* */
│ │ │ │ +
365 template<class FACTORGRAPH>
│ │ │ │ +
366 boost::shared_ptr<FACTORGRAPH>
│ │ │ │ +
│ │ │ │ +
367 EliminateableFactorGraph<FACTORGRAPH>::marginal(
│ │ │ │ +
368 const KeyVector& variables,
│ │ │ │ +
369 const Eliminate& function, OptionalVariableIndex variableIndex) const
│ │ │ │ +
370 {
│ │ │ │ +
371 if(variableIndex)
│ │ │ │ +
372 {
│ │ │ │ +
373 // Compute a total ordering for all variables
│ │ │ │ +
374 Ordering totalOrdering = Ordering::ColamdConstrainedLast(*variableIndex, variables);
│ │ │ │ +
375
│ │ │ │ +
376 // Split out the part for the marginalized variables
│ │ │ │ +
377 Ordering marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() - variables.size());
│ │ │ │ +
378
│ │ │ │ +
379 // Eliminate and return the remaining factor graph
│ │ │ │ +
380 return eliminatePartialMultifrontal(marginalizationOrdering, function, *variableIndex).second;
│ │ │ │ +
381 }
│ │ │ │ +
382 else
│ │ │ │ +
383 {
│ │ │ │ +
384 // If no variable index is provided, compute one and call this function again
│ │ │ │ +
385 VariableIndex computedVariableIndex(asDerived());
│ │ │ │ +
386 return marginal(variables, function, computedVariableIndex);
│ │ │ │ +
387 }
│ │ │ │ +
388 }
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
389
│ │ │ │ +
390
│ │ │ │ +
391}
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │
│ │ │ │ -
278
│ │ │ │ -
279} // namespace gtsam
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -
timing.h
Timing utilities.
│ │ │ │ -
BayesTree.h
Bayes Tree is a tree of cliques of a Bayes Chain.
│ │ │ │ -
ClusterTree.h
Collects factorgraph fragments defined on variable clusters, arranged in a tree.
│ │ │ │ -
Ordering.h
Variable ordering for the elimination algorithm.
│ │ │ │ -
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │ +
inferenceExceptions.h
Exceptions that may be thrown by inference algorithms.
│ │ │ │ +
EliminateableFactorGraph.h
Variable elimination algorithms for factor graphs.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::PrintKeyVector
void PrintKeyVector(const KeyVector &keys, const string &s, const KeyFormatter &keyFormatter)
Utility function to print sets of keys with optional prefix.
Definition Key.cpp:77
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::treeTraversal::CloneForest
FastVector< boost::shared_ptr< typename FOREST::Node > > CloneForest(const FOREST &forest)
Clone a tree, copy-constructing new nodes (calling boost::make_shared) and setting up child pointers ...
Definition treeTraversal-inst.h:189
│ │ │ │ -
gtsam::treeTraversal::PrintForest
void PrintForest(const FOREST &forest, std::string str, const KeyFormatter &keyFormatter)
Print a tree, prefixing each line with str, and formatting keys using keyFormatter.
Definition treeTraversal-inst.h:219
│ │ │ │ -
gtsam::treeTraversal::DepthFirstForestParallel
void DepthFirstForestParallel(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, VISITOR_POST &visitorPost, int problemSizeThreshold=10)
Traverse a forest depth-first with pre-order and post-order visits.
Definition treeTraversal-inst.h:154
│ │ │ │ -
gtsam::TbbOpenMPMixedScope
An object whose scope defines a block where TBB and OpenMP parallelism are mixed.
Definition types.h:192
│ │ │ │ -
gtsam::EliminatableClusterTree
A cluster-tree that eliminates to a Bayes tree.
Definition ClusterTree.h:184
│ │ │ │ -
gtsam::EliminatableClusterTree::operator=
This & operator=(const This &other)
Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are copie...
Definition ClusterTree-inst.h:231
│ │ │ │ -
gtsam::EliminatableClusterTree::eliminate
std::pair< boost::shared_ptr< BayesTreeType >, boost::shared_ptr< FactorGraphType > > eliminate(const Eliminate &function) const
Eliminate the factors to a Bayes tree and remaining factor graph.
Definition ClusterTree-inst.h:245
│ │ │ │ -
gtsam::EliminatableClusterTree::sharedFactor
boost::shared_ptr< FactorType > sharedFactor
Shared pointer to a factor.
Definition ClusterTree.h:197
│ │ │ │ -
gtsam::EliminatableClusterTree::Eliminate
GRAPH::Eliminate Eliminate
Typedef for an eliminate subroutine.
Definition ClusterTree.h:195
│ │ │ │ -
gtsam::BayesTreeOrphanWrapper
Definition BayesTree.h:276
│ │ │ │ -
gtsam::EliminationData
Definition ClusterTree-inst.h:115
│ │ │ │ - │ │ │ │ -
gtsam::ClusterTree
A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman: each node k re...
Definition ClusterTree.h:25
│ │ │ │ -
gtsam::ClusterTree::operator=
This & operator=(const This &other)
Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are copie...
Definition ClusterTree-inst.h:104
│ │ │ │ -
gtsam::ClusterTree::roots_
FastVector< sharedNode > roots_
concept check
Definition ClusterTree.h:116
│ │ │ │ -
gtsam::ClusterTree::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Print the cluster tree.
Definition ClusterTree-inst.h:98
│ │ │ │ -
gtsam::ClusterTree< DiscreteFactorGraph >::sharedFactor
boost::shared_ptr< FactorType > sharedFactor
Shared pointer to a factor.
Definition ClusterTree.h:32
│ │ │ │ -
gtsam::ClusterTree::Cluster::print
virtual void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print this node
Definition ClusterTree-inst.h:26
│ │ │ │ -
gtsam::ClusterTree::Cluster::orderedFrontalKeys
Keys orderedFrontalKeys
Frontal keys of this node.
Definition ClusterTree.h:41
│ │ │ │ -
gtsam::ClusterTree::Cluster::mergeChildren
void mergeChildren(const std::vector< bool > &merge)
Merge all children for which bit is set into this node.
Definition ClusterTree-inst.h:56
│ │ │ │ -
gtsam::ClusterTree::Cluster::merge
void merge(const boost::shared_ptr< Cluster > &cluster)
Merge in given cluster.
Definition ClusterTree-inst.h:44
│ │ │ │ -
gtsam::ClusterTree::Cluster::nrFrontalsOfChildren
std::vector< size_t > nrFrontalsOfChildren() const
Return a vector with nrFrontal keys for each child.
Definition ClusterTree-inst.h:34
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::EliminateableFactorGraph::marginalMultifrontalBayesTree
boost::shared_ptr< BayesTreeType > marginalMultifrontalBayesTree(boost::variant< const Ordering &, const KeyVector & > variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Compute the marginal of the requested variables and return the result as a Bayes tree.
Definition EliminateableFactorGraph-inst.h:300
│ │ │ │ +
gtsam::EliminateableFactorGraph< DiscreteFactorGraph >::JunctionTreeType
EliminationTraitsType::JunctionTreeType JunctionTreeType
Junction tree type that can do multifrontal elimination of this graph.
Definition EliminateableFactorGraph.h:82
│ │ │ │ +
gtsam::EliminateableFactorGraph::marginal
boost::shared_ptr< FactorGraphType > marginal(const KeyVector &variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Compute the marginal factor graph of the requested variables.
Definition EliminateableFactorGraph-inst.h:367
│ │ │ │ +
gtsam::EliminateableFactorGraph::marginalMultifrontalBayesNet
boost::shared_ptr< BayesNetType > marginalMultifrontalBayesNet(boost::variant< const Ordering &, const KeyVector & > variables, const Ordering &marginalizedVariableOrdering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Compute the marginal of the requested variables and return the result as a Bayes net.
Definition EliminateableFactorGraph-inst.h:265
│ │ │ │ +
gtsam::EliminateableFactorGraph::Eliminate
std::function< EliminationResult(const FactorGraphType &, const Ordering &)> Eliminate
The function type that does a single dense elimination step on a subgraph.
Definition EliminateableFactorGraph.h:89
│ │ │ │ +
gtsam::EliminateableFactorGraph::BayesTreeType
EliminationTraitsType::BayesTreeType BayesTreeType
Bayes tree type produced by multifrontal elimination.
Definition EliminateableFactorGraph.h:79
│ │ │ │ +
gtsam::EliminateableFactorGraph::eliminateSequential
boost::shared_ptr< BayesNetType > eliminateSequential(OptionalOrderingType orderingType=boost::none, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Do sequential elimination of all variables to produce a Bayes net.
Definition EliminateableFactorGraph-inst.h:30
│ │ │ │ +
gtsam::EliminateableFactorGraph::eliminateMultifrontal
boost::shared_ptr< BayesTreeType > eliminateMultifrontal(OptionalOrderingType orderingType=boost::none, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Do multifrontal elimination of all variables to produce a Bayes tree.
Definition EliminateableFactorGraph-inst.h:91
│ │ │ │ +
gtsam::EliminateableFactorGraph::EliminationTreeType
EliminationTraitsType::EliminationTreeType EliminationTreeType
Elimination tree type that can do sequential elimination of this graph.
Definition EliminateableFactorGraph.h:76
│ │ │ │ +
gtsam::EliminateableFactorGraph::OptionalVariableIndex
boost::optional< const VariableIndex & > OptionalVariableIndex
Typedef for an optional variable index as an argument to elimination functions.
Definition EliminateableFactorGraph.h:92
│ │ │ │ +
gtsam::EliminateableFactorGraph::eliminatePartialSequential
std::pair< boost::shared_ptr< BayesNetType >, boost::shared_ptr< FactorGraphType > > eliminatePartialSequential(const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Do sequential elimination of some variables, in ordering provided, to produce a Bayes net and a remai...
Definition EliminateableFactorGraph-inst.h:154
│ │ │ │ +
gtsam::EliminateableFactorGraph::marginalMultifrontalBayesNet
boost::shared_ptr< BayesNetType > marginalMultifrontalBayesNet(boost::variant< const Ordering &, const KeyVector & > variables, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Compute the marginal of the requested variables and return the result as a Bayes net.
Definition EliminateableFactorGraph-inst.h:233
│ │ │ │ +
gtsam::EliminateableFactorGraph::eliminatePartialMultifrontal
std::pair< boost::shared_ptr< BayesTreeType >, boost::shared_ptr< FactorGraphType > > eliminatePartialMultifrontal(const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Do multifrontal elimination of some variables, in ordering provided, to produce a Bayes tree and a re...
Definition EliminateableFactorGraph-inst.h:193
│ │ │ │ +
gtsam::EliminateableFactorGraph::OptionalOrderingType
boost::optional< Ordering::OrderingType > OptionalOrderingType
Typedef for an optional ordering type.
Definition EliminateableFactorGraph.h:95
│ │ │ │ +
gtsam::InconsistentEliminationRequested
An inference algorithm was called with inconsistent arguments.
Definition inferenceExceptions.h:29
│ │ │ │ +
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
gtsam::Ordering::Natural
static Ordering Natural(const FACTOR_GRAPH &fg)
Return a natural Ordering. Typically used by iterative solvers.
Definition Ordering.h:190
│ │ │ │ +
gtsam::Ordering::Colamd
static Ordering Colamd(const FACTOR_GRAPH &graph)
Compute a fill-reducing ordering using COLAMD from a factor graph (see details for note on performanc...
Definition Ordering.h:95
│ │ │ │ +
gtsam::Ordering::ColamdConstrainedLast
static Ordering ColamdConstrainedLast(const FACTOR_GRAPH &graph, const KeyVector &constrainLast, bool forceOrder=false)
Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details for note o...
Definition Ordering.h:114
│ │ │ │ +
gtsam::Ordering::ColamdConstrainedFirst
static Ordering ColamdConstrainedFirst(const FACTOR_GRAPH &graph, const KeyVector &constrainFirst, bool forceOrder=false)
Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details for note o...
Definition Ordering.h:141
│ │ │ │ +
gtsam::Ordering::Metis
static GTSAM_EXPORT Ordering Metis(const MetisIndex &met)
Compute an ordering determined by METIS from a VariableIndex.
Definition Ordering.cpp:212
│ │ │ │ +
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition VariableIndex.h:43
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,439 +1,609 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ClusterTree-inst.h │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -1 │ │ │ │ │ -10#pragma once │ │ │ │ │ +EliminateableFactorGraph-inst.h │ │ │ │ │ +1/* --------------------------------------------------------------------------- │ │ │ │ │ +- │ │ │ │ │ +2 │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +5 * All Rights Reserved │ │ │ │ │ +6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +7 │ │ │ │ │ +8 * See LICENSE for the license information │ │ │ │ │ +9 │ │ │ │ │ +10 * ------------------------------------------------------------------------- │ │ │ │ │ +- */ │ │ │ │ │ 11 │ │ │ │ │ -12#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_C_l_u_s_t_e_r_T_r_e_e_._h> │ │ │ │ │ -13#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ -14#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_O_r_d_e_r_i_n_g_._h> │ │ │ │ │ -15#include <_g_t_s_a_m_/_b_a_s_e_/_t_i_m_i_n_g_._h> │ │ │ │ │ -16#include <_g_t_s_a_m_/_b_a_s_e_/_t_r_e_e_T_r_a_v_e_r_s_a_l_-_i_n_s_t_._h> │ │ │ │ │ -17 │ │ │ │ │ -18#ifdef GTSAM_USE_TBB │ │ │ │ │ -19#include │ │ │ │ │ -20#endif │ │ │ │ │ -21 │ │ │ │ │ -22namespace _g_t_s_a_m { │ │ │ │ │ -23 │ │ │ │ │ -24/* ************************************************************************* │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_i_n_f_e_r_e_n_c_e_E_x_c_e_p_t_i_o_n_s_._h> │ │ │ │ │ +23#include │ │ │ │ │ +24 │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ +26 │ │ │ │ │ +27 /* ************************************************************************* │ │ │ │ │ */ │ │ │ │ │ -25template │ │ │ │ │ -_2_6void _C_l_u_s_t_e_r_T_r_e_e_<_G_R_A_P_H_>_:_:_C_l_u_s_t_e_r_:_:_p_r_i_n_t(const std::string& s, │ │ │ │ │ -27 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter) const { │ │ │ │ │ -28 std::cout << s << " (" << problemSize_ << ")"; │ │ │ │ │ -29 _P_r_i_n_t_K_e_y_V_e_c_t_o_r(_o_r_d_e_r_e_d_F_r_o_n_t_a_l_K_e_y_s); │ │ │ │ │ -30} │ │ │ │ │ -31 │ │ │ │ │ -32/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -33template │ │ │ │ │ -_3_4std::vector _C_l_u_s_t_e_r_T_r_e_e_<_G_R_A_P_H_>_:_:_C_l_u_s_t_e_r_:_:_n_r_F_r_o_n_t_a_l_s_O_f_C_h_i_l_d_r_e_n() const │ │ │ │ │ -{ │ │ │ │ │ -35 std::vector nrFrontals; │ │ │ │ │ -36 nrFrontals.reserve(nrChildren()); │ │ │ │ │ -37 for (const sharedNode& child : children) │ │ │ │ │ -38 nrFrontals.push_back(child->nrFrontals()); │ │ │ │ │ -39 return nrFrontals; │ │ │ │ │ -40} │ │ │ │ │ -41 │ │ │ │ │ -42/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -43template │ │ │ │ │ -_4_4void _C_l_u_s_t_e_r_T_r_e_e_<_G_R_A_P_H_>_:_:_C_l_u_s_t_e_r_:_:_m_e_r_g_e(const boost::shared_ptr& │ │ │ │ │ -cluster) { │ │ │ │ │ -45 // Merge keys. For efficiency, we add keys in reverse order at end, calling │ │ │ │ │ -reverse after.. │ │ │ │ │ -46 orderedFrontalKeys.insert(orderedFrontalKeys.end(), cluster- │ │ │ │ │ ->orderedFrontalKeys.rbegin(), │ │ │ │ │ -47 cluster->orderedFrontalKeys.rend()); │ │ │ │ │ -48 factors.push_back(cluster->factors); │ │ │ │ │ -49 children.insert(children.end(), cluster->children.begin(), cluster- │ │ │ │ │ ->children.end()); │ │ │ │ │ -50 // Increment problem size │ │ │ │ │ -51 problemSize_ = std::max(problemSize_, cluster->problemSize_); │ │ │ │ │ -52} │ │ │ │ │ -53 │ │ │ │ │ -54/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -55template │ │ │ │ │ -_5_6void _C_l_u_s_t_e_r_T_r_e_e_<_G_R_A_P_H_>_:_:_C_l_u_s_t_e_r_:_:_m_e_r_g_e_C_h_i_l_d_r_e_n( │ │ │ │ │ -57 const std::vector& merge) { │ │ │ │ │ -58 gttic(Cluster_mergeChildren); │ │ │ │ │ -59 assert(merge.size() == this->children.size()); │ │ │ │ │ +28 template │ │ │ │ │ +29 boost::shared_ptr:: │ │ │ │ │ +BayesNetType> │ │ │ │ │ +_3_0 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_G_R_A_P_H_>_:_:_e_l_i_m_i_n_a_t_e_S_e_q_u_e_n_t_i_a_l( │ │ │ │ │ +31 _O_p_t_i_o_n_a_l_O_r_d_e_r_i_n_g_T_y_p_e orderingType, const _E_l_i_m_i_n_a_t_e& function, │ │ │ │ │ +32 _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex) const { │ │ │ │ │ +33 if(!variableIndex) { │ │ │ │ │ +34 // If no VariableIndex provided, compute one and call this function again │ │ │ │ │ +IMPORTANT: we check │ │ │ │ │ +35 // for no variable index first so that it's always computed if we need to │ │ │ │ │ +call COLAMD because │ │ │ │ │ +36 // no Ordering is provided. When removing optional from VariableIndex, │ │ │ │ │ +create VariableIndex │ │ │ │ │ +37 // before creating ordering. │ │ │ │ │ +38 _V_a_r_i_a_b_l_e_I_n_d_e_x computedVariableIndex(asDerived()); │ │ │ │ │ +39 return eliminateSequential(orderingType, function, computedVariableIndex); │ │ │ │ │ +40 } │ │ │ │ │ +41 else { │ │ │ │ │ +42 // Compute an ordering and call this function again. We are guaranteed to │ │ │ │ │ +have a │ │ │ │ │ +43 // VariableIndex already here because we computed one if needed in the │ │ │ │ │ +previous 'if' block. │ │ │ │ │ +44 if (orderingType == Ordering::METIS) { │ │ │ │ │ +45 _O_r_d_e_r_i_n_g computedOrdering = _O_r_d_e_r_i_n_g_:_:_M_e_t_i_s(asDerived()); │ │ │ │ │ +46 return eliminateSequential(computedOrdering, function, variableIndex); │ │ │ │ │ +47 } else if (orderingType == Ordering::COLAMD) { │ │ │ │ │ +48 _O_r_d_e_r_i_n_g computedOrdering = _O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d(*variableIndex); │ │ │ │ │ +49 return eliminateSequential(computedOrdering, function, variableIndex); │ │ │ │ │ +50 } else if (orderingType == Ordering::NATURAL) { │ │ │ │ │ +51 _O_r_d_e_r_i_n_g computedOrdering = _O_r_d_e_r_i_n_g_:_:_N_a_t_u_r_a_l(asDerived()); │ │ │ │ │ +52 return eliminateSequential(computedOrdering, function, variableIndex); │ │ │ │ │ +53 } else { │ │ │ │ │ +54 _O_r_d_e_r_i_n_g computedOrdering = EliminationTraitsType::DefaultOrderingFunc( │ │ │ │ │ +55 asDerived(), variableIndex); │ │ │ │ │ +56 return eliminateSequential(computedOrdering, function, variableIndex); │ │ │ │ │ +57 } │ │ │ │ │ +58 } │ │ │ │ │ +59 } │ │ │ │ │ 60 │ │ │ │ │ -61 // Count how many keys, factors and children we'll end up with │ │ │ │ │ -62 size_t nrKeys = orderedFrontalKeys.size(); │ │ │ │ │ -63 size_t nrFactors = factors.size(); │ │ │ │ │ -64 size_t nrNewChildren = 0; │ │ │ │ │ -65 // Loop over children │ │ │ │ │ -66 size_t i = 0; │ │ │ │ │ -67 for(const sharedNode& child: this->children) { │ │ │ │ │ -68 if (merge[i]) { │ │ │ │ │ -69 nrKeys += child->orderedFrontalKeys.size(); │ │ │ │ │ -70 nrFactors += child->factors.size(); │ │ │ │ │ -71 nrNewChildren += child->nrChildren(); │ │ │ │ │ -72 } else { │ │ │ │ │ -73 nrNewChildren += 1; // we keep the child │ │ │ │ │ -74 } │ │ │ │ │ -75 ++i; │ │ │ │ │ -76 } │ │ │ │ │ -77 │ │ │ │ │ -78 // now reserve space, and really merge │ │ │ │ │ -79 auto oldChildren = this->children; │ │ │ │ │ -80 this->children.clear(); │ │ │ │ │ -81 this->children.reserve(nrNewChildren); │ │ │ │ │ -82 orderedFrontalKeys.reserve(nrKeys); │ │ │ │ │ -83 factors.reserve(nrFactors); │ │ │ │ │ -84 i = 0; │ │ │ │ │ -85 for (const sharedNode& child : oldChildren) { │ │ │ │ │ -86 if (merge[i]) { │ │ │ │ │ -87 this->merge(child); │ │ │ │ │ -88 } else { │ │ │ │ │ -89 this->addChild(child); // we keep the child │ │ │ │ │ -90 } │ │ │ │ │ -91 ++i; │ │ │ │ │ -92 } │ │ │ │ │ -_9_3 std::reverse(orderedFrontalKeys.begin(), orderedFrontalKeys.end()); │ │ │ │ │ -94} │ │ │ │ │ -95 │ │ │ │ │ -96/* ************************************************************************* │ │ │ │ │ +61 /* ************************************************************************* │ │ │ │ │ */ │ │ │ │ │ -_9_7template │ │ │ │ │ -_9_8void _C_l_u_s_t_e_r_T_r_e_e_<_G_R_A_P_H_>_:_:_p_r_i_n_t(const std::string& s, const _K_e_y_F_o_r_m_a_t_t_e_r& │ │ │ │ │ -keyFormatter) const { │ │ │ │ │ -99 _t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_P_r_i_n_t_F_o_r_e_s_t(*this, s, keyFormatter); │ │ │ │ │ -_1_0_0} │ │ │ │ │ -101 │ │ │ │ │ -102/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -_1_0_3template │ │ │ │ │ -_1_0_4_C_l_u_s_t_e_r_T_r_e_e_<_G_R_A_P_H_>& _C_l_u_s_t_e_r_T_r_e_e_<_G_R_A_P_H_>_:_:_o_p_e_r_a_t_o_r_=(const _T_h_i_s& other) { │ │ │ │ │ -105 // Start by duplicating the tree. │ │ │ │ │ -106 _r_o_o_t_s__ = _t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_C_l_o_n_e_F_o_r_e_s_t(other); │ │ │ │ │ -107 return *this; │ │ │ │ │ -108} │ │ │ │ │ -109 │ │ │ │ │ -110/* ************************************************************************* │ │ │ │ │ +62 template │ │ │ │ │ +63 boost::shared_ptr:: │ │ │ │ │ +BayesNetType> │ │ │ │ │ +_6_4 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_G_R_A_P_H_>_:_:_e_l_i_m_i_n_a_t_e_S_e_q_u_e_n_t_i_a_l( │ │ │ │ │ +65 const _O_r_d_e_r_i_n_g& ordering, const _E_l_i_m_i_n_a_t_e& function, │ │ │ │ │ +66 _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex) const │ │ │ │ │ +67 { │ │ │ │ │ +68 if(!variableIndex) { │ │ │ │ │ +69 // If no VariableIndex provided, compute one and call this function again │ │ │ │ │ +70 _V_a_r_i_a_b_l_e_I_n_d_e_x computedVariableIndex(asDerived()); │ │ │ │ │ +71 return eliminateSequential(ordering, function, computedVariableIndex); │ │ │ │ │ +72 } else { │ │ │ │ │ +73 gttic(eliminateSequential); │ │ │ │ │ +74 // Do elimination │ │ │ │ │ +75 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_T_y_p_e etree(asDerived(), *variableIndex, ordering); │ │ │ │ │ +76 boost::shared_ptr bayesNet; │ │ │ │ │ +77 boost::shared_ptr factorGraph; │ │ │ │ │ +78 boost::tie(bayesNet,factorGraph) = etree.eliminate(function); │ │ │ │ │ +79 // If any factors are remaining, the ordering was incomplete │ │ │ │ │ +80 if(!factorGraph->empty()) │ │ │ │ │ +81 throw _I_n_c_o_n_s_i_s_t_e_n_t_E_l_i_m_i_n_a_t_i_o_n_R_e_q_u_e_s_t_e_d(); │ │ │ │ │ +82 // Return the Bayes net │ │ │ │ │ +83 return bayesNet; │ │ │ │ │ +84 } │ │ │ │ │ +85 } │ │ │ │ │ +86 │ │ │ │ │ +87 /* ************************************************************************* │ │ │ │ │ */ │ │ │ │ │ -111// Elimination traversal data - stores a pointer to the parent data and │ │ │ │ │ -collects │ │ │ │ │ -112// the factors resulting from elimination of the children. Also sets up │ │ │ │ │ -BayesTree │ │ │ │ │ -113// cliques with parent and child pointers. │ │ │ │ │ -114template │ │ │ │ │ -_1_1_5struct _E_l_i_m_i_n_a_t_i_o_n_D_a_t_a { │ │ │ │ │ -116 // Typedefs │ │ │ │ │ -117 typedef typename CLUSTERTREE::sharedFactor sharedFactor; │ │ │ │ │ -118 typedef typename CLUSTERTREE::FactorType FactorType; │ │ │ │ │ -119 typedef typename CLUSTERTREE::FactorGraphType FactorGraphType; │ │ │ │ │ -120 typedef typename CLUSTERTREE::ConditionalType ConditionalType; │ │ │ │ │ -121 typedef typename CLUSTERTREE::BayesTreeType::Node BTNode; │ │ │ │ │ -122 │ │ │ │ │ -123 _E_l_i_m_i_n_a_t_i_o_n_D_a_t_a* const parentData; │ │ │ │ │ -124 size_t myIndexInParent; │ │ │ │ │ -125 _F_a_s_t_V_e_c_t_o_r_<_s_h_a_r_e_d_F_a_c_t_o_r_> childFactors; │ │ │ │ │ -126 boost::shared_ptr bayesTreeNode; │ │ │ │ │ -127#ifdef GTSAM_USE_TBB │ │ │ │ │ -128 boost::shared_ptr writeLock; │ │ │ │ │ -129#endif │ │ │ │ │ -130 │ │ │ │ │ -131 _E_l_i_m_i_n_a_t_i_o_n_D_a_t_a(_E_l_i_m_i_n_a_t_i_o_n_D_a_t_a* _parentData, size_t nChildren) : │ │ │ │ │ -132 parentData(_parentData), bayesTreeNode(boost::make_shared()) │ │ │ │ │ -133#ifdef GTSAM_USE_TBB │ │ │ │ │ -134 , writeLock(boost::make_shared()) │ │ │ │ │ -135#endif │ │ │ │ │ -136 { │ │ │ │ │ -137 if (parentData) { │ │ │ │ │ -_1_3_8#ifdef GTSAM_USE_TBB │ │ │ │ │ -139 parentData->writeLock->lock(); │ │ │ │ │ -140#endif │ │ │ │ │ -141 myIndexInParent = parentData->childFactors.size(); │ │ │ │ │ -142 parentData->childFactors.push_back(_s_h_a_r_e_d_F_a_c_t_o_r()); │ │ │ │ │ -143#ifdef GTSAM_USE_TBB │ │ │ │ │ -144 parentData->writeLock->unlock(); │ │ │ │ │ -145#endif │ │ │ │ │ -146 } else { │ │ │ │ │ -147 myIndexInParent = 0; │ │ │ │ │ +88 template │ │ │ │ │ +89 boost::shared_ptr< │ │ │ │ │ +90 typename _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_G_R_A_P_H_>_:_:_B_a_y_e_s_T_r_e_e_T_y_p_e> │ │ │ │ │ +_9_1 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_G_R_A_P_H_>_:_:_e_l_i_m_i_n_a_t_e_M_u_l_t_i_f_r_o_n_t_a_l( │ │ │ │ │ +92 _O_p_t_i_o_n_a_l_O_r_d_e_r_i_n_g_T_y_p_e orderingType, const _E_l_i_m_i_n_a_t_e& function, │ │ │ │ │ +93 _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex) const { │ │ │ │ │ +94 if (!variableIndex) { │ │ │ │ │ +95 // If no VariableIndex provided, compute one and call this function again │ │ │ │ │ +96 // IMPORTANT: we check for no variable index first so that it's always │ │ │ │ │ +97 // computed if we need to call COLAMD because no Ordering is provided. │ │ │ │ │ +98 // When removing optional from VariableIndex, create VariableIndex before │ │ │ │ │ +99 // creating ordering. │ │ │ │ │ +100 _V_a_r_i_a_b_l_e_I_n_d_e_x computedVariableIndex(asDerived()); │ │ │ │ │ +101 return eliminateMultifrontal(orderingType, function, │ │ │ │ │ +102 computedVariableIndex); │ │ │ │ │ +103 } else { │ │ │ │ │ +104 // Compute an ordering and call this function again. We are guaranteed to │ │ │ │ │ +105 // have a VariableIndex already here because we computed one if needed in │ │ │ │ │ +106 // the previous 'if' block. │ │ │ │ │ +107 if (orderingType == Ordering::METIS) { │ │ │ │ │ +108 _O_r_d_e_r_i_n_g computedOrdering = _O_r_d_e_r_i_n_g_:_:_M_e_t_i_s(asDerived()); │ │ │ │ │ +109 return eliminateMultifrontal(computedOrdering, function, variableIndex); │ │ │ │ │ +110 } else if (orderingType == Ordering::COLAMD) { │ │ │ │ │ +111 _O_r_d_e_r_i_n_g computedOrdering = _O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d(*variableIndex); │ │ │ │ │ +112 return eliminateMultifrontal(computedOrdering, function, variableIndex); │ │ │ │ │ +113 } else if (orderingType == Ordering::NATURAL) { │ │ │ │ │ +114 _O_r_d_e_r_i_n_g computedOrdering = _O_r_d_e_r_i_n_g_:_:_N_a_t_u_r_a_l(asDerived()); │ │ │ │ │ +115 return eliminateMultifrontal(computedOrdering, function, variableIndex); │ │ │ │ │ +116 } else { │ │ │ │ │ +_1_1_7 _O_r_d_e_r_i_n_g computedOrdering = EliminationTraitsType::DefaultOrderingFunc( │ │ │ │ │ +118 asDerived(), variableIndex); │ │ │ │ │ +119 return eliminateMultifrontal(computedOrdering, function, variableIndex); │ │ │ │ │ +120 } │ │ │ │ │ +121 } │ │ │ │ │ +122 } │ │ │ │ │ +123 │ │ │ │ │ +124 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +125 template │ │ │ │ │ +126 boost::shared_ptr:: │ │ │ │ │ +BayesTreeType> │ │ │ │ │ +_1_2_7 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_G_R_A_P_H_>_:_:_e_l_i_m_i_n_a_t_e_M_u_l_t_i_f_r_o_n_t_a_l( │ │ │ │ │ +128 const _O_r_d_e_r_i_n_g& ordering, const _E_l_i_m_i_n_a_t_e& function, │ │ │ │ │ +129 _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex) const │ │ │ │ │ +130 { │ │ │ │ │ +131 if(!variableIndex) { │ │ │ │ │ +132 // If no VariableIndex provided, compute one and call this function again │ │ │ │ │ +133 _V_a_r_i_a_b_l_e_I_n_d_e_x computedVariableIndex(asDerived()); │ │ │ │ │ +134 return eliminateMultifrontal(ordering, function, computedVariableIndex); │ │ │ │ │ +135 } else { │ │ │ │ │ +_1_3_6 gttic(eliminateMultifrontal); │ │ │ │ │ +137 // Do elimination with given ordering │ │ │ │ │ +138 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_T_y_p_e etree(asDerived(), *variableIndex, ordering); │ │ │ │ │ +139 _J_u_n_c_t_i_o_n_T_r_e_e_T_y_p_e junctionTree(etree); │ │ │ │ │ +140 boost::shared_ptr bayesTree; │ │ │ │ │ +141 boost::shared_ptr factorGraph; │ │ │ │ │ +142 boost::tie(bayesTree,factorGraph) = junctionTree.eliminate(function); │ │ │ │ │ +143 // If any factors are remaining, the ordering was incomplete │ │ │ │ │ +144 if(!factorGraph->empty()) │ │ │ │ │ +145 throw _I_n_c_o_n_s_i_s_t_e_n_t_E_l_i_m_i_n_a_t_i_o_n_R_e_q_u_e_s_t_e_d(); │ │ │ │ │ +146 // Return the Bayes tree │ │ │ │ │ +147 return bayesTree; │ │ │ │ │ 148 } │ │ │ │ │ -149 // Set up BayesTree parent and child pointers │ │ │ │ │ -150 if (parentData) { │ │ │ │ │ -151 if (parentData->parentData) // If our parent is not the dummy node │ │ │ │ │ -152 bayesTreeNode->parent_ = parentData->bayesTreeNode; │ │ │ │ │ -153 parentData->bayesTreeNode->children.push_back(bayesTreeNode); │ │ │ │ │ -154 } │ │ │ │ │ -155 } │ │ │ │ │ -156 │ │ │ │ │ -157 // Elimination pre-order visitor - creates the EliminationData structure │ │ │ │ │ -for the visited node. │ │ │ │ │ -158 static EliminationData EliminationPreOrderVisitor( │ │ │ │ │ -159 const typename CLUSTERTREE::sharedNode& node, │ │ │ │ │ -160 EliminationData& parentData) { │ │ │ │ │ -161 assert(node); │ │ │ │ │ -162 EliminationData myData(&parentData, node->nrChildren()); │ │ │ │ │ -163 myData.bayesTreeNode->problemSize_ = node->problemSize(); │ │ │ │ │ -164 return myData; │ │ │ │ │ -165 } │ │ │ │ │ -166 │ │ │ │ │ -167 // Elimination post-order visitor - combine the child factors with our own │ │ │ │ │ -factors, add the │ │ │ │ │ -168 // resulting conditional to the BayesTree, and add the remaining factor to │ │ │ │ │ -the parent. │ │ │ │ │ -_1_6_9 class _E_l_i_m_i_n_a_t_i_o_n_P_o_s_t_O_r_d_e_r_V_i_s_i_t_o_r { │ │ │ │ │ -170 const typename CLUSTERTREE::Eliminate& eliminationFunction_; │ │ │ │ │ -171 typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex_; │ │ │ │ │ -172 │ │ │ │ │ -173 public: │ │ │ │ │ -174 // Construct functor │ │ │ │ │ -_1_7_5 EliminationPostOrderVisitor( │ │ │ │ │ -176 const typename CLUSTERTREE::Eliminate& eliminationFunction, │ │ │ │ │ -177 typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex) : │ │ │ │ │ -178 eliminationFunction_(eliminationFunction), nodesIndex_(nodesIndex) { │ │ │ │ │ -179 } │ │ │ │ │ -180 │ │ │ │ │ -181 // Function that does the HEAVY lifting │ │ │ │ │ -182 void operator()(const typename CLUSTERTREE::sharedNode& node, │ │ │ │ │ -_E_l_i_m_i_n_a_t_i_o_n_D_a_t_a& myData) { │ │ │ │ │ -183 assert(node); │ │ │ │ │ -184 │ │ │ │ │ -185 // Gather factors │ │ │ │ │ -186 FactorGraphType gatheredFactors; │ │ │ │ │ -187 gatheredFactors.reserve(node->factors.size() + node->nrChildren()); │ │ │ │ │ -188 gatheredFactors += node->factors; │ │ │ │ │ -189 gatheredFactors += myData.childFactors; │ │ │ │ │ -190 │ │ │ │ │ -191 // Check for Bayes tree orphan subtrees, and add them to our children │ │ │ │ │ -192 // TODO(frank): should this really happen here? │ │ │ │ │ -193 for (const sharedFactor& factor: node->factors) { │ │ │ │ │ -194 auto asSubtree = dynamic_cast*> │ │ │ │ │ -(factor.get()); │ │ │ │ │ -195 if (asSubtree) { │ │ │ │ │ -196 myData.bayesTreeNode->children.push_back(asSubtree->clique); │ │ │ │ │ -197 asSubtree->clique->parent_ = myData.bayesTreeNode; │ │ │ │ │ -198 } │ │ │ │ │ -199 } │ │ │ │ │ -200 │ │ │ │ │ -201 // >>>>>>>>>>>>>> Do dense elimination step >>>>>>>>>>>>>>>>>>>>>>>>>>>>> │ │ │ │ │ -202 auto eliminationResult = eliminationFunction_(gatheredFactors, node- │ │ │ │ │ ->orderedFrontalKeys); │ │ │ │ │ -203 // <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< │ │ │ │ │ -204 │ │ │ │ │ -205 // Store conditional in BayesTree clique, and in the case of ISAM2Clique │ │ │ │ │ -also store the │ │ │ │ │ -206 // remaining factor │ │ │ │ │ -207 myData.bayesTreeNode->setEliminationResult(eliminationResult); │ │ │ │ │ +149 } │ │ │ │ │ +150 │ │ │ │ │ +151 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +152 template │ │ │ │ │ +153 std::pair::BayesNetType>, boost:: │ │ │ │ │ +shared_ptr > │ │ │ │ │ +_1_5_4 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_G_R_A_P_H_>_:_:_e_l_i_m_i_n_a_t_e_P_a_r_t_i_a_l_S_e_q_u_e_n_t_i_a_l( │ │ │ │ │ +155 const _O_r_d_e_r_i_n_g& ordering, const _E_l_i_m_i_n_a_t_e& function, _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ +variableIndex) const │ │ │ │ │ +156 { │ │ │ │ │ +_1_5_7 if(variableIndex) { │ │ │ │ │ +158 gttic(eliminatePartialSequential); │ │ │ │ │ +159 // Do elimination │ │ │ │ │ +160 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_T_y_p_e etree(asDerived(), *variableIndex, ordering); │ │ │ │ │ +161 return etree.eliminate(function); │ │ │ │ │ +162 } else { │ │ │ │ │ +163 // If no variable index is provided, compute one and call this function │ │ │ │ │ +again │ │ │ │ │ +164 _V_a_r_i_a_b_l_e_I_n_d_e_x computedVariableIndex(asDerived()); │ │ │ │ │ +165 return eliminatePartialSequential(ordering, function, │ │ │ │ │ +computedVariableIndex); │ │ │ │ │ +166 } │ │ │ │ │ +167 } │ │ │ │ │ +168 │ │ │ │ │ +169 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +170 template │ │ │ │ │ +_1_7_1 std::pair::BayesNetType>, boost:: │ │ │ │ │ +shared_ptr > │ │ │ │ │ +_1_7_2 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_G_R_A_P_H_>_:_:_e_l_i_m_i_n_a_t_e_P_a_r_t_i_a_l_S_e_q_u_e_n_t_i_a_l( │ │ │ │ │ +173 const _K_e_y_V_e_c_t_o_r& variables, const _E_l_i_m_i_n_a_t_e& function, │ │ │ │ │ +_O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex) const │ │ │ │ │ +174 { │ │ │ │ │ +175 if(variableIndex) { │ │ │ │ │ +176 gttic(eliminatePartialSequential); │ │ │ │ │ +177 // Compute full ordering │ │ │ │ │ +178 _O_r_d_e_r_i_n_g fullOrdering = _O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_F_i_r_s_t(*variableIndex, │ │ │ │ │ +variables); │ │ │ │ │ +179 │ │ │ │ │ +180 // Split off the part of the ordering for the variables being eliminated │ │ │ │ │ +_1_8_1 _O_r_d_e_r_i_n_g ordering(fullOrdering.begin(), fullOrdering.begin() + │ │ │ │ │ +variables.size()); │ │ │ │ │ +182 return eliminatePartialSequential(ordering, function, variableIndex); │ │ │ │ │ +183 } else { │ │ │ │ │ +184 // If no variable index is provided, compute one and call this function │ │ │ │ │ +again │ │ │ │ │ +185 _V_a_r_i_a_b_l_e_I_n_d_e_x computedVariableIndex(asDerived()); │ │ │ │ │ +186 return eliminatePartialSequential(variables, function, │ │ │ │ │ +computedVariableIndex); │ │ │ │ │ +187 } │ │ │ │ │ +188 } │ │ │ │ │ +189 │ │ │ │ │ +190 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +_1_9_1 template │ │ │ │ │ +192 std::pair::BayesTreeType>, boost:: │ │ │ │ │ +shared_ptr > │ │ │ │ │ +_1_9_3 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_G_R_A_P_H_>_:_:_e_l_i_m_i_n_a_t_e_P_a_r_t_i_a_l_M_u_l_t_i_f_r_o_n_t_a_l( │ │ │ │ │ +194 const _O_r_d_e_r_i_n_g& ordering, const _E_l_i_m_i_n_a_t_e& function, _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ +variableIndex) const │ │ │ │ │ +195 { │ │ │ │ │ +196 if(variableIndex) { │ │ │ │ │ +197 gttic(eliminatePartialMultifrontal); │ │ │ │ │ +198 // Do elimination │ │ │ │ │ +199 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_T_y_p_e etree(asDerived(), *variableIndex, ordering); │ │ │ │ │ +200 _J_u_n_c_t_i_o_n_T_r_e_e_T_y_p_e junctionTree(etree); │ │ │ │ │ +_2_0_1 return junctionTree.eliminate(function); │ │ │ │ │ +202 } else { │ │ │ │ │ +203 // If no variable index is provided, compute one and call this function │ │ │ │ │ +again │ │ │ │ │ +204 _V_a_r_i_a_b_l_e_I_n_d_e_x computedVariableIndex(asDerived()); │ │ │ │ │ +205 return eliminatePartialMultifrontal(ordering, function, │ │ │ │ │ +computedVariableIndex); │ │ │ │ │ +206 } │ │ │ │ │ +207 } │ │ │ │ │ 208 │ │ │ │ │ -209 // Fill nodes index - we do this here instead of calling insertRoot at the │ │ │ │ │ -end to avoid │ │ │ │ │ -210 // putting orphan subtrees in the index - they'll already be in the index │ │ │ │ │ -of the ISAM2 │ │ │ │ │ -211 // object they're added to. │ │ │ │ │ -212 for (const Key& j: myData.bayesTreeNode->conditional()->frontals()) │ │ │ │ │ -213 nodesIndex_.insert(std::make_pair(j, myData.bayesTreeNode)); │ │ │ │ │ -214 │ │ │ │ │ -215 // Store remaining factor in parent's gathered factors │ │ │ │ │ -216 if (!eliminationResult.second->empty()) { │ │ │ │ │ -217#ifdef GTSAM_USE_TBB │ │ │ │ │ -218 myData.parentData->writeLock->lock(); │ │ │ │ │ -219#endif │ │ │ │ │ -220 myData.parentData->childFactors[myData.myIndexInParent] = │ │ │ │ │ -eliminationResult.second; │ │ │ │ │ -221#ifdef GTSAM_USE_TBB │ │ │ │ │ -_2_2_2 myData.parentData->writeLock->unlock(); │ │ │ │ │ -223#endif │ │ │ │ │ -224 } │ │ │ │ │ -225 } │ │ │ │ │ -226 }; │ │ │ │ │ -227}; │ │ │ │ │ -228 │ │ │ │ │ -229/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -230template │ │ │ │ │ -_2_3_1_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_<_B_A_Y_E_S_T_R_E_E_,_ _G_R_A_P_H_>& │ │ │ │ │ -_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_<_B_A_Y_E_S_T_R_E_E_,_ _G_R_A_P_H_>_:_:_o_p_e_r_a_t_o_r_=( │ │ │ │ │ -232 const _T_h_i_s& other) { │ │ │ │ │ -233 _C_l_u_s_t_e_r_T_r_e_e_<_G_R_A_P_H_>_:_:_o_p_e_r_a_t_o_r_=(other); │ │ │ │ │ -234 │ │ │ │ │ -235 // Assign the remaining factors - these are pointers to factors in the │ │ │ │ │ -original factor graph and │ │ │ │ │ -236 // we do not clone them. │ │ │ │ │ -237 remainingFactors_ = other.remainingFactors_; │ │ │ │ │ -238 │ │ │ │ │ -239 return *this; │ │ │ │ │ -240} │ │ │ │ │ -241 │ │ │ │ │ -242/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -_2_4_3template │ │ │ │ │ -244std::pair, boost::shared_ptr > │ │ │ │ │ -_2_4_5_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_<_B_A_Y_E_S_T_R_E_E_,_ _G_R_A_P_H_>_:_:_e_l_i_m_i_n_a_t_e(const _E_l_i_m_i_n_a_t_e& │ │ │ │ │ -function) const { │ │ │ │ │ -246 gttic(ClusterTree_eliminate); │ │ │ │ │ -247 // Do elimination (depth-first traversal). The rootsContainer stores a │ │ │ │ │ -'dummy' BayesTree node │ │ │ │ │ -248 // that contains all of the roots as its children. rootsContainer also │ │ │ │ │ -stores the remaining │ │ │ │ │ -249 // un-eliminated factors passed up from the roots. │ │ │ │ │ -250 boost::shared_ptr result = boost::make_shared │ │ │ │ │ -(); │ │ │ │ │ +209 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +210 template │ │ │ │ │ +_2_1_1 std::pair::BayesTreeType>, boost:: │ │ │ │ │ +shared_ptr > │ │ │ │ │ +_2_1_2 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_G_R_A_P_H_>_:_:_e_l_i_m_i_n_a_t_e_P_a_r_t_i_a_l_M_u_l_t_i_f_r_o_n_t_a_l( │ │ │ │ │ +213 const _K_e_y_V_e_c_t_o_r& variables, const _E_l_i_m_i_n_a_t_e& function, │ │ │ │ │ +_O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex) const │ │ │ │ │ +214 { │ │ │ │ │ +215 if(variableIndex) { │ │ │ │ │ +216 gttic(eliminatePartialMultifrontal); │ │ │ │ │ +217 // Compute full ordering │ │ │ │ │ +218 _O_r_d_e_r_i_n_g fullOrdering = _O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_F_i_r_s_t(*variableIndex, │ │ │ │ │ +variables); │ │ │ │ │ +219 │ │ │ │ │ +220 // Split off the part of the ordering for the variables being eliminated │ │ │ │ │ +221 _O_r_d_e_r_i_n_g ordering(fullOrdering.begin(), fullOrdering.begin() + │ │ │ │ │ +variables.size()); │ │ │ │ │ +222 return eliminatePartialMultifrontal(ordering, function, variableIndex); │ │ │ │ │ +223 } else { │ │ │ │ │ +224 // If no variable index is provided, compute one and call this function │ │ │ │ │ +again │ │ │ │ │ +_2_2_5 _V_a_r_i_a_b_l_e_I_n_d_e_x computedVariableIndex(asDerived()); │ │ │ │ │ +226 return eliminatePartialMultifrontal(variables, function, │ │ │ │ │ +computedVariableIndex); │ │ │ │ │ +227 } │ │ │ │ │ +228 } │ │ │ │ │ +229 │ │ │ │ │ +230 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +231 template │ │ │ │ │ +232 boost::shared_ptr:: │ │ │ │ │ +BayesNetType> │ │ │ │ │ +_2_3_3 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_G_R_A_P_H_>_:_:_m_a_r_g_i_n_a_l_M_u_l_t_i_f_r_o_n_t_a_l_B_a_y_e_s_N_e_t( │ │ │ │ │ +234 boost::variant variables, │ │ │ │ │ +235 const _E_l_i_m_i_n_a_t_e& function, _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex) const │ │ │ │ │ +236 { │ │ │ │ │ +237 if(!variableIndex) { │ │ │ │ │ +238 // If no variable index is provided, compute one and call this function │ │ │ │ │ +again │ │ │ │ │ +239 _V_a_r_i_a_b_l_e_I_n_d_e_x index(asDerived()); │ │ │ │ │ +_2_4_0 return _m_a_r_g_i_n_a_l_M_u_l_t_i_f_r_o_n_t_a_l_B_a_y_e_s_N_e_t(variables, function, index); │ │ │ │ │ +241 } else { │ │ │ │ │ +242 // No ordering was provided for the marginalized variables, so order them │ │ │ │ │ +using constrained │ │ │ │ │ +243 // COLAMD. │ │ │ │ │ +244 bool unmarginalizedAreOrdered = (boost::get(&variables) != │ │ │ │ │ +0); │ │ │ │ │ +245 const _K_e_y_V_e_c_t_o_r* variablesOrOrdering = │ │ │ │ │ +246 unmarginalizedAreOrdered ? │ │ │ │ │ +247 boost::get(&variables) : boost::get │ │ │ │ │ +(&variables); │ │ │ │ │ +248 │ │ │ │ │ +249 _O_r_d_e_r_i_n_g totalOrdering = │ │ │ │ │ +250 _O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_L_a_s_t(*variableIndex, *variablesOrOrdering, │ │ │ │ │ +unmarginalizedAreOrdered); │ │ │ │ │ 251 │ │ │ │ │ -252 typedef _E_l_i_m_i_n_a_t_i_o_n_D_a_t_a_<_T_h_i_s_> Data; │ │ │ │ │ -253 Data rootsContainer(0, this->nrRoots()); │ │ │ │ │ -254 │ │ │ │ │ -255 typename Data::EliminationPostOrderVisitor visitorPost(function, result- │ │ │ │ │ ->nodes_); │ │ │ │ │ -256 { │ │ │ │ │ -257 _T_b_b_O_p_e_n_M_P_M_i_x_e_d_S_c_o_p_e threadLimiter; // Limits OpenMP threads since we're │ │ │ │ │ -mixing TBB and OpenMP │ │ │ │ │ -258 _t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t_P_a_r_a_l_l_e_l(*this, rootsContainer, Data:: │ │ │ │ │ -EliminationPreOrderVisitor, │ │ │ │ │ -259 visitorPost, 10); │ │ │ │ │ +252 // Split up ordering │ │ │ │ │ +253 const size_t nVars = variablesOrOrdering->size(); │ │ │ │ │ +254 _O_r_d_e_r_i_n_g marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() │ │ │ │ │ +- nVars); │ │ │ │ │ +_2_5_5 _O_r_d_e_r_i_n_g marginalVarsOrdering(totalOrdering.end() - nVars, │ │ │ │ │ +totalOrdering.end()); │ │ │ │ │ +256 │ │ │ │ │ +257 // Call this function again with the computed orderings │ │ │ │ │ +258 return marginalMultifrontalBayesNet(marginalVarsOrdering, │ │ │ │ │ +marginalizationOrdering, function, *variableIndex); │ │ │ │ │ +259 } │ │ │ │ │ 260 } │ │ │ │ │ 261 │ │ │ │ │ -262 // Create BayesTree from roots stored in the dummy BayesTree node. │ │ │ │ │ -263 result->roots_.insert(result->roots_.end(), rootsContainer.bayesTreeNode- │ │ │ │ │ ->children.begin(), │ │ │ │ │ -264 rootsContainer.bayesTreeNode->children.end()); │ │ │ │ │ -265 │ │ │ │ │ -266 // Add remaining factors that were not involved with eliminated variables │ │ │ │ │ -267 boost::shared_ptr remaining = boost:: │ │ │ │ │ -make_shared(); │ │ │ │ │ -268 remaining->reserve(remainingFactors_.size() + │ │ │ │ │ -rootsContainer.childFactors.size()); │ │ │ │ │ -269 remaining->push_back(remainingFactors_.begin(), remainingFactors_.end()); │ │ │ │ │ -270 for (const _s_h_a_r_e_d_F_a_c_t_o_r& factor : rootsContainer.childFactors) { │ │ │ │ │ -271 if (factor) │ │ │ │ │ -272 remaining->push_back(factor); │ │ │ │ │ -273 } │ │ │ │ │ -274 │ │ │ │ │ -275 // Return result │ │ │ │ │ -276 return std::make_pair(result, remaining); │ │ │ │ │ -277} │ │ │ │ │ -278 │ │ │ │ │ -279} // namespace gtsam │ │ │ │ │ -_t_r_e_e_T_r_a_v_e_r_s_a_l_-_i_n_s_t_._h │ │ │ │ │ -_t_i_m_i_n_g_._h │ │ │ │ │ -Timing utilities. │ │ │ │ │ -_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ -Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ │ -_C_l_u_s_t_e_r_T_r_e_e_._h │ │ │ │ │ -Collects factorgraph fragments defined on variable clusters, arranged in a │ │ │ │ │ -tree. │ │ │ │ │ -_O_r_d_e_r_i_n_g_._h │ │ │ │ │ -Variable ordering for the elimination algorithm. │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ -std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ -FastVector │ │ │ │ │ -FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ -DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ +262 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +263 template │ │ │ │ │ +264 boost::shared_ptr:: │ │ │ │ │ +BayesNetType> │ │ │ │ │ +_2_6_5 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_G_R_A_P_H_>_:_:_m_a_r_g_i_n_a_l_M_u_l_t_i_f_r_o_n_t_a_l_B_a_y_e_s_N_e_t( │ │ │ │ │ +266 boost::variant variables, │ │ │ │ │ +267 const _O_r_d_e_r_i_n_g& marginalizedVariableOrdering, │ │ │ │ │ +268 const _E_l_i_m_i_n_a_t_e& function, _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex) const │ │ │ │ │ +269 { │ │ │ │ │ +_2_7_0 if(!variableIndex) { │ │ │ │ │ +271 // If no variable index is provided, compute one and call this function │ │ │ │ │ +again │ │ │ │ │ +272 _V_a_r_i_a_b_l_e_I_n_d_e_x index(asDerived()); │ │ │ │ │ +273 return marginalMultifrontalBayesNet(variables, │ │ │ │ │ +marginalizedVariableOrdering, function, index); │ │ │ │ │ +274 } else { │ │ │ │ │ +275 gttic(marginalMultifrontalBayesNet); │ │ │ │ │ +276 // An ordering was provided for the marginalized variables, so we can first │ │ │ │ │ +eliminate them │ │ │ │ │ +_2_7_7 // in the order requested. │ │ │ │ │ +278 boost::shared_ptr bayesTree; │ │ │ │ │ +279 boost::shared_ptr factorGraph; │ │ │ │ │ +280 boost::tie(bayesTree,factorGraph) = │ │ │ │ │ +281 eliminatePartialMultifrontal(marginalizedVariableOrdering, function, │ │ │ │ │ +*variableIndex); │ │ │ │ │ +282 │ │ │ │ │ +283 if(const _O_r_d_e_r_i_n_g* varsAsOrdering = boost::get │ │ │ │ │ +(&variables)) │ │ │ │ │ +284 { │ │ │ │ │ +285 // An ordering was also provided for the unmarginalized variables, so we │ │ │ │ │ +can also │ │ │ │ │ +286 // eliminate them in the order requested. │ │ │ │ │ +287 return factorGraph->eliminateSequential(*varsAsOrdering, function); │ │ │ │ │ +288 } │ │ │ │ │ +289 else │ │ │ │ │ +290 { │ │ │ │ │ +291 // No ordering was provided for the unmarginalized variables, so order them │ │ │ │ │ +with COLAMD. │ │ │ │ │ +292 return factorGraph->eliminateSequential(Ordering::COLAMD, function); │ │ │ │ │ +293 } │ │ │ │ │ +294 } │ │ │ │ │ +295 } │ │ │ │ │ +296 │ │ │ │ │ +297 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +298 template │ │ │ │ │ +299 boost::shared_ptr:: │ │ │ │ │ +BayesTreeType> │ │ │ │ │ +_3_0_0 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_G_R_A_P_H_>_:_:_m_a_r_g_i_n_a_l_M_u_l_t_i_f_r_o_n_t_a_l_B_a_y_e_s_T_r_e_e( │ │ │ │ │ +301 boost::variant variables, │ │ │ │ │ +302 const _E_l_i_m_i_n_a_t_e& function, _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex) const │ │ │ │ │ +303 { │ │ │ │ │ +304 if(!variableIndex) { │ │ │ │ │ +305 // If no variable index is provided, compute one and call this function │ │ │ │ │ +again │ │ │ │ │ +306 _V_a_r_i_a_b_l_e_I_n_d_e_x computedVariableIndex(asDerived()); │ │ │ │ │ +307 return marginalMultifrontalBayesTree(variables, function, │ │ │ │ │ +computedVariableIndex); │ │ │ │ │ +308 } else { │ │ │ │ │ +309 // No ordering was provided for the marginalized variables, so order them │ │ │ │ │ +using constrained │ │ │ │ │ +310 // COLAMD. │ │ │ │ │ +311 bool unmarginalizedAreOrdered = (boost::get(&variables) != │ │ │ │ │ +0); │ │ │ │ │ +312 const _K_e_y_V_e_c_t_o_r* variablesOrOrdering = │ │ │ │ │ +313 unmarginalizedAreOrdered ? │ │ │ │ │ +314 boost::get(&variables) : boost::get │ │ │ │ │ +(&variables); │ │ │ │ │ +315 │ │ │ │ │ +316 _O_r_d_e_r_i_n_g totalOrdering = │ │ │ │ │ +317 _O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_L_a_s_t(*variableIndex, *variablesOrOrdering, │ │ │ │ │ +unmarginalizedAreOrdered); │ │ │ │ │ +318 │ │ │ │ │ +319 // Split up ordering │ │ │ │ │ +320 const size_t nVars = variablesOrOrdering->size(); │ │ │ │ │ +321 _O_r_d_e_r_i_n_g marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() │ │ │ │ │ +- nVars); │ │ │ │ │ +322 _O_r_d_e_r_i_n_g marginalVarsOrdering(totalOrdering.end() - nVars, │ │ │ │ │ +totalOrdering.end()); │ │ │ │ │ +323 │ │ │ │ │ +324 // Call this function again with the computed orderings │ │ │ │ │ +325 return marginalMultifrontalBayesTree(marginalVarsOrdering, │ │ │ │ │ +marginalizationOrdering, function, *variableIndex); │ │ │ │ │ +326 } │ │ │ │ │ +327 } │ │ │ │ │ +328 │ │ │ │ │ +329 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +330 template │ │ │ │ │ +331 boost::shared_ptr:: │ │ │ │ │ +BayesTreeType> │ │ │ │ │ +_3_3_2 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_G_R_A_P_H_>_:_:_m_a_r_g_i_n_a_l_M_u_l_t_i_f_r_o_n_t_a_l_B_a_y_e_s_T_r_e_e( │ │ │ │ │ +333 boost::variant variables, │ │ │ │ │ +334 const _O_r_d_e_r_i_n_g& marginalizedVariableOrdering, │ │ │ │ │ +335 const _E_l_i_m_i_n_a_t_e& function, _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex) const │ │ │ │ │ +336 { │ │ │ │ │ +337 if(!variableIndex) { │ │ │ │ │ +338 // If no variable index is provided, compute one and call this function │ │ │ │ │ +again │ │ │ │ │ +339 _V_a_r_i_a_b_l_e_I_n_d_e_x computedVariableIndex(asDerived()); │ │ │ │ │ +340 return marginalMultifrontalBayesTree(variables, │ │ │ │ │ +marginalizedVariableOrdering, function, computedVariableIndex); │ │ │ │ │ +341 } else { │ │ │ │ │ +342 gttic(marginalMultifrontalBayesTree); │ │ │ │ │ +343 // An ordering was provided for the marginalized variables, so we can first │ │ │ │ │ +eliminate them │ │ │ │ │ +344 // in the order requested. │ │ │ │ │ +345 boost::shared_ptr bayesTree; │ │ │ │ │ +346 boost::shared_ptr factorGraph; │ │ │ │ │ +347 boost::tie(bayesTree,factorGraph) = │ │ │ │ │ +348 eliminatePartialMultifrontal(marginalizedVariableOrdering, function, │ │ │ │ │ +*variableIndex); │ │ │ │ │ +349 │ │ │ │ │ +350 if(const _O_r_d_e_r_i_n_g* varsAsOrdering = boost::get │ │ │ │ │ +(&variables)) │ │ │ │ │ +351 { │ │ │ │ │ +352 // An ordering was also provided for the unmarginalized variables, so we │ │ │ │ │ +can also │ │ │ │ │ +353 // eliminate them in the order requested. │ │ │ │ │ +354 return factorGraph->eliminateMultifrontal(*varsAsOrdering, function); │ │ │ │ │ +355 } │ │ │ │ │ +356 else │ │ │ │ │ +357 { │ │ │ │ │ +358 // No ordering was provided for the unmarginalized variables, so order them │ │ │ │ │ +with COLAMD. │ │ │ │ │ +359 return factorGraph->eliminateMultifrontal(Ordering::COLAMD, function); │ │ │ │ │ +360 } │ │ │ │ │ +361 } │ │ │ │ │ +362 } │ │ │ │ │ +363 │ │ │ │ │ +364 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +365 template │ │ │ │ │ +366 boost::shared_ptr │ │ │ │ │ +_3_6_7 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_G_R_A_P_H_>_:_:_m_a_r_g_i_n_a_l( │ │ │ │ │ +368 const _K_e_y_V_e_c_t_o_r& variables, │ │ │ │ │ +369 const _E_l_i_m_i_n_a_t_e& function, _O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex) const │ │ │ │ │ +370 { │ │ │ │ │ +371 if(variableIndex) │ │ │ │ │ +372 { │ │ │ │ │ +373 // Compute a total ordering for all variables │ │ │ │ │ +374 _O_r_d_e_r_i_n_g totalOrdering = _O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_L_a_s_t(*variableIndex, │ │ │ │ │ +variables); │ │ │ │ │ +375 │ │ │ │ │ +376 // Split out the part for the marginalized variables │ │ │ │ │ +377 _O_r_d_e_r_i_n_g marginalizationOrdering(totalOrdering.begin(), totalOrdering.end() │ │ │ │ │ +- variables.size()); │ │ │ │ │ +378 │ │ │ │ │ +379 // Eliminate and return the remaining factor graph │ │ │ │ │ +380 return eliminatePartialMultifrontal(marginalizationOrdering, function, │ │ │ │ │ +*variableIndex).second; │ │ │ │ │ +381 } │ │ │ │ │ +382 else │ │ │ │ │ +383 { │ │ │ │ │ +384 // If no variable index is provided, compute one and call this function │ │ │ │ │ +again │ │ │ │ │ +385 _V_a_r_i_a_b_l_e_I_n_d_e_x computedVariableIndex(asDerived()); │ │ │ │ │ +386 return marginal(variables, function, computedVariableIndex); │ │ │ │ │ +387 } │ │ │ │ │ +388 } │ │ │ │ │ +389 │ │ │ │ │ +390 │ │ │ │ │ +391} │ │ │ │ │ +_i_n_f_e_r_e_n_c_e_E_x_c_e_p_t_i_o_n_s_._h │ │ │ │ │ +Exceptions that may be thrown by inference algorithms. │ │ │ │ │ +_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Variable elimination algorithms for factor graphs. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_i_n_t_K_e_y_V_e_c_t_o_r │ │ │ │ │ -void PrintKeyVector(const KeyVector &keys, const string &s, const KeyFormatter │ │ │ │ │ -&keyFormatter) │ │ │ │ │ -Utility function to print sets of keys with optional prefix. │ │ │ │ │ -DDeeffiinniittiioonn Key.cpp:77 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_C_l_o_n_e_F_o_r_e_s_t │ │ │ │ │ -FastVector< boost::shared_ptr< typename FOREST::Node > > CloneForest(const │ │ │ │ │ -FOREST &forest) │ │ │ │ │ -Clone a tree, copy-constructing new nodes (calling boost::make_shared) and │ │ │ │ │ -setting up child pointers ... │ │ │ │ │ -DDeeffiinniittiioonn treeTraversal-inst.h:189 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_P_r_i_n_t_F_o_r_e_s_t │ │ │ │ │ -void PrintForest(const FOREST &forest, std::string str, const KeyFormatter │ │ │ │ │ -&keyFormatter) │ │ │ │ │ -Print a tree, prefixing each line with str, and formatting keys using │ │ │ │ │ -keyFormatter. │ │ │ │ │ -DDeeffiinniittiioonn treeTraversal-inst.h:219 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t_P_a_r_a_l_l_e_l │ │ │ │ │ -void DepthFirstForestParallel(FOREST &forest, DATA &rootData, VISITOR_PRE │ │ │ │ │ -&visitorPre, VISITOR_POST &visitorPost, int problemSizeThreshold=10) │ │ │ │ │ -Traverse a forest depth-first with pre-order and post-order visits. │ │ │ │ │ -DDeeffiinniittiioonn treeTraversal-inst.h:154 │ │ │ │ │ -_g_t_s_a_m_:_:_T_b_b_O_p_e_n_M_P_M_i_x_e_d_S_c_o_p_e │ │ │ │ │ -An object whose scope defines a block where TBB and OpenMP parallelism are │ │ │ │ │ -mixed. │ │ │ │ │ -DDeeffiinniittiioonn types.h:192 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e │ │ │ │ │ -A cluster-tree that eliminates to a Bayes tree. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:184 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ -This & operator=(const This &other) │ │ │ │ │ -Assignment operator - makes a deep copy of the tree structure, but only │ │ │ │ │ -pointers to factors are copie... │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree-inst.h:231 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_e_l_i_m_i_n_a_t_e │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_m_a_r_g_i_n_a_l_M_u_l_t_i_f_r_o_n_t_a_l_B_a_y_e_s_T_r_e_e │ │ │ │ │ +boost::shared_ptr< BayesTreeType > marginalMultifrontalBayesTree(boost:: │ │ │ │ │ +variant< const Ordering &, const KeyVector & > variables, const Eliminate │ │ │ │ │ +&function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex │ │ │ │ │ +variableIndex=boost::none) const │ │ │ │ │ +Compute the marginal of the requested variables and return the result as a │ │ │ │ │ +Bayes tree. │ │ │ │ │ +DDeeffiinniittiioonn EliminateableFactorGraph-inst.h:300 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_J_u_n_c_t_i_o_n_T_r_e_e_T_y_p_e │ │ │ │ │ +EliminationTraitsType::JunctionTreeType JunctionTreeType │ │ │ │ │ +Junction tree type that can do multifrontal elimination of this graph. │ │ │ │ │ +DDeeffiinniittiioonn EliminateableFactorGraph.h:82 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_m_a_r_g_i_n_a_l │ │ │ │ │ +boost::shared_ptr< FactorGraphType > marginal(const KeyVector &variables, const │ │ │ │ │ +Eliminate &function=EliminationTraitsType::DefaultEliminate, │ │ │ │ │ +OptionalVariableIndex variableIndex=boost::none) const │ │ │ │ │ +Compute the marginal factor graph of the requested variables. │ │ │ │ │ +DDeeffiinniittiioonn EliminateableFactorGraph-inst.h:367 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_m_a_r_g_i_n_a_l_M_u_l_t_i_f_r_o_n_t_a_l_B_a_y_e_s_N_e_t │ │ │ │ │ +boost::shared_ptr< BayesNetType > marginalMultifrontalBayesNet(boost::variant< │ │ │ │ │ +const Ordering &, const KeyVector & > variables, const Ordering │ │ │ │ │ +&marginalizedVariableOrdering, const Eliminate │ │ │ │ │ +&function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex │ │ │ │ │ +variableIndex=boost::none) const │ │ │ │ │ +Compute the marginal of the requested variables and return the result as a │ │ │ │ │ +Bayes net. │ │ │ │ │ +DDeeffiinniittiioonn EliminateableFactorGraph-inst.h:265 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_E_l_i_m_i_n_a_t_e │ │ │ │ │ +std::function< EliminationResult(const FactorGraphType &, const Ordering &)> │ │ │ │ │ +Eliminate │ │ │ │ │ +The function type that does a single dense elimination step on a subgraph. │ │ │ │ │ +DDeeffiinniittiioonn EliminateableFactorGraph.h:89 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_B_a_y_e_s_T_r_e_e_T_y_p_e │ │ │ │ │ +EliminationTraitsType::BayesTreeType BayesTreeType │ │ │ │ │ +Bayes tree type produced by multifrontal elimination. │ │ │ │ │ +DDeeffiinniittiioonn EliminateableFactorGraph.h:79 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_e_l_i_m_i_n_a_t_e_S_e_q_u_e_n_t_i_a_l │ │ │ │ │ +boost::shared_ptr< BayesNetType > eliminateSequential(OptionalOrderingType │ │ │ │ │ +orderingType=boost::none, const Eliminate &function=EliminationTraitsType:: │ │ │ │ │ +DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const │ │ │ │ │ +Do sequential elimination of all variables to produce a Bayes net. │ │ │ │ │ +DDeeffiinniittiioonn EliminateableFactorGraph-inst.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_e_l_i_m_i_n_a_t_e_M_u_l_t_i_f_r_o_n_t_a_l │ │ │ │ │ +boost::shared_ptr< BayesTreeType > eliminateMultifrontal(OptionalOrderingType │ │ │ │ │ +orderingType=boost::none, const Eliminate &function=EliminationTraitsType:: │ │ │ │ │ +DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const │ │ │ │ │ +Do multifrontal elimination of all variables to produce a Bayes tree. │ │ │ │ │ +DDeeffiinniittiioonn EliminateableFactorGraph-inst.h:91 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_T_y_p_e │ │ │ │ │ +EliminationTraitsType::EliminationTreeType EliminationTreeType │ │ │ │ │ +Elimination tree type that can do sequential elimination of this graph. │ │ │ │ │ +DDeeffiinniittiioonn EliminateableFactorGraph.h:76 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_O_p_t_i_o_n_a_l_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ +boost::optional< const VariableIndex & > OptionalVariableIndex │ │ │ │ │ +Typedef for an optional variable index as an argument to elimination functions. │ │ │ │ │ +DDeeffiinniittiioonn EliminateableFactorGraph.h:92 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_e_l_i_m_i_n_a_t_e_P_a_r_t_i_a_l_S_e_q_u_e_n_t_i_a_l │ │ │ │ │ +std::pair< boost::shared_ptr< BayesNetType >, boost::shared_ptr< │ │ │ │ │ +FactorGraphType > > eliminatePartialSequential(const Ordering &ordering, const │ │ │ │ │ +Eliminate &function=EliminationTraitsType::DefaultEliminate, │ │ │ │ │ +OptionalVariableIndex variableIndex=boost::none) const │ │ │ │ │ +Do sequential elimination of some variables, in ordering provided, to produce a │ │ │ │ │ +Bayes net and a remai... │ │ │ │ │ +DDeeffiinniittiioonn EliminateableFactorGraph-inst.h:154 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_m_a_r_g_i_n_a_l_M_u_l_t_i_f_r_o_n_t_a_l_B_a_y_e_s_N_e_t │ │ │ │ │ +boost::shared_ptr< BayesNetType > marginalMultifrontalBayesNet(boost::variant< │ │ │ │ │ +const Ordering &, const KeyVector & > variables, const Eliminate │ │ │ │ │ +&function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex │ │ │ │ │ +variableIndex=boost::none) const │ │ │ │ │ +Compute the marginal of the requested variables and return the result as a │ │ │ │ │ +Bayes net. │ │ │ │ │ +DDeeffiinniittiioonn EliminateableFactorGraph-inst.h:233 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_e_l_i_m_i_n_a_t_e_P_a_r_t_i_a_l_M_u_l_t_i_f_r_o_n_t_a_l │ │ │ │ │ std::pair< boost::shared_ptr< BayesTreeType >, boost::shared_ptr< │ │ │ │ │ -FactorGraphType > > eliminate(const Eliminate &function) const │ │ │ │ │ -Eliminate the factors to a Bayes tree and remaining factor graph. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree-inst.h:245 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_s_h_a_r_e_d_F_a_c_t_o_r │ │ │ │ │ -boost::shared_ptr< FactorType > sharedFactor │ │ │ │ │ -Shared pointer to a factor. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:197 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_E_l_i_m_i_n_a_t_e │ │ │ │ │ -GRAPH::Eliminate Eliminate │ │ │ │ │ -Typedef for an eliminate subroutine. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:195 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:276 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_D_a_t_a │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree-inst.h:115 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_D_a_t_a_:_:_E_l_i_m_i_n_a_t_i_o_n_P_o_s_t_O_r_d_e_r_V_i_s_i_t_o_r │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree-inst.h:169 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e │ │ │ │ │ -A cluster-tree is associated with a factor graph and is defined as in Koller- │ │ │ │ │ -Friedman: each node k re... │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:25 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ -This & operator=(const This &other) │ │ │ │ │ -Assignment operator - makes a deep copy of the tree structure, but only │ │ │ │ │ -pointers to factors are copie... │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree-inst.h:104 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_r_o_o_t_s__ │ │ │ │ │ -FastVector< sharedNode > roots_ │ │ │ │ │ -concept check │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:116 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ -Print the cluster tree. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree-inst.h:98 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_<_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_s_h_a_r_e_d_F_a_c_t_o_r │ │ │ │ │ -boost::shared_ptr< FactorType > sharedFactor │ │ │ │ │ -Shared pointer to a factor. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_p_r_i_n_t │ │ │ │ │ -virtual void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ -print this node │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree-inst.h:26 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_o_r_d_e_r_e_d_F_r_o_n_t_a_l_K_e_y_s │ │ │ │ │ -Keys orderedFrontalKeys │ │ │ │ │ -Frontal keys of this node. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_m_e_r_g_e_C_h_i_l_d_r_e_n │ │ │ │ │ -void mergeChildren(const std::vector< bool > &merge) │ │ │ │ │ -Merge all children for which bit is set into this node. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree-inst.h:56 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_m_e_r_g_e │ │ │ │ │ -void merge(const boost::shared_ptr< Cluster > &cluster) │ │ │ │ │ -Merge in given cluster. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree-inst.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_n_r_F_r_o_n_t_a_l_s_O_f_C_h_i_l_d_r_e_n │ │ │ │ │ -std::vector< size_t > nrFrontalsOfChildren() const │ │ │ │ │ -Return a vector with nrFrontal keys for each child. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree-inst.h:34 │ │ │ │ │ +FactorGraphType > > eliminatePartialMultifrontal(const Ordering &ordering, │ │ │ │ │ +const Eliminate &function=EliminationTraitsType::DefaultEliminate, │ │ │ │ │ +OptionalVariableIndex variableIndex=boost::none) const │ │ │ │ │ +Do multifrontal elimination of some variables, in ordering provided, to produce │ │ │ │ │ +a Bayes tree and a re... │ │ │ │ │ +DDeeffiinniittiioonn EliminateableFactorGraph-inst.h:193 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_O_p_t_i_o_n_a_l_O_r_d_e_r_i_n_g_T_y_p_e │ │ │ │ │ +boost::optional< Ordering::OrderingType > OptionalOrderingType │ │ │ │ │ +Typedef for an optional ordering type. │ │ │ │ │ +DDeeffiinniittiioonn EliminateableFactorGraph.h:95 │ │ │ │ │ +_g_t_s_a_m_:_:_I_n_c_o_n_s_i_s_t_e_n_t_E_l_i_m_i_n_a_t_i_o_n_R_e_q_u_e_s_t_e_d │ │ │ │ │ +An inference algorithm was called with inconsistent arguments. │ │ │ │ │ +DDeeffiinniittiioonn inferenceExceptions.h:29 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_N_a_t_u_r_a_l │ │ │ │ │ +static Ordering Natural(const FACTOR_GRAPH &fg) │ │ │ │ │ +Return a natural Ordering. Typically used by iterative solvers. │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:190 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d │ │ │ │ │ +static Ordering Colamd(const FACTOR_GRAPH &graph) │ │ │ │ │ +Compute a fill-reducing ordering using COLAMD from a factor graph (see details │ │ │ │ │ +for note on performanc... │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:95 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_L_a_s_t │ │ │ │ │ +static Ordering ColamdConstrainedLast(const FACTOR_GRAPH &graph, const │ │ │ │ │ +KeyVector &constrainLast, bool forceOrder=false) │ │ │ │ │ +Compute a fill-reducing ordering using constrained COLAMD from a factor graph │ │ │ │ │ +(see details for note o... │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:114 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_F_i_r_s_t │ │ │ │ │ +static Ordering ColamdConstrainedFirst(const FACTOR_GRAPH &graph, const │ │ │ │ │ +KeyVector &constrainFirst, bool forceOrder=false) │ │ │ │ │ +Compute a fill-reducing ordering using constrained COLAMD from a factor graph │ │ │ │ │ +(see details for note o... │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:141 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_M_e_t_i_s │ │ │ │ │ +static GTSAM_EXPORT Ordering Metis(const MetisIndex &met) │ │ │ │ │ +Compute an ordering determined by METIS from a VariableIndex. │ │ │ │ │ +DDeeffiinniittiioonn Ordering.cpp:212 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ +The VariableIndex class computes and stores the block column structure of a │ │ │ │ │ +factor graph. │ │ │ │ │ +DDeeffiinniittiioonn VariableIndex.h:43 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _C_l_u_s_t_e_r_T_r_e_e_-_i_n_s_t_._h │ │ │ │ │ + * EElliimmiinnaatteeaabblleeFFaaccttoorrGGrraapphh--iinnsstt..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00698.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTree.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Factor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,40 +95,38 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces
│ │ │ │ -
BayesTree.cpp File Reference
│ │ │ │ +
Factor.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ +

The base class for all factors. │ │ │ │ More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Bayes Tree is a tree of cliques of a Bayes Chain.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +

The base class for all factors.

│ │ │ │ +
Author
Kai Ni
│ │ │ │
│ │ │ │ -Michael Kaess
│ │ │ │ -
│ │ │ │ -Viorela Ila
│ │ │ │ +Frank Dellaert │ │ │ │
│ │ │ │ Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,22 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -BayesTree.cpp File Reference │ │ │ │ │ -Bayes Tree is a tree of cliques of a Bayes Chain. _M_o_r_e_._._. │ │ │ │ │ +Factor.cpp File Reference │ │ │ │ │ +The base class for all factors. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ │ +The base class for all factors. │ │ │ │ │ Author │ │ │ │ │ + Kai Ni │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Michael Kaess │ │ │ │ │ - Viorela Ila │ │ │ │ │ Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _B_a_y_e_s_T_r_e_e_._c_p_p │ │ │ │ │ + * _F_a_c_t_o_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00701.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTree-inst.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/ClusterTree-inst.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,51 +94,50 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
BayesTree-inst.h File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
ClusterTree-inst.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ +

Collects factorgraph fragments defined on variable clusters, arranged in a tree. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

struct  gtsam::EliminationData< CLUSTERTREE >
 
class  gtsam::EliminationData< CLUSTERTREE >::EliminationPostOrderVisitor
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -template<class CLIQUE >
bool gtsam::check_sharedCliques (const std::pair< Key, typename BayesTree< CLIQUE >::sharedClique > &v1, const std::pair< Key, typename BayesTree< CLIQUE >::sharedClique > &v2)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Bayes Tree is a tree of cliques of a Bayes Chain.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ -Michael Kaess
│ │ │ │ +

Collects factorgraph fragments defined on variable clusters, arranged in a tree.

│ │ │ │ +
Date
Oct 8, 2013
│ │ │ │ +
Author
Kai Ni
│ │ │ │
│ │ │ │ -Viorela Ila
│ │ │ │ +Richard Roberts │ │ │ │
│ │ │ │ -Richard Roberts
│ │ │ │ +Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,30 +1,32 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -BayesTree-inst.h File Reference │ │ │ │ │ -Bayes Tree is a tree of cliques of a Bayes Chain. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +ClusterTree-inst.h File Reference │ │ │ │ │ +Collects factorgraph fragments defined on variable clusters, arranged in a │ │ │ │ │ +tree. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +struct   _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_D_a_t_a_<_ _C_L_U_S_T_E_R_T_R_E_E_ _> │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_D_a_t_a_<_ _C_L_U_S_T_E_R_T_R_E_E_ _>_:_:_E_l_i_m_i_n_a_t_i_o_n_P_o_s_t_O_r_d_e_r_V_i_s_i_t_o_r │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ -bool  ggttssaamm::::cchheecckk__sshhaarreeddCClliiqquueess (const std::pair< _K_e_y, typename _B_a_y_e_s_T_r_e_e< │ │ │ │ │ - CLIQUE >::sharedClique > &v1, const std::pair< _K_e_y, typename _B_a_y_e_s_T_r_e_e< │ │ │ │ │ - CLIQUE >::sharedClique > &v2) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ │ +Collects factorgraph fragments defined on variable clusters, arranged in a │ │ │ │ │ +tree. │ │ │ │ │ + Date │ │ │ │ │ + Oct 8, 2013 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Michael Kaess │ │ │ │ │ - Viorela Ila │ │ │ │ │ + Kai Ni │ │ │ │ │ Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _B_a_y_e_s_T_r_e_e_-_i_n_s_t_._h │ │ │ │ │ + * _C_l_u_s_t_e_r_T_r_e_e_-_i_n_s_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00701_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTree-inst.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/ClusterTree-inst.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,680 +98,357 @@ │ │ │ │
No Matches
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
BayesTree-inst.h
│ │ │ │ +
ClusterTree-inst.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ -
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ -
4 * Atlanta, Georgia 30332-0415
│ │ │ │ -
5 * All Rights Reserved
│ │ │ │ -
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ -
7
│ │ │ │ -
8 * See LICENSE for the license information
│ │ │ │ -
9
│ │ │ │ -
10 * -------------------------------------------------------------------------- */
│ │ │ │ +Go to the documentation of this file.
1
│ │ │ │ +
10#pragma once
│ │ │ │
11
│ │ │ │ -
21#pragma once
│ │ │ │ -
22
│ │ │ │ -
23#include <gtsam/inference/Ordering.h>
│ │ │ │ -
24#include <gtsam/inference/BayesTree.h>
│ │ │ │ -
25#include <gtsam/base/treeTraversal-inst.h>
│ │ │ │ -
26#include <gtsam/base/timing.h>
│ │ │ │ -
27
│ │ │ │ -
28#include <boost/optional.hpp>
│ │ │ │ -
29#include <fstream>
│ │ │ │ -
30
│ │ │ │ -
31namespace gtsam {
│ │ │ │ -
32
│ │ │ │ -
33 /* ************************************************************************* */
│ │ │ │ -
34 template<class CLIQUE>
│ │ │ │ -
│ │ │ │ -
35 BayesTreeCliqueData BayesTree<CLIQUE>::getCliqueData() const {
│ │ │ │ -
36 BayesTreeCliqueData stats;
│ │ │ │ -
37 for (const sharedClique& root : roots_) getCliqueData(root, &stats);
│ │ │ │ -
38 return stats;
│ │ │ │ -
39 }
│ │ │ │ -
│ │ │ │ -
40
│ │ │ │ -
41 /* ************************************************************************* */
│ │ │ │ -
42 template <class CLIQUE>
│ │ │ │ -
│ │ │ │ -
43 void BayesTree<CLIQUE>::getCliqueData(sharedClique clique,
│ │ │ │ -
44 BayesTreeCliqueData* stats) const {
│ │ │ │ -
45 const auto conditional = clique->conditional();
│ │ │ │ -
46 stats->conditionalSizes.push_back(conditional->nrFrontals());
│ │ │ │ -
47 stats->separatorSizes.push_back(conditional->nrParents());
│ │ │ │ -
48 for (sharedClique c : clique->children) {
│ │ │ │ -
49 getCliqueData(c, stats);
│ │ │ │ -
50 }
│ │ │ │ -
51 }
│ │ │ │ -
│ │ │ │ -
52
│ │ │ │ -
53 /* ************************************************************************* */
│ │ │ │ -
54 template<class CLIQUE>
│ │ │ │ -
│ │ │ │ -
55 size_t BayesTree<CLIQUE>::numCachedSeparatorMarginals() const {
│ │ │ │ -
56 size_t count = 0;
│ │ │ │ -
57 for(const sharedClique& root: roots_)
│ │ │ │ -
58 count += root->numCachedSeparatorMarginals();
│ │ │ │ -
59 return count;
│ │ │ │ -
60 }
│ │ │ │ -
│ │ │ │ -
61
│ │ │ │ -
62 /* ************************************************************************* */
│ │ │ │ -
63 template <class CLIQUE>
│ │ │ │ -
│ │ │ │ -
64 void BayesTree<CLIQUE>::dot(std::ostream& os,
│ │ │ │ -
65 const KeyFormatter& keyFormatter) const {
│ │ │ │ -
66 if (roots_.empty())
│ │ │ │ -
67 throw std::invalid_argument(
│ │ │ │ -
68 "the root of Bayes tree has not been initialized!");
│ │ │ │ -
69 os << "digraph G{\n";
│ │ │ │ -
70 for (const sharedClique& root : roots_) dot(os, root, keyFormatter);
│ │ │ │ -
71 os << "}";
│ │ │ │ -
72 std::flush(os);
│ │ │ │ -
73 }
│ │ │ │ -
│ │ │ │ -
74
│ │ │ │ -
75 /* ************************************************************************* */
│ │ │ │ -
76 template <class CLIQUE>
│ │ │ │ -
│ │ │ │ -
77 std::string BayesTree<CLIQUE>::dot(const KeyFormatter& keyFormatter) const {
│ │ │ │ -
78 std::stringstream ss;
│ │ │ │ -
79 dot(ss, keyFormatter);
│ │ │ │ -
80 return ss.str();
│ │ │ │ -
81 }
│ │ │ │ -
│ │ │ │ -
82
│ │ │ │ -
83 /* ************************************************************************* */
│ │ │ │ -
84 template <class CLIQUE>
│ │ │ │ -
│ │ │ │ -
85 void BayesTree<CLIQUE>::saveGraph(const std::string& filename,
│ │ │ │ -
86 const KeyFormatter& keyFormatter) const {
│ │ │ │ -
87 std::ofstream of(filename.c_str());
│ │ │ │ -
88 dot(of, keyFormatter);
│ │ │ │ -
89 of.close();
│ │ │ │ -
90 }
│ │ │ │ -
│ │ │ │ -
91
│ │ │ │ -
92 /* ************************************************************************* */
│ │ │ │ -
93 template <class CLIQUE>
│ │ │ │ -
│ │ │ │ -
94 void BayesTree<CLIQUE>::dot(std::ostream& s, sharedClique clique,
│ │ │ │ -
95 const KeyFormatter& keyFormatter,
│ │ │ │ -
96 int parentnum) const {
│ │ │ │ -
97 static int num = 0;
│ │ │ │ -
98 bool first = true;
│ │ │ │ -
99 std::stringstream out;
│ │ │ │ -
100 out << num;
│ │ │ │ -
101 std::string parent = out.str();
│ │ │ │ -
102 parent += "[label=\"";
│ │ │ │ -
103
│ │ │ │ -
104 for (Key key : clique->conditional_->frontals()) {
│ │ │ │ -
105 if (!first) parent += ", ";
│ │ │ │ -
106 first = false;
│ │ │ │ -
107 parent += keyFormatter(key);
│ │ │ │ -
108 }
│ │ │ │ +
12#include <gtsam/inference/ClusterTree.h>
│ │ │ │ +
13#include <gtsam/inference/BayesTree.h>
│ │ │ │ +
14#include <gtsam/inference/Ordering.h>
│ │ │ │ +
15#include <gtsam/base/timing.h>
│ │ │ │ +
16#include <gtsam/base/treeTraversal-inst.h>
│ │ │ │ +
17
│ │ │ │ +
18#ifdef GTSAM_USE_TBB
│ │ │ │ +
19#include <mutex>
│ │ │ │ +
20#endif
│ │ │ │ +
21
│ │ │ │ +
22namespace gtsam {
│ │ │ │ +
23
│ │ │ │ +
24/* ************************************************************************* */
│ │ │ │ +
25template<class GRAPH>
│ │ │ │ +
│ │ │ │ +
26void ClusterTree<GRAPH>::Cluster::print(const std::string& s,
│ │ │ │ +
27 const KeyFormatter& keyFormatter) const {
│ │ │ │ +
28 std::cout << s << " (" << problemSize_ << ")";
│ │ │ │ +
29 PrintKeyVector(orderedFrontalKeys);
│ │ │ │ +
30}
│ │ │ │ +
│ │ │ │ +
31
│ │ │ │ +
32/* ************************************************************************* */
│ │ │ │ +
33template <class GRAPH>
│ │ │ │ +
│ │ │ │ +
34std::vector<size_t> ClusterTree<GRAPH>::Cluster::nrFrontalsOfChildren() const {
│ │ │ │ +
35 std::vector<size_t> nrFrontals;
│ │ │ │ +
36 nrFrontals.reserve(nrChildren());
│ │ │ │ +
37 for (const sharedNode& child : children)
│ │ │ │ +
38 nrFrontals.push_back(child->nrFrontals());
│ │ │ │ +
39 return nrFrontals;
│ │ │ │ +
40}
│ │ │ │ +
│ │ │ │ +
41
│ │ │ │ +
42/* ************************************************************************* */
│ │ │ │ +
43template <class GRAPH>
│ │ │ │ +
│ │ │ │ +
44void ClusterTree<GRAPH>::Cluster::merge(const boost::shared_ptr<Cluster>& cluster) {
│ │ │ │ +
45 // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after..
│ │ │ │ +
46 orderedFrontalKeys.insert(orderedFrontalKeys.end(), cluster->orderedFrontalKeys.rbegin(),
│ │ │ │ +
47 cluster->orderedFrontalKeys.rend());
│ │ │ │ +
48 factors.push_back(cluster->factors);
│ │ │ │ +
49 children.insert(children.end(), cluster->children.begin(), cluster->children.end());
│ │ │ │ +
50 // Increment problem size
│ │ │ │ +
51 problemSize_ = std::max(problemSize_, cluster->problemSize_);
│ │ │ │ +
52}
│ │ │ │ +
│ │ │ │ +
53
│ │ │ │ +
54/* ************************************************************************* */
│ │ │ │ +
55template<class GRAPH>
│ │ │ │ +
│ │ │ │ +
56void ClusterTree<GRAPH>::Cluster::mergeChildren(
│ │ │ │ +
57 const std::vector<bool>& merge) {
│ │ │ │ +
58 gttic(Cluster_mergeChildren);
│ │ │ │ +
59 assert(merge.size() == this->children.size());
│ │ │ │ +
60
│ │ │ │ +
61 // Count how many keys, factors and children we'll end up with
│ │ │ │ +
62 size_t nrKeys = orderedFrontalKeys.size();
│ │ │ │ +
63 size_t nrFactors = factors.size();
│ │ │ │ +
64 size_t nrNewChildren = 0;
│ │ │ │ +
65 // Loop over children
│ │ │ │ +
66 size_t i = 0;
│ │ │ │ +
67 for(const sharedNode& child: this->children) {
│ │ │ │ +
68 if (merge[i]) {
│ │ │ │ +
69 nrKeys += child->orderedFrontalKeys.size();
│ │ │ │ +
70 nrFactors += child->factors.size();
│ │ │ │ +
71 nrNewChildren += child->nrChildren();
│ │ │ │ +
72 } else {
│ │ │ │ +
73 nrNewChildren += 1; // we keep the child
│ │ │ │ +
74 }
│ │ │ │ +
75 ++i;
│ │ │ │ +
76 }
│ │ │ │ +
77
│ │ │ │ +
78 // now reserve space, and really merge
│ │ │ │ +
79 auto oldChildren = this->children;
│ │ │ │ +
80 this->children.clear();
│ │ │ │ +
81 this->children.reserve(nrNewChildren);
│ │ │ │ +
82 orderedFrontalKeys.reserve(nrKeys);
│ │ │ │ +
83 factors.reserve(nrFactors);
│ │ │ │ +
84 i = 0;
│ │ │ │ +
85 for (const sharedNode& child : oldChildren) {
│ │ │ │ +
86 if (merge[i]) {
│ │ │ │ +
87 this->merge(child);
│ │ │ │ +
88 } else {
│ │ │ │ +
89 this->addChild(child); // we keep the child
│ │ │ │ +
90 }
│ │ │ │ +
91 ++i;
│ │ │ │ +
92 }
│ │ │ │ +
│ │ │ │ +
93 std::reverse(orderedFrontalKeys.begin(), orderedFrontalKeys.end());
│ │ │ │ +
94}
│ │ │ │ +
95
│ │ │ │ +
96/* ************************************************************************* */
│ │ │ │ +
│ │ │ │ +
97template <class GRAPH>
│ │ │ │ +
│ │ │ │ +
98void ClusterTree<GRAPH>::print(const std::string& s, const KeyFormatter& keyFormatter) const {
│ │ │ │ +
99 treeTraversal::PrintForest(*this, s, keyFormatter);
│ │ │ │ +
100}
│ │ │ │ +
│ │ │ │ +
101
│ │ │ │ +
102/* ************************************************************************* */
│ │ │ │ +
│ │ │ │ +
103template <class GRAPH>
│ │ │ │ +
│ │ │ │ +
104ClusterTree<GRAPH>& ClusterTree<GRAPH>::operator=(const This& other) {
│ │ │ │ +
105 // Start by duplicating the tree.
│ │ │ │ +
106 roots_ = treeTraversal::CloneForest(other);
│ │ │ │ +
107 return *this;
│ │ │ │ +
108}
│ │ │ │ +
│ │ │ │
109
│ │ │ │ -
110 if (clique->parent()) {
│ │ │ │ -
111 parent += " : ";
│ │ │ │ -
│ │ │ │ -
112 s << parentnum << "->" << num << "\n";
│ │ │ │ -
113 }
│ │ │ │ -
114
│ │ │ │ -
115 first = true;
│ │ │ │ -
116 for (Key parentKey : clique->conditional_->parents()) {
│ │ │ │ -
│ │ │ │ -
117 if (!first) parent += ", ";
│ │ │ │ -
118 first = false;
│ │ │ │ -
119 parent += keyFormatter(parentKey);
│ │ │ │ -
120 }
│ │ │ │ -
121 parent += "\"];\n";
│ │ │ │ -
122 s << parent;
│ │ │ │ -
│ │ │ │ -
123 parentnum = num;
│ │ │ │ -
124
│ │ │ │ -
125 for (sharedClique c : clique->children) {
│ │ │ │ -
126 num++;
│ │ │ │ -
│ │ │ │ -
127 dot(s, c, keyFormatter, parentnum);
│ │ │ │ -
128 }
│ │ │ │ -
129 }
│ │ │ │ +
110/* ************************************************************************* */
│ │ │ │ +
111// Elimination traversal data - stores a pointer to the parent data and collects
│ │ │ │ +
112// the factors resulting from elimination of the children. Also sets up BayesTree
│ │ │ │ +
113// cliques with parent and child pointers.
│ │ │ │ +
114template<class CLUSTERTREE>
│ │ │ │ +
│ │ │ │ +
115struct EliminationData {
│ │ │ │ +
116 // Typedefs
│ │ │ │ +
117 typedef typename CLUSTERTREE::sharedFactor sharedFactor;
│ │ │ │ +
118 typedef typename CLUSTERTREE::FactorType FactorType;
│ │ │ │ +
119 typedef typename CLUSTERTREE::FactorGraphType FactorGraphType;
│ │ │ │ +
120 typedef typename CLUSTERTREE::ConditionalType ConditionalType;
│ │ │ │ +
121 typedef typename CLUSTERTREE::BayesTreeType::Node BTNode;
│ │ │ │ +
122
│ │ │ │ +
123 EliminationData* const parentData;
│ │ │ │ +
124 size_t myIndexInParent;
│ │ │ │ +
125 FastVector<sharedFactor> childFactors;
│ │ │ │ +
126 boost::shared_ptr<BTNode> bayesTreeNode;
│ │ │ │ +
127#ifdef GTSAM_USE_TBB
│ │ │ │ +
128 boost::shared_ptr<std::mutex> writeLock;
│ │ │ │ +
129#endif
│ │ │ │
130
│ │ │ │ -
131 /* ************************************************************************* */
│ │ │ │ -
132 template<class CLIQUE>
│ │ │ │ -
│ │ │ │ -
133 size_t BayesTree<CLIQUE>::size() const {
│ │ │ │ -
134 size_t size = 0;
│ │ │ │ -
│ │ │ │ -
135 for(const sharedClique& clique: roots_)
│ │ │ │ -
136 size += clique->treeSize();
│ │ │ │ -
137 return size;
│ │ │ │ -
138 }
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
139
│ │ │ │ -
140 /* ************************************************************************* */
│ │ │ │ -
141 template<class CLIQUE>
│ │ │ │ -
│ │ │ │ -
142 void BayesTree<CLIQUE>::addClique(const sharedClique& clique, const sharedClique& parent_clique) {
│ │ │ │ -
143 for(Key j: clique->conditional()->frontals())
│ │ │ │ -
144 nodes_[j] = clique;
│ │ │ │ -
145 if (parent_clique != nullptr) {
│ │ │ │ -
146 clique->parent_ = parent_clique;
│ │ │ │ -
147 parent_clique->children.push_back(clique);
│ │ │ │ -
148 } else {
│ │ │ │ -
149 roots_.push_back(clique);
│ │ │ │ -
150 }
│ │ │ │ -
151 }
│ │ │ │ -
│ │ │ │ -
152
│ │ │ │ -
153 /* ************************************************************************* */
│ │ │ │ -
154 namespace {
│ │ │ │ -
155 template <class FACTOR, class CLIQUE>
│ │ │ │ -
156 struct _pushCliqueFunctor {
│ │ │ │ -
157 _pushCliqueFunctor(FactorGraph<FACTOR>* graph_) : graph(graph_) {}
│ │ │ │ -
158 FactorGraph<FACTOR>* graph;
│ │ │ │ -
159 int operator()(const boost::shared_ptr<CLIQUE>& clique, int dummy) {
│ │ │ │ -
160 graph->push_back(clique->conditional_);
│ │ │ │ -
│ │ │ │ -
161 return 0;
│ │ │ │ -
162 }
│ │ │ │ -
163 };
│ │ │ │ -
│ │ │ │ -
164 } // namespace
│ │ │ │ -
165
│ │ │ │ -
166 /* ************************************************************************* */
│ │ │ │ -
167 template <class CLIQUE>
│ │ │ │ -
│ │ │ │ -
168 void BayesTree<CLIQUE>::addFactorsToGraph(
│ │ │ │ -
169 FactorGraph<FactorType>* graph) const {
│ │ │ │ -
170 // Traverse the BayesTree and add all conditionals to this graph
│ │ │ │ -
│ │ │ │ -
171 int data = 0; // Unused
│ │ │ │ -
172 _pushCliqueFunctor<FactorType, CLIQUE> functor(graph);
│ │ │ │ -
173 treeTraversal::DepthFirstForest(*this, data, functor);
│ │ │ │ -
174 }
│ │ │ │ -
175
│ │ │ │ -
176 /* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
177 template<class CLIQUE>
│ │ │ │ -
│ │ │ │ -
178 BayesTree<CLIQUE>::BayesTree(const This& other) {
│ │ │ │ -
179 *this = other;
│ │ │ │ -
180 }
│ │ │ │ -
│ │ │ │ -
181
│ │ │ │ -
182 /* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
183 namespace {
│ │ │ │ -
184 template<typename NODE>
│ │ │ │ -
185 boost::shared_ptr<NODE>
│ │ │ │ -
186 BayesTreeCloneForestVisitorPre(const boost::shared_ptr<NODE>& node, const boost::shared_ptr<NODE>& parentPointer)
│ │ │ │ -
187 {
│ │ │ │ -
188 // Clone the current node and add it to its cloned parent
│ │ │ │ -
│ │ │ │ -
189 boost::shared_ptr<NODE> clone = boost::make_shared<NODE>(*node);
│ │ │ │ -
190 clone->children.clear();
│ │ │ │ -
191 clone->parent_ = parentPointer;
│ │ │ │ -
│ │ │ │ -
192 parentPointer->children.push_back(clone);
│ │ │ │ -
193 return clone;
│ │ │ │ -
194 }
│ │ │ │ -
195 }
│ │ │ │ -
│ │ │ │ -
196
│ │ │ │ -
197 /* ************************************************************************* */
│ │ │ │ -
198 template<class CLIQUE>
│ │ │ │ -
│ │ │ │ -
199 BayesTree<CLIQUE>& BayesTree<CLIQUE>::operator=(const This& other) {
│ │ │ │ -
200 this->clear();
│ │ │ │ -
201 boost::shared_ptr<Clique> rootContainer = boost::make_shared<Clique>();
│ │ │ │ -
202 treeTraversal::DepthFirstForest(other, rootContainer, BayesTreeCloneForestVisitorPre<Clique>);
│ │ │ │ -
203 for(const sharedClique& root: rootContainer->children) {
│ │ │ │ -
204 root->parent_ = typename Clique::weak_ptr(); // Reset the parent since it's set to the dummy clique
│ │ │ │ -
205 insertRoot(root);
│ │ │ │ -
206 }
│ │ │ │ -
207 return *this;
│ │ │ │ -
208 }
│ │ │ │ -
│ │ │ │ -
209
│ │ │ │ -
210 /* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
211 template<class CLIQUE>
│ │ │ │ -
│ │ │ │ -
212 void BayesTree<CLIQUE>::print(const std::string& s, const KeyFormatter& keyFormatter) const {
│ │ │ │ -
213 std::cout << s << ": cliques: " << size() << ", variables: " << nodes_.size() << std::endl;
│ │ │ │ -
│ │ │ │ -
214 treeTraversal::PrintForest(*this, s, keyFormatter);
│ │ │ │ -
215 }
│ │ │ │ -
216
│ │ │ │ -
217 /* ************************************************************************* */
│ │ │ │ -
218 // binary predicate to test equality of a pair for use in equals
│ │ │ │ -
219 template<class CLIQUE>
│ │ │ │ -
│ │ │ │ -
220 bool check_sharedCliques(
│ │ │ │ -
221 const std::pair<Key, typename BayesTree<CLIQUE>::sharedClique>& v1,
│ │ │ │ -
222 const std::pair<Key, typename BayesTree<CLIQUE>::sharedClique>& v2
│ │ │ │ -
223 ) {
│ │ │ │ -
224 return v1.first == v2.first &&
│ │ │ │ -
225 ((!v1.second && !v2.second) || (v1.second && v2.second && v1.second->equals(*v2.second)));
│ │ │ │ -
│ │ │ │ -
226 }
│ │ │ │ -
227
│ │ │ │ -
228 /* ************************************************************************* */
│ │ │ │ -
229 template<class CLIQUE>
│ │ │ │ -
│ │ │ │ -
230 bool BayesTree<CLIQUE>::equals(const BayesTree<CLIQUE>& other, double tol) const {
│ │ │ │ -
231 return size()==other.size() &&
│ │ │ │ -
232 std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques<CLIQUE>);
│ │ │ │ -
233 }
│ │ │ │ -
│ │ │ │ +
131 EliminationData(EliminationData* _parentData, size_t nChildren) :
│ │ │ │ +
132 parentData(_parentData), bayesTreeNode(boost::make_shared<BTNode>())
│ │ │ │ +
133#ifdef GTSAM_USE_TBB
│ │ │ │ +
134 , writeLock(boost::make_shared<std::mutex>())
│ │ │ │ +
135#endif
│ │ │ │ +
136 {
│ │ │ │ +
137 if (parentData) {
│ │ │ │ +
│ │ │ │ +
138#ifdef GTSAM_USE_TBB
│ │ │ │ +
139 parentData->writeLock->lock();
│ │ │ │ +
140#endif
│ │ │ │ +
141 myIndexInParent = parentData->childFactors.size();
│ │ │ │ +
142 parentData->childFactors.push_back(sharedFactor());
│ │ │ │ +
143#ifdef GTSAM_USE_TBB
│ │ │ │ +
144 parentData->writeLock->unlock();
│ │ │ │ +
145#endif
│ │ │ │ +
146 } else {
│ │ │ │ +
147 myIndexInParent = 0;
│ │ │ │ +
148 }
│ │ │ │ +
149 // Set up BayesTree parent and child pointers
│ │ │ │ +
150 if (parentData) {
│ │ │ │ +
151 if (parentData->parentData) // If our parent is not the dummy node
│ │ │ │ +
152 bayesTreeNode->parent_ = parentData->bayesTreeNode;
│ │ │ │ +
153 parentData->bayesTreeNode->children.push_back(bayesTreeNode);
│ │ │ │ +
154 }
│ │ │ │ +
155 }
│ │ │ │ +
156
│ │ │ │ +
157 // Elimination pre-order visitor - creates the EliminationData structure for the visited node.
│ │ │ │ +
158 static EliminationData EliminationPreOrderVisitor(
│ │ │ │ +
159 const typename CLUSTERTREE::sharedNode& node,
│ │ │ │ +
160 EliminationData& parentData) {
│ │ │ │ +
161 assert(node);
│ │ │ │ +
162 EliminationData myData(&parentData, node->nrChildren());
│ │ │ │ +
163 myData.bayesTreeNode->problemSize_ = node->problemSize();
│ │ │ │ +
164 return myData;
│ │ │ │ +
165 }
│ │ │ │ +
166
│ │ │ │ +
167 // Elimination post-order visitor - combine the child factors with our own factors, add the
│ │ │ │ +
168 // resulting conditional to the BayesTree, and add the remaining factor to the parent.
│ │ │ │ +
│ │ │ │ +
169 class EliminationPostOrderVisitor {
│ │ │ │ +
170 const typename CLUSTERTREE::Eliminate& eliminationFunction_;
│ │ │ │ +
171 typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex_;
│ │ │ │ +
172
│ │ │ │ +
173 public:
│ │ │ │ +
174 // Construct functor
│ │ │ │ +
│ │ │ │ +
175 EliminationPostOrderVisitor(
│ │ │ │ +
176 const typename CLUSTERTREE::Eliminate& eliminationFunction,
│ │ │ │ +
177 typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex) :
│ │ │ │ +
178 eliminationFunction_(eliminationFunction), nodesIndex_(nodesIndex) {
│ │ │ │ +
179 }
│ │ │ │ +
180
│ │ │ │ +
181 // Function that does the HEAVY lifting
│ │ │ │ +
182 void operator()(const typename CLUSTERTREE::sharedNode& node, EliminationData& myData) {
│ │ │ │ +
183 assert(node);
│ │ │ │ +
184
│ │ │ │ +
185 // Gather factors
│ │ │ │ +
186 FactorGraphType gatheredFactors;
│ │ │ │ +
187 gatheredFactors.reserve(node->factors.size() + node->nrChildren());
│ │ │ │ +
188 gatheredFactors += node->factors;
│ │ │ │ +
189 gatheredFactors += myData.childFactors;
│ │ │ │ +
190
│ │ │ │ +
191 // Check for Bayes tree orphan subtrees, and add them to our children
│ │ │ │ +
192 // TODO(frank): should this really happen here?
│ │ │ │ +
193 for (const sharedFactor& factor: node->factors) {
│ │ │ │ +
194 auto asSubtree = dynamic_cast<const BayesTreeOrphanWrapper<BTNode>*>(factor.get());
│ │ │ │ +
195 if (asSubtree) {
│ │ │ │ +
196 myData.bayesTreeNode->children.push_back(asSubtree->clique);
│ │ │ │ +
197 asSubtree->clique->parent_ = myData.bayesTreeNode;
│ │ │ │ +
198 }
│ │ │ │ +
199 }
│ │ │ │ +
200
│ │ │ │ +
201 // >>>>>>>>>>>>>> Do dense elimination step >>>>>>>>>>>>>>>>>>>>>>>>>>>>>
│ │ │ │ +
202 auto eliminationResult = eliminationFunction_(gatheredFactors, node->orderedFrontalKeys);
│ │ │ │ +
203 // <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
│ │ │ │ +
204
│ │ │ │ +
205 // Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the
│ │ │ │ +
206 // remaining factor
│ │ │ │ +
207 myData.bayesTreeNode->setEliminationResult(eliminationResult);
│ │ │ │ +
208
│ │ │ │ +
209 // Fill nodes index - we do this here instead of calling insertRoot at the end to avoid
│ │ │ │ +
210 // putting orphan subtrees in the index - they'll already be in the index of the ISAM2
│ │ │ │ +
211 // object they're added to.
│ │ │ │ +
212 for (const Key& j: myData.bayesTreeNode->conditional()->frontals())
│ │ │ │ +
213 nodesIndex_.insert(std::make_pair(j, myData.bayesTreeNode));
│ │ │ │ +
214
│ │ │ │ +
215 // Store remaining factor in parent's gathered factors
│ │ │ │ +
216 if (!eliminationResult.second->empty()) {
│ │ │ │ +
217#ifdef GTSAM_USE_TBB
│ │ │ │ +
218 myData.parentData->writeLock->lock();
│ │ │ │ +
219#endif
│ │ │ │ +
220 myData.parentData->childFactors[myData.myIndexInParent] = eliminationResult.second;
│ │ │ │ +
221#ifdef GTSAM_USE_TBB
│ │ │ │ +
│ │ │ │ +
222 myData.parentData->writeLock->unlock();
│ │ │ │ +
223#endif
│ │ │ │ +
224 }
│ │ │ │ +
225 }
│ │ │ │ +
226 };
│ │ │ │ +
227};
│ │ │ │ +
228
│ │ │ │ +
229/* ************************************************************************* */
│ │ │ │ +
230template<class BAYESTREE, class GRAPH>
│ │ │ │ +
│ │ │ │ +
231EliminatableClusterTree<BAYESTREE, GRAPH>& EliminatableClusterTree<BAYESTREE, GRAPH>::operator=(
│ │ │ │ +
232 const This& other) {
│ │ │ │ +
233 ClusterTree<GRAPH>::operator=(other);
│ │ │ │
234
│ │ │ │ -
│ │ │ │ -
235 /* ************************************************************************* */
│ │ │ │ -
236 template<class CLIQUE>
│ │ │ │ -
237 template<class CONTAINER>
│ │ │ │ -
│ │ │ │ -
238 Key BayesTree<CLIQUE>::findParentClique(const CONTAINER& parents) const {
│ │ │ │ -
239 typename CONTAINER::const_iterator lowestOrderedParent = min_element(parents.begin(), parents.end());
│ │ │ │ -
240 assert(lowestOrderedParent != parents.end());
│ │ │ │ -
│ │ │ │ -
241 return *lowestOrderedParent;
│ │ │ │ -
242 }
│ │ │ │ -
243
│ │ │ │ -
244 /* ************************************************************************* */
│ │ │ │ -
245 template<class CLIQUE>
│ │ │ │ -
│ │ │ │ -
246 void BayesTree<CLIQUE>::fillNodesIndex(const sharedClique& subtree) {
│ │ │ │ -
247 // Add each frontal variable of this root node
│ │ │ │ -
248 for(const Key& j: subtree->conditional()->frontals()) {
│ │ │ │ -
249 bool inserted = nodes_.insert(std::make_pair(j, subtree)).second;
│ │ │ │ -
│ │ │ │ -
250 assert(inserted); (void)inserted;
│ │ │ │ -
251 }
│ │ │ │ -
252 // Fill index for each child
│ │ │ │ -
│ │ │ │ -
253 typedef typename BayesTree<CLIQUE>::sharedClique sharedClique;
│ │ │ │ -
254 for(const sharedClique& child: subtree->children) {
│ │ │ │ -
255 fillNodesIndex(child); }
│ │ │ │ -
256 }
│ │ │ │ -
257
│ │ │ │ -
258 /* ************************************************************************* */
│ │ │ │ -
259 template<class CLIQUE>
│ │ │ │ -
│ │ │ │ -
260 void BayesTree<CLIQUE>::insertRoot(const sharedClique& subtree) {
│ │ │ │ -
261 roots_.push_back(subtree); // Add to roots
│ │ │ │ -
262 fillNodesIndex(subtree); // Populate nodes index
│ │ │ │ -
263 }
│ │ │ │ -
│ │ │ │ -
264
│ │ │ │ -
265 /* ************************************************************************* */
│ │ │ │ -
266 // First finds clique marginal then marginalizes that
│ │ │ │ -
267 /* ************************************************************************* */
│ │ │ │ -
268 template<class CLIQUE>
│ │ │ │ -
269 typename BayesTree<CLIQUE>::sharedConditional
│ │ │ │ -
│ │ │ │ -
270 BayesTree<CLIQUE>::marginalFactor(Key j, const Eliminate& function) const
│ │ │ │ -
271 {
│ │ │ │ -
272 gttic(BayesTree_marginalFactor);
│ │ │ │ -
273
│ │ │ │ -
274 // get clique containing Key j
│ │ │ │ -
275 sharedClique clique = this->clique(j);
│ │ │ │ -
276
│ │ │ │ -
277 // calculate or retrieve its marginal P(C) = P(F,S)
│ │ │ │ -
278 FactorGraphType cliqueMarginal = clique->marginal2(function);
│ │ │ │ -
279
│ │ │ │ -
280 // Now, marginalize out everything that is not variable j
│ │ │ │ -
281 BayesNetType marginalBN =
│ │ │ │ -
282 *cliqueMarginal.marginalMultifrontalBayesNet(Ordering{j}, function);
│ │ │ │ -
283
│ │ │ │ -
284 // The Bayes net should contain only one conditional for variable j, so return it
│ │ │ │ -
285 return marginalBN.front();
│ │ │ │ -
286 }
│ │ │ │ -
│ │ │ │ -
287
│ │ │ │ -
288 /* ************************************************************************* */
│ │ │ │ -
289 // Find two cliques, their joint, then marginalizes
│ │ │ │ -
290 /* ************************************************************************* */
│ │ │ │ -
291 template<class CLIQUE>
│ │ │ │ -
292 typename BayesTree<CLIQUE>::sharedFactorGraph
│ │ │ │ -
│ │ │ │ -
293 BayesTree<CLIQUE>::joint(Key j1, Key j2, const Eliminate& function) const
│ │ │ │ -
294 {
│ │ │ │ -
295 gttic(BayesTree_joint);
│ │ │ │ -
296 return boost::make_shared<FactorGraphType>(*jointBayesNet(j1, j2, function));
│ │ │ │ -
297 }
│ │ │ │ -
│ │ │ │ -
298
│ │ │ │ -
299 /* ************************************************************************* */
│ │ │ │ -
300 template<class CLIQUE>
│ │ │ │ -
301 typename BayesTree<CLIQUE>::sharedBayesNet
│ │ │ │ -
│ │ │ │ -
302 BayesTree<CLIQUE>::jointBayesNet(Key j1, Key j2, const Eliminate& function) const
│ │ │ │ -
303 {
│ │ │ │ -
304 gttic(BayesTree_jointBayesNet);
│ │ │ │ -
305 // get clique C1 and C2
│ │ │ │ -
306 sharedClique C1 = (*this)[j1], C2 = (*this)[j2];
│ │ │ │ -
307
│ │ │ │ -
308 gttic(Lowest_common_ancestor);
│ │ │ │ -
309 // Find lowest common ancestor clique
│ │ │ │ -
310 sharedClique B; {
│ │ │ │ -
311 // Build two paths to the root
│ │ │ │ -
312 FastList<sharedClique> path1, path2; {
│ │ │ │ -
313 sharedClique p = C1;
│ │ │ │ -
314 while(p) {
│ │ │ │ -
315 path1.push_front(p);
│ │ │ │ -
316 p = p->parent();
│ │ │ │ -
317 }
│ │ │ │ -
318 } {
│ │ │ │ -
319 sharedClique p = C2;
│ │ │ │ -
320 while(p) {
│ │ │ │ -
321 path2.push_front(p);
│ │ │ │ -
322 p = p->parent();
│ │ │ │ -
323 }
│ │ │ │ -
324 }
│ │ │ │ -
325 // Find the path intersection
│ │ │ │ -
326 typename FastList<sharedClique>::const_iterator p1 = path1.begin(), p2 = path2.begin();
│ │ │ │ -
327 if(*p1 == *p2)
│ │ │ │ -
328 B = *p1;
│ │ │ │ -
329 while(p1 != path1.end() && p2 != path2.end() && *p1 == *p2) {
│ │ │ │ -
330 B = *p1;
│ │ │ │ -
331 ++p1;
│ │ │ │ -
332 ++p2;
│ │ │ │ -
333 }
│ │ │ │ -
334 }
│ │ │ │ -
335 gttoc(Lowest_common_ancestor);
│ │ │ │ -
336
│ │ │ │ -
337 // Build joint on all involved variables
│ │ │ │ -
338 FactorGraphType p_BC1C2;
│ │ │ │ -
339
│ │ │ │ -
340 if(B)
│ │ │ │ -
341 {
│ │ │ │ -
342 // Compute marginal on lowest common ancestor clique
│ │ │ │ -
343 gttic(LCA_marginal);
│ │ │ │ -
344 FactorGraphType p_B = B->marginal2(function);
│ │ │ │ -
345 gttoc(LCA_marginal);
│ │ │ │ -
346
│ │ │ │ -
347 // Compute shortcuts of the requested cliques given the lowest common ancestor
│ │ │ │ -
348 gttic(Clique_shortcuts);
│ │ │ │ -
349 BayesNetType p_C1_Bred = C1->shortcut(B, function);
│ │ │ │ -
350 BayesNetType p_C2_Bred = C2->shortcut(B, function);
│ │ │ │ -
351 gttoc(Clique_shortcuts);
│ │ │ │ -
352
│ │ │ │ -
353 // Factor the shortcuts to be conditioned on the full root
│ │ │ │ -
354 // Get the set of variables to eliminate, which is C1\B.
│ │ │ │ -
355 gttic(Full_root_factoring);
│ │ │ │ -
356 boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C1_B; {
│ │ │ │ -
357 KeyVector C1_minus_B; {
│ │ │ │ -
358 KeySet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents());
│ │ │ │ -
359 for(const Key j: *B->conditional()) {
│ │ │ │ -
360 C1_minus_B_set.erase(j); }
│ │ │ │ -
361 C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end());
│ │ │ │ -
362 }
│ │ │ │ -
363 // Factor into C1\B | B.
│ │ │ │ -
364 sharedFactorGraph temp_remaining;
│ │ │ │ -
365 boost::tie(p_C1_B, temp_remaining) =
│ │ │ │ -
366 FactorGraphType(p_C1_Bred).eliminatePartialMultifrontal(Ordering(C1_minus_B), function);
│ │ │ │ -
367 }
│ │ │ │ -
368 boost::shared_ptr<typename EliminationTraitsType::BayesTreeType> p_C2_B; {
│ │ │ │ -
369 KeyVector C2_minus_B; {
│ │ │ │ -
370 KeySet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents());
│ │ │ │ -
371 for(const Key j: *B->conditional()) {
│ │ │ │ -
372 C2_minus_B_set.erase(j); }
│ │ │ │ -
373 C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end());
│ │ │ │ -
374 }
│ │ │ │ -
375 // Factor into C2\B | B.
│ │ │ │ -
376 sharedFactorGraph temp_remaining;
│ │ │ │ -
377 boost::tie(p_C2_B, temp_remaining) =
│ │ │ │ -
378 FactorGraphType(p_C2_Bred).eliminatePartialMultifrontal(Ordering(C2_minus_B), function);
│ │ │ │ -
379 }
│ │ │ │ -
380 gttoc(Full_root_factoring);
│ │ │ │ -
381
│ │ │ │ -
382 gttic(Variable_joint);
│ │ │ │ -
383 p_BC1C2 += p_B;
│ │ │ │ -
384 p_BC1C2 += *p_C1_B;
│ │ │ │ -
385 p_BC1C2 += *p_C2_B;
│ │ │ │ -
386 if(C1 != B)
│ │ │ │ -
387 p_BC1C2 += C1->conditional();
│ │ │ │ -
388 if(C2 != B)
│ │ │ │ -
389 p_BC1C2 += C2->conditional();
│ │ │ │ -
390 gttoc(Variable_joint);
│ │ │ │ -
391 }
│ │ │ │ -
392 else
│ │ │ │ -
393 {
│ │ │ │ -
394 // The nodes have no common ancestor, they're in different trees, so they're joint is just the
│ │ │ │ -
395 // product of their marginals.
│ │ │ │ -
396 gttic(Disjoint_marginals);
│ │ │ │ -
397 p_BC1C2 += C1->marginal2(function);
│ │ │ │ -
398 p_BC1C2 += C2->marginal2(function);
│ │ │ │ -
399 gttoc(Disjoint_marginals);
│ │ │ │ -
400 }
│ │ │ │ -
401
│ │ │ │ -
402 // now, marginalize out everything that is not variable j1 or j2
│ │ │ │ -
403 return p_BC1C2.marginalMultifrontalBayesNet(Ordering{j1, j2}, function);
│ │ │ │ -
404 }
│ │ │ │ -
│ │ │ │ -
405
│ │ │ │ -
406 /* ************************************************************************* */
│ │ │ │ -
407 template<class CLIQUE>
│ │ │ │ -
│ │ │ │ -
408 void BayesTree<CLIQUE>::clear() {
│ │ │ │ -
409 // Remove all nodes and clear the root pointer
│ │ │ │ -
410 nodes_.clear();
│ │ │ │ -
411 roots_.clear();
│ │ │ │ -
412 }
│ │ │ │ -
│ │ │ │ -
413
│ │ │ │ -
414 /* ************************************************************************* */
│ │ │ │ -
415 template<class CLIQUE>
│ │ │ │ -
│ │ │ │ -
416 void BayesTree<CLIQUE>::deleteCachedShortcuts() {
│ │ │ │ -
417 for(const sharedClique& root: roots_) {
│ │ │ │ -
418 root->deleteCachedShortcuts();
│ │ │ │ -
419 }
│ │ │ │ -
420 }
│ │ │ │ -
│ │ │ │ -
421
│ │ │ │ -
422 /* ************************************************************************* */
│ │ │ │ -
423 template<class CLIQUE>
│ │ │ │ -
│ │ │ │ -
424 void BayesTree<CLIQUE>::removeClique(sharedClique clique)
│ │ │ │ -
425 {
│ │ │ │ -
426 if (clique->isRoot()) {
│ │ │ │ -
427 typename Roots::iterator root = std::find(roots_.begin(), roots_.end(), clique);
│ │ │ │ -
428 if(root != roots_.end())
│ │ │ │ -
429 roots_.erase(root);
│ │ │ │ -
430 } else { // detach clique from parent
│ │ │ │ -
431 sharedClique parent = clique->parent_.lock();
│ │ │ │ -
432 typename Roots::iterator child = std::find(parent->children.begin(), parent->children.end(), clique);
│ │ │ │ -
433 assert(child != parent->children.end());
│ │ │ │ -
434 parent->children.erase(child);
│ │ │ │ -
435 }
│ │ │ │ -
436
│ │ │ │ -
437 // orphan my children
│ │ │ │ -
438 for(sharedClique child: clique->children)
│ │ │ │ -
439 child->parent_ = typename Clique::weak_ptr();
│ │ │ │ -
440
│ │ │ │ -
441 for(Key j: clique->conditional()->frontals()) {
│ │ │ │ -
442 nodes_.unsafe_erase(j);
│ │ │ │ -
443 }
│ │ │ │ -
444 }
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
445
│ │ │ │ -
446 /* ************************************************************************* */
│ │ │ │ -
447 template <class CLIQUE>
│ │ │ │ -
│ │ │ │ -
448 void BayesTree<CLIQUE>::removePath(sharedClique clique, BayesNetType* bn,
│ │ │ │ -
449 Cliques* orphans) {
│ │ │ │ -
450 // base case is nullptr, if so we do nothing and return empties above
│ │ │ │ -
451 if (clique) {
│ │ │ │ -
452 // remove the clique from orphans in case it has been added earlier
│ │ │ │ -
453 orphans->remove(clique);
│ │ │ │ -
454
│ │ │ │ -
455 // remove me
│ │ │ │ -
456 this->removeClique(clique);
│ │ │ │ -
457
│ │ │ │ -
458 // remove path above me
│ │ │ │ -
459 this->removePath(typename Clique::shared_ptr(clique->parent_.lock()), bn,
│ │ │ │ -
460 orphans);
│ │ │ │ -
461
│ │ │ │ -
462 // add children to list of orphans (splice also removed them from
│ │ │ │ -
463 // clique->children_)
│ │ │ │ -
464 orphans->insert(orphans->begin(), clique->children.begin(),
│ │ │ │ -
465 clique->children.end());
│ │ │ │ -
466 clique->children.clear();
│ │ │ │ -
467
│ │ │ │ -
468 bn->push_back(clique->conditional_);
│ │ │ │ -
469 }
│ │ │ │ -
470 }
│ │ │ │ -
│ │ │ │ -
471
│ │ │ │ -
472 /* *************************************************************************
│ │ │ │ -
473 */
│ │ │ │ -
474 template <class CLIQUE>
│ │ │ │ -
│ │ │ │ -
475 void BayesTree<CLIQUE>::removeTop(const KeyVector& keys, BayesNetType* bn,
│ │ │ │ -
476 Cliques* orphans) {
│ │ │ │ -
477 gttic(removetop);
│ │ │ │ -
478 // process each key of the new factor
│ │ │ │ -
479 for (const Key& j : keys) {
│ │ │ │ -
480 // get the clique
│ │ │ │ -
481 // TODO(frank): Nodes will be searched again in removeClique
│ │ │ │ -
482 typename Nodes::const_iterator node = nodes_.find(j);
│ │ │ │ -
483 if (node != nodes_.end()) {
│ │ │ │ -
484 // remove path from clique to root
│ │ │ │ -
485 this->removePath(node->second, bn, orphans);
│ │ │ │ -
486 }
│ │ │ │ -
487 }
│ │ │ │ -
488
│ │ │ │ -
489 // Delete cachedShortcuts for each orphan subtree
│ │ │ │ -
490 // TODO(frank): Consider Improving
│ │ │ │ -
491 for (sharedClique& orphan : *orphans) orphan->deleteCachedShortcuts();
│ │ │ │ -
492 }
│ │ │ │ -
│ │ │ │ -
493
│ │ │ │ -
494 /* ************************************************************************* */
│ │ │ │ -
495 template<class CLIQUE>
│ │ │ │ -
│ │ │ │ -
496 typename BayesTree<CLIQUE>::Cliques BayesTree<CLIQUE>::removeSubtree(
│ │ │ │ -
497 const sharedClique& subtree)
│ │ │ │ -
498 {
│ │ │ │ -
499 // Result clique list
│ │ │ │ -
500 Cliques cliques;
│ │ │ │ -
501 cliques.push_back(subtree);
│ │ │ │ -
502
│ │ │ │ -
503 // Remove the first clique from its parents
│ │ │ │ -
504 if(!subtree->isRoot())
│ │ │ │ -
505 subtree->parent()->children.erase(std::find(
│ │ │ │ -
506 subtree->parent()->children.begin(), subtree->parent()->children.end(), subtree));
│ │ │ │ -
507 else
│ │ │ │ -
508 roots_.erase(std::find(roots_.begin(), roots_.end(), subtree));
│ │ │ │ -
509
│ │ │ │ -
510 // Add all subtree cliques and erase the children and parent of each
│ │ │ │ -
511 for(typename Cliques::iterator clique = cliques.begin(); clique != cliques.end(); ++clique)
│ │ │ │ -
512 {
│ │ │ │ -
513 // Add children
│ │ │ │ -
514 for(const sharedClique& child: (*clique)->children) {
│ │ │ │ -
515 cliques.push_back(child); }
│ │ │ │ -
516
│ │ │ │ -
517 // Delete cached shortcuts
│ │ │ │ -
518 (*clique)->deleteCachedShortcutsNonRecursive();
│ │ │ │ -
519
│ │ │ │ -
520 // Remove this node from the nodes index
│ │ │ │ -
521 for(Key j: (*clique)->conditional()->frontals()) {
│ │ │ │ -
522 nodes_.unsafe_erase(j); }
│ │ │ │ -
523
│ │ │ │ -
524 // Erase the parent and children pointers
│ │ │ │ -
525 (*clique)->parent_.reset();
│ │ │ │ -
526 (*clique)->children.clear();
│ │ │ │ -
527 }
│ │ │ │ -
528
│ │ │ │ -
529 return cliques;
│ │ │ │ -
530 }
│ │ │ │ -
│ │ │ │ -
531
│ │ │ │ -
532}
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ +
235 // Assign the remaining factors - these are pointers to factors in the original factor graph and
│ │ │ │ +
236 // we do not clone them.
│ │ │ │ +
237 remainingFactors_ = other.remainingFactors_;
│ │ │ │ +
238
│ │ │ │ +
239 return *this;
│ │ │ │ +
240}
│ │ │ │ +
│ │ │ │ +
241
│ │ │ │ +
242/* ************************************************************************* */
│ │ │ │ +
│ │ │ │ +
243template <class BAYESTREE, class GRAPH>
│ │ │ │ +
244std::pair<boost::shared_ptr<BAYESTREE>, boost::shared_ptr<GRAPH> >
│ │ │ │ +
│ │ │ │ +
245EliminatableClusterTree<BAYESTREE, GRAPH>::eliminate(const Eliminate& function) const {
│ │ │ │ +
246 gttic(ClusterTree_eliminate);
│ │ │ │ +
247 // Do elimination (depth-first traversal). The rootsContainer stores a 'dummy' BayesTree node
│ │ │ │ +
248 // that contains all of the roots as its children. rootsContainer also stores the remaining
│ │ │ │ +
249 // un-eliminated factors passed up from the roots.
│ │ │ │ +
250 boost::shared_ptr<BayesTreeType> result = boost::make_shared<BayesTreeType>();
│ │ │ │ +
251
│ │ │ │ +
252 typedef EliminationData<This> Data;
│ │ │ │ +
253 Data rootsContainer(0, this->nrRoots());
│ │ │ │ +
254
│ │ │ │ +
255 typename Data::EliminationPostOrderVisitor visitorPost(function, result->nodes_);
│ │ │ │ +
256 {
│ │ │ │ +
257 TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP
│ │ │ │ +
258 treeTraversal::DepthFirstForestParallel(*this, rootsContainer, Data::EliminationPreOrderVisitor,
│ │ │ │ +
259 visitorPost, 10);
│ │ │ │ +
260 }
│ │ │ │ +
261
│ │ │ │ +
262 // Create BayesTree from roots stored in the dummy BayesTree node.
│ │ │ │ +
263 result->roots_.insert(result->roots_.end(), rootsContainer.bayesTreeNode->children.begin(),
│ │ │ │ +
264 rootsContainer.bayesTreeNode->children.end());
│ │ │ │ +
265
│ │ │ │ +
266 // Add remaining factors that were not involved with eliminated variables
│ │ │ │ +
267 boost::shared_ptr<FactorGraphType> remaining = boost::make_shared<FactorGraphType>();
│ │ │ │ +
268 remaining->reserve(remainingFactors_.size() + rootsContainer.childFactors.size());
│ │ │ │ +
269 remaining->push_back(remainingFactors_.begin(), remainingFactors_.end());
│ │ │ │ +
270 for (const sharedFactor& factor : rootsContainer.childFactors) {
│ │ │ │ +
271 if (factor)
│ │ │ │ +
272 remaining->push_back(factor);
│ │ │ │ +
273 }
│ │ │ │ +
274
│ │ │ │ +
275 // Return result
│ │ │ │ +
276 return std::make_pair(result, remaining);
│ │ │ │ +
277}
│ │ │ │ +
│ │ │ │ +
278
│ │ │ │ +
279} // namespace gtsam
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -
timing.h
Timing utilities.
│ │ │ │ -
BayesTree.h
Bayes Tree is a tree of cliques of a Bayes Chain.
│ │ │ │ -
Ordering.h
Variable ordering for the elimination algorithm.
│ │ │ │ + │ │ │ │ +
timing.h
Timing utilities.
│ │ │ │ +
Ordering.h
Variable ordering for the elimination algorithm.
│ │ │ │ +
ClusterTree.h
Collects factorgraph fragments defined on variable clusters, arranged in a tree.
│ │ │ │ +
BayesTree.h
Bayes Tree is a tree of cliques of a Bayes Chain.
│ │ │ │ +
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::PrintKeyVector
void PrintKeyVector(const KeyVector &keys, const string &s, const KeyFormatter &keyFormatter)
Utility function to print sets of keys with optional prefix.
Definition Key.cpp:77
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::treeTraversal::DepthFirstForest
void DepthFirstForest(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, VISITOR_POST &visitorPost)
Traverse a forest depth-first with pre-order and post-order visits.
Definition treeTraversal-inst.h:77
│ │ │ │ +
gtsam::treeTraversal::CloneForest
FastVector< boost::shared_ptr< typename FOREST::Node > > CloneForest(const FOREST &forest)
Clone a tree, copy-constructing new nodes (calling boost::make_shared) and setting up child pointers ...
Definition treeTraversal-inst.h:189
│ │ │ │
gtsam::treeTraversal::PrintForest
void PrintForest(const FOREST &forest, std::string str, const KeyFormatter &keyFormatter)
Print a tree, prefixing each line with str, and formatting keys using keyFormatter.
Definition treeTraversal-inst.h:219
│ │ │ │ -
gtsam::FastList
FastList is a thin wrapper around std::list that uses the boost fast_pool_allocator instead of the de...
Definition FastList.h:40
│ │ │ │ - │ │ │ │ -
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ -
gtsam::BayesTreeCliqueData
store all the sizes
Definition BayesTree.h:48
│ │ │ │ -
gtsam::BayesTree
Bayes tree.
Definition BayesTree.h:67
│ │ │ │ -
gtsam::BayesTree::nodes_
Nodes nodes_
Map from indices to Clique.
Definition BayesTree.h:100
│ │ │ │ -
gtsam::BayesTree::removeClique
void removeClique(sharedClique clique)
remove a clique: warning, can result in a forest
Definition BayesTree-inst.h:424
│ │ │ │ -
gtsam::BayesTree::joint
sharedFactorGraph joint(Key j1, Key j2, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
return joint on two variables Limitation: can only calculate joint if cliques are disjoint or one of ...
Definition BayesTree-inst.h:293
│ │ │ │ -
gtsam::BayesTree::fillNodesIndex
void fillNodesIndex(const sharedClique &subtree)
Fill the nodes index for a subtree.
Definition BayesTree-inst.h:246
│ │ │ │ -
gtsam::BayesTree::dot
void dot(std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Output to graphviz format, stream version.
Definition BayesTree-inst.h:64
│ │ │ │ -
gtsam::BayesTree::addFactorsToGraph
void addFactorsToGraph(FactorGraph< FactorType > *graph) const
Add all cliques in this BayesTree to the specified factor graph.
Definition BayesTree-inst.h:168
│ │ │ │ -
gtsam::BayesTree::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition BayesTree-inst.h:230
│ │ │ │ -
gtsam::BayesTree::operator=
This & operator=(const This &other)
Assignment operator.
Definition BayesTree-inst.h:199
│ │ │ │ -
gtsam::BayesTree::sharedClique
boost::shared_ptr< Clique > sharedClique
Shared pointer to a clique.
Definition BayesTree.h:74
│ │ │ │ -
gtsam::BayesTree::BayesTree
BayesTree()
Create an empty Bayes Tree.
Definition BayesTree.h:109
│ │ │ │ -
gtsam::BayesTree::clear
void clear()
Remove all nodes.
Definition BayesTree-inst.h:408
│ │ │ │ -
gtsam::BayesTree::addClique
void addClique(const sharedClique &clique, const sharedClique &parent_clique=sharedClique())
add a clique (top down)
Definition BayesTree-inst.h:142
│ │ │ │ -
gtsam::BayesTree::jointBayesNet
sharedBayesNet jointBayesNet(Key j1, Key j2, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
return joint on two variables as a BayesNet Limitation: can only calculate joint if cliques are disjo...
Definition BayesTree-inst.h:302
│ │ │ │ -
gtsam::BayesTree::findParentClique
Key findParentClique(const CONTAINER &parents) const
Find parent clique of a conditional.
Definition BayesTree-inst.h:238
│ │ │ │ -
gtsam::BayesTree::size
size_t size() const
number of cliques
Definition BayesTree-inst.h:133
│ │ │ │ -
gtsam::BayesTree::deleteCachedShortcuts
void deleteCachedShortcuts()
Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data.
Definition BayesTree-inst.h:416
│ │ │ │ -
gtsam::BayesTree::removePath
void removePath(sharedClique clique, BayesNetType *bn, Cliques *orphans)
Remove path from clique to root and return that path as factors plus a list of orphaned subtree roots...
Definition BayesTree-inst.h:448
│ │ │ │ -
gtsam::BayesTree::marginalFactor
sharedConditional marginalFactor(Key j, const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
Return marginal on any variable.
Definition BayesTree-inst.h:270
│ │ │ │ -
gtsam::BayesTree::numCachedSeparatorMarginals
size_t numCachedSeparatorMarginals() const
Collect number of cliques with cached separator marginals.
Definition BayesTree-inst.h:55
│ │ │ │ -
gtsam::BayesTree::getCliqueData
BayesTreeCliqueData getCliqueData() const
Gather data on all cliques.
Definition BayesTree-inst.h:35
│ │ │ │ -
gtsam::BayesTree::removeSubtree
Cliques removeSubtree(const sharedClique &subtree)
Remove the requested subtree.
Definition BayesTree-inst.h:496
│ │ │ │ -
gtsam::BayesTree::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print
Definition BayesTree-inst.h:212
│ │ │ │ -
gtsam::BayesTree::insertRoot
void insertRoot(const sharedClique &subtree)
Insert a new subtree with known parent clique.
Definition BayesTree-inst.h:260
│ │ │ │ -
gtsam::BayesTree::saveGraph
void saveGraph(const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
output to file with graphviz format.
Definition BayesTree-inst.h:85
│ │ │ │ -
gtsam::BayesTree::removeTop
void removeTop(const KeyVector &keys, BayesNetType *bn, Cliques *orphans)
Given a list of indices, turn "contaminated" part of the tree back into a factor graph.
Definition BayesTree-inst.h:475
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
gtsam::treeTraversal::DepthFirstForestParallel
void DepthFirstForestParallel(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, VISITOR_POST &visitorPost, int problemSizeThreshold=10)
Traverse a forest depth-first with pre-order and post-order visits.
Definition treeTraversal-inst.h:154
│ │ │ │ +
gtsam::TbbOpenMPMixedScope
An object whose scope defines a block where TBB and OpenMP parallelism are mixed.
Definition types.h:192
│ │ │ │ +
gtsam::EliminatableClusterTree
A cluster-tree that eliminates to a Bayes tree.
Definition ClusterTree.h:184
│ │ │ │ +
gtsam::EliminatableClusterTree::operator=
This & operator=(const This &other)
Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are copie...
Definition ClusterTree-inst.h:231
│ │ │ │ +
gtsam::EliminatableClusterTree::eliminate
std::pair< boost::shared_ptr< BayesTreeType >, boost::shared_ptr< FactorGraphType > > eliminate(const Eliminate &function) const
Eliminate the factors to a Bayes tree and remaining factor graph.
Definition ClusterTree-inst.h:245
│ │ │ │ +
gtsam::EliminatableClusterTree::sharedFactor
boost::shared_ptr< FactorType > sharedFactor
Shared pointer to a factor.
Definition ClusterTree.h:197
│ │ │ │ +
gtsam::EliminatableClusterTree::Eliminate
GRAPH::Eliminate Eliminate
Typedef for an eliminate subroutine.
Definition ClusterTree.h:195
│ │ │ │ +
gtsam::BayesTreeOrphanWrapper
Definition BayesTree.h:276
│ │ │ │ +
gtsam::EliminationData
Definition ClusterTree-inst.h:115
│ │ │ │ + │ │ │ │ +
gtsam::ClusterTree
A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman: each node k re...
Definition ClusterTree.h:25
│ │ │ │ +
gtsam::ClusterTree::operator=
This & operator=(const This &other)
Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are copie...
Definition ClusterTree-inst.h:104
│ │ │ │ +
gtsam::ClusterTree::roots_
FastVector< sharedNode > roots_
concept check
Definition ClusterTree.h:116
│ │ │ │ +
gtsam::ClusterTree::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Print the cluster tree.
Definition ClusterTree-inst.h:98
│ │ │ │ +
gtsam::ClusterTree< DiscreteFactorGraph >::sharedFactor
boost::shared_ptr< FactorType > sharedFactor
Shared pointer to a factor.
Definition ClusterTree.h:32
│ │ │ │ +
gtsam::ClusterTree::Cluster::print
virtual void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print this node
Definition ClusterTree-inst.h:26
│ │ │ │ +
gtsam::ClusterTree::Cluster::orderedFrontalKeys
Keys orderedFrontalKeys
Frontal keys of this node.
Definition ClusterTree.h:41
│ │ │ │ +
gtsam::ClusterTree::Cluster::mergeChildren
void mergeChildren(const std::vector< bool > &merge)
Merge all children for which bit is set into this node.
Definition ClusterTree-inst.h:56
│ │ │ │ +
gtsam::ClusterTree::Cluster::merge
void merge(const boost::shared_ptr< Cluster > &cluster)
Merge in given cluster.
Definition ClusterTree-inst.h:44
│ │ │ │ +
gtsam::ClusterTree::Cluster::nrFrontalsOfChildren
std::vector< size_t > nrFrontalsOfChildren() const
Return a vector with nrFrontal keys for each child.
Definition ClusterTree-inst.h:34
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,757 +1,439 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -BayesTree-inst.h │ │ │ │ │ +ClusterTree-inst.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -1/* --------------------------------------------------------------------------- │ │ │ │ │ -- │ │ │ │ │ -2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ -4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -5 * All Rights Reserved │ │ │ │ │ -6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ -7 │ │ │ │ │ -8 * See LICENSE for the license information │ │ │ │ │ -9 │ │ │ │ │ -10 * ------------------------------------------------------------------------- │ │ │ │ │ -- */ │ │ │ │ │ +1 │ │ │ │ │ +10#pragma once │ │ │ │ │ 11 │ │ │ │ │ -21#pragma once │ │ │ │ │ -22 │ │ │ │ │ -23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_O_r_d_e_r_i_n_g_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_b_a_s_e_/_t_r_e_e_T_r_a_v_e_r_s_a_l_-_i_n_s_t_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_b_a_s_e_/_t_i_m_i_n_g_._h> │ │ │ │ │ -27 │ │ │ │ │ -28#include │ │ │ │ │ -29#include │ │ │ │ │ -30 │ │ │ │ │ -31namespace _g_t_s_a_m { │ │ │ │ │ -32 │ │ │ │ │ -33 /* ************************************************************************* │ │ │ │ │ +12#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_C_l_u_s_t_e_r_T_r_e_e_._h> │ │ │ │ │ +13#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ +14#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_O_r_d_e_r_i_n_g_._h> │ │ │ │ │ +15#include <_g_t_s_a_m_/_b_a_s_e_/_t_i_m_i_n_g_._h> │ │ │ │ │ +16#include <_g_t_s_a_m_/_b_a_s_e_/_t_r_e_e_T_r_a_v_e_r_s_a_l_-_i_n_s_t_._h> │ │ │ │ │ +17 │ │ │ │ │ +18#ifdef GTSAM_USE_TBB │ │ │ │ │ +19#include │ │ │ │ │ +20#endif │ │ │ │ │ +21 │ │ │ │ │ +22namespace _g_t_s_a_m { │ │ │ │ │ +23 │ │ │ │ │ +24/* ************************************************************************* │ │ │ │ │ */ │ │ │ │ │ -34 template │ │ │ │ │ -_3_5 _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_D_a_t_a _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_g_e_t_C_l_i_q_u_e_D_a_t_a() const { │ │ │ │ │ -36 _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_D_a_t_a stats; │ │ │ │ │ -37 for (const _s_h_a_r_e_d_C_l_i_q_u_e& root : roots_) getCliqueData(root, &stats); │ │ │ │ │ -38 return stats; │ │ │ │ │ -39 } │ │ │ │ │ -40 │ │ │ │ │ -41 /* ************************************************************************* │ │ │ │ │ +25template │ │ │ │ │ +_2_6void _C_l_u_s_t_e_r_T_r_e_e_<_G_R_A_P_H_>_:_:_C_l_u_s_t_e_r_:_:_p_r_i_n_t(const std::string& s, │ │ │ │ │ +27 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter) const { │ │ │ │ │ +28 std::cout << s << " (" << problemSize_ << ")"; │ │ │ │ │ +29 _P_r_i_n_t_K_e_y_V_e_c_t_o_r(_o_r_d_e_r_e_d_F_r_o_n_t_a_l_K_e_y_s); │ │ │ │ │ +30} │ │ │ │ │ +31 │ │ │ │ │ +32/* ************************************************************************* │ │ │ │ │ */ │ │ │ │ │ -42 template │ │ │ │ │ -_4_3 void _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_g_e_t_C_l_i_q_u_e_D_a_t_a(_s_h_a_r_e_d_C_l_i_q_u_e clique, │ │ │ │ │ -44 _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_D_a_t_a* stats) const { │ │ │ │ │ -45 const auto conditional = clique->conditional(); │ │ │ │ │ -46 stats->conditionalSizes.push_back(conditional->nrFrontals()); │ │ │ │ │ -47 stats->separatorSizes.push_back(conditional->nrParents()); │ │ │ │ │ -48 for (_s_h_a_r_e_d_C_l_i_q_u_e c : clique->children) { │ │ │ │ │ -49 getCliqueData(c, stats); │ │ │ │ │ -50 } │ │ │ │ │ -51 } │ │ │ │ │ -52 │ │ │ │ │ -53 /* ************************************************************************* │ │ │ │ │ +33template │ │ │ │ │ +_3_4std::vector _C_l_u_s_t_e_r_T_r_e_e_<_G_R_A_P_H_>_:_:_C_l_u_s_t_e_r_:_:_n_r_F_r_o_n_t_a_l_s_O_f_C_h_i_l_d_r_e_n() const │ │ │ │ │ +{ │ │ │ │ │ +35 std::vector nrFrontals; │ │ │ │ │ +36 nrFrontals.reserve(nrChildren()); │ │ │ │ │ +37 for (const sharedNode& child : children) │ │ │ │ │ +38 nrFrontals.push_back(child->nrFrontals()); │ │ │ │ │ +39 return nrFrontals; │ │ │ │ │ +40} │ │ │ │ │ +41 │ │ │ │ │ +42/* ************************************************************************* │ │ │ │ │ */ │ │ │ │ │ -54 template │ │ │ │ │ -_5_5 size_t _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_n_u_m_C_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l_s() const { │ │ │ │ │ -56 size_t count = 0; │ │ │ │ │ -57 for(const _s_h_a_r_e_d_C_l_i_q_u_e& root: roots_) │ │ │ │ │ -58 count += root->numCachedSeparatorMarginals(); │ │ │ │ │ -59 return count; │ │ │ │ │ -60 } │ │ │ │ │ -61 │ │ │ │ │ -62 /* ************************************************************************* │ │ │ │ │ +43template │ │ │ │ │ +_4_4void _C_l_u_s_t_e_r_T_r_e_e_<_G_R_A_P_H_>_:_:_C_l_u_s_t_e_r_:_:_m_e_r_g_e(const boost::shared_ptr& │ │ │ │ │ +cluster) { │ │ │ │ │ +45 // Merge keys. For efficiency, we add keys in reverse order at end, calling │ │ │ │ │ +reverse after.. │ │ │ │ │ +46 orderedFrontalKeys.insert(orderedFrontalKeys.end(), cluster- │ │ │ │ │ +>orderedFrontalKeys.rbegin(), │ │ │ │ │ +47 cluster->orderedFrontalKeys.rend()); │ │ │ │ │ +48 factors.push_back(cluster->factors); │ │ │ │ │ +49 children.insert(children.end(), cluster->children.begin(), cluster- │ │ │ │ │ +>children.end()); │ │ │ │ │ +50 // Increment problem size │ │ │ │ │ +51 problemSize_ = std::max(problemSize_, cluster->problemSize_); │ │ │ │ │ +52} │ │ │ │ │ +53 │ │ │ │ │ +54/* ************************************************************************* │ │ │ │ │ */ │ │ │ │ │ -63 template │ │ │ │ │ -_6_4 void _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_d_o_t(std::ostream& os, │ │ │ │ │ -65 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter) const { │ │ │ │ │ -66 if (roots_.empty()) │ │ │ │ │ -67 throw std::invalid_argument( │ │ │ │ │ -68 "the root of Bayes tree has not been initialized!"); │ │ │ │ │ -69 os << "digraph G{\n"; │ │ │ │ │ -70 for (const _s_h_a_r_e_d_C_l_i_q_u_e& root : roots_) _d_o_t(os, root, keyFormatter); │ │ │ │ │ -71 os << "}"; │ │ │ │ │ -72 std::flush(os); │ │ │ │ │ -73 } │ │ │ │ │ -74 │ │ │ │ │ -75 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -76 template │ │ │ │ │ -_7_7 std::string _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_d_o_t(const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter) const { │ │ │ │ │ -78 std::stringstream ss; │ │ │ │ │ -79 _d_o_t(ss, keyFormatter); │ │ │ │ │ -80 return ss.str(); │ │ │ │ │ -81 } │ │ │ │ │ -82 │ │ │ │ │ -83 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -84 template │ │ │ │ │ -_8_5 void _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_s_a_v_e_G_r_a_p_h(const std::string& filename, │ │ │ │ │ -86 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter) const { │ │ │ │ │ -87 std::ofstream of(filename.c_str()); │ │ │ │ │ -88 _d_o_t(of, keyFormatter); │ │ │ │ │ -89 of.close(); │ │ │ │ │ +55template │ │ │ │ │ +_5_6void _C_l_u_s_t_e_r_T_r_e_e_<_G_R_A_P_H_>_:_:_C_l_u_s_t_e_r_:_:_m_e_r_g_e_C_h_i_l_d_r_e_n( │ │ │ │ │ +57 const std::vector& merge) { │ │ │ │ │ +58 gttic(Cluster_mergeChildren); │ │ │ │ │ +59 assert(merge.size() == this->children.size()); │ │ │ │ │ +60 │ │ │ │ │ +61 // Count how many keys, factors and children we'll end up with │ │ │ │ │ +62 size_t nrKeys = orderedFrontalKeys.size(); │ │ │ │ │ +63 size_t nrFactors = factors.size(); │ │ │ │ │ +64 size_t nrNewChildren = 0; │ │ │ │ │ +65 // Loop over children │ │ │ │ │ +66 size_t i = 0; │ │ │ │ │ +67 for(const sharedNode& child: this->children) { │ │ │ │ │ +68 if (merge[i]) { │ │ │ │ │ +69 nrKeys += child->orderedFrontalKeys.size(); │ │ │ │ │ +70 nrFactors += child->factors.size(); │ │ │ │ │ +71 nrNewChildren += child->nrChildren(); │ │ │ │ │ +72 } else { │ │ │ │ │ +73 nrNewChildren += 1; // we keep the child │ │ │ │ │ +74 } │ │ │ │ │ +75 ++i; │ │ │ │ │ +76 } │ │ │ │ │ +77 │ │ │ │ │ +78 // now reserve space, and really merge │ │ │ │ │ +79 auto oldChildren = this->children; │ │ │ │ │ +80 this->children.clear(); │ │ │ │ │ +81 this->children.reserve(nrNewChildren); │ │ │ │ │ +82 orderedFrontalKeys.reserve(nrKeys); │ │ │ │ │ +83 factors.reserve(nrFactors); │ │ │ │ │ +84 i = 0; │ │ │ │ │ +85 for (const sharedNode& child : oldChildren) { │ │ │ │ │ +86 if (merge[i]) { │ │ │ │ │ +87 this->merge(child); │ │ │ │ │ +88 } else { │ │ │ │ │ +89 this->addChild(child); // we keep the child │ │ │ │ │ 90 } │ │ │ │ │ -91 │ │ │ │ │ -92 /* ************************************************************************* │ │ │ │ │ +91 ++i; │ │ │ │ │ +92 } │ │ │ │ │ +_9_3 std::reverse(orderedFrontalKeys.begin(), orderedFrontalKeys.end()); │ │ │ │ │ +94} │ │ │ │ │ +95 │ │ │ │ │ +96/* ************************************************************************* │ │ │ │ │ */ │ │ │ │ │ -93 template │ │ │ │ │ -_9_4 void _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_d_o_t(std::ostream& s, _s_h_a_r_e_d_C_l_i_q_u_e clique, │ │ │ │ │ -95 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter, │ │ │ │ │ -96 int parentnum) const { │ │ │ │ │ -97 static int num = 0; │ │ │ │ │ -98 bool first = true; │ │ │ │ │ -99 std::stringstream out; │ │ │ │ │ -100 out << num; │ │ │ │ │ -101 std::string parent = out.str(); │ │ │ │ │ -102 parent += "[label=\""; │ │ │ │ │ -103 │ │ │ │ │ -104 for (_K_e_y key : clique->conditional_->frontals()) { │ │ │ │ │ -105 if (!first) parent += ", "; │ │ │ │ │ -106 first = false; │ │ │ │ │ -107 parent += keyFormatter(key); │ │ │ │ │ -108 } │ │ │ │ │ +_9_7template │ │ │ │ │ +_9_8void _C_l_u_s_t_e_r_T_r_e_e_<_G_R_A_P_H_>_:_:_p_r_i_n_t(const std::string& s, const _K_e_y_F_o_r_m_a_t_t_e_r& │ │ │ │ │ +keyFormatter) const { │ │ │ │ │ +99 _t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_P_r_i_n_t_F_o_r_e_s_t(*this, s, keyFormatter); │ │ │ │ │ +_1_0_0} │ │ │ │ │ +101 │ │ │ │ │ +102/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +_1_0_3template │ │ │ │ │ +_1_0_4_C_l_u_s_t_e_r_T_r_e_e_<_G_R_A_P_H_>& _C_l_u_s_t_e_r_T_r_e_e_<_G_R_A_P_H_>_:_:_o_p_e_r_a_t_o_r_=(const _T_h_i_s& other) { │ │ │ │ │ +105 // Start by duplicating the tree. │ │ │ │ │ +106 _r_o_o_t_s__ = _t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_C_l_o_n_e_F_o_r_e_s_t(other); │ │ │ │ │ +107 return *this; │ │ │ │ │ +108} │ │ │ │ │ 109 │ │ │ │ │ -110 if (clique->parent()) { │ │ │ │ │ -111 parent += " : "; │ │ │ │ │ -_1_1_2 s << parentnum << "->" << num << "\n"; │ │ │ │ │ -113 } │ │ │ │ │ -114 │ │ │ │ │ -115 first = true; │ │ │ │ │ -116 for (_K_e_y parentKey : clique->conditional_->parents()) { │ │ │ │ │ -_1_1_7 if (!first) parent += ", "; │ │ │ │ │ -118 first = false; │ │ │ │ │ -119 parent += keyFormatter(parentKey); │ │ │ │ │ -120 } │ │ │ │ │ -121 parent += "\"];\n"; │ │ │ │ │ -122 s << parent; │ │ │ │ │ -_1_2_3 parentnum = num; │ │ │ │ │ -124 │ │ │ │ │ -125 for (_s_h_a_r_e_d_C_l_i_q_u_e c : clique->children) { │ │ │ │ │ -126 num++; │ │ │ │ │ -_1_2_7 _d_o_t(s, c, keyFormatter, parentnum); │ │ │ │ │ -128 } │ │ │ │ │ -129 } │ │ │ │ │ +110/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +111// Elimination traversal data - stores a pointer to the parent data and │ │ │ │ │ +collects │ │ │ │ │ +112// the factors resulting from elimination of the children. Also sets up │ │ │ │ │ +BayesTree │ │ │ │ │ +113// cliques with parent and child pointers. │ │ │ │ │ +114template │ │ │ │ │ +_1_1_5struct _E_l_i_m_i_n_a_t_i_o_n_D_a_t_a { │ │ │ │ │ +116 // Typedefs │ │ │ │ │ +117 typedef typename CLUSTERTREE::sharedFactor sharedFactor; │ │ │ │ │ +118 typedef typename CLUSTERTREE::FactorType FactorType; │ │ │ │ │ +119 typedef typename CLUSTERTREE::FactorGraphType FactorGraphType; │ │ │ │ │ +120 typedef typename CLUSTERTREE::ConditionalType ConditionalType; │ │ │ │ │ +121 typedef typename CLUSTERTREE::BayesTreeType::Node BTNode; │ │ │ │ │ +122 │ │ │ │ │ +123 _E_l_i_m_i_n_a_t_i_o_n_D_a_t_a* const parentData; │ │ │ │ │ +124 size_t myIndexInParent; │ │ │ │ │ +125 _F_a_s_t_V_e_c_t_o_r_<_s_h_a_r_e_d_F_a_c_t_o_r_> childFactors; │ │ │ │ │ +126 boost::shared_ptr bayesTreeNode; │ │ │ │ │ +127#ifdef GTSAM_USE_TBB │ │ │ │ │ +128 boost::shared_ptr writeLock; │ │ │ │ │ +129#endif │ │ │ │ │ 130 │ │ │ │ │ -131 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -132 template │ │ │ │ │ -_1_3_3 size_t _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_s_i_z_e() const { │ │ │ │ │ -134 size_t size = 0; │ │ │ │ │ -_1_3_5 for(const _s_h_a_r_e_d_C_l_i_q_u_e& clique: roots_) │ │ │ │ │ -136 size += clique->treeSize(); │ │ │ │ │ -137 return size; │ │ │ │ │ -138 } │ │ │ │ │ -139 │ │ │ │ │ -140 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -141 template │ │ │ │ │ -_1_4_2 void _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_a_d_d_C_l_i_q_u_e(const _s_h_a_r_e_d_C_l_i_q_u_e& clique, const │ │ │ │ │ -_s_h_a_r_e_d_C_l_i_q_u_e& parent_clique) { │ │ │ │ │ -143 for(_K_e_y j: clique->conditional()->frontals()) │ │ │ │ │ -144 nodes_[j] = clique; │ │ │ │ │ -145 if (parent_clique != nullptr) { │ │ │ │ │ -146 clique->parent_ = parent_clique; │ │ │ │ │ -147 parent_clique->children.push_back(clique); │ │ │ │ │ -148 } else { │ │ │ │ │ -149 roots_.push_back(clique); │ │ │ │ │ -150 } │ │ │ │ │ -151 } │ │ │ │ │ -152 │ │ │ │ │ -153 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -154 namespace { │ │ │ │ │ -155 template │ │ │ │ │ -156 struct _pushCliqueFunctor { │ │ │ │ │ -157 _pushCliqueFunctor(_F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_>* graph_) : graph(graph_) {} │ │ │ │ │ -158 FactorGraph* graph; │ │ │ │ │ -159 int operator()(const boost::shared_ptr& clique, int dummy) { │ │ │ │ │ -160 graph->push_back(clique->conditional_); │ │ │ │ │ -_1_6_1 return 0; │ │ │ │ │ -162 } │ │ │ │ │ -163 }; │ │ │ │ │ -_1_6_4 } // namespace │ │ │ │ │ -165 │ │ │ │ │ -166 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -167 template │ │ │ │ │ -_1_6_8 void _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_a_d_d_F_a_c_t_o_r_s_T_o_G_r_a_p_h( │ │ │ │ │ -169 _F_a_c_t_o_r_G_r_a_p_h_<_F_a_c_t_o_r_T_y_p_e_>* graph) const { │ │ │ │ │ -170 // Traverse the BayesTree and add all conditionals to this graph │ │ │ │ │ -_1_7_1 int data = 0; // Unused │ │ │ │ │ -172 _pushCliqueFunctor functor(graph); │ │ │ │ │ -173 _t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t(*this, data, functor); │ │ │ │ │ -174 } │ │ │ │ │ -175 │ │ │ │ │ -176 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -_1_7_7 template │ │ │ │ │ -_1_7_8 _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_B_a_y_e_s_T_r_e_e(const _T_h_i_s& other) { │ │ │ │ │ -179 *this = other; │ │ │ │ │ -180 } │ │ │ │ │ -181 │ │ │ │ │ -182 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -_1_8_3 namespace { │ │ │ │ │ -184 template │ │ │ │ │ -185 boost::shared_ptr │ │ │ │ │ -186 BayesTreeCloneForestVisitorPre(const boost::shared_ptr& node, const │ │ │ │ │ -boost::shared_ptr& parentPointer) │ │ │ │ │ -187 { │ │ │ │ │ -188 // Clone the current node and add it to its cloned parent │ │ │ │ │ -_1_8_9 boost::shared_ptr clone = boost::make_shared(*node); │ │ │ │ │ -190 clone->children.clear(); │ │ │ │ │ -191 clone->parent_ = parentPointer; │ │ │ │ │ -_1_9_2 parentPointer->children.push_back(clone); │ │ │ │ │ -193 return clone; │ │ │ │ │ -194 } │ │ │ │ │ -195 } │ │ │ │ │ -_1_9_6 │ │ │ │ │ -197 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -198 template │ │ │ │ │ -_1_9_9 _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>& _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_o_p_e_r_a_t_o_r_=(const _T_h_i_s& other) { │ │ │ │ │ -200 this->clear(); │ │ │ │ │ -201 boost::shared_ptr rootContainer = boost::make_shared(); │ │ │ │ │ -202 _t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t(other, rootContainer, │ │ │ │ │ -BayesTreeCloneForestVisitorPre); │ │ │ │ │ -203 for(const _s_h_a_r_e_d_C_l_i_q_u_e& root: rootContainer->children) { │ │ │ │ │ -204 root->parent_ = typename Clique::weak_ptr(); // Reset the parent since it's │ │ │ │ │ -set to the dummy clique │ │ │ │ │ -205 insertRoot(root); │ │ │ │ │ -206 } │ │ │ │ │ -207 return *this; │ │ │ │ │ -_2_0_8 } │ │ │ │ │ -209 │ │ │ │ │ -210 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -_2_1_1 template │ │ │ │ │ -_2_1_2 void _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_p_r_i_n_t(const std::string& s, const _K_e_y_F_o_r_m_a_t_t_e_r& │ │ │ │ │ -keyFormatter) const { │ │ │ │ │ -213 std::cout << s << ": cliques: " << size() << ", variables: " << nodes_.size │ │ │ │ │ -() << std::endl; │ │ │ │ │ -_2_1_4 _t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_P_r_i_n_t_F_o_r_e_s_t(*this, s, keyFormatter); │ │ │ │ │ -215 } │ │ │ │ │ -216 │ │ │ │ │ -217 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -218 // binary predicate to test equality of a pair for use in equals │ │ │ │ │ -219 template │ │ │ │ │ -_2_2_0 bool check_sharedCliques( │ │ │ │ │ -221 const std::pair<_K_e_y, typename _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_s_h_a_r_e_d_C_l_i_q_u_e>& v1, │ │ │ │ │ -222 const std::pair<_K_e_y, typename _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_s_h_a_r_e_d_C_l_i_q_u_e>& v2 │ │ │ │ │ -223 ) { │ │ │ │ │ -224 return v1.first == v2.first && │ │ │ │ │ -225 ((!v1.second && !v2.second) || (v1.second && v2.second && v1.second->equals │ │ │ │ │ -(*v2.second))); │ │ │ │ │ -_2_2_6 } │ │ │ │ │ -227 │ │ │ │ │ -228 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -229 template │ │ │ │ │ -_2_3_0 bool _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_e_q_u_a_l_s(const _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>& other, double tol) │ │ │ │ │ -const { │ │ │ │ │ -231 return size()==other._s_i_z_e() && │ │ │ │ │ -232 std::equal(nodes_.begin(), nodes_.end(), other._n_o_d_e_s__.begin(), │ │ │ │ │ -&check_sharedCliques); │ │ │ │ │ -233 } │ │ │ │ │ +131 _E_l_i_m_i_n_a_t_i_o_n_D_a_t_a(_E_l_i_m_i_n_a_t_i_o_n_D_a_t_a* _parentData, size_t nChildren) : │ │ │ │ │ +132 parentData(_parentData), bayesTreeNode(boost::make_shared()) │ │ │ │ │ +133#ifdef GTSAM_USE_TBB │ │ │ │ │ +134 , writeLock(boost::make_shared()) │ │ │ │ │ +135#endif │ │ │ │ │ +136 { │ │ │ │ │ +137 if (parentData) { │ │ │ │ │ +_1_3_8#ifdef GTSAM_USE_TBB │ │ │ │ │ +139 parentData->writeLock->lock(); │ │ │ │ │ +140#endif │ │ │ │ │ +141 myIndexInParent = parentData->childFactors.size(); │ │ │ │ │ +142 parentData->childFactors.push_back(_s_h_a_r_e_d_F_a_c_t_o_r()); │ │ │ │ │ +143#ifdef GTSAM_USE_TBB │ │ │ │ │ +144 parentData->writeLock->unlock(); │ │ │ │ │ +145#endif │ │ │ │ │ +146 } else { │ │ │ │ │ +147 myIndexInParent = 0; │ │ │ │ │ +148 } │ │ │ │ │ +149 // Set up BayesTree parent and child pointers │ │ │ │ │ +150 if (parentData) { │ │ │ │ │ +151 if (parentData->parentData) // If our parent is not the dummy node │ │ │ │ │ +152 bayesTreeNode->parent_ = parentData->bayesTreeNode; │ │ │ │ │ +153 parentData->bayesTreeNode->children.push_back(bayesTreeNode); │ │ │ │ │ +154 } │ │ │ │ │ +155 } │ │ │ │ │ +156 │ │ │ │ │ +157 // Elimination pre-order visitor - creates the EliminationData structure │ │ │ │ │ +for the visited node. │ │ │ │ │ +158 static EliminationData EliminationPreOrderVisitor( │ │ │ │ │ +159 const typename CLUSTERTREE::sharedNode& node, │ │ │ │ │ +160 EliminationData& parentData) { │ │ │ │ │ +161 assert(node); │ │ │ │ │ +162 EliminationData myData(&parentData, node->nrChildren()); │ │ │ │ │ +163 myData.bayesTreeNode->problemSize_ = node->problemSize(); │ │ │ │ │ +164 return myData; │ │ │ │ │ +165 } │ │ │ │ │ +166 │ │ │ │ │ +167 // Elimination post-order visitor - combine the child factors with our own │ │ │ │ │ +factors, add the │ │ │ │ │ +168 // resulting conditional to the BayesTree, and add the remaining factor to │ │ │ │ │ +the parent. │ │ │ │ │ +_1_6_9 class _E_l_i_m_i_n_a_t_i_o_n_P_o_s_t_O_r_d_e_r_V_i_s_i_t_o_r { │ │ │ │ │ +170 const typename CLUSTERTREE::Eliminate& eliminationFunction_; │ │ │ │ │ +171 typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex_; │ │ │ │ │ +172 │ │ │ │ │ +173 public: │ │ │ │ │ +174 // Construct functor │ │ │ │ │ +_1_7_5 EliminationPostOrderVisitor( │ │ │ │ │ +176 const typename CLUSTERTREE::Eliminate& eliminationFunction, │ │ │ │ │ +177 typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex) : │ │ │ │ │ +178 eliminationFunction_(eliminationFunction), nodesIndex_(nodesIndex) { │ │ │ │ │ +179 } │ │ │ │ │ +180 │ │ │ │ │ +181 // Function that does the HEAVY lifting │ │ │ │ │ +182 void operator()(const typename CLUSTERTREE::sharedNode& node, │ │ │ │ │ +_E_l_i_m_i_n_a_t_i_o_n_D_a_t_a& myData) { │ │ │ │ │ +183 assert(node); │ │ │ │ │ +184 │ │ │ │ │ +185 // Gather factors │ │ │ │ │ +186 FactorGraphType gatheredFactors; │ │ │ │ │ +187 gatheredFactors.reserve(node->factors.size() + node->nrChildren()); │ │ │ │ │ +188 gatheredFactors += node->factors; │ │ │ │ │ +189 gatheredFactors += myData.childFactors; │ │ │ │ │ +190 │ │ │ │ │ +191 // Check for Bayes tree orphan subtrees, and add them to our children │ │ │ │ │ +192 // TODO(frank): should this really happen here? │ │ │ │ │ +193 for (const sharedFactor& factor: node->factors) { │ │ │ │ │ +194 auto asSubtree = dynamic_cast*> │ │ │ │ │ +(factor.get()); │ │ │ │ │ +195 if (asSubtree) { │ │ │ │ │ +196 myData.bayesTreeNode->children.push_back(asSubtree->clique); │ │ │ │ │ +197 asSubtree->clique->parent_ = myData.bayesTreeNode; │ │ │ │ │ +198 } │ │ │ │ │ +199 } │ │ │ │ │ +200 │ │ │ │ │ +201 // >>>>>>>>>>>>>> Do dense elimination step >>>>>>>>>>>>>>>>>>>>>>>>>>>>> │ │ │ │ │ +202 auto eliminationResult = eliminationFunction_(gatheredFactors, node- │ │ │ │ │ +>orderedFrontalKeys); │ │ │ │ │ +203 // <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< │ │ │ │ │ +204 │ │ │ │ │ +205 // Store conditional in BayesTree clique, and in the case of ISAM2Clique │ │ │ │ │ +also store the │ │ │ │ │ +206 // remaining factor │ │ │ │ │ +207 myData.bayesTreeNode->setEliminationResult(eliminationResult); │ │ │ │ │ +208 │ │ │ │ │ +209 // Fill nodes index - we do this here instead of calling insertRoot at the │ │ │ │ │ +end to avoid │ │ │ │ │ +210 // putting orphan subtrees in the index - they'll already be in the index │ │ │ │ │ +of the ISAM2 │ │ │ │ │ +211 // object they're added to. │ │ │ │ │ +212 for (const Key& j: myData.bayesTreeNode->conditional()->frontals()) │ │ │ │ │ +213 nodesIndex_.insert(std::make_pair(j, myData.bayesTreeNode)); │ │ │ │ │ +214 │ │ │ │ │ +215 // Store remaining factor in parent's gathered factors │ │ │ │ │ +216 if (!eliminationResult.second->empty()) { │ │ │ │ │ +217#ifdef GTSAM_USE_TBB │ │ │ │ │ +218 myData.parentData->writeLock->lock(); │ │ │ │ │ +219#endif │ │ │ │ │ +220 myData.parentData->childFactors[myData.myIndexInParent] = │ │ │ │ │ +eliminationResult.second; │ │ │ │ │ +221#ifdef GTSAM_USE_TBB │ │ │ │ │ +_2_2_2 myData.parentData->writeLock->unlock(); │ │ │ │ │ +223#endif │ │ │ │ │ +224 } │ │ │ │ │ +225 } │ │ │ │ │ +226 }; │ │ │ │ │ +227}; │ │ │ │ │ +228 │ │ │ │ │ +229/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +230template │ │ │ │ │ +_2_3_1_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_<_B_A_Y_E_S_T_R_E_E_,_ _G_R_A_P_H_>& │ │ │ │ │ +_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_<_B_A_Y_E_S_T_R_E_E_,_ _G_R_A_P_H_>_:_:_o_p_e_r_a_t_o_r_=( │ │ │ │ │ +232 const _T_h_i_s& other) { │ │ │ │ │ +233 _C_l_u_s_t_e_r_T_r_e_e_<_G_R_A_P_H_>_:_:_o_p_e_r_a_t_o_r_=(other); │ │ │ │ │ 234 │ │ │ │ │ -_2_3_5 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -236 template │ │ │ │ │ -237 template │ │ │ │ │ -_2_3_8 _K_e_y _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_f_i_n_d_P_a_r_e_n_t_C_l_i_q_u_e(const CONTAINER& parents) const { │ │ │ │ │ -239 typename CONTAINER::const_iterator lowestOrderedParent = min_element │ │ │ │ │ -(parents.begin(), parents.end()); │ │ │ │ │ -240 assert(lowestOrderedParent != parents.end()); │ │ │ │ │ -_2_4_1 return *lowestOrderedParent; │ │ │ │ │ -242 } │ │ │ │ │ -243 │ │ │ │ │ -244 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -245 template │ │ │ │ │ -_2_4_6 void _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_f_i_l_l_N_o_d_e_s_I_n_d_e_x(const _s_h_a_r_e_d_C_l_i_q_u_e& subtree) { │ │ │ │ │ -247 // Add each frontal variable of this root node │ │ │ │ │ -248 for(const _K_e_y& j: subtree->conditional()->frontals()) { │ │ │ │ │ -249 bool inserted = nodes_.insert(std::make_pair(j, subtree)).second; │ │ │ │ │ -_2_5_0 assert(inserted); (void)inserted; │ │ │ │ │ -251 } │ │ │ │ │ -252 // Fill index for each child │ │ │ │ │ -_2_5_3 typedef typename _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_s_h_a_r_e_d_C_l_i_q_u_e _s_h_a_r_e_d_C_l_i_q_u_e; │ │ │ │ │ -254 for(const _s_h_a_r_e_d_C_l_i_q_u_e& child: subtree->children) { │ │ │ │ │ -255 fillNodesIndex(child); } │ │ │ │ │ -_2_5_6 } │ │ │ │ │ -257 │ │ │ │ │ -258 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -259 template │ │ │ │ │ -_2_6_0 void _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_i_n_s_e_r_t_R_o_o_t(const _s_h_a_r_e_d_C_l_i_q_u_e& subtree) { │ │ │ │ │ -261 roots_.push_back(subtree); // Add to roots │ │ │ │ │ -262 fillNodesIndex(subtree); // Populate nodes index │ │ │ │ │ -263 } │ │ │ │ │ -264 │ │ │ │ │ -265 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -266 // First finds clique marginal then marginalizes that │ │ │ │ │ -267 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -268 template │ │ │ │ │ -269 typename BayesTree::sharedConditional │ │ │ │ │ -_2_7_0 _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_m_a_r_g_i_n_a_l_F_a_c_t_o_r(_K_e_y j, const Eliminate& function) const │ │ │ │ │ -271 { │ │ │ │ │ -272 gttic(BayesTree_marginalFactor); │ │ │ │ │ -273 │ │ │ │ │ -274 // get clique containing Key j │ │ │ │ │ -275 _s_h_a_r_e_d_C_l_i_q_u_e clique = this->clique(j); │ │ │ │ │ -276 │ │ │ │ │ -277 // calculate or retrieve its marginal P(C) = P(F,S) │ │ │ │ │ -278 FactorGraphType cliqueMarginal = clique->marginal2(function); │ │ │ │ │ -279 │ │ │ │ │ -280 // Now, marginalize out everything that is not variable j │ │ │ │ │ -281 BayesNetType marginalBN = │ │ │ │ │ -282 *cliqueMarginal.marginalMultifrontalBayesNet(_O_r_d_e_r_i_n_g{j}, function); │ │ │ │ │ -283 │ │ │ │ │ -284 // The Bayes net should contain only one conditional for variable j, so │ │ │ │ │ -return it │ │ │ │ │ -285 return marginalBN.front(); │ │ │ │ │ -286 } │ │ │ │ │ -287 │ │ │ │ │ -288 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -289 // Find two cliques, their joint, then marginalizes │ │ │ │ │ -290 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -291 template │ │ │ │ │ -292 typename BayesTree::sharedFactorGraph │ │ │ │ │ -_2_9_3 _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_j_o_i_n_t(_K_e_y j1, _K_e_y j2, const Eliminate& function) const │ │ │ │ │ -294 { │ │ │ │ │ -295 gttic(BayesTree_joint); │ │ │ │ │ -296 return boost::make_shared(*jointBayesNet(j1, j2, │ │ │ │ │ -function)); │ │ │ │ │ -297 } │ │ │ │ │ -298 │ │ │ │ │ -299 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -300 template │ │ │ │ │ -301 typename BayesTree::sharedBayesNet │ │ │ │ │ -_3_0_2 _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_j_o_i_n_t_B_a_y_e_s_N_e_t(_K_e_y j1, _K_e_y j2, const Eliminate& function) │ │ │ │ │ -const │ │ │ │ │ -303 { │ │ │ │ │ -304 gttic(BayesTree_jointBayesNet); │ │ │ │ │ -305 // get clique C1 and C2 │ │ │ │ │ -306 _s_h_a_r_e_d_C_l_i_q_u_e C1 = (*this)[j1], C2 = (*this)[j2]; │ │ │ │ │ -307 │ │ │ │ │ -308 gttic(Lowest_common_ancestor); │ │ │ │ │ -309 // Find lowest common ancestor clique │ │ │ │ │ -310 _s_h_a_r_e_d_C_l_i_q_u_e B; { │ │ │ │ │ -311 // Build two paths to the root │ │ │ │ │ -312 _F_a_s_t_L_i_s_t_<_s_h_a_r_e_d_C_l_i_q_u_e_> path1, path2; { │ │ │ │ │ -313 _s_h_a_r_e_d_C_l_i_q_u_e p = C1; │ │ │ │ │ -314 while(p) { │ │ │ │ │ -315 path1.push_front(p); │ │ │ │ │ -316 p = p->parent(); │ │ │ │ │ -317 } │ │ │ │ │ -318 } { │ │ │ │ │ -319 _s_h_a_r_e_d_C_l_i_q_u_e p = C2; │ │ │ │ │ -320 while(p) { │ │ │ │ │ -321 path2.push_front(p); │ │ │ │ │ -322 p = p->parent(); │ │ │ │ │ -323 } │ │ │ │ │ -324 } │ │ │ │ │ -325 // Find the path intersection │ │ │ │ │ -326 typename _F_a_s_t_L_i_s_t_<_s_h_a_r_e_d_C_l_i_q_u_e_>_:_:_c_o_n_s_t___i_t_e_r_a_t_o_r p1 = path1.begin(), p2 = │ │ │ │ │ -path2.begin(); │ │ │ │ │ -327 if(*p1 == *p2) │ │ │ │ │ -328 B = *p1; │ │ │ │ │ -329 while(p1 != path1.end() && p2 != path2.end() && *p1 == *p2) { │ │ │ │ │ -330 B = *p1; │ │ │ │ │ -331 ++p1; │ │ │ │ │ -332 ++p2; │ │ │ │ │ -333 } │ │ │ │ │ -334 } │ │ │ │ │ -335 gttoc(Lowest_common_ancestor); │ │ │ │ │ -336 │ │ │ │ │ -337 // Build joint on all involved variables │ │ │ │ │ -338 FactorGraphType p_BC1C2; │ │ │ │ │ -339 │ │ │ │ │ -340 if(B) │ │ │ │ │ -341 { │ │ │ │ │ -342 // Compute marginal on lowest common ancestor clique │ │ │ │ │ -343 gttic(LCA_marginal); │ │ │ │ │ -344 FactorGraphType p_B = B->marginal2(function); │ │ │ │ │ -345 gttoc(LCA_marginal); │ │ │ │ │ -346 │ │ │ │ │ -347 // Compute shortcuts of the requested cliques given the lowest common │ │ │ │ │ -ancestor │ │ │ │ │ -348 gttic(Clique_shortcuts); │ │ │ │ │ -349 BayesNetType p_C1_Bred = C1->shortcut(B, function); │ │ │ │ │ -350 BayesNetType p_C2_Bred = C2->shortcut(B, function); │ │ │ │ │ -351 gttoc(Clique_shortcuts); │ │ │ │ │ -352 │ │ │ │ │ -353 // Factor the shortcuts to be conditioned on the full root │ │ │ │ │ -354 // Get the set of variables to eliminate, which is C1\B. │ │ │ │ │ -355 gttic(Full_root_factoring); │ │ │ │ │ -356 boost::shared_ptr p_C1_B; { │ │ │ │ │ -357 _K_e_y_V_e_c_t_o_r C1_minus_B; { │ │ │ │ │ -358 _K_e_y_S_e_t C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()- │ │ │ │ │ ->endParents()); │ │ │ │ │ -359 for(const _K_e_y j: *B->conditional()) { │ │ │ │ │ -360 C1_minus_B_set.erase(j); } │ │ │ │ │ -361 C1_minus_B.assign(C1_minus_B_set.begin(), C1_minus_B_set.end()); │ │ │ │ │ -362 } │ │ │ │ │ -363 // Factor into C1\B | B. │ │ │ │ │ -364 sharedFactorGraph temp_remaining; │ │ │ │ │ -365 boost::tie(p_C1_B, temp_remaining) = │ │ │ │ │ -366 FactorGraphType(p_C1_Bred).eliminatePartialMultifrontal(_O_r_d_e_r_i_n_g │ │ │ │ │ -(C1_minus_B), function); │ │ │ │ │ -367 } │ │ │ │ │ -368 boost::shared_ptr p_C2_B; { │ │ │ │ │ -369 _K_e_y_V_e_c_t_o_r C2_minus_B; { │ │ │ │ │ -370 _K_e_y_S_e_t C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()- │ │ │ │ │ ->endParents()); │ │ │ │ │ -371 for(const _K_e_y j: *B->conditional()) { │ │ │ │ │ -372 C2_minus_B_set.erase(j); } │ │ │ │ │ -373 C2_minus_B.assign(C2_minus_B_set.begin(), C2_minus_B_set.end()); │ │ │ │ │ -374 } │ │ │ │ │ -375 // Factor into C2\B | B. │ │ │ │ │ -376 sharedFactorGraph temp_remaining; │ │ │ │ │ -377 boost::tie(p_C2_B, temp_remaining) = │ │ │ │ │ -378 FactorGraphType(p_C2_Bred).eliminatePartialMultifrontal(_O_r_d_e_r_i_n_g │ │ │ │ │ -(C2_minus_B), function); │ │ │ │ │ -379 } │ │ │ │ │ -380 gttoc(Full_root_factoring); │ │ │ │ │ -381 │ │ │ │ │ -382 gttic(Variable_joint); │ │ │ │ │ -383 p_BC1C2 += p_B; │ │ │ │ │ -384 p_BC1C2 += *p_C1_B; │ │ │ │ │ -385 p_BC1C2 += *p_C2_B; │ │ │ │ │ -386 if(C1 != B) │ │ │ │ │ -387 p_BC1C2 += C1->conditional(); │ │ │ │ │ -388 if(C2 != B) │ │ │ │ │ -389 p_BC1C2 += C2->conditional(); │ │ │ │ │ -390 gttoc(Variable_joint); │ │ │ │ │ -391 } │ │ │ │ │ -392 else │ │ │ │ │ -393 { │ │ │ │ │ -394 // The nodes have no common ancestor, they're in different trees, so │ │ │ │ │ -they're joint is just the │ │ │ │ │ -395 // product of their marginals. │ │ │ │ │ -396 gttic(Disjoint_marginals); │ │ │ │ │ -397 p_BC1C2 += C1->marginal2(function); │ │ │ │ │ -398 p_BC1C2 += C2->marginal2(function); │ │ │ │ │ -399 gttoc(Disjoint_marginals); │ │ │ │ │ -400 } │ │ │ │ │ -401 │ │ │ │ │ -402 // now, marginalize out everything that is not variable j1 or j2 │ │ │ │ │ -403 return p_BC1C2.marginalMultifrontalBayesNet(_O_r_d_e_r_i_n_g{j1, j2}, function); │ │ │ │ │ -404 } │ │ │ │ │ -405 │ │ │ │ │ -406 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -407 template │ │ │ │ │ -_4_0_8 void _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_c_l_e_a_r() { │ │ │ │ │ -409 // Remove all nodes and clear the root pointer │ │ │ │ │ -410 nodes_.clear(); │ │ │ │ │ -411 roots_.clear(); │ │ │ │ │ -412 } │ │ │ │ │ -413 │ │ │ │ │ -414 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -415 template │ │ │ │ │ -_4_1_6 void _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_d_e_l_e_t_e_C_a_c_h_e_d_S_h_o_r_t_c_u_t_s() { │ │ │ │ │ -417 for(const _s_h_a_r_e_d_C_l_i_q_u_e& root: roots_) { │ │ │ │ │ -418 root->deleteCachedShortcuts(); │ │ │ │ │ -419 } │ │ │ │ │ -420 } │ │ │ │ │ -421 │ │ │ │ │ -422 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -423 template │ │ │ │ │ -_4_2_4 void _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_r_e_m_o_v_e_C_l_i_q_u_e(_s_h_a_r_e_d_C_l_i_q_u_e clique) │ │ │ │ │ -425 { │ │ │ │ │ -426 if (clique->isRoot()) { │ │ │ │ │ -427 typename Roots::iterator root = std::find(roots_.begin(), roots_.end(), │ │ │ │ │ -clique); │ │ │ │ │ -428 if(root != roots_.end()) │ │ │ │ │ -429 roots_.erase(root); │ │ │ │ │ -430 } else { // detach clique from parent │ │ │ │ │ -431 _s_h_a_r_e_d_C_l_i_q_u_e parent = clique->parent_.lock(); │ │ │ │ │ -432 typename Roots::iterator child = std::find(parent->children.begin(), │ │ │ │ │ -parent->children.end(), clique); │ │ │ │ │ -433 assert(child != parent->children.end()); │ │ │ │ │ -434 parent->children.erase(child); │ │ │ │ │ -435 } │ │ │ │ │ -436 │ │ │ │ │ -437 // orphan my children │ │ │ │ │ -438 for(_s_h_a_r_e_d_C_l_i_q_u_e child: clique->children) │ │ │ │ │ -439 child->parent_ = typename Clique::weak_ptr(); │ │ │ │ │ -440 │ │ │ │ │ -441 for(_K_e_y j: clique->conditional()->frontals()) { │ │ │ │ │ -442 nodes_.unsafe_erase(j); │ │ │ │ │ -443 } │ │ │ │ │ -444 } │ │ │ │ │ -445 │ │ │ │ │ -446 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -447 template │ │ │ │ │ -_4_4_8 void _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_r_e_m_o_v_e_P_a_t_h(_s_h_a_r_e_d_C_l_i_q_u_e clique, BayesNetType* bn, │ │ │ │ │ -449 _C_l_i_q_u_e_s* orphans) { │ │ │ │ │ -450 // base case is nullptr, if so we do nothing and return empties above │ │ │ │ │ -451 if (clique) { │ │ │ │ │ -452 // remove the clique from orphans in case it has been added earlier │ │ │ │ │ -453 orphans->remove(clique); │ │ │ │ │ -454 │ │ │ │ │ -455 // remove me │ │ │ │ │ -456 this->removeClique(clique); │ │ │ │ │ -457 │ │ │ │ │ -458 // remove path above me │ │ │ │ │ -459 this->removePath(typename Clique::shared_ptr(clique->parent_.lock()), bn, │ │ │ │ │ -460 orphans); │ │ │ │ │ -461 │ │ │ │ │ -462 // add children to list of orphans (splice also removed them from │ │ │ │ │ -463 // clique->children_) │ │ │ │ │ -464 orphans->insert(orphans->begin(), clique->children.begin(), │ │ │ │ │ -465 clique->children.end()); │ │ │ │ │ -466 clique->children.clear(); │ │ │ │ │ -467 │ │ │ │ │ -468 bn->push_back(clique->conditional_); │ │ │ │ │ -469 } │ │ │ │ │ -470 } │ │ │ │ │ -471 │ │ │ │ │ -472 / │ │ │ │ │ -* ************************************************************************* │ │ │ │ │ -473 */ │ │ │ │ │ -474 template │ │ │ │ │ -_4_7_5 void _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_r_e_m_o_v_e_T_o_p(const _K_e_y_V_e_c_t_o_r& keys, BayesNetType* bn, │ │ │ │ │ -476 _C_l_i_q_u_e_s* orphans) { │ │ │ │ │ -477 gttic(removetop); │ │ │ │ │ -478 // process each key of the new factor │ │ │ │ │ -479 for (const _K_e_y& j : keys) { │ │ │ │ │ -480 // get the clique │ │ │ │ │ -481 // TODO(frank): Nodes will be searched again in removeClique │ │ │ │ │ -482 typename Nodes::const_iterator node = nodes_.find(j); │ │ │ │ │ -483 if (node != nodes_.end()) { │ │ │ │ │ -484 // remove path from clique to root │ │ │ │ │ -485 this->removePath(node->second, bn, orphans); │ │ │ │ │ -486 } │ │ │ │ │ -487 } │ │ │ │ │ -488 │ │ │ │ │ -489 // Delete cachedShortcuts for each orphan subtree │ │ │ │ │ -490 // TODO(frank): Consider Improving │ │ │ │ │ -491 for (_s_h_a_r_e_d_C_l_i_q_u_e& orphan : *orphans) orphan->deleteCachedShortcuts(); │ │ │ │ │ -492 } │ │ │ │ │ -493 │ │ │ │ │ -494 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -495 template │ │ │ │ │ -_4_9_6 typename _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_C_l_i_q_u_e_s _B_a_y_e_s_T_r_e_e_<_C_L_I_Q_U_E_>_:_:_r_e_m_o_v_e_S_u_b_t_r_e_e( │ │ │ │ │ -497 const _s_h_a_r_e_d_C_l_i_q_u_e& subtree) │ │ │ │ │ -498 { │ │ │ │ │ -499 // Result clique list │ │ │ │ │ -500 _C_l_i_q_u_e_s cliques; │ │ │ │ │ -501 cliques.push_back(subtree); │ │ │ │ │ -502 │ │ │ │ │ -503 // Remove the first clique from its parents │ │ │ │ │ -504 if(!subtree->isRoot()) │ │ │ │ │ -505 subtree->parent()->children.erase(std::find( │ │ │ │ │ -506 subtree->parent()->children.begin(), subtree->parent()->children.end(), │ │ │ │ │ -subtree)); │ │ │ │ │ -507 else │ │ │ │ │ -508 roots_.erase(std::find(roots_.begin(), roots_.end(), subtree)); │ │ │ │ │ -509 │ │ │ │ │ -510 // Add all subtree cliques and erase the children and parent of each │ │ │ │ │ -511 for(typename Cliques::iterator clique = cliques.begin(); clique != │ │ │ │ │ -cliques.end(); ++clique) │ │ │ │ │ -512 { │ │ │ │ │ -513 // Add children │ │ │ │ │ -514 for(const _s_h_a_r_e_d_C_l_i_q_u_e& child: (*clique)->children) { │ │ │ │ │ -515 cliques.push_back(child); } │ │ │ │ │ -516 │ │ │ │ │ -517 // Delete cached shortcuts │ │ │ │ │ -518 (*clique)->deleteCachedShortcutsNonRecursive(); │ │ │ │ │ -519 │ │ │ │ │ -520 // Remove this node from the nodes index │ │ │ │ │ -521 for(_K_e_y j: (*clique)->conditional()->frontals()) { │ │ │ │ │ -522 nodes_.unsafe_erase(j); } │ │ │ │ │ -523 │ │ │ │ │ -524 // Erase the parent and children pointers │ │ │ │ │ -525 (*clique)->parent_.reset(); │ │ │ │ │ -526 (*clique)->children.clear(); │ │ │ │ │ -527 } │ │ │ │ │ -528 │ │ │ │ │ -529 return cliques; │ │ │ │ │ -530 } │ │ │ │ │ -531 │ │ │ │ │ -532} │ │ │ │ │ +235 // Assign the remaining factors - these are pointers to factors in the │ │ │ │ │ +original factor graph and │ │ │ │ │ +236 // we do not clone them. │ │ │ │ │ +237 remainingFactors_ = other.remainingFactors_; │ │ │ │ │ +238 │ │ │ │ │ +239 return *this; │ │ │ │ │ +240} │ │ │ │ │ +241 │ │ │ │ │ +242/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +_2_4_3template │ │ │ │ │ +244std::pair, boost::shared_ptr > │ │ │ │ │ +_2_4_5_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_<_B_A_Y_E_S_T_R_E_E_,_ _G_R_A_P_H_>_:_:_e_l_i_m_i_n_a_t_e(const _E_l_i_m_i_n_a_t_e& │ │ │ │ │ +function) const { │ │ │ │ │ +246 gttic(ClusterTree_eliminate); │ │ │ │ │ +247 // Do elimination (depth-first traversal). The rootsContainer stores a │ │ │ │ │ +'dummy' BayesTree node │ │ │ │ │ +248 // that contains all of the roots as its children. rootsContainer also │ │ │ │ │ +stores the remaining │ │ │ │ │ +249 // un-eliminated factors passed up from the roots. │ │ │ │ │ +250 boost::shared_ptr result = boost::make_shared │ │ │ │ │ +(); │ │ │ │ │ +251 │ │ │ │ │ +252 typedef _E_l_i_m_i_n_a_t_i_o_n_D_a_t_a_<_T_h_i_s_> Data; │ │ │ │ │ +253 Data rootsContainer(0, this->nrRoots()); │ │ │ │ │ +254 │ │ │ │ │ +255 typename Data::EliminationPostOrderVisitor visitorPost(function, result- │ │ │ │ │ +>nodes_); │ │ │ │ │ +256 { │ │ │ │ │ +257 _T_b_b_O_p_e_n_M_P_M_i_x_e_d_S_c_o_p_e threadLimiter; // Limits OpenMP threads since we're │ │ │ │ │ +mixing TBB and OpenMP │ │ │ │ │ +258 _t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t_P_a_r_a_l_l_e_l(*this, rootsContainer, Data:: │ │ │ │ │ +EliminationPreOrderVisitor, │ │ │ │ │ +259 visitorPost, 10); │ │ │ │ │ +260 } │ │ │ │ │ +261 │ │ │ │ │ +262 // Create BayesTree from roots stored in the dummy BayesTree node. │ │ │ │ │ +263 result->roots_.insert(result->roots_.end(), rootsContainer.bayesTreeNode- │ │ │ │ │ +>children.begin(), │ │ │ │ │ +264 rootsContainer.bayesTreeNode->children.end()); │ │ │ │ │ +265 │ │ │ │ │ +266 // Add remaining factors that were not involved with eliminated variables │ │ │ │ │ +267 boost::shared_ptr remaining = boost:: │ │ │ │ │ +make_shared(); │ │ │ │ │ +268 remaining->reserve(remainingFactors_.size() + │ │ │ │ │ +rootsContainer.childFactors.size()); │ │ │ │ │ +269 remaining->push_back(remainingFactors_.begin(), remainingFactors_.end()); │ │ │ │ │ +270 for (const _s_h_a_r_e_d_F_a_c_t_o_r& factor : rootsContainer.childFactors) { │ │ │ │ │ +271 if (factor) │ │ │ │ │ +272 remaining->push_back(factor); │ │ │ │ │ +273 } │ │ │ │ │ +274 │ │ │ │ │ +275 // Return result │ │ │ │ │ +276 return std::make_pair(result, remaining); │ │ │ │ │ +277} │ │ │ │ │ +278 │ │ │ │ │ +279} // namespace gtsam │ │ │ │ │ _t_r_e_e_T_r_a_v_e_r_s_a_l_-_i_n_s_t_._h │ │ │ │ │ _t_i_m_i_n_g_._h │ │ │ │ │ Timing utilities. │ │ │ │ │ -_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ -Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ │ _O_r_d_e_r_i_n_g_._h │ │ │ │ │ Variable ordering for the elimination algorithm. │ │ │ │ │ +_C_l_u_s_t_e_r_T_r_e_e_._h │ │ │ │ │ +Collects factorgraph fragments defined on variable clusters, arranged in a │ │ │ │ │ +tree. │ │ │ │ │ +_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ +Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ +std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ +FastVector │ │ │ │ │ +FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ +DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_d_o_t │ │ │ │ │ -double dot(const V1 &a, const V2 &b) │ │ │ │ │ -Dot product. │ │ │ │ │ -DDeeffiinniittiioonn Vector.h:195 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_i_n_t_K_e_y_V_e_c_t_o_r │ │ │ │ │ +void PrintKeyVector(const KeyVector &keys, const string &s, const KeyFormatter │ │ │ │ │ +&keyFormatter) │ │ │ │ │ +Utility function to print sets of keys with optional prefix. │ │ │ │ │ +DDeeffiinniittiioonn Key.cpp:77 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t │ │ │ │ │ -void DepthFirstForest(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, │ │ │ │ │ -VISITOR_POST &visitorPost) │ │ │ │ │ -Traverse a forest depth-first with pre-order and post-order visits. │ │ │ │ │ -DDeeffiinniittiioonn treeTraversal-inst.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_C_l_o_n_e_F_o_r_e_s_t │ │ │ │ │ +FastVector< boost::shared_ptr< typename FOREST::Node > > CloneForest(const │ │ │ │ │ +FOREST &forest) │ │ │ │ │ +Clone a tree, copy-constructing new nodes (calling boost::make_shared) and │ │ │ │ │ +setting up child pointers ... │ │ │ │ │ +DDeeffiinniittiioonn treeTraversal-inst.h:189 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_P_r_i_n_t_F_o_r_e_s_t │ │ │ │ │ void PrintForest(const FOREST &forest, std::string str, const KeyFormatter │ │ │ │ │ &keyFormatter) │ │ │ │ │ Print a tree, prefixing each line with str, and formatting keys using │ │ │ │ │ keyFormatter. │ │ │ │ │ DDeeffiinniittiioonn treeTraversal-inst.h:219 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t │ │ │ │ │ -FastList is a thin wrapper around std::list that uses the boost │ │ │ │ │ -fast_pool_allocator instead of the de... │ │ │ │ │ -DDeeffiinniittiioonn FastList.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ -nodes. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_D_a_t_a │ │ │ │ │ -store all the sizes │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e │ │ │ │ │ -Bayes tree. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_n_o_d_e_s__ │ │ │ │ │ -Nodes nodes_ │ │ │ │ │ -Map from indices to Clique. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_r_e_m_o_v_e_C_l_i_q_u_e │ │ │ │ │ -void removeClique(sharedClique clique) │ │ │ │ │ -remove a clique: warning, can result in a forest │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:424 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_j_o_i_n_t │ │ │ │ │ -sharedFactorGraph joint(Key j1, Key j2, const Eliminate │ │ │ │ │ -&function=EliminationTraitsType::DefaultEliminate) const │ │ │ │ │ -return joint on two variables Limitation: can only calculate joint if cliques │ │ │ │ │ -are disjoint or one of ... │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:293 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_f_i_l_l_N_o_d_e_s_I_n_d_e_x │ │ │ │ │ -void fillNodesIndex(const sharedClique &subtree) │ │ │ │ │ -Fill the nodes index for a subtree. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:246 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_d_o_t │ │ │ │ │ -void dot(std::ostream &os, const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ -Output to graphviz format, stream version. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_a_d_d_F_a_c_t_o_r_s_T_o_G_r_a_p_h │ │ │ │ │ -void addFactorsToGraph(FactorGraph< FactorType > *graph) const │ │ │ │ │ -Add all cliques in this BayesTree to the specified factor graph. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:168 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const This &other, double tol=1e-9) const │ │ │ │ │ -check equality │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:230 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t_P_a_r_a_l_l_e_l │ │ │ │ │ +void DepthFirstForestParallel(FOREST &forest, DATA &rootData, VISITOR_PRE │ │ │ │ │ +&visitorPre, VISITOR_POST &visitorPost, int problemSizeThreshold=10) │ │ │ │ │ +Traverse a forest depth-first with pre-order and post-order visits. │ │ │ │ │ +DDeeffiinniittiioonn treeTraversal-inst.h:154 │ │ │ │ │ +_g_t_s_a_m_:_:_T_b_b_O_p_e_n_M_P_M_i_x_e_d_S_c_o_p_e │ │ │ │ │ +An object whose scope defines a block where TBB and OpenMP parallelism are │ │ │ │ │ +mixed. │ │ │ │ │ +DDeeffiinniittiioonn types.h:192 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e │ │ │ │ │ +A cluster-tree that eliminates to a Bayes tree. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:184 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ +This & operator=(const This &other) │ │ │ │ │ +Assignment operator - makes a deep copy of the tree structure, but only │ │ │ │ │ +pointers to factors are copie... │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree-inst.h:231 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_e_l_i_m_i_n_a_t_e │ │ │ │ │ +std::pair< boost::shared_ptr< BayesTreeType >, boost::shared_ptr< │ │ │ │ │ +FactorGraphType > > eliminate(const Eliminate &function) const │ │ │ │ │ +Eliminate the factors to a Bayes tree and remaining factor graph. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree-inst.h:245 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_s_h_a_r_e_d_F_a_c_t_o_r │ │ │ │ │ +boost::shared_ptr< FactorType > sharedFactor │ │ │ │ │ +Shared pointer to a factor. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:197 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_:_:_E_l_i_m_i_n_a_t_e │ │ │ │ │ +GRAPH::Eliminate Eliminate │ │ │ │ │ +Typedef for an eliminate subroutine. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:195 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:276 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_D_a_t_a │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree-inst.h:115 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_D_a_t_a_:_:_E_l_i_m_i_n_a_t_i_o_n_P_o_s_t_O_r_d_e_r_V_i_s_i_t_o_r │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree-inst.h:169 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e │ │ │ │ │ +A cluster-tree is associated with a factor graph and is defined as in Koller- │ │ │ │ │ +Friedman: each node k re... │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:25 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ This & operator=(const This &other) │ │ │ │ │ -Assignment operator. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:199 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_s_h_a_r_e_d_C_l_i_q_u_e │ │ │ │ │ -boost::shared_ptr< Clique > sharedClique │ │ │ │ │ -Shared pointer to a clique. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_B_a_y_e_s_T_r_e_e │ │ │ │ │ -BayesTree() │ │ │ │ │ -Create an empty Bayes Tree. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:109 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_c_l_e_a_r │ │ │ │ │ -void clear() │ │ │ │ │ -Remove all nodes. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:408 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_a_d_d_C_l_i_q_u_e │ │ │ │ │ -void addClique(const sharedClique &clique, const sharedClique │ │ │ │ │ -&parent_clique=sharedClique()) │ │ │ │ │ -add a clique (top down) │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:142 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_j_o_i_n_t_B_a_y_e_s_N_e_t │ │ │ │ │ -sharedBayesNet jointBayesNet(Key j1, Key j2, const Eliminate │ │ │ │ │ -&function=EliminationTraitsType::DefaultEliminate) const │ │ │ │ │ -return joint on two variables as a BayesNet Limitation: can only calculate │ │ │ │ │ -joint if cliques are disjo... │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:302 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_f_i_n_d_P_a_r_e_n_t_C_l_i_q_u_e │ │ │ │ │ -Key findParentClique(const CONTAINER &parents) const │ │ │ │ │ -Find parent clique of a conditional. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:238 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_s_i_z_e │ │ │ │ │ -size_t size() const │ │ │ │ │ -number of cliques │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:133 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_d_e_l_e_t_e_C_a_c_h_e_d_S_h_o_r_t_c_u_t_s │ │ │ │ │ -void deleteCachedShortcuts() │ │ │ │ │ -Clear all shortcut caches - use before timing on marginal calculation to avoid │ │ │ │ │ -residual cache data. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:416 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_r_e_m_o_v_e_P_a_t_h │ │ │ │ │ -void removePath(sharedClique clique, BayesNetType *bn, Cliques *orphans) │ │ │ │ │ -Remove path from clique to root and return that path as factors plus a list of │ │ │ │ │ -orphaned subtree roots... │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:448 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_m_a_r_g_i_n_a_l_F_a_c_t_o_r │ │ │ │ │ -sharedConditional marginalFactor(Key j, const Eliminate │ │ │ │ │ -&function=EliminationTraitsType::DefaultEliminate) const │ │ │ │ │ -Return marginal on any variable. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:270 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_n_u_m_C_a_c_h_e_d_S_e_p_a_r_a_t_o_r_M_a_r_g_i_n_a_l_s │ │ │ │ │ -size_t numCachedSeparatorMarginals() const │ │ │ │ │ -Collect number of cliques with cached separator marginals. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_g_e_t_C_l_i_q_u_e_D_a_t_a │ │ │ │ │ -BayesTreeCliqueData getCliqueData() const │ │ │ │ │ -Gather data on all cliques. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_r_e_m_o_v_e_S_u_b_t_r_e_e │ │ │ │ │ -Cliques removeSubtree(const sharedClique &subtree) │ │ │ │ │ -Remove the requested subtree. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:496 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_p_r_i_n_t │ │ │ │ │ +Assignment operator - makes a deep copy of the tree structure, but only │ │ │ │ │ +pointers to factors are copie... │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree-inst.h:104 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_r_o_o_t_s__ │ │ │ │ │ +FastVector< sharedNode > roots_ │ │ │ │ │ +concept check │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:116 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_p_r_i_n_t │ │ │ │ │ void print(const std::string &s="", const KeyFormatter │ │ │ │ │ &keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:212 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_i_n_s_e_r_t_R_o_o_t │ │ │ │ │ -void insertRoot(const sharedClique &subtree) │ │ │ │ │ -Insert a new subtree with known parent clique. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:260 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_s_a_v_e_G_r_a_p_h │ │ │ │ │ -void saveGraph(const std::string &filename, const KeyFormatter │ │ │ │ │ +Print the cluster tree. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree-inst.h:98 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_<_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_s_h_a_r_e_d_F_a_c_t_o_r │ │ │ │ │ +boost::shared_ptr< FactorType > sharedFactor │ │ │ │ │ +Shared pointer to a factor. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_p_r_i_n_t │ │ │ │ │ +virtual void print(const std::string &s="", const KeyFormatter │ │ │ │ │ &keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ -output to file with graphviz format. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_r_e_m_o_v_e_T_o_p │ │ │ │ │ -void removeTop(const KeyVector &keys, BayesNetType *bn, Cliques *orphans) │ │ │ │ │ -Given a list of indices, turn "contaminated" part of the tree back into a │ │ │ │ │ -factor graph. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:475 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +print this node │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree-inst.h:26 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_o_r_d_e_r_e_d_F_r_o_n_t_a_l_K_e_y_s │ │ │ │ │ +Keys orderedFrontalKeys │ │ │ │ │ +Frontal keys of this node. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_m_e_r_g_e_C_h_i_l_d_r_e_n │ │ │ │ │ +void mergeChildren(const std::vector< bool > &merge) │ │ │ │ │ +Merge all children for which bit is set into this node. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree-inst.h:56 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_m_e_r_g_e │ │ │ │ │ +void merge(const boost::shared_ptr< Cluster > &cluster) │ │ │ │ │ +Merge in given cluster. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree-inst.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r_:_:_n_r_F_r_o_n_t_a_l_s_O_f_C_h_i_l_d_r_e_n │ │ │ │ │ +std::vector< size_t > nrFrontalsOfChildren() const │ │ │ │ │ +Return a vector with nrFrontal keys for each child. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree-inst.h:34 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _B_a_y_e_s_T_r_e_e_-_i_n_s_t_._h │ │ │ │ │ + * _C_l_u_s_t_e_r_T_r_e_e_-_i_n_s_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00704.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianBayesTree-inl.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/linearExceptions.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,51 +94,36 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
GaussianBayesTree-inl.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
linearExceptions.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ +

Exceptions that may be thrown by linear solver components. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -template<class BAYESTREE >
void gtsam::internal::optimizeInPlace (const typename BAYESTREE::sharedClique &clique, VectorValues &result)
 
│ │ │ │ -template<class BAYESTREE >
double gtsam::internal::logDeterminant (const typename BAYESTREE::sharedClique &clique)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.

│ │ │ │ -

GaussianBayesTree

Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ +

Exceptions that may be thrown by linear solver components.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Aug 17, 2012
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,32 +1,22 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -GaussianBayesTree-inl.h File Reference │ │ │ │ │ -Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +linearExceptions.cpp File Reference │ │ │ │ │ +Exceptions that may be thrown by linear solver components. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ - void  ggttssaamm::::iinntteerrnnaall::::ooppttiimmiizzeeIInnPPllaaccee (const typename BAYESTREE:: │ │ │ │ │ - sharedClique &clique, _V_e_c_t_o_r_V_a_l_u_e_s &result) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -double  ggttssaamm::::iinntteerrnnaall::::llooggDDeetteerrmmiinnaanntt (const typename BAYESTREE::sharedClique │ │ │ │ │ - &clique) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ │ -GaussianBayesTree │ │ │ │ │ +Exceptions that may be thrown by linear solver components. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Aug 17, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_-_i_n_l_._h │ │ │ │ │ + * _l_i_n_e_a_r_E_x_c_e_p_t_i_o_n_s_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00710.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/VectorValues.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/KalmanFilter.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,50 +94,39 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
VectorValues.cpp File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
KalmanFilter.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Implementations for VectorValues. │ │ │ │ +

Simple linear Kalman filter. │ │ │ │ More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -GTSAM_EXPORT ostream & gtsam::operator<< (ostream &os, const VectorValues &v)
 
│ │ │ │ -bool gtsam::internal::structureCompareOp (const boost::tuple< VectorValues::value_type, VectorValues::value_type > &vv)
 
│ │ │ │ -VectorValues gtsam::operator* (const double a, const VectorValues &v)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Implementations for VectorValues.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ +

Simple linear Kalman filter.

│ │ │ │ +

Implemented using factor graphs, i.e., does Cholesky-based SRIF, really.

│ │ │ │ +
Date
Sep 3, 2011
│ │ │ │ +
Author
Stephen Williams
│ │ │ │
│ │ │ │ -Alex Cunningham
│ │ │ │ +Frank Dellaert
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,31 +1,24 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -VectorValues.cpp File Reference │ │ │ │ │ -Implementations for _V_e_c_t_o_r_V_a_l_u_e_s. _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +KalmanFilter.cpp File Reference │ │ │ │ │ +Simple linear Kalman filter. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -GTSAM_EXPORT ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const _V_e_c_t_o_r_V_a_l_u_e_s &v) │ │ │ │ │ -  │ │ │ │ │ - bool  ggttssaamm::::iinntteerrnnaall::::ssttrruuccttuurreeCCoommppaarreeOOpp (const boost:: │ │ │ │ │ - tuple< _V_e_c_t_o_r_V_a_l_u_e_s_:_:_v_a_l_u_e___t_y_p_e, _V_e_c_t_o_r_V_a_l_u_e_s_:_: │ │ │ │ │ - _v_a_l_u_e___t_y_p_e > &vv) │ │ │ │ │ -  │ │ │ │ │ - _V_e_c_t_o_r_V_a_l_u_e_s  ggttssaamm::::ooppeerraattoorr** (const double a, const _V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ - &v) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Implementations for _V_e_c_t_o_r_V_a_l_u_e_s. │ │ │ │ │ +Simple linear Kalman filter. │ │ │ │ │ +Implemented using factor graphs, i.e., does Cholesky-based SRIF, really. │ │ │ │ │ + Date │ │ │ │ │ + Sep 3, 2011 │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Alex Cunningham │ │ │ │ │ + Stephen Williams │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _V_e_c_t_o_r_V_a_l_u_e_s_._c_p_p │ │ │ │ │ + * _K_a_l_m_a_n_F_i_l_t_e_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00713.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianEliminationTree.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/IterativeSolver.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,42 +94,43 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
GaussianEliminationTree.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
IterativeSolver.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Some support classes for iterative solvers. │ │ │ │ +More...

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::GaussianEliminationTree
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +ostream & gtsam::operator<< (ostream &os, const IterativeOptimizationParameters &p)
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Mar 29, 2013
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ +

Some support classes for iterative solvers.

│ │ │ │ +
Date
Sep 3, 2012
│ │ │ │ +
Author
Yong-Dian Jian
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,25 +1,26 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -GaussianEliminationTree.h File Reference │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +IterativeSolver.cpp File Reference │ │ │ │ │ +Some support classes for iterative solvers. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const │ │ │ │ │ + _I_t_e_r_a_t_i_v_e_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s &p) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Some support classes for iterative solvers. │ │ │ │ │ Date │ │ │ │ │ - Mar 29, 2013 │ │ │ │ │ + Sep 3, 2012 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Yong-Dian Jian │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h │ │ │ │ │ + * _I_t_e_r_a_t_i_v_e_S_o_l_v_e_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00716.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianConditional.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/PowerMethod.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,35 +94,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
GaussianConditional.cpp File Reference
│ │ │ │ +
PowerMethod.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Conditional Gaussian Base class. │ │ │ │ +

Power method for fast eigenvalue and eigenvector computation. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::PowerMethod< Operator >
 Compute maximum Eigenpair with power method. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Conditional Gaussian Base class.

│ │ │ │ -
Author
Christian Potthast, Frank Dellaert
│ │ │ │ +

Power method for fast eigenvalue and eigenvector computation.

│ │ │ │ +
Date
Sept 2020
│ │ │ │ +
Author
Jing Wu
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,27 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -GaussianConditional.cpp File Reference │ │ │ │ │ -Conditional Gaussian Base class. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +PowerMethod.h File Reference │ │ │ │ │ +Power method for fast eigenvalue and eigenvector computation. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_P_o_w_e_r_M_e_t_h_o_d_<_ _O_p_e_r_a_t_o_r_ _> │ │ │ │ │ +  Compute maximum Eigenpair with power method. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Conditional Gaussian Base class. │ │ │ │ │ +Power method for fast eigenvalue and eigenvector computation. │ │ │ │ │ + Date │ │ │ │ │ + Sept 2020 │ │ │ │ │ Author │ │ │ │ │ - Christian Potthast, Frank Dellaert │ │ │ │ │ + Jing Wu │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_._c_p_p │ │ │ │ │ + * _P_o_w_e_r_M_e_t_h_o_d_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00719.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/JacobianFactor.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/linearAlgorithms-inst.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,53 +94,54 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs | │ │ │ │ Functions
│ │ │ │ -
JacobianFactor.cpp File Reference
│ │ │ │ +
linearAlgorithms-inst.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ + │ │ │ │ +

Templated algorithms that are used in multiple places in linear. │ │ │ │ +More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

struct  gtsam::internal::linearAlgorithms::OptimizeData
 
struct  gtsam::internal::linearAlgorithms::OptimizeClique< CLIQUE >
 Pre-order visitor for back-substitution in a Bayes tree. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Typedefs

│ │ │ │ -using gtsam::Pairs = std::vector< std::pair< Key, Matrix > >
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

│ │ │ │ -FastVector< VariableSlots::const_iterator > gtsam::orderedSlotsHelper (const Ordering &ordering, const VariableSlots &variableSlots)
 
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > gtsam::EliminateQR (const GaussianFactorGraph &factors, const Ordering &keys)
 Multiply all factors and eliminate the given keys from the resulting factor using a QR variant that handles constraints (zero sigmas).
 
│ │ │ │ +template<class BAYESTREE >
VectorValues gtsam::internal::linearAlgorithms::optimizeBayesTree (const BAYESTREE &bayesTree)
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
│ │ │ │ -Christian Potthast
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ -
Date
Dec 8, 2010
│ │ │ │ +

Templated algorithms that are used in multiple places in linear.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,38 +1,32 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -JacobianFactor.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +linearAlgorithms-inst.h File Reference │ │ │ │ │ +Templated algorithms that are used in multiple places in linear. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +struct   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_l_i_n_e_a_r_A_l_g_o_r_i_t_h_m_s_:_:_O_p_t_i_m_i_z_e_D_a_t_a │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_l_i_n_e_a_r_A_l_g_o_r_i_t_h_m_s_:_:_O_p_t_i_m_i_z_e_C_l_i_q_u_e_<_ _C_L_I_Q_U_E_ _> │ │ │ │ │ +  Pre-order visitor for back-substitution in a Bayes tree. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -using  ggttssaamm::::PPaaiirrss = std::vector< std::pair< _K_e_y, Matrix > > │ │ │ │ │ -  │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ - _F_a_s_t_V_e_c_t_o_r< VariableSlots:: ggttssaamm::::oorrddeerreeddSSlloottssHHeellppeerr (const │ │ │ │ │ - const_iterator >  _O_r_d_e_r_i_n_g &ordering, const _V_a_r_i_a_b_l_e_S_l_o_t_s │ │ │ │ │ - &variableSlots) │ │ │ │ │ -  │ │ │ │ │ - std::pair< _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_: _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_Q_R (const │ │ │ │ │ -_s_h_a_r_e_d___p_t_r, _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &factors, const │ │ │ │ │ - >  _O_r_d_e_r_i_n_g &keys) │ │ │ │ │ - Multiply all factors and eliminate the │ │ │ │ │ -  given keys from the resulting factor │ │ │ │ │ - using a QR variant that handles │ │ │ │ │ - constraints (zero sigmas). │ │ │ │ │ +template │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s  ggttssaamm::::iinntteerrnnaall::::lliinneeaarrAAllggoorriitthhmmss::::ooppttiimmiizzeeBBaayyeessTTrreeee (const │ │ │ │ │ + BAYESTREE &bayesTree) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Templated algorithms that are used in multiple places in linear. │ │ │ │ │ Author │ │ │ │ │ Richard Roberts │ │ │ │ │ - Christian Potthast │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - Dec 8, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _J_a_c_o_b_i_a_n_F_a_c_t_o_r_._c_p_p │ │ │ │ │ + * _l_i_n_e_a_r_A_l_g_o_r_i_t_h_m_s_-_i_n_s_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00719.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,4 @@ │ │ │ │ │ var a00719 = [ │ │ │ │ │ - ["EliminateQR", "a00719.html#ab56c4bfa363f8c23c48eb078e9c84a9c", null] │ │ │ │ │ + ["gtsam::internal::linearAlgorithms::OptimizeData", "a03856.html", null], │ │ │ │ │ + ["gtsam::internal::linearAlgorithms::OptimizeClique< CLIQUE >", "a03860.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00722.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/iterative-inl.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/SubgraphSolver.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,51 +95,48 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
iterative-inl.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
SubgraphSolver.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Iterative methods, template implementation. │ │ │ │ +

Subgraph Solver from IROS 2010. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

struct  gtsam::CGState< S, V, E >
struct  gtsam::SubgraphSolverParameters
 
class  gtsam::SubgraphSolver
 This class implements the linear SPCG solver presented in Dellaert et al in IROS'10. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

template<class S , class V , class E >
V gtsam::conjugateGradients (const S &Ab, V x, const ConjugateGradientParameters &parameters, bool steepest=false)
 Method of conjugate gradients (CG) template "System" class S needs gradient(S,v), e=S*v, v=S^e "Vector" class V needs dot(v,v), -v, v+v, s*v "Vector" class E needs dot(v,v)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Iterative methods, template implementation.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
Date
Dec 28, 2009
│ │ │ │ +

Subgraph Solver from IROS 2010.

│ │ │ │ +
Date
2010
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ +Yong Dian Jian
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,34 +1,31 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -iterative-inl.h File Reference │ │ │ │ │ -Iterative methods, template implementation. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +SubgraphSolver.h File Reference │ │ │ │ │ +Subgraph Solver from IROS 2010. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_C_G_S_t_a_t_e_<_ _S_,_ _V_,_ _E_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_S_o_l_v_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_S_o_l_v_e_r │ │ │ │ │ +  This class implements the linear SPCG solver presented in Dellaert et │ │ │ │ │ + al in IROS'10. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ -V  _g_t_s_a_m_:_:_c_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_s (const S &Ab, V x, const │ │ │ │ │ - _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s ¶meters, bool steepest=false) │ │ │ │ │ - Method of conjugate gradients (CG) template "System" class S needs gradient │ │ │ │ │ -  (S,v), e=S*v, v=S^e "Vector" class V needs dot(v,v), -v, v+v, s*v "Vector" │ │ │ │ │ - class E needs dot(v,v) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Iterative methods, template implementation. │ │ │ │ │ +Subgraph Solver from IROS 2010. │ │ │ │ │ + Date │ │ │ │ │ + 2010 │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - Dec 28, 2009 │ │ │ │ │ + Yong Dian Jian │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _i_t_e_r_a_t_i_v_e_-_i_n_l_._h │ │ │ │ │ + * _S_u_b_g_r_a_p_h_S_o_l_v_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00722.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a00722 = [ │ │ │ │ │ - ["gtsam::CGState< S, V, E >", "a03820.html", "a03820"], │ │ │ │ │ - ["conjugateGradients", "a00722.html#a6516ea957c3a22ddf429cefef1fe9486", null] │ │ │ │ │ + ["gtsam::SubgraphSolverParameters", "a04056.html", null], │ │ │ │ │ + ["gtsam::SubgraphSolver", "a04060.html", "a04060"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00722_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/iterative-inl.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/SubgraphSolver.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,179 +98,114 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
iterative-inl.h
│ │ │ │ +
SubgraphSolver.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/linear/iterative.h>
│ │ │ │ -
22#include <gtsam/linear/ConjugateGradientSolver.h>
│ │ │ │ -
23#include <boost/shared_ptr.hpp>
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/linear/ConjugateGradientSolver.h>
│ │ │ │ +
23#include <gtsam/linear/SubgraphBuilder.h>
│ │ │ │
24
│ │ │ │ -
25namespace gtsam {
│ │ │ │ -
26
│ │ │ │ -
27 /* ************************************************************************* */
│ │ │ │ -
28 // state for CG method
│ │ │ │ -
29 template<class S, class V, class E>
│ │ │ │ -
│ │ │ │ -
30 struct CGState {
│ │ │ │ -
31
│ │ │ │ -
32 typedef ConjugateGradientParameters Parameters;
│ │ │ │ -
33 const Parameters &parameters_;
│ │ │ │ +
25#include <map>
│ │ │ │ +
26#include <utility> // pair
│ │ │ │ +
27
│ │ │ │ +
28namespace gtsam {
│ │ │ │ +
29
│ │ │ │ +
30// Forward declarations
│ │ │ │ +
31class GaussianFactorGraph;
│ │ │ │ +
32class GaussianBayesNet;
│ │ │ │ +
33class SubgraphPreconditioner;
│ │ │ │
34
│ │ │ │ -
35 int k;
│ │ │ │ -
36 bool steepest;
│ │ │ │ -
37 V g, d;
│ │ │ │ -
38 double gamma, threshold;
│ │ │ │ -
39 E Ad;
│ │ │ │ -
40
│ │ │ │ -
41 /* ************************************************************************* */
│ │ │ │ -
42 // Constructor
│ │ │ │ -
43 CGState(const S& Ab, const V& x, const Parameters &parameters, bool steep):
│ │ │ │ -
44 parameters_(parameters),k(0),steepest(steep) {
│ │ │ │ +
│ │ │ │ +
35struct GTSAM_EXPORT SubgraphSolverParameters
│ │ │ │ +
36 : public ConjugateGradientParameters {
│ │ │ │ +
37 SubgraphBuilderParameters builderParams;
│ │ │ │ +
38 explicit SubgraphSolverParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters())
│ │ │ │ +
39 : builderParams(p) {}
│ │ │ │ +
40 void print() const { Base::print(); }
│ │ │ │ +
41 void print(std::ostream &os) const override {
│ │ │ │ +
42 Base::print(os);
│ │ │ │ +
43 }
│ │ │ │ +
44};
│ │ │ │ +
│ │ │ │
45
│ │ │ │ -
46 // Start with g0 = A'*(A*x0-b), d0 = - g0
│ │ │ │ -
47 // i.e., first step is in direction of negative gradient
│ │ │ │ -
48 g = Ab.gradient(x);
│ │ │ │ -
49 d = g; // instead of negating gradient, alpha will be negated
│ │ │ │ -
50
│ │ │ │ -
51 // init gamma and calculate threshold
│ │ │ │ -
52 gamma = dot(g,g);
│ │ │ │ -
53 threshold = std::max(parameters_.epsilon_abs(), parameters_.epsilon() * parameters_.epsilon() * gamma);
│ │ │ │ -
54
│ │ │ │ -
55 // Allocate and calculate A*d for first iteration
│ │ │ │ -
56 if (gamma > parameters_.epsilon_abs()) Ad = Ab * d;
│ │ │ │ -
57 }
│ │ │ │ -
58
│ │ │ │ -
59 /* ************************************************************************* */
│ │ │ │ -
60 // print
│ │ │ │ -
61 void print(const V& x) {
│ │ │ │ -
62 std::cout << "iteration = " << k << std::endl;
│ │ │ │ -
63 gtsam::print(x,"x");
│ │ │ │ -
64 gtsam::print(g, "g");
│ │ │ │ -
65 std::cout << "dotg = " << gamma << std::endl;
│ │ │ │ -
66 gtsam::print(d, "d");
│ │ │ │ -
67 gtsam::print(Ad, "Ad");
│ │ │ │ -
68 }
│ │ │ │ -
69
│ │ │ │ -
70 /* ************************************************************************* */
│ │ │ │ -
71 // step the solution
│ │ │ │ -
72 double takeOptimalStep(V& x) {
│ │ │ │ -
73 // TODO: can we use gamma instead of dot(d,g) ????? Answer not trivial
│ │ │ │ -
74 double alpha = -dot(d, g) / dot(Ad, Ad); // calculate optimal step-size
│ │ │ │ -
75 x += alpha * d; // do step in new search direction, x += alpha*d
│ │ │ │ -
76 return alpha;
│ │ │ │ -
77 }
│ │ │ │ -
78
│ │ │ │ -
79 /* ************************************************************************* */
│ │ │ │ -
80 // take a step, return true if converged
│ │ │ │ -
81 bool step(const S& Ab, V& x) {
│ │ │ │ -
82
│ │ │ │ -
83 if ((++k) >= ((int)parameters_.maxIterations())) return true;
│ │ │ │ -
84
│ │ │ │ -
85 //---------------------------------->
│ │ │ │ -
86 double alpha = takeOptimalStep(x);
│ │ │ │ +
│ │ │ │ +
76class GTSAM_EXPORT SubgraphSolver : public IterativeSolver {
│ │ │ │ +
77 public:
│ │ │ │ +
78 typedef SubgraphSolverParameters Parameters;
│ │ │ │ +
79
│ │ │ │ +
80 protected:
│ │ │ │ +
81 Parameters parameters_;
│ │ │ │ +
82 boost::shared_ptr<SubgraphPreconditioner> pc_;
│ │ │ │ +
83
│ │ │ │ +
84 public:
│ │ │ │
87
│ │ │ │ -
88 // update gradient (or re-calculate at reset time)
│ │ │ │ -
89 if (k % parameters_.reset() == 0) g = Ab.gradient(x);
│ │ │ │ -
90 // axpy(alpha, Ab ^ Ad, g); // g += alpha*(Ab^Ad)
│ │ │ │ -
91 else Ab.transposeMultiplyAdd(alpha, Ad, g);
│ │ │ │ -
92
│ │ │ │ -
93 // check for convergence
│ │ │ │ -
94 double new_gamma = dot(g, g);
│ │ │ │ +
93 SubgraphSolver(const GaussianFactorGraph &A, const Parameters &parameters,
│ │ │ │ +
94 const Ordering &ordering);
│ │ │ │
95
│ │ │ │ -
96 if (parameters_.verbosity() != ConjugateGradientParameters::SILENT)
│ │ │ │ -
97 std::cout << "iteration " << k << ": alpha = " << alpha
│ │ │ │ -
98 << ", dotg = " << new_gamma
│ │ │ │ -
99 << std::endl;
│ │ │ │ -
100
│ │ │ │ -
101 if (new_gamma < threshold) return true;
│ │ │ │ -
102
│ │ │ │ -
103 // calculate new search direction
│ │ │ │ -
104 if (steepest) d = g;
│ │ │ │ -
105 else {
│ │ │ │ -
106 double beta = new_gamma / gamma;
│ │ │ │ -
107 // d = g + d*beta;
│ │ │ │ -
108 d *= beta;
│ │ │ │ -
109 d += 1.0 * g;
│ │ │ │ -
110 }
│ │ │ │ -
111
│ │ │ │ -
112 gamma = new_gamma;
│ │ │ │ +
102 SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2,
│ │ │ │ +
103 const Parameters &parameters, const Ordering &ordering);
│ │ │ │ +
108 SubgraphSolver(const GaussianBayesNet &Rc1, const GaussianFactorGraph &Ab2,
│ │ │ │ +
109 const Parameters &parameters);
│ │ │ │ +
110
│ │ │ │ +
112 ~SubgraphSolver() override {}
│ │ │ │
113
│ │ │ │ -
114 // In-place recalculation Ad <- A*d to avoid re-allocating Ad
│ │ │ │ -
115 Ab.multiplyInPlace(d, Ad);
│ │ │ │ -
116 return false;
│ │ │ │ -
117 }
│ │ │ │ -
118
│ │ │ │ -
119 }; // CGState Class
│ │ │ │ -
│ │ │ │ +
117
│ │ │ │ +
119 VectorValues optimize() const;
│ │ │ │
120
│ │ │ │ -
121 /* ************************************************************************* */
│ │ │ │ -
122 // conjugate gradient method.
│ │ │ │ -
123 // S: linear system, V: step vector, E: errors
│ │ │ │ -
124 template<class S, class V, class E>
│ │ │ │ -
│ │ │ │ -
125 V conjugateGradients(const S& Ab, V x, const ConjugateGradientParameters &parameters, bool steepest) {
│ │ │ │ +
122 VectorValues optimize(const GaussianFactorGraph &gfg,
│ │ │ │ +
123 const KeyInfo &keyInfo,
│ │ │ │ +
124 const std::map<Key, Vector> &lambda,
│ │ │ │ +
125 const VectorValues &initial) override;
│ │ │ │
126
│ │ │ │ -
127 CGState<S, V, E> state(Ab, x, parameters, steepest);
│ │ │ │ -
128
│ │ │ │ -
129 if (parameters.verbosity() != ConjugateGradientParameters::SILENT)
│ │ │ │ -
130 std::cout << "CG: epsilon = " << parameters.epsilon()
│ │ │ │ -
131 << ", maxIterations = " << parameters.maxIterations()
│ │ │ │ -
132 << ", ||g0||^2 = " << state.gamma
│ │ │ │ -
133 << ", threshold = " << state.threshold
│ │ │ │ -
134 << std::endl;
│ │ │ │ -
135
│ │ │ │ -
136 if ( state.gamma < state.threshold ) {
│ │ │ │ -
137 if (parameters.verbosity() != ConjugateGradientParameters::SILENT)
│ │ │ │ -
138 std::cout << "||g0||^2 < threshold, exiting immediately !" << std::endl;
│ │ │ │ -
139
│ │ │ │ -
140 return x;
│ │ │ │ -
141 }
│ │ │ │ -
142
│ │ │ │ -
143 // loop maxIterations times
│ │ │ │ -
144 while (!state.step(Ab, x)) {}
│ │ │ │ -
145 return x;
│ │ │ │ -
146 }
│ │ │ │ -
│ │ │ │ -
147/* ************************************************************************* */
│ │ │ │ -
148
│ │ │ │ -
149} // namespace gtsam
│ │ │ │ -
ConjugateGradientSolver.h
Implementation of Conjugate Gradient solver for a linear system.
│ │ │ │ -
iterative.h
Iterative methods, implementation.
│ │ │ │ +
130
│ │ │ │ +
132 std::pair<GaussianFactorGraph, GaussianFactorGraph> splitGraph(
│ │ │ │ +
133 const GaussianFactorGraph &gfg);
│ │ │ │ +
134
│ │ │ │ +
136};
│ │ │ │ +
│ │ │ │ +
137
│ │ │ │ +
138} // namespace gtsam
│ │ │ │ +
ConjugateGradientSolver.h
Implementation of Conjugate Gradient solver for a linear system.
│ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::conjugateGradients
V conjugateGradients(const S &Ab, V x, const ConjugateGradientParameters &parameters, bool steepest)
Method of conjugate gradients (CG) template "System" class S needs gradient(S,v), e=S*v,...
Definition iterative-inl.h:125
│ │ │ │ -
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
│ │ │ │ +
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │
gtsam::ConjugateGradientParameters
parameters for the conjugate gradient method
Definition ConjugateGradientSolver.h:29
│ │ │ │ -
gtsam::CGState
Definition iterative-inl.h:30
│ │ │ │ -
gtsam::CGState::steepest
bool steepest
flag to indicate we are doing steepest descent
Definition iterative-inl.h:36
│ │ │ │ -
gtsam::CGState::threshold
double threshold
gamma (squared L2 norm of g) and convergence threshold
Definition iterative-inl.h:38
│ │ │ │ -
gtsam::CGState::k
int k
iteration
Definition iterative-inl.h:35
│ │ │ │ -
gtsam::CGState::d
V d
gradient g and search direction d for CG
Definition iterative-inl.h:37
│ │ │ │ +
gtsam::GaussianBayesNet
GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals.
Definition GaussianBayesNet.h:36
│ │ │ │ +
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ +
gtsam::IterativeSolver
Base class for Iterative Solvers like SubgraphSolver.
Definition IterativeSolver.h:86
│ │ │ │ +
gtsam::KeyInfo
Handy data structure for iterative solvers.
Definition IterativeSolver.h:126
│ │ │ │ +
gtsam::SubgraphBuilderParameters
Definition SubgraphBuilder.h:96
│ │ │ │ +
gtsam::SubgraphSolverParameters
Definition SubgraphSolver.h:36
│ │ │ │ +
gtsam::SubgraphSolver
This class implements the linear SPCG solver presented in Dellaert et al in IROS'10.
Definition SubgraphSolver.h:76
│ │ │ │ +
gtsam::SubgraphSolver::~SubgraphSolver
~SubgraphSolver() override
Destructor.
Definition SubgraphSolver.h:112
│ │ │ │ +
gtsam::SubgraphSolver::pc_
boost::shared_ptr< SubgraphPreconditioner > pc_
preconditioner object
Definition SubgraphSolver.h:82
│ │ │ │ +
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,206 +1,139 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -iterative-inl.h │ │ │ │ │ +SubgraphSolver.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_i_t_e_r_a_t_i_v_e_._h> │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ 22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_S_o_l_v_e_r_._h> │ │ │ │ │ -23#include │ │ │ │ │ +23#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_._h> │ │ │ │ │ 24 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ -26 │ │ │ │ │ -27 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -28 // state for CG method │ │ │ │ │ -29 template │ │ │ │ │ -_3_0 struct _C_G_S_t_a_t_e { │ │ │ │ │ -31 │ │ │ │ │ -32 typedef _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s _P_a_r_a_m_e_t_e_r_s; │ │ │ │ │ -33 const _P_a_r_a_m_e_t_e_r_s ¶meters_; │ │ │ │ │ +25#include │ │ │ │ │ +26#include // pair │ │ │ │ │ +27 │ │ │ │ │ +28namespace _g_t_s_a_m { │ │ │ │ │ +29 │ │ │ │ │ +30// Forward declarations │ │ │ │ │ +31class GaussianFactorGraph; │ │ │ │ │ +32class GaussianBayesNet; │ │ │ │ │ +33class SubgraphPreconditioner; │ │ │ │ │ 34 │ │ │ │ │ -_3_5 int _k; │ │ │ │ │ -_3_6 bool _s_t_e_e_p_e_s_t; │ │ │ │ │ -_3_7 V g, _d; │ │ │ │ │ -_3_8 double gamma, _t_h_r_e_s_h_o_l_d; │ │ │ │ │ -39 E Ad; │ │ │ │ │ -40 │ │ │ │ │ -41 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -42 // Constructor │ │ │ │ │ -43 _C_G_S_t_a_t_e(const S& Ab, const V& x, const _P_a_r_a_m_e_t_e_r_s ¶meters, bool steep): │ │ │ │ │ -44 parameters_(parameters),_k(0),_s_t_e_e_p_e_s_t(steep) { │ │ │ │ │ +_3_5struct GTSAM_EXPORT _S_u_b_g_r_a_p_h_S_o_l_v_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ +36 : public _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s { │ │ │ │ │ +37 _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s builderParams; │ │ │ │ │ +38 explicit _S_u_b_g_r_a_p_h_S_o_l_v_e_r_P_a_r_a_m_e_t_e_r_s(const _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s &p = │ │ │ │ │ +_S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s()) │ │ │ │ │ +39 : builderParams(p) {} │ │ │ │ │ +40 void _p_r_i_n_t() const { Base::print(); } │ │ │ │ │ +41 void _p_r_i_n_t(std::ostream &os) const override { │ │ │ │ │ +42 Base::print(os); │ │ │ │ │ +43 } │ │ │ │ │ +44}; │ │ │ │ │ 45 │ │ │ │ │ -46 // Start with g0 = A'*(A*x0-b), d0 = - g0 │ │ │ │ │ -47 // i.e., first step is in direction of negative gradient │ │ │ │ │ -48 g = Ab.gradient(x); │ │ │ │ │ -49 _d = g; // instead of negating gradient, alpha will be negated │ │ │ │ │ -50 │ │ │ │ │ -51 // init gamma and calculate threshold │ │ │ │ │ -52 gamma = _d_o_t(g,g); │ │ │ │ │ -53 _t_h_r_e_s_h_o_l_d = std::max(parameters_.epsilon_abs(), parameters_.epsilon() * │ │ │ │ │ -parameters_.epsilon() * gamma); │ │ │ │ │ -54 │ │ │ │ │ -55 // Allocate and calculate A*d for first iteration │ │ │ │ │ -56 if (gamma > parameters_.epsilon_abs()) Ad = Ab * _d; │ │ │ │ │ -57 } │ │ │ │ │ -58 │ │ │ │ │ -59 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -60 // print │ │ │ │ │ -61 void print(const V& x) { │ │ │ │ │ -62 std::cout << "iteration = " << _k << std::endl; │ │ │ │ │ -63 _g_t_s_a_m_:_:_p_r_i_n_t(x,"x"); │ │ │ │ │ -64 _g_t_s_a_m_:_:_p_r_i_n_t(g, "g"); │ │ │ │ │ -65 std::cout << "dotg = " << gamma << std::endl; │ │ │ │ │ -66 _g_t_s_a_m_:_:_p_r_i_n_t(_d, "d"); │ │ │ │ │ -67 _g_t_s_a_m_:_:_p_r_i_n_t(Ad, "Ad"); │ │ │ │ │ -68 } │ │ │ │ │ -69 │ │ │ │ │ -70 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -71 // step the solution │ │ │ │ │ -72 double takeOptimalStep(V& x) { │ │ │ │ │ -73 // TODO: can we use gamma instead of dot(d,g) ????? Answer not trivial │ │ │ │ │ -74 double alpha = -_d_o_t(_d, g) / _d_o_t(Ad, Ad); // calculate optimal step-size │ │ │ │ │ -75 x += alpha * _d; // do step in new search direction, x += alpha*d │ │ │ │ │ -76 return alpha; │ │ │ │ │ -77 } │ │ │ │ │ -78 │ │ │ │ │ -79 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -80 // take a step, return true if converged │ │ │ │ │ -81 bool step(const S& Ab, V& x) { │ │ │ │ │ -82 │ │ │ │ │ -83 if ((++_k) >= ((int)parameters_.maxIterations())) return true; │ │ │ │ │ -84 │ │ │ │ │ -85 //----------------------------------> │ │ │ │ │ -86 double alpha = takeOptimalStep(x); │ │ │ │ │ +_7_6class GTSAM_EXPORT _S_u_b_g_r_a_p_h_S_o_l_v_e_r : public _I_t_e_r_a_t_i_v_e_S_o_l_v_e_r { │ │ │ │ │ +77 public: │ │ │ │ │ +78 typedef _S_u_b_g_r_a_p_h_S_o_l_v_e_r_P_a_r_a_m_e_t_e_r_s _P_a_r_a_m_e_t_e_r_s; │ │ │ │ │ +79 │ │ │ │ │ +80 protected: │ │ │ │ │ +81 _P_a_r_a_m_e_t_e_r_s parameters_; │ │ │ │ │ +_8_2 boost::shared_ptr _p_c__; │ │ │ │ │ +83 │ │ │ │ │ +84 public: │ │ │ │ │ 87 │ │ │ │ │ -88 // update gradient (or re-calculate at reset time) │ │ │ │ │ -89 if (_k % parameters_.reset() == 0) g = Ab.gradient(x); │ │ │ │ │ -90 // axpy(alpha, Ab ^ Ad, g); // g += alpha*(Ab^Ad) │ │ │ │ │ -91 else Ab.transposeMultiplyAdd(alpha, Ad, g); │ │ │ │ │ -92 │ │ │ │ │ -93 // check for convergence │ │ │ │ │ -94 double new_gamma = _d_o_t(g, g); │ │ │ │ │ +93 _S_u_b_g_r_a_p_h_S_o_l_v_e_r(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &A, const _P_a_r_a_m_e_t_e_r_s ¶meters, │ │ │ │ │ +94 const _O_r_d_e_r_i_n_g &ordering); │ │ │ │ │ 95 │ │ │ │ │ -96 if (parameters_.verbosity() != ConjugateGradientParameters::SILENT) │ │ │ │ │ -97 std::cout << "iteration " << _k << ": alpha = " << alpha │ │ │ │ │ -98 << ", dotg = " << new_gamma │ │ │ │ │ -99 << std::endl; │ │ │ │ │ -100 │ │ │ │ │ -101 if (new_gamma < _t_h_r_e_s_h_o_l_d) return true; │ │ │ │ │ -102 │ │ │ │ │ -103 // calculate new search direction │ │ │ │ │ -104 if (_s_t_e_e_p_e_s_t) _d = g; │ │ │ │ │ -105 else { │ │ │ │ │ -106 double beta = new_gamma / gamma; │ │ │ │ │ -107 // d = g + d*beta; │ │ │ │ │ -108 _d *= beta; │ │ │ │ │ -109 _d += 1.0 * g; │ │ │ │ │ -110 } │ │ │ │ │ -111 │ │ │ │ │ -112 gamma = new_gamma; │ │ │ │ │ +102 _S_u_b_g_r_a_p_h_S_o_l_v_e_r(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &Ab1, const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +&Ab2, │ │ │ │ │ +103 const _P_a_r_a_m_e_t_e_r_s ¶meters, const _O_r_d_e_r_i_n_g &ordering); │ │ │ │ │ +108 _S_u_b_g_r_a_p_h_S_o_l_v_e_r(const _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t &Rc1, const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &Ab2, │ │ │ │ │ +109 const _P_a_r_a_m_e_t_e_r_s ¶meters); │ │ │ │ │ +110 │ │ │ │ │ +_1_1_2 _~_S_u_b_g_r_a_p_h_S_o_l_v_e_r() override {} │ │ │ │ │ 113 │ │ │ │ │ -114 // In-place recalculation Ad <- A*d to avoid re-allocating Ad │ │ │ │ │ -115 Ab.multiplyInPlace(_d, Ad); │ │ │ │ │ -116 return false; │ │ │ │ │ -117 } │ │ │ │ │ -118 │ │ │ │ │ -119 }; // CGState Class │ │ │ │ │ +117 │ │ │ │ │ +119 _V_e_c_t_o_r_V_a_l_u_e_s _o_p_t_i_m_i_z_e() const; │ │ │ │ │ 120 │ │ │ │ │ -121 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -122 // conjugate gradient method. │ │ │ │ │ -123 // S: linear system, V: step vector, E: errors │ │ │ │ │ -124 template │ │ │ │ │ -_1_2_5 V _c_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_s(const S& Ab, V x, const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ -¶meters, bool steepest) { │ │ │ │ │ +122 _V_e_c_t_o_r_V_a_l_u_e_s _o_p_t_i_m_i_z_e(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, │ │ │ │ │ +123 const _K_e_y_I_n_f_o &keyInfo, │ │ │ │ │ +124 const std::map &lambda, │ │ │ │ │ +125 const _V_e_c_t_o_r_V_a_l_u_e_s &initial) override; │ │ │ │ │ 126 │ │ │ │ │ -127 _C_G_S_t_a_t_e_<_S_,_ _V_,_ _E_> state(Ab, x, parameters, steepest); │ │ │ │ │ -128 │ │ │ │ │ -129 if (parameters.verbosity() != ConjugateGradientParameters::SILENT) │ │ │ │ │ -130 std::cout << "CG: epsilon = " << parameters.epsilon() │ │ │ │ │ -131 << ", maxIterations = " << parameters.maxIterations() │ │ │ │ │ -132 << ", ||g0||^2 = " << state.gamma │ │ │ │ │ -133 << ", threshold = " << state._t_h_r_e_s_h_o_l_d │ │ │ │ │ -134 << std::endl; │ │ │ │ │ -135 │ │ │ │ │ -136 if ( state.gamma < state._t_h_r_e_s_h_o_l_d ) { │ │ │ │ │ -137 if (parameters.verbosity() != ConjugateGradientParameters::SILENT) │ │ │ │ │ -138 std::cout << "||g0||^2 < threshold, exiting immediately !" << std::endl; │ │ │ │ │ -139 │ │ │ │ │ -140 return x; │ │ │ │ │ -141 } │ │ │ │ │ -142 │ │ │ │ │ -143 // loop maxIterations times │ │ │ │ │ -144 while (!state.step(Ab, x)) {} │ │ │ │ │ -145 return x; │ │ │ │ │ -146 } │ │ │ │ │ -147/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -148 │ │ │ │ │ -149} // namespace gtsam │ │ │ │ │ +130 │ │ │ │ │ +132 std::pair splitGraph( │ │ │ │ │ +133 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg); │ │ │ │ │ +134 │ │ │ │ │ +136}; │ │ │ │ │ +137 │ │ │ │ │ +138} // namespace gtsam │ │ │ │ │ _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_S_o_l_v_e_r_._h │ │ │ │ │ Implementation of Conjugate Gradient solver for a linear system. │ │ │ │ │ -_i_t_e_r_a_t_i_v_e_._h │ │ │ │ │ -Iterative methods, implementation. │ │ │ │ │ +_S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ +Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key │ │ │ │ │ +landmarkKey) │ │ │ │ │ +Optimize for triangulation. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.cpp:155 │ │ │ │ │ _g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ print without optional string, must specify cout yourself │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_c_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_s │ │ │ │ │ -V conjugateGradients(const S &Ab, V x, const ConjugateGradientParameters │ │ │ │ │ -¶meters, bool steepest) │ │ │ │ │ -Method of conjugate gradients (CG) template "System" class S needs gradient │ │ │ │ │ -(S,v), e=S*v,... │ │ │ │ │ -DDeeffiinniittiioonn iterative-inl.h:125 │ │ │ │ │ -_g_t_s_a_m_:_:_d_o_t │ │ │ │ │ -double dot(const V1 &a, const V2 &b) │ │ │ │ │ -Dot product. │ │ │ │ │ -DDeeffiinniittiioonn Vector.h:195 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ _g_t_s_a_m_:_:_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ parameters for the conjugate gradient method │ │ │ │ │ DDeeffiinniittiioonn ConjugateGradientSolver.h:29 │ │ │ │ │ -_g_t_s_a_m_:_:_C_G_S_t_a_t_e │ │ │ │ │ -DDeeffiinniittiioonn iterative-inl.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_C_G_S_t_a_t_e_:_:_s_t_e_e_p_e_s_t │ │ │ │ │ -bool steepest │ │ │ │ │ -flag to indicate we are doing steepest descent │ │ │ │ │ -DDeeffiinniittiioonn iterative-inl.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_C_G_S_t_a_t_e_:_:_t_h_r_e_s_h_o_l_d │ │ │ │ │ -double threshold │ │ │ │ │ -gamma (squared L2 norm of g) and convergence threshold │ │ │ │ │ -DDeeffiinniittiioonn iterative-inl.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_C_G_S_t_a_t_e_:_:_k │ │ │ │ │ -int k │ │ │ │ │ -iteration │ │ │ │ │ -DDeeffiinniittiioonn iterative-inl.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_C_G_S_t_a_t_e_:_:_d │ │ │ │ │ -V d │ │ │ │ │ -gradient g and search direction d for CG │ │ │ │ │ -DDeeffiinniittiioonn iterative-inl.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t │ │ │ │ │ +GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals. │ │ │ │ │ +DDeeffiinniittiioonn GaussianBayesNet.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_I_t_e_r_a_t_i_v_e_S_o_l_v_e_r │ │ │ │ │ +Base class for Iterative Solvers like SubgraphSolver. │ │ │ │ │ +DDeeffiinniittiioonn IterativeSolver.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_I_n_f_o │ │ │ │ │ +Handy data structure for iterative solvers. │ │ │ │ │ +DDeeffiinniittiioonn IterativeSolver.h:126 │ │ │ │ │ +_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ +DDeeffiinniittiioonn SubgraphBuilder.h:96 │ │ │ │ │ +_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_S_o_l_v_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ +DDeeffiinniittiioonn SubgraphSolver.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_S_o_l_v_e_r │ │ │ │ │ +This class implements the linear SPCG solver presented in Dellaert et al in │ │ │ │ │ +IROS'10. │ │ │ │ │ +DDeeffiinniittiioonn SubgraphSolver.h:76 │ │ │ │ │ +_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_S_o_l_v_e_r_:_:_~_S_u_b_g_r_a_p_h_S_o_l_v_e_r │ │ │ │ │ +~SubgraphSolver() override │ │ │ │ │ +Destructor. │ │ │ │ │ +DDeeffiinniittiioonn SubgraphSolver.h:112 │ │ │ │ │ +_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_S_o_l_v_e_r_:_:_p_c__ │ │ │ │ │ +boost::shared_ptr< SubgraphPreconditioner > pc_ │ │ │ │ │ +preconditioner object │ │ │ │ │ +DDeeffiinniittiioonn SubgraphSolver.h:82 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ +with a unique integer... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _i_t_e_r_a_t_i_v_e_-_i_n_l_._h │ │ │ │ │ + * _S_u_b_g_r_a_p_h_S_o_l_v_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00725_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/RegularJacobianFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/JacobianFactor-inl.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,215 +98,144 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
RegularJacobianFactor.h
│ │ │ │ +
JacobianFactor-inl.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/linear/JacobianFactor.h>
│ │ │ │ -
22#include <gtsam/linear/VectorValues.h>
│ │ │ │ -
23
│ │ │ │ -
24namespace gtsam {
│ │ │ │ -
25
│ │ │ │ -
31template<size_t D>
│ │ │ │ -
│ │ │ │ -
32class RegularJacobianFactor: public JacobianFactor {
│ │ │ │ -
33
│ │ │ │ -
34private:
│ │ │ │ -
35
│ │ │ │ -
36 // Use eigen magic to access raw memory
│ │ │ │ -
37 typedef Eigen::Matrix<double, D, 1> DVector;
│ │ │ │ -
38 typedef Eigen::Map<DVector> DMap;
│ │ │ │ -
39 typedef Eigen::Map<const DVector> ConstDMap;
│ │ │ │ -
40
│ │ │ │ -
41public:
│ │ │ │ -
42
│ │ │ │ -
44 RegularJacobianFactor() {}
│ │ │ │ -
45
│ │ │ │ -
51 template<typename TERMS>
│ │ │ │ -
│ │ │ │ -
52 RegularJacobianFactor(const TERMS& terms, const Vector& b,
│ │ │ │ -
53 const SharedDiagonal& model = SharedDiagonal()) :
│ │ │ │ -
54 JacobianFactor(terms, b, model) {
│ │ │ │ -
55 }
│ │ │ │ -
│ │ │ │ -
56
│ │ │ │ -
63 template<typename KEYS>
│ │ │ │ -
│ │ │ │ -
64 RegularJacobianFactor(const KEYS& keys,
│ │ │ │ -
65 const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas =
│ │ │ │ -
66 SharedDiagonal()) :
│ │ │ │ -
67 JacobianFactor(keys, augmentedMatrix, sigmas) {
│ │ │ │ -
68 }
│ │ │ │ -
│ │ │ │ -
69
│ │ │ │ -
70 using JacobianFactor::multiplyHessianAdd;
│ │ │ │ -
71
│ │ │ │ -
│ │ │ │ -
73 void multiplyHessianAdd(double alpha, const VectorValues& x,
│ │ │ │ -
74 VectorValues& y) const override {
│ │ │ │ -
75 JacobianFactor::multiplyHessianAdd(alpha, x, y);
│ │ │ │ -
76 }
│ │ │ │ -
│ │ │ │ +
21#include <gtsam/linear/linearExceptions.h>
│ │ │ │ +
22
│ │ │ │ +
23namespace gtsam {
│ │ │ │ +
24
│ │ │ │ +
25 /* ************************************************************************* */
│ │ │ │ +
26 template<typename TERMS>
│ │ │ │ +
│ │ │ │ +
27 JacobianFactor::JacobianFactor(const TERMS&terms, const Vector &b, const SharedDiagonal& model)
│ │ │ │ +
28 {
│ │ │ │ +
29 fillTerms(terms, b, model);
│ │ │ │ +
30 }
│ │ │ │ +
│ │ │ │ +
31
│ │ │ │ +
32 /* ************************************************************************* */
│ │ │ │ +
33 template<typename KEYS>
│ │ │ │ +
│ │ │ │ +
34 JacobianFactor::JacobianFactor(
│ │ │ │ +
35 const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& model) :
│ │ │ │ +
36 Base(keys), Ab_(augmentedMatrix)
│ │ │ │ +
37 {
│ │ │ │ +
38 // Check noise model dimension
│ │ │ │ +
39 if(model && (DenseIndex)model->dim() != augmentedMatrix.rows())
│ │ │ │ +
40 throw InvalidNoiseModel(augmentedMatrix.rows(), model->dim());
│ │ │ │ +
41
│ │ │ │ +
42 // Check number of variables
│ │ │ │ +
43 if((DenseIndex)Base::keys_.size() != augmentedMatrix.nBlocks() - 1)
│ │ │ │ +
44 throw std::invalid_argument(
│ │ │ │ +
45 "Error in JacobianFactor constructor input. Number of provided keys plus\n"
│ │ │ │ +
46 "one for the RHS vector must equal the number of provided matrix blocks.");
│ │ │ │ +
47
│ │ │ │ +
48 // Check RHS dimension
│ │ │ │ +
49 if(augmentedMatrix(augmentedMatrix.nBlocks() - 1).cols() != 1)
│ │ │ │ +
50 throw std::invalid_argument(
│ │ │ │ +
51 "Error in JacobianFactor constructor input. The last provided matrix block\n"
│ │ │ │ +
52 "must be the RHS vector, but the last provided block had more than one column.");
│ │ │ │ +
53
│ │ │ │ +
54 // Take noise model
│ │ │ │ +
55 model_ = model;
│ │ │ │ +
56 }
│ │ │ │ +
│ │ │ │ +
57
│ │ │ │ +
58 /* ************************************************************************* */
│ │ │ │ +
59 template<typename TERMS>
│ │ │ │ +
│ │ │ │ +
60 void JacobianFactor::fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel)
│ │ │ │ +
61 {
│ │ │ │ +
62 // Check noise model dimension
│ │ │ │ +
63 if(noiseModel && (DenseIndex)noiseModel->dim() != b.size())
│ │ │ │ +
64 throw InvalidNoiseModel(b.size(), noiseModel->dim());
│ │ │ │ +
65
│ │ │ │ +
66 // Resize base class key vector
│ │ │ │ +
67 Base::keys_.resize(terms.size());
│ │ │ │ +
68
│ │ │ │ +
69 // Get dimensions of matrices
│ │ │ │ +
70 std::vector<size_t> dimensions;
│ │ │ │ +
71 dimensions.reserve(terms.size());
│ │ │ │ +
72 for(typename TERMS::const_iterator it = terms.begin(); it != terms.end(); ++it) {
│ │ │ │ +
73 const std::pair<Key, Matrix>& term = *it;
│ │ │ │ +
74 const Matrix& Ai = term.second;
│ │ │ │ +
75 dimensions.push_back(Ai.cols());
│ │ │ │ +
76 }
│ │ │ │
77
│ │ │ │ -
│ │ │ │ -
82 void multiplyHessianAdd(double alpha, const double* x, double* y) const {
│ │ │ │ -
83 if (empty())
│ │ │ │ -
84 return;
│ │ │ │ -
85 Vector Ax = Vector::Zero(Ab_.rows());
│ │ │ │ -
86
│ │ │ │ -
87 // Just iterate over all A matrices and multiply in correct config part
│ │ │ │ -
88 for (size_t pos = 0; pos < size(); ++pos)
│ │ │ │ -
89 Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]);
│ │ │ │ -
90
│ │ │ │ -
91 // Deal with noise properly, need to Double* whiten as we are dividing by variance
│ │ │ │ -
92 if (model_) {
│ │ │ │ -
93 model_->whitenInPlace(Ax);
│ │ │ │ -
94 model_->whitenInPlace(Ax);
│ │ │ │ -
95 }
│ │ │ │ -
96
│ │ │ │ -
97 // multiply with alpha
│ │ │ │ -
98 Ax *= alpha;
│ │ │ │ +
78 // Construct block matrix
│ │ │ │ +
79 Ab_ = VerticalBlockMatrix(dimensions, b.size(), true);
│ │ │ │ +
80
│ │ │ │ +
81 // Check and add terms
│ │ │ │ +
82 DenseIndex i = 0; // For block index
│ │ │ │ +
83 for(typename TERMS::const_iterator it = terms.begin(); it != terms.end(); ++it) {
│ │ │ │ +
84 const std::pair<Key, Matrix>& term = *it;
│ │ │ │ +
85 Key key = term.first;
│ │ │ │ +
86 const Matrix& Ai = term.second;
│ │ │ │ +
87
│ │ │ │ +
88 // Check block rows
│ │ │ │ +
89 if(Ai.rows() != Ab_.rows())
│ │ │ │ +
90 throw InvalidMatrixBlock(Ab_.rows(), Ai.rows());
│ │ │ │ +
91
│ │ │ │ +
92 // Assign key and matrix
│ │ │ │ +
93 Base::keys_[i] = key;
│ │ │ │ +
94 Ab_(i) = Ai;
│ │ │ │ +
95
│ │ │ │ +
96 // Increment block index
│ │ │ │ +
97 ++ i;
│ │ │ │ +
98 }
│ │ │ │
99
│ │ │ │ -
100 // Again iterate over all A matrices and insert Ai^e into y
│ │ │ │ -
101 for (size_t pos = 0; pos < size(); ++pos)
│ │ │ │ -
102 DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax;
│ │ │ │ -
103 }
│ │ │ │ -
│ │ │ │ -
104
│ │ │ │ -
106 using GaussianFactor::hessianDiagonal;
│ │ │ │ -
107
│ │ │ │ -
│ │ │ │ -
109 void hessianDiagonal(double* d) const override {
│ │ │ │ -
110 // Loop over all variables in the factor
│ │ │ │ -
111 for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
│ │ │ │ -
112 // Get the diagonal block, and insert its diagonal
│ │ │ │ -
113 DVector dj;
│ │ │ │ -
114 for (size_t k = 0; k < D; ++k) {
│ │ │ │ -
115 if (model_) {
│ │ │ │ -
116 Vector column_k = Ab_(j).col(k);
│ │ │ │ -
117 column_k = model_->whiten(column_k);
│ │ │ │ -
118 dj(k) = dot(column_k, column_k);
│ │ │ │ -
119 } else {
│ │ │ │ -
120 dj(k) = Ab_(j).col(k).squaredNorm();
│ │ │ │ -
121 }
│ │ │ │ -
122 }
│ │ │ │ -
123 DMap(d + D * j) += dj;
│ │ │ │ -
124 }
│ │ │ │ -
125 }
│ │ │ │ -
│ │ │ │ -
126
│ │ │ │ -
│ │ │ │ -
128 VectorValues gradientAtZero() const override {
│ │ │ │ -
129 return JacobianFactor::gradientAtZero();
│ │ │ │ -
130 }
│ │ │ │ -
│ │ │ │ -
131
│ │ │ │ -
│ │ │ │ -
133 void gradientAtZero(double* d) const override {
│ │ │ │ -
134
│ │ │ │ -
135 // Get vector b not weighted
│ │ │ │ -
136 Vector b = getb();
│ │ │ │ -
137
│ │ │ │ -
138 // Whitening b
│ │ │ │ -
139 if (model_) {
│ │ │ │ -
140 b = model_->whiten(b);
│ │ │ │ -
141 b = model_->whiten(b);
│ │ │ │ -
142 }
│ │ │ │ -
143
│ │ │ │ -
144 // Just iterate over all A matrices
│ │ │ │ -
145 for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
│ │ │ │ -
146 DVector dj;
│ │ │ │ -
147 // gradient -= A'*b/sigma^2
│ │ │ │ -
148 // Computing with each column
│ │ │ │ -
149 for (size_t k = 0; k < D; ++k) {
│ │ │ │ -
150 Vector column_k = Ab_(j).col(k);
│ │ │ │ -
151 dj(k) = -1.0 * dot(b, column_k);
│ │ │ │ -
152 }
│ │ │ │ -
153 DMap(d + D * j) += dj;
│ │ │ │ -
154 }
│ │ │ │ -
155 }
│ │ │ │ -
│ │ │ │ -
156
│ │ │ │ -
│ │ │ │ -
161 void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const {
│ │ │ │ -
162 Vector E = alpha * (model_ ? model_->whiten(e) : e);
│ │ │ │ -
163 // Just iterate over all A matrices and insert Ai^e into y
│ │ │ │ -
164 for (size_t pos = 0; pos < size(); ++pos)
│ │ │ │ -
165 DMap(x + D * keys_[pos]) += Ab_(pos).transpose() * E;
│ │ │ │ -
166 }
│ │ │ │ -
│ │ │ │ -
167
│ │ │ │ -
│ │ │ │ -
172 Vector operator*(const double* x) const {
│ │ │ │ -
173 Vector Ax = Vector::Zero(Ab_.rows());
│ │ │ │ -
174 if (empty())
│ │ │ │ -
175 return Ax;
│ │ │ │ -
176
│ │ │ │ -
177 // Just iterate over all A matrices and multiply in correct config part
│ │ │ │ -
178 for (size_t pos = 0; pos < size(); ++pos)
│ │ │ │ -
179 Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]);
│ │ │ │ -
180
│ │ │ │ -
181 return model_ ? model_->whiten(Ax) : Ax;
│ │ │ │ -
182 }
│ │ │ │ -
│ │ │ │ -
183
│ │ │ │ -
184};
│ │ │ │ -
│ │ │ │ -
185// end class RegularJacobianFactor
│ │ │ │ -
186
│ │ │ │ -
187}// end namespace gtsam
│ │ │ │ - │ │ │ │ -
VectorValues.h
Factor Graph Values.
│ │ │ │ +
100 // Assign RHS vector
│ │ │ │ +
101 getb() = b;
│ │ │ │ +
102
│ │ │ │ +
103 // Assign noise model
│ │ │ │ +
104 model_ = noiseModel;
│ │ │ │ +
105 }
│ │ │ │ +
│ │ │ │ +
106
│ │ │ │ +
107} // gtsam
│ │ │ │ +
108
│ │ │ │ +
linearExceptions.h
Exceptions that may be thrown by linear solver components.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │ -
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::VerticalBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43
│ │ │ │
gtsam::VerticalBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition VerticalBlockMatrix.h:115
│ │ │ │ -
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
│ │ │ │ +
gtsam::VerticalBlockMatrix::cols
DenseIndex cols() const
Column size.
Definition VerticalBlockMatrix.h:118
│ │ │ │ +
gtsam::VerticalBlockMatrix::nBlocks
DenseIndex nBlocks() const
Block count.
Definition VerticalBlockMatrix.h:121
│ │ │ │
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
│ │ │ │ -
gtsam::Factor::empty
bool empty() const
Whether the factor is empty (involves zero variables).
Definition Factor.h:128
│ │ │ │
gtsam::Factor::size
size_t size() const
Definition Factor.h:157
│ │ │ │ -
gtsam::GaussianFactor::hessianDiagonal
VectorValues hessianDiagonal() const
Return the diagonal of the Hessian for this factor.
Definition GaussianFactor.cpp:35
│ │ │ │ -
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition JacobianFactor.h:91
│ │ │ │ +
gtsam::GaussianFactor
An abstract virtual base class for JacobianFactor and HessianFactor.
Definition GaussianFactor.h:39
│ │ │ │
gtsam::JacobianFactor::getb
const constBVector getb() const
Get a view of the r.h.s.
Definition JacobianFactor.h:297
│ │ │ │ -
gtsam::JacobianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
y += alpha * A'*A*x
Definition JacobianFactor.cpp:649
│ │ │ │ -
gtsam::JacobianFactor::gradientAtZero
VectorValues gradientAtZero() const override
A'*b for Jacobian.
Definition JacobianFactor.cpp:701
│ │ │ │ -
gtsam::RegularJacobianFactor
JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator.
Definition RegularJacobianFactor.h:32
│ │ │ │ -
gtsam::RegularJacobianFactor::operator*
Vector operator*(const double *x) const
double* Matrix-vector multiply, i.e.
Definition RegularJacobianFactor.h:172
│ │ │ │ -
gtsam::RegularJacobianFactor::hessianDiagonal
void hessianDiagonal(double *d) const override
Raw memory access version of hessianDiagonal.
Definition RegularJacobianFactor.h:109
│ │ │ │ -
gtsam::RegularJacobianFactor::gradientAtZero
void gradientAtZero(double *d) const override
Raw memory access version of gradientAtZero.
Definition RegularJacobianFactor.h:133
│ │ │ │ -
gtsam::RegularJacobianFactor::transposeMultiplyAdd
void transposeMultiplyAdd(double alpha, const Vector &e, double *x) const
double* Transpose Matrix-vector multiply, i.e.
Definition RegularJacobianFactor.h:161
│ │ │ │ -
gtsam::RegularJacobianFactor::gradientAtZero
VectorValues gradientAtZero() const override
Expose base class gradientAtZero.
Definition RegularJacobianFactor.h:128
│ │ │ │ -
gtsam::RegularJacobianFactor::RegularJacobianFactor
RegularJacobianFactor(const TERMS &terms, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Construct an n-ary factor.
Definition RegularJacobianFactor.h:52
│ │ │ │ -
gtsam::RegularJacobianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
y += alpha * A'*A*x
Definition RegularJacobianFactor.h:73
│ │ │ │ -
gtsam::RegularJacobianFactor::RegularJacobianFactor
RegularJacobianFactor()
Default constructor.
Definition RegularJacobianFactor.h:44
│ │ │ │ -
gtsam::RegularJacobianFactor::RegularJacobianFactor
RegularJacobianFactor(const KEYS &keys, const VerticalBlockMatrix &augmentedMatrix, const SharedDiagonal &sigmas=SharedDiagonal())
Constructor with arbitrary number keys, and where the augmented matrix is given all together instead ...
Definition RegularJacobianFactor.h:64
│ │ │ │ -
gtsam::RegularJacobianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const double *x, double *y) const
double* Hessian-vector multiply, i.e.
Definition RegularJacobianFactor.h:82
│ │ │ │ -
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
gtsam::JacobianFactor::JacobianFactor
JacobianFactor()
default constructor for I/O
Definition JacobianFactor.cpp:54
│ │ │ │ +
gtsam::JacobianFactor::fillTerms
void fillTerms(const TERMS &terms, const Vector &b, const SharedDiagonal &noiseModel)
Internal function to fill blocks and set dimensions.
Definition JacobianFactor-inl.h:60
│ │ │ │ +
gtsam::InvalidNoiseModel
An exception indicating that the noise model dimension passed into a JacobianFactor has a different d...
Definition linearExceptions.h:106
│ │ │ │ +
gtsam::InvalidMatrixBlock
An exception indicating that a matrix block passed into a JacobianFactor has a different dimensionali...
Definition linearExceptions.h:124
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,14 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -RegularJacobianFactor.h │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +JacobianFactor-inl.h │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ @@ -16,254 +15,169 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 19#pragma once │ │ │ │ │ 20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ -23 │ │ │ │ │ -24namespace _g_t_s_a_m { │ │ │ │ │ -25 │ │ │ │ │ -31template │ │ │ │ │ -_3_2class _R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r: public _J_a_c_o_b_i_a_n_F_a_c_t_o_r { │ │ │ │ │ -33 │ │ │ │ │ -34private: │ │ │ │ │ -35 │ │ │ │ │ -36 // Use eigen magic to access raw memory │ │ │ │ │ -37 typedef Eigen::Matrix DVector; │ │ │ │ │ -38 typedef Eigen::Map DMap; │ │ │ │ │ -39 typedef Eigen::Map ConstDMap; │ │ │ │ │ -40 │ │ │ │ │ -41public: │ │ │ │ │ -42 │ │ │ │ │ -_4_4 _R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r() {} │ │ │ │ │ -45 │ │ │ │ │ -51 template │ │ │ │ │ -_5_2 _R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r(const TERMS& terms, const Vector& b, │ │ │ │ │ -53 const SharedDiagonal& model = SharedDiagonal()) : │ │ │ │ │ -54 _J_a_c_o_b_i_a_n_F_a_c_t_o_r(terms, b, model) { │ │ │ │ │ -55 } │ │ │ │ │ -56 │ │ │ │ │ -63 template │ │ │ │ │ -_6_4 _R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r(const KEYS& _k_e_y_s, │ │ │ │ │ -65 const _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x& augmentedMatrix, const SharedDiagonal& sigmas = │ │ │ │ │ -66 SharedDiagonal()) : │ │ │ │ │ -67 _J_a_c_o_b_i_a_n_F_a_c_t_o_r(_k_e_y_s, augmentedMatrix, sigmas) { │ │ │ │ │ -68 } │ │ │ │ │ -69 │ │ │ │ │ -70 using _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d; │ │ │ │ │ -71 │ │ │ │ │ -_7_3 void _m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d(double alpha, const _V_e_c_t_o_r_V_a_l_u_e_s& x, │ │ │ │ │ -74 _V_e_c_t_o_r_V_a_l_u_e_s& y) const override { │ │ │ │ │ -75 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d(alpha, x, y); │ │ │ │ │ +21#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_l_i_n_e_a_r_E_x_c_e_p_t_i_o_n_s_._h> │ │ │ │ │ +22 │ │ │ │ │ +23namespace _g_t_s_a_m { │ │ │ │ │ +24 │ │ │ │ │ +25 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +26 template │ │ │ │ │ +_2_7 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r(const TERMS&terms, const Vector &b, const │ │ │ │ │ +SharedDiagonal& model) │ │ │ │ │ +28 { │ │ │ │ │ +29 _f_i_l_l_T_e_r_m_s(terms, b, model); │ │ │ │ │ +30 } │ │ │ │ │ +31 │ │ │ │ │ +32 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +33 template │ │ │ │ │ +_3_4 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r( │ │ │ │ │ +35 const KEYS& keys, const _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x& augmentedMatrix, const │ │ │ │ │ +SharedDiagonal& model) : │ │ │ │ │ +36 _B_a_s_e(keys), Ab_(augmentedMatrix) │ │ │ │ │ +37 { │ │ │ │ │ +38 // Check noise model dimension │ │ │ │ │ +39 if(model && (_D_e_n_s_e_I_n_d_e_x)model->dim() != augmentedMatrix._r_o_w_s()) │ │ │ │ │ +40 throw _I_n_v_a_l_i_d_N_o_i_s_e_M_o_d_e_l(augmentedMatrix._r_o_w_s(), model->dim()); │ │ │ │ │ +41 │ │ │ │ │ +42 // Check number of variables │ │ │ │ │ +43 if((_D_e_n_s_e_I_n_d_e_x)_B_a_s_e_:_:_k_e_y_s__._s_i_z_e() != augmentedMatrix._n_B_l_o_c_k_s() - 1) │ │ │ │ │ +44 throw std::invalid_argument( │ │ │ │ │ +45 "Error in JacobianFactor constructor input. Number of provided keys plus\n" │ │ │ │ │ +46 "one for the RHS vector must equal the number of provided matrix blocks."); │ │ │ │ │ +47 │ │ │ │ │ +48 // Check RHS dimension │ │ │ │ │ +49 if(augmentedMatrix(augmentedMatrix._n_B_l_o_c_k_s() - 1)._c_o_l_s() != 1) │ │ │ │ │ +50 throw std::invalid_argument( │ │ │ │ │ +51 "Error in JacobianFactor constructor input. The last provided matrix │ │ │ │ │ +block\n" │ │ │ │ │ +52 "must be the RHS vector, but the last provided block had more than one │ │ │ │ │ +column."); │ │ │ │ │ +53 │ │ │ │ │ +54 // Take noise model │ │ │ │ │ +55 model_ = model; │ │ │ │ │ +56 } │ │ │ │ │ +57 │ │ │ │ │ +58 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +59 template │ │ │ │ │ +_6_0 void _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_f_i_l_l_T_e_r_m_s(const TERMS& terms, const Vector& b, const │ │ │ │ │ +SharedDiagonal& noiseModel) │ │ │ │ │ +61 { │ │ │ │ │ +62 // Check noise model dimension │ │ │ │ │ +63 if(noiseModel && (_D_e_n_s_e_I_n_d_e_x)noiseModel->dim() != b.size()) │ │ │ │ │ +64 throw _I_n_v_a_l_i_d_N_o_i_s_e_M_o_d_e_l(b.size(), noiseModel->dim()); │ │ │ │ │ +65 │ │ │ │ │ +66 // Resize base class key vector │ │ │ │ │ +67 _B_a_s_e_:_:_k_e_y_s__.resize(terms.size()); │ │ │ │ │ +68 │ │ │ │ │ +69 // Get dimensions of matrices │ │ │ │ │ +70 std::vector dimensions; │ │ │ │ │ +71 dimensions.reserve(terms.size()); │ │ │ │ │ +72 for(typename TERMS::const_iterator it = terms.begin(); it != terms.end(); │ │ │ │ │ +++it) { │ │ │ │ │ +73 const std::pair& term = *it; │ │ │ │ │ +74 const Matrix& Ai = term.second; │ │ │ │ │ +75 dimensions.push_back(Ai.cols()); │ │ │ │ │ 76 } │ │ │ │ │ 77 │ │ │ │ │ -_8_2 void _m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d(double alpha, const double* x, double* y) const { │ │ │ │ │ -83 if (_e_m_p_t_y()) │ │ │ │ │ -84 return; │ │ │ │ │ -85 Vector Ax = Vector::Zero(Ab_._r_o_w_s()); │ │ │ │ │ -86 │ │ │ │ │ -87 // Just iterate over all A matrices and multiply in correct config part │ │ │ │ │ -88 for (size_t pos = 0; pos < _s_i_z_e(); ++pos) │ │ │ │ │ -89 Ax += Ab_(pos) * ConstDMap(x + D * _k_e_y_s__[pos]); │ │ │ │ │ -90 │ │ │ │ │ -91 // Deal with noise properly, need to Double* whiten as we are dividing by │ │ │ │ │ -variance │ │ │ │ │ -92 if (model_) { │ │ │ │ │ -93 model_->whitenInPlace(Ax); │ │ │ │ │ -94 model_->whitenInPlace(Ax); │ │ │ │ │ -95 } │ │ │ │ │ -96 │ │ │ │ │ -97 // multiply with alpha │ │ │ │ │ -98 Ax *= alpha; │ │ │ │ │ +78 // Construct block matrix │ │ │ │ │ +79 Ab_ = _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x(dimensions, b.size(), true); │ │ │ │ │ +80 │ │ │ │ │ +81 // Check and add terms │ │ │ │ │ +82 _D_e_n_s_e_I_n_d_e_x i = 0; // For block index │ │ │ │ │ +83 for(typename TERMS::const_iterator it = terms.begin(); it != terms.end(); │ │ │ │ │ +++it) { │ │ │ │ │ +84 const std::pair& term = *it; │ │ │ │ │ +85 _K_e_y key = term.first; │ │ │ │ │ +86 const Matrix& Ai = term.second; │ │ │ │ │ +87 │ │ │ │ │ +88 // Check block rows │ │ │ │ │ +89 if(Ai.rows() != Ab_._r_o_w_s()) │ │ │ │ │ +90 throw _I_n_v_a_l_i_d_M_a_t_r_i_x_B_l_o_c_k(Ab_._r_o_w_s(), Ai.rows()); │ │ │ │ │ +91 │ │ │ │ │ +92 // Assign key and matrix │ │ │ │ │ +93 _B_a_s_e_:_:_k_e_y_s__[i] = key; │ │ │ │ │ +94 Ab_(i) = Ai; │ │ │ │ │ +95 │ │ │ │ │ +96 // Increment block index │ │ │ │ │ +97 ++ i; │ │ │ │ │ +98 } │ │ │ │ │ 99 │ │ │ │ │ -100 // Again iterate over all A matrices and insert Ai^e into y │ │ │ │ │ -101 for (size_t pos = 0; pos < _s_i_z_e(); ++pos) │ │ │ │ │ -102 DMap(y + D * _k_e_y_s__[pos]) += Ab_(pos).transpose() * Ax; │ │ │ │ │ -103 } │ │ │ │ │ -104 │ │ │ │ │ -106 using _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_h_e_s_s_i_a_n_D_i_a_g_o_n_a_l; │ │ │ │ │ -107 │ │ │ │ │ -_1_0_9 void _h_e_s_s_i_a_n_D_i_a_g_o_n_a_l(double* d) const override { │ │ │ │ │ -110 // Loop over all variables in the factor │ │ │ │ │ -111 for (_D_e_n_s_e_I_n_d_e_x j = 0; j < (_D_e_n_s_e_I_n_d_e_x) _s_i_z_e(); ++j) { │ │ │ │ │ -112 // Get the diagonal block, and insert its diagonal │ │ │ │ │ -113 DVector dj; │ │ │ │ │ -114 for (size_t k = 0; k < D; ++k) { │ │ │ │ │ -115 if (model_) { │ │ │ │ │ -116 Vector column_k = Ab_(j).col(k); │ │ │ │ │ -117 column_k = model_->whiten(column_k); │ │ │ │ │ -118 dj(k) = _d_o_t(column_k, column_k); │ │ │ │ │ -119 } else { │ │ │ │ │ -120 dj(k) = Ab_(j).col(k).squaredNorm(); │ │ │ │ │ -121 } │ │ │ │ │ -122 } │ │ │ │ │ -123 DMap(d + D * j) += dj; │ │ │ │ │ -124 } │ │ │ │ │ -125 } │ │ │ │ │ -126 │ │ │ │ │ -_1_2_8 _V_e_c_t_o_r_V_a_l_u_e_s _g_r_a_d_i_e_n_t_A_t_Z_e_r_o() const override { │ │ │ │ │ -129 return _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_r_a_d_i_e_n_t_A_t_Z_e_r_o(); │ │ │ │ │ -130 } │ │ │ │ │ -131 │ │ │ │ │ -_1_3_3 void _g_r_a_d_i_e_n_t_A_t_Z_e_r_o(double* d) const override { │ │ │ │ │ -134 │ │ │ │ │ -135 // Get vector b not weighted │ │ │ │ │ -136 Vector b = _g_e_t_b(); │ │ │ │ │ -137 │ │ │ │ │ -138 // Whitening b │ │ │ │ │ -139 if (model_) { │ │ │ │ │ -140 b = model_->whiten(b); │ │ │ │ │ -141 b = model_->whiten(b); │ │ │ │ │ -142 } │ │ │ │ │ -143 │ │ │ │ │ -144 // Just iterate over all A matrices │ │ │ │ │ -145 for (_D_e_n_s_e_I_n_d_e_x j = 0; j < (_D_e_n_s_e_I_n_d_e_x) _s_i_z_e(); ++j) { │ │ │ │ │ -146 DVector dj; │ │ │ │ │ -147 // gradient -= A'*b/sigma^2 │ │ │ │ │ -148 // Computing with each column │ │ │ │ │ -149 for (size_t k = 0; k < D; ++k) { │ │ │ │ │ -150 Vector column_k = Ab_(j).col(k); │ │ │ │ │ -151 dj(k) = -1.0 * _d_o_t(b, column_k); │ │ │ │ │ -152 } │ │ │ │ │ -153 DMap(d + D * j) += dj; │ │ │ │ │ -154 } │ │ │ │ │ -155 } │ │ │ │ │ -156 │ │ │ │ │ -_1_6_1 void _t_r_a_n_s_p_o_s_e_M_u_l_t_i_p_l_y_A_d_d(double alpha, const Vector& e, double* x) const { │ │ │ │ │ -162 Vector E = alpha * (model_ ? model_->whiten(e) : e); │ │ │ │ │ -163 // Just iterate over all A matrices and insert Ai^e into y │ │ │ │ │ -164 for (size_t pos = 0; pos < _s_i_z_e(); ++pos) │ │ │ │ │ -165 DMap(x + D * _k_e_y_s__[pos]) += Ab_(pos).transpose() * E; │ │ │ │ │ -166 } │ │ │ │ │ -167 │ │ │ │ │ -_1_7_2 Vector _o_p_e_r_a_t_o_r_*(const double* x) const { │ │ │ │ │ -173 Vector Ax = Vector::Zero(Ab_._r_o_w_s()); │ │ │ │ │ -174 if (_e_m_p_t_y()) │ │ │ │ │ -175 return Ax; │ │ │ │ │ -176 │ │ │ │ │ -177 // Just iterate over all A matrices and multiply in correct config part │ │ │ │ │ -178 for (size_t pos = 0; pos < _s_i_z_e(); ++pos) │ │ │ │ │ -179 Ax += Ab_(pos) * ConstDMap(x + D * _k_e_y_s__[pos]); │ │ │ │ │ -180 │ │ │ │ │ -181 return model_ ? model_->whiten(Ax) : Ax; │ │ │ │ │ -182 } │ │ │ │ │ -183 │ │ │ │ │ -184}; │ │ │ │ │ -185// end class RegularJacobianFactor │ │ │ │ │ -186 │ │ │ │ │ -187}// end namespace gtsam │ │ │ │ │ -_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ -Factor Graph Values. │ │ │ │ │ +100 // Assign RHS vector │ │ │ │ │ +101 _g_e_t_b() = b; │ │ │ │ │ +102 │ │ │ │ │ +103 // Assign noise model │ │ │ │ │ +104 model_ = noiseModel; │ │ │ │ │ +105 } │ │ │ │ │ +106 │ │ │ │ │ +107} // gtsam │ │ │ │ │ +108 │ │ │ │ │ +_l_i_n_e_a_r_E_x_c_e_p_t_i_o_n_s_._h │ │ │ │ │ +Exceptions that may be thrown by linear solver components. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_D_e_n_s_e_I_n_d_e_x │ │ │ │ │ ptrdiff_t DenseIndex │ │ │ │ │ The index type for Eigen objects. │ │ │ │ │ DDeeffiinniittiioonn types.h:106 │ │ │ │ │ -_g_t_s_a_m_:_:_d_o_t │ │ │ │ │ -double dot(const V1 &a, const V2 &b) │ │ │ │ │ -Dot product. │ │ │ │ │ -DDeeffiinniittiioonn Vector.h:195 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ of vertical blocks. │ │ │ │ │ DDeeffiinniittiioonn VerticalBlockMatrix.h:43 │ │ │ │ │ _g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_r_o_w_s │ │ │ │ │ DenseIndex rows() const │ │ │ │ │ Row size. │ │ │ │ │ DDeeffiinniittiioonn VerticalBlockMatrix.h:115 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s │ │ │ │ │ -const KeyVector & keys() const │ │ │ │ │ -Access the factor's involved variable keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:140 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_c_o_l_s │ │ │ │ │ +DenseIndex cols() const │ │ │ │ │ +Column size. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:118 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_n_B_l_o_c_k_s │ │ │ │ │ +DenseIndex nBlocks() const │ │ │ │ │ +Block count. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:121 │ │ │ │ │ _g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s__ │ │ │ │ │ KeyVector keys_ │ │ │ │ │ The keys involved in this factor. │ │ │ │ │ DDeeffiinniittiioonn Factor.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_m_p_t_y │ │ │ │ │ -bool empty() const │ │ │ │ │ -Whether the factor is empty (involves zero variables). │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:128 │ │ │ │ │ _g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_s_i_z_e │ │ │ │ │ size_t size() const │ │ │ │ │ DDeeffiinniittiioonn Factor.h:157 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_h_e_s_s_i_a_n_D_i_a_g_o_n_a_l │ │ │ │ │ -VectorValues hessianDiagonal() const │ │ │ │ │ -Return the diagonal of the Hessian for this factor. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactor.cpp:35 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ -A Gaussian factor in the squared-error form. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:91 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +An abstract virtual base class for JacobianFactor and HessianFactor. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactor.h:39 │ │ │ │ │ _g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_e_t_b │ │ │ │ │ const constBVector getb() const │ │ │ │ │ Get a view of the r.h.s. │ │ │ │ │ DDeeffiinniittiioonn JacobianFactor.h:297 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d │ │ │ │ │ -void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) │ │ │ │ │ -const override │ │ │ │ │ -y += alpha * A'*A*x │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.cpp:649 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_r_a_d_i_e_n_t_A_t_Z_e_r_o │ │ │ │ │ -VectorValues gradientAtZero() const override │ │ │ │ │ -A'*b for Jacobian. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.cpp:701 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ -JacobianFactor with constant sized blocks Provides raw memory access versions │ │ │ │ │ -of linear operator. │ │ │ │ │ -DDeeffiinniittiioonn RegularJacobianFactor.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ -Vector operator*(const double *x) const │ │ │ │ │ -double* Matrix-vector multiply, i.e. │ │ │ │ │ -DDeeffiinniittiioonn RegularJacobianFactor.h:172 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_h_e_s_s_i_a_n_D_i_a_g_o_n_a_l │ │ │ │ │ -void hessianDiagonal(double *d) const override │ │ │ │ │ -Raw memory access version of hessianDiagonal. │ │ │ │ │ -DDeeffiinniittiioonn RegularJacobianFactor.h:109 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_r_a_d_i_e_n_t_A_t_Z_e_r_o │ │ │ │ │ -void gradientAtZero(double *d) const override │ │ │ │ │ -Raw memory access version of gradientAtZero. │ │ │ │ │ -DDeeffiinniittiioonn RegularJacobianFactor.h:133 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_t_r_a_n_s_p_o_s_e_M_u_l_t_i_p_l_y_A_d_d │ │ │ │ │ -void transposeMultiplyAdd(double alpha, const Vector &e, double *x) const │ │ │ │ │ -double* Transpose Matrix-vector multiply, i.e. │ │ │ │ │ -DDeeffiinniittiioonn RegularJacobianFactor.h:161 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_r_a_d_i_e_n_t_A_t_Z_e_r_o │ │ │ │ │ -VectorValues gradientAtZero() const override │ │ │ │ │ -Expose base class gradientAtZero. │ │ │ │ │ -DDeeffiinniittiioonn RegularJacobianFactor.h:128 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ -RegularJacobianFactor(const TERMS &terms, const Vector &b, const SharedDiagonal │ │ │ │ │ -&model=SharedDiagonal()) │ │ │ │ │ -Construct an n-ary factor. │ │ │ │ │ -DDeeffiinniittiioonn RegularJacobianFactor.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d │ │ │ │ │ -void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) │ │ │ │ │ -const override │ │ │ │ │ -y += alpha * A'*A*x │ │ │ │ │ -DDeeffiinniittiioonn RegularJacobianFactor.h:73 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ -RegularJacobianFactor() │ │ │ │ │ -Default constructor. │ │ │ │ │ -DDeeffiinniittiioonn RegularJacobianFactor.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ -RegularJacobianFactor(const KEYS &keys, const VerticalBlockMatrix │ │ │ │ │ -&augmentedMatrix, const SharedDiagonal &sigmas=SharedDiagonal()) │ │ │ │ │ -Constructor with arbitrary number keys, and where the augmented matrix is given │ │ │ │ │ -all together instead ... │ │ │ │ │ -DDeeffiinniittiioonn RegularJacobianFactor.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d │ │ │ │ │ -void multiplyHessianAdd(double alpha, const double *x, double *y) const │ │ │ │ │ -double* Hessian-vector multiply, i.e. │ │ │ │ │ -DDeeffiinniittiioonn RegularJacobianFactor.h:82 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ -with a unique integer... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ +JacobianFactor() │ │ │ │ │ +default constructor for I/O │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.cpp:54 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_f_i_l_l_T_e_r_m_s │ │ │ │ │ +void fillTerms(const TERMS &terms, const Vector &b, const SharedDiagonal │ │ │ │ │ +&noiseModel) │ │ │ │ │ +Internal function to fill blocks and set dimensions. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor-inl.h:60 │ │ │ │ │ +_g_t_s_a_m_:_:_I_n_v_a_l_i_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +An exception indicating that the noise model dimension passed into a │ │ │ │ │ +JacobianFactor has a different d... │ │ │ │ │ +DDeeffiinniittiioonn linearExceptions.h:106 │ │ │ │ │ +_g_t_s_a_m_:_:_I_n_v_a_l_i_d_M_a_t_r_i_x_B_l_o_c_k │ │ │ │ │ +An exception indicating that a matrix block passed into a JacobianFactor has a │ │ │ │ │ +different dimensionali... │ │ │ │ │ +DDeeffiinniittiioonn linearExceptions.h:124 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ + * JJaaccoobbiiaannFFaaccttoorr--iinnll..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00728_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Preconditioner.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/ConjugateGradientSolver.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,212 +98,189 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Preconditioner.h
│ │ │ │ +
ConjugateGradientSolver.h
│ │ │ │
│ │ │ │
│ │ │ │ -
1/*
│ │ │ │ -
2 * Preconditioner.h
│ │ │ │ -
3 *
│ │ │ │ -
4 * Created on: Jun 2, 2014
│ │ │ │ -
5 * Author: Yong-Dian Jian
│ │ │ │ -
6 * Author: Sungtae An
│ │ │ │ -
7 */
│ │ │ │ -
8
│ │ │ │ -
9#pragma once
│ │ │ │ -
10
│ │ │ │ -
11#include <gtsam/base/Vector.h>
│ │ │ │ -
12#include <boost/shared_ptr.hpp>
│ │ │ │ -
13#include <iosfwd>
│ │ │ │ -
14#include <map>
│ │ │ │ -
15#include <string>
│ │ │ │ -
16
│ │ │ │ -
17namespace gtsam {
│ │ │ │ -
18
│ │ │ │ -
19class GaussianFactorGraph;
│ │ │ │ -
20class KeyInfo;
│ │ │ │ -
21class VectorValues;
│ │ │ │ -
22
│ │ │ │ -
23/* parameters for the preconditioner */
│ │ │ │ -
│ │ │ │ -
24struct GTSAM_EXPORT PreconditionerParameters {
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +
2
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
4 * Atlanta, Georgia 30332-0415
│ │ │ │ +
5 * All Rights Reserved
│ │ │ │ +
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
7
│ │ │ │ +
8 * See LICENSE for the license information
│ │ │ │ +
9
│ │ │ │ +
10 * -------------------------------------------------------------------------- */
│ │ │ │ +
11
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/linear/IterativeSolver.h>
│ │ │ │ +
23
│ │ │ │ +
24namespace gtsam {
│ │ │ │
25
│ │ │ │ -
26 typedef boost::shared_ptr<PreconditionerParameters> shared_ptr;
│ │ │ │ -
27
│ │ │ │ -
28 enum Kernel { /* Preconditioner Kernel */
│ │ │ │ -
29 GTSAM = 0,
│ │ │ │ -
30 CHOLMOD /* experimental */
│ │ │ │ -
31 } kernel_ ;
│ │ │ │ -
32
│ │ │ │ -
33 enum Verbosity {
│ │ │ │ -
34 SILENT = 0,
│ │ │ │ -
35 COMPLEXITY = 1,
│ │ │ │ -
36 ERROR = 2
│ │ │ │ -
37 } verbosity_ ;
│ │ │ │ -
38
│ │ │ │ -
39 PreconditionerParameters(): kernel_(GTSAM), verbosity_(SILENT) {}
│ │ │ │ -
40 PreconditionerParameters(const PreconditionerParameters &p) : kernel_(p.kernel_), verbosity_(p.verbosity_) {}
│ │ │ │ -
41 virtual ~PreconditionerParameters() {}
│ │ │ │ -
42
│ │ │ │ -
43 /* general interface */
│ │ │ │ -
44 inline Kernel kernel() const { return kernel_; }
│ │ │ │ -
45 inline Verbosity verbosity() const { return verbosity_; }
│ │ │ │ -
46
│ │ │ │ -
47 void print() const;
│ │ │ │ -
48
│ │ │ │ -
49 virtual void print(std::ostream &os) const;
│ │ │ │ -
50
│ │ │ │ -
51 static Kernel kernelTranslator(const std::string &s);
│ │ │ │ -
52 static Verbosity verbosityTranslator(const std::string &s);
│ │ │ │ -
53 static std::string kernelTranslator(Kernel k);
│ │ │ │ -
54 static std::string verbosityTranslator(Verbosity v);
│ │ │ │ -
55
│ │ │ │ -
56 /* for serialization */
│ │ │ │ -
57 friend std::ostream& operator<<(std::ostream &os, const PreconditionerParameters &p);
│ │ │ │ -
58 };
│ │ │ │ -
│ │ │ │ -
59
│ │ │ │ -
60/* PCG aims to solve the problem: A x = b by reparametrizing it as
│ │ │ │ -
61 * L^{-1} A L^{-T} y = L^{-1} b or M^{-1} A x = M^{-1} b,
│ │ │ │ -
62 * where A \approx L L^{T}, or A \approx M
│ │ │ │ -
63 * The goal of this class is to provide a general interface to all preconditioners */
│ │ │ │ -
│ │ │ │ -
64class GTSAM_EXPORT Preconditioner {
│ │ │ │ -
65public:
│ │ │ │ -
66 typedef boost::shared_ptr<Preconditioner> shared_ptr;
│ │ │ │ -
67 typedef std::vector<size_t> Dimensions;
│ │ │ │ -
68
│ │ │ │ -
69 /* Generic Constructor and Destructor */
│ │ │ │ -
70 Preconditioner() {}
│ │ │ │ -
71 virtual ~Preconditioner() {}
│ │ │ │ -
72
│ │ │ │ -
73 /*
│ │ │ │ -
74 * Abstract interface for raw vectors. VectorValues is a speed bottleneck
│ │ │ │ -
75 * and Yong-Dian has profiled preconditioners (outside GTSAM) with the the
│ │ │ │ -
76 * three methods below. In GTSAM, unfortunately, we are still using the
│ │ │ │ -
77 * VectorValues methods called in iterative-inl.h
│ │ │ │ -
78 */
│ │ │ │ -
79
│ │ │ │ -
81 virtual void solve(const Vector& y, Vector &x) const = 0;
│ │ │ │ -
82
│ │ │ │ -
84 virtual void transposeSolve(const Vector& y, Vector& x) const = 0;
│ │ │ │ -
85
│ │ │ │ -
87 virtual void build(
│ │ │ │ -
88 const GaussianFactorGraph &gfg,
│ │ │ │ -
89 const KeyInfo &info,
│ │ │ │ -
90 const std::map<Key,Vector> &lambda
│ │ │ │ -
91 ) = 0;
│ │ │ │ -
92};
│ │ │ │ -
│ │ │ │ -
93
│ │ │ │ -
94/*******************************************************************************************/
│ │ │ │ -
│ │ │ │ -
95struct GTSAM_EXPORT DummyPreconditionerParameters : public PreconditionerParameters {
│ │ │ │ -
96 typedef PreconditionerParameters Base;
│ │ │ │ -
97 typedef boost::shared_ptr<DummyPreconditionerParameters> shared_ptr;
│ │ │ │ -
98 DummyPreconditionerParameters() : Base() {}
│ │ │ │ -
99 ~DummyPreconditionerParameters() override {}
│ │ │ │ -
100};
│ │ │ │ -
│ │ │ │ -
101
│ │ │ │ -
102/*******************************************************************************************/
│ │ │ │ -
│ │ │ │ -
103class GTSAM_EXPORT DummyPreconditioner : public Preconditioner {
│ │ │ │ -
104public:
│ │ │ │ -
105 typedef Preconditioner Base;
│ │ │ │ -
106 typedef boost::shared_ptr<DummyPreconditioner> shared_ptr;
│ │ │ │ -
107
│ │ │ │ -
108public:
│ │ │ │ +
│ │ │ │ +
29class GTSAM_EXPORT ConjugateGradientParameters : public IterativeOptimizationParameters {
│ │ │ │ +
30
│ │ │ │ +
31public:
│ │ │ │ +
32 typedef IterativeOptimizationParameters Base;
│ │ │ │ +
33 typedef boost::shared_ptr<ConjugateGradientParameters> shared_ptr;
│ │ │ │ +
34
│ │ │ │ +
35 size_t minIterations_;
│ │ │ │ +
36 size_t maxIterations_;
│ │ │ │ +
37 size_t reset_;
│ │ │ │ +
38 double epsilon_rel_;
│ │ │ │ +
39 double epsilon_abs_;
│ │ │ │ +
40
│ │ │ │ +
41 /* Matrix Operation Kernel */
│ │ │ │ +
│ │ │ │ +
42 enum BLASKernel {
│ │ │ │ +
43 GTSAM = 0,
│ │ │ │ +
44 } blas_kernel_ ;
│ │ │ │ +
│ │ │ │ +
45
│ │ │ │ +
46 ConjugateGradientParameters()
│ │ │ │ +
47 : minIterations_(1), maxIterations_(500), reset_(501), epsilon_rel_(1e-3),
│ │ │ │ +
48 epsilon_abs_(1e-3), blas_kernel_(GTSAM) {}
│ │ │ │ +
49
│ │ │ │ +
50 ConjugateGradientParameters(size_t minIterations, size_t maxIterations, size_t reset,
│ │ │ │ +
51 double epsilon_rel, double epsilon_abs, BLASKernel blas)
│ │ │ │ +
52 : minIterations_(minIterations), maxIterations_(maxIterations), reset_(reset),
│ │ │ │ +
53 epsilon_rel_(epsilon_rel), epsilon_abs_(epsilon_abs), blas_kernel_(blas) {}
│ │ │ │ +
54
│ │ │ │ +
55 ConjugateGradientParameters(const ConjugateGradientParameters &p)
│ │ │ │ +
56 : Base(p), minIterations_(p.minIterations_), maxIterations_(p.maxIterations_), reset_(p.reset_),
│ │ │ │ +
57 epsilon_rel_(p.epsilon_rel_), epsilon_abs_(p.epsilon_abs_), blas_kernel_(GTSAM) {}
│ │ │ │ +
58
│ │ │ │ +
59 /* general interface */
│ │ │ │ +
60 inline size_t minIterations() const { return minIterations_; }
│ │ │ │ +
61 inline size_t maxIterations() const { return maxIterations_; }
│ │ │ │ +
62 inline size_t reset() const { return reset_; }
│ │ │ │ +
63 inline double epsilon() const { return epsilon_rel_; }
│ │ │ │ +
64 inline double epsilon_rel() const { return epsilon_rel_; }
│ │ │ │ +
65 inline double epsilon_abs() const { return epsilon_abs_; }
│ │ │ │ +
66
│ │ │ │ +
67 inline size_t getMinIterations() const { return minIterations_; }
│ │ │ │ +
68 inline size_t getMaxIterations() const { return maxIterations_; }
│ │ │ │ +
69 inline size_t getReset() const { return reset_; }
│ │ │ │ +
70 inline double getEpsilon() const { return epsilon_rel_; }
│ │ │ │ +
71 inline double getEpsilon_rel() const { return epsilon_rel_; }
│ │ │ │ +
72 inline double getEpsilon_abs() const { return epsilon_abs_; }
│ │ │ │ +
73
│ │ │ │ +
74 inline void setMinIterations(size_t value) { minIterations_ = value; }
│ │ │ │ +
75 inline void setMaxIterations(size_t value) { maxIterations_ = value; }
│ │ │ │ +
76 inline void setReset(size_t value) { reset_ = value; }
│ │ │ │ +
77 inline void setEpsilon(double value) { epsilon_rel_ = value; }
│ │ │ │ +
78 inline void setEpsilon_rel(double value) { epsilon_rel_ = value; }
│ │ │ │ +
79 inline void setEpsilon_abs(double value) { epsilon_abs_ = value; }
│ │ │ │ +
80
│ │ │ │ +
81
│ │ │ │ +
82 void print() const { Base::print(); }
│ │ │ │ +
83 void print(std::ostream &os) const override;
│ │ │ │ +
84
│ │ │ │ +
85 static std::string blasTranslator(const BLASKernel k) ;
│ │ │ │ +
86 static BLASKernel blasTranslator(const std::string &s) ;
│ │ │ │ +
87};
│ │ │ │ +
│ │ │ │ +
88
│ │ │ │ +
89/*
│ │ │ │ +
90 * A template for the linear preconditioned conjugate gradient method.
│ │ │ │ +
91 * System class should support residual(v, g), multiply(v,Av), scal(alpha,v), dot(v,v), axpy(alpha,x,y)
│ │ │ │ +
92 * leftPrecondition(v, L^{-1}v, rightPrecondition(v, L^{-T}v) where preconditioner M = L*L^T
│ │ │ │ +
93 * Note that the residual is in the preconditioned domain. Refer to Section 9.2 of Saad's book.
│ │ │ │ +
94 *
│ │ │ │ +
95 ** REFERENCES:
│ │ │ │ +
96 * [1] Y. Saad, "Preconditioned Iterations," in Iterative Methods for Sparse Linear Systems,
│ │ │ │ +
97 * 2nd ed. SIAM, 2003, ch. 9, sec. 2, pp.276-281.
│ │ │ │ +
98 */
│ │ │ │ +
99template<class S, class V>
│ │ │ │ +
100V preconditionedConjugateGradient(const S &system, const V &initial,
│ │ │ │ +
101 const ConjugateGradientParameters &parameters) {
│ │ │ │ +
102
│ │ │ │ +
103 V estimate, residual, direction, q1, q2;
│ │ │ │ +
104 estimate = residual = direction = q1 = q2 = initial;
│ │ │ │ +
105
│ │ │ │ +
106 system.residual(estimate, q1); /* q1 = b-Ax */
│ │ │ │ +
107 system.leftPrecondition(q1, residual); /* r = L^{-1} (b-Ax) */
│ │ │ │ +
108 system.rightPrecondition(residual, direction);/* p = L^{-T} r */
│ │ │ │
109
│ │ │ │ -
110 DummyPreconditioner() : Base() {}
│ │ │ │ -
111 ~DummyPreconditioner() override {}
│ │ │ │ -
112
│ │ │ │ -
113 /* Computation Interfaces for raw vector */
│ │ │ │ -
114 void solve(const Vector& y, Vector &x) const override { x = y; }
│ │ │ │ -
115 void transposeSolve(const Vector& y, Vector& x) const override { x = y; }
│ │ │ │ -
│ │ │ │ -
116 void build(
│ │ │ │ -
117 const GaussianFactorGraph &gfg,
│ │ │ │ -
118 const KeyInfo &info,
│ │ │ │ -
119 const std::map<Key,Vector> &lambda
│ │ │ │ -
120 ) override {}
│ │ │ │ -
│ │ │ │ -
121};
│ │ │ │ -
│ │ │ │ -
122
│ │ │ │ -
123/*******************************************************************************************/
│ │ │ │ -
│ │ │ │ -
124struct GTSAM_EXPORT BlockJacobiPreconditionerParameters : public PreconditionerParameters {
│ │ │ │ -
125 typedef PreconditionerParameters Base;
│ │ │ │ -
126 BlockJacobiPreconditionerParameters() : Base() {}
│ │ │ │ -
127 ~BlockJacobiPreconditionerParameters() override {}
│ │ │ │ -
128};
│ │ │ │ -
│ │ │ │ -
129
│ │ │ │ -
130/*******************************************************************************************/
│ │ │ │ -
│ │ │ │ -
131class GTSAM_EXPORT BlockJacobiPreconditioner : public Preconditioner {
│ │ │ │ -
132public:
│ │ │ │ -
133 typedef Preconditioner Base;
│ │ │ │ -
134 BlockJacobiPreconditioner() ;
│ │ │ │ -
135 ~BlockJacobiPreconditioner() override ;
│ │ │ │ -
136
│ │ │ │ -
137 /* Computation Interfaces for raw vector */
│ │ │ │ -
138 void solve(const Vector& y, Vector &x) const override;
│ │ │ │ -
139 void transposeSolve(const Vector& y, Vector& x) const override;
│ │ │ │ -
140 void build(
│ │ │ │ -
141 const GaussianFactorGraph &gfg,
│ │ │ │ -
142 const KeyInfo &info,
│ │ │ │ -
143 const std::map<Key,Vector> &lambda
│ │ │ │ -
144 ) override;
│ │ │ │ +
110 double currentGamma = system.dot(residual, residual), prevGamma, alpha, beta;
│ │ │ │ +
111
│ │ │ │ +
112 const size_t iMaxIterations = parameters.maxIterations(),
│ │ │ │ +
113 iMinIterations = parameters.minIterations(),
│ │ │ │ +
114 iReset = parameters.reset() ;
│ │ │ │ +
115 const double threshold = std::max(parameters.epsilon_abs(),
│ │ │ │ +
116 parameters.epsilon() * parameters.epsilon() * currentGamma);
│ │ │ │ +
117
│ │ │ │ +
118 if (parameters.verbosity() >= ConjugateGradientParameters::COMPLEXITY )
│ │ │ │ +
119 std::cout << "[PCG] epsilon = " << parameters.epsilon()
│ │ │ │ +
120 << ", max = " << parameters.maxIterations()
│ │ │ │ +
121 << ", reset = " << parameters.reset()
│ │ │ │ +
122 << ", ||r0||^2 = " << currentGamma
│ │ │ │ +
123 << ", threshold = " << threshold << std::endl;
│ │ │ │ +
124
│ │ │ │ +
125 size_t k;
│ │ │ │ +
126 for ( k = 1 ; k <= iMaxIterations && (currentGamma > threshold || k <= iMinIterations) ; k++ ) {
│ │ │ │ +
127
│ │ │ │ +
128 if ( k % iReset == 0 ) {
│ │ │ │ +
129 system.residual(estimate, q1); /* q1 = b-Ax */
│ │ │ │ +
130 system.leftPrecondition(q1, residual); /* r = L^{-1} (b-Ax) */
│ │ │ │ +
131 system.rightPrecondition(residual, direction); /* p = L^{-T} r */
│ │ │ │ +
132 currentGamma = system.dot(residual, residual);
│ │ │ │ +
133 }
│ │ │ │ +
134 system.multiply(direction, q1); /* q1 = A p */
│ │ │ │ +
135 alpha = currentGamma / system.dot(direction, q1); /* alpha = gamma / (p' A p) */
│ │ │ │ +
136 system.axpy(alpha, direction, estimate); /* estimate += alpha * p */
│ │ │ │ +
137 system.leftPrecondition(q1, q2); /* q2 = L^{-1} * q1 */
│ │ │ │ +
138 system.axpy(-alpha, q2, residual); /* r -= alpha * q2 */
│ │ │ │ +
139 prevGamma = currentGamma;
│ │ │ │ +
140 currentGamma = system.dot(residual, residual); /* gamma = |r|^2 */
│ │ │ │ +
141 beta = currentGamma / prevGamma;
│ │ │ │ +
142 system.rightPrecondition(residual, q1); /* q1 = L^{-T} r */
│ │ │ │ +
143 system.scal(beta, direction);
│ │ │ │ +
144 system.axpy(1.0, q1, direction); /* p = q1 + beta * p */
│ │ │ │
145
│ │ │ │ -
146protected:
│ │ │ │ -
147
│ │ │ │ -
148 void clean() ;
│ │ │ │ -
149
│ │ │ │ -
150 std::vector<size_t> dims_;
│ │ │ │ -
151 double *buffer_;
│ │ │ │ -
152 size_t bufferSize_;
│ │ │ │ -
153 size_t nnz_;
│ │ │ │ -
154};
│ │ │ │ -
│ │ │ │ -
155
│ │ │ │ -
156/*********************************************************************************************/
│ │ │ │ -
157/* factory method to create preconditioners */
│ │ │ │ -
158boost::shared_ptr<Preconditioner> createPreconditioner(const boost::shared_ptr<PreconditionerParameters> parameters);
│ │ │ │ +
146 if (parameters.verbosity() >= ConjugateGradientParameters::ERROR )
│ │ │ │ +
147 std::cout << "[PCG] k = " << k
│ │ │ │ +
148 << ", alpha = " << alpha
│ │ │ │ +
149 << ", beta = " << beta
│ │ │ │ +
150 << ", ||r||^2 = " << currentGamma
│ │ │ │ +
151// << "\nx =\n" << estimate
│ │ │ │ +
152// << "\nr =\n" << residual
│ │ │ │ +
153 << std::endl;
│ │ │ │ +
154 }
│ │ │ │ +
155 if (parameters.verbosity() >= ConjugateGradientParameters::COMPLEXITY )
│ │ │ │ +
156 std::cout << "[PCG] iterations = " << k
│ │ │ │ +
157 << ", ||r||^2 = " << currentGamma
│ │ │ │ +
158 << std::endl;
│ │ │ │
159
│ │ │ │ -
160}
│ │ │ │ -
161
│ │ │ │ +
160 return estimate;
│ │ │ │ +
161}
│ │ │ │
162
│ │ │ │ -
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ +
163
│ │ │ │ +
164}
│ │ │ │ +
IterativeSolver.h
Some support classes for iterative solvers.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ -
gtsam::KeyInfo
Handy data structure for iterative solvers.
Definition IterativeSolver.h:126
│ │ │ │ -
gtsam::PreconditionerParameters
Definition Preconditioner.h:24
│ │ │ │ -
gtsam::Preconditioner
Definition Preconditioner.h:64
│ │ │ │ -
gtsam::Preconditioner::solve
virtual void solve(const Vector &y, Vector &x) const =0
implement x = L^{-1} y
│ │ │ │ -
gtsam::Preconditioner::build
virtual void build(const GaussianFactorGraph &gfg, const KeyInfo &info, const std::map< Key, Vector > &lambda)=0
build/factorize the preconditioner
│ │ │ │ -
gtsam::Preconditioner::transposeSolve
virtual void transposeSolve(const Vector &y, Vector &x) const =0
implement x = L^{-T} y
│ │ │ │ -
gtsam::DummyPreconditionerParameters
Definition Preconditioner.h:95
│ │ │ │ -
gtsam::DummyPreconditioner
Definition Preconditioner.h:103
│ │ │ │ -
gtsam::DummyPreconditioner::build
void build(const GaussianFactorGraph &gfg, const KeyInfo &info, const std::map< Key, Vector > &lambda) override
build/factorize the preconditioner
Definition Preconditioner.h:116
│ │ │ │ -
gtsam::DummyPreconditioner::solve
void solve(const Vector &y, Vector &x) const override
implement x = L^{-1} y
Definition Preconditioner.h:114
│ │ │ │ -
gtsam::DummyPreconditioner::transposeSolve
void transposeSolve(const Vector &y, Vector &x) const override
implement x = L^{-T} y
Definition Preconditioner.h:115
│ │ │ │ -
gtsam::BlockJacobiPreconditionerParameters
Definition Preconditioner.h:124
│ │ │ │ -
gtsam::BlockJacobiPreconditioner
Definition Preconditioner.h:131
│ │ │ │ -
VectorValues
The Factor::error simply extracts the.
│ │ │ │ +
gtsam::ConjugateGradientParameters
parameters for the conjugate gradient method
Definition ConjugateGradientSolver.h:29
│ │ │ │ +
gtsam::ConjugateGradientParameters::minIterations_
size_t minIterations_
minimum number of cg iterations
Definition ConjugateGradientSolver.h:35
│ │ │ │ +
gtsam::ConjugateGradientParameters::reset_
size_t reset_
number of iterations before reset
Definition ConjugateGradientSolver.h:37
│ │ │ │ +
gtsam::ConjugateGradientParameters::BLASKernel
BLASKernel
Definition ConjugateGradientSolver.h:42
│ │ │ │ +
gtsam::ConjugateGradientParameters::epsilon_rel_
double epsilon_rel_
threshold for relative error decrease
Definition ConjugateGradientSolver.h:38
│ │ │ │ +
gtsam::ConjugateGradientParameters::maxIterations_
size_t maxIterations_
maximum number of cg iterations
Definition ConjugateGradientSolver.h:36
│ │ │ │ +
gtsam::ConjugateGradientParameters::epsilon_abs_
double epsilon_abs_
threshold for absolute error decrease
Definition ConjugateGradientSolver.h:39
│ │ │ │ +
gtsam::IterativeOptimizationParameters
parameters for iterative linear solvers
Definition IterativeSolver.h:44
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,232 +1,216 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Preconditioner.h │ │ │ │ │ -1/* │ │ │ │ │ -2 * Preconditioner.h │ │ │ │ │ -3 * │ │ │ │ │ -4 * Created on: Jun 2, 2014 │ │ │ │ │ -5 * Author: Yong-Dian Jian │ │ │ │ │ -6 * Author: Sungtae An │ │ │ │ │ -7 */ │ │ │ │ │ -8 │ │ │ │ │ -9#pragma once │ │ │ │ │ -10 │ │ │ │ │ -11#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_c_t_o_r_._h> │ │ │ │ │ -12#include │ │ │ │ │ -13#include │ │ │ │ │ -14#include │ │ │ │ │ -15#include │ │ │ │ │ -16 │ │ │ │ │ -17namespace _g_t_s_a_m { │ │ │ │ │ -18 │ │ │ │ │ -19class GaussianFactorGraph; │ │ │ │ │ -20class KeyInfo; │ │ │ │ │ -21class _V_e_c_t_o_r_V_a_l_u_e_s; │ │ │ │ │ -22 │ │ │ │ │ -23/* parameters for the preconditioner */ │ │ │ │ │ -_2_4struct GTSAM_EXPORT _P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s { │ │ │ │ │ +ConjugateGradientSolver.h │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +1/* --------------------------------------------------------------------------- │ │ │ │ │ +- │ │ │ │ │ +2 │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +5 * All Rights Reserved │ │ │ │ │ +6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +7 │ │ │ │ │ +8 * See LICENSE for the license information │ │ │ │ │ +9 │ │ │ │ │ +10 * ------------------------------------------------------------------------- │ │ │ │ │ +- */ │ │ │ │ │ +11 │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_I_t_e_r_a_t_i_v_e_S_o_l_v_e_r_._h> │ │ │ │ │ +23 │ │ │ │ │ +24namespace _g_t_s_a_m { │ │ │ │ │ 25 │ │ │ │ │ -26 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -27 │ │ │ │ │ -28 enum Kernel { /* Preconditioner Kernel */ │ │ │ │ │ -29 GTSAM = 0, │ │ │ │ │ -30 CHOLMOD /* experimental */ │ │ │ │ │ -31 } kernel_ ; │ │ │ │ │ -32 │ │ │ │ │ -33 enum Verbosity { │ │ │ │ │ -34 SILENT = 0, │ │ │ │ │ -35 COMPLEXITY = 1, │ │ │ │ │ -36 ERROR = 2 │ │ │ │ │ -37 } verbosity_ ; │ │ │ │ │ -38 │ │ │ │ │ -39 _P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s(): kernel_(GTSAM), verbosity_(SILENT) {} │ │ │ │ │ -40 _P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s(const _P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s &p) : kernel_ │ │ │ │ │ -(p.kernel_), verbosity_(p.verbosity_) {} │ │ │ │ │ -41 virtual _~_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s() {} │ │ │ │ │ -42 │ │ │ │ │ -43 /* general interface */ │ │ │ │ │ -44 inline Kernel kernel() const { return kernel_; } │ │ │ │ │ -45 inline Verbosity verbosity() const { return verbosity_; } │ │ │ │ │ -46 │ │ │ │ │ -47 void _p_r_i_n_t() const; │ │ │ │ │ -48 │ │ │ │ │ -49 virtual void _p_r_i_n_t(std::ostream &os) const; │ │ │ │ │ -50 │ │ │ │ │ -51 static Kernel kernelTranslator(const std::string &s); │ │ │ │ │ -52 static Verbosity verbosityTranslator(const std::string &s); │ │ │ │ │ -53 static std::string kernelTranslator(Kernel k); │ │ │ │ │ -54 static std::string verbosityTranslator(Verbosity v); │ │ │ │ │ -55 │ │ │ │ │ -56 /* for serialization */ │ │ │ │ │ -57 friend std::ostream& operator<<(std::ostream &os, const │ │ │ │ │ -_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s &p); │ │ │ │ │ -58 }; │ │ │ │ │ -59 │ │ │ │ │ -60/* PCG aims to solve the problem: A x = b by reparametrizing it as │ │ │ │ │ -61 * L^{-1} A L^{-T} y = L^{-1} b or M^{-1} A x = M^{-1} b, │ │ │ │ │ -62 * where A \approx L L^{T}, or A \approx M │ │ │ │ │ -63 * The goal of this class is to provide a general interface to all │ │ │ │ │ -preconditioners */ │ │ │ │ │ -_6_4class GTSAM_EXPORT _P_r_e_c_o_n_d_i_t_i_o_n_e_r { │ │ │ │ │ -65public: │ │ │ │ │ -66 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -67 typedef std::vector Dimensions; │ │ │ │ │ -68 │ │ │ │ │ -69 /* Generic Constructor and Destructor */ │ │ │ │ │ -70 _P_r_e_c_o_n_d_i_t_i_o_n_e_r() {} │ │ │ │ │ -71 virtual _~_P_r_e_c_o_n_d_i_t_i_o_n_e_r() {} │ │ │ │ │ -72 │ │ │ │ │ -73 /* │ │ │ │ │ -74 * Abstract interface for raw vectors. VectorValues is a speed bottleneck │ │ │ │ │ -75 * and Yong-Dian has profiled preconditioners (outside GTSAM) with the the │ │ │ │ │ -76 * three methods below. In GTSAM, unfortunately, we are still using the │ │ │ │ │ -77 * VectorValues methods called in iterative-inl.h │ │ │ │ │ -78 */ │ │ │ │ │ -79 │ │ │ │ │ -_8_1 virtual void _s_o_l_v_e(const Vector& y, Vector &x) const = 0; │ │ │ │ │ -82 │ │ │ │ │ -_8_4 virtual void _t_r_a_n_s_p_o_s_e_S_o_l_v_e(const Vector& y, Vector& x) const = 0; │ │ │ │ │ -85 │ │ │ │ │ -_8_7 virtual void _b_u_i_l_d( │ │ │ │ │ -88 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, │ │ │ │ │ -89 const _K_e_y_I_n_f_o &info, │ │ │ │ │ -90 const std::map &lambda │ │ │ │ │ -91 ) = 0; │ │ │ │ │ -92}; │ │ │ │ │ -93 │ │ │ │ │ -94/ │ │ │ │ │ -*******************************************************************************************/ │ │ │ │ │ -_9_5struct GTSAM_EXPORT _D_u_m_m_y_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s : public │ │ │ │ │ -_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s { │ │ │ │ │ -96 typedef _P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s _B_a_s_e; │ │ │ │ │ -97 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -98 _D_u_m_m_y_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s() : _B_a_s_e() {} │ │ │ │ │ -99 _~_D_u_m_m_y_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s() override {} │ │ │ │ │ -100}; │ │ │ │ │ -101 │ │ │ │ │ -102/ │ │ │ │ │ -*******************************************************************************************/ │ │ │ │ │ -_1_0_3class GTSAM_EXPORT _D_u_m_m_y_P_r_e_c_o_n_d_i_t_i_o_n_e_r : public _P_r_e_c_o_n_d_i_t_i_o_n_e_r { │ │ │ │ │ -104public: │ │ │ │ │ -105 typedef _P_r_e_c_o_n_d_i_t_i_o_n_e_r _B_a_s_e; │ │ │ │ │ -106 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -107 │ │ │ │ │ -108public: │ │ │ │ │ +_2_9class GTSAM_EXPORT _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s : public │ │ │ │ │ +_I_t_e_r_a_t_i_v_e_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s { │ │ │ │ │ +30 │ │ │ │ │ +31public: │ │ │ │ │ +32 typedef _I_t_e_r_a_t_i_v_e_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s _B_a_s_e; │ │ │ │ │ +33 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +34 │ │ │ │ │ +_3_5 size_t _m_i_n_I_t_e_r_a_t_i_o_n_s__; │ │ │ │ │ +_3_6 size_t _m_a_x_I_t_e_r_a_t_i_o_n_s__; │ │ │ │ │ +_3_7 size_t _r_e_s_e_t__; │ │ │ │ │ +_3_8 double _e_p_s_i_l_o_n___r_e_l__; │ │ │ │ │ +_3_9 double _e_p_s_i_l_o_n___a_b_s__; │ │ │ │ │ +40 │ │ │ │ │ +41 /* Matrix Operation Kernel */ │ │ │ │ │ +_4_2 enum _B_L_A_S_K_e_r_n_e_l { │ │ │ │ │ +_4_3 GTSAM = 0, │ │ │ │ │ +44 } blas_kernel_ ; │ │ │ │ │ +45 │ │ │ │ │ +46 _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s() │ │ │ │ │ +47 : minIterations_(1), maxIterations_(500), reset_(501), epsilon_rel_(1e-3), │ │ │ │ │ +48 epsilon_abs_(1e-3), blas_kernel_(GTSAM) {} │ │ │ │ │ +49 │ │ │ │ │ +50 ConjugateGradientParameters(size_t minIterations, size_t maxIterations, │ │ │ │ │ +size_t reset, │ │ │ │ │ +51 double epsilon_rel, double epsilon_abs, BLASKernel blas) │ │ │ │ │ +52 : minIterations_(minIterations), maxIterations_(maxIterations), reset_ │ │ │ │ │ +(reset), │ │ │ │ │ +53 epsilon_rel_(epsilon_rel), epsilon_abs_(epsilon_abs), blas_kernel_(blas) {} │ │ │ │ │ +54 │ │ │ │ │ +55 ConjugateGradientParameters(const ConjugateGradientParameters &p) │ │ │ │ │ +56 : Base(p), minIterations_(p.minIterations_), maxIterations_ │ │ │ │ │ +(p.maxIterations_), reset_(p.reset_), │ │ │ │ │ +57 epsilon_rel_(p.epsilon_rel_), epsilon_abs_(p.epsilon_abs_), blas_kernel_ │ │ │ │ │ +(GTSAM) {} │ │ │ │ │ +58 │ │ │ │ │ +59 /* general interface */ │ │ │ │ │ +60 inline size_t minIterations() const { return minIterations_; } │ │ │ │ │ +61 inline size_t maxIterations() const { return maxIterations_; } │ │ │ │ │ +62 inline size_t reset() const { return reset_; } │ │ │ │ │ +63 inline double epsilon() const { return epsilon_rel_; } │ │ │ │ │ +64 inline double epsilon_rel() const { return epsilon_rel_; } │ │ │ │ │ +65 inline double epsilon_abs() const { return epsilon_abs_; } │ │ │ │ │ +66 │ │ │ │ │ +67 inline size_t getMinIterations() const { return minIterations_; } │ │ │ │ │ +68 inline size_t getMaxIterations() const { return maxIterations_; } │ │ │ │ │ +69 inline size_t getReset() const { return reset_; } │ │ │ │ │ +70 inline double getEpsilon() const { return epsilon_rel_; } │ │ │ │ │ +71 inline double getEpsilon_rel() const { return epsilon_rel_; } │ │ │ │ │ +72 inline double getEpsilon_abs() const { return epsilon_abs_; } │ │ │ │ │ +73 │ │ │ │ │ +74 inline void setMinIterations(size_t value) { minIterations_ = value; } │ │ │ │ │ +75 inline void setMaxIterations(size_t value) { maxIterations_ = value; } │ │ │ │ │ +76 inline void setReset(size_t value) { reset_ = value; } │ │ │ │ │ +77 inline void setEpsilon(double value) { epsilon_rel_ = value; } │ │ │ │ │ +78 inline void setEpsilon_rel(double value) { epsilon_rel_ = value; } │ │ │ │ │ +79 inline void setEpsilon_abs(double value) { epsilon_abs_ = value; } │ │ │ │ │ +80 │ │ │ │ │ +81 │ │ │ │ │ +82 void _p_r_i_n_t() const { Base::print(); } │ │ │ │ │ +83 void _p_r_i_n_t(std::ostream &os) const override; │ │ │ │ │ +84 │ │ │ │ │ +85 static std::string blasTranslator(const BLASKernel k) ; │ │ │ │ │ +86 static BLASKernel blasTranslator(const std::string &s) ; │ │ │ │ │ +87}; │ │ │ │ │ +88 │ │ │ │ │ +89/* │ │ │ │ │ +90 * A template for the linear preconditioned conjugate gradient method. │ │ │ │ │ +91 * System class should support residual(v, g), multiply(v,Av), scal(alpha,v), │ │ │ │ │ +dot(v,v), axpy(alpha,x,y) │ │ │ │ │ +92 * leftPrecondition(v, L^{-1}v, rightPrecondition(v, L^{-T}v) where │ │ │ │ │ +preconditioner M = L*L^T │ │ │ │ │ +93 * Note that the residual is in the preconditioned domain. Refer to Section │ │ │ │ │ +9.2 of Saad's book. │ │ │ │ │ +94 * │ │ │ │ │ +95 ** REFERENCES: │ │ │ │ │ +96 * [1] Y. Saad, "Preconditioned Iterations," in Iterative Methods for Sparse │ │ │ │ │ +Linear Systems, │ │ │ │ │ +97 * 2nd ed. SIAM, 2003, ch. 9, sec. 2, pp.276-281. │ │ │ │ │ +98 */ │ │ │ │ │ +99template │ │ │ │ │ +100V preconditionedConjugateGradient(const S &system, const V &initial, │ │ │ │ │ +101 const ConjugateGradientParameters ¶meters) { │ │ │ │ │ +102 │ │ │ │ │ +103 V estimate, residual, direction, q1, q2; │ │ │ │ │ +104 estimate = residual = direction = q1 = q2 = initial; │ │ │ │ │ +105 │ │ │ │ │ +106 system.residual(estimate, q1); /* q1 = b-Ax */ │ │ │ │ │ +107 system.leftPrecondition(q1, residual); /* r = L^{-1} (b-Ax) */ │ │ │ │ │ +108 system.rightPrecondition(residual, direction);/* p = L^{-T} r */ │ │ │ │ │ 109 │ │ │ │ │ -110 _D_u_m_m_y_P_r_e_c_o_n_d_i_t_i_o_n_e_r() : _B_a_s_e() {} │ │ │ │ │ -111 _~_D_u_m_m_y_P_r_e_c_o_n_d_i_t_i_o_n_e_r() override {} │ │ │ │ │ -112 │ │ │ │ │ -113 /* Computation Interfaces for raw vector */ │ │ │ │ │ -_1_1_4 void _s_o_l_v_e(const Vector& y, Vector &x) const override { x = y; } │ │ │ │ │ -_1_1_5 void _t_r_a_n_s_p_o_s_e_S_o_l_v_e(const Vector& y, Vector& x) const override { x = y; } │ │ │ │ │ -_1_1_6 void _b_u_i_l_d( │ │ │ │ │ -117 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, │ │ │ │ │ -118 const _K_e_y_I_n_f_o &info, │ │ │ │ │ -119 const std::map &lambda │ │ │ │ │ -120 ) override {} │ │ │ │ │ -121}; │ │ │ │ │ -122 │ │ │ │ │ -123/ │ │ │ │ │ -*******************************************************************************************/ │ │ │ │ │ -_1_2_4struct GTSAM_EXPORT _B_l_o_c_k_J_a_c_o_b_i_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s : public │ │ │ │ │ -_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s { │ │ │ │ │ -125 typedef _P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s _B_a_s_e; │ │ │ │ │ -126 _B_l_o_c_k_J_a_c_o_b_i_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s() : _B_a_s_e() {} │ │ │ │ │ -127 _~_B_l_o_c_k_J_a_c_o_b_i_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s() override {} │ │ │ │ │ -128}; │ │ │ │ │ -129 │ │ │ │ │ -130/ │ │ │ │ │ -*******************************************************************************************/ │ │ │ │ │ -_1_3_1class GTSAM_EXPORT _B_l_o_c_k_J_a_c_o_b_i_P_r_e_c_o_n_d_i_t_i_o_n_e_r : public _P_r_e_c_o_n_d_i_t_i_o_n_e_r { │ │ │ │ │ -132public: │ │ │ │ │ -133 typedef _P_r_e_c_o_n_d_i_t_i_o_n_e_r _B_a_s_e; │ │ │ │ │ -134 _B_l_o_c_k_J_a_c_o_b_i_P_r_e_c_o_n_d_i_t_i_o_n_e_r() ; │ │ │ │ │ -135 _~_B_l_o_c_k_J_a_c_o_b_i_P_r_e_c_o_n_d_i_t_i_o_n_e_r() override ; │ │ │ │ │ -136 │ │ │ │ │ -137 /* Computation Interfaces for raw vector */ │ │ │ │ │ -138 void solve(const Vector& y, Vector &x) const override; │ │ │ │ │ -139 void transposeSolve(const Vector& y, Vector& x) const override; │ │ │ │ │ -140 void build( │ │ │ │ │ -141 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, │ │ │ │ │ -142 const _K_e_y_I_n_f_o &info, │ │ │ │ │ -143 const std::map &lambda │ │ │ │ │ -144 ) override; │ │ │ │ │ +110 double currentGamma = system.dot(residual, residual), prevGamma, alpha, │ │ │ │ │ +beta; │ │ │ │ │ +111 │ │ │ │ │ +112 const size_t iMaxIterations = parameters.maxIterations(), │ │ │ │ │ +113 iMinIterations = parameters.minIterations(), │ │ │ │ │ +114 iReset = parameters.reset() ; │ │ │ │ │ +115 const double threshold = std::max(parameters.epsilon_abs(), │ │ │ │ │ +116 parameters.epsilon() * parameters.epsilon() * currentGamma); │ │ │ │ │ +117 │ │ │ │ │ +118 if (parameters.verbosity() >= ConjugateGradientParameters::COMPLEXITY ) │ │ │ │ │ +119 std::cout << "[PCG] epsilon = " << parameters.epsilon() │ │ │ │ │ +120 << ", max = " << parameters.maxIterations() │ │ │ │ │ +121 << ", reset = " << parameters.reset() │ │ │ │ │ +122 << ", ||r0||^2 = " << currentGamma │ │ │ │ │ +123 << ", threshold = " << threshold << std::endl; │ │ │ │ │ +124 │ │ │ │ │ +125 size_t k; │ │ │ │ │ +126 for ( k = 1 ; k <= iMaxIterations && (currentGamma > threshold || k <= │ │ │ │ │ +iMinIterations) ; k++ ) { │ │ │ │ │ +127 │ │ │ │ │ +128 if ( k % iReset == 0 ) { │ │ │ │ │ +129 system.residual(estimate, q1); /* q1 = b-Ax */ │ │ │ │ │ +130 system.leftPrecondition(q1, residual); /* r = L^{-1} (b-Ax) */ │ │ │ │ │ +131 system.rightPrecondition(residual, direction); /* p = L^{-T} r */ │ │ │ │ │ +132 currentGamma = system.dot(residual, residual); │ │ │ │ │ +133 } │ │ │ │ │ +134 system.multiply(direction, q1); /* q1 = A p */ │ │ │ │ │ +135 alpha = currentGamma / system.dot(direction, q1); /* alpha = gamma / (p' A │ │ │ │ │ +p) */ │ │ │ │ │ +136 system.axpy(alpha, direction, estimate); /* estimate += alpha * p */ │ │ │ │ │ +137 system.leftPrecondition(q1, q2); /* q2 = L^{-1} * q1 */ │ │ │ │ │ +138 system.axpy(-alpha, q2, residual); /* r -= alpha * q2 */ │ │ │ │ │ +139 prevGamma = currentGamma; │ │ │ │ │ +140 currentGamma = system.dot(residual, residual); /* gamma = |r|^2 */ │ │ │ │ │ +141 beta = currentGamma / prevGamma; │ │ │ │ │ +142 system.rightPrecondition(residual, q1); /* q1 = L^{-T} r */ │ │ │ │ │ +143 system.scal(beta, direction); │ │ │ │ │ +144 system.axpy(1.0, q1, direction); /* p = q1 + beta * p */ │ │ │ │ │ 145 │ │ │ │ │ -146protected: │ │ │ │ │ -147 │ │ │ │ │ -148 void clean() ; │ │ │ │ │ -149 │ │ │ │ │ -150 std::vector dims_; │ │ │ │ │ -151 double *buffer_; │ │ │ │ │ -152 size_t bufferSize_; │ │ │ │ │ -153 size_t nnz_; │ │ │ │ │ -154}; │ │ │ │ │ -155 │ │ │ │ │ -156/ │ │ │ │ │ -*********************************************************************************************/ │ │ │ │ │ -157/* factory method to create preconditioners */ │ │ │ │ │ -158boost::shared_ptr createPreconditioner(const boost:: │ │ │ │ │ -shared_ptr parameters); │ │ │ │ │ +146 if (parameters.verbosity() >= ConjugateGradientParameters::ERROR ) │ │ │ │ │ +147 std::cout << "[PCG] k = " << k │ │ │ │ │ +148 << ", alpha = " << alpha │ │ │ │ │ +149 << ", beta = " << beta │ │ │ │ │ +150 << ", ||r||^2 = " << currentGamma │ │ │ │ │ +151// << "\nx =\n" << estimate │ │ │ │ │ +152// << "\nr =\n" << residual │ │ │ │ │ +153 << std::endl; │ │ │ │ │ +154 } │ │ │ │ │ +155 if (parameters.verbosity() >= ConjugateGradientParameters::COMPLEXITY ) │ │ │ │ │ +156 std::cout << "[PCG] iterations = " << k │ │ │ │ │ +157 << ", ||r||^2 = " << currentGamma │ │ │ │ │ +158 << std::endl; │ │ │ │ │ 159 │ │ │ │ │ -160} │ │ │ │ │ -161 │ │ │ │ │ +160 return estimate; │ │ │ │ │ +161} │ │ │ │ │ 162 │ │ │ │ │ -_V_e_c_t_o_r_._h │ │ │ │ │ -typedef and functions to augment Eigen's VectorXd │ │ │ │ │ +163 │ │ │ │ │ +164} │ │ │ │ │ +_I_t_e_r_a_t_i_v_e_S_o_l_v_e_r_._h │ │ │ │ │ +Some support classes for iterative solvers. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ print without optional string, must specify cout yourself │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_I_n_f_o │ │ │ │ │ -Handy data structure for iterative solvers. │ │ │ │ │ -DDeeffiinniittiioonn IterativeSolver.h:126 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ -DDeeffiinniittiioonn Preconditioner.h:24 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_c_o_n_d_i_t_i_o_n_e_r │ │ │ │ │ -DDeeffiinniittiioonn Preconditioner.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_c_o_n_d_i_t_i_o_n_e_r_:_:_s_o_l_v_e │ │ │ │ │ -virtual void solve(const Vector &y, Vector &x) const =0 │ │ │ │ │ -implement x = L^{-1} y │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_c_o_n_d_i_t_i_o_n_e_r_:_:_b_u_i_l_d │ │ │ │ │ -virtual void build(const GaussianFactorGraph &gfg, const KeyInfo &info, const │ │ │ │ │ -std::map< Key, Vector > &lambda)=0 │ │ │ │ │ -build/factorize the preconditioner │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_c_o_n_d_i_t_i_o_n_e_r_:_:_t_r_a_n_s_p_o_s_e_S_o_l_v_e │ │ │ │ │ -virtual void transposeSolve(const Vector &y, Vector &x) const =0 │ │ │ │ │ -implement x = L^{-T} y │ │ │ │ │ -_g_t_s_a_m_:_:_D_u_m_m_y_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ -DDeeffiinniittiioonn Preconditioner.h:95 │ │ │ │ │ -_g_t_s_a_m_:_:_D_u_m_m_y_P_r_e_c_o_n_d_i_t_i_o_n_e_r │ │ │ │ │ -DDeeffiinniittiioonn Preconditioner.h:103 │ │ │ │ │ -_g_t_s_a_m_:_:_D_u_m_m_y_P_r_e_c_o_n_d_i_t_i_o_n_e_r_:_:_b_u_i_l_d │ │ │ │ │ -void build(const GaussianFactorGraph &gfg, const KeyInfo &info, const std::map< │ │ │ │ │ -Key, Vector > &lambda) override │ │ │ │ │ -build/factorize the preconditioner │ │ │ │ │ -DDeeffiinniittiioonn Preconditioner.h:116 │ │ │ │ │ -_g_t_s_a_m_:_:_D_u_m_m_y_P_r_e_c_o_n_d_i_t_i_o_n_e_r_:_:_s_o_l_v_e │ │ │ │ │ -void solve(const Vector &y, Vector &x) const override │ │ │ │ │ -implement x = L^{-1} y │ │ │ │ │ -DDeeffiinniittiioonn Preconditioner.h:114 │ │ │ │ │ -_g_t_s_a_m_:_:_D_u_m_m_y_P_r_e_c_o_n_d_i_t_i_o_n_e_r_:_:_t_r_a_n_s_p_o_s_e_S_o_l_v_e │ │ │ │ │ -void transposeSolve(const Vector &y, Vector &x) const override │ │ │ │ │ -implement x = L^{-T} y │ │ │ │ │ -DDeeffiinniittiioonn Preconditioner.h:115 │ │ │ │ │ -_g_t_s_a_m_:_:_B_l_o_c_k_J_a_c_o_b_i_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ -DDeeffiinniittiioonn Preconditioner.h:124 │ │ │ │ │ -_g_t_s_a_m_:_:_B_l_o_c_k_J_a_c_o_b_i_P_r_e_c_o_n_d_i_t_i_o_n_e_r │ │ │ │ │ -DDeeffiinniittiioonn Preconditioner.h:131 │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -The Factor::error simply extracts the. │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ +parameters for the conjugate gradient method │ │ │ │ │ +DDeeffiinniittiioonn ConjugateGradientSolver.h:29 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s_:_:_m_i_n_I_t_e_r_a_t_i_o_n_s__ │ │ │ │ │ +size_t minIterations_ │ │ │ │ │ +minimum number of cg iterations │ │ │ │ │ +DDeeffiinniittiioonn ConjugateGradientSolver.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s_:_:_r_e_s_e_t__ │ │ │ │ │ +size_t reset_ │ │ │ │ │ +number of iterations before reset │ │ │ │ │ +DDeeffiinniittiioonn ConjugateGradientSolver.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s_:_:_B_L_A_S_K_e_r_n_e_l │ │ │ │ │ +BLASKernel │ │ │ │ │ +DDeeffiinniittiioonn ConjugateGradientSolver.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s_:_:_e_p_s_i_l_o_n___r_e_l__ │ │ │ │ │ +double epsilon_rel_ │ │ │ │ │ +threshold for relative error decrease │ │ │ │ │ +DDeeffiinniittiioonn ConjugateGradientSolver.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s_:_:_m_a_x_I_t_e_r_a_t_i_o_n_s__ │ │ │ │ │ +size_t maxIterations_ │ │ │ │ │ +maximum number of cg iterations │ │ │ │ │ +DDeeffiinniittiioonn ConjugateGradientSolver.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s_:_:_e_p_s_i_l_o_n___a_b_s__ │ │ │ │ │ +double epsilon_abs_ │ │ │ │ │ +threshold for absolute error decrease │ │ │ │ │ +DDeeffiinniittiioonn ConjugateGradientSolver.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_I_t_e_r_a_t_i_v_e_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ +parameters for iterative linear solvers │ │ │ │ │ +DDeeffiinniittiioonn IterativeSolver.h:44 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * PPrreeccoonnddiittiioonneerr..hh │ │ │ │ │ + * _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_S_o_l_v_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00731_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/JacobianFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,354 +98,360 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
LossFunctions.h
│ │ │ │ +
JacobianFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ -
1
│ │ │ │ -
2/* ----------------------------------------------------------------------------
│ │ │ │ -
3
│ │ │ │ -
4 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ -
5 * Atlanta, Georgia 30332-0415
│ │ │ │ -
6 * All Rights Reserved
│ │ │ │ -
7 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ -
8
│ │ │ │ -
9 * See LICENSE for the license information
│ │ │ │ -
10
│ │ │ │ -
11 * -------------------------------------------------------------------------- */
│ │ │ │ -
12
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/base/Matrix.h>
│ │ │ │ -
23#include <gtsam/base/Testable.h>
│ │ │ │ -
24#include <gtsam/dllexport.h>
│ │ │ │ -
25
│ │ │ │ -
26#include <boost/serialization/extended_type_info.hpp>
│ │ │ │ -
27#include <boost/serialization/nvp.hpp>
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +
2
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
4 * Atlanta, Georgia 30332-0415
│ │ │ │ +
5 * All Rights Reserved
│ │ │ │ +
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
7
│ │ │ │ +
8 * See LICENSE for the license information
│ │ │ │ +
9
│ │ │ │ +
10 * -------------------------------------------------------------------------- */
│ │ │ │ +
11
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/linear/GaussianFactor.h>
│ │ │ │ +
22#include <gtsam/linear/NoiseModel.h>
│ │ │ │ +
23#include <gtsam/base/VerticalBlockMatrix.h>
│ │ │ │ +
24#include <gtsam/global_includes.h>
│ │ │ │ +
25#include <gtsam/inference/VariableSlots.h>
│ │ │ │ +
26
│ │ │ │ +
27#include <boost/make_shared.hpp>
│ │ │ │
28#include <boost/serialization/version.hpp>
│ │ │ │ -
29#include <boost/serialization/optional.hpp>
│ │ │ │ -
30#include <boost/serialization/shared_ptr.hpp>
│ │ │ │ -
31#include <boost/serialization/singleton.hpp>
│ │ │ │ +
29#include <boost/serialization/split_member.hpp>
│ │ │ │ +
30
│ │ │ │ +
31namespace gtsam {
│ │ │ │
32
│ │ │ │ -
33namespace gtsam {
│ │ │ │ -
34namespace noiseModel {
│ │ │ │ -
35// clang-format off
│ │ │ │ -
54// clang-format on
│ │ │ │ -
55namespace mEstimator {
│ │ │ │ -
56
│ │ │ │ -
│ │ │ │ -
63class GTSAM_EXPORT Base {
│ │ │ │ -
64 public:
│ │ │ │ -
67 enum ReweightScheme { Scalar, Block };
│ │ │ │ -
68 typedef boost::shared_ptr<Base> shared_ptr;
│ │ │ │ -
69
│ │ │ │ -
70 protected:
│ │ │ │ -
72 ReweightScheme reweight_;
│ │ │ │ -
73
│ │ │ │ -
74 public:
│ │ │ │ -
75 Base(const ReweightScheme reweight = Block) : reweight_(reweight) {}
│ │ │ │ -
76 virtual ~Base() {}
│ │ │ │ -
77
│ │ │ │ -
79 ReweightScheme reweightScheme() const { return reweight_; }
│ │ │ │ -
80
│ │ │ │ -
94 virtual double loss(double distance) const { return 0; }
│ │ │ │ -
95
│ │ │ │ -
106 virtual double weight(double distance) const = 0;
│ │ │ │ +
33 // Forward declarations
│ │ │ │ +
34 class HessianFactor;
│ │ │ │ +
35 class VariableSlots;
│ │ │ │ +
36 class GaussianFactorGraph;
│ │ │ │ +
37 class GaussianConditional;
│ │ │ │ +
38 class HessianFactor;
│ │ │ │ +
39 class VectorValues;
│ │ │ │ +
40 class Ordering;
│ │ │ │ +
41 class JacobianFactor;
│ │ │ │ +
42
│ │ │ │ +
48 GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<JacobianFactor> >
│ │ │ │ +
49 EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys);
│ │ │ │ +
50
│ │ │ │ +
│ │ │ │ +
90 class GTSAM_EXPORT JacobianFactor : public GaussianFactor
│ │ │ │ +
91 {
│ │ │ │ +
92 public:
│ │ │ │ +
93
│ │ │ │ +
94 typedef JacobianFactor This;
│ │ │ │ +
95 typedef GaussianFactor Base;
│ │ │ │ +
96 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
97
│ │ │ │ +
98 typedef VerticalBlockMatrix::Block ABlock;
│ │ │ │ +
99 typedef VerticalBlockMatrix::constBlock constABlock;
│ │ │ │ +
100 typedef ABlock::ColXpr BVector;
│ │ │ │ +
101 typedef constABlock::ConstColXpr constBVector;
│ │ │ │ +
102
│ │ │ │ +
103 protected:
│ │ │ │ +
104
│ │ │ │ +
105 VerticalBlockMatrix Ab_; // the block view of the full matrix
│ │ │ │ +
106 noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix
│ │ │ │
107
│ │ │ │ -
108 virtual void print(const std::string &s) const = 0;
│ │ │ │ -
109 virtual bool equals(const Base &expected, double tol = 1e-8) const = 0;
│ │ │ │ -
110
│ │ │ │ -
111 double sqrtWeight(double distance) const { return std::sqrt(weight(distance)); }
│ │ │ │ +
108 public:
│ │ │ │ +
109
│ │ │ │ +
111 explicit JacobianFactor(const GaussianFactor& gf);
│ │ │ │
112
│ │ │ │ -
115 Vector weight(const Vector &error) const;
│ │ │ │ -
116
│ │ │ │ -
118 Vector sqrtWeight(const Vector &error) const;
│ │ │ │ -
119
│ │ │ │ -
122 void reweight(Vector &error) const;
│ │ │ │ -
123 void reweight(std::vector<Matrix> &A, Vector &error) const;
│ │ │ │ -
124 void reweight(Matrix &A, Vector &error) const;
│ │ │ │ -
125 void reweight(Matrix &A1, Matrix &A2, Vector &error) const;
│ │ │ │ -
126 void reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const;
│ │ │ │ -
127
│ │ │ │ -
128 private:
│ │ │ │ -
130 friend class boost::serialization::access;
│ │ │ │ -
131 template <class ARCHIVE>
│ │ │ │ -
132 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ -
133 ar &BOOST_SERIALIZATION_NVP(reweight_);
│ │ │ │ -
134 }
│ │ │ │ -
135};
│ │ │ │ -
│ │ │ │ -
136
│ │ │ │ -
│ │ │ │ -
146class GTSAM_EXPORT Null : public Base {
│ │ │ │ -
147 public:
│ │ │ │ -
148 typedef boost::shared_ptr<Null> shared_ptr;
│ │ │ │ -
149
│ │ │ │ -
150 Null(const ReweightScheme reweight = Block) : Base(reweight) {}
│ │ │ │ -
151 ~Null() override {}
│ │ │ │ -
152 double weight(double /*error*/) const override { return 1.0; }
│ │ │ │ -
153 double loss(double distance) const override { return 0.5 * distance * distance; }
│ │ │ │ -
154 void print(const std::string &s) const override;
│ │ │ │ -
155 bool equals(const Base & /*expected*/, double /*tol*/) const override { return true; }
│ │ │ │ -
156 static shared_ptr Create();
│ │ │ │ -
157
│ │ │ │ -
158 private:
│ │ │ │ -
160 friend class boost::serialization::access;
│ │ │ │ -
161 template <class ARCHIVE>
│ │ │ │ -
162 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ -
163 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
164 }
│ │ │ │ -
165};
│ │ │ │ -
│ │ │ │ -
166
│ │ │ │ -
│ │ │ │ -
175class GTSAM_EXPORT Fair : public Base {
│ │ │ │ -
176 protected:
│ │ │ │ -
177 double c_;
│ │ │ │ -
178
│ │ │ │ -
179 public:
│ │ │ │ -
180 typedef boost::shared_ptr<Fair> shared_ptr;
│ │ │ │ -
181
│ │ │ │ -
182 Fair(double c = 1.3998, const ReweightScheme reweight = Block);
│ │ │ │ -
183 double weight(double distance) const override;
│ │ │ │ -
184 double loss(double distance) const override;
│ │ │ │ -
185 void print(const std::string &s) const override;
│ │ │ │ -
186 bool equals(const Base &expected, double tol = 1e-8) const override;
│ │ │ │ -
187 static shared_ptr Create(double c, const ReweightScheme reweight = Block);
│ │ │ │ -
188 double modelParameter() const { return c_; }
│ │ │ │ -
189
│ │ │ │ -
190 private:
│ │ │ │ -
192 friend class boost::serialization::access;
│ │ │ │ -
193 template <class ARCHIVE>
│ │ │ │ -
194 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ -
195 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
196 ar &BOOST_SERIALIZATION_NVP(c_);
│ │ │ │ -
197 }
│ │ │ │ -
198};
│ │ │ │ -
│ │ │ │ -
199
│ │ │ │ -
│ │ │ │ -
208class GTSAM_EXPORT Huber : public Base {
│ │ │ │ -
209 protected:
│ │ │ │ -
210 double k_;
│ │ │ │ -
211
│ │ │ │ -
212 public:
│ │ │ │ -
213 typedef boost::shared_ptr<Huber> shared_ptr;
│ │ │ │ -
214
│ │ │ │ -
215 Huber(double k = 1.345, const ReweightScheme reweight = Block);
│ │ │ │ -
216 double weight(double distance) const override;
│ │ │ │ -
217 double loss(double distance) const override;
│ │ │ │ -
218 void print(const std::string &s) const override;
│ │ │ │ -
219 bool equals(const Base &expected, double tol = 1e-8) const override;
│ │ │ │ -
220 static shared_ptr Create(double k, const ReweightScheme reweight = Block);
│ │ │ │ -
221 double modelParameter() const { return k_; }
│ │ │ │ +
114 JacobianFactor(const JacobianFactor& jf) : Base(jf), Ab_(jf.Ab_), model_(jf.model_) {}
│ │ │ │ +
115
│ │ │ │ +
117 explicit JacobianFactor(const HessianFactor& hf);
│ │ │ │ +
118
│ │ │ │ +
120 JacobianFactor();
│ │ │ │ +
121
│ │ │ │ +
123 explicit JacobianFactor(const Vector& b_in);
│ │ │ │ +
124
│ │ │ │ +
126 JacobianFactor(Key i1, const Matrix& A1,
│ │ │ │ +
127 const Vector& b, const SharedDiagonal& model = SharedDiagonal());
│ │ │ │ +
128
│ │ │ │ +
130 JacobianFactor(Key i1, const Matrix& A1,
│ │ │ │ +
131 Key i2, const Matrix& A2,
│ │ │ │ +
132 const Vector& b, const SharedDiagonal& model = SharedDiagonal());
│ │ │ │ +
133
│ │ │ │ +
135 JacobianFactor(Key i1, const Matrix& A1, Key i2,
│ │ │ │ +
136 const Matrix& A2, Key i3, const Matrix& A3,
│ │ │ │ +
137 const Vector& b, const SharedDiagonal& model = SharedDiagonal());
│ │ │ │ +
138
│ │ │ │ +
142 template<typename TERMS>
│ │ │ │ +
143 JacobianFactor(const TERMS& terms, const Vector& b, const SharedDiagonal& model = SharedDiagonal());
│ │ │ │ +
144
│ │ │ │ +
149 template<typename KEYS>
│ │ │ │ +
150 JacobianFactor(
│ │ │ │ +
151 const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal());
│ │ │ │ +
152
│ │ │ │ +
157 explicit JacobianFactor(
│ │ │ │ +
158 const GaussianFactorGraph& graph);
│ │ │ │ +
159
│ │ │ │ +
164 explicit JacobianFactor(
│ │ │ │ +
165 const GaussianFactorGraph& graph,
│ │ │ │ +
166 const VariableSlots& p_variableSlots);
│ │ │ │ +
167
│ │ │ │ +
172 explicit JacobianFactor(
│ │ │ │ +
173 const GaussianFactorGraph& graph,
│ │ │ │ +
174 const Ordering& ordering);
│ │ │ │ +
175
│ │ │ │ +
180 explicit JacobianFactor(
│ │ │ │ +
181 const GaussianFactorGraph& graph,
│ │ │ │ +
182 const Ordering& ordering,
│ │ │ │ +
183 const VariableSlots& p_variableSlots);
│ │ │ │ +
184
│ │ │ │ +
186 ~JacobianFactor() override {}
│ │ │ │ +
187
│ │ │ │ +
│ │ │ │ +
189 GaussianFactor::shared_ptr clone() const override {
│ │ │ │ +
190 return boost::static_pointer_cast<GaussianFactor>(
│ │ │ │ +
191 boost::make_shared<JacobianFactor>(*this));
│ │ │ │ +
192 }
│ │ │ │ +
│ │ │ │ +
193
│ │ │ │ +
194 // Implementing Testable interface
│ │ │ │ +
195 void print(const std::string& s = "",
│ │ │ │ +
196 const KeyFormatter& formatter = DefaultKeyFormatter) const override;
│ │ │ │ +
197 bool equals(const GaussianFactor& lf, double tol = 1e-9) const override;
│ │ │ │ +
198
│ │ │ │ +
199 Vector unweighted_error(const VectorValues& c) const;
│ │ │ │ +
200 Vector error_vector(const VectorValues& c) const;
│ │ │ │ +
203 using GaussianFactor::error;
│ │ │ │ +
204
│ │ │ │ +
206 double error(const VectorValues& c) const override;
│ │ │ │ +
207
│ │ │ │ +
216 Matrix augmentedInformation() const override;
│ │ │ │ +
217
│ │ │ │ +
221 Matrix information() const override;
│ │ │ │
222
│ │ │ │ -
223 private:
│ │ │ │ -
225 friend class boost::serialization::access;
│ │ │ │ -
226 template <class ARCHIVE>
│ │ │ │ -
227 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ -
228 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
229 ar &BOOST_SERIALIZATION_NVP(k_);
│ │ │ │ -
230 }
│ │ │ │ -
231};
│ │ │ │ -
│ │ │ │ -
232
│ │ │ │ -
│ │ │ │ -
246class GTSAM_EXPORT Cauchy : public Base {
│ │ │ │ -
247 protected:
│ │ │ │ -
248 double k_, ksquared_;
│ │ │ │ +
224 using Base::hessianDiagonal;
│ │ │ │ +
225
│ │ │ │ +
227 void hessianDiagonalAdd(VectorValues& d) const override;
│ │ │ │ +
228
│ │ │ │ +
230 void hessianDiagonal(double* d) const override;
│ │ │ │ +
231
│ │ │ │ +
233 std::map<Key,Matrix> hessianBlockDiagonal() const override;
│ │ │ │ +
234
│ │ │ │ +
238 std::pair<Matrix, Vector> jacobian() const override;
│ │ │ │ +
239
│ │ │ │ +
243 std::pair<Matrix, Vector> jacobianUnweighted() const;
│ │ │ │ +
244
│ │ │ │ +
248 Matrix augmentedJacobian() const override;
│ │ │ │
249
│ │ │ │ -
250 public:
│ │ │ │ -
251 typedef boost::shared_ptr<Cauchy> shared_ptr;
│ │ │ │ -
252
│ │ │ │ -
253 Cauchy(double k = 0.1, const ReweightScheme reweight = Block);
│ │ │ │ -
254 double weight(double distance) const override;
│ │ │ │ -
255 double loss(double distance) const override;
│ │ │ │ -
256 void print(const std::string &s) const override;
│ │ │ │ -
257 bool equals(const Base &expected, double tol = 1e-8) const override;
│ │ │ │ -
258 static shared_ptr Create(double k, const ReweightScheme reweight = Block);
│ │ │ │ -
259 double modelParameter() const { return k_; }
│ │ │ │ +
253 Matrix augmentedJacobianUnweighted() const;
│ │ │ │ +
254
│ │ │ │ +
256 const VerticalBlockMatrix& matrixObject() const { return Ab_; }
│ │ │ │ +
257
│ │ │ │ +
259 VerticalBlockMatrix& matrixObject() { return Ab_; }
│ │ │ │
260
│ │ │ │ -
261 private:
│ │ │ │ -
263 friend class boost::serialization::access;
│ │ │ │ -
264 template <class ARCHIVE>
│ │ │ │ -
265 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ -
266 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
267 ar &BOOST_SERIALIZATION_NVP(k_);
│ │ │ │ -
268 ar &BOOST_SERIALIZATION_NVP(ksquared_);
│ │ │ │ -
269 }
│ │ │ │ -
270};
│ │ │ │ -
│ │ │ │ -
271
│ │ │ │ -
│ │ │ │ -
280class GTSAM_EXPORT Tukey : public Base {
│ │ │ │ -
281 protected:
│ │ │ │ -
282 double c_, csquared_;
│ │ │ │ -
283
│ │ │ │ -
284 public:
│ │ │ │ -
285 typedef boost::shared_ptr<Tukey> shared_ptr;
│ │ │ │ -
286
│ │ │ │ -
287 Tukey(double c = 4.6851, const ReweightScheme reweight = Block);
│ │ │ │ -
288 double weight(double distance) const override;
│ │ │ │ -
289 double loss(double distance) const override;
│ │ │ │ -
290 void print(const std::string &s) const override;
│ │ │ │ -
291 bool equals(const Base &expected, double tol = 1e-8) const override;
│ │ │ │ -
292 static shared_ptr Create(double k, const ReweightScheme reweight = Block);
│ │ │ │ -
293 double modelParameter() const { return c_; }
│ │ │ │ -
294
│ │ │ │ -
295 private:
│ │ │ │ -
297 friend class boost::serialization::access;
│ │ │ │ -
298 template <class ARCHIVE>
│ │ │ │ -
299 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ -
300 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
301 ar &BOOST_SERIALIZATION_NVP(c_);
│ │ │ │ -
302 }
│ │ │ │ -
303};
│ │ │ │ -
│ │ │ │ +
266 GaussianFactor::shared_ptr negate() const override;
│ │ │ │ +
267
│ │ │ │ +
│ │ │ │ +
269 bool isConstrained() const {
│ │ │ │ +
270 return model_ && model_->isConstrained();
│ │ │ │ +
271 }
│ │ │ │ +
│ │ │ │ +
272
│ │ │ │ +
│ │ │ │ +
276 DenseIndex getDim(const_iterator variable) const override {
│ │ │ │ +
277 return Ab_(variable - begin()).cols();
│ │ │ │ +
278 }
│ │ │ │ +
│ │ │ │ +
279
│ │ │ │ +
283 size_t rows() const { return Ab_.rows(); }
│ │ │ │ +
284
│ │ │ │ +
288 size_t cols() const { return Ab_.cols(); }
│ │ │ │ +
289
│ │ │ │ +
291 const SharedDiagonal& get_model() const { return model_; }
│ │ │ │ +
292
│ │ │ │ +
294 SharedDiagonal& get_model() { return model_; }
│ │ │ │ +
295
│ │ │ │ +
297 const constBVector getb() const { return Ab_(size()).col(0); }
│ │ │ │ +
298
│ │ │ │ +
300 constABlock getA(const_iterator variable) const { return Ab_(variable - begin()); }
│ │ │ │ +
301
│ │ │ │ +
303 constABlock getA() const { return Ab_.range(0, size()); }
│ │ │ │
304
│ │ │ │ -
│ │ │ │ -
313class GTSAM_EXPORT Welsch : public Base {
│ │ │ │ -
314 protected:
│ │ │ │ -
315 double c_, csquared_;
│ │ │ │ -
316
│ │ │ │ -
317 public:
│ │ │ │ -
318 typedef boost::shared_ptr<Welsch> shared_ptr;
│ │ │ │ -
319
│ │ │ │ -
320 Welsch(double c = 2.9846, const ReweightScheme reweight = Block);
│ │ │ │ -
321 double weight(double distance) const override;
│ │ │ │ -
322 double loss(double distance) const override;
│ │ │ │ -
323 void print(const std::string &s) const override;
│ │ │ │ -
324 bool equals(const Base &expected, double tol = 1e-8) const override;
│ │ │ │ -
325 static shared_ptr Create(double k, const ReweightScheme reweight = Block);
│ │ │ │ -
326 double modelParameter() const { return c_; }
│ │ │ │ -
327
│ │ │ │ -
328 private:
│ │ │ │ -
330 friend class boost::serialization::access;
│ │ │ │ -
331 template <class ARCHIVE>
│ │ │ │ -
332 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ -
333 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
334 ar &BOOST_SERIALIZATION_NVP(c_);
│ │ │ │ -
335 ar &BOOST_SERIALIZATION_NVP(csquared_);
│ │ │ │ -
336 }
│ │ │ │ -
337};
│ │ │ │ -
│ │ │ │ -
338
│ │ │ │ -
│ │ │ │ -
349class GTSAM_EXPORT GemanMcClure : public Base {
│ │ │ │ -
350 public:
│ │ │ │ -
351 typedef boost::shared_ptr<GemanMcClure> shared_ptr;
│ │ │ │ +
306 BVector getb() { return Ab_(size()).col(0); }
│ │ │ │ +
307
│ │ │ │ +
309 ABlock getA(iterator variable) { return Ab_(variable - begin()); }
│ │ │ │ +
310
│ │ │ │ +
312 ABlock getA() { return Ab_.range(0, size()); }
│ │ │ │ +
313
│ │ │ │ +
319 void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const override;
│ │ │ │ +
320
│ │ │ │ +
322 Vector operator*(const VectorValues& x) const;
│ │ │ │ +
323
│ │ │ │ +
326 void transposeMultiplyAdd(double alpha, const Vector& e,
│ │ │ │ +
327 VectorValues& x) const;
│ │ │ │ +
328
│ │ │ │ +
330 void multiplyHessianAdd(double alpha, const VectorValues& x,
│ │ │ │ +
331 VectorValues& y) const override;
│ │ │ │ +
332
│ │ │ │ +
341 void multiplyHessianAdd(double alpha, const double* x, double* y,
│ │ │ │ +
342 const std::vector<size_t>& accumulatedDims) const;
│ │ │ │ +
343
│ │ │ │ +
345 VectorValues gradientAtZero() const override;
│ │ │ │ +
346
│ │ │ │ +
348 void gradientAtZero(double* d) const override;
│ │ │ │ +
349
│ │ │ │ +
351 Vector gradient(Key key, const VectorValues& x) const override;
│ │ │ │
352
│ │ │ │ -
353 GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block);
│ │ │ │ -
354 ~GemanMcClure() override {}
│ │ │ │ -
355 double weight(double distance) const override;
│ │ │ │ -
356 double loss(double distance) const override;
│ │ │ │ -
357 void print(const std::string &s) const override;
│ │ │ │ -
358 bool equals(const Base &expected, double tol = 1e-8) const override;
│ │ │ │ -
359 static shared_ptr Create(double k, const ReweightScheme reweight = Block);
│ │ │ │ -
360 double modelParameter() const { return c_; }
│ │ │ │ -
361
│ │ │ │ -
362 protected:
│ │ │ │ -
363 double c_;
│ │ │ │ -
364
│ │ │ │ -
365 private:
│ │ │ │ -
367 friend class boost::serialization::access;
│ │ │ │ -
368 template <class ARCHIVE>
│ │ │ │ -
369 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ -
370 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
371 ar &BOOST_SERIALIZATION_NVP(c_);
│ │ │ │ -
372 }
│ │ │ │ -
373};
│ │ │ │ -
│ │ │ │ -
374
│ │ │ │ -
│ │ │ │ -
387class GTSAM_EXPORT DCS : public Base {
│ │ │ │ -
388 public:
│ │ │ │ -
389 typedef boost::shared_ptr<DCS> shared_ptr;
│ │ │ │ -
390
│ │ │ │ -
391 DCS(double c = 1.0, const ReweightScheme reweight = Block);
│ │ │ │ -
392 ~DCS() override {}
│ │ │ │ -
393 double weight(double distance) const override;
│ │ │ │ -
394 double loss(double distance) const override;
│ │ │ │ -
395 void print(const std::string &s) const override;
│ │ │ │ -
396 bool equals(const Base &expected, double tol = 1e-8) const override;
│ │ │ │ -
397 static shared_ptr Create(double k, const ReweightScheme reweight = Block);
│ │ │ │ -
398 double modelParameter() const { return c_; }
│ │ │ │ -
399
│ │ │ │ -
400 protected:
│ │ │ │ -
401 double c_;
│ │ │ │ -
402
│ │ │ │ -
403 private:
│ │ │ │ -
405 friend class boost::serialization::access;
│ │ │ │ -
406 template <class ARCHIVE>
│ │ │ │ -
407 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ -
408 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
409 ar &BOOST_SERIALIZATION_NVP(c_);
│ │ │ │ -
410 }
│ │ │ │ -
411};
│ │ │ │ -
│ │ │ │ -
412
│ │ │ │ -
│ │ │ │ -
426class GTSAM_EXPORT L2WithDeadZone : public Base {
│ │ │ │ -
427 protected:
│ │ │ │ -
428 double k_;
│ │ │ │ -
429
│ │ │ │ -
430 public:
│ │ │ │ -
431 typedef boost::shared_ptr<L2WithDeadZone> shared_ptr;
│ │ │ │ -
432
│ │ │ │ -
433 L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block);
│ │ │ │ -
434 double weight(double distance) const override;
│ │ │ │ -
435 double loss(double distance) const override;
│ │ │ │ -
436 void print(const std::string &s) const override;
│ │ │ │ -
437 bool equals(const Base &expected, double tol = 1e-8) const override;
│ │ │ │ -
438 static shared_ptr Create(double k, const ReweightScheme reweight = Block);
│ │ │ │ -
439 double modelParameter() const { return k_; }
│ │ │ │ -
440
│ │ │ │ -
441 private:
│ │ │ │ -
443 friend class boost::serialization::access;
│ │ │ │ -
444 template <class ARCHIVE>
│ │ │ │ -
445 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ -
446 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
447 ar &BOOST_SERIALIZATION_NVP(k_);
│ │ │ │ -
448 }
│ │ │ │ -
449};
│ │ │ │ -
│ │ │ │ -
450
│ │ │ │ -
451} // namespace mEstimator
│ │ │ │ -
452} // namespace noiseModel
│ │ │ │ -
453} // namespace gtsam
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ -
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
354 JacobianFactor whiten() const;
│ │ │ │ +
355
│ │ │ │ +
357 std::pair<boost::shared_ptr<GaussianConditional>, shared_ptr>
│ │ │ │ +
358 eliminate(const Ordering& keys);
│ │ │ │ +
359
│ │ │ │ +
361 void setModel(bool anyConstrained, const Vector& sigmas);
│ │ │ │ +
362
│ │ │ │ +
374 friend GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, shared_ptr>
│ │ │ │ +
375 EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys);
│ │ │ │ +
376
│ │ │ │ +
384 boost::shared_ptr<GaussianConditional> splitConditional(size_t nrFrontals);
│ │ │ │ +
385
│ │ │ │ +
386 protected:
│ │ │ │ +
387
│ │ │ │ +
389 template<typename TERMS>
│ │ │ │ +
390 void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel);
│ │ │ │ +
391
│ │ │ │ +
392 private:
│ │ │ │ +
393
│ │ │ │ +
398 void JacobianFactorHelper(
│ │ │ │ +
399 const GaussianFactorGraph& graph,
│ │ │ │ +
400 const FastVector<VariableSlots::const_iterator>& orderedSlots);
│ │ │ │ +
401
│ │ │ │ +
408 template<class KEYS, class DIMENSIONS>
│ │ │ │ +
409 JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m,
│ │ │ │ +
410 const SharedDiagonal& model = SharedDiagonal()) :
│ │ │ │ +
411 Base(keys), Ab_(dims.begin(), dims.end(), m, true), model_(model) {
│ │ │ │ +
412 }
│ │ │ │ +
413
│ │ │ │ +
414 // be very selective on who can access these private methods:
│ │ │ │ +
415 template<typename T> friend class ExpressionFactor;
│ │ │ │ +
416
│ │ │ │ +
418 friend class boost::serialization::access;
│ │ │ │ +
419 template<class ARCHIVE>
│ │ │ │ +
420 void save(ARCHIVE & ar, const unsigned int version) const {
│ │ │ │ +
421 // TODO(fan): This is a hack for Boost < 1.66
│ │ │ │ +
422 // We really need to introduce proper versioning in the archives
│ │ │ │ +
423 // As otherwise this will not read objects serialized by older
│ │ │ │ +
424 // versions of GTSAM
│ │ │ │ +
425 ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
426 ar << BOOST_SERIALIZATION_NVP(Ab_);
│ │ │ │ +
427 bool model_null = false;
│ │ │ │ +
428 if(model_.get() == nullptr) {
│ │ │ │ +
429 model_null = true;
│ │ │ │ +
430 ar << boost::serialization::make_nvp("model_null", model_null);
│ │ │ │ +
431 } else {
│ │ │ │ +
432 ar << boost::serialization::make_nvp("model_null", model_null);
│ │ │ │ +
433 ar << BOOST_SERIALIZATION_NVP(model_);
│ │ │ │ +
434 }
│ │ │ │ +
435 }
│ │ │ │ +
436
│ │ │ │ +
437 template<class ARCHIVE>
│ │ │ │ +
438 void load(ARCHIVE & ar, const unsigned int version) {
│ │ │ │ +
439 // invoke serialization of the base class
│ │ │ │ +
440 ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
441 ar >> BOOST_SERIALIZATION_NVP(Ab_);
│ │ │ │ +
442 if (version < 1) {
│ │ │ │ +
443 ar >> BOOST_SERIALIZATION_NVP(model_);
│ │ │ │ +
444 } else {
│ │ │ │ +
445 bool model_null;
│ │ │ │ +
446 ar >> BOOST_SERIALIZATION_NVP(model_null);
│ │ │ │ +
447 if (!model_null) {
│ │ │ │ +
448 ar >> BOOST_SERIALIZATION_NVP(model_);
│ │ │ │ +
449 }
│ │ │ │ +
450 }
│ │ │ │ +
451 }
│ │ │ │ +
452
│ │ │ │ +
453 BOOST_SERIALIZATION_SPLIT_MEMBER()
│ │ │ │ +
454 }; // JacobianFactor
│ │ │ │ +
│ │ │ │ +
456template<>
│ │ │ │ +
│ │ │ │ +
457struct traits<JacobianFactor> : public Testable<JacobianFactor> {
│ │ │ │ +
458};
│ │ │ │ +
│ │ │ │ +
459
│ │ │ │ +
460} // \ namespace gtsam
│ │ │ │ +
461
│ │ │ │ +
462BOOST_CLASS_VERSION(gtsam::JacobianFactor, 1)
│ │ │ │ +
463
│ │ │ │ +
464#include <gtsam/linear/JacobianFactor-inl.h>
│ │ │ │ +
465
│ │ │ │ +
466
│ │ │ │ +
VerticalBlockMatrix.h
A matrix with column blocks of pre-defined sizes.
│ │ │ │ +
VariableSlots.h
VariableSlots describes the structure of a combined factor in terms of where each block comes from in...
│ │ │ │ +
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
│ │ │ │ + │ │ │ │ +
global_includes.h
Included from all GTSAM files.
│ │ │ │ +
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │ +
gtsam::save
void save(const Matrix &A, const string &s, const string &filename)
save a matrix to file, which can be loaded by matlab
Definition Matrix.cpp:167
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
│ │ │ │ +
gtsam::EliminateQR
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Multiply all factors and eliminate the given keys from the resulting factor using a QR variant that h...
Definition JacobianFactor.cpp:789
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
│ │ │ │
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::noiseModel::mEstimator::Base
Pure virtual class for all robust error function classes.
Definition LossFunctions.h:63
│ │ │ │ -
gtsam::noiseModel::mEstimator::Base::loss
virtual double loss(double distance) const
This method is responsible for returning the total penalty for a given amount of error.
Definition LossFunctions.h:94
│ │ │ │ -
gtsam::noiseModel::mEstimator::Base::reweight_
ReweightScheme reweight_
Strategy for reweighting.
Definition LossFunctions.h:72
│ │ │ │ -
gtsam::noiseModel::mEstimator::Base::reweightScheme
ReweightScheme reweightScheme() const
Returns the reweight scheme, as explained in ReweightScheme.
Definition LossFunctions.h:79
│ │ │ │ -
gtsam::noiseModel::mEstimator::Base::weight
virtual double weight(double distance) const =0
This method is responsible for returning the weight function for a given amount of error.
│ │ │ │ -
gtsam::noiseModel::mEstimator::Base::ReweightScheme
ReweightScheme
the rows can be weighted independently according to the error or uniformly with the norm of the right...
Definition LossFunctions.h:67
│ │ │ │ -
gtsam::noiseModel::mEstimator::Null
"Null" robust loss function, equivalent to a Gaussian pdf noise model, or plain least-squares (non-ro...
Definition LossFunctions.h:146
│ │ │ │ -
gtsam::noiseModel::mEstimator::Null::weight
double weight(double) const override
This method is responsible for returning the weight function for a given amount of error.
Definition LossFunctions.h:152
│ │ │ │ -
gtsam::noiseModel::mEstimator::Null::loss
double loss(double distance) const override
This method is responsible for returning the total penalty for a given amount of error.
Definition LossFunctions.h:153
│ │ │ │ -
gtsam::noiseModel::mEstimator::Fair
Implementation of the "Fair" robust error model (Zhang97ivc)
Definition LossFunctions.h:175
│ │ │ │ -
gtsam::noiseModel::mEstimator::Huber
The "Huber" robust error model (Zhang97ivc).
Definition LossFunctions.h:208
│ │ │ │ -
gtsam::noiseModel::mEstimator::Cauchy
Implementation of the "Cauchy" robust error model (Lee2013IROS).
Definition LossFunctions.h:246
│ │ │ │ -
gtsam::noiseModel::mEstimator::Tukey
Implementation of the "Tukey" robust error model (Zhang97ivc).
Definition LossFunctions.h:280
│ │ │ │ -
gtsam::noiseModel::mEstimator::Welsch
Implementation of the "Welsch" robust error model (Zhang97ivc).
Definition LossFunctions.h:313
│ │ │ │ -
gtsam::noiseModel::mEstimator::GemanMcClure
Implementation of the "Geman-McClure" robust error model (Zhang97ivc).
Definition LossFunctions.h:349
│ │ │ │ -
gtsam::noiseModel::mEstimator::DCS
DCS implements the Dynamic Covariance Scaling robust error model from the paper Robust Map Optimizati...
Definition LossFunctions.h:387
│ │ │ │ -
gtsam::noiseModel::mEstimator::L2WithDeadZone
L2WithDeadZone implements a standard L2 penalty, but with a dead zone of width 2*k,...
Definition LossFunctions.h:426
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::VerticalBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43
│ │ │ │ +
gtsam::VerticalBlockMatrix::range
Block range(DenseIndex startBlock, DenseIndex endBlock)
access ranges of blocks at a time
Definition VerticalBlockMatrix.h:130
│ │ │ │ +
gtsam::VerticalBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition VerticalBlockMatrix.h:115
│ │ │ │ +
gtsam::VerticalBlockMatrix::cols
DenseIndex cols() const
Column size.
Definition VerticalBlockMatrix.h:118
│ │ │ │ +
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition Factor.h:80
│ │ │ │ +
gtsam::Factor::iterator
KeyVector::iterator iterator
Iterator over keys.
Definition Factor.h:77
│ │ │ │ +
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
gtsam::VariableSlots
A combined factor is assembled as one block of rows for each component factor.
Definition VariableSlots.h:52
│ │ │ │ +
gtsam::GaussianFactor
An abstract virtual base class for JacobianFactor and HessianFactor.
Definition GaussianFactor.h:39
│ │ │ │ +
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactor.h:42
│ │ │ │ +
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ +
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition HessianFactor.h:101
│ │ │ │ +
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition JacobianFactor.h:91
│ │ │ │ +
gtsam::JacobianFactor::getb
const constBVector getb() const
Get a view of the r.h.s.
Definition JacobianFactor.h:297
│ │ │ │ +
gtsam::JacobianFactor::getb
BVector getb()
Get a view of the r.h.s.
Definition JacobianFactor.h:306
│ │ │ │ +
gtsam::JacobianFactor::get_model
const SharedDiagonal & get_model() const
get a copy of model
Definition JacobianFactor.h:291
│ │ │ │ +
gtsam::JacobianFactor::JacobianFactor
JacobianFactor(const JacobianFactor &jf)
Copy constructor.
Definition JacobianFactor.h:114
│ │ │ │ +
gtsam::JacobianFactor::This
JacobianFactor This
Typedef to this class.
Definition JacobianFactor.h:94
│ │ │ │ +
gtsam::JacobianFactor::isConstrained
bool isConstrained() const
is noise model constrained ?
Definition JacobianFactor.h:269
│ │ │ │ +
gtsam::JacobianFactor::getA
constABlock getA() const
Get a view of the A matrix, not weighted by noise.
Definition JacobianFactor.h:303
│ │ │ │ +
gtsam::JacobianFactor::clone
GaussianFactor::shared_ptr clone() const override
Clone this JacobianFactor.
Definition JacobianFactor.h:189
│ │ │ │ +
gtsam::JacobianFactor::getA
ABlock getA(iterator variable)
Get a view of the A matrix for the variable pointed to by the given key iterator (non-const version)
Definition JacobianFactor.h:309
│ │ │ │ +
gtsam::JacobianFactor::get_model
SharedDiagonal & get_model()
get a copy of model (non-const version)
Definition JacobianFactor.h:294
│ │ │ │ +
gtsam::JacobianFactor::getDim
DenseIndex getDim(const_iterator variable) const override
Return the dimension of the variable pointed to by the given key iterator todo: Remove this in favor ...
Definition JacobianFactor.h:276
│ │ │ │ +
gtsam::JacobianFactor::matrixObject
VerticalBlockMatrix & matrixObject()
Mutable access to the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object.
Definition JacobianFactor.h:259
│ │ │ │ +
gtsam::JacobianFactor::getA
ABlock getA()
Get a view of the A matrix.
Definition JacobianFactor.h:312
│ │ │ │ +
gtsam::JacobianFactor::Base
GaussianFactor Base
Typedef to base class.
Definition JacobianFactor.h:95
│ │ │ │ +
gtsam::JacobianFactor::~JacobianFactor
~JacobianFactor() override
Virtual destructor.
Definition JacobianFactor.h:186
│ │ │ │ +
gtsam::JacobianFactor::matrixObject
const VerticalBlockMatrix & matrixObject() const
Return the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object.
Definition JacobianFactor.h:256
│ │ │ │ +
gtsam::JacobianFactor::rows
size_t rows() const
return the number of rows in the corresponding linear system
Definition JacobianFactor.h:283
│ │ │ │ +
gtsam::JacobianFactor::cols
size_t cols() const
return the number of columns in the corresponding linear system
Definition JacobianFactor.h:288
│ │ │ │ +
gtsam::JacobianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition JacobianFactor.h:96
│ │ │ │ +
gtsam::JacobianFactor::getA
constABlock getA(const_iterator variable) const
Get a view of the A matrix for the variable pointed to by the given key iterator.
Definition JacobianFactor.h:300
│ │ │ │ +
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
JacobianFactor
In Gaussian factors, the error function returns either the negative log-likelihood,...
│ │ │ │ +
GaussianConditional
is the normalization constant.
│ │ │ │ +
VectorValues
The Factor::error simply extracts the.
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,385 +1,502 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -LossFunctions.h │ │ │ │ │ -1 │ │ │ │ │ -2/* --------------------------------------------------------------------------- │ │ │ │ │ +JacobianFactor.h │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ -3 │ │ │ │ │ -4 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ -5 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -6 * All Rights Reserved │ │ │ │ │ -7 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ -8 │ │ │ │ │ -9 * See LICENSE for the license information │ │ │ │ │ -10 │ │ │ │ │ -11 * ------------------------------------------------------------------------- │ │ │ │ │ +2 │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +5 * All Rights Reserved │ │ │ │ │ +6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +7 │ │ │ │ │ +8 * See LICENSE for the license information │ │ │ │ │ +9 │ │ │ │ │ +10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ -12 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ -24#include │ │ │ │ │ -25 │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ +11 │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_N_o_i_s_e_M_o_d_e_l_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_V_a_r_i_a_b_l_e_S_l_o_t_s_._h> │ │ │ │ │ +26 │ │ │ │ │ +27#include │ │ │ │ │ 28#include │ │ │ │ │ -29#include │ │ │ │ │ -30#include │ │ │ │ │ -31#include │ │ │ │ │ +29#include │ │ │ │ │ +30 │ │ │ │ │ +31namespace _g_t_s_a_m { │ │ │ │ │ 32 │ │ │ │ │ -33namespace _g_t_s_a_m { │ │ │ │ │ -34namespace noiseModel { │ │ │ │ │ -35// clang-format off │ │ │ │ │ -54// clang-format on │ │ │ │ │ -55namespace mEstimator { │ │ │ │ │ -56 │ │ │ │ │ -_6_3class GTSAM_EXPORT _B_a_s_e { │ │ │ │ │ -64 public: │ │ │ │ │ -_6_7 enum _R_e_w_e_i_g_h_t_S_c_h_e_m_e { Scalar, Block }; │ │ │ │ │ -68 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -69 │ │ │ │ │ -70 protected: │ │ │ │ │ -_7_2 _R_e_w_e_i_g_h_t_S_c_h_e_m_e _r_e_w_e_i_g_h_t__; │ │ │ │ │ -73 │ │ │ │ │ -74 public: │ │ │ │ │ -75 _B_a_s_e(const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block) : reweight_(reweight) {} │ │ │ │ │ -76 virtual _~_B_a_s_e() {} │ │ │ │ │ -77 │ │ │ │ │ -_7_9 _R_e_w_e_i_g_h_t_S_c_h_e_m_e _r_e_w_e_i_g_h_t_S_c_h_e_m_e() const { return reweight_; } │ │ │ │ │ -80 │ │ │ │ │ -_9_4 virtual double _l_o_s_s(double distance) const { return 0; } │ │ │ │ │ -95 │ │ │ │ │ -_1_0_6 virtual double _w_e_i_g_h_t(double distance) const = 0; │ │ │ │ │ +33 // Forward declarations │ │ │ │ │ +34 class HessianFactor; │ │ │ │ │ +35 class VariableSlots; │ │ │ │ │ +36 class GaussianFactorGraph; │ │ │ │ │ +37 class _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l; │ │ │ │ │ +38 class HessianFactor; │ │ │ │ │ +39 class _V_e_c_t_o_r_V_a_l_u_e_s; │ │ │ │ │ +40 class Ordering; │ │ │ │ │ +41 class _J_a_c_o_b_i_a_n_F_a_c_t_o_r; │ │ │ │ │ +42 │ │ │ │ │ +48 GTSAM_EXPORT std::pair, boost:: │ │ │ │ │ +shared_ptr > │ │ │ │ │ +49 _E_l_i_m_i_n_a_t_e_Q_R(const GaussianFactorGraph& factors, const Ordering& keys); │ │ │ │ │ +50 │ │ │ │ │ +_9_0 class GTSAM_EXPORT _J_a_c_o_b_i_a_n_F_a_c_t_o_r : public _G_a_u_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +91 { │ │ │ │ │ +92 public: │ │ │ │ │ +93 │ │ │ │ │ +_9_4 typedef _J_a_c_o_b_i_a_n_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ +_9_5 typedef _G_a_u_s_s_i_a_n_F_a_c_t_o_r _B_a_s_e; │ │ │ │ │ +_9_6 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +97 │ │ │ │ │ +98 typedef VerticalBlockMatrix::Block ABlock; │ │ │ │ │ +99 typedef VerticalBlockMatrix::constBlock constABlock; │ │ │ │ │ +100 typedef ABlock::ColXpr BVector; │ │ │ │ │ +101 typedef constABlock::ConstColXpr constBVector; │ │ │ │ │ +102 │ │ │ │ │ +103 protected: │ │ │ │ │ +104 │ │ │ │ │ +105 _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x Ab_; // the block view of the full matrix │ │ │ │ │ +106 noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with │ │ │ │ │ +diagonal covariance matrix │ │ │ │ │ 107 │ │ │ │ │ -108 virtual void _p_r_i_n_t(const std::string &s) const = 0; │ │ │ │ │ -109 virtual bool _e_q_u_a_l_s(const _B_a_s_e &expected, double tol = 1e-8) const = 0; │ │ │ │ │ -110 │ │ │ │ │ -111 double sqrtWeight(double distance) const { return std::sqrt(weight │ │ │ │ │ -(distance)); } │ │ │ │ │ +108 public: │ │ │ │ │ +109 │ │ │ │ │ +111 explicit _J_a_c_o_b_i_a_n_F_a_c_t_o_r(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r& gf); │ │ │ │ │ 112 │ │ │ │ │ -115 Vector weight(const Vector &error) const; │ │ │ │ │ -116 │ │ │ │ │ -118 Vector sqrtWeight(const Vector &error) const; │ │ │ │ │ -119 │ │ │ │ │ -122 void reweight(Vector &error) const; │ │ │ │ │ -123 void reweight(std::vector &A, Vector &error) const; │ │ │ │ │ -124 void reweight(Matrix &A, Vector &error) const; │ │ │ │ │ -125 void reweight(Matrix &A1, Matrix &A2, Vector &error) const; │ │ │ │ │ -126 void reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const; │ │ │ │ │ -127 │ │ │ │ │ -128 private: │ │ │ │ │ -_1_3_0 friend class boost::serialization::access; │ │ │ │ │ -131 template │ │ │ │ │ -132 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ -133 ar &BOOST_SERIALIZATION_NVP(reweight_); │ │ │ │ │ -134 } │ │ │ │ │ -135}; │ │ │ │ │ -136 │ │ │ │ │ -_1_4_6class GTSAM_EXPORT _N_u_l_l : public _B_a_s_e { │ │ │ │ │ -147 public: │ │ │ │ │ -148 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -149 │ │ │ │ │ -150 _N_u_l_l(const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block) : _B_a_s_e(reweight) {} │ │ │ │ │ -151 _~_N_u_l_l() override {} │ │ │ │ │ -_1_5_2 double _w_e_i_g_h_t(double /*error*/) const override { return 1.0; } │ │ │ │ │ -_1_5_3 double _l_o_s_s(double distance) const override { return 0.5 * distance * │ │ │ │ │ -distance; } │ │ │ │ │ -154 void _p_r_i_n_t(const std::string &s) const override; │ │ │ │ │ -155 bool _e_q_u_a_l_s(const _B_a_s_e & /*expected*/, double /*tol*/) const override │ │ │ │ │ -{ return true; } │ │ │ │ │ -156 static shared_ptr Create(); │ │ │ │ │ -157 │ │ │ │ │ -158 private: │ │ │ │ │ -_1_6_0 friend class boost::serialization::access; │ │ │ │ │ -161 template │ │ │ │ │ -162 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ -163 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -164 } │ │ │ │ │ -165}; │ │ │ │ │ -166 │ │ │ │ │ -_1_7_5class GTSAM_EXPORT _F_a_i_r : public _B_a_s_e { │ │ │ │ │ -176 protected: │ │ │ │ │ -177 double c_; │ │ │ │ │ -178 │ │ │ │ │ -179 public: │ │ │ │ │ -180 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -181 │ │ │ │ │ -182 _F_a_i_r(double c = 1.3998, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ -183 double weight(double distance) const override; │ │ │ │ │ -184 double loss(double distance) const override; │ │ │ │ │ -185 void _p_r_i_n_t(const std::string &s) const override; │ │ │ │ │ -186 bool _e_q_u_a_l_s(const _B_a_s_e &expected, double tol = 1e-8) const override; │ │ │ │ │ -187 static shared_ptr Create(double c, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ -188 double modelParameter() const { return c_; } │ │ │ │ │ -189 │ │ │ │ │ -190 private: │ │ │ │ │ -_1_9_2 friend class boost::serialization::access; │ │ │ │ │ -193 template │ │ │ │ │ -194 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ -195 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -196 ar &BOOST_SERIALIZATION_NVP(c_); │ │ │ │ │ -197 } │ │ │ │ │ -198}; │ │ │ │ │ -199 │ │ │ │ │ -_2_0_8class GTSAM_EXPORT _H_u_b_e_r : public _B_a_s_e { │ │ │ │ │ -209 protected: │ │ │ │ │ -210 double k_; │ │ │ │ │ -211 │ │ │ │ │ -212 public: │ │ │ │ │ -213 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -214 │ │ │ │ │ -215 _H_u_b_e_r(double k = 1.345, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ -216 double weight(double distance) const override; │ │ │ │ │ -217 double loss(double distance) const override; │ │ │ │ │ -218 void _p_r_i_n_t(const std::string &s) const override; │ │ │ │ │ -219 bool _e_q_u_a_l_s(const _B_a_s_e &expected, double tol = 1e-8) const override; │ │ │ │ │ -220 static shared_ptr Create(double k, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ -221 double modelParameter() const { return k_; } │ │ │ │ │ +_1_1_4 _J_a_c_o_b_i_a_n_F_a_c_t_o_r(const _J_a_c_o_b_i_a_n_F_a_c_t_o_r& jf) : _B_a_s_e(jf), Ab_(jf.Ab_), model_ │ │ │ │ │ +(jf.model_) {} │ │ │ │ │ +115 │ │ │ │ │ +117 explicit _J_a_c_o_b_i_a_n_F_a_c_t_o_r(const _H_e_s_s_i_a_n_F_a_c_t_o_r& hf); │ │ │ │ │ +118 │ │ │ │ │ +120 _J_a_c_o_b_i_a_n_F_a_c_t_o_r(); │ │ │ │ │ +121 │ │ │ │ │ +123 explicit _J_a_c_o_b_i_a_n_F_a_c_t_o_r(const Vector& b_in); │ │ │ │ │ +124 │ │ │ │ │ +126 _J_a_c_o_b_i_a_n_F_a_c_t_o_r(_K_e_y i1, const Matrix& A1, │ │ │ │ │ +127 const Vector& b, const SharedDiagonal& model = SharedDiagonal()); │ │ │ │ │ +128 │ │ │ │ │ +130 _J_a_c_o_b_i_a_n_F_a_c_t_o_r(_K_e_y i1, const Matrix& A1, │ │ │ │ │ +131 _K_e_y i2, const Matrix& A2, │ │ │ │ │ +132 const Vector& b, const SharedDiagonal& model = SharedDiagonal()); │ │ │ │ │ +133 │ │ │ │ │ +135 _J_a_c_o_b_i_a_n_F_a_c_t_o_r(_K_e_y i1, const Matrix& A1, _K_e_y i2, │ │ │ │ │ +136 const Matrix& A2, _K_e_y i3, const Matrix& A3, │ │ │ │ │ +137 const Vector& b, const SharedDiagonal& model = SharedDiagonal()); │ │ │ │ │ +138 │ │ │ │ │ +142 template │ │ │ │ │ +143 _J_a_c_o_b_i_a_n_F_a_c_t_o_r(const TERMS& terms, const Vector& b, const SharedDiagonal& │ │ │ │ │ +model = SharedDiagonal()); │ │ │ │ │ +144 │ │ │ │ │ +149 template │ │ │ │ │ +150 _J_a_c_o_b_i_a_n_F_a_c_t_o_r( │ │ │ │ │ +151 const KEYS& keys, const _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x& augmentedMatrix, const │ │ │ │ │ +SharedDiagonal& sigmas = SharedDiagonal()); │ │ │ │ │ +152 │ │ │ │ │ +157 explicit _J_a_c_o_b_i_a_n_F_a_c_t_o_r( │ │ │ │ │ +158 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& graph); │ │ │ │ │ +159 │ │ │ │ │ +164 explicit _J_a_c_o_b_i_a_n_F_a_c_t_o_r( │ │ │ │ │ +165 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& graph, │ │ │ │ │ +166 const _V_a_r_i_a_b_l_e_S_l_o_t_s& p_variableSlots); │ │ │ │ │ +167 │ │ │ │ │ +172 explicit _J_a_c_o_b_i_a_n_F_a_c_t_o_r( │ │ │ │ │ +173 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& graph, │ │ │ │ │ +174 const _O_r_d_e_r_i_n_g& ordering); │ │ │ │ │ +175 │ │ │ │ │ +180 explicit _J_a_c_o_b_i_a_n_F_a_c_t_o_r( │ │ │ │ │ +181 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& graph, │ │ │ │ │ +182 const _O_r_d_e_r_i_n_g& ordering, │ │ │ │ │ +183 const _V_a_r_i_a_b_l_e_S_l_o_t_s& p_variableSlots); │ │ │ │ │ +184 │ │ │ │ │ +_1_8_6 _~_J_a_c_o_b_i_a_n_F_a_c_t_o_r() override {} │ │ │ │ │ +187 │ │ │ │ │ +_1_8_9 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r _c_l_o_n_e() const override { │ │ │ │ │ +190 return boost::static_pointer_cast( │ │ │ │ │ +191 boost::make_shared(*this)); │ │ │ │ │ +192 } │ │ │ │ │ +193 │ │ │ │ │ +194 // Implementing Testable interface │ │ │ │ │ +195 void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ +196 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override; │ │ │ │ │ +197 bool _e_q_u_a_l_s(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r& lf, double tol = 1e-9) const override; │ │ │ │ │ +198 │ │ │ │ │ +199 Vector unweighted_error(const _V_e_c_t_o_r_V_a_l_u_e_s& c) const; │ │ │ │ │ +200 Vector error_vector(const _V_e_c_t_o_r_V_a_l_u_e_s& c) const; │ │ │ │ │ +203 using GaussianFactor::error; │ │ │ │ │ +204 │ │ │ │ │ +206 double error(const _V_e_c_t_o_r_V_a_l_u_e_s& c) const override; │ │ │ │ │ +207 │ │ │ │ │ +216 Matrix augmentedInformation() const override; │ │ │ │ │ +217 │ │ │ │ │ +221 Matrix information() const override; │ │ │ │ │ 222 │ │ │ │ │ -223 private: │ │ │ │ │ -_2_2_5 friend class boost::serialization::access; │ │ │ │ │ -226 template │ │ │ │ │ -227 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ -228 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -229 ar &BOOST_SERIALIZATION_NVP(k_); │ │ │ │ │ -230 } │ │ │ │ │ -231}; │ │ │ │ │ -232 │ │ │ │ │ -_2_4_6class GTSAM_EXPORT _C_a_u_c_h_y : public _B_a_s_e { │ │ │ │ │ -247 protected: │ │ │ │ │ -248 double k_, ksquared_; │ │ │ │ │ +224 using Base::hessianDiagonal; │ │ │ │ │ +225 │ │ │ │ │ +227 void hessianDiagonalAdd(_V_e_c_t_o_r_V_a_l_u_e_s& d) const override; │ │ │ │ │ +228 │ │ │ │ │ +230 void hessianDiagonal(double* d) const override; │ │ │ │ │ +231 │ │ │ │ │ +233 std::map hessianBlockDiagonal() const override; │ │ │ │ │ +234 │ │ │ │ │ +238 std::pair jacobian() const override; │ │ │ │ │ +239 │ │ │ │ │ +243 std::pair jacobianUnweighted() const; │ │ │ │ │ +244 │ │ │ │ │ +248 Matrix augmentedJacobian() const override; │ │ │ │ │ 249 │ │ │ │ │ -250 public: │ │ │ │ │ -251 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -252 │ │ │ │ │ -253 _C_a_u_c_h_y(double k = 0.1, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ -254 double weight(double distance) const override; │ │ │ │ │ -255 double loss(double distance) const override; │ │ │ │ │ -256 void _p_r_i_n_t(const std::string &s) const override; │ │ │ │ │ -257 bool _e_q_u_a_l_s(const _B_a_s_e &expected, double tol = 1e-8) const override; │ │ │ │ │ -258 static shared_ptr Create(double k, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ -259 double modelParameter() const { return k_; } │ │ │ │ │ +253 Matrix augmentedJacobianUnweighted() const; │ │ │ │ │ +254 │ │ │ │ │ +_2_5_6 const _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x& _m_a_t_r_i_x_O_b_j_e_c_t() const { return Ab_; } │ │ │ │ │ +257 │ │ │ │ │ +_2_5_9 _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x& _m_a_t_r_i_x_O_b_j_e_c_t() { return Ab_; } │ │ │ │ │ 260 │ │ │ │ │ -261 private: │ │ │ │ │ -_2_6_3 friend class boost::serialization::access; │ │ │ │ │ -264 template │ │ │ │ │ -265 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ -266 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -267 ar &BOOST_SERIALIZATION_NVP(k_); │ │ │ │ │ -268 ar &BOOST_SERIALIZATION_NVP(ksquared_); │ │ │ │ │ -269 } │ │ │ │ │ -270}; │ │ │ │ │ -271 │ │ │ │ │ -_2_8_0class GTSAM_EXPORT _T_u_k_e_y : public _B_a_s_e { │ │ │ │ │ -281 protected: │ │ │ │ │ -282 double c_, csquared_; │ │ │ │ │ -283 │ │ │ │ │ -284 public: │ │ │ │ │ -285 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -286 │ │ │ │ │ -287 _T_u_k_e_y(double c = 4.6851, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ -288 double weight(double distance) const override; │ │ │ │ │ -289 double loss(double distance) const override; │ │ │ │ │ -290 void _p_r_i_n_t(const std::string &s) const override; │ │ │ │ │ -291 bool _e_q_u_a_l_s(const _B_a_s_e &expected, double tol = 1e-8) const override; │ │ │ │ │ -292 static shared_ptr Create(double k, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ -293 double modelParameter() const { return c_; } │ │ │ │ │ -294 │ │ │ │ │ -295 private: │ │ │ │ │ -_2_9_7 friend class boost::serialization::access; │ │ │ │ │ -298 template │ │ │ │ │ -299 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ -300 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -301 ar &BOOST_SERIALIZATION_NVP(c_); │ │ │ │ │ -302 } │ │ │ │ │ -303}; │ │ │ │ │ +266 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r negate() const override; │ │ │ │ │ +267 │ │ │ │ │ +_2_6_9 bool _i_s_C_o_n_s_t_r_a_i_n_e_d() const { │ │ │ │ │ +270 return model_ && model_->isConstrained(); │ │ │ │ │ +271 } │ │ │ │ │ +272 │ │ │ │ │ +_2_7_6 _D_e_n_s_e_I_n_d_e_x _g_e_t_D_i_m(_c_o_n_s_t___i_t_e_r_a_t_o_r variable) const override { │ │ │ │ │ +277 return Ab_(variable - begin())._c_o_l_s(); │ │ │ │ │ +278 } │ │ │ │ │ +279 │ │ │ │ │ +_2_8_3 size_t _r_o_w_s() const { return Ab_._r_o_w_s(); } │ │ │ │ │ +284 │ │ │ │ │ +_2_8_8 size_t _c_o_l_s() const { return Ab_._c_o_l_s(); } │ │ │ │ │ +289 │ │ │ │ │ +_2_9_1 const SharedDiagonal& _g_e_t___m_o_d_e_l() const { return model_; } │ │ │ │ │ +292 │ │ │ │ │ +_2_9_4 SharedDiagonal& _g_e_t___m_o_d_e_l() { return model_; } │ │ │ │ │ +295 │ │ │ │ │ +_2_9_7 const constBVector _g_e_t_b() const { return Ab_(size()).col(0); } │ │ │ │ │ +298 │ │ │ │ │ +_3_0_0 constABlock _g_e_t_A(_c_o_n_s_t___i_t_e_r_a_t_o_r variable) const { return Ab_(variable - │ │ │ │ │ +begin()); } │ │ │ │ │ +301 │ │ │ │ │ +_3_0_3 constABlock _g_e_t_A() const { return Ab_._r_a_n_g_e(0, size()); } │ │ │ │ │ 304 │ │ │ │ │ -_3_1_3class GTSAM_EXPORT _W_e_l_s_c_h : public _B_a_s_e { │ │ │ │ │ -314 protected: │ │ │ │ │ -315 double c_, csquared_; │ │ │ │ │ -316 │ │ │ │ │ -317 public: │ │ │ │ │ -318 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -319 │ │ │ │ │ -320 _W_e_l_s_c_h(double c = 2.9846, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ -321 double weight(double distance) const override; │ │ │ │ │ -322 double loss(double distance) const override; │ │ │ │ │ -323 void _p_r_i_n_t(const std::string &s) const override; │ │ │ │ │ -324 bool _e_q_u_a_l_s(const _B_a_s_e &expected, double tol = 1e-8) const override; │ │ │ │ │ -325 static shared_ptr Create(double k, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ -326 double modelParameter() const { return c_; } │ │ │ │ │ -327 │ │ │ │ │ -328 private: │ │ │ │ │ -_3_3_0 friend class boost::serialization::access; │ │ │ │ │ -331 template │ │ │ │ │ -332 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ -333 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -334 ar &BOOST_SERIALIZATION_NVP(c_); │ │ │ │ │ -335 ar &BOOST_SERIALIZATION_NVP(csquared_); │ │ │ │ │ -336 } │ │ │ │ │ -337}; │ │ │ │ │ -338 │ │ │ │ │ -_3_4_9class GTSAM_EXPORT _G_e_m_a_n_M_c_C_l_u_r_e : public _B_a_s_e { │ │ │ │ │ -350 public: │ │ │ │ │ -351 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +_3_0_6 BVector _g_e_t_b() { return Ab_(size()).col(0); } │ │ │ │ │ +307 │ │ │ │ │ +_3_0_9 ABlock _g_e_t_A(_i_t_e_r_a_t_o_r variable) { return Ab_(variable - begin()); } │ │ │ │ │ +310 │ │ │ │ │ +_3_1_2 ABlock _g_e_t_A() { return Ab_._r_a_n_g_e(0, size()); } │ │ │ │ │ +313 │ │ │ │ │ +319 void updateHessian(const _K_e_y_V_e_c_t_o_r& keys, _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x* info) const │ │ │ │ │ +override; │ │ │ │ │ +320 │ │ │ │ │ +322 Vector _o_p_e_r_a_t_o_r_*(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const; │ │ │ │ │ +323 │ │ │ │ │ +326 void transposeMultiplyAdd(double alpha, const Vector& e, │ │ │ │ │ +327 _V_e_c_t_o_r_V_a_l_u_e_s& x) const; │ │ │ │ │ +328 │ │ │ │ │ +330 void multiplyHessianAdd(double alpha, const _V_e_c_t_o_r_V_a_l_u_e_s& x, │ │ │ │ │ +331 _V_e_c_t_o_r_V_a_l_u_e_s& y) const override; │ │ │ │ │ +332 │ │ │ │ │ +341 void multiplyHessianAdd(double alpha, const double* x, double* y, │ │ │ │ │ +342 const std::vector& accumulatedDims) const; │ │ │ │ │ +343 │ │ │ │ │ +345 _V_e_c_t_o_r_V_a_l_u_e_s gradientAtZero() const override; │ │ │ │ │ +346 │ │ │ │ │ +348 void gradientAtZero(double* d) const override; │ │ │ │ │ +349 │ │ │ │ │ +351 Vector gradient(_K_e_y key, const _V_e_c_t_o_r_V_a_l_u_e_s& x) const override; │ │ │ │ │ 352 │ │ │ │ │ -353 _G_e_m_a_n_M_c_C_l_u_r_e(double c = 1.0, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ -354 _~_G_e_m_a_n_M_c_C_l_u_r_e() override {} │ │ │ │ │ -355 double weight(double distance) const override; │ │ │ │ │ -356 double loss(double distance) const override; │ │ │ │ │ -357 void _p_r_i_n_t(const std::string &s) const override; │ │ │ │ │ -358 bool _e_q_u_a_l_s(const _B_a_s_e &expected, double tol = 1e-8) const override; │ │ │ │ │ -359 static shared_ptr Create(double k, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ -360 double modelParameter() const { return c_; } │ │ │ │ │ -361 │ │ │ │ │ -362 protected: │ │ │ │ │ -363 double c_; │ │ │ │ │ -364 │ │ │ │ │ -365 private: │ │ │ │ │ -_3_6_7 friend class boost::serialization::access; │ │ │ │ │ -368 template │ │ │ │ │ -369 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ -370 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -371 ar &BOOST_SERIALIZATION_NVP(c_); │ │ │ │ │ -372 } │ │ │ │ │ -373}; │ │ │ │ │ -374 │ │ │ │ │ -_3_8_7class GTSAM_EXPORT _D_C_S : public _B_a_s_e { │ │ │ │ │ -388 public: │ │ │ │ │ -389 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -390 │ │ │ │ │ -391 _D_C_S(double c = 1.0, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ -392 _~_D_C_S() override {} │ │ │ │ │ -393 double weight(double distance) const override; │ │ │ │ │ -394 double loss(double distance) const override; │ │ │ │ │ -395 void _p_r_i_n_t(const std::string &s) const override; │ │ │ │ │ -396 bool _e_q_u_a_l_s(const _B_a_s_e &expected, double tol = 1e-8) const override; │ │ │ │ │ -397 static shared_ptr Create(double k, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ -398 double modelParameter() const { return c_; } │ │ │ │ │ -399 │ │ │ │ │ -400 protected: │ │ │ │ │ -401 double c_; │ │ │ │ │ -402 │ │ │ │ │ -403 private: │ │ │ │ │ -_4_0_5 friend class boost::serialization::access; │ │ │ │ │ -406 template │ │ │ │ │ -407 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ -408 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -409 ar &BOOST_SERIALIZATION_NVP(c_); │ │ │ │ │ -410 } │ │ │ │ │ -411}; │ │ │ │ │ -412 │ │ │ │ │ -_4_2_6class GTSAM_EXPORT _L_2_W_i_t_h_D_e_a_d_Z_o_n_e : public _B_a_s_e { │ │ │ │ │ -427 protected: │ │ │ │ │ -428 double k_; │ │ │ │ │ -429 │ │ │ │ │ -430 public: │ │ │ │ │ -431 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -432 │ │ │ │ │ -433 _L_2_W_i_t_h_D_e_a_d_Z_o_n_e(double k = 1.0, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ -434 double weight(double distance) const override; │ │ │ │ │ -435 double loss(double distance) const override; │ │ │ │ │ -436 void _p_r_i_n_t(const std::string &s) const override; │ │ │ │ │ -437 bool _e_q_u_a_l_s(const _B_a_s_e &expected, double tol = 1e-8) const override; │ │ │ │ │ -438 static shared_ptr Create(double k, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ -439 double modelParameter() const { return k_; } │ │ │ │ │ -440 │ │ │ │ │ -441 private: │ │ │ │ │ -_4_4_3 friend class boost::serialization::access; │ │ │ │ │ -444 template │ │ │ │ │ -445 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ -446 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -447 ar &BOOST_SERIALIZATION_NVP(k_); │ │ │ │ │ -448 } │ │ │ │ │ -449}; │ │ │ │ │ -450 │ │ │ │ │ -451} // namespace mEstimator │ │ │ │ │ -452} // namespace noiseModel │ │ │ │ │ -453} // namespace gtsam │ │ │ │ │ -_T_e_s_t_a_b_l_e_._h │ │ │ │ │ -Concept check for values that can be used in unit tests. │ │ │ │ │ -_M_a_t_r_i_x_._h │ │ │ │ │ -typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +354 _J_a_c_o_b_i_a_n_F_a_c_t_o_r whiten() const; │ │ │ │ │ +355 │ │ │ │ │ +357 std::pair, shared_ptr> │ │ │ │ │ +358 eliminate(const _O_r_d_e_r_i_n_g& keys); │ │ │ │ │ +359 │ │ │ │ │ +361 void setModel(bool anyConstrained, const Vector& sigmas); │ │ │ │ │ +362 │ │ │ │ │ +374 friend GTSAM_EXPORT std::pair, │ │ │ │ │ +shared_ptr> │ │ │ │ │ +375 _E_l_i_m_i_n_a_t_e_Q_R(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& factors, const _O_r_d_e_r_i_n_g& keys); │ │ │ │ │ +376 │ │ │ │ │ +384 boost::shared_ptr splitConditional(size_t nrFrontals); │ │ │ │ │ +385 │ │ │ │ │ +386 protected: │ │ │ │ │ +387 │ │ │ │ │ +389 template │ │ │ │ │ +390 void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& │ │ │ │ │ +noiseModel); │ │ │ │ │ +391 │ │ │ │ │ +392 private: │ │ │ │ │ +393 │ │ │ │ │ +398 void JacobianFactorHelper( │ │ │ │ │ +399 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& graph, │ │ │ │ │ +400 const _F_a_s_t_V_e_c_t_o_r_<_V_a_r_i_a_b_l_e_S_l_o_t_s_:_:_c_o_n_s_t___i_t_e_r_a_t_o_r_>& orderedSlots); │ │ │ │ │ +401 │ │ │ │ │ +408 template │ │ │ │ │ +409 _J_a_c_o_b_i_a_n_F_a_c_t_o_r(const KEYS& keys, const DIMENSIONS& dims, _D_e_n_s_e_I_n_d_e_x m, │ │ │ │ │ +410 const SharedDiagonal& model = SharedDiagonal()) : │ │ │ │ │ +411 Base(keys), Ab_(dims.begin(), dims.end(), m, true), model_(model) { │ │ │ │ │ +412 } │ │ │ │ │ +413 │ │ │ │ │ +414 // be very selective on who can access these private methods: │ │ │ │ │ +415 template friend class ExpressionFactor; │ │ │ │ │ +416 │ │ │ │ │ +_4_1_8 friend class boost::serialization::access; │ │ │ │ │ +419 template │ │ │ │ │ +420 void _s_a_v_e(ARCHIVE & ar, const unsigned int version) const { │ │ │ │ │ +421 // TODO(fan): This is a hack for Boost < 1.66 │ │ │ │ │ +422 // We really need to introduce proper versioning in the archives │ │ │ │ │ +423 // As otherwise this will not read objects serialized by older │ │ │ │ │ +424 // versions of GTSAM │ │ │ │ │ +425 ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +426 ar << BOOST_SERIALIZATION_NVP(Ab_); │ │ │ │ │ +427 bool model_null = false; │ │ │ │ │ +428 if(model_.get() == nullptr) { │ │ │ │ │ +429 model_null = true; │ │ │ │ │ +430 ar << boost::serialization::make_nvp("model_null", model_null); │ │ │ │ │ +431 } else { │ │ │ │ │ +432 ar << boost::serialization::make_nvp("model_null", model_null); │ │ │ │ │ +433 ar << BOOST_SERIALIZATION_NVP(model_); │ │ │ │ │ +434 } │ │ │ │ │ +435 } │ │ │ │ │ +436 │ │ │ │ │ +437 template │ │ │ │ │ +438 void load(ARCHIVE & ar, const unsigned int version) { │ │ │ │ │ +439 // invoke serialization of the base class │ │ │ │ │ +440 ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); │ │ │ │ │ +441 ar >> BOOST_SERIALIZATION_NVP(Ab_); │ │ │ │ │ +442 if (version < 1) { │ │ │ │ │ +443 ar >> BOOST_SERIALIZATION_NVP(model_); │ │ │ │ │ +444 } else { │ │ │ │ │ +445 bool model_null; │ │ │ │ │ +446 ar >> BOOST_SERIALIZATION_NVP(model_null); │ │ │ │ │ +447 if (!model_null) { │ │ │ │ │ +448 ar >> BOOST_SERIALIZATION_NVP(model_); │ │ │ │ │ +449 } │ │ │ │ │ +450 } │ │ │ │ │ +451 } │ │ │ │ │ +452 │ │ │ │ │ +453 BOOST_SERIALIZATION_SPLIT_MEMBER() │ │ │ │ │ +454 }; // JacobianFactor │ │ │ │ │ +456template<> │ │ │ │ │ +_4_5_7struct _t_r_a_i_t_s<_J_a_c_o_b_i_a_n_F_a_c_t_o_r> : public _T_e_s_t_a_b_l_e { │ │ │ │ │ +458}; │ │ │ │ │ +459 │ │ │ │ │ +460} // \ namespace gtsam │ │ │ │ │ +461 │ │ │ │ │ +462BOOST_CLASS_VERSION(_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r, 1) │ │ │ │ │ +463 │ │ │ │ │ +464#include │ │ │ │ │ +465 │ │ │ │ │ +466 │ │ │ │ │ +_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_._h │ │ │ │ │ +A matrix with column blocks of pre-defined sizes. │ │ │ │ │ +_V_a_r_i_a_b_l_e_S_l_o_t_s_._h │ │ │ │ │ +VariableSlots describes the structure of a combined factor in terms of where │ │ │ │ │ +each block comes from in... │ │ │ │ │ +_G_a_u_s_s_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ +A factor with a quadratic error function - a Gaussian. │ │ │ │ │ +_N_o_i_s_e_M_o_d_e_l_._h │ │ │ │ │ +_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ +Included from all GTSAM files. │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ +std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ +FastVector │ │ │ │ │ +FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ +DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_n_s_e_I_n_d_e_x │ │ │ │ │ +ptrdiff_t DenseIndex │ │ │ │ │ +The index type for Eigen objects. │ │ │ │ │ +DDeeffiinniittiioonn types.h:106 │ │ │ │ │ +_g_t_s_a_m_:_:_s_a_v_e │ │ │ │ │ +void save(const Matrix &A, const string &s, const string &filename) │ │ │ │ │ +save a matrix to file, which can be loaded by matlab │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:167 │ │ │ │ │ _g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ print without optional string, must specify cout yourself │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ +Point2 operator*(double s, const Point2 &p) │ │ │ │ │ +multiply with scalar │ │ │ │ │ +DDeeffiinniittiioonn Point2.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_Q_R │ │ │ │ │ +std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > │ │ │ │ │ +EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys) │ │ │ │ │ +Multiply all factors and eliminate the given keys from the resulting factor │ │ │ │ │ +using a QR variant that h... │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.cpp:789 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ +of blocks. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:52 │ │ │ │ │ _g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ Template to create a binary predicate. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_B_a_s_e │ │ │ │ │ -Pure virtual class for all robust error function classes. │ │ │ │ │ -DDeeffiinniittiioonn LossFunctions.h:63 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_B_a_s_e_:_:_l_o_s_s │ │ │ │ │ -virtual double loss(double distance) const │ │ │ │ │ -This method is responsible for returning the total penalty for a given amount │ │ │ │ │ -of error. │ │ │ │ │ -DDeeffiinniittiioonn LossFunctions.h:94 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_B_a_s_e_:_:_r_e_w_e_i_g_h_t__ │ │ │ │ │ -ReweightScheme reweight_ │ │ │ │ │ -Strategy for reweighting. │ │ │ │ │ -DDeeffiinniittiioonn LossFunctions.h:72 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_B_a_s_e_:_:_r_e_w_e_i_g_h_t_S_c_h_e_m_e │ │ │ │ │ -ReweightScheme reweightScheme() const │ │ │ │ │ -Returns the reweight scheme, as explained in ReweightScheme. │ │ │ │ │ -DDeeffiinniittiioonn LossFunctions.h:79 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_B_a_s_e_:_:_w_e_i_g_h_t │ │ │ │ │ -virtual double weight(double distance) const =0 │ │ │ │ │ -This method is responsible for returning the weight function for a given amount │ │ │ │ │ -of error. │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_B_a_s_e_:_:_R_e_w_e_i_g_h_t_S_c_h_e_m_e │ │ │ │ │ -ReweightScheme │ │ │ │ │ -the rows can be weighted independently according to the error or uniformly with │ │ │ │ │ -the norm of the right... │ │ │ │ │ -DDeeffiinniittiioonn LossFunctions.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_N_u_l_l │ │ │ │ │ -"Null" robust loss function, equivalent to a Gaussian pdf noise model, or plain │ │ │ │ │ -least-squares (non-ro... │ │ │ │ │ -DDeeffiinniittiioonn LossFunctions.h:146 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_N_u_l_l_:_:_w_e_i_g_h_t │ │ │ │ │ -double weight(double) const override │ │ │ │ │ -This method is responsible for returning the weight function for a given amount │ │ │ │ │ -of error. │ │ │ │ │ -DDeeffiinniittiioonn LossFunctions.h:152 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_N_u_l_l_:_:_l_o_s_s │ │ │ │ │ -double loss(double distance) const override │ │ │ │ │ -This method is responsible for returning the total penalty for a given amount │ │ │ │ │ -of error. │ │ │ │ │ -DDeeffiinniittiioonn LossFunctions.h:153 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_F_a_i_r │ │ │ │ │ -Implementation of the "Fair" robust error model (Zhang97ivc) │ │ │ │ │ -DDeeffiinniittiioonn LossFunctions.h:175 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_H_u_b_e_r │ │ │ │ │ -The "Huber" robust error model (Zhang97ivc). │ │ │ │ │ -DDeeffiinniittiioonn LossFunctions.h:208 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_C_a_u_c_h_y │ │ │ │ │ -Implementation of the "Cauchy" robust error model (Lee2013IROS). │ │ │ │ │ -DDeeffiinniittiioonn LossFunctions.h:246 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_T_u_k_e_y │ │ │ │ │ -Implementation of the "Tukey" robust error model (Zhang97ivc). │ │ │ │ │ -DDeeffiinniittiioonn LossFunctions.h:280 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_W_e_l_s_c_h │ │ │ │ │ -Implementation of the "Welsch" robust error model (Zhang97ivc). │ │ │ │ │ -DDeeffiinniittiioonn LossFunctions.h:313 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_G_e_m_a_n_M_c_C_l_u_r_e │ │ │ │ │ -Implementation of the "Geman-McClure" robust error model (Zhang97ivc). │ │ │ │ │ -DDeeffiinniittiioonn LossFunctions.h:349 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_D_C_S │ │ │ │ │ -DCS implements the Dynamic Covariance Scaling robust error model from the paper │ │ │ │ │ -Robust Map Optimizati... │ │ │ │ │ -DDeeffiinniittiioonn LossFunctions.h:387 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_L_2_W_i_t_h_D_e_a_d_Z_o_n_e │ │ │ │ │ -L2WithDeadZone implements a standard L2 penalty, but with a dead zone of width │ │ │ │ │ -2*k,... │ │ │ │ │ -DDeeffiinniittiioonn LossFunctions.h:426 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ +of vertical blocks. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_r_a_n_g_e │ │ │ │ │ +Block range(DenseIndex startBlock, DenseIndex endBlock) │ │ │ │ │ +access ranges of blocks at a time │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:130 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_r_o_w_s │ │ │ │ │ +DenseIndex rows() const │ │ │ │ │ +Row size. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:115 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_c_o_l_s │ │ │ │ │ +DenseIndex cols() const │ │ │ │ │ +Column size. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:118 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_c_o_n_s_t___i_t_e_r_a_t_o_r │ │ │ │ │ +KeyVector::const_iterator const_iterator │ │ │ │ │ +Const iterator over keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:80 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_i_t_e_r_a_t_o_r │ │ │ │ │ +KeyVector::iterator iterator │ │ │ │ │ +Iterator over keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_S_l_o_t_s │ │ │ │ │ +A combined factor is assembled as one block of rows for each component factor. │ │ │ │ │ +DDeeffiinniittiioonn VariableSlots.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +An abstract virtual base class for JacobianFactor and HessianFactor. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactor.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactor.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +A Gaussian factor using the canonical parameters (information form) │ │ │ │ │ +DDeeffiinniittiioonn HessianFactor.h:101 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ +A Gaussian factor in the squared-error form. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:91 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_e_t_b │ │ │ │ │ +const constBVector getb() const │ │ │ │ │ +Get a view of the r.h.s. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:297 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_e_t_b │ │ │ │ │ +BVector getb() │ │ │ │ │ +Get a view of the r.h.s. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:306 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_e_t___m_o_d_e_l │ │ │ │ │ +const SharedDiagonal & get_model() const │ │ │ │ │ +get a copy of model │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:291 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ +JacobianFactor(const JacobianFactor &jf) │ │ │ │ │ +Copy constructor. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:114 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_T_h_i_s │ │ │ │ │ +JacobianFactor This │ │ │ │ │ +Typedef to this class. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:94 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_i_s_C_o_n_s_t_r_a_i_n_e_d │ │ │ │ │ +bool isConstrained() const │ │ │ │ │ +is noise model constrained ? │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:269 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_e_t_A │ │ │ │ │ +constABlock getA() const │ │ │ │ │ +Get a view of the A matrix, not weighted by noise. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:303 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ +GaussianFactor::shared_ptr clone() const override │ │ │ │ │ +Clone this JacobianFactor. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:189 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_e_t_A │ │ │ │ │ +ABlock getA(iterator variable) │ │ │ │ │ +Get a view of the A matrix for the variable pointed to by the given key │ │ │ │ │ +iterator (non-const version) │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:309 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_e_t___m_o_d_e_l │ │ │ │ │ +SharedDiagonal & get_model() │ │ │ │ │ +get a copy of model (non-const version) │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:294 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_e_t_D_i_m │ │ │ │ │ +DenseIndex getDim(const_iterator variable) const override │ │ │ │ │ +Return the dimension of the variable pointed to by the given key iterator todo: │ │ │ │ │ +Remove this in favor ... │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:276 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_m_a_t_r_i_x_O_b_j_e_c_t │ │ │ │ │ +VerticalBlockMatrix & matrixObject() │ │ │ │ │ +Mutable access to the full augmented Jacobian matrix of this factor as a │ │ │ │ │ +VerticalBlockMatrix object. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:259 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_e_t_A │ │ │ │ │ +ABlock getA() │ │ │ │ │ +Get a view of the A matrix. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:312 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_B_a_s_e │ │ │ │ │ +GaussianFactor Base │ │ │ │ │ +Typedef to base class. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:95 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_~_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ +~JacobianFactor() override │ │ │ │ │ +Virtual destructor. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:186 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_m_a_t_r_i_x_O_b_j_e_c_t │ │ │ │ │ +const VerticalBlockMatrix & matrixObject() const │ │ │ │ │ +Return the full augmented Jacobian matrix of this factor as a │ │ │ │ │ +VerticalBlockMatrix object. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:256 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_r_o_w_s │ │ │ │ │ +size_t rows() const │ │ │ │ │ +return the number of rows in the corresponding linear system │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:283 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_c_o_l_s │ │ │ │ │ +size_t cols() const │ │ │ │ │ +return the number of columns in the corresponding linear system │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:288 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:96 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_e_t_A │ │ │ │ │ +constABlock getA(const_iterator variable) const │ │ │ │ │ +Get a view of the A matrix for the variable pointed to by the given key │ │ │ │ │ +iterator. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:300 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ +with a unique integer... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ +In Gaussian factors, the error function returns either the negative log- │ │ │ │ │ +likelihood,... │ │ │ │ │ +_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +is the normalization constant. │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +The Factor::error simply extracts the. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * LLoossssFFuunnccttiioonnss..hh │ │ │ │ │ + * _J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00737_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianConditional-inl.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,60 +98,354 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
GaussianConditional-inl.h
│ │ │ │ +
LossFunctions.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ -
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ -
4 * Atlanta, Georgia 30332-0415
│ │ │ │ -
5 * All Rights Reserved
│ │ │ │ -
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ -
7
│ │ │ │ -
8 * See LICENSE for the license information
│ │ │ │ -
9
│ │ │ │ -
10 * -------------------------------------------------------------------------- */
│ │ │ │ -
11
│ │ │ │ -
18// \callgraph
│ │ │ │ -
19
│ │ │ │ +
1
│ │ │ │ +
2/* ----------------------------------------------------------------------------
│ │ │ │ +
3
│ │ │ │ +
4 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
5 * Atlanta, Georgia 30332-0415
│ │ │ │ +
6 * All Rights Reserved
│ │ │ │ +
7 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
8
│ │ │ │ +
9 * See LICENSE for the license information
│ │ │ │ +
10
│ │ │ │ +
11 * -------------------------------------------------------------------------- */
│ │ │ │ +
12
│ │ │ │
20#pragma once
│ │ │ │
21
│ │ │ │ -
22namespace gtsam {
│ │ │ │ -
23
│ │ │ │ -
24 /* ************************************************************************* */
│ │ │ │ -
25 template<typename TERMS>
│ │ │ │ -
│ │ │ │ -
26 GaussianConditional::GaussianConditional(const TERMS& terms,
│ │ │ │ -
27 size_t nrFrontals, const Vector& d, const SharedDiagonal& sigmas) :
│ │ │ │ -
28 BaseFactor(terms, d, sigmas), BaseConditional(nrFrontals) {}
│ │ │ │ -
│ │ │ │ -
29
│ │ │ │ -
30 /* ************************************************************************* */
│ │ │ │ -
31 template<typename KEYS>
│ │ │ │ -
│ │ │ │ -
32 GaussianConditional::GaussianConditional(
│ │ │ │ -
33 const KEYS& keys, size_t nrFrontals, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas) :
│ │ │ │ -
34 BaseFactor(keys, augmentedMatrix, sigmas), BaseConditional(nrFrontals) {}
│ │ │ │ +
22#include <gtsam/base/Matrix.h>
│ │ │ │ +
23#include <gtsam/base/Testable.h>
│ │ │ │ +
24#include <gtsam/dllexport.h>
│ │ │ │ +
25
│ │ │ │ +
26#include <boost/serialization/extended_type_info.hpp>
│ │ │ │ +
27#include <boost/serialization/nvp.hpp>
│ │ │ │ +
28#include <boost/serialization/version.hpp>
│ │ │ │ +
29#include <boost/serialization/optional.hpp>
│ │ │ │ +
30#include <boost/serialization/shared_ptr.hpp>
│ │ │ │ +
31#include <boost/serialization/singleton.hpp>
│ │ │ │ +
32
│ │ │ │ +
33namespace gtsam {
│ │ │ │ +
34namespace noiseModel {
│ │ │ │ +
35// clang-format off
│ │ │ │ +
54// clang-format on
│ │ │ │ +
55namespace mEstimator {
│ │ │ │ +
56
│ │ │ │ +
│ │ │ │ +
63class GTSAM_EXPORT Base {
│ │ │ │ +
64 public:
│ │ │ │ +
67 enum ReweightScheme { Scalar, Block };
│ │ │ │ +
68 typedef boost::shared_ptr<Base> shared_ptr;
│ │ │ │ +
69
│ │ │ │ +
70 protected:
│ │ │ │ +
72 ReweightScheme reweight_;
│ │ │ │ +
73
│ │ │ │ +
74 public:
│ │ │ │ +
75 Base(const ReweightScheme reweight = Block) : reweight_(reweight) {}
│ │ │ │ +
76 virtual ~Base() {}
│ │ │ │ +
77
│ │ │ │ +
79 ReweightScheme reweightScheme() const { return reweight_; }
│ │ │ │ +
80
│ │ │ │ +
94 virtual double loss(double distance) const { return 0; }
│ │ │ │ +
95
│ │ │ │ +
106 virtual double weight(double distance) const = 0;
│ │ │ │ +
107
│ │ │ │ +
108 virtual void print(const std::string &s) const = 0;
│ │ │ │ +
109 virtual bool equals(const Base &expected, double tol = 1e-8) const = 0;
│ │ │ │ +
110
│ │ │ │ +
111 double sqrtWeight(double distance) const { return std::sqrt(weight(distance)); }
│ │ │ │ +
112
│ │ │ │ +
115 Vector weight(const Vector &error) const;
│ │ │ │ +
116
│ │ │ │ +
118 Vector sqrtWeight(const Vector &error) const;
│ │ │ │ +
119
│ │ │ │ +
122 void reweight(Vector &error) const;
│ │ │ │ +
123 void reweight(std::vector<Matrix> &A, Vector &error) const;
│ │ │ │ +
124 void reweight(Matrix &A, Vector &error) const;
│ │ │ │ +
125 void reweight(Matrix &A1, Matrix &A2, Vector &error) const;
│ │ │ │ +
126 void reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const;
│ │ │ │ +
127
│ │ │ │ +
128 private:
│ │ │ │ +
130 friend class boost::serialization::access;
│ │ │ │ +
131 template <class ARCHIVE>
│ │ │ │ +
132 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ +
133 ar &BOOST_SERIALIZATION_NVP(reweight_);
│ │ │ │ +
134 }
│ │ │ │ +
135};
│ │ │ │
│ │ │ │ -
35
│ │ │ │ -
36} // gtsam
│ │ │ │ +
136
│ │ │ │ +
│ │ │ │ +
146class GTSAM_EXPORT Null : public Base {
│ │ │ │ +
147 public:
│ │ │ │ +
148 typedef boost::shared_ptr<Null> shared_ptr;
│ │ │ │ +
149
│ │ │ │ +
150 Null(const ReweightScheme reweight = Block) : Base(reweight) {}
│ │ │ │ +
151 ~Null() override {}
│ │ │ │ +
152 double weight(double /*error*/) const override { return 1.0; }
│ │ │ │ +
153 double loss(double distance) const override { return 0.5 * distance * distance; }
│ │ │ │ +
154 void print(const std::string &s) const override;
│ │ │ │ +
155 bool equals(const Base & /*expected*/, double /*tol*/) const override { return true; }
│ │ │ │ +
156 static shared_ptr Create();
│ │ │ │ +
157
│ │ │ │ +
158 private:
│ │ │ │ +
160 friend class boost::serialization::access;
│ │ │ │ +
161 template <class ARCHIVE>
│ │ │ │ +
162 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ +
163 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
164 }
│ │ │ │ +
165};
│ │ │ │ +
│ │ │ │ +
166
│ │ │ │ +
│ │ │ │ +
175class GTSAM_EXPORT Fair : public Base {
│ │ │ │ +
176 protected:
│ │ │ │ +
177 double c_;
│ │ │ │ +
178
│ │ │ │ +
179 public:
│ │ │ │ +
180 typedef boost::shared_ptr<Fair> shared_ptr;
│ │ │ │ +
181
│ │ │ │ +
182 Fair(double c = 1.3998, const ReweightScheme reweight = Block);
│ │ │ │ +
183 double weight(double distance) const override;
│ │ │ │ +
184 double loss(double distance) const override;
│ │ │ │ +
185 void print(const std::string &s) const override;
│ │ │ │ +
186 bool equals(const Base &expected, double tol = 1e-8) const override;
│ │ │ │ +
187 static shared_ptr Create(double c, const ReweightScheme reweight = Block);
│ │ │ │ +
188 double modelParameter() const { return c_; }
│ │ │ │ +
189
│ │ │ │ +
190 private:
│ │ │ │ +
192 friend class boost::serialization::access;
│ │ │ │ +
193 template <class ARCHIVE>
│ │ │ │ +
194 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ +
195 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
196 ar &BOOST_SERIALIZATION_NVP(c_);
│ │ │ │ +
197 }
│ │ │ │ +
198};
│ │ │ │ +
│ │ │ │ +
199
│ │ │ │ +
│ │ │ │ +
208class GTSAM_EXPORT Huber : public Base {
│ │ │ │ +
209 protected:
│ │ │ │ +
210 double k_;
│ │ │ │ +
211
│ │ │ │ +
212 public:
│ │ │ │ +
213 typedef boost::shared_ptr<Huber> shared_ptr;
│ │ │ │ +
214
│ │ │ │ +
215 Huber(double k = 1.345, const ReweightScheme reweight = Block);
│ │ │ │ +
216 double weight(double distance) const override;
│ │ │ │ +
217 double loss(double distance) const override;
│ │ │ │ +
218 void print(const std::string &s) const override;
│ │ │ │ +
219 bool equals(const Base &expected, double tol = 1e-8) const override;
│ │ │ │ +
220 static shared_ptr Create(double k, const ReweightScheme reweight = Block);
│ │ │ │ +
221 double modelParameter() const { return k_; }
│ │ │ │ +
222
│ │ │ │ +
223 private:
│ │ │ │ +
225 friend class boost::serialization::access;
│ │ │ │ +
226 template <class ARCHIVE>
│ │ │ │ +
227 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ +
228 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
229 ar &BOOST_SERIALIZATION_NVP(k_);
│ │ │ │ +
230 }
│ │ │ │ +
231};
│ │ │ │ +
│ │ │ │ +
232
│ │ │ │ +
│ │ │ │ +
246class GTSAM_EXPORT Cauchy : public Base {
│ │ │ │ +
247 protected:
│ │ │ │ +
248 double k_, ksquared_;
│ │ │ │ +
249
│ │ │ │ +
250 public:
│ │ │ │ +
251 typedef boost::shared_ptr<Cauchy> shared_ptr;
│ │ │ │ +
252
│ │ │ │ +
253 Cauchy(double k = 0.1, const ReweightScheme reweight = Block);
│ │ │ │ +
254 double weight(double distance) const override;
│ │ │ │ +
255 double loss(double distance) const override;
│ │ │ │ +
256 void print(const std::string &s) const override;
│ │ │ │ +
257 bool equals(const Base &expected, double tol = 1e-8) const override;
│ │ │ │ +
258 static shared_ptr Create(double k, const ReweightScheme reweight = Block);
│ │ │ │ +
259 double modelParameter() const { return k_; }
│ │ │ │ +
260
│ │ │ │ +
261 private:
│ │ │ │ +
263 friend class boost::serialization::access;
│ │ │ │ +
264 template <class ARCHIVE>
│ │ │ │ +
265 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ +
266 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
267 ar &BOOST_SERIALIZATION_NVP(k_);
│ │ │ │ +
268 ar &BOOST_SERIALIZATION_NVP(ksquared_);
│ │ │ │ +
269 }
│ │ │ │ +
270};
│ │ │ │ +
│ │ │ │ +
271
│ │ │ │ +
│ │ │ │ +
280class GTSAM_EXPORT Tukey : public Base {
│ │ │ │ +
281 protected:
│ │ │ │ +
282 double c_, csquared_;
│ │ │ │ +
283
│ │ │ │ +
284 public:
│ │ │ │ +
285 typedef boost::shared_ptr<Tukey> shared_ptr;
│ │ │ │ +
286
│ │ │ │ +
287 Tukey(double c = 4.6851, const ReweightScheme reweight = Block);
│ │ │ │ +
288 double weight(double distance) const override;
│ │ │ │ +
289 double loss(double distance) const override;
│ │ │ │ +
290 void print(const std::string &s) const override;
│ │ │ │ +
291 bool equals(const Base &expected, double tol = 1e-8) const override;
│ │ │ │ +
292 static shared_ptr Create(double k, const ReweightScheme reweight = Block);
│ │ │ │ +
293 double modelParameter() const { return c_; }
│ │ │ │ +
294
│ │ │ │ +
295 private:
│ │ │ │ +
297 friend class boost::serialization::access;
│ │ │ │ +
298 template <class ARCHIVE>
│ │ │ │ +
299 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ +
300 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
301 ar &BOOST_SERIALIZATION_NVP(c_);
│ │ │ │ +
302 }
│ │ │ │ +
303};
│ │ │ │ +
│ │ │ │ +
304
│ │ │ │ +
│ │ │ │ +
313class GTSAM_EXPORT Welsch : public Base {
│ │ │ │ +
314 protected:
│ │ │ │ +
315 double c_, csquared_;
│ │ │ │ +
316
│ │ │ │ +
317 public:
│ │ │ │ +
318 typedef boost::shared_ptr<Welsch> shared_ptr;
│ │ │ │ +
319
│ │ │ │ +
320 Welsch(double c = 2.9846, const ReweightScheme reweight = Block);
│ │ │ │ +
321 double weight(double distance) const override;
│ │ │ │ +
322 double loss(double distance) const override;
│ │ │ │ +
323 void print(const std::string &s) const override;
│ │ │ │ +
324 bool equals(const Base &expected, double tol = 1e-8) const override;
│ │ │ │ +
325 static shared_ptr Create(double k, const ReweightScheme reweight = Block);
│ │ │ │ +
326 double modelParameter() const { return c_; }
│ │ │ │ +
327
│ │ │ │ +
328 private:
│ │ │ │ +
330 friend class boost::serialization::access;
│ │ │ │ +
331 template <class ARCHIVE>
│ │ │ │ +
332 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ +
333 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
334 ar &BOOST_SERIALIZATION_NVP(c_);
│ │ │ │ +
335 ar &BOOST_SERIALIZATION_NVP(csquared_);
│ │ │ │ +
336 }
│ │ │ │ +
337};
│ │ │ │ +
│ │ │ │ +
338
│ │ │ │ +
│ │ │ │ +
349class GTSAM_EXPORT GemanMcClure : public Base {
│ │ │ │ +
350 public:
│ │ │ │ +
351 typedef boost::shared_ptr<GemanMcClure> shared_ptr;
│ │ │ │ +
352
│ │ │ │ +
353 GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block);
│ │ │ │ +
354 ~GemanMcClure() override {}
│ │ │ │ +
355 double weight(double distance) const override;
│ │ │ │ +
356 double loss(double distance) const override;
│ │ │ │ +
357 void print(const std::string &s) const override;
│ │ │ │ +
358 bool equals(const Base &expected, double tol = 1e-8) const override;
│ │ │ │ +
359 static shared_ptr Create(double k, const ReweightScheme reweight = Block);
│ │ │ │ +
360 double modelParameter() const { return c_; }
│ │ │ │ +
361
│ │ │ │ +
362 protected:
│ │ │ │ +
363 double c_;
│ │ │ │ +
364
│ │ │ │ +
365 private:
│ │ │ │ +
367 friend class boost::serialization::access;
│ │ │ │ +
368 template <class ARCHIVE>
│ │ │ │ +
369 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ +
370 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
371 ar &BOOST_SERIALIZATION_NVP(c_);
│ │ │ │ +
372 }
│ │ │ │ +
373};
│ │ │ │ +
│ │ │ │ +
374
│ │ │ │ +
│ │ │ │ +
387class GTSAM_EXPORT DCS : public Base {
│ │ │ │ +
388 public:
│ │ │ │ +
389 typedef boost::shared_ptr<DCS> shared_ptr;
│ │ │ │ +
390
│ │ │ │ +
391 DCS(double c = 1.0, const ReweightScheme reweight = Block);
│ │ │ │ +
392 ~DCS() override {}
│ │ │ │ +
393 double weight(double distance) const override;
│ │ │ │ +
394 double loss(double distance) const override;
│ │ │ │ +
395 void print(const std::string &s) const override;
│ │ │ │ +
396 bool equals(const Base &expected, double tol = 1e-8) const override;
│ │ │ │ +
397 static shared_ptr Create(double k, const ReweightScheme reweight = Block);
│ │ │ │ +
398 double modelParameter() const { return c_; }
│ │ │ │ +
399
│ │ │ │ +
400 protected:
│ │ │ │ +
401 double c_;
│ │ │ │ +
402
│ │ │ │ +
403 private:
│ │ │ │ +
405 friend class boost::serialization::access;
│ │ │ │ +
406 template <class ARCHIVE>
│ │ │ │ +
407 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ +
408 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
409 ar &BOOST_SERIALIZATION_NVP(c_);
│ │ │ │ +
410 }
│ │ │ │ +
411};
│ │ │ │ +
│ │ │ │ +
412
│ │ │ │ +
│ │ │ │ +
426class GTSAM_EXPORT L2WithDeadZone : public Base {
│ │ │ │ +
427 protected:
│ │ │ │ +
428 double k_;
│ │ │ │ +
429
│ │ │ │ +
430 public:
│ │ │ │ +
431 typedef boost::shared_ptr<L2WithDeadZone> shared_ptr;
│ │ │ │ +
432
│ │ │ │ +
433 L2WithDeadZone(double k = 1.0, const ReweightScheme reweight = Block);
│ │ │ │ +
434 double weight(double distance) const override;
│ │ │ │ +
435 double loss(double distance) const override;
│ │ │ │ +
436 void print(const std::string &s) const override;
│ │ │ │ +
437 bool equals(const Base &expected, double tol = 1e-8) const override;
│ │ │ │ +
438 static shared_ptr Create(double k, const ReweightScheme reweight = Block);
│ │ │ │ +
439 double modelParameter() const { return k_; }
│ │ │ │ +
440
│ │ │ │ +
441 private:
│ │ │ │ +
443 friend class boost::serialization::access;
│ │ │ │ +
444 template <class ARCHIVE>
│ │ │ │ +
445 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ +
446 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
447 ar &BOOST_SERIALIZATION_NVP(k_);
│ │ │ │ +
448 }
│ │ │ │ +
449};
│ │ │ │ +
│ │ │ │ +
450
│ │ │ │ +
451} // namespace mEstimator
│ │ │ │ +
452} // namespace noiseModel
│ │ │ │ +
453} // namespace gtsam
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::VerticalBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43
│ │ │ │ -
gtsam::GaussianConditional::GaussianConditional
GaussianConditional()
default constructor needed for serialization
Definition GaussianConditional.h:54
│ │ │ │ -
gtsam::GaussianConditional::BaseConditional
Conditional< BaseFactor, This > BaseConditional
Typedef to our conditional base class.
Definition GaussianConditional.h:48
│ │ │ │ -
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition JacobianFactor.h:91
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::noiseModel::mEstimator::Base
Pure virtual class for all robust error function classes.
Definition LossFunctions.h:63
│ │ │ │ +
gtsam::noiseModel::mEstimator::Base::loss
virtual double loss(double distance) const
This method is responsible for returning the total penalty for a given amount of error.
Definition LossFunctions.h:94
│ │ │ │ +
gtsam::noiseModel::mEstimator::Base::reweight_
ReweightScheme reweight_
Strategy for reweighting.
Definition LossFunctions.h:72
│ │ │ │ +
gtsam::noiseModel::mEstimator::Base::reweightScheme
ReweightScheme reweightScheme() const
Returns the reweight scheme, as explained in ReweightScheme.
Definition LossFunctions.h:79
│ │ │ │ +
gtsam::noiseModel::mEstimator::Base::weight
virtual double weight(double distance) const =0
This method is responsible for returning the weight function for a given amount of error.
│ │ │ │ +
gtsam::noiseModel::mEstimator::Base::ReweightScheme
ReweightScheme
the rows can be weighted independently according to the error or uniformly with the norm of the right...
Definition LossFunctions.h:67
│ │ │ │ +
gtsam::noiseModel::mEstimator::Null
"Null" robust loss function, equivalent to a Gaussian pdf noise model, or plain least-squares (non-ro...
Definition LossFunctions.h:146
│ │ │ │ +
gtsam::noiseModel::mEstimator::Null::weight
double weight(double) const override
This method is responsible for returning the weight function for a given amount of error.
Definition LossFunctions.h:152
│ │ │ │ +
gtsam::noiseModel::mEstimator::Null::loss
double loss(double distance) const override
This method is responsible for returning the total penalty for a given amount of error.
Definition LossFunctions.h:153
│ │ │ │ +
gtsam::noiseModel::mEstimator::Fair
Implementation of the "Fair" robust error model (Zhang97ivc)
Definition LossFunctions.h:175
│ │ │ │ +
gtsam::noiseModel::mEstimator::Huber
The "Huber" robust error model (Zhang97ivc).
Definition LossFunctions.h:208
│ │ │ │ +
gtsam::noiseModel::mEstimator::Cauchy
Implementation of the "Cauchy" robust error model (Lee2013IROS).
Definition LossFunctions.h:246
│ │ │ │ +
gtsam::noiseModel::mEstimator::Tukey
Implementation of the "Tukey" robust error model (Zhang97ivc).
Definition LossFunctions.h:280
│ │ │ │ +
gtsam::noiseModel::mEstimator::Welsch
Implementation of the "Welsch" robust error model (Zhang97ivc).
Definition LossFunctions.h:313
│ │ │ │ +
gtsam::noiseModel::mEstimator::GemanMcClure
Implementation of the "Geman-McClure" robust error model (Zhang97ivc).
Definition LossFunctions.h:349
│ │ │ │ +
gtsam::noiseModel::mEstimator::DCS
DCS implements the Dynamic Covariance Scaling robust error model from the paper Robust Map Optimizati...
Definition LossFunctions.h:387
│ │ │ │ +
gtsam::noiseModel::mEstimator::L2WithDeadZone
L2WithDeadZone implements a standard L2 penalty, but with a dead zone of width 2*k,...
Definition LossFunctions.h:426
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,64 +1,385 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GaussianConditional-inl.h │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -1/* --------------------------------------------------------------------------- │ │ │ │ │ +LossFunctions.h │ │ │ │ │ +1 │ │ │ │ │ +2/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ -2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ -4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -5 * All Rights Reserved │ │ │ │ │ -6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ -7 │ │ │ │ │ -8 * See LICENSE for the license information │ │ │ │ │ -9 │ │ │ │ │ -10 * ------------------------------------------------------------------------- │ │ │ │ │ +3 │ │ │ │ │ +4 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +5 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +6 * All Rights Reserved │ │ │ │ │ +7 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +8 │ │ │ │ │ +9 * See LICENSE for the license information │ │ │ │ │ +10 │ │ │ │ │ +11 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ -11 │ │ │ │ │ -18// \callgraph │ │ │ │ │ -19 │ │ │ │ │ +12 │ │ │ │ │ 20#pragma once │ │ │ │ │ 21 │ │ │ │ │ -22namespace _g_t_s_a_m { │ │ │ │ │ -23 │ │ │ │ │ -24 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -25 template │ │ │ │ │ -_2_6 _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l(const TERMS& terms, │ │ │ │ │ -27 size_t nrFrontals, const Vector& d, const SharedDiagonal& sigmas) : │ │ │ │ │ -28 _B_a_s_e_F_a_c_t_o_r(terms, d, sigmas), _B_a_s_e_C_o_n_d_i_t_i_o_n_a_l(nrFrontals) {} │ │ │ │ │ -29 │ │ │ │ │ -30 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -31 template │ │ │ │ │ -_3_2 _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l( │ │ │ │ │ -33 const KEYS& keys, size_t nrFrontals, const _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x& │ │ │ │ │ -augmentedMatrix, const SharedDiagonal& sigmas) : │ │ │ │ │ -34 _B_a_s_e_F_a_c_t_o_r(keys, augmentedMatrix, sigmas), _B_a_s_e_C_o_n_d_i_t_i_o_n_a_l(nrFrontals) {} │ │ │ │ │ -35 │ │ │ │ │ -36} // gtsam │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +24#include │ │ │ │ │ +25 │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28#include │ │ │ │ │ +29#include │ │ │ │ │ +30#include │ │ │ │ │ +31#include │ │ │ │ │ +32 │ │ │ │ │ +33namespace _g_t_s_a_m { │ │ │ │ │ +34namespace noiseModel { │ │ │ │ │ +35// clang-format off │ │ │ │ │ +54// clang-format on │ │ │ │ │ +55namespace mEstimator { │ │ │ │ │ +56 │ │ │ │ │ +_6_3class GTSAM_EXPORT _B_a_s_e { │ │ │ │ │ +64 public: │ │ │ │ │ +_6_7 enum _R_e_w_e_i_g_h_t_S_c_h_e_m_e { Scalar, Block }; │ │ │ │ │ +68 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +69 │ │ │ │ │ +70 protected: │ │ │ │ │ +_7_2 _R_e_w_e_i_g_h_t_S_c_h_e_m_e _r_e_w_e_i_g_h_t__; │ │ │ │ │ +73 │ │ │ │ │ +74 public: │ │ │ │ │ +75 _B_a_s_e(const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block) : reweight_(reweight) {} │ │ │ │ │ +76 virtual _~_B_a_s_e() {} │ │ │ │ │ +77 │ │ │ │ │ +_7_9 _R_e_w_e_i_g_h_t_S_c_h_e_m_e _r_e_w_e_i_g_h_t_S_c_h_e_m_e() const { return reweight_; } │ │ │ │ │ +80 │ │ │ │ │ +_9_4 virtual double _l_o_s_s(double distance) const { return 0; } │ │ │ │ │ +95 │ │ │ │ │ +_1_0_6 virtual double _w_e_i_g_h_t(double distance) const = 0; │ │ │ │ │ +107 │ │ │ │ │ +108 virtual void _p_r_i_n_t(const std::string &s) const = 0; │ │ │ │ │ +109 virtual bool _e_q_u_a_l_s(const _B_a_s_e &expected, double tol = 1e-8) const = 0; │ │ │ │ │ +110 │ │ │ │ │ +111 double sqrtWeight(double distance) const { return std::sqrt(weight │ │ │ │ │ +(distance)); } │ │ │ │ │ +112 │ │ │ │ │ +115 Vector weight(const Vector &error) const; │ │ │ │ │ +116 │ │ │ │ │ +118 Vector sqrtWeight(const Vector &error) const; │ │ │ │ │ +119 │ │ │ │ │ +122 void reweight(Vector &error) const; │ │ │ │ │ +123 void reweight(std::vector &A, Vector &error) const; │ │ │ │ │ +124 void reweight(Matrix &A, Vector &error) const; │ │ │ │ │ +125 void reweight(Matrix &A1, Matrix &A2, Vector &error) const; │ │ │ │ │ +126 void reweight(Matrix &A1, Matrix &A2, Matrix &A3, Vector &error) const; │ │ │ │ │ +127 │ │ │ │ │ +128 private: │ │ │ │ │ +_1_3_0 friend class boost::serialization::access; │ │ │ │ │ +131 template │ │ │ │ │ +132 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ +133 ar &BOOST_SERIALIZATION_NVP(reweight_); │ │ │ │ │ +134 } │ │ │ │ │ +135}; │ │ │ │ │ +136 │ │ │ │ │ +_1_4_6class GTSAM_EXPORT _N_u_l_l : public _B_a_s_e { │ │ │ │ │ +147 public: │ │ │ │ │ +148 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +149 │ │ │ │ │ +150 _N_u_l_l(const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block) : _B_a_s_e(reweight) {} │ │ │ │ │ +151 _~_N_u_l_l() override {} │ │ │ │ │ +_1_5_2 double _w_e_i_g_h_t(double /*error*/) const override { return 1.0; } │ │ │ │ │ +_1_5_3 double _l_o_s_s(double distance) const override { return 0.5 * distance * │ │ │ │ │ +distance; } │ │ │ │ │ +154 void _p_r_i_n_t(const std::string &s) const override; │ │ │ │ │ +155 bool _e_q_u_a_l_s(const _B_a_s_e & /*expected*/, double /*tol*/) const override │ │ │ │ │ +{ return true; } │ │ │ │ │ +156 static shared_ptr Create(); │ │ │ │ │ +157 │ │ │ │ │ +158 private: │ │ │ │ │ +_1_6_0 friend class boost::serialization::access; │ │ │ │ │ +161 template │ │ │ │ │ +162 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ +163 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +164 } │ │ │ │ │ +165}; │ │ │ │ │ +166 │ │ │ │ │ +_1_7_5class GTSAM_EXPORT _F_a_i_r : public _B_a_s_e { │ │ │ │ │ +176 protected: │ │ │ │ │ +177 double c_; │ │ │ │ │ +178 │ │ │ │ │ +179 public: │ │ │ │ │ +180 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +181 │ │ │ │ │ +182 _F_a_i_r(double c = 1.3998, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ +183 double weight(double distance) const override; │ │ │ │ │ +184 double loss(double distance) const override; │ │ │ │ │ +185 void _p_r_i_n_t(const std::string &s) const override; │ │ │ │ │ +186 bool _e_q_u_a_l_s(const _B_a_s_e &expected, double tol = 1e-8) const override; │ │ │ │ │ +187 static shared_ptr Create(double c, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ +188 double modelParameter() const { return c_; } │ │ │ │ │ +189 │ │ │ │ │ +190 private: │ │ │ │ │ +_1_9_2 friend class boost::serialization::access; │ │ │ │ │ +193 template │ │ │ │ │ +194 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ +195 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +196 ar &BOOST_SERIALIZATION_NVP(c_); │ │ │ │ │ +197 } │ │ │ │ │ +198}; │ │ │ │ │ +199 │ │ │ │ │ +_2_0_8class GTSAM_EXPORT _H_u_b_e_r : public _B_a_s_e { │ │ │ │ │ +209 protected: │ │ │ │ │ +210 double k_; │ │ │ │ │ +211 │ │ │ │ │ +212 public: │ │ │ │ │ +213 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +214 │ │ │ │ │ +215 _H_u_b_e_r(double k = 1.345, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ +216 double weight(double distance) const override; │ │ │ │ │ +217 double loss(double distance) const override; │ │ │ │ │ +218 void _p_r_i_n_t(const std::string &s) const override; │ │ │ │ │ +219 bool _e_q_u_a_l_s(const _B_a_s_e &expected, double tol = 1e-8) const override; │ │ │ │ │ +220 static shared_ptr Create(double k, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ +221 double modelParameter() const { return k_; } │ │ │ │ │ +222 │ │ │ │ │ +223 private: │ │ │ │ │ +_2_2_5 friend class boost::serialization::access; │ │ │ │ │ +226 template │ │ │ │ │ +227 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ +228 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +229 ar &BOOST_SERIALIZATION_NVP(k_); │ │ │ │ │ +230 } │ │ │ │ │ +231}; │ │ │ │ │ +232 │ │ │ │ │ +_2_4_6class GTSAM_EXPORT _C_a_u_c_h_y : public _B_a_s_e { │ │ │ │ │ +247 protected: │ │ │ │ │ +248 double k_, ksquared_; │ │ │ │ │ +249 │ │ │ │ │ +250 public: │ │ │ │ │ +251 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +252 │ │ │ │ │ +253 _C_a_u_c_h_y(double k = 0.1, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ +254 double weight(double distance) const override; │ │ │ │ │ +255 double loss(double distance) const override; │ │ │ │ │ +256 void _p_r_i_n_t(const std::string &s) const override; │ │ │ │ │ +257 bool _e_q_u_a_l_s(const _B_a_s_e &expected, double tol = 1e-8) const override; │ │ │ │ │ +258 static shared_ptr Create(double k, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ +259 double modelParameter() const { return k_; } │ │ │ │ │ +260 │ │ │ │ │ +261 private: │ │ │ │ │ +_2_6_3 friend class boost::serialization::access; │ │ │ │ │ +264 template │ │ │ │ │ +265 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ +266 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +267 ar &BOOST_SERIALIZATION_NVP(k_); │ │ │ │ │ +268 ar &BOOST_SERIALIZATION_NVP(ksquared_); │ │ │ │ │ +269 } │ │ │ │ │ +270}; │ │ │ │ │ +271 │ │ │ │ │ +_2_8_0class GTSAM_EXPORT _T_u_k_e_y : public _B_a_s_e { │ │ │ │ │ +281 protected: │ │ │ │ │ +282 double c_, csquared_; │ │ │ │ │ +283 │ │ │ │ │ +284 public: │ │ │ │ │ +285 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +286 │ │ │ │ │ +287 _T_u_k_e_y(double c = 4.6851, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ +288 double weight(double distance) const override; │ │ │ │ │ +289 double loss(double distance) const override; │ │ │ │ │ +290 void _p_r_i_n_t(const std::string &s) const override; │ │ │ │ │ +291 bool _e_q_u_a_l_s(const _B_a_s_e &expected, double tol = 1e-8) const override; │ │ │ │ │ +292 static shared_ptr Create(double k, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ +293 double modelParameter() const { return c_; } │ │ │ │ │ +294 │ │ │ │ │ +295 private: │ │ │ │ │ +_2_9_7 friend class boost::serialization::access; │ │ │ │ │ +298 template │ │ │ │ │ +299 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ +300 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +301 ar &BOOST_SERIALIZATION_NVP(c_); │ │ │ │ │ +302 } │ │ │ │ │ +303}; │ │ │ │ │ +304 │ │ │ │ │ +_3_1_3class GTSAM_EXPORT _W_e_l_s_c_h : public _B_a_s_e { │ │ │ │ │ +314 protected: │ │ │ │ │ +315 double c_, csquared_; │ │ │ │ │ +316 │ │ │ │ │ +317 public: │ │ │ │ │ +318 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +319 │ │ │ │ │ +320 _W_e_l_s_c_h(double c = 2.9846, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ +321 double weight(double distance) const override; │ │ │ │ │ +322 double loss(double distance) const override; │ │ │ │ │ +323 void _p_r_i_n_t(const std::string &s) const override; │ │ │ │ │ +324 bool _e_q_u_a_l_s(const _B_a_s_e &expected, double tol = 1e-8) const override; │ │ │ │ │ +325 static shared_ptr Create(double k, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ +326 double modelParameter() const { return c_; } │ │ │ │ │ +327 │ │ │ │ │ +328 private: │ │ │ │ │ +_3_3_0 friend class boost::serialization::access; │ │ │ │ │ +331 template │ │ │ │ │ +332 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ +333 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +334 ar &BOOST_SERIALIZATION_NVP(c_); │ │ │ │ │ +335 ar &BOOST_SERIALIZATION_NVP(csquared_); │ │ │ │ │ +336 } │ │ │ │ │ +337}; │ │ │ │ │ +338 │ │ │ │ │ +_3_4_9class GTSAM_EXPORT _G_e_m_a_n_M_c_C_l_u_r_e : public _B_a_s_e { │ │ │ │ │ +350 public: │ │ │ │ │ +351 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +352 │ │ │ │ │ +353 _G_e_m_a_n_M_c_C_l_u_r_e(double c = 1.0, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ +354 _~_G_e_m_a_n_M_c_C_l_u_r_e() override {} │ │ │ │ │ +355 double weight(double distance) const override; │ │ │ │ │ +356 double loss(double distance) const override; │ │ │ │ │ +357 void _p_r_i_n_t(const std::string &s) const override; │ │ │ │ │ +358 bool _e_q_u_a_l_s(const _B_a_s_e &expected, double tol = 1e-8) const override; │ │ │ │ │ +359 static shared_ptr Create(double k, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ +360 double modelParameter() const { return c_; } │ │ │ │ │ +361 │ │ │ │ │ +362 protected: │ │ │ │ │ +363 double c_; │ │ │ │ │ +364 │ │ │ │ │ +365 private: │ │ │ │ │ +_3_6_7 friend class boost::serialization::access; │ │ │ │ │ +368 template │ │ │ │ │ +369 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ +370 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +371 ar &BOOST_SERIALIZATION_NVP(c_); │ │ │ │ │ +372 } │ │ │ │ │ +373}; │ │ │ │ │ +374 │ │ │ │ │ +_3_8_7class GTSAM_EXPORT _D_C_S : public _B_a_s_e { │ │ │ │ │ +388 public: │ │ │ │ │ +389 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +390 │ │ │ │ │ +391 _D_C_S(double c = 1.0, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ +392 _~_D_C_S() override {} │ │ │ │ │ +393 double weight(double distance) const override; │ │ │ │ │ +394 double loss(double distance) const override; │ │ │ │ │ +395 void _p_r_i_n_t(const std::string &s) const override; │ │ │ │ │ +396 bool _e_q_u_a_l_s(const _B_a_s_e &expected, double tol = 1e-8) const override; │ │ │ │ │ +397 static shared_ptr Create(double k, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ +398 double modelParameter() const { return c_; } │ │ │ │ │ +399 │ │ │ │ │ +400 protected: │ │ │ │ │ +401 double c_; │ │ │ │ │ +402 │ │ │ │ │ +403 private: │ │ │ │ │ +_4_0_5 friend class boost::serialization::access; │ │ │ │ │ +406 template │ │ │ │ │ +407 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ +408 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +409 ar &BOOST_SERIALIZATION_NVP(c_); │ │ │ │ │ +410 } │ │ │ │ │ +411}; │ │ │ │ │ +412 │ │ │ │ │ +_4_2_6class GTSAM_EXPORT _L_2_W_i_t_h_D_e_a_d_Z_o_n_e : public _B_a_s_e { │ │ │ │ │ +427 protected: │ │ │ │ │ +428 double k_; │ │ │ │ │ +429 │ │ │ │ │ +430 public: │ │ │ │ │ +431 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +432 │ │ │ │ │ +433 _L_2_W_i_t_h_D_e_a_d_Z_o_n_e(double k = 1.0, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ +434 double weight(double distance) const override; │ │ │ │ │ +435 double loss(double distance) const override; │ │ │ │ │ +436 void _p_r_i_n_t(const std::string &s) const override; │ │ │ │ │ +437 bool _e_q_u_a_l_s(const _B_a_s_e &expected, double tol = 1e-8) const override; │ │ │ │ │ +438 static shared_ptr Create(double k, const _R_e_w_e_i_g_h_t_S_c_h_e_m_e reweight = Block); │ │ │ │ │ +439 double modelParameter() const { return k_; } │ │ │ │ │ +440 │ │ │ │ │ +441 private: │ │ │ │ │ +_4_4_3 friend class boost::serialization::access; │ │ │ │ │ +444 template │ │ │ │ │ +445 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ +446 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +447 ar &BOOST_SERIALIZATION_NVP(k_); │ │ │ │ │ +448 } │ │ │ │ │ +449}; │ │ │ │ │ +450 │ │ │ │ │ +451} // namespace mEstimator │ │ │ │ │ +452} // namespace noiseModel │ │ │ │ │ +453} // namespace gtsam │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ +_M_a_t_r_i_x_._h │ │ │ │ │ +typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ -of vertical blocks. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -GaussianConditional() │ │ │ │ │ -default constructor needed for serialization │ │ │ │ │ -DDeeffiinniittiioonn GaussianConditional.h:54 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_B_a_s_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -Conditional< BaseFactor, This > BaseConditional │ │ │ │ │ -Typedef to our conditional base class. │ │ │ │ │ -DDeeffiinniittiioonn GaussianConditional.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ -A Gaussian factor in the squared-error form. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:91 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_B_a_s_e │ │ │ │ │ +Pure virtual class for all robust error function classes. │ │ │ │ │ +DDeeffiinniittiioonn LossFunctions.h:63 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_B_a_s_e_:_:_l_o_s_s │ │ │ │ │ +virtual double loss(double distance) const │ │ │ │ │ +This method is responsible for returning the total penalty for a given amount │ │ │ │ │ +of error. │ │ │ │ │ +DDeeffiinniittiioonn LossFunctions.h:94 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_B_a_s_e_:_:_r_e_w_e_i_g_h_t__ │ │ │ │ │ +ReweightScheme reweight_ │ │ │ │ │ +Strategy for reweighting. │ │ │ │ │ +DDeeffiinniittiioonn LossFunctions.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_B_a_s_e_:_:_r_e_w_e_i_g_h_t_S_c_h_e_m_e │ │ │ │ │ +ReweightScheme reweightScheme() const │ │ │ │ │ +Returns the reweight scheme, as explained in ReweightScheme. │ │ │ │ │ +DDeeffiinniittiioonn LossFunctions.h:79 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_B_a_s_e_:_:_w_e_i_g_h_t │ │ │ │ │ +virtual double weight(double distance) const =0 │ │ │ │ │ +This method is responsible for returning the weight function for a given amount │ │ │ │ │ +of error. │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_B_a_s_e_:_:_R_e_w_e_i_g_h_t_S_c_h_e_m_e │ │ │ │ │ +ReweightScheme │ │ │ │ │ +the rows can be weighted independently according to the error or uniformly with │ │ │ │ │ +the norm of the right... │ │ │ │ │ +DDeeffiinniittiioonn LossFunctions.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_N_u_l_l │ │ │ │ │ +"Null" robust loss function, equivalent to a Gaussian pdf noise model, or plain │ │ │ │ │ +least-squares (non-ro... │ │ │ │ │ +DDeeffiinniittiioonn LossFunctions.h:146 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_N_u_l_l_:_:_w_e_i_g_h_t │ │ │ │ │ +double weight(double) const override │ │ │ │ │ +This method is responsible for returning the weight function for a given amount │ │ │ │ │ +of error. │ │ │ │ │ +DDeeffiinniittiioonn LossFunctions.h:152 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_N_u_l_l_:_:_l_o_s_s │ │ │ │ │ +double loss(double distance) const override │ │ │ │ │ +This method is responsible for returning the total penalty for a given amount │ │ │ │ │ +of error. │ │ │ │ │ +DDeeffiinniittiioonn LossFunctions.h:153 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_F_a_i_r │ │ │ │ │ +Implementation of the "Fair" robust error model (Zhang97ivc) │ │ │ │ │ +DDeeffiinniittiioonn LossFunctions.h:175 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_H_u_b_e_r │ │ │ │ │ +The "Huber" robust error model (Zhang97ivc). │ │ │ │ │ +DDeeffiinniittiioonn LossFunctions.h:208 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_C_a_u_c_h_y │ │ │ │ │ +Implementation of the "Cauchy" robust error model (Lee2013IROS). │ │ │ │ │ +DDeeffiinniittiioonn LossFunctions.h:246 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_T_u_k_e_y │ │ │ │ │ +Implementation of the "Tukey" robust error model (Zhang97ivc). │ │ │ │ │ +DDeeffiinniittiioonn LossFunctions.h:280 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_W_e_l_s_c_h │ │ │ │ │ +Implementation of the "Welsch" robust error model (Zhang97ivc). │ │ │ │ │ +DDeeffiinniittiioonn LossFunctions.h:313 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_G_e_m_a_n_M_c_C_l_u_r_e │ │ │ │ │ +Implementation of the "Geman-McClure" robust error model (Zhang97ivc). │ │ │ │ │ +DDeeffiinniittiioonn LossFunctions.h:349 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_D_C_S │ │ │ │ │ +DCS implements the Dynamic Covariance Scaling robust error model from the paper │ │ │ │ │ +Robust Map Optimizati... │ │ │ │ │ +DDeeffiinniittiioonn LossFunctions.h:387 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_L_2_W_i_t_h_D_e_a_d_Z_o_n_e │ │ │ │ │ +L2WithDeadZone implements a standard L2 penalty, but with a dead zone of width │ │ │ │ │ +2*k,... │ │ │ │ │ +DDeeffiinniittiioonn LossFunctions.h:426 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_-_i_n_l_._h │ │ │ │ │ + * LLoossssFFuunnccttiioonnss..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00740.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Sampler.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,52 +94,37 @@ │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
NoiseModel.cpp File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
Sampler.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ + │ │ │ │ +

sampling from a diagonal NoiseModel │ │ │ │ +More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
namespace  gtsam::noiseModel
 All noise models live in the noiseModel namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -template<class MATRIX >
void gtsam::noiseModel::updateAb (MATRIX &Ab, int j, const Vector &a, const Vector &rd)
 
│ │ │ │ -boost::optional< Vector > gtsam::noiseModel::checkIfDiagonal (const Matrix &M)
 
│ │ │ │ -template<typename VECTOR >
boost::optional< size_t > gtsam::noiseModel::check_if_constraint (VECTOR a, const Vector &invsigmas, size_t m)
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Jan 13, 2010
│ │ │ │ -
Author
Richard Roberts
│ │ │ │ +

sampling from a diagonal NoiseModel

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │ -Frank Dellaert
│ │ │ │ +Alex Cunningham
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,35 +1,21 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -NoiseModel.cpp File Reference │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +Sampler.cpp File Reference │ │ │ │ │ +sampling from a diagonal NoiseModel _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -namespace   _g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l │ │ │ │ │ -  All noise models live in the _n_o_i_s_e_M_o_d_e_l namespace. │ │ │ │ │ -  │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ - void  ggttssaamm::::nnooiisseeMMooddeell::::uuppddaatteeAAbb (MATRIX &Ab, int j, │ │ │ │ │ - const Vector &a, const Vector &rd) │ │ │ │ │ -  │ │ │ │ │ -boost::optional< Vector >  ggttssaamm::::nnooiisseeMMooddeell::::cchheecckkIIffDDiiaaggoonnaall (const Matrix &M) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -boost::optional< size_t >  ggttssaamm::::nnooiisseeMMooddeell::::cchheecckk__iiff__ccoonnssttrraaiinntt (VECTOR a, │ │ │ │ │ - const Vector &invsigmas, size_t m) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Jan 13, 2010 │ │ │ │ │ +sampling from a diagonal NoiseModel │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Alex Cunningham │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _N_o_i_s_e_M_o_d_e_l_._c_p_p │ │ │ │ │ + * _S_a_m_p_l_e_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00743.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/SubgraphPreconditioner.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianDensity.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,31 +95,35 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces
│ │ │ │ -
SubgraphPreconditioner.cpp File Reference
│ │ │ │ +
GaussianDensity.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ + │ │ │ │ +

A Gaussian Density. │ │ │ │ +More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Dec 31, 2009
│ │ │ │ -
Author
Frank Dellaert, Yong-Dian Jian
│ │ │ │ +

A Gaussian Density.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
Date
Jan 21, 2012
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,22 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SubgraphPreconditioner.cpp File Reference │ │ │ │ │ +GaussianDensity.cpp File Reference │ │ │ │ │ +A Gaussian Density. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Dec 31, 2009 │ │ │ │ │ +A Gaussian Density. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert, Yong-Dian Jian │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + Jan 21, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_._c_p_p │ │ │ │ │ + * _G_a_u_s_s_i_a_n_D_e_n_s_i_t_y_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00746.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianBayesNet.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianISAM.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,35 +94,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
GaussianBayesNet.cpp File Reference
│ │ │ │ +
GaussianISAM.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Chordal Bayes Net, the result of eliminating a factor graph. │ │ │ │ -More...

│ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::GaussianISAM
 
struct  gtsam::traits< GaussianISAM >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Chordal Bayes Net, the result of eliminating a factor graph.

│ │ │ │ -
Author
Frank Dellaert, Varun Agrawal
│ │ │ │ +
Date
July 29, 2013
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -GaussianBayesNet.cpp File Reference │ │ │ │ │ -Chordal Bayes Net, the result of eliminating a factor graph. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +GaussianISAM.h File Reference │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_I_S_A_M │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_I_S_A_M_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Chordal Bayes Net, the result of eliminating a factor graph. │ │ │ │ │ + Date │ │ │ │ │ + July 29, 2013 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert, Varun Agrawal │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._c_p_p │ │ │ │ │ + * _G_a_u_s_s_i_a_n_I_S_A_M_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00752_source.html │ │ │ │ @@ -114,18 +114,18 @@ │ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/linear/GaussianFactor.h>
│ │ │ │ -
22#include <gtsam/linear/Scatter.h>
│ │ │ │ -
23#include <gtsam/base/SymmetricBlockMatrix.h>
│ │ │ │ -
24#include <gtsam/base/FastVector.h>
│ │ │ │ +
21#include <gtsam/linear/GaussianFactor.h>
│ │ │ │ +
22#include <gtsam/linear/Scatter.h>
│ │ │ │ +
23#include <gtsam/base/SymmetricBlockMatrix.h>
│ │ │ │ +
24#include <gtsam/base/FastVector.h>
│ │ │ │
25
│ │ │ │
26#include <boost/make_shared.hpp>
│ │ │ │
27
│ │ │ │
28namespace gtsam {
│ │ │ │
29
│ │ │ │
30 // Forward declarations
│ │ │ │
31 class Ordering;
│ │ │ │ @@ -308,20 +308,20 @@ │ │ │ │
412
│ │ │ │
414template<>
│ │ │ │
415struct traits<HessianFactor> : public Testable<HessianFactor> {};
│ │ │ │
416
│ │ │ │
417} // \ namespace gtsam
│ │ │ │
418
│ │ │ │
419
│ │ │ │ -
420#include <gtsam/linear/HessianFactor-inl.h>
│ │ │ │ -
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ -
SymmetricBlockMatrix.h
Access to matrices via blocks of pre-defined sizes.
│ │ │ │ -
Scatter.h
Maps global variable indices to slot indices.
│ │ │ │ -
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
│ │ │ │ -
HessianFactor-inl.h
Contains the HessianFactor class, a general quadratic factor.
│ │ │ │ +
420#include <gtsam/linear/HessianFactor-inl.h>
│ │ │ │ +
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ +
SymmetricBlockMatrix.h
Access to matrices via blocks of pre-defined sizes.
│ │ │ │ +
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
│ │ │ │ +
HessianFactor-inl.h
Contains the HessianFactor class, a general quadratic factor.
│ │ │ │ +
Scatter.h
Maps global variable indices to slot indices.
│ │ │ │
gtsam::EliminateCholesky
std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< HessianFactor > > EliminateCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
Densely partially eliminate with Cholesky factorization.
Definition HessianFactor.cpp:525
│ │ │ │
gtsam::EliminatePreferCholesky
std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< GaussianFactor > > EliminatePreferCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
Densely partially eliminate with Cholesky factorization.
Definition HessianFactor.cpp:548
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ ├── html2text {} │ │ │ │ │ @@ -206,20 +206,20 @@ │ │ │ │ │ 418 │ │ │ │ │ 419 │ │ │ │ │ 420#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_H_e_s_s_i_a_n_F_a_c_t_o_r_-_i_n_l_._h> │ │ │ │ │ _F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_._h │ │ │ │ │ Access to matrices via blocks of pre-defined sizes. │ │ │ │ │ -_S_c_a_t_t_e_r_._h │ │ │ │ │ -Maps global variable indices to slot indices. │ │ │ │ │ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ A factor with a quadratic error function - a Gaussian. │ │ │ │ │ _H_e_s_s_i_a_n_F_a_c_t_o_r_-_i_n_l_._h │ │ │ │ │ Contains the HessianFactor class, a general quadratic factor. │ │ │ │ │ +_S_c_a_t_t_e_r_._h │ │ │ │ │ +Maps global variable indices to slot indices. │ │ │ │ │ _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_C_h_o_l_e_s_k_y │ │ │ │ │ std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< │ │ │ │ │ HessianFactor > > EliminateCholesky(const GaussianFactorGraph &factors, const │ │ │ │ │ Ordering &keys) │ │ │ │ │ Densely partially eliminate with Cholesky factorization. │ │ │ │ │ DDeeffiinniittiioonn HessianFactor.cpp:525 │ │ │ │ │ _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_P_r_e_f_e_r_C_h_o_l_e_s_k_y │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00755.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/SubgraphSolver.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,101 +94,38 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
│ │ │ │ -
NoiseModel.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
SubgraphSolver.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Subgraph Solver from IROS 2010. │ │ │ │ +More...

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::noiseModel::Base
 noiseModel::Base is the abstract base class for all noise models. More...
 
class  gtsam::noiseModel::Gaussian
 Gaussian implements the mathematical model |R*x|^2 = |y|^2 with R'*R=inv(Sigma) where y = whiten(x) = R*x x = unwhiten(x) = inv(R)*y as indeed |y|^2 = y'*y = x'*R'*R*x Various derived classes are available that are more efficient. More...
 
class  gtsam::noiseModel::Diagonal
 A diagonal noise model implements a diagonal covariance matrix, with the elements of the diagonal specified in a Vector. More...
 
class  gtsam::noiseModel::Constrained
 A Constrained constrained model is a specialization of Diagonal which allows some or all of the sigmas to be zero, forcing the error to be zero there. More...
 
class  gtsam::noiseModel::Isotropic
 An isotropic noise model corresponds to a scaled diagonal covariance To construct, use one of the static methods. More...
 
class  gtsam::noiseModel::Unit
 Unit: i.i.d. More...
 
class  gtsam::noiseModel::Robust
 Base class for robust error models The robust M-estimators above simply tell us how to re-weight the residual, and are isotropic kernels, in that they do not allow for correlated noise. More...
 
struct  gtsam::traits< noiseModel::Gaussian >
 traits More...
 
struct  gtsam::traits< noiseModel::Diagonal >
 
struct  gtsam::traits< noiseModel::Constrained >
 
struct  gtsam::traits< noiseModel::Isotropic >
 
struct  gtsam::traits< noiseModel::Unit >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
namespace  gtsam::noiseModel
 All noise models live in the noiseModel namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Typedefs

typedef noiseModel::Base::shared_ptr gtsam::SharedNoiseModel
 Aliases.
 
│ │ │ │ -typedef noiseModel::Gaussian::shared_ptr gtsam::SharedGaussian
 
│ │ │ │ -typedef noiseModel::Diagonal::shared_ptr gtsam::SharedDiagonal
 
│ │ │ │ -typedef noiseModel::Constrained::shared_ptr gtsam::SharedConstrained
 
│ │ │ │ -typedef noiseModel::Isotropic::shared_ptr gtsam::SharedIsotropic
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -boost::optional< Vector > gtsam::noiseModel::checkIfDiagonal (const Matrix &M)
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Jan 13, 2010
│ │ │ │ -
Author
Richard Roberts
│ │ │ │ +

Subgraph Solver from IROS 2010.

│ │ │ │ +
Date
2010
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │ -Frank Dellaert
│ │ │ │ +Yong Dian Jian
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,83 +1,23 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -NoiseModel.h File Reference │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e │ │ │ │ │ -  _n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e is the abstract base class for all noise models. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_G_a_u_s_s_i_a_n │ │ │ │ │ - _G_a_u_s_s_i_a_n implements the mathematical model |R*x|^2 = |y|^2 with │ │ │ │ │ -  R'*R=inv(Sigma) where y = whiten(x) = R*x x = unwhiten(x) = inv(R)*y │ │ │ │ │ - as indeed |y|^2 = y'*y = x'*R'*R*x Various derived classes are │ │ │ │ │ - available that are more efficient. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_D_i_a_g_o_n_a_l │ │ │ │ │ -  A diagonal noise model implements a diagonal covariance matrix, with │ │ │ │ │ - the elements of the diagonal specified in a Vector. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_C_o_n_s_t_r_a_i_n_e_d │ │ │ │ │ - A _C_o_n_s_t_r_a_i_n_e_d constrained model is a specialization of _D_i_a_g_o_n_a_l which │ │ │ │ │ -  allows some or all of the sigmas to be zero, forcing the error to be │ │ │ │ │ - zero there. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c │ │ │ │ │ -  An isotropic noise model corresponds to a scaled diagonal covariance │ │ │ │ │ - To construct, use one of the static methods. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t │ │ │ │ │ -  _U_n_i_t: i.i.d. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t │ │ │ │ │ - _B_a_s_e class for robust error models The robust M-estimators above │ │ │ │ │ -  simply tell us how to re-weight the residual, and are isotropic │ │ │ │ │ - kernels, in that they do not allow for correlated noise. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _n_o_i_s_e_M_o_d_e_l_:_:_G_a_u_s_s_i_a_n_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _n_o_i_s_e_M_o_d_e_l_:_:_D_i_a_g_o_n_a_l_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _n_o_i_s_e_M_o_d_e_l_:_:_C_o_n_s_t_r_a_i_n_e_d_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_ _> │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +SubgraphSolver.cpp File Reference │ │ │ │ │ +Subgraph Solver from IROS 2010. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -namespace   _g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l │ │ │ │ │ -  All noise models live in the _n_o_i_s_e_M_o_d_e_l namespace. │ │ │ │ │ -  │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ - typedef noiseModel::Base::shared_ptr  _g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -  Aliases. │ │ │ │ │ -  │ │ │ │ │ - typedef noiseModel::Gaussian::shared_ptr  ggttssaamm::::SShhaarreeddGGaauussssiiaann │ │ │ │ │ -  │ │ │ │ │ - typedef noiseModel::Diagonal::shared_ptr  ggttssaamm::::SShhaarreeddDDiiaaggoonnaall │ │ │ │ │ -  │ │ │ │ │ -typedef noiseModel::Constrained::shared_ptr  ggttssaamm::::SShhaarreeddCCoonnssttrraaiinneedd │ │ │ │ │ -  │ │ │ │ │ - typedef noiseModel::Isotropic::shared_ptr  ggttssaamm::::SShhaarreeddIIssoottrrooppiicc │ │ │ │ │ -  │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -boost::optional< Vector >  ggttssaamm::::nnooiisseeMMooddeell::::cchheecckkIIffDDiiaaggoonnaall (const Matrix &M) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Subgraph Solver from IROS 2010. │ │ │ │ │ Date │ │ │ │ │ - Jan 13, 2010 │ │ │ │ │ + 2010 │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Yong Dian Jian │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _N_o_i_s_e_M_o_d_e_l_._h │ │ │ │ │ + * _S_u_b_g_r_a_p_h_S_o_l_v_e_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00758.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/PowerMethod.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianJunctionTree.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,45 +94,34 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
PowerMethod.h File Reference
│ │ │ │ +
GaussianJunctionTree.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -

Power method for fast eigenvalue and eigenvector computation. │ │ │ │ -More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::PowerMethod< Operator >
 Compute maximum Eigenpair with power method. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Power method for fast eigenvalue and eigenvector computation.

│ │ │ │ -
Date
Sept 2020
│ │ │ │ -
Author
Jing Wu
│ │ │ │ +
Date
Mar 29, 2013
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,27 +1,21 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -PowerMethod.h File Reference │ │ │ │ │ -Power method for fast eigenvalue and eigenvector computation. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_P_o_w_e_r_M_e_t_h_o_d_<_ _O_p_e_r_a_t_o_r_ _> │ │ │ │ │ -  Compute maximum Eigenpair with power method. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +GaussianJunctionTree.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Power method for fast eigenvalue and eigenvector computation. │ │ │ │ │ Date │ │ │ │ │ - Sept 2020 │ │ │ │ │ + Mar 29, 2013 │ │ │ │ │ Author │ │ │ │ │ - Jing Wu │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _P_o_w_e_r_M_e_t_h_o_d_._h │ │ │ │ │ + * _G_a_u_s_s_i_a_n_J_u_n_c_t_i_o_n_T_r_e_e_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00761.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/JacobianFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/HessianFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,55 +94,49 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ +Typedefs | │ │ │ │ Functions
│ │ │ │ -
JacobianFactor.h File Reference
│ │ │ │ +
HessianFactor.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::JacobianFactor
 A Gaussian factor in the squared-error form. More...
 
struct  gtsam::traits< JacobianFactor >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Typedefs

│ │ │ │ +using gtsam::Dims = std::vector< Key >
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > gtsam::EliminateQR (const GaussianFactorGraph &factors, const Ordering &keys)
 Multiply all factors and eliminate the given keys from the resulting factor using a QR variant that handles constraints (zero sigmas).
 
std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< HessianFactor > > gtsam::EliminateCholesky (const GaussianFactorGraph &factors, const Ordering &keys)
 Densely partially eliminate with Cholesky factorization.
 
std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< GaussianFactor > > gtsam::EliminatePreferCholesky (const GaussianFactorGraph &factors, const Ordering &keys)
 Densely partially eliminate with Cholesky factorization.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
│ │ │ │ -Christian Potthast
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ +
Author
Richard Roberts
│ │ │ │
Date
Dec 8, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,39 +1,36 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -JacobianFactor.h File Reference │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ -  A Gaussian factor in the squared-error form. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _J_a_c_o_b_i_a_n_F_a_c_t_o_r_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +HessianFactor.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +using  ggttssaamm::::DDiimmss = std::vector< _K_e_y > │ │ │ │ │ +  │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ - std::pair< _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_: _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_Q_R (const │ │ │ │ │ -_s_h_a_r_e_d___p_t_r, _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &factors, const │ │ │ │ │ - >  _O_r_d_e_r_i_n_g &keys) │ │ │ │ │ - Multiply all factors and eliminate the │ │ │ │ │ -  given keys from the resulting factor │ │ │ │ │ - using a QR variant that handles │ │ │ │ │ - constraints (zero sigmas). │ │ │ │ │ + std::pair< boost::shared_ptr< _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_C_h_o_l_e_s_k_y (const │ │ │ │ │ + _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l >, boost:: _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &factors, const │ │ │ │ │ + shared_ptr< _H_e_s_s_i_a_n_F_a_c_t_o_r > >  _O_r_d_e_r_i_n_g &keys) │ │ │ │ │ +  Densely partially eliminate with Cholesky │ │ │ │ │ + factorization. │ │ │ │ │ +  │ │ │ │ │ + std::pair< boost::shared_ptr< _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_P_r_e_f_e_r_C_h_o_l_e_s_k_y (const │ │ │ │ │ + _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l >, boost:: _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &factors, const │ │ │ │ │ +shared_ptr< _G_a_u_s_s_i_a_n_F_a_c_t_o_r > >  _O_r_d_e_r_i_n_g &keys) │ │ │ │ │ +  Densely partially eliminate with Cholesky │ │ │ │ │ + factorization. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Author │ │ │ │ │ Richard Roberts │ │ │ │ │ - Christian Potthast │ │ │ │ │ - Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ Dec 8, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ + * _H_e_s_s_i_a_n_F_a_c_t_o_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00761.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,5 +1,4 @@ │ │ │ │ │ var a00761 = [ │ │ │ │ │ - ["gtsam::JacobianFactor", "a03844.html", "a03844"], │ │ │ │ │ - ["gtsam::traits< JacobianFactor >", "a03848.html", null], │ │ │ │ │ - ["EliminateQR", "a00761.html#ab56c4bfa363f8c23c48eb078e9c84a9c", null] │ │ │ │ │ + ["EliminateCholesky", "a00761.html#ga37760d3ca31c6d8d60a3a2a17babc3a4", null], │ │ │ │ │ + ["EliminatePreferCholesky", "a00761.html#gadbb147d2a9039f67ad3b8b5515d2e5cc", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00764.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Scatter.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Sampler.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,48 +96,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
Scatter.h File Reference
│ │ │ │ +
Sampler.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Maps global variable indices to slot indices. │ │ │ │ +

sampling from a NoiseModel │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

struct  gtsam::SlotEntry
 One SlotEntry stores the slot index for a variable, as well its dim. More...
 
class  gtsam::Scatter
 Scatter is an intermediate data structure used when building a HessianFactor incrementally, to get the keys in the right order. More...
class  gtsam::Sampler
 Sampling structure that keeps internal random number generators for diagonal distributions specified by NoiseModel. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Maps global variable indices to slot indices.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ +

sampling from a NoiseModel

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │ -Frank Dellaert
│ │ │ │ -
Date
June 2015
│ │ │ │ +Alex Cunningham
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,34 +1,27 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Scatter.h File Reference │ │ │ │ │ -Maps global variable indices to slot indices. _M_o_r_e_._._. │ │ │ │ │ +Sampler.h File Reference │ │ │ │ │ +sampling from a NoiseModel _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_S_l_o_t_E_n_t_r_y │ │ │ │ │ -  One _S_l_o_t_E_n_t_r_y stores the slot index for a variable, as well its dim. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_S_c_a_t_t_e_r │ │ │ │ │ - _S_c_a_t_t_e_r is an intermediate data structure used when building a │ │ │ │ │ -  _H_e_s_s_i_a_n_F_a_c_t_o_r incrementally, to get the keys in the right order. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_S_a_m_p_l_e_r │ │ │ │ │ +  Sampling structure that keeps internal random number generators for │ │ │ │ │ + diagonal distributions specified by NoiseModel. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Maps global variable indices to slot indices. │ │ │ │ │ +sampling from a NoiseModel │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - June 2015 │ │ │ │ │ + Alex Cunningham │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _S_c_a_t_t_e_r_._h │ │ │ │ │ + * _S_a_m_p_l_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00764.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,3 @@ │ │ │ │ │ var a00764 = [ │ │ │ │ │ - ["gtsam::SlotEntry", "a04024.html", null], │ │ │ │ │ - ["gtsam::Scatter", "a04028.html", "a04028"] │ │ │ │ │ + ["gtsam::Sampler", "a04020.html", "a04020"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00764_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Scatter.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Sampler.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,84 +98,79 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Scatter.h
│ │ │ │ +
Sampler.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/inference/Key.h>
│ │ │ │ -
23#include <gtsam/base/FastMap.h>
│ │ │ │ -
24#include <gtsam/dllexport.h>
│ │ │ │ -
25
│ │ │ │ -
26namespace gtsam {
│ │ │ │ -
27
│ │ │ │ -
28class GaussianFactorGraph;
│ │ │ │ -
29class Ordering;
│ │ │ │ -
30
│ │ │ │ -
│ │ │ │ -
32struct GTSAM_EXPORT SlotEntry {
│ │ │ │ -
33 Key key;
│ │ │ │ -
34 size_t dimension;
│ │ │ │ -
35 SlotEntry(Key _key, size_t _dimension) : key(_key), dimension(_dimension) {}
│ │ │ │ -
36 std::string toString() const;
│ │ │ │ -
37 friend bool operator<(const SlotEntry& p, const SlotEntry& q) {
│ │ │ │ -
38 return p.key < q.key;
│ │ │ │ -
39 }
│ │ │ │ -
40 static bool Zero(const SlotEntry& p) { return p.dimension==0;}
│ │ │ │ -
41};
│ │ │ │ -
│ │ │ │ -
42
│ │ │ │ -
│ │ │ │ -
49class Scatter : public FastVector<SlotEntry> {
│ │ │ │ -
50 public:
│ │ │ │ -
52 GTSAM_EXPORT Scatter() {}
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/linear/NoiseModel.h>
│ │ │ │ +
22
│ │ │ │ +
23#include <random>
│ │ │ │ +
24
│ │ │ │ +
25namespace gtsam {
│ │ │ │ +
26
│ │ │ │ +
│ │ │ │ +
31class GTSAM_EXPORT Sampler {
│ │ │ │ +
32 protected:
│ │ │ │ +
34 noiseModel::Diagonal::shared_ptr model_;
│ │ │ │ +
35
│ │ │ │ +
37 mutable std::mt19937_64 generator_;
│ │ │ │ +
38
│ │ │ │ +
39 public:
│ │ │ │ +
40 typedef boost::shared_ptr<Sampler> shared_ptr;
│ │ │ │ +
41
│ │ │ │ +
44
│ │ │ │ +
51 explicit Sampler(const noiseModel::Diagonal::shared_ptr& model,
│ │ │ │ +
52 uint_fast64_t seed = 42u);
│ │ │ │
53
│ │ │ │ -
55 GTSAM_EXPORT explicit Scatter(const GaussianFactorGraph& gfg);
│ │ │ │ -
56
│ │ │ │ -
58 GTSAM_EXPORT explicit Scatter(const GaussianFactorGraph& gfg, const Ordering& ordering);
│ │ │ │ -
59
│ │ │ │ -
61 GTSAM_EXPORT void add(Key key, size_t dim);
│ │ │ │ -
62
│ │ │ │ -
63 private:
│ │ │ │ -
65 iterator find(Key key);
│ │ │ │ -
66};
│ │ │ │ -
│ │ │ │ +
60 explicit Sampler(const Vector& sigmas, uint_fast64_t seed = 42u);
│ │ │ │ +
61
│ │ │ │ +
65
│ │ │ │ +
66 size_t dim() const { return model_->dim(); }
│ │ │ │
67
│ │ │ │ -
68} // \ namespace gtsam
│ │ │ │ -
FastMap.h
A thin wrapper around std::map that uses boost's fast_pool_allocator.
│ │ │ │ - │ │ │ │ -
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │ +
68 Vector sigmas() const { return model_->sigmas(); }
│ │ │ │ +
69
│ │ │ │ +
70 const noiseModel::Diagonal::shared_ptr& model() const { return model_; }
│ │ │ │ +
71
│ │ │ │ +
75
│ │ │ │ +
77 Vector sample() const;
│ │ │ │ +
78
│ │ │ │ +
80 static Vector sampleDiagonal(const Vector& sigmas, std::mt19937_64* rng);
│ │ │ │ +
82
│ │ │ │ +
83 protected:
│ │ │ │ +
85 Vector sampleDiagonal(const Vector& sigmas) const;
│ │ │ │ +
86};
│ │ │ │ +
│ │ │ │ +
87
│ │ │ │ +
88} // namespace gtsam
│ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ -
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ -
gtsam::SlotEntry
One SlotEntry stores the slot index for a variable, as well its dim.
Definition Scatter.h:32
│ │ │ │ -
gtsam::Scatter
Scatter is an intermediate data structure used when building a HessianFactor incrementally,...
Definition Scatter.h:49
│ │ │ │ -
gtsam::Scatter::add
GTSAM_EXPORT void add(Key key, size_t dim)
Add a key/dim pair.
Definition Scatter.cpp:76
│ │ │ │ -
gtsam::Scatter::Scatter
GTSAM_EXPORT Scatter()
Default Constructor.
Definition Scatter.h:52
│ │ │ │ +
gtsam::Sampler
Sampling structure that keeps internal random number generators for diagonal distributions specified ...
Definition Sampler.h:31
│ │ │ │ +
gtsam::Sampler::model_
noiseModel::Diagonal::shared_ptr model_
noiseModel created at generation
Definition Sampler.h:34
│ │ │ │ +
gtsam::Sampler::generator_
std::mt19937_64 generator_
generator
Definition Sampler.h:37
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,97 +1,80 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Scatter.h │ │ │ │ │ +Sampler.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_M_a_p_._h> │ │ │ │ │ -24#include │ │ │ │ │ -25 │ │ │ │ │ -26namespace _g_t_s_a_m { │ │ │ │ │ -27 │ │ │ │ │ -28class GaussianFactorGraph; │ │ │ │ │ -29class Ordering; │ │ │ │ │ -30 │ │ │ │ │ -_3_2struct GTSAM_EXPORT _S_l_o_t_E_n_t_r_y { │ │ │ │ │ -33 _K_e_y key; │ │ │ │ │ -34 size_t dimension; │ │ │ │ │ -35 _S_l_o_t_E_n_t_r_y(_K_e_y _key, size_t _dimension) : key(_key), dimension(_dimension) {} │ │ │ │ │ -36 std::string toString() const; │ │ │ │ │ -37 friend bool operator<(const _S_l_o_t_E_n_t_r_y& p, const _S_l_o_t_E_n_t_r_y& q) { │ │ │ │ │ -38 return p.key < q.key; │ │ │ │ │ -39 } │ │ │ │ │ -40 static bool Zero(const _S_l_o_t_E_n_t_r_y& p) { return p.dimension==0;} │ │ │ │ │ -41}; │ │ │ │ │ -42 │ │ │ │ │ -_4_9class _S_c_a_t_t_e_r : public _F_a_s_t_V_e_c_t_o_r { │ │ │ │ │ -50 public: │ │ │ │ │ -_5_2 GTSAM_EXPORT _S_c_a_t_t_e_r() {} │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_N_o_i_s_e_M_o_d_e_l_._h> │ │ │ │ │ +22 │ │ │ │ │ +23#include │ │ │ │ │ +24 │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ +26 │ │ │ │ │ +_3_1class GTSAM_EXPORT _S_a_m_p_l_e_r { │ │ │ │ │ +32 protected: │ │ │ │ │ +_3_4 noiseModel::Diagonal::shared_ptr _m_o_d_e_l__; │ │ │ │ │ +35 │ │ │ │ │ +_3_7 mutable std::mt19937_64 _g_e_n_e_r_a_t_o_r__; │ │ │ │ │ +38 │ │ │ │ │ +39 public: │ │ │ │ │ +40 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +41 │ │ │ │ │ +44 │ │ │ │ │ +51 explicit _S_a_m_p_l_e_r(const noiseModel::Diagonal::shared_ptr& model, │ │ │ │ │ +52 uint_fast64_t seed = 42u); │ │ │ │ │ 53 │ │ │ │ │ -55 GTSAM_EXPORT explicit _S_c_a_t_t_e_r(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& gfg); │ │ │ │ │ -56 │ │ │ │ │ -58 GTSAM_EXPORT explicit _S_c_a_t_t_e_r(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& gfg, const │ │ │ │ │ -_O_r_d_e_r_i_n_g& ordering); │ │ │ │ │ -59 │ │ │ │ │ -61 GTSAM_EXPORT void _a_d_d(_K_e_y key, size_t dim); │ │ │ │ │ -62 │ │ │ │ │ -63 private: │ │ │ │ │ -65 iterator find(_K_e_y key); │ │ │ │ │ -66}; │ │ │ │ │ +60 explicit _S_a_m_p_l_e_r(const Vector& sigmas, uint_fast64_t seed = 42u); │ │ │ │ │ +61 │ │ │ │ │ +65 │ │ │ │ │ +66 size_t dim() const { return model_->dim(); } │ │ │ │ │ 67 │ │ │ │ │ -68} // \ namespace gtsam │ │ │ │ │ -_F_a_s_t_M_a_p_._h │ │ │ │ │ -A thin wrapper around std::map that uses boost's fast_pool_allocator. │ │ │ │ │ -_K_e_y_._h │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ -std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ -FastVector │ │ │ │ │ -FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ -DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ +68 Vector sigmas() const { return model_->sigmas(); } │ │ │ │ │ +69 │ │ │ │ │ +70 const noiseModel::Diagonal::shared_ptr& model() const { return model_; } │ │ │ │ │ +71 │ │ │ │ │ +75 │ │ │ │ │ +77 Vector sample() const; │ │ │ │ │ +78 │ │ │ │ │ +80 static Vector sampleDiagonal(const Vector& sigmas, std::mt19937_64* rng); │ │ │ │ │ +82 │ │ │ │ │ +83 protected: │ │ │ │ │ +85 Vector sampleDiagonal(const Vector& sigmas) const; │ │ │ │ │ +86}; │ │ │ │ │ +87 │ │ │ │ │ +88} // namespace gtsam │ │ │ │ │ +_N_o_i_s_e_M_o_d_e_l_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_S_l_o_t_E_n_t_r_y │ │ │ │ │ -One SlotEntry stores the slot index for a variable, as well its dim. │ │ │ │ │ -DDeeffiinniittiioonn Scatter.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_S_c_a_t_t_e_r │ │ │ │ │ -Scatter is an intermediate data structure used when building a HessianFactor │ │ │ │ │ -incrementally,... │ │ │ │ │ -DDeeffiinniittiioonn Scatter.h:49 │ │ │ │ │ -_g_t_s_a_m_:_:_S_c_a_t_t_e_r_:_:_a_d_d │ │ │ │ │ -GTSAM_EXPORT void add(Key key, size_t dim) │ │ │ │ │ -Add a key/dim pair. │ │ │ │ │ -DDeeffiinniittiioonn Scatter.cpp:76 │ │ │ │ │ -_g_t_s_a_m_:_:_S_c_a_t_t_e_r_:_:_S_c_a_t_t_e_r │ │ │ │ │ -GTSAM_EXPORT Scatter() │ │ │ │ │ -Default Constructor. │ │ │ │ │ -DDeeffiinniittiioonn Scatter.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_S_a_m_p_l_e_r │ │ │ │ │ +Sampling structure that keeps internal random number generators for diagonal │ │ │ │ │ +distributions specified ... │ │ │ │ │ +DDeeffiinniittiioonn Sampler.h:31 │ │ │ │ │ +_g_t_s_a_m_:_:_S_a_m_p_l_e_r_:_:_m_o_d_e_l__ │ │ │ │ │ +noiseModel::Diagonal::shared_ptr model_ │ │ │ │ │ +noiseModel created at generation │ │ │ │ │ +DDeeffiinniittiioonn Sampler.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_S_a_m_p_l_e_r_:_:_g_e_n_e_r_a_t_o_r__ │ │ │ │ │ +std::mt19937_64 generator_ │ │ │ │ │ +generator │ │ │ │ │ +DDeeffiinniittiioonn Sampler.h:37 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _S_c_a_t_t_e_r_._h │ │ │ │ │ + * _S_a_m_p_l_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00767.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/SparseEigen.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianConditional.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,62 +94,35 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
│ │ │ │ -
SparseEigen.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
GaussianConditional.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Utilities for creating Eigen sparse matrices (gtsam::SparseEigen) │ │ │ │ +

Conditional Gaussian Base class. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Typedefs

typedef Eigen::SparseMatrix< double, Eigen::ColMajor, int > gtsam::SparseEigen
 Eigen-format sparse matrix.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -SparseEigen gtsam::sparseJacobianEigen (const GaussianFactorGraph &gfg, const Ordering &ordering)
 Constructs an Eigen-format SparseMatrix of a GaussianFactorGraph.
 
│ │ │ │ -SparseEigen gtsam::sparseJacobianEigen (const GaussianFactorGraph &gfg)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Utilities for creating Eigen sparse matrices (gtsam::SparseEigen)

│ │ │ │ -
Date
Aug 2019
│ │ │ │ -
Author
Mandy Xie
│ │ │ │ -
│ │ │ │ -Fan Jiang
│ │ │ │ -
│ │ │ │ -Gerry Chen
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ +

Conditional Gaussian Base class.

│ │ │ │ +
Author
Christian Potthast, Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,38 +1,20 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -SparseEigen.h File Reference │ │ │ │ │ -Utilities for creating Eigen sparse matrices (_g_t_s_a_m_:_:_S_p_a_r_s_e_E_i_g_e_n) _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +GaussianConditional.cpp File Reference │ │ │ │ │ +Conditional Gaussian Base class. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -typedef Eigen::SparseMatrix< double, Eigen:: │ │ │ │ │ - ColMajor, int >  _g_t_s_a_m_:_:_S_p_a_r_s_e_E_i_g_e_n │ │ │ │ │ -  Eigen-format sparse matrix. │ │ │ │ │ -  │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -_S_p_a_r_s_e_E_i_g_e_n  ggttssaamm::::ssppaarrsseeJJaaccoobbiiaannEEiiggeenn (const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, const │ │ │ │ │ - _O_r_d_e_r_i_n_g &ordering) │ │ │ │ │ -  Constructs an Eigen-format SparseMatrix of a _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h. │ │ │ │ │ -  │ │ │ │ │ -_S_p_a_r_s_e_E_i_g_e_n  ggttssaamm::::ssppaarrsseeJJaaccoobbiiaannEEiiggeenn (const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Utilities for creating Eigen sparse matrices (_g_t_s_a_m_:_:_S_p_a_r_s_e_E_i_g_e_n) │ │ │ │ │ - Date │ │ │ │ │ - Aug 2019 │ │ │ │ │ +Conditional Gaussian Base class. │ │ │ │ │ Author │ │ │ │ │ - Mandy Xie │ │ │ │ │ - Fan Jiang │ │ │ │ │ - Gerry Chen │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Christian Potthast, Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _S_p_a_r_s_e_E_i_g_e_n_._h │ │ │ │ │ + * _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00773.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianBayesTree.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianConditional.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,50 +96,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
GaussianBayesTree.h File Reference
│ │ │ │ +
GaussianConditional.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ +

Conditional Gaussian Base class. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::GaussianBayesTreeClique
 A clique in a GaussianBayesTree. More...
class  gtsam::GaussianConditional
 A GaussianConditional functions as the node in a Bayes network. More...
 
class  gtsam::GaussianBayesTree
 A Bayes tree representing a Gaussian density. More...
 
struct  gtsam::traits< GaussianBayesTree >
 traits More...
struct  gtsam::traits< GaussianConditional >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.

│ │ │ │ -

GaussianBayesTree

Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ +

Conditional Gaussian Base class.

│ │ │ │ +
Author
Christian Potthast
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,33 +1,29 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -GaussianBayesTree.h File Reference │ │ │ │ │ -Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. _M_o_r_e_._._. │ │ │ │ │ +GaussianConditional.h File Reference │ │ │ │ │ +Conditional Gaussian Base class. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e │ │ │ │ │ -  A clique in a _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +  A _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l functions as the node in a Bayes network. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ - class   _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e │ │ │ │ │ -  A Bayes tree representing a Gaussian density. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_ _> │ │ │ │ │   traits _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ │ -GaussianBayesTree │ │ │ │ │ +Conditional Gaussian Base class. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Christian Potthast │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ + * _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00773.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,5 +1,3 @@ │ │ │ │ │ var a00773 = [ │ │ │ │ │ - ["gtsam::GaussianBayesTreeClique", "a03752.html", null], │ │ │ │ │ - ["gtsam::GaussianBayesTree", "a03756.html", "a03756"], │ │ │ │ │ - ["gtsam::traits< GaussianBayesTree >", "a03760.html", null] │ │ │ │ │ + ["gtsam::traits< GaussianConditional >", "a03768.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00773_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianBayesTree.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianConditional.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,120 +98,224 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
GaussianBayesTree.h
│ │ │ │ +
GaussianConditional.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ +
18// \callgraph
│ │ │ │ +
19
│ │ │ │
20#pragma once
│ │ │ │
21
│ │ │ │ -
22#include <gtsam/linear/GaussianBayesNet.h>
│ │ │ │ -
23#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ -
24#include <gtsam/inference/BayesTree.h>
│ │ │ │ -
25#include <gtsam/inference/BayesTreeCliqueBase.h>
│ │ │ │ -
26
│ │ │ │ -
27namespace gtsam {
│ │ │ │ -
28
│ │ │ │ -
29 // Forward declarations
│ │ │ │ -
30 class GaussianConditional;
│ │ │ │ -
31 class VectorValues;
│ │ │ │ -
32
│ │ │ │ -
33 /* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
35 class GTSAM_EXPORT GaussianBayesTreeClique :
│ │ │ │ -
36 public BayesTreeCliqueBase<GaussianBayesTreeClique, GaussianFactorGraph>
│ │ │ │ -
37 {
│ │ │ │ -
38 public:
│ │ │ │ -
39 typedef GaussianBayesTreeClique This;
│ │ │ │ -
40 typedef BayesTreeCliqueBase<GaussianBayesTreeClique, GaussianFactorGraph> Base;
│ │ │ │ -
41 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
42 typedef boost::weak_ptr<This> weak_ptr;
│ │ │ │ -
43 GaussianBayesTreeClique() {}
│ │ │ │ -
44 virtual ~GaussianBayesTreeClique() {}
│ │ │ │ -
45 GaussianBayesTreeClique(const boost::shared_ptr<GaussianConditional>& conditional) : Base(conditional) {}
│ │ │ │ -
46 };
│ │ │ │ -
│ │ │ │ -
47
│ │ │ │ -
48 /* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
50 class GTSAM_EXPORT GaussianBayesTree :
│ │ │ │ -
51 public BayesTree<GaussianBayesTreeClique>
│ │ │ │ -
52 {
│ │ │ │ -
53 private:
│ │ │ │ -
54 typedef BayesTree<GaussianBayesTreeClique> Base;
│ │ │ │ +
22#include <boost/utility.hpp>
│ │ │ │ +
23
│ │ │ │ +
24#include <gtsam/global_includes.h>
│ │ │ │ +
25#include <gtsam/linear/JacobianFactor.h>
│ │ │ │ +
26#include <gtsam/inference/Conditional.h>
│ │ │ │ +
27#include <gtsam/inference/Conditional-inst.h>
│ │ │ │ +
28#include <gtsam/linear/VectorValues.h>
│ │ │ │ +
29
│ │ │ │ +
30#include <random> // for std::mt19937_64
│ │ │ │ +
31
│ │ │ │ +
32namespace gtsam {
│ │ │ │ +
33
│ │ │ │ +
│ │ │ │ +
40 class GTSAM_EXPORT GaussianConditional :
│ │ │ │ +
41 public JacobianFactor,
│ │ │ │ +
42 public Conditional<JacobianFactor, GaussianConditional>
│ │ │ │ +
43 {
│ │ │ │ +
44 public:
│ │ │ │ +
45 typedef GaussianConditional This;
│ │ │ │ +
46 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
47 typedef JacobianFactor BaseFactor;
│ │ │ │ +
48 typedef Conditional<BaseFactor, This> BaseConditional;
│ │ │ │ +
49
│ │ │ │ +
52
│ │ │ │ +
54 GaussianConditional() {}
│ │ │ │
55
│ │ │ │ -
56 public:
│ │ │ │ -
57 typedef GaussianBayesTree This;
│ │ │ │ -
58 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
57 GaussianConditional(Key key, const Vector& d, const Matrix& R,
│ │ │ │ +
58 const SharedDiagonal& sigmas = SharedDiagonal());
│ │ │ │
59
│ │ │ │ -
61 GaussianBayesTree() {}
│ │ │ │ -
62
│ │ │ │ -
64 bool equals(const This& other, double tol = 1e-9) const;
│ │ │ │ -
65
│ │ │ │ -
67 VectorValues optimize() const;
│ │ │ │ -
68
│ │ │ │ -
94 VectorValues optimizeGradientSearch() const;
│ │ │ │ +
61 GaussianConditional(Key key, const Vector& d, const Matrix& R, Key parent1,
│ │ │ │ +
62 const Matrix& S,
│ │ │ │ +
63 const SharedDiagonal& sigmas = SharedDiagonal());
│ │ │ │ +
64
│ │ │ │ +
66 GaussianConditional(Key key, const Vector& d, const Matrix& R, Key parent1,
│ │ │ │ +
67 const Matrix& S, Key parent2, const Matrix& T,
│ │ │ │ +
68 const SharedDiagonal& sigmas = SharedDiagonal());
│ │ │ │ +
69
│ │ │ │ +
73 template<typename TERMS>
│ │ │ │ +
74 GaussianConditional(const TERMS& terms,
│ │ │ │ +
75 size_t nrFrontals, const Vector& d,
│ │ │ │ +
76 const SharedDiagonal& sigmas = SharedDiagonal());
│ │ │ │ +
77
│ │ │ │ +
82 template<typename KEYS>
│ │ │ │ +
83 GaussianConditional(
│ │ │ │ +
84 const KEYS& keys, size_t nrFrontals, const VerticalBlockMatrix& augmentedMatrix,
│ │ │ │ +
85 const SharedDiagonal& sigmas = SharedDiagonal());
│ │ │ │ +
86
│ │ │ │ +
88 static GaussianConditional FromMeanAndStddev(Key key, const Vector& mu,
│ │ │ │ +
89 double sigma);
│ │ │ │ +
90
│ │ │ │ +
92 static GaussianConditional FromMeanAndStddev(Key key, const Matrix& A,
│ │ │ │ +
93 Key parent, const Vector& b,
│ │ │ │ +
94 double sigma);
│ │ │ │
95
│ │ │ │ -
101 VectorValues gradient(const VectorValues& x0) const;
│ │ │ │ +
98 static GaussianConditional FromMeanAndStddev(Key key, //
│ │ │ │ +
99 const Matrix& A1, Key parent1,
│ │ │ │ +
100 const Matrix& A2, Key parent2,
│ │ │ │ +
101 const Vector& b, double sigma);
│ │ │ │
102
│ │ │ │ -
108 VectorValues gradientAtZero() const;
│ │ │ │ -
109
│ │ │ │ -
111 double error(const VectorValues& x) const;
│ │ │ │ -
112
│ │ │ │ -
118 double determinant() const;
│ │ │ │ -
119
│ │ │ │ -
125 double logDeterminant() const;
│ │ │ │ -
126
│ │ │ │ -
129 Matrix marginalCovariance(Key key) const;
│ │ │ │ -
130 };
│ │ │ │ -
│ │ │ │ -
131
│ │ │ │ -
133 template<>
│ │ │ │ -
│ │ │ │ -
134 struct traits<GaussianBayesTree> : public Testable<GaussianBayesTree> {
│ │ │ │ -
135 };
│ │ │ │ -
│ │ │ │ -
136
│ │ │ │ -
137} //\ namespace gtsam
│ │ │ │ -
BayesTree.h
Bayes Tree is a tree of cliques of a Bayes Chain.
│ │ │ │ -
BayesTreeCliqueBase.h
Base class for cliques of a BayesTree.
│ │ │ │ -
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
│ │ │ │ -
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ +
104 template<typename... Args>
│ │ │ │ +
│ │ │ │ +
105 static shared_ptr sharedMeanAndStddev(Args&&... args) {
│ │ │ │ +
106 return boost::make_shared<This>(FromMeanAndStddev(std::forward<Args>(args)...));
│ │ │ │ +
107 }
│ │ │ │ +
│ │ │ │ +
108
│ │ │ │ +
116 template<typename ITERATOR>
│ │ │ │ +
117 static shared_ptr Combine(ITERATOR firstConditional, ITERATOR lastConditional);
│ │ │ │ +
118
│ │ │ │ +
122
│ │ │ │ +
124 void print(
│ │ │ │ +
125 const std::string& = "GaussianConditional",
│ │ │ │ +
126 const KeyFormatter& formatter = DefaultKeyFormatter) const override;
│ │ │ │ +
127
│ │ │ │ +
129 bool equals(const GaussianFactor&cg, double tol = 1e-9) const override;
│ │ │ │ +
130
│ │ │ │ +
134
│ │ │ │ +
139 double logNormalizationConstant() const override;
│ │ │ │ +
140
│ │ │ │ +
148 double logProbability(const VectorValues& x) const;
│ │ │ │ +
149
│ │ │ │ +
155 double evaluate(const VectorValues& x) const;
│ │ │ │ +
156
│ │ │ │ +
│ │ │ │ +
158 double operator()(const VectorValues& x) const {
│ │ │ │ +
159 return evaluate(x);
│ │ │ │ +
160 }
│ │ │ │ +
│ │ │ │ +
161
│ │ │ │ +
175 VectorValues solve(const VectorValues& parents) const;
│ │ │ │ +
176
│ │ │ │ +
177 VectorValues solveOtherRHS(const VectorValues& parents, const VectorValues& rhs) const;
│ │ │ │ +
178
│ │ │ │ +
180 void solveTransposeInPlace(VectorValues& gy) const;
│ │ │ │ +
181
│ │ │ │ +
183 JacobianFactor::shared_ptr likelihood(
│ │ │ │ +
184 const VectorValues& frontalValues) const;
│ │ │ │ +
185
│ │ │ │ +
187 JacobianFactor::shared_ptr likelihood(const Vector& frontal) const;
│ │ │ │ +
188
│ │ │ │ +
195 VectorValues sample(std::mt19937_64* rng) const;
│ │ │ │ +
196
│ │ │ │ +
204 VectorValues sample(const VectorValues& parentsValues,
│ │ │ │ +
205 std::mt19937_64* rng) const;
│ │ │ │ +
206
│ │ │ │ +
208 VectorValues sample() const;
│ │ │ │ +
209
│ │ │ │ +
211 VectorValues sample(const VectorValues& parentsValues) const;
│ │ │ │ +
212
│ │ │ │ +
216
│ │ │ │ +
218 constABlock R() const { return Ab_.range(0, nrFrontals()); }
│ │ │ │ +
219
│ │ │ │ +
221 constABlock S() const { return Ab_.range(nrFrontals(), size()); }
│ │ │ │ +
222
│ │ │ │ +
224 constABlock S(const_iterator it) const { return BaseFactor::getA(it); }
│ │ │ │ +
225
│ │ │ │ +
227 const constBVector d() const { return BaseFactor::getb(); }
│ │ │ │ +
228
│ │ │ │ +
240 inline double determinant() const { return exp(logDeterminant()); }
│ │ │ │ +
241
│ │ │ │ +
253 double logDeterminant() const;
│ │ │ │ +
254
│ │ │ │ +
258
│ │ │ │ +
263 double logProbability(const HybridValues& x) const override;
│ │ │ │ +
264
│ │ │ │ +
269 double evaluate(const HybridValues& x) const override;
│ │ │ │ +
270
│ │ │ │ +
271 using Conditional::operator(); // Expose evaluate(const HybridValues&) method..
│ │ │ │ +
272 using JacobianFactor::error; // Expose error(const HybridValues&) method..
│ │ │ │ +
273
│ │ │ │ +
275
│ │ │ │ +
276
│ │ │ │ +
277#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ +
280
│ │ │ │ +
282 void GTSAM_DEPRECATED scaleFrontalsBySigma(VectorValues& gy) const;
│ │ │ │ +
284#endif
│ │ │ │ +
285
│ │ │ │ +
286 private:
│ │ │ │ +
288 friend class boost::serialization::access;
│ │ │ │ +
289 template<class Archive>
│ │ │ │ +
290 void serialize(Archive & ar, const unsigned int /*version*/) {
│ │ │ │ +
291 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseFactor);
│ │ │ │ +
292 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseConditional);
│ │ │ │ +
293 }
│ │ │ │ +
294 }; // GaussianConditional
│ │ │ │ +
│ │ │ │ +
295
│ │ │ │ +
297template<>
│ │ │ │ +
298struct traits<GaussianConditional> : public Testable<GaussianConditional> {};
│ │ │ │ +
299
│ │ │ │ +
300} // \ namespace gtsam
│ │ │ │ +
301
│ │ │ │ +
302#include <gtsam/linear/GaussianConditional-inl.h>
│ │ │ │ +
303
│ │ │ │ +
Conditional.h
Base class for conditional densities.
│ │ │ │ + │ │ │ │ +
GaussianConditional-inl.h
Conditional Gaussian Base class.
│ │ │ │ +
VectorValues.h
Factor Graph Values.
│ │ │ │ +
global_includes.h
Included from all GTSAM files.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::BayesTree
Bayes tree.
Definition BayesTree.h:67
│ │ │ │ -
gtsam::BayesTreeCliqueBase
This is the base class for BayesTree cliques.
Definition BayesTreeCliqueBase.h:50
│ │ │ │ -
gtsam::GaussianBayesTreeClique
A clique in a GaussianBayesTree.
Definition GaussianBayesTree.h:37
│ │ │ │ -
gtsam::GaussianBayesTree
A Bayes tree representing a Gaussian density.
Definition GaussianBayesTree.h:52
│ │ │ │ -
gtsam::GaussianBayesTree::GaussianBayesTree
GaussianBayesTree()
Default constructor, creates an empty Bayes tree.
Definition GaussianBayesTree.h:61
│ │ │ │ +
gtsam::VerticalBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43
│ │ │ │ +
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ +
gtsam::Conditional
Definition Conditional.h:64
│ │ │ │ +
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition Factor.h:80
│ │ │ │ +
gtsam::GaussianConditional
A GaussianConditional functions as the node in a Bayes network.
Definition GaussianConditional.h:43
│ │ │ │ +
gtsam::GaussianConditional::This
GaussianConditional This
Typedef to this class.
Definition GaussianConditional.h:45
│ │ │ │ +
gtsam::GaussianConditional::S
constABlock S(const_iterator it) const
Get a view of the S matrix for the variable pointed to by the given key iterator.
Definition GaussianConditional.h:224
│ │ │ │ +
gtsam::GaussianConditional::R
constABlock R() const
Return a view of the upper-triangular R block of the conditional.
Definition GaussianConditional.h:218
│ │ │ │ +
gtsam::GaussianConditional::BaseFactor
JacobianFactor BaseFactor
Typedef to our factor base class.
Definition GaussianConditional.h:47
│ │ │ │ +
gtsam::GaussianConditional::GaussianConditional
GaussianConditional()
default constructor needed for serialization
Definition GaussianConditional.h:54
│ │ │ │ +
gtsam::GaussianConditional::BaseConditional
Conditional< BaseFactor, This > BaseConditional
Typedef to our conditional base class.
Definition GaussianConditional.h:48
│ │ │ │ +
gtsam::GaussianConditional::Combine
static shared_ptr Combine(ITERATOR firstConditional, ITERATOR lastConditional)
Combine several GaussianConditional into a single dense GC.
│ │ │ │ +
gtsam::GaussianConditional::determinant
double determinant() const
Compute the determinant of the R matrix.
Definition GaussianConditional.h:240
│ │ │ │ +
gtsam::GaussianConditional::operator()
double operator()(const VectorValues &x) const
Evaluate probability density, sugar.
Definition GaussianConditional.h:158
│ │ │ │ +
gtsam::GaussianConditional::sharedMeanAndStddev
static shared_ptr sharedMeanAndStddev(Args &&... args)
Create shared pointer by forwarding arguments to fromMeanAndStddev.
Definition GaussianConditional.h:105
│ │ │ │ +
gtsam::GaussianConditional::S
constABlock S() const
Get a view of the parent blocks.
Definition GaussianConditional.h:221
│ │ │ │ +
gtsam::GaussianConditional::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianConditional.h:46
│ │ │ │ +
gtsam::GaussianConditional::d
const constBVector d() const
Get a view of the r.h.s.
Definition GaussianConditional.h:227
│ │ │ │ +
gtsam::GaussianFactor
An abstract virtual base class for JacobianFactor and HessianFactor.
Definition GaussianFactor.h:39
│ │ │ │ +
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition JacobianFactor.h:91
│ │ │ │ +
gtsam::JacobianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition JacobianFactor.h:96
│ │ │ │
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ -
GaussianConditional
is the normalization constant.
│ │ │ │ -
VectorValues
The Factor::error simply extracts the.
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,146 +1,298 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GaussianBayesTree.h │ │ │ │ │ +GaussianConditional.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ +18// \callgraph │ │ │ │ │ +19 │ │ │ │ │ 20#pragma once │ │ │ │ │ 21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._h> │ │ │ │ │ -26 │ │ │ │ │ -27namespace _g_t_s_a_m { │ │ │ │ │ -28 │ │ │ │ │ -29 // Forward declarations │ │ │ │ │ -30 class _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l; │ │ │ │ │ -31 class _V_e_c_t_o_r_V_a_l_u_e_s; │ │ │ │ │ -32 │ │ │ │ │ -33 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -_3_5 class GTSAM_EXPORT _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e : │ │ │ │ │ -36 public _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e │ │ │ │ │ -37 { │ │ │ │ │ -38 public: │ │ │ │ │ -39 typedef _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e _T_h_i_s; │ │ │ │ │ -40 typedef _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_,_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_> │ │ │ │ │ -_B_a_s_e; │ │ │ │ │ -41 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -42 typedef boost::weak_ptr weak_ptr; │ │ │ │ │ -43 _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e() {} │ │ │ │ │ -44 virtual _~_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e() {} │ │ │ │ │ -45 _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e(const boost::shared_ptr& │ │ │ │ │ -conditional) : _B_a_s_e(conditional) {} │ │ │ │ │ -46 }; │ │ │ │ │ -47 │ │ │ │ │ -48 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -_5_0 class GTSAM_EXPORT _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e : │ │ │ │ │ -51 public _B_a_y_e_s_T_r_e_e │ │ │ │ │ -52 { │ │ │ │ │ -53 private: │ │ │ │ │ -54 typedef _B_a_y_e_s_T_r_e_e_<_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_> _B_a_s_e; │ │ │ │ │ +22#include │ │ │ │ │ +23 │ │ │ │ │ +24#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ +27#include │ │ │ │ │ +28#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ +29 │ │ │ │ │ +30#include // for std::mt19937_64 │ │ │ │ │ +31 │ │ │ │ │ +32namespace _g_t_s_a_m { │ │ │ │ │ +33 │ │ │ │ │ +_4_0 class GTSAM_EXPORT _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l : │ │ │ │ │ +41 public _J_a_c_o_b_i_a_n_F_a_c_t_o_r, │ │ │ │ │ +42 public _C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +43 { │ │ │ │ │ +44 public: │ │ │ │ │ +_4_5 typedef _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l _T_h_i_s; │ │ │ │ │ +_4_6 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +_4_7 typedef _J_a_c_o_b_i_a_n_F_a_c_t_o_r _B_a_s_e_F_a_c_t_o_r; │ │ │ │ │ +_4_8 typedef _C_o_n_d_i_t_i_o_n_a_l_<_B_a_s_e_F_a_c_t_o_r_,_ _T_h_i_s_> _B_a_s_e_C_o_n_d_i_t_i_o_n_a_l; │ │ │ │ │ +49 │ │ │ │ │ +52 │ │ │ │ │ +_5_4 _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l() {} │ │ │ │ │ 55 │ │ │ │ │ -56 public: │ │ │ │ │ -57 typedef _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e _T_h_i_s; │ │ │ │ │ -58 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +57 _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l(_K_e_y key, const Vector& d, const Matrix& R, │ │ │ │ │ +58 const SharedDiagonal& sigmas = SharedDiagonal()); │ │ │ │ │ 59 │ │ │ │ │ -_6_1 _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e() {} │ │ │ │ │ -62 │ │ │ │ │ -64 bool _e_q_u_a_l_s(const This& other, double tol = 1e-9) const; │ │ │ │ │ -65 │ │ │ │ │ -67 _V_e_c_t_o_r_V_a_l_u_e_s _o_p_t_i_m_i_z_e() const; │ │ │ │ │ -68 │ │ │ │ │ -94 _V_e_c_t_o_r_V_a_l_u_e_s optimizeGradientSearch() const; │ │ │ │ │ +61 _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l(_K_e_y key, const Vector& d, const Matrix& R, _K_e_y parent1, │ │ │ │ │ +62 const Matrix& S, │ │ │ │ │ +63 const SharedDiagonal& sigmas = SharedDiagonal()); │ │ │ │ │ +64 │ │ │ │ │ +66 _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l(_K_e_y key, const Vector& d, const Matrix& R, _K_e_y parent1, │ │ │ │ │ +67 const Matrix& S, _K_e_y parent2, const Matrix& T, │ │ │ │ │ +68 const SharedDiagonal& sigmas = SharedDiagonal()); │ │ │ │ │ +69 │ │ │ │ │ +73 template │ │ │ │ │ +74 _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l(const TERMS& terms, │ │ │ │ │ +75 size_t nrFrontals, const Vector& d, │ │ │ │ │ +76 const SharedDiagonal& sigmas = SharedDiagonal()); │ │ │ │ │ +77 │ │ │ │ │ +82 template │ │ │ │ │ +83 _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l( │ │ │ │ │ +84 const KEYS& keys, size_t nrFrontals, const _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x& │ │ │ │ │ +augmentedMatrix, │ │ │ │ │ +85 const SharedDiagonal& sigmas = SharedDiagonal()); │ │ │ │ │ +86 │ │ │ │ │ +88 static _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l FromMeanAndStddev(_K_e_y key, const Vector& mu, │ │ │ │ │ +89 double sigma); │ │ │ │ │ +90 │ │ │ │ │ +92 static _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l FromMeanAndStddev(_K_e_y key, const Matrix& A, │ │ │ │ │ +93 _K_e_y parent, const Vector& b, │ │ │ │ │ +94 double sigma); │ │ │ │ │ 95 │ │ │ │ │ -101 _V_e_c_t_o_r_V_a_l_u_e_s gradient(const _V_e_c_t_o_r_V_a_l_u_e_s& x0) const; │ │ │ │ │ +98 static _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l FromMeanAndStddev(_K_e_y key, // │ │ │ │ │ +99 const Matrix& A1, _K_e_y parent1, │ │ │ │ │ +100 const Matrix& A2, _K_e_y parent2, │ │ │ │ │ +101 const Vector& b, double sigma); │ │ │ │ │ 102 │ │ │ │ │ -108 _V_e_c_t_o_r_V_a_l_u_e_s gradientAtZero() const; │ │ │ │ │ -109 │ │ │ │ │ -111 double error(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const; │ │ │ │ │ -112 │ │ │ │ │ -118 double determinant() const; │ │ │ │ │ -119 │ │ │ │ │ -125 double logDeterminant() const; │ │ │ │ │ -126 │ │ │ │ │ -129 Matrix marginalCovariance(_K_e_y key) const; │ │ │ │ │ -130 }; │ │ │ │ │ -131 │ │ │ │ │ -133 template<> │ │ │ │ │ -_1_3_4 struct _t_r_a_i_t_s<_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e> : public _T_e_s_t_a_b_l_e { │ │ │ │ │ -135 }; │ │ │ │ │ -136 │ │ │ │ │ -137} //\ namespace gtsam │ │ │ │ │ -_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ -Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ │ -_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._h │ │ │ │ │ -Base class for cliques of a BayesTree. │ │ │ │ │ -_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h │ │ │ │ │ -Chordal Bayes Net, the result of eliminating a factor graph. │ │ │ │ │ -_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ +104 template │ │ │ │ │ +_1_0_5 static _s_h_a_r_e_d___p_t_r _s_h_a_r_e_d_M_e_a_n_A_n_d_S_t_d_d_e_v(Args&&... args) { │ │ │ │ │ +106 return boost::make_shared(FromMeanAndStddev(std::forward │ │ │ │ │ +(args)...)); │ │ │ │ │ +107 } │ │ │ │ │ +108 │ │ │ │ │ +116 template │ │ │ │ │ +_1_1_7 static _s_h_a_r_e_d___p_t_r _C_o_m_b_i_n_e(ITERATOR firstConditional, ITERATOR │ │ │ │ │ +lastConditional); │ │ │ │ │ +118 │ │ │ │ │ +122 │ │ │ │ │ +124 void _p_r_i_n_t( │ │ │ │ │ +125 const std::string& = "GaussianConditional", │ │ │ │ │ +126 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override; │ │ │ │ │ +127 │ │ │ │ │ +129 bool _e_q_u_a_l_s(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r&cg, double tol = 1e-9) const override; │ │ │ │ │ +130 │ │ │ │ │ +134 │ │ │ │ │ +139 double logNormalizationConstant() const override; │ │ │ │ │ +140 │ │ │ │ │ +148 double logProbability(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const; │ │ │ │ │ +149 │ │ │ │ │ +155 double evaluate(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const; │ │ │ │ │ +156 │ │ │ │ │ +_1_5_8 double _o_p_e_r_a_t_o_r_(_)(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const { │ │ │ │ │ +159 return evaluate(x); │ │ │ │ │ +160 } │ │ │ │ │ +161 │ │ │ │ │ +175 _V_e_c_t_o_r_V_a_l_u_e_s solve(const _V_e_c_t_o_r_V_a_l_u_e_s& parents) const; │ │ │ │ │ +176 │ │ │ │ │ +177 _V_e_c_t_o_r_V_a_l_u_e_s solveOtherRHS(const _V_e_c_t_o_r_V_a_l_u_e_s& parents, const _V_e_c_t_o_r_V_a_l_u_e_s& │ │ │ │ │ +rhs) const; │ │ │ │ │ +178 │ │ │ │ │ +180 void solveTransposeInPlace(_V_e_c_t_o_r_V_a_l_u_e_s& gy) const; │ │ │ │ │ +181 │ │ │ │ │ +183 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r likelihood( │ │ │ │ │ +184 const _V_e_c_t_o_r_V_a_l_u_e_s& frontalValues) const; │ │ │ │ │ +185 │ │ │ │ │ +187 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r likelihood(const Vector& frontal) const; │ │ │ │ │ +188 │ │ │ │ │ +195 _V_e_c_t_o_r_V_a_l_u_e_s sample(std::mt19937_64* rng) const; │ │ │ │ │ +196 │ │ │ │ │ +204 _V_e_c_t_o_r_V_a_l_u_e_s sample(const _V_e_c_t_o_r_V_a_l_u_e_s& parentsValues, │ │ │ │ │ +205 std::mt19937_64* rng) const; │ │ │ │ │ +206 │ │ │ │ │ +208 _V_e_c_t_o_r_V_a_l_u_e_s sample() const; │ │ │ │ │ +209 │ │ │ │ │ +211 _V_e_c_t_o_r_V_a_l_u_e_s sample(const _V_e_c_t_o_r_V_a_l_u_e_s& parentsValues) const; │ │ │ │ │ +212 │ │ │ │ │ +216 │ │ │ │ │ +_2_1_8 constABlock _R() const { return Ab_.range(0, nrFrontals()); } │ │ │ │ │ +219 │ │ │ │ │ +_2_2_1 constABlock _S() const { return Ab_.range(nrFrontals(), size()); } │ │ │ │ │ +222 │ │ │ │ │ +_2_2_4 constABlock _S(_c_o_n_s_t___i_t_e_r_a_t_o_r it) const { return BaseFactor::getA(it); } │ │ │ │ │ +225 │ │ │ │ │ +_2_2_7 const constBVector _d() const { return BaseFactor::getb(); } │ │ │ │ │ +228 │ │ │ │ │ +_2_4_0 inline double _d_e_t_e_r_m_i_n_a_n_t() const { return exp(logDeterminant()); } │ │ │ │ │ +241 │ │ │ │ │ +253 double logDeterminant() const; │ │ │ │ │ +254 │ │ │ │ │ +258 │ │ │ │ │ +263 double logProbability(const _H_y_b_r_i_d_V_a_l_u_e_s& x) const override; │ │ │ │ │ +264 │ │ │ │ │ +269 double evaluate(const _H_y_b_r_i_d_V_a_l_u_e_s& x) const override; │ │ │ │ │ +270 │ │ │ │ │ +271 using Conditional::operator(); // Expose evaluate(const HybridValues&) │ │ │ │ │ +method.. │ │ │ │ │ +272 using JacobianFactor::error; // Expose error(const HybridValues&) method.. │ │ │ │ │ +273 │ │ │ │ │ +275 │ │ │ │ │ +276 │ │ │ │ │ +277#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ +280 │ │ │ │ │ +282 void GTSAM_DEPRECATED scaleFrontalsBySigma(_V_e_c_t_o_r_V_a_l_u_e_s& gy) const; │ │ │ │ │ +284#endif │ │ │ │ │ +285 │ │ │ │ │ +286 private: │ │ │ │ │ +_2_8_8 friend class boost::serialization::access; │ │ │ │ │ +289 template │ │ │ │ │ +290 void serialize(Archive & ar, const unsigned int /*version*/) { │ │ │ │ │ +291 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e_F_a_c_t_o_r); │ │ │ │ │ +292 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e_C_o_n_d_i_t_i_o_n_a_l); │ │ │ │ │ +293 } │ │ │ │ │ +294 }; // GaussianConditional │ │ │ │ │ +295 │ │ │ │ │ +297template<> │ │ │ │ │ +_2_9_8struct _t_r_a_i_t_s<_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l> : public _T_e_s_t_a_b_l_e │ │ │ │ │ +{}; │ │ │ │ │ +299 │ │ │ │ │ +300} // \ namespace gtsam │ │ │ │ │ +301 │ │ │ │ │ +302#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_-_i_n_l_._h> │ │ │ │ │ +303 │ │ │ │ │ +_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ +Base class for conditional densities. │ │ │ │ │ +_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ +_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_-_i_n_l_._h │ │ │ │ │ +Conditional Gaussian Base class. │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ +Factor Graph Values. │ │ │ │ │ +_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ +Included from all GTSAM files. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ -Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key │ │ │ │ │ -landmarkKey) │ │ │ │ │ -Optimize for triangulation. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.cpp:155 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ _g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ Template to create a binary predicate. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ _g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ A helper that implements the traits interface for GTSAM types. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e │ │ │ │ │ -Bayes tree. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e │ │ │ │ │ -This is the base class for BayesTree cliques. │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e │ │ │ │ │ -A clique in a GaussianBayesTree. │ │ │ │ │ -DDeeffiinniittiioonn GaussianBayesTree.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e │ │ │ │ │ -A Bayes tree representing a Gaussian density. │ │ │ │ │ -DDeeffiinniittiioonn GaussianBayesTree.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e │ │ │ │ │ -GaussianBayesTree() │ │ │ │ │ -Default constructor, creates an empty Bayes tree. │ │ │ │ │ -DDeeffiinniittiioonn GaussianBayesTree.h:61 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ +of vertical blocks. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_c_o_n_s_t___i_t_e_r_a_t_o_r │ │ │ │ │ +KeyVector::const_iterator const_iterator │ │ │ │ │ +Const iterator over keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:80 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +A GaussianConditional functions as the node in a Bayes network. │ │ │ │ │ +DDeeffiinniittiioonn GaussianConditional.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_T_h_i_s │ │ │ │ │ +GaussianConditional This │ │ │ │ │ +Typedef to this class. │ │ │ │ │ +DDeeffiinniittiioonn GaussianConditional.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_S │ │ │ │ │ +constABlock S(const_iterator it) const │ │ │ │ │ +Get a view of the S matrix for the variable pointed to by the given key │ │ │ │ │ +iterator. │ │ │ │ │ +DDeeffiinniittiioonn GaussianConditional.h:224 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_R │ │ │ │ │ +constABlock R() const │ │ │ │ │ +Return a view of the upper-triangular R block of the conditional. │ │ │ │ │ +DDeeffiinniittiioonn GaussianConditional.h:218 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_B_a_s_e_F_a_c_t_o_r │ │ │ │ │ +JacobianFactor BaseFactor │ │ │ │ │ +Typedef to our factor base class. │ │ │ │ │ +DDeeffiinniittiioonn GaussianConditional.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +GaussianConditional() │ │ │ │ │ +default constructor needed for serialization │ │ │ │ │ +DDeeffiinniittiioonn GaussianConditional.h:54 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_B_a_s_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +Conditional< BaseFactor, This > BaseConditional │ │ │ │ │ +Typedef to our conditional base class. │ │ │ │ │ +DDeeffiinniittiioonn GaussianConditional.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_C_o_m_b_i_n_e │ │ │ │ │ +static shared_ptr Combine(ITERATOR firstConditional, ITERATOR lastConditional) │ │ │ │ │ +Combine several GaussianConditional into a single dense GC. │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_d_e_t_e_r_m_i_n_a_n_t │ │ │ │ │ +double determinant() const │ │ │ │ │ +Compute the determinant of the R matrix. │ │ │ │ │ +DDeeffiinniittiioonn GaussianConditional.h:240 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ +double operator()(const VectorValues &x) const │ │ │ │ │ +Evaluate probability density, sugar. │ │ │ │ │ +DDeeffiinniittiioonn GaussianConditional.h:158 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d_M_e_a_n_A_n_d_S_t_d_d_e_v │ │ │ │ │ +static shared_ptr sharedMeanAndStddev(Args &&... args) │ │ │ │ │ +Create shared pointer by forwarding arguments to fromMeanAndStddev. │ │ │ │ │ +DDeeffiinniittiioonn GaussianConditional.h:105 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_S │ │ │ │ │ +constABlock S() const │ │ │ │ │ +Get a view of the parent blocks. │ │ │ │ │ +DDeeffiinniittiioonn GaussianConditional.h:221 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn GaussianConditional.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_d │ │ │ │ │ +const constBVector d() const │ │ │ │ │ +Get a view of the r.h.s. │ │ │ │ │ +DDeeffiinniittiioonn GaussianConditional.h:227 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +An abstract virtual base class for JacobianFactor and HessianFactor. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactor.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ +A Gaussian factor in the squared-error form. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:91 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:96 │ │ │ │ │ _g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ with a unique integer... │ │ │ │ │ DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ -_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -is the normalization constant. │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -The Factor::error simply extracts the. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ + * _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00785.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianDensity.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianBayesTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,43 +96,50 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
GaussianDensity.h File Reference
│ │ │ │ +
GaussianBayesTree.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

A Gaussian Density. │ │ │ │ +

Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::GaussianDensity
 A GaussianDensity is a GaussianConditional without parents. More...
class  gtsam::GaussianBayesTreeClique
 A clique in a GaussianBayesTree. More...
 
class  gtsam::GaussianBayesTree
 A Bayes tree representing a Gaussian density. More...
 
struct  gtsam::traits< GaussianBayesTree >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

A Gaussian Density.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
Date
Jan 21, 2012
│ │ │ │ +

Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.

│ │ │ │ +

GaussianBayesTree

Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,27 +1,33 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -GaussianDensity.h File Reference │ │ │ │ │ -A Gaussian Density. _M_o_r_e_._._. │ │ │ │ │ +GaussianBayesTree.h File Reference │ │ │ │ │ +Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_D_e_n_s_i_t_y │ │ │ │ │ -  A _G_a_u_s_s_i_a_n_D_e_n_s_i_t_y is a _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l without parents. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e │ │ │ │ │ +  A clique in a _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e │ │ │ │ │ +  A Bayes tree representing a Gaussian density. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A Gaussian Density. │ │ │ │ │ +Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ │ +GaussianBayesTree │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - Jan 21, 2012 │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_D_e_n_s_i_t_y_._h │ │ │ │ │ + * _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00785_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianDensity.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianBayesTree.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,92 +98,120 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
GaussianDensity.h
│ │ │ │ +
GaussianBayesTree.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19// \callgraph
│ │ │ │
20#pragma once
│ │ │ │
21
│ │ │ │ -
22#include <gtsam/linear/GaussianConditional.h>
│ │ │ │ -
23
│ │ │ │ -
24namespace gtsam {
│ │ │ │ -
25
│ │ │ │ -
│ │ │ │ -
32 class GTSAM_EXPORT GaussianDensity : public GaussianConditional {
│ │ │ │ -
33
│ │ │ │ -
34 public:
│ │ │ │ -
35
│ │ │ │ -
36 typedef boost::shared_ptr<GaussianDensity> shared_ptr;
│ │ │ │ -
37
│ │ │ │ -
│ │ │ │ -
39 GaussianDensity() :
│ │ │ │ -
40 GaussianConditional() {
│ │ │ │ -
41 }
│ │ │ │ -
│ │ │ │ -
42
│ │ │ │ -
│ │ │ │ -
44 GaussianDensity(const GaussianConditional& conditional) :
│ │ │ │ -
45 GaussianConditional(conditional) {
│ │ │ │ -
46 if(conditional.nrParents() != 0)
│ │ │ │ -
47 throw std::invalid_argument("GaussianDensity can only be created from a conditional with no parents");
│ │ │ │ -
48 }
│ │ │ │ -
│ │ │ │ -
49
│ │ │ │ -
│ │ │ │ -
51 GaussianDensity(Key key, const Vector& d, const Matrix& R, const SharedDiagonal& noiseModel = SharedDiagonal()) :
│ │ │ │ -
52 GaussianConditional(key, d, R, noiseModel) {}
│ │ │ │ -
│ │ │ │ -
53
│ │ │ │ -
55 static GaussianDensity FromMeanAndStddev(Key key, const Vector& mean,
│ │ │ │ -
56 double sigma);
│ │ │ │ -
57
│ │ │ │ -
59 void print(const std::string& = "GaussianDensity",
│ │ │ │ -
60 const KeyFormatter& formatter = DefaultKeyFormatter) const override;
│ │ │ │ -
61
│ │ │ │ -
63 Vector mean() const;
│ │ │ │ -
64
│ │ │ │ -
66 Matrix covariance() const;
│ │ │ │ -
67
│ │ │ │ -
68 };
│ │ │ │ -
│ │ │ │ -
69 // GaussianDensity
│ │ │ │ -
70
│ │ │ │ -
71}// gtsam
│ │ │ │ -
GaussianConditional.h
Conditional Gaussian Base class.
│ │ │ │ +
22#include <gtsam/linear/GaussianBayesNet.h>
│ │ │ │ +
23#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ +
24#include <gtsam/inference/BayesTree.h>
│ │ │ │ +
25#include <gtsam/inference/BayesTreeCliqueBase.h>
│ │ │ │ +
26
│ │ │ │ +
27namespace gtsam {
│ │ │ │ +
28
│ │ │ │ +
29 // Forward declarations
│ │ │ │ +
30 class GaussianConditional;
│ │ │ │ +
31 class VectorValues;
│ │ │ │ +
32
│ │ │ │ +
33 /* ************************************************************************* */
│ │ │ │ +
│ │ │ │ +
35 class GTSAM_EXPORT GaussianBayesTreeClique :
│ │ │ │ +
36 public BayesTreeCliqueBase<GaussianBayesTreeClique, GaussianFactorGraph>
│ │ │ │ +
37 {
│ │ │ │ +
38 public:
│ │ │ │ +
39 typedef GaussianBayesTreeClique This;
│ │ │ │ +
40 typedef BayesTreeCliqueBase<GaussianBayesTreeClique, GaussianFactorGraph> Base;
│ │ │ │ +
41 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
42 typedef boost::weak_ptr<This> weak_ptr;
│ │ │ │ +
43 GaussianBayesTreeClique() {}
│ │ │ │ +
44 virtual ~GaussianBayesTreeClique() {}
│ │ │ │ +
45 GaussianBayesTreeClique(const boost::shared_ptr<GaussianConditional>& conditional) : Base(conditional) {}
│ │ │ │ +
46 };
│ │ │ │ +
│ │ │ │ +
47
│ │ │ │ +
48 /* ************************************************************************* */
│ │ │ │ +
│ │ │ │ +
50 class GTSAM_EXPORT GaussianBayesTree :
│ │ │ │ +
51 public BayesTree<GaussianBayesTreeClique>
│ │ │ │ +
52 {
│ │ │ │ +
53 private:
│ │ │ │ +
54 typedef BayesTree<GaussianBayesTreeClique> Base;
│ │ │ │ +
55
│ │ │ │ +
56 public:
│ │ │ │ +
57 typedef GaussianBayesTree This;
│ │ │ │ +
58 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
59
│ │ │ │ +
61 GaussianBayesTree() {}
│ │ │ │ +
62
│ │ │ │ +
64 bool equals(const This& other, double tol = 1e-9) const;
│ │ │ │ +
65
│ │ │ │ +
67 VectorValues optimize() const;
│ │ │ │ +
68
│ │ │ │ +
94 VectorValues optimizeGradientSearch() const;
│ │ │ │ +
95
│ │ │ │ +
101 VectorValues gradient(const VectorValues& x0) const;
│ │ │ │ +
102
│ │ │ │ +
108 VectorValues gradientAtZero() const;
│ │ │ │ +
109
│ │ │ │ +
111 double error(const VectorValues& x) const;
│ │ │ │ +
112
│ │ │ │ +
118 double determinant() const;
│ │ │ │ +
119
│ │ │ │ +
125 double logDeterminant() const;
│ │ │ │ +
126
│ │ │ │ +
129 Matrix marginalCovariance(Key key) const;
│ │ │ │ +
130 };
│ │ │ │ +
│ │ │ │ +
131
│ │ │ │ +
133 template<>
│ │ │ │ +
│ │ │ │ +
134 struct traits<GaussianBayesTree> : public Testable<GaussianBayesTree> {
│ │ │ │ +
135 };
│ │ │ │ +
│ │ │ │ +
136
│ │ │ │ +
137} //\ namespace gtsam
│ │ │ │ +
BayesTreeCliqueBase.h
Base class for cliques of a BayesTree.
│ │ │ │ +
BayesTree.h
Bayes Tree is a tree of cliques of a Bayes Chain.
│ │ │ │ +
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ +
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::mean
Point3 mean(const CONTAINER &points)
mean
Definition Point3.h:68
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::Conditional::nrParents
size_t nrParents() const
return the number of parents
Definition Conditional.h:113
│ │ │ │ -
gtsam::GaussianConditional
A GaussianConditional functions as the node in a Bayes network.
Definition GaussianConditional.h:43
│ │ │ │ -
gtsam::GaussianDensity
A GaussianDensity is a GaussianConditional without parents.
Definition GaussianDensity.h:32
│ │ │ │ -
gtsam::GaussianDensity::GaussianDensity
GaussianDensity(Key key, const Vector &d, const Matrix &R, const SharedDiagonal &noiseModel=SharedDiagonal())
constructor using d, R
Definition GaussianDensity.h:51
│ │ │ │ -
gtsam::GaussianDensity::GaussianDensity
GaussianDensity()
default constructor needed for serialization
Definition GaussianDensity.h:39
│ │ │ │ -
gtsam::GaussianDensity::GaussianDensity
GaussianDensity(const GaussianConditional &conditional)
Copy constructor from GaussianConditional.
Definition GaussianDensity.h:44
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::BayesTree
Bayes tree.
Definition BayesTree.h:67
│ │ │ │ +
gtsam::BayesTreeCliqueBase
This is the base class for BayesTree cliques.
Definition BayesTreeCliqueBase.h:50
│ │ │ │ +
gtsam::GaussianBayesTreeClique
A clique in a GaussianBayesTree.
Definition GaussianBayesTree.h:37
│ │ │ │ +
gtsam::GaussianBayesTree
A Bayes tree representing a Gaussian density.
Definition GaussianBayesTree.h:52
│ │ │ │ +
gtsam::GaussianBayesTree::GaussianBayesTree
GaussianBayesTree()
Default constructor, creates an empty Bayes tree.
Definition GaussianBayesTree.h:61
│ │ │ │ +
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
GaussianConditional
is the normalization constant.
│ │ │ │ +
VectorValues
The Factor::error simply extracts the.
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,110 +1,146 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GaussianDensity.h │ │ │ │ │ +GaussianBayesTree.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19// \callgraph │ │ │ │ │ 20#pragma once │ │ │ │ │ 21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ -23 │ │ │ │ │ -24namespace _g_t_s_a_m { │ │ │ │ │ -25 │ │ │ │ │ -_3_2 class GTSAM_EXPORT _G_a_u_s_s_i_a_n_D_e_n_s_i_t_y : public _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l { │ │ │ │ │ -33 │ │ │ │ │ -34 public: │ │ │ │ │ -35 │ │ │ │ │ -36 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -37 │ │ │ │ │ -_3_9 _G_a_u_s_s_i_a_n_D_e_n_s_i_t_y() : │ │ │ │ │ -40 _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l() { │ │ │ │ │ -41 } │ │ │ │ │ -42 │ │ │ │ │ -_4_4 _G_a_u_s_s_i_a_n_D_e_n_s_i_t_y(const _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l& conditional) : │ │ │ │ │ -45 _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l(conditional) { │ │ │ │ │ -46 if(conditional._n_r_P_a_r_e_n_t_s() != 0) │ │ │ │ │ -47 throw std::invalid_argument("GaussianDensity can only be created from a │ │ │ │ │ -conditional with no parents"); │ │ │ │ │ -48 } │ │ │ │ │ -49 │ │ │ │ │ -_5_1 _G_a_u_s_s_i_a_n_D_e_n_s_i_t_y(_K_e_y key, const Vector& d, const Matrix& R, const │ │ │ │ │ -SharedDiagonal& noiseModel = SharedDiagonal()) : │ │ │ │ │ -52 _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l(key, d, R, noiseModel) {} │ │ │ │ │ -53 │ │ │ │ │ -55 static _G_a_u_s_s_i_a_n_D_e_n_s_i_t_y FromMeanAndStddev(_K_e_y key, const Vector& mean, │ │ │ │ │ -56 double sigma); │ │ │ │ │ -57 │ │ │ │ │ -59 void _p_r_i_n_t(const std::string& = "GaussianDensity", │ │ │ │ │ -60 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override; │ │ │ │ │ -61 │ │ │ │ │ -63 Vector _m_e_a_n() const; │ │ │ │ │ -64 │ │ │ │ │ -66 Matrix covariance() const; │ │ │ │ │ -67 │ │ │ │ │ -68 }; │ │ │ │ │ -69 // GaussianDensity │ │ │ │ │ -70 │ │ │ │ │ -71}// gtsam │ │ │ │ │ -_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ -Conditional Gaussian Base class. │ │ │ │ │ +22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._h> │ │ │ │ │ +26 │ │ │ │ │ +27namespace _g_t_s_a_m { │ │ │ │ │ +28 │ │ │ │ │ +29 // Forward declarations │ │ │ │ │ +30 class _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l; │ │ │ │ │ +31 class _V_e_c_t_o_r_V_a_l_u_e_s; │ │ │ │ │ +32 │ │ │ │ │ +33 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +_3_5 class GTSAM_EXPORT _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e : │ │ │ │ │ +36 public _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e │ │ │ │ │ +37 { │ │ │ │ │ +38 public: │ │ │ │ │ +39 typedef _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e _T_h_i_s; │ │ │ │ │ +40 typedef _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_,_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_> │ │ │ │ │ +_B_a_s_e; │ │ │ │ │ +41 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +42 typedef boost::weak_ptr weak_ptr; │ │ │ │ │ +43 _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e() {} │ │ │ │ │ +44 virtual _~_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e() {} │ │ │ │ │ +45 _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e(const boost::shared_ptr& │ │ │ │ │ +conditional) : _B_a_s_e(conditional) {} │ │ │ │ │ +46 }; │ │ │ │ │ +47 │ │ │ │ │ +48 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +_5_0 class GTSAM_EXPORT _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e : │ │ │ │ │ +51 public _B_a_y_e_s_T_r_e_e │ │ │ │ │ +52 { │ │ │ │ │ +53 private: │ │ │ │ │ +54 typedef _B_a_y_e_s_T_r_e_e_<_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_> _B_a_s_e; │ │ │ │ │ +55 │ │ │ │ │ +56 public: │ │ │ │ │ +57 typedef _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e _T_h_i_s; │ │ │ │ │ +58 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +59 │ │ │ │ │ +_6_1 _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e() {} │ │ │ │ │ +62 │ │ │ │ │ +64 bool _e_q_u_a_l_s(const This& other, double tol = 1e-9) const; │ │ │ │ │ +65 │ │ │ │ │ +67 _V_e_c_t_o_r_V_a_l_u_e_s _o_p_t_i_m_i_z_e() const; │ │ │ │ │ +68 │ │ │ │ │ +94 _V_e_c_t_o_r_V_a_l_u_e_s optimizeGradientSearch() const; │ │ │ │ │ +95 │ │ │ │ │ +101 _V_e_c_t_o_r_V_a_l_u_e_s gradient(const _V_e_c_t_o_r_V_a_l_u_e_s& x0) const; │ │ │ │ │ +102 │ │ │ │ │ +108 _V_e_c_t_o_r_V_a_l_u_e_s gradientAtZero() const; │ │ │ │ │ +109 │ │ │ │ │ +111 double error(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const; │ │ │ │ │ +112 │ │ │ │ │ +118 double determinant() const; │ │ │ │ │ +119 │ │ │ │ │ +125 double logDeterminant() const; │ │ │ │ │ +126 │ │ │ │ │ +129 Matrix marginalCovariance(_K_e_y key) const; │ │ │ │ │ +130 }; │ │ │ │ │ +131 │ │ │ │ │ +133 template<> │ │ │ │ │ +_1_3_4 struct _t_r_a_i_t_s<_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e> : public _T_e_s_t_a_b_l_e { │ │ │ │ │ +135 }; │ │ │ │ │ +136 │ │ │ │ │ +137} //\ namespace gtsam │ │ │ │ │ +_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._h │ │ │ │ │ +Base class for cliques of a BayesTree. │ │ │ │ │ +_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ +Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ │ +_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ +_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h │ │ │ │ │ +Chordal Bayes Net, the result of eliminating a factor graph. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_m_e_a_n │ │ │ │ │ -Point3 mean(const CONTAINER &points) │ │ │ │ │ -mean │ │ │ │ │ -DDeeffiinniittiioonn Point3.h:68 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ +Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key │ │ │ │ │ +landmarkKey) │ │ │ │ │ +Optimize for triangulation. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.cpp:155 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_n_r_P_a_r_e_n_t_s │ │ │ │ │ -size_t nrParents() const │ │ │ │ │ -return the number of parents │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:113 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -A GaussianConditional functions as the node in a Bayes network. │ │ │ │ │ -DDeeffiinniittiioonn GaussianConditional.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_D_e_n_s_i_t_y │ │ │ │ │ -A GaussianDensity is a GaussianConditional without parents. │ │ │ │ │ -DDeeffiinniittiioonn GaussianDensity.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_D_e_n_s_i_t_y_:_:_G_a_u_s_s_i_a_n_D_e_n_s_i_t_y │ │ │ │ │ -GaussianDensity(Key key, const Vector &d, const Matrix &R, const SharedDiagonal │ │ │ │ │ -&noiseModel=SharedDiagonal()) │ │ │ │ │ -constructor using d, R │ │ │ │ │ -DDeeffiinniittiioonn GaussianDensity.h:51 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_D_e_n_s_i_t_y_:_:_G_a_u_s_s_i_a_n_D_e_n_s_i_t_y │ │ │ │ │ -GaussianDensity() │ │ │ │ │ -default constructor needed for serialization │ │ │ │ │ -DDeeffiinniittiioonn GaussianDensity.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_D_e_n_s_i_t_y_:_:_G_a_u_s_s_i_a_n_D_e_n_s_i_t_y │ │ │ │ │ -GaussianDensity(const GaussianConditional &conditional) │ │ │ │ │ -Copy constructor from GaussianConditional. │ │ │ │ │ -DDeeffiinniittiioonn GaussianDensity.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e │ │ │ │ │ +Bayes tree. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e │ │ │ │ │ +This is the base class for BayesTree cliques. │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e │ │ │ │ │ +A clique in a GaussianBayesTree. │ │ │ │ │ +DDeeffiinniittiioonn GaussianBayesTree.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e │ │ │ │ │ +A Bayes tree representing a Gaussian density. │ │ │ │ │ +DDeeffiinniittiioonn GaussianBayesTree.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e │ │ │ │ │ +GaussianBayesTree() │ │ │ │ │ +Default constructor, creates an empty Bayes tree. │ │ │ │ │ +DDeeffiinniittiioonn GaussianBayesTree.h:61 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ +with a unique integer... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +is the normalization constant. │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +The Factor::error simply extracts the. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_D_e_n_s_i_t_y_._h │ │ │ │ │ + * _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00788.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianJunctionTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,45 +96,41 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
GaussianFactor.h File Reference
│ │ │ │ +
GaussianJunctionTree.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

A factor with a quadratic error function - a Gaussian. │ │ │ │ -More...

│ │ │ │ - │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::GaussianFactor
 An abstract virtual base class for JacobianFactor and HessianFactor. More...
 
struct  gtsam::traits< GaussianFactor >
 traits More...
class  gtsam::GaussianJunctionTree
 A junction tree specialized to Gaussian factors, i.e., it is a cluster tree with Gaussian factors stored in each cluster. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

A factor with a quadratic error function - a Gaussian.

│ │ │ │ -

GaussianFactor

Author
Richard Roberts, Christian Potthast
│ │ │ │ +
Date
Mar 29, 2013
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,30 +1,27 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -GaussianFactor.h File Reference │ │ │ │ │ -A factor with a quadratic error function - a Gaussian. _M_o_r_e_._._. │ │ │ │ │ +GaussianJunctionTree.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -  An abstract virtual base class for _J_a_c_o_b_i_a_n_F_a_c_t_o_r and _H_e_s_s_i_a_n_F_a_c_t_o_r. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ +  A junction tree specialized to Gaussian factors, i.e., it is a cluster │ │ │ │ │ + tree with Gaussian factors stored in each cluster. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A factor with a quadratic error function - a Gaussian. │ │ │ │ │ -GaussianFactor │ │ │ │ │ + Date │ │ │ │ │ + Mar 29, 2013 │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts, Christian Potthast │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ + * _G_a_u_s_s_i_a_n_J_u_n_c_t_i_o_n_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00788_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianJunctionTree.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,170 +98,66 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
GaussianFactor.h
│ │ │ │ +
GaussianJunctionTree.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19// \callgraph
│ │ │ │ +
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#pragma once
│ │ │ │ -
22
│ │ │ │ -
23#include <gtsam/inference/Factor.h>
│ │ │ │ -
24#include <gtsam/base/Matrix.h>
│ │ │ │ -
25#include <gtsam/base/Testable.h>
│ │ │ │ +
21#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ +
22#include <gtsam/linear/GaussianBayesTree.h>
│ │ │ │ +
23#include <gtsam/inference/JunctionTree.h>
│ │ │ │ +
24
│ │ │ │ +
25namespace gtsam {
│ │ │ │
26
│ │ │ │ -
27namespace gtsam {
│ │ │ │ -
28
│ │ │ │ -
29 // Forward declarations
│ │ │ │ -
30 class VectorValues;
│ │ │ │ -
31 class Scatter;
│ │ │ │ -
32 class SymmetricBlockMatrix;
│ │ │ │ -
33
│ │ │ │ +
27 // Forward declarations
│ │ │ │ +
28 class GaussianEliminationTree;
│ │ │ │ +
29
│ │ │ │
│ │ │ │ -
38 class GTSAM_EXPORT GaussianFactor : public Factor
│ │ │ │ -
39 {
│ │ │ │ +
38 class GTSAM_EXPORT GaussianJunctionTree :
│ │ │ │ +
39 public JunctionTree<GaussianBayesTree, GaussianFactorGraph> {
│ │ │ │
40 public:
│ │ │ │ -
41 typedef GaussianFactor This;
│ │ │ │ -
42 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
43 typedef Factor Base;
│ │ │ │ +
41 typedef JunctionTree<GaussianBayesTree, GaussianFactorGraph> Base;
│ │ │ │ +
42 typedef GaussianJunctionTree This;
│ │ │ │ +
43 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │
44
│ │ │ │ -
46 GaussianFactor() {}
│ │ │ │ -
47
│ │ │ │ -
50 template<typename CONTAINER>
│ │ │ │ -
51 GaussianFactor(const CONTAINER& keys) : Base(keys) {}
│ │ │ │ -
52
│ │ │ │ -
54 virtual ~GaussianFactor() {}
│ │ │ │ -
55
│ │ │ │ -
56 // Implementing Testable interface
│ │ │ │ -
57
│ │ │ │ -
59 void print(
│ │ │ │ -
60 const std::string& s = "",
│ │ │ │ -
61 const KeyFormatter& formatter = DefaultKeyFormatter) const override = 0;
│ │ │ │ -
62
│ │ │ │ -
64 virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const = 0;
│ │ │ │ -
65
│ │ │ │ -
73 virtual double error(const VectorValues& c) const;
│ │ │ │ -
74
│ │ │ │ -
79 double error(const HybridValues& c) const override;
│ │ │ │ -
80
│ │ │ │ -
82 virtual DenseIndex getDim(const_iterator variable) const = 0;
│ │ │ │ -
83
│ │ │ │ -
91 virtual Matrix augmentedJacobian() const = 0;
│ │ │ │ -
92
│ │ │ │ -
100 virtual std::pair<Matrix,Vector> jacobian() const = 0;
│ │ │ │ -
101
│ │ │ │ -
110 virtual Matrix augmentedInformation() const = 0;
│ │ │ │ -
111
│ │ │ │ -
115 virtual Matrix information() const = 0;
│ │ │ │ -
116
│ │ │ │ -
118 VectorValues hessianDiagonal() const;
│ │ │ │ -
119
│ │ │ │ -
121 virtual void hessianDiagonalAdd(VectorValues& d) const = 0;
│ │ │ │ -
122
│ │ │ │ -
124 virtual void hessianDiagonal(double* d) const = 0;
│ │ │ │ -
125
│ │ │ │ -
127 virtual std::map<Key,Matrix> hessianBlockDiagonal() const = 0;
│ │ │ │ -
128
│ │ │ │ -
130 virtual GaussianFactor::shared_ptr clone() const = 0;
│ │ │ │ -
131
│ │ │ │ -
137 virtual GaussianFactor::shared_ptr negate() const = 0;
│ │ │ │ -
138
│ │ │ │ -
144 virtual void updateHessian(const KeyVector& keys,
│ │ │ │ -
145 SymmetricBlockMatrix* info) const = 0;
│ │ │ │ -
146
│ │ │ │ -
148 virtual void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const = 0;
│ │ │ │ -
149
│ │ │ │ -
151 virtual VectorValues gradientAtZero() const = 0;
│ │ │ │ -
152
│ │ │ │ -
154 virtual void gradientAtZero(double* d) const = 0;
│ │ │ │ -
155
│ │ │ │ -
157 virtual Vector gradient(Key key, const VectorValues& x) const = 0;
│ │ │ │ -
158
│ │ │ │ -
159 // Determine position of a given key
│ │ │ │ -
160 template <typename CONTAINER>
│ │ │ │ -
161 static DenseIndex Slot(const CONTAINER& keys, Key key) {
│ │ │ │ -
162 return std::find(keys.begin(), keys.end(), key) - keys.begin();
│ │ │ │ -
163 }
│ │ │ │ -
164
│ │ │ │ -
165 private:
│ │ │ │ -
167 friend class boost::serialization::access;
│ │ │ │ -
168 template<class ARCHIVE>
│ │ │ │ -
169 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
170 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
171 }
│ │ │ │ -
172
│ │ │ │ -
173 }; // GaussianFactor
│ │ │ │ -
│ │ │ │ -
174
│ │ │ │ -
176template<>
│ │ │ │ -
│ │ │ │ -
177struct traits<GaussianFactor> : public Testable<GaussianFactor> {
│ │ │ │ -
178};
│ │ │ │ +
53 GaussianJunctionTree(const GaussianEliminationTree& eliminationTree);
│ │ │ │ +
54 };
│ │ │ │
│ │ │ │ -
179
│ │ │ │ -
180} // \ namespace gtsam
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ -
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ -
Factor.h
The base class for all factors.
│ │ │ │ +
55
│ │ │ │ +
56}
│ │ │ │ +
JunctionTree.h
The junction tree.
│ │ │ │ +
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ +
GaussianBayesTree.h
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ -
gtsam::Factor
Definition Factor.h:68
│ │ │ │ -
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition Factor.h:80
│ │ │ │ -
gtsam::GaussianFactor
An abstract virtual base class for JacobianFactor and HessianFactor.
Definition GaussianFactor.h:39
│ │ │ │ -
gtsam::GaussianFactor::getDim
virtual DenseIndex getDim(const_iterator variable) const =0
Return the dimension of the variable pointed to by the given key iterator.
│ │ │ │ -
gtsam::GaussianFactor::hessianBlockDiagonal
virtual std::map< Key, Matrix > hessianBlockDiagonal() const =0
Return the block diagonal of the Hessian for this factor.
│ │ │ │ -
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactor.h:42
│ │ │ │ -
gtsam::GaussianFactor::jacobian
virtual std::pair< Matrix, Vector > jacobian() const =0
Return the dense Jacobian and right-hand-side , with the noise models baked into A and b.
│ │ │ │ -
gtsam::GaussianFactor::gradient
virtual Vector gradient(Key key, const VectorValues &x) const =0
Gradient wrt a key at any values.
│ │ │ │ -
gtsam::GaussianFactor::clone
virtual GaussianFactor::shared_ptr clone() const =0
Clone a factor (make a deep copy)
│ │ │ │ -
gtsam::GaussianFactor::information
virtual Matrix information() const =0
Return the non-augmented information matrix represented by this GaussianFactor.
│ │ │ │ -
gtsam::GaussianFactor::Base
Factor Base
Our base class.
Definition GaussianFactor.h:43
│ │ │ │ -
gtsam::GaussianFactor::~GaussianFactor
virtual ~GaussianFactor()
Destructor.
Definition GaussianFactor.h:54
│ │ │ │ -
gtsam::GaussianFactor::gradientAtZero
virtual VectorValues gradientAtZero() const =0
A'*b for Jacobian, eta for Hessian.
│ │ │ │ -
gtsam::GaussianFactor::GaussianFactor
GaussianFactor(const CONTAINER &keys)
Construct from container of keys.
Definition GaussianFactor.h:51
│ │ │ │ -
gtsam::GaussianFactor::multiplyHessianAdd
virtual void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const =0
y += alpha * A'*A*x
│ │ │ │ -
gtsam::GaussianFactor::equals
virtual bool equals(const GaussianFactor &lf, double tol=1e-9) const =0
Equals for testable.
│ │ │ │ -
gtsam::GaussianFactor::augmentedInformation
virtual Matrix augmentedInformation() const =0
Return the augmented information matrix represented by this GaussianFactor.
│ │ │ │ -
gtsam::GaussianFactor::augmentedJacobian
virtual Matrix augmentedJacobian() const =0
Return a dense Jacobian matrix, augmented with b with the noise models baked into A and b.
│ │ │ │ -
gtsam::GaussianFactor::GaussianFactor
GaussianFactor()
Default constructor creates empty factor.
Definition GaussianFactor.h:46
│ │ │ │ -
gtsam::GaussianFactor::negate
virtual GaussianFactor::shared_ptr negate() const =0
Construct the corresponding anti-factor to negate information stored stored in this factor.
│ │ │ │ -
gtsam::GaussianFactor::hessianDiagonal
virtual void hessianDiagonal(double *d) const =0
Raw memory access version of hessianDiagonal.
│ │ │ │ -
gtsam::GaussianFactor::This
GaussianFactor This
This class.
Definition GaussianFactor.h:41
│ │ │ │ -
gtsam::GaussianFactor::updateHessian
virtual void updateHessian(const KeyVector &keys, SymmetricBlockMatrix *info) const =0
Update an information matrix by adding the information corresponding to this factor (used internally ...
│ │ │ │ -
gtsam::GaussianFactor::print
void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override=0
print
│ │ │ │ -
gtsam::GaussianFactor::hessianDiagonalAdd
virtual void hessianDiagonalAdd(VectorValues &d) const =0
Add the current diagonal to a VectorValues instance.
│ │ │ │ -
gtsam::GaussianFactor::gradientAtZero
virtual void gradientAtZero(double *d) const =0
Raw memory access version of gradientAtZero.
│ │ │ │ -
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ -
VectorValues
The Factor::error simply extracts the.
│ │ │ │ +
gtsam::JunctionTree
A JunctionTree is a cluster tree, a set of variable clusters with factors, arranged in a tree,...
Definition JunctionTree.h:50
│ │ │ │ +
gtsam::GaussianEliminationTree
Definition GaussianEliminationTree.h:29
│ │ │ │ +
gtsam::GaussianJunctionTree
A junction tree specialized to Gaussian factors, i.e., it is a cluster tree with Gaussian factors sto...
Definition GaussianJunctionTree.h:39
│ │ │ │ +
gtsam::GaussianJunctionTree::This
GaussianJunctionTree This
This class.
Definition GaussianJunctionTree.h:42
│ │ │ │ +
gtsam::GaussianJunctionTree::Base
JunctionTree< GaussianBayesTree, GaussianFactorGraph > Base
Base class.
Definition GaussianJunctionTree.h:41
│ │ │ │ +
gtsam::GaussianJunctionTree::shared_ptr
boost::shared_ptr< This > shared_ptr
Shared pointer to this class.
Definition GaussianJunctionTree.h:43
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,255 +1,77 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GaussianFactor.h │ │ │ │ │ +GaussianJunctionTree.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19// \callgraph │ │ │ │ │ +19#pragma once │ │ │ │ │ 20 │ │ │ │ │ -21#pragma once │ │ │ │ │ -22 │ │ │ │ │ -23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_J_u_n_c_t_i_o_n_T_r_e_e_._h> │ │ │ │ │ +24 │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ 26 │ │ │ │ │ -27namespace _g_t_s_a_m { │ │ │ │ │ -28 │ │ │ │ │ -29 // Forward declarations │ │ │ │ │ -30 class _V_e_c_t_o_r_V_a_l_u_e_s; │ │ │ │ │ -31 class Scatter; │ │ │ │ │ -32 class SymmetricBlockMatrix; │ │ │ │ │ -33 │ │ │ │ │ -_3_8 class GTSAM_EXPORT _G_a_u_s_s_i_a_n_F_a_c_t_o_r : public _F_a_c_t_o_r │ │ │ │ │ -39 { │ │ │ │ │ +27 // Forward declarations │ │ │ │ │ +28 class GaussianEliminationTree; │ │ │ │ │ +29 │ │ │ │ │ +_3_8 class GTSAM_EXPORT _G_a_u_s_s_i_a_n_J_u_n_c_t_i_o_n_T_r_e_e : │ │ │ │ │ +39 public _J_u_n_c_t_i_o_n_T_r_e_e { │ │ │ │ │ 40 public: │ │ │ │ │ -_4_1 typedef _G_a_u_s_s_i_a_n_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ -_4_2 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -_4_3 typedef _F_a_c_t_o_r _B_a_s_e; │ │ │ │ │ +_4_1 typedef _J_u_n_c_t_i_o_n_T_r_e_e_<_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_,_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_> _B_a_s_e; │ │ │ │ │ +_4_2 typedef _G_a_u_s_s_i_a_n_J_u_n_c_t_i_o_n_T_r_e_e _T_h_i_s; │ │ │ │ │ +_4_3 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ 44 │ │ │ │ │ -_4_6 _G_a_u_s_s_i_a_n_F_a_c_t_o_r() {} │ │ │ │ │ -47 │ │ │ │ │ -50 template │ │ │ │ │ -_5_1 _G_a_u_s_s_i_a_n_F_a_c_t_o_r(const CONTAINER& keys) : _B_a_s_e(keys) {} │ │ │ │ │ -52 │ │ │ │ │ -_5_4 virtual _~_G_a_u_s_s_i_a_n_F_a_c_t_o_r() {} │ │ │ │ │ +53 _G_a_u_s_s_i_a_n_J_u_n_c_t_i_o_n_T_r_e_e(const _G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e& eliminationTree); │ │ │ │ │ +54 }; │ │ │ │ │ 55 │ │ │ │ │ -56 // Implementing Testable interface │ │ │ │ │ -57 │ │ │ │ │ -_5_9 void _p_r_i_n_t( │ │ │ │ │ -60 const std::string& s = "", │ │ │ │ │ -61 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override = 0; │ │ │ │ │ -62 │ │ │ │ │ -_6_4 virtual bool _e_q_u_a_l_s(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r& lf, double tol = 1e-9) const = 0; │ │ │ │ │ -65 │ │ │ │ │ -73 virtual double error(const _V_e_c_t_o_r_V_a_l_u_e_s& c) const; │ │ │ │ │ -74 │ │ │ │ │ -79 double error(const _H_y_b_r_i_d_V_a_l_u_e_s& c) const override; │ │ │ │ │ -80 │ │ │ │ │ -_8_2 virtual _D_e_n_s_e_I_n_d_e_x _g_e_t_D_i_m(_c_o_n_s_t___i_t_e_r_a_t_o_r variable) const = 0; │ │ │ │ │ -83 │ │ │ │ │ -_9_1 virtual Matrix _a_u_g_m_e_n_t_e_d_J_a_c_o_b_i_a_n() const = 0; │ │ │ │ │ -92 │ │ │ │ │ -_1_0_0 virtual std::pair _j_a_c_o_b_i_a_n() const = 0; │ │ │ │ │ -101 │ │ │ │ │ -_1_1_0 virtual Matrix _a_u_g_m_e_n_t_e_d_I_n_f_o_r_m_a_t_i_o_n() const = 0; │ │ │ │ │ -111 │ │ │ │ │ -_1_1_5 virtual Matrix _i_n_f_o_r_m_a_t_i_o_n() const = 0; │ │ │ │ │ -116 │ │ │ │ │ -118 _V_e_c_t_o_r_V_a_l_u_e_s hessianDiagonal() const; │ │ │ │ │ -119 │ │ │ │ │ -_1_2_1 virtual void _h_e_s_s_i_a_n_D_i_a_g_o_n_a_l_A_d_d(_V_e_c_t_o_r_V_a_l_u_e_s& d) const = 0; │ │ │ │ │ -122 │ │ │ │ │ -_1_2_4 virtual void _h_e_s_s_i_a_n_D_i_a_g_o_n_a_l(double* d) const = 0; │ │ │ │ │ -125 │ │ │ │ │ -_1_2_7 virtual std::map _h_e_s_s_i_a_n_B_l_o_c_k_D_i_a_g_o_n_a_l() const = 0; │ │ │ │ │ -128 │ │ │ │ │ -_1_3_0 virtual _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r _c_l_o_n_e() const = 0; │ │ │ │ │ -131 │ │ │ │ │ -_1_3_7 virtual _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r _n_e_g_a_t_e() const = 0; │ │ │ │ │ -138 │ │ │ │ │ -_1_4_4 virtual void _u_p_d_a_t_e_H_e_s_s_i_a_n(const _K_e_y_V_e_c_t_o_r& keys, │ │ │ │ │ -145 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x* info) const = 0; │ │ │ │ │ -146 │ │ │ │ │ -_1_4_8 virtual void _m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d(double alpha, const _V_e_c_t_o_r_V_a_l_u_e_s& x, │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s& y) const = 0; │ │ │ │ │ -149 │ │ │ │ │ -_1_5_1 virtual _V_e_c_t_o_r_V_a_l_u_e_s _g_r_a_d_i_e_n_t_A_t_Z_e_r_o() const = 0; │ │ │ │ │ -152 │ │ │ │ │ -_1_5_4 virtual void _g_r_a_d_i_e_n_t_A_t_Z_e_r_o(double* d) const = 0; │ │ │ │ │ -155 │ │ │ │ │ -_1_5_7 virtual Vector _g_r_a_d_i_e_n_t(_K_e_y key, const _V_e_c_t_o_r_V_a_l_u_e_s& x) const = 0; │ │ │ │ │ -158 │ │ │ │ │ -159 // Determine position of a given key │ │ │ │ │ -160 template │ │ │ │ │ -161 static _D_e_n_s_e_I_n_d_e_x Slot(const CONTAINER& keys, _K_e_y key) { │ │ │ │ │ -162 return std::find(keys.begin(), keys.end(), key) - keys.begin(); │ │ │ │ │ -163 } │ │ │ │ │ -164 │ │ │ │ │ -165 private: │ │ │ │ │ -_1_6_7 friend class boost::serialization::access; │ │ │ │ │ -168 template │ │ │ │ │ -169 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -170 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -171 } │ │ │ │ │ -172 │ │ │ │ │ -173 }; // GaussianFactor │ │ │ │ │ -174 │ │ │ │ │ -176template<> │ │ │ │ │ -_1_7_7struct _t_r_a_i_t_s<_G_a_u_s_s_i_a_n_F_a_c_t_o_r> : public _T_e_s_t_a_b_l_e { │ │ │ │ │ -178}; │ │ │ │ │ -179 │ │ │ │ │ -180} // \ namespace gtsam │ │ │ │ │ -_T_e_s_t_a_b_l_e_._h │ │ │ │ │ -Concept check for values that can be used in unit tests. │ │ │ │ │ -_M_a_t_r_i_x_._h │ │ │ │ │ -typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ -_F_a_c_t_o_r_._h │ │ │ │ │ -The base class for all factors. │ │ │ │ │ +56} │ │ │ │ │ +_J_u_n_c_t_i_o_n_T_r_e_e_._h │ │ │ │ │ +The junction tree. │ │ │ │ │ +_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ +_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ +Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_n_s_e_I_n_d_e_x │ │ │ │ │ -ptrdiff_t DenseIndex │ │ │ │ │ -The index type for Eigen objects. │ │ │ │ │ -DDeeffiinniittiioonn types.h:106 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ -of blocks. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:68 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_c_o_n_s_t___i_t_e_r_a_t_o_r │ │ │ │ │ -KeyVector::const_iterator const_iterator │ │ │ │ │ -Const iterator over keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:80 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -An abstract virtual base class for JacobianFactor and HessianFactor. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactor.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_g_e_t_D_i_m │ │ │ │ │ -virtual DenseIndex getDim(const_iterator variable) const =0 │ │ │ │ │ -Return the dimension of the variable pointed to by the given key iterator. │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_h_e_s_s_i_a_n_B_l_o_c_k_D_i_a_g_o_n_a_l │ │ │ │ │ -virtual std::map< Key, Matrix > hessianBlockDiagonal() const =0 │ │ │ │ │ -Return the block diagonal of the Hessian for this factor. │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_j_a_c_o_b_i_a_n │ │ │ │ │ -virtual std::pair< Matrix, Vector > jacobian() const =0 │ │ │ │ │ -Return the dense Jacobian and right-hand-side , with the noise models baked │ │ │ │ │ -into A and b. │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_g_r_a_d_i_e_n_t │ │ │ │ │ -virtual Vector gradient(Key key, const VectorValues &x) const =0 │ │ │ │ │ -Gradient wrt a key at any values. │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ -virtual GaussianFactor::shared_ptr clone() const =0 │ │ │ │ │ -Clone a factor (make a deep copy) │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_i_n_f_o_r_m_a_t_i_o_n │ │ │ │ │ -virtual Matrix information() const =0 │ │ │ │ │ -Return the non-augmented information matrix represented by this GaussianFactor. │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_B_a_s_e │ │ │ │ │ -Factor Base │ │ │ │ │ -Our base class. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactor.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_~_G_a_u_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -virtual ~GaussianFactor() │ │ │ │ │ -Destructor. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactor.h:54 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_g_r_a_d_i_e_n_t_A_t_Z_e_r_o │ │ │ │ │ -virtual VectorValues gradientAtZero() const =0 │ │ │ │ │ -A'*b for Jacobian, eta for Hessian. │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -GaussianFactor(const CONTAINER &keys) │ │ │ │ │ -Construct from container of keys. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactor.h:51 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d │ │ │ │ │ -virtual void multiplyHessianAdd(double alpha, const VectorValues &x, │ │ │ │ │ -VectorValues &y) const =0 │ │ │ │ │ -y += alpha * A'*A*x │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ -virtual bool equals(const GaussianFactor &lf, double tol=1e-9) const =0 │ │ │ │ │ -Equals for testable. │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_a_u_g_m_e_n_t_e_d_I_n_f_o_r_m_a_t_i_o_n │ │ │ │ │ -virtual Matrix augmentedInformation() const =0 │ │ │ │ │ -Return the augmented information matrix represented by this GaussianFactor. │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_a_u_g_m_e_n_t_e_d_J_a_c_o_b_i_a_n │ │ │ │ │ -virtual Matrix augmentedJacobian() const =0 │ │ │ │ │ -Return a dense Jacobian matrix, augmented with b with the noise models baked │ │ │ │ │ -into A and b. │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -GaussianFactor() │ │ │ │ │ -Default constructor creates empty factor. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactor.h:46 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_n_e_g_a_t_e │ │ │ │ │ -virtual GaussianFactor::shared_ptr negate() const =0 │ │ │ │ │ -Construct the corresponding anti-factor to negate information stored stored in │ │ │ │ │ -this factor. │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_h_e_s_s_i_a_n_D_i_a_g_o_n_a_l │ │ │ │ │ -virtual void hessianDiagonal(double *d) const =0 │ │ │ │ │ -Raw memory access version of hessianDiagonal. │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_T_h_i_s │ │ │ │ │ -GaussianFactor This │ │ │ │ │ +_g_t_s_a_m_:_:_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ +A JunctionTree is a cluster tree, a set of variable clusters with factors, │ │ │ │ │ +arranged in a tree,... │ │ │ │ │ +DDeeffiinniittiioonn JunctionTree.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ +DDeeffiinniittiioonn GaussianEliminationTree.h:29 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ +A junction tree specialized to Gaussian factors, i.e., it is a cluster tree │ │ │ │ │ +with Gaussian factors sto... │ │ │ │ │ +DDeeffiinniittiioonn GaussianJunctionTree.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_J_u_n_c_t_i_o_n_T_r_e_e_:_:_T_h_i_s │ │ │ │ │ +GaussianJunctionTree This │ │ │ │ │ This class. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactor.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_u_p_d_a_t_e_H_e_s_s_i_a_n │ │ │ │ │ -virtual void updateHessian(const KeyVector &keys, SymmetricBlockMatrix *info) │ │ │ │ │ -const =0 │ │ │ │ │ -Update an information matrix by adding the information corresponding to this │ │ │ │ │ -factor (used internally ... │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&formatter=DefaultKeyFormatter) const override=0 │ │ │ │ │ -print │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_h_e_s_s_i_a_n_D_i_a_g_o_n_a_l_A_d_d │ │ │ │ │ -virtual void hessianDiagonalAdd(VectorValues &d) const =0 │ │ │ │ │ -Add the current diagonal to a VectorValues instance. │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_g_r_a_d_i_e_n_t_A_t_Z_e_r_o │ │ │ │ │ -virtual void gradientAtZero(double *d) const =0 │ │ │ │ │ -Raw memory access version of gradientAtZero. │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ -with a unique integer... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -The Factor::error simply extracts the. │ │ │ │ │ +DDeeffiinniittiioonn GaussianJunctionTree.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_J_u_n_c_t_i_o_n_T_r_e_e_:_:_B_a_s_e │ │ │ │ │ +JunctionTree< GaussianBayesTree, GaussianFactorGraph > Base │ │ │ │ │ +Base class. │ │ │ │ │ +DDeeffiinniittiioonn GaussianJunctionTree.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_J_u_n_c_t_i_o_n_T_r_e_e_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +Shared pointer to this class. │ │ │ │ │ +DDeeffiinniittiioonn GaussianJunctionTree.h:43 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ + * _G_a_u_s_s_i_a_n_J_u_n_c_t_i_o_n_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00791.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Errors.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/VectorValues.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,70 +96,48 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces | │ │ │ │ Functions
│ │ │ │ -
Errors.cpp File Reference
│ │ │ │ +
VectorValues.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Factor Graph Values. │ │ │ │ +

Implementations for VectorValues. │ │ │ │ More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

│ │ │ │ -Errors gtsam::createErrors (const VectorValues &V)
 Break V into pieces according to its start indices.
 
│ │ │ │ -void gtsam::print (const Errors &e, const std::string &s="Errors")
 Print an Errors instance.
 
│ │ │ │ -bool gtsam::equality (const Errors &actual, const Errors &expected, double tol)
 
│ │ │ │ -Errors gtsam::operator+ (const Errors &a, const Errors &b)
 Addition.
 
│ │ │ │ -Errors gtsam::operator- (const Errors &a, const Errors &b)
 Subtraction.
 
│ │ │ │ -Errors gtsam::operator- (const Errors &a)
 Negation.
 
│ │ │ │ -double gtsam::dot (const Errors &a, const Errors &b)
 Dot product.
 
│ │ │ │ -void gtsam::axpy (double alpha, const Errors &x, Errors &y)
 BLAS level 2 style AXPY, y := alpha*x + y
 
│ │ │ │ +GTSAM_EXPORT ostream & gtsam::operator<< (ostream &os, const VectorValues &v)
 
│ │ │ │ +bool gtsam::internal::structureCompareOp (const boost::tuple< VectorValues::value_type, VectorValues::value_type > &vv)
 
│ │ │ │ +VectorValues gtsam::operator* (const double a, const VectorValues &v)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Factor Graph Values.

│ │ │ │ -

Errors

Author
Carlos Nieto
│ │ │ │ +

Implementations for VectorValues.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │
│ │ │ │ -Christian Potthast
│ │ │ │ +Alex Cunningham
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,47 +1,31 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Errors.cpp File Reference │ │ │ │ │ -Factor Graph _V_a_l_u_e_s. _M_o_r_e_._._. │ │ │ │ │ +VectorValues.cpp File Reference │ │ │ │ │ +Implementations for _V_e_c_t_o_r_V_a_l_u_e_s. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -_E_r_r_o_r_s  ggttssaamm::::ccrreeaatteeEErrrroorrss (const _V_e_c_t_o_r_V_a_l_u_e_s &V) │ │ │ │ │ -  Break V into pieces according to its start indices. │ │ │ │ │ +GTSAM_EXPORT ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const _V_e_c_t_o_r_V_a_l_u_e_s &v) │ │ │ │ │   │ │ │ │ │ - void  ggttssaamm::::pprriinntt (const _E_r_r_o_r_s &e, const std::string &s="Errors") │ │ │ │ │ -  Print an Errors instance. │ │ │ │ │ + bool  ggttssaamm::::iinntteerrnnaall::::ssttrruuccttuurreeCCoommppaarreeOOpp (const boost:: │ │ │ │ │ + tuple< _V_e_c_t_o_r_V_a_l_u_e_s_:_:_v_a_l_u_e___t_y_p_e, _V_e_c_t_o_r_V_a_l_u_e_s_:_: │ │ │ │ │ + _v_a_l_u_e___t_y_p_e > &vv) │ │ │ │ │   │ │ │ │ │ - bool  ggttssaamm::::eeqquuaalliittyy (const _E_r_r_o_r_s &actual, const _E_r_r_o_r_s &expected, double │ │ │ │ │ - tol) │ │ │ │ │ -  │ │ │ │ │ -_E_r_r_o_r_s  ggttssaamm::::ooppeerraattoorr++ (const _E_r_r_o_r_s &a, const _E_r_r_o_r_s &b) │ │ │ │ │ -  Addition. │ │ │ │ │ -  │ │ │ │ │ -_E_r_r_o_r_s  ggttssaamm::::ooppeerraattoorr-- (const _E_r_r_o_r_s &a, const _E_r_r_o_r_s &b) │ │ │ │ │ -  Subtraction. │ │ │ │ │ -  │ │ │ │ │ -_E_r_r_o_r_s  ggttssaamm::::ooppeerraattoorr-- (const _E_r_r_o_r_s &a) │ │ │ │ │ -  Negation. │ │ │ │ │ -  │ │ │ │ │ -double  ggttssaamm::::ddoott (const _E_r_r_o_r_s &a, const _E_r_r_o_r_s &b) │ │ │ │ │ -  Dot product. │ │ │ │ │ -  │ │ │ │ │ - void  ggttssaamm::::aaxxppyy (double alpha, const _E_r_r_o_r_s &x, _E_r_r_o_r_s &y) │ │ │ │ │ -  BLAS level 2 style AXPY, y := alpha*x + y │ │ │ │ │ + _V_e_c_t_o_r_V_a_l_u_e_s  ggttssaamm::::ooppeerraattoorr** (const double a, const _V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ + &v) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Factor Graph _V_a_l_u_e_s. │ │ │ │ │ -Errors │ │ │ │ │ +Implementations for _V_e_c_t_o_r_V_a_l_u_e_s. │ │ │ │ │ Author │ │ │ │ │ - Carlos Nieto │ │ │ │ │ - Christian Potthast │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Alex Cunningham │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _E_r_r_o_r_s_._c_p_p │ │ │ │ │ + * _V_e_c_t_o_r_V_a_l_u_e_s_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00794.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/SubgraphSolver.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/SubgraphPreconditioner.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,47 +96,41 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
SubgraphSolver.h File Reference
│ │ │ │ +
SubgraphPreconditioner.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Subgraph Solver from IROS 2010. │ │ │ │ -More...

│ │ │ │ - │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

struct  gtsam::SubgraphSolverParameters
struct  gtsam::SubgraphPreconditionerParameters
 
class  gtsam::SubgraphSolver
 This class implements the linear SPCG solver presented in Dellaert et al in IROS'10. More...
class  gtsam::SubgraphPreconditioner
 Subgraph conditioner class, as explained in the RSS 2010 submission. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Subgraph Solver from IROS 2010.

│ │ │ │ -
Date
2010
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ -Yong Dian Jian
│ │ │ │ +
Date
Dec 31, 2009
│ │ │ │ +
Author
Frank Dellaert, Yong-Dian Jian
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,31 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SubgraphSolver.h File Reference │ │ │ │ │ -Subgraph Solver from IROS 2010. _M_o_r_e_._._. │ │ │ │ │ +SubgraphPreconditioner.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_S_o_l_v_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ +struct   _g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │   │ │ │ │ │ - class   _g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_S_o_l_v_e_r │ │ │ │ │ -  This class implements the linear SPCG solver presented in Dellaert et │ │ │ │ │ - al in IROS'10. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r │ │ │ │ │ +  _S_u_b_g_r_a_p_h conditioner class, as explained in the RSS 2010 submission. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Subgraph Solver from IROS 2010. │ │ │ │ │ Date │ │ │ │ │ - 2010 │ │ │ │ │ + Dec 31, 2009 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Yong Dian Jian │ │ │ │ │ + Frank Dellaert, Yong-Dian Jian │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _S_u_b_g_r_a_p_h_S_o_l_v_e_r_._h │ │ │ │ │ + * _S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00794.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a00794 = [ │ │ │ │ │ - ["gtsam::SubgraphSolverParameters", "a04056.html", null], │ │ │ │ │ - ["gtsam::SubgraphSolver", "a04060.html", "a04060"] │ │ │ │ │ + ["gtsam::SubgraphPreconditionerParameters", "a04048.html", null], │ │ │ │ │ + ["gtsam::SubgraphPreconditioner", "a04052.html", "a04052"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00794_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/SubgraphSolver.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/SubgraphPreconditioner.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,114 +98,158 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
SubgraphSolver.h
│ │ │ │ +
SubgraphPreconditioner.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/linear/ConjugateGradientSolver.h>
│ │ │ │ -
23#include <gtsam/linear/SubgraphBuilder.h>
│ │ │ │ -
24
│ │ │ │ -
25#include <map>
│ │ │ │ -
26#include <utility> // pair
│ │ │ │ -
27
│ │ │ │ -
28namespace gtsam {
│ │ │ │ -
29
│ │ │ │ -
30// Forward declarations
│ │ │ │ -
31class GaussianFactorGraph;
│ │ │ │ -
32class GaussianBayesNet;
│ │ │ │ -
33class SubgraphPreconditioner;
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/linear/SubgraphBuilder.h>
│ │ │ │ +
21#include <gtsam/linear/Errors.h>
│ │ │ │ +
22#include <gtsam/linear/GaussianBayesNet.h>
│ │ │ │ +
23#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ +
24#include <gtsam/linear/IterativeSolver.h>
│ │ │ │ +
25#include <gtsam/linear/Preconditioner.h>
│ │ │ │ +
26#include <gtsam/linear/VectorValues.h>
│ │ │ │ +
27#include <gtsam/dllexport.h>
│ │ │ │ +
28
│ │ │ │ +
29#include <boost/shared_ptr.hpp>
│ │ │ │ +
30
│ │ │ │ +
31#include <map>
│ │ │ │ +
32
│ │ │ │ +
33namespace gtsam {
│ │ │ │
34
│ │ │ │ -
│ │ │ │ -
35struct GTSAM_EXPORT SubgraphSolverParameters
│ │ │ │ -
36 : public ConjugateGradientParameters {
│ │ │ │ -
37 SubgraphBuilderParameters builderParams;
│ │ │ │ -
38 explicit SubgraphSolverParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters())
│ │ │ │ -
39 : builderParams(p) {}
│ │ │ │ -
40 void print() const { Base::print(); }
│ │ │ │ -
41 void print(std::ostream &os) const override {
│ │ │ │ -
42 Base::print(os);
│ │ │ │ -
43 }
│ │ │ │ -
44};
│ │ │ │ -
│ │ │ │ -
45
│ │ │ │ -
│ │ │ │ -
76class GTSAM_EXPORT SubgraphSolver : public IterativeSolver {
│ │ │ │ -
77 public:
│ │ │ │ -
78 typedef SubgraphSolverParameters Parameters;
│ │ │ │ -
79
│ │ │ │ -
80 protected:
│ │ │ │ -
81 Parameters parameters_;
│ │ │ │ -
82 boost::shared_ptr<SubgraphPreconditioner> pc_;
│ │ │ │ -
83
│ │ │ │ -
84 public:
│ │ │ │ -
87
│ │ │ │ -
93 SubgraphSolver(const GaussianFactorGraph &A, const Parameters &parameters,
│ │ │ │ -
94 const Ordering &ordering);
│ │ │ │ -
95
│ │ │ │ -
102 SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2,
│ │ │ │ -
103 const Parameters &parameters, const Ordering &ordering);
│ │ │ │ -
108 SubgraphSolver(const GaussianBayesNet &Rc1, const GaussianFactorGraph &Ab2,
│ │ │ │ -
109 const Parameters &parameters);
│ │ │ │ -
110
│ │ │ │ -
112 ~SubgraphSolver() override {}
│ │ │ │ -
113
│ │ │ │ -
117
│ │ │ │ -
119 VectorValues optimize() const;
│ │ │ │ -
120
│ │ │ │ -
122 VectorValues optimize(const GaussianFactorGraph &gfg,
│ │ │ │ -
123 const KeyInfo &keyInfo,
│ │ │ │ -
124 const std::map<Key, Vector> &lambda,
│ │ │ │ -
125 const VectorValues &initial) override;
│ │ │ │ -
126
│ │ │ │ +
35 // Forward declarations
│ │ │ │ +
36 class GaussianBayesNet;
│ │ │ │ +
37 class GaussianFactorGraph;
│ │ │ │ +
38 class VectorValues;
│ │ │ │ +
39
│ │ │ │ +
│ │ │ │ +
40 struct GTSAM_EXPORT SubgraphPreconditionerParameters : public PreconditionerParameters {
│ │ │ │ +
41 typedef boost::shared_ptr<SubgraphPreconditionerParameters> shared_ptr;
│ │ │ │ +
42 SubgraphPreconditionerParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters())
│ │ │ │ +
43 : builderParams(p) {}
│ │ │ │ +
44 SubgraphBuilderParameters builderParams;
│ │ │ │ +
45 };
│ │ │ │ +
│ │ │ │ +
46
│ │ │ │ +
│ │ │ │ +
54 class GTSAM_EXPORT SubgraphPreconditioner : public Preconditioner {
│ │ │ │ +
55
│ │ │ │ +
56 public:
│ │ │ │ +
57 typedef boost::shared_ptr<SubgraphPreconditioner> shared_ptr;
│ │ │ │ +
58
│ │ │ │ +
59 private:
│ │ │ │ +
60 GaussianFactorGraph Ab2_;
│ │ │ │ +
61 GaussianBayesNet Rc1_;
│ │ │ │ +
62 VectorValues xbar_;
│ │ │ │ +
63 Errors b2bar_;
│ │ │ │ +
64
│ │ │ │ +
65 KeyInfo keyInfo_;
│ │ │ │ +
66 SubgraphPreconditionerParameters parameters_;
│ │ │ │ +
67
│ │ │ │ +
68 public:
│ │ │ │ +
69
│ │ │ │ +
70 SubgraphPreconditioner(const SubgraphPreconditionerParameters &p = SubgraphPreconditionerParameters());
│ │ │ │ +
71
│ │ │ │ +
78 SubgraphPreconditioner(const GaussianFactorGraph& Ab2, const GaussianBayesNet& Rc1, const VectorValues& xbar,
│ │ │ │ +
79 const SubgraphPreconditionerParameters &p = SubgraphPreconditionerParameters());
│ │ │ │ +
80
│ │ │ │ +
81 ~SubgraphPreconditioner() override {}
│ │ │ │ +
82
│ │ │ │ +
84 void print(const std::string& s = "SubgraphPreconditioner") const;
│ │ │ │ +
85
│ │ │ │ +
87 const GaussianFactorGraph& Ab2() const { return Ab2_; }
│ │ │ │ +
88
│ │ │ │ +
90 const GaussianBayesNet& Rc1() const { return Rc1_; }
│ │ │ │ +
91
│ │ │ │ +
93 const Errors b2bar() const { return b2bar_; }
│ │ │ │ +
94
│ │ │ │ +
100 /* x = xbar + inv(R1)*y */
│ │ │ │ +
101 VectorValues x(const VectorValues& y) const;
│ │ │ │ +
102
│ │ │ │ +
103 /* A zero VectorValues with the structure of xbar */
│ │ │ │ +
104 VectorValues zero() const {
│ │ │ │ +
105 return VectorValues::Zero(xbar_);
│ │ │ │ +
106 }
│ │ │ │ +
107
│ │ │ │ +
113 void transposeMultiplyAdd2(double alpha, Errors::const_iterator begin,
│ │ │ │ +
114 Errors::const_iterator end, VectorValues& y) const;
│ │ │ │ +
115
│ │ │ │ +
116 /* error, given y */
│ │ │ │ +
117 double error(const VectorValues& y) const;
│ │ │ │ +
118
│ │ │ │ +
120 VectorValues gradient(const VectorValues& y) const;
│ │ │ │ +
121
│ │ │ │ +
123 Errors operator*(const VectorValues& y) const;
│ │ │ │ +
124
│ │ │ │ +
126 void multiplyInPlace(const VectorValues& y, Errors& e) const;
│ │ │ │ +
127
│ │ │ │ +
129 VectorValues operator^(const Errors& e) const;
│ │ │ │
130
│ │ │ │ -
132 std::pair<GaussianFactorGraph, GaussianFactorGraph> splitGraph(
│ │ │ │ -
133 const GaussianFactorGraph &gfg);
│ │ │ │ -
134
│ │ │ │ -
136};
│ │ │ │ -
│ │ │ │ -
137
│ │ │ │ -
138} // namespace gtsam
│ │ │ │ -
ConjugateGradientSolver.h
Implementation of Conjugate Gradient solver for a linear system.
│ │ │ │ - │ │ │ │ +
135 void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& y) const;
│ │ │ │ +
136
│ │ │ │ +
137 /*****************************************************************************/
│ │ │ │ +
138 /* implement virtual functions of Preconditioner */
│ │ │ │ +
139
│ │ │ │ +
141 void solve(const Vector& y, Vector &x) const override;
│ │ │ │ +
142
│ │ │ │ +
144 void transposeSolve(const Vector& y, Vector& x) const override;
│ │ │ │ +
145
│ │ │ │ +
147 void build(
│ │ │ │ +
148 const GaussianFactorGraph &gfg,
│ │ │ │ +
149 const KeyInfo &info,
│ │ │ │ +
150 const std::map<Key,Vector> &lambda
│ │ │ │ +
151 ) override;
│ │ │ │ +
152 /*****************************************************************************/
│ │ │ │ +
153 };
│ │ │ │ +
│ │ │ │ +
154
│ │ │ │ +
155} // namespace gtsam
│ │ │ │ +
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ +
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
│ │ │ │ +
IterativeSolver.h
Some support classes for iterative solvers.
│ │ │ │ +
Errors.h
vector of errors
│ │ │ │ + │ │ │ │ +
VectorValues.h
Factor Graph Values.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ -
gtsam::ConjugateGradientParameters
parameters for the conjugate gradient method
Definition ConjugateGradientSolver.h:29
│ │ │ │ + │ │ │ │
gtsam::GaussianBayesNet
GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals.
Definition GaussianBayesNet.h:36
│ │ │ │
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ -
gtsam::IterativeSolver
Base class for Iterative Solvers like SubgraphSolver.
Definition IterativeSolver.h:86
│ │ │ │
gtsam::KeyInfo
Handy data structure for iterative solvers.
Definition IterativeSolver.h:126
│ │ │ │ +
gtsam::PreconditionerParameters
Definition Preconditioner.h:24
│ │ │ │ +
gtsam::Preconditioner
Definition Preconditioner.h:64
│ │ │ │
gtsam::SubgraphBuilderParameters
Definition SubgraphBuilder.h:96
│ │ │ │ -
gtsam::SubgraphSolverParameters
Definition SubgraphSolver.h:36
│ │ │ │ -
gtsam::SubgraphSolver
This class implements the linear SPCG solver presented in Dellaert et al in IROS'10.
Definition SubgraphSolver.h:76
│ │ │ │ -
gtsam::SubgraphSolver::~SubgraphSolver
~SubgraphSolver() override
Destructor.
Definition SubgraphSolver.h:112
│ │ │ │ -
gtsam::SubgraphSolver::pc_
boost::shared_ptr< SubgraphPreconditioner > pc_
preconditioner object
Definition SubgraphSolver.h:82
│ │ │ │ +
gtsam::SubgraphPreconditionerParameters
Definition SubgraphPreconditioner.h:40
│ │ │ │ +
gtsam::SubgraphPreconditioner
Subgraph conditioner class, as explained in the RSS 2010 submission.
Definition SubgraphPreconditioner.h:54
│ │ │ │ +
gtsam::SubgraphPreconditioner::Ab2
const GaussianFactorGraph & Ab2() const
Access Ab2.
Definition SubgraphPreconditioner.h:87
│ │ │ │ +
gtsam::SubgraphPreconditioner::b2bar
const Errors b2bar() const
Access b2bar.
Definition SubgraphPreconditioner.h:93
│ │ │ │ +
gtsam::SubgraphPreconditioner::Rc1
const GaussianBayesNet & Rc1() const
Access Rc1.
Definition SubgraphPreconditioner.h:90
│ │ │ │
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
VectorValues
The Factor::error simply extracts the.
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,139 +1,189 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SubgraphSolver.h │ │ │ │ │ +SubgraphPreconditioner.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_S_o_l_v_e_r_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_._h> │ │ │ │ │ -24 │ │ │ │ │ -25#include │ │ │ │ │ -26#include // pair │ │ │ │ │ -27 │ │ │ │ │ -28namespace _g_t_s_a_m { │ │ │ │ │ -29 │ │ │ │ │ -30// Forward declarations │ │ │ │ │ -31class GaussianFactorGraph; │ │ │ │ │ -32class GaussianBayesNet; │ │ │ │ │ -33class SubgraphPreconditioner; │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_E_r_r_o_r_s_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_I_t_e_r_a_t_i_v_e_S_o_l_v_e_r_._h> │ │ │ │ │ +25#include │ │ │ │ │ +26#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ +27#include │ │ │ │ │ +28 │ │ │ │ │ +29#include │ │ │ │ │ +30 │ │ │ │ │ +31#include │ │ │ │ │ +32 │ │ │ │ │ +33namespace _g_t_s_a_m { │ │ │ │ │ 34 │ │ │ │ │ -_3_5struct GTSAM_EXPORT _S_u_b_g_r_a_p_h_S_o_l_v_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ -36 : public _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s { │ │ │ │ │ -37 _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s builderParams; │ │ │ │ │ -38 explicit _S_u_b_g_r_a_p_h_S_o_l_v_e_r_P_a_r_a_m_e_t_e_r_s(const _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s &p = │ │ │ │ │ +35 // Forward declarations │ │ │ │ │ +36 class GaussianBayesNet; │ │ │ │ │ +37 class GaussianFactorGraph; │ │ │ │ │ +38 class _V_e_c_t_o_r_V_a_l_u_e_s; │ │ │ │ │ +39 │ │ │ │ │ +_4_0 struct GTSAM_EXPORT _S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s : public │ │ │ │ │ +_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s { │ │ │ │ │ +41 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +42 _S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s(const _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s &p = │ │ │ │ │ _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s()) │ │ │ │ │ -39 : builderParams(p) {} │ │ │ │ │ -40 void _p_r_i_n_t() const { Base::print(); } │ │ │ │ │ -41 void _p_r_i_n_t(std::ostream &os) const override { │ │ │ │ │ -42 Base::print(os); │ │ │ │ │ -43 } │ │ │ │ │ -44}; │ │ │ │ │ -45 │ │ │ │ │ -_7_6class GTSAM_EXPORT _S_u_b_g_r_a_p_h_S_o_l_v_e_r : public _I_t_e_r_a_t_i_v_e_S_o_l_v_e_r { │ │ │ │ │ -77 public: │ │ │ │ │ -78 typedef _S_u_b_g_r_a_p_h_S_o_l_v_e_r_P_a_r_a_m_e_t_e_r_s _P_a_r_a_m_e_t_e_r_s; │ │ │ │ │ -79 │ │ │ │ │ -80 protected: │ │ │ │ │ -81 _P_a_r_a_m_e_t_e_r_s parameters_; │ │ │ │ │ -_8_2 boost::shared_ptr _p_c__; │ │ │ │ │ -83 │ │ │ │ │ -84 public: │ │ │ │ │ -87 │ │ │ │ │ -93 _S_u_b_g_r_a_p_h_S_o_l_v_e_r(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &A, const _P_a_r_a_m_e_t_e_r_s ¶meters, │ │ │ │ │ -94 const _O_r_d_e_r_i_n_g &ordering); │ │ │ │ │ -95 │ │ │ │ │ -102 _S_u_b_g_r_a_p_h_S_o_l_v_e_r(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &Ab1, const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -&Ab2, │ │ │ │ │ -103 const _P_a_r_a_m_e_t_e_r_s ¶meters, const _O_r_d_e_r_i_n_g &ordering); │ │ │ │ │ -108 _S_u_b_g_r_a_p_h_S_o_l_v_e_r(const _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t &Rc1, const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &Ab2, │ │ │ │ │ -109 const _P_a_r_a_m_e_t_e_r_s ¶meters); │ │ │ │ │ -110 │ │ │ │ │ -_1_1_2 _~_S_u_b_g_r_a_p_h_S_o_l_v_e_r() override {} │ │ │ │ │ -113 │ │ │ │ │ -117 │ │ │ │ │ -119 _V_e_c_t_o_r_V_a_l_u_e_s _o_p_t_i_m_i_z_e() const; │ │ │ │ │ -120 │ │ │ │ │ -122 _V_e_c_t_o_r_V_a_l_u_e_s _o_p_t_i_m_i_z_e(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, │ │ │ │ │ -123 const _K_e_y_I_n_f_o &keyInfo, │ │ │ │ │ -124 const std::map &lambda, │ │ │ │ │ -125 const _V_e_c_t_o_r_V_a_l_u_e_s &initial) override; │ │ │ │ │ -126 │ │ │ │ │ +43 : builderParams(p) {} │ │ │ │ │ +44 _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s builderParams; │ │ │ │ │ +45 }; │ │ │ │ │ +46 │ │ │ │ │ +_5_4 class GTSAM_EXPORT _S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r : public _P_r_e_c_o_n_d_i_t_i_o_n_e_r { │ │ │ │ │ +55 │ │ │ │ │ +56 public: │ │ │ │ │ +57 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +58 │ │ │ │ │ +59 private: │ │ │ │ │ +60 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h Ab2_; │ │ │ │ │ +61 _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t Rc1_; │ │ │ │ │ +62 _V_e_c_t_o_r_V_a_l_u_e_s xbar_; │ │ │ │ │ +63 _E_r_r_o_r_s b2bar_; │ │ │ │ │ +64 │ │ │ │ │ +65 _K_e_y_I_n_f_o keyInfo_; │ │ │ │ │ +66 _S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s parameters_; │ │ │ │ │ +67 │ │ │ │ │ +68 public: │ │ │ │ │ +69 │ │ │ │ │ +70 _S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r(const _S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s &p = │ │ │ │ │ +_S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s()); │ │ │ │ │ +71 │ │ │ │ │ +78 _S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& Ab2, const │ │ │ │ │ +_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t& Rc1, const _V_e_c_t_o_r_V_a_l_u_e_s& xbar, │ │ │ │ │ +79 const _S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s &p = _S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ +()); │ │ │ │ │ +80 │ │ │ │ │ +81 _~_S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r() override {} │ │ │ │ │ +82 │ │ │ │ │ +84 void _p_r_i_n_t(const std::string& s = "SubgraphPreconditioner") const; │ │ │ │ │ +85 │ │ │ │ │ +_8_7 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& _A_b_2() const { return Ab2_; } │ │ │ │ │ +88 │ │ │ │ │ +_9_0 const _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t& _R_c_1() const { return Rc1_; } │ │ │ │ │ +91 │ │ │ │ │ +_9_3 const _E_r_r_o_r_s _b_2_b_a_r() const { return b2bar_; } │ │ │ │ │ +94 │ │ │ │ │ +100 /* x = xbar + inv(R1)*y */ │ │ │ │ │ +101 _V_e_c_t_o_r_V_a_l_u_e_s x(const _V_e_c_t_o_r_V_a_l_u_e_s& y) const; │ │ │ │ │ +102 │ │ │ │ │ +103 /* A zero VectorValues with the structure of xbar */ │ │ │ │ │ +104 _V_e_c_t_o_r_V_a_l_u_e_s zero() const { │ │ │ │ │ +105 return VectorValues::Zero(xbar_); │ │ │ │ │ +106 } │ │ │ │ │ +107 │ │ │ │ │ +113 void transposeMultiplyAdd2(double alpha, Errors::const_iterator begin, │ │ │ │ │ +114 Errors::const_iterator end, _V_e_c_t_o_r_V_a_l_u_e_s& y) const; │ │ │ │ │ +115 │ │ │ │ │ +116 /* error, given y */ │ │ │ │ │ +117 double error(const _V_e_c_t_o_r_V_a_l_u_e_s& y) const; │ │ │ │ │ +118 │ │ │ │ │ +120 _V_e_c_t_o_r_V_a_l_u_e_s gradient(const _V_e_c_t_o_r_V_a_l_u_e_s& y) const; │ │ │ │ │ +121 │ │ │ │ │ +123 Errors operator*(const _V_e_c_t_o_r_V_a_l_u_e_s& y) const; │ │ │ │ │ +124 │ │ │ │ │ +126 void multiplyInPlace(const _V_e_c_t_o_r_V_a_l_u_e_s& y, Errors& e) const; │ │ │ │ │ +127 │ │ │ │ │ +129 _V_e_c_t_o_r_V_a_l_u_e_s operator^(const Errors& e) const; │ │ │ │ │ 130 │ │ │ │ │ -132 std::pair splitGraph( │ │ │ │ │ -133 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg); │ │ │ │ │ -134 │ │ │ │ │ -136}; │ │ │ │ │ -137 │ │ │ │ │ -138} // namespace gtsam │ │ │ │ │ -_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_S_o_l_v_e_r_._h │ │ │ │ │ -Implementation of Conjugate Gradient solver for a linear system. │ │ │ │ │ +135 void transposeMultiplyAdd(double alpha, const Errors& e, _V_e_c_t_o_r_V_a_l_u_e_s& y) │ │ │ │ │ +const; │ │ │ │ │ +136 │ │ │ │ │ +137 / │ │ │ │ │ +*****************************************************************************/ │ │ │ │ │ +138 /* implement virtual functions of Preconditioner */ │ │ │ │ │ +139 │ │ │ │ │ +141 void solve(const Vector& y, Vector &x) const override; │ │ │ │ │ +142 │ │ │ │ │ +144 void transposeSolve(const Vector& y, Vector& x) const override; │ │ │ │ │ +145 │ │ │ │ │ +147 void build( │ │ │ │ │ +148 const GaussianFactorGraph &gfg, │ │ │ │ │ +149 const KeyInfo &info, │ │ │ │ │ +150 const std::map &lambda │ │ │ │ │ +151 ) override; │ │ │ │ │ +152 / │ │ │ │ │ +*****************************************************************************/ │ │ │ │ │ +153 }; │ │ │ │ │ +154 │ │ │ │ │ +155} // namespace gtsam │ │ │ │ │ +_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ +_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h │ │ │ │ │ +Chordal Bayes Net, the result of eliminating a factor graph. │ │ │ │ │ +_I_t_e_r_a_t_i_v_e_S_o_l_v_e_r_._h │ │ │ │ │ +Some support classes for iterative solvers. │ │ │ │ │ +_E_r_r_o_r_s_._h │ │ │ │ │ +vector of errors │ │ │ │ │ _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_._h │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ +Factor Graph Values. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ -Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key │ │ │ │ │ -landmarkKey) │ │ │ │ │ -Optimize for triangulation. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.cpp:155 │ │ │ │ │ _g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ print without optional string, must specify cout yourself │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ -parameters for the conjugate gradient method │ │ │ │ │ -DDeeffiinniittiioonn ConjugateGradientSolver.h:29 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t_<_ _V_e_c_t_o_r_ _> │ │ │ │ │ _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t │ │ │ │ │ GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals. │ │ │ │ │ DDeeffiinniittiioonn GaussianBayesNet.h:36 │ │ │ │ │ _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_I_t_e_r_a_t_i_v_e_S_o_l_v_e_r │ │ │ │ │ -Base class for Iterative Solvers like SubgraphSolver. │ │ │ │ │ -DDeeffiinniittiioonn IterativeSolver.h:86 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_I_n_f_o │ │ │ │ │ Handy data structure for iterative solvers. │ │ │ │ │ DDeeffiinniittiioonn IterativeSolver.h:126 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ +DDeeffiinniittiioonn Preconditioner.h:24 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_c_o_n_d_i_t_i_o_n_e_r │ │ │ │ │ +DDeeffiinniittiioonn Preconditioner.h:64 │ │ │ │ │ _g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ DDeeffiinniittiioonn SubgraphBuilder.h:96 │ │ │ │ │ -_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_S_o_l_v_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ -DDeeffiinniittiioonn SubgraphSolver.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_S_o_l_v_e_r │ │ │ │ │ -This class implements the linear SPCG solver presented in Dellaert et al in │ │ │ │ │ -IROS'10. │ │ │ │ │ -DDeeffiinniittiioonn SubgraphSolver.h:76 │ │ │ │ │ -_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_S_o_l_v_e_r_:_:_~_S_u_b_g_r_a_p_h_S_o_l_v_e_r │ │ │ │ │ -~SubgraphSolver() override │ │ │ │ │ -Destructor. │ │ │ │ │ -DDeeffiinniittiioonn SubgraphSolver.h:112 │ │ │ │ │ -_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_S_o_l_v_e_r_:_:_p_c__ │ │ │ │ │ -boost::shared_ptr< SubgraphPreconditioner > pc_ │ │ │ │ │ -preconditioner object │ │ │ │ │ -DDeeffiinniittiioonn SubgraphSolver.h:82 │ │ │ │ │ +_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ +DDeeffiinniittiioonn SubgraphPreconditioner.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r │ │ │ │ │ +Subgraph conditioner class, as explained in the RSS 2010 submission. │ │ │ │ │ +DDeeffiinniittiioonn SubgraphPreconditioner.h:54 │ │ │ │ │ +_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_:_:_A_b_2 │ │ │ │ │ +const GaussianFactorGraph & Ab2() const │ │ │ │ │ +Access Ab2. │ │ │ │ │ +DDeeffiinniittiioonn SubgraphPreconditioner.h:87 │ │ │ │ │ +_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_:_:_b_2_b_a_r │ │ │ │ │ +const Errors b2bar() const │ │ │ │ │ +Access b2bar. │ │ │ │ │ +DDeeffiinniittiioonn SubgraphPreconditioner.h:93 │ │ │ │ │ +_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_:_:_R_c_1 │ │ │ │ │ +const GaussianBayesNet & Rc1() const │ │ │ │ │ +Access Rc1. │ │ │ │ │ +DDeeffiinniittiioonn SubgraphPreconditioner.h:90 │ │ │ │ │ _g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ with a unique integer... │ │ │ │ │ DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +The Factor::error simply extracts the. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _S_u_b_g_r_a_p_h_S_o_l_v_e_r_._h │ │ │ │ │ + * _S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00797.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/IterativeSolver.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/iterative.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,53 +95,75 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
IterativeSolver.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
iterative.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Some support classes for iterative solvers. │ │ │ │ +

Iterative methods, implementation. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::IterativeOptimizationParameters
 parameters for iterative linear solvers More...
 
class  gtsam::IterativeSolver
 Base class for Iterative Solvers like SubgraphSolver. More...
 
struct  gtsam::KeyInfoEntry
 Handy data structure for iterative solvers key to (index, dimension, start) More...
 
class  gtsam::KeyInfo
 Handy data structure for iterative solvers. More...
class  gtsam::System
 Helper class encapsulating the combined system |Ax-b_|^2 Needed to run Conjugate Gradients on matrices. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

template<class S , class V , class E >
V gtsam::conjugateGradients (const S &Ab, V x, const ConjugateGradientParameters &parameters, bool steepest=false)
 Method of conjugate gradients (CG) template "System" class S needs gradient(S,v), e=S*v, v=S^e "Vector" class V needs dot(v,v), -v, v+v, s*v "Vector" class E needs dot(v,v)
 
│ │ │ │ +GTSAM_EXPORT Vector gtsam::steepestDescent (const System &Ab, const Vector &x, const IterativeOptimizationParameters &parameters)
 Method of steepest gradients, System version.
 
│ │ │ │ +Vector gtsam::conjugateGradientDescent (const System &Ab, const Vector &x, const ConjugateGradientParameters &parameters)
 Method of conjugate gradients (CG), System version.
 
Vector gtsam::steepestDescent (const Matrix &A, const Vector &b, const Vector &x, const ConjugateGradientParameters &parameters)
 convenience calls using matrices, will create System class internally:
 
│ │ │ │ +Vector gtsam::conjugateGradientDescent (const Matrix &A, const Vector &b, const Vector &x, const ConjugateGradientParameters &parameters)
 Method of conjugate gradients (CG), Matrix version.
 
│ │ │ │ +VectorValues gtsam::steepestDescent (const GaussianFactorGraph &fg, const VectorValues &x, const ConjugateGradientParameters &parameters)
 Method of steepest gradients, Gaussian Factor Graph version.
 
│ │ │ │ +VectorValues gtsam::conjugateGradientDescent (const GaussianFactorGraph &fg, const VectorValues &x, const ConjugateGradientParameters &parameters)
 Method of conjugate gradients (CG), Gaussian Factor Graph version.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Some support classes for iterative solvers.

│ │ │ │ -
Date
2010
│ │ │ │ -
Author
Yong-Dian Jian
│ │ │ │ +

Iterative methods, implementation.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
Date
Dec 28, 2009
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,37 +1,68 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -IterativeSolver.h File Reference │ │ │ │ │ -Some support classes for iterative solvers. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +iterative.h File Reference │ │ │ │ │ +Iterative methods, implementation. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_I_t_e_r_a_t_i_v_e_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ -  parameters for iterative linear solvers _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_I_t_e_r_a_t_i_v_e_S_o_l_v_e_r │ │ │ │ │ -  Base class for Iterative Solvers like _S_u_b_g_r_a_p_h_S_o_l_v_e_r. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_K_e_y_I_n_f_o_E_n_t_r_y │ │ │ │ │ -  Handy data structure for iterative solvers key to (index, dimension, │ │ │ │ │ - start) _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_K_e_y_I_n_f_o │ │ │ │ │ -  Handy data structure for iterative solvers. _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_S_y_s_t_e_m │ │ │ │ │ +  Helper class encapsulating the combined system |Ax-b_|^2 Needed to run │ │ │ │ │ + Conjugate Gradients on matrices. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +template │ │ │ │ │ + V  _g_t_s_a_m_:_:_c_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_s (const S &Ab, V x, const │ │ │ │ │ + _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s ¶meters, bool │ │ │ │ │ + steepest=false) │ │ │ │ │ + Method of conjugate gradients (CG) template "System" class │ │ │ │ │ +  S needs gradient(S,v), e=S*v, v=S^e "Vector" class V needs │ │ │ │ │ + dot(v,v), -v, v+v, s*v "Vector" class E needs dot(v,v) │ │ │ │ │ +  │ │ │ │ │ +GTSAM_EXPORT Vector  ggttssaamm::::sstteeeeppeessttDDeesscceenntt (const _S_y_s_t_e_m &Ab, const Vector &x, │ │ │ │ │ + const _I_t_e_r_a_t_i_v_e_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s ¶meters) │ │ │ │ │ +  Method of steepest gradients, _S_y_s_t_e_m version. │ │ │ │ │ +  │ │ │ │ │ + Vector  ggttssaamm::::ccoonnjjuuggaatteeGGrraaddiieennttDDeesscceenntt (const _S_y_s_t_e_m &Ab, const │ │ │ │ │ + Vector &x, const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s ¶meters) │ │ │ │ │ +  Method of conjugate gradients (CG), _S_y_s_t_e_m version. │ │ │ │ │ +  │ │ │ │ │ + Vector  _g_t_s_a_m_:_:_s_t_e_e_p_e_s_t_D_e_s_c_e_n_t (const Matrix &A, const Vector &b, │ │ │ │ │ + const Vector &x, const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ + ¶meters) │ │ │ │ │ +  convenience calls using matrices, will create _S_y_s_t_e_m class │ │ │ │ │ + internally: │ │ │ │ │ +  │ │ │ │ │ + Vector  ggttssaamm::::ccoonnjjuuggaatteeGGrraaddiieennttDDeesscceenntt (const Matrix &A, const │ │ │ │ │ + Vector &b, const Vector &x, const │ │ │ │ │ + _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s ¶meters) │ │ │ │ │ +  Method of conjugate gradients (CG), Matrix version. │ │ │ │ │ +  │ │ │ │ │ + _V_e_c_t_o_r_V_a_l_u_e_s  ggttssaamm::::sstteeeeppeessttDDeesscceenntt (const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &fg, │ │ │ │ │ + const _V_e_c_t_o_r_V_a_l_u_e_s &x, const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ + ¶meters) │ │ │ │ │ +  Method of steepest gradients, Gaussian _F_a_c_t_o_r Graph │ │ │ │ │ + version. │ │ │ │ │ +  │ │ │ │ │ + _V_e_c_t_o_r_V_a_l_u_e_s  ggttssaamm::::ccoonnjjuuggaatteeGGrraaddiieennttDDeesscceenntt (const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ + &fg, const _V_e_c_t_o_r_V_a_l_u_e_s &x, const │ │ │ │ │ + _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s ¶meters) │ │ │ │ │ +  Method of conjugate gradients (CG), Gaussian _F_a_c_t_o_r Graph │ │ │ │ │ + version. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Some support classes for iterative solvers. │ │ │ │ │ - Date │ │ │ │ │ - 2010 │ │ │ │ │ +Iterative methods, implementation. │ │ │ │ │ Author │ │ │ │ │ - Yong-Dian Jian │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + Dec 28, 2009 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _I_t_e_r_a_t_i_v_e_S_o_l_v_e_r_._h │ │ │ │ │ + * _i_t_e_r_a_t_i_v_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00797.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,6 +1,10 @@ │ │ │ │ │ var a00797 = [ │ │ │ │ │ - ["gtsam::IterativeOptimizationParameters", "a03828.html", null], │ │ │ │ │ - ["gtsam::IterativeSolver", "a03832.html", null], │ │ │ │ │ - ["gtsam::KeyInfoEntry", "a03836.html", null], │ │ │ │ │ - ["gtsam::KeyInfo", "a03840.html", "a03840"] │ │ │ │ │ + ["gtsam::System", "a03824.html", "a03824"], │ │ │ │ │ + ["conjugateGradientDescent", "a00797.html#aa84114ad1593684ad739088a2898afdb", null], │ │ │ │ │ + ["conjugateGradientDescent", "a00797.html#af55440f741d8b2f706101f7a79ba7111", null], │ │ │ │ │ + ["conjugateGradientDescent", "a00797.html#ad2b587fde5d35a27d6c88feb4432e785", null], │ │ │ │ │ + ["conjugateGradients", "a00797.html#a6516ea957c3a22ddf429cefef1fe9486", null], │ │ │ │ │ + ["steepestDescent", "a00797.html#a81b7af7638d28555c3f404318cc67b22", null], │ │ │ │ │ + ["steepestDescent", "a00797.html#aed4b2ca704ddd6530ea70122f883ac88", null], │ │ │ │ │ + ["steepestDescent", "a00797.html#a37ca85391e3bc671c71b68bc324296f0", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00797_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/IterativeSolver.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/iterative.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,15 +98,15 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
IterativeSolver.h
│ │ │ │ +
iterative.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │ @@ -114,182 +114,136 @@ │ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/inference/Ordering.h>
│ │ │ │ -
22#include <gtsam/base/Vector.h>
│ │ │ │ -
23
│ │ │ │ -
24#include <boost/tuple/tuple.hpp>
│ │ │ │ -
25#include <boost/shared_ptr.hpp>
│ │ │ │ -
26#include <boost/optional.hpp>
│ │ │ │ -
27
│ │ │ │ -
28#include <iosfwd>
│ │ │ │ -
29#include <string>
│ │ │ │ -
30#include <map>
│ │ │ │ -
31
│ │ │ │ -
32namespace gtsam {
│ │ │ │ -
33
│ │ │ │ -
34// Forward declarations
│ │ │ │ -
35struct KeyInfoEntry;
│ │ │ │ -
36class KeyInfo;
│ │ │ │ -
37class GaussianFactorGraph;
│ │ │ │ -
38class Values;
│ │ │ │ -
39class VectorValues;
│ │ │ │ -
40
│ │ │ │ +
21#include <gtsam/base/Matrix.h>
│ │ │ │ +
22#include <gtsam/linear/VectorValues.h>
│ │ │ │ +
23#include <gtsam/linear/ConjugateGradientSolver.h>
│ │ │ │ +
24
│ │ │ │ +
25namespace gtsam {
│ │ │ │ +
26
│ │ │ │ +
36 template<class S, class V, class E>
│ │ │ │ +
37 V conjugateGradients(const S& Ab, V x,
│ │ │ │ +
38 const ConjugateGradientParameters &parameters, bool steepest = false);
│ │ │ │ +
39
│ │ │ │
│ │ │ │ -
44class IterativeOptimizationParameters {
│ │ │ │ +
44 class GTSAM_EXPORT System {
│ │ │ │
45
│ │ │ │ -
46public:
│ │ │ │ -
47
│ │ │ │ -
48 typedef boost::shared_ptr<IterativeOptimizationParameters> shared_ptr;
│ │ │ │ -
49 enum Verbosity {
│ │ │ │ -
50 SILENT = 0, COMPLEXITY, ERROR
│ │ │ │ -
51 } verbosity_;
│ │ │ │ -
52
│ │ │ │ -
53public:
│ │ │ │ -
54
│ │ │ │ -
55 IterativeOptimizationParameters(Verbosity v = SILENT) :
│ │ │ │ -
56 verbosity_(v) {
│ │ │ │ -
57 }
│ │ │ │ +
46 private:
│ │ │ │ +
47 const Matrix& A_;
│ │ │ │ +
48 const Vector& b_;
│ │ │ │ +
49
│ │ │ │ +
50 public:
│ │ │ │ +
51
│ │ │ │ +
52 System(const Matrix& A, const Vector& b) :
│ │ │ │ +
53 A_(A), b_(b) {
│ │ │ │ +
54 }
│ │ │ │ +
55
│ │ │ │ +
57 const Matrix& A() const { return A_; }
│ │ │ │
58
│ │ │ │ -
59 virtual ~IterativeOptimizationParameters() {
│ │ │ │ -
60 }
│ │ │ │ +
60 const Vector& b() const { return b_; }
│ │ │ │
61
│ │ │ │ -
62 /* utility */
│ │ │ │ -
63 inline Verbosity verbosity() const {
│ │ │ │ -
64 return verbosity_;
│ │ │ │ -
65 }
│ │ │ │ -
66 GTSAM_EXPORT std::string getVerbosity() const;
│ │ │ │ -
67 GTSAM_EXPORT void setVerbosity(const std::string &s);
│ │ │ │ -
68
│ │ │ │ -
69 /* matlab interface */
│ │ │ │ -
70 GTSAM_EXPORT void print() const;
│ │ │ │ +
│ │ │ │ +
63 Vector operator^(const Vector& e) const {
│ │ │ │ +
64 return A_ ^ e;
│ │ │ │ +
65 }
│ │ │ │ +
│ │ │ │ +
66
│ │ │ │ +
70 void print (const std::string& s = "System") const;
│ │ │ │
71
│ │ │ │ -
72 /* virtual print function */
│ │ │ │ -
73 GTSAM_EXPORT virtual void print(std::ostream &os) const;
│ │ │ │ -
74
│ │ │ │ -
75 /* for serialization */
│ │ │ │ -
76 friend std::ostream& operator<<(std::ostream &os,
│ │ │ │ -
77 const IterativeOptimizationParameters &p);
│ │ │ │ -
78
│ │ │ │ -
79 GTSAM_EXPORT static Verbosity verbosityTranslator(const std::string &s);
│ │ │ │ -
80 GTSAM_EXPORT static std::string verbosityTranslator(Verbosity v);
│ │ │ │ -
81};
│ │ │ │ -
│ │ │ │ -
82
│ │ │ │ -
│ │ │ │ -
86class IterativeSolver {
│ │ │ │ -
87public:
│ │ │ │ -
88 typedef boost::shared_ptr<IterativeSolver> shared_ptr;
│ │ │ │ -
89 IterativeSolver() {
│ │ │ │ -
90 }
│ │ │ │ -
91 virtual ~IterativeSolver() {
│ │ │ │ -
92 }
│ │ │ │ -
93
│ │ │ │ -
94 /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */
│ │ │ │ -
95 GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg,
│ │ │ │ -
96 boost::optional<const KeyInfo&> = boost::none,
│ │ │ │ -
97 boost::optional<const std::map<Key, Vector>&> lambda = boost::none);
│ │ │ │ -
98
│ │ │ │ -
99 /* interface to the nonlinear optimizer, without initial estimate */
│ │ │ │ -
100 GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo,
│ │ │ │ -
101 const std::map<Key, Vector> &lambda);
│ │ │ │ -
102
│ │ │ │ -
103 /* interface to the nonlinear optimizer that the subclasses have to implement */
│ │ │ │ -
104 virtual VectorValues optimize(const GaussianFactorGraph &gfg,
│ │ │ │ -
105 const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
│ │ │ │ -
106 const VectorValues &initial) = 0;
│ │ │ │ -
107
│ │ │ │ -
108};
│ │ │ │ -
│ │ │ │ -
109
│ │ │ │ -
│ │ │ │ -
114struct GTSAM_EXPORT KeyInfoEntry {
│ │ │ │ -
115 size_t index, dim, start;
│ │ │ │ -
116 KeyInfoEntry() {
│ │ │ │ -
117 }
│ │ │ │ -
118 KeyInfoEntry(size_t idx, size_t d, Key start) :
│ │ │ │ -
119 index(idx), dim(d), start(start) {
│ │ │ │ -
120 }
│ │ │ │ -
121};
│ │ │ │ -
│ │ │ │ -
122
│ │ │ │ -
│ │ │ │ -
126class GTSAM_EXPORT KeyInfo: public std::map<Key, KeyInfoEntry> {
│ │ │ │ -
127
│ │ │ │ -
128public:
│ │ │ │ -
129
│ │ │ │ -
130 typedef std::map<Key, KeyInfoEntry> Base;
│ │ │ │ -
131
│ │ │ │ -
132protected:
│ │ │ │ -
133
│ │ │ │ -
134 Ordering ordering_;
│ │ │ │ -
135 size_t numCols_;
│ │ │ │ +
│ │ │ │ +
73 Vector gradient(const Vector& x) const {
│ │ │ │ +
74 return A() ^ (A() * x - b());
│ │ │ │ +
75 }
│ │ │ │ +
│ │ │ │ +
76
│ │ │ │ +
│ │ │ │ +
78 Vector operator*(const Vector& x) const {
│ │ │ │ +
79 return A() * x;
│ │ │ │ +
80 }
│ │ │ │ +
│ │ │ │ +
81
│ │ │ │ +
│ │ │ │ +
83 void multiplyInPlace(const Vector& x, Vector& e) const {
│ │ │ │ +
84 e = A() * x;
│ │ │ │ +
85 }
│ │ │ │ +
│ │ │ │ +
86
│ │ │ │ +
│ │ │ │ +
88 void transposeMultiplyAdd(double alpha, const Vector& e, Vector& x) const {
│ │ │ │ +
89 x += alpha * A().transpose() * e;
│ │ │ │ +
90 }
│ │ │ │ +
│ │ │ │ +
91 };
│ │ │ │ +
│ │ │ │ +
92
│ │ │ │ +
96 GTSAM_EXPORT Vector steepestDescent(
│ │ │ │ +
97 const System& Ab,
│ │ │ │ +
98 const Vector& x,
│ │ │ │ +
99 const IterativeOptimizationParameters & parameters);
│ │ │ │ +
100
│ │ │ │ +
104 GTSAM_EXPORT Vector conjugateGradientDescent(
│ │ │ │ +
105 const System& Ab,
│ │ │ │ +
106 const Vector& x,
│ │ │ │ +
107 const ConjugateGradientParameters & parameters);
│ │ │ │ +
108
│ │ │ │ +
114 GTSAM_EXPORT Vector steepestDescent(
│ │ │ │ +
115 const Matrix& A,
│ │ │ │ +
116 const Vector& b,
│ │ │ │ +
117 const Vector& x,
│ │ │ │ +
118 const ConjugateGradientParameters & parameters);
│ │ │ │ +
119
│ │ │ │ +
123 GTSAM_EXPORT Vector conjugateGradientDescent(
│ │ │ │ +
124 const Matrix& A,
│ │ │ │ +
125 const Vector& b,
│ │ │ │ +
126 const Vector& x,
│ │ │ │ +
127 const ConjugateGradientParameters & parameters);
│ │ │ │ +
128
│ │ │ │ +
132 GTSAM_EXPORT VectorValues steepestDescent(
│ │ │ │ +
133 const GaussianFactorGraph& fg,
│ │ │ │ +
134 const VectorValues& x,
│ │ │ │ +
135 const ConjugateGradientParameters & parameters);
│ │ │ │
136
│ │ │ │ -
137 void initialize(const GaussianFactorGraph &fg);
│ │ │ │ -
138
│ │ │ │ -
139public:
│ │ │ │ -
140
│ │ │ │ -
│ │ │ │ -
142 KeyInfo() :
│ │ │ │ -
143 numCols_(0) {
│ │ │ │ -
144 }
│ │ │ │ -
│ │ │ │ +
140 GTSAM_EXPORT VectorValues conjugateGradientDescent(
│ │ │ │ +
141 const GaussianFactorGraph& fg,
│ │ │ │ +
142 const VectorValues& x,
│ │ │ │ +
143 const ConjugateGradientParameters & parameters);
│ │ │ │ +
144
│ │ │ │
145
│ │ │ │ -
147 KeyInfo(const GaussianFactorGraph &fg);
│ │ │ │ -
148
│ │ │ │ -
150 KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering);
│ │ │ │ -
151
│ │ │ │ -
│ │ │ │ -
153 inline size_t numCols() const {
│ │ │ │ -
154 return numCols_;
│ │ │ │ -
155 }
│ │ │ │ -
│ │ │ │ -
156
│ │ │ │ -
│ │ │ │ -
158 inline const Ordering & ordering() const {
│ │ │ │ -
159 return ordering_;
│ │ │ │ -
160 }
│ │ │ │ -
│ │ │ │ -
161
│ │ │ │ -
163 std::vector<size_t> colSpec() const;
│ │ │ │ -
164
│ │ │ │ -
166 VectorValues x0() const;
│ │ │ │ -
167
│ │ │ │ -
169 Vector x0vector() const;
│ │ │ │ -
170
│ │ │ │ -
171};
│ │ │ │ -
│ │ │ │ -
172
│ │ │ │ -
173} // \ namespace gtsam
│ │ │ │ -
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ -
Ordering.h
Variable ordering for the elimination algorithm.
│ │ │ │ +
146} // namespace gtsam
│ │ │ │ +
147
│ │ │ │ +
148#include <gtsam/linear/iterative-inl.h>
│ │ │ │ +
149
│ │ │ │ +
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
ConjugateGradientSolver.h
Implementation of Conjugate Gradient solver for a linear system.
│ │ │ │ +
iterative-inl.h
Iterative methods, template implementation.
│ │ │ │ +
VectorValues.h
Factor Graph Values.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
gtsam::conjugateGradients
V conjugateGradients(const S &Ab, V x, const ConjugateGradientParameters &parameters, bool steepest)
Method of conjugate gradients (CG) template "System" class S needs gradient(S,v), e=S*v,...
Definition iterative-inl.h:125
│ │ │ │ +
gtsam::conjugateGradientDescent
Vector conjugateGradientDescent(const System &Ab, const Vector &x, const ConjugateGradientParameters &parameters)
Method of conjugate gradients (CG), System version.
Definition iterative.cpp:45
│ │ │ │ +
gtsam::ConjugateGradientParameters
parameters for the conjugate gradient method
Definition ConjugateGradientSolver.h:29
│ │ │ │
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ +
gtsam::System
Helper class encapsulating the combined system |Ax-b_|^2 Needed to run Conjugate Gradients on matrice...
Definition iterative.h:44
│ │ │ │ +
gtsam::System::operator*
Vector operator*(const Vector &x) const
Apply operator A.
Definition iterative.h:78
│ │ │ │ +
gtsam::System::A
const Matrix & A() const
Access A matrix.
Definition iterative.h:57
│ │ │ │ +
gtsam::System::multiplyInPlace
void multiplyInPlace(const Vector &x, Vector &e) const
Apply operator A in place.
Definition iterative.h:83
│ │ │ │ +
gtsam::System::b
const Vector & b() const
Access b vector.
Definition iterative.h:60
│ │ │ │ +
gtsam::System::gradient
Vector gradient(const Vector &x) const
gradient of objective function 0.5*|Ax-b_|^2 at x = A_'*(Ax-b_)
Definition iterative.h:73
│ │ │ │ +
gtsam::System::transposeMultiplyAdd
void transposeMultiplyAdd(double alpha, const Vector &e, Vector &x) const
x += alpha* A'*e
Definition iterative.h:88
│ │ │ │ +
gtsam::System::operator^
Vector operator^(const Vector &e) const
Apply operator A'*e.
Definition iterative.h:63
│ │ │ │
gtsam::IterativeOptimizationParameters
parameters for iterative linear solvers
Definition IterativeSolver.h:44
│ │ │ │ -
gtsam::IterativeSolver
Base class for Iterative Solvers like SubgraphSolver.
Definition IterativeSolver.h:86
│ │ │ │ -
gtsam::KeyInfoEntry
Handy data structure for iterative solvers key to (index, dimension, start)
Definition IterativeSolver.h:114
│ │ │ │ -
gtsam::KeyInfo
Handy data structure for iterative solvers.
Definition IterativeSolver.h:126
│ │ │ │ -
gtsam::KeyInfo::ordering
const Ordering & ordering() const
Return the ordering.
Definition IterativeSolver.h:158
│ │ │ │ -
gtsam::KeyInfo::numCols
size_t numCols() const
Return the total number of columns (scalar variables = sum of dimensions)
Definition IterativeSolver.h:153
│ │ │ │ -
gtsam::KeyInfo::KeyInfo
KeyInfo()
Default Constructor.
Definition IterativeSolver.h:142
│ │ │ │
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ -
VectorValues
The Factor::error simply extracts the.
│ │ │ │ -
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -IterativeSolver.h │ │ │ │ │ +iterative.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ @@ -16,204 +16,168 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 19#pragma once │ │ │ │ │ 20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_O_r_d_e_r_i_n_g_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_c_t_o_r_._h> │ │ │ │ │ -23 │ │ │ │ │ -24#include │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ -27 │ │ │ │ │ -28#include │ │ │ │ │ -29#include │ │ │ │ │ -30#include │ │ │ │ │ -31 │ │ │ │ │ -32namespace _g_t_s_a_m { │ │ │ │ │ -33 │ │ │ │ │ -34// Forward declarations │ │ │ │ │ -35struct KeyInfoEntry; │ │ │ │ │ -36class KeyInfo; │ │ │ │ │ -37class GaussianFactorGraph; │ │ │ │ │ -38class _V_a_l_u_e_s; │ │ │ │ │ -39class _V_e_c_t_o_r_V_a_l_u_e_s; │ │ │ │ │ -40 │ │ │ │ │ -_4_4class _I_t_e_r_a_t_i_v_e_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s { │ │ │ │ │ +21#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_S_o_l_v_e_r_._h> │ │ │ │ │ +24 │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ +26 │ │ │ │ │ +36 template │ │ │ │ │ +37 V _c_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_s(const S& Ab, V x, │ │ │ │ │ +38 const ConjugateGradientParameters ¶meters, bool steepest = false); │ │ │ │ │ +39 │ │ │ │ │ +_4_4 class GTSAM_EXPORT _S_y_s_t_e_m { │ │ │ │ │ 45 │ │ │ │ │ -46public: │ │ │ │ │ -47 │ │ │ │ │ -48 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -49 enum Verbosity { │ │ │ │ │ -50 SILENT = 0, COMPLEXITY, ERROR │ │ │ │ │ -51 } verbosity_; │ │ │ │ │ -52 │ │ │ │ │ -53public: │ │ │ │ │ -54 │ │ │ │ │ -55 _I_t_e_r_a_t_i_v_e_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s(Verbosity v = SILENT) : │ │ │ │ │ -56 verbosity_(v) { │ │ │ │ │ -57 } │ │ │ │ │ +46 private: │ │ │ │ │ +47 const Matrix& A_; │ │ │ │ │ +48 const Vector& b_; │ │ │ │ │ +49 │ │ │ │ │ +50 public: │ │ │ │ │ +51 │ │ │ │ │ +52 _S_y_s_t_e_m(const Matrix& A, const Vector& b) : │ │ │ │ │ +53 A_(A), b_(b) { │ │ │ │ │ +54 } │ │ │ │ │ +55 │ │ │ │ │ +_5_7 const Matrix& _A() const { return A_; } │ │ │ │ │ 58 │ │ │ │ │ -59 virtual _~_I_t_e_r_a_t_i_v_e_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s() { │ │ │ │ │ -60 } │ │ │ │ │ +_6_0 const Vector& _b() const { return b_; } │ │ │ │ │ 61 │ │ │ │ │ -62 /* utility */ │ │ │ │ │ -63 inline Verbosity verbosity() const { │ │ │ │ │ -64 return verbosity_; │ │ │ │ │ +_6_3 Vector _o_p_e_r_a_t_o_r_^(const Vector& e) const { │ │ │ │ │ +64 return A_ ^ e; │ │ │ │ │ 65 } │ │ │ │ │ -66 GTSAM_EXPORT std::string getVerbosity() const; │ │ │ │ │ -67 GTSAM_EXPORT void setVerbosity(const std::string &s); │ │ │ │ │ -68 │ │ │ │ │ -69 /* matlab interface */ │ │ │ │ │ -70 GTSAM_EXPORT void print() const; │ │ │ │ │ +66 │ │ │ │ │ +70 void _p_r_i_n_t (const std::string& s = "System") const; │ │ │ │ │ 71 │ │ │ │ │ -72 /* virtual print function */ │ │ │ │ │ -73 GTSAM_EXPORT virtual void _p_r_i_n_t(std::ostream &os) const; │ │ │ │ │ -74 │ │ │ │ │ -75 /* for serialization */ │ │ │ │ │ -76 friend std::ostream& operator<<(std::ostream &os, │ │ │ │ │ -77 const _I_t_e_r_a_t_i_v_e_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s &p); │ │ │ │ │ -78 │ │ │ │ │ -79 GTSAM_EXPORT static Verbosity verbosityTranslator(const std::string &s); │ │ │ │ │ -80 GTSAM_EXPORT static std::string verbosityTranslator(Verbosity v); │ │ │ │ │ -81}; │ │ │ │ │ -82 │ │ │ │ │ -_8_6class _I_t_e_r_a_t_i_v_e_S_o_l_v_e_r { │ │ │ │ │ -87public: │ │ │ │ │ -88 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -89 _I_t_e_r_a_t_i_v_e_S_o_l_v_e_r() { │ │ │ │ │ +_7_3 Vector _g_r_a_d_i_e_n_t(const Vector& x) const { │ │ │ │ │ +74 return A() ^ (A() * x - b()); │ │ │ │ │ +75 } │ │ │ │ │ +76 │ │ │ │ │ +_7_8 Vector _o_p_e_r_a_t_o_r_*(const Vector& x) const { │ │ │ │ │ +79 return A() * x; │ │ │ │ │ +80 } │ │ │ │ │ +81 │ │ │ │ │ +_8_3 void _m_u_l_t_i_p_l_y_I_n_P_l_a_c_e(const Vector& x, Vector& e) const { │ │ │ │ │ +84 e = A() * x; │ │ │ │ │ +85 } │ │ │ │ │ +86 │ │ │ │ │ +_8_8 void _t_r_a_n_s_p_o_s_e_M_u_l_t_i_p_l_y_A_d_d(double alpha, const Vector& e, Vector& x) const { │ │ │ │ │ +89 x += alpha * A().transpose() * e; │ │ │ │ │ 90 } │ │ │ │ │ -91 virtual _~_I_t_e_r_a_t_i_v_e_S_o_l_v_e_r() { │ │ │ │ │ -92 } │ │ │ │ │ -93 │ │ │ │ │ -94 /* interface to the nonlinear optimizer, without metadata, damping and │ │ │ │ │ -initial estimate */ │ │ │ │ │ -95 GTSAM_EXPORT _V_e_c_t_o_r_V_a_l_u_e_s optimize(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, │ │ │ │ │ -96 boost::optional = boost::none, │ │ │ │ │ -97 boost::optional&> lambda = boost::none); │ │ │ │ │ -98 │ │ │ │ │ -99 /* interface to the nonlinear optimizer, without initial estimate */ │ │ │ │ │ -100 GTSAM_EXPORT _V_e_c_t_o_r_V_a_l_u_e_s _o_p_t_i_m_i_z_e(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, const │ │ │ │ │ -_K_e_y_I_n_f_o &keyInfo, │ │ │ │ │ -101 const std::map &lambda); │ │ │ │ │ -102 │ │ │ │ │ -103 /* interface to the nonlinear optimizer that the subclasses have to │ │ │ │ │ -implement */ │ │ │ │ │ -104 virtual _V_e_c_t_o_r_V_a_l_u_e_s _o_p_t_i_m_i_z_e(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, │ │ │ │ │ -105 const _K_e_y_I_n_f_o &keyInfo, const std::map &lambda, │ │ │ │ │ -106 const _V_e_c_t_o_r_V_a_l_u_e_s &initial) = 0; │ │ │ │ │ -107 │ │ │ │ │ -108}; │ │ │ │ │ -109 │ │ │ │ │ -_1_1_4struct GTSAM_EXPORT _K_e_y_I_n_f_o_E_n_t_r_y { │ │ │ │ │ -115 size_t index, dim, start; │ │ │ │ │ -116 _K_e_y_I_n_f_o_E_n_t_r_y() { │ │ │ │ │ -117 } │ │ │ │ │ -118 _K_e_y_I_n_f_o_E_n_t_r_y(size_t idx, size_t d, _K_e_y start) : │ │ │ │ │ -119 index(idx), dim(d), start(start) { │ │ │ │ │ -120 } │ │ │ │ │ -121}; │ │ │ │ │ -122 │ │ │ │ │ -_1_2_6class GTSAM_EXPORT _K_e_y_I_n_f_o: public std::map { │ │ │ │ │ -127 │ │ │ │ │ -128public: │ │ │ │ │ -129 │ │ │ │ │ -130 typedef std::map Base; │ │ │ │ │ -131 │ │ │ │ │ -132protected: │ │ │ │ │ -133 │ │ │ │ │ -134 _O_r_d_e_r_i_n_g ordering_; │ │ │ │ │ -135 size_t numCols_; │ │ │ │ │ +91 }; │ │ │ │ │ +92 │ │ │ │ │ +_9_6 GTSAM_EXPORT Vector steepestDescent( │ │ │ │ │ +97 const _S_y_s_t_e_m& Ab, │ │ │ │ │ +98 const Vector& x, │ │ │ │ │ +99 const _I_t_e_r_a_t_i_v_e_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s & parameters); │ │ │ │ │ +100 │ │ │ │ │ +104 GTSAM_EXPORT Vector _c_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_D_e_s_c_e_n_t( │ │ │ │ │ +105 const _S_y_s_t_e_m& Ab, │ │ │ │ │ +106 const Vector& x, │ │ │ │ │ +107 const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s & parameters); │ │ │ │ │ +108 │ │ │ │ │ +114 GTSAM_EXPORT Vector steepestDescent( │ │ │ │ │ +115 const Matrix& A, │ │ │ │ │ +116 const Vector& b, │ │ │ │ │ +117 const Vector& x, │ │ │ │ │ +118 const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s & parameters); │ │ │ │ │ +119 │ │ │ │ │ +123 GTSAM_EXPORT Vector _c_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_D_e_s_c_e_n_t( │ │ │ │ │ +124 const Matrix& A, │ │ │ │ │ +125 const Vector& b, │ │ │ │ │ +126 const Vector& x, │ │ │ │ │ +127 const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s & parameters); │ │ │ │ │ +128 │ │ │ │ │ +132 GTSAM_EXPORT _V_e_c_t_o_r_V_a_l_u_e_s steepestDescent( │ │ │ │ │ +133 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& fg, │ │ │ │ │ +134 const _V_e_c_t_o_r_V_a_l_u_e_s& x, │ │ │ │ │ +135 const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s & parameters); │ │ │ │ │ 136 │ │ │ │ │ -137 void initialize(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &fg); │ │ │ │ │ -138 │ │ │ │ │ -139public: │ │ │ │ │ -140 │ │ │ │ │ -_1_4_2 _K_e_y_I_n_f_o() : │ │ │ │ │ -143 numCols_(0) { │ │ │ │ │ -144 } │ │ │ │ │ +140 GTSAM_EXPORT _V_e_c_t_o_r_V_a_l_u_e_s _c_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_D_e_s_c_e_n_t( │ │ │ │ │ +141 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& fg, │ │ │ │ │ +142 const _V_e_c_t_o_r_V_a_l_u_e_s& x, │ │ │ │ │ +143 const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s & parameters); │ │ │ │ │ +144 │ │ │ │ │ 145 │ │ │ │ │ -147 _K_e_y_I_n_f_o(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &fg); │ │ │ │ │ -148 │ │ │ │ │ -150 _K_e_y_I_n_f_o(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &fg, const _O_r_d_e_r_i_n_g &ordering); │ │ │ │ │ -151 │ │ │ │ │ -_1_5_3 inline size_t _n_u_m_C_o_l_s() const { │ │ │ │ │ -154 return numCols_; │ │ │ │ │ -155 } │ │ │ │ │ -156 │ │ │ │ │ -_1_5_8 inline const _O_r_d_e_r_i_n_g & _o_r_d_e_r_i_n_g() const { │ │ │ │ │ -159 return ordering_; │ │ │ │ │ -160 } │ │ │ │ │ -161 │ │ │ │ │ -163 std::vector colSpec() const; │ │ │ │ │ -164 │ │ │ │ │ -166 _V_e_c_t_o_r_V_a_l_u_e_s x0() const; │ │ │ │ │ -167 │ │ │ │ │ -169 Vector x0vector() const; │ │ │ │ │ -170 │ │ │ │ │ -171}; │ │ │ │ │ -172 │ │ │ │ │ -173} // \ namespace gtsam │ │ │ │ │ -_V_e_c_t_o_r_._h │ │ │ │ │ -typedef and functions to augment Eigen's VectorXd │ │ │ │ │ -_O_r_d_e_r_i_n_g_._h │ │ │ │ │ -Variable ordering for the elimination algorithm. │ │ │ │ │ +146} // namespace gtsam │ │ │ │ │ +147 │ │ │ │ │ +148#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_i_t_e_r_a_t_i_v_e_-_i_n_l_._h> │ │ │ │ │ +149 │ │ │ │ │ +_M_a_t_r_i_x_._h │ │ │ │ │ +typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_S_o_l_v_e_r_._h │ │ │ │ │ +Implementation of Conjugate Gradient solver for a linear system. │ │ │ │ │ +_i_t_e_r_a_t_i_v_e_-_i_n_l_._h │ │ │ │ │ +Iterative methods, template implementation. │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ +Factor Graph Values. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ -Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key │ │ │ │ │ -landmarkKey) │ │ │ │ │ -Optimize for triangulation. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.cpp:155 │ │ │ │ │ _g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ print without optional string, must specify cout yourself │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_c_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_s │ │ │ │ │ +V conjugateGradients(const S &Ab, V x, const ConjugateGradientParameters │ │ │ │ │ +¶meters, bool steepest) │ │ │ │ │ +Method of conjugate gradients (CG) template "System" class S needs gradient │ │ │ │ │ +(S,v), e=S*v,... │ │ │ │ │ +DDeeffiinniittiioonn iterative-inl.h:125 │ │ │ │ │ +_g_t_s_a_m_:_:_c_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_D_e_s_c_e_n_t │ │ │ │ │ +Vector conjugateGradientDescent(const System &Ab, const Vector &x, const │ │ │ │ │ +ConjugateGradientParameters ¶meters) │ │ │ │ │ +Method of conjugate gradients (CG), System version. │ │ │ │ │ +DDeeffiinniittiioonn iterative.cpp:45 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ +parameters for the conjugate gradient method │ │ │ │ │ +DDeeffiinniittiioonn ConjugateGradientSolver.h:29 │ │ │ │ │ _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_s_t_e_m │ │ │ │ │ +Helper class encapsulating the combined system |Ax-b_|^2 Needed to run │ │ │ │ │ +Conjugate Gradients on matrice... │ │ │ │ │ +DDeeffiinniittiioonn iterative.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_s_t_e_m_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ +Vector operator*(const Vector &x) const │ │ │ │ │ +Apply operator A. │ │ │ │ │ +DDeeffiinniittiioonn iterative.h:78 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_s_t_e_m_:_:_A │ │ │ │ │ +const Matrix & A() const │ │ │ │ │ +Access A matrix. │ │ │ │ │ +DDeeffiinniittiioonn iterative.h:57 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_s_t_e_m_:_:_m_u_l_t_i_p_l_y_I_n_P_l_a_c_e │ │ │ │ │ +void multiplyInPlace(const Vector &x, Vector &e) const │ │ │ │ │ +Apply operator A in place. │ │ │ │ │ +DDeeffiinniittiioonn iterative.h:83 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_s_t_e_m_:_:_b │ │ │ │ │ +const Vector & b() const │ │ │ │ │ +Access b vector. │ │ │ │ │ +DDeeffiinniittiioonn iterative.h:60 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_s_t_e_m_:_:_g_r_a_d_i_e_n_t │ │ │ │ │ +Vector gradient(const Vector &x) const │ │ │ │ │ +gradient of objective function 0.5*|Ax-b_|^2 at x = A_'*(Ax-b_) │ │ │ │ │ +DDeeffiinniittiioonn iterative.h:73 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_s_t_e_m_:_:_t_r_a_n_s_p_o_s_e_M_u_l_t_i_p_l_y_A_d_d │ │ │ │ │ +void transposeMultiplyAdd(double alpha, const Vector &e, Vector &x) const │ │ │ │ │ +x += alpha* A'*e │ │ │ │ │ +DDeeffiinniittiioonn iterative.h:88 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_s_t_e_m_:_:_o_p_e_r_a_t_o_r_^ │ │ │ │ │ +Vector operator^(const Vector &e) const │ │ │ │ │ +Apply operator A'*e. │ │ │ │ │ +DDeeffiinniittiioonn iterative.h:63 │ │ │ │ │ _g_t_s_a_m_:_:_I_t_e_r_a_t_i_v_e_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ parameters for iterative linear solvers │ │ │ │ │ DDeeffiinniittiioonn IterativeSolver.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_I_t_e_r_a_t_i_v_e_S_o_l_v_e_r │ │ │ │ │ -Base class for Iterative Solvers like SubgraphSolver. │ │ │ │ │ -DDeeffiinniittiioonn IterativeSolver.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_I_n_f_o_E_n_t_r_y │ │ │ │ │ -Handy data structure for iterative solvers key to (index, dimension, start) │ │ │ │ │ -DDeeffiinniittiioonn IterativeSolver.h:114 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_I_n_f_o │ │ │ │ │ -Handy data structure for iterative solvers. │ │ │ │ │ -DDeeffiinniittiioonn IterativeSolver.h:126 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_I_n_f_o_:_:_o_r_d_e_r_i_n_g │ │ │ │ │ -const Ordering & ordering() const │ │ │ │ │ -Return the ordering. │ │ │ │ │ -DDeeffiinniittiioonn IterativeSolver.h:158 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_I_n_f_o_:_:_n_u_m_C_o_l_s │ │ │ │ │ -size_t numCols() const │ │ │ │ │ -Return the total number of columns (scalar variables = sum of dimensions) │ │ │ │ │ -DDeeffiinniittiioonn IterativeSolver.h:153 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_I_n_f_o_:_:_K_e_y_I_n_f_o │ │ │ │ │ -KeyInfo() │ │ │ │ │ -Default Constructor. │ │ │ │ │ -DDeeffiinniittiioonn IterativeSolver.h:142 │ │ │ │ │ _g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ with a unique integer... │ │ │ │ │ DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -The Factor::error simply extracts the. │ │ │ │ │ -_V_a_l_u_e_s │ │ │ │ │ -In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ -a non-linear function... │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _I_t_e_r_a_t_i_v_e_S_o_l_v_e_r_._h │ │ │ │ │ + * _i_t_e_r_a_t_i_v_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00800.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Sampler.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianBayesTree.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,37 +94,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces
│ │ │ │ -
Sampler.cpp File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
GaussianBayesTree.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

sampling from a diagonal NoiseModel │ │ │ │ +

Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +LogDeterminantData & gtsam::internal::logDeterminant (const GaussianBayesTreeClique::shared_ptr &clique, LogDeterminantData &parentSum)
 
│ │ │ │

Detailed Description

│ │ │ │ -

sampling from a diagonal NoiseModel

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +

Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.

│ │ │ │ +

GaussianBayesTree

Author
Frank Dellaert
│ │ │ │
│ │ │ │ -Alex Cunningham
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,27 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Sampler.cpp File Reference │ │ │ │ │ -sampling from a diagonal NoiseModel _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +GaussianBayesTree.cpp File Reference │ │ │ │ │ +Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +LogDeterminantData &  ggttssaamm::::iinntteerrnnaall::::llooggDDeetteerrmmiinnaanntt (const │ │ │ │ │ + GaussianBayesTreeClique::shared_ptr &clique, │ │ │ │ │ + LogDeterminantData &parentSum) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -sampling from a diagonal NoiseModel │ │ │ │ │ +Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ │ +GaussianBayesTree │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Alex Cunningham │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _S_a_m_p_l_e_r_._c_p_p │ │ │ │ │ + * _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00803.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/BinaryJacobianFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/SparseEigen.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,47 +94,62 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
BinaryJacobianFactor.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ +Functions
│ │ │ │ +
SparseEigen.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

A binary JacobianFactor specialization that uses fixed matrix math for speed. │ │ │ │ +

Utilities for creating Eigen sparse matrices (gtsam::SparseEigen) │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

struct  gtsam::BinaryJacobianFactor< M, N1, N2 >
 A binary JacobianFactor specialization that uses fixed matrix math for speed. More...
 
struct  gtsam::traits< BinaryJacobianFactor< M, N1, N2 > >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Typedefs

typedef Eigen::SparseMatrix< double, Eigen::ColMajor, int > gtsam::SparseEigen
 Eigen-format sparse matrix.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +SparseEigen gtsam::sparseJacobianEigen (const GaussianFactorGraph &gfg, const Ordering &ordering)
 Constructs an Eigen-format SparseMatrix of a GaussianFactorGraph.
 
│ │ │ │ +SparseEigen gtsam::sparseJacobianEigen (const GaussianFactorGraph &gfg)
 
│ │ │ │

Detailed Description

│ │ │ │ -

A binary JacobianFactor specialization that uses fixed matrix math for speed.

│ │ │ │ -
Date
June 2015
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +

Utilities for creating Eigen sparse matrices (gtsam::SparseEigen)

│ │ │ │ +
Date
Aug 2019
│ │ │ │ +
Author
Mandy Xie
│ │ │ │ +
│ │ │ │ +Fan Jiang
│ │ │ │ +
│ │ │ │ +Gerry Chen
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,31 +1,38 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -BinaryJacobianFactor.h File Reference │ │ │ │ │ -A binary _J_a_c_o_b_i_a_n_F_a_c_t_o_r specialization that uses fixed matrix math for speed. │ │ │ │ │ -_M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +SparseEigen.h File Reference │ │ │ │ │ +Utilities for creating Eigen sparse matrices (_g_t_s_a_m_:_:_S_p_a_r_s_e_E_i_g_e_n) _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_B_i_n_a_r_y_J_a_c_o_b_i_a_n_F_a_c_t_o_r_<_ _M_,_ _N_1_,_ _N_2_ _> │ │ │ │ │ -  A binary _J_a_c_o_b_i_a_n_F_a_c_t_o_r specialization that uses fixed matrix math for │ │ │ │ │ - speed. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _B_i_n_a_r_y_J_a_c_o_b_i_a_n_F_a_c_t_o_r_<_ _M_,_ _N_1_,_ _N_2_ _>_ _> │ │ │ │ │ -  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +typedef Eigen::SparseMatrix< double, Eigen:: │ │ │ │ │ + ColMajor, int >  _g_t_s_a_m_:_:_S_p_a_r_s_e_E_i_g_e_n │ │ │ │ │ +  Eigen-format sparse matrix. │ │ │ │ │ +  │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +_S_p_a_r_s_e_E_i_g_e_n  ggttssaamm::::ssppaarrsseeJJaaccoobbiiaannEEiiggeenn (const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, const │ │ │ │ │ + _O_r_d_e_r_i_n_g &ordering) │ │ │ │ │ +  Constructs an Eigen-format SparseMatrix of a _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h. │ │ │ │ │ +  │ │ │ │ │ +_S_p_a_r_s_e_E_i_g_e_n  ggttssaamm::::ssppaarrsseeJJaaccoobbiiaannEEiiggeenn (const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A binary _J_a_c_o_b_i_a_n_F_a_c_t_o_r specialization that uses fixed matrix math for speed. │ │ │ │ │ +Utilities for creating Eigen sparse matrices (_g_t_s_a_m_:_:_S_p_a_r_s_e_E_i_g_e_n) │ │ │ │ │ Date │ │ │ │ │ - June 2015 │ │ │ │ │ + Aug 2019 │ │ │ │ │ Author │ │ │ │ │ + Mandy Xie │ │ │ │ │ + Fan Jiang │ │ │ │ │ + Gerry Chen │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _B_i_n_a_r_y_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ + * _S_p_a_r_s_e_E_i_g_e_n_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00803.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a00803 = [ │ │ │ │ │ - ["gtsam::BinaryJacobianFactor< M, N1, N2 >", "a03720.html", "a03720"], │ │ │ │ │ - ["gtsam::traits< BinaryJacobianFactor< M, N1, N2 > >", "a03724.html", null] │ │ │ │ │ + ["SparseEigen", "a00803.html#a52b9b28b076a8ae17d6d3d72ee1229c3", null], │ │ │ │ │ + ["sparseJacobianEigen", "a00803.html#aa9013a9053629f0c00b97726d9d8e19e", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00803_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/BinaryJacobianFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/SparseEigen.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,132 +98,79 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
BinaryJacobianFactor.h
│ │ │ │ +
SparseEigen.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
21#pragma once
│ │ │ │ -
22
│ │ │ │ -
23#include <gtsam/linear/JacobianFactor.h>
│ │ │ │ -
24#include <gtsam/base/SymmetricBlockMatrix.h>
│ │ │ │ -
25#include <gtsam/base/timing.h>
│ │ │ │ -
26
│ │ │ │ -
27namespace gtsam {
│ │ │ │ +
24#pragma once
│ │ │ │ +
25
│ │ │ │ +
26#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ +
27#include <gtsam/linear/VectorValues.h>
│ │ │ │
28
│ │ │ │ -
32template<int M, int N1, int N2>
│ │ │ │ -
│ │ │ │ -
33struct BinaryJacobianFactor: JacobianFactor {
│ │ │ │ -
34
│ │ │ │ -
│ │ │ │ -
36 BinaryJacobianFactor(Key key1, const Eigen::Matrix<double, M, N1>& A1,
│ │ │ │ -
37 Key key2, const Eigen::Matrix<double, M, N2>& A2,
│ │ │ │ -
38 const Eigen::Matrix<double, M, 1>& b, //
│ │ │ │ -
39 const SharedDiagonal& model = SharedDiagonal()) :
│ │ │ │ -
40 JacobianFactor(key1, A1, key2, A2, b, model) {
│ │ │ │ -
41 }
│ │ │ │ -
│ │ │ │ -
42
│ │ │ │ -
43 inline Key key1() const {
│ │ │ │ -
44 return keys_[0];
│ │ │ │ -
45 }
│ │ │ │ -
46 inline Key key2() const {
│ │ │ │ -
47 return keys_[1];
│ │ │ │ -
48 }
│ │ │ │ -
49
│ │ │ │ -
50 // Fixed-size matrix update
│ │ │ │ -
│ │ │ │ -
51 void updateHessian(const KeyVector& infoKeys,
│ │ │ │ -
52 SymmetricBlockMatrix* info) const override {
│ │ │ │ -
53 gttic(updateHessian_BinaryJacobianFactor);
│ │ │ │ -
54 // Whiten the factor if it has a noise model
│ │ │ │ -
55 const SharedDiagonal& model = get_model();
│ │ │ │ -
56 if (model && !model->isUnit()) {
│ │ │ │ -
57 if (model->isConstrained())
│ │ │ │ -
58 throw std::invalid_argument(
│ │ │ │ -
59 "BinaryJacobianFactor::updateHessian: cannot update information with "
│ │ │ │ -
60 "constrained noise model");
│ │ │ │ -
61 BinaryJacobianFactor whitenedFactor(key1(), model->Whiten(getA(begin())),
│ │ │ │ -
62 key2(), model->Whiten(getA(end())), model->whiten(getb()));
│ │ │ │ -
63 whitenedFactor.updateHessian(infoKeys, info);
│ │ │ │ -
64 } else {
│ │ │ │ -
65 // First build an array of slots
│ │ │ │ -
66 DenseIndex slot1 = Slot(infoKeys, key1());
│ │ │ │ -
67 DenseIndex slot2 = Slot(infoKeys, key2());
│ │ │ │ -
68 DenseIndex slotB = info->nBlocks() - 1;
│ │ │ │ -
69
│ │ │ │ -
70 const Matrix& Ab = Ab_.matrix();
│ │ │ │ -
71 Eigen::Block<const Matrix, M, N1> A1(Ab, 0, 0);
│ │ │ │ -
72 Eigen::Block<const Matrix, M, N2> A2(Ab, 0, N1);
│ │ │ │ -
73 Eigen::Block<const Matrix, M, 1> b(Ab, 0, N1 + N2);
│ │ │ │ -
74
│ │ │ │ -
75 // We perform I += A'*A to the upper triangle
│ │ │ │ -
76 info->diagonalBlock(slot1).rankUpdate(A1.transpose());
│ │ │ │ -
77 info->updateOffDiagonalBlock(slot1, slot2, A1.transpose() * A2);
│ │ │ │ -
78 info->updateOffDiagonalBlock(slot1, slotB, A1.transpose() * b);
│ │ │ │ -
79 info->diagonalBlock(slot2).rankUpdate(A2.transpose());
│ │ │ │ -
80 info->updateOffDiagonalBlock(slot2, slotB, A2.transpose() * b);
│ │ │ │ -
81 info->updateDiagonalBlock(slotB, b.transpose() * b);
│ │ │ │ -
82 }
│ │ │ │ -
83 }
│ │ │ │ -
│ │ │ │ -
84};
│ │ │ │ -
│ │ │ │ -
85
│ │ │ │ -
86template<int M, int N1, int N2>
│ │ │ │ -
│ │ │ │ -
87struct traits<BinaryJacobianFactor<M, N1, N2> > : Testable<
│ │ │ │ -
88 BinaryJacobianFactor<M, N1, N2> > {
│ │ │ │ -
89};
│ │ │ │ -
│ │ │ │ -
90
│ │ │ │ -
91} //namespace gtsam
│ │ │ │ -
timing.h
Timing utilities.
│ │ │ │ -
SymmetricBlockMatrix.h
Access to matrices via blocks of pre-defined sizes.
│ │ │ │ - │ │ │ │ +
29#include <Eigen/Sparse>
│ │ │ │ +
30
│ │ │ │ +
31namespace gtsam {
│ │ │ │ +
32
│ │ │ │ +
35typedef Eigen::SparseMatrix<double, Eigen::ColMajor, int> SparseEigen;
│ │ │ │ +
36
│ │ │ │ +
│ │ │ │ +
38SparseEigen sparseJacobianEigen(
│ │ │ │ +
39 const GaussianFactorGraph &gfg, const Ordering &ordering) {
│ │ │ │ +
40 gttic_(SparseEigen_sparseJacobianEigen);
│ │ │ │ +
41 // intermediate `entries` vector is kind of unavoidable due to how expensive
│ │ │ │ +
42 // factor->rows() is, which prevents us from populating SparseEigen directly.
│ │ │ │ +
43 size_t nrows, ncols;
│ │ │ │ +
44 auto entries = gfg.sparseJacobian(ordering, nrows, ncols);
│ │ │ │ +
45 // declare sparse matrix
│ │ │ │ +
46 SparseEigen Ab(nrows, ncols);
│ │ │ │ +
47 // See Eigen::set_from_triplets. This is about 5% faster.
│ │ │ │ +
48 // pass 1: count the nnz per inner-vector
│ │ │ │ +
49 std::vector<int> nnz(ncols, 0);
│ │ │ │ +
50 for (const auto &entry : entries) nnz[std::get<1>(entry)]++;
│ │ │ │ +
51 Ab.reserve(nnz);
│ │ │ │ +
52 // pass 2: insert the elements
│ │ │ │ +
53 for (const auto &entry : entries)
│ │ │ │ +
54 Ab.insert(std::get<0>(entry), std::get<1>(entry)) = std::get<2>(entry);
│ │ │ │ +
55 return Ab;
│ │ │ │ +
56}
│ │ │ │ +
│ │ │ │ +
57
│ │ │ │ +
58SparseEigen sparseJacobianEigen(const GaussianFactorGraph &gfg) {
│ │ │ │ +
59 gttic_(SparseEigen_sparseJacobianEigen_defaultOrdering);
│ │ │ │ +
60 return sparseJacobianEigen(gfg, Ordering(gfg.keys()));
│ │ │ │ +
61}
│ │ │ │ +
62
│ │ │ │ +
63} // namespace gtsam
│ │ │ │ +
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ +
VectorValues.h
Factor Graph Values.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
│ │ │ │ -
gtsam::SymmetricBlockMatrix::updateOffDiagonalBlock
void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
Update an off diagonal block.
Definition SymmetricBlockMatrix.h:228
│ │ │ │ -
gtsam::SymmetricBlockMatrix::diagonalBlock
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J'th diagonal block as a self adjoint view.
Definition SymmetricBlockMatrix.h:135
│ │ │ │ -
gtsam::SymmetricBlockMatrix::updateDiagonalBlock
void updateDiagonalBlock(DenseIndex I, const XprType &xpr)
Increment the diagonal block by the values in xpr. Only reads the upper triangular part of xpr.
Definition SymmetricBlockMatrix.h:212
│ │ │ │ -
gtsam::SymmetricBlockMatrix::nBlocks
DenseIndex nBlocks() const
Block count.
Definition SymmetricBlockMatrix.h:120
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::VerticalBlockMatrix::matrix
const Matrix & matrix() const
Access to full matrix (including any portions excluded by rowStart(), rowEnd(), and firstBlock())
Definition VerticalBlockMatrix.h:188
│ │ │ │ -
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
│ │ │ │ -
gtsam::Factor::begin
const_iterator begin() const
Iterator at beginning of involved variable keys.
Definition Factor.h:143
│ │ │ │ -
gtsam::Factor::end
const_iterator end() const
Iterator at end of involved variable keys.
Definition Factor.h:146
│ │ │ │ -
gtsam::BinaryJacobianFactor
A binary JacobianFactor specialization that uses fixed matrix math for speed.
Definition BinaryJacobianFactor.h:33
│ │ │ │ -
gtsam::BinaryJacobianFactor::BinaryJacobianFactor
BinaryJacobianFactor(Key key1, const Eigen::Matrix< double, M, N1 > &A1, Key key2, const Eigen::Matrix< double, M, N2 > &A2, const Eigen::Matrix< double, M, 1 > &b, const SharedDiagonal &model=SharedDiagonal())
Constructor.
Definition BinaryJacobianFactor.h:36
│ │ │ │ -
gtsam::BinaryJacobianFactor::updateHessian
void updateHessian(const KeyVector &infoKeys, SymmetricBlockMatrix *info) const override
Update an information matrix by adding the information corresponding to this factor (used internally ...
Definition BinaryJacobianFactor.h:51
│ │ │ │ -
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition JacobianFactor.h:91
│ │ │ │ -
gtsam::JacobianFactor::getb
const constBVector getb() const
Get a view of the r.h.s.
Definition JacobianFactor.h:297
│ │ │ │ -
gtsam::JacobianFactor::get_model
const SharedDiagonal & get_model() const
get a copy of model
Definition JacobianFactor.h:291
│ │ │ │ -
gtsam::JacobianFactor::getA
constABlock getA() const
Get a view of the A matrix, not weighted by noise.
Definition JacobianFactor.h:303
│ │ │ │ +
gtsam::SparseEigen
Eigen::SparseMatrix< double, Eigen::ColMajor, int > SparseEigen
Eigen-format sparse matrix.
Definition SparseEigen.h:35
│ │ │ │ +
gtsam::sparseJacobianEigen
SparseEigen sparseJacobianEigen(const GaussianFactorGraph &gfg, const Ordering &ordering)
Constructs an Eigen-format SparseMatrix of a GaussianFactorGraph.
Definition SparseEigen.h:38
│ │ │ │ +
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ +
gtsam::GaussianFactorGraph::sparseJacobian
std::vector< std::tuple< int, int, double > > sparseJacobian(const Ordering &ordering, size_t &nrows, size_t &ncols) const
Returns a sparse augmented Jacbian matrix as a vector of i, j, and s, where i(k) and j(k) are the bas...
Definition GaussianFactorGraph.cpp:119
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,186 +1,89 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -BinaryJacobianFactor.h │ │ │ │ │ +SparseEigen.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -21#pragma once │ │ │ │ │ -22 │ │ │ │ │ -23#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_b_a_s_e_/_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_b_a_s_e_/_t_i_m_i_n_g_._h> │ │ │ │ │ -26 │ │ │ │ │ -27namespace _g_t_s_a_m { │ │ │ │ │ +24#pragma once │ │ │ │ │ +25 │ │ │ │ │ +26#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +27#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ 28 │ │ │ │ │ -32template │ │ │ │ │ -_3_3struct _B_i_n_a_r_y_J_a_c_o_b_i_a_n_F_a_c_t_o_r: _J_a_c_o_b_i_a_n_F_a_c_t_o_r { │ │ │ │ │ -34 │ │ │ │ │ -_3_6 _B_i_n_a_r_y_J_a_c_o_b_i_a_n_F_a_c_t_o_r(_K_e_y key1, const Eigen::Matrix& A1, │ │ │ │ │ -37 _K_e_y key2, const Eigen::Matrix& A2, │ │ │ │ │ -38 const Eigen::Matrix& b, // │ │ │ │ │ -39 const SharedDiagonal& model = SharedDiagonal()) : │ │ │ │ │ -40 _J_a_c_o_b_i_a_n_F_a_c_t_o_r(key1, A1, key2, A2, b, model) { │ │ │ │ │ -41 } │ │ │ │ │ -42 │ │ │ │ │ -43 inline _K_e_y key1() const { │ │ │ │ │ -44 return _k_e_y_s__[0]; │ │ │ │ │ -45 } │ │ │ │ │ -46 inline _K_e_y key2() const { │ │ │ │ │ -47 return _k_e_y_s__[1]; │ │ │ │ │ -48 } │ │ │ │ │ -49 │ │ │ │ │ -50 // Fixed-size matrix update │ │ │ │ │ -_5_1 void _u_p_d_a_t_e_H_e_s_s_i_a_n(const _K_e_y_V_e_c_t_o_r& infoKeys, │ │ │ │ │ -52 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x* info) const override { │ │ │ │ │ -53 gttic(updateHessian_BinaryJacobianFactor); │ │ │ │ │ -54 // Whiten the factor if it has a noise model │ │ │ │ │ -55 const SharedDiagonal& model = _g_e_t___m_o_d_e_l(); │ │ │ │ │ -56 if (model && !model->isUnit()) { │ │ │ │ │ -57 if (model->isConstrained()) │ │ │ │ │ -58 throw std::invalid_argument( │ │ │ │ │ -59 "BinaryJacobianFactor::updateHessian: cannot update information with " │ │ │ │ │ -60 "constrained noise model"); │ │ │ │ │ -61 _B_i_n_a_r_y_J_a_c_o_b_i_a_n_F_a_c_t_o_r whitenedFactor(key1(), model->Whiten(_g_e_t_A(_b_e_g_i_n())), │ │ │ │ │ -62 key2(), model->Whiten(_g_e_t_A(_e_n_d())), model->whiten(_g_e_t_b())); │ │ │ │ │ -63 whitenedFactor._u_p_d_a_t_e_H_e_s_s_i_a_n(infoKeys, info); │ │ │ │ │ -64 } else { │ │ │ │ │ -65 // First build an array of slots │ │ │ │ │ -66 _D_e_n_s_e_I_n_d_e_x slot1 = Slot(infoKeys, key1()); │ │ │ │ │ -67 _D_e_n_s_e_I_n_d_e_x slot2 = Slot(infoKeys, key2()); │ │ │ │ │ -68 _D_e_n_s_e_I_n_d_e_x slotB = info->_n_B_l_o_c_k_s() - 1; │ │ │ │ │ -69 │ │ │ │ │ -70 const Matrix& Ab = Ab_._m_a_t_r_i_x(); │ │ │ │ │ -71 Eigen::Block A1(Ab, 0, 0); │ │ │ │ │ -72 Eigen::Block A2(Ab, 0, N1); │ │ │ │ │ -73 Eigen::Block b(Ab, 0, N1 + N2); │ │ │ │ │ -74 │ │ │ │ │ -75 // We perform I += A'*A to the upper triangle │ │ │ │ │ -76 info->_d_i_a_g_o_n_a_l_B_l_o_c_k(slot1).rankUpdate(A1.transpose()); │ │ │ │ │ -77 info->_u_p_d_a_t_e_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k(slot1, slot2, A1.transpose() * A2); │ │ │ │ │ -78 info->_u_p_d_a_t_e_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k(slot1, slotB, A1.transpose() * b); │ │ │ │ │ -79 info->_d_i_a_g_o_n_a_l_B_l_o_c_k(slot2).rankUpdate(A2.transpose()); │ │ │ │ │ -80 info->_u_p_d_a_t_e_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k(slot2, slotB, A2.transpose() * b); │ │ │ │ │ -81 info->_u_p_d_a_t_e_D_i_a_g_o_n_a_l_B_l_o_c_k(slotB, b.transpose() * b); │ │ │ │ │ -82 } │ │ │ │ │ -83 } │ │ │ │ │ -84}; │ │ │ │ │ -85 │ │ │ │ │ -86template │ │ │ │ │ -_8_7struct _t_r_a_i_t_s<_B_i_n_a_r_y_J_a_c_o_b_i_a_n_F_a_c_t_o_r > : _T_e_s_t_a_b_l_e< │ │ │ │ │ -88 BinaryJacobianFactor > { │ │ │ │ │ -89}; │ │ │ │ │ -90 │ │ │ │ │ -91} //namespace gtsam │ │ │ │ │ -_t_i_m_i_n_g_._h │ │ │ │ │ -Timing utilities. │ │ │ │ │ -_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_._h │ │ │ │ │ -Access to matrices via blocks of pre-defined sizes. │ │ │ │ │ -_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ +29#include │ │ │ │ │ +30 │ │ │ │ │ +31namespace _g_t_s_a_m { │ │ │ │ │ +32 │ │ │ │ │ +_3_5typedef Eigen::SparseMatrix _S_p_a_r_s_e_E_i_g_e_n; │ │ │ │ │ +36 │ │ │ │ │ +_3_8_S_p_a_r_s_e_E_i_g_e_n _s_p_a_r_s_e_J_a_c_o_b_i_a_n_E_i_g_e_n( │ │ │ │ │ +39 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, const _O_r_d_e_r_i_n_g &ordering) { │ │ │ │ │ +40 gttic_(SparseEigen_sparseJacobianEigen); │ │ │ │ │ +41 // intermediate `entries` vector is kind of unavoidable due to how expensive │ │ │ │ │ +42 // factor->rows() is, which prevents us from populating SparseEigen │ │ │ │ │ +directly. │ │ │ │ │ +43 size_t nrows, ncols; │ │ │ │ │ +44 auto entries = gfg._s_p_a_r_s_e_J_a_c_o_b_i_a_n(ordering, nrows, ncols); │ │ │ │ │ +45 // declare sparse matrix │ │ │ │ │ +46 _S_p_a_r_s_e_E_i_g_e_n Ab(nrows, ncols); │ │ │ │ │ +47 // See Eigen::set_from_triplets. This is about 5% faster. │ │ │ │ │ +48 // pass 1: count the nnz per inner-vector │ │ │ │ │ +49 std::vector nnz(ncols, 0); │ │ │ │ │ +50 for (const auto &entry : entries) nnz[std::get<1>(entry)]++; │ │ │ │ │ +51 Ab.reserve(nnz); │ │ │ │ │ +52 // pass 2: insert the elements │ │ │ │ │ +53 for (const auto &entry : entries) │ │ │ │ │ +54 Ab.insert(std::get<0>(entry), std::get<1>(entry)) = std::get<2>(entry); │ │ │ │ │ +55 return Ab; │ │ │ │ │ +56} │ │ │ │ │ +57 │ │ │ │ │ +58_S_p_a_r_s_e_E_i_g_e_n _s_p_a_r_s_e_J_a_c_o_b_i_a_n_E_i_g_e_n(const GaussianFactorGraph &gfg) { │ │ │ │ │ +59 gttic_(SparseEigen_sparseJacobianEigen_defaultOrdering); │ │ │ │ │ +60 return _s_p_a_r_s_e_J_a_c_o_b_i_a_n_E_i_g_e_n(gfg, Ordering(gfg.keys())); │ │ │ │ │ +61} │ │ │ │ │ +62 │ │ │ │ │ +63} // namespace gtsam │ │ │ │ │ +_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ +Factor Graph Values. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_n_s_e_I_n_d_e_x │ │ │ │ │ -ptrdiff_t DenseIndex │ │ │ │ │ -The index type for Eigen objects. │ │ │ │ │ -DDeeffiinniittiioonn types.h:106 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ -of blocks. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_u_p_d_a_t_e_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ -void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr) │ │ │ │ │ -Update an off diagonal block. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:228 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_d_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ -Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J) │ │ │ │ │ -Return the J'th diagonal block as a self adjoint view. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:135 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_u_p_d_a_t_e_D_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ -void updateDiagonalBlock(DenseIndex I, const XprType &xpr) │ │ │ │ │ -Increment the diagonal block by the values in xpr. Only reads the upper │ │ │ │ │ -triangular part of xpr. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:212 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_n_B_l_o_c_k_s │ │ │ │ │ -DenseIndex nBlocks() const │ │ │ │ │ -Block count. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:120 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_m_a_t_r_i_x │ │ │ │ │ -const Matrix & matrix() const │ │ │ │ │ -Access to full matrix (including any portions excluded by rowStart(), rowEnd(), │ │ │ │ │ -and firstBlock()) │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:188 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s__ │ │ │ │ │ -KeyVector keys_ │ │ │ │ │ -The keys involved in this factor. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_b_e_g_i_n │ │ │ │ │ -const_iterator begin() const │ │ │ │ │ -Iterator at beginning of involved variable keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:143 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_n_d │ │ │ │ │ -const_iterator end() const │ │ │ │ │ -Iterator at end of involved variable keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:146 │ │ │ │ │ -_g_t_s_a_m_:_:_B_i_n_a_r_y_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ -A binary JacobianFactor specialization that uses fixed matrix math for speed. │ │ │ │ │ -DDeeffiinniittiioonn BinaryJacobianFactor.h:33 │ │ │ │ │ -_g_t_s_a_m_:_:_B_i_n_a_r_y_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_B_i_n_a_r_y_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ -BinaryJacobianFactor(Key key1, const Eigen::Matrix< double, M, N1 > &A1, Key │ │ │ │ │ -key2, const Eigen::Matrix< double, M, N2 > &A2, const Eigen::Matrix< double, M, │ │ │ │ │ -1 > &b, const SharedDiagonal &model=SharedDiagonal()) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn BinaryJacobianFactor.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_B_i_n_a_r_y_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_u_p_d_a_t_e_H_e_s_s_i_a_n │ │ │ │ │ -void updateHessian(const KeyVector &infoKeys, SymmetricBlockMatrix *info) const │ │ │ │ │ -override │ │ │ │ │ -Update an information matrix by adding the information corresponding to this │ │ │ │ │ -factor (used internally ... │ │ │ │ │ -DDeeffiinniittiioonn BinaryJacobianFactor.h:51 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ -A Gaussian factor in the squared-error form. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:91 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_e_t_b │ │ │ │ │ -const constBVector getb() const │ │ │ │ │ -Get a view of the r.h.s. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:297 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_e_t___m_o_d_e_l │ │ │ │ │ -const SharedDiagonal & get_model() const │ │ │ │ │ -get a copy of model │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:291 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_e_t_A │ │ │ │ │ -constABlock getA() const │ │ │ │ │ -Get a view of the A matrix, not weighted by noise. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:303 │ │ │ │ │ +_g_t_s_a_m_:_:_S_p_a_r_s_e_E_i_g_e_n │ │ │ │ │ +Eigen::SparseMatrix< double, Eigen::ColMajor, int > SparseEigen │ │ │ │ │ +Eigen-format sparse matrix. │ │ │ │ │ +DDeeffiinniittiioonn SparseEigen.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_s_p_a_r_s_e_J_a_c_o_b_i_a_n_E_i_g_e_n │ │ │ │ │ +SparseEigen sparseJacobianEigen(const GaussianFactorGraph &gfg, const Ordering │ │ │ │ │ +&ordering) │ │ │ │ │ +Constructs an Eigen-format SparseMatrix of a GaussianFactorGraph. │ │ │ │ │ +DDeeffiinniittiioonn SparseEigen.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_p_a_r_s_e_J_a_c_o_b_i_a_n │ │ │ │ │ +std::vector< std::tuple< int, int, double > > sparseJacobian(const Ordering │ │ │ │ │ +&ordering, size_t &nrows, size_t &ncols) const │ │ │ │ │ +Returns a sparse augmented Jacbian matrix as a vector of i, j, and s, where i │ │ │ │ │ +(k) and j(k) are the bas... │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.cpp:119 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _B_i_n_a_r_y_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ + * _S_p_a_r_s_e_E_i_g_e_n_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00806.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianDensity.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Errors.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,36 +94,87 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces
│ │ │ │ -
GaussianDensity.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ +Functions
│ │ │ │ +
Errors.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

A Gaussian Density. │ │ │ │ +

vector of errors │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

struct  gtsam::traits< Errors >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Typedefs

│ │ │ │ +using gtsam::Errors = FastList< Vector >
 Errors is a vector of errors.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +Errors gtsam::createErrors (const VectorValues &V)
 Break V into pieces according to its start indices.
 
│ │ │ │ +void gtsam::print (const Errors &e, const std::string &s="Errors")
 Print an Errors instance.
 
│ │ │ │ +bool gtsam::equality (const Errors &actual, const Errors &expected, double tol)
 
│ │ │ │ +Errors gtsam::operator+ (const Errors &a, const Errors &b)
 Addition.
 
│ │ │ │ +Errors gtsam::operator- (const Errors &a, const Errors &b)
 Subtraction.
 
│ │ │ │ +Errors gtsam::operator- (const Errors &a)
 Negation.
 
│ │ │ │ +double gtsam::dot (const Errors &a, const Errors &b)
 Dot product.
 
│ │ │ │ +void gtsam::axpy (double alpha, const Errors &x, Errors &y)
 BLAS level 2 style AXPY, y := alpha*x + y
 
│ │ │ │

Detailed Description

│ │ │ │ -

A Gaussian Density.

│ │ │ │ +

vector of errors

│ │ │ │
Author
Frank Dellaert
│ │ │ │ -
Date
Jan 21, 2012
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,54 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -GaussianDensity.cpp File Reference │ │ │ │ │ -A Gaussian Density. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Errors.h File Reference │ │ │ │ │ +vector of errors _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _E_r_r_o_r_s_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +using  ggttssaamm::::EErrrroorrss = _F_a_s_t_L_i_s_t< Vector > │ │ │ │ │ +  Errors is a vector of errors. │ │ │ │ │ +  │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +_E_r_r_o_r_s  ggttssaamm::::ccrreeaatteeEErrrroorrss (const _V_e_c_t_o_r_V_a_l_u_e_s &V) │ │ │ │ │ +  Break V into pieces according to its start indices. │ │ │ │ │ +  │ │ │ │ │ + void  ggttssaamm::::pprriinntt (const _E_r_r_o_r_s &e, const std::string &s="Errors") │ │ │ │ │ +  Print an Errors instance. │ │ │ │ │ +  │ │ │ │ │ + bool  ggttssaamm::::eeqquuaalliittyy (const _E_r_r_o_r_s &actual, const _E_r_r_o_r_s &expected, double │ │ │ │ │ + tol) │ │ │ │ │ +  │ │ │ │ │ +_E_r_r_o_r_s  ggttssaamm::::ooppeerraattoorr++ (const _E_r_r_o_r_s &a, const _E_r_r_o_r_s &b) │ │ │ │ │ +  Addition. │ │ │ │ │ +  │ │ │ │ │ +_E_r_r_o_r_s  ggttssaamm::::ooppeerraattoorr-- (const _E_r_r_o_r_s &a, const _E_r_r_o_r_s &b) │ │ │ │ │ +  Subtraction. │ │ │ │ │ +  │ │ │ │ │ +_E_r_r_o_r_s  ggttssaamm::::ooppeerraattoorr-- (const _E_r_r_o_r_s &a) │ │ │ │ │ +  Negation. │ │ │ │ │ +  │ │ │ │ │ +double  ggttssaamm::::ddoott (const _E_r_r_o_r_s &a, const _E_r_r_o_r_s &b) │ │ │ │ │ +  Dot product. │ │ │ │ │ +  │ │ │ │ │ + void  ggttssaamm::::aaxxppyy (double alpha, const _E_r_r_o_r_s &x, _E_r_r_o_r_s &y) │ │ │ │ │ +  BLAS level 2 style AXPY, y := alpha*x + y │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A Gaussian Density. │ │ │ │ │ +vector of errors │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - Jan 21, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_D_e_n_s_i_t_y_._c_p_p │ │ │ │ │ + * _E_r_r_o_r_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00809.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/KalmanFilter.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianFactorGraph.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,39 +94,55 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces
│ │ │ │ -
KalmanFilter.cpp File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ +Functions
│ │ │ │ +
GaussianFactorGraph.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Simple linear Kalman filter. │ │ │ │ +

Linear Factor Graph where all factors are Gaussians. │ │ │ │ More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Typedefs

│ │ │ │ +using gtsam::SparseTriplets = std::vector< std::tuple< int, int, double > >
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

bool gtsam::hasConstraints (const GaussianFactorGraph &factors)
 Evaluates whether linear factors have any constrained noise models.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Simple linear Kalman filter.

│ │ │ │ -

Implemented using factor graphs, i.e., does Cholesky-based SRIF, really.

│ │ │ │ -
Date
Sep 3, 2011
│ │ │ │ -
Author
Stephen Williams
│ │ │ │ +

Linear Factor Graph where all factors are Gaussians.

│ │ │ │ +
Author
Kai Ni
│ │ │ │ +
│ │ │ │ +Christian Potthast
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │ Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,24 +1,30 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -KalmanFilter.cpp File Reference │ │ │ │ │ -Simple linear Kalman filter. _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +GaussianFactorGraph.cpp File Reference │ │ │ │ │ +Linear Factor Graph where all factors are Gaussians. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +using  ggttssaamm::::SSppaarrsseeTTrriipplleettss = std::vector< std::tuple< int, int, double > > │ │ │ │ │ +  │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +bool  _g_t_s_a_m_:_:_h_a_s_C_o_n_s_t_r_a_i_n_t_s (const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &factors) │ │ │ │ │ +  Evaluates whether linear factors have any constrained noise models. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Simple linear Kalman filter. │ │ │ │ │ -Implemented using factor graphs, i.e., does Cholesky-based SRIF, really. │ │ │ │ │ - Date │ │ │ │ │ - Sep 3, 2011 │ │ │ │ │ +Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ Author │ │ │ │ │ - Stephen Williams │ │ │ │ │ + Kai Ni │ │ │ │ │ + Christian Potthast │ │ │ │ │ + Richard Roberts │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _K_a_l_m_a_n_F_i_l_t_e_r_._c_p_p │ │ │ │ │ + * _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00812.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/AcceleratedPowerMethod.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianDensity.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,51 +95,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs
│ │ │ │ -
AcceleratedPowerMethod.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
GaussianDensity.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

accelerated power method for fast eigenvalue and eigenvector computation │ │ │ │ +

A Gaussian Density. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::AcceleratedPowerMethod< Operator >
 Compute maximum Eigenpair with accelerated power method. More...
class  gtsam::GaussianDensity
 A GaussianDensity is a GaussianConditional without parents. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Typedefs

│ │ │ │ -using gtsam::Sparse = Eigen::SparseMatrix< double >
 
│ │ │ │

Detailed Description

│ │ │ │ -

accelerated power method for fast eigenvalue and eigenvector computation

│ │ │ │ -
Date
Sept 2020
│ │ │ │ -
Author
Jing Wu
│ │ │ │ +

A Gaussian Density.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
Date
Jan 21, 2012
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,31 +1,27 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ -AcceleratedPowerMethod.h File Reference │ │ │ │ │ -accelerated power method for fast eigenvalue and eigenvector computation │ │ │ │ │ -_M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +GaussianDensity.h File Reference │ │ │ │ │ +A Gaussian Density. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_A_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_M_e_t_h_o_d_<_ _O_p_e_r_a_t_o_r_ _> │ │ │ │ │ -  Compute maximum Eigenpair with accelerated power method. _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_D_e_n_s_i_t_y │ │ │ │ │ +  A _G_a_u_s_s_i_a_n_D_e_n_s_i_t_y is a _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l without parents. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -using  ggttssaamm::::SSppaarrssee = Eigen::SparseMatrix< double > │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -accelerated power method for fast eigenvalue and eigenvector computation │ │ │ │ │ - Date │ │ │ │ │ - Sept 2020 │ │ │ │ │ +A Gaussian Density. │ │ │ │ │ Author │ │ │ │ │ - Jing Wu │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + Jan 21, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _A_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_M_e_t_h_o_d_._h │ │ │ │ │ + * _G_a_u_s_s_i_a_n_D_e_n_s_i_t_y_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00812_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/AcceleratedPowerMethod.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianDensity.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,166 +98,92 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
AcceleratedPowerMethod.h
│ │ │ │ +
GaussianDensity.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ +
19// \callgraph
│ │ │ │
20#pragma once
│ │ │ │
21
│ │ │ │ -
22#include <gtsam/linear/PowerMethod.h>
│ │ │ │ +
22#include <gtsam/linear/GaussianConditional.h>
│ │ │ │
23
│ │ │ │
24namespace gtsam {
│ │ │ │
25
│ │ │ │ -
26using Sparse = Eigen::SparseMatrix<double>;
│ │ │ │ -
27
│ │ │ │ -
50template <class Operator>
│ │ │ │ -
│ │ │ │ -
51class AcceleratedPowerMethod : public PowerMethod<Operator> {
│ │ │ │ -
52
│ │ │ │ -
53 double beta_ = 0; // a Polyak momentum term
│ │ │ │ -
54
│ │ │ │ -
55 Vector previousVector_; // store previous vector
│ │ │ │ -
56
│ │ │ │ -
57 public:
│ │ │ │ -
│ │ │ │ -
62 explicit AcceleratedPowerMethod(
│ │ │ │ -
63 const Operator &A, const boost::optional<Vector> initial = boost::none,
│ │ │ │ -
64 double initialBeta = 0.0)
│ │ │ │ -
65 : PowerMethod<Operator>(A, initial) {
│ │ │ │ -
66 // initialize Ritz eigen vector and previous vector
│ │ │ │ -
67 this->ritzVector_ = initial ? initial.get() : Vector::Random(this->dim_);
│ │ │ │ -
68 this->ritzVector_.normalize();
│ │ │ │ -
69 previousVector_ = Vector::Zero(this->dim_);
│ │ │ │ +
│ │ │ │ +
32 class GTSAM_EXPORT GaussianDensity : public GaussianConditional {
│ │ │ │ +
33
│ │ │ │ +
34 public:
│ │ │ │ +
35
│ │ │ │ +
36 typedef boost::shared_ptr<GaussianDensity> shared_ptr;
│ │ │ │ +
37
│ │ │ │ +
│ │ │ │ +
39 GaussianDensity() :
│ │ │ │ +
40 GaussianConditional() {
│ │ │ │ +
41 }
│ │ │ │ +
│ │ │ │ +
42
│ │ │ │ +
│ │ │ │ +
44 GaussianDensity(const GaussianConditional& conditional) :
│ │ │ │ +
45 GaussianConditional(conditional) {
│ │ │ │ +
46 if(conditional.nrParents() != 0)
│ │ │ │ +
47 throw std::invalid_argument("GaussianDensity can only be created from a conditional with no parents");
│ │ │ │ +
48 }
│ │ │ │ +
│ │ │ │ +
49
│ │ │ │ +
│ │ │ │ +
51 GaussianDensity(Key key, const Vector& d, const Matrix& R, const SharedDiagonal& noiseModel = SharedDiagonal()) :
│ │ │ │ +
52 GaussianConditional(key, d, R, noiseModel) {}
│ │ │ │ +
│ │ │ │ +
53
│ │ │ │ +
55 static GaussianDensity FromMeanAndStddev(Key key, const Vector& mean,
│ │ │ │ +
56 double sigma);
│ │ │ │ +
57
│ │ │ │ +
59 void print(const std::string& = "GaussianDensity",
│ │ │ │ +
60 const KeyFormatter& formatter = DefaultKeyFormatter) const override;
│ │ │ │ +
61
│ │ │ │ +
63 Vector mean() const;
│ │ │ │ +
64
│ │ │ │ +
66 Matrix covariance() const;
│ │ │ │ +
67
│ │ │ │ +
68 };
│ │ │ │ +
│ │ │ │ +
69 // GaussianDensity
│ │ │ │
70
│ │ │ │ -
71 // initialize beta_
│ │ │ │ -
72 beta_ = initialBeta;
│ │ │ │ -
73 }
│ │ │ │ -
│ │ │ │ -
74
│ │ │ │ -
│ │ │ │ -
80 Vector acceleratedPowerIteration (const Vector &x1, const Vector &x0,
│ │ │ │ -
81 const double beta) const {
│ │ │ │ -
82 Vector y = this->A_ * x1 - beta * x0;
│ │ │ │ -
83 y.normalize();
│ │ │ │ -
84 return y;
│ │ │ │ -
85 }
│ │ │ │ -
│ │ │ │ -
86
│ │ │ │ -
│ │ │ │ -
92 Vector acceleratedPowerIteration () const {
│ │ │ │ -
93 Vector y = acceleratedPowerIteration(this->ritzVector_, previousVector_, beta_);
│ │ │ │ -
94 return y;
│ │ │ │ -
95 }
│ │ │ │ -
│ │ │ │ -
96
│ │ │ │ -
│ │ │ │ -
101 double estimateBeta(const size_t T = 10) const {
│ │ │ │ -
102 // set initial estimation of maxBeta
│ │ │ │ -
103 Vector initVector = this->ritzVector_;
│ │ │ │ -
104 const double up = initVector.dot( this->A_ * initVector );
│ │ │ │ -
105 const double down = initVector.dot(initVector);
│ │ │ │ -
106 const double mu = up / down;
│ │ │ │ -
107 double maxBeta = mu * mu / 4;
│ │ │ │ -
108 size_t maxIndex;
│ │ │ │ -
109 std::vector<double> betas;
│ │ │ │ -
110
│ │ │ │ -
111 Matrix R = Matrix::Zero(this->dim_, 10);
│ │ │ │ -
112 // run T times of iteration to find the beta that has the largest Rayleigh quotient
│ │ │ │ -
113 for (size_t t = 0; t < T; t++) {
│ │ │ │ -
114 // after each t iteration, reset the betas with the current maxBeta
│ │ │ │ -
115 betas = {2 / 3 * maxBeta, 0.99 * maxBeta, maxBeta, 1.01 * maxBeta,
│ │ │ │ -
116 1.5 * maxBeta};
│ │ │ │ -
117 // iterate through every beta value
│ │ │ │ -
118 for (size_t k = 0; k < betas.size(); ++k) {
│ │ │ │ -
119 // initialize x0 and x00 in each iteration of each beta
│ │ │ │ -
120 Vector x0 = initVector;
│ │ │ │ -
121 Vector x00 = Vector::Zero(this->dim_);
│ │ │ │ -
122 // run 10 steps of accelerated power iteration with this beta
│ │ │ │ -
123 for (size_t j = 1; j < 10; j++) {
│ │ │ │ -
124 if (j < 2) {
│ │ │ │ -
125 R.col(0) = acceleratedPowerIteration(x0, x00, betas[k]);
│ │ │ │ -
126 R.col(1) = acceleratedPowerIteration(R.col(0), x0, betas[k]);
│ │ │ │ -
127 } else {
│ │ │ │ -
128 R.col(j) = acceleratedPowerIteration(R.col(j - 1), R.col(j - 2),
│ │ │ │ -
129 betas[k]);
│ │ │ │ -
130 }
│ │ │ │ -
131 }
│ │ │ │ -
132 // compute the Rayleigh quotient for the randomly sampled vector after
│ │ │ │ -
133 // 10 steps of power accelerated iteration
│ │ │ │ -
134 const Vector x = R.col(9);
│ │ │ │ -
135 const double up = x.dot(this->A_ * x);
│ │ │ │ -
136 const double down = x.dot(x);
│ │ │ │ -
137 const double mu = up / down;
│ │ │ │ -
138 // store the momentum with largest Rayleigh quotient and its according index of beta_
│ │ │ │ -
139 if (mu * mu / 4 > maxBeta) {
│ │ │ │ -
140 // save the max beta index
│ │ │ │ -
141 maxIndex = k;
│ │ │ │ -
142 maxBeta = mu * mu / 4;
│ │ │ │ -
143 }
│ │ │ │ -
144 }
│ │ │ │ -
145 }
│ │ │ │ -
146 // set beta_ to momentum with largest Rayleigh quotient
│ │ │ │ -
147 return betas[maxIndex];
│ │ │ │ -
148 }
│ │ │ │ -
│ │ │ │ -
149
│ │ │ │ -
│ │ │ │ -
156 bool compute(size_t maxIterations, double tol) {
│ │ │ │ -
157 // Starting
│ │ │ │ -
158 bool isConverged = false;
│ │ │ │ -
159
│ │ │ │ -
160 for (size_t i = 0; i < maxIterations && !isConverged; i++) {
│ │ │ │ -
161 ++(this->nrIterations_);
│ │ │ │ -
162 Vector tmp = this->ritzVector_;
│ │ │ │ -
163 // update the ritzVector after accelerated power iteration
│ │ │ │ -
164 this->ritzVector_ = acceleratedPowerIteration();
│ │ │ │ -
165 // update the previousVector with ritzVector
│ │ │ │ -
166 previousVector_ = tmp;
│ │ │ │ -
167 // update the ritzValue
│ │ │ │ -
168 this->ritzValue_ = this->ritzVector_.dot(this->A_ * this->ritzVector_);
│ │ │ │ -
169 isConverged = this->converged(tol);
│ │ │ │ -
170 }
│ │ │ │ -
171
│ │ │ │ -
172 return isConverged;
│ │ │ │ -
173 }
│ │ │ │ -
│ │ │ │ -
174};
│ │ │ │ -
│ │ │ │ -
175
│ │ │ │ -
176} // namespace gtsam
│ │ │ │ -
PowerMethod.h
Power method for fast eigenvalue and eigenvector computation.
│ │ │ │ +
71}// gtsam
│ │ │ │ +
GaussianConditional.h
Conditional Gaussian Base class.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::AcceleratedPowerMethod
Compute maximum Eigenpair with accelerated power method.
Definition AcceleratedPowerMethod.h:51
│ │ │ │ -
gtsam::AcceleratedPowerMethod::acceleratedPowerIteration
Vector acceleratedPowerIteration(const Vector &x1, const Vector &x0, const double beta) const
Run accelerated power iteration to get ritzVector with beta and previous two ritzVector x0 and x00,...
Definition AcceleratedPowerMethod.h:80
│ │ │ │ -
gtsam::AcceleratedPowerMethod::AcceleratedPowerMethod
AcceleratedPowerMethod(const Operator &A, const boost::optional< Vector > initial=boost::none, double initialBeta=0.0)
Constructor from aim matrix A (given as Matrix or Sparse), optional intial vector as ritzVector.
Definition AcceleratedPowerMethod.h:62
│ │ │ │ -
gtsam::AcceleratedPowerMethod::estimateBeta
double estimateBeta(const size_t T=10) const
Tuning the momentum beta using the Best Heavy Ball algorithm in Ref(3), T is the iteration time to fi...
Definition AcceleratedPowerMethod.h:101
│ │ │ │ -
gtsam::AcceleratedPowerMethod::acceleratedPowerIteration
Vector acceleratedPowerIteration() const
Run accelerated power iteration to get ritzVector with beta and previous two ritzVector x0 and x00,...
Definition AcceleratedPowerMethod.h:92
│ │ │ │ -
gtsam::AcceleratedPowerMethod::compute
bool compute(size_t maxIterations, double tol)
Start the accelerated iteration, after performing the accelerated iteration, calculate the ritz error...
Definition AcceleratedPowerMethod.h:156
│ │ │ │ -
gtsam::PowerMethod
Compute maximum Eigenpair with power method.
Definition PowerMethod.h:57
│ │ │ │ -
gtsam::PowerMethod::A_
const Operator & A_
Const reference to an externally-held matrix whose minimum-eigenvalue we want to compute.
Definition PowerMethod.h:63
│ │ │ │ -
gtsam::PowerMethod::converged
bool converged(double tol) const
After Perform power iteration on a single Ritz value, check if the Ritz residual for the current Ritz...
Definition PowerMethod.h:112
│ │ │ │ +
gtsam::mean
Point3 mean(const CONTAINER &points)
mean
Definition Point3.h:68
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::Conditional::nrParents
size_t nrParents() const
return the number of parents
Definition Conditional.h:113
│ │ │ │ +
gtsam::GaussianConditional
A GaussianConditional functions as the node in a Bayes network.
Definition GaussianConditional.h:43
│ │ │ │ +
gtsam::GaussianDensity
A GaussianDensity is a GaussianConditional without parents.
Definition GaussianDensity.h:32
│ │ │ │ +
gtsam::GaussianDensity::GaussianDensity
GaussianDensity(Key key, const Vector &d, const Matrix &R, const SharedDiagonal &noiseModel=SharedDiagonal())
constructor using d, R
Definition GaussianDensity.h:51
│ │ │ │ +
gtsam::GaussianDensity::GaussianDensity
GaussianDensity()
default constructor needed for serialization
Definition GaussianDensity.h:39
│ │ │ │ +
gtsam::GaussianDensity::GaussianDensity
GaussianDensity(const GaussianConditional &conditional)
Copy constructor from GaussianConditional.
Definition GaussianDensity.h:44
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,186 +1,110 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -AcceleratedPowerMethod.h │ │ │ │ │ +GaussianDensity.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ +19// \callgraph │ │ │ │ │ 20#pragma once │ │ │ │ │ 21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_P_o_w_e_r_M_e_t_h_o_d_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ 23 │ │ │ │ │ 24namespace _g_t_s_a_m { │ │ │ │ │ 25 │ │ │ │ │ -26using Sparse = Eigen::SparseMatrix; │ │ │ │ │ -27 │ │ │ │ │ -50template │ │ │ │ │ -_5_1class _A_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_M_e_t_h_o_d : public _P_o_w_e_r_M_e_t_h_o_d { │ │ │ │ │ -52 │ │ │ │ │ -53 double beta_ = 0; // a Polyak momentum term │ │ │ │ │ -54 │ │ │ │ │ -55 Vector previousVector_; // store previous vector │ │ │ │ │ -56 │ │ │ │ │ -57 public: │ │ │ │ │ -_6_2 explicit _A_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_M_e_t_h_o_d( │ │ │ │ │ -63 const Operator &A, const boost::optional initial = boost::none, │ │ │ │ │ -64 double initialBeta = 0.0) │ │ │ │ │ -65 : _P_o_w_e_r_M_e_t_h_o_d(A, initial) { │ │ │ │ │ -66 // initialize Ritz eigen vector and previous vector │ │ │ │ │ -67 this->ritzVector_ = initial ? initial.get() : Vector::Random(this->dim_); │ │ │ │ │ -68 this->ritzVector_.normalize(); │ │ │ │ │ -69 previousVector_ = Vector::Zero(this->dim_); │ │ │ │ │ +_3_2 class GTSAM_EXPORT _G_a_u_s_s_i_a_n_D_e_n_s_i_t_y : public _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l { │ │ │ │ │ +33 │ │ │ │ │ +34 public: │ │ │ │ │ +35 │ │ │ │ │ +36 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +37 │ │ │ │ │ +_3_9 _G_a_u_s_s_i_a_n_D_e_n_s_i_t_y() : │ │ │ │ │ +40 _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l() { │ │ │ │ │ +41 } │ │ │ │ │ +42 │ │ │ │ │ +_4_4 _G_a_u_s_s_i_a_n_D_e_n_s_i_t_y(const _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l& conditional) : │ │ │ │ │ +45 _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l(conditional) { │ │ │ │ │ +46 if(conditional._n_r_P_a_r_e_n_t_s() != 0) │ │ │ │ │ +47 throw std::invalid_argument("GaussianDensity can only be created from a │ │ │ │ │ +conditional with no parents"); │ │ │ │ │ +48 } │ │ │ │ │ +49 │ │ │ │ │ +_5_1 _G_a_u_s_s_i_a_n_D_e_n_s_i_t_y(_K_e_y key, const Vector& d, const Matrix& R, const │ │ │ │ │ +SharedDiagonal& noiseModel = SharedDiagonal()) : │ │ │ │ │ +52 _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l(key, d, R, noiseModel) {} │ │ │ │ │ +53 │ │ │ │ │ +55 static _G_a_u_s_s_i_a_n_D_e_n_s_i_t_y FromMeanAndStddev(_K_e_y key, const Vector& mean, │ │ │ │ │ +56 double sigma); │ │ │ │ │ +57 │ │ │ │ │ +59 void _p_r_i_n_t(const std::string& = "GaussianDensity", │ │ │ │ │ +60 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override; │ │ │ │ │ +61 │ │ │ │ │ +63 Vector _m_e_a_n() const; │ │ │ │ │ +64 │ │ │ │ │ +66 Matrix covariance() const; │ │ │ │ │ +67 │ │ │ │ │ +68 }; │ │ │ │ │ +69 // GaussianDensity │ │ │ │ │ 70 │ │ │ │ │ -71 // initialize beta_ │ │ │ │ │ -72 beta_ = initialBeta; │ │ │ │ │ -73 } │ │ │ │ │ -74 │ │ │ │ │ -_8_0 Vector _a_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_I_t_e_r_a_t_i_o_n (const Vector &x1, const Vector &x0, │ │ │ │ │ -81 const double beta) const { │ │ │ │ │ -82 Vector y = this->_A__ * x1 - beta * x0; │ │ │ │ │ -83 y.normalize(); │ │ │ │ │ -84 return y; │ │ │ │ │ -85 } │ │ │ │ │ -86 │ │ │ │ │ -_9_2 Vector _a_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_I_t_e_r_a_t_i_o_n () const { │ │ │ │ │ -93 Vector y = _a_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_I_t_e_r_a_t_i_o_n(this->ritzVector_, previousVector_, │ │ │ │ │ -beta_); │ │ │ │ │ -94 return y; │ │ │ │ │ -95 } │ │ │ │ │ -96 │ │ │ │ │ -_1_0_1 double _e_s_t_i_m_a_t_e_B_e_t_a(const size_t T = 10) const { │ │ │ │ │ -102 // set initial estimation of maxBeta │ │ │ │ │ -103 Vector initVector = this->ritzVector_; │ │ │ │ │ -104 const double up = initVector.dot( this->_A__ * initVector ); │ │ │ │ │ -105 const double down = initVector.dot(initVector); │ │ │ │ │ -106 const double mu = up / down; │ │ │ │ │ -107 double maxBeta = mu * mu / 4; │ │ │ │ │ -108 size_t maxIndex; │ │ │ │ │ -109 std::vector betas; │ │ │ │ │ -110 │ │ │ │ │ -111 Matrix R = Matrix::Zero(this->dim_, 10); │ │ │ │ │ -112 // run T times of iteration to find the beta that has the largest Rayleigh │ │ │ │ │ -quotient │ │ │ │ │ -113 for (size_t t = 0; t < T; t++) { │ │ │ │ │ -114 // after each t iteration, reset the betas with the current maxBeta │ │ │ │ │ -115 betas = {2 / 3 * maxBeta, 0.99 * maxBeta, maxBeta, 1.01 * maxBeta, │ │ │ │ │ -116 1.5 * maxBeta}; │ │ │ │ │ -117 // iterate through every beta value │ │ │ │ │ -118 for (size_t k = 0; k < betas.size(); ++k) { │ │ │ │ │ -119 // initialize x0 and x00 in each iteration of each beta │ │ │ │ │ -120 Vector x0 = initVector; │ │ │ │ │ -121 Vector x00 = Vector::Zero(this->dim_); │ │ │ │ │ -122 // run 10 steps of accelerated power iteration with this beta │ │ │ │ │ -123 for (size_t j = 1; j < 10; j++) { │ │ │ │ │ -124 if (j < 2) { │ │ │ │ │ -125 R.col(0) = _a_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_I_t_e_r_a_t_i_o_n(x0, x00, betas[k]); │ │ │ │ │ -126 R.col(1) = _a_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_I_t_e_r_a_t_i_o_n(R.col(0), x0, betas[k]); │ │ │ │ │ -127 } else { │ │ │ │ │ -128 R.col(j) = _a_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_I_t_e_r_a_t_i_o_n(R.col(j - 1), R.col(j - 2), │ │ │ │ │ -129 betas[k]); │ │ │ │ │ -130 } │ │ │ │ │ -131 } │ │ │ │ │ -132 // compute the Rayleigh quotient for the randomly sampled vector after │ │ │ │ │ -133 // 10 steps of power accelerated iteration │ │ │ │ │ -134 const Vector x = R.col(9); │ │ │ │ │ -135 const double up = x.dot(this->_A__ * x); │ │ │ │ │ -136 const double down = x.dot(x); │ │ │ │ │ -137 const double mu = up / down; │ │ │ │ │ -138 // store the momentum with largest Rayleigh quotient and its according │ │ │ │ │ -index of beta_ │ │ │ │ │ -139 if (mu * mu / 4 > maxBeta) { │ │ │ │ │ -140 // save the max beta index │ │ │ │ │ -141 maxIndex = k; │ │ │ │ │ -142 maxBeta = mu * mu / 4; │ │ │ │ │ -143 } │ │ │ │ │ -144 } │ │ │ │ │ -145 } │ │ │ │ │ -146 // set beta_ to momentum with largest Rayleigh quotient │ │ │ │ │ -147 return betas[maxIndex]; │ │ │ │ │ -148 } │ │ │ │ │ -149 │ │ │ │ │ -_1_5_6 bool _c_o_m_p_u_t_e(size_t maxIterations, double tol) { │ │ │ │ │ -157 // Starting │ │ │ │ │ -158 bool isConverged = false; │ │ │ │ │ -159 │ │ │ │ │ -160 for (size_t i = 0; i < maxIterations && !isConverged; i++) { │ │ │ │ │ -161 ++(this->nrIterations_); │ │ │ │ │ -162 Vector tmp = this->ritzVector_; │ │ │ │ │ -163 // update the ritzVector after accelerated power iteration │ │ │ │ │ -164 this->ritzVector_ = _a_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_I_t_e_r_a_t_i_o_n(); │ │ │ │ │ -165 // update the previousVector with ritzVector │ │ │ │ │ -166 previousVector_ = tmp; │ │ │ │ │ -167 // update the ritzValue │ │ │ │ │ -168 this->ritzValue_ = this->ritzVector_.dot(this->_A__ * this->ritzVector_); │ │ │ │ │ -169 isConverged = this->_c_o_n_v_e_r_g_e_d(tol); │ │ │ │ │ -170 } │ │ │ │ │ -171 │ │ │ │ │ -172 return isConverged; │ │ │ │ │ -173 } │ │ │ │ │ -174}; │ │ │ │ │ -175 │ │ │ │ │ -176} // namespace gtsam │ │ │ │ │ -_P_o_w_e_r_M_e_t_h_o_d_._h │ │ │ │ │ -Power method for fast eigenvalue and eigenvector computation. │ │ │ │ │ +71}// gtsam │ │ │ │ │ +_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ +Conditional Gaussian Base class. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_A_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_M_e_t_h_o_d │ │ │ │ │ -Compute maximum Eigenpair with accelerated power method. │ │ │ │ │ -DDeeffiinniittiioonn AcceleratedPowerMethod.h:51 │ │ │ │ │ -_g_t_s_a_m_:_:_A_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_M_e_t_h_o_d_:_:_a_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_I_t_e_r_a_t_i_o_n │ │ │ │ │ -Vector acceleratedPowerIteration(const Vector &x1, const Vector &x0, const │ │ │ │ │ -double beta) const │ │ │ │ │ -Run accelerated power iteration to get ritzVector with beta and previous two │ │ │ │ │ -ritzVector x0 and x00,... │ │ │ │ │ -DDeeffiinniittiioonn AcceleratedPowerMethod.h:80 │ │ │ │ │ -_g_t_s_a_m_:_:_A_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_M_e_t_h_o_d_:_:_A_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_M_e_t_h_o_d │ │ │ │ │ -AcceleratedPowerMethod(const Operator &A, const boost::optional< Vector > │ │ │ │ │ -initial=boost::none, double initialBeta=0.0) │ │ │ │ │ -Constructor from aim matrix A (given as Matrix or Sparse), optional intial │ │ │ │ │ -vector as ritzVector. │ │ │ │ │ -DDeeffiinniittiioonn AcceleratedPowerMethod.h:62 │ │ │ │ │ -_g_t_s_a_m_:_:_A_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_M_e_t_h_o_d_:_:_e_s_t_i_m_a_t_e_B_e_t_a │ │ │ │ │ -double estimateBeta(const size_t T=10) const │ │ │ │ │ -Tuning the momentum beta using the Best Heavy Ball algorithm in Ref(3), T is │ │ │ │ │ -the iteration time to fi... │ │ │ │ │ -DDeeffiinniittiioonn AcceleratedPowerMethod.h:101 │ │ │ │ │ -_g_t_s_a_m_:_:_A_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_M_e_t_h_o_d_:_:_a_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_I_t_e_r_a_t_i_o_n │ │ │ │ │ -Vector acceleratedPowerIteration() const │ │ │ │ │ -Run accelerated power iteration to get ritzVector with beta and previous two │ │ │ │ │ -ritzVector x0 and x00,... │ │ │ │ │ -DDeeffiinniittiioonn AcceleratedPowerMethod.h:92 │ │ │ │ │ -_g_t_s_a_m_:_:_A_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_M_e_t_h_o_d_:_:_c_o_m_p_u_t_e │ │ │ │ │ -bool compute(size_t maxIterations, double tol) │ │ │ │ │ -Start the accelerated iteration, after performing the accelerated iteration, │ │ │ │ │ -calculate the ritz error... │ │ │ │ │ -DDeeffiinniittiioonn AcceleratedPowerMethod.h:156 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_w_e_r_M_e_t_h_o_d │ │ │ │ │ -Compute maximum Eigenpair with power method. │ │ │ │ │ -DDeeffiinniittiioonn PowerMethod.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_w_e_r_M_e_t_h_o_d_:_:_A__ │ │ │ │ │ -const Operator & A_ │ │ │ │ │ -Const reference to an externally-held matrix whose minimum-eigenvalue we want │ │ │ │ │ -to compute. │ │ │ │ │ -DDeeffiinniittiioonn PowerMethod.h:63 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_w_e_r_M_e_t_h_o_d_:_:_c_o_n_v_e_r_g_e_d │ │ │ │ │ -bool converged(double tol) const │ │ │ │ │ -After Perform power iteration on a single Ritz value, check if the Ritz │ │ │ │ │ -residual for the current Ritz... │ │ │ │ │ -DDeeffiinniittiioonn PowerMethod.h:112 │ │ │ │ │ +_g_t_s_a_m_:_:_m_e_a_n │ │ │ │ │ +Point3 mean(const CONTAINER &points) │ │ │ │ │ +mean │ │ │ │ │ +DDeeffiinniittiioonn Point3.h:68 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_:_:_n_r_P_a_r_e_n_t_s │ │ │ │ │ +size_t nrParents() const │ │ │ │ │ +return the number of parents │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:113 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +A GaussianConditional functions as the node in a Bayes network. │ │ │ │ │ +DDeeffiinniittiioonn GaussianConditional.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_D_e_n_s_i_t_y │ │ │ │ │ +A GaussianDensity is a GaussianConditional without parents. │ │ │ │ │ +DDeeffiinniittiioonn GaussianDensity.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_D_e_n_s_i_t_y_:_:_G_a_u_s_s_i_a_n_D_e_n_s_i_t_y │ │ │ │ │ +GaussianDensity(Key key, const Vector &d, const Matrix &R, const SharedDiagonal │ │ │ │ │ +&noiseModel=SharedDiagonal()) │ │ │ │ │ +constructor using d, R │ │ │ │ │ +DDeeffiinniittiioonn GaussianDensity.h:51 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_D_e_n_s_i_t_y_:_:_G_a_u_s_s_i_a_n_D_e_n_s_i_t_y │ │ │ │ │ +GaussianDensity() │ │ │ │ │ +default constructor needed for serialization │ │ │ │ │ +DDeeffiinniittiioonn GaussianDensity.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_D_e_n_s_i_t_y_:_:_G_a_u_s_s_i_a_n_D_e_n_s_i_t_y │ │ │ │ │ +GaussianDensity(const GaussianConditional &conditional) │ │ │ │ │ +Copy constructor from GaussianConditional. │ │ │ │ │ +DDeeffiinniittiioonn GaussianDensity.h:44 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _A_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_M_e_t_h_o_d_._h │ │ │ │ │ + * _G_a_u_s_s_i_a_n_D_e_n_s_i_t_y_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00815.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/ConjugateGradientSolver.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,54 +95,46 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
ConjugateGradientSolver.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
GaussianFactor.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Implementation of Conjugate Gradient solver for a linear system. │ │ │ │ +

A factor with a quadratic error function - a Gaussian. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::ConjugateGradientParameters
 parameters for the conjugate gradient method More...
class  gtsam::GaussianFactor
 An abstract virtual base class for JacobianFactor and HessianFactor. More...
 
struct  gtsam::traits< GaussianFactor >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -template<class S , class V >
V gtsam::preconditionedConjugateGradient (const S &system, const V &initial, const ConjugateGradientParameters &parameters)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Implementation of Conjugate Gradient solver for a linear system.

│ │ │ │ -
Author
Yong-Dian Jian
│ │ │ │ -
│ │ │ │ -Sungtae An
│ │ │ │ -
Date
Nov 6, 2014
│ │ │ │ +

A factor with a quadratic error function - a Gaussian.

│ │ │ │ +

GaussianFactor

Author
Richard Roberts, Christian Potthast
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,33 +1,30 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -ConjugateGradientSolver.h File Reference │ │ │ │ │ -Implementation of Conjugate Gradient solver for a linear system. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +GaussianFactor.h File Reference │ │ │ │ │ +A factor with a quadratic error function - a Gaussian. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ -  parameters for the conjugate gradient method _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +  An abstract virtual base class for _J_a_c_o_b_i_a_n_F_a_c_t_o_r and _H_e_s_s_i_a_n_F_a_c_t_o_r. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ -V  ggttssaamm::::pprreeccoonnddiittiioonneeddCCoonnjjuuggaatteeGGrraaddiieenntt (const S &system, const V &initial, │ │ │ │ │ - const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s ¶meters) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Implementation of Conjugate Gradient solver for a linear system. │ │ │ │ │ +A factor with a quadratic error function - a Gaussian. │ │ │ │ │ +GaussianFactor │ │ │ │ │ Author │ │ │ │ │ - Yong-Dian Jian │ │ │ │ │ - Sungtae An │ │ │ │ │ - Date │ │ │ │ │ - Nov 6, 2014 │ │ │ │ │ + Richard Roberts, Christian Potthast │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_S_o_l_v_e_r_._h │ │ │ │ │ + * _G_a_u_s_s_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00815.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,4 @@ │ │ │ │ │ var a00815 = [ │ │ │ │ │ - ["gtsam::ConjugateGradientParameters", "a03728.html", "a03728"] │ │ │ │ │ + ["gtsam::GaussianFactor", "a03780.html", "a03780"], │ │ │ │ │ + ["gtsam::traits< GaussianFactor >", "a03784.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00815_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/ConjugateGradientSolver.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,189 +98,170 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ConjugateGradientSolver.h
│ │ │ │ +
GaussianFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/linear/IterativeSolver.h>
│ │ │ │ -
23
│ │ │ │ -
24namespace gtsam {
│ │ │ │ -
25
│ │ │ │ -
│ │ │ │ -
29class GTSAM_EXPORT ConjugateGradientParameters : public IterativeOptimizationParameters {
│ │ │ │ -
30
│ │ │ │ -
31public:
│ │ │ │ -
32 typedef IterativeOptimizationParameters Base;
│ │ │ │ -
33 typedef boost::shared_ptr<ConjugateGradientParameters> shared_ptr;
│ │ │ │ -
34
│ │ │ │ -
35 size_t minIterations_;
│ │ │ │ -
36 size_t maxIterations_;
│ │ │ │ -
37 size_t reset_;
│ │ │ │ -
38 double epsilon_rel_;
│ │ │ │ -
39 double epsilon_abs_;
│ │ │ │ -
40
│ │ │ │ -
41 /* Matrix Operation Kernel */
│ │ │ │ -
│ │ │ │ -
42 enum BLASKernel {
│ │ │ │ -
43 GTSAM = 0,
│ │ │ │ -
44 } blas_kernel_ ;
│ │ │ │ -
│ │ │ │ -
45
│ │ │ │ -
46 ConjugateGradientParameters()
│ │ │ │ -
47 : minIterations_(1), maxIterations_(500), reset_(501), epsilon_rel_(1e-3),
│ │ │ │ -
48 epsilon_abs_(1e-3), blas_kernel_(GTSAM) {}
│ │ │ │ -
49
│ │ │ │ -
50 ConjugateGradientParameters(size_t minIterations, size_t maxIterations, size_t reset,
│ │ │ │ -
51 double epsilon_rel, double epsilon_abs, BLASKernel blas)
│ │ │ │ -
52 : minIterations_(minIterations), maxIterations_(maxIterations), reset_(reset),
│ │ │ │ -
53 epsilon_rel_(epsilon_rel), epsilon_abs_(epsilon_abs), blas_kernel_(blas) {}
│ │ │ │ -
54
│ │ │ │ -
55 ConjugateGradientParameters(const ConjugateGradientParameters &p)
│ │ │ │ -
56 : Base(p), minIterations_(p.minIterations_), maxIterations_(p.maxIterations_), reset_(p.reset_),
│ │ │ │ -
57 epsilon_rel_(p.epsilon_rel_), epsilon_abs_(p.epsilon_abs_), blas_kernel_(GTSAM) {}
│ │ │ │ -
58
│ │ │ │ -
59 /* general interface */
│ │ │ │ -
60 inline size_t minIterations() const { return minIterations_; }
│ │ │ │ -
61 inline size_t maxIterations() const { return maxIterations_; }
│ │ │ │ -
62 inline size_t reset() const { return reset_; }
│ │ │ │ -
63 inline double epsilon() const { return epsilon_rel_; }
│ │ │ │ -
64 inline double epsilon_rel() const { return epsilon_rel_; }
│ │ │ │ -
65 inline double epsilon_abs() const { return epsilon_abs_; }
│ │ │ │ -
66
│ │ │ │ -
67 inline size_t getMinIterations() const { return minIterations_; }
│ │ │ │ -
68 inline size_t getMaxIterations() const { return maxIterations_; }
│ │ │ │ -
69 inline size_t getReset() const { return reset_; }
│ │ │ │ -
70 inline double getEpsilon() const { return epsilon_rel_; }
│ │ │ │ -
71 inline double getEpsilon_rel() const { return epsilon_rel_; }
│ │ │ │ -
72 inline double getEpsilon_abs() const { return epsilon_abs_; }
│ │ │ │ -
73
│ │ │ │ -
74 inline void setMinIterations(size_t value) { minIterations_ = value; }
│ │ │ │ -
75 inline void setMaxIterations(size_t value) { maxIterations_ = value; }
│ │ │ │ -
76 inline void setReset(size_t value) { reset_ = value; }
│ │ │ │ -
77 inline void setEpsilon(double value) { epsilon_rel_ = value; }
│ │ │ │ -
78 inline void setEpsilon_rel(double value) { epsilon_rel_ = value; }
│ │ │ │ -
79 inline void setEpsilon_abs(double value) { epsilon_abs_ = value; }
│ │ │ │ +
19// \callgraph
│ │ │ │ +
20
│ │ │ │ +
21#pragma once
│ │ │ │ +
22
│ │ │ │ +
23#include <gtsam/inference/Factor.h>
│ │ │ │ +
24#include <gtsam/base/Matrix.h>
│ │ │ │ +
25#include <gtsam/base/Testable.h>
│ │ │ │ +
26
│ │ │ │ +
27namespace gtsam {
│ │ │ │ +
28
│ │ │ │ +
29 // Forward declarations
│ │ │ │ +
30 class VectorValues;
│ │ │ │ +
31 class Scatter;
│ │ │ │ +
32 class SymmetricBlockMatrix;
│ │ │ │ +
33
│ │ │ │ +
│ │ │ │ +
38 class GTSAM_EXPORT GaussianFactor : public Factor
│ │ │ │ +
39 {
│ │ │ │ +
40 public:
│ │ │ │ +
41 typedef GaussianFactor This;
│ │ │ │ +
42 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
43 typedef Factor Base;
│ │ │ │ +
44
│ │ │ │ +
46 GaussianFactor() {}
│ │ │ │ +
47
│ │ │ │ +
50 template<typename CONTAINER>
│ │ │ │ +
51 GaussianFactor(const CONTAINER& keys) : Base(keys) {}
│ │ │ │ +
52
│ │ │ │ +
54 virtual ~GaussianFactor() {}
│ │ │ │ +
55
│ │ │ │ +
56 // Implementing Testable interface
│ │ │ │ +
57
│ │ │ │ +
59 void print(
│ │ │ │ +
60 const std::string& s = "",
│ │ │ │ +
61 const KeyFormatter& formatter = DefaultKeyFormatter) const override = 0;
│ │ │ │ +
62
│ │ │ │ +
64 virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const = 0;
│ │ │ │ +
65
│ │ │ │ +
73 virtual double error(const VectorValues& c) const;
│ │ │ │ +
74
│ │ │ │ +
79 double error(const HybridValues& c) const override;
│ │ │ │
80
│ │ │ │ -
81
│ │ │ │ -
82 void print() const { Base::print(); }
│ │ │ │ -
83 void print(std::ostream &os) const override;
│ │ │ │ -
84
│ │ │ │ -
85 static std::string blasTranslator(const BLASKernel k) ;
│ │ │ │ -
86 static BLASKernel blasTranslator(const std::string &s) ;
│ │ │ │ -
87};
│ │ │ │ -
│ │ │ │ -
88
│ │ │ │ -
89/*
│ │ │ │ -
90 * A template for the linear preconditioned conjugate gradient method.
│ │ │ │ -
91 * System class should support residual(v, g), multiply(v,Av), scal(alpha,v), dot(v,v), axpy(alpha,x,y)
│ │ │ │ -
92 * leftPrecondition(v, L^{-1}v, rightPrecondition(v, L^{-T}v) where preconditioner M = L*L^T
│ │ │ │ -
93 * Note that the residual is in the preconditioned domain. Refer to Section 9.2 of Saad's book.
│ │ │ │ -
94 *
│ │ │ │ -
95 ** REFERENCES:
│ │ │ │ -
96 * [1] Y. Saad, "Preconditioned Iterations," in Iterative Methods for Sparse Linear Systems,
│ │ │ │ -
97 * 2nd ed. SIAM, 2003, ch. 9, sec. 2, pp.276-281.
│ │ │ │ -
98 */
│ │ │ │ -
99template<class S, class V>
│ │ │ │ -
100V preconditionedConjugateGradient(const S &system, const V &initial,
│ │ │ │ -
101 const ConjugateGradientParameters &parameters) {
│ │ │ │ -
102
│ │ │ │ -
103 V estimate, residual, direction, q1, q2;
│ │ │ │ -
104 estimate = residual = direction = q1 = q2 = initial;
│ │ │ │ -
105
│ │ │ │ -
106 system.residual(estimate, q1); /* q1 = b-Ax */
│ │ │ │ -
107 system.leftPrecondition(q1, residual); /* r = L^{-1} (b-Ax) */
│ │ │ │ -
108 system.rightPrecondition(residual, direction);/* p = L^{-T} r */
│ │ │ │ -
109
│ │ │ │ -
110 double currentGamma = system.dot(residual, residual), prevGamma, alpha, beta;
│ │ │ │ +
82 virtual DenseIndex getDim(const_iterator variable) const = 0;
│ │ │ │ +
83
│ │ │ │ +
91 virtual Matrix augmentedJacobian() const = 0;
│ │ │ │ +
92
│ │ │ │ +
100 virtual std::pair<Matrix,Vector> jacobian() const = 0;
│ │ │ │ +
101
│ │ │ │ +
110 virtual Matrix augmentedInformation() const = 0;
│ │ │ │
111
│ │ │ │ -
112 const size_t iMaxIterations = parameters.maxIterations(),
│ │ │ │ -
113 iMinIterations = parameters.minIterations(),
│ │ │ │ -
114 iReset = parameters.reset() ;
│ │ │ │ -
115 const double threshold = std::max(parameters.epsilon_abs(),
│ │ │ │ -
116 parameters.epsilon() * parameters.epsilon() * currentGamma);
│ │ │ │ -
117
│ │ │ │ -
118 if (parameters.verbosity() >= ConjugateGradientParameters::COMPLEXITY )
│ │ │ │ -
119 std::cout << "[PCG] epsilon = " << parameters.epsilon()
│ │ │ │ -
120 << ", max = " << parameters.maxIterations()
│ │ │ │ -
121 << ", reset = " << parameters.reset()
│ │ │ │ -
122 << ", ||r0||^2 = " << currentGamma
│ │ │ │ -
123 << ", threshold = " << threshold << std::endl;
│ │ │ │ -
124
│ │ │ │ -
125 size_t k;
│ │ │ │ -
126 for ( k = 1 ; k <= iMaxIterations && (currentGamma > threshold || k <= iMinIterations) ; k++ ) {
│ │ │ │ -
127
│ │ │ │ -
128 if ( k % iReset == 0 ) {
│ │ │ │ -
129 system.residual(estimate, q1); /* q1 = b-Ax */
│ │ │ │ -
130 system.leftPrecondition(q1, residual); /* r = L^{-1} (b-Ax) */
│ │ │ │ -
131 system.rightPrecondition(residual, direction); /* p = L^{-T} r */
│ │ │ │ -
132 currentGamma = system.dot(residual, residual);
│ │ │ │ -
133 }
│ │ │ │ -
134 system.multiply(direction, q1); /* q1 = A p */
│ │ │ │ -
135 alpha = currentGamma / system.dot(direction, q1); /* alpha = gamma / (p' A p) */
│ │ │ │ -
136 system.axpy(alpha, direction, estimate); /* estimate += alpha * p */
│ │ │ │ -
137 system.leftPrecondition(q1, q2); /* q2 = L^{-1} * q1 */
│ │ │ │ -
138 system.axpy(-alpha, q2, residual); /* r -= alpha * q2 */
│ │ │ │ -
139 prevGamma = currentGamma;
│ │ │ │ -
140 currentGamma = system.dot(residual, residual); /* gamma = |r|^2 */
│ │ │ │ -
141 beta = currentGamma / prevGamma;
│ │ │ │ -
142 system.rightPrecondition(residual, q1); /* q1 = L^{-T} r */
│ │ │ │ -
143 system.scal(beta, direction);
│ │ │ │ -
144 system.axpy(1.0, q1, direction); /* p = q1 + beta * p */
│ │ │ │ -
145
│ │ │ │ -
146 if (parameters.verbosity() >= ConjugateGradientParameters::ERROR )
│ │ │ │ -
147 std::cout << "[PCG] k = " << k
│ │ │ │ -
148 << ", alpha = " << alpha
│ │ │ │ -
149 << ", beta = " << beta
│ │ │ │ -
150 << ", ||r||^2 = " << currentGamma
│ │ │ │ -
151// << "\nx =\n" << estimate
│ │ │ │ -
152// << "\nr =\n" << residual
│ │ │ │ -
153 << std::endl;
│ │ │ │ -
154 }
│ │ │ │ -
155 if (parameters.verbosity() >= ConjugateGradientParameters::COMPLEXITY )
│ │ │ │ -
156 std::cout << "[PCG] iterations = " << k
│ │ │ │ -
157 << ", ||r||^2 = " << currentGamma
│ │ │ │ -
158 << std::endl;
│ │ │ │ -
159
│ │ │ │ -
160 return estimate;
│ │ │ │ -
161}
│ │ │ │ -
162
│ │ │ │ -
163
│ │ │ │ -
164}
│ │ │ │ -
IterativeSolver.h
Some support classes for iterative solvers.
│ │ │ │ +
115 virtual Matrix information() const = 0;
│ │ │ │ +
116
│ │ │ │ +
118 VectorValues hessianDiagonal() const;
│ │ │ │ +
119
│ │ │ │ +
121 virtual void hessianDiagonalAdd(VectorValues& d) const = 0;
│ │ │ │ +
122
│ │ │ │ +
124 virtual void hessianDiagonal(double* d) const = 0;
│ │ │ │ +
125
│ │ │ │ +
127 virtual std::map<Key,Matrix> hessianBlockDiagonal() const = 0;
│ │ │ │ +
128
│ │ │ │ +
130 virtual GaussianFactor::shared_ptr clone() const = 0;
│ │ │ │ +
131
│ │ │ │ +
137 virtual GaussianFactor::shared_ptr negate() const = 0;
│ │ │ │ +
138
│ │ │ │ +
144 virtual void updateHessian(const KeyVector& keys,
│ │ │ │ +
145 SymmetricBlockMatrix* info) const = 0;
│ │ │ │ +
146
│ │ │ │ +
148 virtual void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const = 0;
│ │ │ │ +
149
│ │ │ │ +
151 virtual VectorValues gradientAtZero() const = 0;
│ │ │ │ +
152
│ │ │ │ +
154 virtual void gradientAtZero(double* d) const = 0;
│ │ │ │ +
155
│ │ │ │ +
157 virtual Vector gradient(Key key, const VectorValues& x) const = 0;
│ │ │ │ +
158
│ │ │ │ +
159 // Determine position of a given key
│ │ │ │ +
160 template <typename CONTAINER>
│ │ │ │ +
161 static DenseIndex Slot(const CONTAINER& keys, Key key) {
│ │ │ │ +
162 return std::find(keys.begin(), keys.end(), key) - keys.begin();
│ │ │ │ +
163 }
│ │ │ │ +
164
│ │ │ │ +
165 private:
│ │ │ │ +
167 friend class boost::serialization::access;
│ │ │ │ +
168 template<class ARCHIVE>
│ │ │ │ +
169 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
170 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
171 }
│ │ │ │ +
172
│ │ │ │ +
173 }; // GaussianFactor
│ │ │ │ +
│ │ │ │ +
174
│ │ │ │ +
176template<>
│ │ │ │ +
│ │ │ │ +
177struct traits<GaussianFactor> : public Testable<GaussianFactor> {
│ │ │ │ +
178};
│ │ │ │ +
│ │ │ │ +
179
│ │ │ │ +
180} // \ namespace gtsam
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
Factor.h
The base class for all factors.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::ConjugateGradientParameters
parameters for the conjugate gradient method
Definition ConjugateGradientSolver.h:29
│ │ │ │ -
gtsam::ConjugateGradientParameters::minIterations_
size_t minIterations_
minimum number of cg iterations
Definition ConjugateGradientSolver.h:35
│ │ │ │ -
gtsam::ConjugateGradientParameters::reset_
size_t reset_
number of iterations before reset
Definition ConjugateGradientSolver.h:37
│ │ │ │ -
gtsam::ConjugateGradientParameters::BLASKernel
BLASKernel
Definition ConjugateGradientSolver.h:42
│ │ │ │ -
gtsam::ConjugateGradientParameters::epsilon_rel_
double epsilon_rel_
threshold for relative error decrease
Definition ConjugateGradientSolver.h:38
│ │ │ │ -
gtsam::ConjugateGradientParameters::maxIterations_
size_t maxIterations_
maximum number of cg iterations
Definition ConjugateGradientSolver.h:36
│ │ │ │ -
gtsam::ConjugateGradientParameters::epsilon_abs_
double epsilon_abs_
threshold for absolute error decrease
Definition ConjugateGradientSolver.h:39
│ │ │ │ -
gtsam::IterativeOptimizationParameters
parameters for iterative linear solvers
Definition IterativeSolver.h:44
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ +
gtsam::Factor
Definition Factor.h:68
│ │ │ │ +
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition Factor.h:80
│ │ │ │ +
gtsam::GaussianFactor
An abstract virtual base class for JacobianFactor and HessianFactor.
Definition GaussianFactor.h:39
│ │ │ │ +
gtsam::GaussianFactor::getDim
virtual DenseIndex getDim(const_iterator variable) const =0
Return the dimension of the variable pointed to by the given key iterator.
│ │ │ │ +
gtsam::GaussianFactor::hessianBlockDiagonal
virtual std::map< Key, Matrix > hessianBlockDiagonal() const =0
Return the block diagonal of the Hessian for this factor.
│ │ │ │ +
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactor.h:42
│ │ │ │ +
gtsam::GaussianFactor::jacobian
virtual std::pair< Matrix, Vector > jacobian() const =0
Return the dense Jacobian and right-hand-side , with the noise models baked into A and b.
│ │ │ │ +
gtsam::GaussianFactor::gradient
virtual Vector gradient(Key key, const VectorValues &x) const =0
Gradient wrt a key at any values.
│ │ │ │ +
gtsam::GaussianFactor::clone
virtual GaussianFactor::shared_ptr clone() const =0
Clone a factor (make a deep copy)
│ │ │ │ +
gtsam::GaussianFactor::information
virtual Matrix information() const =0
Return the non-augmented information matrix represented by this GaussianFactor.
│ │ │ │ +
gtsam::GaussianFactor::Base
Factor Base
Our base class.
Definition GaussianFactor.h:43
│ │ │ │ +
gtsam::GaussianFactor::~GaussianFactor
virtual ~GaussianFactor()
Destructor.
Definition GaussianFactor.h:54
│ │ │ │ +
gtsam::GaussianFactor::gradientAtZero
virtual VectorValues gradientAtZero() const =0
A'*b for Jacobian, eta for Hessian.
│ │ │ │ +
gtsam::GaussianFactor::GaussianFactor
GaussianFactor(const CONTAINER &keys)
Construct from container of keys.
Definition GaussianFactor.h:51
│ │ │ │ +
gtsam::GaussianFactor::multiplyHessianAdd
virtual void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const =0
y += alpha * A'*A*x
│ │ │ │ +
gtsam::GaussianFactor::equals
virtual bool equals(const GaussianFactor &lf, double tol=1e-9) const =0
Equals for testable.
│ │ │ │ +
gtsam::GaussianFactor::augmentedInformation
virtual Matrix augmentedInformation() const =0
Return the augmented information matrix represented by this GaussianFactor.
│ │ │ │ +
gtsam::GaussianFactor::augmentedJacobian
virtual Matrix augmentedJacobian() const =0
Return a dense Jacobian matrix, augmented with b with the noise models baked into A and b.
│ │ │ │ +
gtsam::GaussianFactor::GaussianFactor
GaussianFactor()
Default constructor creates empty factor.
Definition GaussianFactor.h:46
│ │ │ │ +
gtsam::GaussianFactor::negate
virtual GaussianFactor::shared_ptr negate() const =0
Construct the corresponding anti-factor to negate information stored stored in this factor.
│ │ │ │ +
gtsam::GaussianFactor::hessianDiagonal
virtual void hessianDiagonal(double *d) const =0
Raw memory access version of hessianDiagonal.
│ │ │ │ +
gtsam::GaussianFactor::This
GaussianFactor This
This class.
Definition GaussianFactor.h:41
│ │ │ │ +
gtsam::GaussianFactor::updateHessian
virtual void updateHessian(const KeyVector &keys, SymmetricBlockMatrix *info) const =0
Update an information matrix by adding the information corresponding to this factor (used internally ...
│ │ │ │ +
gtsam::GaussianFactor::print
void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override=0
print
│ │ │ │ +
gtsam::GaussianFactor::hessianDiagonalAdd
virtual void hessianDiagonalAdd(VectorValues &d) const =0
Add the current diagonal to a VectorValues instance.
│ │ │ │ +
gtsam::GaussianFactor::gradientAtZero
virtual void gradientAtZero(double *d) const =0
Raw memory access version of gradientAtZero.
│ │ │ │ +
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
VectorValues
The Factor::error simply extracts the.
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,216 +1,255 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ConjugateGradientSolver.h │ │ │ │ │ +GaussianFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_I_t_e_r_a_t_i_v_e_S_o_l_v_e_r_._h> │ │ │ │ │ -23 │ │ │ │ │ -24namespace _g_t_s_a_m { │ │ │ │ │ -25 │ │ │ │ │ -_2_9class GTSAM_EXPORT _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s : public │ │ │ │ │ -_I_t_e_r_a_t_i_v_e_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s { │ │ │ │ │ -30 │ │ │ │ │ -31public: │ │ │ │ │ -32 typedef _I_t_e_r_a_t_i_v_e_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s _B_a_s_e; │ │ │ │ │ -33 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -34 │ │ │ │ │ -_3_5 size_t _m_i_n_I_t_e_r_a_t_i_o_n_s__; │ │ │ │ │ -_3_6 size_t _m_a_x_I_t_e_r_a_t_i_o_n_s__; │ │ │ │ │ -_3_7 size_t _r_e_s_e_t__; │ │ │ │ │ -_3_8 double _e_p_s_i_l_o_n___r_e_l__; │ │ │ │ │ -_3_9 double _e_p_s_i_l_o_n___a_b_s__; │ │ │ │ │ -40 │ │ │ │ │ -41 /* Matrix Operation Kernel */ │ │ │ │ │ -_4_2 enum _B_L_A_S_K_e_r_n_e_l { │ │ │ │ │ -_4_3 GTSAM = 0, │ │ │ │ │ -44 } blas_kernel_ ; │ │ │ │ │ -45 │ │ │ │ │ -46 _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s() │ │ │ │ │ -47 : minIterations_(1), maxIterations_(500), reset_(501), epsilon_rel_(1e-3), │ │ │ │ │ -48 epsilon_abs_(1e-3), blas_kernel_(GTSAM) {} │ │ │ │ │ -49 │ │ │ │ │ -50 ConjugateGradientParameters(size_t minIterations, size_t maxIterations, │ │ │ │ │ -size_t reset, │ │ │ │ │ -51 double epsilon_rel, double epsilon_abs, BLASKernel blas) │ │ │ │ │ -52 : minIterations_(minIterations), maxIterations_(maxIterations), reset_ │ │ │ │ │ -(reset), │ │ │ │ │ -53 epsilon_rel_(epsilon_rel), epsilon_abs_(epsilon_abs), blas_kernel_(blas) {} │ │ │ │ │ -54 │ │ │ │ │ -55 ConjugateGradientParameters(const ConjugateGradientParameters &p) │ │ │ │ │ -56 : Base(p), minIterations_(p.minIterations_), maxIterations_ │ │ │ │ │ -(p.maxIterations_), reset_(p.reset_), │ │ │ │ │ -57 epsilon_rel_(p.epsilon_rel_), epsilon_abs_(p.epsilon_abs_), blas_kernel_ │ │ │ │ │ -(GTSAM) {} │ │ │ │ │ -58 │ │ │ │ │ -59 /* general interface */ │ │ │ │ │ -60 inline size_t minIterations() const { return minIterations_; } │ │ │ │ │ -61 inline size_t maxIterations() const { return maxIterations_; } │ │ │ │ │ -62 inline size_t reset() const { return reset_; } │ │ │ │ │ -63 inline double epsilon() const { return epsilon_rel_; } │ │ │ │ │ -64 inline double epsilon_rel() const { return epsilon_rel_; } │ │ │ │ │ -65 inline double epsilon_abs() const { return epsilon_abs_; } │ │ │ │ │ -66 │ │ │ │ │ -67 inline size_t getMinIterations() const { return minIterations_; } │ │ │ │ │ -68 inline size_t getMaxIterations() const { return maxIterations_; } │ │ │ │ │ -69 inline size_t getReset() const { return reset_; } │ │ │ │ │ -70 inline double getEpsilon() const { return epsilon_rel_; } │ │ │ │ │ -71 inline double getEpsilon_rel() const { return epsilon_rel_; } │ │ │ │ │ -72 inline double getEpsilon_abs() const { return epsilon_abs_; } │ │ │ │ │ -73 │ │ │ │ │ -74 inline void setMinIterations(size_t value) { minIterations_ = value; } │ │ │ │ │ -75 inline void setMaxIterations(size_t value) { maxIterations_ = value; } │ │ │ │ │ -76 inline void setReset(size_t value) { reset_ = value; } │ │ │ │ │ -77 inline void setEpsilon(double value) { epsilon_rel_ = value; } │ │ │ │ │ -78 inline void setEpsilon_rel(double value) { epsilon_rel_ = value; } │ │ │ │ │ -79 inline void setEpsilon_abs(double value) { epsilon_abs_ = value; } │ │ │ │ │ +19// \callgraph │ │ │ │ │ +20 │ │ │ │ │ +21#pragma once │ │ │ │ │ +22 │ │ │ │ │ +23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +26 │ │ │ │ │ +27namespace _g_t_s_a_m { │ │ │ │ │ +28 │ │ │ │ │ +29 // Forward declarations │ │ │ │ │ +30 class _V_e_c_t_o_r_V_a_l_u_e_s; │ │ │ │ │ +31 class Scatter; │ │ │ │ │ +32 class SymmetricBlockMatrix; │ │ │ │ │ +33 │ │ │ │ │ +_3_8 class GTSAM_EXPORT _G_a_u_s_s_i_a_n_F_a_c_t_o_r : public _F_a_c_t_o_r │ │ │ │ │ +39 { │ │ │ │ │ +40 public: │ │ │ │ │ +_4_1 typedef _G_a_u_s_s_i_a_n_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ +_4_2 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +_4_3 typedef _F_a_c_t_o_r _B_a_s_e; │ │ │ │ │ +44 │ │ │ │ │ +_4_6 _G_a_u_s_s_i_a_n_F_a_c_t_o_r() {} │ │ │ │ │ +47 │ │ │ │ │ +50 template │ │ │ │ │ +_5_1 _G_a_u_s_s_i_a_n_F_a_c_t_o_r(const CONTAINER& keys) : _B_a_s_e(keys) {} │ │ │ │ │ +52 │ │ │ │ │ +_5_4 virtual _~_G_a_u_s_s_i_a_n_F_a_c_t_o_r() {} │ │ │ │ │ +55 │ │ │ │ │ +56 // Implementing Testable interface │ │ │ │ │ +57 │ │ │ │ │ +_5_9 void _p_r_i_n_t( │ │ │ │ │ +60 const std::string& s = "", │ │ │ │ │ +61 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override = 0; │ │ │ │ │ +62 │ │ │ │ │ +_6_4 virtual bool _e_q_u_a_l_s(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r& lf, double tol = 1e-9) const = 0; │ │ │ │ │ +65 │ │ │ │ │ +73 virtual double error(const _V_e_c_t_o_r_V_a_l_u_e_s& c) const; │ │ │ │ │ +74 │ │ │ │ │ +79 double error(const _H_y_b_r_i_d_V_a_l_u_e_s& c) const override; │ │ │ │ │ 80 │ │ │ │ │ -81 │ │ │ │ │ -82 void _p_r_i_n_t() const { Base::print(); } │ │ │ │ │ -83 void _p_r_i_n_t(std::ostream &os) const override; │ │ │ │ │ -84 │ │ │ │ │ -85 static std::string blasTranslator(const BLASKernel k) ; │ │ │ │ │ -86 static BLASKernel blasTranslator(const std::string &s) ; │ │ │ │ │ -87}; │ │ │ │ │ -88 │ │ │ │ │ -89/* │ │ │ │ │ -90 * A template for the linear preconditioned conjugate gradient method. │ │ │ │ │ -91 * System class should support residual(v, g), multiply(v,Av), scal(alpha,v), │ │ │ │ │ -dot(v,v), axpy(alpha,x,y) │ │ │ │ │ -92 * leftPrecondition(v, L^{-1}v, rightPrecondition(v, L^{-T}v) where │ │ │ │ │ -preconditioner M = L*L^T │ │ │ │ │ -93 * Note that the residual is in the preconditioned domain. Refer to Section │ │ │ │ │ -9.2 of Saad's book. │ │ │ │ │ -94 * │ │ │ │ │ -95 ** REFERENCES: │ │ │ │ │ -96 * [1] Y. Saad, "Preconditioned Iterations," in Iterative Methods for Sparse │ │ │ │ │ -Linear Systems, │ │ │ │ │ -97 * 2nd ed. SIAM, 2003, ch. 9, sec. 2, pp.276-281. │ │ │ │ │ -98 */ │ │ │ │ │ -99template │ │ │ │ │ -100V preconditionedConjugateGradient(const S &system, const V &initial, │ │ │ │ │ -101 const ConjugateGradientParameters ¶meters) { │ │ │ │ │ -102 │ │ │ │ │ -103 V estimate, residual, direction, q1, q2; │ │ │ │ │ -104 estimate = residual = direction = q1 = q2 = initial; │ │ │ │ │ -105 │ │ │ │ │ -106 system.residual(estimate, q1); /* q1 = b-Ax */ │ │ │ │ │ -107 system.leftPrecondition(q1, residual); /* r = L^{-1} (b-Ax) */ │ │ │ │ │ -108 system.rightPrecondition(residual, direction);/* p = L^{-T} r */ │ │ │ │ │ -109 │ │ │ │ │ -110 double currentGamma = system.dot(residual, residual), prevGamma, alpha, │ │ │ │ │ -beta; │ │ │ │ │ +_8_2 virtual _D_e_n_s_e_I_n_d_e_x _g_e_t_D_i_m(_c_o_n_s_t___i_t_e_r_a_t_o_r variable) const = 0; │ │ │ │ │ +83 │ │ │ │ │ +_9_1 virtual Matrix _a_u_g_m_e_n_t_e_d_J_a_c_o_b_i_a_n() const = 0; │ │ │ │ │ +92 │ │ │ │ │ +_1_0_0 virtual std::pair _j_a_c_o_b_i_a_n() const = 0; │ │ │ │ │ +101 │ │ │ │ │ +_1_1_0 virtual Matrix _a_u_g_m_e_n_t_e_d_I_n_f_o_r_m_a_t_i_o_n() const = 0; │ │ │ │ │ 111 │ │ │ │ │ -112 const size_t iMaxIterations = parameters.maxIterations(), │ │ │ │ │ -113 iMinIterations = parameters.minIterations(), │ │ │ │ │ -114 iReset = parameters.reset() ; │ │ │ │ │ -115 const double threshold = std::max(parameters.epsilon_abs(), │ │ │ │ │ -116 parameters.epsilon() * parameters.epsilon() * currentGamma); │ │ │ │ │ -117 │ │ │ │ │ -118 if (parameters.verbosity() >= ConjugateGradientParameters::COMPLEXITY ) │ │ │ │ │ -119 std::cout << "[PCG] epsilon = " << parameters.epsilon() │ │ │ │ │ -120 << ", max = " << parameters.maxIterations() │ │ │ │ │ -121 << ", reset = " << parameters.reset() │ │ │ │ │ -122 << ", ||r0||^2 = " << currentGamma │ │ │ │ │ -123 << ", threshold = " << threshold << std::endl; │ │ │ │ │ -124 │ │ │ │ │ -125 size_t k; │ │ │ │ │ -126 for ( k = 1 ; k <= iMaxIterations && (currentGamma > threshold || k <= │ │ │ │ │ -iMinIterations) ; k++ ) { │ │ │ │ │ -127 │ │ │ │ │ -128 if ( k % iReset == 0 ) { │ │ │ │ │ -129 system.residual(estimate, q1); /* q1 = b-Ax */ │ │ │ │ │ -130 system.leftPrecondition(q1, residual); /* r = L^{-1} (b-Ax) */ │ │ │ │ │ -131 system.rightPrecondition(residual, direction); /* p = L^{-T} r */ │ │ │ │ │ -132 currentGamma = system.dot(residual, residual); │ │ │ │ │ -133 } │ │ │ │ │ -134 system.multiply(direction, q1); /* q1 = A p */ │ │ │ │ │ -135 alpha = currentGamma / system.dot(direction, q1); /* alpha = gamma / (p' A │ │ │ │ │ -p) */ │ │ │ │ │ -136 system.axpy(alpha, direction, estimate); /* estimate += alpha * p */ │ │ │ │ │ -137 system.leftPrecondition(q1, q2); /* q2 = L^{-1} * q1 */ │ │ │ │ │ -138 system.axpy(-alpha, q2, residual); /* r -= alpha * q2 */ │ │ │ │ │ -139 prevGamma = currentGamma; │ │ │ │ │ -140 currentGamma = system.dot(residual, residual); /* gamma = |r|^2 */ │ │ │ │ │ -141 beta = currentGamma / prevGamma; │ │ │ │ │ -142 system.rightPrecondition(residual, q1); /* q1 = L^{-T} r */ │ │ │ │ │ -143 system.scal(beta, direction); │ │ │ │ │ -144 system.axpy(1.0, q1, direction); /* p = q1 + beta * p */ │ │ │ │ │ -145 │ │ │ │ │ -146 if (parameters.verbosity() >= ConjugateGradientParameters::ERROR ) │ │ │ │ │ -147 std::cout << "[PCG] k = " << k │ │ │ │ │ -148 << ", alpha = " << alpha │ │ │ │ │ -149 << ", beta = " << beta │ │ │ │ │ -150 << ", ||r||^2 = " << currentGamma │ │ │ │ │ -151// << "\nx =\n" << estimate │ │ │ │ │ -152// << "\nr =\n" << residual │ │ │ │ │ -153 << std::endl; │ │ │ │ │ -154 } │ │ │ │ │ -155 if (parameters.verbosity() >= ConjugateGradientParameters::COMPLEXITY ) │ │ │ │ │ -156 std::cout << "[PCG] iterations = " << k │ │ │ │ │ -157 << ", ||r||^2 = " << currentGamma │ │ │ │ │ -158 << std::endl; │ │ │ │ │ -159 │ │ │ │ │ -160 return estimate; │ │ │ │ │ -161} │ │ │ │ │ -162 │ │ │ │ │ -163 │ │ │ │ │ -164} │ │ │ │ │ -_I_t_e_r_a_t_i_v_e_S_o_l_v_e_r_._h │ │ │ │ │ -Some support classes for iterative solvers. │ │ │ │ │ +_1_1_5 virtual Matrix _i_n_f_o_r_m_a_t_i_o_n() const = 0; │ │ │ │ │ +116 │ │ │ │ │ +118 _V_e_c_t_o_r_V_a_l_u_e_s hessianDiagonal() const; │ │ │ │ │ +119 │ │ │ │ │ +_1_2_1 virtual void _h_e_s_s_i_a_n_D_i_a_g_o_n_a_l_A_d_d(_V_e_c_t_o_r_V_a_l_u_e_s& d) const = 0; │ │ │ │ │ +122 │ │ │ │ │ +_1_2_4 virtual void _h_e_s_s_i_a_n_D_i_a_g_o_n_a_l(double* d) const = 0; │ │ │ │ │ +125 │ │ │ │ │ +_1_2_7 virtual std::map _h_e_s_s_i_a_n_B_l_o_c_k_D_i_a_g_o_n_a_l() const = 0; │ │ │ │ │ +128 │ │ │ │ │ +_1_3_0 virtual _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r _c_l_o_n_e() const = 0; │ │ │ │ │ +131 │ │ │ │ │ +_1_3_7 virtual _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r _n_e_g_a_t_e() const = 0; │ │ │ │ │ +138 │ │ │ │ │ +_1_4_4 virtual void _u_p_d_a_t_e_H_e_s_s_i_a_n(const _K_e_y_V_e_c_t_o_r& keys, │ │ │ │ │ +145 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x* info) const = 0; │ │ │ │ │ +146 │ │ │ │ │ +_1_4_8 virtual void _m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d(double alpha, const _V_e_c_t_o_r_V_a_l_u_e_s& x, │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s& y) const = 0; │ │ │ │ │ +149 │ │ │ │ │ +_1_5_1 virtual _V_e_c_t_o_r_V_a_l_u_e_s _g_r_a_d_i_e_n_t_A_t_Z_e_r_o() const = 0; │ │ │ │ │ +152 │ │ │ │ │ +_1_5_4 virtual void _g_r_a_d_i_e_n_t_A_t_Z_e_r_o(double* d) const = 0; │ │ │ │ │ +155 │ │ │ │ │ +_1_5_7 virtual Vector _g_r_a_d_i_e_n_t(_K_e_y key, const _V_e_c_t_o_r_V_a_l_u_e_s& x) const = 0; │ │ │ │ │ +158 │ │ │ │ │ +159 // Determine position of a given key │ │ │ │ │ +160 template │ │ │ │ │ +161 static _D_e_n_s_e_I_n_d_e_x Slot(const CONTAINER& keys, _K_e_y key) { │ │ │ │ │ +162 return std::find(keys.begin(), keys.end(), key) - keys.begin(); │ │ │ │ │ +163 } │ │ │ │ │ +164 │ │ │ │ │ +165 private: │ │ │ │ │ +_1_6_7 friend class boost::serialization::access; │ │ │ │ │ +168 template │ │ │ │ │ +169 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +170 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +171 } │ │ │ │ │ +172 │ │ │ │ │ +173 }; // GaussianFactor │ │ │ │ │ +174 │ │ │ │ │ +176template<> │ │ │ │ │ +_1_7_7struct _t_r_a_i_t_s<_G_a_u_s_s_i_a_n_F_a_c_t_o_r> : public _T_e_s_t_a_b_l_e { │ │ │ │ │ +178}; │ │ │ │ │ +179 │ │ │ │ │ +180} // \ namespace gtsam │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ +_M_a_t_r_i_x_._h │ │ │ │ │ +typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +_F_a_c_t_o_r_._h │ │ │ │ │ +The base class for all factors. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ -parameters for the conjugate gradient method │ │ │ │ │ -DDeeffiinniittiioonn ConjugateGradientSolver.h:29 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s_:_:_m_i_n_I_t_e_r_a_t_i_o_n_s__ │ │ │ │ │ -size_t minIterations_ │ │ │ │ │ -minimum number of cg iterations │ │ │ │ │ -DDeeffiinniittiioonn ConjugateGradientSolver.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s_:_:_r_e_s_e_t__ │ │ │ │ │ -size_t reset_ │ │ │ │ │ -number of iterations before reset │ │ │ │ │ -DDeeffiinniittiioonn ConjugateGradientSolver.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s_:_:_B_L_A_S_K_e_r_n_e_l │ │ │ │ │ -BLASKernel │ │ │ │ │ -DDeeffiinniittiioonn ConjugateGradientSolver.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s_:_:_e_p_s_i_l_o_n___r_e_l__ │ │ │ │ │ -double epsilon_rel_ │ │ │ │ │ -threshold for relative error decrease │ │ │ │ │ -DDeeffiinniittiioonn ConjugateGradientSolver.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s_:_:_m_a_x_I_t_e_r_a_t_i_o_n_s__ │ │ │ │ │ -size_t maxIterations_ │ │ │ │ │ -maximum number of cg iterations │ │ │ │ │ -DDeeffiinniittiioonn ConjugateGradientSolver.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s_:_:_e_p_s_i_l_o_n___a_b_s__ │ │ │ │ │ -double epsilon_abs_ │ │ │ │ │ -threshold for absolute error decrease │ │ │ │ │ -DDeeffiinniittiioonn ConjugateGradientSolver.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_I_t_e_r_a_t_i_v_e_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ -parameters for iterative linear solvers │ │ │ │ │ -DDeeffiinniittiioonn IterativeSolver.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_n_s_e_I_n_d_e_x │ │ │ │ │ +ptrdiff_t DenseIndex │ │ │ │ │ +The index type for Eigen objects. │ │ │ │ │ +DDeeffiinniittiioonn types.h:106 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ +of blocks. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:68 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_c_o_n_s_t___i_t_e_r_a_t_o_r │ │ │ │ │ +KeyVector::const_iterator const_iterator │ │ │ │ │ +Const iterator over keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:80 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +An abstract virtual base class for JacobianFactor and HessianFactor. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactor.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_g_e_t_D_i_m │ │ │ │ │ +virtual DenseIndex getDim(const_iterator variable) const =0 │ │ │ │ │ +Return the dimension of the variable pointed to by the given key iterator. │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_h_e_s_s_i_a_n_B_l_o_c_k_D_i_a_g_o_n_a_l │ │ │ │ │ +virtual std::map< Key, Matrix > hessianBlockDiagonal() const =0 │ │ │ │ │ +Return the block diagonal of the Hessian for this factor. │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactor.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_j_a_c_o_b_i_a_n │ │ │ │ │ +virtual std::pair< Matrix, Vector > jacobian() const =0 │ │ │ │ │ +Return the dense Jacobian and right-hand-side , with the noise models baked │ │ │ │ │ +into A and b. │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_g_r_a_d_i_e_n_t │ │ │ │ │ +virtual Vector gradient(Key key, const VectorValues &x) const =0 │ │ │ │ │ +Gradient wrt a key at any values. │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ +virtual GaussianFactor::shared_ptr clone() const =0 │ │ │ │ │ +Clone a factor (make a deep copy) │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_i_n_f_o_r_m_a_t_i_o_n │ │ │ │ │ +virtual Matrix information() const =0 │ │ │ │ │ +Return the non-augmented information matrix represented by this GaussianFactor. │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_B_a_s_e │ │ │ │ │ +Factor Base │ │ │ │ │ +Our base class. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactor.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_~_G_a_u_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +virtual ~GaussianFactor() │ │ │ │ │ +Destructor. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactor.h:54 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_g_r_a_d_i_e_n_t_A_t_Z_e_r_o │ │ │ │ │ +virtual VectorValues gradientAtZero() const =0 │ │ │ │ │ +A'*b for Jacobian, eta for Hessian. │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +GaussianFactor(const CONTAINER &keys) │ │ │ │ │ +Construct from container of keys. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactor.h:51 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d │ │ │ │ │ +virtual void multiplyHessianAdd(double alpha, const VectorValues &x, │ │ │ │ │ +VectorValues &y) const =0 │ │ │ │ │ +y += alpha * A'*A*x │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +virtual bool equals(const GaussianFactor &lf, double tol=1e-9) const =0 │ │ │ │ │ +Equals for testable. │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_a_u_g_m_e_n_t_e_d_I_n_f_o_r_m_a_t_i_o_n │ │ │ │ │ +virtual Matrix augmentedInformation() const =0 │ │ │ │ │ +Return the augmented information matrix represented by this GaussianFactor. │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_a_u_g_m_e_n_t_e_d_J_a_c_o_b_i_a_n │ │ │ │ │ +virtual Matrix augmentedJacobian() const =0 │ │ │ │ │ +Return a dense Jacobian matrix, augmented with b with the noise models baked │ │ │ │ │ +into A and b. │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +GaussianFactor() │ │ │ │ │ +Default constructor creates empty factor. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactor.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_n_e_g_a_t_e │ │ │ │ │ +virtual GaussianFactor::shared_ptr negate() const =0 │ │ │ │ │ +Construct the corresponding anti-factor to negate information stored stored in │ │ │ │ │ +this factor. │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_h_e_s_s_i_a_n_D_i_a_g_o_n_a_l │ │ │ │ │ +virtual void hessianDiagonal(double *d) const =0 │ │ │ │ │ +Raw memory access version of hessianDiagonal. │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_T_h_i_s │ │ │ │ │ +GaussianFactor This │ │ │ │ │ +This class. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactor.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_u_p_d_a_t_e_H_e_s_s_i_a_n │ │ │ │ │ +virtual void updateHessian(const KeyVector &keys, SymmetricBlockMatrix *info) │ │ │ │ │ +const =0 │ │ │ │ │ +Update an information matrix by adding the information corresponding to this │ │ │ │ │ +factor (used internally ... │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&formatter=DefaultKeyFormatter) const override=0 │ │ │ │ │ +print │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_h_e_s_s_i_a_n_D_i_a_g_o_n_a_l_A_d_d │ │ │ │ │ +virtual void hessianDiagonalAdd(VectorValues &d) const =0 │ │ │ │ │ +Add the current diagonal to a VectorValues instance. │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_g_r_a_d_i_e_n_t_A_t_Z_e_r_o │ │ │ │ │ +virtual void gradientAtZero(double *d) const =0 │ │ │ │ │ +Raw memory access version of gradientAtZero. │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ +with a unique integer... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +The Factor::error simply extracts the. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_S_o_l_v_e_r_._h │ │ │ │ │ + * _G_a_u_s_s_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00818.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/linearExceptions.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Scatter.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,35 +95,33 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces
│ │ │ │ -
linearExceptions.cpp File Reference
│ │ │ │ +
Scatter.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -

Exceptions that may be thrown by linear solver components. │ │ │ │ -More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Exceptions that may be thrown by linear solver components.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Aug 17, 2012
│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +
Date
June 2015
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,21 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -linearExceptions.cpp File Reference │ │ │ │ │ -Exceptions that may be thrown by linear solver components. _M_o_r_e_._._. │ │ │ │ │ +Scatter.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Exceptions that may be thrown by linear solver components. │ │ │ │ │ Author │ │ │ │ │ Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - Aug 17, 2012 │ │ │ │ │ + June 2015 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _l_i_n_e_a_r_E_x_c_e_p_t_i_o_n_s_._c_p_p │ │ │ │ │ + * _S_c_a_t_t_e_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00821.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/linearAlgorithms-inst.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,54 +94,35 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
linearAlgorithms-inst.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
GaussianFactor.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Templated algorithms that are used in multiple places in linear. │ │ │ │ +

A factor with a quadratic error function - a Gaussian. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

struct  gtsam::internal::linearAlgorithms::OptimizeData
 
struct  gtsam::internal::linearAlgorithms::OptimizeClique< CLIQUE >
 Pre-order visitor for back-substitution in a Bayes tree. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -template<class BAYESTREE >
VectorValues gtsam::internal::linearAlgorithms::optimizeBayesTree (const BAYESTREE &bayesTree)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Templated algorithms that are used in multiple places in linear.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ +

A factor with a quadratic error function - a Gaussian.

│ │ │ │ +

GaussianFactor

Author
Fan Jiang
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,32 +1,21 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -linearAlgorithms-inst.h File Reference │ │ │ │ │ -Templated algorithms that are used in multiple places in linear. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_l_i_n_e_a_r_A_l_g_o_r_i_t_h_m_s_:_:_O_p_t_i_m_i_z_e_D_a_t_a │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_l_i_n_e_a_r_A_l_g_o_r_i_t_h_m_s_:_:_O_p_t_i_m_i_z_e_C_l_i_q_u_e_<_ _C_L_I_Q_U_E_ _> │ │ │ │ │ -  Pre-order visitor for back-substitution in a Bayes tree. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +GaussianFactor.cpp File Reference │ │ │ │ │ +A factor with a quadratic error function - a Gaussian. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s  ggttssaamm::::iinntteerrnnaall::::lliinneeaarrAAllggoorriitthhmmss::::ooppttiimmiizzeeBBaayyeessTTrreeee (const │ │ │ │ │ - BAYESTREE &bayesTree) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Templated algorithms that are used in multiple places in linear. │ │ │ │ │ +A factor with a quadratic error function - a Gaussian. │ │ │ │ │ +GaussianFactor │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Fan Jiang │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _l_i_n_e_a_r_A_l_g_o_r_i_t_h_m_s_-_i_n_s_t_._h │ │ │ │ │ + * _G_a_u_s_s_i_a_n_F_a_c_t_o_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00824_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/JacobianFactor-inl.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,144 +98,631 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
JacobianFactor-inl.h
│ │ │ │ +
NoiseModel.h
│ │ │ │
│ │ │ │
│ │ │ │ -
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/linear/linearExceptions.h>
│ │ │ │ -
22
│ │ │ │ -
23namespace gtsam {
│ │ │ │ -
24
│ │ │ │ -
25 /* ************************************************************************* */
│ │ │ │ -
26 template<typename TERMS>
│ │ │ │ -
│ │ │ │ -
27 JacobianFactor::JacobianFactor(const TERMS&terms, const Vector &b, const SharedDiagonal& model)
│ │ │ │ -
28 {
│ │ │ │ -
29 fillTerms(terms, b, model);
│ │ │ │ -
30 }
│ │ │ │ -
│ │ │ │ +
21#include <gtsam/base/Testable.h>
│ │ │ │ +
22#include <gtsam/base/Matrix.h>
│ │ │ │ +
23#include <gtsam/dllexport.h>
│ │ │ │ +
24#include <gtsam/linear/LossFunctions.h>
│ │ │ │ +
25
│ │ │ │ +
26#include <boost/serialization/nvp.hpp>
│ │ │ │ +
27#include <boost/serialization/extended_type_info.hpp>
│ │ │ │ +
28#include <boost/serialization/singleton.hpp>
│ │ │ │ +
29#include <boost/serialization/shared_ptr.hpp>
│ │ │ │ +
30#include <boost/serialization/optional.hpp>
│ │ │ │
31
│ │ │ │ -
32 /* ************************************************************************* */
│ │ │ │ -
33 template<typename KEYS>
│ │ │ │ -
│ │ │ │ -
34 JacobianFactor::JacobianFactor(
│ │ │ │ -
35 const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& model) :
│ │ │ │ -
36 Base(keys), Ab_(augmentedMatrix)
│ │ │ │ -
37 {
│ │ │ │ -
38 // Check noise model dimension
│ │ │ │ -
39 if(model && (DenseIndex)model->dim() != augmentedMatrix.rows())
│ │ │ │ -
40 throw InvalidNoiseModel(augmentedMatrix.rows(), model->dim());
│ │ │ │ -
41
│ │ │ │ -
42 // Check number of variables
│ │ │ │ -
43 if((DenseIndex)Base::keys_.size() != augmentedMatrix.nBlocks() - 1)
│ │ │ │ -
44 throw std::invalid_argument(
│ │ │ │ -
45 "Error in JacobianFactor constructor input. Number of provided keys plus\n"
│ │ │ │ -
46 "one for the RHS vector must equal the number of provided matrix blocks.");
│ │ │ │ -
47
│ │ │ │ -
48 // Check RHS dimension
│ │ │ │ -
49 if(augmentedMatrix(augmentedMatrix.nBlocks() - 1).cols() != 1)
│ │ │ │ -
50 throw std::invalid_argument(
│ │ │ │ -
51 "Error in JacobianFactor constructor input. The last provided matrix block\n"
│ │ │ │ -
52 "must be the RHS vector, but the last provided block had more than one column.");
│ │ │ │ -
53
│ │ │ │ -
54 // Take noise model
│ │ │ │ -
55 model_ = model;
│ │ │ │ -
56 }
│ │ │ │ -
│ │ │ │ +
32namespace gtsam {
│ │ │ │ +
33
│ │ │ │ +
35 namespace noiseModel {
│ │ │ │ +
36
│ │ │ │ +
37 // Forward declaration
│ │ │ │ +
38 class Gaussian;
│ │ │ │ +
39 class Diagonal;
│ │ │ │ +
40 class Constrained;
│ │ │ │ +
41 class Isotropic;
│ │ │ │ +
42 class Unit;
│ │ │ │ +
43 class RobustModel;
│ │ │ │ +
44
│ │ │ │ +
45 //---------------------------------------------------------------------------------------
│ │ │ │ +
46
│ │ │ │ +
│ │ │ │ +
53 class GTSAM_EXPORT Base {
│ │ │ │ +
54
│ │ │ │ +
55 public:
│ │ │ │ +
56 typedef boost::shared_ptr<Base> shared_ptr;
│ │ │ │
57
│ │ │ │ -
58 /* ************************************************************************* */
│ │ │ │ -
59 template<typename TERMS>
│ │ │ │ -
│ │ │ │ -
60 void JacobianFactor::fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel)
│ │ │ │ -
61 {
│ │ │ │ -
62 // Check noise model dimension
│ │ │ │ -
63 if(noiseModel && (DenseIndex)noiseModel->dim() != b.size())
│ │ │ │ -
64 throw InvalidNoiseModel(b.size(), noiseModel->dim());
│ │ │ │ -
65
│ │ │ │ -
66 // Resize base class key vector
│ │ │ │ -
67 Base::keys_.resize(terms.size());
│ │ │ │ -
68
│ │ │ │ -
69 // Get dimensions of matrices
│ │ │ │ -
70 std::vector<size_t> dimensions;
│ │ │ │ -
71 dimensions.reserve(terms.size());
│ │ │ │ -
72 for(typename TERMS::const_iterator it = terms.begin(); it != terms.end(); ++it) {
│ │ │ │ -
73 const std::pair<Key, Matrix>& term = *it;
│ │ │ │ -
74 const Matrix& Ai = term.second;
│ │ │ │ -
75 dimensions.push_back(Ai.cols());
│ │ │ │ -
76 }
│ │ │ │ -
77
│ │ │ │ -
78 // Construct block matrix
│ │ │ │ -
79 Ab_ = VerticalBlockMatrix(dimensions, b.size(), true);
│ │ │ │ +
58 protected:
│ │ │ │ +
59
│ │ │ │ +
60 size_t dim_;
│ │ │ │ +
61
│ │ │ │ +
62 public:
│ │ │ │ +
63
│ │ │ │ +
65 Base(size_t dim = 1):dim_(dim) {}
│ │ │ │ +
66 virtual ~Base() {}
│ │ │ │ +
67
│ │ │ │ +
69 virtual bool isConstrained() const { return false; } // default false
│ │ │ │ +
70
│ │ │ │ +
72 virtual bool isUnit() const { return false; } // default false
│ │ │ │ +
73
│ │ │ │ +
75 inline size_t dim() const { return dim_;}
│ │ │ │ +
76
│ │ │ │ +
77 virtual void print(const std::string& name = "") const = 0;
│ │ │ │ +
78
│ │ │ │ +
79 virtual bool equals(const Base& expected, double tol=1e-9) const = 0;
│ │ │ │
80
│ │ │ │ -
81 // Check and add terms
│ │ │ │ -
82 DenseIndex i = 0; // For block index
│ │ │ │ -
83 for(typename TERMS::const_iterator it = terms.begin(); it != terms.end(); ++it) {
│ │ │ │ -
84 const std::pair<Key, Matrix>& term = *it;
│ │ │ │ -
85 Key key = term.first;
│ │ │ │ -
86 const Matrix& Ai = term.second;
│ │ │ │ -
87
│ │ │ │ -
88 // Check block rows
│ │ │ │ -
89 if(Ai.rows() != Ab_.rows())
│ │ │ │ -
90 throw InvalidMatrixBlock(Ab_.rows(), Ai.rows());
│ │ │ │ -
91
│ │ │ │ -
92 // Assign key and matrix
│ │ │ │ -
93 Base::keys_[i] = key;
│ │ │ │ -
94 Ab_(i) = Ai;
│ │ │ │ +
82 virtual Vector sigmas() const;
│ │ │ │ +
83
│ │ │ │ +
85 virtual Vector whiten(const Vector& v) const = 0;
│ │ │ │ +
86
│ │ │ │ +
88 virtual Matrix Whiten(const Matrix& H) const = 0;
│ │ │ │ +
89
│ │ │ │ +
91 virtual Vector unwhiten(const Vector& v) const = 0;
│ │ │ │ +
92
│ │ │ │ +
94 virtual double squaredMahalanobisDistance(const Vector& v) const;
│ │ │ │
95
│ │ │ │ -
96 // Increment block index
│ │ │ │ -
97 ++ i;
│ │ │ │ -
98 }
│ │ │ │ -
99
│ │ │ │ -
100 // Assign RHS vector
│ │ │ │ -
101 getb() = b;
│ │ │ │ -
102
│ │ │ │ -
103 // Assign noise model
│ │ │ │ -
104 model_ = noiseModel;
│ │ │ │ -
105 }
│ │ │ │ -
│ │ │ │ -
106
│ │ │ │ -
107} // gtsam
│ │ │ │ -
108
│ │ │ │ -
linearExceptions.h
Exceptions that may be thrown by linear solver components.
│ │ │ │ +
│ │ │ │ +
97 virtual double mahalanobisDistance(const Vector& v) const {
│ │ │ │ +
98 return std::sqrt(squaredMahalanobisDistance(v));
│ │ │ │ +
99 }
│ │ │ │ +
│ │ │ │ +
100
│ │ │ │ +
│ │ │ │ +
102 virtual double loss(const double squared_distance) const {
│ │ │ │ +
103 return 0.5 * squared_distance;
│ │ │ │ +
104 }
│ │ │ │ +
│ │ │ │ +
105
│ │ │ │ +
106 virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const = 0;
│ │ │ │ +
107 virtual void WhitenSystem(Matrix& A, Vector& b) const = 0;
│ │ │ │ +
108 virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const = 0;
│ │ │ │ +
109 virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const = 0;
│ │ │ │ +
110
│ │ │ │ +
│ │ │ │ +
112 virtual void whitenInPlace(Vector& v) const {
│ │ │ │ +
113 v = whiten(v);
│ │ │ │ +
114 }
│ │ │ │ +
│ │ │ │ +
115
│ │ │ │ +
│ │ │ │ +
117 virtual void unwhitenInPlace(Vector& v) const {
│ │ │ │ +
118 v = unwhiten(v);
│ │ │ │ +
119 }
│ │ │ │ +
│ │ │ │ +
120
│ │ │ │ +
│ │ │ │ +
122 virtual void whitenInPlace(Eigen::Block<Vector>& v) const {
│ │ │ │ +
123 v = whiten(v);
│ │ │ │ +
124 }
│ │ │ │ +
│ │ │ │ +
125
│ │ │ │ +
│ │ │ │ +
127 virtual void unwhitenInPlace(Eigen::Block<Vector>& v) const {
│ │ │ │ +
128 v = unwhiten(v);
│ │ │ │ +
129 }
│ │ │ │ +
│ │ │ │ +
130
│ │ │ │ +
│ │ │ │ +
132 virtual Vector unweightedWhiten(const Vector& v) const {
│ │ │ │ +
133 return whiten(v);
│ │ │ │ +
134 }
│ │ │ │ +
│ │ │ │ +
135
│ │ │ │ +
137 virtual double weight(const Vector& v) const { return 1.0; }
│ │ │ │ +
138
│ │ │ │ +
139 private:
│ │ │ │ +
141 friend class boost::serialization::access;
│ │ │ │ +
142 template<class ARCHIVE>
│ │ │ │ +
143 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
144 ar & BOOST_SERIALIZATION_NVP(dim_);
│ │ │ │ +
145 }
│ │ │ │ +
146 };
│ │ │ │ +
│ │ │ │ +
147
│ │ │ │ +
148 //---------------------------------------------------------------------------------------
│ │ │ │ +
149
│ │ │ │ +
│ │ │ │ +
162 class GTSAM_EXPORT Gaussian: public Base {
│ │ │ │ +
163
│ │ │ │ +
164 protected:
│ │ │ │ +
165
│ │ │ │ +
167 boost::optional<Matrix> sqrt_information_;
│ │ │ │ +
168
│ │ │ │ +
169 private:
│ │ │ │ +
170
│ │ │ │ +
174 const Matrix& thisR() const {
│ │ │ │ +
175 // should never happen
│ │ │ │ +
176 if (!sqrt_information_) throw std::runtime_error("Gaussian: has no R matrix");
│ │ │ │ +
177 return *sqrt_information_;
│ │ │ │ +
178 }
│ │ │ │ +
179
│ │ │ │ +
180
│ │ │ │ +
181 public:
│ │ │ │ +
182
│ │ │ │ +
183 typedef boost::shared_ptr<Gaussian> shared_ptr;
│ │ │ │ +
184
│ │ │ │ +
│ │ │ │ +
186 Gaussian(size_t dim = 1,
│ │ │ │ +
187 const boost::optional<Matrix>& sqrt_information = boost::none)
│ │ │ │ +
188 : Base(dim), sqrt_information_(sqrt_information) {}
│ │ │ │ +
│ │ │ │ +
189
│ │ │ │ +
190 ~Gaussian() override {}
│ │ │ │ +
191
│ │ │ │ +
197 static shared_ptr SqrtInformation(const Matrix& R, bool smart = true);
│ │ │ │ +
198
│ │ │ │ +
204 static shared_ptr Information(const Matrix& M, bool smart = true);
│ │ │ │ +
205
│ │ │ │ +
211 static shared_ptr Covariance(const Matrix& covariance, bool smart = true);
│ │ │ │ +
212
│ │ │ │ +
213 void print(const std::string& name) const override;
│ │ │ │ +
214 bool equals(const Base& expected, double tol=1e-9) const override;
│ │ │ │ +
215 Vector sigmas() const override;
│ │ │ │ +
216 Vector whiten(const Vector& v) const override;
│ │ │ │ +
217 Vector unwhiten(const Vector& v) const override;
│ │ │ │ +
218
│ │ │ │ +
223 Matrix Whiten(const Matrix& H) const override;
│ │ │ │ +
224
│ │ │ │ +
228 virtual void WhitenInPlace(Matrix& H) const;
│ │ │ │ +
229
│ │ │ │ +
233 virtual void WhitenInPlace(Eigen::Block<Matrix> H) const;
│ │ │ │ +
234
│ │ │ │ +
238 void WhitenSystem(std::vector<Matrix>& A, Vector& b) const override;
│ │ │ │ +
239 void WhitenSystem(Matrix& A, Vector& b) const override;
│ │ │ │ +
240 void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const override;
│ │ │ │ +
241 void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const override;
│ │ │ │ +
242
│ │ │ │ +
252 virtual boost::shared_ptr<Diagonal> QR(Matrix& Ab) const;
│ │ │ │ +
253
│ │ │ │ +
255 virtual Matrix R() const { return thisR();}
│ │ │ │ +
256
│ │ │ │ +
258 virtual Matrix information() const;
│ │ │ │ +
259
│ │ │ │ +
261 virtual Matrix covariance() const;
│ │ │ │ +
262
│ │ │ │ +
263 private:
│ │ │ │ +
265 friend class boost::serialization::access;
│ │ │ │ +
266 template<class ARCHIVE>
│ │ │ │ +
267 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
268 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
269 ar & BOOST_SERIALIZATION_NVP(sqrt_information_);
│ │ │ │ +
270 }
│ │ │ │ +
271
│ │ │ │ +
272 }; // Gaussian
│ │ │ │ +
│ │ │ │ +
273
│ │ │ │ +
274 //---------------------------------------------------------------------------------------
│ │ │ │ +
275
│ │ │ │ +
│ │ │ │ +
281 class GTSAM_EXPORT Diagonal : public Gaussian {
│ │ │ │ +
282 protected:
│ │ │ │ +
283
│ │ │ │ +
289 Vector sigmas_, invsigmas_, precisions_;
│ │ │ │ +
290
│ │ │ │ +
291 protected:
│ │ │ │ +
292
│ │ │ │ +
294 Diagonal(const Vector& sigmas);
│ │ │ │ +
295
│ │ │ │ +
296 public:
│ │ │ │ +
298 Diagonal();
│ │ │ │ +
299
│ │ │ │ +
300 typedef boost::shared_ptr<Diagonal> shared_ptr;
│ │ │ │ +
301
│ │ │ │ +
302 ~Diagonal() override {}
│ │ │ │ +
303
│ │ │ │ +
308 static shared_ptr Sigmas(const Vector& sigmas, bool smart = true);
│ │ │ │ +
309
│ │ │ │ +
316 static shared_ptr Variances(const Vector& variances, bool smart = true);
│ │ │ │ +
317
│ │ │ │ +
322 static shared_ptr Precisions(const Vector& precisions, bool smart = true);
│ │ │ │ +
323
│ │ │ │ +
324 void print(const std::string& name) const override;
│ │ │ │ +
325 Vector sigmas() const override { return sigmas_; }
│ │ │ │ +
326 Vector whiten(const Vector& v) const override;
│ │ │ │ +
327 Vector unwhiten(const Vector& v) const override;
│ │ │ │ +
328 Matrix Whiten(const Matrix& H) const override;
│ │ │ │ +
329 void WhitenInPlace(Matrix& H) const override;
│ │ │ │ +
330 void WhitenInPlace(Eigen::Block<Matrix> H) const override;
│ │ │ │ +
331
│ │ │ │ +
335 inline double sigma(size_t i) const { return sigmas_(i); }
│ │ │ │ +
336
│ │ │ │ +
340 inline const Vector& invsigmas() const { return invsigmas_; }
│ │ │ │ +
341 inline double invsigma(size_t i) const {return invsigmas_(i);}
│ │ │ │ +
342
│ │ │ │ +
346 inline const Vector& precisions() const { return precisions_; }
│ │ │ │ +
347 inline double precision(size_t i) const {return precisions_(i);}
│ │ │ │ +
348
│ │ │ │ +
│ │ │ │ +
352 Matrix R() const override {
│ │ │ │ +
353 return invsigmas().asDiagonal();
│ │ │ │ +
354 }
│ │ │ │ +
│ │ │ │ +
355
│ │ │ │ +
356 private:
│ │ │ │ +
358 friend class boost::serialization::access;
│ │ │ │ +
359 template<class ARCHIVE>
│ │ │ │ +
360 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
361 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Gaussian);
│ │ │ │ +
362 ar & BOOST_SERIALIZATION_NVP(sigmas_);
│ │ │ │ +
363 ar & BOOST_SERIALIZATION_NVP(invsigmas_);
│ │ │ │ +
364 }
│ │ │ │ +
365 }; // Diagonal
│ │ │ │ +
│ │ │ │ +
366
│ │ │ │ +
367 //---------------------------------------------------------------------------------------
│ │ │ │ +
368
│ │ │ │ +
│ │ │ │ +
381 class GTSAM_EXPORT Constrained : public Diagonal {
│ │ │ │ +
382 protected:
│ │ │ │ +
383
│ │ │ │ +
384 // Sigmas are contained in the base class
│ │ │ │ +
385 Vector mu_;
│ │ │ │ +
386
│ │ │ │ +
392 Constrained(const Vector& mu, const Vector& sigmas);
│ │ │ │ +
393
│ │ │ │ +
394 public:
│ │ │ │ +
395
│ │ │ │ +
396 typedef boost::shared_ptr<Constrained> shared_ptr;
│ │ │ │ +
397
│ │ │ │ +
404 Constrained(const Vector& sigmas = Z_1x1);
│ │ │ │ +
405
│ │ │ │ +
406 ~Constrained() override {}
│ │ │ │ +
407
│ │ │ │ +
409 bool isConstrained() const override { return true; }
│ │ │ │ +
410
│ │ │ │ +
412 bool constrained(size_t i) const;
│ │ │ │ +
413
│ │ │ │ +
415 const Vector& mu() const { return mu_; }
│ │ │ │ +
416
│ │ │ │ +
421 static shared_ptr MixedSigmas(const Vector& mu, const Vector& sigmas);
│ │ │ │ +
422
│ │ │ │ +
427 static shared_ptr MixedSigmas(const Vector& sigmas);
│ │ │ │ +
428
│ │ │ │ +
433 static shared_ptr MixedSigmas(double m, const Vector& sigmas);
│ │ │ │ +
434
│ │ │ │ +
439 static shared_ptr MixedVariances(const Vector& mu, const Vector& variances);
│ │ │ │ +
440 static shared_ptr MixedVariances(const Vector& variances);
│ │ │ │ +
441
│ │ │ │ +
446 static shared_ptr MixedPrecisions(const Vector& mu, const Vector& precisions);
│ │ │ │ +
447 static shared_ptr MixedPrecisions(const Vector& precisions);
│ │ │ │ +
448
│ │ │ │ +
454 double squaredMahalanobisDistance(const Vector& v) const override;
│ │ │ │ +
455
│ │ │ │ +
│ │ │ │ +
457 static shared_ptr All(size_t dim) {
│ │ │ │ +
458 return shared_ptr(new Constrained(Vector::Constant(dim, 1000.0), Vector::Constant(dim,0)));
│ │ │ │ +
459 }
│ │ │ │ +
│ │ │ │ +
460
│ │ │ │ +
│ │ │ │ +
462 static shared_ptr All(size_t dim, const Vector& mu) {
│ │ │ │ +
463 return shared_ptr(new Constrained(mu, Vector::Constant(dim,0)));
│ │ │ │ +
464 }
│ │ │ │ +
│ │ │ │ +
465
│ │ │ │ +
│ │ │ │ +
467 static shared_ptr All(size_t dim, double mu) {
│ │ │ │ +
468 return shared_ptr(new Constrained(Vector::Constant(dim, mu), Vector::Constant(dim,0)));
│ │ │ │ +
469 }
│ │ │ │ +
│ │ │ │ +
470
│ │ │ │ +
471 void print(const std::string& name) const override;
│ │ │ │ +
472
│ │ │ │ +
474 Vector whiten(const Vector& v) const override;
│ │ │ │ +
475
│ │ │ │ +
478 Matrix Whiten(const Matrix& H) const override;
│ │ │ │ +
479 void WhitenInPlace(Matrix& H) const override;
│ │ │ │ +
480 void WhitenInPlace(Eigen::Block<Matrix> H) const override;
│ │ │ │ +
481
│ │ │ │ +
491 Diagonal::shared_ptr QR(Matrix& Ab) const override;
│ │ │ │ +
492
│ │ │ │ +
497 shared_ptr unit() const;
│ │ │ │ +
498
│ │ │ │ +
499 private:
│ │ │ │ +
501 friend class boost::serialization::access;
│ │ │ │ +
502 template<class ARCHIVE>
│ │ │ │ +
503 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
504 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Diagonal);
│ │ │ │ +
505 ar & BOOST_SERIALIZATION_NVP(mu_);
│ │ │ │ +
506 }
│ │ │ │ +
507
│ │ │ │ +
508 }; // Constrained
│ │ │ │ +
│ │ │ │ +
509
│ │ │ │ +
510 //---------------------------------------------------------------------------------------
│ │ │ │ +
511
│ │ │ │ +
│ │ │ │ +
516 class GTSAM_EXPORT Isotropic : public Diagonal {
│ │ │ │ +
517 protected:
│ │ │ │ +
518 double sigma_, invsigma_;
│ │ │ │ +
519
│ │ │ │ +
│ │ │ │ +
521 Isotropic(size_t dim, double sigma) :
│ │ │ │ +
522 Diagonal(Vector::Constant(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {}
│ │ │ │ +
│ │ │ │ +
523
│ │ │ │ +
524 public:
│ │ │ │ +
525
│ │ │ │ +
526 /* dummy constructor to allow for serialization */
│ │ │ │ +
527 Isotropic() : Diagonal(Vector1::Constant(1.0)),sigma_(1.0),invsigma_(1.0) {}
│ │ │ │ +
528
│ │ │ │ +
529 ~Isotropic() override {}
│ │ │ │ +
530
│ │ │ │ +
531 typedef boost::shared_ptr<Isotropic> shared_ptr;
│ │ │ │ +
532
│ │ │ │ +
536 static shared_ptr Sigma(size_t dim, double sigma, bool smart = true);
│ │ │ │ +
537
│ │ │ │ +
544 static shared_ptr Variance(size_t dim, double variance, bool smart = true);
│ │ │ │ +
545
│ │ │ │ +
│ │ │ │ +
549 static shared_ptr Precision(size_t dim, double precision, bool smart = true) {
│ │ │ │ +
550 return Variance(dim, 1.0/precision, smart);
│ │ │ │ +
551 }
│ │ │ │ +
│ │ │ │ +
552
│ │ │ │ +
553 void print(const std::string& name) const override;
│ │ │ │ +
554 double squaredMahalanobisDistance(const Vector& v) const override;
│ │ │ │ +
555 Vector whiten(const Vector& v) const override;
│ │ │ │ +
556 Vector unwhiten(const Vector& v) const override;
│ │ │ │ +
557 Matrix Whiten(const Matrix& H) const override;
│ │ │ │ +
558 void WhitenInPlace(Matrix& H) const override;
│ │ │ │ +
559 void whitenInPlace(Vector& v) const override;
│ │ │ │ +
560 void WhitenInPlace(Eigen::Block<Matrix> H) const override;
│ │ │ │ +
561
│ │ │ │ +
565 inline double sigma() const { return sigma_; }
│ │ │ │ +
566
│ │ │ │ +
567 private:
│ │ │ │ +
569 friend class boost::serialization::access;
│ │ │ │ +
570 template<class ARCHIVE>
│ │ │ │ +
571 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
572 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Diagonal);
│ │ │ │ +
573 ar & BOOST_SERIALIZATION_NVP(sigma_);
│ │ │ │ +
574 ar & BOOST_SERIALIZATION_NVP(invsigma_);
│ │ │ │ +
575 }
│ │ │ │ +
576
│ │ │ │ +
577 };
│ │ │ │ +
│ │ │ │ +
578
│ │ │ │ +
579 //---------------------------------------------------------------------------------------
│ │ │ │ +
580
│ │ │ │ +
│ │ │ │ +
584 class GTSAM_EXPORT Unit : public Isotropic {
│ │ │ │ +
585 public:
│ │ │ │ +
586
│ │ │ │ +
587 typedef boost::shared_ptr<Unit> shared_ptr;
│ │ │ │ +
588
│ │ │ │ +
590 Unit(size_t dim=1): Isotropic(dim,1.0) {}
│ │ │ │ +
591
│ │ │ │ +
592 ~Unit() override {}
│ │ │ │ +
593
│ │ │ │ +
│ │ │ │ +
597 static shared_ptr Create(size_t dim) {
│ │ │ │ +
598 return shared_ptr(new Unit(dim));
│ │ │ │ +
599 }
│ │ │ │ +
│ │ │ │ +
600
│ │ │ │ +
602 bool isUnit() const override { return true; }
│ │ │ │ +
603
│ │ │ │ +
604 void print(const std::string& name) const override;
│ │ │ │ +
605 double squaredMahalanobisDistance(const Vector& v) const override;
│ │ │ │ +
606 Vector whiten(const Vector& v) const override { return v; }
│ │ │ │ +
607 Vector unwhiten(const Vector& v) const override { return v; }
│ │ │ │ +
608 Matrix Whiten(const Matrix& H) const override { return H; }
│ │ │ │ +
609 void WhitenInPlace(Matrix& /*H*/) const override {}
│ │ │ │ +
610 void WhitenInPlace(Eigen::Block<Matrix> /*H*/) const override {}
│ │ │ │ +
611 void whitenInPlace(Vector& /*v*/) const override {}
│ │ │ │ +
612 void unwhitenInPlace(Vector& /*v*/) const override {}
│ │ │ │ +
613 void whitenInPlace(Eigen::Block<Vector>& /*v*/) const override {}
│ │ │ │ +
614 void unwhitenInPlace(Eigen::Block<Vector>& /*v*/) const override {}
│ │ │ │ +
615
│ │ │ │ +
616 private:
│ │ │ │ +
618 friend class boost::serialization::access;
│ │ │ │ +
619 template<class ARCHIVE>
│ │ │ │ +
620 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
621 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Isotropic);
│ │ │ │ +
622 }
│ │ │ │ +
623 };
│ │ │ │ +
│ │ │ │ +
624
│ │ │ │ +
│ │ │ │ +
642 class GTSAM_EXPORT Robust : public Base {
│ │ │ │ +
643 public:
│ │ │ │ +
644 typedef boost::shared_ptr<Robust> shared_ptr;
│ │ │ │ +
645
│ │ │ │ +
646 protected:
│ │ │ │ +
647 typedef mEstimator::Base RobustModel;
│ │ │ │ +
648 typedef noiseModel::Base NoiseModel;
│ │ │ │ +
649
│ │ │ │ +
650 const RobustModel::shared_ptr robust_;
│ │ │ │ +
651 const NoiseModel::shared_ptr noise_;
│ │ │ │ +
652
│ │ │ │ +
653 public:
│ │ │ │ +
654
│ │ │ │ +
656 Robust() {};
│ │ │ │ +
657
│ │ │ │ +
│ │ │ │ +
659 Robust(const RobustModel::shared_ptr robust, const NoiseModel::shared_ptr noise)
│ │ │ │ +
660 : Base(noise->dim()), robust_(robust), noise_(noise) {}
│ │ │ │ +
│ │ │ │ +
661
│ │ │ │ +
663 ~Robust() override {}
│ │ │ │ +
664
│ │ │ │ +
665 void print(const std::string& name) const override;
│ │ │ │ +
666 bool equals(const Base& expected, double tol=1e-9) const override;
│ │ │ │ +
667
│ │ │ │ +
669 const RobustModel::shared_ptr& robust() const { return robust_; }
│ │ │ │ +
670
│ │ │ │ +
672 const NoiseModel::shared_ptr& noise() const { return noise_; }
│ │ │ │ +
673
│ │ │ │ +
674 // Functions below are dummy but necessary for the noiseModel::Base
│ │ │ │ +
│ │ │ │ +
675 inline Vector whiten(const Vector& v) const override
│ │ │ │ +
676 { Vector r = v; this->WhitenSystem(r); return r; }
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
677 inline Matrix Whiten(const Matrix& A) const override
│ │ │ │ +
678 { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; }
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
679 inline Vector unwhiten(const Vector& /*v*/) const override
│ │ │ │ +
680 { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); }
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
682 double loss(const double squared_distance) const override {
│ │ │ │ +
683 return robust_->loss(std::sqrt(squared_distance));
│ │ │ │ +
684 }
│ │ │ │ +
│ │ │ │ +
685
│ │ │ │ +
686 // NOTE: This is special because in whiten the base version will do the reweighting
│ │ │ │ +
687 // which is incorrect!
│ │ │ │ +
│ │ │ │ +
688 double squaredMahalanobisDistance(const Vector& v) const override {
│ │ │ │ +
689 return noise_->squaredMahalanobisDistance(v);
│ │ │ │ +
690 }
│ │ │ │ +
│ │ │ │ +
691
│ │ │ │ +
692 // These are really robust iterated re-weighting support functions
│ │ │ │ +
693 virtual void WhitenSystem(Vector& b) const;
│ │ │ │ +
694 void WhitenSystem(std::vector<Matrix>& A, Vector& b) const override;
│ │ │ │ +
695 void WhitenSystem(Matrix& A, Vector& b) const override;
│ │ │ │ +
696 void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const override;
│ │ │ │ +
697 void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const override;
│ │ │ │ +
698
│ │ │ │ +
699 Vector unweightedWhiten(const Vector& v) const override;
│ │ │ │ +
700 double weight(const Vector& v) const override;
│ │ │ │ +
701
│ │ │ │ +
702 static shared_ptr Create(
│ │ │ │ +
703 const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise);
│ │ │ │ +
704
│ │ │ │ +
705 private:
│ │ │ │ +
707 friend class boost::serialization::access;
│ │ │ │ +
708 template<class ARCHIVE>
│ │ │ │ +
709 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
710 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
711 ar & boost::serialization::make_nvp("robust_", const_cast<RobustModel::shared_ptr&>(robust_));
│ │ │ │ +
712 ar & boost::serialization::make_nvp("noise_", const_cast<NoiseModel::shared_ptr&>(noise_));
│ │ │ │ +
713 }
│ │ │ │ +
714 };
│ │ │ │ +
│ │ │ │ +
715
│ │ │ │ +
716 // Helper function
│ │ │ │ +
717 GTSAM_EXPORT boost::optional<Vector> checkIfDiagonal(const Matrix& M);
│ │ │ │ +
718
│ │ │ │ +
719 } // namespace noiseModel
│ │ │ │ +
720
│ │ │ │ +
724 typedef noiseModel::Base::shared_ptr SharedNoiseModel;
│ │ │ │ +
725 typedef noiseModel::Gaussian::shared_ptr SharedGaussian;
│ │ │ │ +
726 typedef noiseModel::Diagonal::shared_ptr SharedDiagonal;
│ │ │ │ +
727 typedef noiseModel::Constrained::shared_ptr SharedConstrained;
│ │ │ │ +
728 typedef noiseModel::Isotropic::shared_ptr SharedIsotropic;
│ │ │ │ +
729
│ │ │ │ +
731 template<> struct traits<noiseModel::Gaussian> : public Testable<noiseModel::Gaussian> {};
│ │ │ │ +
732 template<> struct traits<noiseModel::Diagonal> : public Testable<noiseModel::Diagonal> {};
│ │ │ │ +
733 template<> struct traits<noiseModel::Constrained> : public Testable<noiseModel::Constrained> {};
│ │ │ │ +
734 template<> struct traits<noiseModel::Isotropic> : public Testable<noiseModel::Isotropic> {};
│ │ │ │ +
735 template<> struct traits<noiseModel::Unit> : public Testable<noiseModel::Unit> {};
│ │ │ │ +
736
│ │ │ │ +
737} //\ namespace gtsam
│ │ │ │ +
738
│ │ │ │ +
739
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::VerticalBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43
│ │ │ │ -
gtsam::VerticalBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition VerticalBlockMatrix.h:115
│ │ │ │ -
gtsam::VerticalBlockMatrix::cols
DenseIndex cols() const
Column size.
Definition VerticalBlockMatrix.h:118
│ │ │ │ -
gtsam::VerticalBlockMatrix::nBlocks
DenseIndex nBlocks() const
Block count.
Definition VerticalBlockMatrix.h:121
│ │ │ │ -
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
│ │ │ │ -
gtsam::Factor::size
size_t size() const
Definition Factor.h:157
│ │ │ │ -
gtsam::GaussianFactor
An abstract virtual base class for JacobianFactor and HessianFactor.
Definition GaussianFactor.h:39
│ │ │ │ -
gtsam::JacobianFactor::getb
const constBVector getb() const
Get a view of the r.h.s.
Definition JacobianFactor.h:297
│ │ │ │ -
gtsam::JacobianFactor::JacobianFactor
JacobianFactor()
default constructor for I/O
Definition JacobianFactor.cpp:54
│ │ │ │ -
gtsam::JacobianFactor::fillTerms
void fillTerms(const TERMS &terms, const Vector &b, const SharedDiagonal &noiseModel)
Internal function to fill blocks and set dimensions.
Definition JacobianFactor-inl.h:60
│ │ │ │ -
gtsam::InvalidNoiseModel
An exception indicating that the noise model dimension passed into a JacobianFactor has a different d...
Definition linearExceptions.h:106
│ │ │ │ -
gtsam::InvalidMatrixBlock
An exception indicating that a matrix block passed into a JacobianFactor has a different dimensionali...
Definition linearExceptions.h:124
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::noiseModel::mEstimator::Base
Pure virtual class for all robust error function classes.
Definition LossFunctions.h:63
│ │ │ │ +
gtsam::noiseModel::Base
noiseModel::Base is the abstract base class for all noise models.
Definition NoiseModel.h:53
│ │ │ │ +
gtsam::noiseModel::Base::isConstrained
virtual bool isConstrained() const
true if a constrained noise model, saves slow/clumsy dynamic casting
Definition NoiseModel.h:69
│ │ │ │ +
gtsam::noiseModel::Base::whitenInPlace
virtual void whitenInPlace(Vector &v) const
in-place whiten, override if can be done more efficiently
Definition NoiseModel.h:112
│ │ │ │ +
gtsam::noiseModel::Base::dim
size_t dim() const
Dimensionality.
Definition NoiseModel.h:75
│ │ │ │ +
gtsam::noiseModel::Base::unwhitenInPlace
virtual void unwhitenInPlace(Eigen::Block< Vector > &v) const
in-place unwhiten, override if can be done more efficiently
Definition NoiseModel.h:127
│ │ │ │ +
gtsam::noiseModel::Base::whitenInPlace
virtual void whitenInPlace(Eigen::Block< Vector > &v) const
in-place whiten, override if can be done more efficiently
Definition NoiseModel.h:122
│ │ │ │ +
gtsam::noiseModel::Base::whiten
virtual Vector whiten(const Vector &v) const =0
Whiten an error vector.
│ │ │ │ +
gtsam::noiseModel::Base::mahalanobisDistance
virtual double mahalanobisDistance(const Vector &v) const
Mahalanobis distance.
Definition NoiseModel.h:97
│ │ │ │ +
gtsam::noiseModel::Base::isUnit
virtual bool isUnit() const
true if a unit noise model, saves slow/clumsy dynamic casting
Definition NoiseModel.h:72
│ │ │ │ +
gtsam::noiseModel::Base::weight
virtual double weight(const Vector &v) const
get the weight from the effective loss function on residual vector v
Definition NoiseModel.h:137
│ │ │ │ +
gtsam::noiseModel::Base::unweightedWhiten
virtual Vector unweightedWhiten(const Vector &v) const
Useful function for robust noise models to get the unweighted but whitened error.
Definition NoiseModel.h:132
│ │ │ │ +
gtsam::noiseModel::Base::unwhiten
virtual Vector unwhiten(const Vector &v) const =0
Unwhiten an error vector.
│ │ │ │ +
gtsam::noiseModel::Base::loss
virtual double loss(const double squared_distance) const
loss function, input is Mahalanobis distance
Definition NoiseModel.h:102
│ │ │ │ +
gtsam::noiseModel::Base::unwhitenInPlace
virtual void unwhitenInPlace(Vector &v) const
in-place unwhiten, override if can be done more efficiently
Definition NoiseModel.h:117
│ │ │ │ +
gtsam::noiseModel::Base::Whiten
virtual Matrix Whiten(const Matrix &H) const =0
Whiten a matrix.
│ │ │ │ +
gtsam::noiseModel::Base::Base
Base(size_t dim=1)
primary constructor
Definition NoiseModel.h:65
│ │ │ │ +
gtsam::noiseModel::Gaussian
Gaussian implements the mathematical model |R*x|^2 = |y|^2 with R'*R=inv(Sigma) where y = whiten(x) =...
Definition NoiseModel.h:162
│ │ │ │ +
gtsam::noiseModel::Gaussian::R
virtual Matrix R() const
Return R itself, but note that Whiten(H) is cheaper than R*H.
Definition NoiseModel.h:255
│ │ │ │ +
gtsam::noiseModel::Gaussian::Gaussian
Gaussian(size_t dim=1, const boost::optional< Matrix > &sqrt_information=boost::none)
constructor takes square root information matrix
Definition NoiseModel.h:186
│ │ │ │ +
gtsam::noiseModel::Gaussian::sqrt_information_
boost::optional< Matrix > sqrt_information_
Matrix square root of information matrix (R)
Definition NoiseModel.h:167
│ │ │ │ +
gtsam::noiseModel::Diagonal
A diagonal noise model implements a diagonal covariance matrix, with the elements of the diagonal spe...
Definition NoiseModel.h:281
│ │ │ │ +
gtsam::noiseModel::Diagonal::R
Matrix R() const override
Return R itself, but note that Whiten(H) is cheaper than R*H.
Definition NoiseModel.h:352
│ │ │ │ +
gtsam::noiseModel::Diagonal::sigmas_
Vector sigmas_
Standard deviations (sigmas), their inverse and inverse square (weights/precisions) These are all com...
Definition NoiseModel.h:289
│ │ │ │ +
gtsam::noiseModel::Diagonal::sigma
double sigma(size_t i) const
Return standard deviations (sqrt of diagonal)
Definition NoiseModel.h:335
│ │ │ │ +
gtsam::noiseModel::Diagonal::invsigmas
const Vector & invsigmas() const
Return sqrt precisions.
Definition NoiseModel.h:340
│ │ │ │ +
gtsam::noiseModel::Diagonal::sigmas
Vector sigmas() const override
Calculate standard deviations.
Definition NoiseModel.h:325
│ │ │ │ +
gtsam::noiseModel::Diagonal::precisions
const Vector & precisions() const
Return precisions.
Definition NoiseModel.h:346
│ │ │ │ +
gtsam::noiseModel::Constrained
A Constrained constrained model is a specialization of Diagonal which allows some or all of the sigma...
Definition NoiseModel.h:381
│ │ │ │ +
gtsam::noiseModel::Constrained::isConstrained
bool isConstrained() const override
true if a constrained noise mode, saves slow/clumsy dynamic casting
Definition NoiseModel.h:409
│ │ │ │ +
gtsam::noiseModel::Constrained::All
static shared_ptr All(size_t dim, const Vector &mu)
Fully constrained variations.
Definition NoiseModel.h:462
│ │ │ │ +
gtsam::noiseModel::Constrained::All
static shared_ptr All(size_t dim, double mu)
Fully constrained variations with a mu parameter.
Definition NoiseModel.h:467
│ │ │ │ +
gtsam::noiseModel::Constrained::All
static shared_ptr All(size_t dim)
Fully constrained variations.
Definition NoiseModel.h:457
│ │ │ │ +
gtsam::noiseModel::Constrained::mu
const Vector & mu() const
Access mu as a vector.
Definition NoiseModel.h:415
│ │ │ │ +
gtsam::noiseModel::Constrained::mu_
Vector mu_
Penalty function weight - needs to be large enough to dominate soft constraints.
Definition NoiseModel.h:385
│ │ │ │ +
gtsam::noiseModel::Isotropic
An isotropic noise model corresponds to a scaled diagonal covariance To construct,...
Definition NoiseModel.h:516
│ │ │ │ +
gtsam::noiseModel::Isotropic::sigma
double sigma() const
Return standard deviation.
Definition NoiseModel.h:565
│ │ │ │ +
gtsam::noiseModel::Isotropic::Precision
static shared_ptr Precision(size_t dim, double precision, bool smart=true)
An isotropic noise model created by specifying a precision.
Definition NoiseModel.h:549
│ │ │ │ +
gtsam::noiseModel::Isotropic::Isotropic
Isotropic(size_t dim, double sigma)
protected constructor takes sigma
Definition NoiseModel.h:521
│ │ │ │ +
gtsam::noiseModel::Unit
Unit: i.i.d.
Definition NoiseModel.h:584
│ │ │ │ +
gtsam::noiseModel::Unit::WhitenInPlace
void WhitenInPlace(Eigen::Block< Matrix >) const override
In-place version.
Definition NoiseModel.h:610
│ │ │ │ +
gtsam::noiseModel::Unit::unwhiten
Vector unwhiten(const Vector &v) const override
Unwhiten an error vector.
Definition NoiseModel.h:607
│ │ │ │ +
gtsam::noiseModel::Unit::whitenInPlace
void whitenInPlace(Eigen::Block< Vector > &) const override
in-place whiten, override if can be done more efficiently
Definition NoiseModel.h:613
│ │ │ │ +
gtsam::noiseModel::Unit::isUnit
bool isUnit() const override
true if a unit noise model, saves slow/clumsy dynamic casting
Definition NoiseModel.h:602
│ │ │ │ +
gtsam::noiseModel::Unit::Unit
Unit(size_t dim=1)
constructor for serialization
Definition NoiseModel.h:590
│ │ │ │ +
gtsam::noiseModel::Unit::whiten
Vector whiten(const Vector &v) const override
Whiten an error vector.
Definition NoiseModel.h:606
│ │ │ │ +
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Create a unit covariance noise model.
Definition NoiseModel.h:597
│ │ │ │ +
gtsam::noiseModel::Unit::unwhitenInPlace
void unwhitenInPlace(Vector &) const override
in-place unwhiten, override if can be done more efficiently
Definition NoiseModel.h:612
│ │ │ │ +
gtsam::noiseModel::Unit::unwhitenInPlace
void unwhitenInPlace(Eigen::Block< Vector > &) const override
in-place unwhiten, override if can be done more efficiently
Definition NoiseModel.h:614
│ │ │ │ +
gtsam::noiseModel::Unit::whitenInPlace
void whitenInPlace(Vector &) const override
in-place whiten, override if can be done more efficiently
Definition NoiseModel.h:611
│ │ │ │ +
gtsam::noiseModel::Unit::WhitenInPlace
void WhitenInPlace(Matrix &) const override
In-place version.
Definition NoiseModel.h:609
│ │ │ │ +
gtsam::noiseModel::Unit::Whiten
Matrix Whiten(const Matrix &H) const override
Whiten a matrix.
Definition NoiseModel.h:608
│ │ │ │ +
gtsam::noiseModel::Robust
Base class for robust error models The robust M-estimators above simply tell us how to re-weight the ...
Definition NoiseModel.h:642
│ │ │ │ +
gtsam::noiseModel::Robust::robust
const RobustModel::shared_ptr & robust() const
Return the contained robust error function.
Definition NoiseModel.h:669
│ │ │ │ +
gtsam::noiseModel::Robust::Robust
Robust()
Default Constructor for serialization.
Definition NoiseModel.h:656
│ │ │ │ +
gtsam::noiseModel::Robust::Robust
Robust(const RobustModel::shared_ptr robust, const NoiseModel::shared_ptr noise)
Constructor.
Definition NoiseModel.h:659
│ │ │ │ +
gtsam::noiseModel::Robust::squaredMahalanobisDistance
double squaredMahalanobisDistance(const Vector &v) const override
Squared Mahalanobis distance v'*R'*R*v = <R*v,R*v>
Definition NoiseModel.h:688
│ │ │ │ +
gtsam::noiseModel::Robust::noise_
const NoiseModel::shared_ptr noise_
noise model used
Definition NoiseModel.h:651
│ │ │ │ +
gtsam::noiseModel::Robust::loss
double loss(const double squared_distance) const override
Compute loss from the m-estimator using the Mahalanobis distance.
Definition NoiseModel.h:682
│ │ │ │ +
gtsam::noiseModel::Robust::unwhiten
Vector unwhiten(const Vector &) const override
Unwhiten an error vector.
Definition NoiseModel.h:679
│ │ │ │ +
gtsam::noiseModel::Robust::Whiten
Matrix Whiten(const Matrix &A) const override
Whiten a matrix.
Definition NoiseModel.h:677
│ │ │ │ +
gtsam::noiseModel::Robust::robust_
const RobustModel::shared_ptr robust_
robust error function used
Definition NoiseModel.h:650
│ │ │ │ +
gtsam::noiseModel::Robust::~Robust
~Robust() override
Destructor.
Definition NoiseModel.h:663
│ │ │ │ +
gtsam::noiseModel::Robust::noise
const NoiseModel::shared_ptr & noise() const
Return the contained noise model.
Definition NoiseModel.h:672
│ │ │ │ +
gtsam::noiseModel::Robust::whiten
Vector whiten(const Vector &v) const override
Whiten an error vector.
Definition NoiseModel.h:675
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,14 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -JacobianFactor-inl.h │ │ │ │ │ +NoiseModel.h │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ @@ -15,169 +16,790 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 19#pragma once │ │ │ │ │ 20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_l_i_n_e_a_r_E_x_c_e_p_t_i_o_n_s_._h> │ │ │ │ │ -22 │ │ │ │ │ -23namespace _g_t_s_a_m { │ │ │ │ │ -24 │ │ │ │ │ -25 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -26 template │ │ │ │ │ -_2_7 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r(const TERMS&terms, const Vector &b, const │ │ │ │ │ -SharedDiagonal& model) │ │ │ │ │ -28 { │ │ │ │ │ -29 _f_i_l_l_T_e_r_m_s(terms, b, model); │ │ │ │ │ -30 } │ │ │ │ │ +21#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ +23#include │ │ │ │ │ +24#include │ │ │ │ │ +25 │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28#include │ │ │ │ │ +29#include │ │ │ │ │ +30#include │ │ │ │ │ 31 │ │ │ │ │ -32 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -33 template │ │ │ │ │ -_3_4 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r( │ │ │ │ │ -35 const KEYS& keys, const _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x& augmentedMatrix, const │ │ │ │ │ -SharedDiagonal& model) : │ │ │ │ │ -36 _B_a_s_e(keys), Ab_(augmentedMatrix) │ │ │ │ │ -37 { │ │ │ │ │ -38 // Check noise model dimension │ │ │ │ │ -39 if(model && (_D_e_n_s_e_I_n_d_e_x)model->dim() != augmentedMatrix._r_o_w_s()) │ │ │ │ │ -40 throw _I_n_v_a_l_i_d_N_o_i_s_e_M_o_d_e_l(augmentedMatrix._r_o_w_s(), model->dim()); │ │ │ │ │ -41 │ │ │ │ │ -42 // Check number of variables │ │ │ │ │ -43 if((_D_e_n_s_e_I_n_d_e_x)_B_a_s_e_:_:_k_e_y_s__._s_i_z_e() != augmentedMatrix._n_B_l_o_c_k_s() - 1) │ │ │ │ │ -44 throw std::invalid_argument( │ │ │ │ │ -45 "Error in JacobianFactor constructor input. Number of provided keys plus\n" │ │ │ │ │ -46 "one for the RHS vector must equal the number of provided matrix blocks."); │ │ │ │ │ -47 │ │ │ │ │ -48 // Check RHS dimension │ │ │ │ │ -49 if(augmentedMatrix(augmentedMatrix._n_B_l_o_c_k_s() - 1)._c_o_l_s() != 1) │ │ │ │ │ -50 throw std::invalid_argument( │ │ │ │ │ -51 "Error in JacobianFactor constructor input. The last provided matrix │ │ │ │ │ -block\n" │ │ │ │ │ -52 "must be the RHS vector, but the last provided block had more than one │ │ │ │ │ -column."); │ │ │ │ │ -53 │ │ │ │ │ -54 // Take noise model │ │ │ │ │ -55 model_ = model; │ │ │ │ │ -56 } │ │ │ │ │ +32namespace _g_t_s_a_m { │ │ │ │ │ +33 │ │ │ │ │ +35 namespace noiseModel { │ │ │ │ │ +36 │ │ │ │ │ +37 // Forward declaration │ │ │ │ │ +38 class Gaussian; │ │ │ │ │ +39 class Diagonal; │ │ │ │ │ +40 class Constrained; │ │ │ │ │ +41 class Isotropic; │ │ │ │ │ +42 class Unit; │ │ │ │ │ +43 class RobustModel; │ │ │ │ │ +44 │ │ │ │ │ +45 //-------------------------------------------------------------------------- │ │ │ │ │ +------------- │ │ │ │ │ +46 │ │ │ │ │ +_5_3 class GTSAM_EXPORT _B_a_s_e { │ │ │ │ │ +54 │ │ │ │ │ +55 public: │ │ │ │ │ +56 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ 57 │ │ │ │ │ -58 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -59 template │ │ │ │ │ -_6_0 void _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_f_i_l_l_T_e_r_m_s(const TERMS& terms, const Vector& b, const │ │ │ │ │ -SharedDiagonal& noiseModel) │ │ │ │ │ -61 { │ │ │ │ │ -62 // Check noise model dimension │ │ │ │ │ -63 if(noiseModel && (_D_e_n_s_e_I_n_d_e_x)noiseModel->dim() != b.size()) │ │ │ │ │ -64 throw _I_n_v_a_l_i_d_N_o_i_s_e_M_o_d_e_l(b.size(), noiseModel->dim()); │ │ │ │ │ -65 │ │ │ │ │ -66 // Resize base class key vector │ │ │ │ │ -67 _B_a_s_e_:_:_k_e_y_s__.resize(terms.size()); │ │ │ │ │ -68 │ │ │ │ │ -69 // Get dimensions of matrices │ │ │ │ │ -70 std::vector dimensions; │ │ │ │ │ -71 dimensions.reserve(terms.size()); │ │ │ │ │ -72 for(typename TERMS::const_iterator it = terms.begin(); it != terms.end(); │ │ │ │ │ -++it) { │ │ │ │ │ -73 const std::pair& term = *it; │ │ │ │ │ -74 const Matrix& Ai = term.second; │ │ │ │ │ -75 dimensions.push_back(Ai.cols()); │ │ │ │ │ -76 } │ │ │ │ │ -77 │ │ │ │ │ -78 // Construct block matrix │ │ │ │ │ -79 Ab_ = _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x(dimensions, b.size(), true); │ │ │ │ │ +58 protected: │ │ │ │ │ +59 │ │ │ │ │ +60 size_t dim_; │ │ │ │ │ +61 │ │ │ │ │ +62 public: │ │ │ │ │ +63 │ │ │ │ │ +_6_5 _B_a_s_e(size_t dim = 1):dim_(dim) {} │ │ │ │ │ +66 virtual _~_B_a_s_e() {} │ │ │ │ │ +67 │ │ │ │ │ +_6_9 virtual bool _i_s_C_o_n_s_t_r_a_i_n_e_d() const { return false; } // default false │ │ │ │ │ +70 │ │ │ │ │ +_7_2 virtual bool _i_s_U_n_i_t() const { return false; } // default false │ │ │ │ │ +73 │ │ │ │ │ +_7_5 inline size_t _d_i_m() const { return dim_;} │ │ │ │ │ +76 │ │ │ │ │ +77 virtual void _p_r_i_n_t(const std::string& name = "") const = 0; │ │ │ │ │ +78 │ │ │ │ │ +79 virtual bool _e_q_u_a_l_s(const _B_a_s_e& expected, double tol=1e-9) const = 0; │ │ │ │ │ 80 │ │ │ │ │ -81 // Check and add terms │ │ │ │ │ -82 _D_e_n_s_e_I_n_d_e_x i = 0; // For block index │ │ │ │ │ -83 for(typename TERMS::const_iterator it = terms.begin(); it != terms.end(); │ │ │ │ │ -++it) { │ │ │ │ │ -84 const std::pair& term = *it; │ │ │ │ │ -85 _K_e_y key = term.first; │ │ │ │ │ -86 const Matrix& Ai = term.second; │ │ │ │ │ -87 │ │ │ │ │ -88 // Check block rows │ │ │ │ │ -89 if(Ai.rows() != Ab_._r_o_w_s()) │ │ │ │ │ -90 throw _I_n_v_a_l_i_d_M_a_t_r_i_x_B_l_o_c_k(Ab_._r_o_w_s(), Ai.rows()); │ │ │ │ │ -91 │ │ │ │ │ -92 // Assign key and matrix │ │ │ │ │ -93 _B_a_s_e_:_:_k_e_y_s__[i] = key; │ │ │ │ │ -94 Ab_(i) = Ai; │ │ │ │ │ +82 virtual Vector sigmas() const; │ │ │ │ │ +83 │ │ │ │ │ +_8_5 virtual Vector _w_h_i_t_e_n(const Vector& v) const = 0; │ │ │ │ │ +86 │ │ │ │ │ +_8_8 virtual Matrix _W_h_i_t_e_n(const Matrix& H) const = 0; │ │ │ │ │ +89 │ │ │ │ │ +_9_1 virtual Vector _u_n_w_h_i_t_e_n(const Vector& v) const = 0; │ │ │ │ │ +92 │ │ │ │ │ +94 virtual double squaredMahalanobisDistance(const Vector& v) const; │ │ │ │ │ 95 │ │ │ │ │ -96 // Increment block index │ │ │ │ │ -97 ++ i; │ │ │ │ │ -98 } │ │ │ │ │ -99 │ │ │ │ │ -100 // Assign RHS vector │ │ │ │ │ -101 _g_e_t_b() = b; │ │ │ │ │ -102 │ │ │ │ │ -103 // Assign noise model │ │ │ │ │ -104 model_ = noiseModel; │ │ │ │ │ -105 } │ │ │ │ │ -106 │ │ │ │ │ -107} // gtsam │ │ │ │ │ -108 │ │ │ │ │ -_l_i_n_e_a_r_E_x_c_e_p_t_i_o_n_s_._h │ │ │ │ │ -Exceptions that may be thrown by linear solver components. │ │ │ │ │ +_9_7 virtual double _m_a_h_a_l_a_n_o_b_i_s_D_i_s_t_a_n_c_e(const Vector& v) const { │ │ │ │ │ +98 return std::sqrt(squaredMahalanobisDistance(v)); │ │ │ │ │ +99 } │ │ │ │ │ +100 │ │ │ │ │ +_1_0_2 virtual double _l_o_s_s(const double squared_distance) const { │ │ │ │ │ +103 return 0.5 * squared_distance; │ │ │ │ │ +104 } │ │ │ │ │ +105 │ │ │ │ │ +106 virtual void WhitenSystem(std::vector& A, Vector& b) const = 0; │ │ │ │ │ +107 virtual void WhitenSystem(Matrix& A, Vector& b) const = 0; │ │ │ │ │ +108 virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const = 0; │ │ │ │ │ +109 virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) │ │ │ │ │ +const = 0; │ │ │ │ │ +110 │ │ │ │ │ +_1_1_2 virtual void _w_h_i_t_e_n_I_n_P_l_a_c_e(Vector& v) const { │ │ │ │ │ +113 v = whiten(v); │ │ │ │ │ +114 } │ │ │ │ │ +115 │ │ │ │ │ +_1_1_7 virtual void _u_n_w_h_i_t_e_n_I_n_P_l_a_c_e(Vector& v) const { │ │ │ │ │ +118 v = unwhiten(v); │ │ │ │ │ +119 } │ │ │ │ │ +120 │ │ │ │ │ +_1_2_2 virtual void _w_h_i_t_e_n_I_n_P_l_a_c_e(Eigen::Block& v) const { │ │ │ │ │ +123 v = whiten(v); │ │ │ │ │ +124 } │ │ │ │ │ +125 │ │ │ │ │ +_1_2_7 virtual void _u_n_w_h_i_t_e_n_I_n_P_l_a_c_e(Eigen::Block& v) const { │ │ │ │ │ +128 v = unwhiten(v); │ │ │ │ │ +129 } │ │ │ │ │ +130 │ │ │ │ │ +_1_3_2 virtual Vector _u_n_w_e_i_g_h_t_e_d_W_h_i_t_e_n(const Vector& v) const { │ │ │ │ │ +133 return whiten(v); │ │ │ │ │ +134 } │ │ │ │ │ +135 │ │ │ │ │ +_1_3_7 virtual double _w_e_i_g_h_t(const Vector& v) const { return 1.0; } │ │ │ │ │ +138 │ │ │ │ │ +139 private: │ │ │ │ │ +_1_4_1 friend class boost::serialization::access; │ │ │ │ │ +142 template │ │ │ │ │ +143 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +144 ar & BOOST_SERIALIZATION_NVP(dim_); │ │ │ │ │ +145 } │ │ │ │ │ +146 }; │ │ │ │ │ +147 │ │ │ │ │ +148 //------------------------------------------------------------------------- │ │ │ │ │ +-------------- │ │ │ │ │ +149 │ │ │ │ │ +_1_6_2 class GTSAM_EXPORT _G_a_u_s_s_i_a_n: public _B_a_s_e { │ │ │ │ │ +163 │ │ │ │ │ +164 protected: │ │ │ │ │ +165 │ │ │ │ │ +_1_6_7 boost::optional _s_q_r_t___i_n_f_o_r_m_a_t_i_o_n__; │ │ │ │ │ +168 │ │ │ │ │ +169 private: │ │ │ │ │ +170 │ │ │ │ │ +174 const Matrix& thisR() const { │ │ │ │ │ +175 // should never happen │ │ │ │ │ +176 if (!sqrt_information_) throw std::runtime_error("Gaussian: has no R │ │ │ │ │ +matrix"); │ │ │ │ │ +177 return *sqrt_information_; │ │ │ │ │ +178 } │ │ │ │ │ +179 │ │ │ │ │ +180 │ │ │ │ │ +181 public: │ │ │ │ │ +182 │ │ │ │ │ +183 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +184 │ │ │ │ │ +_1_8_6 _G_a_u_s_s_i_a_n(size_t dim = 1, │ │ │ │ │ +187 const boost::optional& sqrt_information = boost::none) │ │ │ │ │ +188 : _B_a_s_e(dim), sqrt_information_(sqrt_information) {} │ │ │ │ │ +189 │ │ │ │ │ +190 _~_G_a_u_s_s_i_a_n() override {} │ │ │ │ │ +191 │ │ │ │ │ +197 static shared_ptr SqrtInformation(const Matrix& R, bool smart = true); │ │ │ │ │ +198 │ │ │ │ │ +204 static shared_ptr Information(const Matrix& M, bool smart = true); │ │ │ │ │ +205 │ │ │ │ │ +211 static shared_ptr Covariance(const Matrix& covariance, bool smart = true); │ │ │ │ │ +212 │ │ │ │ │ +213 void _p_r_i_n_t(const std::string& name) const override; │ │ │ │ │ +214 bool _e_q_u_a_l_s(const Base& expected, double tol=1e-9) const override; │ │ │ │ │ +215 Vector sigmas() const override; │ │ │ │ │ +216 Vector whiten(const Vector& v) const override; │ │ │ │ │ +217 Vector unwhiten(const Vector& v) const override; │ │ │ │ │ +218 │ │ │ │ │ +223 Matrix Whiten(const Matrix& H) const override; │ │ │ │ │ +224 │ │ │ │ │ +228 virtual void WhitenInPlace(Matrix& H) const; │ │ │ │ │ +229 │ │ │ │ │ +233 virtual void WhitenInPlace(Eigen::Block H) const; │ │ │ │ │ +234 │ │ │ │ │ +238 void WhitenSystem(std::vector& A, Vector& b) const override; │ │ │ │ │ +239 void WhitenSystem(Matrix& A, Vector& b) const override; │ │ │ │ │ +240 void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const override; │ │ │ │ │ +241 void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const │ │ │ │ │ +override; │ │ │ │ │ +242 │ │ │ │ │ +252 virtual boost::shared_ptr QR(Matrix& Ab) const; │ │ │ │ │ +253 │ │ │ │ │ +_2_5_5 virtual Matrix _R() const { return thisR();} │ │ │ │ │ +256 │ │ │ │ │ +258 virtual Matrix information() const; │ │ │ │ │ +259 │ │ │ │ │ +261 virtual Matrix covariance() const; │ │ │ │ │ +262 │ │ │ │ │ +263 private: │ │ │ │ │ +_2_6_5 friend class boost::serialization::access; │ │ │ │ │ +266 template │ │ │ │ │ +267 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +268 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +269 ar & BOOST_SERIALIZATION_NVP(sqrt_information_); │ │ │ │ │ +270 } │ │ │ │ │ +271 │ │ │ │ │ +272 }; // Gaussian │ │ │ │ │ +273 │ │ │ │ │ +274 //------------------------------------------------------------------------- │ │ │ │ │ +-------------- │ │ │ │ │ +275 │ │ │ │ │ +_2_8_1 class GTSAM_EXPORT _D_i_a_g_o_n_a_l : public _G_a_u_s_s_i_a_n { │ │ │ │ │ +282 protected: │ │ │ │ │ +283 │ │ │ │ │ +_2_8_9 Vector _s_i_g_m_a_s__, invsigmas_, precisions_; │ │ │ │ │ +290 │ │ │ │ │ +291 protected: │ │ │ │ │ +292 │ │ │ │ │ +294 _D_i_a_g_o_n_a_l(const Vector& sigmas); │ │ │ │ │ +295 │ │ │ │ │ +296 public: │ │ │ │ │ +298 _D_i_a_g_o_n_a_l(); │ │ │ │ │ +299 │ │ │ │ │ +300 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +301 │ │ │ │ │ +302 _~_D_i_a_g_o_n_a_l() override {} │ │ │ │ │ +303 │ │ │ │ │ +308 static shared_ptr Sigmas(const Vector& sigmas, bool smart = true); │ │ │ │ │ +309 │ │ │ │ │ +316 static shared_ptr Variances(const Vector& variances, bool smart = true); │ │ │ │ │ +317 │ │ │ │ │ +322 static shared_ptr Precisions(const Vector& precisions, bool smart = true); │ │ │ │ │ +323 │ │ │ │ │ +324 void _p_r_i_n_t(const std::string& name) const override; │ │ │ │ │ +_3_2_5 Vector _s_i_g_m_a_s() const override { return sigmas_; } │ │ │ │ │ +326 Vector whiten(const Vector& v) const override; │ │ │ │ │ +327 Vector unwhiten(const Vector& v) const override; │ │ │ │ │ +328 Matrix Whiten(const Matrix& H) const override; │ │ │ │ │ +329 void WhitenInPlace(Matrix& H) const override; │ │ │ │ │ +330 void WhitenInPlace(Eigen::Block H) const override; │ │ │ │ │ +331 │ │ │ │ │ +_3_3_5 inline double _s_i_g_m_a(size_t i) const { return sigmas_(i); } │ │ │ │ │ +336 │ │ │ │ │ +_3_4_0 inline const Vector& _i_n_v_s_i_g_m_a_s() const { return invsigmas_; } │ │ │ │ │ +341 inline double invsigma(size_t i) const {return invsigmas_(i);} │ │ │ │ │ +342 │ │ │ │ │ +_3_4_6 inline const Vector& _p_r_e_c_i_s_i_o_n_s() const { return precisions_; } │ │ │ │ │ +347 inline double precision(size_t i) const {return precisions_(i);} │ │ │ │ │ +348 │ │ │ │ │ +_3_5_2 Matrix _R() const override { │ │ │ │ │ +353 return invsigmas().asDiagonal(); │ │ │ │ │ +354 } │ │ │ │ │ +355 │ │ │ │ │ +356 private: │ │ │ │ │ +_3_5_8 friend class boost::serialization::access; │ │ │ │ │ +359 template │ │ │ │ │ +360 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +361 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_G_a_u_s_s_i_a_n); │ │ │ │ │ +362 ar & BOOST_SERIALIZATION_NVP(sigmas_); │ │ │ │ │ +363 ar & BOOST_SERIALIZATION_NVP(invsigmas_); │ │ │ │ │ +364 } │ │ │ │ │ +365 }; // Diagonal │ │ │ │ │ +366 │ │ │ │ │ +367 //------------------------------------------------------------------------- │ │ │ │ │ +-------------- │ │ │ │ │ +368 │ │ │ │ │ +_3_8_1 class GTSAM_EXPORT _C_o_n_s_t_r_a_i_n_e_d : public _D_i_a_g_o_n_a_l { │ │ │ │ │ +382 protected: │ │ │ │ │ +383 │ │ │ │ │ +384 // Sigmas are contained in the base class │ │ │ │ │ +_3_8_5 Vector _m_u__; │ │ │ │ │ +386 │ │ │ │ │ +392 _C_o_n_s_t_r_a_i_n_e_d(const Vector& mu, const Vector& sigmas); │ │ │ │ │ +393 │ │ │ │ │ +394 public: │ │ │ │ │ +395 │ │ │ │ │ +396 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +397 │ │ │ │ │ +404 _C_o_n_s_t_r_a_i_n_e_d(const Vector& sigmas = Z_1x1); │ │ │ │ │ +405 │ │ │ │ │ +406 _~_C_o_n_s_t_r_a_i_n_e_d() override {} │ │ │ │ │ +407 │ │ │ │ │ +_4_0_9 bool _i_s_C_o_n_s_t_r_a_i_n_e_d() const override { return true; } │ │ │ │ │ +410 │ │ │ │ │ +412 bool constrained(size_t i) const; │ │ │ │ │ +413 │ │ │ │ │ +_4_1_5 const Vector& _m_u() const { return mu_; } │ │ │ │ │ +416 │ │ │ │ │ +421 static shared_ptr MixedSigmas(const Vector& mu, const Vector& sigmas); │ │ │ │ │ +422 │ │ │ │ │ +427 static shared_ptr MixedSigmas(const Vector& sigmas); │ │ │ │ │ +428 │ │ │ │ │ +433 static shared_ptr MixedSigmas(double m, const Vector& sigmas); │ │ │ │ │ +434 │ │ │ │ │ +439 static shared_ptr MixedVariances(const Vector& mu, const Vector& │ │ │ │ │ +variances); │ │ │ │ │ +440 static shared_ptr MixedVariances(const Vector& variances); │ │ │ │ │ +441 │ │ │ │ │ +446 static shared_ptr MixedPrecisions(const Vector& mu, const Vector& │ │ │ │ │ +precisions); │ │ │ │ │ +447 static shared_ptr MixedPrecisions(const Vector& precisions); │ │ │ │ │ +448 │ │ │ │ │ +454 double squaredMahalanobisDistance(const Vector& v) const override; │ │ │ │ │ +455 │ │ │ │ │ +_4_5_7 static shared_ptr _A_l_l(size_t dim) { │ │ │ │ │ +458 return shared_ptr(new _C_o_n_s_t_r_a_i_n_e_d(Vector::Constant(dim, 1000.0), Vector:: │ │ │ │ │ +Constant(dim,0))); │ │ │ │ │ +459 } │ │ │ │ │ +460 │ │ │ │ │ +_4_6_2 static shared_ptr _A_l_l(size_t dim, const Vector& mu) { │ │ │ │ │ +463 return shared_ptr(new _C_o_n_s_t_r_a_i_n_e_d(mu, Vector::Constant(dim,0))); │ │ │ │ │ +464 } │ │ │ │ │ +465 │ │ │ │ │ +_4_6_7 static shared_ptr _A_l_l(size_t dim, double mu) { │ │ │ │ │ +468 return shared_ptr(new _C_o_n_s_t_r_a_i_n_e_d(Vector::Constant(dim, mu), Vector:: │ │ │ │ │ +Constant(dim,0))); │ │ │ │ │ +469 } │ │ │ │ │ +470 │ │ │ │ │ +471 void _p_r_i_n_t(const std::string& name) const override; │ │ │ │ │ +472 │ │ │ │ │ +474 Vector whiten(const Vector& v) const override; │ │ │ │ │ +475 │ │ │ │ │ +478 Matrix Whiten(const Matrix& H) const override; │ │ │ │ │ +479 void WhitenInPlace(Matrix& H) const override; │ │ │ │ │ +480 void WhitenInPlace(Eigen::Block H) const override; │ │ │ │ │ +481 │ │ │ │ │ +491 Diagonal::shared_ptr QR(Matrix& Ab) const override; │ │ │ │ │ +492 │ │ │ │ │ +497 shared_ptr unit() const; │ │ │ │ │ +498 │ │ │ │ │ +499 private: │ │ │ │ │ +_5_0_1 friend class boost::serialization::access; │ │ │ │ │ +502 template │ │ │ │ │ +503 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +504 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_D_i_a_g_o_n_a_l); │ │ │ │ │ +505 ar & BOOST_SERIALIZATION_NVP(mu_); │ │ │ │ │ +506 } │ │ │ │ │ +507 │ │ │ │ │ +508 }; // Constrained │ │ │ │ │ +509 │ │ │ │ │ +510 //------------------------------------------------------------------------- │ │ │ │ │ +-------------- │ │ │ │ │ +511 │ │ │ │ │ +_5_1_6 class GTSAM_EXPORT _I_s_o_t_r_o_p_i_c : public _D_i_a_g_o_n_a_l { │ │ │ │ │ +517 protected: │ │ │ │ │ +518 double sigma_, invsigma_; │ │ │ │ │ +519 │ │ │ │ │ +_5_2_1 _I_s_o_t_r_o_p_i_c(size_t dim, double sigma) : │ │ │ │ │ +522 _D_i_a_g_o_n_a_l(Vector::Constant(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) │ │ │ │ │ +{} │ │ │ │ │ +523 │ │ │ │ │ +524 public: │ │ │ │ │ +525 │ │ │ │ │ +526 /* dummy constructor to allow for serialization */ │ │ │ │ │ +527 _I_s_o_t_r_o_p_i_c() : _D_i_a_g_o_n_a_l(Vector1::Constant(1.0)),sigma_(1.0),invsigma_(1.0) │ │ │ │ │ +{} │ │ │ │ │ +528 │ │ │ │ │ +529 ~Isotropic() override {} │ │ │ │ │ +530 │ │ │ │ │ +531 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +532 │ │ │ │ │ +536 static shared_ptr Sigma(size_t dim, double sigma, bool smart = true); │ │ │ │ │ +537 │ │ │ │ │ +544 static shared_ptr Variance(size_t dim, double variance, bool smart = true); │ │ │ │ │ +545 │ │ │ │ │ +_5_4_9 static shared_ptr _P_r_e_c_i_s_i_o_n(size_t dim, double precision, bool smart = │ │ │ │ │ +true) { │ │ │ │ │ +550 return Variance(dim, 1.0/precision, smart); │ │ │ │ │ +551 } │ │ │ │ │ +552 │ │ │ │ │ +553 void _p_r_i_n_t(const std::string& name) const override; │ │ │ │ │ +554 double squaredMahalanobisDistance(const Vector& v) const override; │ │ │ │ │ +555 Vector whiten(const Vector& v) const override; │ │ │ │ │ +556 Vector unwhiten(const Vector& v) const override; │ │ │ │ │ +557 Matrix Whiten(const Matrix& H) const override; │ │ │ │ │ +558 void WhitenInPlace(Matrix& H) const override; │ │ │ │ │ +559 void whitenInPlace(Vector& v) const override; │ │ │ │ │ +560 void WhitenInPlace(Eigen::Block H) const override; │ │ │ │ │ +561 │ │ │ │ │ +_5_6_5 inline double _s_i_g_m_a() const { return sigma_; } │ │ │ │ │ +566 │ │ │ │ │ +567 private: │ │ │ │ │ +_5_6_9 friend class boost::serialization::access; │ │ │ │ │ +570 template │ │ │ │ │ +571 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +572 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_D_i_a_g_o_n_a_l); │ │ │ │ │ +573 ar & BOOST_SERIALIZATION_NVP(sigma_); │ │ │ │ │ +574 ar & BOOST_SERIALIZATION_NVP(invsigma_); │ │ │ │ │ +575 } │ │ │ │ │ +576 │ │ │ │ │ +577 }; │ │ │ │ │ +578 │ │ │ │ │ +579 //------------------------------------------------------------------------- │ │ │ │ │ +-------------- │ │ │ │ │ +580 │ │ │ │ │ +_5_8_4 class GTSAM_EXPORT _U_n_i_t : public _I_s_o_t_r_o_p_i_c { │ │ │ │ │ +585 public: │ │ │ │ │ +586 │ │ │ │ │ +587 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +588 │ │ │ │ │ +_5_9_0 _U_n_i_t(size_t dim=1): _I_s_o_t_r_o_p_i_c(dim,1.0) {} │ │ │ │ │ +591 │ │ │ │ │ +592 _~_U_n_i_t() override {} │ │ │ │ │ +593 │ │ │ │ │ +_5_9_7 static shared_ptr _C_r_e_a_t_e(size_t dim) { │ │ │ │ │ +598 return shared_ptr(new _U_n_i_t(dim)); │ │ │ │ │ +599 } │ │ │ │ │ +600 │ │ │ │ │ +_6_0_2 bool _i_s_U_n_i_t() const override { return true; } │ │ │ │ │ +603 │ │ │ │ │ +604 void _p_r_i_n_t(const std::string& name) const override; │ │ │ │ │ +605 double squaredMahalanobisDistance(const Vector& v) const override; │ │ │ │ │ +_6_0_6 Vector _w_h_i_t_e_n(const Vector& v) const override { return v; } │ │ │ │ │ +_6_0_7 Vector _u_n_w_h_i_t_e_n(const Vector& v) const override { return v; } │ │ │ │ │ +_6_0_8 Matrix _W_h_i_t_e_n(const Matrix& H) const override { return H; } │ │ │ │ │ +_6_0_9 void _W_h_i_t_e_n_I_n_P_l_a_c_e(Matrix& /*H*/) const override {} │ │ │ │ │ +_6_1_0 void _W_h_i_t_e_n_I_n_P_l_a_c_e(Eigen::Block /*H*/) const override {} │ │ │ │ │ +_6_1_1 void _w_h_i_t_e_n_I_n_P_l_a_c_e(Vector& /*v*/) const override {} │ │ │ │ │ +_6_1_2 void _u_n_w_h_i_t_e_n_I_n_P_l_a_c_e(Vector& /*v*/) const override {} │ │ │ │ │ +_6_1_3 void _w_h_i_t_e_n_I_n_P_l_a_c_e(Eigen::Block& /*v*/) const override {} │ │ │ │ │ +_6_1_4 void _u_n_w_h_i_t_e_n_I_n_P_l_a_c_e(Eigen::Block& /*v*/) const override {} │ │ │ │ │ +615 │ │ │ │ │ +616 private: │ │ │ │ │ +_6_1_8 friend class boost::serialization::access; │ │ │ │ │ +619 template │ │ │ │ │ +620 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +621 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_I_s_o_t_r_o_p_i_c); │ │ │ │ │ +622 } │ │ │ │ │ +623 }; │ │ │ │ │ +624 │ │ │ │ │ +_6_4_2 class GTSAM_EXPORT _R_o_b_u_s_t : public _B_a_s_e { │ │ │ │ │ +643 public: │ │ │ │ │ +644 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +645 │ │ │ │ │ +646 protected: │ │ │ │ │ +647 typedef _m_E_s_t_i_m_a_t_o_r_:_:_B_a_s_e _R_o_b_u_s_t_M_o_d_e_l; │ │ │ │ │ +648 typedef _n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e _N_o_i_s_e_M_o_d_e_l; │ │ │ │ │ +649 │ │ │ │ │ +_6_5_0 const RobustModel::shared_ptr _r_o_b_u_s_t__; │ │ │ │ │ +_6_5_1 const NoiseModel::shared_ptr _n_o_i_s_e__; │ │ │ │ │ +652 │ │ │ │ │ +653 public: │ │ │ │ │ +654 │ │ │ │ │ +_6_5_6 _R_o_b_u_s_t() {}; │ │ │ │ │ +657 │ │ │ │ │ +_6_5_9 _R_o_b_u_s_t(const RobustModel::shared_ptr robust, const NoiseModel::shared_ptr │ │ │ │ │ +noise) │ │ │ │ │ +660 : _B_a_s_e(noise->dim()), robust_(robust), noise_(noise) {} │ │ │ │ │ +661 │ │ │ │ │ +_6_6_3 _~_R_o_b_u_s_t() override {} │ │ │ │ │ +664 │ │ │ │ │ +665 void _p_r_i_n_t(const std::string& name) const override; │ │ │ │ │ +666 bool _e_q_u_a_l_s(const _B_a_s_e& expected, double tol=1e-9) const override; │ │ │ │ │ +667 │ │ │ │ │ +_6_6_9 const RobustModel::shared_ptr& _r_o_b_u_s_t() const { return robust_; } │ │ │ │ │ +670 │ │ │ │ │ +_6_7_2 const NoiseModel::shared_ptr& _n_o_i_s_e() const { return noise_; } │ │ │ │ │ +673 │ │ │ │ │ +674 // Functions below are dummy but necessary for the noiseModel::Base │ │ │ │ │ +_6_7_5 inline Vector _w_h_i_t_e_n(const Vector& v) const override │ │ │ │ │ +676 { Vector r = v; this->WhitenSystem(r); return r; } │ │ │ │ │ +_6_7_7 inline Matrix _W_h_i_t_e_n(const Matrix& A) const override │ │ │ │ │ +678 { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; } │ │ │ │ │ +_6_7_9 inline Vector _u_n_w_h_i_t_e_n(const Vector& /*v*/) const override │ │ │ │ │ +680 { throw std::invalid_argument("unwhiten is not currently supported for │ │ │ │ │ +robust noise models."); } │ │ │ │ │ +_6_8_2 double _l_o_s_s(const double squared_distance) const override { │ │ │ │ │ +683 return robust_->loss(std::sqrt(squared_distance)); │ │ │ │ │ +684 } │ │ │ │ │ +685 │ │ │ │ │ +686 // NOTE: This is special because in whiten the base version will do the │ │ │ │ │ +reweighting │ │ │ │ │ +687 // which is incorrect! │ │ │ │ │ +_6_8_8 double _s_q_u_a_r_e_d_M_a_h_a_l_a_n_o_b_i_s_D_i_s_t_a_n_c_e(const Vector& v) const override { │ │ │ │ │ +689 return noise_->squaredMahalanobisDistance(v); │ │ │ │ │ +690 } │ │ │ │ │ +691 │ │ │ │ │ +692 // These are really robust iterated re-weighting support functions │ │ │ │ │ +693 virtual void WhitenSystem(Vector& b) const; │ │ │ │ │ +694 void WhitenSystem(std::vector& A, Vector& b) const override; │ │ │ │ │ +695 void WhitenSystem(Matrix& A, Vector& b) const override; │ │ │ │ │ +696 void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const override; │ │ │ │ │ +697 void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const │ │ │ │ │ +override; │ │ │ │ │ +698 │ │ │ │ │ +699 Vector unweightedWhiten(const Vector& v) const override; │ │ │ │ │ +700 double weight(const Vector& v) const override; │ │ │ │ │ +701 │ │ │ │ │ +702 static shared_ptr Create( │ │ │ │ │ +703 const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise); │ │ │ │ │ +704 │ │ │ │ │ +705 private: │ │ │ │ │ +_7_0_7 friend class boost::serialization::access; │ │ │ │ │ +708 template │ │ │ │ │ +709 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +710 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +711 ar & boost::serialization::make_nvp("robust_", const_cast(robust_)); │ │ │ │ │ +712 ar & boost::serialization::make_nvp("noise_", const_cast(noise_)); │ │ │ │ │ +713 } │ │ │ │ │ +714 }; │ │ │ │ │ +715 │ │ │ │ │ +716 // Helper function │ │ │ │ │ +717 GTSAM_EXPORT boost::optional checkIfDiagonal(const Matrix& M); │ │ │ │ │ +718 │ │ │ │ │ +719 } // namespace noiseModel │ │ │ │ │ +720 │ │ │ │ │ +_7_2_4 typedef noiseModel::Base::shared_ptr _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l; │ │ │ │ │ +725 typedef noiseModel::Gaussian::shared_ptr SharedGaussian; │ │ │ │ │ +726 typedef noiseModel::Diagonal::shared_ptr SharedDiagonal; │ │ │ │ │ +727 typedef noiseModel::Constrained::shared_ptr SharedConstrained; │ │ │ │ │ +728 typedef noiseModel::Isotropic::shared_ptr SharedIsotropic; │ │ │ │ │ +729 │ │ │ │ │ +_7_3_1 template<> struct _t_r_a_i_t_s : public │ │ │ │ │ +_T_e_s_t_a_b_l_e {}; │ │ │ │ │ +_7_3_2 template<> struct _t_r_a_i_t_s : public │ │ │ │ │ +_T_e_s_t_a_b_l_e {}; │ │ │ │ │ +_7_3_3 template<> struct _t_r_a_i_t_s : public │ │ │ │ │ +_T_e_s_t_a_b_l_e {}; │ │ │ │ │ +_7_3_4 template<> struct _t_r_a_i_t_s : public │ │ │ │ │ +_T_e_s_t_a_b_l_e {}; │ │ │ │ │ +_7_3_5 template<> struct _t_r_a_i_t_s : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ +736 │ │ │ │ │ +737} //\ namespace gtsam │ │ │ │ │ +738 │ │ │ │ │ +739 │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ +_M_a_t_r_i_x_._h │ │ │ │ │ +typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_n_s_e_I_n_d_e_x │ │ │ │ │ -ptrdiff_t DenseIndex │ │ │ │ │ -The index type for Eigen objects. │ │ │ │ │ -DDeeffiinniittiioonn types.h:106 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ -of vertical blocks. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_r_o_w_s │ │ │ │ │ -DenseIndex rows() const │ │ │ │ │ -Row size. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:115 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_c_o_l_s │ │ │ │ │ -DenseIndex cols() const │ │ │ │ │ -Column size. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:118 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_n_B_l_o_c_k_s │ │ │ │ │ -DenseIndex nBlocks() const │ │ │ │ │ -Block count. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:121 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s__ │ │ │ │ │ -KeyVector keys_ │ │ │ │ │ -The keys involved in this factor. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_s_i_z_e │ │ │ │ │ -size_t size() const │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:157 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -An abstract virtual base class for JacobianFactor and HessianFactor. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactor.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_e_t_b │ │ │ │ │ -const constBVector getb() const │ │ │ │ │ -Get a view of the r.h.s. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:297 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ -JacobianFactor() │ │ │ │ │ -default constructor for I/O │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.cpp:54 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_f_i_l_l_T_e_r_m_s │ │ │ │ │ -void fillTerms(const TERMS &terms, const Vector &b, const SharedDiagonal │ │ │ │ │ -&noiseModel) │ │ │ │ │ -Internal function to fill blocks and set dimensions. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor-inl.h:60 │ │ │ │ │ -_g_t_s_a_m_:_:_I_n_v_a_l_i_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -An exception indicating that the noise model dimension passed into a │ │ │ │ │ -JacobianFactor has a different d... │ │ │ │ │ -DDeeffiinniittiioonn linearExceptions.h:106 │ │ │ │ │ -_g_t_s_a_m_:_:_I_n_v_a_l_i_d_M_a_t_r_i_x_B_l_o_c_k │ │ │ │ │ -An exception indicating that a matrix block passed into a JacobianFactor has a │ │ │ │ │ -different dimensionali... │ │ │ │ │ -DDeeffiinniittiioonn linearExceptions.h:124 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ +Aliases. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_B_a_s_e │ │ │ │ │ +Pure virtual class for all robust error function classes. │ │ │ │ │ +DDeeffiinniittiioonn LossFunctions.h:63 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e │ │ │ │ │ +noiseModel::Base is the abstract base class for all noise models. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:53 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_i_s_C_o_n_s_t_r_a_i_n_e_d │ │ │ │ │ +virtual bool isConstrained() const │ │ │ │ │ +true if a constrained noise model, saves slow/clumsy dynamic casting │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:69 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_w_h_i_t_e_n_I_n_P_l_a_c_e │ │ │ │ │ +virtual void whitenInPlace(Vector &v) const │ │ │ │ │ +in-place whiten, override if can be done more efficiently │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:112 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_d_i_m │ │ │ │ │ +size_t dim() const │ │ │ │ │ +Dimensionality. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_u_n_w_h_i_t_e_n_I_n_P_l_a_c_e │ │ │ │ │ +virtual void unwhitenInPlace(Eigen::Block< Vector > &v) const │ │ │ │ │ +in-place unwhiten, override if can be done more efficiently │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:127 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_w_h_i_t_e_n_I_n_P_l_a_c_e │ │ │ │ │ +virtual void whitenInPlace(Eigen::Block< Vector > &v) const │ │ │ │ │ +in-place whiten, override if can be done more efficiently │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:122 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_w_h_i_t_e_n │ │ │ │ │ +virtual Vector whiten(const Vector &v) const =0 │ │ │ │ │ +Whiten an error vector. │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_m_a_h_a_l_a_n_o_b_i_s_D_i_s_t_a_n_c_e │ │ │ │ │ +virtual double mahalanobisDistance(const Vector &v) const │ │ │ │ │ +Mahalanobis distance. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:97 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_i_s_U_n_i_t │ │ │ │ │ +virtual bool isUnit() const │ │ │ │ │ +true if a unit noise model, saves slow/clumsy dynamic casting │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_w_e_i_g_h_t │ │ │ │ │ +virtual double weight(const Vector &v) const │ │ │ │ │ +get the weight from the effective loss function on residual vector v │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:137 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_u_n_w_e_i_g_h_t_e_d_W_h_i_t_e_n │ │ │ │ │ +virtual Vector unweightedWhiten(const Vector &v) const │ │ │ │ │ +Useful function for robust noise models to get the unweighted but whitened │ │ │ │ │ +error. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:132 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_u_n_w_h_i_t_e_n │ │ │ │ │ +virtual Vector unwhiten(const Vector &v) const =0 │ │ │ │ │ +Unwhiten an error vector. │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_l_o_s_s │ │ │ │ │ +virtual double loss(const double squared_distance) const │ │ │ │ │ +loss function, input is Mahalanobis distance │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:102 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_u_n_w_h_i_t_e_n_I_n_P_l_a_c_e │ │ │ │ │ +virtual void unwhitenInPlace(Vector &v) const │ │ │ │ │ +in-place unwhiten, override if can be done more efficiently │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:117 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_W_h_i_t_e_n │ │ │ │ │ +virtual Matrix Whiten(const Matrix &H) const =0 │ │ │ │ │ +Whiten a matrix. │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_B_a_s_e │ │ │ │ │ +Base(size_t dim=1) │ │ │ │ │ +primary constructor │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_G_a_u_s_s_i_a_n │ │ │ │ │ +Gaussian implements the mathematical model |R*x|^2 = |y|^2 with R'*R=inv(Sigma) │ │ │ │ │ +where y = whiten(x) =... │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:162 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_G_a_u_s_s_i_a_n_:_:_R │ │ │ │ │ +virtual Matrix R() const │ │ │ │ │ +Return R itself, but note that Whiten(H) is cheaper than R*H. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:255 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_G_a_u_s_s_i_a_n_:_:_G_a_u_s_s_i_a_n │ │ │ │ │ +Gaussian(size_t dim=1, const boost::optional< Matrix > │ │ │ │ │ +&sqrt_information=boost::none) │ │ │ │ │ +constructor takes square root information matrix │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:186 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_G_a_u_s_s_i_a_n_:_:_s_q_r_t___i_n_f_o_r_m_a_t_i_o_n__ │ │ │ │ │ +boost::optional< Matrix > sqrt_information_ │ │ │ │ │ +Matrix square root of information matrix (R) │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:167 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_D_i_a_g_o_n_a_l │ │ │ │ │ +A diagonal noise model implements a diagonal covariance matrix, with the │ │ │ │ │ +elements of the diagonal spe... │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:281 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_D_i_a_g_o_n_a_l_:_:_R │ │ │ │ │ +Matrix R() const override │ │ │ │ │ +Return R itself, but note that Whiten(H) is cheaper than R*H. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:352 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_D_i_a_g_o_n_a_l_:_:_s_i_g_m_a_s__ │ │ │ │ │ +Vector sigmas_ │ │ │ │ │ +Standard deviations (sigmas), their inverse and inverse square (weights/ │ │ │ │ │ +precisions) These are all com... │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:289 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_D_i_a_g_o_n_a_l_:_:_s_i_g_m_a │ │ │ │ │ +double sigma(size_t i) const │ │ │ │ │ +Return standard deviations (sqrt of diagonal) │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:335 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_D_i_a_g_o_n_a_l_:_:_i_n_v_s_i_g_m_a_s │ │ │ │ │ +const Vector & invsigmas() const │ │ │ │ │ +Return sqrt precisions. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:340 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_D_i_a_g_o_n_a_l_:_:_s_i_g_m_a_s │ │ │ │ │ +Vector sigmas() const override │ │ │ │ │ +Calculate standard deviations. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:325 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_D_i_a_g_o_n_a_l_:_:_p_r_e_c_i_s_i_o_n_s │ │ │ │ │ +const Vector & precisions() const │ │ │ │ │ +Return precisions. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:346 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_C_o_n_s_t_r_a_i_n_e_d │ │ │ │ │ +A Constrained constrained model is a specialization of Diagonal which allows │ │ │ │ │ +some or all of the sigma... │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:381 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_C_o_n_s_t_r_a_i_n_e_d_:_:_i_s_C_o_n_s_t_r_a_i_n_e_d │ │ │ │ │ +bool isConstrained() const override │ │ │ │ │ +true if a constrained noise mode, saves slow/clumsy dynamic casting │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:409 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_C_o_n_s_t_r_a_i_n_e_d_:_:_A_l_l │ │ │ │ │ +static shared_ptr All(size_t dim, const Vector &mu) │ │ │ │ │ +Fully constrained variations. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:462 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_C_o_n_s_t_r_a_i_n_e_d_:_:_A_l_l │ │ │ │ │ +static shared_ptr All(size_t dim, double mu) │ │ │ │ │ +Fully constrained variations with a mu parameter. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:467 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_C_o_n_s_t_r_a_i_n_e_d_:_:_A_l_l │ │ │ │ │ +static shared_ptr All(size_t dim) │ │ │ │ │ +Fully constrained variations. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:457 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_C_o_n_s_t_r_a_i_n_e_d_:_:_m_u │ │ │ │ │ +const Vector & mu() const │ │ │ │ │ +Access mu as a vector. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:415 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_C_o_n_s_t_r_a_i_n_e_d_:_:_m_u__ │ │ │ │ │ +Vector mu_ │ │ │ │ │ +Penalty function weight - needs to be large enough to dominate soft │ │ │ │ │ +constraints. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:385 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c │ │ │ │ │ +An isotropic noise model corresponds to a scaled diagonal covariance To │ │ │ │ │ +construct,... │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:516 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c_:_:_s_i_g_m_a │ │ │ │ │ +double sigma() const │ │ │ │ │ +Return standard deviation. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:565 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c_:_:_P_r_e_c_i_s_i_o_n │ │ │ │ │ +static shared_ptr Precision(size_t dim, double precision, bool smart=true) │ │ │ │ │ +An isotropic noise model created by specifying a precision. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:549 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c_:_:_I_s_o_t_r_o_p_i_c │ │ │ │ │ +Isotropic(size_t dim, double sigma) │ │ │ │ │ +protected constructor takes sigma │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:521 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t │ │ │ │ │ +Unit: i.i.d. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:584 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_W_h_i_t_e_n_I_n_P_l_a_c_e │ │ │ │ │ +void WhitenInPlace(Eigen::Block< Matrix >) const override │ │ │ │ │ +In-place version. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:610 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_u_n_w_h_i_t_e_n │ │ │ │ │ +Vector unwhiten(const Vector &v) const override │ │ │ │ │ +Unwhiten an error vector. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:607 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_w_h_i_t_e_n_I_n_P_l_a_c_e │ │ │ │ │ +void whitenInPlace(Eigen::Block< Vector > &) const override │ │ │ │ │ +in-place whiten, override if can be done more efficiently │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:613 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_i_s_U_n_i_t │ │ │ │ │ +bool isUnit() const override │ │ │ │ │ +true if a unit noise model, saves slow/clumsy dynamic casting │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:602 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_U_n_i_t │ │ │ │ │ +Unit(size_t dim=1) │ │ │ │ │ +constructor for serialization │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:590 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_w_h_i_t_e_n │ │ │ │ │ +Vector whiten(const Vector &v) const override │ │ │ │ │ +Whiten an error vector. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:606 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_C_r_e_a_t_e │ │ │ │ │ +static shared_ptr Create(size_t dim) │ │ │ │ │ +Create a unit covariance noise model. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:597 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_u_n_w_h_i_t_e_n_I_n_P_l_a_c_e │ │ │ │ │ +void unwhitenInPlace(Vector &) const override │ │ │ │ │ +in-place unwhiten, override if can be done more efficiently │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:612 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_u_n_w_h_i_t_e_n_I_n_P_l_a_c_e │ │ │ │ │ +void unwhitenInPlace(Eigen::Block< Vector > &) const override │ │ │ │ │ +in-place unwhiten, override if can be done more efficiently │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:614 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_w_h_i_t_e_n_I_n_P_l_a_c_e │ │ │ │ │ +void whitenInPlace(Vector &) const override │ │ │ │ │ +in-place whiten, override if can be done more efficiently │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:611 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_W_h_i_t_e_n_I_n_P_l_a_c_e │ │ │ │ │ +void WhitenInPlace(Matrix &) const override │ │ │ │ │ +In-place version. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:609 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_W_h_i_t_e_n │ │ │ │ │ +Matrix Whiten(const Matrix &H) const override │ │ │ │ │ +Whiten a matrix. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:608 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t │ │ │ │ │ +Base class for robust error models The robust M-estimators above simply tell us │ │ │ │ │ +how to re-weight the ... │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:642 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t_:_:_r_o_b_u_s_t │ │ │ │ │ +const RobustModel::shared_ptr & robust() const │ │ │ │ │ +Return the contained robust error function. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:669 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t_:_:_R_o_b_u_s_t │ │ │ │ │ +Robust() │ │ │ │ │ +Default Constructor for serialization. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:656 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t_:_:_R_o_b_u_s_t │ │ │ │ │ +Robust(const RobustModel::shared_ptr robust, const NoiseModel::shared_ptr │ │ │ │ │ +noise) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:659 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t_:_:_s_q_u_a_r_e_d_M_a_h_a_l_a_n_o_b_i_s_D_i_s_t_a_n_c_e │ │ │ │ │ +double squaredMahalanobisDistance(const Vector &v) const override │ │ │ │ │ +Squared Mahalanobis distance v'*R'*R*v = │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:688 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t_:_:_n_o_i_s_e__ │ │ │ │ │ +const NoiseModel::shared_ptr noise_ │ │ │ │ │ +noise model used │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:651 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t_:_:_l_o_s_s │ │ │ │ │ +double loss(const double squared_distance) const override │ │ │ │ │ +Compute loss from the m-estimator using the Mahalanobis distance. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:682 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t_:_:_u_n_w_h_i_t_e_n │ │ │ │ │ +Vector unwhiten(const Vector &) const override │ │ │ │ │ +Unwhiten an error vector. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:679 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t_:_:_W_h_i_t_e_n │ │ │ │ │ +Matrix Whiten(const Matrix &A) const override │ │ │ │ │ +Whiten a matrix. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:677 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t_:_:_r_o_b_u_s_t__ │ │ │ │ │ +const RobustModel::shared_ptr robust_ │ │ │ │ │ +robust error function used │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:650 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t_:_:_~_R_o_b_u_s_t │ │ │ │ │ +~Robust() override │ │ │ │ │ +Destructor. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:663 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t_:_:_n_o_i_s_e │ │ │ │ │ +const NoiseModel::shared_ptr & noise() const │ │ │ │ │ +Return the contained noise model. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:672 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t_:_:_w_h_i_t_e_n │ │ │ │ │ +Vector whiten(const Vector &v) const override │ │ │ │ │ +Whiten an error vector. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:675 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * JJaaccoobbiiaannFFaaccttoorr--iinnll..hh │ │ │ │ │ + * _N_o_i_s_e_M_o_d_e_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00827.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianJunctionTree.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianBayesNet.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,33 +95,34 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces
│ │ │ │ -
GaussianJunctionTree.cpp File Reference
│ │ │ │ +
GaussianBayesNet.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ + │ │ │ │ +

Chordal Bayes Net, the result of eliminating a factor graph. │ │ │ │ +More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Mar 29, 2013
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ +

Chordal Bayes Net, the result of eliminating a factor graph.

│ │ │ │ +
Author
Frank Dellaert, Varun Agrawal
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,20 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -GaussianJunctionTree.cpp File Reference │ │ │ │ │ +GaussianBayesNet.cpp File Reference │ │ │ │ │ +Chordal Bayes Net, the result of eliminating a factor graph. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Mar 29, 2013 │ │ │ │ │ +Chordal Bayes Net, the result of eliminating a factor graph. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Frank Dellaert, Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_J_u_n_c_t_i_o_n_T_r_e_e_._c_p_p │ │ │ │ │ + * _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00833_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianConditional.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/PCGSolver.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,224 +98,157 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
GaussianConditional.h
│ │ │ │ +
PCGSolver.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18// \callgraph
│ │ │ │ +
12/*
│ │ │ │ +
13 * @file PCGSolver.h
│ │ │ │ +
14 * @brief Preconditioned Conjugate Gradient Solver for linear systems
│ │ │ │ +
15 * @date Jan 31, 2012
│ │ │ │ +
16 * @author Yong-Dian Jian
│ │ │ │ +
17 * @author Sungtae An
│ │ │ │ +
18 */
│ │ │ │
19
│ │ │ │
20#pragma once
│ │ │ │
21
│ │ │ │ -
22#include <boost/utility.hpp>
│ │ │ │ -
23
│ │ │ │ -
24#include <gtsam/global_includes.h>
│ │ │ │ -
25#include <gtsam/linear/JacobianFactor.h>
│ │ │ │ -
26#include <gtsam/inference/Conditional.h>
│ │ │ │ -
27#include <gtsam/inference/Conditional-inst.h>
│ │ │ │ -
28#include <gtsam/linear/VectorValues.h>
│ │ │ │ -
29
│ │ │ │ -
30#include <random> // for std::mt19937_64
│ │ │ │ -
31
│ │ │ │ -
32namespace gtsam {
│ │ │ │ -
33
│ │ │ │ -
│ │ │ │ -
40 class GTSAM_EXPORT GaussianConditional :
│ │ │ │ -
41 public JacobianFactor,
│ │ │ │ -
42 public Conditional<JacobianFactor, GaussianConditional>
│ │ │ │ -
43 {
│ │ │ │ -
44 public:
│ │ │ │ -
45 typedef GaussianConditional This;
│ │ │ │ -
46 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
47 typedef JacobianFactor BaseFactor;
│ │ │ │ -
48 typedef Conditional<BaseFactor, This> BaseConditional;
│ │ │ │ -
49
│ │ │ │ -
52
│ │ │ │ -
54 GaussianConditional() {}
│ │ │ │ +
22#include <gtsam/linear/ConjugateGradientSolver.h>
│ │ │ │ +
23#include <string>
│ │ │ │ +
24
│ │ │ │ +
25namespace gtsam {
│ │ │ │ +
26
│ │ │ │ +
27class GaussianFactorGraph;
│ │ │ │ +
28class KeyInfo;
│ │ │ │ +
29class Preconditioner;
│ │ │ │ +
30class VectorValues;
│ │ │ │ +
31struct PreconditionerParameters;
│ │ │ │ +
32
│ │ │ │ +
│ │ │ │ +
36struct GTSAM_EXPORT PCGSolverParameters: public ConjugateGradientParameters {
│ │ │ │ +
37public:
│ │ │ │ +
38 typedef ConjugateGradientParameters Base;
│ │ │ │ +
39 typedef boost::shared_ptr<PCGSolverParameters> shared_ptr;
│ │ │ │ +
40
│ │ │ │ +
41 PCGSolverParameters() {
│ │ │ │ +
42 }
│ │ │ │ +
43
│ │ │ │ +
44 void print(std::ostream &os) const override;
│ │ │ │ +
45
│ │ │ │ +
46 /* interface to preconditioner parameters */
│ │ │ │ +
47 inline const PreconditionerParameters& preconditioner() const {
│ │ │ │ +
48 return *preconditioner_;
│ │ │ │ +
49 }
│ │ │ │ +
50
│ │ │ │ +
51 // needed for python wrapper
│ │ │ │ +
52 void print(const std::string &s) const;
│ │ │ │ +
53
│ │ │ │ +
54 boost::shared_ptr<PreconditionerParameters> preconditioner_;
│ │ │ │
55
│ │ │ │ -
57 GaussianConditional(Key key, const Vector& d, const Matrix& R,
│ │ │ │ -
58 const SharedDiagonal& sigmas = SharedDiagonal());
│ │ │ │ -
59
│ │ │ │ -
61 GaussianConditional(Key key, const Vector& d, const Matrix& R, Key parent1,
│ │ │ │ -
62 const Matrix& S,
│ │ │ │ -
63 const SharedDiagonal& sigmas = SharedDiagonal());
│ │ │ │ -
64
│ │ │ │ -
66 GaussianConditional(Key key, const Vector& d, const Matrix& R, Key parent1,
│ │ │ │ -
67 const Matrix& S, Key parent2, const Matrix& T,
│ │ │ │ -
68 const SharedDiagonal& sigmas = SharedDiagonal());
│ │ │ │ -
69
│ │ │ │ -
73 template<typename TERMS>
│ │ │ │ -
74 GaussianConditional(const TERMS& terms,
│ │ │ │ -
75 size_t nrFrontals, const Vector& d,
│ │ │ │ -
76 const SharedDiagonal& sigmas = SharedDiagonal());
│ │ │ │ +
56 void setPreconditionerParams(const boost::shared_ptr<PreconditionerParameters> preconditioner);
│ │ │ │ +
57};
│ │ │ │ +
│ │ │ │ +
58
│ │ │ │ +
│ │ │ │ +
62class GTSAM_EXPORT PCGSolver: public IterativeSolver {
│ │ │ │ +
63public:
│ │ │ │ +
64 typedef IterativeSolver Base;
│ │ │ │ +
65 typedef boost::shared_ptr<PCGSolver> shared_ptr;
│ │ │ │ +
66
│ │ │ │ +
67protected:
│ │ │ │ +
68
│ │ │ │ +
69 PCGSolverParameters parameters_;
│ │ │ │ +
70 boost::shared_ptr<Preconditioner> preconditioner_;
│ │ │ │ +
71
│ │ │ │ +
72public:
│ │ │ │ +
73 /* Interface to initialize a solver without a problem */
│ │ │ │ +
74 PCGSolver(const PCGSolverParameters &p);
│ │ │ │ +
75 ~PCGSolver() override {
│ │ │ │ +
76 }
│ │ │ │
77
│ │ │ │ -
82 template<typename KEYS>
│ │ │ │ -
83 GaussianConditional(
│ │ │ │ -
84 const KEYS& keys, size_t nrFrontals, const VerticalBlockMatrix& augmentedMatrix,
│ │ │ │ -
85 const SharedDiagonal& sigmas = SharedDiagonal());
│ │ │ │ -
86
│ │ │ │ -
88 static GaussianConditional FromMeanAndStddev(Key key, const Vector& mu,
│ │ │ │ -
89 double sigma);
│ │ │ │ -
90
│ │ │ │ -
92 static GaussianConditional FromMeanAndStddev(Key key, const Matrix& A,
│ │ │ │ -
93 Key parent, const Vector& b,
│ │ │ │ -
94 double sigma);
│ │ │ │ +
78 using IterativeSolver::optimize;
│ │ │ │ +
79
│ │ │ │ +
80 VectorValues optimize(const GaussianFactorGraph &gfg,
│ │ │ │ +
81 const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
│ │ │ │ +
82 const VectorValues &initial) override;
│ │ │ │ +
83
│ │ │ │ +
84};
│ │ │ │ +
│ │ │ │ +
85
│ │ │ │ +
│ │ │ │ +
89class GTSAM_EXPORT GaussianFactorGraphSystem {
│ │ │ │ +
90public:
│ │ │ │ +
91
│ │ │ │ +
92 GaussianFactorGraphSystem(const GaussianFactorGraph &gfg,
│ │ │ │ +
93 const Preconditioner &preconditioner, const KeyInfo &info,
│ │ │ │ +
94 const std::map<Key, Vector> &lambda);
│ │ │ │
95
│ │ │ │ -
98 static GaussianConditional FromMeanAndStddev(Key key, //
│ │ │ │ -
99 const Matrix& A1, Key parent1,
│ │ │ │ -
100 const Matrix& A2, Key parent2,
│ │ │ │ -
101 const Vector& b, double sigma);
│ │ │ │ -
102
│ │ │ │ -
104 template<typename... Args>
│ │ │ │ -
│ │ │ │ -
105 static shared_ptr sharedMeanAndStddev(Args&&... args) {
│ │ │ │ -
106 return boost::make_shared<This>(FromMeanAndStddev(std::forward<Args>(args)...));
│ │ │ │ -
107 }
│ │ │ │ -
│ │ │ │ +
96 const GaussianFactorGraph &gfg_;
│ │ │ │ +
97 const Preconditioner &preconditioner_;
│ │ │ │ +
98 const KeyInfo &keyInfo_;
│ │ │ │ +
99 const std::map<Key, Vector> &lambda_;
│ │ │ │ +
100
│ │ │ │ +
101 void residual(const Vector &x, Vector &r) const;
│ │ │ │ +
102 void multiply(const Vector &x, Vector& y) const;
│ │ │ │ +
103 void leftPrecondition(const Vector &x, Vector &y) const;
│ │ │ │ +
104 void rightPrecondition(const Vector &x, Vector &y) const;
│ │ │ │ +
105 void scal(const double alpha, Vector &x) const;
│ │ │ │ +
106 double dot(const Vector &x, const Vector &y) const;
│ │ │ │ +
107 void axpy(const double alpha, const Vector &x, Vector &y) const;
│ │ │ │
108
│ │ │ │ -
116 template<typename ITERATOR>
│ │ │ │ -
117 static shared_ptr Combine(ITERATOR firstConditional, ITERATOR lastConditional);
│ │ │ │ +
109 void getb(Vector &b) const;
│ │ │ │ +
110};
│ │ │ │ +
│ │ │ │ +
111
│ │ │ │ +
114
│ │ │ │ +
116VectorValues buildVectorValues(const Vector &v, const Ordering &ordering,
│ │ │ │ +
117 const std::map<Key, size_t> & dimensions);
│ │ │ │
118
│ │ │ │ -
122
│ │ │ │ -
124 void print(
│ │ │ │ -
125 const std::string& = "GaussianConditional",
│ │ │ │ -
126 const KeyFormatter& formatter = DefaultKeyFormatter) const override;
│ │ │ │ -
127
│ │ │ │ -
129 bool equals(const GaussianFactor&cg, double tol = 1e-9) const override;
│ │ │ │ -
130
│ │ │ │ -
134
│ │ │ │ -
139 double logNormalizationConstant() const override;
│ │ │ │ -
140
│ │ │ │ -
148 double logProbability(const VectorValues& x) const;
│ │ │ │ -
149
│ │ │ │ -
155 double evaluate(const VectorValues& x) const;
│ │ │ │ -
156
│ │ │ │ -
│ │ │ │ -
158 double operator()(const VectorValues& x) const {
│ │ │ │ -
159 return evaluate(x);
│ │ │ │ -
160 }
│ │ │ │ -
│ │ │ │ -
161
│ │ │ │ -
175 VectorValues solve(const VectorValues& parents) const;
│ │ │ │ -
176
│ │ │ │ -
177 VectorValues solveOtherRHS(const VectorValues& parents, const VectorValues& rhs) const;
│ │ │ │ -
178
│ │ │ │ -
180 void solveTransposeInPlace(VectorValues& gy) const;
│ │ │ │ -
181
│ │ │ │ -
183 JacobianFactor::shared_ptr likelihood(
│ │ │ │ -
184 const VectorValues& frontalValues) const;
│ │ │ │ -
185
│ │ │ │ -
187 JacobianFactor::shared_ptr likelihood(const Vector& frontal) const;
│ │ │ │ -
188
│ │ │ │ -
195 VectorValues sample(std::mt19937_64* rng) const;
│ │ │ │ -
196
│ │ │ │ -
204 VectorValues sample(const VectorValues& parentsValues,
│ │ │ │ -
205 std::mt19937_64* rng) const;
│ │ │ │ -
206
│ │ │ │ -
208 VectorValues sample() const;
│ │ │ │ -
209
│ │ │ │ -
211 VectorValues sample(const VectorValues& parentsValues) const;
│ │ │ │ -
212
│ │ │ │ -
216
│ │ │ │ -
218 constABlock R() const { return Ab_.range(0, nrFrontals()); }
│ │ │ │ -
219
│ │ │ │ -
221 constABlock S() const { return Ab_.range(nrFrontals(), size()); }
│ │ │ │ -
222
│ │ │ │ -
224 constABlock S(const_iterator it) const { return BaseFactor::getA(it); }
│ │ │ │ -
225
│ │ │ │ -
227 const constBVector d() const { return BaseFactor::getb(); }
│ │ │ │ -
228
│ │ │ │ -
240 inline double determinant() const { return exp(logDeterminant()); }
│ │ │ │ -
241
│ │ │ │ -
253 double logDeterminant() const;
│ │ │ │ -
254
│ │ │ │ -
258
│ │ │ │ -
263 double logProbability(const HybridValues& x) const override;
│ │ │ │ -
264
│ │ │ │ -
269 double evaluate(const HybridValues& x) const override;
│ │ │ │ -
270
│ │ │ │ -
271 using Conditional::operator(); // Expose evaluate(const HybridValues&) method..
│ │ │ │ -
272 using JacobianFactor::error; // Expose error(const HybridValues&) method..
│ │ │ │ -
273
│ │ │ │ -
275
│ │ │ │ -
276
│ │ │ │ -
277#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ -
280
│ │ │ │ -
282 void GTSAM_DEPRECATED scaleFrontalsBySigma(VectorValues& gy) const;
│ │ │ │ -
284#endif
│ │ │ │ -
285
│ │ │ │ -
286 private:
│ │ │ │ -
288 friend class boost::serialization::access;
│ │ │ │ -
289 template<class Archive>
│ │ │ │ -
290 void serialize(Archive & ar, const unsigned int /*version*/) {
│ │ │ │ -
291 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseFactor);
│ │ │ │ -
292 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseConditional);
│ │ │ │ -
293 }
│ │ │ │ -
294 }; // GaussianConditional
│ │ │ │ -
│ │ │ │ -
295
│ │ │ │ -
297template<>
│ │ │ │ -
298struct traits<GaussianConditional> : public Testable<GaussianConditional> {};
│ │ │ │ -
299
│ │ │ │ -
300} // \ namespace gtsam
│ │ │ │ -
301
│ │ │ │ -
302#include <gtsam/linear/GaussianConditional-inl.h>
│ │ │ │ -
303
│ │ │ │ -
Conditional.h
Base class for conditional densities.
│ │ │ │ -
GaussianConditional-inl.h
Conditional Gaussian Base class.
│ │ │ │ - │ │ │ │ -
VectorValues.h
Factor Graph Values.
│ │ │ │ -
global_includes.h
Included from all GTSAM files.
│ │ │ │ +
120VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo);
│ │ │ │ +
121
│ │ │ │ +
123
│ │ │ │ +
124}
│ │ │ │ +
125
│ │ │ │ +
ConjugateGradientSolver.h
Implementation of Conjugate Gradient solver for a linear system.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::buildVectorValues
VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, const map< Key, size_t > &dimensions)
Create VectorValues from a Vector.
Definition PCGSolver.cpp:151
│ │ │ │ +
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::VerticalBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43
│ │ │ │ -
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ -
gtsam::Conditional
Definition Conditional.h:64
│ │ │ │ -
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition Factor.h:80
│ │ │ │ -
gtsam::GaussianConditional
A GaussianConditional functions as the node in a Bayes network.
Definition GaussianConditional.h:43
│ │ │ │ -
gtsam::GaussianConditional::This
GaussianConditional This
Typedef to this class.
Definition GaussianConditional.h:45
│ │ │ │ -
gtsam::GaussianConditional::S
constABlock S(const_iterator it) const
Get a view of the S matrix for the variable pointed to by the given key iterator.
Definition GaussianConditional.h:224
│ │ │ │ -
gtsam::GaussianConditional::R
constABlock R() const
Return a view of the upper-triangular R block of the conditional.
Definition GaussianConditional.h:218
│ │ │ │ -
gtsam::GaussianConditional::BaseFactor
JacobianFactor BaseFactor
Typedef to our factor base class.
Definition GaussianConditional.h:47
│ │ │ │ -
gtsam::GaussianConditional::GaussianConditional
GaussianConditional()
default constructor needed for serialization
Definition GaussianConditional.h:54
│ │ │ │ -
gtsam::GaussianConditional::BaseConditional
Conditional< BaseFactor, This > BaseConditional
Typedef to our conditional base class.
Definition GaussianConditional.h:48
│ │ │ │ -
gtsam::GaussianConditional::Combine
static shared_ptr Combine(ITERATOR firstConditional, ITERATOR lastConditional)
Combine several GaussianConditional into a single dense GC.
│ │ │ │ -
gtsam::GaussianConditional::determinant
double determinant() const
Compute the determinant of the R matrix.
Definition GaussianConditional.h:240
│ │ │ │ -
gtsam::GaussianConditional::operator()
double operator()(const VectorValues &x) const
Evaluate probability density, sugar.
Definition GaussianConditional.h:158
│ │ │ │ -
gtsam::GaussianConditional::sharedMeanAndStddev
static shared_ptr sharedMeanAndStddev(Args &&... args)
Create shared pointer by forwarding arguments to fromMeanAndStddev.
Definition GaussianConditional.h:105
│ │ │ │ -
gtsam::GaussianConditional::S
constABlock S() const
Get a view of the parent blocks.
Definition GaussianConditional.h:221
│ │ │ │ -
gtsam::GaussianConditional::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianConditional.h:46
│ │ │ │ -
gtsam::GaussianConditional::d
const constBVector d() const
Get a view of the r.h.s.
Definition GaussianConditional.h:227
│ │ │ │ -
gtsam::GaussianFactor
An abstract virtual base class for JacobianFactor and HessianFactor.
Definition GaussianFactor.h:39
│ │ │ │ -
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition JacobianFactor.h:91
│ │ │ │ -
gtsam::JacobianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition JacobianFactor.h:96
│ │ │ │ +
gtsam::axpy
void axpy(double alpha, const Errors &x, Errors &y)
BLAS level 2 style AXPY, y := alpha*x + y
Definition Errors.cpp:111
│ │ │ │ +
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
│ │ │ │ +
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
gtsam::ConjugateGradientParameters
parameters for the conjugate gradient method
Definition ConjugateGradientSolver.h:29
│ │ │ │ +
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ +
gtsam::IterativeSolver
Base class for Iterative Solvers like SubgraphSolver.
Definition IterativeSolver.h:86
│ │ │ │ +
gtsam::KeyInfo
Handy data structure for iterative solvers.
Definition IterativeSolver.h:126
│ │ │ │ +
gtsam::PCGSolverParameters
Parameters for PCG.
Definition PCGSolver.h:36
│ │ │ │ +
gtsam::PCGSolver
A virtual base class for the preconditioned conjugate gradient solver.
Definition PCGSolver.h:62
│ │ │ │ +
gtsam::GaussianFactorGraphSystem
System class needed for calling preconditionedConjugateGradient.
Definition PCGSolver.h:89
│ │ │ │ +
gtsam::PreconditionerParameters
Definition Preconditioner.h:24
│ │ │ │ +
gtsam::Preconditioner
Definition Preconditioner.h:64
│ │ │ │
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
VectorValues
The Factor::error simply extracts the.
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,298 +1,184 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GaussianConditional.h │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +PCGSolver.h │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18// \callgraph │ │ │ │ │ +12/* │ │ │ │ │ +13 * @file PCGSolver.h │ │ │ │ │ +14 * @brief Preconditioned Conjugate Gradient Solver for linear systems │ │ │ │ │ +15 * @date Jan 31, 2012 │ │ │ │ │ +16 * @author Yong-Dian Jian │ │ │ │ │ +17 * @author Sungtae An │ │ │ │ │ +18 */ │ │ │ │ │ 19 │ │ │ │ │ 20#pragma once │ │ │ │ │ 21 │ │ │ │ │ -22#include │ │ │ │ │ -23 │ │ │ │ │ -24#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ -27#include │ │ │ │ │ -28#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ -29 │ │ │ │ │ -30#include // for std::mt19937_64 │ │ │ │ │ -31 │ │ │ │ │ -32namespace _g_t_s_a_m { │ │ │ │ │ -33 │ │ │ │ │ -_4_0 class GTSAM_EXPORT _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l : │ │ │ │ │ -41 public _J_a_c_o_b_i_a_n_F_a_c_t_o_r, │ │ │ │ │ -42 public _C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -43 { │ │ │ │ │ -44 public: │ │ │ │ │ -_4_5 typedef _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l _T_h_i_s; │ │ │ │ │ -_4_6 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -_4_7 typedef _J_a_c_o_b_i_a_n_F_a_c_t_o_r _B_a_s_e_F_a_c_t_o_r; │ │ │ │ │ -_4_8 typedef _C_o_n_d_i_t_i_o_n_a_l_<_B_a_s_e_F_a_c_t_o_r_,_ _T_h_i_s_> _B_a_s_e_C_o_n_d_i_t_i_o_n_a_l; │ │ │ │ │ -49 │ │ │ │ │ -52 │ │ │ │ │ -_5_4 _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l() {} │ │ │ │ │ +22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_S_o_l_v_e_r_._h> │ │ │ │ │ +23#include │ │ │ │ │ +24 │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ +26 │ │ │ │ │ +27class GaussianFactorGraph; │ │ │ │ │ +28class KeyInfo; │ │ │ │ │ +29class Preconditioner; │ │ │ │ │ +30class _V_e_c_t_o_r_V_a_l_u_e_s; │ │ │ │ │ +31struct PreconditionerParameters; │ │ │ │ │ +32 │ │ │ │ │ +_3_6struct GTSAM_EXPORT _P_C_G_S_o_l_v_e_r_P_a_r_a_m_e_t_e_r_s: public _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s { │ │ │ │ │ +37public: │ │ │ │ │ +38 typedef _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s _B_a_s_e; │ │ │ │ │ +39 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +40 │ │ │ │ │ +41 _P_C_G_S_o_l_v_e_r_P_a_r_a_m_e_t_e_r_s() { │ │ │ │ │ +42 } │ │ │ │ │ +43 │ │ │ │ │ +44 void _p_r_i_n_t(std::ostream &os) const override; │ │ │ │ │ +45 │ │ │ │ │ +46 /* interface to preconditioner parameters */ │ │ │ │ │ +47 inline const _P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s& preconditioner() const { │ │ │ │ │ +48 return *preconditioner_; │ │ │ │ │ +49 } │ │ │ │ │ +50 │ │ │ │ │ +51 // needed for python wrapper │ │ │ │ │ +52 void _p_r_i_n_t(const std::string &s) const; │ │ │ │ │ +53 │ │ │ │ │ +54 boost::shared_ptr preconditioner_; │ │ │ │ │ 55 │ │ │ │ │ -57 _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l(_K_e_y key, const Vector& d, const Matrix& R, │ │ │ │ │ -58 const SharedDiagonal& sigmas = SharedDiagonal()); │ │ │ │ │ -59 │ │ │ │ │ -61 _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l(_K_e_y key, const Vector& d, const Matrix& R, _K_e_y parent1, │ │ │ │ │ -62 const Matrix& S, │ │ │ │ │ -63 const SharedDiagonal& sigmas = SharedDiagonal()); │ │ │ │ │ -64 │ │ │ │ │ -66 _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l(_K_e_y key, const Vector& d, const Matrix& R, _K_e_y parent1, │ │ │ │ │ -67 const Matrix& S, _K_e_y parent2, const Matrix& T, │ │ │ │ │ -68 const SharedDiagonal& sigmas = SharedDiagonal()); │ │ │ │ │ -69 │ │ │ │ │ -73 template │ │ │ │ │ -74 _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l(const TERMS& terms, │ │ │ │ │ -75 size_t nrFrontals, const Vector& d, │ │ │ │ │ -76 const SharedDiagonal& sigmas = SharedDiagonal()); │ │ │ │ │ +56 void setPreconditionerParams(const boost:: │ │ │ │ │ +shared_ptr preconditioner); │ │ │ │ │ +57}; │ │ │ │ │ +58 │ │ │ │ │ +_6_2class GTSAM_EXPORT _P_C_G_S_o_l_v_e_r: public _I_t_e_r_a_t_i_v_e_S_o_l_v_e_r { │ │ │ │ │ +63public: │ │ │ │ │ +64 typedef _I_t_e_r_a_t_i_v_e_S_o_l_v_e_r _B_a_s_e; │ │ │ │ │ +65 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +66 │ │ │ │ │ +67protected: │ │ │ │ │ +68 │ │ │ │ │ +69 _P_C_G_S_o_l_v_e_r_P_a_r_a_m_e_t_e_r_s parameters_; │ │ │ │ │ +70 boost::shared_ptr preconditioner_; │ │ │ │ │ +71 │ │ │ │ │ +72public: │ │ │ │ │ +73 /* Interface to initialize a solver without a problem */ │ │ │ │ │ +74 _P_C_G_S_o_l_v_e_r(const _P_C_G_S_o_l_v_e_r_P_a_r_a_m_e_t_e_r_s &p); │ │ │ │ │ +75 _~_P_C_G_S_o_l_v_e_r() override { │ │ │ │ │ +76 } │ │ │ │ │ 77 │ │ │ │ │ -82 template │ │ │ │ │ -83 _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l( │ │ │ │ │ -84 const KEYS& keys, size_t nrFrontals, const _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x& │ │ │ │ │ -augmentedMatrix, │ │ │ │ │ -85 const SharedDiagonal& sigmas = SharedDiagonal()); │ │ │ │ │ -86 │ │ │ │ │ -88 static _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l FromMeanAndStddev(_K_e_y key, const Vector& mu, │ │ │ │ │ -89 double sigma); │ │ │ │ │ -90 │ │ │ │ │ -92 static _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l FromMeanAndStddev(_K_e_y key, const Matrix& A, │ │ │ │ │ -93 _K_e_y parent, const Vector& b, │ │ │ │ │ -94 double sigma); │ │ │ │ │ +78 using IterativeSolver::optimize; │ │ │ │ │ +79 │ │ │ │ │ +80 _V_e_c_t_o_r_V_a_l_u_e_s _o_p_t_i_m_i_z_e(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, │ │ │ │ │ +81 const _K_e_y_I_n_f_o &keyInfo, const std::map &lambda, │ │ │ │ │ +82 const _V_e_c_t_o_r_V_a_l_u_e_s &initial) override; │ │ │ │ │ +83 │ │ │ │ │ +84}; │ │ │ │ │ +85 │ │ │ │ │ +_8_9class GTSAM_EXPORT _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_S_y_s_t_e_m { │ │ │ │ │ +90public: │ │ │ │ │ +91 │ │ │ │ │ +92 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_S_y_s_t_e_m(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, │ │ │ │ │ +93 const _P_r_e_c_o_n_d_i_t_i_o_n_e_r &preconditioner, const _K_e_y_I_n_f_o &info, │ │ │ │ │ +94 const std::map &lambda); │ │ │ │ │ 95 │ │ │ │ │ -98 static _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l FromMeanAndStddev(_K_e_y key, // │ │ │ │ │ -99 const Matrix& A1, _K_e_y parent1, │ │ │ │ │ -100 const Matrix& A2, _K_e_y parent2, │ │ │ │ │ -101 const Vector& b, double sigma); │ │ │ │ │ -102 │ │ │ │ │ -104 template │ │ │ │ │ -_1_0_5 static _s_h_a_r_e_d___p_t_r _s_h_a_r_e_d_M_e_a_n_A_n_d_S_t_d_d_e_v(Args&&... args) { │ │ │ │ │ -106 return boost::make_shared(FromMeanAndStddev(std::forward │ │ │ │ │ -(args)...)); │ │ │ │ │ -107 } │ │ │ │ │ +96 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg_; │ │ │ │ │ +97 const _P_r_e_c_o_n_d_i_t_i_o_n_e_r &preconditioner_; │ │ │ │ │ +98 const _K_e_y_I_n_f_o &keyInfo_; │ │ │ │ │ +99 const std::map &lambda_; │ │ │ │ │ +100 │ │ │ │ │ +101 void residual(const Vector &x, Vector &r) const; │ │ │ │ │ +102 void multiply(const Vector &x, Vector& y) const; │ │ │ │ │ +103 void leftPrecondition(const Vector &x, Vector &y) const; │ │ │ │ │ +104 void rightPrecondition(const Vector &x, Vector &y) const; │ │ │ │ │ +105 void scal(const double alpha, Vector &x) const; │ │ │ │ │ +106 double _d_o_t(const Vector &x, const Vector &y) const; │ │ │ │ │ +107 void _a_x_p_y(const double alpha, const Vector &x, Vector &y) const; │ │ │ │ │ 108 │ │ │ │ │ -116 template │ │ │ │ │ -_1_1_7 static _s_h_a_r_e_d___p_t_r _C_o_m_b_i_n_e(ITERATOR firstConditional, ITERATOR │ │ │ │ │ -lastConditional); │ │ │ │ │ +109 void getb(Vector &b) const; │ │ │ │ │ +110}; │ │ │ │ │ +111 │ │ │ │ │ +114 │ │ │ │ │ +116_V_e_c_t_o_r_V_a_l_u_e_s _b_u_i_l_d_V_e_c_t_o_r_V_a_l_u_e_s(const Vector &v, const _O_r_d_e_r_i_n_g &ordering, │ │ │ │ │ +117 const std::map & dimensions); │ │ │ │ │ 118 │ │ │ │ │ -122 │ │ │ │ │ -124 void _p_r_i_n_t( │ │ │ │ │ -125 const std::string& = "GaussianConditional", │ │ │ │ │ -126 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override; │ │ │ │ │ -127 │ │ │ │ │ -129 bool _e_q_u_a_l_s(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r&cg, double tol = 1e-9) const override; │ │ │ │ │ -130 │ │ │ │ │ -134 │ │ │ │ │ -139 double logNormalizationConstant() const override; │ │ │ │ │ -140 │ │ │ │ │ -148 double logProbability(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const; │ │ │ │ │ -149 │ │ │ │ │ -155 double evaluate(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const; │ │ │ │ │ -156 │ │ │ │ │ -_1_5_8 double _o_p_e_r_a_t_o_r_(_)(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const { │ │ │ │ │ -159 return evaluate(x); │ │ │ │ │ -160 } │ │ │ │ │ -161 │ │ │ │ │ -175 _V_e_c_t_o_r_V_a_l_u_e_s solve(const _V_e_c_t_o_r_V_a_l_u_e_s& parents) const; │ │ │ │ │ -176 │ │ │ │ │ -177 _V_e_c_t_o_r_V_a_l_u_e_s solveOtherRHS(const _V_e_c_t_o_r_V_a_l_u_e_s& parents, const _V_e_c_t_o_r_V_a_l_u_e_s& │ │ │ │ │ -rhs) const; │ │ │ │ │ -178 │ │ │ │ │ -180 void solveTransposeInPlace(_V_e_c_t_o_r_V_a_l_u_e_s& gy) const; │ │ │ │ │ -181 │ │ │ │ │ -183 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r likelihood( │ │ │ │ │ -184 const _V_e_c_t_o_r_V_a_l_u_e_s& frontalValues) const; │ │ │ │ │ -185 │ │ │ │ │ -187 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r likelihood(const Vector& frontal) const; │ │ │ │ │ -188 │ │ │ │ │ -195 _V_e_c_t_o_r_V_a_l_u_e_s sample(std::mt19937_64* rng) const; │ │ │ │ │ -196 │ │ │ │ │ -204 _V_e_c_t_o_r_V_a_l_u_e_s sample(const _V_e_c_t_o_r_V_a_l_u_e_s& parentsValues, │ │ │ │ │ -205 std::mt19937_64* rng) const; │ │ │ │ │ -206 │ │ │ │ │ -208 _V_e_c_t_o_r_V_a_l_u_e_s sample() const; │ │ │ │ │ -209 │ │ │ │ │ -211 _V_e_c_t_o_r_V_a_l_u_e_s sample(const _V_e_c_t_o_r_V_a_l_u_e_s& parentsValues) const; │ │ │ │ │ -212 │ │ │ │ │ -216 │ │ │ │ │ -_2_1_8 constABlock _R() const { return Ab_.range(0, nrFrontals()); } │ │ │ │ │ -219 │ │ │ │ │ -_2_2_1 constABlock _S() const { return Ab_.range(nrFrontals(), size()); } │ │ │ │ │ -222 │ │ │ │ │ -_2_2_4 constABlock _S(_c_o_n_s_t___i_t_e_r_a_t_o_r it) const { return BaseFactor::getA(it); } │ │ │ │ │ -225 │ │ │ │ │ -_2_2_7 const constBVector _d() const { return BaseFactor::getb(); } │ │ │ │ │ -228 │ │ │ │ │ -_2_4_0 inline double _d_e_t_e_r_m_i_n_a_n_t() const { return exp(logDeterminant()); } │ │ │ │ │ -241 │ │ │ │ │ -253 double logDeterminant() const; │ │ │ │ │ -254 │ │ │ │ │ -258 │ │ │ │ │ -263 double logProbability(const _H_y_b_r_i_d_V_a_l_u_e_s& x) const override; │ │ │ │ │ -264 │ │ │ │ │ -269 double evaluate(const _H_y_b_r_i_d_V_a_l_u_e_s& x) const override; │ │ │ │ │ -270 │ │ │ │ │ -271 using Conditional::operator(); // Expose evaluate(const HybridValues&) │ │ │ │ │ -method.. │ │ │ │ │ -272 using JacobianFactor::error; // Expose error(const HybridValues&) method.. │ │ │ │ │ -273 │ │ │ │ │ -275 │ │ │ │ │ -276 │ │ │ │ │ -277#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ -280 │ │ │ │ │ -282 void GTSAM_DEPRECATED scaleFrontalsBySigma(_V_e_c_t_o_r_V_a_l_u_e_s& gy) const; │ │ │ │ │ -284#endif │ │ │ │ │ -285 │ │ │ │ │ -286 private: │ │ │ │ │ -_2_8_8 friend class boost::serialization::access; │ │ │ │ │ -289 template │ │ │ │ │ -290 void serialize(Archive & ar, const unsigned int /*version*/) { │ │ │ │ │ -291 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e_F_a_c_t_o_r); │ │ │ │ │ -292 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e_C_o_n_d_i_t_i_o_n_a_l); │ │ │ │ │ -293 } │ │ │ │ │ -294 }; // GaussianConditional │ │ │ │ │ -295 │ │ │ │ │ -297template<> │ │ │ │ │ -_2_9_8struct _t_r_a_i_t_s<_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l> : public _T_e_s_t_a_b_l_e │ │ │ │ │ -{}; │ │ │ │ │ -299 │ │ │ │ │ -300} // \ namespace gtsam │ │ │ │ │ -301 │ │ │ │ │ -302#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_-_i_n_l_._h> │ │ │ │ │ -303 │ │ │ │ │ -_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ -Base class for conditional densities. │ │ │ │ │ -_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_-_i_n_l_._h │ │ │ │ │ -Conditional Gaussian Base class. │ │ │ │ │ -_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ -Factor Graph Values. │ │ │ │ │ -_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ -Included from all GTSAM files. │ │ │ │ │ +120_V_e_c_t_o_r_V_a_l_u_e_s _b_u_i_l_d_V_e_c_t_o_r_V_a_l_u_e_s(const Vector &v, const _K_e_y_I_n_f_o &keyInfo); │ │ │ │ │ +121 │ │ │ │ │ +123 │ │ │ │ │ +124} │ │ │ │ │ +125 │ │ │ │ │ +_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_S_o_l_v_e_r_._h │ │ │ │ │ +Implementation of Conjugate Gradient solver for a linear system. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_b_u_i_l_d_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, const │ │ │ │ │ +map< Key, size_t > &dimensions) │ │ │ │ │ +Create VectorValues from a Vector. │ │ │ │ │ +DDeeffiinniittiioonn PCGSolver.cpp:151 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ +Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key │ │ │ │ │ +landmarkKey) │ │ │ │ │ +Optimize for triangulation. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.cpp:155 │ │ │ │ │ _g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ print without optional string, must specify cout yourself │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ -of vertical blocks. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_c_o_n_s_t___i_t_e_r_a_t_o_r │ │ │ │ │ -KeyVector::const_iterator const_iterator │ │ │ │ │ -Const iterator over keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:80 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -A GaussianConditional functions as the node in a Bayes network. │ │ │ │ │ -DDeeffiinniittiioonn GaussianConditional.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_T_h_i_s │ │ │ │ │ -GaussianConditional This │ │ │ │ │ -Typedef to this class. │ │ │ │ │ -DDeeffiinniittiioonn GaussianConditional.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_S │ │ │ │ │ -constABlock S(const_iterator it) const │ │ │ │ │ -Get a view of the S matrix for the variable pointed to by the given key │ │ │ │ │ -iterator. │ │ │ │ │ -DDeeffiinniittiioonn GaussianConditional.h:224 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_R │ │ │ │ │ -constABlock R() const │ │ │ │ │ -Return a view of the upper-triangular R block of the conditional. │ │ │ │ │ -DDeeffiinniittiioonn GaussianConditional.h:218 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_B_a_s_e_F_a_c_t_o_r │ │ │ │ │ -JacobianFactor BaseFactor │ │ │ │ │ -Typedef to our factor base class. │ │ │ │ │ -DDeeffiinniittiioonn GaussianConditional.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -GaussianConditional() │ │ │ │ │ -default constructor needed for serialization │ │ │ │ │ -DDeeffiinniittiioonn GaussianConditional.h:54 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_B_a_s_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -Conditional< BaseFactor, This > BaseConditional │ │ │ │ │ -Typedef to our conditional base class. │ │ │ │ │ -DDeeffiinniittiioonn GaussianConditional.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_C_o_m_b_i_n_e │ │ │ │ │ -static shared_ptr Combine(ITERATOR firstConditional, ITERATOR lastConditional) │ │ │ │ │ -Combine several GaussianConditional into a single dense GC. │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_d_e_t_e_r_m_i_n_a_n_t │ │ │ │ │ -double determinant() const │ │ │ │ │ -Compute the determinant of the R matrix. │ │ │ │ │ -DDeeffiinniittiioonn GaussianConditional.h:240 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ -double operator()(const VectorValues &x) const │ │ │ │ │ -Evaluate probability density, sugar. │ │ │ │ │ -DDeeffiinniittiioonn GaussianConditional.h:158 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d_M_e_a_n_A_n_d_S_t_d_d_e_v │ │ │ │ │ -static shared_ptr sharedMeanAndStddev(Args &&... args) │ │ │ │ │ -Create shared pointer by forwarding arguments to fromMeanAndStddev. │ │ │ │ │ -DDeeffiinniittiioonn GaussianConditional.h:105 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_S │ │ │ │ │ -constABlock S() const │ │ │ │ │ -Get a view of the parent blocks. │ │ │ │ │ -DDeeffiinniittiioonn GaussianConditional.h:221 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn GaussianConditional.h:46 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_d │ │ │ │ │ -const constBVector d() const │ │ │ │ │ -Get a view of the r.h.s. │ │ │ │ │ -DDeeffiinniittiioonn GaussianConditional.h:227 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -An abstract virtual base class for JacobianFactor and HessianFactor. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactor.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ -A Gaussian factor in the squared-error form. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:91 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:96 │ │ │ │ │ +_g_t_s_a_m_:_:_a_x_p_y │ │ │ │ │ +void axpy(double alpha, const Errors &x, Errors &y) │ │ │ │ │ +BLAS level 2 style AXPY, y := alpha*x + y │ │ │ │ │ +DDeeffiinniittiioonn Errors.cpp:111 │ │ │ │ │ +_g_t_s_a_m_:_:_d_o_t │ │ │ │ │ +double dot(const V1 &a, const V2 &b) │ │ │ │ │ +Dot product. │ │ │ │ │ +DDeeffiinniittiioonn Vector.h:195 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ +parameters for the conjugate gradient method │ │ │ │ │ +DDeeffiinniittiioonn ConjugateGradientSolver.h:29 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_I_t_e_r_a_t_i_v_e_S_o_l_v_e_r │ │ │ │ │ +Base class for Iterative Solvers like SubgraphSolver. │ │ │ │ │ +DDeeffiinniittiioonn IterativeSolver.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_I_n_f_o │ │ │ │ │ +Handy data structure for iterative solvers. │ │ │ │ │ +DDeeffiinniittiioonn IterativeSolver.h:126 │ │ │ │ │ +_g_t_s_a_m_:_:_P_C_G_S_o_l_v_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ +Parameters for PCG. │ │ │ │ │ +DDeeffiinniittiioonn PCGSolver.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_P_C_G_S_o_l_v_e_r │ │ │ │ │ +A virtual base class for the preconditioned conjugate gradient solver. │ │ │ │ │ +DDeeffiinniittiioonn PCGSolver.h:62 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_S_y_s_t_e_m │ │ │ │ │ +System class needed for calling preconditionedConjugateGradient. │ │ │ │ │ +DDeeffiinniittiioonn PCGSolver.h:89 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ +DDeeffiinniittiioonn Preconditioner.h:24 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_c_o_n_d_i_t_i_o_n_e_r │ │ │ │ │ +DDeeffiinniittiioonn Preconditioner.h:64 │ │ │ │ │ _g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ with a unique integer... │ │ │ │ │ DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +The Factor::error simply extracts the. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ + * PPCCGGSSoollvveerr..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00836.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianFactorGraph.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianEliminationTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,55 +94,42 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
│ │ │ │ -
GaussianFactorGraph.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
GaussianEliminationTree.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Linear Factor Graph where all factors are Gaussians. │ │ │ │ -More...

│ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::GaussianEliminationTree
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Typedefs

│ │ │ │ -using gtsam::SparseTriplets = std::vector< std::tuple< int, int, double > >
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

bool gtsam::hasConstraints (const GaussianFactorGraph &factors)
 Evaluates whether linear factors have any constrained noise models.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Linear Factor Graph where all factors are Gaussians.

│ │ │ │ -
Author
Kai Ni
│ │ │ │ -
│ │ │ │ -Christian Potthast
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ +
Date
Mar 29, 2013
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │ -Frank Dellaert
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,30 +1,25 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -GaussianFactorGraph.cpp File Reference │ │ │ │ │ -Linear Factor Graph where all factors are Gaussians. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +GaussianEliminationTree.h File Reference │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -using  ggttssaamm::::SSppaarrsseeTTrriipplleettss = std::vector< std::tuple< int, int, double > > │ │ │ │ │ -  │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -bool  _g_t_s_a_m_:_:_h_a_s_C_o_n_s_t_r_a_i_n_t_s (const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &factors) │ │ │ │ │ -  Evaluates whether linear factors have any constrained noise models. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ + Date │ │ │ │ │ + Mar 29, 2013 │ │ │ │ │ Author │ │ │ │ │ - Kai Ni │ │ │ │ │ - Christian Potthast │ │ │ │ │ - Richard Roberts │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._c_p_p │ │ │ │ │ + * _G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00836.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a00836 = [ │ │ │ │ │ - ["hasConstraints", "a00836.html#a35c269c3243cab16a7475239a9c91021", null] │ │ │ │ │ + ["gtsam::GaussianEliminationTree", "a03776.html", "a03776"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00839.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianFactor.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/RegularJacobianFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,35 +94,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
GaussianFactor.cpp File Reference
│ │ │ │ +
RegularJacobianFactor.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

A factor with a quadratic error function - a Gaussian. │ │ │ │ +

JacobianFactor class with fixed sized blcoks. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::RegularJacobianFactor< D >
 JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

A factor with a quadratic error function - a Gaussian.

│ │ │ │ -

GaussianFactor

Author
Fan Jiang
│ │ │ │ +

JacobianFactor class with fixed sized blcoks.

│ │ │ │ +
Author
Sungtae An
│ │ │ │ +
Date
Nov 11, 2014
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -GaussianFactor.cpp File Reference │ │ │ │ │ -A factor with a quadratic error function - a Gaussian. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +RegularJacobianFactor.h File Reference │ │ │ │ │ +_J_a_c_o_b_i_a_n_F_a_c_t_o_r class with fixed sized blcoks. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_<_ _D_ _> │ │ │ │ │ +  _J_a_c_o_b_i_a_n_F_a_c_t_o_r with constant sized blocks Provides raw memory access │ │ │ │ │ + versions of linear operator. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A factor with a quadratic error function - a Gaussian. │ │ │ │ │ -GaussianFactor │ │ │ │ │ +_J_a_c_o_b_i_a_n_F_a_c_t_o_r class with fixed sized blcoks. │ │ │ │ │ Author │ │ │ │ │ - Fan Jiang │ │ │ │ │ + Sungtae An │ │ │ │ │ + Date │ │ │ │ │ + Nov 11, 2014 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_F_a_c_t_o_r_._c_p_p │ │ │ │ │ + * _R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00842.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/SubgraphBuilder.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianEliminationTree.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,57 +94,34 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
SubgraphBuilder.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
GaussianEliminationTree.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::Subgraph
 
struct  gtsam::Subgraph::Edge
 
struct  gtsam::SubgraphBuilderParameters
 
class  gtsam::SubgraphBuilder
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -GaussianFactorGraph gtsam::buildFactorSubgraph (const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone)
 Select the factors in a factor graph according to the subgraph.
 
std::pair< GaussianFactorGraph, GaussianFactorGraph > gtsam::splitFactorGraph (const GaussianFactorGraph &factorGraph, const Subgraph &subgraph)
 Split the graph into a subgraph and the remaining edges.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Dec 31, 2009
│ │ │ │ -
Author
Frank Dellaert, Yong-Dian Jian
│ │ │ │ +
Date
Mar 29, 2013
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,43 +1,21 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -SubgraphBuilder.h File Reference │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_S_u_b_g_r_a_p_h │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_:_:_E_d_g_e │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_B_u_i_l_d_e_r │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +GaussianEliminationTree.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ - _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h  ggttssaamm::::bbuuiillddFFaaccttoorrSSuubbggrraapphh (const │ │ │ │ │ - _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, const _S_u_b_g_r_a_p_h │ │ │ │ │ - &subgraph, const bool clone) │ │ │ │ │ -  Select the factors in a factor graph according │ │ │ │ │ - to the subgraph. │ │ │ │ │ -  │ │ │ │ │ -std::pair< _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h, _g_t_s_a_m_:_:_s_p_l_i_t_F_a_c_t_o_r_G_r_a_p_h (const │ │ │ │ │ - _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h >  _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &factorGraph, const │ │ │ │ │ - _S_u_b_g_r_a_p_h &subgraph) │ │ │ │ │ -  Split the graph into a subgraph and the │ │ │ │ │ - remaining edges. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Date │ │ │ │ │ - Dec 31, 2009 │ │ │ │ │ + Mar 29, 2013 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert, Yong-Dian Jian │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_._h │ │ │ │ │ + * _G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00848.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianBayesNet.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/HessianFactor-inl.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,47 +94,38 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
GaussianBayesNet.h File Reference
│ │ │ │ +
HessianFactor-inl.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Chordal Bayes Net, the result of eliminating a factor graph. │ │ │ │ +

Contains the HessianFactor class, a general quadratic factor. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::GaussianBayesNet
 GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals. More...
 
struct  gtsam::traits< GaussianBayesNet >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Chordal Bayes Net, the result of eliminating a factor graph.

│ │ │ │ -

GaussianBayesNet

Author
Frank Dellaert
│ │ │ │ +

Contains the HessianFactor class, a general quadratic factor.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Dec 8, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,30 +1,23 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -GaussianBayesNet.h File Reference │ │ │ │ │ -Chordal Bayes Net, the result of eliminating a factor graph. _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +HessianFactor-inl.h File Reference │ │ │ │ │ +Contains the HessianFactor class, a general quadratic factor. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t │ │ │ │ │ -  _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t is a Bayes net made from linear-Gaussian │ │ │ │ │ - conditionals. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Chordal Bayes Net, the result of eliminating a factor graph. │ │ │ │ │ -GaussianBayesNet │ │ │ │ │ +Contains the HessianFactor class, a general quadratic factor. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Dec 8, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h │ │ │ │ │ + * _H_e_s_s_i_a_n_F_a_c_t_o_r_-_i_n_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00848_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianBayesNet.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/HessianFactor-inl.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,192 +98,65 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
GaussianBayesNet.h
│ │ │ │ +
HessianFactor-inl.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19// \callgraph
│ │ │ │ +
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#pragma once
│ │ │ │ +
21namespace gtsam {
│ │ │ │
22
│ │ │ │ -
23#include <gtsam/linear/GaussianConditional.h>
│ │ │ │ -
24#include <gtsam/inference/BayesNet.h>
│ │ │ │ -
25#include <gtsam/inference/FactorGraph.h>
│ │ │ │ -
26#include <gtsam/global_includes.h>
│ │ │ │ -
27
│ │ │ │ -
28#include <utility>
│ │ │ │ -
29namespace gtsam {
│ │ │ │ -
30
│ │ │ │ -
│ │ │ │ -
35 class GTSAM_EXPORT GaussianBayesNet: public BayesNet<GaussianConditional>
│ │ │ │ -
36 {
│ │ │ │ -
37 public:
│ │ │ │ -
38
│ │ │ │ -
39 typedef BayesNet<GaussianConditional> Base;
│ │ │ │ -
40 typedef GaussianBayesNet This;
│ │ │ │ -
41 typedef GaussianConditional ConditionalType;
│ │ │ │ -
42 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
43 typedef boost::shared_ptr<ConditionalType> sharedConditional;
│ │ │ │ -
44
│ │ │ │ -
47
│ │ │ │ -
49 GaussianBayesNet() {}
│ │ │ │ -
50
│ │ │ │ -
52 template <typename ITERATOR>
│ │ │ │ -
│ │ │ │ -
53 GaussianBayesNet(ITERATOR firstConditional, ITERATOR lastConditional)
│ │ │ │ -
54 : Base(firstConditional, lastConditional) {}
│ │ │ │ +
23 /* ************************************************************************* */
│ │ │ │ +
24 template<typename KEYS>
│ │ │ │ +
│ │ │ │ +
25 HessianFactor::HessianFactor(const KEYS& keys, const SymmetricBlockMatrix& augmentedInformation) :
│ │ │ │ +
26 GaussianFactor(keys), info_(augmentedInformation)
│ │ │ │ +
27 {
│ │ │ │ +
28 // Check number of variables
│ │ │ │ +
29 if((DenseIndex)Base::keys_.size() != augmentedInformation.nBlocks() - 1)
│ │ │ │ +
30 throw std::invalid_argument(
│ │ │ │ +
31 "Error in HessianFactor constructor input. Number of provided keys plus\n"
│ │ │ │ +
32 "one for the information vector must equal the number of provided matrix blocks. ");
│ │ │ │ +
33
│ │ │ │ +
34 // Check RHS dimension
│ │ │ │ +
35 if(augmentedInformation.getDim(augmentedInformation.nBlocks() - 1) != 1)
│ │ │ │ +
36 throw std::invalid_argument(
│ │ │ │ +
37 "Error in HessianFactor constructor input. The last provided matrix block\n"
│ │ │ │ +
38 "must be the information vector, but the last provided block had more than one column.");
│ │ │ │ +
39 }
│ │ │ │
│ │ │ │ -
55
│ │ │ │ -
57 template <class CONTAINER>
│ │ │ │ -
│ │ │ │ -
58 explicit GaussianBayesNet(const CONTAINER& conditionals) {
│ │ │ │ -
59 push_back(conditionals);
│ │ │ │ -
60 }
│ │ │ │ -
│ │ │ │ -
61
│ │ │ │ -
64 template <class DERIVEDCONDITIONAL>
│ │ │ │ -
│ │ │ │ -
65 explicit GaussianBayesNet(const FactorGraph<DERIVEDCONDITIONAL>& graph)
│ │ │ │ -
66 : Base(graph) {}
│ │ │ │ -
│ │ │ │ -
67
│ │ │ │ -
72 template <class DERIVEDCONDITIONAL>
│ │ │ │ -
│ │ │ │ -
73 GaussianBayesNet(
│ │ │ │ -
74 std::initializer_list<boost::shared_ptr<DERIVEDCONDITIONAL> > conditionals)
│ │ │ │ -
75 : Base(conditionals) {}
│ │ │ │ -
│ │ │ │ -
76
│ │ │ │ -
78 virtual ~GaussianBayesNet() = default;
│ │ │ │ -
79
│ │ │ │ -
81
│ │ │ │ -
84
│ │ │ │ -
86 bool equals(const This& bn, double tol = 1e-9) const;
│ │ │ │ -
87
│ │ │ │ -
│ │ │ │ -
89 void print(
│ │ │ │ -
90 const std::string& s = "",
│ │ │ │ -
91 const KeyFormatter& formatter = DefaultKeyFormatter) const override {
│ │ │ │ -
92 Base::print(s, formatter);
│ │ │ │ -
93 }
│ │ │ │ -
│ │ │ │ -
94
│ │ │ │ -
96
│ │ │ │ -
99
│ │ │ │ -
101 double error(const VectorValues& x) const;
│ │ │ │ -
102
│ │ │ │ -
104 double logProbability(const VectorValues& x) const;
│ │ │ │ -
105
│ │ │ │ -
111 double evaluate(const VectorValues& x) const;
│ │ │ │ -
112
│ │ │ │ -
│ │ │ │ -
114 double operator()(const VectorValues& x) const {
│ │ │ │ -
115 return evaluate(x);
│ │ │ │ -
116 }
│ │ │ │ -
│ │ │ │ -
117
│ │ │ │ -
120 VectorValues optimize() const;
│ │ │ │ -
121
│ │ │ │ -
123 VectorValues optimize(const VectorValues& given) const;
│ │ │ │ -
124
│ │ │ │ -
131 VectorValues sample(std::mt19937_64* rng) const;
│ │ │ │ -
132
│ │ │ │ -
140 VectorValues sample(const VectorValues& given, std::mt19937_64* rng) const;
│ │ │ │ -
141
│ │ │ │ -
143 VectorValues sample() const;
│ │ │ │ -
144
│ │ │ │ -
146 VectorValues sample(const VectorValues& given) const;
│ │ │ │ -
147
│ │ │ │ -
154 Ordering ordering() const;
│ │ │ │ -
155
│ │ │ │ -
157
│ │ │ │ -
160
│ │ │ │ -
166 std::pair<Matrix, Vector> matrix(const Ordering& ordering) const;
│ │ │ │ -
167
│ │ │ │ -
173 std::pair<Matrix, Vector> matrix() const;
│ │ │ │ -
174
│ │ │ │ -
200 VectorValues optimizeGradientSearch() const;
│ │ │ │ -
201
│ │ │ │ -
207 VectorValues gradient(const VectorValues& x0) const;
│ │ │ │ -
208
│ │ │ │ -
215 VectorValues gradientAtZero() const;
│ │ │ │ -
216
│ │ │ │ -
224 double determinant() const;
│ │ │ │ -
225
│ │ │ │ -
232 double logDeterminant() const;
│ │ │ │ -
233
│ │ │ │ -
238 VectorValues backSubstitute(const VectorValues& gx) const;
│ │ │ │ -
239
│ │ │ │ -
246 VectorValues backSubstituteTranspose(const VectorValues& gx) const;
│ │ │ │ -
247
│ │ │ │ -
251
│ │ │ │ -
252 using Base::evaluate; // Expose evaluate(const HybridValues&) method..
│ │ │ │ -
253 using Base::logProbability; // Expose logProbability(const HybridValues&) method..
│ │ │ │ -
254 using Base::error; // Expose error(const HybridValues&) method..
│ │ │ │ -
255
│ │ │ │ -
257
│ │ │ │ -
258 private:
│ │ │ │ -
260 friend class boost::serialization::access;
│ │ │ │ -
261 template<class ARCHIVE>
│ │ │ │ -
262 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
263 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
264 }
│ │ │ │ -
265 };
│ │ │ │ -
│ │ │ │ -
266
│ │ │ │ -
268 template<>
│ │ │ │ -
│ │ │ │ -
269 struct traits<GaussianBayesNet> : public Testable<GaussianBayesNet> {
│ │ │ │ -
270 };
│ │ │ │ -
│ │ │ │ -
271
│ │ │ │ -
272} //\ namespace gtsam
│ │ │ │ -
FactorGraph.h
Factor Graph Base Class.
│ │ │ │ -
BayesNet.h
Bayes network.
│ │ │ │ -
GaussianConditional.h
Conditional Gaussian Base class.
│ │ │ │ -
global_includes.h
Included from all GTSAM files.
│ │ │ │ +
40
│ │ │ │ +
41}
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::BayesNet
A BayesNet is a tree of conditionals, stored in elimination order.
Definition BayesNet.h:35
│ │ │ │ -
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ -
gtsam::GaussianBayesNet
GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals.
Definition GaussianBayesNet.h:36
│ │ │ │ -
gtsam::GaussianBayesNet::operator()
double operator()(const VectorValues &x) const
Evaluate probability density, sugar.
Definition GaussianBayesNet.h:114
│ │ │ │ -
gtsam::GaussianBayesNet::print
void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print graph
Definition GaussianBayesNet.h:89
│ │ │ │ -
gtsam::GaussianBayesNet::GaussianBayesNet
GaussianBayesNet(std::initializer_list< boost::shared_ptr< DERIVEDCONDITIONAL > > conditionals)
Constructor that takes an initializer list of shared pointers.
Definition GaussianBayesNet.h:73
│ │ │ │ -
gtsam::GaussianBayesNet::GaussianBayesNet
GaussianBayesNet(const FactorGraph< DERIVEDCONDITIONAL > &graph)
Implicit copy/downcast constructor to override explicit template container constructor.
Definition GaussianBayesNet.h:65
│ │ │ │ -
gtsam::GaussianBayesNet::~GaussianBayesNet
virtual ~GaussianBayesNet()=default
Destructor.
│ │ │ │ -
gtsam::GaussianBayesNet::GaussianBayesNet
GaussianBayesNet(const CONTAINER &conditionals)
Construct from container of factors (shared_ptr or plain objects)
Definition GaussianBayesNet.h:58
│ │ │ │ -
gtsam::GaussianBayesNet::GaussianBayesNet
GaussianBayesNet()
Construct empty bayes net.
Definition GaussianBayesNet.h:49
│ │ │ │ -
gtsam::GaussianBayesNet::GaussianBayesNet
GaussianBayesNet(ITERATOR firstConditional, ITERATOR lastConditional)
Construct from iterator over conditionals.
Definition GaussianBayesNet.h:53
│ │ │ │ -
gtsam::GaussianConditional
A GaussianConditional functions as the node in a Bayes network.
Definition GaussianConditional.h:43
│ │ │ │ -
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │ +
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
│ │ │ │ +
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
│ │ │ │ +
gtsam::Factor::size
size_t size() const
Definition Factor.h:157
│ │ │ │ +
gtsam::GaussianFactor
An abstract virtual base class for JacobianFactor and HessianFactor.
Definition GaussianFactor.h:39
│ │ │ │ +
gtsam::HessianFactor::augmentedInformation
Matrix augmentedInformation() const override
Return the augmented information matrix represented by this GaussianFactor.
Definition HessianFactor.cpp:282
│ │ │ │ +
gtsam::HessianFactor::HessianFactor
HessianFactor()
default constructor for I/O
Definition HessianFactor.cpp:77
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,228 +1,80 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GaussianBayesNet.h │ │ │ │ │ +HessianFactor-inl.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19// \callgraph │ │ │ │ │ +19#pragma once │ │ │ │ │ 20 │ │ │ │ │ -21#pragma once │ │ │ │ │ +21namespace _g_t_s_a_m { │ │ │ │ │ 22 │ │ │ │ │ -23#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ -27 │ │ │ │ │ -28#include │ │ │ │ │ -29namespace _g_t_s_a_m { │ │ │ │ │ -30 │ │ │ │ │ -_3_5 class GTSAM_EXPORT _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t: public _B_a_y_e_s_N_e_t │ │ │ │ │ -36 { │ │ │ │ │ -37 public: │ │ │ │ │ -38 │ │ │ │ │ -39 typedef _B_a_y_e_s_N_e_t_<_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_> _B_a_s_e; │ │ │ │ │ -40 typedef _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t _T_h_i_s; │ │ │ │ │ -41 typedef _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l _C_o_n_d_i_t_i_o_n_a_l_T_y_p_e; │ │ │ │ │ -42 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -43 typedef boost::shared_ptr sharedConditional; │ │ │ │ │ -44 │ │ │ │ │ -47 │ │ │ │ │ -_4_9 _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t() {} │ │ │ │ │ -50 │ │ │ │ │ -52 template │ │ │ │ │ -_5_3 _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t(ITERATOR firstConditional, ITERATOR lastConditional) │ │ │ │ │ -54 : _B_a_s_e(firstConditional, lastConditional) {} │ │ │ │ │ -55 │ │ │ │ │ -57 template │ │ │ │ │ -_5_8 explicit _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t(const CONTAINER& conditionals) { │ │ │ │ │ -59 push_back(conditionals); │ │ │ │ │ -60 } │ │ │ │ │ -61 │ │ │ │ │ -64 template │ │ │ │ │ -_6_5 explicit _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t(const _F_a_c_t_o_r_G_r_a_p_h_<_D_E_R_I_V_E_D_C_O_N_D_I_T_I_O_N_A_L_>& graph) │ │ │ │ │ -66 : _B_a_s_e(graph) {} │ │ │ │ │ -67 │ │ │ │ │ -72 template │ │ │ │ │ -_7_3 _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t( │ │ │ │ │ -74 std::initializer_list > conditionals) │ │ │ │ │ -75 : _B_a_s_e(conditionals) {} │ │ │ │ │ -76 │ │ │ │ │ -_7_8 virtual _~_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t() = default; │ │ │ │ │ -79 │ │ │ │ │ -81 │ │ │ │ │ -84 │ │ │ │ │ -86 bool _e_q_u_a_l_s(const _T_h_i_s& bn, double tol = 1e-9) const; │ │ │ │ │ -87 │ │ │ │ │ -_8_9 void _p_r_i_n_t( │ │ │ │ │ -90 const std::string& s = "", │ │ │ │ │ -91 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override { │ │ │ │ │ -92 Base::print(s, formatter); │ │ │ │ │ -93 } │ │ │ │ │ -94 │ │ │ │ │ -96 │ │ │ │ │ -99 │ │ │ │ │ -101 double error(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const; │ │ │ │ │ -102 │ │ │ │ │ -104 double logProbability(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const; │ │ │ │ │ -105 │ │ │ │ │ -111 double evaluate(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const; │ │ │ │ │ -112 │ │ │ │ │ -_1_1_4 double _o_p_e_r_a_t_o_r_(_)(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const { │ │ │ │ │ -115 return evaluate(x); │ │ │ │ │ -116 } │ │ │ │ │ -117 │ │ │ │ │ -120 _V_e_c_t_o_r_V_a_l_u_e_s _o_p_t_i_m_i_z_e() const; │ │ │ │ │ -121 │ │ │ │ │ -123 _V_e_c_t_o_r_V_a_l_u_e_s _o_p_t_i_m_i_z_e(const _V_e_c_t_o_r_V_a_l_u_e_s& given) const; │ │ │ │ │ -124 │ │ │ │ │ -131 _V_e_c_t_o_r_V_a_l_u_e_s sample(std::mt19937_64* rng) const; │ │ │ │ │ -132 │ │ │ │ │ -140 _V_e_c_t_o_r_V_a_l_u_e_s sample(const _V_e_c_t_o_r_V_a_l_u_e_s& given, std::mt19937_64* rng) const; │ │ │ │ │ -141 │ │ │ │ │ -143 _V_e_c_t_o_r_V_a_l_u_e_s sample() const; │ │ │ │ │ -144 │ │ │ │ │ -146 _V_e_c_t_o_r_V_a_l_u_e_s sample(const _V_e_c_t_o_r_V_a_l_u_e_s& given) const; │ │ │ │ │ -147 │ │ │ │ │ -154 _O_r_d_e_r_i_n_g ordering() const; │ │ │ │ │ -155 │ │ │ │ │ -157 │ │ │ │ │ -160 │ │ │ │ │ -166 std::pair matrix(const _O_r_d_e_r_i_n_g& ordering) const; │ │ │ │ │ -167 │ │ │ │ │ -173 std::pair matrix() const; │ │ │ │ │ -174 │ │ │ │ │ -200 _V_e_c_t_o_r_V_a_l_u_e_s optimizeGradientSearch() const; │ │ │ │ │ -201 │ │ │ │ │ -207 _V_e_c_t_o_r_V_a_l_u_e_s gradient(const _V_e_c_t_o_r_V_a_l_u_e_s& x0) const; │ │ │ │ │ -208 │ │ │ │ │ -215 _V_e_c_t_o_r_V_a_l_u_e_s gradientAtZero() const; │ │ │ │ │ -216 │ │ │ │ │ -224 double determinant() const; │ │ │ │ │ -225 │ │ │ │ │ -232 double logDeterminant() const; │ │ │ │ │ -233 │ │ │ │ │ -238 _V_e_c_t_o_r_V_a_l_u_e_s backSubstitute(const _V_e_c_t_o_r_V_a_l_u_e_s& gx) const; │ │ │ │ │ -239 │ │ │ │ │ -246 _V_e_c_t_o_r_V_a_l_u_e_s backSubstituteTranspose(const _V_e_c_t_o_r_V_a_l_u_e_s& gx) const; │ │ │ │ │ -247 │ │ │ │ │ -251 │ │ │ │ │ -252 using Base::evaluate; // Expose evaluate(const HybridValues&) method.. │ │ │ │ │ -253 using Base::logProbability; // Expose logProbability(const HybridValues&) │ │ │ │ │ -method.. │ │ │ │ │ -254 using Base::error; // Expose error(const HybridValues&) method.. │ │ │ │ │ -255 │ │ │ │ │ -257 │ │ │ │ │ -258 private: │ │ │ │ │ -_2_6_0 friend class boost::serialization::access; │ │ │ │ │ -261 template │ │ │ │ │ -262 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -263 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -264 } │ │ │ │ │ -265 }; │ │ │ │ │ -266 │ │ │ │ │ -268 template<> │ │ │ │ │ -_2_6_9 struct _t_r_a_i_t_s<_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t> : public _T_e_s_t_a_b_l_e { │ │ │ │ │ -270 }; │ │ │ │ │ -271 │ │ │ │ │ -272} //\ namespace gtsam │ │ │ │ │ -_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Factor Graph Base Class. │ │ │ │ │ -_B_a_y_e_s_N_e_t_._h │ │ │ │ │ -Bayes network. │ │ │ │ │ -_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ -Conditional Gaussian Base class. │ │ │ │ │ -_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ -Included from all GTSAM files. │ │ │ │ │ +23 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +24 template │ │ │ │ │ +_2_5 _H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r(const KEYS& keys, const _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x& │ │ │ │ │ +augmentedInformation) : │ │ │ │ │ +26 _G_a_u_s_s_i_a_n_F_a_c_t_o_r(keys), info_(augmentedInformation) │ │ │ │ │ +27 { │ │ │ │ │ +28 // Check number of variables │ │ │ │ │ +29 if((_D_e_n_s_e_I_n_d_e_x)_B_a_s_e_:_:_k_e_y_s__._s_i_z_e() != _a_u_g_m_e_n_t_e_d_I_n_f_o_r_m_a_t_i_o_n.nBlocks() - 1) │ │ │ │ │ +30 throw std::invalid_argument( │ │ │ │ │ +31 "Error in HessianFactor constructor input. Number of provided keys plus\n" │ │ │ │ │ +32 "one for the information vector must equal the number of provided matrix │ │ │ │ │ +blocks. "); │ │ │ │ │ +33 │ │ │ │ │ +34 // Check RHS dimension │ │ │ │ │ +35 if(_a_u_g_m_e_n_t_e_d_I_n_f_o_r_m_a_t_i_o_n.getDim(_a_u_g_m_e_n_t_e_d_I_n_f_o_r_m_a_t_i_o_n.nBlocks() - 1) != 1) │ │ │ │ │ +36 throw std::invalid_argument( │ │ │ │ │ +37 "Error in HessianFactor constructor input. The last provided matrix block\n" │ │ │ │ │ +38 "must be the information vector, but the last provided block had more than │ │ │ │ │ +one column."); │ │ │ │ │ +39 } │ │ │ │ │ +40 │ │ │ │ │ +41} │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ -Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key │ │ │ │ │ -landmarkKey) │ │ │ │ │ -Optimize for triangulation. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.cpp:155 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t │ │ │ │ │ -A BayesNet is a tree of conditionals, stored in elimination order. │ │ │ │ │ -DDeeffiinniittiioonn BayesNet.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ -nodes. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t │ │ │ │ │ -GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals. │ │ │ │ │ -DDeeffiinniittiioonn GaussianBayesNet.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ -double operator()(const VectorValues &x) const │ │ │ │ │ -Evaluate probability density, sugar. │ │ │ │ │ -DDeeffiinniittiioonn GaussianBayesNet.h:114 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&formatter=DefaultKeyFormatter) const override │ │ │ │ │ -print graph │ │ │ │ │ -DDeeffiinniittiioonn GaussianBayesNet.h:89 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t │ │ │ │ │ -GaussianBayesNet(std::initializer_list< boost::shared_ptr< DERIVEDCONDITIONAL > │ │ │ │ │ -> conditionals) │ │ │ │ │ -Constructor that takes an initializer list of shared pointers. │ │ │ │ │ -DDeeffiinniittiioonn GaussianBayesNet.h:73 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t │ │ │ │ │ -GaussianBayesNet(const FactorGraph< DERIVEDCONDITIONAL > &graph) │ │ │ │ │ -Implicit copy/downcast constructor to override explicit template container │ │ │ │ │ -constructor. │ │ │ │ │ -DDeeffiinniittiioonn GaussianBayesNet.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_:_:_~_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t │ │ │ │ │ -virtual ~GaussianBayesNet()=default │ │ │ │ │ -Destructor. │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t │ │ │ │ │ -GaussianBayesNet(const CONTAINER &conditionals) │ │ │ │ │ -Construct from container of factors (shared_ptr or plain objects) │ │ │ │ │ -DDeeffiinniittiioonn GaussianBayesNet.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t │ │ │ │ │ -GaussianBayesNet() │ │ │ │ │ -Construct empty bayes net. │ │ │ │ │ -DDeeffiinniittiioonn GaussianBayesNet.h:49 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t │ │ │ │ │ -GaussianBayesNet(ITERATOR firstConditional, ITERATOR lastConditional) │ │ │ │ │ -Construct from iterator over conditionals. │ │ │ │ │ -DDeeffiinniittiioonn GaussianBayesNet.h:53 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -A GaussianConditional functions as the node in a Bayes network. │ │ │ │ │ -DDeeffiinniittiioonn GaussianConditional.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ -with a unique integer... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_n_s_e_I_n_d_e_x │ │ │ │ │ +ptrdiff_t DenseIndex │ │ │ │ │ +The index type for Eigen objects. │ │ │ │ │ +DDeeffiinniittiioonn types.h:106 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ +of blocks. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s__ │ │ │ │ │ +KeyVector keys_ │ │ │ │ │ +The keys involved in this factor. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_s_i_z_e │ │ │ │ │ +size_t size() const │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:157 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +An abstract virtual base class for JacobianFactor and HessianFactor. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactor.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_a_u_g_m_e_n_t_e_d_I_n_f_o_r_m_a_t_i_o_n │ │ │ │ │ +Matrix augmentedInformation() const override │ │ │ │ │ +Return the augmented information matrix represented by this GaussianFactor. │ │ │ │ │ +DDeeffiinniittiioonn HessianFactor.cpp:282 │ │ │ │ │ +_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +HessianFactor() │ │ │ │ │ +default constructor for I/O │ │ │ │ │ +DDeeffiinniittiioonn HessianFactor.cpp:77 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h │ │ │ │ │ + * _H_e_s_s_i_a_n_F_a_c_t_o_r_-_i_n_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00851.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/iterative.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/JacobianFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,61 +95,52 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces | │ │ │ │ +Typedefs | │ │ │ │ Functions
│ │ │ │ -
iterative.cpp File Reference
│ │ │ │ +
JacobianFactor.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -

Iterative methods, implementation. │ │ │ │ -More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Typedefs

│ │ │ │ +using gtsam::Pairs = std::vector< std::pair< Key, Matrix > >
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

│ │ │ │ -Vector gtsam::steepestDescent (const System &Ab, const Vector &x, const ConjugateGradientParameters &parameters)
 
│ │ │ │ -Vector gtsam::conjugateGradientDescent (const System &Ab, const Vector &x, const ConjugateGradientParameters &parameters)
 Method of conjugate gradients (CG), System version.
 
Vector gtsam::steepestDescent (const Matrix &A, const Vector &b, const Vector &x, const ConjugateGradientParameters &parameters)
 convenience calls using matrices, will create System class internally:
 
│ │ │ │ -Vector gtsam::conjugateGradientDescent (const Matrix &A, const Vector &b, const Vector &x, const ConjugateGradientParameters &parameters)
 Method of conjugate gradients (CG), Matrix version.
 
│ │ │ │ -VectorValues gtsam::steepestDescent (const GaussianFactorGraph &fg, const VectorValues &x, const ConjugateGradientParameters &parameters)
 Method of steepest gradients, Gaussian Factor Graph version.
 
│ │ │ │ -VectorValues gtsam::conjugateGradientDescent (const GaussianFactorGraph &fg, const VectorValues &x, const ConjugateGradientParameters &parameters)
 Method of conjugate gradients (CG), Gaussian Factor Graph version.
 
│ │ │ │ +FastVector< VariableSlots::const_iterator > gtsam::orderedSlotsHelper (const Ordering &ordering, const VariableSlots &variableSlots)
 
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > gtsam::EliminateQR (const GaussianFactorGraph &factors, const Ordering &keys)
 Multiply all factors and eliminate the given keys from the resulting factor using a QR variant that handles constraints (zero sigmas).
 
│ │ │ │

Detailed Description

│ │ │ │ -

Iterative methods, implementation.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
Date
Dec 28, 2009
│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
│ │ │ │ +Christian Potthast
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +
Date
Dec 8, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,50 +1,38 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -iterative.cpp File Reference │ │ │ │ │ -Iterative methods, implementation. _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +JacobianFactor.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ - Vector  ggttssaamm::::sstteeeeppeessttDDeesscceenntt (const _S_y_s_t_e_m &Ab, const Vector &x, const │ │ │ │ │ - _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s ¶meters) │ │ │ │ │ -  │ │ │ │ │ - Vector  ggttssaamm::::ccoonnjjuuggaatteeGGrraaddiieennttDDeesscceenntt (const _S_y_s_t_e_m &Ab, const Vector │ │ │ │ │ - &x, const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s ¶meters) │ │ │ │ │ -  Method of conjugate gradients (CG), _S_y_s_t_e_m version. │ │ │ │ │ -  │ │ │ │ │ - Vector  _g_t_s_a_m_:_:_s_t_e_e_p_e_s_t_D_e_s_c_e_n_t (const Matrix &A, const Vector &b, const │ │ │ │ │ - Vector &x, const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s ¶meters) │ │ │ │ │ -  convenience calls using matrices, will create _S_y_s_t_e_m class │ │ │ │ │ - internally: │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +using  ggttssaamm::::PPaaiirrss = std::vector< std::pair< _K_e_y, Matrix > > │ │ │ │ │   │ │ │ │ │ - Vector  ggttssaamm::::ccoonnjjuuggaatteeGGrraaddiieennttDDeesscceenntt (const Matrix &A, const Vector │ │ │ │ │ - &b, const Vector &x, const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ - ¶meters) │ │ │ │ │ -  Method of conjugate gradients (CG), Matrix version. │ │ │ │ │ -  │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s  ggttssaamm::::sstteeeeppeessttDDeesscceenntt (const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &fg, const │ │ │ │ │ - _V_e_c_t_o_r_V_a_l_u_e_s &x, const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s ¶meters) │ │ │ │ │ -  Method of steepest gradients, Gaussian _F_a_c_t_o_r Graph version. │ │ │ │ │ -  │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s  ggttssaamm::::ccoonnjjuuggaatteeGGrraaddiieennttDDeesscceenntt (const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &fg, │ │ │ │ │ - const _V_e_c_t_o_r_V_a_l_u_e_s &x, const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ - ¶meters) │ │ │ │ │ -  Method of conjugate gradients (CG), Gaussian _F_a_c_t_o_r Graph │ │ │ │ │ - version. │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ + _F_a_s_t_V_e_c_t_o_r< VariableSlots:: ggttssaamm::::oorrddeerreeddSSlloottssHHeellppeerr (const │ │ │ │ │ + const_iterator >  _O_r_d_e_r_i_n_g &ordering, const _V_a_r_i_a_b_l_e_S_l_o_t_s │ │ │ │ │ + &variableSlots) │ │ │ │ │ +  │ │ │ │ │ + std::pair< _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_: _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_Q_R (const │ │ │ │ │ +_s_h_a_r_e_d___p_t_r, _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &factors, const │ │ │ │ │ + >  _O_r_d_e_r_i_n_g &keys) │ │ │ │ │ + Multiply all factors and eliminate the │ │ │ │ │ +  given keys from the resulting factor │ │ │ │ │ + using a QR variant that handles │ │ │ │ │ + constraints (zero sigmas). │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Iterative methods, implementation. │ │ │ │ │ Author │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Christian Potthast │ │ │ │ │ Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - Dec 28, 2009 │ │ │ │ │ + Dec 8, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _i_t_e_r_a_t_i_v_e_._c_p_p │ │ │ │ │ + * _J_a_c_o_b_i_a_n_F_a_c_t_o_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00851.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,7 +1,3 @@ │ │ │ │ │ var a00851 = [ │ │ │ │ │ - ["conjugateGradientDescent", "a00851.html#aa84114ad1593684ad739088a2898afdb", null], │ │ │ │ │ - ["conjugateGradientDescent", "a00851.html#af55440f741d8b2f706101f7a79ba7111", null], │ │ │ │ │ - ["conjugateGradientDescent", "a00851.html#ad2b587fde5d35a27d6c88feb4432e785", null], │ │ │ │ │ - ["steepestDescent", "a00851.html#a81b7af7638d28555c3f404318cc67b22", null], │ │ │ │ │ - ["steepestDescent", "a00851.html#aed4b2ca704ddd6530ea70122f883ac88", null] │ │ │ │ │ + ["EliminateQR", "a00851.html#ab56c4bfa363f8c23c48eb078e9c84a9c", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00854.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Scatter.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianBayesTree-inl.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,34 +94,51 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces
│ │ │ │ -
Scatter.cpp File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
GaussianBayesTree-inl.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ + │ │ │ │ +

Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ +More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +template<class BAYESTREE >
void gtsam::internal::optimizeInPlace (const typename BAYESTREE::sharedClique &clique, VectorValues &result)
 
│ │ │ │ +template<class BAYESTREE >
double gtsam::internal::logDeterminant (const typename BAYESTREE::sharedClique &clique)
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ +

Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.

│ │ │ │ +

GaussianBayesTree

Author
Frank Dellaert
│ │ │ │
│ │ │ │ -Frank Dellaert
│ │ │ │ -
Date
June 2015
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,32 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Scatter.cpp File Reference │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +GaussianBayesTree-inl.h File Reference │ │ │ │ │ +Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +template │ │ │ │ │ + void  ggttssaamm::::iinntteerrnnaall::::ooppttiimmiizzeeIInnPPllaaccee (const typename BAYESTREE:: │ │ │ │ │ + sharedClique &clique, _V_e_c_t_o_r_V_a_l_u_e_s &result) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +double  ggttssaamm::::iinntteerrnnaall::::llooggDDeetteerrmmiinnaanntt (const typename BAYESTREE::sharedClique │ │ │ │ │ + &clique) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ │ +GaussianBayesTree │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - June 2015 │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _S_c_a_t_t_e_r_._c_p_p │ │ │ │ │ + * _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_-_i_n_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00857.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Errors.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/SubgraphBuilder.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,85 +96,55 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs | │ │ │ │ Functions
│ │ │ │ -
Errors.h File Reference
│ │ │ │ +
SubgraphBuilder.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

vector of errors │ │ │ │ -More...

│ │ │ │ - │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

struct  gtsam::traits< Errors >
 traits More...
class  gtsam::Subgraph
 
struct  gtsam::Subgraph::Edge
 
struct  gtsam::SubgraphBuilderParameters
 
class  gtsam::SubgraphBuilder
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Typedefs

│ │ │ │ -using gtsam::Errors = FastList< Vector >
 Errors is a vector of errors.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

│ │ │ │ -Errors gtsam::createErrors (const VectorValues &V)
 Break V into pieces according to its start indices.
 
│ │ │ │ -void gtsam::print (const Errors &e, const std::string &s="Errors")
 Print an Errors instance.
 
│ │ │ │ -bool gtsam::equality (const Errors &actual, const Errors &expected, double tol)
 
│ │ │ │ -Errors gtsam::operator+ (const Errors &a, const Errors &b)
 Addition.
 
│ │ │ │ -Errors gtsam::operator- (const Errors &a, const Errors &b)
 Subtraction.
 
│ │ │ │ -Errors gtsam::operator- (const Errors &a)
 Negation.
 
│ │ │ │ -double gtsam::dot (const Errors &a, const Errors &b)
 Dot product.
 
│ │ │ │ -void gtsam::axpy (double alpha, const Errors &x, Errors &y)
 BLAS level 2 style AXPY, y := alpha*x + y
 
│ │ │ │ +GaussianFactorGraph gtsam::buildFactorSubgraph (const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone)
 Select the factors in a factor graph according to the subgraph.
 
std::pair< GaussianFactorGraph, GaussianFactorGraph > gtsam::splitFactorGraph (const GaussianFactorGraph &factorGraph, const Subgraph &subgraph)
 Split the graph into a subgraph and the remaining edges.
 
│ │ │ │

Detailed Description

│ │ │ │ -

vector of errors

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +
Date
Dec 31, 2009
│ │ │ │ +
Author
Frank Dellaert, Yong-Dian Jian
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,54 +1,43 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Errors.h File Reference │ │ │ │ │ -vector of errors _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +SubgraphBuilder.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _E_r_r_o_r_s_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_S_u_b_g_r_a_p_h │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_:_:_E_d_g_e │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_B_u_i_l_d_e_r │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -using  ggttssaamm::::EErrrroorrss = _F_a_s_t_L_i_s_t< Vector > │ │ │ │ │ -  Errors is a vector of errors. │ │ │ │ │ -  │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -_E_r_r_o_r_s  ggttssaamm::::ccrreeaatteeEErrrroorrss (const _V_e_c_t_o_r_V_a_l_u_e_s &V) │ │ │ │ │ -  Break V into pieces according to its start indices. │ │ │ │ │ -  │ │ │ │ │ - void  ggttssaamm::::pprriinntt (const _E_r_r_o_r_s &e, const std::string &s="Errors") │ │ │ │ │ -  Print an Errors instance. │ │ │ │ │ -  │ │ │ │ │ - bool  ggttssaamm::::eeqquuaalliittyy (const _E_r_r_o_r_s &actual, const _E_r_r_o_r_s &expected, double │ │ │ │ │ - tol) │ │ │ │ │ -  │ │ │ │ │ -_E_r_r_o_r_s  ggttssaamm::::ooppeerraattoorr++ (const _E_r_r_o_r_s &a, const _E_r_r_o_r_s &b) │ │ │ │ │ -  Addition. │ │ │ │ │ -  │ │ │ │ │ -_E_r_r_o_r_s  ggttssaamm::::ooppeerraattoorr-- (const _E_r_r_o_r_s &a, const _E_r_r_o_r_s &b) │ │ │ │ │ -  Subtraction. │ │ │ │ │ -  │ │ │ │ │ -_E_r_r_o_r_s  ggttssaamm::::ooppeerraattoorr-- (const _E_r_r_o_r_s &a) │ │ │ │ │ -  Negation. │ │ │ │ │ -  │ │ │ │ │ -double  ggttssaamm::::ddoott (const _E_r_r_o_r_s &a, const _E_r_r_o_r_s &b) │ │ │ │ │ -  Dot product. │ │ │ │ │ -  │ │ │ │ │ - void  ggttssaamm::::aaxxppyy (double alpha, const _E_r_r_o_r_s &x, _E_r_r_o_r_s &y) │ │ │ │ │ -  BLAS level 2 style AXPY, y := alpha*x + y │ │ │ │ │ + _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h  ggttssaamm::::bbuuiillddFFaaccttoorrSSuubbggrraapphh (const │ │ │ │ │ + _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, const _S_u_b_g_r_a_p_h │ │ │ │ │ + &subgraph, const bool clone) │ │ │ │ │ +  Select the factors in a factor graph according │ │ │ │ │ + to the subgraph. │ │ │ │ │ +  │ │ │ │ │ +std::pair< _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h, _g_t_s_a_m_:_:_s_p_l_i_t_F_a_c_t_o_r_G_r_a_p_h (const │ │ │ │ │ + _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h >  _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &factorGraph, const │ │ │ │ │ + _S_u_b_g_r_a_p_h &subgraph) │ │ │ │ │ +  Split the graph into a subgraph and the │ │ │ │ │ + remaining edges. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -vector of errors │ │ │ │ │ + Date │ │ │ │ │ + Dec 31, 2009 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Frank Dellaert, Yong-Dian Jian │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _E_r_r_o_r_s_._h │ │ │ │ │ + * _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00857.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,11 +1,8 @@ │ │ │ │ │ var a00857 = [ │ │ │ │ │ - ["gtsam::traits< Errors >", "a03736.html", null], │ │ │ │ │ - ["Errors", "a00857.html#a74eb09e3192f06bae9de746aa981624a", null], │ │ │ │ │ - ["axpy", "a00857.html#a5644e12e89afaac433f5d61c3d8e9003", null], │ │ │ │ │ - ["createErrors", "a00857.html#aa84fa88db91d429bec261c655fc54bd6", null], │ │ │ │ │ - ["dot", "a00857.html#a106ec458ddb425604809da93346af55d", null], │ │ │ │ │ - ["operator+", "a00857.html#a552de56fca49d1b4942dd9b8e8357b0a", null], │ │ │ │ │ - ["operator-", "a00857.html#aa91c0485819ec2bfd249a922c81e3eb4", null], │ │ │ │ │ - ["operator-", "a00857.html#af2dc48a6a2e9abf4d0636578847a6b5a", null], │ │ │ │ │ - ["print", "a00857.html#a6abcac97e441f1454790ff53caa1b229", null] │ │ │ │ │ + ["gtsam::Subgraph", "a04032.html", "a04032"], │ │ │ │ │ + ["gtsam::Subgraph::Edge", "a04036.html", null], │ │ │ │ │ + ["gtsam::SubgraphBuilderParameters", "a04040.html", "a04040"], │ │ │ │ │ + ["gtsam::SubgraphBuilder", "a04044.html", null], │ │ │ │ │ + ["buildFactorSubgraph", "a00857.html#a0ec82adc6e13261cf4a012b65b301256", null], │ │ │ │ │ + ["splitFactorGraph", "a00857.html#a2c7eca9f27b43b52756c1afd85478dd8", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00857_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Errors.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/SubgraphBuilder.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,95 +98,216 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Errors.h
│ │ │ │ +
SubgraphBuilder.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18// \callgraph
│ │ │ │ +
18#pragma once
│ │ │ │
19
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/base/FastList.h>
│ │ │ │ -
23#include <gtsam/base/Testable.h>
│ │ │ │ -
24#include <gtsam/base/Vector.h>
│ │ │ │ -
25
│ │ │ │ -
26#include <string>
│ │ │ │ +
20#include <gtsam/base/FastMap.h>
│ │ │ │ +
21#include <gtsam/base/types.h>
│ │ │ │ +
22#include <gtsam/dllexport.h>
│ │ │ │ +
23
│ │ │ │ +
24#include <boost/serialization/version.hpp>
│ │ │ │ +
25#include <boost/serialization/nvp.hpp>
│ │ │ │ +
26#include <boost/shared_ptr.hpp>
│ │ │ │
27
│ │ │ │ -
28namespace gtsam {
│ │ │ │ +
28#include <vector>
│ │ │ │
29
│ │ │ │ -
30// Forward declarations
│ │ │ │ -
31class VectorValues;
│ │ │ │ -
32
│ │ │ │ -
34using Errors = FastList<Vector>;
│ │ │ │ +
30namespace boost {
│ │ │ │ +
31namespace serialization {
│ │ │ │ +
32class access;
│ │ │ │ +
33} /* namespace serialization */
│ │ │ │ +
34} /* namespace boost */
│ │ │ │
35
│ │ │ │ -
37GTSAM_EXPORT Errors createErrors(const VectorValues& V);
│ │ │ │ -
38
│ │ │ │ -
40GTSAM_EXPORT void print(const Errors& e, const std::string& s = "Errors");
│ │ │ │ +
36namespace gtsam {
│ │ │ │ +
37
│ │ │ │ +
38// Forward declarations
│ │ │ │ +
39class GaussianFactorGraph;
│ │ │ │ +
40struct PreconditionerParameters;
│ │ │ │
41
│ │ │ │ -
42// Check equality for unit testing.
│ │ │ │ -
43GTSAM_EXPORT bool equality(const Errors& actual, const Errors& expected,
│ │ │ │ -
44 double tol = 1e-9);
│ │ │ │ -
45
│ │ │ │ -
47GTSAM_EXPORT Errors operator+(const Errors& a, const Errors& b);
│ │ │ │ -
48
│ │ │ │ -
50GTSAM_EXPORT Errors operator-(const Errors& a, const Errors& b);
│ │ │ │ -
51
│ │ │ │ -
53GTSAM_EXPORT Errors operator-(const Errors& a);
│ │ │ │ -
54
│ │ │ │ -
56GTSAM_EXPORT double dot(const Errors& a, const Errors& b);
│ │ │ │ -
57
│ │ │ │ -
59GTSAM_EXPORT void axpy(double alpha, const Errors& x, Errors& y);
│ │ │ │ -
60
│ │ │ │ -
62template <>
│ │ │ │ -
│ │ │ │ -
63struct traits<Errors> {
│ │ │ │ -
64 static void Print(const Errors& e, const std::string& str = "") {
│ │ │ │ -
65 print(e, str);
│ │ │ │ -
66 }
│ │ │ │ -
67 static bool Equals(const Errors& actual, const Errors& expected,
│ │ │ │ -
68 double tol = 1e-8) {
│ │ │ │ -
69 return equality(actual, expected, tol);
│ │ │ │ -
70 }
│ │ │ │ -
71};
│ │ │ │ -
│ │ │ │ -
72
│ │ │ │ -
73} // namespace gtsam
│ │ │ │ -
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ -
FastList.h
A thin wrapper around std::list that uses boost's fast_pool_allocator.
│ │ │ │ +
42/**************************************************************************/
│ │ │ │ +
│ │ │ │ +
43class GTSAM_EXPORT Subgraph {
│ │ │ │ +
44 public:
│ │ │ │ +
│ │ │ │ +
45 struct GTSAM_EXPORT Edge {
│ │ │ │ +
46 size_t index; /* edge id */
│ │ │ │ +
47 double weight; /* edge weight */
│ │ │ │ +
48 inline bool isUnitWeight() const { return (weight == 1.0); }
│ │ │ │ +
49 friend std::ostream &operator<<(std::ostream &os, const Edge &edge);
│ │ │ │ +
50
│ │ │ │ +
51 private:
│ │ │ │ +
52 friend class boost::serialization::access;
│ │ │ │ +
53 template <class Archive>
│ │ │ │ +
54 void serialize(Archive &ar, const unsigned int /*version*/) {
│ │ │ │ +
55 ar &BOOST_SERIALIZATION_NVP(index);
│ │ │ │ +
56 ar &BOOST_SERIALIZATION_NVP(weight);
│ │ │ │ +
57 }
│ │ │ │ +
58 };
│ │ │ │ +
│ │ │ │ +
59
│ │ │ │ +
60 typedef std::vector<Edge> Edges;
│ │ │ │ +
61 typedef std::vector<size_t> EdgeIndices;
│ │ │ │ +
62 typedef Edges::iterator iterator;
│ │ │ │ +
63 typedef Edges::const_iterator const_iterator;
│ │ │ │ +
64
│ │ │ │ +
65 protected:
│ │ │ │ +
66 Edges edges_; /* index to the factors */
│ │ │ │ +
67
│ │ │ │ +
68 public:
│ │ │ │ +
69 Subgraph() {}
│ │ │ │ +
70 Subgraph(const Subgraph &subgraph) : edges_(subgraph.edges()) {}
│ │ │ │ +
71 Subgraph(const Edges &edges) : edges_(edges) {}
│ │ │ │ +
72 Subgraph(const std::vector<size_t> &indices);
│ │ │ │ +
73
│ │ │ │ +
74 inline const Edges &edges() const { return edges_; }
│ │ │ │ +
75 inline size_t size() const { return edges_.size(); }
│ │ │ │ +
76 EdgeIndices edgeIndices() const;
│ │ │ │ +
77
│ │ │ │ +
78 iterator begin() { return edges_.begin(); }
│ │ │ │ +
79 const_iterator begin() const { return edges_.begin(); }
│ │ │ │ +
80 iterator end() { return edges_.end(); }
│ │ │ │ +
81 const_iterator end() const { return edges_.end(); }
│ │ │ │ +
82
│ │ │ │ +
83 void save(const std::string &fn) const;
│ │ │ │ +
84 static Subgraph load(const std::string &fn);
│ │ │ │ +
85 friend std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph);
│ │ │ │ +
86
│ │ │ │ +
87 private:
│ │ │ │ +
88 friend class boost::serialization::access;
│ │ │ │ +
89 template <class Archive>
│ │ │ │ +
90 void serialize(Archive &ar, const unsigned int /*version*/) {
│ │ │ │ +
91 ar &BOOST_SERIALIZATION_NVP(edges_);
│ │ │ │ +
92 }
│ │ │ │ +
93};
│ │ │ │ +
│ │ │ │ +
94
│ │ │ │ +
95/****************************************************************************/
│ │ │ │ +
│ │ │ │ +
96struct GTSAM_EXPORT SubgraphBuilderParameters {
│ │ │ │ +
97 typedef boost::shared_ptr<SubgraphBuilderParameters> shared_ptr;
│ │ │ │ +
98
│ │ │ │ +
99 enum Skeleton {
│ │ │ │ +
100 /* augmented tree */
│ │ │ │ +
101 NATURALCHAIN = 0, /* natural ordering of the graph */
│ │ │ │ +
102 BFS, /* breadth-first search tree */
│ │ │ │ +
103 KRUSKAL, /* maximum weighted spanning tree */
│ │ │ │ +
104 } skeletonType;
│ │ │ │ +
105
│ │ │ │ +
106 enum SkeletonWeight { /* how to weigh the graph edges */
│ │ │ │ +
107 EQUAL = 0, /* every block edge has equal weight */
│ │ │ │ +
108 RHS_2NORM, /* use the 2-norm of the rhs */
│ │ │ │ +
109 LHS_FNORM, /* use the frobenius norm of the lhs */
│ │ │ │ +
110 RANDOM, /* bounded random edge weight */
│ │ │ │ +
111 } skeletonWeight;
│ │ │ │ +
112
│ │ │ │ +
113 enum AugmentationWeight { /* how to weigh the graph edges */
│ │ │ │ +
114 SKELETON = 0, /* use the same weights in building
│ │ │ │ +
115 the skeleton */
│ │ │ │ +
116 // STRETCH, /* stretch in the
│ │ │ │ +
117 // laplacian sense */ GENERALIZED_STRETCH /*
│ │ │ │ +
118 // the generalized stretch defined in
│ │ │ │ +
119 // jian2013iros */
│ │ │ │ +
120 } augmentationWeight;
│ │ │ │ +
121
│ │ │ │ +
123 double augmentationFactor;
│ │ │ │ +
124
│ │ │ │ +
125 SubgraphBuilderParameters()
│ │ │ │ +
126 : skeletonType(KRUSKAL),
│ │ │ │ +
127 skeletonWeight(RANDOM),
│ │ │ │ +
128 augmentationWeight(SKELETON),
│ │ │ │ +
129 augmentationFactor(1.0) {}
│ │ │ │ +
130 virtual ~SubgraphBuilderParameters() {}
│ │ │ │ +
131
│ │ │ │ +
132 /* for serialization */
│ │ │ │ +
133 void print() const;
│ │ │ │ +
134 virtual void print(std::ostream &os) const;
│ │ │ │ +
135 friend std::ostream &operator<<(std::ostream &os,
│ │ │ │ +
136 const PreconditionerParameters &p);
│ │ │ │ +
137
│ │ │ │ +
138 static Skeleton skeletonTranslator(const std::string &s);
│ │ │ │ +
139 static std::string skeletonTranslator(Skeleton s);
│ │ │ │ +
140 static SkeletonWeight skeletonWeightTranslator(const std::string &s);
│ │ │ │ +
141 static std::string skeletonWeightTranslator(SkeletonWeight w);
│ │ │ │ +
142 static AugmentationWeight augmentationWeightTranslator(const std::string &s);
│ │ │ │ +
143 static std::string augmentationWeightTranslator(AugmentationWeight w);
│ │ │ │ +
144};
│ │ │ │ +
│ │ │ │ +
145
│ │ │ │ +
146/*****************************************************************************/
│ │ │ │ +
│ │ │ │ +
147class GTSAM_EXPORT SubgraphBuilder {
│ │ │ │ +
148 public:
│ │ │ │ +
149 typedef SubgraphBuilder Base;
│ │ │ │ +
150 typedef std::vector<double> Weights;
│ │ │ │ +
151
│ │ │ │ +
152 SubgraphBuilder(
│ │ │ │ +
153 const SubgraphBuilderParameters &p = SubgraphBuilderParameters())
│ │ │ │ +
154 : parameters_(p) {}
│ │ │ │ +
155 virtual ~SubgraphBuilder() {}
│ │ │ │ +
156 virtual Subgraph operator()(const GaussianFactorGraph &jfg) const;
│ │ │ │ +
157
│ │ │ │ +
158 private:
│ │ │ │ +
159 std::vector<size_t> buildTree(const GaussianFactorGraph &gfg,
│ │ │ │ +
160 const FastMap<Key, size_t> &ordering,
│ │ │ │ +
161 const std::vector<double> &weights) const;
│ │ │ │ +
162 std::vector<size_t> unary(const GaussianFactorGraph &gfg) const;
│ │ │ │ +
163 std::vector<size_t> natural_chain(const GaussianFactorGraph &gfg) const;
│ │ │ │ +
164 std::vector<size_t> bfs(const GaussianFactorGraph &gfg) const;
│ │ │ │ +
165 std::vector<size_t> kruskal(const GaussianFactorGraph &gfg,
│ │ │ │ +
166 const FastMap<Key, size_t> &ordering,
│ │ │ │ +
167 const std::vector<double> &weights) const;
│ │ │ │ +
168 std::vector<size_t> sample(const std::vector<double> &weights,
│ │ │ │ +
169 const size_t t) const;
│ │ │ │ +
170 Weights weights(const GaussianFactorGraph &gfg) const;
│ │ │ │ +
171 SubgraphBuilderParameters parameters_;
│ │ │ │ +
172};
│ │ │ │ +
│ │ │ │ +
173
│ │ │ │ +
175GaussianFactorGraph buildFactorSubgraph(const GaussianFactorGraph &gfg,
│ │ │ │ +
176 const Subgraph &subgraph,
│ │ │ │ +
177 const bool clone);
│ │ │ │ +
178
│ │ │ │ +
181std::pair<GaussianFactorGraph, GaussianFactorGraph> splitFactorGraph(
│ │ │ │ +
182 const GaussianFactorGraph &factorGraph, const Subgraph &subgraph);
│ │ │ │ +
183
│ │ │ │ +
184} // namespace gtsam
│ │ │ │ +
FastMap.h
A thin wrapper around std::map that uses boost's fast_pool_allocator.
│ │ │ │ +
types.h
Typedefs for easier changing of types.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::operator+
Errors operator+(const Errors &a, const Errors &b)
Addition.
Definition Errors.cpp:60
│ │ │ │ -
gtsam::axpy
void axpy(double alpha, const Errors &x, Errors &y)
BLAS level 2 style AXPY, y := alpha*x + y
Definition Errors.cpp:111
│ │ │ │ -
gtsam::createErrors
Errors createErrors(const VectorValues &V)
Break V into pieces according to its start indices.
Definition Errors.cpp:29
│ │ │ │ -
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
│ │ │ │ -
gtsam::operator-
Errors operator-(const Errors &a, const Errors &b)
Subtraction.
Definition Errors.cpp:75
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ - │ │ │ │ -
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ -
VectorValues
The Factor::error simply extracts the.
│ │ │ │ +
gtsam::buildFactorSubgraph
GaussianFactorGraph buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone)
Select the factors in a factor graph according to the subgraph.
Definition SubgraphBuilder.cpp:447
│ │ │ │ +
gtsam::splitFactorGraph
std::pair< GaussianFactorGraph, GaussianFactorGraph > splitFactorGraph(const GaussianFactorGraph &factorGraph, const Subgraph &subgraph)
Split the graph into a subgraph and the remaining edges.
Definition SubgraphBuilder.cpp:460
│ │ │ │ +
gtsam::save
void save(const Matrix &A, const string &s, const string &filename)
save a matrix to file, which can be loaded by matlab
Definition Matrix.cpp:167
│ │ │ │ +
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ +
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ +
gtsam::Subgraph
Definition SubgraphBuilder.h:43
│ │ │ │ +
gtsam::Subgraph::Edge
Definition SubgraphBuilder.h:45
│ │ │ │ +
gtsam::SubgraphBuilderParameters
Definition SubgraphBuilder.h:96
│ │ │ │ +
gtsam::SubgraphBuilderParameters::augmentationFactor
double augmentationFactor
factor multiplied with n, yields number of extra edges.
Definition SubgraphBuilder.h:123
│ │ │ │ +
gtsam::SubgraphBuilder
Definition SubgraphBuilder.h:147
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,115 +1,230 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Errors.h │ │ │ │ │ +SubgraphBuilder.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18// \callgraph │ │ │ │ │ +18#pragma once │ │ │ │ │ 19 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_L_i_s_t_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_c_t_o_r_._h> │ │ │ │ │ -25 │ │ │ │ │ -26#include │ │ │ │ │ +20#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_M_a_p_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ +22#include │ │ │ │ │ +23 │ │ │ │ │ +24#include │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ 27 │ │ │ │ │ -28namespace _g_t_s_a_m { │ │ │ │ │ +28#include │ │ │ │ │ 29 │ │ │ │ │ -30// Forward declarations │ │ │ │ │ -31class _V_e_c_t_o_r_V_a_l_u_e_s; │ │ │ │ │ -32 │ │ │ │ │ -_3_4using _E_r_r_o_r_s = _F_a_s_t_L_i_s_t_<_V_e_c_t_o_r_>; │ │ │ │ │ +30namespace boost { │ │ │ │ │ +31namespace serialization { │ │ │ │ │ +32class access; │ │ │ │ │ +33} /* namespace serialization */ │ │ │ │ │ +34} /* namespace boost */ │ │ │ │ │ 35 │ │ │ │ │ -37GTSAM_EXPORT _E_r_r_o_r_s _c_r_e_a_t_e_E_r_r_o_r_s(const _V_e_c_t_o_r_V_a_l_u_e_s& V); │ │ │ │ │ -38 │ │ │ │ │ -40GTSAM_EXPORT void _p_r_i_n_t(const _E_r_r_o_r_s& e, const std::string& s = "Errors"); │ │ │ │ │ +36namespace _g_t_s_a_m { │ │ │ │ │ +37 │ │ │ │ │ +38// Forward declarations │ │ │ │ │ +39class GaussianFactorGraph; │ │ │ │ │ +40struct PreconditionerParameters; │ │ │ │ │ 41 │ │ │ │ │ -42// Check equality for unit testing. │ │ │ │ │ -43GTSAM_EXPORT bool equality(const _E_r_r_o_r_s& actual, const _E_r_r_o_r_s& expected, │ │ │ │ │ -44 double tol = 1e-9); │ │ │ │ │ -45 │ │ │ │ │ -47GTSAM_EXPORT _E_r_r_o_r_s _o_p_e_r_a_t_o_r_+(const _E_r_r_o_r_s& a, const _E_r_r_o_r_s& b); │ │ │ │ │ -48 │ │ │ │ │ -50GTSAM_EXPORT _E_r_r_o_r_s _o_p_e_r_a_t_o_r_-(const _E_r_r_o_r_s& a, const _E_r_r_o_r_s& b); │ │ │ │ │ -51 │ │ │ │ │ -53GTSAM_EXPORT _E_r_r_o_r_s _o_p_e_r_a_t_o_r_-(const _E_r_r_o_r_s& a); │ │ │ │ │ -54 │ │ │ │ │ -56GTSAM_EXPORT double _d_o_t(const _E_r_r_o_r_s& a, const _E_r_r_o_r_s& b); │ │ │ │ │ -57 │ │ │ │ │ -59GTSAM_EXPORT void _a_x_p_y(double alpha, const _E_r_r_o_r_s& x, _E_r_r_o_r_s& y); │ │ │ │ │ -60 │ │ │ │ │ -62template <> │ │ │ │ │ -_6_3struct _t_r_a_i_t_s<_E_r_r_o_r_s> { │ │ │ │ │ -64 static void Print(const _E_r_r_o_r_s& e, const std::string& str = "") { │ │ │ │ │ -65 _p_r_i_n_t(e, str); │ │ │ │ │ -66 } │ │ │ │ │ -67 static bool Equals(const _E_r_r_o_r_s& actual, const _E_r_r_o_r_s& expected, │ │ │ │ │ -68 double tol = 1e-8) { │ │ │ │ │ -69 return equality(actual, expected, tol); │ │ │ │ │ -70 } │ │ │ │ │ -71}; │ │ │ │ │ -72 │ │ │ │ │ -73} // namespace gtsam │ │ │ │ │ -_V_e_c_t_o_r_._h │ │ │ │ │ -typedef and functions to augment Eigen's VectorXd │ │ │ │ │ -_T_e_s_t_a_b_l_e_._h │ │ │ │ │ -Concept check for values that can be used in unit tests. │ │ │ │ │ -_F_a_s_t_L_i_s_t_._h │ │ │ │ │ -A thin wrapper around std::list that uses boost's fast_pool_allocator. │ │ │ │ │ +42/**************************************************************************/ │ │ │ │ │ +_4_3class GTSAM_EXPORT _S_u_b_g_r_a_p_h { │ │ │ │ │ +44 public: │ │ │ │ │ +_4_5 struct GTSAM_EXPORT _E_d_g_e { │ │ │ │ │ +46 size_t index; /* edge id */ │ │ │ │ │ +47 double weight; /* edge weight */ │ │ │ │ │ +48 inline bool isUnitWeight() const { return (weight == 1.0); } │ │ │ │ │ +49 friend std::ostream &operator<<(std::ostream &os, const _E_d_g_e &edge); │ │ │ │ │ +50 │ │ │ │ │ +51 private: │ │ │ │ │ +52 friend class boost::serialization::access; │ │ │ │ │ +53 template │ │ │ │ │ +54 void serialize(Archive &ar, const unsigned int /*version*/) { │ │ │ │ │ +55 ar &BOOST_SERIALIZATION_NVP(index); │ │ │ │ │ +56 ar &BOOST_SERIALIZATION_NVP(weight); │ │ │ │ │ +57 } │ │ │ │ │ +58 }; │ │ │ │ │ +59 │ │ │ │ │ +60 typedef std::vector Edges; │ │ │ │ │ +61 typedef std::vector EdgeIndices; │ │ │ │ │ +62 typedef Edges::iterator iterator; │ │ │ │ │ +63 typedef Edges::const_iterator const_iterator; │ │ │ │ │ +64 │ │ │ │ │ +65 protected: │ │ │ │ │ +66 Edges edges_; /* index to the factors */ │ │ │ │ │ +67 │ │ │ │ │ +68 public: │ │ │ │ │ +69 _S_u_b_g_r_a_p_h() {} │ │ │ │ │ +70 _S_u_b_g_r_a_p_h(const _S_u_b_g_r_a_p_h &subgraph) : edges_(subgraph.edges()) {} │ │ │ │ │ +71 Subgraph(const Edges &edges) : edges_(edges) {} │ │ │ │ │ +72 Subgraph(const std::vector &indices); │ │ │ │ │ +73 │ │ │ │ │ +74 inline const Edges &edges() const { return edges_; } │ │ │ │ │ +75 inline size_t size() const { return edges_.size(); } │ │ │ │ │ +76 EdgeIndices edgeIndices() const; │ │ │ │ │ +77 │ │ │ │ │ +78 iterator begin() { return edges_.begin(); } │ │ │ │ │ +79 const_iterator begin() const { return edges_.begin(); } │ │ │ │ │ +80 iterator end() { return edges_.end(); } │ │ │ │ │ +81 const_iterator end() const { return edges_.end(); } │ │ │ │ │ +82 │ │ │ │ │ +83 void _s_a_v_e(const std::string &fn) const; │ │ │ │ │ +84 static Subgraph load(const std::string &fn); │ │ │ │ │ +85 friend std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph); │ │ │ │ │ +86 │ │ │ │ │ +87 private: │ │ │ │ │ +88 friend class boost::serialization::access; │ │ │ │ │ +89 template │ │ │ │ │ +90 void serialize(Archive &ar, const unsigned int /*version*/) { │ │ │ │ │ +91 ar &BOOST_SERIALIZATION_NVP(edges_); │ │ │ │ │ +92 } │ │ │ │ │ +93}; │ │ │ │ │ +94 │ │ │ │ │ +95/ │ │ │ │ │ +****************************************************************************/ │ │ │ │ │ +_9_6struct GTSAM_EXPORT _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s { │ │ │ │ │ +97 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +98 │ │ │ │ │ +99 enum Skeleton { │ │ │ │ │ +100 /* augmented tree */ │ │ │ │ │ +101 NATURALCHAIN = 0, /* natural ordering of the graph */ │ │ │ │ │ +102 BFS, /* breadth-first search tree */ │ │ │ │ │ +103 KRUSKAL, /* maximum weighted spanning tree */ │ │ │ │ │ +104 } skeletonType; │ │ │ │ │ +105 │ │ │ │ │ +106 enum SkeletonWeight { /* how to weigh the graph edges */ │ │ │ │ │ +107 EQUAL = 0, /* every block edge has equal weight */ │ │ │ │ │ +108 RHS_2NORM, /* use the 2-norm of the rhs */ │ │ │ │ │ +109 LHS_FNORM, /* use the frobenius norm of the lhs */ │ │ │ │ │ +110 RANDOM, /* bounded random edge weight */ │ │ │ │ │ +111 } skeletonWeight; │ │ │ │ │ +112 │ │ │ │ │ +113 enum AugmentationWeight { /* how to weigh the graph edges */ │ │ │ │ │ +114 SKELETON = 0, /* use the same weights in building │ │ │ │ │ +115 the skeleton */ │ │ │ │ │ +116 // STRETCH, /* stretch in the │ │ │ │ │ +117 // laplacian sense */ GENERALIZED_STRETCH /* │ │ │ │ │ +118 // the generalized stretch defined in │ │ │ │ │ +119 // jian2013iros */ │ │ │ │ │ +120 } augmentationWeight; │ │ │ │ │ +121 │ │ │ │ │ +_1_2_3 double _a_u_g_m_e_n_t_a_t_i_o_n_F_a_c_t_o_r; │ │ │ │ │ +124 │ │ │ │ │ +125 _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s() │ │ │ │ │ +126 : skeletonType(KRUSKAL), │ │ │ │ │ +127 skeletonWeight(RANDOM), │ │ │ │ │ +128 augmentationWeight(SKELETON), │ │ │ │ │ +129 augmentationFactor(1.0) {} │ │ │ │ │ +130 virtual _~_S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s() {} │ │ │ │ │ +131 │ │ │ │ │ +132 /* for serialization */ │ │ │ │ │ +133 void print() const; │ │ │ │ │ +134 virtual void print(std::ostream &os) const; │ │ │ │ │ +135 friend std::ostream &operator<<(std::ostream &os, │ │ │ │ │ +136 const PreconditionerParameters &p); │ │ │ │ │ +137 │ │ │ │ │ +138 static Skeleton skeletonTranslator(const std::string &s); │ │ │ │ │ +139 static std::string skeletonTranslator(Skeleton s); │ │ │ │ │ +140 static SkeletonWeight skeletonWeightTranslator(const std::string &s); │ │ │ │ │ +141 static std::string skeletonWeightTranslator(SkeletonWeight w); │ │ │ │ │ +142 static AugmentationWeight augmentationWeightTranslator(const std::string │ │ │ │ │ +&s); │ │ │ │ │ +143 static std::string augmentationWeightTranslator(AugmentationWeight w); │ │ │ │ │ +144}; │ │ │ │ │ +145 │ │ │ │ │ +146/ │ │ │ │ │ +*****************************************************************************/ │ │ │ │ │ +_1_4_7class GTSAM_EXPORT _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r { │ │ │ │ │ +148 public: │ │ │ │ │ +149 typedef _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r _B_a_s_e; │ │ │ │ │ +150 typedef std::vector Weights; │ │ │ │ │ +151 │ │ │ │ │ +152 _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r( │ │ │ │ │ +153 const _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s &p = _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s()) │ │ │ │ │ +154 : parameters_(p) {} │ │ │ │ │ +155 virtual _~_S_u_b_g_r_a_p_h_B_u_i_l_d_e_r() {} │ │ │ │ │ +156 virtual _S_u_b_g_r_a_p_h operator()(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &jfg) const; │ │ │ │ │ +157 │ │ │ │ │ +158 private: │ │ │ │ │ +159 std::vector buildTree(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, │ │ │ │ │ +160 const _F_a_s_t_M_a_p_<_K_e_y_,_ _s_i_z_e___t_> &ordering, │ │ │ │ │ +161 const std::vector &weights) const; │ │ │ │ │ +162 std::vector unary(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg) const; │ │ │ │ │ +163 std::vector natural_chain(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg) const; │ │ │ │ │ +164 std::vector bfs(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg) const; │ │ │ │ │ +165 std::vector kruskal(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, │ │ │ │ │ +166 const _F_a_s_t_M_a_p_<_K_e_y_,_ _s_i_z_e___t_> &ordering, │ │ │ │ │ +167 const std::vector &weights) const; │ │ │ │ │ +168 std::vector sample(const std::vector &weights, │ │ │ │ │ +169 const size_t t) const; │ │ │ │ │ +170 Weights weights(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg) const; │ │ │ │ │ +171 _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s parameters_; │ │ │ │ │ +172}; │ │ │ │ │ +173 │ │ │ │ │ +175_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h _b_u_i_l_d_F_a_c_t_o_r_S_u_b_g_r_a_p_h(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, │ │ │ │ │ +176 const _S_u_b_g_r_a_p_h &subgraph, │ │ │ │ │ +177 const bool clone); │ │ │ │ │ +178 │ │ │ │ │ +181std::pair _s_p_l_i_t_F_a_c_t_o_r_G_r_a_p_h( │ │ │ │ │ +182 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &factorGraph, const _S_u_b_g_r_a_p_h &subgraph); │ │ │ │ │ +183 │ │ │ │ │ +184} // namespace gtsam │ │ │ │ │ +_F_a_s_t_M_a_p_._h │ │ │ │ │ +A thin wrapper around std::map that uses boost's fast_pool_allocator. │ │ │ │ │ +_t_y_p_e_s_._h │ │ │ │ │ +Typedefs for easier changing of types. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_+ │ │ │ │ │ -Errors operator+(const Errors &a, const Errors &b) │ │ │ │ │ -Addition. │ │ │ │ │ -DDeeffiinniittiioonn Errors.cpp:60 │ │ │ │ │ -_g_t_s_a_m_:_:_a_x_p_y │ │ │ │ │ -void axpy(double alpha, const Errors &x, Errors &y) │ │ │ │ │ -BLAS level 2 style AXPY, y := alpha*x + y │ │ │ │ │ -DDeeffiinniittiioonn Errors.cpp:111 │ │ │ │ │ -_g_t_s_a_m_:_:_c_r_e_a_t_e_E_r_r_o_r_s │ │ │ │ │ -Errors createErrors(const VectorValues &V) │ │ │ │ │ -Break V into pieces according to its start indices. │ │ │ │ │ -DDeeffiinniittiioonn Errors.cpp:29 │ │ │ │ │ -_g_t_s_a_m_:_:_d_o_t │ │ │ │ │ -double dot(const V1 &a, const V2 &b) │ │ │ │ │ -Dot product. │ │ │ │ │ -DDeeffiinniittiioonn Vector.h:195 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_- │ │ │ │ │ -Errors operator-(const Errors &a, const Errors &b) │ │ │ │ │ -Subtraction. │ │ │ │ │ -DDeeffiinniittiioonn Errors.cpp:75 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t_<_ _V_e_c_t_o_r_ _> │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ -with a unique integer... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -The Factor::error simply extracts the. │ │ │ │ │ +_g_t_s_a_m_:_:_b_u_i_l_d_F_a_c_t_o_r_S_u_b_g_r_a_p_h │ │ │ │ │ +GaussianFactorGraph buildFactorSubgraph(const GaussianFactorGraph &gfg, const │ │ │ │ │ +Subgraph &subgraph, const bool clone) │ │ │ │ │ +Select the factors in a factor graph according to the subgraph. │ │ │ │ │ +DDeeffiinniittiioonn SubgraphBuilder.cpp:447 │ │ │ │ │ +_g_t_s_a_m_:_:_s_p_l_i_t_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +std::pair< GaussianFactorGraph, GaussianFactorGraph > splitFactorGraph(const │ │ │ │ │ +GaussianFactorGraph &factorGraph, const Subgraph &subgraph) │ │ │ │ │ +Split the graph into a subgraph and the remaining edges. │ │ │ │ │ +DDeeffiinniittiioonn SubgraphBuilder.cpp:460 │ │ │ │ │ +_g_t_s_a_m_:_:_s_a_v_e │ │ │ │ │ +void save(const Matrix &A, const string &s, const string &filename) │ │ │ │ │ +save a matrix to file, which can be loaded by matlab │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:167 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ +FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ +fast_pool_allocator instead of the defa... │ │ │ │ │ +DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h │ │ │ │ │ +DDeeffiinniittiioonn SubgraphBuilder.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_:_:_E_d_g_e │ │ │ │ │ +DDeeffiinniittiioonn SubgraphBuilder.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ +DDeeffiinniittiioonn SubgraphBuilder.h:96 │ │ │ │ │ +_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s_:_:_a_u_g_m_e_n_t_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +double augmentationFactor │ │ │ │ │ +factor multiplied with n, yields number of extra edges. │ │ │ │ │ +DDeeffiinniittiioonn SubgraphBuilder.h:123 │ │ │ │ │ +_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_B_u_i_l_d_e_r │ │ │ │ │ +DDeeffiinniittiioonn SubgraphBuilder.h:147 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _E_r_r_o_r_s_._h │ │ │ │ │ + * _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00860.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Sampler.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Scatter.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,44 +96,48 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
Sampler.h File Reference
│ │ │ │ +
Scatter.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

sampling from a NoiseModel │ │ │ │ +

Maps global variable indices to slot indices. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::Sampler
 Sampling structure that keeps internal random number generators for diagonal distributions specified by NoiseModel. More...
struct  gtsam::SlotEntry
 One SlotEntry stores the slot index for a variable, as well its dim. More...
 
class  gtsam::Scatter
 Scatter is an intermediate data structure used when building a HessianFactor incrementally, to get the keys in the right order. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

sampling from a NoiseModel

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +

Maps global variable indices to slot indices.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │
│ │ │ │ -Alex Cunningham
│ │ │ │ +Frank Dellaert
│ │ │ │ +
Date
June 2015
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,27 +1,34 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Sampler.h File Reference │ │ │ │ │ -sampling from a NoiseModel _M_o_r_e_._._. │ │ │ │ │ +Scatter.h File Reference │ │ │ │ │ +Maps global variable indices to slot indices. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_S_a_m_p_l_e_r │ │ │ │ │ -  Sampling structure that keeps internal random number generators for │ │ │ │ │ - diagonal distributions specified by NoiseModel. _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_S_l_o_t_E_n_t_r_y │ │ │ │ │ +  One _S_l_o_t_E_n_t_r_y stores the slot index for a variable, as well its dim. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_S_c_a_t_t_e_r │ │ │ │ │ + _S_c_a_t_t_e_r is an intermediate data structure used when building a │ │ │ │ │ +  _H_e_s_s_i_a_n_F_a_c_t_o_r incrementally, to get the keys in the right order. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -sampling from a NoiseModel │ │ │ │ │ +Maps global variable indices to slot indices. │ │ │ │ │ Author │ │ │ │ │ + Richard Roberts │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Alex Cunningham │ │ │ │ │ + Date │ │ │ │ │ + June 2015 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _S_a_m_p_l_e_r_._h │ │ │ │ │ + * _S_c_a_t_t_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00860.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,4 @@ │ │ │ │ │ var a00860 = [ │ │ │ │ │ - ["gtsam::Sampler", "a04020.html", "a04020"] │ │ │ │ │ + ["gtsam::SlotEntry", "a04024.html", null], │ │ │ │ │ + ["gtsam::Scatter", "a04028.html", "a04028"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00860_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Sampler.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Scatter.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,79 +98,84 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Sampler.h
│ │ │ │ +
Scatter.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/linear/NoiseModel.h>
│ │ │ │ -
22
│ │ │ │ -
23#include <random>
│ │ │ │ -
24
│ │ │ │ -
25namespace gtsam {
│ │ │ │ -
26
│ │ │ │ -
│ │ │ │ -
31class GTSAM_EXPORT Sampler {
│ │ │ │ -
32 protected:
│ │ │ │ -
34 noiseModel::Diagonal::shared_ptr model_;
│ │ │ │ -
35
│ │ │ │ -
37 mutable std::mt19937_64 generator_;
│ │ │ │ -
38
│ │ │ │ -
39 public:
│ │ │ │ -
40 typedef boost::shared_ptr<Sampler> shared_ptr;
│ │ │ │ -
41
│ │ │ │ -
44
│ │ │ │ -
51 explicit Sampler(const noiseModel::Diagonal::shared_ptr& model,
│ │ │ │ -
52 uint_fast64_t seed = 42u);
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/inference/Key.h>
│ │ │ │ +
23#include <gtsam/base/FastMap.h>
│ │ │ │ +
24#include <gtsam/dllexport.h>
│ │ │ │ +
25
│ │ │ │ +
26namespace gtsam {
│ │ │ │ +
27
│ │ │ │ +
28class GaussianFactorGraph;
│ │ │ │ +
29class Ordering;
│ │ │ │ +
30
│ │ │ │ +
│ │ │ │ +
32struct GTSAM_EXPORT SlotEntry {
│ │ │ │ +
33 Key key;
│ │ │ │ +
34 size_t dimension;
│ │ │ │ +
35 SlotEntry(Key _key, size_t _dimension) : key(_key), dimension(_dimension) {}
│ │ │ │ +
36 std::string toString() const;
│ │ │ │ +
37 friend bool operator<(const SlotEntry& p, const SlotEntry& q) {
│ │ │ │ +
38 return p.key < q.key;
│ │ │ │ +
39 }
│ │ │ │ +
40 static bool Zero(const SlotEntry& p) { return p.dimension==0;}
│ │ │ │ +
41};
│ │ │ │ +
│ │ │ │ +
42
│ │ │ │ +
│ │ │ │ +
49class Scatter : public FastVector<SlotEntry> {
│ │ │ │ +
50 public:
│ │ │ │ +
52 GTSAM_EXPORT Scatter() {}
│ │ │ │
53
│ │ │ │ -
60 explicit Sampler(const Vector& sigmas, uint_fast64_t seed = 42u);
│ │ │ │ -
61
│ │ │ │ -
65
│ │ │ │ -
66 size_t dim() const { return model_->dim(); }
│ │ │ │ +
55 GTSAM_EXPORT explicit Scatter(const GaussianFactorGraph& gfg);
│ │ │ │ +
56
│ │ │ │ +
58 GTSAM_EXPORT explicit Scatter(const GaussianFactorGraph& gfg, const Ordering& ordering);
│ │ │ │ +
59
│ │ │ │ +
61 GTSAM_EXPORT void add(Key key, size_t dim);
│ │ │ │ +
62
│ │ │ │ +
63 private:
│ │ │ │ +
65 iterator find(Key key);
│ │ │ │ +
66};
│ │ │ │ +
│ │ │ │
67
│ │ │ │ -
68 Vector sigmas() const { return model_->sigmas(); }
│ │ │ │ -
69
│ │ │ │ -
70 const noiseModel::Diagonal::shared_ptr& model() const { return model_; }
│ │ │ │ -
71
│ │ │ │ -
75
│ │ │ │ -
77 Vector sample() const;
│ │ │ │ -
78
│ │ │ │ -
80 static Vector sampleDiagonal(const Vector& sigmas, std::mt19937_64* rng);
│ │ │ │ -
82
│ │ │ │ -
83 protected:
│ │ │ │ -
85 Vector sampleDiagonal(const Vector& sigmas) const;
│ │ │ │ -
86};
│ │ │ │ -
│ │ │ │ -
87
│ │ │ │ -
88} // namespace gtsam
│ │ │ │ - │ │ │ │ +
68} // \ namespace gtsam
│ │ │ │ +
FastMap.h
A thin wrapper around std::map that uses boost's fast_pool_allocator.
│ │ │ │ + │ │ │ │ +
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::Sampler
Sampling structure that keeps internal random number generators for diagonal distributions specified ...
Definition Sampler.h:31
│ │ │ │ -
gtsam::Sampler::model_
noiseModel::Diagonal::shared_ptr model_
noiseModel created at generation
Definition Sampler.h:34
│ │ │ │ -
gtsam::Sampler::generator_
std::mt19937_64 generator_
generator
Definition Sampler.h:37
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ +
gtsam::SlotEntry
One SlotEntry stores the slot index for a variable, as well its dim.
Definition Scatter.h:32
│ │ │ │ +
gtsam::Scatter
Scatter is an intermediate data structure used when building a HessianFactor incrementally,...
Definition Scatter.h:49
│ │ │ │ +
gtsam::Scatter::add
GTSAM_EXPORT void add(Key key, size_t dim)
Add a key/dim pair.
Definition Scatter.cpp:76
│ │ │ │ +
gtsam::Scatter::Scatter
GTSAM_EXPORT Scatter()
Default Constructor.
Definition Scatter.h:52
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,80 +1,97 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Sampler.h │ │ │ │ │ +Scatter.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_N_o_i_s_e_M_o_d_e_l_._h> │ │ │ │ │ -22 │ │ │ │ │ -23#include │ │ │ │ │ -24 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ -26 │ │ │ │ │ -_3_1class GTSAM_EXPORT _S_a_m_p_l_e_r { │ │ │ │ │ -32 protected: │ │ │ │ │ -_3_4 noiseModel::Diagonal::shared_ptr _m_o_d_e_l__; │ │ │ │ │ -35 │ │ │ │ │ -_3_7 mutable std::mt19937_64 _g_e_n_e_r_a_t_o_r__; │ │ │ │ │ -38 │ │ │ │ │ -39 public: │ │ │ │ │ -40 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -41 │ │ │ │ │ -44 │ │ │ │ │ -51 explicit _S_a_m_p_l_e_r(const noiseModel::Diagonal::shared_ptr& model, │ │ │ │ │ -52 uint_fast64_t seed = 42u); │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_M_a_p_._h> │ │ │ │ │ +24#include │ │ │ │ │ +25 │ │ │ │ │ +26namespace _g_t_s_a_m { │ │ │ │ │ +27 │ │ │ │ │ +28class GaussianFactorGraph; │ │ │ │ │ +29class Ordering; │ │ │ │ │ +30 │ │ │ │ │ +_3_2struct GTSAM_EXPORT _S_l_o_t_E_n_t_r_y { │ │ │ │ │ +33 _K_e_y key; │ │ │ │ │ +34 size_t dimension; │ │ │ │ │ +35 _S_l_o_t_E_n_t_r_y(_K_e_y _key, size_t _dimension) : key(_key), dimension(_dimension) {} │ │ │ │ │ +36 std::string toString() const; │ │ │ │ │ +37 friend bool operator<(const _S_l_o_t_E_n_t_r_y& p, const _S_l_o_t_E_n_t_r_y& q) { │ │ │ │ │ +38 return p.key < q.key; │ │ │ │ │ +39 } │ │ │ │ │ +40 static bool Zero(const _S_l_o_t_E_n_t_r_y& p) { return p.dimension==0;} │ │ │ │ │ +41}; │ │ │ │ │ +42 │ │ │ │ │ +_4_9class _S_c_a_t_t_e_r : public _F_a_s_t_V_e_c_t_o_r { │ │ │ │ │ +50 public: │ │ │ │ │ +_5_2 GTSAM_EXPORT _S_c_a_t_t_e_r() {} │ │ │ │ │ 53 │ │ │ │ │ -60 explicit _S_a_m_p_l_e_r(const Vector& sigmas, uint_fast64_t seed = 42u); │ │ │ │ │ -61 │ │ │ │ │ -65 │ │ │ │ │ -66 size_t dim() const { return model_->dim(); } │ │ │ │ │ +55 GTSAM_EXPORT explicit _S_c_a_t_t_e_r(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& gfg); │ │ │ │ │ +56 │ │ │ │ │ +58 GTSAM_EXPORT explicit _S_c_a_t_t_e_r(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& gfg, const │ │ │ │ │ +_O_r_d_e_r_i_n_g& ordering); │ │ │ │ │ +59 │ │ │ │ │ +61 GTSAM_EXPORT void _a_d_d(_K_e_y key, size_t dim); │ │ │ │ │ +62 │ │ │ │ │ +63 private: │ │ │ │ │ +65 iterator find(_K_e_y key); │ │ │ │ │ +66}; │ │ │ │ │ 67 │ │ │ │ │ -68 Vector sigmas() const { return model_->sigmas(); } │ │ │ │ │ -69 │ │ │ │ │ -70 const noiseModel::Diagonal::shared_ptr& model() const { return model_; } │ │ │ │ │ -71 │ │ │ │ │ -75 │ │ │ │ │ -77 Vector sample() const; │ │ │ │ │ -78 │ │ │ │ │ -80 static Vector sampleDiagonal(const Vector& sigmas, std::mt19937_64* rng); │ │ │ │ │ -82 │ │ │ │ │ -83 protected: │ │ │ │ │ -85 Vector sampleDiagonal(const Vector& sigmas) const; │ │ │ │ │ -86}; │ │ │ │ │ -87 │ │ │ │ │ -88} // namespace gtsam │ │ │ │ │ -_N_o_i_s_e_M_o_d_e_l_._h │ │ │ │ │ +68} // \ namespace gtsam │ │ │ │ │ +_F_a_s_t_M_a_p_._h │ │ │ │ │ +A thin wrapper around std::map that uses boost's fast_pool_allocator. │ │ │ │ │ +_K_e_y_._h │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ +std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ +FastVector │ │ │ │ │ +FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ +DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_S_a_m_p_l_e_r │ │ │ │ │ -Sampling structure that keeps internal random number generators for diagonal │ │ │ │ │ -distributions specified ... │ │ │ │ │ -DDeeffiinniittiioonn Sampler.h:31 │ │ │ │ │ -_g_t_s_a_m_:_:_S_a_m_p_l_e_r_:_:_m_o_d_e_l__ │ │ │ │ │ -noiseModel::Diagonal::shared_ptr model_ │ │ │ │ │ -noiseModel created at generation │ │ │ │ │ -DDeeffiinniittiioonn Sampler.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_S_a_m_p_l_e_r_:_:_g_e_n_e_r_a_t_o_r__ │ │ │ │ │ -std::mt19937_64 generator_ │ │ │ │ │ -generator │ │ │ │ │ -DDeeffiinniittiioonn Sampler.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_S_l_o_t_E_n_t_r_y │ │ │ │ │ +One SlotEntry stores the slot index for a variable, as well its dim. │ │ │ │ │ +DDeeffiinniittiioonn Scatter.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_S_c_a_t_t_e_r │ │ │ │ │ +Scatter is an intermediate data structure used when building a HessianFactor │ │ │ │ │ +incrementally,... │ │ │ │ │ +DDeeffiinniittiioonn Scatter.h:49 │ │ │ │ │ +_g_t_s_a_m_:_:_S_c_a_t_t_e_r_:_:_a_d_d │ │ │ │ │ +GTSAM_EXPORT void add(Key key, size_t dim) │ │ │ │ │ +Add a key/dim pair. │ │ │ │ │ +DDeeffiinniittiioonn Scatter.cpp:76 │ │ │ │ │ +_g_t_s_a_m_:_:_S_c_a_t_t_e_r_:_:_S_c_a_t_t_e_r │ │ │ │ │ +GTSAM_EXPORT Scatter() │ │ │ │ │ +Default Constructor. │ │ │ │ │ +DDeeffiinniittiioonn Scatter.h:52 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _S_a_m_p_l_e_r_._h │ │ │ │ │ + * _S_c_a_t_t_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00866.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianISAM.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/SubgraphPreconditioner.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,45 +94,32 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
GaussianISAM.h File Reference
│ │ │ │ +
SubgraphPreconditioner.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::GaussianISAM
 
struct  gtsam::traits< GaussianISAM >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
July 29, 2013
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ +
Date
Dec 31, 2009
│ │ │ │ +
Author
Frank Dellaert, Yong-Dian Jian
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,20 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -GaussianISAM.h File Reference │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_I_S_A_M │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_I_S_A_M_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +SubgraphPreconditioner.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Date │ │ │ │ │ - July 29, 2013 │ │ │ │ │ + Dec 31, 2009 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Frank Dellaert, Yong-Dian Jian │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_I_S_A_M_._h │ │ │ │ │ + * _S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00869.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/SubgraphSolver.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianConditional-inl.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,37 +95,36 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces
│ │ │ │ -
SubgraphSolver.cpp File Reference
│ │ │ │ +
GaussianConditional-inl.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Subgraph Solver from IROS 2010. │ │ │ │ +

Conditional Gaussian Base class. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Subgraph Solver from IROS 2010.

│ │ │ │ -
Date
2010
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ -Yong Dian Jian
│ │ │ │ +

Conditional Gaussian Base class.

│ │ │ │ +
Author
Christian Potthast
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,21 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SubgraphSolver.cpp File Reference │ │ │ │ │ -Subgraph Solver from IROS 2010. _M_o_r_e_._._. │ │ │ │ │ +GaussianConditional-inl.h File Reference │ │ │ │ │ +Conditional Gaussian Base class. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Subgraph Solver from IROS 2010. │ │ │ │ │ - Date │ │ │ │ │ - 2010 │ │ │ │ │ +Conditional Gaussian Base class. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Yong Dian Jian │ │ │ │ │ + Christian Potthast │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _S_u_b_g_r_a_p_h_S_o_l_v_e_r_._c_p_p │ │ │ │ │ + * _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_-_i_n_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00872.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/HessianFactor-inl.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/RegularHessianFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,38 +94,46 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
HessianFactor-inl.h File Reference
│ │ │ │ +
RegularHessianFactor.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Contains the HessianFactor class, a general quadratic factor. │ │ │ │ +

HessianFactor class with constant sized blocks. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::RegularHessianFactor< D >
 
struct  gtsam::traits< RegularHessianFactor< D > >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Contains the HessianFactor class, a general quadratic factor.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Dec 8, 2010
│ │ │ │ +

HessianFactor class with constant sized blocks.

│ │ │ │ +
Author
Sungtae An
│ │ │ │ +
Date
March 2014
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -HessianFactor-inl.h File Reference │ │ │ │ │ -Contains the HessianFactor class, a general quadratic factor. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +RegularHessianFactor.h File Reference │ │ │ │ │ +HessianFactor class with constant sized blocks. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_<_ _D_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_<_ _D_ _>_ _> │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Contains the HessianFactor class, a general quadratic factor. │ │ │ │ │ +HessianFactor class with constant sized blocks. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Sungtae An │ │ │ │ │ Date │ │ │ │ │ - Dec 8, 2010 │ │ │ │ │ + March 2014 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _H_e_s_s_i_a_n_F_a_c_t_o_r_-_i_n_l_._h │ │ │ │ │ + * _R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00872_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/HessianFactor-inl.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/RegularHessianFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,15 +98,15 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
HessianFactor-inl.h
│ │ │ │ +
RegularHessianFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │ @@ -114,49 +114,258 @@ │ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21namespace gtsam {
│ │ │ │ -
22
│ │ │ │ -
23 /* ************************************************************************* */
│ │ │ │ -
24 template<typename KEYS>
│ │ │ │ -
│ │ │ │ -
25 HessianFactor::HessianFactor(const KEYS& keys, const SymmetricBlockMatrix& augmentedInformation) :
│ │ │ │ -
26 GaussianFactor(keys), info_(augmentedInformation)
│ │ │ │ -
27 {
│ │ │ │ -
28 // Check number of variables
│ │ │ │ -
29 if((DenseIndex)Base::keys_.size() != augmentedInformation.nBlocks() - 1)
│ │ │ │ -
30 throw std::invalid_argument(
│ │ │ │ -
31 "Error in HessianFactor constructor input. Number of provided keys plus\n"
│ │ │ │ -
32 "one for the information vector must equal the number of provided matrix blocks. ");
│ │ │ │ -
33
│ │ │ │ -
34 // Check RHS dimension
│ │ │ │ -
35 if(augmentedInformation.getDim(augmentedInformation.nBlocks() - 1) != 1)
│ │ │ │ -
36 throw std::invalid_argument(
│ │ │ │ -
37 "Error in HessianFactor constructor input. The last provided matrix block\n"
│ │ │ │ -
38 "must be the information vector, but the last provided block had more than one column.");
│ │ │ │ -
39 }
│ │ │ │ +
21#include <gtsam/linear/HessianFactor.h>
│ │ │ │ +
22#include <gtsam/linear/RegularJacobianFactor.h>
│ │ │ │ +
23#include <vector>
│ │ │ │ +
24
│ │ │ │ +
25namespace gtsam {
│ │ │ │ +
26
│ │ │ │ +
27template<size_t D>
│ │ │ │ +
│ │ │ │ +
28class RegularHessianFactor: public HessianFactor {
│ │ │ │ +
29
│ │ │ │ +
30public:
│ │ │ │ +
31
│ │ │ │ +
32 typedef Eigen::Matrix<double, D, 1> VectorD;
│ │ │ │ +
33 typedef Eigen::Matrix<double, D, D> MatrixD;
│ │ │ │ +
34
│ │ │ │ +
│ │ │ │ +
39 RegularHessianFactor(const KeyVector& js,
│ │ │ │ +
40 const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) :
│ │ │ │ +
41 HessianFactor(js, Gs, gs, f) {
│ │ │ │ +
42 checkInvariants();
│ │ │ │ +
43 }
│ │ │ │
│ │ │ │ -
40
│ │ │ │ -
41}
│ │ │ │ +
44
│ │ │ │ +
│ │ │ │ +
49 RegularHessianFactor(Key j1, Key j2, const MatrixD& G11, const MatrixD& G12,
│ │ │ │ +
50 const VectorD& g1, const MatrixD& G22, const VectorD& g2, double f) :
│ │ │ │ +
51 HessianFactor(j1, j2, G11, G12, g1, G22, g2, f) {
│ │ │ │ +
52 }
│ │ │ │ +
│ │ │ │ +
53
│ │ │ │ +
│ │ │ │ +
58 RegularHessianFactor(Key j1, Key j2, Key j3,
│ │ │ │ +
59 const MatrixD& G11, const MatrixD& G12, const MatrixD& G13, const VectorD& g1,
│ │ │ │ +
60 const MatrixD& G22, const MatrixD& G23, const VectorD& g2,
│ │ │ │ +
61 const MatrixD& G33, const VectorD& g3, double f) :
│ │ │ │ +
62 HessianFactor(j1, j2, j3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f) {
│ │ │ │ +
63 }
│ │ │ │ +
│ │ │ │ +
64
│ │ │ │ +
67 template<typename KEYS>
│ │ │ │ +
│ │ │ │ +
68 RegularHessianFactor(const KEYS& keys,
│ │ │ │ +
69 const SymmetricBlockMatrix& augmentedInformation) :
│ │ │ │ +
70 HessianFactor(keys, augmentedInformation) {
│ │ │ │ +
71 checkInvariants();
│ │ │ │ +
72 }
│ │ │ │ +
│ │ │ │ +
73
│ │ │ │ +
│ │ │ │ +
75 RegularHessianFactor(const RegularJacobianFactor<D>& jf)
│ │ │ │ +
76 : HessianFactor(jf) {}
│ │ │ │ +
│ │ │ │ +
77
│ │ │ │ +
│ │ │ │ +
79 RegularHessianFactor(const GaussianFactorGraph& factors,
│ │ │ │ +
80 const Scatter& scatter)
│ │ │ │ +
81 : HessianFactor(factors, scatter) {
│ │ │ │ +
82 checkInvariants();
│ │ │ │ +
83 }
│ │ │ │ +
│ │ │ │ +
84
│ │ │ │ +
│ │ │ │ +
86 RegularHessianFactor(const GaussianFactorGraph& factors)
│ │ │ │ +
87 : HessianFactor(factors) {
│ │ │ │ +
88 checkInvariants();
│ │ │ │ +
89 }
│ │ │ │ +
│ │ │ │ +
90
│ │ │ │ +
91private:
│ │ │ │ +
92
│ │ │ │ +
94 void checkInvariants() {
│ │ │ │ +
95 if (info_.cols() != 1 + (info_.nBlocks()-1) * (DenseIndex)D)
│ │ │ │ +
96 throw std::invalid_argument(
│ │ │ │ +
97 "RegularHessianFactor constructor was given non-regular factors");
│ │ │ │ +
98 }
│ │ │ │ +
99
│ │ │ │ +
100 // Use Eigen magic to access raw memory
│ │ │ │ +
101 typedef Eigen::Map<VectorD> DMap;
│ │ │ │ +
102 typedef Eigen::Map<const VectorD> ConstDMap;
│ │ │ │ +
103
│ │ │ │ +
104 // Scratch space for multiplyHessianAdd
│ │ │ │ +
105 // According to link below this is thread-safe.
│ │ │ │ +
106 // http://stackoverflow.com/questions/11160964/multiple-copies-of-the-same-object-c-thread-safe
│ │ │ │ +
107 mutable std::vector<VectorD> y_;
│ │ │ │ +
108
│ │ │ │ +
109public:
│ │ │ │ +
110
│ │ │ │ +
│ │ │ │ +
112 void multiplyHessianAdd(double alpha, const VectorValues& x,
│ │ │ │ +
113 VectorValues& y) const override {
│ │ │ │ +
114 HessianFactor::multiplyHessianAdd(alpha, x, y);
│ │ │ │ +
115 }
│ │ │ │ +
│ │ │ │ +
116
│ │ │ │ +
│ │ │ │ +
118 void multiplyHessianAdd(double alpha, const double* x,
│ │ │ │ +
119 double* yvalues) const {
│ │ │ │ +
120 // Create a vector of temporary y_ values, corresponding to rows i
│ │ │ │ +
121 y_.resize(size());
│ │ │ │ +
122 for(VectorD & yi: y_)
│ │ │ │ +
123 yi.setZero();
│ │ │ │ +
124
│ │ │ │ +
125 // Accessing the VectorValues one by one is expensive
│ │ │ │ +
126 // So we will loop over columns to access x only once per column
│ │ │ │ +
127 // And fill the above temporary y_ values, to be added into yvalues after
│ │ │ │ +
128 VectorD xj(D);
│ │ │ │ +
129 for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
│ │ │ │ +
130 Key key = keys_[j];
│ │ │ │ +
131 const double* xj = x + key * D;
│ │ │ │ +
132 DenseIndex i = 0;
│ │ │ │ +
133 for (; i < j; ++i)
│ │ │ │ +
134 y_[i] += info_.aboveDiagonalBlock(i, j) * ConstDMap(xj);
│ │ │ │ +
135 // blocks on the diagonal are only half
│ │ │ │ +
136 y_[i] += info_.diagonalBlock(j) * ConstDMap(xj);
│ │ │ │ +
137 // for below diagonal, we take transpose block from upper triangular part
│ │ │ │ +
138 for (i = j + 1; i < (DenseIndex) size(); ++i)
│ │ │ │ +
139 y_[i] += info_.aboveDiagonalBlock(j, i).transpose() * ConstDMap(xj);
│ │ │ │ +
140 }
│ │ │ │ +
141
│ │ │ │ +
142 // copy to yvalues
│ │ │ │ +
143 for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) {
│ │ │ │ +
144 Key key = keys_[i];
│ │ │ │ +
145 DMap(yvalues + key * D) += alpha * y_[i];
│ │ │ │ +
146 }
│ │ │ │ +
147 }
│ │ │ │ +
│ │ │ │ +
148
│ │ │ │ +
│ │ │ │ +
150 void multiplyHessianAdd(double alpha, const double* x, double* yvalues,
│ │ │ │ +
151 std::vector<size_t> offsets) const {
│ │ │ │ +
152
│ │ │ │ +
153 // Create a vector of temporary y_ values, corresponding to rows i
│ │ │ │ +
154 y_.resize(size());
│ │ │ │ +
155 for(VectorD & yi: y_)
│ │ │ │ +
156 yi.setZero();
│ │ │ │ +
157
│ │ │ │ +
158 // Accessing the VectorValues one by one is expensive
│ │ │ │ +
159 // So we will loop over columns to access x only once per column
│ │ │ │ +
160 // And fill the above temporary y_ values, to be added into yvalues after
│ │ │ │ +
161 for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
│ │ │ │ +
162 DenseIndex i = 0;
│ │ │ │ +
163 for (; i < j; ++i)
│ │ │ │ +
164 y_[i] += info_.aboveDiagonalBlock(i, j)
│ │ │ │ +
165 * ConstDMap(x + offsets[keys_[j]],
│ │ │ │ +
166 offsets[keys_[j] + 1] - offsets[keys_[j]]);
│ │ │ │ +
167 // blocks on the diagonal are only half
│ │ │ │ +
168 y_[i] += info_.diagonalBlock(j)
│ │ │ │ +
169 * ConstDMap(x + offsets[keys_[j]],
│ │ │ │ +
170 offsets[keys_[j] + 1] - offsets[keys_[j]]);
│ │ │ │ +
171 // for below diagonal, we take transpose block from upper triangular part
│ │ │ │ +
172 for (i = j + 1; i < (DenseIndex) size(); ++i)
│ │ │ │ +
173 y_[i] += info_.aboveDiagonalBlock(j, i).transpose()
│ │ │ │ +
174 * ConstDMap(x + offsets[keys_[j]],
│ │ │ │ +
175 offsets[keys_[j] + 1] - offsets[keys_[j]]);
│ │ │ │ +
176 }
│ │ │ │ +
177
│ │ │ │ +
178 // copy to yvalues
│ │ │ │ +
179 for (DenseIndex i = 0; i < (DenseIndex) size(); ++i)
│ │ │ │ +
180 DMap(yvalues + offsets[keys_[i]],
│ │ │ │ +
181 offsets[keys_[i] + 1] - offsets[keys_[i]]) += alpha * y_[i];
│ │ │ │ +
182 }
│ │ │ │ +
│ │ │ │ +
183
│ │ │ │ +
│ │ │ │ +
185 void hessianDiagonal(double* d) const override {
│ │ │ │ +
186
│ │ │ │ +
187 // Loop over all variables in the factor
│ │ │ │ +
188 for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) {
│ │ │ │ +
189 Key j = keys_[pos];
│ │ │ │ +
190 // Get the diagonal block, and insert its diagonal
│ │ │ │ +
191 DMap(d + D * j) += info_.diagonal(pos);
│ │ │ │ +
192 }
│ │ │ │ +
193 }
│ │ │ │ +
│ │ │ │ +
194
│ │ │ │ +
│ │ │ │ +
196 void gradientAtZero(double* d) const override {
│ │ │ │ +
197
│ │ │ │ +
198 // Loop over all variables in the factor
│ │ │ │ +
199 for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) {
│ │ │ │ +
200 Key j = keys_[pos];
│ │ │ │ +
201 // Get the diagonal block, and insert its diagonal
│ │ │ │ +
202 DMap(d + D * j) -= info_.aboveDiagonalBlock(pos, size());;
│ │ │ │ +
203 }
│ │ │ │ +
204 }
│ │ │ │ +
│ │ │ │ +
205
│ │ │ │ +
206 /* ************************************************************************* */
│ │ │ │ +
207
│ │ │ │ +
208};
│ │ │ │ +
│ │ │ │ +
209// end class RegularHessianFactor
│ │ │ │ +
210
│ │ │ │ +
211// traits
│ │ │ │ +
│ │ │ │ +
212template<size_t D> struct traits<RegularHessianFactor<D> > : public Testable<
│ │ │ │ +
213 RegularHessianFactor<D> > {
│ │ │ │ +
214};
│ │ │ │ +
│ │ │ │ +
215
│ │ │ │ +
216}
│ │ │ │ +
217
│ │ │ │ +
HessianFactor.h
Contains the HessianFactor class, a general quadratic factor.
│ │ │ │ +
RegularJacobianFactor.h
JacobianFactor class with fixed sized blcoks.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
│ │ │ │ +
gtsam::SymmetricBlockMatrix::diagonal
Vector diagonal(DenseIndex J) const
Get the diagonal of the J'th diagonal block.
Definition SymmetricBlockMatrix.h:145
│ │ │ │ +
gtsam::SymmetricBlockMatrix::cols
DenseIndex cols() const
Column size.
Definition SymmetricBlockMatrix.h:117
│ │ │ │ +
gtsam::SymmetricBlockMatrix::aboveDiagonalBlock
constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
Get block above the diagonal (I, J).
Definition SymmetricBlockMatrix.h:150
│ │ │ │ +
gtsam::SymmetricBlockMatrix::diagonalBlock
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J'th diagonal block as a self adjoint view.
Definition SymmetricBlockMatrix.h:135
│ │ │ │ +
gtsam::SymmetricBlockMatrix::nBlocks
DenseIndex nBlocks() const
Block count.
Definition SymmetricBlockMatrix.h:120
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
│ │ │ │
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
│ │ │ │
gtsam::Factor::size
size_t size() const
Definition Factor.h:157
│ │ │ │ -
gtsam::GaussianFactor
An abstract virtual base class for JacobianFactor and HessianFactor.
Definition GaussianFactor.h:39
│ │ │ │ +
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ +
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition HessianFactor.h:101
│ │ │ │
gtsam::HessianFactor::augmentedInformation
Matrix augmentedInformation() const override
Return the augmented information matrix represented by this GaussianFactor.
Definition HessianFactor.cpp:282
│ │ │ │ -
gtsam::HessianFactor::HessianFactor
HessianFactor()
default constructor for I/O
Definition HessianFactor.cpp:77
│ │ │ │ +
gtsam::HessianFactor::info_
SymmetricBlockMatrix info_
The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'H[x -1].
Definition HessianFactor.h:104
│ │ │ │ +
gtsam::HessianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
y += alpha * A'*A*x
Definition HessianFactor.cpp:391
│ │ │ │ +
gtsam::RegularHessianFactor
Definition RegularHessianFactor.h:28
│ │ │ │ +
gtsam::RegularHessianFactor::RegularHessianFactor
RegularHessianFactor(const GaussianFactorGraph &factors)
Construct from a GaussianFactorGraph.
Definition RegularHessianFactor.h:86
│ │ │ │ +
gtsam::RegularHessianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
y += alpha * A'*A*x
Definition RegularHessianFactor.h:112
│ │ │ │ +
gtsam::RegularHessianFactor::RegularHessianFactor
RegularHessianFactor(Key j1, Key j2, Key j3, const MatrixD &G11, const MatrixD &G12, const MatrixD &G13, const VectorD &g1, const MatrixD &G22, const MatrixD &G23, const VectorD &g2, const MatrixD &G33, const VectorD &g3, double f)
Construct a ternary factor.
Definition RegularHessianFactor.h:58
│ │ │ │ +
gtsam::RegularHessianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const double *x, double *yvalues, std::vector< size_t > offsets) const
Raw memory version, with offsets TODO document reasoning.
Definition RegularHessianFactor.h:150
│ │ │ │ +
gtsam::RegularHessianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const double *x, double *yvalues) const
y += alpha * A'*A*x
Definition RegularHessianFactor.h:118
│ │ │ │ +
gtsam::RegularHessianFactor::hessianDiagonal
void hessianDiagonal(double *d) const override
Return the diagonal of the Hessian for this factor (raw memory version)
Definition RegularHessianFactor.h:185
│ │ │ │ +
gtsam::RegularHessianFactor::RegularHessianFactor
RegularHessianFactor(const KEYS &keys, const SymmetricBlockMatrix &augmentedInformation)
Constructor with an arbitrary number of keys and with the augmented information matrix specified as a...
Definition RegularHessianFactor.h:68
│ │ │ │ +
gtsam::RegularHessianFactor::gradientAtZero
void gradientAtZero(double *d) const override
Add gradient at zero to d TODO: is it really the goal to add ??
Definition RegularHessianFactor.h:196
│ │ │ │ +
gtsam::RegularHessianFactor::RegularHessianFactor
RegularHessianFactor(const GaussianFactorGraph &factors, const Scatter &scatter)
Construct from a GaussianFactorGraph.
Definition RegularHessianFactor.h:79
│ │ │ │ +
gtsam::RegularHessianFactor::RegularHessianFactor
RegularHessianFactor(const KeyVector &js, const std::vector< Matrix > &Gs, const std::vector< Vector > &gs, double f)
Construct an n-way factor.
Definition RegularHessianFactor.h:39
│ │ │ │ +
gtsam::RegularHessianFactor::RegularHessianFactor
RegularHessianFactor(Key j1, Key j2, const MatrixD &G11, const MatrixD &G12, const VectorD &g1, const MatrixD &G22, const VectorD &g2, double f)
Construct a binary factor.
Definition RegularHessianFactor.h:49
│ │ │ │ +
gtsam::RegularHessianFactor::RegularHessianFactor
RegularHessianFactor(const RegularJacobianFactor< D > &jf)
Construct from RegularJacobianFactor.
Definition RegularHessianFactor.h:75
│ │ │ │ +
gtsam::RegularJacobianFactor
JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator.
Definition RegularJacobianFactor.h:32
│ │ │ │ +
gtsam::Scatter
Scatter is an intermediate data structure used when building a HessianFactor incrementally,...
Definition Scatter.h:49
│ │ │ │ +
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -HessianFactor-inl.h │ │ │ │ │ +RegularHessianFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ @@ -16,65 +16,341 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 19#pragma once │ │ │ │ │ 20 │ │ │ │ │ -21namespace _g_t_s_a_m { │ │ │ │ │ -22 │ │ │ │ │ -23 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -24 template │ │ │ │ │ -_2_5 _H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r(const KEYS& keys, const _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x& │ │ │ │ │ -augmentedInformation) : │ │ │ │ │ -26 _G_a_u_s_s_i_a_n_F_a_c_t_o_r(keys), info_(augmentedInformation) │ │ │ │ │ -27 { │ │ │ │ │ -28 // Check number of variables │ │ │ │ │ -29 if((_D_e_n_s_e_I_n_d_e_x)_B_a_s_e_:_:_k_e_y_s__._s_i_z_e() != _a_u_g_m_e_n_t_e_d_I_n_f_o_r_m_a_t_i_o_n.nBlocks() - 1) │ │ │ │ │ -30 throw std::invalid_argument( │ │ │ │ │ -31 "Error in HessianFactor constructor input. Number of provided keys plus\n" │ │ │ │ │ -32 "one for the information vector must equal the number of provided matrix │ │ │ │ │ -blocks. "); │ │ │ │ │ -33 │ │ │ │ │ -34 // Check RHS dimension │ │ │ │ │ -35 if(_a_u_g_m_e_n_t_e_d_I_n_f_o_r_m_a_t_i_o_n.getDim(_a_u_g_m_e_n_t_e_d_I_n_f_o_r_m_a_t_i_o_n.nBlocks() - 1) != 1) │ │ │ │ │ -36 throw std::invalid_argument( │ │ │ │ │ -37 "Error in HessianFactor constructor input. The last provided matrix block\n" │ │ │ │ │ -38 "must be the information vector, but the last provided block had more than │ │ │ │ │ -one column."); │ │ │ │ │ -39 } │ │ │ │ │ -40 │ │ │ │ │ -41} │ │ │ │ │ +21#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_H_e_s_s_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ +23#include │ │ │ │ │ +24 │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ +26 │ │ │ │ │ +27template │ │ │ │ │ +_2_8class _R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r: public _H_e_s_s_i_a_n_F_a_c_t_o_r { │ │ │ │ │ +29 │ │ │ │ │ +30public: │ │ │ │ │ +31 │ │ │ │ │ +32 typedef Eigen::Matrix VectorD; │ │ │ │ │ +33 typedef Eigen::Matrix MatrixD; │ │ │ │ │ +34 │ │ │ │ │ +_3_9 _R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r(const _K_e_y_V_e_c_t_o_r& js, │ │ │ │ │ +40 const std::vector& Gs, const std::vector& gs, double f) : │ │ │ │ │ +41 _H_e_s_s_i_a_n_F_a_c_t_o_r(js, Gs, gs, f) { │ │ │ │ │ +42 checkInvariants(); │ │ │ │ │ +43 } │ │ │ │ │ +44 │ │ │ │ │ +_4_9 _R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r(_K_e_y j1, _K_e_y j2, const MatrixD& G11, const MatrixD& G12, │ │ │ │ │ +50 const VectorD& g1, const MatrixD& G22, const VectorD& g2, double f) : │ │ │ │ │ +51 _H_e_s_s_i_a_n_F_a_c_t_o_r(j1, j2, G11, G12, g1, G22, g2, f) { │ │ │ │ │ +52 } │ │ │ │ │ +53 │ │ │ │ │ +_5_8 _R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r(_K_e_y j1, _K_e_y j2, _K_e_y j3, │ │ │ │ │ +59 const MatrixD& G11, const MatrixD& G12, const MatrixD& G13, const VectorD& │ │ │ │ │ +g1, │ │ │ │ │ +60 const MatrixD& G22, const MatrixD& G23, const VectorD& g2, │ │ │ │ │ +61 const MatrixD& G33, const VectorD& g3, double f) : │ │ │ │ │ +62 _H_e_s_s_i_a_n_F_a_c_t_o_r(j1, j2, j3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f) { │ │ │ │ │ +63 } │ │ │ │ │ +64 │ │ │ │ │ +67 template │ │ │ │ │ +_6_8 _R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r(const KEYS& _k_e_y_s, │ │ │ │ │ +69 const _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x& _a_u_g_m_e_n_t_e_d_I_n_f_o_r_m_a_t_i_o_n) : │ │ │ │ │ +70 _H_e_s_s_i_a_n_F_a_c_t_o_r(_k_e_y_s, _a_u_g_m_e_n_t_e_d_I_n_f_o_r_m_a_t_i_o_n) { │ │ │ │ │ +71 checkInvariants(); │ │ │ │ │ +72 } │ │ │ │ │ +73 │ │ │ │ │ +_7_5 _R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r(const _R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_<_D_>& jf) │ │ │ │ │ +76 : _H_e_s_s_i_a_n_F_a_c_t_o_r(jf) {} │ │ │ │ │ +77 │ │ │ │ │ +_7_9 _R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& factors, │ │ │ │ │ +80 const _S_c_a_t_t_e_r& scatter) │ │ │ │ │ +81 : _H_e_s_s_i_a_n_F_a_c_t_o_r(factors, scatter) { │ │ │ │ │ +82 checkInvariants(); │ │ │ │ │ +83 } │ │ │ │ │ +84 │ │ │ │ │ +_8_6 _R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& factors) │ │ │ │ │ +87 : _H_e_s_s_i_a_n_F_a_c_t_o_r(factors) { │ │ │ │ │ +88 checkInvariants(); │ │ │ │ │ +89 } │ │ │ │ │ +90 │ │ │ │ │ +91private: │ │ │ │ │ +92 │ │ │ │ │ +94 void checkInvariants() { │ │ │ │ │ +95 if (_i_n_f_o__._c_o_l_s() != 1 + (_i_n_f_o__._n_B_l_o_c_k_s()-1) * (_D_e_n_s_e_I_n_d_e_x)D) │ │ │ │ │ +96 throw std::invalid_argument( │ │ │ │ │ +97 "RegularHessianFactor constructor was given non-regular factors"); │ │ │ │ │ +98 } │ │ │ │ │ +99 │ │ │ │ │ +100 // Use Eigen magic to access raw memory │ │ │ │ │ +101 typedef Eigen::Map DMap; │ │ │ │ │ +102 typedef Eigen::Map ConstDMap; │ │ │ │ │ +103 │ │ │ │ │ +104 // Scratch space for multiplyHessianAdd │ │ │ │ │ +105 // According to link below this is thread-safe. │ │ │ │ │ +106 // http://stackoverflow.com/questions/11160964/multiple-copies-of-the-same- │ │ │ │ │ +object-c-thread-safe │ │ │ │ │ +107 mutable std::vector y_; │ │ │ │ │ +108 │ │ │ │ │ +109public: │ │ │ │ │ +110 │ │ │ │ │ +_1_1_2 void _m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d(double alpha, const _V_e_c_t_o_r_V_a_l_u_e_s& x, │ │ │ │ │ +113 _V_e_c_t_o_r_V_a_l_u_e_s& y) const override { │ │ │ │ │ +114 _H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d(alpha, x, y); │ │ │ │ │ +115 } │ │ │ │ │ +116 │ │ │ │ │ +_1_1_8 void _m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d(double alpha, const double* x, │ │ │ │ │ +119 double* yvalues) const { │ │ │ │ │ +120 // Create a vector of temporary y_ values, corresponding to rows i │ │ │ │ │ +121 y_.resize(_s_i_z_e()); │ │ │ │ │ +122 for(VectorD & yi: y_) │ │ │ │ │ +123 yi.setZero(); │ │ │ │ │ +124 │ │ │ │ │ +125 // Accessing the VectorValues one by one is expensive │ │ │ │ │ +126 // So we will loop over columns to access x only once per column │ │ │ │ │ +127 // And fill the above temporary y_ values, to be added into yvalues after │ │ │ │ │ +128 VectorD xj(D); │ │ │ │ │ +129 for (_D_e_n_s_e_I_n_d_e_x j = 0; j < (_D_e_n_s_e_I_n_d_e_x) _s_i_z_e(); ++j) { │ │ │ │ │ +130 _K_e_y key = _k_e_y_s__[j]; │ │ │ │ │ +131 const double* xj = x + key * D; │ │ │ │ │ +132 _D_e_n_s_e_I_n_d_e_x i = 0; │ │ │ │ │ +133 for (; i < j; ++i) │ │ │ │ │ +134 y_[i] += _i_n_f_o__._a_b_o_v_e_D_i_a_g_o_n_a_l_B_l_o_c_k(i, j) * ConstDMap(xj); │ │ │ │ │ +135 // blocks on the diagonal are only half │ │ │ │ │ +136 y_[i] += _i_n_f_o__._d_i_a_g_o_n_a_l_B_l_o_c_k(j) * ConstDMap(xj); │ │ │ │ │ +137 // for below diagonal, we take transpose block from upper triangular part │ │ │ │ │ +138 for (i = j + 1; i < (_D_e_n_s_e_I_n_d_e_x) _s_i_z_e(); ++i) │ │ │ │ │ +139 y_[i] += _i_n_f_o__._a_b_o_v_e_D_i_a_g_o_n_a_l_B_l_o_c_k(j, i).transpose() * ConstDMap(xj); │ │ │ │ │ +140 } │ │ │ │ │ +141 │ │ │ │ │ +142 // copy to yvalues │ │ │ │ │ +143 for (_D_e_n_s_e_I_n_d_e_x i = 0; i < (_D_e_n_s_e_I_n_d_e_x) _s_i_z_e(); ++i) { │ │ │ │ │ +144 _K_e_y key = _k_e_y_s__[i]; │ │ │ │ │ +145 DMap(yvalues + key * D) += alpha * y_[i]; │ │ │ │ │ +146 } │ │ │ │ │ +147 } │ │ │ │ │ +148 │ │ │ │ │ +_1_5_0 void _m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d(double alpha, const double* x, double* yvalues, │ │ │ │ │ +151 std::vector offsets) const { │ │ │ │ │ +152 │ │ │ │ │ +153 // Create a vector of temporary y_ values, corresponding to rows i │ │ │ │ │ +154 y_.resize(_s_i_z_e()); │ │ │ │ │ +155 for(VectorD & yi: y_) │ │ │ │ │ +156 yi.setZero(); │ │ │ │ │ +157 │ │ │ │ │ +158 // Accessing the VectorValues one by one is expensive │ │ │ │ │ +159 // So we will loop over columns to access x only once per column │ │ │ │ │ +160 // And fill the above temporary y_ values, to be added into yvalues after │ │ │ │ │ +161 for (_D_e_n_s_e_I_n_d_e_x j = 0; j < (_D_e_n_s_e_I_n_d_e_x) _s_i_z_e(); ++j) { │ │ │ │ │ +162 _D_e_n_s_e_I_n_d_e_x i = 0; │ │ │ │ │ +163 for (; i < j; ++i) │ │ │ │ │ +164 y_[i] += _i_n_f_o__._a_b_o_v_e_D_i_a_g_o_n_a_l_B_l_o_c_k(i, j) │ │ │ │ │ +165 * ConstDMap(x + offsets[_k_e_y_s__[j]], │ │ │ │ │ +166 offsets[_k_e_y_s__[j] + 1] - offsets[_k_e_y_s__[j]]); │ │ │ │ │ +167 // blocks on the diagonal are only half │ │ │ │ │ +168 y_[i] += _i_n_f_o__._d_i_a_g_o_n_a_l_B_l_o_c_k(j) │ │ │ │ │ +169 * ConstDMap(x + offsets[_k_e_y_s__[j]], │ │ │ │ │ +170 offsets[_k_e_y_s__[j] + 1] - offsets[_k_e_y_s__[j]]); │ │ │ │ │ +171 // for below diagonal, we take transpose block from upper triangular part │ │ │ │ │ +172 for (i = j + 1; i < (_D_e_n_s_e_I_n_d_e_x) _s_i_z_e(); ++i) │ │ │ │ │ +173 y_[i] += _i_n_f_o__._a_b_o_v_e_D_i_a_g_o_n_a_l_B_l_o_c_k(j, i).transpose() │ │ │ │ │ +174 * ConstDMap(x + offsets[_k_e_y_s__[j]], │ │ │ │ │ +175 offsets[_k_e_y_s__[j] + 1] - offsets[_k_e_y_s__[j]]); │ │ │ │ │ +176 } │ │ │ │ │ +177 │ │ │ │ │ +178 // copy to yvalues │ │ │ │ │ +179 for (_D_e_n_s_e_I_n_d_e_x i = 0; i < (_D_e_n_s_e_I_n_d_e_x) _s_i_z_e(); ++i) │ │ │ │ │ +180 DMap(yvalues + offsets[_k_e_y_s__[i]], │ │ │ │ │ +181 offsets[_k_e_y_s__[i] + 1] - offsets[_k_e_y_s__[i]]) += alpha * y_[i]; │ │ │ │ │ +182 } │ │ │ │ │ +183 │ │ │ │ │ +_1_8_5 void _h_e_s_s_i_a_n_D_i_a_g_o_n_a_l(double* d) const override { │ │ │ │ │ +186 │ │ │ │ │ +187 // Loop over all variables in the factor │ │ │ │ │ +188 for (_D_e_n_s_e_I_n_d_e_x pos = 0; pos < (_D_e_n_s_e_I_n_d_e_x) _s_i_z_e(); ++pos) { │ │ │ │ │ +189 _K_e_y j = _k_e_y_s__[pos]; │ │ │ │ │ +190 // Get the diagonal block, and insert its diagonal │ │ │ │ │ +191 DMap(d + D * j) += _i_n_f_o__._d_i_a_g_o_n_a_l(pos); │ │ │ │ │ +192 } │ │ │ │ │ +193 } │ │ │ │ │ +194 │ │ │ │ │ +_1_9_6 void _g_r_a_d_i_e_n_t_A_t_Z_e_r_o(double* d) const override { │ │ │ │ │ +197 │ │ │ │ │ +198 // Loop over all variables in the factor │ │ │ │ │ +199 for (_D_e_n_s_e_I_n_d_e_x pos = 0; pos < (_D_e_n_s_e_I_n_d_e_x) _s_i_z_e(); ++pos) { │ │ │ │ │ +200 _K_e_y j = _k_e_y_s__[pos]; │ │ │ │ │ +201 // Get the diagonal block, and insert its diagonal │ │ │ │ │ +202 DMap(d + D * j) -= _i_n_f_o__._a_b_o_v_e_D_i_a_g_o_n_a_l_B_l_o_c_k(pos, _s_i_z_e());; │ │ │ │ │ +203 } │ │ │ │ │ +204 } │ │ │ │ │ +205 │ │ │ │ │ +206 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +207 │ │ │ │ │ +208}; │ │ │ │ │ +209// end class RegularHessianFactor │ │ │ │ │ +210 │ │ │ │ │ +211// traits │ │ │ │ │ +_2_1_2template struct _t_r_a_i_t_s<_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r > : public │ │ │ │ │ +_T_e_s_t_a_b_l_e< │ │ │ │ │ +213 RegularHessianFactor > { │ │ │ │ │ +214}; │ │ │ │ │ +215 │ │ │ │ │ +216} │ │ │ │ │ +217 │ │ │ │ │ +_H_e_s_s_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ +Contains the HessianFactor class, a general quadratic factor. │ │ │ │ │ +_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ +JacobianFactor class with fixed sized blcoks. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ _g_t_s_a_m_:_:_D_e_n_s_e_I_n_d_e_x │ │ │ │ │ ptrdiff_t DenseIndex │ │ │ │ │ The index type for Eigen objects. │ │ │ │ │ DDeeffiinniittiioonn types.h:106 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ _g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ of blocks. │ │ │ │ │ DDeeffiinniittiioonn SymmetricBlockMatrix.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_d_i_a_g_o_n_a_l │ │ │ │ │ +Vector diagonal(DenseIndex J) const │ │ │ │ │ +Get the diagonal of the J'th diagonal block. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:145 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_c_o_l_s │ │ │ │ │ +DenseIndex cols() const │ │ │ │ │ +Column size. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:117 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_a_b_o_v_e_D_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ +constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const │ │ │ │ │ +Get block above the diagonal (I, J). │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:150 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_d_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ +Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J) │ │ │ │ │ +Return the J'th diagonal block as a self adjoint view. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:135 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_n_B_l_o_c_k_s │ │ │ │ │ +DenseIndex nBlocks() const │ │ │ │ │ +Block count. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:120 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s │ │ │ │ │ +const KeyVector & keys() const │ │ │ │ │ +Access the factor's involved variable keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:140 │ │ │ │ │ _g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s__ │ │ │ │ │ KeyVector keys_ │ │ │ │ │ The keys involved in this factor. │ │ │ │ │ DDeeffiinniittiioonn Factor.h:85 │ │ │ │ │ _g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_s_i_z_e │ │ │ │ │ size_t size() const │ │ │ │ │ DDeeffiinniittiioonn Factor.h:157 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -An abstract virtual base class for JacobianFactor and HessianFactor. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactor.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +A Gaussian factor using the canonical parameters (information form) │ │ │ │ │ +DDeeffiinniittiioonn HessianFactor.h:101 │ │ │ │ │ _g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_a_u_g_m_e_n_t_e_d_I_n_f_o_r_m_a_t_i_o_n │ │ │ │ │ Matrix augmentedInformation() const override │ │ │ │ │ Return the augmented information matrix represented by this GaussianFactor. │ │ │ │ │ DDeeffiinniittiioonn HessianFactor.cpp:282 │ │ │ │ │ -_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -HessianFactor() │ │ │ │ │ -default constructor for I/O │ │ │ │ │ -DDeeffiinniittiioonn HessianFactor.cpp:77 │ │ │ │ │ +_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_i_n_f_o__ │ │ │ │ │ +SymmetricBlockMatrix info_ │ │ │ │ │ +The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'H │ │ │ │ │ +[x -1]. │ │ │ │ │ +DDeeffiinniittiioonn HessianFactor.h:104 │ │ │ │ │ +_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d │ │ │ │ │ +void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) │ │ │ │ │ +const override │ │ │ │ │ +y += alpha * A'*A*x │ │ │ │ │ +DDeeffiinniittiioonn HessianFactor.cpp:391 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +DDeeffiinniittiioonn RegularHessianFactor.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +RegularHessianFactor(const GaussianFactorGraph &factors) │ │ │ │ │ +Construct from a GaussianFactorGraph. │ │ │ │ │ +DDeeffiinniittiioonn RegularHessianFactor.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d │ │ │ │ │ +void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) │ │ │ │ │ +const override │ │ │ │ │ +y += alpha * A'*A*x │ │ │ │ │ +DDeeffiinniittiioonn RegularHessianFactor.h:112 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +RegularHessianFactor(Key j1, Key j2, Key j3, const MatrixD &G11, const MatrixD │ │ │ │ │ +&G12, const MatrixD &G13, const VectorD &g1, const MatrixD &G22, const MatrixD │ │ │ │ │ +&G23, const VectorD &g2, const MatrixD &G33, const VectorD &g3, double f) │ │ │ │ │ +Construct a ternary factor. │ │ │ │ │ +DDeeffiinniittiioonn RegularHessianFactor.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d │ │ │ │ │ +void multiplyHessianAdd(double alpha, const double *x, double *yvalues, std:: │ │ │ │ │ +vector< size_t > offsets) const │ │ │ │ │ +Raw memory version, with offsets TODO document reasoning. │ │ │ │ │ +DDeeffiinniittiioonn RegularHessianFactor.h:150 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d │ │ │ │ │ +void multiplyHessianAdd(double alpha, const double *x, double *yvalues) const │ │ │ │ │ +y += alpha * A'*A*x │ │ │ │ │ +DDeeffiinniittiioonn RegularHessianFactor.h:118 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_h_e_s_s_i_a_n_D_i_a_g_o_n_a_l │ │ │ │ │ +void hessianDiagonal(double *d) const override │ │ │ │ │ +Return the diagonal of the Hessian for this factor (raw memory version) │ │ │ │ │ +DDeeffiinniittiioonn RegularHessianFactor.h:185 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +RegularHessianFactor(const KEYS &keys, const SymmetricBlockMatrix │ │ │ │ │ +&augmentedInformation) │ │ │ │ │ +Constructor with an arbitrary number of keys and with the augmented information │ │ │ │ │ +matrix specified as a... │ │ │ │ │ +DDeeffiinniittiioonn RegularHessianFactor.h:68 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_g_r_a_d_i_e_n_t_A_t_Z_e_r_o │ │ │ │ │ +void gradientAtZero(double *d) const override │ │ │ │ │ +Add gradient at zero to d TODO: is it really the goal to add ?? │ │ │ │ │ +DDeeffiinniittiioonn RegularHessianFactor.h:196 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +RegularHessianFactor(const GaussianFactorGraph &factors, const Scatter │ │ │ │ │ +&scatter) │ │ │ │ │ +Construct from a GaussianFactorGraph. │ │ │ │ │ +DDeeffiinniittiioonn RegularHessianFactor.h:79 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +RegularHessianFactor(const KeyVector &js, const std::vector< Matrix > &Gs, │ │ │ │ │ +const std::vector< Vector > &gs, double f) │ │ │ │ │ +Construct an n-way factor. │ │ │ │ │ +DDeeffiinniittiioonn RegularHessianFactor.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +RegularHessianFactor(Key j1, Key j2, const MatrixD &G11, const MatrixD &G12, │ │ │ │ │ +const VectorD &g1, const MatrixD &G22, const VectorD &g2, double f) │ │ │ │ │ +Construct a binary factor. │ │ │ │ │ +DDeeffiinniittiioonn RegularHessianFactor.h:49 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +RegularHessianFactor(const RegularJacobianFactor< D > &jf) │ │ │ │ │ +Construct from RegularJacobianFactor. │ │ │ │ │ +DDeeffiinniittiioonn RegularHessianFactor.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ +JacobianFactor with constant sized blocks Provides raw memory access versions │ │ │ │ │ +of linear operator. │ │ │ │ │ +DDeeffiinniittiioonn RegularJacobianFactor.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_S_c_a_t_t_e_r │ │ │ │ │ +Scatter is an intermediate data structure used when building a HessianFactor │ │ │ │ │ +incrementally,... │ │ │ │ │ +DDeeffiinniittiioonn Scatter.h:49 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ +with a unique integer... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _H_e_s_s_i_a_n_F_a_c_t_o_r_-_i_n_l_._h │ │ │ │ │ + * _R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00875.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/SubgraphBuilder.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,50 +96,50 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces | │ │ │ │ Functions
│ │ │ │ -
SubgraphBuilder.cpp File Reference
│ │ │ │ +
NoiseModel.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
namespace  gtsam::noiseModel
 All noise models live in the noiseModel namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

│ │ │ │ -ostream & gtsam::operator<< (ostream &os, const Subgraph::Edge &edge)
 
│ │ │ │ -ostream & gtsam::operator<< (ostream &os, const Subgraph &subgraph)
 
│ │ │ │ -ostream & gtsam::operator<< (ostream &os, const SubgraphBuilderParameters &p)
 
│ │ │ │ -GaussianFactorGraph gtsam::buildFactorSubgraph (const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone)
 Select the factors in a factor graph according to the subgraph.
 
std::pair< GaussianFactorGraph, GaussianFactorGraph > gtsam::splitFactorGraph (const GaussianFactorGraph &factorGraph, const Subgraph &subgraph)
 Split the graph into a subgraph and the remaining edges.
 
│ │ │ │ +template<class MATRIX >
void gtsam::noiseModel::updateAb (MATRIX &Ab, int j, const Vector &a, const Vector &rd)
 
│ │ │ │ +boost::optional< Vector > gtsam::noiseModel::checkIfDiagonal (const Matrix &M)
 
│ │ │ │ +template<typename VECTOR >
boost::optional< size_t > gtsam::noiseModel::check_if_constraint (VECTOR a, const Vector &invsigmas, size_t m)
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Dec 31, 2009
│ │ │ │ -
Author
Frank Dellaert, Yong-Dian Jian
│ │ │ │ +
Date
Jan 13, 2010
│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,42 +1,35 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -SubgraphBuilder.cpp File Reference │ │ │ │ │ +NoiseModel.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ - ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const │ │ │ │ │ - _S_u_b_g_r_a_p_h_:_:_E_d_g_e &edge) │ │ │ │ │ -  │ │ │ │ │ - ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const _S_u_b_g_r_a_p_h │ │ │ │ │ - &subgraph) │ │ │ │ │ -  │ │ │ │ │ - ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const │ │ │ │ │ - _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s &p) │ │ │ │ │ +namespace   _g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l │ │ │ │ │ +  All noise models live in the _n_o_i_s_e_M_o_d_e_l namespace. │ │ │ │ │   │ │ │ │ │ - _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h  ggttssaamm::::bbuuiillddFFaaccttoorrSSuubbggrraapphh (const │ │ │ │ │ - _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, const _S_u_b_g_r_a_p_h │ │ │ │ │ - &subgraph, const bool clone) │ │ │ │ │ -  Select the factors in a factor graph according │ │ │ │ │ - to the subgraph. │ │ │ │ │ -  │ │ │ │ │ -std::pair< _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h, _g_t_s_a_m_:_:_s_p_l_i_t_F_a_c_t_o_r_G_r_a_p_h (const │ │ │ │ │ - _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h >  _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &factorGraph, const │ │ │ │ │ - _S_u_b_g_r_a_p_h &subgraph) │ │ │ │ │ -  Split the graph into a subgraph and the │ │ │ │ │ - remaining edges. │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +template │ │ │ │ │ + void  ggttssaamm::::nnooiisseeMMooddeell::::uuppddaatteeAAbb (MATRIX &Ab, int j, │ │ │ │ │ + const Vector &a, const Vector &rd) │ │ │ │ │ +  │ │ │ │ │ +boost::optional< Vector >  ggttssaamm::::nnooiisseeMMooddeell::::cchheecckkIIffDDiiaaggoonnaall (const Matrix &M) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +boost::optional< size_t >  ggttssaamm::::nnooiisseeMMooddeell::::cchheecckk__iiff__ccoonnssttrraaiinntt (VECTOR a, │ │ │ │ │ + const Vector &invsigmas, size_t m) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Date │ │ │ │ │ - Dec 31, 2009 │ │ │ │ │ + Jan 13, 2010 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert, Yong-Dian Jian │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_._c_p_p │ │ │ │ │ + * _N_o_i_s_e_M_o_d_e_l_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00878.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianJunctionTree.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/iterative-inl.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,42 +95,51 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
GaussianJunctionTree.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
iterative-inl.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ +

Iterative methods, template implementation. │ │ │ │ +More...

│ │ │ │ + │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::GaussianJunctionTree
 A junction tree specialized to Gaussian factors, i.e., it is a cluster tree with Gaussian factors stored in each cluster. More...
struct  gtsam::CGState< S, V, E >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

template<class S , class V , class E >
V gtsam::conjugateGradients (const S &Ab, V x, const ConjugateGradientParameters &parameters, bool steepest=false)
 Method of conjugate gradients (CG) template "System" class S needs gradient(S,v), e=S*v, v=S^e "Vector" class V needs dot(v,v), -v, v+v, s*v "Vector" class E needs dot(v,v)
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Mar 29, 2013
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ +

Iterative methods, template implementation.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
Date
Dec 28, 2009
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,27 +1,34 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -GaussianJunctionTree.h File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +iterative-inl.h File Reference │ │ │ │ │ +Iterative methods, template implementation. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ -  A junction tree specialized to Gaussian factors, i.e., it is a cluster │ │ │ │ │ - tree with Gaussian factors stored in each cluster. _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_C_G_S_t_a_t_e_<_ _S_,_ _V_,_ _E_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +template │ │ │ │ │ +V  _g_t_s_a_m_:_:_c_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_s (const S &Ab, V x, const │ │ │ │ │ + _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s ¶meters, bool steepest=false) │ │ │ │ │ + Method of conjugate gradients (CG) template "System" class S needs gradient │ │ │ │ │ +  (S,v), e=S*v, v=S^e "Vector" class V needs dot(v,v), -v, v+v, s*v "Vector" │ │ │ │ │ + class E needs dot(v,v) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Mar 29, 2013 │ │ │ │ │ +Iterative methods, template implementation. │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Dec 28, 2009 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_J_u_n_c_t_i_o_n_T_r_e_e_._h │ │ │ │ │ + * _i_t_e_r_a_t_i_v_e_-_i_n_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00878_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianJunctionTree.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/iterative-inl.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,15 +98,15 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
GaussianJunctionTree.h
│ │ │ │ +
iterative-inl.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │ @@ -114,50 +114,163 @@ │ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ -
22#include <gtsam/linear/GaussianBayesTree.h>
│ │ │ │ -
23#include <gtsam/inference/JunctionTree.h>
│ │ │ │ +
21#include <gtsam/linear/iterative.h>
│ │ │ │ +
22#include <gtsam/linear/ConjugateGradientSolver.h>
│ │ │ │ +
23#include <boost/shared_ptr.hpp>
│ │ │ │
24
│ │ │ │
25namespace gtsam {
│ │ │ │
26
│ │ │ │ -
27 // Forward declarations
│ │ │ │ -
28 class GaussianEliminationTree;
│ │ │ │ -
29
│ │ │ │ -
│ │ │ │ -
38 class GTSAM_EXPORT GaussianJunctionTree :
│ │ │ │ -
39 public JunctionTree<GaussianBayesTree, GaussianFactorGraph> {
│ │ │ │ -
40 public:
│ │ │ │ -
41 typedef JunctionTree<GaussianBayesTree, GaussianFactorGraph> Base;
│ │ │ │ -
42 typedef GaussianJunctionTree This;
│ │ │ │ -
43 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
44
│ │ │ │ -
53 GaussianJunctionTree(const GaussianEliminationTree& eliminationTree);
│ │ │ │ -
54 };
│ │ │ │ -
│ │ │ │ -
55
│ │ │ │ -
56}
│ │ │ │ -
JunctionTree.h
The junction tree.
│ │ │ │ -
GaussianBayesTree.h
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
│ │ │ │ -
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ +
27 /* ************************************************************************* */
│ │ │ │ +
28 // state for CG method
│ │ │ │ +
29 template<class S, class V, class E>
│ │ │ │ +
│ │ │ │ +
30 struct CGState {
│ │ │ │ +
31
│ │ │ │ +
32 typedef ConjugateGradientParameters Parameters;
│ │ │ │ +
33 const Parameters &parameters_;
│ │ │ │ +
34
│ │ │ │ +
35 int k;
│ │ │ │ +
36 bool steepest;
│ │ │ │ +
37 V g, d;
│ │ │ │ +
38 double gamma, threshold;
│ │ │ │ +
39 E Ad;
│ │ │ │ +
40
│ │ │ │ +
41 /* ************************************************************************* */
│ │ │ │ +
42 // Constructor
│ │ │ │ +
43 CGState(const S& Ab, const V& x, const Parameters &parameters, bool steep):
│ │ │ │ +
44 parameters_(parameters),k(0),steepest(steep) {
│ │ │ │ +
45
│ │ │ │ +
46 // Start with g0 = A'*(A*x0-b), d0 = - g0
│ │ │ │ +
47 // i.e., first step is in direction of negative gradient
│ │ │ │ +
48 g = Ab.gradient(x);
│ │ │ │ +
49 d = g; // instead of negating gradient, alpha will be negated
│ │ │ │ +
50
│ │ │ │ +
51 // init gamma and calculate threshold
│ │ │ │ +
52 gamma = dot(g,g);
│ │ │ │ +
53 threshold = std::max(parameters_.epsilon_abs(), parameters_.epsilon() * parameters_.epsilon() * gamma);
│ │ │ │ +
54
│ │ │ │ +
55 // Allocate and calculate A*d for first iteration
│ │ │ │ +
56 if (gamma > parameters_.epsilon_abs()) Ad = Ab * d;
│ │ │ │ +
57 }
│ │ │ │ +
58
│ │ │ │ +
59 /* ************************************************************************* */
│ │ │ │ +
60 // print
│ │ │ │ +
61 void print(const V& x) {
│ │ │ │ +
62 std::cout << "iteration = " << k << std::endl;
│ │ │ │ +
63 gtsam::print(x,"x");
│ │ │ │ +
64 gtsam::print(g, "g");
│ │ │ │ +
65 std::cout << "dotg = " << gamma << std::endl;
│ │ │ │ +
66 gtsam::print(d, "d");
│ │ │ │ +
67 gtsam::print(Ad, "Ad");
│ │ │ │ +
68 }
│ │ │ │ +
69
│ │ │ │ +
70 /* ************************************************************************* */
│ │ │ │ +
71 // step the solution
│ │ │ │ +
72 double takeOptimalStep(V& x) {
│ │ │ │ +
73 // TODO: can we use gamma instead of dot(d,g) ????? Answer not trivial
│ │ │ │ +
74 double alpha = -dot(d, g) / dot(Ad, Ad); // calculate optimal step-size
│ │ │ │ +
75 x += alpha * d; // do step in new search direction, x += alpha*d
│ │ │ │ +
76 return alpha;
│ │ │ │ +
77 }
│ │ │ │ +
78
│ │ │ │ +
79 /* ************************************************************************* */
│ │ │ │ +
80 // take a step, return true if converged
│ │ │ │ +
81 bool step(const S& Ab, V& x) {
│ │ │ │ +
82
│ │ │ │ +
83 if ((++k) >= ((int)parameters_.maxIterations())) return true;
│ │ │ │ +
84
│ │ │ │ +
85 //---------------------------------->
│ │ │ │ +
86 double alpha = takeOptimalStep(x);
│ │ │ │ +
87
│ │ │ │ +
88 // update gradient (or re-calculate at reset time)
│ │ │ │ +
89 if (k % parameters_.reset() == 0) g = Ab.gradient(x);
│ │ │ │ +
90 // axpy(alpha, Ab ^ Ad, g); // g += alpha*(Ab^Ad)
│ │ │ │ +
91 else Ab.transposeMultiplyAdd(alpha, Ad, g);
│ │ │ │ +
92
│ │ │ │ +
93 // check for convergence
│ │ │ │ +
94 double new_gamma = dot(g, g);
│ │ │ │ +
95
│ │ │ │ +
96 if (parameters_.verbosity() != ConjugateGradientParameters::SILENT)
│ │ │ │ +
97 std::cout << "iteration " << k << ": alpha = " << alpha
│ │ │ │ +
98 << ", dotg = " << new_gamma
│ │ │ │ +
99 << std::endl;
│ │ │ │ +
100
│ │ │ │ +
101 if (new_gamma < threshold) return true;
│ │ │ │ +
102
│ │ │ │ +
103 // calculate new search direction
│ │ │ │ +
104 if (steepest) d = g;
│ │ │ │ +
105 else {
│ │ │ │ +
106 double beta = new_gamma / gamma;
│ │ │ │ +
107 // d = g + d*beta;
│ │ │ │ +
108 d *= beta;
│ │ │ │ +
109 d += 1.0 * g;
│ │ │ │ +
110 }
│ │ │ │ +
111
│ │ │ │ +
112 gamma = new_gamma;
│ │ │ │ +
113
│ │ │ │ +
114 // In-place recalculation Ad <- A*d to avoid re-allocating Ad
│ │ │ │ +
115 Ab.multiplyInPlace(d, Ad);
│ │ │ │ +
116 return false;
│ │ │ │ +
117 }
│ │ │ │ +
118
│ │ │ │ +
119 }; // CGState Class
│ │ │ │ +
│ │ │ │ +
120
│ │ │ │ +
121 /* ************************************************************************* */
│ │ │ │ +
122 // conjugate gradient method.
│ │ │ │ +
123 // S: linear system, V: step vector, E: errors
│ │ │ │ +
124 template<class S, class V, class E>
│ │ │ │ +
│ │ │ │ +
125 V conjugateGradients(const S& Ab, V x, const ConjugateGradientParameters &parameters, bool steepest) {
│ │ │ │ +
126
│ │ │ │ +
127 CGState<S, V, E> state(Ab, x, parameters, steepest);
│ │ │ │ +
128
│ │ │ │ +
129 if (parameters.verbosity() != ConjugateGradientParameters::SILENT)
│ │ │ │ +
130 std::cout << "CG: epsilon = " << parameters.epsilon()
│ │ │ │ +
131 << ", maxIterations = " << parameters.maxIterations()
│ │ │ │ +
132 << ", ||g0||^2 = " << state.gamma
│ │ │ │ +
133 << ", threshold = " << state.threshold
│ │ │ │ +
134 << std::endl;
│ │ │ │ +
135
│ │ │ │ +
136 if ( state.gamma < state.threshold ) {
│ │ │ │ +
137 if (parameters.verbosity() != ConjugateGradientParameters::SILENT)
│ │ │ │ +
138 std::cout << "||g0||^2 < threshold, exiting immediately !" << std::endl;
│ │ │ │ +
139
│ │ │ │ +
140 return x;
│ │ │ │ +
141 }
│ │ │ │ +
142
│ │ │ │ +
143 // loop maxIterations times
│ │ │ │ +
144 while (!state.step(Ab, x)) {}
│ │ │ │ +
145 return x;
│ │ │ │ +
146 }
│ │ │ │ +
│ │ │ │ +
147/* ************************************************************************* */
│ │ │ │ +
148
│ │ │ │ +
149} // namespace gtsam
│ │ │ │ +
ConjugateGradientSolver.h
Implementation of Conjugate Gradient solver for a linear system.
│ │ │ │ +
iterative.h
Iterative methods, implementation.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::JunctionTree
A JunctionTree is a cluster tree, a set of variable clusters with factors, arranged in a tree,...
Definition JunctionTree.h:50
│ │ │ │ -
gtsam::GaussianEliminationTree
Definition GaussianEliminationTree.h:29
│ │ │ │ -
gtsam::GaussianJunctionTree
A junction tree specialized to Gaussian factors, i.e., it is a cluster tree with Gaussian factors sto...
Definition GaussianJunctionTree.h:39
│ │ │ │ -
gtsam::GaussianJunctionTree::This
GaussianJunctionTree This
This class.
Definition GaussianJunctionTree.h:42
│ │ │ │ -
gtsam::GaussianJunctionTree::Base
JunctionTree< GaussianBayesTree, GaussianFactorGraph > Base
Base class.
Definition GaussianJunctionTree.h:41
│ │ │ │ -
gtsam::GaussianJunctionTree::shared_ptr
boost::shared_ptr< This > shared_ptr
Shared pointer to this class.
Definition GaussianJunctionTree.h:43
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::conjugateGradients
V conjugateGradients(const S &Ab, V x, const ConjugateGradientParameters &parameters, bool steepest)
Method of conjugate gradients (CG) template "System" class S needs gradient(S,v), e=S*v,...
Definition iterative-inl.h:125
│ │ │ │ +
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
│ │ │ │ +
gtsam::ConjugateGradientParameters
parameters for the conjugate gradient method
Definition ConjugateGradientSolver.h:29
│ │ │ │ +
gtsam::CGState
Definition iterative-inl.h:30
│ │ │ │ +
gtsam::CGState::steepest
bool steepest
flag to indicate we are doing steepest descent
Definition iterative-inl.h:36
│ │ │ │ +
gtsam::CGState::threshold
double threshold
gamma (squared L2 norm of g) and convergence threshold
Definition iterative-inl.h:38
│ │ │ │ +
gtsam::CGState::k
int k
iteration
Definition iterative-inl.h:35
│ │ │ │ +
gtsam::CGState::d
V d
gradient g and search direction d for CG
Definition iterative-inl.h:37
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GaussianJunctionTree.h │ │ │ │ │ +iterative-inl.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ @@ -16,62 +16,191 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 19#pragma once │ │ │ │ │ 20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_J_u_n_c_t_i_o_n_T_r_e_e_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_i_t_e_r_a_t_i_v_e_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_S_o_l_v_e_r_._h> │ │ │ │ │ +23#include │ │ │ │ │ 24 │ │ │ │ │ 25namespace _g_t_s_a_m { │ │ │ │ │ 26 │ │ │ │ │ -27 // Forward declarations │ │ │ │ │ -28 class GaussianEliminationTree; │ │ │ │ │ -29 │ │ │ │ │ -_3_8 class GTSAM_EXPORT _G_a_u_s_s_i_a_n_J_u_n_c_t_i_o_n_T_r_e_e : │ │ │ │ │ -39 public _J_u_n_c_t_i_o_n_T_r_e_e { │ │ │ │ │ -40 public: │ │ │ │ │ -_4_1 typedef _J_u_n_c_t_i_o_n_T_r_e_e_<_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_,_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_> _B_a_s_e; │ │ │ │ │ -_4_2 typedef _G_a_u_s_s_i_a_n_J_u_n_c_t_i_o_n_T_r_e_e _T_h_i_s; │ │ │ │ │ -_4_3 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -44 │ │ │ │ │ -53 _G_a_u_s_s_i_a_n_J_u_n_c_t_i_o_n_T_r_e_e(const _G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e& eliminationTree); │ │ │ │ │ -54 }; │ │ │ │ │ -55 │ │ │ │ │ -56} │ │ │ │ │ -_J_u_n_c_t_i_o_n_T_r_e_e_._h │ │ │ │ │ -The junction tree. │ │ │ │ │ -_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ -Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ │ -_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ +27 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +28 // state for CG method │ │ │ │ │ +29 template │ │ │ │ │ +_3_0 struct _C_G_S_t_a_t_e { │ │ │ │ │ +31 │ │ │ │ │ +32 typedef _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s _P_a_r_a_m_e_t_e_r_s; │ │ │ │ │ +33 const _P_a_r_a_m_e_t_e_r_s ¶meters_; │ │ │ │ │ +34 │ │ │ │ │ +_3_5 int _k; │ │ │ │ │ +_3_6 bool _s_t_e_e_p_e_s_t; │ │ │ │ │ +_3_7 V g, _d; │ │ │ │ │ +_3_8 double gamma, _t_h_r_e_s_h_o_l_d; │ │ │ │ │ +39 E Ad; │ │ │ │ │ +40 │ │ │ │ │ +41 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +42 // Constructor │ │ │ │ │ +43 _C_G_S_t_a_t_e(const S& Ab, const V& x, const _P_a_r_a_m_e_t_e_r_s ¶meters, bool steep): │ │ │ │ │ +44 parameters_(parameters),_k(0),_s_t_e_e_p_e_s_t(steep) { │ │ │ │ │ +45 │ │ │ │ │ +46 // Start with g0 = A'*(A*x0-b), d0 = - g0 │ │ │ │ │ +47 // i.e., first step is in direction of negative gradient │ │ │ │ │ +48 g = Ab.gradient(x); │ │ │ │ │ +49 _d = g; // instead of negating gradient, alpha will be negated │ │ │ │ │ +50 │ │ │ │ │ +51 // init gamma and calculate threshold │ │ │ │ │ +52 gamma = _d_o_t(g,g); │ │ │ │ │ +53 _t_h_r_e_s_h_o_l_d = std::max(parameters_.epsilon_abs(), parameters_.epsilon() * │ │ │ │ │ +parameters_.epsilon() * gamma); │ │ │ │ │ +54 │ │ │ │ │ +55 // Allocate and calculate A*d for first iteration │ │ │ │ │ +56 if (gamma > parameters_.epsilon_abs()) Ad = Ab * _d; │ │ │ │ │ +57 } │ │ │ │ │ +58 │ │ │ │ │ +59 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +60 // print │ │ │ │ │ +61 void print(const V& x) { │ │ │ │ │ +62 std::cout << "iteration = " << _k << std::endl; │ │ │ │ │ +63 _g_t_s_a_m_:_:_p_r_i_n_t(x,"x"); │ │ │ │ │ +64 _g_t_s_a_m_:_:_p_r_i_n_t(g, "g"); │ │ │ │ │ +65 std::cout << "dotg = " << gamma << std::endl; │ │ │ │ │ +66 _g_t_s_a_m_:_:_p_r_i_n_t(_d, "d"); │ │ │ │ │ +67 _g_t_s_a_m_:_:_p_r_i_n_t(Ad, "Ad"); │ │ │ │ │ +68 } │ │ │ │ │ +69 │ │ │ │ │ +70 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +71 // step the solution │ │ │ │ │ +72 double takeOptimalStep(V& x) { │ │ │ │ │ +73 // TODO: can we use gamma instead of dot(d,g) ????? Answer not trivial │ │ │ │ │ +74 double alpha = -_d_o_t(_d, g) / _d_o_t(Ad, Ad); // calculate optimal step-size │ │ │ │ │ +75 x += alpha * _d; // do step in new search direction, x += alpha*d │ │ │ │ │ +76 return alpha; │ │ │ │ │ +77 } │ │ │ │ │ +78 │ │ │ │ │ +79 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +80 // take a step, return true if converged │ │ │ │ │ +81 bool step(const S& Ab, V& x) { │ │ │ │ │ +82 │ │ │ │ │ +83 if ((++_k) >= ((int)parameters_.maxIterations())) return true; │ │ │ │ │ +84 │ │ │ │ │ +85 //----------------------------------> │ │ │ │ │ +86 double alpha = takeOptimalStep(x); │ │ │ │ │ +87 │ │ │ │ │ +88 // update gradient (or re-calculate at reset time) │ │ │ │ │ +89 if (_k % parameters_.reset() == 0) g = Ab.gradient(x); │ │ │ │ │ +90 // axpy(alpha, Ab ^ Ad, g); // g += alpha*(Ab^Ad) │ │ │ │ │ +91 else Ab.transposeMultiplyAdd(alpha, Ad, g); │ │ │ │ │ +92 │ │ │ │ │ +93 // check for convergence │ │ │ │ │ +94 double new_gamma = _d_o_t(g, g); │ │ │ │ │ +95 │ │ │ │ │ +96 if (parameters_.verbosity() != ConjugateGradientParameters::SILENT) │ │ │ │ │ +97 std::cout << "iteration " << _k << ": alpha = " << alpha │ │ │ │ │ +98 << ", dotg = " << new_gamma │ │ │ │ │ +99 << std::endl; │ │ │ │ │ +100 │ │ │ │ │ +101 if (new_gamma < _t_h_r_e_s_h_o_l_d) return true; │ │ │ │ │ +102 │ │ │ │ │ +103 // calculate new search direction │ │ │ │ │ +104 if (_s_t_e_e_p_e_s_t) _d = g; │ │ │ │ │ +105 else { │ │ │ │ │ +106 double beta = new_gamma / gamma; │ │ │ │ │ +107 // d = g + d*beta; │ │ │ │ │ +108 _d *= beta; │ │ │ │ │ +109 _d += 1.0 * g; │ │ │ │ │ +110 } │ │ │ │ │ +111 │ │ │ │ │ +112 gamma = new_gamma; │ │ │ │ │ +113 │ │ │ │ │ +114 // In-place recalculation Ad <- A*d to avoid re-allocating Ad │ │ │ │ │ +115 Ab.multiplyInPlace(_d, Ad); │ │ │ │ │ +116 return false; │ │ │ │ │ +117 } │ │ │ │ │ +118 │ │ │ │ │ +119 }; // CGState Class │ │ │ │ │ +120 │ │ │ │ │ +121 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +122 // conjugate gradient method. │ │ │ │ │ +123 // S: linear system, V: step vector, E: errors │ │ │ │ │ +124 template │ │ │ │ │ +_1_2_5 V _c_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_s(const S& Ab, V x, const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ +¶meters, bool steepest) { │ │ │ │ │ +126 │ │ │ │ │ +127 _C_G_S_t_a_t_e_<_S_,_ _V_,_ _E_> state(Ab, x, parameters, steepest); │ │ │ │ │ +128 │ │ │ │ │ +129 if (parameters.verbosity() != ConjugateGradientParameters::SILENT) │ │ │ │ │ +130 std::cout << "CG: epsilon = " << parameters.epsilon() │ │ │ │ │ +131 << ", maxIterations = " << parameters.maxIterations() │ │ │ │ │ +132 << ", ||g0||^2 = " << state.gamma │ │ │ │ │ +133 << ", threshold = " << state._t_h_r_e_s_h_o_l_d │ │ │ │ │ +134 << std::endl; │ │ │ │ │ +135 │ │ │ │ │ +136 if ( state.gamma < state._t_h_r_e_s_h_o_l_d ) { │ │ │ │ │ +137 if (parameters.verbosity() != ConjugateGradientParameters::SILENT) │ │ │ │ │ +138 std::cout << "||g0||^2 < threshold, exiting immediately !" << std::endl; │ │ │ │ │ +139 │ │ │ │ │ +140 return x; │ │ │ │ │ +141 } │ │ │ │ │ +142 │ │ │ │ │ +143 // loop maxIterations times │ │ │ │ │ +144 while (!state.step(Ab, x)) {} │ │ │ │ │ +145 return x; │ │ │ │ │ +146 } │ │ │ │ │ +147/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +148 │ │ │ │ │ +149} // namespace gtsam │ │ │ │ │ +_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_S_o_l_v_e_r_._h │ │ │ │ │ +Implementation of Conjugate Gradient solver for a linear system. │ │ │ │ │ +_i_t_e_r_a_t_i_v_e_._h │ │ │ │ │ +Iterative methods, implementation. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ -A JunctionTree is a cluster tree, a set of variable clusters with factors, │ │ │ │ │ -arranged in a tree,... │ │ │ │ │ -DDeeffiinniittiioonn JunctionTree.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ -DDeeffiinniittiioonn GaussianEliminationTree.h:29 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ -A junction tree specialized to Gaussian factors, i.e., it is a cluster tree │ │ │ │ │ -with Gaussian factors sto... │ │ │ │ │ -DDeeffiinniittiioonn GaussianJunctionTree.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_J_u_n_c_t_i_o_n_T_r_e_e_:_:_T_h_i_s │ │ │ │ │ -GaussianJunctionTree This │ │ │ │ │ -This class. │ │ │ │ │ -DDeeffiinniittiioonn GaussianJunctionTree.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_J_u_n_c_t_i_o_n_T_r_e_e_:_:_B_a_s_e │ │ │ │ │ -JunctionTree< GaussianBayesTree, GaussianFactorGraph > Base │ │ │ │ │ -Base class. │ │ │ │ │ -DDeeffiinniittiioonn GaussianJunctionTree.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_J_u_n_c_t_i_o_n_T_r_e_e_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -Shared pointer to this class. │ │ │ │ │ -DDeeffiinniittiioonn GaussianJunctionTree.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_c_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_s │ │ │ │ │ +V conjugateGradients(const S &Ab, V x, const ConjugateGradientParameters │ │ │ │ │ +¶meters, bool steepest) │ │ │ │ │ +Method of conjugate gradients (CG) template "System" class S needs gradient │ │ │ │ │ +(S,v), e=S*v,... │ │ │ │ │ +DDeeffiinniittiioonn iterative-inl.h:125 │ │ │ │ │ +_g_t_s_a_m_:_:_d_o_t │ │ │ │ │ +double dot(const V1 &a, const V2 &b) │ │ │ │ │ +Dot product. │ │ │ │ │ +DDeeffiinniittiioonn Vector.h:195 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ +parameters for the conjugate gradient method │ │ │ │ │ +DDeeffiinniittiioonn ConjugateGradientSolver.h:29 │ │ │ │ │ +_g_t_s_a_m_:_:_C_G_S_t_a_t_e │ │ │ │ │ +DDeeffiinniittiioonn iterative-inl.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_C_G_S_t_a_t_e_:_:_s_t_e_e_p_e_s_t │ │ │ │ │ +bool steepest │ │ │ │ │ +flag to indicate we are doing steepest descent │ │ │ │ │ +DDeeffiinniittiioonn iterative-inl.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_C_G_S_t_a_t_e_:_:_t_h_r_e_s_h_o_l_d │ │ │ │ │ +double threshold │ │ │ │ │ +gamma (squared L2 norm of g) and convergence threshold │ │ │ │ │ +DDeeffiinniittiioonn iterative-inl.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_C_G_S_t_a_t_e_:_:_k │ │ │ │ │ +int k │ │ │ │ │ +iteration │ │ │ │ │ +DDeeffiinniittiioonn iterative-inl.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_C_G_S_t_a_t_e_:_:_d │ │ │ │ │ +V d │ │ │ │ │ +gradient g and search direction d for CG │ │ │ │ │ +DDeeffiinniittiioonn iterative-inl.h:37 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_J_u_n_c_t_i_o_n_T_r_e_e_._h │ │ │ │ │ + * _i_t_e_r_a_t_i_v_e_-_i_n_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00881.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/iterative.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/KalmanFilter.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,74 +96,52 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ -Functions
│ │ │ │ -
iterative.h File Reference
│ │ │ │ +Macros
│ │ │ │ +
KalmanFilter.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Iterative methods, implementation. │ │ │ │ +

Simple linear Kalman filter. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::System
 Helper class encapsulating the combined system |Ax-b_|^2 Needed to run Conjugate Gradients on matrices. More...
class  gtsam::KalmanFilter
 Kalman Filter class. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ -Functions

template<class S , class V , class E >
V gtsam::conjugateGradients (const S &Ab, V x, const ConjugateGradientParameters &parameters, bool steepest=false)
 Method of conjugate gradients (CG) template "System" class S needs gradient(S,v), e=S*v, v=S^e "Vector" class V needs dot(v,v), -v, v+v, s*v "Vector" class E needs dot(v,v)
 
│ │ │ │ -GTSAM_EXPORT Vector gtsam::steepestDescent (const System &Ab, const Vector &x, const IterativeOptimizationParameters &parameters)
 Method of steepest gradients, System version.
 
│ │ │ │ -Vector gtsam::conjugateGradientDescent (const System &Ab, const Vector &x, const ConjugateGradientParameters &parameters)
 Method of conjugate gradients (CG), System version.
 
Vector gtsam::steepestDescent (const Matrix &A, const Vector &b, const Vector &x, const ConjugateGradientParameters &parameters)
 convenience calls using matrices, will create System class internally:
 
│ │ │ │ -Vector gtsam::conjugateGradientDescent (const Matrix &A, const Vector &b, const Vector &x, const ConjugateGradientParameters &parameters)
 Method of conjugate gradients (CG), Matrix version.
 
│ │ │ │ -VectorValues gtsam::steepestDescent (const GaussianFactorGraph &fg, const VectorValues &x, const ConjugateGradientParameters &parameters)
 Method of steepest gradients, Gaussian Factor Graph version.
 
│ │ │ │ -VectorValues gtsam::conjugateGradientDescent (const GaussianFactorGraph &fg, const VectorValues &x, const ConjugateGradientParameters &parameters)
 Method of conjugate gradients (CG), Gaussian Factor Graph version.
 

│ │ │ │ +Macros

│ │ │ │ +#define KALMANFILTER_DEFAULT_FACTORIZATION   QR
 
│ │ │ │

Detailed Description

│ │ │ │ -

Iterative methods, implementation.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
Date
Dec 28, 2009
│ │ │ │ +

Simple linear Kalman filter.

│ │ │ │ +

Implemented using factor graphs, i.e., does Cholesky-based SRIF, really.

Date
Sep 3, 2011
│ │ │ │ +
Author
Stephen Williams
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,68 +1,32 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -iterative.h File Reference │ │ │ │ │ -Iterative methods, implementation. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s │ │ │ │ │ +KalmanFilter.h File Reference │ │ │ │ │ +Simple linear Kalman filter. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_S_y_s_t_e_m │ │ │ │ │ -  Helper class encapsulating the combined system |Ax-b_|^2 Needed to run │ │ │ │ │ - Conjugate Gradients on matrices. _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_K_a_l_m_a_n_F_i_l_t_e_r │ │ │ │ │ +  Kalman Filter class. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ - V  _g_t_s_a_m_:_:_c_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_s (const S &Ab, V x, const │ │ │ │ │ - _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s ¶meters, bool │ │ │ │ │ - steepest=false) │ │ │ │ │ - Method of conjugate gradients (CG) template "System" class │ │ │ │ │ -  S needs gradient(S,v), e=S*v, v=S^e "Vector" class V needs │ │ │ │ │ - dot(v,v), -v, v+v, s*v "Vector" class E needs dot(v,v) │ │ │ │ │ -  │ │ │ │ │ -GTSAM_EXPORT Vector  ggttssaamm::::sstteeeeppeessttDDeesscceenntt (const _S_y_s_t_e_m &Ab, const Vector &x, │ │ │ │ │ - const _I_t_e_r_a_t_i_v_e_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s ¶meters) │ │ │ │ │ -  Method of steepest gradients, _S_y_s_t_e_m version. │ │ │ │ │ -  │ │ │ │ │ - Vector  ggttssaamm::::ccoonnjjuuggaatteeGGrraaddiieennttDDeesscceenntt (const _S_y_s_t_e_m &Ab, const │ │ │ │ │ - Vector &x, const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s ¶meters) │ │ │ │ │ -  Method of conjugate gradients (CG), _S_y_s_t_e_m version. │ │ │ │ │ -  │ │ │ │ │ - Vector  _g_t_s_a_m_:_:_s_t_e_e_p_e_s_t_D_e_s_c_e_n_t (const Matrix &A, const Vector &b, │ │ │ │ │ - const Vector &x, const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ - ¶meters) │ │ │ │ │ -  convenience calls using matrices, will create _S_y_s_t_e_m class │ │ │ │ │ - internally: │ │ │ │ │ -  │ │ │ │ │ - Vector  ggttssaamm::::ccoonnjjuuggaatteeGGrraaddiieennttDDeesscceenntt (const Matrix &A, const │ │ │ │ │ - Vector &b, const Vector &x, const │ │ │ │ │ - _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s ¶meters) │ │ │ │ │ -  Method of conjugate gradients (CG), Matrix version. │ │ │ │ │ -  │ │ │ │ │ - _V_e_c_t_o_r_V_a_l_u_e_s  ggttssaamm::::sstteeeeppeessttDDeesscceenntt (const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &fg, │ │ │ │ │ - const _V_e_c_t_o_r_V_a_l_u_e_s &x, const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ - ¶meters) │ │ │ │ │ -  Method of steepest gradients, Gaussian _F_a_c_t_o_r Graph │ │ │ │ │ - version. │ │ │ │ │ -  │ │ │ │ │ - _V_e_c_t_o_r_V_a_l_u_e_s  ggttssaamm::::ccoonnjjuuggaatteeGGrraaddiieennttDDeesscceenntt (const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ - &fg, const _V_e_c_t_o_r_V_a_l_u_e_s &x, const │ │ │ │ │ - _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s ¶meters) │ │ │ │ │ -  Method of conjugate gradients (CG), Gaussian _F_a_c_t_o_r Graph │ │ │ │ │ - version. │ │ │ │ │ +MMaaccrrooss │ │ │ │ │ +#define  KKAALLMMAANNFFIILLTTEERR__DDEEFFAAUULLTT__FFAACCTTOORRIIZZAATTIIOONN   QR │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Iterative methods, implementation. │ │ │ │ │ +Simple linear Kalman filter. │ │ │ │ │ +Implemented using factor graphs, i.e., does Cholesky-based SRIF, really. │ │ │ │ │ + Date │ │ │ │ │ + Sep 3, 2011 │ │ │ │ │ Author │ │ │ │ │ + Stephen Williams │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - Dec 28, 2009 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _i_t_e_r_a_t_i_v_e_._h │ │ │ │ │ + * _K_a_l_m_a_n_F_i_l_t_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00881.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,10 +1,3 @@ │ │ │ │ │ var a00881 = [ │ │ │ │ │ - ["gtsam::System", "a03824.html", "a03824"], │ │ │ │ │ - ["conjugateGradientDescent", "a00881.html#aa84114ad1593684ad739088a2898afdb", null], │ │ │ │ │ - ["conjugateGradientDescent", "a00881.html#af55440f741d8b2f706101f7a79ba7111", null], │ │ │ │ │ - ["conjugateGradientDescent", "a00881.html#ad2b587fde5d35a27d6c88feb4432e785", null], │ │ │ │ │ - ["conjugateGradients", "a00881.html#a6516ea957c3a22ddf429cefef1fe9486", null], │ │ │ │ │ - ["steepestDescent", "a00881.html#a81b7af7638d28555c3f404318cc67b22", null], │ │ │ │ │ - ["steepestDescent", "a00881.html#aed4b2ca704ddd6530ea70122f883ac88", null], │ │ │ │ │ - ["steepestDescent", "a00881.html#a37ca85391e3bc671c71b68bc324296f0", null] │ │ │ │ │ + ["gtsam::KalmanFilter", "a03852.html", "a03852"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00881_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/iterative.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/KalmanFilter.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,152 +98,135 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
iterative.h
│ │ │ │ +
KalmanFilter.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/base/Matrix.h>
│ │ │ │ -
22#include <gtsam/linear/VectorValues.h>
│ │ │ │ -
23#include <gtsam/linear/ConjugateGradientSolver.h>
│ │ │ │ -
24
│ │ │ │ -
25namespace gtsam {
│ │ │ │ -
26
│ │ │ │ -
36 template<class S, class V, class E>
│ │ │ │ -
37 V conjugateGradients(const S& Ab, V x,
│ │ │ │ -
38 const ConjugateGradientParameters &parameters, bool steepest = false);
│ │ │ │ -
39
│ │ │ │ -
│ │ │ │ -
44 class GTSAM_EXPORT System {
│ │ │ │ -
45
│ │ │ │ -
46 private:
│ │ │ │ -
47 const Matrix& A_;
│ │ │ │ -
48 const Vector& b_;
│ │ │ │ -
49
│ │ │ │ -
50 public:
│ │ │ │ -
51
│ │ │ │ -
52 System(const Matrix& A, const Vector& b) :
│ │ │ │ -
53 A_(A), b_(b) {
│ │ │ │ -
54 }
│ │ │ │ -
55
│ │ │ │ -
57 const Matrix& A() const { return A_; }
│ │ │ │ -
58
│ │ │ │ -
60 const Vector& b() const { return b_; }
│ │ │ │ -
61
│ │ │ │ -
│ │ │ │ -
63 Vector operator^(const Vector& e) const {
│ │ │ │ -
64 return A_ ^ e;
│ │ │ │ -
65 }
│ │ │ │ -
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/linear/GaussianDensity.h>
│ │ │ │ +
23#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ +
24#include <gtsam/linear/NoiseModel.h>
│ │ │ │ +
25
│ │ │ │ +
26#ifndef KALMANFILTER_DEFAULT_FACTORIZATION
│ │ │ │ +
27#define KALMANFILTER_DEFAULT_FACTORIZATION QR
│ │ │ │ +
28#endif
│ │ │ │ +
29
│ │ │ │ +
30namespace gtsam {
│ │ │ │ +
31
│ │ │ │ +
│ │ │ │ +
41class GTSAM_EXPORT KalmanFilter {
│ │ │ │ +
42
│ │ │ │ +
43public:
│ │ │ │ +
44
│ │ │ │ +
│ │ │ │ +
49 enum Factorization {
│ │ │ │ +
50 QR, CHOLESKY
│ │ │ │ +
51 };
│ │ │ │ +
│ │ │ │ +
52
│ │ │ │ +
56 typedef GaussianDensity::shared_ptr State;
│ │ │ │ +
57
│ │ │ │ +
58private:
│ │ │ │ +
59
│ │ │ │ +
60 const size_t n_;
│ │ │ │ +
61 const Matrix I_;
│ │ │ │ +
62 const GaussianFactorGraph::Eliminate function_;
│ │ │ │ +
64 State solve(const GaussianFactorGraph& factorGraph) const;
│ │ │ │ +
65 State fuse(const State& p, GaussianFactor::shared_ptr newFactor) const;
│ │ │ │
66
│ │ │ │ -
70 void print (const std::string& s = "System") const;
│ │ │ │ -
71
│ │ │ │ -
│ │ │ │ -
73 Vector gradient(const Vector& x) const {
│ │ │ │ -
74 return A() ^ (A() * x - b());
│ │ │ │ -
75 }
│ │ │ │ -
│ │ │ │ +
67public:
│ │ │ │ +
68
│ │ │ │ +
69 // Constructor
│ │ │ │ +
70 KalmanFilter(size_t n, Factorization method =
│ │ │ │ +
71 KALMANFILTER_DEFAULT_FACTORIZATION) :
│ │ │ │ +
72 n_(n), I_(Matrix::Identity(n_, n_)), function_(
│ │ │ │ +
73 method == QR ? GaussianFactorGraph::Eliminate(EliminateQR) :
│ │ │ │ +
74 GaussianFactorGraph::Eliminate(EliminateCholesky)) {
│ │ │ │ +
75 }
│ │ │ │
76
│ │ │ │ -
│ │ │ │ -
78 Vector operator*(const Vector& x) const {
│ │ │ │ -
79 return A() * x;
│ │ │ │ -
80 }
│ │ │ │ -
│ │ │ │ -
81
│ │ │ │ -
│ │ │ │ -
83 void multiplyInPlace(const Vector& x, Vector& e) const {
│ │ │ │ -
84 e = A() * x;
│ │ │ │ -
85 }
│ │ │ │ -
│ │ │ │ -
86
│ │ │ │ -
│ │ │ │ -
88 void transposeMultiplyAdd(double alpha, const Vector& e, Vector& x) const {
│ │ │ │ -
89 x += alpha * A().transpose() * e;
│ │ │ │ -
90 }
│ │ │ │ -
│ │ │ │ -
91 };
│ │ │ │ -
│ │ │ │ -
92
│ │ │ │ -
96 GTSAM_EXPORT Vector steepestDescent(
│ │ │ │ -
97 const System& Ab,
│ │ │ │ -
98 const Vector& x,
│ │ │ │ -
99 const IterativeOptimizationParameters & parameters);
│ │ │ │ -
100
│ │ │ │ -
104 GTSAM_EXPORT Vector conjugateGradientDescent(
│ │ │ │ -
105 const System& Ab,
│ │ │ │ -
106 const Vector& x,
│ │ │ │ -
107 const ConjugateGradientParameters & parameters);
│ │ │ │ -
108
│ │ │ │ -
114 GTSAM_EXPORT Vector steepestDescent(
│ │ │ │ -
115 const Matrix& A,
│ │ │ │ -
116 const Vector& b,
│ │ │ │ -
117 const Vector& x,
│ │ │ │ -
118 const ConjugateGradientParameters & parameters);
│ │ │ │ -
119
│ │ │ │ -
123 GTSAM_EXPORT Vector conjugateGradientDescent(
│ │ │ │ -
124 const Matrix& A,
│ │ │ │ -
125 const Vector& b,
│ │ │ │ -
126 const Vector& x,
│ │ │ │ -
127 const ConjugateGradientParameters & parameters);
│ │ │ │ -
128
│ │ │ │ -
132 GTSAM_EXPORT VectorValues steepestDescent(
│ │ │ │ -
133 const GaussianFactorGraph& fg,
│ │ │ │ -
134 const VectorValues& x,
│ │ │ │ -
135 const ConjugateGradientParameters & parameters);
│ │ │ │ -
136
│ │ │ │ -
140 GTSAM_EXPORT VectorValues conjugateGradientDescent(
│ │ │ │ -
141 const GaussianFactorGraph& fg,
│ │ │ │ -
142 const VectorValues& x,
│ │ │ │ -
143 const ConjugateGradientParameters & parameters);
│ │ │ │ -
144
│ │ │ │ -
145
│ │ │ │ -
146} // namespace gtsam
│ │ │ │ +
83 State init(const Vector& x0, const SharedDiagonal& P0) const;
│ │ │ │ +
84
│ │ │ │ +
86 State init(const Vector& x0, const Matrix& P0) const;
│ │ │ │ +
87
│ │ │ │ +
89 void print(const std::string& s = "") const;
│ │ │ │ +
90
│ │ │ │ +
│ │ │ │ +
92 static Key step(const State& p) {
│ │ │ │ +
93 return p->firstFrontalKey();
│ │ │ │ +
94 }
│ │ │ │ +
│ │ │ │ +
95
│ │ │ │ +
104 State predict(const State& p, const Matrix& F, const Matrix& B,
│ │ │ │ +
105 const Vector& u, const SharedDiagonal& modelQ) const;
│ │ │ │ +
106
│ │ │ │ +
107 /*
│ │ │ │ +
108 * Version of predict with full covariance
│ │ │ │ +
109 * Q is normally derived as G*w*G^T where w models uncertainty of some
│ │ │ │ +
110 * physical property, such as velocity or acceleration, and G is derived from physics.
│ │ │ │ +
111 * This version allows more realistic models than a diagonal covariance matrix.
│ │ │ │ +
112 */
│ │ │ │ +
113 State predictQ(const State& p, const Matrix& F, const Matrix& B,
│ │ │ │ +
114 const Vector& u, const Matrix& Q) const;
│ │ │ │ +
115
│ │ │ │ +
124 State predict2(const State& p, const Matrix& A0, const Matrix& A1,
│ │ │ │ +
125 const Vector& b, const SharedDiagonal& model) const;
│ │ │ │ +
126
│ │ │ │ +
135 State update(const State& p, const Matrix& H, const Vector& z,
│ │ │ │ +
136 const SharedDiagonal& model) const;
│ │ │ │ +
137
│ │ │ │ +
138 /*
│ │ │ │ +
139 * Version of update with full covariance
│ │ │ │ +
140 * Q is normally derived as G*w*G^T where w models uncertainty of some
│ │ │ │ +
141 * physical property, such as velocity or acceleration, and G is derived from physics.
│ │ │ │ +
142 * This version allows more realistic models than a diagonal covariance matrix.
│ │ │ │ +
143 */
│ │ │ │ +
144 State updateQ(const State& p, const Matrix& H, const Vector& z,
│ │ │ │ +
145 const Matrix& Q) const;
│ │ │ │ +
146};
│ │ │ │ +
│ │ │ │
147
│ │ │ │ -
148#include <gtsam/linear/iterative-inl.h>
│ │ │ │ +
148} // \namespace gtsam
│ │ │ │
149
│ │ │ │ -
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ -
iterative-inl.h
Iterative methods, template implementation.
│ │ │ │ -
ConjugateGradientSolver.h
Implementation of Conjugate Gradient solver for a linear system.
│ │ │ │ -
VectorValues.h
Factor Graph Values.
│ │ │ │ +
150/* ************************************************************************* */
│ │ │ │ +
151
│ │ │ │ +
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ +
GaussianDensity.h
A Gaussian Density.
│ │ │ │ + │ │ │ │ +
gtsam::EliminateCholesky
std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< HessianFactor > > EliminateCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
Densely partially eliminate with Cholesky factorization.
Definition HessianFactor.cpp:525
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::conjugateGradients
V conjugateGradients(const S &Ab, V x, const ConjugateGradientParameters &parameters, bool steepest)
Method of conjugate gradients (CG) template "System" class S needs gradient(S,v), e=S*v,...
Definition iterative-inl.h:125
│ │ │ │ -
gtsam::conjugateGradientDescent
Vector conjugateGradientDescent(const System &Ab, const Vector &x, const ConjugateGradientParameters &parameters)
Method of conjugate gradients (CG), System version.
Definition iterative.cpp:45
│ │ │ │ -
gtsam::ConjugateGradientParameters
parameters for the conjugate gradient method
Definition ConjugateGradientSolver.h:29
│ │ │ │ +
gtsam::EliminateQR
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Multiply all factors and eliminate the given keys from the resulting factor using a QR variant that h...
Definition JacobianFactor.cpp:789
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::EliminateableFactorGraph< GaussianFactorGraph >::Eliminate
std::function< EliminationResult(const FactorGraphType &, const Ordering &)> Eliminate
The function type that does a single dense elimination step on a subgraph.
Definition EliminateableFactorGraph.h:89
│ │ │ │ +
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactor.h:42
│ │ │ │
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ -
gtsam::System
Helper class encapsulating the combined system |Ax-b_|^2 Needed to run Conjugate Gradients on matrice...
Definition iterative.h:44
│ │ │ │ -
gtsam::System::operator*
Vector operator*(const Vector &x) const
Apply operator A.
Definition iterative.h:78
│ │ │ │ -
gtsam::System::A
const Matrix & A() const
Access A matrix.
Definition iterative.h:57
│ │ │ │ -
gtsam::System::multiplyInPlace
void multiplyInPlace(const Vector &x, Vector &e) const
Apply operator A in place.
Definition iterative.h:83
│ │ │ │ -
gtsam::System::b
const Vector & b() const
Access b vector.
Definition iterative.h:60
│ │ │ │ -
gtsam::System::gradient
Vector gradient(const Vector &x) const
gradient of objective function 0.5*|Ax-b_|^2 at x = A_'*(Ax-b_)
Definition iterative.h:73
│ │ │ │ -
gtsam::System::transposeMultiplyAdd
void transposeMultiplyAdd(double alpha, const Vector &e, Vector &x) const
x += alpha* A'*e
Definition iterative.h:88
│ │ │ │ -
gtsam::System::operator^
Vector operator^(const Vector &e) const
Apply operator A'*e.
Definition iterative.h:63
│ │ │ │ -
gtsam::IterativeOptimizationParameters
parameters for iterative linear solvers
Definition IterativeSolver.h:44
│ │ │ │ -
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
gtsam::KalmanFilter
Kalman Filter class.
Definition KalmanFilter.h:41
│ │ │ │ +
gtsam::KalmanFilter::Factorization
Factorization
This Kalman filter is a Square-root Information filter The type below allows you to specify the facto...
Definition KalmanFilter.h:49
│ │ │ │ +
gtsam::KalmanFilter::step
static Key step(const State &p)
Return step index k, starts at 0, incremented at each predict.
Definition KalmanFilter.h:92
│ │ │ │ +
gtsam::KalmanFilter::State
GaussianDensity::shared_ptr State
The Kalman filter state is simply a GaussianDensity.
Definition KalmanFilter.h:56
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,183 +1,167 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -iterative.h │ │ │ │ │ +KalmanFilter.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_S_o_l_v_e_r_._h> │ │ │ │ │ -24 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ -26 │ │ │ │ │ -36 template │ │ │ │ │ -37 V _c_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_s(const S& Ab, V x, │ │ │ │ │ -38 const ConjugateGradientParameters ¶meters, bool steepest = false); │ │ │ │ │ -39 │ │ │ │ │ -_4_4 class GTSAM_EXPORT _S_y_s_t_e_m { │ │ │ │ │ -45 │ │ │ │ │ -46 private: │ │ │ │ │ -47 const Matrix& A_; │ │ │ │ │ -48 const Vector& b_; │ │ │ │ │ -49 │ │ │ │ │ -50 public: │ │ │ │ │ -51 │ │ │ │ │ -52 _S_y_s_t_e_m(const Matrix& A, const Vector& b) : │ │ │ │ │ -53 A_(A), b_(b) { │ │ │ │ │ -54 } │ │ │ │ │ -55 │ │ │ │ │ -_5_7 const Matrix& _A() const { return A_; } │ │ │ │ │ -58 │ │ │ │ │ -_6_0 const Vector& _b() const { return b_; } │ │ │ │ │ -61 │ │ │ │ │ -_6_3 Vector _o_p_e_r_a_t_o_r_^(const Vector& e) const { │ │ │ │ │ -64 return A_ ^ e; │ │ │ │ │ -65 } │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_D_e_n_s_i_t_y_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_N_o_i_s_e_M_o_d_e_l_._h> │ │ │ │ │ +25 │ │ │ │ │ +26#ifndef KALMANFILTER_DEFAULT_FACTORIZATION │ │ │ │ │ +27#define KALMANFILTER_DEFAULT_FACTORIZATION QR │ │ │ │ │ +28#endif │ │ │ │ │ +29 │ │ │ │ │ +30namespace _g_t_s_a_m { │ │ │ │ │ +31 │ │ │ │ │ +_4_1class GTSAM_EXPORT _K_a_l_m_a_n_F_i_l_t_e_r { │ │ │ │ │ +42 │ │ │ │ │ +43public: │ │ │ │ │ +44 │ │ │ │ │ +_4_9 enum _F_a_c_t_o_r_i_z_a_t_i_o_n { │ │ │ │ │ +50 QR, CHOLESKY │ │ │ │ │ +51 }; │ │ │ │ │ +52 │ │ │ │ │ +_5_6 typedef GaussianDensity::shared_ptr _S_t_a_t_e; │ │ │ │ │ +57 │ │ │ │ │ +58private: │ │ │ │ │ +59 │ │ │ │ │ +60 const size_t n_; │ │ │ │ │ +61 const Matrix I_; │ │ │ │ │ +62 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_E_l_i_m_i_n_a_t_e function_; │ │ │ │ │ +64 _S_t_a_t_e solve(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& factorGraph) const; │ │ │ │ │ +65 _S_t_a_t_e fuse(const _S_t_a_t_e& p, _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r newFactor) const; │ │ │ │ │ 66 │ │ │ │ │ -70 void _p_r_i_n_t (const std::string& s = "System") const; │ │ │ │ │ -71 │ │ │ │ │ -_7_3 Vector _g_r_a_d_i_e_n_t(const Vector& x) const { │ │ │ │ │ -74 return A() ^ (A() * x - b()); │ │ │ │ │ +67public: │ │ │ │ │ +68 │ │ │ │ │ +69 // Constructor │ │ │ │ │ +70 _K_a_l_m_a_n_F_i_l_t_e_r(size_t n, _F_a_c_t_o_r_i_z_a_t_i_o_n method = │ │ │ │ │ +71 KALMANFILTER_DEFAULT_FACTORIZATION) : │ │ │ │ │ +72 n_(n), I_(Matrix::Identity(n_, n_)), function_( │ │ │ │ │ +73 method == QR ? _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h::Eliminate(_E_l_i_m_i_n_a_t_e_Q_R) : │ │ │ │ │ +74 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h::Eliminate(_E_l_i_m_i_n_a_t_e_C_h_o_l_e_s_k_y)) { │ │ │ │ │ 75 } │ │ │ │ │ 76 │ │ │ │ │ -_7_8 Vector _o_p_e_r_a_t_o_r_*(const Vector& x) const { │ │ │ │ │ -79 return A() * x; │ │ │ │ │ -80 } │ │ │ │ │ -81 │ │ │ │ │ -_8_3 void _m_u_l_t_i_p_l_y_I_n_P_l_a_c_e(const Vector& x, Vector& e) const { │ │ │ │ │ -84 e = A() * x; │ │ │ │ │ -85 } │ │ │ │ │ -86 │ │ │ │ │ -_8_8 void _t_r_a_n_s_p_o_s_e_M_u_l_t_i_p_l_y_A_d_d(double alpha, const Vector& e, Vector& x) const { │ │ │ │ │ -89 x += alpha * A().transpose() * e; │ │ │ │ │ -90 } │ │ │ │ │ -91 }; │ │ │ │ │ -92 │ │ │ │ │ -_9_6 GTSAM_EXPORT Vector steepestDescent( │ │ │ │ │ -97 const _S_y_s_t_e_m& Ab, │ │ │ │ │ -98 const Vector& x, │ │ │ │ │ -99 const _I_t_e_r_a_t_i_v_e_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s & parameters); │ │ │ │ │ -100 │ │ │ │ │ -104 GTSAM_EXPORT Vector _c_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_D_e_s_c_e_n_t( │ │ │ │ │ -105 const _S_y_s_t_e_m& Ab, │ │ │ │ │ -106 const Vector& x, │ │ │ │ │ -107 const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s & parameters); │ │ │ │ │ -108 │ │ │ │ │ -114 GTSAM_EXPORT Vector steepestDescent( │ │ │ │ │ -115 const Matrix& A, │ │ │ │ │ -116 const Vector& b, │ │ │ │ │ -117 const Vector& x, │ │ │ │ │ -118 const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s & parameters); │ │ │ │ │ -119 │ │ │ │ │ -123 GTSAM_EXPORT Vector _c_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_D_e_s_c_e_n_t( │ │ │ │ │ -124 const Matrix& A, │ │ │ │ │ -125 const Vector& b, │ │ │ │ │ -126 const Vector& x, │ │ │ │ │ -127 const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s & parameters); │ │ │ │ │ -128 │ │ │ │ │ -132 GTSAM_EXPORT _V_e_c_t_o_r_V_a_l_u_e_s steepestDescent( │ │ │ │ │ -133 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& fg, │ │ │ │ │ -134 const _V_e_c_t_o_r_V_a_l_u_e_s& x, │ │ │ │ │ -135 const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s & parameters); │ │ │ │ │ -136 │ │ │ │ │ -140 GTSAM_EXPORT _V_e_c_t_o_r_V_a_l_u_e_s _c_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_D_e_s_c_e_n_t( │ │ │ │ │ -141 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& fg, │ │ │ │ │ -142 const _V_e_c_t_o_r_V_a_l_u_e_s& x, │ │ │ │ │ -143 const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s & parameters); │ │ │ │ │ -144 │ │ │ │ │ -145 │ │ │ │ │ -146} // namespace gtsam │ │ │ │ │ +83 State init(const Vector& x0, const SharedDiagonal& P0) const; │ │ │ │ │ +84 │ │ │ │ │ +86 State init(const Vector& x0, const Matrix& P0) const; │ │ │ │ │ +87 │ │ │ │ │ +89 void _p_r_i_n_t(const std::string& s = "") const; │ │ │ │ │ +90 │ │ │ │ │ +_9_2 static _K_e_y _s_t_e_p(const _S_t_a_t_e& p) { │ │ │ │ │ +93 return p->firstFrontalKey(); │ │ │ │ │ +94 } │ │ │ │ │ +95 │ │ │ │ │ +104 State predict(const State& p, const Matrix& F, const Matrix& B, │ │ │ │ │ +105 const Vector& u, const SharedDiagonal& modelQ) const; │ │ │ │ │ +106 │ │ │ │ │ +107 /* │ │ │ │ │ +108 * Version of predict with full covariance │ │ │ │ │ +109 * Q is normally derived as G*w*G^T where w models uncertainty of some │ │ │ │ │ +110 * physical property, such as velocity or acceleration, and G is derived │ │ │ │ │ +from physics. │ │ │ │ │ +111 * This version allows more realistic models than a diagonal covariance │ │ │ │ │ +matrix. │ │ │ │ │ +112 */ │ │ │ │ │ +113 State predictQ(const State& p, const Matrix& F, const Matrix& B, │ │ │ │ │ +114 const Vector& u, const Matrix& Q) const; │ │ │ │ │ +115 │ │ │ │ │ +124 State predict2(const State& p, const Matrix& A0, const Matrix& A1, │ │ │ │ │ +125 const Vector& b, const SharedDiagonal& model) const; │ │ │ │ │ +126 │ │ │ │ │ +135 State update(const State& p, const Matrix& H, const Vector& z, │ │ │ │ │ +136 const SharedDiagonal& model) const; │ │ │ │ │ +137 │ │ │ │ │ +138 /* │ │ │ │ │ +139 * Version of update with full covariance │ │ │ │ │ +140 * Q is normally derived as G*w*G^T where w models uncertainty of some │ │ │ │ │ +141 * physical property, such as velocity or acceleration, and G is derived │ │ │ │ │ +from physics. │ │ │ │ │ +142 * This version allows more realistic models than a diagonal covariance │ │ │ │ │ +matrix. │ │ │ │ │ +143 */ │ │ │ │ │ +144 State updateQ(const State& p, const Matrix& H, const Vector& z, │ │ │ │ │ +145 const Matrix& Q) const; │ │ │ │ │ +146}; │ │ │ │ │ 147 │ │ │ │ │ -148#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_i_t_e_r_a_t_i_v_e_-_i_n_l_._h> │ │ │ │ │ +148} // \namespace gtsam │ │ │ │ │ 149 │ │ │ │ │ -_M_a_t_r_i_x_._h │ │ │ │ │ -typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ -_i_t_e_r_a_t_i_v_e_-_i_n_l_._h │ │ │ │ │ -Iterative methods, template implementation. │ │ │ │ │ -_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_S_o_l_v_e_r_._h │ │ │ │ │ -Implementation of Conjugate Gradient solver for a linear system. │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ -Factor Graph Values. │ │ │ │ │ +150/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +151 │ │ │ │ │ +_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ +_G_a_u_s_s_i_a_n_D_e_n_s_i_t_y_._h │ │ │ │ │ +A Gaussian Density. │ │ │ │ │ +_N_o_i_s_e_M_o_d_e_l_._h │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_C_h_o_l_e_s_k_y │ │ │ │ │ +std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< │ │ │ │ │ +HessianFactor > > EliminateCholesky(const GaussianFactorGraph &factors, const │ │ │ │ │ +Ordering &keys) │ │ │ │ │ +Densely partially eliminate with Cholesky factorization. │ │ │ │ │ +DDeeffiinniittiioonn HessianFactor.cpp:525 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ print without optional string, must specify cout yourself │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_c_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_s │ │ │ │ │ -V conjugateGradients(const S &Ab, V x, const ConjugateGradientParameters │ │ │ │ │ -¶meters, bool steepest) │ │ │ │ │ -Method of conjugate gradients (CG) template "System" class S needs gradient │ │ │ │ │ -(S,v), e=S*v,... │ │ │ │ │ -DDeeffiinniittiioonn iterative-inl.h:125 │ │ │ │ │ -_g_t_s_a_m_:_:_c_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_D_e_s_c_e_n_t │ │ │ │ │ -Vector conjugateGradientDescent(const System &Ab, const Vector &x, const │ │ │ │ │ -ConjugateGradientParameters ¶meters) │ │ │ │ │ -Method of conjugate gradients (CG), System version. │ │ │ │ │ -DDeeffiinniittiioonn iterative.cpp:45 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ -parameters for the conjugate gradient method │ │ │ │ │ -DDeeffiinniittiioonn ConjugateGradientSolver.h:29 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_Q_R │ │ │ │ │ +std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > │ │ │ │ │ +EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys) │ │ │ │ │ +Multiply all factors and eliminate the given keys from the resulting factor │ │ │ │ │ +using a QR variant that h... │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.cpp:789 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_E_l_i_m_i_n_a_t_e │ │ │ │ │ +std::function< EliminationResult(const FactorGraphType &, const Ordering &)> │ │ │ │ │ +Eliminate │ │ │ │ │ +The function type that does a single dense elimination step on a subgraph. │ │ │ │ │ +DDeeffiinniittiioonn EliminateableFactorGraph.h:89 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactor.h:42 │ │ │ │ │ _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_s_t_e_m │ │ │ │ │ -Helper class encapsulating the combined system |Ax-b_|^2 Needed to run │ │ │ │ │ -Conjugate Gradients on matrice... │ │ │ │ │ -DDeeffiinniittiioonn iterative.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_s_t_e_m_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ -Vector operator*(const Vector &x) const │ │ │ │ │ -Apply operator A. │ │ │ │ │ -DDeeffiinniittiioonn iterative.h:78 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_s_t_e_m_:_:_A │ │ │ │ │ -const Matrix & A() const │ │ │ │ │ -Access A matrix. │ │ │ │ │ -DDeeffiinniittiioonn iterative.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_s_t_e_m_:_:_m_u_l_t_i_p_l_y_I_n_P_l_a_c_e │ │ │ │ │ -void multiplyInPlace(const Vector &x, Vector &e) const │ │ │ │ │ -Apply operator A in place. │ │ │ │ │ -DDeeffiinniittiioonn iterative.h:83 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_s_t_e_m_:_:_b │ │ │ │ │ -const Vector & b() const │ │ │ │ │ -Access b vector. │ │ │ │ │ -DDeeffiinniittiioonn iterative.h:60 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_s_t_e_m_:_:_g_r_a_d_i_e_n_t │ │ │ │ │ -Vector gradient(const Vector &x) const │ │ │ │ │ -gradient of objective function 0.5*|Ax-b_|^2 at x = A_'*(Ax-b_) │ │ │ │ │ -DDeeffiinniittiioonn iterative.h:73 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_s_t_e_m_:_:_t_r_a_n_s_p_o_s_e_M_u_l_t_i_p_l_y_A_d_d │ │ │ │ │ -void transposeMultiplyAdd(double alpha, const Vector &e, Vector &x) const │ │ │ │ │ -x += alpha* A'*e │ │ │ │ │ -DDeeffiinniittiioonn iterative.h:88 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_s_t_e_m_:_:_o_p_e_r_a_t_o_r_^ │ │ │ │ │ -Vector operator^(const Vector &e) const │ │ │ │ │ -Apply operator A'*e. │ │ │ │ │ -DDeeffiinniittiioonn iterative.h:63 │ │ │ │ │ -_g_t_s_a_m_:_:_I_t_e_r_a_t_i_v_e_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ -parameters for iterative linear solvers │ │ │ │ │ -DDeeffiinniittiioonn IterativeSolver.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ -with a unique integer... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_K_a_l_m_a_n_F_i_l_t_e_r │ │ │ │ │ +Kalman Filter class. │ │ │ │ │ +DDeeffiinniittiioonn KalmanFilter.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_K_a_l_m_a_n_F_i_l_t_e_r_:_:_F_a_c_t_o_r_i_z_a_t_i_o_n │ │ │ │ │ +Factorization │ │ │ │ │ +This Kalman filter is a Square-root Information filter The type below allows │ │ │ │ │ +you to specify the facto... │ │ │ │ │ +DDeeffiinniittiioonn KalmanFilter.h:49 │ │ │ │ │ +_g_t_s_a_m_:_:_K_a_l_m_a_n_F_i_l_t_e_r_:_:_s_t_e_p │ │ │ │ │ +static Key step(const State &p) │ │ │ │ │ +Return step index k, starts at 0, incremented at each predict. │ │ │ │ │ +DDeeffiinniittiioonn KalmanFilter.h:92 │ │ │ │ │ +_g_t_s_a_m_:_:_K_a_l_m_a_n_F_i_l_t_e_r_:_:_S_t_a_t_e │ │ │ │ │ +GaussianDensity::shared_ptr State │ │ │ │ │ +The Kalman filter state is simply a GaussianDensity. │ │ │ │ │ +DDeeffiinniittiioonn KalmanFilter.h:56 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _i_t_e_r_a_t_i_v_e_._h │ │ │ │ │ + * _K_a_l_m_a_n_F_i_l_t_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00887.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianEliminationTree.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/linearExceptions.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,34 +94,53 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
GaussianEliminationTree.cpp File Reference
│ │ │ │ +
linearExceptions.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ + │ │ │ │ +

Exceptions that may be thrown by linear solver components. │ │ │ │ +More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::IndeterminantLinearSystemException
 Thrown when a linear system is ill-posed. More...
 
class  gtsam::InvalidNoiseModel
 An exception indicating that the noise model dimension passed into a JacobianFactor has a different dimensionality than the factor. More...
 
class  gtsam::InvalidMatrixBlock
 An exception indicating that a matrix block passed into a JacobianFactor has a different dimensionality than the factor. More...
 
class  gtsam::InvalidDenseElimination
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Mar 29, 2013
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ +

Exceptions that may be thrown by linear solver components.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Aug 17, 2012
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,37 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -GaussianEliminationTree.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +linearExceptions.h File Reference │ │ │ │ │ +Exceptions that may be thrown by linear solver components. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_I_n_d_e_t_e_r_m_i_n_a_n_t_L_i_n_e_a_r_S_y_s_t_e_m_E_x_c_e_p_t_i_o_n │ │ │ │ │ +  Thrown when a linear system is ill-posed. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_I_n_v_a_l_i_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +  An exception indicating that the noise model dimension passed into a │ │ │ │ │ + _J_a_c_o_b_i_a_n_F_a_c_t_o_r has a different dimensionality than the factor. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_I_n_v_a_l_i_d_M_a_t_r_i_x_B_l_o_c_k │ │ │ │ │ +  An exception indicating that a matrix block passed into a │ │ │ │ │ + _J_a_c_o_b_i_a_n_F_a_c_t_o_r has a different dimensionality than the factor. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_I_n_v_a_l_i_d_D_e_n_s_e_E_l_i_m_i_n_a_t_i_o_n │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Mar 29, 2013 │ │ │ │ │ +Exceptions that may be thrown by linear solver components. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Aug 17, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._c_p_p │ │ │ │ │ + * _l_i_n_e_a_r_E_x_c_e_p_t_i_o_n_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00893.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/RegularHessianFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/SubgraphBuilder.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,46 +94,52 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
RegularHessianFactor.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
SubgraphBuilder.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ - │ │ │ │ -

HessianFactor class with constant sized blocks. │ │ │ │ -More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::RegularHessianFactor< D >
 
struct  gtsam::traits< RegularHessianFactor< D > >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +ostream & gtsam::operator<< (ostream &os, const Subgraph::Edge &edge)
 
│ │ │ │ +ostream & gtsam::operator<< (ostream &os, const Subgraph &subgraph)
 
│ │ │ │ +ostream & gtsam::operator<< (ostream &os, const SubgraphBuilderParameters &p)
 
│ │ │ │ +GaussianFactorGraph gtsam::buildFactorSubgraph (const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone)
 Select the factors in a factor graph according to the subgraph.
 
std::pair< GaussianFactorGraph, GaussianFactorGraph > gtsam::splitFactorGraph (const GaussianFactorGraph &factorGraph, const Subgraph &subgraph)
 Split the graph into a subgraph and the remaining edges.
 
│ │ │ │

Detailed Description

│ │ │ │ -

HessianFactor class with constant sized blocks.

│ │ │ │ -
Author
Sungtae An
│ │ │ │ -
Date
March 2014
│ │ │ │ +
Date
Dec 31, 2009
│ │ │ │ +
Author
Frank Dellaert, Yong-Dian Jian
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,42 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -RegularHessianFactor.h File Reference │ │ │ │ │ -HessianFactor class with constant sized blocks. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_<_ _D_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_<_ _D_ _>_ _> │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +SubgraphBuilder.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ + ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const │ │ │ │ │ + _S_u_b_g_r_a_p_h_:_:_E_d_g_e &edge) │ │ │ │ │ +  │ │ │ │ │ + ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const _S_u_b_g_r_a_p_h │ │ │ │ │ + &subgraph) │ │ │ │ │ +  │ │ │ │ │ + ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const │ │ │ │ │ + _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s &p) │ │ │ │ │ +  │ │ │ │ │ + _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h  ggttssaamm::::bbuuiillddFFaaccttoorrSSuubbggrraapphh (const │ │ │ │ │ + _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, const _S_u_b_g_r_a_p_h │ │ │ │ │ + &subgraph, const bool clone) │ │ │ │ │ +  Select the factors in a factor graph according │ │ │ │ │ + to the subgraph. │ │ │ │ │ +  │ │ │ │ │ +std::pair< _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h, _g_t_s_a_m_:_:_s_p_l_i_t_F_a_c_t_o_r_G_r_a_p_h (const │ │ │ │ │ + _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h >  _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &factorGraph, const │ │ │ │ │ + _S_u_b_g_r_a_p_h &subgraph) │ │ │ │ │ +  Split the graph into a subgraph and the │ │ │ │ │ + remaining edges. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -HessianFactor class with constant sized blocks. │ │ │ │ │ - Author │ │ │ │ │ - Sungtae An │ │ │ │ │ Date │ │ │ │ │ - March 2014 │ │ │ │ │ + Dec 31, 2009 │ │ │ │ │ + Author │ │ │ │ │ + Frank Dellaert, Yong-Dian Jian │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ + * _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00893.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a00893 = [ │ │ │ │ │ - ["gtsam::RegularHessianFactor< D >", "a04008.html", "a04008"], │ │ │ │ │ - ["gtsam::traits< RegularHessianFactor< D > >", "a04012.html", null] │ │ │ │ │ + ["buildFactorSubgraph", "a00893.html#a0ec82adc6e13261cf4a012b65b301256", null], │ │ │ │ │ + ["splitFactorGraph", "a00893.html#a2c7eca9f27b43b52756c1afd85478dd8", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00896.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/SubgraphPreconditioner.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/iterative.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,43 +94,62 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
SubgraphPreconditioner.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
iterative.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Iterative methods, implementation. │ │ │ │ +More...

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

struct  gtsam::SubgraphPreconditionerParameters
 
class  gtsam::SubgraphPreconditioner
 Subgraph conditioner class, as explained in the RSS 2010 submission. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +Vector gtsam::steepestDescent (const System &Ab, const Vector &x, const ConjugateGradientParameters &parameters)
 
│ │ │ │ +Vector gtsam::conjugateGradientDescent (const System &Ab, const Vector &x, const ConjugateGradientParameters &parameters)
 Method of conjugate gradients (CG), System version.
 
Vector gtsam::steepestDescent (const Matrix &A, const Vector &b, const Vector &x, const ConjugateGradientParameters &parameters)
 convenience calls using matrices, will create System class internally:
 
│ │ │ │ +Vector gtsam::conjugateGradientDescent (const Matrix &A, const Vector &b, const Vector &x, const ConjugateGradientParameters &parameters)
 Method of conjugate gradients (CG), Matrix version.
 
│ │ │ │ +VectorValues gtsam::steepestDescent (const GaussianFactorGraph &fg, const VectorValues &x, const ConjugateGradientParameters &parameters)
 Method of steepest gradients, Gaussian Factor Graph version.
 
│ │ │ │ +VectorValues gtsam::conjugateGradientDescent (const GaussianFactorGraph &fg, const VectorValues &x, const ConjugateGradientParameters &parameters)
 Method of conjugate gradients (CG), Gaussian Factor Graph version.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Dec 31, 2009
│ │ │ │ -
Author
Frank Dellaert, Yong-Dian Jian
│ │ │ │ +

Iterative methods, implementation.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
Date
Dec 28, 2009
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,50 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SubgraphPreconditioner.h File Reference │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r │ │ │ │ │ -  _S_u_b_g_r_a_p_h conditioner class, as explained in the RSS 2010 submission. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +iterative.cpp File Reference │ │ │ │ │ +Iterative methods, implementation. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ + Vector  ggttssaamm::::sstteeeeppeessttDDeesscceenntt (const _S_y_s_t_e_m &Ab, const Vector &x, const │ │ │ │ │ + _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s ¶meters) │ │ │ │ │ +  │ │ │ │ │ + Vector  ggttssaamm::::ccoonnjjuuggaatteeGGrraaddiieennttDDeesscceenntt (const _S_y_s_t_e_m &Ab, const Vector │ │ │ │ │ + &x, const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s ¶meters) │ │ │ │ │ +  Method of conjugate gradients (CG), _S_y_s_t_e_m version. │ │ │ │ │ +  │ │ │ │ │ + Vector  _g_t_s_a_m_:_:_s_t_e_e_p_e_s_t_D_e_s_c_e_n_t (const Matrix &A, const Vector &b, const │ │ │ │ │ + Vector &x, const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s ¶meters) │ │ │ │ │ +  convenience calls using matrices, will create _S_y_s_t_e_m class │ │ │ │ │ + internally: │ │ │ │ │ +  │ │ │ │ │ + Vector  ggttssaamm::::ccoonnjjuuggaatteeGGrraaddiieennttDDeesscceenntt (const Matrix &A, const Vector │ │ │ │ │ + &b, const Vector &x, const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ + ¶meters) │ │ │ │ │ +  Method of conjugate gradients (CG), Matrix version. │ │ │ │ │ +  │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s  ggttssaamm::::sstteeeeppeessttDDeesscceenntt (const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &fg, const │ │ │ │ │ + _V_e_c_t_o_r_V_a_l_u_e_s &x, const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s ¶meters) │ │ │ │ │ +  Method of steepest gradients, Gaussian _F_a_c_t_o_r Graph version. │ │ │ │ │ +  │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s  ggttssaamm::::ccoonnjjuuggaatteeGGrraaddiieennttDDeesscceenntt (const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &fg, │ │ │ │ │ + const _V_e_c_t_o_r_V_a_l_u_e_s &x, const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ + ¶meters) │ │ │ │ │ +  Method of conjugate gradients (CG), Gaussian _F_a_c_t_o_r Graph │ │ │ │ │ + version. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Dec 31, 2009 │ │ │ │ │ +Iterative methods, implementation. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert, Yong-Dian Jian │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + Dec 28, 2009 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_._h │ │ │ │ │ + * _i_t_e_r_a_t_i_v_e_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00896.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,7 @@ │ │ │ │ │ var a00896 = [ │ │ │ │ │ - ["gtsam::SubgraphPreconditionerParameters", "a04048.html", null], │ │ │ │ │ - ["gtsam::SubgraphPreconditioner", "a04052.html", "a04052"] │ │ │ │ │ + ["conjugateGradientDescent", "a00896.html#aa84114ad1593684ad739088a2898afdb", null], │ │ │ │ │ + ["conjugateGradientDescent", "a00896.html#af55440f741d8b2f706101f7a79ba7111", null], │ │ │ │ │ + ["conjugateGradientDescent", "a00896.html#ad2b587fde5d35a27d6c88feb4432e785", null], │ │ │ │ │ + ["steepestDescent", "a00896.html#a81b7af7638d28555c3f404318cc67b22", null], │ │ │ │ │ + ["steepestDescent", "a00896.html#aed4b2ca704ddd6530ea70122f883ac88", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00899.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianFactorGraph.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/AcceleratedPowerMethod.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,62 +96,50 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ -Functions
│ │ │ │ -
GaussianFactorGraph.h File Reference
│ │ │ │ +Typedefs
│ │ │ │ +
AcceleratedPowerMethod.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Linear Factor Graph where all factors are Gaussians. │ │ │ │ +

accelerated power method for fast eigenvalue and eigenvector computation │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

struct  gtsam::EliminationTraits< GaussianFactorGraph >
 
class  gtsam::GaussianFactorGraph
 A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. More...
 
struct  gtsam::traits< GaussianFactorGraph >
 traits More...
class  gtsam::AcceleratedPowerMethod< Operator >
 Compute maximum Eigenpair with accelerated power method. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ -Functions

bool gtsam::hasConstraints (const GaussianFactorGraph &factors)
 Evaluates whether linear factors have any constrained noise models.
 

│ │ │ │ +Typedefs

│ │ │ │ +using gtsam::Sparse = Eigen::SparseMatrix< double >
 
│ │ │ │

Detailed Description

│ │ │ │ -

Linear Factor Graph where all factors are Gaussians.

│ │ │ │ -
Author
Kai Ni
│ │ │ │ -
│ │ │ │ -Christian Potthast
│ │ │ │ -
│ │ │ │ -Alireza Fathi
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ +

accelerated power method for fast eigenvalue and eigenvector computation

│ │ │ │ +
Date
Sept 2020
│ │ │ │ +
Author
Jing Wu
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,39 +1,31 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -GaussianFactorGraph.h File Reference │ │ │ │ │ -Linear Factor Graph where all factors are Gaussians. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ +AcceleratedPowerMethod.h File Reference │ │ │ │ │ +accelerated power method for fast eigenvalue and eigenvector computation │ │ │ │ │ +_M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _> │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -  A Linear _F_a_c_t_o_r Graph is a factor graph where all factors are │ │ │ │ │ - Gaussian, i.e. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_A_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_M_e_t_h_o_d_<_ _O_p_e_r_a_t_o_r_ _> │ │ │ │ │ +  Compute maximum Eigenpair with accelerated power method. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -bool  _g_t_s_a_m_:_:_h_a_s_C_o_n_s_t_r_a_i_n_t_s (const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &factors) │ │ │ │ │ -  Evaluates whether linear factors have any constrained noise models. │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +using  ggttssaamm::::SSppaarrssee = Eigen::SparseMatrix< double > │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ +accelerated power method for fast eigenvalue and eigenvector computation │ │ │ │ │ + Date │ │ │ │ │ + Sept 2020 │ │ │ │ │ Author │ │ │ │ │ - Kai Ni │ │ │ │ │ - Christian Potthast │ │ │ │ │ - Alireza Fathi │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Jing Wu │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ + * _A_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_M_e_t_h_o_d_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00899.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,6 +1,3 @@ │ │ │ │ │ var a00899 = [ │ │ │ │ │ - ["gtsam::EliminationTraits< GaussianFactorGraph >", "a03788.html", "a03788"], │ │ │ │ │ - ["gtsam::GaussianFactorGraph", "a03792.html", "a03792"], │ │ │ │ │ - ["gtsam::traits< GaussianFactorGraph >", "a03796.html", null], │ │ │ │ │ - ["hasConstraints", "a00899.html#a35c269c3243cab16a7475239a9c91021", null] │ │ │ │ │ + ["gtsam::AcceleratedPowerMethod< Operator >", "a03716.html", "a03716"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00899_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianFactorGraph.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/AcceleratedPowerMethod.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,338 +98,166 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
GaussianFactorGraph.h
│ │ │ │ +
AcceleratedPowerMethod.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
22#pragma once
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/linear/PowerMethod.h>
│ │ │ │
23
│ │ │ │ -
24#include <gtsam/inference/EliminateableFactorGraph.h>
│ │ │ │ -
25#include <gtsam/inference/FactorGraph.h>
│ │ │ │ -
26#include <gtsam/linear/Errors.h> // Included here instead of fw-declared so we can use Errors::iterator
│ │ │ │ -
27#include <gtsam/linear/GaussianFactor.h>
│ │ │ │ -
28#include <gtsam/linear/HessianFactor.h>
│ │ │ │ -
29#include <gtsam/linear/JacobianFactor.h>
│ │ │ │ -
30#include <gtsam/linear/VectorValues.h>
│ │ │ │ -
31
│ │ │ │ -
32namespace gtsam {
│ │ │ │ -
33
│ │ │ │ -
34 // Forward declarations
│ │ │ │ -
35 class GaussianFactorGraph;
│ │ │ │ -
36 class GaussianFactor;
│ │ │ │ -
37 class GaussianConditional;
│ │ │ │ -
38 class GaussianBayesNet;
│ │ │ │ -
39 class GaussianEliminationTree;
│ │ │ │ -
40 class GaussianBayesTree;
│ │ │ │ -
41 class GaussianJunctionTree;
│ │ │ │ -
42
│ │ │ │ -
43 /* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
44 template<> struct EliminationTraits<GaussianFactorGraph>
│ │ │ │ -
45 {
│ │ │ │ -
46 typedef GaussianFactor FactorType;
│ │ │ │ -
47 typedef GaussianFactorGraph FactorGraphType;
│ │ │ │ -
48 typedef GaussianConditional ConditionalType;
│ │ │ │ -
49 typedef GaussianBayesNet BayesNetType;
│ │ │ │ -
50 typedef GaussianEliminationTree EliminationTreeType;
│ │ │ │ -
51 typedef GaussianBayesTree BayesTreeType;
│ │ │ │ -
52 typedef GaussianJunctionTree JunctionTreeType;
│ │ │ │ -
54 static std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> >
│ │ │ │ -
│ │ │ │ -
55 DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) {
│ │ │ │ -
56 return EliminatePreferCholesky(factors, keys); }
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
58 static Ordering DefaultOrderingFunc(
│ │ │ │ -
59 const FactorGraphType& graph,
│ │ │ │ -
60 boost::optional<const VariableIndex&> variableIndex) {
│ │ │ │ -
61 return Ordering::Colamd(*variableIndex);
│ │ │ │ -
62 }
│ │ │ │ -
│ │ │ │ -
63 };
│ │ │ │ -
│ │ │ │ -
64
│ │ │ │ -
65 /* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
72 class GTSAM_EXPORT GaussianFactorGraph :
│ │ │ │ -
73 public FactorGraph<GaussianFactor>,
│ │ │ │ -
74 public EliminateableFactorGraph<GaussianFactorGraph>
│ │ │ │ -
75 {
│ │ │ │ -
76 public:
│ │ │ │ -
77
│ │ │ │ -
78 typedef GaussianFactorGraph This;
│ │ │ │ -
79 typedef FactorGraph<GaussianFactor> Base;
│ │ │ │ -
80 typedef EliminateableFactorGraph<This> BaseEliminateable;
│ │ │ │ -
81 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
82
│ │ │ │ -
85
│ │ │ │ -
87 GaussianFactorGraph() {}
│ │ │ │ -
88
│ │ │ │ -
94 GaussianFactorGraph(std::initializer_list<sharedFactor> factors) : Base(factors) {}
│ │ │ │ -
95
│ │ │ │ +
24namespace gtsam {
│ │ │ │ +
25
│ │ │ │ +
26using Sparse = Eigen::SparseMatrix<double>;
│ │ │ │ +
27
│ │ │ │ +
50template <class Operator>
│ │ │ │ +
│ │ │ │ +
51class AcceleratedPowerMethod : public PowerMethod<Operator> {
│ │ │ │ +
52
│ │ │ │ +
53 double beta_ = 0; // a Polyak momentum term
│ │ │ │ +
54
│ │ │ │ +
55 Vector previousVector_; // store previous vector
│ │ │ │ +
56
│ │ │ │ +
57 public:
│ │ │ │ +
│ │ │ │ +
62 explicit AcceleratedPowerMethod(
│ │ │ │ +
63 const Operator &A, const boost::optional<Vector> initial = boost::none,
│ │ │ │ +
64 double initialBeta = 0.0)
│ │ │ │ +
65 : PowerMethod<Operator>(A, initial) {
│ │ │ │ +
66 // initialize Ritz eigen vector and previous vector
│ │ │ │ +
67 this->ritzVector_ = initial ? initial.get() : Vector::Random(this->dim_);
│ │ │ │ +
68 this->ritzVector_.normalize();
│ │ │ │ +
69 previousVector_ = Vector::Zero(this->dim_);
│ │ │ │ +
70
│ │ │ │ +
71 // initialize beta_
│ │ │ │ +
72 beta_ = initialBeta;
│ │ │ │ +
73 }
│ │ │ │ +
│ │ │ │ +
74
│ │ │ │ +
│ │ │ │ +
80 Vector acceleratedPowerIteration (const Vector &x1, const Vector &x0,
│ │ │ │ +
81 const double beta) const {
│ │ │ │ +
82 Vector y = this->A_ * x1 - beta * x0;
│ │ │ │ +
83 y.normalize();
│ │ │ │ +
84 return y;
│ │ │ │ +
85 }
│ │ │ │ +
│ │ │ │ +
86
│ │ │ │ +
│ │ │ │ +
92 Vector acceleratedPowerIteration () const {
│ │ │ │ +
93 Vector y = acceleratedPowerIteration(this->ritzVector_, previousVector_, beta_);
│ │ │ │ +
94 return y;
│ │ │ │ +
95 }
│ │ │ │ +
│ │ │ │
96
│ │ │ │ -
98 template<typename ITERATOR>
│ │ │ │ -
99 GaussianFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {}
│ │ │ │ -
100
│ │ │ │ -
102 template<class CONTAINER>
│ │ │ │ -
103 explicit GaussianFactorGraph(const CONTAINER& factors) : Base(factors) {}
│ │ │ │ -
104
│ │ │ │ -
106 template<class DERIVEDFACTOR>
│ │ │ │ -
107 GaussianFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
│ │ │ │ -
108
│ │ │ │ -
110 virtual ~GaussianFactorGraph() {}
│ │ │ │ -
111
│ │ │ │ -
115
│ │ │ │ -
116 bool equals(const This& fg, double tol = 1e-9) const;
│ │ │ │ -
117
│ │ │ │ -
119
│ │ │ │ -
│ │ │ │ -
121 friend bool operator==(const GaussianFactorGraph& lhs,
│ │ │ │ -
122 const GaussianFactorGraph& rhs) {
│ │ │ │ -
123 return lhs.isEqual(rhs);
│ │ │ │ -
124 }
│ │ │ │ -
│ │ │ │ -
125
│ │ │ │ -
127 void add(const GaussianFactor& factor) { push_back(factor.clone()); }
│ │ │ │ -
128
│ │ │ │ -
130 void add(const sharedFactor& factor) { push_back(factor); }
│ │ │ │ -
131
│ │ │ │ -
│ │ │ │ -
133 void add(const Vector& b) {
│ │ │ │ -
134 add(JacobianFactor(b)); }
│ │ │ │ -
│ │ │ │ -
135
│ │ │ │ -
│ │ │ │ -
137 void add(Key key1, const Matrix& A1,
│ │ │ │ -
138 const Vector& b, const SharedDiagonal& model = SharedDiagonal()) {
│ │ │ │ -
139 add(JacobianFactor(key1,A1,b,model)); }
│ │ │ │ -
│ │ │ │ -
140
│ │ │ │ -
│ │ │ │ -
142 void add(Key key1, const Matrix& A1,
│ │ │ │ -
143 Key key2, const Matrix& A2,
│ │ │ │ -
144 const Vector& b, const SharedDiagonal& model = SharedDiagonal()) {
│ │ │ │ -
145 add(JacobianFactor(key1,A1,key2,A2,b,model)); }
│ │ │ │ -
│ │ │ │ -
146
│ │ │ │ -
│ │ │ │ -
148 void add(Key key1, const Matrix& A1,
│ │ │ │ -
149 Key key2, const Matrix& A2,
│ │ │ │ -
150 Key key3, const Matrix& A3,
│ │ │ │ -
151 const Vector& b, const SharedDiagonal& model = SharedDiagonal()) {
│ │ │ │ -
152 add(JacobianFactor(key1,A1,key2,A2,key3,A3,b,model)); }
│ │ │ │ +
│ │ │ │ +
101 double estimateBeta(const size_t T = 10) const {
│ │ │ │ +
102 // set initial estimation of maxBeta
│ │ │ │ +
103 Vector initVector = this->ritzVector_;
│ │ │ │ +
104 const double up = initVector.dot( this->A_ * initVector );
│ │ │ │ +
105 const double down = initVector.dot(initVector);
│ │ │ │ +
106 const double mu = up / down;
│ │ │ │ +
107 double maxBeta = mu * mu / 4;
│ │ │ │ +
108 size_t maxIndex;
│ │ │ │ +
109 std::vector<double> betas;
│ │ │ │ +
110
│ │ │ │ +
111 Matrix R = Matrix::Zero(this->dim_, 10);
│ │ │ │ +
112 // run T times of iteration to find the beta that has the largest Rayleigh quotient
│ │ │ │ +
113 for (size_t t = 0; t < T; t++) {
│ │ │ │ +
114 // after each t iteration, reset the betas with the current maxBeta
│ │ │ │ +
115 betas = {2 / 3 * maxBeta, 0.99 * maxBeta, maxBeta, 1.01 * maxBeta,
│ │ │ │ +
116 1.5 * maxBeta};
│ │ │ │ +
117 // iterate through every beta value
│ │ │ │ +
118 for (size_t k = 0; k < betas.size(); ++k) {
│ │ │ │ +
119 // initialize x0 and x00 in each iteration of each beta
│ │ │ │ +
120 Vector x0 = initVector;
│ │ │ │ +
121 Vector x00 = Vector::Zero(this->dim_);
│ │ │ │ +
122 // run 10 steps of accelerated power iteration with this beta
│ │ │ │ +
123 for (size_t j = 1; j < 10; j++) {
│ │ │ │ +
124 if (j < 2) {
│ │ │ │ +
125 R.col(0) = acceleratedPowerIteration(x0, x00, betas[k]);
│ │ │ │ +
126 R.col(1) = acceleratedPowerIteration(R.col(0), x0, betas[k]);
│ │ │ │ +
127 } else {
│ │ │ │ +
128 R.col(j) = acceleratedPowerIteration(R.col(j - 1), R.col(j - 2),
│ │ │ │ +
129 betas[k]);
│ │ │ │ +
130 }
│ │ │ │ +
131 }
│ │ │ │ +
132 // compute the Rayleigh quotient for the randomly sampled vector after
│ │ │ │ +
133 // 10 steps of power accelerated iteration
│ │ │ │ +
134 const Vector x = R.col(9);
│ │ │ │ +
135 const double up = x.dot(this->A_ * x);
│ │ │ │ +
136 const double down = x.dot(x);
│ │ │ │ +
137 const double mu = up / down;
│ │ │ │ +
138 // store the momentum with largest Rayleigh quotient and its according index of beta_
│ │ │ │ +
139 if (mu * mu / 4 > maxBeta) {
│ │ │ │ +
140 // save the max beta index
│ │ │ │ +
141 maxIndex = k;
│ │ │ │ +
142 maxBeta = mu * mu / 4;
│ │ │ │ +
143 }
│ │ │ │ +
144 }
│ │ │ │ +
145 }
│ │ │ │ +
146 // set beta_ to momentum with largest Rayleigh quotient
│ │ │ │ +
147 return betas[maxIndex];
│ │ │ │ +
148 }
│ │ │ │
│ │ │ │ -
153
│ │ │ │ -
155 template<class TERMS>
│ │ │ │ +
149
│ │ │ │
│ │ │ │ -
156 void add(const TERMS& terms, const Vector &b, const SharedDiagonal& model = SharedDiagonal()) {
│ │ │ │ -
157 add(JacobianFactor(terms,b,model)); }
│ │ │ │ -
│ │ │ │ -
158
│ │ │ │ -
163 typedef KeySet Keys;
│ │ │ │ -
164 Keys keys() const;
│ │ │ │ -
165
│ │ │ │ -
166 /* return a map of (Key, dimension) */
│ │ │ │ -
167 std::map<Key, size_t> getKeyDimMap() const;
│ │ │ │ -
168
│ │ │ │ -
170 double error(const VectorValues& x) const;
│ │ │ │ +
156 bool compute(size_t maxIterations, double tol) {
│ │ │ │ +
157 // Starting
│ │ │ │ +
158 bool isConverged = false;
│ │ │ │ +
159
│ │ │ │ +
160 for (size_t i = 0; i < maxIterations && !isConverged; i++) {
│ │ │ │ +
161 ++(this->nrIterations_);
│ │ │ │ +
162 Vector tmp = this->ritzVector_;
│ │ │ │ +
163 // update the ritzVector after accelerated power iteration
│ │ │ │ +
164 this->ritzVector_ = acceleratedPowerIteration();
│ │ │ │ +
165 // update the previousVector with ritzVector
│ │ │ │ +
166 previousVector_ = tmp;
│ │ │ │ +
167 // update the ritzValue
│ │ │ │ +
168 this->ritzValue_ = this->ritzVector_.dot(this->A_ * this->ritzVector_);
│ │ │ │ +
169 isConverged = this->converged(tol);
│ │ │ │ +
170 }
│ │ │ │
171
│ │ │ │ -
173 double probPrime(const VectorValues& c) const;
│ │ │ │ -
174
│ │ │ │ -
180 virtual GaussianFactorGraph clone() const;
│ │ │ │ -
181
│ │ │ │ -
186 virtual GaussianFactorGraph::shared_ptr cloneToPtr() const;
│ │ │ │ -
187
│ │ │ │ -
194 GaussianFactorGraph negate() const;
│ │ │ │ -
195
│ │ │ │ -
198
│ │ │ │ -
209 std::vector<std::tuple<int, int, double> > sparseJacobian(
│ │ │ │ -
210 const Ordering& ordering, size_t& nrows, size_t& ncols) const;
│ │ │ │ -
211
│ │ │ │ -
213 std::vector<std::tuple<int, int, double> > sparseJacobian() const;
│ │ │ │ -
214
│ │ │ │ -
221 Matrix sparseJacobian_() const;
│ │ │ │ -
222
│ │ │ │ -
230 Matrix augmentedJacobian(const Ordering& ordering) const;
│ │ │ │ -
231
│ │ │ │ -
239 Matrix augmentedJacobian() const;
│ │ │ │ -
240
│ │ │ │ -
248 std::pair<Matrix,Vector> jacobian(const Ordering& ordering) const;
│ │ │ │ -
249
│ │ │ │ -
257 std::pair<Matrix,Vector> jacobian() const;
│ │ │ │ -
258
│ │ │ │ -
270 Matrix augmentedHessian(const Ordering& ordering) const;
│ │ │ │ -
271
│ │ │ │ -
283 Matrix augmentedHessian() const;
│ │ │ │ -
284
│ │ │ │ -
291 std::pair<Matrix,Vector> hessian(const Ordering& ordering) const;
│ │ │ │ -
292
│ │ │ │ -
299 std::pair<Matrix,Vector> hessian() const;
│ │ │ │ -
300
│ │ │ │ -
302 virtual VectorValues hessianDiagonal() const;
│ │ │ │ -
303
│ │ │ │ -
305 virtual std::map<Key,Matrix> hessianBlockDiagonal() const;
│ │ │ │ -
306
│ │ │ │ -
311 VectorValues optimize(
│ │ │ │ -
312 const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
│ │ │ │ -
313
│ │ │ │ -
318 VectorValues optimize(const Ordering&,
│ │ │ │ -
319 const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
│ │ │ │ -
320
│ │ │ │ -
324 VectorValues optimizeDensely() const;
│ │ │ │ -
325
│ │ │ │ -
335 VectorValues gradient(const VectorValues& x0) const;
│ │ │ │ -
336
│ │ │ │ -
344 virtual VectorValues gradientAtZero() const;
│ │ │ │ -
345
│ │ │ │ -
370 VectorValues optimizeGradientSearch() const;
│ │ │ │ -
371
│ │ │ │ -
373 VectorValues transposeMultiply(const Errors& e) const;
│ │ │ │ -
374
│ │ │ │ -
376 void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& x) const;
│ │ │ │ -
377
│ │ │ │ -
379 Errors gaussianErrors(const VectorValues& x) const;
│ │ │ │ -
380
│ │ │ │ -
382 Errors operator*(const VectorValues& x) const;
│ │ │ │ -
383
│ │ │ │ -
385 void multiplyHessianAdd(double alpha, const VectorValues& x,
│ │ │ │ -
386 VectorValues& y) const;
│ │ │ │ -
387
│ │ │ │ -
389 void multiplyInPlace(const VectorValues& x, Errors& e) const;
│ │ │ │ -
390
│ │ │ │ -
392 void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const;
│ │ │ │ -
393
│ │ │ │ -
394 void printErrors(
│ │ │ │ -
395 const VectorValues& x,
│ │ │ │ -
396 const std::string& str = "GaussianFactorGraph: ",
│ │ │ │ -
397 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
398 const std::function<bool(const Factor* /*factor*/,
│ │ │ │ -
399 double /*whitenedError*/, size_t /*index*/)>&
│ │ │ │ -
400 printCondition =
│ │ │ │ -
401 [](const Factor*, double, size_t) { return true; }) const;
│ │ │ │ -
403
│ │ │ │ -
404 private:
│ │ │ │ -
406 friend class boost::serialization::access;
│ │ │ │ -
407 template<class ARCHIVE>
│ │ │ │ -
408 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
409 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
410 }
│ │ │ │ -
411
│ │ │ │ -
412 public:
│ │ │ │ -
413
│ │ │ │ -
414#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ -
416 VectorValues GTSAM_DEPRECATED
│ │ │ │ -
417 optimize(boost::none_t, const Eliminate& function =
│ │ │ │ -
418 EliminationTraitsType::DefaultEliminate) const {
│ │ │ │ -
419 return optimize(function);
│ │ │ │ -
420 }
│ │ │ │ -
421#endif
│ │ │ │ -
422
│ │ │ │ -
423 };
│ │ │ │ -
│ │ │ │ -
424
│ │ │ │ -
429 GTSAM_EXPORT bool hasConstraints(const GaussianFactorGraph& factors);
│ │ │ │ -
430
│ │ │ │ -
431 /****** Linear Algebra Operations ******/
│ │ │ │ -
432
│ │ │ │ -
434 //GTSAM_EXPORT void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
│ │ │ │ -
435 //GTSAM_EXPORT void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
│ │ │ │ -
436
│ │ │ │ -
438template<>
│ │ │ │ -
│ │ │ │ -
439struct traits<GaussianFactorGraph> : public Testable<GaussianFactorGraph> {
│ │ │ │ -
440};
│ │ │ │ -
│ │ │ │ -
441
│ │ │ │ -
442} // \ namespace gtsam
│ │ │ │ -
EliminateableFactorGraph.h
Variable elimination algorithms for factor graphs.
│ │ │ │ -
FactorGraph.h
Factor Graph Base Class.
│ │ │ │ -
HessianFactor.h
Contains the HessianFactor class, a general quadratic factor.
│ │ │ │ - │ │ │ │ -
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
│ │ │ │ -
VectorValues.h
Factor Graph Values.
│ │ │ │ -
Errors.h
vector of errors
│ │ │ │ -
gtsam::EliminatePreferCholesky
std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< GaussianFactor > > EliminatePreferCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
Densely partially eliminate with Cholesky factorization.
Definition HessianFactor.cpp:548
│ │ │ │ +
172 return isConverged;
│ │ │ │ +
173 }
│ │ │ │ +
│ │ │ │ +
174};
│ │ │ │ +
│ │ │ │ +
175
│ │ │ │ +
176} // namespace gtsam
│ │ │ │ +
PowerMethod.h
Power method for fast eigenvalue and eigenvector computation.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::hasConstraints
bool hasConstraints(const GaussianFactorGraph &factors)
Evaluates whether linear factors have any constrained noise models.
Definition GaussianFactorGraph.cpp:442
│ │ │ │ -
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
│ │ │ │ -
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ - │ │ │ │ - │ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ -
gtsam::FactorGraph::isEqual
bool isEqual(const FactorGraph &other) const
Check exact equality of the factor pointers. Useful for derived ==.
Definition FactorGraph.h:134
│ │ │ │ -
gtsam::FactorGraph< GaussianFactor >::sharedFactor
boost::shared_ptr< GaussianFactor > sharedFactor
Shared pointer to a factor.
Definition FactorGraph.h:101
│ │ │ │ -
gtsam::EliminationTraits
Traits class for eliminateable factor graphs, specifies the types that result from elimination,...
Definition EliminateableFactorGraph.h:36
│ │ │ │ -
gtsam::EliminateableFactorGraph
EliminateableFactorGraph is a base class for factor graphs that contains elimination algorithms.
Definition EliminateableFactorGraph.h:57
│ │ │ │ -
gtsam::Factor
Definition Factor.h:68
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ -
gtsam::Ordering::Colamd
static Ordering Colamd(const FACTOR_GRAPH &graph)
Compute a fill-reducing ordering using COLAMD from a factor graph (see details for note on performanc...
Definition Ordering.h:95
│ │ │ │ -
gtsam::GaussianBayesNet
GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals.
Definition GaussianBayesNet.h:36
│ │ │ │ -
gtsam::GaussianBayesTree
A Bayes tree representing a Gaussian density.
Definition GaussianBayesTree.h:52
│ │ │ │ -
gtsam::GaussianConditional
A GaussianConditional functions as the node in a Bayes network.
Definition GaussianConditional.h:43
│ │ │ │ -
gtsam::GaussianEliminationTree
Definition GaussianEliminationTree.h:29
│ │ │ │ -
gtsam::GaussianFactor
An abstract virtual base class for JacobianFactor and HessianFactor.
Definition GaussianFactor.h:39
│ │ │ │ -
gtsam::GaussianFactor::clone
virtual GaussianFactor::shared_ptr clone() const =0
Clone a factor (make a deep copy)
│ │ │ │ -
gtsam::EliminationTraits< GaussianFactorGraph >::DefaultOrderingFunc
static Ordering DefaultOrderingFunc(const FactorGraphType &graph, boost::optional< const VariableIndex & > variableIndex)
The default ordering generation function.
Definition GaussianFactorGraph.h:58
│ │ │ │ -
gtsam::EliminationTraits< GaussianFactorGraph >::BayesTreeType
GaussianBayesTree BayesTreeType
Type of Bayes tree.
Definition GaussianFactorGraph.h:51
│ │ │ │ -
gtsam::EliminationTraits< GaussianFactorGraph >::ConditionalType
GaussianConditional ConditionalType
Type of conditionals from elimination.
Definition GaussianFactorGraph.h:48
│ │ │ │ -
gtsam::EliminationTraits< GaussianFactorGraph >::FactorType
GaussianFactor FactorType
Type of factors in factor graph.
Definition GaussianFactorGraph.h:46
│ │ │ │ -
gtsam::EliminationTraits< GaussianFactorGraph >::EliminationTreeType
GaussianEliminationTree EliminationTreeType
Type of elimination tree.
Definition GaussianFactorGraph.h:50
│ │ │ │ -
gtsam::EliminationTraits< GaussianFactorGraph >::FactorGraphType
GaussianFactorGraph FactorGraphType
Type of the factor graph (e.g. GaussianFactorGraph)
Definition GaussianFactorGraph.h:47
│ │ │ │ -
gtsam::EliminationTraits< GaussianFactorGraph >::BayesNetType
GaussianBayesNet BayesNetType
Type of Bayes net from sequential elimination.
Definition GaussianFactorGraph.h:49
│ │ │ │ -
gtsam::EliminationTraits< GaussianFactorGraph >::JunctionTreeType
GaussianJunctionTree JunctionTreeType
Type of Junction tree.
Definition GaussianFactorGraph.h:52
│ │ │ │ -
gtsam::EliminationTraits< GaussianFactorGraph >::DefaultEliminate
static std::pair< boost::shared_ptr< ConditionalType >, boost::shared_ptr< FactorType > > DefaultEliminate(const FactorGraphType &factors, const Ordering &keys)
The default dense elimination function.
Definition GaussianFactorGraph.h:55
│ │ │ │ -
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ -
gtsam::GaussianFactorGraph::BaseEliminateable
EliminateableFactorGraph< This > BaseEliminateable
Typedef to base elimination class.
Definition GaussianFactorGraph.h:80
│ │ │ │ -
gtsam::GaussianFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactorGraph.h:81
│ │ │ │ -
gtsam::GaussianFactorGraph::add
void add(const TERMS &terms, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Add an n-ary factor.
Definition GaussianFactorGraph.h:156
│ │ │ │ -
gtsam::GaussianFactorGraph::GaussianFactorGraph
GaussianFactorGraph(std::initializer_list< sharedFactor > factors)
Construct from an initializer lists of GaussianFactor shared pointers.
Definition GaussianFactorGraph.h:94
│ │ │ │ -
gtsam::GaussianFactorGraph::GaussianFactorGraph
GaussianFactorGraph()
Default constructor.
Definition GaussianFactorGraph.h:87
│ │ │ │ -
gtsam::GaussianFactorGraph::add
void add(const GaussianFactor &factor)
Add a factor by value - makes a copy.
Definition GaussianFactorGraph.h:127
│ │ │ │ -
gtsam::GaussianFactorGraph::add
void add(Key key1, const Matrix &A1, Key key2, const Matrix &A2, Key key3, const Matrix &A3, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Add a ternary factor.
Definition GaussianFactorGraph.h:148
│ │ │ │ -
gtsam::GaussianFactorGraph::add
void add(const sharedFactor &factor)
Add a factor by pointer - stores pointer without copying the factor.
Definition GaussianFactorGraph.h:130
│ │ │ │ -
gtsam::GaussianFactorGraph::operator==
friend bool operator==(const GaussianFactorGraph &lhs, const GaussianFactorGraph &rhs)
Check exact equality.
Definition GaussianFactorGraph.h:121
│ │ │ │ -
gtsam::GaussianFactorGraph::This
GaussianFactorGraph This
Typedef to this class.
Definition GaussianFactorGraph.h:78
│ │ │ │ -
gtsam::GaussianFactorGraph::Keys
KeySet Keys
Return the set of variables involved in the factors (computes a set union).
Definition GaussianFactorGraph.h:163
│ │ │ │ -
gtsam::GaussianFactorGraph::add
void add(const Vector &b)
Add a null factor.
Definition GaussianFactorGraph.h:133
│ │ │ │ -
gtsam::GaussianFactorGraph::add
void add(Key key1, const Matrix &A1, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Add a unary factor.
Definition GaussianFactorGraph.h:137
│ │ │ │ -
gtsam::GaussianFactorGraph::~GaussianFactorGraph
virtual ~GaussianFactorGraph()
Virtual destructor.
Definition GaussianFactorGraph.h:110
│ │ │ │ -
gtsam::GaussianFactorGraph::add
void add(Key key1, const Matrix &A1, Key key2, const Matrix &A2, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Add a binary factor.
Definition GaussianFactorGraph.h:142
│ │ │ │ -
gtsam::GaussianFactorGraph::GaussianFactorGraph
GaussianFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
Construct from iterator over factors.
Definition GaussianFactorGraph.h:99
│ │ │ │ -
gtsam::GaussianFactorGraph::Base
FactorGraph< GaussianFactor > Base
Typedef to base factor graph type.
Definition GaussianFactorGraph.h:79
│ │ │ │ -
gtsam::GaussianFactorGraph::GaussianFactorGraph
GaussianFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
Implicit copy/downcast constructor to override explicit template container constructor.
Definition GaussianFactorGraph.h:107
│ │ │ │ -
gtsam::GaussianFactorGraph::GaussianFactorGraph
GaussianFactorGraph(const CONTAINER &factors)
Construct from container of factors (shared_ptr or plain objects)
Definition GaussianFactorGraph.h:103
│ │ │ │ -
gtsam::GaussianJunctionTree
A junction tree specialized to Gaussian factors, i.e., it is a cluster tree with Gaussian factors sto...
Definition GaussianJunctionTree.h:39
│ │ │ │ -
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition JacobianFactor.h:91
│ │ │ │ -
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ -
GaussianConditional
is the normalization constant.
│ │ │ │ +
gtsam::AcceleratedPowerMethod
Compute maximum Eigenpair with accelerated power method.
Definition AcceleratedPowerMethod.h:51
│ │ │ │ +
gtsam::AcceleratedPowerMethod::acceleratedPowerIteration
Vector acceleratedPowerIteration(const Vector &x1, const Vector &x0, const double beta) const
Run accelerated power iteration to get ritzVector with beta and previous two ritzVector x0 and x00,...
Definition AcceleratedPowerMethod.h:80
│ │ │ │ +
gtsam::AcceleratedPowerMethod::AcceleratedPowerMethod
AcceleratedPowerMethod(const Operator &A, const boost::optional< Vector > initial=boost::none, double initialBeta=0.0)
Constructor from aim matrix A (given as Matrix or Sparse), optional intial vector as ritzVector.
Definition AcceleratedPowerMethod.h:62
│ │ │ │ +
gtsam::AcceleratedPowerMethod::estimateBeta
double estimateBeta(const size_t T=10) const
Tuning the momentum beta using the Best Heavy Ball algorithm in Ref(3), T is the iteration time to fi...
Definition AcceleratedPowerMethod.h:101
│ │ │ │ +
gtsam::AcceleratedPowerMethod::acceleratedPowerIteration
Vector acceleratedPowerIteration() const
Run accelerated power iteration to get ritzVector with beta and previous two ritzVector x0 and x00,...
Definition AcceleratedPowerMethod.h:92
│ │ │ │ +
gtsam::AcceleratedPowerMethod::compute
bool compute(size_t maxIterations, double tol)
Start the accelerated iteration, after performing the accelerated iteration, calculate the ritz error...
Definition AcceleratedPowerMethod.h:156
│ │ │ │ +
gtsam::PowerMethod
Compute maximum Eigenpair with power method.
Definition PowerMethod.h:57
│ │ │ │ +
gtsam::PowerMethod::A_
const Operator & A_
Const reference to an externally-held matrix whose minimum-eigenvalue we want to compute.
Definition PowerMethod.h:63
│ │ │ │ +
gtsam::PowerMethod::converged
bool converged(double tol) const
After Perform power iteration on a single Ritz value, check if the Ritz residual for the current Ritz...
Definition PowerMethod.h:112
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,493 +1,186 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GaussianFactorGraph.h │ │ │ │ │ +AcceleratedPowerMethod.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -22#pragma once │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_P_o_w_e_r_M_e_t_h_o_d_._h> │ │ │ │ │ 23 │ │ │ │ │ -24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_E_r_r_o_r_s_._h> // Included here instead of fw-declared so │ │ │ │ │ -we can use Errors::iterator │ │ │ │ │ -27#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ -28#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_H_e_s_s_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ -29#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ -30#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ -31 │ │ │ │ │ -32namespace _g_t_s_a_m { │ │ │ │ │ -33 │ │ │ │ │ -34 // Forward declarations │ │ │ │ │ -35 class GaussianFactorGraph; │ │ │ │ │ -36 class GaussianFactor; │ │ │ │ │ -37 class _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l; │ │ │ │ │ -38 class GaussianBayesNet; │ │ │ │ │ -39 class GaussianEliminationTree; │ │ │ │ │ -40 class GaussianBayesTree; │ │ │ │ │ -41 class GaussianJunctionTree; │ │ │ │ │ -42 │ │ │ │ │ -43 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -_4_4 template<> struct _E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s<_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h> │ │ │ │ │ -45 { │ │ │ │ │ -_4_6 typedef _G_a_u_s_s_i_a_n_F_a_c_t_o_r _F_a_c_t_o_r_T_y_p_e; │ │ │ │ │ -_4_7 typedef _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e; │ │ │ │ │ -_4_8 typedef _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l _C_o_n_d_i_t_i_o_n_a_l_T_y_p_e; │ │ │ │ │ -_4_9 typedef _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t _B_a_y_e_s_N_e_t_T_y_p_e; │ │ │ │ │ -_5_0 typedef _G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_T_y_p_e; │ │ │ │ │ -_5_1 typedef _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e _B_a_y_e_s_T_r_e_e_T_y_p_e; │ │ │ │ │ -_5_2 typedef _G_a_u_s_s_i_a_n_J_u_n_c_t_i_o_n_T_r_e_e _J_u_n_c_t_i_o_n_T_r_e_e_T_y_p_e; │ │ │ │ │ -54 static std::pair, boost:: │ │ │ │ │ -shared_ptr > │ │ │ │ │ -_5_5 _D_e_f_a_u_l_t_E_l_i_m_i_n_a_t_e(const _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e& factors, const _O_r_d_e_r_i_n_g& keys) { │ │ │ │ │ -56 return _E_l_i_m_i_n_a_t_e_P_r_e_f_e_r_C_h_o_l_e_s_k_y(factors, keys); } │ │ │ │ │ -_5_8 static _O_r_d_e_r_i_n_g _D_e_f_a_u_l_t_O_r_d_e_r_i_n_g_F_u_n_c( │ │ │ │ │ -59 const _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e& graph, │ │ │ │ │ -60 boost::optional variableIndex) { │ │ │ │ │ -61 return _O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d(*variableIndex); │ │ │ │ │ -62 } │ │ │ │ │ -63 }; │ │ │ │ │ -64 │ │ │ │ │ -65 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -_7_2 class GTSAM_EXPORT _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h : │ │ │ │ │ -73 public _F_a_c_t_o_r_G_r_a_p_h, │ │ │ │ │ -74 public _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -75 { │ │ │ │ │ -76 public: │ │ │ │ │ -77 │ │ │ │ │ -_7_8 typedef _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h _T_h_i_s; │ │ │ │ │ -_7_9 typedef _F_a_c_t_o_r_G_r_a_p_h_<_G_a_u_s_s_i_a_n_F_a_c_t_o_r_> _B_a_s_e; │ │ │ │ │ -_8_0 typedef _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_T_h_i_s_> _B_a_s_e_E_l_i_m_i_n_a_t_e_a_b_l_e; │ │ │ │ │ -_8_1 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -82 │ │ │ │ │ -85 │ │ │ │ │ -_8_7 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h() {} │ │ │ │ │ -88 │ │ │ │ │ -_9_4 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h(std::initializer_list factors) : _B_a_s_e │ │ │ │ │ -(factors) {} │ │ │ │ │ -95 │ │ │ │ │ +24namespace _g_t_s_a_m { │ │ │ │ │ +25 │ │ │ │ │ +26using Sparse = Eigen::SparseMatrix; │ │ │ │ │ +27 │ │ │ │ │ +50template │ │ │ │ │ +_5_1class _A_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_M_e_t_h_o_d : public _P_o_w_e_r_M_e_t_h_o_d { │ │ │ │ │ +52 │ │ │ │ │ +53 double beta_ = 0; // a Polyak momentum term │ │ │ │ │ +54 │ │ │ │ │ +55 Vector previousVector_; // store previous vector │ │ │ │ │ +56 │ │ │ │ │ +57 public: │ │ │ │ │ +_6_2 explicit _A_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_M_e_t_h_o_d( │ │ │ │ │ +63 const Operator &A, const boost::optional initial = boost::none, │ │ │ │ │ +64 double initialBeta = 0.0) │ │ │ │ │ +65 : _P_o_w_e_r_M_e_t_h_o_d(A, initial) { │ │ │ │ │ +66 // initialize Ritz eigen vector and previous vector │ │ │ │ │ +67 this->ritzVector_ = initial ? initial.get() : Vector::Random(this->dim_); │ │ │ │ │ +68 this->ritzVector_.normalize(); │ │ │ │ │ +69 previousVector_ = Vector::Zero(this->dim_); │ │ │ │ │ +70 │ │ │ │ │ +71 // initialize beta_ │ │ │ │ │ +72 beta_ = initialBeta; │ │ │ │ │ +73 } │ │ │ │ │ +74 │ │ │ │ │ +_8_0 Vector _a_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_I_t_e_r_a_t_i_o_n (const Vector &x1, const Vector &x0, │ │ │ │ │ +81 const double beta) const { │ │ │ │ │ +82 Vector y = this->_A__ * x1 - beta * x0; │ │ │ │ │ +83 y.normalize(); │ │ │ │ │ +84 return y; │ │ │ │ │ +85 } │ │ │ │ │ +86 │ │ │ │ │ +_9_2 Vector _a_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_I_t_e_r_a_t_i_o_n () const { │ │ │ │ │ +93 Vector y = _a_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_I_t_e_r_a_t_i_o_n(this->ritzVector_, previousVector_, │ │ │ │ │ +beta_); │ │ │ │ │ +94 return y; │ │ │ │ │ +95 } │ │ │ │ │ 96 │ │ │ │ │ -98 template │ │ │ │ │ -_9_9 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h(ITERATOR firstFactor, ITERATOR lastFactor) : _B_a_s_e │ │ │ │ │ -(firstFactor, lastFactor) {} │ │ │ │ │ -100 │ │ │ │ │ -102 template │ │ │ │ │ -_1_0_3 explicit _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h(const CONTAINER& factors) : _B_a_s_e(factors) {} │ │ │ │ │ -104 │ │ │ │ │ -106 template │ │ │ │ │ -_1_0_7 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h(const _F_a_c_t_o_r_G_r_a_p_h_<_D_E_R_I_V_E_D_F_A_C_T_O_R_>& graph) : _B_a_s_e(graph) │ │ │ │ │ -{} │ │ │ │ │ -108 │ │ │ │ │ -_1_1_0 virtual _~_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h() {} │ │ │ │ │ -111 │ │ │ │ │ -115 │ │ │ │ │ -116 bool _e_q_u_a_l_s(const This& fg, double tol = 1e-9) const; │ │ │ │ │ -117 │ │ │ │ │ -119 │ │ │ │ │ -_1_2_1 friend bool _o_p_e_r_a_t_o_r_=_=(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& lhs, │ │ │ │ │ -122 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& rhs) { │ │ │ │ │ -123 return lhs._i_s_E_q_u_a_l(rhs); │ │ │ │ │ -124 } │ │ │ │ │ -125 │ │ │ │ │ -_1_2_7 void _a_d_d(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r& factor) { push_back(factor._c_l_o_n_e()); } │ │ │ │ │ -128 │ │ │ │ │ -_1_3_0 void _a_d_d(const _s_h_a_r_e_d_F_a_c_t_o_r& factor) { push_back(factor); } │ │ │ │ │ -131 │ │ │ │ │ -_1_3_3 void _a_d_d(const Vector& b) { │ │ │ │ │ -134 add(_J_a_c_o_b_i_a_n_F_a_c_t_o_r(b)); } │ │ │ │ │ -135 │ │ │ │ │ -_1_3_7 void _a_d_d(_K_e_y key1, const Matrix& A1, │ │ │ │ │ -138 const Vector& b, const SharedDiagonal& model = SharedDiagonal()) { │ │ │ │ │ -139 add(_J_a_c_o_b_i_a_n_F_a_c_t_o_r(key1,A1,b,model)); } │ │ │ │ │ -140 │ │ │ │ │ -_1_4_2 void _a_d_d(_K_e_y key1, const Matrix& A1, │ │ │ │ │ -143 _K_e_y key2, const Matrix& A2, │ │ │ │ │ -144 const Vector& b, const SharedDiagonal& model = SharedDiagonal()) { │ │ │ │ │ -145 add(_J_a_c_o_b_i_a_n_F_a_c_t_o_r(key1,A1,key2,A2,b,model)); } │ │ │ │ │ -146 │ │ │ │ │ -_1_4_8 void _a_d_d(_K_e_y key1, const Matrix& A1, │ │ │ │ │ -149 _K_e_y key2, const Matrix& A2, │ │ │ │ │ -150 _K_e_y key3, const Matrix& A3, │ │ │ │ │ -151 const Vector& b, const SharedDiagonal& model = SharedDiagonal()) { │ │ │ │ │ -152 add(_J_a_c_o_b_i_a_n_F_a_c_t_o_r(key1,A1,key2,A2,key3,A3,b,model)); } │ │ │ │ │ -153 │ │ │ │ │ -155 template │ │ │ │ │ -_1_5_6 void _a_d_d(const TERMS& terms, const Vector &b, const SharedDiagonal& model = │ │ │ │ │ -SharedDiagonal()) { │ │ │ │ │ -157 add(_J_a_c_o_b_i_a_n_F_a_c_t_o_r(terms,b,model)); } │ │ │ │ │ -158 │ │ │ │ │ -_1_6_3 typedef _K_e_y_S_e_t _K_e_y_s; │ │ │ │ │ -164 _K_e_y_s keys() const; │ │ │ │ │ -165 │ │ │ │ │ -166 /* return a map of (Key, dimension) */ │ │ │ │ │ -167 std::map getKeyDimMap() const; │ │ │ │ │ -168 │ │ │ │ │ -170 double error(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const; │ │ │ │ │ +_1_0_1 double _e_s_t_i_m_a_t_e_B_e_t_a(const size_t T = 10) const { │ │ │ │ │ +102 // set initial estimation of maxBeta │ │ │ │ │ +103 Vector initVector = this->ritzVector_; │ │ │ │ │ +104 const double up = initVector.dot( this->_A__ * initVector ); │ │ │ │ │ +105 const double down = initVector.dot(initVector); │ │ │ │ │ +106 const double mu = up / down; │ │ │ │ │ +107 double maxBeta = mu * mu / 4; │ │ │ │ │ +108 size_t maxIndex; │ │ │ │ │ +109 std::vector betas; │ │ │ │ │ +110 │ │ │ │ │ +111 Matrix R = Matrix::Zero(this->dim_, 10); │ │ │ │ │ +112 // run T times of iteration to find the beta that has the largest Rayleigh │ │ │ │ │ +quotient │ │ │ │ │ +113 for (size_t t = 0; t < T; t++) { │ │ │ │ │ +114 // after each t iteration, reset the betas with the current maxBeta │ │ │ │ │ +115 betas = {2 / 3 * maxBeta, 0.99 * maxBeta, maxBeta, 1.01 * maxBeta, │ │ │ │ │ +116 1.5 * maxBeta}; │ │ │ │ │ +117 // iterate through every beta value │ │ │ │ │ +118 for (size_t k = 0; k < betas.size(); ++k) { │ │ │ │ │ +119 // initialize x0 and x00 in each iteration of each beta │ │ │ │ │ +120 Vector x0 = initVector; │ │ │ │ │ +121 Vector x00 = Vector::Zero(this->dim_); │ │ │ │ │ +122 // run 10 steps of accelerated power iteration with this beta │ │ │ │ │ +123 for (size_t j = 1; j < 10; j++) { │ │ │ │ │ +124 if (j < 2) { │ │ │ │ │ +125 R.col(0) = _a_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_I_t_e_r_a_t_i_o_n(x0, x00, betas[k]); │ │ │ │ │ +126 R.col(1) = _a_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_I_t_e_r_a_t_i_o_n(R.col(0), x0, betas[k]); │ │ │ │ │ +127 } else { │ │ │ │ │ +128 R.col(j) = _a_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_I_t_e_r_a_t_i_o_n(R.col(j - 1), R.col(j - 2), │ │ │ │ │ +129 betas[k]); │ │ │ │ │ +130 } │ │ │ │ │ +131 } │ │ │ │ │ +132 // compute the Rayleigh quotient for the randomly sampled vector after │ │ │ │ │ +133 // 10 steps of power accelerated iteration │ │ │ │ │ +134 const Vector x = R.col(9); │ │ │ │ │ +135 const double up = x.dot(this->_A__ * x); │ │ │ │ │ +136 const double down = x.dot(x); │ │ │ │ │ +137 const double mu = up / down; │ │ │ │ │ +138 // store the momentum with largest Rayleigh quotient and its according │ │ │ │ │ +index of beta_ │ │ │ │ │ +139 if (mu * mu / 4 > maxBeta) { │ │ │ │ │ +140 // save the max beta index │ │ │ │ │ +141 maxIndex = k; │ │ │ │ │ +142 maxBeta = mu * mu / 4; │ │ │ │ │ +143 } │ │ │ │ │ +144 } │ │ │ │ │ +145 } │ │ │ │ │ +146 // set beta_ to momentum with largest Rayleigh quotient │ │ │ │ │ +147 return betas[maxIndex]; │ │ │ │ │ +148 } │ │ │ │ │ +149 │ │ │ │ │ +_1_5_6 bool _c_o_m_p_u_t_e(size_t maxIterations, double tol) { │ │ │ │ │ +157 // Starting │ │ │ │ │ +158 bool isConverged = false; │ │ │ │ │ +159 │ │ │ │ │ +160 for (size_t i = 0; i < maxIterations && !isConverged; i++) { │ │ │ │ │ +161 ++(this->nrIterations_); │ │ │ │ │ +162 Vector tmp = this->ritzVector_; │ │ │ │ │ +163 // update the ritzVector after accelerated power iteration │ │ │ │ │ +164 this->ritzVector_ = _a_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_I_t_e_r_a_t_i_o_n(); │ │ │ │ │ +165 // update the previousVector with ritzVector │ │ │ │ │ +166 previousVector_ = tmp; │ │ │ │ │ +167 // update the ritzValue │ │ │ │ │ +168 this->ritzValue_ = this->ritzVector_.dot(this->_A__ * this->ritzVector_); │ │ │ │ │ +169 isConverged = this->_c_o_n_v_e_r_g_e_d(tol); │ │ │ │ │ +170 } │ │ │ │ │ 171 │ │ │ │ │ -173 double probPrime(const _V_e_c_t_o_r_V_a_l_u_e_s& c) const; │ │ │ │ │ -174 │ │ │ │ │ -180 virtual _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h clone() const; │ │ │ │ │ -181 │ │ │ │ │ -186 virtual _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r cloneToPtr() const; │ │ │ │ │ -187 │ │ │ │ │ -194 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h negate() const; │ │ │ │ │ -195 │ │ │ │ │ -198 │ │ │ │ │ -209 std::vector > sparseJacobian( │ │ │ │ │ -210 const _O_r_d_e_r_i_n_g& ordering, size_t& nrows, size_t& ncols) const; │ │ │ │ │ -211 │ │ │ │ │ -213 std::vector > sparseJacobian() const; │ │ │ │ │ -214 │ │ │ │ │ -221 Matrix sparseJacobian_() const; │ │ │ │ │ -222 │ │ │ │ │ -230 Matrix augmentedJacobian(const _O_r_d_e_r_i_n_g& ordering) const; │ │ │ │ │ -231 │ │ │ │ │ -239 Matrix augmentedJacobian() const; │ │ │ │ │ -240 │ │ │ │ │ -248 std::pair jacobian(const _O_r_d_e_r_i_n_g& ordering) const; │ │ │ │ │ -249 │ │ │ │ │ -257 std::pair jacobian() const; │ │ │ │ │ -258 │ │ │ │ │ -270 Matrix augmentedHessian(const _O_r_d_e_r_i_n_g& ordering) const; │ │ │ │ │ -271 │ │ │ │ │ -283 Matrix augmentedHessian() const; │ │ │ │ │ -284 │ │ │ │ │ -291 std::pair hessian(const _O_r_d_e_r_i_n_g& ordering) const; │ │ │ │ │ -292 │ │ │ │ │ -299 std::pair hessian() const; │ │ │ │ │ -300 │ │ │ │ │ -302 virtual _V_e_c_t_o_r_V_a_l_u_e_s hessianDiagonal() const; │ │ │ │ │ -303 │ │ │ │ │ -305 virtual std::map hessianBlockDiagonal() const; │ │ │ │ │ -306 │ │ │ │ │ -311 _V_e_c_t_o_r_V_a_l_u_e_s _o_p_t_i_m_i_z_e( │ │ │ │ │ -312 const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; │ │ │ │ │ -313 │ │ │ │ │ -318 _V_e_c_t_o_r_V_a_l_u_e_s _o_p_t_i_m_i_z_e(const _O_r_d_e_r_i_n_g&, │ │ │ │ │ -319 const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; │ │ │ │ │ -320 │ │ │ │ │ -324 _V_e_c_t_o_r_V_a_l_u_e_s optimizeDensely() const; │ │ │ │ │ -325 │ │ │ │ │ -335 _V_e_c_t_o_r_V_a_l_u_e_s gradient(const _V_e_c_t_o_r_V_a_l_u_e_s& x0) const; │ │ │ │ │ -336 │ │ │ │ │ -344 virtual _V_e_c_t_o_r_V_a_l_u_e_s gradientAtZero() const; │ │ │ │ │ -345 │ │ │ │ │ -370 _V_e_c_t_o_r_V_a_l_u_e_s optimizeGradientSearch() const; │ │ │ │ │ -371 │ │ │ │ │ -373 _V_e_c_t_o_r_V_a_l_u_e_s transposeMultiply(const _E_r_r_o_r_s& e) const; │ │ │ │ │ -374 │ │ │ │ │ -376 void transposeMultiplyAdd(double alpha, const _E_r_r_o_r_s& e, _V_e_c_t_o_r_V_a_l_u_e_s& x) │ │ │ │ │ -const; │ │ │ │ │ -377 │ │ │ │ │ -379 _E_r_r_o_r_s gaussianErrors(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const; │ │ │ │ │ -380 │ │ │ │ │ -382 _E_r_r_o_r_s _o_p_e_r_a_t_o_r_*(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const; │ │ │ │ │ -383 │ │ │ │ │ -385 void multiplyHessianAdd(double alpha, const _V_e_c_t_o_r_V_a_l_u_e_s& x, │ │ │ │ │ -386 _V_e_c_t_o_r_V_a_l_u_e_s& y) const; │ │ │ │ │ -387 │ │ │ │ │ -389 void multiplyInPlace(const _V_e_c_t_o_r_V_a_l_u_e_s& x, _E_r_r_o_r_s& e) const; │ │ │ │ │ -390 │ │ │ │ │ -392 void multiplyInPlace(const _V_e_c_t_o_r_V_a_l_u_e_s& x, const Errors::iterator& e) │ │ │ │ │ -const; │ │ │ │ │ -393 │ │ │ │ │ -394 void printErrors( │ │ │ │ │ -395 const _V_e_c_t_o_r_V_a_l_u_e_s& x, │ │ │ │ │ -396 const std::string& str = "GaussianFactorGraph: ", │ │ │ │ │ -397 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ -398 const std::function& │ │ │ │ │ -400 printCondition = │ │ │ │ │ -401 [](const _F_a_c_t_o_r*, double, size_t) { return true; }) const; │ │ │ │ │ -403 │ │ │ │ │ -404 private: │ │ │ │ │ -_4_0_6 friend class boost::serialization::access; │ │ │ │ │ -407 template │ │ │ │ │ -408 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -409 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -410 } │ │ │ │ │ -411 │ │ │ │ │ -412 public: │ │ │ │ │ -413 │ │ │ │ │ -414#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ -416 _V_e_c_t_o_r_V_a_l_u_e_s GTSAM_DEPRECATED │ │ │ │ │ -417 _o_p_t_i_m_i_z_e(boost::none_t, const Eliminate& function = │ │ │ │ │ -418 EliminationTraitsType::DefaultEliminate) const { │ │ │ │ │ -419 return _o_p_t_i_m_i_z_e(function); │ │ │ │ │ -420 } │ │ │ │ │ -421#endif │ │ │ │ │ -422 │ │ │ │ │ -423 }; │ │ │ │ │ -424 │ │ │ │ │ -429 GTSAM_EXPORT bool _h_a_s_C_o_n_s_t_r_a_i_n_t_s(const GaussianFactorGraph& factors); │ │ │ │ │ -430 │ │ │ │ │ -431 /****** Linear Algebra Operations ******/ │ │ │ │ │ -432 │ │ │ │ │ -434 //GTSAM_EXPORT void residual(const GaussianFactorGraph& fg, const │ │ │ │ │ -VectorValues &x, VectorValues &r); │ │ │ │ │ -435 //GTSAM_EXPORT void multiply(const GaussianFactorGraph& fg, const │ │ │ │ │ -VectorValues &x, VectorValues &r); │ │ │ │ │ -436 │ │ │ │ │ -438template<> │ │ │ │ │ -_4_3_9struct _t_r_a_i_t_s<_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h> : public _T_e_s_t_a_b_l_e { │ │ │ │ │ -440}; │ │ │ │ │ -441 │ │ │ │ │ -442} // \ namespace gtsam │ │ │ │ │ -_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Variable elimination algorithms for factor graphs. │ │ │ │ │ -_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Factor Graph Base Class. │ │ │ │ │ -_H_e_s_s_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ -Contains the HessianFactor class, a general quadratic factor. │ │ │ │ │ -_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ -_G_a_u_s_s_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ -A factor with a quadratic error function - a Gaussian. │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ -Factor Graph Values. │ │ │ │ │ -_E_r_r_o_r_s_._h │ │ │ │ │ -vector of errors │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_P_r_e_f_e_r_C_h_o_l_e_s_k_y │ │ │ │ │ -std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< │ │ │ │ │ -GaussianFactor > > EliminatePreferCholesky(const GaussianFactorGraph &factors, │ │ │ │ │ -const Ordering &keys) │ │ │ │ │ -Densely partially eliminate with Cholesky factorization. │ │ │ │ │ -DDeeffiinniittiioonn HessianFactor.cpp:548 │ │ │ │ │ +172 return isConverged; │ │ │ │ │ +173 } │ │ │ │ │ +174}; │ │ │ │ │ +175 │ │ │ │ │ +176} // namespace gtsam │ │ │ │ │ +_P_o_w_e_r_M_e_t_h_o_d_._h │ │ │ │ │ +Power method for fast eigenvalue and eigenvector computation. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_h_a_s_C_o_n_s_t_r_a_i_n_t_s │ │ │ │ │ -bool hasConstraints(const GaussianFactorGraph &factors) │ │ │ │ │ -Evaluates whether linear factors have any constrained noise models. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.cpp:442 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ -Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key │ │ │ │ │ -landmarkKey) │ │ │ │ │ -Optimize for triangulation. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.cpp:155 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ -Point2 operator*(double s, const Point2 &p) │ │ │ │ │ -multiply with scalar │ │ │ │ │ -DDeeffiinniittiioonn Point2.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t_<_ _V_e_c_t_o_r_ _> │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ -nodes. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_i_s_E_q_u_a_l │ │ │ │ │ -bool isEqual(const FactorGraph &other) const │ │ │ │ │ -Check exact equality of the factor pointers. Useful for derived ==. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:134 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_ _>_:_:_s_h_a_r_e_d_F_a_c_t_o_r │ │ │ │ │ -boost::shared_ptr< GaussianFactor > sharedFactor │ │ │ │ │ -Shared pointer to a factor. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:101 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s │ │ │ │ │ -Traits class for eliminateable factor graphs, specifies the types that result │ │ │ │ │ -from elimination,... │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -EliminateableFactorGraph is a base class for factor graphs that contains │ │ │ │ │ -elimination algorithms. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:68 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d │ │ │ │ │ -static Ordering Colamd(const FACTOR_GRAPH &graph) │ │ │ │ │ -Compute a fill-reducing ordering using COLAMD from a factor graph (see details │ │ │ │ │ -for note on performanc... │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:95 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t │ │ │ │ │ -GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals. │ │ │ │ │ -DDeeffiinniittiioonn GaussianBayesNet.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e │ │ │ │ │ -A Bayes tree representing a Gaussian density. │ │ │ │ │ -DDeeffiinniittiioonn GaussianBayesTree.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -A GaussianConditional functions as the node in a Bayes network. │ │ │ │ │ -DDeeffiinniittiioonn GaussianConditional.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ -DDeeffiinniittiioonn GaussianEliminationTree.h:29 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -An abstract virtual base class for JacobianFactor and HessianFactor. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactor.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ -virtual GaussianFactor::shared_ptr clone() const =0 │ │ │ │ │ -Clone a factor (make a deep copy) │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_D_e_f_a_u_l_t_O_r_d_e_r_i_n_g_F_u_n_c │ │ │ │ │ -static Ordering DefaultOrderingFunc(const FactorGraphType &graph, boost:: │ │ │ │ │ -optional< const VariableIndex & > variableIndex) │ │ │ │ │ -The default ordering generation function. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_B_a_y_e_s_T_r_e_e_T_y_p_e │ │ │ │ │ -GaussianBayesTree BayesTreeType │ │ │ │ │ -Type of Bayes tree. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:51 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_C_o_n_d_i_t_i_o_n_a_l_T_y_p_e │ │ │ │ │ -GaussianConditional ConditionalType │ │ │ │ │ -Type of conditionals from elimination. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_F_a_c_t_o_r_T_y_p_e │ │ │ │ │ -GaussianFactor FactorType │ │ │ │ │ -Type of factors in factor graph. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:46 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_T_y_p_e │ │ │ │ │ -GaussianEliminationTree EliminationTreeType │ │ │ │ │ -Type of elimination tree. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_F_a_c_t_o_r_G_r_a_p_h_T_y_p_e │ │ │ │ │ -GaussianFactorGraph FactorGraphType │ │ │ │ │ -Type of the factor graph (e.g. GaussianFactorGraph) │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_B_a_y_e_s_N_e_t_T_y_p_e │ │ │ │ │ -GaussianBayesNet BayesNetType │ │ │ │ │ -Type of Bayes net from sequential elimination. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:49 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_J_u_n_c_t_i_o_n_T_r_e_e_T_y_p_e │ │ │ │ │ -GaussianJunctionTree JunctionTreeType │ │ │ │ │ -Type of Junction tree. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_D_e_f_a_u_l_t_E_l_i_m_i_n_a_t_e │ │ │ │ │ -static std::pair< boost::shared_ptr< ConditionalType >, boost::shared_ptr< │ │ │ │ │ -FactorType > > DefaultEliminate(const FactorGraphType &factors, const Ordering │ │ │ │ │ -&keys) │ │ │ │ │ -The default dense elimination function. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_B_a_s_e_E_l_i_m_i_n_a_t_e_a_b_l_e │ │ │ │ │ -EliminateableFactorGraph< This > BaseEliminateable │ │ │ │ │ -Typedef to base elimination class. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:80 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:81 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d │ │ │ │ │ -void add(const TERMS &terms, const Vector &b, const SharedDiagonal │ │ │ │ │ -&model=SharedDiagonal()) │ │ │ │ │ -Add an n-ary factor. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:156 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -GaussianFactorGraph(std::initializer_list< sharedFactor > factors) │ │ │ │ │ -Construct from an initializer lists of GaussianFactor shared pointers. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:94 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -GaussianFactorGraph() │ │ │ │ │ -Default constructor. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:87 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d │ │ │ │ │ -void add(const GaussianFactor &factor) │ │ │ │ │ -Add a factor by value - makes a copy. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:127 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d │ │ │ │ │ -void add(Key key1, const Matrix &A1, Key key2, const Matrix &A2, Key key3, │ │ │ │ │ -const Matrix &A3, const Vector &b, const SharedDiagonal &model=SharedDiagonal │ │ │ │ │ -()) │ │ │ │ │ -Add a ternary factor. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:148 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d │ │ │ │ │ -void add(const sharedFactor &factor) │ │ │ │ │ -Add a factor by pointer - stores pointer without copying the factor. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:130 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_o_p_e_r_a_t_o_r_=_= │ │ │ │ │ -friend bool operator==(const GaussianFactorGraph &lhs, const │ │ │ │ │ -GaussianFactorGraph &rhs) │ │ │ │ │ -Check exact equality. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:121 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_T_h_i_s │ │ │ │ │ -GaussianFactorGraph This │ │ │ │ │ -Typedef to this class. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:78 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_K_e_y_s │ │ │ │ │ -KeySet Keys │ │ │ │ │ -Return the set of variables involved in the factors (computes a set union). │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:163 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d │ │ │ │ │ -void add(const Vector &b) │ │ │ │ │ -Add a null factor. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:133 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d │ │ │ │ │ -void add(Key key1, const Matrix &A1, const Vector &b, const SharedDiagonal │ │ │ │ │ -&model=SharedDiagonal()) │ │ │ │ │ -Add a unary factor. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:137 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_~_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -virtual ~GaussianFactorGraph() │ │ │ │ │ -Virtual destructor. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:110 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d │ │ │ │ │ -void add(Key key1, const Matrix &A1, Key key2, const Matrix &A2, const Vector │ │ │ │ │ -&b, const SharedDiagonal &model=SharedDiagonal()) │ │ │ │ │ -Add a binary factor. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:142 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -GaussianFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) │ │ │ │ │ -Construct from iterator over factors. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:99 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_B_a_s_e │ │ │ │ │ -FactorGraph< GaussianFactor > Base │ │ │ │ │ -Typedef to base factor graph type. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:79 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -GaussianFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph) │ │ │ │ │ -Implicit copy/downcast constructor to override explicit template container │ │ │ │ │ -constructor. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:107 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -GaussianFactorGraph(const CONTAINER &factors) │ │ │ │ │ -Construct from container of factors (shared_ptr or plain objects) │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:103 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ -A junction tree specialized to Gaussian factors, i.e., it is a cluster tree │ │ │ │ │ -with Gaussian factors sto... │ │ │ │ │ -DDeeffiinniittiioonn GaussianJunctionTree.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ -A Gaussian factor in the squared-error form. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:91 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ -with a unique integer... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ -_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -is the normalization constant. │ │ │ │ │ +_g_t_s_a_m_:_:_A_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_M_e_t_h_o_d │ │ │ │ │ +Compute maximum Eigenpair with accelerated power method. │ │ │ │ │ +DDeeffiinniittiioonn AcceleratedPowerMethod.h:51 │ │ │ │ │ +_g_t_s_a_m_:_:_A_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_M_e_t_h_o_d_:_:_a_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_I_t_e_r_a_t_i_o_n │ │ │ │ │ +Vector acceleratedPowerIteration(const Vector &x1, const Vector &x0, const │ │ │ │ │ +double beta) const │ │ │ │ │ +Run accelerated power iteration to get ritzVector with beta and previous two │ │ │ │ │ +ritzVector x0 and x00,... │ │ │ │ │ +DDeeffiinniittiioonn AcceleratedPowerMethod.h:80 │ │ │ │ │ +_g_t_s_a_m_:_:_A_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_M_e_t_h_o_d_:_:_A_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_M_e_t_h_o_d │ │ │ │ │ +AcceleratedPowerMethod(const Operator &A, const boost::optional< Vector > │ │ │ │ │ +initial=boost::none, double initialBeta=0.0) │ │ │ │ │ +Constructor from aim matrix A (given as Matrix or Sparse), optional intial │ │ │ │ │ +vector as ritzVector. │ │ │ │ │ +DDeeffiinniittiioonn AcceleratedPowerMethod.h:62 │ │ │ │ │ +_g_t_s_a_m_:_:_A_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_M_e_t_h_o_d_:_:_e_s_t_i_m_a_t_e_B_e_t_a │ │ │ │ │ +double estimateBeta(const size_t T=10) const │ │ │ │ │ +Tuning the momentum beta using the Best Heavy Ball algorithm in Ref(3), T is │ │ │ │ │ +the iteration time to fi... │ │ │ │ │ +DDeeffiinniittiioonn AcceleratedPowerMethod.h:101 │ │ │ │ │ +_g_t_s_a_m_:_:_A_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_M_e_t_h_o_d_:_:_a_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_I_t_e_r_a_t_i_o_n │ │ │ │ │ +Vector acceleratedPowerIteration() const │ │ │ │ │ +Run accelerated power iteration to get ritzVector with beta and previous two │ │ │ │ │ +ritzVector x0 and x00,... │ │ │ │ │ +DDeeffiinniittiioonn AcceleratedPowerMethod.h:92 │ │ │ │ │ +_g_t_s_a_m_:_:_A_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_M_e_t_h_o_d_:_:_c_o_m_p_u_t_e │ │ │ │ │ +bool compute(size_t maxIterations, double tol) │ │ │ │ │ +Start the accelerated iteration, after performing the accelerated iteration, │ │ │ │ │ +calculate the ritz error... │ │ │ │ │ +DDeeffiinniittiioonn AcceleratedPowerMethod.h:156 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_w_e_r_M_e_t_h_o_d │ │ │ │ │ +Compute maximum Eigenpair with power method. │ │ │ │ │ +DDeeffiinniittiioonn PowerMethod.h:57 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_w_e_r_M_e_t_h_o_d_:_:_A__ │ │ │ │ │ +const Operator & A_ │ │ │ │ │ +Const reference to an externally-held matrix whose minimum-eigenvalue we want │ │ │ │ │ +to compute. │ │ │ │ │ +DDeeffiinniittiioonn PowerMethod.h:63 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_w_e_r_M_e_t_h_o_d_:_:_c_o_n_v_e_r_g_e_d │ │ │ │ │ +bool converged(double tol) const │ │ │ │ │ +After Perform power iteration on a single Ritz value, check if the Ritz │ │ │ │ │ +residual for the current Ritz... │ │ │ │ │ +DDeeffiinniittiioonn PowerMethod.h:112 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ + * _A_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_M_e_t_h_o_d_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00905.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/PreintegrationParams.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/PreintegratedRotation.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,24 +96,29 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
PreintegrationParams.h File Reference
│ │ │ │ +
PreintegratedRotation.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

struct  gtsam::PreintegrationParams
 Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the constructor. More...
struct  gtsam::PreintegratedRotationParams
 Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the constructor. More...
 
class  gtsam::PreintegratedRotation
 PreintegratedRotation is the base class for all PreintegratedMeasurements classes (in AHRSFactor, ImuFactor, and CombinedImuFactor). More...
 
struct  gtsam::traits< PreintegratedRotation >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -125,21 +130,19 @@ │ │ │ │
│ │ │ │ Richard Roberts
│ │ │ │
│ │ │ │ Vadim Indelman
│ │ │ │
│ │ │ │ David Jensen
│ │ │ │
│ │ │ │ -Frank Dellaert
│ │ │ │ -
│ │ │ │ Frank Dellaert
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,30 +1,36 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -PreintegrationParams.h File Reference │ │ │ │ │ +PreintegratedRotation.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ +struct   _g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s │ │ │ │ │   Parameters for pre-integration: Usage: Create just a single Params and │ │ │ │ │ pass a shared pointer to the constructor. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ + class   _g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n │ │ │ │ │ + _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n is the base class for all │ │ │ │ │ +  PreintegratedMeasurements classes (in _A_H_R_S_F_a_c_t_o_r, _I_m_u_F_a_c_t_o_r, and │ │ │ │ │ + _C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r). _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_ _> │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Author │ │ │ │ │ Luca Carlone │ │ │ │ │ Stephen Williams │ │ │ │ │ Richard Roberts │ │ │ │ │ Vadim Indelman │ │ │ │ │ David Jensen │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s_._h │ │ │ │ │ + * _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00905.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,5 @@ │ │ │ │ │ var a00905 = [ │ │ │ │ │ - ["gtsam::PreintegrationParams", "a04220.html", "a04220"] │ │ │ │ │ + ["gtsam::PreintegratedRotationParams", "a04204.html", "a04204"], │ │ │ │ │ + ["gtsam::PreintegratedRotation", "a04208.html", "a04208"], │ │ │ │ │ + ["gtsam::traits< PreintegratedRotation >", "a04212.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00905_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/PreintegrationParams.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/PreintegratedRotation.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,120 +98,203 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
PreintegrationParams.h
│ │ │ │ +
PreintegratedRotation.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
17#pragma once
│ │ │ │ -
18
│ │ │ │ -
19#include <gtsam/navigation/PreintegratedRotation.h>
│ │ │ │ -
20#include <boost/make_shared.hpp>
│ │ │ │ -
21
│ │ │ │ -
22namespace gtsam {
│ │ │ │ +
22#pragma once
│ │ │ │
23
│ │ │ │ -
│ │ │ │ -
26struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
│ │ │ │ -
29 Matrix3 accelerometerCovariance;
│ │ │ │ -
30 Matrix3 integrationCovariance;
│ │ │ │ -
31 bool use2ndOrderCoriolis;
│ │ │ │ -
32 Vector3 n_gravity;
│ │ │ │ -
33
│ │ │ │ -
│ │ │ │ -
35 PreintegrationParams()
│ │ │ │ -
36 : PreintegratedRotationParams(),
│ │ │ │ -
37 accelerometerCovariance(I_3x3),
│ │ │ │ -
38 integrationCovariance(I_3x3),
│ │ │ │ -
39 use2ndOrderCoriolis(false),
│ │ │ │ -
40 n_gravity(0, 0, -1) {}
│ │ │ │ -
│ │ │ │ -
41
│ │ │ │ -
│ │ │ │ -
44 PreintegrationParams(const Vector3& n_gravity)
│ │ │ │ -
45 : PreintegratedRotationParams(),
│ │ │ │ -
46 accelerometerCovariance(I_3x3),
│ │ │ │ -
47 integrationCovariance(I_3x3),
│ │ │ │ -
48 use2ndOrderCoriolis(false),
│ │ │ │ -
49 n_gravity(n_gravity) {}
│ │ │ │ -
│ │ │ │ -
50
│ │ │ │ -
51 // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
│ │ │ │ -
52 static boost::shared_ptr<PreintegrationParams> MakeSharedD(double g = 9.81) {
│ │ │ │ -
53 return boost::shared_ptr<PreintegrationParams>(new PreintegrationParams(Vector3(0, 0, g)));
│ │ │ │ -
54 }
│ │ │ │ +
24#include <gtsam/geometry/Pose3.h>
│ │ │ │ +
25#include <gtsam/base/Matrix.h>
│ │ │ │ +
26
│ │ │ │ +
27namespace gtsam {
│ │ │ │ +
28
│ │ │ │ +
│ │ │ │ +
31struct GTSAM_EXPORT PreintegratedRotationParams {
│ │ │ │ +
34 Matrix3 gyroscopeCovariance;
│ │ │ │ +
35 boost::optional<Vector3> omegaCoriolis;
│ │ │ │ +
36 boost::optional<Pose3> body_P_sensor;
│ │ │ │ +
37
│ │ │ │ +
38 PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {}
│ │ │ │ +
39
│ │ │ │ +
40 PreintegratedRotationParams(const Matrix3& gyroscope_covariance,
│ │ │ │ +
41 boost::optional<Vector3> omega_coriolis)
│ │ │ │ +
42 : gyroscopeCovariance(gyroscope_covariance) {
│ │ │ │ +
43 if (omega_coriolis)
│ │ │ │ +
44 omegaCoriolis.reset(omega_coriolis.get());
│ │ │ │ +
45 }
│ │ │ │ +
46
│ │ │ │ +
47 virtual ~PreintegratedRotationParams() {}
│ │ │ │ +
48
│ │ │ │ +
49 virtual void print(const std::string& s) const;
│ │ │ │ +
50 virtual bool equals(const PreintegratedRotationParams& other, double tol=1e-9) const;
│ │ │ │ +
51
│ │ │ │ +
52 void setGyroscopeCovariance(const Matrix3& cov) { gyroscopeCovariance = cov; }
│ │ │ │ +
53 void setOmegaCoriolis(const Vector3& omega) { omegaCoriolis.reset(omega); }
│ │ │ │ +
54 void setBodyPSensor(const Pose3& pose) { body_P_sensor.reset(pose); }
│ │ │ │
55
│ │ │ │ -
56 // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
│ │ │ │ -
57 static boost::shared_ptr<PreintegrationParams> MakeSharedU(double g = 9.81) {
│ │ │ │ -
58 return boost::shared_ptr<PreintegrationParams>(new PreintegrationParams(Vector3(0, 0, -g)));
│ │ │ │ -
59 }
│ │ │ │ -
60
│ │ │ │ -
61 void print(const std::string& s="") const override;
│ │ │ │ -
62 bool equals(const PreintegratedRotationParams& other, double tol) const override;
│ │ │ │ -
63
│ │ │ │ -
64 void setAccelerometerCovariance(const Matrix3& cov) { accelerometerCovariance = cov; }
│ │ │ │ -
65 void setIntegrationCovariance(const Matrix3& cov) { integrationCovariance = cov; }
│ │ │ │ -
66 void setUse2ndOrderCoriolis(bool flag) { use2ndOrderCoriolis = flag; }
│ │ │ │ -
67
│ │ │ │ -
68 const Matrix3& getAccelerometerCovariance() const { return accelerometerCovariance; }
│ │ │ │ -
69 const Matrix3& getIntegrationCovariance() const { return integrationCovariance; }
│ │ │ │ -
70 const Vector3& getGravity() const { return n_gravity; }
│ │ │ │ -
71 bool getUse2ndOrderCoriolis() const { return use2ndOrderCoriolis; }
│ │ │ │ -
72
│ │ │ │ -
73protected:
│ │ │ │ -
74
│ │ │ │ -
76 friend class boost::serialization::access;
│ │ │ │ -
77 template<class ARCHIVE>
│ │ │ │ -
78 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
79 namespace bs = ::boost::serialization;
│ │ │ │ -
80 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotationParams);
│ │ │ │ -
81 ar & BOOST_SERIALIZATION_NVP(accelerometerCovariance);
│ │ │ │ -
82 ar & BOOST_SERIALIZATION_NVP(integrationCovariance);
│ │ │ │ -
83 ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);
│ │ │ │ -
84 ar & BOOST_SERIALIZATION_NVP(n_gravity);
│ │ │ │ -
85 }
│ │ │ │ -
86
│ │ │ │ -
87#ifdef GTSAM_USE_QUATERNIONS
│ │ │ │ -
88 // Align if we are using Quaternions
│ │ │ │ -
89public:
│ │ │ │ -
90 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
91#endif
│ │ │ │ -
92};
│ │ │ │ -
│ │ │ │ -
93
│ │ │ │ -
94} // namespace gtsam
│ │ │ │ -
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ - │ │ │ │ +
56 const Matrix3& getGyroscopeCovariance() const { return gyroscopeCovariance; }
│ │ │ │ +
57 boost::optional<Vector3> getOmegaCoriolis() const { return omegaCoriolis; }
│ │ │ │ +
58 boost::optional<Pose3> getBodyPSensor() const { return body_P_sensor; }
│ │ │ │ +
59
│ │ │ │ +
60 private:
│ │ │ │ +
62 friend class boost::serialization::access;
│ │ │ │ +
63 template<class ARCHIVE>
│ │ │ │ +
64 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
65 namespace bs = ::boost::serialization;
│ │ │ │ +
66 ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance);
│ │ │ │ +
67 ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
│ │ │ │ +
68
│ │ │ │ +
69 // Provide support for Eigen::Matrix in boost::optional
│ │ │ │ +
70 bool omegaCoriolisFlag = omegaCoriolis.is_initialized();
│ │ │ │ +
71 ar & boost::serialization::make_nvp("omegaCoriolisFlag", omegaCoriolisFlag);
│ │ │ │ +
72 if (omegaCoriolisFlag) {
│ │ │ │ +
73 ar & BOOST_SERIALIZATION_NVP(*omegaCoriolis);
│ │ │ │ +
74 }
│ │ │ │ +
75 }
│ │ │ │ +
76
│ │ │ │ +
77#ifdef GTSAM_USE_QUATERNIONS
│ │ │ │ +
78 // Align if we are using Quaternions
│ │ │ │ +
79public:
│ │ │ │ +
80 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
81#endif
│ │ │ │ +
82};
│ │ │ │ +
│ │ │ │ +
83
│ │ │ │ +
│ │ │ │ +
89class GTSAM_EXPORT PreintegratedRotation {
│ │ │ │ +
90 public:
│ │ │ │ +
91 typedef PreintegratedRotationParams Params;
│ │ │ │ +
92
│ │ │ │ +
93 protected:
│ │ │ │ +
95 boost::shared_ptr<Params> p_;
│ │ │ │ +
96
│ │ │ │ +
97 double deltaTij_;
│ │ │ │ +
98 Rot3 deltaRij_;
│ │ │ │ +
99 Matrix3 delRdelBiasOmega_;
│ │ │ │ +
100
│ │ │ │ +
102 PreintegratedRotation() {}
│ │ │ │ +
103
│ │ │ │ +
104 public:
│ │ │ │ +
107
│ │ │ │ +
│ │ │ │ +
109 explicit PreintegratedRotation(const boost::shared_ptr<Params>& p) : p_(p) {
│ │ │ │ +
110 resetIntegration();
│ │ │ │ +
111 }
│ │ │ │ +
│ │ │ │ +
112
│ │ │ │ +
│ │ │ │ +
114 PreintegratedRotation(const boost::shared_ptr<Params>& p,
│ │ │ │ +
115 double deltaTij, const Rot3& deltaRij,
│ │ │ │ +
116 const Matrix3& delRdelBiasOmega)
│ │ │ │ +
117 : p_(p), deltaTij_(deltaTij), deltaRij_(deltaRij), delRdelBiasOmega_(delRdelBiasOmega) {}
│ │ │ │ +
│ │ │ │ +
118
│ │ │ │ +
120
│ │ │ │ +
123
│ │ │ │ +
125 void resetIntegration();
│ │ │ │ +
126
│ │ │ │ +
│ │ │ │ +
128 bool matchesParamsWith(const PreintegratedRotation& other) const {
│ │ │ │ +
129 return p_ == other.p_;
│ │ │ │ +
130 }
│ │ │ │ +
│ │ │ │ +
132
│ │ │ │ +
135 const boost::shared_ptr<Params>& params() const {
│ │ │ │ +
136 return p_;
│ │ │ │ +
137 }
│ │ │ │ +
138 const double& deltaTij() const {
│ │ │ │ +
139 return deltaTij_;
│ │ │ │ +
140 }
│ │ │ │ +
141 const Rot3& deltaRij() const {
│ │ │ │ +
142 return deltaRij_;
│ │ │ │ +
143 }
│ │ │ │ +
144 const Matrix3& delRdelBiasOmega() const {
│ │ │ │ +
145 return delRdelBiasOmega_;
│ │ │ │ +
146 }
│ │ │ │ +
148
│ │ │ │ +
151 void print(const std::string& s) const;
│ │ │ │ +
152 bool equals(const PreintegratedRotation& other, double tol) const;
│ │ │ │ +
154
│ │ │ │ +
157
│ │ │ │ +
161 Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
│ │ │ │ +
162 OptionalJacobian<3, 3> D_incrR_integratedOmega) const;
│ │ │ │ +
163
│ │ │ │ +
166 void integrateMeasurement(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
│ │ │ │ +
167 OptionalJacobian<3, 3> D_incrR_integratedOmega = boost::none,
│ │ │ │ +
168 OptionalJacobian<3, 3> F = boost::none);
│ │ │ │ +
169
│ │ │ │ +
171 Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
│ │ │ │ +
172 OptionalJacobian<3, 3> H = boost::none) const;
│ │ │ │ +
173
│ │ │ │ +
175 Vector3 integrateCoriolis(const Rot3& rot_i) const;
│ │ │ │ +
176
│ │ │ │ +
178
│ │ │ │ +
179 private:
│ │ │ │ +
181 friend class boost::serialization::access;
│ │ │ │ +
182 template <class ARCHIVE>
│ │ │ │ +
183 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { // NOLINT
│ │ │ │ +
184 ar& BOOST_SERIALIZATION_NVP(p_);
│ │ │ │ +
185 ar& BOOST_SERIALIZATION_NVP(deltaTij_);
│ │ │ │ +
186 ar& BOOST_SERIALIZATION_NVP(deltaRij_);
│ │ │ │ +
187 ar& BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
│ │ │ │ +
188 }
│ │ │ │ +
189
│ │ │ │ +
190#ifdef GTSAM_USE_QUATERNIONS
│ │ │ │ +
191 // Align if we are using Quaternions
│ │ │ │ +
192 public:
│ │ │ │ +
193 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
194#endif
│ │ │ │ +
195};
│ │ │ │ +
│ │ │ │ +
196
│ │ │ │ +
197template <>
│ │ │ │ +
198struct traits<PreintegratedRotation> : public Testable<PreintegratedRotation> {};
│ │ │ │ +
199
│ │ │ │ +
200}
│ │ │ │ +
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ +
Pose3.h
3D Pose
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
│ │ │ │
gtsam::PreintegratedRotationParams
Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
Definition PreintegratedRotation.h:31
│ │ │ │ -
gtsam::PreintegrationParams
Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
Definition PreintegrationParams.h:26
│ │ │ │ -
gtsam::PreintegrationParams::accelerometerCovariance
Matrix3 accelerometerCovariance
Continuous-time "Covariance" of accelerometer The units for stddev are σ = m/s²/√Hz.
Definition PreintegrationParams.h:29
│ │ │ │ -
gtsam::PreintegrationParams::use2ndOrderCoriolis
bool use2ndOrderCoriolis
Whether to use second order Coriolis integration.
Definition PreintegrationParams.h:31
│ │ │ │ -
gtsam::PreintegrationParams::PreintegrationParams
PreintegrationParams()
Default constructor for serialization only.
Definition PreintegrationParams.h:35
│ │ │ │ -
gtsam::PreintegrationParams::n_gravity
Vector3 n_gravity
Gravity vector in nav frame.
Definition PreintegrationParams.h:32
│ │ │ │ -
gtsam::PreintegrationParams::PreintegrationParams
PreintegrationParams(const Vector3 &n_gravity)
The Params constructor insists on getting the navigation frame gravity vector For convenience,...
Definition PreintegrationParams.h:44
│ │ │ │ -
gtsam::PreintegrationParams::integrationCovariance
Matrix3 integrationCovariance
continuous-time "Covariance" describing integration uncertainty
Definition PreintegrationParams.h:30
│ │ │ │ +
gtsam::PreintegratedRotationParams::body_P_sensor
boost::optional< Pose3 > body_P_sensor
The pose of the sensor in the body frame.
Definition PreintegratedRotation.h:36
│ │ │ │ +
gtsam::PreintegratedRotationParams::omegaCoriolis
boost::optional< Vector3 > omegaCoriolis
Coriolis constant.
Definition PreintegratedRotation.h:35
│ │ │ │ +
gtsam::PreintegratedRotationParams::gyroscopeCovariance
Matrix3 gyroscopeCovariance
Continuous-time "Covariance" of gyroscope measurements The units for stddev are σ = rad/s/√Hz.
Definition PreintegratedRotation.h:34
│ │ │ │ +
gtsam::PreintegratedRotation
PreintegratedRotation is the base class for all PreintegratedMeasurements classes (in AHRSFactor,...
Definition PreintegratedRotation.h:89
│ │ │ │ +
gtsam::PreintegratedRotation::delRdelBiasOmega_
Matrix3 delRdelBiasOmega_
Jacobian of preintegrated rotation w.r.t. angular rate bias.
Definition PreintegratedRotation.h:99
│ │ │ │ +
gtsam::PreintegratedRotation::p_
boost::shared_ptr< Params > p_
Parameters.
Definition PreintegratedRotation.h:95
│ │ │ │ +
gtsam::PreintegratedRotation::PreintegratedRotation
PreintegratedRotation(const boost::shared_ptr< Params > &p, double deltaTij, const Rot3 &deltaRij, const Matrix3 &delRdelBiasOmega)
Explicit initialization of all class members.
Definition PreintegratedRotation.h:114
│ │ │ │ +
gtsam::PreintegratedRotation::PreintegratedRotation
PreintegratedRotation(const boost::shared_ptr< Params > &p)
Default constructor, resets integration to zero.
Definition PreintegratedRotation.h:109
│ │ │ │ +
gtsam::PreintegratedRotation::deltaTij_
double deltaTij_
Time interval from i to j.
Definition PreintegratedRotation.h:97
│ │ │ │ +
gtsam::PreintegratedRotation::matchesParamsWith
bool matchesParamsWith(const PreintegratedRotation &other) const
check parameters equality: checks whether shared pointer points to same Params object.
Definition PreintegratedRotation.h:128
│ │ │ │ +
gtsam::PreintegratedRotation::PreintegratedRotation
PreintegratedRotation()
Default constructor for serialization.
Definition PreintegratedRotation.h:102
│ │ │ │ +
gtsam::PreintegratedRotation::deltaRij_
Rot3 deltaRij_
Preintegrated relative orientation (in frame i)
Definition PreintegratedRotation.h:98
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,151 +1,253 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -PreintegrationParams.h │ │ │ │ │ +PreintegratedRotation.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -17#pragma once │ │ │ │ │ -18 │ │ │ │ │ -19#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_._h> │ │ │ │ │ -20#include │ │ │ │ │ -21 │ │ │ │ │ -22namespace _g_t_s_a_m { │ │ │ │ │ +22#pragma once │ │ │ │ │ 23 │ │ │ │ │ -_2_6struct GTSAM_EXPORT _P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s: _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s { │ │ │ │ │ -_2_9 Matrix3 _a_c_c_e_l_e_r_o_m_e_t_e_r_C_o_v_a_r_i_a_n_c_e; │ │ │ │ │ -_3_0 Matrix3 _i_n_t_e_g_r_a_t_i_o_n_C_o_v_a_r_i_a_n_c_e; │ │ │ │ │ -_3_1 bool _u_s_e_2_n_d_O_r_d_e_r_C_o_r_i_o_l_i_s; │ │ │ │ │ -_3_2 Vector3 _n___g_r_a_v_i_t_y; │ │ │ │ │ -33 │ │ │ │ │ -_3_5 _P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s() │ │ │ │ │ -36 : _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s(), │ │ │ │ │ -37 accelerometerCovariance(I_3x3), │ │ │ │ │ -38 integrationCovariance(I_3x3), │ │ │ │ │ -39 use2ndOrderCoriolis(false), │ │ │ │ │ -40 n_gravity(0, 0, -1) {} │ │ │ │ │ -41 │ │ │ │ │ -_4_4 _P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s(const Vector3& n_gravity) │ │ │ │ │ -45 : _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s(), │ │ │ │ │ -46 accelerometerCovariance(I_3x3), │ │ │ │ │ -47 integrationCovariance(I_3x3), │ │ │ │ │ -48 use2ndOrderCoriolis(false), │ │ │ │ │ -49 n_gravity(n_gravity) {} │ │ │ │ │ -50 │ │ │ │ │ -51 // Default Params for a Z-down navigation frame, such as NED: gravity points │ │ │ │ │ -along positive Z-axis │ │ │ │ │ -52 static boost::shared_ptr MakeSharedD(double g = 9.81) │ │ │ │ │ -{ │ │ │ │ │ -53 return boost::shared_ptr(new _P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ -(Vector3(0, 0, g))); │ │ │ │ │ -54 } │ │ │ │ │ +24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_3_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ +26 │ │ │ │ │ +27namespace _g_t_s_a_m { │ │ │ │ │ +28 │ │ │ │ │ +_3_1struct GTSAM_EXPORT _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s { │ │ │ │ │ +_3_4 Matrix3 _g_y_r_o_s_c_o_p_e_C_o_v_a_r_i_a_n_c_e; │ │ │ │ │ +_3_5 boost::optional _o_m_e_g_a_C_o_r_i_o_l_i_s; │ │ │ │ │ +_3_6 boost::optional _b_o_d_y___P___s_e_n_s_o_r; │ │ │ │ │ +37 │ │ │ │ │ +38 _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s() : gyroscopeCovariance(I_3x3) {} │ │ │ │ │ +39 │ │ │ │ │ +40 _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s(const Matrix3& gyroscope_covariance, │ │ │ │ │ +41 boost::optional omega_coriolis) │ │ │ │ │ +42 : gyroscopeCovariance(gyroscope_covariance) { │ │ │ │ │ +43 if (omega_coriolis) │ │ │ │ │ +44 omegaCoriolis.reset(omega_coriolis.get()); │ │ │ │ │ +45 } │ │ │ │ │ +46 │ │ │ │ │ +47 virtual ~PreintegratedRotationParams() {} │ │ │ │ │ +48 │ │ │ │ │ +49 virtual void _p_r_i_n_t(const std::string& s) const; │ │ │ │ │ +50 virtual bool equals(const PreintegratedRotationParams& other, double tol=1e- │ │ │ │ │ +9) const; │ │ │ │ │ +51 │ │ │ │ │ +52 void setGyroscopeCovariance(const Matrix3& cov) { gyroscopeCovariance = cov; │ │ │ │ │ +} │ │ │ │ │ +53 void setOmegaCoriolis(const Vector3& omega) { omegaCoriolis.reset(omega); } │ │ │ │ │ +54 void setBodyPSensor(const Pose3& pose) { body_P_sensor.reset(pose); } │ │ │ │ │ 55 │ │ │ │ │ -56 // Default Params for a Z-up navigation frame, such as ENU: gravity points │ │ │ │ │ -along negative Z-axis │ │ │ │ │ -57 static boost::shared_ptr MakeSharedU(double g = 9.81) │ │ │ │ │ +56 const Matrix3& getGyroscopeCovariance() const { return gyroscopeCovariance; │ │ │ │ │ +} │ │ │ │ │ +57 boost::optional getOmegaCoriolis() const { return omegaCoriolis; } │ │ │ │ │ +58 boost::optional getBodyPSensor() const { return body_P_sensor; } │ │ │ │ │ +59 │ │ │ │ │ +60 private: │ │ │ │ │ +_6_2 friend class boost::serialization::access; │ │ │ │ │ +63 template │ │ │ │ │ +64 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +65 namespace bs = ::boost::serialization; │ │ │ │ │ +66 ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance); │ │ │ │ │ +67 ar & BOOST_SERIALIZATION_NVP(body_P_sensor); │ │ │ │ │ +68 │ │ │ │ │ +69 // Provide support for Eigen::Matrix in boost::optional │ │ │ │ │ +70 bool omegaCoriolisFlag = omegaCoriolis.is_initialized(); │ │ │ │ │ +71 ar & boost::serialization::make_nvp("omegaCoriolisFlag", omegaCoriolisFlag); │ │ │ │ │ +72 if (omegaCoriolisFlag) { │ │ │ │ │ +73 ar & BOOST_SERIALIZATION_NVP(*omegaCoriolis); │ │ │ │ │ +74 } │ │ │ │ │ +75 } │ │ │ │ │ +76 │ │ │ │ │ +77#ifdef GTSAM_USE_QUATERNIONS │ │ │ │ │ +78 // Align if we are using Quaternions │ │ │ │ │ +79public: │ │ │ │ │ +80 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +81#endif │ │ │ │ │ +82}; │ │ │ │ │ +83 │ │ │ │ │ +_8_9class GTSAM_EXPORT _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n { │ │ │ │ │ +90 public: │ │ │ │ │ +91 typedef _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s _P_a_r_a_m_s; │ │ │ │ │ +92 │ │ │ │ │ +93 protected: │ │ │ │ │ +_9_5 boost::shared_ptr _p__; │ │ │ │ │ +96 │ │ │ │ │ +_9_7 double _d_e_l_t_a_T_i_j__; │ │ │ │ │ +_9_8 _R_o_t_3 _d_e_l_t_a_R_i_j__; │ │ │ │ │ +_9_9 Matrix3 _d_e_l_R_d_e_l_B_i_a_s_O_m_e_g_a__; │ │ │ │ │ +100 │ │ │ │ │ +_1_0_2 _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n() {} │ │ │ │ │ +103 │ │ │ │ │ +104 public: │ │ │ │ │ +107 │ │ │ │ │ +_1_0_9 explicit _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n(const boost::shared_ptr& p) : p_(p) │ │ │ │ │ { │ │ │ │ │ -58 return boost::shared_ptr(new PreintegrationParams │ │ │ │ │ -(Vector3(0, 0, -g))); │ │ │ │ │ -59 } │ │ │ │ │ -60 │ │ │ │ │ -61 void _p_r_i_n_t(const std::string& s="") const override; │ │ │ │ │ -62 bool equals(const PreintegratedRotationParams& other, double tol) const │ │ │ │ │ -override; │ │ │ │ │ -63 │ │ │ │ │ -64 void setAccelerometerCovariance(const Matrix3& cov) │ │ │ │ │ -{ accelerometerCovariance = cov; } │ │ │ │ │ -65 void setIntegrationCovariance(const Matrix3& cov) { integrationCovariance = │ │ │ │ │ -cov; } │ │ │ │ │ -66 void setUse2ndOrderCoriolis(bool flag) { use2ndOrderCoriolis = flag; } │ │ │ │ │ -67 │ │ │ │ │ -68 const Matrix3& getAccelerometerCovariance() const { return │ │ │ │ │ -accelerometerCovariance; } │ │ │ │ │ -69 const Matrix3& getIntegrationCovariance() const { return │ │ │ │ │ -integrationCovariance; } │ │ │ │ │ -70 const Vector3& getGravity() const { return n_gravity; } │ │ │ │ │ -71 bool getUse2ndOrderCoriolis() const { return use2ndOrderCoriolis; } │ │ │ │ │ -72 │ │ │ │ │ -73protected: │ │ │ │ │ -74 │ │ │ │ │ -_7_6 friend class boost::serialization::access; │ │ │ │ │ -77 template │ │ │ │ │ -78 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -79 namespace bs = ::boost::serialization; │ │ │ │ │ -80 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s); │ │ │ │ │ -81 ar & BOOST_SERIALIZATION_NVP(accelerometerCovariance); │ │ │ │ │ -82 ar & BOOST_SERIALIZATION_NVP(integrationCovariance); │ │ │ │ │ -83 ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis); │ │ │ │ │ -84 ar & BOOST_SERIALIZATION_NVP(n_gravity); │ │ │ │ │ -85 } │ │ │ │ │ -86 │ │ │ │ │ -87#ifdef GTSAM_USE_QUATERNIONS │ │ │ │ │ -88 // Align if we are using Quaternions │ │ │ │ │ -89public: │ │ │ │ │ -90 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -91#endif │ │ │ │ │ -92}; │ │ │ │ │ -93 │ │ │ │ │ -94} // namespace gtsam │ │ │ │ │ +110 resetIntegration(); │ │ │ │ │ +111 } │ │ │ │ │ +112 │ │ │ │ │ +_1_1_4 _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n(const boost::shared_ptr& p, │ │ │ │ │ +115 double deltaTij, const _R_o_t_3& deltaRij, │ │ │ │ │ +116 const Matrix3& delRdelBiasOmega) │ │ │ │ │ +117 : p_(p), deltaTij_(deltaTij), deltaRij_(deltaRij), delRdelBiasOmega_ │ │ │ │ │ +(delRdelBiasOmega) {} │ │ │ │ │ +118 │ │ │ │ │ +120 │ │ │ │ │ +123 │ │ │ │ │ +125 void resetIntegration(); │ │ │ │ │ +126 │ │ │ │ │ +_1_2_8 bool _m_a_t_c_h_e_s_P_a_r_a_m_s_W_i_t_h(const _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n& other) const { │ │ │ │ │ +129 return p_ == other._p__; │ │ │ │ │ +130 } │ │ │ │ │ +132 │ │ │ │ │ +135 const boost::shared_ptr& params() const { │ │ │ │ │ +136 return p_; │ │ │ │ │ +137 } │ │ │ │ │ +138 const double& deltaTij() const { │ │ │ │ │ +139 return deltaTij_; │ │ │ │ │ +140 } │ │ │ │ │ +141 const Rot3& deltaRij() const { │ │ │ │ │ +142 return deltaRij_; │ │ │ │ │ +143 } │ │ │ │ │ +144 const Matrix3& delRdelBiasOmega() const { │ │ │ │ │ +145 return delRdelBiasOmega_; │ │ │ │ │ +146 } │ │ │ │ │ +148 │ │ │ │ │ +151 void _p_r_i_n_t(const std::string& s) const; │ │ │ │ │ +152 bool equals(const PreintegratedRotation& other, double tol) const; │ │ │ │ │ +154 │ │ │ │ │ +157 │ │ │ │ │ +161 Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& │ │ │ │ │ +biasHat, double deltaT, │ │ │ │ │ +162 OptionalJacobian<3, 3> D_incrR_integratedOmega) const; │ │ │ │ │ +163 │ │ │ │ │ +166 void integrateMeasurement(const Vector3& measuredOmega, const Vector3& │ │ │ │ │ +biasHat, double deltaT, │ │ │ │ │ +167 OptionalJacobian<3, 3> D_incrR_integratedOmega = boost::none, │ │ │ │ │ +168 OptionalJacobian<3, 3> F = boost::none); │ │ │ │ │ +169 │ │ │ │ │ +171 Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr, │ │ │ │ │ +172 OptionalJacobian<3, 3> H = boost::none) const; │ │ │ │ │ +173 │ │ │ │ │ +175 Vector3 integrateCoriolis(const Rot3& rot_i) const; │ │ │ │ │ +176 │ │ │ │ │ +178 │ │ │ │ │ +179 private: │ │ │ │ │ +_1_8_1 friend class boost::serialization::access; │ │ │ │ │ +182 template │ │ │ │ │ +183 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { // NOLINT │ │ │ │ │ +184 ar& BOOST_SERIALIZATION_NVP(p_); │ │ │ │ │ +185 ar& BOOST_SERIALIZATION_NVP(deltaTij_); │ │ │ │ │ +186 ar& BOOST_SERIALIZATION_NVP(deltaRij_); │ │ │ │ │ +187 ar& BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); │ │ │ │ │ +188 } │ │ │ │ │ +189 │ │ │ │ │ +190#ifdef GTSAM_USE_QUATERNIONS │ │ │ │ │ +191 // Align if we are using Quaternions │ │ │ │ │ +192 public: │ │ │ │ │ +193 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +194#endif │ │ │ │ │ +195}; │ │ │ │ │ +196 │ │ │ │ │ +197template <> │ │ │ │ │ +_1_9_8struct _t_r_a_i_t_s<_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n> : public │ │ │ │ │ +_T_e_s_t_a_b_l_e {}; │ │ │ │ │ +199 │ │ │ │ │ +200} │ │ │ │ │ +_M_a_t_r_i_x_._h │ │ │ │ │ +typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ This marks a GTSAM object to require alignment. │ │ │ │ │ DDeeffiinniittiioonn types.h:308 │ │ │ │ │ -_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_._h │ │ │ │ │ +_P_o_s_e_3_._h │ │ │ │ │ +3D Pose │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ print without optional string, must specify cout yourself │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ +Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor │ │ │ │ │ +symbol GTSAM_USE_QUATERNIO... │ │ │ │ │ +DDeeffiinniittiioonn Rot3.h:58 │ │ │ │ │ _g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ Parameters for pre-integration: Usage: Create just a single Params and pass a │ │ │ │ │ shared pointer to the c... │ │ │ │ │ DDeeffiinniittiioonn PreintegratedRotation.h:31 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ -Parameters for pre-integration: Usage: Create just a single Params and pass a │ │ │ │ │ -shared pointer to the c... │ │ │ │ │ -DDeeffiinniittiioonn PreintegrationParams.h:26 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s_:_:_a_c_c_e_l_e_r_o_m_e_t_e_r_C_o_v_a_r_i_a_n_c_e │ │ │ │ │ -Matrix3 accelerometerCovariance │ │ │ │ │ -Continuous-time "Covariance" of accelerometer The units for stddev are σ = m/ │ │ │ │ │ -s²/√Hz. │ │ │ │ │ -DDeeffiinniittiioonn PreintegrationParams.h:29 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s_:_:_u_s_e_2_n_d_O_r_d_e_r_C_o_r_i_o_l_i_s │ │ │ │ │ -bool use2ndOrderCoriolis │ │ │ │ │ -Whether to use second order Coriolis integration. │ │ │ │ │ -DDeeffiinniittiioonn PreintegrationParams.h:31 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ -PreintegrationParams() │ │ │ │ │ -Default constructor for serialization only. │ │ │ │ │ -DDeeffiinniittiioonn PreintegrationParams.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s_:_:_n___g_r_a_v_i_t_y │ │ │ │ │ -Vector3 n_gravity │ │ │ │ │ -Gravity vector in nav frame. │ │ │ │ │ -DDeeffiinniittiioonn PreintegrationParams.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ -PreintegrationParams(const Vector3 &n_gravity) │ │ │ │ │ -The Params constructor insists on getting the navigation frame gravity vector │ │ │ │ │ -For convenience,... │ │ │ │ │ -DDeeffiinniittiioonn PreintegrationParams.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s_:_:_i_n_t_e_g_r_a_t_i_o_n_C_o_v_a_r_i_a_n_c_e │ │ │ │ │ -Matrix3 integrationCovariance │ │ │ │ │ -continuous-time "Covariance" describing integration uncertainty │ │ │ │ │ -DDeeffiinniittiioonn PreintegrationParams.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s_:_:_b_o_d_y___P___s_e_n_s_o_r │ │ │ │ │ +boost::optional< Pose3 > body_P_sensor │ │ │ │ │ +The pose of the sensor in the body frame. │ │ │ │ │ +DDeeffiinniittiioonn PreintegratedRotation.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s_:_:_o_m_e_g_a_C_o_r_i_o_l_i_s │ │ │ │ │ +boost::optional< Vector3 > omegaCoriolis │ │ │ │ │ +Coriolis constant. │ │ │ │ │ +DDeeffiinniittiioonn PreintegratedRotation.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s_:_:_g_y_r_o_s_c_o_p_e_C_o_v_a_r_i_a_n_c_e │ │ │ │ │ +Matrix3 gyroscopeCovariance │ │ │ │ │ +Continuous-time "Covariance" of gyroscope measurements The units for stddev are │ │ │ │ │ +σ = rad/s/√Hz. │ │ │ │ │ +DDeeffiinniittiioonn PreintegratedRotation.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n │ │ │ │ │ +PreintegratedRotation is the base class for all PreintegratedMeasurements │ │ │ │ │ +classes (in AHRSFactor,... │ │ │ │ │ +DDeeffiinniittiioonn PreintegratedRotation.h:89 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_:_:_d_e_l_R_d_e_l_B_i_a_s_O_m_e_g_a__ │ │ │ │ │ +Matrix3 delRdelBiasOmega_ │ │ │ │ │ +Jacobian of preintegrated rotation w.r.t. angular rate bias. │ │ │ │ │ +DDeeffiinniittiioonn PreintegratedRotation.h:99 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_:_:_p__ │ │ │ │ │ +boost::shared_ptr< Params > p_ │ │ │ │ │ +Parameters. │ │ │ │ │ +DDeeffiinniittiioonn PreintegratedRotation.h:95 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n │ │ │ │ │ +PreintegratedRotation(const boost::shared_ptr< Params > &p, double deltaTij, │ │ │ │ │ +const Rot3 &deltaRij, const Matrix3 &delRdelBiasOmega) │ │ │ │ │ +Explicit initialization of all class members. │ │ │ │ │ +DDeeffiinniittiioonn PreintegratedRotation.h:114 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n │ │ │ │ │ +PreintegratedRotation(const boost::shared_ptr< Params > &p) │ │ │ │ │ +Default constructor, resets integration to zero. │ │ │ │ │ +DDeeffiinniittiioonn PreintegratedRotation.h:109 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_:_:_d_e_l_t_a_T_i_j__ │ │ │ │ │ +double deltaTij_ │ │ │ │ │ +Time interval from i to j. │ │ │ │ │ +DDeeffiinniittiioonn PreintegratedRotation.h:97 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_:_:_m_a_t_c_h_e_s_P_a_r_a_m_s_W_i_t_h │ │ │ │ │ +bool matchesParamsWith(const PreintegratedRotation &other) const │ │ │ │ │ +check parameters equality: checks whether shared pointer points to same Params │ │ │ │ │ +object. │ │ │ │ │ +DDeeffiinniittiioonn PreintegratedRotation.h:128 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n │ │ │ │ │ +PreintegratedRotation() │ │ │ │ │ +Default constructor for serialization. │ │ │ │ │ +DDeeffiinniittiioonn PreintegratedRotation.h:102 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_:_:_d_e_l_t_a_R_i_j__ │ │ │ │ │ +Rot3 deltaRij_ │ │ │ │ │ +Preintegrated relative orientation (in frame i) │ │ │ │ │ +DDeeffiinniittiioonn PreintegratedRotation.h:98 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s_._h │ │ │ │ │ + * _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00908.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/PreintegratedRotation.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/NavState.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,54 +95,54 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
PreintegratedRotation.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Typedefs
│ │ │ │ +
NavState.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ +

Navigation state composing of attitude, position, and velocity. │ │ │ │ +More...

│ │ │ │ + │ │ │ │

Go to the source code of this file.

│ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

struct  gtsam::PreintegratedRotationParams
 Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the constructor. More...
 
class  gtsam::PreintegratedRotation
 PreintegratedRotation is the base class for all PreintegratedMeasurements classes (in AHRSFactor, ImuFactor, and CombinedImuFactor). More...
class  gtsam::NavState
 Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make this a Lie group, but it is a 9D manifold. More...
 
struct  gtsam::traits< PreintegratedRotation >
struct  gtsam::traits< NavState >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Typedefs

│ │ │ │ +typedef Vector3 gtsam::Velocity3
 Velocity is currently typedef'd to Vector3.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Luca Carlone
│ │ │ │ -
│ │ │ │ -Stephen Williams
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ -
│ │ │ │ -Vadim Indelman
│ │ │ │ -
│ │ │ │ -David Jensen
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ +

Navigation state composing of attitude, position, and velocity.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
Date
July 2015
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,36 +1,35 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -PreintegratedRotation.h File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ +NavState.h File Reference │ │ │ │ │ +Navigation state composing of attitude, position, and velocity. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ -  Parameters for pre-integration: Usage: Create just a single Params and │ │ │ │ │ - pass a shared pointer to the constructor. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_N_a_v_S_t_a_t_e │ │ │ │ │ + Navigation state: Pose (rotation, translation) + velocity NOTE(frank): │ │ │ │ │ +  it does not make sense to make this a Lie group, but it is a 9D │ │ │ │ │ + manifold. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ - class   _g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n │ │ │ │ │ - _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n is the base class for all │ │ │ │ │ -  PreintegratedMeasurements classes (in _A_H_R_S_F_a_c_t_o_r, _I_m_u_F_a_c_t_o_r, and │ │ │ │ │ - _C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r). _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _N_a_v_S_t_a_t_e_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +typedef Vector3  ggttssaamm::::VVeelloocciittyy33 │ │ │ │ │ +  Velocity is currently typedef'd to Vector3. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Navigation state composing of attitude, position, and velocity. │ │ │ │ │ Author │ │ │ │ │ - Luca Carlone │ │ │ │ │ - Stephen Williams │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Vadim Indelman │ │ │ │ │ - David Jensen │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + July 2015 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_._h │ │ │ │ │ + * _N_a_v_S_t_a_t_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00908.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,5 +1,5 @@ │ │ │ │ │ var a00908 = [ │ │ │ │ │ - ["gtsam::PreintegratedRotationParams", "a04204.html", "a04204"], │ │ │ │ │ - ["gtsam::PreintegratedRotation", "a04208.html", "a04208"], │ │ │ │ │ - ["gtsam::traits< PreintegratedRotation >", "a04212.html", null] │ │ │ │ │ + ["gtsam::NavState", "a04196.html", "a04196"], │ │ │ │ │ + ["gtsam::traits< NavState >", "a04200.html", null], │ │ │ │ │ + ["Velocity3", "a00908.html#ad6ba0a40d5dd8936f4ca53f7bb3ef53b", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00908_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/PreintegratedRotation.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/NavState.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,203 +98,217 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
PreintegratedRotation.h
│ │ │ │ +
NavState.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
22#pragma once
│ │ │ │ -
23
│ │ │ │ -
24#include <gtsam/geometry/Pose3.h>
│ │ │ │ -
25#include <gtsam/base/Matrix.h>
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/geometry/Pose3.h>
│ │ │ │ +
22#include <gtsam/base/Vector.h>
│ │ │ │ +
23#include <gtsam/base/Manifold.h>
│ │ │ │ +
24
│ │ │ │ +
25namespace gtsam {
│ │ │ │
26
│ │ │ │ -
27namespace gtsam {
│ │ │ │ -
28
│ │ │ │ -
│ │ │ │ -
31struct GTSAM_EXPORT PreintegratedRotationParams {
│ │ │ │ -
34 Matrix3 gyroscopeCovariance;
│ │ │ │ -
35 boost::optional<Vector3> omegaCoriolis;
│ │ │ │ -
36 boost::optional<Pose3> body_P_sensor;
│ │ │ │ -
37
│ │ │ │ -
38 PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {}
│ │ │ │ -
39
│ │ │ │ -
40 PreintegratedRotationParams(const Matrix3& gyroscope_covariance,
│ │ │ │ -
41 boost::optional<Vector3> omega_coriolis)
│ │ │ │ -
42 : gyroscopeCovariance(gyroscope_covariance) {
│ │ │ │ -
43 if (omega_coriolis)
│ │ │ │ -
44 omegaCoriolis.reset(omega_coriolis.get());
│ │ │ │ -
45 }
│ │ │ │ -
46
│ │ │ │ -
47 virtual ~PreintegratedRotationParams() {}
│ │ │ │ +
28typedef Vector3 Velocity3;
│ │ │ │ +
29
│ │ │ │ +
│ │ │ │ +
34class GTSAM_EXPORT NavState {
│ │ │ │ +
35private:
│ │ │ │ +
36
│ │ │ │ +
37 // TODO(frank):
│ │ │ │ +
38 // - should we rename t_ to p_? if not, we should rename dP do dT
│ │ │ │ +
39 Rot3 R_;
│ │ │ │ +
40 Point3 t_;
│ │ │ │ +
41 Velocity3 v_;
│ │ │ │ +
42
│ │ │ │ +
43public:
│ │ │ │ +
44
│ │ │ │ +
45 enum {
│ │ │ │ +
46 dimension = 9
│ │ │ │ +
47 };
│ │ │ │
48
│ │ │ │ -
49 virtual void print(const std::string& s) const;
│ │ │ │ -
50 virtual bool equals(const PreintegratedRotationParams& other, double tol=1e-9) const;
│ │ │ │ -
51
│ │ │ │ -
52 void setGyroscopeCovariance(const Matrix3& cov) { gyroscopeCovariance = cov; }
│ │ │ │ -
53 void setOmegaCoriolis(const Vector3& omega) { omegaCoriolis.reset(omega); }
│ │ │ │ -
54 void setBodyPSensor(const Pose3& pose) { body_P_sensor.reset(pose); }
│ │ │ │ -
55
│ │ │ │ -
56 const Matrix3& getGyroscopeCovariance() const { return gyroscopeCovariance; }
│ │ │ │ -
57 boost::optional<Vector3> getOmegaCoriolis() const { return omegaCoriolis; }
│ │ │ │ -
58 boost::optional<Pose3> getBodyPSensor() const { return body_P_sensor; }
│ │ │ │ -
59
│ │ │ │ -
60 private:
│ │ │ │ -
62 friend class boost::serialization::access;
│ │ │ │ -
63 template<class ARCHIVE>
│ │ │ │ -
64 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
65 namespace bs = ::boost::serialization;
│ │ │ │ -
66 ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance);
│ │ │ │ -
67 ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
│ │ │ │ -
68
│ │ │ │ -
69 // Provide support for Eigen::Matrix in boost::optional
│ │ │ │ -
70 bool omegaCoriolisFlag = omegaCoriolis.is_initialized();
│ │ │ │ -
71 ar & boost::serialization::make_nvp("omegaCoriolisFlag", omegaCoriolisFlag);
│ │ │ │ -
72 if (omegaCoriolisFlag) {
│ │ │ │ -
73 ar & BOOST_SERIALIZATION_NVP(*omegaCoriolis);
│ │ │ │ -
74 }
│ │ │ │ -
75 }
│ │ │ │ -
76
│ │ │ │ -
77#ifdef GTSAM_USE_QUATERNIONS
│ │ │ │ -
78 // Align if we are using Quaternions
│ │ │ │ -
79public:
│ │ │ │ -
80 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
81#endif
│ │ │ │ -
82};
│ │ │ │ -
│ │ │ │ -
83
│ │ │ │ -
│ │ │ │ -
89class GTSAM_EXPORT PreintegratedRotation {
│ │ │ │ -
90 public:
│ │ │ │ -
91 typedef PreintegratedRotationParams Params;
│ │ │ │ -
92
│ │ │ │ -
93 protected:
│ │ │ │ -
95 boost::shared_ptr<Params> p_;
│ │ │ │ -
96
│ │ │ │ -
97 double deltaTij_;
│ │ │ │ -
98 Rot3 deltaRij_;
│ │ │ │ -
99 Matrix3 delRdelBiasOmega_;
│ │ │ │ -
100
│ │ │ │ -
102 PreintegratedRotation() {}
│ │ │ │ -
103
│ │ │ │ -
104 public:
│ │ │ │ -
107
│ │ │ │ -
│ │ │ │ -
109 explicit PreintegratedRotation(const boost::shared_ptr<Params>& p) : p_(p) {
│ │ │ │ -
110 resetIntegration();
│ │ │ │ -
111 }
│ │ │ │ +
49 typedef std::pair<Point3, Velocity3> PositionAndVelocity;
│ │ │ │ +
50
│ │ │ │ +
53
│ │ │ │ +
│ │ │ │ +
55 NavState() :
│ │ │ │ +
56 t_(0, 0, 0), v_(Vector3::Zero()) {
│ │ │ │ +
57 }
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
59 NavState(const Rot3& R, const Point3& t, const Velocity3& v) :
│ │ │ │ +
60 R_(R), t_(t), v_(v) {
│ │ │ │ +
61 }
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
63 NavState(const Pose3& pose, const Velocity3& v) :
│ │ │ │ +
64 R_(pose.rotation()), t_(pose.translation()), v_(v) {
│ │ │ │ +
65 }
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
67 NavState(const Matrix3& R, const Vector6& tv) :
│ │ │ │ +
68 R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) {
│ │ │ │ +
69 }
│ │ │ │ +
│ │ │ │ +
71 static NavState Create(const Rot3& R, const Point3& t, const Velocity3& v,
│ │ │ │ +
72 OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2,
│ │ │ │ +
73 OptionalJacobian<9, 3> H3);
│ │ │ │ +
75 static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel,
│ │ │ │ +
76 OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2);
│ │ │ │ +
77
│ │ │ │ +
81
│ │ │ │ +
82 const Rot3& attitude(OptionalJacobian<3, 9> H = boost::none) const;
│ │ │ │ +
83 const Point3& position(OptionalJacobian<3, 9> H = boost::none) const;
│ │ │ │ +
84 const Velocity3& velocity(OptionalJacobian<3, 9> H = boost::none) const;
│ │ │ │ +
85
│ │ │ │ +
86 const Pose3 pose() const {
│ │ │ │ +
87 return Pose3(attitude(), position());
│ │ │ │ +
88 }
│ │ │ │ +
89
│ │ │ │ +
93
│ │ │ │ +
│ │ │ │ +
95 Matrix3 R() const {
│ │ │ │ +
96 return R_.matrix();
│ │ │ │ +
97 }
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
99 Quaternion quaternion() const {
│ │ │ │ +
100 return R_.toQuaternion();
│ │ │ │ +
101 }
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
103 Vector3 t() const {
│ │ │ │ +
104 return t_;
│ │ │ │ +
105 }
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
107 const Vector3& v() const {
│ │ │ │ +
108 return v_;
│ │ │ │ +
109 }
│ │ │ │
│ │ │ │ +
110 // Return velocity in body frame
│ │ │ │ +
111 Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = boost::none) const;
│ │ │ │
112
│ │ │ │ -
│ │ │ │ -
114 PreintegratedRotation(const boost::shared_ptr<Params>& p,
│ │ │ │ -
115 double deltaTij, const Rot3& deltaRij,
│ │ │ │ -
116 const Matrix3& delRdelBiasOmega)
│ │ │ │ -
117 : p_(p), deltaTij_(deltaTij), deltaRij_(deltaRij), delRdelBiasOmega_(delRdelBiasOmega) {}
│ │ │ │ -
│ │ │ │ -
118
│ │ │ │ -
120
│ │ │ │ -
123
│ │ │ │ -
125 void resetIntegration();
│ │ │ │ -
126
│ │ │ │ -
│ │ │ │ -
128 bool matchesParamsWith(const PreintegratedRotation& other) const {
│ │ │ │ -
129 return p_ == other.p_;
│ │ │ │ -
130 }
│ │ │ │ -
│ │ │ │ -
132
│ │ │ │ -
135 const boost::shared_ptr<Params>& params() const {
│ │ │ │ -
136 return p_;
│ │ │ │ -
137 }
│ │ │ │ -
138 const double& deltaTij() const {
│ │ │ │ -
139 return deltaTij_;
│ │ │ │ +
116 Matrix7 matrix() const;
│ │ │ │ +
117
│ │ │ │ +
121
│ │ │ │ +
123 GTSAM_EXPORT
│ │ │ │ +
124 friend std::ostream &operator<<(std::ostream &os, const NavState& state);
│ │ │ │ +
125
│ │ │ │ +
127 void print(const std::string& s = "") const;
│ │ │ │ +
128
│ │ │ │ +
130 bool equals(const NavState& other, double tol = 1e-8) const;
│ │ │ │ +
131
│ │ │ │ +
135
│ │ │ │ +
136 // Tangent space sugar.
│ │ │ │ +
137 // TODO(frank): move to private navstate namespace in cpp
│ │ │ │ +
138 static Eigen::Block<Vector9, 3, 1> dR(Vector9& v) {
│ │ │ │ +
139 return v.segment<3>(0);
│ │ │ │
140 }
│ │ │ │ -
141 const Rot3& deltaRij() const {
│ │ │ │ -
142 return deltaRij_;
│ │ │ │ +
141 static Eigen::Block<Vector9, 3, 1> dP(Vector9& v) {
│ │ │ │ +
142 return v.segment<3>(3);
│ │ │ │
143 }
│ │ │ │ -
144 const Matrix3& delRdelBiasOmega() const {
│ │ │ │ -
145 return delRdelBiasOmega_;
│ │ │ │ +
144 static Eigen::Block<Vector9, 3, 1> dV(Vector9& v) {
│ │ │ │ +
145 return v.segment<3>(6);
│ │ │ │
146 }
│ │ │ │ -
148
│ │ │ │ -
151 void print(const std::string& s) const;
│ │ │ │ -
152 bool equals(const PreintegratedRotation& other, double tol) const;
│ │ │ │ -
154
│ │ │ │ -
157
│ │ │ │ -
161 Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
│ │ │ │ -
162 OptionalJacobian<3, 3> D_incrR_integratedOmega) const;
│ │ │ │ -
163
│ │ │ │ -
166 void integrateMeasurement(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
│ │ │ │ -
167 OptionalJacobian<3, 3> D_incrR_integratedOmega = boost::none,
│ │ │ │ -
168 OptionalJacobian<3, 3> F = boost::none);
│ │ │ │ -
169
│ │ │ │ -
171 Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
│ │ │ │ -
172 OptionalJacobian<3, 3> H = boost::none) const;
│ │ │ │ -
173
│ │ │ │ -
175 Vector3 integrateCoriolis(const Rot3& rot_i) const;
│ │ │ │ +
147 static Eigen::Block<const Vector9, 3, 1> dR(const Vector9& v) {
│ │ │ │ +
148 return v.segment<3>(0);
│ │ │ │ +
149 }
│ │ │ │ +
150 static Eigen::Block<const Vector9, 3, 1> dP(const Vector9& v) {
│ │ │ │ +
151 return v.segment<3>(3);
│ │ │ │ +
152 }
│ │ │ │ +
153 static Eigen::Block<const Vector9, 3, 1> dV(const Vector9& v) {
│ │ │ │ +
154 return v.segment<3>(6);
│ │ │ │ +
155 }
│ │ │ │ +
156
│ │ │ │ +
158 NavState retract(const Vector9& v, //
│ │ │ │ +
159 OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
│ │ │ │ +
160 boost::none) const;
│ │ │ │ +
161
│ │ │ │ +
163 Vector9 localCoordinates(const NavState& g, //
│ │ │ │ +
164 OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
│ │ │ │ +
165 boost::none) const;
│ │ │ │ +
166
│ │ │ │ +
170
│ │ │ │ +
173 NavState update(const Vector3& b_acceleration, const Vector3& b_omega,
│ │ │ │ +
174 const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
│ │ │ │ +
175 OptionalJacobian<9, 3> G2) const;
│ │ │ │
176
│ │ │ │ -
178
│ │ │ │ -
179 private:
│ │ │ │ -
181 friend class boost::serialization::access;
│ │ │ │ -
182 template <class ARCHIVE>
│ │ │ │ -
183 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { // NOLINT
│ │ │ │ -
184 ar& BOOST_SERIALIZATION_NVP(p_);
│ │ │ │ -
185 ar& BOOST_SERIALIZATION_NVP(deltaTij_);
│ │ │ │ -
186 ar& BOOST_SERIALIZATION_NVP(deltaRij_);
│ │ │ │ -
187 ar& BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
│ │ │ │ -
188 }
│ │ │ │ +
178 Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false,
│ │ │ │ +
179 OptionalJacobian<9, 9> H = boost::none) const;
│ │ │ │ +
180
│ │ │ │ +
183 Vector9 correctPIM(const Vector9& pim, double dt, const Vector3& n_gravity,
│ │ │ │ +
184 const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis =
│ │ │ │ +
185 false, OptionalJacobian<9, 9> H1 = boost::none,
│ │ │ │ +
186 OptionalJacobian<9, 9> H2 = boost::none) const;
│ │ │ │ +
187
│ │ │ │
189
│ │ │ │ -
190#ifdef GTSAM_USE_QUATERNIONS
│ │ │ │ -
191 // Align if we are using Quaternions
│ │ │ │ -
192 public:
│ │ │ │ -
193 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
194#endif
│ │ │ │ -
195};
│ │ │ │ -
│ │ │ │ -
196
│ │ │ │ -
197template <>
│ │ │ │ -
198struct traits<PreintegratedRotation> : public Testable<PreintegratedRotation> {};
│ │ │ │ -
199
│ │ │ │ -
200}
│ │ │ │ -
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ -
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ -
Pose3.h
3D Pose
│ │ │ │ +
190private:
│ │ │ │ +
193 friend class boost::serialization::access;
│ │ │ │ +
194 template<class ARCHIVE>
│ │ │ │ +
195 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
196 ar & BOOST_SERIALIZATION_NVP(R_);
│ │ │ │ +
197 ar & BOOST_SERIALIZATION_NVP(t_);
│ │ │ │ +
198 ar & BOOST_SERIALIZATION_NVP(v_);
│ │ │ │ +
199 }
│ │ │ │ +
201};
│ │ │ │ +
│ │ │ │ +
202
│ │ │ │ +
203// Specialize NavState traits to use a Retract/Local that agrees with IMUFactors
│ │ │ │ +
204template<>
│ │ │ │ +
│ │ │ │ +
205struct traits<NavState> : internal::Manifold<NavState> {
│ │ │ │ +
206};
│ │ │ │ +
│ │ │ │ +
207
│ │ │ │ +
208} // namespace gtsam
│ │ │ │ +
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ +
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ +
Pose3.h
3D Pose
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ +
gtsam::Velocity3
Vector3 Velocity3
Velocity is currently typedef'd to Vector3.
Definition NavState.h:28
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
│ │ │ │ +
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
│ │ │ │ -
gtsam::PreintegratedRotationParams
Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
Definition PreintegratedRotation.h:31
│ │ │ │ -
gtsam::PreintegratedRotationParams::body_P_sensor
boost::optional< Pose3 > body_P_sensor
The pose of the sensor in the body frame.
Definition PreintegratedRotation.h:36
│ │ │ │ -
gtsam::PreintegratedRotationParams::omegaCoriolis
boost::optional< Vector3 > omegaCoriolis
Coriolis constant.
Definition PreintegratedRotation.h:35
│ │ │ │ -
gtsam::PreintegratedRotationParams::gyroscopeCovariance
Matrix3 gyroscopeCovariance
Continuous-time "Covariance" of gyroscope measurements The units for stddev are σ = rad/s/√Hz.
Definition PreintegratedRotation.h:34
│ │ │ │ -
gtsam::PreintegratedRotation
PreintegratedRotation is the base class for all PreintegratedMeasurements classes (in AHRSFactor,...
Definition PreintegratedRotation.h:89
│ │ │ │ -
gtsam::PreintegratedRotation::delRdelBiasOmega_
Matrix3 delRdelBiasOmega_
Jacobian of preintegrated rotation w.r.t. angular rate bias.
Definition PreintegratedRotation.h:99
│ │ │ │ -
gtsam::PreintegratedRotation::p_
boost::shared_ptr< Params > p_
Parameters.
Definition PreintegratedRotation.h:95
│ │ │ │ -
gtsam::PreintegratedRotation::PreintegratedRotation
PreintegratedRotation(const boost::shared_ptr< Params > &p, double deltaTij, const Rot3 &deltaRij, const Matrix3 &delRdelBiasOmega)
Explicit initialization of all class members.
Definition PreintegratedRotation.h:114
│ │ │ │ -
gtsam::PreintegratedRotation::PreintegratedRotation
PreintegratedRotation(const boost::shared_ptr< Params > &p)
Default constructor, resets integration to zero.
Definition PreintegratedRotation.h:109
│ │ │ │ -
gtsam::PreintegratedRotation::deltaTij_
double deltaTij_
Time interval from i to j.
Definition PreintegratedRotation.h:97
│ │ │ │ -
gtsam::PreintegratedRotation::matchesParamsWith
bool matchesParamsWith(const PreintegratedRotation &other) const
check parameters equality: checks whether shared pointer points to same Params object.
Definition PreintegratedRotation.h:128
│ │ │ │ -
gtsam::PreintegratedRotation::PreintegratedRotation
PreintegratedRotation()
Default constructor for serialization.
Definition PreintegratedRotation.h:102
│ │ │ │ -
gtsam::PreintegratedRotation::deltaRij_
Rot3 deltaRij_
Preintegrated relative orientation (in frame i)
Definition PreintegratedRotation.h:98
│ │ │ │ +
gtsam::Rot3::toQuaternion
gtsam::Quaternion toQuaternion() const
Compute the quaternion representation of this rotation.
Definition Rot3M.cpp:233
│ │ │ │ +
gtsam::Rot3::matrix
Matrix3 matrix() const
return 3*3 rotation matrix
Definition Rot3M.cpp:219
│ │ │ │ +
gtsam::NavState
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition NavState.h:34
│ │ │ │ +
gtsam::NavState::NavState
NavState()
Default constructor.
Definition NavState.h:55
│ │ │ │ +
gtsam::NavState::NavState
NavState(const Matrix3 &R, const Vector6 &tv)
Construct from SO(3) and R^6.
Definition NavState.h:67
│ │ │ │ +
gtsam::NavState::NavState
NavState(const Rot3 &R, const Point3 &t, const Velocity3 &v)
Construct from attitude, position, velocity.
Definition NavState.h:59
│ │ │ │ +
gtsam::NavState::R
Matrix3 R() const
Return rotation matrix. Induces computation in quaternion mode.
Definition NavState.h:95
│ │ │ │ +
gtsam::NavState::NavState
NavState(const Pose3 &pose, const Velocity3 &v)
Construct from pose and velocity.
Definition NavState.h:63
│ │ │ │ +
gtsam::NavState::v
const Vector3 & v() const
Return velocity as Vector3. Computation-free.
Definition NavState.h:107
│ │ │ │ +
gtsam::NavState::t
Vector3 t() const
Return position as Vector3.
Definition NavState.h:103
│ │ │ │ +
gtsam::NavState::quaternion
Quaternion quaternion() const
Return quaternion. Induces computation in matrix mode.
Definition NavState.h:99
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── encoding │ │ │ │ │ @@ -1 +1 @@ │ │ │ │ │ -utf-8 │ │ │ │ │ +us-ascii │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,253 +1,253 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -PreintegratedRotation.h │ │ │ │ │ +NavState.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -22#pragma once │ │ │ │ │ -23 │ │ │ │ │ -24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_3_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_3_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_c_t_o_r_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ +24 │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ 26 │ │ │ │ │ -27namespace _g_t_s_a_m { │ │ │ │ │ -28 │ │ │ │ │ -_3_1struct GTSAM_EXPORT _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s { │ │ │ │ │ -_3_4 Matrix3 _g_y_r_o_s_c_o_p_e_C_o_v_a_r_i_a_n_c_e; │ │ │ │ │ -_3_5 boost::optional _o_m_e_g_a_C_o_r_i_o_l_i_s; │ │ │ │ │ -_3_6 boost::optional _b_o_d_y___P___s_e_n_s_o_r; │ │ │ │ │ -37 │ │ │ │ │ -38 _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s() : gyroscopeCovariance(I_3x3) {} │ │ │ │ │ -39 │ │ │ │ │ -40 _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s(const Matrix3& gyroscope_covariance, │ │ │ │ │ -41 boost::optional omega_coriolis) │ │ │ │ │ -42 : gyroscopeCovariance(gyroscope_covariance) { │ │ │ │ │ -43 if (omega_coriolis) │ │ │ │ │ -44 omegaCoriolis.reset(omega_coriolis.get()); │ │ │ │ │ -45 } │ │ │ │ │ -46 │ │ │ │ │ -47 virtual ~PreintegratedRotationParams() {} │ │ │ │ │ +_2_8typedef Vector3 _V_e_l_o_c_i_t_y_3; │ │ │ │ │ +29 │ │ │ │ │ +_3_4class GTSAM_EXPORT _N_a_v_S_t_a_t_e { │ │ │ │ │ +35private: │ │ │ │ │ +36 │ │ │ │ │ +37 // TODO(frank): │ │ │ │ │ +38 // - should we rename t_ to p_? if not, we should rename dP do dT │ │ │ │ │ +39 _R_o_t_3 R_; │ │ │ │ │ +40 _P_o_i_n_t_3 t_; │ │ │ │ │ +41 _V_e_l_o_c_i_t_y_3 v_; │ │ │ │ │ +42 │ │ │ │ │ +43public: │ │ │ │ │ +44 │ │ │ │ │ +45 enum { │ │ │ │ │ +46 dimension = 9 │ │ │ │ │ +47 }; │ │ │ │ │ 48 │ │ │ │ │ -49 virtual void _p_r_i_n_t(const std::string& s) const; │ │ │ │ │ -50 virtual bool equals(const PreintegratedRotationParams& other, double tol=1e- │ │ │ │ │ -9) const; │ │ │ │ │ -51 │ │ │ │ │ -52 void setGyroscopeCovariance(const Matrix3& cov) { gyroscopeCovariance = cov; │ │ │ │ │ -} │ │ │ │ │ -53 void setOmegaCoriolis(const Vector3& omega) { omegaCoriolis.reset(omega); } │ │ │ │ │ -54 void setBodyPSensor(const Pose3& pose) { body_P_sensor.reset(pose); } │ │ │ │ │ -55 │ │ │ │ │ -56 const Matrix3& getGyroscopeCovariance() const { return gyroscopeCovariance; │ │ │ │ │ -} │ │ │ │ │ -57 boost::optional getOmegaCoriolis() const { return omegaCoriolis; } │ │ │ │ │ -58 boost::optional getBodyPSensor() const { return body_P_sensor; } │ │ │ │ │ -59 │ │ │ │ │ -60 private: │ │ │ │ │ -_6_2 friend class boost::serialization::access; │ │ │ │ │ -63 template │ │ │ │ │ -64 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -65 namespace bs = ::boost::serialization; │ │ │ │ │ -66 ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance); │ │ │ │ │ -67 ar & BOOST_SERIALIZATION_NVP(body_P_sensor); │ │ │ │ │ -68 │ │ │ │ │ -69 // Provide support for Eigen::Matrix in boost::optional │ │ │ │ │ -70 bool omegaCoriolisFlag = omegaCoriolis.is_initialized(); │ │ │ │ │ -71 ar & boost::serialization::make_nvp("omegaCoriolisFlag", omegaCoriolisFlag); │ │ │ │ │ -72 if (omegaCoriolisFlag) { │ │ │ │ │ -73 ar & BOOST_SERIALIZATION_NVP(*omegaCoriolis); │ │ │ │ │ -74 } │ │ │ │ │ -75 } │ │ │ │ │ -76 │ │ │ │ │ -77#ifdef GTSAM_USE_QUATERNIONS │ │ │ │ │ -78 // Align if we are using Quaternions │ │ │ │ │ -79public: │ │ │ │ │ -80 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -81#endif │ │ │ │ │ -82}; │ │ │ │ │ -83 │ │ │ │ │ -_8_9class GTSAM_EXPORT _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n { │ │ │ │ │ -90 public: │ │ │ │ │ -91 typedef _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s _P_a_r_a_m_s; │ │ │ │ │ -92 │ │ │ │ │ -93 protected: │ │ │ │ │ -_9_5 boost::shared_ptr _p__; │ │ │ │ │ -96 │ │ │ │ │ -_9_7 double _d_e_l_t_a_T_i_j__; │ │ │ │ │ -_9_8 _R_o_t_3 _d_e_l_t_a_R_i_j__; │ │ │ │ │ -_9_9 Matrix3 _d_e_l_R_d_e_l_B_i_a_s_O_m_e_g_a__; │ │ │ │ │ -100 │ │ │ │ │ -_1_0_2 _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n() {} │ │ │ │ │ -103 │ │ │ │ │ -104 public: │ │ │ │ │ -107 │ │ │ │ │ -_1_0_9 explicit _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n(const boost::shared_ptr& p) : p_(p) │ │ │ │ │ -{ │ │ │ │ │ -110 resetIntegration(); │ │ │ │ │ -111 } │ │ │ │ │ +49 typedef std::pair PositionAndVelocity; │ │ │ │ │ +50 │ │ │ │ │ +53 │ │ │ │ │ +_5_5 _N_a_v_S_t_a_t_e() : │ │ │ │ │ +56 t_(0, 0, 0), v_(Vector3::Zero()) { │ │ │ │ │ +57 } │ │ │ │ │ +_5_9 _N_a_v_S_t_a_t_e(const _R_o_t_3& R, const _P_o_i_n_t_3& t, const _V_e_l_o_c_i_t_y_3& v) : │ │ │ │ │ +60 R_(R), t_(t), v_(v) { │ │ │ │ │ +61 } │ │ │ │ │ +_6_3 _N_a_v_S_t_a_t_e(const _P_o_s_e_3& pose, const _V_e_l_o_c_i_t_y_3& v) : │ │ │ │ │ +64 R_(pose.rotation()), t_(pose.translation()), v_(v) { │ │ │ │ │ +65 } │ │ │ │ │ +_6_7 _N_a_v_S_t_a_t_e(const Matrix3& R, const Vector6& tv) : │ │ │ │ │ +68 R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) { │ │ │ │ │ +69 } │ │ │ │ │ +71 static _N_a_v_S_t_a_t_e Create(const _R_o_t_3& R, const _P_o_i_n_t_3& t, const _V_e_l_o_c_i_t_y_3& v, │ │ │ │ │ +72 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _3_> H1, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _3_> H2, │ │ │ │ │ +73 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _3_> H3); │ │ │ │ │ +75 static _N_a_v_S_t_a_t_e FromPoseVelocity(const _P_o_s_e_3& pose, const Vector3& vel, │ │ │ │ │ +76 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _6_> H1, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _3_> H2); │ │ │ │ │ +77 │ │ │ │ │ +81 │ │ │ │ │ +82 const _R_o_t_3& attitude(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _9_> H = boost::none) const; │ │ │ │ │ +83 const _P_o_i_n_t_3& position(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _9_> H = boost::none) const; │ │ │ │ │ +84 const _V_e_l_o_c_i_t_y_3& velocity(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _9_> H = boost::none) const; │ │ │ │ │ +85 │ │ │ │ │ +86 const _P_o_s_e_3 pose() const { │ │ │ │ │ +87 return _P_o_s_e_3(attitude(), position()); │ │ │ │ │ +88 } │ │ │ │ │ +89 │ │ │ │ │ +93 │ │ │ │ │ +_9_5 Matrix3 _R() const { │ │ │ │ │ +96 return R_._m_a_t_r_i_x(); │ │ │ │ │ +97 } │ │ │ │ │ +_9_9 Quaternion _q_u_a_t_e_r_n_i_o_n() const { │ │ │ │ │ +100 return R_._t_o_Q_u_a_t_e_r_n_i_o_n(); │ │ │ │ │ +101 } │ │ │ │ │ +_1_0_3 Vector3 _t() const { │ │ │ │ │ +104 return t_; │ │ │ │ │ +105 } │ │ │ │ │ +_1_0_7 const Vector3& _v() const { │ │ │ │ │ +108 return v_; │ │ │ │ │ +109 } │ │ │ │ │ +110 // Return velocity in body frame │ │ │ │ │ +111 _V_e_l_o_c_i_t_y_3 bodyVelocity(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _9_> H = boost::none) const; │ │ │ │ │ 112 │ │ │ │ │ -_1_1_4 _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n(const boost::shared_ptr& p, │ │ │ │ │ -115 double deltaTij, const _R_o_t_3& deltaRij, │ │ │ │ │ -116 const Matrix3& delRdelBiasOmega) │ │ │ │ │ -117 : p_(p), deltaTij_(deltaTij), deltaRij_(deltaRij), delRdelBiasOmega_ │ │ │ │ │ -(delRdelBiasOmega) {} │ │ │ │ │ -118 │ │ │ │ │ -120 │ │ │ │ │ -123 │ │ │ │ │ -125 void resetIntegration(); │ │ │ │ │ -126 │ │ │ │ │ -_1_2_8 bool _m_a_t_c_h_e_s_P_a_r_a_m_s_W_i_t_h(const _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n& other) const { │ │ │ │ │ -129 return p_ == other._p__; │ │ │ │ │ -130 } │ │ │ │ │ -132 │ │ │ │ │ -135 const boost::shared_ptr& params() const { │ │ │ │ │ -136 return p_; │ │ │ │ │ -137 } │ │ │ │ │ -138 const double& deltaTij() const { │ │ │ │ │ -139 return deltaTij_; │ │ │ │ │ +116 Matrix7 matrix() const; │ │ │ │ │ +117 │ │ │ │ │ +121 │ │ │ │ │ +123 GTSAM_EXPORT │ │ │ │ │ +124 friend std::ostream &operator<<(std::ostream &os, const _N_a_v_S_t_a_t_e& state); │ │ │ │ │ +125 │ │ │ │ │ +127 void _p_r_i_n_t(const std::string& s = "") const; │ │ │ │ │ +128 │ │ │ │ │ +130 bool _e_q_u_a_l_s(const _N_a_v_S_t_a_t_e& other, double tol = 1e-8) const; │ │ │ │ │ +131 │ │ │ │ │ +135 │ │ │ │ │ +136 // Tangent space sugar. │ │ │ │ │ +137 // TODO(frank): move to private navstate namespace in cpp │ │ │ │ │ +138 static Eigen::Block dR(Vector9& v) { │ │ │ │ │ +139 return v.segment<3>(0); │ │ │ │ │ 140 } │ │ │ │ │ -141 const Rot3& deltaRij() const { │ │ │ │ │ -142 return deltaRij_; │ │ │ │ │ +141 static Eigen::Block dP(Vector9& v) { │ │ │ │ │ +142 return v.segment<3>(3); │ │ │ │ │ 143 } │ │ │ │ │ -144 const Matrix3& delRdelBiasOmega() const { │ │ │ │ │ -145 return delRdelBiasOmega_; │ │ │ │ │ +144 static Eigen::Block dV(Vector9& v) { │ │ │ │ │ +145 return v.segment<3>(6); │ │ │ │ │ 146 } │ │ │ │ │ -148 │ │ │ │ │ -151 void _p_r_i_n_t(const std::string& s) const; │ │ │ │ │ -152 bool equals(const PreintegratedRotation& other, double tol) const; │ │ │ │ │ -154 │ │ │ │ │ -157 │ │ │ │ │ -161 Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& │ │ │ │ │ -biasHat, double deltaT, │ │ │ │ │ -162 OptionalJacobian<3, 3> D_incrR_integratedOmega) const; │ │ │ │ │ -163 │ │ │ │ │ -166 void integrateMeasurement(const Vector3& measuredOmega, const Vector3& │ │ │ │ │ -biasHat, double deltaT, │ │ │ │ │ -167 OptionalJacobian<3, 3> D_incrR_integratedOmega = boost::none, │ │ │ │ │ -168 OptionalJacobian<3, 3> F = boost::none); │ │ │ │ │ -169 │ │ │ │ │ -171 Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr, │ │ │ │ │ -172 OptionalJacobian<3, 3> H = boost::none) const; │ │ │ │ │ -173 │ │ │ │ │ -175 Vector3 integrateCoriolis(const Rot3& rot_i) const; │ │ │ │ │ +147 static Eigen::Block dR(const Vector9& v) { │ │ │ │ │ +148 return v.segment<3>(0); │ │ │ │ │ +149 } │ │ │ │ │ +150 static Eigen::Block dP(const Vector9& v) { │ │ │ │ │ +151 return v.segment<3>(3); │ │ │ │ │ +152 } │ │ │ │ │ +153 static Eigen::Block dV(const Vector9& v) { │ │ │ │ │ +154 return v.segment<3>(6); │ │ │ │ │ +155 } │ │ │ │ │ +156 │ │ │ │ │ +158 NavState retract(const Vector9& v, // │ │ │ │ │ +159 OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 = │ │ │ │ │ +160 boost::none) const; │ │ │ │ │ +161 │ │ │ │ │ +163 Vector9 localCoordinates(const NavState& g, // │ │ │ │ │ +164 OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 = │ │ │ │ │ +165 boost::none) const; │ │ │ │ │ +166 │ │ │ │ │ +170 │ │ │ │ │ +173 NavState update(const Vector3& b_acceleration, const Vector3& b_omega, │ │ │ │ │ +174 const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, │ │ │ │ │ +175 OptionalJacobian<9, 3> G2) const; │ │ │ │ │ 176 │ │ │ │ │ -178 │ │ │ │ │ -179 private: │ │ │ │ │ -_1_8_1 friend class boost::serialization::access; │ │ │ │ │ -182 template │ │ │ │ │ -183 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { // NOLINT │ │ │ │ │ -184 ar& BOOST_SERIALIZATION_NVP(p_); │ │ │ │ │ -185 ar& BOOST_SERIALIZATION_NVP(deltaTij_); │ │ │ │ │ -186 ar& BOOST_SERIALIZATION_NVP(deltaRij_); │ │ │ │ │ -187 ar& BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); │ │ │ │ │ -188 } │ │ │ │ │ +178 Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false, │ │ │ │ │ +179 OptionalJacobian<9, 9> H = boost::none) const; │ │ │ │ │ +180 │ │ │ │ │ +183 Vector9 correctPIM(const Vector9& pim, double dt, const Vector3& n_gravity, │ │ │ │ │ +184 const boost::optional& omegaCoriolis, bool use2ndOrderCoriolis = │ │ │ │ │ +185 false, OptionalJacobian<9, 9> H1 = boost::none, │ │ │ │ │ +186 OptionalJacobian<9, 9> H2 = boost::none) const; │ │ │ │ │ +187 │ │ │ │ │ 189 │ │ │ │ │ -190#ifdef GTSAM_USE_QUATERNIONS │ │ │ │ │ -191 // Align if we are using Quaternions │ │ │ │ │ -192 public: │ │ │ │ │ -193 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -194#endif │ │ │ │ │ -195}; │ │ │ │ │ -196 │ │ │ │ │ -197template <> │ │ │ │ │ -_1_9_8struct _t_r_a_i_t_s<_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n> : public │ │ │ │ │ -_T_e_s_t_a_b_l_e {}; │ │ │ │ │ -199 │ │ │ │ │ -200} │ │ │ │ │ -_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ -This marks a GTSAM object to require alignment. │ │ │ │ │ -DDeeffiinniittiioonn types.h:308 │ │ │ │ │ -_M_a_t_r_i_x_._h │ │ │ │ │ -typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +190private: │ │ │ │ │ +_1_9_3 friend class boost::serialization::access; │ │ │ │ │ +194 template │ │ │ │ │ +195 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +196 ar & BOOST_SERIALIZATION_NVP(R_); │ │ │ │ │ +197 ar & BOOST_SERIALIZATION_NVP(t_); │ │ │ │ │ +198 ar & BOOST_SERIALIZATION_NVP(v_); │ │ │ │ │ +199 } │ │ │ │ │ +201}; │ │ │ │ │ +202 │ │ │ │ │ +203// Specialize NavState traits to use a Retract/Local that agrees with │ │ │ │ │ +IMUFactors │ │ │ │ │ +204template<> │ │ │ │ │ +_2_0_5struct _t_r_a_i_t_s<_N_a_v_S_t_a_t_e> : _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d { │ │ │ │ │ +206}; │ │ │ │ │ +207 │ │ │ │ │ +208} // namespace gtsam │ │ │ │ │ +_V_e_c_t_o_r_._h │ │ │ │ │ +typedef and functions to augment Eigen's VectorXd │ │ │ │ │ +_M_a_n_i_f_o_l_d_._h │ │ │ │ │ +Base class and basic functions for Manifold types. │ │ │ │ │ _P_o_s_e_3_._h │ │ │ │ │ 3D Pose │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ print without optional string, must specify cout yourself │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ +Vector3 Point3 │ │ │ │ │ +As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ +typedef Point3 to Vector3... │ │ │ │ │ +DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_l_o_c_i_t_y_3 │ │ │ │ │ +Vector3 Velocity3 │ │ │ │ │ +Velocity is currently typedef'd to Vector3. │ │ │ │ │ +DDeeffiinniittiioonn NavState.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ +Both ManifoldTraits and Testable. │ │ │ │ │ +DDeeffiinniittiioonn Manifold.h:120 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ +either a fixed size o... │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ +A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ _g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor │ │ │ │ │ symbol GTSAM_USE_QUATERNIO... │ │ │ │ │ DDeeffiinniittiioonn Rot3.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ -Parameters for pre-integration: Usage: Create just a single Params and pass a │ │ │ │ │ -shared pointer to the c... │ │ │ │ │ -DDeeffiinniittiioonn PreintegratedRotation.h:31 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s_:_:_b_o_d_y___P___s_e_n_s_o_r │ │ │ │ │ -boost::optional< Pose3 > body_P_sensor │ │ │ │ │ -The pose of the sensor in the body frame. │ │ │ │ │ -DDeeffiinniittiioonn PreintegratedRotation.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s_:_:_o_m_e_g_a_C_o_r_i_o_l_i_s │ │ │ │ │ -boost::optional< Vector3 > omegaCoriolis │ │ │ │ │ -Coriolis constant. │ │ │ │ │ -DDeeffiinniittiioonn PreintegratedRotation.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s_:_:_g_y_r_o_s_c_o_p_e_C_o_v_a_r_i_a_n_c_e │ │ │ │ │ -Matrix3 gyroscopeCovariance │ │ │ │ │ -Continuous-time "Covariance" of gyroscope measurements The units for stddev are │ │ │ │ │ -σ = rad/s/√Hz. │ │ │ │ │ -DDeeffiinniittiioonn PreintegratedRotation.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n │ │ │ │ │ -PreintegratedRotation is the base class for all PreintegratedMeasurements │ │ │ │ │ -classes (in AHRSFactor,... │ │ │ │ │ -DDeeffiinniittiioonn PreintegratedRotation.h:89 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_:_:_d_e_l_R_d_e_l_B_i_a_s_O_m_e_g_a__ │ │ │ │ │ -Matrix3 delRdelBiasOmega_ │ │ │ │ │ -Jacobian of preintegrated rotation w.r.t. angular rate bias. │ │ │ │ │ -DDeeffiinniittiioonn PreintegratedRotation.h:99 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_:_:_p__ │ │ │ │ │ -boost::shared_ptr< Params > p_ │ │ │ │ │ -Parameters. │ │ │ │ │ -DDeeffiinniittiioonn PreintegratedRotation.h:95 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n │ │ │ │ │ -PreintegratedRotation(const boost::shared_ptr< Params > &p, double deltaTij, │ │ │ │ │ -const Rot3 &deltaRij, const Matrix3 &delRdelBiasOmega) │ │ │ │ │ -Explicit initialization of all class members. │ │ │ │ │ -DDeeffiinniittiioonn PreintegratedRotation.h:114 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n │ │ │ │ │ -PreintegratedRotation(const boost::shared_ptr< Params > &p) │ │ │ │ │ -Default constructor, resets integration to zero. │ │ │ │ │ -DDeeffiinniittiioonn PreintegratedRotation.h:109 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_:_:_d_e_l_t_a_T_i_j__ │ │ │ │ │ -double deltaTij_ │ │ │ │ │ -Time interval from i to j. │ │ │ │ │ -DDeeffiinniittiioonn PreintegratedRotation.h:97 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_:_:_m_a_t_c_h_e_s_P_a_r_a_m_s_W_i_t_h │ │ │ │ │ -bool matchesParamsWith(const PreintegratedRotation &other) const │ │ │ │ │ -check parameters equality: checks whether shared pointer points to same Params │ │ │ │ │ -object. │ │ │ │ │ -DDeeffiinniittiioonn PreintegratedRotation.h:128 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n │ │ │ │ │ -PreintegratedRotation() │ │ │ │ │ -Default constructor for serialization. │ │ │ │ │ -DDeeffiinniittiioonn PreintegratedRotation.h:102 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_:_:_d_e_l_t_a_R_i_j__ │ │ │ │ │ -Rot3 deltaRij_ │ │ │ │ │ -Preintegrated relative orientation (in frame i) │ │ │ │ │ -DDeeffiinniittiioonn PreintegratedRotation.h:98 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_3_:_:_t_o_Q_u_a_t_e_r_n_i_o_n │ │ │ │ │ +gtsam::Quaternion toQuaternion() const │ │ │ │ │ +Compute the quaternion representation of this rotation. │ │ │ │ │ +DDeeffiinniittiioonn Rot3M.cpp:233 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_3_:_:_m_a_t_r_i_x │ │ │ │ │ +Matrix3 matrix() const │ │ │ │ │ +return 3*3 rotation matrix │ │ │ │ │ +DDeeffiinniittiioonn Rot3M.cpp:219 │ │ │ │ │ +_g_t_s_a_m_:_:_N_a_v_S_t_a_t_e │ │ │ │ │ +Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does │ │ │ │ │ +not make sense to make... │ │ │ │ │ +DDeeffiinniittiioonn NavState.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_N_a_v_S_t_a_t_e_:_:_N_a_v_S_t_a_t_e │ │ │ │ │ +NavState() │ │ │ │ │ +Default constructor. │ │ │ │ │ +DDeeffiinniittiioonn NavState.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_N_a_v_S_t_a_t_e_:_:_N_a_v_S_t_a_t_e │ │ │ │ │ +NavState(const Matrix3 &R, const Vector6 &tv) │ │ │ │ │ +Construct from SO(3) and R^6. │ │ │ │ │ +DDeeffiinniittiioonn NavState.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_N_a_v_S_t_a_t_e_:_:_N_a_v_S_t_a_t_e │ │ │ │ │ +NavState(const Rot3 &R, const Point3 &t, const Velocity3 &v) │ │ │ │ │ +Construct from attitude, position, velocity. │ │ │ │ │ +DDeeffiinniittiioonn NavState.h:59 │ │ │ │ │ +_g_t_s_a_m_:_:_N_a_v_S_t_a_t_e_:_:_R │ │ │ │ │ +Matrix3 R() const │ │ │ │ │ +Return rotation matrix. Induces computation in quaternion mode. │ │ │ │ │ +DDeeffiinniittiioonn NavState.h:95 │ │ │ │ │ +_g_t_s_a_m_:_:_N_a_v_S_t_a_t_e_:_:_N_a_v_S_t_a_t_e │ │ │ │ │ +NavState(const Pose3 &pose, const Velocity3 &v) │ │ │ │ │ +Construct from pose and velocity. │ │ │ │ │ +DDeeffiinniittiioonn NavState.h:63 │ │ │ │ │ +_g_t_s_a_m_:_:_N_a_v_S_t_a_t_e_:_:_v │ │ │ │ │ +const Vector3 & v() const │ │ │ │ │ +Return velocity as Vector3. Computation-free. │ │ │ │ │ +DDeeffiinniittiioonn NavState.h:107 │ │ │ │ │ +_g_t_s_a_m_:_:_N_a_v_S_t_a_t_e_:_:_t │ │ │ │ │ +Vector3 t() const │ │ │ │ │ +Return position as Vector3. │ │ │ │ │ +DDeeffiinniittiioonn NavState.h:103 │ │ │ │ │ +_g_t_s_a_m_:_:_N_a_v_S_t_a_t_e_:_:_q_u_a_t_e_r_n_i_o_n │ │ │ │ │ +Quaternion quaternion() const │ │ │ │ │ +Return quaternion. Induces computation in matrix mode. │ │ │ │ │ +DDeeffiinniittiioonn NavState.h:99 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_._h │ │ │ │ │ + * _N_a_v_S_t_a_t_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00911.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ImuFactor.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/BarometricFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,51 +94,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
ImuFactor.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
BarometricFactor.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ + │ │ │ │ +

Header file for Barometric factor. │ │ │ │ +More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::BarometricFactor
 Prior on height in a cartesian frame. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -std::ostream & gtsam::operator<< (std::ostream &os, const ImuFactor &f)
 
│ │ │ │ -std::ostream & gtsam::operator<< (std::ostream &os, const ImuFactor2 &f)
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Luca Carlone
│ │ │ │ -
│ │ │ │ -Stephen Williams
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ -
│ │ │ │ -Vadim Indelman
│ │ │ │ -
│ │ │ │ -David Jensen
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ +

Header file for Barometric factor.

│ │ │ │ +
Author
Peter Milani
│ │ │ │ +
Date
December 16, 2021
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,27 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -ImuFactor.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +BarometricFactor.h File Reference │ │ │ │ │ +Header file for Barometric factor. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r │ │ │ │ │ +  Prior on height in a cartesian frame. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _I_m_u_F_a_c_t_o_r &f) │ │ │ │ │ -  │ │ │ │ │ -std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _I_m_u_F_a_c_t_o_r_2 &f) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Header file for Barometric factor. │ │ │ │ │ Author │ │ │ │ │ - Luca Carlone │ │ │ │ │ - Stephen Williams │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Vadim Indelman │ │ │ │ │ - David Jensen │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Peter Milani │ │ │ │ │ + Date │ │ │ │ │ + December 16, 2021 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _I_m_u_F_a_c_t_o_r_._c_p_p │ │ │ │ │ + * _B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00914.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AttitudeFactor.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AHRSFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,36 +94,47 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
AttitudeFactor.cpp File Reference
│ │ │ │ +
AHRSFactor.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Implementation file for Attitude factor. │ │ │ │ -More...

│ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::PreintegratedAhrsMeasurements
 PreintegratedAHRSMeasurements accumulates (integrates) the Gyroscope measurements (rotation rates) and the corresponding covariance matrix. More...
 
class  gtsam::AHRSFactor
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Implementation file for Attitude factor.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
Date
January 28, 2014
│ │ │ │ +
Author
Krunal Chande
│ │ │ │ +
│ │ │ │ +Luca Carlone
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +
Date
July 2014
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,31 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -AttitudeFactor.cpp File Reference │ │ │ │ │ -Implementation file for Attitude factor. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +AHRSFactor.h File Reference │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ + PreintegratedAHRSMeasurements accumulates (integrates) the Gyroscope │ │ │ │ │ +  measurements (rotation rates) and the corresponding covariance matrix. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_A_H_R_S_F_a_c_t_o_r │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Implementation file for Attitude factor. │ │ │ │ │ Author │ │ │ │ │ + Krunal Chande │ │ │ │ │ + Luca Carlone │ │ │ │ │ Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - January 28, 2014 │ │ │ │ │ + July 2014 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _A_t_t_i_t_u_d_e_F_a_c_t_o_r_._c_p_p │ │ │ │ │ + * _A_H_R_S_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00923.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/CombinedImuFactor.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AttitudeFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,87 +94,57 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Macros | │ │ │ │ -Functions
│ │ │ │ -
CombinedImuFactor.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
AttitudeFactor.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ + │ │ │ │ +

Header file for Attitude factor. │ │ │ │ +More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::AttitudeFactor
 Base class for prior on attitude Example: More...
 
class  gtsam::Rot3AttitudeFactor
 Version of AttitudeFactor for Rot3. More...
 
struct  gtsam::traits< Rot3AttitudeFactor >
 traits More...
 
class  gtsam::Pose3AttitudeFactor
 Version of AttitudeFactor for Pose3. More...
 
struct  gtsam::traits< Pose3AttitudeFactor >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Macros

│ │ │ │ -#define D_R_R(H)   (H)->block<3,3>(0,0)
 
│ │ │ │ -#define D_R_t(H)   (H)->block<3,3>(0,3)
 
│ │ │ │ -#define D_R_v(H)   (H)->block<3,3>(0,6)
 
│ │ │ │ -#define D_t_R(H)   (H)->block<3,3>(3,0)
 
│ │ │ │ -#define D_t_t(H)   (H)->block<3,3>(3,3)
 
│ │ │ │ -#define D_t_v(H)   (H)->block<3,3>(3,6)
 
│ │ │ │ -#define D_v_R(H)   (H)->block<3,3>(6,0)
 
│ │ │ │ -#define D_v_t(H)   (H)->block<3,3>(6,3)
 
│ │ │ │ -#define D_v_v(H)   (H)->block<3,3>(6,6)
 
│ │ │ │ -#define D_a_a(H)   (H)->block<3,3>(9,9)
 
│ │ │ │ -#define D_g_g(H)   (H)->block<3,3>(12,12)
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -std::ostream & gtsam::operator<< (std::ostream &os, const CombinedImuFactor &f)
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Luca Carlone
│ │ │ │ -
│ │ │ │ -Stephen Williams
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ -
│ │ │ │ -Vadim Indelman
│ │ │ │ -
│ │ │ │ -David Jensen
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ -
│ │ │ │ -Varun Agrawal
│ │ │ │ +

Header file for Attitude factor.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
Date
January 28, 2014
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,51 +1,39 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -CombinedImuFactor.cpp File Reference │ │ │ │ │ -NNaammeessppaacceess │ │ │ │ │ -namespace   _g_t_s_a_m │ │ │ │ │ -  Global functions in a separate testing namespace. │ │ │ │ │ -  │ │ │ │ │ -MMaaccrrooss │ │ │ │ │ -#define  DD__RR__RR(H)   (H)->block<3,3>(0,0) │ │ │ │ │ -  │ │ │ │ │ -#define  DD__RR__tt(H)   (H)->block<3,3>(0,3) │ │ │ │ │ -  │ │ │ │ │ -#define  DD__RR__vv(H)   (H)->block<3,3>(0,6) │ │ │ │ │ -  │ │ │ │ │ -#define  DD__tt__RR(H)   (H)->block<3,3>(3,0) │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +AttitudeFactor.h File Reference │ │ │ │ │ +Header file for Attitude factor. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_A_t_t_i_t_u_d_e_F_a_c_t_o_r │ │ │ │ │ +  Base class for prior on attitude Example: _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r │ │ │ │ │ +  Version of _A_t_t_i_t_u_d_e_F_a_c_t_o_r for _R_o_t_3. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -#define  DD__tt__tt(H)   (H)->block<3,3>(3,3) │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -#define  DD__tt__vv(H)   (H)->block<3,3>(3,6) │ │ │ │ │ + class   _g_t_s_a_m_:_:_P_o_s_e_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r │ │ │ │ │ +  Version of _A_t_t_i_t_u_d_e_F_a_c_t_o_r for _P_o_s_e_3. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -#define  DD__vv__RR(H)   (H)->block<3,3>(6,0) │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _P_o_s_e_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -#define  DD__vv__tt(H)   (H)->block<3,3>(6,3) │ │ │ │ │ -  │ │ │ │ │ -#define  DD__vv__vv(H)   (H)->block<3,3>(6,6) │ │ │ │ │ -  │ │ │ │ │ -#define  DD__aa__aa(H)   (H)->block<3,3>(9,9) │ │ │ │ │ -  │ │ │ │ │ -#define  DD__gg__gg(H)   (H)->block<3,3>(12,12) │ │ │ │ │ -  │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r │ │ │ │ │ - &f) │ │ │ │ │ +NNaammeessppaacceess │ │ │ │ │ +namespace   _g_t_s_a_m │ │ │ │ │ +  Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Header file for Attitude factor. │ │ │ │ │ Author │ │ │ │ │ - Luca Carlone │ │ │ │ │ - Stephen Williams │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Vadim Indelman │ │ │ │ │ - David Jensen │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + Date │ │ │ │ │ + January 28, 2014 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r_._c_p_p │ │ │ │ │ + * _A_t_t_i_t_u_d_e_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00926.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ConstantVelocityFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ImuBias.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,44 +94,43 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
ConstantVelocityFactor.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
ImuBias.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ - │ │ │ │ -

Maintain a constant velocity motion model between two NavStates. │ │ │ │ -More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::ConstantVelocityFactor
 Binary factor for applying a constant velocity model to a moving body represented as a NavState. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
namespace  gtsam::imuBias
 All bias models live in the imuBias namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +std::ostream & gtsam::imuBias::operator<< (std::ostream &os, const ConstantBias &bias)
 ostream operator
 
│ │ │ │

Detailed Description

│ │ │ │ -

Maintain a constant velocity motion model between two NavStates.

│ │ │ │ -
Author
Asa Hammond
│ │ │ │ +
Date
Feb 2, 2012
│ │ │ │ +
Author
Vadim Indelman, Stephen Williams
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,26 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -ConstantVelocityFactor.h File Reference │ │ │ │ │ -Maintain a constant velocity motion model between two NavStates. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_C_o_n_s_t_a_n_t_V_e_l_o_c_i_t_y_F_a_c_t_o_r │ │ │ │ │ -  Binary factor for applying a constant velocity model to a moving body │ │ │ │ │ - represented as a _N_a_v_S_t_a_t_e. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +ImuBias.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +namespace   _g_t_s_a_m_:_:_i_m_u_B_i_a_s │ │ │ │ │ +  All bias models live in the _i_m_u_B_i_a_s namespace. │ │ │ │ │ +  │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +std::ostream &  ggttssaamm::::iimmuuBBiiaass::::ooppeerraattoorr<<<< (std::ostream &os, const │ │ │ │ │ + _C_o_n_s_t_a_n_t_B_i_a_s &bias) │ │ │ │ │ +  ostream operator │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Maintain a constant velocity motion model between two NavStates. │ │ │ │ │ + Date │ │ │ │ │ + Feb 2, 2012 │ │ │ │ │ Author │ │ │ │ │ - Asa Hammond │ │ │ │ │ + Vadim Indelman, Stephen Williams │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _C_o_n_s_t_a_n_t_V_e_l_o_c_i_t_y_F_a_c_t_o_r_._h │ │ │ │ │ + * _I_m_u_B_i_a_s_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00926.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a00926 = [ │ │ │ │ │ - ["gtsam::ConstantVelocityFactor", "a04128.html", "a04128"] │ │ │ │ │ + ["operator<<", "a00926.html#ae7a6582902c6daf9f20f2f7e466c60d8", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00932.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/Scenario.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/TangentPreintegration.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,48 +96,40 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
Scenario.h File Reference
│ │ │ │ +
TangentPreintegration.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Simple class to test navigation scenarios. │ │ │ │ -More...

│ │ │ │ - │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::Scenario
 Simple trajectory simulator. More...
 
class  gtsam::ConstantTwistScenario
 Scenario with constant twist 3D trajectory. More...
 
class  gtsam::AcceleratingScenario
 Accelerating from an arbitrary initial state, with optional rotation. More...
class  gtsam::TangentPreintegration
 Integrate on the 9D tangent space of the NavState manifold. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Simple class to test navigation scenarios.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ +Adam Bry
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,32 +1,24 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Scenario.h File Reference │ │ │ │ │ -Simple class to test navigation scenarios. _M_o_r_e_._._. │ │ │ │ │ +TangentPreintegration.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_S_c_e_n_a_r_i_o │ │ │ │ │ -  Simple trajectory simulator. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_C_o_n_s_t_a_n_t_T_w_i_s_t_S_c_e_n_a_r_i_o │ │ │ │ │ -  _S_c_e_n_a_r_i_o with constant twist 3D trajectory. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_A_c_c_e_l_e_r_a_t_i_n_g_S_c_e_n_a_r_i_o │ │ │ │ │ -  Accelerating from an arbitrary initial state, with optional rotation. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n │ │ │ │ │ +  Integrate on the 9D tangent space of the _N_a_v_S_t_a_t_e manifold. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Simple class to test navigation scenarios. │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Adam Bry │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _S_c_e_n_a_r_i_o_._h │ │ │ │ │ + * _T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00932.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,5 +1,3 @@ │ │ │ │ │ var a00932 = [ │ │ │ │ │ - ["gtsam::Scenario", "a04224.html", "a04224"], │ │ │ │ │ - ["gtsam::ConstantTwistScenario", "a04228.html", "a04228"], │ │ │ │ │ - ["gtsam::AcceleratingScenario", "a04232.html", "a04232"] │ │ │ │ │ + ["gtsam::TangentPreintegration", "a04244.html", "a04244"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00932_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/Scenario.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/TangentPreintegration.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,152 +98,138 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Scenario.h
│ │ │ │ +
TangentPreintegration.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
18#pragma once
│ │ │ │ -
19#include <gtsam/linear/NoiseModel.h>
│ │ │ │ -
20#include <gtsam/navigation/NavState.h>
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/navigation/PreintegrationBase.h>
│ │ │ │
21
│ │ │ │
22namespace gtsam {
│ │ │ │
23
│ │ │ │ -
│ │ │ │ -
25class Scenario {
│ │ │ │ -
26 public:
│ │ │ │ -
28 virtual ~Scenario() {}
│ │ │ │ -
29
│ │ │ │ -
30 // Quantities a Scenario needs to specify:
│ │ │ │ -
31
│ │ │ │ -
32 virtual Pose3 pose(double t) const = 0;
│ │ │ │ -
33 virtual Vector3 omega_b(double t) const = 0;
│ │ │ │ -
34 virtual Vector3 velocity_n(double t) const = 0;
│ │ │ │ -
35 virtual Vector3 acceleration_n(double t) const = 0;
│ │ │ │ -
36
│ │ │ │ -
37 // Derived quantities:
│ │ │ │ +
│ │ │ │ +
28class GTSAM_EXPORT TangentPreintegration : public PreintegrationBase {
│ │ │ │ +
29 protected:
│ │ │ │ +
30
│ │ │ │ +
35 Vector9 preintegrated_;
│ │ │ │ +
36 Matrix93 preintegrated_H_biasAcc_;
│ │ │ │ +
37 Matrix93 preintegrated_H_biasOmega_;
│ │ │ │
38
│ │ │ │ -
39 Rot3 rotation(double t) const { return pose(t).rotation(); }
│ │ │ │ -
40 NavState navState(double t) const { return NavState(pose(t), velocity_n(t)); }
│ │ │ │ -
41
│ │ │ │ -
42 Vector3 velocity_b(double t) const {
│ │ │ │ -
43 const Rot3 nRb = rotation(t);
│ │ │ │ -
44 return nRb.transpose() * velocity_n(t);
│ │ │ │ -
45 }
│ │ │ │ -
46
│ │ │ │ -
47 Vector3 acceleration_b(double t) const {
│ │ │ │ -
48 const Rot3 nRb = rotation(t);
│ │ │ │ -
49 return nRb.transpose() * acceleration_n(t);
│ │ │ │ -
50 }
│ │ │ │ -
51};
│ │ │ │ -
│ │ │ │ -
52
│ │ │ │ -
│ │ │ │ -
60class ConstantTwistScenario : public Scenario {
│ │ │ │ -
61 public:
│ │ │ │ -
│ │ │ │ -
63 ConstantTwistScenario(const Vector3& w, const Vector3& v,
│ │ │ │ -
64 const Pose3& nTb0 = Pose3())
│ │ │ │ -
65 : twist_((Vector6() << w, v).finished()), a_b_(w.cross(v)), nTb0_(nTb0) {}
│ │ │ │ -
│ │ │ │ +
│ │ │ │ +
40 TangentPreintegration() {
│ │ │ │ +
41 resetIntegration();
│ │ │ │ +
42 }
│ │ │ │ +
│ │ │ │ +
43
│ │ │ │ +
44public:
│ │ │ │ +
47
│ │ │ │ +
53 TangentPreintegration(const boost::shared_ptr<Params>& p,
│ │ │ │ +
54 const imuBias::ConstantBias& biasHat = imuBias::ConstantBias());
│ │ │ │ +
55
│ │ │ │ +
│ │ │ │ +
57 ~TangentPreintegration() override {
│ │ │ │ +
58 }
│ │ │ │ +
│ │ │ │ +
59
│ │ │ │ +
61
│ │ │ │ +
65 void resetIntegration() override;
│ │ │ │
66
│ │ │ │ -
│ │ │ │ -
67 Pose3 pose(double t) const override {
│ │ │ │ -
68 return nTb0_ * Pose3::Expmap(twist_ * t);
│ │ │ │ -
69 }
│ │ │ │ -
│ │ │ │ -
70 Vector3 omega_b(double t) const override { return twist_.head<3>(); }
│ │ │ │ -
│ │ │ │ -
71 Vector3 velocity_n(double t) const override {
│ │ │ │ -
72 return rotation(t).matrix() * twist_.tail<3>();
│ │ │ │ -
73 }
│ │ │ │ -
│ │ │ │ -
74 Vector3 acceleration_n(double t) const override { return rotation(t) * a_b_; }
│ │ │ │ +
68
│ │ │ │ +
71 Vector3 deltaPij() const override { return preintegrated_.segment<3>(3); }
│ │ │ │ +
72 Vector3 deltaVij() const override { return preintegrated_.tail<3>(); }
│ │ │ │ +
73 Rot3 deltaRij() const override { return Rot3::Expmap(theta()); }
│ │ │ │ +
74 NavState deltaXij() const override { return NavState().retract(preintegrated_); }
│ │ │ │
75
│ │ │ │ -
76 private:
│ │ │ │ -
77 const Vector6 twist_;
│ │ │ │ -
78 const Vector3 a_b_; // constant centripetal acceleration in body = w_b * v_b
│ │ │ │ -
79 const Pose3 nTb0_;
│ │ │ │ -
80};
│ │ │ │ -
│ │ │ │ -
81
│ │ │ │ -
│ │ │ │ -
83class AcceleratingScenario : public Scenario {
│ │ │ │ -
84 public:
│ │ │ │ -
│ │ │ │ -
87 AcceleratingScenario(const Rot3& nRb, const Point3& p0, const Vector3& v0,
│ │ │ │ -
88 const Vector3& a_n,
│ │ │ │ -
89 const Vector3& omega_b = Vector3::Zero())
│ │ │ │ -
90 : nRb_(nRb), p0_(p0), v0_(v0), a_n_(a_n), omega_b_(omega_b) {}
│ │ │ │ -
│ │ │ │ -
91
│ │ │ │ -
│ │ │ │ -
92 Pose3 pose(double t) const override {
│ │ │ │ -
93 return Pose3(nRb_.expmap(omega_b_ * t), p0_ + v0_ * t + a_n_ * t * t / 2.0);
│ │ │ │ -
94 }
│ │ │ │ -
│ │ │ │ -
95 Vector3 omega_b(double t) const override { return omega_b_; }
│ │ │ │ -
96 Vector3 velocity_n(double t) const override { return v0_ + a_n_ * t; }
│ │ │ │ -
97 Vector3 acceleration_n(double t) const override { return a_n_; }
│ │ │ │ -
98
│ │ │ │ -
99 private:
│ │ │ │ -
100 const Rot3 nRb_;
│ │ │ │ -
101 const Vector3 p0_, v0_, a_n_, omega_b_;
│ │ │ │ -
102};
│ │ │ │ -
│ │ │ │ -
103
│ │ │ │ -
104} // namespace gtsam
│ │ │ │ - │ │ │ │ -
NavState.h
Navigation state composing of attitude, position, and velocity.
│ │ │ │ +
76 const Vector9& preintegrated() const { return preintegrated_; }
│ │ │ │ +
77 Vector3 theta() const { return preintegrated_.head<3>(); }
│ │ │ │ +
78 const Matrix93& preintegrated_H_biasAcc() const { return preintegrated_H_biasAcc_; }
│ │ │ │ +
79 const Matrix93& preintegrated_H_biasOmega() const { return preintegrated_H_biasOmega_; }
│ │ │ │ +
80
│ │ │ │ +
83 bool equals(const TangentPreintegration& other, double tol) const;
│ │ │ │ +
85
│ │ │ │ +
88
│ │ │ │ +
89 // Update integrated vector on tangent manifold preintegrated with acceleration
│ │ │ │ +
90 // Static, functional version.
│ │ │ │ +
91 static Vector9 UpdatePreintegrated(const Vector3& a_body,
│ │ │ │ +
92 const Vector3& w_body, const double dt,
│ │ │ │ +
93 const Vector9& preintegrated,
│ │ │ │ +
94 OptionalJacobian<9, 9> A = boost::none,
│ │ │ │ +
95 OptionalJacobian<9, 3> B = boost::none,
│ │ │ │ +
96 OptionalJacobian<9, 3> C = boost::none);
│ │ │ │ +
97
│ │ │ │ +
102 void update(const Vector3& measuredAcc, const Vector3& measuredOmega,
│ │ │ │ +
103 const double dt, Matrix9* A, Matrix93* B, Matrix93* C) override;
│ │ │ │ +
104
│ │ │ │ +
108 Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
│ │ │ │ +
109 OptionalJacobian<9, 6> H = boost::none) const override;
│ │ │ │ +
110
│ │ │ │ +
111 // Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with derivatives
│ │ │ │ +
112 static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12,
│ │ │ │ +
113 double deltaT12,
│ │ │ │ +
114 OptionalJacobian<9, 9> H1 = boost::none,
│ │ │ │ +
115 OptionalJacobian<9, 9> H2 = boost::none);
│ │ │ │ +
116
│ │ │ │ +
119 void mergeWith(const TangentPreintegration& pim, Matrix9* H1, Matrix9* H2);
│ │ │ │ +
121
│ │ │ │ +
│ │ │ │ +
123 virtual boost::shared_ptr<TangentPreintegration> clone() const {
│ │ │ │ +
124 return boost::shared_ptr<TangentPreintegration>();
│ │ │ │ +
125 }
│ │ │ │ +
│ │ │ │ +
126
│ │ │ │ +
128
│ │ │ │ +
129private:
│ │ │ │ +
131 friend class boost::serialization::access;
│ │ │ │ +
132 template<class ARCHIVE>
│ │ │ │ +
133 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
134 namespace bs = ::boost::serialization;
│ │ │ │ +
135 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
│ │ │ │ +
136 ar & BOOST_SERIALIZATION_NVP(preintegrated_);
│ │ │ │ +
137 ar & BOOST_SERIALIZATION_NVP(preintegrated_H_biasAcc_);
│ │ │ │ +
138 ar & BOOST_SERIALIZATION_NVP(preintegrated_H_biasOmega_);
│ │ │ │ +
139 }
│ │ │ │ +
140
│ │ │ │ +
141public:
│ │ │ │ +
142 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
143};
│ │ │ │ +
│ │ │ │ +
144
│ │ │ │ +
145}
│ │ │ │ +
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::cross
Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
cross product
Definition Point3.cpp:64
│ │ │ │ -
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ -
gtsam::LieGroup::expmap
Class expmap(const TangentVector &v) const
expmap as required by manifold concept Applies exponential map to v and composes with *this
Definition Lie.h:78
│ │ │ │ -
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ -
gtsam::Pose3::Expmap
static Pose3 Expmap(const Vector6 &xi, OptionalJacobian< 6, 6 > Hxi=boost::none)
Exponential map at identity - create a rotation from canonical coordinates .
Definition Pose3.cpp:169
│ │ │ │ -
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get rotation
Definition Pose3.cpp:315
│ │ │ │ -
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
│ │ │ │ -
gtsam::Rot3::matrix
Matrix3 matrix() const
return 3*3 rotation matrix
Definition Rot3M.cpp:219
│ │ │ │ -
gtsam::NavState
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition NavState.h:34
│ │ │ │ -
gtsam::Scenario
Simple trajectory simulator.
Definition Scenario.h:25
│ │ │ │ -
gtsam::Scenario::pose
virtual Pose3 pose(double t) const =0
pose at time t
│ │ │ │ -
gtsam::Scenario::acceleration_n
virtual Vector3 acceleration_n(double t) const =0
acceleration in nav frame
│ │ │ │ -
gtsam::Scenario::velocity_n
virtual Vector3 velocity_n(double t) const =0
velocity at time t, in nav frame
│ │ │ │ -
gtsam::Scenario::~Scenario
virtual ~Scenario()
virtual destructor
Definition Scenario.h:28
│ │ │ │ -
gtsam::Scenario::omega_b
virtual Vector3 omega_b(double t) const =0
angular velocity in body frame
│ │ │ │ -
gtsam::ConstantTwistScenario
Scenario with constant twist 3D trajectory.
Definition Scenario.h:60
│ │ │ │ -
gtsam::ConstantTwistScenario::ConstantTwistScenario
ConstantTwistScenario(const Vector3 &w, const Vector3 &v, const Pose3 &nTb0=Pose3())
Construct scenario with constant twist [w,v].
Definition Scenario.h:63
│ │ │ │ -
gtsam::ConstantTwistScenario::acceleration_n
Vector3 acceleration_n(double t) const override
acceleration in nav frame
Definition Scenario.h:74
│ │ │ │ -
gtsam::ConstantTwistScenario::omega_b
Vector3 omega_b(double t) const override
angular velocity in body frame
Definition Scenario.h:70
│ │ │ │ -
gtsam::ConstantTwistScenario::pose
Pose3 pose(double t) const override
pose at time t
Definition Scenario.h:67
│ │ │ │ -
gtsam::ConstantTwistScenario::velocity_n
Vector3 velocity_n(double t) const override
velocity at time t, in nav frame
Definition Scenario.h:71
│ │ │ │ -
gtsam::AcceleratingScenario
Accelerating from an arbitrary initial state, with optional rotation.
Definition Scenario.h:83
│ │ │ │ -
gtsam::AcceleratingScenario::pose
Pose3 pose(double t) const override
pose at time t
Definition Scenario.h:92
│ │ │ │ -
gtsam::AcceleratingScenario::omega_b
Vector3 omega_b(double t) const override
angular velocity in body frame
Definition Scenario.h:95
│ │ │ │ -
gtsam::AcceleratingScenario::AcceleratingScenario
AcceleratingScenario(const Rot3 &nRb, const Point3 &p0, const Vector3 &v0, const Vector3 &a_n, const Vector3 &omega_b=Vector3::Zero())
Construct scenario with constant acceleration in navigation frame and optional angular velocity in bo...
Definition Scenario.h:87
│ │ │ │ -
gtsam::AcceleratingScenario::acceleration_n
Vector3 acceleration_n(double t) const override
acceleration in nav frame
Definition Scenario.h:97
│ │ │ │ -
gtsam::AcceleratingScenario::velocity_n
Vector3 velocity_n(double t) const override
velocity at time t, in nav frame
Definition Scenario.h:96
│ │ │ │ +
gtsam::imuBias::ConstantBias
Definition ImuBias.h:30
│ │ │ │ +
gtsam::PreintegrationBase
PreintegrationBase is the base class for PreintegratedMeasurements (in ImuFactor) and CombinedPreinte...
Definition PreintegrationBase.h:41
│ │ │ │ +
gtsam::TangentPreintegration
Integrate on the 9D tangent space of the NavState manifold.
Definition TangentPreintegration.h:28
│ │ │ │ +
gtsam::TangentPreintegration::preintegrated_H_biasOmega_
Matrix93 preintegrated_H_biasOmega_
Jacobian of preintegrated_ w.r.t. angular rate bias.
Definition TangentPreintegration.h:37
│ │ │ │ +
gtsam::TangentPreintegration::~TangentPreintegration
~TangentPreintegration() override
Virtual destructor.
Definition TangentPreintegration.h:57
│ │ │ │ +
gtsam::TangentPreintegration::preintegrated_
Vector9 preintegrated_
Preintegrated navigation state, as a 9D vector on tangent space at frame i Order is: theta,...
Definition TangentPreintegration.h:35
│ │ │ │ +
gtsam::TangentPreintegration::TangentPreintegration
TangentPreintegration()
Default constructor for serialization.
Definition TangentPreintegration.h:40
│ │ │ │ +
gtsam::TangentPreintegration::preintegrated_H_biasAcc_
Matrix93 preintegrated_H_biasAcc_
Jacobian of preintegrated_ w.r.t. acceleration bias.
Definition TangentPreintegration.h:36
│ │ │ │ +
gtsam::TangentPreintegration::clone
virtual boost::shared_ptr< TangentPreintegration > clone() const
Dummy clone for MATLAB.
Definition TangentPreintegration.h:123
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Scenario.h │ │ │ │ │ +TangentPreintegration.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ @@ -15,199 +15,143 @@ │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 18#pragma once │ │ │ │ │ -19#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_N_o_i_s_e_M_o_d_e_l_._h> │ │ │ │ │ -20#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_N_a_v_S_t_a_t_e_._h> │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_._h> │ │ │ │ │ 21 │ │ │ │ │ 22namespace _g_t_s_a_m { │ │ │ │ │ 23 │ │ │ │ │ -_2_5class _S_c_e_n_a_r_i_o { │ │ │ │ │ -26 public: │ │ │ │ │ -_2_8 virtual _~_S_c_e_n_a_r_i_o() {} │ │ │ │ │ -29 │ │ │ │ │ -30 // Quantities a Scenario needs to specify: │ │ │ │ │ -31 │ │ │ │ │ -_3_2 virtual _P_o_s_e_3 _p_o_s_e(double t) const = 0; │ │ │ │ │ -_3_3 virtual Vector3 _o_m_e_g_a___b(double t) const = 0; │ │ │ │ │ -_3_4 virtual Vector3 _v_e_l_o_c_i_t_y___n(double t) const = 0; │ │ │ │ │ -_3_5 virtual Vector3 _a_c_c_e_l_e_r_a_t_i_o_n___n(double t) const = 0; │ │ │ │ │ -36 │ │ │ │ │ -37 // Derived quantities: │ │ │ │ │ +_2_8class GTSAM_EXPORT _T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n : public _P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e { │ │ │ │ │ +29 protected: │ │ │ │ │ +30 │ │ │ │ │ +_3_5 Vector9 _p_r_e_i_n_t_e_g_r_a_t_e_d__; │ │ │ │ │ +_3_6 Matrix93 _p_r_e_i_n_t_e_g_r_a_t_e_d___H___b_i_a_s_A_c_c__; │ │ │ │ │ +_3_7 Matrix93 _p_r_e_i_n_t_e_g_r_a_t_e_d___H___b_i_a_s_O_m_e_g_a__; │ │ │ │ │ 38 │ │ │ │ │ -39 _R_o_t_3 rotation(double t) const { return _p_o_s_e(t)._r_o_t_a_t_i_o_n(); } │ │ │ │ │ -40 _N_a_v_S_t_a_t_e navState(double t) const { return _N_a_v_S_t_a_t_e(_p_o_s_e(t), _v_e_l_o_c_i_t_y___n(t)); │ │ │ │ │ -} │ │ │ │ │ -41 │ │ │ │ │ -42 Vector3 velocity_b(double t) const { │ │ │ │ │ -43 const Rot3 nRb = rotation(t); │ │ │ │ │ -44 return nRb.transpose() * _v_e_l_o_c_i_t_y___n(t); │ │ │ │ │ -45 } │ │ │ │ │ -46 │ │ │ │ │ -47 Vector3 acceleration_b(double t) const { │ │ │ │ │ -48 const Rot3 nRb = rotation(t); │ │ │ │ │ -49 return nRb.transpose() * _a_c_c_e_l_e_r_a_t_i_o_n___n(t); │ │ │ │ │ -50 } │ │ │ │ │ -51}; │ │ │ │ │ -52 │ │ │ │ │ -_6_0class _C_o_n_s_t_a_n_t_T_w_i_s_t_S_c_e_n_a_r_i_o : public _S_c_e_n_a_r_i_o { │ │ │ │ │ -61 public: │ │ │ │ │ -_6_3 _C_o_n_s_t_a_n_t_T_w_i_s_t_S_c_e_n_a_r_i_o(const Vector3& w, const Vector3& v, │ │ │ │ │ -64 const _P_o_s_e_3& nTb0 = _P_o_s_e_3()) │ │ │ │ │ -65 : twist_((Vector6() << w, v).finished()), a_b_(w._c_r_o_s_s(v)), nTb0_(nTb0) {} │ │ │ │ │ +_4_0 _T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n() { │ │ │ │ │ +41 resetIntegration(); │ │ │ │ │ +42 } │ │ │ │ │ +43 │ │ │ │ │ +44public: │ │ │ │ │ +47 │ │ │ │ │ +53 _T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n(const boost::shared_ptr& p, │ │ │ │ │ +54 const _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s& biasHat = _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s()); │ │ │ │ │ +55 │ │ │ │ │ +_5_7 _~_T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n() override { │ │ │ │ │ +58 } │ │ │ │ │ +59 │ │ │ │ │ +61 │ │ │ │ │ +65 void resetIntegration() override; │ │ │ │ │ 66 │ │ │ │ │ -_6_7 _P_o_s_e_3 _p_o_s_e(double t) const override { │ │ │ │ │ -68 return nTb0_ * _P_o_s_e_3_:_:_E_x_p_m_a_p(twist_ * t); │ │ │ │ │ -69 } │ │ │ │ │ -_7_0 Vector3 _o_m_e_g_a___b(double t) const override { return twist_.head<3>(); } │ │ │ │ │ -_7_1 Vector3 _v_e_l_o_c_i_t_y___n(double t) const override { │ │ │ │ │ -72 return rotation(t)._m_a_t_r_i_x() * twist_.tail<3>(); │ │ │ │ │ -73 } │ │ │ │ │ -_7_4 Vector3 _a_c_c_e_l_e_r_a_t_i_o_n___n(double t) const override { return rotation(t) * a_b_; │ │ │ │ │ -} │ │ │ │ │ +68 │ │ │ │ │ +71 Vector3 deltaPij() const override { return preintegrated_.segment<3>(3); } │ │ │ │ │ +72 Vector3 deltaVij() const override { return preintegrated_.tail<3>(); } │ │ │ │ │ +73 Rot3 deltaRij() const override { return Rot3::Expmap(theta()); } │ │ │ │ │ +74 NavState deltaXij() const override { return NavState().retract │ │ │ │ │ +(preintegrated_); } │ │ │ │ │ 75 │ │ │ │ │ -76 private: │ │ │ │ │ -77 const Vector6 twist_; │ │ │ │ │ -78 const Vector3 a_b_; // constant centripetal acceleration in body = w_b * v_b │ │ │ │ │ -79 const _P_o_s_e_3 nTb0_; │ │ │ │ │ -80}; │ │ │ │ │ -81 │ │ │ │ │ -_8_3class _A_c_c_e_l_e_r_a_t_i_n_g_S_c_e_n_a_r_i_o : public _S_c_e_n_a_r_i_o { │ │ │ │ │ -84 public: │ │ │ │ │ -_8_7 _A_c_c_e_l_e_r_a_t_i_n_g_S_c_e_n_a_r_i_o(const _R_o_t_3& nRb, const _P_o_i_n_t_3& p0, const Vector3& v0, │ │ │ │ │ -88 const Vector3& a_n, │ │ │ │ │ -89 const Vector3& _o_m_e_g_a___b = Vector3::Zero()) │ │ │ │ │ -90 : nRb_(nRb), p0_(p0), v0_(v0), a_n_(a_n), omega_b_(_o_m_e_g_a___b) {} │ │ │ │ │ -91 │ │ │ │ │ -_9_2 _P_o_s_e_3 _p_o_s_e(double t) const override { │ │ │ │ │ -93 return _P_o_s_e_3(nRb_._e_x_p_m_a_p(omega_b_ * t), p0_ + v0_ * t + a_n_ * t * t / 2.0); │ │ │ │ │ -94 } │ │ │ │ │ -_9_5 Vector3 _o_m_e_g_a___b(double t) const override { return omega_b_; } │ │ │ │ │ -_9_6 Vector3 _v_e_l_o_c_i_t_y___n(double t) const override { return v0_ + a_n_ * t; } │ │ │ │ │ -_9_7 Vector3 _a_c_c_e_l_e_r_a_t_i_o_n___n(double t) const override { return a_n_; } │ │ │ │ │ -98 │ │ │ │ │ -99 private: │ │ │ │ │ -100 const _R_o_t_3 nRb_; │ │ │ │ │ -101 const Vector3 p0_, v0_, a_n_, omega_b_; │ │ │ │ │ -102}; │ │ │ │ │ -103 │ │ │ │ │ -104} // namespace gtsam │ │ │ │ │ -_N_o_i_s_e_M_o_d_e_l_._h │ │ │ │ │ -_N_a_v_S_t_a_t_e_._h │ │ │ │ │ -Navigation state composing of attitude, position, and velocity. │ │ │ │ │ +76 const Vector9& preintegrated() const { return preintegrated_; } │ │ │ │ │ +77 Vector3 theta() const { return preintegrated_.head<3>(); } │ │ │ │ │ +78 const Matrix93& preintegrated_H_biasAcc() const { return │ │ │ │ │ +preintegrated_H_biasAcc_; } │ │ │ │ │ +79 const Matrix93& preintegrated_H_biasOmega() const { return │ │ │ │ │ +preintegrated_H_biasOmega_; } │ │ │ │ │ +80 │ │ │ │ │ +83 bool equals(const TangentPreintegration& other, double tol) const; │ │ │ │ │ +85 │ │ │ │ │ +88 │ │ │ │ │ +89 // Update integrated vector on tangent manifold preintegrated with │ │ │ │ │ +acceleration │ │ │ │ │ +90 // Static, functional version. │ │ │ │ │ +91 static Vector9 UpdatePreintegrated(const Vector3& a_body, │ │ │ │ │ +92 const Vector3& w_body, const double dt, │ │ │ │ │ +93 const Vector9& preintegrated, │ │ │ │ │ +94 OptionalJacobian<9, 9> A = boost::none, │ │ │ │ │ +95 OptionalJacobian<9, 3> B = boost::none, │ │ │ │ │ +96 OptionalJacobian<9, 3> C = boost::none); │ │ │ │ │ +97 │ │ │ │ │ +102 void update(const Vector3& measuredAcc, const Vector3& measuredOmega, │ │ │ │ │ +103 const double dt, Matrix9* A, Matrix93* B, Matrix93* C) override; │ │ │ │ │ +104 │ │ │ │ │ +108 Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, │ │ │ │ │ +109 OptionalJacobian<9, 6> H = boost::none) const override; │ │ │ │ │ +110 │ │ │ │ │ +111 // Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with │ │ │ │ │ +derivatives │ │ │ │ │ +112 static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12, │ │ │ │ │ +113 double deltaT12, │ │ │ │ │ +114 OptionalJacobian<9, 9> H1 = boost::none, │ │ │ │ │ +115 OptionalJacobian<9, 9> H2 = boost::none); │ │ │ │ │ +116 │ │ │ │ │ +119 void mergeWith(const TangentPreintegration& pim, Matrix9* H1, Matrix9* H2); │ │ │ │ │ +121 │ │ │ │ │ +_1_2_3 virtual boost::shared_ptr _c_l_o_n_e() const { │ │ │ │ │ +124 return boost::shared_ptr(); │ │ │ │ │ +125 } │ │ │ │ │ +126 │ │ │ │ │ +128 │ │ │ │ │ +129private: │ │ │ │ │ +_1_3_1 friend class boost::serialization::access; │ │ │ │ │ +132 template │ │ │ │ │ +133 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +134 namespace bs = ::boost::serialization; │ │ │ │ │ +135 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e); │ │ │ │ │ +136 ar & BOOST_SERIALIZATION_NVP(preintegrated_); │ │ │ │ │ +137 ar & BOOST_SERIALIZATION_NVP(preintegrated_H_biasAcc_); │ │ │ │ │ +138 ar & BOOST_SERIALIZATION_NVP(preintegrated_H_biasOmega_); │ │ │ │ │ +139 } │ │ │ │ │ +140 │ │ │ │ │ +141public: │ │ │ │ │ +142 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +143}; │ │ │ │ │ +144 │ │ │ │ │ +145} │ │ │ │ │ +_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ +This marks a GTSAM object to require alignment. │ │ │ │ │ +DDeeffiinniittiioonn types.h:308 │ │ │ │ │ +_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_c_r_o_s_s │ │ │ │ │ -Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, │ │ │ │ │ -OptionalJacobian< 3, 3 > H2) │ │ │ │ │ -cross product │ │ │ │ │ -DDeeffiinniittiioonn Point3.cpp:64 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ -Vector3 Point3 │ │ │ │ │ -As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point3 to Vector3... │ │ │ │ │ -DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_e_x_p_m_a_p │ │ │ │ │ -Class expmap(const TangentVector &v) const │ │ │ │ │ -expmap as required by manifold concept Applies exponential map to v and │ │ │ │ │ -composes with *this │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:78 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ -A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_E_x_p_m_a_p │ │ │ │ │ -static Pose3 Expmap(const Vector6 &xi, OptionalJacobian< 6, 6 > Hxi=boost:: │ │ │ │ │ -none) │ │ │ │ │ -Exponential map at identity - create a rotation from canonical coordinates . │ │ │ │ │ -DDeeffiinniittiioonn Pose3.cpp:169 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_r_o_t_a_t_i_o_n │ │ │ │ │ -const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const │ │ │ │ │ -get rotation │ │ │ │ │ -DDeeffiinniittiioonn Pose3.cpp:315 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ -Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor │ │ │ │ │ -symbol GTSAM_USE_QUATERNIO... │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_m_a_t_r_i_x │ │ │ │ │ -Matrix3 matrix() const │ │ │ │ │ -return 3*3 rotation matrix │ │ │ │ │ -DDeeffiinniittiioonn Rot3M.cpp:219 │ │ │ │ │ -_g_t_s_a_m_:_:_N_a_v_S_t_a_t_e │ │ │ │ │ -Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does │ │ │ │ │ -not make sense to make... │ │ │ │ │ -DDeeffiinniittiioonn NavState.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_S_c_e_n_a_r_i_o │ │ │ │ │ -Simple trajectory simulator. │ │ │ │ │ -DDeeffiinniittiioonn Scenario.h:25 │ │ │ │ │ -_g_t_s_a_m_:_:_S_c_e_n_a_r_i_o_:_:_p_o_s_e │ │ │ │ │ -virtual Pose3 pose(double t) const =0 │ │ │ │ │ -pose at time t │ │ │ │ │ -_g_t_s_a_m_:_:_S_c_e_n_a_r_i_o_:_:_a_c_c_e_l_e_r_a_t_i_o_n___n │ │ │ │ │ -virtual Vector3 acceleration_n(double t) const =0 │ │ │ │ │ -acceleration in nav frame │ │ │ │ │ -_g_t_s_a_m_:_:_S_c_e_n_a_r_i_o_:_:_v_e_l_o_c_i_t_y___n │ │ │ │ │ -virtual Vector3 velocity_n(double t) const =0 │ │ │ │ │ -velocity at time t, in nav frame │ │ │ │ │ -_g_t_s_a_m_:_:_S_c_e_n_a_r_i_o_:_:_~_S_c_e_n_a_r_i_o │ │ │ │ │ -virtual ~Scenario() │ │ │ │ │ -virtual destructor │ │ │ │ │ -DDeeffiinniittiioonn Scenario.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_S_c_e_n_a_r_i_o_:_:_o_m_e_g_a___b │ │ │ │ │ -virtual Vector3 omega_b(double t) const =0 │ │ │ │ │ -angular velocity in body frame │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_s_t_a_n_t_T_w_i_s_t_S_c_e_n_a_r_i_o │ │ │ │ │ -Scenario with constant twist 3D trajectory. │ │ │ │ │ -DDeeffiinniittiioonn Scenario.h:60 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_s_t_a_n_t_T_w_i_s_t_S_c_e_n_a_r_i_o_:_:_C_o_n_s_t_a_n_t_T_w_i_s_t_S_c_e_n_a_r_i_o │ │ │ │ │ -ConstantTwistScenario(const Vector3 &w, const Vector3 &v, const Pose3 │ │ │ │ │ -&nTb0=Pose3()) │ │ │ │ │ -Construct scenario with constant twist [w,v]. │ │ │ │ │ -DDeeffiinniittiioonn Scenario.h:63 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_s_t_a_n_t_T_w_i_s_t_S_c_e_n_a_r_i_o_:_:_a_c_c_e_l_e_r_a_t_i_o_n___n │ │ │ │ │ -Vector3 acceleration_n(double t) const override │ │ │ │ │ -acceleration in nav frame │ │ │ │ │ -DDeeffiinniittiioonn Scenario.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_s_t_a_n_t_T_w_i_s_t_S_c_e_n_a_r_i_o_:_:_o_m_e_g_a___b │ │ │ │ │ -Vector3 omega_b(double t) const override │ │ │ │ │ -angular velocity in body frame │ │ │ │ │ -DDeeffiinniittiioonn Scenario.h:70 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_s_t_a_n_t_T_w_i_s_t_S_c_e_n_a_r_i_o_:_:_p_o_s_e │ │ │ │ │ -Pose3 pose(double t) const override │ │ │ │ │ -pose at time t │ │ │ │ │ -DDeeffiinniittiioonn Scenario.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_s_t_a_n_t_T_w_i_s_t_S_c_e_n_a_r_i_o_:_:_v_e_l_o_c_i_t_y___n │ │ │ │ │ -Vector3 velocity_n(double t) const override │ │ │ │ │ -velocity at time t, in nav frame │ │ │ │ │ -DDeeffiinniittiioonn Scenario.h:71 │ │ │ │ │ -_g_t_s_a_m_:_:_A_c_c_e_l_e_r_a_t_i_n_g_S_c_e_n_a_r_i_o │ │ │ │ │ -Accelerating from an arbitrary initial state, with optional rotation. │ │ │ │ │ -DDeeffiinniittiioonn Scenario.h:83 │ │ │ │ │ -_g_t_s_a_m_:_:_A_c_c_e_l_e_r_a_t_i_n_g_S_c_e_n_a_r_i_o_:_:_p_o_s_e │ │ │ │ │ -Pose3 pose(double t) const override │ │ │ │ │ -pose at time t │ │ │ │ │ -DDeeffiinniittiioonn Scenario.h:92 │ │ │ │ │ -_g_t_s_a_m_:_:_A_c_c_e_l_e_r_a_t_i_n_g_S_c_e_n_a_r_i_o_:_:_o_m_e_g_a___b │ │ │ │ │ -Vector3 omega_b(double t) const override │ │ │ │ │ -angular velocity in body frame │ │ │ │ │ -DDeeffiinniittiioonn Scenario.h:95 │ │ │ │ │ -_g_t_s_a_m_:_:_A_c_c_e_l_e_r_a_t_i_n_g_S_c_e_n_a_r_i_o_:_:_A_c_c_e_l_e_r_a_t_i_n_g_S_c_e_n_a_r_i_o │ │ │ │ │ -AcceleratingScenario(const Rot3 &nRb, const Point3 &p0, const Vector3 &v0, │ │ │ │ │ -const Vector3 &a_n, const Vector3 &omega_b=Vector3::Zero()) │ │ │ │ │ -Construct scenario with constant acceleration in navigation frame and optional │ │ │ │ │ -angular velocity in bo... │ │ │ │ │ -DDeeffiinniittiioonn Scenario.h:87 │ │ │ │ │ -_g_t_s_a_m_:_:_A_c_c_e_l_e_r_a_t_i_n_g_S_c_e_n_a_r_i_o_:_:_a_c_c_e_l_e_r_a_t_i_o_n___n │ │ │ │ │ -Vector3 acceleration_n(double t) const override │ │ │ │ │ -acceleration in nav frame │ │ │ │ │ -DDeeffiinniittiioonn Scenario.h:97 │ │ │ │ │ -_g_t_s_a_m_:_:_A_c_c_e_l_e_r_a_t_i_n_g_S_c_e_n_a_r_i_o_:_:_v_e_l_o_c_i_t_y___n │ │ │ │ │ -Vector3 velocity_n(double t) const override │ │ │ │ │ -velocity at time t, in nav frame │ │ │ │ │ -DDeeffiinniittiioonn Scenario.h:96 │ │ │ │ │ +_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s │ │ │ │ │ +DDeeffiinniittiioonn ImuBias.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e │ │ │ │ │ +PreintegrationBase is the base class for PreintegratedMeasurements (in │ │ │ │ │ +ImuFactor) and CombinedPreinte... │ │ │ │ │ +DDeeffiinniittiioonn PreintegrationBase.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n │ │ │ │ │ +Integrate on the 9D tangent space of the NavState manifold. │ │ │ │ │ +DDeeffiinniittiioonn TangentPreintegration.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n_:_:_p_r_e_i_n_t_e_g_r_a_t_e_d___H___b_i_a_s_O_m_e_g_a__ │ │ │ │ │ +Matrix93 preintegrated_H_biasOmega_ │ │ │ │ │ +Jacobian of preintegrated_ w.r.t. angular rate bias. │ │ │ │ │ +DDeeffiinniittiioonn TangentPreintegration.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n_:_:_~_T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n │ │ │ │ │ +~TangentPreintegration() override │ │ │ │ │ +Virtual destructor. │ │ │ │ │ +DDeeffiinniittiioonn TangentPreintegration.h:57 │ │ │ │ │ +_g_t_s_a_m_:_:_T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n_:_:_p_r_e_i_n_t_e_g_r_a_t_e_d__ │ │ │ │ │ +Vector9 preintegrated_ │ │ │ │ │ +Preintegrated navigation state, as a 9D vector on tangent space at frame i │ │ │ │ │ +Order is: theta,... │ │ │ │ │ +DDeeffiinniittiioonn TangentPreintegration.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n_:_:_T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n │ │ │ │ │ +TangentPreintegration() │ │ │ │ │ +Default constructor for serialization. │ │ │ │ │ +DDeeffiinniittiioonn TangentPreintegration.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n_:_:_p_r_e_i_n_t_e_g_r_a_t_e_d___H___b_i_a_s_A_c_c__ │ │ │ │ │ +Matrix93 preintegrated_H_biasAcc_ │ │ │ │ │ +Jacobian of preintegrated_ w.r.t. acceleration bias. │ │ │ │ │ +DDeeffiinniittiioonn TangentPreintegration.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n_:_:_c_l_o_n_e │ │ │ │ │ +virtual boost::shared_ptr< TangentPreintegration > clone() const │ │ │ │ │ +Dummy clone for MATLAB. │ │ │ │ │ +DDeeffiinniittiioonn TangentPreintegration.h:123 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _S_c_e_n_a_r_i_o_._h │ │ │ │ │ + * _T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00935.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ManifoldPreintegration.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ScenarioRunner.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,41 +94,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
ManifoldPreintegration.cpp File Reference
│ │ │ │ +
ScenarioRunner.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ + │ │ │ │ +

Simple class to test navigation scenarios. │ │ │ │ +More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::ScenarioRunner
 
class  gtsam::CombinedScenarioRunner
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Luca Carlone
│ │ │ │ -
│ │ │ │ -Stephen Williams
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ -
│ │ │ │ -Vadim Indelman
│ │ │ │ -
│ │ │ │ -David Jensen
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ +

Simple class to test navigation scenarios.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,26 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -ManifoldPreintegration.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +ScenarioRunner.h File Reference │ │ │ │ │ +Simple class to test navigation scenarios. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_S_c_e_n_a_r_i_o_R_u_n_n_e_r │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_C_o_m_b_i_n_e_d_S_c_e_n_a_r_i_o_R_u_n_n_e_r │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Simple class to test navigation scenarios. │ │ │ │ │ Author │ │ │ │ │ - Luca Carlone │ │ │ │ │ - Stephen Williams │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Vadim Indelman │ │ │ │ │ - David Jensen │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_._c_p_p │ │ │ │ │ + * _S_c_e_n_a_r_i_o_R_u_n_n_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00938.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/CombinedImuFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ImuFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,70 +95,61 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs
│ │ │ │ -
CombinedImuFactor.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
ImuFactor.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

struct  gtsam::PreintegrationCombinedParams
 Parameters for pre-integration using PreintegratedCombinedMeasurements: Usage: Create just a single Params and pass a shared pointer to the constructor. More...
class  gtsam::PreintegratedImuMeasurements
 PreintegratedImuMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) and the corresponding covariance matrix. More...
 
class  gtsam::PreintegratedCombinedMeasurements
 PreintegratedCombinedMeasurements integrates the IMU measurements (rotation rates and accelerations) and the corresponding covariance matrix. More...
class  gtsam::ImuFactor
 ImuFactor is a 5-ways factor involving previous state (pose and velocity of the vehicle at previous time step), current state (pose and velocity at current time step), and the bias estimate. More...
 
class  gtsam::CombinedImuFactor
 CombinedImuFactor is a 6-ways factor involving previous state (pose and velocity of the vehicle, as well as bias at previous time step), and current state (pose, velocity, bias at current time step). More...
class  gtsam::ImuFactor2
 ImuFactor2 is a ternary factor that uses NavStates rather than Pose/Velocity. More...
 
struct  gtsam::traits< PreintegrationCombinedParams >
struct  gtsam::traits< PreintegratedImuMeasurements >
 
struct  gtsam::traits< PreintegratedCombinedMeasurements >
struct  gtsam::traits< ImuFactor >
 
struct  gtsam::traits< CombinedImuFactor >
struct  gtsam::traits< ImuFactor2 >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Typedefs

│ │ │ │ -typedef ManifoldPreintegration gtsam::PreintegrationType
 
│ │ │ │

Detailed Description

│ │ │ │
Author
Luca Carlone
│ │ │ │
│ │ │ │ Stephen Williams
│ │ │ │
│ │ │ │ Richard Roberts
│ │ │ │
│ │ │ │ Vadim Indelman
│ │ │ │
│ │ │ │ David Jensen
│ │ │ │
│ │ │ │ -Frank Dellaert
│ │ │ │ -
│ │ │ │ -Varun Agrawal
│ │ │ │ +Frank Dellaert │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,50 +1,45 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ -CombinedImuFactor.h File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +ImuFactor.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s │ │ │ │ │ - Parameters for pre-integration using │ │ │ │ │ -  _P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s: Usage: Create just a single Params │ │ │ │ │ - and pass a shared pointer to the constructor. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ - _P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s integrates the IMU measurements │ │ │ │ │ -  (rotation rates and accelerations) and the corresponding covariance │ │ │ │ │ - matrix. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r │ │ │ │ │ - _C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r is a 6-ways factor involving previous state (pose │ │ │ │ │ -  and velocity of the vehicle, as well as bias at previous time step), │ │ │ │ │ - and current state (pose, velocity, bias at current time step). _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ + _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s accumulates (integrates) the IMU │ │ │ │ │ +  measurements (rotation rates and accelerations) and the corresponding │ │ │ │ │ + covariance matrix. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s_ _> │ │ │ │ │ + class   _g_t_s_a_m_:_:_I_m_u_F_a_c_t_o_r │ │ │ │ │ + _I_m_u_F_a_c_t_o_r is a 5-ways factor involving previous state (pose and │ │ │ │ │ +  velocity of the vehicle at previous time step), current state (pose │ │ │ │ │ + and velocity at current time step), and the bias estimate. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s_ _> │ │ │ │ │ + class   _g_t_s_a_m_:_:_I_m_u_F_a_c_t_o_r_2 │ │ │ │ │ +  _I_m_u_F_a_c_t_o_r_2 is a ternary factor that uses NavStates rather than Pose/ │ │ │ │ │ + Velocity. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _I_m_u_F_a_c_t_o_r_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _I_m_u_F_a_c_t_o_r_2_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -typedef _M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n  ggttssaamm::::PPrreeiinntteeggrraattiioonnTTyyppee │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Author │ │ │ │ │ Luca Carlone │ │ │ │ │ Stephen Williams │ │ │ │ │ Richard Roberts │ │ │ │ │ Vadim Indelman │ │ │ │ │ David Jensen │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r_._h │ │ │ │ │ + * _I_m_u_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00938.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,6 +1,5 @@ │ │ │ │ │ var a00938 = [ │ │ │ │ │ - ["gtsam::PreintegrationCombinedParams", "a04104.html", "a04104"], │ │ │ │ │ - ["gtsam::traits< PreintegrationCombinedParams >", "a04116.html", null], │ │ │ │ │ - ["gtsam::traits< PreintegratedCombinedMeasurements >", "a04120.html", null], │ │ │ │ │ - ["gtsam::traits< CombinedImuFactor >", "a04124.html", null] │ │ │ │ │ + ["gtsam::traits< PreintegratedImuMeasurements >", "a04160.html", null], │ │ │ │ │ + ["gtsam::traits< ImuFactor >", "a04164.html", null], │ │ │ │ │ + ["gtsam::traits< ImuFactor2 >", "a04168.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00938_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/CombinedImuFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ImuFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,319 +98,298 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
CombinedImuFactor.h
│ │ │ │ +
ImuFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
23#pragma once
│ │ │ │ -
24
│ │ │ │ -
25/* GTSAM includes */
│ │ │ │ -
26#include <gtsam/navigation/ManifoldPreintegration.h>
│ │ │ │ -
27#include <gtsam/navigation/TangentPreintegration.h>
│ │ │ │ -
28#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
29#include <gtsam/base/Matrix.h>
│ │ │ │ -
30#include <gtsam/base/serialization.h>
│ │ │ │ +
22#pragma once
│ │ │ │ +
23
│ │ │ │ +
24/* GTSAM includes */
│ │ │ │ +
25#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
26#include <gtsam/navigation/ManifoldPreintegration.h>
│ │ │ │ +
27#include <gtsam/navigation/TangentPreintegration.h>
│ │ │ │ +
28#include <gtsam/base/debug.h>
│ │ │ │ +
29
│ │ │ │ +
30namespace gtsam {
│ │ │ │
31
│ │ │ │ -
32namespace gtsam {
│ │ │ │ -
33
│ │ │ │ -
34#ifdef GTSAM_TANGENT_PREINTEGRATION
│ │ │ │ -
35typedef TangentPreintegration PreintegrationType;
│ │ │ │ -
36#else
│ │ │ │ -
37typedef ManifoldPreintegration PreintegrationType;
│ │ │ │ -
38#endif
│ │ │ │ -
39
│ │ │ │ -
40/*
│ │ │ │ -
41 * If you are using the factor, please cite:
│ │ │ │ -
42 * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating
│ │ │ │ -
43 * conditionally independent sets in factor graphs: a unifying perspective based
│ │ │ │ -
44 * on smart factors, Int. Conf. on Robotics and Automation (ICRA), 2014.
│ │ │ │ -
45 *
│ │ │ │ -
46 * REFERENCES:
│ │ │ │ -
47 * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups",
│ │ │ │ -
48 * Volume 2, 2008.
│ │ │ │ -
49 * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for
│ │ │ │ -
50 * High-Dynamic Motion in Built Environments Without Initial Conditions",
│ │ │ │ -
51 * TRO, 28(1):61-76, 2012.
│ │ │ │ -
52 * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
│ │ │ │ -
53 * Computation of the Jacobian Matrices", Tech. Report, 2013.
│ │ │ │ -
54 * Available in this repo as "PreintegratedIMUJacobians.pdf".
│ │ │ │ -
55 * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, IMU Preintegration on
│ │ │ │ -
56 * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation,
│ │ │ │ -
57 * Robotics: Science and Systems (RSS), 2015.
│ │ │ │ -
58 */
│ │ │ │ -
59
│ │ │ │ -
│ │ │ │ -
62struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
│ │ │ │ -
63 Matrix3 biasAccCovariance;
│ │ │ │ -
64 Matrix3 biasOmegaCovariance;
│ │ │ │ -
65 Matrix6 biasAccOmegaInt;
│ │ │ │ -
66
│ │ │ │ -
│ │ │ │ -
69 PreintegrationCombinedParams()
│ │ │ │ -
70 : biasAccCovariance(I_3x3),
│ │ │ │ -
71 biasOmegaCovariance(I_3x3),
│ │ │ │ -
72 biasAccOmegaInt(I_6x6) {}
│ │ │ │ -
│ │ │ │ +
32#ifdef GTSAM_TANGENT_PREINTEGRATION
│ │ │ │ +
33typedef TangentPreintegration PreintegrationType;
│ │ │ │ +
34#else
│ │ │ │ +
35typedef ManifoldPreintegration PreintegrationType;
│ │ │ │ +
36#endif
│ │ │ │ +
37
│ │ │ │ +
38/*
│ │ │ │ +
39 * If you are using the factor, please cite:
│ │ │ │ +
40 * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, "Eliminating
│ │ │ │ +
41 * conditionally independent sets in factor graphs: a unifying perspective based
│ │ │ │ +
42 * on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014.
│ │ │ │ +
43 *
│ │ │ │ +
44 * C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on
│ │ │ │ +
45 * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation",
│ │ │ │ +
46 * Robotics: Science and Systems (RSS), 2015.
│ │ │ │ +
47 *
│ │ │ │ +
48 * REFERENCES:
│ │ │ │ +
49 * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups",
│ │ │ │ +
50 * Volume 2, 2008.
│ │ │ │ +
51 * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for
│ │ │ │ +
52 * High-Dynamic Motion in Built Environments Without Initial Conditions",
│ │ │ │ +
53 * TRO, 28(1):61-76, 2012.
│ │ │ │ +
54 * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
│ │ │ │ +
55 * Computation of the Jacobian Matrices", Tech. Report, 2013.
│ │ │ │ +
56 * Available in this repo as "PreintegratedIMUJacobians.pdf".
│ │ │ │ +
57 * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on
│ │ │ │ +
58 * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation",
│ │ │ │ +
59 * Robotics: Science and Systems (RSS), 2015.
│ │ │ │ +
60 */
│ │ │ │ +
61
│ │ │ │ +
│ │ │ │ +
72class GTSAM_EXPORT PreintegratedImuMeasurements: public PreintegrationType {
│ │ │ │
73
│ │ │ │ -
│ │ │ │ -
75 PreintegrationCombinedParams(const Vector3& n_gravity) :
│ │ │ │ -
76 PreintegrationParams(n_gravity), biasAccCovariance(I_3x3),
│ │ │ │ -
77 biasOmegaCovariance(I_3x3), biasAccOmegaInt(I_6x6) {
│ │ │ │ +
74 friend class ImuFactor;
│ │ │ │ +
75 friend class ImuFactor2;
│ │ │ │ +
76
│ │ │ │ +
77protected:
│ │ │ │
78
│ │ │ │ -
79 }
│ │ │ │ -
│ │ │ │ -
80
│ │ │ │ -
81 // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
│ │ │ │ -
82 static boost::shared_ptr<PreintegrationCombinedParams> MakeSharedD(double g = 9.81) {
│ │ │ │ -
83 return boost::shared_ptr<PreintegrationCombinedParams>(new PreintegrationCombinedParams(Vector3(0, 0, g)));
│ │ │ │ -
84 }
│ │ │ │ -
85
│ │ │ │ -
86 // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
│ │ │ │ -
87 static boost::shared_ptr<PreintegrationCombinedParams> MakeSharedU(double g = 9.81) {
│ │ │ │ -
88 return boost::shared_ptr<PreintegrationCombinedParams>(new PreintegrationCombinedParams(Vector3(0, 0, -g)));
│ │ │ │ -
89 }
│ │ │ │ -
90
│ │ │ │ -
91 void print(const std::string& s="") const override;
│ │ │ │ -
92 bool equals(const PreintegratedRotationParams& other, double tol) const override;
│ │ │ │ -
93
│ │ │ │ -
94 void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; }
│ │ │ │ -
95 void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; }
│ │ │ │ -
96 void setBiasAccOmegaInit(const Matrix6& cov) { biasAccOmegaInt=cov; }
│ │ │ │ -
97
│ │ │ │ -
98 const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; }
│ │ │ │ -
99 const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; }
│ │ │ │ -
100 const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; }
│ │ │ │ -
101
│ │ │ │ -
102private:
│ │ │ │ -
103
│ │ │ │ -
105 friend class boost::serialization::access;
│ │ │ │ -
106 template <class ARCHIVE>
│ │ │ │ -
107 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ -
108 namespace bs = ::boost::serialization;
│ │ │ │ -
109 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationParams);
│ │ │ │ -
110 ar & BOOST_SERIALIZATION_NVP(biasAccCovariance);
│ │ │ │ -
111 ar & BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
│ │ │ │ -
112 ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
│ │ │ │ -
113 }
│ │ │ │ -
114
│ │ │ │ -
115public:
│ │ │ │ -
116 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
117};
│ │ │ │ -
│ │ │ │ -
118
│ │ │ │ -
│ │ │ │ -
129class GTSAM_EXPORT PreintegratedCombinedMeasurements : public PreintegrationType {
│ │ │ │ -
130
│ │ │ │ -
131public:
│ │ │ │ -
132 typedef PreintegrationCombinedParams Params;
│ │ │ │ -
133
│ │ │ │ -
134 protected:
│ │ │ │ -
135 /* Covariance matrix of the preintegrated measurements
│ │ │ │ -
136 * COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY BiasAcc BiasOmega]
│ │ │ │ -
137 * (first-order propagation from *measurementCovariance*).
│ │ │ │ -
138 * PreintegratedCombinedMeasurements also include the biases and keep the correlation
│ │ │ │ -
139 * between the preintegrated measurements and the biases
│ │ │ │ -
140 */
│ │ │ │ -
141 Eigen::Matrix<double, 15, 15> preintMeasCov_;
│ │ │ │ +
79 Matrix9 preintMeasCov_;
│ │ │ │ +
81
│ │ │ │ +
82public:
│ │ │ │ +
83
│ │ │ │ +
│ │ │ │ +
85 PreintegratedImuMeasurements() {
│ │ │ │ +
86 preintMeasCov_.setZero();
│ │ │ │ +
87 }
│ │ │ │ +
│ │ │ │ +
88
│ │ │ │ +
│ │ │ │ +
94 PreintegratedImuMeasurements(const boost::shared_ptr<PreintegrationParams>& p,
│ │ │ │ +
95 const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
│ │ │ │ +
96 PreintegrationType(p, biasHat) {
│ │ │ │ +
97 preintMeasCov_.setZero();
│ │ │ │ +
98 }
│ │ │ │ +
│ │ │ │ +
99
│ │ │ │ +
│ │ │ │ +
105 PreintegratedImuMeasurements(const PreintegrationType& base, const Matrix9& preintMeasCov)
│ │ │ │ +
106 : PreintegrationType(base),
│ │ │ │ +
107 preintMeasCov_(preintMeasCov) {
│ │ │ │ +
108 }
│ │ │ │ +
│ │ │ │ +
109
│ │ │ │ +
│ │ │ │ +
111 ~PreintegratedImuMeasurements() override {
│ │ │ │ +
112 }
│ │ │ │ +
│ │ │ │ +
113
│ │ │ │ +
115 void print(const std::string& s = "Preintegrated Measurements:") const override;
│ │ │ │ +
116
│ │ │ │ +
118 bool equals(const PreintegratedImuMeasurements& expected, double tol = 1e-9) const;
│ │ │ │ +
119
│ │ │ │ +
121 void resetIntegration() override;
│ │ │ │ +
122
│ │ │ │ +
133 void integrateMeasurement(const Vector3& measuredAcc,
│ │ │ │ +
134 const Vector3& measuredOmega, const double dt) override;
│ │ │ │ +
135
│ │ │ │ +
137 void integrateMeasurements(const Matrix& measuredAccs, const Matrix& measuredOmegas,
│ │ │ │ +
138 const Matrix& dts);
│ │ │ │ +
139
│ │ │ │ +
141 Matrix preintMeasCov() const { return preintMeasCov_; }
│ │ │ │
142
│ │ │ │ -
143 friend class CombinedImuFactor;
│ │ │ │ -
144
│ │ │ │ -
145 public:
│ │ │ │ -
148
│ │ │ │ -
│ │ │ │ -
150 PreintegratedCombinedMeasurements() {
│ │ │ │ -
151 preintMeasCov_.setZero();
│ │ │ │ -
152 }
│ │ │ │ -
│ │ │ │ -
153
│ │ │ │ -
│ │ │ │ -
159 PreintegratedCombinedMeasurements(
│ │ │ │ -
160 const boost::shared_ptr<Params>& p,
│ │ │ │ -
161 const imuBias::ConstantBias& biasHat = imuBias::ConstantBias())
│ │ │ │ -
162 : PreintegrationType(p, biasHat) {
│ │ │ │ -
163 preintMeasCov_.setZero();
│ │ │ │ -
164 }
│ │ │ │ -
│ │ │ │ -
165
│ │ │ │ -
│ │ │ │ -
171 PreintegratedCombinedMeasurements(const PreintegrationType& base, const Eigen::Matrix<double, 15, 15>& preintMeasCov)
│ │ │ │ -
172 : PreintegrationType(base),
│ │ │ │ -
173 preintMeasCov_(preintMeasCov) {
│ │ │ │ -
174 }
│ │ │ │ -
│ │ │ │ -
175
│ │ │ │ -
177 ~PreintegratedCombinedMeasurements() override {}
│ │ │ │ +
143#ifdef GTSAM_TANGENT_PREINTEGRATION
│ │ │ │ +
145 void mergeWith(const PreintegratedImuMeasurements& pim, Matrix9* H1, Matrix9* H2);
│ │ │ │ +
146#endif
│ │ │ │ +
147
│ │ │ │ +
148 private:
│ │ │ │ +
150 friend class boost::serialization::access;
│ │ │ │ +
151 template<class ARCHIVE>
│ │ │ │ +
152 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
153 namespace bs = ::boost::serialization;
│ │ │ │ +
154 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
│ │ │ │ +
155 ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
│ │ │ │ +
156 }
│ │ │ │ +
157};
│ │ │ │ +
│ │ │ │ +
158
│ │ │ │ +
│ │ │ │ +
171class GTSAM_EXPORT ImuFactor: public NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
│ │ │ │ +
172 imuBias::ConstantBias> {
│ │ │ │ +
173private:
│ │ │ │ +
174
│ │ │ │ +
175 typedef ImuFactor This;
│ │ │ │ +
176 typedef NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
│ │ │ │ +
177 imuBias::ConstantBias> Base;
│ │ │ │
178
│ │ │ │ +
179 PreintegratedImuMeasurements _PIM_;
│ │ │ │
180
│ │ │ │ -
183
│ │ │ │ -
185 void resetIntegration() override;
│ │ │ │ -
186
│ │ │ │ -
188 Params& p() const { return *boost::static_pointer_cast<Params>(this->p_); }
│ │ │ │ -
190
│ │ │ │ -
194 Matrix preintMeasCov() const { return preintMeasCov_; }
│ │ │ │ -
196
│ │ │ │ -
200 void print(const std::string& s = "Preintegrated Measurements:") const override;
│ │ │ │ -
202 bool equals(const PreintegratedCombinedMeasurements& expected,
│ │ │ │ -
203 double tol = 1e-9) const;
│ │ │ │ +
181public:
│ │ │ │ +
182
│ │ │ │ +
184#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
│ │ │ │ +
185 typedef typename boost::shared_ptr<ImuFactor> shared_ptr;
│ │ │ │ +
186#else
│ │ │ │ +
187 typedef boost::shared_ptr<ImuFactor> shared_ptr;
│ │ │ │ +
188#endif
│ │ │ │ +
189
│ │ │ │ +
191 ImuFactor() {}
│ │ │ │ +
192
│ │ │ │ +
203 ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
│ │ │ │ +
204 const PreintegratedImuMeasurements& preintegratedMeasurements);
│ │ │ │
205
│ │ │ │ -
206
│ │ │ │ -
209
│ │ │ │ -
220 void integrateMeasurement(const Vector3& measuredAcc,
│ │ │ │ -
221 const Vector3& measuredOmega, const double dt) override;
│ │ │ │ -
222
│ │ │ │ -
224
│ │ │ │ -
225 private:
│ │ │ │ -
227 friend class boost::serialization::access;
│ │ │ │ -
228 template <class ARCHIVE>
│ │ │ │ -
229 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ -
230 namespace bs = ::boost::serialization;
│ │ │ │ -
231 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
│ │ │ │ -
232 ar& BOOST_SERIALIZATION_NVP(preintMeasCov_);
│ │ │ │ -
233 }
│ │ │ │ -
234
│ │ │ │ -
235public:
│ │ │ │ -
236 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
237};
│ │ │ │ -
│ │ │ │ -
238
│ │ │ │ -
│ │ │ │ -
258class GTSAM_EXPORT CombinedImuFactor: public NoiseModelFactorN<Pose3, Vector3, Pose3,
│ │ │ │ -
259 Vector3, imuBias::ConstantBias, imuBias::ConstantBias> {
│ │ │ │ -
260public:
│ │ │ │ -
261
│ │ │ │ -
262private:
│ │ │ │ -
263
│ │ │ │ -
264 typedef CombinedImuFactor This;
│ │ │ │ -
265 typedef NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
│ │ │ │ -
266 imuBias::ConstantBias, imuBias::ConstantBias> Base;
│ │ │ │ -
267
│ │ │ │ -
268 PreintegratedCombinedMeasurements _PIM_;
│ │ │ │ -
269
│ │ │ │ -
270public:
│ │ │ │ -
271
│ │ │ │ -
273#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
│ │ │ │ -
274 typedef typename boost::shared_ptr<CombinedImuFactor> shared_ptr;
│ │ │ │ -
275#else
│ │ │ │ -
276 typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
│ │ │ │ -
277#endif
│ │ │ │ -
278
│ │ │ │ -
280 CombinedImuFactor() {}
│ │ │ │ -
281
│ │ │ │ -
292 CombinedImuFactor(
│ │ │ │ -
293 Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
│ │ │ │ -
294 const PreintegratedCombinedMeasurements& preintegratedMeasurements);
│ │ │ │ -
295
│ │ │ │ -
296 ~CombinedImuFactor() override {}
│ │ │ │ -
297
│ │ │ │ -
299 gtsam::NonlinearFactor::shared_ptr clone() const override;
│ │ │ │ -
300
│ │ │ │ -
305 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
│ │ │ │ -
306 const CombinedImuFactor&);
│ │ │ │ -
308 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
│ │ │ │ -
309 DefaultKeyFormatter) const override;
│ │ │ │ -
310
│ │ │ │ -
312 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
│ │ │ │ -
314
│ │ │ │ -
│ │ │ │ -
317 const PreintegratedCombinedMeasurements& preintegratedMeasurements() const {
│ │ │ │ -
318 return _PIM_;
│ │ │ │ -
319 }
│ │ │ │ -
│ │ │ │ -
320
│ │ │ │ -
324 Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
│ │ │ │ -
325 const Pose3& pose_j, const Vector3& vel_j,
│ │ │ │ -
326 const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
│ │ │ │ -
327 boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
│ │ │ │ -
328 boost::none, boost::optional<Matrix&> H3 = boost::none,
│ │ │ │ -
329 boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 =
│ │ │ │ -
330 boost::none, boost::optional<Matrix&> H6 = boost::none) const override;
│ │ │ │ -
331
│ │ │ │ -
332 private:
│ │ │ │ -
334 friend class boost::serialization::access;
│ │ │ │ -
335 template <class ARCHIVE>
│ │ │ │ -
336 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ -
337 // NoiseModelFactor6 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ -
338 ar& boost::serialization::make_nvp(
│ │ │ │ -
339 "NoiseModelFactor6", boost::serialization::base_object<Base>(*this));
│ │ │ │ -
340 ar& BOOST_SERIALIZATION_NVP(_PIM_);
│ │ │ │ -
341 }
│ │ │ │ -
342
│ │ │ │ -
343public:
│ │ │ │ -
344 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
345};
│ │ │ │ -
│ │ │ │ -
346// class CombinedImuFactor
│ │ │ │ -
347
│ │ │ │ -
348template <>
│ │ │ │ -
│ │ │ │ -
349struct traits<PreintegrationCombinedParams>
│ │ │ │ -
350 : public Testable<PreintegrationCombinedParams> {};
│ │ │ │ -
│ │ │ │ -
351
│ │ │ │ -
352template <>
│ │ │ │ -
│ │ │ │ -
353struct traits<PreintegratedCombinedMeasurements>
│ │ │ │ -
354 : public Testable<PreintegratedCombinedMeasurements> {};
│ │ │ │ -
│ │ │ │ -
355
│ │ │ │ -
356template <>
│ │ │ │ -
357struct traits<CombinedImuFactor> : public Testable<CombinedImuFactor> {};
│ │ │ │ -
358
│ │ │ │ -
359} // namespace gtsam
│ │ │ │ -
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ -
serialization.h
Convenience functions for serializing data structures via boost.serialization.
│ │ │ │ -
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ - │ │ │ │ - │ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
206 ~ImuFactor() override {
│ │ │ │ +
207 }
│ │ │ │ +
208
│ │ │ │ +
210 gtsam::NonlinearFactor::shared_ptr clone() const override;
│ │ │ │ +
211
│ │ │ │ +
214 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor&);
│ │ │ │ +
215 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
│ │ │ │ +
216 DefaultKeyFormatter) const override;
│ │ │ │ +
217 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
│ │ │ │ +
219
│ │ │ │ +
│ │ │ │ +
222 const PreintegratedImuMeasurements& preintegratedMeasurements() const {
│ │ │ │ +
223 return _PIM_;
│ │ │ │ +
224 }
│ │ │ │ +
│ │ │ │ +
225
│ │ │ │ +
229 Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
│ │ │ │ +
230 const Pose3& pose_j, const Vector3& vel_j,
│ │ │ │ +
231 const imuBias::ConstantBias& bias_i, boost::optional<Matrix&> H1 =
│ │ │ │ +
232 boost::none, boost::optional<Matrix&> H2 = boost::none,
│ │ │ │ +
233 boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
│ │ │ │ +
234 boost::none, boost::optional<Matrix&> H5 = boost::none) const override;
│ │ │ │ +
235
│ │ │ │ +
236#ifdef GTSAM_TANGENT_PREINTEGRATION
│ │ │ │ +
238 static PreintegratedImuMeasurements Merge(
│ │ │ │ +
239 const PreintegratedImuMeasurements& pim01,
│ │ │ │ +
240 const PreintegratedImuMeasurements& pim12);
│ │ │ │ +
241
│ │ │ │ +
243 static shared_ptr Merge(const shared_ptr& f01, const shared_ptr& f12);
│ │ │ │ +
244#endif
│ │ │ │ +
245
│ │ │ │ +
246 private:
│ │ │ │ +
248 friend class boost::serialization::access;
│ │ │ │ +
249 template<class ARCHIVE>
│ │ │ │ +
250 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
251 // NoiseModelFactor5 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ +
252 ar & boost::serialization::make_nvp("NoiseModelFactor5",
│ │ │ │ +
253 boost::serialization::base_object<Base>(*this));
│ │ │ │ +
254 ar & BOOST_SERIALIZATION_NVP(_PIM_);
│ │ │ │ +
255 }
│ │ │ │ +
256};
│ │ │ │ +
│ │ │ │ +
257// class ImuFactor
│ │ │ │ +
258
│ │ │ │ +
│ │ │ │ +
263class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactorN<NavState, NavState, imuBias::ConstantBias> {
│ │ │ │ +
264private:
│ │ │ │ +
265
│ │ │ │ +
266 typedef ImuFactor2 This;
│ │ │ │ +
267 typedef NoiseModelFactorN<NavState, NavState, imuBias::ConstantBias> Base;
│ │ │ │ +
268
│ │ │ │ +
269 PreintegratedImuMeasurements _PIM_;
│ │ │ │ +
270
│ │ │ │ +
271public:
│ │ │ │ +
272
│ │ │ │ +
274 ImuFactor2() {}
│ │ │ │ +
275
│ │ │ │ +
282 ImuFactor2(Key state_i, Key state_j, Key bias,
│ │ │ │ +
283 const PreintegratedImuMeasurements& preintegratedMeasurements);
│ │ │ │ +
284
│ │ │ │ +
285 ~ImuFactor2() override {
│ │ │ │ +
286 }
│ │ │ │ +
287
│ │ │ │ +
289 gtsam::NonlinearFactor::shared_ptr clone() const override;
│ │ │ │ +
290
│ │ │ │ +
293 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor2&);
│ │ │ │ +
294 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
│ │ │ │ +
295 DefaultKeyFormatter) const override;
│ │ │ │ +
296 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
│ │ │ │ +
298
│ │ │ │ +
│ │ │ │ +
301 const PreintegratedImuMeasurements& preintegratedMeasurements() const {
│ │ │ │ +
302 return _PIM_;
│ │ │ │ +
303 }
│ │ │ │ +
│ │ │ │ +
304
│ │ │ │ +
308 Vector evaluateError(const NavState& state_i, const NavState& state_j,
│ │ │ │ +
309 const imuBias::ConstantBias& bias_i, //
│ │ │ │ +
310 boost::optional<Matrix&> H1 = boost::none,
│ │ │ │ +
311 boost::optional<Matrix&> H2 = boost::none,
│ │ │ │ +
312 boost::optional<Matrix&> H3 = boost::none) const override;
│ │ │ │ +
313
│ │ │ │ +
314private:
│ │ │ │ +
315
│ │ │ │ +
317 friend class boost::serialization::access;
│ │ │ │ +
318 template<class ARCHIVE>
│ │ │ │ +
319 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
320 // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ +
321 ar & boost::serialization::make_nvp("NoiseModelFactor3",
│ │ │ │ +
322 boost::serialization::base_object<Base>(*this));
│ │ │ │ +
323 ar & BOOST_SERIALIZATION_NVP(_PIM_);
│ │ │ │ +
324 }
│ │ │ │ +
325};
│ │ │ │ +
│ │ │ │ +
326// class ImuFactor2
│ │ │ │ +
327
│ │ │ │ +
328template <>
│ │ │ │ +
329struct traits<PreintegratedImuMeasurements> : public Testable<PreintegratedImuMeasurements> {};
│ │ │ │ +
330
│ │ │ │ +
331template <>
│ │ │ │ +
332struct traits<ImuFactor> : public Testable<ImuFactor> {};
│ │ │ │ +
333
│ │ │ │ +
334template <>
│ │ │ │ +
335struct traits<ImuFactor2> : public Testable<ImuFactor2> {};
│ │ │ │ +
336
│ │ │ │ +
337}
│ │ │ │ +
debug.h
Global debugging flags.
│ │ │ │ + │ │ │ │ + │ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ -
gtsam::PreintegrationCombinedParams
Parameters for pre-integration using PreintegratedCombinedMeasurements: Usage: Create just a single P...
Definition CombinedImuFactor.h:62
│ │ │ │ -
gtsam::PreintegrationCombinedParams::biasAccOmegaInt
Matrix6 biasAccOmegaInt
covariance of bias used as initial estimate.
Definition CombinedImuFactor.h:65
│ │ │ │ -
gtsam::PreintegrationCombinedParams::PreintegrationCombinedParams
PreintegrationCombinedParams(const Vector3 &n_gravity)
See two named constructors below for good values of n_gravity in body frame.
Definition CombinedImuFactor.h:75
│ │ │ │ -
gtsam::PreintegrationCombinedParams::PreintegrationCombinedParams
PreintegrationCombinedParams()
Default constructor makes uninitialized params struct.
Definition CombinedImuFactor.h:69
│ │ │ │ -
gtsam::PreintegrationCombinedParams::biasOmegaCovariance
Matrix3 biasOmegaCovariance
continuous-time "Covariance" describing gyroscope bias random walk
Definition CombinedImuFactor.h:64
│ │ │ │ -
gtsam::PreintegrationCombinedParams::biasAccCovariance
Matrix3 biasAccCovariance
continuous-time "Covariance" describing accelerometer bias random walk
Definition CombinedImuFactor.h:63
│ │ │ │ -
gtsam::PreintegratedCombinedMeasurements
PreintegratedCombinedMeasurements integrates the IMU measurements (rotation rates and accelerations) ...
Definition CombinedImuFactor.h:129
│ │ │ │ -
gtsam::PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements
PreintegratedCombinedMeasurements(const PreintegrationType &base, const Eigen::Matrix< double, 15, 15 > &preintMeasCov)
Construct preintegrated directly from members: base class and preintMeasCov.
Definition CombinedImuFactor.h:171
│ │ │ │ -
gtsam::PreintegratedCombinedMeasurements::~PreintegratedCombinedMeasurements
~PreintegratedCombinedMeasurements() override
Virtual destructor.
Definition CombinedImuFactor.h:177
│ │ │ │ -
gtsam::PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements
PreintegratedCombinedMeasurements()
Default constructor only for serialization and wrappers.
Definition CombinedImuFactor.h:150
│ │ │ │ -
gtsam::PreintegratedCombinedMeasurements::p
Params & p() const
const reference to params, shadows definition in base class
Definition CombinedImuFactor.h:188
│ │ │ │ -
gtsam::PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements
PreintegratedCombinedMeasurements(const boost::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
Default constructor, initializes the class with no measurements.
Definition CombinedImuFactor.h:159
│ │ │ │ -
gtsam::CombinedImuFactor
CombinedImuFactor is a 6-ways factor involving previous state (pose and velocity of the vehicle,...
Definition CombinedImuFactor.h:259
│ │ │ │ -
gtsam::CombinedImuFactor::shared_ptr
boost::shared_ptr< CombinedImuFactor > shared_ptr
Shorthand for a smart pointer to a factor.
Definition CombinedImuFactor.h:276
│ │ │ │ -
gtsam::CombinedImuFactor::CombinedImuFactor
CombinedImuFactor()
Default constructor - only use for serialization.
Definition CombinedImuFactor.h:280
│ │ │ │ -
gtsam::CombinedImuFactor::preintegratedMeasurements
const PreintegratedCombinedMeasurements & preintegratedMeasurements() const
Access the preintegrated measurements.
Definition CombinedImuFactor.h:317
│ │ │ │
gtsam::imuBias::ConstantBias
Definition ImuBias.h:30
│ │ │ │ +
gtsam::PreintegratedImuMeasurements
PreintegratedImuMeasurements accumulates (integrates) the IMU measurements (rotation rates and accele...
Definition ImuFactor.h:72
│ │ │ │ +
gtsam::PreintegratedImuMeasurements::~PreintegratedImuMeasurements
~PreintegratedImuMeasurements() override
Virtual destructor.
Definition ImuFactor.h:111
│ │ │ │ +
gtsam::PreintegratedImuMeasurements::PreintegratedImuMeasurements
PreintegratedImuMeasurements(const boost::shared_ptr< PreintegrationParams > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
Constructor, initializes the class with no measurements.
Definition ImuFactor.h:94
│ │ │ │ +
gtsam::PreintegratedImuMeasurements::PreintegratedImuMeasurements
PreintegratedImuMeasurements(const PreintegrationType &base, const Matrix9 &preintMeasCov)
Construct preintegrated directly from members: base class and preintMeasCov.
Definition ImuFactor.h:105
│ │ │ │ +
gtsam::PreintegratedImuMeasurements::PreintegratedImuMeasurements
PreintegratedImuMeasurements()
Default constructor for serialization and wrappers.
Definition ImuFactor.h:85
│ │ │ │ +
gtsam::PreintegratedImuMeasurements::preintMeasCov
Matrix preintMeasCov() const
Return pre-integrated measurement covariance.
Definition ImuFactor.h:141
│ │ │ │ +
gtsam::PreintegratedImuMeasurements::preintMeasCov_
Matrix9 preintMeasCov_
COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY].
Definition ImuFactor.h:79
│ │ │ │ +
gtsam::ImuFactor
ImuFactor is a 5-ways factor involving previous state (pose and velocity of the vehicle at previous t...
Definition ImuFactor.h:172
│ │ │ │ +
gtsam::ImuFactor::preintegratedMeasurements
const PreintegratedImuMeasurements & preintegratedMeasurements() const
Access the preintegrated measurements.
Definition ImuFactor.h:222
│ │ │ │ +
gtsam::ImuFactor::ImuFactor
ImuFactor()
Default constructor - only use for serialization.
Definition ImuFactor.h:191
│ │ │ │ +
gtsam::ImuFactor::shared_ptr
boost::shared_ptr< ImuFactor > shared_ptr
Shorthand for a smart pointer to a factor.
Definition ImuFactor.h:187
│ │ │ │ +
gtsam::ImuFactor2
ImuFactor2 is a ternary factor that uses NavStates rather than Pose/Velocity.
Definition ImuFactor.h:263
│ │ │ │ +
gtsam::ImuFactor2::ImuFactor2
ImuFactor2()
Default constructor - only use for serialization.
Definition ImuFactor.h:274
│ │ │ │ +
gtsam::ImuFactor2::preintegratedMeasurements
const PreintegratedImuMeasurements & preintegratedMeasurements() const
Access the preintegrated measurements.
Definition ImuFactor.h:301
│ │ │ │
gtsam::ManifoldPreintegration
IMU pre-integration on NavSatet manifold.
Definition ManifoldPreintegration.h:33
│ │ │ │ -
gtsam::PreintegrationParams
Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
Definition PreintegrationParams.h:26
│ │ │ │ +
gtsam::NavState
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition NavState.h:34
│ │ │ │
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,288 +1,266 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -CombinedImuFactor.h │ │ │ │ │ +ImuFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -23#pragma once │ │ │ │ │ -24 │ │ │ │ │ -25/* GTSAM includes */ │ │ │ │ │ +22#pragma once │ │ │ │ │ +23 │ │ │ │ │ +24/* GTSAM includes */ │ │ │ │ │ +25#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ 26#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_._h> │ │ │ │ │ 27#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n_._h> │ │ │ │ │ -28#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -29#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ -30#include <_g_t_s_a_m_/_b_a_s_e_/_s_e_r_i_a_l_i_z_a_t_i_o_n_._h> │ │ │ │ │ +28#include <_g_t_s_a_m_/_b_a_s_e_/_d_e_b_u_g_._h> │ │ │ │ │ +29 │ │ │ │ │ +30namespace _g_t_s_a_m { │ │ │ │ │ 31 │ │ │ │ │ -32namespace _g_t_s_a_m { │ │ │ │ │ -33 │ │ │ │ │ -34#ifdef GTSAM_TANGENT_PREINTEGRATION │ │ │ │ │ -35typedef TangentPreintegration PreintegrationType; │ │ │ │ │ -36#else │ │ │ │ │ -37typedef ManifoldPreintegration PreintegrationType; │ │ │ │ │ -38#endif │ │ │ │ │ -39 │ │ │ │ │ -40/* │ │ │ │ │ -41 * If you are using the factor, please cite: │ │ │ │ │ -42 * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating │ │ │ │ │ -43 * conditionally independent sets in factor graphs: a unifying perspective │ │ │ │ │ +32#ifdef GTSAM_TANGENT_PREINTEGRATION │ │ │ │ │ +33typedef TangentPreintegration PreintegrationType; │ │ │ │ │ +34#else │ │ │ │ │ +35typedef ManifoldPreintegration PreintegrationType; │ │ │ │ │ +36#endif │ │ │ │ │ +37 │ │ │ │ │ +38/* │ │ │ │ │ +39 * If you are using the factor, please cite: │ │ │ │ │ +40 * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, "Eliminating │ │ │ │ │ +41 * conditionally independent sets in factor graphs: a unifying perspective │ │ │ │ │ based │ │ │ │ │ -44 * on smart factors, Int. Conf. on Robotics and Automation (ICRA), 2014. │ │ │ │ │ -45 * │ │ │ │ │ -46 * REFERENCES: │ │ │ │ │ -47 * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie │ │ │ │ │ +42 * on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014. │ │ │ │ │ +43 * │ │ │ │ │ +44 * C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on │ │ │ │ │ +45 * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", │ │ │ │ │ +46 * Robotics: Science and Systems (RSS), 2015. │ │ │ │ │ +47 * │ │ │ │ │ +48 * REFERENCES: │ │ │ │ │ +49 * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie │ │ │ │ │ Groups", │ │ │ │ │ -48 * Volume 2, 2008. │ │ │ │ │ -49 * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for │ │ │ │ │ -50 * High-Dynamic Motion in Built Environments Without Initial Conditions", │ │ │ │ │ -51 * TRO, 28(1):61-76, 2012. │ │ │ │ │ -52 * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: │ │ │ │ │ -53 * Computation of the Jacobian Matrices", Tech. Report, 2013. │ │ │ │ │ -54 * Available in this repo as "PreintegratedIMUJacobians.pdf". │ │ │ │ │ -55 * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, IMU Preintegration │ │ │ │ │ -on │ │ │ │ │ -56 * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation, │ │ │ │ │ -57 * Robotics: Science and Systems (RSS), 2015. │ │ │ │ │ -58 */ │ │ │ │ │ -59 │ │ │ │ │ -_6_2struct GTSAM_EXPORT _P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s : _P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s { │ │ │ │ │ -_6_3 Matrix3 _b_i_a_s_A_c_c_C_o_v_a_r_i_a_n_c_e; │ │ │ │ │ -_6_4 Matrix3 _b_i_a_s_O_m_e_g_a_C_o_v_a_r_i_a_n_c_e; │ │ │ │ │ -_6_5 Matrix6 _b_i_a_s_A_c_c_O_m_e_g_a_I_n_t; │ │ │ │ │ -66 │ │ │ │ │ -_6_9 _P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s() │ │ │ │ │ -70 : biasAccCovariance(I_3x3), │ │ │ │ │ -71 biasOmegaCovariance(I_3x3), │ │ │ │ │ -72 biasAccOmegaInt(I_6x6) {} │ │ │ │ │ +50 * Volume 2, 2008. │ │ │ │ │ +51 * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for │ │ │ │ │ +52 * High-Dynamic Motion in Built Environments Without Initial Conditions", │ │ │ │ │ +53 * TRO, 28(1):61-76, 2012. │ │ │ │ │ +54 * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: │ │ │ │ │ +55 * Computation of the Jacobian Matrices", Tech. Report, 2013. │ │ │ │ │ +56 * Available in this repo as "PreintegratedIMUJacobians.pdf". │ │ │ │ │ +57 * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU │ │ │ │ │ +Preintegration on │ │ │ │ │ +58 * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", │ │ │ │ │ +59 * Robotics: Science and Systems (RSS), 2015. │ │ │ │ │ +60 */ │ │ │ │ │ +61 │ │ │ │ │ +_7_2class GTSAM_EXPORT _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s: public _P_r_e_i_n_t_e_g_r_a_t_i_o_n_T_y_p_e { │ │ │ │ │ 73 │ │ │ │ │ -_7_5 _P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s(const Vector3& n_gravity) : │ │ │ │ │ -76 _P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s(n_gravity), biasAccCovariance(I_3x3), │ │ │ │ │ -77 biasOmegaCovariance(I_3x3), biasAccOmegaInt(I_6x6) { │ │ │ │ │ +74 friend class _I_m_u_F_a_c_t_o_r; │ │ │ │ │ +75 friend class _I_m_u_F_a_c_t_o_r_2; │ │ │ │ │ +76 │ │ │ │ │ +77protected: │ │ │ │ │ 78 │ │ │ │ │ -79 } │ │ │ │ │ -80 │ │ │ │ │ -81 // Default Params for a Z-down navigation frame, such as NED: gravity points │ │ │ │ │ -along positive Z-axis │ │ │ │ │ -82 static boost::shared_ptr MakeSharedD(double g │ │ │ │ │ -= 9.81) { │ │ │ │ │ -83 return boost::shared_ptr(new │ │ │ │ │ -_P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s(Vector3(0, 0, g))); │ │ │ │ │ -84 } │ │ │ │ │ -85 │ │ │ │ │ -86 // Default Params for a Z-up navigation frame, such as ENU: gravity points │ │ │ │ │ -along negative Z-axis │ │ │ │ │ -87 static boost::shared_ptr MakeSharedU(double g │ │ │ │ │ -= 9.81) { │ │ │ │ │ -88 return boost::shared_ptr(new │ │ │ │ │ -PreintegrationCombinedParams(Vector3(0, 0, -g))); │ │ │ │ │ -89 } │ │ │ │ │ -90 │ │ │ │ │ -91 void _p_r_i_n_t(const std::string& s="") const override; │ │ │ │ │ -92 bool equals(const PreintegratedRotationParams& other, double tol) const │ │ │ │ │ +_7_9 Matrix9 _p_r_e_i_n_t_M_e_a_s_C_o_v__; │ │ │ │ │ +81 │ │ │ │ │ +82public: │ │ │ │ │ +83 │ │ │ │ │ +_8_5 _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s() { │ │ │ │ │ +86 preintMeasCov_.setZero(); │ │ │ │ │ +87 } │ │ │ │ │ +88 │ │ │ │ │ +_9_4 _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s(const boost::shared_ptr& │ │ │ │ │ +p, │ │ │ │ │ +95 const _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s& biasHat = _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s()) : │ │ │ │ │ +96 _P_r_e_i_n_t_e_g_r_a_t_i_o_n_T_y_p_e(p, biasHat) { │ │ │ │ │ +97 preintMeasCov_.setZero(); │ │ │ │ │ +98 } │ │ │ │ │ +99 │ │ │ │ │ +_1_0_5 _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s(const _P_r_e_i_n_t_e_g_r_a_t_i_o_n_T_y_p_e& base, const Matrix9& │ │ │ │ │ +preintMeasCov) │ │ │ │ │ +106 : _P_r_e_i_n_t_e_g_r_a_t_i_o_n_T_y_p_e(base), │ │ │ │ │ +107 preintMeasCov_(preintMeasCov) { │ │ │ │ │ +108 } │ │ │ │ │ +109 │ │ │ │ │ +_1_1_1 _~_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s() override { │ │ │ │ │ +112 } │ │ │ │ │ +113 │ │ │ │ │ +115 void _p_r_i_n_t(const std::string& s = "Preintegrated Measurements:") const │ │ │ │ │ override; │ │ │ │ │ -93 │ │ │ │ │ -94 void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; } │ │ │ │ │ -95 void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; } │ │ │ │ │ -96 void setBiasAccOmegaInit(const Matrix6& cov) { biasAccOmegaInt=cov; } │ │ │ │ │ -97 │ │ │ │ │ -98 const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; } │ │ │ │ │ -99 const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; │ │ │ │ │ -} │ │ │ │ │ -100 const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; } │ │ │ │ │ -101 │ │ │ │ │ -102private: │ │ │ │ │ -103 │ │ │ │ │ -_1_0_5 friend class boost::serialization::access; │ │ │ │ │ -106 template │ │ │ │ │ -107 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ -108 namespace bs = ::boost::serialization; │ │ │ │ │ -109 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s); │ │ │ │ │ -110 ar & BOOST_SERIALIZATION_NVP(biasAccCovariance); │ │ │ │ │ -111 ar & BOOST_SERIALIZATION_NVP(biasOmegaCovariance); │ │ │ │ │ -112 ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInt); │ │ │ │ │ -113 } │ │ │ │ │ -114 │ │ │ │ │ -115public: │ │ │ │ │ -116 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -117}; │ │ │ │ │ -118 │ │ │ │ │ -_1_2_9class GTSAM_EXPORT _P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s : public │ │ │ │ │ -_P_r_e_i_n_t_e_g_r_a_t_i_o_n_T_y_p_e { │ │ │ │ │ -130 │ │ │ │ │ -131public: │ │ │ │ │ -132 typedef _P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s _P_a_r_a_m_s; │ │ │ │ │ -133 │ │ │ │ │ -134 protected: │ │ │ │ │ -135 /* Covariance matrix of the preintegrated measurements │ │ │ │ │ -136 * COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY BiasAcc │ │ │ │ │ -BiasOmega] │ │ │ │ │ -137 * (first-order propagation from *measurementCovariance*). │ │ │ │ │ -138 * PreintegratedCombinedMeasurements also include the biases and keep the │ │ │ │ │ -correlation │ │ │ │ │ -139 * between the preintegrated measurements and the biases │ │ │ │ │ -140 */ │ │ │ │ │ -141 Eigen::Matrix preintMeasCov_; │ │ │ │ │ +116 │ │ │ │ │ +118 bool _e_q_u_a_l_s(const _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s& expected, double tol = 1e- │ │ │ │ │ +9) const; │ │ │ │ │ +119 │ │ │ │ │ +121 void resetIntegration() override; │ │ │ │ │ +122 │ │ │ │ │ +133 void integrateMeasurement(const Vector3& measuredAcc, │ │ │ │ │ +134 const Vector3& measuredOmega, const double dt) override; │ │ │ │ │ +135 │ │ │ │ │ +137 void integrateMeasurements(const Matrix& measuredAccs, const Matrix& │ │ │ │ │ +measuredOmegas, │ │ │ │ │ +138 const Matrix& dts); │ │ │ │ │ +139 │ │ │ │ │ +_1_4_1 Matrix _p_r_e_i_n_t_M_e_a_s_C_o_v() const { return preintMeasCov_; } │ │ │ │ │ 142 │ │ │ │ │ -143 friend class _C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r; │ │ │ │ │ -144 │ │ │ │ │ -145 public: │ │ │ │ │ -148 │ │ │ │ │ -_1_5_0 _P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s() { │ │ │ │ │ -151 preintMeasCov_.setZero(); │ │ │ │ │ -152 } │ │ │ │ │ -153 │ │ │ │ │ -_1_5_9 _P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s( │ │ │ │ │ -160 const boost::shared_ptr& p, │ │ │ │ │ -161 const _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s& biasHat = _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s()) │ │ │ │ │ -162 : _P_r_e_i_n_t_e_g_r_a_t_i_o_n_T_y_p_e(p, biasHat) { │ │ │ │ │ -163 preintMeasCov_.setZero(); │ │ │ │ │ -164 } │ │ │ │ │ -165 │ │ │ │ │ -_1_7_1 _P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s(const _P_r_e_i_n_t_e_g_r_a_t_i_o_n_T_y_p_e& base, const │ │ │ │ │ -Eigen::Matrix& preintMeasCov) │ │ │ │ │ -172 : _P_r_e_i_n_t_e_g_r_a_t_i_o_n_T_y_p_e(base), │ │ │ │ │ -173 preintMeasCov_(preintMeasCov) { │ │ │ │ │ -174 } │ │ │ │ │ -175 │ │ │ │ │ -_1_7_7 _~_P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s() override {} │ │ │ │ │ +143#ifdef GTSAM_TANGENT_PREINTEGRATION │ │ │ │ │ +145 void mergeWith(const _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s& pim, Matrix9* H1, │ │ │ │ │ +Matrix9* H2); │ │ │ │ │ +146#endif │ │ │ │ │ +147 │ │ │ │ │ +148 private: │ │ │ │ │ +_1_5_0 friend class boost::serialization::access; │ │ │ │ │ +151 template │ │ │ │ │ +152 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +153 namespace bs = ::boost::serialization; │ │ │ │ │ +154 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_P_r_e_i_n_t_e_g_r_a_t_i_o_n_T_y_p_e); │ │ │ │ │ +155 ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); │ │ │ │ │ +156 } │ │ │ │ │ +157}; │ │ │ │ │ +158 │ │ │ │ │ +_1_7_1class GTSAM_EXPORT _I_m_u_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +173private: │ │ │ │ │ +174 │ │ │ │ │ +175 typedef _I_m_u_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ +176 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N<_P_o_s_e_3, Vector3, _P_o_s_e_3, Vector3, │ │ │ │ │ +177 _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s> _B_a_s_e; │ │ │ │ │ 178 │ │ │ │ │ +179 _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s _PIM_; │ │ │ │ │ 180 │ │ │ │ │ -183 │ │ │ │ │ -185 void resetIntegration() override; │ │ │ │ │ -186 │ │ │ │ │ -_1_8_8 _P_a_r_a_m_s& _p() const { return *boost::static_pointer_cast(this->p_); } │ │ │ │ │ -190 │ │ │ │ │ -194 Matrix preintMeasCov() const { return preintMeasCov_; } │ │ │ │ │ -196 │ │ │ │ │ -200 void print(const std::string& s = "Preintegrated Measurements:") const │ │ │ │ │ -override; │ │ │ │ │ -202 bool equals(const PreintegratedCombinedMeasurements& expected, │ │ │ │ │ -203 double tol = 1e-9) const; │ │ │ │ │ +181public: │ │ │ │ │ +182 │ │ │ │ │ +184#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 │ │ │ │ │ +185 typedef typename boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +186#else │ │ │ │ │ +_1_8_7 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +188#endif │ │ │ │ │ +189 │ │ │ │ │ +_1_9_1 _I_m_u_F_a_c_t_o_r() {} │ │ │ │ │ +192 │ │ │ │ │ +203 _I_m_u_F_a_c_t_o_r(_K_e_y pose_i, _K_e_y vel_i, _K_e_y pose_j, _K_e_y vel_j, _K_e_y bias, │ │ │ │ │ +204 const _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s& preintegratedMeasurements); │ │ │ │ │ 205 │ │ │ │ │ -206 │ │ │ │ │ -209 │ │ │ │ │ -220 void integrateMeasurement(const Vector3& measuredAcc, │ │ │ │ │ -221 const Vector3& measuredOmega, const double dt) override; │ │ │ │ │ -222 │ │ │ │ │ -224 │ │ │ │ │ -225 private: │ │ │ │ │ -_2_2_7 friend class boost::serialization::access; │ │ │ │ │ -228 template │ │ │ │ │ -229 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ -230 namespace bs = ::boost::serialization; │ │ │ │ │ -231 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(_P_r_e_i_n_t_e_g_r_a_t_i_o_n_T_y_p_e); │ │ │ │ │ -232 ar& BOOST_SERIALIZATION_NVP(preintMeasCov_); │ │ │ │ │ -233 } │ │ │ │ │ -234 │ │ │ │ │ -235public: │ │ │ │ │ -236 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -237}; │ │ │ │ │ -238 │ │ │ │ │ -_2_5_8class GTSAM_EXPORT _C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -260public: │ │ │ │ │ -261 │ │ │ │ │ -262private: │ │ │ │ │ -263 │ │ │ │ │ -264 typedef _C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ -265 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N<_P_o_s_e_3, Vector3, _P_o_s_e_3, Vector3, │ │ │ │ │ -266 _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s, _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s> _B_a_s_e; │ │ │ │ │ -267 │ │ │ │ │ -268 _P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s _PIM_; │ │ │ │ │ -269 │ │ │ │ │ -270public: │ │ │ │ │ -271 │ │ │ │ │ -273#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 │ │ │ │ │ -274 typedef typename boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -275#else │ │ │ │ │ -_2_7_6 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -277#endif │ │ │ │ │ -278 │ │ │ │ │ -_2_8_0 _C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r() {} │ │ │ │ │ -281 │ │ │ │ │ -292 _C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r( │ │ │ │ │ -293 _K_e_y pose_i, _K_e_y vel_i, _K_e_y pose_j, _K_e_y vel_j, _K_e_y bias_i, _K_e_y bias_j, │ │ │ │ │ -294 const _P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s& preintegratedMeasurements); │ │ │ │ │ -295 │ │ │ │ │ -296 _~_C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r() override {} │ │ │ │ │ -297 │ │ │ │ │ -299 gtsam::NonlinearFactor::shared_ptr clone() const override; │ │ │ │ │ -300 │ │ │ │ │ -305 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, │ │ │ │ │ -306 const CombinedImuFactor&); │ │ │ │ │ -308 void print(const std::string& s = "", const KeyFormatter& keyFormatter = │ │ │ │ │ -309 DefaultKeyFormatter) const override; │ │ │ │ │ -310 │ │ │ │ │ -312 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const │ │ │ │ │ +206 _~_I_m_u_F_a_c_t_o_r() override { │ │ │ │ │ +207 } │ │ │ │ │ +208 │ │ │ │ │ +210 gtsam::NonlinearFactor::shared_ptr clone() const override; │ │ │ │ │ +211 │ │ │ │ │ +214 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const │ │ │ │ │ +ImuFactor&); │ │ │ │ │ +215 void print(const std::string& s = "", const KeyFormatter& keyFormatter = │ │ │ │ │ +216 DefaultKeyFormatter) const override; │ │ │ │ │ +217 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const │ │ │ │ │ +override; │ │ │ │ │ +219 │ │ │ │ │ +_2_2_2 const _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s& _p_r_e_i_n_t_e_g_r_a_t_e_d_M_e_a_s_u_r_e_m_e_n_t_s() const { │ │ │ │ │ +223 return _PIM_; │ │ │ │ │ +224 } │ │ │ │ │ +225 │ │ │ │ │ +229 Vector evaluateError(const _P_o_s_e_3& pose_i, const Vector3& vel_i, │ │ │ │ │ +230 const _P_o_s_e_3& pose_j, const Vector3& vel_j, │ │ │ │ │ +231 const _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s& bias_i, boost::optional H1 = │ │ │ │ │ +232 boost::none, boost::optional H2 = boost::none, │ │ │ │ │ +233 boost::optional H3 = boost::none, boost::optional H4 = │ │ │ │ │ +234 boost::none, boost::optional H5 = boost::none) const override; │ │ │ │ │ +235 │ │ │ │ │ +236#ifdef GTSAM_TANGENT_PREINTEGRATION │ │ │ │ │ +238 static _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s Merge( │ │ │ │ │ +239 const _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s& pim01, │ │ │ │ │ +240 const _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s& pim12); │ │ │ │ │ +241 │ │ │ │ │ +243 static shared_ptr Merge(const shared_ptr& f01, const shared_ptr& f12); │ │ │ │ │ +244#endif │ │ │ │ │ +245 │ │ │ │ │ +246 private: │ │ │ │ │ +_2_4_8 friend class boost::serialization::access; │ │ │ │ │ +249 template │ │ │ │ │ +250 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +251 // NoiseModelFactor5 instead of NoiseModelFactorN for backward │ │ │ │ │ +compatibility │ │ │ │ │ +252 ar & boost::serialization::make_nvp("NoiseModelFactor5", │ │ │ │ │ +253 boost::serialization::base_object(*this)); │ │ │ │ │ +254 ar & BOOST_SERIALIZATION_NVP(_PIM_); │ │ │ │ │ +255 } │ │ │ │ │ +256}; │ │ │ │ │ +257// class ImuFactor │ │ │ │ │ +258 │ │ │ │ │ +_2_6_3class GTSAM_EXPORT _I_m_u_F_a_c_t_o_r_2 : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +264private: │ │ │ │ │ +265 │ │ │ │ │ +266 typedef _I_m_u_F_a_c_t_o_r_2 _T_h_i_s; │ │ │ │ │ +267 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_N_a_v_S_t_a_t_e_,_ _N_a_v_S_t_a_t_e_,_ _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_> _B_a_s_e; │ │ │ │ │ +268 │ │ │ │ │ +269 _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s _PIM_; │ │ │ │ │ +270 │ │ │ │ │ +271public: │ │ │ │ │ +272 │ │ │ │ │ +_2_7_4 _I_m_u_F_a_c_t_o_r_2() {} │ │ │ │ │ +275 │ │ │ │ │ +282 _I_m_u_F_a_c_t_o_r_2(_K_e_y state_i, _K_e_y state_j, _K_e_y bias, │ │ │ │ │ +283 const _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s& preintegratedMeasurements); │ │ │ │ │ +284 │ │ │ │ │ +285 _~_I_m_u_F_a_c_t_o_r_2() override { │ │ │ │ │ +286 } │ │ │ │ │ +287 │ │ │ │ │ +289 gtsam::NonlinearFactor::shared_ptr clone() const override; │ │ │ │ │ +290 │ │ │ │ │ +293 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const │ │ │ │ │ +ImuFactor2&); │ │ │ │ │ +294 void print(const std::string& s = "", const KeyFormatter& keyFormatter = │ │ │ │ │ +295 DefaultKeyFormatter) const override; │ │ │ │ │ +296 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const │ │ │ │ │ override; │ │ │ │ │ -314 │ │ │ │ │ -_3_1_7 const _P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s& _p_r_e_i_n_t_e_g_r_a_t_e_d_M_e_a_s_u_r_e_m_e_n_t_s() const │ │ │ │ │ -{ │ │ │ │ │ -318 return _PIM_; │ │ │ │ │ -319 } │ │ │ │ │ -320 │ │ │ │ │ -324 Vector evaluateError(const _P_o_s_e_3& pose_i, const Vector3& vel_i, │ │ │ │ │ -325 const _P_o_s_e_3& pose_j, const Vector3& vel_j, │ │ │ │ │ -326 const _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s& bias_i, const _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s& bias_j, │ │ │ │ │ -327 boost::optional H1 = boost::none, boost::optional H2 = │ │ │ │ │ -328 boost::none, boost::optional H3 = boost::none, │ │ │ │ │ -329 boost::optional H4 = boost::none, boost::optional H5 = │ │ │ │ │ -330 boost::none, boost::optional H6 = boost::none) const override; │ │ │ │ │ -331 │ │ │ │ │ -332 private: │ │ │ │ │ -_3_3_4 friend class boost::serialization::access; │ │ │ │ │ -335 template │ │ │ │ │ -336 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ -337 // NoiseModelFactor6 instead of NoiseModelFactorN for backward │ │ │ │ │ +298 │ │ │ │ │ +_3_0_1 const _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s& _p_r_e_i_n_t_e_g_r_a_t_e_d_M_e_a_s_u_r_e_m_e_n_t_s() const { │ │ │ │ │ +302 return _PIM_; │ │ │ │ │ +303 } │ │ │ │ │ +304 │ │ │ │ │ +308 Vector evaluateError(const _N_a_v_S_t_a_t_e& state_i, const _N_a_v_S_t_a_t_e& state_j, │ │ │ │ │ +309 const _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s& bias_i, // │ │ │ │ │ +310 boost::optional H1 = boost::none, │ │ │ │ │ +311 boost::optional H2 = boost::none, │ │ │ │ │ +312 boost::optional H3 = boost::none) const override; │ │ │ │ │ +313 │ │ │ │ │ +314private: │ │ │ │ │ +315 │ │ │ │ │ +_3_1_7 friend class boost::serialization::access; │ │ │ │ │ +318 template │ │ │ │ │ +319 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +320 // NoiseModelFactor3 instead of NoiseModelFactorN for backward │ │ │ │ │ compatibility │ │ │ │ │ -338 ar& boost::serialization::make_nvp( │ │ │ │ │ -339 "NoiseModelFactor6", boost::serialization::base_object(*this)); │ │ │ │ │ -340 ar& BOOST_SERIALIZATION_NVP(_PIM_); │ │ │ │ │ -341 } │ │ │ │ │ -342 │ │ │ │ │ -343public: │ │ │ │ │ -344 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -345}; │ │ │ │ │ -346// class CombinedImuFactor │ │ │ │ │ -347 │ │ │ │ │ -348template <> │ │ │ │ │ -_3_4_9struct _t_r_a_i_t_s<_P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s> │ │ │ │ │ -350 : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ -351 │ │ │ │ │ -352template <> │ │ │ │ │ -_3_5_3struct _t_r_a_i_t_s<_P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s> │ │ │ │ │ -354 : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ -355 │ │ │ │ │ -356template <> │ │ │ │ │ -_3_5_7struct _t_r_a_i_t_s<_C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ -358 │ │ │ │ │ -359} // namespace gtsam │ │ │ │ │ -_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ -This marks a GTSAM object to require alignment. │ │ │ │ │ -DDeeffiinniittiioonn types.h:308 │ │ │ │ │ -_s_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ -Convenience functions for serializing data structures via boost.serialization. │ │ │ │ │ -_M_a_t_r_i_x_._h │ │ │ │ │ -typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +321 ar & boost::serialization::make_nvp("NoiseModelFactor3", │ │ │ │ │ +322 boost::serialization::base_object(*this)); │ │ │ │ │ +323 ar & BOOST_SERIALIZATION_NVP(_PIM_); │ │ │ │ │ +324 } │ │ │ │ │ +325}; │ │ │ │ │ +326// class ImuFactor2 │ │ │ │ │ +327 │ │ │ │ │ +328template <> │ │ │ │ │ +_3_2_9struct _t_r_a_i_t_s<_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s> : public │ │ │ │ │ +_T_e_s_t_a_b_l_e {}; │ │ │ │ │ +330 │ │ │ │ │ +331template <> │ │ │ │ │ +_3_3_2struct _t_r_a_i_t_s<_I_m_u_F_a_c_t_o_r> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ +333 │ │ │ │ │ +334template <> │ │ │ │ │ +_3_3_5struct _t_r_a_i_t_s<_I_m_u_F_a_c_t_o_r_2> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ +336 │ │ │ │ │ +337} │ │ │ │ │ +_d_e_b_u_g_._h │ │ │ │ │ +Global debugging flags. │ │ │ │ │ _T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n_._h │ │ │ │ │ _M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_._h │ │ │ │ │ _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ @@ -294,96 +272,90 @@ │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ _g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ A helper that implements the traits interface for GTSAM types. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ _g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s │ │ │ │ │ -Parameters for pre-integration using PreintegratedCombinedMeasurements: Usage: │ │ │ │ │ -Create just a single P... │ │ │ │ │ -DDeeffiinniittiioonn CombinedImuFactor.h:62 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s_:_:_b_i_a_s_A_c_c_O_m_e_g_a_I_n_t │ │ │ │ │ -Matrix6 biasAccOmegaInt │ │ │ │ │ -covariance of bias used as initial estimate. │ │ │ │ │ -DDeeffiinniittiioonn CombinedImuFactor.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s │ │ │ │ │ -PreintegrationCombinedParams(const Vector3 &n_gravity) │ │ │ │ │ -See two named constructors below for good values of n_gravity in body frame. │ │ │ │ │ -DDeeffiinniittiioonn CombinedImuFactor.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s │ │ │ │ │ -PreintegrationCombinedParams() │ │ │ │ │ -Default constructor makes uninitialized params struct. │ │ │ │ │ -DDeeffiinniittiioonn CombinedImuFactor.h:69 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s_:_:_b_i_a_s_O_m_e_g_a_C_o_v_a_r_i_a_n_c_e │ │ │ │ │ -Matrix3 biasOmegaCovariance │ │ │ │ │ -continuous-time "Covariance" describing gyroscope bias random walk │ │ │ │ │ -DDeeffiinniittiioonn CombinedImuFactor.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s_:_:_b_i_a_s_A_c_c_C_o_v_a_r_i_a_n_c_e │ │ │ │ │ -Matrix3 biasAccCovariance │ │ │ │ │ -continuous-time "Covariance" describing accelerometer bias random walk │ │ │ │ │ -DDeeffiinniittiioonn CombinedImuFactor.h:63 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ -PreintegratedCombinedMeasurements integrates the IMU measurements (rotation │ │ │ │ │ -rates and accelerations) ... │ │ │ │ │ -DDeeffiinniittiioonn CombinedImuFactor.h:129 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ -PreintegratedCombinedMeasurements(const PreintegrationType &base, const Eigen:: │ │ │ │ │ -Matrix< double, 15, 15 > &preintMeasCov) │ │ │ │ │ -Construct preintegrated directly from members: base class and preintMeasCov. │ │ │ │ │ -DDeeffiinniittiioonn CombinedImuFactor.h:171 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s_:_:_~_P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ -~PreintegratedCombinedMeasurements() override │ │ │ │ │ +_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s │ │ │ │ │ +DDeeffiinniittiioonn ImuBias.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ +PreintegratedImuMeasurements accumulates (integrates) the IMU measurements │ │ │ │ │ +(rotation rates and accele... │ │ │ │ │ +DDeeffiinniittiioonn ImuFactor.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s_:_:_~_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ +~PreintegratedImuMeasurements() override │ │ │ │ │ Virtual destructor. │ │ │ │ │ -DDeeffiinniittiioonn CombinedImuFactor.h:177 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ -PreintegratedCombinedMeasurements() │ │ │ │ │ -Default constructor only for serialization and wrappers. │ │ │ │ │ -DDeeffiinniittiioonn CombinedImuFactor.h:150 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s_:_:_p │ │ │ │ │ -Params & p() const │ │ │ │ │ -const reference to params, shadows definition in base class │ │ │ │ │ -DDeeffiinniittiioonn CombinedImuFactor.h:188 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ -PreintegratedCombinedMeasurements(const boost::shared_ptr< Params > &p, const │ │ │ │ │ -imuBias::ConstantBias &biasHat=imuBias::ConstantBias()) │ │ │ │ │ -Default constructor, initializes the class with no measurements. │ │ │ │ │ -DDeeffiinniittiioonn CombinedImuFactor.h:159 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r │ │ │ │ │ -CombinedImuFactor is a 6-ways factor involving previous state (pose and │ │ │ │ │ -velocity of the vehicle,... │ │ │ │ │ -DDeeffiinniittiioonn CombinedImuFactor.h:259 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< CombinedImuFactor > shared_ptr │ │ │ │ │ +DDeeffiinniittiioonn ImuFactor.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ +PreintegratedImuMeasurements(const boost::shared_ptr< PreintegrationParams > │ │ │ │ │ +&p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias()) │ │ │ │ │ +Constructor, initializes the class with no measurements. │ │ │ │ │ +DDeeffiinniittiioonn ImuFactor.h:94 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ +PreintegratedImuMeasurements(const PreintegrationType &base, const Matrix9 │ │ │ │ │ +&preintMeasCov) │ │ │ │ │ +Construct preintegrated directly from members: base class and preintMeasCov. │ │ │ │ │ +DDeeffiinniittiioonn ImuFactor.h:105 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ +PreintegratedImuMeasurements() │ │ │ │ │ +Default constructor for serialization and wrappers. │ │ │ │ │ +DDeeffiinniittiioonn ImuFactor.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s_:_:_p_r_e_i_n_t_M_e_a_s_C_o_v │ │ │ │ │ +Matrix preintMeasCov() const │ │ │ │ │ +Return pre-integrated measurement covariance. │ │ │ │ │ +DDeeffiinniittiioonn ImuFactor.h:141 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s_:_:_p_r_e_i_n_t_M_e_a_s_C_o_v__ │ │ │ │ │ +Matrix9 preintMeasCov_ │ │ │ │ │ +COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY]. │ │ │ │ │ +DDeeffiinniittiioonn ImuFactor.h:79 │ │ │ │ │ +_g_t_s_a_m_:_:_I_m_u_F_a_c_t_o_r │ │ │ │ │ +ImuFactor is a 5-ways factor involving previous state (pose and velocity of the │ │ │ │ │ +vehicle at previous t... │ │ │ │ │ +DDeeffiinniittiioonn ImuFactor.h:172 │ │ │ │ │ +_g_t_s_a_m_:_:_I_m_u_F_a_c_t_o_r_:_:_p_r_e_i_n_t_e_g_r_a_t_e_d_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ +const PreintegratedImuMeasurements & preintegratedMeasurements() const │ │ │ │ │ +Access the preintegrated measurements. │ │ │ │ │ +DDeeffiinniittiioonn ImuFactor.h:222 │ │ │ │ │ +_g_t_s_a_m_:_:_I_m_u_F_a_c_t_o_r_:_:_I_m_u_F_a_c_t_o_r │ │ │ │ │ +ImuFactor() │ │ │ │ │ +Default constructor - only use for serialization. │ │ │ │ │ +DDeeffiinniittiioonn ImuFactor.h:191 │ │ │ │ │ +_g_t_s_a_m_:_:_I_m_u_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< ImuFactor > shared_ptr │ │ │ │ │ Shorthand for a smart pointer to a factor. │ │ │ │ │ -DDeeffiinniittiioonn CombinedImuFactor.h:276 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r_:_:_C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r │ │ │ │ │ -CombinedImuFactor() │ │ │ │ │ +DDeeffiinniittiioonn ImuFactor.h:187 │ │ │ │ │ +_g_t_s_a_m_:_:_I_m_u_F_a_c_t_o_r_2 │ │ │ │ │ +ImuFactor2 is a ternary factor that uses NavStates rather than Pose/Velocity. │ │ │ │ │ +DDeeffiinniittiioonn ImuFactor.h:263 │ │ │ │ │ +_g_t_s_a_m_:_:_I_m_u_F_a_c_t_o_r_2_:_:_I_m_u_F_a_c_t_o_r_2 │ │ │ │ │ +ImuFactor2() │ │ │ │ │ Default constructor - only use for serialization. │ │ │ │ │ -DDeeffiinniittiioonn CombinedImuFactor.h:280 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r_:_:_p_r_e_i_n_t_e_g_r_a_t_e_d_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ -const PreintegratedCombinedMeasurements & preintegratedMeasurements() const │ │ │ │ │ +DDeeffiinniittiioonn ImuFactor.h:274 │ │ │ │ │ +_g_t_s_a_m_:_:_I_m_u_F_a_c_t_o_r_2_:_:_p_r_e_i_n_t_e_g_r_a_t_e_d_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ +const PreintegratedImuMeasurements & preintegratedMeasurements() const │ │ │ │ │ Access the preintegrated measurements. │ │ │ │ │ -DDeeffiinniittiioonn CombinedImuFactor.h:317 │ │ │ │ │ -_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s │ │ │ │ │ -DDeeffiinniittiioonn ImuBias.h:30 │ │ │ │ │ +DDeeffiinniittiioonn ImuFactor.h:301 │ │ │ │ │ _g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n │ │ │ │ │ IMU pre-integration on NavSatet manifold. │ │ │ │ │ DDeeffiinniittiioonn ManifoldPreintegration.h:33 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ -Parameters for pre-integration: Usage: Create just a single Params and pass a │ │ │ │ │ -shared pointer to the c... │ │ │ │ │ -DDeeffiinniittiioonn PreintegrationParams.h:26 │ │ │ │ │ +_g_t_s_a_m_:_:_N_a_v_S_t_a_t_e │ │ │ │ │ +Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does │ │ │ │ │ +not make sense to make... │ │ │ │ │ +DDeeffiinniittiioonn NavState.h:34 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ variables. │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r_._h │ │ │ │ │ + * _I_m_u_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00941.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AHRSFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/CombinedImuFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,47 +94,87 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
AHRSFactor.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Macros | │ │ │ │ +Functions
│ │ │ │ +
CombinedImuFactor.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::PreintegratedAhrsMeasurements
 PreintegratedAHRSMeasurements accumulates (integrates) the Gyroscope measurements (rotation rates) and the corresponding covariance matrix. More...
 
class  gtsam::AHRSFactor
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Macros

│ │ │ │ +#define D_R_R(H)   (H)->block<3,3>(0,0)
 
│ │ │ │ +#define D_R_t(H)   (H)->block<3,3>(0,3)
 
│ │ │ │ +#define D_R_v(H)   (H)->block<3,3>(0,6)
 
│ │ │ │ +#define D_t_R(H)   (H)->block<3,3>(3,0)
 
│ │ │ │ +#define D_t_t(H)   (H)->block<3,3>(3,3)
 
│ │ │ │ +#define D_t_v(H)   (H)->block<3,3>(3,6)
 
│ │ │ │ +#define D_v_R(H)   (H)->block<3,3>(6,0)
 
│ │ │ │ +#define D_v_t(H)   (H)->block<3,3>(6,3)
 
│ │ │ │ +#define D_v_v(H)   (H)->block<3,3>(6,6)
 
│ │ │ │ +#define D_a_a(H)   (H)->block<3,3>(9,9)
 
│ │ │ │ +#define D_g_g(H)   (H)->block<3,3>(12,12)
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +std::ostream & gtsam::operator<< (std::ostream &os, const CombinedImuFactor &f)
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Krunal Chande
│ │ │ │ +
Author
Luca Carlone
│ │ │ │ +
│ │ │ │ +Stephen Williams
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │ +
│ │ │ │ +Vadim Indelman
│ │ │ │ +
│ │ │ │ +David Jensen
│ │ │ │
│ │ │ │ -Luca Carlone
│ │ │ │ +Frank Dellaert │ │ │ │
│ │ │ │ -Frank Dellaert
│ │ │ │ -
Date
July 2014
│ │ │ │ +Varun Agrawal
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,31 +1,51 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -AHRSFactor.h File Reference │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ - PreintegratedAHRSMeasurements accumulates (integrates) the Gyroscope │ │ │ │ │ -  measurements (rotation rates) and the corresponding covariance matrix. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_A_H_R_S_F_a_c_t_o_r │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +CombinedImuFactor.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +MMaaccrrooss │ │ │ │ │ +#define  DD__RR__RR(H)   (H)->block<3,3>(0,0) │ │ │ │ │ +  │ │ │ │ │ +#define  DD__RR__tt(H)   (H)->block<3,3>(0,3) │ │ │ │ │ +  │ │ │ │ │ +#define  DD__RR__vv(H)   (H)->block<3,3>(0,6) │ │ │ │ │ +  │ │ │ │ │ +#define  DD__tt__RR(H)   (H)->block<3,3>(3,0) │ │ │ │ │ +  │ │ │ │ │ +#define  DD__tt__tt(H)   (H)->block<3,3>(3,3) │ │ │ │ │ +  │ │ │ │ │ +#define  DD__tt__vv(H)   (H)->block<3,3>(3,6) │ │ │ │ │ +  │ │ │ │ │ +#define  DD__vv__RR(H)   (H)->block<3,3>(6,0) │ │ │ │ │ +  │ │ │ │ │ +#define  DD__vv__tt(H)   (H)->block<3,3>(6,3) │ │ │ │ │ +  │ │ │ │ │ +#define  DD__vv__vv(H)   (H)->block<3,3>(6,6) │ │ │ │ │ +  │ │ │ │ │ +#define  DD__aa__aa(H)   (H)->block<3,3>(9,9) │ │ │ │ │ +  │ │ │ │ │ +#define  DD__gg__gg(H)   (H)->block<3,3>(12,12) │ │ │ │ │ +  │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r │ │ │ │ │ + &f) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Author │ │ │ │ │ - Krunal Chande │ │ │ │ │ Luca Carlone │ │ │ │ │ + Stephen Williams │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Vadim Indelman │ │ │ │ │ + David Jensen │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - July 2014 │ │ │ │ │ + Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _A_H_R_S_F_a_c_t_o_r_._h │ │ │ │ │ + * _C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00944.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ImuBias.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/PreintegratedRotation.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,45 +94,41 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
ImuBias.h File Reference
│ │ │ │ +
PreintegratedRotation.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::imuBias::ConstantBias
 
struct  gtsam::traits< imuBias::ConstantBias >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
namespace  gtsam::imuBias
 All bias models live in the imuBias namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Feb 2, 2012
│ │ │ │ -
Author
Vadim Indelman, Stephen Williams
│ │ │ │ +
Author
Luca Carlone
│ │ │ │ +
│ │ │ │ +Stephen Williams
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │ +
│ │ │ │ +Vadim Indelman
│ │ │ │ +
│ │ │ │ +David Jensen
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,29 +1,23 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -ImuBias.h File Reference │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_ _> │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +PreintegratedRotation.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -namespace   _g_t_s_a_m_:_:_i_m_u_B_i_a_s │ │ │ │ │ -  All bias models live in the _i_m_u_B_i_a_s namespace. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Feb 2, 2012 │ │ │ │ │ Author │ │ │ │ │ - Vadim Indelman, Stephen Williams │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Stephen Williams │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Vadim Indelman │ │ │ │ │ + David Jensen │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _I_m_u_B_i_a_s_._h │ │ │ │ │ + * _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00947_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/MagPoseFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/Scenario.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,167 +98,152 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
MagPoseFactor.h
│ │ │ │ +
Scenario.h
│ │ │ │
│ │ │ │
│ │ │ │ -
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
12#pragma once
│ │ │ │ -
13
│ │ │ │ -
14#include <gtsam/geometry/concepts.h>
│ │ │ │ -
15#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
16
│ │ │ │ -
17namespace gtsam {
│ │ │ │ -
18
│ │ │ │ -
27template <class POSE>
│ │ │ │ -
│ │ │ │ -
28class MagPoseFactor: public NoiseModelFactorN<POSE> {
│ │ │ │ -
29 private:
│ │ │ │ -
30 using This = MagPoseFactor<POSE>;
│ │ │ │ -
31 using Base = NoiseModelFactorN<POSE>;
│ │ │ │ -
32 using Point = typename POSE::Translation;
│ │ │ │ -
33 using Rot = typename POSE::Rotation;
│ │ │ │ -
34
│ │ │ │ -
35 const Point measured_;
│ │ │ │ -
36 const Point nM_;
│ │ │ │ -
37 const Point bias_;
│ │ │ │ -
38 boost::optional<POSE> body_P_sensor_;
│ │ │ │ -
39
│ │ │ │ -
40 static const int MeasDim = Point::RowsAtCompileTime;
│ │ │ │ -
41 static const int PoseDim = traits<POSE>::dimension;
│ │ │ │ -
42 static const int RotDim = traits<Rot>::dimension;
│ │ │ │ -
43
│ │ │ │ -
45 using shared_ptr = boost::shared_ptr<MagPoseFactor<POSE>>;
│ │ │ │ +
18#pragma once
│ │ │ │ +
19#include <gtsam/linear/NoiseModel.h>
│ │ │ │ +
20#include <gtsam/navigation/NavState.h>
│ │ │ │ +
21
│ │ │ │ +
22namespace gtsam {
│ │ │ │ +
23
│ │ │ │ +
│ │ │ │ +
25class Scenario {
│ │ │ │ +
26 public:
│ │ │ │ +
28 virtual ~Scenario() {}
│ │ │ │ +
29
│ │ │ │ +
30 // Quantities a Scenario needs to specify:
│ │ │ │ +
31
│ │ │ │ +
32 virtual Pose3 pose(double t) const = 0;
│ │ │ │ +
33 virtual Vector3 omega_b(double t) const = 0;
│ │ │ │ +
34 virtual Vector3 velocity_n(double t) const = 0;
│ │ │ │ +
35 virtual Vector3 acceleration_n(double t) const = 0;
│ │ │ │ +
36
│ │ │ │ +
37 // Derived quantities:
│ │ │ │ +
38
│ │ │ │ +
39 Rot3 rotation(double t) const { return pose(t).rotation(); }
│ │ │ │ +
40 NavState navState(double t) const { return NavState(pose(t), velocity_n(t)); }
│ │ │ │ +
41
│ │ │ │ +
42 Vector3 velocity_b(double t) const {
│ │ │ │ +
43 const Rot3 nRb = rotation(t);
│ │ │ │ +
44 return nRb.transpose() * velocity_n(t);
│ │ │ │ +
45 }
│ │ │ │
46
│ │ │ │ -
48 GTSAM_CONCEPT_TESTABLE_TYPE(POSE)
│ │ │ │ -
49 GTSAM_CONCEPT_POSE_TYPE(POSE)
│ │ │ │ -
50
│ │ │ │ -
51 public:
│ │ │ │ -
52 ~MagPoseFactor() override {}
│ │ │ │ -
53
│ │ │ │ -
55 MagPoseFactor() {}
│ │ │ │ -
56
│ │ │ │ +
47 Vector3 acceleration_b(double t) const {
│ │ │ │ +
48 const Rot3 nRb = rotation(t);
│ │ │ │ +
49 return nRb.transpose() * acceleration_n(t);
│ │ │ │ +
50 }
│ │ │ │ +
51};
│ │ │ │ +
│ │ │ │ +
52
│ │ │ │ +
│ │ │ │ +
60class ConstantTwistScenario : public Scenario {
│ │ │ │ +
61 public:
│ │ │ │ +
│ │ │ │ +
63 ConstantTwistScenario(const Vector3& w, const Vector3& v,
│ │ │ │ +
64 const Pose3& nTb0 = Pose3())
│ │ │ │ +
65 : twist_((Vector6() << w, v).finished()), a_b_(w.cross(v)), nTb0_(nTb0) {}
│ │ │ │ +
│ │ │ │ +
66
│ │ │ │
│ │ │ │ -
67 MagPoseFactor(Key pose_key,
│ │ │ │ -
68 const Point& measured,
│ │ │ │ -
69 double scale,
│ │ │ │ -
70 const Point& direction,
│ │ │ │ -
71 const Point& bias,
│ │ │ │ -
72 const SharedNoiseModel& model,
│ │ │ │ -
73 const boost::optional<POSE>& body_P_sensor)
│ │ │ │ -
74 : Base(model, pose_key),
│ │ │ │ -
75 measured_(body_P_sensor ? body_P_sensor->rotation() * measured : measured),
│ │ │ │ -
76 nM_(scale * direction.normalized()),
│ │ │ │ -
77 bias_(body_P_sensor ? body_P_sensor->rotation() * bias : bias),
│ │ │ │ -
78 body_P_sensor_(body_P_sensor) {}
│ │ │ │ -
│ │ │ │ -
79
│ │ │ │ -
│ │ │ │ -
81 NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
82 return boost::static_pointer_cast<NonlinearFactor>(
│ │ │ │ -
83 NonlinearFactor::shared_ptr(new This(*this)));
│ │ │ │ -
84 }
│ │ │ │ -
│ │ │ │ -
85
│ │ │ │ -
87
│ │ │ │ -
88 // Print out the factor.
│ │ │ │ -
│ │ │ │ -
89 void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ -
90 Base::print(s, keyFormatter);
│ │ │ │ -
91 gtsam::print(Vector(nM_), "local field (nM): ");
│ │ │ │ -
92 gtsam::print(Vector(measured_), "measured field (bM): ");
│ │ │ │ -
93 gtsam::print(Vector(bias_), "magnetometer bias: ");
│ │ │ │ +
67 Pose3 pose(double t) const override {
│ │ │ │ +
68 return nTb0_ * Pose3::Expmap(twist_ * t);
│ │ │ │ +
69 }
│ │ │ │ +
│ │ │ │ +
70 Vector3 omega_b(double t) const override { return twist_.head<3>(); }
│ │ │ │ +
│ │ │ │ +
71 Vector3 velocity_n(double t) const override {
│ │ │ │ +
72 return rotation(t).matrix() * twist_.tail<3>();
│ │ │ │ +
73 }
│ │ │ │ +
│ │ │ │ +
74 Vector3 acceleration_n(double t) const override { return rotation(t) * a_b_; }
│ │ │ │ +
75
│ │ │ │ +
76 private:
│ │ │ │ +
77 const Vector6 twist_;
│ │ │ │ +
78 const Vector3 a_b_; // constant centripetal acceleration in body = w_b * v_b
│ │ │ │ +
79 const Pose3 nTb0_;
│ │ │ │ +
80};
│ │ │ │ +
│ │ │ │ +
81
│ │ │ │ +
│ │ │ │ +
83class AcceleratingScenario : public Scenario {
│ │ │ │ +
84 public:
│ │ │ │ +
│ │ │ │ +
87 AcceleratingScenario(const Rot3& nRb, const Point3& p0, const Vector3& v0,
│ │ │ │ +
88 const Vector3& a_n,
│ │ │ │ +
89 const Vector3& omega_b = Vector3::Zero())
│ │ │ │ +
90 : nRb_(nRb), p0_(p0), v0_(v0), a_n_(a_n), omega_b_(omega_b) {}
│ │ │ │ +
│ │ │ │ +
91
│ │ │ │ +
│ │ │ │ +
92 Pose3 pose(double t) const override {
│ │ │ │ +
93 return Pose3(nRb_.expmap(omega_b_ * t), p0_ + v0_ * t + a_n_ * t * t / 2.0);
│ │ │ │
94 }
│ │ │ │
│ │ │ │ -
95
│ │ │ │ -
│ │ │ │ -
97 bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
│ │ │ │ -
98 const This *e = dynamic_cast<const This*> (&expected);
│ │ │ │ -
99 return e != nullptr && Base::equals(*e, tol) &&
│ │ │ │ -
100 gtsam::equal_with_abs_tol(this->measured_, e->measured_, tol) &&
│ │ │ │ -
101 gtsam::equal_with_abs_tol(this->nM_, e->nM_, tol) &&
│ │ │ │ -
102 gtsam::equal_with_abs_tol(this->bias_, e->bias_, tol);
│ │ │ │ -
103 }
│ │ │ │ -
│ │ │ │ -
104
│ │ │ │ -
106
│ │ │ │ -
│ │ │ │ -
111 Vector evaluateError(const POSE& nPb, boost::optional<Matrix&> H = boost::none) const override {
│ │ │ │ -
112 // Predict the measured magnetic field h(x) in the *body* frame.
│ │ │ │ -
113 // If body_P_sensor was given, bias_ will have been rotated into the body frame.
│ │ │ │ -
114 Matrix H_rot = Matrix::Zero(MeasDim, RotDim);
│ │ │ │ -
115 const Point hx = nPb.rotation().unrotate(nM_, H_rot, boost::none) + bias_;
│ │ │ │ -
116
│ │ │ │ -
117 if (H) {
│ │ │ │ -
118 // Fill in the relevant part of the Jacobian (just rotation columns).
│ │ │ │ -
119 *H = Matrix::Zero(MeasDim, PoseDim);
│ │ │ │ -
120 const size_t rot_col0 = nPb.rotationInterval().first;
│ │ │ │ -
121 (*H).block(0, rot_col0, MeasDim, RotDim) = H_rot;
│ │ │ │ -
122 }
│ │ │ │ -
123
│ │ │ │ -
124 return (hx - measured_);
│ │ │ │ -
125 }
│ │ │ │ -
│ │ │ │ -
126
│ │ │ │ -
127 private:
│ │ │ │ -
129 friend class boost::serialization::access;
│ │ │ │ -
130 template<class ARCHIVE>
│ │ │ │ -
131 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
132 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ -
133 ar & boost::serialization::make_nvp("NoiseModelFactor1",
│ │ │ │ -
134 boost::serialization::base_object<Base>(*this));
│ │ │ │ -
135 ar & BOOST_SERIALIZATION_NVP(measured_);
│ │ │ │ -
136 ar & BOOST_SERIALIZATION_NVP(nM_);
│ │ │ │ -
137 ar & BOOST_SERIALIZATION_NVP(bias_);
│ │ │ │ -
138 ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
│ │ │ │ -
139 }
│ │ │ │ -
140}; // \class MagPoseFactor
│ │ │ │ -
│ │ │ │ -
141
│ │ │ │ -
142}
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
95 Vector3 omega_b(double t) const override { return omega_b_; }
│ │ │ │ +
96 Vector3 velocity_n(double t) const override { return v0_ + a_n_ * t; }
│ │ │ │ +
97 Vector3 acceleration_n(double t) const override { return a_n_; }
│ │ │ │ +
98
│ │ │ │ +
99 private:
│ │ │ │ +
100 const Rot3 nRb_;
│ │ │ │ +
101 const Vector3 p0_, v0_, a_n_, omega_b_;
│ │ │ │ +
102};
│ │ │ │ +
│ │ │ │ +
103
│ │ │ │ +
104} // namespace gtsam
│ │ │ │ + │ │ │ │ +
NavState.h
Navigation state composing of attitude, position, and velocity.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition Matrix.h:81
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
│ │ │ │ -
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
│ │ │ │ -
gtsam::MagPoseFactor
Factor to estimate rotation of a Pose2 or Pose3 given a magnetometer reading.
Definition MagPoseFactor.h:28
│ │ │ │ -
gtsam::MagPoseFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Implement functions needed for Testable.
Definition MagPoseFactor.h:89
│ │ │ │ -
gtsam::MagPoseFactor::~MagPoseFactor
~MagPoseFactor() override
Concept check by type.
Definition MagPoseFactor.h:52
│ │ │ │ -
gtsam::MagPoseFactor::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
Equals function.
Definition MagPoseFactor.h:97
│ │ │ │ -
gtsam::MagPoseFactor::clone
NonlinearFactor::shared_ptr clone() const override
Definition MagPoseFactor.h:81
│ │ │ │ -
gtsam::MagPoseFactor::access
friend class boost::serialization::access
Serialization function.
Definition MagPoseFactor.h:129
│ │ │ │ -
gtsam::MagPoseFactor::MagPoseFactor
MagPoseFactor(Key pose_key, const Point &measured, double scale, const Point &direction, const Point &bias, const SharedNoiseModel &model, const boost::optional< POSE > &body_P_sensor)
Construct the factor.
Definition MagPoseFactor.h:67
│ │ │ │ -
gtsam::MagPoseFactor::evaluateError
Vector evaluateError(const POSE &nPb, boost::optional< Matrix & > H=boost::none) const override
Implement functions needed to derive from Factor.
Definition MagPoseFactor.h:111
│ │ │ │ -
gtsam::MagPoseFactor::MagPoseFactor
MagPoseFactor()
Default constructor - only use for serialization.
Definition MagPoseFactor.h:55
│ │ │ │ -
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ -
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ -
concepts.h
Concept-checking macros for geometric objects Each macro instantiates a concept check structure,...
│ │ │ │ +
gtsam::cross
Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
cross product
Definition Point3.cpp:64
│ │ │ │ +
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ +
gtsam::LieGroup::expmap
Class expmap(const TangentVector &v) const
expmap as required by manifold concept Applies exponential map to v and composes with *this
Definition Lie.h:78
│ │ │ │ +
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ +
gtsam::Pose3::Expmap
static Pose3 Expmap(const Vector6 &xi, OptionalJacobian< 6, 6 > Hxi=boost::none)
Exponential map at identity - create a rotation from canonical coordinates .
Definition Pose3.cpp:169
│ │ │ │ +
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get rotation
Definition Pose3.cpp:315
│ │ │ │ +
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
│ │ │ │ +
gtsam::Rot3::matrix
Matrix3 matrix() const
return 3*3 rotation matrix
Definition Rot3M.cpp:219
│ │ │ │ +
gtsam::NavState
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition NavState.h:34
│ │ │ │ +
gtsam::Scenario
Simple trajectory simulator.
Definition Scenario.h:25
│ │ │ │ +
gtsam::Scenario::pose
virtual Pose3 pose(double t) const =0
pose at time t
│ │ │ │ +
gtsam::Scenario::acceleration_n
virtual Vector3 acceleration_n(double t) const =0
acceleration in nav frame
│ │ │ │ +
gtsam::Scenario::velocity_n
virtual Vector3 velocity_n(double t) const =0
velocity at time t, in nav frame
│ │ │ │ +
gtsam::Scenario::~Scenario
virtual ~Scenario()
virtual destructor
Definition Scenario.h:28
│ │ │ │ +
gtsam::Scenario::omega_b
virtual Vector3 omega_b(double t) const =0
angular velocity in body frame
│ │ │ │ +
gtsam::ConstantTwistScenario
Scenario with constant twist 3D trajectory.
Definition Scenario.h:60
│ │ │ │ +
gtsam::ConstantTwistScenario::ConstantTwistScenario
ConstantTwistScenario(const Vector3 &w, const Vector3 &v, const Pose3 &nTb0=Pose3())
Construct scenario with constant twist [w,v].
Definition Scenario.h:63
│ │ │ │ +
gtsam::ConstantTwistScenario::acceleration_n
Vector3 acceleration_n(double t) const override
acceleration in nav frame
Definition Scenario.h:74
│ │ │ │ +
gtsam::ConstantTwistScenario::omega_b
Vector3 omega_b(double t) const override
angular velocity in body frame
Definition Scenario.h:70
│ │ │ │ +
gtsam::ConstantTwistScenario::pose
Pose3 pose(double t) const override
pose at time t
Definition Scenario.h:67
│ │ │ │ +
gtsam::ConstantTwistScenario::velocity_n
Vector3 velocity_n(double t) const override
velocity at time t, in nav frame
Definition Scenario.h:71
│ │ │ │ +
gtsam::AcceleratingScenario
Accelerating from an arbitrary initial state, with optional rotation.
Definition Scenario.h:83
│ │ │ │ +
gtsam::AcceleratingScenario::pose
Pose3 pose(double t) const override
pose at time t
Definition Scenario.h:92
│ │ │ │ +
gtsam::AcceleratingScenario::omega_b
Vector3 omega_b(double t) const override
angular velocity in body frame
Definition Scenario.h:95
│ │ │ │ +
gtsam::AcceleratingScenario::AcceleratingScenario
AcceleratingScenario(const Rot3 &nRb, const Point3 &p0, const Vector3 &v0, const Vector3 &a_n, const Vector3 &omega_b=Vector3::Zero())
Construct scenario with constant acceleration in navigation frame and optional angular velocity in bo...
Definition Scenario.h:87
│ │ │ │ +
gtsam::AcceleratingScenario::acceleration_n
Vector3 acceleration_n(double t) const override
acceleration in nav frame
Definition Scenario.h:97
│ │ │ │ +
gtsam::AcceleratingScenario::velocity_n
Vector3 velocity_n(double t) const override
velocity at time t, in nav frame
Definition Scenario.h:96
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,216 +1,213 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -MagPoseFactor.h │ │ │ │ │ +Scenario.h │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -12#pragma once │ │ │ │ │ -13 │ │ │ │ │ -14#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_c_o_n_c_e_p_t_s_._h> │ │ │ │ │ -15#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -16 │ │ │ │ │ -17namespace _g_t_s_a_m { │ │ │ │ │ -18 │ │ │ │ │ -27template │ │ │ │ │ -_2_8class _M_a_g_P_o_s_e_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -29 private: │ │ │ │ │ -30 using This = _M_a_g_P_o_s_e_F_a_c_t_o_r_<_P_O_S_E_>; │ │ │ │ │ -31 using _B_a_s_e = _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_P_O_S_E_>; │ │ │ │ │ -32 using Point = typename POSE::Translation; │ │ │ │ │ -33 using Rot = typename POSE::Rotation; │ │ │ │ │ -34 │ │ │ │ │ -35 const Point measured_; │ │ │ │ │ -36 const Point nM_; │ │ │ │ │ -37 const Point bias_; │ │ │ │ │ -38 boost::optional body_P_sensor_; │ │ │ │ │ -39 │ │ │ │ │ -40 static const int MeasDim = Point::RowsAtCompileTime; │ │ │ │ │ -41 static const int PoseDim = _t_r_a_i_t_s_<_P_O_S_E_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ -42 static const int RotDim = _t_r_a_i_t_s_<_R_o_t_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ -43 │ │ │ │ │ -45 using shared_ptr = boost::shared_ptr>; │ │ │ │ │ +18#pragma once │ │ │ │ │ +19#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_N_o_i_s_e_M_o_d_e_l_._h> │ │ │ │ │ +20#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_N_a_v_S_t_a_t_e_._h> │ │ │ │ │ +21 │ │ │ │ │ +22namespace _g_t_s_a_m { │ │ │ │ │ +23 │ │ │ │ │ +_2_5class _S_c_e_n_a_r_i_o { │ │ │ │ │ +26 public: │ │ │ │ │ +_2_8 virtual _~_S_c_e_n_a_r_i_o() {} │ │ │ │ │ +29 │ │ │ │ │ +30 // Quantities a Scenario needs to specify: │ │ │ │ │ +31 │ │ │ │ │ +_3_2 virtual _P_o_s_e_3 _p_o_s_e(double t) const = 0; │ │ │ │ │ +_3_3 virtual Vector3 _o_m_e_g_a___b(double t) const = 0; │ │ │ │ │ +_3_4 virtual Vector3 _v_e_l_o_c_i_t_y___n(double t) const = 0; │ │ │ │ │ +_3_5 virtual Vector3 _a_c_c_e_l_e_r_a_t_i_o_n___n(double t) const = 0; │ │ │ │ │ +36 │ │ │ │ │ +37 // Derived quantities: │ │ │ │ │ +38 │ │ │ │ │ +39 _R_o_t_3 rotation(double t) const { return _p_o_s_e(t)._r_o_t_a_t_i_o_n(); } │ │ │ │ │ +40 _N_a_v_S_t_a_t_e navState(double t) const { return _N_a_v_S_t_a_t_e(_p_o_s_e(t), _v_e_l_o_c_i_t_y___n(t)); │ │ │ │ │ +} │ │ │ │ │ +41 │ │ │ │ │ +42 Vector3 velocity_b(double t) const { │ │ │ │ │ +43 const Rot3 nRb = rotation(t); │ │ │ │ │ +44 return nRb.transpose() * _v_e_l_o_c_i_t_y___n(t); │ │ │ │ │ +45 } │ │ │ │ │ 46 │ │ │ │ │ -48 GTSAM_CONCEPT_TESTABLE_TYPE(POSE) │ │ │ │ │ -49 GTSAM_CONCEPT_POSE_TYPE(POSE) │ │ │ │ │ -50 │ │ │ │ │ -51 public: │ │ │ │ │ -_5_2 _~_M_a_g_P_o_s_e_F_a_c_t_o_r() override {} │ │ │ │ │ -53 │ │ │ │ │ -_5_5 _M_a_g_P_o_s_e_F_a_c_t_o_r() {} │ │ │ │ │ -56 │ │ │ │ │ -_6_7 _M_a_g_P_o_s_e_F_a_c_t_o_r(_K_e_y pose_key, │ │ │ │ │ -68 const Point& measured, │ │ │ │ │ -69 double scale, │ │ │ │ │ -70 const Point& direction, │ │ │ │ │ -71 const Point& bias, │ │ │ │ │ -72 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model, │ │ │ │ │ -73 const boost::optional& body_P_sensor) │ │ │ │ │ -74 : _B_a_s_e(model, pose_key), │ │ │ │ │ -75 measured_(body_P_sensor ? body_P_sensor->rotation() * measured : measured), │ │ │ │ │ -76 nM_(scale * direction.normalized()), │ │ │ │ │ -77 bias_(body_P_sensor ? body_P_sensor->rotation() * bias : bias), │ │ │ │ │ -78 body_P_sensor_(body_P_sensor) {} │ │ │ │ │ -79 │ │ │ │ │ -_8_1 NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -82 return boost::static_pointer_cast( │ │ │ │ │ -83 NonlinearFactor::shared_ptr(new This(*this))); │ │ │ │ │ -84 } │ │ │ │ │ -85 │ │ │ │ │ -87 │ │ │ │ │ -88 // Print out the factor. │ │ │ │ │ -_8_9 void _p_r_i_n_t(const std::string& s = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ -DefaultKeyFormatter) const override { │ │ │ │ │ -90 _B_a_s_e_:_:_p_r_i_n_t(s, keyFormatter); │ │ │ │ │ -91 _g_t_s_a_m_:_:_p_r_i_n_t(Vector(nM_), "local field (nM): "); │ │ │ │ │ -92 _g_t_s_a_m_:_:_p_r_i_n_t(Vector(measured_), "measured field (bM): "); │ │ │ │ │ -93 _g_t_s_a_m_:_:_p_r_i_n_t(Vector(bias_), "magnetometer bias: "); │ │ │ │ │ +47 Vector3 acceleration_b(double t) const { │ │ │ │ │ +48 const Rot3 nRb = rotation(t); │ │ │ │ │ +49 return nRb.transpose() * _a_c_c_e_l_e_r_a_t_i_o_n___n(t); │ │ │ │ │ +50 } │ │ │ │ │ +51}; │ │ │ │ │ +52 │ │ │ │ │ +_6_0class _C_o_n_s_t_a_n_t_T_w_i_s_t_S_c_e_n_a_r_i_o : public _S_c_e_n_a_r_i_o { │ │ │ │ │ +61 public: │ │ │ │ │ +_6_3 _C_o_n_s_t_a_n_t_T_w_i_s_t_S_c_e_n_a_r_i_o(const Vector3& w, const Vector3& v, │ │ │ │ │ +64 const _P_o_s_e_3& nTb0 = _P_o_s_e_3()) │ │ │ │ │ +65 : twist_((Vector6() << w, v).finished()), a_b_(w._c_r_o_s_s(v)), nTb0_(nTb0) {} │ │ │ │ │ +66 │ │ │ │ │ +_6_7 _P_o_s_e_3 _p_o_s_e(double t) const override { │ │ │ │ │ +68 return nTb0_ * _P_o_s_e_3_:_:_E_x_p_m_a_p(twist_ * t); │ │ │ │ │ +69 } │ │ │ │ │ +_7_0 Vector3 _o_m_e_g_a___b(double t) const override { return twist_.head<3>(); } │ │ │ │ │ +_7_1 Vector3 _v_e_l_o_c_i_t_y___n(double t) const override { │ │ │ │ │ +72 return rotation(t)._m_a_t_r_i_x() * twist_.tail<3>(); │ │ │ │ │ +73 } │ │ │ │ │ +_7_4 Vector3 _a_c_c_e_l_e_r_a_t_i_o_n___n(double t) const override { return rotation(t) * a_b_; │ │ │ │ │ +} │ │ │ │ │ +75 │ │ │ │ │ +76 private: │ │ │ │ │ +77 const Vector6 twist_; │ │ │ │ │ +78 const Vector3 a_b_; // constant centripetal acceleration in body = w_b * v_b │ │ │ │ │ +79 const _P_o_s_e_3 nTb0_; │ │ │ │ │ +80}; │ │ │ │ │ +81 │ │ │ │ │ +_8_3class _A_c_c_e_l_e_r_a_t_i_n_g_S_c_e_n_a_r_i_o : public _S_c_e_n_a_r_i_o { │ │ │ │ │ +84 public: │ │ │ │ │ +_8_7 _A_c_c_e_l_e_r_a_t_i_n_g_S_c_e_n_a_r_i_o(const _R_o_t_3& nRb, const _P_o_i_n_t_3& p0, const Vector3& v0, │ │ │ │ │ +88 const Vector3& a_n, │ │ │ │ │ +89 const Vector3& _o_m_e_g_a___b = Vector3::Zero()) │ │ │ │ │ +90 : nRb_(nRb), p0_(p0), v0_(v0), a_n_(a_n), omega_b_(_o_m_e_g_a___b) {} │ │ │ │ │ +91 │ │ │ │ │ +_9_2 _P_o_s_e_3 _p_o_s_e(double t) const override { │ │ │ │ │ +93 return _P_o_s_e_3(nRb_._e_x_p_m_a_p(omega_b_ * t), p0_ + v0_ * t + a_n_ * t * t / 2.0); │ │ │ │ │ 94 } │ │ │ │ │ -95 │ │ │ │ │ -_9_7 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& expected, double tol=1e-9) const override │ │ │ │ │ -{ │ │ │ │ │ -98 const This *e = dynamic_cast (&expected); │ │ │ │ │ -99 return e != nullptr && _B_a_s_e_:_:_e_q_u_a_l_s(*e, tol) && │ │ │ │ │ -100 _g_t_s_a_m_:_:_e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(this->measured_, e->measured_, tol) && │ │ │ │ │ -101 _g_t_s_a_m_:_:_e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(this->nM_, e->nM_, tol) && │ │ │ │ │ -102 _g_t_s_a_m_:_:_e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(this->bias_, e->bias_, tol); │ │ │ │ │ -103 } │ │ │ │ │ -104 │ │ │ │ │ -106 │ │ │ │ │ -_1_1_1 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const POSE& nPb, boost::optional H = boost:: │ │ │ │ │ -none) const override { │ │ │ │ │ -112 // Predict the measured magnetic field h(x) in the *body* frame. │ │ │ │ │ -113 // If body_P_sensor was given, bias_ will have been rotated into the body │ │ │ │ │ -frame. │ │ │ │ │ -114 Matrix H_rot = Matrix::Zero(MeasDim, RotDim); │ │ │ │ │ -115 const Point hx = nPb.rotation().unrotate(nM_, H_rot, boost::none) + bias_; │ │ │ │ │ -116 │ │ │ │ │ -117 if (H) { │ │ │ │ │ -118 // Fill in the relevant part of the Jacobian (just rotation columns). │ │ │ │ │ -119 *H = Matrix::Zero(MeasDim, PoseDim); │ │ │ │ │ -120 const size_t rot_col0 = nPb.rotationInterval().first; │ │ │ │ │ -121 (*H).block(0, rot_col0, MeasDim, RotDim) = H_rot; │ │ │ │ │ -122 } │ │ │ │ │ -123 │ │ │ │ │ -124 return (hx - measured_); │ │ │ │ │ -125 } │ │ │ │ │ -126 │ │ │ │ │ -127 private: │ │ │ │ │ -_1_2_9 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -130 template │ │ │ │ │ -131 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -132 // NoiseModelFactor1 instead of NoiseModelFactorN for backward │ │ │ │ │ -compatibility │ │ │ │ │ -133 ar & boost::serialization::make_nvp("NoiseModelFactor1", │ │ │ │ │ -134 boost::serialization::base_object(*this)); │ │ │ │ │ -135 ar & BOOST_SERIALIZATION_NVP(measured_); │ │ │ │ │ -136 ar & BOOST_SERIALIZATION_NVP(nM_); │ │ │ │ │ -137 ar & BOOST_SERIALIZATION_NVP(bias_); │ │ │ │ │ -138 ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); │ │ │ │ │ -139 } │ │ │ │ │ -140}; // \class MagPoseFactor │ │ │ │ │ -141 │ │ │ │ │ -142} │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ -Non-linear factor base classes. │ │ │ │ │ +_9_5 Vector3 _o_m_e_g_a___b(double t) const override { return omega_b_; } │ │ │ │ │ +_9_6 Vector3 _v_e_l_o_c_i_t_y___n(double t) const override { return v0_ + a_n_ * t; } │ │ │ │ │ +_9_7 Vector3 _a_c_c_e_l_e_r_a_t_i_o_n___n(double t) const override { return a_n_; } │ │ │ │ │ +98 │ │ │ │ │ +99 private: │ │ │ │ │ +100 const _R_o_t_3 nRb_; │ │ │ │ │ +101 const Vector3 p0_, v0_, a_n_, omega_b_; │ │ │ │ │ +102}; │ │ │ │ │ +103 │ │ │ │ │ +104} // namespace gtsam │ │ │ │ │ +_N_o_i_s_e_M_o_d_e_l_._h │ │ │ │ │ +_N_a_v_S_t_a_t_e_._h │ │ │ │ │ +Navigation state composing of attitude, position, and velocity. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ -Aliases. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l___w_i_t_h___a_b_s___t_o_l │ │ │ │ │ -bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen:: │ │ │ │ │ -DenseBase< MATRIX > &B, double tol=1e-9) │ │ │ │ │ -equals with a tolerance │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:81 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ -virtual void print(const std::string &s="Factor", const KeyFormatter │ │ │ │ │ -&formatter=DefaultKeyFormatter) const │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn Factor.cpp:29 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const This &other, double tol=1e-9) const │ │ │ │ │ -check equality │ │ │ │ │ -DDeeffiinniittiioonn Factor.cpp:42 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_g_P_o_s_e_F_a_c_t_o_r │ │ │ │ │ -Factor to estimate rotation of a Pose2 or Pose3 given a magnetometer reading. │ │ │ │ │ -DDeeffiinniittiioonn MagPoseFactor.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_g_P_o_s_e_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -Implement functions needed for Testable. │ │ │ │ │ -DDeeffiinniittiioonn MagPoseFactor.h:89 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_g_P_o_s_e_F_a_c_t_o_r_:_:_~_M_a_g_P_o_s_e_F_a_c_t_o_r │ │ │ │ │ -~MagPoseFactor() override │ │ │ │ │ -Concept check by type. │ │ │ │ │ -DDeeffiinniittiioonn MagPoseFactor.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_g_P_o_s_e_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const NonlinearFactor &expected, double tol=1e-9) const override │ │ │ │ │ -Equals function. │ │ │ │ │ -DDeeffiinniittiioonn MagPoseFactor.h:97 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_g_P_o_s_e_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ -NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn MagPoseFactor.h:81 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_g_P_o_s_e_F_a_c_t_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn MagPoseFactor.h:129 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_g_P_o_s_e_F_a_c_t_o_r_:_:_M_a_g_P_o_s_e_F_a_c_t_o_r │ │ │ │ │ -MagPoseFactor(Key pose_key, const Point &measured, double scale, const Point │ │ │ │ │ -&direction, const Point &bias, const SharedNoiseModel &model, const boost:: │ │ │ │ │ -optional< POSE > &body_P_sensor) │ │ │ │ │ -Construct the factor. │ │ │ │ │ -DDeeffiinniittiioonn MagPoseFactor.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_g_P_o_s_e_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ -Vector evaluateError(const POSE &nPb, boost::optional< Matrix & > H=boost:: │ │ │ │ │ -none) const override │ │ │ │ │ -Implement functions needed to derive from Factor. │ │ │ │ │ -DDeeffiinniittiioonn MagPoseFactor.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_g_P_o_s_e_F_a_c_t_o_r_:_:_M_a_g_P_o_s_e_F_a_c_t_o_r │ │ │ │ │ -MagPoseFactor() │ │ │ │ │ -Default constructor - only use for serialization. │ │ │ │ │ -DDeeffiinniittiioonn MagPoseFactor.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ -Nonlinear factor base class. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ -A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ -variables. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ -_c_o_n_c_e_p_t_s_._h │ │ │ │ │ -Concept-checking macros for geometric objects Each macro instantiates a concept │ │ │ │ │ -check structure,... │ │ │ │ │ +_g_t_s_a_m_:_:_c_r_o_s_s │ │ │ │ │ +Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, │ │ │ │ │ +OptionalJacobian< 3, 3 > H2) │ │ │ │ │ +cross product │ │ │ │ │ +DDeeffiinniittiioonn Point3.cpp:64 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ +Vector3 Point3 │ │ │ │ │ +As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ +typedef Point3 to Vector3... │ │ │ │ │ +DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_e_x_p_m_a_p │ │ │ │ │ +Class expmap(const TangentVector &v) const │ │ │ │ │ +expmap as required by manifold concept Applies exponential map to v and │ │ │ │ │ +composes with *this │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:78 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ +A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_E_x_p_m_a_p │ │ │ │ │ +static Pose3 Expmap(const Vector6 &xi, OptionalJacobian< 6, 6 > Hxi=boost:: │ │ │ │ │ +none) │ │ │ │ │ +Exponential map at identity - create a rotation from canonical coordinates . │ │ │ │ │ +DDeeffiinniittiioonn Pose3.cpp:169 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_r_o_t_a_t_i_o_n │ │ │ │ │ +const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const │ │ │ │ │ +get rotation │ │ │ │ │ +DDeeffiinniittiioonn Pose3.cpp:315 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ +Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor │ │ │ │ │ +symbol GTSAM_USE_QUATERNIO... │ │ │ │ │ +DDeeffiinniittiioonn Rot3.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_3_:_:_m_a_t_r_i_x │ │ │ │ │ +Matrix3 matrix() const │ │ │ │ │ +return 3*3 rotation matrix │ │ │ │ │ +DDeeffiinniittiioonn Rot3M.cpp:219 │ │ │ │ │ +_g_t_s_a_m_:_:_N_a_v_S_t_a_t_e │ │ │ │ │ +Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does │ │ │ │ │ +not make sense to make... │ │ │ │ │ +DDeeffiinniittiioonn NavState.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_S_c_e_n_a_r_i_o │ │ │ │ │ +Simple trajectory simulator. │ │ │ │ │ +DDeeffiinniittiioonn Scenario.h:25 │ │ │ │ │ +_g_t_s_a_m_:_:_S_c_e_n_a_r_i_o_:_:_p_o_s_e │ │ │ │ │ +virtual Pose3 pose(double t) const =0 │ │ │ │ │ +pose at time t │ │ │ │ │ +_g_t_s_a_m_:_:_S_c_e_n_a_r_i_o_:_:_a_c_c_e_l_e_r_a_t_i_o_n___n │ │ │ │ │ +virtual Vector3 acceleration_n(double t) const =0 │ │ │ │ │ +acceleration in nav frame │ │ │ │ │ +_g_t_s_a_m_:_:_S_c_e_n_a_r_i_o_:_:_v_e_l_o_c_i_t_y___n │ │ │ │ │ +virtual Vector3 velocity_n(double t) const =0 │ │ │ │ │ +velocity at time t, in nav frame │ │ │ │ │ +_g_t_s_a_m_:_:_S_c_e_n_a_r_i_o_:_:_~_S_c_e_n_a_r_i_o │ │ │ │ │ +virtual ~Scenario() │ │ │ │ │ +virtual destructor │ │ │ │ │ +DDeeffiinniittiioonn Scenario.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_S_c_e_n_a_r_i_o_:_:_o_m_e_g_a___b │ │ │ │ │ +virtual Vector3 omega_b(double t) const =0 │ │ │ │ │ +angular velocity in body frame │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_s_t_a_n_t_T_w_i_s_t_S_c_e_n_a_r_i_o │ │ │ │ │ +Scenario with constant twist 3D trajectory. │ │ │ │ │ +DDeeffiinniittiioonn Scenario.h:60 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_s_t_a_n_t_T_w_i_s_t_S_c_e_n_a_r_i_o_:_:_C_o_n_s_t_a_n_t_T_w_i_s_t_S_c_e_n_a_r_i_o │ │ │ │ │ +ConstantTwistScenario(const Vector3 &w, const Vector3 &v, const Pose3 │ │ │ │ │ +&nTb0=Pose3()) │ │ │ │ │ +Construct scenario with constant twist [w,v]. │ │ │ │ │ +DDeeffiinniittiioonn Scenario.h:63 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_s_t_a_n_t_T_w_i_s_t_S_c_e_n_a_r_i_o_:_:_a_c_c_e_l_e_r_a_t_i_o_n___n │ │ │ │ │ +Vector3 acceleration_n(double t) const override │ │ │ │ │ +acceleration in nav frame │ │ │ │ │ +DDeeffiinniittiioonn Scenario.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_s_t_a_n_t_T_w_i_s_t_S_c_e_n_a_r_i_o_:_:_o_m_e_g_a___b │ │ │ │ │ +Vector3 omega_b(double t) const override │ │ │ │ │ +angular velocity in body frame │ │ │ │ │ +DDeeffiinniittiioonn Scenario.h:70 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_s_t_a_n_t_T_w_i_s_t_S_c_e_n_a_r_i_o_:_:_p_o_s_e │ │ │ │ │ +Pose3 pose(double t) const override │ │ │ │ │ +pose at time t │ │ │ │ │ +DDeeffiinniittiioonn Scenario.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_s_t_a_n_t_T_w_i_s_t_S_c_e_n_a_r_i_o_:_:_v_e_l_o_c_i_t_y___n │ │ │ │ │ +Vector3 velocity_n(double t) const override │ │ │ │ │ +velocity at time t, in nav frame │ │ │ │ │ +DDeeffiinniittiioonn Scenario.h:71 │ │ │ │ │ +_g_t_s_a_m_:_:_A_c_c_e_l_e_r_a_t_i_n_g_S_c_e_n_a_r_i_o │ │ │ │ │ +Accelerating from an arbitrary initial state, with optional rotation. │ │ │ │ │ +DDeeffiinniittiioonn Scenario.h:83 │ │ │ │ │ +_g_t_s_a_m_:_:_A_c_c_e_l_e_r_a_t_i_n_g_S_c_e_n_a_r_i_o_:_:_p_o_s_e │ │ │ │ │ +Pose3 pose(double t) const override │ │ │ │ │ +pose at time t │ │ │ │ │ +DDeeffiinniittiioonn Scenario.h:92 │ │ │ │ │ +_g_t_s_a_m_:_:_A_c_c_e_l_e_r_a_t_i_n_g_S_c_e_n_a_r_i_o_:_:_o_m_e_g_a___b │ │ │ │ │ +Vector3 omega_b(double t) const override │ │ │ │ │ +angular velocity in body frame │ │ │ │ │ +DDeeffiinniittiioonn Scenario.h:95 │ │ │ │ │ +_g_t_s_a_m_:_:_A_c_c_e_l_e_r_a_t_i_n_g_S_c_e_n_a_r_i_o_:_:_A_c_c_e_l_e_r_a_t_i_n_g_S_c_e_n_a_r_i_o │ │ │ │ │ +AcceleratingScenario(const Rot3 &nRb, const Point3 &p0, const Vector3 &v0, │ │ │ │ │ +const Vector3 &a_n, const Vector3 &omega_b=Vector3::Zero()) │ │ │ │ │ +Construct scenario with constant acceleration in navigation frame and optional │ │ │ │ │ +angular velocity in bo... │ │ │ │ │ +DDeeffiinniittiioonn Scenario.h:87 │ │ │ │ │ +_g_t_s_a_m_:_:_A_c_c_e_l_e_r_a_t_i_n_g_S_c_e_n_a_r_i_o_:_:_a_c_c_e_l_e_r_a_t_i_o_n___n │ │ │ │ │ +Vector3 acceleration_n(double t) const override │ │ │ │ │ +acceleration in nav frame │ │ │ │ │ +DDeeffiinniittiioonn Scenario.h:97 │ │ │ │ │ +_g_t_s_a_m_:_:_A_c_c_e_l_e_r_a_t_i_n_g_S_c_e_n_a_r_i_o_:_:_v_e_l_o_c_i_t_y___n │ │ │ │ │ +Vector3 velocity_n(double t) const override │ │ │ │ │ +velocity at time t, in nav frame │ │ │ │ │ +DDeeffiinniittiioonn Scenario.h:96 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * MMaaggPPoosseeFFaaccttoorr..hh │ │ │ │ │ + * _S_c_e_n_a_r_i_o_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00959.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/NavState.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ManifoldPreintegration.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,54 +95,49 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs
│ │ │ │ -
NavState.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
ManifoldPreintegration.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Navigation state composing of attitude, position, and velocity. │ │ │ │ -More...

│ │ │ │ - │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::NavState
 Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make this a Lie group, but it is a 9D manifold. More...
 
struct  gtsam::traits< NavState >
class  gtsam::ManifoldPreintegration
 IMU pre-integration on NavSatet manifold. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Typedefs

│ │ │ │ -typedef Vector3 gtsam::Velocity3
 Velocity is currently typedef'd to Vector3.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Navigation state composing of attitude, position, and velocity.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
Date
July 2015
│ │ │ │ +
Author
Luca Carlone
│ │ │ │ +
│ │ │ │ +Stephen Williams
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │ +
│ │ │ │ +Vadim Indelman
│ │ │ │ +
│ │ │ │ +David Jensen
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,35 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ -NavState.h File Reference │ │ │ │ │ -Navigation state composing of attitude, position, and velocity. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +ManifoldPreintegration.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_N_a_v_S_t_a_t_e │ │ │ │ │ - Navigation state: Pose (rotation, translation) + velocity NOTE(frank): │ │ │ │ │ -  it does not make sense to make this a Lie group, but it is a 9D │ │ │ │ │ - manifold. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _N_a_v_S_t_a_t_e_ _> │ │ │ │ │ +class   _g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n │ │ │ │ │ +  IMU pre-integration on NavSatet manifold. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -typedef Vector3  ggttssaamm::::VVeelloocciittyy33 │ │ │ │ │ -  Velocity is currently typedef'd to Vector3. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Navigation state composing of attitude, position, and velocity. │ │ │ │ │ Author │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Stephen Williams │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Vadim Indelman │ │ │ │ │ + David Jensen │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - July 2015 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _N_a_v_S_t_a_t_e_._h │ │ │ │ │ + * _M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00959.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,5 +1,3 @@ │ │ │ │ │ var a00959 = [ │ │ │ │ │ - ["gtsam::NavState", "a04196.html", "a04196"], │ │ │ │ │ - ["gtsam::traits< NavState >", "a04200.html", null], │ │ │ │ │ - ["Velocity3", "a00959.html#ad6ba0a40d5dd8936f4ca53f7bb3ef53b", null] │ │ │ │ │ + ["gtsam::ManifoldPreintegration", "a04192.html", "a04192"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00959_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/NavState.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ManifoldPreintegration.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,217 +98,124 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
NavState.h
│ │ │ │ +
ManifoldPreintegration.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/geometry/Pose3.h>
│ │ │ │ -
22#include <gtsam/base/Vector.h>
│ │ │ │ -
23#include <gtsam/base/Manifold.h>
│ │ │ │ -
24
│ │ │ │ -
25namespace gtsam {
│ │ │ │ +
22#pragma once
│ │ │ │ +
23
│ │ │ │ +
24#include <gtsam/navigation/NavState.h>
│ │ │ │ +
25#include <gtsam/navigation/PreintegrationBase.h>
│ │ │ │
26
│ │ │ │ -
28typedef Vector3 Velocity3;
│ │ │ │ -
29
│ │ │ │ -
│ │ │ │ -
34class GTSAM_EXPORT NavState {
│ │ │ │ -
35private:
│ │ │ │ -
36
│ │ │ │ -
37 // TODO(frank):
│ │ │ │ -
38 // - should we rename t_ to p_? if not, we should rename dP do dT
│ │ │ │ -
39 Rot3 R_;
│ │ │ │ -
40 Point3 t_;
│ │ │ │ -
41 Velocity3 v_;
│ │ │ │ -
42
│ │ │ │ -
43public:
│ │ │ │ -
44
│ │ │ │ -
45 enum {
│ │ │ │ -
46 dimension = 9
│ │ │ │ -
47 };
│ │ │ │ -
48
│ │ │ │ -
49 typedef std::pair<Point3, Velocity3> PositionAndVelocity;
│ │ │ │ -
50
│ │ │ │ -
53
│ │ │ │ -
│ │ │ │ -
55 NavState() :
│ │ │ │ -
56 t_(0, 0, 0), v_(Vector3::Zero()) {
│ │ │ │ -
57 }
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
59 NavState(const Rot3& R, const Point3& t, const Velocity3& v) :
│ │ │ │ -
60 R_(R), t_(t), v_(v) {
│ │ │ │ -
61 }
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
63 NavState(const Pose3& pose, const Velocity3& v) :
│ │ │ │ -
64 R_(pose.rotation()), t_(pose.translation()), v_(v) {
│ │ │ │ -
65 }
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
67 NavState(const Matrix3& R, const Vector6& tv) :
│ │ │ │ -
68 R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) {
│ │ │ │ -
69 }
│ │ │ │ -
│ │ │ │ -
71 static NavState Create(const Rot3& R, const Point3& t, const Velocity3& v,
│ │ │ │ -
72 OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2,
│ │ │ │ -
73 OptionalJacobian<9, 3> H3);
│ │ │ │ -
75 static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel,
│ │ │ │ -
76 OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2);
│ │ │ │ -
77
│ │ │ │ -
81
│ │ │ │ -
82 const Rot3& attitude(OptionalJacobian<3, 9> H = boost::none) const;
│ │ │ │ -
83 const Point3& position(OptionalJacobian<3, 9> H = boost::none) const;
│ │ │ │ -
84 const Velocity3& velocity(OptionalJacobian<3, 9> H = boost::none) const;
│ │ │ │ -
85
│ │ │ │ -
86 const Pose3 pose() const {
│ │ │ │ -
87 return Pose3(attitude(), position());
│ │ │ │ -
88 }
│ │ │ │ -
89
│ │ │ │ -
93
│ │ │ │ -
│ │ │ │ -
95 Matrix3 R() const {
│ │ │ │ -
96 return R_.matrix();
│ │ │ │ -
97 }
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
99 Quaternion quaternion() const {
│ │ │ │ -
100 return R_.toQuaternion();
│ │ │ │ -
101 }
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
103 Vector3 t() const {
│ │ │ │ -
104 return t_;
│ │ │ │ -
105 }
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
107 const Vector3& v() const {
│ │ │ │ -
108 return v_;
│ │ │ │ -
109 }
│ │ │ │ +
27namespace gtsam {
│ │ │ │ +
28
│ │ │ │ +
│ │ │ │ +
33class GTSAM_EXPORT ManifoldPreintegration : public PreintegrationBase {
│ │ │ │ +
34 protected:
│ │ │ │ +
35
│ │ │ │ +
41 NavState deltaXij_;
│ │ │ │ +
42 Matrix3 delRdelBiasOmega_;
│ │ │ │ +
43 Matrix3 delPdelBiasAcc_;
│ │ │ │ +
44 Matrix3 delPdelBiasOmega_;
│ │ │ │ +
45 Matrix3 delVdelBiasAcc_;
│ │ │ │ +
46 Matrix3 delVdelBiasOmega_;
│ │ │ │ +
47
│ │ │ │ +
│ │ │ │ +
49 ManifoldPreintegration() {
│ │ │ │ +
50 resetIntegration();
│ │ │ │ +
51 }
│ │ │ │ +
│ │ │ │ +
52
│ │ │ │ +
53public:
│ │ │ │ +
56
│ │ │ │ +
62 ManifoldPreintegration(const boost::shared_ptr<Params>& p,
│ │ │ │ +
63 const imuBias::ConstantBias& biasHat = imuBias::ConstantBias());
│ │ │ │ +
64
│ │ │ │ +
66
│ │ │ │ +
70 void resetIntegration() override;
│ │ │ │ +
71
│ │ │ │ +
73
│ │ │ │ +
76 NavState deltaXij() const override { return deltaXij_; }
│ │ │ │ +
77 Rot3 deltaRij() const override { return deltaXij_.attitude(); }
│ │ │ │ +
78 Vector3 deltaPij() const override { return deltaXij_.position(); }
│ │ │ │ +
79 Vector3 deltaVij() const override { return deltaXij_.velocity(); }
│ │ │ │ +
80
│ │ │ │ +
81 Matrix3 delRdelBiasOmega() const { return delRdelBiasOmega_; }
│ │ │ │ +
82 Matrix3 delPdelBiasAcc() const { return delPdelBiasAcc_; }
│ │ │ │ +
83 Matrix3 delPdelBiasOmega() const { return delPdelBiasOmega_; }
│ │ │ │ +
84 Matrix3 delVdelBiasAcc() const { return delVdelBiasAcc_; }
│ │ │ │ +
85 Matrix3 delVdelBiasOmega() const { return delVdelBiasOmega_; }
│ │ │ │ +
86
│ │ │ │ +
89 bool equals(const ManifoldPreintegration& other, double tol) const;
│ │ │ │ +
91
│ │ │ │ +
94
│ │ │ │ +
99 void update(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt,
│ │ │ │ +
100 Matrix9* A, Matrix93* B, Matrix93* C) override;
│ │ │ │ +
101
│ │ │ │ +
105 Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
│ │ │ │ +
106 OptionalJacobian<9, 6> H = boost::none) const override;
│ │ │ │ +
107
│ │ │ │ +
│ │ │ │ +
109 virtual boost::shared_ptr<ManifoldPreintegration> clone() const {
│ │ │ │ +
110 return boost::shared_ptr<ManifoldPreintegration>();
│ │ │ │ +
111 }
│ │ │ │
│ │ │ │ -
110 // Return velocity in body frame
│ │ │ │ -
111 Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = boost::none) const;
│ │ │ │
112
│ │ │ │ -
116 Matrix7 matrix() const;
│ │ │ │ -
117
│ │ │ │ -
121
│ │ │ │ -
123 GTSAM_EXPORT
│ │ │ │ -
124 friend std::ostream &operator<<(std::ostream &os, const NavState& state);
│ │ │ │ -
125
│ │ │ │ -
127 void print(const std::string& s = "") const;
│ │ │ │ -
128
│ │ │ │ -
130 bool equals(const NavState& other, double tol = 1e-8) const;
│ │ │ │ -
131
│ │ │ │ -
135
│ │ │ │ -
136 // Tangent space sugar.
│ │ │ │ -
137 // TODO(frank): move to private navstate namespace in cpp
│ │ │ │ -
138 static Eigen::Block<Vector9, 3, 1> dR(Vector9& v) {
│ │ │ │ -
139 return v.segment<3>(0);
│ │ │ │ -
140 }
│ │ │ │ -
141 static Eigen::Block<Vector9, 3, 1> dP(Vector9& v) {
│ │ │ │ -
142 return v.segment<3>(3);
│ │ │ │ -
143 }
│ │ │ │ -
144 static Eigen::Block<Vector9, 3, 1> dV(Vector9& v) {
│ │ │ │ -
145 return v.segment<3>(6);
│ │ │ │ -
146 }
│ │ │ │ -
147 static Eigen::Block<const Vector9, 3, 1> dR(const Vector9& v) {
│ │ │ │ -
148 return v.segment<3>(0);
│ │ │ │ -
149 }
│ │ │ │ -
150 static Eigen::Block<const Vector9, 3, 1> dP(const Vector9& v) {
│ │ │ │ -
151 return v.segment<3>(3);
│ │ │ │ -
152 }
│ │ │ │ -
153 static Eigen::Block<const Vector9, 3, 1> dV(const Vector9& v) {
│ │ │ │ -
154 return v.segment<3>(6);
│ │ │ │ -
155 }
│ │ │ │ -
156
│ │ │ │ -
158 NavState retract(const Vector9& v, //
│ │ │ │ -
159 OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
│ │ │ │ -
160 boost::none) const;
│ │ │ │ -
161
│ │ │ │ -
163 Vector9 localCoordinates(const NavState& g, //
│ │ │ │ -
164 OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
│ │ │ │ -
165 boost::none) const;
│ │ │ │ -
166
│ │ │ │ -
170
│ │ │ │ -
173 NavState update(const Vector3& b_acceleration, const Vector3& b_omega,
│ │ │ │ -
174 const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
│ │ │ │ -
175 OptionalJacobian<9, 3> G2) const;
│ │ │ │ -
176
│ │ │ │ -
178 Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false,
│ │ │ │ -
179 OptionalJacobian<9, 9> H = boost::none) const;
│ │ │ │ -
180
│ │ │ │ -
183 Vector9 correctPIM(const Vector9& pim, double dt, const Vector3& n_gravity,
│ │ │ │ -
184 const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis =
│ │ │ │ -
185 false, OptionalJacobian<9, 9> H1 = boost::none,
│ │ │ │ -
186 OptionalJacobian<9, 9> H2 = boost::none) const;
│ │ │ │ -
187
│ │ │ │ -
189
│ │ │ │ -
190private:
│ │ │ │ -
193 friend class boost::serialization::access;
│ │ │ │ -
194 template<class ARCHIVE>
│ │ │ │ -
195 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
196 ar & BOOST_SERIALIZATION_NVP(R_);
│ │ │ │ -
197 ar & BOOST_SERIALIZATION_NVP(t_);
│ │ │ │ -
198 ar & BOOST_SERIALIZATION_NVP(v_);
│ │ │ │ -
199 }
│ │ │ │ -
201};
│ │ │ │ -
│ │ │ │ -
202
│ │ │ │ -
203// Specialize NavState traits to use a Retract/Local that agrees with IMUFactors
│ │ │ │ -
204template<>
│ │ │ │ -
│ │ │ │ -
205struct traits<NavState> : internal::Manifold<NavState> {
│ │ │ │ -
206};
│ │ │ │ -
│ │ │ │ -
207
│ │ │ │ -
208} // namespace gtsam
│ │ │ │ -
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ -
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ -
Pose3.h
3D Pose
│ │ │ │ +
114
│ │ │ │ +
115private:
│ │ │ │ +
117 friend class boost::serialization::access;
│ │ │ │ +
118 template<class ARCHIVE>
│ │ │ │ +
119 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
120 namespace bs = ::boost::serialization;
│ │ │ │ +
121 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
│ │ │ │ +
122 ar & BOOST_SERIALIZATION_NVP(deltaXij_);
│ │ │ │ +
123 ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
│ │ │ │ +
124 ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
│ │ │ │ +
125 ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);
│ │ │ │ +
126 ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_);
│ │ │ │ +
127 ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_);
│ │ │ │ +
128 }
│ │ │ │ +
129};
│ │ │ │ +
│ │ │ │ +
130
│ │ │ │ +
131}
│ │ │ │ +
NavState.h
Navigation state composing of attitude, position, and velocity.
│ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ -
gtsam::Velocity3
Vector3 Velocity3
Velocity is currently typedef'd to Vector3.
Definition NavState.h:28
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
│ │ │ │ -
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ -
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
│ │ │ │ -
gtsam::Rot3::toQuaternion
gtsam::Quaternion toQuaternion() const
Compute the quaternion representation of this rotation.
Definition Rot3M.cpp:233
│ │ │ │ -
gtsam::Rot3::matrix
Matrix3 matrix() const
return 3*3 rotation matrix
Definition Rot3M.cpp:219
│ │ │ │ +
gtsam::imuBias::ConstantBias
Definition ImuBias.h:30
│ │ │ │ +
gtsam::ManifoldPreintegration
IMU pre-integration on NavSatet manifold.
Definition ManifoldPreintegration.h:33
│ │ │ │ +
gtsam::ManifoldPreintegration::ManifoldPreintegration
ManifoldPreintegration()
Default constructor for serialization.
Definition ManifoldPreintegration.h:49
│ │ │ │ +
gtsam::ManifoldPreintegration::delVdelBiasAcc_
Matrix3 delVdelBiasAcc_
Jacobian of preintegrated velocity w.r.t. acceleration bias.
Definition ManifoldPreintegration.h:45
│ │ │ │ +
gtsam::ManifoldPreintegration::delRdelBiasOmega_
Matrix3 delRdelBiasOmega_
Jacobian of preintegrated rotation w.r.t. angular rate bias.
Definition ManifoldPreintegration.h:42
│ │ │ │ +
gtsam::ManifoldPreintegration::delPdelBiasAcc_
Matrix3 delPdelBiasAcc_
Jacobian of preintegrated position w.r.t. acceleration bias.
Definition ManifoldPreintegration.h:43
│ │ │ │ +
gtsam::ManifoldPreintegration::deltaXij_
NavState deltaXij_
Pre-integrated navigation state, from frame i to frame j Note: relative position does not take into a...
Definition ManifoldPreintegration.h:41
│ │ │ │ +
gtsam::ManifoldPreintegration::delPdelBiasOmega_
Matrix3 delPdelBiasOmega_
Jacobian of preintegrated position w.r.t. angular rate bias.
Definition ManifoldPreintegration.h:44
│ │ │ │ +
gtsam::ManifoldPreintegration::clone
virtual boost::shared_ptr< ManifoldPreintegration > clone() const
Dummy clone for MATLAB.
Definition ManifoldPreintegration.h:109
│ │ │ │ +
gtsam::ManifoldPreintegration::delVdelBiasOmega_
Matrix3 delVdelBiasOmega_
Jacobian of preintegrated velocity w.r.t. angular rate bias.
Definition ManifoldPreintegration.h:46
│ │ │ │
gtsam::NavState
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition NavState.h:34
│ │ │ │ -
gtsam::NavState::NavState
NavState()
Default constructor.
Definition NavState.h:55
│ │ │ │ -
gtsam::NavState::NavState
NavState(const Matrix3 &R, const Vector6 &tv)
Construct from SO(3) and R^6.
Definition NavState.h:67
│ │ │ │ -
gtsam::NavState::NavState
NavState(const Rot3 &R, const Point3 &t, const Velocity3 &v)
Construct from attitude, position, velocity.
Definition NavState.h:59
│ │ │ │ -
gtsam::NavState::R
Matrix3 R() const
Return rotation matrix. Induces computation in quaternion mode.
Definition NavState.h:95
│ │ │ │ -
gtsam::NavState::NavState
NavState(const Pose3 &pose, const Velocity3 &v)
Construct from pose and velocity.
Definition NavState.h:63
│ │ │ │ -
gtsam::NavState::v
const Vector3 & v() const
Return velocity as Vector3. Computation-free.
Definition NavState.h:107
│ │ │ │ -
gtsam::NavState::t
Vector3 t() const
Return position as Vector3.
Definition NavState.h:103
│ │ │ │ -
gtsam::NavState::quaternion
Quaternion quaternion() const
Return quaternion. Induces computation in matrix mode.
Definition NavState.h:99
│ │ │ │ +
gtsam::PreintegrationBase
PreintegrationBase is the base class for PreintegratedMeasurements (in ImuFactor) and CombinedPreinte...
Definition PreintegrationBase.h:41
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,253 +1,148 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -NavState.h │ │ │ │ │ +ManifoldPreintegration.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_3_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_c_t_o_r_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ -24 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ +22#pragma once │ │ │ │ │ +23 │ │ │ │ │ +24#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_N_a_v_S_t_a_t_e_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_._h> │ │ │ │ │ 26 │ │ │ │ │ -_2_8typedef Vector3 _V_e_l_o_c_i_t_y_3; │ │ │ │ │ -29 │ │ │ │ │ -_3_4class GTSAM_EXPORT _N_a_v_S_t_a_t_e { │ │ │ │ │ -35private: │ │ │ │ │ -36 │ │ │ │ │ -37 // TODO(frank): │ │ │ │ │ -38 // - should we rename t_ to p_? if not, we should rename dP do dT │ │ │ │ │ -39 _R_o_t_3 R_; │ │ │ │ │ -40 _P_o_i_n_t_3 t_; │ │ │ │ │ -41 _V_e_l_o_c_i_t_y_3 v_; │ │ │ │ │ -42 │ │ │ │ │ -43public: │ │ │ │ │ -44 │ │ │ │ │ -45 enum { │ │ │ │ │ -46 dimension = 9 │ │ │ │ │ -47 }; │ │ │ │ │ -48 │ │ │ │ │ -49 typedef std::pair PositionAndVelocity; │ │ │ │ │ -50 │ │ │ │ │ -53 │ │ │ │ │ -_5_5 _N_a_v_S_t_a_t_e() : │ │ │ │ │ -56 t_(0, 0, 0), v_(Vector3::Zero()) { │ │ │ │ │ -57 } │ │ │ │ │ -_5_9 _N_a_v_S_t_a_t_e(const _R_o_t_3& R, const _P_o_i_n_t_3& t, const _V_e_l_o_c_i_t_y_3& v) : │ │ │ │ │ -60 R_(R), t_(t), v_(v) { │ │ │ │ │ -61 } │ │ │ │ │ -_6_3 _N_a_v_S_t_a_t_e(const _P_o_s_e_3& pose, const _V_e_l_o_c_i_t_y_3& v) : │ │ │ │ │ -64 R_(pose.rotation()), t_(pose.translation()), v_(v) { │ │ │ │ │ -65 } │ │ │ │ │ -_6_7 _N_a_v_S_t_a_t_e(const Matrix3& R, const Vector6& tv) : │ │ │ │ │ -68 R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) { │ │ │ │ │ -69 } │ │ │ │ │ -71 static _N_a_v_S_t_a_t_e Create(const _R_o_t_3& R, const _P_o_i_n_t_3& t, const _V_e_l_o_c_i_t_y_3& v, │ │ │ │ │ -72 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _3_> H1, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _3_> H2, │ │ │ │ │ -73 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _3_> H3); │ │ │ │ │ -75 static _N_a_v_S_t_a_t_e FromPoseVelocity(const _P_o_s_e_3& pose, const Vector3& vel, │ │ │ │ │ -76 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _6_> H1, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _3_> H2); │ │ │ │ │ -77 │ │ │ │ │ -81 │ │ │ │ │ -82 const _R_o_t_3& attitude(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _9_> H = boost::none) const; │ │ │ │ │ -83 const _P_o_i_n_t_3& position(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _9_> H = boost::none) const; │ │ │ │ │ -84 const _V_e_l_o_c_i_t_y_3& velocity(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _9_> H = boost::none) const; │ │ │ │ │ -85 │ │ │ │ │ -86 const _P_o_s_e_3 pose() const { │ │ │ │ │ -87 return _P_o_s_e_3(attitude(), position()); │ │ │ │ │ -88 } │ │ │ │ │ -89 │ │ │ │ │ -93 │ │ │ │ │ -_9_5 Matrix3 _R() const { │ │ │ │ │ -96 return R_._m_a_t_r_i_x(); │ │ │ │ │ -97 } │ │ │ │ │ -_9_9 Quaternion _q_u_a_t_e_r_n_i_o_n() const { │ │ │ │ │ -100 return R_._t_o_Q_u_a_t_e_r_n_i_o_n(); │ │ │ │ │ -101 } │ │ │ │ │ -_1_0_3 Vector3 _t() const { │ │ │ │ │ -104 return t_; │ │ │ │ │ -105 } │ │ │ │ │ -_1_0_7 const Vector3& _v() const { │ │ │ │ │ -108 return v_; │ │ │ │ │ -109 } │ │ │ │ │ -110 // Return velocity in body frame │ │ │ │ │ -111 _V_e_l_o_c_i_t_y_3 bodyVelocity(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _9_> H = boost::none) const; │ │ │ │ │ +27namespace _g_t_s_a_m { │ │ │ │ │ +28 │ │ │ │ │ +_3_3class GTSAM_EXPORT _M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n : public _P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e { │ │ │ │ │ +34 protected: │ │ │ │ │ +35 │ │ │ │ │ +_4_1 _N_a_v_S_t_a_t_e _d_e_l_t_a_X_i_j__; │ │ │ │ │ +_4_2 Matrix3 _d_e_l_R_d_e_l_B_i_a_s_O_m_e_g_a__; │ │ │ │ │ +_4_3 Matrix3 _d_e_l_P_d_e_l_B_i_a_s_A_c_c__; │ │ │ │ │ +_4_4 Matrix3 _d_e_l_P_d_e_l_B_i_a_s_O_m_e_g_a__; │ │ │ │ │ +_4_5 Matrix3 _d_e_l_V_d_e_l_B_i_a_s_A_c_c__; │ │ │ │ │ +_4_6 Matrix3 _d_e_l_V_d_e_l_B_i_a_s_O_m_e_g_a__; │ │ │ │ │ +47 │ │ │ │ │ +_4_9 _M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n() { │ │ │ │ │ +50 resetIntegration(); │ │ │ │ │ +51 } │ │ │ │ │ +52 │ │ │ │ │ +53public: │ │ │ │ │ +56 │ │ │ │ │ +62 _M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n(const boost::shared_ptr& p, │ │ │ │ │ +63 const _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s& biasHat = _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s()); │ │ │ │ │ +64 │ │ │ │ │ +66 │ │ │ │ │ +70 void resetIntegration() override; │ │ │ │ │ +71 │ │ │ │ │ +73 │ │ │ │ │ +76 _N_a_v_S_t_a_t_e deltaXij() const override { return deltaXij_; } │ │ │ │ │ +77 Rot3 deltaRij() const override { return deltaXij_.attitude(); } │ │ │ │ │ +78 Vector3 deltaPij() const override { return deltaXij_.position(); } │ │ │ │ │ +79 Vector3 deltaVij() const override { return deltaXij_.velocity(); } │ │ │ │ │ +80 │ │ │ │ │ +81 Matrix3 delRdelBiasOmega() const { return delRdelBiasOmega_; } │ │ │ │ │ +82 Matrix3 delPdelBiasAcc() const { return delPdelBiasAcc_; } │ │ │ │ │ +83 Matrix3 delPdelBiasOmega() const { return delPdelBiasOmega_; } │ │ │ │ │ +84 Matrix3 delVdelBiasAcc() const { return delVdelBiasAcc_; } │ │ │ │ │ +85 Matrix3 delVdelBiasOmega() const { return delVdelBiasOmega_; } │ │ │ │ │ +86 │ │ │ │ │ +89 bool equals(const ManifoldPreintegration& other, double tol) const; │ │ │ │ │ +91 │ │ │ │ │ +94 │ │ │ │ │ +99 void update(const Vector3& measuredAcc, const Vector3& measuredOmega, const │ │ │ │ │ +double dt, │ │ │ │ │ +100 Matrix9* A, Matrix93* B, Matrix93* C) override; │ │ │ │ │ +101 │ │ │ │ │ +105 Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, │ │ │ │ │ +106 OptionalJacobian<9, 6> H = boost::none) const override; │ │ │ │ │ +107 │ │ │ │ │ +_1_0_9 virtual boost::shared_ptr _c_l_o_n_e() const { │ │ │ │ │ +110 return boost::shared_ptr(); │ │ │ │ │ +111 } │ │ │ │ │ 112 │ │ │ │ │ -116 Matrix7 matrix() const; │ │ │ │ │ -117 │ │ │ │ │ -121 │ │ │ │ │ -123 GTSAM_EXPORT │ │ │ │ │ -124 friend std::ostream &operator<<(std::ostream &os, const _N_a_v_S_t_a_t_e& state); │ │ │ │ │ -125 │ │ │ │ │ -127 void _p_r_i_n_t(const std::string& s = "") const; │ │ │ │ │ -128 │ │ │ │ │ -130 bool _e_q_u_a_l_s(const _N_a_v_S_t_a_t_e& other, double tol = 1e-8) const; │ │ │ │ │ -131 │ │ │ │ │ -135 │ │ │ │ │ -136 // Tangent space sugar. │ │ │ │ │ -137 // TODO(frank): move to private navstate namespace in cpp │ │ │ │ │ -138 static Eigen::Block dR(Vector9& v) { │ │ │ │ │ -139 return v.segment<3>(0); │ │ │ │ │ -140 } │ │ │ │ │ -141 static Eigen::Block dP(Vector9& v) { │ │ │ │ │ -142 return v.segment<3>(3); │ │ │ │ │ -143 } │ │ │ │ │ -144 static Eigen::Block dV(Vector9& v) { │ │ │ │ │ -145 return v.segment<3>(6); │ │ │ │ │ -146 } │ │ │ │ │ -147 static Eigen::Block dR(const Vector9& v) { │ │ │ │ │ -148 return v.segment<3>(0); │ │ │ │ │ -149 } │ │ │ │ │ -150 static Eigen::Block dP(const Vector9& v) { │ │ │ │ │ -151 return v.segment<3>(3); │ │ │ │ │ -152 } │ │ │ │ │ -153 static Eigen::Block dV(const Vector9& v) { │ │ │ │ │ -154 return v.segment<3>(6); │ │ │ │ │ -155 } │ │ │ │ │ -156 │ │ │ │ │ -158 NavState retract(const Vector9& v, // │ │ │ │ │ -159 OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 = │ │ │ │ │ -160 boost::none) const; │ │ │ │ │ -161 │ │ │ │ │ -163 Vector9 localCoordinates(const NavState& g, // │ │ │ │ │ -164 OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 = │ │ │ │ │ -165 boost::none) const; │ │ │ │ │ -166 │ │ │ │ │ -170 │ │ │ │ │ -173 NavState update(const Vector3& b_acceleration, const Vector3& b_omega, │ │ │ │ │ -174 const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, │ │ │ │ │ -175 OptionalJacobian<9, 3> G2) const; │ │ │ │ │ -176 │ │ │ │ │ -178 Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false, │ │ │ │ │ -179 OptionalJacobian<9, 9> H = boost::none) const; │ │ │ │ │ -180 │ │ │ │ │ -183 Vector9 correctPIM(const Vector9& pim, double dt, const Vector3& n_gravity, │ │ │ │ │ -184 const boost::optional& omegaCoriolis, bool use2ndOrderCoriolis = │ │ │ │ │ -185 false, OptionalJacobian<9, 9> H1 = boost::none, │ │ │ │ │ -186 OptionalJacobian<9, 9> H2 = boost::none) const; │ │ │ │ │ -187 │ │ │ │ │ -189 │ │ │ │ │ -190private: │ │ │ │ │ -_1_9_3 friend class boost::serialization::access; │ │ │ │ │ -194 template │ │ │ │ │ -195 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -196 ar & BOOST_SERIALIZATION_NVP(R_); │ │ │ │ │ -197 ar & BOOST_SERIALIZATION_NVP(t_); │ │ │ │ │ -198 ar & BOOST_SERIALIZATION_NVP(v_); │ │ │ │ │ -199 } │ │ │ │ │ -201}; │ │ │ │ │ -202 │ │ │ │ │ -203// Specialize NavState traits to use a Retract/Local that agrees with │ │ │ │ │ -IMUFactors │ │ │ │ │ -204template<> │ │ │ │ │ -_2_0_5struct _t_r_a_i_t_s<_N_a_v_S_t_a_t_e> : _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d { │ │ │ │ │ -206}; │ │ │ │ │ -207 │ │ │ │ │ -208} // namespace gtsam │ │ │ │ │ -_V_e_c_t_o_r_._h │ │ │ │ │ -typedef and functions to augment Eigen's VectorXd │ │ │ │ │ -_M_a_n_i_f_o_l_d_._h │ │ │ │ │ -Base class and basic functions for Manifold types. │ │ │ │ │ -_P_o_s_e_3_._h │ │ │ │ │ -3D Pose │ │ │ │ │ +114 │ │ │ │ │ +115private: │ │ │ │ │ +_1_1_7 friend class boost::serialization::access; │ │ │ │ │ +118 template │ │ │ │ │ +119 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +120 namespace bs = ::boost::serialization; │ │ │ │ │ +121 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e); │ │ │ │ │ +122 ar & BOOST_SERIALIZATION_NVP(deltaXij_); │ │ │ │ │ +123 ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); │ │ │ │ │ +124 ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); │ │ │ │ │ +125 ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); │ │ │ │ │ +126 ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); │ │ │ │ │ +127 ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); │ │ │ │ │ +128 } │ │ │ │ │ +129}; │ │ │ │ │ +130 │ │ │ │ │ +131} │ │ │ │ │ +_N_a_v_S_t_a_t_e_._h │ │ │ │ │ +Navigation state composing of attitude, position, and velocity. │ │ │ │ │ +_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ -Vector3 Point3 │ │ │ │ │ -As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point3 to Vector3... │ │ │ │ │ -DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_l_o_c_i_t_y_3 │ │ │ │ │ -Vector3 Velocity3 │ │ │ │ │ -Velocity is currently typedef'd to Vector3. │ │ │ │ │ -DDeeffiinniittiioonn NavState.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ -Both ManifoldTraits and Testable. │ │ │ │ │ -DDeeffiinniittiioonn Manifold.h:120 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ -either a fixed size o... │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ -A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ -Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor │ │ │ │ │ -symbol GTSAM_USE_QUATERNIO... │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_t_o_Q_u_a_t_e_r_n_i_o_n │ │ │ │ │ -gtsam::Quaternion toQuaternion() const │ │ │ │ │ -Compute the quaternion representation of this rotation. │ │ │ │ │ -DDeeffiinniittiioonn Rot3M.cpp:233 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_m_a_t_r_i_x │ │ │ │ │ -Matrix3 matrix() const │ │ │ │ │ -return 3*3 rotation matrix │ │ │ │ │ -DDeeffiinniittiioonn Rot3M.cpp:219 │ │ │ │ │ +_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s │ │ │ │ │ +DDeeffiinniittiioonn ImuBias.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n │ │ │ │ │ +IMU pre-integration on NavSatet manifold. │ │ │ │ │ +DDeeffiinniittiioonn ManifoldPreintegration.h:33 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_:_:_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n │ │ │ │ │ +ManifoldPreintegration() │ │ │ │ │ +Default constructor for serialization. │ │ │ │ │ +DDeeffiinniittiioonn ManifoldPreintegration.h:49 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_:_:_d_e_l_V_d_e_l_B_i_a_s_A_c_c__ │ │ │ │ │ +Matrix3 delVdelBiasAcc_ │ │ │ │ │ +Jacobian of preintegrated velocity w.r.t. acceleration bias. │ │ │ │ │ +DDeeffiinniittiioonn ManifoldPreintegration.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_:_:_d_e_l_R_d_e_l_B_i_a_s_O_m_e_g_a__ │ │ │ │ │ +Matrix3 delRdelBiasOmega_ │ │ │ │ │ +Jacobian of preintegrated rotation w.r.t. angular rate bias. │ │ │ │ │ +DDeeffiinniittiioonn ManifoldPreintegration.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_:_:_d_e_l_P_d_e_l_B_i_a_s_A_c_c__ │ │ │ │ │ +Matrix3 delPdelBiasAcc_ │ │ │ │ │ +Jacobian of preintegrated position w.r.t. acceleration bias. │ │ │ │ │ +DDeeffiinniittiioonn ManifoldPreintegration.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_:_:_d_e_l_t_a_X_i_j__ │ │ │ │ │ +NavState deltaXij_ │ │ │ │ │ +Pre-integrated navigation state, from frame i to frame j Note: relative │ │ │ │ │ +position does not take into a... │ │ │ │ │ +DDeeffiinniittiioonn ManifoldPreintegration.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_:_:_d_e_l_P_d_e_l_B_i_a_s_O_m_e_g_a__ │ │ │ │ │ +Matrix3 delPdelBiasOmega_ │ │ │ │ │ +Jacobian of preintegrated position w.r.t. angular rate bias. │ │ │ │ │ +DDeeffiinniittiioonn ManifoldPreintegration.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_:_:_c_l_o_n_e │ │ │ │ │ +virtual boost::shared_ptr< ManifoldPreintegration > clone() const │ │ │ │ │ +Dummy clone for MATLAB. │ │ │ │ │ +DDeeffiinniittiioonn ManifoldPreintegration.h:109 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_:_:_d_e_l_V_d_e_l_B_i_a_s_O_m_e_g_a__ │ │ │ │ │ +Matrix3 delVdelBiasOmega_ │ │ │ │ │ +Jacobian of preintegrated velocity w.r.t. angular rate bias. │ │ │ │ │ +DDeeffiinniittiioonn ManifoldPreintegration.h:46 │ │ │ │ │ _g_t_s_a_m_:_:_N_a_v_S_t_a_t_e │ │ │ │ │ Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does │ │ │ │ │ not make sense to make... │ │ │ │ │ DDeeffiinniittiioonn NavState.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_N_a_v_S_t_a_t_e_:_:_N_a_v_S_t_a_t_e │ │ │ │ │ -NavState() │ │ │ │ │ -Default constructor. │ │ │ │ │ -DDeeffiinniittiioonn NavState.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_N_a_v_S_t_a_t_e_:_:_N_a_v_S_t_a_t_e │ │ │ │ │ -NavState(const Matrix3 &R, const Vector6 &tv) │ │ │ │ │ -Construct from SO(3) and R^6. │ │ │ │ │ -DDeeffiinniittiioonn NavState.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_N_a_v_S_t_a_t_e_:_:_N_a_v_S_t_a_t_e │ │ │ │ │ -NavState(const Rot3 &R, const Point3 &t, const Velocity3 &v) │ │ │ │ │ -Construct from attitude, position, velocity. │ │ │ │ │ -DDeeffiinniittiioonn NavState.h:59 │ │ │ │ │ -_g_t_s_a_m_:_:_N_a_v_S_t_a_t_e_:_:_R │ │ │ │ │ -Matrix3 R() const │ │ │ │ │ -Return rotation matrix. Induces computation in quaternion mode. │ │ │ │ │ -DDeeffiinniittiioonn NavState.h:95 │ │ │ │ │ -_g_t_s_a_m_:_:_N_a_v_S_t_a_t_e_:_:_N_a_v_S_t_a_t_e │ │ │ │ │ -NavState(const Pose3 &pose, const Velocity3 &v) │ │ │ │ │ -Construct from pose and velocity. │ │ │ │ │ -DDeeffiinniittiioonn NavState.h:63 │ │ │ │ │ -_g_t_s_a_m_:_:_N_a_v_S_t_a_t_e_:_:_v │ │ │ │ │ -const Vector3 & v() const │ │ │ │ │ -Return velocity as Vector3. Computation-free. │ │ │ │ │ -DDeeffiinniittiioonn NavState.h:107 │ │ │ │ │ -_g_t_s_a_m_:_:_N_a_v_S_t_a_t_e_:_:_t │ │ │ │ │ -Vector3 t() const │ │ │ │ │ -Return position as Vector3. │ │ │ │ │ -DDeeffiinniittiioonn NavState.h:103 │ │ │ │ │ -_g_t_s_a_m_:_:_N_a_v_S_t_a_t_e_:_:_q_u_a_t_e_r_n_i_o_n │ │ │ │ │ -Quaternion quaternion() const │ │ │ │ │ -Return quaternion. Induces computation in matrix mode. │ │ │ │ │ -DDeeffiinniittiioonn NavState.h:99 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e │ │ │ │ │ +PreintegrationBase is the base class for PreintegratedMeasurements (in │ │ │ │ │ +ImuFactor) and CombinedPreinte... │ │ │ │ │ +DDeeffiinniittiioonn PreintegrationBase.h:41 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _N_a_v_S_t_a_t_e_._h │ │ │ │ │ + * _M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00968.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/PreintegratedRotation.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ImuFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,24 +94,34 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces
│ │ │ │ -
PreintegratedRotation.cpp File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
ImuFactor.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +std::ostream & gtsam::operator<< (std::ostream &os, const ImuFactor &f)
 
│ │ │ │ +std::ostream & gtsam::operator<< (std::ostream &os, const ImuFactor2 &f)
 
│ │ │ │

Detailed Description

│ │ │ │
Author
Luca Carlone
│ │ │ │
│ │ │ │ Stephen Williams
│ │ │ │
│ │ │ │ Richard Roberts
│ │ │ │ @@ -122,13 +132,13 @@ │ │ │ │
│ │ │ │ Frank Dellaert
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -PreintegratedRotation.cpp File Reference │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +ImuFactor.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _I_m_u_F_a_c_t_o_r &f) │ │ │ │ │ +  │ │ │ │ │ +std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _I_m_u_F_a_c_t_o_r_2 &f) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Author │ │ │ │ │ Luca Carlone │ │ │ │ │ Stephen Williams │ │ │ │ │ Richard Roberts │ │ │ │ │ Vadim Indelman │ │ │ │ │ David Jensen │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_._c_p_p │ │ │ │ │ + * _I_m_u_F_a_c_t_o_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00971.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/GPSFactor.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ConstantVelocityFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,36 +94,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
GPSFactor.cpp File Reference
│ │ │ │ +
ConstantVelocityFactor.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Implementation file for GPS factor. │ │ │ │ +

Maintain a constant velocity motion model between two NavStates. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::ConstantVelocityFactor
 Binary factor for applying a constant velocity model to a moving body represented as a NavState. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Implementation file for GPS factor.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
Date
January 28, 2014
│ │ │ │ +

Maintain a constant velocity motion model between two NavStates.

│ │ │ │ +
Author
Asa Hammond
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,26 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -GPSFactor.cpp File Reference │ │ │ │ │ -Implementation file for GPS factor. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +ConstantVelocityFactor.h File Reference │ │ │ │ │ +Maintain a constant velocity motion model between two NavStates. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_C_o_n_s_t_a_n_t_V_e_l_o_c_i_t_y_F_a_c_t_o_r │ │ │ │ │ +  Binary factor for applying a constant velocity model to a moving body │ │ │ │ │ + represented as a _N_a_v_S_t_a_t_e. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Implementation file for GPS factor. │ │ │ │ │ +Maintain a constant velocity motion model between two NavStates. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - January 28, 2014 │ │ │ │ │ + Asa Hammond │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _G_P_S_F_a_c_t_o_r_._c_p_p │ │ │ │ │ + * _C_o_n_s_t_a_n_t_V_e_l_o_c_i_t_y_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00974.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AttitudeFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/BarometricFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,57 +94,36 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
AttitudeFactor.h File Reference
│ │ │ │ +
BarometricFactor.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Header file for Attitude factor. │ │ │ │ +

Implementation file for Barometric factor. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::AttitudeFactor
 Base class for prior on attitude Example: More...
 
class  gtsam::Rot3AttitudeFactor
 Version of AttitudeFactor for Rot3. More...
 
struct  gtsam::traits< Rot3AttitudeFactor >
 traits More...
 
class  gtsam::Pose3AttitudeFactor
 Version of AttitudeFactor for Pose3. More...
 
struct  gtsam::traits< Pose3AttitudeFactor >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Header file for Attitude factor.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
Date
January 28, 2014
│ │ │ │ +

Implementation file for Barometric factor.

│ │ │ │ +
Author
Peter Milani
│ │ │ │ +
Date
December 16, 2021
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,39 +1,22 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -AttitudeFactor.h File Reference │ │ │ │ │ -Header file for Attitude factor. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_A_t_t_i_t_u_d_e_F_a_c_t_o_r │ │ │ │ │ -  Base class for prior on attitude Example: _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r │ │ │ │ │ -  Version of _A_t_t_i_t_u_d_e_F_a_c_t_o_r for _R_o_t_3. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _R_o_t_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_P_o_s_e_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r │ │ │ │ │ -  Version of _A_t_t_i_t_u_d_e_F_a_c_t_o_r for _P_o_s_e_3. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _P_o_s_e_3_A_t_t_i_t_u_d_e_F_a_c_t_o_r_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +BarometricFactor.cpp File Reference │ │ │ │ │ +Implementation file for Barometric factor. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Header file for Attitude factor. │ │ │ │ │ +Implementation file for Barometric factor. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Peter Milani │ │ │ │ │ Date │ │ │ │ │ - January 28, 2014 │ │ │ │ │ + December 16, 2021 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _A_t_t_i_t_u_d_e_F_a_c_t_o_r_._h │ │ │ │ │ + * _B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00977_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/TangentPreintegration.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/MagPoseFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,138 +98,167 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
TangentPreintegration.h
│ │ │ │ +
MagPoseFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/navigation/PreintegrationBase.h>
│ │ │ │ -
21
│ │ │ │ -
22namespace gtsam {
│ │ │ │ -
23
│ │ │ │ +
12#pragma once
│ │ │ │ +
13
│ │ │ │ +
14#include <gtsam/geometry/concepts.h>
│ │ │ │ +
15#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
16
│ │ │ │ +
17namespace gtsam {
│ │ │ │ +
18
│ │ │ │ +
27template <class POSE>
│ │ │ │
│ │ │ │ -
28class GTSAM_EXPORT TangentPreintegration : public PreintegrationBase {
│ │ │ │ -
29 protected:
│ │ │ │ -
30
│ │ │ │ -
35 Vector9 preintegrated_;
│ │ │ │ -
36 Matrix93 preintegrated_H_biasAcc_;
│ │ │ │ -
37 Matrix93 preintegrated_H_biasOmega_;
│ │ │ │ -
38
│ │ │ │ -
│ │ │ │ -
40 TangentPreintegration() {
│ │ │ │ -
41 resetIntegration();
│ │ │ │ -
42 }
│ │ │ │ -
│ │ │ │ +
28class MagPoseFactor: public NoiseModelFactorN<POSE> {
│ │ │ │ +
29 private:
│ │ │ │ +
30 using This = MagPoseFactor<POSE>;
│ │ │ │ +
31 using Base = NoiseModelFactorN<POSE>;
│ │ │ │ +
32 using Point = typename POSE::Translation;
│ │ │ │ +
33 using Rot = typename POSE::Rotation;
│ │ │ │ +
34
│ │ │ │ +
35 const Point measured_;
│ │ │ │ +
36 const Point nM_;
│ │ │ │ +
37 const Point bias_;
│ │ │ │ +
38 boost::optional<POSE> body_P_sensor_;
│ │ │ │ +
39
│ │ │ │ +
40 static const int MeasDim = Point::RowsAtCompileTime;
│ │ │ │ +
41 static const int PoseDim = traits<POSE>::dimension;
│ │ │ │ +
42 static const int RotDim = traits<Rot>::dimension;
│ │ │ │
43
│ │ │ │ -
44public:
│ │ │ │ -
47
│ │ │ │ -
53 TangentPreintegration(const boost::shared_ptr<Params>& p,
│ │ │ │ -
54 const imuBias::ConstantBias& biasHat = imuBias::ConstantBias());
│ │ │ │ -
55
│ │ │ │ -
│ │ │ │ -
57 ~TangentPreintegration() override {
│ │ │ │ -
58 }
│ │ │ │ -
│ │ │ │ -
59
│ │ │ │ -
61
│ │ │ │ -
65 void resetIntegration() override;
│ │ │ │ -
66
│ │ │ │ -
68
│ │ │ │ -
71 Vector3 deltaPij() const override { return preintegrated_.segment<3>(3); }
│ │ │ │ -
72 Vector3 deltaVij() const override { return preintegrated_.tail<3>(); }
│ │ │ │ -
73 Rot3 deltaRij() const override { return Rot3::Expmap(theta()); }
│ │ │ │ -
74 NavState deltaXij() const override { return NavState().retract(preintegrated_); }
│ │ │ │ -
75
│ │ │ │ -
76 const Vector9& preintegrated() const { return preintegrated_; }
│ │ │ │ -
77 Vector3 theta() const { return preintegrated_.head<3>(); }
│ │ │ │ -
78 const Matrix93& preintegrated_H_biasAcc() const { return preintegrated_H_biasAcc_; }
│ │ │ │ -
79 const Matrix93& preintegrated_H_biasOmega() const { return preintegrated_H_biasOmega_; }
│ │ │ │ -
80
│ │ │ │ -
83 bool equals(const TangentPreintegration& other, double tol) const;
│ │ │ │ +
45 using shared_ptr = boost::shared_ptr<MagPoseFactor<POSE>>;
│ │ │ │ +
46
│ │ │ │ +
48 GTSAM_CONCEPT_TESTABLE_TYPE(POSE)
│ │ │ │ +
49 GTSAM_CONCEPT_POSE_TYPE(POSE)
│ │ │ │ +
50
│ │ │ │ +
51 public:
│ │ │ │ +
52 ~MagPoseFactor() override {}
│ │ │ │ +
53
│ │ │ │ +
55 MagPoseFactor() {}
│ │ │ │ +
56
│ │ │ │ +
│ │ │ │ +
67 MagPoseFactor(Key pose_key,
│ │ │ │ +
68 const Point& measured,
│ │ │ │ +
69 double scale,
│ │ │ │ +
70 const Point& direction,
│ │ │ │ +
71 const Point& bias,
│ │ │ │ +
72 const SharedNoiseModel& model,
│ │ │ │ +
73 const boost::optional<POSE>& body_P_sensor)
│ │ │ │ +
74 : Base(model, pose_key),
│ │ │ │ +
75 measured_(body_P_sensor ? body_P_sensor->rotation() * measured : measured),
│ │ │ │ +
76 nM_(scale * direction.normalized()),
│ │ │ │ +
77 bias_(body_P_sensor ? body_P_sensor->rotation() * bias : bias),
│ │ │ │ +
78 body_P_sensor_(body_P_sensor) {}
│ │ │ │ +
│ │ │ │ +
79
│ │ │ │ +
│ │ │ │ +
81 NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
82 return boost::static_pointer_cast<NonlinearFactor>(
│ │ │ │ +
83 NonlinearFactor::shared_ptr(new This(*this)));
│ │ │ │ +
84 }
│ │ │ │ +
│ │ │ │
85
│ │ │ │ -
88
│ │ │ │ -
89 // Update integrated vector on tangent manifold preintegrated with acceleration
│ │ │ │ -
90 // Static, functional version.
│ │ │ │ -
91 static Vector9 UpdatePreintegrated(const Vector3& a_body,
│ │ │ │ -
92 const Vector3& w_body, const double dt,
│ │ │ │ -
93 const Vector9& preintegrated,
│ │ │ │ -
94 OptionalJacobian<9, 9> A = boost::none,
│ │ │ │ -
95 OptionalJacobian<9, 3> B = boost::none,
│ │ │ │ -
96 OptionalJacobian<9, 3> C = boost::none);
│ │ │ │ -
97
│ │ │ │ -
102 void update(const Vector3& measuredAcc, const Vector3& measuredOmega,
│ │ │ │ -
103 const double dt, Matrix9* A, Matrix93* B, Matrix93* C) override;
│ │ │ │ +
87
│ │ │ │ +
88 // Print out the factor.
│ │ │ │ +
│ │ │ │ +
89 void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ +
90 Base::print(s, keyFormatter);
│ │ │ │ +
91 gtsam::print(Vector(nM_), "local field (nM): ");
│ │ │ │ +
92 gtsam::print(Vector(measured_), "measured field (bM): ");
│ │ │ │ +
93 gtsam::print(Vector(bias_), "magnetometer bias: ");
│ │ │ │ +
94 }
│ │ │ │ +
│ │ │ │ +
95
│ │ │ │ +
│ │ │ │ +
97 bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
│ │ │ │ +
98 const This *e = dynamic_cast<const This*> (&expected);
│ │ │ │ +
99 return e != nullptr && Base::equals(*e, tol) &&
│ │ │ │ +
100 gtsam::equal_with_abs_tol(this->measured_, e->measured_, tol) &&
│ │ │ │ +
101 gtsam::equal_with_abs_tol(this->nM_, e->nM_, tol) &&
│ │ │ │ +
102 gtsam::equal_with_abs_tol(this->bias_, e->bias_, tol);
│ │ │ │ +
103 }
│ │ │ │ +
│ │ │ │
104
│ │ │ │ -
108 Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
│ │ │ │ -
109 OptionalJacobian<9, 6> H = boost::none) const override;
│ │ │ │ -
110
│ │ │ │ -
111 // Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with derivatives
│ │ │ │ -
112 static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12,
│ │ │ │ -
113 double deltaT12,
│ │ │ │ -
114 OptionalJacobian<9, 9> H1 = boost::none,
│ │ │ │ -
115 OptionalJacobian<9, 9> H2 = boost::none);
│ │ │ │ +
106
│ │ │ │ +
│ │ │ │ +
111 Vector evaluateError(const POSE& nPb, boost::optional<Matrix&> H = boost::none) const override {
│ │ │ │ +
112 // Predict the measured magnetic field h(x) in the *body* frame.
│ │ │ │ +
113 // If body_P_sensor was given, bias_ will have been rotated into the body frame.
│ │ │ │ +
114 Matrix H_rot = Matrix::Zero(MeasDim, RotDim);
│ │ │ │ +
115 const Point hx = nPb.rotation().unrotate(nM_, H_rot, boost::none) + bias_;
│ │ │ │
116
│ │ │ │ -
119 void mergeWith(const TangentPreintegration& pim, Matrix9* H1, Matrix9* H2);
│ │ │ │ -
121
│ │ │ │ -
│ │ │ │ -
123 virtual boost::shared_ptr<TangentPreintegration> clone() const {
│ │ │ │ -
124 return boost::shared_ptr<TangentPreintegration>();
│ │ │ │ +
117 if (H) {
│ │ │ │ +
118 // Fill in the relevant part of the Jacobian (just rotation columns).
│ │ │ │ +
119 *H = Matrix::Zero(MeasDim, PoseDim);
│ │ │ │ +
120 const size_t rot_col0 = nPb.rotationInterval().first;
│ │ │ │ +
121 (*H).block(0, rot_col0, MeasDim, RotDim) = H_rot;
│ │ │ │ +
122 }
│ │ │ │ +
123
│ │ │ │ +
124 return (hx - measured_);
│ │ │ │
125 }
│ │ │ │
│ │ │ │
126
│ │ │ │ -
128
│ │ │ │ -
129private:
│ │ │ │ -
131 friend class boost::serialization::access;
│ │ │ │ -
132 template<class ARCHIVE>
│ │ │ │ -
133 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
134 namespace bs = ::boost::serialization;
│ │ │ │ -
135 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
│ │ │ │ -
136 ar & BOOST_SERIALIZATION_NVP(preintegrated_);
│ │ │ │ -
137 ar & BOOST_SERIALIZATION_NVP(preintegrated_H_biasAcc_);
│ │ │ │ -
138 ar & BOOST_SERIALIZATION_NVP(preintegrated_H_biasOmega_);
│ │ │ │ +
127 private:
│ │ │ │ +
129 friend class boost::serialization::access;
│ │ │ │ +
130 template<class ARCHIVE>
│ │ │ │ +
131 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
132 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ +
133 ar & boost::serialization::make_nvp("NoiseModelFactor1",
│ │ │ │ +
134 boost::serialization::base_object<Base>(*this));
│ │ │ │ +
135 ar & BOOST_SERIALIZATION_NVP(measured_);
│ │ │ │ +
136 ar & BOOST_SERIALIZATION_NVP(nM_);
│ │ │ │ +
137 ar & BOOST_SERIALIZATION_NVP(bias_);
│ │ │ │ +
138 ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
│ │ │ │
139 }
│ │ │ │ -
140
│ │ │ │ -
141public:
│ │ │ │ -
142 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
143};
│ │ │ │ -
│ │ │ │ -
144
│ │ │ │ -
145}
│ │ │ │ -
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ - │ │ │ │ +
140}; // \class MagPoseFactor
│ │ │ │ +
│ │ │ │ +
141
│ │ │ │ +
142}
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::imuBias::ConstantBias
Definition ImuBias.h:30
│ │ │ │ -
gtsam::PreintegrationBase
PreintegrationBase is the base class for PreintegratedMeasurements (in ImuFactor) and CombinedPreinte...
Definition PreintegrationBase.h:41
│ │ │ │ -
gtsam::TangentPreintegration
Integrate on the 9D tangent space of the NavState manifold.
Definition TangentPreintegration.h:28
│ │ │ │ -
gtsam::TangentPreintegration::preintegrated_H_biasOmega_
Matrix93 preintegrated_H_biasOmega_
Jacobian of preintegrated_ w.r.t. angular rate bias.
Definition TangentPreintegration.h:37
│ │ │ │ -
gtsam::TangentPreintegration::~TangentPreintegration
~TangentPreintegration() override
Virtual destructor.
Definition TangentPreintegration.h:57
│ │ │ │ -
gtsam::TangentPreintegration::preintegrated_
Vector9 preintegrated_
Preintegrated navigation state, as a 9D vector on tangent space at frame i Order is: theta,...
Definition TangentPreintegration.h:35
│ │ │ │ -
gtsam::TangentPreintegration::TangentPreintegration
TangentPreintegration()
Default constructor for serialization.
Definition TangentPreintegration.h:40
│ │ │ │ -
gtsam::TangentPreintegration::preintegrated_H_biasAcc_
Matrix93 preintegrated_H_biasAcc_
Jacobian of preintegrated_ w.r.t. acceleration bias.
Definition TangentPreintegration.h:36
│ │ │ │ -
gtsam::TangentPreintegration::clone
virtual boost::shared_ptr< TangentPreintegration > clone() const
Dummy clone for MATLAB.
Definition TangentPreintegration.h:123
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition Matrix.h:81
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
│ │ │ │ +
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
│ │ │ │ +
gtsam::MagPoseFactor
Factor to estimate rotation of a Pose2 or Pose3 given a magnetometer reading.
Definition MagPoseFactor.h:28
│ │ │ │ +
gtsam::MagPoseFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Implement functions needed for Testable.
Definition MagPoseFactor.h:89
│ │ │ │ +
gtsam::MagPoseFactor::~MagPoseFactor
~MagPoseFactor() override
Concept check by type.
Definition MagPoseFactor.h:52
│ │ │ │ +
gtsam::MagPoseFactor::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
Equals function.
Definition MagPoseFactor.h:97
│ │ │ │ +
gtsam::MagPoseFactor::clone
NonlinearFactor::shared_ptr clone() const override
Definition MagPoseFactor.h:81
│ │ │ │ +
gtsam::MagPoseFactor::access
friend class boost::serialization::access
Serialization function.
Definition MagPoseFactor.h:129
│ │ │ │ +
gtsam::MagPoseFactor::MagPoseFactor
MagPoseFactor(Key pose_key, const Point &measured, double scale, const Point &direction, const Point &bias, const SharedNoiseModel &model, const boost::optional< POSE > &body_P_sensor)
Construct the factor.
Definition MagPoseFactor.h:67
│ │ │ │ +
gtsam::MagPoseFactor::evaluateError
Vector evaluateError(const POSE &nPb, boost::optional< Matrix & > H=boost::none) const override
Implement functions needed to derive from Factor.
Definition MagPoseFactor.h:111
│ │ │ │ +
gtsam::MagPoseFactor::MagPoseFactor
MagPoseFactor()
Default constructor - only use for serialization.
Definition MagPoseFactor.h:55
│ │ │ │ +
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ +
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ +
concepts.h
Concept-checking macros for geometric objects Each macro instantiates a concept check structure,...
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,157 +1,216 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -TangentPreintegration.h │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +MagPoseFactor.h │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_._h> │ │ │ │ │ -21 │ │ │ │ │ -22namespace _g_t_s_a_m { │ │ │ │ │ -23 │ │ │ │ │ -_2_8class GTSAM_EXPORT _T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n : public _P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e { │ │ │ │ │ -29 protected: │ │ │ │ │ -30 │ │ │ │ │ -_3_5 Vector9 _p_r_e_i_n_t_e_g_r_a_t_e_d__; │ │ │ │ │ -_3_6 Matrix93 _p_r_e_i_n_t_e_g_r_a_t_e_d___H___b_i_a_s_A_c_c__; │ │ │ │ │ -_3_7 Matrix93 _p_r_e_i_n_t_e_g_r_a_t_e_d___H___b_i_a_s_O_m_e_g_a__; │ │ │ │ │ -38 │ │ │ │ │ -_4_0 _T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n() { │ │ │ │ │ -41 resetIntegration(); │ │ │ │ │ -42 } │ │ │ │ │ +12#pragma once │ │ │ │ │ +13 │ │ │ │ │ +14#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_c_o_n_c_e_p_t_s_._h> │ │ │ │ │ +15#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +16 │ │ │ │ │ +17namespace _g_t_s_a_m { │ │ │ │ │ +18 │ │ │ │ │ +27template │ │ │ │ │ +_2_8class _M_a_g_P_o_s_e_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +29 private: │ │ │ │ │ +30 using This = _M_a_g_P_o_s_e_F_a_c_t_o_r_<_P_O_S_E_>; │ │ │ │ │ +31 using _B_a_s_e = _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_P_O_S_E_>; │ │ │ │ │ +32 using Point = typename POSE::Translation; │ │ │ │ │ +33 using Rot = typename POSE::Rotation; │ │ │ │ │ +34 │ │ │ │ │ +35 const Point measured_; │ │ │ │ │ +36 const Point nM_; │ │ │ │ │ +37 const Point bias_; │ │ │ │ │ +38 boost::optional body_P_sensor_; │ │ │ │ │ +39 │ │ │ │ │ +40 static const int MeasDim = Point::RowsAtCompileTime; │ │ │ │ │ +41 static const int PoseDim = _t_r_a_i_t_s_<_P_O_S_E_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ +42 static const int RotDim = _t_r_a_i_t_s_<_R_o_t_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ 43 │ │ │ │ │ -44public: │ │ │ │ │ -47 │ │ │ │ │ -53 _T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n(const boost::shared_ptr& p, │ │ │ │ │ -54 const _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s& biasHat = _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s()); │ │ │ │ │ -55 │ │ │ │ │ -_5_7 _~_T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n() override { │ │ │ │ │ -58 } │ │ │ │ │ -59 │ │ │ │ │ -61 │ │ │ │ │ -65 void resetIntegration() override; │ │ │ │ │ -66 │ │ │ │ │ -68 │ │ │ │ │ -71 Vector3 deltaPij() const override { return preintegrated_.segment<3>(3); } │ │ │ │ │ -72 Vector3 deltaVij() const override { return preintegrated_.tail<3>(); } │ │ │ │ │ -73 Rot3 deltaRij() const override { return Rot3::Expmap(theta()); } │ │ │ │ │ -74 NavState deltaXij() const override { return NavState().retract │ │ │ │ │ -(preintegrated_); } │ │ │ │ │ -75 │ │ │ │ │ -76 const Vector9& preintegrated() const { return preintegrated_; } │ │ │ │ │ -77 Vector3 theta() const { return preintegrated_.head<3>(); } │ │ │ │ │ -78 const Matrix93& preintegrated_H_biasAcc() const { return │ │ │ │ │ -preintegrated_H_biasAcc_; } │ │ │ │ │ -79 const Matrix93& preintegrated_H_biasOmega() const { return │ │ │ │ │ -preintegrated_H_biasOmega_; } │ │ │ │ │ -80 │ │ │ │ │ -83 bool equals(const TangentPreintegration& other, double tol) const; │ │ │ │ │ +45 using shared_ptr = boost::shared_ptr>; │ │ │ │ │ +46 │ │ │ │ │ +48 GTSAM_CONCEPT_TESTABLE_TYPE(POSE) │ │ │ │ │ +49 GTSAM_CONCEPT_POSE_TYPE(POSE) │ │ │ │ │ +50 │ │ │ │ │ +51 public: │ │ │ │ │ +_5_2 _~_M_a_g_P_o_s_e_F_a_c_t_o_r() override {} │ │ │ │ │ +53 │ │ │ │ │ +_5_5 _M_a_g_P_o_s_e_F_a_c_t_o_r() {} │ │ │ │ │ +56 │ │ │ │ │ +_6_7 _M_a_g_P_o_s_e_F_a_c_t_o_r(_K_e_y pose_key, │ │ │ │ │ +68 const Point& measured, │ │ │ │ │ +69 double scale, │ │ │ │ │ +70 const Point& direction, │ │ │ │ │ +71 const Point& bias, │ │ │ │ │ +72 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model, │ │ │ │ │ +73 const boost::optional& body_P_sensor) │ │ │ │ │ +74 : _B_a_s_e(model, pose_key), │ │ │ │ │ +75 measured_(body_P_sensor ? body_P_sensor->rotation() * measured : measured), │ │ │ │ │ +76 nM_(scale * direction.normalized()), │ │ │ │ │ +77 bias_(body_P_sensor ? body_P_sensor->rotation() * bias : bias), │ │ │ │ │ +78 body_P_sensor_(body_P_sensor) {} │ │ │ │ │ +79 │ │ │ │ │ +_8_1 NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +82 return boost::static_pointer_cast( │ │ │ │ │ +83 NonlinearFactor::shared_ptr(new This(*this))); │ │ │ │ │ +84 } │ │ │ │ │ 85 │ │ │ │ │ -88 │ │ │ │ │ -89 // Update integrated vector on tangent manifold preintegrated with │ │ │ │ │ -acceleration │ │ │ │ │ -90 // Static, functional version. │ │ │ │ │ -91 static Vector9 UpdatePreintegrated(const Vector3& a_body, │ │ │ │ │ -92 const Vector3& w_body, const double dt, │ │ │ │ │ -93 const Vector9& preintegrated, │ │ │ │ │ -94 OptionalJacobian<9, 9> A = boost::none, │ │ │ │ │ -95 OptionalJacobian<9, 3> B = boost::none, │ │ │ │ │ -96 OptionalJacobian<9, 3> C = boost::none); │ │ │ │ │ -97 │ │ │ │ │ -102 void update(const Vector3& measuredAcc, const Vector3& measuredOmega, │ │ │ │ │ -103 const double dt, Matrix9* A, Matrix93* B, Matrix93* C) override; │ │ │ │ │ +87 │ │ │ │ │ +88 // Print out the factor. │ │ │ │ │ +_8_9 void _p_r_i_n_t(const std::string& s = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ +DefaultKeyFormatter) const override { │ │ │ │ │ +90 _B_a_s_e_:_:_p_r_i_n_t(s, keyFormatter); │ │ │ │ │ +91 _g_t_s_a_m_:_:_p_r_i_n_t(Vector(nM_), "local field (nM): "); │ │ │ │ │ +92 _g_t_s_a_m_:_:_p_r_i_n_t(Vector(measured_), "measured field (bM): "); │ │ │ │ │ +93 _g_t_s_a_m_:_:_p_r_i_n_t(Vector(bias_), "magnetometer bias: "); │ │ │ │ │ +94 } │ │ │ │ │ +95 │ │ │ │ │ +_9_7 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& expected, double tol=1e-9) const override │ │ │ │ │ +{ │ │ │ │ │ +98 const This *e = dynamic_cast (&expected); │ │ │ │ │ +99 return e != nullptr && _B_a_s_e_:_:_e_q_u_a_l_s(*e, tol) && │ │ │ │ │ +100 _g_t_s_a_m_:_:_e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(this->measured_, e->measured_, tol) && │ │ │ │ │ +101 _g_t_s_a_m_:_:_e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(this->nM_, e->nM_, tol) && │ │ │ │ │ +102 _g_t_s_a_m_:_:_e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(this->bias_, e->bias_, tol); │ │ │ │ │ +103 } │ │ │ │ │ 104 │ │ │ │ │ -108 Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, │ │ │ │ │ -109 OptionalJacobian<9, 6> H = boost::none) const override; │ │ │ │ │ -110 │ │ │ │ │ -111 // Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with │ │ │ │ │ -derivatives │ │ │ │ │ -112 static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12, │ │ │ │ │ -113 double deltaT12, │ │ │ │ │ -114 OptionalJacobian<9, 9> H1 = boost::none, │ │ │ │ │ -115 OptionalJacobian<9, 9> H2 = boost::none); │ │ │ │ │ +106 │ │ │ │ │ +_1_1_1 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const POSE& nPb, boost::optional H = boost:: │ │ │ │ │ +none) const override { │ │ │ │ │ +112 // Predict the measured magnetic field h(x) in the *body* frame. │ │ │ │ │ +113 // If body_P_sensor was given, bias_ will have been rotated into the body │ │ │ │ │ +frame. │ │ │ │ │ +114 Matrix H_rot = Matrix::Zero(MeasDim, RotDim); │ │ │ │ │ +115 const Point hx = nPb.rotation().unrotate(nM_, H_rot, boost::none) + bias_; │ │ │ │ │ 116 │ │ │ │ │ -119 void mergeWith(const TangentPreintegration& pim, Matrix9* H1, Matrix9* H2); │ │ │ │ │ -121 │ │ │ │ │ -_1_2_3 virtual boost::shared_ptr _c_l_o_n_e() const { │ │ │ │ │ -124 return boost::shared_ptr(); │ │ │ │ │ +117 if (H) { │ │ │ │ │ +118 // Fill in the relevant part of the Jacobian (just rotation columns). │ │ │ │ │ +119 *H = Matrix::Zero(MeasDim, PoseDim); │ │ │ │ │ +120 const size_t rot_col0 = nPb.rotationInterval().first; │ │ │ │ │ +121 (*H).block(0, rot_col0, MeasDim, RotDim) = H_rot; │ │ │ │ │ +122 } │ │ │ │ │ +123 │ │ │ │ │ +124 return (hx - measured_); │ │ │ │ │ 125 } │ │ │ │ │ 126 │ │ │ │ │ -128 │ │ │ │ │ -129private: │ │ │ │ │ -_1_3_1 friend class boost::serialization::access; │ │ │ │ │ -132 template │ │ │ │ │ -133 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -134 namespace bs = ::boost::serialization; │ │ │ │ │ -135 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e); │ │ │ │ │ -136 ar & BOOST_SERIALIZATION_NVP(preintegrated_); │ │ │ │ │ -137 ar & BOOST_SERIALIZATION_NVP(preintegrated_H_biasAcc_); │ │ │ │ │ -138 ar & BOOST_SERIALIZATION_NVP(preintegrated_H_biasOmega_); │ │ │ │ │ +127 private: │ │ │ │ │ +_1_2_9 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +130 template │ │ │ │ │ +131 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +132 // NoiseModelFactor1 instead of NoiseModelFactorN for backward │ │ │ │ │ +compatibility │ │ │ │ │ +133 ar & boost::serialization::make_nvp("NoiseModelFactor1", │ │ │ │ │ +134 boost::serialization::base_object(*this)); │ │ │ │ │ +135 ar & BOOST_SERIALIZATION_NVP(measured_); │ │ │ │ │ +136 ar & BOOST_SERIALIZATION_NVP(nM_); │ │ │ │ │ +137 ar & BOOST_SERIALIZATION_NVP(bias_); │ │ │ │ │ +138 ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); │ │ │ │ │ 139 } │ │ │ │ │ -140 │ │ │ │ │ -141public: │ │ │ │ │ -142 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -143}; │ │ │ │ │ -144 │ │ │ │ │ -145} │ │ │ │ │ -_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ -This marks a GTSAM object to require alignment. │ │ │ │ │ -DDeeffiinniittiioonn types.h:308 │ │ │ │ │ -_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_._h │ │ │ │ │ +140}; // \class MagPoseFactor │ │ │ │ │ +141 │ │ │ │ │ +142} │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ +Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s │ │ │ │ │ -DDeeffiinniittiioonn ImuBias.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e │ │ │ │ │ -PreintegrationBase is the base class for PreintegratedMeasurements (in │ │ │ │ │ -ImuFactor) and CombinedPreinte... │ │ │ │ │ -DDeeffiinniittiioonn PreintegrationBase.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n │ │ │ │ │ -Integrate on the 9D tangent space of the NavState manifold. │ │ │ │ │ -DDeeffiinniittiioonn TangentPreintegration.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n_:_:_p_r_e_i_n_t_e_g_r_a_t_e_d___H___b_i_a_s_O_m_e_g_a__ │ │ │ │ │ -Matrix93 preintegrated_H_biasOmega_ │ │ │ │ │ -Jacobian of preintegrated_ w.r.t. angular rate bias. │ │ │ │ │ -DDeeffiinniittiioonn TangentPreintegration.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n_:_:_~_T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n │ │ │ │ │ -~TangentPreintegration() override │ │ │ │ │ -Virtual destructor. │ │ │ │ │ -DDeeffiinniittiioonn TangentPreintegration.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n_:_:_p_r_e_i_n_t_e_g_r_a_t_e_d__ │ │ │ │ │ -Vector9 preintegrated_ │ │ │ │ │ -Preintegrated navigation state, as a 9D vector on tangent space at frame i │ │ │ │ │ -Order is: theta,... │ │ │ │ │ -DDeeffiinniittiioonn TangentPreintegration.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n_:_:_T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n │ │ │ │ │ -TangentPreintegration() │ │ │ │ │ -Default constructor for serialization. │ │ │ │ │ -DDeeffiinniittiioonn TangentPreintegration.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n_:_:_p_r_e_i_n_t_e_g_r_a_t_e_d___H___b_i_a_s_A_c_c__ │ │ │ │ │ -Matrix93 preintegrated_H_biasAcc_ │ │ │ │ │ -Jacobian of preintegrated_ w.r.t. acceleration bias. │ │ │ │ │ -DDeeffiinniittiioonn TangentPreintegration.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n_:_:_c_l_o_n_e │ │ │ │ │ -virtual boost::shared_ptr< TangentPreintegration > clone() const │ │ │ │ │ -Dummy clone for MATLAB. │ │ │ │ │ -DDeeffiinniittiioonn TangentPreintegration.h:123 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ +Aliases. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l___w_i_t_h___a_b_s___t_o_l │ │ │ │ │ +bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen:: │ │ │ │ │ +DenseBase< MATRIX > &B, double tol=1e-9) │ │ │ │ │ +equals with a tolerance │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:81 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +virtual void print(const std::string &s="Factor", const KeyFormatter │ │ │ │ │ +&formatter=DefaultKeyFormatter) const │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn Factor.cpp:29 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const This &other, double tol=1e-9) const │ │ │ │ │ +check equality │ │ │ │ │ +DDeeffiinniittiioonn Factor.cpp:42 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_g_P_o_s_e_F_a_c_t_o_r │ │ │ │ │ +Factor to estimate rotation of a Pose2 or Pose3 given a magnetometer reading. │ │ │ │ │ +DDeeffiinniittiioonn MagPoseFactor.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_g_P_o_s_e_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +Implement functions needed for Testable. │ │ │ │ │ +DDeeffiinniittiioonn MagPoseFactor.h:89 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_g_P_o_s_e_F_a_c_t_o_r_:_:_~_M_a_g_P_o_s_e_F_a_c_t_o_r │ │ │ │ │ +~MagPoseFactor() override │ │ │ │ │ +Concept check by type. │ │ │ │ │ +DDeeffiinniittiioonn MagPoseFactor.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_g_P_o_s_e_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const NonlinearFactor &expected, double tol=1e-9) const override │ │ │ │ │ +Equals function. │ │ │ │ │ +DDeeffiinniittiioonn MagPoseFactor.h:97 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_g_P_o_s_e_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ +NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn MagPoseFactor.h:81 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_g_P_o_s_e_F_a_c_t_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn MagPoseFactor.h:129 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_g_P_o_s_e_F_a_c_t_o_r_:_:_M_a_g_P_o_s_e_F_a_c_t_o_r │ │ │ │ │ +MagPoseFactor(Key pose_key, const Point &measured, double scale, const Point │ │ │ │ │ +&direction, const Point &bias, const SharedNoiseModel &model, const boost:: │ │ │ │ │ +optional< POSE > &body_P_sensor) │ │ │ │ │ +Construct the factor. │ │ │ │ │ +DDeeffiinniittiioonn MagPoseFactor.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_g_P_o_s_e_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ +Vector evaluateError(const POSE &nPb, boost::optional< Matrix & > H=boost:: │ │ │ │ │ +none) const override │ │ │ │ │ +Implement functions needed to derive from Factor. │ │ │ │ │ +DDeeffiinniittiioonn MagPoseFactor.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_g_P_o_s_e_F_a_c_t_o_r_:_:_M_a_g_P_o_s_e_F_a_c_t_o_r │ │ │ │ │ +MagPoseFactor() │ │ │ │ │ +Default constructor - only use for serialization. │ │ │ │ │ +DDeeffiinniittiioonn MagPoseFactor.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ +Nonlinear factor base class. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ +A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ +variables. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ +_c_o_n_c_e_p_t_s_._h │ │ │ │ │ +Concept-checking macros for geometric objects Each macro instantiates a concept │ │ │ │ │ +check structure,... │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n_._h │ │ │ │ │ + * MMaaggPPoosseeFFaaccttoorr..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00980.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/MagFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/CombinedImuFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,53 +95,70 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
MagFactor.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Typedefs
│ │ │ │ +
CombinedImuFactor.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Factors involving magnetometers. │ │ │ │ -More...

│ │ │ │ - │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::MagFactor
 Factor to estimate rotation given magnetometer reading This version uses model measured bM = scale * bRn * direction + bias and assumes scale, direction, and the bias are given. More...
struct  gtsam::PreintegrationCombinedParams
 Parameters for pre-integration using PreintegratedCombinedMeasurements: Usage: Create just a single Params and pass a shared pointer to the constructor. More...
 
class  gtsam::PreintegratedCombinedMeasurements
 PreintegratedCombinedMeasurements integrates the IMU measurements (rotation rates and accelerations) and the corresponding covariance matrix. More...
 
class  gtsam::MagFactor1
 Factor to estimate rotation given magnetometer reading This version uses model measured bM = scale * bRn * direction + bias and assumes scale, direction, and the bias are given. More...
class  gtsam::CombinedImuFactor
 CombinedImuFactor is a 6-ways factor involving previous state (pose and velocity of the vehicle, as well as bias at previous time step), and current state (pose, velocity, bias at current time step). More...
 
class  gtsam::MagFactor2
 Factor to calibrate local Earth magnetic field as well as magnetometer bias This version uses model measured bM = bRn * nM + bias and optimizes for both nM and the bias, where nM is in units defined by magnetometer. More...
struct  gtsam::traits< PreintegrationCombinedParams >
 
class  gtsam::MagFactor3
 Factor to calibrate local Earth magnetic field as well as magnetometer bias This version uses model measured bM = scale * bRn * direction + bias and optimizes for both scale, direction, and the bias. More...
struct  gtsam::traits< PreintegratedCombinedMeasurements >
 
struct  gtsam::traits< CombinedImuFactor >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Typedefs

│ │ │ │ +typedef ManifoldPreintegration gtsam::PreintegrationType
 
│ │ │ │

Detailed Description

│ │ │ │ -

Factors involving magnetometers.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
Date
January 29, 2014
│ │ │ │ +
Author
Luca Carlone
│ │ │ │ +
│ │ │ │ +Stephen Williams
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │ +
│ │ │ │ +Vadim Indelman
│ │ │ │ +
│ │ │ │ +David Jensen
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +
│ │ │ │ +Varun Agrawal
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,45 +1,50 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -MagFactor.h File Reference │ │ │ │ │ -Factors involving magnetometers. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ +CombinedImuFactor.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r │ │ │ │ │ - _F_a_c_t_o_r to estimate rotation given magnetometer reading This version │ │ │ │ │ -  uses model measured bM = scale * bRn * direction + bias and assumes │ │ │ │ │ - scale, direction, and the bias are given. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_1 │ │ │ │ │ - _F_a_c_t_o_r to estimate rotation given magnetometer reading This version │ │ │ │ │ -  uses model measured bM = scale * bRn * direction + bias and assumes │ │ │ │ │ - scale, direction, and the bias are given. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_2 │ │ │ │ │ - _F_a_c_t_o_r to calibrate local Earth magnetic field as well as magnetometer │ │ │ │ │ -  bias This version uses model measured bM = bRn * nM + bias and │ │ │ │ │ - optimizes for both nM and the bias, where nM is in units defined by │ │ │ │ │ - magnetometer. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_3 │ │ │ │ │ - _F_a_c_t_o_r to calibrate local Earth magnetic field as well as magnetometer │ │ │ │ │ -  bias This version uses model measured bM = scale * bRn * direction + │ │ │ │ │ - bias and optimizes for both scale, direction, and the bias. _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s │ │ │ │ │ + Parameters for pre-integration using │ │ │ │ │ +  _P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s: Usage: Create just a single Params │ │ │ │ │ + and pass a shared pointer to the constructor. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ + _P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s integrates the IMU measurements │ │ │ │ │ +  (rotation rates and accelerations) and the corresponding covariance │ │ │ │ │ + matrix. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r │ │ │ │ │ + _C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r is a 6-ways factor involving previous state (pose │ │ │ │ │ +  and velocity of the vehicle, as well as bias at previous time step), │ │ │ │ │ + and current state (pose, velocity, bias at current time step). _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +typedef _M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n  ggttssaamm::::PPrreeiinntteeggrraattiioonnTTyyppee │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Factors involving magnetometers. │ │ │ │ │ Author │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Stephen Williams │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Vadim Indelman │ │ │ │ │ + David Jensen │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - January 29, 2014 │ │ │ │ │ + Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _M_a_g_F_a_c_t_o_r_._h │ │ │ │ │ + * _C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00980.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,6 +1,6 @@ │ │ │ │ │ var a00980 = [ │ │ │ │ │ - ["gtsam::MagFactor", "a04172.html", "a04172"], │ │ │ │ │ - ["gtsam::MagFactor1", "a04176.html", "a04176"], │ │ │ │ │ - ["gtsam::MagFactor2", "a04180.html", "a04180"], │ │ │ │ │ - ["gtsam::MagFactor3", "a04184.html", "a04184"] │ │ │ │ │ + ["gtsam::PreintegrationCombinedParams", "a04104.html", "a04104"], │ │ │ │ │ + ["gtsam::traits< PreintegrationCombinedParams >", "a04116.html", null], │ │ │ │ │ + ["gtsam::traits< PreintegratedCombinedMeasurements >", "a04120.html", null], │ │ │ │ │ + ["gtsam::traits< CombinedImuFactor >", "a04124.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00980_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/MagFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/CombinedImuFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,241 +98,319 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
MagFactor.h
│ │ │ │ +
CombinedImuFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
22#include <gtsam/geometry/Rot2.h>
│ │ │ │ -
23#include <gtsam/geometry/Rot3.h>
│ │ │ │ +
23#pragma once
│ │ │ │
24
│ │ │ │ -
25namespace gtsam {
│ │ │ │ -
26
│ │ │ │ -
│ │ │ │ -
33class MagFactor: public NoiseModelFactorN<Rot2> {
│ │ │ │ -
34
│ │ │ │ -
35 const Point3 measured_;
│ │ │ │ -
36 const Point3 nM_;
│ │ │ │ -
37 const Point3 bias_;
│ │ │ │ -
38
│ │ │ │ -
39public:
│ │ │ │ -
40
│ │ │ │ -
│ │ │ │ -
50 MagFactor(Key key, const Point3& measured, double scale,
│ │ │ │ -
51 const Unit3& direction, const Point3& bias,
│ │ │ │ -
52 const SharedNoiseModel& model) :
│ │ │ │ -
53 NoiseModelFactorN<Rot2>(model, key), //
│ │ │ │ -
54 measured_(measured), nM_(scale * direction), bias_(bias) {
│ │ │ │ -
55 }
│ │ │ │ -
│ │ │ │ -
56
│ │ │ │ -
│ │ │ │ -
58 NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
59 return boost::static_pointer_cast<NonlinearFactor>(
│ │ │ │ -
60 NonlinearFactor::shared_ptr(new MagFactor(*this)));
│ │ │ │ -
61 }
│ │ │ │ -
│ │ │ │ -
62
│ │ │ │ -
63 static Point3 unrotate(const Rot2& R, const Point3& p,
│ │ │ │ -
64 boost::optional<Matrix&> HR = boost::none) {
│ │ │ │ -
65 Point3 q = Rot3::Yaw(R.theta()).unrotate(p, HR, boost::none);
│ │ │ │ -
66 if (HR) {
│ │ │ │ -
67 // assign to temporary first to avoid error in Win-Debug mode
│ │ │ │ -
68 Matrix H = HR->col(2);
│ │ │ │ -
69 *HR = H;
│ │ │ │ -
70 }
│ │ │ │ -
71 return q;
│ │ │ │ -
72 }
│ │ │ │ +
25/* GTSAM includes */
│ │ │ │ +
26#include <gtsam/navigation/ManifoldPreintegration.h>
│ │ │ │ +
27#include <gtsam/navigation/TangentPreintegration.h>
│ │ │ │ +
28#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
29#include <gtsam/base/Matrix.h>
│ │ │ │ +
30#include <gtsam/base/serialization.h>
│ │ │ │ +
31
│ │ │ │ +
32namespace gtsam {
│ │ │ │ +
33
│ │ │ │ +
34#ifdef GTSAM_TANGENT_PREINTEGRATION
│ │ │ │ +
35typedef TangentPreintegration PreintegrationType;
│ │ │ │ +
36#else
│ │ │ │ +
37typedef ManifoldPreintegration PreintegrationType;
│ │ │ │ +
38#endif
│ │ │ │ +
39
│ │ │ │ +
40/*
│ │ │ │ +
41 * If you are using the factor, please cite:
│ │ │ │ +
42 * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating
│ │ │ │ +
43 * conditionally independent sets in factor graphs: a unifying perspective based
│ │ │ │ +
44 * on smart factors, Int. Conf. on Robotics and Automation (ICRA), 2014.
│ │ │ │ +
45 *
│ │ │ │ +
46 * REFERENCES:
│ │ │ │ +
47 * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups",
│ │ │ │ +
48 * Volume 2, 2008.
│ │ │ │ +
49 * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for
│ │ │ │ +
50 * High-Dynamic Motion in Built Environments Without Initial Conditions",
│ │ │ │ +
51 * TRO, 28(1):61-76, 2012.
│ │ │ │ +
52 * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
│ │ │ │ +
53 * Computation of the Jacobian Matrices", Tech. Report, 2013.
│ │ │ │ +
54 * Available in this repo as "PreintegratedIMUJacobians.pdf".
│ │ │ │ +
55 * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, IMU Preintegration on
│ │ │ │ +
56 * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation,
│ │ │ │ +
57 * Robotics: Science and Systems (RSS), 2015.
│ │ │ │ +
58 */
│ │ │ │ +
59
│ │ │ │ +
│ │ │ │ +
62struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
│ │ │ │ +
63 Matrix3 biasAccCovariance;
│ │ │ │ +
64 Matrix3 biasOmegaCovariance;
│ │ │ │ +
65 Matrix6 biasAccOmegaInt;
│ │ │ │ +
66
│ │ │ │ +
│ │ │ │ +
69 PreintegrationCombinedParams()
│ │ │ │ +
70 : biasAccCovariance(I_3x3),
│ │ │ │ +
71 biasOmegaCovariance(I_3x3),
│ │ │ │ +
72 biasAccOmegaInt(I_6x6) {}
│ │ │ │ +
│ │ │ │
73
│ │ │ │ -
│ │ │ │ -
77 Vector evaluateError(const Rot2& nRb,
│ │ │ │ -
78 boost::optional<Matrix&> H = boost::none) const override {
│ │ │ │ -
79 // measured bM = nRb� * nM + b
│ │ │ │ -
80 Point3 hx = unrotate(nRb, nM_, H) + bias_;
│ │ │ │ -
81 return (hx - measured_);
│ │ │ │ -
82 }
│ │ │ │ -
│ │ │ │ -
83};
│ │ │ │ -
│ │ │ │ -
84
│ │ │ │ -
│ │ │ │ -
90class MagFactor1: public NoiseModelFactorN<Rot3> {
│ │ │ │ -
91
│ │ │ │ -
92 const Point3 measured_;
│ │ │ │ -
93 const Point3 nM_;
│ │ │ │ -
94 const Point3 bias_;
│ │ │ │ -
95
│ │ │ │ -
96public:
│ │ │ │ -
97
│ │ │ │ -
│ │ │ │ -
99 MagFactor1(Key key, const Point3& measured, double scale,
│ │ │ │ -
100 const Unit3& direction, const Point3& bias,
│ │ │ │ -
101 const SharedNoiseModel& model) :
│ │ │ │ -
102 NoiseModelFactorN<Rot3>(model, key), //
│ │ │ │ -
103 measured_(measured), nM_(scale * direction), bias_(bias) {
│ │ │ │ -
104 }
│ │ │ │ -
│ │ │ │ -
105
│ │ │ │ -
│ │ │ │ -
107 NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
108 return boost::static_pointer_cast<NonlinearFactor>(
│ │ │ │ -
109 NonlinearFactor::shared_ptr(new MagFactor1(*this)));
│ │ │ │ -
110 }
│ │ │ │ -
│ │ │ │ -
111
│ │ │ │ -
│ │ │ │ -
115 Vector evaluateError(const Rot3& nRb,
│ │ │ │ -
116 boost::optional<Matrix&> H = boost::none) const override {
│ │ │ │ -
117 // measured bM = nRb� * nM + b
│ │ │ │ -
118 Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_;
│ │ │ │ -
119 return (hx - measured_);
│ │ │ │ -
120 }
│ │ │ │ -
│ │ │ │ -
121};
│ │ │ │ -
│ │ │ │ -
122
│ │ │ │ -
│ │ │ │ -
128class MagFactor2: public NoiseModelFactorN<Point3, Point3> {
│ │ │ │ -
129
│ │ │ │ -
130 const Point3 measured_;
│ │ │ │ -
131 const Rot3 bRn_;
│ │ │ │ -
132
│ │ │ │ -
133public:
│ │ │ │ -
134
│ │ │ │ -
│ │ │ │ -
136 MagFactor2(Key key1, Key key2, const Point3& measured, const Rot3& nRb,
│ │ │ │ -
137 const SharedNoiseModel& model) :
│ │ │ │ -
138 NoiseModelFactorN<Point3, Point3>(model, key1, key2), //
│ │ │ │ -
139 measured_(measured), bRn_(nRb.inverse()) {
│ │ │ │ -
140 }
│ │ │ │ -
│ │ │ │ -
141
│ │ │ │ -
│ │ │ │ -
143 NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
144 return boost::static_pointer_cast<NonlinearFactor>(
│ │ │ │ -
145 NonlinearFactor::shared_ptr(new MagFactor2(*this)));
│ │ │ │ -
146 }
│ │ │ │ -
│ │ │ │ -
147
│ │ │ │ -
│ │ │ │ -
153 Vector evaluateError(const Point3& nM, const Point3& bias,
│ │ │ │ -
154 boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
│ │ │ │ -
155 boost::none) const override {
│ │ │ │ -
156 // measured bM = nRb� * nM + b, where b is unknown bias
│ │ │ │ -
157 Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias;
│ │ │ │ -
158 if (H2)
│ │ │ │ -
159 *H2 = I_3x3;
│ │ │ │ -
160 return (hx - measured_);
│ │ │ │ -
161 }
│ │ │ │ -
│ │ │ │ -
162};
│ │ │ │ -
│ │ │ │ -
163
│ │ │ │ -
│ │ │ │ -
169class MagFactor3: public NoiseModelFactorN<double, Unit3, Point3> {
│ │ │ │ -
170
│ │ │ │ -
171 const Point3 measured_;
│ │ │ │ -
172 const Rot3 bRn_;
│ │ │ │ -
173
│ │ │ │ -
174public:
│ │ │ │ +
│ │ │ │ +
75 PreintegrationCombinedParams(const Vector3& n_gravity) :
│ │ │ │ +
76 PreintegrationParams(n_gravity), biasAccCovariance(I_3x3),
│ │ │ │ +
77 biasOmegaCovariance(I_3x3), biasAccOmegaInt(I_6x6) {
│ │ │ │ +
78
│ │ │ │ +
79 }
│ │ │ │ +
│ │ │ │ +
80
│ │ │ │ +
81 // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
│ │ │ │ +
82 static boost::shared_ptr<PreintegrationCombinedParams> MakeSharedD(double g = 9.81) {
│ │ │ │ +
83 return boost::shared_ptr<PreintegrationCombinedParams>(new PreintegrationCombinedParams(Vector3(0, 0, g)));
│ │ │ │ +
84 }
│ │ │ │ +
85
│ │ │ │ +
86 // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
│ │ │ │ +
87 static boost::shared_ptr<PreintegrationCombinedParams> MakeSharedU(double g = 9.81) {
│ │ │ │ +
88 return boost::shared_ptr<PreintegrationCombinedParams>(new PreintegrationCombinedParams(Vector3(0, 0, -g)));
│ │ │ │ +
89 }
│ │ │ │ +
90
│ │ │ │ +
91 void print(const std::string& s="") const override;
│ │ │ │ +
92 bool equals(const PreintegratedRotationParams& other, double tol) const override;
│ │ │ │ +
93
│ │ │ │ +
94 void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; }
│ │ │ │ +
95 void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; }
│ │ │ │ +
96 void setBiasAccOmegaInit(const Matrix6& cov) { biasAccOmegaInt=cov; }
│ │ │ │ +
97
│ │ │ │ +
98 const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; }
│ │ │ │ +
99 const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; }
│ │ │ │ +
100 const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; }
│ │ │ │ +
101
│ │ │ │ +
102private:
│ │ │ │ +
103
│ │ │ │ +
105 friend class boost::serialization::access;
│ │ │ │ +
106 template <class ARCHIVE>
│ │ │ │ +
107 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ +
108 namespace bs = ::boost::serialization;
│ │ │ │ +
109 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationParams);
│ │ │ │ +
110 ar & BOOST_SERIALIZATION_NVP(biasAccCovariance);
│ │ │ │ +
111 ar & BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
│ │ │ │ +
112 ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInt);
│ │ │ │ +
113 }
│ │ │ │ +
114
│ │ │ │ +
115public:
│ │ │ │ +
116 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
117};
│ │ │ │ +
│ │ │ │ +
118
│ │ │ │ +
│ │ │ │ +
129class GTSAM_EXPORT PreintegratedCombinedMeasurements : public PreintegrationType {
│ │ │ │ +
130
│ │ │ │ +
131public:
│ │ │ │ +
132 typedef PreintegrationCombinedParams Params;
│ │ │ │ +
133
│ │ │ │ +
134 protected:
│ │ │ │ +
135 /* Covariance matrix of the preintegrated measurements
│ │ │ │ +
136 * COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY BiasAcc BiasOmega]
│ │ │ │ +
137 * (first-order propagation from *measurementCovariance*).
│ │ │ │ +
138 * PreintegratedCombinedMeasurements also include the biases and keep the correlation
│ │ │ │ +
139 * between the preintegrated measurements and the biases
│ │ │ │ +
140 */
│ │ │ │ +
141 Eigen::Matrix<double, 15, 15> preintMeasCov_;
│ │ │ │ +
142
│ │ │ │ +
143 friend class CombinedImuFactor;
│ │ │ │ +
144
│ │ │ │ +
145 public:
│ │ │ │ +
148
│ │ │ │ +
│ │ │ │ +
150 PreintegratedCombinedMeasurements() {
│ │ │ │ +
151 preintMeasCov_.setZero();
│ │ │ │ +
152 }
│ │ │ │ +
│ │ │ │ +
153
│ │ │ │ +
│ │ │ │ +
159 PreintegratedCombinedMeasurements(
│ │ │ │ +
160 const boost::shared_ptr<Params>& p,
│ │ │ │ +
161 const imuBias::ConstantBias& biasHat = imuBias::ConstantBias())
│ │ │ │ +
162 : PreintegrationType(p, biasHat) {
│ │ │ │ +
163 preintMeasCov_.setZero();
│ │ │ │ +
164 }
│ │ │ │ +
│ │ │ │ +
165
│ │ │ │ +
│ │ │ │ +
171 PreintegratedCombinedMeasurements(const PreintegrationType& base, const Eigen::Matrix<double, 15, 15>& preintMeasCov)
│ │ │ │ +
172 : PreintegrationType(base),
│ │ │ │ +
173 preintMeasCov_(preintMeasCov) {
│ │ │ │ +
174 }
│ │ │ │ +
│ │ │ │
175
│ │ │ │ -
│ │ │ │ -
177 MagFactor3(Key key1, Key key2, Key key3, const Point3& measured,
│ │ │ │ -
178 const Rot3& nRb, const SharedNoiseModel& model) :
│ │ │ │ -
179 NoiseModelFactorN<double, Unit3, Point3>(model, key1, key2, key3), //
│ │ │ │ -
180 measured_(measured), bRn_(nRb.inverse()) {
│ │ │ │ -
181 }
│ │ │ │ -
│ │ │ │ -
182
│ │ │ │ -
│ │ │ │ -
184 NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
185 return boost::static_pointer_cast<NonlinearFactor>(
│ │ │ │ -
186 NonlinearFactor::shared_ptr(new MagFactor3(*this)));
│ │ │ │ -
187 }
│ │ │ │ -
│ │ │ │ -
188
│ │ │ │ -
│ │ │ │ -
194 Vector evaluateError(const double& scale, const Unit3& direction,
│ │ │ │ -
195 const Point3& bias, boost::optional<Matrix&> H1 = boost::none,
│ │ │ │ -
196 boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
│ │ │ │ -
197 boost::none) const override {
│ │ │ │ -
198 // measured bM = nRb� * nM + b, where b is unknown bias
│ │ │ │ -
199 Unit3 rotated = bRn_.rotate(direction, boost::none, H2);
│ │ │ │ -
200 Point3 hx = scale * rotated.point3() + bias;
│ │ │ │ -
201 if (H1)
│ │ │ │ -
202 *H1 = rotated.point3();
│ │ │ │ -
203 if (H2) // H2 is 2*2, but we need 3*2
│ │ │ │ -
204 {
│ │ │ │ -
205 Matrix H;
│ │ │ │ -
206 rotated.point3(H);
│ │ │ │ -
207 *H2 = scale * H * (*H2);
│ │ │ │ -
208 }
│ │ │ │ -
209 if (H3)
│ │ │ │ -
210 *H3 = I_3x3;
│ │ │ │ -
211 return (hx - measured_);
│ │ │ │ -
212 }
│ │ │ │ -
│ │ │ │ -
213};
│ │ │ │ -
│ │ │ │ -
214
│ │ │ │ -
215}
│ │ │ │ -
216
│ │ │ │ -
Rot3.h
3D rotation represented as a rotation matrix or quaternion
│ │ │ │ -
Rot2.h
2D rotation
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
177 ~PreintegratedCombinedMeasurements() override {}
│ │ │ │ +
178
│ │ │ │ +
180
│ │ │ │ +
183
│ │ │ │ +
185 void resetIntegration() override;
│ │ │ │ +
186
│ │ │ │ +
188 Params& p() const { return *boost::static_pointer_cast<Params>(this->p_); }
│ │ │ │ +
190
│ │ │ │ +
194 Matrix preintMeasCov() const { return preintMeasCov_; }
│ │ │ │ +
196
│ │ │ │ +
200 void print(const std::string& s = "Preintegrated Measurements:") const override;
│ │ │ │ +
202 bool equals(const PreintegratedCombinedMeasurements& expected,
│ │ │ │ +
203 double tol = 1e-9) const;
│ │ │ │ +
205
│ │ │ │ +
206
│ │ │ │ +
209
│ │ │ │ +
220 void integrateMeasurement(const Vector3& measuredAcc,
│ │ │ │ +
221 const Vector3& measuredOmega, const double dt) override;
│ │ │ │ +
222
│ │ │ │ +
224
│ │ │ │ +
225 private:
│ │ │ │ +
227 friend class boost::serialization::access;
│ │ │ │ +
228 template <class ARCHIVE>
│ │ │ │ +
229 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ +
230 namespace bs = ::boost::serialization;
│ │ │ │ +
231 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
│ │ │ │ +
232 ar& BOOST_SERIALIZATION_NVP(preintMeasCov_);
│ │ │ │ +
233 }
│ │ │ │ +
234
│ │ │ │ +
235public:
│ │ │ │ +
236 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
237};
│ │ │ │ +
│ │ │ │ +
238
│ │ │ │ +
│ │ │ │ +
258class GTSAM_EXPORT CombinedImuFactor: public NoiseModelFactorN<Pose3, Vector3, Pose3,
│ │ │ │ +
259 Vector3, imuBias::ConstantBias, imuBias::ConstantBias> {
│ │ │ │ +
260public:
│ │ │ │ +
261
│ │ │ │ +
262private:
│ │ │ │ +
263
│ │ │ │ +
264 typedef CombinedImuFactor This;
│ │ │ │ +
265 typedef NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
│ │ │ │ +
266 imuBias::ConstantBias, imuBias::ConstantBias> Base;
│ │ │ │ +
267
│ │ │ │ +
268 PreintegratedCombinedMeasurements _PIM_;
│ │ │ │ +
269
│ │ │ │ +
270public:
│ │ │ │ +
271
│ │ │ │ +
273#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
│ │ │ │ +
274 typedef typename boost::shared_ptr<CombinedImuFactor> shared_ptr;
│ │ │ │ +
275#else
│ │ │ │ +
276 typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
│ │ │ │ +
277#endif
│ │ │ │ +
278
│ │ │ │ +
280 CombinedImuFactor() {}
│ │ │ │ +
281
│ │ │ │ +
292 CombinedImuFactor(
│ │ │ │ +
293 Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
│ │ │ │ +
294 const PreintegratedCombinedMeasurements& preintegratedMeasurements);
│ │ │ │ +
295
│ │ │ │ +
296 ~CombinedImuFactor() override {}
│ │ │ │ +
297
│ │ │ │ +
299 gtsam::NonlinearFactor::shared_ptr clone() const override;
│ │ │ │ +
300
│ │ │ │ +
305 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
│ │ │ │ +
306 const CombinedImuFactor&);
│ │ │ │ +
308 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
│ │ │ │ +
309 DefaultKeyFormatter) const override;
│ │ │ │ +
310
│ │ │ │ +
312 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
│ │ │ │ +
314
│ │ │ │ +
│ │ │ │ +
317 const PreintegratedCombinedMeasurements& preintegratedMeasurements() const {
│ │ │ │ +
318 return _PIM_;
│ │ │ │ +
319 }
│ │ │ │ +
│ │ │ │ +
320
│ │ │ │ +
324 Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
│ │ │ │ +
325 const Pose3& pose_j, const Vector3& vel_j,
│ │ │ │ +
326 const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
│ │ │ │ +
327 boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
│ │ │ │ +
328 boost::none, boost::optional<Matrix&> H3 = boost::none,
│ │ │ │ +
329 boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 =
│ │ │ │ +
330 boost::none, boost::optional<Matrix&> H6 = boost::none) const override;
│ │ │ │ +
331
│ │ │ │ +
332 private:
│ │ │ │ +
334 friend class boost::serialization::access;
│ │ │ │ +
335 template <class ARCHIVE>
│ │ │ │ +
336 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ +
337 // NoiseModelFactor6 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ +
338 ar& boost::serialization::make_nvp(
│ │ │ │ +
339 "NoiseModelFactor6", boost::serialization::base_object<Base>(*this));
│ │ │ │ +
340 ar& BOOST_SERIALIZATION_NVP(_PIM_);
│ │ │ │ +
341 }
│ │ │ │ +
342
│ │ │ │ +
343public:
│ │ │ │ +
344 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
345};
│ │ │ │ +
│ │ │ │ +
346// class CombinedImuFactor
│ │ │ │ +
347
│ │ │ │ +
348template <>
│ │ │ │ +
│ │ │ │ +
349struct traits<PreintegrationCombinedParams>
│ │ │ │ +
350 : public Testable<PreintegrationCombinedParams> {};
│ │ │ │ +
│ │ │ │ +
351
│ │ │ │ +
352template <>
│ │ │ │ +
│ │ │ │ +
353struct traits<PreintegratedCombinedMeasurements>
│ │ │ │ +
354 : public Testable<PreintegratedCombinedMeasurements> {};
│ │ │ │ +
│ │ │ │ +
355
│ │ │ │ +
356template <>
│ │ │ │ +
357struct traits<CombinedImuFactor> : public Testable<CombinedImuFactor> {};
│ │ │ │ +
358
│ │ │ │ +
359} // namespace gtsam
│ │ │ │ +
serialization.h
Convenience functions for serializing data structures via boost.serialization.
│ │ │ │ +
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ + │ │ │ │ + │ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ -
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::Rot2
Rotation matrix NOTE: the angle theta is in radians unless explicitly stated.
Definition Rot2.h:36
│ │ │ │ -
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
│ │ │ │ -
gtsam::Rot3::unrotate
Point3 unrotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
rotate point from world to rotated frame
Definition Rot3.cpp:136
│ │ │ │ -
gtsam::Rot3::Yaw
static Rot3 Yaw(double t)
Positive yaw is to right (as in aircraft heading). See ypr.
Definition Rot3.h:174
│ │ │ │ -
gtsam::Rot3::rotate
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
rotate point from rotated coordinate frame to world
Definition Rot3M.cpp:149
│ │ │ │ -
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
│ │ │ │ -
gtsam::Unit3::point3
Point3 point3(OptionalJacobian< 3, 2 > H=boost::none) const
Return unit-norm Point3.
Definition Unit3.cpp:144
│ │ │ │ -
gtsam::MagFactor
Factor to estimate rotation given magnetometer reading This version uses model measured bM = scale * ...
Definition MagFactor.h:33
│ │ │ │ -
gtsam::MagFactor::clone
NonlinearFactor::shared_ptr clone() const override
Definition MagFactor.h:58
│ │ │ │ -
gtsam::MagFactor::MagFactor
MagFactor(Key key, const Point3 &measured, double scale, const Unit3 &direction, const Point3 &bias, const SharedNoiseModel &model)
Constructor of factor that estimates nav to body rotation bRn.
Definition MagFactor.h:50
│ │ │ │ -
gtsam::MagFactor::evaluateError
Vector evaluateError(const Rot2 &nRb, boost::optional< Matrix & > H=boost::none) const override
vector of errors
Definition MagFactor.h:77
│ │ │ │ -
gtsam::MagFactor1
Factor to estimate rotation given magnetometer reading This version uses model measured bM = scale * ...
Definition MagFactor.h:90
│ │ │ │ -
gtsam::MagFactor1::MagFactor1
MagFactor1(Key key, const Point3 &measured, double scale, const Unit3 &direction, const Point3 &bias, const SharedNoiseModel &model)
Constructor.
Definition MagFactor.h:99
│ │ │ │ -
gtsam::MagFactor1::clone
NonlinearFactor::shared_ptr clone() const override
Definition MagFactor.h:107
│ │ │ │ -
gtsam::MagFactor1::evaluateError
Vector evaluateError(const Rot3 &nRb, boost::optional< Matrix & > H=boost::none) const override
vector of errors
Definition MagFactor.h:115
│ │ │ │ -
gtsam::MagFactor2
Factor to calibrate local Earth magnetic field as well as magnetometer bias This version uses model m...
Definition MagFactor.h:128
│ │ │ │ -
gtsam::MagFactor2::clone
NonlinearFactor::shared_ptr clone() const override
Definition MagFactor.h:143
│ │ │ │ -
gtsam::MagFactor2::MagFactor2
MagFactor2(Key key1, Key key2, const Point3 &measured, const Rot3 &nRb, const SharedNoiseModel &model)
Constructor.
Definition MagFactor.h:136
│ │ │ │ -
gtsam::MagFactor2::evaluateError
Vector evaluateError(const Point3 &nM, const Point3 &bias, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
vector of errors
Definition MagFactor.h:153
│ │ │ │ -
gtsam::MagFactor3
Factor to calibrate local Earth magnetic field as well as magnetometer bias This version uses model m...
Definition MagFactor.h:169
│ │ │ │ -
gtsam::MagFactor3::evaluateError
Vector evaluateError(const double &scale, const Unit3 &direction, const Point3 &bias, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none) const override
vector of errors
Definition MagFactor.h:194
│ │ │ │ -
gtsam::MagFactor3::MagFactor3
MagFactor3(Key key1, Key key2, Key key3, const Point3 &measured, const Rot3 &nRb, const SharedNoiseModel &model)
Constructor.
Definition MagFactor.h:177
│ │ │ │ -
gtsam::MagFactor3::clone
NonlinearFactor::shared_ptr clone() const override
Definition MagFactor.h:184
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ +
gtsam::PreintegrationCombinedParams
Parameters for pre-integration using PreintegratedCombinedMeasurements: Usage: Create just a single P...
Definition CombinedImuFactor.h:62
│ │ │ │ +
gtsam::PreintegrationCombinedParams::biasAccOmegaInt
Matrix6 biasAccOmegaInt
covariance of bias used as initial estimate.
Definition CombinedImuFactor.h:65
│ │ │ │ +
gtsam::PreintegrationCombinedParams::PreintegrationCombinedParams
PreintegrationCombinedParams(const Vector3 &n_gravity)
See two named constructors below for good values of n_gravity in body frame.
Definition CombinedImuFactor.h:75
│ │ │ │ +
gtsam::PreintegrationCombinedParams::PreintegrationCombinedParams
PreintegrationCombinedParams()
Default constructor makes uninitialized params struct.
Definition CombinedImuFactor.h:69
│ │ │ │ +
gtsam::PreintegrationCombinedParams::biasOmegaCovariance
Matrix3 biasOmegaCovariance
continuous-time "Covariance" describing gyroscope bias random walk
Definition CombinedImuFactor.h:64
│ │ │ │ +
gtsam::PreintegrationCombinedParams::biasAccCovariance
Matrix3 biasAccCovariance
continuous-time "Covariance" describing accelerometer bias random walk
Definition CombinedImuFactor.h:63
│ │ │ │ +
gtsam::PreintegratedCombinedMeasurements
PreintegratedCombinedMeasurements integrates the IMU measurements (rotation rates and accelerations) ...
Definition CombinedImuFactor.h:129
│ │ │ │ +
gtsam::PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements
PreintegratedCombinedMeasurements(const PreintegrationType &base, const Eigen::Matrix< double, 15, 15 > &preintMeasCov)
Construct preintegrated directly from members: base class and preintMeasCov.
Definition CombinedImuFactor.h:171
│ │ │ │ +
gtsam::PreintegratedCombinedMeasurements::~PreintegratedCombinedMeasurements
~PreintegratedCombinedMeasurements() override
Virtual destructor.
Definition CombinedImuFactor.h:177
│ │ │ │ +
gtsam::PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements
PreintegratedCombinedMeasurements()
Default constructor only for serialization and wrappers.
Definition CombinedImuFactor.h:150
│ │ │ │ +
gtsam::PreintegratedCombinedMeasurements::p
Params & p() const
const reference to params, shadows definition in base class
Definition CombinedImuFactor.h:188
│ │ │ │ +
gtsam::PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements
PreintegratedCombinedMeasurements(const boost::shared_ptr< Params > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
Default constructor, initializes the class with no measurements.
Definition CombinedImuFactor.h:159
│ │ │ │ +
gtsam::CombinedImuFactor
CombinedImuFactor is a 6-ways factor involving previous state (pose and velocity of the vehicle,...
Definition CombinedImuFactor.h:259
│ │ │ │ +
gtsam::CombinedImuFactor::shared_ptr
boost::shared_ptr< CombinedImuFactor > shared_ptr
Shorthand for a smart pointer to a factor.
Definition CombinedImuFactor.h:276
│ │ │ │ +
gtsam::CombinedImuFactor::CombinedImuFactor
CombinedImuFactor()
Default constructor - only use for serialization.
Definition CombinedImuFactor.h:280
│ │ │ │ +
gtsam::CombinedImuFactor::preintegratedMeasurements
const PreintegratedCombinedMeasurements & preintegratedMeasurements() const
Access the preintegrated measurements.
Definition CombinedImuFactor.h:317
│ │ │ │ +
gtsam::imuBias::ConstantBias
Definition ImuBias.h:30
│ │ │ │ +
gtsam::ManifoldPreintegration
IMU pre-integration on NavSatet manifold.
Definition ManifoldPreintegration.h:33
│ │ │ │ +
gtsam::PreintegrationParams
Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
Definition PreintegrationParams.h:26
│ │ │ │
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ -
gtsam::NoiseModelFactorN< Rot2 >::key
Key key() const
Returns a key.
Definition NonlinearFactor.h:518
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── encoding │ │ │ │ │ @@ -1 +1 @@ │ │ │ │ │ -utf-8 │ │ │ │ │ +us-ascii │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,297 +1,389 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -MagFactor.h │ │ │ │ │ +CombinedImuFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_2_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_3_._h> │ │ │ │ │ +23#pragma once │ │ │ │ │ 24 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ -26 │ │ │ │ │ -_3_3class _M_a_g_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -34 │ │ │ │ │ -35 const _P_o_i_n_t_3 measured_; │ │ │ │ │ -36 const _P_o_i_n_t_3 nM_; │ │ │ │ │ -37 const _P_o_i_n_t_3 bias_; │ │ │ │ │ -38 │ │ │ │ │ -39public: │ │ │ │ │ -40 │ │ │ │ │ -_5_0 _M_a_g_F_a_c_t_o_r(_K_e_y _k_e_y, const _P_o_i_n_t_3& measured, double scale, │ │ │ │ │ -51 const _U_n_i_t_3& direction, const _P_o_i_n_t_3& bias, │ │ │ │ │ -52 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model) : │ │ │ │ │ -53 _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N<_R_o_t_2>(model, _k_e_y), // │ │ │ │ │ -54 measured_(measured), nM_(scale * direction), bias_(bias) { │ │ │ │ │ -55 } │ │ │ │ │ -56 │ │ │ │ │ -_5_8 NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -59 return boost::static_pointer_cast( │ │ │ │ │ -60 NonlinearFactor::shared_ptr(new _M_a_g_F_a_c_t_o_r(*this))); │ │ │ │ │ -61 } │ │ │ │ │ -62 │ │ │ │ │ -63 static _P_o_i_n_t_3 unrotate(const _R_o_t_2& R, const _P_o_i_n_t_3& p, │ │ │ │ │ -64 boost::optional HR = boost::none) { │ │ │ │ │ -65 _P_o_i_n_t_3 q = _R_o_t_3_:_:_Y_a_w(R.theta())._u_n_r_o_t_a_t_e(p, HR, boost::none); │ │ │ │ │ -66 if (HR) { │ │ │ │ │ -67 // assign to temporary first to avoid error in Win-Debug mode │ │ │ │ │ -68 Matrix H = HR->col(2); │ │ │ │ │ -69 *HR = H; │ │ │ │ │ -70 } │ │ │ │ │ -71 return q; │ │ │ │ │ -72 } │ │ │ │ │ +25/* GTSAM includes */ │ │ │ │ │ +26#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_._h> │ │ │ │ │ +27#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n_._h> │ │ │ │ │ +28#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +29#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ +30#include <_g_t_s_a_m_/_b_a_s_e_/_s_e_r_i_a_l_i_z_a_t_i_o_n_._h> │ │ │ │ │ +31 │ │ │ │ │ +32namespace _g_t_s_a_m { │ │ │ │ │ +33 │ │ │ │ │ +34#ifdef GTSAM_TANGENT_PREINTEGRATION │ │ │ │ │ +35typedef TangentPreintegration PreintegrationType; │ │ │ │ │ +36#else │ │ │ │ │ +37typedef ManifoldPreintegration PreintegrationType; │ │ │ │ │ +38#endif │ │ │ │ │ +39 │ │ │ │ │ +40/* │ │ │ │ │ +41 * If you are using the factor, please cite: │ │ │ │ │ +42 * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating │ │ │ │ │ +43 * conditionally independent sets in factor graphs: a unifying perspective │ │ │ │ │ +based │ │ │ │ │ +44 * on smart factors, Int. Conf. on Robotics and Automation (ICRA), 2014. │ │ │ │ │ +45 * │ │ │ │ │ +46 * REFERENCES: │ │ │ │ │ +47 * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie │ │ │ │ │ +Groups", │ │ │ │ │ +48 * Volume 2, 2008. │ │ │ │ │ +49 * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for │ │ │ │ │ +50 * High-Dynamic Motion in Built Environments Without Initial Conditions", │ │ │ │ │ +51 * TRO, 28(1):61-76, 2012. │ │ │ │ │ +52 * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: │ │ │ │ │ +53 * Computation of the Jacobian Matrices", Tech. Report, 2013. │ │ │ │ │ +54 * Available in this repo as "PreintegratedIMUJacobians.pdf". │ │ │ │ │ +55 * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, IMU Preintegration │ │ │ │ │ +on │ │ │ │ │ +56 * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation, │ │ │ │ │ +57 * Robotics: Science and Systems (RSS), 2015. │ │ │ │ │ +58 */ │ │ │ │ │ +59 │ │ │ │ │ +_6_2struct GTSAM_EXPORT _P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s : _P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s { │ │ │ │ │ +_6_3 Matrix3 _b_i_a_s_A_c_c_C_o_v_a_r_i_a_n_c_e; │ │ │ │ │ +_6_4 Matrix3 _b_i_a_s_O_m_e_g_a_C_o_v_a_r_i_a_n_c_e; │ │ │ │ │ +_6_5 Matrix6 _b_i_a_s_A_c_c_O_m_e_g_a_I_n_t; │ │ │ │ │ +66 │ │ │ │ │ +_6_9 _P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s() │ │ │ │ │ +70 : biasAccCovariance(I_3x3), │ │ │ │ │ +71 biasOmegaCovariance(I_3x3), │ │ │ │ │ +72 biasAccOmegaInt(I_6x6) {} │ │ │ │ │ 73 │ │ │ │ │ -_7_7 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const _R_o_t_2& nRb, │ │ │ │ │ -78 boost::optional H = boost::none) const override { │ │ │ │ │ -79 // measured bM = nRb� * nM + b │ │ │ │ │ -80 _P_o_i_n_t_3 hx = unrotate(nRb, nM_, H) + bias_; │ │ │ │ │ -81 return (hx - measured_); │ │ │ │ │ -82 } │ │ │ │ │ -83}; │ │ │ │ │ -84 │ │ │ │ │ -_9_0class _M_a_g_F_a_c_t_o_r_1: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -91 │ │ │ │ │ -92 const _P_o_i_n_t_3 measured_; │ │ │ │ │ -93 const _P_o_i_n_t_3 nM_; │ │ │ │ │ -94 const _P_o_i_n_t_3 bias_; │ │ │ │ │ -95 │ │ │ │ │ -96public: │ │ │ │ │ +_7_5 _P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s(const Vector3& n_gravity) : │ │ │ │ │ +76 _P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s(n_gravity), biasAccCovariance(I_3x3), │ │ │ │ │ +77 biasOmegaCovariance(I_3x3), biasAccOmegaInt(I_6x6) { │ │ │ │ │ +78 │ │ │ │ │ +79 } │ │ │ │ │ +80 │ │ │ │ │ +81 // Default Params for a Z-down navigation frame, such as NED: gravity points │ │ │ │ │ +along positive Z-axis │ │ │ │ │ +82 static boost::shared_ptr MakeSharedD(double g │ │ │ │ │ += 9.81) { │ │ │ │ │ +83 return boost::shared_ptr(new │ │ │ │ │ +_P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s(Vector3(0, 0, g))); │ │ │ │ │ +84 } │ │ │ │ │ +85 │ │ │ │ │ +86 // Default Params for a Z-up navigation frame, such as ENU: gravity points │ │ │ │ │ +along negative Z-axis │ │ │ │ │ +87 static boost::shared_ptr MakeSharedU(double g │ │ │ │ │ += 9.81) { │ │ │ │ │ +88 return boost::shared_ptr(new │ │ │ │ │ +PreintegrationCombinedParams(Vector3(0, 0, -g))); │ │ │ │ │ +89 } │ │ │ │ │ +90 │ │ │ │ │ +91 void _p_r_i_n_t(const std::string& s="") const override; │ │ │ │ │ +92 bool equals(const PreintegratedRotationParams& other, double tol) const │ │ │ │ │ +override; │ │ │ │ │ +93 │ │ │ │ │ +94 void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; } │ │ │ │ │ +95 void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; } │ │ │ │ │ +96 void setBiasAccOmegaInit(const Matrix6& cov) { biasAccOmegaInt=cov; } │ │ │ │ │ 97 │ │ │ │ │ -_9_9 _M_a_g_F_a_c_t_o_r_1(_K_e_y _k_e_y, const _P_o_i_n_t_3& measured, double scale, │ │ │ │ │ -100 const _U_n_i_t_3& direction, const _P_o_i_n_t_3& bias, │ │ │ │ │ -101 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model) : │ │ │ │ │ -102 _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N<_R_o_t_3>(model, _k_e_y), // │ │ │ │ │ -103 measured_(measured), nM_(scale * direction), bias_(bias) { │ │ │ │ │ -104 } │ │ │ │ │ -105 │ │ │ │ │ -_1_0_7 NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -108 return boost::static_pointer_cast( │ │ │ │ │ -109 NonlinearFactor::shared_ptr(new _M_a_g_F_a_c_t_o_r_1(*this))); │ │ │ │ │ -110 } │ │ │ │ │ -111 │ │ │ │ │ -_1_1_5 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const _R_o_t_3& nRb, │ │ │ │ │ -116 boost::optional H = boost::none) const override { │ │ │ │ │ -117 // measured bM = nRb� * nM + b │ │ │ │ │ -118 _P_o_i_n_t_3 hx = nRb._u_n_r_o_t_a_t_e(nM_, H, boost::none) + bias_; │ │ │ │ │ -119 return (hx - measured_); │ │ │ │ │ -120 } │ │ │ │ │ -121}; │ │ │ │ │ -122 │ │ │ │ │ -_1_2_8class _M_a_g_F_a_c_t_o_r_2: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -129 │ │ │ │ │ -130 const _P_o_i_n_t_3 measured_; │ │ │ │ │ -131 const _R_o_t_3 bRn_; │ │ │ │ │ -132 │ │ │ │ │ -133public: │ │ │ │ │ -134 │ │ │ │ │ -_1_3_6 _M_a_g_F_a_c_t_o_r_2(_K_e_y key1, _K_e_y key2, const _P_o_i_n_t_3& measured, const _R_o_t_3& nRb, │ │ │ │ │ -137 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model) : │ │ │ │ │ -138 _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N<_P_o_i_n_t_3, _P_o_i_n_t_3>(model, key1, key2), // │ │ │ │ │ -139 measured_(measured), bRn_(nRb.inverse()) { │ │ │ │ │ -140 } │ │ │ │ │ -141 │ │ │ │ │ -_1_4_3 NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -144 return boost::static_pointer_cast( │ │ │ │ │ -145 NonlinearFactor::shared_ptr(new _M_a_g_F_a_c_t_o_r_2(*this))); │ │ │ │ │ -146 } │ │ │ │ │ -147 │ │ │ │ │ -_1_5_3 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const _P_o_i_n_t_3& nM, const _P_o_i_n_t_3& bias, │ │ │ │ │ -154 boost::optional H1 = boost::none, boost::optional H2 = │ │ │ │ │ -155 boost::none) const override { │ │ │ │ │ -156 // measured bM = nRb� * nM + b, where b is unknown bias │ │ │ │ │ -157 _P_o_i_n_t_3 hx = bRn_._r_o_t_a_t_e(nM, boost::none, H1) + bias; │ │ │ │ │ -158 if (H2) │ │ │ │ │ -159 *H2 = I_3x3; │ │ │ │ │ -160 return (hx - measured_); │ │ │ │ │ -161 } │ │ │ │ │ -162}; │ │ │ │ │ -163 │ │ │ │ │ -_1_6_9class _M_a_g_F_a_c_t_o_r_3: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -170 │ │ │ │ │ -171 const _P_o_i_n_t_3 measured_; │ │ │ │ │ -172 const _R_o_t_3 bRn_; │ │ │ │ │ -173 │ │ │ │ │ -174public: │ │ │ │ │ +98 const Matrix3& getBiasAccCovariance() const { return biasAccCovariance; } │ │ │ │ │ +99 const Matrix3& getBiasOmegaCovariance() const { return biasOmegaCovariance; │ │ │ │ │ +} │ │ │ │ │ +100 const Matrix6& getBiasAccOmegaInit() const { return biasAccOmegaInt; } │ │ │ │ │ +101 │ │ │ │ │ +102private: │ │ │ │ │ +103 │ │ │ │ │ +_1_0_5 friend class boost::serialization::access; │ │ │ │ │ +106 template │ │ │ │ │ +107 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ +108 namespace bs = ::boost::serialization; │ │ │ │ │ +109 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s); │ │ │ │ │ +110 ar & BOOST_SERIALIZATION_NVP(biasAccCovariance); │ │ │ │ │ +111 ar & BOOST_SERIALIZATION_NVP(biasOmegaCovariance); │ │ │ │ │ +112 ar & BOOST_SERIALIZATION_NVP(biasAccOmegaInt); │ │ │ │ │ +113 } │ │ │ │ │ +114 │ │ │ │ │ +115public: │ │ │ │ │ +116 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +117}; │ │ │ │ │ +118 │ │ │ │ │ +_1_2_9class GTSAM_EXPORT _P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s : public │ │ │ │ │ +_P_r_e_i_n_t_e_g_r_a_t_i_o_n_T_y_p_e { │ │ │ │ │ +130 │ │ │ │ │ +131public: │ │ │ │ │ +132 typedef _P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s _P_a_r_a_m_s; │ │ │ │ │ +133 │ │ │ │ │ +134 protected: │ │ │ │ │ +135 /* Covariance matrix of the preintegrated measurements │ │ │ │ │ +136 * COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY BiasAcc │ │ │ │ │ +BiasOmega] │ │ │ │ │ +137 * (first-order propagation from *measurementCovariance*). │ │ │ │ │ +138 * PreintegratedCombinedMeasurements also include the biases and keep the │ │ │ │ │ +correlation │ │ │ │ │ +139 * between the preintegrated measurements and the biases │ │ │ │ │ +140 */ │ │ │ │ │ +141 Eigen::Matrix preintMeasCov_; │ │ │ │ │ +142 │ │ │ │ │ +143 friend class _C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r; │ │ │ │ │ +144 │ │ │ │ │ +145 public: │ │ │ │ │ +148 │ │ │ │ │ +_1_5_0 _P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s() { │ │ │ │ │ +151 preintMeasCov_.setZero(); │ │ │ │ │ +152 } │ │ │ │ │ +153 │ │ │ │ │ +_1_5_9 _P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s( │ │ │ │ │ +160 const boost::shared_ptr& p, │ │ │ │ │ +161 const _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s& biasHat = _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s()) │ │ │ │ │ +162 : _P_r_e_i_n_t_e_g_r_a_t_i_o_n_T_y_p_e(p, biasHat) { │ │ │ │ │ +163 preintMeasCov_.setZero(); │ │ │ │ │ +164 } │ │ │ │ │ +165 │ │ │ │ │ +_1_7_1 _P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s(const _P_r_e_i_n_t_e_g_r_a_t_i_o_n_T_y_p_e& base, const │ │ │ │ │ +Eigen::Matrix& preintMeasCov) │ │ │ │ │ +172 : _P_r_e_i_n_t_e_g_r_a_t_i_o_n_T_y_p_e(base), │ │ │ │ │ +173 preintMeasCov_(preintMeasCov) { │ │ │ │ │ +174 } │ │ │ │ │ 175 │ │ │ │ │ -_1_7_7 _M_a_g_F_a_c_t_o_r_3(_K_e_y key1, _K_e_y key2, _K_e_y key3, const _P_o_i_n_t_3& measured, │ │ │ │ │ -178 const _R_o_t_3& nRb, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model) : │ │ │ │ │ -179 _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N(model, key1, key2, key3), // │ │ │ │ │ -180 measured_(measured), bRn_(nRb.inverse()) { │ │ │ │ │ -181 } │ │ │ │ │ -182 │ │ │ │ │ -_1_8_4 NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -185 return boost::static_pointer_cast( │ │ │ │ │ -186 NonlinearFactor::shared_ptr(new _M_a_g_F_a_c_t_o_r_3(*this))); │ │ │ │ │ -187 } │ │ │ │ │ -188 │ │ │ │ │ -_1_9_4 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const double& scale, const _U_n_i_t_3& direction, │ │ │ │ │ -195 const _P_o_i_n_t_3& bias, boost::optional H1 = boost::none, │ │ │ │ │ -196 boost::optional H2 = boost::none, boost::optional H3 = │ │ │ │ │ -197 boost::none) const override { │ │ │ │ │ -198 // measured bM = nRb� * nM + b, where b is unknown bias │ │ │ │ │ -199 _U_n_i_t_3 rotated = bRn_._r_o_t_a_t_e(direction, boost::none, H2); │ │ │ │ │ -200 _P_o_i_n_t_3 hx = scale * rotated._p_o_i_n_t_3() + bias; │ │ │ │ │ -201 if (H1) │ │ │ │ │ -202 *H1 = rotated._p_o_i_n_t_3(); │ │ │ │ │ -203 if (H2) // H2 is 2*2, but we need 3*2 │ │ │ │ │ -204 { │ │ │ │ │ -205 Matrix H; │ │ │ │ │ -206 rotated._p_o_i_n_t_3(H); │ │ │ │ │ -207 *H2 = scale * H * (*H2); │ │ │ │ │ -208 } │ │ │ │ │ -209 if (H3) │ │ │ │ │ -210 *H3 = I_3x3; │ │ │ │ │ -211 return (hx - measured_); │ │ │ │ │ -212 } │ │ │ │ │ -213}; │ │ │ │ │ -214 │ │ │ │ │ -215} │ │ │ │ │ -216 │ │ │ │ │ -_R_o_t_3_._h │ │ │ │ │ -3D rotation represented as a rotation matrix or quaternion │ │ │ │ │ -_R_o_t_2_._h │ │ │ │ │ -2D rotation │ │ │ │ │ +_1_7_7 _~_P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s() override {} │ │ │ │ │ +178 │ │ │ │ │ +180 │ │ │ │ │ +183 │ │ │ │ │ +185 void resetIntegration() override; │ │ │ │ │ +186 │ │ │ │ │ +_1_8_8 _P_a_r_a_m_s& _p() const { return *boost::static_pointer_cast(this->p_); } │ │ │ │ │ +190 │ │ │ │ │ +194 Matrix preintMeasCov() const { return preintMeasCov_; } │ │ │ │ │ +196 │ │ │ │ │ +200 void print(const std::string& s = "Preintegrated Measurements:") const │ │ │ │ │ +override; │ │ │ │ │ +202 bool equals(const PreintegratedCombinedMeasurements& expected, │ │ │ │ │ +203 double tol = 1e-9) const; │ │ │ │ │ +205 │ │ │ │ │ +206 │ │ │ │ │ +209 │ │ │ │ │ +220 void integrateMeasurement(const Vector3& measuredAcc, │ │ │ │ │ +221 const Vector3& measuredOmega, const double dt) override; │ │ │ │ │ +222 │ │ │ │ │ +224 │ │ │ │ │ +225 private: │ │ │ │ │ +_2_2_7 friend class boost::serialization::access; │ │ │ │ │ +228 template │ │ │ │ │ +229 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ +230 namespace bs = ::boost::serialization; │ │ │ │ │ +231 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(_P_r_e_i_n_t_e_g_r_a_t_i_o_n_T_y_p_e); │ │ │ │ │ +232 ar& BOOST_SERIALIZATION_NVP(preintMeasCov_); │ │ │ │ │ +233 } │ │ │ │ │ +234 │ │ │ │ │ +235public: │ │ │ │ │ +236 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +237}; │ │ │ │ │ +238 │ │ │ │ │ +_2_5_8class GTSAM_EXPORT _C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +260public: │ │ │ │ │ +261 │ │ │ │ │ +262private: │ │ │ │ │ +263 │ │ │ │ │ +264 typedef _C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ +265 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N<_P_o_s_e_3, Vector3, _P_o_s_e_3, Vector3, │ │ │ │ │ +266 _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s, _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s> _B_a_s_e; │ │ │ │ │ +267 │ │ │ │ │ +268 _P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s _PIM_; │ │ │ │ │ +269 │ │ │ │ │ +270public: │ │ │ │ │ +271 │ │ │ │ │ +273#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 │ │ │ │ │ +274 typedef typename boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +275#else │ │ │ │ │ +_2_7_6 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +277#endif │ │ │ │ │ +278 │ │ │ │ │ +_2_8_0 _C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r() {} │ │ │ │ │ +281 │ │ │ │ │ +292 _C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r( │ │ │ │ │ +293 _K_e_y pose_i, _K_e_y vel_i, _K_e_y pose_j, _K_e_y vel_j, _K_e_y bias_i, _K_e_y bias_j, │ │ │ │ │ +294 const _P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s& preintegratedMeasurements); │ │ │ │ │ +295 │ │ │ │ │ +296 _~_C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r() override {} │ │ │ │ │ +297 │ │ │ │ │ +299 gtsam::NonlinearFactor::shared_ptr clone() const override; │ │ │ │ │ +300 │ │ │ │ │ +305 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, │ │ │ │ │ +306 const CombinedImuFactor&); │ │ │ │ │ +308 void print(const std::string& s = "", const KeyFormatter& keyFormatter = │ │ │ │ │ +309 DefaultKeyFormatter) const override; │ │ │ │ │ +310 │ │ │ │ │ +312 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const │ │ │ │ │ +override; │ │ │ │ │ +314 │ │ │ │ │ +_3_1_7 const _P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s& _p_r_e_i_n_t_e_g_r_a_t_e_d_M_e_a_s_u_r_e_m_e_n_t_s() const │ │ │ │ │ +{ │ │ │ │ │ +318 return _PIM_; │ │ │ │ │ +319 } │ │ │ │ │ +320 │ │ │ │ │ +324 Vector evaluateError(const _P_o_s_e_3& pose_i, const Vector3& vel_i, │ │ │ │ │ +325 const _P_o_s_e_3& pose_j, const Vector3& vel_j, │ │ │ │ │ +326 const _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s& bias_i, const _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s& bias_j, │ │ │ │ │ +327 boost::optional H1 = boost::none, boost::optional H2 = │ │ │ │ │ +328 boost::none, boost::optional H3 = boost::none, │ │ │ │ │ +329 boost::optional H4 = boost::none, boost::optional H5 = │ │ │ │ │ +330 boost::none, boost::optional H6 = boost::none) const override; │ │ │ │ │ +331 │ │ │ │ │ +332 private: │ │ │ │ │ +_3_3_4 friend class boost::serialization::access; │ │ │ │ │ +335 template │ │ │ │ │ +336 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ +337 // NoiseModelFactor6 instead of NoiseModelFactorN for backward │ │ │ │ │ +compatibility │ │ │ │ │ +338 ar& boost::serialization::make_nvp( │ │ │ │ │ +339 "NoiseModelFactor6", boost::serialization::base_object(*this)); │ │ │ │ │ +340 ar& BOOST_SERIALIZATION_NVP(_PIM_); │ │ │ │ │ +341 } │ │ │ │ │ +342 │ │ │ │ │ +343public: │ │ │ │ │ +344 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +345}; │ │ │ │ │ +346// class CombinedImuFactor │ │ │ │ │ +347 │ │ │ │ │ +348template <> │ │ │ │ │ +_3_4_9struct _t_r_a_i_t_s<_P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s> │ │ │ │ │ +350 : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ +351 │ │ │ │ │ +352template <> │ │ │ │ │ +_3_5_3struct _t_r_a_i_t_s<_P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s> │ │ │ │ │ +354 : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ +355 │ │ │ │ │ +356template <> │ │ │ │ │ +_3_5_7struct _t_r_a_i_t_s<_C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ +358 │ │ │ │ │ +359} // namespace gtsam │ │ │ │ │ +_s_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ +Convenience functions for serializing data structures via boost.serialization. │ │ │ │ │ +_M_a_t_r_i_x_._h │ │ │ │ │ +typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ +This marks a GTSAM object to require alignment. │ │ │ │ │ +DDeeffiinniittiioonn types.h:308 │ │ │ │ │ +_T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n_._h │ │ │ │ │ +_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_._h │ │ │ │ │ _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ -Vector3 Point3 │ │ │ │ │ -As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point3 to Vector3... │ │ │ │ │ -DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ -Aliases. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_2 │ │ │ │ │ -Rotation matrix NOTE: the angle theta is in radians unless explicitly stated. │ │ │ │ │ -DDeeffiinniittiioonn Rot2.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ -Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor │ │ │ │ │ -symbol GTSAM_USE_QUATERNIO... │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_u_n_r_o_t_a_t_e │ │ │ │ │ -Point3 unrotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, │ │ │ │ │ -OptionalJacobian< 3, 3 > H2=boost::none) const │ │ │ │ │ -rotate point from world to rotated frame │ │ │ │ │ -DDeeffiinniittiioonn Rot3.cpp:136 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_Y_a_w │ │ │ │ │ -static Rot3 Yaw(double t) │ │ │ │ │ -Positive yaw is to right (as in aircraft heading). See ypr. │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:174 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_r_o_t_a_t_e │ │ │ │ │ -Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, │ │ │ │ │ -OptionalJacobian< 3, 3 > H2=boost::none) const │ │ │ │ │ -rotate point from rotated coordinate frame to world │ │ │ │ │ -DDeeffiinniittiioonn Rot3M.cpp:149 │ │ │ │ │ -_g_t_s_a_m_:_:_U_n_i_t_3 │ │ │ │ │ -Represents a 3D point on a unit sphere. │ │ │ │ │ -DDeeffiinniittiioonn Unit3.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_U_n_i_t_3_:_:_p_o_i_n_t_3 │ │ │ │ │ -Point3 point3(OptionalJacobian< 3, 2 > H=boost::none) const │ │ │ │ │ -Return unit-norm Point3. │ │ │ │ │ -DDeeffiinniittiioonn Unit3.cpp:144 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r │ │ │ │ │ -Factor to estimate rotation given magnetometer reading This version uses model │ │ │ │ │ -measured bM = scale * ... │ │ │ │ │ -DDeeffiinniittiioonn MagFactor.h:33 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ -NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn MagFactor.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_:_:_M_a_g_F_a_c_t_o_r │ │ │ │ │ -MagFactor(Key key, const Point3 &measured, double scale, const Unit3 │ │ │ │ │ -&direction, const Point3 &bias, const SharedNoiseModel &model) │ │ │ │ │ -Constructor of factor that estimates nav to body rotation bRn. │ │ │ │ │ -DDeeffiinniittiioonn MagFactor.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ -Vector evaluateError(const Rot2 &nRb, boost::optional< Matrix & > H=boost:: │ │ │ │ │ -none) const override │ │ │ │ │ -vector of errors │ │ │ │ │ -DDeeffiinniittiioonn MagFactor.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_1 │ │ │ │ │ -Factor to estimate rotation given magnetometer reading This version uses model │ │ │ │ │ -measured bM = scale * ... │ │ │ │ │ -DDeeffiinniittiioonn MagFactor.h:90 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_1_:_:_M_a_g_F_a_c_t_o_r_1 │ │ │ │ │ -MagFactor1(Key key, const Point3 &measured, double scale, const Unit3 │ │ │ │ │ -&direction, const Point3 &bias, const SharedNoiseModel &model) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn MagFactor.h:99 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_1_:_:_c_l_o_n_e │ │ │ │ │ -NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn MagFactor.h:107 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_1_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ -Vector evaluateError(const Rot3 &nRb, boost::optional< Matrix & > H=boost:: │ │ │ │ │ -none) const override │ │ │ │ │ -vector of errors │ │ │ │ │ -DDeeffiinniittiioonn MagFactor.h:115 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_2 │ │ │ │ │ -Factor to calibrate local Earth magnetic field as well as magnetometer bias │ │ │ │ │ -This version uses model m... │ │ │ │ │ -DDeeffiinniittiioonn MagFactor.h:128 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_2_:_:_c_l_o_n_e │ │ │ │ │ -NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn MagFactor.h:143 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_2_:_:_M_a_g_F_a_c_t_o_r_2 │ │ │ │ │ -MagFactor2(Key key1, Key key2, const Point3 &measured, const Rot3 &nRb, const │ │ │ │ │ -SharedNoiseModel &model) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn MagFactor.h:136 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_2_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ -Vector evaluateError(const Point3 &nM, const Point3 &bias, boost::optional< │ │ │ │ │ -Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const │ │ │ │ │ -override │ │ │ │ │ -vector of errors │ │ │ │ │ -DDeeffiinniittiioonn MagFactor.h:153 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_3 │ │ │ │ │ -Factor to calibrate local Earth magnetic field as well as magnetometer bias │ │ │ │ │ -This version uses model m... │ │ │ │ │ -DDeeffiinniittiioonn MagFactor.h:169 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_3_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ -Vector evaluateError(const double &scale, const Unit3 &direction, const Point3 │ │ │ │ │ -&bias, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > │ │ │ │ │ -H2=boost::none, boost::optional< Matrix & > H3=boost::none) const override │ │ │ │ │ -vector of errors │ │ │ │ │ -DDeeffiinniittiioonn MagFactor.h:194 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_3_:_:_M_a_g_F_a_c_t_o_r_3 │ │ │ │ │ -MagFactor3(Key key1, Key key2, Key key3, const Point3 &measured, const Rot3 │ │ │ │ │ -&nRb, const SharedNoiseModel &model) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn MagFactor.h:177 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_3_:_:_c_l_o_n_e │ │ │ │ │ -NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn MagFactor.h:184 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ +A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s │ │ │ │ │ +Parameters for pre-integration using PreintegratedCombinedMeasurements: Usage: │ │ │ │ │ +Create just a single P... │ │ │ │ │ +DDeeffiinniittiioonn CombinedImuFactor.h:62 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s_:_:_b_i_a_s_A_c_c_O_m_e_g_a_I_n_t │ │ │ │ │ +Matrix6 biasAccOmegaInt │ │ │ │ │ +covariance of bias used as initial estimate. │ │ │ │ │ +DDeeffiinniittiioonn CombinedImuFactor.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s │ │ │ │ │ +PreintegrationCombinedParams(const Vector3 &n_gravity) │ │ │ │ │ +See two named constructors below for good values of n_gravity in body frame. │ │ │ │ │ +DDeeffiinniittiioonn CombinedImuFactor.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s │ │ │ │ │ +PreintegrationCombinedParams() │ │ │ │ │ +Default constructor makes uninitialized params struct. │ │ │ │ │ +DDeeffiinniittiioonn CombinedImuFactor.h:69 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s_:_:_b_i_a_s_O_m_e_g_a_C_o_v_a_r_i_a_n_c_e │ │ │ │ │ +Matrix3 biasOmegaCovariance │ │ │ │ │ +continuous-time "Covariance" describing gyroscope bias random walk │ │ │ │ │ +DDeeffiinniittiioonn CombinedImuFactor.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_C_o_m_b_i_n_e_d_P_a_r_a_m_s_:_:_b_i_a_s_A_c_c_C_o_v_a_r_i_a_n_c_e │ │ │ │ │ +Matrix3 biasAccCovariance │ │ │ │ │ +continuous-time "Covariance" describing accelerometer bias random walk │ │ │ │ │ +DDeeffiinniittiioonn CombinedImuFactor.h:63 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ +PreintegratedCombinedMeasurements integrates the IMU measurements (rotation │ │ │ │ │ +rates and accelerations) ... │ │ │ │ │ +DDeeffiinniittiioonn CombinedImuFactor.h:129 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ +PreintegratedCombinedMeasurements(const PreintegrationType &base, const Eigen:: │ │ │ │ │ +Matrix< double, 15, 15 > &preintMeasCov) │ │ │ │ │ +Construct preintegrated directly from members: base class and preintMeasCov. │ │ │ │ │ +DDeeffiinniittiioonn CombinedImuFactor.h:171 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s_:_:_~_P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ +~PreintegratedCombinedMeasurements() override │ │ │ │ │ +Virtual destructor. │ │ │ │ │ +DDeeffiinniittiioonn CombinedImuFactor.h:177 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ +PreintegratedCombinedMeasurements() │ │ │ │ │ +Default constructor only for serialization and wrappers. │ │ │ │ │ +DDeeffiinniittiioonn CombinedImuFactor.h:150 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s_:_:_p │ │ │ │ │ +Params & p() const │ │ │ │ │ +const reference to params, shadows definition in base class │ │ │ │ │ +DDeeffiinniittiioonn CombinedImuFactor.h:188 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ +PreintegratedCombinedMeasurements(const boost::shared_ptr< Params > &p, const │ │ │ │ │ +imuBias::ConstantBias &biasHat=imuBias::ConstantBias()) │ │ │ │ │ +Default constructor, initializes the class with no measurements. │ │ │ │ │ +DDeeffiinniittiioonn CombinedImuFactor.h:159 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r │ │ │ │ │ +CombinedImuFactor is a 6-ways factor involving previous state (pose and │ │ │ │ │ +velocity of the vehicle,... │ │ │ │ │ +DDeeffiinniittiioonn CombinedImuFactor.h:259 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< CombinedImuFactor > shared_ptr │ │ │ │ │ +Shorthand for a smart pointer to a factor. │ │ │ │ │ +DDeeffiinniittiioonn CombinedImuFactor.h:276 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r_:_:_C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r │ │ │ │ │ +CombinedImuFactor() │ │ │ │ │ +Default constructor - only use for serialization. │ │ │ │ │ +DDeeffiinniittiioonn CombinedImuFactor.h:280 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r_:_:_p_r_e_i_n_t_e_g_r_a_t_e_d_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ +const PreintegratedCombinedMeasurements & preintegratedMeasurements() const │ │ │ │ │ +Access the preintegrated measurements. │ │ │ │ │ +DDeeffiinniittiioonn CombinedImuFactor.h:317 │ │ │ │ │ +_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s │ │ │ │ │ +DDeeffiinniittiioonn ImuBias.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n │ │ │ │ │ +IMU pre-integration on NavSatet manifold. │ │ │ │ │ +DDeeffiinniittiioonn ManifoldPreintegration.h:33 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ +Parameters for pre-integration: Usage: Create just a single Params and pass a │ │ │ │ │ +shared pointer to the c... │ │ │ │ │ +DDeeffiinniittiioonn PreintegrationParams.h:26 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ variables. │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_ _R_o_t_2_ _>_:_:_k_e_y │ │ │ │ │ -Key key() const │ │ │ │ │ -Returns a key. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:518 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _M_a_g_F_a_c_t_o_r_._h │ │ │ │ │ + * _C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00986.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ImuFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/GPSFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,60 +96,46 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
ImuFactor.h File Reference
│ │ │ │ +
GPSFactor.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ +

Header file for GPS factor. │ │ │ │ +More...

│ │ │ │ + │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::PreintegratedImuMeasurements
 PreintegratedImuMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) and the corresponding covariance matrix. More...
 
class  gtsam::ImuFactor
 ImuFactor is a 5-ways factor involving previous state (pose and velocity of the vehicle at previous time step), current state (pose and velocity at current time step), and the bias estimate. More...
 
class  gtsam::ImuFactor2
 ImuFactor2 is a ternary factor that uses NavStates rather than Pose/Velocity. More...
 
struct  gtsam::traits< PreintegratedImuMeasurements >
 
struct  gtsam::traits< ImuFactor >
class  gtsam::GPSFactor
 Prior on position in a Cartesian frame. More...
 
struct  gtsam::traits< ImuFactor2 >
class  gtsam::GPSFactor2
 Version of GPSFactor for NavState. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Luca Carlone
│ │ │ │ -
│ │ │ │ -Stephen Williams
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ -
│ │ │ │ -Vadim Indelman
│ │ │ │ -
│ │ │ │ -David Jensen
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ +

Header file for GPS factor.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
Date
January 22, 2014
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,45 +1,30 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -ImuFactor.h File Reference │ │ │ │ │ +GPSFactor.h File Reference │ │ │ │ │ +Header file for GPS factor. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ - _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s accumulates (integrates) the IMU │ │ │ │ │ -  measurements (rotation rates and accelerations) and the corresponding │ │ │ │ │ - covariance matrix. _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_G_P_S_F_a_c_t_o_r │ │ │ │ │ +  Prior on position in a Cartesian frame. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ - class   _g_t_s_a_m_:_:_I_m_u_F_a_c_t_o_r │ │ │ │ │ - _I_m_u_F_a_c_t_o_r is a 5-ways factor involving previous state (pose and │ │ │ │ │ -  velocity of the vehicle at previous time step), current state (pose │ │ │ │ │ - and velocity at current time step), and the bias estimate. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_I_m_u_F_a_c_t_o_r_2 │ │ │ │ │ -  _I_m_u_F_a_c_t_o_r_2 is a ternary factor that uses NavStates rather than Pose/ │ │ │ │ │ - Velocity. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _I_m_u_F_a_c_t_o_r_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _I_m_u_F_a_c_t_o_r_2_ _> │ │ │ │ │ +class   _g_t_s_a_m_:_:_G_P_S_F_a_c_t_o_r_2 │ │ │ │ │ +  Version of _G_P_S_F_a_c_t_o_r for _N_a_v_S_t_a_t_e. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Header file for GPS factor. │ │ │ │ │ Author │ │ │ │ │ - Luca Carlone │ │ │ │ │ - Stephen Williams │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Vadim Indelman │ │ │ │ │ - David Jensen │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + January 22, 2014 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _I_m_u_F_a_c_t_o_r_._h │ │ │ │ │ + * _G_P_S_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00986_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ImuFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/GPSFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,298 +98,186 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ImuFactor.h
│ │ │ │ +
GPSFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
22#pragma once
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
21#include <gtsam/navigation/NavState.h>
│ │ │ │ +
22#include <gtsam/geometry/Pose3.h>
│ │ │ │
23
│ │ │ │ -
24/* GTSAM includes */
│ │ │ │ -
25#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
26#include <gtsam/navigation/ManifoldPreintegration.h>
│ │ │ │ -
27#include <gtsam/navigation/TangentPreintegration.h>
│ │ │ │ -
28#include <gtsam/base/debug.h>
│ │ │ │ -
29
│ │ │ │ -
30namespace gtsam {
│ │ │ │ -
31
│ │ │ │ -
32#ifdef GTSAM_TANGENT_PREINTEGRATION
│ │ │ │ -
33typedef TangentPreintegration PreintegrationType;
│ │ │ │ -
34#else
│ │ │ │ -
35typedef ManifoldPreintegration PreintegrationType;
│ │ │ │ -
36#endif
│ │ │ │ -
37
│ │ │ │ -
38/*
│ │ │ │ -
39 * If you are using the factor, please cite:
│ │ │ │ -
40 * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, "Eliminating
│ │ │ │ -
41 * conditionally independent sets in factor graphs: a unifying perspective based
│ │ │ │ -
42 * on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014.
│ │ │ │ -
43 *
│ │ │ │ -
44 * C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on
│ │ │ │ -
45 * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation",
│ │ │ │ -
46 * Robotics: Science and Systems (RSS), 2015.
│ │ │ │ -
47 *
│ │ │ │ -
48 * REFERENCES:
│ │ │ │ -
49 * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups",
│ │ │ │ -
50 * Volume 2, 2008.
│ │ │ │ -
51 * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for
│ │ │ │ -
52 * High-Dynamic Motion in Built Environments Without Initial Conditions",
│ │ │ │ -
53 * TRO, 28(1):61-76, 2012.
│ │ │ │ -
54 * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
│ │ │ │ -
55 * Computation of the Jacobian Matrices", Tech. Report, 2013.
│ │ │ │ -
56 * Available in this repo as "PreintegratedIMUJacobians.pdf".
│ │ │ │ -
57 * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on
│ │ │ │ -
58 * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation",
│ │ │ │ -
59 * Robotics: Science and Systems (RSS), 2015.
│ │ │ │ -
60 */
│ │ │ │ -
61
│ │ │ │ -
│ │ │ │ -
72class GTSAM_EXPORT PreintegratedImuMeasurements: public PreintegrationType {
│ │ │ │ -
73
│ │ │ │ -
74 friend class ImuFactor;
│ │ │ │ -
75 friend class ImuFactor2;
│ │ │ │ +
24namespace gtsam {
│ │ │ │ +
25
│ │ │ │ +
│ │ │ │ +
35class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
│ │ │ │ +
36
│ │ │ │ +
37private:
│ │ │ │ +
38
│ │ │ │ +
39 typedef NoiseModelFactorN<Pose3> Base;
│ │ │ │ +
40
│ │ │ │ +
41 Point3 nT_;
│ │ │ │ +
42
│ │ │ │ +
43public:
│ │ │ │ +
44
│ │ │ │ +
46 typedef boost::shared_ptr<GPSFactor> shared_ptr;
│ │ │ │ +
47
│ │ │ │ +
49 typedef GPSFactor This;
│ │ │ │ +
50
│ │ │ │ +
52 GPSFactor(): nT_(0, 0, 0) {}
│ │ │ │ +
53
│ │ │ │ +
54 ~GPSFactor() override {}
│ │ │ │ +
55
│ │ │ │ +
│ │ │ │ +
63 GPSFactor(Key key, const Point3& gpsIn, const SharedNoiseModel& model) :
│ │ │ │ +
64 Base(model, key), nT_(gpsIn) {
│ │ │ │ +
65 }
│ │ │ │ +
│ │ │ │ +
66
│ │ │ │ +
│ │ │ │ +
68 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
69 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ +
70 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
│ │ │ │ +
71 }
│ │ │ │ +
│ │ │ │ +
72
│ │ │ │ +
74 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
│ │ │ │ +
75 DefaultKeyFormatter) const override;
│ │ │ │
76
│ │ │ │ -
77protected:
│ │ │ │ -
78
│ │ │ │ -
79 Matrix9 preintMeasCov_;
│ │ │ │ -
81
│ │ │ │ -
82public:
│ │ │ │ +
78 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
│ │ │ │ +
79
│ │ │ │ +
81 Vector evaluateError(const Pose3& p,
│ │ │ │ +
82 boost::optional<Matrix&> H = boost::none) const override;
│ │ │ │
83
│ │ │ │ -
│ │ │ │ -
85 PreintegratedImuMeasurements() {
│ │ │ │ -
86 preintMeasCov_.setZero();
│ │ │ │ -
87 }
│ │ │ │ -
│ │ │ │ -
88
│ │ │ │ -
│ │ │ │ -
94 PreintegratedImuMeasurements(const boost::shared_ptr<PreintegrationParams>& p,
│ │ │ │ -
95 const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
│ │ │ │ -
96 PreintegrationType(p, biasHat) {
│ │ │ │ -
97 preintMeasCov_.setZero();
│ │ │ │ -
98 }
│ │ │ │ -
│ │ │ │ -
99
│ │ │ │ -
│ │ │ │ -
105 PreintegratedImuMeasurements(const PreintegrationType& base, const Matrix9& preintMeasCov)
│ │ │ │ -
106 : PreintegrationType(base),
│ │ │ │ -
107 preintMeasCov_(preintMeasCov) {
│ │ │ │ -
108 }
│ │ │ │ +
84 inline const Point3 & measurementIn() const {
│ │ │ │ +
85 return nT_;
│ │ │ │ +
86 }
│ │ │ │ +
87
│ │ │ │ +
93 static std::pair<Pose3, Vector3> EstimateState(double t1, const Point3& NED1,
│ │ │ │ +
94 double t2, const Point3& NED2, double timestamp);
│ │ │ │ +
95
│ │ │ │ +
96private:
│ │ │ │ +
97
│ │ │ │ +
99 friend class boost::serialization::access;
│ │ │ │ +
100 template<class ARCHIVE>
│ │ │ │ +
101 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
102 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ +
103 ar
│ │ │ │ +
104 & boost::serialization::make_nvp("NoiseModelFactor1",
│ │ │ │ +
105 boost::serialization::base_object<Base>(*this));
│ │ │ │ +
106 ar & BOOST_SERIALIZATION_NVP(nT_);
│ │ │ │ +
107 }
│ │ │ │ +
108};
│ │ │ │
│ │ │ │
109
│ │ │ │ -
│ │ │ │ -
111 ~PreintegratedImuMeasurements() override {
│ │ │ │ -
112 }
│ │ │ │ -
│ │ │ │ -
113
│ │ │ │ -
115 void print(const std::string& s = "Preintegrated Measurements:") const override;
│ │ │ │ -
116
│ │ │ │ -
118 bool equals(const PreintegratedImuMeasurements& expected, double tol = 1e-9) const;
│ │ │ │ +
│ │ │ │ +
114class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
│ │ │ │ +
115
│ │ │ │ +
116private:
│ │ │ │ +
117
│ │ │ │ +
118 typedef NoiseModelFactorN<NavState> Base;
│ │ │ │
119
│ │ │ │ -
121 void resetIntegration() override;
│ │ │ │ -
122
│ │ │ │ -
133 void integrateMeasurement(const Vector3& measuredAcc,
│ │ │ │ -
134 const Vector3& measuredOmega, const double dt) override;
│ │ │ │ -
135
│ │ │ │ -
137 void integrateMeasurements(const Matrix& measuredAccs, const Matrix& measuredOmegas,
│ │ │ │ -
138 const Matrix& dts);
│ │ │ │ +
120 Point3 nT_;
│ │ │ │ +
121
│ │ │ │ +
122public:
│ │ │ │ +
123
│ │ │ │ +
125 typedef boost::shared_ptr<GPSFactor2> shared_ptr;
│ │ │ │ +
126
│ │ │ │ +
128 typedef GPSFactor2 This;
│ │ │ │ +
129
│ │ │ │ +
131 GPSFactor2():nT_(0, 0, 0) {}
│ │ │ │ +
132
│ │ │ │ +
133 ~GPSFactor2() override {}
│ │ │ │ +
134
│ │ │ │ +
│ │ │ │ +
136 GPSFactor2(Key key, const Point3& gpsIn, const SharedNoiseModel& model) :
│ │ │ │ +
137 Base(model, key), nT_(gpsIn) {
│ │ │ │ +
138 }
│ │ │ │ +
│ │ │ │
139
│ │ │ │ -
141 Matrix preintMeasCov() const { return preintMeasCov_; }
│ │ │ │ -
142
│ │ │ │ -
143#ifdef GTSAM_TANGENT_PREINTEGRATION
│ │ │ │ -
145 void mergeWith(const PreintegratedImuMeasurements& pim, Matrix9* H1, Matrix9* H2);
│ │ │ │ -
146#endif
│ │ │ │ -
147
│ │ │ │ -
148 private:
│ │ │ │ -
150 friend class boost::serialization::access;
│ │ │ │ -
151 template<class ARCHIVE>
│ │ │ │ -
152 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
153 namespace bs = ::boost::serialization;
│ │ │ │ -
154 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
│ │ │ │ -
155 ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
│ │ │ │ -
156 }
│ │ │ │ -
157};
│ │ │ │ -
│ │ │ │ -
158
│ │ │ │ -
│ │ │ │ -
171class GTSAM_EXPORT ImuFactor: public NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
│ │ │ │ -
172 imuBias::ConstantBias> {
│ │ │ │ -
173private:
│ │ │ │ +
│ │ │ │ +
141 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
142 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ +
143 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
│ │ │ │ +
144 }
│ │ │ │ +
│ │ │ │ +
145
│ │ │ │ +
147 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
│ │ │ │ +
148 DefaultKeyFormatter) const override;
│ │ │ │ +
149
│ │ │ │ +
151 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
│ │ │ │ +
152
│ │ │ │ +
154 Vector evaluateError(const NavState& p,
│ │ │ │ +
155 boost::optional<Matrix&> H = boost::none) const override;
│ │ │ │ +
156
│ │ │ │ +
157 inline const Point3 & measurementIn() const {
│ │ │ │ +
158 return nT_;
│ │ │ │ +
159 }
│ │ │ │ +
160
│ │ │ │ +
161private:
│ │ │ │ +
162
│ │ │ │ +
164 friend class boost::serialization::access;
│ │ │ │ +
165 template<class ARCHIVE>
│ │ │ │ +
166 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
167 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ +
168 ar
│ │ │ │ +
169 & boost::serialization::make_nvp("NoiseModelFactor1",
│ │ │ │ +
170 boost::serialization::base_object<Base>(*this));
│ │ │ │ +
171 ar & BOOST_SERIALIZATION_NVP(nT_);
│ │ │ │ +
172 }
│ │ │ │ +
173};
│ │ │ │ +
│ │ │ │
174
│ │ │ │ -
175 typedef ImuFactor This;
│ │ │ │ -
176 typedef NoiseModelFactorN<Pose3, Vector3, Pose3, Vector3,
│ │ │ │ -
177 imuBias::ConstantBias> Base;
│ │ │ │ -
178
│ │ │ │ -
179 PreintegratedImuMeasurements _PIM_;
│ │ │ │ -
180
│ │ │ │ -
181public:
│ │ │ │ -
182
│ │ │ │ -
184#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
│ │ │ │ -
185 typedef typename boost::shared_ptr<ImuFactor> shared_ptr;
│ │ │ │ -
186#else
│ │ │ │ -
187 typedef boost::shared_ptr<ImuFactor> shared_ptr;
│ │ │ │ -
188#endif
│ │ │ │ -
189
│ │ │ │ -
191 ImuFactor() {}
│ │ │ │ -
192
│ │ │ │ -
203 ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
│ │ │ │ -
204 const PreintegratedImuMeasurements& preintegratedMeasurements);
│ │ │ │ -
205
│ │ │ │ -
206 ~ImuFactor() override {
│ │ │ │ -
207 }
│ │ │ │ -
208
│ │ │ │ -
210 gtsam::NonlinearFactor::shared_ptr clone() const override;
│ │ │ │ -
211
│ │ │ │ -
214 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor&);
│ │ │ │ -
215 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
│ │ │ │ -
216 DefaultKeyFormatter) const override;
│ │ │ │ -
217 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
│ │ │ │ -
219
│ │ │ │ -
│ │ │ │ -
222 const PreintegratedImuMeasurements& preintegratedMeasurements() const {
│ │ │ │ -
223 return _PIM_;
│ │ │ │ -
224 }
│ │ │ │ -
│ │ │ │ -
225
│ │ │ │ -
229 Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i,
│ │ │ │ -
230 const Pose3& pose_j, const Vector3& vel_j,
│ │ │ │ -
231 const imuBias::ConstantBias& bias_i, boost::optional<Matrix&> H1 =
│ │ │ │ -
232 boost::none, boost::optional<Matrix&> H2 = boost::none,
│ │ │ │ -
233 boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
│ │ │ │ -
234 boost::none, boost::optional<Matrix&> H5 = boost::none) const override;
│ │ │ │ -
235
│ │ │ │ -
236#ifdef GTSAM_TANGENT_PREINTEGRATION
│ │ │ │ -
238 static PreintegratedImuMeasurements Merge(
│ │ │ │ -
239 const PreintegratedImuMeasurements& pim01,
│ │ │ │ -
240 const PreintegratedImuMeasurements& pim12);
│ │ │ │ -
241
│ │ │ │ -
243 static shared_ptr Merge(const shared_ptr& f01, const shared_ptr& f12);
│ │ │ │ -
244#endif
│ │ │ │ -
245
│ │ │ │ -
246 private:
│ │ │ │ -
248 friend class boost::serialization::access;
│ │ │ │ -
249 template<class ARCHIVE>
│ │ │ │ -
250 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
251 // NoiseModelFactor5 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ -
252 ar & boost::serialization::make_nvp("NoiseModelFactor5",
│ │ │ │ -
253 boost::serialization::base_object<Base>(*this));
│ │ │ │ -
254 ar & BOOST_SERIALIZATION_NVP(_PIM_);
│ │ │ │ -
255 }
│ │ │ │ -
256};
│ │ │ │ -
│ │ │ │ -
257// class ImuFactor
│ │ │ │ -
258
│ │ │ │ -
│ │ │ │ -
263class GTSAM_EXPORT ImuFactor2 : public NoiseModelFactorN<NavState, NavState, imuBias::ConstantBias> {
│ │ │ │ -
264private:
│ │ │ │ -
265
│ │ │ │ -
266 typedef ImuFactor2 This;
│ │ │ │ -
267 typedef NoiseModelFactorN<NavState, NavState, imuBias::ConstantBias> Base;
│ │ │ │ -
268
│ │ │ │ -
269 PreintegratedImuMeasurements _PIM_;
│ │ │ │ -
270
│ │ │ │ -
271public:
│ │ │ │ -
272
│ │ │ │ -
274 ImuFactor2() {}
│ │ │ │ -
275
│ │ │ │ -
282 ImuFactor2(Key state_i, Key state_j, Key bias,
│ │ │ │ -
283 const PreintegratedImuMeasurements& preintegratedMeasurements);
│ │ │ │ -
284
│ │ │ │ -
285 ~ImuFactor2() override {
│ │ │ │ -
286 }
│ │ │ │ -
287
│ │ │ │ -
289 gtsam::NonlinearFactor::shared_ptr clone() const override;
│ │ │ │ -
290
│ │ │ │ -
293 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor2&);
│ │ │ │ -
294 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
│ │ │ │ -
295 DefaultKeyFormatter) const override;
│ │ │ │ -
296 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
│ │ │ │ -
298
│ │ │ │ -
│ │ │ │ -
301 const PreintegratedImuMeasurements& preintegratedMeasurements() const {
│ │ │ │ -
302 return _PIM_;
│ │ │ │ -
303 }
│ │ │ │ -
│ │ │ │ -
304
│ │ │ │ -
308 Vector evaluateError(const NavState& state_i, const NavState& state_j,
│ │ │ │ -
309 const imuBias::ConstantBias& bias_i, //
│ │ │ │ -
310 boost::optional<Matrix&> H1 = boost::none,
│ │ │ │ -
311 boost::optional<Matrix&> H2 = boost::none,
│ │ │ │ -
312 boost::optional<Matrix&> H3 = boost::none) const override;
│ │ │ │ -
313
│ │ │ │ -
314private:
│ │ │ │ -
315
│ │ │ │ -
317 friend class boost::serialization::access;
│ │ │ │ -
318 template<class ARCHIVE>
│ │ │ │ -
319 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
320 // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ -
321 ar & boost::serialization::make_nvp("NoiseModelFactor3",
│ │ │ │ -
322 boost::serialization::base_object<Base>(*this));
│ │ │ │ -
323 ar & BOOST_SERIALIZATION_NVP(_PIM_);
│ │ │ │ -
324 }
│ │ │ │ -
325};
│ │ │ │ -
│ │ │ │ -
326// class ImuFactor2
│ │ │ │ -
327
│ │ │ │ -
328template <>
│ │ │ │ -
329struct traits<PreintegratedImuMeasurements> : public Testable<PreintegratedImuMeasurements> {};
│ │ │ │ -
330
│ │ │ │ -
331template <>
│ │ │ │ -
332struct traits<ImuFactor> : public Testable<ImuFactor> {};
│ │ │ │ -
333
│ │ │ │ -
334template <>
│ │ │ │ -
335struct traits<ImuFactor2> : public Testable<ImuFactor2> {};
│ │ │ │ -
336
│ │ │ │ -
337}
│ │ │ │ -
debug.h
Global debugging flags.
│ │ │ │ - │ │ │ │ - │ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
175}
│ │ │ │ +
Pose3.h
3D Pose
│ │ │ │ +
NavState.h
Navigation state composing of attitude, position, and velocity.
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ +
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ -
gtsam::imuBias::ConstantBias
Definition ImuBias.h:30
│ │ │ │ -
gtsam::PreintegratedImuMeasurements
PreintegratedImuMeasurements accumulates (integrates) the IMU measurements (rotation rates and accele...
Definition ImuFactor.h:72
│ │ │ │ -
gtsam::PreintegratedImuMeasurements::~PreintegratedImuMeasurements
~PreintegratedImuMeasurements() override
Virtual destructor.
Definition ImuFactor.h:111
│ │ │ │ -
gtsam::PreintegratedImuMeasurements::PreintegratedImuMeasurements
PreintegratedImuMeasurements(const boost::shared_ptr< PreintegrationParams > &p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias())
Constructor, initializes the class with no measurements.
Definition ImuFactor.h:94
│ │ │ │ -
gtsam::PreintegratedImuMeasurements::PreintegratedImuMeasurements
PreintegratedImuMeasurements(const PreintegrationType &base, const Matrix9 &preintMeasCov)
Construct preintegrated directly from members: base class and preintMeasCov.
Definition ImuFactor.h:105
│ │ │ │ -
gtsam::PreintegratedImuMeasurements::PreintegratedImuMeasurements
PreintegratedImuMeasurements()
Default constructor for serialization and wrappers.
Definition ImuFactor.h:85
│ │ │ │ -
gtsam::PreintegratedImuMeasurements::preintMeasCov
Matrix preintMeasCov() const
Return pre-integrated measurement covariance.
Definition ImuFactor.h:141
│ │ │ │ -
gtsam::PreintegratedImuMeasurements::preintMeasCov_
Matrix9 preintMeasCov_
COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY].
Definition ImuFactor.h:79
│ │ │ │ -
gtsam::ImuFactor
ImuFactor is a 5-ways factor involving previous state (pose and velocity of the vehicle at previous t...
Definition ImuFactor.h:172
│ │ │ │ -
gtsam::ImuFactor::preintegratedMeasurements
const PreintegratedImuMeasurements & preintegratedMeasurements() const
Access the preintegrated measurements.
Definition ImuFactor.h:222
│ │ │ │ -
gtsam::ImuFactor::ImuFactor
ImuFactor()
Default constructor - only use for serialization.
Definition ImuFactor.h:191
│ │ │ │ -
gtsam::ImuFactor::shared_ptr
boost::shared_ptr< ImuFactor > shared_ptr
Shorthand for a smart pointer to a factor.
Definition ImuFactor.h:187
│ │ │ │ -
gtsam::ImuFactor2
ImuFactor2 is a ternary factor that uses NavStates rather than Pose/Velocity.
Definition ImuFactor.h:263
│ │ │ │ -
gtsam::ImuFactor2::ImuFactor2
ImuFactor2()
Default constructor - only use for serialization.
Definition ImuFactor.h:274
│ │ │ │ -
gtsam::ImuFactor2::preintegratedMeasurements
const PreintegratedImuMeasurements & preintegratedMeasurements() const
Access the preintegrated measurements.
Definition ImuFactor.h:301
│ │ │ │ -
gtsam::ManifoldPreintegration
IMU pre-integration on NavSatet manifold.
Definition ManifoldPreintegration.h:33
│ │ │ │ +
gtsam::GPSFactor
Prior on position in a Cartesian frame.
Definition GPSFactor.h:35
│ │ │ │ +
gtsam::GPSFactor::This
GPSFactor This
Typedef to this class.
Definition GPSFactor.h:49
│ │ │ │ +
gtsam::GPSFactor::GPSFactor
GPSFactor()
default constructor - only use for serialization
Definition GPSFactor.h:52
│ │ │ │ +
gtsam::GPSFactor::GPSFactor
GPSFactor(Key key, const Point3 &gpsIn, const SharedNoiseModel &model)
Constructor from a measurement in a Cartesian frame.
Definition GPSFactor.h:63
│ │ │ │ +
gtsam::GPSFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition GPSFactor.h:68
│ │ │ │ +
gtsam::GPSFactor::shared_ptr
boost::shared_ptr< GPSFactor > shared_ptr
shorthand for a smart pointer to a factor
Definition GPSFactor.h:46
│ │ │ │ +
gtsam::GPSFactor2
Version of GPSFactor for NavState.
Definition GPSFactor.h:114
│ │ │ │ +
gtsam::GPSFactor2::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition GPSFactor.h:141
│ │ │ │ +
gtsam::GPSFactor2::This
GPSFactor2 This
Typedef to this class.
Definition GPSFactor.h:128
│ │ │ │ +
gtsam::GPSFactor2::GPSFactor2
GPSFactor2(Key key, const Point3 &gpsIn, const SharedNoiseModel &model)
Constructor from a measurement in a Cartesian frame.
Definition GPSFactor.h:136
│ │ │ │ +
gtsam::GPSFactor2::shared_ptr
boost::shared_ptr< GPSFactor2 > shared_ptr
shorthand for a smart pointer to a factor
Definition GPSFactor.h:125
│ │ │ │ +
gtsam::GPSFactor2::GPSFactor2
GPSFactor2()
default constructor - only use for serialization
Definition GPSFactor.h:131
│ │ │ │
gtsam::NavState
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition NavState.h:34
│ │ │ │ +
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,361 +1,236 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ImuFactor.h │ │ │ │ │ +GPSFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -22#pragma once │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_N_a_v_S_t_a_t_e_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_3_._h> │ │ │ │ │ 23 │ │ │ │ │ -24/* GTSAM includes */ │ │ │ │ │ -25#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_._h> │ │ │ │ │ -27#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n_._h> │ │ │ │ │ -28#include <_g_t_s_a_m_/_b_a_s_e_/_d_e_b_u_g_._h> │ │ │ │ │ -29 │ │ │ │ │ -30namespace _g_t_s_a_m { │ │ │ │ │ -31 │ │ │ │ │ -32#ifdef GTSAM_TANGENT_PREINTEGRATION │ │ │ │ │ -33typedef TangentPreintegration PreintegrationType; │ │ │ │ │ -34#else │ │ │ │ │ -35typedef ManifoldPreintegration PreintegrationType; │ │ │ │ │ -36#endif │ │ │ │ │ -37 │ │ │ │ │ -38/* │ │ │ │ │ -39 * If you are using the factor, please cite: │ │ │ │ │ -40 * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, "Eliminating │ │ │ │ │ -41 * conditionally independent sets in factor graphs: a unifying perspective │ │ │ │ │ -based │ │ │ │ │ -42 * on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014. │ │ │ │ │ -43 * │ │ │ │ │ -44 * C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on │ │ │ │ │ -45 * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", │ │ │ │ │ -46 * Robotics: Science and Systems (RSS), 2015. │ │ │ │ │ -47 * │ │ │ │ │ -48 * REFERENCES: │ │ │ │ │ -49 * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie │ │ │ │ │ -Groups", │ │ │ │ │ -50 * Volume 2, 2008. │ │ │ │ │ -51 * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for │ │ │ │ │ -52 * High-Dynamic Motion in Built Environments Without Initial Conditions", │ │ │ │ │ -53 * TRO, 28(1):61-76, 2012. │ │ │ │ │ -54 * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: │ │ │ │ │ -55 * Computation of the Jacobian Matrices", Tech. Report, 2013. │ │ │ │ │ -56 * Available in this repo as "PreintegratedIMUJacobians.pdf". │ │ │ │ │ -57 * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU │ │ │ │ │ -Preintegration on │ │ │ │ │ -58 * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", │ │ │ │ │ -59 * Robotics: Science and Systems (RSS), 2015. │ │ │ │ │ -60 */ │ │ │ │ │ -61 │ │ │ │ │ -_7_2class GTSAM_EXPORT _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s: public _P_r_e_i_n_t_e_g_r_a_t_i_o_n_T_y_p_e { │ │ │ │ │ -73 │ │ │ │ │ -74 friend class _I_m_u_F_a_c_t_o_r; │ │ │ │ │ -75 friend class _I_m_u_F_a_c_t_o_r_2; │ │ │ │ │ +24namespace _g_t_s_a_m { │ │ │ │ │ +25 │ │ │ │ │ +_3_5class GTSAM_EXPORT _G_P_S_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +36 │ │ │ │ │ +37private: │ │ │ │ │ +38 │ │ │ │ │ +39 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_P_o_s_e_3_> _B_a_s_e; │ │ │ │ │ +40 │ │ │ │ │ +41 _P_o_i_n_t_3 nT_; │ │ │ │ │ +42 │ │ │ │ │ +43public: │ │ │ │ │ +44 │ │ │ │ │ +_4_6 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +47 │ │ │ │ │ +_4_9 typedef _G_P_S_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ +50 │ │ │ │ │ +_5_2 _G_P_S_F_a_c_t_o_r(): nT_(0, 0, 0) {} │ │ │ │ │ +53 │ │ │ │ │ +54 _~_G_P_S_F_a_c_t_o_r() override {} │ │ │ │ │ +55 │ │ │ │ │ +_6_3 _G_P_S_F_a_c_t_o_r(_K_e_y key, const _P_o_i_n_t_3& gpsIn, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model) : │ │ │ │ │ +64 _B_a_s_e(model, key), nT_(gpsIn) { │ │ │ │ │ +65 } │ │ │ │ │ +66 │ │ │ │ │ +_6_8 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +69 return boost::static_pointer_cast( │ │ │ │ │ +70 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); │ │ │ │ │ +71 } │ │ │ │ │ +72 │ │ │ │ │ +74 void _p_r_i_n_t(const std::string& s = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ +75 DefaultKeyFormatter) const override; │ │ │ │ │ 76 │ │ │ │ │ -77protected: │ │ │ │ │ -78 │ │ │ │ │ -_7_9 Matrix9 _p_r_e_i_n_t_M_e_a_s_C_o_v__; │ │ │ │ │ -81 │ │ │ │ │ -82public: │ │ │ │ │ +78 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& expected, double tol = 1e-9) const │ │ │ │ │ +override; │ │ │ │ │ +79 │ │ │ │ │ +81 Vector evaluateError(const _P_o_s_e_3& p, │ │ │ │ │ +82 boost::optional H = boost::none) const override; │ │ │ │ │ 83 │ │ │ │ │ -_8_5 _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s() { │ │ │ │ │ -86 preintMeasCov_.setZero(); │ │ │ │ │ -87 } │ │ │ │ │ -88 │ │ │ │ │ -_9_4 _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s(const boost::shared_ptr& │ │ │ │ │ -p, │ │ │ │ │ -95 const _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s& biasHat = _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s()) : │ │ │ │ │ -96 _P_r_e_i_n_t_e_g_r_a_t_i_o_n_T_y_p_e(p, biasHat) { │ │ │ │ │ -97 preintMeasCov_.setZero(); │ │ │ │ │ -98 } │ │ │ │ │ -99 │ │ │ │ │ -_1_0_5 _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s(const _P_r_e_i_n_t_e_g_r_a_t_i_o_n_T_y_p_e& base, const Matrix9& │ │ │ │ │ -preintMeasCov) │ │ │ │ │ -106 : _P_r_e_i_n_t_e_g_r_a_t_i_o_n_T_y_p_e(base), │ │ │ │ │ -107 preintMeasCov_(preintMeasCov) { │ │ │ │ │ -108 } │ │ │ │ │ +84 inline const _P_o_i_n_t_3 & measurementIn() const { │ │ │ │ │ +85 return nT_; │ │ │ │ │ +86 } │ │ │ │ │ +87 │ │ │ │ │ +93 static std::pair EstimateState(double t1, const Point3& │ │ │ │ │ +NED1, │ │ │ │ │ +94 double t2, const Point3& NED2, double timestamp); │ │ │ │ │ +95 │ │ │ │ │ +96private: │ │ │ │ │ +97 │ │ │ │ │ +_9_9 friend class boost::serialization::access; │ │ │ │ │ +100 template │ │ │ │ │ +101 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +102 // NoiseModelFactor1 instead of NoiseModelFactorN for backward │ │ │ │ │ +compatibility │ │ │ │ │ +103 ar │ │ │ │ │ +104 & boost::serialization::make_nvp("NoiseModelFactor1", │ │ │ │ │ +105 boost::serialization::base_object(*this)); │ │ │ │ │ +106 ar & BOOST_SERIALIZATION_NVP(nT_); │ │ │ │ │ +107 } │ │ │ │ │ +108}; │ │ │ │ │ 109 │ │ │ │ │ -_1_1_1 _~_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s() override { │ │ │ │ │ -112 } │ │ │ │ │ -113 │ │ │ │ │ -115 void _p_r_i_n_t(const std::string& s = "Preintegrated Measurements:") const │ │ │ │ │ -override; │ │ │ │ │ -116 │ │ │ │ │ -118 bool _e_q_u_a_l_s(const _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s& expected, double tol = 1e- │ │ │ │ │ -9) const; │ │ │ │ │ +_1_1_4class GTSAM_EXPORT _G_P_S_F_a_c_t_o_r_2: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +115 │ │ │ │ │ +116private: │ │ │ │ │ +117 │ │ │ │ │ +118 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_N_a_v_S_t_a_t_e_> _B_a_s_e; │ │ │ │ │ 119 │ │ │ │ │ -121 void resetIntegration() override; │ │ │ │ │ -122 │ │ │ │ │ -133 void integrateMeasurement(const Vector3& measuredAcc, │ │ │ │ │ -134 const Vector3& measuredOmega, const double dt) override; │ │ │ │ │ -135 │ │ │ │ │ -137 void integrateMeasurements(const Matrix& measuredAccs, const Matrix& │ │ │ │ │ -measuredOmegas, │ │ │ │ │ -138 const Matrix& dts); │ │ │ │ │ +120 _P_o_i_n_t_3 nT_; │ │ │ │ │ +121 │ │ │ │ │ +122public: │ │ │ │ │ +123 │ │ │ │ │ +_1_2_5 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +126 │ │ │ │ │ +_1_2_8 typedef _G_P_S_F_a_c_t_o_r_2 _T_h_i_s; │ │ │ │ │ +129 │ │ │ │ │ +_1_3_1 _G_P_S_F_a_c_t_o_r_2():nT_(0, 0, 0) {} │ │ │ │ │ +132 │ │ │ │ │ +133 _~_G_P_S_F_a_c_t_o_r_2() override {} │ │ │ │ │ +134 │ │ │ │ │ +_1_3_6 _G_P_S_F_a_c_t_o_r_2(_K_e_y key, const _P_o_i_n_t_3& gpsIn, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model) : │ │ │ │ │ +137 _B_a_s_e(model, key), nT_(gpsIn) { │ │ │ │ │ +138 } │ │ │ │ │ 139 │ │ │ │ │ -_1_4_1 Matrix _p_r_e_i_n_t_M_e_a_s_C_o_v() const { return preintMeasCov_; } │ │ │ │ │ -142 │ │ │ │ │ -143#ifdef GTSAM_TANGENT_PREINTEGRATION │ │ │ │ │ -145 void mergeWith(const _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s& pim, Matrix9* H1, │ │ │ │ │ -Matrix9* H2); │ │ │ │ │ -146#endif │ │ │ │ │ -147 │ │ │ │ │ -148 private: │ │ │ │ │ -_1_5_0 friend class boost::serialization::access; │ │ │ │ │ -151 template │ │ │ │ │ -152 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -153 namespace bs = ::boost::serialization; │ │ │ │ │ -154 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_P_r_e_i_n_t_e_g_r_a_t_i_o_n_T_y_p_e); │ │ │ │ │ -155 ar & BOOST_SERIALIZATION_NVP(preintMeasCov_); │ │ │ │ │ -156 } │ │ │ │ │ -157}; │ │ │ │ │ -158 │ │ │ │ │ -_1_7_1class GTSAM_EXPORT _I_m_u_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -173private: │ │ │ │ │ -174 │ │ │ │ │ -175 typedef _I_m_u_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ -176 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N<_P_o_s_e_3, Vector3, _P_o_s_e_3, Vector3, │ │ │ │ │ -177 _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s> _B_a_s_e; │ │ │ │ │ -178 │ │ │ │ │ -179 _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s _PIM_; │ │ │ │ │ -180 │ │ │ │ │ -181public: │ │ │ │ │ -182 │ │ │ │ │ -184#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 │ │ │ │ │ -185 typedef typename boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -186#else │ │ │ │ │ -_1_8_7 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -188#endif │ │ │ │ │ -189 │ │ │ │ │ -_1_9_1 _I_m_u_F_a_c_t_o_r() {} │ │ │ │ │ -192 │ │ │ │ │ -203 _I_m_u_F_a_c_t_o_r(_K_e_y pose_i, _K_e_y vel_i, _K_e_y pose_j, _K_e_y vel_j, _K_e_y bias, │ │ │ │ │ -204 const _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s& preintegratedMeasurements); │ │ │ │ │ -205 │ │ │ │ │ -206 _~_I_m_u_F_a_c_t_o_r() override { │ │ │ │ │ -207 } │ │ │ │ │ -208 │ │ │ │ │ -210 gtsam::NonlinearFactor::shared_ptr clone() const override; │ │ │ │ │ -211 │ │ │ │ │ -214 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const │ │ │ │ │ -ImuFactor&); │ │ │ │ │ -215 void print(const std::string& s = "", const KeyFormatter& keyFormatter = │ │ │ │ │ -216 DefaultKeyFormatter) const override; │ │ │ │ │ -217 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const │ │ │ │ │ +_1_4_1 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +142 return boost::static_pointer_cast( │ │ │ │ │ +143 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); │ │ │ │ │ +144 } │ │ │ │ │ +145 │ │ │ │ │ +147 void _p_r_i_n_t(const std::string& s = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ +148 DefaultKeyFormatter) const override; │ │ │ │ │ +149 │ │ │ │ │ +151 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& expected, double tol = 1e-9) const │ │ │ │ │ override; │ │ │ │ │ -219 │ │ │ │ │ -_2_2_2 const _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s& _p_r_e_i_n_t_e_g_r_a_t_e_d_M_e_a_s_u_r_e_m_e_n_t_s() const { │ │ │ │ │ -223 return _PIM_; │ │ │ │ │ -224 } │ │ │ │ │ -225 │ │ │ │ │ -229 Vector evaluateError(const _P_o_s_e_3& pose_i, const Vector3& vel_i, │ │ │ │ │ -230 const _P_o_s_e_3& pose_j, const Vector3& vel_j, │ │ │ │ │ -231 const _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s& bias_i, boost::optional H1 = │ │ │ │ │ -232 boost::none, boost::optional H2 = boost::none, │ │ │ │ │ -233 boost::optional H3 = boost::none, boost::optional H4 = │ │ │ │ │ -234 boost::none, boost::optional H5 = boost::none) const override; │ │ │ │ │ -235 │ │ │ │ │ -236#ifdef GTSAM_TANGENT_PREINTEGRATION │ │ │ │ │ -238 static _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s Merge( │ │ │ │ │ -239 const _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s& pim01, │ │ │ │ │ -240 const _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s& pim12); │ │ │ │ │ -241 │ │ │ │ │ -243 static shared_ptr Merge(const shared_ptr& f01, const shared_ptr& f12); │ │ │ │ │ -244#endif │ │ │ │ │ -245 │ │ │ │ │ -246 private: │ │ │ │ │ -_2_4_8 friend class boost::serialization::access; │ │ │ │ │ -249 template │ │ │ │ │ -250 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -251 // NoiseModelFactor5 instead of NoiseModelFactorN for backward │ │ │ │ │ +152 │ │ │ │ │ +154 Vector evaluateError(const _N_a_v_S_t_a_t_e& p, │ │ │ │ │ +155 boost::optional H = boost::none) const override; │ │ │ │ │ +156 │ │ │ │ │ +157 inline const _P_o_i_n_t_3 & measurementIn() const { │ │ │ │ │ +158 return nT_; │ │ │ │ │ +159 } │ │ │ │ │ +160 │ │ │ │ │ +161private: │ │ │ │ │ +162 │ │ │ │ │ +_1_6_4 friend class boost::serialization::access; │ │ │ │ │ +165 template │ │ │ │ │ +166 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +167 // NoiseModelFactor1 instead of NoiseModelFactorN for backward │ │ │ │ │ compatibility │ │ │ │ │ -252 ar & boost::serialization::make_nvp("NoiseModelFactor5", │ │ │ │ │ -253 boost::serialization::base_object(*this)); │ │ │ │ │ -254 ar & BOOST_SERIALIZATION_NVP(_PIM_); │ │ │ │ │ -255 } │ │ │ │ │ -256}; │ │ │ │ │ -257// class ImuFactor │ │ │ │ │ -258 │ │ │ │ │ -_2_6_3class GTSAM_EXPORT _I_m_u_F_a_c_t_o_r_2 : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -264private: │ │ │ │ │ -265 │ │ │ │ │ -266 typedef _I_m_u_F_a_c_t_o_r_2 _T_h_i_s; │ │ │ │ │ -267 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_N_a_v_S_t_a_t_e_,_ _N_a_v_S_t_a_t_e_,_ _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_> _B_a_s_e; │ │ │ │ │ -268 │ │ │ │ │ -269 _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s _PIM_; │ │ │ │ │ -270 │ │ │ │ │ -271public: │ │ │ │ │ -272 │ │ │ │ │ -_2_7_4 _I_m_u_F_a_c_t_o_r_2() {} │ │ │ │ │ -275 │ │ │ │ │ -282 _I_m_u_F_a_c_t_o_r_2(_K_e_y state_i, _K_e_y state_j, _K_e_y bias, │ │ │ │ │ -283 const _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s& preintegratedMeasurements); │ │ │ │ │ -284 │ │ │ │ │ -285 _~_I_m_u_F_a_c_t_o_r_2() override { │ │ │ │ │ -286 } │ │ │ │ │ -287 │ │ │ │ │ -289 gtsam::NonlinearFactor::shared_ptr clone() const override; │ │ │ │ │ -290 │ │ │ │ │ -293 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const │ │ │ │ │ -ImuFactor2&); │ │ │ │ │ -294 void print(const std::string& s = "", const KeyFormatter& keyFormatter = │ │ │ │ │ -295 DefaultKeyFormatter) const override; │ │ │ │ │ -296 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const │ │ │ │ │ -override; │ │ │ │ │ -298 │ │ │ │ │ -_3_0_1 const _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s& _p_r_e_i_n_t_e_g_r_a_t_e_d_M_e_a_s_u_r_e_m_e_n_t_s() const { │ │ │ │ │ -302 return _PIM_; │ │ │ │ │ -303 } │ │ │ │ │ -304 │ │ │ │ │ -308 Vector evaluateError(const _N_a_v_S_t_a_t_e& state_i, const _N_a_v_S_t_a_t_e& state_j, │ │ │ │ │ -309 const _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s& bias_i, // │ │ │ │ │ -310 boost::optional H1 = boost::none, │ │ │ │ │ -311 boost::optional H2 = boost::none, │ │ │ │ │ -312 boost::optional H3 = boost::none) const override; │ │ │ │ │ -313 │ │ │ │ │ -314private: │ │ │ │ │ -315 │ │ │ │ │ -_3_1_7 friend class boost::serialization::access; │ │ │ │ │ -318 template │ │ │ │ │ -319 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -320 // NoiseModelFactor3 instead of NoiseModelFactorN for backward │ │ │ │ │ -compatibility │ │ │ │ │ -321 ar & boost::serialization::make_nvp("NoiseModelFactor3", │ │ │ │ │ -322 boost::serialization::base_object(*this)); │ │ │ │ │ -323 ar & BOOST_SERIALIZATION_NVP(_PIM_); │ │ │ │ │ -324 } │ │ │ │ │ -325}; │ │ │ │ │ -326// class ImuFactor2 │ │ │ │ │ -327 │ │ │ │ │ -328template <> │ │ │ │ │ -_3_2_9struct _t_r_a_i_t_s<_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s> : public │ │ │ │ │ -_T_e_s_t_a_b_l_e {}; │ │ │ │ │ -330 │ │ │ │ │ -331template <> │ │ │ │ │ -_3_3_2struct _t_r_a_i_t_s<_I_m_u_F_a_c_t_o_r> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ -333 │ │ │ │ │ -334template <> │ │ │ │ │ -_3_3_5struct _t_r_a_i_t_s<_I_m_u_F_a_c_t_o_r_2> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ -336 │ │ │ │ │ -337} │ │ │ │ │ -_d_e_b_u_g_._h │ │ │ │ │ -Global debugging flags. │ │ │ │ │ -_T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n_._h │ │ │ │ │ -_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_._h │ │ │ │ │ +168 ar │ │ │ │ │ +169 & boost::serialization::make_nvp("NoiseModelFactor1", │ │ │ │ │ +170 boost::serialization::base_object(*this)); │ │ │ │ │ +171 ar & BOOST_SERIALIZATION_NVP(nT_); │ │ │ │ │ +172 } │ │ │ │ │ +173}; │ │ │ │ │ +174 │ │ │ │ │ +175} │ │ │ │ │ +_P_o_s_e_3_._h │ │ │ │ │ +3D Pose │ │ │ │ │ +_N_a_v_S_t_a_t_e_._h │ │ │ │ │ +Navigation state composing of attitude, position, and velocity. │ │ │ │ │ _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ print without optional string, must specify cout yourself │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ +Vector3 Point3 │ │ │ │ │ +As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ +typedef Point3 to Vector3... │ │ │ │ │ +DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ +Aliases. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ _g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ Template to create a binary predicate. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ _g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s │ │ │ │ │ -DDeeffiinniittiioonn ImuBias.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ -PreintegratedImuMeasurements accumulates (integrates) the IMU measurements │ │ │ │ │ -(rotation rates and accele... │ │ │ │ │ -DDeeffiinniittiioonn ImuFactor.h:72 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s_:_:_~_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ -~PreintegratedImuMeasurements() override │ │ │ │ │ -Virtual destructor. │ │ │ │ │ -DDeeffiinniittiioonn ImuFactor.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ -PreintegratedImuMeasurements(const boost::shared_ptr< PreintegrationParams > │ │ │ │ │ -&p, const imuBias::ConstantBias &biasHat=imuBias::ConstantBias()) │ │ │ │ │ -Constructor, initializes the class with no measurements. │ │ │ │ │ -DDeeffiinniittiioonn ImuFactor.h:94 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ -PreintegratedImuMeasurements(const PreintegrationType &base, const Matrix9 │ │ │ │ │ -&preintMeasCov) │ │ │ │ │ -Construct preintegrated directly from members: base class and preintMeasCov. │ │ │ │ │ -DDeeffiinniittiioonn ImuFactor.h:105 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ -PreintegratedImuMeasurements() │ │ │ │ │ -Default constructor for serialization and wrappers. │ │ │ │ │ -DDeeffiinniittiioonn ImuFactor.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s_:_:_p_r_e_i_n_t_M_e_a_s_C_o_v │ │ │ │ │ -Matrix preintMeasCov() const │ │ │ │ │ -Return pre-integrated measurement covariance. │ │ │ │ │ -DDeeffiinniittiioonn ImuFactor.h:141 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s_:_:_p_r_e_i_n_t_M_e_a_s_C_o_v__ │ │ │ │ │ -Matrix9 preintMeasCov_ │ │ │ │ │ -COVARIANCE OF: [PreintROTATION PreintPOSITION PreintVELOCITY]. │ │ │ │ │ -DDeeffiinniittiioonn ImuFactor.h:79 │ │ │ │ │ -_g_t_s_a_m_:_:_I_m_u_F_a_c_t_o_r │ │ │ │ │ -ImuFactor is a 5-ways factor involving previous state (pose and velocity of the │ │ │ │ │ -vehicle at previous t... │ │ │ │ │ -DDeeffiinniittiioonn ImuFactor.h:172 │ │ │ │ │ -_g_t_s_a_m_:_:_I_m_u_F_a_c_t_o_r_:_:_p_r_e_i_n_t_e_g_r_a_t_e_d_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ -const PreintegratedImuMeasurements & preintegratedMeasurements() const │ │ │ │ │ -Access the preintegrated measurements. │ │ │ │ │ -DDeeffiinniittiioonn ImuFactor.h:222 │ │ │ │ │ -_g_t_s_a_m_:_:_I_m_u_F_a_c_t_o_r_:_:_I_m_u_F_a_c_t_o_r │ │ │ │ │ -ImuFactor() │ │ │ │ │ -Default constructor - only use for serialization. │ │ │ │ │ -DDeeffiinniittiioonn ImuFactor.h:191 │ │ │ │ │ -_g_t_s_a_m_:_:_I_m_u_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< ImuFactor > shared_ptr │ │ │ │ │ -Shorthand for a smart pointer to a factor. │ │ │ │ │ -DDeeffiinniittiioonn ImuFactor.h:187 │ │ │ │ │ -_g_t_s_a_m_:_:_I_m_u_F_a_c_t_o_r_2 │ │ │ │ │ -ImuFactor2 is a ternary factor that uses NavStates rather than Pose/Velocity. │ │ │ │ │ -DDeeffiinniittiioonn ImuFactor.h:263 │ │ │ │ │ -_g_t_s_a_m_:_:_I_m_u_F_a_c_t_o_r_2_:_:_I_m_u_F_a_c_t_o_r_2 │ │ │ │ │ -ImuFactor2() │ │ │ │ │ -Default constructor - only use for serialization. │ │ │ │ │ -DDeeffiinniittiioonn ImuFactor.h:274 │ │ │ │ │ -_g_t_s_a_m_:_:_I_m_u_F_a_c_t_o_r_2_:_:_p_r_e_i_n_t_e_g_r_a_t_e_d_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ -const PreintegratedImuMeasurements & preintegratedMeasurements() const │ │ │ │ │ -Access the preintegrated measurements. │ │ │ │ │ -DDeeffiinniittiioonn ImuFactor.h:301 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n │ │ │ │ │ -IMU pre-integration on NavSatet manifold. │ │ │ │ │ -DDeeffiinniittiioonn ManifoldPreintegration.h:33 │ │ │ │ │ +_g_t_s_a_m_:_:_G_P_S_F_a_c_t_o_r │ │ │ │ │ +Prior on position in a Cartesian frame. │ │ │ │ │ +DDeeffiinniittiioonn GPSFactor.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_G_P_S_F_a_c_t_o_r_:_:_T_h_i_s │ │ │ │ │ +GPSFactor This │ │ │ │ │ +Typedef to this class. │ │ │ │ │ +DDeeffiinniittiioonn GPSFactor.h:49 │ │ │ │ │ +_g_t_s_a_m_:_:_G_P_S_F_a_c_t_o_r_:_:_G_P_S_F_a_c_t_o_r │ │ │ │ │ +GPSFactor() │ │ │ │ │ +default constructor - only use for serialization │ │ │ │ │ +DDeeffiinniittiioonn GPSFactor.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_G_P_S_F_a_c_t_o_r_:_:_G_P_S_F_a_c_t_o_r │ │ │ │ │ +GPSFactor(Key key, const Point3 &gpsIn, const SharedNoiseModel &model) │ │ │ │ │ +Constructor from a measurement in a Cartesian frame. │ │ │ │ │ +DDeeffiinniittiioonn GPSFactor.h:63 │ │ │ │ │ +_g_t_s_a_m_:_:_G_P_S_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ +gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn GPSFactor.h:68 │ │ │ │ │ +_g_t_s_a_m_:_:_G_P_S_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< GPSFactor > shared_ptr │ │ │ │ │ +shorthand for a smart pointer to a factor │ │ │ │ │ +DDeeffiinniittiioonn GPSFactor.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_G_P_S_F_a_c_t_o_r_2 │ │ │ │ │ +Version of GPSFactor for NavState. │ │ │ │ │ +DDeeffiinniittiioonn GPSFactor.h:114 │ │ │ │ │ +_g_t_s_a_m_:_:_G_P_S_F_a_c_t_o_r_2_:_:_c_l_o_n_e │ │ │ │ │ +gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn GPSFactor.h:141 │ │ │ │ │ +_g_t_s_a_m_:_:_G_P_S_F_a_c_t_o_r_2_:_:_T_h_i_s │ │ │ │ │ +GPSFactor2 This │ │ │ │ │ +Typedef to this class. │ │ │ │ │ +DDeeffiinniittiioonn GPSFactor.h:128 │ │ │ │ │ +_g_t_s_a_m_:_:_G_P_S_F_a_c_t_o_r_2_:_:_G_P_S_F_a_c_t_o_r_2 │ │ │ │ │ +GPSFactor2(Key key, const Point3 &gpsIn, const SharedNoiseModel &model) │ │ │ │ │ +Constructor from a measurement in a Cartesian frame. │ │ │ │ │ +DDeeffiinniittiioonn GPSFactor.h:136 │ │ │ │ │ +_g_t_s_a_m_:_:_G_P_S_F_a_c_t_o_r_2_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< GPSFactor2 > shared_ptr │ │ │ │ │ +shorthand for a smart pointer to a factor │ │ │ │ │ +DDeeffiinniittiioonn GPSFactor.h:125 │ │ │ │ │ +_g_t_s_a_m_:_:_G_P_S_F_a_c_t_o_r_2_:_:_G_P_S_F_a_c_t_o_r_2 │ │ │ │ │ +GPSFactor2() │ │ │ │ │ +default constructor - only use for serialization │ │ │ │ │ +DDeeffiinniittiioonn GPSFactor.h:131 │ │ │ │ │ _g_t_s_a_m_:_:_N_a_v_S_t_a_t_e │ │ │ │ │ Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does │ │ │ │ │ not make sense to make... │ │ │ │ │ DDeeffiinniittiioonn NavState.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ +Nonlinear factor base class. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ variables. │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _I_m_u_F_a_c_t_o_r_._h │ │ │ │ │ + * _G_P_S_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00989.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/BarometricFactor.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ManifoldPreintegration.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,35 +95,40 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces
│ │ │ │ -
BarometricFactor.cpp File Reference
│ │ │ │ +
ManifoldPreintegration.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -

Implementation file for Barometric factor. │ │ │ │ -More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Implementation file for Barometric factor.

│ │ │ │ -
Author
Peter Milani
│ │ │ │ -
Date
December 16, 2021
│ │ │ │ +
Author
Luca Carlone
│ │ │ │ +
│ │ │ │ +Stephen Williams
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │ +
│ │ │ │ +Vadim Indelman
│ │ │ │ +
│ │ │ │ +David Jensen
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,23 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -BarometricFactor.cpp File Reference │ │ │ │ │ -Implementation file for Barometric factor. _M_o_r_e_._._. │ │ │ │ │ +ManifoldPreintegration.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Implementation file for Barometric factor. │ │ │ │ │ Author │ │ │ │ │ - Peter Milani │ │ │ │ │ - Date │ │ │ │ │ - December 16, 2021 │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Stephen Williams │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Vadim Indelman │ │ │ │ │ + David Jensen │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r_._c_p_p │ │ │ │ │ + * _M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00992.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ManifoldPreintegration.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/PreintegrationBase.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,24 +96,24 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
ManifoldPreintegration.h File Reference
│ │ │ │ +
PreintegrationBase.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::ManifoldPreintegration
 IMU pre-integration on NavSatet manifold. More...
class  gtsam::PreintegrationBase
 PreintegrationBase is the base class for PreintegratedMeasurements (in ImuFactor) and CombinedPreintegratedMeasurements (in CombinedImuFactor). More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -125,19 +125,33 @@ │ │ │ │
│ │ │ │ Richard Roberts
│ │ │ │
│ │ │ │ Vadim Indelman
│ │ │ │
│ │ │ │ David Jensen
│ │ │ │
│ │ │ │ +Frank Dellaert
│ │ │ │ +
│ │ │ │ +Varun Agrawal
│ │ │ │ +
│ │ │ │ +Luca Carlone
│ │ │ │ +
│ │ │ │ +Stephen Williams
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │ +
│ │ │ │ +Vadim Indelman
│ │ │ │ +
│ │ │ │ +David Jensen
│ │ │ │ +
│ │ │ │ Frank Dellaert
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,37 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -ManifoldPreintegration.h File Reference │ │ │ │ │ +PreintegrationBase.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n │ │ │ │ │ -  IMU pre-integration on NavSatet manifold. _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e │ │ │ │ │ + _P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e is the base class for PreintegratedMeasurements (in │ │ │ │ │ +  _I_m_u_F_a_c_t_o_r) and CombinedPreintegratedMeasurements (in │ │ │ │ │ + _C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r). _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Author │ │ │ │ │ Luca Carlone │ │ │ │ │ Stephen Williams │ │ │ │ │ Richard Roberts │ │ │ │ │ Vadim Indelman │ │ │ │ │ David Jensen │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Varun Agrawal │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Stephen Williams │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Vadim Indelman │ │ │ │ │ + David Jensen │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_._h │ │ │ │ │ + * _P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00992.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a00992 = [ │ │ │ │ │ - ["gtsam::ManifoldPreintegration", "a04192.html", "a04192"] │ │ │ │ │ + ["gtsam::PreintegrationBase", "a04216.html", "a04216"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00992_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ManifoldPreintegration.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/PreintegrationBase.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,15 +98,15 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ManifoldPreintegration.h
│ │ │ │ +
PreintegrationBase.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │ @@ -114,108 +114,159 @@ │ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
22#pragma once
│ │ │ │
23
│ │ │ │ -
24#include <gtsam/navigation/NavState.h>
│ │ │ │ -
25#include <gtsam/navigation/PreintegrationBase.h>
│ │ │ │ -
26
│ │ │ │ -
27namespace gtsam {
│ │ │ │ +
24#include <gtsam/navigation/PreintegrationParams.h>
│ │ │ │ +
25#include <gtsam/navigation/NavState.h>
│ │ │ │ +
26#include <gtsam/navigation/ImuBias.h>
│ │ │ │ +
27#include <gtsam/linear/NoiseModel.h>
│ │ │ │
28
│ │ │ │ -
│ │ │ │ -
33class GTSAM_EXPORT ManifoldPreintegration : public PreintegrationBase {
│ │ │ │ -
34 protected:
│ │ │ │ -
35
│ │ │ │ -
41 NavState deltaXij_;
│ │ │ │ -
42 Matrix3 delRdelBiasOmega_;
│ │ │ │ -
43 Matrix3 delPdelBiasAcc_;
│ │ │ │ -
44 Matrix3 delPdelBiasOmega_;
│ │ │ │ -
45 Matrix3 delVdelBiasAcc_;
│ │ │ │ -
46 Matrix3 delVdelBiasOmega_;
│ │ │ │ -
47
│ │ │ │ -
│ │ │ │ -
49 ManifoldPreintegration() {
│ │ │ │ -
50 resetIntegration();
│ │ │ │ -
51 }
│ │ │ │ -
│ │ │ │ -
52
│ │ │ │ -
53public:
│ │ │ │ -
56
│ │ │ │ -
62 ManifoldPreintegration(const boost::shared_ptr<Params>& p,
│ │ │ │ -
63 const imuBias::ConstantBias& biasHat = imuBias::ConstantBias());
│ │ │ │ +
29#include <iosfwd>
│ │ │ │ +
30#include <string>
│ │ │ │ +
31#include <utility>
│ │ │ │ +
32
│ │ │ │ +
33namespace gtsam {
│ │ │ │ +
34
│ │ │ │ +
│ │ │ │ +
41class GTSAM_EXPORT PreintegrationBase {
│ │ │ │ +
42 public:
│ │ │ │ +
43 typedef imuBias::ConstantBias Bias;
│ │ │ │ +
44 typedef PreintegrationParams Params;
│ │ │ │ +
45
│ │ │ │ +
46 protected:
│ │ │ │ +
47 boost::shared_ptr<Params> p_;
│ │ │ │ +
48
│ │ │ │ +
50 Bias biasHat_;
│ │ │ │ +
51
│ │ │ │ +
53 double deltaTij_;
│ │ │ │ +
54
│ │ │ │ +
56 PreintegrationBase() {}
│ │ │ │ +
57
│ │ │ │ +
59 virtual ~PreintegrationBase() {}
│ │ │ │ +
60
│ │ │ │ +
61 public:
│ │ │ │
64
│ │ │ │ -
66
│ │ │ │ -
70 void resetIntegration() override;
│ │ │ │ -
71
│ │ │ │ -
73
│ │ │ │ -
76 NavState deltaXij() const override { return deltaXij_; }
│ │ │ │ -
77 Rot3 deltaRij() const override { return deltaXij_.attitude(); }
│ │ │ │ -
78 Vector3 deltaPij() const override { return deltaXij_.position(); }
│ │ │ │ -
79 Vector3 deltaVij() const override { return deltaXij_.velocity(); }
│ │ │ │ -
80
│ │ │ │ -
81 Matrix3 delRdelBiasOmega() const { return delRdelBiasOmega_; }
│ │ │ │ -
82 Matrix3 delPdelBiasAcc() const { return delPdelBiasAcc_; }
│ │ │ │ -
83 Matrix3 delPdelBiasOmega() const { return delPdelBiasOmega_; }
│ │ │ │ -
84 Matrix3 delVdelBiasAcc() const { return delVdelBiasAcc_; }
│ │ │ │ -
85 Matrix3 delVdelBiasOmega() const { return delVdelBiasOmega_; }
│ │ │ │ -
86
│ │ │ │ -
89 bool equals(const ManifoldPreintegration& other, double tol) const;
│ │ │ │ -
91
│ │ │ │ +
70 PreintegrationBase(const boost::shared_ptr<Params>& p,
│ │ │ │ +
71 const imuBias::ConstantBias& biasHat = imuBias::ConstantBias());
│ │ │ │ +
72
│ │ │ │ +
74
│ │ │ │ +
78 virtual void resetIntegration() = 0;
│ │ │ │ +
79
│ │ │ │ +
83 void resetIntegrationAndSetBias(const Bias& biasHat);
│ │ │ │ +
84
│ │ │ │ +
│ │ │ │ +
86 bool matchesParamsWith(const PreintegrationBase& other) const {
│ │ │ │ +
87 return p_.get() == other.p_.get();
│ │ │ │ +
88 }
│ │ │ │ +
│ │ │ │ +
89
│ │ │ │ +
│ │ │ │ +
91 const boost::shared_ptr<Params>& params() const {
│ │ │ │ +
92 return p_;
│ │ │ │ +
93 }
│ │ │ │ +
│ │ │ │
94
│ │ │ │ -
99 void update(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt,
│ │ │ │ -
100 Matrix9* A, Matrix93* B, Matrix93* C) override;
│ │ │ │ +
│ │ │ │ +
96 Params& p() const {
│ │ │ │ +
97 return *p_;
│ │ │ │ +
98 }
│ │ │ │ +
│ │ │ │ +
99
│ │ │ │
101
│ │ │ │ -
105 Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
│ │ │ │ -
106 OptionalJacobian<9, 6> H = boost::none) const override;
│ │ │ │ -
107
│ │ │ │ -
│ │ │ │ -
109 virtual boost::shared_ptr<ManifoldPreintegration> clone() const {
│ │ │ │ -
110 return boost::shared_ptr<ManifoldPreintegration>();
│ │ │ │ -
111 }
│ │ │ │ -
│ │ │ │ -
112
│ │ │ │ -
114
│ │ │ │ -
115private:
│ │ │ │ -
117 friend class boost::serialization::access;
│ │ │ │ -
118 template<class ARCHIVE>
│ │ │ │ -
119 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
120 namespace bs = ::boost::serialization;
│ │ │ │ -
121 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
│ │ │ │ -
122 ar & BOOST_SERIALIZATION_NVP(deltaXij_);
│ │ │ │ -
123 ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
│ │ │ │ -
124 ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
│ │ │ │ -
125 ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);
│ │ │ │ -
126 ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_);
│ │ │ │ -
127 ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_);
│ │ │ │ -
128 }
│ │ │ │ -
129};
│ │ │ │ -
│ │ │ │ -
130
│ │ │ │ -
131}
│ │ │ │ -
NavState.h
Navigation state composing of attitude, position, and velocity.
│ │ │ │ - │ │ │ │ +
104 const imuBias::ConstantBias& biasHat() const { return biasHat_; }
│ │ │ │ +
105 double deltaTij() const { return deltaTij_; }
│ │ │ │ +
106
│ │ │ │ +
107 virtual Vector3 deltaPij() const = 0;
│ │ │ │ +
108 virtual Vector3 deltaVij() const = 0;
│ │ │ │ +
109 virtual Rot3 deltaRij() const = 0;
│ │ │ │ +
110 virtual NavState deltaXij() const = 0;
│ │ │ │ +
111
│ │ │ │ +
112 // Exposed for MATLAB
│ │ │ │ +
113 Vector6 biasHatVector() const { return biasHat_.vector(); }
│ │ │ │ +
115
│ │ │ │ +
118 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const PreintegrationBase& pim);
│ │ │ │ +
119 virtual void print(const std::string& s="") const;
│ │ │ │ +
121
│ │ │ │ +
124
│ │ │ │ +
130 std::pair<Vector3, Vector3> correctMeasurementsBySensorPose(
│ │ │ │ +
131 const Vector3& unbiasedAcc, const Vector3& unbiasedOmega,
│ │ │ │ +
132 OptionalJacobian<3, 3> correctedAcc_H_unbiasedAcc = boost::none,
│ │ │ │ +
133 OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none,
│ │ │ │ +
134 OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = boost::none) const;
│ │ │ │ +
135
│ │ │ │ +
141 virtual void update(const Vector3& measuredAcc, const Vector3& measuredOmega,
│ │ │ │ +
142 const double dt, Matrix9* A, Matrix93* B, Matrix93* C) = 0;
│ │ │ │ +
143
│ │ │ │ +
145 virtual void integrateMeasurement(const Vector3& measuredAcc,
│ │ │ │ +
146 const Vector3& measuredOmega, const double dt);
│ │ │ │ +
147
│ │ │ │ +
150 virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
│ │ │ │ +
151 OptionalJacobian<9, 6> H = boost::none) const = 0;
│ │ │ │ +
152
│ │ │ │ +
154 NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
│ │ │ │ +
155 OptionalJacobian<9, 9> H1 = boost::none,
│ │ │ │ +
156 OptionalJacobian<9, 6> H2 = boost::none) const;
│ │ │ │ +
157
│ │ │ │ +
159 Vector9 computeError(const NavState& state_i, const NavState& state_j,
│ │ │ │ +
160 const imuBias::ConstantBias& bias_i,
│ │ │ │ +
161 OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2,
│ │ │ │ +
162 OptionalJacobian<9, 6> H3) const;
│ │ │ │ +
163
│ │ │ │ +
168 Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
│ │ │ │ +
169 const Pose3& pose_j, const Vector3& vel_j,
│ │ │ │ +
170 const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H1 =
│ │ │ │ +
171 boost::none, OptionalJacobian<9, 3> H2 = boost::none,
│ │ │ │ +
172 OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
│ │ │ │ +
173 boost::none, OptionalJacobian<9, 6> H5 = boost::none) const;
│ │ │ │ +
174
│ │ │ │ +
175 private:
│ │ │ │ +
177 friend class boost::serialization::access;
│ │ │ │ +
178 template<class ARCHIVE>
│ │ │ │ +
179 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
180 ar & BOOST_SERIALIZATION_NVP(p_);
│ │ │ │ +
181 ar & BOOST_SERIALIZATION_NVP(biasHat_);
│ │ │ │ +
182 ar & BOOST_SERIALIZATION_NVP(deltaTij_);
│ │ │ │ +
183 }
│ │ │ │ +
184
│ │ │ │ +
185 public:
│ │ │ │ +
186 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
187};
│ │ │ │ +
│ │ │ │ +
188
│ │ │ │ +
189}
│ │ │ │ +
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ + │ │ │ │ +
NavState.h
Navigation state composing of attitude, position, and velocity.
│ │ │ │ + │ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ +
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │
gtsam::imuBias::ConstantBias
Definition ImuBias.h:30
│ │ │ │ -
gtsam::ManifoldPreintegration
IMU pre-integration on NavSatet manifold.
Definition ManifoldPreintegration.h:33
│ │ │ │ -
gtsam::ManifoldPreintegration::ManifoldPreintegration
ManifoldPreintegration()
Default constructor for serialization.
Definition ManifoldPreintegration.h:49
│ │ │ │ -
gtsam::ManifoldPreintegration::delVdelBiasAcc_
Matrix3 delVdelBiasAcc_
Jacobian of preintegrated velocity w.r.t. acceleration bias.
Definition ManifoldPreintegration.h:45
│ │ │ │ -
gtsam::ManifoldPreintegration::delRdelBiasOmega_
Matrix3 delRdelBiasOmega_
Jacobian of preintegrated rotation w.r.t. angular rate bias.
Definition ManifoldPreintegration.h:42
│ │ │ │ -
gtsam::ManifoldPreintegration::delPdelBiasAcc_
Matrix3 delPdelBiasAcc_
Jacobian of preintegrated position w.r.t. acceleration bias.
Definition ManifoldPreintegration.h:43
│ │ │ │ -
gtsam::ManifoldPreintegration::deltaXij_
NavState deltaXij_
Pre-integrated navigation state, from frame i to frame j Note: relative position does not take into a...
Definition ManifoldPreintegration.h:41
│ │ │ │ -
gtsam::ManifoldPreintegration::delPdelBiasOmega_
Matrix3 delPdelBiasOmega_
Jacobian of preintegrated position w.r.t. angular rate bias.
Definition ManifoldPreintegration.h:44
│ │ │ │ -
gtsam::ManifoldPreintegration::clone
virtual boost::shared_ptr< ManifoldPreintegration > clone() const
Dummy clone for MATLAB.
Definition ManifoldPreintegration.h:109
│ │ │ │ -
gtsam::ManifoldPreintegration::delVdelBiasOmega_
Matrix3 delVdelBiasOmega_
Jacobian of preintegrated velocity w.r.t. angular rate bias.
Definition ManifoldPreintegration.h:46
│ │ │ │ +
gtsam::imuBias::ConstantBias::vector
Vector6 vector() const
return the accelerometer and gyro biases in a single vector
Definition ImuBias.h:57
│ │ │ │
gtsam::NavState
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition NavState.h:34
│ │ │ │
gtsam::PreintegrationBase
PreintegrationBase is the base class for PreintegratedMeasurements (in ImuFactor) and CombinedPreinte...
Definition PreintegrationBase.h:41
│ │ │ │ +
gtsam::PreintegrationBase::deltaTij_
double deltaTij_
Time interval from i to j.
Definition PreintegrationBase.h:53
│ │ │ │ +
gtsam::PreintegrationBase::~PreintegrationBase
virtual ~PreintegrationBase()
Virtual destructor for serialization.
Definition PreintegrationBase.h:59
│ │ │ │ +
gtsam::PreintegrationBase::matchesParamsWith
bool matchesParamsWith(const PreintegrationBase &other) const
check parameters equality: checks whether shared pointer points to same Params object.
Definition PreintegrationBase.h:86
│ │ │ │ +
gtsam::PreintegrationBase::biasCorrectedDelta
virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 6 > H=boost::none) const =0
Given the estimate of the bias, return a NavState tangent vector summarizing the preintegrated IMU me...
│ │ │ │ +
gtsam::PreintegrationBase::p
Params & p() const
const reference to params
Definition PreintegrationBase.h:96
│ │ │ │ +
gtsam::PreintegrationBase::biasHat_
Bias biasHat_
Acceleration and gyro bias used for preintegration.
Definition PreintegrationBase.h:50
│ │ │ │ +
gtsam::PreintegrationBase::PreintegrationBase
PreintegrationBase()
Default constructor for serialization.
Definition PreintegrationBase.h:56
│ │ │ │ +
gtsam::PreintegrationBase::params
const boost::shared_ptr< Params > & params() const
shared pointer to params
Definition PreintegrationBase.h:91
│ │ │ │ +
gtsam::PreintegrationBase::update
virtual void update(const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt, Matrix9 *A, Matrix93 *B, Matrix93 *C)=0
Update preintegrated measurements and get derivatives It takes measured quantities in the j frame Mod...
│ │ │ │ +
gtsam::PreintegrationParams
Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
Definition PreintegrationParams.h:26
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ManifoldPreintegration.h │ │ │ │ │ +PreintegrationBase.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ @@ -16,133 +16,204 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 22#pragma once │ │ │ │ │ 23 │ │ │ │ │ -24#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_N_a_v_S_t_a_t_e_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_._h> │ │ │ │ │ -26 │ │ │ │ │ -27namespace _g_t_s_a_m { │ │ │ │ │ +24#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_N_a_v_S_t_a_t_e_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_I_m_u_B_i_a_s_._h> │ │ │ │ │ +27#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_N_o_i_s_e_M_o_d_e_l_._h> │ │ │ │ │ 28 │ │ │ │ │ -_3_3class GTSAM_EXPORT _M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n : public _P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e { │ │ │ │ │ -34 protected: │ │ │ │ │ -35 │ │ │ │ │ -_4_1 _N_a_v_S_t_a_t_e _d_e_l_t_a_X_i_j__; │ │ │ │ │ -_4_2 Matrix3 _d_e_l_R_d_e_l_B_i_a_s_O_m_e_g_a__; │ │ │ │ │ -_4_3 Matrix3 _d_e_l_P_d_e_l_B_i_a_s_A_c_c__; │ │ │ │ │ -_4_4 Matrix3 _d_e_l_P_d_e_l_B_i_a_s_O_m_e_g_a__; │ │ │ │ │ -_4_5 Matrix3 _d_e_l_V_d_e_l_B_i_a_s_A_c_c__; │ │ │ │ │ -_4_6 Matrix3 _d_e_l_V_d_e_l_B_i_a_s_O_m_e_g_a__; │ │ │ │ │ -47 │ │ │ │ │ -_4_9 _M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n() { │ │ │ │ │ -50 resetIntegration(); │ │ │ │ │ -51 } │ │ │ │ │ -52 │ │ │ │ │ -53public: │ │ │ │ │ -56 │ │ │ │ │ -62 _M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n(const boost::shared_ptr& p, │ │ │ │ │ -63 const _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s& biasHat = _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s()); │ │ │ │ │ +29#include │ │ │ │ │ +30#include │ │ │ │ │ +31#include │ │ │ │ │ +32 │ │ │ │ │ +33namespace _g_t_s_a_m { │ │ │ │ │ +34 │ │ │ │ │ +_4_1class GTSAM_EXPORT _P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e { │ │ │ │ │ +42 public: │ │ │ │ │ +43 typedef _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s _B_i_a_s; │ │ │ │ │ +44 typedef _P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s _P_a_r_a_m_s; │ │ │ │ │ +45 │ │ │ │ │ +46 protected: │ │ │ │ │ +47 boost::shared_ptr p_; │ │ │ │ │ +48 │ │ │ │ │ +_5_0 _B_i_a_s _b_i_a_s_H_a_t__; │ │ │ │ │ +51 │ │ │ │ │ +_5_3 double _d_e_l_t_a_T_i_j__; │ │ │ │ │ +54 │ │ │ │ │ +_5_6 _P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e() {} │ │ │ │ │ +57 │ │ │ │ │ +_5_9 virtual _~_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e() {} │ │ │ │ │ +60 │ │ │ │ │ +61 public: │ │ │ │ │ 64 │ │ │ │ │ -66 │ │ │ │ │ -70 void resetIntegration() override; │ │ │ │ │ -71 │ │ │ │ │ -73 │ │ │ │ │ -76 _N_a_v_S_t_a_t_e deltaXij() const override { return deltaXij_; } │ │ │ │ │ -77 Rot3 deltaRij() const override { return deltaXij_.attitude(); } │ │ │ │ │ -78 Vector3 deltaPij() const override { return deltaXij_.position(); } │ │ │ │ │ -79 Vector3 deltaVij() const override { return deltaXij_.velocity(); } │ │ │ │ │ -80 │ │ │ │ │ -81 Matrix3 delRdelBiasOmega() const { return delRdelBiasOmega_; } │ │ │ │ │ -82 Matrix3 delPdelBiasAcc() const { return delPdelBiasAcc_; } │ │ │ │ │ -83 Matrix3 delPdelBiasOmega() const { return delPdelBiasOmega_; } │ │ │ │ │ -84 Matrix3 delVdelBiasAcc() const { return delVdelBiasAcc_; } │ │ │ │ │ -85 Matrix3 delVdelBiasOmega() const { return delVdelBiasOmega_; } │ │ │ │ │ -86 │ │ │ │ │ -89 bool equals(const ManifoldPreintegration& other, double tol) const; │ │ │ │ │ -91 │ │ │ │ │ +70 _P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e(const boost::shared_ptr& p, │ │ │ │ │ +71 const _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s& biasHat = _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s()); │ │ │ │ │ +72 │ │ │ │ │ +74 │ │ │ │ │ +78 virtual void resetIntegration() = 0; │ │ │ │ │ +79 │ │ │ │ │ +83 void resetIntegrationAndSetBias(const Bias& biasHat); │ │ │ │ │ +84 │ │ │ │ │ +_8_6 bool _m_a_t_c_h_e_s_P_a_r_a_m_s_W_i_t_h(const _P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e& other) const { │ │ │ │ │ +87 return p_.get() == other.p_.get(); │ │ │ │ │ +88 } │ │ │ │ │ +89 │ │ │ │ │ +_9_1 const boost::shared_ptr& _p_a_r_a_m_s() const { │ │ │ │ │ +92 return p_; │ │ │ │ │ +93 } │ │ │ │ │ 94 │ │ │ │ │ -99 void update(const Vector3& measuredAcc, const Vector3& measuredOmega, const │ │ │ │ │ -double dt, │ │ │ │ │ -100 Matrix9* A, Matrix93* B, Matrix93* C) override; │ │ │ │ │ +_9_6 _P_a_r_a_m_s& _p() const { │ │ │ │ │ +97 return *p_; │ │ │ │ │ +98 } │ │ │ │ │ +99 │ │ │ │ │ 101 │ │ │ │ │ -105 Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, │ │ │ │ │ -106 OptionalJacobian<9, 6> H = boost::none) const override; │ │ │ │ │ -107 │ │ │ │ │ -_1_0_9 virtual boost::shared_ptr _c_l_o_n_e() const { │ │ │ │ │ -110 return boost::shared_ptr(); │ │ │ │ │ -111 } │ │ │ │ │ -112 │ │ │ │ │ -114 │ │ │ │ │ -115private: │ │ │ │ │ -_1_1_7 friend class boost::serialization::access; │ │ │ │ │ -118 template │ │ │ │ │ -119 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -120 namespace bs = ::boost::serialization; │ │ │ │ │ -121 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e); │ │ │ │ │ -122 ar & BOOST_SERIALIZATION_NVP(deltaXij_); │ │ │ │ │ -123 ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); │ │ │ │ │ -124 ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); │ │ │ │ │ -125 ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); │ │ │ │ │ -126 ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); │ │ │ │ │ -127 ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); │ │ │ │ │ -128 } │ │ │ │ │ -129}; │ │ │ │ │ -130 │ │ │ │ │ -131} │ │ │ │ │ +104 const _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s& biasHat() const { return biasHat_; } │ │ │ │ │ +105 double deltaTij() const { return deltaTij_; } │ │ │ │ │ +106 │ │ │ │ │ +107 virtual Vector3 deltaPij() const = 0; │ │ │ │ │ +108 virtual Vector3 deltaVij() const = 0; │ │ │ │ │ +109 virtual Rot3 deltaRij() const = 0; │ │ │ │ │ +110 virtual NavState deltaXij() const = 0; │ │ │ │ │ +111 │ │ │ │ │ +112 // Exposed for MATLAB │ │ │ │ │ +113 Vector6 biasHatVector() const { return biasHat_._v_e_c_t_o_r(); } │ │ │ │ │ +115 │ │ │ │ │ +118 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const │ │ │ │ │ +PreintegrationBase& pim); │ │ │ │ │ +119 virtual void _p_r_i_n_t(const std::string& s="") const; │ │ │ │ │ +121 │ │ │ │ │ +124 │ │ │ │ │ +130 std::pair correctMeasurementsBySensorPose( │ │ │ │ │ +131 const Vector3& unbiasedAcc, const Vector3& unbiasedOmega, │ │ │ │ │ +132 OptionalJacobian<3, 3> correctedAcc_H_unbiasedAcc = boost::none, │ │ │ │ │ +133 OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none, │ │ │ │ │ +134 OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = boost::none) const; │ │ │ │ │ +135 │ │ │ │ │ +_1_4_1 virtual void _u_p_d_a_t_e(const Vector3& measuredAcc, const Vector3& │ │ │ │ │ +measuredOmega, │ │ │ │ │ +142 const double dt, Matrix9* A, Matrix93* B, Matrix93* C) = 0; │ │ │ │ │ +143 │ │ │ │ │ +145 virtual void integrateMeasurement(const Vector3& measuredAcc, │ │ │ │ │ +146 const Vector3& measuredOmega, const double dt); │ │ │ │ │ +147 │ │ │ │ │ +_1_5_0 virtual Vector9 _b_i_a_s_C_o_r_r_e_c_t_e_d_D_e_l_t_a(const _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s& bias_i, │ │ │ │ │ +151 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _6_> H = boost::none) const = 0; │ │ │ │ │ +152 │ │ │ │ │ +154 _N_a_v_S_t_a_t_e predict(const _N_a_v_S_t_a_t_e& state_i, const _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s& │ │ │ │ │ +bias_i, │ │ │ │ │ +155 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _9_> H1 = boost::none, │ │ │ │ │ +156 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _6_> H2 = boost::none) const; │ │ │ │ │ +157 │ │ │ │ │ +159 Vector9 computeError(const _N_a_v_S_t_a_t_e& state_i, const _N_a_v_S_t_a_t_e& state_j, │ │ │ │ │ +160 const _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s& bias_i, │ │ │ │ │ +161 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _9_> H1, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _9_> H2, │ │ │ │ │ +162 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _6_> H3) const; │ │ │ │ │ +163 │ │ │ │ │ +168 Vector9 computeErrorAndJacobians(const _P_o_s_e_3& pose_i, const Vector3& vel_i, │ │ │ │ │ +169 const _P_o_s_e_3& pose_j, const Vector3& vel_j, │ │ │ │ │ +170 const _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s& bias_i, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _6_> H1 = │ │ │ │ │ +171 boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _3_> H2 = boost::none, │ │ │ │ │ +172 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _6_> H3 = boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _3_> H4 = │ │ │ │ │ +173 boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _6_> H5 = boost::none) const; │ │ │ │ │ +174 │ │ │ │ │ +175 private: │ │ │ │ │ +_1_7_7 friend class boost::serialization::access; │ │ │ │ │ +178 template │ │ │ │ │ +179 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +180 ar & BOOST_SERIALIZATION_NVP(p_); │ │ │ │ │ +181 ar & BOOST_SERIALIZATION_NVP(biasHat_); │ │ │ │ │ +182 ar & BOOST_SERIALIZATION_NVP(deltaTij_); │ │ │ │ │ +183 } │ │ │ │ │ +184 │ │ │ │ │ +185 public: │ │ │ │ │ +186 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +187}; │ │ │ │ │ +188 │ │ │ │ │ +189} │ │ │ │ │ +_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ +This marks a GTSAM object to require alignment. │ │ │ │ │ +DDeeffiinniittiioonn types.h:308 │ │ │ │ │ +_N_o_i_s_e_M_o_d_e_l_._h │ │ │ │ │ _N_a_v_S_t_a_t_e_._h │ │ │ │ │ Navigation state composing of attitude, position, and velocity. │ │ │ │ │ -_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_._h │ │ │ │ │ +_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s_._h │ │ │ │ │ +_I_m_u_B_i_a_s_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ +either a fixed size o... │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ +A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ _g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s │ │ │ │ │ DDeeffiinniittiioonn ImuBias.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n │ │ │ │ │ -IMU pre-integration on NavSatet manifold. │ │ │ │ │ -DDeeffiinniittiioonn ManifoldPreintegration.h:33 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_:_:_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n │ │ │ │ │ -ManifoldPreintegration() │ │ │ │ │ -Default constructor for serialization. │ │ │ │ │ -DDeeffiinniittiioonn ManifoldPreintegration.h:49 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_:_:_d_e_l_V_d_e_l_B_i_a_s_A_c_c__ │ │ │ │ │ -Matrix3 delVdelBiasAcc_ │ │ │ │ │ -Jacobian of preintegrated velocity w.r.t. acceleration bias. │ │ │ │ │ -DDeeffiinniittiioonn ManifoldPreintegration.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_:_:_d_e_l_R_d_e_l_B_i_a_s_O_m_e_g_a__ │ │ │ │ │ -Matrix3 delRdelBiasOmega_ │ │ │ │ │ -Jacobian of preintegrated rotation w.r.t. angular rate bias. │ │ │ │ │ -DDeeffiinniittiioonn ManifoldPreintegration.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_:_:_d_e_l_P_d_e_l_B_i_a_s_A_c_c__ │ │ │ │ │ -Matrix3 delPdelBiasAcc_ │ │ │ │ │ -Jacobian of preintegrated position w.r.t. acceleration bias. │ │ │ │ │ -DDeeffiinniittiioonn ManifoldPreintegration.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_:_:_d_e_l_t_a_X_i_j__ │ │ │ │ │ -NavState deltaXij_ │ │ │ │ │ -Pre-integrated navigation state, from frame i to frame j Note: relative │ │ │ │ │ -position does not take into a... │ │ │ │ │ -DDeeffiinniittiioonn ManifoldPreintegration.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_:_:_d_e_l_P_d_e_l_B_i_a_s_O_m_e_g_a__ │ │ │ │ │ -Matrix3 delPdelBiasOmega_ │ │ │ │ │ -Jacobian of preintegrated position w.r.t. angular rate bias. │ │ │ │ │ -DDeeffiinniittiioonn ManifoldPreintegration.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_:_:_c_l_o_n_e │ │ │ │ │ -virtual boost::shared_ptr< ManifoldPreintegration > clone() const │ │ │ │ │ -Dummy clone for MATLAB. │ │ │ │ │ -DDeeffiinniittiioonn ManifoldPreintegration.h:109 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_:_:_d_e_l_V_d_e_l_B_i_a_s_O_m_e_g_a__ │ │ │ │ │ -Matrix3 delVdelBiasOmega_ │ │ │ │ │ -Jacobian of preintegrated velocity w.r.t. angular rate bias. │ │ │ │ │ -DDeeffiinniittiioonn ManifoldPreintegration.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_:_:_v_e_c_t_o_r │ │ │ │ │ +Vector6 vector() const │ │ │ │ │ +return the accelerometer and gyro biases in a single vector │ │ │ │ │ +DDeeffiinniittiioonn ImuBias.h:57 │ │ │ │ │ _g_t_s_a_m_:_:_N_a_v_S_t_a_t_e │ │ │ │ │ Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does │ │ │ │ │ not make sense to make... │ │ │ │ │ DDeeffiinniittiioonn NavState.h:34 │ │ │ │ │ _g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e │ │ │ │ │ PreintegrationBase is the base class for PreintegratedMeasurements (in │ │ │ │ │ ImuFactor) and CombinedPreinte... │ │ │ │ │ DDeeffiinniittiioonn PreintegrationBase.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_:_:_d_e_l_t_a_T_i_j__ │ │ │ │ │ +double deltaTij_ │ │ │ │ │ +Time interval from i to j. │ │ │ │ │ +DDeeffiinniittiioonn PreintegrationBase.h:53 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_:_:_~_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e │ │ │ │ │ +virtual ~PreintegrationBase() │ │ │ │ │ +Virtual destructor for serialization. │ │ │ │ │ +DDeeffiinniittiioonn PreintegrationBase.h:59 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_:_:_m_a_t_c_h_e_s_P_a_r_a_m_s_W_i_t_h │ │ │ │ │ +bool matchesParamsWith(const PreintegrationBase &other) const │ │ │ │ │ +check parameters equality: checks whether shared pointer points to same Params │ │ │ │ │ +object. │ │ │ │ │ +DDeeffiinniittiioonn PreintegrationBase.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_:_:_b_i_a_s_C_o_r_r_e_c_t_e_d_D_e_l_t_a │ │ │ │ │ +virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias &bias_i, │ │ │ │ │ +OptionalJacobian< 9, 6 > H=boost::none) const =0 │ │ │ │ │ +Given the estimate of the bias, return a NavState tangent vector summarizing │ │ │ │ │ +the preintegrated IMU me... │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_:_:_p │ │ │ │ │ +Params & p() const │ │ │ │ │ +const reference to params │ │ │ │ │ +DDeeffiinniittiioonn PreintegrationBase.h:96 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_:_:_b_i_a_s_H_a_t__ │ │ │ │ │ +Bias biasHat_ │ │ │ │ │ +Acceleration and gyro bias used for preintegration. │ │ │ │ │ +DDeeffiinniittiioonn PreintegrationBase.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e │ │ │ │ │ +PreintegrationBase() │ │ │ │ │ +Default constructor for serialization. │ │ │ │ │ +DDeeffiinniittiioonn PreintegrationBase.h:56 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_:_:_p_a_r_a_m_s │ │ │ │ │ +const boost::shared_ptr< Params > & params() const │ │ │ │ │ +shared pointer to params │ │ │ │ │ +DDeeffiinniittiioonn PreintegrationBase.h:91 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_:_:_u_p_d_a_t_e │ │ │ │ │ +virtual void update(const Vector3 &measuredAcc, const Vector3 &measuredOmega, │ │ │ │ │ +const double dt, Matrix9 *A, Matrix93 *B, Matrix93 *C)=0 │ │ │ │ │ +Update preintegrated measurements and get derivatives It takes measured │ │ │ │ │ +quantities in the j frame Mod... │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ +Parameters for pre-integration: Usage: Create just a single Params and pass a │ │ │ │ │ +shared pointer to the c... │ │ │ │ │ +DDeeffiinniittiioonn PreintegrationParams.h:26 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _M_a_n_i_f_o_l_d_P_r_e_i_n_t_e_g_r_a_t_i_o_n_._h │ │ │ │ │ + * _P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00995.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/GPSFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/MagFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,46 +96,52 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
GPSFactor.h File Reference
│ │ │ │ +
MagFactor.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Header file for GPS factor. │ │ │ │ +

Factors involving magnetometers. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::GPSFactor
 Prior on position in a Cartesian frame. More...
class  gtsam::MagFactor
 Factor to estimate rotation given magnetometer reading This version uses model measured bM = scale * bRn * direction + bias and assumes scale, direction, and the bias are given. More...
 
class  gtsam::GPSFactor2
 Version of GPSFactor for NavState. More...
class  gtsam::MagFactor1
 Factor to estimate rotation given magnetometer reading This version uses model measured bM = scale * bRn * direction + bias and assumes scale, direction, and the bias are given. More...
 
class  gtsam::MagFactor2
 Factor to calibrate local Earth magnetic field as well as magnetometer bias This version uses model measured bM = bRn * nM + bias and optimizes for both nM and the bias, where nM is in units defined by magnetometer. More...
 
class  gtsam::MagFactor3
 Factor to calibrate local Earth magnetic field as well as magnetometer bias This version uses model measured bM = scale * bRn * direction + bias and optimizes for both scale, direction, and the bias. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Header file for GPS factor.

│ │ │ │ +

Factors involving magnetometers.

│ │ │ │
Author
Frank Dellaert
│ │ │ │ -
Date
January 22, 2014
│ │ │ │ +
Date
January 29, 2014
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,30 +1,45 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -GPSFactor.h File Reference │ │ │ │ │ -Header file for GPS factor. _M_o_r_e_._._. │ │ │ │ │ +MagFactor.h File Reference │ │ │ │ │ +Factors involving magnetometers. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_G_P_S_F_a_c_t_o_r │ │ │ │ │ -  Prior on position in a Cartesian frame. _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r │ │ │ │ │ + _F_a_c_t_o_r to estimate rotation given magnetometer reading This version │ │ │ │ │ +  uses model measured bM = scale * bRn * direction + bias and assumes │ │ │ │ │ + scale, direction, and the bias are given. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -class   _g_t_s_a_m_:_:_G_P_S_F_a_c_t_o_r_2 │ │ │ │ │ -  Version of _G_P_S_F_a_c_t_o_r for _N_a_v_S_t_a_t_e. _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_1 │ │ │ │ │ + _F_a_c_t_o_r to estimate rotation given magnetometer reading This version │ │ │ │ │ +  uses model measured bM = scale * bRn * direction + bias and assumes │ │ │ │ │ + scale, direction, and the bias are given. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_2 │ │ │ │ │ + _F_a_c_t_o_r to calibrate local Earth magnetic field as well as magnetometer │ │ │ │ │ +  bias This version uses model measured bM = bRn * nM + bias and │ │ │ │ │ + optimizes for both nM and the bias, where nM is in units defined by │ │ │ │ │ + magnetometer. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_3 │ │ │ │ │ + _F_a_c_t_o_r to calibrate local Earth magnetic field as well as magnetometer │ │ │ │ │ +  bias This version uses model measured bM = scale * bRn * direction + │ │ │ │ │ + bias and optimizes for both scale, direction, and the bias. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Header file for GPS factor. │ │ │ │ │ +Factors involving magnetometers. │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - January 22, 2014 │ │ │ │ │ + January 29, 2014 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _G_P_S_F_a_c_t_o_r_._h │ │ │ │ │ + * _M_a_g_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00995_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/GPSFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/MagFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,186 +98,241 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
GPSFactor.h
│ │ │ │ +
MagFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
21#include <gtsam/navigation/NavState.h>
│ │ │ │ -
22#include <gtsam/geometry/Pose3.h>
│ │ │ │ -
23
│ │ │ │ -
24namespace gtsam {
│ │ │ │ -
25
│ │ │ │ -
│ │ │ │ -
35class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
│ │ │ │ -
36
│ │ │ │ -
37private:
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
22#include <gtsam/geometry/Rot2.h>
│ │ │ │ +
23#include <gtsam/geometry/Rot3.h>
│ │ │ │ +
24
│ │ │ │ +
25namespace gtsam {
│ │ │ │ +
26
│ │ │ │ +
│ │ │ │ +
33class MagFactor: public NoiseModelFactorN<Rot2> {
│ │ │ │ +
34
│ │ │ │ +
35 const Point3 measured_;
│ │ │ │ +
36 const Point3 nM_;
│ │ │ │ +
37 const Point3 bias_;
│ │ │ │
38
│ │ │ │ -
39 typedef NoiseModelFactorN<Pose3> Base;
│ │ │ │ +
39public:
│ │ │ │
40
│ │ │ │ -
41 Point3 nT_;
│ │ │ │ -
42
│ │ │ │ -
43public:
│ │ │ │ -
44
│ │ │ │ -
46 typedef boost::shared_ptr<GPSFactor> shared_ptr;
│ │ │ │ -
47
│ │ │ │ -
49 typedef GPSFactor This;
│ │ │ │ -
50
│ │ │ │ -
52 GPSFactor(): nT_(0, 0, 0) {}
│ │ │ │ -
53
│ │ │ │ -
54 ~GPSFactor() override {}
│ │ │ │ -
55
│ │ │ │ -
│ │ │ │ -
63 GPSFactor(Key key, const Point3& gpsIn, const SharedNoiseModel& model) :
│ │ │ │ -
64 Base(model, key), nT_(gpsIn) {
│ │ │ │ -
65 }
│ │ │ │ -
│ │ │ │ -
66
│ │ │ │ -
│ │ │ │ -
68 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
69 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ -
70 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
│ │ │ │ -
71 }
│ │ │ │ -
│ │ │ │ -
72
│ │ │ │ -
74 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
│ │ │ │ -
75 DefaultKeyFormatter) const override;
│ │ │ │ -
76
│ │ │ │ -
78 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
│ │ │ │ -
79
│ │ │ │ -
81 Vector evaluateError(const Pose3& p,
│ │ │ │ -
82 boost::optional<Matrix&> H = boost::none) const override;
│ │ │ │ -
83
│ │ │ │ -
84 inline const Point3 & measurementIn() const {
│ │ │ │ -
85 return nT_;
│ │ │ │ -
86 }
│ │ │ │ -
87
│ │ │ │ -
93 static std::pair<Pose3, Vector3> EstimateState(double t1, const Point3& NED1,
│ │ │ │ -
94 double t2, const Point3& NED2, double timestamp);
│ │ │ │ +
│ │ │ │ +
50 MagFactor(Key key, const Point3& measured, double scale,
│ │ │ │ +
51 const Unit3& direction, const Point3& bias,
│ │ │ │ +
52 const SharedNoiseModel& model) :
│ │ │ │ +
53 NoiseModelFactorN<Rot2>(model, key), //
│ │ │ │ +
54 measured_(measured), nM_(scale * direction), bias_(bias) {
│ │ │ │ +
55 }
│ │ │ │ +
│ │ │ │ +
56
│ │ │ │ +
│ │ │ │ +
58 NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
59 return boost::static_pointer_cast<NonlinearFactor>(
│ │ │ │ +
60 NonlinearFactor::shared_ptr(new MagFactor(*this)));
│ │ │ │ +
61 }
│ │ │ │ +
│ │ │ │ +
62
│ │ │ │ +
63 static Point3 unrotate(const Rot2& R, const Point3& p,
│ │ │ │ +
64 boost::optional<Matrix&> HR = boost::none) {
│ │ │ │ +
65 Point3 q = Rot3::Yaw(R.theta()).unrotate(p, HR, boost::none);
│ │ │ │ +
66 if (HR) {
│ │ │ │ +
67 // assign to temporary first to avoid error in Win-Debug mode
│ │ │ │ +
68 Matrix H = HR->col(2);
│ │ │ │ +
69 *HR = H;
│ │ │ │ +
70 }
│ │ │ │ +
71 return q;
│ │ │ │ +
72 }
│ │ │ │ +
73
│ │ │ │ +
│ │ │ │ +
77 Vector evaluateError(const Rot2& nRb,
│ │ │ │ +
78 boost::optional<Matrix&> H = boost::none) const override {
│ │ │ │ +
79 // measured bM = nRb� * nM + b
│ │ │ │ +
80 Point3 hx = unrotate(nRb, nM_, H) + bias_;
│ │ │ │ +
81 return (hx - measured_);
│ │ │ │ +
82 }
│ │ │ │ +
│ │ │ │ +
83};
│ │ │ │ +
│ │ │ │ +
84
│ │ │ │ +
│ │ │ │ +
90class MagFactor1: public NoiseModelFactorN<Rot3> {
│ │ │ │ +
91
│ │ │ │ +
92 const Point3 measured_;
│ │ │ │ +
93 const Point3 nM_;
│ │ │ │ +
94 const Point3 bias_;
│ │ │ │
95
│ │ │ │ -
96private:
│ │ │ │ +
96public:
│ │ │ │
97
│ │ │ │ -
99 friend class boost::serialization::access;
│ │ │ │ -
100 template<class ARCHIVE>
│ │ │ │ -
101 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
102 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ -
103 ar
│ │ │ │ -
104 & boost::serialization::make_nvp("NoiseModelFactor1",
│ │ │ │ -
105 boost::serialization::base_object<Base>(*this));
│ │ │ │ -
106 ar & BOOST_SERIALIZATION_NVP(nT_);
│ │ │ │ -
107 }
│ │ │ │ -
108};
│ │ │ │ -
│ │ │ │ -
109
│ │ │ │ -
│ │ │ │ -
114class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
│ │ │ │ -
115
│ │ │ │ -
116private:
│ │ │ │ -
117
│ │ │ │ -
118 typedef NoiseModelFactorN<NavState> Base;
│ │ │ │ -
119
│ │ │ │ -
120 Point3 nT_;
│ │ │ │ -
121
│ │ │ │ -
122public:
│ │ │ │ -
123
│ │ │ │ -
125 typedef boost::shared_ptr<GPSFactor2> shared_ptr;
│ │ │ │ -
126
│ │ │ │ -
128 typedef GPSFactor2 This;
│ │ │ │ +
│ │ │ │ +
99 MagFactor1(Key key, const Point3& measured, double scale,
│ │ │ │ +
100 const Unit3& direction, const Point3& bias,
│ │ │ │ +
101 const SharedNoiseModel& model) :
│ │ │ │ +
102 NoiseModelFactorN<Rot3>(model, key), //
│ │ │ │ +
103 measured_(measured), nM_(scale * direction), bias_(bias) {
│ │ │ │ +
104 }
│ │ │ │ +
│ │ │ │ +
105
│ │ │ │ +
│ │ │ │ +
107 NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
108 return boost::static_pointer_cast<NonlinearFactor>(
│ │ │ │ +
109 NonlinearFactor::shared_ptr(new MagFactor1(*this)));
│ │ │ │ +
110 }
│ │ │ │ +
│ │ │ │ +
111
│ │ │ │ +
│ │ │ │ +
115 Vector evaluateError(const Rot3& nRb,
│ │ │ │ +
116 boost::optional<Matrix&> H = boost::none) const override {
│ │ │ │ +
117 // measured bM = nRb� * nM + b
│ │ │ │ +
118 Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_;
│ │ │ │ +
119 return (hx - measured_);
│ │ │ │ +
120 }
│ │ │ │ +
│ │ │ │ +
121};
│ │ │ │ +
│ │ │ │ +
122
│ │ │ │ +
│ │ │ │ +
128class MagFactor2: public NoiseModelFactorN<Point3, Point3> {
│ │ │ │
129
│ │ │ │ -
131 GPSFactor2():nT_(0, 0, 0) {}
│ │ │ │ +
130 const Point3 measured_;
│ │ │ │ +
131 const Rot3 bRn_;
│ │ │ │
132
│ │ │ │ -
133 ~GPSFactor2() override {}
│ │ │ │ +
133public:
│ │ │ │
134
│ │ │ │
│ │ │ │ -
136 GPSFactor2(Key key, const Point3& gpsIn, const SharedNoiseModel& model) :
│ │ │ │ -
137 Base(model, key), nT_(gpsIn) {
│ │ │ │ -
138 }
│ │ │ │ -
│ │ │ │ -
139
│ │ │ │ -
│ │ │ │ -
141 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
142 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ -
143 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
│ │ │ │ -
144 }
│ │ │ │ -
│ │ │ │ -
145
│ │ │ │ -
147 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
│ │ │ │ -
148 DefaultKeyFormatter) const override;
│ │ │ │ -
149
│ │ │ │ -
151 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
│ │ │ │ -
152
│ │ │ │ -
154 Vector evaluateError(const NavState& p,
│ │ │ │ -
155 boost::optional<Matrix&> H = boost::none) const override;
│ │ │ │ -
156
│ │ │ │ -
157 inline const Point3 & measurementIn() const {
│ │ │ │ -
158 return nT_;
│ │ │ │ -
159 }
│ │ │ │ -
160
│ │ │ │ -
161private:
│ │ │ │ -
162
│ │ │ │ -
164 friend class boost::serialization::access;
│ │ │ │ -
165 template<class ARCHIVE>
│ │ │ │ -
166 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
167 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ -
168 ar
│ │ │ │ -
169 & boost::serialization::make_nvp("NoiseModelFactor1",
│ │ │ │ -
170 boost::serialization::base_object<Base>(*this));
│ │ │ │ -
171 ar & BOOST_SERIALIZATION_NVP(nT_);
│ │ │ │ -
172 }
│ │ │ │ -
173};
│ │ │ │ -
│ │ │ │ -
174
│ │ │ │ -
175}
│ │ │ │ -
Pose3.h
3D Pose
│ │ │ │ -
NavState.h
Navigation state composing of attitude, position, and velocity.
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
136 MagFactor2(Key key1, Key key2, const Point3& measured, const Rot3& nRb,
│ │ │ │ +
137 const SharedNoiseModel& model) :
│ │ │ │ +
138 NoiseModelFactorN<Point3, Point3>(model, key1, key2), //
│ │ │ │ +
139 measured_(measured), bRn_(nRb.inverse()) {
│ │ │ │ +
140 }
│ │ │ │ +
│ │ │ │ +
141
│ │ │ │ +
│ │ │ │ +
143 NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
144 return boost::static_pointer_cast<NonlinearFactor>(
│ │ │ │ +
145 NonlinearFactor::shared_ptr(new MagFactor2(*this)));
│ │ │ │ +
146 }
│ │ │ │ +
│ │ │ │ +
147
│ │ │ │ +
│ │ │ │ +
153 Vector evaluateError(const Point3& nM, const Point3& bias,
│ │ │ │ +
154 boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
│ │ │ │ +
155 boost::none) const override {
│ │ │ │ +
156 // measured bM = nRb� * nM + b, where b is unknown bias
│ │ │ │ +
157 Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias;
│ │ │ │ +
158 if (H2)
│ │ │ │ +
159 *H2 = I_3x3;
│ │ │ │ +
160 return (hx - measured_);
│ │ │ │ +
161 }
│ │ │ │ +
│ │ │ │ +
162};
│ │ │ │ +
│ │ │ │ +
163
│ │ │ │ +
│ │ │ │ +
169class MagFactor3: public NoiseModelFactorN<double, Unit3, Point3> {
│ │ │ │ +
170
│ │ │ │ +
171 const Point3 measured_;
│ │ │ │ +
172 const Rot3 bRn_;
│ │ │ │ +
173
│ │ │ │ +
174public:
│ │ │ │ +
175
│ │ │ │ +
│ │ │ │ +
177 MagFactor3(Key key1, Key key2, Key key3, const Point3& measured,
│ │ │ │ +
178 const Rot3& nRb, const SharedNoiseModel& model) :
│ │ │ │ +
179 NoiseModelFactorN<double, Unit3, Point3>(model, key1, key2, key3), //
│ │ │ │ +
180 measured_(measured), bRn_(nRb.inverse()) {
│ │ │ │ +
181 }
│ │ │ │ +
│ │ │ │ +
182
│ │ │ │ +
│ │ │ │ +
184 NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
185 return boost::static_pointer_cast<NonlinearFactor>(
│ │ │ │ +
186 NonlinearFactor::shared_ptr(new MagFactor3(*this)));
│ │ │ │ +
187 }
│ │ │ │ +
│ │ │ │ +
188
│ │ │ │ +
│ │ │ │ +
194 Vector evaluateError(const double& scale, const Unit3& direction,
│ │ │ │ +
195 const Point3& bias, boost::optional<Matrix&> H1 = boost::none,
│ │ │ │ +
196 boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
│ │ │ │ +
197 boost::none) const override {
│ │ │ │ +
198 // measured bM = nRb� * nM + b, where b is unknown bias
│ │ │ │ +
199 Unit3 rotated = bRn_.rotate(direction, boost::none, H2);
│ │ │ │ +
200 Point3 hx = scale * rotated.point3() + bias;
│ │ │ │ +
201 if (H1)
│ │ │ │ +
202 *H1 = rotated.point3();
│ │ │ │ +
203 if (H2) // H2 is 2*2, but we need 3*2
│ │ │ │ +
204 {
│ │ │ │ +
205 Matrix H;
│ │ │ │ +
206 rotated.point3(H);
│ │ │ │ +
207 *H2 = scale * H * (*H2);
│ │ │ │ +
208 }
│ │ │ │ +
209 if (H3)
│ │ │ │ +
210 *H3 = I_3x3;
│ │ │ │ +
211 return (hx - measured_);
│ │ │ │ +
212 }
│ │ │ │ +
│ │ │ │ +
213};
│ │ │ │ +
│ │ │ │ +
214
│ │ │ │ +
215}
│ │ │ │ +
216
│ │ │ │ +
Rot3.h
3D rotation represented as a rotation matrix or quaternion
│ │ │ │ +
Rot2.h
2D rotation
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ -
gtsam::GPSFactor
Prior on position in a Cartesian frame.
Definition GPSFactor.h:35
│ │ │ │ -
gtsam::GPSFactor::This
GPSFactor This
Typedef to this class.
Definition GPSFactor.h:49
│ │ │ │ -
gtsam::GPSFactor::GPSFactor
GPSFactor()
default constructor - only use for serialization
Definition GPSFactor.h:52
│ │ │ │ -
gtsam::GPSFactor::GPSFactor
GPSFactor(Key key, const Point3 &gpsIn, const SharedNoiseModel &model)
Constructor from a measurement in a Cartesian frame.
Definition GPSFactor.h:63
│ │ │ │ -
gtsam::GPSFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition GPSFactor.h:68
│ │ │ │ -
gtsam::GPSFactor::shared_ptr
boost::shared_ptr< GPSFactor > shared_ptr
shorthand for a smart pointer to a factor
Definition GPSFactor.h:46
│ │ │ │ -
gtsam::GPSFactor2
Version of GPSFactor for NavState.
Definition GPSFactor.h:114
│ │ │ │ -
gtsam::GPSFactor2::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition GPSFactor.h:141
│ │ │ │ -
gtsam::GPSFactor2::This
GPSFactor2 This
Typedef to this class.
Definition GPSFactor.h:128
│ │ │ │ -
gtsam::GPSFactor2::GPSFactor2
GPSFactor2(Key key, const Point3 &gpsIn, const SharedNoiseModel &model)
Constructor from a measurement in a Cartesian frame.
Definition GPSFactor.h:136
│ │ │ │ -
gtsam::GPSFactor2::shared_ptr
boost::shared_ptr< GPSFactor2 > shared_ptr
shorthand for a smart pointer to a factor
Definition GPSFactor.h:125
│ │ │ │ -
gtsam::GPSFactor2::GPSFactor2
GPSFactor2()
default constructor - only use for serialization
Definition GPSFactor.h:131
│ │ │ │ -
gtsam::NavState
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition NavState.h:34
│ │ │ │ -
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ +
gtsam::Rot2
Rotation matrix NOTE: the angle theta is in radians unless explicitly stated.
Definition Rot2.h:36
│ │ │ │ +
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
│ │ │ │ +
gtsam::Rot3::unrotate
Point3 unrotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
rotate point from world to rotated frame
Definition Rot3.cpp:136
│ │ │ │ +
gtsam::Rot3::Yaw
static Rot3 Yaw(double t)
Positive yaw is to right (as in aircraft heading). See ypr.
Definition Rot3.h:174
│ │ │ │ +
gtsam::Rot3::rotate
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
rotate point from rotated coordinate frame to world
Definition Rot3M.cpp:149
│ │ │ │ +
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
│ │ │ │ +
gtsam::Unit3::point3
Point3 point3(OptionalJacobian< 3, 2 > H=boost::none) const
Return unit-norm Point3.
Definition Unit3.cpp:144
│ │ │ │ +
gtsam::MagFactor
Factor to estimate rotation given magnetometer reading This version uses model measured bM = scale * ...
Definition MagFactor.h:33
│ │ │ │ +
gtsam::MagFactor::clone
NonlinearFactor::shared_ptr clone() const override
Definition MagFactor.h:58
│ │ │ │ +
gtsam::MagFactor::MagFactor
MagFactor(Key key, const Point3 &measured, double scale, const Unit3 &direction, const Point3 &bias, const SharedNoiseModel &model)
Constructor of factor that estimates nav to body rotation bRn.
Definition MagFactor.h:50
│ │ │ │ +
gtsam::MagFactor::evaluateError
Vector evaluateError(const Rot2 &nRb, boost::optional< Matrix & > H=boost::none) const override
vector of errors
Definition MagFactor.h:77
│ │ │ │ +
gtsam::MagFactor1
Factor to estimate rotation given magnetometer reading This version uses model measured bM = scale * ...
Definition MagFactor.h:90
│ │ │ │ +
gtsam::MagFactor1::MagFactor1
MagFactor1(Key key, const Point3 &measured, double scale, const Unit3 &direction, const Point3 &bias, const SharedNoiseModel &model)
Constructor.
Definition MagFactor.h:99
│ │ │ │ +
gtsam::MagFactor1::clone
NonlinearFactor::shared_ptr clone() const override
Definition MagFactor.h:107
│ │ │ │ +
gtsam::MagFactor1::evaluateError
Vector evaluateError(const Rot3 &nRb, boost::optional< Matrix & > H=boost::none) const override
vector of errors
Definition MagFactor.h:115
│ │ │ │ +
gtsam::MagFactor2
Factor to calibrate local Earth magnetic field as well as magnetometer bias This version uses model m...
Definition MagFactor.h:128
│ │ │ │ +
gtsam::MagFactor2::clone
NonlinearFactor::shared_ptr clone() const override
Definition MagFactor.h:143
│ │ │ │ +
gtsam::MagFactor2::MagFactor2
MagFactor2(Key key1, Key key2, const Point3 &measured, const Rot3 &nRb, const SharedNoiseModel &model)
Constructor.
Definition MagFactor.h:136
│ │ │ │ +
gtsam::MagFactor2::evaluateError
Vector evaluateError(const Point3 &nM, const Point3 &bias, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
vector of errors
Definition MagFactor.h:153
│ │ │ │ +
gtsam::MagFactor3
Factor to calibrate local Earth magnetic field as well as magnetometer bias This version uses model m...
Definition MagFactor.h:169
│ │ │ │ +
gtsam::MagFactor3::evaluateError
Vector evaluateError(const double &scale, const Unit3 &direction, const Point3 &bias, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none) const override
vector of errors
Definition MagFactor.h:194
│ │ │ │ +
gtsam::MagFactor3::MagFactor3
MagFactor3(Key key1, Key key2, Key key3, const Point3 &measured, const Rot3 &nRb, const SharedNoiseModel &model)
Constructor.
Definition MagFactor.h:177
│ │ │ │ +
gtsam::MagFactor3::clone
NonlinearFactor::shared_ptr clone() const override
Definition MagFactor.h:184
│ │ │ │
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ +
gtsam::NoiseModelFactorN< Rot2 >::key
Key key() const
Returns a key.
Definition NonlinearFactor.h:518
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── encoding │ │ │ │ │ @@ -1 +1 @@ │ │ │ │ │ -us-ascii │ │ │ │ │ +utf-8 │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,236 +1,297 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GPSFactor.h │ │ │ │ │ +MagFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_N_a_v_S_t_a_t_e_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_3_._h> │ │ │ │ │ -23 │ │ │ │ │ -24namespace _g_t_s_a_m { │ │ │ │ │ -25 │ │ │ │ │ -_3_5class GTSAM_EXPORT _G_P_S_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -36 │ │ │ │ │ -37private: │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_2_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_3_._h> │ │ │ │ │ +24 │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ +26 │ │ │ │ │ +_3_3class _M_a_g_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +34 │ │ │ │ │ +35 const _P_o_i_n_t_3 measured_; │ │ │ │ │ +36 const _P_o_i_n_t_3 nM_; │ │ │ │ │ +37 const _P_o_i_n_t_3 bias_; │ │ │ │ │ 38 │ │ │ │ │ -39 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_P_o_s_e_3_> _B_a_s_e; │ │ │ │ │ +39public: │ │ │ │ │ 40 │ │ │ │ │ -41 _P_o_i_n_t_3 nT_; │ │ │ │ │ -42 │ │ │ │ │ -43public: │ │ │ │ │ -44 │ │ │ │ │ -_4_6 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -47 │ │ │ │ │ -_4_9 typedef _G_P_S_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ -50 │ │ │ │ │ -_5_2 _G_P_S_F_a_c_t_o_r(): nT_(0, 0, 0) {} │ │ │ │ │ -53 │ │ │ │ │ -54 _~_G_P_S_F_a_c_t_o_r() override {} │ │ │ │ │ -55 │ │ │ │ │ -_6_3 _G_P_S_F_a_c_t_o_r(_K_e_y key, const _P_o_i_n_t_3& gpsIn, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model) : │ │ │ │ │ -64 _B_a_s_e(model, key), nT_(gpsIn) { │ │ │ │ │ -65 } │ │ │ │ │ -66 │ │ │ │ │ -_6_8 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -69 return boost::static_pointer_cast( │ │ │ │ │ -70 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); │ │ │ │ │ -71 } │ │ │ │ │ -72 │ │ │ │ │ -74 void _p_r_i_n_t(const std::string& s = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ -75 DefaultKeyFormatter) const override; │ │ │ │ │ -76 │ │ │ │ │ -78 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& expected, double tol = 1e-9) const │ │ │ │ │ -override; │ │ │ │ │ -79 │ │ │ │ │ -81 Vector evaluateError(const _P_o_s_e_3& p, │ │ │ │ │ -82 boost::optional H = boost::none) const override; │ │ │ │ │ -83 │ │ │ │ │ -84 inline const _P_o_i_n_t_3 & measurementIn() const { │ │ │ │ │ -85 return nT_; │ │ │ │ │ -86 } │ │ │ │ │ -87 │ │ │ │ │ -93 static std::pair EstimateState(double t1, const Point3& │ │ │ │ │ -NED1, │ │ │ │ │ -94 double t2, const Point3& NED2, double timestamp); │ │ │ │ │ +_5_0 _M_a_g_F_a_c_t_o_r(_K_e_y _k_e_y, const _P_o_i_n_t_3& measured, double scale, │ │ │ │ │ +51 const _U_n_i_t_3& direction, const _P_o_i_n_t_3& bias, │ │ │ │ │ +52 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model) : │ │ │ │ │ +53 _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N<_R_o_t_2>(model, _k_e_y), // │ │ │ │ │ +54 measured_(measured), nM_(scale * direction), bias_(bias) { │ │ │ │ │ +55 } │ │ │ │ │ +56 │ │ │ │ │ +_5_8 NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +59 return boost::static_pointer_cast( │ │ │ │ │ +60 NonlinearFactor::shared_ptr(new _M_a_g_F_a_c_t_o_r(*this))); │ │ │ │ │ +61 } │ │ │ │ │ +62 │ │ │ │ │ +63 static _P_o_i_n_t_3 unrotate(const _R_o_t_2& R, const _P_o_i_n_t_3& p, │ │ │ │ │ +64 boost::optional HR = boost::none) { │ │ │ │ │ +65 _P_o_i_n_t_3 q = _R_o_t_3_:_:_Y_a_w(R.theta())._u_n_r_o_t_a_t_e(p, HR, boost::none); │ │ │ │ │ +66 if (HR) { │ │ │ │ │ +67 // assign to temporary first to avoid error in Win-Debug mode │ │ │ │ │ +68 Matrix H = HR->col(2); │ │ │ │ │ +69 *HR = H; │ │ │ │ │ +70 } │ │ │ │ │ +71 return q; │ │ │ │ │ +72 } │ │ │ │ │ +73 │ │ │ │ │ +_7_7 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const _R_o_t_2& nRb, │ │ │ │ │ +78 boost::optional H = boost::none) const override { │ │ │ │ │ +79 // measured bM = nRb� * nM + b │ │ │ │ │ +80 _P_o_i_n_t_3 hx = unrotate(nRb, nM_, H) + bias_; │ │ │ │ │ +81 return (hx - measured_); │ │ │ │ │ +82 } │ │ │ │ │ +83}; │ │ │ │ │ +84 │ │ │ │ │ +_9_0class _M_a_g_F_a_c_t_o_r_1: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +91 │ │ │ │ │ +92 const _P_o_i_n_t_3 measured_; │ │ │ │ │ +93 const _P_o_i_n_t_3 nM_; │ │ │ │ │ +94 const _P_o_i_n_t_3 bias_; │ │ │ │ │ 95 │ │ │ │ │ -96private: │ │ │ │ │ +96public: │ │ │ │ │ 97 │ │ │ │ │ -_9_9 friend class boost::serialization::access; │ │ │ │ │ -100 template │ │ │ │ │ -101 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -102 // NoiseModelFactor1 instead of NoiseModelFactorN for backward │ │ │ │ │ -compatibility │ │ │ │ │ -103 ar │ │ │ │ │ -104 & boost::serialization::make_nvp("NoiseModelFactor1", │ │ │ │ │ -105 boost::serialization::base_object(*this)); │ │ │ │ │ -106 ar & BOOST_SERIALIZATION_NVP(nT_); │ │ │ │ │ -107 } │ │ │ │ │ -108}; │ │ │ │ │ -109 │ │ │ │ │ -_1_1_4class GTSAM_EXPORT _G_P_S_F_a_c_t_o_r_2: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -115 │ │ │ │ │ -116private: │ │ │ │ │ -117 │ │ │ │ │ -118 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_N_a_v_S_t_a_t_e_> _B_a_s_e; │ │ │ │ │ -119 │ │ │ │ │ -120 _P_o_i_n_t_3 nT_; │ │ │ │ │ -121 │ │ │ │ │ -122public: │ │ │ │ │ -123 │ │ │ │ │ -_1_2_5 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -126 │ │ │ │ │ -_1_2_8 typedef _G_P_S_F_a_c_t_o_r_2 _T_h_i_s; │ │ │ │ │ +_9_9 _M_a_g_F_a_c_t_o_r_1(_K_e_y _k_e_y, const _P_o_i_n_t_3& measured, double scale, │ │ │ │ │ +100 const _U_n_i_t_3& direction, const _P_o_i_n_t_3& bias, │ │ │ │ │ +101 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model) : │ │ │ │ │ +102 _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N<_R_o_t_3>(model, _k_e_y), // │ │ │ │ │ +103 measured_(measured), nM_(scale * direction), bias_(bias) { │ │ │ │ │ +104 } │ │ │ │ │ +105 │ │ │ │ │ +_1_0_7 NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +108 return boost::static_pointer_cast( │ │ │ │ │ +109 NonlinearFactor::shared_ptr(new _M_a_g_F_a_c_t_o_r_1(*this))); │ │ │ │ │ +110 } │ │ │ │ │ +111 │ │ │ │ │ +_1_1_5 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const _R_o_t_3& nRb, │ │ │ │ │ +116 boost::optional H = boost::none) const override { │ │ │ │ │ +117 // measured bM = nRb� * nM + b │ │ │ │ │ +118 _P_o_i_n_t_3 hx = nRb._u_n_r_o_t_a_t_e(nM_, H, boost::none) + bias_; │ │ │ │ │ +119 return (hx - measured_); │ │ │ │ │ +120 } │ │ │ │ │ +121}; │ │ │ │ │ +122 │ │ │ │ │ +_1_2_8class _M_a_g_F_a_c_t_o_r_2: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ 129 │ │ │ │ │ -_1_3_1 _G_P_S_F_a_c_t_o_r_2():nT_(0, 0, 0) {} │ │ │ │ │ +130 const _P_o_i_n_t_3 measured_; │ │ │ │ │ +131 const _R_o_t_3 bRn_; │ │ │ │ │ 132 │ │ │ │ │ -133 _~_G_P_S_F_a_c_t_o_r_2() override {} │ │ │ │ │ +133public: │ │ │ │ │ 134 │ │ │ │ │ -_1_3_6 _G_P_S_F_a_c_t_o_r_2(_K_e_y key, const _P_o_i_n_t_3& gpsIn, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model) : │ │ │ │ │ -137 _B_a_s_e(model, key), nT_(gpsIn) { │ │ │ │ │ -138 } │ │ │ │ │ -139 │ │ │ │ │ -_1_4_1 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -142 return boost::static_pointer_cast( │ │ │ │ │ -143 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); │ │ │ │ │ -144 } │ │ │ │ │ -145 │ │ │ │ │ -147 void _p_r_i_n_t(const std::string& s = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ -148 DefaultKeyFormatter) const override; │ │ │ │ │ -149 │ │ │ │ │ -151 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& expected, double tol = 1e-9) const │ │ │ │ │ -override; │ │ │ │ │ -152 │ │ │ │ │ -154 Vector evaluateError(const _N_a_v_S_t_a_t_e& p, │ │ │ │ │ -155 boost::optional H = boost::none) const override; │ │ │ │ │ -156 │ │ │ │ │ -157 inline const _P_o_i_n_t_3 & measurementIn() const { │ │ │ │ │ -158 return nT_; │ │ │ │ │ -159 } │ │ │ │ │ -160 │ │ │ │ │ -161private: │ │ │ │ │ -162 │ │ │ │ │ -_1_6_4 friend class boost::serialization::access; │ │ │ │ │ -165 template │ │ │ │ │ -166 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -167 // NoiseModelFactor1 instead of NoiseModelFactorN for backward │ │ │ │ │ -compatibility │ │ │ │ │ -168 ar │ │ │ │ │ -169 & boost::serialization::make_nvp("NoiseModelFactor1", │ │ │ │ │ -170 boost::serialization::base_object(*this)); │ │ │ │ │ -171 ar & BOOST_SERIALIZATION_NVP(nT_); │ │ │ │ │ -172 } │ │ │ │ │ -173}; │ │ │ │ │ -174 │ │ │ │ │ -175} │ │ │ │ │ -_P_o_s_e_3_._h │ │ │ │ │ -3D Pose │ │ │ │ │ -_N_a_v_S_t_a_t_e_._h │ │ │ │ │ -Navigation state composing of attitude, position, and velocity. │ │ │ │ │ +_1_3_6 _M_a_g_F_a_c_t_o_r_2(_K_e_y key1, _K_e_y key2, const _P_o_i_n_t_3& measured, const _R_o_t_3& nRb, │ │ │ │ │ +137 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model) : │ │ │ │ │ +138 _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N<_P_o_i_n_t_3, _P_o_i_n_t_3>(model, key1, key2), // │ │ │ │ │ +139 measured_(measured), bRn_(nRb.inverse()) { │ │ │ │ │ +140 } │ │ │ │ │ +141 │ │ │ │ │ +_1_4_3 NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +144 return boost::static_pointer_cast( │ │ │ │ │ +145 NonlinearFactor::shared_ptr(new _M_a_g_F_a_c_t_o_r_2(*this))); │ │ │ │ │ +146 } │ │ │ │ │ +147 │ │ │ │ │ +_1_5_3 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const _P_o_i_n_t_3& nM, const _P_o_i_n_t_3& bias, │ │ │ │ │ +154 boost::optional H1 = boost::none, boost::optional H2 = │ │ │ │ │ +155 boost::none) const override { │ │ │ │ │ +156 // measured bM = nRb� * nM + b, where b is unknown bias │ │ │ │ │ +157 _P_o_i_n_t_3 hx = bRn_._r_o_t_a_t_e(nM, boost::none, H1) + bias; │ │ │ │ │ +158 if (H2) │ │ │ │ │ +159 *H2 = I_3x3; │ │ │ │ │ +160 return (hx - measured_); │ │ │ │ │ +161 } │ │ │ │ │ +162}; │ │ │ │ │ +163 │ │ │ │ │ +_1_6_9class _M_a_g_F_a_c_t_o_r_3: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +170 │ │ │ │ │ +171 const _P_o_i_n_t_3 measured_; │ │ │ │ │ +172 const _R_o_t_3 bRn_; │ │ │ │ │ +173 │ │ │ │ │ +174public: │ │ │ │ │ +175 │ │ │ │ │ +_1_7_7 _M_a_g_F_a_c_t_o_r_3(_K_e_y key1, _K_e_y key2, _K_e_y key3, const _P_o_i_n_t_3& measured, │ │ │ │ │ +178 const _R_o_t_3& nRb, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model) : │ │ │ │ │ +179 _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N(model, key1, key2, key3), // │ │ │ │ │ +180 measured_(measured), bRn_(nRb.inverse()) { │ │ │ │ │ +181 } │ │ │ │ │ +182 │ │ │ │ │ +_1_8_4 NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +185 return boost::static_pointer_cast( │ │ │ │ │ +186 NonlinearFactor::shared_ptr(new _M_a_g_F_a_c_t_o_r_3(*this))); │ │ │ │ │ +187 } │ │ │ │ │ +188 │ │ │ │ │ +_1_9_4 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const double& scale, const _U_n_i_t_3& direction, │ │ │ │ │ +195 const _P_o_i_n_t_3& bias, boost::optional H1 = boost::none, │ │ │ │ │ +196 boost::optional H2 = boost::none, boost::optional H3 = │ │ │ │ │ +197 boost::none) const override { │ │ │ │ │ +198 // measured bM = nRb� * nM + b, where b is unknown bias │ │ │ │ │ +199 _U_n_i_t_3 rotated = bRn_._r_o_t_a_t_e(direction, boost::none, H2); │ │ │ │ │ +200 _P_o_i_n_t_3 hx = scale * rotated._p_o_i_n_t_3() + bias; │ │ │ │ │ +201 if (H1) │ │ │ │ │ +202 *H1 = rotated._p_o_i_n_t_3(); │ │ │ │ │ +203 if (H2) // H2 is 2*2, but we need 3*2 │ │ │ │ │ +204 { │ │ │ │ │ +205 Matrix H; │ │ │ │ │ +206 rotated._p_o_i_n_t_3(H); │ │ │ │ │ +207 *H2 = scale * H * (*H2); │ │ │ │ │ +208 } │ │ │ │ │ +209 if (H3) │ │ │ │ │ +210 *H3 = I_3x3; │ │ │ │ │ +211 return (hx - measured_); │ │ │ │ │ +212 } │ │ │ │ │ +213}; │ │ │ │ │ +214 │ │ │ │ │ +215} │ │ │ │ │ +216 │ │ │ │ │ +_R_o_t_3_._h │ │ │ │ │ +3D rotation represented as a rotation matrix or quaternion │ │ │ │ │ +_R_o_t_2_._h │ │ │ │ │ +2D rotation │ │ │ │ │ _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ _g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ Vector3 Point3 │ │ │ │ │ As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ typedef Point3 to Vector3... │ │ │ │ │ DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ _g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ Aliases. │ │ │ │ │ DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ -A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_G_P_S_F_a_c_t_o_r │ │ │ │ │ -Prior on position in a Cartesian frame. │ │ │ │ │ -DDeeffiinniittiioonn GPSFactor.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_G_P_S_F_a_c_t_o_r_:_:_T_h_i_s │ │ │ │ │ -GPSFactor This │ │ │ │ │ -Typedef to this class. │ │ │ │ │ -DDeeffiinniittiioonn GPSFactor.h:49 │ │ │ │ │ -_g_t_s_a_m_:_:_G_P_S_F_a_c_t_o_r_:_:_G_P_S_F_a_c_t_o_r │ │ │ │ │ -GPSFactor() │ │ │ │ │ -default constructor - only use for serialization │ │ │ │ │ -DDeeffiinniittiioonn GPSFactor.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_G_P_S_F_a_c_t_o_r_:_:_G_P_S_F_a_c_t_o_r │ │ │ │ │ -GPSFactor(Key key, const Point3 &gpsIn, const SharedNoiseModel &model) │ │ │ │ │ -Constructor from a measurement in a Cartesian frame. │ │ │ │ │ -DDeeffiinniittiioonn GPSFactor.h:63 │ │ │ │ │ -_g_t_s_a_m_:_:_G_P_S_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ -gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn GPSFactor.h:68 │ │ │ │ │ -_g_t_s_a_m_:_:_G_P_S_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< GPSFactor > shared_ptr │ │ │ │ │ -shorthand for a smart pointer to a factor │ │ │ │ │ -DDeeffiinniittiioonn GPSFactor.h:46 │ │ │ │ │ -_g_t_s_a_m_:_:_G_P_S_F_a_c_t_o_r_2 │ │ │ │ │ -Version of GPSFactor for NavState. │ │ │ │ │ -DDeeffiinniittiioonn GPSFactor.h:114 │ │ │ │ │ -_g_t_s_a_m_:_:_G_P_S_F_a_c_t_o_r_2_:_:_c_l_o_n_e │ │ │ │ │ -gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn GPSFactor.h:141 │ │ │ │ │ -_g_t_s_a_m_:_:_G_P_S_F_a_c_t_o_r_2_:_:_T_h_i_s │ │ │ │ │ -GPSFactor2 This │ │ │ │ │ -Typedef to this class. │ │ │ │ │ -DDeeffiinniittiioonn GPSFactor.h:128 │ │ │ │ │ -_g_t_s_a_m_:_:_G_P_S_F_a_c_t_o_r_2_:_:_G_P_S_F_a_c_t_o_r_2 │ │ │ │ │ -GPSFactor2(Key key, const Point3 &gpsIn, const SharedNoiseModel &model) │ │ │ │ │ -Constructor from a measurement in a Cartesian frame. │ │ │ │ │ -DDeeffiinniittiioonn GPSFactor.h:136 │ │ │ │ │ -_g_t_s_a_m_:_:_G_P_S_F_a_c_t_o_r_2_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< GPSFactor2 > shared_ptr │ │ │ │ │ -shorthand for a smart pointer to a factor │ │ │ │ │ -DDeeffiinniittiioonn GPSFactor.h:125 │ │ │ │ │ -_g_t_s_a_m_:_:_G_P_S_F_a_c_t_o_r_2_:_:_G_P_S_F_a_c_t_o_r_2 │ │ │ │ │ -GPSFactor2() │ │ │ │ │ -default constructor - only use for serialization │ │ │ │ │ -DDeeffiinniittiioonn GPSFactor.h:131 │ │ │ │ │ -_g_t_s_a_m_:_:_N_a_v_S_t_a_t_e │ │ │ │ │ -Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does │ │ │ │ │ -not make sense to make... │ │ │ │ │ -DDeeffiinniittiioonn NavState.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ -Nonlinear factor base class. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_2 │ │ │ │ │ +Rotation matrix NOTE: the angle theta is in radians unless explicitly stated. │ │ │ │ │ +DDeeffiinniittiioonn Rot2.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ +Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor │ │ │ │ │ +symbol GTSAM_USE_QUATERNIO... │ │ │ │ │ +DDeeffiinniittiioonn Rot3.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_3_:_:_u_n_r_o_t_a_t_e │ │ │ │ │ +Point3 unrotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, │ │ │ │ │ +OptionalJacobian< 3, 3 > H2=boost::none) const │ │ │ │ │ +rotate point from world to rotated frame │ │ │ │ │ +DDeeffiinniittiioonn Rot3.cpp:136 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_3_:_:_Y_a_w │ │ │ │ │ +static Rot3 Yaw(double t) │ │ │ │ │ +Positive yaw is to right (as in aircraft heading). See ypr. │ │ │ │ │ +DDeeffiinniittiioonn Rot3.h:174 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_3_:_:_r_o_t_a_t_e │ │ │ │ │ +Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, │ │ │ │ │ +OptionalJacobian< 3, 3 > H2=boost::none) const │ │ │ │ │ +rotate point from rotated coordinate frame to world │ │ │ │ │ +DDeeffiinniittiioonn Rot3M.cpp:149 │ │ │ │ │ +_g_t_s_a_m_:_:_U_n_i_t_3 │ │ │ │ │ +Represents a 3D point on a unit sphere. │ │ │ │ │ +DDeeffiinniittiioonn Unit3.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_U_n_i_t_3_:_:_p_o_i_n_t_3 │ │ │ │ │ +Point3 point3(OptionalJacobian< 3, 2 > H=boost::none) const │ │ │ │ │ +Return unit-norm Point3. │ │ │ │ │ +DDeeffiinniittiioonn Unit3.cpp:144 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r │ │ │ │ │ +Factor to estimate rotation given magnetometer reading This version uses model │ │ │ │ │ +measured bM = scale * ... │ │ │ │ │ +DDeeffiinniittiioonn MagFactor.h:33 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ +NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn MagFactor.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_:_:_M_a_g_F_a_c_t_o_r │ │ │ │ │ +MagFactor(Key key, const Point3 &measured, double scale, const Unit3 │ │ │ │ │ +&direction, const Point3 &bias, const SharedNoiseModel &model) │ │ │ │ │ +Constructor of factor that estimates nav to body rotation bRn. │ │ │ │ │ +DDeeffiinniittiioonn MagFactor.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ +Vector evaluateError(const Rot2 &nRb, boost::optional< Matrix & > H=boost:: │ │ │ │ │ +none) const override │ │ │ │ │ +vector of errors │ │ │ │ │ +DDeeffiinniittiioonn MagFactor.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_1 │ │ │ │ │ +Factor to estimate rotation given magnetometer reading This version uses model │ │ │ │ │ +measured bM = scale * ... │ │ │ │ │ +DDeeffiinniittiioonn MagFactor.h:90 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_1_:_:_M_a_g_F_a_c_t_o_r_1 │ │ │ │ │ +MagFactor1(Key key, const Point3 &measured, double scale, const Unit3 │ │ │ │ │ +&direction, const Point3 &bias, const SharedNoiseModel &model) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn MagFactor.h:99 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_1_:_:_c_l_o_n_e │ │ │ │ │ +NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn MagFactor.h:107 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_1_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ +Vector evaluateError(const Rot3 &nRb, boost::optional< Matrix & > H=boost:: │ │ │ │ │ +none) const override │ │ │ │ │ +vector of errors │ │ │ │ │ +DDeeffiinniittiioonn MagFactor.h:115 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_2 │ │ │ │ │ +Factor to calibrate local Earth magnetic field as well as magnetometer bias │ │ │ │ │ +This version uses model m... │ │ │ │ │ +DDeeffiinniittiioonn MagFactor.h:128 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_2_:_:_c_l_o_n_e │ │ │ │ │ +NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn MagFactor.h:143 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_2_:_:_M_a_g_F_a_c_t_o_r_2 │ │ │ │ │ +MagFactor2(Key key1, Key key2, const Point3 &measured, const Rot3 &nRb, const │ │ │ │ │ +SharedNoiseModel &model) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn MagFactor.h:136 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_2_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ +Vector evaluateError(const Point3 &nM, const Point3 &bias, boost::optional< │ │ │ │ │ +Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const │ │ │ │ │ +override │ │ │ │ │ +vector of errors │ │ │ │ │ +DDeeffiinniittiioonn MagFactor.h:153 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_3 │ │ │ │ │ +Factor to calibrate local Earth magnetic field as well as magnetometer bias │ │ │ │ │ +This version uses model m... │ │ │ │ │ +DDeeffiinniittiioonn MagFactor.h:169 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_3_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ +Vector evaluateError(const double &scale, const Unit3 &direction, const Point3 │ │ │ │ │ +&bias, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > │ │ │ │ │ +H2=boost::none, boost::optional< Matrix & > H3=boost::none) const override │ │ │ │ │ +vector of errors │ │ │ │ │ +DDeeffiinniittiioonn MagFactor.h:194 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_3_:_:_M_a_g_F_a_c_t_o_r_3 │ │ │ │ │ +MagFactor3(Key key1, Key key2, Key key3, const Point3 &measured, const Rot3 │ │ │ │ │ +&nRb, const SharedNoiseModel &model) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn MagFactor.h:177 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_g_F_a_c_t_o_r_3_:_:_c_l_o_n_e │ │ │ │ │ +NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn MagFactor.h:184 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ variables. │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_ _R_o_t_2_ _>_:_:_k_e_y │ │ │ │ │ +Key key() const │ │ │ │ │ +Returns a key. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:518 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _G_P_S_F_a_c_t_o_r_._h │ │ │ │ │ + * _M_a_g_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00998.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/BarometricFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ImuBias.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,43 +96,43 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
BarometricFactor.h File Reference
│ │ │ │ +
ImuBias.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Header file for Barometric factor. │ │ │ │ -More...

│ │ │ │ - │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::BarometricFactor
 Prior on height in a cartesian frame. More...
class  gtsam::imuBias::ConstantBias
 
struct  gtsam::traits< imuBias::ConstantBias >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
namespace  gtsam::imuBias
 All bias models live in the imuBias namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Header file for Barometric factor.

│ │ │ │ -
Author
Peter Milani
│ │ │ │ -
Date
December 16, 2021
│ │ │ │ +
Date
Feb 2, 2012
│ │ │ │ +
Author
Vadim Indelman, Stephen Williams
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,27 +1,29 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -BarometricFactor.h File Reference │ │ │ │ │ -Header file for Barometric factor. _M_o_r_e_._._. │ │ │ │ │ +ImuBias.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r │ │ │ │ │ -  Prior on height in a cartesian frame. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +namespace   _g_t_s_a_m_:_:_i_m_u_B_i_a_s │ │ │ │ │ +  All bias models live in the _i_m_u_B_i_a_s namespace. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Header file for Barometric factor. │ │ │ │ │ - Author │ │ │ │ │ - Peter Milani │ │ │ │ │ Date │ │ │ │ │ - December 16, 2021 │ │ │ │ │ + Feb 2, 2012 │ │ │ │ │ + Author │ │ │ │ │ + Vadim Indelman, Stephen Williams │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r_._h │ │ │ │ │ + * _I_m_u_B_i_a_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a00998_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/BarometricFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ImuBias.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,15 +98,15 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
BarometricFactor.h
│ │ │ │ +
ImuBias.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │ @@ -114,110 +114,204 @@ │ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
18#pragma once
│ │ │ │
19
│ │ │ │ -
20#include <gtsam/geometry/Pose3.h>
│ │ │ │ -
21#include <gtsam/navigation/NavState.h>
│ │ │ │ -
22#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
23
│ │ │ │ -
24namespace gtsam {
│ │ │ │ -
25
│ │ │ │ -
│ │ │ │ -
34class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN<Pose3, double> {
│ │ │ │ -
35 private:
│ │ │ │ -
36 typedef NoiseModelFactorN<Pose3, double> Base;
│ │ │ │ -
37
│ │ │ │ -
38 double nT_;
│ │ │ │ -
39
│ │ │ │ -
40 public:
│ │ │ │ -
42 typedef boost::shared_ptr<BarometricFactor> shared_ptr;
│ │ │ │ -
43
│ │ │ │ -
45 typedef BarometricFactor This;
│ │ │ │ -
46
│ │ │ │ -
48 BarometricFactor() : nT_(0) {}
│ │ │ │ +
20#include <gtsam/base/OptionalJacobian.h>
│ │ │ │ +
21#include <gtsam/base/VectorSpace.h>
│ │ │ │ +
22#include <iosfwd>
│ │ │ │ +
23#include <boost/serialization/nvp.hpp>
│ │ │ │ +
24
│ │ │ │ +
25namespace gtsam {
│ │ │ │ +
26
│ │ │ │ +
28namespace imuBias {
│ │ │ │ +
29
│ │ │ │ +
│ │ │ │ +
30class GTSAM_EXPORT ConstantBias {
│ │ │ │ +
31private:
│ │ │ │ +
32 Vector3 biasAcc_;
│ │ │ │ +
33 Vector3 biasGyro_;
│ │ │ │ +
34
│ │ │ │ +
35public:
│ │ │ │ +
37 static const size_t dimension = 6;
│ │ │ │ +
38
│ │ │ │ +
41
│ │ │ │ +
42 ConstantBias() :
│ │ │ │ +
43 biasAcc_(0.0, 0.0, 0.0), biasGyro_(0.0, 0.0, 0.0) {
│ │ │ │ +
44 }
│ │ │ │ +
45
│ │ │ │ +
46 ConstantBias(const Vector3& biasAcc, const Vector3& biasGyro) :
│ │ │ │ +
47 biasAcc_(biasAcc), biasGyro_(biasGyro) {
│ │ │ │ +
48 }
│ │ │ │
49
│ │ │ │ -
50 ~BarometricFactor() override {}
│ │ │ │ -
51
│ │ │ │ -
│ │ │ │ -
59 BarometricFactor(Key key, Key baroKey, const double& baroIn,
│ │ │ │ -
60 const SharedNoiseModel& model)
│ │ │ │ -
61 : Base(model, key, baroKey), nT_(heightOut(baroIn)) {}
│ │ │ │ +
50 explicit ConstantBias(const Vector6& v) :
│ │ │ │ +
51 biasAcc_(v.head<3>()), biasGyro_(v.tail<3>()) {
│ │ │ │ +
52 }
│ │ │ │ +
53
│ │ │ │ +
55
│ │ │ │ +
│ │ │ │ +
57 Vector6 vector() const {
│ │ │ │ +
58 Vector6 v;
│ │ │ │ +
59 v << biasAcc_, biasGyro_;
│ │ │ │ +
60 return v;
│ │ │ │ +
61 }
│ │ │ │
│ │ │ │
62
│ │ │ │
│ │ │ │ -
64 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
65 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ -
66 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
│ │ │ │ -
67 }
│ │ │ │ -
│ │ │ │ -
68
│ │ │ │ -
70 void print(
│ │ │ │ -
71 const std::string& s = "",
│ │ │ │ -
72 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
│ │ │ │ -
73
│ │ │ │ -
75 bool equals(const NonlinearFactor& expected,
│ │ │ │ -
76 double tol = 1e-9) const override;
│ │ │ │ -
77
│ │ │ │ -
79 Vector evaluateError(
│ │ │ │ -
80 const Pose3& p, const double& b,
│ │ │ │ -
81 boost::optional<Matrix&> H = boost::none,
│ │ │ │ -
82 boost::optional<Matrix&> H2 = boost::none) const override;
│ │ │ │ -
83
│ │ │ │ -
84 inline const double& measurementIn() const { return nT_; }
│ │ │ │ -
85
│ │ │ │ -
86 inline double heightOut(double n) const {
│ │ │ │ -
87 // From https://www.grc.nasa.gov/www/k-12/airplane/atmosmet.html
│ │ │ │ -
88 return (std::pow(n / 101.29, 1. / 5.256) * 288.08 - 273.1 - 15.04) /
│ │ │ │ -
89 -0.00649;
│ │ │ │ -
90 };
│ │ │ │ -
91
│ │ │ │ -
92 inline double baroOut(const double& meters) {
│ │ │ │ -
93 double temp = 15.04 - 0.00649 * meters;
│ │ │ │ -
94 return 101.29 * std::pow(((temp + 273.1) / 288.08), 5.256);
│ │ │ │ -
95 };
│ │ │ │ -
96
│ │ │ │ -
97 private:
│ │ │ │ -
99 friend class boost::serialization::access;
│ │ │ │ -
100 template <class ARCHIVE>
│ │ │ │ -
101 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ -
102 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ -
103 ar& boost::serialization::make_nvp(
│ │ │ │ -
104 "NoiseModelFactor1",
│ │ │ │ -
105 boost::serialization::base_object<Base>(*this));
│ │ │ │ -
106 ar& BOOST_SERIALIZATION_NVP(nT_);
│ │ │ │ -
107 }
│ │ │ │ -
108};
│ │ │ │ -
│ │ │ │ -
109
│ │ │ │ -
110} // namespace gtsam
│ │ │ │ -
Pose3.h
3D Pose
│ │ │ │ -
NavState.h
Navigation state composing of attitude, position, and velocity.
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
64 const Vector3& accelerometer() const {
│ │ │ │ +
65 return biasAcc_;
│ │ │ │ +
66 }
│ │ │ │ +
│ │ │ │ +
67
│ │ │ │ +
│ │ │ │ +
69 const Vector3& gyroscope() const {
│ │ │ │ +
70 return biasGyro_;
│ │ │ │ +
71 }
│ │ │ │ +
│ │ │ │ +
72
│ │ │ │ +
│ │ │ │ +
74 Vector3 correctAccelerometer(const Vector3& measurement,
│ │ │ │ +
75 OptionalJacobian<3, 6> H1 = boost::none,
│ │ │ │ +
76 OptionalJacobian<3, 3> H2 = boost::none) const {
│ │ │ │ +
77 if (H1) (*H1) << -I_3x3, Z_3x3;
│ │ │ │ +
78 if (H2) (*H2) << I_3x3;
│ │ │ │ +
79 return measurement - biasAcc_;
│ │ │ │ +
80 }
│ │ │ │ +
│ │ │ │ +
81
│ │ │ │ +
│ │ │ │ +
83 Vector3 correctGyroscope(const Vector3& measurement,
│ │ │ │ +
84 OptionalJacobian<3, 6> H1 = boost::none,
│ │ │ │ +
85 OptionalJacobian<3, 3> H2 = boost::none) const {
│ │ │ │ +
86 if (H1) (*H1) << Z_3x3, -I_3x3;
│ │ │ │ +
87 if (H2) (*H2) << I_3x3;
│ │ │ │ +
88 return measurement - biasGyro_;
│ │ │ │ +
89 }
│ │ │ │ +
│ │ │ │ +
90
│ │ │ │ +
93
│ │ │ │ +
95 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
│ │ │ │ +
96 const ConstantBias& bias);
│ │ │ │ +
97
│ │ │ │ +
99 void print(const std::string& s = "") const;
│ │ │ │ +
100
│ │ │ │ +
│ │ │ │ +
102 inline bool equals(const ConstantBias& expected, double tol = 1e-5) const {
│ │ │ │ +
103 return equal_with_abs_tol(biasAcc_, expected.biasAcc_, tol)
│ │ │ │ +
104 && equal_with_abs_tol(biasGyro_, expected.biasGyro_, tol);
│ │ │ │ +
105 }
│ │ │ │ +
│ │ │ │ +
106
│ │ │ │ +
110
│ │ │ │ +
│ │ │ │ +
112 static ConstantBias Identity() {
│ │ │ │ +
113 return ConstantBias();
│ │ │ │ +
114 }
│ │ │ │ +
│ │ │ │ +
115
│ │ │ │ +
│ │ │ │ +
117 inline ConstantBias operator-() const {
│ │ │ │ +
118 return ConstantBias(-biasAcc_, -biasGyro_);
│ │ │ │ +
119 }
│ │ │ │ +
│ │ │ │ +
120
│ │ │ │ +
│ │ │ │ +
122 ConstantBias operator+(const Vector6& v) const {
│ │ │ │ +
123 return ConstantBias(biasAcc_ + v.head<3>(), biasGyro_ + v.tail<3>());
│ │ │ │ +
124 }
│ │ │ │ +
│ │ │ │ +
125
│ │ │ │ +
│ │ │ │ +
127 ConstantBias operator+(const ConstantBias& b) const {
│ │ │ │ +
128 return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_);
│ │ │ │ +
129 }
│ │ │ │ +
│ │ │ │ +
130
│ │ │ │ +
│ │ │ │ +
132 ConstantBias operator-(const ConstantBias& b) const {
│ │ │ │ +
133 return ConstantBias(biasAcc_ - b.biasAcc_, biasGyro_ - b.biasGyro_);
│ │ │ │ +
134 }
│ │ │ │ +
│ │ │ │ +
135
│ │ │ │ +
137
│ │ │ │ +
138#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ +
141 ConstantBias GTSAM_DEPRECATED inverse() { return -(*this); }
│ │ │ │ +
142 ConstantBias GTSAM_DEPRECATED compose(const ConstantBias& q) {
│ │ │ │ +
143 return (*this) + q;
│ │ │ │ +
144 }
│ │ │ │ +
145 ConstantBias GTSAM_DEPRECATED between(const ConstantBias& q) {
│ │ │ │ +
146 return q - (*this);
│ │ │ │ +
147 }
│ │ │ │ +
148 Vector6 GTSAM_DEPRECATED localCoordinates(const ConstantBias& q) {
│ │ │ │ +
149 return (q - (*this)).vector();
│ │ │ │ +
150 }
│ │ │ │ +
151 ConstantBias GTSAM_DEPRECATED retract(const Vector6& v) {
│ │ │ │ +
152 return (*this) + ConstantBias(v);
│ │ │ │ +
153 }
│ │ │ │ +
154 static Vector6 GTSAM_DEPRECATED Logmap(const ConstantBias& p) {
│ │ │ │ +
155 return p.vector();
│ │ │ │ +
156 }
│ │ │ │ +
157 static ConstantBias GTSAM_DEPRECATED Expmap(const Vector6& v) {
│ │ │ │ +
158 return ConstantBias(v);
│ │ │ │ +
159 }
│ │ │ │ +
161#endif
│ │ │ │ +
162
│ │ │ │ +
163private:
│ │ │ │ +
164
│ │ │ │ +
167
│ │ │ │ +
169 friend class boost::serialization::access;
│ │ │ │ +
170 template<class ARCHIVE>
│ │ │ │ +
171 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
172 ar & BOOST_SERIALIZATION_NVP(biasAcc_);
│ │ │ │ +
173 ar & BOOST_SERIALIZATION_NVP(biasGyro_);
│ │ │ │ +
174 }
│ │ │ │ +
175
│ │ │ │ +
176
│ │ │ │ +
177public:
│ │ │ │ +
178 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
180
│ │ │ │ +
181}; // ConstantBias class
│ │ │ │ +
│ │ │ │ +
182} // namespace imuBias
│ │ │ │ +
183
│ │ │ │ +
184template<>
│ │ │ │ +
│ │ │ │ +
185struct traits<imuBias::ConstantBias> : public internal::VectorSpace<
│ │ │ │ +
186 imuBias::ConstantBias> {
│ │ │ │ +
187};
│ │ │ │ +
│ │ │ │ +
188
│ │ │ │ +
189} // namespace gtsam
│ │ │ │ +
190
│ │ │ │ +
OptionalJacobian.h
Special class for optional Jacobian arguments.
│ │ │ │ +
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ -
gtsam::BarometricFactor
Prior on height in a cartesian frame.
Definition BarometricFactor.h:34
│ │ │ │ -
gtsam::BarometricFactor::This
BarometricFactor This
Typedef to this class.
Definition BarometricFactor.h:45
│ │ │ │ -
gtsam::BarometricFactor::BarometricFactor
BarometricFactor(Key key, Key baroKey, const double &baroIn, const SharedNoiseModel &model)
Constructor from a measurement of pressure in KPa.
Definition BarometricFactor.h:59
│ │ │ │ -
gtsam::BarometricFactor::shared_ptr
boost::shared_ptr< BarometricFactor > shared_ptr
shorthand for a smart pointer to a factor
Definition BarometricFactor.h:42
│ │ │ │ -
gtsam::BarometricFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition BarometricFactor.h:64
│ │ │ │ -
gtsam::BarometricFactor::BarometricFactor
BarometricFactor()
default constructor - only use for serialization
Definition BarometricFactor.h:48
│ │ │ │ -
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ -
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ +
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition Matrix.h:81
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ +
gtsam::internal::VectorSpace
VectorSpace provides both Testable and VectorSpaceTraits.
Definition VectorSpace.h:207
│ │ │ │ +
gtsam::imuBias::ConstantBias
Definition ImuBias.h:30
│ │ │ │ +
gtsam::imuBias::ConstantBias::gyroscope
const Vector3 & gyroscope() const
get gyroscope bias
Definition ImuBias.h:69
│ │ │ │ +
gtsam::imuBias::ConstantBias::Identity
static ConstantBias Identity()
identity for group operation
Definition ImuBias.h:112
│ │ │ │ +
gtsam::imuBias::ConstantBias::equals
bool equals(const ConstantBias &expected, double tol=1e-5) const
equality up to tolerance
Definition ImuBias.h:102
│ │ │ │ +
gtsam::imuBias::ConstantBias::operator-
ConstantBias operator-(const ConstantBias &b) const
subtraction
Definition ImuBias.h:132
│ │ │ │ +
gtsam::imuBias::ConstantBias::correctAccelerometer
Vector3 correctAccelerometer(const Vector3 &measurement, OptionalJacobian< 3, 6 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
Correct an accelerometer measurement using this bias model, and optionally compute Jacobians.
Definition ImuBias.h:74
│ │ │ │ +
gtsam::imuBias::ConstantBias::operator-
ConstantBias operator-() const
inverse
Definition ImuBias.h:117
│ │ │ │ +
gtsam::imuBias::ConstantBias::correctGyroscope
Vector3 correctGyroscope(const Vector3 &measurement, OptionalJacobian< 3, 6 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
Correct a gyroscope measurement using this bias model, and optionally compute Jacobians.
Definition ImuBias.h:83
│ │ │ │ +
gtsam::imuBias::ConstantBias::vector
Vector6 vector() const
return the accelerometer and gyro biases in a single vector
Definition ImuBias.h:57
│ │ │ │ +
gtsam::imuBias::ConstantBias::operator+
ConstantBias operator+(const Vector6 &v) const
addition of vector on right
Definition ImuBias.h:122
│ │ │ │ +
gtsam::imuBias::ConstantBias::operator+
ConstantBias operator+(const ConstantBias &b) const
addition
Definition ImuBias.h:127
│ │ │ │ +
gtsam::imuBias::ConstantBias::accelerometer
const Vector3 & accelerometer() const
get accelerometer bias
Definition ImuBias.h:64
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -BarometricFactor.h │ │ │ │ │ +ImuBias.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ @@ -16,142 +16,230 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 18#pragma once │ │ │ │ │ 19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_3_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_N_a_v_S_t_a_t_e_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -23 │ │ │ │ │ -24namespace _g_t_s_a_m { │ │ │ │ │ -25 │ │ │ │ │ -_3_4class GTSAM_EXPORT _B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ -{ │ │ │ │ │ -35 private: │ │ │ │ │ -36 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_P_o_s_e_3_,_ _d_o_u_b_l_e_> _B_a_s_e; │ │ │ │ │ -37 │ │ │ │ │ -38 double nT_; │ │ │ │ │ -39 │ │ │ │ │ -40 public: │ │ │ │ │ -_4_2 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -43 │ │ │ │ │ -_4_5 typedef _B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ -46 │ │ │ │ │ -_4_8 _B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r() : nT_(0) {} │ │ │ │ │ +20#include <_g_t_s_a_m_/_b_a_s_e_/_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h> │ │ │ │ │ +21#include │ │ │ │ │ +22#include │ │ │ │ │ +23#include │ │ │ │ │ +24 │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ +26 │ │ │ │ │ +28namespace imuBias { │ │ │ │ │ +29 │ │ │ │ │ +_3_0class GTSAM_EXPORT _C_o_n_s_t_a_n_t_B_i_a_s { │ │ │ │ │ +31private: │ │ │ │ │ +32 Vector3 biasAcc_; │ │ │ │ │ +33 Vector3 biasGyro_; │ │ │ │ │ +34 │ │ │ │ │ +35public: │ │ │ │ │ +_3_7 static const size_t dimension = 6; │ │ │ │ │ +38 │ │ │ │ │ +41 │ │ │ │ │ +42 _C_o_n_s_t_a_n_t_B_i_a_s() : │ │ │ │ │ +43 biasAcc_(0.0, 0.0, 0.0), biasGyro_(0.0, 0.0, 0.0) { │ │ │ │ │ +44 } │ │ │ │ │ +45 │ │ │ │ │ +46 _C_o_n_s_t_a_n_t_B_i_a_s(const Vector3& biasAcc, const Vector3& biasGyro) : │ │ │ │ │ +47 biasAcc_(biasAcc), biasGyro_(biasGyro) { │ │ │ │ │ +48 } │ │ │ │ │ 49 │ │ │ │ │ -50 _~_B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r() override {} │ │ │ │ │ -51 │ │ │ │ │ -_5_9 _B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r(_K_e_y key, _K_e_y baroKey, const double& baroIn, │ │ │ │ │ -60 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model) │ │ │ │ │ -61 : _B_a_s_e(model, key, baroKey), nT_(heightOut(baroIn)) {} │ │ │ │ │ +50 explicit ConstantBias(const Vector6& v) : │ │ │ │ │ +51 biasAcc_(v.head<3>()), biasGyro_(v.tail<3>()) { │ │ │ │ │ +52 } │ │ │ │ │ +53 │ │ │ │ │ +55 │ │ │ │ │ +_5_7 Vector6 _v_e_c_t_o_r() const { │ │ │ │ │ +58 Vector6 v; │ │ │ │ │ +59 v << biasAcc_, biasGyro_; │ │ │ │ │ +60 return v; │ │ │ │ │ +61 } │ │ │ │ │ 62 │ │ │ │ │ -_6_4 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -65 return boost::static_pointer_cast( │ │ │ │ │ -66 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); │ │ │ │ │ -67 } │ │ │ │ │ -68 │ │ │ │ │ -70 void _p_r_i_n_t( │ │ │ │ │ -71 const std::string& s = "", │ │ │ │ │ -72 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override; │ │ │ │ │ -73 │ │ │ │ │ -75 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& expected, │ │ │ │ │ -76 double tol = 1e-9) const override; │ │ │ │ │ -77 │ │ │ │ │ -79 Vector evaluateError( │ │ │ │ │ -80 const _P_o_s_e_3& p, const double& b, │ │ │ │ │ -81 boost::optional H = boost::none, │ │ │ │ │ -82 boost::optional H2 = boost::none) const override; │ │ │ │ │ -83 │ │ │ │ │ -84 inline const double& measurementIn() const { return nT_; } │ │ │ │ │ -85 │ │ │ │ │ -86 inline double heightOut(double n) const { │ │ │ │ │ -87 // From https://www.grc.nasa.gov/www/k-12/airplane/atmosmet.html │ │ │ │ │ -88 return (std::pow(n / 101.29, 1. / 5.256) * 288.08 - 273.1 - 15.04) / │ │ │ │ │ -89 -0.00649; │ │ │ │ │ -90 }; │ │ │ │ │ -91 │ │ │ │ │ -92 inline double baroOut(const double& meters) { │ │ │ │ │ -93 double temp = 15.04 - 0.00649 * meters; │ │ │ │ │ -94 return 101.29 * std::pow(((temp + 273.1) / 288.08), 5.256); │ │ │ │ │ -95 }; │ │ │ │ │ -96 │ │ │ │ │ -97 private: │ │ │ │ │ -_9_9 friend class boost::serialization::access; │ │ │ │ │ -100 template │ │ │ │ │ -101 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ -102 // NoiseModelFactor1 instead of NoiseModelFactorN for backward │ │ │ │ │ -compatibility │ │ │ │ │ -103 ar& boost::serialization::make_nvp( │ │ │ │ │ -104 "NoiseModelFactor1", │ │ │ │ │ -105 boost::serialization::base_object(*this)); │ │ │ │ │ -106 ar& BOOST_SERIALIZATION_NVP(nT_); │ │ │ │ │ -107 } │ │ │ │ │ -108}; │ │ │ │ │ -109 │ │ │ │ │ -110} // namespace gtsam │ │ │ │ │ -_P_o_s_e_3_._h │ │ │ │ │ -3D Pose │ │ │ │ │ -_N_a_v_S_t_a_t_e_._h │ │ │ │ │ -Navigation state composing of attitude, position, and velocity. │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ -Non-linear factor base classes. │ │ │ │ │ +_6_4 const Vector3& _a_c_c_e_l_e_r_o_m_e_t_e_r() const { │ │ │ │ │ +65 return biasAcc_; │ │ │ │ │ +66 } │ │ │ │ │ +67 │ │ │ │ │ +_6_9 const Vector3& _g_y_r_o_s_c_o_p_e() const { │ │ │ │ │ +70 return biasGyro_; │ │ │ │ │ +71 } │ │ │ │ │ +72 │ │ │ │ │ +_7_4 Vector3 _c_o_r_r_e_c_t_A_c_c_e_l_e_r_o_m_e_t_e_r(const Vector3& measurement, │ │ │ │ │ +75 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _6_> H1 = boost::none, │ │ │ │ │ +76 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H2 = boost::none) const { │ │ │ │ │ +77 if (H1) (*H1) << -I_3x3, Z_3x3; │ │ │ │ │ +78 if (H2) (*H2) << I_3x3; │ │ │ │ │ +79 return measurement - biasAcc_; │ │ │ │ │ +80 } │ │ │ │ │ +81 │ │ │ │ │ +_8_3 Vector3 _c_o_r_r_e_c_t_G_y_r_o_s_c_o_p_e(const Vector3& measurement, │ │ │ │ │ +84 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _6_> H1 = boost::none, │ │ │ │ │ +85 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H2 = boost::none) const { │ │ │ │ │ +86 if (H1) (*H1) << Z_3x3, -I_3x3; │ │ │ │ │ +87 if (H2) (*H2) << I_3x3; │ │ │ │ │ +88 return measurement - biasGyro_; │ │ │ │ │ +89 } │ │ │ │ │ +90 │ │ │ │ │ +93 │ │ │ │ │ +95 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, │ │ │ │ │ +96 const _C_o_n_s_t_a_n_t_B_i_a_s& bias); │ │ │ │ │ +97 │ │ │ │ │ +99 void _p_r_i_n_t(const std::string& s = "") const; │ │ │ │ │ +100 │ │ │ │ │ +_1_0_2 inline bool _e_q_u_a_l_s(const _C_o_n_s_t_a_n_t_B_i_a_s& expected, double tol = 1e-5) const { │ │ │ │ │ +103 return _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(biasAcc_, expected.biasAcc_, tol) │ │ │ │ │ +104 && _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(biasGyro_, expected.biasGyro_, tol); │ │ │ │ │ +105 } │ │ │ │ │ +106 │ │ │ │ │ +110 │ │ │ │ │ +_1_1_2 static _C_o_n_s_t_a_n_t_B_i_a_s _I_d_e_n_t_i_t_y() { │ │ │ │ │ +113 return _C_o_n_s_t_a_n_t_B_i_a_s(); │ │ │ │ │ +114 } │ │ │ │ │ +115 │ │ │ │ │ +_1_1_7 inline _C_o_n_s_t_a_n_t_B_i_a_s _o_p_e_r_a_t_o_r_-() const { │ │ │ │ │ +118 return _C_o_n_s_t_a_n_t_B_i_a_s(-biasAcc_, -biasGyro_); │ │ │ │ │ +119 } │ │ │ │ │ +120 │ │ │ │ │ +_1_2_2 _C_o_n_s_t_a_n_t_B_i_a_s _o_p_e_r_a_t_o_r_+(const Vector6& v) const { │ │ │ │ │ +123 return _C_o_n_s_t_a_n_t_B_i_a_s(biasAcc_ + v.head<3>(), biasGyro_ + v.tail<3>()); │ │ │ │ │ +124 } │ │ │ │ │ +125 │ │ │ │ │ +_1_2_7 _C_o_n_s_t_a_n_t_B_i_a_s _o_p_e_r_a_t_o_r_+(const _C_o_n_s_t_a_n_t_B_i_a_s& b) const { │ │ │ │ │ +128 return _C_o_n_s_t_a_n_t_B_i_a_s(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_); │ │ │ │ │ +129 } │ │ │ │ │ +130 │ │ │ │ │ +_1_3_2 _C_o_n_s_t_a_n_t_B_i_a_s _o_p_e_r_a_t_o_r_-(const _C_o_n_s_t_a_n_t_B_i_a_s& b) const { │ │ │ │ │ +133 return _C_o_n_s_t_a_n_t_B_i_a_s(biasAcc_ - b.biasAcc_, biasGyro_ - b.biasGyro_); │ │ │ │ │ +134 } │ │ │ │ │ +135 │ │ │ │ │ +137 │ │ │ │ │ +138#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ +141 _C_o_n_s_t_a_n_t_B_i_a_s GTSAM_DEPRECATED inverse() { return -(*this); } │ │ │ │ │ +142 ConstantBias GTSAM_DEPRECATED compose(const ConstantBias& q) { │ │ │ │ │ +143 return (*this) + q; │ │ │ │ │ +144 } │ │ │ │ │ +145 ConstantBias GTSAM_DEPRECATED between(const ConstantBias& q) { │ │ │ │ │ +146 return q - (*this); │ │ │ │ │ +147 } │ │ │ │ │ +148 Vector6 GTSAM_DEPRECATED localCoordinates(const ConstantBias& q) { │ │ │ │ │ +149 return (q - (*this)).vector(); │ │ │ │ │ +150 } │ │ │ │ │ +151 ConstantBias GTSAM_DEPRECATED retract(const Vector6& v) { │ │ │ │ │ +152 return (*this) + ConstantBias(v); │ │ │ │ │ +153 } │ │ │ │ │ +154 static Vector6 GTSAM_DEPRECATED Logmap(const ConstantBias& p) { │ │ │ │ │ +155 return p.vector(); │ │ │ │ │ +156 } │ │ │ │ │ +157 static ConstantBias GTSAM_DEPRECATED Expmap(const Vector6& v) { │ │ │ │ │ +158 return ConstantBias(v); │ │ │ │ │ +159 } │ │ │ │ │ +161#endif │ │ │ │ │ +162 │ │ │ │ │ +163private: │ │ │ │ │ +164 │ │ │ │ │ +167 │ │ │ │ │ +_1_6_9 friend class boost::serialization::access; │ │ │ │ │ +170 template │ │ │ │ │ +171 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +172 ar & BOOST_SERIALIZATION_NVP(biasAcc_); │ │ │ │ │ +173 ar & BOOST_SERIALIZATION_NVP(biasGyro_); │ │ │ │ │ +174 } │ │ │ │ │ +175 │ │ │ │ │ +176 │ │ │ │ │ +177public: │ │ │ │ │ +178 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +180 │ │ │ │ │ +181}; // ConstantBias class │ │ │ │ │ +182} // namespace imuBias │ │ │ │ │ +183 │ │ │ │ │ +184template<> │ │ │ │ │ +_1_8_5struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_V_e_c_t_o_r_S_p_a_c_e< │ │ │ │ │ +186 imuBias::ConstantBias> { │ │ │ │ │ +187}; │ │ │ │ │ +188 │ │ │ │ │ +189} // namespace gtsam │ │ │ │ │ +190 │ │ │ │ │ +_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h │ │ │ │ │ +Special class for optional Jacobian arguments. │ │ │ │ │ +_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ +This marks a GTSAM object to require alignment. │ │ │ │ │ +DDeeffiinniittiioonn types.h:308 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ print without optional string, must specify cout yourself │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ -Aliases. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ -A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r │ │ │ │ │ -Prior on height in a cartesian frame. │ │ │ │ │ -DDeeffiinniittiioonn BarometricFactor.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r_:_:_T_h_i_s │ │ │ │ │ -BarometricFactor This │ │ │ │ │ -Typedef to this class. │ │ │ │ │ -DDeeffiinniittiioonn BarometricFactor.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r_:_:_B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r │ │ │ │ │ -BarometricFactor(Key key, Key baroKey, const double &baroIn, const │ │ │ │ │ -SharedNoiseModel &model) │ │ │ │ │ -Constructor from a measurement of pressure in KPa. │ │ │ │ │ -DDeeffiinniittiioonn BarometricFactor.h:59 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< BarometricFactor > shared_ptr │ │ │ │ │ -shorthand for a smart pointer to a factor │ │ │ │ │ -DDeeffiinniittiioonn BarometricFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ -gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn BarometricFactor.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r_:_:_B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r │ │ │ │ │ -BarometricFactor() │ │ │ │ │ -default constructor - only use for serialization │ │ │ │ │ -DDeeffiinniittiioonn BarometricFactor.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ -Nonlinear factor base class. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ -A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ -variables. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l___w_i_t_h___a_b_s___t_o_l │ │ │ │ │ +bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen:: │ │ │ │ │ +DenseBase< MATRIX > &B, double tol=1e-9) │ │ │ │ │ +equals with a tolerance │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:81 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ +either a fixed size o... │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_V_e_c_t_o_r_S_p_a_c_e │ │ │ │ │ +VectorSpace provides both Testable and VectorSpaceTraits. │ │ │ │ │ +DDeeffiinniittiioonn VectorSpace.h:207 │ │ │ │ │ +_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s │ │ │ │ │ +DDeeffiinniittiioonn ImuBias.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_:_:_g_y_r_o_s_c_o_p_e │ │ │ │ │ +const Vector3 & gyroscope() const │ │ │ │ │ +get gyroscope bias │ │ │ │ │ +DDeeffiinniittiioonn ImuBias.h:69 │ │ │ │ │ +_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_:_:_I_d_e_n_t_i_t_y │ │ │ │ │ +static ConstantBias Identity() │ │ │ │ │ +identity for group operation │ │ │ │ │ +DDeeffiinniittiioonn ImuBias.h:112 │ │ │ │ │ +_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const ConstantBias &expected, double tol=1e-5) const │ │ │ │ │ +equality up to tolerance │ │ │ │ │ +DDeeffiinniittiioonn ImuBias.h:102 │ │ │ │ │ +_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_:_:_o_p_e_r_a_t_o_r_- │ │ │ │ │ +ConstantBias operator-(const ConstantBias &b) const │ │ │ │ │ +subtraction │ │ │ │ │ +DDeeffiinniittiioonn ImuBias.h:132 │ │ │ │ │ +_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_:_:_c_o_r_r_e_c_t_A_c_c_e_l_e_r_o_m_e_t_e_r │ │ │ │ │ +Vector3 correctAccelerometer(const Vector3 &measurement, OptionalJacobian< 3, 6 │ │ │ │ │ +> H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const │ │ │ │ │ +Correct an accelerometer measurement using this bias model, and optionally │ │ │ │ │ +compute Jacobians. │ │ │ │ │ +DDeeffiinniittiioonn ImuBias.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_:_:_o_p_e_r_a_t_o_r_- │ │ │ │ │ +ConstantBias operator-() const │ │ │ │ │ +inverse │ │ │ │ │ +DDeeffiinniittiioonn ImuBias.h:117 │ │ │ │ │ +_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_:_:_c_o_r_r_e_c_t_G_y_r_o_s_c_o_p_e │ │ │ │ │ +Vector3 correctGyroscope(const Vector3 &measurement, OptionalJacobian< 3, 6 > │ │ │ │ │ +H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const │ │ │ │ │ +Correct a gyroscope measurement using this bias model, and optionally compute │ │ │ │ │ +Jacobians. │ │ │ │ │ +DDeeffiinniittiioonn ImuBias.h:83 │ │ │ │ │ +_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_:_:_v_e_c_t_o_r │ │ │ │ │ +Vector6 vector() const │ │ │ │ │ +return the accelerometer and gyro biases in a single vector │ │ │ │ │ +DDeeffiinniittiioonn ImuBias.h:57 │ │ │ │ │ +_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_:_:_o_p_e_r_a_t_o_r_+ │ │ │ │ │ +ConstantBias operator+(const Vector6 &v) const │ │ │ │ │ +addition of vector on right │ │ │ │ │ +DDeeffiinniittiioonn ImuBias.h:122 │ │ │ │ │ +_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_:_:_o_p_e_r_a_t_o_r_+ │ │ │ │ │ +ConstantBias operator+(const ConstantBias &b) const │ │ │ │ │ +addition │ │ │ │ │ +DDeeffiinniittiioonn ImuBias.h:127 │ │ │ │ │ +_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_:_:_a_c_c_e_l_e_r_o_m_e_t_e_r │ │ │ │ │ +const Vector3 & accelerometer() const │ │ │ │ │ +get accelerometer bias │ │ │ │ │ +DDeeffiinniittiioonn ImuBias.h:64 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r_._h │ │ │ │ │ + * _I_m_u_B_i_a_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01001.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearOptimizerParams.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/LinearContainerFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,41 +95,31 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces
│ │ │ │ -
NonlinearOptimizerParams.cpp File Reference
│ │ │ │ +
LinearContainerFactor.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -

Parameters for nonlinear optimization. │ │ │ │ -More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Parameters for nonlinear optimization.

│ │ │ │ -
Date
Jul 24, 2012
│ │ │ │ -
Author
Yong-Dian Jian
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ -
│ │ │ │ -Andrew Melim
│ │ │ │ +
Date
Jul 6, 2012
│ │ │ │ +
Author
Alex Cunningham
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,25 +1,20 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -NonlinearOptimizerParams.cpp File Reference │ │ │ │ │ -Parameters for nonlinear optimization. _M_o_r_e_._._. │ │ │ │ │ +LinearContainerFactor.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Parameters for nonlinear optimization. │ │ │ │ │ Date │ │ │ │ │ - Jul 24, 2012 │ │ │ │ │ + Jul 6, 2012 │ │ │ │ │ Author │ │ │ │ │ - Yong-Dian Jian │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Andrew Melim │ │ │ │ │ + Alex Cunningham │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_._c_p_p │ │ │ │ │ + * _L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01004.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Values.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/nonlinearExceptions.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,66 +96,43 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
Values.h File Reference
│ │ │ │ +
nonlinearExceptions.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

A non-templated config holding any types of Manifold-group elements. │ │ │ │ +

Exceptions that may be thrown by nonlinear optimization components. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::ValueCloneAllocator
 
class  gtsam::Values
 A non-templated config holding any types of Manifold-group elements. More...
 
struct  gtsam::Values::KeyValuePair
 A key-value pair, which you get by dereferencing iterators. More...
 
struct  gtsam::Values::ConstKeyValuePair
 A key-value pair, which you get by dereferencing iterators. More...
 
struct  gtsam::Values::deref_iterator
 
class  gtsam::ValuesKeyAlreadyExists
 
class  gtsam::ValuesKeyDoesNotExist
 
class  gtsam::ValuesIncorrectType
 
class  gtsam::DynamicValuesMismatched
 
class  gtsam::NoMatchFoundForFixed
 
struct  gtsam::traits< Values >
 traits More...
class  gtsam::MarginalizeNonleafException
 Thrown when requesting to marginalize out variables from ISAM2 that are not leaves. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

A non-templated config holding any types of Manifold-group elements.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -

Detailed story: A values structure is a map from keys to values. It is used to specify the value of a bunch of variables in a factor graph. A Values is a values structure which can hold variables that are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type which is also a manifold element, and hence supports operations dim, retract, and localCoordinates.

│ │ │ │ +

Exceptions that may be thrown by nonlinear optimization components.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Aug 17, 2012
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,55 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Values.h File Reference │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. _M_o_r_e_._._. │ │ │ │ │ +nonlinearExceptions.h File Reference │ │ │ │ │ +Exceptions that may be thrown by nonlinear optimization components. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_V_a_l_u_e_C_l_o_n_e_A_l_l_o_c_a_t_o_r │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ -  A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_K_e_y_V_a_l_u_e_P_a_i_r │ │ │ │ │ -  A key-value pair, which you get by dereferencing iterators. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r │ │ │ │ │ -  A key-value pair, which you get by dereferencing iterators. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_d_e_r_e_f___i_t_e_r_a_t_o_r │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_V_a_l_u_e_s_K_e_y_A_l_r_e_a_d_y_E_x_i_s_t_s │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_V_a_l_u_e_s_K_e_y_D_o_e_s_N_o_t_E_x_i_s_t │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_D_y_n_a_m_i_c_V_a_l_u_e_s_M_i_s_m_a_t_c_h_e_d │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_N_o_M_a_t_c_h_F_o_u_n_d_F_o_r_F_i_x_e_d │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _V_a_l_u_e_s_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_M_a_r_g_i_n_a_l_i_z_e_N_o_n_l_e_a_f_E_x_c_e_p_t_i_o_n │ │ │ │ │ +  Thrown when requesting to marginalize out variables from _I_S_A_M_2 that are │ │ │ │ │ + not leaves. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +Exceptions that may be thrown by nonlinear optimization components. │ │ │ │ │ Author │ │ │ │ │ Richard Roberts │ │ │ │ │ -Detailed story: A values structure is a map from keys to values. It is used to │ │ │ │ │ -specify the value of a bunch of variables in a factor graph. A _V_a_l_u_e_s is a │ │ │ │ │ -values structure which can hold variables that are elements on manifolds, not │ │ │ │ │ -just vectors. It then, as a whole, implements a aggregate type which is also a │ │ │ │ │ -manifold element, and hence supports operations dim, retract, and │ │ │ │ │ -localCoordinates. │ │ │ │ │ + Date │ │ │ │ │ + Aug 17, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _V_a_l_u_e_s_._h │ │ │ │ │ + * _n_o_n_l_i_n_e_a_r_E_x_c_e_p_t_i_o_n_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01004.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,13 +1,3 @@ │ │ │ │ │ var a01004 = [ │ │ │ │ │ - ["gtsam::ValueCloneAllocator", "a04608.html", null], │ │ │ │ │ - ["gtsam::Values", "a04612.html", "a04612"], │ │ │ │ │ - ["gtsam::Values::KeyValuePair", "a04616.html", "a04616"], │ │ │ │ │ - ["gtsam::Values::ConstKeyValuePair", "a04620.html", "a04620"], │ │ │ │ │ - ["gtsam::Values::deref_iterator", "a04624.html", null], │ │ │ │ │ - ["gtsam::ValuesKeyAlreadyExists", "a04628.html", "a04628"], │ │ │ │ │ - ["gtsam::ValuesKeyDoesNotExist", "a04632.html", "a04632"], │ │ │ │ │ - ["gtsam::ValuesIncorrectType", "a04636.html", "a04636"], │ │ │ │ │ - ["gtsam::DynamicValuesMismatched", "a04640.html", null], │ │ │ │ │ - ["gtsam::NoMatchFoundForFixed", "a04644.html", null], │ │ │ │ │ - ["gtsam::traits< Values >", "a04648.html", null] │ │ │ │ │ + ["gtsam::MarginalizeNonleafException", "a04492.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01004_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Values.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/nonlinearExceptions.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,513 +98,69 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Values.h
│ │ │ │ +
nonlinearExceptions.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
25#pragma once
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <boost/lexical_cast.hpp>
│ │ │ │ +
21#include <exception>
│ │ │ │ +
22
│ │ │ │ +
23#include <gtsam/inference/Key.h>
│ │ │ │ +
24
│ │ │ │ +
25namespace gtsam {
│ │ │ │
26
│ │ │ │ -
27#include <gtsam/base/FastDefaultAllocator.h>
│ │ │ │ -
28#include <gtsam/base/GenericValue.h>
│ │ │ │ -
29#include <gtsam/base/VectorSpace.h>
│ │ │ │ -
30#include <gtsam/inference/Key.h>
│ │ │ │ -
31#include <boost/ptr_container/serialize_ptr_map.hpp>
│ │ │ │ -
32#include <boost/shared_ptr.hpp>
│ │ │ │ -
33#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ -
34#include <boost/iterator/transform_iterator.hpp>
│ │ │ │ -
35#include <boost/iterator/filter_iterator.hpp>
│ │ │ │ -
36#endif
│ │ │ │ -
37
│ │ │ │ -
38#include <string>
│ │ │ │ -
39#include <utility>
│ │ │ │ -
40
│ │ │ │ -
41namespace gtsam {
│ │ │ │ -
42
│ │ │ │ -
43 // Forward declarations / utilities
│ │ │ │ -
44 class VectorValues;
│ │ │ │ -
45 class ValueAutomaticCasting;
│ │ │ │ -
46 template<typename T> static bool _truePredicate(const T&) { return true; }
│ │ │ │ -
47
│ │ │ │ -
48 /* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
49 class GTSAM_EXPORT ValueCloneAllocator {
│ │ │ │ -
50 public:
│ │ │ │ -
51 static Value* allocate_clone(const Value& a) { return a.clone_(); }
│ │ │ │ -
52 static void deallocate_clone(const Value* a) { a->deallocate_(); }
│ │ │ │ -
53 ValueCloneAllocator() {}
│ │ │ │ -
54 };
│ │ │ │ -
│ │ │ │ -
55
│ │ │ │ -
│ │ │ │ -
65 class GTSAM_EXPORT Values {
│ │ │ │ -
66
│ │ │ │ -
67 private:
│ │ │ │ -
68 // Internally we store a boost ptr_map, with a ValueCloneAllocator (defined
│ │ │ │ -
69 // below) to clone and deallocate the Value objects, and our compile-flag-
│ │ │ │ -
70 // dependent FastDefaultAllocator to allocate map nodes. In this way, the
│ │ │ │ -
71 // user defines the allocation details (i.e. optimize for memory pool/arenas
│ │ │ │ -
72 // concurrency).
│ │ │ │ -
73 typedef internal::FastDefaultAllocator<typename std::pair<const Key, void*>>::type KeyValuePtrPairAllocator;
│ │ │ │ -
74 typedef boost::ptr_map<
│ │ │ │ -
75 Key,
│ │ │ │ -
76 Value,
│ │ │ │ -
77 std::less<Key>,
│ │ │ │ -
78 ValueCloneAllocator,
│ │ │ │ -
79 KeyValuePtrPairAllocator > KeyValueMap;
│ │ │ │ -
80
│ │ │ │ -
81 // The member to store the values, see just above
│ │ │ │ -
82 KeyValueMap values_;
│ │ │ │ -
83
│ │ │ │ -
84 public:
│ │ │ │ -
85
│ │ │ │ -
87 typedef boost::shared_ptr<Values> shared_ptr;
│ │ │ │ -
88
│ │ │ │ -
90 typedef boost::shared_ptr<const Values> const_shared_ptr;
│ │ │ │ -
91
│ │ │ │ -
│ │ │ │ -
93 struct GTSAM_EXPORT KeyValuePair {
│ │ │ │ -
94 const Key key;
│ │ │ │ -
95 Value& value;
│ │ │ │ -
96
│ │ │ │ -
97 KeyValuePair(Key _key, Value& _value) : key(_key), value(_value) {}
│ │ │ │ -
98 };
│ │ │ │ -
│ │ │ │ -
99
│ │ │ │ -
│ │ │ │ -
101 struct GTSAM_EXPORT ConstKeyValuePair {
│ │ │ │ -
102 const Key key;
│ │ │ │ -
103 const Value& value;
│ │ │ │ -
104
│ │ │ │ -
105 ConstKeyValuePair(Key _key, const Value& _value) : key(_key), value(_value) {}
│ │ │ │ -
106 ConstKeyValuePair(const KeyValuePair& kv) : key(kv.key), value(kv.value) {}
│ │ │ │ -
107 };
│ │ │ │ -
│ │ │ │ -
108
│ │ │ │ -
109 typedef KeyValuePair value_type;
│ │ │ │ -
110
│ │ │ │ -
113
│ │ │ │ -
115 Values() = default;
│ │ │ │ -
116
│ │ │ │ -
118 Values(const Values& other);
│ │ │ │ -
119
│ │ │ │ -
121 Values(Values&& other);
│ │ │ │ -
122
│ │ │ │ -
128 Values(std::initializer_list<ConstKeyValuePair> init);
│ │ │ │ -
129
│ │ │ │ -
131 Values(const Values& other, const VectorValues& delta);
│ │ │ │ -
132
│ │ │ │ -
136
│ │ │ │ -
138 void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ -
139
│ │ │ │ -
141 bool equals(const Values& other, double tol=1e-9) const;
│ │ │ │ -
142
│ │ │ │ -
146
│ │ │ │ -
155 template <typename ValueType>
│ │ │ │ -
156 const ValueType at(Key j) const;
│ │ │ │ -
157
│ │ │ │ -
159 double atDouble(size_t key) const { return at<double>(key);}
│ │ │ │ -
160
│ │ │ │ -
166 const Value& at(Key j) const;
│ │ │ │ -
167
│ │ │ │ -
171 bool exists(Key j) const;
│ │ │ │ -
172
│ │ │ │ -
177 template<typename ValueType>
│ │ │ │ -
178 boost::optional<const ValueType&> exists(Key j) const;
│ │ │ │ -
179
│ │ │ │ -
181 size_t size() const { return values_.size(); }
│ │ │ │ -
182
│ │ │ │ -
184 bool empty() const { return values_.empty(); }
│ │ │ │ -
185
│ │ │ │ -
189
│ │ │ │ -
│ │ │ │ -
190 struct deref_iterator {
│ │ │ │ -
191 using const_iterator_type = typename KeyValueMap::const_iterator;
│ │ │ │ -
192 const_iterator_type it_;
│ │ │ │ -
193 deref_iterator(const_iterator_type it) : it_(it) {}
│ │ │ │ -
194 ConstKeyValuePair operator*() const { return {it_->first, *(it_->second)}; }
│ │ │ │ -
195 boost::shared_ptr<ConstKeyValuePair> operator->() {
│ │ │ │ -
196 return boost::make_shared<ConstKeyValuePair>(it_->first, *(it_->second));
│ │ │ │ -
197 }
│ │ │ │ -
198 bool operator==(const deref_iterator& other) const {
│ │ │ │ -
199 return it_ == other.it_;
│ │ │ │ -
200 }
│ │ │ │ -
201 bool operator!=(const deref_iterator& other) const { return it_ != other.it_; }
│ │ │ │ -
202 deref_iterator& operator++() {
│ │ │ │ -
203 ++it_;
│ │ │ │ -
204 return *this;
│ │ │ │ -
205 }
│ │ │ │ -
206 };
│ │ │ │ -
│ │ │ │ -
207
│ │ │ │ -
208 deref_iterator begin() const { return deref_iterator(values_.begin()); }
│ │ │ │ -
209 deref_iterator end() const { return deref_iterator(values_.end()); }
│ │ │ │ -
210
│ │ │ │ -
213 deref_iterator find(Key j) const { return deref_iterator(values_.find(j)); }
│ │ │ │ -
214
│ │ │ │ -
216 deref_iterator lower_bound(Key j) const { return deref_iterator(values_.lower_bound(j)); }
│ │ │ │ -
217
│ │ │ │ -
219 deref_iterator upper_bound(Key j) const { return deref_iterator(values_.upper_bound(j)); }
│ │ │ │ -
220
│ │ │ │ -
224
│ │ │ │ -
226 Values retract(const VectorValues& delta) const;
│ │ │ │ -
227
│ │ │ │ -
232 void retractMasked(const VectorValues& delta, const KeySet& mask);
│ │ │ │ -
233
│ │ │ │ -
235 VectorValues localCoordinates(const Values& cp) const;
│ │ │ │ -
236
│ │ │ │ -
238
│ │ │ │ -
240 void insert(Key j, const Value& val);
│ │ │ │ -
241
│ │ │ │ -
243 void insert(const Values& values);
│ │ │ │ -
244
│ │ │ │ -
248 template <typename ValueType>
│ │ │ │ -
249 void insert(Key j, const ValueType& val);
│ │ │ │ -
250
│ │ │ │ -
252 void insertDouble(Key j, double c) { insert<double>(j,c); }
│ │ │ │ -
253
│ │ │ │ -
255 void update(Key j, const Value& val);
│ │ │ │ -
256
│ │ │ │ -
261 template <typename T>
│ │ │ │ -
262 void update(Key j, const T& val);
│ │ │ │ -
263
│ │ │ │ -
265 void update(const Values& values);
│ │ │ │ -
266
│ │ │ │ -
268 void insert_or_assign(Key j, const Value& val);
│ │ │ │ -
269
│ │ │ │ -
274 void insert_or_assign(const Values& values);
│ │ │ │ -
275
│ │ │ │ -
277 template <typename ValueType>
│ │ │ │ -
278 void insert_or_assign(Key j, const ValueType& val);
│ │ │ │ -
279
│ │ │ │ -
281 void erase(Key j);
│ │ │ │ -
282
│ │ │ │ -
287 KeyVector keys() const;
│ │ │ │ -
288
│ │ │ │ -
292 KeySet keySet() const;
│ │ │ │ -
293
│ │ │ │ -
295 Values& operator=(const Values& rhs);
│ │ │ │ -
296
│ │ │ │ -
298 void swap(Values& other) { values_.swap(other.values_); }
│ │ │ │ -
299
│ │ │ │ -
301 void clear() { values_.clear(); }
│ │ │ │ -
302
│ │ │ │ -
304 size_t dim() const;
│ │ │ │ -
305
│ │ │ │ -
307 std::map<Key,size_t> dims() const;
│ │ │ │ -
308
│ │ │ │ -
310 VectorValues zeroVectors() const;
│ │ │ │ -
311
│ │ │ │ -
312 // Count values of given type \c ValueType
│ │ │ │ -
313 template<class ValueType>
│ │ │ │ -
314 size_t count() const {
│ │ │ │ -
315 size_t i = 0;
│ │ │ │ -
316 for (const auto key_value : values_) {
│ │ │ │ -
317 if (dynamic_cast<const GenericValue<ValueType>*>(key_value.second))
│ │ │ │ -
318 ++i;
│ │ │ │ -
319 }
│ │ │ │ -
320 return i;
│ │ │ │ -
321 }
│ │ │ │ -
322
│ │ │ │ -
342 template <class ValueType>
│ │ │ │ -
343 std::map<Key, ValueType> // , std::less<Key>, Eigen::aligned_allocator<ValueType>
│ │ │ │ -
344 extract(const std::function<bool(Key)>& filterFcn = &_truePredicate<Key>) const;
│ │ │ │ -
345
│ │ │ │ -
346#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ -
347 // Types obtained by iterating
│ │ │ │ -
348 typedef KeyValueMap::const_iterator::value_type ConstKeyValuePtrPair;
│ │ │ │ -
349 typedef KeyValueMap::iterator::value_type KeyValuePtrPair;
│ │ │ │ -
350
│ │ │ │ -
352 typedef boost::transform_iterator<
│ │ │ │ -
353 std::function<KeyValuePair(const KeyValuePtrPair&)>, KeyValueMap::iterator> iterator;
│ │ │ │ -
354
│ │ │ │ -
356 typedef boost::transform_iterator<
│ │ │ │ -
357 std::function<ConstKeyValuePair(const ConstKeyValuePtrPair&)>, KeyValueMap::const_iterator> const_iterator;
│ │ │ │ -
358
│ │ │ │ -
360 typedef boost::transform_iterator<
│ │ │ │ -
361 std::function<KeyValuePair(const KeyValuePtrPair&)>, KeyValueMap::reverse_iterator> reverse_iterator;
│ │ │ │ -
362
│ │ │ │ -
364 typedef boost::transform_iterator<
│ │ │ │ -
365 std::function<ConstKeyValuePair(const ConstKeyValuePtrPair&)>, KeyValueMap::const_reverse_iterator> const_reverse_iterator;
│ │ │ │ -
366
│ │ │ │ -
371 std::pair<iterator, bool> tryInsert(Key j, const Value& value);
│ │ │ │ -
372
│ │ │ │ -
373 static ConstKeyValuePair make_const_deref_pair(const KeyValueMap::const_iterator::value_type& key_value) {
│ │ │ │ -
374 return ConstKeyValuePair(key_value.first, *key_value.second); }
│ │ │ │ -
375
│ │ │ │ -
376 static KeyValuePair make_deref_pair(const KeyValueMap::iterator::value_type& key_value) {
│ │ │ │ -
377 return KeyValuePair(key_value.first, *key_value.second); }
│ │ │ │ -
378
│ │ │ │ -
379 const_iterator _begin() const { return boost::make_transform_iterator(values_.begin(), &make_const_deref_pair); }
│ │ │ │ -
380 const_iterator _end() const { return boost::make_transform_iterator(values_.end(), &make_const_deref_pair); }
│ │ │ │ -
381 iterator begin() { return boost::make_transform_iterator(values_.begin(), &make_deref_pair); }
│ │ │ │ -
382 iterator end() { return boost::make_transform_iterator(values_.end(), &make_deref_pair); }
│ │ │ │ -
383 const_reverse_iterator rbegin() const { return boost::make_transform_iterator(values_.rbegin(), &make_const_deref_pair); }
│ │ │ │ -
384 const_reverse_iterator rend() const { return boost::make_transform_iterator(values_.rend(), &make_const_deref_pair); }
│ │ │ │ -
385 reverse_iterator rbegin() { return boost::make_transform_iterator(values_.rbegin(), &make_deref_pair); }
│ │ │ │ -
386 reverse_iterator rend() { return boost::make_transform_iterator(values_.rend(), &make_deref_pair); }
│ │ │ │ -
387
│ │ │ │ -
390 iterator find(Key j) { return boost::make_transform_iterator(values_.find(j), &make_deref_pair); }
│ │ │ │ -
391
│ │ │ │ -
393 iterator lower_bound(Key j) { return boost::make_transform_iterator(values_.lower_bound(j), &make_deref_pair); }
│ │ │ │ -
394
│ │ │ │ -
396 iterator upper_bound(Key j) { return boost::make_transform_iterator(values_.upper_bound(j), &make_deref_pair); }
│ │ │ │ -
397
│ │ │ │ -
399 template <class ValueType = Value>
│ │ │ │ -
400 class Filtered;
│ │ │ │ -
401
│ │ │ │ -
403 template <class ValueType = Value>
│ │ │ │ -
404 class ConstFiltered;
│ │ │ │ -
405
│ │ │ │ -
407 template <class ValueType>
│ │ │ │ -
408 Values(const Filtered<ValueType>& view);
│ │ │ │ -
409
│ │ │ │ -
411 template <class ValueType>
│ │ │ │ -
412 Values(const ConstFiltered<ValueType>& view);
│ │ │ │ -
413
│ │ │ │ -
415 Filtered<Value> GTSAM_DEPRECATED
│ │ │ │ -
416 filter(const std::function<bool(Key)>& filterFcn);
│ │ │ │ -
417
│ │ │ │ -
419 template <class ValueType>
│ │ │ │ -
420 Filtered<ValueType> GTSAM_DEPRECATED
│ │ │ │ -
421 filter(const std::function<bool(Key)>& filterFcn = &_truePredicate<Key>);
│ │ │ │ -
422
│ │ │ │ -
424 ConstFiltered<Value> GTSAM_DEPRECATED
│ │ │ │ -
425 filter(const std::function<bool(Key)>& filterFcn) const;
│ │ │ │ -
426
│ │ │ │ -
428 template <class ValueType>
│ │ │ │ -
429 ConstFiltered<ValueType> GTSAM_DEPRECATED filter(
│ │ │ │ -
430 const std::function<bool(Key)>& filterFcn = &_truePredicate<Key>) const;
│ │ │ │ -
431#endif
│ │ │ │ -
432
│ │ │ │ -
433 private:
│ │ │ │ -
434 // Filters based on ValueType (if not Value) and also based on the user-
│ │ │ │ -
435 // supplied \c filter function.
│ │ │ │ -
436 template<class ValueType>
│ │ │ │ -
437 static bool filterHelper(const std::function<bool(Key)> filter, const ConstKeyValuePair& key_value) {
│ │ │ │ -
438 BOOST_STATIC_ASSERT((!boost::is_same<ValueType, Value>::value));
│ │ │ │ -
439 // Filter and check the type
│ │ │ │ -
440 return filter(key_value.key) && (dynamic_cast<const GenericValue<ValueType>*>(&key_value.value));
│ │ │ │ -
441 }
│ │ │ │ -
442
│ │ │ │ -
444 friend class boost::serialization::access;
│ │ │ │ -
445 template<class ARCHIVE>
│ │ │ │ -
446 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
447 ar & BOOST_SERIALIZATION_NVP(values_);
│ │ │ │ -
448 }
│ │ │ │ -
449
│ │ │ │ -
450 };
│ │ │ │ -
│ │ │ │ -
451
│ │ │ │ -
452 /* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
453 class ValuesKeyAlreadyExists : public std::exception {
│ │ │ │ -
454 protected:
│ │ │ │ -
455 const Key key_;
│ │ │ │ -
456
│ │ │ │ -
457 private:
│ │ │ │ -
458 mutable std::string message_;
│ │ │ │ -
459
│ │ │ │ -
460 public:
│ │ │ │ -
│ │ │ │ -
462 ValuesKeyAlreadyExists(Key key) noexcept :
│ │ │ │ -
463 key_(key) {}
│ │ │ │ -
│ │ │ │ -
464
│ │ │ │ -
465 ~ValuesKeyAlreadyExists() noexcept override {}
│ │ │ │ -
466
│ │ │ │ -
468 Key key() const noexcept { return key_; }
│ │ │ │ -
469
│ │ │ │ -
471 GTSAM_EXPORT const char* what() const noexcept override;
│ │ │ │ -
472 };
│ │ │ │ -
│ │ │ │ -
473
│ │ │ │ -
474 /* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
475 class ValuesKeyDoesNotExist : public std::exception {
│ │ │ │ -
476 protected:
│ │ │ │ -
477 const char* operation_;
│ │ │ │ -
478 const Key key_;
│ │ │ │ -
479
│ │ │ │ -
480 private:
│ │ │ │ -
481 mutable std::string message_;
│ │ │ │ -
482
│ │ │ │ -
483 public:
│ │ │ │ -
│ │ │ │ -
485 ValuesKeyDoesNotExist(const char* operation, Key key) noexcept :
│ │ │ │ -
486 operation_(operation), key_(key) {}
│ │ │ │ -
│ │ │ │ -
487
│ │ │ │ -
488 ~ValuesKeyDoesNotExist() noexcept override {}
│ │ │ │ -
489
│ │ │ │ -
491 Key key() const noexcept { return key_; }
│ │ │ │ -
492
│ │ │ │ -
494 GTSAM_EXPORT const char* what() const noexcept override;
│ │ │ │ -
495 };
│ │ │ │ -
│ │ │ │ -
496
│ │ │ │ -
497 /* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
498 class ValuesIncorrectType : public std::exception {
│ │ │ │ -
499 protected:
│ │ │ │ -
500 const Key key_;
│ │ │ │ -
501 const std::type_info& storedTypeId_;
│ │ │ │ -
502 const std::type_info& requestedTypeId_;
│ │ │ │ -
503
│ │ │ │ -
504 private:
│ │ │ │ -
505 mutable std::string message_;
│ │ │ │ -
506
│ │ │ │ -
507 public:
│ │ │ │ -
│ │ │ │ -
509 ValuesIncorrectType(Key key,
│ │ │ │ -
510 const std::type_info& storedTypeId, const std::type_info& requestedTypeId) noexcept :
│ │ │ │ -
511 key_(key), storedTypeId_(storedTypeId), requestedTypeId_(requestedTypeId) {}
│ │ │ │ -
│ │ │ │ -
512
│ │ │ │ -
513 ~ValuesIncorrectType() noexcept override {}
│ │ │ │ -
514
│ │ │ │ -
516 Key key() const noexcept { return key_; }
│ │ │ │ -
517
│ │ │ │ -
519 const std::type_info& storedTypeId() const { return storedTypeId_; }
│ │ │ │ -
520
│ │ │ │ -
522 const std::type_info& requestedTypeId() const { return requestedTypeId_; }
│ │ │ │ -
523
│ │ │ │ -
525 GTSAM_EXPORT const char* what() const noexcept override;
│ │ │ │ -
526 };
│ │ │ │ -
│ │ │ │ -
527
│ │ │ │ -
528 /* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
529 class DynamicValuesMismatched : public std::exception {
│ │ │ │ -
530
│ │ │ │ -
531 public:
│ │ │ │ -
532 DynamicValuesMismatched() noexcept {}
│ │ │ │ -
533
│ │ │ │ -
534 ~DynamicValuesMismatched() noexcept override {}
│ │ │ │ -
535
│ │ │ │ -
536 const char* what() const noexcept override {
│ │ │ │ -
537 return "The Values 'this' and the argument passed to Values::localCoordinates have mismatched keys and values";
│ │ │ │ -
538 }
│ │ │ │ -
539 };
│ │ │ │ -
│ │ │ │ -
540
│ │ │ │ -
541 /* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
542 class NoMatchFoundForFixed: public std::exception {
│ │ │ │ -
543
│ │ │ │ -
544 protected:
│ │ │ │ -
545 const size_t M1_, N1_;
│ │ │ │ -
546 const size_t M2_, N2_;
│ │ │ │ -
547
│ │ │ │ -
548 private:
│ │ │ │ -
549 mutable std::string message_;
│ │ │ │ -
550
│ │ │ │ -
551 public:
│ │ │ │ -
552 NoMatchFoundForFixed(size_t M1, size_t N1, size_t M2, size_t N2) noexcept :
│ │ │ │ -
553 M1_(M1), N1_(N1), M2_(M2), N2_(N2) {
│ │ │ │ -
554 }
│ │ │ │ -
555
│ │ │ │ -
556 ~NoMatchFoundForFixed() noexcept override {
│ │ │ │ -
557 }
│ │ │ │ -
558
│ │ │ │ -
559 GTSAM_EXPORT const char* what() const noexcept override;
│ │ │ │ -
560 };
│ │ │ │ -
│ │ │ │ -
561
│ │ │ │ -
562 /* ************************************************************************* */
│ │ │ │ -
564 template<>
│ │ │ │ -
│ │ │ │ -
565 struct traits<Values> : public Testable<Values> {
│ │ │ │ -
566 };
│ │ │ │ -
│ │ │ │ -
567
│ │ │ │ -
568} //\ namespace gtsam
│ │ │ │ -
569
│ │ │ │ -
570
│ │ │ │ -
571#include <gtsam/nonlinear/Values-inl.h>
│ │ │ │ -
FastDefaultAllocator.h
An easy way to control which allocator is used for Fast* collections.
│ │ │ │ - │ │ │ │ +
│ │ │ │ +
33 class MarginalizeNonleafException : public std::exception {
│ │ │ │ +
34 Key key_;
│ │ │ │ +
35 KeyFormatter formatter_;
│ │ │ │ +
36 mutable std::string what_;
│ │ │ │ +
37 public:
│ │ │ │ +
38 MarginalizeNonleafException(Key key, KeyFormatter formatter = DefaultKeyFormatter) noexcept :
│ │ │ │ +
39 key_(key), formatter_(formatter) {}
│ │ │ │ +
40 virtual ~MarginalizeNonleafException() noexcept {}
│ │ │ │ +
41 Key key() const { return key_; }
│ │ │ │ +
42 const char* what() const noexcept override {
│ │ │ │ +
43 if(what_.empty())
│ │ │ │ +
44 what_ =
│ │ │ │ +
45"\nRequested to marginalize out variable " + formatter_(key_) + ", but this variable\n\
│ │ │ │ +
46is not a leaf. To make the variables you would like to marginalize be leaves,\n\
│ │ │ │ +
47their ordering should be constrained using the constrainedKeys argument to\n\
│ │ │ │ +
48ISAM2::update().\n";
│ │ │ │ +
49 return what_.c_str();
│ │ │ │ +
50 }
│ │ │ │ +
51 };
│ │ │ │ +
│ │ │ │ +
52
│ │ │ │ +
53}
│ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
│ │ │ │ -
gtsam::operator!=
bool operator!=(const Matrix &A, const Matrix &B)
inequality
Definition Matrix.h:107
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::operator==
bool operator==(const Matrix &A, const Matrix &B)
equality is just equal_with_abs_tol 1e-9
Definition Matrix.h:100
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::internal::FastDefaultAllocator
Default allocator for list, map, and set types.
Definition FastDefaultAllocator.h:50
│ │ │ │ - │ │ │ │ -
gtsam::GenericValue
Wraps any type T so it can play as a Value.
Definition GenericValue.h:47
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::Value
This is the base class for any type to be stored in Values.
Definition Value.h:37
│ │ │ │ -
gtsam::Value::deallocate_
virtual void deallocate_() const =0
Deallocate a raw pointer of this value.
│ │ │ │ -
gtsam::Value::clone_
virtual Value * clone_() const =0
Clone this value in a special memory pool, must be deleted with Value::deallocate_,...
│ │ │ │ -
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ -
gtsam::ValueCloneAllocator
Definition Values.h:49
│ │ │ │ -
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
gtsam::Values::update
void update(Key j, const T &val)
Templated version to update a variable with the given j, throws KeyDoesNotExist<J> if j is not presen...
│ │ │ │ -
gtsam::Values::empty
bool empty() const
whether the config is empty
Definition Values.h:184
│ │ │ │ -
gtsam::Values::upper_bound
deref_iterator upper_bound(Key j) const
Find the lowest-ordered element greater than the specified key.
Definition Values.h:219
│ │ │ │ -
gtsam::Values::shared_ptr
boost::shared_ptr< Values > shared_ptr
A shared_ptr to this class.
Definition Values.h:87
│ │ │ │ -
gtsam::Values::clear
void clear()
Remove all variables from the config.
Definition Values.h:301
│ │ │ │ -
gtsam::Values::swap
void swap(Values &other)
Swap the contents of two Values without copying data.
Definition Values.h:298
│ │ │ │ -
gtsam::Values::insertDouble
void insertDouble(Key j, double c)
version for double
Definition Values.h:252
│ │ │ │ -
gtsam::Values::size
size_t size() const
The number of variables in this config.
Definition Values.h:181
│ │ │ │ -
gtsam::Values::find
deref_iterator find(Key j) const
Find an element by key, returning an iterator, or end() if the key was not found.
Definition Values.h:213
│ │ │ │ -
gtsam::Values::const_shared_ptr
boost::shared_ptr< const Values > const_shared_ptr
A const shared_ptr to this class.
Definition Values.h:90
│ │ │ │ -
gtsam::Values::Values
Values()=default
Default constructor creates an empty Values class.
│ │ │ │ -
gtsam::Values::lower_bound
deref_iterator lower_bound(Key j) const
Find the element greater than or equal to the specified key.
Definition Values.h:216
│ │ │ │ -
gtsam::Values::atDouble
double atDouble(size_t key) const
version for double
Definition Values.h:159
│ │ │ │ -
gtsam::Values::KeyValuePair
A key-value pair, which you get by dereferencing iterators.
Definition Values.h:93
│ │ │ │ -
gtsam::Values::KeyValuePair::value
Value & value
The value.
Definition Values.h:95
│ │ │ │ -
gtsam::Values::KeyValuePair::key
const Key key
The key.
Definition Values.h:94
│ │ │ │ -
gtsam::Values::ConstKeyValuePair
A key-value pair, which you get by dereferencing iterators.
Definition Values.h:101
│ │ │ │ -
gtsam::Values::ConstKeyValuePair::key
const Key key
The key.
Definition Values.h:102
│ │ │ │ -
gtsam::Values::ConstKeyValuePair::value
const Value & value
The value.
Definition Values.h:103
│ │ │ │ -
gtsam::Values::deref_iterator
Definition Values.h:190
│ │ │ │ -
gtsam::ValuesKeyAlreadyExists
Definition Values.h:453
│ │ │ │ -
gtsam::ValuesKeyAlreadyExists::key_
const Key key_
The key that already existed.
Definition Values.h:455
│ │ │ │ -
gtsam::ValuesKeyAlreadyExists::what
GTSAM_EXPORT const char * what() const noexcept override
The message to be displayed to the user.
Definition Values.cpp:280
│ │ │ │ -
gtsam::ValuesKeyAlreadyExists::ValuesKeyAlreadyExists
ValuesKeyAlreadyExists(Key key) noexcept
Construct with the key-value pair attempted to be added.
Definition Values.h:462
│ │ │ │ -
gtsam::ValuesKeyAlreadyExists::key
Key key() const noexcept
The duplicate key that was attempted to be added.
Definition Values.h:468
│ │ │ │ -
gtsam::ValuesKeyDoesNotExist
Definition Values.h:475
│ │ │ │ -
gtsam::ValuesKeyDoesNotExist::ValuesKeyDoesNotExist
ValuesKeyDoesNotExist(const char *operation, Key key) noexcept
Construct with the key that does not exist in the values.
Definition Values.h:485
│ │ │ │ -
gtsam::ValuesKeyDoesNotExist::key_
const Key key_
The key that does not exist.
Definition Values.h:478
│ │ │ │ -
gtsam::ValuesKeyDoesNotExist::key
Key key() const noexcept
The key that was attempted to be accessed that does not exist.
Definition Values.h:491
│ │ │ │ -
gtsam::ValuesKeyDoesNotExist::operation_
const char * operation_
The operation that attempted to access the key.
Definition Values.h:477
│ │ │ │ -
gtsam::ValuesIncorrectType
Definition Values.h:498
│ │ │ │ -
gtsam::ValuesIncorrectType::storedTypeId
const std::type_info & storedTypeId() const
The typeid of the value stores in the Values.
Definition Values.h:519
│ │ │ │ -
gtsam::ValuesIncorrectType::key
Key key() const noexcept
The key that was attempted to be accessed that does not exist.
Definition Values.h:516
│ │ │ │ -
gtsam::ValuesIncorrectType::key_
const Key key_
The key requested.
Definition Values.h:500
│ │ │ │ -
gtsam::ValuesIncorrectType::ValuesIncorrectType
ValuesIncorrectType(Key key, const std::type_info &storedTypeId, const std::type_info &requestedTypeId) noexcept
Construct with the key that does not exist in the values.
Definition Values.h:509
│ │ │ │ -
gtsam::ValuesIncorrectType::requestedTypeId
const std::type_info & requestedTypeId() const
The requested typeid.
Definition Values.h:522
│ │ │ │ -
gtsam::DynamicValuesMismatched
Definition Values.h:529
│ │ │ │ -
gtsam::NoMatchFoundForFixed
Definition Values.h:542
│ │ │ │ -
VectorValues
The Factor::error simply extracts the.
│ │ │ │ -
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │ +
gtsam::MarginalizeNonleafException
Thrown when requesting to marginalize out variables from ISAM2 that are not leaves.
Definition nonlinearExceptions.h:33
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,668 +1,73 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Values.h │ │ │ │ │ +nonlinearExceptions.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -25#pragma once │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include │ │ │ │ │ +21#include │ │ │ │ │ +22 │ │ │ │ │ +23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ +24 │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ 26 │ │ │ │ │ -27#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_._h> │ │ │ │ │ -28#include │ │ │ │ │ -29#include │ │ │ │ │ -30#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ -31#include │ │ │ │ │ -32#include │ │ │ │ │ -33#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ -34#include │ │ │ │ │ -35#include │ │ │ │ │ -36#endif │ │ │ │ │ -37 │ │ │ │ │ -38#include │ │ │ │ │ -39#include │ │ │ │ │ -40 │ │ │ │ │ -41namespace _g_t_s_a_m { │ │ │ │ │ -42 │ │ │ │ │ -43 // Forward declarations / utilities │ │ │ │ │ -44 class _V_e_c_t_o_r_V_a_l_u_e_s; │ │ │ │ │ -45 class ValueAutomaticCasting; │ │ │ │ │ -46 template static bool _truePredicate(const T&) { return true; } │ │ │ │ │ -47 │ │ │ │ │ -48 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -_4_9 class GTSAM_EXPORT _V_a_l_u_e_C_l_o_n_e_A_l_l_o_c_a_t_o_r { │ │ │ │ │ -50 public: │ │ │ │ │ -51 static _V_a_l_u_e* allocate_clone(const _V_a_l_u_e& a) { return a._c_l_o_n_e__(); } │ │ │ │ │ -52 static void deallocate_clone(const _V_a_l_u_e* a) { a->_d_e_a_l_l_o_c_a_t_e__(); } │ │ │ │ │ -53 _V_a_l_u_e_C_l_o_n_e_A_l_l_o_c_a_t_o_r() {} │ │ │ │ │ -54 }; │ │ │ │ │ -55 │ │ │ │ │ -_6_5 class GTSAM_EXPORT _V_a_l_u_e_s { │ │ │ │ │ -66 │ │ │ │ │ -67 private: │ │ │ │ │ -68 // Internally we store a boost ptr_map, with a ValueCloneAllocator (defined │ │ │ │ │ -69 // below) to clone and deallocate the Value objects, and our compile-flag- │ │ │ │ │ -70 // dependent FastDefaultAllocator to allocate map nodes. In this way, the │ │ │ │ │ -71 // user defines the allocation details (i.e. optimize for memory pool/arenas │ │ │ │ │ -72 // concurrency). │ │ │ │ │ -73 typedef _i_n_t_e_r_n_a_l_:_:_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_<_t_y_p_e_n_a_m_e_ _s_t_d_:_:_p_a_i_r_<_c_o_n_s_t_ _K_e_y_, │ │ │ │ │ -_v_o_i_d_*_>>::type KeyValuePtrPairAllocator; │ │ │ │ │ -74 typedef boost::ptr_map< │ │ │ │ │ -75 _K_e_y, │ │ │ │ │ -76 _V_a_l_u_e, │ │ │ │ │ -77 std::less, │ │ │ │ │ -78 _V_a_l_u_e_C_l_o_n_e_A_l_l_o_c_a_t_o_r, │ │ │ │ │ -79 KeyValuePtrPairAllocator > KeyValueMap; │ │ │ │ │ -80 │ │ │ │ │ -81 // The member to store the values, see just above │ │ │ │ │ -82 KeyValueMap values_; │ │ │ │ │ -83 │ │ │ │ │ -84 public: │ │ │ │ │ -85 │ │ │ │ │ -_8_7 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -88 │ │ │ │ │ -_9_0 typedef boost::shared_ptr _c_o_n_s_t___s_h_a_r_e_d___p_t_r; │ │ │ │ │ -91 │ │ │ │ │ -_9_3 struct GTSAM_EXPORT _K_e_y_V_a_l_u_e_P_a_i_r { │ │ │ │ │ -_9_4 const _K_e_y _k_e_y; │ │ │ │ │ -_9_5 _V_a_l_u_e& _v_a_l_u_e; │ │ │ │ │ -96 │ │ │ │ │ -97 _K_e_y_V_a_l_u_e_P_a_i_r(_K_e_y _key, _V_a_l_u_e& _value) : key(_key), value(_value) {} │ │ │ │ │ -98 }; │ │ │ │ │ -99 │ │ │ │ │ -_1_0_1 struct GTSAM_EXPORT _C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r { │ │ │ │ │ -_1_0_2 const _K_e_y _k_e_y; │ │ │ │ │ -_1_0_3 const _V_a_l_u_e& _v_a_l_u_e; │ │ │ │ │ -104 │ │ │ │ │ -105 _C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r(_K_e_y _key, const _V_a_l_u_e& _value) : key(_key), value(_value) │ │ │ │ │ -{} │ │ │ │ │ -106 _C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r(const _K_e_y_V_a_l_u_e_P_a_i_r& kv) : key(kv.key), value(kv.value) {} │ │ │ │ │ -107 }; │ │ │ │ │ -108 │ │ │ │ │ -109 typedef KeyValuePair value_type; │ │ │ │ │ -110 │ │ │ │ │ -113 │ │ │ │ │ -_1_1_5 _V_a_l_u_e_s() = default; │ │ │ │ │ -116 │ │ │ │ │ -118 _V_a_l_u_e_s(const _V_a_l_u_e_s& other); │ │ │ │ │ -119 │ │ │ │ │ -121 _V_a_l_u_e_s(_V_a_l_u_e_s&& other); │ │ │ │ │ -122 │ │ │ │ │ -128 _V_a_l_u_e_s(std::initializer_list init); │ │ │ │ │ -129 │ │ │ │ │ -131 _V_a_l_u_e_s(const _V_a_l_u_e_s& other, const _V_e_c_t_o_r_V_a_l_u_e_s& delta); │ │ │ │ │ -132 │ │ │ │ │ -136 │ │ │ │ │ -138 void _p_r_i_n_t(const std::string& str = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ -DefaultKeyFormatter) const; │ │ │ │ │ -139 │ │ │ │ │ -141 bool _e_q_u_a_l_s(const _V_a_l_u_e_s& other, double tol=1e-9) const; │ │ │ │ │ -142 │ │ │ │ │ -146 │ │ │ │ │ -155 template │ │ │ │ │ -156 const ValueType at(_K_e_y j) const; │ │ │ │ │ -157 │ │ │ │ │ -_1_5_9 double _a_t_D_o_u_b_l_e(size_t key) const { return at(key);} │ │ │ │ │ -160 │ │ │ │ │ -166 const _V_a_l_u_e& at(_K_e_y j) const; │ │ │ │ │ -167 │ │ │ │ │ -171 bool exists(_K_e_y j) const; │ │ │ │ │ -172 │ │ │ │ │ -177 template │ │ │ │ │ -178 boost::optional exists(_K_e_y j) const; │ │ │ │ │ -179 │ │ │ │ │ -_1_8_1 size_t _s_i_z_e() const { return values_.size(); } │ │ │ │ │ -182 │ │ │ │ │ -_1_8_4 bool _e_m_p_t_y() const { return values_.empty(); } │ │ │ │ │ -185 │ │ │ │ │ -189 │ │ │ │ │ -_1_9_0 struct _d_e_r_e_f___i_t_e_r_a_t_o_r { │ │ │ │ │ -191 using const_iterator_type = typename KeyValueMap::const_iterator; │ │ │ │ │ -192 const_iterator_type it_; │ │ │ │ │ -193 _d_e_r_e_f___i_t_e_r_a_t_o_r(const_iterator_type it) : it_(it) {} │ │ │ │ │ -194 _C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r _o_p_e_r_a_t_o_r_*() const { return {it_->first, *(it_->second)}; │ │ │ │ │ -} │ │ │ │ │ -195 boost::shared_ptr operator->() { │ │ │ │ │ -196 return boost::make_shared(it_->first, *(it_->second)); │ │ │ │ │ -197 } │ │ │ │ │ -198 bool _o_p_e_r_a_t_o_r_=_=(const _d_e_r_e_f___i_t_e_r_a_t_o_r& other) const { │ │ │ │ │ -199 return it_ == other.it_; │ │ │ │ │ -200 } │ │ │ │ │ -201 bool _o_p_e_r_a_t_o_r_!_=(const _d_e_r_e_f___i_t_e_r_a_t_o_r& other) const { return it_ != │ │ │ │ │ -other.it_; } │ │ │ │ │ -202 _d_e_r_e_f___i_t_e_r_a_t_o_r& operator++() { │ │ │ │ │ -203 ++it_; │ │ │ │ │ -204 return *this; │ │ │ │ │ -205 } │ │ │ │ │ -206 }; │ │ │ │ │ -207 │ │ │ │ │ -208 _d_e_r_e_f___i_t_e_r_a_t_o_r begin() const { return _d_e_r_e_f___i_t_e_r_a_t_o_r(values_.begin()); } │ │ │ │ │ -209 deref_iterator end() const { return deref_iterator(values_.end()); } │ │ │ │ │ -210 │ │ │ │ │ -_2_1_3 _d_e_r_e_f___i_t_e_r_a_t_o_r _f_i_n_d(_K_e_y j) const { return _d_e_r_e_f___i_t_e_r_a_t_o_r(values_.find(j)); │ │ │ │ │ -} │ │ │ │ │ -214 │ │ │ │ │ -_2_1_6 _d_e_r_e_f___i_t_e_r_a_t_o_r _l_o_w_e_r___b_o_u_n_d(_K_e_y j) const { return _d_e_r_e_f___i_t_e_r_a_t_o_r │ │ │ │ │ -(values_.lower_bound(j)); } │ │ │ │ │ -217 │ │ │ │ │ -_2_1_9 _d_e_r_e_f___i_t_e_r_a_t_o_r _u_p_p_e_r___b_o_u_n_d(_K_e_y j) const { return _d_e_r_e_f___i_t_e_r_a_t_o_r │ │ │ │ │ -(values_.upper_bound(j)); } │ │ │ │ │ -220 │ │ │ │ │ -224 │ │ │ │ │ -226 _V_a_l_u_e_s retract(const _V_e_c_t_o_r_V_a_l_u_e_s& delta) const; │ │ │ │ │ -227 │ │ │ │ │ -232 void retractMasked(const _V_e_c_t_o_r_V_a_l_u_e_s& delta, const _K_e_y_S_e_t& mask); │ │ │ │ │ -233 │ │ │ │ │ -235 _V_e_c_t_o_r_V_a_l_u_e_s localCoordinates(const _V_a_l_u_e_s& cp) const; │ │ │ │ │ -236 │ │ │ │ │ -238 │ │ │ │ │ -240 void insert(_K_e_y j, const _V_a_l_u_e& val); │ │ │ │ │ -241 │ │ │ │ │ -243 void insert(const _V_a_l_u_e_s& values); │ │ │ │ │ -244 │ │ │ │ │ -248 template │ │ │ │ │ -249 void insert(_K_e_y j, const ValueType& val); │ │ │ │ │ -250 │ │ │ │ │ -_2_5_2 void _i_n_s_e_r_t_D_o_u_b_l_e(_K_e_y j, double c) { insert(j,c); } │ │ │ │ │ -253 │ │ │ │ │ -255 void update(_K_e_y j, const _V_a_l_u_e& val); │ │ │ │ │ -256 │ │ │ │ │ -261 template │ │ │ │ │ -_2_6_2 void _u_p_d_a_t_e(_K_e_y j, const T& val); │ │ │ │ │ -263 │ │ │ │ │ -265 void update(const _V_a_l_u_e_s& values); │ │ │ │ │ -266 │ │ │ │ │ -268 void insert_or_assign(_K_e_y j, const _V_a_l_u_e& val); │ │ │ │ │ -269 │ │ │ │ │ -274 void insert_or_assign(const _V_a_l_u_e_s& values); │ │ │ │ │ -275 │ │ │ │ │ -277 template │ │ │ │ │ -278 void insert_or_assign(_K_e_y j, const ValueType& val); │ │ │ │ │ -279 │ │ │ │ │ -281 void erase(_K_e_y j); │ │ │ │ │ -282 │ │ │ │ │ -287 _K_e_y_V_e_c_t_o_r keys() const; │ │ │ │ │ -288 │ │ │ │ │ -292 _K_e_y_S_e_t keySet() const; │ │ │ │ │ -293 │ │ │ │ │ -295 _V_a_l_u_e_s& operator=(const _V_a_l_u_e_s& rhs); │ │ │ │ │ -296 │ │ │ │ │ -_2_9_8 void _s_w_a_p(_V_a_l_u_e_s& other) { values_.swap(other.values_); } │ │ │ │ │ -299 │ │ │ │ │ -_3_0_1 void _c_l_e_a_r() { values_.clear(); } │ │ │ │ │ -302 │ │ │ │ │ -304 size_t dim() const; │ │ │ │ │ -305 │ │ │ │ │ -307 std::map dims() const; │ │ │ │ │ -308 │ │ │ │ │ -310 _V_e_c_t_o_r_V_a_l_u_e_s zeroVectors() const; │ │ │ │ │ -311 │ │ │ │ │ -312 // Count values of given type \c ValueType │ │ │ │ │ -313 template │ │ │ │ │ -314 size_t count() const { │ │ │ │ │ -315 size_t i = 0; │ │ │ │ │ -316 for (const auto key_value : values_) { │ │ │ │ │ -317 if (dynamic_cast*>(key_value.second)) │ │ │ │ │ -318 ++i; │ │ │ │ │ -319 } │ │ │ │ │ -320 return i; │ │ │ │ │ -321 } │ │ │ │ │ -322 │ │ │ │ │ -342 template │ │ │ │ │ -343 std::map // , std::less, Eigen:: │ │ │ │ │ -aligned_allocator │ │ │ │ │ -344 extract(const std::function& filterFcn = &_truePredicate) │ │ │ │ │ -const; │ │ │ │ │ -345 │ │ │ │ │ -346#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ -347 // Types obtained by iterating │ │ │ │ │ -348 typedef KeyValueMap::const_iterator::value_type ConstKeyValuePtrPair; │ │ │ │ │ -349 typedef KeyValueMap::iterator::value_type KeyValuePtrPair; │ │ │ │ │ -350 │ │ │ │ │ -352 typedef boost::transform_iterator< │ │ │ │ │ -353 std::function, KeyValueMap::iterator> │ │ │ │ │ -iterator; │ │ │ │ │ -354 │ │ │ │ │ -356 typedef boost::transform_iterator< │ │ │ │ │ -357 std::function, │ │ │ │ │ -KeyValueMap::const_iterator> const_iterator; │ │ │ │ │ -358 │ │ │ │ │ -360 typedef boost::transform_iterator< │ │ │ │ │ -361 std::function, KeyValueMap:: │ │ │ │ │ -reverse_iterator> reverse_iterator; │ │ │ │ │ -362 │ │ │ │ │ -364 typedef boost::transform_iterator< │ │ │ │ │ -365 std::function, │ │ │ │ │ -KeyValueMap::const_reverse_iterator> const_reverse_iterator; │ │ │ │ │ -366 │ │ │ │ │ -371 std::pair tryInsert(Key j, const Value& value); │ │ │ │ │ -372 │ │ │ │ │ -373 static ConstKeyValuePair make_const_deref_pair(const KeyValueMap:: │ │ │ │ │ -const_iterator::value_type& key_value) { │ │ │ │ │ -374 return ConstKeyValuePair(key_value.first, *key_value.second); } │ │ │ │ │ -375 │ │ │ │ │ -376 static KeyValuePair make_deref_pair(const KeyValueMap::iterator:: │ │ │ │ │ -value_type& key_value) { │ │ │ │ │ -377 return KeyValuePair(key_value.first, *key_value.second); } │ │ │ │ │ -378 │ │ │ │ │ -379 const_iterator _begin() const { return boost::make_transform_iterator │ │ │ │ │ -(values_.begin(), &make_const_deref_pair); } │ │ │ │ │ -380 const_iterator _end() const { return boost::make_transform_iterator │ │ │ │ │ -(values_.end(), &make_const_deref_pair); } │ │ │ │ │ -381 iterator begin() { return boost::make_transform_iterator(values_.begin(), │ │ │ │ │ -&make_deref_pair); } │ │ │ │ │ -382 iterator end() { return boost::make_transform_iterator(values_.end(), │ │ │ │ │ -&make_deref_pair); } │ │ │ │ │ -383 const_reverse_iterator rbegin() const { return boost:: │ │ │ │ │ -make_transform_iterator(values_.rbegin(), &make_const_deref_pair); } │ │ │ │ │ -384 const_reverse_iterator rend() const { return boost::make_transform_iterator │ │ │ │ │ -(values_.rend(), &make_const_deref_pair); } │ │ │ │ │ -385 reverse_iterator rbegin() { return boost::make_transform_iterator │ │ │ │ │ -(values_.rbegin(), &make_deref_pair); } │ │ │ │ │ -386 reverse_iterator rend() { return boost::make_transform_iterator │ │ │ │ │ -(values_.rend(), &make_deref_pair); } │ │ │ │ │ -387 │ │ │ │ │ -390 iterator find(Key j) { return boost::make_transform_iterator(values_.find │ │ │ │ │ -(j), &make_deref_pair); } │ │ │ │ │ -391 │ │ │ │ │ -393 iterator lower_bound(Key j) { return boost::make_transform_iterator │ │ │ │ │ -(values_.lower_bound(j), &make_deref_pair); } │ │ │ │ │ -394 │ │ │ │ │ -396 iterator upper_bound(Key j) { return boost::make_transform_iterator │ │ │ │ │ -(values_.upper_bound(j), &make_deref_pair); } │ │ │ │ │ -397 │ │ │ │ │ -399 template │ │ │ │ │ -400 class Filtered; │ │ │ │ │ -401 │ │ │ │ │ -403 template │ │ │ │ │ -404 class ConstFiltered; │ │ │ │ │ -405 │ │ │ │ │ -407 template │ │ │ │ │ -408 _V_a_l_u_e_s(const Filtered& view); │ │ │ │ │ -409 │ │ │ │ │ -411 template │ │ │ │ │ -412 _V_a_l_u_e_s(const ConstFiltered& view); │ │ │ │ │ -413 │ │ │ │ │ -415 Filtered GTSAM_DEPRECATED │ │ │ │ │ -416 filter(const std::function& filterFcn); │ │ │ │ │ -417 │ │ │ │ │ -419 template │ │ │ │ │ -420 Filtered GTSAM_DEPRECATED │ │ │ │ │ -421 filter(const std::function& filterFcn = &_truePredicate); │ │ │ │ │ -422 │ │ │ │ │ -424 ConstFiltered GTSAM_DEPRECATED │ │ │ │ │ -425 filter(const std::function& filterFcn) const; │ │ │ │ │ -426 │ │ │ │ │ -428 template │ │ │ │ │ -429 ConstFiltered GTSAM_DEPRECATED filter( │ │ │ │ │ -430 const std::function& filterFcn = &_truePredicate) const; │ │ │ │ │ -431#endif │ │ │ │ │ -432 │ │ │ │ │ -433 private: │ │ │ │ │ -434 // Filters based on ValueType (if not Value) and also based on the user- │ │ │ │ │ -435 // supplied \c filter function. │ │ │ │ │ -436 template │ │ │ │ │ -437 static bool filterHelper(const std::function filter, const │ │ │ │ │ -ConstKeyValuePair& key_value) { │ │ │ │ │ -438 BOOST_STATIC_ASSERT((!boost::is_same::value)); │ │ │ │ │ -439 // Filter and check the type │ │ │ │ │ -440 return filter(key_value.key) && (dynamic_cast*>(&key_value.value)); │ │ │ │ │ -441 } │ │ │ │ │ -442 │ │ │ │ │ -_4_4_4 friend class boost::serialization::access; │ │ │ │ │ -445 template │ │ │ │ │ -446 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -447 ar & BOOST_SERIALIZATION_NVP(values_); │ │ │ │ │ -448 } │ │ │ │ │ -449 │ │ │ │ │ -450 }; │ │ │ │ │ -451 │ │ │ │ │ -452 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -_4_5_3 class _V_a_l_u_e_s_K_e_y_A_l_r_e_a_d_y_E_x_i_s_t_s : public std::exception { │ │ │ │ │ -454 protected: │ │ │ │ │ -_4_5_5 const _K_e_y _k_e_y__; │ │ │ │ │ -456 │ │ │ │ │ -457 private: │ │ │ │ │ -458 mutable std::string message_; │ │ │ │ │ -459 │ │ │ │ │ -460 public: │ │ │ │ │ -_4_6_2 _V_a_l_u_e_s_K_e_y_A_l_r_e_a_d_y_E_x_i_s_t_s(_K_e_y _k_e_y) noexcept : │ │ │ │ │ -463 _k_e_y__(_k_e_y) {} │ │ │ │ │ -464 │ │ │ │ │ -465 _~_V_a_l_u_e_s_K_e_y_A_l_r_e_a_d_y_E_x_i_s_t_s() noexcept override {} │ │ │ │ │ -466 │ │ │ │ │ -_4_6_8 _K_e_y _k_e_y() const noexcept { return _k_e_y__; } │ │ │ │ │ -469 │ │ │ │ │ -471 GTSAM_EXPORT const char* _w_h_a_t() const noexcept override; │ │ │ │ │ -472 }; │ │ │ │ │ -473 │ │ │ │ │ -474 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -_4_7_5 class _V_a_l_u_e_s_K_e_y_D_o_e_s_N_o_t_E_x_i_s_t : public std::exception { │ │ │ │ │ -476 protected: │ │ │ │ │ -_4_7_7 const char* _o_p_e_r_a_t_i_o_n__; │ │ │ │ │ -_4_7_8 const _K_e_y _k_e_y__; │ │ │ │ │ -479 │ │ │ │ │ -480 private: │ │ │ │ │ -481 mutable std::string message_; │ │ │ │ │ -482 │ │ │ │ │ -483 public: │ │ │ │ │ -_4_8_5 _V_a_l_u_e_s_K_e_y_D_o_e_s_N_o_t_E_x_i_s_t(const char* operation, _K_e_y _k_e_y) noexcept : │ │ │ │ │ -486 operation_(operation), _k_e_y__(_k_e_y) {} │ │ │ │ │ -487 │ │ │ │ │ -488 _~_V_a_l_u_e_s_K_e_y_D_o_e_s_N_o_t_E_x_i_s_t() noexcept override {} │ │ │ │ │ -489 │ │ │ │ │ -_4_9_1 _K_e_y _k_e_y() const noexcept { return _k_e_y__; } │ │ │ │ │ -492 │ │ │ │ │ -494 GTSAM_EXPORT const char* _w_h_a_t() const noexcept override; │ │ │ │ │ -495 }; │ │ │ │ │ -496 │ │ │ │ │ -497 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -_4_9_8 class _V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e : public std::exception { │ │ │ │ │ -499 protected: │ │ │ │ │ -_5_0_0 const _K_e_y _k_e_y__; │ │ │ │ │ -501 const std::type_info& storedTypeId_; │ │ │ │ │ -502 const std::type_info& requestedTypeId_; │ │ │ │ │ -503 │ │ │ │ │ -504 private: │ │ │ │ │ -505 mutable std::string message_; │ │ │ │ │ -506 │ │ │ │ │ -507 public: │ │ │ │ │ -_5_0_9 _V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e(_K_e_y _k_e_y, │ │ │ │ │ -510 const std::type_info& storedTypeId, const std::type_info& requestedTypeId) │ │ │ │ │ -noexcept : │ │ │ │ │ -511 _k_e_y__(_k_e_y), storedTypeId_(storedTypeId), requestedTypeId_(requestedTypeId) │ │ │ │ │ -{} │ │ │ │ │ -512 │ │ │ │ │ -513 _~_V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e() noexcept override {} │ │ │ │ │ -514 │ │ │ │ │ -_5_1_6 _K_e_y _k_e_y() const noexcept { return _k_e_y__; } │ │ │ │ │ -517 │ │ │ │ │ -_5_1_9 const std::type_info& _s_t_o_r_e_d_T_y_p_e_I_d() const { return storedTypeId_; } │ │ │ │ │ -520 │ │ │ │ │ -_5_2_2 const std::type_info& _r_e_q_u_e_s_t_e_d_T_y_p_e_I_d() const { return requestedTypeId_; } │ │ │ │ │ -523 │ │ │ │ │ -525 GTSAM_EXPORT const char* _w_h_a_t() const noexcept override; │ │ │ │ │ -526 }; │ │ │ │ │ -527 │ │ │ │ │ -528 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -_5_2_9 class _D_y_n_a_m_i_c_V_a_l_u_e_s_M_i_s_m_a_t_c_h_e_d : public std::exception { │ │ │ │ │ -530 │ │ │ │ │ -531 public: │ │ │ │ │ -532 _D_y_n_a_m_i_c_V_a_l_u_e_s_M_i_s_m_a_t_c_h_e_d() noexcept {} │ │ │ │ │ -533 │ │ │ │ │ -534 _~_D_y_n_a_m_i_c_V_a_l_u_e_s_M_i_s_m_a_t_c_h_e_d() noexcept override {} │ │ │ │ │ -535 │ │ │ │ │ -536 const char* _w_h_a_t() const noexcept override { │ │ │ │ │ -537 return "The Values 'this' and the argument passed to Values:: │ │ │ │ │ -localCoordinates have mismatched keys and values"; │ │ │ │ │ -538 } │ │ │ │ │ -539 }; │ │ │ │ │ -540 │ │ │ │ │ -541 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -_5_4_2 class _N_o_M_a_t_c_h_F_o_u_n_d_F_o_r_F_i_x_e_d: public std::exception { │ │ │ │ │ -543 │ │ │ │ │ -544 protected: │ │ │ │ │ -545 const size_t M1_, N1_; │ │ │ │ │ -546 const size_t M2_, N2_; │ │ │ │ │ -547 │ │ │ │ │ -548 private: │ │ │ │ │ -549 mutable std::string message_; │ │ │ │ │ -550 │ │ │ │ │ -551 public: │ │ │ │ │ -552 _N_o_M_a_t_c_h_F_o_u_n_d_F_o_r_F_i_x_e_d(size_t M1, size_t N1, size_t M2, size_t N2) noexcept : │ │ │ │ │ -553 M1_(M1), N1_(N1), M2_(M2), N2_(N2) { │ │ │ │ │ -554 } │ │ │ │ │ -555 │ │ │ │ │ -556 _~_N_o_M_a_t_c_h_F_o_u_n_d_F_o_r_F_i_x_e_d() noexcept override { │ │ │ │ │ -557 } │ │ │ │ │ -558 │ │ │ │ │ -559 GTSAM_EXPORT const char* what() const noexcept override; │ │ │ │ │ -560 }; │ │ │ │ │ -561 │ │ │ │ │ -562 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -564 template<> │ │ │ │ │ -_5_6_5 struct _t_r_a_i_t_s<_V_a_l_u_e_s> : public _T_e_s_t_a_b_l_e { │ │ │ │ │ -566 }; │ │ │ │ │ -567 │ │ │ │ │ -568} //\ namespace gtsam │ │ │ │ │ -569 │ │ │ │ │ -570 │ │ │ │ │ -571#include │ │ │ │ │ -_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_._h │ │ │ │ │ -An easy way to control which allocator is used for Fast* collections. │ │ │ │ │ +_3_3 class _M_a_r_g_i_n_a_l_i_z_e_N_o_n_l_e_a_f_E_x_c_e_p_t_i_o_n : public std::exception { │ │ │ │ │ +34 _K_e_y key_; │ │ │ │ │ +35 _K_e_y_F_o_r_m_a_t_t_e_r formatter_; │ │ │ │ │ +36 mutable std::string what_; │ │ │ │ │ +37 public: │ │ │ │ │ +38 _M_a_r_g_i_n_a_l_i_z_e_N_o_n_l_e_a_f_E_x_c_e_p_t_i_o_n(_K_e_y key, _K_e_y_F_o_r_m_a_t_t_e_r formatter = │ │ │ │ │ +DefaultKeyFormatter) noexcept : │ │ │ │ │ +39 key_(key), formatter_(formatter) {} │ │ │ │ │ +40 virtual _~_M_a_r_g_i_n_a_l_i_z_e_N_o_n_l_e_a_f_E_x_c_e_p_t_i_o_n() noexcept {} │ │ │ │ │ +41 _K_e_y key() const { return key_; } │ │ │ │ │ +42 const char* what() const noexcept override { │ │ │ │ │ +43 if(what_.empty()) │ │ │ │ │ +44 what_ = │ │ │ │ │ +45"\nRequested to marginalize out variable " + formatter_(key_) + ", but this │ │ │ │ │ +variable\n\ │ │ │ │ │ +46is not a leaf. To make the variables you would like to marginalize be │ │ │ │ │ +leaves,\n\ │ │ │ │ │ +47their ordering should be constrained using the constrainedKeys argument to\n\ │ │ │ │ │ +48ISAM2::update().\n"; │ │ │ │ │ +49 return what_.c_str(); │ │ │ │ │ +50 } │ │ │ │ │ +51 }; │ │ │ │ │ +52 │ │ │ │ │ +53} │ │ │ │ │ _K_e_y_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ -Point2 operator*(double s, const Point2 &p) │ │ │ │ │ -multiply with scalar │ │ │ │ │ -DDeeffiinniittiioonn Point2.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_!_= │ │ │ │ │ -bool operator!=(const Matrix &A, const Matrix &B) │ │ │ │ │ -inequality │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:107 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_=_= │ │ │ │ │ -bool operator==(const Matrix &A, const Matrix &B) │ │ │ │ │ -equality is just equal_with_abs_tol 1e-9 │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r │ │ │ │ │ -Default allocator for list, map, and set types. │ │ │ │ │ -DDeeffiinniittiioonn FastDefaultAllocator.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e │ │ │ │ │ -Wraps any type T so it can play as a Value. │ │ │ │ │ -DDeeffiinniittiioonn GenericValue.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e │ │ │ │ │ -This is the base class for any type to be stored in Values. │ │ │ │ │ -DDeeffiinniittiioonn Value.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_:_:_d_e_a_l_l_o_c_a_t_e__ │ │ │ │ │ -virtual void deallocate_() const =0 │ │ │ │ │ -Deallocate a raw pointer of this value. │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_:_:_c_l_o_n_e__ │ │ │ │ │ -virtual Value * clone_() const =0 │ │ │ │ │ -Clone this value in a special memory pool, must be deleted with Value:: │ │ │ │ │ -deallocate_,... │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ -with a unique integer... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_C_l_o_n_e_A_l_l_o_c_a_t_o_r │ │ │ │ │ -DDeeffiinniittiioonn Values.h:49 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_u_p_d_a_t_e │ │ │ │ │ -void update(Key j, const T &val) │ │ │ │ │ -Templated version to update a variable with the given j, throws │ │ │ │ │ -KeyDoesNotExist if j is not presen... │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_e_m_p_t_y │ │ │ │ │ -bool empty() const │ │ │ │ │ -whether the config is empty │ │ │ │ │ -DDeeffiinniittiioonn Values.h:184 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_u_p_p_e_r___b_o_u_n_d │ │ │ │ │ -deref_iterator upper_bound(Key j) const │ │ │ │ │ -Find the lowest-ordered element greater than the specified key. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:219 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< Values > shared_ptr │ │ │ │ │ -A shared_ptr to this class. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:87 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_c_l_e_a_r │ │ │ │ │ -void clear() │ │ │ │ │ -Remove all variables from the config. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:301 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_s_w_a_p │ │ │ │ │ -void swap(Values &other) │ │ │ │ │ -Swap the contents of two Values without copying data. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:298 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_i_n_s_e_r_t_D_o_u_b_l_e │ │ │ │ │ -void insertDouble(Key j, double c) │ │ │ │ │ -version for double │ │ │ │ │ -DDeeffiinniittiioonn Values.h:252 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_s_i_z_e │ │ │ │ │ -size_t size() const │ │ │ │ │ -The number of variables in this config. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:181 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_f_i_n_d │ │ │ │ │ -deref_iterator find(Key j) const │ │ │ │ │ -Find an element by key, returning an iterator, or end() if the key was not │ │ │ │ │ -found. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:213 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_c_o_n_s_t___s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< const Values > const_shared_ptr │ │ │ │ │ -A const shared_ptr to this class. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:90 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_V_a_l_u_e_s │ │ │ │ │ -Values()=default │ │ │ │ │ -Default constructor creates an empty Values class. │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_l_o_w_e_r___b_o_u_n_d │ │ │ │ │ -deref_iterator lower_bound(Key j) const │ │ │ │ │ -Find the element greater than or equal to the specified key. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:216 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_a_t_D_o_u_b_l_e │ │ │ │ │ -double atDouble(size_t key) const │ │ │ │ │ -version for double │ │ │ │ │ -DDeeffiinniittiioonn Values.h:159 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_K_e_y_V_a_l_u_e_P_a_i_r │ │ │ │ │ -A key-value pair, which you get by dereferencing iterators. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:93 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_K_e_y_V_a_l_u_e_P_a_i_r_:_:_v_a_l_u_e │ │ │ │ │ -Value & value │ │ │ │ │ -The value. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:95 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_K_e_y_V_a_l_u_e_P_a_i_r_:_:_k_e_y │ │ │ │ │ -const Key key │ │ │ │ │ -The key. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:94 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r │ │ │ │ │ -A key-value pair, which you get by dereferencing iterators. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:101 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r_:_:_k_e_y │ │ │ │ │ -const Key key │ │ │ │ │ -The key. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:102 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r_:_:_v_a_l_u_e │ │ │ │ │ -const Value & value │ │ │ │ │ -The value. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:103 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_d_e_r_e_f___i_t_e_r_a_t_o_r │ │ │ │ │ -DDeeffiinniittiioonn Values.h:190 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_K_e_y_A_l_r_e_a_d_y_E_x_i_s_t_s │ │ │ │ │ -DDeeffiinniittiioonn Values.h:453 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_K_e_y_A_l_r_e_a_d_y_E_x_i_s_t_s_:_:_k_e_y__ │ │ │ │ │ -const Key key_ │ │ │ │ │ -The key that already existed. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:455 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_K_e_y_A_l_r_e_a_d_y_E_x_i_s_t_s_:_:_w_h_a_t │ │ │ │ │ -GTSAM_EXPORT const char * what() const noexcept override │ │ │ │ │ -The message to be displayed to the user. │ │ │ │ │ -DDeeffiinniittiioonn Values.cpp:280 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_K_e_y_A_l_r_e_a_d_y_E_x_i_s_t_s_:_:_V_a_l_u_e_s_K_e_y_A_l_r_e_a_d_y_E_x_i_s_t_s │ │ │ │ │ -ValuesKeyAlreadyExists(Key key) noexcept │ │ │ │ │ -Construct with the key-value pair attempted to be added. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:462 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_K_e_y_A_l_r_e_a_d_y_E_x_i_s_t_s_:_:_k_e_y │ │ │ │ │ -Key key() const noexcept │ │ │ │ │ -The duplicate key that was attempted to be added. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:468 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_K_e_y_D_o_e_s_N_o_t_E_x_i_s_t │ │ │ │ │ -DDeeffiinniittiioonn Values.h:475 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_K_e_y_D_o_e_s_N_o_t_E_x_i_s_t_:_:_V_a_l_u_e_s_K_e_y_D_o_e_s_N_o_t_E_x_i_s_t │ │ │ │ │ -ValuesKeyDoesNotExist(const char *operation, Key key) noexcept │ │ │ │ │ -Construct with the key that does not exist in the values. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:485 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_K_e_y_D_o_e_s_N_o_t_E_x_i_s_t_:_:_k_e_y__ │ │ │ │ │ -const Key key_ │ │ │ │ │ -The key that does not exist. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:478 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_K_e_y_D_o_e_s_N_o_t_E_x_i_s_t_:_:_k_e_y │ │ │ │ │ -Key key() const noexcept │ │ │ │ │ -The key that was attempted to be accessed that does not exist. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:491 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_K_e_y_D_o_e_s_N_o_t_E_x_i_s_t_:_:_o_p_e_r_a_t_i_o_n__ │ │ │ │ │ -const char * operation_ │ │ │ │ │ -The operation that attempted to access the key. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:477 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e │ │ │ │ │ -DDeeffiinniittiioonn Values.h:498 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e_:_:_s_t_o_r_e_d_T_y_p_e_I_d │ │ │ │ │ -const std::type_info & storedTypeId() const │ │ │ │ │ -The typeid of the value stores in the Values. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:519 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e_:_:_k_e_y │ │ │ │ │ -Key key() const noexcept │ │ │ │ │ -The key that was attempted to be accessed that does not exist. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:516 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e_:_:_k_e_y__ │ │ │ │ │ -const Key key_ │ │ │ │ │ -The key requested. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:500 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e_:_:_V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e │ │ │ │ │ -ValuesIncorrectType(Key key, const std::type_info &storedTypeId, const std:: │ │ │ │ │ -type_info &requestedTypeId) noexcept │ │ │ │ │ -Construct with the key that does not exist in the values. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:509 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e_:_:_r_e_q_u_e_s_t_e_d_T_y_p_e_I_d │ │ │ │ │ -const std::type_info & requestedTypeId() const │ │ │ │ │ -The requested typeid. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:522 │ │ │ │ │ -_g_t_s_a_m_:_:_D_y_n_a_m_i_c_V_a_l_u_e_s_M_i_s_m_a_t_c_h_e_d │ │ │ │ │ -DDeeffiinniittiioonn Values.h:529 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_M_a_t_c_h_F_o_u_n_d_F_o_r_F_i_x_e_d │ │ │ │ │ -DDeeffiinniittiioonn Values.h:542 │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -The Factor::error simply extracts the. │ │ │ │ │ -_V_a_l_u_e_s │ │ │ │ │ -In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ -a non-linear function... │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_r_g_i_n_a_l_i_z_e_N_o_n_l_e_a_f_E_x_c_e_p_t_i_o_n │ │ │ │ │ +Thrown when requesting to marginalize out variables from ISAM2 that are not │ │ │ │ │ +leaves. │ │ │ │ │ +DDeeffiinniittiioonn nonlinearExceptions.h:33 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _V_a_l_u_e_s_._h │ │ │ │ │ + * _n_o_n_l_i_n_e_a_r_E_x_c_e_p_t_i_o_n_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01010.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/factorTesting.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/CustomFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,109 +94,53 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces | │ │ │ │ -Macros | │ │ │ │ -Functions
│ │ │ │ -
factorTesting.h File Reference
│ │ │ │ +Typedefs
│ │ │ │ +
CustomFactor.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Evaluate derivatives of a nonlinear factor numerically. │ │ │ │ +

Class to enable arbitrary factors with runtime swappable error function. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::CustomFactor
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Macros

#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance)    { EXPECT(gtsam::internal::testFactorJacobians(name_, factor, values, numerical_derivative_step, tolerance)); }
 Check the Jacobians produced by a factor against finite differences.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ -Functions

JacobianFactor gtsam::linearizeNumerically (const NoiseModelFactor &factor, const Values &values, double delta=1e-5)
 Linearize a nonlinear factor using numerical differentiation The benefit of this method is that it does not need to know what types are involved to evaluate the factor.
 
│ │ │ │ -bool gtsam::internal::testFactorJacobians (const std::string &name_, const NoiseModelFactor &factor, const gtsam::Values &values, double delta, double tolerance)
 

│ │ │ │ +Typedefs

│ │ │ │ +using gtsam::JacobianVector = std::vector< Matrix >
 
│ │ │ │ +using gtsam::CustomErrorFunction = std::function< Vector(const CustomFactor &, const Values &, const JacobianVector *)>
 
│ │ │ │

Detailed Description

│ │ │ │ -

Evaluate derivatives of a nonlinear factor numerically.

│ │ │ │ -
Date
September 18, 2014
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ -Paul Furgale
│ │ │ │ -

Macro Definition Documentation

│ │ │ │ - │ │ │ │ -

◆ EXPECT_CORRECT_FACTOR_JACOBIANS

│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
#define EXPECT_CORRECT_FACTOR_JACOBIANS( factor,
 values,
 numerical_derivative_step,
 tolerance 
)    { EXPECT(gtsam::internal::testFactorJacobians(name_, factor, values, numerical_derivative_step, tolerance)); }
│ │ │ │ -
│ │ │ │ - │ │ │ │ -

Check the Jacobians produced by a factor against finite differences.

│ │ │ │ -
Parameters
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
factorThe factor to test.
valuesValues filled in for testing the Jacobians.
numerical_derivative_stepThe step to use when computing the numerical derivative Jacobians
toleranceThe numerical tolerance to use when comparing Jacobians.
│ │ │ │ -
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ +

Class to enable arbitrary factors with runtime swappable error function.

│ │ │ │ +
Author
Fan Jiang
│ │ │ │ +
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,60 +1,31 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -factorTesting.h File Reference │ │ │ │ │ -Evaluate derivatives of a nonlinear factor numerically. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ +CustomFactor.h File Reference │ │ │ │ │ +Class to enable arbitrary factors with runtime swappable error function. │ │ │ │ │ +_M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_C_u_s_t_o_m_F_a_c_t_o_r │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -MMaaccrrooss │ │ │ │ │ -#define  _E_X_P_E_C_T___C_O_R_R_E_C_T___F_A_C_T_O_R___J_A_C_O_B_I_A_N_S(factor, values, │ │ │ │ │ - numerical_derivative_step, tolerance)    { EXPECT(gtsam::internal:: │ │ │ │ │ - testFactorJacobians(name_, factor, values, numerical_derivative_step, │ │ │ │ │ - tolerance)); } │ │ │ │ │ -  Check the Jacobians produced by a factor against finite differences. │ │ │ │ │ -  │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -_J_a_c_o_b_i_a_n_F_a_c_t_o_r  _g_t_s_a_m_:_:_l_i_n_e_a_r_i_z_e_N_u_m_e_r_i_c_a_l_l_y (const _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r &factor, │ │ │ │ │ - const _V_a_l_u_e_s &values, double delta=1e-5) │ │ │ │ │ - Linearize a nonlinear factor using numerical differentiation │ │ │ │ │ -  The benefit of this method is that it does not need to know │ │ │ │ │ - what types are involved to evaluate the factor. │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +using  ggttssaamm::::JJaaccoobbiiaannVVeeccttoorr = std::vector< Matrix > │ │ │ │ │   │ │ │ │ │ - bool  ggttssaamm::::iinntteerrnnaall::::tteessttFFaaccttoorrJJaaccoobbiiaannss (const std::string &name_, │ │ │ │ │ - const _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r &factor, const _g_t_s_a_m_:_:_V_a_l_u_e_s &values, │ │ │ │ │ - double delta, double tolerance) │ │ │ │ │ +using  ggttssaamm::::CCuussttoommEErrrroorrFFuunnccttiioonn = std::function< Vector(const _C_u_s_t_o_m_F_a_c_t_o_r &, │ │ │ │ │ + const _V_a_l_u_e_s &, const JacobianVector *)> │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Evaluate derivatives of a nonlinear factor numerically. │ │ │ │ │ - Date │ │ │ │ │ - September 18, 2014 │ │ │ │ │ +Class to enable arbitrary factors with runtime swappable error function. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Paul Furgale │ │ │ │ │ -********** MMaaccrroo DDeeffiinniittiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ -********** _?◆_? EEXXPPEECCTT__CCOORRRREECCTT__FFAACCTTOORR__JJAACCOOBBIIAANNSS ********** │ │ │ │ │ -#define EXPECT_CORRECT_FACTOR_JACOBIANS (   factor, │ │ │ │ │ -   values, │ │ │ │ │ -   numerical_derivative_step, │ │ │ │ │ -   tolerance  │ │ │ │ │ -     { EXPECT(gtsam::internal:: │ │ │ │ │ - ) testFactorJacobians(name_, factor, │ │ │ │ │ - values, numerical_derivative_step, │ │ │ │ │ - tolerance)); } │ │ │ │ │ -Check the Jacobians produced by a factor against finite differences. │ │ │ │ │ - Parameters │ │ │ │ │ - factor The factor to test. │ │ │ │ │ - values _V_a_l_u_e_s filled in for testing the Jacobians. │ │ │ │ │ - numerical_derivative_step The step to use when computing the numerical │ │ │ │ │ - derivative Jacobians │ │ │ │ │ - tolerance The numerical tolerance to use when comparing │ │ │ │ │ - Jacobians. │ │ │ │ │ + Fan Jiang │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _f_a_c_t_o_r_T_e_s_t_i_n_g_._h │ │ │ │ │ + * _C_u_s_t_o_m_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01010.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,3 @@ │ │ │ │ │ var a01010 = [ │ │ │ │ │ - ["EXPECT_CORRECT_FACTOR_JACOBIANS", "a01010.html#a8ec37fe83eda47404b8588e1f012df21", null], │ │ │ │ │ - ["linearizeNumerically", "a01010.html#a9753b4e62378151d30bf8af5e2fc1602", null] │ │ │ │ │ + ["gtsam::CustomFactor", "a04248.html", "a04248"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01010_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/factorTesting.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/CustomFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,136 +98,116 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
factorTesting.h
│ │ │ │ +
CustomFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │
21
│ │ │ │ -
22#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
23#include <gtsam/base/numericalDerivative.h>
│ │ │ │ -
24#include <string>
│ │ │ │ -
25#include <vector>
│ │ │ │ -
26
│ │ │ │ -
27namespace gtsam {
│ │ │ │ -
28
│ │ │ │ -
│ │ │ │ -
39inline JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
│ │ │ │ -
40 const Values& values,
│ │ │ │ -
41 double delta = 1e-5) {
│ │ │ │ -
42 // We will fill a vector of key/Jacobians pairs (a map would sort)
│ │ │ │ -
43 std::vector<std::pair<Key, Matrix> > jacobians;
│ │ │ │ -
44
│ │ │ │ -
45 // Get size
│ │ │ │ -
46 const Vector e = factor.whitenedError(values);
│ │ │ │ -
47 const size_t rows = e.size();
│ │ │ │ +
22namespace gtsam {
│ │ │ │ +
23
│ │ │ │ +
24using JacobianVector = std::vector<Matrix>;
│ │ │ │ +
25
│ │ │ │ +
26class CustomFactor;
│ │ │ │ +
27
│ │ │ │ +
28/*
│ │ │ │ +
29 * NOTE
│ │ │ │ +
30 * ==========
│ │ │ │ +
31 * pybind11 will invoke a copy if this is `JacobianVector &`, and modifications in Python will not be reflected.
│ │ │ │ +
32 *
│ │ │ │ +
33 * This is safe because this is passing a const pointer, and pybind11 will maintain the `std::vector` memory layout.
│ │ │ │ +
34 * Thus the pointer will never be invalidated.
│ │ │ │ +
35 */
│ │ │ │ +
36using CustomErrorFunction = std::function<Vector(const CustomFactor &, const Values &, const JacobianVector *)>;
│ │ │ │ +
37
│ │ │ │ +
│ │ │ │ +
45class CustomFactor: public NoiseModelFactor {
│ │ │ │ +
46protected:
│ │ │ │ +
47 CustomErrorFunction error_function_;
│ │ │ │
48
│ │ │ │ -
49 // Loop over all variables
│ │ │ │ -
50 const double one_over_2delta = 1.0 / (2.0 * delta);
│ │ │ │ -
51 for (Key key : factor) {
│ │ │ │ -
52 // Compute central differences using the values struct.
│ │ │ │ -
53 VectorValues dX = values.zeroVectors();
│ │ │ │ -
54 const size_t cols = dX.dim(key);
│ │ │ │ -
55 Matrix J = Matrix::Zero(rows, cols);
│ │ │ │ -
56 for (size_t col = 0; col < cols; ++col) {
│ │ │ │ -
57 Vector dx = Vector::Zero(cols);
│ │ │ │ -
58 dx(col) = delta;
│ │ │ │ -
59 dX[key] = dx;
│ │ │ │ -
60 Values eval_values = values.retract(dX);
│ │ │ │ -
61 const Vector left = factor.whitenedError(eval_values);
│ │ │ │ -
62 dx(col) = -delta;
│ │ │ │ -
63 dX[key] = dx;
│ │ │ │ -
64 eval_values = values.retract(dX);
│ │ │ │ -
65 const Vector right = factor.whitenedError(eval_values);
│ │ │ │ -
66 J.col(col) = (left - right) * one_over_2delta;
│ │ │ │ -
67 }
│ │ │ │ -
68 jacobians.emplace_back(key, J);
│ │ │ │ -
69 }
│ │ │ │ -
70
│ │ │ │ -
71 // Next step...return JacobianFactor
│ │ │ │ -
72 return JacobianFactor(jacobians, -e);
│ │ │ │ -
73}
│ │ │ │ -
│ │ │ │ -
74
│ │ │ │ -
75namespace internal {
│ │ │ │ -
76// CPPUnitLite-style test for linearization of a factor
│ │ │ │ -
77inline bool testFactorJacobians(const std::string& name_,
│ │ │ │ -
78 const NoiseModelFactor& factor,
│ │ │ │ -
79 const gtsam::Values& values, double delta,
│ │ │ │ -
80 double tolerance) {
│ │ │ │ -
81 // Create expected value by numerical differentiation
│ │ │ │ -
82 JacobianFactor expected = linearizeNumerically(factor, values, delta);
│ │ │ │ +
49protected:
│ │ │ │ +
50
│ │ │ │ +
51 using Base = NoiseModelFactor;
│ │ │ │ +
52 using This = CustomFactor;
│ │ │ │ +
53
│ │ │ │ +
54public:
│ │ │ │ +
55
│ │ │ │ +
59 CustomFactor() = default;
│ │ │ │ +
60
│ │ │ │ +
│ │ │ │ +
67 CustomFactor(const SharedNoiseModel &noiseModel, const KeyVector &keys, const CustomErrorFunction &errorFunction) :
│ │ │ │ +
68 Base(noiseModel, keys) {
│ │ │ │ +
69 this->error_function_ = errorFunction;
│ │ │ │ +
70 }
│ │ │ │ +
│ │ │ │ +
71
│ │ │ │ +
72 ~CustomFactor() override = default;
│ │ │ │ +
73
│ │ │ │ +
78 Vector unwhitenedError(const Values &x, boost::optional<std::vector<Matrix> &> H = boost::none) const override;
│ │ │ │ +
79
│ │ │ │ +
81 void print(const std::string &s,
│ │ │ │ +
82 const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override;
│ │ │ │
83
│ │ │ │ -
84 // Create actual value by linearize
│ │ │ │ -
85 auto actual =
│ │ │ │ -
86 boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(values));
│ │ │ │ -
87 if (!actual) return false;
│ │ │ │ -
88
│ │ │ │ -
89 // Check cast result and then equality
│ │ │ │ -
90 bool equal = assert_equal(expected, *actual, tolerance);
│ │ │ │ -
91
│ │ │ │ -
92 // if not equal, test individual jacobians:
│ │ │ │ -
93 if (!equal) {
│ │ │ │ -
94 for (size_t i = 0; i < actual->size(); i++) {
│ │ │ │ -
95 bool i_good =
│ │ │ │ -
96 assert_equal((Matrix)(expected.getA(expected.begin() + i)),
│ │ │ │ -
97 (Matrix)(actual->getA(actual->begin() + i)), tolerance);
│ │ │ │ -
98 if (!i_good) {
│ │ │ │ -
99 std::cout << "Mismatch in Jacobian " << i + 1
│ │ │ │ -
100 << " (base 1), as shown above" << std::endl;
│ │ │ │ -
101 }
│ │ │ │ -
102 }
│ │ │ │ -
103 }
│ │ │ │ -
104
│ │ │ │ -
105 return equal;
│ │ │ │ -
106}
│ │ │ │ -
107} // namespace internal
│ │ │ │ -
108
│ │ │ │ -
│ │ │ │ -
114#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \
│ │ │ │ -
115 { EXPECT(gtsam::internal::testFactorJacobians(name_, factor, values, numerical_derivative_step, tolerance)); }
│ │ │ │ -
│ │ │ │ -
116
│ │ │ │ -
117} // namespace gtsam
│ │ │ │ -
numericalDerivative.h
Some functions to compute numerical derivatives.
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
│ │ │ │ +
87 bool sendable() const override {
│ │ │ │ +
88 return false;
│ │ │ │ +
89 }
│ │ │ │ +
│ │ │ │ +
90
│ │ │ │ +
91private:
│ │ │ │ +
92
│ │ │ │ +
94 friend class boost::serialization::access;
│ │ │ │ +
95 template<class ARCHIVE>
│ │ │ │ +
96 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ +
97 ar & boost::serialization::make_nvp("CustomFactor",
│ │ │ │ +
98 boost::serialization::base_object<Base>(*this));
│ │ │ │ +
99 }
│ │ │ │ +
100};
│ │ │ │ +
│ │ │ │ +
101
│ │ │ │ +
102}
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
equals with an tolerance, prints out message if unequal
Definition Matrix.cpp:43
│ │ │ │ -
gtsam::linearizeNumerically
JacobianFactor linearizeNumerically(const NoiseModelFactor &factor, const Values &values, double delta=1e-5)
Linearize a nonlinear factor using numerical differentiation The benefit of this method is that it do...
Definition factorTesting.h:39
│ │ │ │ -
gtsam::equal
bool equal(const T &obj1, const T &obj2, double tol)
Call equal on the object.
Definition Testable.h:84
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition JacobianFactor.h:91
│ │ │ │ -
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ -
gtsam::VectorValues::dim
size_t dim(Key j) const
Return the dimension of variable j.
Definition VectorValues.h:130
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
│ │ │ │ +
gtsam::CustomFactor
Definition CustomFactor.h:45
│ │ │ │ +
gtsam::CustomFactor::unwhitenedError
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
Calls the errorFunction closure, which is a std::function object One can check if a derivative is nee...
Definition CustomFactor.cpp:25
│ │ │ │ +
gtsam::CustomFactor::CustomFactor
CustomFactor(const SharedNoiseModel &noiseModel, const KeyVector &keys, const CustomErrorFunction &errorFunction)
Constructor.
Definition CustomFactor.h:67
│ │ │ │ +
gtsam::CustomFactor::CustomFactor
CustomFactor()=default
Default Constructor for I/O.
│ │ │ │ +
gtsam::CustomFactor::access
friend class boost::serialization::access
Serialization function.
Definition CustomFactor.h:94
│ │ │ │ +
gtsam::CustomFactor::sendable
bool sendable() const override
Mark not sendable.
Definition CustomFactor.h:87
│ │ │ │ +
gtsam::CustomFactor::print
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition CustomFactor.cpp:59
│ │ │ │
gtsam::NoiseModelFactor
A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
Definition NonlinearFactor.h:174
│ │ │ │ -
gtsam::NoiseModelFactor::whitenedError
Vector whitenedError(const Values &c) const
Vector of errors, whitened This is the raw error, i.e., i.e.
Definition NonlinearFactor.cpp:109
│ │ │ │ +
gtsam::NoiseModelFactor::NoiseModelFactor
NoiseModelFactor()
Default constructor for I/O only.
Definition NonlinearFactor.h:189
│ │ │ │ +
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition NonlinearFactor.h:223
│ │ │ │
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
gtsam::Values::retract
Values retract(const VectorValues &delta) const
Add a delta config to current config and returns a new config.
Definition Values.cpp:99
│ │ │ │ -
gtsam::Values::zeroVectors
VectorValues zeroVectors() const
Return a VectorValues of zero vectors for each variable in this Values.
Definition Values.cpp:272
│ │ │ │ -
JacobianFactor
In Gaussian factors, the error function returns either the negative log-likelihood,...
│ │ │ │ -
NoiseModelFactor
noise model to the factor, and calculates the error by asking the user to implement the method
│ │ │ │ +
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,170 +1,157 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -factorTesting.h │ │ │ │ │ +CustomFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ 21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_e_/_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_._h> │ │ │ │ │ -24#include │ │ │ │ │ -25#include │ │ │ │ │ -26 │ │ │ │ │ -27namespace _g_t_s_a_m { │ │ │ │ │ -28 │ │ │ │ │ -_3_9inline _J_a_c_o_b_i_a_n_F_a_c_t_o_r _l_i_n_e_a_r_i_z_e_N_u_m_e_r_i_c_a_l_l_y(const _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r& factor, │ │ │ │ │ -40 const _V_a_l_u_e_s& values, │ │ │ │ │ -41 double delta = 1e-5) { │ │ │ │ │ -42 // We will fill a vector of key/Jacobians pairs (a map would sort) │ │ │ │ │ -43 std::vector > jacobians; │ │ │ │ │ -44 │ │ │ │ │ -45 // Get size │ │ │ │ │ -46 const Vector e = factor._w_h_i_t_e_n_e_d_E_r_r_o_r(values); │ │ │ │ │ -47 const size_t rows = e.size(); │ │ │ │ │ +22namespace _g_t_s_a_m { │ │ │ │ │ +23 │ │ │ │ │ +24using JacobianVector = std::vector; │ │ │ │ │ +25 │ │ │ │ │ +26class CustomFactor; │ │ │ │ │ +27 │ │ │ │ │ +28/* │ │ │ │ │ +29 * NOTE │ │ │ │ │ +30 * ========== │ │ │ │ │ +31 * pybind11 will invoke a copy if this is `JacobianVector &`, and │ │ │ │ │ +modifications in Python will not be reflected. │ │ │ │ │ +32 * │ │ │ │ │ +33 * This is safe because this is passing a const pointer, and pybind11 will │ │ │ │ │ +maintain the `std::vector` memory layout. │ │ │ │ │ +34 * Thus the pointer will never be invalidated. │ │ │ │ │ +35 */ │ │ │ │ │ +36using CustomErrorFunction = std::function; │ │ │ │ │ +37 │ │ │ │ │ +_4_5class _C_u_s_t_o_m_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r { │ │ │ │ │ +46protected: │ │ │ │ │ +47 CustomErrorFunction error_function_; │ │ │ │ │ 48 │ │ │ │ │ -49 // Loop over all variables │ │ │ │ │ -50 const double one_over_2delta = 1.0 / (2.0 * delta); │ │ │ │ │ -51 for (_K_e_y key : factor) { │ │ │ │ │ -52 // Compute central differences using the values struct. │ │ │ │ │ -53 _V_e_c_t_o_r_V_a_l_u_e_s dX = values._z_e_r_o_V_e_c_t_o_r_s(); │ │ │ │ │ -54 const size_t cols = dX._d_i_m(key); │ │ │ │ │ -55 Matrix J = Matrix::Zero(rows, cols); │ │ │ │ │ -56 for (size_t col = 0; col < cols; ++col) { │ │ │ │ │ -57 Vector dx = Vector::Zero(cols); │ │ │ │ │ -58 dx(col) = delta; │ │ │ │ │ -59 dX[key] = dx; │ │ │ │ │ -60 _V_a_l_u_e_s eval_values = values._r_e_t_r_a_c_t(dX); │ │ │ │ │ -61 const Vector left = factor._w_h_i_t_e_n_e_d_E_r_r_o_r(eval_values); │ │ │ │ │ -62 dx(col) = -delta; │ │ │ │ │ -63 dX[key] = dx; │ │ │ │ │ -64 eval_values = values._r_e_t_r_a_c_t(dX); │ │ │ │ │ -65 const Vector right = factor._w_h_i_t_e_n_e_d_E_r_r_o_r(eval_values); │ │ │ │ │ -66 J.col(col) = (left - right) * one_over_2delta; │ │ │ │ │ -67 } │ │ │ │ │ -68 jacobians.emplace_back(key, J); │ │ │ │ │ -69 } │ │ │ │ │ -70 │ │ │ │ │ -71 // Next step...return JacobianFactor │ │ │ │ │ -72 return _J_a_c_o_b_i_a_n_F_a_c_t_o_r(jacobians, -e); │ │ │ │ │ -73} │ │ │ │ │ -74 │ │ │ │ │ -75namespace internal { │ │ │ │ │ -76// CPPUnitLite-style test for linearization of a factor │ │ │ │ │ -77inline bool testFactorJacobians(const std::string& name_, │ │ │ │ │ -78 const _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r& factor, │ │ │ │ │ -79 const _g_t_s_a_m_:_:_V_a_l_u_e_s& values, double delta, │ │ │ │ │ -80 double tolerance) { │ │ │ │ │ -81 // Create expected value by numerical differentiation │ │ │ │ │ -82 _J_a_c_o_b_i_a_n_F_a_c_t_o_r expected = _l_i_n_e_a_r_i_z_e_N_u_m_e_r_i_c_a_l_l_y(factor, values, delta); │ │ │ │ │ +49protected: │ │ │ │ │ +50 │ │ │ │ │ +51 using _B_a_s_e = _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r; │ │ │ │ │ +52 using _T_h_i_s = _C_u_s_t_o_m_F_a_c_t_o_r; │ │ │ │ │ +53 │ │ │ │ │ +54public: │ │ │ │ │ +55 │ │ │ │ │ +_5_9 _C_u_s_t_o_m_F_a_c_t_o_r() = default; │ │ │ │ │ +60 │ │ │ │ │ +_6_7 _C_u_s_t_o_m_F_a_c_t_o_r(const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &_n_o_i_s_e_M_o_d_e_l, const _K_e_y_V_e_c_t_o_r &_k_e_y_s, │ │ │ │ │ +const CustomErrorFunction &errorFunction) : │ │ │ │ │ +68 _B_a_s_e(_n_o_i_s_e_M_o_d_e_l, _k_e_y_s) { │ │ │ │ │ +69 this->error_function_ = errorFunction; │ │ │ │ │ +70 } │ │ │ │ │ +71 │ │ │ │ │ +72 _~_C_u_s_t_o_m_F_a_c_t_o_r() override = default; │ │ │ │ │ +73 │ │ │ │ │ +78 Vector _u_n_w_h_i_t_e_n_e_d_E_r_r_o_r(const _V_a_l_u_e_s &x, boost::optional │ │ │ │ │ +&> H = boost::none) const override; │ │ │ │ │ +79 │ │ │ │ │ +81 void _p_r_i_n_t(const std::string &s, │ │ │ │ │ +82 const _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter = DefaultKeyFormatter) const override; │ │ │ │ │ 83 │ │ │ │ │ -84 // Create actual value by linearize │ │ │ │ │ -85 auto actual = │ │ │ │ │ -86 boost::dynamic_pointer_cast(factor.linearize(values)); │ │ │ │ │ -87 if (!actual) return false; │ │ │ │ │ -88 │ │ │ │ │ -89 // Check cast result and then equality │ │ │ │ │ -90 bool _e_q_u_a_l = _a_s_s_e_r_t___e_q_u_a_l(expected, *actual, tolerance); │ │ │ │ │ -91 │ │ │ │ │ -92 // if not equal, test individual jacobians: │ │ │ │ │ -93 if (!_e_q_u_a_l) { │ │ │ │ │ -94 for (size_t i = 0; i < actual->size(); i++) { │ │ │ │ │ -95 bool i_good = │ │ │ │ │ -96 _a_s_s_e_r_t___e_q_u_a_l((Matrix)(expected.getA(expected.begin() + i)), │ │ │ │ │ -97 (Matrix)(actual->getA(actual->begin() + i)), tolerance); │ │ │ │ │ -98 if (!i_good) { │ │ │ │ │ -99 std::cout << "Mismatch in Jacobian " << i + 1 │ │ │ │ │ -100 << " (base 1), as shown above" << std::endl; │ │ │ │ │ -101 } │ │ │ │ │ -102 } │ │ │ │ │ -103 } │ │ │ │ │ -104 │ │ │ │ │ -105 return _e_q_u_a_l; │ │ │ │ │ -106} │ │ │ │ │ -107} // namespace internal │ │ │ │ │ -108 │ │ │ │ │ -_1_1_4#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, │ │ │ │ │ -numerical_derivative_step, tolerance) \ │ │ │ │ │ -115 { EXPECT(gtsam::internal::testFactorJacobians(name_, factor, values, │ │ │ │ │ -numerical_derivative_step, tolerance)); } │ │ │ │ │ -116 │ │ │ │ │ -117} // namespace gtsam │ │ │ │ │ -_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_._h │ │ │ │ │ -Some functions to compute numerical derivatives. │ │ │ │ │ +_8_7 bool _s_e_n_d_a_b_l_e() const override { │ │ │ │ │ +88 return false; │ │ │ │ │ +89 } │ │ │ │ │ +90 │ │ │ │ │ +91private: │ │ │ │ │ +92 │ │ │ │ │ +_9_4 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +95 template │ │ │ │ │ +96 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ +97 ar & boost::serialization::make_nvp("CustomFactor", │ │ │ │ │ +98 boost::serialization::base_object(*this)); │ │ │ │ │ +99 } │ │ │ │ │ +100}; │ │ │ │ │ +101 │ │ │ │ │ +102} │ │ │ │ │ _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_a_s_s_e_r_t___e_q_u_a_l │ │ │ │ │ -bool assert_equal(const Matrix &expected, const Matrix &actual, double tol) │ │ │ │ │ -equals with an tolerance, prints out message if unequal │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:43 │ │ │ │ │ -_g_t_s_a_m_:_:_l_i_n_e_a_r_i_z_e_N_u_m_e_r_i_c_a_l_l_y │ │ │ │ │ -JacobianFactor linearizeNumerically(const NoiseModelFactor &factor, const │ │ │ │ │ -Values &values, double delta=1e-5) │ │ │ │ │ -Linearize a nonlinear factor using numerical differentiation The benefit of │ │ │ │ │ -this method is that it do... │ │ │ │ │ -DDeeffiinniittiioonn factorTesting.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l │ │ │ │ │ -bool equal(const T &obj1, const T &obj2, double tol) │ │ │ │ │ -Call equal on the object. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:84 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ -A Gaussian factor in the squared-error form. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:91 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ -with a unique integer... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_d_i_m │ │ │ │ │ -size_t dim(Key j) const │ │ │ │ │ -Return the dimension of variable j. │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:130 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ +Aliases. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s │ │ │ │ │ +const KeyVector & keys() const │ │ │ │ │ +Access the factor's involved variable keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:140 │ │ │ │ │ +_g_t_s_a_m_:_:_C_u_s_t_o_m_F_a_c_t_o_r │ │ │ │ │ +DDeeffiinniittiioonn CustomFactor.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_C_u_s_t_o_m_F_a_c_t_o_r_:_:_u_n_w_h_i_t_e_n_e_d_E_r_r_o_r │ │ │ │ │ +Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > │ │ │ │ │ +& > H=boost::none) const override │ │ │ │ │ +Calls the errorFunction closure, which is a std::function object One can check │ │ │ │ │ +if a derivative is nee... │ │ │ │ │ +DDeeffiinniittiioonn CustomFactor.cpp:25 │ │ │ │ │ +_g_t_s_a_m_:_:_C_u_s_t_o_m_F_a_c_t_o_r_:_:_C_u_s_t_o_m_F_a_c_t_o_r │ │ │ │ │ +CustomFactor(const SharedNoiseModel &noiseModel, const KeyVector &keys, const │ │ │ │ │ +CustomErrorFunction &errorFunction) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn CustomFactor.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_C_u_s_t_o_m_F_a_c_t_o_r_:_:_C_u_s_t_o_m_F_a_c_t_o_r │ │ │ │ │ +CustomFactor()=default │ │ │ │ │ +Default Constructor for I/O. │ │ │ │ │ +_g_t_s_a_m_:_:_C_u_s_t_o_m_F_a_c_t_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn CustomFactor.h:94 │ │ │ │ │ +_g_t_s_a_m_:_:_C_u_s_t_o_m_F_a_c_t_o_r_:_:_s_e_n_d_a_b_l_e │ │ │ │ │ +bool sendable() const override │ │ │ │ │ +Mark not sendable. │ │ │ │ │ +DDeeffiinniittiioonn CustomFactor.h:87 │ │ │ │ │ +_g_t_s_a_m_:_:_C_u_s_t_o_m_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s, const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn CustomFactor.cpp:59 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r │ │ │ │ │ A nonlinear sum-of-squares factor with a zero-mean noise model implementing the │ │ │ │ │ density Templated on... │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactor.h:174 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_w_h_i_t_e_n_e_d_E_r_r_o_r │ │ │ │ │ -Vector whitenedError(const Values &c) const │ │ │ │ │ -Vector of errors, whitened This is the raw error, i.e., i.e. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.cpp:109 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r │ │ │ │ │ +NoiseModelFactor() │ │ │ │ │ +Default constructor for I/O only. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:189 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_n_o_i_s_e_M_o_d_e_l │ │ │ │ │ +const SharedNoiseModel & noiseModel() const │ │ │ │ │ +access to the noise model │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:223 │ │ │ │ │ _g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_r_e_t_r_a_c_t │ │ │ │ │ -Values retract(const VectorValues &delta) const │ │ │ │ │ -Add a delta config to current config and returns a new config. │ │ │ │ │ -DDeeffiinniittiioonn Values.cpp:99 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_z_e_r_o_V_e_c_t_o_r_s │ │ │ │ │ -VectorValues zeroVectors() const │ │ │ │ │ -Return a VectorValues of zero vectors for each variable in this Values. │ │ │ │ │ -DDeeffiinniittiioonn Values.cpp:272 │ │ │ │ │ -_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ -In Gaussian factors, the error function returns either the negative log- │ │ │ │ │ -likelihood,... │ │ │ │ │ -_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r │ │ │ │ │ -noise model to the factor, and calculates the error by asking the user to │ │ │ │ │ -implement the method │ │ │ │ │ +_V_a_l_u_e_s │ │ │ │ │ +In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ +a non-linear function... │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _f_a_c_t_o_r_T_e_s_t_i_n_g_._h │ │ │ │ │ + * _C_u_s_t_o_m_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01013.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ExtendedKalmanFilter.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearISAM.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,46 +94,32 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
ExtendedKalmanFilter.h File Reference
│ │ │ │ +
NonlinearISAM.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -

Class to perform generic Kalman Filtering using nonlinear factor graphs. │ │ │ │ -More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::ExtendedKalmanFilter< VALUE >
 This is a generic Extended Kalman Filter class implemented using nonlinear factors. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Class to perform generic Kalman Filtering using nonlinear factor graphs.

│ │ │ │ -
Author
Stephen Williams
│ │ │ │ -
│ │ │ │ -Chris Beall
│ │ │ │ +
Date
Jan 19, 2010
│ │ │ │ +
Author
Viorela Ila and Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,20 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -ExtendedKalmanFilter.h File Reference │ │ │ │ │ -Class to perform generic Kalman Filtering using nonlinear factor graphs. │ │ │ │ │ -_M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_<_ _V_A_L_U_E_ _> │ │ │ │ │ -  This is a generic Extended Kalman Filter class implemented using │ │ │ │ │ - nonlinear factors. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +NonlinearISAM.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Class to perform generic Kalman Filtering using nonlinear factor graphs. │ │ │ │ │ + Date │ │ │ │ │ + Jan 19, 2010 │ │ │ │ │ Author │ │ │ │ │ - Stephen Williams │ │ │ │ │ - Chris Beall │ │ │ │ │ + Viorela Ila and Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_._h │ │ │ │ │ + * _N_o_n_l_i_n_e_a_r_I_S_A_M_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01016_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ExpressionFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/factorTesting.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,371 +98,136 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ExpressionFactor.h
│ │ │ │ +
factorTesting.h
│ │ │ │
│ │ │ │
│ │ │ │ -
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
20#pragma once
│ │ │ │
21
│ │ │ │ -
22#include <array>
│ │ │ │ -
23#include <gtsam/config.h>
│ │ │ │ -
24#include <gtsam/base/Testable.h>
│ │ │ │ -
25#include <gtsam/nonlinear/Expression.h>
│ │ │ │ -
26#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
27#include <numeric>
│ │ │ │ +
22#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
23#include <gtsam/base/numericalDerivative.h>
│ │ │ │ +
24#include <string>
│ │ │ │ +
25#include <vector>
│ │ │ │ +
26
│ │ │ │ +
27namespace gtsam {
│ │ │ │
28
│ │ │ │ -
29namespace gtsam {
│ │ │ │ -
30
│ │ │ │ -
43template<typename T>
│ │ │ │ -
│ │ │ │ -
44class ExpressionFactor: public NoiseModelFactor {
│ │ │ │ -
45 BOOST_CONCEPT_ASSERT((IsTestable<T>));
│ │ │ │ -
46
│ │ │ │ -
47protected:
│ │ │ │ +
│ │ │ │ +
39inline JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
│ │ │ │ +
40 const Values& values,
│ │ │ │ +
41 double delta = 1e-5) {
│ │ │ │ +
42 // We will fill a vector of key/Jacobians pairs (a map would sort)
│ │ │ │ +
43 std::vector<std::pair<Key, Matrix> > jacobians;
│ │ │ │ +
44
│ │ │ │ +
45 // Get size
│ │ │ │ +
46 const Vector e = factor.whitenedError(values);
│ │ │ │ +
47 const size_t rows = e.size();
│ │ │ │
48
│ │ │ │ -
49 typedef ExpressionFactor<T> This;
│ │ │ │ -
50 static const int Dim = traits<T>::dimension;
│ │ │ │ -
51
│ │ │ │ -
52 T measured_;
│ │ │ │ -
53 Expression<T> expression_;
│ │ │ │ -
54 FastVector<int> dims_;
│ │ │ │ -
55
│ │ │ │ -
56
│ │ │ │ -
57 public:
│ │ │ │ -
58 typedef boost::shared_ptr<ExpressionFactor<T> > shared_ptr;
│ │ │ │ -
59
│ │ │ │ -
│ │ │ │ -
67 ExpressionFactor(const SharedNoiseModel& noiseModel, //
│ │ │ │ -
68 const T& measurement, const Expression<T>& expression)
│ │ │ │ -
69 : NoiseModelFactor(noiseModel), measured_(measurement) {
│ │ │ │ -
70 initialize(expression);
│ │ │ │ -
71 }
│ │ │ │ +
49 // Loop over all variables
│ │ │ │ +
50 const double one_over_2delta = 1.0 / (2.0 * delta);
│ │ │ │ +
51 for (Key key : factor) {
│ │ │ │ +
52 // Compute central differences using the values struct.
│ │ │ │ +
53 VectorValues dX = values.zeroVectors();
│ │ │ │ +
54 const size_t cols = dX.dim(key);
│ │ │ │ +
55 Matrix J = Matrix::Zero(rows, cols);
│ │ │ │ +
56 for (size_t col = 0; col < cols; ++col) {
│ │ │ │ +
57 Vector dx = Vector::Zero(cols);
│ │ │ │ +
58 dx(col) = delta;
│ │ │ │ +
59 dX[key] = dx;
│ │ │ │ +
60 Values eval_values = values.retract(dX);
│ │ │ │ +
61 const Vector left = factor.whitenedError(eval_values);
│ │ │ │ +
62 dx(col) = -delta;
│ │ │ │ +
63 dX[key] = dx;
│ │ │ │ +
64 eval_values = values.retract(dX);
│ │ │ │ +
65 const Vector right = factor.whitenedError(eval_values);
│ │ │ │ +
66 J.col(col) = (left - right) * one_over_2delta;
│ │ │ │ +
67 }
│ │ │ │ +
68 jacobians.emplace_back(key, J);
│ │ │ │ +
69 }
│ │ │ │ +
70
│ │ │ │ +
71 // Next step...return JacobianFactor
│ │ │ │ +
72 return JacobianFactor(jacobians, -e);
│ │ │ │ +
73}
│ │ │ │ +
│ │ │ │ +
74
│ │ │ │ +
75namespace internal {
│ │ │ │ +
76// CPPUnitLite-style test for linearization of a factor
│ │ │ │ +
77inline bool testFactorJacobians(const std::string& name_,
│ │ │ │ +
78 const NoiseModelFactor& factor,
│ │ │ │ +
79 const gtsam::Values& values, double delta,
│ │ │ │ +
80 double tolerance) {
│ │ │ │ +
81 // Create expected value by numerical differentiation
│ │ │ │ +
82 JacobianFactor expected = linearizeNumerically(factor, values, delta);
│ │ │ │ +
83
│ │ │ │ +
84 // Create actual value by linearize
│ │ │ │ +
85 auto actual =
│ │ │ │ +
86 boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(values));
│ │ │ │ +
87 if (!actual) return false;
│ │ │ │ +
88
│ │ │ │ +
89 // Check cast result and then equality
│ │ │ │ +
90 bool equal = assert_equal(expected, *actual, tolerance);
│ │ │ │ +
91
│ │ │ │ +
92 // if not equal, test individual jacobians:
│ │ │ │ +
93 if (!equal) {
│ │ │ │ +
94 for (size_t i = 0; i < actual->size(); i++) {
│ │ │ │ +
95 bool i_good =
│ │ │ │ +
96 assert_equal((Matrix)(expected.getA(expected.begin() + i)),
│ │ │ │ +
97 (Matrix)(actual->getA(actual->begin() + i)), tolerance);
│ │ │ │ +
98 if (!i_good) {
│ │ │ │ +
99 std::cout << "Mismatch in Jacobian " << i + 1
│ │ │ │ +
100 << " (base 1), as shown above" << std::endl;
│ │ │ │ +
101 }
│ │ │ │ +
102 }
│ │ │ │ +
103 }
│ │ │ │ +
104
│ │ │ │ +
105 return equal;
│ │ │ │ +
106}
│ │ │ │ +
107} // namespace internal
│ │ │ │ +
108
│ │ │ │ +
│ │ │ │ +
114#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \
│ │ │ │ +
115 { EXPECT(gtsam::internal::testFactorJacobians(name_, factor, values, numerical_derivative_step, tolerance)); }
│ │ │ │
│ │ │ │ -
72
│ │ │ │ -
74 ~ExpressionFactor() override {}
│ │ │ │ -
75
│ │ │ │ -
77 const T& measured() const { return measured_; }
│ │ │ │ -
78
│ │ │ │ -
│ │ │ │ -
80 void print(const std::string& s = "",
│ │ │ │ -
81 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ -
82 NoiseModelFactor::print(s, keyFormatter);
│ │ │ │ -
83 traits<T>::Print(measured_, "ExpressionFactor with measurement: ");
│ │ │ │ -
84 }
│ │ │ │ -
│ │ │ │ -
85
│ │ │ │ -
│ │ │ │ -
87 bool equals(const NonlinearFactor& f, double tol) const override {
│ │ │ │ -
88 const ExpressionFactor* p = dynamic_cast<const ExpressionFactor*>(&f);
│ │ │ │ -
89 return p && NoiseModelFactor::equals(f, tol) &&
│ │ │ │ -
90 traits<T>::Equals(measured_, p->measured_, tol) &&
│ │ │ │ -
91 dims_ == p->dims_;
│ │ │ │ -
92 }
│ │ │ │ -
│ │ │ │ -
93
│ │ │ │ -
│ │ │ │ -
99 Vector unwhitenedError(const Values& x,
│ │ │ │ -
100 boost::optional<std::vector<Matrix>&> H = boost::none) const override {
│ │ │ │ -
101 if (H) {
│ │ │ │ -
102 const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H);
│ │ │ │ -
103 // NOTE(hayk): Doing the reverse, AKA Local(measured_, value) is not correct here
│ │ │ │ -
104 // because it would use the tangent space of the measurement instead of the value.
│ │ │ │ -
105 return -traits<T>::Local(value, measured_);
│ │ │ │ -
106 } else {
│ │ │ │ -
107 const T value = expression_.value(x);
│ │ │ │ -
108 return -traits<T>::Local(value, measured_);
│ │ │ │ -
109 }
│ │ │ │ -
110 }
│ │ │ │ -
│ │ │ │ -
111
│ │ │ │ -
│ │ │ │ -
112 boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override {
│ │ │ │ -
113 // Only linearize if the factor is active
│ │ │ │ -
114 if (!active(x))
│ │ │ │ -
115 return boost::shared_ptr<JacobianFactor>();
│ │ │ │
116
│ │ │ │ -
117 // In case noise model is constrained, we need to provide a noise model
│ │ │ │ -
118 SharedDiagonal noiseModel;
│ │ │ │ -
119 if (noiseModel_ && noiseModel_->isConstrained()) {
│ │ │ │ -
120 noiseModel = boost::static_pointer_cast<noiseModel::Constrained>(
│ │ │ │ -
121 noiseModel_)->unit();
│ │ │ │ -
122 }
│ │ │ │ -
123
│ │ │ │ -
124 // Create a writeable JacobianFactor in advance
│ │ │ │ -
125 boost::shared_ptr<JacobianFactor> factor(
│ │ │ │ -
126 new JacobianFactor(keys_, dims_, Dim, noiseModel));
│ │ │ │ -
127
│ │ │ │ -
128 // Wrap keys and VerticalBlockMatrix into structure passed to expression_
│ │ │ │ -
129 VerticalBlockMatrix& Ab = factor->matrixObject();
│ │ │ │ -
130 internal::JacobianMap jacobianMap(keys_, Ab);
│ │ │ │ -
131
│ │ │ │ -
132 // Zero out Jacobian so we can simply add to it
│ │ │ │ -
133 Ab.matrix().setZero();
│ │ │ │ -
134
│ │ │ │ -
135 // Get value and Jacobians, writing directly into JacobianFactor
│ │ │ │ -
136 T value = expression_.valueAndJacobianMap(x, jacobianMap); // <<< Reverse AD happens here !
│ │ │ │ -
137
│ │ │ │ -
138 // Evaluate error and set RHS vector b
│ │ │ │ -
139 Ab(size()).col(0) = traits<T>::Local(value, measured_);
│ │ │ │ -
140
│ │ │ │ -
141 // Whiten the corresponding system, Ab already contains RHS
│ │ │ │ -
142 if (noiseModel_) {
│ │ │ │ -
143 Vector b = Ab(size()).col(0); // need b to be valid for Robust noise models
│ │ │ │ -
144 noiseModel_->WhitenSystem(Ab.matrix(), b);
│ │ │ │ -
145 }
│ │ │ │ -
146
│ │ │ │ -
147 return factor;
│ │ │ │ -
148 }
│ │ │ │ -
│ │ │ │ -
149
│ │ │ │ -
│ │ │ │ -
151 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
152 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ -
153 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
│ │ │ │ -
154 }
│ │ │ │ -
│ │ │ │ -
155
│ │ │ │ -
156protected:
│ │ │ │ -
157 ExpressionFactor() {}
│ │ │ │ -
159
│ │ │ │ -
│ │ │ │ -
161 ExpressionFactor(const SharedNoiseModel& noiseModel, const T& measurement)
│ │ │ │ -
162 : NoiseModelFactor(noiseModel), measured_(measurement) {
│ │ │ │ -
163 // Not properly initialized yet, need to call initialize
│ │ │ │ -
164 }
│ │ │ │ -
│ │ │ │ -
165
│ │ │ │ -
│ │ │ │ -
167 void initialize(const Expression<T>& expression) {
│ │ │ │ -
168 if (!noiseModel_)
│ │ │ │ -
169 throw std::invalid_argument("ExpressionFactor: no NoiseModel.");
│ │ │ │ -
170 if (noiseModel_->dim() != Dim)
│ │ │ │ -
171 throw std::invalid_argument(
│ │ │ │ -
172 "ExpressionFactor was created with a NoiseModel of incorrect dimension.");
│ │ │ │ -
173 expression_ = expression;
│ │ │ │ -
174
│ │ │ │ -
175 // Get keys and dimensions for Jacobian matrices
│ │ │ │ -
176 // An Expression is assumed unmutable, so we do this now
│ │ │ │ -
177 if (keys_.empty()) {
│ │ │ │ -
178 // This is the case when called in ExpressionFactor Constructor.
│ │ │ │ -
179 // We then take the keys from the expression in sorted order.
│ │ │ │ -
180 boost::tie(keys_, dims_) = expression_.keysAndDims();
│ │ │ │ -
181 } else {
│ │ │ │ -
182 // This happens with classes derived from BinaryExpressionFactor etc.
│ │ │ │ -
183 // In that case, the keys_ are already defined and we just need to grab
│ │ │ │ -
184 // the dimensions in the correct order.
│ │ │ │ -
185 std::map<Key, int> keyedDims;
│ │ │ │ -
186 expression_.dims(keyedDims);
│ │ │ │ -
187 for (Key key : keys_) dims_.push_back(keyedDims[key]);
│ │ │ │ -
188 }
│ │ │ │ -
189 }
│ │ │ │ -
│ │ │ │ -
190
│ │ │ │ -
│ │ │ │ -
193 virtual Expression<T> expression() const {
│ │ │ │ -
194 throw std::runtime_error("ExpressionFactor::expression not provided: cannot deserialize.");
│ │ │ │ -
195 }
│ │ │ │ -
│ │ │ │ -
196
│ │ │ │ -
197private:
│ │ │ │ -
199 template <class Archive>
│ │ │ │ -
200 void save(Archive& ar, const unsigned int /*version*/) const {
│ │ │ │ -
201 ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor);
│ │ │ │ -
202 ar << boost::serialization::make_nvp("measured_", this->measured_);
│ │ │ │ -
203 }
│ │ │ │ -
204
│ │ │ │ -
207 template <class Archive>
│ │ │ │ -
208 void load(Archive& ar, const unsigned int /*version*/) {
│ │ │ │ -
209 ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor);
│ │ │ │ -
210 ar >> boost::serialization::make_nvp("measured_", this->measured_);
│ │ │ │ -
211 this->initialize(expression());
│ │ │ │ -
212 }
│ │ │ │ -
213
│ │ │ │ -
214 // Indicate that we implement save/load separately, and be friendly to boost
│ │ │ │ -
215 BOOST_SERIALIZATION_SPLIT_MEMBER()
│ │ │ │ -
216
│ │ │ │ -
217 friend class boost::serialization::access;
│ │ │ │ -
218
│ │ │ │ -
219 // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
│ │ │ │ -
220 enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
│ │ │ │ -
221 public:
│ │ │ │ -
222 GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
│ │ │ │ -
223};
│ │ │ │ -
│ │ │ │ -
224// ExpressionFactor
│ │ │ │ -
225
│ │ │ │ -
227template <typename T>
│ │ │ │ -
228struct traits<ExpressionFactor<T> > : public Testable<ExpressionFactor<T> > {};
│ │ │ │ -
229
│ │ │ │ -
241template <typename T, typename... Args>
│ │ │ │ -
│ │ │ │ -
242class ExpressionFactorN : public ExpressionFactor<T> {
│ │ │ │ -
243public:
│ │ │ │ -
244 static const std::size_t NARY_EXPRESSION_SIZE = sizeof...(Args);
│ │ │ │ -
245 using ArrayNKeys = std::array<Key, NARY_EXPRESSION_SIZE>;
│ │ │ │ -
246
│ │ │ │ -
248 ~ExpressionFactorN() override = default;
│ │ │ │ -
249
│ │ │ │ -
250 // Don't provide backward compatible evaluateVector(), due to its problematic
│ │ │ │ -
251 // variable length of optional Jacobian arguments. Vector evaluateError(const
│ │ │ │ -
252 // Args... args,...);
│ │ │ │ -
253
│ │ │ │ -
│ │ │ │ -
256 virtual Expression<T> expression(const ArrayNKeys &keys) const {
│ │ │ │ -
257 throw std::runtime_error(
│ │ │ │ -
258 "ExpressionFactorN::expression not provided: cannot deserialize.");
│ │ │ │ -
259 }
│ │ │ │ -
│ │ │ │ -
260
│ │ │ │ -
261protected:
│ │ │ │ -
263 ExpressionFactorN() = default;
│ │ │ │ -
264
│ │ │ │ -
│ │ │ │ -
266 ExpressionFactorN(const ArrayNKeys &keys, const SharedNoiseModel &noiseModel,
│ │ │ │ -
267 const T &measurement)
│ │ │ │ -
268 : ExpressionFactor<T>(noiseModel, measurement) {
│ │ │ │ -
269 for (const auto &key : keys)
│ │ │ │ -
270 Factor::keys_.push_back(key);
│ │ │ │ -
271 }
│ │ │ │ -
│ │ │ │ -
272
│ │ │ │ -
273private:
│ │ │ │ -
275 Expression<T> expression() const override {
│ │ │ │ -
276 ArrayNKeys keys;
│ │ │ │ -
277 int idx = 0;
│ │ │ │ -
278 for (const auto &key : Factor::keys_)
│ │ │ │ -
279 keys[idx++] = key;
│ │ │ │ -
280 return expression(keys);
│ │ │ │ -
281 }
│ │ │ │ -
282
│ │ │ │ -
283 friend class boost::serialization::access;
│ │ │ │ -
284 template <class ARCHIVE>
│ │ │ │ -
285 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ -
286 ar &boost::serialization::make_nvp(
│ │ │ │ -
287 "ExpressionFactorN",
│ │ │ │ -
288 boost::serialization::base_object<ExpressionFactor<T>>(*this));
│ │ │ │ -
289 }
│ │ │ │ -
290};
│ │ │ │ -
│ │ │ │ -
292template <typename T, typename... Args>
│ │ │ │ -
│ │ │ │ -
293struct traits<ExpressionFactorN<T, Args...>>
│ │ │ │ -
294 : public Testable<ExpressionFactorN<T, Args...>> {};
│ │ │ │ -
│ │ │ │ -
295// ExpressionFactorN
│ │ │ │ -
296
│ │ │ │ -
297
│ │ │ │ -
298#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V42)
│ │ │ │ -
307template <typename T, typename A1, typename A2>
│ │ │ │ -
308class GTSAM_DEPRECATED ExpressionFactor2 : public ExpressionFactorN<T, A1, A2> {
│ │ │ │ -
309public:
│ │ │ │ -
311 ~ExpressionFactor2() override {}
│ │ │ │ -
312
│ │ │ │ -
314 Vector evaluateError(const A1 &a1, const A2 &a2,
│ │ │ │ -
315 boost::optional<Matrix &> H1 = boost::none,
│ │ │ │ -
316 boost::optional<Matrix &> H2 = boost::none) const {
│ │ │ │ -
317 Values values;
│ │ │ │ -
318 values.insert(this->keys_[0], a1);
│ │ │ │ -
319 values.insert(this->keys_[1], a2);
│ │ │ │ -
320 std::vector<Matrix> H(2);
│ │ │ │ -
321 Vector error = ExpressionFactor<T>::unwhitenedError(values, H);
│ │ │ │ -
322 if (H1) (*H1) = H[0];
│ │ │ │ -
323 if (H2) (*H2) = H[1];
│ │ │ │ -
324 return error;
│ │ │ │ -
325 }
│ │ │ │ -
326
│ │ │ │ -
329 virtual Expression<T> expression(Key key1, Key key2) const {
│ │ │ │ -
330 throw std::runtime_error(
│ │ │ │ -
331 "ExpressionFactor2::expression not provided: cannot deserialize.");
│ │ │ │ -
332 }
│ │ │ │ -
333 Expression<T>
│ │ │ │ -
334 expression(const typename ExpressionFactorN<T, A1, A2>::ArrayNKeys &keys)
│ │ │ │ -
335 const override {
│ │ │ │ -
336 return expression(keys[0], keys[1]);
│ │ │ │ -
337 }
│ │ │ │ -
338
│ │ │ │ -
339protected:
│ │ │ │ -
341 ExpressionFactor2() {}
│ │ │ │ -
342
│ │ │ │ -
344 ExpressionFactor2(Key key1, Key key2, const SharedNoiseModel &noiseModel,
│ │ │ │ -
345 const T &measurement)
│ │ │ │ -
346 : ExpressionFactorN<T, A1, A2>({key1, key2}, noiseModel, measurement) {}
│ │ │ │ -
347};
│ │ │ │ -
348// ExpressionFactor2
│ │ │ │ -
349#endif
│ │ │ │ -
350
│ │ │ │ -
351} // namespace gtsam
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ -
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
This marks a GTSAM object to require alignment.
Definition types.h:317
│ │ │ │ -
Expression.h
Expressions for Block Automatic Differentiation.
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ -
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │ +
117} // namespace gtsam
│ │ │ │ +
numericalDerivative.h
Some functions to compute numerical derivatives.
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ +
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
equals with an tolerance, prints out message if unequal
Definition Matrix.cpp:43
│ │ │ │ +
gtsam::linearizeNumerically
JacobianFactor linearizeNumerically(const NoiseModelFactor &factor, const Values &values, double delta=1e-5)
Linearize a nonlinear factor using numerical differentiation The benefit of this method is that it do...
Definition factorTesting.h:39
│ │ │ │ +
gtsam::equal
bool equal(const T &obj1, const T &obj2, double tol)
Call equal on the object.
Definition Testable.h:84
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::IsTestable
A testable concept check that should be placed in applicable unit tests and in generic algorithms.
Definition Testable.h:58
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::VerticalBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43
│ │ │ │ -
gtsam::VerticalBlockMatrix::matrix
const Matrix & matrix() const
Access to full matrix (including any portions excluded by rowStart(), rowEnd(), and firstBlock())
Definition VerticalBlockMatrix.h:188
│ │ │ │ -
gtsam::Factor
Definition Factor.h:68
│ │ │ │ -
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
│ │ │ │ -
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
│ │ │ │ -
gtsam::Factor::size
size_t size() const
Definition Factor.h:157
│ │ │ │
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition JacobianFactor.h:91
│ │ │ │ -
gtsam::ExpressionFactor
Factor that supports arbitrary expressions via AD.
Definition ExpressionFactor.h:44
│ │ │ │ -
gtsam::ExpressionFactor::unwhitenedError
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
Error function without the NoiseModel, .
Definition ExpressionFactor.h:99
│ │ │ │ -
gtsam::ExpressionFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print relies on Testable traits being defined for T
Definition ExpressionFactor.h:80
│ │ │ │ -
gtsam::ExpressionFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition ExpressionFactor.h:151
│ │ │ │ -
gtsam::ExpressionFactor::expression
virtual Expression< T > expression() const
Recreate expression from keys_ and measured_, used in load below.
Definition ExpressionFactor.h:193
│ │ │ │ -
gtsam::ExpressionFactor::expression_
Expression< T > expression_
the expression that is AD enabled
Definition ExpressionFactor.h:53
│ │ │ │ -
gtsam::ExpressionFactor::measured_
T measured_
the measurement to be compared with the expression
Definition ExpressionFactor.h:52
│ │ │ │ -
gtsam::ExpressionFactor::dims_
FastVector< int > dims_
dimensions of the Jacobian matrices
Definition ExpressionFactor.h:54
│ │ │ │ -
gtsam::ExpressionFactor::measured
const T & measured() const
return the measurement
Definition ExpressionFactor.h:77
│ │ │ │ -
gtsam::ExpressionFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override
linearize to a GaussianFactor
Definition ExpressionFactor.h:112
│ │ │ │ -
gtsam::ExpressionFactor::~ExpressionFactor
~ExpressionFactor() override
Destructor.
Definition ExpressionFactor.h:74
│ │ │ │ -
gtsam::ExpressionFactor::equals
bool equals(const NonlinearFactor &f, double tol) const override
equals relies on Testable traits being defined for T
Definition ExpressionFactor.h:87
│ │ │ │ -
gtsam::ExpressionFactor::ExpressionFactor
ExpressionFactor(const SharedNoiseModel &noiseModel, const T &measurement)
Default constructor, for serialization.
Definition ExpressionFactor.h:161
│ │ │ │ -
gtsam::ExpressionFactor::ExpressionFactor
ExpressionFactor(const SharedNoiseModel &noiseModel, const T &measurement, const Expression< T > &expression)
Constructor: creates a factor from a measurement and measurement function.
Definition ExpressionFactor.h:67
│ │ │ │ -
gtsam::ExpressionFactor::initialize
void initialize(const Expression< T > &expression)
Initialize with constructor arguments.
Definition ExpressionFactor.h:167
│ │ │ │ -
gtsam::Expression
Expression class that supports automatic differentiation.
Definition Expression.h:48
│ │ │ │ -
gtsam::ExpressionFactorN
N-ary variadic template for ExpressionFactor meant as a base class for N-ary factors.
Definition ExpressionFactor.h:242
│ │ │ │ -
gtsam::ExpressionFactorN::expression
virtual Expression< T > expression(const ArrayNKeys &keys) const
Recreate expression from given keys_ and measured_, used in load Needed to deserialize a derived fact...
Definition ExpressionFactor.h:256
│ │ │ │ -
gtsam::ExpressionFactorN::~ExpressionFactorN
~ExpressionFactorN() override=default
Destructor.
│ │ │ │ -
gtsam::ExpressionFactorN::ExpressionFactorN
ExpressionFactorN()=default
Default constructor, for serialization.
│ │ │ │ -
gtsam::ExpressionFactorN::ExpressionFactorN
ExpressionFactorN(const ArrayNKeys &keys, const SharedNoiseModel &noiseModel, const T &measurement)
Constructor takes care of keys, but still need to call initialize.
Definition ExpressionFactor.h:266
│ │ │ │ -
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ -
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition NonlinearFactor.h:118
│ │ │ │ +
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
gtsam::VectorValues::dim
size_t dim(Key j) const
Return the dimension of variable j.
Definition VectorValues.h:130
│ │ │ │
gtsam::NoiseModelFactor
A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
Definition NonlinearFactor.h:174
│ │ │ │ -
gtsam::NoiseModelFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition NonlinearFactor.cpp:74
│ │ │ │ -
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Check if two factors are equal.
Definition NonlinearFactor.cpp:82
│ │ │ │ -
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition NonlinearFactor.h:223
│ │ │ │ +
gtsam::NoiseModelFactor::whitenedError
Vector whitenedError(const Values &c) const
Vector of errors, whitened This is the raw error, i.e., i.e.
Definition NonlinearFactor.cpp:109
│ │ │ │
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │ +
gtsam::Values::retract
Values retract(const VectorValues &delta) const
Add a delta config to current config and returns a new config.
Definition Values.cpp:99
│ │ │ │ +
gtsam::Values::zeroVectors
VectorValues zeroVectors() const
Return a VectorValues of zero vectors for each variable in this Values.
Definition Values.cpp:272
│ │ │ │ +
JacobianFactor
In Gaussian factors, the error function returns either the negative log-likelihood,...
│ │ │ │
NoiseModelFactor
noise model to the factor, and calculates the error by asking the user to implement the method
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,14 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ExpressionFactor.h │ │ │ │ │ +factorTesting.h │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ @@ -15,466 +16,155 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 20#pragma once │ │ │ │ │ 21 │ │ │ │ │ -22#include │ │ │ │ │ -23#include │ │ │ │ │ -24#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_E_x_p_r_e_s_s_i_o_n_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -27#include │ │ │ │ │ +22#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_b_a_s_e_/_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_._h> │ │ │ │ │ +24#include │ │ │ │ │ +25#include │ │ │ │ │ +26 │ │ │ │ │ +27namespace _g_t_s_a_m { │ │ │ │ │ 28 │ │ │ │ │ -29namespace _g_t_s_a_m { │ │ │ │ │ -30 │ │ │ │ │ -43template │ │ │ │ │ -_4_4class _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r { │ │ │ │ │ -45 BOOST_CONCEPT_ASSERT((_I_s_T_e_s_t_a_b_l_e_<_T_>)); │ │ │ │ │ -46 │ │ │ │ │ -47protected: │ │ │ │ │ +_3_9inline _J_a_c_o_b_i_a_n_F_a_c_t_o_r _l_i_n_e_a_r_i_z_e_N_u_m_e_r_i_c_a_l_l_y(const _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r& factor, │ │ │ │ │ +40 const _V_a_l_u_e_s& values, │ │ │ │ │ +41 double delta = 1e-5) { │ │ │ │ │ +42 // We will fill a vector of key/Jacobians pairs (a map would sort) │ │ │ │ │ +43 std::vector > jacobians; │ │ │ │ │ +44 │ │ │ │ │ +45 // Get size │ │ │ │ │ +46 const Vector e = factor._w_h_i_t_e_n_e_d_E_r_r_o_r(values); │ │ │ │ │ +47 const size_t rows = e.size(); │ │ │ │ │ 48 │ │ │ │ │ -49 typedef _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_<_T_> _T_h_i_s; │ │ │ │ │ -50 static const int Dim = _t_r_a_i_t_s_<_T_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ -51 │ │ │ │ │ -_5_2 T _m_e_a_s_u_r_e_d__; │ │ │ │ │ -_5_3 _E_x_p_r_e_s_s_i_o_n_<_T_> _e_x_p_r_e_s_s_i_o_n__; │ │ │ │ │ -_5_4 _F_a_s_t_V_e_c_t_o_r_<_i_n_t_> _d_i_m_s__; │ │ │ │ │ -55 │ │ │ │ │ -56 │ │ │ │ │ -57 public: │ │ │ │ │ -58 typedef boost::shared_ptr > shared_ptr; │ │ │ │ │ -59 │ │ │ │ │ -_6_7 _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r(const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& _n_o_i_s_e_M_o_d_e_l, // │ │ │ │ │ -68 const T& measurement, const _E_x_p_r_e_s_s_i_o_n_<_T_>& _e_x_p_r_e_s_s_i_o_n) │ │ │ │ │ -69 : _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r(_n_o_i_s_e_M_o_d_e_l), _m_e_a_s_u_r_e_d__(measurement) { │ │ │ │ │ -70 _i_n_i_t_i_a_l_i_z_e(_e_x_p_r_e_s_s_i_o_n); │ │ │ │ │ -71 } │ │ │ │ │ -72 │ │ │ │ │ -_7_4 _~_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r() override {} │ │ │ │ │ -75 │ │ │ │ │ -_7_7 const T& _m_e_a_s_u_r_e_d() const { return _m_e_a_s_u_r_e_d__; } │ │ │ │ │ -78 │ │ │ │ │ -_8_0 void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ -81 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ -82 _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_p_r_i_n_t(s, keyFormatter); │ │ │ │ │ -83 _t_r_a_i_t_s_<_T_>_:_:_P_r_i_n_t(_m_e_a_s_u_r_e_d__, "ExpressionFactor with measurement: "); │ │ │ │ │ -84 } │ │ │ │ │ -85 │ │ │ │ │ -_8_7 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& f, double tol) const override { │ │ │ │ │ -88 const _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r* p = dynamic_cast(&f); │ │ │ │ │ -89 return p && _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_e_q_u_a_l_s(f, tol) && │ │ │ │ │ -90 _t_r_a_i_t_s_<_T_>_:_:_E_q_u_a_l_s(_m_e_a_s_u_r_e_d__, p->_m_e_a_s_u_r_e_d__, tol) && │ │ │ │ │ -91 _d_i_m_s__ == p->_d_i_m_s__; │ │ │ │ │ -92 } │ │ │ │ │ -93 │ │ │ │ │ -_9_9 Vector _u_n_w_h_i_t_e_n_e_d_E_r_r_o_r(const _V_a_l_u_e_s& x, │ │ │ │ │ -100 boost::optional&> H = boost::none) const override { │ │ │ │ │ -101 if (H) { │ │ │ │ │ -102 const T value = _e_x_p_r_e_s_s_i_o_n__.valueAndDerivatives(x, _k_e_y_s__, _d_i_m_s__, *H); │ │ │ │ │ -103 // NOTE(hayk): Doing the reverse, AKA Local(measured_, value) is not │ │ │ │ │ -correct here │ │ │ │ │ -104 // because it would use the tangent space of the measurement instead of the │ │ │ │ │ -value. │ │ │ │ │ -105 return -_t_r_a_i_t_s_<_T_>_:_:_L_o_c_a_l(value, _m_e_a_s_u_r_e_d__); │ │ │ │ │ -106 } else { │ │ │ │ │ -107 const T value = _e_x_p_r_e_s_s_i_o_n__.value(x); │ │ │ │ │ -108 return -_t_r_a_i_t_s_<_T_>_:_:_L_o_c_a_l(value, _m_e_a_s_u_r_e_d__); │ │ │ │ │ -109 } │ │ │ │ │ -110 } │ │ │ │ │ -111 │ │ │ │ │ -_1_1_2 boost::shared_ptr _l_i_n_e_a_r_i_z_e(const _V_a_l_u_e_s& x) const override │ │ │ │ │ -{ │ │ │ │ │ -113 // Only linearize if the factor is active │ │ │ │ │ -114 if (!_a_c_t_i_v_e(x)) │ │ │ │ │ -115 return boost::shared_ptr(); │ │ │ │ │ +49 // Loop over all variables │ │ │ │ │ +50 const double one_over_2delta = 1.0 / (2.0 * delta); │ │ │ │ │ +51 for (_K_e_y key : factor) { │ │ │ │ │ +52 // Compute central differences using the values struct. │ │ │ │ │ +53 _V_e_c_t_o_r_V_a_l_u_e_s dX = values._z_e_r_o_V_e_c_t_o_r_s(); │ │ │ │ │ +54 const size_t cols = dX._d_i_m(key); │ │ │ │ │ +55 Matrix J = Matrix::Zero(rows, cols); │ │ │ │ │ +56 for (size_t col = 0; col < cols; ++col) { │ │ │ │ │ +57 Vector dx = Vector::Zero(cols); │ │ │ │ │ +58 dx(col) = delta; │ │ │ │ │ +59 dX[key] = dx; │ │ │ │ │ +60 _V_a_l_u_e_s eval_values = values._r_e_t_r_a_c_t(dX); │ │ │ │ │ +61 const Vector left = factor._w_h_i_t_e_n_e_d_E_r_r_o_r(eval_values); │ │ │ │ │ +62 dx(col) = -delta; │ │ │ │ │ +63 dX[key] = dx; │ │ │ │ │ +64 eval_values = values._r_e_t_r_a_c_t(dX); │ │ │ │ │ +65 const Vector right = factor._w_h_i_t_e_n_e_d_E_r_r_o_r(eval_values); │ │ │ │ │ +66 J.col(col) = (left - right) * one_over_2delta; │ │ │ │ │ +67 } │ │ │ │ │ +68 jacobians.emplace_back(key, J); │ │ │ │ │ +69 } │ │ │ │ │ +70 │ │ │ │ │ +71 // Next step...return JacobianFactor │ │ │ │ │ +72 return _J_a_c_o_b_i_a_n_F_a_c_t_o_r(jacobians, -e); │ │ │ │ │ +73} │ │ │ │ │ +74 │ │ │ │ │ +75namespace internal { │ │ │ │ │ +76// CPPUnitLite-style test for linearization of a factor │ │ │ │ │ +77inline bool testFactorJacobians(const std::string& name_, │ │ │ │ │ +78 const _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r& factor, │ │ │ │ │ +79 const _g_t_s_a_m_:_:_V_a_l_u_e_s& values, double delta, │ │ │ │ │ +80 double tolerance) { │ │ │ │ │ +81 // Create expected value by numerical differentiation │ │ │ │ │ +82 _J_a_c_o_b_i_a_n_F_a_c_t_o_r expected = _l_i_n_e_a_r_i_z_e_N_u_m_e_r_i_c_a_l_l_y(factor, values, delta); │ │ │ │ │ +83 │ │ │ │ │ +84 // Create actual value by linearize │ │ │ │ │ +85 auto actual = │ │ │ │ │ +86 boost::dynamic_pointer_cast(factor.linearize(values)); │ │ │ │ │ +87 if (!actual) return false; │ │ │ │ │ +88 │ │ │ │ │ +89 // Check cast result and then equality │ │ │ │ │ +90 bool _e_q_u_a_l = _a_s_s_e_r_t___e_q_u_a_l(expected, *actual, tolerance); │ │ │ │ │ +91 │ │ │ │ │ +92 // if not equal, test individual jacobians: │ │ │ │ │ +93 if (!_e_q_u_a_l) { │ │ │ │ │ +94 for (size_t i = 0; i < actual->size(); i++) { │ │ │ │ │ +95 bool i_good = │ │ │ │ │ +96 _a_s_s_e_r_t___e_q_u_a_l((Matrix)(expected.getA(expected.begin() + i)), │ │ │ │ │ +97 (Matrix)(actual->getA(actual->begin() + i)), tolerance); │ │ │ │ │ +98 if (!i_good) { │ │ │ │ │ +99 std::cout << "Mismatch in Jacobian " << i + 1 │ │ │ │ │ +100 << " (base 1), as shown above" << std::endl; │ │ │ │ │ +101 } │ │ │ │ │ +102 } │ │ │ │ │ +103 } │ │ │ │ │ +104 │ │ │ │ │ +105 return _e_q_u_a_l; │ │ │ │ │ +106} │ │ │ │ │ +107} // namespace internal │ │ │ │ │ +108 │ │ │ │ │ +_1_1_4#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, │ │ │ │ │ +numerical_derivative_step, tolerance) \ │ │ │ │ │ +115 { EXPECT(gtsam::internal::testFactorJacobians(name_, factor, values, │ │ │ │ │ +numerical_derivative_step, tolerance)); } │ │ │ │ │ 116 │ │ │ │ │ -117 // In case noise model is constrained, we need to provide a noise model │ │ │ │ │ -118 SharedDiagonal _n_o_i_s_e_M_o_d_e_l; │ │ │ │ │ -119 if (noiseModel_ && noiseModel_->isConstrained()) { │ │ │ │ │ -120 _n_o_i_s_e_M_o_d_e_l = boost::static_pointer_cast( │ │ │ │ │ -121 noiseModel_)->unit(); │ │ │ │ │ -122 } │ │ │ │ │ -123 │ │ │ │ │ -124 // Create a writeable JacobianFactor in advance │ │ │ │ │ -125 boost::shared_ptr factor( │ │ │ │ │ -126 new _J_a_c_o_b_i_a_n_F_a_c_t_o_r(_k_e_y_s__, _d_i_m_s__, Dim, _n_o_i_s_e_M_o_d_e_l)); │ │ │ │ │ -127 │ │ │ │ │ -128 // Wrap keys and VerticalBlockMatrix into structure passed to expression_ │ │ │ │ │ -129 _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x& Ab = factor->matrixObject(); │ │ │ │ │ -130 internal::JacobianMap jacobianMap(_k_e_y_s__, Ab); │ │ │ │ │ -131 │ │ │ │ │ -132 // Zero out Jacobian so we can simply add to it │ │ │ │ │ -133 Ab._m_a_t_r_i_x().setZero(); │ │ │ │ │ -134 │ │ │ │ │ -135 // Get value and Jacobians, writing directly into JacobianFactor │ │ │ │ │ -136 T value = _e_x_p_r_e_s_s_i_o_n__.valueAndJacobianMap(x, jacobianMap); // <<< Reverse │ │ │ │ │ -AD happens here ! │ │ │ │ │ -137 │ │ │ │ │ -138 // Evaluate error and set RHS vector b │ │ │ │ │ -139 Ab(_s_i_z_e()).col(0) = _t_r_a_i_t_s_<_T_>_:_:_L_o_c_a_l(value, _m_e_a_s_u_r_e_d__); │ │ │ │ │ -140 │ │ │ │ │ -141 // Whiten the corresponding system, Ab already contains RHS │ │ │ │ │ -142 if (noiseModel_) { │ │ │ │ │ -143 Vector b = Ab(_s_i_z_e()).col(0); // need b to be valid for Robust noise models │ │ │ │ │ -144 noiseModel_->WhitenSystem(Ab._m_a_t_r_i_x(), b); │ │ │ │ │ -145 } │ │ │ │ │ -146 │ │ │ │ │ -147 return factor; │ │ │ │ │ -148 } │ │ │ │ │ -149 │ │ │ │ │ -_1_5_1 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -152 return boost::static_pointer_cast( │ │ │ │ │ -153 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); │ │ │ │ │ -154 } │ │ │ │ │ -155 │ │ │ │ │ -156protected: │ │ │ │ │ -157 _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r() {} │ │ │ │ │ -159 │ │ │ │ │ -_1_6_1 _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r(const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& _n_o_i_s_e_M_o_d_e_l, const T& measurement) │ │ │ │ │ -162 : _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r(_n_o_i_s_e_M_o_d_e_l), _m_e_a_s_u_r_e_d__(measurement) { │ │ │ │ │ -163 // Not properly initialized yet, need to call initialize │ │ │ │ │ -164 } │ │ │ │ │ -165 │ │ │ │ │ -_1_6_7 void _i_n_i_t_i_a_l_i_z_e(const _E_x_p_r_e_s_s_i_o_n_<_T_>& _e_x_p_r_e_s_s_i_o_n) { │ │ │ │ │ -168 if (!noiseModel_) │ │ │ │ │ -169 throw std::invalid_argument("ExpressionFactor: no NoiseModel."); │ │ │ │ │ -170 if (noiseModel_->dim() != Dim) │ │ │ │ │ -171 throw std::invalid_argument( │ │ │ │ │ -172 "ExpressionFactor was created with a NoiseModel of incorrect dimension."); │ │ │ │ │ -173 _e_x_p_r_e_s_s_i_o_n__ = _e_x_p_r_e_s_s_i_o_n; │ │ │ │ │ -174 │ │ │ │ │ -175 // Get keys and dimensions for Jacobian matrices │ │ │ │ │ -176 // An Expression is assumed unmutable, so we do this now │ │ │ │ │ -177 if (_k_e_y_s__.empty()) { │ │ │ │ │ -178 // This is the case when called in ExpressionFactor Constructor. │ │ │ │ │ -179 // We then take the keys from the expression in sorted order. │ │ │ │ │ -180 boost::tie(_k_e_y_s__, _d_i_m_s__) = _e_x_p_r_e_s_s_i_o_n__.keysAndDims(); │ │ │ │ │ -181 } else { │ │ │ │ │ -182 // This happens with classes derived from BinaryExpressionFactor etc. │ │ │ │ │ -183 // In that case, the keys_ are already defined and we just need to grab │ │ │ │ │ -184 // the dimensions in the correct order. │ │ │ │ │ -185 std::map keyedDims; │ │ │ │ │ -186 _e_x_p_r_e_s_s_i_o_n__.dims(keyedDims); │ │ │ │ │ -187 for (_K_e_y key : _k_e_y_s__) _d_i_m_s__.push_back(keyedDims[key]); │ │ │ │ │ -188 } │ │ │ │ │ -189 } │ │ │ │ │ -190 │ │ │ │ │ -_1_9_3 virtual _E_x_p_r_e_s_s_i_o_n_<_T_> _e_x_p_r_e_s_s_i_o_n() const { │ │ │ │ │ -194 throw std::runtime_error("ExpressionFactor::expression not provided: cannot │ │ │ │ │ -deserialize."); │ │ │ │ │ -195 } │ │ │ │ │ -196 │ │ │ │ │ -197private: │ │ │ │ │ -199 template │ │ │ │ │ -200 void save(Archive& ar, const unsigned int /*version*/) const { │ │ │ │ │ -201 ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r); │ │ │ │ │ -202 ar << boost::serialization::make_nvp("measured_", this->measured_); │ │ │ │ │ -203 } │ │ │ │ │ -204 │ │ │ │ │ -207 template │ │ │ │ │ -208 void load(Archive& ar, const unsigned int /*version*/) { │ │ │ │ │ -209 ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r); │ │ │ │ │ -210 ar >> boost::serialization::make_nvp("measured_", this->measured_); │ │ │ │ │ -211 this->initialize(_e_x_p_r_e_s_s_i_o_n()); │ │ │ │ │ -212 } │ │ │ │ │ -213 │ │ │ │ │ -214 // Indicate that we implement save/load separately, and be friendly to │ │ │ │ │ -boost │ │ │ │ │ -215 BOOST_SERIALIZATION_SPLIT_MEMBER() │ │ │ │ │ -216 │ │ │ │ │ -217 friend class boost::serialization::access; │ │ │ │ │ -218 │ │ │ │ │ -219 // Alignment, see https://eigen.tuxfamily.org/dox/ │ │ │ │ │ -group__TopicStructHavingEigenMembers.html │ │ │ │ │ -220 enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; │ │ │ │ │ -221 public: │ │ │ │ │ -222 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W___I_F(NeedsToAlign) │ │ │ │ │ -223}; │ │ │ │ │ -224// ExpressionFactor │ │ │ │ │ -225 │ │ │ │ │ -227template │ │ │ │ │ -_2_2_8struct _t_r_a_i_t_s<_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r > : public _T_e_s_t_a_b_l_e > │ │ │ │ │ -{}; │ │ │ │ │ -229 │ │ │ │ │ -241template │ │ │ │ │ -_2_4_2class _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_N : public _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r { │ │ │ │ │ -243public: │ │ │ │ │ -244 static const std::size_t NARY_EXPRESSION_SIZE = sizeof...(Args); │ │ │ │ │ -245 using ArrayNKeys = std::array; │ │ │ │ │ -246 │ │ │ │ │ -_2_4_8 _~_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_N() override = default; │ │ │ │ │ -249 │ │ │ │ │ -250 // Don't provide backward compatible evaluateVector(), due to its │ │ │ │ │ -problematic │ │ │ │ │ -251 // variable length of optional Jacobian arguments. Vector evaluateError │ │ │ │ │ -(const │ │ │ │ │ -252 // Args... args,...); │ │ │ │ │ -253 │ │ │ │ │ -_2_5_6 virtual _E_x_p_r_e_s_s_i_o_n_<_T_> _e_x_p_r_e_s_s_i_o_n(const ArrayNKeys &_k_e_y_s) const { │ │ │ │ │ -257 throw std::runtime_error( │ │ │ │ │ -258 "ExpressionFactorN::expression not provided: cannot deserialize."); │ │ │ │ │ -259 } │ │ │ │ │ -260 │ │ │ │ │ -261protected: │ │ │ │ │ -_2_6_3 _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_N() = default; │ │ │ │ │ -264 │ │ │ │ │ -_2_6_6 _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_N(const ArrayNKeys &_k_e_y_s, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -&noiseModel, │ │ │ │ │ -267 const T &measurement) │ │ │ │ │ -268 : _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r(noiseModel, measurement) { │ │ │ │ │ -269 for (const auto &key : _k_e_y_s) │ │ │ │ │ -270 _F_a_c_t_o_r_:_:_k_e_y_s__.push_back(key); │ │ │ │ │ -271 } │ │ │ │ │ -272 │ │ │ │ │ -273private: │ │ │ │ │ -275 _E_x_p_r_e_s_s_i_o_n_<_T_> expression() const override { │ │ │ │ │ -276 ArrayNKeys _k_e_y_s; │ │ │ │ │ -277 int idx = 0; │ │ │ │ │ -278 for (const auto &key : _F_a_c_t_o_r::_k_e_y_s__) │ │ │ │ │ -279 _k_e_y_s[idx++] = key; │ │ │ │ │ -280 return expression(_k_e_y_s); │ │ │ │ │ -281 } │ │ │ │ │ -282 │ │ │ │ │ -283 friend class boost::serialization::access; │ │ │ │ │ -284 template │ │ │ │ │ -285 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ -286 ar &boost::serialization::make_nvp( │ │ │ │ │ -287 "ExpressionFactorN", │ │ │ │ │ -288 boost::serialization::base_object>(*this)); │ │ │ │ │ -289 } │ │ │ │ │ -290}; │ │ │ │ │ -292template │ │ │ │ │ -_2_9_3struct _t_r_a_i_t_s<_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_N> │ │ │ │ │ -294 : public _T_e_s_t_a_b_l_e> {}; │ │ │ │ │ -295// ExpressionFactorN │ │ │ │ │ -296 │ │ │ │ │ -297 │ │ │ │ │ -298#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V42) │ │ │ │ │ -307template │ │ │ │ │ -308class GTSAM_DEPRECATED ExpressionFactor2 : public _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_N { │ │ │ │ │ -309public: │ │ │ │ │ -311 ~ExpressionFactor2() override {} │ │ │ │ │ -312 │ │ │ │ │ -314 Vector evaluateError(const A1 &a1, const A2 &a2, │ │ │ │ │ -315 boost::optional H1 = boost::none, │ │ │ │ │ -316 boost::optional H2 = boost::none) const { │ │ │ │ │ -317 _V_a_l_u_e_s values; │ │ │ │ │ -318 values.insert(this->keys_[0], a1); │ │ │ │ │ -319 values.insert(this->keys_[1], a2); │ │ │ │ │ -320 std::vector H(2); │ │ │ │ │ -321 Vector error = ExpressionFactor::unwhitenedError(values, H); │ │ │ │ │ -322 if (H1) (*H1) = H[0]; │ │ │ │ │ -323 if (H2) (*H2) = H[1]; │ │ │ │ │ -324 return error; │ │ │ │ │ -325 } │ │ │ │ │ -326 │ │ │ │ │ -329 virtual Expression expression(Key key1, Key key2) const { │ │ │ │ │ -330 throw std::runtime_error( │ │ │ │ │ -331 "ExpressionFactor2::expression not provided: cannot deserialize."); │ │ │ │ │ -332 } │ │ │ │ │ -333 Expression │ │ │ │ │ -334 expression(const typename ExpressionFactorN::ArrayNKeys &keys) │ │ │ │ │ -335 const override { │ │ │ │ │ -336 return expression(keys[0], keys[1]); │ │ │ │ │ -337 } │ │ │ │ │ -338 │ │ │ │ │ -339protected: │ │ │ │ │ -341 ExpressionFactor2() {} │ │ │ │ │ -342 │ │ │ │ │ -344 ExpressionFactor2(Key key1, Key key2, const SharedNoiseModel &noiseModel, │ │ │ │ │ -345 const T &measurement) │ │ │ │ │ -346 : ExpressionFactorN({key1, key2}, noiseModel, measurement) {} │ │ │ │ │ -347}; │ │ │ │ │ -348// ExpressionFactor2 │ │ │ │ │ -349#endif │ │ │ │ │ -350 │ │ │ │ │ -351} // namespace gtsam │ │ │ │ │ -_T_e_s_t_a_b_l_e_._h │ │ │ │ │ -Concept check for values that can be used in unit tests. │ │ │ │ │ -_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W___I_F │ │ │ │ │ -#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) │ │ │ │ │ -This marks a GTSAM object to require alignment. │ │ │ │ │ -DDeeffiinniittiioonn types.h:317 │ │ │ │ │ -_E_x_p_r_e_s_s_i_o_n_._h │ │ │ │ │ -Expressions for Block Automatic Differentiation. │ │ │ │ │ +117} // namespace gtsam │ │ │ │ │ +_n_u_m_e_r_i_c_a_l_D_e_r_i_v_a_t_i_v_e_._h │ │ │ │ │ +Some functions to compute numerical derivatives. │ │ │ │ │ _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ Non-linear factor base classes. │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ -std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ -FastVector │ │ │ │ │ -FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ -DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ -Aliases. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ +_g_t_s_a_m_:_:_a_s_s_e_r_t___e_q_u_a_l │ │ │ │ │ +bool assert_equal(const Matrix &expected, const Matrix &actual, double tol) │ │ │ │ │ +equals with an tolerance, prints out message if unequal │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:43 │ │ │ │ │ +_g_t_s_a_m_:_:_l_i_n_e_a_r_i_z_e_N_u_m_e_r_i_c_a_l_l_y │ │ │ │ │ +JacobianFactor linearizeNumerically(const NoiseModelFactor &factor, const │ │ │ │ │ +Values &values, double delta=1e-5) │ │ │ │ │ +Linearize a nonlinear factor using numerical differentiation The benefit of │ │ │ │ │ +this method is that it do... │ │ │ │ │ +DDeeffiinniittiioonn factorTesting.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l │ │ │ │ │ +bool equal(const T &obj1, const T &obj2, double tol) │ │ │ │ │ +Call equal on the object. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:84 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_I_s_T_e_s_t_a_b_l_e │ │ │ │ │ -A testable concept check that should be placed in applicable unit tests and in │ │ │ │ │ -generic algorithms. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ -of vertical blocks. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_m_a_t_r_i_x │ │ │ │ │ -const Matrix & matrix() const │ │ │ │ │ -Access to full matrix (including any portions excluded by rowStart(), rowEnd(), │ │ │ │ │ -and firstBlock()) │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:188 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:68 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s │ │ │ │ │ -const KeyVector & keys() const │ │ │ │ │ -Access the factor's involved variable keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:140 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s__ │ │ │ │ │ -KeyVector keys_ │ │ │ │ │ -The keys involved in this factor. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_s_i_z_e │ │ │ │ │ -size_t size() const │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:157 │ │ │ │ │ _g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ A Gaussian factor in the squared-error form. │ │ │ │ │ DDeeffiinniittiioonn JacobianFactor.h:91 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r │ │ │ │ │ -Factor that supports arbitrary expressions via AD. │ │ │ │ │ -DDeeffiinniittiioonn ExpressionFactor.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_:_:_u_n_w_h_i_t_e_n_e_d_E_r_r_o_r │ │ │ │ │ -Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > │ │ │ │ │ -& > H=boost::none) const override │ │ │ │ │ -Error function without the NoiseModel, . │ │ │ │ │ -DDeeffiinniittiioonn ExpressionFactor.h:99 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -print relies on Testable traits being defined for T │ │ │ │ │ -DDeeffiinniittiioonn ExpressionFactor.h:80 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ -gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn ExpressionFactor.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_:_:_e_x_p_r_e_s_s_i_o_n │ │ │ │ │ -virtual Expression< T > expression() const │ │ │ │ │ -Recreate expression from keys_ and measured_, used in load below. │ │ │ │ │ -DDeeffiinniittiioonn ExpressionFactor.h:193 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_:_:_e_x_p_r_e_s_s_i_o_n__ │ │ │ │ │ -Expression< T > expression_ │ │ │ │ │ -the expression that is AD enabled │ │ │ │ │ -DDeeffiinniittiioonn ExpressionFactor.h:53 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_:_:_m_e_a_s_u_r_e_d__ │ │ │ │ │ -T measured_ │ │ │ │ │ -the measurement to be compared with the expression │ │ │ │ │ -DDeeffiinniittiioonn ExpressionFactor.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_:_:_d_i_m_s__ │ │ │ │ │ -FastVector< int > dims_ │ │ │ │ │ -dimensions of the Jacobian matrices │ │ │ │ │ -DDeeffiinniittiioonn ExpressionFactor.h:54 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_:_:_m_e_a_s_u_r_e_d │ │ │ │ │ -const T & measured() const │ │ │ │ │ -return the measurement │ │ │ │ │ -DDeeffiinniittiioonn ExpressionFactor.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e │ │ │ │ │ -boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override │ │ │ │ │ -linearize to a GaussianFactor │ │ │ │ │ -DDeeffiinniittiioonn ExpressionFactor.h:112 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_:_:_~_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r │ │ │ │ │ -~ExpressionFactor() override │ │ │ │ │ -Destructor. │ │ │ │ │ -DDeeffiinniittiioonn ExpressionFactor.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const NonlinearFactor &f, double tol) const override │ │ │ │ │ -equals relies on Testable traits being defined for T │ │ │ │ │ -DDeeffiinniittiioonn ExpressionFactor.h:87 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r │ │ │ │ │ -ExpressionFactor(const SharedNoiseModel &noiseModel, const T &measurement) │ │ │ │ │ -Default constructor, for serialization. │ │ │ │ │ -DDeeffiinniittiioonn ExpressionFactor.h:161 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r │ │ │ │ │ -ExpressionFactor(const SharedNoiseModel &noiseModel, const T &measurement, │ │ │ │ │ -const Expression< T > &expression) │ │ │ │ │ -Constructor: creates a factor from a measurement and measurement function. │ │ │ │ │ -DDeeffiinniittiioonn ExpressionFactor.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_:_:_i_n_i_t_i_a_l_i_z_e │ │ │ │ │ -void initialize(const Expression< T > &expression) │ │ │ │ │ -Initialize with constructor arguments. │ │ │ │ │ -DDeeffiinniittiioonn ExpressionFactor.h:167 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n │ │ │ │ │ -Expression class that supports automatic differentiation. │ │ │ │ │ -DDeeffiinniittiioonn Expression.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_N │ │ │ │ │ -N-ary variadic template for ExpressionFactor meant as a base class for N-ary │ │ │ │ │ -factors. │ │ │ │ │ -DDeeffiinniittiioonn ExpressionFactor.h:242 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_N_:_:_e_x_p_r_e_s_s_i_o_n │ │ │ │ │ -virtual Expression< T > expression(const ArrayNKeys &keys) const │ │ │ │ │ -Recreate expression from given keys_ and measured_, used in load Needed to │ │ │ │ │ -deserialize a derived fact... │ │ │ │ │ -DDeeffiinniittiioonn ExpressionFactor.h:256 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_N_:_:_~_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_N │ │ │ │ │ -~ExpressionFactorN() override=default │ │ │ │ │ -Destructor. │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_N_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_N │ │ │ │ │ -ExpressionFactorN()=default │ │ │ │ │ -Default constructor, for serialization. │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_N_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_N │ │ │ │ │ -ExpressionFactorN(const ArrayNKeys &keys, const SharedNoiseModel &noiseModel, │ │ │ │ │ -const T &measurement) │ │ │ │ │ -Constructor takes care of keys, but still need to call initialize. │ │ │ │ │ -DDeeffiinniittiioonn ExpressionFactor.h:266 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ -Nonlinear factor base class. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_:_:_a_c_t_i_v_e │ │ │ │ │ -virtual bool active(const Values &) const │ │ │ │ │ -Checks whether a factor should be used based on a set of values. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:118 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ +with a unique integer... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_d_i_m │ │ │ │ │ +size_t dim(Key j) const │ │ │ │ │ +Return the dimension of variable j. │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:130 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r │ │ │ │ │ A nonlinear sum-of-squares factor with a zero-mean noise model implementing the │ │ │ │ │ density Templated on... │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactor.h:174 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -Print. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.cpp:74 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const NonlinearFactor &f, double tol=1e-9) const override │ │ │ │ │ -Check if two factors are equal. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.cpp:82 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_n_o_i_s_e_M_o_d_e_l │ │ │ │ │ -const SharedNoiseModel & noiseModel() const │ │ │ │ │ -access to the noise model │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:223 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_w_h_i_t_e_n_e_d_E_r_r_o_r │ │ │ │ │ +Vector whitenedError(const Values &c) const │ │ │ │ │ +Vector of errors, whitened This is the raw error, i.e., i.e. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.cpp:109 │ │ │ │ │ _g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_V_a_l_u_e_s │ │ │ │ │ -In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ -a non-linear function... │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_r_e_t_r_a_c_t │ │ │ │ │ +Values retract(const VectorValues &delta) const │ │ │ │ │ +Add a delta config to current config and returns a new config. │ │ │ │ │ +DDeeffiinniittiioonn Values.cpp:99 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_z_e_r_o_V_e_c_t_o_r_s │ │ │ │ │ +VectorValues zeroVectors() const │ │ │ │ │ +Return a VectorValues of zero vectors for each variable in this Values. │ │ │ │ │ +DDeeffiinniittiioonn Values.cpp:272 │ │ │ │ │ +_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ +In Gaussian factors, the error function returns either the negative log- │ │ │ │ │ +likelihood,... │ │ │ │ │ _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r │ │ │ │ │ noise model to the factor, and calculates the error by asking the user to │ │ │ │ │ implement the method │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * EExxpprreessssiioonnFFaaccttoorr..hh │ │ │ │ │ + * _f_a_c_t_o_r_T_e_s_t_i_n_g_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01019.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearOptimizer.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Marginals.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,56 +94,32 @@ │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
NonlinearOptimizer.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
Marginals.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ - │ │ │ │ -

Base class and parameters for nonlinear optimization algorithms. │ │ │ │ -More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::NonlinearOptimizer
 This is the abstract interface for classes that can optimize for the maximum-likelihood estimate of a NonlinearFactorGraph. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -bool gtsam::checkConvergence (double relativeErrorTreshold, double absoluteErrorTreshold, double errorThreshold, double currentError, double newError, NonlinearOptimizerParams::Verbosity verbosity=NonlinearOptimizerParams::SILENT)
 Check whether the relative error decrease is less than relativeErrorTreshold, the absolute error decrease is less than absoluteErrorTreshold, or the error itself is less than errorThreshold.
 
│ │ │ │ -GTSAM_EXPORT bool gtsam::checkConvergence (const NonlinearOptimizerParams &params, double currentError, double newError)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Base class and parameters for nonlinear optimization algorithms.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Sep 7, 2009
│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
May 14, 2012
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,41 +1,20 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -NonlinearOptimizer.h File Reference │ │ │ │ │ -Base class and parameters for nonlinear optimization algorithms. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r │ │ │ │ │ -  This is the abstract interface for classes that can optimize for the │ │ │ │ │ - maximum-likelihood estimate of a _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +Marginals.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ - bool  ggttssaamm::::cchheecckkCCoonnvveerrggeennccee (double relativeErrorTreshold, │ │ │ │ │ - double absoluteErrorTreshold, double errorThreshold, double │ │ │ │ │ - currentError, double newError, _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_: │ │ │ │ │ - _V_e_r_b_o_s_i_t_y verbosity=NonlinearOptimizerParams::SILENT) │ │ │ │ │ - Check whether the relative error decrease is less than │ │ │ │ │ -  relativeErrorTreshold, the absolute error decrease is less │ │ │ │ │ - than absoluteErrorTreshold, oorr the error itself is less than │ │ │ │ │ - errorThreshold. │ │ │ │ │ -  │ │ │ │ │ -GTSAM_EXPORT bool  ggttssaamm::::cchheecckkCCoonnvveerrggeennccee (const _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s │ │ │ │ │ - ¶ms, double currentError, double newError) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Base class and parameters for nonlinear optimization algorithms. │ │ │ │ │ Author │ │ │ │ │ Richard Roberts │ │ │ │ │ Date │ │ │ │ │ - Sep 7, 2009 │ │ │ │ │ + May 14, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ + * _M_a_r_g_i_n_a_l_s_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01022.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearFactorGraph.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/LinearContainerFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,39 +94,47 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
NonlinearFactorGraph.cpp File Reference
│ │ │ │ +
LinearContainerFactor.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Factor Graph Consisting of non-linear factors. │ │ │ │ +

Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::LinearContainerFactor
 Dummy version of a generic linear factor to be injected into a nonlinear factor graph. More...
 
struct  gtsam::traits< LinearContainerFactor >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Factor Graph Consisting of non-linear factors.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ -Carlos Nieto
│ │ │ │ -
│ │ │ │ -Christian Potthast
│ │ │ │ +

Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph.

│ │ │ │ +
Date
Jul 6, 2012
│ │ │ │ +
Author
Alex Cunningham
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,32 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -NonlinearFactorGraph.cpp File Reference │ │ │ │ │ -Factor Graph Consisting of non-linear factors. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +LinearContainerFactor.h File Reference │ │ │ │ │ +Wrap Jacobian and Hessian linear factors to allow simple injection into a │ │ │ │ │ +nonlinear graph. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r │ │ │ │ │ +  Dummy version of a generic linear factor to be injected into a │ │ │ │ │ + nonlinear factor graph. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r_ _> │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Factor Graph Consisting of non-linear factors. │ │ │ │ │ +Wrap Jacobian and Hessian linear factors to allow simple injection into a │ │ │ │ │ +nonlinear graph. │ │ │ │ │ + Date │ │ │ │ │ + Jul 6, 2012 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Carlos Nieto │ │ │ │ │ - Christian Potthast │ │ │ │ │ + Alex Cunningham │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._c_p_p │ │ │ │ │ + * _L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01025.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Params.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/DoglegOptimizer.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,58 +95,43 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs
│ │ │ │ -
ISAM2Params.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
DoglegOptimizer.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Parameters for iSAM 2. │ │ │ │ -More...

│ │ │ │ - │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

struct  gtsam::ISAM2GaussNewtonParams
 Parameters for ISAM2 using Gauss-Newton optimization. More...
 
struct  gtsam::ISAM2DoglegParams
 Parameters for ISAM2 using Dogleg optimization. More...
class  gtsam::DoglegParams
 Parameters for Levenberg-Marquardt optimization. More...
 
struct  gtsam::ISAM2Params
class  gtsam::DoglegOptimizer
 This class performs Dogleg nonlinear optimization. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Typedefs

typedef FastMap< char, Vector > gtsam::ISAM2ThresholdMap
 Parameters for the ISAM2 algorithm.
 
│ │ │ │ -typedef ISAM2ThresholdMap::value_type gtsam::ISAM2ThresholdMapValue
 
│ │ │ │

Detailed Description

│ │ │ │ -

Parameters for iSAM 2.

│ │ │ │ -
Author
Michael Kaess, Richard Roberts, Frank Dellaert
│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Feb 26, 2012
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,36 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ -ISAM2Params.h File Reference │ │ │ │ │ -Parameters for iSAM 2. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +DoglegOptimizer.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_I_S_A_M_2_G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s │ │ │ │ │ -  Parameters for _I_S_A_M_2 using Gauss-Newton optimization. _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_D_o_g_l_e_g_P_a_r_a_m_s │ │ │ │ │ +  Parameters for Levenberg-Marquardt optimization. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_I_S_A_M_2_D_o_g_l_e_g_P_a_r_a_m_s │ │ │ │ │ -  Parameters for _I_S_A_M_2 using Dogleg optimization. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s │ │ │ │ │ +class   _g_t_s_a_m_:_:_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r │ │ │ │ │ +  This class performs Dogleg nonlinear optimization. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ - typedef _F_a_s_t_M_a_p< char, Vector >  _g_t_s_a_m_:_:_I_S_A_M_2_T_h_r_e_s_h_o_l_d_M_a_p │ │ │ │ │ -  Parameters for the _I_S_A_M_2 algorithm. │ │ │ │ │ -  │ │ │ │ │ -typedef ISAM2ThresholdMap::value_type  ggttssaamm::::IISSAAMM22TThhrreesshhoollddMMaappVVaalluuee │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Parameters for iSAM 2. │ │ │ │ │ Author │ │ │ │ │ - Michael Kaess, Richard Roberts, Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Feb 26, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _I_S_A_M_2_P_a_r_a_m_s_._h │ │ │ │ │ + * _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01025.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a01025 = [ │ │ │ │ │ - ["gtsam::ISAM2Params", "a04416.html", "a04416"], │ │ │ │ │ - ["ISAM2ThresholdMap", "a01025.html#ga238e84a1d4a91f43465b12592a8bf8e4", null] │ │ │ │ │ + ["gtsam::DoglegParams", "a04256.html", "a04256"], │ │ │ │ │ + ["gtsam::DoglegOptimizer", "a04260.html", "a04260"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01025_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Params.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/DoglegOptimizer.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,307 +98,127 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ISAM2Params.h
│ │ │ │ +
DoglegOptimizer.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18// \callgraph
│ │ │ │ -
19
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ -
23#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
│ │ │ │ -
24#include <boost/variant.hpp>
│ │ │ │ -
25#include <string>
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/nonlinear/NonlinearOptimizer.h>
│ │ │ │ +
22
│ │ │ │ +
23namespace gtsam {
│ │ │ │ +
24
│ │ │ │ +
25class DoglegOptimizer;
│ │ │ │
26
│ │ │ │ -
27namespace gtsam {
│ │ │ │ -
28
│ │ │ │ +
│ │ │ │ +
32class GTSAM_EXPORT DoglegParams : public NonlinearOptimizerParams {
│ │ │ │ +
33public:
│ │ │ │
│ │ │ │ -
35struct GTSAM_EXPORT ISAM2GaussNewtonParams {
│ │ │ │ -
36 double
│ │ │ │ -
37 wildfireThreshold;
│ │ │ │ -
39
│ │ │ │ -
│ │ │ │ -
41 ISAM2GaussNewtonParams(
│ │ │ │ -
42 double _wildfireThreshold =
│ │ │ │ -
43 0.001
│ │ │ │ -
45 )
│ │ │ │ -
46 : wildfireThreshold(_wildfireThreshold) {}
│ │ │ │ +
35 enum VerbosityDL {
│ │ │ │ +
36 SILENT,
│ │ │ │ +
37 VERBOSE
│ │ │ │ +
38 };
│ │ │ │
│ │ │ │ +
39
│ │ │ │ +
40 double deltaInitial;
│ │ │ │ +
41 VerbosityDL verbosityDL;
│ │ │ │ +
42
│ │ │ │ +
43 DoglegParams() :
│ │ │ │ +
44 deltaInitial(1.0), verbosityDL(SILENT) {}
│ │ │ │ +
45
│ │ │ │ +
46 ~DoglegParams() override {}
│ │ │ │
47
│ │ │ │ -
48 void print(const std::string str = "") const {
│ │ │ │ -
49 using std::cout;
│ │ │ │ -
50 cout << str << "type: ISAM2GaussNewtonParams\n";
│ │ │ │ -
51 cout << str << "wildfireThreshold: " << wildfireThreshold << "\n";
│ │ │ │ -
52 cout.flush();
│ │ │ │ -
53 }
│ │ │ │ -
54
│ │ │ │ -
55 double getWildfireThreshold() const { return wildfireThreshold; }
│ │ │ │ -
56 void setWildfireThreshold(double wildfireThreshold) {
│ │ │ │ -
57 this->wildfireThreshold = wildfireThreshold;
│ │ │ │ -
58 }
│ │ │ │ -
59};
│ │ │ │ -
│ │ │ │ -
60
│ │ │ │ -
│ │ │ │ -
67struct GTSAM_EXPORT ISAM2DoglegParams {
│ │ │ │ -
68 double initialDelta;
│ │ │ │ -
69 double
│ │ │ │ -
70 wildfireThreshold;
│ │ │ │ -
72 DoglegOptimizerImpl::TrustRegionAdaptationMode
│ │ │ │ -
73 adaptationMode;
│ │ │ │ -
75 bool
│ │ │ │ -
76 verbose;
│ │ │ │ -
77
│ │ │ │ -
│ │ │ │ -
79 ISAM2DoglegParams(
│ │ │ │ -
80 double _initialDelta = 1.0,
│ │ │ │ -
81 double _wildfireThreshold =
│ │ │ │ -
82 1e-5,
│ │ │ │ -
83 DoglegOptimizerImpl::TrustRegionAdaptationMode _adaptationMode =
│ │ │ │ -
84 DoglegOptimizerImpl::
│ │ │ │ -
85 SEARCH_EACH_ITERATION,
│ │ │ │ -
86 bool _verbose = false
│ │ │ │ -
87 )
│ │ │ │ -
88 : initialDelta(_initialDelta),
│ │ │ │ -
89 wildfireThreshold(_wildfireThreshold),
│ │ │ │ -
90 adaptationMode(_adaptationMode),
│ │ │ │ -
91 verbose(_verbose) {}
│ │ │ │ -
│ │ │ │ -
92
│ │ │ │ -
93 void print(const std::string str = "") const {
│ │ │ │ -
94 using std::cout;
│ │ │ │ -
95 cout << str << "type: ISAM2DoglegParams\n";
│ │ │ │ -
96 cout << str << "initialDelta: " << initialDelta << "\n";
│ │ │ │ -
97 cout << str << "wildfireThreshold: " << wildfireThreshold << "\n";
│ │ │ │ -
98 cout << str
│ │ │ │ -
99 << "adaptationMode: " << adaptationModeTranslator(adaptationMode)
│ │ │ │ -
100 << "\n";
│ │ │ │ -
101 cout.flush();
│ │ │ │ -
102 }
│ │ │ │ -
103
│ │ │ │ -
104 double getInitialDelta() const { return initialDelta; }
│ │ │ │ -
105 double getWildfireThreshold() const { return wildfireThreshold; }
│ │ │ │ -
106 std::string getAdaptationMode() const {
│ │ │ │ -
107 return adaptationModeTranslator(adaptationMode);
│ │ │ │ -
108 }
│ │ │ │ -
109 bool isVerbose() const { return verbose; }
│ │ │ │ -
110 void setInitialDelta(double initialDelta) {
│ │ │ │ -
111 this->initialDelta = initialDelta;
│ │ │ │ -
112 }
│ │ │ │ -
113 void setWildfireThreshold(double wildfireThreshold) {
│ │ │ │ -
114 this->wildfireThreshold = wildfireThreshold;
│ │ │ │ -
115 }
│ │ │ │ -
116 void setAdaptationMode(const std::string& adaptationMode) {
│ │ │ │ -
117 this->adaptationMode = adaptationModeTranslator(adaptationMode);
│ │ │ │ -
118 }
│ │ │ │ -
119 void setVerbose(bool verbose) { this->verbose = verbose; }
│ │ │ │ -
120
│ │ │ │ -
121 std::string adaptationModeTranslator(
│ │ │ │ -
122 const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode)
│ │ │ │ -
123 const;
│ │ │ │ -
124 DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationModeTranslator(
│ │ │ │ -
125 const std::string& adaptationMode) const;
│ │ │ │ -
126};
│ │ │ │ -
│ │ │ │ -
127
│ │ │ │ -
133typedef FastMap<char, Vector> ISAM2ThresholdMap;
│ │ │ │ -
134typedef ISAM2ThresholdMap::value_type ISAM2ThresholdMapValue;
│ │ │ │ -
│ │ │ │ -
135struct GTSAM_EXPORT ISAM2Params {
│ │ │ │ -
136 typedef boost::variant<ISAM2GaussNewtonParams, ISAM2DoglegParams>
│ │ │ │ -
137 OptimizationParams;
│ │ │ │ -
139 typedef boost::variant<double, FastMap<char, Vector> >
│ │ │ │ -
140 RelinearizationThreshold;
│ │ │ │ -
143
│ │ │ │ -
150 OptimizationParams optimizationParams;
│ │ │ │ -
151
│ │ │ │ -
168 RelinearizationThreshold relinearizeThreshold;
│ │ │ │ -
169
│ │ │ │ -
170 int relinearizeSkip;
│ │ │ │ -
173
│ │ │ │ -
174 bool enableRelinearization;
│ │ │ │ -
176
│ │ │ │ -
177 bool evaluateNonlinearError;
│ │ │ │ -
180
│ │ │ │ -
181 enum Factorization { CHOLESKY, QR };
│ │ │ │ -
193 Factorization factorization;
│ │ │ │ -
194
│ │ │ │ -
200 bool cacheLinearizedFactors;
│ │ │ │ -
201
│ │ │ │ -
202 KeyFormatter
│ │ │ │ -
203 keyFormatter;
│ │ │ │ -
205
│ │ │ │ -
206 bool enableDetailedResults;
│ │ │ │ -
209
│ │ │ │ -
218 bool enablePartialRelinearizationCheck;
│ │ │ │ -
219
│ │ │ │ -
224 bool findUnusedFactorSlots;
│ │ │ │ -
225
│ │ │ │ -
│ │ │ │ -
230 ISAM2Params(OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(),
│ │ │ │ -
231 RelinearizationThreshold _relinearizeThreshold = 0.1,
│ │ │ │ -
232 int _relinearizeSkip = 10, bool _enableRelinearization = true,
│ │ │ │ -
233 bool _evaluateNonlinearError = false,
│ │ │ │ -
234 Factorization _factorization = ISAM2Params::CHOLESKY,
│ │ │ │ -
235 bool _cacheLinearizedFactors = true,
│ │ │ │ -
236 const KeyFormatter& _keyFormatter =
│ │ │ │ -
237 DefaultKeyFormatter,
│ │ │ │ -
238 bool _enableDetailedResults = false)
│ │ │ │ -
239 : optimizationParams(_optimizationParams),
│ │ │ │ -
240 relinearizeThreshold(_relinearizeThreshold),
│ │ │ │ -
241 relinearizeSkip(_relinearizeSkip),
│ │ │ │ -
242 enableRelinearization(_enableRelinearization),
│ │ │ │ -
243 evaluateNonlinearError(_evaluateNonlinearError),
│ │ │ │ -
244 factorization(_factorization),
│ │ │ │ -
245 cacheLinearizedFactors(_cacheLinearizedFactors),
│ │ │ │ -
246 keyFormatter(_keyFormatter),
│ │ │ │ -
247 enableDetailedResults(_enableDetailedResults),
│ │ │ │ -
248 enablePartialRelinearizationCheck(false),
│ │ │ │ -
249 findUnusedFactorSlots(false) {}
│ │ │ │ -
│ │ │ │ -
250
│ │ │ │ -
│ │ │ │ -
252 void print(const std::string& str = "") const {
│ │ │ │ -
253 using std::cout;
│ │ │ │ -
254 cout << str << "\n";
│ │ │ │ -
255
│ │ │ │ -
256 static const std::string kStr("optimizationParams: ");
│ │ │ │ -
257 if (optimizationParams.type() == typeid(ISAM2GaussNewtonParams))
│ │ │ │ -
258 boost::get<ISAM2GaussNewtonParams>(optimizationParams).print();
│ │ │ │ -
259 else if (optimizationParams.type() == typeid(ISAM2DoglegParams))
│ │ │ │ -
260 boost::get<ISAM2DoglegParams>(optimizationParams).print(kStr);
│ │ │ │ -
261 else
│ │ │ │ -
262 cout << kStr << "{unknown type}\n";
│ │ │ │ -
263
│ │ │ │ -
264 cout << "relinearizeThreshold: ";
│ │ │ │ -
265 if (relinearizeThreshold.type() == typeid(double)) {
│ │ │ │ -
266 cout << boost::get<double>(relinearizeThreshold) << "\n";
│ │ │ │ -
267 } else {
│ │ │ │ -
268 cout << "{mapped}\n";
│ │ │ │ -
269 for (const ISAM2ThresholdMapValue& value :
│ │ │ │ -
270 boost::get<ISAM2ThresholdMap>(relinearizeThreshold)) {
│ │ │ │ -
271 cout << " '" << value.first
│ │ │ │ -
272 << "' -> [" << value.second.transpose() << " ]\n";
│ │ │ │ -
273 }
│ │ │ │ -
274 }
│ │ │ │ -
275
│ │ │ │ -
276 cout << "relinearizeSkip: " << relinearizeSkip << "\n";
│ │ │ │ -
277 cout << "enableRelinearization: " << enableRelinearization
│ │ │ │ -
278 << "\n";
│ │ │ │ -
279 cout << "evaluateNonlinearError: " << evaluateNonlinearError
│ │ │ │ -
280 << "\n";
│ │ │ │ -
281 cout << "factorization: "
│ │ │ │ -
282 << factorizationTranslator(factorization) << "\n";
│ │ │ │ -
283 cout << "cacheLinearizedFactors: " << cacheLinearizedFactors
│ │ │ │ -
284 << "\n";
│ │ │ │ -
285 cout << "enableDetailedResults: " << enableDetailedResults
│ │ │ │ -
286 << "\n";
│ │ │ │ -
287 cout << "enablePartialRelinearizationCheck: "
│ │ │ │ -
288 << enablePartialRelinearizationCheck << "\n";
│ │ │ │ -
289 cout << "findUnusedFactorSlots: " << findUnusedFactorSlots
│ │ │ │ -
290 << "\n";
│ │ │ │ -
291 cout.flush();
│ │ │ │ -
292 }
│ │ │ │ -
│ │ │ │ -
293
│ │ │ │ -
296
│ │ │ │ -
297 OptimizationParams getOptimizationParams() const {
│ │ │ │ -
298 return this->optimizationParams;
│ │ │ │ -
299 }
│ │ │ │ -
300 RelinearizationThreshold getRelinearizeThreshold() const {
│ │ │ │ -
301 return relinearizeThreshold;
│ │ │ │ -
302 }
│ │ │ │ -
303 std::string getFactorization() const {
│ │ │ │ -
304 return factorizationTranslator(factorization);
│ │ │ │ -
305 }
│ │ │ │ -
306 KeyFormatter getKeyFormatter() const { return keyFormatter; }
│ │ │ │ -
307
│ │ │ │ -
308 void setOptimizationParams(OptimizationParams optimizationParams) {
│ │ │ │ -
309 this->optimizationParams = optimizationParams;
│ │ │ │ -
310 }
│ │ │ │ -
311 void setRelinearizeThreshold(RelinearizationThreshold relinearizeThreshold) {
│ │ │ │ -
312 this->relinearizeThreshold = relinearizeThreshold;
│ │ │ │ -
313 }
│ │ │ │ -
314 void setFactorization(const std::string& factorization) {
│ │ │ │ -
315 this->factorization = factorizationTranslator(factorization);
│ │ │ │ -
316 }
│ │ │ │ -
317 void setKeyFormatter(KeyFormatter keyFormatter) {
│ │ │ │ -
318 this->keyFormatter = keyFormatter;
│ │ │ │ -
319 }
│ │ │ │ -
320
│ │ │ │ -
321 GaussianFactorGraph::Eliminate getEliminationFunction() const {
│ │ │ │ -
322 return factorization == CHOLESKY
│ │ │ │ -
323 ? (GaussianFactorGraph::Eliminate)EliminatePreferCholesky
│ │ │ │ -
324 : (GaussianFactorGraph::Eliminate)EliminateQR;
│ │ │ │ -
325 }
│ │ │ │ -
326
│ │ │ │ -
328
│ │ │ │ -
331
│ │ │ │ -
332 static Factorization factorizationTranslator(const std::string& str);
│ │ │ │ -
333 static std::string factorizationTranslator(const Factorization& value);
│ │ │ │ -
334
│ │ │ │ -
336};
│ │ │ │ -
│ │ │ │ -
337
│ │ │ │ -
338} // namespace gtsam
│ │ │ │ -
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ -
DoglegOptimizerImpl.h
Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail implementation)
│ │ │ │ -
gtsam::ISAM2ThresholdMap
FastMap< char, Vector > ISAM2ThresholdMap
Parameters for the ISAM2 algorithm.
Definition ISAM2Params.h:133
│ │ │ │ +
48 void print(const std::string& str = "") const override {
│ │ │ │ +
49 NonlinearOptimizerParams::print(str);
│ │ │ │ +
50 std::cout << " deltaInitial: " << deltaInitial << "\n";
│ │ │ │ +
51 std::cout.flush();
│ │ │ │ +
52 }
│ │ │ │ +
53
│ │ │ │ +
54 double getDeltaInitial() const { return deltaInitial; }
│ │ │ │ +
55 std::string getVerbosityDL() const { return verbosityDLTranslator(verbosityDL); }
│ │ │ │ +
56
│ │ │ │ +
57 void setDeltaInitial(double deltaInitial) { this->deltaInitial = deltaInitial; }
│ │ │ │ +
58 void setVerbosityDL(const std::string& verbosityDL) { this->verbosityDL = verbosityDLTranslator(verbosityDL); }
│ │ │ │ +
59
│ │ │ │ +
60private:
│ │ │ │ +
61 VerbosityDL verbosityDLTranslator(const std::string& verbosityDL) const;
│ │ │ │ +
62 std::string verbosityDLTranslator(VerbosityDL verbosityDL) const;
│ │ │ │ +
63};
│ │ │ │ +
│ │ │ │ +
64
│ │ │ │ +
│ │ │ │ +
68class GTSAM_EXPORT DoglegOptimizer : public NonlinearOptimizer {
│ │ │ │ +
69
│ │ │ │ +
70protected:
│ │ │ │ +
71 DoglegParams params_;
│ │ │ │ +
72
│ │ │ │ +
73public:
│ │ │ │ +
74 typedef boost::shared_ptr<DoglegOptimizer> shared_ptr;
│ │ │ │ +
75
│ │ │ │ +
78
│ │ │ │ +
87 DoglegOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
│ │ │ │ +
88 const DoglegParams& params = DoglegParams());
│ │ │ │ +
89
│ │ │ │ +
97 DoglegOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
│ │ │ │ +
98 const Ordering& ordering);
│ │ │ │ +
99
│ │ │ │ +
101
│ │ │ │ +
104
│ │ │ │ +
106 ~DoglegOptimizer() override {}
│ │ │ │ +
107
│ │ │ │ +
112 GaussianFactorGraph::shared_ptr iterate() override;
│ │ │ │ +
113
│ │ │ │ +
115 const DoglegParams& params() const { return params_; }
│ │ │ │ +
116
│ │ │ │ +
118 double getDelta() const;
│ │ │ │ +
119
│ │ │ │ +
121
│ │ │ │ +
122protected:
│ │ │ │ +
124 const NonlinearOptimizerParams& _params() const override { return params_; }
│ │ │ │ +
125
│ │ │ │ +
127 DoglegParams ensureHasOrdering(DoglegParams params, const NonlinearFactorGraph& graph) const;
│ │ │ │ +
128};
│ │ │ │ +
│ │ │ │ +
129
│ │ │ │ +
130}
│ │ │ │ +
NonlinearOptimizer.h
Base class and parameters for nonlinear optimization algorithms.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::EliminateQR
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Multiply all factors and eliminate the given keys from the resulting factor using a QR variant that h...
Definition JacobianFactor.cpp:789
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ -
gtsam::DoglegOptimizerImpl
This class contains the implementation of the Dogleg algorithm.
Definition DoglegOptimizerImpl.h:32
│ │ │ │ -
gtsam::DoglegOptimizerImpl::TrustRegionAdaptationMode
TrustRegionAdaptationMode
Specifies how the trust region is adapted at each Dogleg iteration.
Definition DoglegOptimizerImpl.h:53
│ │ │ │ -
gtsam::ISAM2GaussNewtonParams
Parameters for ISAM2 using Gauss-Newton optimization.
Definition ISAM2Params.h:35
│ │ │ │ -
gtsam::ISAM2GaussNewtonParams::wildfireThreshold
double wildfireThreshold
Continue updating the linear delta only when changes are above this threshold (default: 0....
Definition ISAM2Params.h:37
│ │ │ │ -
gtsam::ISAM2GaussNewtonParams::ISAM2GaussNewtonParams
ISAM2GaussNewtonParams(double _wildfireThreshold=0.001)
Specify parameters as constructor arguments.
Definition ISAM2Params.h:41
│ │ │ │ -
gtsam::ISAM2DoglegParams
Parameters for ISAM2 using Dogleg optimization.
Definition ISAM2Params.h:67
│ │ │ │ -
gtsam::ISAM2DoglegParams::adaptationMode
DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationMode
See description in DoglegOptimizerImpl::TrustRegionAdaptationMode.
Definition ISAM2Params.h:73
│ │ │ │ -
gtsam::ISAM2DoglegParams::verbose
bool verbose
Whether Dogleg prints iteration and convergence information.
Definition ISAM2Params.h:76
│ │ │ │ -
gtsam::ISAM2DoglegParams::ISAM2DoglegParams
ISAM2DoglegParams(double _initialDelta=1.0, double _wildfireThreshold=1e-5, DoglegOptimizerImpl::TrustRegionAdaptationMode _adaptationMode=DoglegOptimizerImpl::SEARCH_EACH_ITERATION, bool _verbose=false)
Specify parameters as constructor arguments.
Definition ISAM2Params.h:79
│ │ │ │ -
gtsam::ISAM2DoglegParams::initialDelta
double initialDelta
The initial trust region radius for Dogleg.
Definition ISAM2Params.h:68
│ │ │ │ -
gtsam::ISAM2DoglegParams::wildfireThreshold
double wildfireThreshold
Continue updating the linear delta only when changes are above this threshold (default: 1e-5)
Definition ISAM2Params.h:70
│ │ │ │ -
gtsam::ISAM2Params
Definition ISAM2Params.h:135
│ │ │ │ -
gtsam::ISAM2Params::enablePartialRelinearizationCheck
bool enablePartialRelinearizationCheck
Check variables for relinearization in tree-order, stopping the check once a variable does not need t...
Definition ISAM2Params.h:218
│ │ │ │ -
gtsam::ISAM2Params::optimizationParams
OptimizationParams optimizationParams
Optimization parameters, this both selects the nonlinear optimization method and specifies its parame...
Definition ISAM2Params.h:150
│ │ │ │ -
gtsam::ISAM2Params::print
void print(const std::string &str="") const
print iSAM2 parameters
Definition ISAM2Params.h:252
│ │ │ │ -
gtsam::ISAM2Params::enableRelinearization
bool enableRelinearization
Controls whether ISAM2 will ever relinearize any variables (default: true)
Definition ISAM2Params.h:174
│ │ │ │ -
gtsam::ISAM2Params::cacheLinearizedFactors
bool cacheLinearizedFactors
Whether to cache linear factors (default: true).
Definition ISAM2Params.h:200
│ │ │ │ -
gtsam::ISAM2Params::evaluateNonlinearError
bool evaluateNonlinearError
Whether to evaluate the nonlinear error before and after the update, to return in ISAM2Result from up...
Definition ISAM2Params.h:177
│ │ │ │ -
gtsam::ISAM2Params::enableDetailedResults
bool enableDetailedResults
Whether to compute and return ISAM2Result::detailedResults, this can increase running time (default: ...
Definition ISAM2Params.h:206
│ │ │ │ -
gtsam::ISAM2Params::OptimizationParams
boost::variant< ISAM2GaussNewtonParams, ISAM2DoglegParams > OptimizationParams
Either ISAM2GaussNewtonParams or ISAM2DoglegParams.
Definition ISAM2Params.h:137
│ │ │ │ -
gtsam::ISAM2Params::findUnusedFactorSlots
bool findUnusedFactorSlots
When you will be removing many factors, e.g.
Definition ISAM2Params.h:224
│ │ │ │ -
gtsam::ISAM2Params::RelinearizationThreshold
boost::variant< double, FastMap< char, Vector > > RelinearizationThreshold
Either a constant relinearization threshold or a per-variable-type set of thresholds.
Definition ISAM2Params.h:140
│ │ │ │ -
gtsam::ISAM2Params::relinearizeSkip
int relinearizeSkip
Only relinearize any variables every relinearizeSkip calls to ISAM2::update (default: 10)
Definition ISAM2Params.h:170
│ │ │ │ -
gtsam::ISAM2Params::factorization
Factorization factorization
Specifies whether to use QR or CHOESKY numerical factorization (default: CHOLESKY).
Definition ISAM2Params.h:193
│ │ │ │ -
gtsam::ISAM2Params::keyFormatter
KeyFormatter keyFormatter
A KeyFormatter for when keys are printed during debugging (default: DefaultKeyFormatter)
Definition ISAM2Params.h:203
│ │ │ │ -
gtsam::ISAM2Params::relinearizeThreshold
RelinearizationThreshold relinearizeThreshold
Only relinearize variables whose linear delta magnitude is greater than this threshold (default: 0....
Definition ISAM2Params.h:168
│ │ │ │ -
gtsam::ISAM2Params::ISAM2Params
ISAM2Params(OptimizationParams _optimizationParams=ISAM2GaussNewtonParams(), RelinearizationThreshold _relinearizeThreshold=0.1, int _relinearizeSkip=10, bool _enableRelinearization=true, bool _evaluateNonlinearError=false, Factorization _factorization=ISAM2Params::CHOLESKY, bool _cacheLinearizedFactors=true, const KeyFormatter &_keyFormatter=DefaultKeyFormatter, bool _enableDetailedResults=false)
Specify parameters as constructor arguments See the documentation of member variables above.
Definition ISAM2Params.h:230
│ │ │ │ +
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
gtsam::GaussianFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactorGraph.h:81
│ │ │ │ +
gtsam::DoglegParams
Parameters for Levenberg-Marquardt optimization.
Definition DoglegOptimizer.h:32
│ │ │ │ +
gtsam::DoglegParams::VerbosityDL
VerbosityDL
See DoglegParams::dlVerbosity.
Definition DoglegOptimizer.h:35
│ │ │ │ +
gtsam::DoglegParams::verbosityDL
VerbosityDL verbosityDL
The verbosity level for Dogleg (default: SILENT), see also NonlinearOptimizerParams::verbosity.
Definition DoglegOptimizer.h:41
│ │ │ │ +
gtsam::DoglegParams::deltaInitial
double deltaInitial
The initial trust region radius (default: 10.0)
Definition DoglegOptimizer.h:40
│ │ │ │ +
gtsam::DoglegOptimizer
This class performs Dogleg nonlinear optimization.
Definition DoglegOptimizer.h:68
│ │ │ │ +
gtsam::DoglegOptimizer::~DoglegOptimizer
~DoglegOptimizer() override
Virtual destructor.
Definition DoglegOptimizer.h:106
│ │ │ │ +
gtsam::DoglegOptimizer::_params
const NonlinearOptimizerParams & _params() const override
Access the parameters (base class version)
Definition DoglegOptimizer.h:124
│ │ │ │ +
gtsam::DoglegOptimizer::params
const DoglegParams & params() const
Read-only access the parameters.
Definition DoglegOptimizer.h:115
│ │ │ │ +
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │ +
gtsam::NonlinearOptimizer
This is the abstract interface for classes that can optimize for the maximum-likelihood estimate of a...
Definition NonlinearOptimizer.h:75
│ │ │ │ +
gtsam::NonlinearOptimizerParams
The common parameters for Nonlinear optimizers.
Definition NonlinearOptimizerParams.h:34
│ │ │ │ +
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,405 +1,158 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ISAM2Params.h │ │ │ │ │ +DoglegOptimizer.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18// \callgraph │ │ │ │ │ -19 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_._h> │ │ │ │ │ -24#include │ │ │ │ │ -25#include │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_._h> │ │ │ │ │ +22 │ │ │ │ │ +23namespace _g_t_s_a_m { │ │ │ │ │ +24 │ │ │ │ │ +25class DoglegOptimizer; │ │ │ │ │ 26 │ │ │ │ │ -27namespace _g_t_s_a_m { │ │ │ │ │ -28 │ │ │ │ │ -_3_5struct GTSAM_EXPORT _I_S_A_M_2_G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s { │ │ │ │ │ -36 double │ │ │ │ │ -_3_7 _w_i_l_d_f_i_r_e_T_h_r_e_s_h_o_l_d; │ │ │ │ │ +_3_2class GTSAM_EXPORT _D_o_g_l_e_g_P_a_r_a_m_s : public _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s { │ │ │ │ │ +33public: │ │ │ │ │ +_3_5 enum _V_e_r_b_o_s_i_t_y_D_L { │ │ │ │ │ +36 SILENT, │ │ │ │ │ +37 VERBOSE │ │ │ │ │ +38 }; │ │ │ │ │ 39 │ │ │ │ │ -_4_1 _I_S_A_M_2_G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s( │ │ │ │ │ -42 double _wildfireThreshold = │ │ │ │ │ -43 0.001 │ │ │ │ │ -45 ) │ │ │ │ │ -46 : wildfireThreshold(_wildfireThreshold) {} │ │ │ │ │ +_4_0 double _d_e_l_t_a_I_n_i_t_i_a_l; │ │ │ │ │ +_4_1 _V_e_r_b_o_s_i_t_y_D_L _v_e_r_b_o_s_i_t_y_D_L; │ │ │ │ │ +42 │ │ │ │ │ +43 _D_o_g_l_e_g_P_a_r_a_m_s() : │ │ │ │ │ +44 deltaInitial(1.0), verbosityDL(SILENT) {} │ │ │ │ │ +45 │ │ │ │ │ +46 _~_D_o_g_l_e_g_P_a_r_a_m_s() override {} │ │ │ │ │ 47 │ │ │ │ │ -48 void _p_r_i_n_t(const std::string str = "") const { │ │ │ │ │ -49 using std::cout; │ │ │ │ │ -50 cout << str << "type: ISAM2GaussNewtonParams\n"; │ │ │ │ │ -51 cout << str << "wildfireThreshold: " << wildfireThreshold << "\n"; │ │ │ │ │ -52 cout.flush(); │ │ │ │ │ -53 } │ │ │ │ │ -54 │ │ │ │ │ -55 double getWildfireThreshold() const { return wildfireThreshold; } │ │ │ │ │ -56 void setWildfireThreshold(double wildfireThreshold) { │ │ │ │ │ -57 this->wildfireThreshold = wildfireThreshold; │ │ │ │ │ -58 } │ │ │ │ │ -59}; │ │ │ │ │ -60 │ │ │ │ │ -_6_7struct GTSAM_EXPORT _I_S_A_M_2_D_o_g_l_e_g_P_a_r_a_m_s { │ │ │ │ │ -_6_8 double _i_n_i_t_i_a_l_D_e_l_t_a; │ │ │ │ │ -69 double │ │ │ │ │ -_7_0 _w_i_l_d_f_i_r_e_T_h_r_e_s_h_o_l_d; │ │ │ │ │ -72 _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_:_:_T_r_u_s_t_R_e_g_i_o_n_A_d_a_p_t_a_t_i_o_n_M_o_d_e │ │ │ │ │ -_7_3 _a_d_a_p_t_a_t_i_o_n_M_o_d_e; │ │ │ │ │ -75 bool │ │ │ │ │ -_7_6 _v_e_r_b_o_s_e; │ │ │ │ │ -77 │ │ │ │ │ -_7_9 _I_S_A_M_2_D_o_g_l_e_g_P_a_r_a_m_s( │ │ │ │ │ -80 double _initialDelta = 1.0, │ │ │ │ │ -81 double _wildfireThreshold = │ │ │ │ │ -82 1e-5, │ │ │ │ │ -83 _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_:_:_T_r_u_s_t_R_e_g_i_o_n_A_d_a_p_t_a_t_i_o_n_M_o_d_e _adaptationMode = │ │ │ │ │ -84 _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l:: │ │ │ │ │ -85 SEARCH_EACH_ITERATION, │ │ │ │ │ -86 bool _verbose = false │ │ │ │ │ -87 ) │ │ │ │ │ -88 : initialDelta(_initialDelta), │ │ │ │ │ -89 wildfireThreshold(_wildfireThreshold), │ │ │ │ │ -90 adaptationMode(_adaptationMode), │ │ │ │ │ -91 verbose(_verbose) {} │ │ │ │ │ -92 │ │ │ │ │ -93 void _p_r_i_n_t(const std::string str = "") const { │ │ │ │ │ -94 using std::cout; │ │ │ │ │ -95 cout << str << "type: ISAM2DoglegParams\n"; │ │ │ │ │ -96 cout << str << "initialDelta: " << initialDelta << "\n"; │ │ │ │ │ -97 cout << str << "wildfireThreshold: " << wildfireThreshold << "\n"; │ │ │ │ │ -98 cout << str │ │ │ │ │ -99 << "adaptationMode: " << adaptationModeTranslator(adaptationMode) │ │ │ │ │ -100 << "\n"; │ │ │ │ │ -101 cout.flush(); │ │ │ │ │ -102 } │ │ │ │ │ -103 │ │ │ │ │ -104 double getInitialDelta() const { return initialDelta; } │ │ │ │ │ -105 double getWildfireThreshold() const { return wildfireThreshold; } │ │ │ │ │ -106 std::string getAdaptationMode() const { │ │ │ │ │ -107 return adaptationModeTranslator(adaptationMode); │ │ │ │ │ -108 } │ │ │ │ │ -109 bool isVerbose() const { return verbose; } │ │ │ │ │ -110 void setInitialDelta(double initialDelta) { │ │ │ │ │ -111 this->initialDelta = initialDelta; │ │ │ │ │ -112 } │ │ │ │ │ -113 void setWildfireThreshold(double wildfireThreshold) { │ │ │ │ │ -114 this->wildfireThreshold = wildfireThreshold; │ │ │ │ │ -115 } │ │ │ │ │ -116 void setAdaptationMode(const std::string& adaptationMode) { │ │ │ │ │ -117 this->adaptationMode = adaptationModeTranslator(adaptationMode); │ │ │ │ │ -118 } │ │ │ │ │ -119 void setVerbose(bool verbose) { this->verbose = verbose; } │ │ │ │ │ -120 │ │ │ │ │ -121 std::string adaptationModeTranslator( │ │ │ │ │ -122 const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) │ │ │ │ │ -123 const; │ │ │ │ │ -124 DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationModeTranslator( │ │ │ │ │ -125 const std::string& adaptationMode) const; │ │ │ │ │ -126}; │ │ │ │ │ -127 │ │ │ │ │ -_1_3_3typedef _F_a_s_t_M_a_p_<_c_h_a_r_,_ _V_e_c_t_o_r_> _I_S_A_M_2_T_h_r_e_s_h_o_l_d_M_a_p; │ │ │ │ │ -134typedef ISAM2ThresholdMap::value_type ISAM2ThresholdMapValue; │ │ │ │ │ -_1_3_5struct GTSAM_EXPORT _I_S_A_M_2_P_a_r_a_m_s { │ │ │ │ │ -136 typedef boost::variant │ │ │ │ │ -_1_3_7 _O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_s; │ │ │ │ │ -139 typedef boost::variant > │ │ │ │ │ -_1_4_0 _R_e_l_i_n_e_a_r_i_z_a_t_i_o_n_T_h_r_e_s_h_o_l_d; │ │ │ │ │ -143 │ │ │ │ │ -_1_5_0 _O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_s _o_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_s; │ │ │ │ │ -151 │ │ │ │ │ -_1_6_8 _R_e_l_i_n_e_a_r_i_z_a_t_i_o_n_T_h_r_e_s_h_o_l_d _r_e_l_i_n_e_a_r_i_z_e_T_h_r_e_s_h_o_l_d; │ │ │ │ │ -169 │ │ │ │ │ -_1_7_0 int _r_e_l_i_n_e_a_r_i_z_e_S_k_i_p; │ │ │ │ │ -173 │ │ │ │ │ -_1_7_4 bool _e_n_a_b_l_e_R_e_l_i_n_e_a_r_i_z_a_t_i_o_n; │ │ │ │ │ -176 │ │ │ │ │ -_1_7_7 bool _e_v_a_l_u_a_t_e_N_o_n_l_i_n_e_a_r_E_r_r_o_r; │ │ │ │ │ -180 │ │ │ │ │ -181 enum Factorization { CHOLESKY, QR }; │ │ │ │ │ -_1_9_3 Factorization _f_a_c_t_o_r_i_z_a_t_i_o_n; │ │ │ │ │ -194 │ │ │ │ │ -_2_0_0 bool _c_a_c_h_e_L_i_n_e_a_r_i_z_e_d_F_a_c_t_o_r_s; │ │ │ │ │ -201 │ │ │ │ │ -202 _K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -_2_0_3 _k_e_y_F_o_r_m_a_t_t_e_r; │ │ │ │ │ -205 │ │ │ │ │ -_2_0_6 bool _e_n_a_b_l_e_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s; │ │ │ │ │ -209 │ │ │ │ │ -_2_1_8 bool _e_n_a_b_l_e_P_a_r_t_i_a_l_R_e_l_i_n_e_a_r_i_z_a_t_i_o_n_C_h_e_c_k; │ │ │ │ │ -219 │ │ │ │ │ -_2_2_4 bool _f_i_n_d_U_n_u_s_e_d_F_a_c_t_o_r_S_l_o_t_s; │ │ │ │ │ -225 │ │ │ │ │ -_2_3_0 _I_S_A_M_2_P_a_r_a_m_s(_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_s _optimizationParams = _I_S_A_M_2_G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s │ │ │ │ │ -(), │ │ │ │ │ -231 _R_e_l_i_n_e_a_r_i_z_a_t_i_o_n_T_h_r_e_s_h_o_l_d _relinearizeThreshold = 0.1, │ │ │ │ │ -232 int _relinearizeSkip = 10, bool _enableRelinearization = true, │ │ │ │ │ -233 bool _evaluateNonlinearError = false, │ │ │ │ │ -234 Factorization _factorization = ISAM2Params::CHOLESKY, │ │ │ │ │ -235 bool _cacheLinearizedFactors = true, │ │ │ │ │ -236 const _K_e_y_F_o_r_m_a_t_t_e_r& _keyFormatter = │ │ │ │ │ -237 DefaultKeyFormatter, │ │ │ │ │ -238 bool _enableDetailedResults = false) │ │ │ │ │ -239 : optimizationParams(_optimizationParams), │ │ │ │ │ -240 relinearizeThreshold(_relinearizeThreshold), │ │ │ │ │ -241 relinearizeSkip(_relinearizeSkip), │ │ │ │ │ -242 enableRelinearization(_enableRelinearization), │ │ │ │ │ -243 evaluateNonlinearError(_evaluateNonlinearError), │ │ │ │ │ -244 factorization(_factorization), │ │ │ │ │ -245 cacheLinearizedFactors(_cacheLinearizedFactors), │ │ │ │ │ -246 keyFormatter(_keyFormatter), │ │ │ │ │ -247 enableDetailedResults(_enableDetailedResults), │ │ │ │ │ -248 enablePartialRelinearizationCheck(false), │ │ │ │ │ -249 findUnusedFactorSlots(false) {} │ │ │ │ │ -250 │ │ │ │ │ -_2_5_2 void _p_r_i_n_t(const std::string& str = "") const { │ │ │ │ │ -253 using std::cout; │ │ │ │ │ -254 cout << str << "\n"; │ │ │ │ │ -255 │ │ │ │ │ -256 static const std::string kStr("optimizationParams: "); │ │ │ │ │ -257 if (optimizationParams.type() == typeid(_I_S_A_M_2_G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s)) │ │ │ │ │ -258 boost::get(optimizationParams).print(); │ │ │ │ │ -259 else if (optimizationParams.type() == typeid(_I_S_A_M_2_D_o_g_l_e_g_P_a_r_a_m_s)) │ │ │ │ │ -260 boost::get(optimizationParams).print(kStr); │ │ │ │ │ -261 else │ │ │ │ │ -262 cout << kStr << "{unknown type}\n"; │ │ │ │ │ -263 │ │ │ │ │ -264 cout << "relinearizeThreshold: "; │ │ │ │ │ -265 if (relinearizeThreshold.type() == typeid(double)) { │ │ │ │ │ -266 cout << boost::get(relinearizeThreshold) << "\n"; │ │ │ │ │ -267 } else { │ │ │ │ │ -268 cout << "{mapped}\n"; │ │ │ │ │ -269 for (const ISAM2ThresholdMapValue& value : │ │ │ │ │ -270 boost::get(relinearizeThreshold)) { │ │ │ │ │ -271 cout << " '" << value.first │ │ │ │ │ -272 << "' -> [" << value.second.transpose() << " ]\n"; │ │ │ │ │ -273 } │ │ │ │ │ -274 } │ │ │ │ │ -275 │ │ │ │ │ -276 cout << "relinearizeSkip: " << relinearizeSkip << "\n"; │ │ │ │ │ -277 cout << "enableRelinearization: " << enableRelinearization │ │ │ │ │ -278 << "\n"; │ │ │ │ │ -279 cout << "evaluateNonlinearError: " << evaluateNonlinearError │ │ │ │ │ -280 << "\n"; │ │ │ │ │ -281 cout << "factorization: " │ │ │ │ │ -282 << factorizationTranslator(factorization) << "\n"; │ │ │ │ │ -283 cout << "cacheLinearizedFactors: " << cacheLinearizedFactors │ │ │ │ │ -284 << "\n"; │ │ │ │ │ -285 cout << "enableDetailedResults: " << enableDetailedResults │ │ │ │ │ -286 << "\n"; │ │ │ │ │ -287 cout << "enablePartialRelinearizationCheck: " │ │ │ │ │ -288 << enablePartialRelinearizationCheck << "\n"; │ │ │ │ │ -289 cout << "findUnusedFactorSlots: " << findUnusedFactorSlots │ │ │ │ │ -290 << "\n"; │ │ │ │ │ -291 cout.flush(); │ │ │ │ │ -292 } │ │ │ │ │ -293 │ │ │ │ │ -296 │ │ │ │ │ -297 OptimizationParams getOptimizationParams() const { │ │ │ │ │ -298 return this->optimizationParams; │ │ │ │ │ -299 } │ │ │ │ │ -300 RelinearizationThreshold getRelinearizeThreshold() const { │ │ │ │ │ -301 return relinearizeThreshold; │ │ │ │ │ -302 } │ │ │ │ │ -303 std::string getFactorization() const { │ │ │ │ │ -304 return factorizationTranslator(factorization); │ │ │ │ │ -305 } │ │ │ │ │ -306 _K_e_y_F_o_r_m_a_t_t_e_r getKeyFormatter() const { return keyFormatter; } │ │ │ │ │ -307 │ │ │ │ │ -308 void setOptimizationParams(OptimizationParams optimizationParams) { │ │ │ │ │ -309 this->optimizationParams = optimizationParams; │ │ │ │ │ -310 } │ │ │ │ │ -311 void setRelinearizeThreshold(RelinearizationThreshold relinearizeThreshold) │ │ │ │ │ -{ │ │ │ │ │ -312 this->relinearizeThreshold = relinearizeThreshold; │ │ │ │ │ -313 } │ │ │ │ │ -314 void setFactorization(const std::string& factorization) { │ │ │ │ │ -315 this->factorization = factorizationTranslator(factorization); │ │ │ │ │ -316 } │ │ │ │ │ -317 void setKeyFormatter(KeyFormatter keyFormatter) { │ │ │ │ │ -318 this->keyFormatter = keyFormatter; │ │ │ │ │ -319 } │ │ │ │ │ -320 │ │ │ │ │ -321 GaussianFactorGraph::Eliminate getEliminationFunction() const { │ │ │ │ │ -322 return factorization == CHOLESKY │ │ │ │ │ -323 ? (GaussianFactorGraph::Eliminate)EliminatePreferCholesky │ │ │ │ │ -324 : (GaussianFactorGraph::Eliminate)_E_l_i_m_i_n_a_t_e_Q_R; │ │ │ │ │ -325 } │ │ │ │ │ -326 │ │ │ │ │ -328 │ │ │ │ │ -331 │ │ │ │ │ -332 static Factorization factorizationTranslator(const std::string& str); │ │ │ │ │ -333 static std::string factorizationTranslator(const Factorization& value); │ │ │ │ │ -334 │ │ │ │ │ -336}; │ │ │ │ │ -337 │ │ │ │ │ -338} // namespace gtsam │ │ │ │ │ -_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ -_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_._h │ │ │ │ │ -Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail │ │ │ │ │ -implementation) │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_T_h_r_e_s_h_o_l_d_M_a_p │ │ │ │ │ -FastMap< char, Vector > ISAM2ThresholdMap │ │ │ │ │ -Parameters for the ISAM2 algorithm. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:133 │ │ │ │ │ +48 void print(const std::string& str = "") const override { │ │ │ │ │ +49 NonlinearOptimizerParams::print(str); │ │ │ │ │ +50 std::cout << " deltaInitial: " << deltaInitial << "\n"; │ │ │ │ │ +51 std::cout.flush(); │ │ │ │ │ +52 } │ │ │ │ │ +53 │ │ │ │ │ +54 double getDeltaInitial() const { return deltaInitial; } │ │ │ │ │ +55 std::string getVerbosityDL() const { return verbosityDLTranslator │ │ │ │ │ +(verbosityDL); } │ │ │ │ │ +56 │ │ │ │ │ +57 void setDeltaInitial(double deltaInitial) { this->deltaInitial = │ │ │ │ │ +deltaInitial; } │ │ │ │ │ +58 void setVerbosityDL(const std::string& verbosityDL) { this->verbosityDL = │ │ │ │ │ +verbosityDLTranslator(verbosityDL); } │ │ │ │ │ +59 │ │ │ │ │ +60private: │ │ │ │ │ +61 VerbosityDL verbosityDLTranslator(const std::string& verbosityDL) const; │ │ │ │ │ +62 std::string verbosityDLTranslator(VerbosityDL verbosityDL) const; │ │ │ │ │ +63}; │ │ │ │ │ +64 │ │ │ │ │ +_6_8class GTSAM_EXPORT _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r : public _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r { │ │ │ │ │ +69 │ │ │ │ │ +70protected: │ │ │ │ │ +71 _D_o_g_l_e_g_P_a_r_a_m_s params_; │ │ │ │ │ +72 │ │ │ │ │ +73public: │ │ │ │ │ +74 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +75 │ │ │ │ │ +78 │ │ │ │ │ +87 _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, const _V_a_l_u_e_s& │ │ │ │ │ +initialValues, │ │ │ │ │ +88 const _D_o_g_l_e_g_P_a_r_a_m_s& params = _D_o_g_l_e_g_P_a_r_a_m_s()); │ │ │ │ │ +89 │ │ │ │ │ +97 _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, const _V_a_l_u_e_s& │ │ │ │ │ +initialValues, │ │ │ │ │ +98 const _O_r_d_e_r_i_n_g& ordering); │ │ │ │ │ +99 │ │ │ │ │ +101 │ │ │ │ │ +104 │ │ │ │ │ +_1_0_6 _~_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r() override {} │ │ │ │ │ +107 │ │ │ │ │ +112 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r iterate() override; │ │ │ │ │ +113 │ │ │ │ │ +_1_1_5 const _D_o_g_l_e_g_P_a_r_a_m_s& _p_a_r_a_m_s() const { return params_; } │ │ │ │ │ +116 │ │ │ │ │ +118 double getDelta() const; │ │ │ │ │ +119 │ │ │ │ │ +121 │ │ │ │ │ +122protected: │ │ │ │ │ +_1_2_4 const _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s& ___p_a_r_a_m_s() const override { return params_; │ │ │ │ │ +} │ │ │ │ │ +125 │ │ │ │ │ +127 _D_o_g_l_e_g_P_a_r_a_m_s ensureHasOrdering(_D_o_g_l_e_g_P_a_r_a_m_s params, const │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph) const; │ │ │ │ │ +128}; │ │ │ │ │ +129 │ │ │ │ │ +130} │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ +Base class and parameters for nonlinear optimization algorithms. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_Q_R │ │ │ │ │ -std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > │ │ │ │ │ -EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys) │ │ │ │ │ -Multiply all factors and eliminate the given keys from the resulting factor │ │ │ │ │ -using a QR variant that h... │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.cpp:789 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ -FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ -fast_pool_allocator instead of the defa... │ │ │ │ │ -DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l │ │ │ │ │ -This class contains the implementation of the Dogleg algorithm. │ │ │ │ │ -DDeeffiinniittiioonn DoglegOptimizerImpl.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_:_:_T_r_u_s_t_R_e_g_i_o_n_A_d_a_p_t_a_t_i_o_n_M_o_d_e │ │ │ │ │ -TrustRegionAdaptationMode │ │ │ │ │ -Specifies how the trust region is adapted at each Dogleg iteration. │ │ │ │ │ -DDeeffiinniittiioonn DoglegOptimizerImpl.h:53 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s │ │ │ │ │ -Parameters for ISAM2 using Gauss-Newton optimization. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s_:_:_w_i_l_d_f_i_r_e_T_h_r_e_s_h_o_l_d │ │ │ │ │ -double wildfireThreshold │ │ │ │ │ -Continue updating the linear delta only when changes are above this threshold │ │ │ │ │ -(default: 0.... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s_:_:_I_S_A_M_2_G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s │ │ │ │ │ -ISAM2GaussNewtonParams(double _wildfireThreshold=0.001) │ │ │ │ │ -Specify parameters as constructor arguments. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_D_o_g_l_e_g_P_a_r_a_m_s │ │ │ │ │ -Parameters for ISAM2 using Dogleg optimization. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_D_o_g_l_e_g_P_a_r_a_m_s_:_:_a_d_a_p_t_a_t_i_o_n_M_o_d_e │ │ │ │ │ -DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationMode │ │ │ │ │ -See description in DoglegOptimizerImpl::TrustRegionAdaptationMode. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:73 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_D_o_g_l_e_g_P_a_r_a_m_s_:_:_v_e_r_b_o_s_e │ │ │ │ │ -bool verbose │ │ │ │ │ -Whether Dogleg prints iteration and convergence information. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:76 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_D_o_g_l_e_g_P_a_r_a_m_s_:_:_I_S_A_M_2_D_o_g_l_e_g_P_a_r_a_m_s │ │ │ │ │ -ISAM2DoglegParams(double _initialDelta=1.0, double _wildfireThreshold=1e-5, │ │ │ │ │ -DoglegOptimizerImpl::TrustRegionAdaptationMode │ │ │ │ │ -_adaptationMode=DoglegOptimizerImpl::SEARCH_EACH_ITERATION, bool │ │ │ │ │ -_verbose=false) │ │ │ │ │ -Specify parameters as constructor arguments. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:79 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_D_o_g_l_e_g_P_a_r_a_m_s_:_:_i_n_i_t_i_a_l_D_e_l_t_a │ │ │ │ │ -double initialDelta │ │ │ │ │ -The initial trust region radius for Dogleg. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:68 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_D_o_g_l_e_g_P_a_r_a_m_s_:_:_w_i_l_d_f_i_r_e_T_h_r_e_s_h_o_l_d │ │ │ │ │ -double wildfireThreshold │ │ │ │ │ -Continue updating the linear delta only when changes are above this threshold │ │ │ │ │ -(default: 1e-5) │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:70 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:135 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_e_n_a_b_l_e_P_a_r_t_i_a_l_R_e_l_i_n_e_a_r_i_z_a_t_i_o_n_C_h_e_c_k │ │ │ │ │ -bool enablePartialRelinearizationCheck │ │ │ │ │ -Check variables for relinearization in tree-order, stopping the check once a │ │ │ │ │ -variable does not need t... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:218 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_o_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ -OptimizationParams optimizationParams │ │ │ │ │ -Optimization parameters, this both selects the nonlinear optimization method │ │ │ │ │ -and specifies its parame... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:150 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &str="") const │ │ │ │ │ -print iSAM2 parameters │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:252 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_e_n_a_b_l_e_R_e_l_i_n_e_a_r_i_z_a_t_i_o_n │ │ │ │ │ -bool enableRelinearization │ │ │ │ │ -Controls whether ISAM2 will ever relinearize any variables (default: true) │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:174 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_c_a_c_h_e_L_i_n_e_a_r_i_z_e_d_F_a_c_t_o_r_s │ │ │ │ │ -bool cacheLinearizedFactors │ │ │ │ │ -Whether to cache linear factors (default: true). │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:200 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_e_v_a_l_u_a_t_e_N_o_n_l_i_n_e_a_r_E_r_r_o_r │ │ │ │ │ -bool evaluateNonlinearError │ │ │ │ │ -Whether to evaluate the nonlinear error before and after the update, to return │ │ │ │ │ -in ISAM2Result from up... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:177 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_e_n_a_b_l_e_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s │ │ │ │ │ -bool enableDetailedResults │ │ │ │ │ -Whether to compute and return ISAM2Result::detailedResults, this can increase │ │ │ │ │ -running time (default: ... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:206 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ -boost::variant< ISAM2GaussNewtonParams, ISAM2DoglegParams > OptimizationParams │ │ │ │ │ -Either ISAM2GaussNewtonParams or ISAM2DoglegParams. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:137 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_f_i_n_d_U_n_u_s_e_d_F_a_c_t_o_r_S_l_o_t_s │ │ │ │ │ -bool findUnusedFactorSlots │ │ │ │ │ -When you will be removing many factors, e.g. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:224 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_R_e_l_i_n_e_a_r_i_z_a_t_i_o_n_T_h_r_e_s_h_o_l_d │ │ │ │ │ -boost::variant< double, FastMap< char, Vector > > RelinearizationThreshold │ │ │ │ │ -Either a constant relinearization threshold or a per-variable-type set of │ │ │ │ │ -thresholds. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:140 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_r_e_l_i_n_e_a_r_i_z_e_S_k_i_p │ │ │ │ │ -int relinearizeSkip │ │ │ │ │ -Only relinearize any variables every relinearizeSkip calls to ISAM2::update │ │ │ │ │ -(default: 10) │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:170 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_f_a_c_t_o_r_i_z_a_t_i_o_n │ │ │ │ │ -Factorization factorization │ │ │ │ │ -Specifies whether to use QR or CHOESKY numerical factorization (default: │ │ │ │ │ -CHOLESKY). │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:193 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_k_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -KeyFormatter keyFormatter │ │ │ │ │ -A KeyFormatter for when keys are printed during debugging (default: │ │ │ │ │ -DefaultKeyFormatter) │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:203 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_r_e_l_i_n_e_a_r_i_z_e_T_h_r_e_s_h_o_l_d │ │ │ │ │ -RelinearizationThreshold relinearizeThreshold │ │ │ │ │ -Only relinearize variables whose linear delta magnitude is greater than this │ │ │ │ │ -threshold (default: 0.... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:168 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_I_S_A_M_2_P_a_r_a_m_s │ │ │ │ │ -ISAM2Params(OptimizationParams _optimizationParams=ISAM2GaussNewtonParams(), │ │ │ │ │ -RelinearizationThreshold _relinearizeThreshold=0.1, int _relinearizeSkip=10, │ │ │ │ │ -bool _enableRelinearization=true, bool _evaluateNonlinearError=false, │ │ │ │ │ -Factorization _factorization=ISAM2Params::CHOLESKY, bool │ │ │ │ │ -_cacheLinearizedFactors=true, const KeyFormatter │ │ │ │ │ -&_keyFormatter=DefaultKeyFormatter, bool _enableDetailedResults=false) │ │ │ │ │ -Specify parameters as constructor arguments See the documentation of member │ │ │ │ │ -variables above. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:230 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:81 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_g_l_e_g_P_a_r_a_m_s │ │ │ │ │ +Parameters for Levenberg-Marquardt optimization. │ │ │ │ │ +DDeeffiinniittiioonn DoglegOptimizer.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_g_l_e_g_P_a_r_a_m_s_:_:_V_e_r_b_o_s_i_t_y_D_L │ │ │ │ │ +VerbosityDL │ │ │ │ │ +See DoglegParams::dlVerbosity. │ │ │ │ │ +DDeeffiinniittiioonn DoglegOptimizer.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_g_l_e_g_P_a_r_a_m_s_:_:_v_e_r_b_o_s_i_t_y_D_L │ │ │ │ │ +VerbosityDL verbosityDL │ │ │ │ │ +The verbosity level for Dogleg (default: SILENT), see also │ │ │ │ │ +NonlinearOptimizerParams::verbosity. │ │ │ │ │ +DDeeffiinniittiioonn DoglegOptimizer.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_g_l_e_g_P_a_r_a_m_s_:_:_d_e_l_t_a_I_n_i_t_i_a_l │ │ │ │ │ +double deltaInitial │ │ │ │ │ +The initial trust region radius (default: 10.0) │ │ │ │ │ +DDeeffiinniittiioonn DoglegOptimizer.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r │ │ │ │ │ +This class performs Dogleg nonlinear optimization. │ │ │ │ │ +DDeeffiinniittiioonn DoglegOptimizer.h:68 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_:_:_~_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r │ │ │ │ │ +~DoglegOptimizer() override │ │ │ │ │ +Virtual destructor. │ │ │ │ │ +DDeeffiinniittiioonn DoglegOptimizer.h:106 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_:_:___p_a_r_a_m_s │ │ │ │ │ +const NonlinearOptimizerParams & _params() const override │ │ │ │ │ +Access the parameters (base class version) │ │ │ │ │ +DDeeffiinniittiioonn DoglegOptimizer.h:124 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_:_:_p_a_r_a_m_s │ │ │ │ │ +const DoglegParams & params() const │ │ │ │ │ +Read-only access the parameters. │ │ │ │ │ +DDeeffiinniittiioonn DoglegOptimizer.h:115 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r │ │ │ │ │ +This is the abstract interface for classes that can optimize for the maximum- │ │ │ │ │ +likelihood estimate of a... │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizer.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s │ │ │ │ │ +The common parameters for Nonlinear optimizers. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizerParams.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _I_S_A_M_2_P_a_r_a_m_s_._h │ │ │ │ │ + * _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01031.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/CustomFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/GraphvizFormatting.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,53 +94,36 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs
│ │ │ │ -
CustomFactor.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
GraphvizFormatting.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Class to enable arbitrary factors with runtime swappable error function. │ │ │ │ +

Graphviz formatter for NonlinearFactorGraph. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::CustomFactor
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Typedefs

│ │ │ │ -using gtsam::JacobianVector = std::vector< Matrix >
 
│ │ │ │ -using gtsam::CustomErrorFunction = std::function< Vector(const CustomFactor &, const Values &, const JacobianVector *)>
 
│ │ │ │

Detailed Description

│ │ │ │ -

Class to enable arbitrary factors with runtime swappable error function.

│ │ │ │ -
Author
Fan Jiang
│ │ │ │ +

Graphviz formatter for NonlinearFactorGraph.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
Date
December, 2021
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,31 +1,22 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ -CustomFactor.h File Reference │ │ │ │ │ -Class to enable arbitrary factors with runtime swappable error function. │ │ │ │ │ -_M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_C_u_s_t_o_m_F_a_c_t_o_r │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +GraphvizFormatting.cpp File Reference │ │ │ │ │ +Graphviz formatter for NonlinearFactorGraph. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -using  ggttssaamm::::JJaaccoobbiiaannVVeeccttoorr = std::vector< Matrix > │ │ │ │ │ -  │ │ │ │ │ -using  ggttssaamm::::CCuussttoommEErrrroorrFFuunnccttiioonn = std::function< Vector(const _C_u_s_t_o_m_F_a_c_t_o_r &, │ │ │ │ │ - const _V_a_l_u_e_s &, const JacobianVector *)> │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Class to enable arbitrary factors with runtime swappable error function. │ │ │ │ │ +Graphviz formatter for NonlinearFactorGraph. │ │ │ │ │ Author │ │ │ │ │ - Fan Jiang │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + December, 2021 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _C_u_s_t_o_m_F_a_c_t_o_r_._h │ │ │ │ │ + * _G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01034.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/DoglegOptimizer.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Expression.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,39 +94,90 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs
│ │ │ │ -
DoglegOptimizer.cpp File Reference
│ │ │ │ +Functions
│ │ │ │ +
Expression.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ + │ │ │ │ +

Expressions for Block Automatic Differentiation. │ │ │ │ +More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::Expression< T >
 Expression class that supports automatic differentiation. More...
 
struct  gtsam::Expression< T >::UnaryFunction< A1 >
 
struct  gtsam::Expression< T >::BinaryFunction< A1, A2 >
 
struct  gtsam::Expression< T >::TernaryFunction< A1, A2, A3 >
 
class  gtsam::ScalarMultiplyExpression< T >
 A ScalarMultiplyExpression is a specialization of Expression that multiplies with a scalar It optimizes the Jacobian calculation for this specific case. More...
 
class  gtsam::BinarySumExpression< T >
 A BinarySumExpression is a specialization of Expression that adds two expressions together It optimizes the Jacobian calculation for this specific case. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ -Typedefs

│ │ │ │ -typedef internal::DoglegState gtsam::State
 

│ │ │ │ +Functions

template<typename T , typename A >
Expression< T > gtsam::linearExpression (const std::function< T(A)> &f, const Expression< A > &expression, const Eigen::Matrix< double, traits< T >::dimension, traits< A >::dimension > &dTdA)
 Create an expression out of a linear function f:T->A with (constant) Jacobian dTdA TODO(frank): create a more efficient version like ScalarMultiplyExpression.
 
│ │ │ │ +template<typename T >
ScalarMultiplyExpression< T > gtsam::operator* (double s, const Expression< T > &e)
 Construct an expression that executes the scalar multiplication with an input expression The type T must be a vector space Example: Expression<Point2> a(0), b = 12 * a;.
 
│ │ │ │ +template<typename T >
BinarySumExpression< T > gtsam::operator+ (const Expression< T > &e1, const Expression< T > &e2)
 Construct an expression that sums two input expressions of the same type T The type T must be a vector space Example: Expression<Point2> a(0), b(1), c = a + b;.
 
│ │ │ │ +template<typename T >
BinarySumExpression< T > gtsam::operator- (const Expression< T > &e1, const Expression< T > &e2)
 Construct an expression that subtracts one expression from another.
 
template<typename T >
Expression< T > gtsam::operator* (const Expression< T > &expression1, const Expression< T > &expression2)
 Construct a product expression, assumes T::compose(T) -> T.
 
template<typename T >
std::vector< Expression< T > > gtsam::createUnknowns (size_t n, char c, size_t start)
 Construct an array of leaves.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Feb 26, 2012
│ │ │ │ +

Expressions for Block Automatic Differentiation.

│ │ │ │ +
Date
September 18, 2014
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ +Paul Furgale
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,89 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ -DoglegOptimizer.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Expression.h File Reference │ │ │ │ │ +Expressions for Block Automatic Differentiation. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_<_ _T_ _> │ │ │ │ │ +  _E_x_p_r_e_s_s_i_o_n class that supports automatic differentiation. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_<_ _T_ _>_:_:_U_n_a_r_y_F_u_n_c_t_i_o_n_<_ _A_1_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_<_ _T_ _>_:_:_B_i_n_a_r_y_F_u_n_c_t_i_o_n_<_ _A_1_,_ _A_2_ _> │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_<_ _T_ _>_:_:_T_e_r_n_a_r_y_F_u_n_c_t_i_o_n_<_ _A_1_,_ _A_2_,_ _A_3_ _> │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_S_c_a_l_a_r_M_u_l_t_i_p_l_y_E_x_p_r_e_s_s_i_o_n_<_ _T_ _> │ │ │ │ │ + A _S_c_a_l_a_r_M_u_l_t_i_p_l_y_E_x_p_r_e_s_s_i_o_n is a specialization of _E_x_p_r_e_s_s_i_o_n that │ │ │ │ │ +  multiplies with a scalar It optimizes the Jacobian calculation for │ │ │ │ │ + this specific case. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_B_i_n_a_r_y_S_u_m_E_x_p_r_e_s_s_i_o_n_<_ _T_ _> │ │ │ │ │ + A _B_i_n_a_r_y_S_u_m_E_x_p_r_e_s_s_i_o_n is a specialization of _E_x_p_r_e_s_s_i_o_n that adds two │ │ │ │ │ +  expressions together It optimizes the Jacobian calculation for this │ │ │ │ │ + specific case. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -typedef internal::DoglegState  ggttssaamm::::SSttaattee │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +template │ │ │ │ │ + _E_x_p_r_e_s_s_i_o_n< T >  _g_t_s_a_m_:_:_l_i_n_e_a_r_E_x_p_r_e_s_s_i_o_n (const std::function< T │ │ │ │ │ + (A)> &f, const _E_x_p_r_e_s_s_i_o_n< A > &expression, │ │ │ │ │ + const Eigen::Matrix< double, _t_r_a_i_t_s< T >:: │ │ │ │ │ + dimension, _t_r_a_i_t_s< A >::dimension > &dTdA) │ │ │ │ │ + Create an expression out of a linear function │ │ │ │ │ +  f:T->A with (constant) Jacobian dTdA TODO │ │ │ │ │ + (frank): create a more efficient version like │ │ │ │ │ + _S_c_a_l_a_r_M_u_l_t_i_p_l_y_E_x_p_r_e_s_s_i_o_n. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _S_c_a_l_a_r_M_u_l_t_i_p_l_y_E_x_p_r_e_s_s_i_o_n< T >  ggttssaamm::::ooppeerraattoorr** (double s, const _E_x_p_r_e_s_s_i_o_n< T │ │ │ │ │ + > &e) │ │ │ │ │ + Construct an expression that executes the │ │ │ │ │ +  scalar multiplication with an input expression │ │ │ │ │ + The type T must be a vector space Example: │ │ │ │ │ + Expression a(0), b = 12 * a;. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _B_i_n_a_r_y_S_u_m_E_x_p_r_e_s_s_i_o_n< T >  ggttssaamm::::ooppeerraattoorr++ (const _E_x_p_r_e_s_s_i_o_n< T > &e1, │ │ │ │ │ + const _E_x_p_r_e_s_s_i_o_n< T > &e2) │ │ │ │ │ + Construct an expression that sums two input │ │ │ │ │ +  expressions of the same type T The type T must │ │ │ │ │ + be a vector space Example: Expression a │ │ │ │ │ + (0), b(1), c = a + b;. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _B_i_n_a_r_y_S_u_m_E_x_p_r_e_s_s_i_o_n< T >  ggttssaamm::::ooppeerraattoorr-- (const _E_x_p_r_e_s_s_i_o_n< T > &e1, │ │ │ │ │ + const _E_x_p_r_e_s_s_i_o_n< T > &e2) │ │ │ │ │ +  Construct an expression that subtracts one │ │ │ │ │ + expression from another. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + _E_x_p_r_e_s_s_i_o_n< T >  _g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_* (const _E_x_p_r_e_s_s_i_o_n< T > │ │ │ │ │ + &expression1, const _E_x_p_r_e_s_s_i_o_n< T > │ │ │ │ │ + &expression2) │ │ │ │ │ +  Construct a product expression, assumes T:: │ │ │ │ │ + compose(T) -> T. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +std::vector< _E_x_p_r_e_s_s_i_o_n< T > >  _g_t_s_a_m_:_:_c_r_e_a_t_e_U_n_k_n_o_w_n_s (size_t n, char c, size_t │ │ │ │ │ + start) │ │ │ │ │ +  Construct an array of leaves. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ +Expressions for Block Automatic Differentiation. │ │ │ │ │ Date │ │ │ │ │ - Feb 26, 2012 │ │ │ │ │ + September 18, 2014 │ │ │ │ │ + Author │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Paul Furgale │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_._c_p_p │ │ │ │ │ + * _E_x_p_r_e_s_s_i_o_n_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01040.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Clique.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,54 +94,37 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
ISAM2Clique.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
NonlinearFactor.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Specialized iSAM2 Clique. │ │ │ │ +

Nonlinear Factor base classes. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::ISAM2Clique
 Specialized Clique structure for ISAM2, incorporating caching and gradient contribution TODO: more documentation. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

size_t gtsam::optimizeWildfire (const ISAM2Clique::shared_ptr &root, double threshold, const KeySet &replaced, VectorValues *delta)
 Optimize the BayesTree, starting from the root.
 
│ │ │ │ -size_t gtsam::optimizeWildfireNonRecursive (const ISAM2Clique::shared_ptr &root, double threshold, const KeySet &keys, VectorValues *delta)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Specialized iSAM2 Clique.

│ │ │ │ -
Author
Michael Kaess, Richard Roberts
│ │ │ │ +

Nonlinear Factor base classes.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,34 +1,21 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -ISAM2Clique.h File Reference │ │ │ │ │ -Specialized iSAM2 Clique. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_I_S_A_M_2_C_l_i_q_u_e │ │ │ │ │ -  Specialized Clique structure for _I_S_A_M_2, incorporating caching and │ │ │ │ │ - gradient contribution TODO: more documentation. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +NonlinearFactor.cpp File Reference │ │ │ │ │ +Nonlinear Factor base classes. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -size_t  _g_t_s_a_m_:_:_o_p_t_i_m_i_z_e_W_i_l_d_f_i_r_e (const ISAM2Clique::shared_ptr &root, double │ │ │ │ │ - threshold, const _K_e_y_S_e_t &replaced, _V_e_c_t_o_r_V_a_l_u_e_s *delta) │ │ │ │ │ -  Optimize the _B_a_y_e_s_T_r_e_e, starting from the root. │ │ │ │ │ -  │ │ │ │ │ -size_t  ggttssaamm::::ooppttiimmiizzeeWWiillddffiirreeNNoonnRReeccuurrssiivvee (const ISAM2Clique::shared_ptr │ │ │ │ │ - &root, double threshold, const _K_e_y_S_e_t &keys, _V_e_c_t_o_r_V_a_l_u_e_s *delta) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Specialized iSAM2 Clique. │ │ │ │ │ +Nonlinear Factor base classes. │ │ │ │ │ Author │ │ │ │ │ - Michael Kaess, Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _I_S_A_M_2_C_l_i_q_u_e_._h │ │ │ │ │ + * _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01043.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2-impl.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearOptimizer.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,44 +94,56 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces | │ │ │ │ Functions
│ │ │ │ -
ISAM2-impl.cpp File Reference
│ │ │ │ +
NonlinearOptimizer.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. │ │ │ │ +

Base class and parameters for nonlinear optimization algorithms. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::NonlinearOptimizer
 This is the abstract interface for classes that can optimize for the maximum-likelihood estimate of a NonlinearFactorGraph. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

│ │ │ │ -void gtsam::internal::updateRgProd (const ISAM2::sharedClique &clique, const KeySet &replacedKeys, const VectorValues &grad, VectorValues *RgProd, size_t *varsUpdated)
 
│ │ │ │ +bool gtsam::checkConvergence (double relativeErrorTreshold, double absoluteErrorTreshold, double errorThreshold, double currentError, double newError, NonlinearOptimizerParams::Verbosity verbosity=NonlinearOptimizerParams::SILENT)
 Check whether the relative error decrease is less than relativeErrorTreshold, the absolute error decrease is less than absoluteErrorTreshold, or the error itself is less than errorThreshold.
 
│ │ │ │ +GTSAM_EXPORT bool gtsam::checkConvergence (const NonlinearOptimizerParams &params, double currentError, double newError)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.

│ │ │ │ -
Author
Michael Kaess
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ +

Base class and parameters for nonlinear optimization algorithms.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Sep 7, 2009
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,41 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -ISAM2-impl.cpp File Reference │ │ │ │ │ -Incremental update functionality (ISAM2) for BayesTree, with fluid │ │ │ │ │ -relinearization. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +NonlinearOptimizer.h File Reference │ │ │ │ │ +Base class and parameters for nonlinear optimization algorithms. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r │ │ │ │ │ +  This is the abstract interface for classes that can optimize for the │ │ │ │ │ + maximum-likelihood estimate of a _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -void  ggttssaamm::::iinntteerrnnaall::::uuppddaatteeRRggPPrroodd (const _I_S_A_M_2_:_:_s_h_a_r_e_d_C_l_i_q_u_e &clique, const │ │ │ │ │ - _K_e_y_S_e_t &replacedKeys, const _V_e_c_t_o_r_V_a_l_u_e_s &grad, _V_e_c_t_o_r_V_a_l_u_e_s *RgProd, │ │ │ │ │ - size_t *varsUpdated) │ │ │ │ │ + bool  ggttssaamm::::cchheecckkCCoonnvveerrggeennccee (double relativeErrorTreshold, │ │ │ │ │ + double absoluteErrorTreshold, double errorThreshold, double │ │ │ │ │ + currentError, double newError, _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_: │ │ │ │ │ + _V_e_r_b_o_s_i_t_y verbosity=NonlinearOptimizerParams::SILENT) │ │ │ │ │ + Check whether the relative error decrease is less than │ │ │ │ │ +  relativeErrorTreshold, the absolute error decrease is less │ │ │ │ │ + than absoluteErrorTreshold, oorr the error itself is less than │ │ │ │ │ + errorThreshold. │ │ │ │ │ +  │ │ │ │ │ +GTSAM_EXPORT bool  ggttssaamm::::cchheecckkCCoonnvveerrggeennccee (const _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s │ │ │ │ │ + ¶ms, double currentError, double newError) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Incremental update functionality (ISAM2) for BayesTree, with fluid │ │ │ │ │ -relinearization. │ │ │ │ │ +Base class and parameters for nonlinear optimization algorithms. │ │ │ │ │ Author │ │ │ │ │ - Michael Kaess │ │ │ │ │ Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Sep 7, 2009 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _I_S_A_M_2_-_i_m_p_l_._c_p_p │ │ │ │ │ + * _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01046.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/DoglegOptimizerImpl.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,36 +94,46 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
NonlinearConjugateGradientOptimizer.cpp File Reference
│ │ │ │ +
DoglegOptimizerImpl.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Simple non-linear optimizer that solves using non-preconditioned CG. │ │ │ │ +

Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail implementation) │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

struct  gtsam::DoglegOptimizerImpl
 This class contains the implementation of the Dogleg algorithm. More...
 
struct  gtsam::DoglegOptimizerImpl::IterationResult
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Simple non-linear optimizer that solves using non-preconditioned CG.

│ │ │ │ -
Author
Yong-Dian Jian
│ │ │ │ -
Date
Jun 11, 2012
│ │ │ │ +

Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail implementation)

│ │ │ │ +
Author
Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,30 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -NonlinearConjugateGradientOptimizer.cpp File Reference │ │ │ │ │ -Simple non-linear optimizer that solves using nnoonn--pprreeccoonnddiittiioonneedd CG. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +DoglegOptimizerImpl.h File Reference │ │ │ │ │ +Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail │ │ │ │ │ +implementation) _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +struct   _g_t_s_a_m_:_:_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l │ │ │ │ │ +  This class contains the implementation of the Dogleg algorithm. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_:_:_I_t_e_r_a_t_i_o_n_R_e_s_u_l_t │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Simple non-linear optimizer that solves using nnoonn--pprreeccoonnddiittiioonneedd CG. │ │ │ │ │ +Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail │ │ │ │ │ +implementation) │ │ │ │ │ Author │ │ │ │ │ - Yong-Dian Jian │ │ │ │ │ - Date │ │ │ │ │ - Jun 11, 2012 │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _N_o_n_l_i_n_e_a_r_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_O_p_t_i_m_i_z_e_r_._c_p_p │ │ │ │ │ + * _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01049.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearOptimizer.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Result.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,47 +94,50 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
NonlinearOptimizer.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
ISAM2Result.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Convergence functions not dependent on graph types. │ │ │ │ +

Class that stores detailed iSAM2 result. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

struct  gtsam::ISAM2Result
 This struct is returned from ISAM2::update() and contains information about the update that is useful for determining whether the solution is converging, and about how much work was required for the update. More...
 
struct  gtsam::ISAM2Result::DetailedResults
 A struct holding detailed results, which must be enabled with ISAM2Params::enableDetailedResults. More...
 
struct  gtsam::ISAM2Result::DetailedResults::VariableStatus
 The status of a single variable, this struct is stored in DetailedResults::variableStatus. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -bool gtsam::checkConvergence (double relativeErrorTreshold, double absoluteErrorTreshold, double errorThreshold, double currentError, double newError, NonlinearOptimizerParams::Verbosity verbosity=NonlinearOptimizerParams::SILENT)
 Check whether the relative error decrease is less than relativeErrorTreshold, the absolute error decrease is less than absoluteErrorTreshold, or the error itself is less than errorThreshold.
 
│ │ │ │ -GTSAM_EXPORT bool gtsam::checkConvergence (const NonlinearOptimizerParams &params, double currentError, double newError)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Convergence functions not dependent on graph types.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
Date
Jul 17, 2010
│ │ │ │ +

Class that stores detailed iSAM2 result.

│ │ │ │ +
Author
Michael Kaess, Richard Roberts, Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,35 +1,36 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -NonlinearOptimizer.cpp File Reference │ │ │ │ │ -Convergence functions not dependent on graph types. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +ISAM2Result.h File Reference │ │ │ │ │ +Class that stores detailed iSAM2 result. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +struct   _g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t │ │ │ │ │ + This struct is returned from _I_S_A_M_2_:_:_u_p_d_a_t_e_(_) and contains information │ │ │ │ │ +  about the update that is useful for determining whether the solution │ │ │ │ │ + is converging, and about how much work was required for the update. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s │ │ │ │ │ +  A struct holding detailed results, which must be enabled with │ │ │ │ │ + _I_S_A_M_2_P_a_r_a_m_s_:_:_e_n_a_b_l_e_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s_:_:_V_a_r_i_a_b_l_e_S_t_a_t_u_s │ │ │ │ │ +  The status of a single variable, this struct is stored in │ │ │ │ │ + _D_e_t_a_i_l_e_d_R_e_s_u_l_t_s_:_:_v_a_r_i_a_b_l_e_S_t_a_t_u_s. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ - bool  ggttssaamm::::cchheecckkCCoonnvveerrggeennccee (double relativeErrorTreshold, │ │ │ │ │ - double absoluteErrorTreshold, double errorThreshold, double │ │ │ │ │ - currentError, double newError, _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_: │ │ │ │ │ - _V_e_r_b_o_s_i_t_y verbosity=NonlinearOptimizerParams::SILENT) │ │ │ │ │ - Check whether the relative error decrease is less than │ │ │ │ │ -  relativeErrorTreshold, the absolute error decrease is less │ │ │ │ │ - than absoluteErrorTreshold, oorr the error itself is less than │ │ │ │ │ - errorThreshold. │ │ │ │ │ -  │ │ │ │ │ -GTSAM_EXPORT bool  ggttssaamm::::cchheecckkCCoonnvveerrggeennccee (const _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s │ │ │ │ │ - ¶ms, double currentError, double newError) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Convergence functions not dependent on graph types. │ │ │ │ │ +Class that stores detailed iSAM2 result. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - Jul 17, 2010 │ │ │ │ │ + Michael Kaess, Richard Roberts, Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_._c_p_p │ │ │ │ │ + * _I_S_A_M_2_R_e_s_u_l_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01049.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,4 @@ │ │ │ │ │ var a01049 = [ │ │ │ │ │ - ["checkConvergence", "a01049.html#a3382482454c164c6f5817dcbe6f932b3", null] │ │ │ │ │ + ["gtsam::ISAM2Result::DetailedResults", "a04424.html", "a04424"], │ │ │ │ │ + ["gtsam::ISAM2Result::DetailedResults::VariableStatus", "a04428.html", "a04428"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01055.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Expression.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearOptimizerParams.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,88 +96,56 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ -Functions
│ │ │ │ -
Expression.h File Reference
│ │ │ │ +Typedefs
│ │ │ │ +
NonlinearOptimizerParams.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Expressions for Block Automatic Differentiation. │ │ │ │ +

Parameters for nonlinear optimization. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::Expression< T >
 Expression class that supports automatic differentiation. More...
 
struct  gtsam::Expression< T >::UnaryFunction< A1 >
 
struct  gtsam::Expression< T >::BinaryFunction< A1, A2 >
 
struct  gtsam::Expression< T >::TernaryFunction< A1, A2, A3 >
 
class  gtsam::ScalarMultiplyExpression< T >
 A ScalarMultiplyExpression is a specialization of Expression that multiplies with a scalar It optimizes the Jacobian calculation for this specific case. More...
 
class  gtsam::BinarySumExpression< T >
 A BinarySumExpression is a specialization of Expression that adds two expressions together It optimizes the Jacobian calculation for this specific case. More...
class  gtsam::NonlinearOptimizerParams
 The common parameters for Nonlinear optimizers. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ -Functions

template<typename T , typename A >
Expression< T > gtsam::linearExpression (const std::function< T(A)> &f, const Expression< A > &expression, const Eigen::Matrix< double, traits< T >::dimension, traits< A >::dimension > &dTdA)
 Create an expression out of a linear function f:T->A with (constant) Jacobian dTdA TODO(frank): create a more efficient version like ScalarMultiplyExpression.
 
│ │ │ │ -template<typename T >
ScalarMultiplyExpression< T > gtsam::operator* (double s, const Expression< T > &e)
 Construct an expression that executes the scalar multiplication with an input expression The type T must be a vector space Example: Expression<Point2> a(0), b = 12 * a;.
 
│ │ │ │ -template<typename T >
BinarySumExpression< T > gtsam::operator+ (const Expression< T > &e1, const Expression< T > &e2)
 Construct an expression that sums two input expressions of the same type T The type T must be a vector space Example: Expression<Point2> a(0), b(1), c = a + b;.
 
│ │ │ │ -template<typename T >
BinarySumExpression< T > gtsam::operator- (const Expression< T > &e1, const Expression< T > &e2)
 Construct an expression that subtracts one expression from another.
 
template<typename T >
Expression< T > gtsam::operator* (const Expression< T > &expression1, const Expression< T > &expression2)
 Construct a product expression, assumes T::compose(T) -> T.
 
template<typename T >
std::vector< Expression< T > > gtsam::createUnknowns (size_t n, char c, size_t start)
 Construct an array of leaves.
 

│ │ │ │ +Typedefs

│ │ │ │ +typedef NonlinearOptimizerParams gtsam::SuccessiveLinearizationParams
 
│ │ │ │

Detailed Description

│ │ │ │ -

Expressions for Block Automatic Differentiation.

│ │ │ │ -
Date
September 18, 2014
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +

Parameters for nonlinear optimization.

│ │ │ │ +
Author
Yong-Dian Jian
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │
│ │ │ │ -Paul Furgale
│ │ │ │ +Andrew Melim
│ │ │ │ +
Date
Apr 1, 2012
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,89 +1,33 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Expression.h File Reference │ │ │ │ │ -Expressions for Block Automatic Differentiation. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ +NonlinearOptimizerParams.h File Reference │ │ │ │ │ +Parameters for nonlinear optimization. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_<_ _T_ _> │ │ │ │ │ -  _E_x_p_r_e_s_s_i_o_n class that supports automatic differentiation. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_<_ _T_ _>_:_:_U_n_a_r_y_F_u_n_c_t_i_o_n_<_ _A_1_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_<_ _T_ _>_:_:_B_i_n_a_r_y_F_u_n_c_t_i_o_n_<_ _A_1_,_ _A_2_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_<_ _T_ _>_:_:_T_e_r_n_a_r_y_F_u_n_c_t_i_o_n_<_ _A_1_,_ _A_2_,_ _A_3_ _> │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_S_c_a_l_a_r_M_u_l_t_i_p_l_y_E_x_p_r_e_s_s_i_o_n_<_ _T_ _> │ │ │ │ │ - A _S_c_a_l_a_r_M_u_l_t_i_p_l_y_E_x_p_r_e_s_s_i_o_n is a specialization of _E_x_p_r_e_s_s_i_o_n that │ │ │ │ │ -  multiplies with a scalar It optimizes the Jacobian calculation for │ │ │ │ │ - this specific case. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_B_i_n_a_r_y_S_u_m_E_x_p_r_e_s_s_i_o_n_<_ _T_ _> │ │ │ │ │ - A _B_i_n_a_r_y_S_u_m_E_x_p_r_e_s_s_i_o_n is a specialization of _E_x_p_r_e_s_s_i_o_n that adds two │ │ │ │ │ -  expressions together It optimizes the Jacobian calculation for this │ │ │ │ │ - specific case. _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s │ │ │ │ │ +  The common parameters for Nonlinear optimizers. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ - _E_x_p_r_e_s_s_i_o_n< T >  _g_t_s_a_m_:_:_l_i_n_e_a_r_E_x_p_r_e_s_s_i_o_n (const std::function< T │ │ │ │ │ - (A)> &f, const _E_x_p_r_e_s_s_i_o_n< A > &expression, │ │ │ │ │ - const Eigen::Matrix< double, _t_r_a_i_t_s< T >:: │ │ │ │ │ - dimension, _t_r_a_i_t_s< A >::dimension > &dTdA) │ │ │ │ │ - Create an expression out of a linear function │ │ │ │ │ -  f:T->A with (constant) Jacobian dTdA TODO │ │ │ │ │ - (frank): create a more efficient version like │ │ │ │ │ - _S_c_a_l_a_r_M_u_l_t_i_p_l_y_E_x_p_r_e_s_s_i_o_n. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - _S_c_a_l_a_r_M_u_l_t_i_p_l_y_E_x_p_r_e_s_s_i_o_n< T >  ggttssaamm::::ooppeerraattoorr** (double s, const _E_x_p_r_e_s_s_i_o_n< T │ │ │ │ │ - > &e) │ │ │ │ │ - Construct an expression that executes the │ │ │ │ │ -  scalar multiplication with an input expression │ │ │ │ │ - The type T must be a vector space Example: │ │ │ │ │ - Expression a(0), b = 12 * a;. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - _B_i_n_a_r_y_S_u_m_E_x_p_r_e_s_s_i_o_n< T >  ggttssaamm::::ooppeerraattoorr++ (const _E_x_p_r_e_s_s_i_o_n< T > &e1, │ │ │ │ │ - const _E_x_p_r_e_s_s_i_o_n< T > &e2) │ │ │ │ │ - Construct an expression that sums two input │ │ │ │ │ -  expressions of the same type T The type T must │ │ │ │ │ - be a vector space Example: Expression a │ │ │ │ │ - (0), b(1), c = a + b;. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - _B_i_n_a_r_y_S_u_m_E_x_p_r_e_s_s_i_o_n< T >  ggttssaamm::::ooppeerraattoorr-- (const _E_x_p_r_e_s_s_i_o_n< T > &e1, │ │ │ │ │ - const _E_x_p_r_e_s_s_i_o_n< T > &e2) │ │ │ │ │ -  Construct an expression that subtracts one │ │ │ │ │ - expression from another. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - _E_x_p_r_e_s_s_i_o_n< T >  _g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_* (const _E_x_p_r_e_s_s_i_o_n< T > │ │ │ │ │ - &expression1, const _E_x_p_r_e_s_s_i_o_n< T > │ │ │ │ │ - &expression2) │ │ │ │ │ -  Construct a product expression, assumes T:: │ │ │ │ │ - compose(T) -> T. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -std::vector< _E_x_p_r_e_s_s_i_o_n< T > >  _g_t_s_a_m_:_:_c_r_e_a_t_e_U_n_k_n_o_w_n_s (size_t n, char c, size_t │ │ │ │ │ - start) │ │ │ │ │ -  Construct an array of leaves. │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +typedef _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s  ggttssaamm::::SSuucccceessssiivveeLLiinneeaarriizzaattiioonnPPaarraammss │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Expressions for Block Automatic Differentiation. │ │ │ │ │ - Date │ │ │ │ │ - September 18, 2014 │ │ │ │ │ +Parameters for nonlinear optimization. │ │ │ │ │ Author │ │ │ │ │ + Yong-Dian Jian │ │ │ │ │ + Richard Roberts │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Paul Furgale │ │ │ │ │ + Andrew Melim │ │ │ │ │ + Date │ │ │ │ │ + Apr 1, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _E_x_p_r_e_s_s_i_o_n_._h │ │ │ │ │ + * _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01055.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,14 +1,3 @@ │ │ │ │ │ var a01055 = [ │ │ │ │ │ - ["gtsam::Expression< T >", "a04292.html", "a04292"], │ │ │ │ │ - ["gtsam::Expression< T >::UnaryFunction< A1 >", "a04296.html", null], │ │ │ │ │ - ["gtsam::Expression< T >::BinaryFunction< A1, A2 >", "a04300.html", null], │ │ │ │ │ - ["gtsam::Expression< T >::TernaryFunction< A1, A2, A3 >", "a04304.html", null], │ │ │ │ │ - ["gtsam::ScalarMultiplyExpression< T >", "a04308.html", null], │ │ │ │ │ - ["gtsam::BinarySumExpression< T >", "a04312.html", null], │ │ │ │ │ - ["createUnknowns", "a01055.html#a51881c20dd5ecdd129cb993ea8374846", null], │ │ │ │ │ - ["linearExpression", "a01055.html#a794029fe8dfc03f67a8cb64ad05d47c9", null], │ │ │ │ │ - ["operator*", "a01055.html#a3db519caa1b7e43412c9a7e13acf8329", null], │ │ │ │ │ - ["operator*", "a01055.html#a405f3793afc2cc3173c3fbdecadb162a", null], │ │ │ │ │ - ["operator+", "a01055.html#a589e3ff8cd71e5a17ee1dcfae8b2df26", null], │ │ │ │ │ - ["operator-", "a01055.html#a9512eb02324210895442f2bf8268902d", null] │ │ │ │ │ + ["gtsam::NonlinearOptimizerParams", "a04556.html", "a04556"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01055_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Expression.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearOptimizerParams.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,310 +98,196 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Expression.h
│ │ │ │ +
NonlinearOptimizerParams.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/nonlinear/internal/JacobianMap.h>
│ │ │ │ -
23#include <gtsam/inference/Symbol.h>
│ │ │ │ -
24#include <gtsam/base/OptionalJacobian.h>
│ │ │ │ -
25#include <gtsam/base/VectorSpace.h>
│ │ │ │ -
26
│ │ │ │ -
27#include <boost/make_shared.hpp>
│ │ │ │ -
28#include <map>
│ │ │ │ -
29
│ │ │ │ -
30// Forward declare tests
│ │ │ │ -
31class ExpressionFactorShallowTest;
│ │ │ │ -
32
│ │ │ │ -
33namespace gtsam {
│ │ │ │ -
34
│ │ │ │ -
35// Forward declares
│ │ │ │ -
36class Values;
│ │ │ │ -
37template<typename T> class ExpressionFactor;
│ │ │ │ -
38
│ │ │ │ -
39namespace internal {
│ │ │ │ -
40template<typename T> class ExecutionTrace;
│ │ │ │ -
41template<typename T> class ExpressionNode;
│ │ │ │ -
42}
│ │ │ │ -
43
│ │ │ │ -
47template<typename T>
│ │ │ │ -
│ │ │ │ -
48class Expression {
│ │ │ │ -
49
│ │ │ │ -
50public:
│ │ │ │ -
51
│ │ │ │ -
53 typedef Expression<T> type;
│ │ │ │ -
54
│ │ │ │ -
55protected:
│ │ │ │ -
56
│ │ │ │ -
57 // Paul's trick shared pointer, polymorphic root of entire expression tree
│ │ │ │ -
58 boost::shared_ptr<internal::ExpressionNode<T> > root_;
│ │ │ │ -
59
│ │ │ │ -
61 Expression(const boost::shared_ptr<internal::ExpressionNode<T> >& root) : root_(root) {}
│ │ │ │ -
62
│ │ │ │ -
63public:
│ │ │ │ +
22#pragma once
│ │ │ │ +
23
│ │ │ │ +
24#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ +
25#include <gtsam/linear/SubgraphSolver.h>
│ │ │ │ +
26#include <boost/optional.hpp>
│ │ │ │ +
27#include <string>
│ │ │ │ +
28
│ │ │ │ +
29namespace gtsam {
│ │ │ │ +
30
│ │ │ │ +
│ │ │ │ +
34class GTSAM_EXPORT NonlinearOptimizerParams {
│ │ │ │ +
35public:
│ │ │ │ +
│ │ │ │ +
37 enum Verbosity {
│ │ │ │ +
38 SILENT, TERMINATION, ERROR, VALUES, DELTA, LINEAR
│ │ │ │ +
39 };
│ │ │ │ +
│ │ │ │ +
40
│ │ │ │ +
41 size_t maxIterations = 100;
│ │ │ │ +
42 double relativeErrorTol = 1e-5;
│ │ │ │ +
43 double absoluteErrorTol = 1e-5;
│ │ │ │ +
44 double errorTol = 0.0;
│ │ │ │ +
45 Verbosity verbosity = SILENT;
│ │ │ │ +
46 Ordering::OrderingType orderingType = Ordering::COLAMD;
│ │ │ │ +
47
│ │ │ │ +
48 size_t getMaxIterations() const { return maxIterations; }
│ │ │ │ +
49 double getRelativeErrorTol() const { return relativeErrorTol; }
│ │ │ │ +
50 double getAbsoluteErrorTol() const { return absoluteErrorTol; }
│ │ │ │ +
51 double getErrorTol() const { return errorTol; }
│ │ │ │ +
52 std::string getVerbosity() const { return verbosityTranslator(verbosity); }
│ │ │ │ +
53
│ │ │ │ +
54 void setMaxIterations(int value) { maxIterations = value; }
│ │ │ │ +
55 void setRelativeErrorTol(double value) { relativeErrorTol = value; }
│ │ │ │ +
56 void setAbsoluteErrorTol(double value) { absoluteErrorTol = value; }
│ │ │ │ +
57 void setErrorTol(double value) { errorTol = value; }
│ │ │ │ +
58 void setVerbosity(const std::string& src) {
│ │ │ │ +
59 verbosity = verbosityTranslator(src);
│ │ │ │ +
60 }
│ │ │ │ +
61
│ │ │ │ +
62 static Verbosity verbosityTranslator(const std::string &s) ;
│ │ │ │ +
63 static std::string verbosityTranslator(Verbosity value) ;
│ │ │ │
64
│ │ │ │ -
65 // Expressions wrap trees of functions that can evaluate their own derivatives.
│ │ │ │ -
66 // The meta-functions below are useful to specify the type of those functions.
│ │ │ │ -
67 // Example, a function taking a camera and a 3D point and yielding a 2D point:
│ │ │ │ -
68 // Expression<Point2>::BinaryFunction<PinholeCamera<Cal3_S2>,Point3>::type
│ │ │ │ -
69 template<class A1>
│ │ │ │ -
│ │ │ │ -
70 struct UnaryFunction {
│ │ │ │ -
71 typedef std::function<
│ │ │ │ -
72 T(const A1&, typename MakeOptionalJacobian<T, A1>::type)> type;
│ │ │ │ -
73 };
│ │ │ │ -
│ │ │ │ -
74
│ │ │ │ -
75 template<class A1, class A2>
│ │ │ │ -
│ │ │ │ -
76 struct BinaryFunction {
│ │ │ │ -
77 typedef std::function<
│ │ │ │ -
78 T(const A1&, const A2&, typename MakeOptionalJacobian<T, A1>::type,
│ │ │ │ -
79 typename MakeOptionalJacobian<T, A2>::type)> type;
│ │ │ │ -
80 };
│ │ │ │ -
│ │ │ │ -
81
│ │ │ │ -
82 template<class A1, class A2, class A3>
│ │ │ │ -
│ │ │ │ -
83 struct TernaryFunction {
│ │ │ │ -
84 typedef std::function<
│ │ │ │ -
85 T(const A1&, const A2&, const A3&,
│ │ │ │ -
86 typename MakeOptionalJacobian<T, A1>::type,
│ │ │ │ -
87 typename MakeOptionalJacobian<T, A2>::type,
│ │ │ │ -
88 typename MakeOptionalJacobian<T, A3>::type)> type;
│ │ │ │ -
89 };
│ │ │ │ -
│ │ │ │ -
90
│ │ │ │ -
92 Expression(const T& value);
│ │ │ │ -
93
│ │ │ │ -
95 Expression(const Key& key);
│ │ │ │ -
96
│ │ │ │ -
98 Expression(const Symbol& symbol);
│ │ │ │ -
99
│ │ │ │ -
101 Expression(unsigned char c, std::uint64_t j);
│ │ │ │ -
102
│ │ │ │ -
104 template<typename A>
│ │ │ │ -
105 Expression(typename UnaryFunction<A>::type function,
│ │ │ │ -
106 const Expression<A>& expression);
│ │ │ │ -
107
│ │ │ │ -
109 template<typename A1, typename A2>
│ │ │ │ -
110 Expression(typename BinaryFunction<A1, A2>::type function,
│ │ │ │ -
111 const Expression<A1>& expression1, const Expression<A2>& expression2);
│ │ │ │ -
112
│ │ │ │ -
114 template<typename A1, typename A2, typename A3>
│ │ │ │ -
115 Expression(typename TernaryFunction<A1, A2, A3>::type function,
│ │ │ │ -
116 const Expression<A1>& expression1, const Expression<A2>& expression2,
│ │ │ │ -
117 const Expression<A3>& expression3);
│ │ │ │ -
118
│ │ │ │ -
120 template<typename A>
│ │ │ │ -
121 Expression(const Expression<A>& expression,
│ │ │ │ -
122 T (A::*method)(typename MakeOptionalJacobian<T, A>::type) const);
│ │ │ │ -
123
│ │ │ │ -
125 template<typename A1, typename A2>
│ │ │ │ -
126 Expression(const Expression<A1>& expression1,
│ │ │ │ -
127 T (A1::*method)(const A2&, typename MakeOptionalJacobian<T, A1>::type,
│ │ │ │ -
128 typename MakeOptionalJacobian<T, A2>::type) const,
│ │ │ │ -
129 const Expression<A2>& expression2);
│ │ │ │ -
130
│ │ │ │ -
132 template<typename A1, typename A2, typename A3>
│ │ │ │ -
133 Expression(const Expression<A1>& expression1,
│ │ │ │ -
134 T (A1::*method)(const A2&, const A3&,
│ │ │ │ -
135 typename MakeOptionalJacobian<T, A1>::type,
│ │ │ │ -
136 typename MakeOptionalJacobian<T, A2>::type,
│ │ │ │ -
137 typename MakeOptionalJacobian<T, A3>::type) const,
│ │ │ │ -
138 const Expression<A2>& expression2, const Expression<A3>& expression3);
│ │ │ │ -
139
│ │ │ │ -
│ │ │ │ -
141 virtual ~Expression() {
│ │ │ │ -
142 }
│ │ │ │ -
│ │ │ │ -
143
│ │ │ │ -
145 std::set<Key> keys() const;
│ │ │ │ -
146
│ │ │ │ -
148 void dims(std::map<Key, int>& map) const;
│ │ │ │ -
149
│ │ │ │ -
151 void print(const std::string& s) const;
│ │ │ │ -
152
│ │ │ │ -
158 T value(const Values& values, boost::optional<std::vector<Matrix>&> H =
│ │ │ │ -
159 boost::none) const;
│ │ │ │ +
67 using IterationHook = std::function<
│ │ │ │ +
68 void(size_t /*iteration*/, double/*errorBefore*/, double/*errorAfter*/)>;
│ │ │ │ +
69
│ │ │ │ +
94 IterationHook iterationHook;
│ │ │ │ +
95
│ │ │ │ +
│ │ │ │ +
97 enum LinearSolverType {
│ │ │ │ +
98 MULTIFRONTAL_CHOLESKY,
│ │ │ │ +
99 MULTIFRONTAL_QR,
│ │ │ │ +
100 SEQUENTIAL_CHOLESKY,
│ │ │ │ +
101 SEQUENTIAL_QR,
│ │ │ │ +
102 Iterative, /* Experimental Flag */
│ │ │ │ +
103 CHOLMOD, /* Experimental Flag */
│ │ │ │ +
104 };
│ │ │ │ +
│ │ │ │ +
105
│ │ │ │ +
106 LinearSolverType linearSolverType = MULTIFRONTAL_CHOLESKY;
│ │ │ │ +
107 boost::optional<Ordering> ordering;
│ │ │ │ +
108 IterativeOptimizationParameters::shared_ptr iterativeParams;
│ │ │ │ +
109
│ │ │ │ +
110 NonlinearOptimizerParams() = default;
│ │ │ │ +
111 virtual ~NonlinearOptimizerParams() {
│ │ │ │ +
112 }
│ │ │ │ +
113
│ │ │ │ +
114 virtual void print(const std::string& str = "") const;
│ │ │ │ +
115
│ │ │ │ +
116 bool equals(const NonlinearOptimizerParams& other, double tol = 1e-9) const {
│ │ │ │ +
117 return maxIterations == other.getMaxIterations()
│ │ │ │ +
118 && std::abs(relativeErrorTol - other.getRelativeErrorTol()) <= tol
│ │ │ │ +
119 && std::abs(absoluteErrorTol - other.getAbsoluteErrorTol()) <= tol
│ │ │ │ +
120 && std::abs(errorTol - other.getErrorTol()) <= tol
│ │ │ │ +
121 && verbosityTranslator(verbosity) == other.getVerbosity();
│ │ │ │ +
122 // && orderingType.equals(other.getOrderingType()_;
│ │ │ │ +
123 // && linearSolverType == other.getLinearSolverType();
│ │ │ │ +
124 // TODO: check ordering, iterativeParams, and iterationsHook
│ │ │ │ +
125 }
│ │ │ │ +
126
│ │ │ │ +
127 inline bool isMultifrontal() const {
│ │ │ │ +
128 return (linearSolverType == MULTIFRONTAL_CHOLESKY)
│ │ │ │ +
129 || (linearSolverType == MULTIFRONTAL_QR);
│ │ │ │ +
130 }
│ │ │ │ +
131
│ │ │ │ +
132 inline bool isSequential() const {
│ │ │ │ +
133 return (linearSolverType == SEQUENTIAL_CHOLESKY)
│ │ │ │ +
134 || (linearSolverType == SEQUENTIAL_QR);
│ │ │ │ +
135 }
│ │ │ │ +
136
│ │ │ │ +
137 inline bool isCholmod() const {
│ │ │ │ +
138 return (linearSolverType == CHOLMOD);
│ │ │ │ +
139 }
│ │ │ │ +
140
│ │ │ │ +
141 inline bool isIterative() const {
│ │ │ │ +
142 return (linearSolverType == Iterative);
│ │ │ │ +
143 }
│ │ │ │ +
144
│ │ │ │ +
145 GaussianFactorGraph::Eliminate getEliminationFunction() const {
│ │ │ │ +
146 switch (linearSolverType) {
│ │ │ │ +
147 case MULTIFRONTAL_CHOLESKY:
│ │ │ │ +
148 case SEQUENTIAL_CHOLESKY:
│ │ │ │ +
149 return EliminatePreferCholesky;
│ │ │ │ +
150
│ │ │ │ +
151 case MULTIFRONTAL_QR:
│ │ │ │ +
152 case SEQUENTIAL_QR:
│ │ │ │ +
153 return EliminateQR;
│ │ │ │ +
154
│ │ │ │ +
155 default:
│ │ │ │ +
156 throw std::runtime_error(
│ │ │ │ +
157 "Nonlinear optimization parameter \"factorization\" is invalid");
│ │ │ │ +
158 }
│ │ │ │ +
159 }
│ │ │ │
160
│ │ │ │ -
│ │ │ │ -
166 virtual boost::shared_ptr<Expression> clone() const {
│ │ │ │ -
167 return boost::make_shared<Expression>(*this);
│ │ │ │ -
168 }
│ │ │ │ -
│ │ │ │ -
169
│ │ │ │ -
171 const boost::shared_ptr<internal::ExpressionNode<T> >& root() const;
│ │ │ │ -
172
│ │ │ │ -
174 size_t traceSize() const;
│ │ │ │ +
161 std::string getLinearSolverType() const {
│ │ │ │ +
162 return linearSolverTranslator(linearSolverType);
│ │ │ │ +
163 }
│ │ │ │ +
164
│ │ │ │ +
165 void setLinearSolverType(const std::string& solver) {
│ │ │ │ +
166 linearSolverType = linearSolverTranslator(solver);
│ │ │ │ +
167 }
│ │ │ │ +
168
│ │ │ │ +
169 void setIterativeParams(const boost::shared_ptr<IterativeOptimizationParameters> params);
│ │ │ │ +
170
│ │ │ │ +
171 void setOrdering(const Ordering& ordering) {
│ │ │ │ +
172 this->ordering = ordering;
│ │ │ │ +
173 this->orderingType = Ordering::CUSTOM;
│ │ │ │ +
174 }
│ │ │ │
175
│ │ │ │ -
177 Expression<T>& operator+=(const Expression<T>& e);
│ │ │ │ -
178
│ │ │ │ -
179protected:
│ │ │ │ -
180
│ │ │ │ -
182 Expression() {}
│ │ │ │ -
183
│ │ │ │ -
185 typedef std::pair<KeyVector, FastVector<int> > KeysAndDims;
│ │ │ │ -
186 KeysAndDims keysAndDims() const;
│ │ │ │ -
187
│ │ │ │ -
189 T valueAndDerivatives(const Values& values, const KeyVector& keys,
│ │ │ │ -
190 const FastVector<int>& dims, std::vector<Matrix>& H) const;
│ │ │ │ +
176 std::string getOrderingType() const {
│ │ │ │ +
177 return orderingTypeTranslator(orderingType);
│ │ │ │ +
178 }
│ │ │ │ +
179
│ │ │ │ +
180 // Note that if you want to use a custom ordering, you must set the ordering directly, this will switch to custom type
│ │ │ │ +
181 void setOrderingType(const std::string& ordering){
│ │ │ │ +
182 orderingType = orderingTypeTranslator(ordering);
│ │ │ │ +
183 }
│ │ │ │ +
184
│ │ │ │ +
185private:
│ │ │ │ +
186 std::string linearSolverTranslator(LinearSolverType linearSolverType) const;
│ │ │ │ +
187 LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const;
│ │ │ │ +
188 std::string orderingTypeTranslator(Ordering::OrderingType type) const;
│ │ │ │ +
189 Ordering::OrderingType orderingTypeTranslator(const std::string& type) const;
│ │ │ │ +
190};
│ │ │ │ +
│ │ │ │
191
│ │ │ │ -
193 T traceExecution(const Values& values, internal::ExecutionTrace<T>& trace,
│ │ │ │ -
194 void* traceStorage) const;
│ │ │ │ -
195
│ │ │ │ -
197 T valueAndJacobianMap(const Values& values,
│ │ │ │ -
198 internal::JacobianMap& jacobians) const;
│ │ │ │ -
199
│ │ │ │ -
200 // be very selective on who can access these private methods:
│ │ │ │ -
201 friend class ExpressionFactor<T> ;
│ │ │ │ -
202 friend class internal::ExpressionNode<T>;
│ │ │ │ -
203
│ │ │ │ -
204 // and add tests
│ │ │ │ -
205 friend class ::ExpressionFactorShallowTest;
│ │ │ │ -
206};
│ │ │ │ -
│ │ │ │ -
207
│ │ │ │ -
212template <typename T>
│ │ │ │ -
│ │ │ │ -
213class ScalarMultiplyExpression : public Expression<T> {
│ │ │ │ -
214 // Check that T is a vector space
│ │ │ │ -
215 BOOST_CONCEPT_ASSERT((gtsam::IsVectorSpace<T>));
│ │ │ │ -
216
│ │ │ │ -
217 public:
│ │ │ │ -
218 explicit ScalarMultiplyExpression(double s, const Expression<T>& e);
│ │ │ │ -
219};
│ │ │ │ -
│ │ │ │ -
220
│ │ │ │ -
225template <typename T>
│ │ │ │ -
│ │ │ │ -
226class BinarySumExpression : public Expression<T> {
│ │ │ │ -
227 // Check that T is a vector space
│ │ │ │ -
228 BOOST_CONCEPT_ASSERT((gtsam::IsVectorSpace<T>));
│ │ │ │ -
229
│ │ │ │ -
230 public:
│ │ │ │ -
231 explicit BinarySumExpression(const Expression<T>& e1, const Expression<T>& e2);
│ │ │ │ -
232};
│ │ │ │ -
│ │ │ │ -
233
│ │ │ │ -
234
│ │ │ │ -
240template <typename T, typename A>
│ │ │ │ -
│ │ │ │ -
241Expression<T> linearExpression(
│ │ │ │ -
242 const std::function<T(A)>& f, const Expression<A>& expression,
│ │ │ │ -
243 const Eigen::Matrix<double, traits<T>::dimension, traits<A>::dimension>& dTdA) {
│ │ │ │ -
244 // Use lambda to endow f with a linear Jacobian
│ │ │ │ -
245 typename Expression<T>::template UnaryFunction<A>::type g =
│ │ │ │ -
246 [=](const A& value, typename MakeOptionalJacobian<T, A>::type H) {
│ │ │ │ -
247 if (H)
│ │ │ │ -
248 *H << dTdA;
│ │ │ │ -
249 return f(value);
│ │ │ │ -
250 };
│ │ │ │ -
251 return Expression<T>(g, expression);
│ │ │ │ -
252}
│ │ │ │ -
│ │ │ │ -
253
│ │ │ │ -
260template <typename T>
│ │ │ │ -
│ │ │ │ -
261ScalarMultiplyExpression<T> operator*(double s, const Expression<T>& e) {
│ │ │ │ -
262 return ScalarMultiplyExpression<T>(s, e);
│ │ │ │ -
263}
│ │ │ │ -
│ │ │ │ -
264
│ │ │ │ -
271template <typename T>
│ │ │ │ -
│ │ │ │ -
272BinarySumExpression<T> operator+(const Expression<T>& e1, const Expression<T>& e2) {
│ │ │ │ -
273 return BinarySumExpression<T>(e1, e2);
│ │ │ │ -
274}
│ │ │ │ -
│ │ │ │ -
275
│ │ │ │ -
277template <typename T>
│ │ │ │ -
│ │ │ │ -
278BinarySumExpression<T> operator-(const Expression<T>& e1, const Expression<T>& e2) {
│ │ │ │ -
279 // TODO(frank, abe): Implement an actual negate operator instead of multiplying by -1
│ │ │ │ -
280 return e1 + (-1.0) * e2;
│ │ │ │ -
281}
│ │ │ │ -
│ │ │ │ -
282
│ │ │ │ -
288template<typename T>
│ │ │ │ -
289Expression<T> operator*(const Expression<T>& e1, const Expression<T>& e2);
│ │ │ │ -
290
│ │ │ │ -
296template<typename T>
│ │ │ │ -
297std::vector<Expression<T> > createUnknowns(size_t n, char c, size_t start = 0);
│ │ │ │ -
298
│ │ │ │ -
299} // namespace gtsam
│ │ │ │ -
300
│ │ │ │ -
301#include <gtsam/nonlinear/Expression-inl.h>
│ │ │ │ -
302
│ │ │ │ -
OptionalJacobian.h
Special class for optional Jacobian arguments.
│ │ │ │ -
Expression-inl.h
Internals for Expression.h, not for general consumption.
│ │ │ │ -
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │ +
192// For backward compatibility:
│ │ │ │ +
193typedef NonlinearOptimizerParams SuccessiveLinearizationParams;
│ │ │ │ +
194
│ │ │ │ +
195} /* namespace gtsam */
│ │ │ │ +
SubgraphSolver.h
Subgraph Solver from IROS 2010.
│ │ │ │ +
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ +
gtsam::EliminatePreferCholesky
std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< GaussianFactor > > EliminatePreferCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
Densely partially eliminate with Cholesky factorization.
Definition HessianFactor.cpp:548
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::symbol
Key symbol(unsigned char c, std::uint64_t j)
Create a symbol key from a character and index, i.e.
Definition Symbol.h:135
│ │ │ │ -
gtsam::createUnknowns
std::vector< Expression< T > > createUnknowns(size_t n, char c, size_t start)
Construct an array of leaves.
Definition Expression-inl.h:284
│ │ │ │ -
gtsam::operator+
Errors operator+(const Errors &a, const Errors &b)
Addition.
Definition Errors.cpp:60
│ │ │ │ -
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
│ │ │ │ -
gtsam::linearExpression
Expression< T > linearExpression(const std::function< T(A)> &f, const Expression< A > &expression, const Eigen::Matrix< double, traits< T >::dimension, traits< A >::dimension > &dTdA)
Create an expression out of a linear function f:T->A with (constant) Jacobian dTdA TODO(frank): creat...
Definition Expression.h:241
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::operator-
Errors operator-(const Errors &a, const Errors &b)
Subtraction.
Definition Errors.cpp:75
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::MakeOptionalJacobian
: meta-function to generate JacobianTA optional reference Used mainly by Expressions
Definition OptionalJacobian.h:261
│ │ │ │ -
gtsam::IsVectorSpace
Vector Space concept.
Definition VectorSpace.h:470
│ │ │ │ -
gtsam::Symbol
Character and index key used to refer to variables.
Definition Symbol.h:35
│ │ │ │ -
gtsam::ExpressionFactor
Factor that supports arbitrary expressions via AD.
Definition ExpressionFactor.h:44
│ │ │ │ -
gtsam::internal::ExecutionTrace
Definition Expression.h:40
│ │ │ │ -
gtsam::internal::ExpressionNode
Definition Expression.h:41
│ │ │ │ -
gtsam::Expression
Expression class that supports automatic differentiation.
Definition Expression.h:48
│ │ │ │ -
gtsam::Expression::root
const boost::shared_ptr< internal::ExpressionNode< T > > & root() const
Return root.
Definition Expression-inl.h:162
│ │ │ │ -
gtsam::Expression::type
Expression< T > type
Define type so we can apply it as a meta-function.
Definition Expression.h:53
│ │ │ │ -
gtsam::Expression::Expression
Expression()
Default constructor, for serialization.
Definition Expression.h:182
│ │ │ │ -
gtsam::Expression::~Expression
virtual ~Expression()
Destructor.
Definition Expression.h:141
│ │ │ │ -
gtsam::Expression::clone
virtual boost::shared_ptr< Expression > clone() const
Definition Expression.h:166
│ │ │ │ -
gtsam::Expression::Expression
Expression(const boost::shared_ptr< internal::ExpressionNode< T > > &root)
Construct with a custom root.
Definition Expression.h:61
│ │ │ │ -
gtsam::Expression::keys
std::set< Key > keys() const
Return keys that play in this expression.
Definition Expression-inl.h:132
│ │ │ │ -
gtsam::Expression::KeysAndDims
std::pair< KeyVector, FastVector< int > > KeysAndDims
Keys and dimensions in same order.
Definition Expression.h:185
│ │ │ │ -
gtsam::Expression::dims
void dims(std::map< Key, int > &map) const
Return dimensions for each argument, as a map.
Definition Expression-inl.h:137
│ │ │ │ -
gtsam::Expression::print
void print(const std::string &s) const
Print.
Definition Expression-inl.h:142
│ │ │ │ -
gtsam::Expression::valueAndJacobianMap
T valueAndJacobianMap(const Values &values, internal::JacobianMap &jacobians) const
brief Return value and derivatives, reverse AD version
Definition Expression-inl.h:205
│ │ │ │ -
gtsam::Expression::valueAndDerivatives
T valueAndDerivatives(const Values &values, const KeyVector &keys, const FastVector< int > &dims, std::vector< Matrix > &H) const
private version that takes keys and dimensions, returns derivatives
Definition Expression-inl.h:174
│ │ │ │ -
gtsam::Expression::value
T value(const Values &values, boost::optional< std::vector< Matrix > & > H=boost::none) const
Return value and optional derivatives, reverse AD version Notes: this is not terribly efficient,...
Definition Expression-inl.h:147
│ │ │ │ -
gtsam::Expression::operator+=
Expression< T > & operator+=(const Expression< T > &e)
Add another expression to this expression.
Definition Expression-inl.h:302
│ │ │ │ -
gtsam::Expression::traceSize
size_t traceSize() const
Return size needed for memory buffer in traceExecution.
Definition Expression-inl.h:167
│ │ │ │ -
gtsam::Expression::traceExecution
T traceExecution(const Values &values, internal::ExecutionTrace< T > &trace, void *traceStorage) const
trace execution, very unsafe
Definition Expression-inl.h:198
│ │ │ │ -
gtsam::Expression::UnaryFunction
Definition Expression.h:70
│ │ │ │ -
gtsam::Expression::BinaryFunction
Definition Expression.h:76
│ │ │ │ -
gtsam::Expression::TernaryFunction
Definition Expression.h:83
│ │ │ │ -
gtsam::ScalarMultiplyExpression
A ScalarMultiplyExpression is a specialization of Expression that multiplies with a scalar It optimiz...
Definition Expression.h:213
│ │ │ │ -
gtsam::BinarySumExpression
A BinarySumExpression is a specialization of Expression that adds two expressions together It optimiz...
Definition Expression.h:226
│ │ │ │ -
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │ - │ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::EliminateQR
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Multiply all factors and eliminate the given keys from the resulting factor using a QR variant that h...
Definition JacobianFactor.cpp:789
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::Ordering::OrderingType
OrderingType
Type of ordering to use.
Definition Ordering.h:41
│ │ │ │ +
gtsam::NonlinearOptimizerParams
The common parameters for Nonlinear optimizers.
Definition NonlinearOptimizerParams.h:34
│ │ │ │ +
gtsam::NonlinearOptimizerParams::iterationHook
IterationHook iterationHook
Optional user-provided iteration hook to be called after each optimization iteration (Default: none).
Definition NonlinearOptimizerParams.h:94
│ │ │ │ +
gtsam::NonlinearOptimizerParams::iterativeParams
IterativeOptimizationParameters::shared_ptr iterativeParams
The container for iterativeOptimization parameters. used in CG Solvers.
Definition NonlinearOptimizerParams.h:108
│ │ │ │ +
gtsam::NonlinearOptimizerParams::Verbosity
Verbosity
See NonlinearOptimizerParams::verbosity.
Definition NonlinearOptimizerParams.h:37
│ │ │ │ +
gtsam::NonlinearOptimizerParams::LinearSolverType
LinearSolverType
See NonlinearOptimizerParams::linearSolverType.
Definition NonlinearOptimizerParams.h:97
│ │ │ │ +
gtsam::NonlinearOptimizerParams::IterationHook
std::function< void(size_t, double, double)> IterationHook
Type for an optional user-provided hook to be called after each internal optimizer iteration.
Definition NonlinearOptimizerParams.h:68
│ │ │ │ +
gtsam::NonlinearOptimizerParams::ordering
boost::optional< Ordering > ordering
The optional variable elimination ordering, or empty to use COLAMD (default: empty)
Definition NonlinearOptimizerParams.h:107
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,404 +1,233 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Expression.h │ │ │ │ │ +NonlinearOptimizerParams.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include │ │ │ │ │ -23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_S_y_m_b_o_l_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_b_a_s_e_/_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h> │ │ │ │ │ -25#include │ │ │ │ │ -26 │ │ │ │ │ -27#include │ │ │ │ │ -28#include │ │ │ │ │ -29 │ │ │ │ │ -30// Forward declare tests │ │ │ │ │ -31class ExpressionFactorShallowTest; │ │ │ │ │ -32 │ │ │ │ │ -33namespace _g_t_s_a_m { │ │ │ │ │ -34 │ │ │ │ │ -35// Forward declares │ │ │ │ │ -36class _V_a_l_u_e_s; │ │ │ │ │ -37template class ExpressionFactor; │ │ │ │ │ -38 │ │ │ │ │ -39namespace internal { │ │ │ │ │ -_4_0template class _E_x_e_c_u_t_i_o_n_T_r_a_c_e; │ │ │ │ │ -_4_1template class _E_x_p_r_e_s_s_i_o_n_N_o_d_e; │ │ │ │ │ -42} │ │ │ │ │ -43 │ │ │ │ │ -47template │ │ │ │ │ -_4_8class _E_x_p_r_e_s_s_i_o_n { │ │ │ │ │ -49 │ │ │ │ │ -50public: │ │ │ │ │ -51 │ │ │ │ │ -_5_3 typedef _E_x_p_r_e_s_s_i_o_n_<_T_> _t_y_p_e; │ │ │ │ │ -54 │ │ │ │ │ -55protected: │ │ │ │ │ -56 │ │ │ │ │ -57 // Paul's trick shared pointer, polymorphic root of entire expression tree │ │ │ │ │ -58 boost::shared_ptr > root_; │ │ │ │ │ -59 │ │ │ │ │ -_6_1 _E_x_p_r_e_s_s_i_o_n(const boost::shared_ptr<_i_n_t_e_r_n_a_l_:_:_E_x_p_r_e_s_s_i_o_n_N_o_d_e_<_T_> >& _r_o_o_t) : │ │ │ │ │ -root_(_r_o_o_t) {} │ │ │ │ │ -62 │ │ │ │ │ -63public: │ │ │ │ │ +22#pragma once │ │ │ │ │ +23 │ │ │ │ │ +24#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_S_u_b_g_r_a_p_h_S_o_l_v_e_r_._h> │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28 │ │ │ │ │ +29namespace _g_t_s_a_m { │ │ │ │ │ +30 │ │ │ │ │ +_3_4class GTSAM_EXPORT _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s { │ │ │ │ │ +35public: │ │ │ │ │ +_3_7 enum _V_e_r_b_o_s_i_t_y { │ │ │ │ │ +38 SILENT, TERMINATION, ERROR, VALUES, DELTA, LINEAR │ │ │ │ │ +39 }; │ │ │ │ │ +40 │ │ │ │ │ +_4_1 size_t maxIterations = 100; │ │ │ │ │ +_4_2 double relativeErrorTol = 1e-5; │ │ │ │ │ +_4_3 double absoluteErrorTol = 1e-5; │ │ │ │ │ +_4_4 double errorTol = 0.0; │ │ │ │ │ +_4_5 _V_e_r_b_o_s_i_t_y verbosity = SILENT; │ │ │ │ │ +_4_6 _O_r_d_e_r_i_n_g_:_:_O_r_d_e_r_i_n_g_T_y_p_e orderingType = Ordering::COLAMD; │ │ │ │ │ +47 │ │ │ │ │ +48 size_t getMaxIterations() const { return maxIterations; } │ │ │ │ │ +49 double getRelativeErrorTol() const { return relativeErrorTol; } │ │ │ │ │ +50 double getAbsoluteErrorTol() const { return absoluteErrorTol; } │ │ │ │ │ +51 double getErrorTol() const { return errorTol; } │ │ │ │ │ +52 std::string getVerbosity() const { return verbosityTranslator(verbosity); } │ │ │ │ │ +53 │ │ │ │ │ +54 void setMaxIterations(int value) { maxIterations = value; } │ │ │ │ │ +55 void setRelativeErrorTol(double value) { relativeErrorTol = value; } │ │ │ │ │ +56 void setAbsoluteErrorTol(double value) { absoluteErrorTol = value; } │ │ │ │ │ +57 void setErrorTol(double value) { errorTol = value; } │ │ │ │ │ +58 void setVerbosity(const std::string& src) { │ │ │ │ │ +59 verbosity = verbosityTranslator(src); │ │ │ │ │ +60 } │ │ │ │ │ +61 │ │ │ │ │ +62 static Verbosity verbosityTranslator(const std::string &s) ; │ │ │ │ │ +63 static std::string verbosityTranslator(Verbosity value) ; │ │ │ │ │ 64 │ │ │ │ │ -65 // Expressions wrap trees of functions that can evaluate their own │ │ │ │ │ -derivatives. │ │ │ │ │ -66 // The meta-functions below are useful to specify the type of those │ │ │ │ │ -functions. │ │ │ │ │ -67 // Example, a function taking a camera and a 3D point and yielding a 2D │ │ │ │ │ -point: │ │ │ │ │ -68 // Expression::BinaryFunction,Point3>::type │ │ │ │ │ -69 template │ │ │ │ │ -_7_0 struct _U_n_a_r_y_F_u_n_c_t_i_o_n { │ │ │ │ │ -71 typedef std::function< │ │ │ │ │ -72 T(const A1&, typename MakeOptionalJacobian::type)> type; │ │ │ │ │ -73 }; │ │ │ │ │ -74 │ │ │ │ │ -75 template │ │ │ │ │ -_7_6 struct _B_i_n_a_r_y_F_u_n_c_t_i_o_n { │ │ │ │ │ -77 typedef std::function< │ │ │ │ │ -78 T(const A1&, const A2&, typename MakeOptionalJacobian::type, │ │ │ │ │ -79 typename MakeOptionalJacobian::type)> type; │ │ │ │ │ -80 }; │ │ │ │ │ -81 │ │ │ │ │ -82 template │ │ │ │ │ -_8_3 struct _T_e_r_n_a_r_y_F_u_n_c_t_i_o_n { │ │ │ │ │ -84 typedef std::function< │ │ │ │ │ -85 T(const A1&, const A2&, const A3&, │ │ │ │ │ -86 typename MakeOptionalJacobian::type, │ │ │ │ │ -87 typename MakeOptionalJacobian::type, │ │ │ │ │ -88 typename MakeOptionalJacobian::type)> type; │ │ │ │ │ -89 }; │ │ │ │ │ -90 │ │ │ │ │ -92 _E_x_p_r_e_s_s_i_o_n(const T& _v_a_l_u_e); │ │ │ │ │ -93 │ │ │ │ │ -95 _E_x_p_r_e_s_s_i_o_n(const _K_e_y& key); │ │ │ │ │ -96 │ │ │ │ │ -98 _E_x_p_r_e_s_s_i_o_n(const _S_y_m_b_o_l& _s_y_m_b_o_l); │ │ │ │ │ -99 │ │ │ │ │ -101 _E_x_p_r_e_s_s_i_o_n(unsigned char c, std::uint64_t j); │ │ │ │ │ -102 │ │ │ │ │ -104 template │ │ │ │ │ -105 _E_x_p_r_e_s_s_i_o_n(typename UnaryFunction::type function, │ │ │ │ │ -106 const _E_x_p_r_e_s_s_i_o_n_<_A_>& expression); │ │ │ │ │ -107 │ │ │ │ │ -109 template │ │ │ │ │ -110 _E_x_p_r_e_s_s_i_o_n(typename BinaryFunction::type function, │ │ │ │ │ -111 const _E_x_p_r_e_s_s_i_o_n_<_A_1_>& expression1, const _E_x_p_r_e_s_s_i_o_n_<_A_2_>& expression2); │ │ │ │ │ -112 │ │ │ │ │ -114 template │ │ │ │ │ -115 _E_x_p_r_e_s_s_i_o_n(typename TernaryFunction::type function, │ │ │ │ │ -116 const _E_x_p_r_e_s_s_i_o_n_<_A_1_>& expression1, const _E_x_p_r_e_s_s_i_o_n_<_A_2_>& expression2, │ │ │ │ │ -117 const _E_x_p_r_e_s_s_i_o_n_<_A_3_>& expression3); │ │ │ │ │ -118 │ │ │ │ │ -120 template │ │ │ │ │ -121 _E_x_p_r_e_s_s_i_o_n(const _E_x_p_r_e_s_s_i_o_n_<_A_>& expression, │ │ │ │ │ -122 T (A::*method)(typename _M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n::_t_y_p_e) const); │ │ │ │ │ -123 │ │ │ │ │ -125 template │ │ │ │ │ -126 _E_x_p_r_e_s_s_i_o_n(const _E_x_p_r_e_s_s_i_o_n_<_A_1_>& expression1, │ │ │ │ │ -127 T (A1::*method)(const A2&, typename _M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n::_t_y_p_e, │ │ │ │ │ -128 typename _M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n::_t_y_p_e) const, │ │ │ │ │ -129 const _E_x_p_r_e_s_s_i_o_n& expression2); │ │ │ │ │ -130 │ │ │ │ │ -132 template │ │ │ │ │ -133 _E_x_p_r_e_s_s_i_o_n(const _E_x_p_r_e_s_s_i_o_n_<_A_1_>& expression1, │ │ │ │ │ -134 T (A1::*method)(const A2&, const A3&, │ │ │ │ │ -135 typename _M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n::_t_y_p_e, │ │ │ │ │ -136 typename _M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n::_t_y_p_e, │ │ │ │ │ -137 typename _M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n::_t_y_p_e) const, │ │ │ │ │ -138 const _E_x_p_r_e_s_s_i_o_n& expression2, const _E_x_p_r_e_s_s_i_o_n& expression3); │ │ │ │ │ -139 │ │ │ │ │ -_1_4_1 virtual _~_E_x_p_r_e_s_s_i_o_n() { │ │ │ │ │ -142 } │ │ │ │ │ -143 │ │ │ │ │ -145 std::set _k_e_y_s() const; │ │ │ │ │ -146 │ │ │ │ │ -148 void _d_i_m_s(std::map& map) const; │ │ │ │ │ -149 │ │ │ │ │ -151 void _p_r_i_n_t(const std::string& s) const; │ │ │ │ │ -152 │ │ │ │ │ -158 T _v_a_l_u_e(const _V_a_l_u_e_s& values, boost::optional&> H = │ │ │ │ │ -159 boost::none) const; │ │ │ │ │ +_6_7 using _I_t_e_r_a_t_i_o_n_H_o_o_k = std::function< │ │ │ │ │ +68 void(size_t /*iteration*/, double/*errorBefore*/, double/*errorAfter*/)>; │ │ │ │ │ +69 │ │ │ │ │ +_9_4 _I_t_e_r_a_t_i_o_n_H_o_o_k _i_t_e_r_a_t_i_o_n_H_o_o_k; │ │ │ │ │ +95 │ │ │ │ │ +_9_7 enum _L_i_n_e_a_r_S_o_l_v_e_r_T_y_p_e { │ │ │ │ │ +98 MULTIFRONTAL_CHOLESKY, │ │ │ │ │ +99 MULTIFRONTAL_QR, │ │ │ │ │ +100 SEQUENTIAL_CHOLESKY, │ │ │ │ │ +101 SEQUENTIAL_QR, │ │ │ │ │ +102 Iterative, /* Experimental Flag */ │ │ │ │ │ +103 CHOLMOD, /* Experimental Flag */ │ │ │ │ │ +104 }; │ │ │ │ │ +105 │ │ │ │ │ +_1_0_6 _L_i_n_e_a_r_S_o_l_v_e_r_T_y_p_e linearSolverType = MULTIFRONTAL_CHOLESKY; │ │ │ │ │ +_1_0_7 boost::optional _o_r_d_e_r_i_n_g; │ │ │ │ │ +_1_0_8 IterativeOptimizationParameters::shared_ptr _i_t_e_r_a_t_i_v_e_P_a_r_a_m_s; │ │ │ │ │ +109 │ │ │ │ │ +110 _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s() = default; │ │ │ │ │ +111 virtual _~_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s() { │ │ │ │ │ +112 } │ │ │ │ │ +113 │ │ │ │ │ +114 virtual void _p_r_i_n_t(const std::string& str = "") const; │ │ │ │ │ +115 │ │ │ │ │ +116 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s& other, double tol = 1e-9) const │ │ │ │ │ +{ │ │ │ │ │ +117 return maxIterations == other.getMaxIterations() │ │ │ │ │ +118 && std::abs(relativeErrorTol - other.getRelativeErrorTol()) <= tol │ │ │ │ │ +119 && std::abs(absoluteErrorTol - other.getAbsoluteErrorTol()) <= tol │ │ │ │ │ +120 && std::abs(errorTol - other.getErrorTol()) <= tol │ │ │ │ │ +121 && verbosityTranslator(verbosity) == other.getVerbosity(); │ │ │ │ │ +122 // && orderingType.equals(other.getOrderingType()_; │ │ │ │ │ +123 // && linearSolverType == other.getLinearSolverType(); │ │ │ │ │ +124 // TODO: check ordering, iterativeParams, and iterationsHook │ │ │ │ │ +125 } │ │ │ │ │ +126 │ │ │ │ │ +127 inline bool isMultifrontal() const { │ │ │ │ │ +128 return (linearSolverType == MULTIFRONTAL_CHOLESKY) │ │ │ │ │ +129 || (linearSolverType == MULTIFRONTAL_QR); │ │ │ │ │ +130 } │ │ │ │ │ +131 │ │ │ │ │ +132 inline bool isSequential() const { │ │ │ │ │ +133 return (linearSolverType == SEQUENTIAL_CHOLESKY) │ │ │ │ │ +134 || (linearSolverType == SEQUENTIAL_QR); │ │ │ │ │ +135 } │ │ │ │ │ +136 │ │ │ │ │ +137 inline bool isCholmod() const { │ │ │ │ │ +138 return (linearSolverType == CHOLMOD); │ │ │ │ │ +139 } │ │ │ │ │ +140 │ │ │ │ │ +141 inline bool isIterative() const { │ │ │ │ │ +142 return (linearSolverType == Iterative); │ │ │ │ │ +143 } │ │ │ │ │ +144 │ │ │ │ │ +145 GaussianFactorGraph::Eliminate getEliminationFunction() const { │ │ │ │ │ +146 switch (linearSolverType) { │ │ │ │ │ +147 case MULTIFRONTAL_CHOLESKY: │ │ │ │ │ +148 case SEQUENTIAL_CHOLESKY: │ │ │ │ │ +149 return _E_l_i_m_i_n_a_t_e_P_r_e_f_e_r_C_h_o_l_e_s_k_y; │ │ │ │ │ +150 │ │ │ │ │ +151 case MULTIFRONTAL_QR: │ │ │ │ │ +152 case SEQUENTIAL_QR: │ │ │ │ │ +153 return _E_l_i_m_i_n_a_t_e_Q_R; │ │ │ │ │ +154 │ │ │ │ │ +155 default: │ │ │ │ │ +156 throw std::runtime_error( │ │ │ │ │ +157 "Nonlinear optimization parameter \"factorization\" is invalid"); │ │ │ │ │ +158 } │ │ │ │ │ +159 } │ │ │ │ │ 160 │ │ │ │ │ -_1_6_6 virtual boost::shared_ptr _c_l_o_n_e() const { │ │ │ │ │ -167 return boost::make_shared(*this); │ │ │ │ │ -168 } │ │ │ │ │ -169 │ │ │ │ │ -171 const boost::shared_ptr >& _r_o_o_t() const; │ │ │ │ │ -172 │ │ │ │ │ -174 size_t _t_r_a_c_e_S_i_z_e() const; │ │ │ │ │ +161 std::string getLinearSolverType() const { │ │ │ │ │ +162 return linearSolverTranslator(linearSolverType); │ │ │ │ │ +163 } │ │ │ │ │ +164 │ │ │ │ │ +165 void setLinearSolverType(const std::string& solver) { │ │ │ │ │ +166 linearSolverType = linearSolverTranslator(solver); │ │ │ │ │ +167 } │ │ │ │ │ +168 │ │ │ │ │ +169 void setIterativeParams(const boost:: │ │ │ │ │ +shared_ptr params); │ │ │ │ │ +170 │ │ │ │ │ +171 void setOrdering(const Ordering& ordering) { │ │ │ │ │ +172 this->ordering = ordering; │ │ │ │ │ +173 this->orderingType = Ordering::CUSTOM; │ │ │ │ │ +174 } │ │ │ │ │ 175 │ │ │ │ │ -177 _E_x_p_r_e_s_s_i_o_n_<_T_>& _o_p_e_r_a_t_o_r_+_=(const _E_x_p_r_e_s_s_i_o_n_<_T_>& e); │ │ │ │ │ -178 │ │ │ │ │ -179protected: │ │ │ │ │ -180 │ │ │ │ │ -_1_8_2 _E_x_p_r_e_s_s_i_o_n() {} │ │ │ │ │ -183 │ │ │ │ │ -_1_8_5 typedef std::pair > _K_e_y_s_A_n_d_D_i_m_s; │ │ │ │ │ -186 _K_e_y_s_A_n_d_D_i_m_s keysAndDims() const; │ │ │ │ │ -187 │ │ │ │ │ -189 T _v_a_l_u_e_A_n_d_D_e_r_i_v_a_t_i_v_e_s(const _V_a_l_u_e_s& values, const _K_e_y_V_e_c_t_o_r& _k_e_y_s, │ │ │ │ │ -190 const _F_a_s_t_V_e_c_t_o_r_<_i_n_t_>& _d_i_m_s, std::vector& H) const; │ │ │ │ │ +176 std::string getOrderingType() const { │ │ │ │ │ +177 return orderingTypeTranslator(orderingType); │ │ │ │ │ +178 } │ │ │ │ │ +179 │ │ │ │ │ +180 // Note that if you want to use a custom ordering, you must set the │ │ │ │ │ +ordering directly, this will switch to custom type │ │ │ │ │ +181 void setOrderingType(const std::string& ordering){ │ │ │ │ │ +182 orderingType = orderingTypeTranslator(ordering); │ │ │ │ │ +183 } │ │ │ │ │ +184 │ │ │ │ │ +185private: │ │ │ │ │ +186 std::string linearSolverTranslator(LinearSolverType linearSolverType) │ │ │ │ │ +const; │ │ │ │ │ +187 LinearSolverType linearSolverTranslator(const std::string& │ │ │ │ │ +linearSolverType) const; │ │ │ │ │ +188 std::string orderingTypeTranslator(Ordering::OrderingType type) const; │ │ │ │ │ +189 Ordering::OrderingType orderingTypeTranslator(const std::string& type) │ │ │ │ │ +const; │ │ │ │ │ +190}; │ │ │ │ │ 191 │ │ │ │ │ -193 T _t_r_a_c_e_E_x_e_c_u_t_i_o_n(const _V_a_l_u_e_s& values, _i_n_t_e_r_n_a_l_:_:_E_x_e_c_u_t_i_o_n_T_r_a_c_e_<_T_>& trace, │ │ │ │ │ -194 void* traceStorage) const; │ │ │ │ │ -195 │ │ │ │ │ -197 T _v_a_l_u_e_A_n_d_J_a_c_o_b_i_a_n_M_a_p(const _V_a_l_u_e_s& values, │ │ │ │ │ -198 internal::JacobianMap& jacobians) const; │ │ │ │ │ -199 │ │ │ │ │ -200 // be very selective on who can access these private methods: │ │ │ │ │ -201 friend class _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r ; │ │ │ │ │ -202 friend class _i_n_t_e_r_n_a_l_:_:_E_x_p_r_e_s_s_i_o_n_N_o_d_e; │ │ │ │ │ -203 │ │ │ │ │ -204 // and add tests │ │ │ │ │ -205 friend class ::ExpressionFactorShallowTest; │ │ │ │ │ -206}; │ │ │ │ │ -207 │ │ │ │ │ -212template │ │ │ │ │ -_2_1_3class _S_c_a_l_a_r_M_u_l_t_i_p_l_y_E_x_p_r_e_s_s_i_o_n : public _E_x_p_r_e_s_s_i_o_n { │ │ │ │ │ -214 // Check that T is a vector space │ │ │ │ │ -215 BOOST_CONCEPT_ASSERT((_g_t_s_a_m_:_:_I_s_V_e_c_t_o_r_S_p_a_c_e_<_T_>)); │ │ │ │ │ -216 │ │ │ │ │ -217 public: │ │ │ │ │ -218 explicit _S_c_a_l_a_r_M_u_l_t_i_p_l_y_E_x_p_r_e_s_s_i_o_n(double s, const _E_x_p_r_e_s_s_i_o_n_<_T_>& e); │ │ │ │ │ -219}; │ │ │ │ │ -220 │ │ │ │ │ -225template │ │ │ │ │ -_2_2_6class _B_i_n_a_r_y_S_u_m_E_x_p_r_e_s_s_i_o_n : public _E_x_p_r_e_s_s_i_o_n { │ │ │ │ │ -227 // Check that T is a vector space │ │ │ │ │ -228 BOOST_CONCEPT_ASSERT((_g_t_s_a_m_:_:_I_s_V_e_c_t_o_r_S_p_a_c_e_<_T_>)); │ │ │ │ │ -229 │ │ │ │ │ -230 public: │ │ │ │ │ -231 explicit _B_i_n_a_r_y_S_u_m_E_x_p_r_e_s_s_i_o_n(const _E_x_p_r_e_s_s_i_o_n_<_T_>& e1, const _E_x_p_r_e_s_s_i_o_n_<_T_>& │ │ │ │ │ -e2); │ │ │ │ │ -232}; │ │ │ │ │ -233 │ │ │ │ │ -234 │ │ │ │ │ -240template │ │ │ │ │ -_2_4_1_E_x_p_r_e_s_s_i_o_n_<_T_> _l_i_n_e_a_r_E_x_p_r_e_s_s_i_o_n( │ │ │ │ │ -242 const std::function& f, const _E_x_p_r_e_s_s_i_o_n_<_A_>& expression, │ │ │ │ │ -243 const Eigen::Matrix_:_:_d_i_m_e_n_s_i_o_n, _t_r_a_i_t_s_<_A_>_:_:_d_i_m_e_n_s_i_o_n>& │ │ │ │ │ -dTdA) { │ │ │ │ │ -244 // Use lambda to endow f with a linear Jacobian │ │ │ │ │ -245 typename _E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_t_e_m_p_l_a_t_e UnaryFunction::type g = │ │ │ │ │ -246 [=](const A& value, typename MakeOptionalJacobian::type H) { │ │ │ │ │ -247 if (H) │ │ │ │ │ -248 *H << dTdA; │ │ │ │ │ -249 return f(value); │ │ │ │ │ -250 }; │ │ │ │ │ -251 return _E_x_p_r_e_s_s_i_o_n_<_T_>(g, expression); │ │ │ │ │ -252} │ │ │ │ │ -253 │ │ │ │ │ -260template │ │ │ │ │ -_2_6_1_S_c_a_l_a_r_M_u_l_t_i_p_l_y_E_x_p_r_e_s_s_i_o_n_<_T_> _o_p_e_r_a_t_o_r_*(double s, const _E_x_p_r_e_s_s_i_o_n_<_T_>& e) { │ │ │ │ │ -262 return _S_c_a_l_a_r_M_u_l_t_i_p_l_y_E_x_p_r_e_s_s_i_o_n_<_T_>(s, e); │ │ │ │ │ -263} │ │ │ │ │ -264 │ │ │ │ │ -271template │ │ │ │ │ -_2_7_2_B_i_n_a_r_y_S_u_m_E_x_p_r_e_s_s_i_o_n_<_T_> _o_p_e_r_a_t_o_r_+(const _E_x_p_r_e_s_s_i_o_n_<_T_>& e1, const │ │ │ │ │ -_E_x_p_r_e_s_s_i_o_n_<_T_>& e2) { │ │ │ │ │ -273 return _B_i_n_a_r_y_S_u_m_E_x_p_r_e_s_s_i_o_n_<_T_>(e1, e2); │ │ │ │ │ -274} │ │ │ │ │ -275 │ │ │ │ │ -277template │ │ │ │ │ -_2_7_8_B_i_n_a_r_y_S_u_m_E_x_p_r_e_s_s_i_o_n_<_T_> _o_p_e_r_a_t_o_r_-(const _E_x_p_r_e_s_s_i_o_n_<_T_>& e1, const │ │ │ │ │ -_E_x_p_r_e_s_s_i_o_n_<_T_>& e2) { │ │ │ │ │ -279 // TODO(frank, abe): Implement an actual negate operator instead of │ │ │ │ │ -multiplying by -1 │ │ │ │ │ -280 return e1 + (-1.0) * e2; │ │ │ │ │ -281} │ │ │ │ │ -282 │ │ │ │ │ -288template │ │ │ │ │ -289Expression _o_p_e_r_a_t_o_r_*(const Expression& e1, const Expression& e2); │ │ │ │ │ -290 │ │ │ │ │ -296template │ │ │ │ │ -297std::vector > _c_r_e_a_t_e_U_n_k_n_o_w_n_s(size_t n, char c, size_t start = │ │ │ │ │ -0); │ │ │ │ │ -298 │ │ │ │ │ -299} // namespace gtsam │ │ │ │ │ -300 │ │ │ │ │ -301#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_E_x_p_r_e_s_s_i_o_n_-_i_n_l_._h> │ │ │ │ │ -302 │ │ │ │ │ -_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h │ │ │ │ │ -Special class for optional Jacobian arguments. │ │ │ │ │ -_E_x_p_r_e_s_s_i_o_n_-_i_n_l_._h │ │ │ │ │ -Internals for Expression.h, not for general consumption. │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ -std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ -FastVector │ │ │ │ │ -FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ -DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ +192// For backward compatibility: │ │ │ │ │ +193typedef NonlinearOptimizerParams SuccessiveLinearizationParams; │ │ │ │ │ +194 │ │ │ │ │ +195} /* namespace gtsam */ │ │ │ │ │ +_S_u_b_g_r_a_p_h_S_o_l_v_e_r_._h │ │ │ │ │ +Subgraph Solver from IROS 2010. │ │ │ │ │ +_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_P_r_e_f_e_r_C_h_o_l_e_s_k_y │ │ │ │ │ +std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< │ │ │ │ │ +GaussianFactor > > EliminatePreferCholesky(const GaussianFactorGraph &factors, │ │ │ │ │ +const Ordering &keys) │ │ │ │ │ +Densely partially eliminate with Cholesky factorization. │ │ │ │ │ +DDeeffiinniittiioonn HessianFactor.cpp:548 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_s_y_m_b_o_l │ │ │ │ │ -Key symbol(unsigned char c, std::uint64_t j) │ │ │ │ │ -Create a symbol key from a character and index, i.e. │ │ │ │ │ -DDeeffiinniittiioonn Symbol.h:135 │ │ │ │ │ -_g_t_s_a_m_:_:_c_r_e_a_t_e_U_n_k_n_o_w_n_s │ │ │ │ │ -std::vector< Expression< T > > createUnknowns(size_t n, char c, size_t start) │ │ │ │ │ -Construct an array of leaves. │ │ │ │ │ -DDeeffiinniittiioonn Expression-inl.h:284 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_+ │ │ │ │ │ -Errors operator+(const Errors &a, const Errors &b) │ │ │ │ │ -Addition. │ │ │ │ │ -DDeeffiinniittiioonn Errors.cpp:60 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ -Point2 operator*(double s, const Point2 &p) │ │ │ │ │ -multiply with scalar │ │ │ │ │ -DDeeffiinniittiioonn Point2.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_l_i_n_e_a_r_E_x_p_r_e_s_s_i_o_n │ │ │ │ │ -Expression< T > linearExpression(const std::function< T(A)> &f, const │ │ │ │ │ -Expression< A > &expression, const Eigen::Matrix< double, traits< T >:: │ │ │ │ │ -dimension, traits< A >::dimension > &dTdA) │ │ │ │ │ -Create an expression out of a linear function f:T->A with (constant) Jacobian │ │ │ │ │ -dTdA TODO(frank): creat... │ │ │ │ │ -DDeeffiinniittiioonn Expression.h:241 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_- │ │ │ │ │ -Errors operator-(const Errors &a, const Errors &b) │ │ │ │ │ -Subtraction. │ │ │ │ │ -DDeeffiinniittiioonn Errors.cpp:75 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -: meta-function to generate JacobianTA optional reference Used mainly by │ │ │ │ │ -Expressions │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:261 │ │ │ │ │ -_g_t_s_a_m_:_:_I_s_V_e_c_t_o_r_S_p_a_c_e │ │ │ │ │ -Vector Space concept. │ │ │ │ │ -DDeeffiinniittiioonn VectorSpace.h:470 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l │ │ │ │ │ -Character and index key used to refer to variables. │ │ │ │ │ -DDeeffiinniittiioonn Symbol.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r │ │ │ │ │ -Factor that supports arbitrary expressions via AD. │ │ │ │ │ -DDeeffiinniittiioonn ExpressionFactor.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_E_x_e_c_u_t_i_o_n_T_r_a_c_e │ │ │ │ │ -DDeeffiinniittiioonn Expression.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_E_x_p_r_e_s_s_i_o_n_N_o_d_e │ │ │ │ │ -DDeeffiinniittiioonn Expression.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n │ │ │ │ │ -Expression class that supports automatic differentiation. │ │ │ │ │ -DDeeffiinniittiioonn Expression.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_r_o_o_t │ │ │ │ │ -const boost::shared_ptr< internal::ExpressionNode< T > > & root() const │ │ │ │ │ -Return root. │ │ │ │ │ -DDeeffiinniittiioonn Expression-inl.h:162 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_t_y_p_e │ │ │ │ │ -Expression< T > type │ │ │ │ │ -Define type so we can apply it as a meta-function. │ │ │ │ │ -DDeeffiinniittiioonn Expression.h:53 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_E_x_p_r_e_s_s_i_o_n │ │ │ │ │ -Expression() │ │ │ │ │ -Default constructor, for serialization. │ │ │ │ │ -DDeeffiinniittiioonn Expression.h:182 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_~_E_x_p_r_e_s_s_i_o_n │ │ │ │ │ -virtual ~Expression() │ │ │ │ │ -Destructor. │ │ │ │ │ -DDeeffiinniittiioonn Expression.h:141 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_c_l_o_n_e │ │ │ │ │ -virtual boost::shared_ptr< Expression > clone() const │ │ │ │ │ -DDeeffiinniittiioonn Expression.h:166 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_E_x_p_r_e_s_s_i_o_n │ │ │ │ │ -Expression(const boost::shared_ptr< internal::ExpressionNode< T > > &root) │ │ │ │ │ -Construct with a custom root. │ │ │ │ │ -DDeeffiinniittiioonn Expression.h:61 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_k_e_y_s │ │ │ │ │ -std::set< Key > keys() const │ │ │ │ │ -Return keys that play in this expression. │ │ │ │ │ -DDeeffiinniittiioonn Expression-inl.h:132 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_K_e_y_s_A_n_d_D_i_m_s │ │ │ │ │ -std::pair< KeyVector, FastVector< int > > KeysAndDims │ │ │ │ │ -Keys and dimensions in same order. │ │ │ │ │ -DDeeffiinniittiioonn Expression.h:185 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_d_i_m_s │ │ │ │ │ -void dims(std::map< Key, int > &map) const │ │ │ │ │ -Return dimensions for each argument, as a map. │ │ │ │ │ -DDeeffiinniittiioonn Expression-inl.h:137 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s) const │ │ │ │ │ -Print. │ │ │ │ │ -DDeeffiinniittiioonn Expression-inl.h:142 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_v_a_l_u_e_A_n_d_J_a_c_o_b_i_a_n_M_a_p │ │ │ │ │ -T valueAndJacobianMap(const Values &values, internal::JacobianMap &jacobians) │ │ │ │ │ -const │ │ │ │ │ -brief Return value and derivatives, reverse AD version │ │ │ │ │ -DDeeffiinniittiioonn Expression-inl.h:205 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_v_a_l_u_e_A_n_d_D_e_r_i_v_a_t_i_v_e_s │ │ │ │ │ -T valueAndDerivatives(const Values &values, const KeyVector &keys, const │ │ │ │ │ -FastVector< int > &dims, std::vector< Matrix > &H) const │ │ │ │ │ -private version that takes keys and dimensions, returns derivatives │ │ │ │ │ -DDeeffiinniittiioonn Expression-inl.h:174 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_v_a_l_u_e │ │ │ │ │ -T value(const Values &values, boost::optional< std::vector< Matrix > & > │ │ │ │ │ -H=boost::none) const │ │ │ │ │ -Return value and optional derivatives, reverse AD version Notes: this is not │ │ │ │ │ -terribly efficient,... │ │ │ │ │ -DDeeffiinniittiioonn Expression-inl.h:147 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_o_p_e_r_a_t_o_r_+_= │ │ │ │ │ -Expression< T > & operator+=(const Expression< T > &e) │ │ │ │ │ -Add another expression to this expression. │ │ │ │ │ -DDeeffiinniittiioonn Expression-inl.h:302 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_t_r_a_c_e_S_i_z_e │ │ │ │ │ -size_t traceSize() const │ │ │ │ │ -Return size needed for memory buffer in traceExecution. │ │ │ │ │ -DDeeffiinniittiioonn Expression-inl.h:167 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_t_r_a_c_e_E_x_e_c_u_t_i_o_n │ │ │ │ │ -T traceExecution(const Values &values, internal::ExecutionTrace< T > &trace, │ │ │ │ │ -void *traceStorage) const │ │ │ │ │ -trace execution, very unsafe │ │ │ │ │ -DDeeffiinniittiioonn Expression-inl.h:198 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_U_n_a_r_y_F_u_n_c_t_i_o_n │ │ │ │ │ -DDeeffiinniittiioonn Expression.h:70 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_B_i_n_a_r_y_F_u_n_c_t_i_o_n │ │ │ │ │ -DDeeffiinniittiioonn Expression.h:76 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_T_e_r_n_a_r_y_F_u_n_c_t_i_o_n │ │ │ │ │ -DDeeffiinniittiioonn Expression.h:83 │ │ │ │ │ -_g_t_s_a_m_:_:_S_c_a_l_a_r_M_u_l_t_i_p_l_y_E_x_p_r_e_s_s_i_o_n │ │ │ │ │ -A ScalarMultiplyExpression is a specialization of Expression that multiplies │ │ │ │ │ -with a scalar It optimiz... │ │ │ │ │ -DDeeffiinniittiioonn Expression.h:213 │ │ │ │ │ -_g_t_s_a_m_:_:_B_i_n_a_r_y_S_u_m_E_x_p_r_e_s_s_i_o_n │ │ │ │ │ -A BinarySumExpression is a specialization of Expression that adds two │ │ │ │ │ -expressions together It optimiz... │ │ │ │ │ -DDeeffiinniittiioonn Expression.h:226 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_V_a_l_u_e_s │ │ │ │ │ -In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ -a non-linear function... │ │ │ │ │ -_S_y_m_b_o_l_._h │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_Q_R │ │ │ │ │ +std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > │ │ │ │ │ +EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys) │ │ │ │ │ +Multiply all factors and eliminate the given keys from the resulting factor │ │ │ │ │ +using a QR variant that h... │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.cpp:789 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_O_r_d_e_r_i_n_g_T_y_p_e │ │ │ │ │ +OrderingType │ │ │ │ │ +Type of ordering to use. │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s │ │ │ │ │ +The common parameters for Nonlinear optimizers. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizerParams.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_i_t_e_r_a_t_i_o_n_H_o_o_k │ │ │ │ │ +IterationHook iterationHook │ │ │ │ │ +Optional user-provided iteration hook to be called after each optimization │ │ │ │ │ +iteration (Default: none). │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizerParams.h:94 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_i_t_e_r_a_t_i_v_e_P_a_r_a_m_s │ │ │ │ │ +IterativeOptimizationParameters::shared_ptr iterativeParams │ │ │ │ │ +The container for iterativeOptimization parameters. used in CG Solvers. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizerParams.h:108 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_V_e_r_b_o_s_i_t_y │ │ │ │ │ +Verbosity │ │ │ │ │ +See NonlinearOptimizerParams::verbosity. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizerParams.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_L_i_n_e_a_r_S_o_l_v_e_r_T_y_p_e │ │ │ │ │ +LinearSolverType │ │ │ │ │ +See NonlinearOptimizerParams::linearSolverType. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizerParams.h:97 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_I_t_e_r_a_t_i_o_n_H_o_o_k │ │ │ │ │ +std::function< void(size_t, double, double)> IterationHook │ │ │ │ │ +Type for an optional user-provided hook to be called after each internal │ │ │ │ │ +optimizer iteration. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizerParams.h:68 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_o_r_d_e_r_i_n_g │ │ │ │ │ +boost::optional< Ordering > ordering │ │ │ │ │ +The optional variable elimination ordering, or empty to use COLAMD (default: │ │ │ │ │ +empty) │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizerParams.h:107 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _E_x_p_r_e_s_s_i_o_n_._h │ │ │ │ │ + * _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01061.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2UpdateParams.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Params.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,43 +95,58 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
ISAM2UpdateParams.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Typedefs
│ │ │ │ +
ISAM2Params.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Class that stores extra params for ISAM2::update() │ │ │ │ +

Parameters for iSAM 2. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

struct  gtsam::ISAM2UpdateParams
 This struct is used by ISAM2::update() to pass additional parameters to give the user a fine-grained control on how factors and relinearized, etc. More...
struct  gtsam::ISAM2GaussNewtonParams
 Parameters for ISAM2 using Gauss-Newton optimization. More...
 
struct  gtsam::ISAM2DoglegParams
 Parameters for ISAM2 using Dogleg optimization. More...
 
struct  gtsam::ISAM2Params
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Typedefs

typedef FastMap< char, Vector > gtsam::ISAM2ThresholdMap
 Parameters for the ISAM2 algorithm.
 
│ │ │ │ +typedef ISAM2ThresholdMap::value_type gtsam::ISAM2ThresholdMapValue
 
│ │ │ │

Detailed Description

│ │ │ │ -

Class that stores extra params for ISAM2::update()

│ │ │ │ -
Author
Michael Kaess, Richard Roberts, Frank Dellaert, Jose Luis Blanco
│ │ │ │ +

Parameters for iSAM 2.

│ │ │ │ +
Author
Michael Kaess, Richard Roberts, Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,27 +1,36 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -ISAM2UpdateParams.h File Reference │ │ │ │ │ -Class that stores extra params for ISAM2::update() _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ +ISAM2Params.h File Reference │ │ │ │ │ +Parameters for iSAM 2. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s │ │ │ │ │ - This struct is used by _I_S_A_M_2_:_:_u_p_d_a_t_e_(_) to pass additional parameters │ │ │ │ │ -  to give the user a fine-grained control on how factors and │ │ │ │ │ - relinearized, etc. _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_I_S_A_M_2_G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s │ │ │ │ │ +  Parameters for _I_S_A_M_2 using Gauss-Newton optimization. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_I_S_A_M_2_D_o_g_l_e_g_P_a_r_a_m_s │ │ │ │ │ +  Parameters for _I_S_A_M_2 using Dogleg optimization. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ + typedef _F_a_s_t_M_a_p< char, Vector >  _g_t_s_a_m_:_:_I_S_A_M_2_T_h_r_e_s_h_o_l_d_M_a_p │ │ │ │ │ +  Parameters for the _I_S_A_M_2 algorithm. │ │ │ │ │ +  │ │ │ │ │ +typedef ISAM2ThresholdMap::value_type  ggttssaamm::::IISSAAMM22TThhrreesshhoollddMMaappVVaalluuee │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Class that stores extra params for ISAM2::update() │ │ │ │ │ +Parameters for iSAM 2. │ │ │ │ │ Author │ │ │ │ │ - Michael Kaess, Richard Roberts, Frank Dellaert, Jose Luis Blanco │ │ │ │ │ + Michael Kaess, Richard Roberts, Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_._h │ │ │ │ │ + * _I_S_A_M_2_P_a_r_a_m_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01061_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2UpdateParams.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Params.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,75 +98,307 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ISAM2UpdateParams.h
│ │ │ │ +
ISAM2Params.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ -
2 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ -
3 * Atlanta, Georgia 30332-0415
│ │ │ │ -
4 * All Rights Reserved
│ │ │ │ -
5 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ -
6 * See LICENSE for the license information
│ │ │ │ -
7 * -------------------------------------------------------------------------- */
│ │ │ │ -
8
│ │ │ │ -
15// \callgraph
│ │ │ │ -
16
│ │ │ │ -
17#pragma once
│ │ │ │ -
18
│ │ │ │ -
19#include <gtsam/base/FastList.h>
│ │ │ │ -
20#include <gtsam/dllexport.h> // GTSAM_EXPORT
│ │ │ │ -
21#include <gtsam/inference/Key.h> // Key, KeySet
│ │ │ │ -
22#include <gtsam/nonlinear/ISAM2Result.h> //FactorIndices
│ │ │ │ -
23#include <boost/optional.hpp>
│ │ │ │ -
24
│ │ │ │ -
25namespace gtsam {
│ │ │ │ +
2
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
4 * Atlanta, Georgia 30332-0415
│ │ │ │ +
5 * All Rights Reserved
│ │ │ │ +
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
7
│ │ │ │ +
8 * See LICENSE for the license information
│ │ │ │ +
9
│ │ │ │ +
10 * -------------------------------------------------------------------------- */
│ │ │ │ +
11
│ │ │ │ +
18// \callgraph
│ │ │ │ +
19
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ +
23#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
│ │ │ │ +
24#include <boost/variant.hpp>
│ │ │ │ +
25#include <string>
│ │ │ │
26
│ │ │ │ -
│ │ │ │ -
32struct ISAM2UpdateParams {
│ │ │ │ -
33 ISAM2UpdateParams() = default;
│ │ │ │ -
34
│ │ │ │ -
36 FactorIndices removeFactorIndices;
│ │ │ │ -
37
│ │ │ │ -
40 boost::optional<FastMap<Key, int>> constrainedKeys{boost::none};
│ │ │ │ -
41
│ │ │ │ -
44 boost::optional<FastList<Key>> noRelinKeys{boost::none};
│ │ │ │ -
45
│ │ │ │ -
49 boost::optional<FastList<Key>> extraReelimKeys{boost::none};
│ │ │ │ -
50
│ │ │ │ -
54 bool force_relinearize{false};
│ │ │ │ -
55
│ │ │ │ -
66 boost::optional<FastMap<FactorIndex, KeySet>> newAffectedKeys{boost::none};
│ │ │ │ -
67
│ │ │ │ -
71 bool forceFullSolve{false};
│ │ │ │ -
72};
│ │ │ │ -
│ │ │ │ -
73
│ │ │ │ -
74} // namespace gtsam
│ │ │ │ -
FastList.h
A thin wrapper around std::list that uses boost's fast_pool_allocator.
│ │ │ │ - │ │ │ │ -
ISAM2Result.h
Class that stores detailed iSAM2 result.
│ │ │ │ +
27namespace gtsam {
│ │ │ │ +
28
│ │ │ │ +
│ │ │ │ +
35struct GTSAM_EXPORT ISAM2GaussNewtonParams {
│ │ │ │ +
36 double
│ │ │ │ +
37 wildfireThreshold;
│ │ │ │ +
39
│ │ │ │ +
│ │ │ │ +
41 ISAM2GaussNewtonParams(
│ │ │ │ +
42 double _wildfireThreshold =
│ │ │ │ +
43 0.001
│ │ │ │ +
45 )
│ │ │ │ +
46 : wildfireThreshold(_wildfireThreshold) {}
│ │ │ │ +
│ │ │ │ +
47
│ │ │ │ +
48 void print(const std::string str = "") const {
│ │ │ │ +
49 using std::cout;
│ │ │ │ +
50 cout << str << "type: ISAM2GaussNewtonParams\n";
│ │ │ │ +
51 cout << str << "wildfireThreshold: " << wildfireThreshold << "\n";
│ │ │ │ +
52 cout.flush();
│ │ │ │ +
53 }
│ │ │ │ +
54
│ │ │ │ +
55 double getWildfireThreshold() const { return wildfireThreshold; }
│ │ │ │ +
56 void setWildfireThreshold(double wildfireThreshold) {
│ │ │ │ +
57 this->wildfireThreshold = wildfireThreshold;
│ │ │ │ +
58 }
│ │ │ │ +
59};
│ │ │ │ +
│ │ │ │ +
60
│ │ │ │ +
│ │ │ │ +
67struct GTSAM_EXPORT ISAM2DoglegParams {
│ │ │ │ +
68 double initialDelta;
│ │ │ │ +
69 double
│ │ │ │ +
70 wildfireThreshold;
│ │ │ │ +
72 DoglegOptimizerImpl::TrustRegionAdaptationMode
│ │ │ │ +
73 adaptationMode;
│ │ │ │ +
75 bool
│ │ │ │ +
76 verbose;
│ │ │ │ +
77
│ │ │ │ +
│ │ │ │ +
79 ISAM2DoglegParams(
│ │ │ │ +
80 double _initialDelta = 1.0,
│ │ │ │ +
81 double _wildfireThreshold =
│ │ │ │ +
82 1e-5,
│ │ │ │ +
83 DoglegOptimizerImpl::TrustRegionAdaptationMode _adaptationMode =
│ │ │ │ +
84 DoglegOptimizerImpl::
│ │ │ │ +
85 SEARCH_EACH_ITERATION,
│ │ │ │ +
86 bool _verbose = false
│ │ │ │ +
87 )
│ │ │ │ +
88 : initialDelta(_initialDelta),
│ │ │ │ +
89 wildfireThreshold(_wildfireThreshold),
│ │ │ │ +
90 adaptationMode(_adaptationMode),
│ │ │ │ +
91 verbose(_verbose) {}
│ │ │ │ +
│ │ │ │ +
92
│ │ │ │ +
93 void print(const std::string str = "") const {
│ │ │ │ +
94 using std::cout;
│ │ │ │ +
95 cout << str << "type: ISAM2DoglegParams\n";
│ │ │ │ +
96 cout << str << "initialDelta: " << initialDelta << "\n";
│ │ │ │ +
97 cout << str << "wildfireThreshold: " << wildfireThreshold << "\n";
│ │ │ │ +
98 cout << str
│ │ │ │ +
99 << "adaptationMode: " << adaptationModeTranslator(adaptationMode)
│ │ │ │ +
100 << "\n";
│ │ │ │ +
101 cout.flush();
│ │ │ │ +
102 }
│ │ │ │ +
103
│ │ │ │ +
104 double getInitialDelta() const { return initialDelta; }
│ │ │ │ +
105 double getWildfireThreshold() const { return wildfireThreshold; }
│ │ │ │ +
106 std::string getAdaptationMode() const {
│ │ │ │ +
107 return adaptationModeTranslator(adaptationMode);
│ │ │ │ +
108 }
│ │ │ │ +
109 bool isVerbose() const { return verbose; }
│ │ │ │ +
110 void setInitialDelta(double initialDelta) {
│ │ │ │ +
111 this->initialDelta = initialDelta;
│ │ │ │ +
112 }
│ │ │ │ +
113 void setWildfireThreshold(double wildfireThreshold) {
│ │ │ │ +
114 this->wildfireThreshold = wildfireThreshold;
│ │ │ │ +
115 }
│ │ │ │ +
116 void setAdaptationMode(const std::string& adaptationMode) {
│ │ │ │ +
117 this->adaptationMode = adaptationModeTranslator(adaptationMode);
│ │ │ │ +
118 }
│ │ │ │ +
119 void setVerbose(bool verbose) { this->verbose = verbose; }
│ │ │ │ +
120
│ │ │ │ +
121 std::string adaptationModeTranslator(
│ │ │ │ +
122 const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode)
│ │ │ │ +
123 const;
│ │ │ │ +
124 DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationModeTranslator(
│ │ │ │ +
125 const std::string& adaptationMode) const;
│ │ │ │ +
126};
│ │ │ │ +
│ │ │ │ +
127
│ │ │ │ +
133typedef FastMap<char, Vector> ISAM2ThresholdMap;
│ │ │ │ +
134typedef ISAM2ThresholdMap::value_type ISAM2ThresholdMapValue;
│ │ │ │ +
│ │ │ │ +
135struct GTSAM_EXPORT ISAM2Params {
│ │ │ │ +
136 typedef boost::variant<ISAM2GaussNewtonParams, ISAM2DoglegParams>
│ │ │ │ +
137 OptimizationParams;
│ │ │ │ +
139 typedef boost::variant<double, FastMap<char, Vector> >
│ │ │ │ +
140 RelinearizationThreshold;
│ │ │ │ +
143
│ │ │ │ +
150 OptimizationParams optimizationParams;
│ │ │ │ +
151
│ │ │ │ +
168 RelinearizationThreshold relinearizeThreshold;
│ │ │ │ +
169
│ │ │ │ +
170 int relinearizeSkip;
│ │ │ │ +
173
│ │ │ │ +
174 bool enableRelinearization;
│ │ │ │ +
176
│ │ │ │ +
177 bool evaluateNonlinearError;
│ │ │ │ +
180
│ │ │ │ +
181 enum Factorization { CHOLESKY, QR };
│ │ │ │ +
193 Factorization factorization;
│ │ │ │ +
194
│ │ │ │ +
200 bool cacheLinearizedFactors;
│ │ │ │ +
201
│ │ │ │ +
202 KeyFormatter
│ │ │ │ +
203 keyFormatter;
│ │ │ │ +
205
│ │ │ │ +
206 bool enableDetailedResults;
│ │ │ │ +
209
│ │ │ │ +
218 bool enablePartialRelinearizationCheck;
│ │ │ │ +
219
│ │ │ │ +
224 bool findUnusedFactorSlots;
│ │ │ │ +
225
│ │ │ │ +
│ │ │ │ +
230 ISAM2Params(OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(),
│ │ │ │ +
231 RelinearizationThreshold _relinearizeThreshold = 0.1,
│ │ │ │ +
232 int _relinearizeSkip = 10, bool _enableRelinearization = true,
│ │ │ │ +
233 bool _evaluateNonlinearError = false,
│ │ │ │ +
234 Factorization _factorization = ISAM2Params::CHOLESKY,
│ │ │ │ +
235 bool _cacheLinearizedFactors = true,
│ │ │ │ +
236 const KeyFormatter& _keyFormatter =
│ │ │ │ +
237 DefaultKeyFormatter,
│ │ │ │ +
238 bool _enableDetailedResults = false)
│ │ │ │ +
239 : optimizationParams(_optimizationParams),
│ │ │ │ +
240 relinearizeThreshold(_relinearizeThreshold),
│ │ │ │ +
241 relinearizeSkip(_relinearizeSkip),
│ │ │ │ +
242 enableRelinearization(_enableRelinearization),
│ │ │ │ +
243 evaluateNonlinearError(_evaluateNonlinearError),
│ │ │ │ +
244 factorization(_factorization),
│ │ │ │ +
245 cacheLinearizedFactors(_cacheLinearizedFactors),
│ │ │ │ +
246 keyFormatter(_keyFormatter),
│ │ │ │ +
247 enableDetailedResults(_enableDetailedResults),
│ │ │ │ +
248 enablePartialRelinearizationCheck(false),
│ │ │ │ +
249 findUnusedFactorSlots(false) {}
│ │ │ │ +
│ │ │ │ +
250
│ │ │ │ +
│ │ │ │ +
252 void print(const std::string& str = "") const {
│ │ │ │ +
253 using std::cout;
│ │ │ │ +
254 cout << str << "\n";
│ │ │ │ +
255
│ │ │ │ +
256 static const std::string kStr("optimizationParams: ");
│ │ │ │ +
257 if (optimizationParams.type() == typeid(ISAM2GaussNewtonParams))
│ │ │ │ +
258 boost::get<ISAM2GaussNewtonParams>(optimizationParams).print();
│ │ │ │ +
259 else if (optimizationParams.type() == typeid(ISAM2DoglegParams))
│ │ │ │ +
260 boost::get<ISAM2DoglegParams>(optimizationParams).print(kStr);
│ │ │ │ +
261 else
│ │ │ │ +
262 cout << kStr << "{unknown type}\n";
│ │ │ │ +
263
│ │ │ │ +
264 cout << "relinearizeThreshold: ";
│ │ │ │ +
265 if (relinearizeThreshold.type() == typeid(double)) {
│ │ │ │ +
266 cout << boost::get<double>(relinearizeThreshold) << "\n";
│ │ │ │ +
267 } else {
│ │ │ │ +
268 cout << "{mapped}\n";
│ │ │ │ +
269 for (const ISAM2ThresholdMapValue& value :
│ │ │ │ +
270 boost::get<ISAM2ThresholdMap>(relinearizeThreshold)) {
│ │ │ │ +
271 cout << " '" << value.first
│ │ │ │ +
272 << "' -> [" << value.second.transpose() << " ]\n";
│ │ │ │ +
273 }
│ │ │ │ +
274 }
│ │ │ │ +
275
│ │ │ │ +
276 cout << "relinearizeSkip: " << relinearizeSkip << "\n";
│ │ │ │ +
277 cout << "enableRelinearization: " << enableRelinearization
│ │ │ │ +
278 << "\n";
│ │ │ │ +
279 cout << "evaluateNonlinearError: " << evaluateNonlinearError
│ │ │ │ +
280 << "\n";
│ │ │ │ +
281 cout << "factorization: "
│ │ │ │ +
282 << factorizationTranslator(factorization) << "\n";
│ │ │ │ +
283 cout << "cacheLinearizedFactors: " << cacheLinearizedFactors
│ │ │ │ +
284 << "\n";
│ │ │ │ +
285 cout << "enableDetailedResults: " << enableDetailedResults
│ │ │ │ +
286 << "\n";
│ │ │ │ +
287 cout << "enablePartialRelinearizationCheck: "
│ │ │ │ +
288 << enablePartialRelinearizationCheck << "\n";
│ │ │ │ +
289 cout << "findUnusedFactorSlots: " << findUnusedFactorSlots
│ │ │ │ +
290 << "\n";
│ │ │ │ +
291 cout.flush();
│ │ │ │ +
292 }
│ │ │ │ +
│ │ │ │ +
293
│ │ │ │ +
296
│ │ │ │ +
297 OptimizationParams getOptimizationParams() const {
│ │ │ │ +
298 return this->optimizationParams;
│ │ │ │ +
299 }
│ │ │ │ +
300 RelinearizationThreshold getRelinearizeThreshold() const {
│ │ │ │ +
301 return relinearizeThreshold;
│ │ │ │ +
302 }
│ │ │ │ +
303 std::string getFactorization() const {
│ │ │ │ +
304 return factorizationTranslator(factorization);
│ │ │ │ +
305 }
│ │ │ │ +
306 KeyFormatter getKeyFormatter() const { return keyFormatter; }
│ │ │ │ +
307
│ │ │ │ +
308 void setOptimizationParams(OptimizationParams optimizationParams) {
│ │ │ │ +
309 this->optimizationParams = optimizationParams;
│ │ │ │ +
310 }
│ │ │ │ +
311 void setRelinearizeThreshold(RelinearizationThreshold relinearizeThreshold) {
│ │ │ │ +
312 this->relinearizeThreshold = relinearizeThreshold;
│ │ │ │ +
313 }
│ │ │ │ +
314 void setFactorization(const std::string& factorization) {
│ │ │ │ +
315 this->factorization = factorizationTranslator(factorization);
│ │ │ │ +
316 }
│ │ │ │ +
317 void setKeyFormatter(KeyFormatter keyFormatter) {
│ │ │ │ +
318 this->keyFormatter = keyFormatter;
│ │ │ │ +
319 }
│ │ │ │ +
320
│ │ │ │ +
321 GaussianFactorGraph::Eliminate getEliminationFunction() const {
│ │ │ │ +
322 return factorization == CHOLESKY
│ │ │ │ +
323 ? (GaussianFactorGraph::Eliminate)EliminatePreferCholesky
│ │ │ │ +
324 : (GaussianFactorGraph::Eliminate)EliminateQR;
│ │ │ │ +
325 }
│ │ │ │ +
326
│ │ │ │ +
328
│ │ │ │ +
331
│ │ │ │ +
332 static Factorization factorizationTranslator(const std::string& str);
│ │ │ │ +
333 static std::string factorizationTranslator(const Factorization& value);
│ │ │ │ +
334
│ │ │ │ +
336};
│ │ │ │ +
│ │ │ │ +
337
│ │ │ │ +
338} // namespace gtsam
│ │ │ │ +
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ +
DoglegOptimizerImpl.h
Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail implementation)
│ │ │ │ +
gtsam::ISAM2ThresholdMap
FastMap< char, Vector > ISAM2ThresholdMap
Parameters for the ISAM2 algorithm.
Definition ISAM2Params.h:133
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition Factor.h:34
│ │ │ │ -
gtsam::ISAM2UpdateParams
This struct is used by ISAM2::update() to pass additional parameters to give the user a fine-grained ...
Definition ISAM2UpdateParams.h:32
│ │ │ │ -
gtsam::ISAM2UpdateParams::constrainedKeys
boost::optional< FastMap< Key, int > > constrainedKeys
An optional map of keys to group labels, such that a variable can be constrained to a particular grou...
Definition ISAM2UpdateParams.h:40
│ │ │ │ -
gtsam::ISAM2UpdateParams::force_relinearize
bool force_relinearize
Relinearize any variables whose delta magnitude is sufficiently large (Params::relinearizeThreshold),...
Definition ISAM2UpdateParams.h:54
│ │ │ │ -
gtsam::ISAM2UpdateParams::removeFactorIndices
FactorIndices removeFactorIndices
Indices of factors to remove from system (default: empty)
Definition ISAM2UpdateParams.h:36
│ │ │ │ -
gtsam::ISAM2UpdateParams::forceFullSolve
bool forceFullSolve
By default, iSAM2 uses a wildfire update scheme that stops updating when the deltas become too small ...
Definition ISAM2UpdateParams.h:71
│ │ │ │ -
gtsam::ISAM2UpdateParams::extraReelimKeys
boost::optional< FastList< Key > > extraReelimKeys
An optional set of nonlinear keys that iSAM2 will re-eliminate, regardless of the size of the linear ...
Definition ISAM2UpdateParams.h:49
│ │ │ │ -
gtsam::ISAM2UpdateParams::noRelinKeys
boost::optional< FastList< Key > > noRelinKeys
An optional set of nonlinear keys that iSAM2 will hold at a constant linearization point,...
Definition ISAM2UpdateParams.h:44
│ │ │ │ -
gtsam::ISAM2UpdateParams::newAffectedKeys
boost::optional< FastMap< FactorIndex, KeySet > > newAffectedKeys
An optional set of new Keys that are now affected by factors, indexed by factor indices (as returned ...
Definition ISAM2UpdateParams.h:66
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::EliminateQR
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Multiply all factors and eliminate the given keys from the resulting factor using a QR variant that h...
Definition JacobianFactor.cpp:789
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ +
gtsam::DoglegOptimizerImpl
This class contains the implementation of the Dogleg algorithm.
Definition DoglegOptimizerImpl.h:32
│ │ │ │ +
gtsam::DoglegOptimizerImpl::TrustRegionAdaptationMode
TrustRegionAdaptationMode
Specifies how the trust region is adapted at each Dogleg iteration.
Definition DoglegOptimizerImpl.h:53
│ │ │ │ +
gtsam::ISAM2GaussNewtonParams
Parameters for ISAM2 using Gauss-Newton optimization.
Definition ISAM2Params.h:35
│ │ │ │ +
gtsam::ISAM2GaussNewtonParams::wildfireThreshold
double wildfireThreshold
Continue updating the linear delta only when changes are above this threshold (default: 0....
Definition ISAM2Params.h:37
│ │ │ │ +
gtsam::ISAM2GaussNewtonParams::ISAM2GaussNewtonParams
ISAM2GaussNewtonParams(double _wildfireThreshold=0.001)
Specify parameters as constructor arguments.
Definition ISAM2Params.h:41
│ │ │ │ +
gtsam::ISAM2DoglegParams
Parameters for ISAM2 using Dogleg optimization.
Definition ISAM2Params.h:67
│ │ │ │ +
gtsam::ISAM2DoglegParams::adaptationMode
DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationMode
See description in DoglegOptimizerImpl::TrustRegionAdaptationMode.
Definition ISAM2Params.h:73
│ │ │ │ +
gtsam::ISAM2DoglegParams::verbose
bool verbose
Whether Dogleg prints iteration and convergence information.
Definition ISAM2Params.h:76
│ │ │ │ +
gtsam::ISAM2DoglegParams::ISAM2DoglegParams
ISAM2DoglegParams(double _initialDelta=1.0, double _wildfireThreshold=1e-5, DoglegOptimizerImpl::TrustRegionAdaptationMode _adaptationMode=DoglegOptimizerImpl::SEARCH_EACH_ITERATION, bool _verbose=false)
Specify parameters as constructor arguments.
Definition ISAM2Params.h:79
│ │ │ │ +
gtsam::ISAM2DoglegParams::initialDelta
double initialDelta
The initial trust region radius for Dogleg.
Definition ISAM2Params.h:68
│ │ │ │ +
gtsam::ISAM2DoglegParams::wildfireThreshold
double wildfireThreshold
Continue updating the linear delta only when changes are above this threshold (default: 1e-5)
Definition ISAM2Params.h:70
│ │ │ │ +
gtsam::ISAM2Params
Definition ISAM2Params.h:135
│ │ │ │ +
gtsam::ISAM2Params::enablePartialRelinearizationCheck
bool enablePartialRelinearizationCheck
Check variables for relinearization in tree-order, stopping the check once a variable does not need t...
Definition ISAM2Params.h:218
│ │ │ │ +
gtsam::ISAM2Params::optimizationParams
OptimizationParams optimizationParams
Optimization parameters, this both selects the nonlinear optimization method and specifies its parame...
Definition ISAM2Params.h:150
│ │ │ │ +
gtsam::ISAM2Params::print
void print(const std::string &str="") const
print iSAM2 parameters
Definition ISAM2Params.h:252
│ │ │ │ +
gtsam::ISAM2Params::enableRelinearization
bool enableRelinearization
Controls whether ISAM2 will ever relinearize any variables (default: true)
Definition ISAM2Params.h:174
│ │ │ │ +
gtsam::ISAM2Params::cacheLinearizedFactors
bool cacheLinearizedFactors
Whether to cache linear factors (default: true).
Definition ISAM2Params.h:200
│ │ │ │ +
gtsam::ISAM2Params::evaluateNonlinearError
bool evaluateNonlinearError
Whether to evaluate the nonlinear error before and after the update, to return in ISAM2Result from up...
Definition ISAM2Params.h:177
│ │ │ │ +
gtsam::ISAM2Params::enableDetailedResults
bool enableDetailedResults
Whether to compute and return ISAM2Result::detailedResults, this can increase running time (default: ...
Definition ISAM2Params.h:206
│ │ │ │ +
gtsam::ISAM2Params::OptimizationParams
boost::variant< ISAM2GaussNewtonParams, ISAM2DoglegParams > OptimizationParams
Either ISAM2GaussNewtonParams or ISAM2DoglegParams.
Definition ISAM2Params.h:137
│ │ │ │ +
gtsam::ISAM2Params::findUnusedFactorSlots
bool findUnusedFactorSlots
When you will be removing many factors, e.g.
Definition ISAM2Params.h:224
│ │ │ │ +
gtsam::ISAM2Params::RelinearizationThreshold
boost::variant< double, FastMap< char, Vector > > RelinearizationThreshold
Either a constant relinearization threshold or a per-variable-type set of thresholds.
Definition ISAM2Params.h:140
│ │ │ │ +
gtsam::ISAM2Params::relinearizeSkip
int relinearizeSkip
Only relinearize any variables every relinearizeSkip calls to ISAM2::update (default: 10)
Definition ISAM2Params.h:170
│ │ │ │ +
gtsam::ISAM2Params::factorization
Factorization factorization
Specifies whether to use QR or CHOESKY numerical factorization (default: CHOLESKY).
Definition ISAM2Params.h:193
│ │ │ │ +
gtsam::ISAM2Params::keyFormatter
KeyFormatter keyFormatter
A KeyFormatter for when keys are printed during debugging (default: DefaultKeyFormatter)
Definition ISAM2Params.h:203
│ │ │ │ +
gtsam::ISAM2Params::relinearizeThreshold
RelinearizationThreshold relinearizeThreshold
Only relinearize variables whose linear delta magnitude is greater than this threshold (default: 0....
Definition ISAM2Params.h:168
│ │ │ │ +
gtsam::ISAM2Params::ISAM2Params
ISAM2Params(OptimizationParams _optimizationParams=ISAM2GaussNewtonParams(), RelinearizationThreshold _relinearizeThreshold=0.1, int _relinearizeSkip=10, bool _enableRelinearization=true, bool _evaluateNonlinearError=false, Factorization _factorization=ISAM2Params::CHOLESKY, bool _cacheLinearizedFactors=true, const KeyFormatter &_keyFormatter=DefaultKeyFormatter, bool _enableDetailedResults=false)
Specify parameters as constructor arguments See the documentation of member variables above.
Definition ISAM2Params.h:230
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,102 +1,405 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ISAM2UpdateParams.h │ │ │ │ │ +ISAM2Params.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ -2 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ -3 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -4 * All Rights Reserved │ │ │ │ │ -5 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ -6 * See LICENSE for the license information │ │ │ │ │ -7 * ------------------------------------------------------------------------- │ │ │ │ │ +2 │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +5 * All Rights Reserved │ │ │ │ │ +6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +7 │ │ │ │ │ +8 * See LICENSE for the license information │ │ │ │ │ +9 │ │ │ │ │ +10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ -8 │ │ │ │ │ -15// \callgraph │ │ │ │ │ -16 │ │ │ │ │ -17#pragma once │ │ │ │ │ -18 │ │ │ │ │ -19#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_L_i_s_t_._h> │ │ │ │ │ -20#include // GTSAM_EXPORT │ │ │ │ │ -21#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> // Key, KeySet │ │ │ │ │ -22#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_I_S_A_M_2_R_e_s_u_l_t_._h> //FactorIndices │ │ │ │ │ -23#include │ │ │ │ │ -24 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ +11 │ │ │ │ │ +18// \callgraph │ │ │ │ │ +19 │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_._h> │ │ │ │ │ +24#include │ │ │ │ │ +25#include │ │ │ │ │ 26 │ │ │ │ │ -_3_2struct _I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s { │ │ │ │ │ -33 _I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s() = default; │ │ │ │ │ -34 │ │ │ │ │ -_3_6 _F_a_c_t_o_r_I_n_d_i_c_e_s _r_e_m_o_v_e_F_a_c_t_o_r_I_n_d_i_c_e_s; │ │ │ │ │ -37 │ │ │ │ │ -_4_0 boost::optional> _c_o_n_s_t_r_a_i_n_e_d_K_e_y_s{boost::none}; │ │ │ │ │ -41 │ │ │ │ │ -_4_4 boost::optional> _n_o_R_e_l_i_n_K_e_y_s{boost::none}; │ │ │ │ │ -45 │ │ │ │ │ -_4_9 boost::optional> _e_x_t_r_a_R_e_e_l_i_m_K_e_y_s{boost::none}; │ │ │ │ │ -50 │ │ │ │ │ -_5_4 bool _f_o_r_c_e___r_e_l_i_n_e_a_r_i_z_e{false}; │ │ │ │ │ -55 │ │ │ │ │ -_6_6 boost::optional> _n_e_w_A_f_f_e_c_t_e_d_K_e_y_s{boost::none}; │ │ │ │ │ -67 │ │ │ │ │ -_7_1 bool _f_o_r_c_e_F_u_l_l_S_o_l_v_e{false}; │ │ │ │ │ -72}; │ │ │ │ │ -73 │ │ │ │ │ -74} // namespace gtsam │ │ │ │ │ -_F_a_s_t_L_i_s_t_._h │ │ │ │ │ -A thin wrapper around std::list that uses boost's fast_pool_allocator. │ │ │ │ │ -_K_e_y_._h │ │ │ │ │ -_I_S_A_M_2_R_e_s_u_l_t_._h │ │ │ │ │ -Class that stores detailed iSAM2 result. │ │ │ │ │ +27namespace _g_t_s_a_m { │ │ │ │ │ +28 │ │ │ │ │ +_3_5struct GTSAM_EXPORT _I_S_A_M_2_G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s { │ │ │ │ │ +36 double │ │ │ │ │ +_3_7 _w_i_l_d_f_i_r_e_T_h_r_e_s_h_o_l_d; │ │ │ │ │ +39 │ │ │ │ │ +_4_1 _I_S_A_M_2_G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s( │ │ │ │ │ +42 double _wildfireThreshold = │ │ │ │ │ +43 0.001 │ │ │ │ │ +45 ) │ │ │ │ │ +46 : wildfireThreshold(_wildfireThreshold) {} │ │ │ │ │ +47 │ │ │ │ │ +48 void _p_r_i_n_t(const std::string str = "") const { │ │ │ │ │ +49 using std::cout; │ │ │ │ │ +50 cout << str << "type: ISAM2GaussNewtonParams\n"; │ │ │ │ │ +51 cout << str << "wildfireThreshold: " << wildfireThreshold << "\n"; │ │ │ │ │ +52 cout.flush(); │ │ │ │ │ +53 } │ │ │ │ │ +54 │ │ │ │ │ +55 double getWildfireThreshold() const { return wildfireThreshold; } │ │ │ │ │ +56 void setWildfireThreshold(double wildfireThreshold) { │ │ │ │ │ +57 this->wildfireThreshold = wildfireThreshold; │ │ │ │ │ +58 } │ │ │ │ │ +59}; │ │ │ │ │ +60 │ │ │ │ │ +_6_7struct GTSAM_EXPORT _I_S_A_M_2_D_o_g_l_e_g_P_a_r_a_m_s { │ │ │ │ │ +_6_8 double _i_n_i_t_i_a_l_D_e_l_t_a; │ │ │ │ │ +69 double │ │ │ │ │ +_7_0 _w_i_l_d_f_i_r_e_T_h_r_e_s_h_o_l_d; │ │ │ │ │ +72 _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_:_:_T_r_u_s_t_R_e_g_i_o_n_A_d_a_p_t_a_t_i_o_n_M_o_d_e │ │ │ │ │ +_7_3 _a_d_a_p_t_a_t_i_o_n_M_o_d_e; │ │ │ │ │ +75 bool │ │ │ │ │ +_7_6 _v_e_r_b_o_s_e; │ │ │ │ │ +77 │ │ │ │ │ +_7_9 _I_S_A_M_2_D_o_g_l_e_g_P_a_r_a_m_s( │ │ │ │ │ +80 double _initialDelta = 1.0, │ │ │ │ │ +81 double _wildfireThreshold = │ │ │ │ │ +82 1e-5, │ │ │ │ │ +83 _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_:_:_T_r_u_s_t_R_e_g_i_o_n_A_d_a_p_t_a_t_i_o_n_M_o_d_e _adaptationMode = │ │ │ │ │ +84 _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l:: │ │ │ │ │ +85 SEARCH_EACH_ITERATION, │ │ │ │ │ +86 bool _verbose = false │ │ │ │ │ +87 ) │ │ │ │ │ +88 : initialDelta(_initialDelta), │ │ │ │ │ +89 wildfireThreshold(_wildfireThreshold), │ │ │ │ │ +90 adaptationMode(_adaptationMode), │ │ │ │ │ +91 verbose(_verbose) {} │ │ │ │ │ +92 │ │ │ │ │ +93 void _p_r_i_n_t(const std::string str = "") const { │ │ │ │ │ +94 using std::cout; │ │ │ │ │ +95 cout << str << "type: ISAM2DoglegParams\n"; │ │ │ │ │ +96 cout << str << "initialDelta: " << initialDelta << "\n"; │ │ │ │ │ +97 cout << str << "wildfireThreshold: " << wildfireThreshold << "\n"; │ │ │ │ │ +98 cout << str │ │ │ │ │ +99 << "adaptationMode: " << adaptationModeTranslator(adaptationMode) │ │ │ │ │ +100 << "\n"; │ │ │ │ │ +101 cout.flush(); │ │ │ │ │ +102 } │ │ │ │ │ +103 │ │ │ │ │ +104 double getInitialDelta() const { return initialDelta; } │ │ │ │ │ +105 double getWildfireThreshold() const { return wildfireThreshold; } │ │ │ │ │ +106 std::string getAdaptationMode() const { │ │ │ │ │ +107 return adaptationModeTranslator(adaptationMode); │ │ │ │ │ +108 } │ │ │ │ │ +109 bool isVerbose() const { return verbose; } │ │ │ │ │ +110 void setInitialDelta(double initialDelta) { │ │ │ │ │ +111 this->initialDelta = initialDelta; │ │ │ │ │ +112 } │ │ │ │ │ +113 void setWildfireThreshold(double wildfireThreshold) { │ │ │ │ │ +114 this->wildfireThreshold = wildfireThreshold; │ │ │ │ │ +115 } │ │ │ │ │ +116 void setAdaptationMode(const std::string& adaptationMode) { │ │ │ │ │ +117 this->adaptationMode = adaptationModeTranslator(adaptationMode); │ │ │ │ │ +118 } │ │ │ │ │ +119 void setVerbose(bool verbose) { this->verbose = verbose; } │ │ │ │ │ +120 │ │ │ │ │ +121 std::string adaptationModeTranslator( │ │ │ │ │ +122 const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) │ │ │ │ │ +123 const; │ │ │ │ │ +124 DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationModeTranslator( │ │ │ │ │ +125 const std::string& adaptationMode) const; │ │ │ │ │ +126}; │ │ │ │ │ +127 │ │ │ │ │ +_1_3_3typedef _F_a_s_t_M_a_p_<_c_h_a_r_,_ _V_e_c_t_o_r_> _I_S_A_M_2_T_h_r_e_s_h_o_l_d_M_a_p; │ │ │ │ │ +134typedef ISAM2ThresholdMap::value_type ISAM2ThresholdMapValue; │ │ │ │ │ +_1_3_5struct GTSAM_EXPORT _I_S_A_M_2_P_a_r_a_m_s { │ │ │ │ │ +136 typedef boost::variant │ │ │ │ │ +_1_3_7 _O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_s; │ │ │ │ │ +139 typedef boost::variant > │ │ │ │ │ +_1_4_0 _R_e_l_i_n_e_a_r_i_z_a_t_i_o_n_T_h_r_e_s_h_o_l_d; │ │ │ │ │ +143 │ │ │ │ │ +_1_5_0 _O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_s _o_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_s; │ │ │ │ │ +151 │ │ │ │ │ +_1_6_8 _R_e_l_i_n_e_a_r_i_z_a_t_i_o_n_T_h_r_e_s_h_o_l_d _r_e_l_i_n_e_a_r_i_z_e_T_h_r_e_s_h_o_l_d; │ │ │ │ │ +169 │ │ │ │ │ +_1_7_0 int _r_e_l_i_n_e_a_r_i_z_e_S_k_i_p; │ │ │ │ │ +173 │ │ │ │ │ +_1_7_4 bool _e_n_a_b_l_e_R_e_l_i_n_e_a_r_i_z_a_t_i_o_n; │ │ │ │ │ +176 │ │ │ │ │ +_1_7_7 bool _e_v_a_l_u_a_t_e_N_o_n_l_i_n_e_a_r_E_r_r_o_r; │ │ │ │ │ +180 │ │ │ │ │ +181 enum Factorization { CHOLESKY, QR }; │ │ │ │ │ +_1_9_3 Factorization _f_a_c_t_o_r_i_z_a_t_i_o_n; │ │ │ │ │ +194 │ │ │ │ │ +_2_0_0 bool _c_a_c_h_e_L_i_n_e_a_r_i_z_e_d_F_a_c_t_o_r_s; │ │ │ │ │ +201 │ │ │ │ │ +202 _K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +_2_0_3 _k_e_y_F_o_r_m_a_t_t_e_r; │ │ │ │ │ +205 │ │ │ │ │ +_2_0_6 bool _e_n_a_b_l_e_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s; │ │ │ │ │ +209 │ │ │ │ │ +_2_1_8 bool _e_n_a_b_l_e_P_a_r_t_i_a_l_R_e_l_i_n_e_a_r_i_z_a_t_i_o_n_C_h_e_c_k; │ │ │ │ │ +219 │ │ │ │ │ +_2_2_4 bool _f_i_n_d_U_n_u_s_e_d_F_a_c_t_o_r_S_l_o_t_s; │ │ │ │ │ +225 │ │ │ │ │ +_2_3_0 _I_S_A_M_2_P_a_r_a_m_s(_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_s _optimizationParams = _I_S_A_M_2_G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s │ │ │ │ │ +(), │ │ │ │ │ +231 _R_e_l_i_n_e_a_r_i_z_a_t_i_o_n_T_h_r_e_s_h_o_l_d _relinearizeThreshold = 0.1, │ │ │ │ │ +232 int _relinearizeSkip = 10, bool _enableRelinearization = true, │ │ │ │ │ +233 bool _evaluateNonlinearError = false, │ │ │ │ │ +234 Factorization _factorization = ISAM2Params::CHOLESKY, │ │ │ │ │ +235 bool _cacheLinearizedFactors = true, │ │ │ │ │ +236 const _K_e_y_F_o_r_m_a_t_t_e_r& _keyFormatter = │ │ │ │ │ +237 DefaultKeyFormatter, │ │ │ │ │ +238 bool _enableDetailedResults = false) │ │ │ │ │ +239 : optimizationParams(_optimizationParams), │ │ │ │ │ +240 relinearizeThreshold(_relinearizeThreshold), │ │ │ │ │ +241 relinearizeSkip(_relinearizeSkip), │ │ │ │ │ +242 enableRelinearization(_enableRelinearization), │ │ │ │ │ +243 evaluateNonlinearError(_evaluateNonlinearError), │ │ │ │ │ +244 factorization(_factorization), │ │ │ │ │ +245 cacheLinearizedFactors(_cacheLinearizedFactors), │ │ │ │ │ +246 keyFormatter(_keyFormatter), │ │ │ │ │ +247 enableDetailedResults(_enableDetailedResults), │ │ │ │ │ +248 enablePartialRelinearizationCheck(false), │ │ │ │ │ +249 findUnusedFactorSlots(false) {} │ │ │ │ │ +250 │ │ │ │ │ +_2_5_2 void _p_r_i_n_t(const std::string& str = "") const { │ │ │ │ │ +253 using std::cout; │ │ │ │ │ +254 cout << str << "\n"; │ │ │ │ │ +255 │ │ │ │ │ +256 static const std::string kStr("optimizationParams: "); │ │ │ │ │ +257 if (optimizationParams.type() == typeid(_I_S_A_M_2_G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s)) │ │ │ │ │ +258 boost::get(optimizationParams).print(); │ │ │ │ │ +259 else if (optimizationParams.type() == typeid(_I_S_A_M_2_D_o_g_l_e_g_P_a_r_a_m_s)) │ │ │ │ │ +260 boost::get(optimizationParams).print(kStr); │ │ │ │ │ +261 else │ │ │ │ │ +262 cout << kStr << "{unknown type}\n"; │ │ │ │ │ +263 │ │ │ │ │ +264 cout << "relinearizeThreshold: "; │ │ │ │ │ +265 if (relinearizeThreshold.type() == typeid(double)) { │ │ │ │ │ +266 cout << boost::get(relinearizeThreshold) << "\n"; │ │ │ │ │ +267 } else { │ │ │ │ │ +268 cout << "{mapped}\n"; │ │ │ │ │ +269 for (const ISAM2ThresholdMapValue& value : │ │ │ │ │ +270 boost::get(relinearizeThreshold)) { │ │ │ │ │ +271 cout << " '" << value.first │ │ │ │ │ +272 << "' -> [" << value.second.transpose() << " ]\n"; │ │ │ │ │ +273 } │ │ │ │ │ +274 } │ │ │ │ │ +275 │ │ │ │ │ +276 cout << "relinearizeSkip: " << relinearizeSkip << "\n"; │ │ │ │ │ +277 cout << "enableRelinearization: " << enableRelinearization │ │ │ │ │ +278 << "\n"; │ │ │ │ │ +279 cout << "evaluateNonlinearError: " << evaluateNonlinearError │ │ │ │ │ +280 << "\n"; │ │ │ │ │ +281 cout << "factorization: " │ │ │ │ │ +282 << factorizationTranslator(factorization) << "\n"; │ │ │ │ │ +283 cout << "cacheLinearizedFactors: " << cacheLinearizedFactors │ │ │ │ │ +284 << "\n"; │ │ │ │ │ +285 cout << "enableDetailedResults: " << enableDetailedResults │ │ │ │ │ +286 << "\n"; │ │ │ │ │ +287 cout << "enablePartialRelinearizationCheck: " │ │ │ │ │ +288 << enablePartialRelinearizationCheck << "\n"; │ │ │ │ │ +289 cout << "findUnusedFactorSlots: " << findUnusedFactorSlots │ │ │ │ │ +290 << "\n"; │ │ │ │ │ +291 cout.flush(); │ │ │ │ │ +292 } │ │ │ │ │ +293 │ │ │ │ │ +296 │ │ │ │ │ +297 OptimizationParams getOptimizationParams() const { │ │ │ │ │ +298 return this->optimizationParams; │ │ │ │ │ +299 } │ │ │ │ │ +300 RelinearizationThreshold getRelinearizeThreshold() const { │ │ │ │ │ +301 return relinearizeThreshold; │ │ │ │ │ +302 } │ │ │ │ │ +303 std::string getFactorization() const { │ │ │ │ │ +304 return factorizationTranslator(factorization); │ │ │ │ │ +305 } │ │ │ │ │ +306 _K_e_y_F_o_r_m_a_t_t_e_r getKeyFormatter() const { return keyFormatter; } │ │ │ │ │ +307 │ │ │ │ │ +308 void setOptimizationParams(OptimizationParams optimizationParams) { │ │ │ │ │ +309 this->optimizationParams = optimizationParams; │ │ │ │ │ +310 } │ │ │ │ │ +311 void setRelinearizeThreshold(RelinearizationThreshold relinearizeThreshold) │ │ │ │ │ +{ │ │ │ │ │ +312 this->relinearizeThreshold = relinearizeThreshold; │ │ │ │ │ +313 } │ │ │ │ │ +314 void setFactorization(const std::string& factorization) { │ │ │ │ │ +315 this->factorization = factorizationTranslator(factorization); │ │ │ │ │ +316 } │ │ │ │ │ +317 void setKeyFormatter(KeyFormatter keyFormatter) { │ │ │ │ │ +318 this->keyFormatter = keyFormatter; │ │ │ │ │ +319 } │ │ │ │ │ +320 │ │ │ │ │ +321 GaussianFactorGraph::Eliminate getEliminationFunction() const { │ │ │ │ │ +322 return factorization == CHOLESKY │ │ │ │ │ +323 ? (GaussianFactorGraph::Eliminate)EliminatePreferCholesky │ │ │ │ │ +324 : (GaussianFactorGraph::Eliminate)_E_l_i_m_i_n_a_t_e_Q_R; │ │ │ │ │ +325 } │ │ │ │ │ +326 │ │ │ │ │ +328 │ │ │ │ │ +331 │ │ │ │ │ +332 static Factorization factorizationTranslator(const std::string& str); │ │ │ │ │ +333 static std::string factorizationTranslator(const Factorization& value); │ │ │ │ │ +334 │ │ │ │ │ +336}; │ │ │ │ │ +337 │ │ │ │ │ +338} // namespace gtsam │ │ │ │ │ +_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ +_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_._h │ │ │ │ │ +Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail │ │ │ │ │ +implementation) │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_T_h_r_e_s_h_o_l_d_M_a_p │ │ │ │ │ +FastMap< char, Vector > ISAM2ThresholdMap │ │ │ │ │ +Parameters for the ISAM2 algorithm. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:133 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_I_n_d_i_c_e_s │ │ │ │ │ -FastVector< FactorIndex > FactorIndices │ │ │ │ │ -Define collection types: │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s │ │ │ │ │ -This struct is used by ISAM2::update() to pass additional parameters to give │ │ │ │ │ -the user a fine-grained ... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2UpdateParams.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_:_:_c_o_n_s_t_r_a_i_n_e_d_K_e_y_s │ │ │ │ │ -boost::optional< FastMap< Key, int > > constrainedKeys │ │ │ │ │ -An optional map of keys to group labels, such that a variable can be │ │ │ │ │ -constrained to a particular grou... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2UpdateParams.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_:_:_f_o_r_c_e___r_e_l_i_n_e_a_r_i_z_e │ │ │ │ │ -bool force_relinearize │ │ │ │ │ -Relinearize any variables whose delta magnitude is sufficiently large (Params:: │ │ │ │ │ -relinearizeThreshold),... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2UpdateParams.h:54 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_:_:_r_e_m_o_v_e_F_a_c_t_o_r_I_n_d_i_c_e_s │ │ │ │ │ -FactorIndices removeFactorIndices │ │ │ │ │ -Indices of factors to remove from system (default: empty) │ │ │ │ │ -DDeeffiinniittiioonn ISAM2UpdateParams.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_:_:_f_o_r_c_e_F_u_l_l_S_o_l_v_e │ │ │ │ │ -bool forceFullSolve │ │ │ │ │ -By default, iSAM2 uses a wildfire update scheme that stops updating when the │ │ │ │ │ -deltas become too small ... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2UpdateParams.h:71 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_:_:_e_x_t_r_a_R_e_e_l_i_m_K_e_y_s │ │ │ │ │ -boost::optional< FastList< Key > > extraReelimKeys │ │ │ │ │ -An optional set of nonlinear keys that iSAM2 will re-eliminate, regardless of │ │ │ │ │ -the size of the linear ... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2UpdateParams.h:49 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_:_:_n_o_R_e_l_i_n_K_e_y_s │ │ │ │ │ -boost::optional< FastList< Key > > noRelinKeys │ │ │ │ │ -An optional set of nonlinear keys that iSAM2 will hold at a constant │ │ │ │ │ -linearization point,... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2UpdateParams.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_:_:_n_e_w_A_f_f_e_c_t_e_d_K_e_y_s │ │ │ │ │ -boost::optional< FastMap< FactorIndex, KeySet > > newAffectedKeys │ │ │ │ │ -An optional set of new Keys that are now affected by factors, indexed by factor │ │ │ │ │ -indices (as returned ... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2UpdateParams.h:66 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_Q_R │ │ │ │ │ +std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > │ │ │ │ │ +EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys) │ │ │ │ │ +Multiply all factors and eliminate the given keys from the resulting factor │ │ │ │ │ +using a QR variant that h... │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.cpp:789 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ +FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ +fast_pool_allocator instead of the defa... │ │ │ │ │ +DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l │ │ │ │ │ +This class contains the implementation of the Dogleg algorithm. │ │ │ │ │ +DDeeffiinniittiioonn DoglegOptimizerImpl.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_:_:_T_r_u_s_t_R_e_g_i_o_n_A_d_a_p_t_a_t_i_o_n_M_o_d_e │ │ │ │ │ +TrustRegionAdaptationMode │ │ │ │ │ +Specifies how the trust region is adapted at each Dogleg iteration. │ │ │ │ │ +DDeeffiinniittiioonn DoglegOptimizerImpl.h:53 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s │ │ │ │ │ +Parameters for ISAM2 using Gauss-Newton optimization. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s_:_:_w_i_l_d_f_i_r_e_T_h_r_e_s_h_o_l_d │ │ │ │ │ +double wildfireThreshold │ │ │ │ │ +Continue updating the linear delta only when changes are above this threshold │ │ │ │ │ +(default: 0.... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s_:_:_I_S_A_M_2_G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s │ │ │ │ │ +ISAM2GaussNewtonParams(double _wildfireThreshold=0.001) │ │ │ │ │ +Specify parameters as constructor arguments. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_D_o_g_l_e_g_P_a_r_a_m_s │ │ │ │ │ +Parameters for ISAM2 using Dogleg optimization. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_D_o_g_l_e_g_P_a_r_a_m_s_:_:_a_d_a_p_t_a_t_i_o_n_M_o_d_e │ │ │ │ │ +DoglegOptimizerImpl::TrustRegionAdaptationMode adaptationMode │ │ │ │ │ +See description in DoglegOptimizerImpl::TrustRegionAdaptationMode. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:73 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_D_o_g_l_e_g_P_a_r_a_m_s_:_:_v_e_r_b_o_s_e │ │ │ │ │ +bool verbose │ │ │ │ │ +Whether Dogleg prints iteration and convergence information. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:76 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_D_o_g_l_e_g_P_a_r_a_m_s_:_:_I_S_A_M_2_D_o_g_l_e_g_P_a_r_a_m_s │ │ │ │ │ +ISAM2DoglegParams(double _initialDelta=1.0, double _wildfireThreshold=1e-5, │ │ │ │ │ +DoglegOptimizerImpl::TrustRegionAdaptationMode │ │ │ │ │ +_adaptationMode=DoglegOptimizerImpl::SEARCH_EACH_ITERATION, bool │ │ │ │ │ +_verbose=false) │ │ │ │ │ +Specify parameters as constructor arguments. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:79 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_D_o_g_l_e_g_P_a_r_a_m_s_:_:_i_n_i_t_i_a_l_D_e_l_t_a │ │ │ │ │ +double initialDelta │ │ │ │ │ +The initial trust region radius for Dogleg. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:68 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_D_o_g_l_e_g_P_a_r_a_m_s_:_:_w_i_l_d_f_i_r_e_T_h_r_e_s_h_o_l_d │ │ │ │ │ +double wildfireThreshold │ │ │ │ │ +Continue updating the linear delta only when changes are above this threshold │ │ │ │ │ +(default: 1e-5) │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:70 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:135 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_e_n_a_b_l_e_P_a_r_t_i_a_l_R_e_l_i_n_e_a_r_i_z_a_t_i_o_n_C_h_e_c_k │ │ │ │ │ +bool enablePartialRelinearizationCheck │ │ │ │ │ +Check variables for relinearization in tree-order, stopping the check once a │ │ │ │ │ +variable does not need t... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:218 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_o_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ +OptimizationParams optimizationParams │ │ │ │ │ +Optimization parameters, this both selects the nonlinear optimization method │ │ │ │ │ +and specifies its parame... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:150 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &str="") const │ │ │ │ │ +print iSAM2 parameters │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:252 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_e_n_a_b_l_e_R_e_l_i_n_e_a_r_i_z_a_t_i_o_n │ │ │ │ │ +bool enableRelinearization │ │ │ │ │ +Controls whether ISAM2 will ever relinearize any variables (default: true) │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:174 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_c_a_c_h_e_L_i_n_e_a_r_i_z_e_d_F_a_c_t_o_r_s │ │ │ │ │ +bool cacheLinearizedFactors │ │ │ │ │ +Whether to cache linear factors (default: true). │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:200 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_e_v_a_l_u_a_t_e_N_o_n_l_i_n_e_a_r_E_r_r_o_r │ │ │ │ │ +bool evaluateNonlinearError │ │ │ │ │ +Whether to evaluate the nonlinear error before and after the update, to return │ │ │ │ │ +in ISAM2Result from up... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:177 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_e_n_a_b_l_e_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s │ │ │ │ │ +bool enableDetailedResults │ │ │ │ │ +Whether to compute and return ISAM2Result::detailedResults, this can increase │ │ │ │ │ +running time (default: ... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:206 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ +boost::variant< ISAM2GaussNewtonParams, ISAM2DoglegParams > OptimizationParams │ │ │ │ │ +Either ISAM2GaussNewtonParams or ISAM2DoglegParams. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:137 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_f_i_n_d_U_n_u_s_e_d_F_a_c_t_o_r_S_l_o_t_s │ │ │ │ │ +bool findUnusedFactorSlots │ │ │ │ │ +When you will be removing many factors, e.g. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:224 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_R_e_l_i_n_e_a_r_i_z_a_t_i_o_n_T_h_r_e_s_h_o_l_d │ │ │ │ │ +boost::variant< double, FastMap< char, Vector > > RelinearizationThreshold │ │ │ │ │ +Either a constant relinearization threshold or a per-variable-type set of │ │ │ │ │ +thresholds. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:140 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_r_e_l_i_n_e_a_r_i_z_e_S_k_i_p │ │ │ │ │ +int relinearizeSkip │ │ │ │ │ +Only relinearize any variables every relinearizeSkip calls to ISAM2::update │ │ │ │ │ +(default: 10) │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:170 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_f_a_c_t_o_r_i_z_a_t_i_o_n │ │ │ │ │ +Factorization factorization │ │ │ │ │ +Specifies whether to use QR or CHOESKY numerical factorization (default: │ │ │ │ │ +CHOLESKY). │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:193 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_k_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +KeyFormatter keyFormatter │ │ │ │ │ +A KeyFormatter for when keys are printed during debugging (default: │ │ │ │ │ +DefaultKeyFormatter) │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:203 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_r_e_l_i_n_e_a_r_i_z_e_T_h_r_e_s_h_o_l_d │ │ │ │ │ +RelinearizationThreshold relinearizeThreshold │ │ │ │ │ +Only relinearize variables whose linear delta magnitude is greater than this │ │ │ │ │ +threshold (default: 0.... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:168 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_I_S_A_M_2_P_a_r_a_m_s │ │ │ │ │ +ISAM2Params(OptimizationParams _optimizationParams=ISAM2GaussNewtonParams(), │ │ │ │ │ +RelinearizationThreshold _relinearizeThreshold=0.1, int _relinearizeSkip=10, │ │ │ │ │ +bool _enableRelinearization=true, bool _evaluateNonlinearError=false, │ │ │ │ │ +Factorization _factorization=ISAM2Params::CHOLESKY, bool │ │ │ │ │ +_cacheLinearizedFactors=true, const KeyFormatter │ │ │ │ │ +&_keyFormatter=DefaultKeyFormatter, bool _enableDetailedResults=false) │ │ │ │ │ +Specify parameters as constructor arguments See the documentation of member │ │ │ │ │ +variables above. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:230 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_._h │ │ │ │ │ + * _I_S_A_M_2_P_a_r_a_m_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01064.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/GraphvizFormatting.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/GaussNewtonOptimizer.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,43 +96,42 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
GraphvizFormatting.h File Reference
│ │ │ │ +
GaussNewtonOptimizer.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Graphviz formatter for NonlinearFactorGraph. │ │ │ │ -More...

│ │ │ │ - │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

struct  gtsam::GraphvizFormatting
 Formatting options and functions for saving a NonlinearFactorGraph instance in GraphViz format. More...
class  gtsam::GaussNewtonParams
 Parameters for Gauss-Newton optimization, inherits from NonlinearOptimizationParams. More...
 
class  gtsam::GaussNewtonOptimizer
 This class performs Gauss-Newton nonlinear optimization. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Graphviz formatter for NonlinearFactorGraph.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
Date
December, 2021
│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Feb 26, 2012
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,29 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -GraphvizFormatting.h File Reference │ │ │ │ │ -Graphviz formatter for NonlinearFactorGraph. _M_o_r_e_._._. │ │ │ │ │ +GaussNewtonOptimizer.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g │ │ │ │ │ -  Formatting options and functions for saving a _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ - instance in GraphViz format. _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s │ │ │ │ │ +  Parameters for Gauss-Newton optimization, inherits from │ │ │ │ │ + NonlinearOptimizationParams. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r │ │ │ │ │ +  This class performs Gauss-Newton nonlinear optimization. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Graphviz formatter for NonlinearFactorGraph. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ Date │ │ │ │ │ - December, 2021 │ │ │ │ │ + Feb 26, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g_._h │ │ │ │ │ + * _G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01064.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,4 @@ │ │ │ │ │ var a01064 = [ │ │ │ │ │ - ["gtsam::GraphvizFormatting", "a04368.html", "a04368"] │ │ │ │ │ + ["gtsam::GaussNewtonParams", "a04352.html", null], │ │ │ │ │ + ["gtsam::GaussNewtonOptimizer", "a04356.html", "a04356"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01064_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/GraphvizFormatting.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/GaussNewtonOptimizer.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,86 +98,91 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
GraphvizFormatting.h
│ │ │ │ +
GaussNewtonOptimizer.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010-2021, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/inference/DotWriter.h>
│ │ │ │ +
21#include <gtsam/nonlinear/NonlinearOptimizer.h>
│ │ │ │
22
│ │ │ │
23namespace gtsam {
│ │ │ │
24
│ │ │ │ -
25class Values;
│ │ │ │ -
26class Value;
│ │ │ │ -
27
│ │ │ │ -
│ │ │ │ -
32struct GTSAM_EXPORT GraphvizFormatting : public DotWriter {
│ │ │ │ -
34 enum Axis { X, Y, Z, NEGX, NEGY, NEGZ };
│ │ │ │ -
35
│ │ │ │ -
36 Axis paperHorizontalAxis;
│ │ │ │ -
38 Axis paperVerticalAxis;
│ │ │ │ -
40 double scale;
│ │ │ │ -
41 bool mergeSimilarFactors;
│ │ │ │ -
43
│ │ │ │ -
│ │ │ │ -
46 GraphvizFormatting()
│ │ │ │ -
47 : paperHorizontalAxis(Y),
│ │ │ │ -
48 paperVerticalAxis(X),
│ │ │ │ -
49 scale(1),
│ │ │ │ -
50 mergeSimilarFactors(false) {}
│ │ │ │ -
│ │ │ │ -
51
│ │ │ │ -
52 // Find bounds
│ │ │ │ -
53 Vector2 findBounds(const Values& values, const KeySet& keys) const;
│ │ │ │ -
54
│ │ │ │ -
56 boost::optional<Vector2> extractPosition(const Value& value) const;
│ │ │ │ +
25class GaussNewtonOptimizer;
│ │ │ │ +
26
│ │ │ │ +
│ │ │ │ +
30class GTSAM_EXPORT GaussNewtonParams : public NonlinearOptimizerParams {
│ │ │ │ +
31public:
│ │ │ │ +
32 using OptimizerType = GaussNewtonOptimizer;
│ │ │ │ +
33};
│ │ │ │ +
│ │ │ │ +
34
│ │ │ │ +
│ │ │ │ +
38class GTSAM_EXPORT GaussNewtonOptimizer : public NonlinearOptimizer {
│ │ │ │ +
39
│ │ │ │ +
40protected:
│ │ │ │ +
41 GaussNewtonParams params_;
│ │ │ │ +
42
│ │ │ │ +
43public:
│ │ │ │ +
46
│ │ │ │ +
55 GaussNewtonOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
│ │ │ │ +
56 const GaussNewtonParams& params = GaussNewtonParams());
│ │ │ │
57
│ │ │ │ -
59 boost::optional<Vector2> variablePos(const Values& values, const Vector2& min,
│ │ │ │ -
60 Key key) const;
│ │ │ │ -
61
│ │ │ │ -
63 boost::optional<Vector2> factorPos(const Vector2& min, size_t i) const;
│ │ │ │ -
64};
│ │ │ │ -
│ │ │ │ -
65
│ │ │ │ -
66} // namespace gtsam
│ │ │ │ -
DotWriter.h
Graphviz formatter.
│ │ │ │ +
65 GaussNewtonOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
│ │ │ │ +
66 const Ordering& ordering);
│ │ │ │ +
68
│ │ │ │ +
71
│ │ │ │ +
73 ~GaussNewtonOptimizer() override {}
│ │ │ │ +
74
│ │ │ │ +
79 GaussianFactorGraph::shared_ptr iterate() override;
│ │ │ │ +
80
│ │ │ │ +
82 const GaussNewtonParams& params() const { return params_; }
│ │ │ │ +
83
│ │ │ │ +
85
│ │ │ │ +
86protected:
│ │ │ │ +
88 const NonlinearOptimizerParams& _params() const override { return params_; }
│ │ │ │ +
89
│ │ │ │ +
91 GaussNewtonParams ensureHasOrdering(GaussNewtonParams params, const NonlinearFactorGraph& graph) const;
│ │ │ │ +
92
│ │ │ │ +
93};
│ │ │ │ +
│ │ │ │ +
94
│ │ │ │ +
95}
│ │ │ │ +
NonlinearOptimizer.h
Base class and parameters for nonlinear optimization algorithms.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ - │ │ │ │ -
gtsam::Value
This is the base class for any type to be stored in Values.
Definition Value.h:37
│ │ │ │ -
gtsam::DotWriter
DotWriter is a helper class for writing graphviz .dot files.
Definition DotWriter.h:35
│ │ │ │ -
gtsam::GraphvizFormatting
Formatting options and functions for saving a NonlinearFactorGraph instance in GraphViz format.
Definition GraphvizFormatting.h:32
│ │ │ │ -
gtsam::GraphvizFormatting::Axis
Axis
World axes to be assigned to paper axes.
Definition GraphvizFormatting.h:34
│ │ │ │ -
gtsam::GraphvizFormatting::paperVerticalAxis
Axis paperVerticalAxis
The world axis assigned to the vertical paper axis.
Definition GraphvizFormatting.h:38
│ │ │ │ -
gtsam::GraphvizFormatting::GraphvizFormatting
GraphvizFormatting()
Default constructor sets up robot coordinates.
Definition GraphvizFormatting.h:46
│ │ │ │ -
gtsam::GraphvizFormatting::mergeSimilarFactors
bool mergeSimilarFactors
Merge multiple factors that have the same connectivity.
Definition GraphvizFormatting.h:41
│ │ │ │ -
gtsam::GraphvizFormatting::scale
double scale
Scale all positions to reduce / increase density.
Definition GraphvizFormatting.h:40
│ │ │ │ -
gtsam::GraphvizFormatting::paperHorizontalAxis
Axis paperHorizontalAxis
The world axis assigned to the horizontal paper axis.
Definition GraphvizFormatting.h:36
│ │ │ │ +
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
gtsam::GaussianFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactorGraph.h:81
│ │ │ │ +
gtsam::GaussNewtonParams
Parameters for Gauss-Newton optimization, inherits from NonlinearOptimizationParams.
Definition GaussNewtonOptimizer.h:30
│ │ │ │ +
gtsam::GaussNewtonOptimizer
This class performs Gauss-Newton nonlinear optimization.
Definition GaussNewtonOptimizer.h:38
│ │ │ │ +
gtsam::GaussNewtonOptimizer::params
const GaussNewtonParams & params() const
Read-only access the parameters.
Definition GaussNewtonOptimizer.h:82
│ │ │ │ +
gtsam::GaussNewtonOptimizer::~GaussNewtonOptimizer
~GaussNewtonOptimizer() override
Virtual destructor.
Definition GaussNewtonOptimizer.h:73
│ │ │ │ +
gtsam::GaussNewtonOptimizer::_params
const NonlinearOptimizerParams & _params() const override
Access the parameters (base class version)
Definition GaussNewtonOptimizer.h:88
│ │ │ │ +
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │ +
gtsam::NonlinearOptimizer
This is the abstract interface for classes that can optimize for the maximum-likelihood estimate of a...
Definition NonlinearOptimizer.h:75
│ │ │ │ +
gtsam::NonlinearOptimizerParams
The common parameters for Nonlinear optimizers.
Definition NonlinearOptimizerParams.h:34
│ │ │ │
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,110 +1,111 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GraphvizFormatting.h │ │ │ │ │ +GaussNewtonOptimizer.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010-2021, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 19#pragma once │ │ │ │ │ 20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_D_o_t_W_r_i_t_e_r_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_._h> │ │ │ │ │ 22 │ │ │ │ │ 23namespace _g_t_s_a_m { │ │ │ │ │ 24 │ │ │ │ │ -25class _V_a_l_u_e_s; │ │ │ │ │ -26class Value; │ │ │ │ │ -27 │ │ │ │ │ -_3_2struct GTSAM_EXPORT _G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g : public _D_o_t_W_r_i_t_e_r { │ │ │ │ │ -_3_4 enum _A_x_i_s { X, Y, Z, NEGX, NEGY, NEGZ }; │ │ │ │ │ -35 │ │ │ │ │ -_3_6 _A_x_i_s _p_a_p_e_r_H_o_r_i_z_o_n_t_a_l_A_x_i_s; │ │ │ │ │ -_3_8 _A_x_i_s _p_a_p_e_r_V_e_r_t_i_c_a_l_A_x_i_s; │ │ │ │ │ -_4_0 double _s_c_a_l_e; │ │ │ │ │ -_4_1 bool _m_e_r_g_e_S_i_m_i_l_a_r_F_a_c_t_o_r_s; │ │ │ │ │ -43 │ │ │ │ │ -_4_6 _G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g() │ │ │ │ │ -47 : paperHorizontalAxis(Y), │ │ │ │ │ -48 paperVerticalAxis(X), │ │ │ │ │ -49 scale(1), │ │ │ │ │ -50 mergeSimilarFactors(false) {} │ │ │ │ │ -51 │ │ │ │ │ -52 // Find bounds │ │ │ │ │ -53 Vector2 findBounds(const _V_a_l_u_e_s& values, const _K_e_y_S_e_t& keys) const; │ │ │ │ │ -54 │ │ │ │ │ -56 boost::optional extractPosition(const _V_a_l_u_e& value) const; │ │ │ │ │ +25class GaussNewtonOptimizer; │ │ │ │ │ +26 │ │ │ │ │ +_3_0class GTSAM_EXPORT _G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s : public _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s { │ │ │ │ │ +31public: │ │ │ │ │ +32 using _O_p_t_i_m_i_z_e_r_T_y_p_e = _G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r; │ │ │ │ │ +33}; │ │ │ │ │ +34 │ │ │ │ │ +_3_8class GTSAM_EXPORT _G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r : public _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r { │ │ │ │ │ +39 │ │ │ │ │ +40protected: │ │ │ │ │ +41 _G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s params_; │ │ │ │ │ +42 │ │ │ │ │ +43public: │ │ │ │ │ +46 │ │ │ │ │ +55 _G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, const _V_a_l_u_e_s& │ │ │ │ │ +initialValues, │ │ │ │ │ +56 const _G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s& params = _G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s()); │ │ │ │ │ 57 │ │ │ │ │ -59 boost::optional variablePos(const _V_a_l_u_e_s& values, const Vector2& │ │ │ │ │ -min, │ │ │ │ │ -60 _K_e_y key) const; │ │ │ │ │ -61 │ │ │ │ │ -63 boost::optional factorPos(const Vector2& min, size_t i) const; │ │ │ │ │ -64}; │ │ │ │ │ -65 │ │ │ │ │ -66} // namespace gtsam │ │ │ │ │ -_D_o_t_W_r_i_t_e_r_._h │ │ │ │ │ -Graphviz formatter. │ │ │ │ │ +65 _G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, const _V_a_l_u_e_s& │ │ │ │ │ +initialValues, │ │ │ │ │ +66 const _O_r_d_e_r_i_n_g& ordering); │ │ │ │ │ +68 │ │ │ │ │ +71 │ │ │ │ │ +_7_3 _~_G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r() override {} │ │ │ │ │ +74 │ │ │ │ │ +79 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r iterate() override; │ │ │ │ │ +80 │ │ │ │ │ +_8_2 const _G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s& _p_a_r_a_m_s() const { return params_; } │ │ │ │ │ +83 │ │ │ │ │ +85 │ │ │ │ │ +86protected: │ │ │ │ │ +_8_8 const _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s& ___p_a_r_a_m_s() const override { return params_; } │ │ │ │ │ +89 │ │ │ │ │ +91 _G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s ensureHasOrdering(_G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s params, const │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph) const; │ │ │ │ │ +92 │ │ │ │ │ +93}; │ │ │ │ │ +94 │ │ │ │ │ +95} │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ +Base class and parameters for nonlinear optimization algorithms. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e │ │ │ │ │ -This is the base class for any type to be stored in Values. │ │ │ │ │ -DDeeffiinniittiioonn Value.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r │ │ │ │ │ -DotWriter is a helper class for writing graphviz .dot files. │ │ │ │ │ -DDeeffiinniittiioonn DotWriter.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g │ │ │ │ │ -Formatting options and functions for saving a NonlinearFactorGraph instance in │ │ │ │ │ -GraphViz format. │ │ │ │ │ -DDeeffiinniittiioonn GraphvizFormatting.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g_:_:_A_x_i_s │ │ │ │ │ -Axis │ │ │ │ │ -World axes to be assigned to paper axes. │ │ │ │ │ -DDeeffiinniittiioonn GraphvizFormatting.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g_:_:_p_a_p_e_r_V_e_r_t_i_c_a_l_A_x_i_s │ │ │ │ │ -Axis paperVerticalAxis │ │ │ │ │ -The world axis assigned to the vertical paper axis. │ │ │ │ │ -DDeeffiinniittiioonn GraphvizFormatting.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g_:_:_G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g │ │ │ │ │ -GraphvizFormatting() │ │ │ │ │ -Default constructor sets up robot coordinates. │ │ │ │ │ -DDeeffiinniittiioonn GraphvizFormatting.h:46 │ │ │ │ │ -_g_t_s_a_m_:_:_G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g_:_:_m_e_r_g_e_S_i_m_i_l_a_r_F_a_c_t_o_r_s │ │ │ │ │ -bool mergeSimilarFactors │ │ │ │ │ -Merge multiple factors that have the same connectivity. │ │ │ │ │ -DDeeffiinniittiioonn GraphvizFormatting.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g_:_:_s_c_a_l_e │ │ │ │ │ -double scale │ │ │ │ │ -Scale all positions to reduce / increase density. │ │ │ │ │ -DDeeffiinniittiioonn GraphvizFormatting.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g_:_:_p_a_p_e_r_H_o_r_i_z_o_n_t_a_l_A_x_i_s │ │ │ │ │ -Axis paperHorizontalAxis │ │ │ │ │ -The world axis assigned to the horizontal paper axis. │ │ │ │ │ -DDeeffiinniittiioonn GraphvizFormatting.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:81 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s │ │ │ │ │ +Parameters for Gauss-Newton optimization, inherits from │ │ │ │ │ +NonlinearOptimizationParams. │ │ │ │ │ +DDeeffiinniittiioonn GaussNewtonOptimizer.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r │ │ │ │ │ +This class performs Gauss-Newton nonlinear optimization. │ │ │ │ │ +DDeeffiinniittiioonn GaussNewtonOptimizer.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r_:_:_p_a_r_a_m_s │ │ │ │ │ +const GaussNewtonParams & params() const │ │ │ │ │ +Read-only access the parameters. │ │ │ │ │ +DDeeffiinniittiioonn GaussNewtonOptimizer.h:82 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r_:_:_~_G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r │ │ │ │ │ +~GaussNewtonOptimizer() override │ │ │ │ │ +Virtual destructor. │ │ │ │ │ +DDeeffiinniittiioonn GaussNewtonOptimizer.h:73 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r_:_:___p_a_r_a_m_s │ │ │ │ │ +const NonlinearOptimizerParams & _params() const override │ │ │ │ │ +Access the parameters (base class version) │ │ │ │ │ +DDeeffiinniittiioonn GaussNewtonOptimizer.h:88 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r │ │ │ │ │ +This is the abstract interface for classes that can optimize for the maximum- │ │ │ │ │ +likelihood estimate of a... │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizer.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s │ │ │ │ │ +The common parameters for Nonlinear optimizers. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizerParams.h:34 │ │ │ │ │ _g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_V_a_l_u_e_s │ │ │ │ │ -In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ -a non-linear function... │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g_._h │ │ │ │ │ + * _G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01067.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/GaussNewtonOptimizer.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2UpdateParams.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,42 +96,42 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
GaussNewtonOptimizer.h File Reference
│ │ │ │ +
ISAM2UpdateParams.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ +

Class that stores extra params for ISAM2::update() │ │ │ │ +More...

│ │ │ │ + │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::GaussNewtonParams
 Parameters for Gauss-Newton optimization, inherits from NonlinearOptimizationParams. More...
 
class  gtsam::GaussNewtonOptimizer
 This class performs Gauss-Newton nonlinear optimization. More...
struct  gtsam::ISAM2UpdateParams
 This struct is used by ISAM2::update() to pass additional parameters to give the user a fine-grained control on how factors and relinearized, etc. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Feb 26, 2012
│ │ │ │ +

Class that stores extra params for ISAM2::update()

│ │ │ │ +
Author
Michael Kaess, Richard Roberts, Frank Dellaert, Jose Luis Blanco
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,29 +1,27 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -GaussNewtonOptimizer.h File Reference │ │ │ │ │ +ISAM2UpdateParams.h File Reference │ │ │ │ │ +Class that stores extra params for ISAM2::update() _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s │ │ │ │ │ -  Parameters for Gauss-Newton optimization, inherits from │ │ │ │ │ - NonlinearOptimizationParams. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r │ │ │ │ │ -  This class performs Gauss-Newton nonlinear optimization. _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s │ │ │ │ │ + This struct is used by _I_S_A_M_2_:_:_u_p_d_a_t_e_(_) to pass additional parameters │ │ │ │ │ +  to give the user a fine-grained control on how factors and │ │ │ │ │ + relinearized, etc. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Class that stores extra params for ISAM2::update() │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Date │ │ │ │ │ - Feb 26, 2012 │ │ │ │ │ + Michael Kaess, Richard Roberts, Frank Dellaert, Jose Luis Blanco │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ + * _I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01067_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/GaussNewtonOptimizer.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2UpdateParams.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,91 +98,75 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
GaussNewtonOptimizer.h
│ │ │ │ +
ISAM2UpdateParams.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ -
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ -
4 * Atlanta, Georgia 30332-0415
│ │ │ │ -
5 * All Rights Reserved
│ │ │ │ -
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ -
7
│ │ │ │ -
8 * See LICENSE for the license information
│ │ │ │ -
9
│ │ │ │ -
10 * -------------------------------------------------------------------------- */
│ │ │ │ -
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/nonlinear/NonlinearOptimizer.h>
│ │ │ │ -
22
│ │ │ │ -
23namespace gtsam {
│ │ │ │ +
2 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
3 * Atlanta, Georgia 30332-0415
│ │ │ │ +
4 * All Rights Reserved
│ │ │ │ +
5 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
6 * See LICENSE for the license information
│ │ │ │ +
7 * -------------------------------------------------------------------------- */
│ │ │ │ +
8
│ │ │ │ +
15// \callgraph
│ │ │ │ +
16
│ │ │ │ +
17#pragma once
│ │ │ │ +
18
│ │ │ │ +
19#include <gtsam/base/FastList.h>
│ │ │ │ +
20#include <gtsam/dllexport.h> // GTSAM_EXPORT
│ │ │ │ +
21#include <gtsam/inference/Key.h> // Key, KeySet
│ │ │ │ +
22#include <gtsam/nonlinear/ISAM2Result.h> //FactorIndices
│ │ │ │ +
23#include <boost/optional.hpp>
│ │ │ │
24
│ │ │ │ -
25class GaussNewtonOptimizer;
│ │ │ │ +
25namespace gtsam {
│ │ │ │
26
│ │ │ │ -
│ │ │ │ -
30class GTSAM_EXPORT GaussNewtonParams : public NonlinearOptimizerParams {
│ │ │ │ -
31public:
│ │ │ │ -
32 using OptimizerType = GaussNewtonOptimizer;
│ │ │ │ -
33};
│ │ │ │ -
│ │ │ │ +
│ │ │ │ +
32struct ISAM2UpdateParams {
│ │ │ │ +
33 ISAM2UpdateParams() = default;
│ │ │ │
34
│ │ │ │ -
│ │ │ │ -
38class GTSAM_EXPORT GaussNewtonOptimizer : public NonlinearOptimizer {
│ │ │ │ -
39
│ │ │ │ -
40protected:
│ │ │ │ -
41 GaussNewtonParams params_;
│ │ │ │ -
42
│ │ │ │ -
43public:
│ │ │ │ -
46
│ │ │ │ -
55 GaussNewtonOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
│ │ │ │ -
56 const GaussNewtonParams& params = GaussNewtonParams());
│ │ │ │ -
57
│ │ │ │ -
65 GaussNewtonOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
│ │ │ │ -
66 const Ordering& ordering);
│ │ │ │ -
68
│ │ │ │ -
71
│ │ │ │ -
73 ~GaussNewtonOptimizer() override {}
│ │ │ │ -
74
│ │ │ │ -
79 GaussianFactorGraph::shared_ptr iterate() override;
│ │ │ │ -
80
│ │ │ │ -
82 const GaussNewtonParams& params() const { return params_; }
│ │ │ │ -
83
│ │ │ │ -
85
│ │ │ │ -
86protected:
│ │ │ │ -
88 const NonlinearOptimizerParams& _params() const override { return params_; }
│ │ │ │ -
89
│ │ │ │ -
91 GaussNewtonParams ensureHasOrdering(GaussNewtonParams params, const NonlinearFactorGraph& graph) const;
│ │ │ │ -
92
│ │ │ │ -
93};
│ │ │ │ -
│ │ │ │ -
94
│ │ │ │ -
95}
│ │ │ │ -
NonlinearOptimizer.h
Base class and parameters for nonlinear optimization algorithms.
│ │ │ │ +
36 FactorIndices removeFactorIndices;
│ │ │ │ +
37
│ │ │ │ +
40 boost::optional<FastMap<Key, int>> constrainedKeys{boost::none};
│ │ │ │ +
41
│ │ │ │ +
44 boost::optional<FastList<Key>> noRelinKeys{boost::none};
│ │ │ │ +
45
│ │ │ │ +
49 boost::optional<FastList<Key>> extraReelimKeys{boost::none};
│ │ │ │ +
50
│ │ │ │ +
54 bool force_relinearize{false};
│ │ │ │ +
55
│ │ │ │ +
66 boost::optional<FastMap<FactorIndex, KeySet>> newAffectedKeys{boost::none};
│ │ │ │ +
67
│ │ │ │ +
71 bool forceFullSolve{false};
│ │ │ │ +
72};
│ │ │ │ +
│ │ │ │ +
73
│ │ │ │ +
74} // namespace gtsam
│ │ │ │ +
FastList.h
A thin wrapper around std::list that uses boost's fast_pool_allocator.
│ │ │ │ + │ │ │ │ +
ISAM2Result.h
Class that stores detailed iSAM2 result.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ -
gtsam::GaussianFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactorGraph.h:81
│ │ │ │ -
gtsam::GaussNewtonParams
Parameters for Gauss-Newton optimization, inherits from NonlinearOptimizationParams.
Definition GaussNewtonOptimizer.h:30
│ │ │ │ -
gtsam::GaussNewtonOptimizer
This class performs Gauss-Newton nonlinear optimization.
Definition GaussNewtonOptimizer.h:38
│ │ │ │ -
gtsam::GaussNewtonOptimizer::params
const GaussNewtonParams & params() const
Read-only access the parameters.
Definition GaussNewtonOptimizer.h:82
│ │ │ │ -
gtsam::GaussNewtonOptimizer::~GaussNewtonOptimizer
~GaussNewtonOptimizer() override
Virtual destructor.
Definition GaussNewtonOptimizer.h:73
│ │ │ │ -
gtsam::GaussNewtonOptimizer::_params
const NonlinearOptimizerParams & _params() const override
Access the parameters (base class version)
Definition GaussNewtonOptimizer.h:88
│ │ │ │ -
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │ -
gtsam::NonlinearOptimizer
This is the abstract interface for classes that can optimize for the maximum-likelihood estimate of a...
Definition NonlinearOptimizer.h:75
│ │ │ │ -
gtsam::NonlinearOptimizerParams
The common parameters for Nonlinear optimizers.
Definition NonlinearOptimizerParams.h:34
│ │ │ │ -
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition Factor.h:34
│ │ │ │ +
gtsam::ISAM2UpdateParams
This struct is used by ISAM2::update() to pass additional parameters to give the user a fine-grained ...
Definition ISAM2UpdateParams.h:32
│ │ │ │ +
gtsam::ISAM2UpdateParams::constrainedKeys
boost::optional< FastMap< Key, int > > constrainedKeys
An optional map of keys to group labels, such that a variable can be constrained to a particular grou...
Definition ISAM2UpdateParams.h:40
│ │ │ │ +
gtsam::ISAM2UpdateParams::force_relinearize
bool force_relinearize
Relinearize any variables whose delta magnitude is sufficiently large (Params::relinearizeThreshold),...
Definition ISAM2UpdateParams.h:54
│ │ │ │ +
gtsam::ISAM2UpdateParams::removeFactorIndices
FactorIndices removeFactorIndices
Indices of factors to remove from system (default: empty)
Definition ISAM2UpdateParams.h:36
│ │ │ │ +
gtsam::ISAM2UpdateParams::forceFullSolve
bool forceFullSolve
By default, iSAM2 uses a wildfire update scheme that stops updating when the deltas become too small ...
Definition ISAM2UpdateParams.h:71
│ │ │ │ +
gtsam::ISAM2UpdateParams::extraReelimKeys
boost::optional< FastList< Key > > extraReelimKeys
An optional set of nonlinear keys that iSAM2 will re-eliminate, regardless of the size of the linear ...
Definition ISAM2UpdateParams.h:49
│ │ │ │ +
gtsam::ISAM2UpdateParams::noRelinKeys
boost::optional< FastList< Key > > noRelinKeys
An optional set of nonlinear keys that iSAM2 will hold at a constant linearization point,...
Definition ISAM2UpdateParams.h:44
│ │ │ │ +
gtsam::ISAM2UpdateParams::newAffectedKeys
boost::optional< FastMap< FactorIndex, KeySet > > newAffectedKeys
An optional set of new Keys that are now affected by factors, indexed by factor indices (as returned ...
Definition ISAM2UpdateParams.h:66
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,111 +1,102 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GaussNewtonOptimizer.h │ │ │ │ │ +ISAM2UpdateParams.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ -2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ -4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -5 * All Rights Reserved │ │ │ │ │ -6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ -7 │ │ │ │ │ -8 * See LICENSE for the license information │ │ │ │ │ -9 │ │ │ │ │ -10 * ------------------------------------------------------------------------- │ │ │ │ │ +2 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +3 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +4 * All Rights Reserved │ │ │ │ │ +5 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +6 * See LICENSE for the license information │ │ │ │ │ +7 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ -11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_._h> │ │ │ │ │ -22 │ │ │ │ │ -23namespace _g_t_s_a_m { │ │ │ │ │ +8 │ │ │ │ │ +15// \callgraph │ │ │ │ │ +16 │ │ │ │ │ +17#pragma once │ │ │ │ │ +18 │ │ │ │ │ +19#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_L_i_s_t_._h> │ │ │ │ │ +20#include // GTSAM_EXPORT │ │ │ │ │ +21#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> // Key, KeySet │ │ │ │ │ +22#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_I_S_A_M_2_R_e_s_u_l_t_._h> //FactorIndices │ │ │ │ │ +23#include │ │ │ │ │ 24 │ │ │ │ │ -25class GaussNewtonOptimizer; │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ 26 │ │ │ │ │ -_3_0class GTSAM_EXPORT _G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s : public _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s { │ │ │ │ │ -31public: │ │ │ │ │ -32 using _O_p_t_i_m_i_z_e_r_T_y_p_e = _G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r; │ │ │ │ │ -33}; │ │ │ │ │ +_3_2struct _I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s { │ │ │ │ │ +33 _I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s() = default; │ │ │ │ │ 34 │ │ │ │ │ -_3_8class GTSAM_EXPORT _G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r : public _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r { │ │ │ │ │ -39 │ │ │ │ │ -40protected: │ │ │ │ │ -41 _G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s params_; │ │ │ │ │ -42 │ │ │ │ │ -43public: │ │ │ │ │ -46 │ │ │ │ │ -55 _G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, const _V_a_l_u_e_s& │ │ │ │ │ -initialValues, │ │ │ │ │ -56 const _G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s& params = _G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s()); │ │ │ │ │ -57 │ │ │ │ │ -65 _G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, const _V_a_l_u_e_s& │ │ │ │ │ -initialValues, │ │ │ │ │ -66 const _O_r_d_e_r_i_n_g& ordering); │ │ │ │ │ -68 │ │ │ │ │ -71 │ │ │ │ │ -_7_3 _~_G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r() override {} │ │ │ │ │ -74 │ │ │ │ │ -79 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r iterate() override; │ │ │ │ │ -80 │ │ │ │ │ -_8_2 const _G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s& _p_a_r_a_m_s() const { return params_; } │ │ │ │ │ -83 │ │ │ │ │ -85 │ │ │ │ │ -86protected: │ │ │ │ │ -_8_8 const _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s& ___p_a_r_a_m_s() const override { return params_; } │ │ │ │ │ -89 │ │ │ │ │ -91 _G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s ensureHasOrdering(_G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s params, const │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph) const; │ │ │ │ │ -92 │ │ │ │ │ -93}; │ │ │ │ │ -94 │ │ │ │ │ -95} │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ -Base class and parameters for nonlinear optimization algorithms. │ │ │ │ │ +_3_6 _F_a_c_t_o_r_I_n_d_i_c_e_s _r_e_m_o_v_e_F_a_c_t_o_r_I_n_d_i_c_e_s; │ │ │ │ │ +37 │ │ │ │ │ +_4_0 boost::optional> _c_o_n_s_t_r_a_i_n_e_d_K_e_y_s{boost::none}; │ │ │ │ │ +41 │ │ │ │ │ +_4_4 boost::optional> _n_o_R_e_l_i_n_K_e_y_s{boost::none}; │ │ │ │ │ +45 │ │ │ │ │ +_4_9 boost::optional> _e_x_t_r_a_R_e_e_l_i_m_K_e_y_s{boost::none}; │ │ │ │ │ +50 │ │ │ │ │ +_5_4 bool _f_o_r_c_e___r_e_l_i_n_e_a_r_i_z_e{false}; │ │ │ │ │ +55 │ │ │ │ │ +_6_6 boost::optional> _n_e_w_A_f_f_e_c_t_e_d_K_e_y_s{boost::none}; │ │ │ │ │ +67 │ │ │ │ │ +_7_1 bool _f_o_r_c_e_F_u_l_l_S_o_l_v_e{false}; │ │ │ │ │ +72}; │ │ │ │ │ +73 │ │ │ │ │ +74} // namespace gtsam │ │ │ │ │ +_F_a_s_t_L_i_s_t_._h │ │ │ │ │ +A thin wrapper around std::list that uses boost's fast_pool_allocator. │ │ │ │ │ +_K_e_y_._h │ │ │ │ │ +_I_S_A_M_2_R_e_s_u_l_t_._h │ │ │ │ │ +Class that stores detailed iSAM2 result. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:81 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_N_e_w_t_o_n_P_a_r_a_m_s │ │ │ │ │ -Parameters for Gauss-Newton optimization, inherits from │ │ │ │ │ -NonlinearOptimizationParams. │ │ │ │ │ -DDeeffiinniittiioonn GaussNewtonOptimizer.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r │ │ │ │ │ -This class performs Gauss-Newton nonlinear optimization. │ │ │ │ │ -DDeeffiinniittiioonn GaussNewtonOptimizer.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r_:_:_p_a_r_a_m_s │ │ │ │ │ -const GaussNewtonParams & params() const │ │ │ │ │ -Read-only access the parameters. │ │ │ │ │ -DDeeffiinniittiioonn GaussNewtonOptimizer.h:82 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r_:_:_~_G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r │ │ │ │ │ -~GaussNewtonOptimizer() override │ │ │ │ │ -Virtual destructor. │ │ │ │ │ -DDeeffiinniittiioonn GaussNewtonOptimizer.h:73 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r_:_:___p_a_r_a_m_s │ │ │ │ │ -const NonlinearOptimizerParams & _params() const override │ │ │ │ │ -Access the parameters (base class version) │ │ │ │ │ -DDeeffiinniittiioonn GaussNewtonOptimizer.h:88 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r │ │ │ │ │ -This is the abstract interface for classes that can optimize for the maximum- │ │ │ │ │ -likelihood estimate of a... │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizer.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s │ │ │ │ │ -The common parameters for Nonlinear optimizers. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizerParams.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_I_n_d_i_c_e_s │ │ │ │ │ +FastVector< FactorIndex > FactorIndices │ │ │ │ │ +Define collection types: │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s │ │ │ │ │ +This struct is used by ISAM2::update() to pass additional parameters to give │ │ │ │ │ +the user a fine-grained ... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2UpdateParams.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_:_:_c_o_n_s_t_r_a_i_n_e_d_K_e_y_s │ │ │ │ │ +boost::optional< FastMap< Key, int > > constrainedKeys │ │ │ │ │ +An optional map of keys to group labels, such that a variable can be │ │ │ │ │ +constrained to a particular grou... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2UpdateParams.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_:_:_f_o_r_c_e___r_e_l_i_n_e_a_r_i_z_e │ │ │ │ │ +bool force_relinearize │ │ │ │ │ +Relinearize any variables whose delta magnitude is sufficiently large (Params:: │ │ │ │ │ +relinearizeThreshold),... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2UpdateParams.h:54 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_:_:_r_e_m_o_v_e_F_a_c_t_o_r_I_n_d_i_c_e_s │ │ │ │ │ +FactorIndices removeFactorIndices │ │ │ │ │ +Indices of factors to remove from system (default: empty) │ │ │ │ │ +DDeeffiinniittiioonn ISAM2UpdateParams.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_:_:_f_o_r_c_e_F_u_l_l_S_o_l_v_e │ │ │ │ │ +bool forceFullSolve │ │ │ │ │ +By default, iSAM2 uses a wildfire update scheme that stops updating when the │ │ │ │ │ +deltas become too small ... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2UpdateParams.h:71 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_:_:_e_x_t_r_a_R_e_e_l_i_m_K_e_y_s │ │ │ │ │ +boost::optional< FastList< Key > > extraReelimKeys │ │ │ │ │ +An optional set of nonlinear keys that iSAM2 will re-eliminate, regardless of │ │ │ │ │ +the size of the linear ... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2UpdateParams.h:49 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_:_:_n_o_R_e_l_i_n_K_e_y_s │ │ │ │ │ +boost::optional< FastList< Key > > noRelinKeys │ │ │ │ │ +An optional set of nonlinear keys that iSAM2 will hold at a constant │ │ │ │ │ +linearization point,... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2UpdateParams.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_:_:_n_e_w_A_f_f_e_c_t_e_d_K_e_y_s │ │ │ │ │ +boost::optional< FastMap< FactorIndex, KeySet > > newAffectedKeys │ │ │ │ │ +An optional set of new Keys that are now affected by factors, indexed by factor │ │ │ │ │ +indices (as returned ... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2UpdateParams.h:66 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ + * _I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01070.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Marginals.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/LevenbergMarquardtParams.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,48 +94,40 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
Marginals.h File Reference
│ │ │ │ +
LevenbergMarquardtParams.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

A class for computing marginals in a NonlinearFactorGraph. │ │ │ │ +

Parameters for Levenberg-Marquardt trust-region scheme. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::Marginals
 A class for computing Gaussian marginals of variables in a NonlinearFactorGraph. More...
 
class  gtsam::JointMarginal
 A class to store and access a joint marginal, returned from Marginals::jointMarginalCovariance and Marginals::jointMarginalInformation. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

A class for computing marginals in a NonlinearFactorGraph.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
May 14, 2012
│ │ │ │ +

Parameters for Levenberg-Marquardt trust-region scheme.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +
│ │ │ │ +Luca Carlone
│ │ │ │ +
Date
Feb 26, 2012
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,33 +1,24 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Marginals.h File Reference │ │ │ │ │ -A class for computing marginals in a NonlinearFactorGraph. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_M_a_r_g_i_n_a_l_s │ │ │ │ │ -  A class for computing Gaussian marginals of variables in a │ │ │ │ │ - _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_J_o_i_n_t_M_a_r_g_i_n_a_l │ │ │ │ │ - A class to store and access a joint marginal, returned from _M_a_r_g_i_n_a_l_s_:_: │ │ │ │ │ -  _j_o_i_n_t_M_a_r_g_i_n_a_l_C_o_v_a_r_i_a_n_c_e and _M_a_r_g_i_n_a_l_s_:_:_j_o_i_n_t_M_a_r_g_i_n_a_l_I_n_f_o_r_m_a_t_i_o_n. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +LevenbergMarquardtParams.cpp File Reference │ │ │ │ │ +Parameters for Levenberg-Marquardt trust-region scheme. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A class for computing marginals in a NonlinearFactorGraph. │ │ │ │ │ +Parameters for Levenberg-Marquardt trust-region scheme. │ │ │ │ │ Author │ │ │ │ │ Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Luca Carlone │ │ │ │ │ Date │ │ │ │ │ - May 14, 2012 │ │ │ │ │ + Feb 26, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _M_a_r_g_i_n_a_l_s_._h │ │ │ │ │ + * _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01073.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/GraphvizFormatting.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Values.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,36 +94,68 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
GraphvizFormatting.cpp File Reference
│ │ │ │ +
Values.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Graphviz formatter for NonlinearFactorGraph. │ │ │ │ +

A non-templated config holding any types of Manifold-group elements. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::ValueCloneAllocator
 
class  gtsam::Values
 A non-templated config holding any types of Manifold-group elements. More...
 
struct  gtsam::Values::KeyValuePair
 A key-value pair, which you get by dereferencing iterators. More...
 
struct  gtsam::Values::ConstKeyValuePair
 A key-value pair, which you get by dereferencing iterators. More...
 
struct  gtsam::Values::deref_iterator
 
class  gtsam::ValuesKeyAlreadyExists
 
class  gtsam::ValuesKeyDoesNotExist
 
class  gtsam::ValuesIncorrectType
 
class  gtsam::DynamicValuesMismatched
 
class  gtsam::NoMatchFoundForFixed
 
struct  gtsam::traits< Values >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Graphviz formatter for NonlinearFactorGraph.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
Date
December, 2021
│ │ │ │ +

A non-templated config holding any types of Manifold-group elements.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +

Detailed story: A values structure is a map from keys to values. It is used to specify the value of a bunch of variables in a factor graph. A Values is a values structure which can hold variables that are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type which is also a manifold element, and hence supports operations dim, retract, and localCoordinates.

│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,55 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -GraphvizFormatting.cpp File Reference │ │ │ │ │ -Graphviz formatter for NonlinearFactorGraph. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +Values.h File Reference │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_V_a_l_u_e_C_l_o_n_e_A_l_l_o_c_a_t_o_r │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ +  A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_K_e_y_V_a_l_u_e_P_a_i_r │ │ │ │ │ +  A key-value pair, which you get by dereferencing iterators. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r │ │ │ │ │ +  A key-value pair, which you get by dereferencing iterators. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_d_e_r_e_f___i_t_e_r_a_t_o_r │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_V_a_l_u_e_s_K_e_y_A_l_r_e_a_d_y_E_x_i_s_t_s │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_V_a_l_u_e_s_K_e_y_D_o_e_s_N_o_t_E_x_i_s_t │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_D_y_n_a_m_i_c_V_a_l_u_e_s_M_i_s_m_a_t_c_h_e_d │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_N_o_M_a_t_c_h_F_o_u_n_d_F_o_r_F_i_x_e_d │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _V_a_l_u_e_s_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Graphviz formatter for NonlinearFactorGraph. │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - December, 2021 │ │ │ │ │ + Richard Roberts │ │ │ │ │ +Detailed story: A values structure is a map from keys to values. It is used to │ │ │ │ │ +specify the value of a bunch of variables in a factor graph. A _V_a_l_u_e_s is a │ │ │ │ │ +values structure which can hold variables that are elements on manifolds, not │ │ │ │ │ +just vectors. It then, as a whole, implements a aggregate type which is also a │ │ │ │ │ +manifold element, and hence supports operations dim, retract, and │ │ │ │ │ +localCoordinates. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g_._c_p_p │ │ │ │ │ + * _V_a_l_u_e_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01076.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/DoglegOptimizer.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,44 +94,35 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
DoglegOptimizer.h File Reference
│ │ │ │ +
ISAM2.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. │ │ │ │ +More...

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::DoglegParams
 Parameters for Levenberg-Marquardt optimization. More...
 
class  gtsam::DoglegOptimizer
 This class performs Dogleg nonlinear optimization. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Feb 26, 2012
│ │ │ │ +

Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.

│ │ │ │ +
Author
Michael Kaess, Richard Roberts, Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,22 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -DoglegOptimizer.h File Reference │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_D_o_g_l_e_g_P_a_r_a_m_s │ │ │ │ │ -  Parameters for Levenberg-Marquardt optimization. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r │ │ │ │ │ -  This class performs Dogleg nonlinear optimization. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +ISAM2.cpp File Reference │ │ │ │ │ +Incremental update functionality (ISAM2) for BayesTree, with fluid │ │ │ │ │ +relinearization. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Incremental update functionality (ISAM2) for BayesTree, with fluid │ │ │ │ │ +relinearization. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Date │ │ │ │ │ - Feb 26, 2012 │ │ │ │ │ + Michael Kaess, Richard Roberts, Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ + * _I_S_A_M_2_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01079.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/LevenbergMarquardtParams.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/CustomFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,39 +95,34 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces
│ │ │ │ -
LevenbergMarquardtParams.cpp File Reference
│ │ │ │ +
CustomFactor.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Parameters for Levenberg-Marquardt trust-region scheme. │ │ │ │ +

Class to enable arbitrary factors with runtime swappable error function. │ │ │ │ More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Parameters for Levenberg-Marquardt trust-region scheme.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ -
│ │ │ │ -Luca Carlone
│ │ │ │ -
Date
Feb 26, 2012
│ │ │ │ +

Class to enable arbitrary factors with runtime swappable error function.

│ │ │ │ +
Author
Fan Jiang
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,24 +1,21 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -LevenbergMarquardtParams.cpp File Reference │ │ │ │ │ -Parameters for Levenberg-Marquardt trust-region scheme. _M_o_r_e_._._. │ │ │ │ │ +CustomFactor.cpp File Reference │ │ │ │ │ +Class to enable arbitrary factors with runtime swappable error function. │ │ │ │ │ +_M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Parameters for Levenberg-Marquardt trust-region scheme. │ │ │ │ │ +Class to enable arbitrary factors with runtime swappable error function. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Luca Carlone │ │ │ │ │ - Date │ │ │ │ │ - Feb 26, 2012 │ │ │ │ │ + Fan Jiang │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_._c_p_p │ │ │ │ │ + * _C_u_s_t_o_m_F_a_c_t_o_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01082.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/FunctorizedFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,35 +94,62 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces
│ │ │ │ -
ISAM2.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
FunctorizedFactor.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. │ │ │ │ -More...

│ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::FunctorizedFactor< R, T >
 Factor which evaluates provided unary functor and uses the result to compute error with respect to the provided measurement. More...
 
struct  gtsam::traits< FunctorizedFactor< R, T > >
 traits More...
 
class  gtsam::FunctorizedFactor2< R, T1, T2 >
 Factor which evaluates provided binary functor and uses the result to compute error with respect to the provided measurement. More...
 
struct  gtsam::traits< FunctorizedFactor2< R, T1, T2 > >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

template<typename T , typename R , typename FUNC >
FunctorizedFactor< R, T > gtsam::MakeFunctorizedFactor (Key key, const R &z, const SharedNoiseModel &model, const FUNC func)
 Helper function to create a functorized factor.
 
template<typename T1 , typename T2 , typename R , typename FUNC >
FunctorizedFactor2< R, T1, T2 > gtsam::MakeFunctorizedFactor2 (Key key1, Key key2, const R &z, const SharedNoiseModel &model, const FUNC func)
 Helper function to create a functorized factor.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.

│ │ │ │ -
Author
Michael Kaess, Richard Roberts, Frank Dellaert
│ │ │ │ +
Date
May 31, 2020
│ │ │ │ +
Author
Varun Agrawal
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,51 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -ISAM2.cpp File Reference │ │ │ │ │ -Incremental update functionality (ISAM2) for BayesTree, with fluid │ │ │ │ │ -relinearization. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +FunctorizedFactor.h File Reference │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_<_ _R_,_ _T_ _> │ │ │ │ │ +  _F_a_c_t_o_r which evaluates provided unary functor and uses the result to │ │ │ │ │ + compute error with respect to the provided measurement. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_<_ _R_,_ _T_ _>_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2_<_ _R_,_ _T_1_,_ _T_2_ _> │ │ │ │ │ +  _F_a_c_t_o_r which evaluates provided binary functor and uses the result to │ │ │ │ │ + compute error with respect to the provided measurement. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2_<_ _R_,_ _T_1_,_ _T_2_ _>_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +template │ │ │ │ │ + _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r< R, T >  _g_t_s_a_m_:_:_M_a_k_e_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r (_K_e_y key, const R │ │ │ │ │ + &z, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, const FUNC │ │ │ │ │ + func) │ │ │ │ │ +  Helper function to create a functorized │ │ │ │ │ + factor. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2< R, T1, T2 >  _g_t_s_a_m_:_:_M_a_k_e_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2 (_K_e_y key1, _K_e_y │ │ │ │ │ + key2, const R &z, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ + &model, const FUNC func) │ │ │ │ │ +  Helper function to create a functorized │ │ │ │ │ + factor. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Incremental update functionality (ISAM2) for BayesTree, with fluid │ │ │ │ │ -relinearization. │ │ │ │ │ + Date │ │ │ │ │ + May 31, 2020 │ │ │ │ │ Author │ │ │ │ │ - Michael Kaess, Richard Roberts, Frank Dellaert │ │ │ │ │ + Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _I_S_A_M_2_._c_p_p │ │ │ │ │ + * _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01085.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/LevenbergMarquardtOptimizer.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Marginals.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,47 +96,46 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
LevenbergMarquardtOptimizer.h File Reference
│ │ │ │ +
Marginals.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme. │ │ │ │ +

A class for computing marginals in a NonlinearFactorGraph. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::LevenbergMarquardtOptimizer
 This class performs Levenberg-Marquardt nonlinear optimization. More...
class  gtsam::Marginals
 A class for computing Gaussian marginals of variables in a NonlinearFactorGraph. More...
 
class  gtsam::JointMarginal
 A class to store and access a joint marginal, returned from Marginals::jointMarginalCovariance and Marginals::jointMarginalInformation. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ -
│ │ │ │ -Luca Carlone
│ │ │ │ -
Date
Feb 26, 2012
│ │ │ │ +

A class for computing marginals in a NonlinearFactorGraph.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
May 14, 2012
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,30 +1,33 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -LevenbergMarquardtOptimizer.h File Reference │ │ │ │ │ -A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme. │ │ │ │ │ -_M_o_r_e_._._. │ │ │ │ │ +Marginals.h File Reference │ │ │ │ │ +A class for computing marginals in a NonlinearFactorGraph. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r │ │ │ │ │ -  This class performs Levenberg-Marquardt nonlinear optimization. _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_M_a_r_g_i_n_a_l_s │ │ │ │ │ +  A class for computing Gaussian marginals of variables in a │ │ │ │ │ + _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_J_o_i_n_t_M_a_r_g_i_n_a_l │ │ │ │ │ + A class to store and access a joint marginal, returned from _M_a_r_g_i_n_a_l_s_:_: │ │ │ │ │ +  _j_o_i_n_t_M_a_r_g_i_n_a_l_C_o_v_a_r_i_a_n_c_e and _M_a_r_g_i_n_a_l_s_:_:_j_o_i_n_t_M_a_r_g_i_n_a_l_I_n_f_o_r_m_a_t_i_o_n. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme. │ │ │ │ │ +A class for computing marginals in a NonlinearFactorGraph. │ │ │ │ │ Author │ │ │ │ │ Richard Roberts │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Luca Carlone │ │ │ │ │ Date │ │ │ │ │ - Feb 26, 2012 │ │ │ │ │ + May 14, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ + * _M_a_r_g_i_n_a_l_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01085.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,4 @@ │ │ │ │ │ var a01085 = [ │ │ │ │ │ - ["gtsam::LevenbergMarquardtOptimizer", "a04436.html", "a04436"] │ │ │ │ │ + ["gtsam::Marginals", "a04452.html", "a04452"], │ │ │ │ │ + ["gtsam::JointMarginal", "a04456.html", "a04456"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01085_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/LevenbergMarquardtOptimizer.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Marginals.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,131 +98,188 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
LevenbergMarquardtOptimizer.h
│ │ │ │ +
Marginals.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
21#pragma once
│ │ │ │ -
22
│ │ │ │ -
23#include <gtsam/nonlinear/NonlinearOptimizer.h>
│ │ │ │ -
24#include <gtsam/nonlinear/LevenbergMarquardtParams.h>
│ │ │ │ -
25#include <gtsam/linear/VectorValues.h>
│ │ │ │ -
26#include <boost/date_time/posix_time/posix_time.hpp>
│ │ │ │ -
27
│ │ │ │ -
28class NonlinearOptimizerMoreOptimizationTest;
│ │ │ │ -
29
│ │ │ │ -
30namespace gtsam {
│ │ │ │ -
31
│ │ │ │ -
│ │ │ │ -
35class GTSAM_EXPORT LevenbergMarquardtOptimizer: public NonlinearOptimizer {
│ │ │ │ -
36
│ │ │ │ -
37protected:
│ │ │ │ -
38 const LevenbergMarquardtParams params_;
│ │ │ │ -
39 boost::posix_time::ptime startTime_;
│ │ │ │ -
40
│ │ │ │ -
41 void initTime();
│ │ │ │ -
42
│ │ │ │ -
43public:
│ │ │ │ -
44 typedef boost::shared_ptr<LevenbergMarquardtOptimizer> shared_ptr;
│ │ │ │ -
45
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/linear/GaussianBayesTree.h>
│ │ │ │ +
22#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ +
23#include <gtsam/nonlinear/Values.h>
│ │ │ │ +
24
│ │ │ │ +
25namespace gtsam {
│ │ │ │ +
26
│ │ │ │ +
27class JointMarginal;
│ │ │ │ +
28
│ │ │ │ +
│ │ │ │ +
32class GTSAM_EXPORT Marginals {
│ │ │ │ +
33
│ │ │ │ +
34public:
│ │ │ │ +
35
│ │ │ │ +
│ │ │ │ +
37 enum Factorization {
│ │ │ │ +
38 CHOLESKY,
│ │ │ │ +
39 QR
│ │ │ │ +
40 };
│ │ │ │ +
│ │ │ │ +
41
│ │ │ │ +
42protected:
│ │ │ │ +
43
│ │ │ │ +
44 GaussianFactorGraph graph_;
│ │ │ │ +
45 Values values_;
│ │ │ │ +
46 Factorization factorization_;
│ │ │ │ +
47 GaussianBayesTree bayesTree_;
│ │ │ │
48
│ │ │ │ -
57 LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
│ │ │ │ -
58 const LevenbergMarquardtParams& params = LevenbergMarquardtParams());
│ │ │ │ -
59
│ │ │ │ -
67 LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
│ │ │ │ -
68 const Ordering& ordering,
│ │ │ │ -
69 const LevenbergMarquardtParams& params = LevenbergMarquardtParams());
│ │ │ │ -
70
│ │ │ │ -
│ │ │ │ -
72 ~LevenbergMarquardtOptimizer() override {
│ │ │ │ -
73 }
│ │ │ │ -
│ │ │ │ -
74
│ │ │ │ +
49public:
│ │ │ │ +
50
│ │ │ │ +
52 Marginals(){}
│ │ │ │ +
53
│ │ │ │ +
59 Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY);
│ │ │ │ +
60
│ │ │ │ +
67 Marginals(const NonlinearFactorGraph& graph, const Values& solution, const Ordering& ordering,
│ │ │ │ +
68 Factorization factorization = CHOLESKY);
│ │ │ │ +
69
│ │ │ │ +
75 Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY);
│ │ │ │
76
│ │ │ │ -
79
│ │ │ │ -
81 double lambda() const;
│ │ │ │ -
82
│ │ │ │ -
84 int getInnerIterations() const;
│ │ │ │ +
83 Marginals(const GaussianFactorGraph& graph, const Values& solution, const Ordering& ordering,
│ │ │ │ +
84 Factorization factorization = CHOLESKY);
│ │ │ │
85
│ │ │ │ -
│ │ │ │ -
87 void print(const std::string& str = "") const {
│ │ │ │ -
88 std::cout << str << "LevenbergMarquardtOptimizer" << std::endl;
│ │ │ │ -
89 this->params_.print(" parameters:\n");
│ │ │ │ -
90 }
│ │ │ │ -
│ │ │ │ -
91
│ │ │ │ +
92 Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization = CHOLESKY);
│ │ │ │
93
│ │ │ │ -
96
│ │ │ │ -
101 GaussianFactorGraph::shared_ptr iterate() override;
│ │ │ │ +
100 Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, const Ordering& ordering,
│ │ │ │ +
101 Factorization factorization = CHOLESKY);
│ │ │ │
102
│ │ │ │ -
│ │ │ │ -
104 const LevenbergMarquardtParams& params() const {
│ │ │ │ -
105 return params_;
│ │ │ │ -
106 }
│ │ │ │ -
│ │ │ │ -
107
│ │ │ │ -
108 void writeLogFile(double currentError);
│ │ │ │ -
109
│ │ │ │ -
111 virtual GaussianFactorGraph::shared_ptr linearize() const;
│ │ │ │ +
104 void print(const std::string& str = "Marginals: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ +
105
│ │ │ │ +
107 GaussianFactor::shared_ptr marginalFactor(Key variable) const;
│ │ │ │ +
108
│ │ │ │ +
111 Matrix marginalInformation(Key variable) const;
│ │ │ │
112
│ │ │ │ -
114 GaussianFactorGraph buildDampedSystem(const GaussianFactorGraph& linear,
│ │ │ │ -
115 const VectorValues& sqrtHessianDiagonal) const;
│ │ │ │ -
116
│ │ │ │ -
118 bool tryLambda(const GaussianFactorGraph& linear, const VectorValues& sqrtHessianDiagonal);
│ │ │ │ -
119
│ │ │ │ +
114 Matrix marginalCovariance(Key variable) const;
│ │ │ │ +
115
│ │ │ │ +
117 JointMarginal jointMarginalCovariance(const KeyVector& variables) const;
│ │ │ │ +
118
│ │ │ │ +
120 JointMarginal jointMarginalInformation(const KeyVector& variables) const;
│ │ │ │
121
│ │ │ │ -
122protected:
│ │ │ │ -
123
│ │ │ │ -
│ │ │ │ -
125 const NonlinearOptimizerParams& _params() const override {
│ │ │ │ -
126 return params_;
│ │ │ │ -
127 }
│ │ │ │ -
│ │ │ │ -
128};
│ │ │ │ -
│ │ │ │ +
123 VectorValues optimize() const;
│ │ │ │ +
124
│ │ │ │ +
125protected:
│ │ │ │ +
126
│ │ │ │ +
128 void computeBayesTree();
│ │ │ │
129
│ │ │ │ -
130}
│ │ │ │ -
VectorValues.h
Factor Graph Values.
│ │ │ │ -
NonlinearOptimizer.h
Base class and parameters for nonlinear optimization algorithms.
│ │ │ │ -
LevenbergMarquardtParams.h
Parameters for Levenberg-Marquardt trust-region scheme.
│ │ │ │ +
131 void computeBayesTree(const Ordering& ordering);
│ │ │ │ +
132
│ │ │ │ +
133public:
│ │ │ │ +
134#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ +
136 GTSAM_DEPRECATED Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization,
│ │ │ │ +
137 const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {}
│ │ │ │ +
138
│ │ │ │ +
140 GTSAM_DEPRECATED Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization,
│ │ │ │ +
141 const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {}
│ │ │ │ +
142
│ │ │ │ +
144 GTSAM_DEPRECATED Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization,
│ │ │ │ +
145 const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {}
│ │ │ │ +
146#endif
│ │ │ │ +
147
│ │ │ │ +
148};
│ │ │ │ +
│ │ │ │ +
149
│ │ │ │ +
│ │ │ │ +
153class GTSAM_EXPORT JointMarginal {
│ │ │ │ +
154
│ │ │ │ +
155protected:
│ │ │ │ +
156 SymmetricBlockMatrix blockMatrix_;
│ │ │ │ +
157 KeyVector keys_;
│ │ │ │ +
158 FastMap<Key, size_t> indices_;
│ │ │ │ +
159
│ │ │ │ +
160public:
│ │ │ │ +
162 JointMarginal() {}
│ │ │ │ +
163
│ │ │ │ +
│ │ │ │ +
177 Matrix operator()(Key iVariable, Key jVariable) const {
│ │ │ │ +
178 const auto indexI = indices_.at(iVariable);
│ │ │ │ +
179 const auto indexJ = indices_.at(jVariable);
│ │ │ │ +
180 return blockMatrix_.block(indexI, indexJ);
│ │ │ │ +
181 }
│ │ │ │ +
│ │ │ │ +
182
│ │ │ │ +
│ │ │ │ +
184 Matrix at(Key iVariable, Key jVariable) const {
│ │ │ │ +
185 return (*this)(iVariable, jVariable);
│ │ │ │ +
186 }
│ │ │ │ +
│ │ │ │ +
187
│ │ │ │ +
│ │ │ │ +
189 Matrix fullMatrix() const {
│ │ │ │ +
190 return blockMatrix_.selfadjointView();
│ │ │ │ +
191 }
│ │ │ │ +
│ │ │ │ +
192
│ │ │ │ +
194 void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const;
│ │ │ │ +
195
│ │ │ │ +
196protected:
│ │ │ │ +
197 JointMarginal(const Matrix& fullMatrix, const std::vector<size_t>& dims, const KeyVector& keys) :
│ │ │ │ +
198 blockMatrix_(dims, fullMatrix), keys_(keys), indices_(Ordering(keys).invert()) {}
│ │ │ │ +
199
│ │ │ │ +
200 friend class Marginals;
│ │ │ │ +
201
│ │ │ │ +
202};
│ │ │ │ +
│ │ │ │ +
203
│ │ │ │ +
204} /* namespace gtsam */
│ │ │ │ +
GaussianBayesTree.h
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
│ │ │ │ +
Values.h
A non-templated config holding any types of Manifold-group elements.
│ │ │ │ +
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ +
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
│ │ │ │ +
gtsam::SymmetricBlockMatrix::block
Matrix block(DenseIndex I, DenseIndex J) const
Get a copy of a block (anywhere in the matrix).
Definition SymmetricBlockMatrix.cpp:60
│ │ │ │ +
gtsam::SymmetricBlockMatrix::selfadjointView
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j).
Definition SymmetricBlockMatrix.h:156
│ │ │ │
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
gtsam::GaussianBayesTree
A Bayes tree representing a Gaussian density.
Definition GaussianBayesTree.h:52
│ │ │ │ +
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactor.h:42
│ │ │ │
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ -
gtsam::GaussianFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactorGraph.h:81
│ │ │ │
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ -
gtsam::LevenbergMarquardtOptimizer
This class performs Levenberg-Marquardt nonlinear optimization.
Definition LevenbergMarquardtOptimizer.h:35
│ │ │ │ -
gtsam::LevenbergMarquardtOptimizer::~LevenbergMarquardtOptimizer
~LevenbergMarquardtOptimizer() override
Virtual destructor.
Definition LevenbergMarquardtOptimizer.h:72
│ │ │ │ -
gtsam::LevenbergMarquardtOptimizer::params
const LevenbergMarquardtParams & params() const
Read-only access the parameters.
Definition LevenbergMarquardtOptimizer.h:104
│ │ │ │ -
gtsam::LevenbergMarquardtOptimizer::_params
const NonlinearOptimizerParams & _params() const override
Access the parameters (base class version)
Definition LevenbergMarquardtOptimizer.h:125
│ │ │ │ -
gtsam::LevenbergMarquardtOptimizer::params_
const LevenbergMarquardtParams params_
LM parameters.
Definition LevenbergMarquardtOptimizer.h:38
│ │ │ │ -
gtsam::LevenbergMarquardtOptimizer::print
void print(const std::string &str="") const
print
Definition LevenbergMarquardtOptimizer.h:87
│ │ │ │ -
gtsam::LevenbergMarquardtParams
Parameters for Levenberg-Marquardt optimization.
Definition LevenbergMarquardtParams.h:35
│ │ │ │ +
gtsam::Marginals
A class for computing Gaussian marginals of variables in a NonlinearFactorGraph.
Definition Marginals.h:32
│ │ │ │ +
gtsam::Marginals::Factorization
Factorization
The linear factorization mode - either CHOLESKY (faster and suitable for most problems) or QR (slower...
Definition Marginals.h:37
│ │ │ │ +
gtsam::Marginals::Marginals
Marginals()
Default constructor only for wrappers.
Definition Marginals.h:52
│ │ │ │ +
gtsam::JointMarginal
A class to store and access a joint marginal, returned from Marginals::jointMarginalCovariance and Ma...
Definition Marginals.h:153
│ │ │ │ +
gtsam::JointMarginal::at
Matrix at(Key iVariable, Key jVariable) const
Synonym for operator()
Definition Marginals.h:184
│ │ │ │ +
gtsam::JointMarginal::fullMatrix
Matrix fullMatrix() const
The full, dense covariance/information matrix of the joint marginal.
Definition Marginals.h:189
│ │ │ │ +
gtsam::JointMarginal::operator()
Matrix operator()(Key iVariable, Key jVariable) const
Access a block, corresponding to a pair of variables, of the joint marginal.
Definition Marginals.h:177
│ │ │ │ +
gtsam::JointMarginal::JointMarginal
JointMarginal()
Default constructor only for wrappers.
Definition Marginals.h:162
│ │ │ │
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │ -
gtsam::NonlinearOptimizer
This is the abstract interface for classes that can optimize for the maximum-likelihood estimate of a...
Definition NonlinearOptimizer.h:75
│ │ │ │ -
gtsam::NonlinearOptimizerParams
The common parameters for Nonlinear optimizers.
Definition NonlinearOptimizerParams.h:34
│ │ │ │
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
VectorValues
The Factor::error simply extracts the.
│ │ │ │ +
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,157 +1,260 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -LevenbergMarquardtOptimizer.h │ │ │ │ │ +Marginals.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -21#pragma once │ │ │ │ │ -22 │ │ │ │ │ -23#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ -26#include │ │ │ │ │ -27 │ │ │ │ │ -28class NonlinearOptimizerMoreOptimizationTest; │ │ │ │ │ -29 │ │ │ │ │ -30namespace _g_t_s_a_m { │ │ │ │ │ -31 │ │ │ │ │ -_3_5class GTSAM_EXPORT _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r: public _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r { │ │ │ │ │ -36 │ │ │ │ │ -37protected: │ │ │ │ │ -_3_8 const _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s _p_a_r_a_m_s__; │ │ │ │ │ -39 boost::posix_time::ptime startTime_; │ │ │ │ │ -40 │ │ │ │ │ -41 void initTime(); │ │ │ │ │ -42 │ │ │ │ │ -43public: │ │ │ │ │ -44 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -45 │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_V_a_l_u_e_s_._h> │ │ │ │ │ +24 │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ +26 │ │ │ │ │ +27class JointMarginal; │ │ │ │ │ +28 │ │ │ │ │ +_3_2class GTSAM_EXPORT _M_a_r_g_i_n_a_l_s { │ │ │ │ │ +33 │ │ │ │ │ +34public: │ │ │ │ │ +35 │ │ │ │ │ +_3_7 enum _F_a_c_t_o_r_i_z_a_t_i_o_n { │ │ │ │ │ +38 CHOLESKY, │ │ │ │ │ +39 QR │ │ │ │ │ +40 }; │ │ │ │ │ +41 │ │ │ │ │ +42protected: │ │ │ │ │ +43 │ │ │ │ │ +44 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h graph_; │ │ │ │ │ +45 _V_a_l_u_e_s values_; │ │ │ │ │ +46 Factorization factorization_; │ │ │ │ │ +47 _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e bayesTree_; │ │ │ │ │ 48 │ │ │ │ │ -57 _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, const _V_a_l_u_e_s& │ │ │ │ │ -initialValues, │ │ │ │ │ -58 const _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s& params = _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s()); │ │ │ │ │ -59 │ │ │ │ │ -67 _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, const _V_a_l_u_e_s& │ │ │ │ │ -initialValues, │ │ │ │ │ -68 const _O_r_d_e_r_i_n_g& ordering, │ │ │ │ │ -69 const _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s& params = _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s()); │ │ │ │ │ -70 │ │ │ │ │ -_7_2 _~_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r() override { │ │ │ │ │ -73 } │ │ │ │ │ -74 │ │ │ │ │ +49public: │ │ │ │ │ +50 │ │ │ │ │ +_5_2 _M_a_r_g_i_n_a_l_s(){} │ │ │ │ │ +53 │ │ │ │ │ +59 _M_a_r_g_i_n_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, const _V_a_l_u_e_s& solution, │ │ │ │ │ +Factorization factorization = CHOLESKY); │ │ │ │ │ +60 │ │ │ │ │ +67 _M_a_r_g_i_n_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, const _V_a_l_u_e_s& solution, const │ │ │ │ │ +_O_r_d_e_r_i_n_g& ordering, │ │ │ │ │ +68 Factorization factorization = CHOLESKY); │ │ │ │ │ +69 │ │ │ │ │ +75 _M_a_r_g_i_n_a_l_s(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& graph, const _V_a_l_u_e_s& solution, │ │ │ │ │ +Factorization factorization = CHOLESKY); │ │ │ │ │ 76 │ │ │ │ │ -79 │ │ │ │ │ -81 double lambda() const; │ │ │ │ │ -82 │ │ │ │ │ -84 int getInnerIterations() const; │ │ │ │ │ +83 _M_a_r_g_i_n_a_l_s(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& graph, const _V_a_l_u_e_s& solution, const │ │ │ │ │ +_O_r_d_e_r_i_n_g& ordering, │ │ │ │ │ +84 Factorization factorization = CHOLESKY); │ │ │ │ │ 85 │ │ │ │ │ -_8_7 void _p_r_i_n_t(const std::string& str = "") const { │ │ │ │ │ -88 std::cout << str << "LevenbergMarquardtOptimizer" << std::endl; │ │ │ │ │ -89 this->params_.print(" parameters:\n"); │ │ │ │ │ -90 } │ │ │ │ │ -91 │ │ │ │ │ +92 _M_a_r_g_i_n_a_l_s(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& graph, const _V_e_c_t_o_r_V_a_l_u_e_s& solution, │ │ │ │ │ +Factorization factorization = CHOLESKY); │ │ │ │ │ 93 │ │ │ │ │ -96 │ │ │ │ │ -101 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r iterate() override; │ │ │ │ │ +100 _M_a_r_g_i_n_a_l_s(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& graph, const _V_e_c_t_o_r_V_a_l_u_e_s& solution, │ │ │ │ │ +const _O_r_d_e_r_i_n_g& ordering, │ │ │ │ │ +101 Factorization factorization = CHOLESKY); │ │ │ │ │ 102 │ │ │ │ │ -_1_0_4 const _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s& _p_a_r_a_m_s() const { │ │ │ │ │ -105 return params_; │ │ │ │ │ -106 } │ │ │ │ │ -107 │ │ │ │ │ -108 void writeLogFile(double currentError); │ │ │ │ │ -109 │ │ │ │ │ -111 virtual _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r linearize() const; │ │ │ │ │ +104 void _p_r_i_n_t(const std::string& str = "Marginals: ", const _K_e_y_F_o_r_m_a_t_t_e_r& │ │ │ │ │ +keyFormatter = DefaultKeyFormatter) const; │ │ │ │ │ +105 │ │ │ │ │ +107 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r marginalFactor(_K_e_y variable) const; │ │ │ │ │ +108 │ │ │ │ │ +111 Matrix marginalInformation(_K_e_y variable) const; │ │ │ │ │ 112 │ │ │ │ │ -114 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h buildDampedSystem(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& linear, │ │ │ │ │ -115 const _V_e_c_t_o_r_V_a_l_u_e_s& sqrtHessianDiagonal) const; │ │ │ │ │ -116 │ │ │ │ │ -118 bool tryLambda(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& linear, const _V_e_c_t_o_r_V_a_l_u_e_s& │ │ │ │ │ -sqrtHessianDiagonal); │ │ │ │ │ -119 │ │ │ │ │ +114 Matrix marginalCovariance(_K_e_y variable) const; │ │ │ │ │ +115 │ │ │ │ │ +117 _J_o_i_n_t_M_a_r_g_i_n_a_l jointMarginalCovariance(const _K_e_y_V_e_c_t_o_r& variables) const; │ │ │ │ │ +118 │ │ │ │ │ +120 _J_o_i_n_t_M_a_r_g_i_n_a_l jointMarginalInformation(const _K_e_y_V_e_c_t_o_r& variables) const; │ │ │ │ │ 121 │ │ │ │ │ -122protected: │ │ │ │ │ -123 │ │ │ │ │ -_1_2_5 const _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s& ___p_a_r_a_m_s() const override { │ │ │ │ │ -126 return params_; │ │ │ │ │ -127 } │ │ │ │ │ -128}; │ │ │ │ │ +123 _V_e_c_t_o_r_V_a_l_u_e_s _o_p_t_i_m_i_z_e() const; │ │ │ │ │ +124 │ │ │ │ │ +125protected: │ │ │ │ │ +126 │ │ │ │ │ +128 void computeBayesTree(); │ │ │ │ │ 129 │ │ │ │ │ -130} │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ -Factor Graph Values. │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ -Base class and parameters for nonlinear optimization algorithms. │ │ │ │ │ -_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_._h │ │ │ │ │ -Parameters for Levenberg-Marquardt trust-region scheme. │ │ │ │ │ +131 void computeBayesTree(const _O_r_d_e_r_i_n_g& ordering); │ │ │ │ │ +132 │ │ │ │ │ +133public: │ │ │ │ │ +134#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ +136 GTSAM_DEPRECATED _M_a_r_g_i_n_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, const _V_a_l_u_e_s& │ │ │ │ │ +solution, Factorization factorization, │ │ │ │ │ +137 const _O_r_d_e_r_i_n_g& ordering) : _M_a_r_g_i_n_a_l_s(graph, solution, ordering, │ │ │ │ │ +factorization) {} │ │ │ │ │ +138 │ │ │ │ │ +140 GTSAM_DEPRECATED Marginals(const GaussianFactorGraph& graph, const _V_a_l_u_e_s& │ │ │ │ │ +solution, Factorization factorization, │ │ │ │ │ +141 const Ordering& ordering) : Marginals(graph, solution, ordering, │ │ │ │ │ +factorization) {} │ │ │ │ │ +142 │ │ │ │ │ +144 GTSAM_DEPRECATED Marginals(const GaussianFactorGraph& graph, const │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s& solution, Factorization factorization, │ │ │ │ │ +145 const Ordering& ordering) : Marginals(graph, solution, ordering, │ │ │ │ │ +factorization) {} │ │ │ │ │ +146#endif │ │ │ │ │ +147 │ │ │ │ │ +148}; │ │ │ │ │ +149 │ │ │ │ │ +_1_5_3class GTSAM_EXPORT _J_o_i_n_t_M_a_r_g_i_n_a_l { │ │ │ │ │ +154 │ │ │ │ │ +155protected: │ │ │ │ │ +156 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x blockMatrix_; │ │ │ │ │ +157 _K_e_y_V_e_c_t_o_r keys_; │ │ │ │ │ +158 _F_a_s_t_M_a_p_<_K_e_y_,_ _s_i_z_e___t_> indices_; │ │ │ │ │ +159 │ │ │ │ │ +160public: │ │ │ │ │ +_1_6_2 _J_o_i_n_t_M_a_r_g_i_n_a_l() {} │ │ │ │ │ +163 │ │ │ │ │ +_1_7_7 Matrix _o_p_e_r_a_t_o_r_(_)(_K_e_y iVariable, _K_e_y jVariable) const { │ │ │ │ │ +178 const auto indexI = indices_.at(iVariable); │ │ │ │ │ +179 const auto indexJ = indices_.at(jVariable); │ │ │ │ │ +180 return blockMatrix_._b_l_o_c_k(indexI, indexJ); │ │ │ │ │ +181 } │ │ │ │ │ +182 │ │ │ │ │ +_1_8_4 Matrix _a_t(_K_e_y iVariable, _K_e_y jVariable) const { │ │ │ │ │ +185 return (*this)(iVariable, jVariable); │ │ │ │ │ +186 } │ │ │ │ │ +187 │ │ │ │ │ +_1_8_9 Matrix _f_u_l_l_M_a_t_r_i_x() const { │ │ │ │ │ +190 return blockMatrix_._s_e_l_f_a_d_j_o_i_n_t_V_i_e_w(); │ │ │ │ │ +191 } │ │ │ │ │ +192 │ │ │ │ │ +194 void _p_r_i_n_t(const std::string& s = "", const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = │ │ │ │ │ +DefaultKeyFormatter) const; │ │ │ │ │ +195 │ │ │ │ │ +196protected: │ │ │ │ │ +197 _J_o_i_n_t_M_a_r_g_i_n_a_l(const Matrix& fullMatrix, const std::vector& dims, │ │ │ │ │ +const _K_e_y_V_e_c_t_o_r& keys) : │ │ │ │ │ +198 blockMatrix_(dims, fullMatrix), keys_(keys), indices_(_O_r_d_e_r_i_n_g(keys).invert │ │ │ │ │ +()) {} │ │ │ │ │ +199 │ │ │ │ │ +200 friend class Marginals; │ │ │ │ │ +201 │ │ │ │ │ +202}; │ │ │ │ │ +203 │ │ │ │ │ +204} /* namespace gtsam */ │ │ │ │ │ +_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ +Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ │ +_V_a_l_u_e_s_._h │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Factor Graph consisting of non-linear factors. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ +Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key │ │ │ │ │ +landmarkKey) │ │ │ │ │ +Optimize for triangulation. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.cpp:155 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ +FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ +fast_pool_allocator instead of the defa... │ │ │ │ │ +DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ +of blocks. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_b_l_o_c_k │ │ │ │ │ +Matrix block(DenseIndex I, DenseIndex J) const │ │ │ │ │ +Get a copy of a block (anywhere in the matrix). │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.cpp:60 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_s_e_l_f_a_d_j_o_i_n_t_V_i_e_w │ │ │ │ │ +Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex │ │ │ │ │ +I, DenseIndex J) const │ │ │ │ │ +Return the square sub-matrix that contains blocks(i:j, i:j). │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:156 │ │ │ │ │ _g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e │ │ │ │ │ +A Bayes tree representing a Gaussian density. │ │ │ │ │ +DDeeffiinniittiioonn GaussianBayesTree.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactor.h:42 │ │ │ │ │ _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:81 │ │ │ │ │ _g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ with a unique integer... │ │ │ │ │ DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r │ │ │ │ │ -This class performs Levenberg-Marquardt nonlinear optimization. │ │ │ │ │ -DDeeffiinniittiioonn LevenbergMarquardtOptimizer.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r_:_:_~_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r │ │ │ │ │ -~LevenbergMarquardtOptimizer() override │ │ │ │ │ -Virtual destructor. │ │ │ │ │ -DDeeffiinniittiioonn LevenbergMarquardtOptimizer.h:72 │ │ │ │ │ -_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r_:_:_p_a_r_a_m_s │ │ │ │ │ -const LevenbergMarquardtParams & params() const │ │ │ │ │ -Read-only access the parameters. │ │ │ │ │ -DDeeffiinniittiioonn LevenbergMarquardtOptimizer.h:104 │ │ │ │ │ -_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r_:_:___p_a_r_a_m_s │ │ │ │ │ -const NonlinearOptimizerParams & _params() const override │ │ │ │ │ -Access the parameters (base class version) │ │ │ │ │ -DDeeffiinniittiioonn LevenbergMarquardtOptimizer.h:125 │ │ │ │ │ -_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r_:_:_p_a_r_a_m_s__ │ │ │ │ │ -const LevenbergMarquardtParams params_ │ │ │ │ │ -LM parameters. │ │ │ │ │ -DDeeffiinniittiioonn LevenbergMarquardtOptimizer.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &str="") const │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn LevenbergMarquardtOptimizer.h:87 │ │ │ │ │ -_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s │ │ │ │ │ -Parameters for Levenberg-Marquardt optimization. │ │ │ │ │ -DDeeffiinniittiioonn LevenbergMarquardtParams.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_r_g_i_n_a_l_s │ │ │ │ │ +A class for computing Gaussian marginals of variables in a │ │ │ │ │ +NonlinearFactorGraph. │ │ │ │ │ +DDeeffiinniittiioonn Marginals.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_r_g_i_n_a_l_s_:_:_F_a_c_t_o_r_i_z_a_t_i_o_n │ │ │ │ │ +Factorization │ │ │ │ │ +The linear factorization mode - either CHOLESKY (faster and suitable for most │ │ │ │ │ +problems) or QR (slower... │ │ │ │ │ +DDeeffiinniittiioonn Marginals.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_r_g_i_n_a_l_s_:_:_M_a_r_g_i_n_a_l_s │ │ │ │ │ +Marginals() │ │ │ │ │ +Default constructor only for wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Marginals.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_J_o_i_n_t_M_a_r_g_i_n_a_l │ │ │ │ │ +A class to store and access a joint marginal, returned from Marginals:: │ │ │ │ │ +jointMarginalCovariance and Ma... │ │ │ │ │ +DDeeffiinniittiioonn Marginals.h:153 │ │ │ │ │ +_g_t_s_a_m_:_:_J_o_i_n_t_M_a_r_g_i_n_a_l_:_:_a_t │ │ │ │ │ +Matrix at(Key iVariable, Key jVariable) const │ │ │ │ │ +Synonym for operator() │ │ │ │ │ +DDeeffiinniittiioonn Marginals.h:184 │ │ │ │ │ +_g_t_s_a_m_:_:_J_o_i_n_t_M_a_r_g_i_n_a_l_:_:_f_u_l_l_M_a_t_r_i_x │ │ │ │ │ +Matrix fullMatrix() const │ │ │ │ │ +The full, dense covariance/information matrix of the joint marginal. │ │ │ │ │ +DDeeffiinniittiioonn Marginals.h:189 │ │ │ │ │ +_g_t_s_a_m_:_:_J_o_i_n_t_M_a_r_g_i_n_a_l_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ +Matrix operator()(Key iVariable, Key jVariable) const │ │ │ │ │ +Access a block, corresponding to a pair of variables, of the joint marginal. │ │ │ │ │ +DDeeffiinniittiioonn Marginals.h:177 │ │ │ │ │ +_g_t_s_a_m_:_:_J_o_i_n_t_M_a_r_g_i_n_a_l_:_:_J_o_i_n_t_M_a_r_g_i_n_a_l │ │ │ │ │ +JointMarginal() │ │ │ │ │ +Default constructor only for wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Marginals.h:162 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r │ │ │ │ │ -This is the abstract interface for classes that can optimize for the maximum- │ │ │ │ │ -likelihood estimate of a... │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizer.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s │ │ │ │ │ -The common parameters for Nonlinear optimizers. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizerParams.h:34 │ │ │ │ │ _g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +The Factor::error simply extracts the. │ │ │ │ │ +_V_a_l_u_e_s │ │ │ │ │ +In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ +a non-linear function... │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ + * _M_a_r_g_i_n_a_l_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01088.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/WhiteNoiseFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearOptimizerParams.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,55 +94,42 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Variables
│ │ │ │ -
WhiteNoiseFactor.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
NonlinearOptimizerParams.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Binary white noise factor. │ │ │ │ +

Parameters for nonlinear optimization. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::WhiteNoiseFactor
 Binary factor to estimate parameters of zero-mean Gaussian white noise. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Variables

│ │ │ │ -const double gtsam::logSqrt2PI = log(std::sqrt(2.0 * M_PI))
 constant needed below
 
│ │ │ │

Detailed Description

│ │ │ │ -

Binary white noise factor.

│ │ │ │ -
Author
Chris Beall
│ │ │ │ +

Parameters for nonlinear optimization.

│ │ │ │ +
Date
Jul 24, 2012
│ │ │ │ +
Author
Yong-Dian Jian
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │
│ │ │ │ -Frank Dellaert
│ │ │ │ -
Date
September 2011
│ │ │ │ +Andrew Melim
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,33 +1,25 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _V_a_r_i_a_b_l_e_s │ │ │ │ │ -WhiteNoiseFactor.h File Reference │ │ │ │ │ -Binary white noise factor. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r │ │ │ │ │ -  Binary factor to estimate parameters of zero-mean Gaussian white noise. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +NonlinearOptimizerParams.cpp File Reference │ │ │ │ │ +Parameters for nonlinear optimization. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -VVaarriiaabblleess │ │ │ │ │ -const double  ggttssaamm::::llooggSSqqrrtt22PPII = log(std::sqrt(2.0 * M_PI)) │ │ │ │ │ -  constant needed below │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Binary white noise factor. │ │ │ │ │ +Parameters for nonlinear optimization. │ │ │ │ │ + Date │ │ │ │ │ + Jul 24, 2012 │ │ │ │ │ Author │ │ │ │ │ - Chris Beall │ │ │ │ │ + Yong-Dian Jian │ │ │ │ │ + Richard Roberts │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - September 2011 │ │ │ │ │ + Andrew Melim │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r_._h │ │ │ │ │ + * _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01091.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,40 +94,58 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces
│ │ │ │ -
LevenbergMarquardtOptimizer.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
NonlinearConjugateGradientOptimizer.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme. │ │ │ │ +

Simple non-linear optimizer that solves using non-preconditioned CG. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::NonlinearConjugateGradientOptimizer
 An implementation of the nonlinear CG method using the template below. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +template<class S , class V , class W >
double gtsam::lineSearch (const S &system, const V currentValues, const W &gradient)
 Implement the golden-section line search algorithm.
 
template<class S , class V >
boost::tuple< V, int > gtsam::nonlinearConjugateGradient (const S &system, const V &initial, const NonlinearOptimizerParams &params, const bool singleIteration, const bool gradientDescent=false)
 Implement the nonlinear conjugate gradient method using the Polak-Ribiere formula suggested in http://en.wikipedia.org/wiki/Nonlinear_conjugate_gradient_method.
 
│ │ │ │

Detailed Description

│ │ │ │ -

A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ -
│ │ │ │ -Luca Carlone
│ │ │ │ -
Date
Feb 26, 2012
│ │ │ │ +

Simple non-linear optimizer that solves using non-preconditioned CG.

│ │ │ │ +
Author
Yong-Dian Jian
│ │ │ │ +
Date
June 11, 2012
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,25 +1,44 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -LevenbergMarquardtOptimizer.cpp File Reference │ │ │ │ │ -A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme. │ │ │ │ │ -_M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +NonlinearConjugateGradientOptimizer.h File Reference │ │ │ │ │ +Simple non-linear optimizer that solves using nnoonn--pprreeccoonnddiittiioonneedd CG. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_O_p_t_i_m_i_z_e_r │ │ │ │ │ +  An implementation of the nonlinear CG method using the template below. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +template │ │ │ │ │ + double  ggttssaamm::::lliinneeSSeeaarrcchh (const S &system, const V │ │ │ │ │ + currentValues, const W &gradient) │ │ │ │ │ +  Implement the golden-section line search algorithm. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +boost::tuple< V, int >  _g_t_s_a_m_:_:_n_o_n_l_i_n_e_a_r_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t (const S &system, │ │ │ │ │ + const V &initial, const _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s │ │ │ │ │ + ¶ms, const bool singleIteration, const bool │ │ │ │ │ + gradientDescent=false) │ │ │ │ │ + Implement the nonlinear conjugate gradient method using │ │ │ │ │ +  the Polak-Ribiere formula suggested in _h_t_t_p_:_/_/ │ │ │ │ │ + _e_n_._w_i_k_i_p_e_d_i_a_._o_r_g_/_w_i_k_i_/ │ │ │ │ │ + _N_o_n_l_i_n_e_a_r___c_o_n_j_u_g_a_t_e___g_r_a_d_i_e_n_t___m_e_t_h_o_d. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme. │ │ │ │ │ +Simple non-linear optimizer that solves using nnoonn--pprreeccoonnddiittiioonneedd CG. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Luca Carlone │ │ │ │ │ + Yong-Dian Jian │ │ │ │ │ Date │ │ │ │ │ - Feb 26, 2012 │ │ │ │ │ + June 11, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r_._c_p_p │ │ │ │ │ + * _N_o_n_l_i_n_e_a_r_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01094.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearFactorGraph.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearFactorGraph.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,50 +94,39 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
NonlinearFactorGraph.h File Reference
│ │ │ │ +
NonlinearFactorGraph.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Factor Graph consisting of non-linear factors. │ │ │ │ +

Factor Graph Consisting of non-linear factors. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::NonlinearFactorGraph
 
struct  gtsam::traits< NonlinearFactorGraph >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Factor Graph consisting of non-linear factors.

│ │ │ │ +

Factor Graph Consisting of non-linear factors.

│ │ │ │
Author
Frank Dellaert
│ │ │ │
│ │ │ │ Carlos Nieto
│ │ │ │
│ │ │ │ Christian Potthast
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,29 +1,22 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -NonlinearFactorGraph.h File Reference │ │ │ │ │ -Factor Graph consisting of non-linear factors. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +NonlinearFactorGraph.cpp File Reference │ │ │ │ │ +Factor Graph Consisting of non-linear factors. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Factor Graph consisting of non-linear factors. │ │ │ │ │ +Factor Graph Consisting of non-linear factors. │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ Carlos Nieto │ │ │ │ │ Christian Potthast │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ + * _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01100.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/nonlinearExceptions.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearISAM.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,43 +96,39 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
nonlinearExceptions.h File Reference
│ │ │ │ +
NonlinearISAM.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Exceptions that may be thrown by nonlinear optimization components. │ │ │ │ -More...

│ │ │ │ - │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::MarginalizeNonleafException
 Thrown when requesting to marginalize out variables from ISAM2 that are not leaves. More...
class  gtsam::NonlinearISAM
 Wrapper class to manage ISAM in a nonlinear context. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Exceptions that may be thrown by nonlinear optimization components.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Aug 17, 2012
│ │ │ │ +
Date
Jan 19, 2010
│ │ │ │ +
Author
Viorela Ila and Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,25 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -nonlinearExceptions.h File Reference │ │ │ │ │ -Exceptions that may be thrown by nonlinear optimization components. _M_o_r_e_._._. │ │ │ │ │ +NonlinearISAM.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_M_a_r_g_i_n_a_l_i_z_e_N_o_n_l_e_a_f_E_x_c_e_p_t_i_o_n │ │ │ │ │ -  Thrown when requesting to marginalize out variables from _I_S_A_M_2 that are │ │ │ │ │ - not leaves. _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_I_S_A_M │ │ │ │ │ +  Wrapper class to manage _I_S_A_M in a nonlinear context. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Exceptions that may be thrown by nonlinear optimization components. │ │ │ │ │ - Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ Date │ │ │ │ │ - Aug 17, 2012 │ │ │ │ │ + Jan 19, 2010 │ │ │ │ │ + Author │ │ │ │ │ + Viorela Ila and Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _n_o_n_l_i_n_e_a_r_E_x_c_e_p_t_i_o_n_s_._h │ │ │ │ │ + * _N_o_n_l_i_n_e_a_r_I_S_A_M_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01100.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a01100 = [ │ │ │ │ │ - ["gtsam::MarginalizeNonleafException", "a04492.html", null] │ │ │ │ │ + ["gtsam::NonlinearISAM", "a04548.html", "a04548"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01100_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/nonlinearExceptions.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearISAM.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,15 +98,15 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
nonlinearExceptions.h
│ │ │ │ +
NonlinearISAM.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │ @@ -114,53 +114,102 @@ │ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
18#pragma once
│ │ │ │
19
│ │ │ │ -
20#include <boost/lexical_cast.hpp>
│ │ │ │ -
21#include <exception>
│ │ │ │ +
20#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ +
21#include <gtsam/linear/GaussianISAM.h>
│ │ │ │
22
│ │ │ │ -
23#include <gtsam/inference/Key.h>
│ │ │ │ -
24
│ │ │ │ -
25namespace gtsam {
│ │ │ │ -
26
│ │ │ │ -
│ │ │ │ -
33 class MarginalizeNonleafException : public std::exception {
│ │ │ │ -
34 Key key_;
│ │ │ │ -
35 KeyFormatter formatter_;
│ │ │ │ -
36 mutable std::string what_;
│ │ │ │ -
37 public:
│ │ │ │ -
38 MarginalizeNonleafException(Key key, KeyFormatter formatter = DefaultKeyFormatter) noexcept :
│ │ │ │ -
39 key_(key), formatter_(formatter) {}
│ │ │ │ -
40 virtual ~MarginalizeNonleafException() noexcept {}
│ │ │ │ -
41 Key key() const { return key_; }
│ │ │ │ -
42 const char* what() const noexcept override {
│ │ │ │ -
43 if(what_.empty())
│ │ │ │ -
44 what_ =
│ │ │ │ -
45"\nRequested to marginalize out variable " + formatter_(key_) + ", but this variable\n\
│ │ │ │ -
46is not a leaf. To make the variables you would like to marginalize be leaves,\n\
│ │ │ │ -
47their ordering should be constrained using the constrainedKeys argument to\n\
│ │ │ │ -
48ISAM2::update().\n";
│ │ │ │ -
49 return what_.c_str();
│ │ │ │ -
50 }
│ │ │ │ -
51 };
│ │ │ │ -
│ │ │ │ -
52
│ │ │ │ -
53}
│ │ │ │ - │ │ │ │ +
23namespace gtsam {
│ │ │ │ +
│ │ │ │ +
27class GTSAM_EXPORT NonlinearISAM {
│ │ │ │ +
28protected:
│ │ │ │ +
29
│ │ │ │ +
31 gtsam::GaussianISAM isam_;
│ │ │ │ +
32
│ │ │ │ +
34 Values linPoint_;
│ │ │ │ +
35
│ │ │ │ +
37 NonlinearFactorGraph factors_;
│ │ │ │ +
38
│ │ │ │ +
40 int reorderInterval_;
│ │ │ │ +
41 int reorderCounter_;
│ │ │ │ +
42
│ │ │ │ +
44 GaussianFactorGraph::Eliminate eliminationFunction_;
│ │ │ │ +
45
│ │ │ │ +
46public:
│ │ │ │ +
47
│ │ │ │ +
50
│ │ │ │ +
│ │ │ │ +
58 NonlinearISAM(int reorderInterval = 1,
│ │ │ │ +
59 const GaussianFactorGraph::Eliminate& eliminationFunction = GaussianFactorGraph::EliminationTraitsType::DefaultEliminate) :
│ │ │ │ +
60 reorderInterval_(reorderInterval), reorderCounter_(0), eliminationFunction_(eliminationFunction) {}
│ │ │ │ +
│ │ │ │ +
61
│ │ │ │ +
65
│ │ │ │ +
67 Values estimate() const;
│ │ │ │ +
68
│ │ │ │ +
70 Matrix marginalCovariance(Key key) const;
│ │ │ │ +
71
│ │ │ │ +
72 // access
│ │ │ │ +
73
│ │ │ │ +
75 const GaussianISAM& bayesTree() const { return isam_; }
│ │ │ │ +
76
│ │ │ │ +
78 const Values& getLinearizationPoint() const { return linPoint_; }
│ │ │ │ +
79
│ │ │ │ +
81 const NonlinearFactorGraph& getFactorsUnsafe() const { return factors_; }
│ │ │ │ +
82
│ │ │ │ +
84 int reorderInterval() const { return reorderInterval_; }
│ │ │ │ +
│ │ │ │ +
85 int reorderCounter() const { return reorderCounter_; }
│ │ │ │ +
86
│ │ │ │ +
88 void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ +
89
│ │ │ │ +
91 void printStats() const;
│ │ │ │ +
92
│ │ │ │ +
94 void saveGraph(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ +
95
│ │ │ │ +
99
│ │ │ │ +
101 void update(const NonlinearFactorGraph& newFactors, const Values& initialValues);
│ │ │ │ +
102
│ │ │ │ +
104 void reorder_relinearize();
│ │ │ │ +
105
│ │ │ │ +
107
│ │ │ │ +
108};
│ │ │ │ +
109
│ │ │ │ +
110} // \namespace gtsam
│ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::MarginalizeNonleafException
Thrown when requesting to marginalize out variables from ISAM2 that are not leaves.
Definition nonlinearExceptions.h:33
│ │ │ │ +
gtsam::EliminateableFactorGraph< GaussianFactorGraph >::Eliminate
std::function< EliminationResult(const FactorGraphType &, const Ordering &)> Eliminate
The function type that does a single dense elimination step on a subgraph.
Definition EliminateableFactorGraph.h:89
│ │ │ │ +
gtsam::GaussianISAM
Definition GaussianISAM.h:28
│ │ │ │ +
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │ +
gtsam::NonlinearISAM
Wrapper class to manage ISAM in a nonlinear context.
Definition NonlinearISAM.h:27
│ │ │ │ +
gtsam::NonlinearISAM::eliminationFunction_
GaussianFactorGraph::Eliminate eliminationFunction_
The elimination function.
Definition NonlinearISAM.h:44
│ │ │ │ +
gtsam::NonlinearISAM::factors_
NonlinearFactorGraph factors_
The original factors, used when relinearizing.
Definition NonlinearISAM.h:37
│ │ │ │ +
gtsam::NonlinearISAM::bayesTree
const GaussianISAM & bayesTree() const
access the underlying bayes tree
Definition NonlinearISAM.h:75
│ │ │ │ +
gtsam::NonlinearISAM::reorderInterval
int reorderInterval() const
get counters
Definition NonlinearISAM.h:84
│ │ │ │ +
gtsam::NonlinearISAM::NonlinearISAM
NonlinearISAM(int reorderInterval=1, const GaussianFactorGraph::Eliminate &eliminationFunction=GaussianFactorGraph::EliminationTraitsType::DefaultEliminate)
Periodically reorder and relinearize.
Definition NonlinearISAM.h:58
│ │ │ │ +
gtsam::NonlinearISAM::reorderCounter
int reorderCounter() const
TODO: comment.
Definition NonlinearISAM.h:85
│ │ │ │ +
gtsam::NonlinearISAM::getFactorsUnsafe
const NonlinearFactorGraph & getFactorsUnsafe() const
get underlying nonlinear graph
Definition NonlinearISAM.h:81
│ │ │ │ +
gtsam::NonlinearISAM::reorderInterval_
int reorderInterval_
The reordering interval and counter.
Definition NonlinearISAM.h:40
│ │ │ │ +
gtsam::NonlinearISAM::linPoint_
Values linPoint_
The current linearization point.
Definition NonlinearISAM.h:34
│ │ │ │ +
gtsam::NonlinearISAM::isam_
gtsam::GaussianISAM isam_
The internal iSAM object.
Definition NonlinearISAM.h:31
│ │ │ │ +
gtsam::NonlinearISAM::getLinearizationPoint
const Values & getLinearizationPoint() const
Return the current linearization point.
Definition NonlinearISAM.h:78
│ │ │ │ +
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -nonlinearExceptions.h │ │ │ │ │ +NonlinearISAM.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ @@ -16,58 +16,151 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 18#pragma once │ │ │ │ │ 19 │ │ │ │ │ -20#include │ │ │ │ │ -21#include │ │ │ │ │ +20#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_I_S_A_M_._h> │ │ │ │ │ 22 │ │ │ │ │ -23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ -24 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ -26 │ │ │ │ │ -_3_3 class _M_a_r_g_i_n_a_l_i_z_e_N_o_n_l_e_a_f_E_x_c_e_p_t_i_o_n : public std::exception { │ │ │ │ │ -34 _K_e_y key_; │ │ │ │ │ -35 _K_e_y_F_o_r_m_a_t_t_e_r formatter_; │ │ │ │ │ -36 mutable std::string what_; │ │ │ │ │ -37 public: │ │ │ │ │ -38 _M_a_r_g_i_n_a_l_i_z_e_N_o_n_l_e_a_f_E_x_c_e_p_t_i_o_n(_K_e_y key, _K_e_y_F_o_r_m_a_t_t_e_r formatter = │ │ │ │ │ -DefaultKeyFormatter) noexcept : │ │ │ │ │ -39 key_(key), formatter_(formatter) {} │ │ │ │ │ -40 virtual _~_M_a_r_g_i_n_a_l_i_z_e_N_o_n_l_e_a_f_E_x_c_e_p_t_i_o_n() noexcept {} │ │ │ │ │ -41 _K_e_y key() const { return key_; } │ │ │ │ │ -42 const char* what() const noexcept override { │ │ │ │ │ -43 if(what_.empty()) │ │ │ │ │ -44 what_ = │ │ │ │ │ -45"\nRequested to marginalize out variable " + formatter_(key_) + ", but this │ │ │ │ │ -variable\n\ │ │ │ │ │ -46is not a leaf. To make the variables you would like to marginalize be │ │ │ │ │ -leaves,\n\ │ │ │ │ │ -47their ordering should be constrained using the constrainedKeys argument to\n\ │ │ │ │ │ -48ISAM2::update().\n"; │ │ │ │ │ -49 return what_.c_str(); │ │ │ │ │ -50 } │ │ │ │ │ -51 }; │ │ │ │ │ -52 │ │ │ │ │ -53} │ │ │ │ │ -_K_e_y_._h │ │ │ │ │ +23namespace _g_t_s_a_m { │ │ │ │ │ +_2_7class GTSAM_EXPORT _N_o_n_l_i_n_e_a_r_I_S_A_M { │ │ │ │ │ +28protected: │ │ │ │ │ +29 │ │ │ │ │ +_3_1 _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_I_S_A_M _i_s_a_m__; │ │ │ │ │ +32 │ │ │ │ │ +_3_4 _V_a_l_u_e_s _l_i_n_P_o_i_n_t__; │ │ │ │ │ +35 │ │ │ │ │ +_3_7 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h _f_a_c_t_o_r_s__; │ │ │ │ │ +38 │ │ │ │ │ +_4_0 int _r_e_o_r_d_e_r_I_n_t_e_r_v_a_l__; │ │ │ │ │ +41 int reorderCounter_; │ │ │ │ │ +42 │ │ │ │ │ +_4_4 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_E_l_i_m_i_n_a_t_e _e_l_i_m_i_n_a_t_i_o_n_F_u_n_c_t_i_o_n__; │ │ │ │ │ +45 │ │ │ │ │ +46public: │ │ │ │ │ +47 │ │ │ │ │ +50 │ │ │ │ │ +_5_8 _N_o_n_l_i_n_e_a_r_I_S_A_M(int reorderInterval = 1, │ │ │ │ │ +59 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_E_l_i_m_i_n_a_t_e& eliminationFunction = │ │ │ │ │ +GaussianFactorGraph::EliminationTraitsType::DefaultEliminate) : │ │ │ │ │ +60 reorderInterval_(reorderInterval), reorderCounter_(0), eliminationFunction_ │ │ │ │ │ +(eliminationFunction) {} │ │ │ │ │ +61 │ │ │ │ │ +65 │ │ │ │ │ +67 _V_a_l_u_e_s estimate() const; │ │ │ │ │ +68 │ │ │ │ │ +70 Matrix marginalCovariance(_K_e_y key) const; │ │ │ │ │ +71 │ │ │ │ │ +72 // access │ │ │ │ │ +73 │ │ │ │ │ +_7_5 const _G_a_u_s_s_i_a_n_I_S_A_M& _b_a_y_e_s_T_r_e_e() const { return isam_; } │ │ │ │ │ +76 │ │ │ │ │ +_7_8 const _V_a_l_u_e_s& _g_e_t_L_i_n_e_a_r_i_z_a_t_i_o_n_P_o_i_n_t() const { return linPoint_; } │ │ │ │ │ +79 │ │ │ │ │ +_8_1 const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& _g_e_t_F_a_c_t_o_r_s_U_n_s_a_f_e() const { return factors_; } │ │ │ │ │ +82 │ │ │ │ │ +_8_4 int _r_e_o_r_d_e_r_I_n_t_e_r_v_a_l() const { return reorderInterval_; } │ │ │ │ │ +_8_5 int _r_e_o_r_d_e_r_C_o_u_n_t_e_r() const { return reorderCounter_; } │ │ │ │ │ +86 │ │ │ │ │ +88 void _p_r_i_n_t(const std::string& s="", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ +DefaultKeyFormatter) const; │ │ │ │ │ +89 │ │ │ │ │ +91 void printStats() const; │ │ │ │ │ +92 │ │ │ │ │ +94 void saveGraph(const std::string& s, const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ +DefaultKeyFormatter) const; │ │ │ │ │ +95 │ │ │ │ │ +99 │ │ │ │ │ +101 void update(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& newFactors, const _V_a_l_u_e_s& │ │ │ │ │ +initialValues); │ │ │ │ │ +102 │ │ │ │ │ +104 void reorder_relinearize(); │ │ │ │ │ +105 │ │ │ │ │ +107 │ │ │ │ │ +108}; │ │ │ │ │ +109 │ │ │ │ │ +110} // \namespace gtsam │ │ │ │ │ +_G_a_u_s_s_i_a_n_I_S_A_M_._h │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Factor Graph consisting of non-linear factors. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_r_g_i_n_a_l_i_z_e_N_o_n_l_e_a_f_E_x_c_e_p_t_i_o_n │ │ │ │ │ -Thrown when requesting to marginalize out variables from ISAM2 that are not │ │ │ │ │ -leaves. │ │ │ │ │ -DDeeffiinniittiioonn nonlinearExceptions.h:33 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_E_l_i_m_i_n_a_t_e │ │ │ │ │ +std::function< EliminationResult(const FactorGraphType &, const Ordering &)> │ │ │ │ │ +Eliminate │ │ │ │ │ +The function type that does a single dense elimination step on a subgraph. │ │ │ │ │ +DDeeffiinniittiioonn EliminateableFactorGraph.h:89 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_I_S_A_M │ │ │ │ │ +DDeeffiinniittiioonn GaussianISAM.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_I_S_A_M │ │ │ │ │ +Wrapper class to manage ISAM in a nonlinear context. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearISAM.h:27 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_e_l_i_m_i_n_a_t_i_o_n_F_u_n_c_t_i_o_n__ │ │ │ │ │ +GaussianFactorGraph::Eliminate eliminationFunction_ │ │ │ │ │ +The elimination function. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearISAM.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_f_a_c_t_o_r_s__ │ │ │ │ │ +NonlinearFactorGraph factors_ │ │ │ │ │ +The original factors, used when relinearizing. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearISAM.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_b_a_y_e_s_T_r_e_e │ │ │ │ │ +const GaussianISAM & bayesTree() const │ │ │ │ │ +access the underlying bayes tree │ │ │ │ │ +DDeeffiinniittiioonn NonlinearISAM.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_r_e_o_r_d_e_r_I_n_t_e_r_v_a_l │ │ │ │ │ +int reorderInterval() const │ │ │ │ │ +get counters │ │ │ │ │ +DDeeffiinniittiioonn NonlinearISAM.h:84 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_N_o_n_l_i_n_e_a_r_I_S_A_M │ │ │ │ │ +NonlinearISAM(int reorderInterval=1, const GaussianFactorGraph::Eliminate │ │ │ │ │ +&eliminationFunction=GaussianFactorGraph::EliminationTraitsType:: │ │ │ │ │ +DefaultEliminate) │ │ │ │ │ +Periodically reorder and relinearize. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearISAM.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_r_e_o_r_d_e_r_C_o_u_n_t_e_r │ │ │ │ │ +int reorderCounter() const │ │ │ │ │ +TODO: comment. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearISAM.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_g_e_t_F_a_c_t_o_r_s_U_n_s_a_f_e │ │ │ │ │ +const NonlinearFactorGraph & getFactorsUnsafe() const │ │ │ │ │ +get underlying nonlinear graph │ │ │ │ │ +DDeeffiinniittiioonn NonlinearISAM.h:81 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_r_e_o_r_d_e_r_I_n_t_e_r_v_a_l__ │ │ │ │ │ +int reorderInterval_ │ │ │ │ │ +The reordering interval and counter. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearISAM.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_l_i_n_P_o_i_n_t__ │ │ │ │ │ +Values linPoint_ │ │ │ │ │ +The current linearization point. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearISAM.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_i_s_a_m__ │ │ │ │ │ +gtsam::GaussianISAM isam_ │ │ │ │ │ +The internal iSAM object. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearISAM.h:31 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_g_e_t_L_i_n_e_a_r_i_z_a_t_i_o_n_P_o_i_n_t │ │ │ │ │ +const Values & getLinearizationPoint() const │ │ │ │ │ +Return the current linearization point. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearISAM.h:78 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _n_o_n_l_i_n_e_a_r_E_x_c_e_p_t_i_o_n_s_._h │ │ │ │ │ + * _N_o_n_l_i_n_e_a_r_I_S_A_M_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01103.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Clique.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2-impl.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,45 +94,54 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
ISAM2Clique.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
ISAM2-impl.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Specialized iSAM2 Clique. │ │ │ │ +

Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::ISAM2BayesTree
 
class  gtsam::ISAM2JunctionTree
 
struct  gtsam::DeltaImpl
 
struct  gtsam::DeltaImpl::PartialSolveResult
 
struct  gtsam::DeltaImpl::ReorderingMode
 
struct  gtsam::UpdateImpl
 Implementation functions for update method All of the methods below have clear inputs and outputs, even if not functional: iSAM2 is inherintly imperative. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

size_t gtsam::optimizeWildfire (const ISAM2Clique::shared_ptr &root, double threshold, const KeySet &replaced, VectorValues *delta)
 Optimize the BayesTree, starting from the root.
 
│ │ │ │ -size_t gtsam::optimizeWildfireNonRecursive (const ISAM2Clique::shared_ptr &root, double threshold, const KeySet &keys, VectorValues *delta)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Specialized iSAM2 Clique.

│ │ │ │ +

Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.

│ │ │ │
Author
Michael Kaess, Richard Roberts, Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,39 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -ISAM2Clique.cpp File Reference │ │ │ │ │ -Specialized iSAM2 Clique. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +ISAM2-impl.h File Reference │ │ │ │ │ +Incremental update functionality (ISAM2) for BayesTree, with fluid │ │ │ │ │ +relinearization. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_I_S_A_M_2_B_a_y_e_s_T_r_e_e │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_I_S_A_M_2_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_D_e_l_t_a_I_m_p_l │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_D_e_l_t_a_I_m_p_l_:_:_P_a_r_t_i_a_l_S_o_l_v_e_R_e_s_u_l_t │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_D_e_l_t_a_I_m_p_l_:_:_R_e_o_r_d_e_r_i_n_g_M_o_d_e │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_U_p_d_a_t_e_I_m_p_l │ │ │ │ │ + Implementation functions for update method All of the methods below │ │ │ │ │ +  have clear inputs and outputs, even if not functional: iSAM2 is │ │ │ │ │ + inherintly imperative. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -size_t  _g_t_s_a_m_:_:_o_p_t_i_m_i_z_e_W_i_l_d_f_i_r_e (const ISAM2Clique::shared_ptr &root, double │ │ │ │ │ - threshold, const _K_e_y_S_e_t &replaced, _V_e_c_t_o_r_V_a_l_u_e_s *delta) │ │ │ │ │ -  Optimize the _B_a_y_e_s_T_r_e_e, starting from the root. │ │ │ │ │ -  │ │ │ │ │ -size_t  ggttssaamm::::ooppttiimmiizzeeWWiillddffiirreeNNoonnRReeccuurrssiivvee (const ISAM2Clique::shared_ptr │ │ │ │ │ - &root, double threshold, const _K_e_y_S_e_t &keys, _V_e_c_t_o_r_V_a_l_u_e_s *delta) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Specialized iSAM2 Clique. │ │ │ │ │ +Incremental update functionality (ISAM2) for BayesTree, with fluid │ │ │ │ │ +relinearization. │ │ │ │ │ Author │ │ │ │ │ Michael Kaess, Richard Roberts, Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _I_S_A_M_2_C_l_i_q_u_e_._c_p_p │ │ │ │ │ + * _I_S_A_M_2_-_i_m_p_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01103.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,8 @@ │ │ │ │ │ var a01103 = [ │ │ │ │ │ - ["optimizeWildfire", "a01103.html#a2b0857edd76f8d63eeee0ce9944e28d4", null] │ │ │ │ │ + ["gtsam::ISAM2BayesTree", "a04372.html", null], │ │ │ │ │ + ["gtsam::ISAM2JunctionTree", "a04376.html", null], │ │ │ │ │ + ["gtsam::DeltaImpl", "a04380.html", "a04380"], │ │ │ │ │ + ["gtsam::DeltaImpl::PartialSolveResult", "a04384.html", null], │ │ │ │ │ + ["gtsam::DeltaImpl::ReorderingMode", "a04388.html", null], │ │ │ │ │ + ["gtsam::UpdateImpl", "a04392.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01106.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Expression-inl.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ExpressionFactorGraph.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,59 +95,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
Expression-inl.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
ExpressionFactorGraph.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Internals for Expression.h, not for general consumption. │ │ │ │ +

Factor graph that supports adding ExpressionFactors directly. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

struct  gtsam::internal::apply_compose< T >
 
struct  gtsam::internal::apply_compose< double >
class  gtsam::ExpressionFactorGraph
 Factor graph that supports adding ExpressionFactors directly. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

template<typename T >
Expression< T > gtsam::operator* (const Expression< T > &expression1, const Expression< T > &expression2)
 Construct a product expression, assumes T::compose(T) -> T.
 
template<typename T >
std::vector< Expression< T > > gtsam::createUnknowns (size_t n, char c, size_t start)
 Construct an array of leaves.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Internals for Expression.h, not for general consumption.

│ │ │ │ -
Date
September 18, 2014
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ -Paul Furgale
│ │ │ │ +

Factor graph that supports adding ExpressionFactors directly.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
Date
December 2014
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,42 +1,27 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Expression-inl.h File Reference │ │ │ │ │ -Internals for _E_x_p_r_e_s_s_i_o_n_._h, not for general consumption. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +ExpressionFactorGraph.h File Reference │ │ │ │ │ +Factor graph that supports adding ExpressionFactors directly. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_a_p_p_l_y___c_o_m_p_o_s_e_<_ _T_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_a_p_p_l_y___c_o_m_p_o_s_e_<_ _d_o_u_b_l_e_ _> │ │ │ │ │ +class   _g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +  _F_a_c_t_o_r graph that supports adding ExpressionFactors directly. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ - _E_x_p_r_e_s_s_i_o_n< T >  _g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_* (const _E_x_p_r_e_s_s_i_o_n< T > │ │ │ │ │ - &expression1, const _E_x_p_r_e_s_s_i_o_n< T > │ │ │ │ │ - &expression2) │ │ │ │ │ -  Construct a product expression, assumes T:: │ │ │ │ │ - compose(T) -> T. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -std::vector< _E_x_p_r_e_s_s_i_o_n< T > >  _g_t_s_a_m_:_:_c_r_e_a_t_e_U_n_k_n_o_w_n_s (size_t n, char c, size_t │ │ │ │ │ - start) │ │ │ │ │ -  Construct an array of leaves. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Internals for _E_x_p_r_e_s_s_i_o_n_._h, not for general consumption. │ │ │ │ │ - Date │ │ │ │ │ - September 18, 2014 │ │ │ │ │ +Factor graph that supports adding ExpressionFactors directly. │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Paul Furgale │ │ │ │ │ + Date │ │ │ │ │ + December 2014 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _E_x_p_r_e_s_s_i_o_n_-_i_n_l_._h │ │ │ │ │ + * _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01106.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,6 +1,3 @@ │ │ │ │ │ var a01106 = [ │ │ │ │ │ - ["gtsam::internal::apply_compose< T >", "a04272.html", null], │ │ │ │ │ - ["gtsam::internal::apply_compose< double >", "a04276.html", null], │ │ │ │ │ - ["createUnknowns", "a01106.html#a51881c20dd5ecdd129cb993ea8374846", null], │ │ │ │ │ - ["operator*", "a01106.html#a3db519caa1b7e43412c9a7e13acf8329", null] │ │ │ │ │ + ["gtsam::ExpressionFactorGraph", "a04328.html", "a04328"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01106_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Expression-inl.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ExpressionFactorGraph.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,419 +98,67 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Expression-inl.h
│ │ │ │ +
ExpressionFactorGraph.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22// The MSVC compiler workaround for the unsupported variable length array
│ │ │ │ -
23// utilizes the std::unique_ptr<> custom deleter.
│ │ │ │ -
24// See Expression<T>::valueAndJacobianMap() below.
│ │ │ │ -
25#ifdef _MSC_VER
│ │ │ │ -
26#include <memory>
│ │ │ │ -
27#endif
│ │ │ │ -
28
│ │ │ │ -
29#include <gtsam/nonlinear/internal/ExpressionNode.h>
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/nonlinear/ExpressionFactor.h>
│ │ │ │ +
22#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ +
23
│ │ │ │ +
24namespace gtsam {
│ │ │ │ +
25
│ │ │ │ +
│ │ │ │ +
29class ExpressionFactorGraph: public NonlinearFactorGraph {
│ │ │ │
30
│ │ │ │ -
31#include <boost/bind/bind.hpp>
│ │ │ │ -
32#include <boost/tuple/tuple.hpp>
│ │ │ │ -
33#include <boost/range/adaptor/map.hpp>
│ │ │ │ -
34#include <boost/range/algorithm.hpp>
│ │ │ │ +
31public:
│ │ │ │ +
32
│ │ │ │
35
│ │ │ │ -
36namespace gtsam {
│ │ │ │ -
37
│ │ │ │ -
38template<typename T>
│ │ │ │ -
│ │ │ │ -
39Expression<T>::Expression(const T& value) :
│ │ │ │ -
40 root_(new internal::ConstantExpression<T>(value)) {
│ │ │ │ -
41}
│ │ │ │ -
│ │ │ │ -
42
│ │ │ │ -
43template<typename T>
│ │ │ │ -
│ │ │ │ -
44Expression<T>::Expression(const Key& key) :
│ │ │ │ -
45 root_(new internal::LeafExpression<T>(key)) {
│ │ │ │ -
46}
│ │ │ │ -
│ │ │ │ -
47
│ │ │ │ -
48template<typename T>
│ │ │ │ -
│ │ │ │ -
49Expression<T>::Expression(const Symbol& symbol) :
│ │ │ │ -
50 root_(new internal::LeafExpression<T>(symbol)) {
│ │ │ │ -
51}
│ │ │ │ -
│ │ │ │ -
52
│ │ │ │ -
53template<typename T>
│ │ │ │ -
│ │ │ │ -
54Expression<T>::Expression(unsigned char c, std::uint64_t j) :
│ │ │ │ -
55 root_(new internal::LeafExpression<T>(Symbol(c, j))) {
│ │ │ │ -
56}
│ │ │ │ -
│ │ │ │ -
57
│ │ │ │ -
59template<typename T>
│ │ │ │ -
60template<typename A>
│ │ │ │ -
│ │ │ │ -
61Expression<T>::Expression(typename UnaryFunction<A>::type function,
│ │ │ │ -
62 const Expression<A>& expression) :
│ │ │ │ -
63 root_(new internal::UnaryExpression<T, A>(function, expression)) {
│ │ │ │ -
64}
│ │ │ │ -
│ │ │ │ -
65
│ │ │ │ -
67template<typename T>
│ │ │ │ -
68template<typename A1, typename A2>
│ │ │ │ -
│ │ │ │ -
69Expression<T>::Expression(typename BinaryFunction<A1, A2>::type function,
│ │ │ │ -
70 const Expression<A1>& expression1, const Expression<A2>& expression2) :
│ │ │ │ -
71 root_(
│ │ │ │ -
72 new internal::BinaryExpression<T, A1, A2>(function, expression1,
│ │ │ │ -
73 expression2)) {
│ │ │ │ -
74}
│ │ │ │ -
│ │ │ │ -
75
│ │ │ │ -
77template<typename T>
│ │ │ │ -
78template<typename A1, typename A2, typename A3>
│ │ │ │ -
│ │ │ │ -
79Expression<T>::Expression(typename TernaryFunction<A1, A2, A3>::type function,
│ │ │ │ -
80 const Expression<A1>& expression1, const Expression<A2>& expression2,
│ │ │ │ -
81 const Expression<A3>& expression3) :
│ │ │ │ -
82 root_(
│ │ │ │ -
83 new internal::TernaryExpression<T, A1, A2, A3>(function, expression1,
│ │ │ │ -
84 expression2, expression3)) {
│ │ │ │ -
85}
│ │ │ │ -
│ │ │ │ -
86
│ │ │ │ -
88template<typename T>
│ │ │ │ -
89template<typename A>
│ │ │ │ -
│ │ │ │ -
90Expression<T>::Expression(const Expression<A>& expression,
│ │ │ │ -
91 T (A::*method)(typename MakeOptionalJacobian<T, A>::type) const) :
│ │ │ │ -
│ │ │ │ -
92 root_(
│ │ │ │ -
93 new internal::UnaryExpression<T, A>(std::bind(method,
│ │ │ │ -
94 std::placeholders::_1, std::placeholders::_2),
│ │ │ │ -
│ │ │ │ -
95 expression)) {
│ │ │ │ -
96}
│ │ │ │ -
97
│ │ │ │ -
99template<typename T>
│ │ │ │ -
100template<typename A1, typename A2>
│ │ │ │ -
│ │ │ │ -
101Expression<T>::Expression(const Expression<A1>& expression1,
│ │ │ │ -
102 T (A1::*method)(const A2&, typename MakeOptionalJacobian<T, A1>::type,
│ │ │ │ -
103 typename MakeOptionalJacobian<T, A2>::type) const,
│ │ │ │ -
104 const Expression<A2>& expression2) :
│ │ │ │ -
│ │ │ │ -
105 root_(
│ │ │ │ -
106 new internal::BinaryExpression<T, A1, A2>(
│ │ │ │ -
107 std::bind(method, std::placeholders::_1,
│ │ │ │ -
108 std::placeholders::_2, std::placeholders::_3,
│ │ │ │ -
109 std::placeholders::_4),
│ │ │ │ -
│ │ │ │ -
110 expression1, expression2)) {
│ │ │ │ -
111}
│ │ │ │ -
112
│ │ │ │ -
114template<typename T>
│ │ │ │ -
│ │ │ │ -
115template<typename A1, typename A2, typename A3>
│ │ │ │ -
│ │ │ │ -
116Expression<T>::Expression(const Expression<A1>& expression1,
│ │ │ │ -
117 T (A1::*method)(const A2&, const A3&,
│ │ │ │ -
118 typename MakeOptionalJacobian<T, A1>::type,
│ │ │ │ -
119 typename MakeOptionalJacobian<T, A2>::type,
│ │ │ │ -
120 typename MakeOptionalJacobian<T, A3>::type) const,
│ │ │ │ -
│ │ │ │ -
121 const Expression<A2>& expression2, const Expression<A3>& expression3) :
│ │ │ │ -
122 root_(
│ │ │ │ -
123 new internal::TernaryExpression<T, A1, A2, A3>(
│ │ │ │ -
124 std::bind(method, std::placeholders::_1,
│ │ │ │ -
125 std::placeholders::_2, std::placeholders::_3,
│ │ │ │ -
│ │ │ │ -
126 std::placeholders::_4, std::placeholders::_5,
│ │ │ │ -
127 std::placeholders::_6),
│ │ │ │ -
128 expression1, expression2, expression3)) {
│ │ │ │ -
129}
│ │ │ │ -
130
│ │ │ │ -
131template<typename T>
│ │ │ │ -
│ │ │ │ -
132std::set<Key> Expression<T>::keys() const {
│ │ │ │ -
│ │ │ │ -
133 return root_->keys();
│ │ │ │ -
134}
│ │ │ │ -
135
│ │ │ │ -
136template<typename T>
│ │ │ │ -
│ │ │ │ -
137void Expression<T>::dims(std::map<Key, int>& map) const {
│ │ │ │ -
138 root_->dims(map);
│ │ │ │ -
139}
│ │ │ │ -
│ │ │ │ -
140
│ │ │ │ -
141template<typename T>
│ │ │ │ -
│ │ │ │ -
142void Expression<T>::print(const std::string& s) const {
│ │ │ │ -
143 root_->print(s);
│ │ │ │ -
144}
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
145
│ │ │ │ -
146template<typename T>
│ │ │ │ -
│ │ │ │ -
147T Expression<T>::value(const Values& values,
│ │ │ │ -
│ │ │ │ -
148 boost::optional<std::vector<Matrix>&> H) const {
│ │ │ │ -
149
│ │ │ │ -
150 if (H) {
│ │ │ │ -
│ │ │ │ -
151 // Call private version that returns derivatives in H
│ │ │ │ -
152 KeyVector keys;
│ │ │ │ -
153 FastVector<int> dims;
│ │ │ │ -
154 boost::tie(keys, dims) = keysAndDims();
│ │ │ │ -
155 return valueAndDerivatives(values, keys, dims, *H);
│ │ │ │ -
156 } else
│ │ │ │ -
157 // no derivatives needed, just return value
│ │ │ │ -
│ │ │ │ -
158 return root_->value(values);
│ │ │ │ -
159}
│ │ │ │ -
│ │ │ │ -
160
│ │ │ │ -
161template<typename T>
│ │ │ │ -
│ │ │ │ -
162const boost::shared_ptr<internal::ExpressionNode<T> >& Expression<T>::root() const {
│ │ │ │ -
163 return root_;
│ │ │ │ -
164}
│ │ │ │ -
│ │ │ │ -
165
│ │ │ │ -
166template<typename T>
│ │ │ │ -
│ │ │ │ -
167size_t Expression<T>::traceSize() const {
│ │ │ │ -
168 return root_->traceSize();
│ │ │ │ -
169}
│ │ │ │ -
│ │ │ │ -
170
│ │ │ │ -
│ │ │ │ -
171// Private methods:
│ │ │ │ -
172
│ │ │ │ -
173template<typename T>
│ │ │ │ -
│ │ │ │ -
174T Expression<T>::valueAndDerivatives(const Values& values,
│ │ │ │ -
175 const KeyVector& keys, const FastVector<int>& dims,
│ │ │ │ -
176 std::vector<Matrix>& H) const {
│ │ │ │ -
│ │ │ │ -
177
│ │ │ │ -
178 // H should be pre-allocated
│ │ │ │ -
179 assert(H.size()==keys.size());
│ │ │ │ -
180
│ │ │ │ -
181 // Pre-allocate and zero VerticalBlockMatrix
│ │ │ │ -
182 static const int Dim = traits<T>::dimension;
│ │ │ │ -
183 VerticalBlockMatrix Ab(dims, Dim);
│ │ │ │ -
184 Ab.matrix().setZero();
│ │ │ │ -
185 internal::JacobianMap jacobianMap(keys, Ab);
│ │ │ │ -
186
│ │ │ │ -
187 // Call unsafe version
│ │ │ │ -
188 T result = valueAndJacobianMap(values, jacobianMap);
│ │ │ │ -
│ │ │ │ -
189
│ │ │ │ -
190 // Copy blocks into the vector of jacobians passed in
│ │ │ │ -
191 for (DenseIndex i = 0; i < static_cast<DenseIndex>(keys.size()); i++)
│ │ │ │ -
192 H[i] = Ab(i);
│ │ │ │ -
│ │ │ │ -
193
│ │ │ │ -
194 return result;
│ │ │ │ -
195}
│ │ │ │ -
196
│ │ │ │ -
│ │ │ │ -
197template<typename T>
│ │ │ │ -
│ │ │ │ -
198T Expression<T>::traceExecution(const Values& values,
│ │ │ │ -
199 internal::ExecutionTrace<T>& trace, void* traceStorage) const {
│ │ │ │ -
200 return root_->traceExecution(values, trace,
│ │ │ │ -
201 static_cast<internal::ExecutionTraceStorage*>(traceStorage));
│ │ │ │ -
202}
│ │ │ │ -
│ │ │ │ -
203
│ │ │ │ -
204template<typename T>
│ │ │ │ -
│ │ │ │ -
205T Expression<T>::valueAndJacobianMap(const Values& values,
│ │ │ │ -
206 internal::JacobianMap& jacobians) const {
│ │ │ │ -
207 // The following piece of code is absolutely crucial for performance.
│ │ │ │ -
208 // We allocate a block of memory on the stack, which can be done at runtime
│ │ │ │ -
209 // with modern C++ compilers. The traceExecution then fills this memory
│ │ │ │ -
210 // with an execution trace, made up entirely of "Record" structs, see
│ │ │ │ -
211 // the FunctionalNode class in expression-inl.h
│ │ │ │ -
212 size_t size = traceSize();
│ │ │ │ -
213
│ │ │ │ -
214 // Windows does not support variable length arrays, so memory must be dynamically
│ │ │ │ -
215 // allocated on Visual Studio. For more information see the issue below
│ │ │ │ -
216 // https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio
│ │ │ │ -
217#ifdef _MSC_VER
│ │ │ │ -
218 std::unique_ptr<void, void(*)(void*)> traceStorageDeleter(
│ │ │ │ -
219 _aligned_malloc(size, internal::TraceAlignment),
│ │ │ │ -
220 [](void *ptr){ _aligned_free(ptr); });
│ │ │ │ -
221 auto traceStorage = static_cast<internal::ExecutionTraceStorage*>(traceStorageDeleter.get());
│ │ │ │ -
222#else
│ │ │ │ -
223 internal::ExecutionTraceStorage traceStorage[size];
│ │ │ │ -
224#endif
│ │ │ │ -
225
│ │ │ │ -
226 internal::ExecutionTrace<T> trace;
│ │ │ │ -
227 T value(this->traceExecution(values, trace, traceStorage));
│ │ │ │ -
228 trace.startReverseAD1(jacobians);
│ │ │ │ -
229
│ │ │ │ -
230 return value;
│ │ │ │ -
231}
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
232
│ │ │ │ -
233template<typename T>
│ │ │ │ -
234typename Expression<T>::KeysAndDims Expression<T>::keysAndDims() const {
│ │ │ │ -
235 std::map<Key, int> map;
│ │ │ │ -
236 dims(map);
│ │ │ │ -
237 size_t n = map.size();
│ │ │ │ -
238 KeysAndDims pair = std::make_pair(KeyVector(n), FastVector<int>(n));
│ │ │ │ -
239 boost::copy(map | boost::adaptors::map_keys, pair.first.begin());
│ │ │ │ -
240 boost::copy(map | boost::adaptors::map_values, pair.second.begin());
│ │ │ │ -
241 return pair;
│ │ │ │ -
242}
│ │ │ │ -
243
│ │ │ │ -
244namespace internal {
│ │ │ │ -
245// http://stackoverflow.com/questions/16260445/boost-bind-to-operator
│ │ │ │ -
246template<class T>
│ │ │ │ -
│ │ │ │ -
247struct apply_compose {
│ │ │ │ -
248 typedef T result_type;
│ │ │ │ -
249 static const int Dim = traits<T>::dimension;
│ │ │ │ -
250 T operator()(const T& x, const T& y, OptionalJacobian<Dim, Dim> H1 =
│ │ │ │ -
251 boost::none, OptionalJacobian<Dim, Dim> H2 = boost::none) const {
│ │ │ │ -
252 return x.compose(y, H1, H2);
│ │ │ │ -
253 }
│ │ │ │ -
254};
│ │ │ │ -
│ │ │ │ -
255
│ │ │ │ -
256template <>
│ │ │ │ -
│ │ │ │ -
257struct apply_compose<double> {
│ │ │ │ -
258 double operator()(const double& x, const double& y,
│ │ │ │ -
259 OptionalJacobian<1, 1> H1 = boost::none,
│ │ │ │ -
260 OptionalJacobian<1, 1> H2 = boost::none) const {
│ │ │ │ -
261 if (H1) H1->setConstant(y);
│ │ │ │ -
262 if (H2) H2->setConstant(x);
│ │ │ │ -
263 return x * y;
│ │ │ │ -
264 }
│ │ │ │ -
265};
│ │ │ │ -
│ │ │ │ -
266
│ │ │ │ -
267}
│ │ │ │ -
268
│ │ │ │ -
269// Global methods:
│ │ │ │ -
270
│ │ │ │ -
272template<typename T>
│ │ │ │ -
│ │ │ │ -
273Expression<T> operator*(const Expression<T>& expression1,
│ │ │ │ -
274 const Expression<T>& expression2) {
│ │ │ │ -
275 return Expression<T>(
│ │ │ │ -
276 std::bind(internal::apply_compose<T>(), std::placeholders::_1,
│ │ │ │ -
277 std::placeholders::_2, std::placeholders::_3,
│ │ │ │ -
278 std::placeholders::_4),
│ │ │ │ -
279 expression1, expression2);
│ │ │ │ -
280}
│ │ │ │ -
│ │ │ │ -
281
│ │ │ │ -
283template<typename T>
│ │ │ │ -
│ │ │ │ -
284std::vector<Expression<T> > createUnknowns(size_t n, char c, size_t start) {
│ │ │ │ -
285 std::vector<Expression<T> > unknowns;
│ │ │ │ -
286 unknowns.reserve(n);
│ │ │ │ -
287 for (size_t i = start; i < start + n; i++)
│ │ │ │ -
288 unknowns.push_back(Expression<T>(c, i));
│ │ │ │ -
289 return unknowns;
│ │ │ │ -
290}
│ │ │ │ -
│ │ │ │ -
291
│ │ │ │ -
292template <typename T>
│ │ │ │ -
293ScalarMultiplyExpression<T>::ScalarMultiplyExpression(double s, const Expression<T>& e)
│ │ │ │ -
294 : Expression<T>(boost::make_shared<internal::ScalarMultiplyNode<T>>(s, e)) {}
│ │ │ │ -
295
│ │ │ │ -
296
│ │ │ │ -
297template <typename T>
│ │ │ │ -
298BinarySumExpression<T>::BinarySumExpression(const Expression<T>& e1, const Expression<T>& e2)
│ │ │ │ -
299 : Expression<T>(boost::make_shared<internal::BinarySumNode<T>>(e1, e2)) {}
│ │ │ │ -
300
│ │ │ │ -
301template <typename T>
│ │ │ │ -
│ │ │ │ -
302Expression<T>& Expression<T>::operator+=(const Expression<T>& e) {
│ │ │ │ -
303 root_ = boost::make_shared<internal::BinarySumNode<T>>(*this, e);
│ │ │ │ -
304 return *this;
│ │ │ │ -
305}
│ │ │ │ -
│ │ │ │ -
306
│ │ │ │ -
307} // namespace gtsam
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │ +
42 template<typename T>
│ │ │ │ +
│ │ │ │ +
43 void addExpressionFactor(const Expression<T>& h, const T& z,
│ │ │ │ +
44 const SharedNoiseModel& R) {
│ │ │ │ +
45 using F = ExpressionFactor<T>;
│ │ │ │ +
46 push_back(boost::allocate_shared<F>(Eigen::aligned_allocator<F>(), R, z, h));
│ │ │ │ +
47 }
│ │ │ │ +
│ │ │ │ +
48
│ │ │ │ +
50};
│ │ │ │ +
│ │ │ │ +
51
│ │ │ │ +
52}
│ │ │ │ +
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │ -
gtsam::symbol
Key symbol(unsigned char c, std::uint64_t j)
Create a symbol key from a character and index, i.e.
Definition Symbol.h:135
│ │ │ │ -
gtsam::createUnknowns
std::vector< Expression< T > > createUnknowns(size_t n, char c, size_t start)
Construct an array of leaves.
Definition Expression-inl.h:284
│ │ │ │ -
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
│ │ │ │ -
gtsam::make_shared
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost::shared_ptr< T > > make_shared(Args &&... args)
Add our own make_shared as a layer of wrapping on boost::make_shared This solves the problem with the...
Definition make_shared.h:57
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
gtsam::MakeOptionalJacobian
: meta-function to generate JacobianTA optional reference Used mainly by Expressions
Definition OptionalJacobian.h:261
│ │ │ │ -
gtsam::VerticalBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43
│ │ │ │ -
gtsam::Symbol
Character and index key used to refer to variables.
Definition Symbol.h:35
│ │ │ │ -
gtsam::internal::apply_compose
Definition Expression-inl.h:247
│ │ │ │ -
gtsam::internal::ExecutionTrace
Definition Expression.h:40
│ │ │ │ +
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ +
gtsam::FactorGraph< NonlinearFactor >::push_back
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition FactorGraph.h:186
│ │ │ │ +
gtsam::ExpressionFactor
Factor that supports arbitrary expressions via AD.
Definition ExpressionFactor.h:44
│ │ │ │
gtsam::Expression
Expression class that supports automatic differentiation.
Definition Expression.h:48
│ │ │ │ -
gtsam::Expression::Expression
Expression()
Default constructor, for serialization.
Definition Expression.h:182
│ │ │ │ -
gtsam::Expression::keys
std::set< Key > keys() const
Return keys that play in this expression.
Definition Expression-inl.h:132
│ │ │ │ -
gtsam::Expression::KeysAndDims
std::pair< KeyVector, FastVector< int > > KeysAndDims
Keys and dimensions in same order.
Definition Expression.h:185
│ │ │ │ -
gtsam::Expression::dims
void dims(std::map< Key, int > &map) const
Return dimensions for each argument, as a map.
Definition Expression-inl.h:137
│ │ │ │ -
gtsam::Expression::print
void print(const std::string &s) const
Print.
Definition Expression-inl.h:142
│ │ │ │ -
gtsam::Expression::traceSize
size_t traceSize() const
Return size needed for memory buffer in traceExecution.
Definition Expression-inl.h:167
│ │ │ │ -
gtsam::Expression::traceExecution
T traceExecution(const Values &values, internal::ExecutionTrace< T > &trace, void *traceStorage) const
trace execution, very unsafe
Definition Expression-inl.h:198
│ │ │ │ -
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
gtsam::ExpressionFactorGraph
Factor graph that supports adding ExpressionFactors directly.
Definition ExpressionFactorGraph.h:29
│ │ │ │ +
gtsam::ExpressionFactorGraph::addExpressionFactor
void addExpressionFactor(const Expression< T > &h, const T &z, const SharedNoiseModel &R)
Directly add ExpressionFactor that implements |h(x)-z|^2_R.
Definition ExpressionFactorGraph.h:43
│ │ │ │ +
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,408 +1,76 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Expression-inl.h │ │ │ │ │ +ExpressionFactorGraph.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22// The MSVC compiler workaround for the unsupported variable length array │ │ │ │ │ -23// utilizes the std::unique_ptr<> custom deleter. │ │ │ │ │ -24// See Expression::valueAndJacobianMap() below. │ │ │ │ │ -25#ifdef _MSC_VER │ │ │ │ │ -26#include │ │ │ │ │ -27#endif │ │ │ │ │ -28 │ │ │ │ │ -29#include │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include │ │ │ │ │ +22#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +23 │ │ │ │ │ +24namespace _g_t_s_a_m { │ │ │ │ │ +25 │ │ │ │ │ +_2_9class _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_G_r_a_p_h: public _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h { │ │ │ │ │ 30 │ │ │ │ │ -31#include │ │ │ │ │ -32#include │ │ │ │ │ -33#include │ │ │ │ │ -34#include │ │ │ │ │ +31public: │ │ │ │ │ +32 │ │ │ │ │ 35 │ │ │ │ │ -36namespace _g_t_s_a_m { │ │ │ │ │ -37 │ │ │ │ │ -38template │ │ │ │ │ -_3_9_E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_E_x_p_r_e_s_s_i_o_n(const T& value) : │ │ │ │ │ -40 root_(new internal::ConstantExpression(value)) { │ │ │ │ │ -41} │ │ │ │ │ -42 │ │ │ │ │ -43template │ │ │ │ │ -_4_4_E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_E_x_p_r_e_s_s_i_o_n(const _K_e_y& key) : │ │ │ │ │ -45 root_(new internal::LeafExpression(key)) { │ │ │ │ │ -46} │ │ │ │ │ -47 │ │ │ │ │ -48template │ │ │ │ │ -_4_9_E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_E_x_p_r_e_s_s_i_o_n(const _S_y_m_b_o_l& _s_y_m_b_o_l) : │ │ │ │ │ -50 root_(new internal::LeafExpression(_s_y_m_b_o_l)) { │ │ │ │ │ -51} │ │ │ │ │ -52 │ │ │ │ │ -53template │ │ │ │ │ -_5_4_E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_E_x_p_r_e_s_s_i_o_n(unsigned char c, std::uint64_t j) : │ │ │ │ │ -55 root_(new internal::LeafExpression(_S_y_m_b_o_l(c, j))) { │ │ │ │ │ -56} │ │ │ │ │ -57 │ │ │ │ │ -59template │ │ │ │ │ -60template │ │ │ │ │ -_6_1_E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_E_x_p_r_e_s_s_i_o_n(typename UnaryFunction::type function, │ │ │ │ │ -62 const _E_x_p_r_e_s_s_i_o_n_<_A_>& expression) : │ │ │ │ │ -63 root_(new internal::UnaryExpression(function, expression)) { │ │ │ │ │ -64} │ │ │ │ │ -65 │ │ │ │ │ -67template │ │ │ │ │ -68template │ │ │ │ │ -_6_9_E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_E_x_p_r_e_s_s_i_o_n(typename BinaryFunction::type function, │ │ │ │ │ -70 const _E_x_p_r_e_s_s_i_o_n_<_A_1_>& expression1, const _E_x_p_r_e_s_s_i_o_n_<_A_2_>& expression2) : │ │ │ │ │ -71 root_( │ │ │ │ │ -72 new internal::BinaryExpression(function, expression1, │ │ │ │ │ -73 expression2)) { │ │ │ │ │ -74} │ │ │ │ │ -75 │ │ │ │ │ -77template │ │ │ │ │ -78template │ │ │ │ │ -_7_9_E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_E_x_p_r_e_s_s_i_o_n(typename TernaryFunction::type │ │ │ │ │ -function, │ │ │ │ │ -80 const _E_x_p_r_e_s_s_i_o_n_<_A_1_>& expression1, const _E_x_p_r_e_s_s_i_o_n_<_A_2_>& expression2, │ │ │ │ │ -81 const _E_x_p_r_e_s_s_i_o_n_<_A_3_>& expression3) : │ │ │ │ │ -82 root_( │ │ │ │ │ -83 new internal::TernaryExpression(function, expression1, │ │ │ │ │ -84 expression2, expression3)) { │ │ │ │ │ -85} │ │ │ │ │ -86 │ │ │ │ │ -88template │ │ │ │ │ -89template │ │ │ │ │ -_9_0_E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_E_x_p_r_e_s_s_i_o_n(const _E_x_p_r_e_s_s_i_o_n_<_A_>& expression, │ │ │ │ │ -91 T (A::*method)(typename _M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n::_t_y_p_e) const) : │ │ │ │ │ -_9_2 root_( │ │ │ │ │ -93 new internal::UnaryExpression(std::bind(method, │ │ │ │ │ -94 std::placeholders::_1, std::placeholders::_2), │ │ │ │ │ -_9_5 expression)) { │ │ │ │ │ -96} │ │ │ │ │ -97 │ │ │ │ │ -99template │ │ │ │ │ -100template │ │ │ │ │ -_1_0_1_E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_E_x_p_r_e_s_s_i_o_n(const _E_x_p_r_e_s_s_i_o_n_<_A_1_>& expression1, │ │ │ │ │ -102 T (A1::*method)(const A2&, typename _M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n::_t_y_p_e, │ │ │ │ │ -103 typename _M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n::_t_y_p_e) const, │ │ │ │ │ -104 const _E_x_p_r_e_s_s_i_o_n& expression2) : │ │ │ │ │ -_1_0_5 root_( │ │ │ │ │ -106 new internal::BinaryExpression( │ │ │ │ │ -107 std::bind(method, std::placeholders::_1, │ │ │ │ │ -108 std::placeholders::_2, std::placeholders::_3, │ │ │ │ │ -109 std::placeholders::_4), │ │ │ │ │ -_1_1_0 expression1, expression2)) { │ │ │ │ │ -111} │ │ │ │ │ -112 │ │ │ │ │ -114template │ │ │ │ │ -_1_1_5template │ │ │ │ │ -_1_1_6_E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_E_x_p_r_e_s_s_i_o_n(const _E_x_p_r_e_s_s_i_o_n_<_A_1_>& expression1, │ │ │ │ │ -117 T (A1::*method)(const A2&, const A3&, │ │ │ │ │ -118 typename _M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n::_t_y_p_e, │ │ │ │ │ -119 typename _M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n::_t_y_p_e, │ │ │ │ │ -120 typename _M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n::_t_y_p_e) const, │ │ │ │ │ -_1_2_1 const _E_x_p_r_e_s_s_i_o_n& expression2, const _E_x_p_r_e_s_s_i_o_n& expression3) : │ │ │ │ │ -122 root_( │ │ │ │ │ -123 new internal::TernaryExpression( │ │ │ │ │ -124 std::bind(method, std::placeholders::_1, │ │ │ │ │ -125 std::placeholders::_2, std::placeholders::_3, │ │ │ │ │ -_1_2_6 std::placeholders::_4, std::placeholders::_5, │ │ │ │ │ -127 std::placeholders::_6), │ │ │ │ │ -128 expression1, expression2, expression3)) { │ │ │ │ │ -129} │ │ │ │ │ -130 │ │ │ │ │ -131template │ │ │ │ │ -_1_3_2std::set _E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_k_e_y_s() const { │ │ │ │ │ -_1_3_3 return root_->_k_e_y_s(); │ │ │ │ │ -134} │ │ │ │ │ -135 │ │ │ │ │ -136template │ │ │ │ │ -_1_3_7void _E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_d_i_m_s(std::map& map) const { │ │ │ │ │ -138 root_->_d_i_m_s(map); │ │ │ │ │ -139} │ │ │ │ │ -140 │ │ │ │ │ -141template │ │ │ │ │ -_1_4_2void _E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_p_r_i_n_t(const std::string& s) const { │ │ │ │ │ -143 root_->_p_r_i_n_t(s); │ │ │ │ │ -144} │ │ │ │ │ -_1_4_5 │ │ │ │ │ -146template │ │ │ │ │ -_1_4_7T _E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_v_a_l_u_e(const _V_a_l_u_e_s& values, │ │ │ │ │ -_1_4_8 boost::optional&> H) const { │ │ │ │ │ -149 │ │ │ │ │ -150 if (H) { │ │ │ │ │ -_1_5_1 // Call private version that returns derivatives in H │ │ │ │ │ -152 _K_e_y_V_e_c_t_o_r keys; │ │ │ │ │ -153 _F_a_s_t_V_e_c_t_o_r_<_i_n_t_> dims; │ │ │ │ │ -154 boost::tie(keys, dims) = keysAndDims(); │ │ │ │ │ -155 return valueAndDerivatives(values, keys, dims, *H); │ │ │ │ │ -156 } else │ │ │ │ │ -157 // no derivatives needed, just return value │ │ │ │ │ -_1_5_8 return root_->value(values); │ │ │ │ │ -159} │ │ │ │ │ -160 │ │ │ │ │ -161template │ │ │ │ │ -_1_6_2const boost::shared_ptr >& _E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_r_o_o_t() │ │ │ │ │ -const { │ │ │ │ │ -163 return root_; │ │ │ │ │ -164} │ │ │ │ │ -165 │ │ │ │ │ -166template │ │ │ │ │ -_1_6_7size_t _E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_t_r_a_c_e_S_i_z_e() const { │ │ │ │ │ -168 return root_->_t_r_a_c_e_S_i_z_e(); │ │ │ │ │ -169} │ │ │ │ │ -170 │ │ │ │ │ -_1_7_1// Private methods: │ │ │ │ │ -172 │ │ │ │ │ -173template │ │ │ │ │ -_1_7_4T _E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_v_a_l_u_e_A_n_d_D_e_r_i_v_a_t_i_v_e_s(const _V_a_l_u_e_s& values, │ │ │ │ │ -175 const _K_e_y_V_e_c_t_o_r& keys, const _F_a_s_t_V_e_c_t_o_r_<_i_n_t_>& dims, │ │ │ │ │ -176 std::vector& H) const { │ │ │ │ │ -_1_7_7 │ │ │ │ │ -178 // H should be pre-allocated │ │ │ │ │ -179 assert(H.size()==keys.size()); │ │ │ │ │ -180 │ │ │ │ │ -181 // Pre-allocate and zero VerticalBlockMatrix │ │ │ │ │ -182 static const int Dim = _t_r_a_i_t_s_<_T_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ -183 _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x Ab(dims, Dim); │ │ │ │ │ -184 Ab.matrix().setZero(); │ │ │ │ │ -185 internal::JacobianMap jacobianMap(keys, Ab); │ │ │ │ │ -186 │ │ │ │ │ -187 // Call unsafe version │ │ │ │ │ -188 T result = valueAndJacobianMap(values, jacobianMap); │ │ │ │ │ -_1_8_9 │ │ │ │ │ -190 // Copy blocks into the vector of jacobians passed in │ │ │ │ │ -191 for (_D_e_n_s_e_I_n_d_e_x i = 0; i < static_cast(keys.size()); i++) │ │ │ │ │ -192 H[i] = Ab(i); │ │ │ │ │ -_1_9_3 │ │ │ │ │ -194 return result; │ │ │ │ │ -195} │ │ │ │ │ -196 │ │ │ │ │ -_1_9_7template │ │ │ │ │ -_1_9_8T _E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_t_r_a_c_e_E_x_e_c_u_t_i_o_n(const _V_a_l_u_e_s& values, │ │ │ │ │ -199 _i_n_t_e_r_n_a_l_:_:_E_x_e_c_u_t_i_o_n_T_r_a_c_e_<_T_>& trace, void* traceStorage) const { │ │ │ │ │ -200 return root_->_t_r_a_c_e_E_x_e_c_u_t_i_o_n(values, trace, │ │ │ │ │ -201 static_cast(traceStorage)); │ │ │ │ │ -202} │ │ │ │ │ -203 │ │ │ │ │ -204template │ │ │ │ │ -_2_0_5T _E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_v_a_l_u_e_A_n_d_J_a_c_o_b_i_a_n_M_a_p(const _V_a_l_u_e_s& values, │ │ │ │ │ -206 internal::JacobianMap& jacobians) const { │ │ │ │ │ -207 // The following piece of code is absolutely crucial for performance. │ │ │ │ │ -208 // We allocate a block of memory on the stack, which can be done at runtime │ │ │ │ │ -209 // with modern C++ compilers. The traceExecution then fills this memory │ │ │ │ │ -210 // with an execution trace, made up entirely of "Record" structs, see │ │ │ │ │ -211 // the FunctionalNode class in expression-inl.h │ │ │ │ │ -212 size_t size = traceSize(); │ │ │ │ │ -213 │ │ │ │ │ -214 // Windows does not support variable length arrays, so memory must be │ │ │ │ │ -dynamically │ │ │ │ │ -215 // allocated on Visual Studio. For more information see the issue below │ │ │ │ │ -216 // https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual- │ │ │ │ │ -studio │ │ │ │ │ -217#ifdef _MSC_VER │ │ │ │ │ -218 std::unique_ptr traceStorageDeleter( │ │ │ │ │ -219 _aligned_malloc(size, internal::TraceAlignment), │ │ │ │ │ -220 [](void *ptr){ _aligned_free(ptr); }); │ │ │ │ │ -221 auto traceStorage = static_cast │ │ │ │ │ -(traceStorageDeleter.get()); │ │ │ │ │ -222#else │ │ │ │ │ -223 internal::ExecutionTraceStorage traceStorage[size]; │ │ │ │ │ -224#endif │ │ │ │ │ -225 │ │ │ │ │ -226 _i_n_t_e_r_n_a_l_:_:_E_x_e_c_u_t_i_o_n_T_r_a_c_e_<_T_> trace; │ │ │ │ │ -227 T value(this->traceExecution(values, trace, traceStorage)); │ │ │ │ │ -228 trace.startReverseAD1(jacobians); │ │ │ │ │ -229 │ │ │ │ │ -230 return value; │ │ │ │ │ -231} │ │ │ │ │ -232 │ │ │ │ │ -233template │ │ │ │ │ -234typename _E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_K_e_y_s_A_n_d_D_i_m_s _E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_k_e_y_s_A_n_d_D_i_m_s() const { │ │ │ │ │ -235 std::map map; │ │ │ │ │ -236 dims(map); │ │ │ │ │ -237 size_t n = map.size(); │ │ │ │ │ -238 KeysAndDims pair = std::make_pair(_K_e_y_V_e_c_t_o_r(n), _F_a_s_t_V_e_c_t_o_r_<_i_n_t_>(n)); │ │ │ │ │ -239 boost::copy(map | boost::adaptors::map_keys, pair.first.begin()); │ │ │ │ │ -240 boost::copy(map | boost::adaptors::map_values, pair.second.begin()); │ │ │ │ │ -241 return pair; │ │ │ │ │ -242} │ │ │ │ │ -243 │ │ │ │ │ -244namespace internal { │ │ │ │ │ -245// http://stackoverflow.com/questions/16260445/boost-bind-to-operator │ │ │ │ │ -246template │ │ │ │ │ -_2_4_7struct _a_p_p_l_y___c_o_m_p_o_s_e { │ │ │ │ │ -248 typedef T result_type; │ │ │ │ │ -249 static const int Dim = _t_r_a_i_t_s_<_T_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ -250 T operator()(const T& x, const T& y, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_D_i_m_,_ _D_i_m_> H1 = │ │ │ │ │ -251 boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_D_i_m_,_ _D_i_m_> H2 = boost::none) const { │ │ │ │ │ -252 return x.compose(y, H1, H2); │ │ │ │ │ -253 } │ │ │ │ │ -254}; │ │ │ │ │ -255 │ │ │ │ │ -256template <> │ │ │ │ │ -_2_5_7struct _a_p_p_l_y___c_o_m_p_o_s_e { │ │ │ │ │ -258 double operator()(const double& x, const double& y, │ │ │ │ │ -259 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _1_> H1 = boost::none, │ │ │ │ │ -260 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _1_> H2 = boost::none) const { │ │ │ │ │ -261 if (H1) H1->setConstant(y); │ │ │ │ │ -262 if (H2) H2->setConstant(x); │ │ │ │ │ -263 return x * y; │ │ │ │ │ -264 } │ │ │ │ │ -265}; │ │ │ │ │ -266 │ │ │ │ │ -267} │ │ │ │ │ -268 │ │ │ │ │ -269// Global methods: │ │ │ │ │ -270 │ │ │ │ │ -272template │ │ │ │ │ -_2_7_3_E_x_p_r_e_s_s_i_o_n_<_T_> _o_p_e_r_a_t_o_r_*(const _E_x_p_r_e_s_s_i_o_n_<_T_>& expression1, │ │ │ │ │ -274 const _E_x_p_r_e_s_s_i_o_n_<_T_>& expression2) { │ │ │ │ │ -275 return _E_x_p_r_e_s_s_i_o_n_<_T_>( │ │ │ │ │ -276 std::bind(_i_n_t_e_r_n_a_l_:_:_a_p_p_l_y___c_o_m_p_o_s_e_<_T_>(), std::placeholders::_1, │ │ │ │ │ -277 std::placeholders::_2, std::placeholders::_3, │ │ │ │ │ -278 std::placeholders::_4), │ │ │ │ │ -279 expression1, expression2); │ │ │ │ │ -280} │ │ │ │ │ -281 │ │ │ │ │ -283template │ │ │ │ │ -_2_8_4std::vector > _c_r_e_a_t_e_U_n_k_n_o_w_n_s(size_t n, char c, size_t start) { │ │ │ │ │ -285 std::vector > unknowns; │ │ │ │ │ -286 unknowns.reserve(n); │ │ │ │ │ -287 for (size_t i = start; i < start + n; i++) │ │ │ │ │ -288 unknowns.push_back(_E_x_p_r_e_s_s_i_o_n_<_T_>(c, i)); │ │ │ │ │ -289 return unknowns; │ │ │ │ │ -290} │ │ │ │ │ -291 │ │ │ │ │ -292template │ │ │ │ │ -293ScalarMultiplyExpression::ScalarMultiplyExpression(double s, const │ │ │ │ │ -Expression& e) │ │ │ │ │ -294 : Expression(boost::make_shared>(s, e)) │ │ │ │ │ -{} │ │ │ │ │ -295 │ │ │ │ │ -296 │ │ │ │ │ -297template │ │ │ │ │ -298BinarySumExpression::BinarySumExpression(const Expression& e1, const │ │ │ │ │ -Expression& e2) │ │ │ │ │ -299 : Expression(boost::_m_a_k_e___s_h_a_r_e_d>(e1, e2)) {} │ │ │ │ │ -300 │ │ │ │ │ -301template │ │ │ │ │ -_3_0_2_E_x_p_r_e_s_s_i_o_n_<_T_>& _E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_o_p_e_r_a_t_o_r_+_=(const _E_x_p_r_e_s_s_i_o_n_<_T_>& e) { │ │ │ │ │ -303 root_ = boost::make_shared>(*this, e); │ │ │ │ │ -304 return *this; │ │ │ │ │ -305} │ │ │ │ │ -306 │ │ │ │ │ -307} // namespace gtsam │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ -std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ -FastVector │ │ │ │ │ -FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ -DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ +42 template │ │ │ │ │ +_4_3 void _a_d_d_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r(const _E_x_p_r_e_s_s_i_o_n_<_T_>& h, const T& z, │ │ │ │ │ +44 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& R) { │ │ │ │ │ +45 using F = _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_<_T_>; │ │ │ │ │ +46 _p_u_s_h___b_a_c_k(boost::allocate_shared(Eigen::aligned_allocator(), R, z, │ │ │ │ │ +h)); │ │ │ │ │ +47 } │ │ │ │ │ +48 │ │ │ │ │ +50}; │ │ │ │ │ +51 │ │ │ │ │ +52} │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Factor Graph consisting of non-linear factors. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_n_s_e_I_n_d_e_x │ │ │ │ │ -ptrdiff_t DenseIndex │ │ │ │ │ -The index type for Eigen objects. │ │ │ │ │ -DDeeffiinniittiioonn types.h:106 │ │ │ │ │ -_g_t_s_a_m_:_:_s_y_m_b_o_l │ │ │ │ │ -Key symbol(unsigned char c, std::uint64_t j) │ │ │ │ │ -Create a symbol key from a character and index, i.e. │ │ │ │ │ -DDeeffiinniittiioonn Symbol.h:135 │ │ │ │ │ -_g_t_s_a_m_:_:_c_r_e_a_t_e_U_n_k_n_o_w_n_s │ │ │ │ │ -std::vector< Expression< T > > createUnknowns(size_t n, char c, size_t start) │ │ │ │ │ -Construct an array of leaves. │ │ │ │ │ -DDeeffiinniittiioonn Expression-inl.h:284 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ -Point2 operator*(double s, const Point2 &p) │ │ │ │ │ -multiply with scalar │ │ │ │ │ -DDeeffiinniittiioonn Point2.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_m_a_k_e___s_h_a_r_e_d │ │ │ │ │ -gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost:: │ │ │ │ │ -shared_ptr< T > > make_shared(Args &&... args) │ │ │ │ │ -Add our own make_shared as a layer of wrapping on boost::make_shared This │ │ │ │ │ -solves the problem with the... │ │ │ │ │ -DDeeffiinniittiioonn make_shared.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ -either a fixed size o... │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -: meta-function to generate JacobianTA optional reference Used mainly by │ │ │ │ │ -Expressions │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:261 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ -of vertical blocks. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l │ │ │ │ │ -Character and index key used to refer to variables. │ │ │ │ │ -DDeeffiinniittiioonn Symbol.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_a_p_p_l_y___c_o_m_p_o_s_e │ │ │ │ │ -DDeeffiinniittiioonn Expression-inl.h:247 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_E_x_e_c_u_t_i_o_n_T_r_a_c_e │ │ │ │ │ -DDeeffiinniittiioonn Expression.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ +Aliases. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_<_ _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_ _>_:_:_p_u_s_h___b_a_c_k │ │ │ │ │ +IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor) │ │ │ │ │ +Add a factor directly using a shared_ptr. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:186 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r │ │ │ │ │ +Factor that supports arbitrary expressions via AD. │ │ │ │ │ +DDeeffiinniittiioonn ExpressionFactor.h:44 │ │ │ │ │ _g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n │ │ │ │ │ Expression class that supports automatic differentiation. │ │ │ │ │ DDeeffiinniittiioonn Expression.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_E_x_p_r_e_s_s_i_o_n │ │ │ │ │ -Expression() │ │ │ │ │ -Default constructor, for serialization. │ │ │ │ │ -DDeeffiinniittiioonn Expression.h:182 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_k_e_y_s │ │ │ │ │ -std::set< Key > keys() const │ │ │ │ │ -Return keys that play in this expression. │ │ │ │ │ -DDeeffiinniittiioonn Expression-inl.h:132 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_K_e_y_s_A_n_d_D_i_m_s │ │ │ │ │ -std::pair< KeyVector, FastVector< int > > KeysAndDims │ │ │ │ │ -Keys and dimensions in same order. │ │ │ │ │ -DDeeffiinniittiioonn Expression.h:185 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_d_i_m_s │ │ │ │ │ -void dims(std::map< Key, int > &map) const │ │ │ │ │ -Return dimensions for each argument, as a map. │ │ │ │ │ -DDeeffiinniittiioonn Expression-inl.h:137 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s) const │ │ │ │ │ -Print. │ │ │ │ │ -DDeeffiinniittiioonn Expression-inl.h:142 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_t_r_a_c_e_S_i_z_e │ │ │ │ │ -size_t traceSize() const │ │ │ │ │ -Return size needed for memory buffer in traceExecution. │ │ │ │ │ -DDeeffiinniittiioonn Expression-inl.h:167 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_t_r_a_c_e_E_x_e_c_u_t_i_o_n │ │ │ │ │ -T traceExecution(const Values &values, internal::ExecutionTrace< T > &trace, │ │ │ │ │ -void *traceStorage) const │ │ │ │ │ -trace execution, very unsafe │ │ │ │ │ -DDeeffiinniittiioonn Expression-inl.h:198 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +Factor graph that supports adding ExpressionFactors directly. │ │ │ │ │ +DDeeffiinniittiioonn ExpressionFactorGraph.h:29 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r │ │ │ │ │ +void addExpressionFactor(const Expression< T > &h, const T &z, const │ │ │ │ │ +SharedNoiseModel &R) │ │ │ │ │ +Directly add ExpressionFactor that implements |h(x)-z|^2_R. │ │ │ │ │ +DDeeffiinniittiioonn ExpressionFactorGraph.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _E_x_p_r_e_s_s_i_o_n_-_i_n_l_._h │ │ │ │ │ + * _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01115_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Result.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearEquality.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,161 +98,402 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ISAM2Result.h
│ │ │ │ +
NonlinearEquality.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18// \callgraph
│ │ │ │ +
12/*
│ │ │ │ +
13 * @file NonlinearEquality.h
│ │ │ │ +
14 * @brief Factor to handle enforced equality between factors
│ │ │ │ +
15 * @author Alex Cunningham
│ │ │ │ +
16 */
│ │ │ │ +
17
│ │ │ │ +
18#pragma once
│ │ │ │
19
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <string>
│ │ │ │ -
23#include <vector>
│ │ │ │ -
24
│ │ │ │ -
25#include <gtsam/linear/GaussianBayesTree.h>
│ │ │ │ -
26#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
│ │ │ │ -
27#include <gtsam/nonlinear/ISAM2Params.h>
│ │ │ │ -
28#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ +
20#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
21#include <gtsam/base/Testable.h>
│ │ │ │ +
22#include <gtsam/base/Manifold.h>
│ │ │ │ +
23
│ │ │ │ +
24#include <boost/bind/bind.hpp>
│ │ │ │ +
25
│ │ │ │ +
26#include <limits>
│ │ │ │ +
27#include <iostream>
│ │ │ │ +
28#include <cmath>
│ │ │ │
29
│ │ │ │ -
30#include <boost/variant.hpp>
│ │ │ │ +
30namespace gtsam {
│ │ │ │
31
│ │ │ │ -
32namespace gtsam {
│ │ │ │ -
33
│ │ │ │ -
│ │ │ │ -
41struct ISAM2Result {
│ │ │ │ -
54 boost::optional<double> errorBefore;
│ │ │ │ -
55
│ │ │ │ -
66 boost::optional<double> errorAfter;
│ │ │ │ -
67
│ │ │ │ -
76 size_t variablesRelinearized;
│ │ │ │ -
77
│ │ │ │ -
84 size_t variablesReeliminated;
│ │ │ │ -
85
│ │ │ │ -
88 size_t factorsRecalculated;
│ │ │ │ -
89
│ │ │ │ -
91 size_t cliques;
│ │ │ │ -
92
│ │ │ │ -
97 FactorIndices newFactorsIndices;
│ │ │ │ -
98
│ │ │ │ -
102 KeySet unusedKeys;
│ │ │ │ -
103
│ │ │ │ -
105 KeyVector observedKeys;
│ │ │ │ -
106
│ │ │ │ -
108 KeySet keysWithRemovedFactors;
│ │ │ │ -
109
│ │ │ │ -
111 KeySet markedKeys;
│ │ │ │ -
112
│ │ │ │ -
│ │ │ │ -
117 struct DetailedResults {
│ │ │ │ -
│ │ │ │ -
120 struct VariableStatus {
│ │ │ │ -
124 bool isReeliminated;
│ │ │ │ -
125 bool isAboveRelinThreshold;
│ │ │ │ -
128 bool isRelinearizeInvolved;
│ │ │ │ -
133 bool isRelinearized;
│ │ │ │ -
136 bool isObserved;
│ │ │ │ -
138 bool isNew;
│ │ │ │ -
139 bool inRootClique;
│ │ │ │ -
140 VariableStatus()
│ │ │ │ -
141 : isReeliminated(false),
│ │ │ │ -
142 isAboveRelinThreshold(false),
│ │ │ │ -
143 isRelinearizeInvolved(false),
│ │ │ │ -
144 isRelinearized(false),
│ │ │ │ -
145 isObserved(false),
│ │ │ │ -
146 isNew(false),
│ │ │ │ -
147 inRootClique(false) {}
│ │ │ │ -
148 };
│ │ │ │ -
│ │ │ │ -
149
│ │ │ │ -
150 using StatusMap = FastMap<Key, VariableStatus>;
│ │ │ │ -
151
│ │ │ │ -
153 StatusMap variableStatus;
│ │ │ │ -
154 };
│ │ │ │ -
│ │ │ │ -
155
│ │ │ │ -
158 boost::optional<DetailedResults> detail;
│ │ │ │ -
159
│ │ │ │ -
160 explicit ISAM2Result(bool enableDetailedResults = false) {
│ │ │ │ -
161 if (enableDetailedResults) detail.reset(DetailedResults());
│ │ │ │ -
162 }
│ │ │ │ -
163
│ │ │ │ -
165 DetailedResults* details() { return detail.get_ptr(); }
│ │ │ │ -
166
│ │ │ │ -
│ │ │ │ -
168 void print(const std::string str = "") const {
│ │ │ │ -
169 using std::cout;
│ │ │ │ -
170 cout << str << " Reelimintated: " << variablesReeliminated
│ │ │ │ -
171 << " Relinearized: " << variablesRelinearized
│ │ │ │ -
172 << " Cliques: " << cliques << std::endl;
│ │ │ │ -
173 }
│ │ │ │ -
│ │ │ │ -
174
│ │ │ │ -
176 size_t getVariablesRelinearized() const { return variablesRelinearized; }
│ │ │ │ -
177 size_t getVariablesReeliminated() const { return variablesReeliminated; }
│ │ │ │ -
178 FactorIndices getNewFactorsIndices() const { return newFactorsIndices; }
│ │ │ │ -
179 size_t getCliques() const { return cliques; }
│ │ │ │ -
180 double getErrorBefore() const { return errorBefore ? *errorBefore : std::nan(""); }
│ │ │ │ -
181 double getErrorAfter() const { return errorAfter ? *errorAfter : std::nan(""); }
│ │ │ │ -
182};
│ │ │ │ -
│ │ │ │ -
183
│ │ │ │ -
184} // namespace gtsam
│ │ │ │ -
GaussianBayesTree.h
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
│ │ │ │ -
ISAM2Params.h
Parameters for iSAM 2.
│ │ │ │ -
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │ -
DoglegOptimizerImpl.h
Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail implementation)
│ │ │ │ +
44template<class VALUE>
│ │ │ │ +
│ │ │ │ +
45class NonlinearEquality: public NoiseModelFactorN<VALUE> {
│ │ │ │ +
46
│ │ │ │ +
47public:
│ │ │ │ +
48 typedef VALUE T;
│ │ │ │ +
49
│ │ │ │ +
50private:
│ │ │ │ +
51
│ │ │ │ +
52 // feasible value
│ │ │ │ +
53 T feasible_;
│ │ │ │ +
54
│ │ │ │ +
55 // error handling flag
│ │ │ │ +
56 bool allow_error_;
│ │ │ │ +
57
│ │ │ │ +
58 // error gain in allow error case
│ │ │ │ +
59 double error_gain_;
│ │ │ │ +
60
│ │ │ │ +
61 // typedef to this class
│ │ │ │ +
62 using This = NonlinearEquality<VALUE>;
│ │ │ │ +
63
│ │ │ │ +
64 // typedef to base class
│ │ │ │ +
65 using Base = NoiseModelFactorN<VALUE>;
│ │ │ │ +
66
│ │ │ │ +
67public:
│ │ │ │ +
68
│ │ │ │ +
70 using CompareFunction = std::function<bool(const T&, const T&)>;
│ │ │ │ +
71 CompareFunction compare_;
│ │ │ │ +
72
│ │ │ │ +
│ │ │ │ +
74 NonlinearEquality() {
│ │ │ │ +
75 }
│ │ │ │ +
│ │ │ │ +
76
│ │ │ │ +
77 ~NonlinearEquality() override {
│ │ │ │ +
78 }
│ │ │ │ +
79
│ │ │ │ +
82
│ │ │ │ +
│ │ │ │ +
86 NonlinearEquality(Key j, const T& feasible,
│ │ │ │ +
87 const CompareFunction &_compare = std::bind(traits<T>::Equals,
│ │ │ │ +
88 std::placeholders::_1, std::placeholders::_2, 1e-9)) :
│ │ │ │ +
89 Base(noiseModel::Constrained::All(traits<T>::GetDimension(feasible)),
│ │ │ │ +
90 j), feasible_(feasible), allow_error_(false), error_gain_(0.0), //
│ │ │ │ +
91 compare_(_compare) {
│ │ │ │ +
92 }
│ │ │ │ +
│ │ │ │ +
93
│ │ │ │ +
│ │ │ │ +
97 NonlinearEquality(Key j, const T& feasible, double error_gain,
│ │ │ │ +
98 const CompareFunction &_compare = std::bind(traits<T>::Equals,
│ │ │ │ +
99 std::placeholders::_1, std::placeholders::_2, 1e-9)) :
│ │ │ │ +
100 Base(noiseModel::Constrained::All(traits<T>::GetDimension(feasible)),
│ │ │ │ +
101 j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), //
│ │ │ │ +
102 compare_(_compare) {
│ │ │ │ +
103 }
│ │ │ │ +
│ │ │ │ +
104
│ │ │ │ +
108
│ │ │ │ +
│ │ │ │ +
109 void print(const std::string& s = "",
│ │ │ │ +
110 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ +
111 std::cout << (s.empty() ? s : s + " ") << "Constraint: on ["
│ │ │ │ +
112 << keyFormatter(this->key()) << "]\n";
│ │ │ │ +
113 traits<VALUE>::Print(feasible_, "Feasible Point:\n");
│ │ │ │ +
114 std::cout << "Variable Dimension: " << traits<T>::GetDimension(feasible_)
│ │ │ │ +
115 << std::endl;
│ │ │ │ +
116 }
│ │ │ │ +
│ │ │ │ +
117
│ │ │ │ +
│ │ │ │ +
119 bool equals(const NonlinearFactor& f, double tol = 1e-9) const override {
│ │ │ │ +
120 const This* e = dynamic_cast<const This*>(&f);
│ │ │ │ +
121 return e && Base::equals(f) && traits<T>::Equals(feasible_,e->feasible_, tol)
│ │ │ │ +
122 && std::abs(error_gain_ - e->error_gain_) < tol;
│ │ │ │ +
123 }
│ │ │ │ +
│ │ │ │ +
124
│ │ │ │ +
128
│ │ │ │ +
│ │ │ │ +
130 double error(const Values& c) const override {
│ │ │ │ +
131 const T& xj = c.at<T>(this->key());
│ │ │ │ +
132 Vector e = this->unwhitenedError(c);
│ │ │ │ +
133 if (allow_error_ || !compare_(xj, feasible_)) {
│ │ │ │ +
134 return error_gain_ * dot(e, e);
│ │ │ │ +
135 } else {
│ │ │ │ +
136 return 0.0;
│ │ │ │ +
137 }
│ │ │ │ +
138 }
│ │ │ │ +
│ │ │ │ +
139
│ │ │ │ +
│ │ │ │ +
141 Vector evaluateError(const T& xj,
│ │ │ │ +
142 boost::optional<Matrix&> H = boost::none) const override {
│ │ │ │ +
143 const size_t nj = traits<T>::GetDimension(feasible_);
│ │ │ │ +
144 if (allow_error_) {
│ │ │ │ +
145 if (H)
│ │ │ │ +
146 *H = Matrix::Identity(nj,nj); // FIXME: this is not the right linearization for nonlinear compare
│ │ │ │ +
147 return traits<T>::Local(xj,feasible_);
│ │ │ │ +
148 } else if (compare_(feasible_, xj)) {
│ │ │ │ +
149 if (H)
│ │ │ │ +
150 *H = Matrix::Identity(nj,nj);
│ │ │ │ +
151 return Vector::Zero(nj); // set error to zero if equal
│ │ │ │ +
152 } else {
│ │ │ │ +
153 if (H)
│ │ │ │ +
154 throw std::invalid_argument(
│ │ │ │ +
155 "Linearization point not feasible for "
│ │ │ │ +
156 + DefaultKeyFormatter(this->key()) + "!");
│ │ │ │ +
157 return Vector::Constant(nj, std::numeric_limits<double>::infinity()); // set error to infinity if not equal
│ │ │ │ +
158 }
│ │ │ │ +
159 }
│ │ │ │ +
│ │ │ │ +
160
│ │ │ │ +
│ │ │ │ +
162 GaussianFactor::shared_ptr linearize(const Values& x) const override {
│ │ │ │ +
163 const T& xj = x.at<T>(this->key());
│ │ │ │ +
164 Matrix A;
│ │ │ │ +
165 Vector b = evaluateError(xj, A);
│ │ │ │ +
166 SharedDiagonal model = noiseModel::Constrained::All(b.size());
│ │ │ │ +
167 return GaussianFactor::shared_ptr(
│ │ │ │ +
168 new JacobianFactor(this->key(), A, b, model));
│ │ │ │ +
169 }
│ │ │ │ +
│ │ │ │ +
170
│ │ │ │ +
│ │ │ │ +
172 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
173 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ +
174 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
│ │ │ │ +
175 }
│ │ │ │ +
│ │ │ │ +
176
│ │ │ │ +
178
│ │ │ │ +
179 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
180
│ │ │ │ +
181private:
│ │ │ │ +
182
│ │ │ │ +
184 friend class boost::serialization::access;
│ │ │ │ +
185 template<class ARCHIVE>
│ │ │ │ +
186 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
187 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ +
188 ar
│ │ │ │ +
189 & boost::serialization::make_nvp("NoiseModelFactor1",
│ │ │ │ +
190 boost::serialization::base_object<Base>(*this));
│ │ │ │ +
191 ar & BOOST_SERIALIZATION_NVP(feasible_);
│ │ │ │ +
192 ar & BOOST_SERIALIZATION_NVP(allow_error_);
│ │ │ │ +
193 ar & BOOST_SERIALIZATION_NVP(error_gain_);
│ │ │ │ +
194 }
│ │ │ │ +
195
│ │ │ │ +
196};
│ │ │ │ +
│ │ │ │ +
197// \class NonlinearEquality
│ │ │ │ +
198
│ │ │ │ +
199template <typename VALUE>
│ │ │ │ +
200struct traits<NonlinearEquality<VALUE>> : Testable<NonlinearEquality<VALUE>> {};
│ │ │ │ +
201
│ │ │ │ +
202/* ************************************************************************* */
│ │ │ │ +
206template<class VALUE>
│ │ │ │ +
│ │ │ │ +
207class NonlinearEquality1: public NoiseModelFactorN<VALUE> {
│ │ │ │ +
208
│ │ │ │ +
209public:
│ │ │ │ +
210 typedef VALUE X;
│ │ │ │ +
211
│ │ │ │ +
212protected:
│ │ │ │ +
213 typedef NoiseModelFactorN<VALUE> Base;
│ │ │ │ +
214 typedef NonlinearEquality1<VALUE> This;
│ │ │ │ +
215
│ │ │ │ +
│ │ │ │ +
217 NonlinearEquality1() {
│ │ │ │ +
218 }
│ │ │ │ +
│ │ │ │ +
219
│ │ │ │ +
220 X value_;
│ │ │ │ +
221
│ │ │ │ +
222 GTSAM_CONCEPT_MANIFOLD_TYPE(X)
│ │ │ │ +
223 GTSAM_CONCEPT_TESTABLE_TYPE(X)
│ │ │ │ +
224
│ │ │ │ +
225public:
│ │ │ │ +
226
│ │ │ │ +
227 typedef boost::shared_ptr<NonlinearEquality1<VALUE> > shared_ptr;
│ │ │ │ +
228
│ │ │ │ +
│ │ │ │ +
235 NonlinearEquality1(const X& value, Key key, double mu = 1000.0)
│ │ │ │ +
236 : Base(noiseModel::Constrained::All(traits<X>::GetDimension(value),
│ │ │ │ +
237 std::abs(mu)),
│ │ │ │ +
238 key),
│ │ │ │ +
239 value_(value) {}
│ │ │ │ +
│ │ │ │ +
240
│ │ │ │ +
241 ~NonlinearEquality1() override {
│ │ │ │ +
242 }
│ │ │ │ +
243
│ │ │ │ +
│ │ │ │ +
245 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
246 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ +
247 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
│ │ │ │ +
248 }
│ │ │ │ +
│ │ │ │ +
249
│ │ │ │ +
│ │ │ │ +
251 Vector evaluateError(const X& x1,
│ │ │ │ +
252 boost::optional<Matrix&> H = boost::none) const override {
│ │ │ │ +
253 if (H)
│ │ │ │ +
254 (*H) = Matrix::Identity(traits<X>::GetDimension(x1),traits<X>::GetDimension(x1));
│ │ │ │ +
255 // manifold equivalent of h(x)-z -> log(z,h(x))
│ │ │ │ +
256 return traits<X>::Local(value_,x1);
│ │ │ │ +
257 }
│ │ │ │ +
│ │ │ │ +
258
│ │ │ │ +
│ │ │ │ +
260 void print(const std::string& s = "",
│ │ │ │ +
261 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ +
262 std::cout << s << ": NonlinearEquality1(" << keyFormatter(this->key())
│ │ │ │ +
263 << ")," << "\n";
│ │ │ │ +
264 this->noiseModel_->print();
│ │ │ │ +
265 traits<X>::Print(value_, "Value");
│ │ │ │ +
266 }
│ │ │ │ +
│ │ │ │ +
267
│ │ │ │ +
268 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
269
│ │ │ │ +
270private:
│ │ │ │ +
271
│ │ │ │ +
273 friend class boost::serialization::access;
│ │ │ │ +
274 template<class ARCHIVE>
│ │ │ │ +
275 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
276 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ +
277 ar
│ │ │ │ +
278 & boost::serialization::make_nvp("NoiseModelFactor1",
│ │ │ │ +
279 boost::serialization::base_object<Base>(*this));
│ │ │ │ +
280 ar & BOOST_SERIALIZATION_NVP(value_);
│ │ │ │ +
281 }
│ │ │ │ +
282};
│ │ │ │ +
│ │ │ │ +
283// \NonlinearEquality1
│ │ │ │ +
284
│ │ │ │ +
285template <typename VALUE>
│ │ │ │ +
│ │ │ │ +
286struct traits<NonlinearEquality1<VALUE> >
│ │ │ │ +
287 : Testable<NonlinearEquality1<VALUE> > {};
│ │ │ │ +
│ │ │ │ +
288
│ │ │ │ +
289/* ************************************************************************* */
│ │ │ │ +
294template <class T>
│ │ │ │ +
│ │ │ │ +
295class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
│ │ │ │ +
296 protected:
│ │ │ │ +
297 using Base = NoiseModelFactorN<T, T>;
│ │ │ │ +
298 using This = NonlinearEquality2<T>;
│ │ │ │ +
299
│ │ │ │ +
300 GTSAM_CONCEPT_MANIFOLD_TYPE(T)
│ │ │ │ +
301
│ │ │ │ +
302
│ │ │ │ +
303 NonlinearEquality2() {}
│ │ │ │ +
304
│ │ │ │ +
305 public:
│ │ │ │ +
306 typedef boost::shared_ptr<NonlinearEquality2<T>> shared_ptr;
│ │ │ │ +
307
│ │ │ │ +
│ │ │ │ +
314 NonlinearEquality2(Key key1, Key key2, double mu = 1e4)
│ │ │ │ +
315 : Base(noiseModel::Constrained::All(traits<T>::dimension, std::abs(mu)),
│ │ │ │ +
316 key1, key2) {}
│ │ │ │ +
│ │ │ │ +
317 ~NonlinearEquality2() override {}
│ │ │ │ +
318
│ │ │ │ +
│ │ │ │ +
320 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
321 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ +
322 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
│ │ │ │ +
323 }
│ │ │ │ +
│ │ │ │ +
324
│ │ │ │ +
│ │ │ │ +
326 Vector evaluateError(
│ │ │ │ +
327 const T& x1, const T& x2, boost::optional<Matrix&> H1 = boost::none,
│ │ │ │ +
328 boost::optional<Matrix&> H2 = boost::none) const override {
│ │ │ │ +
329 static const size_t p = traits<T>::dimension;
│ │ │ │ +
330 if (H1) *H1 = -Matrix::Identity(p, p);
│ │ │ │ +
331 if (H2) *H2 = Matrix::Identity(p, p);
│ │ │ │ +
332 return traits<T>::Local(x1, x2);
│ │ │ │ +
333 }
│ │ │ │ +
│ │ │ │ +
334
│ │ │ │ +
335 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
336
│ │ │ │ +
337 private:
│ │ │ │ +
339 friend class boost::serialization::access;
│ │ │ │ +
340 template <class ARCHIVE>
│ │ │ │ +
341 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ +
342 // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ +
343 ar& boost::serialization::make_nvp(
│ │ │ │ +
344 "NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
│ │ │ │ +
345 }
│ │ │ │ +
346};
│ │ │ │ +
│ │ │ │ +
347// \NonlinearEquality2
│ │ │ │ +
348
│ │ │ │ +
349template <typename VALUE>
│ │ │ │ +
│ │ │ │ +
350struct traits<NonlinearEquality2<VALUE>> : Testable<NonlinearEquality2<VALUE>> {
│ │ │ │ +
351};
│ │ │ │ +
│ │ │ │ +
352
│ │ │ │ +
353}// namespace gtsam
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ +
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition Factor.h:34
│ │ │ │ -
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ - │ │ │ │ -
gtsam::ISAM2Result
This struct is returned from ISAM2::update() and contains information about the update that is useful...
Definition ISAM2Result.h:41
│ │ │ │ -
gtsam::ISAM2Result::variablesRelinearized
size_t variablesRelinearized
The number of variables that were relinearized because their linear deltas exceeded the reslinearizat...
Definition ISAM2Result.h:76
│ │ │ │ -
gtsam::ISAM2Result::getVariablesRelinearized
size_t getVariablesRelinearized() const
Getters and Setters.
Definition ISAM2Result.h:176
│ │ │ │ -
gtsam::ISAM2Result::factorsRecalculated
size_t factorsRecalculated
The number of factors that were included in reelimination of the Bayes' tree.
Definition ISAM2Result.h:88
│ │ │ │ -
gtsam::ISAM2Result::details
DetailedResults * details()
Return pointer to detail, 0 if no detail requested.
Definition ISAM2Result.h:165
│ │ │ │ -
gtsam::ISAM2Result::errorBefore
boost::optional< double > errorBefore
The nonlinear error of all of the factors, including new factors and variables added during the curre...
Definition ISAM2Result.h:54
│ │ │ │ -
gtsam::ISAM2Result::print
void print(const std::string str="") const
Print results.
Definition ISAM2Result.h:168
│ │ │ │ -
gtsam::ISAM2Result::detail
boost::optional< DetailedResults > detail
Detailed results, if enabled by ISAM2Params::enableDetailedResults.
Definition ISAM2Result.h:158
│ │ │ │ -
gtsam::ISAM2Result::cliques
size_t cliques
The number of cliques in the Bayes' Tree.
Definition ISAM2Result.h:91
│ │ │ │ -
gtsam::ISAM2Result::keysWithRemovedFactors
KeySet keysWithRemovedFactors
Keys of variables that had factors removed.
Definition ISAM2Result.h:108
│ │ │ │ -
gtsam::ISAM2Result::variablesReeliminated
size_t variablesReeliminated
The number of variables that were reeliminated as parts of the Bayes' Tree were recalculated,...
Definition ISAM2Result.h:84
│ │ │ │ -
gtsam::ISAM2Result::unusedKeys
KeySet unusedKeys
Unused keys, and indices for unused keys, i.e., keys that are empty now and do not appear in the new ...
Definition ISAM2Result.h:102
│ │ │ │ -
gtsam::ISAM2Result::observedKeys
KeyVector observedKeys
keys for variables that were observed, i.e., not unused.
Definition ISAM2Result.h:105
│ │ │ │ -
gtsam::ISAM2Result::markedKeys
KeySet markedKeys
All keys that were marked during the update process.
Definition ISAM2Result.h:111
│ │ │ │ -
gtsam::ISAM2Result::newFactorsIndices
FactorIndices newFactorsIndices
The indices of the newly-added factors, in 1-to-1 correspondence with the factors passed as newFactor...
Definition ISAM2Result.h:97
│ │ │ │ -
gtsam::ISAM2Result::errorAfter
boost::optional< double > errorAfter
The nonlinear error of all of the factors computed after the current update, meaning that variables a...
Definition ISAM2Result.h:66
│ │ │ │ -
gtsam::ISAM2Result::DetailedResults
A struct holding detailed results, which must be enabled with ISAM2Params::enableDetailedResults.
Definition ISAM2Result.h:117
│ │ │ │ -
gtsam::ISAM2Result::DetailedResults::variableStatus
StatusMap variableStatus
The status of each variable during this update, see VariableStatus.
Definition ISAM2Result.h:153
│ │ │ │ -
gtsam::ISAM2Result::DetailedResults::VariableStatus
The status of a single variable, this struct is stored in DetailedResults::variableStatus.
Definition ISAM2Result.h:120
│ │ │ │ -
gtsam::ISAM2Result::DetailedResults::VariableStatus::isRelinearizeInvolved
bool isRelinearizeInvolved
Whether the variable was below the relinearization threshold but was relinearized by being involved i...
Definition ISAM2Result.h:128
│ │ │ │ -
gtsam::ISAM2Result::DetailedResults::VariableStatus::isObserved
bool isObserved
Whether the variable was relinearized, either by being above the relinearization threshold or by invo...
Definition ISAM2Result.h:136
│ │ │ │ -
gtsam::ISAM2Result::DetailedResults::VariableStatus::isNew
bool isNew
Whether the variable itself was just added.
Definition ISAM2Result.h:138
│ │ │ │ -
gtsam::ISAM2Result::DetailedResults::VariableStatus::isAboveRelinThreshold
bool isAboveRelinThreshold
Whether the variable was just relinearized due to being above the relinearization threshold.
Definition ISAM2Result.h:125
│ │ │ │ -
gtsam::ISAM2Result::DetailedResults::VariableStatus::isReeliminated
bool isReeliminated
Whether the variable was just reeliminated, due to being relinearized, observed, new,...
Definition ISAM2Result.h:124
│ │ │ │ -
gtsam::ISAM2Result::DetailedResults::VariableStatus::inRootClique
bool inRootClique
Whether the variable is in the root clique.
Definition ISAM2Result.h:139
│ │ │ │ +
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
│ │ │ │ +
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactor.h:42
│ │ │ │ +
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition JacobianFactor.h:91
│ │ │ │ +
gtsam::noiseModel::Constrained::All
static shared_ptr All(size_t dim)
Fully constrained variations.
Definition NoiseModel.h:457
│ │ │ │ +
gtsam::NonlinearEquality
An equality factor that forces either one variable to a constant, or a set of variables to be equal t...
Definition NonlinearEquality.h:45
│ │ │ │ +
gtsam::NonlinearEquality::error
double error(const Values &c) const override
Actual error function calculation.
Definition NonlinearEquality.h:130
│ │ │ │ +
gtsam::NonlinearEquality::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition NonlinearEquality.h:109
│ │ │ │ +
gtsam::NonlinearEquality::CompareFunction
std::function< bool(const T &, const T &)> CompareFunction
Function that compares two values.
Definition NonlinearEquality.h:70
│ │ │ │ +
gtsam::NonlinearEquality::NonlinearEquality
NonlinearEquality()
Default constructor - only for serialization.
Definition NonlinearEquality.h:74
│ │ │ │ +
gtsam::NonlinearEquality::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Check if two factors are equal.
Definition NonlinearEquality.h:119
│ │ │ │ +
gtsam::NonlinearEquality::NonlinearEquality
NonlinearEquality(Key j, const T &feasible, double error_gain, const CompareFunction &_compare=std::bind(traits< T >::Equals, std::placeholders::_1, std::placeholders::_2, 1e-9))
Constructor - allows inexact evaluation.
Definition NonlinearEquality.h:97
│ │ │ │ +
gtsam::NonlinearEquality::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition NonlinearEquality.h:172
│ │ │ │ +
gtsam::NonlinearEquality::evaluateError
Vector evaluateError(const T &xj, boost::optional< Matrix & > H=boost::none) const override
Error function.
Definition NonlinearEquality.h:141
│ │ │ │ +
gtsam::NonlinearEquality::access
friend class boost::serialization::access
Serialization function.
Definition NonlinearEquality.h:184
│ │ │ │ +
gtsam::NonlinearEquality::NonlinearEquality
NonlinearEquality(Key j, const T &feasible, const CompareFunction &_compare=std::bind(traits< T >::Equals, std::placeholders::_1, std::placeholders::_2, 1e-9))
Constructor - forces exact evaluation.
Definition NonlinearEquality.h:86
│ │ │ │ +
gtsam::NonlinearEquality::linearize
GaussianFactor::shared_ptr linearize(const Values &x) const override
Linearize is over-written, because base linearization tries to whiten.
Definition NonlinearEquality.h:162
│ │ │ │ +
gtsam::NonlinearEquality1
Simple unary equality constraint - fixes a value for a variable.
Definition NonlinearEquality.h:207
│ │ │ │ +
gtsam::NonlinearEquality1::evaluateError
Vector evaluateError(const X &x1, boost::optional< Matrix & > H=boost::none) const override
g(x) with optional derivative
Definition NonlinearEquality.h:251
│ │ │ │ +
gtsam::NonlinearEquality1::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition NonlinearEquality.h:245
│ │ │ │ +
gtsam::NonlinearEquality1::shared_ptr
boost::shared_ptr< NonlinearEquality1< VALUE > > shared_ptr
fixed value for variable
Definition NonlinearEquality.h:227
│ │ │ │ +
gtsam::NonlinearEquality1::NonlinearEquality1
NonlinearEquality1(const X &value, Key key, double mu=1000.0)
Constructor.
Definition NonlinearEquality.h:235
│ │ │ │ +
gtsam::NonlinearEquality1::NonlinearEquality1
NonlinearEquality1()
Default constructor to allow for serialization.
Definition NonlinearEquality.h:217
│ │ │ │ +
gtsam::NonlinearEquality1::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition NonlinearEquality.h:260
│ │ │ │ +
gtsam::NonlinearEquality1::access
friend class boost::serialization::access
Serialization function.
Definition NonlinearEquality.h:273
│ │ │ │ +
gtsam::NonlinearEquality2
Simple binary equality constraint - this constraint forces two variables to be the same.
Definition NonlinearEquality.h:295
│ │ │ │ +
gtsam::NonlinearEquality2::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition NonlinearEquality.h:320
│ │ │ │ +
gtsam::NonlinearEquality2::evaluateError
Vector evaluateError(const T &x1, const T &x2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
g(x) with optional derivative2
Definition NonlinearEquality.h:326
│ │ │ │ +
gtsam::NonlinearEquality2::NonlinearEquality2
NonlinearEquality2(Key key1, Key key2, double mu=1e4)
Constructor.
Definition NonlinearEquality.h:314
│ │ │ │ +
gtsam::NonlinearEquality2::access
friend class boost::serialization::access
Serialization function.
Definition NonlinearEquality.h:339
│ │ │ │ +
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ +
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition NonlinearFactor.h:223
│ │ │ │ +
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ +
gtsam::NoiseModelFactorN< VALUE >::unwhitenedError
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
This implements the unwhitenedError virtual function by calling the n-key specific version of evaluat...
Definition NonlinearFactor.h:542
│ │ │ │ +
gtsam::NoiseModelFactorN< VALUE >::key
Key key() const
Returns a key.
Definition NonlinearFactor.h:518
│ │ │ │ +
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
gtsam::Values::at
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,248 +1,494 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ISAM2Result.h │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +NonlinearEquality.h │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18// \callgraph │ │ │ │ │ +12/* │ │ │ │ │ +13 * @file NonlinearEquality.h │ │ │ │ │ +14 * @brief Factor to handle enforced equality between factors │ │ │ │ │ +15 * @author Alex Cunningham │ │ │ │ │ +16 */ │ │ │ │ │ +17 │ │ │ │ │ +18#pragma once │ │ │ │ │ 19 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include │ │ │ │ │ -23#include │ │ │ │ │ -24 │ │ │ │ │ -25#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_._h> │ │ │ │ │ -27#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_I_S_A_M_2_P_a_r_a_m_s_._h> │ │ │ │ │ -28#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +20#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ +23 │ │ │ │ │ +24#include │ │ │ │ │ +25 │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28#include │ │ │ │ │ 29 │ │ │ │ │ -30#include │ │ │ │ │ +30namespace _g_t_s_a_m { │ │ │ │ │ 31 │ │ │ │ │ -32namespace _g_t_s_a_m { │ │ │ │ │ -33 │ │ │ │ │ -_4_1struct _I_S_A_M_2_R_e_s_u_l_t { │ │ │ │ │ -_5_4 boost::optional _e_r_r_o_r_B_e_f_o_r_e; │ │ │ │ │ -55 │ │ │ │ │ -_6_6 boost::optional _e_r_r_o_r_A_f_t_e_r; │ │ │ │ │ -67 │ │ │ │ │ -_7_6 size_t _v_a_r_i_a_b_l_e_s_R_e_l_i_n_e_a_r_i_z_e_d; │ │ │ │ │ -77 │ │ │ │ │ -_8_4 size_t _v_a_r_i_a_b_l_e_s_R_e_e_l_i_m_i_n_a_t_e_d; │ │ │ │ │ -85 │ │ │ │ │ -_8_8 size_t _f_a_c_t_o_r_s_R_e_c_a_l_c_u_l_a_t_e_d; │ │ │ │ │ -89 │ │ │ │ │ -_9_1 size_t _c_l_i_q_u_e_s; │ │ │ │ │ -92 │ │ │ │ │ -_9_7 _F_a_c_t_o_r_I_n_d_i_c_e_s _n_e_w_F_a_c_t_o_r_s_I_n_d_i_c_e_s; │ │ │ │ │ -98 │ │ │ │ │ -_1_0_2 _K_e_y_S_e_t _u_n_u_s_e_d_K_e_y_s; │ │ │ │ │ -103 │ │ │ │ │ -_1_0_5 _K_e_y_V_e_c_t_o_r _o_b_s_e_r_v_e_d_K_e_y_s; │ │ │ │ │ -106 │ │ │ │ │ -_1_0_8 _K_e_y_S_e_t _k_e_y_s_W_i_t_h_R_e_m_o_v_e_d_F_a_c_t_o_r_s; │ │ │ │ │ -109 │ │ │ │ │ -_1_1_1 _K_e_y_S_e_t _m_a_r_k_e_d_K_e_y_s; │ │ │ │ │ -112 │ │ │ │ │ -_1_1_7 struct _D_e_t_a_i_l_e_d_R_e_s_u_l_t_s { │ │ │ │ │ -_1_2_0 struct _V_a_r_i_a_b_l_e_S_t_a_t_u_s { │ │ │ │ │ -_1_2_4 bool _i_s_R_e_e_l_i_m_i_n_a_t_e_d; │ │ │ │ │ -_1_2_5 bool _i_s_A_b_o_v_e_R_e_l_i_n_T_h_r_e_s_h_o_l_d; │ │ │ │ │ -_1_2_8 bool _i_s_R_e_l_i_n_e_a_r_i_z_e_I_n_v_o_l_v_e_d; │ │ │ │ │ -133 bool isRelinearized; │ │ │ │ │ -_1_3_6 bool _i_s_O_b_s_e_r_v_e_d; │ │ │ │ │ -_1_3_8 bool _i_s_N_e_w; │ │ │ │ │ -_1_3_9 bool _i_n_R_o_o_t_C_l_i_q_u_e; │ │ │ │ │ -140 _V_a_r_i_a_b_l_e_S_t_a_t_u_s() │ │ │ │ │ -141 : _i_s_R_e_e_l_i_m_i_n_a_t_e_d(false), │ │ │ │ │ -142 _i_s_A_b_o_v_e_R_e_l_i_n_T_h_r_e_s_h_o_l_d(false), │ │ │ │ │ -143 _i_s_R_e_l_i_n_e_a_r_i_z_e_I_n_v_o_l_v_e_d(false), │ │ │ │ │ -144 isRelinearized(false), │ │ │ │ │ -145 _i_s_O_b_s_e_r_v_e_d(false), │ │ │ │ │ -146 _i_s_N_e_w(false), │ │ │ │ │ -147 _i_n_R_o_o_t_C_l_i_q_u_e(false) {} │ │ │ │ │ -148 }; │ │ │ │ │ -149 │ │ │ │ │ -150 using StatusMap = _F_a_s_t_M_a_p_<_K_e_y_,_ _V_a_r_i_a_b_l_e_S_t_a_t_u_s_>; │ │ │ │ │ -151 │ │ │ │ │ -_1_5_3 _S_t_a_t_u_s_M_a_p _v_a_r_i_a_b_l_e_S_t_a_t_u_s; │ │ │ │ │ -154 }; │ │ │ │ │ -155 │ │ │ │ │ -_1_5_8 boost::optional _d_e_t_a_i_l; │ │ │ │ │ -159 │ │ │ │ │ -160 explicit _I_S_A_M_2_R_e_s_u_l_t(bool enableDetailedResults = false) { │ │ │ │ │ -161 if (enableDetailedResults) _d_e_t_a_i_l.reset(_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s()); │ │ │ │ │ -162 } │ │ │ │ │ -163 │ │ │ │ │ -_1_6_5 _D_e_t_a_i_l_e_d_R_e_s_u_l_t_s* _d_e_t_a_i_l_s() { return _d_e_t_a_i_l.get_ptr(); } │ │ │ │ │ -166 │ │ │ │ │ -_1_6_8 void _p_r_i_n_t(const std::string str = "") const { │ │ │ │ │ -169 using std::cout; │ │ │ │ │ -170 cout << str << " Reelimintated: " << _v_a_r_i_a_b_l_e_s_R_e_e_l_i_m_i_n_a_t_e_d │ │ │ │ │ -171 << " Relinearized: " << _v_a_r_i_a_b_l_e_s_R_e_l_i_n_e_a_r_i_z_e_d │ │ │ │ │ -172 << " Cliques: " << _c_l_i_q_u_e_s << std::endl; │ │ │ │ │ -173 } │ │ │ │ │ -174 │ │ │ │ │ -_1_7_6 size_t _g_e_t_V_a_r_i_a_b_l_e_s_R_e_l_i_n_e_a_r_i_z_e_d() const { return _v_a_r_i_a_b_l_e_s_R_e_l_i_n_e_a_r_i_z_e_d; } │ │ │ │ │ -177 size_t getVariablesReeliminated() const { return _v_a_r_i_a_b_l_e_s_R_e_e_l_i_m_i_n_a_t_e_d; } │ │ │ │ │ -178 _F_a_c_t_o_r_I_n_d_i_c_e_s getNewFactorsIndices() const { return _n_e_w_F_a_c_t_o_r_s_I_n_d_i_c_e_s; } │ │ │ │ │ -179 size_t getCliques() const { return _c_l_i_q_u_e_s; } │ │ │ │ │ -180 double getErrorBefore() const { return _e_r_r_o_r_B_e_f_o_r_e ? *_e_r_r_o_r_B_e_f_o_r_e : std:: │ │ │ │ │ -nan(""); } │ │ │ │ │ -181 double getErrorAfter() const { return _e_r_r_o_r_A_f_t_e_r ? *_e_r_r_o_r_A_f_t_e_r : std::nan │ │ │ │ │ -(""); } │ │ │ │ │ -182}; │ │ │ │ │ -183 │ │ │ │ │ -184} // namespace gtsam │ │ │ │ │ -_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ -Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ │ -_I_S_A_M_2_P_a_r_a_m_s_._h │ │ │ │ │ -Parameters for iSAM 2. │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Factor Graph consisting of non-linear factors. │ │ │ │ │ -_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_._h │ │ │ │ │ -Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail │ │ │ │ │ -implementation) │ │ │ │ │ +44template │ │ │ │ │ +_4_5class _N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +46 │ │ │ │ │ +47public: │ │ │ │ │ +48 typedef VALUE T; │ │ │ │ │ +49 │ │ │ │ │ +50private: │ │ │ │ │ +51 │ │ │ │ │ +52 // feasible value │ │ │ │ │ +53 T feasible_; │ │ │ │ │ +54 │ │ │ │ │ +55 // error handling flag │ │ │ │ │ +56 bool allow_error_; │ │ │ │ │ +57 │ │ │ │ │ +58 // error gain in allow error case │ │ │ │ │ +59 double error_gain_; │ │ │ │ │ +60 │ │ │ │ │ +61 // typedef to this class │ │ │ │ │ +62 using This = _N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_<_V_A_L_U_E_>; │ │ │ │ │ +63 │ │ │ │ │ +64 // typedef to base class │ │ │ │ │ +65 using _B_a_s_e = _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_V_A_L_U_E_>; │ │ │ │ │ +66 │ │ │ │ │ +67public: │ │ │ │ │ +68 │ │ │ │ │ +_7_0 using _C_o_m_p_a_r_e_F_u_n_c_t_i_o_n = std::function; │ │ │ │ │ +71 _C_o_m_p_a_r_e_F_u_n_c_t_i_o_n compare_; │ │ │ │ │ +72 │ │ │ │ │ +_7_4 _N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y() { │ │ │ │ │ +75 } │ │ │ │ │ +76 │ │ │ │ │ +77 _~_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y() override { │ │ │ │ │ +78 } │ │ │ │ │ +79 │ │ │ │ │ +82 │ │ │ │ │ +_8_6 _N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y(_K_e_y j, const T& feasible, │ │ │ │ │ +87 const _C_o_m_p_a_r_e_F_u_n_c_t_i_o_n &_compare = std::bind(_t_r_a_i_t_s_<_T_>_:_:_E_q_u_a_l_s, │ │ │ │ │ +88 std::placeholders::_1, std::placeholders::_2, 1e-9)) : │ │ │ │ │ +89 _B_a_s_e(_n_o_i_s_e_M_o_d_e_l::Constrained::All(_t_r_a_i_t_s::GetDimension(feasible)), │ │ │ │ │ +90 j), feasible_(feasible), allow_error_(false), error_gain_(0.0), // │ │ │ │ │ +91 compare_(_compare) { │ │ │ │ │ +92 } │ │ │ │ │ +93 │ │ │ │ │ +_9_7 _N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y(_K_e_y j, const T& feasible, double error_gain, │ │ │ │ │ +98 const _C_o_m_p_a_r_e_F_u_n_c_t_i_o_n &_compare = std::bind(_t_r_a_i_t_s_<_T_>_:_:_E_q_u_a_l_s, │ │ │ │ │ +99 std::placeholders::_1, std::placeholders::_2, 1e-9)) : │ │ │ │ │ +100 _B_a_s_e(_n_o_i_s_e_M_o_d_e_l::Constrained::All(_t_r_a_i_t_s::GetDimension(feasible)), │ │ │ │ │ +101 j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), // │ │ │ │ │ +102 compare_(_compare) { │ │ │ │ │ +103 } │ │ │ │ │ +104 │ │ │ │ │ +108 │ │ │ │ │ +_1_0_9 void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ +110 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ +111 std::cout << (s.empty() ? s : s + " ") << "Constraint: on [" │ │ │ │ │ +112 << keyFormatter(this->_k_e_y()) << "]\n"; │ │ │ │ │ +113 _t_r_a_i_t_s_<_V_A_L_U_E_>_:_:_P_r_i_n_t(feasible_, "Feasible Point:\n"); │ │ │ │ │ +114 std::cout << "Variable Dimension: " << _t_r_a_i_t_s_<_T_>_:_:_G_e_t_D_i_m_e_n_s_i_o_n(feasible_) │ │ │ │ │ +115 << std::endl; │ │ │ │ │ +116 } │ │ │ │ │ +117 │ │ │ │ │ +_1_1_9 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& f, double tol = 1e-9) const override { │ │ │ │ │ +120 const This* e = dynamic_cast(&f); │ │ │ │ │ +121 return e && _B_a_s_e_:_:_e_q_u_a_l_s(f) && _t_r_a_i_t_s_<_T_>_:_:_E_q_u_a_l_s(feasible_,e->feasible_, │ │ │ │ │ +tol) │ │ │ │ │ +122 && std::abs(error_gain_ - e->error_gain_) < tol; │ │ │ │ │ +123 } │ │ │ │ │ +124 │ │ │ │ │ +128 │ │ │ │ │ +_1_3_0 double _e_r_r_o_r(const _V_a_l_u_e_s& c) const override { │ │ │ │ │ +131 const T& xj = c._a_t(this->_k_e_y()); │ │ │ │ │ +132 Vector e = this->_u_n_w_h_i_t_e_n_e_d_E_r_r_o_r(c); │ │ │ │ │ +133 if (allow_error_ || !compare_(xj, feasible_)) { │ │ │ │ │ +134 return error_gain_ * _d_o_t(e, e); │ │ │ │ │ +135 } else { │ │ │ │ │ +136 return 0.0; │ │ │ │ │ +137 } │ │ │ │ │ +138 } │ │ │ │ │ +139 │ │ │ │ │ +_1_4_1 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const T& xj, │ │ │ │ │ +142 boost::optional H = boost::none) const override { │ │ │ │ │ +143 const size_t nj = _t_r_a_i_t_s_<_T_>_:_:_G_e_t_D_i_m_e_n_s_i_o_n(feasible_); │ │ │ │ │ +144 if (allow_error_) { │ │ │ │ │ +145 if (H) │ │ │ │ │ +146 *H = Matrix::Identity(nj,nj); // FIXME: this is not the right linearization │ │ │ │ │ +for nonlinear compare │ │ │ │ │ +147 return _t_r_a_i_t_s_<_T_>_:_:_L_o_c_a_l(xj,feasible_); │ │ │ │ │ +148 } else if (compare_(feasible_, xj)) { │ │ │ │ │ +149 if (H) │ │ │ │ │ +150 *H = Matrix::Identity(nj,nj); │ │ │ │ │ +151 return Vector::Zero(nj); // set error to zero if equal │ │ │ │ │ +152 } else { │ │ │ │ │ +153 if (H) │ │ │ │ │ +154 throw std::invalid_argument( │ │ │ │ │ +155 "Linearization point not feasible for " │ │ │ │ │ +156 + DefaultKeyFormatter(this->_k_e_y()) + "!"); │ │ │ │ │ +157 return Vector::Constant(nj, std::numeric_limits::infinity()); / │ │ │ │ │ +/ set error to infinity if not equal │ │ │ │ │ +158 } │ │ │ │ │ +159 } │ │ │ │ │ +160 │ │ │ │ │ +_1_6_2 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r _l_i_n_e_a_r_i_z_e(const _V_a_l_u_e_s& x) const override { │ │ │ │ │ +163 const T& xj = x._a_t(this->_k_e_y()); │ │ │ │ │ +164 Matrix A; │ │ │ │ │ +165 Vector b = _e_v_a_l_u_a_t_e_E_r_r_o_r(xj, A); │ │ │ │ │ +166 SharedDiagonal model = _n_o_i_s_e_M_o_d_e_l_:_:_C_o_n_s_t_r_a_i_n_e_d_:_:_A_l_l(b.size()); │ │ │ │ │ +167 return _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r( │ │ │ │ │ +168 new _J_a_c_o_b_i_a_n_F_a_c_t_o_r(this->_k_e_y(), A, b, model)); │ │ │ │ │ +169 } │ │ │ │ │ +170 │ │ │ │ │ +_1_7_2 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +173 return boost::static_pointer_cast( │ │ │ │ │ +174 gtsam::NonlinearFactor::shared_ptr(new This(*this))); │ │ │ │ │ +175 } │ │ │ │ │ +176 │ │ │ │ │ +178 │ │ │ │ │ +179 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +180 │ │ │ │ │ +181private: │ │ │ │ │ +182 │ │ │ │ │ +_1_8_4 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +185 template │ │ │ │ │ +186 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +187 // NoiseModelFactor1 instead of NoiseModelFactorN for backward │ │ │ │ │ +compatibility │ │ │ │ │ +188 ar │ │ │ │ │ +189 & boost::serialization::make_nvp("NoiseModelFactor1", │ │ │ │ │ +190 boost::serialization::base_object(*this)); │ │ │ │ │ +191 ar & BOOST_SERIALIZATION_NVP(feasible_); │ │ │ │ │ +192 ar & BOOST_SERIALIZATION_NVP(allow_error_); │ │ │ │ │ +193 ar & BOOST_SERIALIZATION_NVP(error_gain_); │ │ │ │ │ +194 } │ │ │ │ │ +195 │ │ │ │ │ +196}; │ │ │ │ │ +197// \class NonlinearEquality │ │ │ │ │ +198 │ │ │ │ │ +199template │ │ │ │ │ +_2_0_0struct _t_r_a_i_t_s<_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y> : _T_e_s_t_a_b_l_e> │ │ │ │ │ +{}; │ │ │ │ │ +201 │ │ │ │ │ +202/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +206template │ │ │ │ │ +_2_0_7class _N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +208 │ │ │ │ │ +209public: │ │ │ │ │ +210 typedef VALUE X; │ │ │ │ │ +211 │ │ │ │ │ +212protected: │ │ │ │ │ +213 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_V_A_L_U_E_> _B_a_s_e; │ │ │ │ │ +214 typedef _N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1_<_V_A_L_U_E_> This; │ │ │ │ │ +215 │ │ │ │ │ +_2_1_7 _N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1() { │ │ │ │ │ +218 } │ │ │ │ │ +219 │ │ │ │ │ +220 X value_; │ │ │ │ │ +221 │ │ │ │ │ +222 GTSAM_CONCEPT_MANIFOLD_TYPE(X) │ │ │ │ │ +223 GTSAM_CONCEPT_TESTABLE_TYPE(X) │ │ │ │ │ +224 │ │ │ │ │ +225public: │ │ │ │ │ +226 │ │ │ │ │ +_2_2_7 typedef boost::shared_ptr > _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +228 │ │ │ │ │ +_2_3_5 _N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1(const X& value, _K_e_y _k_e_y, double mu = 1000.0) │ │ │ │ │ +236 : _B_a_s_e(_n_o_i_s_e_M_o_d_e_l::Constrained::All(_t_r_a_i_t_s::GetDimension(value), │ │ │ │ │ +237 std::abs(mu)), │ │ │ │ │ +238 _k_e_y), │ │ │ │ │ +239 value_(value) {} │ │ │ │ │ +240 │ │ │ │ │ +241 _~_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1() override { │ │ │ │ │ +242 } │ │ │ │ │ +243 │ │ │ │ │ +_2_4_5 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +246 return boost::static_pointer_cast( │ │ │ │ │ +247 gtsam::NonlinearFactor::shared_ptr(new This(*this))); │ │ │ │ │ +248 } │ │ │ │ │ +249 │ │ │ │ │ +_2_5_1 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const X& x1, │ │ │ │ │ +252 boost::optional H = boost::none) const override { │ │ │ │ │ +253 if (H) │ │ │ │ │ +254 (*H) = Matrix::Identity(_t_r_a_i_t_s_<_X_>_:_:_G_e_t_D_i_m_e_n_s_i_o_n(x1),_t_r_a_i_t_s_<_X_>_:_:_G_e_t_D_i_m_e_n_s_i_o_n │ │ │ │ │ +(x1)); │ │ │ │ │ +255 // manifold equivalent of h(x)-z -> log(z,h(x)) │ │ │ │ │ +256 return _t_r_a_i_t_s_<_X_>_:_:_L_o_c_a_l(value_,x1); │ │ │ │ │ +257 } │ │ │ │ │ +258 │ │ │ │ │ +_2_6_0 void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ +261 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ +262 std::cout << s << ": NonlinearEquality1(" << keyFormatter(this->_k_e_y()) │ │ │ │ │ +263 << ")," << "\n"; │ │ │ │ │ +264 this->noiseModel_->print(); │ │ │ │ │ +265 _t_r_a_i_t_s_<_X_>_:_:_P_r_i_n_t(value_, "Value"); │ │ │ │ │ +266 } │ │ │ │ │ +267 │ │ │ │ │ +268 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +269 │ │ │ │ │ +270private: │ │ │ │ │ +271 │ │ │ │ │ +_2_7_3 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +274 template │ │ │ │ │ +275 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +276 // NoiseModelFactor1 instead of NoiseModelFactorN for backward │ │ │ │ │ +compatibility │ │ │ │ │ +277 ar │ │ │ │ │ +278 & boost::serialization::make_nvp("NoiseModelFactor1", │ │ │ │ │ +279 boost::serialization::base_object(*this)); │ │ │ │ │ +280 ar & BOOST_SERIALIZATION_NVP(value_); │ │ │ │ │ +281 } │ │ │ │ │ +282}; │ │ │ │ │ +283// \NonlinearEquality1 │ │ │ │ │ +284 │ │ │ │ │ +285template │ │ │ │ │ +_2_8_6struct _t_r_a_i_t_s<_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1 > │ │ │ │ │ +287 : _T_e_s_t_a_b_l_e > {}; │ │ │ │ │ +288 │ │ │ │ │ +289/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +294template │ │ │ │ │ +_2_9_5class _N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_2 : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +296 protected: │ │ │ │ │ +297 using _B_a_s_e = _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_T_,_ _T_>; │ │ │ │ │ +298 using This = _N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_2_<_T_>; │ │ │ │ │ +299 │ │ │ │ │ +300 GTSAM_CONCEPT_MANIFOLD_TYPE(T) │ │ │ │ │ +301 │ │ │ │ │ +302 │ │ │ │ │ +_3_0_3 _N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_2() {} │ │ │ │ │ +304 │ │ │ │ │ +305 public: │ │ │ │ │ +306 typedef boost::shared_ptr> shared_ptr; │ │ │ │ │ +307 │ │ │ │ │ +_3_1_4 _N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_2(_K_e_y key1, _K_e_y key2, double mu = 1e4) │ │ │ │ │ +315 : _B_a_s_e(_n_o_i_s_e_M_o_d_e_l::Constrained::All(_t_r_a_i_t_s::dimension, std::abs(mu)), │ │ │ │ │ +316 key1, key2) {} │ │ │ │ │ +317 _~_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_2() override {} │ │ │ │ │ +318 │ │ │ │ │ +_3_2_0 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +321 return boost::static_pointer_cast( │ │ │ │ │ +322 gtsam::NonlinearFactor::shared_ptr(new This(*this))); │ │ │ │ │ +323 } │ │ │ │ │ +324 │ │ │ │ │ +_3_2_6 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r( │ │ │ │ │ +327 const T& x1, const T& x2, boost::optional H1 = boost::none, │ │ │ │ │ +328 boost::optional H2 = boost::none) const override { │ │ │ │ │ +329 static const size_t p = _t_r_a_i_t_s_<_T_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ +330 if (H1) *H1 = -Matrix::Identity(p, p); │ │ │ │ │ +331 if (H2) *H2 = Matrix::Identity(p, p); │ │ │ │ │ +332 return _t_r_a_i_t_s_<_T_>_:_:_L_o_c_a_l(x1, x2); │ │ │ │ │ +333 } │ │ │ │ │ +334 │ │ │ │ │ +335 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +336 │ │ │ │ │ +337 private: │ │ │ │ │ +_3_3_9 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +340 template │ │ │ │ │ +341 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ +342 // NoiseModelFactor2 instead of NoiseModelFactorN for backward │ │ │ │ │ +compatibility │ │ │ │ │ +343 ar& boost::serialization::make_nvp( │ │ │ │ │ +344 "NoiseModelFactor2", boost::serialization::base_object(*this)); │ │ │ │ │ +345 } │ │ │ │ │ +346}; │ │ │ │ │ +347// \NonlinearEquality2 │ │ │ │ │ +348 │ │ │ │ │ +349template │ │ │ │ │ +_3_5_0struct _t_r_a_i_t_s<_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_2> : │ │ │ │ │ +_T_e_s_t_a_b_l_e> { │ │ │ │ │ +351}; │ │ │ │ │ +352 │ │ │ │ │ +353}// namespace gtsam │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ +_M_a_n_i_f_o_l_d_._h │ │ │ │ │ +Base class and basic functions for Manifold types. │ │ │ │ │ +_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ +This marks a GTSAM object to require alignment. │ │ │ │ │ +DDeeffiinniittiioonn types.h:308 │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ +Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_I_n_d_i_c_e_s │ │ │ │ │ -FastVector< FactorIndex > FactorIndices │ │ │ │ │ -Define collection types: │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ -FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ -fast_pool_allocator instead of the defa... │ │ │ │ │ -DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t │ │ │ │ │ -This struct is returned from ISAM2::update() and contains information about the │ │ │ │ │ -update that is useful... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Result.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_v_a_r_i_a_b_l_e_s_R_e_l_i_n_e_a_r_i_z_e_d │ │ │ │ │ -size_t variablesRelinearized │ │ │ │ │ -The number of variables that were relinearized because their linear deltas │ │ │ │ │ -exceeded the reslinearizat... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Result.h:76 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_g_e_t_V_a_r_i_a_b_l_e_s_R_e_l_i_n_e_a_r_i_z_e_d │ │ │ │ │ -size_t getVariablesRelinearized() const │ │ │ │ │ -Getters and Setters. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Result.h:176 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_f_a_c_t_o_r_s_R_e_c_a_l_c_u_l_a_t_e_d │ │ │ │ │ -size_t factorsRecalculated │ │ │ │ │ -The number of factors that were included in reelimination of the Bayes' tree. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Result.h:88 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_d_e_t_a_i_l_s │ │ │ │ │ -DetailedResults * details() │ │ │ │ │ -Return pointer to detail, 0 if no detail requested. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Result.h:165 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_e_r_r_o_r_B_e_f_o_r_e │ │ │ │ │ -boost::optional< double > errorBefore │ │ │ │ │ -The nonlinear error of all of the factors, including new factors and variables │ │ │ │ │ -added during the curre... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Result.h:54 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string str="") const │ │ │ │ │ -Print results. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Result.h:168 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_d_e_t_a_i_l │ │ │ │ │ -boost::optional< DetailedResults > detail │ │ │ │ │ -Detailed results, if enabled by ISAM2Params::enableDetailedResults. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Result.h:158 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_c_l_i_q_u_e_s │ │ │ │ │ -size_t cliques │ │ │ │ │ -The number of cliques in the Bayes' Tree. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Result.h:91 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_k_e_y_s_W_i_t_h_R_e_m_o_v_e_d_F_a_c_t_o_r_s │ │ │ │ │ -KeySet keysWithRemovedFactors │ │ │ │ │ -Keys of variables that had factors removed. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Result.h:108 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_v_a_r_i_a_b_l_e_s_R_e_e_l_i_m_i_n_a_t_e_d │ │ │ │ │ -size_t variablesReeliminated │ │ │ │ │ -The number of variables that were reeliminated as parts of the Bayes' Tree were │ │ │ │ │ -recalculated,... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Result.h:84 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_u_n_u_s_e_d_K_e_y_s │ │ │ │ │ -KeySet unusedKeys │ │ │ │ │ -Unused keys, and indices for unused keys, i.e., keys that are empty now and do │ │ │ │ │ -not appear in the new ... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Result.h:102 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_o_b_s_e_r_v_e_d_K_e_y_s │ │ │ │ │ -KeyVector observedKeys │ │ │ │ │ -keys for variables that were observed, i.e., not unused. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Result.h:105 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_m_a_r_k_e_d_K_e_y_s │ │ │ │ │ -KeySet markedKeys │ │ │ │ │ -All keys that were marked during the update process. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Result.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_n_e_w_F_a_c_t_o_r_s_I_n_d_i_c_e_s │ │ │ │ │ -FactorIndices newFactorsIndices │ │ │ │ │ -The indices of the newly-added factors, in 1-to-1 correspondence with the │ │ │ │ │ -factors passed as newFactor... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Result.h:97 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_e_r_r_o_r_A_f_t_e_r │ │ │ │ │ -boost::optional< double > errorAfter │ │ │ │ │ -The nonlinear error of all of the factors computed after the current update, │ │ │ │ │ -meaning that variables a... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Result.h:66 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s │ │ │ │ │ -A struct holding detailed results, which must be enabled with ISAM2Params:: │ │ │ │ │ -enableDetailedResults. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Result.h:117 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s_:_:_v_a_r_i_a_b_l_e_S_t_a_t_u_s │ │ │ │ │ -StatusMap variableStatus │ │ │ │ │ -The status of each variable during this update, see VariableStatus. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Result.h:153 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s_:_:_V_a_r_i_a_b_l_e_S_t_a_t_u_s │ │ │ │ │ -The status of a single variable, this struct is stored in DetailedResults:: │ │ │ │ │ -variableStatus. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Result.h:120 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s_:_:_V_a_r_i_a_b_l_e_S_t_a_t_u_s_:_:_i_s_R_e_l_i_n_e_a_r_i_z_e_I_n_v_o_l_v_e_d │ │ │ │ │ -bool isRelinearizeInvolved │ │ │ │ │ -Whether the variable was below the relinearization threshold but was │ │ │ │ │ -relinearized by being involved i... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Result.h:128 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s_:_:_V_a_r_i_a_b_l_e_S_t_a_t_u_s_:_:_i_s_O_b_s_e_r_v_e_d │ │ │ │ │ -bool isObserved │ │ │ │ │ -Whether the variable was relinearized, either by being above the │ │ │ │ │ -relinearization threshold or by invo... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Result.h:136 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s_:_:_V_a_r_i_a_b_l_e_S_t_a_t_u_s_:_:_i_s_N_e_w │ │ │ │ │ -bool isNew │ │ │ │ │ -Whether the variable itself was just added. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Result.h:138 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s_:_:_V_a_r_i_a_b_l_e_S_t_a_t_u_s_:_:_i_s_A_b_o_v_e_R_e_l_i_n_T_h_r_e_s_h_o_l_d │ │ │ │ │ -bool isAboveRelinThreshold │ │ │ │ │ -Whether the variable was just relinearized due to being above the │ │ │ │ │ -relinearization threshold. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Result.h:125 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s_:_:_V_a_r_i_a_b_l_e_S_t_a_t_u_s_:_:_i_s_R_e_e_l_i_m_i_n_a_t_e_d │ │ │ │ │ -bool isReeliminated │ │ │ │ │ -Whether the variable was just reeliminated, due to being relinearized, │ │ │ │ │ -observed, new,... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Result.h:124 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s_:_:_V_a_r_i_a_b_l_e_S_t_a_t_u_s_:_:_i_n_R_o_o_t_C_l_i_q_u_e │ │ │ │ │ -bool inRootClique │ │ │ │ │ -Whether the variable is in the root clique. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Result.h:139 │ │ │ │ │ +_g_t_s_a_m_:_:_d_o_t │ │ │ │ │ +double dot(const V1 &a, const V2 &b) │ │ │ │ │ +Dot product. │ │ │ │ │ +DDeeffiinniittiioonn Vector.h:195 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const This &other, double tol=1e-9) const │ │ │ │ │ +check equality │ │ │ │ │ +DDeeffiinniittiioonn Factor.cpp:42 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactor.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ +A Gaussian factor in the squared-error form. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:91 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_C_o_n_s_t_r_a_i_n_e_d_:_:_A_l_l │ │ │ │ │ +static shared_ptr All(size_t dim) │ │ │ │ │ +Fully constrained variations. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:457 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y │ │ │ │ │ +An equality factor that forces either one variable to a constant, or a set of │ │ │ │ │ +variables to be equal t... │ │ │ │ │ +DDeeffiinniittiioonn NonlinearEquality.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_:_:_e_r_r_o_r │ │ │ │ │ +double error(const Values &c) const override │ │ │ │ │ +Actual error function calculation. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearEquality.h:130 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +Print. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearEquality.h:109 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_:_:_C_o_m_p_a_r_e_F_u_n_c_t_i_o_n │ │ │ │ │ +std::function< bool(const T &, const T &)> CompareFunction │ │ │ │ │ +Function that compares two values. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearEquality.h:70 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y │ │ │ │ │ +NonlinearEquality() │ │ │ │ │ +Default constructor - only for serialization. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearEquality.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const NonlinearFactor &f, double tol=1e-9) const override │ │ │ │ │ +Check if two factors are equal. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearEquality.h:119 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y │ │ │ │ │ +NonlinearEquality(Key j, const T &feasible, double error_gain, const │ │ │ │ │ +CompareFunction &_compare=std::bind(traits< T >::Equals, std::placeholders::_1, │ │ │ │ │ +std::placeholders::_2, 1e-9)) │ │ │ │ │ +Constructor - allows inexact evaluation. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearEquality.h:97 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_:_:_c_l_o_n_e │ │ │ │ │ +gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn NonlinearEquality.h:172 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ +Vector evaluateError(const T &xj, boost::optional< Matrix & > H=boost::none) │ │ │ │ │ +const override │ │ │ │ │ +Error function. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearEquality.h:141 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearEquality.h:184 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y │ │ │ │ │ +NonlinearEquality(Key j, const T &feasible, const CompareFunction │ │ │ │ │ +&_compare=std::bind(traits< T >::Equals, std::placeholders::_1, std:: │ │ │ │ │ +placeholders::_2, 1e-9)) │ │ │ │ │ +Constructor - forces exact evaluation. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearEquality.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_:_:_l_i_n_e_a_r_i_z_e │ │ │ │ │ +GaussianFactor::shared_ptr linearize(const Values &x) const override │ │ │ │ │ +Linearize is over-written, because base linearization tries to whiten. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearEquality.h:162 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1 │ │ │ │ │ +Simple unary equality constraint - fixes a value for a variable. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearEquality.h:207 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ +Vector evaluateError(const X &x1, boost::optional< Matrix & > H=boost::none) │ │ │ │ │ +const override │ │ │ │ │ +g(x) with optional derivative │ │ │ │ │ +DDeeffiinniittiioonn NonlinearEquality.h:251 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1_:_:_c_l_o_n_e │ │ │ │ │ +gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn NonlinearEquality.h:245 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< NonlinearEquality1< VALUE > > shared_ptr │ │ │ │ │ +fixed value for variable │ │ │ │ │ +DDeeffiinniittiioonn NonlinearEquality.h:227 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1 │ │ │ │ │ +NonlinearEquality1(const X &value, Key key, double mu=1000.0) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearEquality.h:235 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1 │ │ │ │ │ +NonlinearEquality1() │ │ │ │ │ +Default constructor to allow for serialization. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearEquality.h:217 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +Print. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearEquality.h:260 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearEquality.h:273 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_2 │ │ │ │ │ +Simple binary equality constraint - this constraint forces two variables to be │ │ │ │ │ +the same. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearEquality.h:295 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_2_:_:_c_l_o_n_e │ │ │ │ │ +gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn NonlinearEquality.h:320 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_2_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ +Vector evaluateError(const T &x1, const T &x2, boost::optional< Matrix & > │ │ │ │ │ +H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override │ │ │ │ │ +g(x) with optional derivative2 │ │ │ │ │ +DDeeffiinniittiioonn NonlinearEquality.h:326 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_2_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_2 │ │ │ │ │ +NonlinearEquality2(Key key1, Key key2, double mu=1e4) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearEquality.h:314 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_2_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearEquality.h:339 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ +Nonlinear factor base class. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_n_o_i_s_e_M_o_d_e_l │ │ │ │ │ +const SharedNoiseModel & noiseModel() const │ │ │ │ │ +access to the noise model │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:223 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ +A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ +variables. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_ _V_A_L_U_E_ _>_:_:_u_n_w_h_i_t_e_n_e_d_E_r_r_o_r │ │ │ │ │ +Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > │ │ │ │ │ +& > H=boost::none) const override │ │ │ │ │ +This implements the unwhitenedError virtual function by calling the n-key │ │ │ │ │ +specific version of evaluat... │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:542 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_ _V_A_L_U_E_ _>_:_:_k_e_y │ │ │ │ │ +Key key() const │ │ │ │ │ +Returns a key. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:518 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_a_t │ │ │ │ │ +const ValueType at(Key j) const │ │ │ │ │ +Retrieve a variable by key j. │ │ │ │ │ +DDeeffiinniittiioonn Values-inl.h:361 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _I_S_A_M_2_R_e_s_u_l_t_._h │ │ │ │ │ + * NNoonnlliinneeaarrEEqquuaalliittyy..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01118.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearISAM.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/DoglegOptimizer.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,41 +94,39 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
NonlinearISAM.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Typedefs
│ │ │ │ +
DoglegOptimizer.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::NonlinearISAM
 Wrapper class to manage ISAM in a nonlinear context. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Typedefs

│ │ │ │ +typedef internal::DoglegState gtsam::State
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Jan 19, 2010
│ │ │ │ -
Author
Viorela Ila and Richard Roberts
│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Feb 26, 2012
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,25 +1,23 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -NonlinearISAM.h File Reference │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_I_S_A_M │ │ │ │ │ -  Wrapper class to manage _I_S_A_M in a nonlinear context. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ +DoglegOptimizer.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +typedef internal::DoglegState  ggttssaamm::::SSttaattee │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Jan 19, 2010 │ │ │ │ │ Author │ │ │ │ │ - Viorela Ila and Richard Roberts │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Feb 26, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _N_o_n_l_i_n_e_a_r_I_S_A_M_._h │ │ │ │ │ + * _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01121.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/LevenbergMarquardtParams.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Clique.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,48 +95,53 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
LevenbergMarquardtParams.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
ISAM2Clique.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Parameters for Levenberg-Marquardt trust-region scheme. │ │ │ │ +

Specialized iSAM2 Clique. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::LevenbergMarquardtParams
 Parameters for Levenberg-Marquardt optimization. More...
class  gtsam::ISAM2Clique
 Specialized Clique structure for ISAM2, incorporating caching and gradient contribution TODO: more documentation. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

size_t gtsam::optimizeWildfire (const ISAM2Clique::shared_ptr &root, double threshold, const KeySet &replaced, VectorValues *delta)
 Optimize the BayesTree, starting from the root.
 
│ │ │ │ +size_t gtsam::optimizeWildfireNonRecursive (const ISAM2Clique::shared_ptr &root, double threshold, const KeySet &keys, VectorValues *delta)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Parameters for Levenberg-Marquardt trust-region scheme.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ -
│ │ │ │ -Luca Carlone
│ │ │ │ -
Date
Feb 26, 2012
│ │ │ │ +

Specialized iSAM2 Clique.

│ │ │ │ +
Author
Michael Kaess, Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,29 +1,34 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -LevenbergMarquardtParams.h File Reference │ │ │ │ │ -Parameters for Levenberg-Marquardt trust-region scheme. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +ISAM2Clique.h File Reference │ │ │ │ │ +Specialized iSAM2 Clique. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s │ │ │ │ │ -  Parameters for Levenberg-Marquardt optimization. _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_I_S_A_M_2_C_l_i_q_u_e │ │ │ │ │ +  Specialized Clique structure for _I_S_A_M_2, incorporating caching and │ │ │ │ │ + gradient contribution TODO: more documentation. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +size_t  _g_t_s_a_m_:_:_o_p_t_i_m_i_z_e_W_i_l_d_f_i_r_e (const ISAM2Clique::shared_ptr &root, double │ │ │ │ │ + threshold, const _K_e_y_S_e_t &replaced, _V_e_c_t_o_r_V_a_l_u_e_s *delta) │ │ │ │ │ +  Optimize the _B_a_y_e_s_T_r_e_e, starting from the root. │ │ │ │ │ +  │ │ │ │ │ +size_t  ggttssaamm::::ooppttiimmiizzeeWWiillddffiirreeNNoonnRReeccuurrssiivvee (const ISAM2Clique::shared_ptr │ │ │ │ │ + &root, double threshold, const _K_e_y_S_e_t &keys, _V_e_c_t_o_r_V_a_l_u_e_s *delta) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Parameters for Levenberg-Marquardt trust-region scheme. │ │ │ │ │ +Specialized iSAM2 Clique. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Luca Carlone │ │ │ │ │ - Date │ │ │ │ │ - Feb 26, 2012 │ │ │ │ │ + Michael Kaess, Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_._h │ │ │ │ │ + * _I_S_A_M_2_C_l_i_q_u_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01121.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,4 @@ │ │ │ │ │ var a01121 = [ │ │ │ │ │ - ["gtsam::LevenbergMarquardtParams", "a04440.html", "a04440"] │ │ │ │ │ + ["gtsam::ISAM2Clique", "a04404.html", "a04404"], │ │ │ │ │ + ["optimizeWildfire", "a01121.html#a2b0857edd76f8d63eeee0ce9944e28d4", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01121_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/LevenbergMarquardtParams.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Clique.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,188 +98,166 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
LevenbergMarquardtParams.h
│ │ │ │ +
ISAM2Clique.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
21#pragma once
│ │ │ │ -
22
│ │ │ │ -
23#include <gtsam/nonlinear/NonlinearOptimizerParams.h>
│ │ │ │ -
24#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ -
25
│ │ │ │ -
26namespace gtsam {
│ │ │ │ -
27
│ │ │ │ -
28class LevenbergMarquardtOptimizer;
│ │ │ │ -
29
│ │ │ │ -
│ │ │ │ -
35class GTSAM_EXPORT LevenbergMarquardtParams: public NonlinearOptimizerParams {
│ │ │ │ -
36
│ │ │ │ -
37public:
│ │ │ │ -
│ │ │ │ -
39 enum VerbosityLM {
│ │ │ │ -
40 SILENT = 0, SUMMARY, TERMINATION, LAMBDA, TRYLAMBDA, TRYCONFIG, DAMPED, TRYDELTA
│ │ │ │ -
41 };
│ │ │ │ -
│ │ │ │ -
42
│ │ │ │ -
43 static VerbosityLM verbosityLMTranslator(const std::string &s);
│ │ │ │ -
44 static std::string verbosityLMTranslator(VerbosityLM value);
│ │ │ │ -
45 using OptimizerType = LevenbergMarquardtOptimizer;
│ │ │ │ -
46
│ │ │ │ -
47public:
│ │ │ │ -
48
│ │ │ │ -
49 double lambdaInitial;
│ │ │ │ -
50 double lambdaFactor;
│ │ │ │ -
51 double lambdaUpperBound;
│ │ │ │ -
52 double lambdaLowerBound;
│ │ │ │ -
53 VerbosityLM verbosityLM;
│ │ │ │ -
54 double minModelFidelity;
│ │ │ │ -
55 std::string logFile;
│ │ │ │ -
56 bool diagonalDamping;
│ │ │ │ -
57 bool useFixedLambdaFactor;
│ │ │ │ -
58 double minDiagonal;
│ │ │ │ -
59 double maxDiagonal;
│ │ │ │ -
60
│ │ │ │ -
61 LevenbergMarquardtParams()
│ │ │ │ -
62 : verbosityLM(SILENT),
│ │ │ │ -
63 diagonalDamping(false),
│ │ │ │ -
64 minDiagonal(1e-6),
│ │ │ │ -
65 maxDiagonal(1e32) {
│ │ │ │ -
66 SetLegacyDefaults(this);
│ │ │ │ -
67 }
│ │ │ │ -
68
│ │ │ │ -
69 static void SetLegacyDefaults(LevenbergMarquardtParams* p) {
│ │ │ │ -
70 // Relevant NonlinearOptimizerParams:
│ │ │ │ -
71 p->maxIterations = 100;
│ │ │ │ -
72 p->relativeErrorTol = 1e-5;
│ │ │ │ -
73 p->absoluteErrorTol = 1e-5;
│ │ │ │ -
74 // LM-specific:
│ │ │ │ -
75 p->lambdaInitial = 1e-5;
│ │ │ │ -
76 p->lambdaFactor = 10.0;
│ │ │ │ -
77 p->lambdaUpperBound = 1e5;
│ │ │ │ -
78 p->lambdaLowerBound = 0.0;
│ │ │ │ -
79 p->minModelFidelity = 1e-3;
│ │ │ │ -
80 p->diagonalDamping = false;
│ │ │ │ -
81 p->useFixedLambdaFactor = true;
│ │ │ │ -
82 }
│ │ │ │ -
83
│ │ │ │ -
84 // these do seem to work better for SFM
│ │ │ │ -
85 static void SetCeresDefaults(LevenbergMarquardtParams* p) {
│ │ │ │ -
86 // Relevant NonlinearOptimizerParams:
│ │ │ │ -
87 p->maxIterations = 50;
│ │ │ │ -
88 p->absoluteErrorTol = 0; // No corresponding option in CERES
│ │ │ │ -
89 p->relativeErrorTol = 1e-6; // This is function_tolerance
│ │ │ │ -
90 // LM-specific:
│ │ │ │ -
91 p->lambdaUpperBound = 1e32;
│ │ │ │ -
92 p->lambdaLowerBound = 1e-16;
│ │ │ │ -
93 p->lambdaInitial = 1e-04;
│ │ │ │ -
94 p->lambdaFactor = 2.0;
│ │ │ │ -
95 p->minModelFidelity = 1e-3; // options.min_relative_decrease in CERES
│ │ │ │ -
96 p->diagonalDamping = true;
│ │ │ │ -
97 p->useFixedLambdaFactor = false; // This is important
│ │ │ │ -
98 }
│ │ │ │ -
99
│ │ │ │ -
100 static LevenbergMarquardtParams LegacyDefaults() {
│ │ │ │ -
101 LevenbergMarquardtParams p;
│ │ │ │ -
102 SetLegacyDefaults(&p);
│ │ │ │ -
103 return p;
│ │ │ │ -
104 }
│ │ │ │ +
18// \callgraph
│ │ │ │ +
19
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/inference/BayesTreeCliqueBase.h>
│ │ │ │ +
23#include <gtsam/inference/Key.h>
│ │ │ │ +
24#include <gtsam/linear/GaussianBayesNet.h>
│ │ │ │ +
25#include <gtsam/linear/GaussianConditional.h>
│ │ │ │ +
26#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ +
27#include <string>
│ │ │ │ +
28
│ │ │ │ +
29namespace gtsam {
│ │ │ │ +
30
│ │ │ │ +
│ │ │ │ +
36class GTSAM_EXPORT ISAM2Clique
│ │ │ │ +
37 : public BayesTreeCliqueBase<ISAM2Clique, GaussianFactorGraph> {
│ │ │ │ +
38 public:
│ │ │ │ +
39 typedef ISAM2Clique This;
│ │ │ │ +
40 typedef BayesTreeCliqueBase<This, GaussianFactorGraph> Base;
│ │ │ │ +
41 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
42 typedef boost::weak_ptr<This> weak_ptr;
│ │ │ │ +
43 typedef GaussianConditional ConditionalType;
│ │ │ │ +
44 typedef ConditionalType::shared_ptr sharedConditional;
│ │ │ │ +
45
│ │ │ │ +
46 Base::FactorType::shared_ptr cachedFactor_;
│ │ │ │ +
47 Vector gradientContribution_;
│ │ │ │ +
48#ifdef USE_BROKEN_FAST_BACKSUBSTITUTE
│ │ │ │ +
49 mutable FastMap<Key, VectorValues::iterator> solnPointers_;
│ │ │ │ +
50#endif
│ │ │ │ +
51
│ │ │ │ +
53 ISAM2Clique() : Base() {}
│ │ │ │ +
54 virtual ~ISAM2Clique() = default;
│ │ │ │ +
55
│ │ │ │ +
│ │ │ │ +
58 ISAM2Clique(const ISAM2Clique& other)
│ │ │ │ +
59 : Base(other),
│ │ │ │ +
60 cachedFactor_(other.cachedFactor_),
│ │ │ │ +
61 gradientContribution_(other.gradientContribution_) {}
│ │ │ │ +
│ │ │ │ +
62
│ │ │ │ +
│ │ │ │ +
65 ISAM2Clique& operator=(const ISAM2Clique& other) {
│ │ │ │ +
66 Base::operator=(other);
│ │ │ │ +
67 cachedFactor_ = other.cachedFactor_;
│ │ │ │ +
68 gradientContribution_ = other.gradientContribution_;
│ │ │ │ +
69 return *this;
│ │ │ │ +
70 }
│ │ │ │ +
│ │ │ │ +
71
│ │ │ │ +
73 void setEliminationResult(
│ │ │ │ +
74 const FactorGraphType::EliminationResult& eliminationResult);
│ │ │ │ +
75
│ │ │ │ +
77 Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; }
│ │ │ │ +
78
│ │ │ │ +
80 const Vector& gradientContribution() const { return gradientContribution_; }
│ │ │ │ +
81
│ │ │ │ +
83 void addGradientAtZero(VectorValues* g) const;
│ │ │ │ +
84
│ │ │ │ +
85 bool equals(const This& other, double tol = 1e-9) const;
│ │ │ │ +
86
│ │ │ │ +
88 void print(const std::string& s = "",
│ │ │ │ +
89 const KeyFormatter& formatter = DefaultKeyFormatter) const override;
│ │ │ │ +
90
│ │ │ │ +
91 void optimizeWildfire(const KeySet& replaced, double threshold,
│ │ │ │ +
92 KeySet* changed, VectorValues* delta,
│ │ │ │ +
93 size_t* count) const;
│ │ │ │ +
94
│ │ │ │ +
95 bool optimizeWildfireNode(const KeySet& replaced, double threshold,
│ │ │ │ +
96 KeySet* changed, VectorValues* delta,
│ │ │ │ +
97 size_t* count) const;
│ │ │ │ +
98
│ │ │ │ +
103 void nnz_internal(size_t* result) const;
│ │ │ │ +
104 size_t calculate_nnz() const;
│ │ │ │
105
│ │ │ │ -
106 static LevenbergMarquardtParams CeresDefaults() {
│ │ │ │ -
107 LevenbergMarquardtParams p;
│ │ │ │ -
108 SetCeresDefaults(&p);
│ │ │ │ -
109 return p;
│ │ │ │ -
110 }
│ │ │ │ -
111
│ │ │ │ -
112 static LevenbergMarquardtParams EnsureHasOrdering(LevenbergMarquardtParams params,
│ │ │ │ -
113 const NonlinearFactorGraph& graph) {
│ │ │ │ -
114 if (!params.ordering)
│ │ │ │ -
115 params.ordering = Ordering::Create(params.orderingType, graph);
│ │ │ │ -
116 return params;
│ │ │ │ -
117 }
│ │ │ │ -
118
│ │ │ │ -
119 static LevenbergMarquardtParams ReplaceOrdering(LevenbergMarquardtParams params,
│ │ │ │ -
120 const Ordering& ordering) {
│ │ │ │ -
121 params.ordering = ordering;
│ │ │ │ -
122 return params;
│ │ │ │ -
123 }
│ │ │ │ -
124
│ │ │ │ -
125 ~LevenbergMarquardtParams() override {}
│ │ │ │ -
126 void print(const std::string& str = "") const override;
│ │ │ │ -
127
│ │ │ │ -
130 bool getDiagonalDamping() const { return diagonalDamping; }
│ │ │ │ -
131 double getlambdaFactor() const { return lambdaFactor; }
│ │ │ │ -
132 double getlambdaInitial() const { return lambdaInitial; }
│ │ │ │ -
133 double getlambdaLowerBound() const { return lambdaLowerBound; }
│ │ │ │ -
134 double getlambdaUpperBound() const { return lambdaUpperBound; }
│ │ │ │ -
135 bool getUseFixedLambdaFactor() { return useFixedLambdaFactor; }
│ │ │ │ -
136 std::string getLogFile() const { return logFile; }
│ │ │ │ -
137 std::string getVerbosityLM() const { return verbosityLMTranslator(verbosityLM);}
│ │ │ │ -
138
│ │ │ │ -
139 void setDiagonalDamping(bool flag) { diagonalDamping = flag; }
│ │ │ │ -
140 void setlambdaFactor(double value) { lambdaFactor = value; }
│ │ │ │ -
141 void setlambdaInitial(double value) { lambdaInitial = value; }
│ │ │ │ -
142 void setlambdaLowerBound(double value) { lambdaLowerBound = value; }
│ │ │ │ -
143 void setlambdaUpperBound(double value) { lambdaUpperBound = value; }
│ │ │ │ -
144 void setUseFixedLambdaFactor(bool flag) { useFixedLambdaFactor = flag;}
│ │ │ │ -
145 void setLogFile(const std::string& s) { logFile = s; }
│ │ │ │ -
146 void setVerbosityLM(const std::string& s) { verbosityLM = verbosityLMTranslator(s);}
│ │ │ │ -
147 // @}
│ │ │ │ -
150
│ │ │ │ -
│ │ │ │ -
152 boost::shared_ptr<NonlinearOptimizerParams> clone() const {
│ │ │ │ -
153 return boost::shared_ptr<NonlinearOptimizerParams>(new LevenbergMarquardtParams(*this));
│ │ │ │ -
154 }
│ │ │ │ -
│ │ │ │ -
155
│ │ │ │ -
157};
│ │ │ │ -
│ │ │ │ -
158
│ │ │ │ -
159}
│ │ │ │ -
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │ -
NonlinearOptimizerParams.h
Parameters for nonlinear optimization.
│ │ │ │ +
121 void findAll(const KeySet& markedMask, KeySet* keys) const;
│ │ │ │ +
122
│ │ │ │ +
123 private:
│ │ │ │ +
128 bool isDirty(const KeySet& replaced, const KeySet& changed) const;
│ │ │ │ +
129
│ │ │ │ +
134 void fastBackSubstitute(VectorValues* delta) const;
│ │ │ │ +
135
│ │ │ │ +
136 /*
│ │ │ │ +
137 * Check whether the values changed above a threshold, or always true if the
│ │ │ │ +
138 * clique was replaced.
│ │ │ │ +
139 */
│ │ │ │ +
140 bool valuesChanged(const KeySet& replaced, const Vector& originalValues,
│ │ │ │ +
141 const VectorValues& delta, double threshold) const;
│ │ │ │ +
142
│ │ │ │ +
144 void markFrontalsAsChanged(KeySet* changed) const;
│ │ │ │ +
145
│ │ │ │ +
147 void restoreFromOriginals(const Vector& originalValues,
│ │ │ │ +
148 VectorValues* delta) const;
│ │ │ │ +
149
│ │ │ │ +
151 friend class boost::serialization::access;
│ │ │ │ +
152 template <class ARCHIVE>
│ │ │ │ +
153 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ +
154 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
155 ar& BOOST_SERIALIZATION_NVP(cachedFactor_);
│ │ │ │ +
156 ar& BOOST_SERIALIZATION_NVP(gradientContribution_);
│ │ │ │ +
157 }
│ │ │ │ +
158}; // \struct ISAM2Clique
│ │ │ │ +
│ │ │ │ +
159
│ │ │ │ +
171size_t optimizeWildfire(const ISAM2Clique::shared_ptr& root, double threshold,
│ │ │ │ +
172 const KeySet& replaced, VectorValues* delta);
│ │ │ │ +
173
│ │ │ │ +
174size_t optimizeWildfireNonRecursive(const ISAM2Clique::shared_ptr& root,
│ │ │ │ +
175 double threshold, const KeySet& replaced,
│ │ │ │ +
176 VectorValues* delta);
│ │ │ │ +
177
│ │ │ │ +
178} // namespace gtsam
│ │ │ │ +
BayesTreeCliqueBase.h
Base class for cliques of a BayesTree.
│ │ │ │ + │ │ │ │ +
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ +
GaussianConditional.h
Conditional Gaussian Base class.
│ │ │ │ +
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::optimizeWildfire
size_t optimizeWildfire(const ISAM2Clique::shared_ptr &root, double threshold, const KeySet &keys, VectorValues *delta)
Optimize the BayesTree, starting from the root.
Definition ISAM2Clique.cpp:226
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::LevenbergMarquardtOptimizer
This class performs Levenberg-Marquardt nonlinear optimization.
Definition LevenbergMarquardtOptimizer.h:35
│ │ │ │ -
gtsam::LevenbergMarquardtParams
Parameters for Levenberg-Marquardt optimization.
Definition LevenbergMarquardtParams.h:35
│ │ │ │ -
gtsam::LevenbergMarquardtParams::lambdaFactor
double lambdaFactor
The amount by which to multiply or divide lambda when adjusting lambda (default: 10....
Definition LevenbergMarquardtParams.h:50
│ │ │ │ -
gtsam::LevenbergMarquardtParams::minDiagonal
double minDiagonal
when using diagonal damping saturates the minimum diagonal entries (default: 1e-6)
Definition LevenbergMarquardtParams.h:58
│ │ │ │ -
gtsam::LevenbergMarquardtParams::lambdaUpperBound
double lambdaUpperBound
The maximum lambda to try before assuming the optimization has failed (default: 1e5)
Definition LevenbergMarquardtParams.h:51
│ │ │ │ -
gtsam::LevenbergMarquardtParams::lambdaInitial
double lambdaInitial
The initial Levenberg-Marquardt damping term (default: 1e-5)
Definition LevenbergMarquardtParams.h:49
│ │ │ │ -
gtsam::LevenbergMarquardtParams::maxDiagonal
double maxDiagonal
when using diagonal damping saturates the maximum diagonal entries (default: 1e32)
Definition LevenbergMarquardtParams.h:59
│ │ │ │ -
gtsam::LevenbergMarquardtParams::minModelFidelity
double minModelFidelity
Lower bound for the modelFidelity to accept the result of an LM iteration.
Definition LevenbergMarquardtParams.h:54
│ │ │ │ -
gtsam::LevenbergMarquardtParams::lambdaLowerBound
double lambdaLowerBound
The minimum lambda used in LM (default: 0)
Definition LevenbergMarquardtParams.h:52
│ │ │ │ -
gtsam::LevenbergMarquardtParams::clone
boost::shared_ptr< NonlinearOptimizerParams > clone() const
Definition LevenbergMarquardtParams.h:152
│ │ │ │ -
gtsam::LevenbergMarquardtParams::diagonalDamping
bool diagonalDamping
if true, use diagonal of Hessian
Definition LevenbergMarquardtParams.h:56
│ │ │ │ -
gtsam::LevenbergMarquardtParams::useFixedLambdaFactor
bool useFixedLambdaFactor
if true applies constant increase (or decrease) to lambda according to lambdaFactor
Definition LevenbergMarquardtParams.h:57
│ │ │ │ -
gtsam::LevenbergMarquardtParams::logFile
std::string logFile
an optional CSV log file, with [iteration, time, error, lambda]
Definition LevenbergMarquardtParams.h:55
│ │ │ │ -
gtsam::LevenbergMarquardtParams::VerbosityLM
VerbosityLM
See LevenbergMarquardtParams::verbosityLM.
Definition LevenbergMarquardtParams.h:39
│ │ │ │ -
gtsam::LevenbergMarquardtParams::verbosityLM
VerbosityLM verbosityLM
The verbosity level for Levenberg-Marquardt (default: SILENT), see also NonlinearOptimizerParams::ver...
Definition LevenbergMarquardtParams.h:53
│ │ │ │ -
gtsam::NonlinearOptimizerParams
The common parameters for Nonlinear optimizers.
Definition NonlinearOptimizerParams.h:34
│ │ │ │ -
gtsam::NonlinearOptimizerParams::absoluteErrorTol
double absoluteErrorTol
The maximum absolute error decrease to stop iterating (default 1e-5)
Definition NonlinearOptimizerParams.h:43
│ │ │ │ -
gtsam::NonlinearOptimizerParams::maxIterations
size_t maxIterations
The maximum iterations to stop iterating (default 100)
Definition NonlinearOptimizerParams.h:41
│ │ │ │ -
gtsam::NonlinearOptimizerParams::relativeErrorTol
double relativeErrorTol
The maximum relative error decrease to stop iterating (default 1e-5)
Definition NonlinearOptimizerParams.h:42
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ + │ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::BayesTreeCliqueBase
This is the base class for BayesTree cliques.
Definition BayesTreeCliqueBase.h:50
│ │ │ │ +
gtsam::GaussianConditional
A GaussianConditional functions as the node in a Bayes network.
Definition GaussianConditional.h:43
│ │ │ │ +
gtsam::GaussianConditional::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianConditional.h:46
│ │ │ │ +
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
gtsam::ISAM2Clique
Specialized Clique structure for ISAM2, incorporating caching and gradient contribution TODO: more do...
Definition ISAM2Clique.h:37
│ │ │ │ +
gtsam::ISAM2Clique::gradientContribution
const Vector & gradientContribution() const
Access the gradient contribution.
Definition ISAM2Clique.h:80
│ │ │ │ +
gtsam::ISAM2Clique::ISAM2Clique
ISAM2Clique(const ISAM2Clique &other)
Copy constructor, does not copy solution pointers as these are invalid in different trees.
Definition ISAM2Clique.h:58
│ │ │ │ +
gtsam::ISAM2Clique::operator=
ISAM2Clique & operator=(const ISAM2Clique &other)
Assignment operator, does not copy solution pointers as these are invalid in different trees.
Definition ISAM2Clique.h:65
│ │ │ │ +
gtsam::ISAM2Clique::cachedFactor
Base::FactorType::shared_ptr & cachedFactor()
Access the cached factor.
Definition ISAM2Clique.h:77
│ │ │ │ +
gtsam::ISAM2Clique::ISAM2Clique
ISAM2Clique()
Default constructor.
Definition ISAM2Clique.h:53
│ │ │ │ +
VectorValues
The Factor::error simply extracts the.
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,247 +1,205 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -LevenbergMarquardtParams.h │ │ │ │ │ +ISAM2Clique.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -21#pragma once │ │ │ │ │ -22 │ │ │ │ │ -23#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -25 │ │ │ │ │ -26namespace _g_t_s_a_m { │ │ │ │ │ -27 │ │ │ │ │ -28class LevenbergMarquardtOptimizer; │ │ │ │ │ -29 │ │ │ │ │ -_3_5class GTSAM_EXPORT _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s: public _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s │ │ │ │ │ -{ │ │ │ │ │ -36 │ │ │ │ │ -37public: │ │ │ │ │ -_3_9 enum _V_e_r_b_o_s_i_t_y_L_M { │ │ │ │ │ -40 SILENT = 0, SUMMARY, TERMINATION, LAMBDA, TRYLAMBDA, TRYCONFIG, DAMPED, │ │ │ │ │ -TRYDELTA │ │ │ │ │ -41 }; │ │ │ │ │ -42 │ │ │ │ │ -43 static VerbosityLM verbosityLMTranslator(const std::string &s); │ │ │ │ │ -44 static std::string verbosityLMTranslator(VerbosityLM value); │ │ │ │ │ -45 using OptimizerType = _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r; │ │ │ │ │ -46 │ │ │ │ │ -47public: │ │ │ │ │ -48 │ │ │ │ │ -_4_9 double _l_a_m_b_d_a_I_n_i_t_i_a_l; │ │ │ │ │ -_5_0 double _l_a_m_b_d_a_F_a_c_t_o_r; │ │ │ │ │ -_5_1 double _l_a_m_b_d_a_U_p_p_e_r_B_o_u_n_d; │ │ │ │ │ -_5_2 double _l_a_m_b_d_a_L_o_w_e_r_B_o_u_n_d; │ │ │ │ │ -_5_3 _V_e_r_b_o_s_i_t_y_L_M _v_e_r_b_o_s_i_t_y_L_M; │ │ │ │ │ -_5_4 double _m_i_n_M_o_d_e_l_F_i_d_e_l_i_t_y; │ │ │ │ │ -_5_5 std::string _l_o_g_F_i_l_e; │ │ │ │ │ -_5_6 bool _d_i_a_g_o_n_a_l_D_a_m_p_i_n_g; │ │ │ │ │ -_5_7 bool _u_s_e_F_i_x_e_d_L_a_m_b_d_a_F_a_c_t_o_r; │ │ │ │ │ -_5_8 double _m_i_n_D_i_a_g_o_n_a_l; │ │ │ │ │ -_5_9 double _m_a_x_D_i_a_g_o_n_a_l; │ │ │ │ │ -60 │ │ │ │ │ -61 _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s() │ │ │ │ │ -62 : verbosityLM(SILENT), │ │ │ │ │ -63 diagonalDamping(false), │ │ │ │ │ -64 minDiagonal(1e-6), │ │ │ │ │ -65 maxDiagonal(1e32) { │ │ │ │ │ -66 SetLegacyDefaults(this); │ │ │ │ │ -67 } │ │ │ │ │ -68 │ │ │ │ │ -69 static void SetLegacyDefaults(_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s* p) { │ │ │ │ │ -70 // Relevant NonlinearOptimizerParams: │ │ │ │ │ -71 p->_m_a_x_I_t_e_r_a_t_i_o_n_s = 100; │ │ │ │ │ -72 p->_r_e_l_a_t_i_v_e_E_r_r_o_r_T_o_l = 1e-5; │ │ │ │ │ -73 p->_a_b_s_o_l_u_t_e_E_r_r_o_r_T_o_l = 1e-5; │ │ │ │ │ -74 // LM-specific: │ │ │ │ │ -75 p->_l_a_m_b_d_a_I_n_i_t_i_a_l = 1e-5; │ │ │ │ │ -76 p->_l_a_m_b_d_a_F_a_c_t_o_r = 10.0; │ │ │ │ │ -77 p->_l_a_m_b_d_a_U_p_p_e_r_B_o_u_n_d = 1e5; │ │ │ │ │ -78 p->_l_a_m_b_d_a_L_o_w_e_r_B_o_u_n_d = 0.0; │ │ │ │ │ -79 p->_m_i_n_M_o_d_e_l_F_i_d_e_l_i_t_y = 1e-3; │ │ │ │ │ -80 p->_d_i_a_g_o_n_a_l_D_a_m_p_i_n_g = false; │ │ │ │ │ -81 p->_u_s_e_F_i_x_e_d_L_a_m_b_d_a_F_a_c_t_o_r = true; │ │ │ │ │ -82 } │ │ │ │ │ -83 │ │ │ │ │ -84 // these do seem to work better for SFM │ │ │ │ │ -85 static void SetCeresDefaults(LevenbergMarquardtParams* p) { │ │ │ │ │ -86 // Relevant NonlinearOptimizerParams: │ │ │ │ │ -87 p->maxIterations = 50; │ │ │ │ │ -88 p->absoluteErrorTol = 0; // No corresponding option in CERES │ │ │ │ │ -89 p->relativeErrorTol = 1e-6; // This is function_tolerance │ │ │ │ │ -90 // LM-specific: │ │ │ │ │ -91 p->lambdaUpperBound = 1e32; │ │ │ │ │ -92 p->lambdaLowerBound = 1e-16; │ │ │ │ │ -93 p->lambdaInitial = 1e-04; │ │ │ │ │ -94 p->lambdaFactor = 2.0; │ │ │ │ │ -95 p->minModelFidelity = 1e-3; // options.min_relative_decrease in CERES │ │ │ │ │ -96 p->diagonalDamping = true; │ │ │ │ │ -97 p->useFixedLambdaFactor = false; // This is important │ │ │ │ │ -98 } │ │ │ │ │ -99 │ │ │ │ │ -100 static LevenbergMarquardtParams LegacyDefaults() { │ │ │ │ │ -101 LevenbergMarquardtParams p; │ │ │ │ │ -102 SetLegacyDefaults(&p); │ │ │ │ │ -103 return p; │ │ │ │ │ -104 } │ │ │ │ │ +18// \callgraph │ │ │ │ │ +19 │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +27#include │ │ │ │ │ +28 │ │ │ │ │ +29namespace _g_t_s_a_m { │ │ │ │ │ +30 │ │ │ │ │ +_3_6class GTSAM_EXPORT _I_S_A_M_2_C_l_i_q_u_e │ │ │ │ │ +37 : public _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e { │ │ │ │ │ +38 public: │ │ │ │ │ +39 typedef _I_S_A_M_2_C_l_i_q_u_e _T_h_i_s; │ │ │ │ │ +40 typedef _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_T_h_i_s_,_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_> Base; │ │ │ │ │ +41 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +42 typedef boost::weak_ptr weak_ptr; │ │ │ │ │ +43 typedef _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l _C_o_n_d_i_t_i_o_n_a_l_T_y_p_e; │ │ │ │ │ +44 typedef _C_o_n_d_i_t_i_o_n_a_l_T_y_p_e_:_:_s_h_a_r_e_d___p_t_r sharedConditional; │ │ │ │ │ +45 │ │ │ │ │ +46 Base::FactorType::shared_ptr cachedFactor_; │ │ │ │ │ +47 Vector gradientContribution_; │ │ │ │ │ +48#ifdef USE_BROKEN_FAST_BACKSUBSTITUTE │ │ │ │ │ +49 mutable _F_a_s_t_M_a_p_<_K_e_y_,_ _V_e_c_t_o_r_V_a_l_u_e_s_:_:_i_t_e_r_a_t_o_r_> solnPointers_; │ │ │ │ │ +50#endif │ │ │ │ │ +51 │ │ │ │ │ +_5_3 _I_S_A_M_2_C_l_i_q_u_e() : Base() {} │ │ │ │ │ +54 virtual _~_I_S_A_M_2_C_l_i_q_u_e() = default; │ │ │ │ │ +55 │ │ │ │ │ +_5_8 _I_S_A_M_2_C_l_i_q_u_e(const _I_S_A_M_2_C_l_i_q_u_e& other) │ │ │ │ │ +59 : Base(other), │ │ │ │ │ +60 cachedFactor_(other.cachedFactor_), │ │ │ │ │ +61 gradientContribution_(other.gradientContribution_) {} │ │ │ │ │ +62 │ │ │ │ │ +_6_5 _I_S_A_M_2_C_l_i_q_u_e& _o_p_e_r_a_t_o_r_=(const _I_S_A_M_2_C_l_i_q_u_e& other) { │ │ │ │ │ +66 Base::operator=(other); │ │ │ │ │ +67 cachedFactor_ = other.cachedFactor_; │ │ │ │ │ +68 gradientContribution_ = other.gradientContribution_; │ │ │ │ │ +69 return *this; │ │ │ │ │ +70 } │ │ │ │ │ +71 │ │ │ │ │ +73 void setEliminationResult( │ │ │ │ │ +74 const FactorGraphType::EliminationResult& eliminationResult); │ │ │ │ │ +75 │ │ │ │ │ +_7_7 Base::FactorType::shared_ptr& _c_a_c_h_e_d_F_a_c_t_o_r() { return cachedFactor_; } │ │ │ │ │ +78 │ │ │ │ │ +_8_0 const Vector& _g_r_a_d_i_e_n_t_C_o_n_t_r_i_b_u_t_i_o_n() const { return gradientContribution_; } │ │ │ │ │ +81 │ │ │ │ │ +83 void addGradientAtZero(_V_e_c_t_o_r_V_a_l_u_e_s* g) const; │ │ │ │ │ +84 │ │ │ │ │ +85 bool _e_q_u_a_l_s(const This& other, double tol = 1e-9) const; │ │ │ │ │ +86 │ │ │ │ │ +88 void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ +89 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override; │ │ │ │ │ +90 │ │ │ │ │ +91 void _o_p_t_i_m_i_z_e_W_i_l_d_f_i_r_e(const _K_e_y_S_e_t& replaced, double threshold, │ │ │ │ │ +92 _K_e_y_S_e_t* changed, _V_e_c_t_o_r_V_a_l_u_e_s* delta, │ │ │ │ │ +93 size_t* count) const; │ │ │ │ │ +94 │ │ │ │ │ +95 bool optimizeWildfireNode(const _K_e_y_S_e_t& replaced, double threshold, │ │ │ │ │ +96 _K_e_y_S_e_t* changed, _V_e_c_t_o_r_V_a_l_u_e_s* delta, │ │ │ │ │ +97 size_t* count) const; │ │ │ │ │ +98 │ │ │ │ │ +103 void nnz_internal(size_t* result) const; │ │ │ │ │ +104 size_t calculate_nnz() const; │ │ │ │ │ 105 │ │ │ │ │ -106 static LevenbergMarquardtParams CeresDefaults() { │ │ │ │ │ -107 LevenbergMarquardtParams p; │ │ │ │ │ -108 SetCeresDefaults(&p); │ │ │ │ │ -109 return p; │ │ │ │ │ -110 } │ │ │ │ │ -111 │ │ │ │ │ -112 static LevenbergMarquardtParams EnsureHasOrdering(LevenbergMarquardtParams │ │ │ │ │ -params, │ │ │ │ │ -113 const NonlinearFactorGraph& graph) { │ │ │ │ │ -114 if (!params.ordering) │ │ │ │ │ -115 params.ordering = Ordering::Create(params.orderingType, graph); │ │ │ │ │ -116 return params; │ │ │ │ │ -117 } │ │ │ │ │ -118 │ │ │ │ │ -119 static LevenbergMarquardtParams ReplaceOrdering(LevenbergMarquardtParams │ │ │ │ │ -params, │ │ │ │ │ -120 const Ordering& ordering) { │ │ │ │ │ -121 params.ordering = ordering; │ │ │ │ │ -122 return params; │ │ │ │ │ -123 } │ │ │ │ │ -124 │ │ │ │ │ -125 ~LevenbergMarquardtParams() override {} │ │ │ │ │ -126 void _p_r_i_n_t(const std::string& str = "") const override; │ │ │ │ │ -127 │ │ │ │ │ -130 bool getDiagonalDamping() const { return diagonalDamping; } │ │ │ │ │ -131 double getlambdaFactor() const { return lambdaFactor; } │ │ │ │ │ -132 double getlambdaInitial() const { return lambdaInitial; } │ │ │ │ │ -133 double getlambdaLowerBound() const { return lambdaLowerBound; } │ │ │ │ │ -134 double getlambdaUpperBound() const { return lambdaUpperBound; } │ │ │ │ │ -135 bool getUseFixedLambdaFactor() { return useFixedLambdaFactor; } │ │ │ │ │ -136 std::string getLogFile() const { return logFile; } │ │ │ │ │ -137 std::string getVerbosityLM() const { return verbosityLMTranslator │ │ │ │ │ -(verbosityLM);} │ │ │ │ │ -138 │ │ │ │ │ -139 void setDiagonalDamping(bool flag) { diagonalDamping = flag; } │ │ │ │ │ -140 void setlambdaFactor(double value) { lambdaFactor = value; } │ │ │ │ │ -141 void setlambdaInitial(double value) { lambdaInitial = value; } │ │ │ │ │ -142 void setlambdaLowerBound(double value) { lambdaLowerBound = value; } │ │ │ │ │ -143 void setlambdaUpperBound(double value) { lambdaUpperBound = value; } │ │ │ │ │ -144 void setUseFixedLambdaFactor(bool flag) { useFixedLambdaFactor = flag;} │ │ │ │ │ -145 void setLogFile(const std::string& s) { logFile = s; } │ │ │ │ │ -146 void setVerbosityLM(const std::string& s) { verbosityLM = │ │ │ │ │ -verbosityLMTranslator(s);} │ │ │ │ │ -147 // @} │ │ │ │ │ -150 │ │ │ │ │ -_1_5_2 boost::shared_ptr _c_l_o_n_e() const { │ │ │ │ │ -153 return boost::shared_ptr(new │ │ │ │ │ -_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s(*this)); │ │ │ │ │ -154 } │ │ │ │ │ -155 │ │ │ │ │ -157}; │ │ │ │ │ -158 │ │ │ │ │ -159} │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Factor Graph consisting of non-linear factors. │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_._h │ │ │ │ │ -Parameters for nonlinear optimization. │ │ │ │ │ +121 void findAll(const _K_e_y_S_e_t& markedMask, _K_e_y_S_e_t* keys) const; │ │ │ │ │ +122 │ │ │ │ │ +123 private: │ │ │ │ │ +128 bool isDirty(const _K_e_y_S_e_t& replaced, const _K_e_y_S_e_t& changed) const; │ │ │ │ │ +129 │ │ │ │ │ +134 void fastBackSubstitute(_V_e_c_t_o_r_V_a_l_u_e_s* delta) const; │ │ │ │ │ +135 │ │ │ │ │ +136 /* │ │ │ │ │ +137 * Check whether the values changed above a threshold, or always true if the │ │ │ │ │ +138 * clique was replaced. │ │ │ │ │ +139 */ │ │ │ │ │ +140 bool valuesChanged(const _K_e_y_S_e_t& replaced, const Vector& originalValues, │ │ │ │ │ +141 const _V_e_c_t_o_r_V_a_l_u_e_s& delta, double threshold) const; │ │ │ │ │ +142 │ │ │ │ │ +144 void markFrontalsAsChanged(_K_e_y_S_e_t* changed) const; │ │ │ │ │ +145 │ │ │ │ │ +147 void restoreFromOriginals(const Vector& originalValues, │ │ │ │ │ +148 _V_e_c_t_o_r_V_a_l_u_e_s* delta) const; │ │ │ │ │ +149 │ │ │ │ │ +_1_5_1 friend class boost::serialization::access; │ │ │ │ │ +152 template │ │ │ │ │ +153 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ +154 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); │ │ │ │ │ +155 ar& BOOST_SERIALIZATION_NVP(cachedFactor_); │ │ │ │ │ +156 ar& BOOST_SERIALIZATION_NVP(gradientContribution_); │ │ │ │ │ +157 } │ │ │ │ │ +158}; // \struct ISAM2Clique │ │ │ │ │ +159 │ │ │ │ │ +171size_t _o_p_t_i_m_i_z_e_W_i_l_d_f_i_r_e(const ISAM2Clique::shared_ptr& root, double │ │ │ │ │ +threshold, │ │ │ │ │ +172 const KeySet& replaced, _V_e_c_t_o_r_V_a_l_u_e_s* delta); │ │ │ │ │ +173 │ │ │ │ │ +174size_t optimizeWildfireNonRecursive(const ISAM2Clique::shared_ptr& root, │ │ │ │ │ +175 double threshold, const KeySet& replaced, │ │ │ │ │ +176 _V_e_c_t_o_r_V_a_l_u_e_s* delta); │ │ │ │ │ +177 │ │ │ │ │ +178} // namespace gtsam │ │ │ │ │ +_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._h │ │ │ │ │ +Base class for cliques of a BayesTree. │ │ │ │ │ +_K_e_y_._h │ │ │ │ │ +_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ +_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ +Conditional Gaussian Base class. │ │ │ │ │ +_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h │ │ │ │ │ +Chordal Bayes Net, the result of eliminating a factor graph. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_t_i_m_i_z_e_W_i_l_d_f_i_r_e │ │ │ │ │ +size_t optimizeWildfire(const ISAM2Clique::shared_ptr &root, double threshold, │ │ │ │ │ +const KeySet &keys, VectorValues *delta) │ │ │ │ │ +Optimize the BayesTree, starting from the root. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Clique.cpp:226 │ │ │ │ │ _g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ print without optional string, must specify cout yourself │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r │ │ │ │ │ -This class performs Levenberg-Marquardt nonlinear optimization. │ │ │ │ │ -DDeeffiinniittiioonn LevenbergMarquardtOptimizer.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s │ │ │ │ │ -Parameters for Levenberg-Marquardt optimization. │ │ │ │ │ -DDeeffiinniittiioonn LevenbergMarquardtParams.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_:_:_l_a_m_b_d_a_F_a_c_t_o_r │ │ │ │ │ -double lambdaFactor │ │ │ │ │ -The amount by which to multiply or divide lambda when adjusting lambda │ │ │ │ │ -(default: 10.... │ │ │ │ │ -DDeeffiinniittiioonn LevenbergMarquardtParams.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_:_:_m_i_n_D_i_a_g_o_n_a_l │ │ │ │ │ -double minDiagonal │ │ │ │ │ -when using diagonal damping saturates the minimum diagonal entries (default: │ │ │ │ │ -1e-6) │ │ │ │ │ -DDeeffiinniittiioonn LevenbergMarquardtParams.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_:_:_l_a_m_b_d_a_U_p_p_e_r_B_o_u_n_d │ │ │ │ │ -double lambdaUpperBound │ │ │ │ │ -The maximum lambda to try before assuming the optimization has failed (default: │ │ │ │ │ -1e5) │ │ │ │ │ -DDeeffiinniittiioonn LevenbergMarquardtParams.h:51 │ │ │ │ │ -_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_:_:_l_a_m_b_d_a_I_n_i_t_i_a_l │ │ │ │ │ -double lambdaInitial │ │ │ │ │ -The initial Levenberg-Marquardt damping term (default: 1e-5) │ │ │ │ │ -DDeeffiinniittiioonn LevenbergMarquardtParams.h:49 │ │ │ │ │ -_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_:_:_m_a_x_D_i_a_g_o_n_a_l │ │ │ │ │ -double maxDiagonal │ │ │ │ │ -when using diagonal damping saturates the maximum diagonal entries (default: │ │ │ │ │ -1e32) │ │ │ │ │ -DDeeffiinniittiioonn LevenbergMarquardtParams.h:59 │ │ │ │ │ -_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_:_:_m_i_n_M_o_d_e_l_F_i_d_e_l_i_t_y │ │ │ │ │ -double minModelFidelity │ │ │ │ │ -Lower bound for the modelFidelity to accept the result of an LM iteration. │ │ │ │ │ -DDeeffiinniittiioonn LevenbergMarquardtParams.h:54 │ │ │ │ │ -_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_:_:_l_a_m_b_d_a_L_o_w_e_r_B_o_u_n_d │ │ │ │ │ -double lambdaLowerBound │ │ │ │ │ -The minimum lambda used in LM (default: 0) │ │ │ │ │ -DDeeffiinniittiioonn LevenbergMarquardtParams.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_:_:_c_l_o_n_e │ │ │ │ │ -boost::shared_ptr< NonlinearOptimizerParams > clone() const │ │ │ │ │ -DDeeffiinniittiioonn LevenbergMarquardtParams.h:152 │ │ │ │ │ -_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_:_:_d_i_a_g_o_n_a_l_D_a_m_p_i_n_g │ │ │ │ │ -bool diagonalDamping │ │ │ │ │ -if true, use diagonal of Hessian │ │ │ │ │ -DDeeffiinniittiioonn LevenbergMarquardtParams.h:56 │ │ │ │ │ -_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_:_:_u_s_e_F_i_x_e_d_L_a_m_b_d_a_F_a_c_t_o_r │ │ │ │ │ -bool useFixedLambdaFactor │ │ │ │ │ -if true applies constant increase (or decrease) to lambda according to │ │ │ │ │ -lambdaFactor │ │ │ │ │ -DDeeffiinniittiioonn LevenbergMarquardtParams.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_:_:_l_o_g_F_i_l_e │ │ │ │ │ -std::string logFile │ │ │ │ │ -an optional CSV log file, with [iteration, time, error, lambda] │ │ │ │ │ -DDeeffiinniittiioonn LevenbergMarquardtParams.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_:_:_V_e_r_b_o_s_i_t_y_L_M │ │ │ │ │ -VerbosityLM │ │ │ │ │ -See LevenbergMarquardtParams::verbosityLM. │ │ │ │ │ -DDeeffiinniittiioonn LevenbergMarquardtParams.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_:_:_v_e_r_b_o_s_i_t_y_L_M │ │ │ │ │ -VerbosityLM verbosityLM │ │ │ │ │ -The verbosity level for Levenberg-Marquardt (default: SILENT), see also │ │ │ │ │ -NonlinearOptimizerParams::ver... │ │ │ │ │ -DDeeffiinniittiioonn LevenbergMarquardtParams.h:53 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s │ │ │ │ │ -The common parameters for Nonlinear optimizers. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizerParams.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_a_b_s_o_l_u_t_e_E_r_r_o_r_T_o_l │ │ │ │ │ -double absoluteErrorTol │ │ │ │ │ -The maximum absolute error decrease to stop iterating (default 1e-5) │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizerParams.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_m_a_x_I_t_e_r_a_t_i_o_n_s │ │ │ │ │ -size_t maxIterations │ │ │ │ │ -The maximum iterations to stop iterating (default 100) │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizerParams.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_r_e_l_a_t_i_v_e_E_r_r_o_r_T_o_l │ │ │ │ │ -double relativeErrorTol │ │ │ │ │ -The maximum relative error decrease to stop iterating (default 1e-5) │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizerParams.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ +FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ +fast_pool_allocator instead of the defa... │ │ │ │ │ +DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e │ │ │ │ │ +This is the base class for BayesTree cliques. │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +A GaussianConditional functions as the node in a Bayes network. │ │ │ │ │ +DDeeffiinniittiioonn GaussianConditional.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn GaussianConditional.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ +with a unique integer... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_C_l_i_q_u_e │ │ │ │ │ +Specialized Clique structure for ISAM2, incorporating caching and gradient │ │ │ │ │ +contribution TODO: more do... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Clique.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_C_l_i_q_u_e_:_:_g_r_a_d_i_e_n_t_C_o_n_t_r_i_b_u_t_i_o_n │ │ │ │ │ +const Vector & gradientContribution() const │ │ │ │ │ +Access the gradient contribution. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Clique.h:80 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_C_l_i_q_u_e_:_:_I_S_A_M_2_C_l_i_q_u_e │ │ │ │ │ +ISAM2Clique(const ISAM2Clique &other) │ │ │ │ │ +Copy constructor, does not copy solution pointers as these are invalid in │ │ │ │ │ +different trees. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Clique.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_C_l_i_q_u_e_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ +ISAM2Clique & operator=(const ISAM2Clique &other) │ │ │ │ │ +Assignment operator, does not copy solution pointers as these are invalid in │ │ │ │ │ +different trees. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Clique.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_C_l_i_q_u_e_:_:_c_a_c_h_e_d_F_a_c_t_o_r │ │ │ │ │ +Base::FactorType::shared_ptr & cachedFactor() │ │ │ │ │ +Access the cached factor. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Clique.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_C_l_i_q_u_e_:_:_I_S_A_M_2_C_l_i_q_u_e │ │ │ │ │ +ISAM2Clique() │ │ │ │ │ +Default constructor. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Clique.h:53 │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +The Factor::error simply extracts the. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_._h │ │ │ │ │ + * _I_S_A_M_2_C_l_i_q_u_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01124.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/DoglegOptimizerImpl.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearOptimizer.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,46 +94,47 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
DoglegOptimizerImpl.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
NonlinearOptimizer.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail implementation) │ │ │ │ +

Convergence functions not dependent on graph types. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

struct  gtsam::DoglegOptimizerImpl
 This class contains the implementation of the Dogleg algorithm. More...
 
struct  gtsam::DoglegOptimizerImpl::IterationResult
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +bool gtsam::checkConvergence (double relativeErrorTreshold, double absoluteErrorTreshold, double errorThreshold, double currentError, double newError, NonlinearOptimizerParams::Verbosity verbosity=NonlinearOptimizerParams::SILENT)
 Check whether the relative error decrease is less than relativeErrorTreshold, the absolute error decrease is less than absoluteErrorTreshold, or the error itself is less than errorThreshold.
 
│ │ │ │ +GTSAM_EXPORT bool gtsam::checkConvergence (const NonlinearOptimizerParams &params, double currentError, double newError)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail implementation)

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ +

Convergence functions not dependent on graph types.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
Date
Jul 17, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,30 +1,35 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -DoglegOptimizerImpl.h File Reference │ │ │ │ │ -Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail │ │ │ │ │ -implementation) _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l │ │ │ │ │ -  This class contains the implementation of the Dogleg algorithm. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_:_:_I_t_e_r_a_t_i_o_n_R_e_s_u_l_t │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +NonlinearOptimizer.cpp File Reference │ │ │ │ │ +Convergence functions not dependent on graph types. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ + bool  ggttssaamm::::cchheecckkCCoonnvveerrggeennccee (double relativeErrorTreshold, │ │ │ │ │ + double absoluteErrorTreshold, double errorThreshold, double │ │ │ │ │ + currentError, double newError, _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_: │ │ │ │ │ + _V_e_r_b_o_s_i_t_y verbosity=NonlinearOptimizerParams::SILENT) │ │ │ │ │ + Check whether the relative error decrease is less than │ │ │ │ │ +  relativeErrorTreshold, the absolute error decrease is less │ │ │ │ │ + than absoluteErrorTreshold, oorr the error itself is less than │ │ │ │ │ + errorThreshold. │ │ │ │ │ +  │ │ │ │ │ +GTSAM_EXPORT bool  ggttssaamm::::cchheecckkCCoonnvveerrggeennccee (const _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s │ │ │ │ │ + ¶ms, double currentError, double newError) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail │ │ │ │ │ -implementation) │ │ │ │ │ +Convergence functions not dependent on graph types. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + Jul 17, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_._h │ │ │ │ │ + * _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01124.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,3 @@ │ │ │ │ │ var a01124 = [ │ │ │ │ │ - ["gtsam::DoglegOptimizerImpl", "a04264.html", "a04264"], │ │ │ │ │ - ["gtsam::DoglegOptimizerImpl::IterationResult", "a04268.html", null] │ │ │ │ │ + ["checkConvergence", "a01124.html#a3382482454c164c6f5817dcbe6f932b3", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01130.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Marginals.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/GncOptimizer.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,32 +94,50 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
Marginals.cpp File Reference
│ │ │ │ +
GncOptimizer.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ + │ │ │ │ +

The GncOptimizer class. │ │ │ │ +More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::GncOptimizer< GncParameters >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
May 14, 2012
│ │ │ │ +

The GncOptimizer class.

│ │ │ │ +
Author
Jingnan Shi
│ │ │ │ +
│ │ │ │ +Luca Carlone
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +

Implementation of the paper: Yang, Antonante, Tzoumas, Carlone, "Graduated Non-Convexity for Robust Spatial Perception: │ │ │ │ +From Non-Minimal Solvers to Global Outlier Rejection", ICRA/RAL, 2020. (arxiv version: https://arxiv.org/pdf/1909.08605.pdf)

│ │ │ │ +

See also: Antonante, Tzoumas, Yang, Carlone, "Outlier-Robust Estimation: Hardness, Minimally-Tuned Algorithms, and Applications", arxiv: https://arxiv.org/pdf/2007.15109.pdf, 2020.

│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,33 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Marginals.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +GncOptimizer.h File Reference │ │ │ │ │ +The GncOptimizer class. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_<_ _G_n_c_P_a_r_a_m_e_t_e_r_s_ _> │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +The GncOptimizer class. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Date │ │ │ │ │ - May 14, 2012 │ │ │ │ │ + Jingnan Shi │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Frank Dellaert │ │ │ │ │ +Implementation of the paper: Yang, Antonante, Tzoumas, Carlone, "Graduated Non- │ │ │ │ │ +Convexity for Robust Spatial Perception: From Non-Minimal Solvers to Global │ │ │ │ │ +Outlier Rejection", ICRA/RAL, 2020. (arxiv version: _h_t_t_p_s_:_/_/_a_r_x_i_v_._o_r_g_/_p_d_f_/ │ │ │ │ │ +_1_9_0_9_._0_8_6_0_5_._p_d_f) │ │ │ │ │ +See also: Antonante, Tzoumas, Yang, Carlone, "Outlier-Robust Estimation: │ │ │ │ │ +Hardness, Minimally-Tuned Algorithms, and Applications", arxiv: _h_t_t_p_s_:_/_/ │ │ │ │ │ +_a_r_x_i_v_._o_r_g_/_p_d_f_/_2_0_0_7_._1_5_1_0_9_._p_d_f, 2020. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _M_a_r_g_i_n_a_l_s_._c_p_p │ │ │ │ │ + * _G_n_c_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01133.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/GaussNewtonOptimizer.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,32 +94,79 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces
│ │ │ │ -
GaussNewtonOptimizer.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces | │ │ │ │ +Macros
│ │ │ │ +
NonlinearFactor.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ + │ │ │ │ +

Non-linear factor base classes. │ │ │ │ +More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::NonlinearFactor
 Nonlinear factor base class. More...
 
struct  gtsam::traits< NonlinearFactor >
 traits More...
 
class  gtsam::NoiseModelFactor
 A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density \( P(z|x) \propto exp -0.5*|z-h(x)|^2_C \) Templated on the parameter type X and the values structure Values There is no return type specified for h(x). More...
 
class  gtsam::NoiseModelFactorN< ValueTypes >
 A convenient base class for creating your own NoiseModelFactor with n variables. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Macros

│ │ │ │ +#define NoiseModelFactor1   NoiseModelFactorN
 
│ │ │ │ +#define NoiseModelFactor2   NoiseModelFactorN
 
│ │ │ │ +#define NoiseModelFactor3   NoiseModelFactorN
 
│ │ │ │ +#define NoiseModelFactor4   NoiseModelFactorN
 
│ │ │ │ +#define NoiseModelFactor5   NoiseModelFactorN
 
│ │ │ │ +#define NoiseModelFactor6   NoiseModelFactorN
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Feb 26, 2012
│ │ │ │ +

Non-linear factor base classes.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │ +
│ │ │ │ +Gerry Chen
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,53 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -GaussNewtonOptimizer.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s │ │ │ │ │ +NonlinearFactor.h File Reference │ │ │ │ │ +Non-linear factor base classes. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ +  Nonlinear factor base class. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r │ │ │ │ │ + A nonlinear sum-of-squares factor with a zero-mean noise model │ │ │ │ │ +  implementing the density \( P(z|x) \propto exp -0.5*|z-h(x)|^2_C \) │ │ │ │ │ + Templated on the parameter type X and the values structure _V_a_l_u_e_s │ │ │ │ │ + There is no return type specified for h(x). _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_ _V_a_l_u_e_T_y_p_e_s_ _> │ │ │ │ │ +  A convenient base class for creating your own _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r with n │ │ │ │ │ + variables. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +MMaaccrrooss │ │ │ │ │ +#define  NNooiisseeMMooddeellFFaaccttoorr11   NoiseModelFactorN │ │ │ │ │ +  │ │ │ │ │ +#define  NNooiisseeMMooddeellFFaaccttoorr22   NoiseModelFactorN │ │ │ │ │ +  │ │ │ │ │ +#define  NNooiisseeMMooddeellFFaaccttoorr33   NoiseModelFactorN │ │ │ │ │ +  │ │ │ │ │ +#define  NNooiisseeMMooddeellFFaaccttoorr44   NoiseModelFactorN │ │ │ │ │ +  │ │ │ │ │ +#define  NNooiisseeMMooddeellFFaaccttoorr55   NoiseModelFactorN │ │ │ │ │ +  │ │ │ │ │ +#define  NNooiisseeMMooddeellFFaaccttoorr66   NoiseModelFactorN │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Non-linear factor base classes. │ │ │ │ │ Author │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ - Date │ │ │ │ │ - Feb 26, 2012 │ │ │ │ │ + Gerry Chen │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r_._c_p_p │ │ │ │ │ + * _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01139.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2-impl.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Expression-inl.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,53 +95,59 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
ISAM2-impl.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
Expression-inl.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. │ │ │ │ +

Internals for Expression.h, not for general consumption. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::ISAM2BayesTree
struct  gtsam::internal::apply_compose< T >
 
class  gtsam::ISAM2JunctionTree
 
struct  gtsam::DeltaImpl
 
struct  gtsam::DeltaImpl::PartialSolveResult
 
struct  gtsam::DeltaImpl::ReorderingMode
 
struct  gtsam::UpdateImpl
 Implementation functions for update method All of the methods below have clear inputs and outputs, even if not functional: iSAM2 is inherintly imperative. More...
struct  gtsam::internal::apply_compose< double >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

template<typename T >
Expression< T > gtsam::operator* (const Expression< T > &expression1, const Expression< T > &expression2)
 Construct a product expression, assumes T::compose(T) -> T.
 
template<typename T >
std::vector< Expression< T > > gtsam::createUnknowns (size_t n, char c, size_t start)
 Construct an array of leaves.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.

│ │ │ │ -
Author
Michael Kaess, Richard Roberts, Frank Dellaert
│ │ │ │ +

Internals for Expression.h, not for general consumption.

│ │ │ │ +
Date
September 18, 2014
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ +Paul Furgale
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,39 +1,42 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -ISAM2-impl.h File Reference │ │ │ │ │ -Incremental update functionality (ISAM2) for BayesTree, with fluid │ │ │ │ │ -relinearization. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Expression-inl.h File Reference │ │ │ │ │ +Internals for _E_x_p_r_e_s_s_i_o_n_._h, not for general consumption. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_I_S_A_M_2_B_a_y_e_s_T_r_e_e │ │ │ │ │ +struct   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_a_p_p_l_y___c_o_m_p_o_s_e_<_ _T_ _> │ │ │ │ │   │ │ │ │ │ - class   _g_t_s_a_m_:_:_I_S_A_M_2_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_D_e_l_t_a_I_m_p_l │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_D_e_l_t_a_I_m_p_l_:_:_P_a_r_t_i_a_l_S_o_l_v_e_R_e_s_u_l_t │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_D_e_l_t_a_I_m_p_l_:_:_R_e_o_r_d_e_r_i_n_g_M_o_d_e │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_U_p_d_a_t_e_I_m_p_l │ │ │ │ │ - Implementation functions for update method All of the methods below │ │ │ │ │ -  have clear inputs and outputs, even if not functional: iSAM2 is │ │ │ │ │ - inherintly imperative. _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_a_p_p_l_y___c_o_m_p_o_s_e_<_ _d_o_u_b_l_e_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +template │ │ │ │ │ + _E_x_p_r_e_s_s_i_o_n< T >  _g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_* (const _E_x_p_r_e_s_s_i_o_n< T > │ │ │ │ │ + &expression1, const _E_x_p_r_e_s_s_i_o_n< T > │ │ │ │ │ + &expression2) │ │ │ │ │ +  Construct a product expression, assumes T:: │ │ │ │ │ + compose(T) -> T. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +std::vector< _E_x_p_r_e_s_s_i_o_n< T > >  _g_t_s_a_m_:_:_c_r_e_a_t_e_U_n_k_n_o_w_n_s (size_t n, char c, size_t │ │ │ │ │ + start) │ │ │ │ │ +  Construct an array of leaves. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Incremental update functionality (ISAM2) for BayesTree, with fluid │ │ │ │ │ -relinearization. │ │ │ │ │ +Internals for _E_x_p_r_e_s_s_i_o_n_._h, not for general consumption. │ │ │ │ │ + Date │ │ │ │ │ + September 18, 2014 │ │ │ │ │ Author │ │ │ │ │ - Michael Kaess, Richard Roberts, Frank Dellaert │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Paul Furgale │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _I_S_A_M_2_-_i_m_p_l_._h │ │ │ │ │ + * _E_x_p_r_e_s_s_i_o_n_-_i_n_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01139.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,8 +1,6 @@ │ │ │ │ │ var a01139 = [ │ │ │ │ │ - ["gtsam::ISAM2BayesTree", "a04372.html", null], │ │ │ │ │ - ["gtsam::ISAM2JunctionTree", "a04376.html", null], │ │ │ │ │ - ["gtsam::DeltaImpl", "a04380.html", "a04380"], │ │ │ │ │ - ["gtsam::DeltaImpl::PartialSolveResult", "a04384.html", null], │ │ │ │ │ - ["gtsam::DeltaImpl::ReorderingMode", "a04388.html", null], │ │ │ │ │ - ["gtsam::UpdateImpl", "a04392.html", null] │ │ │ │ │ + ["gtsam::internal::apply_compose< T >", "a04272.html", null], │ │ │ │ │ + ["gtsam::internal::apply_compose< double >", "a04276.html", null], │ │ │ │ │ + ["createUnknowns", "a01139.html#a51881c20dd5ecdd129cb993ea8374846", null], │ │ │ │ │ + ["operator*", "a01139.html#a3db519caa1b7e43412c9a7e13acf8329", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01139_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2-impl.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Expression-inl.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,566 +98,419 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ISAM2-impl.h
│ │ │ │ +
Expression-inl.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/nonlinear/ISAM2.h>
│ │ │ │ -
22#include <gtsam/nonlinear/ISAM2Result.h>
│ │ │ │ -
23
│ │ │ │ -
24#include <gtsam/base/debug.h>
│ │ │ │ -
25#include <gtsam/inference/JunctionTree-inst.h> // We need the inst file because we'll make a special JT templated on ISAM2
│ │ │ │ -
26#include <gtsam/inference/Symbol.h>
│ │ │ │ -
27#include <gtsam/inference/VariableIndex.h>
│ │ │ │ -
28#include <gtsam/linear/GaussianBayesTree.h>
│ │ │ │ -
29#include <gtsam/linear/GaussianEliminationTree.h>
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22// The MSVC compiler workaround for the unsupported variable length array
│ │ │ │ +
23// utilizes the std::unique_ptr<> custom deleter.
│ │ │ │ +
24// See Expression<T>::valueAndJacobianMap() below.
│ │ │ │ +
25#ifdef _MSC_VER
│ │ │ │ +
26#include <memory>
│ │ │ │ +
27#endif
│ │ │ │ +
28
│ │ │ │ +
29#include <gtsam/nonlinear/internal/ExpressionNode.h>
│ │ │ │
30
│ │ │ │ -
31#include <boost/range/adaptors.hpp>
│ │ │ │ -
32#include <boost/range/algorithm/copy.hpp>
│ │ │ │ -
33namespace br {
│ │ │ │ -
34using namespace boost::range;
│ │ │ │ -
35using namespace boost::adaptors;
│ │ │ │ -
36} // namespace br
│ │ │ │ +
31#include <boost/bind/bind.hpp>
│ │ │ │ +
32#include <boost/tuple/tuple.hpp>
│ │ │ │ +
33#include <boost/range/adaptor/map.hpp>
│ │ │ │ +
34#include <boost/range/algorithm.hpp>
│ │ │ │ +
35
│ │ │ │ +
36namespace gtsam {
│ │ │ │
37
│ │ │ │ -
38#include <algorithm>
│ │ │ │ -
39#include <limits>
│ │ │ │ -
40#include <string>
│ │ │ │ -
41#include <utility>
│ │ │ │ +
38template<typename T>
│ │ │ │ +
│ │ │ │ +
39Expression<T>::Expression(const T& value) :
│ │ │ │ +
40 root_(new internal::ConstantExpression<T>(value)) {
│ │ │ │ +
41}
│ │ │ │ +
│ │ │ │
42
│ │ │ │ -
43namespace gtsam {
│ │ │ │ -
44
│ │ │ │ -
45/* ************************************************************************* */
│ │ │ │ -
46// Special BayesTree class that uses ISAM2 cliques - this is the result of
│ │ │ │ -
47// reeliminating ISAM2 subtrees.
│ │ │ │ -
│ │ │ │ -
48class ISAM2BayesTree : public ISAM2::Base {
│ │ │ │ -
49 public:
│ │ │ │ -
50 typedef ISAM2::Base Base;
│ │ │ │ -
51 typedef ISAM2BayesTree This;
│ │ │ │ -
52 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
53
│ │ │ │ -
54 ISAM2BayesTree() {}
│ │ │ │ -
55};
│ │ │ │ -
│ │ │ │ -
56
│ │ │ │ -
57/* ************************************************************************* */
│ │ │ │ -
58// Special JunctionTree class that produces ISAM2 BayesTree cliques, used for
│ │ │ │ -
59// reeliminating ISAM2 subtrees.
│ │ │ │ -
│ │ │ │ -
60class ISAM2JunctionTree
│ │ │ │ -
61 : public JunctionTree<ISAM2BayesTree, GaussianFactorGraph> {
│ │ │ │ -
62 public:
│ │ │ │ -
63 typedef JunctionTree<ISAM2BayesTree, GaussianFactorGraph> Base;
│ │ │ │ -
64 typedef ISAM2JunctionTree This;
│ │ │ │ -
65 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
66
│ │ │ │ -
67 explicit ISAM2JunctionTree(const GaussianEliminationTree& eliminationTree)
│ │ │ │ -
68 : Base(eliminationTree) {}
│ │ │ │ -
69};
│ │ │ │ -
│ │ │ │ -
70
│ │ │ │ -
71/* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
72struct GTSAM_EXPORT DeltaImpl {
│ │ │ │ -
│ │ │ │ -
73 struct GTSAM_EXPORT PartialSolveResult {
│ │ │ │ -
74 ISAM2::sharedClique bayesTree;
│ │ │ │ -
75 };
│ │ │ │ -
│ │ │ │ -
76
│ │ │ │ -
│ │ │ │ -
77 struct GTSAM_EXPORT ReorderingMode {
│ │ │ │ -
78 size_t nFullSystemVars;
│ │ │ │ -
79 enum { /*AS_ADDED,*/ COLAMD } algorithm;
│ │ │ │ -
80 enum { NO_CONSTRAINT, CONSTRAIN_LAST } constrain;
│ │ │ │ -
81 boost::optional<FastMap<Key, int> > constrainedKeys;
│ │ │ │ -
82 };
│ │ │ │ -
│ │ │ │ -
83
│ │ │ │ -
87 static size_t UpdateGaussNewtonDelta(const ISAM2::Roots& roots,
│ │ │ │ -
88 const KeySet& replacedKeys,
│ │ │ │ -
89 double wildfireThreshold,
│ │ │ │ -
90 VectorValues* delta);
│ │ │ │ -
91
│ │ │ │ -
96 static size_t UpdateRgProd(const ISAM2::Roots& roots,
│ │ │ │ -
97 const KeySet& replacedKeys,
│ │ │ │ -
98 const VectorValues& gradAtZero,
│ │ │ │ -
99 VectorValues* RgProd);
│ │ │ │ -
100
│ │ │ │ -
104 static VectorValues ComputeGradientSearch(const VectorValues& gradAtZero,
│ │ │ │ -
105 const VectorValues& RgProd);
│ │ │ │ -
106};
│ │ │ │ -
│ │ │ │ -
107
│ │ │ │ -
108/* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
114struct GTSAM_EXPORT UpdateImpl {
│ │ │ │ -
115 const ISAM2Params& params_;
│ │ │ │ -
116 const ISAM2UpdateParams& updateParams_;
│ │ │ │ -
117 UpdateImpl(const ISAM2Params& params, const ISAM2UpdateParams& updateParams)
│ │ │ │ -
118 : params_(params), updateParams_(updateParams) {}
│ │ │ │ -
119
│ │ │ │ -
120 // Provide some debugging information at the start of update
│ │ │ │ -
121 static void LogStartingUpdate(const NonlinearFactorGraph& newFactors,
│ │ │ │ -
122 const ISAM2& isam2) {
│ │ │ │ -
123 gttic(pushBackFactors);
│ │ │ │ -
124 const bool debug = ISDEBUG("ISAM2 update");
│ │ │ │ -
125 const bool verbose = ISDEBUG("ISAM2 update verbose");
│ │ │ │ -
126
│ │ │ │ -
127 if (verbose) {
│ │ │ │ -
128 std::cout << "ISAM2::update\n";
│ │ │ │ -
129 isam2.print("ISAM2: ");
│ │ │ │ -
130 }
│ │ │ │ -
131
│ │ │ │ -
132 if (debug || verbose) {
│ │ │ │ -
133 newFactors.print("The new factors are: ");
│ │ │ │ -
134 }
│ │ │ │ -
135 }
│ │ │ │ -
136
│ │ │ │ -
137 // Check relinearization if we're at the nth step, or we are using a looser
│ │ │ │ -
138 // loop relinerization threshold.
│ │ │ │ -
139 bool relinarizationNeeded(size_t update_count) const {
│ │ │ │ -
140 return updateParams_.force_relinearize ||
│ │ │ │ -
141 (params_.enableRelinearization &&
│ │ │ │ -
142 update_count % params_.relinearizeSkip == 0);
│ │ │ │ -
143 }
│ │ │ │ -
144
│ │ │ │ -
145 // Add any new factors \Factors:=\Factors\cup\Factors'.
│ │ │ │ -
146 void pushBackFactors(const NonlinearFactorGraph& newFactors,
│ │ │ │ -
147 NonlinearFactorGraph* nonlinearFactors,
│ │ │ │ -
148 GaussianFactorGraph* linearFactors,
│ │ │ │ -
149 VariableIndex* variableIndex,
│ │ │ │ -
150 FactorIndices* newFactorsIndices,
│ │ │ │ -
151 KeySet* keysWithRemovedFactors) const {
│ │ │ │ -
152 gttic(pushBackFactors);
│ │ │ │ -
153
│ │ │ │ -
154 // Perform the first part of the bookkeeping updates for adding new factors.
│ │ │ │ -
155 // Adds them to the complete list of nonlinear factors, and populates the
│ │ │ │ -
156 // list of new factor indices, both optionally finding and reusing empty
│ │ │ │ -
157 // factor slots.
│ │ │ │ -
158 *newFactorsIndices = nonlinearFactors->add_factors(
│ │ │ │ -
159 newFactors, params_.findUnusedFactorSlots);
│ │ │ │ +
43template<typename T>
│ │ │ │ +
│ │ │ │ +
44Expression<T>::Expression(const Key& key) :
│ │ │ │ +
45 root_(new internal::LeafExpression<T>(key)) {
│ │ │ │ +
46}
│ │ │ │ +
│ │ │ │ +
47
│ │ │ │ +
48template<typename T>
│ │ │ │ +
│ │ │ │ +
49Expression<T>::Expression(const Symbol& symbol) :
│ │ │ │ +
50 root_(new internal::LeafExpression<T>(symbol)) {
│ │ │ │ +
51}
│ │ │ │ +
│ │ │ │ +
52
│ │ │ │ +
53template<typename T>
│ │ │ │ +
│ │ │ │ +
54Expression<T>::Expression(unsigned char c, std::uint64_t j) :
│ │ │ │ +
55 root_(new internal::LeafExpression<T>(Symbol(c, j))) {
│ │ │ │ +
56}
│ │ │ │ +
│ │ │ │ +
57
│ │ │ │ +
59template<typename T>
│ │ │ │ +
60template<typename A>
│ │ │ │ +
│ │ │ │ +
61Expression<T>::Expression(typename UnaryFunction<A>::type function,
│ │ │ │ +
62 const Expression<A>& expression) :
│ │ │ │ +
63 root_(new internal::UnaryExpression<T, A>(function, expression)) {
│ │ │ │ +
64}
│ │ │ │ +
│ │ │ │ +
65
│ │ │ │ +
67template<typename T>
│ │ │ │ +
68template<typename A1, typename A2>
│ │ │ │ +
│ │ │ │ +
69Expression<T>::Expression(typename BinaryFunction<A1, A2>::type function,
│ │ │ │ +
70 const Expression<A1>& expression1, const Expression<A2>& expression2) :
│ │ │ │ +
71 root_(
│ │ │ │ +
72 new internal::BinaryExpression<T, A1, A2>(function, expression1,
│ │ │ │ +
73 expression2)) {
│ │ │ │ +
74}
│ │ │ │ +
│ │ │ │ +
75
│ │ │ │ +
77template<typename T>
│ │ │ │ +
78template<typename A1, typename A2, typename A3>
│ │ │ │ +
│ │ │ │ +
79Expression<T>::Expression(typename TernaryFunction<A1, A2, A3>::type function,
│ │ │ │ +
80 const Expression<A1>& expression1, const Expression<A2>& expression2,
│ │ │ │ +
81 const Expression<A3>& expression3) :
│ │ │ │ +
82 root_(
│ │ │ │ +
83 new internal::TernaryExpression<T, A1, A2, A3>(function, expression1,
│ │ │ │ +
84 expression2, expression3)) {
│ │ │ │ +
85}
│ │ │ │ +
│ │ │ │ +
86
│ │ │ │ +
88template<typename T>
│ │ │ │ +
89template<typename A>
│ │ │ │ +
│ │ │ │ +
90Expression<T>::Expression(const Expression<A>& expression,
│ │ │ │ +
91 T (A::*method)(typename MakeOptionalJacobian<T, A>::type) const) :
│ │ │ │ +
│ │ │ │ +
92 root_(
│ │ │ │ +
93 new internal::UnaryExpression<T, A>(std::bind(method,
│ │ │ │ +
94 std::placeholders::_1, std::placeholders::_2),
│ │ │ │ +
│ │ │ │ +
95 expression)) {
│ │ │ │ +
96}
│ │ │ │ +
97
│ │ │ │ +
99template<typename T>
│ │ │ │ +
100template<typename A1, typename A2>
│ │ │ │ +
│ │ │ │ +
101Expression<T>::Expression(const Expression<A1>& expression1,
│ │ │ │ +
102 T (A1::*method)(const A2&, typename MakeOptionalJacobian<T, A1>::type,
│ │ │ │ +
103 typename MakeOptionalJacobian<T, A2>::type) const,
│ │ │ │ +
104 const Expression<A2>& expression2) :
│ │ │ │ +
│ │ │ │ +
105 root_(
│ │ │ │ +
106 new internal::BinaryExpression<T, A1, A2>(
│ │ │ │ +
107 std::bind(method, std::placeholders::_1,
│ │ │ │ +
108 std::placeholders::_2, std::placeholders::_3,
│ │ │ │ +
109 std::placeholders::_4),
│ │ │ │ +
│ │ │ │ +
110 expression1, expression2)) {
│ │ │ │ +
111}
│ │ │ │ +
112
│ │ │ │ +
114template<typename T>
│ │ │ │ +
│ │ │ │ +
115template<typename A1, typename A2, typename A3>
│ │ │ │ +
│ │ │ │ +
116Expression<T>::Expression(const Expression<A1>& expression1,
│ │ │ │ +
117 T (A1::*method)(const A2&, const A3&,
│ │ │ │ +
118 typename MakeOptionalJacobian<T, A1>::type,
│ │ │ │ +
119 typename MakeOptionalJacobian<T, A2>::type,
│ │ │ │ +
120 typename MakeOptionalJacobian<T, A3>::type) const,
│ │ │ │ +
│ │ │ │ +
121 const Expression<A2>& expression2, const Expression<A3>& expression3) :
│ │ │ │ +
122 root_(
│ │ │ │ +
123 new internal::TernaryExpression<T, A1, A2, A3>(
│ │ │ │ +
124 std::bind(method, std::placeholders::_1,
│ │ │ │ +
125 std::placeholders::_2, std::placeholders::_3,
│ │ │ │ +
│ │ │ │ +
126 std::placeholders::_4, std::placeholders::_5,
│ │ │ │ +
127 std::placeholders::_6),
│ │ │ │ +
128 expression1, expression2, expression3)) {
│ │ │ │ +
129}
│ │ │ │ +
130
│ │ │ │ +
131template<typename T>
│ │ │ │ +
│ │ │ │ +
132std::set<Key> Expression<T>::keys() const {
│ │ │ │ +
│ │ │ │ +
133 return root_->keys();
│ │ │ │ +
134}
│ │ │ │ +
135
│ │ │ │ +
136template<typename T>
│ │ │ │ +
│ │ │ │ +
137void Expression<T>::dims(std::map<Key, int>& map) const {
│ │ │ │ +
138 root_->dims(map);
│ │ │ │ +
139}
│ │ │ │ +
│ │ │ │ +
140
│ │ │ │ +
141template<typename T>
│ │ │ │ +
│ │ │ │ +
142void Expression<T>::print(const std::string& s) const {
│ │ │ │ +
143 root_->print(s);
│ │ │ │ +
144}
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
145
│ │ │ │ +
146template<typename T>
│ │ │ │ +
│ │ │ │ +
147T Expression<T>::value(const Values& values,
│ │ │ │ +
│ │ │ │ +
148 boost::optional<std::vector<Matrix>&> H) const {
│ │ │ │ +
149
│ │ │ │ +
150 if (H) {
│ │ │ │ +
│ │ │ │ +
151 // Call private version that returns derivatives in H
│ │ │ │ +
152 KeyVector keys;
│ │ │ │ +
153 FastVector<int> dims;
│ │ │ │ +
154 boost::tie(keys, dims) = keysAndDims();
│ │ │ │ +
155 return valueAndDerivatives(values, keys, dims, *H);
│ │ │ │ +
156 } else
│ │ │ │ +
157 // no derivatives needed, just return value
│ │ │ │ +
│ │ │ │ +
158 return root_->value(values);
│ │ │ │ +
159}
│ │ │ │ +
│ │ │ │
160
│ │ │ │ -
161 // Remove the removed factors
│ │ │ │ -
162 NonlinearFactorGraph removedFactors;
│ │ │ │ -
163 removedFactors.reserve(updateParams_.removeFactorIndices.size());
│ │ │ │ -
164 for (const auto index : updateParams_.removeFactorIndices) {
│ │ │ │ -
165 removedFactors.push_back(nonlinearFactors->at(index));
│ │ │ │ -
166 nonlinearFactors->remove(index);
│ │ │ │ -
167 if (params_.cacheLinearizedFactors) linearFactors->remove(index);
│ │ │ │ -
168 }
│ │ │ │ -
169
│ │ │ │ -
170 // Remove removed factors from the variable index so we do not attempt to
│ │ │ │ -
171 // relinearize them
│ │ │ │ -
172 variableIndex->remove(updateParams_.removeFactorIndices.begin(),
│ │ │ │ -
173 updateParams_.removeFactorIndices.end(),
│ │ │ │ -
174 removedFactors);
│ │ │ │ -
175 *keysWithRemovedFactors = removedFactors.keys();
│ │ │ │ -
176 }
│ │ │ │ -
177
│ │ │ │ -
178 // Get keys from removed factors and new factors, and compute unused keys,
│ │ │ │ -
179 // i.e., keys that are empty now and do not appear in the new factors.
│ │ │ │ -
180 void computeUnusedKeys(const NonlinearFactorGraph& newFactors,
│ │ │ │ -
181 const VariableIndex& variableIndex,
│ │ │ │ -
182 const KeySet& keysWithRemovedFactors,
│ │ │ │ -
183 KeySet* unusedKeys) const {
│ │ │ │ -
184 gttic(computeUnusedKeys);
│ │ │ │ -
185 KeySet removedAndEmpty;
│ │ │ │ -
186 for (Key key : keysWithRemovedFactors) {
│ │ │ │ -
187 if (variableIndex.empty(key))
│ │ │ │ -
188 removedAndEmpty.insert(removedAndEmpty.end(), key);
│ │ │ │ -
189 }
│ │ │ │ -
190 KeySet newFactorSymbKeys = newFactors.keys();
│ │ │ │ -
191 std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(),
│ │ │ │ -
192 newFactorSymbKeys.begin(), newFactorSymbKeys.end(),
│ │ │ │ -
193 std::inserter(*unusedKeys, unusedKeys->end()));
│ │ │ │ -
194 }
│ │ │ │ -
195
│ │ │ │ -
196 // Calculate nonlinear error
│ │ │ │ -
197 void error(const NonlinearFactorGraph& nonlinearFactors,
│ │ │ │ -
198 const Values& estimate, boost::optional<double>* result) const {
│ │ │ │ -
199 gttic(error);
│ │ │ │ -
200 result->reset(nonlinearFactors.error(estimate));
│ │ │ │ -
201 }
│ │ │ │ -
202
│ │ │ │ -
203 // Mark linear update
│ │ │ │ -
204 void gatherInvolvedKeys(const NonlinearFactorGraph& newFactors,
│ │ │ │ -
205 const NonlinearFactorGraph& nonlinearFactors,
│ │ │ │ -
206 const KeySet& keysWithRemovedFactors,
│ │ │ │ -
207 KeySet* markedKeys) const {
│ │ │ │ -
208 gttic(gatherInvolvedKeys);
│ │ │ │ -
209 *markedKeys = newFactors.keys(); // Get keys from new factors
│ │ │ │ -
210 // Also mark keys involved in removed factors
│ │ │ │ -
211 markedKeys->insert(keysWithRemovedFactors.begin(),
│ │ │ │ -
212 keysWithRemovedFactors.end());
│ │ │ │ +
161template<typename T>
│ │ │ │ +
│ │ │ │ +
162const boost::shared_ptr<internal::ExpressionNode<T> >& Expression<T>::root() const {
│ │ │ │ +
163 return root_;
│ │ │ │ +
164}
│ │ │ │ +
│ │ │ │ +
165
│ │ │ │ +
166template<typename T>
│ │ │ │ +
│ │ │ │ +
167size_t Expression<T>::traceSize() const {
│ │ │ │ +
168 return root_->traceSize();
│ │ │ │ +
169}
│ │ │ │ +
│ │ │ │ +
170
│ │ │ │ +
│ │ │ │ +
171// Private methods:
│ │ │ │ +
172
│ │ │ │ +
173template<typename T>
│ │ │ │ +
│ │ │ │ +
174T Expression<T>::valueAndDerivatives(const Values& values,
│ │ │ │ +
175 const KeyVector& keys, const FastVector<int>& dims,
│ │ │ │ +
176 std::vector<Matrix>& H) const {
│ │ │ │ +
│ │ │ │ +
177
│ │ │ │ +
178 // H should be pre-allocated
│ │ │ │ +
179 assert(H.size()==keys.size());
│ │ │ │ +
180
│ │ │ │ +
181 // Pre-allocate and zero VerticalBlockMatrix
│ │ │ │ +
182 static const int Dim = traits<T>::dimension;
│ │ │ │ +
183 VerticalBlockMatrix Ab(dims, Dim);
│ │ │ │ +
184 Ab.matrix().setZero();
│ │ │ │ +
185 internal::JacobianMap jacobianMap(keys, Ab);
│ │ │ │ +
186
│ │ │ │ +
187 // Call unsafe version
│ │ │ │ +
188 T result = valueAndJacobianMap(values, jacobianMap);
│ │ │ │ +
│ │ │ │ +
189
│ │ │ │ +
190 // Copy blocks into the vector of jacobians passed in
│ │ │ │ +
191 for (DenseIndex i = 0; i < static_cast<DenseIndex>(keys.size()); i++)
│ │ │ │ +
192 H[i] = Ab(i);
│ │ │ │ +
│ │ │ │ +
193
│ │ │ │ +
194 return result;
│ │ │ │ +
195}
│ │ │ │ +
196
│ │ │ │ +
│ │ │ │ +
197template<typename T>
│ │ │ │ +
│ │ │ │ +
198T Expression<T>::traceExecution(const Values& values,
│ │ │ │ +
199 internal::ExecutionTrace<T>& trace, void* traceStorage) const {
│ │ │ │ +
200 return root_->traceExecution(values, trace,
│ │ │ │ +
201 static_cast<internal::ExecutionTraceStorage*>(traceStorage));
│ │ │ │ +
202}
│ │ │ │ +
│ │ │ │ +
203
│ │ │ │ +
204template<typename T>
│ │ │ │ +
│ │ │ │ +
205T Expression<T>::valueAndJacobianMap(const Values& values,
│ │ │ │ +
206 internal::JacobianMap& jacobians) const {
│ │ │ │ +
207 // The following piece of code is absolutely crucial for performance.
│ │ │ │ +
208 // We allocate a block of memory on the stack, which can be done at runtime
│ │ │ │ +
209 // with modern C++ compilers. The traceExecution then fills this memory
│ │ │ │ +
210 // with an execution trace, made up entirely of "Record" structs, see
│ │ │ │ +
211 // the FunctionalNode class in expression-inl.h
│ │ │ │ +
212 size_t size = traceSize();
│ │ │ │
213
│ │ │ │ -
214 // Also mark any provided extra re-eliminate keys
│ │ │ │ -
215 if (updateParams_.extraReelimKeys) {
│ │ │ │ -
216 for (Key key : *updateParams_.extraReelimKeys) {
│ │ │ │ -
217 markedKeys->insert(key);
│ │ │ │ -
218 }
│ │ │ │ -
219 }
│ │ │ │ -
220
│ │ │ │ -
221 // Also, keys that were not observed in existing factors, but whose affected
│ │ │ │ -
222 // keys have been extended now (e.g. smart factors)
│ │ │ │ -
223 if (updateParams_.newAffectedKeys) {
│ │ │ │ -
224 for (const auto& factorAddedKeys : *updateParams_.newAffectedKeys) {
│ │ │ │ -
225 const auto factorIdx = factorAddedKeys.first;
│ │ │ │ -
226 const auto& affectedKeys = nonlinearFactors.at(factorIdx)->keys();
│ │ │ │ -
227 markedKeys->insert(affectedKeys.begin(), affectedKeys.end());
│ │ │ │ -
228 }
│ │ │ │ -
229 }
│ │ │ │ -
230 }
│ │ │ │ -
231
│ │ │ │ -
232 // Update detail, unused, and observed keys from markedKeys
│ │ │ │ -
233 void updateKeys(const KeySet& markedKeys, ISAM2Result* result) const {
│ │ │ │ -
234 gttic(updateKeys);
│ │ │ │ -
235 // Observed keys for detailed results
│ │ │ │ -
236 if (result->detail && params_.enableDetailedResults) {
│ │ │ │ -
237 for (Key key : markedKeys) {
│ │ │ │ -
238 result->detail->variableStatus[key].isObserved = true;
│ │ │ │ -
239 }
│ │ │ │ -
240 }
│ │ │ │ -
241
│ │ │ │ -
242 for (Key index : markedKeys) {
│ │ │ │ -
243 // Only add if not unused
│ │ │ │ -
244 if (result->unusedKeys.find(index) == result->unusedKeys.end())
│ │ │ │ -
245 // Make a copy of these, as we'll soon add to them
│ │ │ │ -
246 result->observedKeys.push_back(index);
│ │ │ │ -
247 }
│ │ │ │ -
248 }
│ │ │ │ -
249
│ │ │ │ -
250 static void CheckRelinearizationRecursiveMap(
│ │ │ │ -
251 const FastMap<char, Vector>& thresholds, const VectorValues& delta,
│ │ │ │ -
252 const ISAM2::sharedClique& clique, KeySet* relinKeys) {
│ │ │ │ -
253 // Check the current clique for relinearization
│ │ │ │ -
254 bool relinearize = false;
│ │ │ │ -
255 for (Key var : *clique->conditional()) {
│ │ │ │ -
256 // Find the threshold for this variable type
│ │ │ │ -
257 const Vector& threshold = thresholds.find(Symbol(var).chr())->second;
│ │ │ │ -
258
│ │ │ │ -
259 const Vector& deltaVar = delta[var];
│ │ │ │ -
260
│ │ │ │ -
261 // Verify the threshold vector matches the actual variable size
│ │ │ │ -
262 if (threshold.rows() != deltaVar.rows())
│ │ │ │ -
263 throw std::invalid_argument(
│ │ │ │ -
264 "Relinearization threshold vector dimensionality for '" +
│ │ │ │ -
265 std::string(1, Symbol(var).chr()) +
│ │ │ │ -
266 "' passed into iSAM2 parameters does not match actual variable "
│ │ │ │ -
267 "dimensionality.");
│ │ │ │ +
214 // Windows does not support variable length arrays, so memory must be dynamically
│ │ │ │ +
215 // allocated on Visual Studio. For more information see the issue below
│ │ │ │ +
216 // https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio
│ │ │ │ +
217#ifdef _MSC_VER
│ │ │ │ +
218 std::unique_ptr<void, void(*)(void*)> traceStorageDeleter(
│ │ │ │ +
219 _aligned_malloc(size, internal::TraceAlignment),
│ │ │ │ +
220 [](void *ptr){ _aligned_free(ptr); });
│ │ │ │ +
221 auto traceStorage = static_cast<internal::ExecutionTraceStorage*>(traceStorageDeleter.get());
│ │ │ │ +
222#else
│ │ │ │ +
223 internal::ExecutionTraceStorage traceStorage[size];
│ │ │ │ +
224#endif
│ │ │ │ +
225
│ │ │ │ +
226 internal::ExecutionTrace<T> trace;
│ │ │ │ +
227 T value(this->traceExecution(values, trace, traceStorage));
│ │ │ │ +
228 trace.startReverseAD1(jacobians);
│ │ │ │ +
229
│ │ │ │ +
230 return value;
│ │ │ │ +
231}
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
232
│ │ │ │ +
233template<typename T>
│ │ │ │ +
234typename Expression<T>::KeysAndDims Expression<T>::keysAndDims() const {
│ │ │ │ +
235 std::map<Key, int> map;
│ │ │ │ +
236 dims(map);
│ │ │ │ +
237 size_t n = map.size();
│ │ │ │ +
238 KeysAndDims pair = std::make_pair(KeyVector(n), FastVector<int>(n));
│ │ │ │ +
239 boost::copy(map | boost::adaptors::map_keys, pair.first.begin());
│ │ │ │ +
240 boost::copy(map | boost::adaptors::map_values, pair.second.begin());
│ │ │ │ +
241 return pair;
│ │ │ │ +
242}
│ │ │ │ +
243
│ │ │ │ +
244namespace internal {
│ │ │ │ +
245// http://stackoverflow.com/questions/16260445/boost-bind-to-operator
│ │ │ │ +
246template<class T>
│ │ │ │ +
│ │ │ │ +
247struct apply_compose {
│ │ │ │ +
248 typedef T result_type;
│ │ │ │ +
249 static const int Dim = traits<T>::dimension;
│ │ │ │ +
250 T operator()(const T& x, const T& y, OptionalJacobian<Dim, Dim> H1 =
│ │ │ │ +
251 boost::none, OptionalJacobian<Dim, Dim> H2 = boost::none) const {
│ │ │ │ +
252 return x.compose(y, H1, H2);
│ │ │ │ +
253 }
│ │ │ │ +
254};
│ │ │ │ +
│ │ │ │ +
255
│ │ │ │ +
256template <>
│ │ │ │ +
│ │ │ │ +
257struct apply_compose<double> {
│ │ │ │ +
258 double operator()(const double& x, const double& y,
│ │ │ │ +
259 OptionalJacobian<1, 1> H1 = boost::none,
│ │ │ │ +
260 OptionalJacobian<1, 1> H2 = boost::none) const {
│ │ │ │ +
261 if (H1) H1->setConstant(y);
│ │ │ │ +
262 if (H2) H2->setConstant(x);
│ │ │ │ +
263 return x * y;
│ │ │ │ +
264 }
│ │ │ │ +
265};
│ │ │ │ +
│ │ │ │ +
266
│ │ │ │ +
267}
│ │ │ │
268
│ │ │ │ -
269 // Check for relinearization
│ │ │ │ -
270 if ((deltaVar.array().abs() > threshold.array()).any()) {
│ │ │ │ -
271 relinKeys->insert(var);
│ │ │ │ -
272 relinearize = true;
│ │ │ │ -
273 }
│ │ │ │ -
274 }
│ │ │ │ -
275
│ │ │ │ -
276 // If this node was relinearized, also check its children
│ │ │ │ -
277 if (relinearize) {
│ │ │ │ -
278 for (const ISAM2::sharedClique& child : clique->children) {
│ │ │ │ -
279 CheckRelinearizationRecursiveMap(thresholds, delta, child, relinKeys);
│ │ │ │ -
280 }
│ │ │ │ -
281 }
│ │ │ │ -
282 }
│ │ │ │ -
283
│ │ │ │ -
284 static void CheckRelinearizationRecursiveDouble(
│ │ │ │ -
285 double threshold, const VectorValues& delta,
│ │ │ │ -
286 const ISAM2::sharedClique& clique, KeySet* relinKeys) {
│ │ │ │ -
287 // Check the current clique for relinearization
│ │ │ │ -
288 bool relinearize = false;
│ │ │ │ -
289 for (Key var : *clique->conditional()) {
│ │ │ │ -
290 double maxDelta = delta[var].lpNorm<Eigen::Infinity>();
│ │ │ │ -
291 if (maxDelta >= threshold) {
│ │ │ │ -
292 relinKeys->insert(var);
│ │ │ │ -
293 relinearize = true;
│ │ │ │ -
294 }
│ │ │ │ -
295 }
│ │ │ │ +
269// Global methods:
│ │ │ │ +
270
│ │ │ │ +
272template<typename T>
│ │ │ │ +
│ │ │ │ +
273Expression<T> operator*(const Expression<T>& expression1,
│ │ │ │ +
274 const Expression<T>& expression2) {
│ │ │ │ +
275 return Expression<T>(
│ │ │ │ +
276 std::bind(internal::apply_compose<T>(), std::placeholders::_1,
│ │ │ │ +
277 std::placeholders::_2, std::placeholders::_3,
│ │ │ │ +
278 std::placeholders::_4),
│ │ │ │ +
279 expression1, expression2);
│ │ │ │ +
280}
│ │ │ │ +
│ │ │ │ +
281
│ │ │ │ +
283template<typename T>
│ │ │ │ +
│ │ │ │ +
284std::vector<Expression<T> > createUnknowns(size_t n, char c, size_t start) {
│ │ │ │ +
285 std::vector<Expression<T> > unknowns;
│ │ │ │ +
286 unknowns.reserve(n);
│ │ │ │ +
287 for (size_t i = start; i < start + n; i++)
│ │ │ │ +
288 unknowns.push_back(Expression<T>(c, i));
│ │ │ │ +
289 return unknowns;
│ │ │ │ +
290}
│ │ │ │ +
│ │ │ │ +
291
│ │ │ │ +
292template <typename T>
│ │ │ │ +
293ScalarMultiplyExpression<T>::ScalarMultiplyExpression(double s, const Expression<T>& e)
│ │ │ │ +
294 : Expression<T>(boost::make_shared<internal::ScalarMultiplyNode<T>>(s, e)) {}
│ │ │ │ +
295
│ │ │ │
296
│ │ │ │ -
297 // If this node was relinearized, also check its children
│ │ │ │ -
298 if (relinearize) {
│ │ │ │ -
299 for (const ISAM2::sharedClique& child : clique->children) {
│ │ │ │ -
300 CheckRelinearizationRecursiveDouble(threshold, delta, child, relinKeys);
│ │ │ │ -
301 }
│ │ │ │ -
302 }
│ │ │ │ -
303 }
│ │ │ │ -
304
│ │ │ │ -
│ │ │ │ -
318 static KeySet CheckRelinearizationPartial(
│ │ │ │ -
319 const ISAM2::Roots& roots, const VectorValues& delta,
│ │ │ │ -
320 const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
│ │ │ │ -
321 KeySet relinKeys;
│ │ │ │ -
322 for (const ISAM2::sharedClique& root : roots) {
│ │ │ │ -
323 if (relinearizeThreshold.type() == typeid(double))
│ │ │ │ -
324 CheckRelinearizationRecursiveDouble(
│ │ │ │ -
325 boost::get<double>(relinearizeThreshold), delta, root, &relinKeys);
│ │ │ │ -
326 else if (relinearizeThreshold.type() == typeid(FastMap<char, Vector>))
│ │ │ │ -
327 CheckRelinearizationRecursiveMap(
│ │ │ │ -
328 boost::get<FastMap<char, Vector> >(relinearizeThreshold), delta,
│ │ │ │ -
329 root, &relinKeys);
│ │ │ │ -
330 }
│ │ │ │ -
331 return relinKeys;
│ │ │ │ -
332 }
│ │ │ │ -
│ │ │ │ -
333
│ │ │ │ -
│ │ │ │ -
345 static KeySet CheckRelinearizationFull(
│ │ │ │ -
346 const VectorValues& delta,
│ │ │ │ -
347 const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
│ │ │ │ -
348 KeySet relinKeys;
│ │ │ │ -
349
│ │ │ │ -
350 if (const double* threshold = boost::get<double>(&relinearizeThreshold)) {
│ │ │ │ -
351 for (const VectorValues::KeyValuePair& key_delta : delta) {
│ │ │ │ -
352 double maxDelta = key_delta.second.lpNorm<Eigen::Infinity>();
│ │ │ │ -
353 if (maxDelta >= *threshold) relinKeys.insert(key_delta.first);
│ │ │ │ -
354 }
│ │ │ │ -
355 } else if (const FastMap<char, Vector>* thresholds =
│ │ │ │ -
356 boost::get<FastMap<char, Vector> >(&relinearizeThreshold)) {
│ │ │ │ -
357 for (const VectorValues::KeyValuePair& key_delta : delta) {
│ │ │ │ -
358 const Vector& threshold =
│ │ │ │ -
359 thresholds->find(Symbol(key_delta.first).chr())->second;
│ │ │ │ -
360 if (threshold.rows() != key_delta.second.rows())
│ │ │ │ -
361 throw std::invalid_argument(
│ │ │ │ -
362 "Relinearization threshold vector dimensionality for '" +
│ │ │ │ -
363 std::string(1, Symbol(key_delta.first).chr()) +
│ │ │ │ -
364 "' passed into iSAM2 parameters does not match actual variable "
│ │ │ │ -
365 "dimensionality.");
│ │ │ │ -
366 if ((key_delta.second.array().abs() > threshold.array()).any())
│ │ │ │ -
367 relinKeys.insert(key_delta.first);
│ │ │ │ -
368 }
│ │ │ │ -
369 }
│ │ │ │ -
370
│ │ │ │ -
371 return relinKeys;
│ │ │ │ -
372 }
│ │ │ │ -
│ │ │ │ -
373
│ │ │ │ -
374 // Mark keys in \Delta above threshold \beta:
│ │ │ │ -
375 KeySet gatherRelinearizeKeys(const ISAM2::Roots& roots,
│ │ │ │ -
376 const VectorValues& delta,
│ │ │ │ -
377 const KeySet& fixedVariables,
│ │ │ │ -
378 KeySet* markedKeys) const {
│ │ │ │ -
379 gttic(gatherRelinearizeKeys);
│ │ │ │ -
380 // J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
│ │ │ │ -
381 KeySet relinKeys =
│ │ │ │ -
382 params_.enablePartialRelinearizationCheck
│ │ │ │ -
383 ? CheckRelinearizationPartial(roots, delta,
│ │ │ │ -
384 params_.relinearizeThreshold)
│ │ │ │ -
385 : CheckRelinearizationFull(delta, params_.relinearizeThreshold);
│ │ │ │ -
386 if (updateParams_.forceFullSolve)
│ │ │ │ -
387 relinKeys = CheckRelinearizationFull(delta, 0.0); // for debugging
│ │ │ │ -
388
│ │ │ │ -
389 // Remove from relinKeys any keys whose linearization points are fixed
│ │ │ │ -
390 for (Key key : fixedVariables) {
│ │ │ │ -
391 relinKeys.erase(key);
│ │ │ │ -
392 }
│ │ │ │ -
393 if (updateParams_.noRelinKeys) {
│ │ │ │ -
394 for (Key key : *updateParams_.noRelinKeys) {
│ │ │ │ -
395 relinKeys.erase(key);
│ │ │ │ -
396 }
│ │ │ │ -
397 }
│ │ │ │ -
398
│ │ │ │ -
399 // Add the variables being relinearized to the marked keys
│ │ │ │ -
400 markedKeys->insert(relinKeys.begin(), relinKeys.end());
│ │ │ │ -
401 return relinKeys;
│ │ │ │ -
402 }
│ │ │ │ -
403
│ │ │ │ -
404 // Record relinerization threshold keys in detailed results
│ │ │ │ -
405 void recordRelinearizeDetail(const KeySet& relinKeys,
│ │ │ │ -
406 ISAM2Result::DetailedResults* detail) const {
│ │ │ │ -
407 if (detail && params_.enableDetailedResults) {
│ │ │ │ -
408 for (Key key : relinKeys) {
│ │ │ │ -
409 detail->variableStatus[key].isAboveRelinThreshold = true;
│ │ │ │ -
410 detail->variableStatus[key].isRelinearized = true;
│ │ │ │ -
411 }
│ │ │ │ -
412 }
│ │ │ │ -
413 }
│ │ │ │ -
414
│ │ │ │ -
415 // Mark all cliques that involve marked variables \Theta_{J} and all
│ │ │ │ -
416 // their ancestors.
│ │ │ │ -
417 void findFluid(const ISAM2::Roots& roots, const KeySet& relinKeys,
│ │ │ │ -
418 KeySet* markedKeys,
│ │ │ │ -
419 ISAM2Result::DetailedResults* detail) const {
│ │ │ │ -
420 gttic(findFluid);
│ │ │ │ -
421 for (const auto& root : roots)
│ │ │ │ -
422 // add other cliques that have the marked ones in the separator
│ │ │ │ -
423 root->findAll(relinKeys, markedKeys);
│ │ │ │ -
424
│ │ │ │ -
425 // Relinearization-involved keys for detailed results
│ │ │ │ -
426 if (detail && params_.enableDetailedResults) {
│ │ │ │ -
427 KeySet involvedRelinKeys;
│ │ │ │ -
428 for (const auto& root : roots)
│ │ │ │ -
429 root->findAll(relinKeys, &involvedRelinKeys);
│ │ │ │ -
430 for (Key key : involvedRelinKeys) {
│ │ │ │ -
431 if (!detail->variableStatus[key].isAboveRelinThreshold) {
│ │ │ │ -
432 detail->variableStatus[key].isRelinearizeInvolved = true;
│ │ │ │ -
433 detail->variableStatus[key].isRelinearized = true;
│ │ │ │ -
434 }
│ │ │ │ -
435 }
│ │ │ │ -
436 }
│ │ │ │ -
437 }
│ │ │ │ -
438
│ │ │ │ -
439 // Linearize new factors
│ │ │ │ -
440 void linearizeNewFactors(const NonlinearFactorGraph& newFactors,
│ │ │ │ -
441 const Values& theta, size_t numNonlinearFactors,
│ │ │ │ -
442 const FactorIndices& newFactorsIndices,
│ │ │ │ -
443 GaussianFactorGraph* linearFactors) const {
│ │ │ │ -
444 gttic(linearizeNewFactors);
│ │ │ │ -
445 auto linearized = newFactors.linearize(theta);
│ │ │ │ -
446 if (params_.findUnusedFactorSlots) {
│ │ │ │ -
447 linearFactors->resize(numNonlinearFactors);
│ │ │ │ -
448 for (size_t i = 0; i < newFactors.size(); ++i)
│ │ │ │ -
449 (*linearFactors)[newFactorsIndices[i]] = (*linearized)[i];
│ │ │ │ -
450 } else {
│ │ │ │ -
451 linearFactors->push_back(*linearized);
│ │ │ │ -
452 }
│ │ │ │ -
453 assert(linearFactors->size() == numNonlinearFactors);
│ │ │ │ -
454 }
│ │ │ │ -
455
│ │ │ │ -
456 void augmentVariableIndex(const NonlinearFactorGraph& newFactors,
│ │ │ │ -
457 const FactorIndices& newFactorsIndices,
│ │ │ │ -
458 VariableIndex* variableIndex) const {
│ │ │ │ -
459 gttic(augmentVariableIndex);
│ │ │ │ -
460 // Augment the variable index with the new factors
│ │ │ │ -
461 if (params_.findUnusedFactorSlots)
│ │ │ │ -
462 variableIndex->augment(newFactors, newFactorsIndices);
│ │ │ │ -
463 else
│ │ │ │ -
464 variableIndex->augment(newFactors);
│ │ │ │ -
465
│ │ │ │ -
466 // Augment it with existing factors which now affect to more variables:
│ │ │ │ -
467 if (updateParams_.newAffectedKeys) {
│ │ │ │ -
468 for (const auto& factorAddedKeys : *updateParams_.newAffectedKeys) {
│ │ │ │ -
469 const auto factorIdx = factorAddedKeys.first;
│ │ │ │ -
470 variableIndex->augmentExistingFactor(factorIdx, factorAddedKeys.second);
│ │ │ │ -
471 }
│ │ │ │ -
472 }
│ │ │ │ -
473 }
│ │ │ │ -
474
│ │ │ │ -
475 static void LogRecalculateKeys(const ISAM2Result& result) {
│ │ │ │ -
476 const bool debug = ISDEBUG("ISAM2 recalculate");
│ │ │ │ -
477
│ │ │ │ -
478 if (debug) {
│ │ │ │ -
479 std::cout << "markedKeys: ";
│ │ │ │ -
480 for (const Key key : result.markedKeys) {
│ │ │ │ -
481 std::cout << key << " ";
│ │ │ │ -
482 }
│ │ │ │ -
483 std::cout << std::endl;
│ │ │ │ -
484 std::cout << "observedKeys: ";
│ │ │ │ -
485 for (const Key key : result.observedKeys) {
│ │ │ │ -
486 std::cout << key << " ";
│ │ │ │ -
487 }
│ │ │ │ -
488 std::cout << std::endl;
│ │ │ │ -
489 }
│ │ │ │ -
490 }
│ │ │ │ -
491
│ │ │ │ -
492 static FactorIndexSet GetAffectedFactors(const KeyList& keys,
│ │ │ │ -
493 const VariableIndex& variableIndex) {
│ │ │ │ -
494 gttic(GetAffectedFactors);
│ │ │ │ -
495 FactorIndexSet indices;
│ │ │ │ -
496 for (const Key key : keys) {
│ │ │ │ -
497 const FactorIndices& factors(variableIndex[key]);
│ │ │ │ -
498 indices.insert(factors.begin(), factors.end());
│ │ │ │ -
499 }
│ │ │ │ -
500 return indices;
│ │ │ │ -
501 }
│ │ │ │ -
502
│ │ │ │ -
503 // find intermediate (linearized) factors from cache that are passed into
│ │ │ │ -
504 // the affected area
│ │ │ │ -
505 static GaussianFactorGraph GetCachedBoundaryFactors(
│ │ │ │ -
506 const ISAM2::Cliques& orphans) {
│ │ │ │ -
507 GaussianFactorGraph cachedBoundary;
│ │ │ │ -
508
│ │ │ │ -
509 for (const auto& orphan : orphans) {
│ │ │ │ -
510 // retrieve the cached factor and add to boundary
│ │ │ │ -
511 cachedBoundary.push_back(orphan->cachedFactor());
│ │ │ │ -
512 }
│ │ │ │ -
513
│ │ │ │ -
514 return cachedBoundary;
│ │ │ │ -
515 }
│ │ │ │ -
516};
│ │ │ │ -
│ │ │ │ -
517
│ │ │ │ -
518} // namespace gtsam
│ │ │ │ -
debug.h
Global debugging flags.
│ │ │ │ -
JunctionTree-inst.h
The junction tree, template bodies.
│ │ │ │ - │ │ │ │ - │ │ │ │ -
GaussianBayesTree.h
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
│ │ │ │ -
ISAM2Result.h
Class that stores detailed iSAM2 result.
│ │ │ │ -
ISAM2.h
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
│ │ │ │ +
297template <typename T>
│ │ │ │ +
298BinarySumExpression<T>::BinarySumExpression(const Expression<T>& e1, const Expression<T>& e2)
│ │ │ │ +
299 : Expression<T>(boost::make_shared<internal::BinarySumNode<T>>(e1, e2)) {}
│ │ │ │ +
300
│ │ │ │ +
301template <typename T>
│ │ │ │ +
│ │ │ │ +
302Expression<T>& Expression<T>::operator+=(const Expression<T>& e) {
│ │ │ │ +
303 root_ = boost::make_shared<internal::BinarySumNode<T>>(*this, e);
│ │ │ │ +
304 return *this;
│ │ │ │ +
305}
│ │ │ │ +
│ │ │ │ +
306
│ │ │ │ +
307} // namespace gtsam
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition Factor.h:34
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │ +
gtsam::symbol
Key symbol(unsigned char c, std::uint64_t j)
Create a symbol key from a character and index, i.e.
Definition Symbol.h:135
│ │ │ │ +
gtsam::createUnknowns
std::vector< Expression< T > > createUnknowns(size_t n, char c, size_t start)
Construct an array of leaves.
Definition Expression-inl.h:284
│ │ │ │ +
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
│ │ │ │ +
gtsam::make_shared
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost::shared_ptr< T > > make_shared(Args &&... args)
Add our own make_shared as a layer of wrapping on boost::make_shared This solves the problem with the...
Definition make_shared.h:57
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ - │ │ │ │ -
gtsam::FactorGraph::keys
KeySet keys() const
Potentially slow function to return all keys involved, sorted, as a set.
Definition FactorGraph-inst.h:85
│ │ │ │ -
gtsam::FactorGraph::add_factors
FactorIndices add_factors(const CONTAINER &factors, bool useEmptySlots=false)
Add new factors to a factor graph and returns a list of new factor indices, optionally finding and re...
Definition FactorGraph-inst.h:109
│ │ │ │ -
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition FactorGraph.h:186
│ │ │ │ -
gtsam::FactorGraph::remove
void remove(size_t i)
delete factor without re-arranging indexes by inserting a nullptr pointer
Definition FactorGraph.h:385
│ │ │ │ -
gtsam::FactorGraph::at
const sharedFactor at(size_t i) const
Get a specific factor by index (this checks array bounds and may throw an exception,...
Definition FactorGraph.h:335
│ │ │ │ -
gtsam::FactorGraph::reserve
void reserve(size_t size)
Reserve space for the specified number of factors if you know in advance how many there will be (work...
Definition FactorGraph.h:182
│ │ │ │ - │ │ │ │ -
gtsam::BayesTree::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print
Definition BayesTree-inst.h:212
│ │ │ │ -
gtsam::BayesTree< ISAM2Clique >::Roots
FastVector< sharedClique > Roots
Root cliques.
Definition BayesTree.h:95
│ │ │ │ -
gtsam::JunctionTree
A JunctionTree is a cluster tree, a set of variable clusters with factors, arranged in a tree,...
Definition JunctionTree.h:50
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ +
gtsam::MakeOptionalJacobian
: meta-function to generate JacobianTA optional reference Used mainly by Expressions
Definition OptionalJacobian.h:261
│ │ │ │ +
gtsam::VerticalBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43
│ │ │ │
gtsam::Symbol
Character and index key used to refer to variables.
Definition Symbol.h:35
│ │ │ │ -
gtsam::Symbol::chr
unsigned char chr() const
Retrieve key character.
Definition Symbol.h:73
│ │ │ │ -
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition VariableIndex.h:43
│ │ │ │ -
gtsam::VariableIndex::remove
void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG &factors)
Remove entries corresponding to the specified factors.
Definition VariableIndex-inl.h:54
│ │ │ │ -
gtsam::VariableIndex::empty
bool empty(Key variable) const
Return true if no factors associated with a variable.
Definition VariableIndex.h:98
│ │ │ │ -
gtsam::GaussianEliminationTree
Definition GaussianEliminationTree.h:29
│ │ │ │ -
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ -
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ -
gtsam::ISAM2BayesTree
Definition ISAM2-impl.h:48
│ │ │ │ -
gtsam::ISAM2JunctionTree
Definition ISAM2-impl.h:61
│ │ │ │ -
gtsam::DeltaImpl
Definition ISAM2-impl.h:72
│ │ │ │ -
gtsam::DeltaImpl::PartialSolveResult
Definition ISAM2-impl.h:73
│ │ │ │ -
gtsam::DeltaImpl::ReorderingMode
Definition ISAM2-impl.h:77
│ │ │ │ -
gtsam::UpdateImpl
Implementation functions for update method All of the methods below have clear inputs and outputs,...
Definition ISAM2-impl.h:114
│ │ │ │ -
gtsam::UpdateImpl::CheckRelinearizationFull
static KeySet CheckRelinearizationFull(const VectorValues &delta, const ISAM2Params::RelinearizationThreshold &relinearizeThreshold)
Find the set of variables to be relinearized according to relinearizeThreshold.
Definition ISAM2-impl.h:345
│ │ │ │ -
gtsam::UpdateImpl::CheckRelinearizationPartial
static KeySet CheckRelinearizationPartial(const ISAM2::Roots &roots, const VectorValues &delta, const ISAM2Params::RelinearizationThreshold &relinearizeThreshold)
Find the set of variables to be relinearized according to relinearizeThreshold.
Definition ISAM2-impl.h:318
│ │ │ │ -
gtsam::ISAM2
Implementation of the full ISAM2 algorithm for incremental nonlinear optimization.
Definition ISAM2.h:45
│ │ │ │ -
gtsam::ISAM2::sharedClique
Base::sharedClique sharedClique
Shared pointer to a clique.
Definition ISAM2.h:103
│ │ │ │ -
gtsam::ISAM2Params
Definition ISAM2Params.h:135
│ │ │ │ -
gtsam::ISAM2Params::RelinearizationThreshold
boost::variant< double, FastMap< char, Vector > > RelinearizationThreshold
Either a constant relinearization threshold or a per-variable-type set of thresholds.
Definition ISAM2Params.h:140
│ │ │ │ -
gtsam::ISAM2Result
This struct is returned from ISAM2::update() and contains information about the update that is useful...
Definition ISAM2Result.h:41
│ │ │ │ -
gtsam::ISAM2UpdateParams
This struct is used by ISAM2::update() to pass additional parameters to give the user a fine-grained ...
Definition ISAM2UpdateParams.h:32
│ │ │ │ -
gtsam::ISAM2UpdateParams::force_relinearize
bool force_relinearize
Relinearize any variables whose delta magnitude is sufficiently large (Params::relinearizeThreshold),...
Definition ISAM2UpdateParams.h:54
│ │ │ │ -
gtsam::ISAM2UpdateParams::removeFactorIndices
FactorIndices removeFactorIndices
Indices of factors to remove from system (default: empty)
Definition ISAM2UpdateParams.h:36
│ │ │ │ -
gtsam::ISAM2UpdateParams::forceFullSolve
bool forceFullSolve
By default, iSAM2 uses a wildfire update scheme that stops updating when the deltas become too small ...
Definition ISAM2UpdateParams.h:71
│ │ │ │ -
gtsam::ISAM2UpdateParams::extraReelimKeys
boost::optional< FastList< Key > > extraReelimKeys
An optional set of nonlinear keys that iSAM2 will re-eliminate, regardless of the size of the linear ...
Definition ISAM2UpdateParams.h:49
│ │ │ │ -
gtsam::ISAM2UpdateParams::noRelinKeys
boost::optional< FastList< Key > > noRelinKeys
An optional set of nonlinear keys that iSAM2 will hold at a constant linearization point,...
Definition ISAM2UpdateParams.h:44
│ │ │ │ -
gtsam::ISAM2UpdateParams::newAffectedKeys
boost::optional< FastMap< FactorIndex, KeySet > > newAffectedKeys
An optional set of new Keys that are now affected by factors, indexed by factor indices (as returned ...
Definition ISAM2UpdateParams.h:66
│ │ │ │ -
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │ -
gtsam::NonlinearFactorGraph::print
void print(const std::string &str="NonlinearFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition NonlinearFactorGraph.cpp:55
│ │ │ │ -
gtsam::NonlinearFactorGraph::error
double error(const Values &values) const
unnormalized error, in the most common case
Definition NonlinearFactorGraph.cpp:170
│ │ │ │ +
gtsam::internal::apply_compose
Definition Expression-inl.h:247
│ │ │ │ +
gtsam::internal::ExecutionTrace
Definition Expression.h:40
│ │ │ │ +
gtsam::Expression
Expression class that supports automatic differentiation.
Definition Expression.h:48
│ │ │ │ +
gtsam::Expression::Expression
Expression()
Default constructor, for serialization.
Definition Expression.h:182
│ │ │ │ +
gtsam::Expression::keys
std::set< Key > keys() const
Return keys that play in this expression.
Definition Expression-inl.h:132
│ │ │ │ +
gtsam::Expression::KeysAndDims
std::pair< KeyVector, FastVector< int > > KeysAndDims
Keys and dimensions in same order.
Definition Expression.h:185
│ │ │ │ +
gtsam::Expression::dims
void dims(std::map< Key, int > &map) const
Return dimensions for each argument, as a map.
Definition Expression-inl.h:137
│ │ │ │ +
gtsam::Expression::print
void print(const std::string &s) const
Print.
Definition Expression-inl.h:142
│ │ │ │ +
gtsam::Expression::traceSize
size_t traceSize() const
Return size needed for memory buffer in traceExecution.
Definition Expression-inl.h:167
│ │ │ │ +
gtsam::Expression::traceExecution
T traceExecution(const Values &values, internal::ExecutionTrace< T > &trace, void *traceStorage) const
trace execution, very unsafe
Definition Expression-inl.h:198
│ │ │ │
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
gtsam::Values::KeyValuePair
A key-value pair, which you get by dereferencing iterators.
Definition Values.h:93
│ │ │ │ -
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │ - │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,689 +1,408 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ISAM2-impl.h │ │ │ │ │ +Expression-inl.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_I_S_A_M_2_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_I_S_A_M_2_R_e_s_u_l_t_._h> │ │ │ │ │ -23 │ │ │ │ │ -24#include <_g_t_s_a_m_/_b_a_s_e_/_d_e_b_u_g_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_J_u_n_c_t_i_o_n_T_r_e_e_-_i_n_s_t_._h> // We need the inst file │ │ │ │ │ -because we'll make a special JT templated on ISAM2 │ │ │ │ │ -26#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_S_y_m_b_o_l_._h> │ │ │ │ │ -27#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_V_a_r_i_a_b_l_e_I_n_d_e_x_._h> │ │ │ │ │ -28#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ -29#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h> │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22// The MSVC compiler workaround for the unsupported variable length array │ │ │ │ │ +23// utilizes the std::unique_ptr<> custom deleter. │ │ │ │ │ +24// See Expression::valueAndJacobianMap() below. │ │ │ │ │ +25#ifdef _MSC_VER │ │ │ │ │ +26#include │ │ │ │ │ +27#endif │ │ │ │ │ +28 │ │ │ │ │ +29#include │ │ │ │ │ 30 │ │ │ │ │ -31#include │ │ │ │ │ -32#include │ │ │ │ │ -33namespace br { │ │ │ │ │ -34using namespace boost::range; │ │ │ │ │ -35using namespace boost::adaptors; │ │ │ │ │ -36} // namespace br │ │ │ │ │ +31#include │ │ │ │ │ +32#include │ │ │ │ │ +33#include │ │ │ │ │ +34#include │ │ │ │ │ +35 │ │ │ │ │ +36namespace _g_t_s_a_m { │ │ │ │ │ 37 │ │ │ │ │ -38#include │ │ │ │ │ -39#include │ │ │ │ │ -40#include │ │ │ │ │ -41#include │ │ │ │ │ +38template │ │ │ │ │ +_3_9_E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_E_x_p_r_e_s_s_i_o_n(const T& value) : │ │ │ │ │ +40 root_(new internal::ConstantExpression(value)) { │ │ │ │ │ +41} │ │ │ │ │ 42 │ │ │ │ │ -43namespace _g_t_s_a_m { │ │ │ │ │ -44 │ │ │ │ │ -45/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -46// Special BayesTree class that uses ISAM2 cliques - this is the result of │ │ │ │ │ -47// reeliminating ISAM2 subtrees. │ │ │ │ │ -_4_8class _I_S_A_M_2_B_a_y_e_s_T_r_e_e : public _I_S_A_M_2_:_:_B_a_s_e { │ │ │ │ │ -49 public: │ │ │ │ │ -50 typedef _I_S_A_M_2_:_:_B_a_s_e _B_a_s_e; │ │ │ │ │ -51 typedef _I_S_A_M_2_B_a_y_e_s_T_r_e_e _T_h_i_s; │ │ │ │ │ -52 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -53 │ │ │ │ │ -54 _I_S_A_M_2_B_a_y_e_s_T_r_e_e() {} │ │ │ │ │ -55}; │ │ │ │ │ -56 │ │ │ │ │ -57/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -58// Special JunctionTree class that produces ISAM2 BayesTree cliques, used for │ │ │ │ │ -59// reeliminating ISAM2 subtrees. │ │ │ │ │ -_6_0class _I_S_A_M_2_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ -61 : public _J_u_n_c_t_i_o_n_T_r_e_e { │ │ │ │ │ -62 public: │ │ │ │ │ -63 typedef _J_u_n_c_t_i_o_n_T_r_e_e_<_I_S_A_M_2_B_a_y_e_s_T_r_e_e_,_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_> _B_a_s_e; │ │ │ │ │ -64 typedef _I_S_A_M_2_J_u_n_c_t_i_o_n_T_r_e_e _T_h_i_s; │ │ │ │ │ -65 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -66 │ │ │ │ │ -67 explicit _I_S_A_M_2_J_u_n_c_t_i_o_n_T_r_e_e(const _G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e& eliminationTree) │ │ │ │ │ -68 : _B_a_s_e(eliminationTree) {} │ │ │ │ │ -69}; │ │ │ │ │ -70 │ │ │ │ │ -71/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -_7_2struct GTSAM_EXPORT _D_e_l_t_a_I_m_p_l { │ │ │ │ │ -_7_3 struct GTSAM_EXPORT _P_a_r_t_i_a_l_S_o_l_v_e_R_e_s_u_l_t { │ │ │ │ │ -74 _I_S_A_M_2_:_:_s_h_a_r_e_d_C_l_i_q_u_e bayesTree; │ │ │ │ │ -75 }; │ │ │ │ │ -76 │ │ │ │ │ -_7_7 struct GTSAM_EXPORT _R_e_o_r_d_e_r_i_n_g_M_o_d_e { │ │ │ │ │ -78 size_t nFullSystemVars; │ │ │ │ │ -79 enum { /*AS_ADDED,*/ COLAMD } algorithm; │ │ │ │ │ -80 enum { NO_CONSTRAINT, CONSTRAIN_LAST } constrain; │ │ │ │ │ -81 boost::optional > constrainedKeys; │ │ │ │ │ -82 }; │ │ │ │ │ -83 │ │ │ │ │ -87 static size_t UpdateGaussNewtonDelta(const ISAM2::Roots& roots, │ │ │ │ │ -88 const _K_e_y_S_e_t& replacedKeys, │ │ │ │ │ -89 double wildfireThreshold, │ │ │ │ │ -90 _V_e_c_t_o_r_V_a_l_u_e_s* delta); │ │ │ │ │ -91 │ │ │ │ │ -96 static size_t UpdateRgProd(const ISAM2::Roots& roots, │ │ │ │ │ -97 const _K_e_y_S_e_t& replacedKeys, │ │ │ │ │ -98 const _V_e_c_t_o_r_V_a_l_u_e_s& gradAtZero, │ │ │ │ │ -99 _V_e_c_t_o_r_V_a_l_u_e_s* RgProd); │ │ │ │ │ -100 │ │ │ │ │ -104 static _V_e_c_t_o_r_V_a_l_u_e_s ComputeGradientSearch(const _V_e_c_t_o_r_V_a_l_u_e_s& gradAtZero, │ │ │ │ │ -105 const _V_e_c_t_o_r_V_a_l_u_e_s& RgProd); │ │ │ │ │ -106}; │ │ │ │ │ -107 │ │ │ │ │ -108/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -_1_1_4struct GTSAM_EXPORT _U_p_d_a_t_e_I_m_p_l { │ │ │ │ │ -115 const _I_S_A_M_2_P_a_r_a_m_s& params_; │ │ │ │ │ -116 const _I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s& updateParams_; │ │ │ │ │ -117 _U_p_d_a_t_e_I_m_p_l(const _I_S_A_M_2_P_a_r_a_m_s& params, const _I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s& │ │ │ │ │ -updateParams) │ │ │ │ │ -118 : params_(params), updateParams_(updateParams) {} │ │ │ │ │ -119 │ │ │ │ │ -120 // Provide some debugging information at the start of update │ │ │ │ │ -121 static void LogStartingUpdate(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& newFactors, │ │ │ │ │ -122 const _I_S_A_M_2& isam2) { │ │ │ │ │ -123 gttic(pushBackFactors); │ │ │ │ │ -124 const bool debug = ISDEBUG("ISAM2 update"); │ │ │ │ │ -125 const bool verbose = ISDEBUG("ISAM2 update verbose"); │ │ │ │ │ -126 │ │ │ │ │ -127 if (verbose) { │ │ │ │ │ -128 std::cout << "ISAM2::update\n"; │ │ │ │ │ -129 isam2._p_r_i_n_t("ISAM2: "); │ │ │ │ │ -130 } │ │ │ │ │ -131 │ │ │ │ │ -132 if (debug || verbose) { │ │ │ │ │ -133 newFactors._p_r_i_n_t("The new factors are: "); │ │ │ │ │ -134 } │ │ │ │ │ -135 } │ │ │ │ │ -136 │ │ │ │ │ -137 // Check relinearization if we're at the nth step, or we are using a looser │ │ │ │ │ -138 // loop relinerization threshold. │ │ │ │ │ -139 bool relinarizationNeeded(size_t update_count) const { │ │ │ │ │ -140 return updateParams_._f_o_r_c_e___r_e_l_i_n_e_a_r_i_z_e || │ │ │ │ │ -141 (params_.enableRelinearization && │ │ │ │ │ -142 update_count % params_.relinearizeSkip == 0); │ │ │ │ │ -143 } │ │ │ │ │ -144 │ │ │ │ │ -145 // Add any new factors \Factors:=\Factors\cup\Factors'. │ │ │ │ │ -146 void pushBackFactors(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& newFactors, │ │ │ │ │ -147 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h* nonlinearFactors, │ │ │ │ │ -148 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h* linearFactors, │ │ │ │ │ -149 _V_a_r_i_a_b_l_e_I_n_d_e_x* variableIndex, │ │ │ │ │ -150 _F_a_c_t_o_r_I_n_d_i_c_e_s* newFactorsIndices, │ │ │ │ │ -151 _K_e_y_S_e_t* keysWithRemovedFactors) const { │ │ │ │ │ -152 gttic(pushBackFactors); │ │ │ │ │ -153 │ │ │ │ │ -154 // Perform the first part of the bookkeeping updates for adding new │ │ │ │ │ -factors. │ │ │ │ │ -155 // Adds them to the complete list of nonlinear factors, and populates the │ │ │ │ │ -156 // list of new factor indices, both optionally finding and reusing empty │ │ │ │ │ -157 // factor slots. │ │ │ │ │ -158 *newFactorsIndices = nonlinearFactors->_a_d_d___f_a_c_t_o_r_s( │ │ │ │ │ -159 newFactors, params_.findUnusedFactorSlots); │ │ │ │ │ +43template │ │ │ │ │ +_4_4_E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_E_x_p_r_e_s_s_i_o_n(const _K_e_y& key) : │ │ │ │ │ +45 root_(new internal::LeafExpression(key)) { │ │ │ │ │ +46} │ │ │ │ │ +47 │ │ │ │ │ +48template │ │ │ │ │ +_4_9_E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_E_x_p_r_e_s_s_i_o_n(const _S_y_m_b_o_l& _s_y_m_b_o_l) : │ │ │ │ │ +50 root_(new internal::LeafExpression(_s_y_m_b_o_l)) { │ │ │ │ │ +51} │ │ │ │ │ +52 │ │ │ │ │ +53template │ │ │ │ │ +_5_4_E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_E_x_p_r_e_s_s_i_o_n(unsigned char c, std::uint64_t j) : │ │ │ │ │ +55 root_(new internal::LeafExpression(_S_y_m_b_o_l(c, j))) { │ │ │ │ │ +56} │ │ │ │ │ +57 │ │ │ │ │ +59template │ │ │ │ │ +60template │ │ │ │ │ +_6_1_E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_E_x_p_r_e_s_s_i_o_n(typename UnaryFunction::type function, │ │ │ │ │ +62 const _E_x_p_r_e_s_s_i_o_n_<_A_>& expression) : │ │ │ │ │ +63 root_(new internal::UnaryExpression(function, expression)) { │ │ │ │ │ +64} │ │ │ │ │ +65 │ │ │ │ │ +67template │ │ │ │ │ +68template │ │ │ │ │ +_6_9_E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_E_x_p_r_e_s_s_i_o_n(typename BinaryFunction::type function, │ │ │ │ │ +70 const _E_x_p_r_e_s_s_i_o_n_<_A_1_>& expression1, const _E_x_p_r_e_s_s_i_o_n_<_A_2_>& expression2) : │ │ │ │ │ +71 root_( │ │ │ │ │ +72 new internal::BinaryExpression(function, expression1, │ │ │ │ │ +73 expression2)) { │ │ │ │ │ +74} │ │ │ │ │ +75 │ │ │ │ │ +77template │ │ │ │ │ +78template │ │ │ │ │ +_7_9_E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_E_x_p_r_e_s_s_i_o_n(typename TernaryFunction::type │ │ │ │ │ +function, │ │ │ │ │ +80 const _E_x_p_r_e_s_s_i_o_n_<_A_1_>& expression1, const _E_x_p_r_e_s_s_i_o_n_<_A_2_>& expression2, │ │ │ │ │ +81 const _E_x_p_r_e_s_s_i_o_n_<_A_3_>& expression3) : │ │ │ │ │ +82 root_( │ │ │ │ │ +83 new internal::TernaryExpression(function, expression1, │ │ │ │ │ +84 expression2, expression3)) { │ │ │ │ │ +85} │ │ │ │ │ +86 │ │ │ │ │ +88template │ │ │ │ │ +89template │ │ │ │ │ +_9_0_E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_E_x_p_r_e_s_s_i_o_n(const _E_x_p_r_e_s_s_i_o_n_<_A_>& expression, │ │ │ │ │ +91 T (A::*method)(typename _M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n::_t_y_p_e) const) : │ │ │ │ │ +_9_2 root_( │ │ │ │ │ +93 new internal::UnaryExpression(std::bind(method, │ │ │ │ │ +94 std::placeholders::_1, std::placeholders::_2), │ │ │ │ │ +_9_5 expression)) { │ │ │ │ │ +96} │ │ │ │ │ +97 │ │ │ │ │ +99template │ │ │ │ │ +100template │ │ │ │ │ +_1_0_1_E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_E_x_p_r_e_s_s_i_o_n(const _E_x_p_r_e_s_s_i_o_n_<_A_1_>& expression1, │ │ │ │ │ +102 T (A1::*method)(const A2&, typename _M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n::_t_y_p_e, │ │ │ │ │ +103 typename _M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n::_t_y_p_e) const, │ │ │ │ │ +104 const _E_x_p_r_e_s_s_i_o_n& expression2) : │ │ │ │ │ +_1_0_5 root_( │ │ │ │ │ +106 new internal::BinaryExpression( │ │ │ │ │ +107 std::bind(method, std::placeholders::_1, │ │ │ │ │ +108 std::placeholders::_2, std::placeholders::_3, │ │ │ │ │ +109 std::placeholders::_4), │ │ │ │ │ +_1_1_0 expression1, expression2)) { │ │ │ │ │ +111} │ │ │ │ │ +112 │ │ │ │ │ +114template │ │ │ │ │ +_1_1_5template │ │ │ │ │ +_1_1_6_E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_E_x_p_r_e_s_s_i_o_n(const _E_x_p_r_e_s_s_i_o_n_<_A_1_>& expression1, │ │ │ │ │ +117 T (A1::*method)(const A2&, const A3&, │ │ │ │ │ +118 typename _M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n::_t_y_p_e, │ │ │ │ │ +119 typename _M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n::_t_y_p_e, │ │ │ │ │ +120 typename _M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n::_t_y_p_e) const, │ │ │ │ │ +_1_2_1 const _E_x_p_r_e_s_s_i_o_n& expression2, const _E_x_p_r_e_s_s_i_o_n& expression3) : │ │ │ │ │ +122 root_( │ │ │ │ │ +123 new internal::TernaryExpression( │ │ │ │ │ +124 std::bind(method, std::placeholders::_1, │ │ │ │ │ +125 std::placeholders::_2, std::placeholders::_3, │ │ │ │ │ +_1_2_6 std::placeholders::_4, std::placeholders::_5, │ │ │ │ │ +127 std::placeholders::_6), │ │ │ │ │ +128 expression1, expression2, expression3)) { │ │ │ │ │ +129} │ │ │ │ │ +130 │ │ │ │ │ +131template │ │ │ │ │ +_1_3_2std::set _E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_k_e_y_s() const { │ │ │ │ │ +_1_3_3 return root_->_k_e_y_s(); │ │ │ │ │ +134} │ │ │ │ │ +135 │ │ │ │ │ +136template │ │ │ │ │ +_1_3_7void _E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_d_i_m_s(std::map& map) const { │ │ │ │ │ +138 root_->_d_i_m_s(map); │ │ │ │ │ +139} │ │ │ │ │ +140 │ │ │ │ │ +141template │ │ │ │ │ +_1_4_2void _E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_p_r_i_n_t(const std::string& s) const { │ │ │ │ │ +143 root_->_p_r_i_n_t(s); │ │ │ │ │ +144} │ │ │ │ │ +_1_4_5 │ │ │ │ │ +146template │ │ │ │ │ +_1_4_7T _E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_v_a_l_u_e(const _V_a_l_u_e_s& values, │ │ │ │ │ +_1_4_8 boost::optional&> H) const { │ │ │ │ │ +149 │ │ │ │ │ +150 if (H) { │ │ │ │ │ +_1_5_1 // Call private version that returns derivatives in H │ │ │ │ │ +152 _K_e_y_V_e_c_t_o_r keys; │ │ │ │ │ +153 _F_a_s_t_V_e_c_t_o_r_<_i_n_t_> dims; │ │ │ │ │ +154 boost::tie(keys, dims) = keysAndDims(); │ │ │ │ │ +155 return valueAndDerivatives(values, keys, dims, *H); │ │ │ │ │ +156 } else │ │ │ │ │ +157 // no derivatives needed, just return value │ │ │ │ │ +_1_5_8 return root_->value(values); │ │ │ │ │ +159} │ │ │ │ │ 160 │ │ │ │ │ -161 // Remove the removed factors │ │ │ │ │ -162 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h removedFactors; │ │ │ │ │ -163 removedFactors._r_e_s_e_r_v_e(updateParams_._r_e_m_o_v_e_F_a_c_t_o_r_I_n_d_i_c_e_s.size()); │ │ │ │ │ -164 for (const auto index : updateParams_._r_e_m_o_v_e_F_a_c_t_o_r_I_n_d_i_c_e_s) { │ │ │ │ │ -165 removedFactors._p_u_s_h___b_a_c_k(nonlinearFactors->_a_t(index)); │ │ │ │ │ -166 nonlinearFactors->_r_e_m_o_v_e(index); │ │ │ │ │ -167 if (params_.cacheLinearizedFactors) linearFactors->_r_e_m_o_v_e(index); │ │ │ │ │ -168 } │ │ │ │ │ -169 │ │ │ │ │ -170 // Remove removed factors from the variable index so we do not attempt to │ │ │ │ │ -171 // relinearize them │ │ │ │ │ -172 variableIndex->_r_e_m_o_v_e(updateParams_._r_e_m_o_v_e_F_a_c_t_o_r_I_n_d_i_c_e_s.begin(), │ │ │ │ │ -173 updateParams_._r_e_m_o_v_e_F_a_c_t_o_r_I_n_d_i_c_e_s.end(), │ │ │ │ │ -174 removedFactors); │ │ │ │ │ -175 *keysWithRemovedFactors = removedFactors._k_e_y_s(); │ │ │ │ │ -176 } │ │ │ │ │ -177 │ │ │ │ │ -178 // Get keys from removed factors and new factors, and compute unused keys, │ │ │ │ │ -179 // i.e., keys that are empty now and do not appear in the new factors. │ │ │ │ │ -180 void computeUnusedKeys(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& newFactors, │ │ │ │ │ -181 const _V_a_r_i_a_b_l_e_I_n_d_e_x& variableIndex, │ │ │ │ │ -182 const _K_e_y_S_e_t& keysWithRemovedFactors, │ │ │ │ │ -183 _K_e_y_S_e_t* unusedKeys) const { │ │ │ │ │ -184 gttic(computeUnusedKeys); │ │ │ │ │ -185 _K_e_y_S_e_t removedAndEmpty; │ │ │ │ │ -186 for (_K_e_y key : keysWithRemovedFactors) { │ │ │ │ │ -187 if (variableIndex._e_m_p_t_y(key)) │ │ │ │ │ -188 removedAndEmpty.insert(removedAndEmpty.end(), key); │ │ │ │ │ -189 } │ │ │ │ │ -190 _K_e_y_S_e_t newFactorSymbKeys = newFactors._k_e_y_s(); │ │ │ │ │ -191 std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(), │ │ │ │ │ -192 newFactorSymbKeys.begin(), newFactorSymbKeys.end(), │ │ │ │ │ -193 std::inserter(*unusedKeys, unusedKeys->end())); │ │ │ │ │ -194 } │ │ │ │ │ -195 │ │ │ │ │ -196 // Calculate nonlinear error │ │ │ │ │ -197 void error(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& nonlinearFactors, │ │ │ │ │ -198 const _V_a_l_u_e_s& estimate, boost::optional* result) const { │ │ │ │ │ -199 gttic(error); │ │ │ │ │ -200 result->reset(nonlinearFactors._e_r_r_o_r(estimate)); │ │ │ │ │ -201 } │ │ │ │ │ -202 │ │ │ │ │ -203 // Mark linear update │ │ │ │ │ -204 void gatherInvolvedKeys(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& newFactors, │ │ │ │ │ -205 const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& nonlinearFactors, │ │ │ │ │ -206 const _K_e_y_S_e_t& keysWithRemovedFactors, │ │ │ │ │ -207 _K_e_y_S_e_t* markedKeys) const { │ │ │ │ │ -208 gttic(gatherInvolvedKeys); │ │ │ │ │ -209 *markedKeys = newFactors._k_e_y_s(); // Get keys from new factors │ │ │ │ │ -210 // Also mark keys involved in removed factors │ │ │ │ │ -211 markedKeys->insert(keysWithRemovedFactors.begin(), │ │ │ │ │ -212 keysWithRemovedFactors.end()); │ │ │ │ │ +161template │ │ │ │ │ +_1_6_2const boost::shared_ptr >& _E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_r_o_o_t() │ │ │ │ │ +const { │ │ │ │ │ +163 return root_; │ │ │ │ │ +164} │ │ │ │ │ +165 │ │ │ │ │ +166template │ │ │ │ │ +_1_6_7size_t _E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_t_r_a_c_e_S_i_z_e() const { │ │ │ │ │ +168 return root_->_t_r_a_c_e_S_i_z_e(); │ │ │ │ │ +169} │ │ │ │ │ +170 │ │ │ │ │ +_1_7_1// Private methods: │ │ │ │ │ +172 │ │ │ │ │ +173template │ │ │ │ │ +_1_7_4T _E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_v_a_l_u_e_A_n_d_D_e_r_i_v_a_t_i_v_e_s(const _V_a_l_u_e_s& values, │ │ │ │ │ +175 const _K_e_y_V_e_c_t_o_r& keys, const _F_a_s_t_V_e_c_t_o_r_<_i_n_t_>& dims, │ │ │ │ │ +176 std::vector& H) const { │ │ │ │ │ +_1_7_7 │ │ │ │ │ +178 // H should be pre-allocated │ │ │ │ │ +179 assert(H.size()==keys.size()); │ │ │ │ │ +180 │ │ │ │ │ +181 // Pre-allocate and zero VerticalBlockMatrix │ │ │ │ │ +182 static const int Dim = _t_r_a_i_t_s_<_T_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ +183 _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x Ab(dims, Dim); │ │ │ │ │ +184 Ab.matrix().setZero(); │ │ │ │ │ +185 internal::JacobianMap jacobianMap(keys, Ab); │ │ │ │ │ +186 │ │ │ │ │ +187 // Call unsafe version │ │ │ │ │ +188 T result = valueAndJacobianMap(values, jacobianMap); │ │ │ │ │ +_1_8_9 │ │ │ │ │ +190 // Copy blocks into the vector of jacobians passed in │ │ │ │ │ +191 for (_D_e_n_s_e_I_n_d_e_x i = 0; i < static_cast(keys.size()); i++) │ │ │ │ │ +192 H[i] = Ab(i); │ │ │ │ │ +_1_9_3 │ │ │ │ │ +194 return result; │ │ │ │ │ +195} │ │ │ │ │ +196 │ │ │ │ │ +_1_9_7template │ │ │ │ │ +_1_9_8T _E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_t_r_a_c_e_E_x_e_c_u_t_i_o_n(const _V_a_l_u_e_s& values, │ │ │ │ │ +199 _i_n_t_e_r_n_a_l_:_:_E_x_e_c_u_t_i_o_n_T_r_a_c_e_<_T_>& trace, void* traceStorage) const { │ │ │ │ │ +200 return root_->_t_r_a_c_e_E_x_e_c_u_t_i_o_n(values, trace, │ │ │ │ │ +201 static_cast(traceStorage)); │ │ │ │ │ +202} │ │ │ │ │ +203 │ │ │ │ │ +204template │ │ │ │ │ +_2_0_5T _E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_v_a_l_u_e_A_n_d_J_a_c_o_b_i_a_n_M_a_p(const _V_a_l_u_e_s& values, │ │ │ │ │ +206 internal::JacobianMap& jacobians) const { │ │ │ │ │ +207 // The following piece of code is absolutely crucial for performance. │ │ │ │ │ +208 // We allocate a block of memory on the stack, which can be done at runtime │ │ │ │ │ +209 // with modern C++ compilers. The traceExecution then fills this memory │ │ │ │ │ +210 // with an execution trace, made up entirely of "Record" structs, see │ │ │ │ │ +211 // the FunctionalNode class in expression-inl.h │ │ │ │ │ +212 size_t size = traceSize(); │ │ │ │ │ 213 │ │ │ │ │ -214 // Also mark any provided extra re-eliminate keys │ │ │ │ │ -215 if (updateParams_._e_x_t_r_a_R_e_e_l_i_m_K_e_y_s) { │ │ │ │ │ -216 for (_K_e_y key : *updateParams_._e_x_t_r_a_R_e_e_l_i_m_K_e_y_s) { │ │ │ │ │ -217 markedKeys->insert(key); │ │ │ │ │ -218 } │ │ │ │ │ -219 } │ │ │ │ │ -220 │ │ │ │ │ -221 // Also, keys that were not observed in existing factors, but whose │ │ │ │ │ -affected │ │ │ │ │ -222 // keys have been extended now (e.g. smart factors) │ │ │ │ │ -223 if (updateParams_._n_e_w_A_f_f_e_c_t_e_d_K_e_y_s) { │ │ │ │ │ -224 for (const auto& factorAddedKeys : *updateParams_._n_e_w_A_f_f_e_c_t_e_d_K_e_y_s) { │ │ │ │ │ -225 const auto factorIdx = factorAddedKeys.first; │ │ │ │ │ -226 const auto& affectedKeys = nonlinearFactors._a_t(factorIdx)->keys(); │ │ │ │ │ -227 markedKeys->insert(affectedKeys.begin(), affectedKeys.end()); │ │ │ │ │ -228 } │ │ │ │ │ -229 } │ │ │ │ │ -230 } │ │ │ │ │ -231 │ │ │ │ │ -232 // Update detail, unused, and observed keys from markedKeys │ │ │ │ │ -233 void updateKeys(const _K_e_y_S_e_t& markedKeys, _I_S_A_M_2_R_e_s_u_l_t* result) const { │ │ │ │ │ -234 gttic(updateKeys); │ │ │ │ │ -235 // Observed keys for detailed results │ │ │ │ │ -236 if (result->detail && params_.enableDetailedResults) { │ │ │ │ │ -237 for (_K_e_y key : markedKeys) { │ │ │ │ │ -238 result->detail->variableStatus[key].isObserved = true; │ │ │ │ │ -239 } │ │ │ │ │ -240 } │ │ │ │ │ -241 │ │ │ │ │ -242 for (_K_e_y index : markedKeys) { │ │ │ │ │ -243 // Only add if not unused │ │ │ │ │ -244 if (result->unusedKeys.find(index) == result->unusedKeys.end()) │ │ │ │ │ -245 // Make a copy of these, as we'll soon add to them │ │ │ │ │ -246 result->observedKeys.push_back(index); │ │ │ │ │ -247 } │ │ │ │ │ -248 } │ │ │ │ │ -249 │ │ │ │ │ -250 static void CheckRelinearizationRecursiveMap( │ │ │ │ │ -251 const _F_a_s_t_M_a_p_<_c_h_a_r_,_ _V_e_c_t_o_r_>& thresholds, const _V_e_c_t_o_r_V_a_l_u_e_s& delta, │ │ │ │ │ -252 const _I_S_A_M_2_:_:_s_h_a_r_e_d_C_l_i_q_u_e& clique, _K_e_y_S_e_t* relinKeys) { │ │ │ │ │ -253 // Check the current clique for relinearization │ │ │ │ │ -254 bool relinearize = false; │ │ │ │ │ -255 for (_K_e_y var : *clique->conditional()) { │ │ │ │ │ -256 // Find the threshold for this variable type │ │ │ │ │ -257 const Vector& threshold = thresholds.find(_S_y_m_b_o_l(var)._c_h_r())->second; │ │ │ │ │ -258 │ │ │ │ │ -259 const Vector& deltaVar = delta[var]; │ │ │ │ │ -260 │ │ │ │ │ -261 // Verify the threshold vector matches the actual variable size │ │ │ │ │ -262 if (threshold.rows() != deltaVar.rows()) │ │ │ │ │ -263 throw std::invalid_argument( │ │ │ │ │ -264 "Relinearization threshold vector dimensionality for '" + │ │ │ │ │ -265 std::string(1, _S_y_m_b_o_l(var).chr()) + │ │ │ │ │ -266 "' passed into iSAM2 parameters does not match actual variable " │ │ │ │ │ -267 "dimensionality."); │ │ │ │ │ +214 // Windows does not support variable length arrays, so memory must be │ │ │ │ │ +dynamically │ │ │ │ │ +215 // allocated on Visual Studio. For more information see the issue below │ │ │ │ │ +216 // https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual- │ │ │ │ │ +studio │ │ │ │ │ +217#ifdef _MSC_VER │ │ │ │ │ +218 std::unique_ptr traceStorageDeleter( │ │ │ │ │ +219 _aligned_malloc(size, internal::TraceAlignment), │ │ │ │ │ +220 [](void *ptr){ _aligned_free(ptr); }); │ │ │ │ │ +221 auto traceStorage = static_cast │ │ │ │ │ +(traceStorageDeleter.get()); │ │ │ │ │ +222#else │ │ │ │ │ +223 internal::ExecutionTraceStorage traceStorage[size]; │ │ │ │ │ +224#endif │ │ │ │ │ +225 │ │ │ │ │ +226 _i_n_t_e_r_n_a_l_:_:_E_x_e_c_u_t_i_o_n_T_r_a_c_e_<_T_> trace; │ │ │ │ │ +227 T value(this->traceExecution(values, trace, traceStorage)); │ │ │ │ │ +228 trace.startReverseAD1(jacobians); │ │ │ │ │ +229 │ │ │ │ │ +230 return value; │ │ │ │ │ +231} │ │ │ │ │ +232 │ │ │ │ │ +233template │ │ │ │ │ +234typename _E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_K_e_y_s_A_n_d_D_i_m_s _E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_k_e_y_s_A_n_d_D_i_m_s() const { │ │ │ │ │ +235 std::map map; │ │ │ │ │ +236 dims(map); │ │ │ │ │ +237 size_t n = map.size(); │ │ │ │ │ +238 KeysAndDims pair = std::make_pair(_K_e_y_V_e_c_t_o_r(n), _F_a_s_t_V_e_c_t_o_r_<_i_n_t_>(n)); │ │ │ │ │ +239 boost::copy(map | boost::adaptors::map_keys, pair.first.begin()); │ │ │ │ │ +240 boost::copy(map | boost::adaptors::map_values, pair.second.begin()); │ │ │ │ │ +241 return pair; │ │ │ │ │ +242} │ │ │ │ │ +243 │ │ │ │ │ +244namespace internal { │ │ │ │ │ +245// http://stackoverflow.com/questions/16260445/boost-bind-to-operator │ │ │ │ │ +246template │ │ │ │ │ +_2_4_7struct _a_p_p_l_y___c_o_m_p_o_s_e { │ │ │ │ │ +248 typedef T result_type; │ │ │ │ │ +249 static const int Dim = _t_r_a_i_t_s_<_T_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ +250 T operator()(const T& x, const T& y, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_D_i_m_,_ _D_i_m_> H1 = │ │ │ │ │ +251 boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_D_i_m_,_ _D_i_m_> H2 = boost::none) const { │ │ │ │ │ +252 return x.compose(y, H1, H2); │ │ │ │ │ +253 } │ │ │ │ │ +254}; │ │ │ │ │ +255 │ │ │ │ │ +256template <> │ │ │ │ │ +_2_5_7struct _a_p_p_l_y___c_o_m_p_o_s_e { │ │ │ │ │ +258 double operator()(const double& x, const double& y, │ │ │ │ │ +259 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _1_> H1 = boost::none, │ │ │ │ │ +260 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _1_> H2 = boost::none) const { │ │ │ │ │ +261 if (H1) H1->setConstant(y); │ │ │ │ │ +262 if (H2) H2->setConstant(x); │ │ │ │ │ +263 return x * y; │ │ │ │ │ +264 } │ │ │ │ │ +265}; │ │ │ │ │ +266 │ │ │ │ │ +267} │ │ │ │ │ 268 │ │ │ │ │ -269 // Check for relinearization │ │ │ │ │ -270 if ((deltaVar.array().abs() > threshold.array()).any()) { │ │ │ │ │ -271 relinKeys->insert(var); │ │ │ │ │ -272 relinearize = true; │ │ │ │ │ -273 } │ │ │ │ │ -274 } │ │ │ │ │ -275 │ │ │ │ │ -276 // If this node was relinearized, also check its children │ │ │ │ │ -277 if (relinearize) { │ │ │ │ │ -278 for (const _I_S_A_M_2_:_:_s_h_a_r_e_d_C_l_i_q_u_e& child : clique->children) { │ │ │ │ │ -279 CheckRelinearizationRecursiveMap(thresholds, delta, child, relinKeys); │ │ │ │ │ -280 } │ │ │ │ │ -281 } │ │ │ │ │ -282 } │ │ │ │ │ -283 │ │ │ │ │ -284 static void CheckRelinearizationRecursiveDouble( │ │ │ │ │ -285 double threshold, const _V_e_c_t_o_r_V_a_l_u_e_s& delta, │ │ │ │ │ -286 const _I_S_A_M_2_:_:_s_h_a_r_e_d_C_l_i_q_u_e& clique, _K_e_y_S_e_t* relinKeys) { │ │ │ │ │ -287 // Check the current clique for relinearization │ │ │ │ │ -288 bool relinearize = false; │ │ │ │ │ -289 for (_K_e_y var : *clique->conditional()) { │ │ │ │ │ -290 double maxDelta = delta[var].lpNorm(); │ │ │ │ │ -291 if (maxDelta >= threshold) { │ │ │ │ │ -292 relinKeys->insert(var); │ │ │ │ │ -293 relinearize = true; │ │ │ │ │ -294 } │ │ │ │ │ -295 } │ │ │ │ │ +269// Global methods: │ │ │ │ │ +270 │ │ │ │ │ +272template │ │ │ │ │ +_2_7_3_E_x_p_r_e_s_s_i_o_n_<_T_> _o_p_e_r_a_t_o_r_*(const _E_x_p_r_e_s_s_i_o_n_<_T_>& expression1, │ │ │ │ │ +274 const _E_x_p_r_e_s_s_i_o_n_<_T_>& expression2) { │ │ │ │ │ +275 return _E_x_p_r_e_s_s_i_o_n_<_T_>( │ │ │ │ │ +276 std::bind(_i_n_t_e_r_n_a_l_:_:_a_p_p_l_y___c_o_m_p_o_s_e_<_T_>(), std::placeholders::_1, │ │ │ │ │ +277 std::placeholders::_2, std::placeholders::_3, │ │ │ │ │ +278 std::placeholders::_4), │ │ │ │ │ +279 expression1, expression2); │ │ │ │ │ +280} │ │ │ │ │ +281 │ │ │ │ │ +283template │ │ │ │ │ +_2_8_4std::vector > _c_r_e_a_t_e_U_n_k_n_o_w_n_s(size_t n, char c, size_t start) { │ │ │ │ │ +285 std::vector > unknowns; │ │ │ │ │ +286 unknowns.reserve(n); │ │ │ │ │ +287 for (size_t i = start; i < start + n; i++) │ │ │ │ │ +288 unknowns.push_back(_E_x_p_r_e_s_s_i_o_n_<_T_>(c, i)); │ │ │ │ │ +289 return unknowns; │ │ │ │ │ +290} │ │ │ │ │ +291 │ │ │ │ │ +292template │ │ │ │ │ +293ScalarMultiplyExpression::ScalarMultiplyExpression(double s, const │ │ │ │ │ +Expression& e) │ │ │ │ │ +294 : Expression(boost::make_shared>(s, e)) │ │ │ │ │ +{} │ │ │ │ │ +295 │ │ │ │ │ 296 │ │ │ │ │ -297 // If this node was relinearized, also check its children │ │ │ │ │ -298 if (relinearize) { │ │ │ │ │ -299 for (const _I_S_A_M_2_:_:_s_h_a_r_e_d_C_l_i_q_u_e& child : clique->children) { │ │ │ │ │ -300 CheckRelinearizationRecursiveDouble(threshold, delta, child, relinKeys); │ │ │ │ │ -301 } │ │ │ │ │ -302 } │ │ │ │ │ -303 } │ │ │ │ │ -304 │ │ │ │ │ -_3_1_8 static _K_e_y_S_e_t _C_h_e_c_k_R_e_l_i_n_e_a_r_i_z_a_t_i_o_n_P_a_r_t_i_a_l( │ │ │ │ │ -319 const _I_S_A_M_2_:_:_R_o_o_t_s& roots, const _V_e_c_t_o_r_V_a_l_u_e_s& delta, │ │ │ │ │ -320 const _I_S_A_M_2_P_a_r_a_m_s_:_:_R_e_l_i_n_e_a_r_i_z_a_t_i_o_n_T_h_r_e_s_h_o_l_d& relinearizeThreshold) { │ │ │ │ │ -321 _K_e_y_S_e_t relinKeys; │ │ │ │ │ -322 for (const _I_S_A_M_2_:_:_s_h_a_r_e_d_C_l_i_q_u_e& root : roots) { │ │ │ │ │ -323 if (relinearizeThreshold.type() == typeid(double)) │ │ │ │ │ -324 CheckRelinearizationRecursiveDouble( │ │ │ │ │ -325 boost::get(relinearizeThreshold), delta, root, &relinKeys); │ │ │ │ │ -326 else if (relinearizeThreshold.type() == typeid(_F_a_s_t_M_a_p_<_c_h_a_r_,_ _V_e_c_t_o_r_>)) │ │ │ │ │ -327 CheckRelinearizationRecursiveMap( │ │ │ │ │ -328 boost::get<_F_a_s_t_M_a_p_<_c_h_a_r_,_ _V_e_c_t_o_r_> >(relinearizeThreshold), delta, │ │ │ │ │ -329 root, &relinKeys); │ │ │ │ │ -330 } │ │ │ │ │ -331 return relinKeys; │ │ │ │ │ -332 } │ │ │ │ │ -333 │ │ │ │ │ -_3_4_5 static _K_e_y_S_e_t _C_h_e_c_k_R_e_l_i_n_e_a_r_i_z_a_t_i_o_n_F_u_l_l( │ │ │ │ │ -346 const _V_e_c_t_o_r_V_a_l_u_e_s& delta, │ │ │ │ │ -347 const _I_S_A_M_2_P_a_r_a_m_s_:_:_R_e_l_i_n_e_a_r_i_z_a_t_i_o_n_T_h_r_e_s_h_o_l_d& relinearizeThreshold) { │ │ │ │ │ -348 _K_e_y_S_e_t relinKeys; │ │ │ │ │ -349 │ │ │ │ │ -350 if (const double* threshold = boost::get(&relinearizeThreshold)) { │ │ │ │ │ -351 for (const _V_e_c_t_o_r_V_a_l_u_e_s_:_:_K_e_y_V_a_l_u_e_P_a_i_r& key_delta : delta) { │ │ │ │ │ -352 double maxDelta = key_delta.second.lpNorm(); │ │ │ │ │ -353 if (maxDelta >= *threshold) relinKeys.insert(key_delta.first); │ │ │ │ │ -354 } │ │ │ │ │ -355 } else if (const _F_a_s_t_M_a_p_<_c_h_a_r_,_ _V_e_c_t_o_r_>* thresholds = │ │ │ │ │ -356 boost::get<_F_a_s_t_M_a_p_<_c_h_a_r_,_ _V_e_c_t_o_r_> >(&relinearizeThreshold)) { │ │ │ │ │ -357 for (const _V_e_c_t_o_r_V_a_l_u_e_s_:_:_K_e_y_V_a_l_u_e_P_a_i_r& key_delta : delta) { │ │ │ │ │ -358 const Vector& threshold = │ │ │ │ │ -359 thresholds->find(_S_y_m_b_o_l(key_delta.first)._c_h_r())->second; │ │ │ │ │ -360 if (threshold.rows() != key_delta.second.rows()) │ │ │ │ │ -361 throw std::invalid_argument( │ │ │ │ │ -362 "Relinearization threshold vector dimensionality for '" + │ │ │ │ │ -363 std::string(1, _S_y_m_b_o_l(key_delta.first)._c_h_r()) + │ │ │ │ │ -364 "' passed into iSAM2 parameters does not match actual variable " │ │ │ │ │ -365 "dimensionality."); │ │ │ │ │ -366 if ((key_delta.second.array().abs() > threshold.array()).any()) │ │ │ │ │ -367 relinKeys.insert(key_delta.first); │ │ │ │ │ -368 } │ │ │ │ │ -369 } │ │ │ │ │ -370 │ │ │ │ │ -371 return relinKeys; │ │ │ │ │ -372 } │ │ │ │ │ -373 │ │ │ │ │ -374 // Mark keys in \Delta above threshold \beta: │ │ │ │ │ -375 _K_e_y_S_e_t gatherRelinearizeKeys(const _I_S_A_M_2_:_:_R_o_o_t_s& roots, │ │ │ │ │ -376 const _V_e_c_t_o_r_V_a_l_u_e_s& delta, │ │ │ │ │ -377 const _K_e_y_S_e_t& fixedVariables, │ │ │ │ │ -378 _K_e_y_S_e_t* markedKeys) const { │ │ │ │ │ -379 gttic(gatherRelinearizeKeys); │ │ │ │ │ -380 // J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. │ │ │ │ │ -381 _K_e_y_S_e_t relinKeys = │ │ │ │ │ -382 params_.enablePartialRelinearizationCheck │ │ │ │ │ -383 ? CheckRelinearizationPartial(roots, delta, │ │ │ │ │ -384 params_.relinearizeThreshold) │ │ │ │ │ -385 : CheckRelinearizationFull(delta, params_.relinearizeThreshold); │ │ │ │ │ -386 if (updateParams_._f_o_r_c_e_F_u_l_l_S_o_l_v_e) │ │ │ │ │ -387 relinKeys = CheckRelinearizationFull(delta, 0.0); // for debugging │ │ │ │ │ -388 │ │ │ │ │ -389 // Remove from relinKeys any keys whose linearization points are fixed │ │ │ │ │ -390 for (_K_e_y key : fixedVariables) { │ │ │ │ │ -391 relinKeys.erase(key); │ │ │ │ │ -392 } │ │ │ │ │ -393 if (updateParams_._n_o_R_e_l_i_n_K_e_y_s) { │ │ │ │ │ -394 for (Key key : *updateParams_.noRelinKeys) { │ │ │ │ │ -395 relinKeys.erase(key); │ │ │ │ │ -396 } │ │ │ │ │ -397 } │ │ │ │ │ -398 │ │ │ │ │ -399 // Add the variables being relinearized to the marked keys │ │ │ │ │ -400 markedKeys->insert(relinKeys.begin(), relinKeys.end()); │ │ │ │ │ -401 return relinKeys; │ │ │ │ │ -402 } │ │ │ │ │ -403 │ │ │ │ │ -404 // Record relinerization threshold keys in detailed results │ │ │ │ │ -405 void recordRelinearizeDetail(const KeySet& relinKeys, │ │ │ │ │ -406 ISAM2Result::DetailedResults* detail) const { │ │ │ │ │ -407 if (detail && params_.enableDetailedResults) { │ │ │ │ │ -408 for (Key key : relinKeys) { │ │ │ │ │ -409 detail->variableStatus[key].isAboveRelinThreshold = true; │ │ │ │ │ -410 detail->variableStatus[key].isRelinearized = true; │ │ │ │ │ -411 } │ │ │ │ │ -412 } │ │ │ │ │ -413 } │ │ │ │ │ -414 │ │ │ │ │ -415 // Mark all cliques that involve marked variables \Theta_{J} and all │ │ │ │ │ -416 // their ancestors. │ │ │ │ │ -417 void findFluid(const ISAM2::Roots& roots, const KeySet& relinKeys, │ │ │ │ │ -418 KeySet* markedKeys, │ │ │ │ │ -419 ISAM2Result::DetailedResults* detail) const { │ │ │ │ │ -420 gttic(findFluid); │ │ │ │ │ -421 for (const auto& root : roots) │ │ │ │ │ -422 // add other cliques that have the marked ones in the separator │ │ │ │ │ -423 root->findAll(relinKeys, markedKeys); │ │ │ │ │ -424 │ │ │ │ │ -425 // Relinearization-involved keys for detailed results │ │ │ │ │ -426 if (detail && params_.enableDetailedResults) { │ │ │ │ │ -427 KeySet involvedRelinKeys; │ │ │ │ │ -428 for (const auto& root : roots) │ │ │ │ │ -429 root->findAll(relinKeys, &involvedRelinKeys); │ │ │ │ │ -430 for (Key key : involvedRelinKeys) { │ │ │ │ │ -431 if (!detail->variableStatus[key].isAboveRelinThreshold) { │ │ │ │ │ -432 detail->variableStatus[key].isRelinearizeInvolved = true; │ │ │ │ │ -433 detail->variableStatus[key].isRelinearized = true; │ │ │ │ │ -434 } │ │ │ │ │ -435 } │ │ │ │ │ -436 } │ │ │ │ │ -437 } │ │ │ │ │ -438 │ │ │ │ │ -439 // Linearize new factors │ │ │ │ │ -440 void linearizeNewFactors(const NonlinearFactorGraph& newFactors, │ │ │ │ │ -441 const _V_a_l_u_e_s& theta, size_t numNonlinearFactors, │ │ │ │ │ -442 const FactorIndices& newFactorsIndices, │ │ │ │ │ -443 GaussianFactorGraph* linearFactors) const { │ │ │ │ │ -444 gttic(linearizeNewFactors); │ │ │ │ │ -445 auto linearized = newFactors.linearize(theta); │ │ │ │ │ -446 if (params_.findUnusedFactorSlots) { │ │ │ │ │ -447 linearFactors->resize(numNonlinearFactors); │ │ │ │ │ -448 for (size_t i = 0; i < newFactors.size(); ++i) │ │ │ │ │ -449 (*linearFactors)[newFactorsIndices[i]] = (*linearized)[i]; │ │ │ │ │ -450 } else { │ │ │ │ │ -451 linearFactors->push_back(*linearized); │ │ │ │ │ -452 } │ │ │ │ │ -453 assert(linearFactors->size() == numNonlinearFactors); │ │ │ │ │ -454 } │ │ │ │ │ -455 │ │ │ │ │ -456 void augmentVariableIndex(const NonlinearFactorGraph& newFactors, │ │ │ │ │ -457 const FactorIndices& newFactorsIndices, │ │ │ │ │ -458 VariableIndex* variableIndex) const { │ │ │ │ │ -459 gttic(augmentVariableIndex); │ │ │ │ │ -460 // Augment the variable index with the new factors │ │ │ │ │ -461 if (params_.findUnusedFactorSlots) │ │ │ │ │ -462 variableIndex->augment(newFactors, newFactorsIndices); │ │ │ │ │ -463 else │ │ │ │ │ -464 variableIndex->augment(newFactors); │ │ │ │ │ -465 │ │ │ │ │ -466 // Augment it with existing factors which now affect to more variables: │ │ │ │ │ -467 if (updateParams_._n_e_w_A_f_f_e_c_t_e_d_K_e_y_s) { │ │ │ │ │ -468 for (const auto& factorAddedKeys : *updateParams_.newAffectedKeys) { │ │ │ │ │ -469 const auto factorIdx = factorAddedKeys.first; │ │ │ │ │ -470 variableIndex->augmentExistingFactor(factorIdx, factorAddedKeys.second); │ │ │ │ │ -471 } │ │ │ │ │ -472 } │ │ │ │ │ -473 } │ │ │ │ │ -474 │ │ │ │ │ -475 static void LogRecalculateKeys(const ISAM2Result& result) { │ │ │ │ │ -476 const bool debug = ISDEBUG("ISAM2 recalculate"); │ │ │ │ │ -477 │ │ │ │ │ -478 if (debug) { │ │ │ │ │ -479 std::cout << "markedKeys: "; │ │ │ │ │ -480 for (const Key key : result.markedKeys) { │ │ │ │ │ -481 std::cout << key << " "; │ │ │ │ │ -482 } │ │ │ │ │ -483 std::cout << std::endl; │ │ │ │ │ -484 std::cout << "observedKeys: "; │ │ │ │ │ -485 for (const Key key : result.observedKeys) { │ │ │ │ │ -486 std::cout << key << " "; │ │ │ │ │ -487 } │ │ │ │ │ -488 std::cout << std::endl; │ │ │ │ │ -489 } │ │ │ │ │ -490 } │ │ │ │ │ -491 │ │ │ │ │ -492 static FactorIndexSet GetAffectedFactors(const KeyList& keys, │ │ │ │ │ -493 const VariableIndex& variableIndex) { │ │ │ │ │ -494 gttic(GetAffectedFactors); │ │ │ │ │ -495 FactorIndexSet indices; │ │ │ │ │ -496 for (const Key key : keys) { │ │ │ │ │ -497 const _F_a_c_t_o_r_I_n_d_i_c_e_s& factors(variableIndex[key]); │ │ │ │ │ -498 indices.insert(factors.begin(), factors.end()); │ │ │ │ │ -499 } │ │ │ │ │ -500 return indices; │ │ │ │ │ -501 } │ │ │ │ │ -502 │ │ │ │ │ -503 // find intermediate (linearized) factors from cache that are passed into │ │ │ │ │ -504 // the affected area │ │ │ │ │ -505 static GaussianFactorGraph GetCachedBoundaryFactors( │ │ │ │ │ -506 const ISAM2::Cliques& orphans) { │ │ │ │ │ -507 GaussianFactorGraph cachedBoundary; │ │ │ │ │ -508 │ │ │ │ │ -509 for (const auto& orphan : orphans) { │ │ │ │ │ -510 // retrieve the cached factor and add to boundary │ │ │ │ │ -511 cachedBoundary.push_back(orphan->cachedFactor()); │ │ │ │ │ -512 } │ │ │ │ │ -513 │ │ │ │ │ -514 return cachedBoundary; │ │ │ │ │ -515 } │ │ │ │ │ -516}; │ │ │ │ │ -517 │ │ │ │ │ -518} // namespace gtsam │ │ │ │ │ -_d_e_b_u_g_._h │ │ │ │ │ -Global debugging flags. │ │ │ │ │ -_J_u_n_c_t_i_o_n_T_r_e_e_-_i_n_s_t_._h │ │ │ │ │ -The junction tree, template bodies. │ │ │ │ │ -_V_a_r_i_a_b_l_e_I_n_d_e_x_._h │ │ │ │ │ -_G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h │ │ │ │ │ -_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ -Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ │ -_I_S_A_M_2_R_e_s_u_l_t_._h │ │ │ │ │ -Class that stores detailed iSAM2 result. │ │ │ │ │ -_I_S_A_M_2_._h │ │ │ │ │ -Incremental update functionality (ISAM2) for BayesTree, with fluid │ │ │ │ │ -relinearization. │ │ │ │ │ +297template │ │ │ │ │ +298BinarySumExpression::BinarySumExpression(const Expression& e1, const │ │ │ │ │ +Expression& e2) │ │ │ │ │ +299 : Expression(boost::_m_a_k_e___s_h_a_r_e_d>(e1, e2)) {} │ │ │ │ │ +300 │ │ │ │ │ +301template │ │ │ │ │ +_3_0_2_E_x_p_r_e_s_s_i_o_n_<_T_>& _E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_o_p_e_r_a_t_o_r_+_=(const _E_x_p_r_e_s_s_i_o_n_<_T_>& e) { │ │ │ │ │ +303 root_ = boost::make_shared>(*this, e); │ │ │ │ │ +304 return *this; │ │ │ │ │ +305} │ │ │ │ │ +306 │ │ │ │ │ +307} // namespace gtsam │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ +std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ +FastVector │ │ │ │ │ +FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ +DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_I_n_d_i_c_e_s │ │ │ │ │ -FastVector< FactorIndex > FactorIndices │ │ │ │ │ -Define collection types: │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_n_s_e_I_n_d_e_x │ │ │ │ │ +ptrdiff_t DenseIndex │ │ │ │ │ +The index type for Eigen objects. │ │ │ │ │ +DDeeffiinniittiioonn types.h:106 │ │ │ │ │ +_g_t_s_a_m_:_:_s_y_m_b_o_l │ │ │ │ │ +Key symbol(unsigned char c, std::uint64_t j) │ │ │ │ │ +Create a symbol key from a character and index, i.e. │ │ │ │ │ +DDeeffiinniittiioonn Symbol.h:135 │ │ │ │ │ +_g_t_s_a_m_:_:_c_r_e_a_t_e_U_n_k_n_o_w_n_s │ │ │ │ │ +std::vector< Expression< T > > createUnknowns(size_t n, char c, size_t start) │ │ │ │ │ +Construct an array of leaves. │ │ │ │ │ +DDeeffiinniittiioonn Expression-inl.h:284 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ +Point2 operator*(double s, const Point2 &p) │ │ │ │ │ +multiply with scalar │ │ │ │ │ +DDeeffiinniittiioonn Point2.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_m_a_k_e___s_h_a_r_e_d │ │ │ │ │ +gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost:: │ │ │ │ │ +shared_ptr< T > > make_shared(Args &&... args) │ │ │ │ │ +Add our own make_shared as a layer of wrapping on boost::make_shared This │ │ │ │ │ +solves the problem with the... │ │ │ │ │ +DDeeffiinniittiioonn make_shared.h:57 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ -FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ -fast_pool_allocator instead of the defa... │ │ │ │ │ -DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_k_e_y_s │ │ │ │ │ -KeySet keys() const │ │ │ │ │ -Potentially slow function to return all keys involved, sorted, as a set. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph-inst.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d___f_a_c_t_o_r_s │ │ │ │ │ -FactorIndices add_factors(const CONTAINER &factors, bool useEmptySlots=false) │ │ │ │ │ -Add new factors to a factor graph and returns a list of new factor indices, │ │ │ │ │ -optionally finding and re... │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph-inst.h:109 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_p_u_s_h___b_a_c_k │ │ │ │ │ -IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor) │ │ │ │ │ -Add a factor directly using a shared_ptr. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:186 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_r_e_m_o_v_e │ │ │ │ │ -void remove(size_t i) │ │ │ │ │ -delete factor without re-arranging indexes by inserting a nullptr pointer │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:385 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_a_t │ │ │ │ │ -const sharedFactor at(size_t i) const │ │ │ │ │ -Get a specific factor by index (this checks array bounds and may throw an │ │ │ │ │ -exception,... │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:335 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_r_e_s_e_r_v_e │ │ │ │ │ -void reserve(size_t size) │ │ │ │ │ -Reserve space for the specified number of factors if you know in advance how │ │ │ │ │ -many there will be (work... │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:182 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_<_ _I_S_A_M_2_C_l_i_q_u_e_ _> │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn BayesTree-inst.h:212 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_<_ _I_S_A_M_2_C_l_i_q_u_e_ _>_:_:_R_o_o_t_s │ │ │ │ │ -FastVector< sharedClique > Roots │ │ │ │ │ -Root cliques. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:95 │ │ │ │ │ -_g_t_s_a_m_:_:_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ -A JunctionTree is a cluster tree, a set of variable clusters with factors, │ │ │ │ │ -arranged in a tree,... │ │ │ │ │ -DDeeffiinniittiioonn JunctionTree.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ +either a fixed size o... │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +: meta-function to generate JacobianTA optional reference Used mainly by │ │ │ │ │ +Expressions │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:261 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ +of vertical blocks. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:43 │ │ │ │ │ _g_t_s_a_m_:_:_S_y_m_b_o_l │ │ │ │ │ Character and index key used to refer to variables. │ │ │ │ │ DDeeffiinniittiioonn Symbol.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_:_:_c_h_r │ │ │ │ │ -unsigned char chr() const │ │ │ │ │ -Retrieve key character. │ │ │ │ │ -DDeeffiinniittiioonn Symbol.h:73 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ -The VariableIndex class computes and stores the block column structure of a │ │ │ │ │ -factor graph. │ │ │ │ │ -DDeeffiinniittiioonn VariableIndex.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_r_e_m_o_v_e │ │ │ │ │ -void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG &factors) │ │ │ │ │ -Remove entries corresponding to the specified factors. │ │ │ │ │ -DDeeffiinniittiioonn VariableIndex-inl.h:54 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_e_m_p_t_y │ │ │ │ │ -bool empty(Key variable) const │ │ │ │ │ -Return true if no factors associated with a variable. │ │ │ │ │ -DDeeffiinniittiioonn VariableIndex.h:98 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ -DDeeffiinniittiioonn GaussianEliminationTree.h:29 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ -with a unique integer... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_B_a_y_e_s_T_r_e_e │ │ │ │ │ -DDeeffiinniittiioonn ISAM2-impl.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ -DDeeffiinniittiioonn ISAM2-impl.h:61 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_l_t_a_I_m_p_l │ │ │ │ │ -DDeeffiinniittiioonn ISAM2-impl.h:72 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_l_t_a_I_m_p_l_:_:_P_a_r_t_i_a_l_S_o_l_v_e_R_e_s_u_l_t │ │ │ │ │ -DDeeffiinniittiioonn ISAM2-impl.h:73 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_l_t_a_I_m_p_l_:_:_R_e_o_r_d_e_r_i_n_g_M_o_d_e │ │ │ │ │ -DDeeffiinniittiioonn ISAM2-impl.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_U_p_d_a_t_e_I_m_p_l │ │ │ │ │ -Implementation functions for update method All of the methods below have clear │ │ │ │ │ -inputs and outputs,... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2-impl.h:114 │ │ │ │ │ -_g_t_s_a_m_:_:_U_p_d_a_t_e_I_m_p_l_:_:_C_h_e_c_k_R_e_l_i_n_e_a_r_i_z_a_t_i_o_n_F_u_l_l │ │ │ │ │ -static KeySet CheckRelinearizationFull(const VectorValues &delta, const │ │ │ │ │ -ISAM2Params::RelinearizationThreshold &relinearizeThreshold) │ │ │ │ │ -Find the set of variables to be relinearized according to relinearizeThreshold. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2-impl.h:345 │ │ │ │ │ -_g_t_s_a_m_:_:_U_p_d_a_t_e_I_m_p_l_:_:_C_h_e_c_k_R_e_l_i_n_e_a_r_i_z_a_t_i_o_n_P_a_r_t_i_a_l │ │ │ │ │ -static KeySet CheckRelinearizationPartial(const ISAM2::Roots &roots, const │ │ │ │ │ -VectorValues &delta, const ISAM2Params::RelinearizationThreshold │ │ │ │ │ -&relinearizeThreshold) │ │ │ │ │ -Find the set of variables to be relinearized according to relinearizeThreshold. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2-impl.h:318 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2 │ │ │ │ │ -Implementation of the full ISAM2 algorithm for incremental nonlinear │ │ │ │ │ -optimization. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_:_:_s_h_a_r_e_d_C_l_i_q_u_e │ │ │ │ │ -Base::sharedClique sharedClique │ │ │ │ │ -Shared pointer to a clique. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2.h:103 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:135 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_R_e_l_i_n_e_a_r_i_z_a_t_i_o_n_T_h_r_e_s_h_o_l_d │ │ │ │ │ -boost::variant< double, FastMap< char, Vector > > RelinearizationThreshold │ │ │ │ │ -Either a constant relinearization threshold or a per-variable-type set of │ │ │ │ │ -thresholds. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:140 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t │ │ │ │ │ -This struct is returned from ISAM2::update() and contains information about the │ │ │ │ │ -update that is useful... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Result.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s │ │ │ │ │ -This struct is used by ISAM2::update() to pass additional parameters to give │ │ │ │ │ -the user a fine-grained ... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2UpdateParams.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_:_:_f_o_r_c_e___r_e_l_i_n_e_a_r_i_z_e │ │ │ │ │ -bool force_relinearize │ │ │ │ │ -Relinearize any variables whose delta magnitude is sufficiently large (Params:: │ │ │ │ │ -relinearizeThreshold),... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2UpdateParams.h:54 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_:_:_r_e_m_o_v_e_F_a_c_t_o_r_I_n_d_i_c_e_s │ │ │ │ │ -FactorIndices removeFactorIndices │ │ │ │ │ -Indices of factors to remove from system (default: empty) │ │ │ │ │ -DDeeffiinniittiioonn ISAM2UpdateParams.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_:_:_f_o_r_c_e_F_u_l_l_S_o_l_v_e │ │ │ │ │ -bool forceFullSolve │ │ │ │ │ -By default, iSAM2 uses a wildfire update scheme that stops updating when the │ │ │ │ │ -deltas become too small ... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2UpdateParams.h:71 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_:_:_e_x_t_r_a_R_e_e_l_i_m_K_e_y_s │ │ │ │ │ -boost::optional< FastList< Key > > extraReelimKeys │ │ │ │ │ -An optional set of nonlinear keys that iSAM2 will re-eliminate, regardless of │ │ │ │ │ -the size of the linear ... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2UpdateParams.h:49 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_:_:_n_o_R_e_l_i_n_K_e_y_s │ │ │ │ │ -boost::optional< FastList< Key > > noRelinKeys │ │ │ │ │ -An optional set of nonlinear keys that iSAM2 will hold at a constant │ │ │ │ │ -linearization point,... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2UpdateParams.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_:_:_n_e_w_A_f_f_e_c_t_e_d_K_e_y_s │ │ │ │ │ -boost::optional< FastMap< FactorIndex, KeySet > > newAffectedKeys │ │ │ │ │ -An optional set of new Keys that are now affected by factors, indexed by factor │ │ │ │ │ -indices (as returned ... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2UpdateParams.h:66 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &str="NonlinearFactorGraph: ", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactorGraph.cpp:55 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_e_r_r_o_r │ │ │ │ │ -double error(const Values &values) const │ │ │ │ │ -unnormalized error, in the most common case │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactorGraph.cpp:170 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_a_p_p_l_y___c_o_m_p_o_s_e │ │ │ │ │ +DDeeffiinniittiioonn Expression-inl.h:247 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_E_x_e_c_u_t_i_o_n_T_r_a_c_e │ │ │ │ │ +DDeeffiinniittiioonn Expression.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n │ │ │ │ │ +Expression class that supports automatic differentiation. │ │ │ │ │ +DDeeffiinniittiioonn Expression.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_E_x_p_r_e_s_s_i_o_n │ │ │ │ │ +Expression() │ │ │ │ │ +Default constructor, for serialization. │ │ │ │ │ +DDeeffiinniittiioonn Expression.h:182 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_k_e_y_s │ │ │ │ │ +std::set< Key > keys() const │ │ │ │ │ +Return keys that play in this expression. │ │ │ │ │ +DDeeffiinniittiioonn Expression-inl.h:132 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_K_e_y_s_A_n_d_D_i_m_s │ │ │ │ │ +std::pair< KeyVector, FastVector< int > > KeysAndDims │ │ │ │ │ +Keys and dimensions in same order. │ │ │ │ │ +DDeeffiinniittiioonn Expression.h:185 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_d_i_m_s │ │ │ │ │ +void dims(std::map< Key, int > &map) const │ │ │ │ │ +Return dimensions for each argument, as a map. │ │ │ │ │ +DDeeffiinniittiioonn Expression-inl.h:137 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s) const │ │ │ │ │ +Print. │ │ │ │ │ +DDeeffiinniittiioonn Expression-inl.h:142 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_t_r_a_c_e_S_i_z_e │ │ │ │ │ +size_t traceSize() const │ │ │ │ │ +Return size needed for memory buffer in traceExecution. │ │ │ │ │ +DDeeffiinniittiioonn Expression-inl.h:167 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_t_r_a_c_e_E_x_e_c_u_t_i_o_n │ │ │ │ │ +T traceExecution(const Values &values, internal::ExecutionTrace< T > &trace, │ │ │ │ │ +void *traceStorage) const │ │ │ │ │ +trace execution, very unsafe │ │ │ │ │ +DDeeffiinniittiioonn Expression-inl.h:198 │ │ │ │ │ _g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_K_e_y_V_a_l_u_e_P_a_i_r │ │ │ │ │ -A key-value pair, which you get by dereferencing iterators. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:93 │ │ │ │ │ -_V_a_l_u_e_s │ │ │ │ │ -In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ -a non-linear function... │ │ │ │ │ -_S_y_m_b_o_l_._h │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _I_S_A_M_2_-_i_m_p_l_._h │ │ │ │ │ + * _E_x_p_r_e_s_s_i_o_n_-_i_n_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01142.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/GaussNewtonOptimizer.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,47 +94,32 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
ISAM2.h File Reference
│ │ │ │ +
GaussNewtonOptimizer.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -

Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. │ │ │ │ -More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::ISAM2
 Implementation of the full ISAM2 algorithm for incremental nonlinear optimization. More...
 
struct  gtsam::traits< ISAM2 >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.

│ │ │ │ -
Author
Michael Kaess, Richard Roberts, Frank Dellaert
│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Feb 26, 2012
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,31 +1,20 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -ISAM2.h File Reference │ │ │ │ │ -Incremental update functionality (ISAM2) for BayesTree, with fluid │ │ │ │ │ -relinearization. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_I_S_A_M_2 │ │ │ │ │ -  Implementation of the full _I_S_A_M_2 algorithm for incremental nonlinear │ │ │ │ │ - optimization. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _I_S_A_M_2_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +GaussNewtonOptimizer.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Incremental update functionality (ISAM2) for BayesTree, with fluid │ │ │ │ │ -relinearization. │ │ │ │ │ Author │ │ │ │ │ - Michael Kaess, Richard Roberts, Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Feb 26, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _I_S_A_M_2_._h │ │ │ │ │ + * _G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01145.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearISAM.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearFactorGraph.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,32 +94,50 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
NonlinearISAM.cpp File Reference
│ │ │ │ +
NonlinearFactorGraph.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ + │ │ │ │ +

Factor Graph consisting of non-linear factors. │ │ │ │ +More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::NonlinearFactorGraph
 
struct  gtsam::traits< NonlinearFactorGraph >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Jan 19, 2010
│ │ │ │ -
Author
Viorela Ila and Richard Roberts
│ │ │ │ +

Factor Graph consisting of non-linear factors.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ +Carlos Nieto
│ │ │ │ +
│ │ │ │ +Christian Potthast
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,29 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -NonlinearISAM.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +NonlinearFactorGraph.h File Reference │ │ │ │ │ +Factor Graph consisting of non-linear factors. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Jan 19, 2010 │ │ │ │ │ +Factor Graph consisting of non-linear factors. │ │ │ │ │ Author │ │ │ │ │ - Viorela Ila and Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Carlos Nieto │ │ │ │ │ + Christian Potthast │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _N_o_n_l_i_n_e_a_r_I_S_A_M_._c_p_p │ │ │ │ │ + * _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01148.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ExpressionFactorGraph.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,43 +96,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
ExpressionFactorGraph.h File Reference
│ │ │ │ +
ISAM2.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Factor graph that supports adding ExpressionFactors directly. │ │ │ │ +

Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::ExpressionFactorGraph
 Factor graph that supports adding ExpressionFactors directly. More...
class  gtsam::ISAM2
 Implementation of the full ISAM2 algorithm for incremental nonlinear optimization. More...
 
struct  gtsam::traits< ISAM2 >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Factor graph that supports adding ExpressionFactors directly.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
Date
December 2014
│ │ │ │ +

Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.

│ │ │ │ +
Author
Michael Kaess, Richard Roberts, Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,27 +1,31 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -ExpressionFactorGraph.h File Reference │ │ │ │ │ -Factor graph that supports adding ExpressionFactors directly. _M_o_r_e_._._. │ │ │ │ │ +ISAM2.h File Reference │ │ │ │ │ +Incremental update functionality (ISAM2) for BayesTree, with fluid │ │ │ │ │ +relinearization. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -  _F_a_c_t_o_r graph that supports adding ExpressionFactors directly. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_I_S_A_M_2 │ │ │ │ │ +  Implementation of the full _I_S_A_M_2 algorithm for incremental nonlinear │ │ │ │ │ + optimization. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _I_S_A_M_2_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Factor graph that supports adding ExpressionFactors directly. │ │ │ │ │ +Incremental update functionality (ISAM2) for BayesTree, with fluid │ │ │ │ │ +relinearization. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - December 2014 │ │ │ │ │ + Michael Kaess, Richard Roberts, Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ + * _I_S_A_M_2_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01148.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a01148 = [ │ │ │ │ │ - ["gtsam::ExpressionFactorGraph", "a04328.html", "a04328"] │ │ │ │ │ + ["gtsam::traits< ISAM2 >", "a04400.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01148_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ExpressionFactorGraph.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,67 +98,254 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ExpressionFactorGraph.h
│ │ │ │ +
ISAM2.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ +
19// \callgraph
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/nonlinear/ExpressionFactor.h>
│ │ │ │ -
22#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ -
23
│ │ │ │ -
24namespace gtsam {
│ │ │ │ -
25
│ │ │ │ -
│ │ │ │ -
29class ExpressionFactorGraph: public NonlinearFactorGraph {
│ │ │ │ -
30
│ │ │ │ -
31public:
│ │ │ │ -
32
│ │ │ │ -
35
│ │ │ │ -
42 template<typename T>
│ │ │ │ -
│ │ │ │ -
43 void addExpressionFactor(const Expression<T>& h, const T& z,
│ │ │ │ -
44 const SharedNoiseModel& R) {
│ │ │ │ -
45 using F = ExpressionFactor<T>;
│ │ │ │ -
46 push_back(boost::allocate_shared<F>(Eigen::aligned_allocator<F>(), R, z, h));
│ │ │ │ -
47 }
│ │ │ │ -
│ │ │ │ -
48
│ │ │ │ -
50};
│ │ │ │ -
│ │ │ │ -
51
│ │ │ │ -
52}
│ │ │ │ -
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │ +
21#pragma once
│ │ │ │ +
22
│ │ │ │ +
23#include <gtsam/linear/GaussianBayesTree.h>
│ │ │ │ +
24#include <gtsam/nonlinear/ISAM2Clique.h>
│ │ │ │ +
25#include <gtsam/nonlinear/ISAM2Params.h>
│ │ │ │ +
26#include <gtsam/nonlinear/ISAM2Result.h>
│ │ │ │ +
27#include <gtsam/nonlinear/ISAM2UpdateParams.h>
│ │ │ │ +
28#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ +
29
│ │ │ │ +
30#include <vector>
│ │ │ │ +
31
│ │ │ │ +
32namespace gtsam {
│ │ │ │ +
33
│ │ │ │ +
│ │ │ │ +
45class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
│ │ │ │ +
46 protected:
│ │ │ │ +
48 Values theta_;
│ │ │ │ +
49
│ │ │ │ +
52 VariableIndex variableIndex_;
│ │ │ │ +
53
│ │ │ │ +
61 mutable VectorValues delta_;
│ │ │ │ +
62
│ │ │ │ +
63 mutable VectorValues deltaNewton_; // Only used when using Dogleg - stores
│ │ │ │ +
64 // the Gauss-Newton update
│ │ │ │ +
65 mutable VectorValues RgProd_; // Only used when using Dogleg - stores R*g and
│ │ │ │ +
66 // is updated incrementally
│ │ │ │ +
67
│ │ │ │ +
76 mutable KeySet deltaReplacedMask_; // TODO(dellaert): Make sure accessed in
│ │ │ │ +
77 // the right way
│ │ │ │ +
78
│ │ │ │ +
81 NonlinearFactorGraph nonlinearFactors_;
│ │ │ │ +
82
│ │ │ │ +
84 mutable GaussianFactorGraph linearFactors_;
│ │ │ │ +
85
│ │ │ │ +
87 ISAM2Params params_;
│ │ │ │ +
88
│ │ │ │ +
90 mutable boost::optional<double> doglegDelta_;
│ │ │ │ +
91
│ │ │ │ +
94 KeySet fixedVariables_;
│ │ │ │ +
95
│ │ │ │ +
96 int update_count_;
│ │ │ │ +
98
│ │ │ │ +
99 public:
│ │ │ │ +
100 using This = ISAM2;
│ │ │ │ +
101 using Base = BayesTree<ISAM2Clique>;
│ │ │ │ +
102 using Clique = Base::Clique;
│ │ │ │ +
103 using sharedClique = Base::sharedClique;
│ │ │ │ +
104 using Cliques = Base::Cliques;
│ │ │ │ +
105
│ │ │ │ +
107 explicit ISAM2(const ISAM2Params& params);
│ │ │ │ +
108
│ │ │ │ +
111 ISAM2();
│ │ │ │ +
112
│ │ │ │ +
114 virtual ~ISAM2() {}
│ │ │ │ +
115
│ │ │ │ +
117 virtual bool equals(const ISAM2& other, double tol = 1e-9) const;
│ │ │ │ +
118
│ │ │ │ +
151 virtual ISAM2Result update(
│ │ │ │ +
152 const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
│ │ │ │ +
153 const Values& newTheta = Values(),
│ │ │ │ +
154 const FactorIndices& removeFactorIndices = FactorIndices(),
│ │ │ │ +
155 const boost::optional<FastMap<Key, int> >& constrainedKeys = boost::none,
│ │ │ │ +
156 const boost::optional<FastList<Key> >& noRelinKeys = boost::none,
│ │ │ │ +
157 const boost::optional<FastList<Key> >& extraReelimKeys = boost::none,
│ │ │ │ +
158 bool force_relinearize = false);
│ │ │ │ +
159
│ │ │ │ +
178 virtual ISAM2Result update(const NonlinearFactorGraph& newFactors,
│ │ │ │ +
179 const Values& newTheta,
│ │ │ │ +
180 const ISAM2UpdateParams& updateParams);
│ │ │ │ +
181
│ │ │ │ +
199 void marginalizeLeaves(
│ │ │ │ +
200 const FastList<Key>& leafKeys,
│ │ │ │ +
201 boost::optional<FactorIndices&> marginalFactorsIndices = boost::none,
│ │ │ │ +
202 boost::optional<FactorIndices&> deletedFactorsIndices = boost::none);
│ │ │ │ +
203
│ │ │ │ +
205 const Values& getLinearizationPoint() const { return theta_; }
│ │ │ │ +
206
│ │ │ │ +
208 bool valueExists(Key key) const { return theta_.exists(key); }
│ │ │ │ +
209
│ │ │ │ +
215 Values calculateEstimate() const;
│ │ │ │ +
216
│ │ │ │ +
223 template <class VALUE>
│ │ │ │ +
│ │ │ │ +
224 VALUE calculateEstimate(Key key) const {
│ │ │ │ +
225 const Vector& delta = getDelta()[key];
│ │ │ │ +
226 return traits<VALUE>::Retract(theta_.at<VALUE>(key), delta);
│ │ │ │ +
227 }
│ │ │ │ +
│ │ │ │ +
228
│ │ │ │ +
237 const Value& calculateEstimate(Key key) const;
│ │ │ │ +
238
│ │ │ │ +
240 Matrix marginalCovariance(Key key) const;
│ │ │ │ +
241
│ │ │ │ +
244
│ │ │ │ +
248 Values calculateBestEstimate() const;
│ │ │ │ +
249
│ │ │ │ +
251 const VectorValues& getDelta() const;
│ │ │ │ +
252
│ │ │ │ +
254 double error(const VectorValues& x) const;
│ │ │ │ +
255
│ │ │ │ +
│ │ │ │ +
257 const NonlinearFactorGraph& getFactorsUnsafe() const {
│ │ │ │ +
258 return nonlinearFactors_;
│ │ │ │ +
259 }
│ │ │ │ +
│ │ │ │ +
260
│ │ │ │ +
262 const VariableIndex& getVariableIndex() const { return variableIndex_; }
│ │ │ │ +
263
│ │ │ │ +
265 const KeySet& getFixedVariables() const { return fixedVariables_; }
│ │ │ │ +
266
│ │ │ │ +
267 const ISAM2Params& params() const { return params_; }
│ │ │ │ +
268
│ │ │ │ +
270 void printStats() const { getCliqueData().getStats().print(); }
│ │ │ │ +
271
│ │ │ │ +
279 VectorValues gradientAtZero() const;
│ │ │ │ +
280
│ │ │ │ +
282
│ │ │ │ +
283 protected:
│ │ │ │ +
285 void recalculate(const ISAM2UpdateParams& updateParams,
│ │ │ │ +
286 const KeySet& relinKeys, ISAM2Result* result);
│ │ │ │ +
287
│ │ │ │ +
288 // Do a batch step - reorder and relinearize all variables
│ │ │ │ +
289 void recalculateBatch(const ISAM2UpdateParams& updateParams,
│ │ │ │ +
290 KeySet* affectedKeysSet, ISAM2Result* result);
│ │ │ │ +
291
│ │ │ │ +
292 // retrieve all factors that ONLY contain the affected variables
│ │ │ │ +
293 // (note that the remaining stuff is summarized in the cached factors)
│ │ │ │ +
294 GaussianFactorGraph relinearizeAffectedFactors(
│ │ │ │ +
295 const ISAM2UpdateParams& updateParams, const FastList<Key>& affectedKeys,
│ │ │ │ +
296 const KeySet& relinKeys);
│ │ │ │ +
297
│ │ │ │ +
309 void recalculateIncremental(const ISAM2UpdateParams& updateParams,
│ │ │ │ +
310 const KeySet& relinKeys,
│ │ │ │ +
311 const FastList<Key>& affectedKeys,
│ │ │ │ +
312 KeySet* affectedKeysSet, Cliques* orphans,
│ │ │ │ +
313 ISAM2Result* result);
│ │ │ │ +
314
│ │ │ │ +
320 void addVariables(const Values& newTheta,
│ │ │ │ +
321 ISAM2Result::DetailedResults* detail = 0);
│ │ │ │ +
322
│ │ │ │ +
326 void removeVariables(const KeySet& unusedKeys);
│ │ │ │ +
327
│ │ │ │ +
328 void updateDelta(bool forceFullSolve = false) const;
│ │ │ │ +
329
│ │ │ │ +
330 private:
│ │ │ │ +
332 friend class boost::serialization::access;
│ │ │ │ +
333 template<class ARCHIVE>
│ │ │ │ +
334 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
335 ar & boost::serialization::base_object<BayesTree<ISAM2Clique> >(*this);
│ │ │ │ +
336 ar & BOOST_SERIALIZATION_NVP(theta_);
│ │ │ │ +
337 ar & BOOST_SERIALIZATION_NVP(variableIndex_);
│ │ │ │ +
338 ar & BOOST_SERIALIZATION_NVP(delta_);
│ │ │ │ +
339 ar & BOOST_SERIALIZATION_NVP(deltaNewton_);
│ │ │ │ +
340 ar & BOOST_SERIALIZATION_NVP(RgProd_);
│ │ │ │ +
341 ar & BOOST_SERIALIZATION_NVP(deltaReplacedMask_);
│ │ │ │ +
342 ar & BOOST_SERIALIZATION_NVP(nonlinearFactors_);
│ │ │ │ +
343 ar & BOOST_SERIALIZATION_NVP(linearFactors_);
│ │ │ │ +
344 ar & BOOST_SERIALIZATION_NVP(doglegDelta_);
│ │ │ │ +
345 ar & BOOST_SERIALIZATION_NVP(fixedVariables_);
│ │ │ │ +
346 ar & BOOST_SERIALIZATION_NVP(update_count_);
│ │ │ │ +
347 }
│ │ │ │ +
348
│ │ │ │ +
349}; // ISAM2
│ │ │ │ +
│ │ │ │ +
350
│ │ │ │ +
352template <>
│ │ │ │ +
353struct traits<ISAM2> : public Testable<ISAM2> {};
│ │ │ │ +
354
│ │ │ │ +
355} // namespace gtsam
│ │ │ │ +
GaussianBayesTree.h
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
│ │ │ │ +
ISAM2Result.h
Class that stores detailed iSAM2 result.
│ │ │ │ +
ISAM2Params.h
Parameters for iSAM 2.
│ │ │ │ +
ISAM2UpdateParams.h
Class that stores extra params for ISAM2::update()
│ │ │ │ +
ISAM2Clique.h
Specialized iSAM2 Clique.
│ │ │ │ +
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ -
gtsam::FactorGraph< NonlinearFactor >::push_back
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition FactorGraph.h:186
│ │ │ │ -
gtsam::ExpressionFactor
Factor that supports arbitrary expressions via AD.
Definition ExpressionFactor.h:44
│ │ │ │ -
gtsam::Expression
Expression class that supports automatic differentiation.
Definition Expression.h:48
│ │ │ │ -
gtsam::ExpressionFactorGraph
Factor graph that supports adding ExpressionFactors directly.
Definition ExpressionFactorGraph.h:29
│ │ │ │ -
gtsam::ExpressionFactorGraph::addExpressionFactor
void addExpressionFactor(const Expression< T > &h, const T &z, const SharedNoiseModel &R)
Directly add ExpressionFactor that implements |h(x)-z|^2_R.
Definition ExpressionFactorGraph.h:43
│ │ │ │ +
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition Factor.h:34
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::FastList
FastList is a thin wrapper around std::list that uses the boost fast_pool_allocator instead of the de...
Definition FastList.h:40
│ │ │ │ +
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ + │ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::Value
This is the base class for any type to be stored in Values.
Definition Value.h:37
│ │ │ │ +
gtsam::BayesTree
Bayes tree.
Definition BayesTree.h:67
│ │ │ │ +
gtsam::BayesTree< ISAM2Clique >::sharedClique
boost::shared_ptr< Clique > sharedClique
Shared pointer to a clique.
Definition BayesTree.h:74
│ │ │ │ +
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition VariableIndex.h:43
│ │ │ │ +
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ +
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
gtsam::ISAM2
Implementation of the full ISAM2 algorithm for incremental nonlinear optimization.
Definition ISAM2.h:45
│ │ │ │ +
gtsam::ISAM2::fixedVariables_
KeySet fixedVariables_
Set of variables that are involved with linear factors from marginalized variables and thus cannot ha...
Definition ISAM2.h:94
│ │ │ │ +
gtsam::ISAM2::deltaReplacedMask_
KeySet deltaReplacedMask_
A cumulative mask for the variables that were replaced and have not yet been updated in the linear so...
Definition ISAM2.h:76
│ │ │ │ +
gtsam::ISAM2::update_count_
int update_count_
Counter incremented every update(), used to determine periodic relinearization.
Definition ISAM2.h:96
│ │ │ │ +
gtsam::ISAM2::calculateEstimate
VALUE calculateEstimate(Key key) const
Compute an estimate for a single variable using its incomplete linear delta computed during the last ...
Definition ISAM2.h:224
│ │ │ │ +
gtsam::ISAM2::~ISAM2
virtual ~ISAM2()
default virtual destructor
Definition ISAM2.h:114
│ │ │ │ +
gtsam::ISAM2::getFixedVariables
const KeySet & getFixedVariables() const
Access the nonlinear variable index.
Definition ISAM2.h:265
│ │ │ │ +
gtsam::ISAM2::Cliques
Base::Cliques Cliques
List of Cliques.
Definition ISAM2.h:104
│ │ │ │ +
gtsam::ISAM2::printStats
void printStats() const
prints out clique statistics
Definition ISAM2.h:270
│ │ │ │ +
gtsam::ISAM2::nonlinearFactors_
NonlinearFactorGraph nonlinearFactors_
All original nonlinear factors are stored here to use during relinearization.
Definition ISAM2.h:81
│ │ │ │ +
gtsam::ISAM2::getVariableIndex
const VariableIndex & getVariableIndex() const
Access the nonlinear variable index.
Definition ISAM2.h:262
│ │ │ │ +
gtsam::ISAM2::sharedClique
Base::sharedClique sharedClique
Shared pointer to a clique.
Definition ISAM2.h:103
│ │ │ │ +
gtsam::ISAM2::delta_
VectorValues delta_
The linear delta from the last linear solution, an update to the estimate in theta.
Definition ISAM2.h:61
│ │ │ │ +
gtsam::ISAM2::variableIndex_
VariableIndex variableIndex_
VariableIndex lets us look up factors by involved variable and keeps track of dimensions.
Definition ISAM2.h:52
│ │ │ │ +
gtsam::ISAM2::getLinearizationPoint
const Values & getLinearizationPoint() const
Access the current linearization point.
Definition ISAM2.h:205
│ │ │ │ +
gtsam::ISAM2::theta_
Values theta_
The current linearization point.
Definition ISAM2.h:48
│ │ │ │ +
gtsam::ISAM2::params_
ISAM2Params params_
The current parameters.
Definition ISAM2.h:87
│ │ │ │ +
gtsam::ISAM2::doglegDelta_
boost::optional< double > doglegDelta_
The current Dogleg Delta (trust region radius)
Definition ISAM2.h:90
│ │ │ │ +
gtsam::ISAM2::linearFactors_
GaussianFactorGraph linearFactors_
The current linear factors, which are only updated as needed.
Definition ISAM2.h:84
│ │ │ │ +
gtsam::ISAM2::getFactorsUnsafe
const NonlinearFactorGraph & getFactorsUnsafe() const
Access the set of nonlinear factors.
Definition ISAM2.h:257
│ │ │ │ +
gtsam::ISAM2::valueExists
bool valueExists(Key key) const
Check whether variable with given key exists in linearization point.
Definition ISAM2.h:208
│ │ │ │ +
gtsam::ISAM2Clique
Specialized Clique structure for ISAM2, incorporating caching and gradient contribution TODO: more do...
Definition ISAM2Clique.h:37
│ │ │ │ +
gtsam::ISAM2Params
Definition ISAM2Params.h:135
│ │ │ │ +
gtsam::ISAM2Result
This struct is returned from ISAM2::update() and contains information about the update that is useful...
Definition ISAM2Result.h:41
│ │ │ │ +
gtsam::ISAM2Result::DetailedResults
A struct holding detailed results, which must be enabled with ISAM2Params::enableDetailedResults.
Definition ISAM2Result.h:117
│ │ │ │ +
gtsam::ISAM2UpdateParams
This struct is used by ISAM2::update() to pass additional parameters to give the user a fine-grained ...
Definition ISAM2UpdateParams.h:32
│ │ │ │
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │ +
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
gtsam::Values::at
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
│ │ │ │ +
gtsam::Values::exists
bool exists(Key j) const
Check if a value exists with key j.
Definition Values.cpp:94
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,76 +1,373 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ExpressionFactorGraph.h │ │ │ │ │ +ISAM2.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ +19// \callgraph │ │ │ │ │ 20 │ │ │ │ │ -21#include │ │ │ │ │ -22#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -23 │ │ │ │ │ -24namespace _g_t_s_a_m { │ │ │ │ │ -25 │ │ │ │ │ -_2_9class _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_G_r_a_p_h: public _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h { │ │ │ │ │ -30 │ │ │ │ │ -31public: │ │ │ │ │ -32 │ │ │ │ │ -35 │ │ │ │ │ -42 template │ │ │ │ │ -_4_3 void _a_d_d_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r(const _E_x_p_r_e_s_s_i_o_n_<_T_>& h, const T& z, │ │ │ │ │ -44 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& R) { │ │ │ │ │ -45 using F = _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_<_T_>; │ │ │ │ │ -46 _p_u_s_h___b_a_c_k(boost::allocate_shared(Eigen::aligned_allocator(), R, z, │ │ │ │ │ -h)); │ │ │ │ │ -47 } │ │ │ │ │ -48 │ │ │ │ │ -50}; │ │ │ │ │ -51 │ │ │ │ │ -52} │ │ │ │ │ +21#pragma once │ │ │ │ │ +22 │ │ │ │ │ +23#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_I_S_A_M_2_C_l_i_q_u_e_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_I_S_A_M_2_P_a_r_a_m_s_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_I_S_A_M_2_R_e_s_u_l_t_._h> │ │ │ │ │ +27#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_._h> │ │ │ │ │ +28#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +29 │ │ │ │ │ +30#include │ │ │ │ │ +31 │ │ │ │ │ +32namespace _g_t_s_a_m { │ │ │ │ │ +33 │ │ │ │ │ +_4_5class GTSAM_EXPORT _I_S_A_M_2 : public _B_a_y_e_s_T_r_e_e { │ │ │ │ │ +46 protected: │ │ │ │ │ +_4_8 _V_a_l_u_e_s _t_h_e_t_a__; │ │ │ │ │ +49 │ │ │ │ │ +_5_2 _V_a_r_i_a_b_l_e_I_n_d_e_x _v_a_r_i_a_b_l_e_I_n_d_e_x__; │ │ │ │ │ +53 │ │ │ │ │ +_6_1 mutable _V_e_c_t_o_r_V_a_l_u_e_s _d_e_l_t_a__; │ │ │ │ │ +62 │ │ │ │ │ +63 mutable _V_e_c_t_o_r_V_a_l_u_e_s deltaNewton_; // Only used when using Dogleg - stores │ │ │ │ │ +64 // the Gauss-Newton update │ │ │ │ │ +65 mutable _V_e_c_t_o_r_V_a_l_u_e_s RgProd_; // Only used when using Dogleg - stores R*g │ │ │ │ │ +and │ │ │ │ │ +66 // is updated incrementally │ │ │ │ │ +67 │ │ │ │ │ +_7_6 mutable _K_e_y_S_e_t _d_e_l_t_a_R_e_p_l_a_c_e_d_M_a_s_k__; // TODO(dellaert): Make sure accessed in │ │ │ │ │ +77 // the right way │ │ │ │ │ +78 │ │ │ │ │ +_8_1 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h _n_o_n_l_i_n_e_a_r_F_a_c_t_o_r_s__; │ │ │ │ │ +82 │ │ │ │ │ +_8_4 mutable _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h _l_i_n_e_a_r_F_a_c_t_o_r_s__; │ │ │ │ │ +85 │ │ │ │ │ +_8_7 _I_S_A_M_2_P_a_r_a_m_s _p_a_r_a_m_s__; │ │ │ │ │ +88 │ │ │ │ │ +_9_0 mutable boost::optional _d_o_g_l_e_g_D_e_l_t_a__; │ │ │ │ │ +91 │ │ │ │ │ +_9_4 _K_e_y_S_e_t _f_i_x_e_d_V_a_r_i_a_b_l_e_s__; │ │ │ │ │ +95 │ │ │ │ │ +_9_6 int _u_p_d_a_t_e___c_o_u_n_t__; │ │ │ │ │ +98 │ │ │ │ │ +99 public: │ │ │ │ │ +_1_0_0 using _T_h_i_s = _I_S_A_M_2; │ │ │ │ │ +_1_0_1 using _B_a_s_e = _B_a_y_e_s_T_r_e_e_<_I_S_A_M_2_C_l_i_q_u_e_>; │ │ │ │ │ +_1_0_2 using _C_l_i_q_u_e = _B_a_s_e_:_:_C_l_i_q_u_e; │ │ │ │ │ +_1_0_3 using _s_h_a_r_e_d_C_l_i_q_u_e = _B_a_s_e_:_:_s_h_a_r_e_d_C_l_i_q_u_e; │ │ │ │ │ +_1_0_4 using _C_l_i_q_u_e_s = Base::Cliques; │ │ │ │ │ +105 │ │ │ │ │ +107 explicit _I_S_A_M_2(const _I_S_A_M_2_P_a_r_a_m_s& params); │ │ │ │ │ +108 │ │ │ │ │ +111 _I_S_A_M_2(); │ │ │ │ │ +112 │ │ │ │ │ +_1_1_4 virtual _~_I_S_A_M_2() {} │ │ │ │ │ +115 │ │ │ │ │ +117 virtual bool _e_q_u_a_l_s(const _I_S_A_M_2& other, double tol = 1e-9) const; │ │ │ │ │ +118 │ │ │ │ │ +151 virtual _I_S_A_M_2_R_e_s_u_l_t update( │ │ │ │ │ +152 const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& newFactors = _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h(), │ │ │ │ │ +153 const _V_a_l_u_e_s& newTheta = _V_a_l_u_e_s(), │ │ │ │ │ +154 const _F_a_c_t_o_r_I_n_d_i_c_e_s& removeFactorIndices = _F_a_c_t_o_r_I_n_d_i_c_e_s(), │ │ │ │ │ +155 const boost::optional<_F_a_s_t_M_a_p_<_K_e_y_,_ _i_n_t_> >& constrainedKeys = boost::none, │ │ │ │ │ +156 const boost::optional<_F_a_s_t_L_i_s_t_<_K_e_y_> >& noRelinKeys = boost::none, │ │ │ │ │ +157 const boost::optional<_F_a_s_t_L_i_s_t_<_K_e_y_> >& extraReelimKeys = boost::none, │ │ │ │ │ +158 bool force_relinearize = false); │ │ │ │ │ +159 │ │ │ │ │ +178 virtual _I_S_A_M_2_R_e_s_u_l_t update(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& newFactors, │ │ │ │ │ +179 const _V_a_l_u_e_s& newTheta, │ │ │ │ │ +180 const _I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s& updateParams); │ │ │ │ │ +181 │ │ │ │ │ +199 void marginalizeLeaves( │ │ │ │ │ +200 const _F_a_s_t_L_i_s_t_<_K_e_y_>& leafKeys, │ │ │ │ │ +201 boost::optional marginalFactorsIndices = boost::none, │ │ │ │ │ +202 boost::optional deletedFactorsIndices = boost::none); │ │ │ │ │ +203 │ │ │ │ │ +_2_0_5 const _V_a_l_u_e_s& _g_e_t_L_i_n_e_a_r_i_z_a_t_i_o_n_P_o_i_n_t() const { return theta_; } │ │ │ │ │ +206 │ │ │ │ │ +_2_0_8 bool _v_a_l_u_e_E_x_i_s_t_s(_K_e_y key) const { return theta_._e_x_i_s_t_s(key); } │ │ │ │ │ +209 │ │ │ │ │ +215 _V_a_l_u_e_s calculateEstimate() const; │ │ │ │ │ +216 │ │ │ │ │ +223 template │ │ │ │ │ +_2_2_4 VALUE _c_a_l_c_u_l_a_t_e_E_s_t_i_m_a_t_e(_K_e_y key) const { │ │ │ │ │ +225 const Vector& delta = getDelta()[key]; │ │ │ │ │ +226 return _t_r_a_i_t_s_<_V_A_L_U_E_>_:_:_R_e_t_r_a_c_t(theta_._a_t(key), delta); │ │ │ │ │ +227 } │ │ │ │ │ +228 │ │ │ │ │ +237 const _V_a_l_u_e& calculateEstimate(_K_e_y key) const; │ │ │ │ │ +238 │ │ │ │ │ +240 Matrix marginalCovariance(_K_e_y key) const; │ │ │ │ │ +241 │ │ │ │ │ +244 │ │ │ │ │ +248 _V_a_l_u_e_s calculateBestEstimate() const; │ │ │ │ │ +249 │ │ │ │ │ +251 const _V_e_c_t_o_r_V_a_l_u_e_s& getDelta() const; │ │ │ │ │ +252 │ │ │ │ │ +254 double error(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const; │ │ │ │ │ +255 │ │ │ │ │ +_2_5_7 const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& _g_e_t_F_a_c_t_o_r_s_U_n_s_a_f_e() const { │ │ │ │ │ +258 return nonlinearFactors_; │ │ │ │ │ +259 } │ │ │ │ │ +260 │ │ │ │ │ +_2_6_2 const _V_a_r_i_a_b_l_e_I_n_d_e_x& _g_e_t_V_a_r_i_a_b_l_e_I_n_d_e_x() const { return variableIndex_; } │ │ │ │ │ +263 │ │ │ │ │ +_2_6_5 const _K_e_y_S_e_t& _g_e_t_F_i_x_e_d_V_a_r_i_a_b_l_e_s() const { return fixedVariables_; } │ │ │ │ │ +266 │ │ │ │ │ +267 const _I_S_A_M_2_P_a_r_a_m_s& params() const { return params_; } │ │ │ │ │ +268 │ │ │ │ │ +_2_7_0 void _p_r_i_n_t_S_t_a_t_s() const { getCliqueData().getStats().print(); } │ │ │ │ │ +271 │ │ │ │ │ +279 _V_e_c_t_o_r_V_a_l_u_e_s gradientAtZero() const; │ │ │ │ │ +280 │ │ │ │ │ +282 │ │ │ │ │ +283 protected: │ │ │ │ │ +285 void recalculate(const _I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s& updateParams, │ │ │ │ │ +286 const _K_e_y_S_e_t& relinKeys, _I_S_A_M_2_R_e_s_u_l_t* result); │ │ │ │ │ +287 │ │ │ │ │ +288 // Do a batch step - reorder and relinearize all variables │ │ │ │ │ +289 void recalculateBatch(const _I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s& updateParams, │ │ │ │ │ +290 _K_e_y_S_e_t* affectedKeysSet, _I_S_A_M_2_R_e_s_u_l_t* result); │ │ │ │ │ +291 │ │ │ │ │ +292 // retrieve all factors that ONLY contain the affected variables │ │ │ │ │ +293 // (note that the remaining stuff is summarized in the cached factors) │ │ │ │ │ +294 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h relinearizeAffectedFactors( │ │ │ │ │ +295 const _I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s& updateParams, const _F_a_s_t_L_i_s_t_<_K_e_y_>& affectedKeys, │ │ │ │ │ +296 const _K_e_y_S_e_t& relinKeys); │ │ │ │ │ +297 │ │ │ │ │ +309 void recalculateIncremental(const _I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s& updateParams, │ │ │ │ │ +310 const _K_e_y_S_e_t& relinKeys, │ │ │ │ │ +311 const _F_a_s_t_L_i_s_t_<_K_e_y_>& affectedKeys, │ │ │ │ │ +312 _K_e_y_S_e_t* affectedKeysSet, Cliques* orphans, │ │ │ │ │ +313 _I_S_A_M_2_R_e_s_u_l_t* result); │ │ │ │ │ +314 │ │ │ │ │ +320 void addVariables(const _V_a_l_u_e_s& newTheta, │ │ │ │ │ +321 _I_S_A_M_2_R_e_s_u_l_t_:_:_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s* detail = 0); │ │ │ │ │ +322 │ │ │ │ │ +326 void removeVariables(const _K_e_y_S_e_t& unusedKeys); │ │ │ │ │ +327 │ │ │ │ │ +328 void updateDelta(bool forceFullSolve = false) const; │ │ │ │ │ +329 │ │ │ │ │ +330 private: │ │ │ │ │ +_3_3_2 friend class boost::serialization::access; │ │ │ │ │ +333 template │ │ │ │ │ +334 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +335 ar & boost::serialization::base_object >(*this); │ │ │ │ │ +336 ar & BOOST_SERIALIZATION_NVP(theta_); │ │ │ │ │ +337 ar & BOOST_SERIALIZATION_NVP(variableIndex_); │ │ │ │ │ +338 ar & BOOST_SERIALIZATION_NVP(delta_); │ │ │ │ │ +339 ar & BOOST_SERIALIZATION_NVP(deltaNewton_); │ │ │ │ │ +340 ar & BOOST_SERIALIZATION_NVP(RgProd_); │ │ │ │ │ +341 ar & BOOST_SERIALIZATION_NVP(deltaReplacedMask_); │ │ │ │ │ +342 ar & BOOST_SERIALIZATION_NVP(nonlinearFactors_); │ │ │ │ │ +343 ar & BOOST_SERIALIZATION_NVP(linearFactors_); │ │ │ │ │ +344 ar & BOOST_SERIALIZATION_NVP(doglegDelta_); │ │ │ │ │ +345 ar & BOOST_SERIALIZATION_NVP(fixedVariables_); │ │ │ │ │ +346 ar & BOOST_SERIALIZATION_NVP(update_count_); │ │ │ │ │ +347 } │ │ │ │ │ +348 │ │ │ │ │ +349}; // ISAM2 │ │ │ │ │ +350 │ │ │ │ │ +352template <> │ │ │ │ │ +_3_5_3struct _t_r_a_i_t_s<_I_S_A_M_2> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ +354 │ │ │ │ │ +355} // namespace gtsam │ │ │ │ │ +_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ +Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ │ +_I_S_A_M_2_R_e_s_u_l_t_._h │ │ │ │ │ +Class that stores detailed iSAM2 result. │ │ │ │ │ +_I_S_A_M_2_P_a_r_a_m_s_._h │ │ │ │ │ +Parameters for iSAM 2. │ │ │ │ │ +_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_._h │ │ │ │ │ +Class that stores extra params for ISAM2::update() │ │ │ │ │ +_I_S_A_M_2_C_l_i_q_u_e_._h │ │ │ │ │ +Specialized iSAM2 Clique. │ │ │ │ │ _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ Factor Graph consisting of non-linear factors. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ -Aliases. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_<_ _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_ _>_:_:_p_u_s_h___b_a_c_k │ │ │ │ │ -IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor) │ │ │ │ │ -Add a factor directly using a shared_ptr. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:186 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r │ │ │ │ │ -Factor that supports arbitrary expressions via AD. │ │ │ │ │ -DDeeffiinniittiioonn ExpressionFactor.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n │ │ │ │ │ -Expression class that supports automatic differentiation. │ │ │ │ │ -DDeeffiinniittiioonn Expression.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -Factor graph that supports adding ExpressionFactors directly. │ │ │ │ │ -DDeeffiinniittiioonn ExpressionFactorGraph.h:29 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r │ │ │ │ │ -void addExpressionFactor(const Expression< T > &h, const T &z, const │ │ │ │ │ -SharedNoiseModel &R) │ │ │ │ │ -Directly add ExpressionFactor that implements |h(x)-z|^2_R. │ │ │ │ │ -DDeeffiinniittiioonn ExpressionFactorGraph.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_I_n_d_i_c_e_s │ │ │ │ │ +FastVector< FactorIndex > FactorIndices │ │ │ │ │ +Define collection types: │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t │ │ │ │ │ +FastList is a thin wrapper around std::list that uses the boost │ │ │ │ │ +fast_pool_allocator instead of the de... │ │ │ │ │ +DDeeffiinniittiioonn FastList.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ +FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ +fast_pool_allocator instead of the defa... │ │ │ │ │ +DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e │ │ │ │ │ +This is the base class for any type to be stored in Values. │ │ │ │ │ +DDeeffiinniittiioonn Value.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e │ │ │ │ │ +Bayes tree. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_<_ _I_S_A_M_2_C_l_i_q_u_e_ _>_:_:_s_h_a_r_e_d_C_l_i_q_u_e │ │ │ │ │ +boost::shared_ptr< Clique > sharedClique │ │ │ │ │ +Shared pointer to a clique. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ +The VariableIndex class computes and stores the block column structure of a │ │ │ │ │ +factor graph. │ │ │ │ │ +DDeeffiinniittiioonn VariableIndex.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ +with a unique integer... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2 │ │ │ │ │ +Implementation of the full ISAM2 algorithm for incremental nonlinear │ │ │ │ │ +optimization. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_:_:_f_i_x_e_d_V_a_r_i_a_b_l_e_s__ │ │ │ │ │ +KeySet fixedVariables_ │ │ │ │ │ +Set of variables that are involved with linear factors from marginalized │ │ │ │ │ +variables and thus cannot ha... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2.h:94 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_:_:_d_e_l_t_a_R_e_p_l_a_c_e_d_M_a_s_k__ │ │ │ │ │ +KeySet deltaReplacedMask_ │ │ │ │ │ +A cumulative mask for the variables that were replaced and have not yet been │ │ │ │ │ +updated in the linear so... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2.h:76 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_:_:_u_p_d_a_t_e___c_o_u_n_t__ │ │ │ │ │ +int update_count_ │ │ │ │ │ +Counter incremented every update(), used to determine periodic relinearization. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2.h:96 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_:_:_c_a_l_c_u_l_a_t_e_E_s_t_i_m_a_t_e │ │ │ │ │ +VALUE calculateEstimate(Key key) const │ │ │ │ │ +Compute an estimate for a single variable using its incomplete linear delta │ │ │ │ │ +computed during the last ... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2.h:224 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_:_:_~_I_S_A_M_2 │ │ │ │ │ +virtual ~ISAM2() │ │ │ │ │ +default virtual destructor │ │ │ │ │ +DDeeffiinniittiioonn ISAM2.h:114 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_:_:_g_e_t_F_i_x_e_d_V_a_r_i_a_b_l_e_s │ │ │ │ │ +const KeySet & getFixedVariables() const │ │ │ │ │ +Access the nonlinear variable index. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2.h:265 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_:_:_C_l_i_q_u_e_s │ │ │ │ │ +Base::Cliques Cliques │ │ │ │ │ +List of Cliques. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2.h:104 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_:_:_p_r_i_n_t_S_t_a_t_s │ │ │ │ │ +void printStats() const │ │ │ │ │ +prints out clique statistics │ │ │ │ │ +DDeeffiinniittiioonn ISAM2.h:270 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_:_:_n_o_n_l_i_n_e_a_r_F_a_c_t_o_r_s__ │ │ │ │ │ +NonlinearFactorGraph nonlinearFactors_ │ │ │ │ │ +All original nonlinear factors are stored here to use during relinearization. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2.h:81 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_:_:_g_e_t_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ +const VariableIndex & getVariableIndex() const │ │ │ │ │ +Access the nonlinear variable index. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2.h:262 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_:_:_s_h_a_r_e_d_C_l_i_q_u_e │ │ │ │ │ +Base::sharedClique sharedClique │ │ │ │ │ +Shared pointer to a clique. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2.h:103 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_:_:_d_e_l_t_a__ │ │ │ │ │ +VectorValues delta_ │ │ │ │ │ +The linear delta from the last linear solution, an update to the estimate in │ │ │ │ │ +theta. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2.h:61 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_:_:_v_a_r_i_a_b_l_e_I_n_d_e_x__ │ │ │ │ │ +VariableIndex variableIndex_ │ │ │ │ │ +VariableIndex lets us look up factors by involved variable and keeps track of │ │ │ │ │ +dimensions. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_:_:_g_e_t_L_i_n_e_a_r_i_z_a_t_i_o_n_P_o_i_n_t │ │ │ │ │ +const Values & getLinearizationPoint() const │ │ │ │ │ +Access the current linearization point. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2.h:205 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_:_:_t_h_e_t_a__ │ │ │ │ │ +Values theta_ │ │ │ │ │ +The current linearization point. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_:_:_p_a_r_a_m_s__ │ │ │ │ │ +ISAM2Params params_ │ │ │ │ │ +The current parameters. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2.h:87 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_:_:_d_o_g_l_e_g_D_e_l_t_a__ │ │ │ │ │ +boost::optional< double > doglegDelta_ │ │ │ │ │ +The current Dogleg Delta (trust region radius) │ │ │ │ │ +DDeeffiinniittiioonn ISAM2.h:90 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_:_:_l_i_n_e_a_r_F_a_c_t_o_r_s__ │ │ │ │ │ +GaussianFactorGraph linearFactors_ │ │ │ │ │ +The current linear factors, which are only updated as needed. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2.h:84 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_:_:_g_e_t_F_a_c_t_o_r_s_U_n_s_a_f_e │ │ │ │ │ +const NonlinearFactorGraph & getFactorsUnsafe() const │ │ │ │ │ +Access the set of nonlinear factors. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2.h:257 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_:_:_v_a_l_u_e_E_x_i_s_t_s │ │ │ │ │ +bool valueExists(Key key) const │ │ │ │ │ +Check whether variable with given key exists in linearization point. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2.h:208 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_C_l_i_q_u_e │ │ │ │ │ +Specialized Clique structure for ISAM2, incorporating caching and gradient │ │ │ │ │ +contribution TODO: more do... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Clique.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:135 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t │ │ │ │ │ +This struct is returned from ISAM2::update() and contains information about the │ │ │ │ │ +update that is useful... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Result.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s │ │ │ │ │ +A struct holding detailed results, which must be enabled with ISAM2Params:: │ │ │ │ │ +enableDetailedResults. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Result.h:117 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s │ │ │ │ │ +This struct is used by ISAM2::update() to pass additional parameters to give │ │ │ │ │ +the user a fine-grained ... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2UpdateParams.h:32 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_a_t │ │ │ │ │ +const ValueType at(Key j) const │ │ │ │ │ +Retrieve a variable by key j. │ │ │ │ │ +DDeeffiinniittiioonn Values-inl.h:361 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_e_x_i_s_t_s │ │ │ │ │ +bool exists(Key j) const │ │ │ │ │ +Check if a value exists with key j. │ │ │ │ │ +DDeeffiinniittiioonn Values.cpp:94 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ + * _I_S_A_M_2_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01151.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/CustomFactor.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/LevenbergMarquardtOptimizer.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,35 +94,49 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
CustomFactor.cpp File Reference
│ │ │ │ +
LevenbergMarquardtOptimizer.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Class to enable arbitrary factors with runtime swappable error function. │ │ │ │ +

A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::LevenbergMarquardtOptimizer
 This class performs Levenberg-Marquardt nonlinear optimization. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Class to enable arbitrary factors with runtime swappable error function.

│ │ │ │ -
Author
Fan Jiang
│ │ │ │ +

A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +
│ │ │ │ +Luca Carlone
│ │ │ │ +
Date
Feb 26, 2012
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,30 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -CustomFactor.cpp File Reference │ │ │ │ │ -Class to enable arbitrary factors with runtime swappable error function. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +LevenbergMarquardtOptimizer.h File Reference │ │ │ │ │ +A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme. │ │ │ │ │ _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r │ │ │ │ │ +  This class performs Levenberg-Marquardt nonlinear optimization. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Class to enable arbitrary factors with runtime swappable error function. │ │ │ │ │ +A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme. │ │ │ │ │ Author │ │ │ │ │ - Fan Jiang │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Date │ │ │ │ │ + Feb 26, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _C_u_s_t_o_m_F_a_c_t_o_r_._c_p_p │ │ │ │ │ + * _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01154.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/expressionTesting.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/GraphvizFormatting.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,107 +94,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Macros | │ │ │ │ -Functions
│ │ │ │ -
expressionTesting.h File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
GraphvizFormatting.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Test harness methods for expressions. │ │ │ │ +

Graphviz formatter for NonlinearFactorGraph. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

struct  gtsam::GraphvizFormatting
 Formatting options and functions for saving a NonlinearFactorGraph instance in GraphViz format. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Macros

#define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, numerical_derivative_step, tolerance)    { EXPECT(gtsam::internal::testExpressionJacobians(name_, expression, values, numerical_derivative_step, tolerance)); }
 Check the Jacobians produced by an expression against finite differences.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -template<typename T >
bool gtsam::internal::testExpressionJacobians (const std::string &name_, const gtsam::Expression< T > &expression, const gtsam::Values &values, double nd_step, double tolerance)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Test harness methods for expressions.

│ │ │ │ -
Date
September 18, 2014
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ -Paul Furgale
│ │ │ │ -

Macro Definition Documentation

│ │ │ │ - │ │ │ │ -

◆ EXPECT_CORRECT_EXPRESSION_JACOBIANS

│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
#define EXPECT_CORRECT_EXPRESSION_JACOBIANS( expression,
 values,
 numerical_derivative_step,
 tolerance 
)    { EXPECT(gtsam::internal::testExpressionJacobians(name_, expression, values, numerical_derivative_step, tolerance)); }
│ │ │ │ -
│ │ │ │ - │ │ │ │ -

Check the Jacobians produced by an expression against finite differences.

│ │ │ │ -
Parameters
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
expressionThe expression to test.
valuesValues filled in for testing the Jacobians.
numerical_derivative_stepThe step to use when computing the finite difference Jacobians
toleranceThe numerical tolerance to use when comparing Jacobians.
│ │ │ │ -
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ +

Graphviz formatter for NonlinearFactorGraph.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
Date
December, 2021
│ │ │ │ +
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,58 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -expressionTesting.h File Reference │ │ │ │ │ -Test harness methods for expressions. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +GraphvizFormatting.h File Reference │ │ │ │ │ +Graphviz formatter for NonlinearFactorGraph. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +struct   _g_t_s_a_m_:_:_G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g │ │ │ │ │ +  Formatting options and functions for saving a _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ + instance in GraphViz format. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -MMaaccrrooss │ │ │ │ │ -#define  _E_X_P_E_C_T___C_O_R_R_E_C_T___E_X_P_R_E_S_S_I_O_N___J_A_C_O_B_I_A_N_S(expression, values, │ │ │ │ │ - numerical_derivative_step, tolerance)    { EXPECT(gtsam::internal:: │ │ │ │ │ - testExpressionJacobians(name_, expression, values, │ │ │ │ │ - numerical_derivative_step, tolerance)); } │ │ │ │ │ -  Check the Jacobians produced by an expression against finite │ │ │ │ │ - differences. │ │ │ │ │ -  │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ -bool  ggttssaamm::::iinntteerrnnaall::::tteessttEExxpprreessssiioonnJJaaccoobbiiaannss (const std::string &name_, const │ │ │ │ │ - _g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n< T > &expression, const _g_t_s_a_m_:_:_V_a_l_u_e_s &values, double │ │ │ │ │ - nd_step, double tolerance) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Test harness methods for expressions. │ │ │ │ │ - Date │ │ │ │ │ - September 18, 2014 │ │ │ │ │ +Graphviz formatter for NonlinearFactorGraph. │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Paul Furgale │ │ │ │ │ -********** MMaaccrroo DDeeffiinniittiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ -********** _?◆_? EEXXPPEECCTT__CCOORRRREECCTT__EEXXPPRREESSSSIIOONN__JJAACCOOBBIIAANNSS ********** │ │ │ │ │ -#define (   expression, │ │ │ │ │ -EXPECT_CORRECT_EXPRESSION_JACOBIANS │ │ │ │ │ -   values, │ │ │ │ │ -   numerical_derivative_step, │ │ │ │ │ -   tolerance  │ │ │ │ │ -     { EXPECT(gtsam::internal:: │ │ │ │ │ - testExpressionJacobians(name_, │ │ │ │ │ - ) expression, values, │ │ │ │ │ - numerical_derivative_step, tolerance)); │ │ │ │ │ - } │ │ │ │ │ -Check the Jacobians produced by an expression against finite differences. │ │ │ │ │ - Parameters │ │ │ │ │ - expression The expression to test. │ │ │ │ │ - values _V_a_l_u_e_s filled in for testing the Jacobians. │ │ │ │ │ - numerical_derivative_step The step to use when computing the finite │ │ │ │ │ - difference Jacobians │ │ │ │ │ - tolerance The numerical tolerance to use when comparing │ │ │ │ │ - Jacobians. │ │ │ │ │ + Date │ │ │ │ │ + December, 2021 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _e_x_p_r_e_s_s_i_o_n_T_e_s_t_i_n_g_._h │ │ │ │ │ + * _G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01154.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a01154 = [ │ │ │ │ │ - ["EXPECT_CORRECT_EXPRESSION_JACOBIANS", "a01154.html#a701d0cd12b81a725f7f9cd2432fe9e2a", null] │ │ │ │ │ + ["gtsam::GraphvizFormatting", "a04368.html", "a04368"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01154_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/expressionTesting.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/GraphvizFormatting.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,66 +98,86 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
expressionTesting.h
│ │ │ │ +
GraphvizFormatting.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010-2021, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/nonlinear/ExpressionFactor.h>
│ │ │ │ -
23#include <gtsam/nonlinear/factorTesting.h>
│ │ │ │ -
24#include <gtsam/base/Testable.h>
│ │ │ │ -
25
│ │ │ │ -
26namespace gtsam {
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/inference/DotWriter.h>
│ │ │ │ +
22
│ │ │ │ +
23namespace gtsam {
│ │ │ │ +
24
│ │ │ │ +
25class Values;
│ │ │ │ +
26class Value;
│ │ │ │
27
│ │ │ │ -
28namespace internal {
│ │ │ │ -
29// CPPUnitLite-style test for linearization of an ExpressionFactor
│ │ │ │ -
30template<typename T>
│ │ │ │ -
31bool testExpressionJacobians(const std::string& name_,
│ │ │ │ -
32 const gtsam::Expression<T>& expression, const gtsam::Values& values,
│ │ │ │ -
33 double nd_step, double tolerance) {
│ │ │ │ -
34 // Create factor
│ │ │ │ -
35 size_t size = traits<T>::dimension;
│ │ │ │ -
36 ExpressionFactor<T> f(noiseModel::Unit::Create(size),
│ │ │ │ -
37 expression.value(values), expression);
│ │ │ │ -
38 return testFactorJacobians(name_, f, values, nd_step, tolerance);
│ │ │ │ -
39}
│ │ │ │ -
40} // namespace internal
│ │ │ │ -
41} // namespace gtsam
│ │ │ │ -
42
│ │ │ │ -
│ │ │ │ -
48#define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, numerical_derivative_step, tolerance) \
│ │ │ │ -
49 { EXPECT(gtsam::internal::testExpressionJacobians(name_, expression, values, numerical_derivative_step, tolerance)); }
│ │ │ │ -
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ -
factorTesting.h
Evaluate derivatives of a nonlinear factor numerically.
│ │ │ │ +
│ │ │ │ +
32struct GTSAM_EXPORT GraphvizFormatting : public DotWriter {
│ │ │ │ +
34 enum Axis { X, Y, Z, NEGX, NEGY, NEGZ };
│ │ │ │ +
35
│ │ │ │ +
36 Axis paperHorizontalAxis;
│ │ │ │ +
38 Axis paperVerticalAxis;
│ │ │ │ +
40 double scale;
│ │ │ │ +
41 bool mergeSimilarFactors;
│ │ │ │ +
43
│ │ │ │ +
│ │ │ │ +
46 GraphvizFormatting()
│ │ │ │ +
47 : paperHorizontalAxis(Y),
│ │ │ │ +
48 paperVerticalAxis(X),
│ │ │ │ +
49 scale(1),
│ │ │ │ +
50 mergeSimilarFactors(false) {}
│ │ │ │ +
│ │ │ │ +
51
│ │ │ │ +
52 // Find bounds
│ │ │ │ +
53 Vector2 findBounds(const Values& values, const KeySet& keys) const;
│ │ │ │ +
54
│ │ │ │ +
56 boost::optional<Vector2> extractPosition(const Value& value) const;
│ │ │ │ +
57
│ │ │ │ +
59 boost::optional<Vector2> variablePos(const Values& values, const Vector2& min,
│ │ │ │ +
60 Key key) const;
│ │ │ │ +
61
│ │ │ │ +
63 boost::optional<Vector2> factorPos(const Vector2& min, size_t i) const;
│ │ │ │ +
64};
│ │ │ │ +
│ │ │ │ +
65
│ │ │ │ +
66} // namespace gtsam
│ │ │ │ +
DotWriter.h
Graphviz formatter.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Create a unit covariance noise model.
Definition NoiseModel.h:597
│ │ │ │ -
gtsam::Expression
Expression class that supports automatic differentiation.
Definition Expression.h:48
│ │ │ │ -
gtsam::Expression::value
T value(const Values &values, boost::optional< std::vector< Matrix > & > H=boost::none) const
Return value and optional derivatives, reverse AD version Notes: this is not terribly efficient,...
Definition Expression-inl.h:147
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ + │ │ │ │ +
gtsam::Value
This is the base class for any type to be stored in Values.
Definition Value.h:37
│ │ │ │ +
gtsam::DotWriter
DotWriter is a helper class for writing graphviz .dot files.
Definition DotWriter.h:35
│ │ │ │ +
gtsam::GraphvizFormatting
Formatting options and functions for saving a NonlinearFactorGraph instance in GraphViz format.
Definition GraphvizFormatting.h:32
│ │ │ │ +
gtsam::GraphvizFormatting::Axis
Axis
World axes to be assigned to paper axes.
Definition GraphvizFormatting.h:34
│ │ │ │ +
gtsam::GraphvizFormatting::paperVerticalAxis
Axis paperVerticalAxis
The world axis assigned to the vertical paper axis.
Definition GraphvizFormatting.h:38
│ │ │ │ +
gtsam::GraphvizFormatting::GraphvizFormatting
GraphvizFormatting()
Default constructor sets up robot coordinates.
Definition GraphvizFormatting.h:46
│ │ │ │ +
gtsam::GraphvizFormatting::mergeSimilarFactors
bool mergeSimilarFactors
Merge multiple factors that have the same connectivity.
Definition GraphvizFormatting.h:41
│ │ │ │ +
gtsam::GraphvizFormatting::scale
double scale
Scale all positions to reduce / increase density.
Definition GraphvizFormatting.h:40
│ │ │ │ +
gtsam::GraphvizFormatting::paperHorizontalAxis
Axis paperHorizontalAxis
The world axis assigned to the horizontal paper axis.
Definition GraphvizFormatting.h:36
│ │ │ │
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,74 +1,110 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -expressionTesting.h │ │ │ │ │ +GraphvizFormatting.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010-2021, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include │ │ │ │ │ -23#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_f_a_c_t_o_r_T_e_s_t_i_n_g_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ -25 │ │ │ │ │ -26namespace _g_t_s_a_m { │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_D_o_t_W_r_i_t_e_r_._h> │ │ │ │ │ +22 │ │ │ │ │ +23namespace _g_t_s_a_m { │ │ │ │ │ +24 │ │ │ │ │ +25class _V_a_l_u_e_s; │ │ │ │ │ +26class Value; │ │ │ │ │ 27 │ │ │ │ │ -28namespace internal { │ │ │ │ │ -29// CPPUnitLite-style test for linearization of an ExpressionFactor │ │ │ │ │ -30template │ │ │ │ │ -31bool testExpressionJacobians(const std::string& name_, │ │ │ │ │ -32 const _g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_<_T_>& expression, const _g_t_s_a_m_:_:_V_a_l_u_e_s& values, │ │ │ │ │ -33 double nd_step, double tolerance) { │ │ │ │ │ -34 // Create factor │ │ │ │ │ -35 size_t size = traits::dimension; │ │ │ │ │ -36 ExpressionFactor f(_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_C_r_e_a_t_e(size), │ │ │ │ │ -37 expression._v_a_l_u_e(values), expression); │ │ │ │ │ -38 return testFactorJacobians(name_, f, values, nd_step, tolerance); │ │ │ │ │ -39} │ │ │ │ │ -40} // namespace internal │ │ │ │ │ -41} // namespace gtsam │ │ │ │ │ -42 │ │ │ │ │ -_4_8#define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, │ │ │ │ │ -numerical_derivative_step, tolerance) \ │ │ │ │ │ -49 { EXPECT(gtsam::internal::testExpressionJacobians(name_, expression, values, │ │ │ │ │ -numerical_derivative_step, tolerance)); } │ │ │ │ │ -_T_e_s_t_a_b_l_e_._h │ │ │ │ │ -Concept check for values that can be used in unit tests. │ │ │ │ │ -_f_a_c_t_o_r_T_e_s_t_i_n_g_._h │ │ │ │ │ -Evaluate derivatives of a nonlinear factor numerically. │ │ │ │ │ +_3_2struct GTSAM_EXPORT _G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g : public _D_o_t_W_r_i_t_e_r { │ │ │ │ │ +_3_4 enum _A_x_i_s { X, Y, Z, NEGX, NEGY, NEGZ }; │ │ │ │ │ +35 │ │ │ │ │ +_3_6 _A_x_i_s _p_a_p_e_r_H_o_r_i_z_o_n_t_a_l_A_x_i_s; │ │ │ │ │ +_3_8 _A_x_i_s _p_a_p_e_r_V_e_r_t_i_c_a_l_A_x_i_s; │ │ │ │ │ +_4_0 double _s_c_a_l_e; │ │ │ │ │ +_4_1 bool _m_e_r_g_e_S_i_m_i_l_a_r_F_a_c_t_o_r_s; │ │ │ │ │ +43 │ │ │ │ │ +_4_6 _G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g() │ │ │ │ │ +47 : paperHorizontalAxis(Y), │ │ │ │ │ +48 paperVerticalAxis(X), │ │ │ │ │ +49 scale(1), │ │ │ │ │ +50 mergeSimilarFactors(false) {} │ │ │ │ │ +51 │ │ │ │ │ +52 // Find bounds │ │ │ │ │ +53 Vector2 findBounds(const _V_a_l_u_e_s& values, const _K_e_y_S_e_t& keys) const; │ │ │ │ │ +54 │ │ │ │ │ +56 boost::optional extractPosition(const _V_a_l_u_e& value) const; │ │ │ │ │ +57 │ │ │ │ │ +59 boost::optional variablePos(const _V_a_l_u_e_s& values, const Vector2& │ │ │ │ │ +min, │ │ │ │ │ +60 _K_e_y key) const; │ │ │ │ │ +61 │ │ │ │ │ +63 boost::optional factorPos(const Vector2& min, size_t i) const; │ │ │ │ │ +64}; │ │ │ │ │ +65 │ │ │ │ │ +66} // namespace gtsam │ │ │ │ │ +_D_o_t_W_r_i_t_e_r_._h │ │ │ │ │ +Graphviz formatter. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_C_r_e_a_t_e │ │ │ │ │ -static shared_ptr Create(size_t dim) │ │ │ │ │ -Create a unit covariance noise model. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:597 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n │ │ │ │ │ -Expression class that supports automatic differentiation. │ │ │ │ │ -DDeeffiinniittiioonn Expression.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_v_a_l_u_e │ │ │ │ │ -T value(const Values &values, boost::optional< std::vector< Matrix > & > │ │ │ │ │ -H=boost::none) const │ │ │ │ │ -Return value and optional derivatives, reverse AD version Notes: this is not │ │ │ │ │ -terribly efficient,... │ │ │ │ │ -DDeeffiinniittiioonn Expression-inl.h:147 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e │ │ │ │ │ +This is the base class for any type to be stored in Values. │ │ │ │ │ +DDeeffiinniittiioonn Value.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r │ │ │ │ │ +DotWriter is a helper class for writing graphviz .dot files. │ │ │ │ │ +DDeeffiinniittiioonn DotWriter.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g │ │ │ │ │ +Formatting options and functions for saving a NonlinearFactorGraph instance in │ │ │ │ │ +GraphViz format. │ │ │ │ │ +DDeeffiinniittiioonn GraphvizFormatting.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g_:_:_A_x_i_s │ │ │ │ │ +Axis │ │ │ │ │ +World axes to be assigned to paper axes. │ │ │ │ │ +DDeeffiinniittiioonn GraphvizFormatting.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g_:_:_p_a_p_e_r_V_e_r_t_i_c_a_l_A_x_i_s │ │ │ │ │ +Axis paperVerticalAxis │ │ │ │ │ +The world axis assigned to the vertical paper axis. │ │ │ │ │ +DDeeffiinniittiioonn GraphvizFormatting.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g_:_:_G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g │ │ │ │ │ +GraphvizFormatting() │ │ │ │ │ +Default constructor sets up robot coordinates. │ │ │ │ │ +DDeeffiinniittiioonn GraphvizFormatting.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g_:_:_m_e_r_g_e_S_i_m_i_l_a_r_F_a_c_t_o_r_s │ │ │ │ │ +bool mergeSimilarFactors │ │ │ │ │ +Merge multiple factors that have the same connectivity. │ │ │ │ │ +DDeeffiinniittiioonn GraphvizFormatting.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g_:_:_s_c_a_l_e │ │ │ │ │ +double scale │ │ │ │ │ +Scale all positions to reduce / increase density. │ │ │ │ │ +DDeeffiinniittiioonn GraphvizFormatting.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g_:_:_p_a_p_e_r_H_o_r_i_z_o_n_t_a_l_A_x_i_s │ │ │ │ │ +Axis paperHorizontalAxis │ │ │ │ │ +The world axis assigned to the horizontal paper axis. │ │ │ │ │ +DDeeffiinniittiioonn GraphvizFormatting.h:36 │ │ │ │ │ _g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +_V_a_l_u_e_s │ │ │ │ │ +In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ +a non-linear function... │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _e_x_p_r_e_s_s_i_o_n_T_e_s_t_i_n_g_._h │ │ │ │ │ + * _G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01157.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/FunctorizedFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Clique.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,62 +94,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ Functions
│ │ │ │ -
FunctorizedFactor.h File Reference
│ │ │ │ +
ISAM2Clique.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Specialized iSAM2 Clique. │ │ │ │ +More...

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::FunctorizedFactor< R, T >
 Factor which evaluates provided unary functor and uses the result to compute error with respect to the provided measurement. More...
 
struct  gtsam::traits< FunctorizedFactor< R, T > >
 traits More...
 
class  gtsam::FunctorizedFactor2< R, T1, T2 >
 Factor which evaluates provided binary functor and uses the result to compute error with respect to the provided measurement. More...
 
struct  gtsam::traits< FunctorizedFactor2< R, T1, T2 > >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

template<typename T , typename R , typename FUNC >
FunctorizedFactor< R, T > gtsam::MakeFunctorizedFactor (Key key, const R &z, const SharedNoiseModel &model, const FUNC func)
 Helper function to create a functorized factor.
 
template<typename T1 , typename T2 , typename R , typename FUNC >
FunctorizedFactor2< R, T1, T2 > gtsam::MakeFunctorizedFactor2 (Key key1, Key key2, const R &z, const SharedNoiseModel &model, const FUNC func)
 Helper function to create a functorized factor.
 
size_t gtsam::optimizeWildfire (const ISAM2Clique::shared_ptr &root, double threshold, const KeySet &replaced, VectorValues *delta)
 Optimize the BayesTree, starting from the root.
 
│ │ │ │ +size_t gtsam::optimizeWildfireNonRecursive (const ISAM2Clique::shared_ptr &root, double threshold, const KeySet &keys, VectorValues *delta)
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
May 31, 2020
│ │ │ │ -
Author
Varun Agrawal
│ │ │ │ +

Specialized iSAM2 Clique.

│ │ │ │ +
Author
Michael Kaess, Richard Roberts, Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,51 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -FunctorizedFactor.h File Reference │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_<_ _R_,_ _T_ _> │ │ │ │ │ -  _F_a_c_t_o_r which evaluates provided unary functor and uses the result to │ │ │ │ │ - compute error with respect to the provided measurement. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_<_ _R_,_ _T_ _>_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2_<_ _R_,_ _T_1_,_ _T_2_ _> │ │ │ │ │ -  _F_a_c_t_o_r which evaluates provided binary functor and uses the result to │ │ │ │ │ - compute error with respect to the provided measurement. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2_<_ _R_,_ _T_1_,_ _T_2_ _>_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +ISAM2Clique.cpp File Reference │ │ │ │ │ +Specialized iSAM2 Clique. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ - _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r< R, T >  _g_t_s_a_m_:_:_M_a_k_e_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r (_K_e_y key, const R │ │ │ │ │ - &z, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, const FUNC │ │ │ │ │ - func) │ │ │ │ │ -  Helper function to create a functorized │ │ │ │ │ - factor. │ │ │ │ │ +size_t  _g_t_s_a_m_:_:_o_p_t_i_m_i_z_e_W_i_l_d_f_i_r_e (const ISAM2Clique::shared_ptr &root, double │ │ │ │ │ + threshold, const _K_e_y_S_e_t &replaced, _V_e_c_t_o_r_V_a_l_u_e_s *delta) │ │ │ │ │ +  Optimize the _B_a_y_e_s_T_r_e_e, starting from the root. │ │ │ │ │   │ │ │ │ │ -template │ │ │ │ │ -_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2< R, T1, T2 >  _g_t_s_a_m_:_:_M_a_k_e_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2 (_K_e_y key1, _K_e_y │ │ │ │ │ - key2, const R &z, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ - &model, const FUNC func) │ │ │ │ │ -  Helper function to create a functorized │ │ │ │ │ - factor. │ │ │ │ │ +size_t  ggttssaamm::::ooppttiimmiizzeeWWiillddffiirreeNNoonnRReeccuurrssiivvee (const ISAM2Clique::shared_ptr │ │ │ │ │ + &root, double threshold, const _K_e_y_S_e_t &keys, _V_e_c_t_o_r_V_a_l_u_e_s *delta) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - May 31, 2020 │ │ │ │ │ +Specialized iSAM2 Clique. │ │ │ │ │ Author │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + Michael Kaess, Richard Roberts, Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_._h │ │ │ │ │ + * _I_S_A_M_2_C_l_i_q_u_e_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01157.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,8 +1,3 @@ │ │ │ │ │ var a01157 = [ │ │ │ │ │ - ["gtsam::FunctorizedFactor< R, T >", "a04336.html", "a04336"], │ │ │ │ │ - ["gtsam::traits< FunctorizedFactor< R, T > >", "a04340.html", null], │ │ │ │ │ - ["gtsam::FunctorizedFactor2< R, T1, T2 >", "a04344.html", "a04344"], │ │ │ │ │ - ["gtsam::traits< FunctorizedFactor2< R, T1, T2 > >", "a04348.html", null], │ │ │ │ │ - ["MakeFunctorizedFactor", "a01157.html#a9d2a0b16c9f78f20fc78fa39c7426242", null], │ │ │ │ │ - ["MakeFunctorizedFactor2", "a01157.html#a313f0e290f370cff0b7e25024d1b8c6d", null] │ │ │ │ │ + ["optimizeWildfire", "a01157.html#a2b0857edd76f8d63eeee0ce9944e28d4", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01160.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,58 +94,36 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
NonlinearConjugateGradientOptimizer.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
NonlinearConjugateGradientOptimizer.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │

Simple non-linear optimizer that solves using non-preconditioned CG. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::NonlinearConjugateGradientOptimizer
 An implementation of the nonlinear CG method using the template below. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -template<class S , class V , class W >
double gtsam::lineSearch (const S &system, const V currentValues, const W &gradient)
 Implement the golden-section line search algorithm.
 
template<class S , class V >
boost::tuple< V, int > gtsam::nonlinearConjugateGradient (const S &system, const V &initial, const NonlinearOptimizerParams &params, const bool singleIteration, const bool gradientDescent=false)
 Implement the nonlinear conjugate gradient method using the Polak-Ribiere formula suggested in http://en.wikipedia.org/wiki/Nonlinear_conjugate_gradient_method.
 
│ │ │ │

Detailed Description

│ │ │ │

Simple non-linear optimizer that solves using non-preconditioned CG.

│ │ │ │
Author
Yong-Dian Jian
│ │ │ │ -
Date
June 11, 2012
│ │ │ │ +
Date
Jun 11, 2012
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,44 +1,22 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -NonlinearConjugateGradientOptimizer.h File Reference │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +NonlinearConjugateGradientOptimizer.cpp File Reference │ │ │ │ │ Simple non-linear optimizer that solves using nnoonn--pprreeccoonnddiittiioonneedd CG. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_O_p_t_i_m_i_z_e_r │ │ │ │ │ -  An implementation of the nonlinear CG method using the template below. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ - double  ggttssaamm::::lliinneeSSeeaarrcchh (const S &system, const V │ │ │ │ │ - currentValues, const W &gradient) │ │ │ │ │ -  Implement the golden-section line search algorithm. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -boost::tuple< V, int >  _g_t_s_a_m_:_:_n_o_n_l_i_n_e_a_r_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t (const S &system, │ │ │ │ │ - const V &initial, const _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s │ │ │ │ │ - ¶ms, const bool singleIteration, const bool │ │ │ │ │ - gradientDescent=false) │ │ │ │ │ - Implement the nonlinear conjugate gradient method using │ │ │ │ │ -  the Polak-Ribiere formula suggested in _h_t_t_p_:_/_/ │ │ │ │ │ - _e_n_._w_i_k_i_p_e_d_i_a_._o_r_g_/_w_i_k_i_/ │ │ │ │ │ - _N_o_n_l_i_n_e_a_r___c_o_n_j_u_g_a_t_e___g_r_a_d_i_e_n_t___m_e_t_h_o_d. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Simple non-linear optimizer that solves using nnoonn--pprreeccoonnddiittiioonneedd CG. │ │ │ │ │ Author │ │ │ │ │ Yong-Dian Jian │ │ │ │ │ Date │ │ │ │ │ - June 11, 2012 │ │ │ │ │ + Jun 11, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _N_o_n_l_i_n_e_a_r_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ + * _N_o_n_l_i_n_e_a_r_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_O_p_t_i_m_i_z_e_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01163.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ExtendedKalmanFilter-inl.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ExtendedKalmanFilter.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,24 +94,31 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
ExtendedKalmanFilter-inl.h File Reference
│ │ │ │ +
ExtendedKalmanFilter.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │

Class to perform generic Kalman Filtering using nonlinear factor graphs. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::ExtendedKalmanFilter< VALUE >
 This is a generic Extended Kalman Filter class implemented using nonlinear factors. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ @@ -120,13 +127,13 @@ │ │ │ │
│ │ │ │ Chris Beall
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -ExtendedKalmanFilter-inl.h File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +ExtendedKalmanFilter.h File Reference │ │ │ │ │ Class to perform generic Kalman Filtering using nonlinear factor graphs. │ │ │ │ │ _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_<_ _V_A_L_U_E_ _> │ │ │ │ │ +  This is a generic Extended Kalman Filter class implemented using │ │ │ │ │ + nonlinear factors. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Class to perform generic Kalman Filtering using nonlinear factor graphs. │ │ │ │ │ Author │ │ │ │ │ Stephen Williams │ │ │ │ │ Chris Beall │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_-_i_n_l_._h │ │ │ │ │ + * _E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01163_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ExtendedKalmanFilter-inl.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ExtendedKalmanFilter.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,164 +98,110 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ExtendedKalmanFilter-inl.h
│ │ │ │ +
ExtendedKalmanFilter.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/nonlinear/ExtendedKalmanFilter.h>
│ │ │ │ -
22#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
23#include <gtsam/linear/GaussianBayesNet.h>
│ │ │ │ -
24#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ -
25
│ │ │ │ -
26namespace gtsam {
│ │ │ │ -
27
│ │ │ │ -
28 /* ************************************************************************* */
│ │ │ │ -
29 template<class VALUE>
│ │ │ │ -
30 typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::solve_(
│ │ │ │ -
31 const GaussianFactorGraph& linearFactorGraph,
│ │ │ │ -
32 const Values& linearizationPoint, Key lastKey,
│ │ │ │ -
33 JacobianFactor::shared_ptr* newPrior)
│ │ │ │ -
34 {
│ │ │ │ -
35 // Compute the marginal on the last key
│ │ │ │ -
36 // Solve the linear factor graph, converting it into a linear Bayes Network
│ │ │ │ -
37 // P(x0,x1) = P(x0|x1)*P(x1)
│ │ │ │ -
38 Ordering lastKeyAsOrdering;
│ │ │ │ -
39 lastKeyAsOrdering += lastKey;
│ │ │ │ -
40 const GaussianConditional::shared_ptr marginal =
│ │ │ │ -
41 linearFactorGraph.marginalMultifrontalBayesNet(lastKeyAsOrdering)->front();
│ │ │ │ -
42
│ │ │ │ -
43 // Extract the current estimate of x1,P1
│ │ │ │ -
44 VectorValues result = marginal->solve(VectorValues());
│ │ │ │ -
45 const T& current = linearizationPoint.at<T>(lastKey);
│ │ │ │ -
46 T x = traits<T>::Retract(current, result[lastKey]);
│ │ │ │ -
47
│ │ │ │ -
48 // Create a Jacobian Factor from the root node of the produced Bayes Net.
│ │ │ │ -
49 // This will act as a prior for the next iteration.
│ │ │ │ -
50 // The linearization point of this prior must be moved to the new estimate of x,
│ │ │ │ -
51 // and the key/index needs to be reset to 0, the first key in the next iteration.
│ │ │ │ -
52 assert(marginal->nrFrontals() == 1);
│ │ │ │ -
53 assert(marginal->nrParents() == 0);
│ │ │ │ -
54 *newPrior = boost::make_shared<JacobianFactor>(
│ │ │ │ -
55 marginal->keys().front(),
│ │ │ │ -
56 marginal->getA(marginal->begin()),
│ │ │ │ -
57 marginal->getb() - marginal->getA(marginal->begin()) * result[lastKey],
│ │ │ │ -
58 marginal->get_model());
│ │ │ │ +
19// \callgraph
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ +
23#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
24
│ │ │ │ +
25namespace gtsam {
│ │ │ │ +
26
│ │ │ │ +
44template <class VALUE>
│ │ │ │ +
│ │ │ │ +
45class ExtendedKalmanFilter {
│ │ │ │ +
46 // Check that VALUE type is a testable Manifold
│ │ │ │ +
47 BOOST_CONCEPT_ASSERT((IsTestable<VALUE>));
│ │ │ │ +
48 BOOST_CONCEPT_ASSERT((IsManifold<VALUE>));
│ │ │ │ +
49
│ │ │ │ +
50 public:
│ │ │ │ +
51 typedef boost::shared_ptr<ExtendedKalmanFilter<VALUE> > shared_ptr;
│ │ │ │ +
52 typedef VALUE T;
│ │ │ │ +
53
│ │ │ │ +
54#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ +
55 //@deprecated: any NoiseModelFactor will do, as long as they have the right keys
│ │ │ │ +
56 typedef NoiseModelFactorN<VALUE, VALUE> MotionFactor;
│ │ │ │ +
57 typedef NoiseModelFactorN<VALUE> MeasurementFactor;
│ │ │ │ +
58#endif
│ │ │ │
59
│ │ │ │ -
60 return x;
│ │ │ │ -
61 }
│ │ │ │ -
62
│ │ │ │ -
63 /* ************************************************************************* */
│ │ │ │ -
64 template <class VALUE>
│ │ │ │ -
65 ExtendedKalmanFilter<VALUE>::ExtendedKalmanFilter(
│ │ │ │ -
66 Key key_initial, T x_initial, noiseModel::Gaussian::shared_ptr P_initial)
│ │ │ │ -
67 : x_(x_initial) // Set the initial linearization point
│ │ │ │ -
68 {
│ │ │ │ -
69 // Create a Jacobian Prior Factor directly P_initial.
│ │ │ │ -
70 // Since x0 is set to the provided mean, the b vector in the prior will be zero
│ │ │ │ -
71 // TODO(Frank): is there a reason why noiseModel is not simply P_initial?
│ │ │ │ -
72 int n = traits<T>::GetDimension(x_initial);
│ │ │ │ -
73 priorFactor_ = JacobianFactor::shared_ptr(
│ │ │ │ -
74 new JacobianFactor(key_initial, P_initial->R(), Vector::Zero(n),
│ │ │ │ -
75 noiseModel::Unit::Create(n)));
│ │ │ │ -
76 }
│ │ │ │ -
77
│ │ │ │ -
78 /* ************************************************************************* */
│ │ │ │ -
79 template<class VALUE>
│ │ │ │ -
│ │ │ │ -
80 typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::predict(
│ │ │ │ -
81 const NoiseModelFactor& motionFactor) {
│ │ │ │ -
82 const auto keys = motionFactor.keys();
│ │ │ │ +
60 protected:
│ │ │ │ +
61 T x_; // linearization point
│ │ │ │ +
62 JacobianFactor::shared_ptr priorFactor_; // Gaussian density on x_
│ │ │ │ +
63
│ │ │ │ +
64 static T solve_(const GaussianFactorGraph& linearFactorGraph, const Values& linearizationPoints,
│ │ │ │ +
65 Key x, JacobianFactor::shared_ptr* newPrior);
│ │ │ │ +
66
│ │ │ │ +
67 public:
│ │ │ │ +
70
│ │ │ │ +
71 ExtendedKalmanFilter(Key key_initial, T x_initial, noiseModel::Gaussian::shared_ptr P_initial);
│ │ │ │ +
72
│ │ │ │ +
76
│ │ │ │ +
│ │ │ │ +
78 void print(const std::string& s = "") const {
│ │ │ │ +
79 std::cout << s << "\n";
│ │ │ │ +
80 x_.print(s + "x");
│ │ │ │ +
81 priorFactor_->print(s + "density");
│ │ │ │ +
82 }
│ │ │ │ +
│ │ │ │
83
│ │ │ │ -
84 // Create a Gaussian Factor Graph
│ │ │ │ -
85 GaussianFactorGraph linearFactorGraph;
│ │ │ │ -
86
│ │ │ │ -
87 // Add in previous posterior as prior on the first state
│ │ │ │ -
88 linearFactorGraph.push_back(priorFactor_);
│ │ │ │ -
89
│ │ │ │ -
90 // Linearize motion model and add it to the Kalman Filter graph
│ │ │ │ -
91 Values linearizationPoint;
│ │ │ │ -
92 linearizationPoint.insert(keys[0], x_);
│ │ │ │ -
93 linearizationPoint.insert(keys[1], x_); // TODO should this really be x_ ?
│ │ │ │ -
94 linearFactorGraph.push_back(motionFactor.linearize(linearizationPoint));
│ │ │ │ -
95
│ │ │ │ -
96 // Solve the factor graph and update the current state estimate
│ │ │ │ -
97 // and the posterior for the next iteration.
│ │ │ │ -
98 x_ = solve_(linearFactorGraph, linearizationPoint, keys[1], &priorFactor_);
│ │ │ │ -
99
│ │ │ │ -
100 return x_;
│ │ │ │ -
101 }
│ │ │ │ -
│ │ │ │ -
102
│ │ │ │ -
103 /* ************************************************************************* */
│ │ │ │ -
104 template<class VALUE>
│ │ │ │ -
│ │ │ │ -
105 typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::update(
│ │ │ │ -
106 const NoiseModelFactor& measurementFactor) {
│ │ │ │ -
107 const auto keys = measurementFactor.keys();
│ │ │ │ +
87
│ │ │ │ +
93 T predict(const NoiseModelFactor& motionFactor);
│ │ │ │ +
94
│ │ │ │ +
99 T update(const NoiseModelFactor& measurementFactor);
│ │ │ │ +
100
│ │ │ │ +
│ │ │ │ +
102 const JacobianFactor::shared_ptr Density() const {
│ │ │ │ +
103 return priorFactor_;
│ │ │ │ +
104 }
│ │ │ │ +
│ │ │ │ +
105
│ │ │ │ +
107};
│ │ │ │ +
│ │ │ │
108
│ │ │ │ -
109 // Create a Gaussian Factor Graph
│ │ │ │ -
110 GaussianFactorGraph linearFactorGraph;
│ │ │ │ -
111
│ │ │ │ -
112 // Add in the prior on the first state
│ │ │ │ -
113 linearFactorGraph.push_back(priorFactor_);
│ │ │ │ -
114
│ │ │ │ -
115 // Linearize measurement factor and add it to the Kalman Filter graph
│ │ │ │ -
116 Values linearizationPoint;
│ │ │ │ -
117 linearizationPoint.insert(keys[0], x_);
│ │ │ │ -
118 linearFactorGraph.push_back(measurementFactor.linearize(linearizationPoint));
│ │ │ │ -
119
│ │ │ │ -
120 // Solve the factor graph and update the current state estimate
│ │ │ │ -
121 // and the prior factor for the next iteration
│ │ │ │ -
122 x_ = solve_(linearFactorGraph, linearizationPoint, keys[0], &priorFactor_);
│ │ │ │ -
123
│ │ │ │ -
124 return x_;
│ │ │ │ -
125 }
│ │ │ │ -
│ │ │ │ -
126
│ │ │ │ -
127} // namespace gtsam
│ │ │ │ -
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
│ │ │ │ -
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ -
ExtendedKalmanFilter.h
Class to perform generic Kalman Filtering using nonlinear factor graphs.
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
109} // namespace
│ │ │ │ +
110
│ │ │ │ +
111#include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │ +
ExtendedKalmanFilter-inl.h
Class to perform generic Kalman Filtering using nonlinear factor graphs.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition FactorGraph.h:186
│ │ │ │ -
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
│ │ │ │ -
gtsam::GaussianConditional::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianConditional.h:46
│ │ │ │ +
gtsam::IsTestable
A testable concept check that should be placed in applicable unit tests and in generic algorithms.
Definition Testable.h:58
│ │ │ │
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │
gtsam::JacobianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition JacobianFactor.h:96
│ │ │ │
gtsam::ExtendedKalmanFilter
This is a generic Extended Kalman Filter class implemented using nonlinear factors.
Definition ExtendedKalmanFilter.h:45
│ │ │ │ +
gtsam::ExtendedKalmanFilter::update
T update(const NoiseModelFactor &measurementFactor)
Calculate posterior density P(x_) ~ L(z|x) P(x) The likelihood L(z|x) should be given as a unary fact...
Definition ExtendedKalmanFilter-inl.h:105
│ │ │ │ +
gtsam::ExtendedKalmanFilter::Density
const JacobianFactor::shared_ptr Density() const
Return current predictive (if called after predict)/posterior (if called after update)
Definition ExtendedKalmanFilter.h:102
│ │ │ │ +
gtsam::ExtendedKalmanFilter::predict
T predict(const NoiseModelFactor &motionFactor)
Calculate predictive density The motion model should be given as a factor with key1 for and key2 fo...
Definition ExtendedKalmanFilter-inl.h:80
│ │ │ │ +
gtsam::ExtendedKalmanFilter::print
void print(const std::string &s="") const
print
Definition ExtendedKalmanFilter.h:78
│ │ │ │
gtsam::NoiseModelFactor
A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
Definition NonlinearFactor.h:174
│ │ │ │ -
gtsam::NoiseModelFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override
Linearize a non-linearFactorN to get a GaussianFactor, Hence .
Definition NonlinearFactor.cpp:152
│ │ │ │ +
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
gtsam::Values::insert
void insert(Key j, const Value &val)
Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present.
Definition Values.cpp:157
│ │ │ │ -
JacobianFactor
In Gaussian factors, the error function returns either the negative log-likelihood,...
│ │ │ │ -
VectorValues
The Factor::error simply extracts the.
│ │ │ │ -
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,205 +1,141 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ExtendedKalmanFilter-inl.h │ │ │ │ │ +ExtendedKalmanFilter.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -25 │ │ │ │ │ -26namespace _g_t_s_a_m { │ │ │ │ │ -27 │ │ │ │ │ -28 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -29 template │ │ │ │ │ -30 typename ExtendedKalmanFilter::T ExtendedKalmanFilter::solve_( │ │ │ │ │ -31 const GaussianFactorGraph& linearFactorGraph, │ │ │ │ │ -32 const _V_a_l_u_e_s& linearizationPoint, _K_e_y lastKey, │ │ │ │ │ -33 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r* newPrior) │ │ │ │ │ -34 { │ │ │ │ │ -35 // Compute the marginal on the last key │ │ │ │ │ -36 // Solve the linear factor graph, converting it into a linear Bayes Network │ │ │ │ │ -37 // P(x0,x1) = P(x0|x1)*P(x1) │ │ │ │ │ -38 Ordering lastKeyAsOrdering; │ │ │ │ │ -39 lastKeyAsOrdering += lastKey; │ │ │ │ │ -40 const _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r marginal = │ │ │ │ │ -41 linearFactorGraph.marginalMultifrontalBayesNet(lastKeyAsOrdering)->front(); │ │ │ │ │ -42 │ │ │ │ │ -43 // Extract the current estimate of x1,P1 │ │ │ │ │ -44 _V_e_c_t_o_r_V_a_l_u_e_s result = marginal->solve(_V_e_c_t_o_r_V_a_l_u_e_s()); │ │ │ │ │ -45 const T& current = linearizationPoint.at(lastKey); │ │ │ │ │ -46 T x = traits::Retract(current, result[lastKey]); │ │ │ │ │ -47 │ │ │ │ │ -48 // Create a Jacobian Factor from the root node of the produced Bayes Net. │ │ │ │ │ -49 // This will act as a prior for the next iteration. │ │ │ │ │ -50 // The linearization point of this prior must be moved to the new estimate │ │ │ │ │ -of x, │ │ │ │ │ -51 // and the key/index needs to be reset to 0, the first key in the next │ │ │ │ │ -iteration. │ │ │ │ │ -52 assert(marginal->nrFrontals() == 1); │ │ │ │ │ -53 assert(marginal->nrParents() == 0); │ │ │ │ │ -54 *newPrior = boost::make_shared( │ │ │ │ │ -55 marginal->keys().front(), │ │ │ │ │ -56 marginal->getA(marginal->begin()), │ │ │ │ │ -57 marginal->getb() - marginal->getA(marginal->begin()) * result[lastKey], │ │ │ │ │ -58 marginal->get_model()); │ │ │ │ │ +19// \callgraph │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +24 │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ +26 │ │ │ │ │ +44template │ │ │ │ │ +_4_5class _E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r { │ │ │ │ │ +46 // Check that VALUE type is a testable Manifold │ │ │ │ │ +47 BOOST_CONCEPT_ASSERT((_I_s_T_e_s_t_a_b_l_e_<_V_A_L_U_E_>)); │ │ │ │ │ +48 BOOST_CONCEPT_ASSERT((IsManifold)); │ │ │ │ │ +49 │ │ │ │ │ +50 public: │ │ │ │ │ +51 typedef boost::shared_ptr > shared_ptr; │ │ │ │ │ +52 typedef VALUE T; │ │ │ │ │ +53 │ │ │ │ │ +54#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ +55 //@deprecated: any NoiseModelFactor will do, as long as they have the right │ │ │ │ │ +keys │ │ │ │ │ +56 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_V_A_L_U_E_,_ _V_A_L_U_E_> MotionFactor; │ │ │ │ │ +57 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_V_A_L_U_E_> MeasurementFactor; │ │ │ │ │ +58#endif │ │ │ │ │ 59 │ │ │ │ │ -60 return x; │ │ │ │ │ -61 } │ │ │ │ │ -62 │ │ │ │ │ -63 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -64 template │ │ │ │ │ -65 ExtendedKalmanFilter::ExtendedKalmanFilter( │ │ │ │ │ -66 _K_e_y key_initial, T x_initial, noiseModel::Gaussian::shared_ptr P_initial) │ │ │ │ │ -67 : x_(x_initial) // Set the initial linearization point │ │ │ │ │ -68 { │ │ │ │ │ -69 // Create a Jacobian Prior Factor directly P_initial. │ │ │ │ │ -70 // Since x0 is set to the provided mean, the b vector in the prior will be │ │ │ │ │ -zero │ │ │ │ │ -71 // TODO(Frank): is there a reason why noiseModel is not simply P_initial? │ │ │ │ │ -72 int n = traits::GetDimension(x_initial); │ │ │ │ │ -73 priorFactor_ = JacobianFactor::shared_ptr( │ │ │ │ │ -74 new _J_a_c_o_b_i_a_n_F_a_c_t_o_r(key_initial, P_initial->R(), Vector::Zero(n), │ │ │ │ │ -75 noiseModel::Unit::Create(n))); │ │ │ │ │ -76 } │ │ │ │ │ -77 │ │ │ │ │ -78 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -79 template │ │ │ │ │ -_8_0 typename ExtendedKalmanFilter::T _E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_<_V_A_L_U_E_>_:_:_p_r_e_d_i_c_t │ │ │ │ │ -( │ │ │ │ │ -81 const _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r& motionFactor) { │ │ │ │ │ -82 const auto keys = motionFactor._k_e_y_s(); │ │ │ │ │ +60 protected: │ │ │ │ │ +61 T x_; // linearization point │ │ │ │ │ +62 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r priorFactor_; // Gaussian density on x_ │ │ │ │ │ +63 │ │ │ │ │ +64 static T solve_(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& linearFactorGraph, const _V_a_l_u_e_s& │ │ │ │ │ +linearizationPoints, │ │ │ │ │ +65 _K_e_y x, _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r* newPrior); │ │ │ │ │ +66 │ │ │ │ │ +67 public: │ │ │ │ │ +70 │ │ │ │ │ +71 _E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r(_K_e_y key_initial, T x_initial, noiseModel::Gaussian:: │ │ │ │ │ +shared_ptr P_initial); │ │ │ │ │ +72 │ │ │ │ │ +76 │ │ │ │ │ +_7_8 void _p_r_i_n_t(const std::string& s = "") const { │ │ │ │ │ +79 std::cout << s << "\n"; │ │ │ │ │ +80 x_.print(s + "x"); │ │ │ │ │ +81 priorFactor_->print(s + "density"); │ │ │ │ │ +82 } │ │ │ │ │ 83 │ │ │ │ │ -84 // Create a Gaussian Factor Graph │ │ │ │ │ -85 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h linearFactorGraph; │ │ │ │ │ -86 │ │ │ │ │ -87 // Add in previous posterior as prior on the first state │ │ │ │ │ -88 linearFactorGraph._p_u_s_h___b_a_c_k(priorFactor_); │ │ │ │ │ -89 │ │ │ │ │ -90 // Linearize motion model and add it to the Kalman Filter graph │ │ │ │ │ -91 _V_a_l_u_e_s linearizationPoint; │ │ │ │ │ -92 linearizationPoint._i_n_s_e_r_t(keys[0], x_); │ │ │ │ │ -93 linearizationPoint._i_n_s_e_r_t(keys[1], x_); // TODO should this really be x_ ? │ │ │ │ │ -94 linearFactorGraph._p_u_s_h___b_a_c_k(motionFactor._l_i_n_e_a_r_i_z_e(linearizationPoint)); │ │ │ │ │ -95 │ │ │ │ │ -96 // Solve the factor graph and update the current state estimate │ │ │ │ │ -97 // and the posterior for the next iteration. │ │ │ │ │ -98 x_ = solve_(linearFactorGraph, linearizationPoint, keys[1], &priorFactor_); │ │ │ │ │ -99 │ │ │ │ │ -100 return x_; │ │ │ │ │ -101 } │ │ │ │ │ -102 │ │ │ │ │ -103 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -104 template │ │ │ │ │ -_1_0_5 typename ExtendedKalmanFilter::T _E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_<_V_A_L_U_E_>_:_:_u_p_d_a_t_e │ │ │ │ │ -( │ │ │ │ │ -106 const _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r& measurementFactor) { │ │ │ │ │ -107 const auto keys = measurementFactor._k_e_y_s(); │ │ │ │ │ +87 │ │ │ │ │ +93 T _p_r_e_d_i_c_t(const _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r& motionFactor); │ │ │ │ │ +94 │ │ │ │ │ +99 T _u_p_d_a_t_e(const _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r& measurementFactor); │ │ │ │ │ +100 │ │ │ │ │ +_1_0_2 const _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r _D_e_n_s_i_t_y() const { │ │ │ │ │ +103 return priorFactor_; │ │ │ │ │ +104 } │ │ │ │ │ +105 │ │ │ │ │ +107}; │ │ │ │ │ 108 │ │ │ │ │ -109 // Create a Gaussian Factor Graph │ │ │ │ │ -110 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h linearFactorGraph; │ │ │ │ │ -111 │ │ │ │ │ -112 // Add in the prior on the first state │ │ │ │ │ -113 linearFactorGraph._p_u_s_h___b_a_c_k(priorFactor_); │ │ │ │ │ -114 │ │ │ │ │ -115 // Linearize measurement factor and add it to the Kalman Filter graph │ │ │ │ │ -116 _V_a_l_u_e_s linearizationPoint; │ │ │ │ │ -117 linearizationPoint._i_n_s_e_r_t(keys[0], x_); │ │ │ │ │ -118 linearFactorGraph._p_u_s_h___b_a_c_k(measurementFactor._l_i_n_e_a_r_i_z_e │ │ │ │ │ -(linearizationPoint)); │ │ │ │ │ -119 │ │ │ │ │ -120 // Solve the factor graph and update the current state estimate │ │ │ │ │ -121 // and the prior factor for the next iteration │ │ │ │ │ -122 x_ = solve_(linearFactorGraph, linearizationPoint, keys[0], &priorFactor_); │ │ │ │ │ -123 │ │ │ │ │ -124 return x_; │ │ │ │ │ -125 } │ │ │ │ │ -126 │ │ │ │ │ -127} // namespace gtsam │ │ │ │ │ -_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h │ │ │ │ │ -Chordal Bayes Net, the result of eliminating a factor graph. │ │ │ │ │ -_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ -_E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_._h │ │ │ │ │ -Class to perform generic Kalman Filtering using nonlinear factor graphs. │ │ │ │ │ +109} // namespace │ │ │ │ │ +110 │ │ │ │ │ +111#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_-_i_n_l_._h> │ │ │ │ │ _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ Non-linear factor base classes. │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Factor Graph consisting of non-linear factors. │ │ │ │ │ +_E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_-_i_n_l_._h │ │ │ │ │ +Class to perform generic Kalman Filtering using nonlinear factor graphs. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_p_u_s_h___b_a_c_k │ │ │ │ │ -IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor) │ │ │ │ │ -Add a factor directly using a shared_ptr. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:186 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s │ │ │ │ │ -const KeyVector & keys() const │ │ │ │ │ -Access the factor's involved variable keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:140 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn GaussianConditional.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_I_s_T_e_s_t_a_b_l_e │ │ │ │ │ +A testable concept check that should be placed in applicable unit tests and in │ │ │ │ │ +generic algorithms. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:58 │ │ │ │ │ _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ _g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ boost::shared_ptr< This > shared_ptr │ │ │ │ │ shared_ptr to this class │ │ │ │ │ DDeeffiinniittiioonn JacobianFactor.h:96 │ │ │ │ │ _g_t_s_a_m_:_:_E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r │ │ │ │ │ This is a generic Extended Kalman Filter class implemented using nonlinear │ │ │ │ │ factors. │ │ │ │ │ DDeeffiinniittiioonn ExtendedKalmanFilter.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_:_:_u_p_d_a_t_e │ │ │ │ │ +T update(const NoiseModelFactor &measurementFactor) │ │ │ │ │ +Calculate posterior density P(x_) ~ L(z|x) P(x) The likelihood L(z|x) should be │ │ │ │ │ +given as a unary fact... │ │ │ │ │ +DDeeffiinniittiioonn ExtendedKalmanFilter-inl.h:105 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_:_:_D_e_n_s_i_t_y │ │ │ │ │ +const JacobianFactor::shared_ptr Density() const │ │ │ │ │ +Return current predictive (if called after predict)/posterior (if called after │ │ │ │ │ +update) │ │ │ │ │ +DDeeffiinniittiioonn ExtendedKalmanFilter.h:102 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_:_:_p_r_e_d_i_c_t │ │ │ │ │ +T predict(const NoiseModelFactor &motionFactor) │ │ │ │ │ +Calculate predictive density The motion model should be given as a factor with │ │ │ │ │ +key1 for and key2 fo... │ │ │ │ │ +DDeeffiinniittiioonn ExtendedKalmanFilter-inl.h:80 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="") const │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn ExtendedKalmanFilter.h:78 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r │ │ │ │ │ A nonlinear sum-of-squares factor with a zero-mean noise model implementing the │ │ │ │ │ density Templated on... │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactor.h:174 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e │ │ │ │ │ -boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override │ │ │ │ │ -Linearize a non-linearFactorN to get a GaussianFactor, Hence . │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.cpp:152 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ +A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ +variables. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ _g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_i_n_s_e_r_t │ │ │ │ │ -void insert(Key j, const Value &val) │ │ │ │ │ -Add a variable with the given j, throws KeyAlreadyExists if j is already │ │ │ │ │ -present. │ │ │ │ │ -DDeeffiinniittiioonn Values.cpp:157 │ │ │ │ │ -_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ -In Gaussian factors, the error function returns either the negative log- │ │ │ │ │ -likelihood,... │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -The Factor::error simply extracts the. │ │ │ │ │ -_V_a_l_u_e_s │ │ │ │ │ -In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ -a non-linear function... │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_-_i_n_l_._h │ │ │ │ │ + * _E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01166.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/GncOptimizer.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/WhiteNoiseFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,49 +95,54 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
GncOptimizer.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Variables
│ │ │ │ +
WhiteNoiseFactor.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

The GncOptimizer class. │ │ │ │ +

Binary white noise factor. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::GncOptimizer< GncParameters >
class  gtsam::WhiteNoiseFactor
 Binary factor to estimate parameters of zero-mean Gaussian white noise. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Variables

│ │ │ │ +const double gtsam::logSqrt2PI = log(std::sqrt(2.0 * M_PI))
 constant needed below
 
│ │ │ │

Detailed Description

│ │ │ │ -

The GncOptimizer class.

│ │ │ │ -
Author
Jingnan Shi
│ │ │ │ -
│ │ │ │ -Luca Carlone
│ │ │ │ +

Binary white noise factor.

│ │ │ │ +
Author
Chris Beall
│ │ │ │
│ │ │ │ -Frank Dellaert
│ │ │ │ -

Implementation of the paper: Yang, Antonante, Tzoumas, Carlone, "Graduated Non-Convexity for Robust Spatial Perception: │ │ │ │ -From Non-Minimal Solvers to Global Outlier Rejection", ICRA/RAL, 2020. (arxiv version: https://arxiv.org/pdf/1909.08605.pdf)

│ │ │ │ -

See also: Antonante, Tzoumas, Yang, Carlone, "Outlier-Robust Estimation: Hardness, Minimally-Tuned Algorithms, and Applications", arxiv: https://arxiv.org/pdf/2007.15109.pdf, 2020.

│ │ │ │ +Frank Dellaert
│ │ │ │ +
Date
September 2011
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,33 +1,33 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -GncOptimizer.h File Reference │ │ │ │ │ -The GncOptimizer class. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _V_a_r_i_a_b_l_e_s │ │ │ │ │ +WhiteNoiseFactor.h File Reference │ │ │ │ │ +Binary white noise factor. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_<_ _G_n_c_P_a_r_a_m_e_t_e_r_s_ _> │ │ │ │ │ +class   _g_t_s_a_m_:_:_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r │ │ │ │ │ +  Binary factor to estimate parameters of zero-mean Gaussian white noise. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +VVaarriiaabblleess │ │ │ │ │ +const double  ggttssaamm::::llooggSSqqrrtt22PPII = log(std::sqrt(2.0 * M_PI)) │ │ │ │ │ +  constant needed below │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -The GncOptimizer class. │ │ │ │ │ +Binary white noise factor. │ │ │ │ │ Author │ │ │ │ │ - Jingnan Shi │ │ │ │ │ - Luca Carlone │ │ │ │ │ + Chris Beall │ │ │ │ │ Frank Dellaert │ │ │ │ │ -Implementation of the paper: Yang, Antonante, Tzoumas, Carlone, "Graduated Non- │ │ │ │ │ -Convexity for Robust Spatial Perception: From Non-Minimal Solvers to Global │ │ │ │ │ -Outlier Rejection", ICRA/RAL, 2020. (arxiv version: _h_t_t_p_s_:_/_/_a_r_x_i_v_._o_r_g_/_p_d_f_/ │ │ │ │ │ -_1_9_0_9_._0_8_6_0_5_._p_d_f) │ │ │ │ │ -See also: Antonante, Tzoumas, Yang, Carlone, "Outlier-Robust Estimation: │ │ │ │ │ -Hardness, Minimally-Tuned Algorithms, and Applications", arxiv: _h_t_t_p_s_:_/_/ │ │ │ │ │ -_a_r_x_i_v_._o_r_g_/_p_d_f_/_2_0_0_7_._1_5_1_0_9_._p_d_f, 2020. │ │ │ │ │ + Date │ │ │ │ │ + September 2011 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _G_n_c_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ + * _W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01166.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,4 @@ │ │ │ │ │ var a01166 = [ │ │ │ │ │ - ["gtsam::GncOptimizer< GncParameters >", "a04360.html", "a04360"] │ │ │ │ │ + ["gtsam::WhiteNoiseFactor", "a04652.html", "a04652"], │ │ │ │ │ + ["logSqrt2PI", "a01166.html#ad5602eb85d05df6cba60e47eebbd3636", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01166_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/GncOptimizer.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/WhiteNoiseFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,516 +98,169 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
GncOptimizer.h
│ │ │ │ +
WhiteNoiseFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
27#pragma once
│ │ │ │ -
28
│ │ │ │ -
29#include <gtsam/nonlinear/GncParams.h>
│ │ │ │ -
30#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ -
31#include <boost/math/distributions/chi_squared.hpp>
│ │ │ │ -
32
│ │ │ │ -
33namespace gtsam {
│ │ │ │ -
34/*
│ │ │ │ -
35 * Quantile of chi-squared distribution with given degrees of freedom at probability alpha.
│ │ │ │ -
36 * Equivalent to chi2inv in Matlab.
│ │ │ │ -
37 */
│ │ │ │ -
38static double Chi2inv(const double alpha, const size_t dofs) {
│ │ │ │ -
39 boost::math::chi_squared_distribution<double> chi2(dofs);
│ │ │ │ -
40 return boost::math::quantile(chi2, alpha);
│ │ │ │ -
41}
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
23#include <gtsam/linear/HessianFactor.h>
│ │ │ │ +
24#include <cmath>
│ │ │ │ +
25
│ │ │ │ +
26namespace gtsam {
│ │ │ │ +
27
│ │ │ │ +
28 const double logSqrt2PI = log(std::sqrt(2.0 * M_PI));
│ │ │ │ +
29
│ │ │ │ +
│ │ │ │ +
41 class WhiteNoiseFactor: public NonlinearFactor {
│ │ │ │
42
│ │ │ │ -
43/* ************************************************************************* */
│ │ │ │ -
44template<class GncParameters>
│ │ │ │ -
│ │ │ │ -
45class GncOptimizer {
│ │ │ │ -
46 public:
│ │ │ │ -
48 typedef typename GncParameters::OptimizerType BaseOptimizer;
│ │ │ │ +
43 private:
│ │ │ │ +
44
│ │ │ │ +
45 double z_;
│ │ │ │ +
46
│ │ │ │ +
47 Key meanKey_;
│ │ │ │ +
48 Key precisionKey_;
│ │ │ │
49
│ │ │ │ -
50 private:
│ │ │ │ -
51 NonlinearFactorGraph nfg_;
│ │ │ │ -
52 Values state_;
│ │ │ │ -
53 GncParameters params_;
│ │ │ │ -
54 Vector weights_;
│ │ │ │ -
55 Vector barcSq_;
│ │ │ │ -
56
│ │ │ │ -
57 public:
│ │ │ │ -
│ │ │ │ -
59 GncOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
│ │ │ │ -
60 const GncParameters& params = GncParameters())
│ │ │ │ -
61 : state_(initialValues),
│ │ │ │ -
62 params_(params) {
│ │ │ │ -
63
│ │ │ │ -
64 // make sure all noiseModels are Gaussian or convert to Gaussian
│ │ │ │ -
65 nfg_.resize(graph.size());
│ │ │ │ -
66 for (size_t i = 0; i < graph.size(); i++) {
│ │ │ │ -
67 if (graph[i]) {
│ │ │ │ -
68 NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast<
│ │ │ │ -
69 NoiseModelFactor>(graph[i]);
│ │ │ │ -
70 auto robust = boost::dynamic_pointer_cast<
│ │ │ │ -
71 noiseModel::Robust>(factor->noiseModel());
│ │ │ │ -
72 // if the factor has a robust loss, we remove the robust loss
│ │ │ │ -
73 nfg_[i] = robust ? factor-> cloneWithNewNoiseModel(robust->noise()) : factor;
│ │ │ │ -
74 }
│ │ │ │ -
75 }
│ │ │ │ -
76
│ │ │ │ -
77 // check that known inliers and outliers make sense:
│ │ │ │ -
78 std::vector<size_t> inconsistentlySpecifiedWeights; // measurements the user has incorrectly specified
│ │ │ │ -
79 // to be BOTH known inliers and known outliers
│ │ │ │ -
80 std::set_intersection(params.knownInliers.begin(),params.knownInliers.end(),
│ │ │ │ -
81 params.knownOutliers.begin(),params.knownOutliers.end(),
│ │ │ │ -
82 std::inserter(inconsistentlySpecifiedWeights, inconsistentlySpecifiedWeights.begin()));
│ │ │ │ -
83 if(inconsistentlySpecifiedWeights.size() > 0){ // if we have inconsistently specified weights, we throw an exception
│ │ │ │ -
84 params.print("params\n");
│ │ │ │ -
85 throw std::runtime_error("GncOptimizer::constructor: the user has selected one or more measurements"
│ │ │ │ -
86 " to be BOTH a known inlier and a known outlier.");
│ │ │ │ -
87 }
│ │ │ │ -
88 // check that known inliers are in the graph
│ │ │ │ -
89 for (size_t i = 0; i < params.knownInliers.size(); i++){
│ │ │ │ -
90 if( params.knownInliers[i] > nfg_.size()-1 ){ // outside graph
│ │ │ │ -
91 throw std::runtime_error("GncOptimizer::constructor: the user has selected one or more measurements"
│ │ │ │ -
92 "that are not in the factor graph to be known inliers.");
│ │ │ │ -
93 }
│ │ │ │ -
94 }
│ │ │ │ -
95 // check that known outliers are in the graph
│ │ │ │ -
96 for (size_t i = 0; i < params.knownOutliers.size(); i++){
│ │ │ │ -
97 if( params.knownOutliers[i] > nfg_.size()-1 ){ // outside graph
│ │ │ │ -
98 throw std::runtime_error("GncOptimizer::constructor: the user has selected one or more measurements"
│ │ │ │ -
99 "that are not in the factor graph to be known outliers.");
│ │ │ │ -
100 }
│ │ │ │ -
101 }
│ │ │ │ -
102 // initialize weights (if we don't have prior knowledge of inliers/outliers
│ │ │ │ -
103 // the weights are all initialized to 1.
│ │ │ │ -
104 weights_ = initializeWeightsFromKnownInliersAndOutliers();
│ │ │ │ -
105
│ │ │ │ -
106 // set default barcSq_ (inlier threshold)
│ │ │ │ -
107 double alpha = 0.99; // with this (default) probability, inlier residuals are smaller than barcSq_
│ │ │ │ -
108 setInlierCostThresholdsAtProbability(alpha);
│ │ │ │ -
109 }
│ │ │ │ -
│ │ │ │ -
110
│ │ │ │ -
│ │ │ │ -
117 void setInlierCostThresholds(const double inth) {
│ │ │ │ -
118 barcSq_ = inth * Vector::Ones(nfg_.size());
│ │ │ │ -
119 }
│ │ │ │ -
│ │ │ │ -
120
│ │ │ │ -
│ │ │ │ -
125 void setInlierCostThresholds(const Vector& inthVec) {
│ │ │ │ -
126 barcSq_ = inthVec;
│ │ │ │ -
127 }
│ │ │ │ -
│ │ │ │ -
128
│ │ │ │ -
│ │ │ │ -
132 void setInlierCostThresholdsAtProbability(const double alpha) {
│ │ │ │ -
133 barcSq_ = Vector::Ones(nfg_.size()); // initialize
│ │ │ │ -
134 for (size_t k = 0; k < nfg_.size(); k++) {
│ │ │ │ -
135 if (nfg_[k]) {
│ │ │ │ -
136 barcSq_[k] = 0.5 * Chi2inv(alpha, nfg_[k]->dim()); // 0.5 derives from the error definition in gtsam
│ │ │ │ -
137 }
│ │ │ │ -
138 }
│ │ │ │ -
139 }
│ │ │ │ -
│ │ │ │ -
140
│ │ │ │ -
│ │ │ │ -
144 void setWeights(const Vector w) {
│ │ │ │ -
145 if (size_t(w.size()) != nfg_.size()) {
│ │ │ │ -
146 throw std::runtime_error(
│ │ │ │ -
147 "GncOptimizer::setWeights: the number of specified weights"
│ │ │ │ -
148 " does not match the size of the factor graph.");
│ │ │ │ -
149 }
│ │ │ │ -
150 weights_ = w;
│ │ │ │ -
151 }
│ │ │ │ -
│ │ │ │ +
50 typedef NonlinearFactor Base;
│ │ │ │ +
51
│ │ │ │ +
52 public:
│ │ │ │ +
53
│ │ │ │ +
│ │ │ │ +
61 static double f(double z, double u, double p) {
│ │ │ │ +
62 return logSqrt2PI - 0.5 * log(p) + 0.5 * (z - u) * (z - u) * p;
│ │ │ │ +
63 }
│ │ │ │ +
│ │ │ │ +
64
│ │ │ │ +
│ │ │ │ +
75 static HessianFactor::shared_ptr linearize(double z, double u, double p,
│ │ │ │ +
76 Key j1, Key j2) {
│ │ │ │ +
77 double e = u - z, e2 = e * e;
│ │ │ │ +
78 double c = 2 * logSqrt2PI - log(p) + e2 * p;
│ │ │ │ +
79 Vector g1 = (Vector(1) << -e * p).finished();
│ │ │ │ +
80 Vector g2 = (Vector(1) << 0.5 / p - 0.5 * e2).finished();
│ │ │ │ +
81 Matrix G11 = (Matrix(1, 1) << p).finished();
│ │ │ │ +
82 Matrix G12 = (Matrix(1, 1) << e).finished();
│ │ │ │ +
83 Matrix G22 = (Matrix(1, 1) << 0.5 / (p * p)).finished();
│ │ │ │ +
84 return HessianFactor::shared_ptr(
│ │ │ │ +
85 new HessianFactor(j1, j2, G11, G12, g1, G22, g2, c));
│ │ │ │ +
86 }
│ │ │ │ +
│ │ │ │ +
87
│ │ │ │ +
90
│ │ │ │ +
│ │ │ │ +
96 WhiteNoiseFactor(double z, Key meanKey, Key precisionKey) :
│ │ │ │ +
97 Base(), z_(z), meanKey_(meanKey), precisionKey_(precisionKey) {
│ │ │ │ +
98 }
│ │ │ │ +
│ │ │ │ +
99
│ │ │ │ +
103
│ │ │ │ +
│ │ │ │ +
105 ~WhiteNoiseFactor() override {
│ │ │ │ +
106 }
│ │ │ │ +
│ │ │ │ +
107
│ │ │ │ +
111
│ │ │ │ +
│ │ │ │ +
113 void print(const std::string& p = "WhiteNoiseFactor",
│ │ │ │ +
114 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ +
115 Base::print(p, keyFormatter);
│ │ │ │ +
116 std::cout << p + ".z: " << z_ << std::endl;
│ │ │ │ +
117 }
│ │ │ │ +
│ │ │ │ +
118
│ │ │ │ +
122
│ │ │ │ +
│ │ │ │ +
124 size_t dim() const override {
│ │ │ │ +
125 return 2;
│ │ │ │ +
126 }
│ │ │ │ +
│ │ │ │ +
127
│ │ │ │ +
│ │ │ │ +
129 double error(const Values& x) const override {
│ │ │ │ +
130 return f(z_, x.at<double>(meanKey_), x.at<double>(precisionKey_));
│ │ │ │ +
131 }
│ │ │ │ +
│ │ │ │ +
132
│ │ │ │ +
│ │ │ │ +
140 virtual Vector unwhitenedError(const Values& x) const {
│ │ │ │ +
141 return (Vector(1) << std::sqrt(2 * error(x))).finished();
│ │ │ │ +
142 }
│ │ │ │ +
│ │ │ │ +
143
│ │ │ │ +
148// virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
│ │ │ │ +
149// const Key j1 = ordering[meanKey_], j2 = ordering[precisionKey_];
│ │ │ │ +
150// return IndexFactor::shared_ptr(new IndexFactor(j1, j2));
│ │ │ │ +
151// }
│ │ │ │
152
│ │ │ │ -
154 const NonlinearFactorGraph& getFactors() const { return nfg_; }
│ │ │ │ -
155
│ │ │ │ -
157 const Values& getState() const { return state_; }
│ │ │ │ -
158
│ │ │ │ -
160 const GncParameters& getParams() const { return params_;}
│ │ │ │ -
161
│ │ │ │ -
163 const Vector& getWeights() const { return weights_;}
│ │ │ │ -
164
│ │ │ │ -
166 const Vector& getInlierCostThresholds() const {return barcSq_;}
│ │ │ │ -
167
│ │ │ │ -
│ │ │ │ -
169 bool equals(const GncOptimizer& other, double tol = 1e-9) const {
│ │ │ │ -
170 return nfg_.equals(other.getFactors())
│ │ │ │ -
171 && equal(weights_, other.getWeights())
│ │ │ │ -
172 && params_.equals(other.getParams())
│ │ │ │ -
173 && equal(barcSq_, other.getInlierCostThresholds());
│ │ │ │ -
174 }
│ │ │ │ -
│ │ │ │ -
175
│ │ │ │ -
176 Vector initializeWeightsFromKnownInliersAndOutliers() const{
│ │ │ │ -
177 Vector weights = Vector::Ones(nfg_.size());
│ │ │ │ -
178 for (size_t i = 0; i < params_.knownOutliers.size(); i++){
│ │ │ │ -
179 weights[ params_.knownOutliers[i] ] = 0.0; // known to be outliers
│ │ │ │ -
180 }
│ │ │ │ -
181 return weights;
│ │ │ │ -
182 }
│ │ │ │ -
183
│ │ │ │ -
│ │ │ │ -
185 Values optimize() {
│ │ │ │ -
186 NonlinearFactorGraph graph_initial = this->makeWeightedGraph(weights_);
│ │ │ │ -
187 BaseOptimizer baseOptimizer(
│ │ │ │ -
188 graph_initial, state_, params_.baseOptimizerParams);
│ │ │ │ -
189 Values result = baseOptimizer.optimize();
│ │ │ │ -
190 double mu = initializeMu();
│ │ │ │ -
191 double prev_cost = graph_initial.error(result);
│ │ │ │ -
192 double cost = 0.0; // this will be updated in the main loop
│ │ │ │ -
193
│ │ │ │ -
194 // handle the degenerate case that corresponds to small
│ │ │ │ -
195 // maximum residual errors at initialization
│ │ │ │ -
196 // For GM: if residual error is small, mu -> 0
│ │ │ │ -
197 // For TLS: if residual error is small, mu -> -1
│ │ │ │ -
198 int nrUnknownInOrOut = nfg_.size() - ( params_.knownInliers.size() + params_.knownOutliers.size() );
│ │ │ │ -
199 // ^^ number of measurements that are not known to be inliers or outliers (GNC will need to figure them out)
│ │ │ │ -
200 if (mu <= 0 || nrUnknownInOrOut == 0) { // no need to even call GNC in this case
│ │ │ │ -
201 if (mu <= 0 && params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
│ │ │ │ -
202 std::cout << "GNC Optimizer stopped because maximum residual at "
│ │ │ │ -
203 "initialization is small."
│ │ │ │ -
204 << std::endl;
│ │ │ │ -
205 }
│ │ │ │ -
206 if (nrUnknownInOrOut==0 && params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
│ │ │ │ -
207 std::cout << "GNC Optimizer stopped because all measurements are already known to be inliers or outliers"
│ │ │ │ -
208 << std::endl;
│ │ │ │ -
209 }
│ │ │ │ -
210 if (params_.verbosity >= GncParameters::Verbosity::MU) {
│ │ │ │ -
211 std::cout << "mu: " << mu << std::endl;
│ │ │ │ -
212 }
│ │ │ │ -
213 if (params_.verbosity >= GncParameters::Verbosity::VALUES) {
│ │ │ │ -
214 result.print("result\n");
│ │ │ │ -
215 }
│ │ │ │ -
216 return result;
│ │ │ │ -
217 }
│ │ │ │ -
218
│ │ │ │ -
219 size_t iter;
│ │ │ │ -
220 for (iter = 0; iter < params_.maxIterations; iter++) {
│ │ │ │ -
221
│ │ │ │ -
222 // display info
│ │ │ │ -
223 if (params_.verbosity >= GncParameters::Verbosity::MU) {
│ │ │ │ -
224 std::cout << "iter: " << iter << std::endl;
│ │ │ │ -
225 std::cout << "mu: " << mu << std::endl;
│ │ │ │ -
226 }
│ │ │ │ -
227 if (params_.verbosity >= GncParameters::Verbosity::WEIGHTS) {
│ │ │ │ -
228 std::cout << "weights: " << weights_ << std::endl;
│ │ │ │ -
229 }
│ │ │ │ -
230 if (params_.verbosity >= GncParameters::Verbosity::VALUES) {
│ │ │ │ -
231 result.print("result\n");
│ │ │ │ -
232 }
│ │ │ │ -
233 // weights update
│ │ │ │ -
234 weights_ = calculateWeights(result, mu);
│ │ │ │ -
235
│ │ │ │ -
236 // variable/values update
│ │ │ │ -
237 NonlinearFactorGraph graph_iter = this->makeWeightedGraph(weights_);
│ │ │ │ -
238 BaseOptimizer baseOptimizer_iter(
│ │ │ │ -
239 graph_iter, state_, params_.baseOptimizerParams);
│ │ │ │ -
240 result = baseOptimizer_iter.optimize();
│ │ │ │ -
241
│ │ │ │ -
242 // stopping condition
│ │ │ │ -
243 cost = graph_iter.error(result);
│ │ │ │ -
244 if (checkConvergence(mu, weights_, cost, prev_cost)) {
│ │ │ │ -
245 break;
│ │ │ │ -
246 }
│ │ │ │ -
247
│ │ │ │ -
248 // update mu
│ │ │ │ -
249 mu = updateMu(mu);
│ │ │ │ -
250
│ │ │ │ -
251 // get ready for next iteration
│ │ │ │ -
252 prev_cost = cost;
│ │ │ │ -
253
│ │ │ │ -
254 // display info
│ │ │ │ -
255 if (params_.verbosity >= GncParameters::Verbosity::VALUES) {
│ │ │ │ -
256 std::cout << "previous cost: " << prev_cost << std::endl;
│ │ │ │ -
257 std::cout << "current cost: " << cost << std::endl;
│ │ │ │ -
258 }
│ │ │ │ -
259 }
│ │ │ │ -
260 // display info
│ │ │ │ -
261 if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
│ │ │ │ -
262 std::cout << "final iterations: " << iter << std::endl;
│ │ │ │ -
263 std::cout << "final mu: " << mu << std::endl;
│ │ │ │ -
264 std::cout << "previous cost: " << prev_cost << std::endl;
│ │ │ │ -
265 std::cout << "current cost: " << cost << std::endl;
│ │ │ │ -
266 }
│ │ │ │ -
267 if (params_.verbosity >= GncParameters::Verbosity::WEIGHTS) {
│ │ │ │ -
268 std::cout << "final weights: " << weights_ << std::endl;
│ │ │ │ -
269 }
│ │ │ │ -
270 return result;
│ │ │ │ -
271 }
│ │ │ │ -
│ │ │ │ -
272
│ │ │ │ -
│ │ │ │ -
274 double initializeMu() const {
│ │ │ │ -
275
│ │ │ │ -
276 double mu_init = 0.0;
│ │ │ │ -
277 // initialize mu to the value specified in Remark 5 in GNC paper.
│ │ │ │ -
278 switch (params_.lossType) {
│ │ │ │ -
279 case GncLossType::GM:
│ │ │ │ -
280 /* surrogate cost is convex for large mu. initialize as in remark 5 in GNC paper.
│ │ │ │ -
281 Since barcSq_ can be different for each factor, we compute the max of the quantity in remark 5 in GNC paper
│ │ │ │ -
282 */
│ │ │ │ -
283 for (size_t k = 0; k < nfg_.size(); k++) {
│ │ │ │ -
284 if (nfg_[k]) {
│ │ │ │ -
285 mu_init = std::max(mu_init, 2 * nfg_[k]->error(state_) / barcSq_[k]);
│ │ │ │ -
286 }
│ │ │ │ -
287 }
│ │ │ │ -
288 return mu_init; // initial mu
│ │ │ │ -
289 case GncLossType::TLS:
│ │ │ │ -
290 /* surrogate cost is convex for mu close to zero. initialize as in remark 5 in GNC paper.
│ │ │ │ -
291 degenerate case: 2 * rmax_sq - params_.barcSq < 0 (handled in the main loop)
│ │ │ │ -
292 according to remark mu = params_.barcSq / (2 * rmax_sq - params_.barcSq) = params_.barcSq/ excessResidual
│ │ │ │ -
293 however, if the denominator is 0 or negative, we return mu = -1 which leads to termination of the main GNC loop.
│ │ │ │ -
294 Since barcSq_ can be different for each factor, we look for the minimimum (positive) quantity in remark 5 in GNC paper
│ │ │ │ -
295 */
│ │ │ │ -
296 mu_init = std::numeric_limits<double>::infinity();
│ │ │ │ -
297 for (size_t k = 0; k < nfg_.size(); k++) {
│ │ │ │ -
298 if (nfg_[k]) {
│ │ │ │ -
299 double rk = nfg_[k]->error(state_);
│ │ │ │ -
300 mu_init = (2 * rk - barcSq_[k]) > 0 ? // if positive, update mu, otherwise keep same
│ │ │ │ -
301 std::min(mu_init, barcSq_[k] / (2 * rk - barcSq_[k]) ) : mu_init;
│ │ │ │ -
302 }
│ │ │ │ -
303 }
│ │ │ │ -
304 if (mu_init >= 0 && mu_init < 1e-6){
│ │ │ │ -
305 mu_init = 1e-6; // if mu ~ 0 (but positive), that means we have measurements with large errors,
│ │ │ │ -
306 // i.e., rk > barcSq_[k] and rk very large, hence we threshold to 1e-6 to avoid mu = 0
│ │ │ │ -
307 }
│ │ │ │ -
308
│ │ │ │ -
309 return mu_init > 0 && !std::isinf(mu_init) ? mu_init : -1; // if mu <= 0 or mu = inf, return -1,
│ │ │ │ -
310 // which leads to termination of the main gnc loop. In this case, all residuals are already below the threshold
│ │ │ │ -
311 // and there is no need to robustify (TLS = least squares)
│ │ │ │ -
312 default:
│ │ │ │ -
313 throw std::runtime_error(
│ │ │ │ -
314 "GncOptimizer::initializeMu: called with unknown loss type.");
│ │ │ │ -
315 }
│ │ │ │ -
316 }
│ │ │ │ -
│ │ │ │ -
317
│ │ │ │ -
│ │ │ │ -
319 double updateMu(const double mu) const {
│ │ │ │ -
320 switch (params_.lossType) {
│ │ │ │ -
321 case GncLossType::GM:
│ │ │ │ -
322 // reduce mu, but saturate at 1 (original cost is recovered for mu -> 1)
│ │ │ │ -
323 return std::max(1.0, mu / params_.muStep);
│ │ │ │ -
324 case GncLossType::TLS:
│ │ │ │ -
325 // increases mu at each iteration (original cost is recovered for mu -> inf)
│ │ │ │ -
326 return mu * params_.muStep;
│ │ │ │ -
327 default:
│ │ │ │ -
328 throw std::runtime_error(
│ │ │ │ -
329 "GncOptimizer::updateMu: called with unknown loss type.");
│ │ │ │ -
330 }
│ │ │ │ -
331 }
│ │ │ │ -
│ │ │ │ -
332
│ │ │ │ -
│ │ │ │ -
334 bool checkMuConvergence(const double mu) const {
│ │ │ │ -
335 bool muConverged = false;
│ │ │ │ -
336 switch (params_.lossType) {
│ │ │ │ -
337 case GncLossType::GM:
│ │ │ │ -
338 muConverged = std::fabs(mu - 1.0) < 1e-9; // mu=1 recovers the original GM function
│ │ │ │ -
339 break;
│ │ │ │ -
340 case GncLossType::TLS:
│ │ │ │ -
341 muConverged = false; // for TLS there is no stopping condition on mu (it must tend to infinity)
│ │ │ │ -
342 break;
│ │ │ │ -
343 default:
│ │ │ │ -
344 throw std::runtime_error(
│ │ │ │ -
345 "GncOptimizer::checkMuConvergence: called with unknown loss type.");
│ │ │ │ -
346 }
│ │ │ │ -
347 if (muConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY)
│ │ │ │ -
348 std::cout << "muConverged = true " << std::endl;
│ │ │ │ -
349 return muConverged;
│ │ │ │ -
350 }
│ │ │ │ -
│ │ │ │ -
351
│ │ │ │ -
│ │ │ │ -
353 bool checkCostConvergence(const double cost, const double prev_cost) const {
│ │ │ │ -
354 bool costConverged = std::fabs(cost - prev_cost) / std::max(prev_cost, 1e-7)
│ │ │ │ -
355 < params_.relativeCostTol;
│ │ │ │ -
356 if (costConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY){
│ │ │ │ -
357 std::cout << "checkCostConvergence = true (prev. cost = " << prev_cost
│ │ │ │ -
358 << ", curr. cost = " << cost << ")" << std::endl;
│ │ │ │ -
359 }
│ │ │ │ -
360 return costConverged;
│ │ │ │ -
361 }
│ │ │ │ -
│ │ │ │ -
362
│ │ │ │ -
│ │ │ │ -
364 bool checkWeightsConvergence(const Vector& weights) const {
│ │ │ │ -
365 bool weightsConverged = false;
│ │ │ │ -
366 switch (params_.lossType) {
│ │ │ │ -
367 case GncLossType::GM:
│ │ │ │ -
368 weightsConverged = false; // for GM, there is no clear binary convergence for the weights
│ │ │ │ -
369 break;
│ │ │ │ -
370 case GncLossType::TLS:
│ │ │ │ -
371 weightsConverged = true;
│ │ │ │ -
372 for (int i = 0; i < weights.size(); i++) {
│ │ │ │ -
373 if (std::fabs(weights[i] - std::round(weights[i]))
│ │ │ │ -
374 > params_.weightsTol) {
│ │ │ │ -
375 weightsConverged = false;
│ │ │ │ -
376 break;
│ │ │ │ -
377 }
│ │ │ │ -
378 }
│ │ │ │ -
379 break;
│ │ │ │ -
380 default:
│ │ │ │ -
381 throw std::runtime_error(
│ │ │ │ -
382 "GncOptimizer::checkWeightsConvergence: called with unknown loss type.");
│ │ │ │ -
383 }
│ │ │ │ -
384 if (weightsConverged
│ │ │ │ -
385 && params_.verbosity >= GncParameters::Verbosity::SUMMARY)
│ │ │ │ -
386 std::cout << "weightsConverged = true " << std::endl;
│ │ │ │ -
387 return weightsConverged;
│ │ │ │ -
388 }
│ │ │ │ -
│ │ │ │ -
389
│ │ │ │ -
│ │ │ │ -
391 bool checkConvergence(const double mu, const Vector& weights,
│ │ │ │ -
392 const double cost, const double prev_cost) const {
│ │ │ │ -
393 return checkCostConvergence(cost, prev_cost)
│ │ │ │ -
394 || checkWeightsConvergence(weights) || checkMuConvergence(mu);
│ │ │ │ -
395 }
│ │ │ │ -
│ │ │ │ -
396
│ │ │ │ -
│ │ │ │ -
398 NonlinearFactorGraph makeWeightedGraph(const Vector& weights) const {
│ │ │ │ -
399 // make sure all noiseModels are Gaussian or convert to Gaussian
│ │ │ │ -
400 NonlinearFactorGraph newGraph;
│ │ │ │ -
401 newGraph.resize(nfg_.size());
│ │ │ │ -
402 for (size_t i = 0; i < nfg_.size(); i++) {
│ │ │ │ -
403 if (nfg_[i]) {
│ │ │ │ -
404 auto factor = boost::dynamic_pointer_cast<
│ │ │ │ -
405 NoiseModelFactor>(nfg_[i]);
│ │ │ │ -
406 auto noiseModel =
│ │ │ │ -
407 boost::dynamic_pointer_cast<noiseModel::Gaussian>(
│ │ │ │ -
408 factor->noiseModel());
│ │ │ │ -
409 if (noiseModel) {
│ │ │ │ -
410 Matrix newInfo = weights[i] * noiseModel->information();
│ │ │ │ -
411 auto newNoiseModel = noiseModel::Gaussian::Information(newInfo);
│ │ │ │ -
412 newGraph[i] = factor->cloneWithNewNoiseModel(newNoiseModel);
│ │ │ │ -
413 } else {
│ │ │ │ -
414 throw std::runtime_error(
│ │ │ │ -
415 "GncOptimizer::makeWeightedGraph: unexpected non-Gaussian noise model.");
│ │ │ │ -
416 }
│ │ │ │ -
417 }
│ │ │ │ -
418 }
│ │ │ │ -
419 return newGraph;
│ │ │ │ -
420 }
│ │ │ │ -
│ │ │ │ -
421
│ │ │ │ -
│ │ │ │ -
423 Vector calculateWeights(const Values& currentEstimate, const double mu) {
│ │ │ │ -
424 Vector weights = initializeWeightsFromKnownInliersAndOutliers();
│ │ │ │ -
425
│ │ │ │ -
426 // do not update the weights that the user has decided are known inliers
│ │ │ │ -
427 std::vector<size_t> allWeights;
│ │ │ │ -
428 for (size_t k = 0; k < nfg_.size(); k++) {
│ │ │ │ -
429 allWeights.push_back(k);
│ │ │ │ -
430 }
│ │ │ │ -
431 std::vector<size_t> knownWeights;
│ │ │ │ -
432 std::set_union(params_.knownInliers.begin(), params_.knownInliers.end(),
│ │ │ │ -
433 params_.knownOutliers.begin(), params_.knownOutliers.end(),
│ │ │ │ -
434 std::inserter(knownWeights, knownWeights.begin()));
│ │ │ │ -
435
│ │ │ │ -
436 std::vector<size_t> unknownWeights;
│ │ │ │ -
437 std::set_difference(allWeights.begin(), allWeights.end(),
│ │ │ │ -
438 knownWeights.begin(), knownWeights.end(),
│ │ │ │ -
439 std::inserter(unknownWeights, unknownWeights.begin()));
│ │ │ │ -
440
│ │ │ │ -
441 // update weights of known inlier/outlier measurements
│ │ │ │ -
442 switch (params_.lossType) {
│ │ │ │ -
443 case GncLossType::GM: { // use eq (12) in GNC paper
│ │ │ │ -
444 for (size_t k : unknownWeights) {
│ │ │ │ -
445 if (nfg_[k]) {
│ │ │ │ -
446 double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual
│ │ │ │ -
447 weights[k] = std::pow(
│ │ │ │ -
448 (mu * barcSq_[k]) / (u2_k + mu * barcSq_[k]), 2);
│ │ │ │ -
449 }
│ │ │ │ -
450 }
│ │ │ │ -
451 return weights;
│ │ │ │ -
452 }
│ │ │ │ -
453 case GncLossType::TLS: { // use eq (14) in GNC paper
│ │ │ │ -
454 for (size_t k : unknownWeights) {
│ │ │ │ -
455 if (nfg_[k]) {
│ │ │ │ -
456 double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual
│ │ │ │ -
457 double upperbound = (mu + 1) / mu * barcSq_[k];
│ │ │ │ -
458 double lowerbound = mu / (mu + 1) * barcSq_[k];
│ │ │ │ -
459 weights[k] = std::sqrt(barcSq_[k] * mu * (mu + 1) / u2_k) - mu;
│ │ │ │ -
460 if (u2_k >= upperbound || weights[k] < 0) {
│ │ │ │ -
461 weights[k] = 0;
│ │ │ │ -
462 } else if (u2_k <= lowerbound || weights[k] > 1) {
│ │ │ │ -
463 weights[k] = 1;
│ │ │ │ -
464 }
│ │ │ │ -
465 }
│ │ │ │ -
466 }
│ │ │ │ -
467 return weights;
│ │ │ │ -
468 }
│ │ │ │ -
469 default:
│ │ │ │ -
470 throw std::runtime_error(
│ │ │ │ -
471 "GncOptimizer::calculateWeights: called with unknown loss type.");
│ │ │ │ -
472 }
│ │ │ │ -
473 }
│ │ │ │ -
│ │ │ │ -
474};
│ │ │ │ -
│ │ │ │ -
475
│ │ │ │ -
476}
│ │ │ │ -
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │ +
156
│ │ │ │ +
│ │ │ │ +
158 boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override {
│ │ │ │ +
159 double u = x.at<double>(meanKey_);
│ │ │ │ +
160 double p = x.at<double>(precisionKey_);
│ │ │ │ +
161 Key j1 = meanKey_;
│ │ │ │ +
162 Key j2 = precisionKey_;
│ │ │ │ +
163 return linearize(z_, u, p, j1, j2);
│ │ │ │ +
164 }
│ │ │ │ +
│ │ │ │ +
165
│ │ │ │ +
166 // TODO: Frank commented this out for now, can it go?
│ │ │ │ +
167 // /// @return a deep copy of this factor
│ │ │ │ +
168 // gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
169 // return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ +
170 // gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
│ │ │ │ +
171
│ │ │ │ +
173
│ │ │ │ +
174 };
│ │ │ │ +
│ │ │ │ +
175// WhiteNoiseFactor
│ │ │ │ +
176
│ │ │ │ +
177}// namespace gtsam
│ │ │ │ +
178
│ │ │ │ +
HessianFactor.h
Contains the HessianFactor class, a general quadratic factor.
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::equal
bool equal(const T &obj1, const T &obj2, double tol)
Call equal on the object.
Definition Testable.h:84
│ │ │ │ -
gtsam::FactorGraph::resize
virtual void resize(size_t size)
Directly resize the number of factors in the graph.
Definition FactorGraph.h:381
│ │ │ │ -
gtsam::FactorGraph::size
size_t size() const
return the number of factors (including any null factors set by remove() ).
Definition FactorGraph.h:326
│ │ │ │ -
gtsam::noiseModel::Gaussian::Information
static shared_ptr Information(const Matrix &M, bool smart=true)
A Gaussian noise model created by specifying an information matrix.
Definition NoiseModel.cpp:100
│ │ │ │ -
gtsam::noiseModel::Robust
Base class for robust error models The robust M-estimators above simply tell us how to re-weight the ...
Definition NoiseModel.h:642
│ │ │ │ -
gtsam::GncOptimizer
Definition GncOptimizer.h:45
│ │ │ │ -
gtsam::GncOptimizer::checkWeightsConvergence
bool checkWeightsConvergence(const Vector &weights) const
Check convergence of weights to binary values.
Definition GncOptimizer.h:364
│ │ │ │ -
gtsam::GncOptimizer::setWeights
void setWeights(const Vector w)
Set weights for each factor.
Definition GncOptimizer.h:144
│ │ │ │ -
gtsam::GncOptimizer::checkMuConvergence
bool checkMuConvergence(const double mu) const
Check if we have reached the value of mu for which the surrogate loss matches the original loss.
Definition GncOptimizer.h:334
│ │ │ │ -
gtsam::GncOptimizer::BaseOptimizer
GncParameters::OptimizerType BaseOptimizer
For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimi...
Definition GncOptimizer.h:48
│ │ │ │ -
gtsam::GncOptimizer::optimize
Values optimize()
Compute optimal solution using graduated non-convexity.
Definition GncOptimizer.h:185
│ │ │ │ -
gtsam::GncOptimizer::getInlierCostThresholds
const Vector & getInlierCostThresholds() const
Get the inlier threshold.
Definition GncOptimizer.h:166
│ │ │ │ -
gtsam::GncOptimizer::checkCostConvergence
bool checkCostConvergence(const double cost, const double prev_cost) const
Check convergence of relative cost differences.
Definition GncOptimizer.h:353
│ │ │ │ -
gtsam::GncOptimizer::getState
const Values & getState() const
Access a copy of the internal values.
Definition GncOptimizer.h:157
│ │ │ │ -
gtsam::GncOptimizer::setInlierCostThresholds
void setInlierCostThresholds(const Vector &inthVec)
Set the maximum weighted residual error for an inlier (one for each factor).
Definition GncOptimizer.h:125
│ │ │ │ -
gtsam::GncOptimizer::calculateWeights
Vector calculateWeights(const Values &currentEstimate, const double mu)
Calculate gnc weights.
Definition GncOptimizer.h:423
│ │ │ │ -
gtsam::GncOptimizer::initializeMu
double initializeMu() const
Initialize the gnc parameter mu such that loss is approximately convex (remark 5 in GNC paper).
Definition GncOptimizer.h:274
│ │ │ │ -
gtsam::GncOptimizer::updateMu
double updateMu(const double mu) const
Update the gnc parameter mu to gradually increase nonconvexity.
Definition GncOptimizer.h:319
│ │ │ │ -
gtsam::GncOptimizer::equals
bool equals(const GncOptimizer &other, double tol=1e-9) const
Equals.
Definition GncOptimizer.h:169
│ │ │ │ -
gtsam::GncOptimizer::checkConvergence
bool checkConvergence(const double mu, const Vector &weights, const double cost, const double prev_cost) const
Check for convergence between consecutive GNC iterations.
Definition GncOptimizer.h:391
│ │ │ │ -
gtsam::GncOptimizer::setInlierCostThresholds
void setInlierCostThresholds(const double inth)
Set the maximum weighted residual error for an inlier (same for all factors).
Definition GncOptimizer.h:117
│ │ │ │ -
gtsam::GncOptimizer::setInlierCostThresholdsAtProbability
void setInlierCostThresholdsAtProbability(const double alpha)
Set the maximum weighted residual error threshold by specifying the probability alpha that the inlier...
Definition GncOptimizer.h:132
│ │ │ │ -
gtsam::GncOptimizer::makeWeightedGraph
NonlinearFactorGraph makeWeightedGraph(const Vector &weights) const
Create a graph where each factor is weighted by the gnc weights.
Definition GncOptimizer.h:398
│ │ │ │ -
gtsam::GncOptimizer::getFactors
const NonlinearFactorGraph & getFactors() const
Access a copy of the internal factor graph.
Definition GncOptimizer.h:154
│ │ │ │ -
gtsam::GncOptimizer::getWeights
const Vector & getWeights() const
Access a copy of the GNC weights.
Definition GncOptimizer.h:163
│ │ │ │ -
gtsam::GncOptimizer::getParams
const GncParameters & getParams() const
Access a copy of the parameters.
Definition GncOptimizer.h:160
│ │ │ │ -
gtsam::GncOptimizer::GncOptimizer
GncOptimizer(const NonlinearFactorGraph &graph, const Values &initialValues, const GncParameters &params=GncParameters())
Constructor.
Definition GncOptimizer.h:59
│ │ │ │ -
gtsam::NoiseModelFactor
A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
Definition NonlinearFactor.h:174
│ │ │ │ -
gtsam::NoiseModelFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
Noise model.
Definition NonlinearFactor.h:186
│ │ │ │ -
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │ -
gtsam::NonlinearFactorGraph::equals
bool equals(const NonlinearFactorGraph &other, double tol=1e-9) const
Test equality.
Definition NonlinearFactorGraph.cpp:97
│ │ │ │ -
gtsam::NonlinearFactorGraph::error
double error(const Values &values) const
unnormalized error, in the most common case
Definition NonlinearFactorGraph.cpp:170
│ │ │ │ +
gtsam::logSqrt2PI
const double logSqrt2PI
constant needed below
Definition WhiteNoiseFactor.h:28
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition HessianFactor.h:101
│ │ │ │ +
gtsam::HessianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
A shared_ptr to this class.
Definition HessianFactor.h:110
│ │ │ │ +
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ +
gtsam::NonlinearFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition NonlinearFactor.cpp:37
│ │ │ │
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
gtsam::Values::at
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
│ │ │ │ +
gtsam::WhiteNoiseFactor
Binary factor to estimate parameters of zero-mean Gaussian white noise.
Definition WhiteNoiseFactor.h:41
│ │ │ │ +
gtsam::WhiteNoiseFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override
linearize returns a Hessianfactor that is an approximation of error(p)
Definition WhiteNoiseFactor.h:158
│ │ │ │ +
gtsam::WhiteNoiseFactor::~WhiteNoiseFactor
~WhiteNoiseFactor() override
Destructor.
Definition WhiteNoiseFactor.h:105
│ │ │ │ +
gtsam::WhiteNoiseFactor::linearize
static HessianFactor::shared_ptr linearize(double z, double u, double p, Key j1, Key j2)
linearize returns a Hessianfactor that approximates error Hessian is
Definition WhiteNoiseFactor.h:75
│ │ │ │ +
gtsam::WhiteNoiseFactor::print
void print(const std::string &p="WhiteNoiseFactor", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition WhiteNoiseFactor.h:113
│ │ │ │ +
gtsam::WhiteNoiseFactor::WhiteNoiseFactor
WhiteNoiseFactor(double z, Key meanKey, Key precisionKey)
Construct from measurement.
Definition WhiteNoiseFactor.h:96
│ │ │ │ +
gtsam::WhiteNoiseFactor::unwhitenedError
virtual Vector unwhitenedError(const Values &x) const
Vector of errors "unwhitened" does not make sense for this factor What is meant typically is only "e"...
Definition WhiteNoiseFactor.h:140
│ │ │ │ +
gtsam::WhiteNoiseFactor::f
static double f(double z, double u, double p)
negative log likelihood as a function of mean and precision
Definition WhiteNoiseFactor.h:61
│ │ │ │ +
gtsam::WhiteNoiseFactor::error
double error(const Values &x) const override
Calculate the error of the factor, typically equal to log-likelihood.
Definition WhiteNoiseFactor.h:129
│ │ │ │ +
gtsam::WhiteNoiseFactor::dim
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition WhiteNoiseFactor.h:124
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,616 +1,204 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GncOptimizer.h │ │ │ │ │ +WhiteNoiseFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -27#pragma once │ │ │ │ │ -28 │ │ │ │ │ -29#include │ │ │ │ │ -30#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -31#include │ │ │ │ │ -32 │ │ │ │ │ -33namespace _g_t_s_a_m { │ │ │ │ │ -34/* │ │ │ │ │ -35 * Quantile of chi-squared distribution with given degrees of freedom at │ │ │ │ │ -probability alpha. │ │ │ │ │ -36 * Equivalent to chi2inv in Matlab. │ │ │ │ │ -37 */ │ │ │ │ │ -38static double Chi2inv(const double alpha, const size_t dofs) { │ │ │ │ │ -39 boost::math::chi_squared_distribution chi2(dofs); │ │ │ │ │ -40 return boost::math::quantile(chi2, alpha); │ │ │ │ │ -41} │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_H_e_s_s_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ +24#include │ │ │ │ │ +25 │ │ │ │ │ +26namespace _g_t_s_a_m { │ │ │ │ │ +27 │ │ │ │ │ +_2_8 const double _l_o_g_S_q_r_t_2_P_I = log(std::sqrt(2.0 * M_PI)); │ │ │ │ │ +29 │ │ │ │ │ +_4_1 class _W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r: public _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r { │ │ │ │ │ 42 │ │ │ │ │ -43/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -44template │ │ │ │ │ -_4_5class _G_n_c_O_p_t_i_m_i_z_e_r { │ │ │ │ │ -46 public: │ │ │ │ │ -_4_8 typedef typename GncParameters::OptimizerType _B_a_s_e_O_p_t_i_m_i_z_e_r; │ │ │ │ │ +43 private: │ │ │ │ │ +44 │ │ │ │ │ +45 double z_; │ │ │ │ │ +46 │ │ │ │ │ +47 _K_e_y meanKey_; │ │ │ │ │ +48 _K_e_y precisionKey_; │ │ │ │ │ 49 │ │ │ │ │ -50 private: │ │ │ │ │ -51 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h nfg_; │ │ │ │ │ -52 _V_a_l_u_e_s state_; │ │ │ │ │ -53 GncParameters params_; │ │ │ │ │ -54 Vector weights_; │ │ │ │ │ -55 Vector barcSq_; │ │ │ │ │ -56 │ │ │ │ │ -57 public: │ │ │ │ │ -_5_9 _G_n_c_O_p_t_i_m_i_z_e_r(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, const _V_a_l_u_e_s& initialValues, │ │ │ │ │ -60 const GncParameters& params = GncParameters()) │ │ │ │ │ -61 : state_(initialValues), │ │ │ │ │ -62 params_(params) { │ │ │ │ │ -63 │ │ │ │ │ -64 // make sure all noiseModels are Gaussian or convert to Gaussian │ │ │ │ │ -65 nfg_._r_e_s_i_z_e(graph.size()); │ │ │ │ │ -66 for (size_t i = 0; i < graph.size(); i++) { │ │ │ │ │ -67 if (graph[i]) { │ │ │ │ │ -68 _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r factor = boost::dynamic_pointer_cast< │ │ │ │ │ -69 _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r>(graph[i]); │ │ │ │ │ -70 auto robust = boost::dynamic_pointer_cast< │ │ │ │ │ -71 _n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t>(factor->noiseModel()); │ │ │ │ │ -72 // if the factor has a robust loss, we remove the robust loss │ │ │ │ │ -73 nfg_[i] = robust ? factor-> cloneWithNewNoiseModel(robust->noise()) : │ │ │ │ │ -factor; │ │ │ │ │ -74 } │ │ │ │ │ -75 } │ │ │ │ │ -76 │ │ │ │ │ -77 // check that known inliers and outliers make sense: │ │ │ │ │ -78 std::vector inconsistentlySpecifiedWeights; // measurements the user │ │ │ │ │ -has incorrectly specified │ │ │ │ │ -79 // to be BOTH known inliers and known outliers │ │ │ │ │ -80 std::set_intersection(params.knownInliers.begin(),params.knownInliers.end(), │ │ │ │ │ -81 params.knownOutliers.begin(),params.knownOutliers.end(), │ │ │ │ │ -82 std::inserter(inconsistentlySpecifiedWeights, │ │ │ │ │ -inconsistentlySpecifiedWeights.begin())); │ │ │ │ │ -83 if(inconsistentlySpecifiedWeights.size() > 0){ // if we have inconsistently │ │ │ │ │ -specified weights, we throw an exception │ │ │ │ │ -84 params.print("params\n"); │ │ │ │ │ -85 throw std::runtime_error("GncOptimizer::constructor: the user has selected │ │ │ │ │ -one or more measurements" │ │ │ │ │ -86 " to be BOTH a known inlier and a known outlier."); │ │ │ │ │ -87 } │ │ │ │ │ -88 // check that known inliers are in the graph │ │ │ │ │ -89 for (size_t i = 0; i < params.knownInliers.size(); i++){ │ │ │ │ │ -90 if( params.knownInliers[i] > nfg_._s_i_z_e()-1 ){ // outside graph │ │ │ │ │ -91 throw std::runtime_error("GncOptimizer::constructor: the user has selected │ │ │ │ │ -one or more measurements" │ │ │ │ │ -92 "that are not in the factor graph to be known inliers."); │ │ │ │ │ -93 } │ │ │ │ │ -94 } │ │ │ │ │ -95 // check that known outliers are in the graph │ │ │ │ │ -96 for (size_t i = 0; i < params.knownOutliers.size(); i++){ │ │ │ │ │ -97 if( params.knownOutliers[i] > nfg_._s_i_z_e()-1 ){ // outside graph │ │ │ │ │ -98 throw std::runtime_error("GncOptimizer::constructor: the user has selected │ │ │ │ │ -one or more measurements" │ │ │ │ │ -99 "that are not in the factor graph to be known outliers."); │ │ │ │ │ -100 } │ │ │ │ │ -101 } │ │ │ │ │ -102 // initialize weights (if we don't have prior knowledge of inliers/outliers │ │ │ │ │ -103 // the weights are all initialized to 1. │ │ │ │ │ -104 weights_ = initializeWeightsFromKnownInliersAndOutliers(); │ │ │ │ │ -105 │ │ │ │ │ -106 // set default barcSq_ (inlier threshold) │ │ │ │ │ -107 double alpha = 0.99; // with this (default) probability, inlier residuals │ │ │ │ │ -are smaller than barcSq_ │ │ │ │ │ -108 _s_e_t_I_n_l_i_e_r_C_o_s_t_T_h_r_e_s_h_o_l_d_s_A_t_P_r_o_b_a_b_i_l_i_t_y(alpha); │ │ │ │ │ -109 } │ │ │ │ │ -110 │ │ │ │ │ -_1_1_7 void _s_e_t_I_n_l_i_e_r_C_o_s_t_T_h_r_e_s_h_o_l_d_s(const double inth) { │ │ │ │ │ -118 barcSq_ = inth * Vector::Ones(nfg_._s_i_z_e()); │ │ │ │ │ -119 } │ │ │ │ │ -120 │ │ │ │ │ -_1_2_5 void _s_e_t_I_n_l_i_e_r_C_o_s_t_T_h_r_e_s_h_o_l_d_s(const Vector& inthVec) { │ │ │ │ │ -126 barcSq_ = inthVec; │ │ │ │ │ -127 } │ │ │ │ │ -128 │ │ │ │ │ -_1_3_2 void _s_e_t_I_n_l_i_e_r_C_o_s_t_T_h_r_e_s_h_o_l_d_s_A_t_P_r_o_b_a_b_i_l_i_t_y(const double alpha) { │ │ │ │ │ -133 barcSq_ = Vector::Ones(nfg_._s_i_z_e()); // initialize │ │ │ │ │ -134 for (size_t k = 0; k < nfg_._s_i_z_e(); k++) { │ │ │ │ │ -135 if (nfg_[k]) { │ │ │ │ │ -136 barcSq_[k] = 0.5 * Chi2inv(alpha, nfg_[k]->dim()); // 0.5 derives from the │ │ │ │ │ -error definition in gtsam │ │ │ │ │ -137 } │ │ │ │ │ -138 } │ │ │ │ │ -139 } │ │ │ │ │ -140 │ │ │ │ │ -_1_4_4 void _s_e_t_W_e_i_g_h_t_s(const Vector w) { │ │ │ │ │ -145 if (size_t(w.size()) != nfg_._s_i_z_e()) { │ │ │ │ │ -146 throw std::runtime_error( │ │ │ │ │ -147 "GncOptimizer::setWeights: the number of specified weights" │ │ │ │ │ -148 " does not match the size of the factor graph."); │ │ │ │ │ -149 } │ │ │ │ │ -150 weights_ = w; │ │ │ │ │ -151 } │ │ │ │ │ +50 typedef _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r _B_a_s_e; │ │ │ │ │ +51 │ │ │ │ │ +52 public: │ │ │ │ │ +53 │ │ │ │ │ +_6_1 static double _f(double z, double u, double p) { │ │ │ │ │ +62 return _l_o_g_S_q_r_t_2_P_I - 0.5 * log(p) + 0.5 * (z - u) * (z - u) * p; │ │ │ │ │ +63 } │ │ │ │ │ +64 │ │ │ │ │ +_7_5 static _H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r _l_i_n_e_a_r_i_z_e(double z, double u, double p, │ │ │ │ │ +76 _K_e_y j1, _K_e_y j2) { │ │ │ │ │ +77 double e = u - z, e2 = e * e; │ │ │ │ │ +78 double c = 2 * _l_o_g_S_q_r_t_2_P_I - log(p) + e2 * p; │ │ │ │ │ +79 Vector g1 = (Vector(1) << -e * p).finished(); │ │ │ │ │ +80 Vector g2 = (Vector(1) << 0.5 / p - 0.5 * e2).finished(); │ │ │ │ │ +81 Matrix G11 = (Matrix(1, 1) << p).finished(); │ │ │ │ │ +82 Matrix G12 = (Matrix(1, 1) << e).finished(); │ │ │ │ │ +83 Matrix G22 = (Matrix(1, 1) << 0.5 / (p * p)).finished(); │ │ │ │ │ +84 return _H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r( │ │ │ │ │ +85 new _H_e_s_s_i_a_n_F_a_c_t_o_r(j1, j2, G11, G12, g1, G22, g2, c)); │ │ │ │ │ +86 } │ │ │ │ │ +87 │ │ │ │ │ +90 │ │ │ │ │ +_9_6 _W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r(double z, _K_e_y meanKey, _K_e_y precisionKey) : │ │ │ │ │ +97 _B_a_s_e(), z_(z), meanKey_(meanKey), precisionKey_(precisionKey) { │ │ │ │ │ +98 } │ │ │ │ │ +99 │ │ │ │ │ +103 │ │ │ │ │ +_1_0_5 _~_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r() override { │ │ │ │ │ +106 } │ │ │ │ │ +107 │ │ │ │ │ +111 │ │ │ │ │ +_1_1_3 void _p_r_i_n_t(const std::string& p = "WhiteNoiseFactor", │ │ │ │ │ +114 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ +115 _B_a_s_e_:_:_p_r_i_n_t(p, keyFormatter); │ │ │ │ │ +116 std::cout << p + ".z: " << z_ << std::endl; │ │ │ │ │ +117 } │ │ │ │ │ +118 │ │ │ │ │ +122 │ │ │ │ │ +_1_2_4 size_t _d_i_m() const override { │ │ │ │ │ +125 return 2; │ │ │ │ │ +126 } │ │ │ │ │ +127 │ │ │ │ │ +_1_2_9 double _e_r_r_o_r(const _V_a_l_u_e_s& x) const override { │ │ │ │ │ +130 return _f(z_, x._a_t(meanKey_), x._a_t(precisionKey_)); │ │ │ │ │ +131 } │ │ │ │ │ +132 │ │ │ │ │ +_1_4_0 virtual Vector _u_n_w_h_i_t_e_n_e_d_E_r_r_o_r(const _V_a_l_u_e_s& x) const { │ │ │ │ │ +141 return (Vector(1) << std::sqrt(2 * _e_r_r_o_r(x))).finished(); │ │ │ │ │ +142 } │ │ │ │ │ +143 │ │ │ │ │ +148// virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const │ │ │ │ │ +{ │ │ │ │ │ +149// const Key j1 = ordering[meanKey_], j2 = ordering[precisionKey_]; │ │ │ │ │ +150// return IndexFactor::shared_ptr(new IndexFactor(j1, j2)); │ │ │ │ │ +151// } │ │ │ │ │ 152 │ │ │ │ │ -_1_5_4 const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& _g_e_t_F_a_c_t_o_r_s() const { return nfg_; } │ │ │ │ │ -155 │ │ │ │ │ -_1_5_7 const _V_a_l_u_e_s& _g_e_t_S_t_a_t_e() const { return state_; } │ │ │ │ │ -158 │ │ │ │ │ -_1_6_0 const GncParameters& _g_e_t_P_a_r_a_m_s() const { return params_;} │ │ │ │ │ -161 │ │ │ │ │ -_1_6_3 const Vector& _g_e_t_W_e_i_g_h_t_s() const { return weights_;} │ │ │ │ │ -164 │ │ │ │ │ -_1_6_6 const Vector& _g_e_t_I_n_l_i_e_r_C_o_s_t_T_h_r_e_s_h_o_l_d_s() const {return barcSq_;} │ │ │ │ │ -167 │ │ │ │ │ -_1_6_9 bool _e_q_u_a_l_s(const _G_n_c_O_p_t_i_m_i_z_e_r& other, double tol = 1e-9) const { │ │ │ │ │ -170 return nfg_._e_q_u_a_l_s(other._g_e_t_F_a_c_t_o_r_s()) │ │ │ │ │ -171 && _e_q_u_a_l(weights_, other._g_e_t_W_e_i_g_h_t_s()) │ │ │ │ │ -172 && params_.equals(other._g_e_t_P_a_r_a_m_s()) │ │ │ │ │ -173 && _e_q_u_a_l(barcSq_, other._g_e_t_I_n_l_i_e_r_C_o_s_t_T_h_r_e_s_h_o_l_d_s()); │ │ │ │ │ -174 } │ │ │ │ │ -175 │ │ │ │ │ -176 Vector initializeWeightsFromKnownInliersAndOutliers() const{ │ │ │ │ │ -177 Vector weights = Vector::Ones(nfg_._s_i_z_e()); │ │ │ │ │ -178 for (size_t i = 0; i < params_.knownOutliers.size(); i++){ │ │ │ │ │ -179 weights[ params_.knownOutliers[i] ] = 0.0; // known to be outliers │ │ │ │ │ -180 } │ │ │ │ │ -181 return weights; │ │ │ │ │ -182 } │ │ │ │ │ -183 │ │ │ │ │ -_1_8_5 _V_a_l_u_e_s _o_p_t_i_m_i_z_e() { │ │ │ │ │ -186 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h graph_initial = this->_m_a_k_e_W_e_i_g_h_t_e_d_G_r_a_p_h(weights_); │ │ │ │ │ -187 _B_a_s_e_O_p_t_i_m_i_z_e_r baseOptimizer( │ │ │ │ │ -188 graph_initial, state_, params_.baseOptimizerParams); │ │ │ │ │ -189 _V_a_l_u_e_s result = baseOptimizer.optimize(); │ │ │ │ │ -190 double mu = _i_n_i_t_i_a_l_i_z_e_M_u(); │ │ │ │ │ -191 double prev_cost = graph_initial._e_r_r_o_r(result); │ │ │ │ │ -192 double cost = 0.0; // this will be updated in the main loop │ │ │ │ │ -193 │ │ │ │ │ -194 // handle the degenerate case that corresponds to small │ │ │ │ │ -195 // maximum residual errors at initialization │ │ │ │ │ -196 // For GM: if residual error is small, mu -> 0 │ │ │ │ │ -197 // For TLS: if residual error is small, mu -> -1 │ │ │ │ │ -198 int nrUnknownInOrOut = nfg_._s_i_z_e() - ( params_.knownInliers.size() + │ │ │ │ │ -params_.knownOutliers.size() ); │ │ │ │ │ -199 // ^^ number of measurements that are not known to be inliers or outliers │ │ │ │ │ -(GNC will need to figure them out) │ │ │ │ │ -200 if (mu <= 0 || nrUnknownInOrOut == 0) { // no need to even call GNC in this │ │ │ │ │ -case │ │ │ │ │ -201 if (mu <= 0 && params_.verbosity >= GncParameters::Verbosity::SUMMARY) { │ │ │ │ │ -202 std::cout << "GNC Optimizer stopped because maximum residual at " │ │ │ │ │ -203 "initialization is small." │ │ │ │ │ -204 << std::endl; │ │ │ │ │ -205 } │ │ │ │ │ -206 if (nrUnknownInOrOut==0 && params_.verbosity >= GncParameters::Verbosity:: │ │ │ │ │ -SUMMARY) { │ │ │ │ │ -207 std::cout << "GNC Optimizer stopped because all measurements are already │ │ │ │ │ -known to be inliers or outliers" │ │ │ │ │ -208 << std::endl; │ │ │ │ │ -209 } │ │ │ │ │ -210 if (params_.verbosity >= GncParameters::Verbosity::MU) { │ │ │ │ │ -211 std::cout << "mu: " << mu << std::endl; │ │ │ │ │ -212 } │ │ │ │ │ -213 if (params_.verbosity >= GncParameters::Verbosity::VALUES) { │ │ │ │ │ -214 result.print("result\n"); │ │ │ │ │ -215 } │ │ │ │ │ -216 return result; │ │ │ │ │ -217 } │ │ │ │ │ -218 │ │ │ │ │ -219 size_t iter; │ │ │ │ │ -220 for (iter = 0; iter < params_.maxIterations; iter++) { │ │ │ │ │ -221 │ │ │ │ │ -222 // display info │ │ │ │ │ -223 if (params_.verbosity >= GncParameters::Verbosity::MU) { │ │ │ │ │ -224 std::cout << "iter: " << iter << std::endl; │ │ │ │ │ -225 std::cout << "mu: " << mu << std::endl; │ │ │ │ │ -226 } │ │ │ │ │ -227 if (params_.verbosity >= GncParameters::Verbosity::WEIGHTS) { │ │ │ │ │ -228 std::cout << "weights: " << weights_ << std::endl; │ │ │ │ │ -229 } │ │ │ │ │ -230 if (params_.verbosity >= GncParameters::Verbosity::VALUES) { │ │ │ │ │ -231 result.print("result\n"); │ │ │ │ │ -232 } │ │ │ │ │ -233 // weights update │ │ │ │ │ -234 weights_ = _c_a_l_c_u_l_a_t_e_W_e_i_g_h_t_s(result, mu); │ │ │ │ │ -235 │ │ │ │ │ -236 // variable/values update │ │ │ │ │ -237 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h graph_iter = this->_m_a_k_e_W_e_i_g_h_t_e_d_G_r_a_p_h(weights_); │ │ │ │ │ -238 _B_a_s_e_O_p_t_i_m_i_z_e_r baseOptimizer_iter( │ │ │ │ │ -239 graph_iter, state_, params_.baseOptimizerParams); │ │ │ │ │ -240 result = baseOptimizer_iter.optimize(); │ │ │ │ │ -241 │ │ │ │ │ -242 // stopping condition │ │ │ │ │ -243 cost = graph_iter._e_r_r_o_r(result); │ │ │ │ │ -244 if (_c_h_e_c_k_C_o_n_v_e_r_g_e_n_c_e(mu, weights_, cost, prev_cost)) { │ │ │ │ │ -245 break; │ │ │ │ │ -246 } │ │ │ │ │ -247 │ │ │ │ │ -248 // update mu │ │ │ │ │ -249 mu = _u_p_d_a_t_e_M_u(mu); │ │ │ │ │ -250 │ │ │ │ │ -251 // get ready for next iteration │ │ │ │ │ -252 prev_cost = cost; │ │ │ │ │ -253 │ │ │ │ │ -254 // display info │ │ │ │ │ -255 if (params_.verbosity >= GncParameters::Verbosity::VALUES) { │ │ │ │ │ -256 std::cout << "previous cost: " << prev_cost << std::endl; │ │ │ │ │ -257 std::cout << "current cost: " << cost << std::endl; │ │ │ │ │ -258 } │ │ │ │ │ -259 } │ │ │ │ │ -260 // display info │ │ │ │ │ -261 if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) { │ │ │ │ │ -262 std::cout << "final iterations: " << iter << std::endl; │ │ │ │ │ -263 std::cout << "final mu: " << mu << std::endl; │ │ │ │ │ -264 std::cout << "previous cost: " << prev_cost << std::endl; │ │ │ │ │ -265 std::cout << "current cost: " << cost << std::endl; │ │ │ │ │ -266 } │ │ │ │ │ -267 if (params_.verbosity >= GncParameters::Verbosity::WEIGHTS) { │ │ │ │ │ -268 std::cout << "final weights: " << weights_ << std::endl; │ │ │ │ │ -269 } │ │ │ │ │ -270 return result; │ │ │ │ │ -271 } │ │ │ │ │ -272 │ │ │ │ │ -_2_7_4 double _i_n_i_t_i_a_l_i_z_e_M_u() const { │ │ │ │ │ -275 │ │ │ │ │ -276 double mu_init = 0.0; │ │ │ │ │ -277 // initialize mu to the value specified in Remark 5 in GNC paper. │ │ │ │ │ -278 switch (params_.lossType) { │ │ │ │ │ -279 case GncLossType::GM: │ │ │ │ │ -280 /* surrogate cost is convex for large mu. initialize as in remark 5 in GNC │ │ │ │ │ -paper. │ │ │ │ │ -281 Since barcSq_ can be different for each factor, we compute the max of the │ │ │ │ │ -quantity in remark 5 in GNC paper │ │ │ │ │ -282 */ │ │ │ │ │ -283 for (size_t k = 0; k < nfg_._s_i_z_e(); k++) { │ │ │ │ │ -284 if (nfg_[k]) { │ │ │ │ │ -285 mu_init = std::max(mu_init, 2 * nfg_[k]->error(state_) / barcSq_[k]); │ │ │ │ │ -286 } │ │ │ │ │ -287 } │ │ │ │ │ -288 return mu_init; // initial mu │ │ │ │ │ -289 case GncLossType::TLS: │ │ │ │ │ -290 /* surrogate cost is convex for mu close to zero. initialize as in remark 5 │ │ │ │ │ -in GNC paper. │ │ │ │ │ -291 degenerate case: 2 * rmax_sq - params_.barcSq < 0 (handled in the main │ │ │ │ │ -loop) │ │ │ │ │ -292 according to remark mu = params_.barcSq / (2 * rmax_sq - params_.barcSq) = │ │ │ │ │ -params_.barcSq/ excessResidual │ │ │ │ │ -293 however, if the denominator is 0 or negative, we return mu = -1 which leads │ │ │ │ │ -to termination of the main GNC loop. │ │ │ │ │ -294 Since barcSq_ can be different for each factor, we look for the minimimum │ │ │ │ │ -(positive) quantity in remark 5 in GNC paper │ │ │ │ │ -295 */ │ │ │ │ │ -296 mu_init = std::numeric_limits::infinity(); │ │ │ │ │ -297 for (size_t k = 0; k < nfg_._s_i_z_e(); k++) { │ │ │ │ │ -298 if (nfg_[k]) { │ │ │ │ │ -299 double rk = nfg_[k]->_e_r_r_o_r(state_); │ │ │ │ │ -300 mu_init = (2 * rk - barcSq_[k]) > 0 ? // if positive, update mu, otherwise │ │ │ │ │ -keep same │ │ │ │ │ -301 std::min(mu_init, barcSq_[k] / (2 * rk - barcSq_[k]) ) : mu_init; │ │ │ │ │ -302 } │ │ │ │ │ -303 } │ │ │ │ │ -304 if (mu_init >= 0 && mu_init < 1e-6){ │ │ │ │ │ -305 mu_init = 1e-6; // if mu ~ 0 (but positive), that means we have │ │ │ │ │ -measurements with large errors, │ │ │ │ │ -306 // i.e., rk > barcSq_[k] and rk very large, hence we threshold to 1e-6 to │ │ │ │ │ -avoid mu = 0 │ │ │ │ │ -307 } │ │ │ │ │ -308 │ │ │ │ │ -309 return mu_init > 0 && !std::isinf(mu_init) ? mu_init : -1; // if mu <= 0 or │ │ │ │ │ -mu = inf, return -1, │ │ │ │ │ -310 // which leads to termination of the main gnc loop. In this case, all │ │ │ │ │ -residuals are already below the threshold │ │ │ │ │ -311 // and there is no need to robustify (TLS = least squares) │ │ │ │ │ -312 default: │ │ │ │ │ -313 throw std::runtime_error( │ │ │ │ │ -314 "GncOptimizer::initializeMu: called with unknown loss type."); │ │ │ │ │ -315 } │ │ │ │ │ -316 } │ │ │ │ │ -317 │ │ │ │ │ -_3_1_9 double _u_p_d_a_t_e_M_u(const double mu) const { │ │ │ │ │ -320 switch (params_.lossType) { │ │ │ │ │ -321 case GncLossType::GM: │ │ │ │ │ -322 // reduce mu, but saturate at 1 (original cost is recovered for mu -> 1) │ │ │ │ │ -323 return std::max(1.0, mu / params_.muStep); │ │ │ │ │ -324 case GncLossType::TLS: │ │ │ │ │ -325 // increases mu at each iteration (original cost is recovered for mu - │ │ │ │ │ -> inf) │ │ │ │ │ -326 return mu * params_.muStep; │ │ │ │ │ -327 default: │ │ │ │ │ -328 throw std::runtime_error( │ │ │ │ │ -329 "GncOptimizer::updateMu: called with unknown loss type."); │ │ │ │ │ -330 } │ │ │ │ │ -331 } │ │ │ │ │ -332 │ │ │ │ │ -_3_3_4 bool _c_h_e_c_k_M_u_C_o_n_v_e_r_g_e_n_c_e(const double mu) const { │ │ │ │ │ -335 bool muConverged = false; │ │ │ │ │ -336 switch (params_.lossType) { │ │ │ │ │ -337 case GncLossType::GM: │ │ │ │ │ -338 muConverged = std::fabs(mu - 1.0) < 1e-9; // mu=1 recovers the original GM │ │ │ │ │ -function │ │ │ │ │ -339 break; │ │ │ │ │ -340 case GncLossType::TLS: │ │ │ │ │ -341 muConverged = false; // for TLS there is no stopping condition on mu (it │ │ │ │ │ -must tend to infinity) │ │ │ │ │ -342 break; │ │ │ │ │ -343 default: │ │ │ │ │ -344 throw std::runtime_error( │ │ │ │ │ -345 "GncOptimizer::checkMuConvergence: called with unknown loss type."); │ │ │ │ │ -346 } │ │ │ │ │ -347 if (muConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY) │ │ │ │ │ -348 std::cout << "muConverged = true " << std::endl; │ │ │ │ │ -349 return muConverged; │ │ │ │ │ -350 } │ │ │ │ │ -351 │ │ │ │ │ -_3_5_3 bool _c_h_e_c_k_C_o_s_t_C_o_n_v_e_r_g_e_n_c_e(const double cost, const double prev_cost) const │ │ │ │ │ +156 │ │ │ │ │ +_1_5_8 boost::shared_ptr _l_i_n_e_a_r_i_z_e(const _V_a_l_u_e_s& x) const override │ │ │ │ │ { │ │ │ │ │ -354 bool costConverged = std::fabs(cost - prev_cost) / std::max(prev_cost, 1e- │ │ │ │ │ -7) │ │ │ │ │ -355 < params_.relativeCostTol; │ │ │ │ │ -356 if (costConverged && params_.verbosity >= GncParameters::Verbosity:: │ │ │ │ │ -SUMMARY){ │ │ │ │ │ -357 std::cout << "checkCostConvergence = true (prev. cost = " << prev_cost │ │ │ │ │ -358 << ", curr. cost = " << cost << ")" << std::endl; │ │ │ │ │ -359 } │ │ │ │ │ -360 return costConverged; │ │ │ │ │ -361 } │ │ │ │ │ -362 │ │ │ │ │ -_3_6_4 bool _c_h_e_c_k_W_e_i_g_h_t_s_C_o_n_v_e_r_g_e_n_c_e(const Vector& weights) const { │ │ │ │ │ -365 bool weightsConverged = false; │ │ │ │ │ -366 switch (params_.lossType) { │ │ │ │ │ -367 case GncLossType::GM: │ │ │ │ │ -368 weightsConverged = false; // for GM, there is no clear binary convergence │ │ │ │ │ -for the weights │ │ │ │ │ -369 break; │ │ │ │ │ -370 case GncLossType::TLS: │ │ │ │ │ -371 weightsConverged = true; │ │ │ │ │ -372 for (int i = 0; i < weights.size(); i++) { │ │ │ │ │ -373 if (std::fabs(weights[i] - std::round(weights[i])) │ │ │ │ │ -374 > params_.weightsTol) { │ │ │ │ │ -375 weightsConverged = false; │ │ │ │ │ -376 break; │ │ │ │ │ -377 } │ │ │ │ │ -378 } │ │ │ │ │ -379 break; │ │ │ │ │ -380 default: │ │ │ │ │ -381 throw std::runtime_error( │ │ │ │ │ -382 "GncOptimizer::checkWeightsConvergence: called with unknown loss type."); │ │ │ │ │ -383 } │ │ │ │ │ -384 if (weightsConverged │ │ │ │ │ -385 && params_.verbosity >= GncParameters::Verbosity::SUMMARY) │ │ │ │ │ -386 std::cout << "weightsConverged = true " << std::endl; │ │ │ │ │ -387 return weightsConverged; │ │ │ │ │ -388 } │ │ │ │ │ -389 │ │ │ │ │ -_3_9_1 bool _c_h_e_c_k_C_o_n_v_e_r_g_e_n_c_e(const double mu, const Vector& weights, │ │ │ │ │ -392 const double cost, const double prev_cost) const { │ │ │ │ │ -393 return _c_h_e_c_k_C_o_s_t_C_o_n_v_e_r_g_e_n_c_e(cost, prev_cost) │ │ │ │ │ -394 || _c_h_e_c_k_W_e_i_g_h_t_s_C_o_n_v_e_r_g_e_n_c_e(weights) || _c_h_e_c_k_M_u_C_o_n_v_e_r_g_e_n_c_e(mu); │ │ │ │ │ -395 } │ │ │ │ │ -396 │ │ │ │ │ -_3_9_8 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h _m_a_k_e_W_e_i_g_h_t_e_d_G_r_a_p_h(const Vector& weights) const { │ │ │ │ │ -399 // make sure all noiseModels are Gaussian or convert to Gaussian │ │ │ │ │ -400 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h newGraph; │ │ │ │ │ -401 newGraph._r_e_s_i_z_e(nfg_._s_i_z_e()); │ │ │ │ │ -402 for (size_t i = 0; i < nfg_._s_i_z_e(); i++) { │ │ │ │ │ -403 if (nfg_[i]) { │ │ │ │ │ -404 auto factor = boost::dynamic_pointer_cast< │ │ │ │ │ -405 _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r>(nfg_[i]); │ │ │ │ │ -406 auto noiseModel = │ │ │ │ │ -407 boost::dynamic_pointer_cast( │ │ │ │ │ -408 factor->noiseModel()); │ │ │ │ │ -409 if (noiseModel) { │ │ │ │ │ -410 Matrix newInfo = weights[i] * noiseModel->information(); │ │ │ │ │ -411 auto newNoiseModel = _n_o_i_s_e_M_o_d_e_l_:_:_G_a_u_s_s_i_a_n_:_:_I_n_f_o_r_m_a_t_i_o_n(newInfo); │ │ │ │ │ -412 newGraph[i] = factor->cloneWithNewNoiseModel(newNoiseModel); │ │ │ │ │ -413 } else { │ │ │ │ │ -414 throw std::runtime_error( │ │ │ │ │ -415 "GncOptimizer::makeWeightedGraph: unexpected non-Gaussian noise model."); │ │ │ │ │ -416 } │ │ │ │ │ -417 } │ │ │ │ │ -418 } │ │ │ │ │ -419 return newGraph; │ │ │ │ │ -420 } │ │ │ │ │ -421 │ │ │ │ │ -_4_2_3 Vector _c_a_l_c_u_l_a_t_e_W_e_i_g_h_t_s(const _V_a_l_u_e_s& currentEstimate, const double mu) { │ │ │ │ │ -424 Vector weights = initializeWeightsFromKnownInliersAndOutliers(); │ │ │ │ │ -425 │ │ │ │ │ -426 // do not update the weights that the user has decided are known inliers │ │ │ │ │ -427 std::vector allWeights; │ │ │ │ │ -428 for (size_t k = 0; k < nfg_._s_i_z_e(); k++) { │ │ │ │ │ -429 allWeights.push_back(k); │ │ │ │ │ -430 } │ │ │ │ │ -431 std::vector knownWeights; │ │ │ │ │ -432 std::set_union(params_.knownInliers.begin(), params_.knownInliers.end(), │ │ │ │ │ -433 params_.knownOutliers.begin(), params_.knownOutliers.end(), │ │ │ │ │ -434 std::inserter(knownWeights, knownWeights.begin())); │ │ │ │ │ -435 │ │ │ │ │ -436 std::vector unknownWeights; │ │ │ │ │ -437 std::set_difference(allWeights.begin(), allWeights.end(), │ │ │ │ │ -438 knownWeights.begin(), knownWeights.end(), │ │ │ │ │ -439 std::inserter(unknownWeights, unknownWeights.begin())); │ │ │ │ │ -440 │ │ │ │ │ -441 // update weights of known inlier/outlier measurements │ │ │ │ │ -442 switch (params_.lossType) { │ │ │ │ │ -443 case GncLossType::GM: { // use eq (12) in GNC paper │ │ │ │ │ -444 for (size_t k : unknownWeights) { │ │ │ │ │ -445 if (nfg_[k]) { │ │ │ │ │ -446 double u2_k = nfg_[k]->_e_r_r_o_r(currentEstimate); // squared (and whitened) │ │ │ │ │ -residual │ │ │ │ │ -447 weights[k] = std::pow( │ │ │ │ │ -448 (mu * barcSq_[k]) / (u2_k + mu * barcSq_[k]), 2); │ │ │ │ │ -449 } │ │ │ │ │ -450 } │ │ │ │ │ -451 return weights; │ │ │ │ │ -452 } │ │ │ │ │ -453 case GncLossType::TLS: { // use eq (14) in GNC paper │ │ │ │ │ -454 for (size_t k : unknownWeights) { │ │ │ │ │ -455 if (nfg_[k]) { │ │ │ │ │ -456 double u2_k = nfg_[k]->_e_r_r_o_r(currentEstimate); // squared (and whitened) │ │ │ │ │ -residual │ │ │ │ │ -457 double upperbound = (mu + 1) / mu * barcSq_[k]; │ │ │ │ │ -458 double lowerbound = mu / (mu + 1) * barcSq_[k]; │ │ │ │ │ -459 weights[k] = std::sqrt(barcSq_[k] * mu * (mu + 1) / u2_k) - mu; │ │ │ │ │ -460 if (u2_k >= upperbound || weights[k] < 0) { │ │ │ │ │ -461 weights[k] = 0; │ │ │ │ │ -462 } else if (u2_k <= lowerbound || weights[k] > 1) { │ │ │ │ │ -463 weights[k] = 1; │ │ │ │ │ -464 } │ │ │ │ │ -465 } │ │ │ │ │ -466 } │ │ │ │ │ -467 return weights; │ │ │ │ │ -468 } │ │ │ │ │ -469 default: │ │ │ │ │ -470 throw std::runtime_error( │ │ │ │ │ -471 "GncOptimizer::calculateWeights: called with unknown loss type."); │ │ │ │ │ -472 } │ │ │ │ │ -473 } │ │ │ │ │ -474}; │ │ │ │ │ -475 │ │ │ │ │ -476} │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Factor Graph consisting of non-linear factors. │ │ │ │ │ +159 double u = x._a_t(meanKey_); │ │ │ │ │ +160 double p = x._a_t(precisionKey_); │ │ │ │ │ +161 _K_e_y j1 = meanKey_; │ │ │ │ │ +162 _K_e_y j2 = precisionKey_; │ │ │ │ │ +163 return _l_i_n_e_a_r_i_z_e(z_, u, p, j1, j2); │ │ │ │ │ +164 } │ │ │ │ │ +165 │ │ │ │ │ +166 // TODO: Frank commented this out for now, can it go? │ │ │ │ │ +167 // /// @return a deep copy of this factor │ │ │ │ │ +168 // gtsam::NonlinearFactor::shared_ptr clone() const override { │ │ │ │ │ +169 // return boost::static_pointer_cast( │ │ │ │ │ +170 // gtsam::NonlinearFactor::shared_ptr(new This(*this))); } │ │ │ │ │ +171 │ │ │ │ │ +173 │ │ │ │ │ +174 }; │ │ │ │ │ +175// WhiteNoiseFactor │ │ │ │ │ +176 │ │ │ │ │ +177}// namespace gtsam │ │ │ │ │ +178 │ │ │ │ │ +_H_e_s_s_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ +Contains the HessianFactor class, a general quadratic factor. │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ +Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l │ │ │ │ │ -bool equal(const T &obj1, const T &obj2, double tol) │ │ │ │ │ -Call equal on the object. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:84 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_r_e_s_i_z_e │ │ │ │ │ -virtual void resize(size_t size) │ │ │ │ │ -Directly resize the number of factors in the graph. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:381 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_s_i_z_e │ │ │ │ │ -size_t size() const │ │ │ │ │ -return the number of factors (including any null factors set by remove() ). │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:326 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_G_a_u_s_s_i_a_n_:_:_I_n_f_o_r_m_a_t_i_o_n │ │ │ │ │ -static shared_ptr Information(const Matrix &M, bool smart=true) │ │ │ │ │ -A Gaussian noise model created by specifying an information matrix. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.cpp:100 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t │ │ │ │ │ -Base class for robust error models The robust M-estimators above simply tell us │ │ │ │ │ -how to re-weight the ... │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:642 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r │ │ │ │ │ -DDeeffiinniittiioonn GncOptimizer.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_c_h_e_c_k_W_e_i_g_h_t_s_C_o_n_v_e_r_g_e_n_c_e │ │ │ │ │ -bool checkWeightsConvergence(const Vector &weights) const │ │ │ │ │ -Check convergence of weights to binary values. │ │ │ │ │ -DDeeffiinniittiioonn GncOptimizer.h:364 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_s_e_t_W_e_i_g_h_t_s │ │ │ │ │ -void setWeights(const Vector w) │ │ │ │ │ -Set weights for each factor. │ │ │ │ │ -DDeeffiinniittiioonn GncOptimizer.h:144 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_c_h_e_c_k_M_u_C_o_n_v_e_r_g_e_n_c_e │ │ │ │ │ -bool checkMuConvergence(const double mu) const │ │ │ │ │ -Check if we have reached the value of mu for which the surrogate loss matches │ │ │ │ │ -the original loss. │ │ │ │ │ -DDeeffiinniittiioonn GncOptimizer.h:334 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_B_a_s_e_O_p_t_i_m_i_z_e_r │ │ │ │ │ -GncParameters::OptimizerType BaseOptimizer │ │ │ │ │ -For each parameter, specify the corresponding optimizer: e.g., │ │ │ │ │ -GaussNewtonParams -> GaussNewtonOptimi... │ │ │ │ │ -DDeeffiinniittiioonn GncOptimizer.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ -Values optimize() │ │ │ │ │ -Compute optimal solution using graduated non-convexity. │ │ │ │ │ -DDeeffiinniittiioonn GncOptimizer.h:185 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_g_e_t_I_n_l_i_e_r_C_o_s_t_T_h_r_e_s_h_o_l_d_s │ │ │ │ │ -const Vector & getInlierCostThresholds() const │ │ │ │ │ -Get the inlier threshold. │ │ │ │ │ -DDeeffiinniittiioonn GncOptimizer.h:166 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_c_h_e_c_k_C_o_s_t_C_o_n_v_e_r_g_e_n_c_e │ │ │ │ │ -bool checkCostConvergence(const double cost, const double prev_cost) const │ │ │ │ │ -Check convergence of relative cost differences. │ │ │ │ │ -DDeeffiinniittiioonn GncOptimizer.h:353 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_g_e_t_S_t_a_t_e │ │ │ │ │ -const Values & getState() const │ │ │ │ │ -Access a copy of the internal values. │ │ │ │ │ -DDeeffiinniittiioonn GncOptimizer.h:157 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_s_e_t_I_n_l_i_e_r_C_o_s_t_T_h_r_e_s_h_o_l_d_s │ │ │ │ │ -void setInlierCostThresholds(const Vector &inthVec) │ │ │ │ │ -Set the maximum weighted residual error for an inlier (one for each factor). │ │ │ │ │ -DDeeffiinniittiioonn GncOptimizer.h:125 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_c_a_l_c_u_l_a_t_e_W_e_i_g_h_t_s │ │ │ │ │ -Vector calculateWeights(const Values ¤tEstimate, const double mu) │ │ │ │ │ -Calculate gnc weights. │ │ │ │ │ -DDeeffiinniittiioonn GncOptimizer.h:423 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_i_n_i_t_i_a_l_i_z_e_M_u │ │ │ │ │ -double initializeMu() const │ │ │ │ │ -Initialize the gnc parameter mu such that loss is approximately convex (remark │ │ │ │ │ -5 in GNC paper). │ │ │ │ │ -DDeeffiinniittiioonn GncOptimizer.h:274 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_u_p_d_a_t_e_M_u │ │ │ │ │ -double updateMu(const double mu) const │ │ │ │ │ -Update the gnc parameter mu to gradually increase nonconvexity. │ │ │ │ │ -DDeeffiinniittiioonn GncOptimizer.h:319 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const GncOptimizer &other, double tol=1e-9) const │ │ │ │ │ -Equals. │ │ │ │ │ -DDeeffiinniittiioonn GncOptimizer.h:169 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_c_h_e_c_k_C_o_n_v_e_r_g_e_n_c_e │ │ │ │ │ -bool checkConvergence(const double mu, const Vector &weights, const double │ │ │ │ │ -cost, const double prev_cost) const │ │ │ │ │ -Check for convergence between consecutive GNC iterations. │ │ │ │ │ -DDeeffiinniittiioonn GncOptimizer.h:391 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_s_e_t_I_n_l_i_e_r_C_o_s_t_T_h_r_e_s_h_o_l_d_s │ │ │ │ │ -void setInlierCostThresholds(const double inth) │ │ │ │ │ -Set the maximum weighted residual error for an inlier (same for all factors). │ │ │ │ │ -DDeeffiinniittiioonn GncOptimizer.h:117 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_s_e_t_I_n_l_i_e_r_C_o_s_t_T_h_r_e_s_h_o_l_d_s_A_t_P_r_o_b_a_b_i_l_i_t_y │ │ │ │ │ -void setInlierCostThresholdsAtProbability(const double alpha) │ │ │ │ │ -Set the maximum weighted residual error threshold by specifying the probability │ │ │ │ │ -alpha that the inlier... │ │ │ │ │ -DDeeffiinniittiioonn GncOptimizer.h:132 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_m_a_k_e_W_e_i_g_h_t_e_d_G_r_a_p_h │ │ │ │ │ -NonlinearFactorGraph makeWeightedGraph(const Vector &weights) const │ │ │ │ │ -Create a graph where each factor is weighted by the gnc weights. │ │ │ │ │ -DDeeffiinniittiioonn GncOptimizer.h:398 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_g_e_t_F_a_c_t_o_r_s │ │ │ │ │ -const NonlinearFactorGraph & getFactors() const │ │ │ │ │ -Access a copy of the internal factor graph. │ │ │ │ │ -DDeeffiinniittiioonn GncOptimizer.h:154 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_g_e_t_W_e_i_g_h_t_s │ │ │ │ │ -const Vector & getWeights() const │ │ │ │ │ -Access a copy of the GNC weights. │ │ │ │ │ -DDeeffiinniittiioonn GncOptimizer.h:163 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_g_e_t_P_a_r_a_m_s │ │ │ │ │ -const GncParameters & getParams() const │ │ │ │ │ -Access a copy of the parameters. │ │ │ │ │ -DDeeffiinniittiioonn GncOptimizer.h:160 │ │ │ │ │ -_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_G_n_c_O_p_t_i_m_i_z_e_r │ │ │ │ │ -GncOptimizer(const NonlinearFactorGraph &graph, const Values &initialValues, │ │ │ │ │ -const GncParameters ¶ms=GncParameters()) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn GncOptimizer.h:59 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r │ │ │ │ │ -A nonlinear sum-of-squares factor with a zero-mean noise model implementing the │ │ │ │ │ -density Templated on... │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:174 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +_g_t_s_a_m_:_:_l_o_g_S_q_r_t_2_P_I │ │ │ │ │ +const double logSqrt2PI │ │ │ │ │ +constant needed below │ │ │ │ │ +DDeeffiinniittiioonn WhiteNoiseFactor.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +A Gaussian factor using the canonical parameters (information form) │ │ │ │ │ +DDeeffiinniittiioonn HessianFactor.h:101 │ │ │ │ │ +_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ boost::shared_ptr< This > shared_ptr │ │ │ │ │ -Noise model. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:186 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const NonlinearFactorGraph &other, double tol=1e-9) const │ │ │ │ │ -Test equality. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactorGraph.cpp:97 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_e_r_r_o_r │ │ │ │ │ -double error(const Values &values) const │ │ │ │ │ -unnormalized error, in the most common case │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactorGraph.cpp:170 │ │ │ │ │ +A shared_ptr to this class. │ │ │ │ │ +DDeeffiinniittiioonn HessianFactor.h:110 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ +Nonlinear factor base class. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.cpp:37 │ │ │ │ │ _g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_a_t │ │ │ │ │ +const ValueType at(Key j) const │ │ │ │ │ +Retrieve a variable by key j. │ │ │ │ │ +DDeeffiinniittiioonn Values-inl.h:361 │ │ │ │ │ +_g_t_s_a_m_:_:_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r │ │ │ │ │ +Binary factor to estimate parameters of zero-mean Gaussian white noise. │ │ │ │ │ +DDeeffiinniittiioonn WhiteNoiseFactor.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e │ │ │ │ │ +boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override │ │ │ │ │ +linearize returns a Hessianfactor that is an approximation of error(p) │ │ │ │ │ +DDeeffiinniittiioonn WhiteNoiseFactor.h:158 │ │ │ │ │ +_g_t_s_a_m_:_:_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r_:_:_~_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r │ │ │ │ │ +~WhiteNoiseFactor() override │ │ │ │ │ +Destructor. │ │ │ │ │ +DDeeffiinniittiioonn WhiteNoiseFactor.h:105 │ │ │ │ │ +_g_t_s_a_m_:_:_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e │ │ │ │ │ +static HessianFactor::shared_ptr linearize(double z, double u, double p, Key │ │ │ │ │ +j1, Key j2) │ │ │ │ │ +linearize returns a Hessianfactor that approximates error Hessian is │ │ │ │ │ +DDeeffiinniittiioonn WhiteNoiseFactor.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &p="WhiteNoiseFactor", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +Print. │ │ │ │ │ +DDeeffiinniittiioonn WhiteNoiseFactor.h:113 │ │ │ │ │ +_g_t_s_a_m_:_:_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r_:_:_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r │ │ │ │ │ +WhiteNoiseFactor(double z, Key meanKey, Key precisionKey) │ │ │ │ │ +Construct from measurement. │ │ │ │ │ +DDeeffiinniittiioonn WhiteNoiseFactor.h:96 │ │ │ │ │ +_g_t_s_a_m_:_:_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r_:_:_u_n_w_h_i_t_e_n_e_d_E_r_r_o_r │ │ │ │ │ +virtual Vector unwhitenedError(const Values &x) const │ │ │ │ │ +Vector of errors "unwhitened" does not make sense for this factor What is meant │ │ │ │ │ +typically is only "e"... │ │ │ │ │ +DDeeffiinniittiioonn WhiteNoiseFactor.h:140 │ │ │ │ │ +_g_t_s_a_m_:_:_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r_:_:_f │ │ │ │ │ +static double f(double z, double u, double p) │ │ │ │ │ +negative log likelihood as a function of mean and precision │ │ │ │ │ +DDeeffiinniittiioonn WhiteNoiseFactor.h:61 │ │ │ │ │ +_g_t_s_a_m_:_:_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r_:_:_e_r_r_o_r │ │ │ │ │ +double error(const Values &x) const override │ │ │ │ │ +Calculate the error of the factor, typically equal to log-likelihood. │ │ │ │ │ +DDeeffiinniittiioonn WhiteNoiseFactor.h:129 │ │ │ │ │ +_g_t_s_a_m_:_:_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r_:_:_d_i_m │ │ │ │ │ +size_t dim() const override │ │ │ │ │ +get the dimension of the factor (number of rows on linearization) │ │ │ │ │ +DDeeffiinniittiioonn WhiteNoiseFactor.h:124 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _G_n_c_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ + * _W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01169.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ExtendedKalmanFilter-inl.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,79 +94,39 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Macros
│ │ │ │ -
NonlinearFactor.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
ExtendedKalmanFilter-inl.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Non-linear factor base classes. │ │ │ │ +

Class to perform generic Kalman Filtering using nonlinear factor graphs. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::NonlinearFactor
 Nonlinear factor base class. More...
 
struct  gtsam::traits< NonlinearFactor >
 traits More...
 
class  gtsam::NoiseModelFactor
 A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density \( P(z|x) \propto exp -0.5*|z-h(x)|^2_C \) Templated on the parameter type X and the values structure Values There is no return type specified for h(x). More...
 
class  gtsam::NoiseModelFactorN< ValueTypes >
 A convenient base class for creating your own NoiseModelFactor with n variables. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Macros

│ │ │ │ -#define NoiseModelFactor1   NoiseModelFactorN
 
│ │ │ │ -#define NoiseModelFactor2   NoiseModelFactorN
 
│ │ │ │ -#define NoiseModelFactor3   NoiseModelFactorN
 
│ │ │ │ -#define NoiseModelFactor4   NoiseModelFactorN
 
│ │ │ │ -#define NoiseModelFactor5   NoiseModelFactorN
 
│ │ │ │ -#define NoiseModelFactor6   NoiseModelFactorN
 
│ │ │ │

Detailed Description

│ │ │ │ -

Non-linear factor base classes.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ +

Class to perform generic Kalman Filtering using nonlinear factor graphs.

│ │ │ │ +
Author
Stephen Williams
│ │ │ │
│ │ │ │ -Gerry Chen
│ │ │ │ +Chris Beall
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,53 +1,23 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s │ │ │ │ │ -NonlinearFactor.h File Reference │ │ │ │ │ -Non-linear factor base classes. _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +ExtendedKalmanFilter-inl.h File Reference │ │ │ │ │ +Class to perform generic Kalman Filtering using nonlinear factor graphs. │ │ │ │ │ +_M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ -  Nonlinear factor base class. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r │ │ │ │ │ - A nonlinear sum-of-squares factor with a zero-mean noise model │ │ │ │ │ -  implementing the density \( P(z|x) \propto exp -0.5*|z-h(x)|^2_C \) │ │ │ │ │ - Templated on the parameter type X and the values structure _V_a_l_u_e_s │ │ │ │ │ - There is no return type specified for h(x). _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_ _V_a_l_u_e_T_y_p_e_s_ _> │ │ │ │ │ -  A convenient base class for creating your own _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r with n │ │ │ │ │ - variables. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -MMaaccrrooss │ │ │ │ │ -#define  NNooiisseeMMooddeellFFaaccttoorr11   NoiseModelFactorN │ │ │ │ │ -  │ │ │ │ │ -#define  NNooiisseeMMooddeellFFaaccttoorr22   NoiseModelFactorN │ │ │ │ │ -  │ │ │ │ │ -#define  NNooiisseeMMooddeellFFaaccttoorr33   NoiseModelFactorN │ │ │ │ │ -  │ │ │ │ │ -#define  NNooiisseeMMooddeellFFaaccttoorr44   NoiseModelFactorN │ │ │ │ │ -  │ │ │ │ │ -#define  NNooiisseeMMooddeellFFaaccttoorr55   NoiseModelFactorN │ │ │ │ │ -  │ │ │ │ │ -#define  NNooiisseeMMooddeellFFaaccttoorr66   NoiseModelFactorN │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Non-linear factor base classes. │ │ │ │ │ +Class to perform generic Kalman Filtering using nonlinear factor graphs. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Gerry Chen │ │ │ │ │ + Stephen Williams │ │ │ │ │ + Chris Beall │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ + * _E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_-_i_n_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01169_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ExtendedKalmanFilter-inl.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,469 +98,164 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
NonlinearFactor.h
│ │ │ │ +
ExtendedKalmanFilter-inl.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20// \callgraph
│ │ │ │ -
21
│ │ │ │ -
22#pragma once
│ │ │ │ -
23
│ │ │ │ -
24#include <gtsam/nonlinear/Values.h>
│ │ │ │ -
25#include <gtsam/linear/NoiseModel.h>
│ │ │ │ -
26#include <gtsam/linear/JacobianFactor.h>
│ │ │ │ -
27#include <gtsam/inference/Factor.h>
│ │ │ │ -
28#include <gtsam/base/OptionalJacobian.h>
│ │ │ │ -
29#include <gtsam/base/utilities.h> // boost::index_sequence
│ │ │ │ -
30
│ │ │ │ -
31#include <boost/serialization/base_object.hpp>
│ │ │ │ -
32
│ │ │ │ -
33namespace gtsam {
│ │ │ │ -
34
│ │ │ │ -
35/* ************************************************************************* */
│ │ │ │ -
36
│ │ │ │ -
│ │ │ │ -
42class GTSAM_EXPORT NonlinearFactor: public Factor {
│ │ │ │ -
43
│ │ │ │ -
44protected:
│ │ │ │ -
45
│ │ │ │ -
46 // Some handy typedefs
│ │ │ │ -
47 typedef Factor Base;
│ │ │ │ -
48 typedef NonlinearFactor This;
│ │ │ │ -
49
│ │ │ │ -
50public:
│ │ │ │ -
51
│ │ │ │ -
52 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
53
│ │ │ │ -
56
│ │ │ │ -
58 NonlinearFactor() {}
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/nonlinear/ExtendedKalmanFilter.h>
│ │ │ │ +
22#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
23#include <gtsam/linear/GaussianBayesNet.h>
│ │ │ │ +
24#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ +
25
│ │ │ │ +
26namespace gtsam {
│ │ │ │ +
27
│ │ │ │ +
28 /* ************************************************************************* */
│ │ │ │ +
29 template<class VALUE>
│ │ │ │ +
30 typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::solve_(
│ │ │ │ +
31 const GaussianFactorGraph& linearFactorGraph,
│ │ │ │ +
32 const Values& linearizationPoint, Key lastKey,
│ │ │ │ +
33 JacobianFactor::shared_ptr* newPrior)
│ │ │ │ +
34 {
│ │ │ │ +
35 // Compute the marginal on the last key
│ │ │ │ +
36 // Solve the linear factor graph, converting it into a linear Bayes Network
│ │ │ │ +
37 // P(x0,x1) = P(x0|x1)*P(x1)
│ │ │ │ +
38 Ordering lastKeyAsOrdering;
│ │ │ │ +
39 lastKeyAsOrdering += lastKey;
│ │ │ │ +
40 const GaussianConditional::shared_ptr marginal =
│ │ │ │ +
41 linearFactorGraph.marginalMultifrontalBayesNet(lastKeyAsOrdering)->front();
│ │ │ │ +
42
│ │ │ │ +
43 // Extract the current estimate of x1,P1
│ │ │ │ +
44 VectorValues result = marginal->solve(VectorValues());
│ │ │ │ +
45 const T& current = linearizationPoint.at<T>(lastKey);
│ │ │ │ +
46 T x = traits<T>::Retract(current, result[lastKey]);
│ │ │ │ +
47
│ │ │ │ +
48 // Create a Jacobian Factor from the root node of the produced Bayes Net.
│ │ │ │ +
49 // This will act as a prior for the next iteration.
│ │ │ │ +
50 // The linearization point of this prior must be moved to the new estimate of x,
│ │ │ │ +
51 // and the key/index needs to be reset to 0, the first key in the next iteration.
│ │ │ │ +
52 assert(marginal->nrFrontals() == 1);
│ │ │ │ +
53 assert(marginal->nrParents() == 0);
│ │ │ │ +
54 *newPrior = boost::make_shared<JacobianFactor>(
│ │ │ │ +
55 marginal->keys().front(),
│ │ │ │ +
56 marginal->getA(marginal->begin()),
│ │ │ │ +
57 marginal->getb() - marginal->getA(marginal->begin()) * result[lastKey],
│ │ │ │ +
58 marginal->get_model());
│ │ │ │
59
│ │ │ │ -
63 template<typename CONTAINER>
│ │ │ │ -
│ │ │ │ -
64 NonlinearFactor(const CONTAINER& keys) :
│ │ │ │ -
65 Base(keys) {}
│ │ │ │ -
│ │ │ │ -
66
│ │ │ │ -
70
│ │ │ │ -
72 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
│ │ │ │ -
73 DefaultKeyFormatter) const override;
│ │ │ │ -
74
│ │ │ │ -
76 virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const;
│ │ │ │ +
60 return x;
│ │ │ │ +
61 }
│ │ │ │ +
62
│ │ │ │ +
63 /* ************************************************************************* */
│ │ │ │ +
64 template <class VALUE>
│ │ │ │ +
65 ExtendedKalmanFilter<VALUE>::ExtendedKalmanFilter(
│ │ │ │ +
66 Key key_initial, T x_initial, noiseModel::Gaussian::shared_ptr P_initial)
│ │ │ │ +
67 : x_(x_initial) // Set the initial linearization point
│ │ │ │ +
68 {
│ │ │ │ +
69 // Create a Jacobian Prior Factor directly P_initial.
│ │ │ │ +
70 // Since x0 is set to the provided mean, the b vector in the prior will be zero
│ │ │ │ +
71 // TODO(Frank): is there a reason why noiseModel is not simply P_initial?
│ │ │ │ +
72 int n = traits<T>::GetDimension(x_initial);
│ │ │ │ +
73 priorFactor_ = JacobianFactor::shared_ptr(
│ │ │ │ +
74 new JacobianFactor(key_initial, P_initial->R(), Vector::Zero(n),
│ │ │ │ +
75 noiseModel::Unit::Create(n)));
│ │ │ │ +
76 }
│ │ │ │
77
│ │ │ │ -
81
│ │ │ │ -
83 virtual ~NonlinearFactor() {}
│ │ │ │ -
84
│ │ │ │ -
97 virtual double error(const Values& c) const;
│ │ │ │ -
98
│ │ │ │ -
103 double error(const HybridValues& c) const override;
│ │ │ │ -
104
│ │ │ │ -
106 virtual size_t dim() const = 0;
│ │ │ │ -
107
│ │ │ │ -
118 virtual bool active(const Values& /*c*/) const { return true; }
│ │ │ │ +
78 /* ************************************************************************* */
│ │ │ │ +
79 template<class VALUE>
│ │ │ │ +
│ │ │ │ +
80 typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::predict(
│ │ │ │ +
81 const NoiseModelFactor& motionFactor) {
│ │ │ │ +
82 const auto keys = motionFactor.keys();
│ │ │ │ +
83
│ │ │ │ +
84 // Create a Gaussian Factor Graph
│ │ │ │ +
85 GaussianFactorGraph linearFactorGraph;
│ │ │ │ +
86
│ │ │ │ +
87 // Add in previous posterior as prior on the first state
│ │ │ │ +
88 linearFactorGraph.push_back(priorFactor_);
│ │ │ │ +
89
│ │ │ │ +
90 // Linearize motion model and add it to the Kalman Filter graph
│ │ │ │ +
91 Values linearizationPoint;
│ │ │ │ +
92 linearizationPoint.insert(keys[0], x_);
│ │ │ │ +
93 linearizationPoint.insert(keys[1], x_); // TODO should this really be x_ ?
│ │ │ │ +
94 linearFactorGraph.push_back(motionFactor.linearize(linearizationPoint));
│ │ │ │ +
95
│ │ │ │ +
96 // Solve the factor graph and update the current state estimate
│ │ │ │ +
97 // and the posterior for the next iteration.
│ │ │ │ +
98 x_ = solve_(linearFactorGraph, linearizationPoint, keys[1], &priorFactor_);
│ │ │ │ +
99
│ │ │ │ +
100 return x_;
│ │ │ │ +
101 }
│ │ │ │ +
│ │ │ │ +
102
│ │ │ │ +
103 /* ************************************************************************* */
│ │ │ │ +
104 template<class VALUE>
│ │ │ │ +
│ │ │ │ +
105 typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::update(
│ │ │ │ +
106 const NoiseModelFactor& measurementFactor) {
│ │ │ │ +
107 const auto keys = measurementFactor.keys();
│ │ │ │ +
108
│ │ │ │ +
109 // Create a Gaussian Factor Graph
│ │ │ │ +
110 GaussianFactorGraph linearFactorGraph;
│ │ │ │ +
111
│ │ │ │ +
112 // Add in the prior on the first state
│ │ │ │ +
113 linearFactorGraph.push_back(priorFactor_);
│ │ │ │ +
114
│ │ │ │ +
115 // Linearize measurement factor and add it to the Kalman Filter graph
│ │ │ │ +
116 Values linearizationPoint;
│ │ │ │ +
117 linearizationPoint.insert(keys[0], x_);
│ │ │ │ +
118 linearFactorGraph.push_back(measurementFactor.linearize(linearizationPoint));
│ │ │ │
119
│ │ │ │ -
121 virtual boost::shared_ptr<GaussianFactor>
│ │ │ │ -
122 linearize(const Values& c) const = 0;
│ │ │ │ +
120 // Solve the factor graph and update the current state estimate
│ │ │ │ +
121 // and the prior factor for the next iteration
│ │ │ │ +
122 x_ = solve_(linearFactorGraph, linearizationPoint, keys[0], &priorFactor_);
│ │ │ │
123
│ │ │ │ -
│ │ │ │ -
130 virtual shared_ptr clone() const {
│ │ │ │ -
131 // TODO: choose better exception to throw here
│ │ │ │ -
132 throw std::runtime_error("NonlinearFactor::clone(): Attempting to clone factor with no clone() implemented!");
│ │ │ │ -
133 return shared_ptr();
│ │ │ │ -
134 }
│ │ │ │ -
│ │ │ │ -
135
│ │ │ │ -
141 virtual shared_ptr rekey(const std::map<Key,Key>& rekey_mapping) const;
│ │ │ │ -
142
│ │ │ │ -
147 virtual shared_ptr rekey(const KeyVector& new_keys) const;
│ │ │ │ -
148
│ │ │ │ -
│ │ │ │ -
153 virtual bool sendable() const {
│ │ │ │ -
154 return true;
│ │ │ │ -
155 }
│ │ │ │ -
│ │ │ │ -
156
│ │ │ │ -
157}; // \class NonlinearFactor
│ │ │ │ -
│ │ │ │ -
158
│ │ │ │ -
│ │ │ │ -
160template<> struct traits<NonlinearFactor> : public Testable<NonlinearFactor> {
│ │ │ │ -
161};
│ │ │ │ -
│ │ │ │ -
162
│ │ │ │ -
163/* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
174class GTSAM_EXPORT NoiseModelFactor: public NonlinearFactor {
│ │ │ │ -
175
│ │ │ │ -
176protected:
│ │ │ │ -
177
│ │ │ │ -
178 // handy typedefs
│ │ │ │ -
179 typedef NonlinearFactor Base;
│ │ │ │ -
180 typedef NoiseModelFactor This;
│ │ │ │ -
181
│ │ │ │ -
182 SharedNoiseModel noiseModel_;
│ │ │ │ -
184public:
│ │ │ │ -
185
│ │ │ │ -
186 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
187
│ │ │ │ -
189 NoiseModelFactor() {}
│ │ │ │ -
190
│ │ │ │ -
192 ~NoiseModelFactor() override {}
│ │ │ │ -
193
│ │ │ │ -
197 template<typename CONTAINER>
│ │ │ │ -
│ │ │ │ -
198 NoiseModelFactor(const SharedNoiseModel& noiseModel, const CONTAINER& keys) :
│ │ │ │ -
199 Base(keys), noiseModel_(noiseModel) {}
│ │ │ │ -
│ │ │ │ -
200
│ │ │ │ -
201protected:
│ │ │ │ -
202
│ │ │ │ -
206 NoiseModelFactor(const SharedNoiseModel& noiseModel) : noiseModel_(noiseModel) {}
│ │ │ │ -
207
│ │ │ │ -
208public:
│ │ │ │ -
209
│ │ │ │ -
211 void print(const std::string& s = "",
│ │ │ │ -
212 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
│ │ │ │ -
213
│ │ │ │ -
215 bool equals(const NonlinearFactor& f, double tol = 1e-9) const override;
│ │ │ │ -
216
│ │ │ │ -
│ │ │ │ -
218 size_t dim() const override {
│ │ │ │ -
219 return noiseModel_->dim();
│ │ │ │ -
220 }
│ │ │ │ -
│ │ │ │ -
221
│ │ │ │ -
│ │ │ │ -
223 const SharedNoiseModel& noiseModel() const {
│ │ │ │ -
224 return noiseModel_;
│ │ │ │ -
225 }
│ │ │ │ -
│ │ │ │ -
226
│ │ │ │ -
233 virtual Vector unwhitenedError(const Values& x,
│ │ │ │ -
234 boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;
│ │ │ │ -
235
│ │ │ │ -
240 Vector whitenedError(const Values& c) const;
│ │ │ │ -
241
│ │ │ │ -
245 Vector unweightedWhitenedError(const Values& c) const;
│ │ │ │ -
246
│ │ │ │ -
250 double weight(const Values& c) const;
│ │ │ │ -
251
│ │ │ │ -
258 double error(const Values& c) const override;
│ │ │ │ -
259
│ │ │ │ -
265 boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override;
│ │ │ │ -
266
│ │ │ │ -
271 shared_ptr cloneWithNewNoiseModel(const SharedNoiseModel newNoise) const;
│ │ │ │ -
272
│ │ │ │ -
273 private:
│ │ │ │ -
275 friend class boost::serialization::access;
│ │ │ │ -
276 template<class ARCHIVE>
│ │ │ │ -
277 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
278 ar & boost::serialization::make_nvp("NonlinearFactor",
│ │ │ │ -
279 boost::serialization::base_object<Base>(*this));
│ │ │ │ -
280 ar & BOOST_SERIALIZATION_NVP(noiseModel_);
│ │ │ │ -
281 }
│ │ │ │ -
282
│ │ │ │ -
283}; // \class NoiseModelFactor
│ │ │ │ -
│ │ │ │ -
284
│ │ │ │ -
285/* ************************************************************************* */
│ │ │ │ -
286namespace detail {
│ │ │ │ -
299template <typename, typename...>
│ │ │ │ -
300struct NoiseModelFactorAliases {};
│ │ │ │ -
301template <typename T1>
│ │ │ │ -
302struct NoiseModelFactorAliases<T1> {
│ │ │ │ -
303 using X = T1;
│ │ │ │ -
304 using X1 = T1;
│ │ │ │ -
305};
│ │ │ │ -
306template <typename T1, typename T2>
│ │ │ │ -
307struct NoiseModelFactorAliases<T1, T2> {
│ │ │ │ -
308 using X1 = T1;
│ │ │ │ -
309 using X2 = T2;
│ │ │ │ -
310};
│ │ │ │ -
311template <typename T1, typename T2, typename T3>
│ │ │ │ -
312struct NoiseModelFactorAliases<T1, T2, T3> {
│ │ │ │ -
313 using X1 = T1;
│ │ │ │ -
314 using X2 = T2;
│ │ │ │ -
315 using X3 = T3;
│ │ │ │ -
316};
│ │ │ │ -
317template <typename T1, typename T2, typename T3, typename T4>
│ │ │ │ -
318struct NoiseModelFactorAliases<T1, T2, T3, T4> {
│ │ │ │ -
319 using X1 = T1;
│ │ │ │ -
320 using X2 = T2;
│ │ │ │ -
321 using X3 = T3;
│ │ │ │ -
322 using X4 = T4;
│ │ │ │ -
323};
│ │ │ │ -
324template <typename T1, typename T2, typename T3, typename T4, typename T5>
│ │ │ │ -
325struct NoiseModelFactorAliases<T1, T2, T3, T4, T5> {
│ │ │ │ -
326 using X1 = T1;
│ │ │ │ -
327 using X2 = T2;
│ │ │ │ -
328 using X3 = T3;
│ │ │ │ -
329 using X4 = T4;
│ │ │ │ -
330 using X5 = T5;
│ │ │ │ -
331};
│ │ │ │ -
332template <typename T1, typename T2, typename T3, typename T4, typename T5,
│ │ │ │ -
333 typename T6, typename... TExtra>
│ │ │ │ -
334struct NoiseModelFactorAliases<T1, T2, T3, T4, T5, T6, TExtra...> {
│ │ │ │ -
335 using X1 = T1;
│ │ │ │ -
336 using X2 = T2;
│ │ │ │ -
337 using X3 = T3;
│ │ │ │ -
338 using X4 = T4;
│ │ │ │ -
339 using X5 = T5;
│ │ │ │ -
340 using X6 = T6;
│ │ │ │ -
341};
│ │ │ │ -
342} // namespace detail
│ │ │ │ -
343
│ │ │ │ -
344/* ************************************************************************* */
│ │ │ │ -
397template <class... ValueTypes>
│ │ │ │ -
│ │ │ │ -
398class NoiseModelFactorN
│ │ │ │ -
399 : public NoiseModelFactor,
│ │ │ │ -
400 public detail::NoiseModelFactorAliases<ValueTypes...> {
│ │ │ │ -
401 public:
│ │ │ │ -
403 enum { N = sizeof...(ValueTypes) };
│ │ │ │ -
404
│ │ │ │ -
405 protected:
│ │ │ │ -
406 using Base = NoiseModelFactor;
│ │ │ │ -
407 using This = NoiseModelFactorN<ValueTypes...>;
│ │ │ │ -
408
│ │ │ │ -
411
│ │ │ │ -
412 template <typename From, typename To>
│ │ │ │ -
413 using IsConvertible =
│ │ │ │ -
414 typename std::enable_if<std::is_convertible<From, To>::value, void>::type;
│ │ │ │ -
415
│ │ │ │ -
416 template <int I>
│ │ │ │ -
417 using IndexIsValid = typename std::enable_if<(I >= 1) && (I <= N),
│ │ │ │ -
418 void>::type; // 1-indexed!
│ │ │ │ -
419
│ │ │ │ -
420 template <typename Container>
│ │ │ │ -
421 using ContainerElementType =
│ │ │ │ -
422 typename std::decay<decltype(*std::declval<Container>().begin())>::type;
│ │ │ │ -
423 template <typename Container>
│ │ │ │ -
424 using IsContainerOfKeys = IsConvertible<ContainerElementType<Container>, Key>;
│ │ │ │ -
425
│ │ │ │ -
427
│ │ │ │ -
428 /* Like std::void_t, except produces `boost::optional<Matrix&>` instead of
│ │ │ │ -
429 * `void`. Used to expand fixed-type parameter-packs with same length as
│ │ │ │ -
430 * ValueTypes. */
│ │ │ │ -
431 template <typename T>
│ │ │ │ -
432 using OptionalMatrix = boost::optional<Matrix&>;
│ │ │ │ -
433
│ │ │ │ -
434 /* Like std::void_t, except produces `Key` instead of `void`. Used to expand
│ │ │ │ -
435 * fixed-type parameter-packs with same length as ValueTypes. */
│ │ │ │ -
436 template <typename T>
│ │ │ │ -
437 using KeyType = Key;
│ │ │ │ -
438
│ │ │ │ -
439 public:
│ │ │ │ -
459 template <int I, typename = IndexIsValid<I>>
│ │ │ │ -
460 using ValueType =
│ │ │ │ -
461 typename std::tuple_element<I - 1, std::tuple<ValueTypes...>>::type;
│ │ │ │ -
462
│ │ │ │ -
463 public:
│ │ │ │ -
464
│ │ │ │ -
467
│ │ │ │ -
469 NoiseModelFactorN() {}
│ │ │ │ -
470
│ │ │ │ -
│ │ │ │ -
478 NoiseModelFactorN(const SharedNoiseModel& noiseModel,
│ │ │ │ -
479 KeyType<ValueTypes>... keys)
│ │ │ │ -
480 : Base(noiseModel, std::array<Key, N>{keys...}) {}
│ │ │ │ -
│ │ │ │ -
481
│ │ │ │ -
489 template <typename CONTAINER = std::initializer_list<Key>,
│ │ │ │ -
490 typename = IsContainerOfKeys<CONTAINER>>
│ │ │ │ -
│ │ │ │ -
491 NoiseModelFactorN(const SharedNoiseModel& noiseModel, CONTAINER keys)
│ │ │ │ -
492 : Base(noiseModel, keys) {
│ │ │ │ -
493 if (keys.size() != N) {
│ │ │ │ -
494 throw std::invalid_argument(
│ │ │ │ -
495 "NoiseModelFactorN: wrong number of keys given");
│ │ │ │ -
496 }
│ │ │ │ -
497 }
│ │ │ │ -
│ │ │ │ -
498
│ │ │ │ -
500
│ │ │ │ -
501 ~NoiseModelFactorN() override {}
│ │ │ │ -
502
│ │ │ │ -
517 template <int I = 1>
│ │ │ │ -
│ │ │ │ -
518 inline Key key() const {
│ │ │ │ -
519 static_assert(I <= N, "Index out of bounds");
│ │ │ │ -
520 return keys_[I - 1];
│ │ │ │ -
521 }
│ │ │ │ -
│ │ │ │ -
522
│ │ │ │ -
525
│ │ │ │ -
│ │ │ │ -
542 Vector unwhitenedError(
│ │ │ │ -
543 const Values& x,
│ │ │ │ -
544 boost::optional<std::vector<Matrix>&> H = boost::none) const override {
│ │ │ │ -
545 return unwhitenedError(boost::mp11::index_sequence_for<ValueTypes...>{}, x,
│ │ │ │ -
546 H);
│ │ │ │ -
547 }
│ │ │ │ -
│ │ │ │ -
548
│ │ │ │ -
552
│ │ │ │ -
575 virtual Vector evaluateError(const ValueTypes&... x,
│ │ │ │ -
576 OptionalMatrix<ValueTypes>... H) const = 0;
│ │ │ │ -
577
│ │ │ │ -
579
│ │ │ │ -
582
│ │ │ │ -
│ │ │ │ -
589 inline Vector evaluateError(const ValueTypes&... x) const {
│ │ │ │ -
590 return evaluateError(x..., OptionalMatrix<ValueTypes>()...);
│ │ │ │ -
591 }
│ │ │ │ -
│ │ │ │ -
592
│ │ │ │ -
597 template <typename... OptionalJacArgs,
│ │ │ │ -
598 typename = IndexIsValid<sizeof...(OptionalJacArgs) + 1>>
│ │ │ │ -
│ │ │ │ -
599 inline Vector evaluateError(const ValueTypes&... x,
│ │ │ │ -
600 OptionalJacArgs&&... H) const {
│ │ │ │ -
601 return evaluateError(x..., std::forward<OptionalJacArgs>(H)...,
│ │ │ │ -
602 boost::none);
│ │ │ │ -
603 }
│ │ │ │ -
│ │ │ │ -
604
│ │ │ │ -
606
│ │ │ │ -
607 private:
│ │ │ │ -
614 template <std::size_t... Indices>
│ │ │ │ -
615 inline Vector unwhitenedError(
│ │ │ │ -
616 boost::mp11::index_sequence<Indices...>, //
│ │ │ │ -
617 const Values& x,
│ │ │ │ -
618 boost::optional<std::vector<Matrix>&> H = boost::none) const {
│ │ │ │ -
619 if (this->active(x)) {
│ │ │ │ -
620 if (H) {
│ │ │ │ -
621 return evaluateError(x.at<ValueTypes>(keys_[Indices])...,
│ │ │ │ -
622 (*H)[Indices]...);
│ │ │ │ -
623 } else {
│ │ │ │ -
624 return evaluateError(x.at<ValueTypes>(keys_[Indices])...);
│ │ │ │ -
625 }
│ │ │ │ -
626 } else {
│ │ │ │ -
627 return Vector::Zero(this->dim());
│ │ │ │ -
628 }
│ │ │ │ -
629 }
│ │ │ │ -
630
│ │ │ │ -
632 friend class boost::serialization::access;
│ │ │ │ -
633 template <class ARCHIVE>
│ │ │ │ -
634 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ -
635 ar& boost::serialization::make_nvp(
│ │ │ │ -
636 "NoiseModelFactor", boost::serialization::base_object<Base>(*this));
│ │ │ │ -
637 }
│ │ │ │ -
638
│ │ │ │ -
639 public:
│ │ │ │ -
642
│ │ │ │ -
643 inline Key key1() const {
│ │ │ │ -
644 return key<1>();
│ │ │ │ -
645 }
│ │ │ │ -
646 template <int I = 2>
│ │ │ │ -
647 inline Key key2() const {
│ │ │ │ -
648 static_assert(I <= N, "Index out of bounds");
│ │ │ │ -
649 return key<2>();
│ │ │ │ -
650 }
│ │ │ │ -
651 template <int I = 3>
│ │ │ │ -
652 inline Key key3() const {
│ │ │ │ -
653 static_assert(I <= N, "Index out of bounds");
│ │ │ │ -
654 return key<3>();
│ │ │ │ -
655 }
│ │ │ │ -
656 template <int I = 4>
│ │ │ │ -
657 inline Key key4() const {
│ │ │ │ -
658 static_assert(I <= N, "Index out of bounds");
│ │ │ │ -
659 return key<4>();
│ │ │ │ -
660 }
│ │ │ │ -
661 template <int I = 5>
│ │ │ │ -
662 inline Key key5() const {
│ │ │ │ -
663 static_assert(I <= N, "Index out of bounds");
│ │ │ │ -
664 return key<5>();
│ │ │ │ -
665 }
│ │ │ │ -
666 template <int I = 6>
│ │ │ │ -
667 inline Key key6() const {
│ │ │ │ -
668 static_assert(I <= N, "Index out of bounds");
│ │ │ │ -
669 return key<6>();
│ │ │ │ -
670 }
│ │ │ │ -
671
│ │ │ │ -
673
│ │ │ │ -
674}; // \class NoiseModelFactorN
│ │ │ │ +
124 return x_;
│ │ │ │ +
125 }
│ │ │ │
│ │ │ │ -
675
│ │ │ │ -
676#define NoiseModelFactor1 NoiseModelFactorN
│ │ │ │ -
677#define NoiseModelFactor2 NoiseModelFactorN
│ │ │ │ -
678#define NoiseModelFactor3 NoiseModelFactorN
│ │ │ │ -
679#define NoiseModelFactor4 NoiseModelFactorN
│ │ │ │ -
680#define NoiseModelFactor5 NoiseModelFactorN
│ │ │ │ -
681#define NoiseModelFactor6 NoiseModelFactorN
│ │ │ │ -
682
│ │ │ │ -
683} // namespace gtsam
│ │ │ │ -
OptionalJacobian.h
Special class for optional Jacobian arguments.
│ │ │ │ -
Factor.h
The base class for all factors.
│ │ │ │ - │ │ │ │ - │ │ │ │ -
Values.h
A non-templated config holding any types of Manifold-group elements.
│ │ │ │ +
126
│ │ │ │ +
127} // namespace gtsam
│ │ │ │ +
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ +
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
ExtendedKalmanFilter.h
Class to perform generic Kalman Filtering using nonlinear factor graphs.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ -
gtsam::Factor
Definition Factor.h:68
│ │ │ │ +
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition FactorGraph.h:186
│ │ │ │
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
│ │ │ │ -
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
│ │ │ │ -
gtsam::Factor::begin
const_iterator begin() const
Iterator at beginning of involved variable keys.
Definition Factor.h:143
│ │ │ │ -
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ -
gtsam::NonlinearFactor::linearize
virtual boost::shared_ptr< GaussianFactor > linearize(const Values &c) const =0
linearize to a GaussianFactor
│ │ │ │ -
gtsam::NonlinearFactor::dim
virtual size_t dim() const =0
get the dimension of the factor (number of rows on linearization)
│ │ │ │ -
gtsam::NonlinearFactor::NonlinearFactor
NonlinearFactor()
Default constructor for I/O only.
Definition NonlinearFactor.h:58
│ │ │ │ -
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition NonlinearFactor.h:118
│ │ │ │ -
gtsam::NonlinearFactor::NonlinearFactor
NonlinearFactor(const CONTAINER &keys)
Constructor from a collection of the keys involved in this factor.
Definition NonlinearFactor.h:64
│ │ │ │ -
gtsam::NonlinearFactor::sendable
virtual bool sendable() const
Should the factor be evaluated in the same thread as the caller This is to enable factors that has sh...
Definition NonlinearFactor.h:153
│ │ │ │ -
gtsam::NonlinearFactor::clone
virtual shared_ptr clone() const
Creates a shared_ptr clone of the factor - needs to be specialized to allow for subclasses.
Definition NonlinearFactor.h:130
│ │ │ │ -
gtsam::NonlinearFactor::~NonlinearFactor
virtual ~NonlinearFactor()
Destructor.
Definition NonlinearFactor.h:83
│ │ │ │ +
gtsam::GaussianConditional::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianConditional.h:46
│ │ │ │ +
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ +
gtsam::JacobianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition JacobianFactor.h:96
│ │ │ │ +
gtsam::ExtendedKalmanFilter
This is a generic Extended Kalman Filter class implemented using nonlinear factors.
Definition ExtendedKalmanFilter.h:45
│ │ │ │
gtsam::NoiseModelFactor
A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
Definition NonlinearFactor.h:174
│ │ │ │ -
gtsam::NoiseModelFactor::NoiseModelFactor
NoiseModelFactor(const SharedNoiseModel &noiseModel, const CONTAINER &keys)
Constructor.
Definition NonlinearFactor.h:198
│ │ │ │ -
gtsam::NoiseModelFactor::~NoiseModelFactor
~NoiseModelFactor() override
Destructor.
Definition NonlinearFactor.h:192
│ │ │ │ -
gtsam::NoiseModelFactor::NoiseModelFactor
NoiseModelFactor(const SharedNoiseModel &noiseModel)
Constructor - only for subclasses, as this does not set keys.
Definition NonlinearFactor.h:206
│ │ │ │ -
gtsam::NoiseModelFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
Noise model.
Definition NonlinearFactor.h:186
│ │ │ │ -
gtsam::NoiseModelFactor::dim
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition NonlinearFactor.h:218
│ │ │ │ -
gtsam::NoiseModelFactor::NoiseModelFactor
NoiseModelFactor()
Default constructor for I/O only.
Definition NonlinearFactor.h:189
│ │ │ │ -
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition NonlinearFactor.h:223
│ │ │ │ -
gtsam::NoiseModelFactor::unwhitenedError
virtual Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const =0
Error function without the NoiseModel, .
│ │ │ │ -
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ -
gtsam::NoiseModelFactorN::evaluateError
Vector evaluateError(const ValueTypes &... x) const
No-Jacobians requested function overload.
Definition NonlinearFactor.h:589
│ │ │ │ -
gtsam::NoiseModelFactorN::NoiseModelFactorN
NoiseModelFactorN(const SharedNoiseModel &noiseModel, CONTAINER keys)
Constructor.
Definition NonlinearFactor.h:491
│ │ │ │ -
gtsam::NoiseModelFactorN::NoiseModelFactorN
NoiseModelFactorN(const SharedNoiseModel &noiseModel, KeyType< ValueTypes >... keys)
Constructor.
Definition NonlinearFactor.h:478
│ │ │ │ -
gtsam::NoiseModelFactorN::unwhitenedError
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
This implements the unwhitenedError virtual function by calling the n-key specific version of evaluat...
Definition NonlinearFactor.h:542
│ │ │ │ -
gtsam::NoiseModelFactorN::NoiseModelFactorN
NoiseModelFactorN()
Default Constructor for I/O.
Definition NonlinearFactor.h:469
│ │ │ │ -
gtsam::NoiseModelFactorN::evaluateError
virtual Vector evaluateError(const ValueTypes &... x, OptionalMatrix< ValueTypes >... H) const =0
Override evaluateError to finish implementing an n-way factor.
│ │ │ │ -
gtsam::NoiseModelFactorN::ValueType
typename std::tuple_element< I - 1, std::tuple< ValueTypes... > >::type ValueType
The type of the I'th template param can be obtained as ValueType.
Definition NonlinearFactor.h:461
│ │ │ │ -
gtsam::NoiseModelFactorN::access
friend class boost::serialization::access
Serialization function.
Definition NonlinearFactor.h:632
│ │ │ │ -
gtsam::NoiseModelFactorN::key
Key key() const
Returns a key.
Definition NonlinearFactor.h:518
│ │ │ │ -
gtsam::NoiseModelFactorN::evaluateError
Vector evaluateError(const ValueTypes &... x, OptionalJacArgs &&... H) const
Some (but not all) optional Jacobians are omitted (function overload)
Definition NonlinearFactor.h:599
│ │ │ │ +
gtsam::NoiseModelFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override
Linearize a non-linearFactorN to get a GaussianFactor, Hence .
Definition NonlinearFactor.cpp:152
│ │ │ │
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
gtsam::Values::at
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
│ │ │ │ +
gtsam::Values::insert
void insert(Key j, const Value &val)
Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present.
Definition Values.cpp:157
│ │ │ │ +
JacobianFactor
In Gaussian factors, the error function returns either the negative log-likelihood,...
│ │ │ │ +
VectorValues
The Factor::error simply extracts the.
│ │ │ │ +
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,574 +1,205 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -NonlinearFactor.h │ │ │ │ │ +ExtendedKalmanFilter-inl.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20// \callgraph │ │ │ │ │ -21 │ │ │ │ │ -22#pragma once │ │ │ │ │ -23 │ │ │ │ │ -24#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_V_a_l_u_e_s_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_N_o_i_s_e_M_o_d_e_l_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ -27#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_._h> │ │ │ │ │ -28#include <_g_t_s_a_m_/_b_a_s_e_/_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h> │ │ │ │ │ -29#include // boost::index_sequence │ │ │ │ │ -30 │ │ │ │ │ -31#include │ │ │ │ │ -32 │ │ │ │ │ -33namespace _g_t_s_a_m { │ │ │ │ │ -34 │ │ │ │ │ -35/* ************************************************************************* │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +25 │ │ │ │ │ +26namespace _g_t_s_a_m { │ │ │ │ │ +27 │ │ │ │ │ +28 /* ************************************************************************* │ │ │ │ │ */ │ │ │ │ │ -36 │ │ │ │ │ -_4_2class GTSAM_EXPORT _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r: public _F_a_c_t_o_r { │ │ │ │ │ -43 │ │ │ │ │ -44protected: │ │ │ │ │ -45 │ │ │ │ │ -46 // Some handy typedefs │ │ │ │ │ -47 typedef _F_a_c_t_o_r _B_a_s_e; │ │ │ │ │ -48 typedef _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ -49 │ │ │ │ │ -50public: │ │ │ │ │ -51 │ │ │ │ │ -52 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -53 │ │ │ │ │ -56 │ │ │ │ │ -_5_8 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r() {} │ │ │ │ │ +29 template │ │ │ │ │ +30 typename ExtendedKalmanFilter::T ExtendedKalmanFilter::solve_( │ │ │ │ │ +31 const GaussianFactorGraph& linearFactorGraph, │ │ │ │ │ +32 const _V_a_l_u_e_s& linearizationPoint, _K_e_y lastKey, │ │ │ │ │ +33 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r* newPrior) │ │ │ │ │ +34 { │ │ │ │ │ +35 // Compute the marginal on the last key │ │ │ │ │ +36 // Solve the linear factor graph, converting it into a linear Bayes Network │ │ │ │ │ +37 // P(x0,x1) = P(x0|x1)*P(x1) │ │ │ │ │ +38 Ordering lastKeyAsOrdering; │ │ │ │ │ +39 lastKeyAsOrdering += lastKey; │ │ │ │ │ +40 const _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r marginal = │ │ │ │ │ +41 linearFactorGraph.marginalMultifrontalBayesNet(lastKeyAsOrdering)->front(); │ │ │ │ │ +42 │ │ │ │ │ +43 // Extract the current estimate of x1,P1 │ │ │ │ │ +44 _V_e_c_t_o_r_V_a_l_u_e_s result = marginal->solve(_V_e_c_t_o_r_V_a_l_u_e_s()); │ │ │ │ │ +45 const T& current = linearizationPoint.at(lastKey); │ │ │ │ │ +46 T x = traits::Retract(current, result[lastKey]); │ │ │ │ │ +47 │ │ │ │ │ +48 // Create a Jacobian Factor from the root node of the produced Bayes Net. │ │ │ │ │ +49 // This will act as a prior for the next iteration. │ │ │ │ │ +50 // The linearization point of this prior must be moved to the new estimate │ │ │ │ │ +of x, │ │ │ │ │ +51 // and the key/index needs to be reset to 0, the first key in the next │ │ │ │ │ +iteration. │ │ │ │ │ +52 assert(marginal->nrFrontals() == 1); │ │ │ │ │ +53 assert(marginal->nrParents() == 0); │ │ │ │ │ +54 *newPrior = boost::make_shared( │ │ │ │ │ +55 marginal->keys().front(), │ │ │ │ │ +56 marginal->getA(marginal->begin()), │ │ │ │ │ +57 marginal->getb() - marginal->getA(marginal->begin()) * result[lastKey], │ │ │ │ │ +58 marginal->get_model()); │ │ │ │ │ 59 │ │ │ │ │ -63 template │ │ │ │ │ -_6_4 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r(const CONTAINER& keys) : │ │ │ │ │ -65 _B_a_s_e(keys) {} │ │ │ │ │ -66 │ │ │ │ │ -70 │ │ │ │ │ -72 void _p_r_i_n_t(const std::string& s = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ -73 DefaultKeyFormatter) const override; │ │ │ │ │ -74 │ │ │ │ │ -76 virtual bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& f, double tol = 1e-9) const; │ │ │ │ │ +60 return x; │ │ │ │ │ +61 } │ │ │ │ │ +62 │ │ │ │ │ +63 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +64 template │ │ │ │ │ +65 ExtendedKalmanFilter::ExtendedKalmanFilter( │ │ │ │ │ +66 _K_e_y key_initial, T x_initial, noiseModel::Gaussian::shared_ptr P_initial) │ │ │ │ │ +67 : x_(x_initial) // Set the initial linearization point │ │ │ │ │ +68 { │ │ │ │ │ +69 // Create a Jacobian Prior Factor directly P_initial. │ │ │ │ │ +70 // Since x0 is set to the provided mean, the b vector in the prior will be │ │ │ │ │ +zero │ │ │ │ │ +71 // TODO(Frank): is there a reason why noiseModel is not simply P_initial? │ │ │ │ │ +72 int n = traits::GetDimension(x_initial); │ │ │ │ │ +73 priorFactor_ = JacobianFactor::shared_ptr( │ │ │ │ │ +74 new _J_a_c_o_b_i_a_n_F_a_c_t_o_r(key_initial, P_initial->R(), Vector::Zero(n), │ │ │ │ │ +75 noiseModel::Unit::Create(n))); │ │ │ │ │ +76 } │ │ │ │ │ 77 │ │ │ │ │ -81 │ │ │ │ │ -_8_3 virtual _~_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r() {} │ │ │ │ │ -84 │ │ │ │ │ -97 virtual double error(const _V_a_l_u_e_s& c) const; │ │ │ │ │ -98 │ │ │ │ │ -103 double error(const _H_y_b_r_i_d_V_a_l_u_e_s& c) const override; │ │ │ │ │ -104 │ │ │ │ │ -_1_0_6 virtual size_t _d_i_m() const = 0; │ │ │ │ │ -107 │ │ │ │ │ -_1_1_8 virtual bool _a_c_t_i_v_e(const _V_a_l_u_e_s& /*c*/) const { return true; } │ │ │ │ │ +78 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +79 template │ │ │ │ │ +_8_0 typename ExtendedKalmanFilter::T _E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_<_V_A_L_U_E_>_:_:_p_r_e_d_i_c_t │ │ │ │ │ +( │ │ │ │ │ +81 const _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r& motionFactor) { │ │ │ │ │ +82 const auto keys = motionFactor._k_e_y_s(); │ │ │ │ │ +83 │ │ │ │ │ +84 // Create a Gaussian Factor Graph │ │ │ │ │ +85 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h linearFactorGraph; │ │ │ │ │ +86 │ │ │ │ │ +87 // Add in previous posterior as prior on the first state │ │ │ │ │ +88 linearFactorGraph._p_u_s_h___b_a_c_k(priorFactor_); │ │ │ │ │ +89 │ │ │ │ │ +90 // Linearize motion model and add it to the Kalman Filter graph │ │ │ │ │ +91 _V_a_l_u_e_s linearizationPoint; │ │ │ │ │ +92 linearizationPoint._i_n_s_e_r_t(keys[0], x_); │ │ │ │ │ +93 linearizationPoint._i_n_s_e_r_t(keys[1], x_); // TODO should this really be x_ ? │ │ │ │ │ +94 linearFactorGraph._p_u_s_h___b_a_c_k(motionFactor._l_i_n_e_a_r_i_z_e(linearizationPoint)); │ │ │ │ │ +95 │ │ │ │ │ +96 // Solve the factor graph and update the current state estimate │ │ │ │ │ +97 // and the posterior for the next iteration. │ │ │ │ │ +98 x_ = solve_(linearFactorGraph, linearizationPoint, keys[1], &priorFactor_); │ │ │ │ │ +99 │ │ │ │ │ +100 return x_; │ │ │ │ │ +101 } │ │ │ │ │ +102 │ │ │ │ │ +103 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +104 template │ │ │ │ │ +_1_0_5 typename ExtendedKalmanFilter::T _E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_<_V_A_L_U_E_>_:_:_u_p_d_a_t_e │ │ │ │ │ +( │ │ │ │ │ +106 const _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r& measurementFactor) { │ │ │ │ │ +107 const auto keys = measurementFactor._k_e_y_s(); │ │ │ │ │ +108 │ │ │ │ │ +109 // Create a Gaussian Factor Graph │ │ │ │ │ +110 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h linearFactorGraph; │ │ │ │ │ +111 │ │ │ │ │ +112 // Add in the prior on the first state │ │ │ │ │ +113 linearFactorGraph._p_u_s_h___b_a_c_k(priorFactor_); │ │ │ │ │ +114 │ │ │ │ │ +115 // Linearize measurement factor and add it to the Kalman Filter graph │ │ │ │ │ +116 _V_a_l_u_e_s linearizationPoint; │ │ │ │ │ +117 linearizationPoint._i_n_s_e_r_t(keys[0], x_); │ │ │ │ │ +118 linearFactorGraph._p_u_s_h___b_a_c_k(measurementFactor._l_i_n_e_a_r_i_z_e │ │ │ │ │ +(linearizationPoint)); │ │ │ │ │ 119 │ │ │ │ │ -121 virtual boost::shared_ptr │ │ │ │ │ -_1_2_2 _l_i_n_e_a_r_i_z_e(const _V_a_l_u_e_s& c) const = 0; │ │ │ │ │ +120 // Solve the factor graph and update the current state estimate │ │ │ │ │ +121 // and the prior factor for the next iteration │ │ │ │ │ +122 x_ = solve_(linearFactorGraph, linearizationPoint, keys[0], &priorFactor_); │ │ │ │ │ 123 │ │ │ │ │ -_1_3_0 virtual shared_ptr _c_l_o_n_e() const { │ │ │ │ │ -131 // TODO: choose better exception to throw here │ │ │ │ │ -132 throw std::runtime_error("NonlinearFactor::clone(): Attempting to clone │ │ │ │ │ -factor with no clone() implemented!"); │ │ │ │ │ -133 return shared_ptr(); │ │ │ │ │ -134 } │ │ │ │ │ -135 │ │ │ │ │ -141 virtual shared_ptr rekey(const std::map& rekey_mapping) const; │ │ │ │ │ -142 │ │ │ │ │ -147 virtual shared_ptr rekey(const _K_e_y_V_e_c_t_o_r& new_keys) const; │ │ │ │ │ -148 │ │ │ │ │ -_1_5_3 virtual bool _s_e_n_d_a_b_l_e() const { │ │ │ │ │ -154 return true; │ │ │ │ │ -155 } │ │ │ │ │ -156 │ │ │ │ │ -157}; // \class NonlinearFactor │ │ │ │ │ -158 │ │ │ │ │ -_1_6_0template<> struct _t_r_a_i_t_s<_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r> : public _T_e_s_t_a_b_l_e │ │ │ │ │ -{ │ │ │ │ │ -161}; │ │ │ │ │ -162 │ │ │ │ │ -163/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -_1_7_4class GTSAM_EXPORT _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r: public _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r { │ │ │ │ │ -175 │ │ │ │ │ -176protected: │ │ │ │ │ -177 │ │ │ │ │ -178 // handy typedefs │ │ │ │ │ -179 typedef _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r _B_a_s_e; │ │ │ │ │ -180 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ -181 │ │ │ │ │ -182 _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l noiseModel_; │ │ │ │ │ -184public: │ │ │ │ │ -185 │ │ │ │ │ -_1_8_6 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -187 │ │ │ │ │ -_1_8_9 _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r() {} │ │ │ │ │ -190 │ │ │ │ │ -_1_9_2 _~_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r() override {} │ │ │ │ │ -193 │ │ │ │ │ -197 template │ │ │ │ │ -_1_9_8 _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r(const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& noiseModel, const CONTAINER& keys) │ │ │ │ │ -: │ │ │ │ │ -199 _B_a_s_e(keys), noiseModel_(noiseModel) {} │ │ │ │ │ -200 │ │ │ │ │ -201protected: │ │ │ │ │ -202 │ │ │ │ │ -_2_0_6 _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r(const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& noiseModel) : noiseModel_ │ │ │ │ │ -(noiseModel) {} │ │ │ │ │ -207 │ │ │ │ │ -208public: │ │ │ │ │ -209 │ │ │ │ │ -211 void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ -212 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override; │ │ │ │ │ -213 │ │ │ │ │ -215 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& f, double tol = 1e-9) const override; │ │ │ │ │ -216 │ │ │ │ │ -_2_1_8 size_t _d_i_m() const override { │ │ │ │ │ -219 return noiseModel_->dim(); │ │ │ │ │ -220 } │ │ │ │ │ -221 │ │ │ │ │ -_2_2_3 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& _n_o_i_s_e_M_o_d_e_l() const { │ │ │ │ │ -224 return noiseModel_; │ │ │ │ │ -225 } │ │ │ │ │ -226 │ │ │ │ │ -_2_3_3 virtual Vector _u_n_w_h_i_t_e_n_e_d_E_r_r_o_r(const _V_a_l_u_e_s& x, │ │ │ │ │ -234 boost::optional&> H = boost::none) const = 0; │ │ │ │ │ -235 │ │ │ │ │ -240 Vector whitenedError(const _V_a_l_u_e_s& c) const; │ │ │ │ │ -241 │ │ │ │ │ -245 Vector unweightedWhitenedError(const _V_a_l_u_e_s& c) const; │ │ │ │ │ -246 │ │ │ │ │ -250 double weight(const _V_a_l_u_e_s& c) const; │ │ │ │ │ -251 │ │ │ │ │ -258 double error(const _V_a_l_u_e_s& c) const override; │ │ │ │ │ -259 │ │ │ │ │ -265 boost::shared_ptr linearize(const _V_a_l_u_e_s& x) const │ │ │ │ │ -override; │ │ │ │ │ -266 │ │ │ │ │ -271 _s_h_a_r_e_d___p_t_r cloneWithNewNoiseModel(const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l newNoise) const; │ │ │ │ │ -272 │ │ │ │ │ -273 private: │ │ │ │ │ -_2_7_5 friend class boost::serialization::access; │ │ │ │ │ -276 template │ │ │ │ │ -277 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -278 ar & boost::serialization::make_nvp("NonlinearFactor", │ │ │ │ │ -279 boost::serialization::base_object(*this)); │ │ │ │ │ -280 ar & BOOST_SERIALIZATION_NVP(noiseModel_); │ │ │ │ │ -281 } │ │ │ │ │ -282 │ │ │ │ │ -283}; // \class NoiseModelFactor │ │ │ │ │ -284 │ │ │ │ │ -285/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -286namespace detail { │ │ │ │ │ -299template │ │ │ │ │ -300struct NoiseModelFactorAliases {}; │ │ │ │ │ -301template │ │ │ │ │ -302struct NoiseModelFactorAliases { │ │ │ │ │ -303 using X = T1; │ │ │ │ │ -304 using X1 = T1; │ │ │ │ │ -305}; │ │ │ │ │ -306template │ │ │ │ │ -307struct NoiseModelFactorAliases { │ │ │ │ │ -308 using X1 = T1; │ │ │ │ │ -309 using X2 = T2; │ │ │ │ │ -310}; │ │ │ │ │ -311template │ │ │ │ │ -312struct NoiseModelFactorAliases { │ │ │ │ │ -313 using X1 = T1; │ │ │ │ │ -314 using X2 = T2; │ │ │ │ │ -315 using X3 = T3; │ │ │ │ │ -316}; │ │ │ │ │ -317template │ │ │ │ │ -318struct NoiseModelFactorAliases { │ │ │ │ │ -319 using X1 = T1; │ │ │ │ │ -320 using X2 = T2; │ │ │ │ │ -321 using X3 = T3; │ │ │ │ │ -322 using X4 = T4; │ │ │ │ │ -323}; │ │ │ │ │ -324template │ │ │ │ │ -325struct NoiseModelFactorAliases { │ │ │ │ │ -326 using X1 = T1; │ │ │ │ │ -327 using X2 = T2; │ │ │ │ │ -328 using X3 = T3; │ │ │ │ │ -329 using X4 = T4; │ │ │ │ │ -330 using X5 = T5; │ │ │ │ │ -331}; │ │ │ │ │ -332template │ │ │ │ │ -334struct NoiseModelFactorAliases { │ │ │ │ │ -335 using X1 = T1; │ │ │ │ │ -336 using X2 = T2; │ │ │ │ │ -337 using X3 = T3; │ │ │ │ │ -338 using X4 = T4; │ │ │ │ │ -339 using X5 = T5; │ │ │ │ │ -340 using X6 = T6; │ │ │ │ │ -341}; │ │ │ │ │ -342} // namespace detail │ │ │ │ │ -343 │ │ │ │ │ -344/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -397template │ │ │ │ │ -_3_9_8class _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ -399 : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r, │ │ │ │ │ -400 public detail::NoiseModelFactorAliases { │ │ │ │ │ -401 public: │ │ │ │ │ -403 enum { N = sizeof...(ValueTypes) }; │ │ │ │ │ -404 │ │ │ │ │ -405 protected: │ │ │ │ │ -406 using _B_a_s_e = _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r; │ │ │ │ │ -407 using This = _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N; │ │ │ │ │ -408 │ │ │ │ │ -411 │ │ │ │ │ -412 template │ │ │ │ │ -413 using IsConvertible = │ │ │ │ │ -414 typename std::enable_if::value, void>::type; │ │ │ │ │ -415 │ │ │ │ │ -416 template │ │ │ │ │ -417 using IndexIsValid = typename std::enable_if<(I >= 1) && (I <= N), │ │ │ │ │ -418 void>::type; // 1-indexed! │ │ │ │ │ -419 │ │ │ │ │ -420 template │ │ │ │ │ -421 using ContainerElementType = │ │ │ │ │ -422 typename std::decay()._b_e_g_i_n())>::type; │ │ │ │ │ -423 template │ │ │ │ │ -424 using IsContainerOfKeys = IsConvertible, │ │ │ │ │ -_K_e_y>; │ │ │ │ │ -425 │ │ │ │ │ -427 │ │ │ │ │ -428 /* Like std::void_t, except produces `boost::optional` instead of │ │ │ │ │ -429 * `void`. Used to expand fixed-type parameter-packs with same length as │ │ │ │ │ -430 * ValueTypes. */ │ │ │ │ │ -431 template │ │ │ │ │ -432 using OptionalMatrix = boost::optional; │ │ │ │ │ -433 │ │ │ │ │ -434 /* Like std::void_t, except produces `Key` instead of `void`. Used to │ │ │ │ │ -expand │ │ │ │ │ -435 * fixed-type parameter-packs with same length as ValueTypes. */ │ │ │ │ │ -436 template │ │ │ │ │ -437 using KeyType = _K_e_y; │ │ │ │ │ -438 │ │ │ │ │ -439 public: │ │ │ │ │ -459 template > │ │ │ │ │ -_4_6_0 using _V_a_l_u_e_T_y_p_e = │ │ │ │ │ -461 typename std::tuple_element>::type; │ │ │ │ │ -462 │ │ │ │ │ -463 public: │ │ │ │ │ -464 │ │ │ │ │ -467 │ │ │ │ │ -_4_6_9 _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N() {} │ │ │ │ │ -470 │ │ │ │ │ -_4_7_8 _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N(const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& _n_o_i_s_e_M_o_d_e_l, │ │ │ │ │ -479 KeyType... _k_e_y_s) │ │ │ │ │ -480 : _B_a_s_e(_n_o_i_s_e_M_o_d_e_l, std::array<_K_e_y, N>{_k_e_y_s...}) {} │ │ │ │ │ -481 │ │ │ │ │ -489 template , │ │ │ │ │ -490 typename = IsContainerOfKeys> │ │ │ │ │ -_4_9_1 _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N(const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& _n_o_i_s_e_M_o_d_e_l, CONTAINER _k_e_y_s) │ │ │ │ │ -492 : _B_a_s_e(_n_o_i_s_e_M_o_d_e_l, _k_e_y_s) { │ │ │ │ │ -493 if (_k_e_y_s.size() != N) { │ │ │ │ │ -494 throw std::invalid_argument( │ │ │ │ │ -495 "NoiseModelFactorN: wrong number of keys given"); │ │ │ │ │ -496 } │ │ │ │ │ -497 } │ │ │ │ │ -498 │ │ │ │ │ -500 │ │ │ │ │ -501 _~_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N() override {} │ │ │ │ │ -502 │ │ │ │ │ -517 template │ │ │ │ │ -_5_1_8 inline _K_e_y _k_e_y() const { │ │ │ │ │ -519 static_assert(I <= N, "Index out of bounds"); │ │ │ │ │ -520 return _k_e_y_s__[I - 1]; │ │ │ │ │ -521 } │ │ │ │ │ -522 │ │ │ │ │ -525 │ │ │ │ │ -_5_4_2 Vector _u_n_w_h_i_t_e_n_e_d_E_r_r_o_r( │ │ │ │ │ -543 const _V_a_l_u_e_s& x, │ │ │ │ │ -544 boost::optional&> H = boost::none) const override { │ │ │ │ │ -545 return _u_n_w_h_i_t_e_n_e_d_E_r_r_o_r(boost::mp11::index_sequence_for{}, x, │ │ │ │ │ -546 H); │ │ │ │ │ -547 } │ │ │ │ │ -548 │ │ │ │ │ -552 │ │ │ │ │ -_5_7_5 virtual Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const ValueTypes&... x, │ │ │ │ │ -576 OptionalMatrix... H) const = 0; │ │ │ │ │ -577 │ │ │ │ │ -579 │ │ │ │ │ -582 │ │ │ │ │ -_5_8_9 inline Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const ValueTypes&... x) const { │ │ │ │ │ -590 return _e_v_a_l_u_a_t_e_E_r_r_o_r(x..., OptionalMatrix()...); │ │ │ │ │ -591 } │ │ │ │ │ -592 │ │ │ │ │ -597 template > │ │ │ │ │ -_5_9_9 inline Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const ValueTypes&... x, │ │ │ │ │ -600 OptionalJacArgs&&... H) const { │ │ │ │ │ -601 return _e_v_a_l_u_a_t_e_E_r_r_o_r(x..., std::forward(H)..., │ │ │ │ │ -602 boost::none); │ │ │ │ │ -603 } │ │ │ │ │ -604 │ │ │ │ │ -606 │ │ │ │ │ -607 private: │ │ │ │ │ -614 template │ │ │ │ │ -615 inline Vector _u_n_w_h_i_t_e_n_e_d_E_r_r_o_r( │ │ │ │ │ -616 boost::mp11::index_sequence, // │ │ │ │ │ -617 const _V_a_l_u_e_s& x, │ │ │ │ │ -618 boost::optional&> H = boost::none) const { │ │ │ │ │ -619 if (this->_a_c_t_i_v_e(x)) { │ │ │ │ │ -620 if (H) { │ │ │ │ │ -621 return _e_v_a_l_u_a_t_e_E_r_r_o_r(x._a_t(_k_e_y_s__[Indices])..., │ │ │ │ │ -622 (*H)[Indices]...); │ │ │ │ │ -623 } else { │ │ │ │ │ -624 return _e_v_a_l_u_a_t_e_E_r_r_o_r(x._a_t(_k_e_y_s__[Indices])...); │ │ │ │ │ -625 } │ │ │ │ │ -626 } else { │ │ │ │ │ -627 return Vector::Zero(this->_d_i_m()); │ │ │ │ │ -628 } │ │ │ │ │ -629 } │ │ │ │ │ -630 │ │ │ │ │ -_6_3_2 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -633 template │ │ │ │ │ -634 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ -635 ar& boost::serialization::make_nvp( │ │ │ │ │ -636 "NoiseModelFactor", boost::serialization::base_object(*this)); │ │ │ │ │ -637 } │ │ │ │ │ -638 │ │ │ │ │ -639 public: │ │ │ │ │ -642 │ │ │ │ │ -643 inline _K_e_y key1() const { │ │ │ │ │ -644 return key<1>(); │ │ │ │ │ -645 } │ │ │ │ │ -646 template │ │ │ │ │ -647 inline _K_e_y key2() const { │ │ │ │ │ -648 static_assert(I <= N, "Index out of bounds"); │ │ │ │ │ -649 return key<2>(); │ │ │ │ │ -650 } │ │ │ │ │ -651 template │ │ │ │ │ -652 inline _K_e_y key3() const { │ │ │ │ │ -653 static_assert(I <= N, "Index out of bounds"); │ │ │ │ │ -654 return key<3>(); │ │ │ │ │ -655 } │ │ │ │ │ -656 template │ │ │ │ │ -657 inline _K_e_y key4() const { │ │ │ │ │ -658 static_assert(I <= N, "Index out of bounds"); │ │ │ │ │ -659 return key<4>(); │ │ │ │ │ -660 } │ │ │ │ │ -661 template │ │ │ │ │ -662 inline _K_e_y key5() const { │ │ │ │ │ -663 static_assert(I <= N, "Index out of bounds"); │ │ │ │ │ -664 return key<5>(); │ │ │ │ │ -665 } │ │ │ │ │ -666 template │ │ │ │ │ -667 inline _K_e_y key6() const { │ │ │ │ │ -668 static_assert(I <= N, "Index out of bounds"); │ │ │ │ │ -669 return key<6>(); │ │ │ │ │ -670 } │ │ │ │ │ -671 │ │ │ │ │ -673 │ │ │ │ │ -674}; // \class NoiseModelFactorN │ │ │ │ │ -675 │ │ │ │ │ -676#define NoiseModelFactor1 NoiseModelFactorN │ │ │ │ │ -677#define NoiseModelFactor2 NoiseModelFactorN │ │ │ │ │ -678#define NoiseModelFactor3 NoiseModelFactorN │ │ │ │ │ -679#define NoiseModelFactor4 NoiseModelFactorN │ │ │ │ │ -680#define NoiseModelFactor5 NoiseModelFactorN │ │ │ │ │ -681#define NoiseModelFactor6 NoiseModelFactorN │ │ │ │ │ -682 │ │ │ │ │ -683} // namespace gtsam │ │ │ │ │ -_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h │ │ │ │ │ -Special class for optional Jacobian arguments. │ │ │ │ │ -_F_a_c_t_o_r_._h │ │ │ │ │ -The base class for all factors. │ │ │ │ │ -_N_o_i_s_e_M_o_d_e_l_._h │ │ │ │ │ -_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ -_V_a_l_u_e_s_._h │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +124 return x_; │ │ │ │ │ +125 } │ │ │ │ │ +126 │ │ │ │ │ +127} // namespace gtsam │ │ │ │ │ +_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ +_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h │ │ │ │ │ +Chordal Bayes Net, the result of eliminating a factor graph. │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ +Non-linear factor base classes. │ │ │ │ │ +_E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_._h │ │ │ │ │ +Class to perform generic Kalman Filtering using nonlinear factor graphs. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ -Aliases. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:68 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_p_u_s_h___b_a_c_k │ │ │ │ │ +IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor) │ │ │ │ │ +Add a factor directly using a shared_ptr. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:186 │ │ │ │ │ _g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s │ │ │ │ │ const KeyVector & keys() const │ │ │ │ │ Access the factor's involved variable keys. │ │ │ │ │ DDeeffiinniittiioonn Factor.h:140 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s__ │ │ │ │ │ -KeyVector keys_ │ │ │ │ │ -The keys involved in this factor. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_b_e_g_i_n │ │ │ │ │ -const_iterator begin() const │ │ │ │ │ -Iterator at beginning of involved variable keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:143 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ -Nonlinear factor base class. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e │ │ │ │ │ -virtual boost::shared_ptr< GaussianFactor > linearize(const Values &c) const =0 │ │ │ │ │ -linearize to a GaussianFactor │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_:_:_d_i_m │ │ │ │ │ -virtual size_t dim() const =0 │ │ │ │ │ -get the dimension of the factor (number of rows on linearization) │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ -NonlinearFactor() │ │ │ │ │ -Default constructor for I/O only. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_:_:_a_c_t_i_v_e │ │ │ │ │ -virtual bool active(const Values &) const │ │ │ │ │ -Checks whether a factor should be used based on a set of values. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:118 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ -NonlinearFactor(const CONTAINER &keys) │ │ │ │ │ -Constructor from a collection of the keys involved in this factor. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_:_:_s_e_n_d_a_b_l_e │ │ │ │ │ -virtual bool sendable() const │ │ │ │ │ -Should the factor be evaluated in the same thread as the caller This is to │ │ │ │ │ -enable factors that has sh... │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:153 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ -virtual shared_ptr clone() const │ │ │ │ │ -Creates a shared_ptr clone of the factor - needs to be specialized to allow for │ │ │ │ │ -subclasses. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:130 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_:_:_~_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ -virtual ~NonlinearFactor() │ │ │ │ │ -Destructor. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:83 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn GaussianConditional.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:96 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r │ │ │ │ │ +This is a generic Extended Kalman Filter class implemented using nonlinear │ │ │ │ │ +factors. │ │ │ │ │ +DDeeffiinniittiioonn ExtendedKalmanFilter.h:45 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r │ │ │ │ │ A nonlinear sum-of-squares factor with a zero-mean noise model implementing the │ │ │ │ │ density Templated on... │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactor.h:174 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r │ │ │ │ │ -NoiseModelFactor(const SharedNoiseModel &noiseModel, const CONTAINER &keys) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:198 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_~_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r │ │ │ │ │ -~NoiseModelFactor() override │ │ │ │ │ -Destructor. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:192 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r │ │ │ │ │ -NoiseModelFactor(const SharedNoiseModel &noiseModel) │ │ │ │ │ -Constructor - only for subclasses, as this does not set keys. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:206 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -Noise model. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:186 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_d_i_m │ │ │ │ │ -size_t dim() const override │ │ │ │ │ -get the dimension of the factor (number of rows on linearization) │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:218 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r │ │ │ │ │ -NoiseModelFactor() │ │ │ │ │ -Default constructor for I/O only. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:189 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_n_o_i_s_e_M_o_d_e_l │ │ │ │ │ -const SharedNoiseModel & noiseModel() const │ │ │ │ │ -access to the noise model │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:223 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_u_n_w_h_i_t_e_n_e_d_E_r_r_o_r │ │ │ │ │ -virtual Vector unwhitenedError(const Values &x, boost::optional< std::vector< │ │ │ │ │ -Matrix > & > H=boost::none) const =0 │ │ │ │ │ -Error function without the NoiseModel, . │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ -A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ -variables. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ -Vector evaluateError(const ValueTypes &... x) const │ │ │ │ │ -No-Jacobians requested function overload. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:589 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ -NoiseModelFactorN(const SharedNoiseModel &noiseModel, CONTAINER keys) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:491 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ -NoiseModelFactorN(const SharedNoiseModel &noiseModel, KeyType< ValueTypes >... │ │ │ │ │ -keys) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:478 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_:_:_u_n_w_h_i_t_e_n_e_d_E_r_r_o_r │ │ │ │ │ -Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > │ │ │ │ │ -& > H=boost::none) const override │ │ │ │ │ -This implements the unwhitenedError virtual function by calling the n-key │ │ │ │ │ -specific version of evaluat... │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:542 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ -NoiseModelFactorN() │ │ │ │ │ -Default Constructor for I/O. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:469 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ -virtual Vector evaluateError(const ValueTypes &... x, OptionalMatrix< │ │ │ │ │ -ValueTypes >... H) const =0 │ │ │ │ │ -Override evaluateError to finish implementing an n-way factor. │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_:_:_V_a_l_u_e_T_y_p_e │ │ │ │ │ -typename std::tuple_element< I - 1, std::tuple< ValueTypes... > >::type │ │ │ │ │ -ValueType │ │ │ │ │ -The type of the I'th template param can be obtained as ValueType. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:461 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:632 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_:_:_k_e_y │ │ │ │ │ -Key key() const │ │ │ │ │ -Returns a key. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:518 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ -Vector evaluateError(const ValueTypes &... x, OptionalJacArgs &&... H) const │ │ │ │ │ -Some (but not all) optional Jacobians are omitted (function overload) │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:599 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e │ │ │ │ │ +boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override │ │ │ │ │ +Linearize a non-linearFactorN to get a GaussianFactor, Hence . │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.cpp:152 │ │ │ │ │ _g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_a_t │ │ │ │ │ -const ValueType at(Key j) const │ │ │ │ │ -Retrieve a variable by key j. │ │ │ │ │ -DDeeffiinniittiioonn Values-inl.h:361 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_i_n_s_e_r_t │ │ │ │ │ +void insert(Key j, const Value &val) │ │ │ │ │ +Add a variable with the given j, throws KeyAlreadyExists if j is already │ │ │ │ │ +present. │ │ │ │ │ +DDeeffiinniittiioonn Values.cpp:157 │ │ │ │ │ +_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ +In Gaussian factors, the error function returns either the negative log- │ │ │ │ │ +likelihood,... │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +The Factor::error simply extracts the. │ │ │ │ │ +_V_a_l_u_e_s │ │ │ │ │ +In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ +a non-linear function... │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ + * _E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_-_i_n_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01172_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/LinearContainerFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ExpressionFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,143 +98,371 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
LinearContainerFactor.h
│ │ │ │ +
ExpressionFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1
│ │ │ │ -
10#pragma once
│ │ │ │ +
1/* ----------------------------------------------------------------------------
│ │ │ │ +
2
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
4 * Atlanta, Georgia 30332-0415
│ │ │ │ +
5 * All Rights Reserved
│ │ │ │ +
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
7
│ │ │ │ +
8 * See LICENSE for the license information
│ │ │ │ +
9
│ │ │ │ +
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
12#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ -
13
│ │ │ │ -
14namespace gtsam {
│ │ │ │ -
15
│ │ │ │ -
16 // Forward declarations
│ │ │ │ -
17 class JacobianFactor;
│ │ │ │ -
18 class HessianFactor;
│ │ │ │ -
19
│ │ │ │ -
│ │ │ │ -
26class GTSAM_EXPORT LinearContainerFactor : public NonlinearFactor {
│ │ │ │ -
27protected:
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <array>
│ │ │ │ +
23#include <gtsam/config.h>
│ │ │ │ +
24#include <gtsam/base/Testable.h>
│ │ │ │ +
25#include <gtsam/nonlinear/Expression.h>
│ │ │ │ +
26#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
27#include <numeric>
│ │ │ │
28
│ │ │ │ -
29 GaussianFactor::shared_ptr factor_;
│ │ │ │ -
30 boost::optional<Values> linearizationPoint_;
│ │ │ │ -
31
│ │ │ │ -
33 LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const boost::optional<Values>& linearizationPoint);
│ │ │ │ -
34
│ │ │ │ -
35 // Some handy typedefs
│ │ │ │ -
36 typedef NonlinearFactor Base;
│ │ │ │ -
37 typedef LinearContainerFactor This;
│ │ │ │ -
38
│ │ │ │ -
39public:
│ │ │ │ -
40
│ │ │ │ -
41 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
42
│ │ │ │ -
44 LinearContainerFactor() {}
│ │ │ │ -
45
│ │ │ │ -
47 LinearContainerFactor(const JacobianFactor& factor, const Values& linearizationPoint = Values());
│ │ │ │ +
29namespace gtsam {
│ │ │ │ +
30
│ │ │ │ +
43template<typename T>
│ │ │ │ +
│ │ │ │ +
44class ExpressionFactor: public NoiseModelFactor {
│ │ │ │ +
45 BOOST_CONCEPT_ASSERT((IsTestable<T>));
│ │ │ │ +
46
│ │ │ │ +
47protected:
│ │ │ │
48
│ │ │ │ -
50 LinearContainerFactor(const HessianFactor& factor, const Values& linearizationPoint = Values());
│ │ │ │ +
49 typedef ExpressionFactor<T> This;
│ │ │ │ +
50 static const int Dim = traits<T>::dimension;
│ │ │ │
51
│ │ │ │ -
53 LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const Values& linearizationPoint = Values());
│ │ │ │ -
54
│ │ │ │ -
55 // Access
│ │ │ │ +
52 T measured_;
│ │ │ │ +
53 Expression<T> expression_;
│ │ │ │ +
54 FastVector<int> dims_;
│ │ │ │ +
55
│ │ │ │
56
│ │ │ │ -
57 const GaussianFactor::shared_ptr& factor() const { return factor_; }
│ │ │ │ -
58
│ │ │ │ -
59 // Testable
│ │ │ │ -
60
│ │ │ │ -
62 void print(const std::string& s = "", const KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const override;
│ │ │ │ -
63
│ │ │ │ -
65 bool equals(const NonlinearFactor& f, double tol = 1e-9) const override;
│ │ │ │ -
66
│ │ │ │ -
67 // NonlinearFactor
│ │ │ │ -
68
│ │ │ │ -
77 double error(const Values& c) const override;
│ │ │ │ +
57 public:
│ │ │ │ +
58 typedef boost::shared_ptr<ExpressionFactor<T> > shared_ptr;
│ │ │ │ +
59
│ │ │ │ +
│ │ │ │ +
67 ExpressionFactor(const SharedNoiseModel& noiseModel, //
│ │ │ │ +
68 const T& measurement, const Expression<T>& expression)
│ │ │ │ +
69 : NoiseModelFactor(noiseModel), measured_(measurement) {
│ │ │ │ +
70 initialize(expression);
│ │ │ │ +
71 }
│ │ │ │ +
│ │ │ │ +
72
│ │ │ │ +
74 ~ExpressionFactor() override {}
│ │ │ │ +
75
│ │ │ │ +
77 const T& measured() const { return measured_; }
│ │ │ │
78
│ │ │ │ -
80 size_t dim() const override;
│ │ │ │ -
81
│ │ │ │ -
83 const boost::optional<Values>& linearizationPoint() const { return linearizationPoint_; }
│ │ │ │ -
84
│ │ │ │ -
101 GaussianFactor::shared_ptr linearize(const Values& c) const override;
│ │ │ │ -
102
│ │ │ │ -
106 GaussianFactor::shared_ptr negateToGaussian() const;
│ │ │ │ -
107
│ │ │ │ -
111 NonlinearFactor::shared_ptr negateToNonlinear() const;
│ │ │ │ -
112
│ │ │ │ -
│ │ │ │ -
119 NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
120 return NonlinearFactor::shared_ptr(new LinearContainerFactor(factor_,linearizationPoint_));
│ │ │ │ -
121 }
│ │ │ │ -
│ │ │ │ -
122
│ │ │ │ -
128 NonlinearFactor::shared_ptr rekey(
│ │ │ │ -
129 const std::map<Key, Key>& rekey_mapping) const override;
│ │ │ │ -
130
│ │ │ │ -
135 NonlinearFactor::shared_ptr rekey(const KeyVector& new_keys) const override;
│ │ │ │ -
136
│ │ │ │ -
138 inline bool hasLinearizationPoint() const { return linearizationPoint_.is_initialized(); }
│ │ │ │ -
139
│ │ │ │ -
143 bool isJacobian() const;
│ │ │ │ -
144 bool isHessian() const;
│ │ │ │ -
145
│ │ │ │ -
147 boost::shared_ptr<JacobianFactor> toJacobian() const;
│ │ │ │ -
148
│ │ │ │ -
150 boost::shared_ptr<HessianFactor> toHessian() const;
│ │ │ │ -
151
│ │ │ │ -
156 static NonlinearFactorGraph ConvertLinearGraph(const GaussianFactorGraph& linear_graph,
│ │ │ │ -
157 const Values& linearizationPoint = Values());
│ │ │ │ -
158
│ │ │ │ -
159 protected:
│ │ │ │ -
160 void initializeLinearizationPoint(const Values& linearizationPoint);
│ │ │ │ -
161
│ │ │ │ -
162 private:
│ │ │ │ -
164 friend class boost::serialization::access;
│ │ │ │ -
165 template<class ARCHIVE>
│ │ │ │ -
166 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
167 ar & boost::serialization::make_nvp("NonlinearFactor",
│ │ │ │ -
168 boost::serialization::base_object<Base>(*this));
│ │ │ │ -
169 ar & BOOST_SERIALIZATION_NVP(factor_);
│ │ │ │ -
170 ar & BOOST_SERIALIZATION_NVP(linearizationPoint_);
│ │ │ │ -
171 }
│ │ │ │ -
172
│ │ │ │ -
173}; // \class LinearContainerFactor
│ │ │ │ +
│ │ │ │ +
80 void print(const std::string& s = "",
│ │ │ │ +
81 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ +
82 NoiseModelFactor::print(s, keyFormatter);
│ │ │ │ +
83 traits<T>::Print(measured_, "ExpressionFactor with measurement: ");
│ │ │ │ +
84 }
│ │ │ │ +
│ │ │ │ +
85
│ │ │ │ +
│ │ │ │ +
87 bool equals(const NonlinearFactor& f, double tol) const override {
│ │ │ │ +
88 const ExpressionFactor* p = dynamic_cast<const ExpressionFactor*>(&f);
│ │ │ │ +
89 return p && NoiseModelFactor::equals(f, tol) &&
│ │ │ │ +
90 traits<T>::Equals(measured_, p->measured_, tol) &&
│ │ │ │ +
91 dims_ == p->dims_;
│ │ │ │ +
92 }
│ │ │ │ +
│ │ │ │ +
93
│ │ │ │ +
│ │ │ │ +
99 Vector unwhitenedError(const Values& x,
│ │ │ │ +
100 boost::optional<std::vector<Matrix>&> H = boost::none) const override {
│ │ │ │ +
101 if (H) {
│ │ │ │ +
102 const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H);
│ │ │ │ +
103 // NOTE(hayk): Doing the reverse, AKA Local(measured_, value) is not correct here
│ │ │ │ +
104 // because it would use the tangent space of the measurement instead of the value.
│ │ │ │ +
105 return -traits<T>::Local(value, measured_);
│ │ │ │ +
106 } else {
│ │ │ │ +
107 const T value = expression_.value(x);
│ │ │ │ +
108 return -traits<T>::Local(value, measured_);
│ │ │ │ +
109 }
│ │ │ │ +
110 }
│ │ │ │ +
│ │ │ │ +
111
│ │ │ │ +
│ │ │ │ +
112 boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override {
│ │ │ │ +
113 // Only linearize if the factor is active
│ │ │ │ +
114 if (!active(x))
│ │ │ │ +
115 return boost::shared_ptr<JacobianFactor>();
│ │ │ │ +
116
│ │ │ │ +
117 // In case noise model is constrained, we need to provide a noise model
│ │ │ │ +
118 SharedDiagonal noiseModel;
│ │ │ │ +
119 if (noiseModel_ && noiseModel_->isConstrained()) {
│ │ │ │ +
120 noiseModel = boost::static_pointer_cast<noiseModel::Constrained>(
│ │ │ │ +
121 noiseModel_)->unit();
│ │ │ │ +
122 }
│ │ │ │ +
123
│ │ │ │ +
124 // Create a writeable JacobianFactor in advance
│ │ │ │ +
125 boost::shared_ptr<JacobianFactor> factor(
│ │ │ │ +
126 new JacobianFactor(keys_, dims_, Dim, noiseModel));
│ │ │ │ +
127
│ │ │ │ +
128 // Wrap keys and VerticalBlockMatrix into structure passed to expression_
│ │ │ │ +
129 VerticalBlockMatrix& Ab = factor->matrixObject();
│ │ │ │ +
130 internal::JacobianMap jacobianMap(keys_, Ab);
│ │ │ │ +
131
│ │ │ │ +
132 // Zero out Jacobian so we can simply add to it
│ │ │ │ +
133 Ab.matrix().setZero();
│ │ │ │ +
134
│ │ │ │ +
135 // Get value and Jacobians, writing directly into JacobianFactor
│ │ │ │ +
136 T value = expression_.valueAndJacobianMap(x, jacobianMap); // <<< Reverse AD happens here !
│ │ │ │ +
137
│ │ │ │ +
138 // Evaluate error and set RHS vector b
│ │ │ │ +
139 Ab(size()).col(0) = traits<T>::Local(value, measured_);
│ │ │ │ +
140
│ │ │ │ +
141 // Whiten the corresponding system, Ab already contains RHS
│ │ │ │ +
142 if (noiseModel_) {
│ │ │ │ +
143 Vector b = Ab(size()).col(0); // need b to be valid for Robust noise models
│ │ │ │ +
144 noiseModel_->WhitenSystem(Ab.matrix(), b);
│ │ │ │ +
145 }
│ │ │ │ +
146
│ │ │ │ +
147 return factor;
│ │ │ │ +
148 }
│ │ │ │
│ │ │ │ +
149
│ │ │ │ +
│ │ │ │ +
151 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
152 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ +
153 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
│ │ │ │ +
154 }
│ │ │ │ +
│ │ │ │ +
155
│ │ │ │ +
156protected:
│ │ │ │ +
157 ExpressionFactor() {}
│ │ │ │ +
159
│ │ │ │ +
│ │ │ │ +
161 ExpressionFactor(const SharedNoiseModel& noiseModel, const T& measurement)
│ │ │ │ +
162 : NoiseModelFactor(noiseModel), measured_(measurement) {
│ │ │ │ +
163 // Not properly initialized yet, need to call initialize
│ │ │ │ +
164 }
│ │ │ │ +
│ │ │ │ +
165
│ │ │ │ +
│ │ │ │ +
167 void initialize(const Expression<T>& expression) {
│ │ │ │ +
168 if (!noiseModel_)
│ │ │ │ +
169 throw std::invalid_argument("ExpressionFactor: no NoiseModel.");
│ │ │ │ +
170 if (noiseModel_->dim() != Dim)
│ │ │ │ +
171 throw std::invalid_argument(
│ │ │ │ +
172 "ExpressionFactor was created with a NoiseModel of incorrect dimension.");
│ │ │ │ +
173 expression_ = expression;
│ │ │ │
174
│ │ │ │ -
175template<> struct traits<LinearContainerFactor> : public Testable<LinearContainerFactor> {};
│ │ │ │ -
176
│ │ │ │ -
177} // \namespace gtsam
│ │ │ │ -
178
│ │ │ │ -
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │ +
175 // Get keys and dimensions for Jacobian matrices
│ │ │ │ +
176 // An Expression is assumed unmutable, so we do this now
│ │ │ │ +
177 if (keys_.empty()) {
│ │ │ │ +
178 // This is the case when called in ExpressionFactor Constructor.
│ │ │ │ +
179 // We then take the keys from the expression in sorted order.
│ │ │ │ +
180 boost::tie(keys_, dims_) = expression_.keysAndDims();
│ │ │ │ +
181 } else {
│ │ │ │ +
182 // This happens with classes derived from BinaryExpressionFactor etc.
│ │ │ │ +
183 // In that case, the keys_ are already defined and we just need to grab
│ │ │ │ +
184 // the dimensions in the correct order.
│ │ │ │ +
185 std::map<Key, int> keyedDims;
│ │ │ │ +
186 expression_.dims(keyedDims);
│ │ │ │ +
187 for (Key key : keys_) dims_.push_back(keyedDims[key]);
│ │ │ │ +
188 }
│ │ │ │ +
189 }
│ │ │ │ +
│ │ │ │ +
190
│ │ │ │ +
│ │ │ │ +
193 virtual Expression<T> expression() const {
│ │ │ │ +
194 throw std::runtime_error("ExpressionFactor::expression not provided: cannot deserialize.");
│ │ │ │ +
195 }
│ │ │ │ +
│ │ │ │ +
196
│ │ │ │ +
197private:
│ │ │ │ +
199 template <class Archive>
│ │ │ │ +
200 void save(Archive& ar, const unsigned int /*version*/) const {
│ │ │ │ +
201 ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor);
│ │ │ │ +
202 ar << boost::serialization::make_nvp("measured_", this->measured_);
│ │ │ │ +
203 }
│ │ │ │ +
204
│ │ │ │ +
207 template <class Archive>
│ │ │ │ +
208 void load(Archive& ar, const unsigned int /*version*/) {
│ │ │ │ +
209 ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(NoiseModelFactor);
│ │ │ │ +
210 ar >> boost::serialization::make_nvp("measured_", this->measured_);
│ │ │ │ +
211 this->initialize(expression());
│ │ │ │ +
212 }
│ │ │ │ +
213
│ │ │ │ +
214 // Indicate that we implement save/load separately, and be friendly to boost
│ │ │ │ +
215 BOOST_SERIALIZATION_SPLIT_MEMBER()
│ │ │ │ +
216
│ │ │ │ +
217 friend class boost::serialization::access;
│ │ │ │ +
218
│ │ │ │ +
219 // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
│ │ │ │ +
220 enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
│ │ │ │ +
221 public:
│ │ │ │ +
222 GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
│ │ │ │ +
223};
│ │ │ │ +
│ │ │ │ +
224// ExpressionFactor
│ │ │ │ +
225
│ │ │ │ +
227template <typename T>
│ │ │ │ +
228struct traits<ExpressionFactor<T> > : public Testable<ExpressionFactor<T> > {};
│ │ │ │ +
229
│ │ │ │ +
241template <typename T, typename... Args>
│ │ │ │ +
│ │ │ │ +
242class ExpressionFactorN : public ExpressionFactor<T> {
│ │ │ │ +
243public:
│ │ │ │ +
244 static const std::size_t NARY_EXPRESSION_SIZE = sizeof...(Args);
│ │ │ │ +
245 using ArrayNKeys = std::array<Key, NARY_EXPRESSION_SIZE>;
│ │ │ │ +
246
│ │ │ │ +
248 ~ExpressionFactorN() override = default;
│ │ │ │ +
249
│ │ │ │ +
250 // Don't provide backward compatible evaluateVector(), due to its problematic
│ │ │ │ +
251 // variable length of optional Jacobian arguments. Vector evaluateError(const
│ │ │ │ +
252 // Args... args,...);
│ │ │ │ +
253
│ │ │ │ +
│ │ │ │ +
256 virtual Expression<T> expression(const ArrayNKeys &keys) const {
│ │ │ │ +
257 throw std::runtime_error(
│ │ │ │ +
258 "ExpressionFactorN::expression not provided: cannot deserialize.");
│ │ │ │ +
259 }
│ │ │ │ +
│ │ │ │ +
260
│ │ │ │ +
261protected:
│ │ │ │ +
263 ExpressionFactorN() = default;
│ │ │ │ +
264
│ │ │ │ +
│ │ │ │ +
266 ExpressionFactorN(const ArrayNKeys &keys, const SharedNoiseModel &noiseModel,
│ │ │ │ +
267 const T &measurement)
│ │ │ │ +
268 : ExpressionFactor<T>(noiseModel, measurement) {
│ │ │ │ +
269 for (const auto &key : keys)
│ │ │ │ +
270 Factor::keys_.push_back(key);
│ │ │ │ +
271 }
│ │ │ │ +
│ │ │ │ +
272
│ │ │ │ +
273private:
│ │ │ │ +
275 Expression<T> expression() const override {
│ │ │ │ +
276 ArrayNKeys keys;
│ │ │ │ +
277 int idx = 0;
│ │ │ │ +
278 for (const auto &key : Factor::keys_)
│ │ │ │ +
279 keys[idx++] = key;
│ │ │ │ +
280 return expression(keys);
│ │ │ │ +
281 }
│ │ │ │ +
282
│ │ │ │ +
283 friend class boost::serialization::access;
│ │ │ │ +
284 template <class ARCHIVE>
│ │ │ │ +
285 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ +
286 ar &boost::serialization::make_nvp(
│ │ │ │ +
287 "ExpressionFactorN",
│ │ │ │ +
288 boost::serialization::base_object<ExpressionFactor<T>>(*this));
│ │ │ │ +
289 }
│ │ │ │ +
290};
│ │ │ │ +
│ │ │ │ +
292template <typename T, typename... Args>
│ │ │ │ +
│ │ │ │ +
293struct traits<ExpressionFactorN<T, Args...>>
│ │ │ │ +
294 : public Testable<ExpressionFactorN<T, Args...>> {};
│ │ │ │ +
│ │ │ │ +
295// ExpressionFactorN
│ │ │ │ +
296
│ │ │ │ +
297
│ │ │ │ +
298#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V42)
│ │ │ │ +
307template <typename T, typename A1, typename A2>
│ │ │ │ +
308class GTSAM_DEPRECATED ExpressionFactor2 : public ExpressionFactorN<T, A1, A2> {
│ │ │ │ +
309public:
│ │ │ │ +
311 ~ExpressionFactor2() override {}
│ │ │ │ +
312
│ │ │ │ +
314 Vector evaluateError(const A1 &a1, const A2 &a2,
│ │ │ │ +
315 boost::optional<Matrix &> H1 = boost::none,
│ │ │ │ +
316 boost::optional<Matrix &> H2 = boost::none) const {
│ │ │ │ +
317 Values values;
│ │ │ │ +
318 values.insert(this->keys_[0], a1);
│ │ │ │ +
319 values.insert(this->keys_[1], a2);
│ │ │ │ +
320 std::vector<Matrix> H(2);
│ │ │ │ +
321 Vector error = ExpressionFactor<T>::unwhitenedError(values, H);
│ │ │ │ +
322 if (H1) (*H1) = H[0];
│ │ │ │ +
323 if (H2) (*H2) = H[1];
│ │ │ │ +
324 return error;
│ │ │ │ +
325 }
│ │ │ │ +
326
│ │ │ │ +
329 virtual Expression<T> expression(Key key1, Key key2) const {
│ │ │ │ +
330 throw std::runtime_error(
│ │ │ │ +
331 "ExpressionFactor2::expression not provided: cannot deserialize.");
│ │ │ │ +
332 }
│ │ │ │ +
333 Expression<T>
│ │ │ │ +
334 expression(const typename ExpressionFactorN<T, A1, A2>::ArrayNKeys &keys)
│ │ │ │ +
335 const override {
│ │ │ │ +
336 return expression(keys[0], keys[1]);
│ │ │ │ +
337 }
│ │ │ │ +
338
│ │ │ │ +
339protected:
│ │ │ │ +
341 ExpressionFactor2() {}
│ │ │ │ +
342
│ │ │ │ +
344 ExpressionFactor2(Key key1, Key key2, const SharedNoiseModel &noiseModel,
│ │ │ │ +
345 const T &measurement)
│ │ │ │ +
346 : ExpressionFactorN<T, A1, A2>({key1, key2}, noiseModel, measurement) {}
│ │ │ │ +
347};
│ │ │ │ +
348// ExpressionFactor2
│ │ │ │ +
349#endif
│ │ │ │ +
350
│ │ │ │ +
351} // namespace gtsam
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
This marks a GTSAM object to require alignment.
Definition types.h:317
│ │ │ │ +
Expression.h
Expressions for Block Automatic Differentiation.
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::IsTestable
A testable concept check that should be placed in applicable unit tests and in generic algorithms.
Definition Testable.h:58
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactor.h:42
│ │ │ │ -
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ -
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition HessianFactor.h:101
│ │ │ │ +
gtsam::VerticalBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43
│ │ │ │ +
gtsam::VerticalBlockMatrix::matrix
const Matrix & matrix() const
Access to full matrix (including any portions excluded by rowStart(), rowEnd(), and firstBlock())
Definition VerticalBlockMatrix.h:188
│ │ │ │ +
gtsam::Factor
Definition Factor.h:68
│ │ │ │ +
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
│ │ │ │ +
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
│ │ │ │ +
gtsam::Factor::size
size_t size() const
Definition Factor.h:157
│ │ │ │
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition JacobianFactor.h:91
│ │ │ │ -
gtsam::LinearContainerFactor
Dummy version of a generic linear factor to be injected into a nonlinear factor graph.
Definition LinearContainerFactor.h:26
│ │ │ │ -
gtsam::LinearContainerFactor::clone
NonlinearFactor::shared_ptr clone() const override
Creates a shared_ptr clone of the factor - needs to be specialized to allow for subclasses.
Definition LinearContainerFactor.h:119
│ │ │ │ -
gtsam::LinearContainerFactor::LinearContainerFactor
LinearContainerFactor()
Default constructor - necessary for serialization.
Definition LinearContainerFactor.h:44
│ │ │ │ -
gtsam::LinearContainerFactor::linearizationPoint
const boost::optional< Values > & linearizationPoint() const
Extract the linearization point used in recalculating error.
Definition LinearContainerFactor.h:83
│ │ │ │ -
gtsam::LinearContainerFactor::hasLinearizationPoint
bool hasLinearizationPoint() const
Casting syntactic sugar.
Definition LinearContainerFactor.h:138
│ │ │ │ +
gtsam::ExpressionFactor
Factor that supports arbitrary expressions via AD.
Definition ExpressionFactor.h:44
│ │ │ │ +
gtsam::ExpressionFactor::unwhitenedError
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
Error function without the NoiseModel, .
Definition ExpressionFactor.h:99
│ │ │ │ +
gtsam::ExpressionFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print relies on Testable traits being defined for T
Definition ExpressionFactor.h:80
│ │ │ │ +
gtsam::ExpressionFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition ExpressionFactor.h:151
│ │ │ │ +
gtsam::ExpressionFactor::expression
virtual Expression< T > expression() const
Recreate expression from keys_ and measured_, used in load below.
Definition ExpressionFactor.h:193
│ │ │ │ +
gtsam::ExpressionFactor::expression_
Expression< T > expression_
the expression that is AD enabled
Definition ExpressionFactor.h:53
│ │ │ │ +
gtsam::ExpressionFactor::measured_
T measured_
the measurement to be compared with the expression
Definition ExpressionFactor.h:52
│ │ │ │ +
gtsam::ExpressionFactor::dims_
FastVector< int > dims_
dimensions of the Jacobian matrices
Definition ExpressionFactor.h:54
│ │ │ │ +
gtsam::ExpressionFactor::measured
const T & measured() const
return the measurement
Definition ExpressionFactor.h:77
│ │ │ │ +
gtsam::ExpressionFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override
linearize to a GaussianFactor
Definition ExpressionFactor.h:112
│ │ │ │ +
gtsam::ExpressionFactor::~ExpressionFactor
~ExpressionFactor() override
Destructor.
Definition ExpressionFactor.h:74
│ │ │ │ +
gtsam::ExpressionFactor::equals
bool equals(const NonlinearFactor &f, double tol) const override
equals relies on Testable traits being defined for T
Definition ExpressionFactor.h:87
│ │ │ │ +
gtsam::ExpressionFactor::ExpressionFactor
ExpressionFactor(const SharedNoiseModel &noiseModel, const T &measurement)
Default constructor, for serialization.
Definition ExpressionFactor.h:161
│ │ │ │ +
gtsam::ExpressionFactor::ExpressionFactor
ExpressionFactor(const SharedNoiseModel &noiseModel, const T &measurement, const Expression< T > &expression)
Constructor: creates a factor from a measurement and measurement function.
Definition ExpressionFactor.h:67
│ │ │ │ +
gtsam::ExpressionFactor::initialize
void initialize(const Expression< T > &expression)
Initialize with constructor arguments.
Definition ExpressionFactor.h:167
│ │ │ │ +
gtsam::Expression
Expression class that supports automatic differentiation.
Definition Expression.h:48
│ │ │ │ +
gtsam::ExpressionFactorN
N-ary variadic template for ExpressionFactor meant as a base class for N-ary factors.
Definition ExpressionFactor.h:242
│ │ │ │ +
gtsam::ExpressionFactorN::expression
virtual Expression< T > expression(const ArrayNKeys &keys) const
Recreate expression from given keys_ and measured_, used in load Needed to deserialize a derived fact...
Definition ExpressionFactor.h:256
│ │ │ │ +
gtsam::ExpressionFactorN::~ExpressionFactorN
~ExpressionFactorN() override=default
Destructor.
│ │ │ │ +
gtsam::ExpressionFactorN::ExpressionFactorN
ExpressionFactorN()=default
Default constructor, for serialization.
│ │ │ │ +
gtsam::ExpressionFactorN::ExpressionFactorN
ExpressionFactorN(const ArrayNKeys &keys, const SharedNoiseModel &noiseModel, const T &measurement)
Constructor takes care of keys, but still need to call initialize.
Definition ExpressionFactor.h:266
│ │ │ │
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ -
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │ +
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition NonlinearFactor.h:118
│ │ │ │ +
gtsam::NoiseModelFactor
A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
Definition NonlinearFactor.h:174
│ │ │ │ +
gtsam::NoiseModelFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition NonlinearFactor.cpp:74
│ │ │ │ +
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Check if two factors are equal.
Definition NonlinearFactor.cpp:82
│ │ │ │ +
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition NonlinearFactor.h:223
│ │ │ │
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
JacobianFactor
In Gaussian factors, the error function returns either the negative log-likelihood,...
│ │ │ │
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │ +
NoiseModelFactor
noise model to the factor, and calculates the error by asking the user to implement the method
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,185 +1,480 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -LinearContainerFactor.h │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -1 │ │ │ │ │ -10#pragma once │ │ │ │ │ +ExpressionFactor.h │ │ │ │ │ +1/* --------------------------------------------------------------------------- │ │ │ │ │ +- │ │ │ │ │ +2 │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +5 * All Rights Reserved │ │ │ │ │ +6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +7 │ │ │ │ │ +8 * See LICENSE for the license information │ │ │ │ │ +9 │ │ │ │ │ +10 * ------------------------------------------------------------------------- │ │ │ │ │ +- */ │ │ │ │ │ 11 │ │ │ │ │ -12#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -13 │ │ │ │ │ -14namespace _g_t_s_a_m { │ │ │ │ │ -15 │ │ │ │ │ -16 // Forward declarations │ │ │ │ │ -17 class _J_a_c_o_b_i_a_n_F_a_c_t_o_r; │ │ │ │ │ -18 class HessianFactor; │ │ │ │ │ -19 │ │ │ │ │ -_2_6class GTSAM_EXPORT _L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r : public _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r { │ │ │ │ │ -27protected: │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include │ │ │ │ │ +23#include │ │ │ │ │ +24#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_E_x_p_r_e_s_s_i_o_n_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +27#include │ │ │ │ │ 28 │ │ │ │ │ -29 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r factor_; │ │ │ │ │ -30 boost::optional linearizationPoint_; │ │ │ │ │ -31 │ │ │ │ │ -33 _L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r& factor, const │ │ │ │ │ -boost::optional& linearizationPoint); │ │ │ │ │ -34 │ │ │ │ │ -35 // Some handy typedefs │ │ │ │ │ -36 typedef _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r _B_a_s_e; │ │ │ │ │ -37 typedef _L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ -38 │ │ │ │ │ -39public: │ │ │ │ │ -40 │ │ │ │ │ -41 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -42 │ │ │ │ │ -_4_4 _L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r() {} │ │ │ │ │ -45 │ │ │ │ │ -47 _L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r(const _J_a_c_o_b_i_a_n_F_a_c_t_o_r& factor, const _V_a_l_u_e_s& │ │ │ │ │ -linearizationPoint = _V_a_l_u_e_s()); │ │ │ │ │ +29namespace _g_t_s_a_m { │ │ │ │ │ +30 │ │ │ │ │ +43template │ │ │ │ │ +_4_4class _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r { │ │ │ │ │ +45 BOOST_CONCEPT_ASSERT((_I_s_T_e_s_t_a_b_l_e_<_T_>)); │ │ │ │ │ +46 │ │ │ │ │ +47protected: │ │ │ │ │ 48 │ │ │ │ │ -50 _L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r(const _H_e_s_s_i_a_n_F_a_c_t_o_r& factor, const _V_a_l_u_e_s& │ │ │ │ │ -linearizationPoint = _V_a_l_u_e_s()); │ │ │ │ │ +49 typedef _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_<_T_> _T_h_i_s; │ │ │ │ │ +50 static const int Dim = _t_r_a_i_t_s_<_T_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ 51 │ │ │ │ │ -53 _L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r& factor, const │ │ │ │ │ -_V_a_l_u_e_s& linearizationPoint = _V_a_l_u_e_s()); │ │ │ │ │ -54 │ │ │ │ │ -55 // Access │ │ │ │ │ +_5_2 T _m_e_a_s_u_r_e_d__; │ │ │ │ │ +_5_3 _E_x_p_r_e_s_s_i_o_n_<_T_> _e_x_p_r_e_s_s_i_o_n__; │ │ │ │ │ +_5_4 _F_a_s_t_V_e_c_t_o_r_<_i_n_t_> _d_i_m_s__; │ │ │ │ │ +55 │ │ │ │ │ 56 │ │ │ │ │ -57 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r& factor() const { return factor_; } │ │ │ │ │ -58 │ │ │ │ │ -59 // Testable │ │ │ │ │ -60 │ │ │ │ │ -62 void print(const std::string& s = "", const KeyFormatter& keyFormatter = │ │ │ │ │ -gtsam::DefaultKeyFormatter) const override; │ │ │ │ │ -63 │ │ │ │ │ -65 bool equals(const NonlinearFactor& f, double tol = 1e-9) const override; │ │ │ │ │ -66 │ │ │ │ │ -67 // NonlinearFactor │ │ │ │ │ -68 │ │ │ │ │ -77 double error(const _V_a_l_u_e_s& c) const override; │ │ │ │ │ +57 public: │ │ │ │ │ +58 typedef boost::shared_ptr > shared_ptr; │ │ │ │ │ +59 │ │ │ │ │ +_6_7 _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r(const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& _n_o_i_s_e_M_o_d_e_l, // │ │ │ │ │ +68 const T& measurement, const _E_x_p_r_e_s_s_i_o_n_<_T_>& _e_x_p_r_e_s_s_i_o_n) │ │ │ │ │ +69 : _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r(_n_o_i_s_e_M_o_d_e_l), _m_e_a_s_u_r_e_d__(measurement) { │ │ │ │ │ +70 _i_n_i_t_i_a_l_i_z_e(_e_x_p_r_e_s_s_i_o_n); │ │ │ │ │ +71 } │ │ │ │ │ +72 │ │ │ │ │ +_7_4 _~_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r() override {} │ │ │ │ │ +75 │ │ │ │ │ +_7_7 const T& _m_e_a_s_u_r_e_d() const { return _m_e_a_s_u_r_e_d__; } │ │ │ │ │ 78 │ │ │ │ │ -80 size_t dim() const override; │ │ │ │ │ -81 │ │ │ │ │ -_8_3 const boost::optional& _l_i_n_e_a_r_i_z_a_t_i_o_n_P_o_i_n_t() const { return │ │ │ │ │ -linearizationPoint_; } │ │ │ │ │ -84 │ │ │ │ │ -101 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r linearize(const _V_a_l_u_e_s& c) const override; │ │ │ │ │ -102 │ │ │ │ │ -106 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r negateToGaussian() const; │ │ │ │ │ -107 │ │ │ │ │ -111 NonlinearFactor::shared_ptr negateToNonlinear() const; │ │ │ │ │ -112 │ │ │ │ │ -_1_1_9 NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -120 return NonlinearFactor::shared_ptr(new _L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r │ │ │ │ │ -(factor_,linearizationPoint_)); │ │ │ │ │ -121 } │ │ │ │ │ -122 │ │ │ │ │ -128 NonlinearFactor::shared_ptr rekey( │ │ │ │ │ -129 const std::map& rekey_mapping) const override; │ │ │ │ │ -130 │ │ │ │ │ -135 NonlinearFactor::shared_ptr rekey(const _K_e_y_V_e_c_t_o_r& new_keys) const │ │ │ │ │ -override; │ │ │ │ │ -136 │ │ │ │ │ -_1_3_8 inline bool _h_a_s_L_i_n_e_a_r_i_z_a_t_i_o_n_P_o_i_n_t() const { return │ │ │ │ │ -linearizationPoint_.is_initialized(); } │ │ │ │ │ -139 │ │ │ │ │ -143 bool isJacobian() const; │ │ │ │ │ -144 bool isHessian() const; │ │ │ │ │ -145 │ │ │ │ │ -147 boost::shared_ptr toJacobian() const; │ │ │ │ │ -148 │ │ │ │ │ -150 boost::shared_ptr toHessian() const; │ │ │ │ │ -151 │ │ │ │ │ -156 static _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h ConvertLinearGraph(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& │ │ │ │ │ -linear_graph, │ │ │ │ │ -157 const _V_a_l_u_e_s& linearizationPoint = _V_a_l_u_e_s()); │ │ │ │ │ -158 │ │ │ │ │ -159 protected: │ │ │ │ │ -160 void initializeLinearizationPoint(const _V_a_l_u_e_s& linearizationPoint); │ │ │ │ │ -161 │ │ │ │ │ -162 private: │ │ │ │ │ -_1_6_4 friend class boost::serialization::access; │ │ │ │ │ -165 template │ │ │ │ │ -166 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -167 ar & boost::serialization::make_nvp("NonlinearFactor", │ │ │ │ │ -168 boost::serialization::base_object(*this)); │ │ │ │ │ -169 ar & BOOST_SERIALIZATION_NVP(factor_); │ │ │ │ │ -170 ar & BOOST_SERIALIZATION_NVP(linearizationPoint_); │ │ │ │ │ -171 } │ │ │ │ │ -172 │ │ │ │ │ -173}; // \class LinearContainerFactor │ │ │ │ │ +_8_0 void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ +81 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ +82 _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_p_r_i_n_t(s, keyFormatter); │ │ │ │ │ +83 _t_r_a_i_t_s_<_T_>_:_:_P_r_i_n_t(_m_e_a_s_u_r_e_d__, "ExpressionFactor with measurement: "); │ │ │ │ │ +84 } │ │ │ │ │ +85 │ │ │ │ │ +_8_7 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& f, double tol) const override { │ │ │ │ │ +88 const _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r* p = dynamic_cast(&f); │ │ │ │ │ +89 return p && _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_e_q_u_a_l_s(f, tol) && │ │ │ │ │ +90 _t_r_a_i_t_s_<_T_>_:_:_E_q_u_a_l_s(_m_e_a_s_u_r_e_d__, p->_m_e_a_s_u_r_e_d__, tol) && │ │ │ │ │ +91 _d_i_m_s__ == p->_d_i_m_s__; │ │ │ │ │ +92 } │ │ │ │ │ +93 │ │ │ │ │ +_9_9 Vector _u_n_w_h_i_t_e_n_e_d_E_r_r_o_r(const _V_a_l_u_e_s& x, │ │ │ │ │ +100 boost::optional&> H = boost::none) const override { │ │ │ │ │ +101 if (H) { │ │ │ │ │ +102 const T value = _e_x_p_r_e_s_s_i_o_n__.valueAndDerivatives(x, _k_e_y_s__, _d_i_m_s__, *H); │ │ │ │ │ +103 // NOTE(hayk): Doing the reverse, AKA Local(measured_, value) is not │ │ │ │ │ +correct here │ │ │ │ │ +104 // because it would use the tangent space of the measurement instead of the │ │ │ │ │ +value. │ │ │ │ │ +105 return -_t_r_a_i_t_s_<_T_>_:_:_L_o_c_a_l(value, _m_e_a_s_u_r_e_d__); │ │ │ │ │ +106 } else { │ │ │ │ │ +107 const T value = _e_x_p_r_e_s_s_i_o_n__.value(x); │ │ │ │ │ +108 return -_t_r_a_i_t_s_<_T_>_:_:_L_o_c_a_l(value, _m_e_a_s_u_r_e_d__); │ │ │ │ │ +109 } │ │ │ │ │ +110 } │ │ │ │ │ +111 │ │ │ │ │ +_1_1_2 boost::shared_ptr _l_i_n_e_a_r_i_z_e(const _V_a_l_u_e_s& x) const override │ │ │ │ │ +{ │ │ │ │ │ +113 // Only linearize if the factor is active │ │ │ │ │ +114 if (!_a_c_t_i_v_e(x)) │ │ │ │ │ +115 return boost::shared_ptr(); │ │ │ │ │ +116 │ │ │ │ │ +117 // In case noise model is constrained, we need to provide a noise model │ │ │ │ │ +118 SharedDiagonal _n_o_i_s_e_M_o_d_e_l; │ │ │ │ │ +119 if (noiseModel_ && noiseModel_->isConstrained()) { │ │ │ │ │ +120 _n_o_i_s_e_M_o_d_e_l = boost::static_pointer_cast( │ │ │ │ │ +121 noiseModel_)->unit(); │ │ │ │ │ +122 } │ │ │ │ │ +123 │ │ │ │ │ +124 // Create a writeable JacobianFactor in advance │ │ │ │ │ +125 boost::shared_ptr factor( │ │ │ │ │ +126 new _J_a_c_o_b_i_a_n_F_a_c_t_o_r(_k_e_y_s__, _d_i_m_s__, Dim, _n_o_i_s_e_M_o_d_e_l)); │ │ │ │ │ +127 │ │ │ │ │ +128 // Wrap keys and VerticalBlockMatrix into structure passed to expression_ │ │ │ │ │ +129 _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x& Ab = factor->matrixObject(); │ │ │ │ │ +130 internal::JacobianMap jacobianMap(_k_e_y_s__, Ab); │ │ │ │ │ +131 │ │ │ │ │ +132 // Zero out Jacobian so we can simply add to it │ │ │ │ │ +133 Ab._m_a_t_r_i_x().setZero(); │ │ │ │ │ +134 │ │ │ │ │ +135 // Get value and Jacobians, writing directly into JacobianFactor │ │ │ │ │ +136 T value = _e_x_p_r_e_s_s_i_o_n__.valueAndJacobianMap(x, jacobianMap); // <<< Reverse │ │ │ │ │ +AD happens here ! │ │ │ │ │ +137 │ │ │ │ │ +138 // Evaluate error and set RHS vector b │ │ │ │ │ +139 Ab(_s_i_z_e()).col(0) = _t_r_a_i_t_s_<_T_>_:_:_L_o_c_a_l(value, _m_e_a_s_u_r_e_d__); │ │ │ │ │ +140 │ │ │ │ │ +141 // Whiten the corresponding system, Ab already contains RHS │ │ │ │ │ +142 if (noiseModel_) { │ │ │ │ │ +143 Vector b = Ab(_s_i_z_e()).col(0); // need b to be valid for Robust noise models │ │ │ │ │ +144 noiseModel_->WhitenSystem(Ab._m_a_t_r_i_x(), b); │ │ │ │ │ +145 } │ │ │ │ │ +146 │ │ │ │ │ +147 return factor; │ │ │ │ │ +148 } │ │ │ │ │ +149 │ │ │ │ │ +_1_5_1 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +152 return boost::static_pointer_cast( │ │ │ │ │ +153 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); │ │ │ │ │ +154 } │ │ │ │ │ +155 │ │ │ │ │ +156protected: │ │ │ │ │ +157 _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r() {} │ │ │ │ │ +159 │ │ │ │ │ +_1_6_1 _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r(const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& _n_o_i_s_e_M_o_d_e_l, const T& measurement) │ │ │ │ │ +162 : _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r(_n_o_i_s_e_M_o_d_e_l), _m_e_a_s_u_r_e_d__(measurement) { │ │ │ │ │ +163 // Not properly initialized yet, need to call initialize │ │ │ │ │ +164 } │ │ │ │ │ +165 │ │ │ │ │ +_1_6_7 void _i_n_i_t_i_a_l_i_z_e(const _E_x_p_r_e_s_s_i_o_n_<_T_>& _e_x_p_r_e_s_s_i_o_n) { │ │ │ │ │ +168 if (!noiseModel_) │ │ │ │ │ +169 throw std::invalid_argument("ExpressionFactor: no NoiseModel."); │ │ │ │ │ +170 if (noiseModel_->dim() != Dim) │ │ │ │ │ +171 throw std::invalid_argument( │ │ │ │ │ +172 "ExpressionFactor was created with a NoiseModel of incorrect dimension."); │ │ │ │ │ +173 _e_x_p_r_e_s_s_i_o_n__ = _e_x_p_r_e_s_s_i_o_n; │ │ │ │ │ 174 │ │ │ │ │ -_1_7_5template<> struct _t_r_a_i_t_s<_L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r> : public │ │ │ │ │ -_T_e_s_t_a_b_l_e {}; │ │ │ │ │ -176 │ │ │ │ │ -177} // \namespace gtsam │ │ │ │ │ -178 │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Factor Graph consisting of non-linear factors. │ │ │ │ │ +175 // Get keys and dimensions for Jacobian matrices │ │ │ │ │ +176 // An Expression is assumed unmutable, so we do this now │ │ │ │ │ +177 if (_k_e_y_s__.empty()) { │ │ │ │ │ +178 // This is the case when called in ExpressionFactor Constructor. │ │ │ │ │ +179 // We then take the keys from the expression in sorted order. │ │ │ │ │ +180 boost::tie(_k_e_y_s__, _d_i_m_s__) = _e_x_p_r_e_s_s_i_o_n__.keysAndDims(); │ │ │ │ │ +181 } else { │ │ │ │ │ +182 // This happens with classes derived from BinaryExpressionFactor etc. │ │ │ │ │ +183 // In that case, the keys_ are already defined and we just need to grab │ │ │ │ │ +184 // the dimensions in the correct order. │ │ │ │ │ +185 std::map keyedDims; │ │ │ │ │ +186 _e_x_p_r_e_s_s_i_o_n__.dims(keyedDims); │ │ │ │ │ +187 for (_K_e_y key : _k_e_y_s__) _d_i_m_s__.push_back(keyedDims[key]); │ │ │ │ │ +188 } │ │ │ │ │ +189 } │ │ │ │ │ +190 │ │ │ │ │ +_1_9_3 virtual _E_x_p_r_e_s_s_i_o_n_<_T_> _e_x_p_r_e_s_s_i_o_n() const { │ │ │ │ │ +194 throw std::runtime_error("ExpressionFactor::expression not provided: cannot │ │ │ │ │ +deserialize."); │ │ │ │ │ +195 } │ │ │ │ │ +196 │ │ │ │ │ +197private: │ │ │ │ │ +199 template │ │ │ │ │ +200 void save(Archive& ar, const unsigned int /*version*/) const { │ │ │ │ │ +201 ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r); │ │ │ │ │ +202 ar << boost::serialization::make_nvp("measured_", this->measured_); │ │ │ │ │ +203 } │ │ │ │ │ +204 │ │ │ │ │ +207 template │ │ │ │ │ +208 void load(Archive& ar, const unsigned int /*version*/) { │ │ │ │ │ +209 ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r); │ │ │ │ │ +210 ar >> boost::serialization::make_nvp("measured_", this->measured_); │ │ │ │ │ +211 this->initialize(_e_x_p_r_e_s_s_i_o_n()); │ │ │ │ │ +212 } │ │ │ │ │ +213 │ │ │ │ │ +214 // Indicate that we implement save/load separately, and be friendly to │ │ │ │ │ +boost │ │ │ │ │ +215 BOOST_SERIALIZATION_SPLIT_MEMBER() │ │ │ │ │ +216 │ │ │ │ │ +217 friend class boost::serialization::access; │ │ │ │ │ +218 │ │ │ │ │ +219 // Alignment, see https://eigen.tuxfamily.org/dox/ │ │ │ │ │ +group__TopicStructHavingEigenMembers.html │ │ │ │ │ +220 enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; │ │ │ │ │ +221 public: │ │ │ │ │ +222 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W___I_F(NeedsToAlign) │ │ │ │ │ +223}; │ │ │ │ │ +224// ExpressionFactor │ │ │ │ │ +225 │ │ │ │ │ +227template │ │ │ │ │ +_2_2_8struct _t_r_a_i_t_s<_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r > : public _T_e_s_t_a_b_l_e > │ │ │ │ │ +{}; │ │ │ │ │ +229 │ │ │ │ │ +241template │ │ │ │ │ +_2_4_2class _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_N : public _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r { │ │ │ │ │ +243public: │ │ │ │ │ +244 static const std::size_t NARY_EXPRESSION_SIZE = sizeof...(Args); │ │ │ │ │ +245 using ArrayNKeys = std::array; │ │ │ │ │ +246 │ │ │ │ │ +_2_4_8 _~_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_N() override = default; │ │ │ │ │ +249 │ │ │ │ │ +250 // Don't provide backward compatible evaluateVector(), due to its │ │ │ │ │ +problematic │ │ │ │ │ +251 // variable length of optional Jacobian arguments. Vector evaluateError │ │ │ │ │ +(const │ │ │ │ │ +252 // Args... args,...); │ │ │ │ │ +253 │ │ │ │ │ +_2_5_6 virtual _E_x_p_r_e_s_s_i_o_n_<_T_> _e_x_p_r_e_s_s_i_o_n(const ArrayNKeys &_k_e_y_s) const { │ │ │ │ │ +257 throw std::runtime_error( │ │ │ │ │ +258 "ExpressionFactorN::expression not provided: cannot deserialize."); │ │ │ │ │ +259 } │ │ │ │ │ +260 │ │ │ │ │ +261protected: │ │ │ │ │ +_2_6_3 _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_N() = default; │ │ │ │ │ +264 │ │ │ │ │ +_2_6_6 _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_N(const ArrayNKeys &_k_e_y_s, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +&noiseModel, │ │ │ │ │ +267 const T &measurement) │ │ │ │ │ +268 : _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r(noiseModel, measurement) { │ │ │ │ │ +269 for (const auto &key : _k_e_y_s) │ │ │ │ │ +270 _F_a_c_t_o_r_:_:_k_e_y_s__.push_back(key); │ │ │ │ │ +271 } │ │ │ │ │ +272 │ │ │ │ │ +273private: │ │ │ │ │ +275 _E_x_p_r_e_s_s_i_o_n_<_T_> expression() const override { │ │ │ │ │ +276 ArrayNKeys _k_e_y_s; │ │ │ │ │ +277 int idx = 0; │ │ │ │ │ +278 for (const auto &key : _F_a_c_t_o_r::_k_e_y_s__) │ │ │ │ │ +279 _k_e_y_s[idx++] = key; │ │ │ │ │ +280 return expression(_k_e_y_s); │ │ │ │ │ +281 } │ │ │ │ │ +282 │ │ │ │ │ +283 friend class boost::serialization::access; │ │ │ │ │ +284 template │ │ │ │ │ +285 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ +286 ar &boost::serialization::make_nvp( │ │ │ │ │ +287 "ExpressionFactorN", │ │ │ │ │ +288 boost::serialization::base_object>(*this)); │ │ │ │ │ +289 } │ │ │ │ │ +290}; │ │ │ │ │ +292template │ │ │ │ │ +_2_9_3struct _t_r_a_i_t_s<_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_N> │ │ │ │ │ +294 : public _T_e_s_t_a_b_l_e> {}; │ │ │ │ │ +295// ExpressionFactorN │ │ │ │ │ +296 │ │ │ │ │ +297 │ │ │ │ │ +298#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V42) │ │ │ │ │ +307template │ │ │ │ │ +308class GTSAM_DEPRECATED ExpressionFactor2 : public _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_N { │ │ │ │ │ +309public: │ │ │ │ │ +311 ~ExpressionFactor2() override {} │ │ │ │ │ +312 │ │ │ │ │ +314 Vector evaluateError(const A1 &a1, const A2 &a2, │ │ │ │ │ +315 boost::optional H1 = boost::none, │ │ │ │ │ +316 boost::optional H2 = boost::none) const { │ │ │ │ │ +317 _V_a_l_u_e_s values; │ │ │ │ │ +318 values.insert(this->keys_[0], a1); │ │ │ │ │ +319 values.insert(this->keys_[1], a2); │ │ │ │ │ +320 std::vector H(2); │ │ │ │ │ +321 Vector error = ExpressionFactor::unwhitenedError(values, H); │ │ │ │ │ +322 if (H1) (*H1) = H[0]; │ │ │ │ │ +323 if (H2) (*H2) = H[1]; │ │ │ │ │ +324 return error; │ │ │ │ │ +325 } │ │ │ │ │ +326 │ │ │ │ │ +329 virtual Expression expression(Key key1, Key key2) const { │ │ │ │ │ +330 throw std::runtime_error( │ │ │ │ │ +331 "ExpressionFactor2::expression not provided: cannot deserialize."); │ │ │ │ │ +332 } │ │ │ │ │ +333 Expression │ │ │ │ │ +334 expression(const typename ExpressionFactorN::ArrayNKeys &keys) │ │ │ │ │ +335 const override { │ │ │ │ │ +336 return expression(keys[0], keys[1]); │ │ │ │ │ +337 } │ │ │ │ │ +338 │ │ │ │ │ +339protected: │ │ │ │ │ +341 ExpressionFactor2() {} │ │ │ │ │ +342 │ │ │ │ │ +344 ExpressionFactor2(Key key1, Key key2, const SharedNoiseModel &noiseModel, │ │ │ │ │ +345 const T &measurement) │ │ │ │ │ +346 : ExpressionFactorN({key1, key2}, noiseModel, measurement) {} │ │ │ │ │ +347}; │ │ │ │ │ +348// ExpressionFactor2 │ │ │ │ │ +349#endif │ │ │ │ │ +350 │ │ │ │ │ +351} // namespace gtsam │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ +_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W___I_F │ │ │ │ │ +#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) │ │ │ │ │ +This marks a GTSAM object to require alignment. │ │ │ │ │ +DDeeffiinniittiioonn types.h:317 │ │ │ │ │ +_E_x_p_r_e_s_s_i_o_n_._h │ │ │ │ │ +Expressions for Block Automatic Differentiation. │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ +Non-linear factor base classes. │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ +std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ +FastVector │ │ │ │ │ +FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ +DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ +Aliases. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_I_s_T_e_s_t_a_b_l_e │ │ │ │ │ +A testable concept check that should be placed in applicable unit tests and in │ │ │ │ │ +generic algorithms. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:58 │ │ │ │ │ _g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ A helper that implements the traits interface for GTSAM types. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -A Gaussian factor using the canonical parameters (information form) │ │ │ │ │ -DDeeffiinniittiioonn HessianFactor.h:101 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ +of vertical blocks. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_m_a_t_r_i_x │ │ │ │ │ +const Matrix & matrix() const │ │ │ │ │ +Access to full matrix (including any portions excluded by rowStart(), rowEnd(), │ │ │ │ │ +and firstBlock()) │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:188 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:68 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s │ │ │ │ │ +const KeyVector & keys() const │ │ │ │ │ +Access the factor's involved variable keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:140 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s__ │ │ │ │ │ +KeyVector keys_ │ │ │ │ │ +The keys involved in this factor. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_s_i_z_e │ │ │ │ │ +size_t size() const │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:157 │ │ │ │ │ _g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ A Gaussian factor in the squared-error form. │ │ │ │ │ DDeeffiinniittiioonn JacobianFactor.h:91 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r │ │ │ │ │ -Dummy version of a generic linear factor to be injected into a nonlinear factor │ │ │ │ │ -graph. │ │ │ │ │ -DDeeffiinniittiioonn LinearContainerFactor.h:26 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ -NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -Creates a shared_ptr clone of the factor - needs to be specialized to allow for │ │ │ │ │ -subclasses. │ │ │ │ │ -DDeeffiinniittiioonn LinearContainerFactor.h:119 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r_:_:_L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r │ │ │ │ │ -LinearContainerFactor() │ │ │ │ │ -Default constructor - necessary for serialization. │ │ │ │ │ -DDeeffiinniittiioonn LinearContainerFactor.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_a_t_i_o_n_P_o_i_n_t │ │ │ │ │ -const boost::optional< Values > & linearizationPoint() const │ │ │ │ │ -Extract the linearization point used in recalculating error. │ │ │ │ │ -DDeeffiinniittiioonn LinearContainerFactor.h:83 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r_:_:_h_a_s_L_i_n_e_a_r_i_z_a_t_i_o_n_P_o_i_n_t │ │ │ │ │ -bool hasLinearizationPoint() const │ │ │ │ │ -Casting syntactic sugar. │ │ │ │ │ -DDeeffiinniittiioonn LinearContainerFactor.h:138 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r │ │ │ │ │ +Factor that supports arbitrary expressions via AD. │ │ │ │ │ +DDeeffiinniittiioonn ExpressionFactor.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_:_:_u_n_w_h_i_t_e_n_e_d_E_r_r_o_r │ │ │ │ │ +Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > │ │ │ │ │ +& > H=boost::none) const override │ │ │ │ │ +Error function without the NoiseModel, . │ │ │ │ │ +DDeeffiinniittiioonn ExpressionFactor.h:99 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +print relies on Testable traits being defined for T │ │ │ │ │ +DDeeffiinniittiioonn ExpressionFactor.h:80 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ +gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn ExpressionFactor.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_:_:_e_x_p_r_e_s_s_i_o_n │ │ │ │ │ +virtual Expression< T > expression() const │ │ │ │ │ +Recreate expression from keys_ and measured_, used in load below. │ │ │ │ │ +DDeeffiinniittiioonn ExpressionFactor.h:193 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_:_:_e_x_p_r_e_s_s_i_o_n__ │ │ │ │ │ +Expression< T > expression_ │ │ │ │ │ +the expression that is AD enabled │ │ │ │ │ +DDeeffiinniittiioonn ExpressionFactor.h:53 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_:_:_m_e_a_s_u_r_e_d__ │ │ │ │ │ +T measured_ │ │ │ │ │ +the measurement to be compared with the expression │ │ │ │ │ +DDeeffiinniittiioonn ExpressionFactor.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_:_:_d_i_m_s__ │ │ │ │ │ +FastVector< int > dims_ │ │ │ │ │ +dimensions of the Jacobian matrices │ │ │ │ │ +DDeeffiinniittiioonn ExpressionFactor.h:54 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_:_:_m_e_a_s_u_r_e_d │ │ │ │ │ +const T & measured() const │ │ │ │ │ +return the measurement │ │ │ │ │ +DDeeffiinniittiioonn ExpressionFactor.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e │ │ │ │ │ +boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override │ │ │ │ │ +linearize to a GaussianFactor │ │ │ │ │ +DDeeffiinniittiioonn ExpressionFactor.h:112 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_:_:_~_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r │ │ │ │ │ +~ExpressionFactor() override │ │ │ │ │ +Destructor. │ │ │ │ │ +DDeeffiinniittiioonn ExpressionFactor.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const NonlinearFactor &f, double tol) const override │ │ │ │ │ +equals relies on Testable traits being defined for T │ │ │ │ │ +DDeeffiinniittiioonn ExpressionFactor.h:87 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r │ │ │ │ │ +ExpressionFactor(const SharedNoiseModel &noiseModel, const T &measurement) │ │ │ │ │ +Default constructor, for serialization. │ │ │ │ │ +DDeeffiinniittiioonn ExpressionFactor.h:161 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r │ │ │ │ │ +ExpressionFactor(const SharedNoiseModel &noiseModel, const T &measurement, │ │ │ │ │ +const Expression< T > &expression) │ │ │ │ │ +Constructor: creates a factor from a measurement and measurement function. │ │ │ │ │ +DDeeffiinniittiioonn ExpressionFactor.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_:_:_i_n_i_t_i_a_l_i_z_e │ │ │ │ │ +void initialize(const Expression< T > &expression) │ │ │ │ │ +Initialize with constructor arguments. │ │ │ │ │ +DDeeffiinniittiioonn ExpressionFactor.h:167 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n │ │ │ │ │ +Expression class that supports automatic differentiation. │ │ │ │ │ +DDeeffiinniittiioonn Expression.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_N │ │ │ │ │ +N-ary variadic template for ExpressionFactor meant as a base class for N-ary │ │ │ │ │ +factors. │ │ │ │ │ +DDeeffiinniittiioonn ExpressionFactor.h:242 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_N_:_:_e_x_p_r_e_s_s_i_o_n │ │ │ │ │ +virtual Expression< T > expression(const ArrayNKeys &keys) const │ │ │ │ │ +Recreate expression from given keys_ and measured_, used in load Needed to │ │ │ │ │ +deserialize a derived fact... │ │ │ │ │ +DDeeffiinniittiioonn ExpressionFactor.h:256 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_N_:_:_~_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_N │ │ │ │ │ +~ExpressionFactorN() override=default │ │ │ │ │ +Destructor. │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_N_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_N │ │ │ │ │ +ExpressionFactorN()=default │ │ │ │ │ +Default constructor, for serialization. │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_N_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_N │ │ │ │ │ +ExpressionFactorN(const ArrayNKeys &keys, const SharedNoiseModel &noiseModel, │ │ │ │ │ +const T &measurement) │ │ │ │ │ +Constructor takes care of keys, but still need to call initialize. │ │ │ │ │ +DDeeffiinniittiioonn ExpressionFactor.h:266 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ Nonlinear factor base class. │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_:_:_a_c_t_i_v_e │ │ │ │ │ +virtual bool active(const Values &) const │ │ │ │ │ +Checks whether a factor should be used based on a set of values. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:118 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r │ │ │ │ │ +A nonlinear sum-of-squares factor with a zero-mean noise model implementing the │ │ │ │ │ +density Templated on... │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:174 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +Print. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.cpp:74 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const NonlinearFactor &f, double tol=1e-9) const override │ │ │ │ │ +Check if two factors are equal. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.cpp:82 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_n_o_i_s_e_M_o_d_e_l │ │ │ │ │ +const SharedNoiseModel & noiseModel() const │ │ │ │ │ +access to the noise model │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:223 │ │ │ │ │ _g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ -In Gaussian factors, the error function returns either the negative log- │ │ │ │ │ -likelihood,... │ │ │ │ │ _V_a_l_u_e_s │ │ │ │ │ In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ a non-linear function... │ │ │ │ │ +_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r │ │ │ │ │ +noise model to the factor, and calculates the error by asking the user to │ │ │ │ │ +implement the method │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r_._h │ │ │ │ │ + * EExxpprreessssiioonnFFaaccttoorr..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01184.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/MFAS.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/ShonanGaugeFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,45 +94,45 @@ │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Functions
│ │ │ │ -
MFAS.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
ShonanGaugeFactor.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Source file for the MFAS class. │ │ │ │ +

Factor used in Shonan Averaging to clamp down gauge freedom. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ -Functions

│ │ │ │ -unordered_map< Key, GraphNode > graphFromEdges (const map< MFAS::KeyPair, double > &edgeWeights)
 
│ │ │ │ -Key selectNextNodeInOrdering (const unordered_map< Key, GraphNode > &graph)
 
│ │ │ │ -double absWeightOfEdge (const Key key1, const Key key2, const map< MFAS::KeyPair, double > &edgeWeights)
 
│ │ │ │ -void removeNodeFromGraph (const Key node, const map< MFAS::KeyPair, double > edgeWeights, unordered_map< Key, GraphNode > &graph)
 

│ │ │ │ +Classes

class  gtsam::ShonanGaugeFactor
 The ShonanGaugeFactor creates a constraint on a single SO(n) to avoid moving in the stabilizer. More...
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Source file for the MFAS class.

│ │ │ │ -
Author
Akshay Krishnan
│ │ │ │ -
Date
July 2020
│ │ │ │ +

Factor used in Shonan Averaging to clamp down gauge freedom.

│ │ │ │ +
Date
March 2019
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,33 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_F_u_n_c_t_i_o_n_s │ │ │ │ │ -MFAS.cpp File Reference │ │ │ │ │ -Source file for the MFAS class. _M_o_r_e_._._. │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -unordered_map< _K_e_y, GraphNode >  ggrraapphhFFrroommEEddggeess (const map< MFAS::KeyPair, │ │ │ │ │ - double > &edgeWeights) │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +ShonanGaugeFactor.h File Reference │ │ │ │ │ +Factor used in Shonan Averaging to clamp down gauge freedom. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r │ │ │ │ │ +  The _S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r creates a constraint on a single SO(n) to avoid │ │ │ │ │ + moving in the stabilizer. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ - _K_e_y  sseelleeccttNNeexxttNNooddeeIInnOOrrddeerriinngg (const unordered_map< │ │ │ │ │ - _K_e_y, GraphNode > &graph) │ │ │ │ │ -  │ │ │ │ │ - double  aabbssWWeeiigghhttOOffEEddggee (const _K_e_y key1, const _K_e_y │ │ │ │ │ - key2, const map< MFAS::KeyPair, double > │ │ │ │ │ - &edgeWeights) │ │ │ │ │ -  │ │ │ │ │ - void  rreemmoovveeNNooddeeFFrroommGGrraapphh (const _K_e_y node, const │ │ │ │ │ - map< MFAS::KeyPair, double > edgeWeights, │ │ │ │ │ - unordered_map< _K_e_y, GraphNode > &graph) │ │ │ │ │ +NNaammeessppaacceess │ │ │ │ │ +namespace   _g_t_s_a_m │ │ │ │ │ +  Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Source file for the MFAS class. │ │ │ │ │ - Author │ │ │ │ │ - Akshay Krishnan │ │ │ │ │ +Factor used in Shonan Averaging to clamp down gauge freedom. │ │ │ │ │ Date │ │ │ │ │ - July 2020 │ │ │ │ │ + March 2019 │ │ │ │ │ + Author │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_f_m │ │ │ │ │ - * _M_F_A_S_._c_p_p │ │ │ │ │ + * _S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01187.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/TranslationRecovery.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/MFAS.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,44 +94,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
TranslationRecovery.h File Reference
│ │ │ │ +Functions
│ │ │ │ +
MFAS.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Recovering translations in an epipolar graph when rotations are given. │ │ │ │ +

Source file for the MFAS class. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::TranslationRecovery
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ -Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 

│ │ │ │ +Functions

│ │ │ │ +unordered_map< Key, GraphNode > graphFromEdges (const map< MFAS::KeyPair, double > &edgeWeights)
 
│ │ │ │ +Key selectNextNodeInOrdering (const unordered_map< Key, GraphNode > &graph)
 
│ │ │ │ +double absWeightOfEdge (const Key key1, const Key key2, const map< MFAS::KeyPair, double > &edgeWeights)
 
│ │ │ │ +void removeNodeFromGraph (const Key node, const map< MFAS::KeyPair, double > edgeWeights, unordered_map< Key, GraphNode > &graph)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Recovering translations in an epipolar graph when rotations are given.

│ │ │ │ -
Author
Frank Dellaert, Akshay Krishnan
│ │ │ │ -
Date
March 2020
│ │ │ │ +

Source file for the MFAS class.

│ │ │ │ +
Author
Akshay Krishnan
│ │ │ │ +
Date
July 2020
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,26 +1,33 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -TranslationRecovery.h File Reference │ │ │ │ │ -Recovering translations in an epipolar graph when rotations are given. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y │ │ │ │ │ +_F_u_n_c_t_i_o_n_s │ │ │ │ │ +MFAS.cpp File Reference │ │ │ │ │ +Source file for the MFAS class. _M_o_r_e_._._. │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +unordered_map< _K_e_y, GraphNode >  ggrraapphhFFrroommEEddggeess (const map< MFAS::KeyPair, │ │ │ │ │ + double > &edgeWeights) │ │ │ │ │   │ │ │ │ │ -NNaammeessppaacceess │ │ │ │ │ -namespace   _g_t_s_a_m │ │ │ │ │ -  Global functions in a separate testing namespace. │ │ │ │ │ + _K_e_y  sseelleeccttNNeexxttNNooddeeIInnOOrrddeerriinngg (const unordered_map< │ │ │ │ │ + _K_e_y, GraphNode > &graph) │ │ │ │ │ +  │ │ │ │ │ + double  aabbssWWeeiigghhttOOffEEddggee (const _K_e_y key1, const _K_e_y │ │ │ │ │ + key2, const map< MFAS::KeyPair, double > │ │ │ │ │ + &edgeWeights) │ │ │ │ │ +  │ │ │ │ │ + void  rreemmoovveeNNooddeeFFrroommGGrraapphh (const _K_e_y node, const │ │ │ │ │ + map< MFAS::KeyPair, double > edgeWeights, │ │ │ │ │ + unordered_map< _K_e_y, GraphNode > &graph) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Recovering translations in an epipolar graph when rotations are given. │ │ │ │ │ +Source file for the MFAS class. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert, Akshay Krishnan │ │ │ │ │ + Akshay Krishnan │ │ │ │ │ Date │ │ │ │ │ - March 2020 │ │ │ │ │ + July 2020 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_f_m │ │ │ │ │ - * _T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y_._h │ │ │ │ │ + * _M_F_A_S_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01190.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/TranslationFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/DsfTrackGenerator.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,44 +95,108 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
TranslationFactor.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ +Functions
│ │ │ │ +
DsfTrackGenerator.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Binary factor for a relative translation direction measurement. │ │ │ │ +

Identifies connected components in the keypoint matches graph. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::TranslationFactor
 Binary factor for a relative translation direction measurement w_aZb. More...
struct  gtsam::gtsfm::Keypoints
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Typedefs

│ │ │ │ +typedef Eigen::MatrixX2i gtsam::gtsfm::CorrespondenceIndices
 
│ │ │ │ +using gtsam::gtsfm::KeypointsVector = std::vector< Keypoints >
 
│ │ │ │ +using gtsam::gtsfm::MatchIndicesMap = std::map< IndexPair, CorrespondenceIndices >
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

std::vector< SfmTrack2d > gtsam::gtsfm::tracksFromPairwiseMatches (const MatchIndicesMap &matches, const KeypointsVector &keypoints, bool verbose)
 Creates a list of tracks from 2d point correspondences.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Binary factor for a relative translation direction measurement.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
Date
March 2020
│ │ │ │ -
│ │ │ │ +

Identifies connected components in the keypoint matches graph.

│ │ │ │ +
Date
July 2022
│ │ │ │ +
Author
John Lambert
│ │ │ │ +

Function Documentation

│ │ │ │ + │ │ │ │ +

◆ tracksFromPairwiseMatches()

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
std::vector< SfmTrack2d > gtsam::gtsfm::tracksFromPairwiseMatches (const MatchIndicesMap & matches,
const KeypointsVector & keypoints,
bool verbose 
)
│ │ │ │ +
│ │ │ │ + │ │ │ │ +

Creates a list of tracks from 2d point correspondences.

│ │ │ │ +

Creates a disjoint-set forest (DSF) and 2d tracks from pairwise matches. We create a singleton for union-find set elements from camera index of a detection and the index of that detection in that camera's keypoint list, i.e. (i,k).

│ │ │ │ +
Parameters
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
Mapfrom (i1,i2) image pair indices to (K,2) matrix, for K correspondence indices, from each image.
Length-Nlist of keypoints, for N images/cameras.
│ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,57 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -TranslationFactor.h File Reference │ │ │ │ │ -Binary factor for a relative translation direction measurement. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +DsfTrackGenerator.h File Reference │ │ │ │ │ +Identifies connected components in the keypoint matches graph. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_T_r_a_n_s_l_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -  Binary factor for a relative translation direction measurement w_aZb. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_g_t_s_f_m_:_:_K_e_y_p_o_i_n_t_s │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +typedef Eigen::MatrixX2i  ggttssaamm::::ggttssffmm::::CCoorrrreessppoonnddeenncceeIInnddiicceess │ │ │ │ │ +  │ │ │ │ │ + using  ggttssaamm::::ggttssffmm::::KKeeyyppooiinnttssVVeeccttoorr = std::vector< │ │ │ │ │ + _K_e_y_p_o_i_n_t_s > │ │ │ │ │ +  │ │ │ │ │ + using  ggttssaamm::::ggttssffmm::::MMaattcchhIInnddiicceessMMaapp = std::map< _I_n_d_e_x_P_a_i_r, │ │ │ │ │ + CorrespondenceIndices > │ │ │ │ │ +  │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +std::vector< _S_f_m_T_r_a_c_k_2_d >  _g_t_s_a_m_:_:_g_t_s_f_m_:_:_t_r_a_c_k_s_F_r_o_m_P_a_i_r_w_i_s_e_M_a_t_c_h_e_s (const │ │ │ │ │ + MatchIndicesMap &matches, const KeypointsVector │ │ │ │ │ + &keypoints, bool verbose) │ │ │ │ │ +  Creates a list of tracks from 2d point │ │ │ │ │ + correspondences. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Binary factor for a relative translation direction measurement. │ │ │ │ │ - Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ +Identifies connected components in the keypoint matches graph. │ │ │ │ │ Date │ │ │ │ │ - March 2020 │ │ │ │ │ + July 2022 │ │ │ │ │ + Author │ │ │ │ │ + John Lambert │ │ │ │ │ +********** FFuunnccttiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ +********** _?◆_? ttrraacckkssFFrroommPPaaiirrwwiisseeMMaattcchheess(()) ********** │ │ │ │ │ +std::vector< SfmTrack2d > gtsam::gtsfm:: ( const MatchIndicesMap &  mmaattcchheess, │ │ │ │ │ +tracksFromPairwiseMatches │ │ │ │ │ + const KeypointsVector &  kkeeyyppooiinnttss, │ │ │ │ │ + bool  vveerrbboossee  │ │ │ │ │ + ) │ │ │ │ │ +Creates a list of tracks from 2d point correspondences. │ │ │ │ │ +Creates a disjoint-set forest (DSF) and 2d tracks from pairwise matches. We │ │ │ │ │ +create a singleton for union-find set elements from camera index of a detection │ │ │ │ │ +and the index of that detection in that camera's keypoint list, i.e. (i,k). │ │ │ │ │ + Parameters │ │ │ │ │ + Map from (i1,i2) image pair indices to (K,2) matrix, for K │ │ │ │ │ + correspondence indices, from each image. │ │ │ │ │ + Length-N list of keypoints, for N images/cameras. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_f_m │ │ │ │ │ - * _T_r_a_n_s_l_a_t_i_o_n_F_a_c_t_o_r_._h │ │ │ │ │ + * _D_s_f_T_r_a_c_k_G_e_n_e_r_a_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01190_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/TranslationFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/DsfTrackGenerator.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,91 +98,87 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
TranslationFactor.h
│ │ │ │ +
DsfTrackGenerator.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
12#pragma once
│ │ │ │ -
13
│ │ │ │ -
21#include <gtsam/geometry/Point3.h>
│ │ │ │ -
22#include <gtsam/geometry/Unit3.h>
│ │ │ │ -
23#include <gtsam/linear/NoiseModel.h>
│ │ │ │ -
24#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
25
│ │ │ │ -
26namespace gtsam {
│ │ │ │ -
27
│ │ │ │ -
│ │ │ │ -
42class TranslationFactor : public NoiseModelFactorN<Point3, Point3> {
│ │ │ │ -
43 private:
│ │ │ │ -
44 typedef NoiseModelFactorN<Point3, Point3> Base;
│ │ │ │ -
45 Point3 measured_w_aZb_;
│ │ │ │ -
46
│ │ │ │ -
47 public:
│ │ │ │ -
49 TranslationFactor() {}
│ │ │ │ -
50
│ │ │ │ -
51 TranslationFactor(Key a, Key b, const Unit3& w_aZb,
│ │ │ │ -
52 const SharedNoiseModel& noiseModel)
│ │ │ │ -
53 : Base(noiseModel, a, b), measured_w_aZb_(w_aZb.point3()) {}
│ │ │ │ -
54
│ │ │ │ -
│ │ │ │ -
66 Vector evaluateError(
│ │ │ │ -
67 const Point3& Ta, const Point3& Tb,
│ │ │ │ -
68 boost::optional<Matrix&> H1 = boost::none,
│ │ │ │ -
69 boost::optional<Matrix&> H2 = boost::none) const override {
│ │ │ │ -
70 const Point3 dir = Tb - Ta;
│ │ │ │ -
71 Matrix33 H_predicted_dir;
│ │ │ │ -
72 const Point3 predicted = normalize(dir, H1 || H2 ? &H_predicted_dir : nullptr);
│ │ │ │ -
73 if (H1) *H1 = -H_predicted_dir;
│ │ │ │ -
74 if (H2) *H2 = H_predicted_dir;
│ │ │ │ -
75 return predicted - measured_w_aZb_;
│ │ │ │ -
76 }
│ │ │ │ -
│ │ │ │ -
77
│ │ │ │ -
78 private:
│ │ │ │ -
79 friend class boost::serialization::access;
│ │ │ │ -
80 template <class ARCHIVE>
│ │ │ │ -
81 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ -
82 ar& boost::serialization::make_nvp(
│ │ │ │ -
83 "Base", boost::serialization::base_object<Base>(*this));
│ │ │ │ -
84 }
│ │ │ │ -
85}; // \ TranslationFactor
│ │ │ │ -
│ │ │ │ -
86} // namespace gtsam
│ │ │ │ -
Point3.h
3D Point
│ │ │ │ - │ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
19#pragma once
│ │ │ │ +
20#include <gtsam/base/DSFMap.h>
│ │ │ │ +
21#include <gtsam/sfm/SfmTrack.h>
│ │ │ │ +
22
│ │ │ │ +
23#include <boost/optional.hpp>
│ │ │ │ +
24
│ │ │ │ +
25#include <Eigen/Core>
│ │ │ │ +
26#include <map>
│ │ │ │ +
27#include <vector>
│ │ │ │ +
28
│ │ │ │ +
29namespace gtsam {
│ │ │ │ +
30
│ │ │ │ +
31namespace gtsfm {
│ │ │ │ +
32
│ │ │ │ +
33typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array
│ │ │ │ +
34
│ │ │ │ +
35// Output of detections in an image.
│ │ │ │ +
36// Coordinate system convention:
│ │ │ │ +
37// 1. The x coordinate denotes the horizontal direction (+ve direction towards
│ │ │ │ +
38// the right).
│ │ │ │ +
39// 2. The y coordinate denotes the vertical direction (+ve direction downwards).
│ │ │ │ +
40// 3. Origin is at the top left corner of the image.
│ │ │ │ +
│ │ │ │ +
41struct Keypoints {
│ │ │ │ +
42 // The (x, y) coordinates of the features, of shape Nx2.
│ │ │ │ +
43 Eigen::MatrixX2d coordinates;
│ │ │ │ +
44
│ │ │ │ +
45 // Optional scale of the detections, of shape N.
│ │ │ │ +
46 // Note: gtsam::Vector is typedef'd for Eigen::VectorXd.
│ │ │ │ +
47 boost::optional<gtsam::Vector> scales;
│ │ │ │ +
48
│ │ │ │ +
50 boost::optional<gtsam::Vector> responses;
│ │ │ │ +
51
│ │ │ │ +
52 Keypoints(const Eigen::MatrixX2d& coordinates)
│ │ │ │ +
53 : coordinates(coordinates){}; // boost::none
│ │ │ │ +
54};
│ │ │ │ +
│ │ │ │ +
55
│ │ │ │ +
56using KeypointsVector = std::vector<Keypoints>;
│ │ │ │ +
57// Mapping from each image pair to (N,2) array representing indices of matching
│ │ │ │ +
58// keypoints.
│ │ │ │ +
59using MatchIndicesMap = std::map<IndexPair, CorrespondenceIndices>;
│ │ │ │ +
60
│ │ │ │ +
73std::vector<SfmTrack2d> tracksFromPairwiseMatches(
│ │ │ │ +
74 const MatchIndicesMap& matches, const KeypointsVector& keypoints,
│ │ │ │ +
75 bool verbose = false);
│ │ │ │ +
76
│ │ │ │ +
77} // namespace gtsfm
│ │ │ │ +
78
│ │ │ │ +
79} // namespace gtsam
│ │ │ │ +
DSFMap.h
Allow for arbitrary type in DSF.
│ │ │ │ +
SfmTrack.h
A simple data structure for a track in Structure from Motion.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ -
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ -
gtsam::normalize
Point3 normalize(const Point3 &p, OptionalJacobian< 3, 3 > H)
normalize, with optional Jacobian
Definition Point3.cpp:52
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
│ │ │ │ -
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ -
gtsam::TranslationFactor
Binary factor for a relative translation direction measurement w_aZb.
Definition TranslationFactor.h:42
│ │ │ │ -
gtsam::TranslationFactor::TranslationFactor
TranslationFactor()
default constructor
Definition TranslationFactor.h:49
│ │ │ │ -
gtsam::TranslationFactor::evaluateError
Vector evaluateError(const Point3 &Ta, const Point3 &Tb, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Caclulate error: (norm(Tb - Ta) - measurement) where Tb and Ta are Point3 translations and measuremen...
Definition TranslationFactor.h:66
│ │ │ │ +
gtsam::gtsfm::Keypoints
Definition DsfTrackGenerator.h:41
│ │ │ │ +
gtsam::gtsfm::Keypoints::responses
boost::optional< gtsam::Vector > responses
Optional confidences/responses for each detection, of shape N.
Definition DsfTrackGenerator.h:50
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,113 +1,88 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -TranslationFactor.h │ │ │ │ │ +DsfTrackGenerator.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -12#pragma once │ │ │ │ │ -13 │ │ │ │ │ -21#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_3_._h> │ │ │ │ │ -22#include │ │ │ │ │ -23#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_N_o_i_s_e_M_o_d_e_l_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -25 │ │ │ │ │ -26namespace _g_t_s_a_m { │ │ │ │ │ -27 │ │ │ │ │ -_4_2class _T_r_a_n_s_l_a_t_i_o_n_F_a_c_t_o_r : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -43 private: │ │ │ │ │ -44 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_P_o_i_n_t_3_,_ _P_o_i_n_t_3_> _B_a_s_e; │ │ │ │ │ -45 _P_o_i_n_t_3 measured_w_aZb_; │ │ │ │ │ -46 │ │ │ │ │ -47 public: │ │ │ │ │ -_4_9 _T_r_a_n_s_l_a_t_i_o_n_F_a_c_t_o_r() {} │ │ │ │ │ -50 │ │ │ │ │ -51 _T_r_a_n_s_l_a_t_i_o_n_F_a_c_t_o_r(_K_e_y a, _K_e_y b, const _U_n_i_t_3& w_aZb, │ │ │ │ │ -52 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& noiseModel) │ │ │ │ │ -53 : Base(noiseModel, a, b), measured_w_aZb_(w_aZb.point3()) {} │ │ │ │ │ -54 │ │ │ │ │ -_6_6 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r( │ │ │ │ │ -67 const _P_o_i_n_t_3& Ta, const _P_o_i_n_t_3& Tb, │ │ │ │ │ -68 boost::optional H1 = boost::none, │ │ │ │ │ -69 boost::optional H2 = boost::none) const override { │ │ │ │ │ -70 const _P_o_i_n_t_3 dir = Tb - Ta; │ │ │ │ │ -71 Matrix33 H_predicted_dir; │ │ │ │ │ -72 const _P_o_i_n_t_3 predicted = _n_o_r_m_a_l_i_z_e(dir, H1 || H2 ? &H_predicted_dir : │ │ │ │ │ -nullptr); │ │ │ │ │ -73 if (H1) *H1 = -H_predicted_dir; │ │ │ │ │ -74 if (H2) *H2 = H_predicted_dir; │ │ │ │ │ -75 return predicted - measured_w_aZb_; │ │ │ │ │ -76 } │ │ │ │ │ -77 │ │ │ │ │ -78 private: │ │ │ │ │ -79 friend class boost::serialization::access; │ │ │ │ │ -80 template │ │ │ │ │ -81 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ -82 ar& boost::serialization::make_nvp( │ │ │ │ │ -83 "Base", boost::serialization::base_object(*this)); │ │ │ │ │ -84 } │ │ │ │ │ -85}; // \ TranslationFactor │ │ │ │ │ -86} // namespace gtsam │ │ │ │ │ -_P_o_i_n_t_3_._h │ │ │ │ │ -3D Point │ │ │ │ │ -_N_o_i_s_e_M_o_d_e_l_._h │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ -Non-linear factor base classes. │ │ │ │ │ +19#pragma once │ │ │ │ │ +20#include <_g_t_s_a_m_/_b_a_s_e_/_D_S_F_M_a_p_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_s_f_m_/_S_f_m_T_r_a_c_k_._h> │ │ │ │ │ +22 │ │ │ │ │ +23#include │ │ │ │ │ +24 │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28 │ │ │ │ │ +29namespace _g_t_s_a_m { │ │ │ │ │ +30 │ │ │ │ │ +31namespace gtsfm { │ │ │ │ │ +32 │ │ │ │ │ +33typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array │ │ │ │ │ +34 │ │ │ │ │ +35// Output of detections in an image. │ │ │ │ │ +36// Coordinate system convention: │ │ │ │ │ +37// 1. The x coordinate denotes the horizontal direction (+ve direction │ │ │ │ │ +towards │ │ │ │ │ +38// the right). │ │ │ │ │ +39// 2. The y coordinate denotes the vertical direction (+ve direction │ │ │ │ │ +downwards). │ │ │ │ │ +40// 3. Origin is at the top left corner of the image. │ │ │ │ │ +_4_1struct _K_e_y_p_o_i_n_t_s { │ │ │ │ │ +42 // The (x, y) coordinates of the features, of shape Nx2. │ │ │ │ │ +43 Eigen::MatrixX2d coordinates; │ │ │ │ │ +44 │ │ │ │ │ +45 // Optional scale of the detections, of shape N. │ │ │ │ │ +46 // Note: gtsam::Vector is typedef'd for Eigen::VectorXd. │ │ │ │ │ +47 boost::optional scales; │ │ │ │ │ +48 │ │ │ │ │ +_5_0 boost::optional _r_e_s_p_o_n_s_e_s; │ │ │ │ │ +51 │ │ │ │ │ +52 _K_e_y_p_o_i_n_t_s(const Eigen::MatrixX2d& coordinates) │ │ │ │ │ +53 : coordinates(coordinates){}; // boost::none │ │ │ │ │ +54}; │ │ │ │ │ +55 │ │ │ │ │ +56using KeypointsVector = std::vector; │ │ │ │ │ +57// Mapping from each image pair to (N,2) array representing indices of │ │ │ │ │ +matching │ │ │ │ │ +58// keypoints. │ │ │ │ │ +59using MatchIndicesMap = std::map; │ │ │ │ │ +60 │ │ │ │ │ +73std::vector tracksFromPairwiseMatches( │ │ │ │ │ +74 const MatchIndicesMap& matches, const KeypointsVector& keypoints, │ │ │ │ │ +75 bool verbose = false); │ │ │ │ │ +76 │ │ │ │ │ +77} // namespace gtsfm │ │ │ │ │ +78 │ │ │ │ │ +79} // namespace gtsam │ │ │ │ │ +_D_S_F_M_a_p_._h │ │ │ │ │ +Allow for arbitrary type in DSF. │ │ │ │ │ +_S_f_m_T_r_a_c_k_._h │ │ │ │ │ +A simple data structure for a track in Structure from Motion. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ -Vector3 Point3 │ │ │ │ │ -As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point3 to Vector3... │ │ │ │ │ -DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ -Aliases. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_r_m_a_l_i_z_e │ │ │ │ │ -Point3 normalize(const Point3 &p, OptionalJacobian< 3, 3 > H) │ │ │ │ │ -normalize, with optional Jacobian │ │ │ │ │ -DDeeffiinniittiioonn Point3.cpp:52 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_U_n_i_t_3 │ │ │ │ │ -Represents a 3D point on a unit sphere. │ │ │ │ │ -DDeeffiinniittiioonn Unit3.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ -A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ -variables. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_a_n_s_l_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -Binary factor for a relative translation direction measurement w_aZb. │ │ │ │ │ -DDeeffiinniittiioonn TranslationFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_a_n_s_l_a_t_i_o_n_F_a_c_t_o_r_:_:_T_r_a_n_s_l_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -TranslationFactor() │ │ │ │ │ -default constructor │ │ │ │ │ -DDeeffiinniittiioonn TranslationFactor.h:49 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_a_n_s_l_a_t_i_o_n_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ -Vector evaluateError(const Point3 &Ta, const Point3 &Tb, boost::optional< │ │ │ │ │ -Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const │ │ │ │ │ -override │ │ │ │ │ -Caclulate error: (norm(Tb - Ta) - measurement) where Tb and Ta are Point3 │ │ │ │ │ -translations and measuremen... │ │ │ │ │ -DDeeffiinniittiioonn TranslationFactor.h:66 │ │ │ │ │ +_g_t_s_a_m_:_:_g_t_s_f_m_:_:_K_e_y_p_o_i_n_t_s │ │ │ │ │ +DDeeffiinniittiioonn DsfTrackGenerator.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_g_t_s_f_m_:_:_K_e_y_p_o_i_n_t_s_:_:_r_e_s_p_o_n_s_e_s │ │ │ │ │ +boost::optional< gtsam::Vector > responses │ │ │ │ │ +Optional confidences/responses for each detection, of shape N. │ │ │ │ │ +DDeeffiinniittiioonn DsfTrackGenerator.h:50 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_f_m │ │ │ │ │ - * _T_r_a_n_s_l_a_t_i_o_n_F_a_c_t_o_r_._h │ │ │ │ │ + * _D_s_f_T_r_a_c_k_G_e_n_e_r_a_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01193.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/SfmData.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/TranslationRecovery.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,67 +94,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
SfmData.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
TranslationRecovery.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Data structure for dealing with Structure from Motion data. │ │ │ │ +

Recovering translations in an epipolar graph when rotations are given. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::TranslationRecovery
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -Rot3 gtsam::openGLFixedRotation ()
 
Pose3 gtsam::openGL2gtsam (const Rot3 &R, double tx, double ty, double tz)
 This function converts an openGL camera pose to an GTSAM camera pose.
 
Pose3 gtsam::gtsam2openGL (const Rot3 &R, double tx, double ty, double tz)
 This function converts a GTSAM camera pose to an openGL camera pose.
 
Pose3 gtsam::gtsam2openGL (const Pose3 &PoseGTSAM)
 This function converts a GTSAM camera pose to an openGL camera pose.
 
bool gtsam::writeBAL (const std::string &filename, const SfmData &data)
 This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure.
 
SfmData gtsam::readBal (const std::string &filename)
 This function parses a "Bundle Adjustment in the Large" (BAL) file and returns the data as a SfmData structure.
 
bool gtsam::writeBALfromValues (const std::string &filename, const SfmData &data, const Values &values)
 This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure and a value structure (measurements are the same as the SfM input data, while camera poses and values are read from Values)
 
Values gtsam::initialCamerasEstimate (const SfmData &db)
 This function creates initial values for cameras from db.
 
Values gtsam::initialCamerasAndPointsEstimate (const SfmData &db)
 This function creates initial values for cameras and points from db.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Data structure for dealing with Structure from Motion data.

│ │ │ │ -
Date
January 2022
│ │ │ │ -
Author
Frank dellaert
│ │ │ │ +

Recovering translations in an epipolar graph when rotations are given.

│ │ │ │ +
Author
Frank Dellaert, Akshay Krishnan
│ │ │ │ +
Date
March 2020
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,55 +1,26 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -SfmData.cpp File Reference │ │ │ │ │ -Data structure for dealing with Structure from Motion data. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +TranslationRecovery.h File Reference │ │ │ │ │ +Recovering translations in an epipolar graph when rotations are given. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ - _R_o_t_3  ggttssaamm::::ooppeennGGLLFFiixxeeddRRoottaattiioonn () │ │ │ │ │ -  │ │ │ │ │ - _P_o_s_e_3  _g_t_s_a_m_:_:_o_p_e_n_G_L_2_g_t_s_a_m (const _R_o_t_3 &R, double tx, double ty, double tz) │ │ │ │ │ -  This function converts an openGL camera pose to an GTSAM camera pose. │ │ │ │ │ -  │ │ │ │ │ - _P_o_s_e_3  _g_t_s_a_m_:_:_g_t_s_a_m_2_o_p_e_n_G_L (const _R_o_t_3 &R, double tx, double ty, double tz) │ │ │ │ │ -  This function converts a GTSAM camera pose to an openGL camera pose. │ │ │ │ │ -  │ │ │ │ │ - _P_o_s_e_3  _g_t_s_a_m_:_:_g_t_s_a_m_2_o_p_e_n_G_L (const _P_o_s_e_3 &PoseGTSAM) │ │ │ │ │ -  This function converts a GTSAM camera pose to an openGL camera pose. │ │ │ │ │ -  │ │ │ │ │ - bool  _g_t_s_a_m_:_:_w_r_i_t_e_B_A_L (const std::string &filename, const _S_f_m_D_a_t_a &data) │ │ │ │ │ -  This function writes a "Bundle Adjustment in the Large" (BAL) file │ │ │ │ │ - from a _S_f_m_D_a_t_a structure. │ │ │ │ │ -  │ │ │ │ │ -_S_f_m_D_a_t_a  _g_t_s_a_m_:_:_r_e_a_d_B_a_l (const std::string &filename) │ │ │ │ │ -  This function parses a "Bundle Adjustment in the Large" (BAL) file and │ │ │ │ │ - returns the data as a _S_f_m_D_a_t_a structure. │ │ │ │ │ -  │ │ │ │ │ - bool  _g_t_s_a_m_:_:_w_r_i_t_e_B_A_L_f_r_o_m_V_a_l_u_e_s (const std::string &filename, const _S_f_m_D_a_t_a │ │ │ │ │ - &data, const _V_a_l_u_e_s &values) │ │ │ │ │ - This function writes a "Bundle Adjustment in the Large" (BAL) file │ │ │ │ │ -  from a _S_f_m_D_a_t_a structure and a value structure (measurements are the │ │ │ │ │ - same as the SfM input data, while camera poses and values are read │ │ │ │ │ - from _V_a_l_u_e_s) │ │ │ │ │ -  │ │ │ │ │ - _V_a_l_u_e_s  _g_t_s_a_m_:_:_i_n_i_t_i_a_l_C_a_m_e_r_a_s_E_s_t_i_m_a_t_e (const _S_f_m_D_a_t_a &db) │ │ │ │ │ -  This function creates initial values for cameras from db. │ │ │ │ │ -  │ │ │ │ │ - _V_a_l_u_e_s  _g_t_s_a_m_:_:_i_n_i_t_i_a_l_C_a_m_e_r_a_s_A_n_d_P_o_i_n_t_s_E_s_t_i_m_a_t_e (const _S_f_m_D_a_t_a &db) │ │ │ │ │ -  This function creates initial values for cameras and points from db. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Data structure for dealing with Structure from Motion data. │ │ │ │ │ - Date │ │ │ │ │ - January 2022 │ │ │ │ │ +Recovering translations in an epipolar graph when rotations are given. │ │ │ │ │ Author │ │ │ │ │ - Frank dellaert │ │ │ │ │ + Frank Dellaert, Akshay Krishnan │ │ │ │ │ + Date │ │ │ │ │ + March 2020 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_f_m │ │ │ │ │ - * _S_f_m_D_a_t_a_._c_p_p │ │ │ │ │ + * _T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01193.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,10 +1,3 @@ │ │ │ │ │ var a01193 = [ │ │ │ │ │ - ["gtsam2openGL", "a01193.html#ad632ed56581bf174eef379bbe272a06a", null], │ │ │ │ │ - ["gtsam2openGL", "a01193.html#aaff07ef70e6161c4d1d2bd7546ff91e6", null], │ │ │ │ │ - ["initialCamerasAndPointsEstimate", "a01193.html#aba34ed4a103d9057536ee5cac27cfdef", null], │ │ │ │ │ - ["initialCamerasEstimate", "a01193.html#ad8b55993353cbab0fde06b872783fe92", null], │ │ │ │ │ - ["openGL2gtsam", "a01193.html#ade6eca65dc0168a65f641961ca97924c", null], │ │ │ │ │ - ["readBal", "a01193.html#a710ec0322f4a2c0883841b58516b7a6a", null], │ │ │ │ │ - ["writeBAL", "a01193.html#a43c2c2ead4fddd79d9c3f9a5bfa764ae", null], │ │ │ │ │ - ["writeBALfromValues", "a01193.html#a4a2433812cf34ee359e3ec99f4f5bcc4", null] │ │ │ │ │ + ["gtsam::TranslationRecovery", "a04756.html", "a04756"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01196.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/ShonanFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/MFAS.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -97,52 +97,49 @@ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ Typedefs
│ │ │ │ -
ShonanFactor.h File Reference
│ │ │ │ +
MFAS.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Main factor type in Shonan averaging, on SO(n) pairs. │ │ │ │ +

MFAS class to solve Minimum Feedback Arc Set graph problem. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::ShonanFactor< d >
 ShonanFactor is a BetweenFactor that moves in SO(p), but will land on the SO(d) sub-manifold of SO(p) at the global minimum. More...
class  gtsam::MFAS
 The MFAS class to solve a Minimum feedback arc set (MFAS) problem. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Typedefs

│ │ │ │ -using gtsam::ShonanFactor2 = ShonanFactor< 2 >
 
│ │ │ │ -using gtsam::ShonanFactor3 = ShonanFactor< 3 >
 
│ │ │ │ +typedef std::map< std::pair< Key, Key >, double > gtsam::KeyPairDoubleMap
 
│ │ │ │

Detailed Description

│ │ │ │ -

Main factor type in Shonan averaging, on SO(n) pairs.

│ │ │ │ -
Date
March 2019
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +

MFAS class to solve Minimum Feedback Arc Set graph problem.

│ │ │ │ +
Author
Akshay Krishnan
│ │ │ │ +
Date
September 2020
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,33 +1,31 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ -ShonanFactor.h File Reference │ │ │ │ │ -Main factor type in Shonan averaging, on SO(n) pairs. _M_o_r_e_._._. │ │ │ │ │ +MFAS.h File Reference │ │ │ │ │ +MFAS class to solve Minimum Feedback Arc Set graph problem. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_S_h_o_n_a_n_F_a_c_t_o_r_<_ _d_ _> │ │ │ │ │ -  _S_h_o_n_a_n_F_a_c_t_o_r is a _B_e_t_w_e_e_n_F_a_c_t_o_r that moves in SO(p), but will land on │ │ │ │ │ - the SO(d) sub-manifold of SO(p) at the global minimum. _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_M_F_A_S │ │ │ │ │ +  The _M_F_A_S class to solve a Minimum feedback arc set (_M_F_A_S) problem. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ TTyyppeeddeeffss │ │ │ │ │ -using  ggttssaamm::::SShhoonnaannFFaaccttoorr22 = _S_h_o_n_a_n_F_a_c_t_o_r< 2 > │ │ │ │ │ -  │ │ │ │ │ -using  ggttssaamm::::SShhoonnaannFFaaccttoorr33 = _S_h_o_n_a_n_F_a_c_t_o_r< 3 > │ │ │ │ │ +typedef std::map< std::pair< _K_e_y, _K_e_y >, double >  ggttssaamm::::KKeeyyPPaaiirrDDoouubblleeMMaapp │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Main factor type in Shonan averaging, on SO(n) pairs. │ │ │ │ │ - Date │ │ │ │ │ - March 2019 │ │ │ │ │ +MFAS class to solve Minimum Feedback Arc Set graph problem. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Akshay Krishnan │ │ │ │ │ + Date │ │ │ │ │ + September 2020 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_f_m │ │ │ │ │ - * _S_h_o_n_a_n_F_a_c_t_o_r_._h │ │ │ │ │ + * _M_F_A_S_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01196_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/ShonanFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/MFAS.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,99 +98,82 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ShonanFactor.h
│ │ │ │ +
MFAS.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/geometry/Rot2.h>
│ │ │ │ -
22#include <gtsam/geometry/Rot3.h>
│ │ │ │ -
23#include <gtsam/geometry/SOn.h>
│ │ │ │ -
24#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
25
│ │ │ │ -
26#include <type_traits>
│ │ │ │ -
27
│ │ │ │ -
28namespace gtsam {
│ │ │ │ -
29
│ │ │ │ -
35template <size_t d>
│ │ │ │ -
│ │ │ │ -
36class GTSAM_EXPORT ShonanFactor : public NoiseModelFactorN<SOn, SOn> {
│ │ │ │ -
37 Matrix M_;
│ │ │ │ -
38 size_t p_, pp_;
│ │ │ │ -
39 boost::shared_ptr<Matrix> G_;
│ │ │ │ -
40
│ │ │ │ -
41 // Select Rot2 or Rot3 interface based template parameter d
│ │ │ │ -
42 using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;
│ │ │ │ -
43
│ │ │ │ -
44public:
│ │ │ │ -
47
│ │ │ │ -
51 ShonanFactor(Key j1, Key j2, const Rot &R12, size_t p,
│ │ │ │ -
52 const SharedNoiseModel &model = nullptr,
│ │ │ │ -
53 const boost::shared_ptr<Matrix> &G = nullptr);
│ │ │ │ -
54
│ │ │ │ -
58
│ │ │ │ -
60 void
│ │ │ │ -
61 print(const std::string &s,
│ │ │ │ -
62 const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override;
│ │ │ │ -
63
│ │ │ │ -
65 bool equals(const NonlinearFactor &expected,
│ │ │ │ -
66 double tol = 1e-9) const override;
│ │ │ │ -
67
│ │ │ │ +
12#pragma once
│ │ │ │ +
13
│ │ │ │ +
21#include <gtsam/geometry/Unit3.h>
│ │ │ │ +
22#include <gtsam/inference/Key.h>
│ │ │ │ +
23#include <gtsam/sfm/BinaryMeasurement.h>
│ │ │ │ +
24
│ │ │ │ +
25#include <memory>
│ │ │ │ +
26#include <unordered_map>
│ │ │ │ +
27#include <vector>
│ │ │ │ +
28
│ │ │ │ +
29namespace gtsam {
│ │ │ │ +
30
│ │ │ │ +
│ │ │ │ +
51class GTSAM_EXPORT MFAS {
│ │ │ │ +
52 public:
│ │ │ │ +
53 // used to represent edges between two nodes in the graph. When used in
│ │ │ │ +
54 // translation averaging for global SfM
│ │ │ │ +
55 using KeyPair = std::pair<Key, Key>;
│ │ │ │ +
56 using TranslationEdges = std::vector<BinaryMeasurement<Unit3>>;
│ │ │ │ +
57
│ │ │ │ +
58 private:
│ │ │ │ +
59 // edges with a direction such that all weights are positive
│ │ │ │ +
60 // i.e, edges that originally had negative weights are flipped
│ │ │ │ +
61 std::map<KeyPair, double> edgeWeights_;
│ │ │ │ +
62
│ │ │ │ +
63 public:
│ │ │ │ +
│ │ │ │ +
69 MFAS(const std::map<KeyPair, double> &edgeWeights)
│ │ │ │ +
70 : edgeWeights_(edgeWeights) {}
│ │ │ │ +
│ │ │ │
71
│ │ │ │ -
74 Vector
│ │ │ │ -
75 evaluateError(const SOn &Q1, const SOn &Q2,
│ │ │ │ -
76 boost::optional<Matrix &> H1 = boost::none,
│ │ │ │ -
77 boost::optional<Matrix &> H2 = boost::none) const override;
│ │ │ │ -
79
│ │ │ │ -
80private:
│ │ │ │ -
82 void fillJacobians(const Matrix &M1, const Matrix &M2,
│ │ │ │ -
83 boost::optional<Matrix &> H1,
│ │ │ │ -
84 boost::optional<Matrix &> H2) const;
│ │ │ │ -
85};
│ │ │ │ -
│ │ │ │ -
86
│ │ │ │ -
87// Explicit instantiation for d=2 and d=3 in .cpp file:
│ │ │ │ -
88using ShonanFactor2 = ShonanFactor<2>;
│ │ │ │ -
89using ShonanFactor3 = ShonanFactor<3>;
│ │ │ │ -
90
│ │ │ │ -
91} // namespace gtsam
│ │ │ │ -
SOn.h
N*N matrix representation of SO(N).
│ │ │ │ -
Rot3.h
3D rotation represented as a rotation matrix or quaternion
│ │ │ │ -
Rot2.h
2D rotation
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
80 MFAS(const TranslationEdges &relativeTranslations,
│ │ │ │ +
81 const Unit3 &projectionDirection);
│ │ │ │ +
82
│ │ │ │ +
87 KeyVector computeOrdering() const;
│ │ │ │ +
88
│ │ │ │ +
96 std::map<KeyPair, double> computeOutlierWeights() const;
│ │ │ │ +
97};
│ │ │ │ +
│ │ │ │ +
98
│ │ │ │ +
99typedef std::map<std::pair<Key, Key>, double> KeyPairDoubleMap;
│ │ │ │ +
100
│ │ │ │ +
101} // namespace gtsam
│ │ │ │ + │ │ │ │ +
BinaryMeasurement.h
Binary measurement represents a measurement between two keys in a graph. A binary measurement is simi...
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::SO
Manifold of special orthogonal rotation matrices SO<N>.
Definition SOn.h:52
│ │ │ │ -
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ -
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ -
gtsam::ShonanFactor
ShonanFactor is a BetweenFactor that moves in SO(p), but will land on the SO(d) sub-manifold of SO(p)...
Definition ShonanFactor.h:36
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
│ │ │ │ +
gtsam::MFAS
The MFAS class to solve a Minimum feedback arc set (MFAS) problem.
Definition MFAS.h:51
│ │ │ │ +
gtsam::MFAS::MFAS
MFAS(const std::map< KeyPair, double > &edgeWeights)
Construct from the weighted directed edges between the nodes.
Definition MFAS.h:69
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,119 +1,84 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ShonanFactor.h │ │ │ │ │ +MFAS.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_2_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_3_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_S_O_n_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -25 │ │ │ │ │ -26#include │ │ │ │ │ -27 │ │ │ │ │ -28namespace _g_t_s_a_m { │ │ │ │ │ -29 │ │ │ │ │ -35template │ │ │ │ │ -_3_6class GTSAM_EXPORT _S_h_o_n_a_n_F_a_c_t_o_r : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -37 Matrix M_; │ │ │ │ │ -38 size_t p_, pp_; │ │ │ │ │ -39 boost::shared_ptr G_; │ │ │ │ │ -40 │ │ │ │ │ -41 // Select Rot2 or Rot3 interface based template parameter d │ │ │ │ │ -42 using Rot = typename std::conditional::type; │ │ │ │ │ -43 │ │ │ │ │ -44public: │ │ │ │ │ -47 │ │ │ │ │ -51 _S_h_o_n_a_n_F_a_c_t_o_r(_K_e_y j1, _K_e_y j2, const Rot &R12, size_t p, │ │ │ │ │ -52 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model = nullptr, │ │ │ │ │ -53 const boost::shared_ptr &G = nullptr); │ │ │ │ │ -54 │ │ │ │ │ -58 │ │ │ │ │ -60 void │ │ │ │ │ -61 _p_r_i_n_t(const std::string &s, │ │ │ │ │ -62 const _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter = DefaultKeyFormatter) const override; │ │ │ │ │ -63 │ │ │ │ │ -65 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r &expected, │ │ │ │ │ -66 double tol = 1e-9) const override; │ │ │ │ │ -67 │ │ │ │ │ +12#pragma once │ │ │ │ │ +13 │ │ │ │ │ +21#include │ │ │ │ │ +22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_s_f_m_/_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_._h> │ │ │ │ │ +24 │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28 │ │ │ │ │ +29namespace _g_t_s_a_m { │ │ │ │ │ +30 │ │ │ │ │ +_5_1class GTSAM_EXPORT _M_F_A_S { │ │ │ │ │ +52 public: │ │ │ │ │ +53 // used to represent edges between two nodes in the graph. When used in │ │ │ │ │ +54 // translation averaging for global SfM │ │ │ │ │ +55 using KeyPair = std::pair; │ │ │ │ │ +56 using TranslationEdges = std::vector>; │ │ │ │ │ +57 │ │ │ │ │ +58 private: │ │ │ │ │ +59 // edges with a direction such that all weights are positive │ │ │ │ │ +60 // i.e, edges that originally had negative weights are flipped │ │ │ │ │ +61 std::map edgeWeights_; │ │ │ │ │ +62 │ │ │ │ │ +63 public: │ │ │ │ │ +_6_9 _M_F_A_S(const std::map &edgeWeights) │ │ │ │ │ +70 : edgeWeights_(edgeWeights) {} │ │ │ │ │ 71 │ │ │ │ │ -74 Vector │ │ │ │ │ -75 evaluateError(const _S_O_n &Q1, const _S_O_n &Q2, │ │ │ │ │ -76 boost::optional H1 = boost::none, │ │ │ │ │ -77 boost::optional H2 = boost::none) const override; │ │ │ │ │ -79 │ │ │ │ │ -80private: │ │ │ │ │ -82 void fillJacobians(const Matrix &M1, const Matrix &M2, │ │ │ │ │ -83 boost::optional H1, │ │ │ │ │ -84 boost::optional H2) const; │ │ │ │ │ -85}; │ │ │ │ │ -86 │ │ │ │ │ -87// Explicit instantiation for d=2 and d=3 in .cpp file: │ │ │ │ │ -88using ShonanFactor2 = _S_h_o_n_a_n_F_a_c_t_o_r_<_2_>; │ │ │ │ │ -89using ShonanFactor3 = _S_h_o_n_a_n_F_a_c_t_o_r_<_3_>; │ │ │ │ │ -90 │ │ │ │ │ -91} // namespace gtsam │ │ │ │ │ -_S_O_n_._h │ │ │ │ │ -N*N matrix representation of SO(N). │ │ │ │ │ -_R_o_t_3_._h │ │ │ │ │ -3D rotation represented as a rotation matrix or quaternion │ │ │ │ │ -_R_o_t_2_._h │ │ │ │ │ -2D rotation │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ -Non-linear factor base classes. │ │ │ │ │ +80 _M_F_A_S(const TranslationEdges &relativeTranslations, │ │ │ │ │ +81 const _U_n_i_t_3 &projectionDirection); │ │ │ │ │ +82 │ │ │ │ │ +87 _K_e_y_V_e_c_t_o_r computeOrdering() const; │ │ │ │ │ +88 │ │ │ │ │ +96 std::map computeOutlierWeights() const; │ │ │ │ │ +97}; │ │ │ │ │ +98 │ │ │ │ │ +99typedef std::map, double> KeyPairDoubleMap; │ │ │ │ │ +100 │ │ │ │ │ +101} // namespace gtsam │ │ │ │ │ +_K_e_y_._h │ │ │ │ │ +_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_._h │ │ │ │ │ +Binary measurement represents a measurement between two keys in a graph. A │ │ │ │ │ +binary measurement is simi... │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ -Aliases. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O │ │ │ │ │ -Manifold of special orthogonal rotation matrices SO. │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ -Nonlinear factor base class. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ -A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ -variables. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_F_a_c_t_o_r │ │ │ │ │ -ShonanFactor is a BetweenFactor that moves in SO(p), but will land on the SO(d) │ │ │ │ │ -sub-manifold of SO(p)... │ │ │ │ │ -DDeeffiinniittiioonn ShonanFactor.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_U_n_i_t_3 │ │ │ │ │ +Represents a 3D point on a unit sphere. │ │ │ │ │ +DDeeffiinniittiioonn Unit3.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_M_F_A_S │ │ │ │ │ +The MFAS class to solve a Minimum feedback arc set (MFAS) problem. │ │ │ │ │ +DDeeffiinniittiioonn MFAS.h:51 │ │ │ │ │ +_g_t_s_a_m_:_:_M_F_A_S_:_:_M_F_A_S │ │ │ │ │ +MFAS(const std::map< KeyPair, double > &edgeWeights) │ │ │ │ │ +Construct from the weighted directed edges between the nodes. │ │ │ │ │ +DDeeffiinniittiioonn MFAS.h:69 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_f_m │ │ │ │ │ - * _S_h_o_n_a_n_F_a_c_t_o_r_._h │ │ │ │ │ + * _M_F_A_S_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01199.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/MFAS.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/TranslationRecovery.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,52 +94,43 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs
│ │ │ │ -
MFAS.h File Reference
│ │ │ │ +Functions
│ │ │ │ +
TranslationRecovery.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

MFAS class to solve Minimum Feedback Arc Set graph problem. │ │ │ │ +

Source code for recovering translations when rotations are given. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::MFAS
 The MFAS class to solve a Minimum feedback arc set (MFAS) problem. More...
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ -Typedefs

│ │ │ │ -typedef std::map< std::pair< Key, Key >, double > gtsam::KeyPairDoubleMap
 

│ │ │ │ +Functions

│ │ │ │ +DSFMap< Key > getSameTranslationDSFMap (const std::vector< BinaryMeasurement< Unit3 > > &relativeTranslations)
 
│ │ │ │ +template<typename T >
std::vector< BinaryMeasurement< T > > removeSameTranslationNodes (const std::vector< BinaryMeasurement< T > > &edges, const DSFMap< Key > &sameTranslationDSFMap)
 
│ │ │ │ +Values addSameTranslationNodes (const Values &result, const DSFMap< Key > &sameTranslationDSFMap)
 
│ │ │ │

Detailed Description

│ │ │ │ -

MFAS class to solve Minimum Feedback Arc Set graph problem.

│ │ │ │ -
Author
Akshay Krishnan
│ │ │ │ -
Date
September 2020
│ │ │ │ +

Source code for recovering translations when rotations are given.

│ │ │ │ +
Author
Frank Dellaert, Akshay Krishnan
│ │ │ │ +
Date
March 2020
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,31 +1,33 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ -MFAS.h File Reference │ │ │ │ │ -MFAS class to solve Minimum Feedback Arc Set graph problem. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_M_F_A_S │ │ │ │ │ -  The _M_F_A_S class to solve a Minimum feedback arc set (_M_F_A_S) problem. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ +_F_u_n_c_t_i_o_n_s │ │ │ │ │ +TranslationRecovery.cpp File Reference │ │ │ │ │ +Source code for recovering translations when rotations are given. _M_o_r_e_._._. │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ + _D_S_F_M_a_p< _K_e_y >  ggeettSSaammeeTTrraannssllaattiioonnDDSSFFMMaapp (const std:: │ │ │ │ │ + vector< _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t< _U_n_i_t_3 > > │ │ │ │ │ + &relativeTranslations) │ │ │ │ │   │ │ │ │ │ -NNaammeessppaacceess │ │ │ │ │ -namespace   _g_t_s_a_m │ │ │ │ │ -  Global functions in a separate testing namespace. │ │ │ │ │ +template │ │ │ │ │ +std::vector< _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t< T > >  rreemmoovveeSSaammeeTTrraannssllaattiioonnNNooddeess (const std:: │ │ │ │ │ + vector< _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t< T > > &edges, │ │ │ │ │ + const _D_S_F_M_a_p< _K_e_y > │ │ │ │ │ + &sameTranslationDSFMap) │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -typedef std::map< std::pair< _K_e_y, _K_e_y >, double >  ggttssaamm::::KKeeyyPPaaiirrDDoouubblleeMMaapp │ │ │ │ │ + _V_a_l_u_e_s  aaddddSSaammeeTTrraannssllaattiioonnNNooddeess (const _V_a_l_u_e_s │ │ │ │ │ + &result, const _D_S_F_M_a_p< _K_e_y > │ │ │ │ │ + &sameTranslationDSFMap) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -MFAS class to solve Minimum Feedback Arc Set graph problem. │ │ │ │ │ +Source code for recovering translations when rotations are given. │ │ │ │ │ Author │ │ │ │ │ - Akshay Krishnan │ │ │ │ │ + Frank Dellaert, Akshay Krishnan │ │ │ │ │ Date │ │ │ │ │ - September 2020 │ │ │ │ │ + March 2020 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_f_m │ │ │ │ │ - * _M_F_A_S_._h │ │ │ │ │ + * _T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01205.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/SfmTrack.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/TranslationFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,36 +94,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
SfmTrack.cpp File Reference
│ │ │ │ +
TranslationFactor.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

A simple data structure for a track in Structure from Motion. │ │ │ │ +

Binary factor for a relative translation direction measurement. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::TranslationFactor
 Binary factor for a relative translation direction measurement w_aZb. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

A simple data structure for a track in Structure from Motion.

│ │ │ │ -
Date
January 2022
│ │ │ │ +

Binary factor for a relative translation direction measurement.

│ │ │ │
Author
Frank Dellaert
│ │ │ │ +
Date
March 2020
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SfmTrack.cpp File Reference │ │ │ │ │ -A simple data structure for a track in Structure from Motion. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +TranslationFactor.h File Reference │ │ │ │ │ +Binary factor for a relative translation direction measurement. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_T_r_a_n_s_l_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +  Binary factor for a relative translation direction measurement w_aZb. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A simple data structure for a track in Structure from Motion. │ │ │ │ │ - Date │ │ │ │ │ - January 2022 │ │ │ │ │ +Binary factor for a relative translation direction measurement. │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + March 2020 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_f_m │ │ │ │ │ - * _S_f_m_T_r_a_c_k_._c_p_p │ │ │ │ │ + * _T_r_a_n_s_l_a_t_i_o_n_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01208.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/DsfTrackGenerator.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/BinaryMeasurement.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,95 +94,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
│ │ │ │ -
DsfTrackGenerator.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
BinaryMeasurement.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Identifies connected components in the keypoint matches graph. │ │ │ │ +

Binary measurement represents a measurement between two keys in a graph. A binary measurement is similar to a BetweenFactor, except that it does not contain an error function. It is a measurement (along with a noise model) from one key to another. Note that the direction is important. A measurement from key1 to key2 is not the same as the same measurement from key2 to key1. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::BinaryMeasurement< T >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Typedefs

│ │ │ │ -typedef DSFMap< IndexPair > gtsam::gtsfm::DSFMapIndexPair
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

std::vector< SfmTrack2d > gtsam::gtsfm::tracksFromPairwiseMatches (const MatchIndicesMap &matches, const KeypointsVector &keypoints, bool verbose)
 Creates a list of tracks from 2d point correspondences.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Identifies connected components in the keypoint matches graph.

│ │ │ │ -
Date
October 2022
│ │ │ │ -
Author
John Lambert
│ │ │ │ -

Function Documentation

│ │ │ │ - │ │ │ │ -

◆ tracksFromPairwiseMatches()

│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
std::vector< SfmTrack2d > gtsam::gtsfm::tracksFromPairwiseMatches (const MatchIndicesMap & matches,
const KeypointsVector & keypoints,
bool verbose 
)
│ │ │ │ -
│ │ │ │ - │ │ │ │ -

Creates a list of tracks from 2d point correspondences.

│ │ │ │ -

Creates a disjoint-set forest (DSF) and 2d tracks from pairwise matches. We create a singleton for union-find set elements from camera index of a detection and the index of that detection in that camera's keypoint list, i.e. (i,k).

│ │ │ │ -
Parameters
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
Mapfrom (i1,i2) image pair indices to (K,2) matrix, for K correspondence indices, from each image.
Length-Nlist of keypoints, for N images/cameras.
│ │ │ │ -
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ +

Binary measurement represents a measurement between two keys in a graph. A binary measurement is similar to a BetweenFactor, except that it does not contain an error function. It is a measurement (along with a noise model) from one key to another. Note that the direction is important. A measurement from key1 to key2 is not the same as the same measurement from key2 to key1.

│ │ │ │ +
Author
Akshay Krishnan
│ │ │ │ +
Date
July 2020
│ │ │ │ +
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,47 +1,34 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -DsfTrackGenerator.cpp File Reference │ │ │ │ │ -Identifies connected components in the keypoint matches graph. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +BinaryMeasurement.h File Reference │ │ │ │ │ +Binary measurement represents a measurement between two keys in a graph. A │ │ │ │ │ +binary measurement is similar to a BetweenFactor, except that it does not │ │ │ │ │ +contain an error function. It is a measurement (along with a noise model) from │ │ │ │ │ +one key to another. Note that the direction is important. A measurement from │ │ │ │ │ +key1 to key2 is not the same as the same measurement from key2 to key1. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_<_ _T_ _> │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -typedef _D_S_F_M_a_p< _I_n_d_e_x_P_a_i_r >  ggttssaamm::::ggttssffmm::::DDSSFFMMaappIInnddeexxPPaaiirr │ │ │ │ │ -  │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -std::vector< _S_f_m_T_r_a_c_k_2_d >  _g_t_s_a_m_:_:_g_t_s_f_m_:_:_t_r_a_c_k_s_F_r_o_m_P_a_i_r_w_i_s_e_M_a_t_c_h_e_s (const │ │ │ │ │ - MatchIndicesMap &matches, const KeypointsVector │ │ │ │ │ - &keypoints, bool verbose) │ │ │ │ │ -  Creates a list of tracks from 2d point │ │ │ │ │ - correspondences. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Identifies connected components in the keypoint matches graph. │ │ │ │ │ - Date │ │ │ │ │ - October 2022 │ │ │ │ │ +Binary measurement represents a measurement between two keys in a graph. A │ │ │ │ │ +binary measurement is similar to a BetweenFactor, except that it does not │ │ │ │ │ +contain an error function. It is a measurement (along with a noise model) from │ │ │ │ │ +one key to another. Note that the direction is important. A measurement from │ │ │ │ │ +key1 to key2 is not the same as the same measurement from key2 to key1. │ │ │ │ │ Author │ │ │ │ │ - John Lambert │ │ │ │ │ -********** FFuunnccttiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ -********** _?◆_? ttrraacckkssFFrroommPPaaiirrwwiisseeMMaattcchheess(()) ********** │ │ │ │ │ -std::vector< SfmTrack2d > gtsam::gtsfm:: ( const MatchIndicesMap &  mmaattcchheess, │ │ │ │ │ -tracksFromPairwiseMatches │ │ │ │ │ - const KeypointsVector &  kkeeyyppooiinnttss, │ │ │ │ │ - bool  vveerrbboossee  │ │ │ │ │ - ) │ │ │ │ │ -Creates a list of tracks from 2d point correspondences. │ │ │ │ │ -Creates a disjoint-set forest (DSF) and 2d tracks from pairwise matches. We │ │ │ │ │ -create a singleton for union-find set elements from camera index of a detection │ │ │ │ │ -and the index of that detection in that camera's keypoint list, i.e. (i,k). │ │ │ │ │ - Parameters │ │ │ │ │ - Map from (i1,i2) image pair indices to (K,2) matrix, for K │ │ │ │ │ - correspondence indices, from each image. │ │ │ │ │ - Length-N list of keypoints, for N images/cameras. │ │ │ │ │ + Akshay Krishnan │ │ │ │ │ + Date │ │ │ │ │ + July 2020 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_f_m │ │ │ │ │ - * _D_s_f_T_r_a_c_k_G_e_n_e_r_a_t_o_r_._c_p_p │ │ │ │ │ + * _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01208.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a01208 = [ │ │ │ │ │ - ["tracksFromPairwiseMatches", "a01208.html#ac50218675ff25e1e9202d335ecfa6b3a", null] │ │ │ │ │ + ["gtsam::BinaryMeasurement< T >", "a04688.html", "a04688"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01211.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/BinaryMeasurement.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/DsfTrackGenerator.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,44 +94,95 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
BinaryMeasurement.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ +Functions
│ │ │ │ +
DsfTrackGenerator.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Binary measurement represents a measurement between two keys in a graph. A binary measurement is similar to a BetweenFactor, except that it does not contain an error function. It is a measurement (along with a noise model) from one key to another. Note that the direction is important. A measurement from key1 to key2 is not the same as the same measurement from key2 to key1. │ │ │ │ +

Identifies connected components in the keypoint matches graph. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::BinaryMeasurement< T >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Typedefs

│ │ │ │ +typedef DSFMap< IndexPair > gtsam::gtsfm::DSFMapIndexPair
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

std::vector< SfmTrack2d > gtsam::gtsfm::tracksFromPairwiseMatches (const MatchIndicesMap &matches, const KeypointsVector &keypoints, bool verbose)
 Creates a list of tracks from 2d point correspondences.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Binary measurement represents a measurement between two keys in a graph. A binary measurement is similar to a BetweenFactor, except that it does not contain an error function. It is a measurement (along with a noise model) from one key to another. Note that the direction is important. A measurement from key1 to key2 is not the same as the same measurement from key2 to key1.

│ │ │ │ -
Author
Akshay Krishnan
│ │ │ │ -
Date
July 2020
│ │ │ │ -
│ │ │ │ +

Identifies connected components in the keypoint matches graph.

│ │ │ │ +
Date
October 2022
│ │ │ │ +
Author
John Lambert
│ │ │ │ +

Function Documentation

│ │ │ │ + │ │ │ │ +

◆ tracksFromPairwiseMatches()

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
std::vector< SfmTrack2d > gtsam::gtsfm::tracksFromPairwiseMatches (const MatchIndicesMap & matches,
const KeypointsVector & keypoints,
bool verbose 
)
│ │ │ │ +
│ │ │ │ + │ │ │ │ +

Creates a list of tracks from 2d point correspondences.

│ │ │ │ +

Creates a disjoint-set forest (DSF) and 2d tracks from pairwise matches. We create a singleton for union-find set elements from camera index of a detection and the index of that detection in that camera's keypoint list, i.e. (i,k).

│ │ │ │ +
Parameters
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
Mapfrom (i1,i2) image pair indices to (K,2) matrix, for K correspondence indices, from each image.
Length-Nlist of keypoints, for N images/cameras.
│ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,34 +1,47 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -BinaryMeasurement.h File Reference │ │ │ │ │ -Binary measurement represents a measurement between two keys in a graph. A │ │ │ │ │ -binary measurement is similar to a BetweenFactor, except that it does not │ │ │ │ │ -contain an error function. It is a measurement (along with a noise model) from │ │ │ │ │ -one key to another. Note that the direction is important. A measurement from │ │ │ │ │ -key1 to key2 is not the same as the same measurement from key2 to key1. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_<_ _T_ _> │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +DsfTrackGenerator.cpp File Reference │ │ │ │ │ +Identifies connected components in the keypoint matches graph. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +typedef _D_S_F_M_a_p< _I_n_d_e_x_P_a_i_r >  ggttssaamm::::ggttssffmm::::DDSSFFMMaappIInnddeexxPPaaiirr │ │ │ │ │ +  │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +std::vector< _S_f_m_T_r_a_c_k_2_d >  _g_t_s_a_m_:_:_g_t_s_f_m_:_:_t_r_a_c_k_s_F_r_o_m_P_a_i_r_w_i_s_e_M_a_t_c_h_e_s (const │ │ │ │ │ + MatchIndicesMap &matches, const KeypointsVector │ │ │ │ │ + &keypoints, bool verbose) │ │ │ │ │ +  Creates a list of tracks from 2d point │ │ │ │ │ + correspondences. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Binary measurement represents a measurement between two keys in a graph. A │ │ │ │ │ -binary measurement is similar to a BetweenFactor, except that it does not │ │ │ │ │ -contain an error function. It is a measurement (along with a noise model) from │ │ │ │ │ -one key to another. Note that the direction is important. A measurement from │ │ │ │ │ -key1 to key2 is not the same as the same measurement from key2 to key1. │ │ │ │ │ - Author │ │ │ │ │ - Akshay Krishnan │ │ │ │ │ +Identifies connected components in the keypoint matches graph. │ │ │ │ │ Date │ │ │ │ │ - July 2020 │ │ │ │ │ + October 2022 │ │ │ │ │ + Author │ │ │ │ │ + John Lambert │ │ │ │ │ +********** FFuunnccttiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ +********** _?◆_? ttrraacckkssFFrroommPPaaiirrwwiisseeMMaattcchheess(()) ********** │ │ │ │ │ +std::vector< SfmTrack2d > gtsam::gtsfm:: ( const MatchIndicesMap &  mmaattcchheess, │ │ │ │ │ +tracksFromPairwiseMatches │ │ │ │ │ + const KeypointsVector &  kkeeyyppooiinnttss, │ │ │ │ │ + bool  vveerrbboossee  │ │ │ │ │ + ) │ │ │ │ │ +Creates a list of tracks from 2d point correspondences. │ │ │ │ │ +Creates a disjoint-set forest (DSF) and 2d tracks from pairwise matches. We │ │ │ │ │ +create a singleton for union-find set elements from camera index of a detection │ │ │ │ │ +and the index of that detection in that camera's keypoint list, i.e. (i,k). │ │ │ │ │ + Parameters │ │ │ │ │ + Map from (i1,i2) image pair indices to (K,2) matrix, for K │ │ │ │ │ + correspondence indices, from each image. │ │ │ │ │ + Length-N list of keypoints, for N images/cameras. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_f_m │ │ │ │ │ - * _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_._h │ │ │ │ │ + * _D_s_f_T_r_a_c_k_G_e_n_e_r_a_t_o_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01211.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a01211 = [ │ │ │ │ │ - ["gtsam::BinaryMeasurement< T >", "a04688.html", "a04688"] │ │ │ │ │ + ["tracksFromPairwiseMatches", "a01211.html#ac50218675ff25e1e9202d335ecfa6b3a", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01214.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/ShonanAveraging.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/SfmTrack.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,36 +94,64 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces
│ │ │ │ -
ShonanAveraging.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces | │ │ │ │ +Typedefs
│ │ │ │ +
SfmTrack.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Shonan Averaging algorithm. │ │ │ │ +

A simple data structure for a track in Structure from Motion. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

struct  gtsam::SfmTrack2d
 Track containing 2D measurements associated with a single 3D point. More...
 
struct  gtsam::SfmTrack
 
struct  gtsam::traits< SfmTrack >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Typedefs

│ │ │ │ +typedef std::pair< size_t, Point2 > gtsam::SfmMeasurement
 A measurement with its camera index.
 
│ │ │ │ +typedef std::pair< size_t, size_t > gtsam::SiftIndex
 Sift index for SfmTrack.
 
│ │ │ │ +using gtsam::SfmTrack2dVector = std::vector< SfmTrack2d >
 
│ │ │ │

Detailed Description

│ │ │ │ -

Shonan Averaging algorithm.

│ │ │ │ -
Date
March 2019 - August 2020
│ │ │ │ -
Author
Frank Dellaert, David Rosen, and Jing Wu
│ │ │ │ +

A simple data structure for a track in Structure from Motion.

│ │ │ │ +
Date
January 2022
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,42 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -ShonanAveraging.cpp File Reference │ │ │ │ │ -Shonan Averaging algorithm. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ +SfmTrack.h File Reference │ │ │ │ │ +A simple data structure for a track in Structure from Motion. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +struct   _g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_2_d │ │ │ │ │ +  Track containing 2D measurements associated with a single 3D point. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_S_f_m_T_r_a_c_k │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_f_m_T_r_a_c_k_ _> │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +typedef std::pair< size_t, _P_o_i_n_t_2 >  ggttssaamm::::SSffmmMMeeaassuurreemmeenntt │ │ │ │ │ +  A measurement with its camera index. │ │ │ │ │ +  │ │ │ │ │ +typedef std::pair< size_t, size_t >  ggttssaamm::::SSiiffttIInnddeexx │ │ │ │ │ +  Sift index for _S_f_m_T_r_a_c_k. │ │ │ │ │ +  │ │ │ │ │ + using  ggttssaamm::::SSffmmTTrraacckk22ddVVeeccttoorr = std::vector< │ │ │ │ │ + _S_f_m_T_r_a_c_k_2_d > │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Shonan Averaging algorithm. │ │ │ │ │ +A simple data structure for a track in Structure from Motion. │ │ │ │ │ Date │ │ │ │ │ - March 2019 - August 2020 │ │ │ │ │ + January 2022 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert, David Rosen, and Jing Wu │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_f_m │ │ │ │ │ - * _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_._c_p_p │ │ │ │ │ + * _S_f_m_T_r_a_c_k_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01217.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/SfmData.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/ShonanAveraging.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,84 +94,36 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
│ │ │ │ -
SfmData.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
ShonanAveraging.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Data structure for dealing with Structure from Motion data. │ │ │ │ +

Shonan Averaging algorithm. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

struct  gtsam::SfmData
 SfmData stores a bunch of SfmTracks. More...
 
struct  gtsam::traits< SfmData >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Typedefs

│ │ │ │ -typedef PinholeCamera< Cal3Bundler > gtsam::SfmCamera
 Define the structure for the camera poses.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

SfmData gtsam::readBal (const std::string &filename)
 This function parses a "Bundle Adjustment in the Large" (BAL) file and returns the data as a SfmData structure.
 
bool gtsam::writeBAL (const std::string &filename, const SfmData &data)
 This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure.
 
bool gtsam::writeBALfromValues (const std::string &filename, const SfmData &data, const Values &values)
 This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure and a value structure (measurements are the same as the SfM input data, while camera poses and values are read from Values)
 
Pose3 gtsam::openGL2gtsam (const Rot3 &R, double tx, double ty, double tz)
 This function converts an openGL camera pose to an GTSAM camera pose.
 
Pose3 gtsam::gtsam2openGL (const Rot3 &R, double tx, double ty, double tz)
 This function converts a GTSAM camera pose to an openGL camera pose.
 
Pose3 gtsam::gtsam2openGL (const Pose3 &PoseGTSAM)
 This function converts a GTSAM camera pose to an openGL camera pose.
 
Values gtsam::initialCamerasEstimate (const SfmData &db)
 This function creates initial values for cameras from db.
 
Values gtsam::initialCamerasAndPointsEstimate (const SfmData &db)
 This function creates initial values for cameras and points from db.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Data structure for dealing with Structure from Motion data.

│ │ │ │ -
Date
January 2022
│ │ │ │ -
Author
Frank dellaert
│ │ │ │ +

Shonan Averaging algorithm.

│ │ │ │ +
Date
March 2019 - August 2020
│ │ │ │ +
Author
Frank Dellaert, David Rosen, and Jing Wu
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,66 +1,22 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -SfmData.h File Reference │ │ │ │ │ -Data structure for dealing with Structure from Motion data. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_S_f_m_D_a_t_a │ │ │ │ │ -  _S_f_m_D_a_t_a stores a bunch of SfmTracks. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_f_m_D_a_t_a_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +ShonanAveraging.cpp File Reference │ │ │ │ │ +Shonan Averaging algorithm. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -typedef _P_i_n_h_o_l_e_C_a_m_e_r_a< _C_a_l_3_B_u_n_d_l_e_r >  ggttssaamm::::SSffmmCCaammeerraa │ │ │ │ │ -  Define the structure for the camera │ │ │ │ │ - poses. │ │ │ │ │ -  │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -_S_f_m_D_a_t_a  _g_t_s_a_m_:_:_r_e_a_d_B_a_l (const std::string &filename) │ │ │ │ │ -  This function parses a "Bundle Adjustment in the Large" (BAL) file and │ │ │ │ │ - returns the data as a _S_f_m_D_a_t_a structure. │ │ │ │ │ -  │ │ │ │ │ - bool  _g_t_s_a_m_:_:_w_r_i_t_e_B_A_L (const std::string &filename, const _S_f_m_D_a_t_a &data) │ │ │ │ │ -  This function writes a "Bundle Adjustment in the Large" (BAL) file │ │ │ │ │ - from a _S_f_m_D_a_t_a structure. │ │ │ │ │ -  │ │ │ │ │ - bool  _g_t_s_a_m_:_:_w_r_i_t_e_B_A_L_f_r_o_m_V_a_l_u_e_s (const std::string &filename, const _S_f_m_D_a_t_a │ │ │ │ │ - &data, const _V_a_l_u_e_s &values) │ │ │ │ │ - This function writes a "Bundle Adjustment in the Large" (BAL) file │ │ │ │ │ -  from a _S_f_m_D_a_t_a structure and a value structure (measurements are the │ │ │ │ │ - same as the SfM input data, while camera poses and values are read │ │ │ │ │ - from _V_a_l_u_e_s) │ │ │ │ │ -  │ │ │ │ │ - _P_o_s_e_3  _g_t_s_a_m_:_:_o_p_e_n_G_L_2_g_t_s_a_m (const _R_o_t_3 &R, double tx, double ty, double tz) │ │ │ │ │ -  This function converts an openGL camera pose to an GTSAM camera pose. │ │ │ │ │ -  │ │ │ │ │ - _P_o_s_e_3  _g_t_s_a_m_:_:_g_t_s_a_m_2_o_p_e_n_G_L (const _R_o_t_3 &R, double tx, double ty, double tz) │ │ │ │ │ -  This function converts a GTSAM camera pose to an openGL camera pose. │ │ │ │ │ -  │ │ │ │ │ - _P_o_s_e_3  _g_t_s_a_m_:_:_g_t_s_a_m_2_o_p_e_n_G_L (const _P_o_s_e_3 &PoseGTSAM) │ │ │ │ │ -  This function converts a GTSAM camera pose to an openGL camera pose. │ │ │ │ │ -  │ │ │ │ │ - _V_a_l_u_e_s  _g_t_s_a_m_:_:_i_n_i_t_i_a_l_C_a_m_e_r_a_s_E_s_t_i_m_a_t_e (const _S_f_m_D_a_t_a &db) │ │ │ │ │ -  This function creates initial values for cameras from db. │ │ │ │ │ -  │ │ │ │ │ - _V_a_l_u_e_s  _g_t_s_a_m_:_:_i_n_i_t_i_a_l_C_a_m_e_r_a_s_A_n_d_P_o_i_n_t_s_E_s_t_i_m_a_t_e (const _S_f_m_D_a_t_a &db) │ │ │ │ │ -  This function creates initial values for cameras and points from db. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Data structure for dealing with Structure from Motion data. │ │ │ │ │ +Shonan Averaging algorithm. │ │ │ │ │ Date │ │ │ │ │ - January 2022 │ │ │ │ │ + March 2019 - August 2020 │ │ │ │ │ Author │ │ │ │ │ - Frank dellaert │ │ │ │ │ + Frank Dellaert, David Rosen, and Jing Wu │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_f_m │ │ │ │ │ - * _S_f_m_D_a_t_a_._h │ │ │ │ │ + * _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01220.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/DsfTrackGenerator.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/ShonanFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,107 +96,53 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
│ │ │ │ -
DsfTrackGenerator.h File Reference
│ │ │ │ +Typedefs
│ │ │ │ +
ShonanFactor.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Identifies connected components in the keypoint matches graph. │ │ │ │ +

Main factor type in Shonan averaging, on SO(n) pairs. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

struct  gtsam::gtsfm::Keypoints
class  gtsam::ShonanFactor< d >
 ShonanFactor is a BetweenFactor that moves in SO(p), but will land on the SO(d) sub-manifold of SO(p) at the global minimum. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ Typedefs

│ │ │ │ -typedef Eigen::MatrixX2i gtsam::gtsfm::CorrespondenceIndices
 
│ │ │ │ -using gtsam::gtsfm::KeypointsVector = std::vector< Keypoints >
 
│ │ │ │ -using gtsam::gtsfm::MatchIndicesMap = std::map< IndexPair, CorrespondenceIndices >
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ -Functions

std::vector< SfmTrack2d > gtsam::gtsfm::tracksFromPairwiseMatches (const MatchIndicesMap &matches, const KeypointsVector &keypoints, bool verbose)
 Creates a list of tracks from 2d point correspondences.
 
│ │ │ │ +using gtsam::ShonanFactor2 = ShonanFactor< 2 >
 
│ │ │ │ +using gtsam::ShonanFactor3 = ShonanFactor< 3 >
 
│ │ │ │

Detailed Description

│ │ │ │ -

Identifies connected components in the keypoint matches graph.

│ │ │ │ -
Date
July 2022
│ │ │ │ -
Author
John Lambert
│ │ │ │ -

Function Documentation

│ │ │ │ - │ │ │ │ -

◆ tracksFromPairwiseMatches()

│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
std::vector< SfmTrack2d > gtsam::gtsfm::tracksFromPairwiseMatches (const MatchIndicesMap & matches,
const KeypointsVector & keypoints,
bool verbose 
)
│ │ │ │ -
│ │ │ │ - │ │ │ │ -

Creates a list of tracks from 2d point correspondences.

│ │ │ │ -

Creates a disjoint-set forest (DSF) and 2d tracks from pairwise matches. We create a singleton for union-find set elements from camera index of a detection and the index of that detection in that camera's keypoint list, i.e. (i,k).

│ │ │ │ -
Parameters
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
Mapfrom (i1,i2) image pair indices to (K,2) matrix, for K correspondence indices, from each image.
Length-Nlist of keypoints, for N images/cameras.
│ │ │ │ -
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ +

Main factor type in Shonan averaging, on SO(n) pairs.

│ │ │ │ +
Date
March 2019
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,57 +1,33 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -DsfTrackGenerator.h File Reference │ │ │ │ │ -Identifies connected components in the keypoint matches graph. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ +ShonanFactor.h File Reference │ │ │ │ │ +Main factor type in Shonan averaging, on SO(n) pairs. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_g_t_s_f_m_:_:_K_e_y_p_o_i_n_t_s │ │ │ │ │ +class   _g_t_s_a_m_:_:_S_h_o_n_a_n_F_a_c_t_o_r_<_ _d_ _> │ │ │ │ │ +  _S_h_o_n_a_n_F_a_c_t_o_r is a _B_e_t_w_e_e_n_F_a_c_t_o_r that moves in SO(p), but will land on │ │ │ │ │ + the SO(d) sub-manifold of SO(p) at the global minimum. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ TTyyppeeddeeffss │ │ │ │ │ -typedef Eigen::MatrixX2i  ggttssaamm::::ggttssffmm::::CCoorrrreessppoonnddeenncceeIInnddiicceess │ │ │ │ │ +using  ggttssaamm::::SShhoonnaannFFaaccttoorr22 = _S_h_o_n_a_n_F_a_c_t_o_r< 2 > │ │ │ │ │   │ │ │ │ │ - using  ggttssaamm::::ggttssffmm::::KKeeyyppooiinnttssVVeeccttoorr = std::vector< │ │ │ │ │ - _K_e_y_p_o_i_n_t_s > │ │ │ │ │ -  │ │ │ │ │ - using  ggttssaamm::::ggttssffmm::::MMaattcchhIInnddiicceessMMaapp = std::map< _I_n_d_e_x_P_a_i_r, │ │ │ │ │ - CorrespondenceIndices > │ │ │ │ │ -  │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -std::vector< _S_f_m_T_r_a_c_k_2_d >  _g_t_s_a_m_:_:_g_t_s_f_m_:_:_t_r_a_c_k_s_F_r_o_m_P_a_i_r_w_i_s_e_M_a_t_c_h_e_s (const │ │ │ │ │ - MatchIndicesMap &matches, const KeypointsVector │ │ │ │ │ - &keypoints, bool verbose) │ │ │ │ │ -  Creates a list of tracks from 2d point │ │ │ │ │ - correspondences. │ │ │ │ │ +using  ggttssaamm::::SShhoonnaannFFaaccttoorr33 = _S_h_o_n_a_n_F_a_c_t_o_r< 3 > │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Identifies connected components in the keypoint matches graph. │ │ │ │ │ +Main factor type in Shonan averaging, on SO(n) pairs. │ │ │ │ │ Date │ │ │ │ │ - July 2022 │ │ │ │ │ + March 2019 │ │ │ │ │ Author │ │ │ │ │ - John Lambert │ │ │ │ │ -********** FFuunnccttiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ -********** _?◆_? ttrraacckkssFFrroommPPaaiirrwwiisseeMMaattcchheess(()) ********** │ │ │ │ │ -std::vector< SfmTrack2d > gtsam::gtsfm:: ( const MatchIndicesMap &  mmaattcchheess, │ │ │ │ │ -tracksFromPairwiseMatches │ │ │ │ │ - const KeypointsVector &  kkeeyyppooiinnttss, │ │ │ │ │ - bool  vveerrbboossee  │ │ │ │ │ - ) │ │ │ │ │ -Creates a list of tracks from 2d point correspondences. │ │ │ │ │ -Creates a disjoint-set forest (DSF) and 2d tracks from pairwise matches. We │ │ │ │ │ -create a singleton for union-find set elements from camera index of a detection │ │ │ │ │ -and the index of that detection in that camera's keypoint list, i.e. (i,k). │ │ │ │ │ - Parameters │ │ │ │ │ - Map from (i1,i2) image pair indices to (K,2) matrix, for K │ │ │ │ │ - correspondence indices, from each image. │ │ │ │ │ - Length-N list of keypoints, for N images/cameras. │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_f_m │ │ │ │ │ - * _D_s_f_T_r_a_c_k_G_e_n_e_r_a_t_o_r_._h │ │ │ │ │ + * _S_h_o_n_a_n_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01220.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,3 @@ │ │ │ │ │ var a01220 = [ │ │ │ │ │ - ["gtsam::gtsfm::Keypoints", "a04692.html", "a04692"], │ │ │ │ │ - ["tracksFromPairwiseMatches", "a01220.html#ac50218675ff25e1e9202d335ecfa6b3a", null] │ │ │ │ │ + ["gtsam::ShonanFactor< d >", "a04744.html", "a04744"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01220_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/DsfTrackGenerator.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/ShonanFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,87 +98,99 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
DsfTrackGenerator.h
│ │ │ │ +
ShonanFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │ -
20#include <gtsam/base/DSFMap.h>
│ │ │ │ -
21#include <gtsam/sfm/SfmTrack.h>
│ │ │ │ -
22
│ │ │ │ -
23#include <boost/optional.hpp>
│ │ │ │ -
24
│ │ │ │ -
25#include <Eigen/Core>
│ │ │ │ -
26#include <map>
│ │ │ │ -
27#include <vector>
│ │ │ │ -
28
│ │ │ │ -
29namespace gtsam {
│ │ │ │ -
30
│ │ │ │ -
31namespace gtsfm {
│ │ │ │ -
32
│ │ │ │ -
33typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array
│ │ │ │ -
34
│ │ │ │ -
35// Output of detections in an image.
│ │ │ │ -
36// Coordinate system convention:
│ │ │ │ -
37// 1. The x coordinate denotes the horizontal direction (+ve direction towards
│ │ │ │ -
38// the right).
│ │ │ │ -
39// 2. The y coordinate denotes the vertical direction (+ve direction downwards).
│ │ │ │ -
40// 3. Origin is at the top left corner of the image.
│ │ │ │ -
│ │ │ │ -
41struct Keypoints {
│ │ │ │ -
42 // The (x, y) coordinates of the features, of shape Nx2.
│ │ │ │ -
43 Eigen::MatrixX2d coordinates;
│ │ │ │ -
44
│ │ │ │ -
45 // Optional scale of the detections, of shape N.
│ │ │ │ -
46 // Note: gtsam::Vector is typedef'd for Eigen::VectorXd.
│ │ │ │ -
47 boost::optional<gtsam::Vector> scales;
│ │ │ │ -
48
│ │ │ │ -
50 boost::optional<gtsam::Vector> responses;
│ │ │ │ -
51
│ │ │ │ -
52 Keypoints(const Eigen::MatrixX2d& coordinates)
│ │ │ │ -
53 : coordinates(coordinates){}; // boost::none
│ │ │ │ -
54};
│ │ │ │ -
│ │ │ │ -
55
│ │ │ │ -
56using KeypointsVector = std::vector<Keypoints>;
│ │ │ │ -
57// Mapping from each image pair to (N,2) array representing indices of matching
│ │ │ │ -
58// keypoints.
│ │ │ │ -
59using MatchIndicesMap = std::map<IndexPair, CorrespondenceIndices>;
│ │ │ │ -
60
│ │ │ │ -
73std::vector<SfmTrack2d> tracksFromPairwiseMatches(
│ │ │ │ -
74 const MatchIndicesMap& matches, const KeypointsVector& keypoints,
│ │ │ │ -
75 bool verbose = false);
│ │ │ │ -
76
│ │ │ │ -
77} // namespace gtsfm
│ │ │ │ -
78
│ │ │ │ -
79} // namespace gtsam
│ │ │ │ -
DSFMap.h
Allow for arbitrary type in DSF.
│ │ │ │ -
SfmTrack.h
A simple data structure for a track in Structure from Motion.
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/geometry/Rot2.h>
│ │ │ │ +
22#include <gtsam/geometry/Rot3.h>
│ │ │ │ +
23#include <gtsam/geometry/SOn.h>
│ │ │ │ +
24#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
25
│ │ │ │ +
26#include <type_traits>
│ │ │ │ +
27
│ │ │ │ +
28namespace gtsam {
│ │ │ │ +
29
│ │ │ │ +
35template <size_t d>
│ │ │ │ +
│ │ │ │ +
36class GTSAM_EXPORT ShonanFactor : public NoiseModelFactorN<SOn, SOn> {
│ │ │ │ +
37 Matrix M_;
│ │ │ │ +
38 size_t p_, pp_;
│ │ │ │ +
39 boost::shared_ptr<Matrix> G_;
│ │ │ │ +
40
│ │ │ │ +
41 // Select Rot2 or Rot3 interface based template parameter d
│ │ │ │ +
42 using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;
│ │ │ │ +
43
│ │ │ │ +
44public:
│ │ │ │ +
47
│ │ │ │ +
51 ShonanFactor(Key j1, Key j2, const Rot &R12, size_t p,
│ │ │ │ +
52 const SharedNoiseModel &model = nullptr,
│ │ │ │ +
53 const boost::shared_ptr<Matrix> &G = nullptr);
│ │ │ │ +
54
│ │ │ │ +
58
│ │ │ │ +
60 void
│ │ │ │ +
61 print(const std::string &s,
│ │ │ │ +
62 const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override;
│ │ │ │ +
63
│ │ │ │ +
65 bool equals(const NonlinearFactor &expected,
│ │ │ │ +
66 double tol = 1e-9) const override;
│ │ │ │ +
67
│ │ │ │ +
71
│ │ │ │ +
74 Vector
│ │ │ │ +
75 evaluateError(const SOn &Q1, const SOn &Q2,
│ │ │ │ +
76 boost::optional<Matrix &> H1 = boost::none,
│ │ │ │ +
77 boost::optional<Matrix &> H2 = boost::none) const override;
│ │ │ │ +
79
│ │ │ │ +
80private:
│ │ │ │ +
82 void fillJacobians(const Matrix &M1, const Matrix &M2,
│ │ │ │ +
83 boost::optional<Matrix &> H1,
│ │ │ │ +
84 boost::optional<Matrix &> H2) const;
│ │ │ │ +
85};
│ │ │ │ +
│ │ │ │ +
86
│ │ │ │ +
87// Explicit instantiation for d=2 and d=3 in .cpp file:
│ │ │ │ +
88using ShonanFactor2 = ShonanFactor<2>;
│ │ │ │ +
89using ShonanFactor3 = ShonanFactor<3>;
│ │ │ │ +
90
│ │ │ │ +
91} // namespace gtsam
│ │ │ │ +
Rot3.h
3D rotation represented as a rotation matrix or quaternion
│ │ │ │ +
SOn.h
N*N matrix representation of SO(N).
│ │ │ │ +
Rot2.h
2D rotation
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::gtsfm::Keypoints
Definition DsfTrackGenerator.h:41
│ │ │ │ -
gtsam::gtsfm::Keypoints::responses
boost::optional< gtsam::Vector > responses
Optional confidences/responses for each detection, of shape N.
Definition DsfTrackGenerator.h:50
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::SO
Manifold of special orthogonal rotation matrices SO<N>.
Definition SOn.h:52
│ │ │ │ +
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ +
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ +
gtsam::ShonanFactor
ShonanFactor is a BetweenFactor that moves in SO(p), but will land on the SO(d) sub-manifold of SO(p)...
Definition ShonanFactor.h:36
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,88 +1,119 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DsfTrackGenerator.h │ │ │ │ │ +ShonanFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 19#pragma once │ │ │ │ │ -20#include <_g_t_s_a_m_/_b_a_s_e_/_D_S_F_M_a_p_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_s_f_m_/_S_f_m_T_r_a_c_k_._h> │ │ │ │ │ -22 │ │ │ │ │ -23#include │ │ │ │ │ -24 │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ -28 │ │ │ │ │ -29namespace _g_t_s_a_m { │ │ │ │ │ -30 │ │ │ │ │ -31namespace gtsfm { │ │ │ │ │ -32 │ │ │ │ │ -33typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array │ │ │ │ │ -34 │ │ │ │ │ -35// Output of detections in an image. │ │ │ │ │ -36// Coordinate system convention: │ │ │ │ │ -37// 1. The x coordinate denotes the horizontal direction (+ve direction │ │ │ │ │ -towards │ │ │ │ │ -38// the right). │ │ │ │ │ -39// 2. The y coordinate denotes the vertical direction (+ve direction │ │ │ │ │ -downwards). │ │ │ │ │ -40// 3. Origin is at the top left corner of the image. │ │ │ │ │ -_4_1struct _K_e_y_p_o_i_n_t_s { │ │ │ │ │ -42 // The (x, y) coordinates of the features, of shape Nx2. │ │ │ │ │ -43 Eigen::MatrixX2d coordinates; │ │ │ │ │ -44 │ │ │ │ │ -45 // Optional scale of the detections, of shape N. │ │ │ │ │ -46 // Note: gtsam::Vector is typedef'd for Eigen::VectorXd. │ │ │ │ │ -47 boost::optional scales; │ │ │ │ │ -48 │ │ │ │ │ -_5_0 boost::optional _r_e_s_p_o_n_s_e_s; │ │ │ │ │ -51 │ │ │ │ │ -52 _K_e_y_p_o_i_n_t_s(const Eigen::MatrixX2d& coordinates) │ │ │ │ │ -53 : coordinates(coordinates){}; // boost::none │ │ │ │ │ -54}; │ │ │ │ │ -55 │ │ │ │ │ -56using KeypointsVector = std::vector; │ │ │ │ │ -57// Mapping from each image pair to (N,2) array representing indices of │ │ │ │ │ -matching │ │ │ │ │ -58// keypoints. │ │ │ │ │ -59using MatchIndicesMap = std::map; │ │ │ │ │ -60 │ │ │ │ │ -73std::vector tracksFromPairwiseMatches( │ │ │ │ │ -74 const MatchIndicesMap& matches, const KeypointsVector& keypoints, │ │ │ │ │ -75 bool verbose = false); │ │ │ │ │ -76 │ │ │ │ │ -77} // namespace gtsfm │ │ │ │ │ -78 │ │ │ │ │ -79} // namespace gtsam │ │ │ │ │ -_D_S_F_M_a_p_._h │ │ │ │ │ -Allow for arbitrary type in DSF. │ │ │ │ │ -_S_f_m_T_r_a_c_k_._h │ │ │ │ │ -A simple data structure for a track in Structure from Motion. │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_2_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_3_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_S_O_n_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +25 │ │ │ │ │ +26#include │ │ │ │ │ +27 │ │ │ │ │ +28namespace _g_t_s_a_m { │ │ │ │ │ +29 │ │ │ │ │ +35template │ │ │ │ │ +_3_6class GTSAM_EXPORT _S_h_o_n_a_n_F_a_c_t_o_r : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +37 Matrix M_; │ │ │ │ │ +38 size_t p_, pp_; │ │ │ │ │ +39 boost::shared_ptr G_; │ │ │ │ │ +40 │ │ │ │ │ +41 // Select Rot2 or Rot3 interface based template parameter d │ │ │ │ │ +42 using Rot = typename std::conditional::type; │ │ │ │ │ +43 │ │ │ │ │ +44public: │ │ │ │ │ +47 │ │ │ │ │ +51 _S_h_o_n_a_n_F_a_c_t_o_r(_K_e_y j1, _K_e_y j2, const Rot &R12, size_t p, │ │ │ │ │ +52 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model = nullptr, │ │ │ │ │ +53 const boost::shared_ptr &G = nullptr); │ │ │ │ │ +54 │ │ │ │ │ +58 │ │ │ │ │ +60 void │ │ │ │ │ +61 _p_r_i_n_t(const std::string &s, │ │ │ │ │ +62 const _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter = DefaultKeyFormatter) const override; │ │ │ │ │ +63 │ │ │ │ │ +65 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r &expected, │ │ │ │ │ +66 double tol = 1e-9) const override; │ │ │ │ │ +67 │ │ │ │ │ +71 │ │ │ │ │ +74 Vector │ │ │ │ │ +75 evaluateError(const _S_O_n &Q1, const _S_O_n &Q2, │ │ │ │ │ +76 boost::optional H1 = boost::none, │ │ │ │ │ +77 boost::optional H2 = boost::none) const override; │ │ │ │ │ +79 │ │ │ │ │ +80private: │ │ │ │ │ +82 void fillJacobians(const Matrix &M1, const Matrix &M2, │ │ │ │ │ +83 boost::optional H1, │ │ │ │ │ +84 boost::optional H2) const; │ │ │ │ │ +85}; │ │ │ │ │ +86 │ │ │ │ │ +87// Explicit instantiation for d=2 and d=3 in .cpp file: │ │ │ │ │ +88using ShonanFactor2 = _S_h_o_n_a_n_F_a_c_t_o_r_<_2_>; │ │ │ │ │ +89using ShonanFactor3 = _S_h_o_n_a_n_F_a_c_t_o_r_<_3_>; │ │ │ │ │ +90 │ │ │ │ │ +91} // namespace gtsam │ │ │ │ │ +_R_o_t_3_._h │ │ │ │ │ +3D rotation represented as a rotation matrix or quaternion │ │ │ │ │ +_S_O_n_._h │ │ │ │ │ +N*N matrix representation of SO(N). │ │ │ │ │ +_R_o_t_2_._h │ │ │ │ │ +2D rotation │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ +Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_g_t_s_f_m_:_:_K_e_y_p_o_i_n_t_s │ │ │ │ │ -DDeeffiinniittiioonn DsfTrackGenerator.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_g_t_s_f_m_:_:_K_e_y_p_o_i_n_t_s_:_:_r_e_s_p_o_n_s_e_s │ │ │ │ │ -boost::optional< gtsam::Vector > responses │ │ │ │ │ -Optional confidences/responses for each detection, of shape N. │ │ │ │ │ -DDeeffiinniittiioonn DsfTrackGenerator.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ +Aliases. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O │ │ │ │ │ +Manifold of special orthogonal rotation matrices SO. │ │ │ │ │ +DDeeffiinniittiioonn SOn.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ +Nonlinear factor base class. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ +A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ +variables. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_F_a_c_t_o_r │ │ │ │ │ +ShonanFactor is a BetweenFactor that moves in SO(p), but will land on the SO(d) │ │ │ │ │ +sub-manifold of SO(p)... │ │ │ │ │ +DDeeffiinniittiioonn ShonanFactor.h:36 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_f_m │ │ │ │ │ - * _D_s_f_T_r_a_c_k_G_e_n_e_r_a_t_o_r_._h │ │ │ │ │ + * _S_h_o_n_a_n_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01223.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/ShonanAveraging.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/SfmData.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,62 +94,67 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs
│ │ │ │ -
ShonanAveraging.h File Reference
│ │ │ │ +Functions
│ │ │ │ +
SfmData.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Shonan Averaging algorithm. │ │ │ │ +

Data structure for dealing with Structure from Motion data. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

struct  gtsam::ShonanAveragingParameters< d >
 Parameters governing optimization etc. More...
 
class  gtsam::ShonanAveraging< d >
 Class that implements Shonan Averaging from our ECCV'20 paper. More...
 
class  gtsam::ShonanAveraging2
 
class  gtsam::ShonanAveraging3
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ -Typedefs

│ │ │ │ -using gtsam::ShonanAveragingParameters2 = ShonanAveragingParameters< 2 >
 
│ │ │ │ -using gtsam::ShonanAveragingParameters3 = ShonanAveragingParameters< 3 >
 

│ │ │ │ +Functions

│ │ │ │ +Rot3 gtsam::openGLFixedRotation ()
 
Pose3 gtsam::openGL2gtsam (const Rot3 &R, double tx, double ty, double tz)
 This function converts an openGL camera pose to an GTSAM camera pose.
 
Pose3 gtsam::gtsam2openGL (const Rot3 &R, double tx, double ty, double tz)
 This function converts a GTSAM camera pose to an openGL camera pose.
 
Pose3 gtsam::gtsam2openGL (const Pose3 &PoseGTSAM)
 This function converts a GTSAM camera pose to an openGL camera pose.
 
bool gtsam::writeBAL (const std::string &filename, const SfmData &data)
 This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure.
 
SfmData gtsam::readBal (const std::string &filename)
 This function parses a "Bundle Adjustment in the Large" (BAL) file and returns the data as a SfmData structure.
 
bool gtsam::writeBALfromValues (const std::string &filename, const SfmData &data, const Values &values)
 This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure and a value structure (measurements are the same as the SfM input data, while camera poses and values are read from Values)
 
Values gtsam::initialCamerasEstimate (const SfmData &db)
 This function creates initial values for cameras from db.
 
Values gtsam::initialCamerasAndPointsEstimate (const SfmData &db)
 This function creates initial values for cameras and points from db.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Shonan Averaging algorithm.

│ │ │ │ -
Date
March 2019 - August 2020
│ │ │ │ -
Author
Frank Dellaert, David Rosen, and Jing Wu
│ │ │ │ +

Data structure for dealing with Structure from Motion data.

│ │ │ │ +
Date
January 2022
│ │ │ │ +
Author
Frank dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,39 +1,55 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ -ShonanAveraging.h File Reference │ │ │ │ │ -Shonan Averaging algorithm. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s_<_ _d_ _> │ │ │ │ │ -  Parameters governing optimization etc. _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +SfmData.cpp File Reference │ │ │ │ │ +Data structure for dealing with Structure from Motion data. _M_o_r_e_._._. │ │ │ │ │ +NNaammeessppaacceess │ │ │ │ │ +namespace   _g_t_s_a_m │ │ │ │ │ +  Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ - class   _g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_<_ _d_ _> │ │ │ │ │ -  Class that implements Shonan Averaging from our ECCV'20 paper. _M_o_r_e_._._. │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ + _R_o_t_3  ggttssaamm::::ooppeennGGLLFFiixxeeddRRoottaattiioonn () │ │ │ │ │   │ │ │ │ │ - class   _g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_2 │ │ │ │ │ + _P_o_s_e_3  _g_t_s_a_m_:_:_o_p_e_n_G_L_2_g_t_s_a_m (const _R_o_t_3 &R, double tx, double ty, double tz) │ │ │ │ │ +  This function converts an openGL camera pose to an GTSAM camera pose. │ │ │ │ │   │ │ │ │ │ - class   _g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_3 │ │ │ │ │ + _P_o_s_e_3  _g_t_s_a_m_:_:_g_t_s_a_m_2_o_p_e_n_G_L (const _R_o_t_3 &R, double tx, double ty, double tz) │ │ │ │ │ +  This function converts a GTSAM camera pose to an openGL camera pose. │ │ │ │ │   │ │ │ │ │ -NNaammeessppaacceess │ │ │ │ │ -namespace   _g_t_s_a_m │ │ │ │ │ -  Global functions in a separate testing namespace. │ │ │ │ │ + _P_o_s_e_3  _g_t_s_a_m_:_:_g_t_s_a_m_2_o_p_e_n_G_L (const _P_o_s_e_3 &PoseGTSAM) │ │ │ │ │ +  This function converts a GTSAM camera pose to an openGL camera pose. │ │ │ │ │ +  │ │ │ │ │ + bool  _g_t_s_a_m_:_:_w_r_i_t_e_B_A_L (const std::string &filename, const _S_f_m_D_a_t_a &data) │ │ │ │ │ +  This function writes a "Bundle Adjustment in the Large" (BAL) file │ │ │ │ │ + from a _S_f_m_D_a_t_a structure. │ │ │ │ │ +  │ │ │ │ │ +_S_f_m_D_a_t_a  _g_t_s_a_m_:_:_r_e_a_d_B_a_l (const std::string &filename) │ │ │ │ │ +  This function parses a "Bundle Adjustment in the Large" (BAL) file and │ │ │ │ │ + returns the data as a _S_f_m_D_a_t_a structure. │ │ │ │ │ +  │ │ │ │ │ + bool  _g_t_s_a_m_:_:_w_r_i_t_e_B_A_L_f_r_o_m_V_a_l_u_e_s (const std::string &filename, const _S_f_m_D_a_t_a │ │ │ │ │ + &data, const _V_a_l_u_e_s &values) │ │ │ │ │ + This function writes a "Bundle Adjustment in the Large" (BAL) file │ │ │ │ │ +  from a _S_f_m_D_a_t_a structure and a value structure (measurements are the │ │ │ │ │ + same as the SfM input data, while camera poses and values are read │ │ │ │ │ + from _V_a_l_u_e_s) │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -using  ggttssaamm::::SShhoonnaannAAvveerraaggiinnggPPaarraammeetteerrss22 = _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s< 2 > │ │ │ │ │ + _V_a_l_u_e_s  _g_t_s_a_m_:_:_i_n_i_t_i_a_l_C_a_m_e_r_a_s_E_s_t_i_m_a_t_e (const _S_f_m_D_a_t_a &db) │ │ │ │ │ +  This function creates initial values for cameras from db. │ │ │ │ │   │ │ │ │ │ -using  ggttssaamm::::SShhoonnaannAAvveerraaggiinnggPPaarraammeetteerrss33 = _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s< 3 > │ │ │ │ │ + _V_a_l_u_e_s  _g_t_s_a_m_:_:_i_n_i_t_i_a_l_C_a_m_e_r_a_s_A_n_d_P_o_i_n_t_s_E_s_t_i_m_a_t_e (const _S_f_m_D_a_t_a &db) │ │ │ │ │ +  This function creates initial values for cameras and points from db. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Shonan Averaging algorithm. │ │ │ │ │ +Data structure for dealing with Structure from Motion data. │ │ │ │ │ Date │ │ │ │ │ - March 2019 - August 2020 │ │ │ │ │ + January 2022 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert, David Rosen, and Jing Wu │ │ │ │ │ + Frank dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_f_m │ │ │ │ │ - * _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_._h │ │ │ │ │ + * _S_f_m_D_a_t_a_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01223.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,6 +1,10 @@ │ │ │ │ │ var a01223 = [ │ │ │ │ │ - ["gtsam::ShonanAveragingParameters< d >", "a04728.html", "a04728"], │ │ │ │ │ - ["gtsam::ShonanAveraging< d >", "a04732.html", "a04732"], │ │ │ │ │ - ["gtsam::ShonanAveraging2", "a04736.html", null], │ │ │ │ │ - ["gtsam::ShonanAveraging3", "a04740.html", null] │ │ │ │ │ + ["gtsam2openGL", "a01223.html#ad632ed56581bf174eef379bbe272a06a", null], │ │ │ │ │ + ["gtsam2openGL", "a01223.html#aaff07ef70e6161c4d1d2bd7546ff91e6", null], │ │ │ │ │ + ["initialCamerasAndPointsEstimate", "a01223.html#aba34ed4a103d9057536ee5cac27cfdef", null], │ │ │ │ │ + ["initialCamerasEstimate", "a01223.html#ad8b55993353cbab0fde06b872783fe92", null], │ │ │ │ │ + ["openGL2gtsam", "a01223.html#ade6eca65dc0168a65f641961ca97924c", null], │ │ │ │ │ + ["readBal", "a01223.html#a710ec0322f4a2c0883841b58516b7a6a", null], │ │ │ │ │ + ["writeBAL", "a01223.html#a43c2c2ead4fddd79d9c3f9a5bfa764ae", null], │ │ │ │ │ + ["writeBALfromValues", "a01223.html#a4a2433812cf34ee359e3ec99f4f5bcc4", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01226.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/TranslationRecovery.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/SfmData.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,43 +94,84 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ Functions
│ │ │ │ -
TranslationRecovery.cpp File Reference
│ │ │ │ +
SfmData.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Source code for recovering translations when rotations are given. │ │ │ │ +

Data structure for dealing with Structure from Motion data. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

struct  gtsam::SfmData
 SfmData stores a bunch of SfmTracks. More...
 
struct  gtsam::traits< SfmData >
 traits More...
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Typedefs

│ │ │ │ +typedef PinholeCamera< Cal3Bundler > gtsam::SfmCamera
 Define the structure for the camera poses.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

│ │ │ │ -DSFMap< Key > getSameTranslationDSFMap (const std::vector< BinaryMeasurement< Unit3 > > &relativeTranslations)
 
│ │ │ │ -template<typename T >
std::vector< BinaryMeasurement< T > > removeSameTranslationNodes (const std::vector< BinaryMeasurement< T > > &edges, const DSFMap< Key > &sameTranslationDSFMap)
 
│ │ │ │ -Values addSameTranslationNodes (const Values &result, const DSFMap< Key > &sameTranslationDSFMap)
 
SfmData gtsam::readBal (const std::string &filename)
 This function parses a "Bundle Adjustment in the Large" (BAL) file and returns the data as a SfmData structure.
 
bool gtsam::writeBAL (const std::string &filename, const SfmData &data)
 This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure.
 
bool gtsam::writeBALfromValues (const std::string &filename, const SfmData &data, const Values &values)
 This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure and a value structure (measurements are the same as the SfM input data, while camera poses and values are read from Values)
 
Pose3 gtsam::openGL2gtsam (const Rot3 &R, double tx, double ty, double tz)
 This function converts an openGL camera pose to an GTSAM camera pose.
 
Pose3 gtsam::gtsam2openGL (const Rot3 &R, double tx, double ty, double tz)
 This function converts a GTSAM camera pose to an openGL camera pose.
 
Pose3 gtsam::gtsam2openGL (const Pose3 &PoseGTSAM)
 This function converts a GTSAM camera pose to an openGL camera pose.
 
Values gtsam::initialCamerasEstimate (const SfmData &db)
 This function creates initial values for cameras from db.
 
Values gtsam::initialCamerasAndPointsEstimate (const SfmData &db)
 This function creates initial values for cameras and points from db.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Source code for recovering translations when rotations are given.

│ │ │ │ -
Author
Frank Dellaert, Akshay Krishnan
│ │ │ │ -
Date
March 2020
│ │ │ │ +

Data structure for dealing with Structure from Motion data.

│ │ │ │ +
Date
January 2022
│ │ │ │ +
Author
Frank dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,33 +1,66 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_F_u_n_c_t_i_o_n_s │ │ │ │ │ -TranslationRecovery.cpp File Reference │ │ │ │ │ -Source code for recovering translations when rotations are given. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +SfmData.h File Reference │ │ │ │ │ +Data structure for dealing with Structure from Motion data. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +struct   _g_t_s_a_m_:_:_S_f_m_D_a_t_a │ │ │ │ │ +  _S_f_m_D_a_t_a stores a bunch of SfmTracks. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_f_m_D_a_t_a_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +NNaammeessppaacceess │ │ │ │ │ +namespace   _g_t_s_a_m │ │ │ │ │ +  Global functions in a separate testing namespace. │ │ │ │ │ +  │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +typedef _P_i_n_h_o_l_e_C_a_m_e_r_a< _C_a_l_3_B_u_n_d_l_e_r >  ggttssaamm::::SSffmmCCaammeerraa │ │ │ │ │ +  Define the structure for the camera │ │ │ │ │ + poses. │ │ │ │ │ +  │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ - _D_S_F_M_a_p< _K_e_y >  ggeettSSaammeeTTrraannssllaattiioonnDDSSFFMMaapp (const std:: │ │ │ │ │ - vector< _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t< _U_n_i_t_3 > > │ │ │ │ │ - &relativeTranslations) │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -std::vector< _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t< T > >  rreemmoovveeSSaammeeTTrraannssllaattiioonnNNooddeess (const std:: │ │ │ │ │ - vector< _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t< T > > &edges, │ │ │ │ │ - const _D_S_F_M_a_p< _K_e_y > │ │ │ │ │ - &sameTranslationDSFMap) │ │ │ │ │ -  │ │ │ │ │ - _V_a_l_u_e_s  aaddddSSaammeeTTrraannssllaattiioonnNNooddeess (const _V_a_l_u_e_s │ │ │ │ │ - &result, const _D_S_F_M_a_p< _K_e_y > │ │ │ │ │ - &sameTranslationDSFMap) │ │ │ │ │ +_S_f_m_D_a_t_a  _g_t_s_a_m_:_:_r_e_a_d_B_a_l (const std::string &filename) │ │ │ │ │ +  This function parses a "Bundle Adjustment in the Large" (BAL) file and │ │ │ │ │ + returns the data as a _S_f_m_D_a_t_a structure. │ │ │ │ │ +  │ │ │ │ │ + bool  _g_t_s_a_m_:_:_w_r_i_t_e_B_A_L (const std::string &filename, const _S_f_m_D_a_t_a &data) │ │ │ │ │ +  This function writes a "Bundle Adjustment in the Large" (BAL) file │ │ │ │ │ + from a _S_f_m_D_a_t_a structure. │ │ │ │ │ +  │ │ │ │ │ + bool  _g_t_s_a_m_:_:_w_r_i_t_e_B_A_L_f_r_o_m_V_a_l_u_e_s (const std::string &filename, const _S_f_m_D_a_t_a │ │ │ │ │ + &data, const _V_a_l_u_e_s &values) │ │ │ │ │ + This function writes a "Bundle Adjustment in the Large" (BAL) file │ │ │ │ │ +  from a _S_f_m_D_a_t_a structure and a value structure (measurements are the │ │ │ │ │ + same as the SfM input data, while camera poses and values are read │ │ │ │ │ + from _V_a_l_u_e_s) │ │ │ │ │ +  │ │ │ │ │ + _P_o_s_e_3  _g_t_s_a_m_:_:_o_p_e_n_G_L_2_g_t_s_a_m (const _R_o_t_3 &R, double tx, double ty, double tz) │ │ │ │ │ +  This function converts an openGL camera pose to an GTSAM camera pose. │ │ │ │ │ +  │ │ │ │ │ + _P_o_s_e_3  _g_t_s_a_m_:_:_g_t_s_a_m_2_o_p_e_n_G_L (const _R_o_t_3 &R, double tx, double ty, double tz) │ │ │ │ │ +  This function converts a GTSAM camera pose to an openGL camera pose. │ │ │ │ │ +  │ │ │ │ │ + _P_o_s_e_3  _g_t_s_a_m_:_:_g_t_s_a_m_2_o_p_e_n_G_L (const _P_o_s_e_3 &PoseGTSAM) │ │ │ │ │ +  This function converts a GTSAM camera pose to an openGL camera pose. │ │ │ │ │ +  │ │ │ │ │ + _V_a_l_u_e_s  _g_t_s_a_m_:_:_i_n_i_t_i_a_l_C_a_m_e_r_a_s_E_s_t_i_m_a_t_e (const _S_f_m_D_a_t_a &db) │ │ │ │ │ +  This function creates initial values for cameras from db. │ │ │ │ │ +  │ │ │ │ │ + _V_a_l_u_e_s  _g_t_s_a_m_:_:_i_n_i_t_i_a_l_C_a_m_e_r_a_s_A_n_d_P_o_i_n_t_s_E_s_t_i_m_a_t_e (const _S_f_m_D_a_t_a &db) │ │ │ │ │ +  This function creates initial values for cameras and points from db. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Source code for recovering translations when rotations are given. │ │ │ │ │ - Author │ │ │ │ │ - Frank Dellaert, Akshay Krishnan │ │ │ │ │ +Data structure for dealing with Structure from Motion data. │ │ │ │ │ Date │ │ │ │ │ - March 2020 │ │ │ │ │ + January 2022 │ │ │ │ │ + Author │ │ │ │ │ + Frank dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_f_m │ │ │ │ │ - * _T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y_._c_p_p │ │ │ │ │ + * _S_f_m_D_a_t_a_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01229.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/ShonanGaugeFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/ShonanAveraging.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,44 +95,61 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
ShonanGaugeFactor.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Typedefs
│ │ │ │ +
ShonanAveraging.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Factor used in Shonan Averaging to clamp down gauge freedom. │ │ │ │ +

Shonan Averaging algorithm. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::ShonanGaugeFactor
 The ShonanGaugeFactor creates a constraint on a single SO(n) to avoid moving in the stabilizer. More...
struct  gtsam::ShonanAveragingParameters< d >
 Parameters governing optimization etc. More...
 
class  gtsam::ShonanAveraging< d >
 Class that implements Shonan Averaging from our ECCV'20 paper. More...
 
class  gtsam::ShonanAveraging2
 
class  gtsam::ShonanAveraging3
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Typedefs

│ │ │ │ +using gtsam::ShonanAveragingParameters2 = ShonanAveragingParameters< 2 >
 
│ │ │ │ +using gtsam::ShonanAveragingParameters3 = ShonanAveragingParameters< 3 >
 
│ │ │ │

Detailed Description

│ │ │ │ -

Factor used in Shonan Averaging to clamp down gauge freedom.

│ │ │ │ -
Date
March 2019
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +

Shonan Averaging algorithm.

│ │ │ │ +
Date
March 2019 - August 2020
│ │ │ │ +
Author
Frank Dellaert, David Rosen, and Jing Wu
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,39 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -ShonanGaugeFactor.h File Reference │ │ │ │ │ -Factor used in Shonan Averaging to clamp down gauge freedom. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ +ShonanAveraging.h File Reference │ │ │ │ │ +Shonan Averaging algorithm. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r │ │ │ │ │ -  The _S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r creates a constraint on a single SO(n) to avoid │ │ │ │ │ - moving in the stabilizer. _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s_<_ _d_ _> │ │ │ │ │ +  Parameters governing optimization etc. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_<_ _d_ _> │ │ │ │ │ +  Class that implements Shonan Averaging from our ECCV'20 paper. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_2 │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_3 │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +using  ggttssaamm::::SShhoonnaannAAvveerraaggiinnggPPaarraammeetteerrss22 = _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s< 2 > │ │ │ │ │ +  │ │ │ │ │ +using  ggttssaamm::::SShhoonnaannAAvveerraaggiinnggPPaarraammeetteerrss33 = _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s< 3 > │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Factor used in Shonan Averaging to clamp down gauge freedom. │ │ │ │ │ +Shonan Averaging algorithm. │ │ │ │ │ Date │ │ │ │ │ - March 2019 │ │ │ │ │ + March 2019 - August 2020 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Frank Dellaert, David Rosen, and Jing Wu │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_f_m │ │ │ │ │ - * _S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r_._h │ │ │ │ │ + * _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01229.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,6 @@ │ │ │ │ │ var a01229 = [ │ │ │ │ │ - ["gtsam::ShonanGaugeFactor", "a04748.html", "a04748"] │ │ │ │ │ + ["gtsam::ShonanAveragingParameters< d >", "a04728.html", "a04728"], │ │ │ │ │ + ["gtsam::ShonanAveraging< d >", "a04732.html", "a04732"], │ │ │ │ │ + ["gtsam::ShonanAveraging2", "a04736.html", null], │ │ │ │ │ + ["gtsam::ShonanAveraging3", "a04740.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01229_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/ShonanGaugeFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/ShonanAveraging.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,15 +98,15 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ShonanGaugeFactor.h
│ │ │ │ +
ShonanAveraging.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │ @@ -114,95 +114,374 @@ │ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/geometry/SOn.h>
│ │ │ │ -
22#include <gtsam/linear/JacobianFactor.h>
│ │ │ │ -
23#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
24
│ │ │ │ -
25namespace gtsam {
│ │ │ │ -
│ │ │ │ -
45class GTSAM_EXPORT ShonanGaugeFactor : public NonlinearFactor {
│ │ │ │ -
46 // Row dimension, equal to the dimensionality of SO(p-d)
│ │ │ │ -
47 size_t rows_;
│ │ │ │ -
48
│ │ │ │ -
50 boost::shared_ptr<JacobianFactor> whitenedJacobian_;
│ │ │ │ -
51
│ │ │ │ -
52public:
│ │ │ │ -
│ │ │ │ -
58 ShonanGaugeFactor(Key key, size_t p, size_t d = 3,
│ │ │ │ -
59 boost::optional<double> gamma = boost::none)
│ │ │ │ -
60 : NonlinearFactor(KeyVector{key}) {
│ │ │ │ -
61 if (p < d) {
│ │ │ │ -
62 throw std::invalid_argument("ShonanGaugeFactor must have p>=d.");
│ │ │ │ -
63 }
│ │ │ │ -
64 // Calculate dimensions
│ │ │ │ -
65 size_t q = p - d;
│ │ │ │ -
66 size_t P = SOn::Dimension(p); // dimensionality of SO(p)
│ │ │ │ -
67 rows_ = SOn::Dimension(q); // dimensionality of SO(q), the gauge
│ │ │ │ -
68
│ │ │ │ -
69 // Create constant Jacobian as a rows_*P matrix: there are rows_ penalized
│ │ │ │ -
70 // dimensions, but it is a bit tricky to find them among the P columns.
│ │ │ │ -
71 // The key is to look at how skew-symmetric matrices are laid out in SOn.h:
│ │ │ │ -
72 // the first tangent dimension will always be included, but beyond that we
│ │ │ │ -
73 // have to be careful. We always need to skip the d top-rows of the skew-
│ │ │ │ -
74 // symmetric matrix as they below to K, part of the Stiefel manifold.
│ │ │ │ -
75 Matrix A(rows_, P);
│ │ │ │ -
76 A.setZero();
│ │ │ │ -
77 double invSigma = gamma ? std::sqrt(*gamma) : 1.0;
│ │ │ │ -
78 size_t i = 0, j = 0, n = p - 1 - d;
│ │ │ │ -
79 while (i < rows_) {
│ │ │ │ -
80 A.block(i, j, n, n) = invSigma * Matrix::Identity(n, n);
│ │ │ │ -
81 i += n;
│ │ │ │ -
82 j += n + d; // skip d columns
│ │ │ │ -
83 n -= 1;
│ │ │ │ -
84 }
│ │ │ │ -
85 // TODO(frank): assign the right one in the right columns
│ │ │ │ -
86 whitenedJacobian_ =
│ │ │ │ -
87 boost::make_shared<JacobianFactor>(key, A, Vector::Zero(rows_));
│ │ │ │ -
88 }
│ │ │ │ -
│ │ │ │ +
21#include <gtsam/base/Matrix.h>
│ │ │ │ +
22#include <gtsam/base/Vector.h>
│ │ │ │ +
23#include <gtsam/dllexport.h>
│ │ │ │ +
24#include <gtsam/geometry/Rot2.h>
│ │ │ │ +
25#include <gtsam/geometry/Rot3.h>
│ │ │ │ +
26#include <gtsam/linear/VectorValues.h>
│ │ │ │ +
27#include <gtsam/nonlinear/LevenbergMarquardtParams.h>
│ │ │ │ +
28#include <gtsam/sfm/BinaryMeasurement.h>
│ │ │ │ +
29#include <gtsam/linear/PowerMethod.h>
│ │ │ │ +
30#include <gtsam/linear/AcceleratedPowerMethod.h>
│ │ │ │ +
31#include <gtsam/slam/dataset.h>
│ │ │ │ +
32
│ │ │ │ +
33#include <Eigen/Sparse>
│ │ │ │ +
34#include <map>
│ │ │ │ +
35#include <string>
│ │ │ │ +
36#include <type_traits>
│ │ │ │ +
37#include <utility>
│ │ │ │ +
38#include <vector>
│ │ │ │ +
39
│ │ │ │ +
40namespace gtsam {
│ │ │ │ +
41class NonlinearFactorGraph;
│ │ │ │ +
42class LevenbergMarquardtOptimizer;
│ │ │ │ +
43
│ │ │ │ +
45template <size_t d>
│ │ │ │ +
│ │ │ │ +
46struct GTSAM_EXPORT ShonanAveragingParameters {
│ │ │ │ +
47 // Select Rot2 or Rot3 interface based template parameter d
│ │ │ │ +
48 using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;
│ │ │ │ +
49 using Anchor = std::pair<size_t, Rot>;
│ │ │ │ +
50
│ │ │ │ +
51 // Parameters themselves:
│ │ │ │ +
52 LevenbergMarquardtParams lm;
│ │ │ │ +
53 double optimalityThreshold;
│ │ │ │ +
54 Anchor anchor;
│ │ │ │ +
55 double alpha;
│ │ │ │ +
56 double beta;
│ │ │ │ +
57 double gamma;
│ │ │ │ +
59 bool useHuber;
│ │ │ │ +
61 bool certifyOptimality;
│ │ │ │ +
62
│ │ │ │ +
63 ShonanAveragingParameters(const LevenbergMarquardtParams &lm =
│ │ │ │ +
64 LevenbergMarquardtParams::CeresDefaults(),
│ │ │ │ +
65 const std::string &method = "JACOBI",
│ │ │ │ +
66 double optimalityThreshold = -1e-4,
│ │ │ │ +
67 double alpha = 0.0, double beta = 1.0,
│ │ │ │ +
68 double gamma = 0.0);
│ │ │ │ +
69
│ │ │ │ +
70 LevenbergMarquardtParams getLMParams() const { return lm; }
│ │ │ │ +
71
│ │ │ │ +
72 void setOptimalityThreshold(double value) { optimalityThreshold = value; }
│ │ │ │ +
73 double getOptimalityThreshold() const { return optimalityThreshold; }
│ │ │ │ +
74
│ │ │ │ +
75 void setAnchor(size_t index, const Rot &value) { anchor = {index, value}; }
│ │ │ │ +
76 std::pair<size_t, Rot> getAnchor() const { return anchor; }
│ │ │ │ +
77
│ │ │ │ +
78 void setAnchorWeight(double value) { alpha = value; }
│ │ │ │ +
79 double getAnchorWeight() const { return alpha; }
│ │ │ │ +
80
│ │ │ │ +
81 void setKarcherWeight(double value) { beta = value; }
│ │ │ │ +
82 double getKarcherWeight() const { return beta; }
│ │ │ │ +
83
│ │ │ │ +
84 void setGaugesWeight(double value) { gamma = value; }
│ │ │ │ +
85 double getGaugesWeight() const { return gamma; }
│ │ │ │ +
86
│ │ │ │ +
87 void setUseHuber(bool value) { useHuber = value; }
│ │ │ │ +
88 bool getUseHuber() const { return useHuber; }
│ │ │ │
89
│ │ │ │ -
91 ~ShonanGaugeFactor() override {}
│ │ │ │ +
90 void setCertifyOptimality(bool value) { certifyOptimality = value; }
│ │ │ │ +
91 bool getCertifyOptimality() const { return certifyOptimality; }
│ │ │ │
92
│ │ │ │ -
94 double error(const Values &c) const override { return 0; }
│ │ │ │ -
95
│ │ │ │ -
97 size_t dim() const override { return rows_; }
│ │ │ │ -
98
│ │ │ │ -
│ │ │ │ -
100 boost::shared_ptr<GaussianFactor> linearize(const Values &c) const override {
│ │ │ │ -
101 return whitenedJacobian_;
│ │ │ │ -
102 }
│ │ │ │ -
│ │ │ │ -
103};
│ │ │ │ -
│ │ │ │ -
104// \ShonanGaugeFactor
│ │ │ │ -
105
│ │ │ │ -
106} // namespace gtsam
│ │ │ │ -
SOn.h
N*N matrix representation of SO(N).
│ │ │ │ - │ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
│ │ │ │ +
94 void print(const std::string &s = "") const {
│ │ │ │ +
95 std::cout << (s.empty() ? s : s + " ");
│ │ │ │ +
96 std::cout << " ShonanAveragingParameters: " << std::endl;
│ │ │ │ +
97 std::cout << " alpha: " << alpha << std::endl;
│ │ │ │ +
98 std::cout << " beta: " << beta << std::endl;
│ │ │ │ +
99 std::cout << " gamma: " << gamma << std::endl;
│ │ │ │ +
100 std::cout << " useHuber: " << useHuber << std::endl;
│ │ │ │ +
101 }
│ │ │ │ +
│ │ │ │ +
102};
│ │ │ │ +
│ │ │ │ +
103
│ │ │ │ +
104using ShonanAveragingParameters2 = ShonanAveragingParameters<2>;
│ │ │ │ +
105using ShonanAveragingParameters3 = ShonanAveragingParameters<3>;
│ │ │ │ +
106
│ │ │ │ +
122template <size_t d>
│ │ │ │ +
│ │ │ │ +
123class GTSAM_EXPORT ShonanAveraging {
│ │ │ │ +
124 public:
│ │ │ │ +
125 using Sparse = Eigen::SparseMatrix<double>;
│ │ │ │ +
126
│ │ │ │ +
127 // Define the Parameters type and use its typedef of the rotation type:
│ │ │ │ +
128 using Parameters = ShonanAveragingParameters<d>;
│ │ │ │ +
129 using Rot = typename Parameters::Rot;
│ │ │ │ +
130
│ │ │ │ +
131 // We store SO(d) BetweenFactors to get noise model
│ │ │ │ +
132 using Measurements = std::vector<BinaryMeasurement<Rot>>;
│ │ │ │ +
133
│ │ │ │ +
134 private:
│ │ │ │ +
135 Parameters parameters_;
│ │ │ │ +
136 Measurements measurements_;
│ │ │ │ +
137 size_t nrUnknowns_;
│ │ │ │ +
138 Sparse D_; // Sparse (diagonal) degree matrix
│ │ │ │ +
139 Sparse Q_; // Sparse measurement matrix, == \tilde{R} in Eriksson18cvpr
│ │ │ │ +
140 Sparse L_; // connection Laplacian L = D - Q, needed for optimality check
│ │ │ │ +
141
│ │ │ │ +
146 Sparse buildQ() const;
│ │ │ │ +
147
│ │ │ │ +
149 Sparse buildD() const;
│ │ │ │ +
150
│ │ │ │ +
151 public:
│ │ │ │ +
154
│ │ │ │ +
157 ShonanAveraging(const Measurements &measurements,
│ │ │ │ +
158 const Parameters &parameters = Parameters());
│ │ │ │ +
159
│ │ │ │ +
163
│ │ │ │ +
165 size_t nrUnknowns() const { return nrUnknowns_; }
│ │ │ │ +
166
│ │ │ │ +
168 size_t numberMeasurements() const { return measurements_.size(); }
│ │ │ │ +
169
│ │ │ │ +
│ │ │ │ +
171 const BinaryMeasurement<Rot> &measurement(size_t k) const {
│ │ │ │ +
172 return measurements_[k];
│ │ │ │ +
173 }
│ │ │ │ +
│ │ │ │ +
174
│ │ │ │ +
│ │ │ │ +
181 Measurements makeNoiseModelRobust(const Measurements &measurements,
│ │ │ │ +
182 double k = 1.345) const {
│ │ │ │ +
183 Measurements robustMeasurements;
│ │ │ │ +
184 for (auto &measurement : measurements) {
│ │ │ │ +
185 auto model = measurement.noiseModel();
│ │ │ │ +
186 const auto &robust =
│ │ │ │ +
187 boost::dynamic_pointer_cast<noiseModel::Robust>(model);
│ │ │ │ +
188
│ │ │ │ +
189 SharedNoiseModel robust_model;
│ │ │ │ +
190 // Check if the noise model is already robust
│ │ │ │ +
191 if (robust) {
│ │ │ │ +
192 robust_model = model;
│ │ │ │ +
193 } else {
│ │ │ │ +
194 // make robust
│ │ │ │ +
195 robust_model = noiseModel::Robust::Create(
│ │ │ │ +
196 noiseModel::mEstimator::Huber::Create(k), model);
│ │ │ │ +
197 }
│ │ │ │ +
198 BinaryMeasurement<Rot> meas(measurement.key1(), measurement.key2(),
│ │ │ │ +
199 measurement.measured(), robust_model);
│ │ │ │ +
200 robustMeasurements.push_back(meas);
│ │ │ │ +
201 }
│ │ │ │ +
202 return robustMeasurements;
│ │ │ │ +
203 }
│ │ │ │ +
│ │ │ │ +
204
│ │ │ │ +
206 const Rot &measured(size_t k) const { return measurements_[k].measured(); }
│ │ │ │ +
207
│ │ │ │ +
209 const KeyVector &keys(size_t k) const { return measurements_[k].keys(); }
│ │ │ │ +
210
│ │ │ │ +
214
│ │ │ │ +
215 Sparse D() const { return D_; }
│ │ │ │ +
│ │ │ │ +
216 Matrix denseD() const { return Matrix(D_); }
│ │ │ │ +
│ │ │ │ +
217 Sparse Q() const { return Q_; }
│ │ │ │ +
│ │ │ │ +
218 Matrix denseQ() const { return Matrix(Q_); }
│ │ │ │ +
│ │ │ │ +
219 Sparse L() const { return L_; }
│ │ │ │ +
│ │ │ │ +
220 Matrix denseL() const { return Matrix(L_); }
│ │ │ │ +
221
│ │ │ │ +
223 Sparse computeLambda(const Matrix &S) const;
│ │ │ │ +
224
│ │ │ │ +
│ │ │ │ +
226 Matrix computeLambda_(const Values &values) const {
│ │ │ │ +
227 return Matrix(computeLambda(values));
│ │ │ │ +
228 }
│ │ │ │ +
│ │ │ │ +
229
│ │ │ │ +
│ │ │ │ +
231 Matrix computeLambda_(const Matrix &S) const {
│ │ │ │ +
232 return Matrix(computeLambda(S));
│ │ │ │ +
233 }
│ │ │ │ +
│ │ │ │ +
234
│ │ │ │ +
236 Sparse computeA(const Values &values) const;
│ │ │ │ +
237
│ │ │ │ +
239 Sparse computeA(const Matrix &S) const;
│ │ │ │ +
240
│ │ │ │ +
│ │ │ │ +
242 Matrix computeA_(const Values &values) const {
│ │ │ │ +
243 return Matrix(computeA(values));
│ │ │ │ +
244 }
│ │ │ │ +
│ │ │ │ +
245
│ │ │ │ +
247 static Matrix StiefelElementMatrix(const Values &values);
│ │ │ │ +
248
│ │ │ │ +
253 double computeMinEigenValue(const Values &values,
│ │ │ │ +
254 Vector *minEigenVector = nullptr) const;
│ │ │ │ +
255
│ │ │ │ +
260 double computeMinEigenValueAP(const Values &values,
│ │ │ │ +
261 Vector *minEigenVector = nullptr) const;
│ │ │ │ +
262
│ │ │ │ +
264 Values roundSolutionS(const Matrix &S) const;
│ │ │ │ +
265
│ │ │ │ +
267 static VectorValues TangentVectorValues(size_t p, const Vector &v);
│ │ │ │ +
268
│ │ │ │ +
270 Matrix riemannianGradient(size_t p, const Values &values) const;
│ │ │ │ +
271
│ │ │ │ +
276 static Values LiftwithDescent(size_t p, const Values &values,
│ │ │ │ +
277 const Vector &minEigenVector);
│ │ │ │ +
278
│ │ │ │ +
286 Values initializeWithDescent(
│ │ │ │ +
287 size_t p, const Values &values, const Vector &minEigenVector,
│ │ │ │ +
288 double minEigenValue, double gradienTolerance = 1e-2,
│ │ │ │ +
289 double preconditionedGradNormTolerance = 1e-4) const;
│ │ │ │ +
293
│ │ │ │ +
298 NonlinearFactorGraph buildGraphAt(size_t p) const;
│ │ │ │ +
299
│ │ │ │ +
305 Values initializeRandomlyAt(size_t p, std::mt19937 &rng) const;
│ │ │ │ +
306
│ │ │ │ +
308 Values initializeRandomlyAt(size_t p) const;
│ │ │ │ +
309
│ │ │ │ +
314 double costAt(size_t p, const Values &values) const;
│ │ │ │ +
315
│ │ │ │ +
321 Sparse computeLambda(const Values &values) const;
│ │ │ │ +
322
│ │ │ │ +
328 std::pair<double, Vector> computeMinEigenVector(const Values &values) const;
│ │ │ │ +
329
│ │ │ │ +
334 bool checkOptimality(const Values &values) const;
│ │ │ │ +
335
│ │ │ │ +
342 boost::shared_ptr<LevenbergMarquardtOptimizer> createOptimizerAt(
│ │ │ │ +
343 size_t p, const Values &initial) const;
│ │ │ │ +
344
│ │ │ │ +
351 Values tryOptimizingAt(size_t p, const Values &initial) const;
│ │ │ │ +
352
│ │ │ │ +
357 Values projectFrom(size_t p, const Values &values) const;
│ │ │ │ +
358
│ │ │ │ +
363 Values roundSolution(const Values &values) const;
│ │ │ │ +
364
│ │ │ │ +
366 template <class T>
│ │ │ │ +
│ │ │ │ +
367 static Values LiftTo(size_t p, const Values &values) {
│ │ │ │ +
368 Values result;
│ │ │ │ +
369 for (const auto it : values.extract<T>()) {
│ │ │ │ +
370 result.insert(it.first, SOn::Lift(p, it.second.matrix()));
│ │ │ │ +
371 }
│ │ │ │ +
372 return result;
│ │ │ │ +
373 }
│ │ │ │ +
│ │ │ │ +
374
│ │ │ │ +
378
│ │ │ │ +
383 double cost(const Values &values) const;
│ │ │ │ +
384
│ │ │ │ +
392 Values initializeRandomly(std::mt19937 &rng) const;
│ │ │ │ +
393
│ │ │ │ +
395 Values initializeRandomly() const;
│ │ │ │ +
396
│ │ │ │ +
404 std::pair<Values, double> run(const Values &initialEstimate, size_t pMin = d,
│ │ │ │ +
405 size_t pMax = 10) const;
│ │ │ │ +
407
│ │ │ │ +
417 template <typename T>
│ │ │ │ +
│ │ │ │ +
418 inline std::vector<BinaryMeasurement<T>> maybeRobust(
│ │ │ │ +
419 const std::vector<BinaryMeasurement<T>> &measurements,
│ │ │ │ +
420 bool useRobustModel = false) const {
│ │ │ │ +
421 return useRobustModel ? makeNoiseModelRobust(measurements) : measurements;
│ │ │ │ +
422 }
│ │ │ │ +
│ │ │ │ +
423};
│ │ │ │ +
424
│ │ │ │ +
425// Subclasses for d=2 and d=3 that explicitly instantiate, as well as provide a
│ │ │ │ +
426// convenience interface with file access.
│ │ │ │ +
427
│ │ │ │ +
│ │ │ │ +
428class GTSAM_EXPORT ShonanAveraging2 : public ShonanAveraging<2> {
│ │ │ │ +
429 public:
│ │ │ │ +
430 ShonanAveraging2(const Measurements &measurements,
│ │ │ │ +
431 const Parameters &parameters = Parameters());
│ │ │ │ +
432 explicit ShonanAveraging2(std::string g2oFile,
│ │ │ │ +
433 const Parameters &parameters = Parameters());
│ │ │ │ +
434 ShonanAveraging2(const BetweenFactorPose2s &factors,
│ │ │ │ +
435 const Parameters &parameters = Parameters());
│ │ │ │ +
436};
│ │ │ │ +
│ │ │ │ +
437
│ │ │ │ +
│ │ │ │ +
438class GTSAM_EXPORT ShonanAveraging3 : public ShonanAveraging<3> {
│ │ │ │ +
439 public:
│ │ │ │ +
440 ShonanAveraging3(const Measurements &measurements,
│ │ │ │ +
441 const Parameters &parameters = Parameters());
│ │ │ │ +
442 explicit ShonanAveraging3(std::string g2oFile,
│ │ │ │ +
443 const Parameters &parameters = Parameters());
│ │ │ │ +
444
│ │ │ │ +
445 // TODO(frank): Deprecate after we land pybind wrapper
│ │ │ │ +
446 ShonanAveraging3(const BetweenFactorPose3s &factors,
│ │ │ │ +
447 const Parameters &parameters = Parameters());
│ │ │ │ +
448};
│ │ │ │ +
│ │ │ │ +
449} // namespace gtsam
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ +
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
Rot3.h
3D rotation represented as a rotation matrix or quaternion
│ │ │ │ +
Rot2.h
2D rotation
│ │ │ │ +
PowerMethod.h
Power method for fast eigenvalue and eigenvector computation.
│ │ │ │ +
VectorValues.h
Factor Graph Values.
│ │ │ │ +
AcceleratedPowerMethod.h
accelerated power method for fast eigenvalue and eigenvector computation
│ │ │ │ +
LevenbergMarquardtParams.h
Parameters for Levenberg-Marquardt trust-region scheme.
│ │ │ │ +
BinaryMeasurement.h
Binary measurement represents a measurement between two keys in a graph. A binary measurement is simi...
│ │ │ │ +
dataset.h
utility functions for loading datasets
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ +
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ +
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
gtsam::LevenbergMarquardtParams
Parameters for Levenberg-Marquardt optimization.
Definition LevenbergMarquardtParams.h:35
│ │ │ │ +
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
gtsam::ShonanGaugeFactor
The ShonanGaugeFactor creates a constraint on a single SO(n) to avoid moving in the stabilizer.
Definition ShonanGaugeFactor.h:45
│ │ │ │ -
gtsam::ShonanGaugeFactor::error
double error(const Values &c) const override
Calculate the error of the factor: always zero.
Definition ShonanGaugeFactor.h:94
│ │ │ │ -
gtsam::ShonanGaugeFactor::~ShonanGaugeFactor
~ShonanGaugeFactor() override
Destructor.
Definition ShonanGaugeFactor.h:91
│ │ │ │ -
gtsam::ShonanGaugeFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &c) const override
linearize to a GaussianFactor
Definition ShonanGaugeFactor.h:100
│ │ │ │ -
gtsam::ShonanGaugeFactor::dim
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition ShonanGaugeFactor.h:97
│ │ │ │ -
gtsam::ShonanGaugeFactor::ShonanGaugeFactor
ShonanGaugeFactor(Key key, size_t p, size_t d=3, boost::optional< double > gamma=boost::none)
Construct from key for an SO(p) matrix, for base dimension d (2 or 3) If parameter gamma is given,...
Definition ShonanGaugeFactor.h:58
│ │ │ │ +
gtsam::Values::extract
std::map< Key, ValueType > extract(const std::function< bool(Key)> &filterFcn=&_truePredicate< Key >) const
Extract a subset of values of the given type ValueType.
Definition Values-inl.h:94
│ │ │ │ +
gtsam::BinaryMeasurement
Definition BinaryMeasurement.h:36
│ │ │ │ +
gtsam::ShonanAveragingParameters
Parameters governing optimization etc.
Definition ShonanAveraging.h:46
│ │ │ │ +
gtsam::ShonanAveragingParameters::alpha
double alpha
weight of anchor-based prior (default 0)
Definition ShonanAveraging.h:55
│ │ │ │ +
gtsam::ShonanAveragingParameters::print
void print(const std::string &s="") const
Print the parameters and flags used for rotation averaging.
Definition ShonanAveraging.h:94
│ │ │ │ +
gtsam::ShonanAveragingParameters::lm
LevenbergMarquardtParams lm
LM parameters.
Definition ShonanAveraging.h:52
│ │ │ │ +
gtsam::ShonanAveragingParameters::useHuber
bool useHuber
if enabled, the Huber loss is used (default false)
Definition ShonanAveraging.h:59
│ │ │ │ +
gtsam::ShonanAveragingParameters::optimalityThreshold
double optimalityThreshold
threshold used in checkOptimality
Definition ShonanAveraging.h:53
│ │ │ │ +
gtsam::ShonanAveragingParameters::beta
double beta
weight of Karcher-based prior (default 1)
Definition ShonanAveraging.h:56
│ │ │ │ +
gtsam::ShonanAveragingParameters::gamma
double gamma
weight of gauge-fixing factors (default 0)
Definition ShonanAveraging.h:57
│ │ │ │ +
gtsam::ShonanAveragingParameters::anchor
Anchor anchor
pose to use as anchor if not Karcher
Definition ShonanAveraging.h:54
│ │ │ │ +
gtsam::ShonanAveragingParameters::certifyOptimality
bool certifyOptimality
if enabled solution optimality is certified (default true)
Definition ShonanAveraging.h:61
│ │ │ │ +
gtsam::ShonanAveraging
Class that implements Shonan Averaging from our ECCV'20 paper.
Definition ShonanAveraging.h:123
│ │ │ │ +
gtsam::ShonanAveraging::D
Sparse D() const
Sparse version of D.
Definition ShonanAveraging.h:215
│ │ │ │ +
gtsam::ShonanAveraging::measurement
const BinaryMeasurement< Rot > & measurement(size_t k) const
k^th binary measurement
Definition ShonanAveraging.h:171
│ │ │ │ +
gtsam::ShonanAveraging::makeNoiseModelRobust
Measurements makeNoiseModelRobust(const Measurements &measurements, double k=1.345) const
Update factors to use robust Huber loss.
Definition ShonanAveraging.h:181
│ │ │ │ +
gtsam::ShonanAveraging::roundSolutionS
Values roundSolutionS(const Matrix &S) const
Project pxdN Stiefel manifold matrix S to Rot3^N.
│ │ │ │ +
gtsam::ShonanAveraging::maybeRobust
std::vector< BinaryMeasurement< T > > maybeRobust(const std::vector< BinaryMeasurement< T > > &measurements, bool useRobustModel=false) const
Helper function to convert measurements to robust noise model if flag is set.
Definition ShonanAveraging.h:418
│ │ │ │ +
gtsam::ShonanAveraging::L
Sparse L() const
Sparse version of L.
Definition ShonanAveraging.h:219
│ │ │ │ +
gtsam::ShonanAveraging::measured
const Rot & measured(size_t k) const
k^th measurement, as a Rot.
Definition ShonanAveraging.h:206
│ │ │ │ +
gtsam::ShonanAveraging::Q
Sparse Q() const
Sparse version of Q.
Definition ShonanAveraging.h:217
│ │ │ │ +
gtsam::ShonanAveraging::denseD
Matrix denseD() const
Dense version of D.
Definition ShonanAveraging.h:216
│ │ │ │ +
gtsam::ShonanAveraging::nrUnknowns
size_t nrUnknowns() const
Return number of unknowns.
Definition ShonanAveraging.h:165
│ │ │ │ +
gtsam::ShonanAveraging::projectFrom
Values projectFrom(size_t p, const Values &values) const
Project from SO(p) to Rot2 or Rot3 Values should be of type SO(p)
│ │ │ │ +
gtsam::ShonanAveraging::numberMeasurements
size_t numberMeasurements() const
Return number of measurements.
Definition ShonanAveraging.h:168
│ │ │ │ +
gtsam::ShonanAveraging::denseL
Matrix denseL() const
Dense version of L.
Definition ShonanAveraging.h:220
│ │ │ │ +
gtsam::ShonanAveraging::computeLambda_
Matrix computeLambda_(const Matrix &S) const
Dense versions of computeLambda for wrapper/testing.
Definition ShonanAveraging.h:231
│ │ │ │ +
gtsam::ShonanAveraging::denseQ
Matrix denseQ() const
Dense version of Q.
Definition ShonanAveraging.h:218
│ │ │ │ +
gtsam::ShonanAveraging::keys
const KeyVector & keys(size_t k) const
Keys for k^th measurement, as a vector of Key values.
Definition ShonanAveraging.h:209
│ │ │ │ +
gtsam::ShonanAveraging::computeLambda_
Matrix computeLambda_(const Values &values) const
Dense versions of computeLambda for wrapper/testing.
Definition ShonanAveraging.h:226
│ │ │ │ +
gtsam::ShonanAveraging::computeA_
Matrix computeA_(const Values &values) const
Dense version of computeA for wrapper/testing.
Definition ShonanAveraging.h:242
│ │ │ │ +
gtsam::ShonanAveraging::LiftTo
static Values LiftTo(size_t p, const Values &values)
Lift Values of type T to SO(p)
Definition ShonanAveraging.h:367
│ │ │ │ +
gtsam::ShonanAveraging2
Definition ShonanAveraging.h:428
│ │ │ │ +
gtsam::ShonanAveraging3
Definition ShonanAveraging.h:438
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ShonanGaugeFactor.h │ │ │ │ │ +ShonanAveraging.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ @@ -16,117 +16,458 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 19#pragma once │ │ │ │ │ 20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_S_O_n_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -24 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ -_4_5class GTSAM_EXPORT _S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r : public _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r { │ │ │ │ │ -46 // Row dimension, equal to the dimensionality of SO(p-d) │ │ │ │ │ -47 size_t rows_; │ │ │ │ │ -48 │ │ │ │ │ -50 boost::shared_ptr whitenedJacobian_; │ │ │ │ │ -51 │ │ │ │ │ -52public: │ │ │ │ │ -_5_8 _S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r(_K_e_y key, size_t p, size_t d = 3, │ │ │ │ │ -59 boost::optional gamma = boost::none) │ │ │ │ │ -60 : _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r(_K_e_y_V_e_c_t_o_r{key}) { │ │ │ │ │ -61 if (p < d) { │ │ │ │ │ -62 throw std::invalid_argument("ShonanGaugeFactor must have p>=d."); │ │ │ │ │ -63 } │ │ │ │ │ -64 // Calculate dimensions │ │ │ │ │ -65 size_t q = p - d; │ │ │ │ │ -66 size_t P = SOn::Dimension(p); // dimensionality of SO(p) │ │ │ │ │ -67 rows_ = SOn::Dimension(q); // dimensionality of SO(q), the gauge │ │ │ │ │ -68 │ │ │ │ │ -69 // Create constant Jacobian as a rows_*P matrix: there are rows_ penalized │ │ │ │ │ -70 // dimensions, but it is a bit tricky to find them among the P columns. │ │ │ │ │ -71 // The key is to look at how skew-symmetric matrices are laid out in SOn.h: │ │ │ │ │ -72 // the first tangent dimension will always be included, but beyond that we │ │ │ │ │ -73 // have to be careful. We always need to skip the d top-rows of the skew- │ │ │ │ │ -74 // symmetric matrix as they below to K, part of the Stiefel manifold. │ │ │ │ │ -75 Matrix A(rows_, P); │ │ │ │ │ -76 A.setZero(); │ │ │ │ │ -77 double invSigma = gamma ? std::sqrt(*gamma) : 1.0; │ │ │ │ │ -78 size_t i = 0, j = 0, n = p - 1 - d; │ │ │ │ │ -79 while (i < rows_) { │ │ │ │ │ -80 A.block(i, j, n, n) = invSigma * Matrix::Identity(n, n); │ │ │ │ │ -81 i += n; │ │ │ │ │ -82 j += n + d; // skip d columns │ │ │ │ │ -83 n -= 1; │ │ │ │ │ -84 } │ │ │ │ │ -85 // TODO(frank): assign the right one in the right columns │ │ │ │ │ -86 whitenedJacobian_ = │ │ │ │ │ -87 boost::make_shared(key, A, Vector::Zero(rows_)); │ │ │ │ │ -88 } │ │ │ │ │ +21#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_c_t_o_r_._h> │ │ │ │ │ +23#include │ │ │ │ │ +24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_2_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_3_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ +27#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_._h> │ │ │ │ │ +28#include <_g_t_s_a_m_/_s_f_m_/_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_._h> │ │ │ │ │ +29#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_P_o_w_e_r_M_e_t_h_o_d_._h> │ │ │ │ │ +30#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_A_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_M_e_t_h_o_d_._h> │ │ │ │ │ +31#include <_g_t_s_a_m_/_s_l_a_m_/_d_a_t_a_s_e_t_._h> │ │ │ │ │ +32 │ │ │ │ │ +33#include │ │ │ │ │ +34#include │ │ │ │ │ +35#include │ │ │ │ │ +36#include │ │ │ │ │ +37#include │ │ │ │ │ +38#include │ │ │ │ │ +39 │ │ │ │ │ +40namespace _g_t_s_a_m { │ │ │ │ │ +41class NonlinearFactorGraph; │ │ │ │ │ +42class LevenbergMarquardtOptimizer; │ │ │ │ │ +43 │ │ │ │ │ +45template │ │ │ │ │ +_4_6struct GTSAM_EXPORT _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s { │ │ │ │ │ +47 // Select Rot2 or Rot3 interface based template parameter d │ │ │ │ │ +48 using Rot = typename std::conditional::type; │ │ │ │ │ +49 using Anchor = std::pair; │ │ │ │ │ +50 │ │ │ │ │ +51 // Parameters themselves: │ │ │ │ │ +_5_2 _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s _l_m; │ │ │ │ │ +_5_3 double _o_p_t_i_m_a_l_i_t_y_T_h_r_e_s_h_o_l_d; │ │ │ │ │ +_5_4 Anchor _a_n_c_h_o_r; │ │ │ │ │ +_5_5 double _a_l_p_h_a; │ │ │ │ │ +_5_6 double _b_e_t_a; │ │ │ │ │ +_5_7 double _g_a_m_m_a; │ │ │ │ │ +_5_9 bool _u_s_e_H_u_b_e_r; │ │ │ │ │ +_6_1 bool _c_e_r_t_i_f_y_O_p_t_i_m_a_l_i_t_y; │ │ │ │ │ +62 │ │ │ │ │ +63 _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s(const _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s &lm = │ │ │ │ │ +64 LevenbergMarquardtParams::CeresDefaults(), │ │ │ │ │ +65 const std::string &method = "JACOBI", │ │ │ │ │ +66 double optimalityThreshold = -1e-4, │ │ │ │ │ +67 double alpha = 0.0, double beta = 1.0, │ │ │ │ │ +68 double gamma = 0.0); │ │ │ │ │ +69 │ │ │ │ │ +70 _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s getLMParams() const { return lm; } │ │ │ │ │ +71 │ │ │ │ │ +72 void setOptimalityThreshold(double value) { optimalityThreshold = value; } │ │ │ │ │ +73 double getOptimalityThreshold() const { return optimalityThreshold; } │ │ │ │ │ +74 │ │ │ │ │ +75 void setAnchor(size_t index, const Rot &value) { anchor = {index, value}; } │ │ │ │ │ +76 std::pair getAnchor() const { return anchor; } │ │ │ │ │ +77 │ │ │ │ │ +78 void setAnchorWeight(double value) { alpha = value; } │ │ │ │ │ +79 double getAnchorWeight() const { return alpha; } │ │ │ │ │ +80 │ │ │ │ │ +81 void setKarcherWeight(double value) { beta = value; } │ │ │ │ │ +82 double getKarcherWeight() const { return beta; } │ │ │ │ │ +83 │ │ │ │ │ +84 void setGaugesWeight(double value) { gamma = value; } │ │ │ │ │ +85 double getGaugesWeight() const { return gamma; } │ │ │ │ │ +86 │ │ │ │ │ +87 void setUseHuber(bool value) { useHuber = value; } │ │ │ │ │ +88 bool getUseHuber() const { return useHuber; } │ │ │ │ │ 89 │ │ │ │ │ -_9_1 _~_S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r() override {} │ │ │ │ │ +90 void setCertifyOptimality(bool value) { certifyOptimality = value; } │ │ │ │ │ +91 bool getCertifyOptimality() const { return certifyOptimality; } │ │ │ │ │ 92 │ │ │ │ │ -_9_4 double _e_r_r_o_r(const _V_a_l_u_e_s &c) const override { return 0; } │ │ │ │ │ -95 │ │ │ │ │ -_9_7 size_t _d_i_m() const override { return rows_; } │ │ │ │ │ -98 │ │ │ │ │ -_1_0_0 boost::shared_ptr _l_i_n_e_a_r_i_z_e(const _V_a_l_u_e_s &c) const override │ │ │ │ │ -{ │ │ │ │ │ -101 return whitenedJacobian_; │ │ │ │ │ -102 } │ │ │ │ │ -103}; │ │ │ │ │ -104// \ShonanGaugeFactor │ │ │ │ │ -105 │ │ │ │ │ -106} // namespace gtsam │ │ │ │ │ -_S_O_n_._h │ │ │ │ │ -N*N matrix representation of SO(N). │ │ │ │ │ -_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ -Non-linear factor base classes. │ │ │ │ │ +_9_4 void _p_r_i_n_t(const std::string &s = "") const { │ │ │ │ │ +95 std::cout << (s.empty() ? s : s + " "); │ │ │ │ │ +96 std::cout << " ShonanAveragingParameters: " << std::endl; │ │ │ │ │ +97 std::cout << " alpha: " << alpha << std::endl; │ │ │ │ │ +98 std::cout << " beta: " << beta << std::endl; │ │ │ │ │ +99 std::cout << " gamma: " << gamma << std::endl; │ │ │ │ │ +100 std::cout << " useHuber: " << useHuber << std::endl; │ │ │ │ │ +101 } │ │ │ │ │ +102}; │ │ │ │ │ +103 │ │ │ │ │ +104using ShonanAveragingParameters2 = ShonanAveragingParameters<2>; │ │ │ │ │ +105using ShonanAveragingParameters3 = ShonanAveragingParameters<3>; │ │ │ │ │ +106 │ │ │ │ │ +122template │ │ │ │ │ +_1_2_3class GTSAM_EXPORT _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g { │ │ │ │ │ +124 public: │ │ │ │ │ +125 using Sparse = Eigen::SparseMatrix; │ │ │ │ │ +126 │ │ │ │ │ +127 // Define the Parameters type and use its typedef of the rotation type: │ │ │ │ │ +128 using _P_a_r_a_m_e_t_e_r_s = _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s_<_d_>; │ │ │ │ │ +129 using Rot = typename Parameters::Rot; │ │ │ │ │ +130 │ │ │ │ │ +131 // We store SO(d) BetweenFactors to get noise model │ │ │ │ │ +132 using Measurements = std::vector>; │ │ │ │ │ +133 │ │ │ │ │ +134 private: │ │ │ │ │ +135 _P_a_r_a_m_e_t_e_r_s parameters_; │ │ │ │ │ +136 Measurements measurements_; │ │ │ │ │ +137 size_t nrUnknowns_; │ │ │ │ │ +138 Sparse D_; // Sparse (diagonal) degree matrix │ │ │ │ │ +139 Sparse Q_; // Sparse measurement matrix, == \tilde{R} in Eriksson18cvpr │ │ │ │ │ +140 Sparse L_; // connection Laplacian L = D - Q, needed for optimality check │ │ │ │ │ +141 │ │ │ │ │ +146 Sparse buildQ() const; │ │ │ │ │ +147 │ │ │ │ │ +149 Sparse buildD() const; │ │ │ │ │ +150 │ │ │ │ │ +151 public: │ │ │ │ │ +154 │ │ │ │ │ +157 _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g(const Measurements &measurements, │ │ │ │ │ +158 const _P_a_r_a_m_e_t_e_r_s ¶meters = _P_a_r_a_m_e_t_e_r_s()); │ │ │ │ │ +159 │ │ │ │ │ +163 │ │ │ │ │ +_1_6_5 size_t _n_r_U_n_k_n_o_w_n_s() const { return nrUnknowns_; } │ │ │ │ │ +166 │ │ │ │ │ +_1_6_8 size_t _n_u_m_b_e_r_M_e_a_s_u_r_e_m_e_n_t_s() const { return measurements_.size(); } │ │ │ │ │ +169 │ │ │ │ │ +_1_7_1 const _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_<_R_o_t_> &_m_e_a_s_u_r_e_m_e_n_t(size_t k) const { │ │ │ │ │ +172 return measurements_[k]; │ │ │ │ │ +173 } │ │ │ │ │ +174 │ │ │ │ │ +_1_8_1 Measurements _m_a_k_e_N_o_i_s_e_M_o_d_e_l_R_o_b_u_s_t(const Measurements &measurements, │ │ │ │ │ +182 double k = 1.345) const { │ │ │ │ │ +183 Measurements robustMeasurements; │ │ │ │ │ +184 for (auto &measurement : measurements) { │ │ │ │ │ +185 auto model = measurement.noiseModel(); │ │ │ │ │ +186 const auto &robust = │ │ │ │ │ +187 boost::dynamic_pointer_cast(model); │ │ │ │ │ +188 │ │ │ │ │ +189 _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l robust_model; │ │ │ │ │ +190 // Check if the noise model is already robust │ │ │ │ │ +191 if (robust) { │ │ │ │ │ +192 robust_model = model; │ │ │ │ │ +193 } else { │ │ │ │ │ +194 // make robust │ │ │ │ │ +195 robust_model = noiseModel::Robust::Create( │ │ │ │ │ +196 noiseModel::mEstimator::Huber::Create(k), model); │ │ │ │ │ +197 } │ │ │ │ │ +198 _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_<_R_o_t_> meas(measurement.key1(), measurement.key2(), │ │ │ │ │ +199 measurement.measured(), robust_model); │ │ │ │ │ +200 robustMeasurements.push_back(meas); │ │ │ │ │ +201 } │ │ │ │ │ +202 return robustMeasurements; │ │ │ │ │ +203 } │ │ │ │ │ +204 │ │ │ │ │ +_2_0_6 const Rot &_m_e_a_s_u_r_e_d(size_t k) const { return measurements_[k].measured(); } │ │ │ │ │ +207 │ │ │ │ │ +_2_0_9 const _K_e_y_V_e_c_t_o_r &_k_e_y_s(size_t k) const { return measurements_[k].keys(); } │ │ │ │ │ +210 │ │ │ │ │ +214 │ │ │ │ │ +_2_1_5 Sparse _D() const { return D_; } │ │ │ │ │ +_2_1_6 Matrix _d_e_n_s_e_D() const { return Matrix(D_); } │ │ │ │ │ +_2_1_7 Sparse _Q() const { return Q_; } │ │ │ │ │ +_2_1_8 Matrix _d_e_n_s_e_Q() const { return Matrix(Q_); } │ │ │ │ │ +_2_1_9 Sparse _L() const { return L_; } │ │ │ │ │ +_2_2_0 Matrix _d_e_n_s_e_L() const { return Matrix(L_); } │ │ │ │ │ +221 │ │ │ │ │ +223 Sparse computeLambda(const Matrix &S) const; │ │ │ │ │ +224 │ │ │ │ │ +_2_2_6 Matrix _c_o_m_p_u_t_e_L_a_m_b_d_a__(const _V_a_l_u_e_s &values) const { │ │ │ │ │ +227 return Matrix(computeLambda(values)); │ │ │ │ │ +228 } │ │ │ │ │ +229 │ │ │ │ │ +_2_3_1 Matrix _c_o_m_p_u_t_e_L_a_m_b_d_a__(const Matrix &S) const { │ │ │ │ │ +232 return Matrix(computeLambda(S)); │ │ │ │ │ +233 } │ │ │ │ │ +234 │ │ │ │ │ +236 Sparse computeA(const _V_a_l_u_e_s &values) const; │ │ │ │ │ +237 │ │ │ │ │ +239 Sparse computeA(const Matrix &S) const; │ │ │ │ │ +240 │ │ │ │ │ +_2_4_2 Matrix _c_o_m_p_u_t_e_A__(const _V_a_l_u_e_s &values) const { │ │ │ │ │ +243 return Matrix(computeA(values)); │ │ │ │ │ +244 } │ │ │ │ │ +245 │ │ │ │ │ +247 static Matrix StiefelElementMatrix(const _V_a_l_u_e_s &values); │ │ │ │ │ +248 │ │ │ │ │ +253 double computeMinEigenValue(const _V_a_l_u_e_s &values, │ │ │ │ │ +254 Vector *minEigenVector = nullptr) const; │ │ │ │ │ +255 │ │ │ │ │ +260 double computeMinEigenValueAP(const _V_a_l_u_e_s &values, │ │ │ │ │ +261 Vector *minEigenVector = nullptr) const; │ │ │ │ │ +262 │ │ │ │ │ +_2_6_4 _V_a_l_u_e_s _r_o_u_n_d_S_o_l_u_t_i_o_n_S(const Matrix &S) const; │ │ │ │ │ +265 │ │ │ │ │ +267 static _V_e_c_t_o_r_V_a_l_u_e_s TangentVectorValues(size_t p, const Vector &v); │ │ │ │ │ +268 │ │ │ │ │ +270 Matrix riemannianGradient(size_t p, const _V_a_l_u_e_s &values) const; │ │ │ │ │ +271 │ │ │ │ │ +276 static _V_a_l_u_e_s LiftwithDescent(size_t p, const _V_a_l_u_e_s &values, │ │ │ │ │ +277 const Vector &minEigenVector); │ │ │ │ │ +278 │ │ │ │ │ +286 _V_a_l_u_e_s initializeWithDescent( │ │ │ │ │ +287 size_t p, const _V_a_l_u_e_s &values, const Vector &minEigenVector, │ │ │ │ │ +288 double minEigenValue, double gradienTolerance = 1e-2, │ │ │ │ │ +289 double preconditionedGradNormTolerance = 1e-4) const; │ │ │ │ │ +293 │ │ │ │ │ +298 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h buildGraphAt(size_t p) const; │ │ │ │ │ +299 │ │ │ │ │ +305 _V_a_l_u_e_s initializeRandomlyAt(size_t p, std::mt19937 &rng) const; │ │ │ │ │ +306 │ │ │ │ │ +308 _V_a_l_u_e_s initializeRandomlyAt(size_t p) const; │ │ │ │ │ +309 │ │ │ │ │ +314 double costAt(size_t p, const _V_a_l_u_e_s &values) const; │ │ │ │ │ +315 │ │ │ │ │ +321 Sparse computeLambda(const _V_a_l_u_e_s &values) const; │ │ │ │ │ +322 │ │ │ │ │ +328 std::pair computeMinEigenVector(const _V_a_l_u_e_s &values) │ │ │ │ │ +const; │ │ │ │ │ +329 │ │ │ │ │ +334 bool checkOptimality(const _V_a_l_u_e_s &values) const; │ │ │ │ │ +335 │ │ │ │ │ +342 boost::shared_ptr createOptimizerAt( │ │ │ │ │ +343 size_t p, const _V_a_l_u_e_s &initial) const; │ │ │ │ │ +344 │ │ │ │ │ +351 _V_a_l_u_e_s tryOptimizingAt(size_t p, const _V_a_l_u_e_s &initial) const; │ │ │ │ │ +352 │ │ │ │ │ +_3_5_7 _V_a_l_u_e_s _p_r_o_j_e_c_t_F_r_o_m(size_t p, const _V_a_l_u_e_s &values) const; │ │ │ │ │ +358 │ │ │ │ │ +363 _V_a_l_u_e_s roundSolution(const _V_a_l_u_e_s &values) const; │ │ │ │ │ +364 │ │ │ │ │ +366 template │ │ │ │ │ +_3_6_7 static _V_a_l_u_e_s _L_i_f_t_T_o(size_t p, const _V_a_l_u_e_s &values) { │ │ │ │ │ +368 _V_a_l_u_e_s result; │ │ │ │ │ +369 for (const auto it : values._e_x_t_r_a_c_t()) { │ │ │ │ │ +370 result.insert(it.first, SOn::Lift(p, it.second.matrix())); │ │ │ │ │ +371 } │ │ │ │ │ +372 return result; │ │ │ │ │ +373 } │ │ │ │ │ +374 │ │ │ │ │ +378 │ │ │ │ │ +383 double cost(const _V_a_l_u_e_s &values) const; │ │ │ │ │ +384 │ │ │ │ │ +392 _V_a_l_u_e_s initializeRandomly(std::mt19937 &rng) const; │ │ │ │ │ +393 │ │ │ │ │ +395 _V_a_l_u_e_s initializeRandomly() const; │ │ │ │ │ +396 │ │ │ │ │ +404 std::pair run(const _V_a_l_u_e_s &initialEstimate, size_t pMin = │ │ │ │ │ +d, │ │ │ │ │ +405 size_t pMax = 10) const; │ │ │ │ │ +407 │ │ │ │ │ +417 template │ │ │ │ │ +_4_1_8 inline std::vector> _m_a_y_b_e_R_o_b_u_s_t( │ │ │ │ │ +419 const std::vector<_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_<_T_>> &measurements, │ │ │ │ │ +420 bool useRobustModel = false) const { │ │ │ │ │ +421 return useRobustModel ? makeNoiseModelRobust(measurements) : measurements; │ │ │ │ │ +422 } │ │ │ │ │ +423}; │ │ │ │ │ +424 │ │ │ │ │ +425// Subclasses for d=2 and d=3 that explicitly instantiate, as well as │ │ │ │ │ +provide a │ │ │ │ │ +426// convenience interface with file access. │ │ │ │ │ +427 │ │ │ │ │ +_4_2_8class GTSAM_EXPORT _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_2 : public _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g<2> { │ │ │ │ │ +429 public: │ │ │ │ │ +430 _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_2(const Measurements &measurements, │ │ │ │ │ +431 const Parameters ¶meters = Parameters()); │ │ │ │ │ +432 explicit _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_2(std::string g2oFile, │ │ │ │ │ +433 const Parameters ¶meters = Parameters()); │ │ │ │ │ +434 _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_2(const BetweenFactorPose2s &factors, │ │ │ │ │ +435 const Parameters ¶meters = Parameters()); │ │ │ │ │ +436}; │ │ │ │ │ +437 │ │ │ │ │ +_4_3_8class GTSAM_EXPORT _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_3 : public _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g<3> { │ │ │ │ │ +439 public: │ │ │ │ │ +440 _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_3(const Measurements &measurements, │ │ │ │ │ +441 const Parameters ¶meters = Parameters()); │ │ │ │ │ +442 explicit _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_3(std::string g2oFile, │ │ │ │ │ +443 const Parameters ¶meters = Parameters()); │ │ │ │ │ +444 │ │ │ │ │ +445 // TODO(frank): Deprecate after we land pybind wrapper │ │ │ │ │ +446 _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_3(const BetweenFactorPose3s &factors, │ │ │ │ │ +447 const Parameters ¶meters = Parameters()); │ │ │ │ │ +448}; │ │ │ │ │ +449} // namespace gtsam │ │ │ │ │ +_V_e_c_t_o_r_._h │ │ │ │ │ +typedef and functions to augment Eigen's VectorXd │ │ │ │ │ +_M_a_t_r_i_x_._h │ │ │ │ │ +typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +_R_o_t_3_._h │ │ │ │ │ +3D rotation represented as a rotation matrix or quaternion │ │ │ │ │ +_R_o_t_2_._h │ │ │ │ │ +2D rotation │ │ │ │ │ +_P_o_w_e_r_M_e_t_h_o_d_._h │ │ │ │ │ +Power method for fast eigenvalue and eigenvector computation. │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ +Factor Graph Values. │ │ │ │ │ +_A_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_M_e_t_h_o_d_._h │ │ │ │ │ +accelerated power method for fast eigenvalue and eigenvector computation │ │ │ │ │ +_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_._h │ │ │ │ │ +Parameters for Levenberg-Marquardt trust-region scheme. │ │ │ │ │ +_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_._h │ │ │ │ │ +Binary measurement represents a measurement between two keys in a graph. A │ │ │ │ │ +binary measurement is simi... │ │ │ │ │ +_d_a_t_a_s_e_t_._h │ │ │ │ │ +utility functions for loading datasets │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ FastVector< Key > KeyVector │ │ │ │ │ Define collection type once and for all - also used in wrappers. │ │ │ │ │ DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ -Nonlinear factor base class. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ +Aliases. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ +with a unique integer... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s │ │ │ │ │ +Parameters for Levenberg-Marquardt optimization. │ │ │ │ │ +DDeeffiinniittiioonn LevenbergMarquardtParams.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ _g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r │ │ │ │ │ -The ShonanGaugeFactor creates a constraint on a single SO(n) to avoid moving in │ │ │ │ │ -the stabilizer. │ │ │ │ │ -DDeeffiinniittiioonn ShonanGaugeFactor.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r_:_:_e_r_r_o_r │ │ │ │ │ -double error(const Values &c) const override │ │ │ │ │ -Calculate the error of the factor: always zero. │ │ │ │ │ -DDeeffiinniittiioonn ShonanGaugeFactor.h:94 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r_:_:_~_S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r │ │ │ │ │ -~ShonanGaugeFactor() override │ │ │ │ │ -Destructor. │ │ │ │ │ -DDeeffiinniittiioonn ShonanGaugeFactor.h:91 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e │ │ │ │ │ -boost::shared_ptr< GaussianFactor > linearize(const Values &c) const override │ │ │ │ │ -linearize to a GaussianFactor │ │ │ │ │ -DDeeffiinniittiioonn ShonanGaugeFactor.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r_:_:_d_i_m │ │ │ │ │ -size_t dim() const override │ │ │ │ │ -get the dimension of the factor (number of rows on linearization) │ │ │ │ │ -DDeeffiinniittiioonn ShonanGaugeFactor.h:97 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r_:_:_S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r │ │ │ │ │ -ShonanGaugeFactor(Key key, size_t p, size_t d=3, boost::optional< double > │ │ │ │ │ -gamma=boost::none) │ │ │ │ │ -Construct from key for an SO(p) matrix, for base dimension d (2 or 3) If │ │ │ │ │ -parameter gamma is given,... │ │ │ │ │ -DDeeffiinniittiioonn ShonanGaugeFactor.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_e_x_t_r_a_c_t │ │ │ │ │ +std::map< Key, ValueType > extract(const std::function< bool(Key)> │ │ │ │ │ +&filterFcn=&_truePredicate< Key >) const │ │ │ │ │ +Extract a subset of values of the given type ValueType. │ │ │ │ │ +DDeeffiinniittiioonn Values-inl.h:94 │ │ │ │ │ +_g_t_s_a_m_:_:_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t │ │ │ │ │ +DDeeffiinniittiioonn BinaryMeasurement.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ +Parameters governing optimization etc. │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s_:_:_a_l_p_h_a │ │ │ │ │ +double alpha │ │ │ │ │ +weight of anchor-based prior (default 0) │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="") const │ │ │ │ │ +Print the parameters and flags used for rotation averaging. │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:94 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s_:_:_l_m │ │ │ │ │ +LevenbergMarquardtParams lm │ │ │ │ │ +LM parameters. │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s_:_:_u_s_e_H_u_b_e_r │ │ │ │ │ +bool useHuber │ │ │ │ │ +if enabled, the Huber loss is used (default false) │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:59 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s_:_:_o_p_t_i_m_a_l_i_t_y_T_h_r_e_s_h_o_l_d │ │ │ │ │ +double optimalityThreshold │ │ │ │ │ +threshold used in checkOptimality │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:53 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s_:_:_b_e_t_a │ │ │ │ │ +double beta │ │ │ │ │ +weight of Karcher-based prior (default 1) │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:56 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s_:_:_g_a_m_m_a │ │ │ │ │ +double gamma │ │ │ │ │ +weight of gauge-fixing factors (default 0) │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:57 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s_:_:_a_n_c_h_o_r │ │ │ │ │ +Anchor anchor │ │ │ │ │ +pose to use as anchor if not Karcher │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:54 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s_:_:_c_e_r_t_i_f_y_O_p_t_i_m_a_l_i_t_y │ │ │ │ │ +bool certifyOptimality │ │ │ │ │ +if enabled solution optimality is certified (default true) │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:61 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g │ │ │ │ │ +Class that implements Shonan Averaging from our ECCV'20 paper. │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:123 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_D │ │ │ │ │ +Sparse D() const │ │ │ │ │ +Sparse version of D. │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:215 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_m_e_a_s_u_r_e_m_e_n_t │ │ │ │ │ +const BinaryMeasurement< Rot > & measurement(size_t k) const │ │ │ │ │ +k^th binary measurement │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:171 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_m_a_k_e_N_o_i_s_e_M_o_d_e_l_R_o_b_u_s_t │ │ │ │ │ +Measurements makeNoiseModelRobust(const Measurements &measurements, double │ │ │ │ │ +k=1.345) const │ │ │ │ │ +Update factors to use robust Huber loss. │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:181 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_r_o_u_n_d_S_o_l_u_t_i_o_n_S │ │ │ │ │ +Values roundSolutionS(const Matrix &S) const │ │ │ │ │ +Project pxdN Stiefel manifold matrix S to Rot3^N. │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_m_a_y_b_e_R_o_b_u_s_t │ │ │ │ │ +std::vector< BinaryMeasurement< T > > maybeRobust(const std::vector< │ │ │ │ │ +BinaryMeasurement< T > > &measurements, bool useRobustModel=false) const │ │ │ │ │ +Helper function to convert measurements to robust noise model if flag is set. │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:418 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_L │ │ │ │ │ +Sparse L() const │ │ │ │ │ +Sparse version of L. │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:219 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_m_e_a_s_u_r_e_d │ │ │ │ │ +const Rot & measured(size_t k) const │ │ │ │ │ +k^th measurement, as a Rot. │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:206 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_Q │ │ │ │ │ +Sparse Q() const │ │ │ │ │ +Sparse version of Q. │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:217 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_d_e_n_s_e_D │ │ │ │ │ +Matrix denseD() const │ │ │ │ │ +Dense version of D. │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:216 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_n_r_U_n_k_n_o_w_n_s │ │ │ │ │ +size_t nrUnknowns() const │ │ │ │ │ +Return number of unknowns. │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:165 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_p_r_o_j_e_c_t_F_r_o_m │ │ │ │ │ +Values projectFrom(size_t p, const Values &values) const │ │ │ │ │ +Project from SO(p) to Rot2 or Rot3 Values should be of type SO(p) │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_n_u_m_b_e_r_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ +size_t numberMeasurements() const │ │ │ │ │ +Return number of measurements. │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:168 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_d_e_n_s_e_L │ │ │ │ │ +Matrix denseL() const │ │ │ │ │ +Dense version of L. │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:220 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_c_o_m_p_u_t_e_L_a_m_b_d_a__ │ │ │ │ │ +Matrix computeLambda_(const Matrix &S) const │ │ │ │ │ +Dense versions of computeLambda for wrapper/testing. │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:231 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_d_e_n_s_e_Q │ │ │ │ │ +Matrix denseQ() const │ │ │ │ │ +Dense version of Q. │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:218 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_k_e_y_s │ │ │ │ │ +const KeyVector & keys(size_t k) const │ │ │ │ │ +Keys for k^th measurement, as a vector of Key values. │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:209 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_c_o_m_p_u_t_e_L_a_m_b_d_a__ │ │ │ │ │ +Matrix computeLambda_(const Values &values) const │ │ │ │ │ +Dense versions of computeLambda for wrapper/testing. │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:226 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_c_o_m_p_u_t_e_A__ │ │ │ │ │ +Matrix computeA_(const Values &values) const │ │ │ │ │ +Dense version of computeA for wrapper/testing. │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:242 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_L_i_f_t_T_o │ │ │ │ │ +static Values LiftTo(size_t p, const Values &values) │ │ │ │ │ +Lift Values of type T to SO(p) │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:367 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_2 │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:428 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_3 │ │ │ │ │ +DDeeffiinniittiioonn ShonanAveraging.h:438 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_f_m │ │ │ │ │ - * _S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r_._h │ │ │ │ │ + * _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01235.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/FrobeniusFactor.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/PoseTranslationPrior.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,43 +94,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
FrobeniusFactor.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
PoseTranslationPrior.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Various factors that minimize some Frobenius norm. │ │ │ │ +

Implements a prior on the translation component of a pose. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::PoseTranslationPrior< POSE >
 A prior on the translation part of a pose. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

SharedNoiseModel gtsam::ConvertNoiseModel (const SharedNoiseModel &model, size_t n, bool defaultToUnit=true)
 When creating (any) FrobeniusFactor we can convert a Rot/Pose BetweenFactor noise model into a n-dimensional isotropic noise model used to weight the Frobenius norm.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Various factors that minimize some Frobenius norm.

│ │ │ │ -
Date
March 2019
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +

Implements a prior on the translation component of a pose.

│ │ │ │ +
Date
Jun 14, 2012
│ │ │ │ +
Author
Alex Cunningham
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,29 +1,27 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -FrobeniusFactor.cpp File Reference │ │ │ │ │ -Various factors that minimize some Frobenius norm. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +PoseTranslationPrior.h File Reference │ │ │ │ │ +Implements a prior on the translation component of a pose. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r_<_ _P_O_S_E_ _> │ │ │ │ │ +  A prior on the translation part of a pose. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l  _g_t_s_a_m_:_:_C_o_n_v_e_r_t_N_o_i_s_e_M_o_d_e_l (const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, │ │ │ │ │ - size_t n, bool defaultToUnit=true) │ │ │ │ │ - When creating (any) _F_r_o_b_e_n_i_u_s_F_a_c_t_o_r we can convert a Rot/Pose │ │ │ │ │ -  _B_e_t_w_e_e_n_F_a_c_t_o_r noise model into a n-dimensional isotropic │ │ │ │ │ - noise model used to weight the Frobenius norm. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Various factors that minimize some Frobenius norm. │ │ │ │ │ +Implements a prior on the translation component of a pose. │ │ │ │ │ Date │ │ │ │ │ - March 2019 │ │ │ │ │ + Jun 14, 2012 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Alex Cunningham │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _F_r_o_b_e_n_i_u_s_F_a_c_t_o_r_._c_p_p │ │ │ │ │ + * _P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01235.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a01235 = [ │ │ │ │ │ - ["ConvertNoiseModel", "a01235.html#aa43e7fd7c2b86873458a91ddafc506e4", null] │ │ │ │ │ + ["gtsam::PoseTranslationPrior< POSE >", "a04892.html", "a04892"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01241.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/RegularImplicitSchurFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/SmartProjectionFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,46 +96,49 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
RegularImplicitSchurFactor.h File Reference
│ │ │ │ +
SmartProjectionFactor.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

A subclass of GaussianFactor specialized to structureless SFM. │ │ │ │ +

Smart factor on cameras (pose + calibration) │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::RegularImplicitSchurFactor< CAMERA >
 RegularImplicitSchurFactor. More...
class  gtsam::SmartProjectionFactor< CAMERA >
 SmartProjectionFactor: triangulates point and keeps an estimate of it around. More...
 
struct  gtsam::traits< RegularImplicitSchurFactor< CAMERA > >
struct  gtsam::traits< SmartProjectionFactor< CAMERA > >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

A subclass of GaussianFactor specialized to structureless SFM.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +

Smart factor on cameras (pose + calibration)

│ │ │ │ +
Author
Luca Carlone
│ │ │ │
│ │ │ │ -Luca Carlone
│ │ │ │ +Zsolt Kira │ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,31 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -RegularImplicitSchurFactor.h File Reference │ │ │ │ │ -A subclass of GaussianFactor specialized to structureless SFM. _M_o_r_e_._._. │ │ │ │ │ +SmartProjectionFactor.h File Reference │ │ │ │ │ +Smart factor on cameras (pose + calibration) _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_<_ _C_A_M_E_R_A_ _> │ │ │ │ │ -  _R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_<_ _C_A_M_E_R_A_ _> │ │ │ │ │ +  _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r: triangulates point and keeps an estimate of it │ │ │ │ │ + around. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_<_ _C_A_M_E_R_A_ _>_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_<_ _C_A_M_E_R_A_ _>_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A subclass of GaussianFactor specialized to structureless SFM. │ │ │ │ │ +Smart factor on cameras (pose + calibration) │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ Luca Carlone │ │ │ │ │ + Zsolt Kira │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_._h │ │ │ │ │ + * _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01241.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,4 @@ │ │ │ │ │ var a01241 = [ │ │ │ │ │ - ["gtsam::RegularImplicitSchurFactor< CAMERA >", "a04912.html", "a04912"], │ │ │ │ │ - ["gtsam::traits< RegularImplicitSchurFactor< CAMERA > >", "a04916.html", null] │ │ │ │ │ + ["gtsam::SmartProjectionFactor< CAMERA >", "a04936.html", "a04936"], │ │ │ │ │ + ["gtsam::traits< SmartProjectionFactor< CAMERA > >", "a04940.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01241_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/RegularImplicitSchurFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/SmartProjectionFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,587 +98,539 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
RegularImplicitSchurFactor.h
│ │ │ │ +
SmartProjectionFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1
│ │ │ │ -
8#pragma once
│ │ │ │ -
9
│ │ │ │ -
10#include <gtsam/geometry/CameraSet.h>
│ │ │ │ -
11#include <gtsam/linear/JacobianFactor.h>
│ │ │ │ -
12#include <gtsam/linear/VectorValues.h>
│ │ │ │ -
13
│ │ │ │ -
14#include <iosfwd>
│ │ │ │ -
15#include <map>
│ │ │ │ -
16#include <string>
│ │ │ │ -
17#include <vector>
│ │ │ │ -
18
│ │ │ │ -
19namespace gtsam {
│ │ │ │ -
20
│ │ │ │ -
38template<class CAMERA>
│ │ │ │ -
│ │ │ │ -
39class RegularImplicitSchurFactor: public GaussianFactor {
│ │ │ │ -
40
│ │ │ │ -
41public:
│ │ │ │ -
42 typedef RegularImplicitSchurFactor This;
│ │ │ │ -
43 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
44
│ │ │ │ -
45protected:
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +
2
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
4 * Atlanta, Georgia 30332-0415
│ │ │ │ +
5 * All Rights Reserved
│ │ │ │ +
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
7
│ │ │ │ +
8 * See LICENSE for the license information
│ │ │ │ +
9
│ │ │ │ +
10 * -------------------------------------------------------------------------- */
│ │ │ │ +
11
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/slam/SmartFactorBase.h>
│ │ │ │ +
23#include <gtsam/slam/SmartFactorParams.h>
│ │ │ │ +
24
│ │ │ │ +
25#include <gtsam/geometry/triangulation.h>
│ │ │ │ +
26#include <gtsam/inference/Symbol.h>
│ │ │ │ +
27#include <gtsam/slam/dataset.h>
│ │ │ │ +
28
│ │ │ │ +
29#include <boost/optional.hpp>
│ │ │ │ +
30#include <boost/make_shared.hpp>
│ │ │ │ +
31#include <vector>
│ │ │ │ +
32
│ │ │ │ +
33namespace gtsam {
│ │ │ │ +
34
│ │ │ │ +
44template<class CAMERA>
│ │ │ │ +
│ │ │ │ +
45class SmartProjectionFactor: public SmartFactorBase<CAMERA> {
│ │ │ │
46
│ │ │ │ -
47 // This factor is closely related to a CameraSet
│ │ │ │ -
48 typedef CameraSet<CAMERA> Set;
│ │ │ │ -
49
│ │ │ │ -
50 typedef typename CAMERA::Measurement Z;
│ │ │ │ -
51 static const int D = traits<CAMERA>::dimension;
│ │ │ │ -
52 static const int ZDim = traits<Z>::dimension;
│ │ │ │ +
47public:
│ │ │ │ +
48
│ │ │ │ +
49private:
│ │ │ │ +
50 typedef SmartFactorBase<CAMERA> Base;
│ │ │ │ +
51 typedef SmartProjectionFactor<CAMERA> This;
│ │ │ │ +
52 typedef SmartProjectionFactor<CAMERA> SmartProjectionCameraFactor;
│ │ │ │
53
│ │ │ │ -
54 typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
│ │ │ │ -
55 typedef Eigen::Matrix<double, D, D> MatrixDD;
│ │ │ │ -
56 typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks;
│ │ │ │ -
57
│ │ │ │ -
58 FBlocks FBlocks_;
│ │ │ │ -
59 const Matrix PointCovariance_;
│ │ │ │ -
60 const Matrix E_;
│ │ │ │ -
61 const Vector b_;
│ │ │ │ -
62
│ │ │ │ -
63public:
│ │ │ │ -
64
│ │ │ │ -
│ │ │ │ -
66 RegularImplicitSchurFactor() {
│ │ │ │ -
67 }
│ │ │ │ -
│ │ │ │ -
68
│ │ │ │ -
70
│ │ │ │ -
│ │ │ │ -
80 RegularImplicitSchurFactor(const KeyVector& keys, const FBlocks& Fs,
│ │ │ │ -
81 const Matrix& E, const Matrix& P, const Vector& b)
│ │ │ │ -
82 : GaussianFactor(keys), FBlocks_(Fs), PointCovariance_(P), E_(E), b_(b) {}
│ │ │ │ -
│ │ │ │ -
83
│ │ │ │ -
│ │ │ │ -
85 ~RegularImplicitSchurFactor() override {
│ │ │ │ -
86 }
│ │ │ │ -
│ │ │ │ -
87
│ │ │ │ -
88 const FBlocks& Fs() const {
│ │ │ │ -
89 return FBlocks_;
│ │ │ │ -
90 }
│ │ │ │ -
91
│ │ │ │ -
92 const Matrix& E() const {
│ │ │ │ -
93 return E_;
│ │ │ │ -
94 }
│ │ │ │ -
95
│ │ │ │ -
96 const Vector& b() const {
│ │ │ │ -
97 return b_;
│ │ │ │ -
98 }
│ │ │ │ -
99
│ │ │ │ -
100 const Matrix& getPointCovariance() const {
│ │ │ │ -
101 return PointCovariance_;
│ │ │ │ -
102 }
│ │ │ │ -
103
│ │ │ │ -
│ │ │ │ -
105 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
│ │ │ │ -
106 DefaultKeyFormatter) const override {
│ │ │ │ -
107 std::cout << " RegularImplicitSchurFactor " << std::endl;
│ │ │ │ -
108 Factor::print(s);
│ │ │ │ -
109 for (size_t pos = 0; pos < size(); ++pos) {
│ │ │ │ -
110 std::cout << "Fblock:\n" << FBlocks_[pos] << std::endl;
│ │ │ │ -
111 }
│ │ │ │ -
112 std::cout << "PointCovariance:\n" << PointCovariance_ << std::endl;
│ │ │ │ -
113 std::cout << "E:\n" << E_ << std::endl;
│ │ │ │ -
114 std::cout << "b:\n" << b_.transpose() << std::endl;
│ │ │ │ -
115 }
│ │ │ │ -
│ │ │ │ -
116
│ │ │ │ -
│ │ │ │ -
118 bool equals(const GaussianFactor& lf, double tol) const override {
│ │ │ │ -
119 const This* f = dynamic_cast<const This*>(&lf);
│ │ │ │ -
120 if (!f)
│ │ │ │ -
121 return false;
│ │ │ │ -
122 for (size_t k = 0; k < FBlocks_.size(); ++k) {
│ │ │ │ -
123 if (keys_[k] != f->keys_[k])
│ │ │ │ -
124 return false;
│ │ │ │ -
125 if (!equal_with_abs_tol(FBlocks_[k], f->FBlocks_[k], tol))
│ │ │ │ -
126 return false;
│ │ │ │ -
127 }
│ │ │ │ -
128 return equal_with_abs_tol(PointCovariance_, f->PointCovariance_, tol)
│ │ │ │ -
129 && equal_with_abs_tol(E_, f->E_, tol)
│ │ │ │ -
130 && equal_with_abs_tol(b_, f->b_, tol);
│ │ │ │ -
131 }
│ │ │ │ -
│ │ │ │ -
132
│ │ │ │ -
│ │ │ │ -
134 DenseIndex getDim(const_iterator variable) const override {
│ │ │ │ -
135 return D;
│ │ │ │ -
136 }
│ │ │ │ -
│ │ │ │ -
137
│ │ │ │ -
│ │ │ │ -
138 void updateHessian(const KeyVector& keys,
│ │ │ │ -
139 SymmetricBlockMatrix* info) const override {
│ │ │ │ -
140 throw std::runtime_error(
│ │ │ │ -
141 "RegularImplicitSchurFactor::updateHessian non implemented");
│ │ │ │ -
142 }
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
143 Matrix augmentedJacobian() const override {
│ │ │ │ -
144 throw std::runtime_error(
│ │ │ │ -
145 "RegularImplicitSchurFactor::augmentedJacobian non implemented");
│ │ │ │ -
146 return Matrix();
│ │ │ │ -
147 }
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
148 std::pair<Matrix, Vector> jacobian() const override {
│ │ │ │ -
149 throw std::runtime_error(
│ │ │ │ -
150 "RegularImplicitSchurFactor::jacobian non implemented");
│ │ │ │ -
151 return std::make_pair(Matrix(), Vector());
│ │ │ │ -
152 }
│ │ │ │ -
│ │ │ │ -
153
│ │ │ │ -
│ │ │ │ -
155 Matrix augmentedInformation() const override {
│ │ │ │ -
156 // Do the Schur complement
│ │ │ │ -
157 SymmetricBlockMatrix augmentedHessian =
│ │ │ │ -
158 Set::SchurComplement(FBlocks_, E_, b_);
│ │ │ │ -
159 return augmentedHessian.selfadjointView();
│ │ │ │ -
160 }
│ │ │ │ -
│ │ │ │ -
161
│ │ │ │ -
│ │ │ │ -
163 Matrix information() const override {
│ │ │ │ -
164 Matrix augmented = augmentedInformation();
│ │ │ │ -
165 int m = this->keys_.size();
│ │ │ │ -
166 size_t M = D * m;
│ │ │ │ -
167 return augmented.block(0, 0, M, M);
│ │ │ │ -
168 }
│ │ │ │ -
│ │ │ │ -
169
│ │ │ │ -
171 using GaussianFactor::hessianDiagonal;
│ │ │ │ -
172
│ │ │ │ +
54protected:
│ │ │ │ +
55
│ │ │ │ +
58 SmartProjectionParams params_;
│ │ │ │ +
60
│ │ │ │ +
63 mutable TriangulationResult result_;
│ │ │ │ +
64 mutable std::vector<Pose3, Eigen::aligned_allocator<Pose3> >
│ │ │ │ +
65 cameraPosesTriangulation_;
│ │ │ │ +
67
│ │ │ │ +
68 public:
│ │ │ │ +
69
│ │ │ │ +
71 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
72
│ │ │ │ +
74 typedef CAMERA Camera;
│ │ │ │ +
75 typedef CameraSet<CAMERA> Cameras;
│ │ │ │ +
76
│ │ │ │ +
80 SmartProjectionFactor() {}
│ │ │ │ +
81
│ │ │ │ +
│ │ │ │ +
87 SmartProjectionFactor(
│ │ │ │ +
88 const SharedNoiseModel& sharedNoiseModel,
│ │ │ │ +
89 const SmartProjectionParams& params = SmartProjectionParams())
│ │ │ │ +
90 : Base(sharedNoiseModel),
│ │ │ │ +
91 params_(params),
│ │ │ │ +
92 result_(TriangulationResult::Degenerate()) {}
│ │ │ │ +
│ │ │ │ +
93
│ │ │ │ +
│ │ │ │ +
95 ~SmartProjectionFactor() override {
│ │ │ │ +
96 }
│ │ │ │ +
│ │ │ │ +
97
│ │ │ │ +
│ │ │ │ +
103 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
│ │ │ │ +
104 DefaultKeyFormatter) const override {
│ │ │ │ +
105 std::cout << s << "SmartProjectionFactor\n";
│ │ │ │ +
106 std::cout << "linearizationMode: " << params_.linearizationMode
│ │ │ │ +
107 << std::endl;
│ │ │ │ +
108 std::cout << "triangulationParameters:\n" << params_.triangulation
│ │ │ │ +
109 << std::endl;
│ │ │ │ +
110 std::cout << "result:\n" << result_ << std::endl;
│ │ │ │ +
111 Base::print("", keyFormatter);
│ │ │ │ +
112 }
│ │ │ │ +
│ │ │ │ +
113
│ │ │ │ +
│ │ │ │ +
115 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
│ │ │ │ +
116 const This *e = dynamic_cast<const This*>(&p);
│ │ │ │ +
117 return e && params_.linearizationMode == e->params_.linearizationMode
│ │ │ │ +
118 && Base::equals(p, tol);
│ │ │ │ +
119 }
│ │ │ │ +
│ │ │ │ +
120
│ │ │ │ +
│ │ │ │ +
128 bool decideIfTriangulate(const Cameras& cameras) const {
│ │ │ │ +
129 // Several calls to linearize will be done from the same linearization
│ │ │ │ +
130 // point, hence it is not needed to re-triangulate. Note that this is not
│ │ │ │ +
131 // yet "selecting linearization", that will come later, and we only check if
│ │ │ │ +
132 // the current linearization is the "same" (up to tolerance) w.r.t. the last
│ │ │ │ +
133 // time we triangulated the point.
│ │ │ │ +
134
│ │ │ │ +
135 size_t m = cameras.size();
│ │ │ │ +
136
│ │ │ │ +
137 bool retriangulate = false;
│ │ │ │ +
138
│ │ │ │ +
139 // Definitely true if we do not have a previous linearization point or the
│ │ │ │ +
140 // new linearization point includes more poses.
│ │ │ │ +
141 if (cameraPosesTriangulation_.empty()
│ │ │ │ +
142 || cameras.size() != cameraPosesTriangulation_.size())
│ │ │ │ +
143 retriangulate = true;
│ │ │ │ +
144
│ │ │ │ +
145 // Otherwise, check poses against cache.
│ │ │ │ +
146 if (!retriangulate) {
│ │ │ │ +
147 for (size_t i = 0; i < cameras.size(); i++) {
│ │ │ │ +
148 if (!cameras[i].pose().equals(cameraPosesTriangulation_[i],
│ │ │ │ +
149 params_.retriangulationThreshold)) {
│ │ │ │ +
150 retriangulate = true; // at least two poses are different, hence we retriangulate
│ │ │ │ +
151 break;
│ │ │ │ +
152 }
│ │ │ │ +
153 }
│ │ │ │ +
154 }
│ │ │ │ +
155
│ │ │ │ +
156 // Store the current poses used for triangulation if we will re-triangulate.
│ │ │ │ +
157 if (retriangulate) {
│ │ │ │ +
158 cameraPosesTriangulation_.clear();
│ │ │ │ +
159 cameraPosesTriangulation_.reserve(m);
│ │ │ │ +
160 for (size_t i = 0; i < m; i++)
│ │ │ │ +
161 // cameraPosesTriangulation_[i] = cameras[i].pose();
│ │ │ │ +
162 cameraPosesTriangulation_.push_back(cameras[i].pose());
│ │ │ │ +
163 }
│ │ │ │ +
164
│ │ │ │ +
165 return retriangulate;
│ │ │ │ +
166 }
│ │ │ │ +
│ │ │ │ +
167
│ │ │ │
│ │ │ │ -
174 void hessianDiagonalAdd(VectorValues &d) const override {
│ │ │ │ -
175 // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
│ │ │ │ -
176 for (size_t k = 0; k < size(); ++k) { // for each camera
│ │ │ │ -
177 Key j = keys_[k];
│ │ │ │ -
178
│ │ │ │ -
179 // Calculate Fj'*Ej for the current camera (observing a single point)
│ │ │ │ -
180 // D x 3 = (D x ZDim) * (ZDim x 3)
│ │ │ │ -
181 const MatrixZD& Fj = FBlocks_[k];
│ │ │ │ -
182 Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
│ │ │ │ -
183 * E_.block<ZDim, 3>(ZDim * k, 0);
│ │ │ │ -
184
│ │ │ │ -
185 Eigen::Matrix<double, D, 1> dj;
│ │ │ │ -
186 for (int k = 0; k < D; ++k) { // for each diagonal element of the camera hessian
│ │ │ │ -
187 // Vector column_k_Fj = Fj.col(k);
│ │ │ │ -
188 dj(k) = Fj.col(k).squaredNorm(); // dot(column_k_Fj, column_k_Fj);
│ │ │ │ -
189 // Vector column_k_FtE = FtE.row(k);
│ │ │ │ -
190 // (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1)
│ │ │ │ -
191 dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose();
│ │ │ │ -
192 }
│ │ │ │ -
193
│ │ │ │ -
194 auto result = d.emplace(j, dj);
│ │ │ │ -
195 if(!result.second) {
│ │ │ │ -
196 result.first->second += dj;
│ │ │ │ -
197 }
│ │ │ │ -
198 }
│ │ │ │ -
199 }
│ │ │ │ -
│ │ │ │ -
200
│ │ │ │ -
│ │ │ │ -
205 void hessianDiagonal(double* d) const override {
│ │ │ │ -
206 // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
│ │ │ │ -
207 // Use eigen magic to access raw memory
│ │ │ │ -
208 typedef Eigen::Matrix<double, D, 1> DVector;
│ │ │ │ -
209 typedef Eigen::Map<DVector> DMap;
│ │ │ │ -
210
│ │ │ │ -
211 for (size_t pos = 0; pos < size(); ++pos) { // for each camera in the factor
│ │ │ │ -
212 Key j = keys_[pos];
│ │ │ │ -
213
│ │ │ │ -
214 // Calculate Fj'*Ej for the current camera (observing a single point)
│ │ │ │ -
215 // D x 3 = (D x ZDim) * (ZDim x 3)
│ │ │ │ -
216 const MatrixZD& Fj = FBlocks_[pos];
│ │ │ │ -
217 Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
│ │ │ │ -
218 * E_.block<ZDim, 3>(ZDim * pos, 0);
│ │ │ │ -
219
│ │ │ │ -
220 DVector dj;
│ │ │ │ -
221 for (int k = 0; k < D; ++k) { // for each diagonal element of the camera hessian
│ │ │ │ -
222 dj(k) = Fj.col(k).squaredNorm();
│ │ │ │ -
223 // (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1)
│ │ │ │ -
224 dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose();
│ │ │ │ -
225 }
│ │ │ │ -
226 DMap(d + D * j) += dj;
│ │ │ │ -
227 }
│ │ │ │ -
228 }
│ │ │ │ -
│ │ │ │ -
229
│ │ │ │ -
│ │ │ │ -
231 std::map<Key, Matrix> hessianBlockDiagonal() const override {
│ │ │ │ -
232 std::map<Key, Matrix> blocks;
│ │ │ │ -
233 // F'*(I - E*P*E')*F
│ │ │ │ -
234 for (size_t pos = 0; pos < size(); ++pos) {
│ │ │ │ -
235 Key j = keys_[pos];
│ │ │ │ -
236 // F'*F - F'*E*P*E'*F e.g. (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9)
│ │ │ │ -
237 const MatrixZD& Fj = FBlocks_[pos];
│ │ │ │ -
238 // Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
│ │ │ │ -
239 // * E_.block<ZDim, 3>(ZDim * pos, 0);
│ │ │ │ -
240 // blocks[j] = Fj.transpose() * Fj
│ │ │ │ -
241 // - FtE * PointCovariance_ * FtE.transpose();
│ │ │ │ -
242
│ │ │ │ -
243 const Matrix23& Ej = E_.block<ZDim, 3>(ZDim * pos, 0);
│ │ │ │ -
244 blocks[j] = Fj.transpose()
│ │ │ │ -
245 * (Fj - Ej * PointCovariance_ * Ej.transpose() * Fj);
│ │ │ │ -
246
│ │ │ │ -
247 // F'*(I - E*P*E')*F, TODO: this should work, but it does not :-(
│ │ │ │ -
248 // static const Eigen::Matrix<double, ZDim, ZDim> I2 = eye(ZDim);
│ │ │ │ -
249 // Matrix2 Q = //
│ │ │ │ -
250 // I2 - E_.block<ZDim, 3>(ZDim * pos, 0) * PointCovariance_ * E_.block<ZDim, 3>(ZDim * pos, 0).transpose();
│ │ │ │ -
251 // blocks[j] = Fj.transpose() * Q * Fj;
│ │ │ │ -
252 }
│ │ │ │ -
253 return blocks;
│ │ │ │ -
254 }
│ │ │ │ -
│ │ │ │ -
255
│ │ │ │ -
│ │ │ │ -
256 GaussianFactor::shared_ptr clone() const override {
│ │ │ │ -
257 return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
│ │ │ │ -
258 FBlocks_, PointCovariance_, E_, b_);
│ │ │ │ -
259 throw std::runtime_error(
│ │ │ │ -
260 "RegularImplicitSchurFactor::clone non implemented");
│ │ │ │ -
261 }
│ │ │ │ -
│ │ │ │ -
262
│ │ │ │ -
│ │ │ │ -
263 GaussianFactor::shared_ptr negate() const override {
│ │ │ │ -
264 return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
│ │ │ │ -
265 FBlocks_, PointCovariance_, E_, b_);
│ │ │ │ -
266 throw std::runtime_error(
│ │ │ │ -
267 "RegularImplicitSchurFactor::negate non implemented");
│ │ │ │ -
268 }
│ │ │ │ -
│ │ │ │ -
269
│ │ │ │ -
270 // Raw Vector version of y += F'*alpha*(I - E*P*E')*F*x, for testing
│ │ │ │ -
271 static
│ │ │ │ -
272 void multiplyHessianAdd(const Matrix& F, const Matrix& E,
│ │ │ │ -
273 const Matrix& PointCovariance, double alpha, const Vector& x, Vector& y) {
│ │ │ │ -
274 Vector e1 = F * x;
│ │ │ │ -
275 Vector d1 = E.transpose() * e1;
│ │ │ │ -
276 Vector d2 = PointCovariance * d1;
│ │ │ │ -
277 Vector e2 = E * d2;
│ │ │ │ -
278 Vector e3 = alpha * (e1 - e2);
│ │ │ │ -
279 y += F.transpose() * e3;
│ │ │ │ +
174 TriangulationResult triangulateSafe(const Cameras& cameras) const {
│ │ │ │ +
175
│ │ │ │ +
176 size_t m = cameras.size();
│ │ │ │ +
177 if (m < 2) // if we have a single pose the corresponding factor is uninformative
│ │ │ │ +
178 return TriangulationResult::Degenerate();
│ │ │ │ +
179
│ │ │ │ +
180 bool retriangulate = decideIfTriangulate(cameras);
│ │ │ │ +
181 if (retriangulate)
│ │ │ │ +
182 result_ = gtsam::triangulateSafe(cameras, this->measured_,
│ │ │ │ +
183 params_.triangulation);
│ │ │ │ +
184 return result_;
│ │ │ │ +
185 }
│ │ │ │ +
│ │ │ │ +
186
│ │ │ │ +
│ │ │ │ +
193 bool triangulateForLinearize(const Cameras& cameras) const {
│ │ │ │ +
194 triangulateSafe(cameras); // imperative, might reset result_
│ │ │ │ +
195 return bool(result_);
│ │ │ │ +
196 }
│ │ │ │ +
│ │ │ │ +
197
│ │ │ │ +
│ │ │ │ +
199 boost::shared_ptr<RegularHessianFactor<Base::Dim> > createHessianFactor(
│ │ │ │ +
200 const Cameras& cameras, const double lambda = 0.0,
│ │ │ │ +
201 bool diagonalDamping = false) const {
│ │ │ │ +
202 size_t numKeys = this->keys_.size();
│ │ │ │ +
203 // Create structures for Hessian Factors
│ │ │ │ +
204 KeyVector js;
│ │ │ │ +
205 std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
│ │ │ │ +
206 std::vector<Vector> gs(numKeys);
│ │ │ │ +
207
│ │ │ │ +
208 if (this->measured_.size() != cameras.size())
│ │ │ │ +
209 throw std::runtime_error(
│ │ │ │ +
210 "SmartProjectionHessianFactor: this->measured_"
│ │ │ │ +
211 ".size() inconsistent with input");
│ │ │ │ +
212
│ │ │ │ +
213 triangulateSafe(cameras);
│ │ │ │ +
214
│ │ │ │ +
215 if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
│ │ │ │ +
216 // failed: return"empty" Hessian
│ │ │ │ +
217 for (Matrix& m : Gs) m = Matrix::Zero(Base::Dim, Base::Dim);
│ │ │ │ +
218 for (Vector& v : gs) v = Vector::Zero(Base::Dim);
│ │ │ │ +
219 return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
│ │ │ │ +
220 Gs, gs, 0.0);
│ │ │ │ +
221 }
│ │ │ │ +
222
│ │ │ │ +
223 // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
│ │ │ │ +
224 typename Base::FBlocks Fs;
│ │ │ │ +
225 Matrix E;
│ │ │ │ +
226 Vector b;
│ │ │ │ +
227 computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
│ │ │ │ +
228
│ │ │ │ +
229 // Whiten using noise model
│ │ │ │ +
230 Base::whitenJacobians(Fs, E, b);
│ │ │ │ +
231
│ │ │ │ +
232 // build augmented hessian
│ │ │ │ +
233 SymmetricBlockMatrix augmentedHessian = //
│ │ │ │ +
234 Cameras::SchurComplement(Fs, E, b, lambda, diagonalDamping);
│ │ │ │ +
235
│ │ │ │ +
236 return boost::make_shared<RegularHessianFactor<Base::Dim> >(
│ │ │ │ +
237 this->keys_, augmentedHessian);
│ │ │ │ +
238 }
│ │ │ │ +
│ │ │ │ +
239
│ │ │ │ +
240 // Create RegularImplicitSchurFactor factor.
│ │ │ │ +
241 boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > createRegularImplicitSchurFactor(
│ │ │ │ +
242 const Cameras& cameras, double lambda) const {
│ │ │ │ +
243 if (triangulateForLinearize(cameras))
│ │ │ │ +
244 return Base::createRegularImplicitSchurFactor(cameras, *result_, lambda);
│ │ │ │ +
245 else
│ │ │ │ +
246 // failed: return empty
│ │ │ │ +
247 return boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> >();
│ │ │ │ +
248 }
│ │ │ │ +
249
│ │ │ │ +
│ │ │ │ +
251 boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor(
│ │ │ │ +
252 const Cameras& cameras, double lambda) const {
│ │ │ │ +
253 if (triangulateForLinearize(cameras))
│ │ │ │ +
254 return Base::createJacobianQFactor(cameras, *result_, lambda);
│ │ │ │ +
255 else
│ │ │ │ +
256 // failed: return empty
│ │ │ │ +
257 return boost::make_shared<JacobianFactorQ<Base::Dim, 2> >(this->keys_);
│ │ │ │ +
258 }
│ │ │ │ +
│ │ │ │ +
259
│ │ │ │ +
│ │ │ │ +
261 boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor(
│ │ │ │ +
262 const Values& values, double lambda) const {
│ │ │ │ +
263 return createJacobianQFactor(this->cameras(values), lambda);
│ │ │ │ +
264 }
│ │ │ │ +
│ │ │ │ +
265
│ │ │ │ +
│ │ │ │ +
267 boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
│ │ │ │ +
268 const Cameras& cameras, double lambda) const {
│ │ │ │ +
269 if (triangulateForLinearize(cameras))
│ │ │ │ +
270 return Base::createJacobianSVDFactor(cameras, *result_, lambda);
│ │ │ │ +
271 else
│ │ │ │ +
272 // failed: return empty
│ │ │ │ +
273 return boost::make_shared<JacobianFactorSVD<Base::Dim, 2> >(this->keys_);
│ │ │ │ +
274 }
│ │ │ │ +
│ │ │ │ +
275
│ │ │ │ +
│ │ │ │ +
277 virtual boost::shared_ptr<RegularHessianFactor<Base::Dim> > linearizeToHessian(
│ │ │ │ +
278 const Values& values, double lambda = 0.0) const {
│ │ │ │ +
279 return createHessianFactor(this->cameras(values), lambda);
│ │ │ │
280 }
│ │ │ │ +
│ │ │ │
281
│ │ │ │ -
282 typedef std::vector<Vector2, Eigen::aligned_allocator<Vector2>> Error2s;
│ │ │ │ -
283
│ │ │ │ -
│ │ │ │ -
287 void projectError2(const Error2s& e1, Error2s& e2) const {
│ │ │ │ -
288
│ │ │ │ -
289 // d1 = E.transpose() * (e1-ZDim*b) = (3*2m)*2m
│ │ │ │ -
290 Vector3 d1;
│ │ │ │ -
291 d1.setZero();
│ │ │ │ -
292 for (size_t k = 0; k < size(); k++)
│ │ │ │ -
293 d1 += E_.block<ZDim, 3>(ZDim * k, 0).transpose()
│ │ │ │ -
294 * (e1[k] - ZDim * b_.segment<ZDim>(k * ZDim));
│ │ │ │ -
295
│ │ │ │ -
296 // d2 = E.transpose() * e1 = (3*2m)*2m
│ │ │ │ -
297 Vector3 d2 = PointCovariance_ * d1;
│ │ │ │ -
298
│ │ │ │ -
299 // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
│ │ │ │ -
300 for (size_t k = 0; k < size(); k++)
│ │ │ │ -
301 e2[k] = e1[k] - ZDim * b_.segment<ZDim>(k * ZDim)
│ │ │ │ -
302 - E_.block<ZDim, 3>(ZDim * k, 0) * d2;
│ │ │ │ -
303 }
│ │ │ │ -
│ │ │ │ -
304
│ │ │ │ -
305 /*
│ │ │ │ -
306 * This definition matches the linearized error in the Hessian Factor:
│ │ │ │ -
307 * LinError(x) = x'*H*x - 2*x'*eta + f
│ │ │ │ -
308 * with:
│ │ │ │ -
309 * H = F' * (I-E'*P*E) * F = F' * Q * F
│ │ │ │ -
310 * eta = F' * (I-E'*P*E) * b = F' * Q * b
│ │ │ │ -
311 * f = nonlinear error
│ │ │ │ -
312 * (x'*H*x - 2*x'*eta + f) = x'*F'*Q*F*x - 2*x'*F'*Q *b + f = x'*F'*Q*(F*x - 2*b) + f
│ │ │ │ -
313 */
│ │ │ │ -
314 double error(const VectorValues& x) const override {
│ │ │ │ +
│ │ │ │ +
283 virtual boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > linearizeToImplicit(
│ │ │ │ +
284 const Values& values, double lambda = 0.0) const {
│ │ │ │ +
285 return createRegularImplicitSchurFactor(this->cameras(values), lambda);
│ │ │ │ +
286 }
│ │ │ │ +
│ │ │ │ +
287
│ │ │ │ +
│ │ │ │ +
289 virtual boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > linearizeToJacobian(
│ │ │ │ +
290 const Values& values, double lambda = 0.0) const {
│ │ │ │ +
291 return createJacobianQFactor(this->cameras(values), lambda);
│ │ │ │ +
292 }
│ │ │ │ +
│ │ │ │ +
293
│ │ │ │ +
│ │ │ │ +
299 boost::shared_ptr<GaussianFactor> linearizeDamped(const Cameras& cameras,
│ │ │ │ +
300 const double lambda = 0.0) const {
│ │ │ │ +
301 // depending on flag set on construction we may linearize to different linear factors
│ │ │ │ +
302 switch (params_.linearizationMode) {
│ │ │ │ +
303 case HESSIAN:
│ │ │ │ +
304 return createHessianFactor(cameras, lambda);
│ │ │ │ +
305 case IMPLICIT_SCHUR:
│ │ │ │ +
306 return createRegularImplicitSchurFactor(cameras, lambda);
│ │ │ │ +
307 case JACOBIAN_SVD:
│ │ │ │ +
308 return createJacobianSVDFactor(cameras, lambda);
│ │ │ │ +
309 case JACOBIAN_Q:
│ │ │ │ +
310 return createJacobianQFactor(cameras, lambda);
│ │ │ │ +
311 default:
│ │ │ │ +
312 throw std::runtime_error("SmartFactorlinearize: unknown mode");
│ │ │ │ +
313 }
│ │ │ │ +
314 }
│ │ │ │ +
│ │ │ │
315
│ │ │ │ -
316 // resize does not do malloc if correct size
│ │ │ │ -
317 e1.resize(size());
│ │ │ │ -
318 e2.resize(size());
│ │ │ │ -
319
│ │ │ │ -
320 // e1 = F * x - b = (2m*dm)*dm
│ │ │ │ -
321 for (size_t k = 0; k < size(); ++k)
│ │ │ │ -
322 e1[k] = FBlocks_[k] * x.at(keys_[k]);
│ │ │ │ -
323 projectError2(e1, e2);
│ │ │ │ -
324
│ │ │ │ -
325 double result = 0;
│ │ │ │ -
326 for (size_t k = 0; k < size(); ++k)
│ │ │ │ -
327 result += dot(e1[k], e2[k]);
│ │ │ │ -
328
│ │ │ │ -
329 double f = b_.squaredNorm();
│ │ │ │ -
330 return 0.5 * (result + f);
│ │ │ │ -
331 }
│ │ │ │ -
332
│ │ │ │ -
333 // needed to be GaussianFactor - (I - E*P*E')*(F*x - b)
│ │ │ │ -
334 // This is wrong and does not match the definition in Hessian,
│ │ │ │ -
335 // but it matches the definition of the Jacobian factor (JF)
│ │ │ │ -
336 double errorJF(const VectorValues& x) const {
│ │ │ │ -
337
│ │ │ │ -
338 // resize does not do malloc if correct size
│ │ │ │ -
339 e1.resize(size());
│ │ │ │ -
340 e2.resize(size());
│ │ │ │ -
341
│ │ │ │ -
342 // e1 = F * x - b = (2m*dm)*dm
│ │ │ │ -
343 for (size_t k = 0; k < size(); ++k)
│ │ │ │ -
344 e1[k] = FBlocks_[k] * x.at(keys_[k]) - b_.segment<ZDim>(k * ZDim);
│ │ │ │ -
345 projectError(e1, e2);
│ │ │ │ -
346
│ │ │ │ -
347 double result = 0;
│ │ │ │ -
348 for (size_t k = 0; k < size(); ++k)
│ │ │ │ -
349 result += dot(e2[k], e2[k]);
│ │ │ │ -
350
│ │ │ │ -
351 // std::cout << "implicitFactor::error result " << result << std::endl;
│ │ │ │ -
352 return 0.5 * result;
│ │ │ │ -
353 }
│ │ │ │ +
│ │ │ │ +
321 boost::shared_ptr<GaussianFactor> linearizeDamped(const Values& values,
│ │ │ │ +
322 const double lambda = 0.0) const {
│ │ │ │ +
323 // depending on flag set on construction we may linearize to different linear factors
│ │ │ │ +
324 Cameras cameras = this->cameras(values);
│ │ │ │ +
325 return linearizeDamped(cameras, lambda);
│ │ │ │ +
326 }
│ │ │ │ +
│ │ │ │ +
327
│ │ │ │ +
│ │ │ │ +
329 boost::shared_ptr<GaussianFactor> linearize(
│ │ │ │ +
330 const Values& values) const override {
│ │ │ │ +
331 return linearizeDamped(values);
│ │ │ │ +
332 }
│ │ │ │ +
│ │ │ │ +
333
│ │ │ │ +
│ │ │ │ +
338 bool triangulateAndComputeE(Matrix& E, const Cameras& cameras) const {
│ │ │ │ +
339 bool nonDegenerate = triangulateForLinearize(cameras);
│ │ │ │ +
340 if (nonDegenerate)
│ │ │ │ +
341 cameras.project2(*result_, boost::none, E);
│ │ │ │ +
342 return nonDegenerate;
│ │ │ │ +
343 }
│ │ │ │ +
│ │ │ │ +
344
│ │ │ │ +
│ │ │ │ +
349 bool triangulateAndComputeE(Matrix& E, const Values& values) const {
│ │ │ │ +
350 Cameras cameras = this->cameras(values);
│ │ │ │ +
351 return triangulateAndComputeE(E, cameras);
│ │ │ │ +
352 }
│ │ │ │ +
│ │ │ │ +
353
│ │ │ │
│ │ │ │ -
357 void projectError(const Error2s& e1, Error2s& e2) const {
│ │ │ │ -
358
│ │ │ │ -
359 // d1 = E.transpose() * e1 = (3*2m)*2m
│ │ │ │ -
360 Vector3 d1;
│ │ │ │ -
361 d1.setZero();
│ │ │ │ -
362 for (size_t k = 0; k < size(); k++)
│ │ │ │ -
363 d1 += E_.block<ZDim, 3>(ZDim * k, 0).transpose() * e1[k];
│ │ │ │ -
364
│ │ │ │ -
365 // d2 = E.transpose() * e1 = (3*2m)*2m
│ │ │ │ -
366 Vector3 d2 = PointCovariance_ * d1;
│ │ │ │ -
367
│ │ │ │ -
368 // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
│ │ │ │ -
369 for (size_t k = 0; k < size(); k++)
│ │ │ │ -
370 e2[k] = e1[k] - E_.block<ZDim, 3>(ZDim * k, 0) * d2;
│ │ │ │ +
357 void computeJacobiansWithTriangulatedPoint(
│ │ │ │ +
358 typename Base::FBlocks& Fs, Matrix& E, Vector& b,
│ │ │ │ +
359 const Cameras& cameras) const {
│ │ │ │ +
360
│ │ │ │ +
361 if (!result_) {
│ │ │ │ +
362 // Handle degeneracy
│ │ │ │ +
363 // TODO check flag whether we should do this
│ │ │ │ +
364 Unit3 backProjected = cameras[0].backprojectPointAtInfinity(
│ │ │ │ +
365 this->measured_.at(0));
│ │ │ │ +
366 Base::computeJacobians(Fs, E, b, cameras, backProjected);
│ │ │ │ +
367 } else {
│ │ │ │ +
368 // valid result: just return Base version
│ │ │ │ +
369 Base::computeJacobians(Fs, E, b, cameras, *result_);
│ │ │ │ +
370 }
│ │ │ │
371 }
│ │ │ │
│ │ │ │
372
│ │ │ │ -
374 mutable Error2s e1, e2;
│ │ │ │ -
375
│ │ │ │ -
│ │ │ │ -
380 void multiplyHessianAdd(double alpha, const double* x, double* y) const {
│ │ │ │ -
381
│ │ │ │ -
382 // Use eigen magic to access raw memory
│ │ │ │ -
383 typedef Eigen::Matrix<double, D, 1> DVector;
│ │ │ │ -
384 typedef Eigen::Map<DVector> DMap;
│ │ │ │ -
385 typedef Eigen::Map<const DVector> ConstDMap;
│ │ │ │ -
386
│ │ │ │ -
387 // resize does not do malloc if correct size
│ │ │ │ -
388 e1.resize(size());
│ │ │ │ -
389 e2.resize(size());
│ │ │ │ -
390
│ │ │ │ -
391 // e1 = F * x = (2m*dm)*dm
│ │ │ │ -
392 for (size_t k = 0; k < size(); ++k) {
│ │ │ │ -
393 Key key = keys_[k];
│ │ │ │ -
394 e1[k] = FBlocks_[k] * ConstDMap(x + D * key);
│ │ │ │ -
395 }
│ │ │ │ -
396
│ │ │ │ -
397 projectError(e1, e2);
│ │ │ │ -
398
│ │ │ │ -
399 // y += F.transpose()*e2 = (2d*2m)*2m
│ │ │ │ -
400 for (size_t k = 0; k < size(); ++k) {
│ │ │ │ -
401 Key key = keys_[k];
│ │ │ │ -
402 DMap(y + D * key) += FBlocks_[k].transpose() * alpha * e2[k];
│ │ │ │ -
403 }
│ │ │ │ -
404 }
│ │ │ │ -
│ │ │ │ -
405
│ │ │ │ -
406 void multiplyHessianAdd(double alpha, const double* x, double* y,
│ │ │ │ -
407 std::vector<size_t> keys) const {
│ │ │ │ -
408 }
│ │ │ │ -
409
│ │ │ │ -
│ │ │ │ -
413 void multiplyHessianAdd(double alpha, const VectorValues& x,
│ │ │ │ -
414 VectorValues& y) const override {
│ │ │ │ -
415
│ │ │ │ -
416 // resize does not do malloc if correct size
│ │ │ │ -
417 e1.resize(size());
│ │ │ │ -
418 e2.resize(size());
│ │ │ │ -
419
│ │ │ │ -
420 // e1 = F * x = (2m*dm)*dm
│ │ │ │ -
421 for (size_t k = 0; k < size(); ++k)
│ │ │ │ -
422 e1[k] = FBlocks_[k] * x.at(keys_[k]);
│ │ │ │ -
423
│ │ │ │ -
424 projectError(e1, e2);
│ │ │ │ -
425
│ │ │ │ -
426 // y += F.transpose()*e2 = (2d*2m)*2m
│ │ │ │ -
427 for (size_t k = 0; k < size(); ++k) {
│ │ │ │ -
428 Key key = keys_[k];
│ │ │ │ -
429 static const Vector empty;
│ │ │ │ -
430 std::pair<VectorValues::iterator, bool> it = y.tryInsert(key, empty);
│ │ │ │ -
431 Vector& yi = it.first->second;
│ │ │ │ -
432 // Create the value as a zero vector if it does not exist.
│ │ │ │ -
433 if (it.second)
│ │ │ │ -
434 yi = Vector::Zero(FBlocks_[k].cols());
│ │ │ │ -
435 yi += FBlocks_[k].transpose() * alpha * e2[k];
│ │ │ │ -
436 }
│ │ │ │ -
437 }
│ │ │ │ +
│ │ │ │ +
374 bool triangulateAndComputeJacobians(
│ │ │ │ +
375 typename Base::FBlocks& Fs, Matrix& E, Vector& b,
│ │ │ │ +
376 const Values& values) const {
│ │ │ │ +
377 Cameras cameras = this->cameras(values);
│ │ │ │ +
378 bool nonDegenerate = triangulateForLinearize(cameras);
│ │ │ │ +
379 if (nonDegenerate)
│ │ │ │ +
380 computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
│ │ │ │ +
381 return nonDegenerate;
│ │ │ │ +
382 }
│ │ │ │ +
│ │ │ │ +
383
│ │ │ │ +
│ │ │ │ +
385 bool triangulateAndComputeJacobiansSVD(
│ │ │ │ +
386 typename Base::FBlocks& Fs, Matrix& Enull, Vector& b,
│ │ │ │ +
387 const Values& values) const {
│ │ │ │ +
388 Cameras cameras = this->cameras(values);
│ │ │ │ +
389 bool nonDegenerate = triangulateForLinearize(cameras);
│ │ │ │ +
390 if (nonDegenerate)
│ │ │ │ +
391 Base::computeJacobiansSVD(Fs, Enull, b, cameras, *result_);
│ │ │ │ +
392 return nonDegenerate;
│ │ │ │ +
393 }
│ │ │ │ +
│ │ │ │ +
394
│ │ │ │ +
│ │ │ │ +
396 Vector reprojectionErrorAfterTriangulation(const Values& values) const {
│ │ │ │ +
397 Cameras cameras = this->cameras(values);
│ │ │ │ +
398 bool nonDegenerate = triangulateForLinearize(cameras);
│ │ │ │ +
399 if (nonDegenerate)
│ │ │ │ +
400 return Base::unwhitenedError(cameras, *result_);
│ │ │ │ +
401 else
│ │ │ │ +
402 return Vector::Zero(cameras.size() * 2);
│ │ │ │ +
403 }
│ │ │ │ +
│ │ │ │ +
404
│ │ │ │ +
│ │ │ │ +
411 double totalReprojectionError(const Cameras& cameras,
│ │ │ │ +
412 boost::optional<Point3> externalPoint = boost::none) const {
│ │ │ │ +
413
│ │ │ │ +
414 if (externalPoint)
│ │ │ │ +
415 result_ = TriangulationResult(*externalPoint);
│ │ │ │ +
416 else
│ │ │ │ +
417 result_ = triangulateSafe(cameras);
│ │ │ │ +
418
│ │ │ │ +
419 if (result_)
│ │ │ │ +
420 // All good, just use version in base class
│ │ │ │ +
421 return Base::totalReprojectionError(cameras, *result_);
│ │ │ │ +
422 else if (params_.degeneracyMode == HANDLE_INFINITY) {
│ │ │ │ +
423 // Otherwise, manage the exceptions with rotation-only factors
│ │ │ │ +
424 Unit3 backprojected = cameras.front().backprojectPointAtInfinity(
│ │ │ │ +
425 this->measured_.at(0));
│ │ │ │ +
426 return Base::totalReprojectionError(cameras, backprojected);
│ │ │ │ +
427 } else
│ │ │ │ +
428 // if we don't want to manage the exceptions we discard the factor
│ │ │ │ +
429 return 0.0;
│ │ │ │ +
430 }
│ │ │ │ +
│ │ │ │ +
431
│ │ │ │ +
│ │ │ │ +
433 double error(const Values& values) const override {
│ │ │ │ +
434 if (this->active(values)) {
│ │ │ │ +
435 return totalReprojectionError(Base::cameras(values));
│ │ │ │ +
436 } else { // else of active flag
│ │ │ │ +
437 return 0.0;
│ │ │ │ +
438 }
│ │ │ │ +
439 }
│ │ │ │
│ │ │ │ -
438
│ │ │ │ +
440
│ │ │ │
│ │ │ │ -
442 void multiplyHessianDummy(double alpha, const VectorValues& x,
│ │ │ │ -
443 VectorValues& y) const {
│ │ │ │ -
444
│ │ │ │ -
445 for (size_t k = 0; k < size(); ++k) {
│ │ │ │ -
446 static const Vector empty;
│ │ │ │ -
447 Key key = keys_[k];
│ │ │ │ -
448 std::pair<VectorValues::iterator, bool> it = y.tryInsert(key, empty);
│ │ │ │ -
449 Vector& yi = it.first->second;
│ │ │ │ -
450 yi = x.at(key);
│ │ │ │ -
451 }
│ │ │ │ -
452 }
│ │ │ │ -
│ │ │ │ -
453
│ │ │ │ -
│ │ │ │ -
457 VectorValues gradientAtZero() const override {
│ │ │ │ -
458 // calculate Q*b
│ │ │ │ -
459 e1.resize(size());
│ │ │ │ -
460 e2.resize(size());
│ │ │ │ -
461 for (size_t k = 0; k < size(); k++)
│ │ │ │ -
462 e1[k] = b_.segment<ZDim>(ZDim * k);
│ │ │ │ -
463 projectError(e1, e2);
│ │ │ │ -
464
│ │ │ │ -
465 // g = F.transpose()*e2
│ │ │ │ -
466 VectorValues g;
│ │ │ │ -
467 for (size_t k = 0; k < size(); ++k) {
│ │ │ │ -
468 Key key = keys_[k];
│ │ │ │ -
469 g.insert(key, -FBlocks_[k].transpose() * e2[k]);
│ │ │ │ -
470 }
│ │ │ │ -
471
│ │ │ │ -
472 // return it
│ │ │ │ -
473 return g;
│ │ │ │ -
474 }
│ │ │ │ -
│ │ │ │ -
475
│ │ │ │ -
│ │ │ │ -
479 void gradientAtZero(double* d) const override {
│ │ │ │ +
442 TriangulationResult point() const {
│ │ │ │ +
443 return result_;
│ │ │ │ +
444 }
│ │ │ │ +
│ │ │ │ +
445
│ │ │ │ +
│ │ │ │ +
447 TriangulationResult point(const Values& values) const {
│ │ │ │ +
448 Cameras cameras = this->cameras(values);
│ │ │ │ +
449 return triangulateSafe(cameras);
│ │ │ │ +
450 }
│ │ │ │ +
│ │ │ │ +
451
│ │ │ │ +
453 bool isValid() const { return result_.valid(); }
│ │ │ │ +
454
│ │ │ │ +
456 bool isDegenerate() const { return result_.degenerate(); }
│ │ │ │ +
457
│ │ │ │ +
459 bool isPointBehindCamera() const { return result_.behindCamera(); }
│ │ │ │ +
460
│ │ │ │ +
462 bool isOutlier() const { return result_.outlier(); }
│ │ │ │ +
463
│ │ │ │ +
465 bool isFarPoint() const { return result_.farPoint(); }
│ │ │ │ +
466
│ │ │ │ +
467 private:
│ │ │ │ +
468
│ │ │ │ +
470 friend class boost::serialization::access;
│ │ │ │ +
471 template<class ARCHIVE>
│ │ │ │ +
472 void serialize(ARCHIVE & ar, const unsigned int version) {
│ │ │ │ +
473 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
474 ar & BOOST_SERIALIZATION_NVP(params_);
│ │ │ │ +
475 ar & BOOST_SERIALIZATION_NVP(result_);
│ │ │ │ +
476 ar & BOOST_SERIALIZATION_NVP(cameraPosesTriangulation_);
│ │ │ │ +
477 }
│ │ │ │ +
478}
│ │ │ │ +
│ │ │ │ +
479;
│ │ │ │
480
│ │ │ │ -
481 // Use eigen magic to access raw memory
│ │ │ │ -
482 typedef Eigen::Matrix<double, D, 1> DVector;
│ │ │ │ -
483 typedef Eigen::Map<DVector> DMap;
│ │ │ │ -
484
│ │ │ │ -
485 // calculate Q*b
│ │ │ │ -
486 e1.resize(size());
│ │ │ │ -
487 e2.resize(size());
│ │ │ │ -
488 for (size_t k = 0; k < size(); k++)
│ │ │ │ -
489 e1[k] = b_.segment<ZDim>(ZDim * k);
│ │ │ │ -
490 projectError(e1, e2);
│ │ │ │ -
491
│ │ │ │ -
492 for (size_t k = 0; k < size(); ++k) { // for each camera in the factor
│ │ │ │ -
493 Key j = keys_[k];
│ │ │ │ -
494 DMap(d + D * j) += -FBlocks_[k].transpose() * e2[k];
│ │ │ │ -
495 }
│ │ │ │ -
496 }
│ │ │ │ -
│ │ │ │ -
497
│ │ │ │ -
│ │ │ │ -
499 Vector gradient(Key key, const VectorValues& x) const override {
│ │ │ │ -
500 throw std::runtime_error(
│ │ │ │ -
501 "gradient for RegularImplicitSchurFactor is not implemented yet");
│ │ │ │ -
502 }
│ │ │ │ -
│ │ │ │ -
503
│ │ │ │ -
504};
│ │ │ │ -
│ │ │ │ -
505// end class RegularImplicitSchurFactor
│ │ │ │ -
506
│ │ │ │ -
507template<class CAMERA>
│ │ │ │ -
508const int RegularImplicitSchurFactor<CAMERA>::D;
│ │ │ │ -
509
│ │ │ │ -
510template<class CAMERA>
│ │ │ │ -
511const int RegularImplicitSchurFactor<CAMERA>::ZDim;
│ │ │ │ -
512
│ │ │ │ -
513// traits
│ │ │ │ -
│ │ │ │ -
514template<class CAMERA> struct traits<RegularImplicitSchurFactor<CAMERA> > : public Testable<
│ │ │ │ -
515 RegularImplicitSchurFactor<CAMERA> > {
│ │ │ │ -
516};
│ │ │ │ -
│ │ │ │ -
517
│ │ │ │ -
518}
│ │ │ │ -
519
│ │ │ │ -
CameraSet.h
Base class to create smart factors on poses or cameras.
│ │ │ │ - │ │ │ │ -
VectorValues.h
Factor Graph Values.
│ │ │ │ +
482template<class CAMERA>
│ │ │ │ +
│ │ │ │ +
483struct traits<SmartProjectionFactor<CAMERA> > : public Testable<
│ │ │ │ +
484 SmartProjectionFactor<CAMERA> > {
│ │ │ │ +
485};
│ │ │ │ +
│ │ │ │ +
486
│ │ │ │ +
487} // \ namespace gtsam
│ │ │ │ +
triangulation.h
Functions for triangulation.
│ │ │ │ +
SmartFactorBase.h
Base class to create smart factors on poses or cameras.
│ │ │ │ +
SmartFactorParams.h
Collect common parameters for SmartProjection and SmartStereoProjection factors.
│ │ │ │ +
dataset.h
utility functions for loading datasets
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │ -
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::triangulateSafe
TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters &params)
triangulateSafe: extensive checking of the outcome
Definition triangulation.h:680
│ │ │ │ +
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition Matrix.h:81
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
│ │ │ │ -
gtsam::SymmetricBlockMatrix::selfadjointView
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j).
Definition SymmetricBlockMatrix.h:156
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition CameraSet.h:36
│ │ │ │ -
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
│ │ │ │ +
gtsam::CameraSet::SchurComplement
static SymmetricBlockMatrix SchurComplement(const std::vector< Eigen::Matrix< double, ZDim, ND >, Eigen::aligned_allocator< Eigen::Matrix< double, ZDim, ND > > > &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b)
Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = F' * F - F' * E * P * ...
Definition CameraSet.h:150
│ │ │ │ +
gtsam::CameraSet::project2
ZVector project2(const POINT &point, boost::optional< FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const
Project a point (possibly Unit3 at infinity), with derivatives Note that F is a sparse block-diagonal...
Definition CameraSet.h:108
│ │ │ │ +
gtsam::TriangulationResult
TriangulationResult is an optional point, along with the reasons why it is invalid.
Definition triangulation.h:626
│ │ │ │ +
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
│ │ │ │
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
│ │ │ │ -
gtsam::Factor::empty
bool empty() const
Whether the factor is empty (involves zero variables).
Definition Factor.h:128
│ │ │ │ -
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
│ │ │ │ -
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition Factor.h:80
│ │ │ │ -
gtsam::Factor::size
size_t size() const
Definition Factor.h:157
│ │ │ │ -
gtsam::GaussianFactor
An abstract virtual base class for JacobianFactor and HessianFactor.
Definition GaussianFactor.h:39
│ │ │ │ -
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactor.h:42
│ │ │ │ -
gtsam::GaussianFactor::hessianDiagonal
VectorValues hessianDiagonal() const
Return the diagonal of the Hessian for this factor.
Definition GaussianFactor.cpp:35
│ │ │ │ -
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ -
gtsam::VectorValues::insert
iterator insert(const std::pair< Key, Vector > &key_value)
Insert a vector value with key j.
Definition VectorValues.cpp:91
│ │ │ │ -
gtsam::VectorValues::emplace
std::pair< VectorValues::iterator, bool > emplace(Key j, Args &&... args)
Emplace a vector value with key j.
Definition VectorValues.h:185
│ │ │ │ -
gtsam::VectorValues::at
Vector & at(Key j)
Read/write access to the vector value with key j, throws std::out_of_range if j does not exist,...
Definition VectorValues.h:139
│ │ │ │ -
gtsam::VectorValues::tryInsert
std::pair< iterator, bool > tryInsert(Key j, const Vector &value)
insert that mimics the STL map insert - if the value already exists, the map is not modified and an i...
Definition VectorValues.h:209
│ │ │ │ -
gtsam::RegularImplicitSchurFactor
RegularImplicitSchurFactor.
Definition RegularImplicitSchurFactor.h:39
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::E_
const Matrix E_
The 2m*3 E Jacobian with respect to the point.
Definition RegularImplicitSchurFactor.h:60
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::projectError
void projectError(const Error2s &e1, Error2s &e2) const
Calculate corrected error Q*e = (I - E*P*E')*e.
Definition RegularImplicitSchurFactor.h:357
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::clone
GaussianFactor::shared_ptr clone() const override
Clone a factor (make a deep copy)
Definition RegularImplicitSchurFactor.h:256
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::hessianDiagonalAdd
void hessianDiagonalAdd(VectorValues &d) const override
Add the diagonal of the Hessian for this factor to existing VectorValues.
Definition RegularImplicitSchurFactor.h:174
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::RegularImplicitSchurFactor
RegularImplicitSchurFactor(const KeyVector &keys, const FBlocks &Fs, const Matrix &E, const Matrix &P, const Vector &b)
Construct from blocks of F, E, inv(E'*E), and RHS vector b.
Definition RegularImplicitSchurFactor.h:80
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::RegularImplicitSchurFactor
RegularImplicitSchurFactor()
Constructor.
Definition RegularImplicitSchurFactor.h:66
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::b_
const Vector b_
2m-dimensional RHS vector
Definition RegularImplicitSchurFactor.h:61
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::jacobian
std::pair< Matrix, Vector > jacobian() const override
Return the dense Jacobian and right-hand-side , with the noise models baked into A and b.
Definition RegularImplicitSchurFactor.h:148
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::MatrixDD
Eigen::Matrix< double, D, D > MatrixDD
camera Hessian
Definition RegularImplicitSchurFactor.h:55
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::gradientAtZero
void gradientAtZero(double *d) const override
Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS.
Definition RegularImplicitSchurFactor.h:479
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::ZDim
static const int ZDim
Measurement dimension.
Definition RegularImplicitSchurFactor.h:52
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::information
Matrix information() const override
Compute full information matrix
Definition RegularImplicitSchurFactor.h:163
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::~RegularImplicitSchurFactor
~RegularImplicitSchurFactor() override
Destructor.
Definition RegularImplicitSchurFactor.h:85
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::updateHessian
void updateHessian(const KeyVector &keys, SymmetricBlockMatrix *info) const override
Update an information matrix by adding the information corresponding to this factor (used internally ...
Definition RegularImplicitSchurFactor.h:138
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::FBlocks_
FBlocks FBlocks_
All ZDim*D F blocks (one for each camera)
Definition RegularImplicitSchurFactor.h:58
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::gradientAtZero
VectorValues gradientAtZero() const override
Calculate gradient, which is -F'Q*b, see paper.
Definition RegularImplicitSchurFactor.h:457
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::MatrixZD
Eigen::Matrix< double, ZDim, D > MatrixZD
type of an F block
Definition RegularImplicitSchurFactor.h:54
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::PointCovariance_
const Matrix PointCovariance_
the 3*3 matrix P = inv(E'E) (2*2 if degenerate)
Definition RegularImplicitSchurFactor.h:59
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
Hessian-vector multiply, i.e.
Definition RegularImplicitSchurFactor.h:413
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::hessianBlockDiagonal
std::map< Key, Matrix > hessianBlockDiagonal() const override
Return the block diagonal of the Hessian for this factor.
Definition RegularImplicitSchurFactor.h:231
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::augmentedInformation
Matrix augmentedInformation() const override
Compute full augmented information matrix
Definition RegularImplicitSchurFactor.h:155
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::negate
GaussianFactor::shared_ptr negate() const override
Construct the corresponding anti-factor to negate information stored stored in this factor.
Definition RegularImplicitSchurFactor.h:263
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition RegularImplicitSchurFactor.h:43
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::e1
Error2s e1
Scratch space for multiplyHessianAdd.
Definition RegularImplicitSchurFactor.h:374
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition RegularImplicitSchurFactor.h:105
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::getDim
DenseIndex getDim(const_iterator variable) const override
Degrees of freedom of camera.
Definition RegularImplicitSchurFactor.h:134
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::equals
bool equals(const GaussianFactor &lf, double tol) const override
equals
Definition RegularImplicitSchurFactor.h:118
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::augmentedJacobian
Matrix augmentedJacobian() const override
Return a dense Jacobian matrix, augmented with b with the noise models baked into A and b.
Definition RegularImplicitSchurFactor.h:143
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::This
RegularImplicitSchurFactor This
Typedef to this class.
Definition RegularImplicitSchurFactor.h:42
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::D
static const int D
Camera dimension.
Definition RegularImplicitSchurFactor.h:51
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::projectError2
void projectError2(const Error2s &e1, Error2s &e2) const
Calculate corrected error Q*(e-ZDim*b) = (I - E*P*E')*(e-ZDim*b)
Definition RegularImplicitSchurFactor.h:287
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const double *x, double *y) const
double* Hessian-vector multiply, i.e.
Definition RegularImplicitSchurFactor.h:380
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::hessianDiagonal
void hessianDiagonal(double *d) const override
add the contribution of this factor to the diagonal of the hessian d(output) = d(input) + deltaHessia...
Definition RegularImplicitSchurFactor.h:205
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::multiplyHessianDummy
void multiplyHessianDummy(double alpha, const VectorValues &x, VectorValues &y) const
Dummy version to measure overhead of key access.
Definition RegularImplicitSchurFactor.h:442
│ │ │ │ -
gtsam::RegularImplicitSchurFactor::gradient
Vector gradient(Key key, const VectorValues &x) const override
Gradient wrt a key at any values.
Definition RegularImplicitSchurFactor.h:499
│ │ │ │ -
VectorValues
The Factor::error simply extracts the.
│ │ │ │ +
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ +
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition NonlinearFactor.h:118
│ │ │ │ +
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
gtsam::SmartFactorBase
Base class for smart factors.
Definition SmartFactorBase.h:50
│ │ │ │ +
gtsam::SmartFactorBase::computeJacobians
void computeJacobians(FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras, const POINT &point) const
Compute F, E, and b (called below in both vanilla and SVD versions), where F is a vector of derivativ...
Definition SmartFactorBase.h:285
│ │ │ │ +
gtsam::SmartFactorBase::createRegularImplicitSchurFactor
boost::shared_ptr< RegularImplicitSchurFactor< CAMERA > > createRegularImplicitSchurFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0, bool diagonalDamping=false) const
Return Jacobians as RegularImplicitSchurFactor with raw access.
Definition SmartFactorBase.h:356
│ │ │ │ +
gtsam::SmartFactorBase::Dim
static const int Dim
Camera dimension.
Definition SmartFactorBase.h:60
│ │ │ │ +
gtsam::SmartFactorBase::cameras
virtual Cameras cameras(const Values &values) const
Collect all cameras: important that in key order.
Definition SmartFactorBase.h:162
│ │ │ │ +
gtsam::SmartFactorBase::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, const POINT &point) const
Calculate the error of the factor.
Definition SmartFactorBase.h:267
│ │ │ │ +
gtsam::SmartFactorBase::computeJacobiansSVD
void computeJacobiansSVD(FBlocks &Fs, Matrix &Enull, Vector &b, const Cameras &cameras, const POINT &point) const
SVD version that produces smaller Jacobian matrices by doing an SVD decomposition on E,...
Definition SmartFactorBase.h:300
│ │ │ │ +
gtsam::SmartFactorBase::measured_
ZVector measured_
Measurements for each of the m views.
Definition SmartFactorBase.h:79
│ │ │ │ +
gtsam::SmartFactorBase::unwhitenedError
Vector unwhitenedError(const Cameras &cameras, const POINT &point, boost::optional< typename Cameras::FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const
Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and derivatives.
Definition SmartFactorBase.h:204
│ │ │ │ +
gtsam::SmartFactorBase::whitenJacobians
void whitenJacobians(FBlocks &F, Matrix &E, Vector &b) const
Whiten the Jacobians computed by computeJacobians using noiseModel_.
Definition SmartFactorBase.h:347
│ │ │ │ +
gtsam::SmartFactorBase::createJacobianSVDFactor
boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0) const
Return Jacobians as JacobianFactorSVD.
Definition SmartFactorBase.h:386
│ │ │ │ +
gtsam::SmartFactorBase::createJacobianQFactor
boost::shared_ptr< JacobianFactorQ< Dim, ZDim > > createJacobianQFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0, bool diagonalDamping=false) const
Return Jacobians as JacobianFactorQ.
Definition SmartFactorBase.h:369
│ │ │ │ +
gtsam::SmartFactorBase::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition SmartFactorBase.h:174
│ │ │ │ +
gtsam::SmartFactorBase::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition SmartFactorBase.h:187
│ │ │ │ +
gtsam::SmartProjectionParams
Definition SmartFactorParams.h:42
│ │ │ │ +
gtsam::SmartProjectionParams::linearizationMode
LinearizationMode linearizationMode
How to linearize the factor.
Definition SmartFactorParams.h:44
│ │ │ │ +
gtsam::SmartProjectionParams::retriangulationThreshold
double retriangulationThreshold
threshold to decide whether to re-triangulate
Definition SmartFactorParams.h:50
│ │ │ │ +
gtsam::SmartProjectionParams::degeneracyMode
DegeneracyMode degeneracyMode
How to linearize the factor.
Definition SmartFactorParams.h:45
│ │ │ │ +
gtsam::SmartProjectionFactor
SmartProjectionFactor: triangulates point and keeps an estimate of it around.
Definition SmartProjectionFactor.h:45
│ │ │ │ +
gtsam::SmartProjectionFactor::decideIfTriangulate
bool decideIfTriangulate(const Cameras &cameras) const
Check if the new linearization point is the same as the one used for previous triangulation.
Definition SmartProjectionFactor.h:128
│ │ │ │ +
gtsam::SmartProjectionFactor::createJacobianQFactor
boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > createJacobianQFactor(const Values &values, double lambda) const
Create JacobianFactorQ factor, takes values.
Definition SmartProjectionFactor.h:261
│ │ │ │ +
gtsam::SmartProjectionFactor::createJacobianSVDFactor
boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, double lambda) const
Different (faster) way to compute a JacobianFactorSVD factor.
Definition SmartProjectionFactor.h:267
│ │ │ │ +
gtsam::SmartProjectionFactor::computeJacobiansWithTriangulatedPoint
void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras) const
Compute F, E only (called below in both vanilla and SVD versions) Assumes the point has been computed...
Definition SmartProjectionFactor.h:357
│ │ │ │ +
gtsam::SmartProjectionFactor::linearizeToHessian
virtual boost::shared_ptr< RegularHessianFactor< Base::Dim > > linearizeToHessian(const Values &values, double lambda=0.0) const
Linearize to a Hessianfactor.
Definition SmartProjectionFactor.h:277
│ │ │ │ +
gtsam::SmartProjectionFactor::isOutlier
bool isOutlier() const
return the outlier state
Definition SmartProjectionFactor.h:462
│ │ │ │ +
gtsam::SmartProjectionFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition SmartProjectionFactor.h:103
│ │ │ │ +
gtsam::SmartProjectionFactor::triangulateAndComputeE
bool triangulateAndComputeE(Matrix &E, const Values &values) const
Triangulate and compute derivative of error with respect to point.
Definition SmartProjectionFactor.h:349
│ │ │ │ +
gtsam::SmartProjectionFactor::linearizeDamped
boost::shared_ptr< GaussianFactor > linearizeDamped(const Cameras &cameras, const double lambda=0.0) const
Linearize to Gaussian Factor.
Definition SmartProjectionFactor.h:299
│ │ │ │ +
gtsam::SmartProjectionFactor::isFarPoint
bool isFarPoint() const
return the farPoint state
Definition SmartProjectionFactor.h:465
│ │ │ │ +
gtsam::SmartProjectionFactor::result_
TriangulationResult result_
result from triangulateSafe
Definition SmartProjectionFactor.h:63
│ │ │ │ +
gtsam::SmartProjectionFactor::createHessianFactor
boost::shared_ptr< RegularHessianFactor< Base::Dim > > createHessianFactor(const Cameras &cameras, const double lambda=0.0, bool diagonalDamping=false) const
Create a Hessianfactor that is an approximation of error(p).
Definition SmartProjectionFactor.h:199
│ │ │ │ +
gtsam::SmartProjectionFactor::triangulateSafe
TriangulationResult triangulateSafe(const Cameras &cameras) const
Call gtsam::triangulateSafe iff we need to re-triangulate.
Definition SmartProjectionFactor.h:174
│ │ │ │ +
gtsam::SmartProjectionFactor::~SmartProjectionFactor
~SmartProjectionFactor() override
Virtual destructor.
Definition SmartProjectionFactor.h:95
│ │ │ │ +
gtsam::SmartProjectionFactor::error
double error(const Values &values) const override
Calculate total reprojection error.
Definition SmartProjectionFactor.h:433
│ │ │ │ +
gtsam::SmartProjectionFactor::isValid
bool isValid() const
Is result valid?
Definition SmartProjectionFactor.h:453
│ │ │ │ +
gtsam::SmartProjectionFactor::cameraPosesTriangulation_
std::vector< Pose3, Eigen::aligned_allocator< Pose3 > > cameraPosesTriangulation_
current triangulation poses
Definition SmartProjectionFactor.h:65
│ │ │ │ +
gtsam::SmartProjectionFactor::triangulateAndComputeE
bool triangulateAndComputeE(Matrix &E, const Cameras &cameras) const
Triangulate and compute derivative of error with respect to point.
Definition SmartProjectionFactor.h:338
│ │ │ │ +
gtsam::SmartProjectionFactor::linearizeToImplicit
virtual boost::shared_ptr< RegularImplicitSchurFactor< CAMERA > > linearizeToImplicit(const Values &values, double lambda=0.0) const
Linearize to an Implicit Schur factor.
Definition SmartProjectionFactor.h:283
│ │ │ │ +
gtsam::SmartProjectionFactor::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, boost::optional< Point3 > externalPoint=boost::none) const
Calculate the error of the factor.
Definition SmartProjectionFactor.h:411
│ │ │ │ +
gtsam::SmartProjectionFactor::linearizeToJacobian
virtual boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > linearizeToJacobian(const Values &values, double lambda=0.0) const
Linearize to a JacobianfactorQ.
Definition SmartProjectionFactor.h:289
│ │ │ │ +
gtsam::SmartProjectionFactor::isDegenerate
bool isDegenerate() const
return the degenerate state
Definition SmartProjectionFactor.h:456
│ │ │ │ +
gtsam::SmartProjectionFactor::triangulateAndComputeJacobians
bool triangulateAndComputeJacobians(typename Base::FBlocks &Fs, Matrix &E, Vector &b, const Values &values) const
Version that takes values, and creates the point.
Definition SmartProjectionFactor.h:374
│ │ │ │ +
gtsam::SmartProjectionFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition SmartProjectionFactor.h:115
│ │ │ │ +
gtsam::SmartProjectionFactor::Camera
CAMERA Camera
shorthand for a set of cameras
Definition SmartProjectionFactor.h:74
│ │ │ │ +
gtsam::SmartProjectionFactor::createJacobianQFactor
boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > createJacobianQFactor(const Cameras &cameras, double lambda) const
Create JacobianFactorQ factor.
Definition SmartProjectionFactor.h:251
│ │ │ │ +
gtsam::SmartProjectionFactor::access
friend class boost::serialization::access
Serialization function.
Definition SmartProjectionFactor.h:470
│ │ │ │ +
gtsam::SmartProjectionFactor::point
TriangulationResult point(const Values &values) const
COMPUTE the landmark.
Definition SmartProjectionFactor.h:447
│ │ │ │ +
gtsam::SmartProjectionFactor::SmartProjectionFactor
SmartProjectionFactor(const SharedNoiseModel &sharedNoiseModel, const SmartProjectionParams &params=SmartProjectionParams())
Constructor.
Definition SmartProjectionFactor.h:87
│ │ │ │ +
gtsam::SmartProjectionFactor::isPointBehindCamera
bool isPointBehindCamera() const
return the cheirality status flag
Definition SmartProjectionFactor.h:459
│ │ │ │ +
gtsam::SmartProjectionFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &values) const override
linearize
Definition SmartProjectionFactor.h:329
│ │ │ │ +
gtsam::SmartProjectionFactor::triangulateAndComputeJacobiansSVD
bool triangulateAndComputeJacobiansSVD(typename Base::FBlocks &Fs, Matrix &Enull, Vector &b, const Values &values) const
takes values
Definition SmartProjectionFactor.h:385
│ │ │ │ +
gtsam::SmartProjectionFactor::reprojectionErrorAfterTriangulation
Vector reprojectionErrorAfterTriangulation(const Values &values) const
Calculate vector of re-projection errors, before applying noise model.
Definition SmartProjectionFactor.h:396
│ │ │ │ +
gtsam::SmartProjectionFactor::point
TriangulationResult point() const
return the landmark
Definition SmartProjectionFactor.h:442
│ │ │ │ +
gtsam::SmartProjectionFactor::triangulateForLinearize
bool triangulateForLinearize(const Cameras &cameras) const
Possibly re-triangulate before calculating Jacobians.
Definition SmartProjectionFactor.h:193
│ │ │ │ +
gtsam::SmartProjectionFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition SmartProjectionFactor.h:71
│ │ │ │ +
gtsam::SmartProjectionFactor::SmartProjectionFactor
SmartProjectionFactor()
Default constructor, only for serialization.
Definition SmartProjectionFactor.h:80
│ │ │ │ +
gtsam::SmartProjectionFactor::linearizeDamped
boost::shared_ptr< GaussianFactor > linearizeDamped(const Values &values, const double lambda=0.0) const
Linearize to Gaussian Factor.
Definition SmartProjectionFactor.h:321
│ │ │ │ + │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,729 +1,743 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -RegularImplicitSchurFactor.h │ │ │ │ │ +SmartProjectionFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -1 │ │ │ │ │ -8#pragma once │ │ │ │ │ +1/* --------------------------------------------------------------------------- │ │ │ │ │ +- │ │ │ │ │ +2 │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +5 * All Rights Reserved │ │ │ │ │ +6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +7 │ │ │ │ │ +8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ -10#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_m_e_r_a_S_e_t_._h> │ │ │ │ │ -11#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ -12#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ -13 │ │ │ │ │ -14#include │ │ │ │ │ -15#include │ │ │ │ │ -16#include │ │ │ │ │ -17#include │ │ │ │ │ -18 │ │ │ │ │ -19namespace _g_t_s_a_m { │ │ │ │ │ -20 │ │ │ │ │ -38template │ │ │ │ │ -_3_9class _R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r: public _G_a_u_s_s_i_a_n_F_a_c_t_o_r { │ │ │ │ │ -40 │ │ │ │ │ -41public: │ │ │ │ │ -_4_2 typedef _R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ -_4_3 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -44 │ │ │ │ │ -45protected: │ │ │ │ │ +10 * ------------------------------------------------------------------------- │ │ │ │ │ +- */ │ │ │ │ │ +11 │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include <_g_t_s_a_m_/_s_l_a_m_/_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_s_l_a_m_/_S_m_a_r_t_F_a_c_t_o_r_P_a_r_a_m_s_._h> │ │ │ │ │ +24 │ │ │ │ │ +25#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_t_r_i_a_n_g_u_l_a_t_i_o_n_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_S_y_m_b_o_l_._h> │ │ │ │ │ +27#include <_g_t_s_a_m_/_s_l_a_m_/_d_a_t_a_s_e_t_._h> │ │ │ │ │ +28 │ │ │ │ │ +29#include │ │ │ │ │ +30#include │ │ │ │ │ +31#include │ │ │ │ │ +32 │ │ │ │ │ +33namespace _g_t_s_a_m { │ │ │ │ │ +34 │ │ │ │ │ +44template │ │ │ │ │ +_4_5class _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r: public _S_m_a_r_t_F_a_c_t_o_r_B_a_s_e { │ │ │ │ │ 46 │ │ │ │ │ -47 // This factor is closely related to a CameraSet │ │ │ │ │ -48 typedef _C_a_m_e_r_a_S_e_t_<_C_A_M_E_R_A_> Set; │ │ │ │ │ -49 │ │ │ │ │ -50 typedef typename CAMERA::Measurement Z; │ │ │ │ │ -_5_1 static const int _D = _t_r_a_i_t_s_<_C_A_M_E_R_A_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ -_5_2 static const int _Z_D_i_m = _t_r_a_i_t_s_<_Z_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ +47public: │ │ │ │ │ +48 │ │ │ │ │ +49private: │ │ │ │ │ +50 typedef _S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_<_C_A_M_E_R_A_> _B_a_s_e; │ │ │ │ │ +51 typedef _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_<_C_A_M_E_R_A_> This; │ │ │ │ │ +52 typedef _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_<_C_A_M_E_R_A_> SmartProjectionCameraFactor; │ │ │ │ │ 53 │ │ │ │ │ -_5_4 typedef Eigen::Matrix _M_a_t_r_i_x_Z_D; │ │ │ │ │ -_5_5 typedef Eigen::Matrix _M_a_t_r_i_x_D_D; │ │ │ │ │ -56 typedef std::vector > FBlocks; │ │ │ │ │ -57 │ │ │ │ │ -_5_8 FBlocks _F_B_l_o_c_k_s__; │ │ │ │ │ -_5_9 const Matrix _P_o_i_n_t_C_o_v_a_r_i_a_n_c_e__; │ │ │ │ │ -_6_0 const Matrix _E__; │ │ │ │ │ -_6_1 const Vector _b__; │ │ │ │ │ -62 │ │ │ │ │ -63public: │ │ │ │ │ -64 │ │ │ │ │ -_6_6 _R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r() { │ │ │ │ │ -67 } │ │ │ │ │ -68 │ │ │ │ │ -70 │ │ │ │ │ -_8_0 _R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r(const _K_e_y_V_e_c_t_o_r& _k_e_y_s, const FBlocks& Fs, │ │ │ │ │ -81 const Matrix& E, const Matrix& P, const Vector& b) │ │ │ │ │ -82 : _G_a_u_s_s_i_a_n_F_a_c_t_o_r(_k_e_y_s), _F_B_l_o_c_k_s__(Fs), _P_o_i_n_t_C_o_v_a_r_i_a_n_c_e__(P), _E__(E), _b__(b) {} │ │ │ │ │ -83 │ │ │ │ │ -_8_5 _~_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r() override { │ │ │ │ │ -86 } │ │ │ │ │ -87 │ │ │ │ │ -88 const FBlocks& Fs() const { │ │ │ │ │ -89 return _F_B_l_o_c_k_s__; │ │ │ │ │ -90 } │ │ │ │ │ -91 │ │ │ │ │ -92 const Matrix& E() const { │ │ │ │ │ -93 return _E__; │ │ │ │ │ -94 } │ │ │ │ │ -95 │ │ │ │ │ -96 const Vector& b() const { │ │ │ │ │ -97 return _b__; │ │ │ │ │ -98 } │ │ │ │ │ -99 │ │ │ │ │ -100 const Matrix& getPointCovariance() const { │ │ │ │ │ -101 return _P_o_i_n_t_C_o_v_a_r_i_a_n_c_e__; │ │ │ │ │ -102 } │ │ │ │ │ -103 │ │ │ │ │ -_1_0_5 void _p_r_i_n_t(const std::string& s = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ -106 DefaultKeyFormatter) const override { │ │ │ │ │ -107 std::cout << " RegularImplicitSchurFactor " << std::endl; │ │ │ │ │ -108 _F_a_c_t_o_r_:_:_p_r_i_n_t(s); │ │ │ │ │ -109 for (size_t pos = 0; pos < _s_i_z_e(); ++pos) { │ │ │ │ │ -110 std::cout << "Fblock:\n" << _F_B_l_o_c_k_s__[pos] << std::endl; │ │ │ │ │ -111 } │ │ │ │ │ -112 std::cout << "PointCovariance:\n" << _P_o_i_n_t_C_o_v_a_r_i_a_n_c_e__ << std::endl; │ │ │ │ │ -113 std::cout << "E:\n" << _E__ << std::endl; │ │ │ │ │ -114 std::cout << "b:\n" << _b__.transpose() << std::endl; │ │ │ │ │ -115 } │ │ │ │ │ -116 │ │ │ │ │ -_1_1_8 bool _e_q_u_a_l_s(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r& lf, double tol) const override { │ │ │ │ │ -119 const _T_h_i_s* f = dynamic_cast(&lf); │ │ │ │ │ -120 if (!f) │ │ │ │ │ -121 return false; │ │ │ │ │ -122 for (size_t k = 0; k < _F_B_l_o_c_k_s__.size(); ++k) { │ │ │ │ │ -123 if (_k_e_y_s__[k] != f->_k_e_y_s__[k]) │ │ │ │ │ -124 return false; │ │ │ │ │ -125 if (!_e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(_F_B_l_o_c_k_s__[k], f->_F_B_l_o_c_k_s__[k], tol)) │ │ │ │ │ -126 return false; │ │ │ │ │ -127 } │ │ │ │ │ -128 return _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(_P_o_i_n_t_C_o_v_a_r_i_a_n_c_e__, f->_P_o_i_n_t_C_o_v_a_r_i_a_n_c_e__, tol) │ │ │ │ │ -129 && _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(_E__, f->_E__, tol) │ │ │ │ │ -130 && _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(_b__, f->_b__, tol); │ │ │ │ │ -131 } │ │ │ │ │ -132 │ │ │ │ │ -_1_3_4 _D_e_n_s_e_I_n_d_e_x _g_e_t_D_i_m(_c_o_n_s_t___i_t_e_r_a_t_o_r variable) const override { │ │ │ │ │ -135 return _D; │ │ │ │ │ -136 } │ │ │ │ │ -137 │ │ │ │ │ -_1_3_8 void _u_p_d_a_t_e_H_e_s_s_i_a_n(const _K_e_y_V_e_c_t_o_r& _k_e_y_s, │ │ │ │ │ -139 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x* info) const override { │ │ │ │ │ -140 throw std::runtime_error( │ │ │ │ │ -141 "RegularImplicitSchurFactor::updateHessian non implemented"); │ │ │ │ │ -142 } │ │ │ │ │ -_1_4_3 Matrix _a_u_g_m_e_n_t_e_d_J_a_c_o_b_i_a_n() const override { │ │ │ │ │ -144 throw std::runtime_error( │ │ │ │ │ -145 "RegularImplicitSchurFactor::augmentedJacobian non implemented"); │ │ │ │ │ -146 return Matrix(); │ │ │ │ │ -147 } │ │ │ │ │ -_1_4_8 std::pair _j_a_c_o_b_i_a_n() const override { │ │ │ │ │ -149 throw std::runtime_error( │ │ │ │ │ -150 "RegularImplicitSchurFactor::jacobian non implemented"); │ │ │ │ │ -151 return std::make_pair(Matrix(), Vector()); │ │ │ │ │ +54protected: │ │ │ │ │ +55 │ │ │ │ │ +58 _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s params_; │ │ │ │ │ +60 │ │ │ │ │ +_6_3 mutable _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t _r_e_s_u_l_t__; │ │ │ │ │ +64 mutable std::vector > │ │ │ │ │ +_6_5 _c_a_m_e_r_a_P_o_s_e_s_T_r_i_a_n_g_u_l_a_t_i_o_n__; │ │ │ │ │ +67 │ │ │ │ │ +68 public: │ │ │ │ │ +69 │ │ │ │ │ +_7_1 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +72 │ │ │ │ │ +_7_4 typedef CAMERA _C_a_m_e_r_a; │ │ │ │ │ +75 typedef _C_a_m_e_r_a_S_e_t_<_C_A_M_E_R_A_> _C_a_m_e_r_a_s; │ │ │ │ │ +76 │ │ │ │ │ +_8_0 _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r() {} │ │ │ │ │ +81 │ │ │ │ │ +_8_7 _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r( │ │ │ │ │ +88 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& sharedNoiseModel, │ │ │ │ │ +89 const _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s& params = _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s()) │ │ │ │ │ +90 : _B_a_s_e(sharedNoiseModel), │ │ │ │ │ +91 params_(params), │ │ │ │ │ +92 _r_e_s_u_l_t__(_T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t::Degenerate()) {} │ │ │ │ │ +93 │ │ │ │ │ +_9_5 _~_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r() override { │ │ │ │ │ +96 } │ │ │ │ │ +97 │ │ │ │ │ +_1_0_3 void _p_r_i_n_t(const std::string& s = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ +104 DefaultKeyFormatter) const override { │ │ │ │ │ +105 std::cout << s << "SmartProjectionFactor\n"; │ │ │ │ │ +106 std::cout << "linearizationMode: " << params_._l_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e │ │ │ │ │ +107 << std::endl; │ │ │ │ │ +108 std::cout << "triangulationParameters:\n" << params_.triangulation │ │ │ │ │ +109 << std::endl; │ │ │ │ │ +110 std::cout << "result:\n" << _r_e_s_u_l_t__ << std::endl; │ │ │ │ │ +111 _B_a_s_e_:_:_p_r_i_n_t("", keyFormatter); │ │ │ │ │ +112 } │ │ │ │ │ +113 │ │ │ │ │ +_1_1_5 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& p, double tol = 1e-9) const override { │ │ │ │ │ +116 const This *e = dynamic_cast(&p); │ │ │ │ │ +117 return e && params_._l_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e == e->params_.linearizationMode │ │ │ │ │ +118 && _B_a_s_e_:_:_e_q_u_a_l_s(p, tol); │ │ │ │ │ +119 } │ │ │ │ │ +120 │ │ │ │ │ +_1_2_8 bool _d_e_c_i_d_e_I_f_T_r_i_a_n_g_u_l_a_t_e(const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s) const { │ │ │ │ │ +129 // Several calls to linearize will be done from the same linearization │ │ │ │ │ +130 // point, hence it is not needed to re-triangulate. Note that this is not │ │ │ │ │ +131 // yet "selecting linearization", that will come later, and we only check │ │ │ │ │ +if │ │ │ │ │ +132 // the current linearization is the "same" (up to tolerance) w.r.t. the │ │ │ │ │ +last │ │ │ │ │ +133 // time we triangulated the point. │ │ │ │ │ +134 │ │ │ │ │ +135 size_t m = _c_a_m_e_r_a_s.size(); │ │ │ │ │ +136 │ │ │ │ │ +137 bool retriangulate = false; │ │ │ │ │ +138 │ │ │ │ │ +139 // Definitely true if we do not have a previous linearization point or the │ │ │ │ │ +140 // new linearization point includes more poses. │ │ │ │ │ +141 if (_c_a_m_e_r_a_P_o_s_e_s_T_r_i_a_n_g_u_l_a_t_i_o_n__.empty() │ │ │ │ │ +142 || _c_a_m_e_r_a_s.size() != _c_a_m_e_r_a_P_o_s_e_s_T_r_i_a_n_g_u_l_a_t_i_o_n__.size()) │ │ │ │ │ +143 retriangulate = true; │ │ │ │ │ +144 │ │ │ │ │ +145 // Otherwise, check poses against cache. │ │ │ │ │ +146 if (!retriangulate) { │ │ │ │ │ +147 for (size_t i = 0; i < _c_a_m_e_r_a_s.size(); i++) { │ │ │ │ │ +148 if (!_c_a_m_e_r_a_s[i].pose()._e_q_u_a_l_s(_c_a_m_e_r_a_P_o_s_e_s_T_r_i_a_n_g_u_l_a_t_i_o_n__[i], │ │ │ │ │ +149 params_._r_e_t_r_i_a_n_g_u_l_a_t_i_o_n_T_h_r_e_s_h_o_l_d)) { │ │ │ │ │ +150 retriangulate = true; // at least two poses are different, hence we │ │ │ │ │ +retriangulate │ │ │ │ │ +151 break; │ │ │ │ │ 152 } │ │ │ │ │ -153 │ │ │ │ │ -_1_5_5 Matrix _a_u_g_m_e_n_t_e_d_I_n_f_o_r_m_a_t_i_o_n() const override { │ │ │ │ │ -156 // Do the Schur complement │ │ │ │ │ -157 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x augmentedHessian = │ │ │ │ │ -158 Set::SchurComplement(_F_B_l_o_c_k_s__, _E__, _b__); │ │ │ │ │ -159 return augmentedHessian._s_e_l_f_a_d_j_o_i_n_t_V_i_e_w(); │ │ │ │ │ -160 } │ │ │ │ │ -161 │ │ │ │ │ -_1_6_3 Matrix _i_n_f_o_r_m_a_t_i_o_n() const override { │ │ │ │ │ -164 Matrix augmented = _a_u_g_m_e_n_t_e_d_I_n_f_o_r_m_a_t_i_o_n(); │ │ │ │ │ -165 int m = this->_k_e_y_s__.size(); │ │ │ │ │ -166 size_t M = _D * m; │ │ │ │ │ -167 return augmented.block(0, 0, M, M); │ │ │ │ │ -168 } │ │ │ │ │ -169 │ │ │ │ │ -171 using _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_h_e_s_s_i_a_n_D_i_a_g_o_n_a_l; │ │ │ │ │ -172 │ │ │ │ │ -_1_7_4 void _h_e_s_s_i_a_n_D_i_a_g_o_n_a_l_A_d_d(_V_e_c_t_o_r_V_a_l_u_e_s &d) const override { │ │ │ │ │ -175 // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F); │ │ │ │ │ -176 for (size_t k = 0; k < _s_i_z_e(); ++k) { // for each camera │ │ │ │ │ -177 _K_e_y j = _k_e_y_s__[k]; │ │ │ │ │ -178 │ │ │ │ │ -179 // Calculate Fj'*Ej for the current camera (observing a single point) │ │ │ │ │ -180 // D x 3 = (D x ZDim) * (ZDim x 3) │ │ │ │ │ -181 const _M_a_t_r_i_x_Z_D& Fj = _F_B_l_o_c_k_s__[k]; │ │ │ │ │ -182 Eigen::Matrix FtE = Fj.transpose() │ │ │ │ │ -183 * _E__.block<_Z_D_i_m, 3>(_Z_D_i_m * k, 0); │ │ │ │ │ -184 │ │ │ │ │ -185 Eigen::Matrix dj; │ │ │ │ │ -186 for (int k = 0; k < _D; ++k) { // for each diagonal element of the camera │ │ │ │ │ -hessian │ │ │ │ │ -187 // Vector column_k_Fj = Fj.col(k); │ │ │ │ │ -188 dj(k) = Fj.col(k).squaredNorm(); // dot(column_k_Fj, column_k_Fj); │ │ │ │ │ -189 // Vector column_k_FtE = FtE.row(k); │ │ │ │ │ -190 // (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1) │ │ │ │ │ -191 dj(k) -= FtE.row(k) * _P_o_i_n_t_C_o_v_a_r_i_a_n_c_e__ * FtE.row(k).transpose(); │ │ │ │ │ -192 } │ │ │ │ │ -193 │ │ │ │ │ -194 auto result = d._e_m_p_l_a_c_e(j, dj); │ │ │ │ │ -195 if(!result.second) { │ │ │ │ │ -196 result.first->second += dj; │ │ │ │ │ -197 } │ │ │ │ │ -198 } │ │ │ │ │ -199 } │ │ │ │ │ -200 │ │ │ │ │ -_2_0_5 void _h_e_s_s_i_a_n_D_i_a_g_o_n_a_l(double* d) const override { │ │ │ │ │ -206 // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F); │ │ │ │ │ -207 // Use eigen magic to access raw memory │ │ │ │ │ -208 typedef Eigen::Matrix DVector; │ │ │ │ │ -209 typedef Eigen::Map DMap; │ │ │ │ │ -210 │ │ │ │ │ -211 for (size_t pos = 0; pos < _s_i_z_e(); ++pos) { // for each camera in the │ │ │ │ │ -factor │ │ │ │ │ -212 _K_e_y j = _k_e_y_s__[pos]; │ │ │ │ │ -213 │ │ │ │ │ -214 // Calculate Fj'*Ej for the current camera (observing a single point) │ │ │ │ │ -215 // D x 3 = (D x ZDim) * (ZDim x 3) │ │ │ │ │ -216 const _M_a_t_r_i_x_Z_D& Fj = _F_B_l_o_c_k_s__[pos]; │ │ │ │ │ -217 Eigen::Matrix FtE = Fj.transpose() │ │ │ │ │ -218 * _E__.block<_Z_D_i_m, 3>(_Z_D_i_m * pos, 0); │ │ │ │ │ -219 │ │ │ │ │ -220 DVector dj; │ │ │ │ │ -221 for (int k = 0; k < _D; ++k) { // for each diagonal element of the camera │ │ │ │ │ -hessian │ │ │ │ │ -222 dj(k) = Fj.col(k).squaredNorm(); │ │ │ │ │ -223 // (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1) │ │ │ │ │ -224 dj(k) -= FtE.row(k) * _P_o_i_n_t_C_o_v_a_r_i_a_n_c_e__ * FtE.row(k).transpose(); │ │ │ │ │ -225 } │ │ │ │ │ -226 DMap(d + _D * j) += dj; │ │ │ │ │ -227 } │ │ │ │ │ -228 } │ │ │ │ │ -229 │ │ │ │ │ -_2_3_1 std::map _h_e_s_s_i_a_n_B_l_o_c_k_D_i_a_g_o_n_a_l() const override { │ │ │ │ │ -232 std::map blocks; │ │ │ │ │ -233 // F'*(I - E*P*E')*F │ │ │ │ │ -234 for (size_t pos = 0; pos < _s_i_z_e(); ++pos) { │ │ │ │ │ -235 _K_e_y j = _k_e_y_s__[pos]; │ │ │ │ │ -236 // F'*F - F'*E*P*E'*F e.g. (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9) │ │ │ │ │ -237 const _M_a_t_r_i_x_Z_D& Fj = _F_B_l_o_c_k_s__[pos]; │ │ │ │ │ -238 // Eigen::Matrix FtE = Fj.transpose() │ │ │ │ │ -239 // * E_.block(ZDim * pos, 0); │ │ │ │ │ -240 // blocks[j] = Fj.transpose() * Fj │ │ │ │ │ -241 // - FtE * PointCovariance_ * FtE.transpose(); │ │ │ │ │ -242 │ │ │ │ │ -243 const Matrix23& Ej = _E__.block<_Z_D_i_m, 3>(_Z_D_i_m * pos, 0); │ │ │ │ │ -244 blocks[j] = Fj.transpose() │ │ │ │ │ -245 * (Fj - Ej * _P_o_i_n_t_C_o_v_a_r_i_a_n_c_e__ * Ej.transpose() * Fj); │ │ │ │ │ -246 │ │ │ │ │ -247 // F'*(I - E*P*E')*F, TODO: this should work, but it does not :-( │ │ │ │ │ -248 // static const Eigen::Matrix I2 = eye(ZDim); │ │ │ │ │ -249 // Matrix2 Q = // │ │ │ │ │ -250 // I2 - E_.block(ZDim * pos, 0) * PointCovariance_ * │ │ │ │ │ -E_.block(ZDim * pos, 0).transpose(); │ │ │ │ │ -251 // blocks[j] = Fj.transpose() * Q * Fj; │ │ │ │ │ -252 } │ │ │ │ │ -253 return blocks; │ │ │ │ │ -254 } │ │ │ │ │ -255 │ │ │ │ │ -_2_5_6 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r _c_l_o_n_e() const override { │ │ │ │ │ -257 return boost::make_shared >(_k_e_y_s__, │ │ │ │ │ -258 _F_B_l_o_c_k_s__, _P_o_i_n_t_C_o_v_a_r_i_a_n_c_e__, _E__, _b__); │ │ │ │ │ -259 throw std::runtime_error( │ │ │ │ │ -260 "RegularImplicitSchurFactor::clone non implemented"); │ │ │ │ │ -261 } │ │ │ │ │ -262 │ │ │ │ │ -_2_6_3 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r _n_e_g_a_t_e() const override { │ │ │ │ │ -264 return boost::make_shared >(_k_e_y_s__, │ │ │ │ │ -265 _F_B_l_o_c_k_s__, _P_o_i_n_t_C_o_v_a_r_i_a_n_c_e__, _E__, _b__); │ │ │ │ │ -266 throw std::runtime_error( │ │ │ │ │ -267 "RegularImplicitSchurFactor::negate non implemented"); │ │ │ │ │ -268 } │ │ │ │ │ -269 │ │ │ │ │ -270 // Raw Vector version of y += F'*alpha*(I - E*P*E')*F*x, for testing │ │ │ │ │ -271 static │ │ │ │ │ -272 void multiplyHessianAdd(const Matrix& F, const Matrix& E, │ │ │ │ │ -273 const Matrix& PointCovariance, double alpha, const Vector& x, Vector& y) { │ │ │ │ │ -274 Vector _e_1 = F * x; │ │ │ │ │ -275 Vector d1 = E.transpose() * _e_1; │ │ │ │ │ -276 Vector d2 = PointCovariance * d1; │ │ │ │ │ -277 Vector e2 = E * d2; │ │ │ │ │ -278 Vector e3 = alpha * (_e_1 - e2); │ │ │ │ │ -279 y += F.transpose() * e3; │ │ │ │ │ +153 } │ │ │ │ │ +154 } │ │ │ │ │ +155 │ │ │ │ │ +156 // Store the current poses used for triangulation if we will re- │ │ │ │ │ +triangulate. │ │ │ │ │ +157 if (retriangulate) { │ │ │ │ │ +158 _c_a_m_e_r_a_P_o_s_e_s_T_r_i_a_n_g_u_l_a_t_i_o_n__.clear(); │ │ │ │ │ +159 _c_a_m_e_r_a_P_o_s_e_s_T_r_i_a_n_g_u_l_a_t_i_o_n__.reserve(m); │ │ │ │ │ +160 for (size_t i = 0; i < m; i++) │ │ │ │ │ +161 // cameraPosesTriangulation_[i] = cameras[i].pose(); │ │ │ │ │ +162 _c_a_m_e_r_a_P_o_s_e_s_T_r_i_a_n_g_u_l_a_t_i_o_n__.push_back(_c_a_m_e_r_a_s[i].pose()); │ │ │ │ │ +163 } │ │ │ │ │ +164 │ │ │ │ │ +165 return retriangulate; │ │ │ │ │ +166 } │ │ │ │ │ +167 │ │ │ │ │ +_1_7_4 _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t _t_r_i_a_n_g_u_l_a_t_e_S_a_f_e(const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s) const { │ │ │ │ │ +175 │ │ │ │ │ +176 size_t m = _c_a_m_e_r_a_s.size(); │ │ │ │ │ +177 if (m < 2) // if we have a single pose the corresponding factor is │ │ │ │ │ +uninformative │ │ │ │ │ +178 return TriangulationResult::Degenerate(); │ │ │ │ │ +179 │ │ │ │ │ +180 bool retriangulate = _d_e_c_i_d_e_I_f_T_r_i_a_n_g_u_l_a_t_e(_c_a_m_e_r_a_s); │ │ │ │ │ +181 if (retriangulate) │ │ │ │ │ +182 _r_e_s_u_l_t__ = _g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_S_a_f_e(_c_a_m_e_r_a_s, this->_m_e_a_s_u_r_e_d__, │ │ │ │ │ +183 params_.triangulation); │ │ │ │ │ +184 return _r_e_s_u_l_t__; │ │ │ │ │ +185 } │ │ │ │ │ +186 │ │ │ │ │ +_1_9_3 bool _t_r_i_a_n_g_u_l_a_t_e_F_o_r_L_i_n_e_a_r_i_z_e(const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s) const { │ │ │ │ │ +194 _t_r_i_a_n_g_u_l_a_t_e_S_a_f_e(_c_a_m_e_r_a_s); // imperative, might reset result_ │ │ │ │ │ +195 return bool(_r_e_s_u_l_t__); │ │ │ │ │ +196 } │ │ │ │ │ +197 │ │ │ │ │ +_1_9_9 boost::shared_ptr > _c_r_e_a_t_e_H_e_s_s_i_a_n_F_a_c_t_o_r( │ │ │ │ │ +200 const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, const double lambda = 0.0, │ │ │ │ │ +201 bool diagonalDamping = false) const { │ │ │ │ │ +202 size_t numKeys = this->_k_e_y_s__.size(); │ │ │ │ │ +203 // Create structures for Hessian Factors │ │ │ │ │ +204 _K_e_y_V_e_c_t_o_r js; │ │ │ │ │ +205 std::vector Gs(numKeys * (numKeys + 1) / 2); │ │ │ │ │ +206 std::vector gs(numKeys); │ │ │ │ │ +207 │ │ │ │ │ +208 if (this->_m_e_a_s_u_r_e_d__.size() != cameras.size()) │ │ │ │ │ +209 throw std::runtime_error( │ │ │ │ │ +210 "SmartProjectionHessianFactor: this->measured_" │ │ │ │ │ +211 ".size() inconsistent with input"); │ │ │ │ │ +212 │ │ │ │ │ +213 _t_r_i_a_n_g_u_l_a_t_e_S_a_f_e(_c_a_m_e_r_a_s); │ │ │ │ │ +214 │ │ │ │ │ +215 if (params_._d_e_g_e_n_e_r_a_c_y_M_o_d_e == ZERO_ON_DEGENERACY && !_r_e_s_u_l_t__) { │ │ │ │ │ +216 // failed: return"empty" Hessian │ │ │ │ │ +217 for (Matrix& m : Gs) m = Matrix::Zero(_B_a_s_e_:_:_D_i_m, _B_a_s_e_:_:_D_i_m); │ │ │ │ │ +218 for (Vector& v : gs) v = Vector::Zero(_B_a_s_e_:_:_D_i_m); │ │ │ │ │ +219 return boost::make_shared >(this->_k_e_y_s__, │ │ │ │ │ +220 Gs, gs, 0.0); │ │ │ │ │ +221 } │ │ │ │ │ +222 │ │ │ │ │ +223 // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols(). │ │ │ │ │ +224 typename Base::FBlocks Fs; │ │ │ │ │ +225 Matrix E; │ │ │ │ │ +226 Vector b; │ │ │ │ │ +227 _c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s_W_i_t_h_T_r_i_a_n_g_u_l_a_t_e_d_P_o_i_n_t(Fs, E, b, _c_a_m_e_r_a_s); │ │ │ │ │ +228 │ │ │ │ │ +229 // Whiten using noise model │ │ │ │ │ +230 _B_a_s_e_:_:_w_h_i_t_e_n_J_a_c_o_b_i_a_n_s(Fs, E, b); │ │ │ │ │ +231 │ │ │ │ │ +232 // build augmented hessian │ │ │ │ │ +233 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x augmentedHessian = // │ │ │ │ │ +234 _C_a_m_e_r_a_s_:_:_S_c_h_u_r_C_o_m_p_l_e_m_e_n_t(Fs, E, b, lambda, diagonalDamping); │ │ │ │ │ +235 │ │ │ │ │ +236 return boost::make_shared >( │ │ │ │ │ +237 this->_k_e_y_s__, augmentedHessian); │ │ │ │ │ +238 } │ │ │ │ │ +239 │ │ │ │ │ +240 // Create RegularImplicitSchurFactor factor. │ │ │ │ │ +241 boost::shared_ptr > │ │ │ │ │ +createRegularImplicitSchurFactor( │ │ │ │ │ +242 const Cameras& _c_a_m_e_r_a_s, double lambda) const { │ │ │ │ │ +243 if (_t_r_i_a_n_g_u_l_a_t_e_F_o_r_L_i_n_e_a_r_i_z_e(_c_a_m_e_r_a_s)) │ │ │ │ │ +244 return _B_a_s_e_:_:_c_r_e_a_t_e_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r(_c_a_m_e_r_a_s, *_r_e_s_u_l_t__, lambda); │ │ │ │ │ +245 else │ │ │ │ │ +246 // failed: return empty │ │ │ │ │ +247 return boost::shared_ptr >(); │ │ │ │ │ +248 } │ │ │ │ │ +249 │ │ │ │ │ +_2_5_1 boost::shared_ptr > _c_r_e_a_t_e_J_a_c_o_b_i_a_n_Q_F_a_c_t_o_r( │ │ │ │ │ +252 const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, double lambda) const { │ │ │ │ │ +253 if (_t_r_i_a_n_g_u_l_a_t_e_F_o_r_L_i_n_e_a_r_i_z_e(_c_a_m_e_r_a_s)) │ │ │ │ │ +254 return _B_a_s_e_:_:_c_r_e_a_t_e_J_a_c_o_b_i_a_n_Q_F_a_c_t_o_r(_c_a_m_e_r_a_s, *_r_e_s_u_l_t__, lambda); │ │ │ │ │ +255 else │ │ │ │ │ +256 // failed: return empty │ │ │ │ │ +257 return boost::make_shared >(this->_k_e_y_s__); │ │ │ │ │ +258 } │ │ │ │ │ +259 │ │ │ │ │ +_2_6_1 boost::shared_ptr > _c_r_e_a_t_e_J_a_c_o_b_i_a_n_Q_F_a_c_t_o_r( │ │ │ │ │ +262 const _V_a_l_u_e_s& values, double lambda) const { │ │ │ │ │ +263 return _c_r_e_a_t_e_J_a_c_o_b_i_a_n_Q_F_a_c_t_o_r(this->_c_a_m_e_r_a_s(values), lambda); │ │ │ │ │ +264 } │ │ │ │ │ +265 │ │ │ │ │ +_2_6_7 boost::shared_ptr _c_r_e_a_t_e_J_a_c_o_b_i_a_n_S_V_D_F_a_c_t_o_r( │ │ │ │ │ +268 const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, double lambda) const { │ │ │ │ │ +269 if (_t_r_i_a_n_g_u_l_a_t_e_F_o_r_L_i_n_e_a_r_i_z_e(_c_a_m_e_r_a_s)) │ │ │ │ │ +270 return _B_a_s_e_:_:_c_r_e_a_t_e_J_a_c_o_b_i_a_n_S_V_D_F_a_c_t_o_r(_c_a_m_e_r_a_s, *_r_e_s_u_l_t__, lambda); │ │ │ │ │ +271 else │ │ │ │ │ +272 // failed: return empty │ │ │ │ │ +273 return boost::make_shared >(this->_k_e_y_s__); │ │ │ │ │ +274 } │ │ │ │ │ +275 │ │ │ │ │ +_2_7_7 virtual boost::shared_ptr > │ │ │ │ │ +_l_i_n_e_a_r_i_z_e_T_o_H_e_s_s_i_a_n( │ │ │ │ │ +278 const _V_a_l_u_e_s& values, double lambda = 0.0) const { │ │ │ │ │ +279 return _c_r_e_a_t_e_H_e_s_s_i_a_n_F_a_c_t_o_r(this->_c_a_m_e_r_a_s(values), lambda); │ │ │ │ │ 280 } │ │ │ │ │ 281 │ │ │ │ │ -282 typedef std::vector> Error2s; │ │ │ │ │ -283 │ │ │ │ │ -_2_8_7 void _p_r_o_j_e_c_t_E_r_r_o_r_2(const Error2s& _e_1, Error2s& e2) const { │ │ │ │ │ -288 │ │ │ │ │ -289 // d1 = E.transpose() * (e1-ZDim*b) = (3*2m)*2m │ │ │ │ │ -290 Vector3 d1; │ │ │ │ │ -291 d1.setZero(); │ │ │ │ │ -292 for (size_t k = 0; k < _s_i_z_e(); k++) │ │ │ │ │ -293 d1 += _E__.block<_Z_D_i_m, 3>(_Z_D_i_m * k, 0).transpose() │ │ │ │ │ -294 * (_e_1[k] - _Z_D_i_m * _b__.segment<_Z_D_i_m>(k * _Z_D_i_m)); │ │ │ │ │ -295 │ │ │ │ │ -296 // d2 = E.transpose() * e1 = (3*2m)*2m │ │ │ │ │ -297 Vector3 d2 = _P_o_i_n_t_C_o_v_a_r_i_a_n_c_e__ * d1; │ │ │ │ │ -298 │ │ │ │ │ -299 // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] │ │ │ │ │ -300 for (size_t k = 0; k < _s_i_z_e(); k++) │ │ │ │ │ -301 e2[k] = _e_1[k] - _Z_D_i_m * _b__.segment<_Z_D_i_m>(k * _Z_D_i_m) │ │ │ │ │ -302 - _E__.block<_Z_D_i_m, 3>(_Z_D_i_m * k, 0) * d2; │ │ │ │ │ -303 } │ │ │ │ │ -304 │ │ │ │ │ -305 /* │ │ │ │ │ -306 * This definition matches the linearized error in the Hessian Factor: │ │ │ │ │ -307 * LinError(x) = x'*H*x - 2*x'*eta + f │ │ │ │ │ -308 * with: │ │ │ │ │ -309 * H = F' * (I-E'*P*E) * F = F' * Q * F │ │ │ │ │ -310 * eta = F' * (I-E'*P*E) * b = F' * Q * b │ │ │ │ │ -311 * f = nonlinear error │ │ │ │ │ -312 * (x'*H*x - 2*x'*eta + f) = x'*F'*Q*F*x - 2*x'*F'*Q *b + f = x'*F'*Q*(F*x - │ │ │ │ │ -2*b) + f │ │ │ │ │ -313 */ │ │ │ │ │ -314 double error(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const override { │ │ │ │ │ +_2_8_3 virtual boost::shared_ptr > │ │ │ │ │ +_l_i_n_e_a_r_i_z_e_T_o_I_m_p_l_i_c_i_t( │ │ │ │ │ +284 const _V_a_l_u_e_s& values, double lambda = 0.0) const { │ │ │ │ │ +285 return createRegularImplicitSchurFactor(this->_c_a_m_e_r_a_s(values), lambda); │ │ │ │ │ +286 } │ │ │ │ │ +287 │ │ │ │ │ +_2_8_9 virtual boost::shared_ptr > │ │ │ │ │ +_l_i_n_e_a_r_i_z_e_T_o_J_a_c_o_b_i_a_n( │ │ │ │ │ +290 const _V_a_l_u_e_s& values, double lambda = 0.0) const { │ │ │ │ │ +291 return _c_r_e_a_t_e_J_a_c_o_b_i_a_n_Q_F_a_c_t_o_r(this->_c_a_m_e_r_a_s(values), lambda); │ │ │ │ │ +292 } │ │ │ │ │ +293 │ │ │ │ │ +_2_9_9 boost::shared_ptr _l_i_n_e_a_r_i_z_e_D_a_m_p_e_d(const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, │ │ │ │ │ +300 const double lambda = 0.0) const { │ │ │ │ │ +301 // depending on flag set on construction we may linearize to different │ │ │ │ │ +linear factors │ │ │ │ │ +302 switch (params_._l_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e) { │ │ │ │ │ +303 case HESSIAN: │ │ │ │ │ +304 return _c_r_e_a_t_e_H_e_s_s_i_a_n_F_a_c_t_o_r(_c_a_m_e_r_a_s, lambda); │ │ │ │ │ +305 case IMPLICIT_SCHUR: │ │ │ │ │ +306 return createRegularImplicitSchurFactor(_c_a_m_e_r_a_s, lambda); │ │ │ │ │ +307 case JACOBIAN_SVD: │ │ │ │ │ +308 return _c_r_e_a_t_e_J_a_c_o_b_i_a_n_S_V_D_F_a_c_t_o_r(_c_a_m_e_r_a_s, lambda); │ │ │ │ │ +309 case JACOBIAN_Q: │ │ │ │ │ +310 return _c_r_e_a_t_e_J_a_c_o_b_i_a_n_Q_F_a_c_t_o_r(_c_a_m_e_r_a_s, lambda); │ │ │ │ │ +311 default: │ │ │ │ │ +312 throw std::runtime_error("SmartFactorlinearize: unknown mode"); │ │ │ │ │ +313 } │ │ │ │ │ +314 } │ │ │ │ │ 315 │ │ │ │ │ -316 // resize does not do malloc if correct size │ │ │ │ │ -317 _e_1.resize(_s_i_z_e()); │ │ │ │ │ -318 e2.resize(_s_i_z_e()); │ │ │ │ │ -319 │ │ │ │ │ -320 // e1 = F * x - b = (2m*dm)*dm │ │ │ │ │ -321 for (size_t k = 0; k < _s_i_z_e(); ++k) │ │ │ │ │ -322 _e_1[k] = _F_B_l_o_c_k_s__[k] * x._a_t(_k_e_y_s__[k]); │ │ │ │ │ -323 _p_r_o_j_e_c_t_E_r_r_o_r_2(_e_1, e2); │ │ │ │ │ -324 │ │ │ │ │ -325 double result = 0; │ │ │ │ │ -326 for (size_t k = 0; k < _s_i_z_e(); ++k) │ │ │ │ │ -327 result += _d_o_t(_e_1[k], e2[k]); │ │ │ │ │ -328 │ │ │ │ │ -329 double f = _b__.squaredNorm(); │ │ │ │ │ -330 return 0.5 * (result + f); │ │ │ │ │ -331 } │ │ │ │ │ -332 │ │ │ │ │ -333 // needed to be GaussianFactor - (I - E*P*E')*(F*x - b) │ │ │ │ │ -334 // This is wrong and does not match the definition in Hessian, │ │ │ │ │ -335 // but it matches the definition of the Jacobian factor (JF) │ │ │ │ │ -336 double errorJF(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const { │ │ │ │ │ -337 │ │ │ │ │ -338 // resize does not do malloc if correct size │ │ │ │ │ -339 _e_1.resize(_s_i_z_e()); │ │ │ │ │ -340 e2.resize(_s_i_z_e()); │ │ │ │ │ -341 │ │ │ │ │ -342 // e1 = F * x - b = (2m*dm)*dm │ │ │ │ │ -343 for (size_t k = 0; k < _s_i_z_e(); ++k) │ │ │ │ │ -344 _e_1[k] = _F_B_l_o_c_k_s__[k] * x.at(_k_e_y_s__[k]) - _b__.segment<_Z_D_i_m>(k * _Z_D_i_m); │ │ │ │ │ -345 _p_r_o_j_e_c_t_E_r_r_o_r(_e_1, e2); │ │ │ │ │ -346 │ │ │ │ │ -347 double result = 0; │ │ │ │ │ -348 for (size_t k = 0; k < _s_i_z_e(); ++k) │ │ │ │ │ -349 result += _d_o_t(e2[k], e2[k]); │ │ │ │ │ -350 │ │ │ │ │ -351 // std::cout << "implicitFactor::error result " << result << std::endl; │ │ │ │ │ -352 return 0.5 * result; │ │ │ │ │ -353 } │ │ │ │ │ -_3_5_7 void _p_r_o_j_e_c_t_E_r_r_o_r(const Error2s& _e_1, Error2s& e2) const { │ │ │ │ │ -358 │ │ │ │ │ -359 // d1 = E.transpose() * e1 = (3*2m)*2m │ │ │ │ │ -360 Vector3 d1; │ │ │ │ │ -361 d1.setZero(); │ │ │ │ │ -362 for (size_t k = 0; k < _s_i_z_e(); k++) │ │ │ │ │ -363 d1 += _E__.block<_Z_D_i_m, 3>(_Z_D_i_m * k, 0).transpose() * _e_1[k]; │ │ │ │ │ -364 │ │ │ │ │ -365 // d2 = E.transpose() * e1 = (3*2m)*2m │ │ │ │ │ -366 Vector3 d2 = _P_o_i_n_t_C_o_v_a_r_i_a_n_c_e__ * d1; │ │ │ │ │ -367 │ │ │ │ │ -368 // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] │ │ │ │ │ -369 for (size_t k = 0; k < _s_i_z_e(); k++) │ │ │ │ │ -370 e2[k] = _e_1[k] - _E__.block<_Z_D_i_m, 3>(_Z_D_i_m * k, 0) * d2; │ │ │ │ │ +_3_2_1 boost::shared_ptr _l_i_n_e_a_r_i_z_e_D_a_m_p_e_d(const _V_a_l_u_e_s& values, │ │ │ │ │ +322 const double lambda = 0.0) const { │ │ │ │ │ +323 // depending on flag set on construction we may linearize to different │ │ │ │ │ +linear factors │ │ │ │ │ +324 _C_a_m_e_r_a_s _c_a_m_e_r_a_s = this->_c_a_m_e_r_a_s(values); │ │ │ │ │ +325 return _l_i_n_e_a_r_i_z_e_D_a_m_p_e_d(_c_a_m_e_r_a_s, lambda); │ │ │ │ │ +326 } │ │ │ │ │ +327 │ │ │ │ │ +_3_2_9 boost::shared_ptr _l_i_n_e_a_r_i_z_e( │ │ │ │ │ +330 const _V_a_l_u_e_s& values) const override { │ │ │ │ │ +331 return _l_i_n_e_a_r_i_z_e_D_a_m_p_e_d(values); │ │ │ │ │ +332 } │ │ │ │ │ +333 │ │ │ │ │ +_3_3_8 bool _t_r_i_a_n_g_u_l_a_t_e_A_n_d_C_o_m_p_u_t_e_E(Matrix& E, const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s) const { │ │ │ │ │ +339 bool nonDegenerate = _t_r_i_a_n_g_u_l_a_t_e_F_o_r_L_i_n_e_a_r_i_z_e(_c_a_m_e_r_a_s); │ │ │ │ │ +340 if (nonDegenerate) │ │ │ │ │ +341 _c_a_m_e_r_a_s._p_r_o_j_e_c_t_2(*_r_e_s_u_l_t__, boost::none, E); │ │ │ │ │ +342 return nonDegenerate; │ │ │ │ │ +343 } │ │ │ │ │ +344 │ │ │ │ │ +_3_4_9 bool _t_r_i_a_n_g_u_l_a_t_e_A_n_d_C_o_m_p_u_t_e_E(Matrix& E, const _V_a_l_u_e_s& values) const { │ │ │ │ │ +350 _C_a_m_e_r_a_s _c_a_m_e_r_a_s = this->_c_a_m_e_r_a_s(values); │ │ │ │ │ +351 return _t_r_i_a_n_g_u_l_a_t_e_A_n_d_C_o_m_p_u_t_e_E(E, _c_a_m_e_r_a_s); │ │ │ │ │ +352 } │ │ │ │ │ +353 │ │ │ │ │ +_3_5_7 void _c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s_W_i_t_h_T_r_i_a_n_g_u_l_a_t_e_d_P_o_i_n_t( │ │ │ │ │ +358 typename Base::FBlocks& Fs, Matrix& E, Vector& b, │ │ │ │ │ +359 const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s) const { │ │ │ │ │ +360 │ │ │ │ │ +361 if (!_r_e_s_u_l_t__) { │ │ │ │ │ +362 // Handle degeneracy │ │ │ │ │ +363 // TODO check flag whether we should do this │ │ │ │ │ +364 _U_n_i_t_3 backProjected = _c_a_m_e_r_a_s[0].backprojectPointAtInfinity( │ │ │ │ │ +365 this->_m_e_a_s_u_r_e_d__.at(0)); │ │ │ │ │ +366 _B_a_s_e_:_:_c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s(Fs, E, b, _c_a_m_e_r_a_s, backProjected); │ │ │ │ │ +367 } else { │ │ │ │ │ +368 // valid result: just return Base version │ │ │ │ │ +369 _B_a_s_e_:_:_c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s(Fs, E, b, _c_a_m_e_r_a_s, *_r_e_s_u_l_t__); │ │ │ │ │ +370 } │ │ │ │ │ 371 } │ │ │ │ │ 372 │ │ │ │ │ -_3_7_4 mutable Error2s _e_1, e2; │ │ │ │ │ -375 │ │ │ │ │ -_3_8_0 void _m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d(double alpha, const double* x, double* y) const { │ │ │ │ │ -381 │ │ │ │ │ -382 // Use eigen magic to access raw memory │ │ │ │ │ -383 typedef Eigen::Matrix DVector; │ │ │ │ │ -384 typedef Eigen::Map DMap; │ │ │ │ │ -385 typedef Eigen::Map ConstDMap; │ │ │ │ │ -386 │ │ │ │ │ -387 // resize does not do malloc if correct size │ │ │ │ │ -388 _e_1.resize(_s_i_z_e()); │ │ │ │ │ -389 e2.resize(_s_i_z_e()); │ │ │ │ │ -390 │ │ │ │ │ -391 // e1 = F * x = (2m*dm)*dm │ │ │ │ │ -392 for (size_t k = 0; k < _s_i_z_e(); ++k) { │ │ │ │ │ -393 _K_e_y key = _k_e_y_s__[k]; │ │ │ │ │ -394 _e_1[k] = _F_B_l_o_c_k_s__[k] * ConstDMap(x + _D * key); │ │ │ │ │ -395 } │ │ │ │ │ -396 │ │ │ │ │ -397 _p_r_o_j_e_c_t_E_r_r_o_r(_e_1, e2); │ │ │ │ │ -398 │ │ │ │ │ -399 // y += F.transpose()*e2 = (2d*2m)*2m │ │ │ │ │ -400 for (size_t k = 0; k < _s_i_z_e(); ++k) { │ │ │ │ │ -401 _K_e_y key = _k_e_y_s__[k]; │ │ │ │ │ -402 DMap(y + _D * key) += _F_B_l_o_c_k_s__[k].transpose() * alpha * e2[k]; │ │ │ │ │ +_3_7_4 bool _t_r_i_a_n_g_u_l_a_t_e_A_n_d_C_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s( │ │ │ │ │ +375 typename Base::FBlocks& Fs, Matrix& E, Vector& b, │ │ │ │ │ +376 const _V_a_l_u_e_s& values) const { │ │ │ │ │ +377 _C_a_m_e_r_a_s _c_a_m_e_r_a_s = this->_c_a_m_e_r_a_s(values); │ │ │ │ │ +378 bool nonDegenerate = _t_r_i_a_n_g_u_l_a_t_e_F_o_r_L_i_n_e_a_r_i_z_e(_c_a_m_e_r_a_s); │ │ │ │ │ +379 if (nonDegenerate) │ │ │ │ │ +380 _c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s_W_i_t_h_T_r_i_a_n_g_u_l_a_t_e_d_P_o_i_n_t(Fs, E, b, _c_a_m_e_r_a_s); │ │ │ │ │ +381 return nonDegenerate; │ │ │ │ │ +382 } │ │ │ │ │ +383 │ │ │ │ │ +_3_8_5 bool _t_r_i_a_n_g_u_l_a_t_e_A_n_d_C_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s_S_V_D( │ │ │ │ │ +386 typename Base::FBlocks& Fs, Matrix& Enull, Vector& b, │ │ │ │ │ +387 const _V_a_l_u_e_s& values) const { │ │ │ │ │ +388 _C_a_m_e_r_a_s _c_a_m_e_r_a_s = this->_c_a_m_e_r_a_s(values); │ │ │ │ │ +389 bool nonDegenerate = _t_r_i_a_n_g_u_l_a_t_e_F_o_r_L_i_n_e_a_r_i_z_e(_c_a_m_e_r_a_s); │ │ │ │ │ +390 if (nonDegenerate) │ │ │ │ │ +391 _B_a_s_e_:_:_c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s_S_V_D(Fs, Enull, b, _c_a_m_e_r_a_s, *_r_e_s_u_l_t__); │ │ │ │ │ +392 return nonDegenerate; │ │ │ │ │ +393 } │ │ │ │ │ +394 │ │ │ │ │ +_3_9_6 Vector _r_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r_A_f_t_e_r_T_r_i_a_n_g_u_l_a_t_i_o_n(const _V_a_l_u_e_s& values) const { │ │ │ │ │ +397 _C_a_m_e_r_a_s _c_a_m_e_r_a_s = this->_c_a_m_e_r_a_s(values); │ │ │ │ │ +398 bool nonDegenerate = _t_r_i_a_n_g_u_l_a_t_e_F_o_r_L_i_n_e_a_r_i_z_e(_c_a_m_e_r_a_s); │ │ │ │ │ +399 if (nonDegenerate) │ │ │ │ │ +400 return _B_a_s_e_:_:_u_n_w_h_i_t_e_n_e_d_E_r_r_o_r(_c_a_m_e_r_a_s, *_r_e_s_u_l_t__); │ │ │ │ │ +401 else │ │ │ │ │ +402 return Vector::Zero(_c_a_m_e_r_a_s.size() * 2); │ │ │ │ │ 403 } │ │ │ │ │ -404 } │ │ │ │ │ -405 │ │ │ │ │ -406 void multiplyHessianAdd(double alpha, const double* x, double* y, │ │ │ │ │ -407 std::vector _k_e_y_s) const { │ │ │ │ │ -408 } │ │ │ │ │ -409 │ │ │ │ │ -_4_1_3 void _m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d(double alpha, const _V_e_c_t_o_r_V_a_l_u_e_s& x, │ │ │ │ │ -414 _V_e_c_t_o_r_V_a_l_u_e_s& y) const override { │ │ │ │ │ -415 │ │ │ │ │ -416 // resize does not do malloc if correct size │ │ │ │ │ -417 _e_1.resize(_s_i_z_e()); │ │ │ │ │ -418 e2.resize(_s_i_z_e()); │ │ │ │ │ -419 │ │ │ │ │ -420 // e1 = F * x = (2m*dm)*dm │ │ │ │ │ -421 for (size_t k = 0; k < _s_i_z_e(); ++k) │ │ │ │ │ -422 _e_1[k] = _F_B_l_o_c_k_s__[k] * x._a_t(_k_e_y_s__[k]); │ │ │ │ │ -423 │ │ │ │ │ -424 _p_r_o_j_e_c_t_E_r_r_o_r(_e_1, e2); │ │ │ │ │ -425 │ │ │ │ │ -426 // y += F.transpose()*e2 = (2d*2m)*2m │ │ │ │ │ -427 for (size_t k = 0; k < _s_i_z_e(); ++k) { │ │ │ │ │ -428 _K_e_y key = _k_e_y_s__[k]; │ │ │ │ │ -429 static const Vector _e_m_p_t_y; │ │ │ │ │ -430 std::pair it = y._t_r_y_I_n_s_e_r_t(key, _e_m_p_t_y); │ │ │ │ │ -431 Vector& yi = it.first->second; │ │ │ │ │ -432 // Create the value as a zero vector if it does not exist. │ │ │ │ │ -433 if (it.second) │ │ │ │ │ -434 yi = Vector::Zero(_F_B_l_o_c_k_s__[k].cols()); │ │ │ │ │ -435 yi += _F_B_l_o_c_k_s__[k].transpose() * alpha * e2[k]; │ │ │ │ │ -436 } │ │ │ │ │ -437 } │ │ │ │ │ -438 │ │ │ │ │ -_4_4_2 void _m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_D_u_m_m_y(double alpha, const _V_e_c_t_o_r_V_a_l_u_e_s& x, │ │ │ │ │ -443 _V_e_c_t_o_r_V_a_l_u_e_s& y) const { │ │ │ │ │ -444 │ │ │ │ │ -445 for (size_t k = 0; k < _s_i_z_e(); ++k) { │ │ │ │ │ -446 static const Vector _e_m_p_t_y; │ │ │ │ │ -447 _K_e_y key = _k_e_y_s__[k]; │ │ │ │ │ -448 std::pair it = y._t_r_y_I_n_s_e_r_t(key, _e_m_p_t_y); │ │ │ │ │ -449 Vector& yi = it.first->second; │ │ │ │ │ -450 yi = x._a_t(key); │ │ │ │ │ -451 } │ │ │ │ │ -452 } │ │ │ │ │ -453 │ │ │ │ │ -_4_5_7 _V_e_c_t_o_r_V_a_l_u_e_s _g_r_a_d_i_e_n_t_A_t_Z_e_r_o() const override { │ │ │ │ │ -458 // calculate Q*b │ │ │ │ │ -459 _e_1.resize(_s_i_z_e()); │ │ │ │ │ -460 e2.resize(_s_i_z_e()); │ │ │ │ │ -461 for (size_t k = 0; k < _s_i_z_e(); k++) │ │ │ │ │ -462 _e_1[k] = _b__.segment<_Z_D_i_m>(_Z_D_i_m * k); │ │ │ │ │ -463 _p_r_o_j_e_c_t_E_r_r_o_r(_e_1, e2); │ │ │ │ │ -464 │ │ │ │ │ -465 // g = F.transpose()*e2 │ │ │ │ │ -466 _V_e_c_t_o_r_V_a_l_u_e_s g; │ │ │ │ │ -467 for (size_t k = 0; k < _s_i_z_e(); ++k) { │ │ │ │ │ -468 _K_e_y key = _k_e_y_s__[k]; │ │ │ │ │ -469 g._i_n_s_e_r_t(key, -_F_B_l_o_c_k_s__[k].transpose() * e2[k]); │ │ │ │ │ -470 } │ │ │ │ │ -471 │ │ │ │ │ -472 // return it │ │ │ │ │ -473 return g; │ │ │ │ │ -474 } │ │ │ │ │ -475 │ │ │ │ │ -_4_7_9 void _g_r_a_d_i_e_n_t_A_t_Z_e_r_o(double* d) const override { │ │ │ │ │ +404 │ │ │ │ │ +_4_1_1 double _t_o_t_a_l_R_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r(const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, │ │ │ │ │ +412 boost::optional externalPoint = boost::none) const { │ │ │ │ │ +413 │ │ │ │ │ +414 if (externalPoint) │ │ │ │ │ +415 _r_e_s_u_l_t__ = _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t(*externalPoint); │ │ │ │ │ +416 else │ │ │ │ │ +417 _r_e_s_u_l_t__ = _t_r_i_a_n_g_u_l_a_t_e_S_a_f_e(_c_a_m_e_r_a_s); │ │ │ │ │ +418 │ │ │ │ │ +419 if (_r_e_s_u_l_t__) │ │ │ │ │ +420 // All good, just use version in base class │ │ │ │ │ +421 return _B_a_s_e_:_:_t_o_t_a_l_R_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r(_c_a_m_e_r_a_s, *_r_e_s_u_l_t__); │ │ │ │ │ +422 else if (params_._d_e_g_e_n_e_r_a_c_y_M_o_d_e == HANDLE_INFINITY) { │ │ │ │ │ +423 // Otherwise, manage the exceptions with rotation-only factors │ │ │ │ │ +424 _U_n_i_t_3 backprojected = _c_a_m_e_r_a_s.front().backprojectPointAtInfinity( │ │ │ │ │ +425 this->_m_e_a_s_u_r_e_d__.at(0)); │ │ │ │ │ +426 return _B_a_s_e_:_:_t_o_t_a_l_R_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r(cameras, backprojected); │ │ │ │ │ +427 } else │ │ │ │ │ +428 // if we don't want to manage the exceptions we discard the factor │ │ │ │ │ +429 return 0.0; │ │ │ │ │ +430 } │ │ │ │ │ +431 │ │ │ │ │ +_4_3_3 double _e_r_r_o_r(const _V_a_l_u_e_s& values) const override { │ │ │ │ │ +434 if (this->_a_c_t_i_v_e(values)) { │ │ │ │ │ +435 return _t_o_t_a_l_R_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r(_B_a_s_e_:_:_c_a_m_e_r_a_s(values)); │ │ │ │ │ +436 } else { // else of active flag │ │ │ │ │ +437 return 0.0; │ │ │ │ │ +438 } │ │ │ │ │ +439 } │ │ │ │ │ +440 │ │ │ │ │ +_4_4_2 _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t _p_o_i_n_t() const { │ │ │ │ │ +443 return _r_e_s_u_l_t__; │ │ │ │ │ +444 } │ │ │ │ │ +445 │ │ │ │ │ +_4_4_7 _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t _p_o_i_n_t(const _V_a_l_u_e_s& values) const { │ │ │ │ │ +448 _C_a_m_e_r_a_s _c_a_m_e_r_a_s = this->_c_a_m_e_r_a_s(values); │ │ │ │ │ +449 return _t_r_i_a_n_g_u_l_a_t_e_S_a_f_e(_c_a_m_e_r_a_s); │ │ │ │ │ +450 } │ │ │ │ │ +451 │ │ │ │ │ +_4_5_3 bool _i_s_V_a_l_i_d() const { return _r_e_s_u_l_t__.valid(); } │ │ │ │ │ +454 │ │ │ │ │ +_4_5_6 bool _i_s_D_e_g_e_n_e_r_a_t_e() const { return _r_e_s_u_l_t__.degenerate(); } │ │ │ │ │ +457 │ │ │ │ │ +_4_5_9 bool _i_s_P_o_i_n_t_B_e_h_i_n_d_C_a_m_e_r_a() const { return _r_e_s_u_l_t__.behindCamera(); } │ │ │ │ │ +460 │ │ │ │ │ +_4_6_2 bool _i_s_O_u_t_l_i_e_r() const { return _r_e_s_u_l_t__.outlier(); } │ │ │ │ │ +463 │ │ │ │ │ +_4_6_5 bool _i_s_F_a_r_P_o_i_n_t() const { return _r_e_s_u_l_t__.farPoint(); } │ │ │ │ │ +466 │ │ │ │ │ +467 private: │ │ │ │ │ +468 │ │ │ │ │ +_4_7_0 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +471 template │ │ │ │ │ +472 void serialize(ARCHIVE & ar, const unsigned int version) { │ │ │ │ │ +473 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +474 ar & BOOST_SERIALIZATION_NVP(params_); │ │ │ │ │ +475 ar & BOOST_SERIALIZATION_NVP(_r_e_s_u_l_t__); │ │ │ │ │ +476 ar & BOOST_SERIALIZATION_NVP(_c_a_m_e_r_a_P_o_s_e_s_T_r_i_a_n_g_u_l_a_t_i_o_n__); │ │ │ │ │ +477 } │ │ │ │ │ +478} │ │ │ │ │ +479; │ │ │ │ │ 480 │ │ │ │ │ -481 // Use eigen magic to access raw memory │ │ │ │ │ -482 typedef Eigen::Matrix DVector; │ │ │ │ │ -483 typedef Eigen::Map DMap; │ │ │ │ │ -484 │ │ │ │ │ -485 // calculate Q*b │ │ │ │ │ -486 _e_1.resize(_s_i_z_e()); │ │ │ │ │ -487 e2.resize(_s_i_z_e()); │ │ │ │ │ -488 for (size_t k = 0; k < _s_i_z_e(); k++) │ │ │ │ │ -489 _e_1[k] = _b__.segment<_Z_D_i_m>(_Z_D_i_m * k); │ │ │ │ │ -490 _p_r_o_j_e_c_t_E_r_r_o_r(_e_1, e2); │ │ │ │ │ -491 │ │ │ │ │ -492 for (size_t k = 0; k < _s_i_z_e(); ++k) { // for each camera in the factor │ │ │ │ │ -493 _K_e_y j = _k_e_y_s__[k]; │ │ │ │ │ -494 DMap(d + _D * j) += -_F_B_l_o_c_k_s__[k].transpose() * e2[k]; │ │ │ │ │ -495 } │ │ │ │ │ -496 } │ │ │ │ │ -497 │ │ │ │ │ -_4_9_9 Vector _g_r_a_d_i_e_n_t(_K_e_y key, const _V_e_c_t_o_r_V_a_l_u_e_s& x) const override { │ │ │ │ │ -500 throw std::runtime_error( │ │ │ │ │ -501 "gradient for RegularImplicitSchurFactor is not implemented yet"); │ │ │ │ │ -502 } │ │ │ │ │ -503 │ │ │ │ │ -504}; │ │ │ │ │ -505// end class RegularImplicitSchurFactor │ │ │ │ │ -506 │ │ │ │ │ -507template │ │ │ │ │ -508const int _R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_<_C_A_M_E_R_A_>_:_:_D; │ │ │ │ │ -509 │ │ │ │ │ -510template │ │ │ │ │ -511const int _R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_<_C_A_M_E_R_A_>_:_:_Z_D_i_m; │ │ │ │ │ -512 │ │ │ │ │ -513// traits │ │ │ │ │ -_5_1_4template struct _t_r_a_i_t_s<_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r > : │ │ │ │ │ -public _T_e_s_t_a_b_l_e< │ │ │ │ │ -515 RegularImplicitSchurFactor > { │ │ │ │ │ -516}; │ │ │ │ │ -517 │ │ │ │ │ -518} │ │ │ │ │ -519 │ │ │ │ │ -_C_a_m_e_r_a_S_e_t_._h │ │ │ │ │ +482template │ │ │ │ │ +_4_8_3struct _t_r_a_i_t_s<_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r > : public _T_e_s_t_a_b_l_e< │ │ │ │ │ +484 SmartProjectionFactor > { │ │ │ │ │ +485}; │ │ │ │ │ +486 │ │ │ │ │ +487} // \ namespace gtsam │ │ │ │ │ +_t_r_i_a_n_g_u_l_a_t_i_o_n_._h │ │ │ │ │ +Functions for triangulation. │ │ │ │ │ +_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_._h │ │ │ │ │ Base class to create smart factors on poses or cameras. │ │ │ │ │ -_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ -Factor Graph Values. │ │ │ │ │ +_S_m_a_r_t_F_a_c_t_o_r_P_a_r_a_m_s_._h │ │ │ │ │ +Collect common parameters for SmartProjection and SmartStereoProjection │ │ │ │ │ +factors. │ │ │ │ │ +_d_a_t_a_s_e_t_._h │ │ │ │ │ +utility functions for loading datasets │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ FastVector< Key > KeyVector │ │ │ │ │ Define collection type once and for all - also used in wrappers. │ │ │ │ │ DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_n_s_e_I_n_d_e_x │ │ │ │ │ -ptrdiff_t DenseIndex │ │ │ │ │ -The index type for Eigen objects. │ │ │ │ │ -DDeeffiinniittiioonn types.h:106 │ │ │ │ │ -_g_t_s_a_m_:_:_d_o_t │ │ │ │ │ -double dot(const V1 &a, const V2 &b) │ │ │ │ │ -Dot product. │ │ │ │ │ -DDeeffiinniittiioonn Vector.h:195 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_S_a_f_e │ │ │ │ │ +TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const │ │ │ │ │ +typename CAMERA::MeasurementVector &measured, const TriangulationParameters │ │ │ │ │ +¶ms) │ │ │ │ │ +triangulateSafe: extensive checking of the outcome │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:680 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ +Aliases. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l___w_i_t_h___a_b_s___t_o_l │ │ │ │ │ -bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen:: │ │ │ │ │ -DenseBase< MATRIX > &B, double tol=1e-9) │ │ │ │ │ -equals with a tolerance │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:81 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ _g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ of blocks. │ │ │ │ │ DDeeffiinniittiioonn SymmetricBlockMatrix.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_s_e_l_f_a_d_j_o_i_n_t_V_i_e_w │ │ │ │ │ -Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex │ │ │ │ │ -I, DenseIndex J) const │ │ │ │ │ -Return the square sub-matrix that contains blocks(i:j, i:j). │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:156 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ _g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ A helper that implements the traits interface for GTSAM types. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ _g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t │ │ │ │ │ A set of cameras, all with their own calibration. │ │ │ │ │ DDeeffiinniittiioonn CameraSet.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s │ │ │ │ │ -const KeyVector & keys() const │ │ │ │ │ -Access the factor's involved variable keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:140 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_S_c_h_u_r_C_o_m_p_l_e_m_e_n_t │ │ │ │ │ +static SymmetricBlockMatrix SchurComplement(const std::vector< Eigen::Matrix< │ │ │ │ │ +double, ZDim, ND >, Eigen::aligned_allocator< Eigen::Matrix< double, ZDim, ND > │ │ │ │ │ +> > &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector │ │ │ │ │ +&b) │ │ │ │ │ +Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = │ │ │ │ │ +F' * F - F' * E * P * ... │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:150 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_p_r_o_j_e_c_t_2 │ │ │ │ │ +ZVector project2(const POINT &point, boost::optional< FBlocks & > Fs=boost:: │ │ │ │ │ +none, boost::optional< Matrix & > E=boost::none) const │ │ │ │ │ +Project a point (possibly Unit3 at infinity), with derivatives Note that F is a │ │ │ │ │ +sparse block-diagonal... │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:108 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t │ │ │ │ │ +TriangulationResult is an optional point, along with the reasons why it is │ │ │ │ │ +invalid. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:626 │ │ │ │ │ +_g_t_s_a_m_:_:_U_n_i_t_3 │ │ │ │ │ +Represents a 3D point on a unit sphere. │ │ │ │ │ +DDeeffiinniittiioonn Unit3.h:43 │ │ │ │ │ _g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s__ │ │ │ │ │ KeyVector keys_ │ │ │ │ │ The keys involved in this factor. │ │ │ │ │ DDeeffiinniittiioonn Factor.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_m_p_t_y │ │ │ │ │ -bool empty() const │ │ │ │ │ -Whether the factor is empty (involves zero variables). │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:128 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ -virtual void print(const std::string &s="Factor", const KeyFormatter │ │ │ │ │ -&formatter=DefaultKeyFormatter) const │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn Factor.cpp:29 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_c_o_n_s_t___i_t_e_r_a_t_o_r │ │ │ │ │ -KeyVector::const_iterator const_iterator │ │ │ │ │ -Const iterator over keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:80 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_s_i_z_e │ │ │ │ │ -size_t size() const │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:157 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -An abstract virtual base class for JacobianFactor and HessianFactor. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactor.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_h_e_s_s_i_a_n_D_i_a_g_o_n_a_l │ │ │ │ │ -VectorValues hessianDiagonal() const │ │ │ │ │ -Return the diagonal of the Hessian for this factor. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactor.cpp:35 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ -with a unique integer... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_i_n_s_e_r_t │ │ │ │ │ -iterator insert(const std::pair< Key, Vector > &key_value) │ │ │ │ │ -Insert a vector value with key j. │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.cpp:91 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_e_m_p_l_a_c_e │ │ │ │ │ -std::pair< VectorValues::iterator, bool > emplace(Key j, Args &&... args) │ │ │ │ │ -Emplace a vector value with key j. │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:185 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_a_t │ │ │ │ │ -Vector & at(Key j) │ │ │ │ │ -Read/write access to the vector value with key j, throws std::out_of_range if j │ │ │ │ │ -does not exist,... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:139 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_t_r_y_I_n_s_e_r_t │ │ │ │ │ -std::pair< iterator, bool > tryInsert(Key j, const Vector &value) │ │ │ │ │ -insert that mimics the STL map insert - if the value already exists, the map is │ │ │ │ │ -not modified and an i... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:209 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r │ │ │ │ │ -RegularImplicitSchurFactor. │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_E__ │ │ │ │ │ -const Matrix E_ │ │ │ │ │ -The 2m*3 E Jacobian with respect to the point. │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:60 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_p_r_o_j_e_c_t_E_r_r_o_r │ │ │ │ │ -void projectError(const Error2s &e1, Error2s &e2) const │ │ │ │ │ -Calculate corrected error Q*e = (I - E*P*E')*e. │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:357 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ -GaussianFactor::shared_ptr clone() const override │ │ │ │ │ -Clone a factor (make a deep copy) │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:256 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_h_e_s_s_i_a_n_D_i_a_g_o_n_a_l_A_d_d │ │ │ │ │ -void hessianDiagonalAdd(VectorValues &d) const override │ │ │ │ │ -Add the diagonal of the Hessian for this factor to existing VectorValues. │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:174 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r │ │ │ │ │ -RegularImplicitSchurFactor(const KeyVector &keys, const FBlocks &Fs, const │ │ │ │ │ -Matrix &E, const Matrix &P, const Vector &b) │ │ │ │ │ -Construct from blocks of F, E, inv(E'*E), and RHS vector b. │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:80 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r │ │ │ │ │ -RegularImplicitSchurFactor() │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:66 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_b__ │ │ │ │ │ -const Vector b_ │ │ │ │ │ -2m-dimensional RHS vector │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:61 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_j_a_c_o_b_i_a_n │ │ │ │ │ -std::pair< Matrix, Vector > jacobian() const override │ │ │ │ │ -Return the dense Jacobian and right-hand-side , with the noise models baked │ │ │ │ │ -into A and b. │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:148 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_M_a_t_r_i_x_D_D │ │ │ │ │ -Eigen::Matrix< double, D, D > MatrixDD │ │ │ │ │ -camera Hessian │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_g_r_a_d_i_e_n_t_A_t_Z_e_r_o │ │ │ │ │ -void gradientAtZero(double *d) const override │ │ │ │ │ -Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS. │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:479 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_Z_D_i_m │ │ │ │ │ -static const int ZDim │ │ │ │ │ -Measurement dimension. │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_i_n_f_o_r_m_a_t_i_o_n │ │ │ │ │ -Matrix information() const override │ │ │ │ │ -Compute full information matrix │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:163 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_~_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r │ │ │ │ │ -~RegularImplicitSchurFactor() override │ │ │ │ │ -Destructor. │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_u_p_d_a_t_e_H_e_s_s_i_a_n │ │ │ │ │ -void updateHessian(const KeyVector &keys, SymmetricBlockMatrix *info) const │ │ │ │ │ -override │ │ │ │ │ -Update an information matrix by adding the information corresponding to this │ │ │ │ │ -factor (used internally ... │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:138 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_F_B_l_o_c_k_s__ │ │ │ │ │ -FBlocks FBlocks_ │ │ │ │ │ -All ZDim*D F blocks (one for each camera) │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_g_r_a_d_i_e_n_t_A_t_Z_e_r_o │ │ │ │ │ -VectorValues gradientAtZero() const override │ │ │ │ │ -Calculate gradient, which is -F'Q*b, see paper. │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:457 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_M_a_t_r_i_x_Z_D │ │ │ │ │ -Eigen::Matrix< double, ZDim, D > MatrixZD │ │ │ │ │ -type of an F block │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:54 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_P_o_i_n_t_C_o_v_a_r_i_a_n_c_e__ │ │ │ │ │ -const Matrix PointCovariance_ │ │ │ │ │ -the 3*3 matrix P = inv(E'E) (2*2 if degenerate) │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:59 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d │ │ │ │ │ -void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) │ │ │ │ │ -const override │ │ │ │ │ -Hessian-vector multiply, i.e. │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:413 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_h_e_s_s_i_a_n_B_l_o_c_k_D_i_a_g_o_n_a_l │ │ │ │ │ -std::map< Key, Matrix > hessianBlockDiagonal() const override │ │ │ │ │ -Return the block diagonal of the Hessian for this factor. │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:231 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_a_u_g_m_e_n_t_e_d_I_n_f_o_r_m_a_t_i_o_n │ │ │ │ │ -Matrix augmentedInformation() const override │ │ │ │ │ -Compute full augmented information matrix │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:155 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_n_e_g_a_t_e │ │ │ │ │ -GaussianFactor::shared_ptr negate() const override │ │ │ │ │ -Construct the corresponding anti-factor to negate information stored stored in │ │ │ │ │ -this factor. │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:263 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_e_1 │ │ │ │ │ -Error2s e1 │ │ │ │ │ -Scratch space for multiplyHessianAdd. │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:374 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ +Nonlinear factor base class. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_:_:_a_c_t_i_v_e │ │ │ │ │ +virtual bool active(const Values &) const │ │ │ │ │ +Checks whether a factor should be used based on a set of values. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:118 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e │ │ │ │ │ +Base class for smart factors. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s │ │ │ │ │ +void computeJacobians(FBlocks &Fs, Matrix &E, Vector &b, const Cameras │ │ │ │ │ +&cameras, const POINT &point) const │ │ │ │ │ +Compute F, E, and b (called below in both vanilla and SVD versions), where F is │ │ │ │ │ +a vector of derivativ... │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:285 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_c_r_e_a_t_e_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r │ │ │ │ │ +boost::shared_ptr< RegularImplicitSchurFactor< CAMERA > > │ │ │ │ │ +createRegularImplicitSchurFactor(const Cameras &cameras, const Point3 &point, │ │ │ │ │ +double lambda=0.0, bool diagonalDamping=false) const │ │ │ │ │ +Return Jacobians as RegularImplicitSchurFactor with raw access. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:356 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_D_i_m │ │ │ │ │ +static const int Dim │ │ │ │ │ +Camera dimension. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:60 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_c_a_m_e_r_a_s │ │ │ │ │ +virtual Cameras cameras(const Values &values) const │ │ │ │ │ +Collect all cameras: important that in key order. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:162 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_t_o_t_a_l_R_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r │ │ │ │ │ +double totalReprojectionError(const Cameras &cameras, const POINT &point) const │ │ │ │ │ +Calculate the error of the factor. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:267 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s_S_V_D │ │ │ │ │ +void computeJacobiansSVD(FBlocks &Fs, Matrix &Enull, Vector &b, const Cameras │ │ │ │ │ +&cameras, const POINT &point) const │ │ │ │ │ +SVD version that produces smaller Jacobian matrices by doing an SVD │ │ │ │ │ +decomposition on E,... │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:300 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_m_e_a_s_u_r_e_d__ │ │ │ │ │ +ZVector measured_ │ │ │ │ │ +Measurements for each of the m views. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:79 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_u_n_w_h_i_t_e_n_e_d_E_r_r_o_r │ │ │ │ │ +Vector unwhitenedError(const Cameras &cameras, const POINT &point, boost:: │ │ │ │ │ +optional< typename Cameras::FBlocks & > Fs=boost::none, boost::optional< Matrix │ │ │ │ │ +& > E=boost::none) const │ │ │ │ │ +Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and derivatives. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:204 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_w_h_i_t_e_n_J_a_c_o_b_i_a_n_s │ │ │ │ │ +void whitenJacobians(FBlocks &F, Matrix &E, Vector &b) const │ │ │ │ │ +Whiten the Jacobians computed by computeJacobians using noiseModel_. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:347 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_c_r_e_a_t_e_J_a_c_o_b_i_a_n_S_V_D_F_a_c_t_o_r │ │ │ │ │ +boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras │ │ │ │ │ +&cameras, const Point3 &point, double lambda=0.0) const │ │ │ │ │ +Return Jacobians as JacobianFactorSVD. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:386 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_c_r_e_a_t_e_J_a_c_o_b_i_a_n_Q_F_a_c_t_o_r │ │ │ │ │ +boost::shared_ptr< JacobianFactorQ< Dim, ZDim > > createJacobianQFactor(const │ │ │ │ │ +Cameras &cameras, const Point3 &point, double lambda=0.0, bool │ │ │ │ │ +diagonalDamping=false) const │ │ │ │ │ +Return Jacobians as JacobianFactorQ. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:369 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_p_r_i_n_t │ │ │ │ │ void print(const std::string &s="", const KeyFormatter │ │ │ │ │ &keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ print │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:105 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_g_e_t_D_i_m │ │ │ │ │ -DenseIndex getDim(const_iterator variable) const override │ │ │ │ │ -Degrees of freedom of camera. │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:134 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const GaussianFactor &lf, double tol) const override │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:174 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const NonlinearFactor &p, double tol=1e-9) const override │ │ │ │ │ equals │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:118 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_a_u_g_m_e_n_t_e_d_J_a_c_o_b_i_a_n │ │ │ │ │ -Matrix augmentedJacobian() const override │ │ │ │ │ -Return a dense Jacobian matrix, augmented with b with the noise models baked │ │ │ │ │ -into A and b. │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:143 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_T_h_i_s │ │ │ │ │ -RegularImplicitSchurFactor This │ │ │ │ │ -Typedef to this class. │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_D │ │ │ │ │ -static const int D │ │ │ │ │ -Camera dimension. │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:51 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_p_r_o_j_e_c_t_E_r_r_o_r_2 │ │ │ │ │ -void projectError2(const Error2s &e1, Error2s &e2) const │ │ │ │ │ -Calculate corrected error Q*(e-ZDim*b) = (I - E*P*E')*(e-ZDim*b) │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:287 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d │ │ │ │ │ -void multiplyHessianAdd(double alpha, const double *x, double *y) const │ │ │ │ │ -double* Hessian-vector multiply, i.e. │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:380 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_h_e_s_s_i_a_n_D_i_a_g_o_n_a_l │ │ │ │ │ -void hessianDiagonal(double *d) const override │ │ │ │ │ -add the contribution of this factor to the diagonal of the hessian d(output) = │ │ │ │ │ -d(input) + deltaHessia... │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:205 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_D_u_m_m_y │ │ │ │ │ -void multiplyHessianDummy(double alpha, const VectorValues &x, VectorValues &y) │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:187 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorParams.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s_:_:_l_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e │ │ │ │ │ +LinearizationMode linearizationMode │ │ │ │ │ +How to linearize the factor. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorParams.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s_:_:_r_e_t_r_i_a_n_g_u_l_a_t_i_o_n_T_h_r_e_s_h_o_l_d │ │ │ │ │ +double retriangulationThreshold │ │ │ │ │ +threshold to decide whether to re-triangulate │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorParams.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s_:_:_d_e_g_e_n_e_r_a_c_y_M_o_d_e │ │ │ │ │ +DegeneracyMode degeneracyMode │ │ │ │ │ +How to linearize the factor. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorParams.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +SmartProjectionFactor: triangulates point and keeps an estimate of it around. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_d_e_c_i_d_e_I_f_T_r_i_a_n_g_u_l_a_t_e │ │ │ │ │ +bool decideIfTriangulate(const Cameras &cameras) const │ │ │ │ │ +Check if the new linearization point is the same as the one used for previous │ │ │ │ │ +triangulation. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:128 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_c_r_e_a_t_e_J_a_c_o_b_i_a_n_Q_F_a_c_t_o_r │ │ │ │ │ +boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > createJacobianQFactor │ │ │ │ │ +(const Values &values, double lambda) const │ │ │ │ │ +Create JacobianFactorQ factor, takes values. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:261 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_c_r_e_a_t_e_J_a_c_o_b_i_a_n_S_V_D_F_a_c_t_o_r │ │ │ │ │ +boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras │ │ │ │ │ +&cameras, double lambda) const │ │ │ │ │ +Different (faster) way to compute a JacobianFactorSVD factor. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:267 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s_W_i_t_h_T_r_i_a_n_g_u_l_a_t_e_d_P_o_i_n_t │ │ │ │ │ +void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks &Fs, Matrix │ │ │ │ │ +&E, Vector &b, const Cameras &cameras) const │ │ │ │ │ +Compute F, E only (called below in both vanilla and SVD versions) Assumes the │ │ │ │ │ +point has been computed... │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:357 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e_T_o_H_e_s_s_i_a_n │ │ │ │ │ +virtual boost::shared_ptr< RegularHessianFactor< Base::Dim > > │ │ │ │ │ +linearizeToHessian(const Values &values, double lambda=0.0) const │ │ │ │ │ +Linearize to a Hessianfactor. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:277 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_i_s_O_u_t_l_i_e_r │ │ │ │ │ +bool isOutlier() const │ │ │ │ │ +return the outlier state │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:462 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:103 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_t_r_i_a_n_g_u_l_a_t_e_A_n_d_C_o_m_p_u_t_e_E │ │ │ │ │ +bool triangulateAndComputeE(Matrix &E, const Values &values) const │ │ │ │ │ +Triangulate and compute derivative of error with respect to point. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:349 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e_D_a_m_p_e_d │ │ │ │ │ +boost::shared_ptr< GaussianFactor > linearizeDamped(const Cameras &cameras, │ │ │ │ │ +const double lambda=0.0) const │ │ │ │ │ +Linearize to Gaussian Factor. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:299 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_i_s_F_a_r_P_o_i_n_t │ │ │ │ │ +bool isFarPoint() const │ │ │ │ │ +return the farPoint state │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:465 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_r_e_s_u_l_t__ │ │ │ │ │ +TriangulationResult result_ │ │ │ │ │ +result from triangulateSafe │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:63 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_c_r_e_a_t_e_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +boost::shared_ptr< RegularHessianFactor< Base::Dim > > createHessianFactor │ │ │ │ │ +(const Cameras &cameras, const double lambda=0.0, bool diagonalDamping=false) │ │ │ │ │ const │ │ │ │ │ -Dummy version to measure overhead of key access. │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:442 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_g_r_a_d_i_e_n_t │ │ │ │ │ -Vector gradient(Key key, const VectorValues &x) const override │ │ │ │ │ -Gradient wrt a key at any values. │ │ │ │ │ -DDeeffiinniittiioonn RegularImplicitSchurFactor.h:499 │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -The Factor::error simply extracts the. │ │ │ │ │ +Create a Hessianfactor that is an approximation of error(p). │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:199 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_t_r_i_a_n_g_u_l_a_t_e_S_a_f_e │ │ │ │ │ +TriangulationResult triangulateSafe(const Cameras &cameras) const │ │ │ │ │ +Call gtsam::triangulateSafe iff we need to re-triangulate. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:174 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_~_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +~SmartProjectionFactor() override │ │ │ │ │ +Virtual destructor. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:95 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_e_r_r_o_r │ │ │ │ │ +double error(const Values &values) const override │ │ │ │ │ +Calculate total reprojection error. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:433 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_i_s_V_a_l_i_d │ │ │ │ │ +bool isValid() const │ │ │ │ │ +Is result valid? │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:453 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_c_a_m_e_r_a_P_o_s_e_s_T_r_i_a_n_g_u_l_a_t_i_o_n__ │ │ │ │ │ +std::vector< Pose3, Eigen::aligned_allocator< Pose3 > > │ │ │ │ │ +cameraPosesTriangulation_ │ │ │ │ │ +current triangulation poses │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_t_r_i_a_n_g_u_l_a_t_e_A_n_d_C_o_m_p_u_t_e_E │ │ │ │ │ +bool triangulateAndComputeE(Matrix &E, const Cameras &cameras) const │ │ │ │ │ +Triangulate and compute derivative of error with respect to point. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:338 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e_T_o_I_m_p_l_i_c_i_t │ │ │ │ │ +virtual boost::shared_ptr< RegularImplicitSchurFactor< CAMERA > > │ │ │ │ │ +linearizeToImplicit(const Values &values, double lambda=0.0) const │ │ │ │ │ +Linearize to an Implicit Schur factor. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:283 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_t_o_t_a_l_R_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r │ │ │ │ │ +double totalReprojectionError(const Cameras &cameras, boost::optional< Point3 > │ │ │ │ │ +externalPoint=boost::none) const │ │ │ │ │ +Calculate the error of the factor. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:411 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e_T_o_J_a_c_o_b_i_a_n │ │ │ │ │ +virtual boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > │ │ │ │ │ +linearizeToJacobian(const Values &values, double lambda=0.0) const │ │ │ │ │ +Linearize to a JacobianfactorQ. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:289 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_i_s_D_e_g_e_n_e_r_a_t_e │ │ │ │ │ +bool isDegenerate() const │ │ │ │ │ +return the degenerate state │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:456 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_t_r_i_a_n_g_u_l_a_t_e_A_n_d_C_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s │ │ │ │ │ +bool triangulateAndComputeJacobians(typename Base::FBlocks &Fs, Matrix &E, │ │ │ │ │ +Vector &b, const Values &values) const │ │ │ │ │ +Version that takes values, and creates the point. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:374 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const NonlinearFactor &p, double tol=1e-9) const override │ │ │ │ │ +equals │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:115 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_C_a_m_e_r_a │ │ │ │ │ +CAMERA Camera │ │ │ │ │ +shorthand for a set of cameras │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_c_r_e_a_t_e_J_a_c_o_b_i_a_n_Q_F_a_c_t_o_r │ │ │ │ │ +boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > createJacobianQFactor │ │ │ │ │ +(const Cameras &cameras, double lambda) const │ │ │ │ │ +Create JacobianFactorQ factor. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:251 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:470 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_p_o_i_n_t │ │ │ │ │ +TriangulationResult point(const Values &values) const │ │ │ │ │ +COMPUTE the landmark. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:447 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +SmartProjectionFactor(const SharedNoiseModel &sharedNoiseModel, const │ │ │ │ │ +SmartProjectionParams ¶ms=SmartProjectionParams()) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:87 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_i_s_P_o_i_n_t_B_e_h_i_n_d_C_a_m_e_r_a │ │ │ │ │ +bool isPointBehindCamera() const │ │ │ │ │ +return the cheirality status flag │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:459 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e │ │ │ │ │ +boost::shared_ptr< GaussianFactor > linearize(const Values &values) const │ │ │ │ │ +override │ │ │ │ │ +linearize │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:329 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_t_r_i_a_n_g_u_l_a_t_e_A_n_d_C_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s_S_V_D │ │ │ │ │ +bool triangulateAndComputeJacobiansSVD(typename Base::FBlocks &Fs, Matrix │ │ │ │ │ +&Enull, Vector &b, const Values &values) const │ │ │ │ │ +takes values │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:385 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_r_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r_A_f_t_e_r_T_r_i_a_n_g_u_l_a_t_i_o_n │ │ │ │ │ +Vector reprojectionErrorAfterTriangulation(const Values &values) const │ │ │ │ │ +Calculate vector of re-projection errors, before applying noise model. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:396 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_p_o_i_n_t │ │ │ │ │ +TriangulationResult point() const │ │ │ │ │ +return the landmark │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:442 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_t_r_i_a_n_g_u_l_a_t_e_F_o_r_L_i_n_e_a_r_i_z_e │ │ │ │ │ +bool triangulateForLinearize(const Cameras &cameras) const │ │ │ │ │ +Possibly re-triangulate before calculating Jacobians. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:193 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shorthand for a smart pointer to a factor │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:71 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +SmartProjectionFactor() │ │ │ │ │ +Default constructor, only for serialization. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:80 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e_D_a_m_p_e_d │ │ │ │ │ +boost::shared_ptr< GaussianFactor > linearizeDamped(const Values &values, const │ │ │ │ │ +double lambda=0.0) const │ │ │ │ │ +Linearize to Gaussian Factor. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:321 │ │ │ │ │ +_S_y_m_b_o_l_._h │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_._h │ │ │ │ │ + * _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01247.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/dataset.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/PoseRotationPrior.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,171 +94,44 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Macros | │ │ │ │ -Typedefs | │ │ │ │ -Functions
│ │ │ │ -
dataset.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
PoseRotationPrior.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

utility functions for loading datasets │ │ │ │ +

Implements a prior on the rotation component of a pose. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::PoseRotationPrior< POSE >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Macros

│ │ │ │ -#define LINESIZE   81920
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Typedefs

│ │ │ │ -template<typename T >
using gtsam::Parser = std::function< boost::optional< T >(istream &is, const string &tag)>
 
│ │ │ │ -using gtsam::BearingRange2D = BearingRange< Pose2, Point2 >
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

string gtsam::findExampleDataFile (const std::string &name)
 Find the full path to an example dataset distributed with gtsam.
 
│ │ │ │ -string gtsam::createRewrittenFileName (const std::string &name)
 Creates a temporary file name that needs to be ignored in .gitingnore for checking read-write oprations.
 
│ │ │ │ -template<typename T >
map< size_t, T > gtsam::parseToMap (const string &filename, Parser< pair< size_t, T > > parse, size_t maxIndex)
 
boost::optional< IndexedPose > gtsam::parseVertexPose (std::istream &is, const std::string &tag)
 Parse TORO/G2O vertex "id x y yaw".
 
│ │ │ │ -template<>
GTSAM_EXPORT std::map< size_t, Pose2 > gtsam::parseVariables< Pose2 > (const std::string &filename, size_t maxIndex)
 
boost::optional< IndexedLandmark > gtsam::parseVertexLandmark (std::istream &is, const std::string &tag)
 Parse G2O landmark vertex "id x y".
 
│ │ │ │ -template<>
GTSAM_EXPORT std::map< size_t, Point2 > gtsam::parseVariables< Point2 > (const std::string &filename, size_t maxIndex)
 
boost::optional< IndexedEdge > gtsam::parseEdge (std::istream &is, const std::string &tag)
 Parse TORO/G2O edge "id1 id2 x y yaw".
 
│ │ │ │ -boost::shared_ptr< Sampler > gtsam::createSampler (const SharedNoiseModel &model)
 
│ │ │ │ -template<>
GTSAM_EXPORT std::vector< BinaryMeasurement< Pose2 > > gtsam::parseMeasurements (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
 
│ │ │ │ -template<>
GTSAM_EXPORT std::vector< BinaryMeasurement< Rot2 > > gtsam::parseMeasurements (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
 
│ │ │ │ -template<>
GTSAM_EXPORT std::vector< BetweenFactor< Pose2 >::shared_ptr > gtsam::parseFactors< Pose2 > (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
 
GraphAndValues gtsam::load2D (const std::string &filename, SharedNoiseModel model=SharedNoiseModel(), size_t maxIndex=0, bool addNoise=false, bool smart=true, NoiseFormat noiseFormat=NoiseFormatAUTO, KernelFunctionType kernelFunctionType=KernelFunctionTypeNONE)
 Load TORO/G2O style graph files.
 
GraphAndValues gtsam::load2D (pair< string, SharedNoiseModel > dataset, size_t maxIndex, bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType)
 Load TORO 2D Graph.
 
│ │ │ │ -GraphAndValues gtsam::load2D_robust (const string &filename, const noiseModel::Base::shared_ptr &model, size_t maxIndex)
 
│ │ │ │ -void gtsam::save2D (const NonlinearFactorGraph &graph, const Values &config, const noiseModel::Diagonal::shared_ptr model, const std::string &filename)
 save 2d graph
 
GraphAndValues gtsam::readG2o (const std::string &g2oFile, const bool is3D=false, KernelFunctionType kernelFunctionType=KernelFunctionTypeNONE)
 This function parses a g2o file and stores the measurements into a NonlinearFactorGraph and the initial guess in a Values structure.
 
void gtsam::writeG2o (const NonlinearFactorGraph &graph, const Values &estimate, const std::string &filename)
 This function writes a g2o file from NonlinearFactorGraph and a Values structure.
 
│ │ │ │ -istream & gtsam::operator>> (istream &is, Quaternion &q)
 
│ │ │ │ -istream & gtsam::operator>> (istream &is, Rot3 &R)
 
│ │ │ │ -boost::optional< pair< size_t, Pose3 > > gtsam::parseVertexPose3 (istream &is, const string &tag)
 
│ │ │ │ -template<>
GTSAM_EXPORT std::map< size_t, Pose3 > gtsam::parseVariables< Pose3 > (const std::string &filename, size_t maxIndex)
 
│ │ │ │ -boost::optional< pair< size_t, Point3 > > gtsam::parseVertexPoint3 (istream &is, const string &tag)
 
│ │ │ │ -template<>
GTSAM_EXPORT std::map< size_t, Point3 > gtsam::parseVariables< Point3 > (const std::string &filename, size_t maxIndex)
 
│ │ │ │ -istream & gtsam::operator>> (istream &is, Matrix6 &m)
 
│ │ │ │ -template<>
GTSAM_EXPORT std::vector< BinaryMeasurement< Pose3 > > gtsam::parseMeasurements (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
 
│ │ │ │ -template<>
GTSAM_EXPORT std::vector< BinaryMeasurement< Rot3 > > gtsam::parseMeasurements (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
 
│ │ │ │ -template<>
GTSAM_EXPORT std::vector< BetweenFactor< Pose3 >::shared_ptr > gtsam::parseFactors< Pose3 > (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
 
│ │ │ │ -GraphAndValues gtsam::load3D (const std::string &filename)
 Load TORO 3D Graph.
 
│ │ │ │ -BetweenFactorPose2s gtsam::parse2DFactors (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
 
│ │ │ │ -BetweenFactorPose3s gtsam::parse3DFactors (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
 
│ │ │ │

Detailed Description

│ │ │ │ -

utility functions for loading datasets

│ │ │ │ -
Date
Jan 22, 2010
│ │ │ │ -
Author
Kai Ni
│ │ │ │ -
│ │ │ │ -Luca Carlone
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ -
│ │ │ │ -Varun Agrawal
│ │ │ │ +

Implements a prior on the rotation component of a pose.

│ │ │ │ +
Date
Jun 14, 2012
│ │ │ │ +
Author
Alex Cunningham
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,188 +1,26 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -dataset.cpp File Reference │ │ │ │ │ -utility functions for loading datasets _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +PoseRotationPrior.h File Reference │ │ │ │ │ +Implements a prior on the rotation component of a pose. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r_<_ _P_O_S_E_ _> │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -MMaaccrrooss │ │ │ │ │ -#define  LLIINNEESSIIZZEE   81920 │ │ │ │ │ -  │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -template │ │ │ │ │ -using  ggttssaamm::::PPaarrsseerr = std::function< boost::optional< T >(istream &is, const │ │ │ │ │ - string &tag)> │ │ │ │ │ -  │ │ │ │ │ -using  ggttssaamm::::BBeeaarriinnggRRaannggee22DD = _B_e_a_r_i_n_g_R_a_n_g_e< _P_o_s_e_2, _P_o_i_n_t_2 > │ │ │ │ │ -  │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ - string  _g_t_s_a_m_:_:_f_i_n_d_E_x_a_m_p_l_e_D_a_t_a_F_i_l_e (const std:: │ │ │ │ │ - string &name) │ │ │ │ │ -  Find the full path to an example dataset │ │ │ │ │ - distributed with gtsam. │ │ │ │ │ -  │ │ │ │ │ - string  ggttssaamm::::ccrreeaatteeRReewwrriitttteennFFiilleeNNaammee (const │ │ │ │ │ - std::string &name) │ │ │ │ │ - Creates a temporary file name that needs │ │ │ │ │ -  to be ignored in .gitingnore for checking │ │ │ │ │ - read-write oprations. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - map< size_t, T >  ggttssaamm::::ppaarrsseeTTooMMaapp (const string &filename, │ │ │ │ │ - Parser< pair< size_t, T > > parse, size_t │ │ │ │ │ - maxIndex) │ │ │ │ │ -  │ │ │ │ │ - boost::optional< _I_n_d_e_x_e_d_P_o_s_e >  _g_t_s_a_m_:_:_p_a_r_s_e_V_e_r_t_e_x_P_o_s_e (std::istream &is, │ │ │ │ │ - const std::string &tag) │ │ │ │ │ -  Parse TORO/G2O vertex "id x y yaw". │ │ │ │ │ -  │ │ │ │ │ - template<> │ │ │ │ │ -GTSAM_EXPORT std::map< size_t, _P_o_s_e_2 ggttssaamm::::ppaarrsseeVVaarriiaabblleess<< PPoossee22 >> (const │ │ │ │ │ - >  std::string &filename, size_t maxIndex) │ │ │ │ │ -  │ │ │ │ │ - boost::optional< IndexedLandmark >  _g_t_s_a_m_:_:_p_a_r_s_e_V_e_r_t_e_x_L_a_n_d_m_a_r_k (std::istream │ │ │ │ │ - &is, const std::string &tag) │ │ │ │ │ -  Parse G2O landmark vertex "id x y". │ │ │ │ │ -  │ │ │ │ │ - template<> │ │ │ │ │ - GTSAM_EXPORT std::map< size_t, ggttssaamm::::ppaarrsseeVVaarriiaabblleess<< PPooiinntt22 >> (const │ │ │ │ │ - _P_o_i_n_t_2 >  std::string &filename, size_t maxIndex) │ │ │ │ │ -  │ │ │ │ │ - boost::optional< IndexedEdge >  _g_t_s_a_m_:_:_p_a_r_s_e_E_d_g_e (std::istream &is, const │ │ │ │ │ - std::string &tag) │ │ │ │ │ -  Parse TORO/G2O edge "id1 id2 x y yaw". │ │ │ │ │ -  │ │ │ │ │ - boost::shared_ptr< _S_a_m_p_l_e_r >  ggttssaamm::::ccrreeaatteeSSaammpplleerr (const │ │ │ │ │ - _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model) │ │ │ │ │ -  │ │ │ │ │ - template<> │ │ │ │ │ - GTSAM_EXPORT std::vector< ggttssaamm::::ppaarrsseeMMeeaassuurreemmeennttss (const std:: │ │ │ │ │ - _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t< _P_o_s_e_2 > >  string &filename, const noiseModel:: │ │ │ │ │ - Diagonal::shared_ptr &model, size_t │ │ │ │ │ - maxIndex) │ │ │ │ │ -  │ │ │ │ │ - template<> │ │ │ │ │ - GTSAM_EXPORT std::vector< ggttssaamm::::ppaarrsseeMMeeaassuurreemmeennttss (const std:: │ │ │ │ │ - _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t< _R_o_t_2 > >  string &filename, const noiseModel:: │ │ │ │ │ - Diagonal::shared_ptr &model, size_t │ │ │ │ │ - maxIndex) │ │ │ │ │ -  │ │ │ │ │ - template<> │ │ │ │ │ - GTSAM_EXPORT std::vector< ggttssaamm::::ppaarrsseeFFaaccttoorrss<< PPoossee22 >> (const std:: │ │ │ │ │ - _B_e_t_w_e_e_n_F_a_c_t_o_r< _P_o_s_e_2 >::shared_ptr string &filename, const noiseModel:: │ │ │ │ │ - >  Diagonal::shared_ptr &model, size_t │ │ │ │ │ - maxIndex) │ │ │ │ │ -  │ │ │ │ │ - _G_r_a_p_h_A_n_d_V_a_l_u_e_s  _g_t_s_a_m_:_:_l_o_a_d_2_D (const std::string │ │ │ │ │ - &filename, _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ - model=_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l(), size_t │ │ │ │ │ - maxIndex=0, bool addNoise=false, bool │ │ │ │ │ - smart=true, _N_o_i_s_e_F_o_r_m_a_t │ │ │ │ │ - noiseFormat=_N_o_i_s_e_F_o_r_m_a_t_A_U_T_O, │ │ │ │ │ - _K_e_r_n_e_l_F_u_n_c_t_i_o_n_T_y_p_e │ │ │ │ │ - kernelFunctionType=KernelFunctionTypeNONE) │ │ │ │ │ -  Load TORO/G2O style graph files. │ │ │ │ │ -  │ │ │ │ │ - _G_r_a_p_h_A_n_d_V_a_l_u_e_s  _g_t_s_a_m_:_:_l_o_a_d_2_D (pair< string, │ │ │ │ │ - _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l > dataset, size_t │ │ │ │ │ - maxIndex, bool addNoise, bool smart, │ │ │ │ │ - _N_o_i_s_e_F_o_r_m_a_t noiseFormat, │ │ │ │ │ - _K_e_r_n_e_l_F_u_n_c_t_i_o_n_T_y_p_e kernelFunctionType) │ │ │ │ │ -  Load TORO 2D Graph. │ │ │ │ │ -  │ │ │ │ │ - _G_r_a_p_h_A_n_d_V_a_l_u_e_s  ggttssaamm::::llooaadd22DD__rroobbuusstt (const string │ │ │ │ │ - &filename, const noiseModel::Base:: │ │ │ │ │ - shared_ptr &model, size_t maxIndex) │ │ │ │ │ -  │ │ │ │ │ - void  ggttssaamm::::ssaavvee22DD (const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ - &graph, const _V_a_l_u_e_s &config, const │ │ │ │ │ - noiseModel::Diagonal::shared_ptr model, │ │ │ │ │ - const std::string &filename) │ │ │ │ │ -  save 2d graph │ │ │ │ │ -  │ │ │ │ │ - _G_r_a_p_h_A_n_d_V_a_l_u_e_s  _g_t_s_a_m_:_:_r_e_a_d_G_2_o (const std::string │ │ │ │ │ - &g2oFile, const bool is3D=false, │ │ │ │ │ - _K_e_r_n_e_l_F_u_n_c_t_i_o_n_T_y_p_e │ │ │ │ │ - kernelFunctionType=KernelFunctionTypeNONE) │ │ │ │ │ - This function parses a g2o file and stores │ │ │ │ │ -  the measurements into a │ │ │ │ │ - _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h and the initial guess │ │ │ │ │ - in a _V_a_l_u_e_s structure. │ │ │ │ │ -  │ │ │ │ │ - void  _g_t_s_a_m_:_:_w_r_i_t_e_G_2_o (const │ │ │ │ │ - _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h &graph, const _V_a_l_u_e_s │ │ │ │ │ - &estimate, const std::string &filename) │ │ │ │ │ - This function writes a g2o file from │ │ │ │ │ -  _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h and a _V_a_l_u_e_s │ │ │ │ │ - structure. │ │ │ │ │ -  │ │ │ │ │ - istream &  ggttssaamm::::ooppeerraattoorr>>>> (istream &is, Quaternion │ │ │ │ │ - &q) │ │ │ │ │ -  │ │ │ │ │ - istream &  ggttssaamm::::ooppeerraattoorr>>>> (istream &is, _R_o_t_3 &R) │ │ │ │ │ -  │ │ │ │ │ -boost::optional< pair< size_t, _P_o_s_e_3 ggttssaamm::::ppaarrsseeVVeerrtteexxPPoossee33 (istream &is, │ │ │ │ │ - > >  const string &tag) │ │ │ │ │ -  │ │ │ │ │ - template<> │ │ │ │ │ -GTSAM_EXPORT std::map< size_t, _P_o_s_e_3 ggttssaamm::::ppaarrsseeVVaarriiaabblleess<< PPoossee33 >> (const │ │ │ │ │ - >  std::string &filename, size_t maxIndex) │ │ │ │ │ -  │ │ │ │ │ - boost::optional< pair< size_t, ggttssaamm::::ppaarrsseeVVeerrtteexxPPooiinntt33 (istream &is, │ │ │ │ │ - _P_o_i_n_t_3 > >  const string &tag) │ │ │ │ │ -  │ │ │ │ │ - template<> │ │ │ │ │ - GTSAM_EXPORT std::map< size_t, ggttssaamm::::ppaarrsseeVVaarriiaabblleess<< PPooiinntt33 >> (const │ │ │ │ │ - _P_o_i_n_t_3 >  std::string &filename, size_t maxIndex) │ │ │ │ │ -  │ │ │ │ │ - istream &  ggttssaamm::::ooppeerraattoorr>>>> (istream &is, Matrix6 │ │ │ │ │ - &m) │ │ │ │ │ -  │ │ │ │ │ - template<> │ │ │ │ │ - GTSAM_EXPORT std::vector< ggttssaamm::::ppaarrsseeMMeeaassuurreemmeennttss (const std:: │ │ │ │ │ - _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t< _P_o_s_e_3 > >  string &filename, const noiseModel:: │ │ │ │ │ - Diagonal::shared_ptr &model, size_t │ │ │ │ │ - maxIndex) │ │ │ │ │ -  │ │ │ │ │ - template<> │ │ │ │ │ - GTSAM_EXPORT std::vector< ggttssaamm::::ppaarrsseeMMeeaassuurreemmeennttss (const std:: │ │ │ │ │ - _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t< _R_o_t_3 > >  string &filename, const noiseModel:: │ │ │ │ │ - Diagonal::shared_ptr &model, size_t │ │ │ │ │ - maxIndex) │ │ │ │ │ -  │ │ │ │ │ - template<> │ │ │ │ │ - GTSAM_EXPORT std::vector< ggttssaamm::::ppaarrsseeFFaaccttoorrss<< PPoossee33 >> (const std:: │ │ │ │ │ - _B_e_t_w_e_e_n_F_a_c_t_o_r< _P_o_s_e_3 >::shared_ptr string &filename, const noiseModel:: │ │ │ │ │ - >  Diagonal::shared_ptr &model, size_t │ │ │ │ │ - maxIndex) │ │ │ │ │ -  │ │ │ │ │ - _G_r_a_p_h_A_n_d_V_a_l_u_e_s  ggttssaamm::::llooaadd33DD (const std::string │ │ │ │ │ - &filename) │ │ │ │ │ -  Load TORO 3D Graph. │ │ │ │ │ -  │ │ │ │ │ - BetweenFactorPose2s  ggttssaamm::::ppaarrssee22DDFFaaccttoorrss (const std::string │ │ │ │ │ - &filename, const noiseModel::Diagonal:: │ │ │ │ │ - shared_ptr &model, size_t maxIndex) │ │ │ │ │ -  │ │ │ │ │ - BetweenFactorPose3s  ggttssaamm::::ppaarrssee33DDFFaaccttoorrss (const std::string │ │ │ │ │ - &filename, const noiseModel::Diagonal:: │ │ │ │ │ - shared_ptr &model, size_t maxIndex) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -utility functions for loading datasets │ │ │ │ │ +Implements a prior on the rotation component of a pose. │ │ │ │ │ Date │ │ │ │ │ - Jan 22, 2010 │ │ │ │ │ + Jun 14, 2012 │ │ │ │ │ Author │ │ │ │ │ - Kai Ni │ │ │ │ │ - Luca Carlone │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + Alex Cunningham │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _d_a_t_a_s_e_t_._c_p_p │ │ │ │ │ + * _P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01247.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,13 +1,3 @@ │ │ │ │ │ var a01247 = [ │ │ │ │ │ - ["createRewrittenFileName", "a01247.html#adbe09e573771fd72b08069353d537725", null], │ │ │ │ │ - ["findExampleDataFile", "a01247.html#aef971b2c69845e4fcce707306703c7b8", null], │ │ │ │ │ - ["load2D", "a01247.html#a635ec096b94adc2b05f081cd07eb2c0f", null], │ │ │ │ │ - ["load2D", "a01247.html#a79f6cde1c018ea0f4ca3c7fc83123a14", null], │ │ │ │ │ - ["load3D", "a01247.html#ac96511703a91071d7b2033976f9739c3", null], │ │ │ │ │ - ["parseEdge", "a01247.html#abc8bb9db0893652e8b5e7b47782ec990", null], │ │ │ │ │ - ["parseVertexLandmark", "a01247.html#a961e0398bbfc2504c62fdaad70472418", null], │ │ │ │ │ - ["parseVertexPose", "a01247.html#abfc0157e4790e3b03dc601d3e86c684d", null], │ │ │ │ │ - ["readG2o", "a01247.html#a71d086efc70f4af31d3c3b00dde4f7c0", null], │ │ │ │ │ - ["save2D", "a01247.html#acfe516bc2d9ca4cc4530dd3b8460ce3d", null], │ │ │ │ │ - ["writeG2o", "a01247.html#a68bc584d72da3747666ac134a9490f92", null] │ │ │ │ │ + ["gtsam::PoseRotationPrior< POSE >", "a04888.html", "a04888"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01250_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/SmartFactorParams.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/OrientedPlane3Factor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,157 +98,113 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
SmartFactorParams.h
│ │ │ │ +
OrientedPlane3Factor.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ -
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ -
4 * Atlanta, Georgia 30332-0415
│ │ │ │ -
5 * All Rights Reserved
│ │ │ │ -
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ -
7
│ │ │ │ -
8 * See LICENSE for the license information
│ │ │ │ -
9
│ │ │ │ -
10 * -------------------------------------------------------------------------- */
│ │ │ │ -
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/geometry/triangulation.h>
│ │ │ │ -
23
│ │ │ │ -
24namespace gtsam {
│ │ │ │ -
25
│ │ │ │ -
│ │ │ │ -
30enum LinearizationMode {
│ │ │ │ -
31 HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
│ │ │ │ -
32};
│ │ │ │ -
│ │ │ │ -
33
│ │ │ │ -
│ │ │ │ -
35enum DegeneracyMode {
│ │ │ │ -
36 IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY
│ │ │ │ -
37};
│ │ │ │ -
│ │ │ │ -
38
│ │ │ │ -
39/*
│ │ │ │ -
40 * Parameters for the smart (stereo) projection factors
│ │ │ │ -
41 */
│ │ │ │ -
│ │ │ │ -
42struct SmartProjectionParams {
│ │ │ │ +
1/*
│ │ │ │ +
2 * @file OrientedPlane3Factor.cpp
│ │ │ │ +
3 * @brief OrientedPlane3 Factor class
│ │ │ │ +
4 * @author Alex Trevor
│ │ │ │ +
5 * @date December 22, 2013
│ │ │ │ +
6 */
│ │ │ │ +
7
│ │ │ │ +
8#pragma once
│ │ │ │ +
9
│ │ │ │ +
10#include <gtsam/geometry/OrientedPlane3.h>
│ │ │ │ +
11#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
12
│ │ │ │ +
13namespace gtsam {
│ │ │ │ +
14
│ │ │ │ +
│ │ │ │ +
18class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactorN<Pose3, OrientedPlane3> {
│ │ │ │ +
19 protected:
│ │ │ │ +
20 OrientedPlane3 measured_p_;
│ │ │ │ +
21 typedef NoiseModelFactorN<Pose3, OrientedPlane3> Base;
│ │ │ │ +
22
│ │ │ │ +
23 public:
│ │ │ │ +
│ │ │ │ +
25 OrientedPlane3Factor() {
│ │ │ │ +
26 }
│ │ │ │ +
│ │ │ │ +
27 ~OrientedPlane3Factor() override {}
│ │ │ │ +
28
│ │ │ │ +
│ │ │ │ +
36 OrientedPlane3Factor(const Vector4& z, const SharedGaussian& noiseModel,
│ │ │ │ +
37 Key poseKey, Key landmarkKey)
│ │ │ │ +
38 : Base(noiseModel, poseKey, landmarkKey), measured_p_(z) {}
│ │ │ │ +
│ │ │ │ +
39
│ │ │ │ +
41 void print(const std::string& s = "OrientedPlane3Factor",
│ │ │ │ +
42 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
│ │ │ │
43
│ │ │ │ -
44 LinearizationMode linearizationMode;
│ │ │ │ -
45 DegeneracyMode degeneracyMode;
│ │ │ │ -
46
│ │ │ │ -
49 TriangulationParameters triangulation;
│ │ │ │ -
50 double retriangulationThreshold;
│ │ │ │ -
52
│ │ │ │ -
55 bool throwCheirality;
│ │ │ │ -
56 bool verboseCheirality;
│ │ │ │ -
58
│ │ │ │ -
59 // Constructor
│ │ │ │ -
60 SmartProjectionParams(LinearizationMode linMode = HESSIAN,
│ │ │ │ -
61 DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false,
│ │ │ │ -
62 bool verboseCheirality = false, double retriangulationTh = 1e-5) :
│ │ │ │ -
63 linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold(
│ │ │ │ -
64 retriangulationTh), throwCheirality(throwCheirality), verboseCheirality(
│ │ │ │ -
65 verboseCheirality) {
│ │ │ │ -
66 }
│ │ │ │ +
45 Vector evaluateError(
│ │ │ │ +
46 const Pose3& pose, const OrientedPlane3& plane,
│ │ │ │ +
47 boost::optional<Matrix&> H1 = boost::none,
│ │ │ │ +
48 boost::optional<Matrix&> H2 = boost::none) const override;
│ │ │ │ +
49};
│ │ │ │ +
│ │ │ │ +
50
│ │ │ │ +
51// TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior
│ │ │ │ +
│ │ │ │ +
52class GTSAM_EXPORT OrientedPlane3DirectionPrior : public NoiseModelFactorN<OrientedPlane3> {
│ │ │ │ +
53 protected:
│ │ │ │ +
54 OrientedPlane3 measured_p_;
│ │ │ │ +
55 typedef NoiseModelFactorN<OrientedPlane3> Base;
│ │ │ │ +
56
│ │ │ │ +
57 public:
│ │ │ │ +
58 typedef OrientedPlane3DirectionPrior This;
│ │ │ │ +
│ │ │ │ +
60 OrientedPlane3DirectionPrior() {
│ │ │ │ +
61 }
│ │ │ │ +
│ │ │ │ +
62
│ │ │ │ +
│ │ │ │ +
64 OrientedPlane3DirectionPrior(Key key, const Vector4& z,
│ │ │ │ +
65 const SharedGaussian& noiseModel)
│ │ │ │ +
66 : Base(noiseModel, key), measured_p_(z) {}
│ │ │ │ +
│ │ │ │
67
│ │ │ │ -
68 virtual ~SmartProjectionParams() {
│ │ │ │ -
69 }
│ │ │ │ -
70
│ │ │ │ -
71 void print(const std::string& str = "") const {
│ │ │ │ -
72 std::cout << "linearizationMode: " << linearizationMode << "\n";
│ │ │ │ -
73 std::cout << " degeneracyMode: " << degeneracyMode << "\n";
│ │ │ │ -
74 std::cout << triangulation << std::endl;
│ │ │ │ -
75 }
│ │ │ │ -
76
│ │ │ │ -
77 // get class variables
│ │ │ │ -
78 LinearizationMode getLinearizationMode() const {
│ │ │ │ -
79 return linearizationMode;
│ │ │ │ -
80 }
│ │ │ │ -
81 DegeneracyMode getDegeneracyMode() const {
│ │ │ │ -
82 return degeneracyMode;
│ │ │ │ -
83 }
│ │ │ │ -
84 TriangulationParameters getTriangulationParameters() const {
│ │ │ │ -
85 return triangulation;
│ │ │ │ -
86 }
│ │ │ │ -
87 bool getVerboseCheirality() const {
│ │ │ │ -
88 return verboseCheirality;
│ │ │ │ -
89 }
│ │ │ │ -
90 bool getThrowCheirality() const {
│ │ │ │ -
91 return throwCheirality;
│ │ │ │ -
92 }
│ │ │ │ -
93 double getRetriangulationThreshold() const {
│ │ │ │ -
94 return retriangulationThreshold;
│ │ │ │ -
95 }
│ │ │ │ -
96 // set class variables
│ │ │ │ -
97 void setLinearizationMode(LinearizationMode linMode) {
│ │ │ │ -
98 linearizationMode = linMode;
│ │ │ │ -
99 }
│ │ │ │ -
100 void setDegeneracyMode(DegeneracyMode degMode) {
│ │ │ │ -
101 degeneracyMode = degMode;
│ │ │ │ -
102 }
│ │ │ │ -
103 void setRetriangulationThreshold(double retriangulationTh) {
│ │ │ │ -
104 retriangulationThreshold = retriangulationTh;
│ │ │ │ -
105 }
│ │ │ │ -
106 void setRankTolerance(double rankTol) {
│ │ │ │ -
107 triangulation.rankTolerance = rankTol;
│ │ │ │ -
108 }
│ │ │ │ -
109 void setEnableEPI(bool enableEPI) {
│ │ │ │ -
110 triangulation.enableEPI = enableEPI;
│ │ │ │ -
111 }
│ │ │ │ -
112 void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) {
│ │ │ │ -
113 triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold;
│ │ │ │ -
114 }
│ │ │ │ -
115 void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) {
│ │ │ │ -
116 triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold;
│ │ │ │ -
117 }
│ │ │ │ -
118
│ │ │ │ -
119private:
│ │ │ │ -
120
│ │ │ │ -
122 friend class boost::serialization::access;
│ │ │ │ -
123 template<class ARCHIVE>
│ │ │ │ -
124 void serialize(ARCHIVE & ar, const unsigned int version) {
│ │ │ │ -
125 ar & BOOST_SERIALIZATION_NVP(linearizationMode);
│ │ │ │ -
126 ar & BOOST_SERIALIZATION_NVP(degeneracyMode);
│ │ │ │ -
127 ar & BOOST_SERIALIZATION_NVP(triangulation);
│ │ │ │ -
128 ar & BOOST_SERIALIZATION_NVP(retriangulationThreshold);
│ │ │ │ -
129 ar & BOOST_SERIALIZATION_NVP(throwCheirality);
│ │ │ │ -
130 ar & BOOST_SERIALIZATION_NVP(verboseCheirality);
│ │ │ │ -
131 }
│ │ │ │ -
132};
│ │ │ │ -
│ │ │ │ -
133
│ │ │ │ -
134} // \ namespace gtsam
│ │ │ │ -
triangulation.h
Functions for triangulation.
│ │ │ │ +
69 void print(const std::string& s = "OrientedPlane3DirectionPrior",
│ │ │ │ +
70 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
│ │ │ │ +
71
│ │ │ │ +
73 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
│ │ │ │ +
74
│ │ │ │ +
75 Vector evaluateError(const OrientedPlane3& plane,
│ │ │ │ +
76 boost::optional<Matrix&> H = boost::none) const override;
│ │ │ │ +
77};
│ │ │ │ +
│ │ │ │ +
78
│ │ │ │ +
79} // gtsam
│ │ │ │ +
80
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::DegeneracyMode
DegeneracyMode
How to manage degeneracy.
Definition SmartFactorParams.h:35
│ │ │ │ -
gtsam::LinearizationMode
LinearizationMode
SmartFactorParams: parameters and (linearization/degeneracy) modes for SmartProjection and SmartStere...
Definition SmartFactorParams.h:30
│ │ │ │ -
gtsam::TriangulationParameters
Definition triangulation.h:556
│ │ │ │ -
gtsam::TriangulationParameters::dynamicOutlierRejectionThreshold
double dynamicOutlierRejectionThreshold
If this is nonnegative the we will check if the average reprojection error is smaller than this thres...
Definition triangulation.h:573
│ │ │ │ -
gtsam::TriangulationParameters::rankTolerance
double rankTolerance
threshold to decide whether triangulation is result.degenerate
Definition triangulation.h:558
│ │ │ │ -
gtsam::TriangulationParameters::landmarkDistanceThreshold
double landmarkDistanceThreshold
if the landmark is triangulated at distance larger than this, result is flagged as degenerate.
Definition triangulation.h:566
│ │ │ │ -
gtsam::TriangulationParameters::enableEPI
bool enableEPI
if set to true, will refine triangulation using LM
Definition triangulation.h:560
│ │ │ │ -
gtsam::SmartProjectionParams
Definition SmartFactorParams.h:42
│ │ │ │ -
gtsam::SmartProjectionParams::throwCheirality
bool throwCheirality
If true, re-throws Cheirality exceptions (default: false)
Definition SmartFactorParams.h:55
│ │ │ │ -
gtsam::SmartProjectionParams::linearizationMode
LinearizationMode linearizationMode
How to linearize the factor.
Definition SmartFactorParams.h:44
│ │ │ │ -
gtsam::SmartProjectionParams::retriangulationThreshold
double retriangulationThreshold
threshold to decide whether to re-triangulate
Definition SmartFactorParams.h:50
│ │ │ │ -
gtsam::SmartProjectionParams::degeneracyMode
DegeneracyMode degeneracyMode
How to linearize the factor.
Definition SmartFactorParams.h:45
│ │ │ │ -
gtsam::SmartProjectionParams::verboseCheirality
bool verboseCheirality
If true, prints text for Cheirality exceptions (default: false)
Definition SmartFactorParams.h:56
│ │ │ │ -
gtsam::SmartProjectionParams::access
friend class boost::serialization::access
Serialization function.
Definition SmartFactorParams.h:122
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::OrientedPlane3
Represents an infinite plane in 3D, which is composed of a planar normal and its perpendicular distan...
Definition OrientedPlane3.h:36
│ │ │ │ +
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ +
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ +
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ +
gtsam::OrientedPlane3Factor
Factor to measure a planar landmark from a given pose.
Definition OrientedPlane3Factor.h:18
│ │ │ │ +
gtsam::OrientedPlane3Factor::OrientedPlane3Factor
OrientedPlane3Factor(const Vector4 &z, const SharedGaussian &noiseModel, Key poseKey, Key landmarkKey)
Constructor with measured plane (a,b,c,d) coefficients.
Definition OrientedPlane3Factor.h:36
│ │ │ │ +
gtsam::OrientedPlane3Factor::OrientedPlane3Factor
OrientedPlane3Factor()
Constructor.
Definition OrientedPlane3Factor.h:25
│ │ │ │ +
gtsam::OrientedPlane3DirectionPrior
Definition OrientedPlane3Factor.h:52
│ │ │ │ +
gtsam::OrientedPlane3DirectionPrior::OrientedPlane3DirectionPrior
OrientedPlane3DirectionPrior(Key key, const Vector4 &z, const SharedGaussian &noiseModel)
Constructor with measured plane coefficients (a,b,c,d), noise model, landmark symbol.
Definition OrientedPlane3Factor.h:64
│ │ │ │ +
gtsam::OrientedPlane3DirectionPrior::Base
NoiseModelFactorN< OrientedPlane3 > Base
measured plane parameters
Definition OrientedPlane3Factor.h:55
│ │ │ │ +
gtsam::OrientedPlane3DirectionPrior::OrientedPlane3DirectionPrior
OrientedPlane3DirectionPrior()
Constructor.
Definition OrientedPlane3Factor.h:60
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,188 +1,139 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SmartFactorParams.h │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -1/* --------------------------------------------------------------------------- │ │ │ │ │ -- │ │ │ │ │ -2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ -4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -5 * All Rights Reserved │ │ │ │ │ -6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +OrientedPlane3Factor.h │ │ │ │ │ +1/* │ │ │ │ │ +2 * @file OrientedPlane3Factor.cpp │ │ │ │ │ +3 * @brief OrientedPlane3 Factor class │ │ │ │ │ +4 * @author Alex Trevor │ │ │ │ │ +5 * @date December 22, 2013 │ │ │ │ │ +6 */ │ │ │ │ │ 7 │ │ │ │ │ -8 * See LICENSE for the license information │ │ │ │ │ +8#pragma once │ │ │ │ │ 9 │ │ │ │ │ -10 * ------------------------------------------------------------------------- │ │ │ │ │ -- */ │ │ │ │ │ -11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_t_r_i_a_n_g_u_l_a_t_i_o_n_._h> │ │ │ │ │ -23 │ │ │ │ │ -24namespace _g_t_s_a_m { │ │ │ │ │ -25 │ │ │ │ │ -_3_0enum _L_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e { │ │ │ │ │ -31 HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD │ │ │ │ │ -32}; │ │ │ │ │ -33 │ │ │ │ │ -_3_5enum _D_e_g_e_n_e_r_a_c_y_M_o_d_e { │ │ │ │ │ -36 IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY │ │ │ │ │ -37}; │ │ │ │ │ -38 │ │ │ │ │ -39/* │ │ │ │ │ -40 * Parameters for the smart (stereo) projection factors │ │ │ │ │ -41 */ │ │ │ │ │ -_4_2struct _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s { │ │ │ │ │ +10#include │ │ │ │ │ +11#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +12 │ │ │ │ │ +13namespace _g_t_s_a_m { │ │ │ │ │ +14 │ │ │ │ │ +_1_8class GTSAM_EXPORT _O_r_i_e_n_t_e_d_P_l_a_n_e_3_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +19 protected: │ │ │ │ │ +20 _O_r_i_e_n_t_e_d_P_l_a_n_e_3 measured_p_; │ │ │ │ │ +21 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_P_o_s_e_3_,_ _O_r_i_e_n_t_e_d_P_l_a_n_e_3_> _B_a_s_e; │ │ │ │ │ +22 │ │ │ │ │ +23 public: │ │ │ │ │ +_2_5 _O_r_i_e_n_t_e_d_P_l_a_n_e_3_F_a_c_t_o_r() { │ │ │ │ │ +26 } │ │ │ │ │ +27 _~_O_r_i_e_n_t_e_d_P_l_a_n_e_3_F_a_c_t_o_r() override {} │ │ │ │ │ +28 │ │ │ │ │ +_3_6 _O_r_i_e_n_t_e_d_P_l_a_n_e_3_F_a_c_t_o_r(const Vector4& z, const SharedGaussian& noiseModel, │ │ │ │ │ +37 _K_e_y poseKey, _K_e_y landmarkKey) │ │ │ │ │ +38 : _B_a_s_e(noiseModel, poseKey, landmarkKey), measured_p_(z) {} │ │ │ │ │ +39 │ │ │ │ │ +41 void _p_r_i_n_t(const std::string& s = "OrientedPlane3Factor", │ │ │ │ │ +42 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override; │ │ │ │ │ 43 │ │ │ │ │ -_4_4 _L_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e _l_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e; │ │ │ │ │ -_4_5 _D_e_g_e_n_e_r_a_c_y_M_o_d_e _d_e_g_e_n_e_r_a_c_y_M_o_d_e; │ │ │ │ │ -46 │ │ │ │ │ -49 _T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s triangulation; │ │ │ │ │ -_5_0 double _r_e_t_r_i_a_n_g_u_l_a_t_i_o_n_T_h_r_e_s_h_o_l_d; │ │ │ │ │ -52 │ │ │ │ │ -_5_5 bool _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y; │ │ │ │ │ -_5_6 bool _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y; │ │ │ │ │ -58 │ │ │ │ │ -59 // Constructor │ │ │ │ │ -60 _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s(_L_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e linMode = HESSIAN, │ │ │ │ │ -61 _D_e_g_e_n_e_r_a_c_y_M_o_d_e degMode = IGNORE_DEGENERACY, bool _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y = false, │ │ │ │ │ -62 bool _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y = false, double retriangulationTh = 1e-5) : │ │ │ │ │ -63 _l_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e(linMode), _d_e_g_e_n_e_r_a_c_y_M_o_d_e(degMode), │ │ │ │ │ -_r_e_t_r_i_a_n_g_u_l_a_t_i_o_n_T_h_r_e_s_h_o_l_d( │ │ │ │ │ -64 retriangulationTh), _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y(_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y), _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y( │ │ │ │ │ -65 _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y) { │ │ │ │ │ -66 } │ │ │ │ │ +45 Vector evaluateError( │ │ │ │ │ +46 const _P_o_s_e_3& pose, const _O_r_i_e_n_t_e_d_P_l_a_n_e_3& plane, │ │ │ │ │ +47 boost::optional H1 = boost::none, │ │ │ │ │ +48 boost::optional H2 = boost::none) const override; │ │ │ │ │ +49}; │ │ │ │ │ +50 │ │ │ │ │ +51// TODO: Convert this factor to dimension two, three dimensions is redundant │ │ │ │ │ +for direction prior │ │ │ │ │ +_5_2class GTSAM_EXPORT _O_r_i_e_n_t_e_d_P_l_a_n_e_3_D_i_r_e_c_t_i_o_n_P_r_i_o_r : public │ │ │ │ │ +_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +53 protected: │ │ │ │ │ +54 _O_r_i_e_n_t_e_d_P_l_a_n_e_3 measured_p_; │ │ │ │ │ +_5_5 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_O_r_i_e_n_t_e_d_P_l_a_n_e_3_> _B_a_s_e; │ │ │ │ │ +56 │ │ │ │ │ +57 public: │ │ │ │ │ +58 typedef _O_r_i_e_n_t_e_d_P_l_a_n_e_3_D_i_r_e_c_t_i_o_n_P_r_i_o_r _T_h_i_s; │ │ │ │ │ +_6_0 _O_r_i_e_n_t_e_d_P_l_a_n_e_3_D_i_r_e_c_t_i_o_n_P_r_i_o_r() { │ │ │ │ │ +61 } │ │ │ │ │ +62 │ │ │ │ │ +_6_4 _O_r_i_e_n_t_e_d_P_l_a_n_e_3_D_i_r_e_c_t_i_o_n_P_r_i_o_r(_K_e_y key, const Vector4& z, │ │ │ │ │ +65 const SharedGaussian& noiseModel) │ │ │ │ │ +66 : _B_a_s_e(noiseModel, key), measured_p_(z) {} │ │ │ │ │ 67 │ │ │ │ │ -68 virtual _~_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s() { │ │ │ │ │ -69 } │ │ │ │ │ -70 │ │ │ │ │ -71 void print(const std::string& str = "") const { │ │ │ │ │ -72 std::cout << "linearizationMode: " << _l_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e << "\n"; │ │ │ │ │ -73 std::cout << " degeneracyMode: " << _d_e_g_e_n_e_r_a_c_y_M_o_d_e << "\n"; │ │ │ │ │ -74 std::cout << triangulation << std::endl; │ │ │ │ │ -75 } │ │ │ │ │ -76 │ │ │ │ │ -77 // get class variables │ │ │ │ │ -78 _L_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e getLinearizationMode() const { │ │ │ │ │ -79 return _l_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e; │ │ │ │ │ -80 } │ │ │ │ │ -81 _D_e_g_e_n_e_r_a_c_y_M_o_d_e getDegeneracyMode() const { │ │ │ │ │ -82 return _d_e_g_e_n_e_r_a_c_y_M_o_d_e; │ │ │ │ │ -83 } │ │ │ │ │ -84 TriangulationParameters getTriangulationParameters() const { │ │ │ │ │ -85 return triangulation; │ │ │ │ │ -86 } │ │ │ │ │ -87 bool getVerboseCheirality() const { │ │ │ │ │ -88 return _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y; │ │ │ │ │ -89 } │ │ │ │ │ -90 bool getThrowCheirality() const { │ │ │ │ │ -91 return _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y; │ │ │ │ │ -92 } │ │ │ │ │ -93 double getRetriangulationThreshold() const { │ │ │ │ │ -94 return _r_e_t_r_i_a_n_g_u_l_a_t_i_o_n_T_h_r_e_s_h_o_l_d; │ │ │ │ │ -95 } │ │ │ │ │ -96 // set class variables │ │ │ │ │ -97 void setLinearizationMode(_L_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e linMode) { │ │ │ │ │ -98 _l_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e = linMode; │ │ │ │ │ -99 } │ │ │ │ │ -100 void setDegeneracyMode(_D_e_g_e_n_e_r_a_c_y_M_o_d_e degMode) { │ │ │ │ │ -101 _d_e_g_e_n_e_r_a_c_y_M_o_d_e = degMode; │ │ │ │ │ -102 } │ │ │ │ │ -103 void setRetriangulationThreshold(double retriangulationTh) { │ │ │ │ │ -104 _r_e_t_r_i_a_n_g_u_l_a_t_i_o_n_T_h_r_e_s_h_o_l_d = retriangulationTh; │ │ │ │ │ -105 } │ │ │ │ │ -106 void setRankTolerance(double rankTol) { │ │ │ │ │ -107 triangulation._r_a_n_k_T_o_l_e_r_a_n_c_e = rankTol; │ │ │ │ │ -108 } │ │ │ │ │ -109 void setEnableEPI(bool enableEPI) { │ │ │ │ │ -110 triangulation._e_n_a_b_l_e_E_P_I = enableEPI; │ │ │ │ │ -111 } │ │ │ │ │ -112 void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) { │ │ │ │ │ -113 triangulation._l_a_n_d_m_a_r_k_D_i_s_t_a_n_c_e_T_h_r_e_s_h_o_l_d = landmarkDistanceThreshold; │ │ │ │ │ -114 } │ │ │ │ │ -115 void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) { │ │ │ │ │ -116 triangulation._d_y_n_a_m_i_c_O_u_t_l_i_e_r_R_e_j_e_c_t_i_o_n_T_h_r_e_s_h_o_l_d = dynOutRejectionThreshold; │ │ │ │ │ -117 } │ │ │ │ │ -118 │ │ │ │ │ -119private: │ │ │ │ │ -120 │ │ │ │ │ -_1_2_2 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -123 template │ │ │ │ │ -124 void serialize(ARCHIVE & ar, const unsigned int version) { │ │ │ │ │ -125 ar & BOOST_SERIALIZATION_NVP(_l_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e); │ │ │ │ │ -126 ar & BOOST_SERIALIZATION_NVP(_d_e_g_e_n_e_r_a_c_y_M_o_d_e); │ │ │ │ │ -127 ar & BOOST_SERIALIZATION_NVP(triangulation); │ │ │ │ │ -128 ar & BOOST_SERIALIZATION_NVP(_r_e_t_r_i_a_n_g_u_l_a_t_i_o_n_T_h_r_e_s_h_o_l_d); │ │ │ │ │ -129 ar & BOOST_SERIALIZATION_NVP(_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y); │ │ │ │ │ -130 ar & BOOST_SERIALIZATION_NVP(_v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y); │ │ │ │ │ -131 } │ │ │ │ │ -132}; │ │ │ │ │ -133 │ │ │ │ │ -134} // \ namespace gtsam │ │ │ │ │ -_t_r_i_a_n_g_u_l_a_t_i_o_n_._h │ │ │ │ │ -Functions for triangulation. │ │ │ │ │ +69 void _p_r_i_n_t(const std::string& s = "OrientedPlane3DirectionPrior", │ │ │ │ │ +70 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override; │ │ │ │ │ +71 │ │ │ │ │ +73 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& expected, double tol = 1e-9) const │ │ │ │ │ +override; │ │ │ │ │ +74 │ │ │ │ │ +75 Vector evaluateError(const _O_r_i_e_n_t_e_d_P_l_a_n_e_3& plane, │ │ │ │ │ +76 boost::optional H = boost::none) const override; │ │ │ │ │ +77}; │ │ │ │ │ +78 │ │ │ │ │ +79} // gtsam │ │ │ │ │ +80 │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ +Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_g_e_n_e_r_a_c_y_M_o_d_e │ │ │ │ │ -DegeneracyMode │ │ │ │ │ -How to manage degeneracy. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorParams.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e │ │ │ │ │ -LinearizationMode │ │ │ │ │ -SmartFactorParams: parameters and (linearization/degeneracy) modes for │ │ │ │ │ -SmartProjection and SmartStere... │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorParams.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:556 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s_:_:_d_y_n_a_m_i_c_O_u_t_l_i_e_r_R_e_j_e_c_t_i_o_n_T_h_r_e_s_h_o_l_d │ │ │ │ │ -double dynamicOutlierRejectionThreshold │ │ │ │ │ -If this is nonnegative the we will check if the average reprojection error is │ │ │ │ │ -smaller than this thres... │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:573 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s_:_:_r_a_n_k_T_o_l_e_r_a_n_c_e │ │ │ │ │ -double rankTolerance │ │ │ │ │ -threshold to decide whether triangulation is result.degenerate │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:558 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s_:_:_l_a_n_d_m_a_r_k_D_i_s_t_a_n_c_e_T_h_r_e_s_h_o_l_d │ │ │ │ │ -double landmarkDistanceThreshold │ │ │ │ │ -if the landmark is triangulated at distance larger than this, result is flagged │ │ │ │ │ -as degenerate. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:566 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s_:_:_e_n_a_b_l_e_E_P_I │ │ │ │ │ -bool enableEPI │ │ │ │ │ -if set to true, will refine triangulation using LM │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:560 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorParams.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s_:_:_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y │ │ │ │ │ -bool throwCheirality │ │ │ │ │ -If true, re-throws Cheirality exceptions (default: false) │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorParams.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s_:_:_l_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e │ │ │ │ │ -LinearizationMode linearizationMode │ │ │ │ │ -How to linearize the factor. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorParams.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s_:_:_r_e_t_r_i_a_n_g_u_l_a_t_i_o_n_T_h_r_e_s_h_o_l_d │ │ │ │ │ -double retriangulationThreshold │ │ │ │ │ -threshold to decide whether to re-triangulate │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorParams.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s_:_:_d_e_g_e_n_e_r_a_c_y_M_o_d_e │ │ │ │ │ -DegeneracyMode degeneracyMode │ │ │ │ │ -How to linearize the factor. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorParams.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s_:_:_v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y │ │ │ │ │ -bool verboseCheirality │ │ │ │ │ -If true, prints text for Cheirality exceptions (default: false) │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorParams.h:56 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorParams.h:122 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3 │ │ │ │ │ +Represents an infinite plane in 3D, which is composed of a planar normal and │ │ │ │ │ +its perpendicular distan... │ │ │ │ │ +DDeeffiinniittiioonn OrientedPlane3.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ +A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ +Nonlinear factor base class. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ +A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ +variables. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_F_a_c_t_o_r │ │ │ │ │ +Factor to measure a planar landmark from a given pose. │ │ │ │ │ +DDeeffiinniittiioonn OrientedPlane3Factor.h:18 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_F_a_c_t_o_r_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_F_a_c_t_o_r │ │ │ │ │ +OrientedPlane3Factor(const Vector4 &z, const SharedGaussian &noiseModel, Key │ │ │ │ │ +poseKey, Key landmarkKey) │ │ │ │ │ +Constructor with measured plane (a,b,c,d) coefficients. │ │ │ │ │ +DDeeffiinniittiioonn OrientedPlane3Factor.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_F_a_c_t_o_r_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_F_a_c_t_o_r │ │ │ │ │ +OrientedPlane3Factor() │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn OrientedPlane3Factor.h:25 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_D_i_r_e_c_t_i_o_n_P_r_i_o_r │ │ │ │ │ +DDeeffiinniittiioonn OrientedPlane3Factor.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_D_i_r_e_c_t_i_o_n_P_r_i_o_r_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_D_i_r_e_c_t_i_o_n_P_r_i_o_r │ │ │ │ │ +OrientedPlane3DirectionPrior(Key key, const Vector4 &z, const SharedGaussian │ │ │ │ │ +&noiseModel) │ │ │ │ │ +Constructor with measured plane coefficients (a,b,c,d), noise model, landmark │ │ │ │ │ +symbol. │ │ │ │ │ +DDeeffiinniittiioonn OrientedPlane3Factor.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_D_i_r_e_c_t_i_o_n_P_r_i_o_r_:_:_B_a_s_e │ │ │ │ │ +NoiseModelFactorN< OrientedPlane3 > Base │ │ │ │ │ +measured plane parameters │ │ │ │ │ +DDeeffiinniittiioonn OrientedPlane3Factor.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_D_i_r_e_c_t_i_o_n_P_r_i_o_r_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_D_i_r_e_c_t_i_o_n_P_r_i_o_r │ │ │ │ │ +OrientedPlane3DirectionPrior() │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn OrientedPlane3Factor.h:60 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _S_m_a_r_t_F_a_c_t_o_r_P_a_r_a_m_s_._h │ │ │ │ │ + * OOrriieenntteeddPPllaannee33FFaaccttoorr..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01253.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/SmartProjectionRigFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/AntiFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,52 +96,38 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
SmartProjectionRigFactor.h File Reference
│ │ │ │ +
AntiFactor.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Smart factor on poses, assuming camera calibration is fixed. │ │ │ │ -More...

│ │ │ │ - │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::SmartProjectionRigFactor< CAMERA >
 If you are using the factor, please cite: L. More...
 
struct  gtsam::traits< SmartProjectionRigFactor< CAMERA > >
 traits More...
class  gtsam::AntiFactor
 A class for downdating an existing factor from a graph. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Smart factor on poses, assuming camera calibration is fixed.

│ │ │ │ -

Same as SmartProjectionPoseFactor, except:

    │ │ │ │ -
  • it is templated on CAMERA (i.e., it allows cameras beyond pinhole)
    • │ │ │ │ -
  • it admits a different calibration for each measurement (i.e., it can model a multi-camera rig system)
    • │ │ │ │ -
  • it allows multiple observations from the same pose/key (again, to model a multi-camera system)
    Author
    Luca Carlone
    │ │ │ │ -
    │ │ │ │ -Frank Dellaert
    │ │ │ │ -
    • │ │ │ │ -
│ │ │ │ +
Author
Stephen Williams
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,35 +1,23 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SmartProjectionRigFactor.h File Reference │ │ │ │ │ -Smart factor on poses, assuming camera calibration is fixed. _M_o_r_e_._._. │ │ │ │ │ +AntiFactor.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_<_ _C_A_M_E_R_A_ _> │ │ │ │ │ -  If you are using the factor, please cite: L. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_<_ _C_A_M_E_R_A_ _>_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_A_n_t_i_F_a_c_t_o_r │ │ │ │ │ +  A class for downdating an existing factor from a graph. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Smart factor on poses, assuming camera calibration is fixed. │ │ │ │ │ -Same as SmartProjectionPoseFactor, except: │ │ │ │ │ - * it is templated on CAMERA (i.e., it allows cameras beyond pinhole) │ │ │ │ │ - * it admits a different calibration for each measurement (i.e., it can │ │ │ │ │ - model a multi-camera rig system) │ │ │ │ │ - * it allows multiple observations from the same pose/key (again, to model a │ │ │ │ │ - multi-camera system) │ │ │ │ │ - Author │ │ │ │ │ - Luca Carlone │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Author │ │ │ │ │ + Stephen Williams │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_._h │ │ │ │ │ + * _A_n_t_i_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01253_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/SmartProjectionRigFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/AntiFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,366 +98,134 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
SmartProjectionRigFactor.h
│ │ │ │ +
AntiFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
25#pragma once
│ │ │ │ -
26
│ │ │ │ -
27#include <gtsam/slam/SmartProjectionFactor.h>
│ │ │ │ -
28
│ │ │ │ -
29namespace gtsam {
│ │ │ │ -
51template <class CAMERA>
│ │ │ │ -
│ │ │ │ -
52class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
│ │ │ │ -
53 private:
│ │ │ │ -
54 typedef SmartProjectionFactor<CAMERA> Base;
│ │ │ │ -
55 typedef SmartProjectionRigFactor<CAMERA> This;
│ │ │ │ -
56 typedef typename CAMERA::CalibrationType CALIBRATION;
│ │ │ │ -
57 typedef typename CAMERA::Measurement MEASUREMENT;
│ │ │ │ -
58 typedef typename CAMERA::MeasurementVector MEASUREMENTS;
│ │ │ │ -
59
│ │ │ │ -
60 static const int DimPose = 6;
│ │ │ │ -
61 static const int ZDim = 2;
│ │ │ │ -
62
│ │ │ │ -
63 protected:
│ │ │ │ -
65 KeyVector nonUniqueKeys_;
│ │ │ │ +
16#pragma once
│ │ │ │ +
17
│ │ │ │ +
18#include <ostream>
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
21#include <gtsam/linear/GaussianFactor.h>
│ │ │ │ +
22
│ │ │ │ +
23namespace gtsam {
│ │ │ │ +
24
│ │ │ │ +
│ │ │ │ +
31 class AntiFactor: public NonlinearFactor {
│ │ │ │ +
32
│ │ │ │ +
33 private:
│ │ │ │ +
34
│ │ │ │ +
35 typedef AntiFactor This;
│ │ │ │ +
36 typedef NonlinearFactor Base;
│ │ │ │ +
37 typedef NonlinearFactor::shared_ptr sharedFactor;
│ │ │ │ +
38
│ │ │ │ +
39 sharedFactor factor_;
│ │ │ │ +
40
│ │ │ │ +
41 public:
│ │ │ │ +
42
│ │ │ │ +
43 // shorthand for a smart pointer to a factor
│ │ │ │ +
44 typedef boost::shared_ptr<AntiFactor> shared_ptr;
│ │ │ │ +
45
│ │ │ │ +
47 AntiFactor() {}
│ │ │ │ +
48
│ │ │ │ +
50 AntiFactor(NonlinearFactor::shared_ptr factor) : Base(factor->keys()), factor_(factor) {}
│ │ │ │ +
51
│ │ │ │ +
52 ~AntiFactor() override {}
│ │ │ │ +
53
│ │ │ │ +
│ │ │ │ +
55 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
56 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ +
57 gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
│ │ │ │ +
│ │ │ │ +
58
│ │ │ │ +
│ │ │ │ +
62 void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ +
63 std::cout << s << "AntiFactor version of:" << std::endl;
│ │ │ │ +
64 factor_->print(s, keyFormatter);
│ │ │ │ +
65 }
│ │ │ │ +
│ │ │ │
66
│ │ │ │ -
68 boost::shared_ptr<typename Base::Cameras> cameraRig_;
│ │ │ │ -
69
│ │ │ │ -
72 FastVector<size_t> cameraIds_;
│ │ │ │ -
73
│ │ │ │ -
74 public:
│ │ │ │ -
75 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
76
│ │ │ │ -
77 typedef CAMERA Camera;
│ │ │ │ -
78 typedef CameraSet<CAMERA> Cameras;
│ │ │ │ -
79
│ │ │ │ -
81 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
82
│ │ │ │ -
84 SmartProjectionRigFactor() {}
│ │ │ │ -
85
│ │ │ │ -
│ │ │ │ -
94 SmartProjectionRigFactor(
│ │ │ │ -
95 const SharedNoiseModel& sharedNoiseModel,
│ │ │ │ -
96 const boost::shared_ptr<Cameras>& cameraRig,
│ │ │ │ -
97 const SmartProjectionParams& params = SmartProjectionParams())
│ │ │ │ -
98 : Base(sharedNoiseModel, params), cameraRig_(cameraRig) {
│ │ │ │ -
99 // throw exception if configuration is not supported by this factor
│ │ │ │ -
100 if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY)
│ │ │ │ -
101 throw std::runtime_error(
│ │ │ │ -
102 "SmartProjectionRigFactor: "
│ │ │ │ -
103 "degeneracyMode must be set to ZERO_ON_DEGENERACY");
│ │ │ │ -
104 if (Base::params_.linearizationMode != gtsam::HESSIAN)
│ │ │ │ -
105 throw std::runtime_error(
│ │ │ │ -
106 "SmartProjectionRigFactor: "
│ │ │ │ -
107 "linearizationMode must be set to HESSIAN");
│ │ │ │ -
108 }
│ │ │ │ -
│ │ │ │ -
109
│ │ │ │ -
111 ~SmartProjectionRigFactor() override = default;
│ │ │ │ -
112
│ │ │ │ -
│ │ │ │ -
123 void add(const MEASUREMENT& measured, const Key& poseKey,
│ │ │ │ -
124 const size_t& cameraId = 0) {
│ │ │ │ -
125 // store measurement and key
│ │ │ │ -
126 this->measured_.push_back(measured);
│ │ │ │ -
127 this->nonUniqueKeys_.push_back(poseKey);
│ │ │ │ -
128
│ │ │ │ -
129 // also store keys in the keys_ vector: these keys are assumed to be
│ │ │ │ -
130 // unique, so we avoid duplicates here
│ │ │ │ -
131 if (std::find(this->keys_.begin(), this->keys_.end(), poseKey) ==
│ │ │ │ -
132 this->keys_.end())
│ │ │ │ -
133 this->keys_.push_back(poseKey); // add only unique keys
│ │ │ │ -
134
│ │ │ │ -
135 // store id of the camera taking the measurement
│ │ │ │ -
136 cameraIds_.push_back(cameraId);
│ │ │ │ -
137 }
│ │ │ │ -
│ │ │ │ -
138
│ │ │ │ -
│ │ │ │ -
149 void add(const MEASUREMENTS& measurements, const KeyVector& poseKeys,
│ │ │ │ -
150 const FastVector<size_t>& cameraIds = FastVector<size_t>()) {
│ │ │ │ -
151 if (poseKeys.size() != measurements.size() ||
│ │ │ │ -
152 (poseKeys.size() != cameraIds.size() && cameraIds.size() != 0)) {
│ │ │ │ -
153 throw std::runtime_error(
│ │ │ │ -
154 "SmartProjectionRigFactor: "
│ │ │ │ -
155 "trying to add inconsistent inputs");
│ │ │ │ -
156 }
│ │ │ │ -
157 if (cameraIds.size() == 0 && cameraRig_->size() > 1) {
│ │ │ │ -
158 throw std::runtime_error(
│ │ │ │ -
159 "SmartProjectionRigFactor: "
│ │ │ │ -
160 "camera rig includes multiple camera "
│ │ │ │ -
161 "but add did not input cameraIds");
│ │ │ │ -
162 }
│ │ │ │ -
163 for (size_t i = 0; i < measurements.size(); i++) {
│ │ │ │ -
164 add(measurements[i], poseKeys[i],
│ │ │ │ -
165 cameraIds.size() == 0 ? 0 : cameraIds[i]);
│ │ │ │ -
166 }
│ │ │ │ -
167 }
│ │ │ │ -
│ │ │ │ -
168
│ │ │ │ -
171 const KeyVector& nonUniqueKeys() const { return nonUniqueKeys_; }
│ │ │ │ -
172
│ │ │ │ -
174 const boost::shared_ptr<Cameras>& cameraRig() const { return cameraRig_; }
│ │ │ │ -
175
│ │ │ │ -
177 const FastVector<size_t>& cameraIds() const { return cameraIds_; }
│ │ │ │ -
178
│ │ │ │ -
│ │ │ │ -
184 void print(
│ │ │ │ -
185 const std::string& s = "",
│ │ │ │ -
186 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ -
187 std::cout << s << "SmartProjectionRigFactor: \n ";
│ │ │ │ -
188 for (size_t i = 0; i < nonUniqueKeys_.size(); i++) {
│ │ │ │ -
189 std::cout << "-- Measurement nr " << i << std::endl;
│ │ │ │ -
190 std::cout << "key: " << keyFormatter(nonUniqueKeys_[i]) << std::endl;
│ │ │ │ -
191 std::cout << "cameraId: " << cameraIds_[i] << std::endl;
│ │ │ │ -
192 (*cameraRig_)[cameraIds_[i]].print("camera in rig:\n");
│ │ │ │ -
193 }
│ │ │ │ -
194 Base::print("", keyFormatter);
│ │ │ │ -
195 }
│ │ │ │ -
│ │ │ │ -
196
│ │ │ │ -
│ │ │ │ -
198 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
│ │ │ │ -
199 const This* e = dynamic_cast<const This*>(&p);
│ │ │ │ -
200 return e && Base::equals(p, tol) && nonUniqueKeys_ == e->nonUniqueKeys() &&
│ │ │ │ -
201 cameraRig_->equals(*(e->cameraRig())) &&
│ │ │ │ -
202 std::equal(cameraIds_.begin(), cameraIds_.end(),
│ │ │ │ -
203 e->cameraIds().begin());
│ │ │ │ -
204 }
│ │ │ │ -
│ │ │ │ -
205
│ │ │ │ -
│ │ │ │ -
212 typename Base::Cameras cameras(const Values& values) const override {
│ │ │ │ -
213 typename Base::Cameras cameras;
│ │ │ │ -
214 cameras.reserve(nonUniqueKeys_.size()); // preallocate
│ │ │ │ -
215 for (size_t i = 0; i < nonUniqueKeys_.size(); i++) {
│ │ │ │ -
216 const typename Base::Camera& camera_i = (*cameraRig_)[cameraIds_[i]];
│ │ │ │ -
217 const Pose3 world_P_sensor_i =
│ │ │ │ -
218 values.at<Pose3>(nonUniqueKeys_[i]) // = world_P_body
│ │ │ │ -
219 * camera_i.pose(); // = body_P_cam_i
│ │ │ │ -
220 cameras.emplace_back(world_P_sensor_i,
│ │ │ │ -
221 make_shared<typename CAMERA::CalibrationType>(
│ │ │ │ -
222 camera_i.calibration()));
│ │ │ │ -
223 }
│ │ │ │ -
224 return cameras;
│ │ │ │ -
225 }
│ │ │ │ -
│ │ │ │ -
226
│ │ │ │ -
│ │ │ │ -
230 double error(const Values& values) const override {
│ │ │ │ -
231 if (this->active(values)) {
│ │ │ │ -
232 return this->totalReprojectionError(this->cameras(values));
│ │ │ │ -
233 } else { // else of active flag
│ │ │ │ -
234 return 0.0;
│ │ │ │ -
235 }
│ │ │ │ -
236 }
│ │ │ │ -
│ │ │ │ -
237
│ │ │ │ -
│ │ │ │ -
247 void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks& Fs,
│ │ │ │ -
248 Matrix& E, Vector& b,
│ │ │ │ -
249 const Cameras& cameras) const {
│ │ │ │ -
250 if (!this->result_) {
│ │ │ │ -
251 throw("computeJacobiansWithTriangulatedPoint");
│ │ │ │ -
252 } else { // valid result: compute jacobians
│ │ │ │ -
253 b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E);
│ │ │ │ -
254 for (size_t i = 0; i < Fs.size(); i++) {
│ │ │ │ -
255 const Pose3& body_P_sensor = (*cameraRig_)[cameraIds_[i]].pose();
│ │ │ │ -
256 const Pose3 world_P_body = cameras[i].pose() * body_P_sensor.inverse();
│ │ │ │ -
257 Eigen::Matrix<double, DimPose, DimPose> H;
│ │ │ │ -
258 world_P_body.compose(body_P_sensor, H);
│ │ │ │ -
259 Fs.at(i) = Fs.at(i) * H;
│ │ │ │ -
260 }
│ │ │ │ -
261 }
│ │ │ │ -
262 }
│ │ │ │ -
│ │ │ │ -
263
│ │ │ │ -
│ │ │ │ -
265 boost::shared_ptr<RegularHessianFactor<DimPose> > createHessianFactor(
│ │ │ │ -
266 const Values& values, const double& lambda = 0.0,
│ │ │ │ -
267 bool diagonalDamping = false) const {
│ │ │ │ -
268 // we may have multiple observation sharing the same keys (e.g., 2 cameras
│ │ │ │ -
269 // measuring from the same body pose), hence the number of unique keys may
│ │ │ │ -
270 // be smaller than nrMeasurements
│ │ │ │ -
271 size_t nrUniqueKeys =
│ │ │ │ -
272 this->keys_
│ │ │ │ -
273 .size(); // note: by construction, keys_ only contains unique keys
│ │ │ │ -
274
│ │ │ │ -
275 Cameras cameras = this->cameras(values);
│ │ │ │ -
276
│ │ │ │ -
277 // Create structures for Hessian Factors
│ │ │ │ -
278 std::vector<size_t> js;
│ │ │ │ -
279 std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
│ │ │ │ -
280 std::vector<Vector> gs(nrUniqueKeys);
│ │ │ │ -
281
│ │ │ │ -
282 if (this->measured_.size() != cameras.size()) // 1 observation per camera
│ │ │ │ -
283 throw std::runtime_error(
│ │ │ │ -
284 "SmartProjectionRigFactor: "
│ │ │ │ -
285 "measured_.size() inconsistent with input");
│ │ │ │ -
286
│ │ │ │ -
287 // triangulate 3D point at given linearization point
│ │ │ │ -
288 this->triangulateSafe(cameras);
│ │ │ │ -
289
│ │ │ │ -
290 if (!this->result_) { // failed: return "empty/zero" Hessian
│ │ │ │ -
291 if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) {
│ │ │ │ -
292 for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose);
│ │ │ │ -
293 for (Vector& v : gs) v = Vector::Zero(DimPose);
│ │ │ │ -
294 return boost::make_shared<RegularHessianFactor<DimPose> >(this->keys_,
│ │ │ │ -
295 Gs, gs, 0.0);
│ │ │ │ -
296 } else {
│ │ │ │ -
297 throw std::runtime_error(
│ │ │ │ -
298 "SmartProjectionRigFactor: "
│ │ │ │ -
299 "only supported degeneracy mode is ZERO_ON_DEGENERACY");
│ │ │ │ -
300 }
│ │ │ │ -
301 }
│ │ │ │ -
302
│ │ │ │ -
303 // compute Jacobian given triangulated 3D Point
│ │ │ │ -
304 typename Base::FBlocks Fs;
│ │ │ │ -
305 Matrix E;
│ │ │ │ -
306 Vector b;
│ │ │ │ -
307 this->computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
│ │ │ │ -
308
│ │ │ │ -
309 // Whiten using noise model
│ │ │ │ -
310 this->noiseModel_->WhitenSystem(E, b);
│ │ │ │ -
311 for (size_t i = 0; i < Fs.size(); i++) {
│ │ │ │ -
312 Fs[i] = this->noiseModel_->Whiten(Fs[i]);
│ │ │ │ -
313 }
│ │ │ │ -
314
│ │ │ │ -
315 const Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping);
│ │ │ │ -
316
│ │ │ │ -
317 // Build augmented Hessian (with last row/column being the information
│ │ │ │ -
318 // vector) Note: we need to get the augumented hessian wrt the unique keys
│ │ │ │ -
319 // in key_
│ │ │ │ -
320 SymmetricBlockMatrix augmentedHessianUniqueKeys =
│ │ │ │ -
321 Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 6, 6>(
│ │ │ │ -
322 Fs, E, P, b, nonUniqueKeys_, this->keys_);
│ │ │ │ -
323
│ │ │ │ -
324 return boost::make_shared<RegularHessianFactor<DimPose> >(
│ │ │ │ -
325 this->keys_, augmentedHessianUniqueKeys);
│ │ │ │ -
326 }
│ │ │ │ -
│ │ │ │ -
327
│ │ │ │ -
│ │ │ │ -
335 boost::shared_ptr<GaussianFactor> linearizeDamped(
│ │ │ │ -
336 const Values& values, const double& lambda = 0.0) const {
│ │ │ │ -
337 // depending on flag set on construction we may linearize to different
│ │ │ │ -
338 // linear factors
│ │ │ │ -
339 switch (this->params_.linearizationMode) {
│ │ │ │ -
340 case HESSIAN:
│ │ │ │ -
341 return this->createHessianFactor(values, lambda);
│ │ │ │ -
342 default:
│ │ │ │ -
343 throw std::runtime_error(
│ │ │ │ -
344 "SmartProjectionRigFactor: unknown linearization mode");
│ │ │ │ -
345 }
│ │ │ │ -
346 }
│ │ │ │ -
│ │ │ │ -
347
│ │ │ │ -
│ │ │ │ -
349 boost::shared_ptr<GaussianFactor> linearize(
│ │ │ │ -
350 const Values& values) const override {
│ │ │ │ -
351 return this->linearizeDamped(values);
│ │ │ │ -
352 }
│ │ │ │ -
│ │ │ │ -
353
│ │ │ │ -
354 private:
│ │ │ │ -
356 friend class boost::serialization::access;
│ │ │ │ -
357 template <class ARCHIVE>
│ │ │ │ -
358 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ -
359 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
360 // ar& BOOST_SERIALIZATION_NVP(nonUniqueKeys_);
│ │ │ │ -
361 // ar& BOOST_SERIALIZATION_NVP(cameraRig_);
│ │ │ │ -
362 // ar& BOOST_SERIALIZATION_NVP(cameraIds_);
│ │ │ │ -
363 }
│ │ │ │ -
364};
│ │ │ │ -
│ │ │ │ -
365// end of class declaration
│ │ │ │ -
366
│ │ │ │ -
368template <class CAMERA>
│ │ │ │ -
│ │ │ │ -
369struct traits<SmartProjectionRigFactor<CAMERA> >
│ │ │ │ -
370 : public Testable<SmartProjectionRigFactor<CAMERA> > {};
│ │ │ │ -
│ │ │ │ -
371
│ │ │ │ -
372} // namespace gtsam
│ │ │ │ -
SmartProjectionFactor.h
Smart factor on cameras (pose + calibration)
│ │ │ │ -
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │ +
│ │ │ │ +
68 bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
│ │ │ │ +
69 const This *e = dynamic_cast<const This*> (&expected);
│ │ │ │ +
70 return e != nullptr && Base::equals(*e, tol) && this->factor_->equals(*e->factor_, tol);
│ │ │ │ +
71 }
│ │ │ │ +
│ │ │ │ +
72
│ │ │ │ +
80 double error(const Values& c) const override { return -factor_->error(c); }
│ │ │ │ +
81
│ │ │ │ +
83 size_t dim() const override { return factor_->dim(); }
│ │ │ │ +
84
│ │ │ │ +
89 bool active(const Values& c) const override { return factor_->active(c); }
│ │ │ │ +
90
│ │ │ │ +
│ │ │ │ +
97 boost::shared_ptr<GaussianFactor> linearize(const Values& c) const override {
│ │ │ │ +
98
│ │ │ │ +
99 // Generate the linearized factor from the contained nonlinear factor
│ │ │ │ +
100 GaussianFactor::shared_ptr gaussianFactor = factor_->linearize(c);
│ │ │ │ +
101
│ │ │ │ +
102 // return the negated version of the factor
│ │ │ │ +
103 return gaussianFactor->negate();
│ │ │ │ +
104 }
│ │ │ │ +
│ │ │ │ +
105
│ │ │ │ +
106
│ │ │ │ +
107 private:
│ │ │ │ +
108
│ │ │ │ +
110 friend class boost::serialization::access;
│ │ │ │ +
111 template<class ARCHIVE>
│ │ │ │ +
112 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
113 ar & boost::serialization::make_nvp("AntiFactor",
│ │ │ │ +
114 boost::serialization::base_object<Base>(*this));
│ │ │ │ +
115 ar & BOOST_SERIALIZATION_NVP(factor_);
│ │ │ │ +
116 }
│ │ │ │ +
117 }; // \class AntiFactor
│ │ │ │ +
│ │ │ │ +
118
│ │ │ │ +
119}
│ │ │ │ +
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition CameraSet.h:36
│ │ │ │ -
gtsam::CameraSet::reprojectionError
Vector reprojectionError(const POINT &point, const ZVector &measured, boost::optional< FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const
Calculate vector [project2(point)-z] of re-projection errors.
Definition CameraSet.h:136
│ │ │ │ -
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ -
gtsam::Pose3::inverse
Pose3 inverse() const
inverse transformation with derivatives
Definition Pose3.cpp:49
│ │ │ │ -
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
│ │ │ │ +
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
│ │ │ │ +
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactor.h:42
│ │ │ │
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ -
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition NonlinearFactor.h:118
│ │ │ │ +
gtsam::NonlinearFactor::equals
virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const
Check if two factors are equal.
Definition NonlinearFactor.cpp:47
│ │ │ │
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
gtsam::Values::at
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
│ │ │ │ -
gtsam::SmartFactorBase::noiseModel_
SharedIsotropic noiseModel_
As of Feb 22, 2015, the noise model is the same for all measurements and is isotropic.
Definition SmartFactorBase.h:72
│ │ │ │ -
gtsam::SmartFactorBase::measured_
ZVector measured_
Measurements for each of the m views.
Definition SmartFactorBase.h:79
│ │ │ │ -
gtsam::SmartFactorBase::measured
const ZVector & measured() const
Return the 2D measurements (ZDim, in general).
Definition SmartFactorBase.h:159
│ │ │ │ -
gtsam::SmartFactorBase::PointCov
static Matrix PointCov(const Matrix &E)
Computes Point Covariance P from the "point Jacobian" E.
Definition SmartFactorBase.h:274
│ │ │ │ -
gtsam::SmartProjectionParams
Definition SmartFactorParams.h:42
│ │ │ │ -
gtsam::SmartProjectionParams::linearizationMode
LinearizationMode linearizationMode
How to linearize the factor.
Definition SmartFactorParams.h:44
│ │ │ │ -
gtsam::SmartProjectionParams::degeneracyMode
DegeneracyMode degeneracyMode
How to linearize the factor.
Definition SmartFactorParams.h:45
│ │ │ │ -
gtsam::SmartProjectionFactor
SmartProjectionFactor: triangulates point and keeps an estimate of it around.
Definition SmartProjectionFactor.h:45
│ │ │ │ -
gtsam::SmartProjectionFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition SmartProjectionFactor.h:103
│ │ │ │ -
gtsam::SmartProjectionFactor::result_
TriangulationResult result_
result from triangulateSafe
Definition SmartProjectionFactor.h:63
│ │ │ │ -
gtsam::SmartProjectionFactor::triangulateSafe
TriangulationResult triangulateSafe(const Cameras &cameras) const
Call gtsam::triangulateSafe iff we need to re-triangulate.
Definition SmartProjectionFactor.h:174
│ │ │ │ -
gtsam::SmartProjectionFactor::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, boost::optional< Point3 > externalPoint=boost::none) const
Calculate the error of the factor.
Definition SmartProjectionFactor.h:411
│ │ │ │ -
gtsam::SmartProjectionFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition SmartProjectionFactor.h:115
│ │ │ │ -
gtsam::SmartProjectionFactor::Camera
CAMERA Camera
shorthand for a set of cameras
Definition SmartProjectionFactor.h:74
│ │ │ │ -
gtsam::SmartProjectionRigFactor
If you are using the factor, please cite: L.
Definition SmartProjectionRigFactor.h:52
│ │ │ │ -
gtsam::SmartProjectionRigFactor::nonUniqueKeys_
KeyVector nonUniqueKeys_
vector of keys (one for each observation) with potentially repeated keys
Definition SmartProjectionRigFactor.h:65
│ │ │ │ -
gtsam::SmartProjectionRigFactor::cameraIds_
FastVector< size_t > cameraIds_
vector of camera Ids (one for each observation, in the same order), identifying which camera took the...
Definition SmartProjectionRigFactor.h:72
│ │ │ │ -
gtsam::SmartProjectionRigFactor::cameraIds
const FastVector< size_t > & cameraIds() const
return the calibration object
Definition SmartProjectionRigFactor.h:177
│ │ │ │ -
gtsam::SmartProjectionRigFactor::createHessianFactor
boost::shared_ptr< RegularHessianFactor< DimPose > > createHessianFactor(const Values &values, const double &lambda=0.0, bool diagonalDamping=false) const
linearize and return a Hessianfactor that is an approximation of error(p)
Definition SmartProjectionRigFactor.h:265
│ │ │ │ -
gtsam::SmartProjectionRigFactor::add
void add(const MEASUREMENT &measured, const Key &poseKey, const size_t &cameraId=0)
add a new measurement, corresponding to an observation from pose "poseKey" and taken from the camera ...
Definition SmartProjectionRigFactor.h:123
│ │ │ │ -
gtsam::SmartProjectionRigFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition SmartProjectionRigFactor.h:81
│ │ │ │ -
gtsam::SmartProjectionRigFactor::computeJacobiansWithTriangulatedPoint
void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras) const
Compute jacobian F, E and error vector at a given linearization point.
Definition SmartProjectionRigFactor.h:247
│ │ │ │ -
gtsam::SmartProjectionRigFactor::add
void add(const MEASUREMENTS &measurements, const KeyVector &poseKeys, const FastVector< size_t > &cameraIds=FastVector< size_t >())
Variant of the previous "add" function in which we include multiple measurements.
Definition SmartProjectionRigFactor.h:149
│ │ │ │ -
gtsam::SmartProjectionRigFactor::SmartProjectionRigFactor
SmartProjectionRigFactor()
Default constructor, only for serialization.
Definition SmartProjectionRigFactor.h:84
│ │ │ │ -
gtsam::SmartProjectionRigFactor::error
double error(const Values &values) const override
error calculates the error of the factor.
Definition SmartProjectionRigFactor.h:230
│ │ │ │ -
gtsam::SmartProjectionRigFactor::cameras
Base::Cameras cameras(const Values &values) const override
Collect all cameras involved in this factor.
Definition SmartProjectionRigFactor.h:212
│ │ │ │ -
gtsam::SmartProjectionRigFactor::~SmartProjectionRigFactor
~SmartProjectionRigFactor() override=default
Virtual destructor.
│ │ │ │ -
gtsam::SmartProjectionRigFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition SmartProjectionRigFactor.h:184
│ │ │ │ -
gtsam::SmartProjectionRigFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &values) const override
linearize
Definition SmartProjectionRigFactor.h:349
│ │ │ │ -
gtsam::SmartProjectionRigFactor::access
friend class boost::serialization::access
Serialization function.
Definition SmartProjectionRigFactor.h:356
│ │ │ │ -
gtsam::SmartProjectionRigFactor::nonUniqueKeys
const KeyVector & nonUniqueKeys() const
return (for each observation) the (possibly non unique) keys involved in the measurements
Definition SmartProjectionRigFactor.h:171
│ │ │ │ -
gtsam::SmartProjectionRigFactor::cameraRig_
boost::shared_ptr< typename Base::Cameras > cameraRig_
cameras in the rig (fixed poses wrt body and intrinsics, for each camera)
Definition SmartProjectionRigFactor.h:68
│ │ │ │ -
gtsam::SmartProjectionRigFactor::SmartProjectionRigFactor
SmartProjectionRigFactor(const SharedNoiseModel &sharedNoiseModel, const boost::shared_ptr< Cameras > &cameraRig, const SmartProjectionParams &params=SmartProjectionParams())
Constructor.
Definition SmartProjectionRigFactor.h:94
│ │ │ │ -
gtsam::SmartProjectionRigFactor::cameraRig
const boost::shared_ptr< Cameras > & cameraRig() const
return the calibration object
Definition SmartProjectionRigFactor.h:174
│ │ │ │ -
gtsam::SmartProjectionRigFactor::linearizeDamped
boost::shared_ptr< GaussianFactor > linearizeDamped(const Values &values, const double &lambda=0.0) const
Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM)
Definition SmartProjectionRigFactor.h:335
│ │ │ │ -
gtsam::SmartProjectionRigFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition SmartProjectionRigFactor.h:198
│ │ │ │ +
gtsam::AntiFactor
A class for downdating an existing factor from a graph.
Definition AntiFactor.h:31
│ │ │ │ +
gtsam::AntiFactor::AntiFactor
AntiFactor(NonlinearFactor::shared_ptr factor)
constructor - Creates the equivalent AntiFactor from an existing factor
Definition AntiFactor.h:50
│ │ │ │ +
gtsam::AntiFactor::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
equals
Definition AntiFactor.h:68
│ │ │ │ +
gtsam::AntiFactor::AntiFactor
AntiFactor()
default constructor - only use for serialization
Definition AntiFactor.h:47
│ │ │ │ +
gtsam::AntiFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &c) const override
Linearize to a GaussianFactor.
Definition AntiFactor.h:97
│ │ │ │ +
gtsam::AntiFactor::active
bool active(const Values &c) const override
Checks whether this factor should be used based on a set of values.
Definition AntiFactor.h:89
│ │ │ │ +
gtsam::AntiFactor::dim
size_t dim() const override
get the dimension of the factor (same as the original factor)
Definition AntiFactor.h:83
│ │ │ │ +
gtsam::AntiFactor::print
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
implement functions needed for Testable
Definition AntiFactor.h:62
│ │ │ │ +
gtsam::AntiFactor::error
double error(const Values &c) const override
implement functions needed to derive from Factor
Definition AntiFactor.h:80
│ │ │ │ +
gtsam::AntiFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition AntiFactor.h:55
│ │ │ │ +
gtsam::AntiFactor::access
friend class boost::serialization::access
Serialization function.
Definition AntiFactor.h:110
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,503 +1,173 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SmartProjectionRigFactor.h │ │ │ │ │ +AntiFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -25#pragma once │ │ │ │ │ -26 │ │ │ │ │ -27#include <_g_t_s_a_m_/_s_l_a_m_/_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_._h> │ │ │ │ │ -28 │ │ │ │ │ -29namespace _g_t_s_a_m { │ │ │ │ │ -51template │ │ │ │ │ -_5_2class _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r : public _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r { │ │ │ │ │ -53 private: │ │ │ │ │ -54 typedef _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_<_C_A_M_E_R_A_> _B_a_s_e; │ │ │ │ │ -55 typedef _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_<_C_A_M_E_R_A_> This; │ │ │ │ │ -56 typedef typename CAMERA::CalibrationType CALIBRATION; │ │ │ │ │ -57 typedef typename CAMERA::Measurement MEASUREMENT; │ │ │ │ │ -58 typedef typename CAMERA::MeasurementVector MEASUREMENTS; │ │ │ │ │ -59 │ │ │ │ │ -60 static const int DimPose = 6; │ │ │ │ │ -61 static const int ZDim = 2; │ │ │ │ │ -62 │ │ │ │ │ -63 protected: │ │ │ │ │ -_6_5 _K_e_y_V_e_c_t_o_r _n_o_n_U_n_i_q_u_e_K_e_y_s__; │ │ │ │ │ +16#pragma once │ │ │ │ │ +17 │ │ │ │ │ +18#include │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ +22 │ │ │ │ │ +23namespace _g_t_s_a_m { │ │ │ │ │ +24 │ │ │ │ │ +_3_1 class _A_n_t_i_F_a_c_t_o_r: public _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r { │ │ │ │ │ +32 │ │ │ │ │ +33 private: │ │ │ │ │ +34 │ │ │ │ │ +35 typedef _A_n_t_i_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ +36 typedef _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r _B_a_s_e; │ │ │ │ │ +37 typedef NonlinearFactor::shared_ptr sharedFactor; │ │ │ │ │ +38 │ │ │ │ │ +39 sharedFactor factor_; │ │ │ │ │ +40 │ │ │ │ │ +41 public: │ │ │ │ │ +42 │ │ │ │ │ +43 // shorthand for a smart pointer to a factor │ │ │ │ │ +44 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +45 │ │ │ │ │ +_4_7 _A_n_t_i_F_a_c_t_o_r() {} │ │ │ │ │ +48 │ │ │ │ │ +_5_0 _A_n_t_i_F_a_c_t_o_r(NonlinearFactor::shared_ptr factor) : _B_a_s_e(factor->_k_e_y_s()), │ │ │ │ │ +factor_(factor) {} │ │ │ │ │ +51 │ │ │ │ │ +52 _~_A_n_t_i_F_a_c_t_o_r() override {} │ │ │ │ │ +53 │ │ │ │ │ +_5_5 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +56 return boost::static_pointer_cast( │ │ │ │ │ +57 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); } │ │ │ │ │ +58 │ │ │ │ │ +_6_2 void _p_r_i_n_t(const std::string& s, const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ +DefaultKeyFormatter) const override { │ │ │ │ │ +63 std::cout << s << "AntiFactor version of:" << std::endl; │ │ │ │ │ +64 factor_->print(s, keyFormatter); │ │ │ │ │ +65 } │ │ │ │ │ 66 │ │ │ │ │ -_6_8 boost::shared_ptr _c_a_m_e_r_a_R_i_g__; │ │ │ │ │ -69 │ │ │ │ │ -_7_2 _F_a_s_t_V_e_c_t_o_r_<_s_i_z_e___t_> _c_a_m_e_r_a_I_d_s__; │ │ │ │ │ -73 │ │ │ │ │ -74 public: │ │ │ │ │ -75 EIGEN_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ -76 │ │ │ │ │ -77 typedef CAMERA _C_a_m_e_r_a; │ │ │ │ │ -78 typedef _C_a_m_e_r_a_S_e_t_<_C_A_M_E_R_A_> _C_a_m_e_r_a_s; │ │ │ │ │ -79 │ │ │ │ │ -_8_1 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -82 │ │ │ │ │ -_8_4 _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r() {} │ │ │ │ │ -85 │ │ │ │ │ -_9_4 _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r( │ │ │ │ │ -95 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& sharedNoiseModel, │ │ │ │ │ -96 const boost::shared_ptr& _c_a_m_e_r_a_R_i_g, │ │ │ │ │ -97 const _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s& params = _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s()) │ │ │ │ │ -98 : _B_a_s_e(sharedNoiseModel, params), _c_a_m_e_r_a_R_i_g__(_c_a_m_e_r_a_R_i_g) { │ │ │ │ │ -99 // throw exception if configuration is not supported by this factor │ │ │ │ │ -100 if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY) │ │ │ │ │ -101 throw std::runtime_error( │ │ │ │ │ -102 "SmartProjectionRigFactor: " │ │ │ │ │ -103 "degeneracyMode must be set to ZERO_ON_DEGENERACY"); │ │ │ │ │ -104 if (Base::params_.linearizationMode != gtsam::HESSIAN) │ │ │ │ │ -105 throw std::runtime_error( │ │ │ │ │ -106 "SmartProjectionRigFactor: " │ │ │ │ │ -107 "linearizationMode must be set to HESSIAN"); │ │ │ │ │ -108 } │ │ │ │ │ -109 │ │ │ │ │ -_1_1_1 _~_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r() override = default; │ │ │ │ │ -112 │ │ │ │ │ -_1_2_3 void _a_d_d(const MEASUREMENT& _m_e_a_s_u_r_e_d, const _K_e_y& poseKey, │ │ │ │ │ -124 const size_t& cameraId = 0) { │ │ │ │ │ -125 // store measurement and key │ │ │ │ │ -126 this->_m_e_a_s_u_r_e_d__.push_back(measured); │ │ │ │ │ -127 this->nonUniqueKeys_.push_back(poseKey); │ │ │ │ │ -128 │ │ │ │ │ -129 // also store keys in the keys_ vector: these keys are assumed to be │ │ │ │ │ -130 // unique, so we avoid duplicates here │ │ │ │ │ -131 if (std::find(this->_k_e_y_s__.begin(), this->keys_.end(), poseKey) == │ │ │ │ │ -132 this->keys_.end()) │ │ │ │ │ -133 this->_k_e_y_s__.push_back(poseKey); // add only unique keys │ │ │ │ │ -134 │ │ │ │ │ -135 // store id of the camera taking the measurement │ │ │ │ │ -136 _c_a_m_e_r_a_I_d_s__.push_back(cameraId); │ │ │ │ │ -137 } │ │ │ │ │ -138 │ │ │ │ │ -_1_4_9 void _a_d_d(const MEASUREMENTS& measurements, const _K_e_y_V_e_c_t_o_r& poseKeys, │ │ │ │ │ -150 const _F_a_s_t_V_e_c_t_o_r_<_s_i_z_e___t_>& _c_a_m_e_r_a_I_d_s = _F_a_s_t_V_e_c_t_o_r_<_s_i_z_e___t_>()) { │ │ │ │ │ -151 if (poseKeys.size() != measurements.size() || │ │ │ │ │ -152 (poseKeys.size() != _c_a_m_e_r_a_I_d_s.size() && _c_a_m_e_r_a_I_d_s.size() != 0)) { │ │ │ │ │ -153 throw std::runtime_error( │ │ │ │ │ -154 "SmartProjectionRigFactor: " │ │ │ │ │ -155 "trying to add inconsistent inputs"); │ │ │ │ │ -156 } │ │ │ │ │ -157 if (_c_a_m_e_r_a_I_d_s.size() == 0 && _c_a_m_e_r_a_R_i_g__->size() > 1) { │ │ │ │ │ -158 throw std::runtime_error( │ │ │ │ │ -159 "SmartProjectionRigFactor: " │ │ │ │ │ -160 "camera rig includes multiple camera " │ │ │ │ │ -161 "but add did not input cameraIds"); │ │ │ │ │ -162 } │ │ │ │ │ -163 for (size_t i = 0; i < measurements.size(); i++) { │ │ │ │ │ -164 _a_d_d(measurements[i], poseKeys[i], │ │ │ │ │ -165 _c_a_m_e_r_a_I_d_s.size() == 0 ? 0 : _c_a_m_e_r_a_I_d_s[i]); │ │ │ │ │ -166 } │ │ │ │ │ -167 } │ │ │ │ │ -168 │ │ │ │ │ -_1_7_1 const _K_e_y_V_e_c_t_o_r& _n_o_n_U_n_i_q_u_e_K_e_y_s() const { return _n_o_n_U_n_i_q_u_e_K_e_y_s__; } │ │ │ │ │ -172 │ │ │ │ │ -_1_7_4 const boost::shared_ptr& _c_a_m_e_r_a_R_i_g() const { return _c_a_m_e_r_a_R_i_g__; } │ │ │ │ │ -175 │ │ │ │ │ -_1_7_7 const _F_a_s_t_V_e_c_t_o_r_<_s_i_z_e___t_>& _c_a_m_e_r_a_I_d_s() const { return _c_a_m_e_r_a_I_d_s__; } │ │ │ │ │ -178 │ │ │ │ │ -_1_8_4 void _p_r_i_n_t( │ │ │ │ │ -185 const std::string& s = "", │ │ │ │ │ -186 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ -187 std::cout << s << "SmartProjectionRigFactor: \n "; │ │ │ │ │ -188 for (size_t i = 0; i < _n_o_n_U_n_i_q_u_e_K_e_y_s__.size(); i++) { │ │ │ │ │ -189 std::cout << "-- Measurement nr " << i << std::endl; │ │ │ │ │ -190 std::cout << "key: " << keyFormatter(_n_o_n_U_n_i_q_u_e_K_e_y_s__[i]) << std::endl; │ │ │ │ │ -191 std::cout << "cameraId: " << _c_a_m_e_r_a_I_d_s__[i] << std::endl; │ │ │ │ │ -192 (*cameraRig_)[_c_a_m_e_r_a_I_d_s__[i]].print("camera in rig:\n"); │ │ │ │ │ -193 } │ │ │ │ │ -194 _B_a_s_e_:_:_p_r_i_n_t("", keyFormatter); │ │ │ │ │ -195 } │ │ │ │ │ -196 │ │ │ │ │ -_1_9_8 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& p, double tol = 1e-9) const override { │ │ │ │ │ -199 const This* e = dynamic_cast(&p); │ │ │ │ │ -200 return e && _B_a_s_e_:_:_e_q_u_a_l_s(p, tol) && _n_o_n_U_n_i_q_u_e_K_e_y_s__ == e->nonUniqueKeys() && │ │ │ │ │ -201 _c_a_m_e_r_a_R_i_g__->equals(*(e->cameraRig())) && │ │ │ │ │ -202 std::equal(_c_a_m_e_r_a_I_d_s__.begin(), _c_a_m_e_r_a_I_d_s__.end(), │ │ │ │ │ -203 e->cameraIds().begin()); │ │ │ │ │ -204 } │ │ │ │ │ -205 │ │ │ │ │ -_2_1_2 typename _B_a_s_e_:_:_C_a_m_e_r_a_s _c_a_m_e_r_a_s(const _V_a_l_u_e_s& values) const override { │ │ │ │ │ -213 typename _B_a_s_e_:_:_C_a_m_e_r_a_s _c_a_m_e_r_a_s; │ │ │ │ │ -214 _c_a_m_e_r_a_s.reserve(_n_o_n_U_n_i_q_u_e_K_e_y_s__.size()); // preallocate │ │ │ │ │ -215 for (size_t i = 0; i < _n_o_n_U_n_i_q_u_e_K_e_y_s__.size(); i++) { │ │ │ │ │ -216 const typename _B_a_s_e_:_:_C_a_m_e_r_a& camera_i = (*cameraRig_)[_c_a_m_e_r_a_I_d_s__[i]]; │ │ │ │ │ -217 const _P_o_s_e_3 world_P_sensor_i = │ │ │ │ │ -218 values._a_t<_P_o_s_e_3>(_n_o_n_U_n_i_q_u_e_K_e_y_s__[i]) // = world_P_body │ │ │ │ │ -219 * camera_i.pose(); // = body_P_cam_i │ │ │ │ │ -220 _c_a_m_e_r_a_s.emplace_back(world_P_sensor_i, │ │ │ │ │ -221 make_shared( │ │ │ │ │ -222 camera_i.calibration())); │ │ │ │ │ -223 } │ │ │ │ │ -224 return _c_a_m_e_r_a_s; │ │ │ │ │ -225 } │ │ │ │ │ -226 │ │ │ │ │ -_2_3_0 double _e_r_r_o_r(const _V_a_l_u_e_s& values) const override { │ │ │ │ │ -231 if (this->_a_c_t_i_v_e(values)) { │ │ │ │ │ -232 return this->_t_o_t_a_l_R_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r(this->_c_a_m_e_r_a_s(values)); │ │ │ │ │ -233 } else { // else of active flag │ │ │ │ │ -234 return 0.0; │ │ │ │ │ -235 } │ │ │ │ │ -236 } │ │ │ │ │ -237 │ │ │ │ │ -_2_4_7 void _c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s_W_i_t_h_T_r_i_a_n_g_u_l_a_t_e_d_P_o_i_n_t(typename Base::FBlocks& Fs, │ │ │ │ │ -248 Matrix& E, Vector& b, │ │ │ │ │ -249 const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s) const { │ │ │ │ │ -250 if (!this->_r_e_s_u_l_t__) { │ │ │ │ │ -251 throw("computeJacobiansWithTriangulatedPoint"); │ │ │ │ │ -252 } else { // valid result: compute jacobians │ │ │ │ │ -253 b = -_c_a_m_e_r_a_s._r_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r(*this->_r_e_s_u_l_t__, this->_m_e_a_s_u_r_e_d__, Fs, E); │ │ │ │ │ -254 for (size_t i = 0; i < Fs.size(); i++) { │ │ │ │ │ -255 const _P_o_s_e_3& body_P_sensor = (*cameraRig_)[_c_a_m_e_r_a_I_d_s__[i]].pose(); │ │ │ │ │ -256 const _P_o_s_e_3 world_P_body = _c_a_m_e_r_a_s[i].pose() * body_P_sensor._i_n_v_e_r_s_e(); │ │ │ │ │ -257 Eigen::Matrix H; │ │ │ │ │ -258 world_P_body.compose(body_P_sensor, H); │ │ │ │ │ -259 Fs.at(i) = Fs.at(i) * H; │ │ │ │ │ -260 } │ │ │ │ │ -261 } │ │ │ │ │ -262 } │ │ │ │ │ -263 │ │ │ │ │ -_2_6_5 boost::shared_ptr > _c_r_e_a_t_e_H_e_s_s_i_a_n_F_a_c_t_o_r( │ │ │ │ │ -266 const _V_a_l_u_e_s& values, const double& lambda = 0.0, │ │ │ │ │ -267 bool diagonalDamping = false) const { │ │ │ │ │ -268 // we may have multiple observation sharing the same keys (e.g., 2 cameras │ │ │ │ │ -269 // measuring from the same body pose), hence the number of unique keys may │ │ │ │ │ -270 // be smaller than nrMeasurements │ │ │ │ │ -271 size_t nrUniqueKeys = │ │ │ │ │ -272 this->_k_e_y_s__ │ │ │ │ │ -273 .size(); // note: by construction, keys_ only contains unique keys │ │ │ │ │ -274 │ │ │ │ │ -275 Cameras _c_a_m_e_r_a_s = this->_c_a_m_e_r_a_s(values); │ │ │ │ │ -276 │ │ │ │ │ -277 // Create structures for Hessian Factors │ │ │ │ │ -278 std::vector js; │ │ │ │ │ -279 std::vector Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); │ │ │ │ │ -280 std::vector gs(nrUniqueKeys); │ │ │ │ │ -281 │ │ │ │ │ -282 if (this->_m_e_a_s_u_r_e_d__.size() != cameras.size()) // 1 observation per camera │ │ │ │ │ -283 throw std::runtime_error( │ │ │ │ │ -284 "SmartProjectionRigFactor: " │ │ │ │ │ -285 "measured_.size() inconsistent with input"); │ │ │ │ │ -286 │ │ │ │ │ -287 // triangulate 3D point at given linearization point │ │ │ │ │ -288 this->_t_r_i_a_n_g_u_l_a_t_e_S_a_f_e(cameras); │ │ │ │ │ -289 │ │ │ │ │ -290 if (!this->_r_e_s_u_l_t__) { // failed: return "empty/zero" Hessian │ │ │ │ │ -291 if (this->params_._d_e_g_e_n_e_r_a_c_y_M_o_d_e == ZERO_ON_DEGENERACY) { │ │ │ │ │ -292 for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose); │ │ │ │ │ -293 for (Vector& v : gs) v = Vector::Zero(DimPose); │ │ │ │ │ -294 return boost::make_shared >(this->_k_e_y_s__, │ │ │ │ │ -295 Gs, gs, 0.0); │ │ │ │ │ -296 } else { │ │ │ │ │ -297 throw std::runtime_error( │ │ │ │ │ -298 "SmartProjectionRigFactor: " │ │ │ │ │ -299 "only supported degeneracy mode is ZERO_ON_DEGENERACY"); │ │ │ │ │ -300 } │ │ │ │ │ -301 } │ │ │ │ │ -302 │ │ │ │ │ -303 // compute Jacobian given triangulated 3D Point │ │ │ │ │ -304 typename Base::FBlocks Fs; │ │ │ │ │ -305 Matrix E; │ │ │ │ │ -306 Vector b; │ │ │ │ │ -307 this->_c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s_W_i_t_h_T_r_i_a_n_g_u_l_a_t_e_d_P_o_i_n_t(Fs, E, b, _c_a_m_e_r_a_s); │ │ │ │ │ -308 │ │ │ │ │ -309 // Whiten using noise model │ │ │ │ │ -310 this->_n_o_i_s_e_M_o_d_e_l__->WhitenSystem(E, b); │ │ │ │ │ -311 for (size_t i = 0; i < Fs.size(); i++) { │ │ │ │ │ -312 Fs[i] = this->_n_o_i_s_e_M_o_d_e_l__->Whiten(Fs[i]); │ │ │ │ │ -313 } │ │ │ │ │ -314 │ │ │ │ │ -315 const Matrix3 P = _B_a_s_e_:_:_C_a_m_e_r_a_s_:_:_P_o_i_n_t_C_o_v(E, lambda, diagonalDamping); │ │ │ │ │ -316 │ │ │ │ │ -317 // Build augmented Hessian (with last row/column being the information │ │ │ │ │ -318 // vector) Note: we need to get the augumented hessian wrt the unique keys │ │ │ │ │ -319 // in key_ │ │ │ │ │ -320 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x augmentedHessianUniqueKeys = │ │ │ │ │ -321 Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 6, 6>( │ │ │ │ │ -322 Fs, E, P, b, _n_o_n_U_n_i_q_u_e_K_e_y_s__, this->_k_e_y_s__); │ │ │ │ │ -323 │ │ │ │ │ -324 return boost::make_shared >( │ │ │ │ │ -325 this->_k_e_y_s__, augmentedHessianUniqueKeys); │ │ │ │ │ -326 } │ │ │ │ │ -327 │ │ │ │ │ -_3_3_5 boost::shared_ptr _l_i_n_e_a_r_i_z_e_D_a_m_p_e_d( │ │ │ │ │ -336 const _V_a_l_u_e_s& values, const double& lambda = 0.0) const { │ │ │ │ │ -337 // depending on flag set on construction we may linearize to different │ │ │ │ │ -338 // linear factors │ │ │ │ │ -339 switch (this->params_._l_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e) { │ │ │ │ │ -340 case HESSIAN: │ │ │ │ │ -341 return this->_c_r_e_a_t_e_H_e_s_s_i_a_n_F_a_c_t_o_r(values, lambda); │ │ │ │ │ -342 default: │ │ │ │ │ -343 throw std::runtime_error( │ │ │ │ │ -344 "SmartProjectionRigFactor: unknown linearization mode"); │ │ │ │ │ -345 } │ │ │ │ │ -346 } │ │ │ │ │ -347 │ │ │ │ │ -_3_4_9 boost::shared_ptr _l_i_n_e_a_r_i_z_e( │ │ │ │ │ -350 const _V_a_l_u_e_s& values) const override { │ │ │ │ │ -351 return this->_l_i_n_e_a_r_i_z_e_D_a_m_p_e_d(values); │ │ │ │ │ -352 } │ │ │ │ │ -353 │ │ │ │ │ -354 private: │ │ │ │ │ -_3_5_6 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -357 template │ │ │ │ │ -358 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ -359 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -360 // ar& BOOST_SERIALIZATION_NVP(nonUniqueKeys_); │ │ │ │ │ -361 // ar& BOOST_SERIALIZATION_NVP(cameraRig_); │ │ │ │ │ -362 // ar& BOOST_SERIALIZATION_NVP(cameraIds_); │ │ │ │ │ -363 } │ │ │ │ │ -364}; │ │ │ │ │ -365// end of class declaration │ │ │ │ │ -366 │ │ │ │ │ -368template │ │ │ │ │ -_3_6_9struct _t_r_a_i_t_s<_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r > │ │ │ │ │ -370 : public _T_e_s_t_a_b_l_e > {}; │ │ │ │ │ -371 │ │ │ │ │ -372} // namespace gtsam │ │ │ │ │ -_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_._h │ │ │ │ │ -Smart factor on cameras (pose + calibration) │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ -std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ -FastVector │ │ │ │ │ -FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ -DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ +_6_8 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& expected, double tol=1e-9) const override │ │ │ │ │ +{ │ │ │ │ │ +69 const _T_h_i_s *e = dynamic_cast (&expected); │ │ │ │ │ +70 return e != nullptr && _B_a_s_e_:_:_e_q_u_a_l_s(*e, tol) && this->factor_->_e_q_u_a_l_s(*e- │ │ │ │ │ +>factor_, tol); │ │ │ │ │ +71 } │ │ │ │ │ +72 │ │ │ │ │ +_8_0 double _e_r_r_o_r(const _V_a_l_u_e_s& c) const override { return -factor_->error(c); } │ │ │ │ │ +81 │ │ │ │ │ +_8_3 size_t _d_i_m() const override { return factor_->dim(); } │ │ │ │ │ +84 │ │ │ │ │ +_8_9 bool _a_c_t_i_v_e(const _V_a_l_u_e_s& c) const override { return factor_->active(c); } │ │ │ │ │ +90 │ │ │ │ │ +_9_7 boost::shared_ptr _l_i_n_e_a_r_i_z_e(const _V_a_l_u_e_s& c) const override │ │ │ │ │ +{ │ │ │ │ │ +98 │ │ │ │ │ +99 // Generate the linearized factor from the contained nonlinear factor │ │ │ │ │ +100 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r gaussianFactor = factor_->linearize(c); │ │ │ │ │ +101 │ │ │ │ │ +102 // return the negated version of the factor │ │ │ │ │ +103 return gaussianFactor->negate(); │ │ │ │ │ +104 } │ │ │ │ │ +105 │ │ │ │ │ +106 │ │ │ │ │ +107 private: │ │ │ │ │ +108 │ │ │ │ │ +_1_1_0 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +111 template │ │ │ │ │ +112 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +113 ar & boost::serialization::make_nvp("AntiFactor", │ │ │ │ │ +114 boost::serialization::base_object(*this)); │ │ │ │ │ +115 ar & BOOST_SERIALIZATION_NVP(factor_); │ │ │ │ │ +116 } │ │ │ │ │ +117 }; // \class AntiFactor │ │ │ │ │ +118 │ │ │ │ │ +119} │ │ │ │ │ +_G_a_u_s_s_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ +A factor with a quadratic error function - a Gaussian. │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ +Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ -Aliases. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ -of blocks. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t │ │ │ │ │ -A set of cameras, all with their own calibration. │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_r_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r │ │ │ │ │ -Vector reprojectionError(const POINT &point, const ZVector &measured, boost:: │ │ │ │ │ -optional< FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost:: │ │ │ │ │ -none) const │ │ │ │ │ -Calculate vector [project2(point)-z] of re-projection errors. │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:136 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ -A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_i_n_v_e_r_s_e │ │ │ │ │ -Pose3 inverse() const │ │ │ │ │ -inverse transformation with derivatives │ │ │ │ │ -DDeeffiinniittiioonn Pose3.cpp:49 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s__ │ │ │ │ │ -KeyVector keys_ │ │ │ │ │ -The keys involved in this factor. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s │ │ │ │ │ +const KeyVector & keys() const │ │ │ │ │ +Access the factor's involved variable keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:140 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactor.h:42 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ Nonlinear factor base class. │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_:_:_a_c_t_i_v_e │ │ │ │ │ -virtual bool active(const Values &) const │ │ │ │ │ -Checks whether a factor should be used based on a set of values. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:118 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const │ │ │ │ │ +Check if two factors are equal. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.cpp:47 │ │ │ │ │ _g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_a_t │ │ │ │ │ -const ValueType at(Key j) const │ │ │ │ │ -Retrieve a variable by key j. │ │ │ │ │ -DDeeffiinniittiioonn Values-inl.h:361 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_n_o_i_s_e_M_o_d_e_l__ │ │ │ │ │ -SharedIsotropic noiseModel_ │ │ │ │ │ -As of Feb 22, 2015, the noise model is the same for all measurements and is │ │ │ │ │ -isotropic. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:72 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_m_e_a_s_u_r_e_d__ │ │ │ │ │ -ZVector measured_ │ │ │ │ │ -Measurements for each of the m views. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:79 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_m_e_a_s_u_r_e_d │ │ │ │ │ -const ZVector & measured() const │ │ │ │ │ -Return the 2D measurements (ZDim, in general). │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:159 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_P_o_i_n_t_C_o_v │ │ │ │ │ -static Matrix PointCov(const Matrix &E) │ │ │ │ │ -Computes Point Covariance P from the "point Jacobian" E. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:274 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorParams.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s_:_:_l_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e │ │ │ │ │ -LinearizationMode linearizationMode │ │ │ │ │ -How to linearize the factor. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorParams.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s_:_:_d_e_g_e_n_e_r_a_c_y_M_o_d_e │ │ │ │ │ -DegeneracyMode degeneracyMode │ │ │ │ │ -How to linearize the factor. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorParams.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -SmartProjectionFactor: triangulates point and keeps an estimate of it around. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:103 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_r_e_s_u_l_t__ │ │ │ │ │ -TriangulationResult result_ │ │ │ │ │ -result from triangulateSafe │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:63 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_t_r_i_a_n_g_u_l_a_t_e_S_a_f_e │ │ │ │ │ -TriangulationResult triangulateSafe(const Cameras &cameras) const │ │ │ │ │ -Call gtsam::triangulateSafe iff we need to re-triangulate. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:174 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_t_o_t_a_l_R_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r │ │ │ │ │ -double totalReprojectionError(const Cameras &cameras, boost::optional< Point3 > │ │ │ │ │ -externalPoint=boost::none) const │ │ │ │ │ -Calculate the error of the factor. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:411 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const NonlinearFactor &p, double tol=1e-9) const override │ │ │ │ │ +_g_t_s_a_m_:_:_A_n_t_i_F_a_c_t_o_r │ │ │ │ │ +A class for downdating an existing factor from a graph. │ │ │ │ │ +DDeeffiinniittiioonn AntiFactor.h:31 │ │ │ │ │ +_g_t_s_a_m_:_:_A_n_t_i_F_a_c_t_o_r_:_:_A_n_t_i_F_a_c_t_o_r │ │ │ │ │ +AntiFactor(NonlinearFactor::shared_ptr factor) │ │ │ │ │ +constructor - Creates the equivalent AntiFactor from an existing factor │ │ │ │ │ +DDeeffiinniittiioonn AntiFactor.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_A_n_t_i_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const NonlinearFactor &expected, double tol=1e-9) const override │ │ │ │ │ equals │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:115 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_C_a_m_e_r_a │ │ │ │ │ -CAMERA Camera │ │ │ │ │ -shorthand for a set of cameras │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r │ │ │ │ │ -If you are using the factor, please cite: L. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionRigFactor.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_n_o_n_U_n_i_q_u_e_K_e_y_s__ │ │ │ │ │ -KeyVector nonUniqueKeys_ │ │ │ │ │ -vector of keys (one for each observation) with potentially repeated keys │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionRigFactor.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_c_a_m_e_r_a_I_d_s__ │ │ │ │ │ -FastVector< size_t > cameraIds_ │ │ │ │ │ -vector of camera Ids (one for each observation, in the same order), identifying │ │ │ │ │ -which camera took the... │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionRigFactor.h:72 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_c_a_m_e_r_a_I_d_s │ │ │ │ │ -const FastVector< size_t > & cameraIds() const │ │ │ │ │ -return the calibration object │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionRigFactor.h:177 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_c_r_e_a_t_e_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -boost::shared_ptr< RegularHessianFactor< DimPose > > createHessianFactor(const │ │ │ │ │ -Values &values, const double &lambda=0.0, bool diagonalDamping=false) const │ │ │ │ │ -linearize and return a Hessianfactor that is an approximation of error(p) │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionRigFactor.h:265 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_a_d_d │ │ │ │ │ -void add(const MEASUREMENT &measured, const Key &poseKey, const size_t │ │ │ │ │ -&cameraId=0) │ │ │ │ │ -add a new measurement, corresponding to an observation from pose "poseKey" and │ │ │ │ │ -taken from the camera ... │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionRigFactor.h:123 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shorthand for a smart pointer to a factor │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionRigFactor.h:81 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s_W_i_t_h_T_r_i_a_n_g_u_l_a_t_e_d_P_o_i_n_t │ │ │ │ │ -void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks &Fs, Matrix │ │ │ │ │ -&E, Vector &b, const Cameras &cameras) const │ │ │ │ │ -Compute jacobian F, E and error vector at a given linearization point. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionRigFactor.h:247 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_a_d_d │ │ │ │ │ -void add(const MEASUREMENTS &measurements, const KeyVector &poseKeys, const │ │ │ │ │ -FastVector< size_t > &cameraIds=FastVector< size_t >()) │ │ │ │ │ -Variant of the previous "add" function in which we include multiple │ │ │ │ │ -measurements. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionRigFactor.h:149 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r │ │ │ │ │ -SmartProjectionRigFactor() │ │ │ │ │ -Default constructor, only for serialization. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionRigFactor.h:84 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_e_r_r_o_r │ │ │ │ │ -double error(const Values &values) const override │ │ │ │ │ -error calculates the error of the factor. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionRigFactor.h:230 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_c_a_m_e_r_a_s │ │ │ │ │ -Base::Cameras cameras(const Values &values) const override │ │ │ │ │ -Collect all cameras involved in this factor. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionRigFactor.h:212 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_~_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r │ │ │ │ │ -~SmartProjectionRigFactor() override=default │ │ │ │ │ -Virtual destructor. │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +DDeeffiinniittiioonn AntiFactor.h:68 │ │ │ │ │ +_g_t_s_a_m_:_:_A_n_t_i_F_a_c_t_o_r_:_:_A_n_t_i_F_a_c_t_o_r │ │ │ │ │ +AntiFactor() │ │ │ │ │ +default constructor - only use for serialization │ │ │ │ │ +DDeeffiinniittiioonn AntiFactor.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_A_n_t_i_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e │ │ │ │ │ +boost::shared_ptr< GaussianFactor > linearize(const Values &c) const override │ │ │ │ │ +Linearize to a GaussianFactor. │ │ │ │ │ +DDeeffiinniittiioonn AntiFactor.h:97 │ │ │ │ │ +_g_t_s_a_m_:_:_A_n_t_i_F_a_c_t_o_r_:_:_a_c_t_i_v_e │ │ │ │ │ +bool active(const Values &c) const override │ │ │ │ │ +Checks whether this factor should be used based on a set of values. │ │ │ │ │ +DDeeffiinniittiioonn AntiFactor.h:89 │ │ │ │ │ +_g_t_s_a_m_:_:_A_n_t_i_F_a_c_t_o_r_:_:_d_i_m │ │ │ │ │ +size_t dim() const override │ │ │ │ │ +get the dimension of the factor (same as the original factor) │ │ │ │ │ +DDeeffiinniittiioonn AntiFactor.h:83 │ │ │ │ │ +_g_t_s_a_m_:_:_A_n_t_i_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s, const KeyFormatter │ │ │ │ │ &keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionRigFactor.h:184 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e │ │ │ │ │ -boost::shared_ptr< GaussianFactor > linearize(const Values &values) const │ │ │ │ │ -override │ │ │ │ │ -linearize │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionRigFactor.h:349 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ +implement functions needed for Testable │ │ │ │ │ +DDeeffiinniittiioonn AntiFactor.h:62 │ │ │ │ │ +_g_t_s_a_m_:_:_A_n_t_i_F_a_c_t_o_r_:_:_e_r_r_o_r │ │ │ │ │ +double error(const Values &c) const override │ │ │ │ │ +implement functions needed to derive from Factor │ │ │ │ │ +DDeeffiinniittiioonn AntiFactor.h:80 │ │ │ │ │ +_g_t_s_a_m_:_:_A_n_t_i_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ +gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn AntiFactor.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_A_n_t_i_F_a_c_t_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ friend class boost::serialization::access │ │ │ │ │ Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionRigFactor.h:356 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_n_o_n_U_n_i_q_u_e_K_e_y_s │ │ │ │ │ -const KeyVector & nonUniqueKeys() const │ │ │ │ │ -return (for each observation) the (possibly non unique) keys involved in the │ │ │ │ │ -measurements │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionRigFactor.h:171 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_c_a_m_e_r_a_R_i_g__ │ │ │ │ │ -boost::shared_ptr< typename Base::Cameras > cameraRig_ │ │ │ │ │ -cameras in the rig (fixed poses wrt body and intrinsics, for each camera) │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionRigFactor.h:68 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r │ │ │ │ │ -SmartProjectionRigFactor(const SharedNoiseModel &sharedNoiseModel, const │ │ │ │ │ -boost::shared_ptr< Cameras > &cameraRig, const SmartProjectionParams │ │ │ │ │ -¶ms=SmartProjectionParams()) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionRigFactor.h:94 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_c_a_m_e_r_a_R_i_g │ │ │ │ │ -const boost::shared_ptr< Cameras > & cameraRig() const │ │ │ │ │ -return the calibration object │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionRigFactor.h:174 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e_D_a_m_p_e_d │ │ │ │ │ -boost::shared_ptr< GaussianFactor > linearizeDamped(const Values &values, const │ │ │ │ │ -double &lambda=0.0) const │ │ │ │ │ -Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM) │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionRigFactor.h:335 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const NonlinearFactor &p, double tol=1e-9) const override │ │ │ │ │ -equals │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionRigFactor.h:198 │ │ │ │ │ +DDeeffiinniittiioonn AntiFactor.h:110 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_._h │ │ │ │ │ + * _A_n_t_i_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01259.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/EssentialMatrixConstraint.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/FrobeniusFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,34 +94,43 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces
│ │ │ │ -
EssentialMatrixConstraint.cpp File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
FrobeniusFactor.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ + │ │ │ │ +

Various factors that minimize some Frobenius norm. │ │ │ │ +More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

SharedNoiseModel gtsam::ConvertNoiseModel (const SharedNoiseModel &model, size_t n, bool defaultToUnit=true)
 When creating (any) FrobeniusFactor we can convert a Rot/Pose BetweenFactor noise model into a n-dimensional isotropic noise model used to weight the Frobenius norm.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ -Pablo Alcantarilla
│ │ │ │ -
Date
Jan 5, 2014
│ │ │ │ +

Various factors that minimize some Frobenius norm.

│ │ │ │ +
Date
March 2019
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,29 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -EssentialMatrixConstraint.cpp File Reference │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +FrobeniusFactor.cpp File Reference │ │ │ │ │ +Various factors that minimize some Frobenius norm. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l  _g_t_s_a_m_:_:_C_o_n_v_e_r_t_N_o_i_s_e_M_o_d_e_l (const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, │ │ │ │ │ + size_t n, bool defaultToUnit=true) │ │ │ │ │ + When creating (any) _F_r_o_b_e_n_i_u_s_F_a_c_t_o_r we can convert a Rot/Pose │ │ │ │ │ +  _B_e_t_w_e_e_n_F_a_c_t_o_r noise model into a n-dimensional isotropic │ │ │ │ │ + noise model used to weight the Frobenius norm. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Various factors that minimize some Frobenius norm. │ │ │ │ │ + Date │ │ │ │ │ + March 2019 │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Pablo Alcantarilla │ │ │ │ │ - Date │ │ │ │ │ - Jan 5, 2014 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t_._c_p_p │ │ │ │ │ + * _F_r_o_b_e_n_i_u_s_F_a_c_t_o_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01262.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/SmartProjectionFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/GeneralSFMFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,49 +96,50 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
SmartProjectionFactor.h File Reference
│ │ │ │ +
GeneralSFMFactor.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Smart factor on cameras (pose + calibration) │ │ │ │ +

a general SFM factor with an unknown calibration │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::SmartProjectionFactor< CAMERA >
 SmartProjectionFactor: triangulates point and keeps an estimate of it around. More...
class  gtsam::GeneralSFMFactor< CAMERA, LANDMARK >
 Non-linear factor for a constraint derived from a 2D measurement. More...
 
struct  gtsam::traits< SmartProjectionFactor< CAMERA > >
 traits More...
struct  gtsam::traits< GeneralSFMFactor< CAMERA, LANDMARK > >
 
class  gtsam::GeneralSFMFactor2< CALIBRATION >
 Non-linear factor for a constraint derived from a 2D measurement. More...
 
struct  gtsam::traits< GeneralSFMFactor2< CALIBRATION > >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Smart factor on cameras (pose + calibration)

│ │ │ │ -
Author
Luca Carlone
│ │ │ │ -
│ │ │ │ -Zsolt Kira
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ +

a general SFM factor with an unknown calibration

│ │ │ │ +
Date
Dec 15, 2010
│ │ │ │ +
Author
Kai Ni
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,31 +1,36 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SmartProjectionFactor.h File Reference │ │ │ │ │ -Smart factor on cameras (pose + calibration) _M_o_r_e_._._. │ │ │ │ │ +GeneralSFMFactor.h File Reference │ │ │ │ │ +a general SFM factor with an unknown calibration _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_<_ _C_A_M_E_R_A_ _> │ │ │ │ │ -  _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r: triangulates point and keeps an estimate of it │ │ │ │ │ - around. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_<_ _C_A_M_E_R_A_,_ _L_A_N_D_M_A_R_K_ _> │ │ │ │ │ +  Non-linear factor for a constraint derived from a 2D measurement. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_<_ _C_A_M_E_R_A_ _>_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_<_ _C_A_M_E_R_A_,_ _L_A_N_D_M_A_R_K_ _>_ _> │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_<_ _C_A_L_I_B_R_A_T_I_O_N_ _> │ │ │ │ │ +  Non-linear factor for a constraint derived from a 2D measurement. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_<_ _C_A_L_I_B_R_A_T_I_O_N_ _>_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Smart factor on cameras (pose + calibration) │ │ │ │ │ +a general SFM factor with an unknown calibration │ │ │ │ │ + Date │ │ │ │ │ + Dec 15, 2010 │ │ │ │ │ Author │ │ │ │ │ - Luca Carlone │ │ │ │ │ - Zsolt Kira │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Kai Ni │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_._h │ │ │ │ │ + * _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01262.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,4 +1,5 @@ │ │ │ │ │ var a01262 = [ │ │ │ │ │ - ["gtsam::SmartProjectionFactor< CAMERA >", "a04936.html", "a04936"], │ │ │ │ │ - ["gtsam::traits< SmartProjectionFactor< CAMERA > >", "a04940.html", null] │ │ │ │ │ + ["gtsam::traits< GeneralSFMFactor< CAMERA, LANDMARK > >", "a04844.html", null], │ │ │ │ │ + ["gtsam::GeneralSFMFactor2< CALIBRATION >", "a04848.html", "a04848"], │ │ │ │ │ + ["gtsam::traits< GeneralSFMFactor2< CALIBRATION > >", "a04852.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01262_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/SmartProjectionFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/GeneralSFMFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,539 +98,346 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
SmartProjectionFactor.h
│ │ │ │ +
GeneralSFMFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/slam/SmartFactorBase.h>
│ │ │ │ -
23#include <gtsam/slam/SmartFactorParams.h>
│ │ │ │ -
24
│ │ │ │ -
25#include <gtsam/geometry/triangulation.h>
│ │ │ │ -
26#include <gtsam/inference/Symbol.h>
│ │ │ │ -
27#include <gtsam/slam/dataset.h>
│ │ │ │ -
28
│ │ │ │ -
29#include <boost/optional.hpp>
│ │ │ │ -
30#include <boost/make_shared.hpp>
│ │ │ │ -
31#include <vector>
│ │ │ │ -
32
│ │ │ │ -
33namespace gtsam {
│ │ │ │ -
34
│ │ │ │ -
44template<class CAMERA>
│ │ │ │ -
│ │ │ │ -
45class SmartProjectionFactor: public SmartFactorBase<CAMERA> {
│ │ │ │ -
46
│ │ │ │ -
47public:
│ │ │ │ -
48
│ │ │ │ -
49private:
│ │ │ │ -
50 typedef SmartFactorBase<CAMERA> Base;
│ │ │ │ -
51 typedef SmartProjectionFactor<CAMERA> This;
│ │ │ │ -
52 typedef SmartProjectionFactor<CAMERA> SmartProjectionCameraFactor;
│ │ │ │ +
21#pragma once
│ │ │ │ +
22
│ │ │ │ +
23#include <gtsam/geometry/PinholeCamera.h>
│ │ │ │ +
24#include <gtsam/geometry/Point2.h>
│ │ │ │ +
25#include <gtsam/geometry/Point3.h>
│ │ │ │ +
26#include <gtsam/geometry/Pose3.h>
│ │ │ │ +
27#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
28#include <gtsam/linear/BinaryJacobianFactor.h>
│ │ │ │ +
29#include <gtsam/linear/NoiseModel.h>
│ │ │ │ +
30#include <gtsam/base/concepts.h>
│ │ │ │ +
31#include <gtsam/base/Manifold.h>
│ │ │ │ +
32#include <gtsam/base/Matrix.h>
│ │ │ │ +
33#include <gtsam/base/SymmetricBlockMatrix.h>
│ │ │ │ +
34#include <gtsam/base/types.h>
│ │ │ │ +
35#include <gtsam/base/Testable.h>
│ │ │ │ +
36#include <gtsam/base/Vector.h>
│ │ │ │ +
37#include <gtsam/base/timing.h>
│ │ │ │ +
38
│ │ │ │ +
39#include <boost/none.hpp>
│ │ │ │ +
40#include <boost/optional/optional.hpp>
│ │ │ │ +
41#include <boost/serialization/nvp.hpp>
│ │ │ │ +
42#include <boost/smart_ptr/shared_ptr.hpp>
│ │ │ │ +
43#include <iostream>
│ │ │ │ +
44#include <string>
│ │ │ │ +
45
│ │ │ │ +
46namespace boost {
│ │ │ │ +
47namespace serialization {
│ │ │ │ +
48class access;
│ │ │ │ +
49} /* namespace serialization */
│ │ │ │ +
50} /* namespace boost */
│ │ │ │ +
51
│ │ │ │ +
52namespace gtsam {
│ │ │ │
53
│ │ │ │ -
54protected:
│ │ │ │ -
55
│ │ │ │ -
58 SmartProjectionParams params_;
│ │ │ │ -
60
│ │ │ │ -
63 mutable TriangulationResult result_;
│ │ │ │ -
64 mutable std::vector<Pose3, Eigen::aligned_allocator<Pose3> >
│ │ │ │ -
65 cameraPosesTriangulation_;
│ │ │ │ -
67
│ │ │ │ -
68 public:
│ │ │ │ +
59template<class CAMERA, class LANDMARK>
│ │ │ │ +
│ │ │ │ +
60class GeneralSFMFactor: public NoiseModelFactorN<CAMERA, LANDMARK> {
│ │ │ │ +
61
│ │ │ │ +
62 GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA)
│ │ │ │ +
63 GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK)
│ │ │ │ +
64
│ │ │ │ +
65 static const int DimC = FixedDimension<CAMERA>::value;
│ │ │ │ +
66 static const int DimL = FixedDimension<LANDMARK>::value;
│ │ │ │ +
67 typedef Eigen::Matrix<double, 2, DimC> JacobianC;
│ │ │ │ +
68 typedef Eigen::Matrix<double, 2, DimL> JacobianL;
│ │ │ │
69
│ │ │ │ -
71 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
72
│ │ │ │ -
74 typedef CAMERA Camera;
│ │ │ │ -
75 typedef CameraSet<CAMERA> Cameras;
│ │ │ │ -
76
│ │ │ │ -
80 SmartProjectionFactor() {}
│ │ │ │ +
70protected:
│ │ │ │ +
71
│ │ │ │ +
72 Point2 measured_;
│ │ │ │ +
73
│ │ │ │ +
74public:
│ │ │ │ +
75
│ │ │ │ +
76 typedef GeneralSFMFactor<CAMERA, LANDMARK> This;
│ │ │ │ +
77 typedef NoiseModelFactorN<CAMERA, LANDMARK> Base;
│ │ │ │ +
78
│ │ │ │ +
79 // shorthand for a smart pointer to a factor
│ │ │ │ +
80 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │
81
│ │ │ │ -
│ │ │ │ -
87 SmartProjectionFactor(
│ │ │ │ -
88 const SharedNoiseModel& sharedNoiseModel,
│ │ │ │ -
89 const SmartProjectionParams& params = SmartProjectionParams())
│ │ │ │ -
90 : Base(sharedNoiseModel),
│ │ │ │ -
91 params_(params),
│ │ │ │ -
92 result_(TriangulationResult::Degenerate()) {}
│ │ │ │ -
│ │ │ │ -
93
│ │ │ │ -
│ │ │ │ -
95 ~SmartProjectionFactor() override {
│ │ │ │ -
96 }
│ │ │ │ -
│ │ │ │ -
97
│ │ │ │ -
│ │ │ │ -
103 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
│ │ │ │ -
104 DefaultKeyFormatter) const override {
│ │ │ │ -
105 std::cout << s << "SmartProjectionFactor\n";
│ │ │ │ -
106 std::cout << "linearizationMode: " << params_.linearizationMode
│ │ │ │ -
107 << std::endl;
│ │ │ │ -
108 std::cout << "triangulationParameters:\n" << params_.triangulation
│ │ │ │ -
109 << std::endl;
│ │ │ │ -
110 std::cout << "result:\n" << result_ << std::endl;
│ │ │ │ -
111 Base::print("", keyFormatter);
│ │ │ │ -
112 }
│ │ │ │ -
│ │ │ │ -
113
│ │ │ │ -
│ │ │ │ -
115 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
│ │ │ │ -
116 const This *e = dynamic_cast<const This*>(&p);
│ │ │ │ -
117 return e && params_.linearizationMode == e->params_.linearizationMode
│ │ │ │ -
118 && Base::equals(p, tol);
│ │ │ │ -
119 }
│ │ │ │ -
│ │ │ │ -
120
│ │ │ │ -
│ │ │ │ -
128 bool decideIfTriangulate(const Cameras& cameras) const {
│ │ │ │ -
129 // Several calls to linearize will be done from the same linearization
│ │ │ │ -
130 // point, hence it is not needed to re-triangulate. Note that this is not
│ │ │ │ -
131 // yet "selecting linearization", that will come later, and we only check if
│ │ │ │ -
132 // the current linearization is the "same" (up to tolerance) w.r.t. the last
│ │ │ │ -
133 // time we triangulated the point.
│ │ │ │ -
134
│ │ │ │ -
135 size_t m = cameras.size();
│ │ │ │ -
136
│ │ │ │ -
137 bool retriangulate = false;
│ │ │ │ -
138
│ │ │ │ -
139 // Definitely true if we do not have a previous linearization point or the
│ │ │ │ -
140 // new linearization point includes more poses.
│ │ │ │ -
141 if (cameraPosesTriangulation_.empty()
│ │ │ │ -
142 || cameras.size() != cameraPosesTriangulation_.size())
│ │ │ │ -
143 retriangulate = true;
│ │ │ │ -
144
│ │ │ │ -
145 // Otherwise, check poses against cache.
│ │ │ │ -
146 if (!retriangulate) {
│ │ │ │ -
147 for (size_t i = 0; i < cameras.size(); i++) {
│ │ │ │ -
148 if (!cameras[i].pose().equals(cameraPosesTriangulation_[i],
│ │ │ │ -
149 params_.retriangulationThreshold)) {
│ │ │ │ -
150 retriangulate = true; // at least two poses are different, hence we retriangulate
│ │ │ │ -
151 break;
│ │ │ │ -
152 }
│ │ │ │ -
153 }
│ │ │ │ -
154 }
│ │ │ │ -
155
│ │ │ │ -
156 // Store the current poses used for triangulation if we will re-triangulate.
│ │ │ │ -
157 if (retriangulate) {
│ │ │ │ -
158 cameraPosesTriangulation_.clear();
│ │ │ │ -
159 cameraPosesTriangulation_.reserve(m);
│ │ │ │ -
160 for (size_t i = 0; i < m; i++)
│ │ │ │ -
161 // cameraPosesTriangulation_[i] = cameras[i].pose();
│ │ │ │ -
162 cameraPosesTriangulation_.push_back(cameras[i].pose());
│ │ │ │ -
163 }
│ │ │ │ -
164
│ │ │ │ -
165 return retriangulate;
│ │ │ │ -
166 }
│ │ │ │ -
│ │ │ │ +
│ │ │ │ +
89 GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model,
│ │ │ │ +
90 Key cameraKey, Key landmarkKey)
│ │ │ │ +
91 : Base(model, cameraKey, landmarkKey), measured_(measured) {}
│ │ │ │ +
│ │ │ │ +
92
│ │ │ │ +
93 GeneralSFMFactor() : measured_(0.0, 0.0) {}
│ │ │ │ +
95 GeneralSFMFactor(const Point2& p) : measured_(p) {}
│ │ │ │ +
97 GeneralSFMFactor(double x, double y) : measured_(x, y) {}
│ │ │ │ +
98
│ │ │ │ +
99 ~GeneralSFMFactor() override {}
│ │ │ │ +
100
│ │ │ │ +
102 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
103 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ +
104 gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
│ │ │ │ +
105
│ │ │ │ +
111 void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ +
112 Base::print(s, keyFormatter);
│ │ │ │ +
113 traits<Point2>::Print(measured_, s + ".z");
│ │ │ │ +
114 }
│ │ │ │ +
115
│ │ │ │ +
119 bool equals(const NonlinearFactor &p, double tol = 1e-9) const override {
│ │ │ │ +
120 const This* e = dynamic_cast<const This*>(&p);
│ │ │ │ +
121 return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol);
│ │ │ │ +
122 }
│ │ │ │ +
123
│ │ │ │ +
125 Vector evaluateError(const CAMERA& camera, const LANDMARK& point,
│ │ │ │ +
126 boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const override {
│ │ │ │ +
127 try {
│ │ │ │ +
128 return camera.project2(point,H1,H2) - measured_;
│ │ │ │ +
129 }
│ │ │ │ +
130 catch( CheiralityException& e) {
│ │ │ │ +
131 if (H1) *H1 = JacobianC::Zero();
│ │ │ │ +
132 if (H2) *H2 = JacobianL::Zero();
│ │ │ │ +
133 //TODO Print the exception via logging
│ │ │ │ +
134 return Z_2x1;
│ │ │ │ +
135 }
│ │ │ │ +
136 }
│ │ │ │ +
137
│ │ │ │ +
139 boost::shared_ptr<GaussianFactor> linearize(const Values& values) const override {
│ │ │ │ +
140 // Only linearize if the factor is active
│ │ │ │ +
141 if (!this->active(values)) return boost::shared_ptr<JacobianFactor>();
│ │ │ │ +
142
│ │ │ │ +
143 const Key key1 = this->key1(), key2 = this->key2();
│ │ │ │ +
144 JacobianC H1;
│ │ │ │ +
145 JacobianL H2;
│ │ │ │ +
146 Vector2 b;
│ │ │ │ +
147 try {
│ │ │ │ +
148 const CAMERA& camera = values.at<CAMERA>(key1);
│ │ │ │ +
149 const LANDMARK& point = values.at<LANDMARK>(key2);
│ │ │ │ +
150 b = measured() - camera.project2(point, H1, H2);
│ │ │ │ +
151 } catch (CheiralityException& e) {
│ │ │ │ +
152 H1.setZero();
│ │ │ │ +
153 H2.setZero();
│ │ │ │ +
154 b.setZero();
│ │ │ │ +
155 //TODO Print the exception via logging
│ │ │ │ +
156 }
│ │ │ │ +
157
│ │ │ │ +
158 // Whiten the system if needed
│ │ │ │ +
159 const SharedNoiseModel& noiseModel = this->noiseModel();
│ │ │ │ +
160 if (noiseModel && !noiseModel->isUnit()) {
│ │ │ │ +
161 // TODO: implement WhitenSystem for fixed size matrices and include
│ │ │ │ +
162 // above
│ │ │ │ +
163 H1 = noiseModel->Whiten(H1);
│ │ │ │ +
164 H2 = noiseModel->Whiten(H2);
│ │ │ │ +
165 b = noiseModel->Whiten(b);
│ │ │ │ +
166 }
│ │ │ │
167
│ │ │ │ -
│ │ │ │ -
174 TriangulationResult triangulateSafe(const Cameras& cameras) const {
│ │ │ │ -
175
│ │ │ │ -
176 size_t m = cameras.size();
│ │ │ │ -
177 if (m < 2) // if we have a single pose the corresponding factor is uninformative
│ │ │ │ -
178 return TriangulationResult::Degenerate();
│ │ │ │ -
179
│ │ │ │ -
180 bool retriangulate = decideIfTriangulate(cameras);
│ │ │ │ -
181 if (retriangulate)
│ │ │ │ -
182 result_ = gtsam::triangulateSafe(cameras, this->measured_,
│ │ │ │ -
183 params_.triangulation);
│ │ │ │ -
184 return result_;
│ │ │ │ -
185 }
│ │ │ │ -
│ │ │ │ -
186
│ │ │ │ -
│ │ │ │ -
193 bool triangulateForLinearize(const Cameras& cameras) const {
│ │ │ │ -
194 triangulateSafe(cameras); // imperative, might reset result_
│ │ │ │ -
195 return bool(result_);
│ │ │ │ -
196 }
│ │ │ │ -
│ │ │ │ -
197
│ │ │ │ -
│ │ │ │ -
199 boost::shared_ptr<RegularHessianFactor<Base::Dim> > createHessianFactor(
│ │ │ │ -
200 const Cameras& cameras, const double lambda = 0.0,
│ │ │ │ -
201 bool diagonalDamping = false) const {
│ │ │ │ -
202 size_t numKeys = this->keys_.size();
│ │ │ │ -
203 // Create structures for Hessian Factors
│ │ │ │ -
204 KeyVector js;
│ │ │ │ -
205 std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
│ │ │ │ -
206 std::vector<Vector> gs(numKeys);
│ │ │ │ -
207
│ │ │ │ -
208 if (this->measured_.size() != cameras.size())
│ │ │ │ -
209 throw std::runtime_error(
│ │ │ │ -
210 "SmartProjectionHessianFactor: this->measured_"
│ │ │ │ -
211 ".size() inconsistent with input");
│ │ │ │ +
168 // Create new (unit) noiseModel, preserving constraints if applicable
│ │ │ │ +
169 SharedDiagonal model;
│ │ │ │ +
170 if (noiseModel && noiseModel->isConstrained()) {
│ │ │ │ +
171 model = boost::static_pointer_cast<noiseModel::Constrained>(noiseModel)->unit();
│ │ │ │ +
172 }
│ │ │ │ +
173
│ │ │ │ +
174 return boost::make_shared<BinaryJacobianFactor<2, DimC, DimL> >(key1, H1, key2, H2, b, model);
│ │ │ │ +
175 }
│ │ │ │ +
176
│ │ │ │ +
178 inline const Point2 measured() const {
│ │ │ │ +
179 return measured_;
│ │ │ │ +
180 }
│ │ │ │ +
181
│ │ │ │ +
182private:
│ │ │ │ +
184 friend class boost::serialization::access;
│ │ │ │ +
185 template<class Archive>
│ │ │ │ +
186 void serialize(Archive & ar, const unsigned int /*version*/) {
│ │ │ │ +
187 // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ +
188 ar & boost::serialization::make_nvp("NoiseModelFactor2",
│ │ │ │ +
189 boost::serialization::base_object<Base>(*this));
│ │ │ │ +
190 ar & BOOST_SERIALIZATION_NVP(measured_);
│ │ │ │ +
191 }
│ │ │ │ +
192};
│ │ │ │ +
│ │ │ │ +
193
│ │ │ │ +
194template<class CAMERA, class LANDMARK>
│ │ │ │ +
│ │ │ │ +
195struct traits<GeneralSFMFactor<CAMERA, LANDMARK> > : Testable<
│ │ │ │ +
196 GeneralSFMFactor<CAMERA, LANDMARK> > {
│ │ │ │ +
197};
│ │ │ │ +
│ │ │ │ +
198
│ │ │ │ +
203template<class CALIBRATION>
│ │ │ │ +
│ │ │ │ +
204class GeneralSFMFactor2: public NoiseModelFactorN<Pose3, Point3, CALIBRATION> {
│ │ │ │ +
205
│ │ │ │ +
206 GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
│ │ │ │ +
207 static const int DimK = FixedDimension<CALIBRATION>::value;
│ │ │ │ +
208
│ │ │ │ +
209protected:
│ │ │ │ +
210
│ │ │ │ +
211 Point2 measured_;
│ │ │ │
212
│ │ │ │ -
213 triangulateSafe(cameras);
│ │ │ │ +
213public:
│ │ │ │
214
│ │ │ │ -
215 if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
│ │ │ │ -
216 // failed: return"empty" Hessian
│ │ │ │ -
217 for (Matrix& m : Gs) m = Matrix::Zero(Base::Dim, Base::Dim);
│ │ │ │ -
218 for (Vector& v : gs) v = Vector::Zero(Base::Dim);
│ │ │ │ -
219 return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
│ │ │ │ -
220 Gs, gs, 0.0);
│ │ │ │ -
221 }
│ │ │ │ -
222
│ │ │ │ -
223 // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
│ │ │ │ -
224 typename Base::FBlocks Fs;
│ │ │ │ -
225 Matrix E;
│ │ │ │ -
226 Vector b;
│ │ │ │ -
227 computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
│ │ │ │ -
228
│ │ │ │ -
229 // Whiten using noise model
│ │ │ │ -
230 Base::whitenJacobians(Fs, E, b);
│ │ │ │ -
231
│ │ │ │ -
232 // build augmented hessian
│ │ │ │ -
233 SymmetricBlockMatrix augmentedHessian = //
│ │ │ │ -
234 Cameras::SchurComplement(Fs, E, b, lambda, diagonalDamping);
│ │ │ │ +
215 typedef GeneralSFMFactor2<CALIBRATION> This;
│ │ │ │ +
216 typedef PinholeCamera<CALIBRATION> Camera;
│ │ │ │ +
217 typedef NoiseModelFactorN<Pose3, Point3, CALIBRATION> Base;
│ │ │ │ +
218
│ │ │ │ +
219 // shorthand for a smart pointer to a factor
│ │ │ │ +
220 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
221
│ │ │ │ +
│ │ │ │ +
230 GeneralSFMFactor2(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, Key calibKey) :
│ │ │ │ +
231 Base(model, poseKey, landmarkKey, calibKey), measured_(measured) {}
│ │ │ │ +
│ │ │ │ +
232 GeneralSFMFactor2():measured_(0.0,0.0) {}
│ │ │ │ +
233
│ │ │ │ +
│ │ │ │ +
234 ~GeneralSFMFactor2() override {}
│ │ │ │
235
│ │ │ │ -
236 return boost::make_shared<RegularHessianFactor<Base::Dim> >(
│ │ │ │ -
237 this->keys_, augmentedHessian);
│ │ │ │ -
238 }
│ │ │ │ -
│ │ │ │ -
239
│ │ │ │ -
240 // Create RegularImplicitSchurFactor factor.
│ │ │ │ -
241 boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > createRegularImplicitSchurFactor(
│ │ │ │ -
242 const Cameras& cameras, double lambda) const {
│ │ │ │ -
243 if (triangulateForLinearize(cameras))
│ │ │ │ -
244 return Base::createRegularImplicitSchurFactor(cameras, *result_, lambda);
│ │ │ │ -
245 else
│ │ │ │ -
246 // failed: return empty
│ │ │ │ -
247 return boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> >();
│ │ │ │ -
248 }
│ │ │ │ -
249
│ │ │ │ -
│ │ │ │ -
251 boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor(
│ │ │ │ -
252 const Cameras& cameras, double lambda) const {
│ │ │ │ -
253 if (triangulateForLinearize(cameras))
│ │ │ │ -
254 return Base::createJacobianQFactor(cameras, *result_, lambda);
│ │ │ │ -
255 else
│ │ │ │ -
256 // failed: return empty
│ │ │ │ -
257 return boost::make_shared<JacobianFactorQ<Base::Dim, 2> >(this->keys_);
│ │ │ │ -
258 }
│ │ │ │ -
│ │ │ │ -
259
│ │ │ │ -
│ │ │ │ -
261 boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > createJacobianQFactor(
│ │ │ │ -
262 const Values& values, double lambda) const {
│ │ │ │ -
263 return createJacobianQFactor(this->cameras(values), lambda);
│ │ │ │ -
264 }
│ │ │ │ -
│ │ │ │ -
265
│ │ │ │ -
│ │ │ │ -
267 boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
│ │ │ │ -
268 const Cameras& cameras, double lambda) const {
│ │ │ │ -
269 if (triangulateForLinearize(cameras))
│ │ │ │ -
270 return Base::createJacobianSVDFactor(cameras, *result_, lambda);
│ │ │ │ -
271 else
│ │ │ │ -
272 // failed: return empty
│ │ │ │ -
273 return boost::make_shared<JacobianFactorSVD<Base::Dim, 2> >(this->keys_);
│ │ │ │ -
274 }
│ │ │ │ -
│ │ │ │ -
275
│ │ │ │ -
│ │ │ │ -
277 virtual boost::shared_ptr<RegularHessianFactor<Base::Dim> > linearizeToHessian(
│ │ │ │ -
278 const Values& values, double lambda = 0.0) const {
│ │ │ │ -
279 return createHessianFactor(this->cameras(values), lambda);
│ │ │ │ -
280 }
│ │ │ │ -
│ │ │ │ -
281
│ │ │ │ -
│ │ │ │ -
283 virtual boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > linearizeToImplicit(
│ │ │ │ -
284 const Values& values, double lambda = 0.0) const {
│ │ │ │ -
285 return createRegularImplicitSchurFactor(this->cameras(values), lambda);
│ │ │ │ -
286 }
│ │ │ │ -
│ │ │ │ -
287
│ │ │ │ -
│ │ │ │ -
289 virtual boost::shared_ptr<JacobianFactorQ<Base::Dim, 2> > linearizeToJacobian(
│ │ │ │ -
290 const Values& values, double lambda = 0.0) const {
│ │ │ │ -
291 return createJacobianQFactor(this->cameras(values), lambda);
│ │ │ │ -
292 }
│ │ │ │ -
│ │ │ │ -
293
│ │ │ │ -
│ │ │ │ -
299 boost::shared_ptr<GaussianFactor> linearizeDamped(const Cameras& cameras,
│ │ │ │ -
300 const double lambda = 0.0) const {
│ │ │ │ -
301 // depending on flag set on construction we may linearize to different linear factors
│ │ │ │ -
302 switch (params_.linearizationMode) {
│ │ │ │ -
303 case HESSIAN:
│ │ │ │ -
304 return createHessianFactor(cameras, lambda);
│ │ │ │ -
305 case IMPLICIT_SCHUR:
│ │ │ │ -
306 return createRegularImplicitSchurFactor(cameras, lambda);
│ │ │ │ -
307 case JACOBIAN_SVD:
│ │ │ │ -
308 return createJacobianSVDFactor(cameras, lambda);
│ │ │ │ -
309 case JACOBIAN_Q:
│ │ │ │ -
310 return createJacobianQFactor(cameras, lambda);
│ │ │ │ -
311 default:
│ │ │ │ -
312 throw std::runtime_error("SmartFactorlinearize: unknown mode");
│ │ │ │ -
313 }
│ │ │ │ -
314 }
│ │ │ │ -
│ │ │ │ -
315
│ │ │ │ -
│ │ │ │ -
321 boost::shared_ptr<GaussianFactor> linearizeDamped(const Values& values,
│ │ │ │ -
322 const double lambda = 0.0) const {
│ │ │ │ -
323 // depending on flag set on construction we may linearize to different linear factors
│ │ │ │ -
324 Cameras cameras = this->cameras(values);
│ │ │ │ -
325 return linearizeDamped(cameras, lambda);
│ │ │ │ -
326 }
│ │ │ │ -
│ │ │ │ -
327
│ │ │ │ -
│ │ │ │ -
329 boost::shared_ptr<GaussianFactor> linearize(
│ │ │ │ -
330 const Values& values) const override {
│ │ │ │ -
331 return linearizeDamped(values);
│ │ │ │ -
332 }
│ │ │ │ -
│ │ │ │ -
333
│ │ │ │ -
│ │ │ │ -
338 bool triangulateAndComputeE(Matrix& E, const Cameras& cameras) const {
│ │ │ │ -
339 bool nonDegenerate = triangulateForLinearize(cameras);
│ │ │ │ -
340 if (nonDegenerate)
│ │ │ │ -
341 cameras.project2(*result_, boost::none, E);
│ │ │ │ -
342 return nonDegenerate;
│ │ │ │ -
343 }
│ │ │ │ -
│ │ │ │ -
344
│ │ │ │ -
│ │ │ │ -
349 bool triangulateAndComputeE(Matrix& E, const Values& values) const {
│ │ │ │ -
350 Cameras cameras = this->cameras(values);
│ │ │ │ -
351 return triangulateAndComputeE(E, cameras);
│ │ │ │ -
352 }
│ │ │ │ -
│ │ │ │ -
353
│ │ │ │ -
│ │ │ │ -
357 void computeJacobiansWithTriangulatedPoint(
│ │ │ │ -
358 typename Base::FBlocks& Fs, Matrix& E, Vector& b,
│ │ │ │ -
359 const Cameras& cameras) const {
│ │ │ │ -
360
│ │ │ │ -
361 if (!result_) {
│ │ │ │ -
362 // Handle degeneracy
│ │ │ │ -
363 // TODO check flag whether we should do this
│ │ │ │ -
364 Unit3 backProjected = cameras[0].backprojectPointAtInfinity(
│ │ │ │ -
365 this->measured_.at(0));
│ │ │ │ -
366 Base::computeJacobians(Fs, E, b, cameras, backProjected);
│ │ │ │ -
367 } else {
│ │ │ │ -
368 // valid result: just return Base version
│ │ │ │ -
369 Base::computeJacobians(Fs, E, b, cameras, *result_);
│ │ │ │ -
370 }
│ │ │ │ -
371 }
│ │ │ │ -
│ │ │ │ -
372
│ │ │ │ -
│ │ │ │ -
374 bool triangulateAndComputeJacobians(
│ │ │ │ -
375 typename Base::FBlocks& Fs, Matrix& E, Vector& b,
│ │ │ │ -
376 const Values& values) const {
│ │ │ │ -
377 Cameras cameras = this->cameras(values);
│ │ │ │ -
378 bool nonDegenerate = triangulateForLinearize(cameras);
│ │ │ │ -
379 if (nonDegenerate)
│ │ │ │ -
380 computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
│ │ │ │ -
381 return nonDegenerate;
│ │ │ │ -
382 }
│ │ │ │ -
│ │ │ │ -
383
│ │ │ │ -
│ │ │ │ -
385 bool triangulateAndComputeJacobiansSVD(
│ │ │ │ -
386 typename Base::FBlocks& Fs, Matrix& Enull, Vector& b,
│ │ │ │ -
387 const Values& values) const {
│ │ │ │ -
388 Cameras cameras = this->cameras(values);
│ │ │ │ -
389 bool nonDegenerate = triangulateForLinearize(cameras);
│ │ │ │ -
390 if (nonDegenerate)
│ │ │ │ -
391 Base::computeJacobiansSVD(Fs, Enull, b, cameras, *result_);
│ │ │ │ -
392 return nonDegenerate;
│ │ │ │ -
393 }
│ │ │ │ -
│ │ │ │ -
394
│ │ │ │ -
│ │ │ │ -
396 Vector reprojectionErrorAfterTriangulation(const Values& values) const {
│ │ │ │ -
397 Cameras cameras = this->cameras(values);
│ │ │ │ -
398 bool nonDegenerate = triangulateForLinearize(cameras);
│ │ │ │ -
399 if (nonDegenerate)
│ │ │ │ -
400 return Base::unwhitenedError(cameras, *result_);
│ │ │ │ -
401 else
│ │ │ │ -
402 return Vector::Zero(cameras.size() * 2);
│ │ │ │ -
403 }
│ │ │ │ -
│ │ │ │ -
404
│ │ │ │ -
│ │ │ │ -
411 double totalReprojectionError(const Cameras& cameras,
│ │ │ │ -
412 boost::optional<Point3> externalPoint = boost::none) const {
│ │ │ │ -
413
│ │ │ │ -
414 if (externalPoint)
│ │ │ │ -
415 result_ = TriangulationResult(*externalPoint);
│ │ │ │ -
416 else
│ │ │ │ -
417 result_ = triangulateSafe(cameras);
│ │ │ │ -
418
│ │ │ │ -
419 if (result_)
│ │ │ │ -
420 // All good, just use version in base class
│ │ │ │ -
421 return Base::totalReprojectionError(cameras, *result_);
│ │ │ │ -
422 else if (params_.degeneracyMode == HANDLE_INFINITY) {
│ │ │ │ -
423 // Otherwise, manage the exceptions with rotation-only factors
│ │ │ │ -
424 Unit3 backprojected = cameras.front().backprojectPointAtInfinity(
│ │ │ │ -
425 this->measured_.at(0));
│ │ │ │ -
426 return Base::totalReprojectionError(cameras, backprojected);
│ │ │ │ -
427 } else
│ │ │ │ -
428 // if we don't want to manage the exceptions we discard the factor
│ │ │ │ -
429 return 0.0;
│ │ │ │ -
430 }
│ │ │ │ -
│ │ │ │ -
431
│ │ │ │ -
│ │ │ │ -
433 double error(const Values& values) const override {
│ │ │ │ -
434 if (this->active(values)) {
│ │ │ │ -
435 return totalReprojectionError(Base::cameras(values));
│ │ │ │ -
436 } else { // else of active flag
│ │ │ │ -
437 return 0.0;
│ │ │ │ -
438 }
│ │ │ │ -
439 }
│ │ │ │ -
│ │ │ │ -
440
│ │ │ │ -
│ │ │ │ -
442 TriangulationResult point() const {
│ │ │ │ -
443 return result_;
│ │ │ │ -
444 }
│ │ │ │ -
│ │ │ │ -
445
│ │ │ │ -
│ │ │ │ -
447 TriangulationResult point(const Values& values) const {
│ │ │ │ -
448 Cameras cameras = this->cameras(values);
│ │ │ │ -
449 return triangulateSafe(cameras);
│ │ │ │ -
450 }
│ │ │ │ -
│ │ │ │ -
451
│ │ │ │ -
453 bool isValid() const { return result_.valid(); }
│ │ │ │ -
454
│ │ │ │ -
456 bool isDegenerate() const { return result_.degenerate(); }
│ │ │ │ -
457
│ │ │ │ -
459 bool isPointBehindCamera() const { return result_.behindCamera(); }
│ │ │ │ -
460
│ │ │ │ -
462 bool isOutlier() const { return result_.outlier(); }
│ │ │ │ -
463
│ │ │ │ -
465 bool isFarPoint() const { return result_.farPoint(); }
│ │ │ │ -
466
│ │ │ │ -
467 private:
│ │ │ │ -
468
│ │ │ │ -
470 friend class boost::serialization::access;
│ │ │ │ -
471 template<class ARCHIVE>
│ │ │ │ -
472 void serialize(ARCHIVE & ar, const unsigned int version) {
│ │ │ │ -
473 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
474 ar & BOOST_SERIALIZATION_NVP(params_);
│ │ │ │ -
475 ar & BOOST_SERIALIZATION_NVP(result_);
│ │ │ │ -
476 ar & BOOST_SERIALIZATION_NVP(cameraPosesTriangulation_);
│ │ │ │ -
477 }
│ │ │ │ -
478}
│ │ │ │ -
│ │ │ │ -
479;
│ │ │ │ -
480
│ │ │ │ -
482template<class CAMERA>
│ │ │ │ -
│ │ │ │ -
483struct traits<SmartProjectionFactor<CAMERA> > : public Testable<
│ │ │ │ -
484 SmartProjectionFactor<CAMERA> > {
│ │ │ │ -
485};
│ │ │ │ -
│ │ │ │ -
486
│ │ │ │ -
487} // \ namespace gtsam
│ │ │ │ -
triangulation.h
Functions for triangulation.
│ │ │ │ -
SmartFactorParams.h
Collect common parameters for SmartProjection and SmartStereoProjection factors.
│ │ │ │ -
SmartFactorBase.h
Base class to create smart factors on poses or cameras.
│ │ │ │ -
dataset.h
utility functions for loading datasets
│ │ │ │ +
│ │ │ │ +
237 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
238 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ +
239 gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
│ │ │ │ +
│ │ │ │ +
240
│ │ │ │ +
│ │ │ │ +
246 void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ +
247 Base::print(s, keyFormatter);
│ │ │ │ +
248 traits<Point2>::Print(measured_, s + ".z");
│ │ │ │ +
249 }
│ │ │ │ +
│ │ │ │ +
250
│ │ │ │ +
│ │ │ │ +
254 bool equals(const NonlinearFactor &p, double tol = 1e-9) const override {
│ │ │ │ +
255 const This* e = dynamic_cast<const This*>(&p);
│ │ │ │ +
256 return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol);
│ │ │ │ +
257 }
│ │ │ │ +
│ │ │ │ +
258
│ │ │ │ +
│ │ │ │ +
260 Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib,
│ │ │ │ +
261 boost::optional<Matrix&> H1=boost::none,
│ │ │ │ +
262 boost::optional<Matrix&> H2=boost::none,
│ │ │ │ +
263 boost::optional<Matrix&> H3=boost::none) const override
│ │ │ │ +
264 {
│ │ │ │ +
265 try {
│ │ │ │ +
266 Camera camera(pose3,calib);
│ │ │ │ +
267 return camera.project(point, H1, H2, H3) - measured_;
│ │ │ │ +
268 }
│ │ │ │ +
269 catch( CheiralityException& e) {
│ │ │ │ +
270 if (H1) *H1 = Matrix::Zero(2, 6);
│ │ │ │ +
271 if (H2) *H2 = Matrix::Zero(2, 3);
│ │ │ │ +
272 if (H3) *H3 = Matrix::Zero(2, DimK);
│ │ │ │ +
273 std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
│ │ │ │ +
274 << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
│ │ │ │ +
275 }
│ │ │ │ +
276 return Z_2x1;
│ │ │ │ +
277 }
│ │ │ │ +
│ │ │ │ +
278
│ │ │ │ +
│ │ │ │ +
280 inline const Point2 measured() const {
│ │ │ │ +
281 return measured_;
│ │ │ │ +
282 }
│ │ │ │ +
│ │ │ │ +
283
│ │ │ │ +
284private:
│ │ │ │ +
286 friend class boost::serialization::access;
│ │ │ │ +
287 template<class Archive>
│ │ │ │ +
288 void serialize(Archive & ar, const unsigned int /*version*/) {
│ │ │ │ +
289 // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ +
290 ar & boost::serialization::make_nvp("NoiseModelFactor3",
│ │ │ │ +
291 boost::serialization::base_object<Base>(*this));
│ │ │ │ +
292 ar & BOOST_SERIALIZATION_NVP(measured_);
│ │ │ │ +
293 }
│ │ │ │ +
294};
│ │ │ │ +
295
│ │ │ │ +
296template<class CALIBRATION>
│ │ │ │ +
│ │ │ │ +
297struct traits<GeneralSFMFactor2<CALIBRATION> > : Testable<
│ │ │ │ +
298 GeneralSFMFactor2<CALIBRATION> > {
│ │ │ │ +
299};
│ │ │ │ +
│ │ │ │ +
300
│ │ │ │ +
301} //namespace
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ +
SymmetricBlockMatrix.h
Access to matrices via blocks of pre-defined sizes.
│ │ │ │ +
timing.h
Timing utilities.
│ │ │ │ +
types.h
Typedefs for easier changing of types.
│ │ │ │ +
Point2.h
2D Point
│ │ │ │ +
Pose3.h
3D Pose
│ │ │ │ +
Point3.h
3D Point
│ │ │ │ +
PinholeCamera.h
Base class for all pinhole cameras.
│ │ │ │ + │ │ │ │ +
BinaryJacobianFactor.h
A binary JacobianFactor specialization that uses fixed matrix math for speed.
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::triangulateSafe
TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters &params)
triangulateSafe: extensive checking of the outcome
Definition triangulation.h:680
│ │ │ │ +
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │ +
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::FixedDimension
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition Manifold.h:164
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition CameraSet.h:36
│ │ │ │ -
gtsam::CameraSet::SchurComplement
static SymmetricBlockMatrix SchurComplement(const std::vector< Eigen::Matrix< double, ZDim, ND >, Eigen::aligned_allocator< Eigen::Matrix< double, ZDim, ND > > > &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b)
Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = F' * F - F' * E * P * ...
Definition CameraSet.h:150
│ │ │ │ -
gtsam::CameraSet::project2
ZVector project2(const POINT &point, boost::optional< FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const
Project a point (possibly Unit3 at infinity), with derivatives Note that F is a sparse block-diagonal...
Definition CameraSet.h:108
│ │ │ │ -
gtsam::TriangulationResult
TriangulationResult is an optional point, along with the reasons why it is invalid.
Definition triangulation.h:626
│ │ │ │ -
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
│ │ │ │ -
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
│ │ │ │ +
gtsam::CheiralityException
Definition CalibratedCamera.h:32
│ │ │ │ +
gtsam::PinholeCamera
A pinhole camera class that has a Pose3 and a Calibration.
Definition PinholeCamera.h:33
│ │ │ │ +
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ +
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
│ │ │ │ +
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
│ │ │ │
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition NonlinearFactor.h:118
│ │ │ │ -
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
gtsam::SmartFactorBase
Base class for smart factors.
Definition SmartFactorBase.h:50
│ │ │ │ -
gtsam::SmartFactorBase::computeJacobians
void computeJacobians(FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras, const POINT &point) const
Compute F, E, and b (called below in both vanilla and SVD versions), where F is a vector of derivativ...
Definition SmartFactorBase.h:285
│ │ │ │ -
gtsam::SmartFactorBase::createRegularImplicitSchurFactor
boost::shared_ptr< RegularImplicitSchurFactor< CAMERA > > createRegularImplicitSchurFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0, bool diagonalDamping=false) const
Return Jacobians as RegularImplicitSchurFactor with raw access.
Definition SmartFactorBase.h:356
│ │ │ │ -
gtsam::SmartFactorBase::Dim
static const int Dim
Camera dimension.
Definition SmartFactorBase.h:60
│ │ │ │ -
gtsam::SmartFactorBase::cameras
virtual Cameras cameras(const Values &values) const
Collect all cameras: important that in key order.
Definition SmartFactorBase.h:162
│ │ │ │ -
gtsam::SmartFactorBase::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, const POINT &point) const
Calculate the error of the factor.
Definition SmartFactorBase.h:267
│ │ │ │ -
gtsam::SmartFactorBase::computeJacobiansSVD
void computeJacobiansSVD(FBlocks &Fs, Matrix &Enull, Vector &b, const Cameras &cameras, const POINT &point) const
SVD version that produces smaller Jacobian matrices by doing an SVD decomposition on E,...
Definition SmartFactorBase.h:300
│ │ │ │ -
gtsam::SmartFactorBase::measured_
ZVector measured_
Measurements for each of the m views.
Definition SmartFactorBase.h:79
│ │ │ │ -
gtsam::SmartFactorBase::unwhitenedError
Vector unwhitenedError(const Cameras &cameras, const POINT &point, boost::optional< typename Cameras::FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const
Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and derivatives.
Definition SmartFactorBase.h:204
│ │ │ │ -
gtsam::SmartFactorBase::whitenJacobians
void whitenJacobians(FBlocks &F, Matrix &E, Vector &b) const
Whiten the Jacobians computed by computeJacobians using noiseModel_.
Definition SmartFactorBase.h:347
│ │ │ │ -
gtsam::SmartFactorBase::createJacobianSVDFactor
boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0) const
Return Jacobians as JacobianFactorSVD.
Definition SmartFactorBase.h:386
│ │ │ │ -
gtsam::SmartFactorBase::createJacobianQFactor
boost::shared_ptr< JacobianFactorQ< Dim, ZDim > > createJacobianQFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0, bool diagonalDamping=false) const
Return Jacobians as JacobianFactorQ.
Definition SmartFactorBase.h:369
│ │ │ │ -
gtsam::SmartFactorBase::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition SmartFactorBase.h:174
│ │ │ │ -
gtsam::SmartFactorBase::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition SmartFactorBase.h:187
│ │ │ │ -
gtsam::SmartProjectionParams
Definition SmartFactorParams.h:42
│ │ │ │ -
gtsam::SmartProjectionParams::linearizationMode
LinearizationMode linearizationMode
How to linearize the factor.
Definition SmartFactorParams.h:44
│ │ │ │ -
gtsam::SmartProjectionParams::retriangulationThreshold
double retriangulationThreshold
threshold to decide whether to re-triangulate
Definition SmartFactorParams.h:50
│ │ │ │ -
gtsam::SmartProjectionParams::degeneracyMode
DegeneracyMode degeneracyMode
How to linearize the factor.
Definition SmartFactorParams.h:45
│ │ │ │ -
gtsam::SmartProjectionFactor
SmartProjectionFactor: triangulates point and keeps an estimate of it around.
Definition SmartProjectionFactor.h:45
│ │ │ │ -
gtsam::SmartProjectionFactor::decideIfTriangulate
bool decideIfTriangulate(const Cameras &cameras) const
Check if the new linearization point is the same as the one used for previous triangulation.
Definition SmartProjectionFactor.h:128
│ │ │ │ -
gtsam::SmartProjectionFactor::createJacobianQFactor
boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > createJacobianQFactor(const Values &values, double lambda) const
Create JacobianFactorQ factor, takes values.
Definition SmartProjectionFactor.h:261
│ │ │ │ -
gtsam::SmartProjectionFactor::createJacobianSVDFactor
boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, double lambda) const
Different (faster) way to compute a JacobianFactorSVD factor.
Definition SmartProjectionFactor.h:267
│ │ │ │ -
gtsam::SmartProjectionFactor::computeJacobiansWithTriangulatedPoint
void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras) const
Compute F, E only (called below in both vanilla and SVD versions) Assumes the point has been computed...
Definition SmartProjectionFactor.h:357
│ │ │ │ -
gtsam::SmartProjectionFactor::linearizeToHessian
virtual boost::shared_ptr< RegularHessianFactor< Base::Dim > > linearizeToHessian(const Values &values, double lambda=0.0) const
Linearize to a Hessianfactor.
Definition SmartProjectionFactor.h:277
│ │ │ │ -
gtsam::SmartProjectionFactor::isOutlier
bool isOutlier() const
return the outlier state
Definition SmartProjectionFactor.h:462
│ │ │ │ -
gtsam::SmartProjectionFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition SmartProjectionFactor.h:103
│ │ │ │ -
gtsam::SmartProjectionFactor::triangulateAndComputeE
bool triangulateAndComputeE(Matrix &E, const Values &values) const
Triangulate and compute derivative of error with respect to point.
Definition SmartProjectionFactor.h:349
│ │ │ │ -
gtsam::SmartProjectionFactor::linearizeDamped
boost::shared_ptr< GaussianFactor > linearizeDamped(const Cameras &cameras, const double lambda=0.0) const
Linearize to Gaussian Factor.
Definition SmartProjectionFactor.h:299
│ │ │ │ -
gtsam::SmartProjectionFactor::isFarPoint
bool isFarPoint() const
return the farPoint state
Definition SmartProjectionFactor.h:465
│ │ │ │ -
gtsam::SmartProjectionFactor::result_
TriangulationResult result_
result from triangulateSafe
Definition SmartProjectionFactor.h:63
│ │ │ │ -
gtsam::SmartProjectionFactor::createHessianFactor
boost::shared_ptr< RegularHessianFactor< Base::Dim > > createHessianFactor(const Cameras &cameras, const double lambda=0.0, bool diagonalDamping=false) const
Create a Hessianfactor that is an approximation of error(p).
Definition SmartProjectionFactor.h:199
│ │ │ │ -
gtsam::SmartProjectionFactor::triangulateSafe
TriangulationResult triangulateSafe(const Cameras &cameras) const
Call gtsam::triangulateSafe iff we need to re-triangulate.
Definition SmartProjectionFactor.h:174
│ │ │ │ -
gtsam::SmartProjectionFactor::~SmartProjectionFactor
~SmartProjectionFactor() override
Virtual destructor.
Definition SmartProjectionFactor.h:95
│ │ │ │ -
gtsam::SmartProjectionFactor::error
double error(const Values &values) const override
Calculate total reprojection error.
Definition SmartProjectionFactor.h:433
│ │ │ │ -
gtsam::SmartProjectionFactor::isValid
bool isValid() const
Is result valid?
Definition SmartProjectionFactor.h:453
│ │ │ │ -
gtsam::SmartProjectionFactor::cameraPosesTriangulation_
std::vector< Pose3, Eigen::aligned_allocator< Pose3 > > cameraPosesTriangulation_
current triangulation poses
Definition SmartProjectionFactor.h:65
│ │ │ │ -
gtsam::SmartProjectionFactor::triangulateAndComputeE
bool triangulateAndComputeE(Matrix &E, const Cameras &cameras) const
Triangulate and compute derivative of error with respect to point.
Definition SmartProjectionFactor.h:338
│ │ │ │ -
gtsam::SmartProjectionFactor::linearizeToImplicit
virtual boost::shared_ptr< RegularImplicitSchurFactor< CAMERA > > linearizeToImplicit(const Values &values, double lambda=0.0) const
Linearize to an Implicit Schur factor.
Definition SmartProjectionFactor.h:283
│ │ │ │ -
gtsam::SmartProjectionFactor::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, boost::optional< Point3 > externalPoint=boost::none) const
Calculate the error of the factor.
Definition SmartProjectionFactor.h:411
│ │ │ │ -
gtsam::SmartProjectionFactor::linearizeToJacobian
virtual boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > linearizeToJacobian(const Values &values, double lambda=0.0) const
Linearize to a JacobianfactorQ.
Definition SmartProjectionFactor.h:289
│ │ │ │ -
gtsam::SmartProjectionFactor::isDegenerate
bool isDegenerate() const
return the degenerate state
Definition SmartProjectionFactor.h:456
│ │ │ │ -
gtsam::SmartProjectionFactor::triangulateAndComputeJacobians
bool triangulateAndComputeJacobians(typename Base::FBlocks &Fs, Matrix &E, Vector &b, const Values &values) const
Version that takes values, and creates the point.
Definition SmartProjectionFactor.h:374
│ │ │ │ -
gtsam::SmartProjectionFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition SmartProjectionFactor.h:115
│ │ │ │ -
gtsam::SmartProjectionFactor::Camera
CAMERA Camera
shorthand for a set of cameras
Definition SmartProjectionFactor.h:74
│ │ │ │ -
gtsam::SmartProjectionFactor::createJacobianQFactor
boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > createJacobianQFactor(const Cameras &cameras, double lambda) const
Create JacobianFactorQ factor.
Definition SmartProjectionFactor.h:251
│ │ │ │ -
gtsam::SmartProjectionFactor::access
friend class boost::serialization::access
Serialization function.
Definition SmartProjectionFactor.h:470
│ │ │ │ -
gtsam::SmartProjectionFactor::point
TriangulationResult point(const Values &values) const
COMPUTE the landmark.
Definition SmartProjectionFactor.h:447
│ │ │ │ -
gtsam::SmartProjectionFactor::SmartProjectionFactor
SmartProjectionFactor(const SharedNoiseModel &sharedNoiseModel, const SmartProjectionParams &params=SmartProjectionParams())
Constructor.
Definition SmartProjectionFactor.h:87
│ │ │ │ -
gtsam::SmartProjectionFactor::isPointBehindCamera
bool isPointBehindCamera() const
return the cheirality status flag
Definition SmartProjectionFactor.h:459
│ │ │ │ -
gtsam::SmartProjectionFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &values) const override
linearize
Definition SmartProjectionFactor.h:329
│ │ │ │ -
gtsam::SmartProjectionFactor::triangulateAndComputeJacobiansSVD
bool triangulateAndComputeJacobiansSVD(typename Base::FBlocks &Fs, Matrix &Enull, Vector &b, const Values &values) const
takes values
Definition SmartProjectionFactor.h:385
│ │ │ │ -
gtsam::SmartProjectionFactor::reprojectionErrorAfterTriangulation
Vector reprojectionErrorAfterTriangulation(const Values &values) const
Calculate vector of re-projection errors, before applying noise model.
Definition SmartProjectionFactor.h:396
│ │ │ │ -
gtsam::SmartProjectionFactor::point
TriangulationResult point() const
return the landmark
Definition SmartProjectionFactor.h:442
│ │ │ │ -
gtsam::SmartProjectionFactor::triangulateForLinearize
bool triangulateForLinearize(const Cameras &cameras) const
Possibly re-triangulate before calculating Jacobians.
Definition SmartProjectionFactor.h:193
│ │ │ │ -
gtsam::SmartProjectionFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition SmartProjectionFactor.h:71
│ │ │ │ -
gtsam::SmartProjectionFactor::SmartProjectionFactor
SmartProjectionFactor()
Default constructor, only for serialization.
Definition SmartProjectionFactor.h:80
│ │ │ │ -
gtsam::SmartProjectionFactor::linearizeDamped
boost::shared_ptr< GaussianFactor > linearizeDamped(const Values &values, const double lambda=0.0) const
Linearize to Gaussian Factor.
Definition SmartProjectionFactor.h:321
│ │ │ │ - │ │ │ │ +
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition NonlinearFactor.h:223
│ │ │ │ +
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ +
gtsam::GeneralSFMFactor
Non-linear factor for a constraint derived from a 2D measurement.
Definition GeneralSFMFactor.h:60
│ │ │ │ +
gtsam::GeneralSFMFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &values) const override
Linearize using fixed-size matrices.
Definition GeneralSFMFactor.h:138
│ │ │ │ +
gtsam::GeneralSFMFactor::evaluateError
Vector evaluateError(const CAMERA &camera, const LANDMARK &point, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
h(x)-z
Definition GeneralSFMFactor.h:124
│ │ │ │ +
gtsam::GeneralSFMFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition GeneralSFMFactor.h:101
│ │ │ │ +
gtsam::GeneralSFMFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition GeneralSFMFactor.h:118
│ │ │ │ +
gtsam::GeneralSFMFactor::measured
const Point2 measured() const
return the measured
Definition GeneralSFMFactor.h:177
│ │ │ │ +
gtsam::GeneralSFMFactor::GeneralSFMFactor
GeneralSFMFactor(const Point2 &measured, const SharedNoiseModel &model, Key cameraKey, Key landmarkKey)
Constructor.
Definition GeneralSFMFactor.h:89
│ │ │ │ +
gtsam::GeneralSFMFactor::GeneralSFMFactor
GeneralSFMFactor()
default constructor
Definition GeneralSFMFactor.h:93
│ │ │ │ +
gtsam::GeneralSFMFactor::This
GeneralSFMFactor< CAMERA, LANDMARK > This
typedef for this object
Definition GeneralSFMFactor.h:76
│ │ │ │ +
gtsam::GeneralSFMFactor::~GeneralSFMFactor
~GeneralSFMFactor() override
destructor
Definition GeneralSFMFactor.h:98
│ │ │ │ +
gtsam::GeneralSFMFactor::Base
NoiseModelFactorN< CAMERA, LANDMARK > Base
typedef for the base class
Definition GeneralSFMFactor.h:77
│ │ │ │ +
gtsam::GeneralSFMFactor::print
void print(const std::string &s="SFMFactor", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition GeneralSFMFactor.h:110
│ │ │ │ +
gtsam::GeneralSFMFactor::access
friend class boost::serialization::access
Serialization function.
Definition GeneralSFMFactor.h:183
│ │ │ │ +
gtsam::GeneralSFMFactor::measured_
Point2 measured_
the 2D measurement
Definition GeneralSFMFactor.h:72
│ │ │ │ +
gtsam::GeneralSFMFactor2
Non-linear factor for a constraint derived from a 2D measurement.
Definition GeneralSFMFactor.h:204
│ │ │ │ +
gtsam::GeneralSFMFactor2::GeneralSFMFactor2
GeneralSFMFactor2()
default constructor
Definition GeneralSFMFactor.h:232
│ │ │ │ +
gtsam::GeneralSFMFactor2::~GeneralSFMFactor2
~GeneralSFMFactor2() override
destructor
Definition GeneralSFMFactor.h:234
│ │ │ │ +
gtsam::GeneralSFMFactor2::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition GeneralSFMFactor.h:237
│ │ │ │ +
gtsam::GeneralSFMFactor2::Base
NoiseModelFactorN< Pose3, Point3, CALIBRATION > Base
typedef for the base class
Definition GeneralSFMFactor.h:217
│ │ │ │ +
gtsam::GeneralSFMFactor2::evaluateError
Vector evaluateError(const Pose3 &pose3, const Point3 &point, const CALIBRATION &calib, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none) const override
h(x)-z
Definition GeneralSFMFactor.h:260
│ │ │ │ +
gtsam::GeneralSFMFactor2::print
void print(const std::string &s="SFMFactor2", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition GeneralSFMFactor.h:246
│ │ │ │ +
gtsam::GeneralSFMFactor2::measured_
Point2 measured_
the 2D measurement
Definition GeneralSFMFactor.h:211
│ │ │ │ +
gtsam::GeneralSFMFactor2::measured
const Point2 measured() const
return the measured
Definition GeneralSFMFactor.h:280
│ │ │ │ +
gtsam::GeneralSFMFactor2::Camera
PinholeCamera< CALIBRATION > Camera
typedef for camera type
Definition GeneralSFMFactor.h:216
│ │ │ │ +
gtsam::GeneralSFMFactor2::GeneralSFMFactor2
GeneralSFMFactor2(const Point2 &measured, const SharedNoiseModel &model, Key poseKey, Key landmarkKey, Key calibKey)
Constructor.
Definition GeneralSFMFactor.h:230
│ │ │ │ +
gtsam::GeneralSFMFactor2::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition GeneralSFMFactor.h:254
│ │ │ │ +
gtsam::GeneralSFMFactor2::access
friend class boost::serialization::access
Serialization function.
Definition GeneralSFMFactor.h:286
│ │ │ │ +
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,743 +1,477 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SmartProjectionFactor.h │ │ │ │ │ +GeneralSFMFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_s_l_a_m_/_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_s_l_a_m_/_S_m_a_r_t_F_a_c_t_o_r_P_a_r_a_m_s_._h> │ │ │ │ │ -24 │ │ │ │ │ -25#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_t_r_i_a_n_g_u_l_a_t_i_o_n_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_S_y_m_b_o_l_._h> │ │ │ │ │ -27#include <_g_t_s_a_m_/_s_l_a_m_/_d_a_t_a_s_e_t_._h> │ │ │ │ │ -28 │ │ │ │ │ -29#include │ │ │ │ │ -30#include │ │ │ │ │ -31#include │ │ │ │ │ -32 │ │ │ │ │ -33namespace _g_t_s_a_m { │ │ │ │ │ -34 │ │ │ │ │ -44template │ │ │ │ │ -_4_5class _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r: public _S_m_a_r_t_F_a_c_t_o_r_B_a_s_e { │ │ │ │ │ -46 │ │ │ │ │ -47public: │ │ │ │ │ -48 │ │ │ │ │ -49private: │ │ │ │ │ -50 typedef _S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_<_C_A_M_E_R_A_> _B_a_s_e; │ │ │ │ │ -51 typedef _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_<_C_A_M_E_R_A_> This; │ │ │ │ │ -52 typedef _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_<_C_A_M_E_R_A_> SmartProjectionCameraFactor; │ │ │ │ │ +21#pragma once │ │ │ │ │ +22 │ │ │ │ │ +23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_i_n_h_o_l_e_C_a_m_e_r_a_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_3_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_3_._h> │ │ │ │ │ +27#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +28#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_B_i_n_a_r_y_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ +29#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_N_o_i_s_e_M_o_d_e_l_._h> │ │ │ │ │ +30#include │ │ │ │ │ +31#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ +32#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ +33#include <_g_t_s_a_m_/_b_a_s_e_/_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_._h> │ │ │ │ │ +34#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ +35#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +36#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_c_t_o_r_._h> │ │ │ │ │ +37#include <_g_t_s_a_m_/_b_a_s_e_/_t_i_m_i_n_g_._h> │ │ │ │ │ +38 │ │ │ │ │ +39#include │ │ │ │ │ +40#include │ │ │ │ │ +41#include │ │ │ │ │ +42#include │ │ │ │ │ +43#include │ │ │ │ │ +44#include │ │ │ │ │ +45 │ │ │ │ │ +46namespace boost { │ │ │ │ │ +47namespace serialization { │ │ │ │ │ +48class access; │ │ │ │ │ +49} /* namespace serialization */ │ │ │ │ │ +50} /* namespace boost */ │ │ │ │ │ +51 │ │ │ │ │ +52namespace _g_t_s_a_m { │ │ │ │ │ 53 │ │ │ │ │ -54protected: │ │ │ │ │ -55 │ │ │ │ │ -58 _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s params_; │ │ │ │ │ -60 │ │ │ │ │ -_6_3 mutable _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t _r_e_s_u_l_t__; │ │ │ │ │ -64 mutable std::vector > │ │ │ │ │ -_6_5 _c_a_m_e_r_a_P_o_s_e_s_T_r_i_a_n_g_u_l_a_t_i_o_n__; │ │ │ │ │ -67 │ │ │ │ │ -68 public: │ │ │ │ │ +59template │ │ │ │ │ +_6_0class _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +61 │ │ │ │ │ +62 GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA) │ │ │ │ │ +63 GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK) │ │ │ │ │ +64 │ │ │ │ │ +65 static const int DimC = _F_i_x_e_d_D_i_m_e_n_s_i_o_n_<_C_A_M_E_R_A_>_:_:_v_a_l_u_e; │ │ │ │ │ +66 static const int DimL = _F_i_x_e_d_D_i_m_e_n_s_i_o_n_<_L_A_N_D_M_A_R_K_>_:_:_v_a_l_u_e; │ │ │ │ │ +67 typedef Eigen::Matrix JacobianC; │ │ │ │ │ +68 typedef Eigen::Matrix JacobianL; │ │ │ │ │ 69 │ │ │ │ │ -_7_1 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -72 │ │ │ │ │ -_7_4 typedef CAMERA _C_a_m_e_r_a; │ │ │ │ │ -75 typedef _C_a_m_e_r_a_S_e_t_<_C_A_M_E_R_A_> _C_a_m_e_r_a_s; │ │ │ │ │ -76 │ │ │ │ │ -_8_0 _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r() {} │ │ │ │ │ +70protected: │ │ │ │ │ +71 │ │ │ │ │ +_7_2 _P_o_i_n_t_2 _m_e_a_s_u_r_e_d__; │ │ │ │ │ +73 │ │ │ │ │ +74public: │ │ │ │ │ +75 │ │ │ │ │ +_7_6 typedef _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_<_C_A_M_E_R_A_,_ _L_A_N_D_M_A_R_K_> _T_h_i_s; │ │ │ │ │ +_7_7 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_C_A_M_E_R_A_,_ _L_A_N_D_M_A_R_K_> _B_a_s_e; │ │ │ │ │ +78 │ │ │ │ │ +79 // shorthand for a smart pointer to a factor │ │ │ │ │ +80 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ 81 │ │ │ │ │ -_8_7 _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r( │ │ │ │ │ -88 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& sharedNoiseModel, │ │ │ │ │ -89 const _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s& params = _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s()) │ │ │ │ │ -90 : _B_a_s_e(sharedNoiseModel), │ │ │ │ │ -91 params_(params), │ │ │ │ │ -92 _r_e_s_u_l_t__(_T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t::Degenerate()) {} │ │ │ │ │ -93 │ │ │ │ │ -_9_5 _~_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r() override { │ │ │ │ │ -96 } │ │ │ │ │ -97 │ │ │ │ │ -_1_0_3 void _p_r_i_n_t(const std::string& s = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ -104 DefaultKeyFormatter) const override { │ │ │ │ │ -105 std::cout << s << "SmartProjectionFactor\n"; │ │ │ │ │ -106 std::cout << "linearizationMode: " << params_._l_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e │ │ │ │ │ -107 << std::endl; │ │ │ │ │ -108 std::cout << "triangulationParameters:\n" << params_.triangulation │ │ │ │ │ -109 << std::endl; │ │ │ │ │ -110 std::cout << "result:\n" << _r_e_s_u_l_t__ << std::endl; │ │ │ │ │ -111 _B_a_s_e_:_:_p_r_i_n_t("", keyFormatter); │ │ │ │ │ -112 } │ │ │ │ │ -113 │ │ │ │ │ -_1_1_5 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& p, double tol = 1e-9) const override { │ │ │ │ │ -116 const This *e = dynamic_cast(&p); │ │ │ │ │ -117 return e && params_._l_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e == e->params_.linearizationMode │ │ │ │ │ -118 && _B_a_s_e_:_:_e_q_u_a_l_s(p, tol); │ │ │ │ │ -119 } │ │ │ │ │ -120 │ │ │ │ │ -_1_2_8 bool _d_e_c_i_d_e_I_f_T_r_i_a_n_g_u_l_a_t_e(const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s) const { │ │ │ │ │ -129 // Several calls to linearize will be done from the same linearization │ │ │ │ │ -130 // point, hence it is not needed to re-triangulate. Note that this is not │ │ │ │ │ -131 // yet "selecting linearization", that will come later, and we only check │ │ │ │ │ -if │ │ │ │ │ -132 // the current linearization is the "same" (up to tolerance) w.r.t. the │ │ │ │ │ -last │ │ │ │ │ -133 // time we triangulated the point. │ │ │ │ │ -134 │ │ │ │ │ -135 size_t m = _c_a_m_e_r_a_s.size(); │ │ │ │ │ -136 │ │ │ │ │ -137 bool retriangulate = false; │ │ │ │ │ -138 │ │ │ │ │ -139 // Definitely true if we do not have a previous linearization point or the │ │ │ │ │ -140 // new linearization point includes more poses. │ │ │ │ │ -141 if (_c_a_m_e_r_a_P_o_s_e_s_T_r_i_a_n_g_u_l_a_t_i_o_n__.empty() │ │ │ │ │ -142 || _c_a_m_e_r_a_s.size() != _c_a_m_e_r_a_P_o_s_e_s_T_r_i_a_n_g_u_l_a_t_i_o_n__.size()) │ │ │ │ │ -143 retriangulate = true; │ │ │ │ │ -144 │ │ │ │ │ -145 // Otherwise, check poses against cache. │ │ │ │ │ -146 if (!retriangulate) { │ │ │ │ │ -147 for (size_t i = 0; i < _c_a_m_e_r_a_s.size(); i++) { │ │ │ │ │ -148 if (!_c_a_m_e_r_a_s[i].pose()._e_q_u_a_l_s(_c_a_m_e_r_a_P_o_s_e_s_T_r_i_a_n_g_u_l_a_t_i_o_n__[i], │ │ │ │ │ -149 params_._r_e_t_r_i_a_n_g_u_l_a_t_i_o_n_T_h_r_e_s_h_o_l_d)) { │ │ │ │ │ -150 retriangulate = true; // at least two poses are different, hence we │ │ │ │ │ -retriangulate │ │ │ │ │ -151 break; │ │ │ │ │ -152 } │ │ │ │ │ -153 } │ │ │ │ │ -154 } │ │ │ │ │ -155 │ │ │ │ │ -156 // Store the current poses used for triangulation if we will re- │ │ │ │ │ -triangulate. │ │ │ │ │ -157 if (retriangulate) { │ │ │ │ │ -158 _c_a_m_e_r_a_P_o_s_e_s_T_r_i_a_n_g_u_l_a_t_i_o_n__.clear(); │ │ │ │ │ -159 _c_a_m_e_r_a_P_o_s_e_s_T_r_i_a_n_g_u_l_a_t_i_o_n__.reserve(m); │ │ │ │ │ -160 for (size_t i = 0; i < m; i++) │ │ │ │ │ -161 // cameraPosesTriangulation_[i] = cameras[i].pose(); │ │ │ │ │ -162 _c_a_m_e_r_a_P_o_s_e_s_T_r_i_a_n_g_u_l_a_t_i_o_n__.push_back(_c_a_m_e_r_a_s[i].pose()); │ │ │ │ │ -163 } │ │ │ │ │ -164 │ │ │ │ │ -165 return retriangulate; │ │ │ │ │ +_8_9 _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r(const _P_o_i_n_t_2& _m_e_a_s_u_r_e_d, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model, │ │ │ │ │ +90 _K_e_y cameraKey, _K_e_y landmarkKey) │ │ │ │ │ +91 : _B_a_s_e(model, cameraKey, landmarkKey), _m_e_a_s_u_r_e_d__(_m_e_a_s_u_r_e_d) {} │ │ │ │ │ +92 │ │ │ │ │ +_9_3 _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r() : _m_e_a_s_u_r_e_d__(0.0, 0.0) {} │ │ │ │ │ +95 _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r(const _P_o_i_n_t_2& p) : _m_e_a_s_u_r_e_d__(p) {} │ │ │ │ │ +97 _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r(double x, double y) : _m_e_a_s_u_r_e_d__(x, y) {} │ │ │ │ │ +_9_8 │ │ │ │ │ +99 _~_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r() override {} │ │ │ │ │ +100 │ │ │ │ │ +102 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +103 return boost::static_pointer_cast( │ │ │ │ │ +104 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this)));} │ │ │ │ │ +105 │ │ │ │ │ +111 void _p_r_i_n_t(const std::string& s = "SFMFactor", const _K_e_y_F_o_r_m_a_t_t_e_r& │ │ │ │ │ +keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ +112 _B_a_s_e_:_:_p_r_i_n_t(s, keyFormatter); │ │ │ │ │ +113 traits::Print(_m_e_a_s_u_r_e_d__, s + ".z"); │ │ │ │ │ +114 } │ │ │ │ │ +115 │ │ │ │ │ +119 bool _e_q_u_a_l_s(const NonlinearFactor &p, double tol = 1e-9) const override { │ │ │ │ │ +120 const _T_h_i_s* e = dynamic_cast(&p); │ │ │ │ │ +121 return e && _B_a_s_e_:_:_e_q_u_a_l_s(p, tol) && traits::Equals(this->measured_, │ │ │ │ │ +e->measured_, tol); │ │ │ │ │ +122 } │ │ │ │ │ +123 │ │ │ │ │ +125 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const CAMERA& camera, const LANDMARK& point, │ │ │ │ │ +126 boost::optional H1=boost::none, boost::optional │ │ │ │ │ +H2=boost::none) const override { │ │ │ │ │ +127 try { │ │ │ │ │ +128 return camera.project2(point,H1,H2) - _m_e_a_s_u_r_e_d__; │ │ │ │ │ +129 } │ │ │ │ │ +130 catch( CheiralityException& e) { │ │ │ │ │ +131 if (H1) *H1 = JacobianC::Zero(); │ │ │ │ │ +132 if (H2) *H2 = JacobianL::Zero(); │ │ │ │ │ +133 //TODO Print the exception via logging │ │ │ │ │ +134 return Z_2x1; │ │ │ │ │ +135 } │ │ │ │ │ +136 } │ │ │ │ │ +137 │ │ │ │ │ +139 boost::shared_ptr _l_i_n_e_a_r_i_z_e(const _V_a_l_u_e_s& values) const │ │ │ │ │ +override { │ │ │ │ │ +140 // Only linearize if the factor is active │ │ │ │ │ +141 if (!this->_a_c_t_i_v_e(values)) return boost::shared_ptr(); │ │ │ │ │ +142 │ │ │ │ │ +143 const _K_e_y key1 = this->key1(), key2 = this->key2(); │ │ │ │ │ +144 JacobianC H1; │ │ │ │ │ +145 JacobianL H2; │ │ │ │ │ +146 Vector2 b; │ │ │ │ │ +147 try { │ │ │ │ │ +148 const CAMERA& camera = values.at(key1); │ │ │ │ │ +149 const LANDMARK& point = values.at(key2); │ │ │ │ │ +150 b = _m_e_a_s_u_r_e_d() - camera.project2(point, H1, H2); │ │ │ │ │ +151 } catch (CheiralityException& e) { │ │ │ │ │ +152 H1.setZero(); │ │ │ │ │ +153 H2.setZero(); │ │ │ │ │ +154 b.setZero(); │ │ │ │ │ +155 //TODO Print the exception via logging │ │ │ │ │ +156 } │ │ │ │ │ +157 │ │ │ │ │ +158 // Whiten the system if needed │ │ │ │ │ +159 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& _n_o_i_s_e_M_o_d_e_l = this->noiseModel(); │ │ │ │ │ +160 if (noiseModel && !noiseModel->isUnit()) { │ │ │ │ │ +161 // TODO: implement WhitenSystem for fixed size matrices and include │ │ │ │ │ +162 // above │ │ │ │ │ +163 H1 = noiseModel->Whiten(H1); │ │ │ │ │ +164 H2 = noiseModel->Whiten(H2); │ │ │ │ │ +165 b = noiseModel->Whiten(b); │ │ │ │ │ 166 } │ │ │ │ │ 167 │ │ │ │ │ -_1_7_4 _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t _t_r_i_a_n_g_u_l_a_t_e_S_a_f_e(const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s) const { │ │ │ │ │ -175 │ │ │ │ │ -176 size_t m = _c_a_m_e_r_a_s.size(); │ │ │ │ │ -177 if (m < 2) // if we have a single pose the corresponding factor is │ │ │ │ │ -uninformative │ │ │ │ │ -178 return TriangulationResult::Degenerate(); │ │ │ │ │ -179 │ │ │ │ │ -180 bool retriangulate = _d_e_c_i_d_e_I_f_T_r_i_a_n_g_u_l_a_t_e(_c_a_m_e_r_a_s); │ │ │ │ │ -181 if (retriangulate) │ │ │ │ │ -182 _r_e_s_u_l_t__ = _g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_S_a_f_e(_c_a_m_e_r_a_s, this->_m_e_a_s_u_r_e_d__, │ │ │ │ │ -183 params_.triangulation); │ │ │ │ │ -184 return _r_e_s_u_l_t__; │ │ │ │ │ -185 } │ │ │ │ │ -186 │ │ │ │ │ -_1_9_3 bool _t_r_i_a_n_g_u_l_a_t_e_F_o_r_L_i_n_e_a_r_i_z_e(const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s) const { │ │ │ │ │ -194 _t_r_i_a_n_g_u_l_a_t_e_S_a_f_e(_c_a_m_e_r_a_s); // imperative, might reset result_ │ │ │ │ │ -195 return bool(_r_e_s_u_l_t__); │ │ │ │ │ -196 } │ │ │ │ │ -197 │ │ │ │ │ -_1_9_9 boost::shared_ptr > _c_r_e_a_t_e_H_e_s_s_i_a_n_F_a_c_t_o_r( │ │ │ │ │ -200 const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, const double lambda = 0.0, │ │ │ │ │ -201 bool diagonalDamping = false) const { │ │ │ │ │ -202 size_t numKeys = this->_k_e_y_s__.size(); │ │ │ │ │ -203 // Create structures for Hessian Factors │ │ │ │ │ -204 _K_e_y_V_e_c_t_o_r js; │ │ │ │ │ -205 std::vector Gs(numKeys * (numKeys + 1) / 2); │ │ │ │ │ -206 std::vector gs(numKeys); │ │ │ │ │ -207 │ │ │ │ │ -208 if (this->_m_e_a_s_u_r_e_d__.size() != cameras.size()) │ │ │ │ │ -209 throw std::runtime_error( │ │ │ │ │ -210 "SmartProjectionHessianFactor: this->measured_" │ │ │ │ │ -211 ".size() inconsistent with input"); │ │ │ │ │ +168 // Create new (unit) noiseModel, preserving constraints if applicable │ │ │ │ │ +169 SharedDiagonal model; │ │ │ │ │ +170 if (noiseModel && noiseModel->isConstrained()) { │ │ │ │ │ +171 model = boost::static_pointer_cast(noiseModel)- │ │ │ │ │ +>unit(); │ │ │ │ │ +172 } │ │ │ │ │ +173 │ │ │ │ │ +174 return boost::make_shared >(key1, H1, │ │ │ │ │ +key2, H2, b, model); │ │ │ │ │ +175 } │ │ │ │ │ +176 │ │ │ │ │ +178 inline const _P_o_i_n_t_2 _m_e_a_s_u_r_e_d() const { │ │ │ │ │ +179 return _m_e_a_s_u_r_e_d__; │ │ │ │ │ +180 } │ │ │ │ │ +181 │ │ │ │ │ +182private: │ │ │ │ │ +184 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +185 template │ │ │ │ │ +186 void serialize(Archive & ar, const unsigned int /*version*/) { │ │ │ │ │ +187 // NoiseModelFactor2 instead of NoiseModelFactorN for backward │ │ │ │ │ +compatibility │ │ │ │ │ +188 ar & boost::serialization::make_nvp("NoiseModelFactor2", │ │ │ │ │ +189 boost::serialization::base_object(*this)); │ │ │ │ │ +190 ar & BOOST_SERIALIZATION_NVP(_m_e_a_s_u_r_e_d__); │ │ │ │ │ +191 } │ │ │ │ │ +192}; │ │ │ │ │ +193 │ │ │ │ │ +194template │ │ │ │ │ +_1_9_5struct _t_r_a_i_t_s<_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r > : _T_e_s_t_a_b_l_e< │ │ │ │ │ +196 GeneralSFMFactor > { │ │ │ │ │ +197}; │ │ │ │ │ +198 │ │ │ │ │ +203template │ │ │ │ │ +_2_0_4class _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +205 │ │ │ │ │ +206 GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) │ │ │ │ │ +207 static const int DimK = _F_i_x_e_d_D_i_m_e_n_s_i_o_n_<_C_A_L_I_B_R_A_T_I_O_N_>_:_:_v_a_l_u_e; │ │ │ │ │ +208 │ │ │ │ │ +209protected: │ │ │ │ │ +210 │ │ │ │ │ +_2_1_1 _P_o_i_n_t_2 _m_e_a_s_u_r_e_d__; │ │ │ │ │ 212 │ │ │ │ │ -213 _t_r_i_a_n_g_u_l_a_t_e_S_a_f_e(_c_a_m_e_r_a_s); │ │ │ │ │ +213public: │ │ │ │ │ 214 │ │ │ │ │ -215 if (params_._d_e_g_e_n_e_r_a_c_y_M_o_d_e == ZERO_ON_DEGENERACY && !_r_e_s_u_l_t__) { │ │ │ │ │ -216 // failed: return"empty" Hessian │ │ │ │ │ -217 for (Matrix& m : Gs) m = Matrix::Zero(_B_a_s_e_:_:_D_i_m, _B_a_s_e_:_:_D_i_m); │ │ │ │ │ -218 for (Vector& v : gs) v = Vector::Zero(_B_a_s_e_:_:_D_i_m); │ │ │ │ │ -219 return boost::make_shared >(this->_k_e_y_s__, │ │ │ │ │ -220 Gs, gs, 0.0); │ │ │ │ │ -221 } │ │ │ │ │ -222 │ │ │ │ │ -223 // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols(). │ │ │ │ │ -224 typename Base::FBlocks Fs; │ │ │ │ │ -225 Matrix E; │ │ │ │ │ -226 Vector b; │ │ │ │ │ -227 _c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s_W_i_t_h_T_r_i_a_n_g_u_l_a_t_e_d_P_o_i_n_t(Fs, E, b, _c_a_m_e_r_a_s); │ │ │ │ │ -228 │ │ │ │ │ -229 // Whiten using noise model │ │ │ │ │ -230 _B_a_s_e_:_:_w_h_i_t_e_n_J_a_c_o_b_i_a_n_s(Fs, E, b); │ │ │ │ │ -231 │ │ │ │ │ -232 // build augmented hessian │ │ │ │ │ -233 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x augmentedHessian = // │ │ │ │ │ -234 _C_a_m_e_r_a_s_:_:_S_c_h_u_r_C_o_m_p_l_e_m_e_n_t(Fs, E, b, lambda, diagonalDamping); │ │ │ │ │ +215 typedef _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_<_C_A_L_I_B_R_A_T_I_O_N_> This; │ │ │ │ │ +_2_1_6 typedef _P_i_n_h_o_l_e_C_a_m_e_r_a_<_C_A_L_I_B_R_A_T_I_O_N_> _C_a_m_e_r_a; │ │ │ │ │ +_2_1_7 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_P_o_s_e_3_,_ _P_o_i_n_t_3_,_ _C_A_L_I_B_R_A_T_I_O_N_> _B_a_s_e; │ │ │ │ │ +218 │ │ │ │ │ +219 // shorthand for a smart pointer to a factor │ │ │ │ │ +220 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +221 │ │ │ │ │ +_2_3_0 _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2(const _P_o_i_n_t_2& _m_e_a_s_u_r_e_d, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model, │ │ │ │ │ +_K_e_y poseKey, _K_e_y landmarkKey, _K_e_y calibKey) : │ │ │ │ │ +231 _B_a_s_e(model, poseKey, landmarkKey, calibKey), _m_e_a_s_u_r_e_d__(_m_e_a_s_u_r_e_d) {} │ │ │ │ │ +_2_3_2 _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2():_m_e_a_s_u_r_e_d__(0.0,0.0) {} │ │ │ │ │ +233 │ │ │ │ │ +_2_3_4 _~_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2() override {} │ │ │ │ │ 235 │ │ │ │ │ -236 return boost::make_shared >( │ │ │ │ │ -237 this->_k_e_y_s__, augmentedHessian); │ │ │ │ │ -238 } │ │ │ │ │ -239 │ │ │ │ │ -240 // Create RegularImplicitSchurFactor factor. │ │ │ │ │ -241 boost::shared_ptr > │ │ │ │ │ -createRegularImplicitSchurFactor( │ │ │ │ │ -242 const Cameras& _c_a_m_e_r_a_s, double lambda) const { │ │ │ │ │ -243 if (_t_r_i_a_n_g_u_l_a_t_e_F_o_r_L_i_n_e_a_r_i_z_e(_c_a_m_e_r_a_s)) │ │ │ │ │ -244 return _B_a_s_e_:_:_c_r_e_a_t_e_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r(_c_a_m_e_r_a_s, *_r_e_s_u_l_t__, lambda); │ │ │ │ │ -245 else │ │ │ │ │ -246 // failed: return empty │ │ │ │ │ -247 return boost::shared_ptr >(); │ │ │ │ │ -248 } │ │ │ │ │ -249 │ │ │ │ │ -_2_5_1 boost::shared_ptr > _c_r_e_a_t_e_J_a_c_o_b_i_a_n_Q_F_a_c_t_o_r( │ │ │ │ │ -252 const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, double lambda) const { │ │ │ │ │ -253 if (_t_r_i_a_n_g_u_l_a_t_e_F_o_r_L_i_n_e_a_r_i_z_e(_c_a_m_e_r_a_s)) │ │ │ │ │ -254 return _B_a_s_e_:_:_c_r_e_a_t_e_J_a_c_o_b_i_a_n_Q_F_a_c_t_o_r(_c_a_m_e_r_a_s, *_r_e_s_u_l_t__, lambda); │ │ │ │ │ -255 else │ │ │ │ │ -256 // failed: return empty │ │ │ │ │ -257 return boost::make_shared >(this->_k_e_y_s__); │ │ │ │ │ -258 } │ │ │ │ │ -259 │ │ │ │ │ -_2_6_1 boost::shared_ptr > _c_r_e_a_t_e_J_a_c_o_b_i_a_n_Q_F_a_c_t_o_r( │ │ │ │ │ -262 const _V_a_l_u_e_s& values, double lambda) const { │ │ │ │ │ -263 return _c_r_e_a_t_e_J_a_c_o_b_i_a_n_Q_F_a_c_t_o_r(this->_c_a_m_e_r_a_s(values), lambda); │ │ │ │ │ -264 } │ │ │ │ │ -265 │ │ │ │ │ -_2_6_7 boost::shared_ptr _c_r_e_a_t_e_J_a_c_o_b_i_a_n_S_V_D_F_a_c_t_o_r( │ │ │ │ │ -268 const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, double lambda) const { │ │ │ │ │ -269 if (_t_r_i_a_n_g_u_l_a_t_e_F_o_r_L_i_n_e_a_r_i_z_e(_c_a_m_e_r_a_s)) │ │ │ │ │ -270 return _B_a_s_e_:_:_c_r_e_a_t_e_J_a_c_o_b_i_a_n_S_V_D_F_a_c_t_o_r(_c_a_m_e_r_a_s, *_r_e_s_u_l_t__, lambda); │ │ │ │ │ -271 else │ │ │ │ │ -272 // failed: return empty │ │ │ │ │ -273 return boost::make_shared >(this->_k_e_y_s__); │ │ │ │ │ -274 } │ │ │ │ │ -275 │ │ │ │ │ -_2_7_7 virtual boost::shared_ptr > │ │ │ │ │ -_l_i_n_e_a_r_i_z_e_T_o_H_e_s_s_i_a_n( │ │ │ │ │ -278 const _V_a_l_u_e_s& values, double lambda = 0.0) const { │ │ │ │ │ -279 return _c_r_e_a_t_e_H_e_s_s_i_a_n_F_a_c_t_o_r(this->_c_a_m_e_r_a_s(values), lambda); │ │ │ │ │ -280 } │ │ │ │ │ -281 │ │ │ │ │ -_2_8_3 virtual boost::shared_ptr > │ │ │ │ │ -_l_i_n_e_a_r_i_z_e_T_o_I_m_p_l_i_c_i_t( │ │ │ │ │ -284 const _V_a_l_u_e_s& values, double lambda = 0.0) const { │ │ │ │ │ -285 return createRegularImplicitSchurFactor(this->_c_a_m_e_r_a_s(values), lambda); │ │ │ │ │ -286 } │ │ │ │ │ -287 │ │ │ │ │ -_2_8_9 virtual boost::shared_ptr > │ │ │ │ │ -_l_i_n_e_a_r_i_z_e_T_o_J_a_c_o_b_i_a_n( │ │ │ │ │ -290 const _V_a_l_u_e_s& values, double lambda = 0.0) const { │ │ │ │ │ -291 return _c_r_e_a_t_e_J_a_c_o_b_i_a_n_Q_F_a_c_t_o_r(this->_c_a_m_e_r_a_s(values), lambda); │ │ │ │ │ -292 } │ │ │ │ │ -293 │ │ │ │ │ -_2_9_9 boost::shared_ptr _l_i_n_e_a_r_i_z_e_D_a_m_p_e_d(const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, │ │ │ │ │ -300 const double lambda = 0.0) const { │ │ │ │ │ -301 // depending on flag set on construction we may linearize to different │ │ │ │ │ -linear factors │ │ │ │ │ -302 switch (params_._l_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e) { │ │ │ │ │ -303 case HESSIAN: │ │ │ │ │ -304 return _c_r_e_a_t_e_H_e_s_s_i_a_n_F_a_c_t_o_r(_c_a_m_e_r_a_s, lambda); │ │ │ │ │ -305 case IMPLICIT_SCHUR: │ │ │ │ │ -306 return createRegularImplicitSchurFactor(_c_a_m_e_r_a_s, lambda); │ │ │ │ │ -307 case JACOBIAN_SVD: │ │ │ │ │ -308 return _c_r_e_a_t_e_J_a_c_o_b_i_a_n_S_V_D_F_a_c_t_o_r(_c_a_m_e_r_a_s, lambda); │ │ │ │ │ -309 case JACOBIAN_Q: │ │ │ │ │ -310 return _c_r_e_a_t_e_J_a_c_o_b_i_a_n_Q_F_a_c_t_o_r(_c_a_m_e_r_a_s, lambda); │ │ │ │ │ -311 default: │ │ │ │ │ -312 throw std::runtime_error("SmartFactorlinearize: unknown mode"); │ │ │ │ │ -313 } │ │ │ │ │ -314 } │ │ │ │ │ -315 │ │ │ │ │ -_3_2_1 boost::shared_ptr _l_i_n_e_a_r_i_z_e_D_a_m_p_e_d(const _V_a_l_u_e_s& values, │ │ │ │ │ -322 const double lambda = 0.0) const { │ │ │ │ │ -323 // depending on flag set on construction we may linearize to different │ │ │ │ │ -linear factors │ │ │ │ │ -324 _C_a_m_e_r_a_s _c_a_m_e_r_a_s = this->_c_a_m_e_r_a_s(values); │ │ │ │ │ -325 return _l_i_n_e_a_r_i_z_e_D_a_m_p_e_d(_c_a_m_e_r_a_s, lambda); │ │ │ │ │ -326 } │ │ │ │ │ -327 │ │ │ │ │ -_3_2_9 boost::shared_ptr _l_i_n_e_a_r_i_z_e( │ │ │ │ │ -330 const _V_a_l_u_e_s& values) const override { │ │ │ │ │ -331 return _l_i_n_e_a_r_i_z_e_D_a_m_p_e_d(values); │ │ │ │ │ -332 } │ │ │ │ │ -333 │ │ │ │ │ -_3_3_8 bool _t_r_i_a_n_g_u_l_a_t_e_A_n_d_C_o_m_p_u_t_e_E(Matrix& E, const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s) const { │ │ │ │ │ -339 bool nonDegenerate = _t_r_i_a_n_g_u_l_a_t_e_F_o_r_L_i_n_e_a_r_i_z_e(_c_a_m_e_r_a_s); │ │ │ │ │ -340 if (nonDegenerate) │ │ │ │ │ -341 _c_a_m_e_r_a_s._p_r_o_j_e_c_t_2(*_r_e_s_u_l_t__, boost::none, E); │ │ │ │ │ -342 return nonDegenerate; │ │ │ │ │ -343 } │ │ │ │ │ -344 │ │ │ │ │ -_3_4_9 bool _t_r_i_a_n_g_u_l_a_t_e_A_n_d_C_o_m_p_u_t_e_E(Matrix& E, const _V_a_l_u_e_s& values) const { │ │ │ │ │ -350 _C_a_m_e_r_a_s _c_a_m_e_r_a_s = this->_c_a_m_e_r_a_s(values); │ │ │ │ │ -351 return _t_r_i_a_n_g_u_l_a_t_e_A_n_d_C_o_m_p_u_t_e_E(E, _c_a_m_e_r_a_s); │ │ │ │ │ -352 } │ │ │ │ │ -353 │ │ │ │ │ -_3_5_7 void _c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s_W_i_t_h_T_r_i_a_n_g_u_l_a_t_e_d_P_o_i_n_t( │ │ │ │ │ -358 typename Base::FBlocks& Fs, Matrix& E, Vector& b, │ │ │ │ │ -359 const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s) const { │ │ │ │ │ -360 │ │ │ │ │ -361 if (!_r_e_s_u_l_t__) { │ │ │ │ │ -362 // Handle degeneracy │ │ │ │ │ -363 // TODO check flag whether we should do this │ │ │ │ │ -364 _U_n_i_t_3 backProjected = _c_a_m_e_r_a_s[0].backprojectPointAtInfinity( │ │ │ │ │ -365 this->_m_e_a_s_u_r_e_d__.at(0)); │ │ │ │ │ -366 _B_a_s_e_:_:_c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s(Fs, E, b, _c_a_m_e_r_a_s, backProjected); │ │ │ │ │ -367 } else { │ │ │ │ │ -368 // valid result: just return Base version │ │ │ │ │ -369 _B_a_s_e_:_:_c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s(Fs, E, b, _c_a_m_e_r_a_s, *_r_e_s_u_l_t__); │ │ │ │ │ -370 } │ │ │ │ │ -371 } │ │ │ │ │ -372 │ │ │ │ │ -_3_7_4 bool _t_r_i_a_n_g_u_l_a_t_e_A_n_d_C_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s( │ │ │ │ │ -375 typename Base::FBlocks& Fs, Matrix& E, Vector& b, │ │ │ │ │ -376 const _V_a_l_u_e_s& values) const { │ │ │ │ │ -377 _C_a_m_e_r_a_s _c_a_m_e_r_a_s = this->_c_a_m_e_r_a_s(values); │ │ │ │ │ -378 bool nonDegenerate = _t_r_i_a_n_g_u_l_a_t_e_F_o_r_L_i_n_e_a_r_i_z_e(_c_a_m_e_r_a_s); │ │ │ │ │ -379 if (nonDegenerate) │ │ │ │ │ -380 _c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s_W_i_t_h_T_r_i_a_n_g_u_l_a_t_e_d_P_o_i_n_t(Fs, E, b, _c_a_m_e_r_a_s); │ │ │ │ │ -381 return nonDegenerate; │ │ │ │ │ -382 } │ │ │ │ │ -383 │ │ │ │ │ -_3_8_5 bool _t_r_i_a_n_g_u_l_a_t_e_A_n_d_C_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s_S_V_D( │ │ │ │ │ -386 typename Base::FBlocks& Fs, Matrix& Enull, Vector& b, │ │ │ │ │ -387 const _V_a_l_u_e_s& values) const { │ │ │ │ │ -388 _C_a_m_e_r_a_s _c_a_m_e_r_a_s = this->_c_a_m_e_r_a_s(values); │ │ │ │ │ -389 bool nonDegenerate = _t_r_i_a_n_g_u_l_a_t_e_F_o_r_L_i_n_e_a_r_i_z_e(_c_a_m_e_r_a_s); │ │ │ │ │ -390 if (nonDegenerate) │ │ │ │ │ -391 _B_a_s_e_:_:_c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s_S_V_D(Fs, Enull, b, _c_a_m_e_r_a_s, *_r_e_s_u_l_t__); │ │ │ │ │ -392 return nonDegenerate; │ │ │ │ │ -393 } │ │ │ │ │ -394 │ │ │ │ │ -_3_9_6 Vector _r_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r_A_f_t_e_r_T_r_i_a_n_g_u_l_a_t_i_o_n(const _V_a_l_u_e_s& values) const { │ │ │ │ │ -397 _C_a_m_e_r_a_s _c_a_m_e_r_a_s = this->_c_a_m_e_r_a_s(values); │ │ │ │ │ -398 bool nonDegenerate = _t_r_i_a_n_g_u_l_a_t_e_F_o_r_L_i_n_e_a_r_i_z_e(_c_a_m_e_r_a_s); │ │ │ │ │ -399 if (nonDegenerate) │ │ │ │ │ -400 return _B_a_s_e_:_:_u_n_w_h_i_t_e_n_e_d_E_r_r_o_r(_c_a_m_e_r_a_s, *_r_e_s_u_l_t__); │ │ │ │ │ -401 else │ │ │ │ │ -402 return Vector::Zero(_c_a_m_e_r_a_s.size() * 2); │ │ │ │ │ -403 } │ │ │ │ │ -404 │ │ │ │ │ -_4_1_1 double _t_o_t_a_l_R_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r(const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, │ │ │ │ │ -412 boost::optional externalPoint = boost::none) const { │ │ │ │ │ -413 │ │ │ │ │ -414 if (externalPoint) │ │ │ │ │ -415 _r_e_s_u_l_t__ = _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t(*externalPoint); │ │ │ │ │ -416 else │ │ │ │ │ -417 _r_e_s_u_l_t__ = _t_r_i_a_n_g_u_l_a_t_e_S_a_f_e(_c_a_m_e_r_a_s); │ │ │ │ │ -418 │ │ │ │ │ -419 if (_r_e_s_u_l_t__) │ │ │ │ │ -420 // All good, just use version in base class │ │ │ │ │ -421 return _B_a_s_e_:_:_t_o_t_a_l_R_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r(_c_a_m_e_r_a_s, *_r_e_s_u_l_t__); │ │ │ │ │ -422 else if (params_._d_e_g_e_n_e_r_a_c_y_M_o_d_e == HANDLE_INFINITY) { │ │ │ │ │ -423 // Otherwise, manage the exceptions with rotation-only factors │ │ │ │ │ -424 _U_n_i_t_3 backprojected = _c_a_m_e_r_a_s.front().backprojectPointAtInfinity( │ │ │ │ │ -425 this->_m_e_a_s_u_r_e_d__.at(0)); │ │ │ │ │ -426 return _B_a_s_e_:_:_t_o_t_a_l_R_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r(cameras, backprojected); │ │ │ │ │ -427 } else │ │ │ │ │ -428 // if we don't want to manage the exceptions we discard the factor │ │ │ │ │ -429 return 0.0; │ │ │ │ │ -430 } │ │ │ │ │ -431 │ │ │ │ │ -_4_3_3 double _e_r_r_o_r(const _V_a_l_u_e_s& values) const override { │ │ │ │ │ -434 if (this->_a_c_t_i_v_e(values)) { │ │ │ │ │ -435 return _t_o_t_a_l_R_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r(_B_a_s_e_:_:_c_a_m_e_r_a_s(values)); │ │ │ │ │ -436 } else { // else of active flag │ │ │ │ │ -437 return 0.0; │ │ │ │ │ -438 } │ │ │ │ │ -439 } │ │ │ │ │ -440 │ │ │ │ │ -_4_4_2 _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t _p_o_i_n_t() const { │ │ │ │ │ -443 return _r_e_s_u_l_t__; │ │ │ │ │ -444 } │ │ │ │ │ -445 │ │ │ │ │ -_4_4_7 _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t _p_o_i_n_t(const _V_a_l_u_e_s& values) const { │ │ │ │ │ -448 _C_a_m_e_r_a_s _c_a_m_e_r_a_s = this->_c_a_m_e_r_a_s(values); │ │ │ │ │ -449 return _t_r_i_a_n_g_u_l_a_t_e_S_a_f_e(_c_a_m_e_r_a_s); │ │ │ │ │ -450 } │ │ │ │ │ -451 │ │ │ │ │ -_4_5_3 bool _i_s_V_a_l_i_d() const { return _r_e_s_u_l_t__.valid(); } │ │ │ │ │ -454 │ │ │ │ │ -_4_5_6 bool _i_s_D_e_g_e_n_e_r_a_t_e() const { return _r_e_s_u_l_t__.degenerate(); } │ │ │ │ │ -457 │ │ │ │ │ -_4_5_9 bool _i_s_P_o_i_n_t_B_e_h_i_n_d_C_a_m_e_r_a() const { return _r_e_s_u_l_t__.behindCamera(); } │ │ │ │ │ -460 │ │ │ │ │ -_4_6_2 bool _i_s_O_u_t_l_i_e_r() const { return _r_e_s_u_l_t__.outlier(); } │ │ │ │ │ -463 │ │ │ │ │ -_4_6_5 bool _i_s_F_a_r_P_o_i_n_t() const { return _r_e_s_u_l_t__.farPoint(); } │ │ │ │ │ -466 │ │ │ │ │ -467 private: │ │ │ │ │ -468 │ │ │ │ │ -_4_7_0 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -471 template │ │ │ │ │ -472 void serialize(ARCHIVE & ar, const unsigned int version) { │ │ │ │ │ -473 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -474 ar & BOOST_SERIALIZATION_NVP(params_); │ │ │ │ │ -475 ar & BOOST_SERIALIZATION_NVP(_r_e_s_u_l_t__); │ │ │ │ │ -476 ar & BOOST_SERIALIZATION_NVP(_c_a_m_e_r_a_P_o_s_e_s_T_r_i_a_n_g_u_l_a_t_i_o_n__); │ │ │ │ │ -477 } │ │ │ │ │ -478} │ │ │ │ │ -479; │ │ │ │ │ -480 │ │ │ │ │ -482template │ │ │ │ │ -_4_8_3struct _t_r_a_i_t_s<_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r > : public _T_e_s_t_a_b_l_e< │ │ │ │ │ -484 SmartProjectionFactor > { │ │ │ │ │ -485}; │ │ │ │ │ -486 │ │ │ │ │ -487} // \ namespace gtsam │ │ │ │ │ -_t_r_i_a_n_g_u_l_a_t_i_o_n_._h │ │ │ │ │ -Functions for triangulation. │ │ │ │ │ -_S_m_a_r_t_F_a_c_t_o_r_P_a_r_a_m_s_._h │ │ │ │ │ -Collect common parameters for SmartProjection and SmartStereoProjection │ │ │ │ │ -factors. │ │ │ │ │ -_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_._h │ │ │ │ │ -Base class to create smart factors on poses or cameras. │ │ │ │ │ -_d_a_t_a_s_e_t_._h │ │ │ │ │ -utility functions for loading datasets │ │ │ │ │ +_2_3_7 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +238 return boost::static_pointer_cast( │ │ │ │ │ +239 gtsam::NonlinearFactor::shared_ptr(new This(*this)));} │ │ │ │ │ +240 │ │ │ │ │ +_2_4_6 void _p_r_i_n_t(const std::string& s = "SFMFactor2", const _K_e_y_F_o_r_m_a_t_t_e_r& │ │ │ │ │ +keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ +247 _B_a_s_e_:_:_p_r_i_n_t(s, keyFormatter); │ │ │ │ │ +248 _t_r_a_i_t_s_<_P_o_i_n_t_2_>_:_:_P_r_i_n_t(_m_e_a_s_u_r_e_d__, s + ".z"); │ │ │ │ │ +249 } │ │ │ │ │ +250 │ │ │ │ │ +_2_5_4 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r &p, double tol = 1e-9) const override { │ │ │ │ │ +255 const This* e = dynamic_cast(&p); │ │ │ │ │ +256 return e && _B_a_s_e_:_:_e_q_u_a_l_s(p, tol) && _t_r_a_i_t_s_<_P_o_i_n_t_2_>_:_:_E_q_u_a_l_s(this->measured_, │ │ │ │ │ +e->measured_, tol); │ │ │ │ │ +257 } │ │ │ │ │ +258 │ │ │ │ │ +_2_6_0 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const _P_o_s_e_3& pose3, const _P_o_i_n_t_3& point, const │ │ │ │ │ +CALIBRATION &calib, │ │ │ │ │ +261 boost::optional H1=boost::none, │ │ │ │ │ +262 boost::optional H2=boost::none, │ │ │ │ │ +263 boost::optional H3=boost::none) const override │ │ │ │ │ +264 { │ │ │ │ │ +265 try { │ │ │ │ │ +266 _C_a_m_e_r_a camera(pose3,calib); │ │ │ │ │ +267 return camera.project(point, H1, H2, H3) - _m_e_a_s_u_r_e_d__; │ │ │ │ │ +268 } │ │ │ │ │ +269 catch( _C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n& e) { │ │ │ │ │ +270 if (H1) *H1 = Matrix::Zero(2, 6); │ │ │ │ │ +271 if (H2) *H2 = Matrix::Zero(2, 3); │ │ │ │ │ +272 if (H3) *H3 = Matrix::Zero(2, DimK); │ │ │ │ │ +273 std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) │ │ │ │ │ +274 << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; │ │ │ │ │ +275 } │ │ │ │ │ +276 return Z_2x1; │ │ │ │ │ +277 } │ │ │ │ │ +278 │ │ │ │ │ +_2_8_0 inline const _P_o_i_n_t_2 _m_e_a_s_u_r_e_d() const { │ │ │ │ │ +281 return _m_e_a_s_u_r_e_d__; │ │ │ │ │ +282 } │ │ │ │ │ +283 │ │ │ │ │ +284private: │ │ │ │ │ +_2_8_6 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +287 template │ │ │ │ │ +288 void serialize(Archive & ar, const unsigned int /*version*/) { │ │ │ │ │ +289 // NoiseModelFactor3 instead of NoiseModelFactorN for backward │ │ │ │ │ +compatibility │ │ │ │ │ +290 ar & boost::serialization::make_nvp("NoiseModelFactor3", │ │ │ │ │ +291 boost::serialization::base_object(*this)); │ │ │ │ │ +292 ar & BOOST_SERIALIZATION_NVP(_m_e_a_s_u_r_e_d__); │ │ │ │ │ +293 } │ │ │ │ │ +294}; │ │ │ │ │ +295 │ │ │ │ │ +296template │ │ │ │ │ +_2_9_7struct _t_r_a_i_t_s<_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2 > : _T_e_s_t_a_b_l_e< │ │ │ │ │ +298 GeneralSFMFactor2 > { │ │ │ │ │ +299}; │ │ │ │ │ +300 │ │ │ │ │ +301} //namespace │ │ │ │ │ +_V_e_c_t_o_r_._h │ │ │ │ │ +typedef and functions to augment Eigen's VectorXd │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ +_M_a_t_r_i_x_._h │ │ │ │ │ +typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +_M_a_n_i_f_o_l_d_._h │ │ │ │ │ +Base class and basic functions for Manifold types. │ │ │ │ │ +_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_._h │ │ │ │ │ +Access to matrices via blocks of pre-defined sizes. │ │ │ │ │ +_t_i_m_i_n_g_._h │ │ │ │ │ +Timing utilities. │ │ │ │ │ +_t_y_p_e_s_._h │ │ │ │ │ +Typedefs for easier changing of types. │ │ │ │ │ +_P_o_i_n_t_2_._h │ │ │ │ │ +2D Point │ │ │ │ │ +_P_o_s_e_3_._h │ │ │ │ │ +3D Pose │ │ │ │ │ +_P_o_i_n_t_3_._h │ │ │ │ │ +3D Point │ │ │ │ │ +_P_i_n_h_o_l_e_C_a_m_e_r_a_._h │ │ │ │ │ +Base class for all pinhole cameras. │ │ │ │ │ +_N_o_i_s_e_M_o_d_e_l_._h │ │ │ │ │ +_B_i_n_a_r_y_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ +A binary JacobianFactor specialization that uses fixed matrix math for speed. │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ +Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_S_a_f_e │ │ │ │ │ -TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const │ │ │ │ │ -typename CAMERA::MeasurementVector &measured, const TriangulationParameters │ │ │ │ │ -¶ms) │ │ │ │ │ -triangulateSafe: extensive checking of the outcome │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:680 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ +Vector2 Point2 │ │ │ │ │ +As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ +typedef Point2 to Vector2... │ │ │ │ │ +DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ +Vector3 Point3 │ │ │ │ │ +As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ +typedef Point3 to Vector3... │ │ │ │ │ +DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ _g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ Aliases. │ │ │ │ │ DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ -of blocks. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_F_i_x_e_d_D_i_m_e_n_s_i_o_n │ │ │ │ │ +Give fixed size dimension of a type, fails at compile time if dynamic. │ │ │ │ │ +DDeeffiinniittiioonn Manifold.h:164 │ │ │ │ │ _g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ A helper that implements the traits interface for GTSAM types. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t │ │ │ │ │ -A set of cameras, all with their own calibration. │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_S_c_h_u_r_C_o_m_p_l_e_m_e_n_t │ │ │ │ │ -static SymmetricBlockMatrix SchurComplement(const std::vector< Eigen::Matrix< │ │ │ │ │ -double, ZDim, ND >, Eigen::aligned_allocator< Eigen::Matrix< double, ZDim, ND > │ │ │ │ │ -> > &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector │ │ │ │ │ -&b) │ │ │ │ │ -Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = │ │ │ │ │ -F' * F - F' * E * P * ... │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:150 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_p_r_o_j_e_c_t_2 │ │ │ │ │ -ZVector project2(const POINT &point, boost::optional< FBlocks & > Fs=boost:: │ │ │ │ │ -none, boost::optional< Matrix & > E=boost::none) const │ │ │ │ │ -Project a point (possibly Unit3 at infinity), with derivatives Note that F is a │ │ │ │ │ -sparse block-diagonal... │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:108 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t │ │ │ │ │ -TriangulationResult is an optional point, along with the reasons why it is │ │ │ │ │ -invalid. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:626 │ │ │ │ │ -_g_t_s_a_m_:_:_U_n_i_t_3 │ │ │ │ │ -Represents a 3D point on a unit sphere. │ │ │ │ │ -DDeeffiinniittiioonn Unit3.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s__ │ │ │ │ │ -KeyVector keys_ │ │ │ │ │ -The keys involved in this factor. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a │ │ │ │ │ +A pinhole camera class that has a Pose3 and a Calibration. │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:33 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ +A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +virtual void print(const std::string &s="Factor", const KeyFormatter │ │ │ │ │ +&formatter=DefaultKeyFormatter) const │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn Factor.cpp:29 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const This &other, double tol=1e-9) const │ │ │ │ │ +check equality │ │ │ │ │ +DDeeffiinniittiioonn Factor.cpp:42 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ Nonlinear factor base class. │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_:_:_a_c_t_i_v_e │ │ │ │ │ virtual bool active(const Values &) const │ │ │ │ │ Checks whether a factor should be used based on a set of values. │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactor.h:118 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e │ │ │ │ │ -Base class for smart factors. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s │ │ │ │ │ -void computeJacobians(FBlocks &Fs, Matrix &E, Vector &b, const Cameras │ │ │ │ │ -&cameras, const POINT &point) const │ │ │ │ │ -Compute F, E, and b (called below in both vanilla and SVD versions), where F is │ │ │ │ │ -a vector of derivativ... │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:285 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_c_r_e_a_t_e_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r │ │ │ │ │ -boost::shared_ptr< RegularImplicitSchurFactor< CAMERA > > │ │ │ │ │ -createRegularImplicitSchurFactor(const Cameras &cameras, const Point3 &point, │ │ │ │ │ -double lambda=0.0, bool diagonalDamping=false) const │ │ │ │ │ -Return Jacobians as RegularImplicitSchurFactor with raw access. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:356 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_D_i_m │ │ │ │ │ -static const int Dim │ │ │ │ │ -Camera dimension. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:60 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_c_a_m_e_r_a_s │ │ │ │ │ -virtual Cameras cameras(const Values &values) const │ │ │ │ │ -Collect all cameras: important that in key order. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:162 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_t_o_t_a_l_R_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r │ │ │ │ │ -double totalReprojectionError(const Cameras &cameras, const POINT &point) const │ │ │ │ │ -Calculate the error of the factor. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:267 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s_S_V_D │ │ │ │ │ -void computeJacobiansSVD(FBlocks &Fs, Matrix &Enull, Vector &b, const Cameras │ │ │ │ │ -&cameras, const POINT &point) const │ │ │ │ │ -SVD version that produces smaller Jacobian matrices by doing an SVD │ │ │ │ │ -decomposition on E,... │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:300 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_m_e_a_s_u_r_e_d__ │ │ │ │ │ -ZVector measured_ │ │ │ │ │ -Measurements for each of the m views. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:79 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_u_n_w_h_i_t_e_n_e_d_E_r_r_o_r │ │ │ │ │ -Vector unwhitenedError(const Cameras &cameras, const POINT &point, boost:: │ │ │ │ │ -optional< typename Cameras::FBlocks & > Fs=boost::none, boost::optional< Matrix │ │ │ │ │ -& > E=boost::none) const │ │ │ │ │ -Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and derivatives. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:204 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_w_h_i_t_e_n_J_a_c_o_b_i_a_n_s │ │ │ │ │ -void whitenJacobians(FBlocks &F, Matrix &E, Vector &b) const │ │ │ │ │ -Whiten the Jacobians computed by computeJacobians using noiseModel_. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:347 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_c_r_e_a_t_e_J_a_c_o_b_i_a_n_S_V_D_F_a_c_t_o_r │ │ │ │ │ -boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras │ │ │ │ │ -&cameras, const Point3 &point, double lambda=0.0) const │ │ │ │ │ -Return Jacobians as JacobianFactorSVD. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:386 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_c_r_e_a_t_e_J_a_c_o_b_i_a_n_Q_F_a_c_t_o_r │ │ │ │ │ -boost::shared_ptr< JacobianFactorQ< Dim, ZDim > > createJacobianQFactor(const │ │ │ │ │ -Cameras &cameras, const Point3 &point, double lambda=0.0, bool │ │ │ │ │ -diagonalDamping=false) const │ │ │ │ │ -Return Jacobians as JacobianFactorQ. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:369 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:174 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_e_q_u_a_l_s │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_n_o_i_s_e_M_o_d_e_l │ │ │ │ │ +const SharedNoiseModel & noiseModel() const │ │ │ │ │ +access to the noise model │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:223 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ +A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ +variables. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r │ │ │ │ │ +Non-linear factor for a constraint derived from a 2D measurement. │ │ │ │ │ +DDeeffiinniittiioonn GeneralSFMFactor.h:60 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e │ │ │ │ │ +boost::shared_ptr< GaussianFactor > linearize(const Values &values) const │ │ │ │ │ +override │ │ │ │ │ +Linearize using fixed-size matrices. │ │ │ │ │ +DDeeffiinniittiioonn GeneralSFMFactor.h:138 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ +Vector evaluateError(const CAMERA &camera, const LANDMARK &point, boost:: │ │ │ │ │ +optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost:: │ │ │ │ │ +none) const override │ │ │ │ │ +h(x)-z │ │ │ │ │ +DDeeffiinniittiioonn GeneralSFMFactor.h:124 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ +gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn GeneralSFMFactor.h:101 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ bool equals(const NonlinearFactor &p, double tol=1e-9) const override │ │ │ │ │ equals │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:187 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorParams.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s_:_:_l_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e │ │ │ │ │ -LinearizationMode linearizationMode │ │ │ │ │ -How to linearize the factor. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorParams.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s_:_:_r_e_t_r_i_a_n_g_u_l_a_t_i_o_n_T_h_r_e_s_h_o_l_d │ │ │ │ │ -double retriangulationThreshold │ │ │ │ │ -threshold to decide whether to re-triangulate │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorParams.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s_:_:_d_e_g_e_n_e_r_a_c_y_M_o_d_e │ │ │ │ │ -DegeneracyMode degeneracyMode │ │ │ │ │ -How to linearize the factor. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorParams.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -SmartProjectionFactor: triangulates point and keeps an estimate of it around. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_d_e_c_i_d_e_I_f_T_r_i_a_n_g_u_l_a_t_e │ │ │ │ │ -bool decideIfTriangulate(const Cameras &cameras) const │ │ │ │ │ -Check if the new linearization point is the same as the one used for previous │ │ │ │ │ -triangulation. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:128 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_c_r_e_a_t_e_J_a_c_o_b_i_a_n_Q_F_a_c_t_o_r │ │ │ │ │ -boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > createJacobianQFactor │ │ │ │ │ -(const Values &values, double lambda) const │ │ │ │ │ -Create JacobianFactorQ factor, takes values. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:261 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_c_r_e_a_t_e_J_a_c_o_b_i_a_n_S_V_D_F_a_c_t_o_r │ │ │ │ │ -boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras │ │ │ │ │ -&cameras, double lambda) const │ │ │ │ │ -Different (faster) way to compute a JacobianFactorSVD factor. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:267 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s_W_i_t_h_T_r_i_a_n_g_u_l_a_t_e_d_P_o_i_n_t │ │ │ │ │ -void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks &Fs, Matrix │ │ │ │ │ -&E, Vector &b, const Cameras &cameras) const │ │ │ │ │ -Compute F, E only (called below in both vanilla and SVD versions) Assumes the │ │ │ │ │ -point has been computed... │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:357 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e_T_o_H_e_s_s_i_a_n │ │ │ │ │ -virtual boost::shared_ptr< RegularHessianFactor< Base::Dim > > │ │ │ │ │ -linearizeToHessian(const Values &values, double lambda=0.0) const │ │ │ │ │ -Linearize to a Hessianfactor. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:277 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_i_s_O_u_t_l_i_e_r │ │ │ │ │ -bool isOutlier() const │ │ │ │ │ -return the outlier state │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:462 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +DDeeffiinniittiioonn GeneralSFMFactor.h:118 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_:_:_m_e_a_s_u_r_e_d │ │ │ │ │ +const Point2 measured() const │ │ │ │ │ +return the measured │ │ │ │ │ +DDeeffiinniittiioonn GeneralSFMFactor.h:177 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r │ │ │ │ │ +GeneralSFMFactor(const Point2 &measured, const SharedNoiseModel &model, Key │ │ │ │ │ +cameraKey, Key landmarkKey) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn GeneralSFMFactor.h:89 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r │ │ │ │ │ +GeneralSFMFactor() │ │ │ │ │ +default constructor │ │ │ │ │ +DDeeffiinniittiioonn GeneralSFMFactor.h:93 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_:_:_T_h_i_s │ │ │ │ │ +GeneralSFMFactor< CAMERA, LANDMARK > This │ │ │ │ │ +typedef for this object │ │ │ │ │ +DDeeffiinniittiioonn GeneralSFMFactor.h:76 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_:_:_~_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r │ │ │ │ │ +~GeneralSFMFactor() override │ │ │ │ │ +destructor │ │ │ │ │ +DDeeffiinniittiioonn GeneralSFMFactor.h:98 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_:_:_B_a_s_e │ │ │ │ │ +NoiseModelFactorN< CAMERA, LANDMARK > Base │ │ │ │ │ +typedef for the base class │ │ │ │ │ +DDeeffiinniittiioonn GeneralSFMFactor.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="SFMFactor", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn GeneralSFMFactor.h:110 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn GeneralSFMFactor.h:183 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_:_:_m_e_a_s_u_r_e_d__ │ │ │ │ │ +Point2 measured_ │ │ │ │ │ +the 2D measurement │ │ │ │ │ +DDeeffiinniittiioonn GeneralSFMFactor.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2 │ │ │ │ │ +Non-linear factor for a constraint derived from a 2D measurement. │ │ │ │ │ +DDeeffiinniittiioonn GeneralSFMFactor.h:204 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2 │ │ │ │ │ +GeneralSFMFactor2() │ │ │ │ │ +default constructor │ │ │ │ │ +DDeeffiinniittiioonn GeneralSFMFactor.h:232 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_:_:_~_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2 │ │ │ │ │ +~GeneralSFMFactor2() override │ │ │ │ │ +destructor │ │ │ │ │ +DDeeffiinniittiioonn GeneralSFMFactor.h:234 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_:_:_c_l_o_n_e │ │ │ │ │ +gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn GeneralSFMFactor.h:237 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_:_:_B_a_s_e │ │ │ │ │ +NoiseModelFactorN< Pose3, Point3, CALIBRATION > Base │ │ │ │ │ +typedef for the base class │ │ │ │ │ +DDeeffiinniittiioonn GeneralSFMFactor.h:217 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ +Vector evaluateError(const Pose3 &pose3, const Point3 &point, const CALIBRATION │ │ │ │ │ +&calib, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > │ │ │ │ │ +H2=boost::none, boost::optional< Matrix & > H3=boost::none) const override │ │ │ │ │ +h(x)-z │ │ │ │ │ +DDeeffiinniittiioonn GeneralSFMFactor.h:260 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="SFMFactor2", const KeyFormatter │ │ │ │ │ &keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ print │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:103 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_t_r_i_a_n_g_u_l_a_t_e_A_n_d_C_o_m_p_u_t_e_E │ │ │ │ │ -bool triangulateAndComputeE(Matrix &E, const Values &values) const │ │ │ │ │ -Triangulate and compute derivative of error with respect to point. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:349 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e_D_a_m_p_e_d │ │ │ │ │ -boost::shared_ptr< GaussianFactor > linearizeDamped(const Cameras &cameras, │ │ │ │ │ -const double lambda=0.0) const │ │ │ │ │ -Linearize to Gaussian Factor. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:299 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_i_s_F_a_r_P_o_i_n_t │ │ │ │ │ -bool isFarPoint() const │ │ │ │ │ -return the farPoint state │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:465 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_r_e_s_u_l_t__ │ │ │ │ │ -TriangulationResult result_ │ │ │ │ │ -result from triangulateSafe │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:63 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_c_r_e_a_t_e_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -boost::shared_ptr< RegularHessianFactor< Base::Dim > > createHessianFactor │ │ │ │ │ -(const Cameras &cameras, const double lambda=0.0, bool diagonalDamping=false) │ │ │ │ │ -const │ │ │ │ │ -Create a Hessianfactor that is an approximation of error(p). │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:199 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_t_r_i_a_n_g_u_l_a_t_e_S_a_f_e │ │ │ │ │ -TriangulationResult triangulateSafe(const Cameras &cameras) const │ │ │ │ │ -Call gtsam::triangulateSafe iff we need to re-triangulate. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:174 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_~_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -~SmartProjectionFactor() override │ │ │ │ │ -Virtual destructor. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:95 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_e_r_r_o_r │ │ │ │ │ -double error(const Values &values) const override │ │ │ │ │ -Calculate total reprojection error. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:433 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_i_s_V_a_l_i_d │ │ │ │ │ -bool isValid() const │ │ │ │ │ -Is result valid? │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:453 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_c_a_m_e_r_a_P_o_s_e_s_T_r_i_a_n_g_u_l_a_t_i_o_n__ │ │ │ │ │ -std::vector< Pose3, Eigen::aligned_allocator< Pose3 > > │ │ │ │ │ -cameraPosesTriangulation_ │ │ │ │ │ -current triangulation poses │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_t_r_i_a_n_g_u_l_a_t_e_A_n_d_C_o_m_p_u_t_e_E │ │ │ │ │ -bool triangulateAndComputeE(Matrix &E, const Cameras &cameras) const │ │ │ │ │ -Triangulate and compute derivative of error with respect to point. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:338 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e_T_o_I_m_p_l_i_c_i_t │ │ │ │ │ -virtual boost::shared_ptr< RegularImplicitSchurFactor< CAMERA > > │ │ │ │ │ -linearizeToImplicit(const Values &values, double lambda=0.0) const │ │ │ │ │ -Linearize to an Implicit Schur factor. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:283 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_t_o_t_a_l_R_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r │ │ │ │ │ -double totalReprojectionError(const Cameras &cameras, boost::optional< Point3 > │ │ │ │ │ -externalPoint=boost::none) const │ │ │ │ │ -Calculate the error of the factor. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:411 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e_T_o_J_a_c_o_b_i_a_n │ │ │ │ │ -virtual boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > │ │ │ │ │ -linearizeToJacobian(const Values &values, double lambda=0.0) const │ │ │ │ │ -Linearize to a JacobianfactorQ. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:289 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_i_s_D_e_g_e_n_e_r_a_t_e │ │ │ │ │ -bool isDegenerate() const │ │ │ │ │ -return the degenerate state │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:456 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_t_r_i_a_n_g_u_l_a_t_e_A_n_d_C_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s │ │ │ │ │ -bool triangulateAndComputeJacobians(typename Base::FBlocks &Fs, Matrix &E, │ │ │ │ │ -Vector &b, const Values &values) const │ │ │ │ │ -Version that takes values, and creates the point. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:374 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +DDeeffiinniittiioonn GeneralSFMFactor.h:246 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_:_:_m_e_a_s_u_r_e_d__ │ │ │ │ │ +Point2 measured_ │ │ │ │ │ +the 2D measurement │ │ │ │ │ +DDeeffiinniittiioonn GeneralSFMFactor.h:211 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_:_:_m_e_a_s_u_r_e_d │ │ │ │ │ +const Point2 measured() const │ │ │ │ │ +return the measured │ │ │ │ │ +DDeeffiinniittiioonn GeneralSFMFactor.h:280 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_:_:_C_a_m_e_r_a │ │ │ │ │ +PinholeCamera< CALIBRATION > Camera │ │ │ │ │ +typedef for camera type │ │ │ │ │ +DDeeffiinniittiioonn GeneralSFMFactor.h:216 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2 │ │ │ │ │ +GeneralSFMFactor2(const Point2 &measured, const SharedNoiseModel &model, Key │ │ │ │ │ +poseKey, Key landmarkKey, Key calibKey) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn GeneralSFMFactor.h:230 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_:_:_e_q_u_a_l_s │ │ │ │ │ bool equals(const NonlinearFactor &p, double tol=1e-9) const override │ │ │ │ │ equals │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:115 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_C_a_m_e_r_a │ │ │ │ │ -CAMERA Camera │ │ │ │ │ -shorthand for a set of cameras │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_c_r_e_a_t_e_J_a_c_o_b_i_a_n_Q_F_a_c_t_o_r │ │ │ │ │ -boost::shared_ptr< JacobianFactorQ< Base::Dim, 2 > > createJacobianQFactor │ │ │ │ │ -(const Cameras &cameras, double lambda) const │ │ │ │ │ -Create JacobianFactorQ factor. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:251 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ +DDeeffiinniittiioonn GeneralSFMFactor.h:254 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_:_:_a_c_c_e_s_s │ │ │ │ │ friend class boost::serialization::access │ │ │ │ │ Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:470 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_p_o_i_n_t │ │ │ │ │ -TriangulationResult point(const Values &values) const │ │ │ │ │ -COMPUTE the landmark. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:447 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -SmartProjectionFactor(const SharedNoiseModel &sharedNoiseModel, const │ │ │ │ │ -SmartProjectionParams ¶ms=SmartProjectionParams()) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:87 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_i_s_P_o_i_n_t_B_e_h_i_n_d_C_a_m_e_r_a │ │ │ │ │ -bool isPointBehindCamera() const │ │ │ │ │ -return the cheirality status flag │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:459 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e │ │ │ │ │ -boost::shared_ptr< GaussianFactor > linearize(const Values &values) const │ │ │ │ │ -override │ │ │ │ │ -linearize │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:329 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_t_r_i_a_n_g_u_l_a_t_e_A_n_d_C_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s_S_V_D │ │ │ │ │ -bool triangulateAndComputeJacobiansSVD(typename Base::FBlocks &Fs, Matrix │ │ │ │ │ -&Enull, Vector &b, const Values &values) const │ │ │ │ │ -takes values │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:385 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_r_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r_A_f_t_e_r_T_r_i_a_n_g_u_l_a_t_i_o_n │ │ │ │ │ -Vector reprojectionErrorAfterTriangulation(const Values &values) const │ │ │ │ │ -Calculate vector of re-projection errors, before applying noise model. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:396 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_p_o_i_n_t │ │ │ │ │ -TriangulationResult point() const │ │ │ │ │ -return the landmark │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:442 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_t_r_i_a_n_g_u_l_a_t_e_F_o_r_L_i_n_e_a_r_i_z_e │ │ │ │ │ -bool triangulateForLinearize(const Cameras &cameras) const │ │ │ │ │ -Possibly re-triangulate before calculating Jacobians. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:193 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shorthand for a smart pointer to a factor │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:71 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -SmartProjectionFactor() │ │ │ │ │ -Default constructor, only for serialization. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:80 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e_D_a_m_p_e_d │ │ │ │ │ -boost::shared_ptr< GaussianFactor > linearizeDamped(const Values &values, const │ │ │ │ │ -double lambda=0.0) const │ │ │ │ │ -Linearize to Gaussian Factor. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:321 │ │ │ │ │ -_S_y_m_b_o_l_._h │ │ │ │ │ +DDeeffiinniittiioonn GeneralSFMFactor.h:286 │ │ │ │ │ +_V_a_l_u_e_s │ │ │ │ │ +In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ +a non-linear function... │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_._h │ │ │ │ │ + * _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01265_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/JacobianFactorQR.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/SmartFactorBase.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,95 +98,464 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
JacobianFactorQR.h
│ │ │ │ +
SmartFactorBase.h
│ │ │ │
│ │ │ │
│ │ │ │ -
1/*
│ │ │ │ -
2 * @file JacobianFactorQR.h
│ │ │ │ -
3 * @brief Jacobianfactor that combines and eliminates points
│ │ │ │ -
4 * @date Oct 27, 2013
│ │ │ │ -
5 * @uthor Frank Dellaert
│ │ │ │ -
6 */
│ │ │ │ -
7
│ │ │ │ -
8#pragma once
│ │ │ │ -
9#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ -
10#include <gtsam/linear/RegularJacobianFactor.h>
│ │ │ │ -
11#include <gtsam/inference/Symbol.h>
│ │ │ │ -
12
│ │ │ │ -
13namespace gtsam {
│ │ │ │ -
14
│ │ │ │ -
15class GaussianBayesNet;
│ │ │ │ -
16
│ │ │ │ -
20template<size_t D, size_t ZDim>
│ │ │ │ -
│ │ │ │ -
21class JacobianFactorQR: public RegularJacobianFactor<D> {
│ │ │ │ -
22
│ │ │ │ -
23 typedef RegularJacobianFactor<D> Base;
│ │ │ │ -
24 typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
│ │ │ │ -
25
│ │ │ │ -
26public:
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +
2
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
4 * Atlanta, Georgia 30332-0415
│ │ │ │ +
5 * All Rights Reserved
│ │ │ │ +
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
7
│ │ │ │ +
8 * See LICENSE for the license information
│ │ │ │ +
9
│ │ │ │ +
10 * -------------------------------------------------------------------------- */
│ │ │ │ +
11
│ │ │ │ +
22#pragma once
│ │ │ │ +
23
│ │ │ │ +
24#include <gtsam/slam/JacobianFactorQ.h>
│ │ │ │ +
25#include <gtsam/slam/JacobianFactorSVD.h>
│ │ │ │ +
26#include <gtsam/slam/RegularImplicitSchurFactor.h>
│ │ │ │
27
│ │ │ │ -
│ │ │ │ -
31 JacobianFactorQR(const KeyVector& keys,
│ │ │ │ -
32 const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix3& P,
│ │ │ │ -
33 const Vector& b, //
│ │ │ │ -
34 const SharedDiagonal& model = SharedDiagonal()) :
│ │ │ │ -
35 Base() {
│ │ │ │ -
36 // Create a number of Jacobian factors in a factor graph
│ │ │ │ -
37 GaussianFactorGraph gfg;
│ │ │ │ -
38 Symbol pointKey('p', 0);
│ │ │ │ -
39 for (size_t k = 0; k < FBlocks.size(); ++k) {
│ │ │ │ -
40 Key key = keys[k];
│ │ │ │ -
41 gfg.add(pointKey, E.block<ZDim, 3>(ZDim * k, 0), key, FBlocks[k],
│ │ │ │ -
42 b.segment < ZDim > (ZDim * k), model);
│ │ │ │ -
43 }
│ │ │ │ -
44 //gfg.print("gfg");
│ │ │ │ -
45
│ │ │ │ -
46 // eliminate the point
│ │ │ │ -
47 boost::shared_ptr<GaussianBayesNet> bn;
│ │ │ │ -
48 GaussianFactorGraph::shared_ptr fg;
│ │ │ │ -
49 KeyVector variables;
│ │ │ │ -
50 variables.push_back(pointKey);
│ │ │ │ -
51 boost::tie(bn, fg) = gfg.eliminatePartialSequential(variables, EliminateQR);
│ │ │ │ -
52 //fg->print("fg");
│ │ │ │ -
53
│ │ │ │ -
54 JacobianFactor::operator=(JacobianFactor(*fg));
│ │ │ │ -
55 }
│ │ │ │ -
│ │ │ │ -
56};
│ │ │ │ -
│ │ │ │ -
57// end class JacobianFactorQR
│ │ │ │ -
58
│ │ │ │ -
59}// end namespace gtsam
│ │ │ │ -
RegularJacobianFactor.h
JacobianFactor class with fixed sized blcoks.
│ │ │ │ -
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ -
gtsam::JacobianFactor::EliminateQR
friend GTSAM_EXPORT std::pair< boost::shared_ptr< GaussianConditional >, shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Densely partially eliminate with QR factorization, this is usually provided as an argument to one of ...
Definition JacobianFactor.cpp:789
│ │ │ │ +
28#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
29#include <gtsam/linear/RegularHessianFactor.h>
│ │ │ │ +
30#include <gtsam/geometry/CameraSet.h>
│ │ │ │ +
31
│ │ │ │ +
32#include <boost/optional.hpp>
│ │ │ │ +
33#include <boost/serialization/optional.hpp>
│ │ │ │ +
34#include <boost/make_shared.hpp>
│ │ │ │ +
35#include <vector>
│ │ │ │ +
36
│ │ │ │ +
37namespace gtsam {
│ │ │ │ +
38
│ │ │ │ +
49template<class CAMERA>
│ │ │ │ +
│ │ │ │ +
50class SmartFactorBase: public NonlinearFactor {
│ │ │ │ +
51
│ │ │ │ +
52private:
│ │ │ │ +
53 typedef NonlinearFactor Base;
│ │ │ │ +
54 typedef SmartFactorBase<CAMERA> This;
│ │ │ │ +
55 typedef typename CAMERA::Measurement Z;
│ │ │ │ +
56 typedef typename CAMERA::MeasurementVector ZVector;
│ │ │ │ +
57
│ │ │ │ +
58public:
│ │ │ │ +
59
│ │ │ │ +
60 static const int Dim = traits<CAMERA>::dimension;
│ │ │ │ +
61 static const int ZDim = traits<Z>::dimension;
│ │ │ │ +
62 typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F blocks (derivatives wrpt camera)
│ │ │ │ +
63 typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks; // vector of F blocks
│ │ │ │ +
64
│ │ │ │ +
65protected:
│ │ │ │ +
72 SharedIsotropic noiseModel_;
│ │ │ │ +
73
│ │ │ │ +
79 ZVector measured_;
│ │ │ │ +
80
│ │ │ │ +
81 boost::optional<Pose3>
│ │ │ │ +
82 body_P_sensor_;
│ │ │ │ +
83
│ │ │ │ +
84 // Cache for Fblocks, to avoid a malloc ever time we re-linearize
│ │ │ │ +
85 mutable FBlocks Fs;
│ │ │ │ +
86
│ │ │ │ +
87 public:
│ │ │ │ +
88 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
89
│ │ │ │ +
91 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
92
│ │ │ │ +
94 typedef CameraSet<CAMERA> Cameras;
│ │ │ │ +
95
│ │ │ │ +
97 SmartFactorBase() {}
│ │ │ │ +
98
│ │ │ │ +
│ │ │ │ +
100 SmartFactorBase(const SharedNoiseModel& sharedNoiseModel,
│ │ │ │ +
101 boost::optional<Pose3> body_P_sensor = boost::none,
│ │ │ │ +
102 size_t expectedNumberCameras = 10)
│ │ │ │ +
103 : body_P_sensor_(body_P_sensor), Fs(expectedNumberCameras) {
│ │ │ │ +
104
│ │ │ │ +
105 if (!sharedNoiseModel)
│ │ │ │ +
106 throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required");
│ │ │ │ +
107
│ │ │ │ +
108 SharedIsotropic sharedIsotropic = boost::dynamic_pointer_cast<
│ │ │ │ +
109 noiseModel::Isotropic>(sharedNoiseModel);
│ │ │ │ +
110
│ │ │ │ +
111 if (!sharedIsotropic)
│ │ │ │ +
112 throw std::runtime_error("SmartFactorBase: needs isotropic");
│ │ │ │ +
113
│ │ │ │ +
114 noiseModel_ = sharedIsotropic;
│ │ │ │ +
115 }
│ │ │ │ +
│ │ │ │ +
116
│ │ │ │ +
│ │ │ │ +
118 ~SmartFactorBase() override {
│ │ │ │ +
119 }
│ │ │ │ +
│ │ │ │ +
120
│ │ │ │ +
│ │ │ │ +
126 void add(const Z& measured, const Key& key) {
│ │ │ │ +
127 if(std::find(keys_.begin(), keys_.end(), key) != keys_.end()) {
│ │ │ │ +
128 throw std::invalid_argument(
│ │ │ │ +
129 "SmartFactorBase::add: adding duplicate measurement for key.");
│ │ │ │ +
130 }
│ │ │ │ +
131 this->measured_.push_back(measured);
│ │ │ │ +
132 this->keys_.push_back(key);
│ │ │ │ +
133 }
│ │ │ │ +
│ │ │ │ +
134
│ │ │ │ +
│ │ │ │ +
136 void add(const ZVector& measurements, const KeyVector& cameraKeys) {
│ │ │ │ +
137 assert(measurements.size() == cameraKeys.size());
│ │ │ │ +
138 for (size_t i = 0; i < measurements.size(); i++) {
│ │ │ │ +
139 this->add(measurements[i], cameraKeys[i]);
│ │ │ │ +
140 }
│ │ │ │ +
141 }
│ │ │ │ +
│ │ │ │ +
142
│ │ │ │ +
147 template<class SFM_TRACK>
│ │ │ │ +
│ │ │ │ +
148 void add(const SFM_TRACK& trackToAdd) {
│ │ │ │ +
149 for (size_t k = 0; k < trackToAdd.numberMeasurements(); k++) {
│ │ │ │ +
150 this->measured_.push_back(trackToAdd.measurements[k].second);
│ │ │ │ +
151 this->keys_.push_back(trackToAdd.measurements[k].first);
│ │ │ │ +
152 }
│ │ │ │ +
153 }
│ │ │ │ +
│ │ │ │ +
154
│ │ │ │ +
156 size_t dim() const override { return ZDim * this->measured_.size(); }
│ │ │ │ +
157
│ │ │ │ +
159 const ZVector& measured() const { return measured_; }
│ │ │ │ +
160
│ │ │ │ +
│ │ │ │ +
162 virtual Cameras cameras(const Values& values) const {
│ │ │ │ +
163 Cameras cameras;
│ │ │ │ +
164 for(const Key& k: this->keys_)
│ │ │ │ +
165 cameras.push_back(values.at<CAMERA>(k));
│ │ │ │ +
166 return cameras;
│ │ │ │ +
167 }
│ │ │ │ +
│ │ │ │ +
168
│ │ │ │ +
│ │ │ │ +
174 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
│ │ │ │ +
175 DefaultKeyFormatter) const override {
│ │ │ │ +
176 std::cout << s << "SmartFactorBase, z = \n";
│ │ │ │ +
177 for (size_t k = 0; k < measured_.size(); ++k) {
│ │ │ │ +
178 std::cout << "measurement " << k<<", px = \n" << measured_[k] << "\n";
│ │ │ │ +
179 noiseModel_->print("noise model = ");
│ │ │ │ +
180 }
│ │ │ │ +
181 if(body_P_sensor_)
│ │ │ │ +
182 body_P_sensor_->print("body_P_sensor_:\n");
│ │ │ │ +
183 Base::print("", keyFormatter);
│ │ │ │ +
184 }
│ │ │ │ +
│ │ │ │ +
185
│ │ │ │ +
│ │ │ │ +
187 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
│ │ │ │ +
188 if (const This* e = dynamic_cast<const This*>(&p)) {
│ │ │ │ +
189 // Check that all measurements are the same.
│ │ │ │ +
190 for (size_t i = 0; i < measured_.size(); i++) {
│ │ │ │ +
191 if (!traits<Z>::Equals(this->measured_.at(i), e->measured_.at(i), tol))
│ │ │ │ +
192 return false;
│ │ │ │ +
193 }
│ │ │ │ +
194 // If so, check base class.
│ │ │ │ +
195 return Base::equals(p, tol);
│ │ │ │ +
196 } else {
│ │ │ │ +
197 return false;
│ │ │ │ +
198 }
│ │ │ │ +
199 }
│ │ │ │ +
│ │ │ │ +
200
│ │ │ │ +
203 template <class POINT>
│ │ │ │ +
│ │ │ │ +
204 Vector unwhitenedError(
│ │ │ │ +
205 const Cameras& cameras, const POINT& point,
│ │ │ │ +
206 boost::optional<typename Cameras::FBlocks&> Fs = boost::none, //
│ │ │ │ +
207 boost::optional<Matrix&> E = boost::none) const {
│ │ │ │ +
208 // Reproject, with optional derivatives.
│ │ │ │ +
209 Vector error = cameras.reprojectionError(point, measured_, Fs, E);
│ │ │ │ +
210
│ │ │ │ +
211 // Apply chain rule if body_P_sensor_ is given.
│ │ │ │ +
212 if (body_P_sensor_ && Fs) {
│ │ │ │ +
213 const Pose3 sensor_P_body = body_P_sensor_->inverse();
│ │ │ │ +
214 constexpr int camera_dim = traits<CAMERA>::dimension;
│ │ │ │ +
215 constexpr int pose_dim = traits<Pose3>::dimension;
│ │ │ │ +
216
│ │ │ │ +
217 for (size_t i = 0; i < Fs->size(); i++) {
│ │ │ │ +
218 const Pose3 world_P_body = cameras[i].pose() * sensor_P_body;
│ │ │ │ +
219 Eigen::Matrix<double, camera_dim, camera_dim> J;
│ │ │ │ +
220 J.setZero();
│ │ │ │ +
221 Eigen::Matrix<double, pose_dim, pose_dim> H;
│ │ │ │ +
222 // Call compose to compute Jacobian for camera extrinsics
│ │ │ │ +
223 world_P_body.compose(*body_P_sensor_, H);
│ │ │ │ +
224 // Assign extrinsics part of the Jacobian
│ │ │ │ +
225 J.template block<pose_dim, pose_dim>(0, 0) = H;
│ │ │ │ +
226 Fs->at(i) = Fs->at(i) * J;
│ │ │ │ +
227 }
│ │ │ │ +
228 }
│ │ │ │ +
229
│ │ │ │ +
230 // Correct the Jacobians in case some measurements are missing.
│ │ │ │ +
231 correctForMissingMeasurements(cameras, error, Fs, E);
│ │ │ │ +
232
│ │ │ │ +
233 return error;
│ │ │ │ +
234 }
│ │ │ │ +
│ │ │ │ +
235
│ │ │ │ +
│ │ │ │ +
241 virtual void correctForMissingMeasurements(
│ │ │ │ +
242 const Cameras& cameras, Vector& ue,
│ │ │ │ +
243 boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
│ │ │ │ +
244 boost::optional<Matrix&> E = boost::none) const {}
│ │ │ │ +
│ │ │ │ +
245
│ │ │ │ +
250 template<class POINT>
│ │ │ │ +
│ │ │ │ +
251 Vector whitenedError(const Cameras& cameras, const POINT& point) const {
│ │ │ │ +
252 Vector error = cameras.reprojectionError(point, measured_);
│ │ │ │ +
253 if (noiseModel_)
│ │ │ │ +
254 noiseModel_->whitenInPlace(error);
│ │ │ │ +
255 return error;
│ │ │ │ +
256 }
│ │ │ │ +
│ │ │ │ +
257
│ │ │ │ +
266 template<class POINT>
│ │ │ │ +
│ │ │ │ +
267 double totalReprojectionError(const Cameras& cameras,
│ │ │ │ +
268 const POINT& point) const {
│ │ │ │ +
269 Vector error = whitenedError(cameras, point);
│ │ │ │ +
270 return 0.5 * error.dot(error);
│ │ │ │ +
271 }
│ │ │ │ +
│ │ │ │ +
272
│ │ │ │ +
│ │ │ │ +
274 static Matrix PointCov(const Matrix& E) {
│ │ │ │ +
275 return (E.transpose() * E).inverse();
│ │ │ │ +
276 }
│ │ │ │ +
│ │ │ │ +
277
│ │ │ │ +
284 template<class POINT>
│ │ │ │ +
│ │ │ │ +
285 void computeJacobians(FBlocks& Fs, Matrix& E, Vector& b,
│ │ │ │ +
286 const Cameras& cameras, const POINT& point) const {
│ │ │ │ +
287 // Project into Camera set and calculate derivatives
│ │ │ │ +
288 // As in expressionFactor, RHS vector b = - (h(x_bar) - z) = z-h(x_bar)
│ │ │ │ +
289 // Indeed, nonlinear error |h(x_bar+dx)-z| ~ |h(x_bar) + A*dx - z|
│ │ │ │ +
290 // = |A*dx - (z-h(x_bar))|
│ │ │ │ +
291 b = -unwhitenedError(cameras, point, Fs, E);
│ │ │ │ +
292 }
│ │ │ │ +
│ │ │ │ +
293
│ │ │ │ +
299 template<class POINT>
│ │ │ │ +
│ │ │ │ +
300 void computeJacobiansSVD(FBlocks& Fs, Matrix& Enull,
│ │ │ │ +
301 Vector& b, const Cameras& cameras, const POINT& point) const {
│ │ │ │ +
302
│ │ │ │ +
303 Matrix E;
│ │ │ │ +
304 computeJacobians(Fs, E, b, cameras, point);
│ │ │ │ +
305
│ │ │ │ +
306 static const int N = FixedDimension<POINT>::value; // 2 (Unit3) or 3 (Point3)
│ │ │ │ +
307
│ │ │ │ +
308 // Do SVD on A.
│ │ │ │ +
309 Eigen::JacobiSVD<Matrix> svd(E, Eigen::ComputeFullU);
│ │ │ │ +
310 size_t m = this->keys_.size();
│ │ │ │ +
311 Enull = svd.matrixU().block(0, N, ZDim * m, ZDim * m - N); // last ZDim*m-N columns
│ │ │ │ +
312 }
│ │ │ │ +
│ │ │ │ +
313
│ │ │ │ +
315 // TODO(dellaert): Not used/tested anywhere and not properly whitened.
│ │ │ │ +
│ │ │ │ +
316 boost::shared_ptr<RegularHessianFactor<Dim> > createHessianFactor(
│ │ │ │ +
317 const Cameras& cameras, const Point3& point, const double lambda = 0.0,
│ │ │ │ +
318 bool diagonalDamping = false) const {
│ │ │ │ +
319
│ │ │ │ +
320 Matrix E;
│ │ │ │ +
321 Vector b;
│ │ │ │ +
322 computeJacobians(Fs, E, b, cameras, point);
│ │ │ │ +
323
│ │ │ │ +
324 // build augmented hessian
│ │ │ │ +
325 SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fs, E, b);
│ │ │ │ +
326
│ │ │ │ +
327 return boost::make_shared<RegularHessianFactor<Dim> >(keys_,
│ │ │ │ +
328 augmentedHessian);
│ │ │ │ +
329 }
│ │ │ │ +
│ │ │ │ +
330
│ │ │ │ +
│ │ │ │ +
336 void updateAugmentedHessian(const Cameras& cameras, const Point3& point,
│ │ │ │ +
337 const double lambda, bool diagonalDamping,
│ │ │ │ +
338 SymmetricBlockMatrix& augmentedHessian,
│ │ │ │ +
339 const KeyVector allKeys) const {
│ │ │ │ +
340 Matrix E;
│ │ │ │ +
341 Vector b;
│ │ │ │ +
342 computeJacobians(Fs, E, b, cameras, point);
│ │ │ │ +
343 Cameras::UpdateSchurComplement(Fs, E, b, allKeys, keys_, augmentedHessian);
│ │ │ │ +
344 }
│ │ │ │ +
│ │ │ │ +
345
│ │ │ │ +
│ │ │ │ +
347 void whitenJacobians(FBlocks& F, Matrix& E, Vector& b) const {
│ │ │ │ +
348 noiseModel_->WhitenSystem(E, b);
│ │ │ │ +
349 // TODO make WhitenInPlace work with any dense matrix type
│ │ │ │ +
350 for (size_t i = 0; i < F.size(); i++)
│ │ │ │ +
351 F[i] = noiseModel_->Whiten(F[i]);
│ │ │ │ +
352 }
│ │ │ │ +
│ │ │ │ +
353
│ │ │ │ +
355 boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > //
│ │ │ │ +
│ │ │ │ +
356 createRegularImplicitSchurFactor(const Cameras& cameras, const Point3& point,
│ │ │ │ +
357 double lambda = 0.0, bool diagonalDamping = false) const {
│ │ │ │ +
358 Matrix E;
│ │ │ │ +
359 Vector b;
│ │ │ │ +
360 FBlocks F;
│ │ │ │ +
361 computeJacobians(F, E, b, cameras, point);
│ │ │ │ +
362 whitenJacobians(F, E, b);
│ │ │ │ +
363 Matrix P = Cameras::PointCov(E, lambda, diagonalDamping);
│ │ │ │ +
364 return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_, F, E,
│ │ │ │ +
365 P, b);
│ │ │ │ +
366 }
│ │ │ │ +
│ │ │ │ +
367
│ │ │ │ +
│ │ │ │ +
369 boost::shared_ptr<JacobianFactorQ<Dim, ZDim> > createJacobianQFactor(
│ │ │ │ +
370 const Cameras& cameras, const Point3& point, double lambda = 0.0,
│ │ │ │ +
371 bool diagonalDamping = false) const {
│ │ │ │ +
372 Matrix E;
│ │ │ │ +
373 Vector b;
│ │ │ │ +
374 FBlocks F;
│ │ │ │ +
375 computeJacobians(F, E, b, cameras, point);
│ │ │ │ +
376 const size_t M = b.size();
│ │ │ │ +
377 Matrix P = Cameras::PointCov(E, lambda, diagonalDamping);
│ │ │ │ +
378 SharedIsotropic n = noiseModel::Isotropic::Sigma(M, noiseModel_->sigma());
│ │ │ │ +
379 return boost::make_shared<JacobianFactorQ<Dim, ZDim> >(keys_, F, E, P, b, n);
│ │ │ │ +
380 }
│ │ │ │ +
│ │ │ │ +
381
│ │ │ │ +
│ │ │ │ +
386 boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
│ │ │ │ +
387 const Cameras& cameras, const Point3& point, double lambda = 0.0) const {
│ │ │ │ +
388 size_t m = this->keys_.size();
│ │ │ │ +
389 FBlocks F;
│ │ │ │ +
390 Vector b;
│ │ │ │ +
391 const size_t M = ZDim * m;
│ │ │ │ +
392 Matrix E0(M, M - 3);
│ │ │ │ +
393 computeJacobiansSVD(F, E0, b, cameras, point);
│ │ │ │ +
394 SharedIsotropic n = noiseModel::Isotropic::Sigma(M - 3,
│ │ │ │ +
395 noiseModel_->sigma());
│ │ │ │ +
396 return boost::make_shared<JacobianFactorSVD<Dim, ZDim> >(keys_, F, E0, b, n);
│ │ │ │ +
397 }
│ │ │ │ +
│ │ │ │ +
398
│ │ │ │ +
│ │ │ │ +
400 static void FillDiagonalF(const FBlocks& Fs, Matrix& F) {
│ │ │ │ +
401 size_t m = Fs.size();
│ │ │ │ +
402 F.resize(ZDim * m, Dim * m);
│ │ │ │ +
403 F.setZero();
│ │ │ │ +
404 for (size_t i = 0; i < m; ++i)
│ │ │ │ +
405 F.block<ZDim, Dim>(ZDim * i, Dim * i) = Fs.at(i);
│ │ │ │ +
406 }
│ │ │ │ +
│ │ │ │ +
407
│ │ │ │ +
408 // Return sensor pose.
│ │ │ │ +
409 Pose3 body_P_sensor() const{
│ │ │ │ +
410 if(body_P_sensor_)
│ │ │ │ +
411 return *body_P_sensor_;
│ │ │ │ +
412 else
│ │ │ │ +
413 return Pose3(); // if unspecified, the transformation is the identity
│ │ │ │ +
414 }
│ │ │ │ +
415
│ │ │ │ +
416private:
│ │ │ │ +
417
│ │ │ │ +
419 friend class boost::serialization::access;
│ │ │ │ +
420 template<class ARCHIVE>
│ │ │ │ +
421 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
422 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
423 ar & BOOST_SERIALIZATION_NVP(noiseModel_);
│ │ │ │ +
424 ar & BOOST_SERIALIZATION_NVP(measured_);
│ │ │ │ +
425 ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
│ │ │ │ +
426 }
│ │ │ │ +
427};
│ │ │ │ +
│ │ │ │ +
428// end class SmartFactorBase
│ │ │ │ +
429
│ │ │ │ +
430// Definitions need to avoid link errors (above are only declarations)
│ │ │ │ +
431template<class CAMERA> const int SmartFactorBase<CAMERA>::Dim;
│ │ │ │ +
432template<class CAMERA> const int SmartFactorBase<CAMERA>::ZDim;
│ │ │ │ +
433
│ │ │ │ +
434} // \ namespace gtsam
│ │ │ │ +
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ +
CameraSet.h
Base class to create smart factors on poses or cameras.
│ │ │ │ +
RegularHessianFactor.h
HessianFactor class with constant sized blocks.
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
RegularImplicitSchurFactor.h
A subclass of GaussianFactor specialized to structureless SFM.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::svd
void svd(const Matrix &A, Matrix &U, Vector &S, Matrix &V)
SVD computes economy SVD A=U*S*V'.
Definition Matrix.cpp:560
│ │ │ │ +
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ +
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::EliminateableFactorGraph::eliminatePartialSequential
std::pair< boost::shared_ptr< BayesNetType >, boost::shared_ptr< FactorGraphType > > eliminatePartialSequential(const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Do sequential elimination of some variables, in ordering provided, to produce a Bayes net and a remai...
Definition EliminateableFactorGraph-inst.h:154
│ │ │ │ -
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
│ │ │ │ -
gtsam::Symbol
Character and index key used to refer to variables.
Definition Symbol.h:35
│ │ │ │ -
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ -
gtsam::GaussianFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactorGraph.h:81
│ │ │ │ -
gtsam::GaussianFactorGraph::add
void add(const GaussianFactor &factor)
Add a factor by value - makes a copy.
Definition GaussianFactorGraph.h:127
│ │ │ │ -
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition JacobianFactor.h:91
│ │ │ │ -
gtsam::RegularJacobianFactor
JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator.
Definition RegularJacobianFactor.h:32
│ │ │ │ -
gtsam::JacobianFactorQR
JacobianFactor for Schur complement that uses Q noise model.
Definition JacobianFactorQR.h:21
│ │ │ │ -
gtsam::JacobianFactorQR::JacobianFactorQR
JacobianFactorQR(const KeyVector &keys, const std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > &FBlocks, const Matrix &E, const Matrix3 &P, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Constructor.
Definition JacobianFactorQR.h:31
│ │ │ │ - │ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::FixedDimension
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition Manifold.h:164
│ │ │ │ +
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
│ │ │ │ +
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition CameraSet.h:36
│ │ │ │ +
gtsam::CameraSet::reprojectionError
Vector reprojectionError(const POINT &point, const ZVector &measured, boost::optional< FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const
Calculate vector [project2(point)-z] of re-projection errors.
Definition CameraSet.h:136
│ │ │ │ +
gtsam::CameraSet::PointCov
static Matrix PointCov(const Matrix &E, const double lambda=0.0, bool diagonalDamping=false)
Computes Point Covariance P, with lambda parameter, dynamic version.
Definition CameraSet.h:331
│ │ │ │ +
gtsam::CameraSet::UpdateSchurComplement
static void UpdateSchurComplement(const FBlocks &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b, const KeyVector &allKeys, const KeyVector &keys, SymmetricBlockMatrix &augmentedHessian)
Applies Schur complement (exploiting block structure) to get a smart factor on cameras,...
Definition CameraSet.h:369
│ │ │ │ +
gtsam::CameraSet::SchurComplement
static SymmetricBlockMatrix SchurComplement(const std::vector< Eigen::Matrix< double, ZDim, ND >, Eigen::aligned_allocator< Eigen::Matrix< double, ZDim, ND > > > &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b)
Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = F' * F - F' * E * P * ...
Definition CameraSet.h:150
│ │ │ │ +
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ +
gtsam::Pose3::inverse
Pose3 inverse() const
inverse transformation with derivatives
Definition Pose3.cpp:49
│ │ │ │ +
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
│ │ │ │ +
gtsam::noiseModel::Isotropic
An isotropic noise model corresponds to a scaled diagonal covariance To construct,...
Definition NoiseModel.h:516
│ │ │ │ +
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
An isotropic noise model created by specifying a standard devation sigma.
Definition NoiseModel.cpp:597
│ │ │ │ +
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ +
gtsam::NonlinearFactor::equals
virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const
Check if two factors are equal.
Definition NonlinearFactor.cpp:47
│ │ │ │ +
gtsam::NonlinearFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition NonlinearFactor.cpp:37
│ │ │ │ +
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
gtsam::Values::at
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
│ │ │ │ +
gtsam::SmartFactorBase
Base class for smart factors.
Definition SmartFactorBase.h:50
│ │ │ │ +
gtsam::SmartFactorBase::computeJacobians
void computeJacobians(FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras, const POINT &point) const
Compute F, E, and b (called below in both vanilla and SVD versions), where F is a vector of derivativ...
Definition SmartFactorBase.h:285
│ │ │ │ +
gtsam::SmartFactorBase::add
void add(const Z &measured, const Key &key)
Add a new measurement and pose/camera key.
Definition SmartFactorBase.h:126
│ │ │ │ +
gtsam::SmartFactorBase::~SmartFactorBase
~SmartFactorBase() override
Virtual destructor, subclasses from NonlinearFactor.
Definition SmartFactorBase.h:118
│ │ │ │ +
gtsam::SmartFactorBase::updateAugmentedHessian
void updateAugmentedHessian(const Cameras &cameras, const Point3 &point, const double lambda, bool diagonalDamping, SymmetricBlockMatrix &augmentedHessian, const KeyVector allKeys) const
Add the contribution of the smart factor to a pre-allocated Hessian, using sparse linear algebra.
Definition SmartFactorBase.h:336
│ │ │ │ +
gtsam::SmartFactorBase::createRegularImplicitSchurFactor
boost::shared_ptr< RegularImplicitSchurFactor< CAMERA > > createRegularImplicitSchurFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0, bool diagonalDamping=false) const
Return Jacobians as RegularImplicitSchurFactor with raw access.
Definition SmartFactorBase.h:356
│ │ │ │ +
gtsam::SmartFactorBase::noiseModel_
SharedIsotropic noiseModel_
As of Feb 22, 2015, the noise model is the same for all measurements and is isotropic.
Definition SmartFactorBase.h:72
│ │ │ │ +
gtsam::SmartFactorBase::Dim
static const int Dim
Camera dimension.
Definition SmartFactorBase.h:60
│ │ │ │ +
gtsam::SmartFactorBase::whitenedError
Vector whitenedError(const Cameras &cameras, const POINT &point) const
Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z], with the noise model ap...
Definition SmartFactorBase.h:251
│ │ │ │ +
gtsam::SmartFactorBase::cameras
virtual Cameras cameras(const Values &values) const
Collect all cameras: important that in key order.
Definition SmartFactorBase.h:162
│ │ │ │ +
gtsam::SmartFactorBase::FillDiagonalF
static void FillDiagonalF(const FBlocks &Fs, Matrix &F)
Create BIG block-diagonal matrix F from Fblocks.
Definition SmartFactorBase.h:400
│ │ │ │ +
gtsam::SmartFactorBase::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, const POINT &point) const
Calculate the error of the factor.
Definition SmartFactorBase.h:267
│ │ │ │ +
gtsam::SmartFactorBase::add
void add(const ZVector &measurements, const KeyVector &cameraKeys)
Add a bunch of measurements, together with the camera keys.
Definition SmartFactorBase.h:136
│ │ │ │ +
gtsam::SmartFactorBase::createHessianFactor
boost::shared_ptr< RegularHessianFactor< Dim > > createHessianFactor(const Cameras &cameras, const Point3 &point, const double lambda=0.0, bool diagonalDamping=false) const
Linearize to a Hessianfactor.
Definition SmartFactorBase.h:316
│ │ │ │ +
gtsam::SmartFactorBase::computeJacobiansSVD
void computeJacobiansSVD(FBlocks &Fs, Matrix &Enull, Vector &b, const Cameras &cameras, const POINT &point) const
SVD version that produces smaller Jacobian matrices by doing an SVD decomposition on E,...
Definition SmartFactorBase.h:300
│ │ │ │ +
gtsam::SmartFactorBase::measured_
ZVector measured_
Measurements for each of the m views.
Definition SmartFactorBase.h:79
│ │ │ │ +
gtsam::SmartFactorBase::shared_ptr
GTSAM_MAKE_ALIGNED_OPERATOR_NEW typedef boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor.
Definition SmartFactorBase.h:91
│ │ │ │ +
gtsam::SmartFactorBase::unwhitenedError
Vector unwhitenedError(const Cameras &cameras, const POINT &point, boost::optional< typename Cameras::FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const
Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and derivatives.
Definition SmartFactorBase.h:204
│ │ │ │ +
gtsam::SmartFactorBase::SmartFactorBase
SmartFactorBase()
Default Constructor, for serialization.
Definition SmartFactorBase.h:97
│ │ │ │ +
gtsam::SmartFactorBase::whitenJacobians
void whitenJacobians(FBlocks &F, Matrix &E, Vector &b) const
Whiten the Jacobians computed by computeJacobians using noiseModel_.
Definition SmartFactorBase.h:347
│ │ │ │ +
gtsam::SmartFactorBase::SmartFactorBase
SmartFactorBase(const SharedNoiseModel &sharedNoiseModel, boost::optional< Pose3 > body_P_sensor=boost::none, size_t expectedNumberCameras=10)
Construct with given noise model and optional arguments.
Definition SmartFactorBase.h:100
│ │ │ │ +
gtsam::SmartFactorBase::add
void add(const SFM_TRACK &trackToAdd)
Add an entire SfM_track (collection of cameras observing a single point).
Definition SmartFactorBase.h:148
│ │ │ │ +
gtsam::SmartFactorBase::measured
const ZVector & measured() const
Return the 2D measurements (ZDim, in general).
Definition SmartFactorBase.h:159
│ │ │ │ +
gtsam::SmartFactorBase::createJacobianSVDFactor
boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0) const
Return Jacobians as JacobianFactorSVD.
Definition SmartFactorBase.h:386
│ │ │ │ +
gtsam::SmartFactorBase::dim
size_t dim() const override
Return the dimension (number of rows!) of the factor.
Definition SmartFactorBase.h:156
│ │ │ │ +
gtsam::SmartFactorBase::createJacobianQFactor
boost::shared_ptr< JacobianFactorQ< Dim, ZDim > > createJacobianQFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0, bool diagonalDamping=false) const
Return Jacobians as JacobianFactorQ.
Definition SmartFactorBase.h:369
│ │ │ │ +
gtsam::SmartFactorBase::access
friend class boost::serialization::access
Serialization function.
Definition SmartFactorBase.h:419
│ │ │ │ +
gtsam::SmartFactorBase::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition SmartFactorBase.h:174
│ │ │ │ +
gtsam::SmartFactorBase::body_P_sensor_
boost::optional< Pose3 > body_P_sensor_
Pose of the camera in the body frame.
Definition SmartFactorBase.h:82
│ │ │ │ +
gtsam::SmartFactorBase::correctForMissingMeasurements
virtual void correctForMissingMeasurements(const Cameras &cameras, Vector &ue, boost::optional< typename Cameras::FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const
This corrects the Jacobians for the case in which some 2D measurement is missing (nan).
Definition SmartFactorBase.h:241
│ │ │ │ +
gtsam::SmartFactorBase::ZDim
static const int ZDim
Measurement dimension.
Definition SmartFactorBase.h:61
│ │ │ │ +
gtsam::SmartFactorBase::PointCov
static Matrix PointCov(const Matrix &E)
Computes Point Covariance P from the "point Jacobian" E.
Definition SmartFactorBase.h:274
│ │ │ │ +
gtsam::SmartFactorBase::Cameras
CameraSet< CAMERA > Cameras
The CameraSet data structure is used to refer to a set of cameras.
Definition SmartFactorBase.h:94
│ │ │ │ +
gtsam::SmartFactorBase::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition SmartFactorBase.h:187
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,129 +1,629 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -JacobianFactorQR.h │ │ │ │ │ -1/* │ │ │ │ │ -2 * @file JacobianFactorQR.h │ │ │ │ │ -3 * @brief Jacobianfactor that combines and eliminates points │ │ │ │ │ -4 * @date Oct 27, 2013 │ │ │ │ │ -5 * @uthor Frank Dellaert │ │ │ │ │ -6 */ │ │ │ │ │ +SmartFactorBase.h │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +1/* --------------------------------------------------------------------------- │ │ │ │ │ +- │ │ │ │ │ +2 │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +5 * All Rights Reserved │ │ │ │ │ +6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ -8#pragma once │ │ │ │ │ -9#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -10#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ -11#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_S_y_m_b_o_l_._h> │ │ │ │ │ -12 │ │ │ │ │ -13namespace _g_t_s_a_m { │ │ │ │ │ -14 │ │ │ │ │ -15class GaussianBayesNet; │ │ │ │ │ -16 │ │ │ │ │ -20template │ │ │ │ │ -_2_1class _J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q_R: public _R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r { │ │ │ │ │ -22 │ │ │ │ │ -23 typedef _R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_<_D_> Base; │ │ │ │ │ -24 typedef Eigen::Matrix MatrixZD; │ │ │ │ │ -25 │ │ │ │ │ -26public: │ │ │ │ │ +8 * See LICENSE for the license information │ │ │ │ │ +9 │ │ │ │ │ +10 * ------------------------------------------------------------------------- │ │ │ │ │ +- */ │ │ │ │ │ +11 │ │ │ │ │ +22#pragma once │ │ │ │ │ +23 │ │ │ │ │ +24#include │ │ │ │ │ +25#include │ │ │ │ │ +26#include <_g_t_s_a_m_/_s_l_a_m_/_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_._h> │ │ │ │ │ 27 │ │ │ │ │ -_3_1 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q_R(const _K_e_y_V_e_c_t_o_r& _k_e_y_s, │ │ │ │ │ -32 const std::vector >& FBlocks, │ │ │ │ │ -const Matrix& E, const Matrix3& P, │ │ │ │ │ -33 const Vector& b, // │ │ │ │ │ -34 const SharedDiagonal& model = SharedDiagonal()) : │ │ │ │ │ -35 Base() { │ │ │ │ │ -36 // Create a number of Jacobian factors in a factor graph │ │ │ │ │ -37 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h gfg; │ │ │ │ │ -38 _S_y_m_b_o_l pointKey('p', 0); │ │ │ │ │ -39 for (size_t k = 0; k < FBlocks.size(); ++k) { │ │ │ │ │ -40 _K_e_y key = _k_e_y_s[k]; │ │ │ │ │ -41 gfg._a_d_d(pointKey, E.block(ZDim * k, 0), key, FBlocks[k], │ │ │ │ │ -42 b.segment < ZDim > (ZDim * k), model); │ │ │ │ │ -43 } │ │ │ │ │ -44 //gfg.print("gfg"); │ │ │ │ │ -45 │ │ │ │ │ -46 // eliminate the point │ │ │ │ │ -47 boost::shared_ptr bn; │ │ │ │ │ -48 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r fg; │ │ │ │ │ -49 _K_e_y_V_e_c_t_o_r variables; │ │ │ │ │ -50 variables.push_back(pointKey); │ │ │ │ │ -51 boost::tie(bn, fg) = gfg._e_l_i_m_i_n_a_t_e_P_a_r_t_i_a_l_S_e_q_u_e_n_t_i_a_l(variables, _E_l_i_m_i_n_a_t_e_Q_R); │ │ │ │ │ -52 //fg->print("fg"); │ │ │ │ │ -53 │ │ │ │ │ -54 JacobianFactor::operator=(_J_a_c_o_b_i_a_n_F_a_c_t_o_r(*fg)); │ │ │ │ │ -55 } │ │ │ │ │ -56}; │ │ │ │ │ -57// end class JacobianFactorQR │ │ │ │ │ -58 │ │ │ │ │ -59}// end namespace gtsam │ │ │ │ │ -_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ -JacobianFactor class with fixed sized blcoks. │ │ │ │ │ -_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_E_l_i_m_i_n_a_t_e_Q_R │ │ │ │ │ -friend GTSAM_EXPORT std::pair< boost::shared_ptr< GaussianConditional >, │ │ │ │ │ -shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering │ │ │ │ │ -&keys) │ │ │ │ │ -Densely partially eliminate with QR factorization, this is usually provided as │ │ │ │ │ -an argument to one of ... │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.cpp:789 │ │ │ │ │ +28#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +29#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ +30#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_m_e_r_a_S_e_t_._h> │ │ │ │ │ +31 │ │ │ │ │ +32#include │ │ │ │ │ +33#include │ │ │ │ │ +34#include │ │ │ │ │ +35#include │ │ │ │ │ +36 │ │ │ │ │ +37namespace _g_t_s_a_m { │ │ │ │ │ +38 │ │ │ │ │ +49template │ │ │ │ │ +_5_0class _S_m_a_r_t_F_a_c_t_o_r_B_a_s_e: public _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r { │ │ │ │ │ +51 │ │ │ │ │ +52private: │ │ │ │ │ +53 typedef _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r _B_a_s_e; │ │ │ │ │ +54 typedef _S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_<_C_A_M_E_R_A_> This; │ │ │ │ │ +55 typedef typename CAMERA::Measurement Z; │ │ │ │ │ +56 typedef typename CAMERA::MeasurementVector ZVector; │ │ │ │ │ +57 │ │ │ │ │ +58public: │ │ │ │ │ +59 │ │ │ │ │ +_6_0 static const int _D_i_m = _t_r_a_i_t_s_<_C_A_M_E_R_A_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ +_6_1 static const int _Z_D_i_m = _t_r_a_i_t_s_<_Z_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ +62 typedef Eigen::Matrix MatrixZD; // F blocks (derivatives │ │ │ │ │ +wrpt camera) │ │ │ │ │ +63 typedef std::vector > FBlocks; │ │ │ │ │ +// vector of F blocks │ │ │ │ │ +64 │ │ │ │ │ +65protected: │ │ │ │ │ +_7_2 SharedIsotropic _n_o_i_s_e_M_o_d_e_l__; │ │ │ │ │ +73 │ │ │ │ │ +_7_9 ZVector _m_e_a_s_u_r_e_d__; │ │ │ │ │ +80 │ │ │ │ │ +81 boost::optional │ │ │ │ │ +_8_2 _b_o_d_y___P___s_e_n_s_o_r__; │ │ │ │ │ +83 │ │ │ │ │ +84 // Cache for Fblocks, to avoid a malloc ever time we re-linearize │ │ │ │ │ +85 mutable FBlocks Fs; │ │ │ │ │ +86 │ │ │ │ │ +87 public: │ │ │ │ │ +88 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +89 │ │ │ │ │ +_9_1 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +92 │ │ │ │ │ +_9_4 typedef _C_a_m_e_r_a_S_e_t_<_C_A_M_E_R_A_> _C_a_m_e_r_a_s; │ │ │ │ │ +95 │ │ │ │ │ +_9_7 _S_m_a_r_t_F_a_c_t_o_r_B_a_s_e() {} │ │ │ │ │ +98 │ │ │ │ │ +_1_0_0 _S_m_a_r_t_F_a_c_t_o_r_B_a_s_e(const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& sharedNoiseModel, │ │ │ │ │ +101 boost::optional body_P_sensor = boost::none, │ │ │ │ │ +102 size_t expectedNumberCameras = 10) │ │ │ │ │ +103 : _b_o_d_y___P___s_e_n_s_o_r__(body_P_sensor), Fs(expectedNumberCameras) { │ │ │ │ │ +104 │ │ │ │ │ +105 if (!sharedNoiseModel) │ │ │ │ │ +106 throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required"); │ │ │ │ │ +107 │ │ │ │ │ +108 SharedIsotropic sharedIsotropic = boost::dynamic_pointer_cast< │ │ │ │ │ +109 _n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c>(sharedNoiseModel); │ │ │ │ │ +110 │ │ │ │ │ +111 if (!sharedIsotropic) │ │ │ │ │ +112 throw std::runtime_error("SmartFactorBase: needs isotropic"); │ │ │ │ │ +113 │ │ │ │ │ +114 _n_o_i_s_e_M_o_d_e_l__ = sharedIsotropic; │ │ │ │ │ +115 } │ │ │ │ │ +116 │ │ │ │ │ +_1_1_8 _~_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e() override { │ │ │ │ │ +119 } │ │ │ │ │ +120 │ │ │ │ │ +_1_2_6 void _a_d_d(const Z& _m_e_a_s_u_r_e_d, const _K_e_y& key) { │ │ │ │ │ +127 if(std::find(_k_e_y_s__.begin(), _k_e_y_s__.end(), key) != _k_e_y_s__.end()) { │ │ │ │ │ +128 throw std::invalid_argument( │ │ │ │ │ +129 "SmartFactorBase::add: adding duplicate measurement for key."); │ │ │ │ │ +130 } │ │ │ │ │ +131 this->measured_.push_back(_m_e_a_s_u_r_e_d); │ │ │ │ │ +132 this->_k_e_y_s__.push_back(key); │ │ │ │ │ +133 } │ │ │ │ │ +134 │ │ │ │ │ +_1_3_6 void _a_d_d(const ZVector& measurements, const _K_e_y_V_e_c_t_o_r& cameraKeys) { │ │ │ │ │ +137 assert(measurements.size() == cameraKeys.size()); │ │ │ │ │ +138 for (size_t i = 0; i < measurements.size(); i++) { │ │ │ │ │ +139 this->_a_d_d(measurements[i], cameraKeys[i]); │ │ │ │ │ +140 } │ │ │ │ │ +141 } │ │ │ │ │ +142 │ │ │ │ │ +147 template │ │ │ │ │ +_1_4_8 void _a_d_d(const SFM_TRACK& trackToAdd) { │ │ │ │ │ +149 for (size_t k = 0; k < trackToAdd.numberMeasurements(); k++) { │ │ │ │ │ +150 this->measured_.push_back(trackToAdd.measurements[k].second); │ │ │ │ │ +151 this->_k_e_y_s__.push_back(trackToAdd.measurements[k].first); │ │ │ │ │ +152 } │ │ │ │ │ +153 } │ │ │ │ │ +154 │ │ │ │ │ +_1_5_6 size_t _d_i_m() const override { return _Z_D_i_m * this->measured_.size(); } │ │ │ │ │ +157 │ │ │ │ │ +_1_5_9 const ZVector& _m_e_a_s_u_r_e_d() const { return _m_e_a_s_u_r_e_d__; } │ │ │ │ │ +160 │ │ │ │ │ +_1_6_2 virtual _C_a_m_e_r_a_s _c_a_m_e_r_a_s(const _V_a_l_u_e_s& values) const { │ │ │ │ │ +163 _C_a_m_e_r_a_s _c_a_m_e_r_a_s; │ │ │ │ │ +164 for(const _K_e_y& k: this->_k_e_y_s__) │ │ │ │ │ +165 cameras.push_back(values._a_t(k)); │ │ │ │ │ +166 return _c_a_m_e_r_a_s; │ │ │ │ │ +167 } │ │ │ │ │ +168 │ │ │ │ │ +_1_7_4 void _p_r_i_n_t(const std::string& s = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ +175 DefaultKeyFormatter) const override { │ │ │ │ │ +176 std::cout << s << "SmartFactorBase, z = \n"; │ │ │ │ │ +177 for (size_t k = 0; k < _m_e_a_s_u_r_e_d__.size(); ++k) { │ │ │ │ │ +178 std::cout << "measurement " << k<<", px = \n" << _m_e_a_s_u_r_e_d__[k] << "\n"; │ │ │ │ │ +179 _n_o_i_s_e_M_o_d_e_l__->print("noise model = "); │ │ │ │ │ +180 } │ │ │ │ │ +181 if(_b_o_d_y___P___s_e_n_s_o_r__) │ │ │ │ │ +182 _b_o_d_y___P___s_e_n_s_o_r__->print("body_P_sensor_:\n"); │ │ │ │ │ +183 _B_a_s_e_:_:_p_r_i_n_t("", keyFormatter); │ │ │ │ │ +184 } │ │ │ │ │ +185 │ │ │ │ │ +_1_8_7 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& p, double tol = 1e-9) const override { │ │ │ │ │ +188 if (const This* e = dynamic_cast(&p)) { │ │ │ │ │ +189 // Check that all measurements are the same. │ │ │ │ │ +190 for (size_t i = 0; i < _m_e_a_s_u_r_e_d__.size(); i++) { │ │ │ │ │ +191 if (!_t_r_a_i_t_s_<_Z_>_:_:_E_q_u_a_l_s(this->measured_.at(i), e->measured_.at(i), tol)) │ │ │ │ │ +192 return false; │ │ │ │ │ +193 } │ │ │ │ │ +194 // If so, check base class. │ │ │ │ │ +195 return _B_a_s_e_:_:_e_q_u_a_l_s(p, tol); │ │ │ │ │ +196 } else { │ │ │ │ │ +197 return false; │ │ │ │ │ +198 } │ │ │ │ │ +199 } │ │ │ │ │ +200 │ │ │ │ │ +203 template │ │ │ │ │ +_2_0_4 Vector _u_n_w_h_i_t_e_n_e_d_E_r_r_o_r( │ │ │ │ │ +205 const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, const POINT& point, │ │ │ │ │ +206 boost::optional Fs = boost::none, // │ │ │ │ │ +207 boost::optional E = boost::none) const { │ │ │ │ │ +208 // Reproject, with optional derivatives. │ │ │ │ │ +209 Vector error = _c_a_m_e_r_a_s._r_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r(point, _m_e_a_s_u_r_e_d__, Fs, E); │ │ │ │ │ +210 │ │ │ │ │ +211 // Apply chain rule if body_P_sensor_ is given. │ │ │ │ │ +212 if (_b_o_d_y___P___s_e_n_s_o_r__ && Fs) { │ │ │ │ │ +213 const _P_o_s_e_3 sensor_P_body = _b_o_d_y___P___s_e_n_s_o_r__->_i_n_v_e_r_s_e(); │ │ │ │ │ +214 constexpr int camera_dim = _t_r_a_i_t_s_<_C_A_M_E_R_A_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ +215 constexpr int pose_dim = _t_r_a_i_t_s_<_P_o_s_e_3_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ +216 │ │ │ │ │ +217 for (size_t i = 0; i < Fs->size(); i++) { │ │ │ │ │ +218 const _P_o_s_e_3 world_P_body = _c_a_m_e_r_a_s[i].pose() * sensor_P_body; │ │ │ │ │ +219 Eigen::Matrix J; │ │ │ │ │ +220 J.setZero(); │ │ │ │ │ +221 Eigen::Matrix H; │ │ │ │ │ +222 // Call compose to compute Jacobian for camera extrinsics │ │ │ │ │ +223 world_P_body.compose(*_b_o_d_y___P___s_e_n_s_o_r__, H); │ │ │ │ │ +224 // Assign extrinsics part of the Jacobian │ │ │ │ │ +225 J.template block(0, 0) = H; │ │ │ │ │ +226 Fs->at(i) = Fs->at(i) * J; │ │ │ │ │ +227 } │ │ │ │ │ +228 } │ │ │ │ │ +229 │ │ │ │ │ +230 // Correct the Jacobians in case some measurements are missing. │ │ │ │ │ +231 _c_o_r_r_e_c_t_F_o_r_M_i_s_s_i_n_g_M_e_a_s_u_r_e_m_e_n_t_s(_c_a_m_e_r_a_s, error, Fs, E); │ │ │ │ │ +232 │ │ │ │ │ +233 return error; │ │ │ │ │ +234 } │ │ │ │ │ +235 │ │ │ │ │ +_2_4_1 virtual void _c_o_r_r_e_c_t_F_o_r_M_i_s_s_i_n_g_M_e_a_s_u_r_e_m_e_n_t_s( │ │ │ │ │ +242 const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, Vector& ue, │ │ │ │ │ +243 boost::optional Fs = boost::none, │ │ │ │ │ +244 boost::optional E = boost::none) const {} │ │ │ │ │ +245 │ │ │ │ │ +250 template │ │ │ │ │ +_2_5_1 Vector _w_h_i_t_e_n_e_d_E_r_r_o_r(const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, const POINT& point) const { │ │ │ │ │ +252 Vector error = _c_a_m_e_r_a_s._r_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r(point, _m_e_a_s_u_r_e_d__); │ │ │ │ │ +253 if (_n_o_i_s_e_M_o_d_e_l__) │ │ │ │ │ +254 _n_o_i_s_e_M_o_d_e_l__->whitenInPlace(error); │ │ │ │ │ +255 return error; │ │ │ │ │ +256 } │ │ │ │ │ +257 │ │ │ │ │ +266 template │ │ │ │ │ +_2_6_7 double _t_o_t_a_l_R_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r(const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, │ │ │ │ │ +268 const POINT& point) const { │ │ │ │ │ +269 Vector error = _w_h_i_t_e_n_e_d_E_r_r_o_r(_c_a_m_e_r_a_s, point); │ │ │ │ │ +270 return 0.5 * error.dot(error); │ │ │ │ │ +271 } │ │ │ │ │ +272 │ │ │ │ │ +_2_7_4 static Matrix _P_o_i_n_t_C_o_v(const Matrix& E) { │ │ │ │ │ +275 return (E.transpose() * E).inverse(); │ │ │ │ │ +276 } │ │ │ │ │ +277 │ │ │ │ │ +284 template │ │ │ │ │ +_2_8_5 void _c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s(FBlocks& Fs, Matrix& E, Vector& b, │ │ │ │ │ +286 const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, const POINT& point) const { │ │ │ │ │ +287 // Project into Camera set and calculate derivatives │ │ │ │ │ +288 // As in expressionFactor, RHS vector b = - (h(x_bar) - z) = z-h(x_bar) │ │ │ │ │ +289 // Indeed, nonlinear error |h(x_bar+dx)-z| ~ |h(x_bar) + A*dx - z| │ │ │ │ │ +290 // = |A*dx - (z-h(x_bar))| │ │ │ │ │ +291 b = -_u_n_w_h_i_t_e_n_e_d_E_r_r_o_r(_c_a_m_e_r_a_s, point, Fs, E); │ │ │ │ │ +292 } │ │ │ │ │ +293 │ │ │ │ │ +299 template │ │ │ │ │ +_3_0_0 void _c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s_S_V_D(FBlocks& Fs, Matrix& Enull, │ │ │ │ │ +301 Vector& b, const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, const POINT& point) const { │ │ │ │ │ +302 │ │ │ │ │ +303 Matrix E; │ │ │ │ │ +304 _c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s(Fs, E, b, _c_a_m_e_r_a_s, point); │ │ │ │ │ +305 │ │ │ │ │ +306 static const int N = _F_i_x_e_d_D_i_m_e_n_s_i_o_n_<_P_O_I_N_T_>_:_:_v_a_l_u_e; // 2 (Unit3) or 3 │ │ │ │ │ +(Point3) │ │ │ │ │ +307 │ │ │ │ │ +308 // Do SVD on A. │ │ │ │ │ +309 Eigen::JacobiSVD _s_v_d(E, Eigen::ComputeFullU); │ │ │ │ │ +310 size_t m = this->_k_e_y_s__.size(); │ │ │ │ │ +311 Enull = _s_v_d.matrixU().block(0, N, _Z_D_i_m * m, _Z_D_i_m * m - N); // last ZDim*m- │ │ │ │ │ +N columns │ │ │ │ │ +312 } │ │ │ │ │ +313 │ │ │ │ │ +315 // TODO(dellaert): Not used/tested anywhere and not properly whitened. │ │ │ │ │ +_3_1_6 boost::shared_ptr > _c_r_e_a_t_e_H_e_s_s_i_a_n_F_a_c_t_o_r( │ │ │ │ │ +317 const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, const _P_o_i_n_t_3& point, const double lambda = 0.0, │ │ │ │ │ +318 bool diagonalDamping = false) const { │ │ │ │ │ +319 │ │ │ │ │ +320 Matrix E; │ │ │ │ │ +321 Vector b; │ │ │ │ │ +322 _c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s(Fs, E, b, _c_a_m_e_r_a_s, point); │ │ │ │ │ +323 │ │ │ │ │ +324 // build augmented hessian │ │ │ │ │ +325 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x augmentedHessian = _C_a_m_e_r_a_s_:_:_S_c_h_u_r_C_o_m_p_l_e_m_e_n_t(Fs, E, b); │ │ │ │ │ +326 │ │ │ │ │ +327 return boost::make_shared >(_k_e_y_s__, │ │ │ │ │ +328 augmentedHessian); │ │ │ │ │ +329 } │ │ │ │ │ +330 │ │ │ │ │ +_3_3_6 void _u_p_d_a_t_e_A_u_g_m_e_n_t_e_d_H_e_s_s_i_a_n(const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, const _P_o_i_n_t_3& point, │ │ │ │ │ +337 const double lambda, bool diagonalDamping, │ │ │ │ │ +338 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x& augmentedHessian, │ │ │ │ │ +339 const _K_e_y_V_e_c_t_o_r allKeys) const { │ │ │ │ │ +340 Matrix E; │ │ │ │ │ +341 Vector b; │ │ │ │ │ +342 _c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s(Fs, E, b, _c_a_m_e_r_a_s, point); │ │ │ │ │ +343 _C_a_m_e_r_a_s_:_:_U_p_d_a_t_e_S_c_h_u_r_C_o_m_p_l_e_m_e_n_t(Fs, E, b, allKeys, _k_e_y_s__, augmentedHessian); │ │ │ │ │ +344 } │ │ │ │ │ +345 │ │ │ │ │ +_3_4_7 void _w_h_i_t_e_n_J_a_c_o_b_i_a_n_s(FBlocks& F, Matrix& E, Vector& b) const { │ │ │ │ │ +348 _n_o_i_s_e_M_o_d_e_l__->WhitenSystem(E, b); │ │ │ │ │ +349 // TODO make WhitenInPlace work with any dense matrix type │ │ │ │ │ +350 for (size_t i = 0; i < F.size(); i++) │ │ │ │ │ +351 F[i] = _n_o_i_s_e_M_o_d_e_l__->Whiten(F[i]); │ │ │ │ │ +352 } │ │ │ │ │ +353 │ │ │ │ │ +355 boost::shared_ptr > // │ │ │ │ │ +_3_5_6 _c_r_e_a_t_e_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r(const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, const _P_o_i_n_t_3& │ │ │ │ │ +point, │ │ │ │ │ +357 double lambda = 0.0, bool diagonalDamping = false) const { │ │ │ │ │ +358 Matrix E; │ │ │ │ │ +359 Vector b; │ │ │ │ │ +360 FBlocks F; │ │ │ │ │ +361 _c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s(F, E, b, _c_a_m_e_r_a_s, point); │ │ │ │ │ +362 _w_h_i_t_e_n_J_a_c_o_b_i_a_n_s(F, E, b); │ │ │ │ │ +363 Matrix P = _C_a_m_e_r_a_s_:_:_P_o_i_n_t_C_o_v(E, lambda, diagonalDamping); │ │ │ │ │ +364 return boost::make_shared >(_k_e_y_s__, F, E, │ │ │ │ │ +365 P, b); │ │ │ │ │ +366 } │ │ │ │ │ +367 │ │ │ │ │ +_3_6_9 boost::shared_ptr > _c_r_e_a_t_e_J_a_c_o_b_i_a_n_Q_F_a_c_t_o_r( │ │ │ │ │ +370 const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, const _P_o_i_n_t_3& point, double lambda = 0.0, │ │ │ │ │ +371 bool diagonalDamping = false) const { │ │ │ │ │ +372 Matrix E; │ │ │ │ │ +373 Vector b; │ │ │ │ │ +374 FBlocks F; │ │ │ │ │ +375 _c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s(F, E, b, _c_a_m_e_r_a_s, point); │ │ │ │ │ +376 const size_t M = b.size(); │ │ │ │ │ +377 Matrix P = _C_a_m_e_r_a_s_:_:_P_o_i_n_t_C_o_v(E, lambda, diagonalDamping); │ │ │ │ │ +378 SharedIsotropic n = _n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c_:_:_S_i_g_m_a(M, _n_o_i_s_e_M_o_d_e_l__->sigma()); │ │ │ │ │ +379 return boost::make_shared >(_k_e_y_s__, F, E, P, b, │ │ │ │ │ +n); │ │ │ │ │ +380 } │ │ │ │ │ +381 │ │ │ │ │ +_3_8_6 boost::shared_ptr _c_r_e_a_t_e_J_a_c_o_b_i_a_n_S_V_D_F_a_c_t_o_r( │ │ │ │ │ +387 const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, const _P_o_i_n_t_3& point, double lambda = 0.0) const { │ │ │ │ │ +388 size_t m = this->_k_e_y_s__.size(); │ │ │ │ │ +389 FBlocks F; │ │ │ │ │ +390 Vector b; │ │ │ │ │ +391 const size_t M = _Z_D_i_m * m; │ │ │ │ │ +392 Matrix E0(M, M - 3); │ │ │ │ │ +393 _c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s_S_V_D(F, E0, b, _c_a_m_e_r_a_s, point); │ │ │ │ │ +394 SharedIsotropic n = _n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c_:_:_S_i_g_m_a(M - 3, │ │ │ │ │ +395 _n_o_i_s_e_M_o_d_e_l__->sigma()); │ │ │ │ │ +396 return boost::make_shared >(_k_e_y_s__, F, E0, b, │ │ │ │ │ +n); │ │ │ │ │ +397 } │ │ │ │ │ +398 │ │ │ │ │ +_4_0_0 static void _F_i_l_l_D_i_a_g_o_n_a_l_F(const FBlocks& Fs, Matrix& F) { │ │ │ │ │ +401 size_t m = Fs.size(); │ │ │ │ │ +402 F.resize(_Z_D_i_m * m, _D_i_m * m); │ │ │ │ │ +403 F.setZero(); │ │ │ │ │ +404 for (size_t i = 0; i < m; ++i) │ │ │ │ │ +405 F.block<_Z_D_i_m, _D_i_m>(_Z_D_i_m * i, _D_i_m * i) = Fs.at(i); │ │ │ │ │ +406 } │ │ │ │ │ +407 │ │ │ │ │ +408 // Return sensor pose. │ │ │ │ │ +409 _P_o_s_e_3 body_P_sensor() const{ │ │ │ │ │ +410 if(_b_o_d_y___P___s_e_n_s_o_r__) │ │ │ │ │ +411 return *_b_o_d_y___P___s_e_n_s_o_r__; │ │ │ │ │ +412 else │ │ │ │ │ +413 return _P_o_s_e_3(); // if unspecified, the transformation is the identity │ │ │ │ │ +414 } │ │ │ │ │ +415 │ │ │ │ │ +416private: │ │ │ │ │ +417 │ │ │ │ │ +_4_1_9 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +420 template │ │ │ │ │ +421 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +422 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +423 ar & BOOST_SERIALIZATION_NVP(_n_o_i_s_e_M_o_d_e_l__); │ │ │ │ │ +424 ar & BOOST_SERIALIZATION_NVP(_m_e_a_s_u_r_e_d__); │ │ │ │ │ +425 ar & BOOST_SERIALIZATION_NVP(_b_o_d_y___P___s_e_n_s_o_r__); │ │ │ │ │ +426 } │ │ │ │ │ +427}; │ │ │ │ │ +428// end class SmartFactorBase │ │ │ │ │ +429 │ │ │ │ │ +430// Definitions need to avoid link errors (above are only declarations) │ │ │ │ │ +431template const int _S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_<_C_A_M_E_R_A_>_:_:_D_i_m; │ │ │ │ │ +432template const int _S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_<_C_A_M_E_R_A_>_:_:_Z_D_i_m; │ │ │ │ │ +433 │ │ │ │ │ +434} // \ namespace gtsam │ │ │ │ │ +_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ +This marks a GTSAM object to require alignment. │ │ │ │ │ +DDeeffiinniittiioonn types.h:308 │ │ │ │ │ +_C_a_m_e_r_a_S_e_t_._h │ │ │ │ │ +Base class to create smart factors on poses or cameras. │ │ │ │ │ +_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ +HessianFactor class with constant sized blocks. │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ +Non-linear factor base classes. │ │ │ │ │ +_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_._h │ │ │ │ │ +A subclass of GaussianFactor specialized to structureless SFM. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ FastVector< Key > KeyVector │ │ │ │ │ Define collection type once and for all - also used in wrappers. │ │ │ │ │ DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_s_v_d │ │ │ │ │ +void svd(const Matrix &A, Matrix &U, Vector &S, Matrix &V) │ │ │ │ │ +SVD computes economy SVD A=U*S*V'. │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:560 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ +Vector3 Point3 │ │ │ │ │ +As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ +typedef Point3 to Vector3... │ │ │ │ │ +DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ +Aliases. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_e_l_i_m_i_n_a_t_e_P_a_r_t_i_a_l_S_e_q_u_e_n_t_i_a_l │ │ │ │ │ -std::pair< boost::shared_ptr< BayesNetType >, boost::shared_ptr< │ │ │ │ │ -FactorGraphType > > eliminatePartialSequential(const Ordering &ordering, const │ │ │ │ │ -Eliminate &function=EliminationTraitsType::DefaultEliminate, │ │ │ │ │ -OptionalVariableIndex variableIndex=boost::none) const │ │ │ │ │ -Do sequential elimination of some variables, in ordering provided, to produce a │ │ │ │ │ -Bayes net and a remai... │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph-inst.h:154 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s │ │ │ │ │ -const KeyVector & keys() const │ │ │ │ │ -Access the factor's involved variable keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:140 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l │ │ │ │ │ -Character and index key used to refer to variables. │ │ │ │ │ -DDeeffiinniittiioonn Symbol.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:81 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d │ │ │ │ │ -void add(const GaussianFactor &factor) │ │ │ │ │ -Add a factor by value - makes a copy. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:127 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ -A Gaussian factor in the squared-error form. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:91 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ -JacobianFactor with constant sized blocks Provides raw memory access versions │ │ │ │ │ -of linear operator. │ │ │ │ │ -DDeeffiinniittiioonn RegularJacobianFactor.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q_R │ │ │ │ │ -JacobianFactor for Schur complement that uses Q noise model. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactorQR.h:21 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q_R_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q_R │ │ │ │ │ -JacobianFactorQR(const KeyVector &keys, const std::vector< MatrixZD, Eigen:: │ │ │ │ │ -aligned_allocator< MatrixZD > > &FBlocks, const Matrix &E, const Matrix3 &P, │ │ │ │ │ -const Vector &b, const SharedDiagonal &model=SharedDiagonal()) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactorQR.h:31 │ │ │ │ │ -_S_y_m_b_o_l_._h │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_F_i_x_e_d_D_i_m_e_n_s_i_o_n │ │ │ │ │ +Give fixed size dimension of a type, fails at compile time if dynamic. │ │ │ │ │ +DDeeffiinniittiioonn Manifold.h:164 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ +of blocks. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t │ │ │ │ │ +A set of cameras, all with their own calibration. │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_r_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r │ │ │ │ │ +Vector reprojectionError(const POINT &point, const ZVector &measured, boost:: │ │ │ │ │ +optional< FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost:: │ │ │ │ │ +none) const │ │ │ │ │ +Calculate vector [project2(point)-z] of re-projection errors. │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:136 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_P_o_i_n_t_C_o_v │ │ │ │ │ +static Matrix PointCov(const Matrix &E, const double lambda=0.0, bool │ │ │ │ │ +diagonalDamping=false) │ │ │ │ │ +Computes Point Covariance P, with lambda parameter, dynamic version. │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:331 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_U_p_d_a_t_e_S_c_h_u_r_C_o_m_p_l_e_m_e_n_t │ │ │ │ │ +static void UpdateSchurComplement(const FBlocks &Fs, const Matrix &E, const │ │ │ │ │ +Eigen::Matrix< double, N, N > &P, const Vector &b, const KeyVector &allKeys, │ │ │ │ │ +const KeyVector &keys, SymmetricBlockMatrix &augmentedHessian) │ │ │ │ │ +Applies Schur complement (exploiting block structure) to get a smart factor on │ │ │ │ │ +cameras,... │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:369 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_S_c_h_u_r_C_o_m_p_l_e_m_e_n_t │ │ │ │ │ +static SymmetricBlockMatrix SchurComplement(const std::vector< Eigen::Matrix< │ │ │ │ │ +double, ZDim, ND >, Eigen::aligned_allocator< Eigen::Matrix< double, ZDim, ND > │ │ │ │ │ +> > &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector │ │ │ │ │ +&b) │ │ │ │ │ +Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = │ │ │ │ │ +F' * F - F' * E * P * ... │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:150 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ +A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_i_n_v_e_r_s_e │ │ │ │ │ +Pose3 inverse() const │ │ │ │ │ +inverse transformation with derivatives │ │ │ │ │ +DDeeffiinniittiioonn Pose3.cpp:49 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s__ │ │ │ │ │ +KeyVector keys_ │ │ │ │ │ +The keys involved in this factor. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c │ │ │ │ │ +An isotropic noise model corresponds to a scaled diagonal covariance To │ │ │ │ │ +construct,... │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:516 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c_:_:_S_i_g_m_a │ │ │ │ │ +static shared_ptr Sigma(size_t dim, double sigma, bool smart=true) │ │ │ │ │ +An isotropic noise model created by specifying a standard devation sigma. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.cpp:597 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ +Nonlinear factor base class. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const │ │ │ │ │ +Check if two factors are equal. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.cpp:47 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.cpp:37 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_a_t │ │ │ │ │ +const ValueType at(Key j) const │ │ │ │ │ +Retrieve a variable by key j. │ │ │ │ │ +DDeeffiinniittiioonn Values-inl.h:361 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e │ │ │ │ │ +Base class for smart factors. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s │ │ │ │ │ +void computeJacobians(FBlocks &Fs, Matrix &E, Vector &b, const Cameras │ │ │ │ │ +&cameras, const POINT &point) const │ │ │ │ │ +Compute F, E, and b (called below in both vanilla and SVD versions), where F is │ │ │ │ │ +a vector of derivativ... │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:285 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_a_d_d │ │ │ │ │ +void add(const Z &measured, const Key &key) │ │ │ │ │ +Add a new measurement and pose/camera key. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:126 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_~_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e │ │ │ │ │ +~SmartFactorBase() override │ │ │ │ │ +Virtual destructor, subclasses from NonlinearFactor. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:118 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_u_p_d_a_t_e_A_u_g_m_e_n_t_e_d_H_e_s_s_i_a_n │ │ │ │ │ +void updateAugmentedHessian(const Cameras &cameras, const Point3 &point, const │ │ │ │ │ +double lambda, bool diagonalDamping, SymmetricBlockMatrix &augmentedHessian, │ │ │ │ │ +const KeyVector allKeys) const │ │ │ │ │ +Add the contribution of the smart factor to a pre-allocated Hessian, using │ │ │ │ │ +sparse linear algebra. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:336 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_c_r_e_a_t_e_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r │ │ │ │ │ +boost::shared_ptr< RegularImplicitSchurFactor< CAMERA > > │ │ │ │ │ +createRegularImplicitSchurFactor(const Cameras &cameras, const Point3 &point, │ │ │ │ │ +double lambda=0.0, bool diagonalDamping=false) const │ │ │ │ │ +Return Jacobians as RegularImplicitSchurFactor with raw access. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:356 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_n_o_i_s_e_M_o_d_e_l__ │ │ │ │ │ +SharedIsotropic noiseModel_ │ │ │ │ │ +As of Feb 22, 2015, the noise model is the same for all measurements and is │ │ │ │ │ +isotropic. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_D_i_m │ │ │ │ │ +static const int Dim │ │ │ │ │ +Camera dimension. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:60 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_w_h_i_t_e_n_e_d_E_r_r_o_r │ │ │ │ │ +Vector whitenedError(const Cameras &cameras, const POINT &point) const │ │ │ │ │ +Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z], │ │ │ │ │ +with the noise model ap... │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:251 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_c_a_m_e_r_a_s │ │ │ │ │ +virtual Cameras cameras(const Values &values) const │ │ │ │ │ +Collect all cameras: important that in key order. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:162 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_F_i_l_l_D_i_a_g_o_n_a_l_F │ │ │ │ │ +static void FillDiagonalF(const FBlocks &Fs, Matrix &F) │ │ │ │ │ +Create BIG block-diagonal matrix F from Fblocks. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:400 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_t_o_t_a_l_R_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r │ │ │ │ │ +double totalReprojectionError(const Cameras &cameras, const POINT &point) const │ │ │ │ │ +Calculate the error of the factor. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:267 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_a_d_d │ │ │ │ │ +void add(const ZVector &measurements, const KeyVector &cameraKeys) │ │ │ │ │ +Add a bunch of measurements, together with the camera keys. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:136 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_c_r_e_a_t_e_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +boost::shared_ptr< RegularHessianFactor< Dim > > createHessianFactor(const │ │ │ │ │ +Cameras &cameras, const Point3 &point, const double lambda=0.0, bool │ │ │ │ │ +diagonalDamping=false) const │ │ │ │ │ +Linearize to a Hessianfactor. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:316 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s_S_V_D │ │ │ │ │ +void computeJacobiansSVD(FBlocks &Fs, Matrix &Enull, Vector &b, const Cameras │ │ │ │ │ +&cameras, const POINT &point) const │ │ │ │ │ +SVD version that produces smaller Jacobian matrices by doing an SVD │ │ │ │ │ +decomposition on E,... │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:300 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_m_e_a_s_u_r_e_d__ │ │ │ │ │ +ZVector measured_ │ │ │ │ │ +Measurements for each of the m views. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:79 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +GTSAM_MAKE_ALIGNED_OPERATOR_NEW typedef boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shorthand for a smart pointer to a factor. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:91 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_u_n_w_h_i_t_e_n_e_d_E_r_r_o_r │ │ │ │ │ +Vector unwhitenedError(const Cameras &cameras, const POINT &point, boost:: │ │ │ │ │ +optional< typename Cameras::FBlocks & > Fs=boost::none, boost::optional< Matrix │ │ │ │ │ +& > E=boost::none) const │ │ │ │ │ +Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and derivatives. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:204 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e │ │ │ │ │ +SmartFactorBase() │ │ │ │ │ +Default Constructor, for serialization. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:97 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_w_h_i_t_e_n_J_a_c_o_b_i_a_n_s │ │ │ │ │ +void whitenJacobians(FBlocks &F, Matrix &E, Vector &b) const │ │ │ │ │ +Whiten the Jacobians computed by computeJacobians using noiseModel_. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:347 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e │ │ │ │ │ +SmartFactorBase(const SharedNoiseModel &sharedNoiseModel, boost::optional< │ │ │ │ │ +Pose3 > body_P_sensor=boost::none, size_t expectedNumberCameras=10) │ │ │ │ │ +Construct with given noise model and optional arguments. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_a_d_d │ │ │ │ │ +void add(const SFM_TRACK &trackToAdd) │ │ │ │ │ +Add an entire SfM_track (collection of cameras observing a single point). │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:148 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_m_e_a_s_u_r_e_d │ │ │ │ │ +const ZVector & measured() const │ │ │ │ │ +Return the 2D measurements (ZDim, in general). │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:159 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_c_r_e_a_t_e_J_a_c_o_b_i_a_n_S_V_D_F_a_c_t_o_r │ │ │ │ │ +boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras │ │ │ │ │ +&cameras, const Point3 &point, double lambda=0.0) const │ │ │ │ │ +Return Jacobians as JacobianFactorSVD. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:386 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_d_i_m │ │ │ │ │ +size_t dim() const override │ │ │ │ │ +Return the dimension (number of rows!) of the factor. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:156 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_c_r_e_a_t_e_J_a_c_o_b_i_a_n_Q_F_a_c_t_o_r │ │ │ │ │ +boost::shared_ptr< JacobianFactorQ< Dim, ZDim > > createJacobianQFactor(const │ │ │ │ │ +Cameras &cameras, const Point3 &point, double lambda=0.0, bool │ │ │ │ │ +diagonalDamping=false) const │ │ │ │ │ +Return Jacobians as JacobianFactorQ. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:369 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:419 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:174 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_b_o_d_y___P___s_e_n_s_o_r__ │ │ │ │ │ +boost::optional< Pose3 > body_P_sensor_ │ │ │ │ │ +Pose of the camera in the body frame. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:82 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_c_o_r_r_e_c_t_F_o_r_M_i_s_s_i_n_g_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ +virtual void correctForMissingMeasurements(const Cameras &cameras, Vector &ue, │ │ │ │ │ +boost::optional< typename Cameras::FBlocks & > Fs=boost::none, boost::optional< │ │ │ │ │ +Matrix & > E=boost::none) const │ │ │ │ │ +This corrects the Jacobians for the case in which some 2D measurement is │ │ │ │ │ +missing (nan). │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:241 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_Z_D_i_m │ │ │ │ │ +static const int ZDim │ │ │ │ │ +Measurement dimension. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:61 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_P_o_i_n_t_C_o_v │ │ │ │ │ +static Matrix PointCov(const Matrix &E) │ │ │ │ │ +Computes Point Covariance P from the "point Jacobian" E. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:274 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_C_a_m_e_r_a_s │ │ │ │ │ +CameraSet< CAMERA > Cameras │ │ │ │ │ +The CameraSet data structure is used to refer to a set of cameras. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:94 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const NonlinearFactor &p, double tol=1e-9) const override │ │ │ │ │ +equals │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:187 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * JJaaccoobbiiaannFFaaccttoorrQQRR..hh │ │ │ │ │ + * _S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01268.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/SmartFactorBase.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/InitializePose.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,52 +94,38 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
SmartFactorBase.h File Reference
│ │ │ │ +
InitializePose.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Base class to create smart factors on poses or cameras. │ │ │ │ +

common code between lago.* (2D) and InitializePose3.* (3D) │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::SmartFactorBase< CAMERA >
 Base class for smart factors. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Base class to create smart factors on poses or cameras.

│ │ │ │ -
Author
Luca Carlone
│ │ │ │ -
│ │ │ │ -Antoni Rosinol
│ │ │ │ -
│ │ │ │ -Zsolt Kira
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ -
│ │ │ │ -Chris Beall
│ │ │ │ +

common code between lago.* (2D) and InitializePose3.* (3D)

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
Date
August, 2020
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,29 +1,23 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SmartFactorBase.h File Reference │ │ │ │ │ -Base class to create smart factors on poses or cameras. _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +InitializePose.h File Reference │ │ │ │ │ +common code between lago.* (2D) and InitializePose3.* (3D) _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_<_ _C_A_M_E_R_A_ _> │ │ │ │ │ -  Base class for smart factors. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Base class to create smart factors on poses or cameras. │ │ │ │ │ +common code between lago.* (2D) and InitializePose3.* (3D) │ │ │ │ │ Author │ │ │ │ │ - Luca Carlone │ │ │ │ │ - Antoni Rosinol │ │ │ │ │ - Zsolt Kira │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Chris Beall │ │ │ │ │ + Date │ │ │ │ │ + August, 2020 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_._h │ │ │ │ │ + * _I_n_i_t_i_a_l_i_z_e_P_o_s_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01268_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/SmartFactorBase.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/InitializePose.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,464 +98,116 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
SmartFactorBase.h
│ │ │ │ +
InitializePose.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
22#pragma once
│ │ │ │ -
23
│ │ │ │ -
24#include <gtsam/slam/JacobianFactorQ.h>
│ │ │ │ -
25#include <gtsam/slam/JacobianFactorSVD.h>
│ │ │ │ -
26#include <gtsam/slam/RegularImplicitSchurFactor.h>
│ │ │ │ -
27
│ │ │ │ -
28#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
29#include <gtsam/linear/RegularHessianFactor.h>
│ │ │ │ -
30#include <gtsam/geometry/CameraSet.h>
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/inference/Symbol.h>
│ │ │ │ +
22#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
│ │ │ │ +
23#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ +
24#include <gtsam/nonlinear/PriorFactor.h>
│ │ │ │ +
25#include <gtsam/slam/BetweenFactor.h>
│ │ │ │ +
26
│ │ │ │ +
27namespace gtsam {
│ │ │ │ +
28namespace initialize {
│ │ │ │ +
29
│ │ │ │ +
30static constexpr Key kAnchorKey = 99999999;
│ │ │ │
31
│ │ │ │ -
32#include <boost/optional.hpp>
│ │ │ │ -
33#include <boost/serialization/optional.hpp>
│ │ │ │ -
34#include <boost/make_shared.hpp>
│ │ │ │ -
35#include <vector>
│ │ │ │ -
36
│ │ │ │ -
37namespace gtsam {
│ │ │ │ -
38
│ │ │ │ -
49template<class CAMERA>
│ │ │ │ -
│ │ │ │ -
50class SmartFactorBase: public NonlinearFactor {
│ │ │ │ -
51
│ │ │ │ -
52private:
│ │ │ │ -
53 typedef NonlinearFactor Base;
│ │ │ │ -
54 typedef SmartFactorBase<CAMERA> This;
│ │ │ │ -
55 typedef typename CAMERA::Measurement Z;
│ │ │ │ -
56 typedef typename CAMERA::MeasurementVector ZVector;
│ │ │ │ -
57
│ │ │ │ -
58public:
│ │ │ │ -
59
│ │ │ │ -
60 static const int Dim = traits<CAMERA>::dimension;
│ │ │ │ -
61 static const int ZDim = traits<Z>::dimension;
│ │ │ │ -
62 typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F blocks (derivatives wrpt camera)
│ │ │ │ -
63 typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks; // vector of F blocks
│ │ │ │ -
64
│ │ │ │ -
65protected:
│ │ │ │ -
72 SharedIsotropic noiseModel_;
│ │ │ │ -
73
│ │ │ │ -
79 ZVector measured_;
│ │ │ │ -
80
│ │ │ │ -
81 boost::optional<Pose3>
│ │ │ │ -
82 body_P_sensor_;
│ │ │ │ -
83
│ │ │ │ -
84 // Cache for Fblocks, to avoid a malloc ever time we re-linearize
│ │ │ │ -
85 mutable FBlocks Fs;
│ │ │ │ +
36template <class Pose>
│ │ │ │ +
37static NonlinearFactorGraph buildPoseGraph(const NonlinearFactorGraph& graph) {
│ │ │ │ +
38 NonlinearFactorGraph poseGraph;
│ │ │ │ +
39
│ │ │ │ +
40 for (const auto& factor : graph) {
│ │ │ │ +
41 // recast to a between on Pose
│ │ │ │ +
42 if (auto between =
│ │ │ │ +
43 boost::dynamic_pointer_cast<BetweenFactor<Pose> >(factor))
│ │ │ │ +
44 poseGraph.add(between);
│ │ │ │ +
45
│ │ │ │ +
46 // recast PriorFactor<Pose> to BetweenFactor<Pose>
│ │ │ │ +
47 if (auto prior = boost::dynamic_pointer_cast<PriorFactor<Pose> >(factor))
│ │ │ │ +
48 poseGraph.emplace_shared<BetweenFactor<Pose> >(
│ │ │ │ +
49 kAnchorKey, prior->keys()[0], prior->prior(), prior->noiseModel());
│ │ │ │ +
50 }
│ │ │ │ +
51 return poseGraph;
│ │ │ │ +
52}
│ │ │ │ +
53
│ │ │ │ +
57template <class Pose>
│ │ │ │ +
58static Values computePoses(const Values& initialRot,
│ │ │ │ +
59 NonlinearFactorGraph* posegraph,
│ │ │ │ +
60 bool singleIter = true) {
│ │ │ │ +
61 const auto origin = Pose().translation();
│ │ │ │ +
62
│ │ │ │ +
63 // Upgrade rotations to full poses
│ │ │ │ +
64 Values initialPose;
│ │ │ │ +
65 for (const auto& key_rot : initialRot.extract<typename Pose::Rotation>()) {
│ │ │ │ +
66 const Key& key = key_rot.first;
│ │ │ │ +
67 const auto& rot = key_rot.second;
│ │ │ │ +
68 const Pose initializedPose(rot, origin);
│ │ │ │ +
69 initialPose.insert(key, initializedPose);
│ │ │ │ +
70 }
│ │ │ │ +
71
│ │ │ │ +
72 // add prior on dummy node
│ │ │ │ +
73 auto priorModel = noiseModel::Unit::Create(Pose::dimension);
│ │ │ │ +
74 initialPose.insert(kAnchorKey, Pose());
│ │ │ │ +
75 posegraph->emplace_shared<PriorFactor<Pose> >(kAnchorKey, Pose(), priorModel);
│ │ │ │ +
76
│ │ │ │ +
77 // Create optimizer
│ │ │ │ +
78 GaussNewtonParams params;
│ │ │ │ +
79 if (singleIter) {
│ │ │ │ +
80 params.maxIterations = 1;
│ │ │ │ +
81 } else {
│ │ │ │ +
82 params.setVerbosity("TERMINATION");
│ │ │ │ +
83 }
│ │ │ │ +
84 GaussNewtonOptimizer optimizer(*posegraph, initialPose, params);
│ │ │ │ +
85 const Values GNresult = optimizer.optimize();
│ │ │ │
86
│ │ │ │ -
87 public:
│ │ │ │ -
88 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
89
│ │ │ │ -
91 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
92
│ │ │ │ -
94 typedef CameraSet<CAMERA> Cameras;
│ │ │ │ -
95
│ │ │ │ -
97 SmartFactorBase() {}
│ │ │ │ -
98
│ │ │ │ -
│ │ │ │ -
100 SmartFactorBase(const SharedNoiseModel& sharedNoiseModel,
│ │ │ │ -
101 boost::optional<Pose3> body_P_sensor = boost::none,
│ │ │ │ -
102 size_t expectedNumberCameras = 10)
│ │ │ │ -
103 : body_P_sensor_(body_P_sensor), Fs(expectedNumberCameras) {
│ │ │ │ -
104
│ │ │ │ -
105 if (!sharedNoiseModel)
│ │ │ │ -
106 throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required");
│ │ │ │ -
107
│ │ │ │ -
108 SharedIsotropic sharedIsotropic = boost::dynamic_pointer_cast<
│ │ │ │ -
109 noiseModel::Isotropic>(sharedNoiseModel);
│ │ │ │ -
110
│ │ │ │ -
111 if (!sharedIsotropic)
│ │ │ │ -
112 throw std::runtime_error("SmartFactorBase: needs isotropic");
│ │ │ │ -
113
│ │ │ │ -
114 noiseModel_ = sharedIsotropic;
│ │ │ │ -
115 }
│ │ │ │ -
│ │ │ │ -
116
│ │ │ │ -
│ │ │ │ -
118 ~SmartFactorBase() override {
│ │ │ │ -
119 }
│ │ │ │ -
│ │ │ │ -
120
│ │ │ │ -
│ │ │ │ -
126 void add(const Z& measured, const Key& key) {
│ │ │ │ -
127 if(std::find(keys_.begin(), keys_.end(), key) != keys_.end()) {
│ │ │ │ -
128 throw std::invalid_argument(
│ │ │ │ -
129 "SmartFactorBase::add: adding duplicate measurement for key.");
│ │ │ │ -
130 }
│ │ │ │ -
131 this->measured_.push_back(measured);
│ │ │ │ -
132 this->keys_.push_back(key);
│ │ │ │ -
133 }
│ │ │ │ -
│ │ │ │ -
134
│ │ │ │ -
│ │ │ │ -
136 void add(const ZVector& measurements, const KeyVector& cameraKeys) {
│ │ │ │ -
137 assert(measurements.size() == cameraKeys.size());
│ │ │ │ -
138 for (size_t i = 0; i < measurements.size(); i++) {
│ │ │ │ -
139 this->add(measurements[i], cameraKeys[i]);
│ │ │ │ -
140 }
│ │ │ │ -
141 }
│ │ │ │ -
│ │ │ │ -
142
│ │ │ │ -
147 template<class SFM_TRACK>
│ │ │ │ -
│ │ │ │ -
148 void add(const SFM_TRACK& trackToAdd) {
│ │ │ │ -
149 for (size_t k = 0; k < trackToAdd.numberMeasurements(); k++) {
│ │ │ │ -
150 this->measured_.push_back(trackToAdd.measurements[k].second);
│ │ │ │ -
151 this->keys_.push_back(trackToAdd.measurements[k].first);
│ │ │ │ -
152 }
│ │ │ │ -
153 }
│ │ │ │ -
│ │ │ │ -
154
│ │ │ │ -
156 size_t dim() const override { return ZDim * this->measured_.size(); }
│ │ │ │ -
157
│ │ │ │ -
159 const ZVector& measured() const { return measured_; }
│ │ │ │ -
160
│ │ │ │ -
│ │ │ │ -
162 virtual Cameras cameras(const Values& values) const {
│ │ │ │ -
163 Cameras cameras;
│ │ │ │ -
164 for(const Key& k: this->keys_)
│ │ │ │ -
165 cameras.push_back(values.at<CAMERA>(k));
│ │ │ │ -
166 return cameras;
│ │ │ │ -
167 }
│ │ │ │ -
│ │ │ │ -
168
│ │ │ │ -
│ │ │ │ -
174 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
│ │ │ │ -
175 DefaultKeyFormatter) const override {
│ │ │ │ -
176 std::cout << s << "SmartFactorBase, z = \n";
│ │ │ │ -
177 for (size_t k = 0; k < measured_.size(); ++k) {
│ │ │ │ -
178 std::cout << "measurement " << k<<", px = \n" << measured_[k] << "\n";
│ │ │ │ -
179 noiseModel_->print("noise model = ");
│ │ │ │ -
180 }
│ │ │ │ -
181 if(body_P_sensor_)
│ │ │ │ -
182 body_P_sensor_->print("body_P_sensor_:\n");
│ │ │ │ -
183 Base::print("", keyFormatter);
│ │ │ │ -
184 }
│ │ │ │ -
│ │ │ │ -
185
│ │ │ │ -
│ │ │ │ -
187 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
│ │ │ │ -
188 if (const This* e = dynamic_cast<const This*>(&p)) {
│ │ │ │ -
189 // Check that all measurements are the same.
│ │ │ │ -
190 for (size_t i = 0; i < measured_.size(); i++) {
│ │ │ │ -
191 if (!traits<Z>::Equals(this->measured_.at(i), e->measured_.at(i), tol))
│ │ │ │ -
192 return false;
│ │ │ │ -
193 }
│ │ │ │ -
194 // If so, check base class.
│ │ │ │ -
195 return Base::equals(p, tol);
│ │ │ │ -
196 } else {
│ │ │ │ -
197 return false;
│ │ │ │ -
198 }
│ │ │ │ -
199 }
│ │ │ │ -
│ │ │ │ -
200
│ │ │ │ -
203 template <class POINT>
│ │ │ │ -
│ │ │ │ -
204 Vector unwhitenedError(
│ │ │ │ -
205 const Cameras& cameras, const POINT& point,
│ │ │ │ -
206 boost::optional<typename Cameras::FBlocks&> Fs = boost::none, //
│ │ │ │ -
207 boost::optional<Matrix&> E = boost::none) const {
│ │ │ │ -
208 // Reproject, with optional derivatives.
│ │ │ │ -
209 Vector error = cameras.reprojectionError(point, measured_, Fs, E);
│ │ │ │ -
210
│ │ │ │ -
211 // Apply chain rule if body_P_sensor_ is given.
│ │ │ │ -
212 if (body_P_sensor_ && Fs) {
│ │ │ │ -
213 const Pose3 sensor_P_body = body_P_sensor_->inverse();
│ │ │ │ -
214 constexpr int camera_dim = traits<CAMERA>::dimension;
│ │ │ │ -
215 constexpr int pose_dim = traits<Pose3>::dimension;
│ │ │ │ -
216
│ │ │ │ -
217 for (size_t i = 0; i < Fs->size(); i++) {
│ │ │ │ -
218 const Pose3 world_P_body = cameras[i].pose() * sensor_P_body;
│ │ │ │ -
219 Eigen::Matrix<double, camera_dim, camera_dim> J;
│ │ │ │ -
220 J.setZero();
│ │ │ │ -
221 Eigen::Matrix<double, pose_dim, pose_dim> H;
│ │ │ │ -
222 // Call compose to compute Jacobian for camera extrinsics
│ │ │ │ -
223 world_P_body.compose(*body_P_sensor_, H);
│ │ │ │ -
224 // Assign extrinsics part of the Jacobian
│ │ │ │ -
225 J.template block<pose_dim, pose_dim>(0, 0) = H;
│ │ │ │ -
226 Fs->at(i) = Fs->at(i) * J;
│ │ │ │ -
227 }
│ │ │ │ -
228 }
│ │ │ │ -
229
│ │ │ │ -
230 // Correct the Jacobians in case some measurements are missing.
│ │ │ │ -
231 correctForMissingMeasurements(cameras, error, Fs, E);
│ │ │ │ -
232
│ │ │ │ -
233 return error;
│ │ │ │ -
234 }
│ │ │ │ -
│ │ │ │ -
235
│ │ │ │ -
│ │ │ │ -
241 virtual void correctForMissingMeasurements(
│ │ │ │ -
242 const Cameras& cameras, Vector& ue,
│ │ │ │ -
243 boost::optional<typename Cameras::FBlocks&> Fs = boost::none,
│ │ │ │ -
244 boost::optional<Matrix&> E = boost::none) const {}
│ │ │ │ -
│ │ │ │ -
245
│ │ │ │ -
250 template<class POINT>
│ │ │ │ -
│ │ │ │ -
251 Vector whitenedError(const Cameras& cameras, const POINT& point) const {
│ │ │ │ -
252 Vector error = cameras.reprojectionError(point, measured_);
│ │ │ │ -
253 if (noiseModel_)
│ │ │ │ -
254 noiseModel_->whitenInPlace(error);
│ │ │ │ -
255 return error;
│ │ │ │ -
256 }
│ │ │ │ -
│ │ │ │ -
257
│ │ │ │ -
266 template<class POINT>
│ │ │ │ -
│ │ │ │ -
267 double totalReprojectionError(const Cameras& cameras,
│ │ │ │ -
268 const POINT& point) const {
│ │ │ │ -
269 Vector error = whitenedError(cameras, point);
│ │ │ │ -
270 return 0.5 * error.dot(error);
│ │ │ │ -
271 }
│ │ │ │ -
│ │ │ │ -
272
│ │ │ │ -
│ │ │ │ -
274 static Matrix PointCov(const Matrix& E) {
│ │ │ │ -
275 return (E.transpose() * E).inverse();
│ │ │ │ -
276 }
│ │ │ │ -
│ │ │ │ -
277
│ │ │ │ -
284 template<class POINT>
│ │ │ │ -
│ │ │ │ -
285 void computeJacobians(FBlocks& Fs, Matrix& E, Vector& b,
│ │ │ │ -
286 const Cameras& cameras, const POINT& point) const {
│ │ │ │ -
287 // Project into Camera set and calculate derivatives
│ │ │ │ -
288 // As in expressionFactor, RHS vector b = - (h(x_bar) - z) = z-h(x_bar)
│ │ │ │ -
289 // Indeed, nonlinear error |h(x_bar+dx)-z| ~ |h(x_bar) + A*dx - z|
│ │ │ │ -
290 // = |A*dx - (z-h(x_bar))|
│ │ │ │ -
291 b = -unwhitenedError(cameras, point, Fs, E);
│ │ │ │ -
292 }
│ │ │ │ -
│ │ │ │ -
293
│ │ │ │ -
299 template<class POINT>
│ │ │ │ -
│ │ │ │ -
300 void computeJacobiansSVD(FBlocks& Fs, Matrix& Enull,
│ │ │ │ -
301 Vector& b, const Cameras& cameras, const POINT& point) const {
│ │ │ │ -
302
│ │ │ │ -
303 Matrix E;
│ │ │ │ -
304 computeJacobians(Fs, E, b, cameras, point);
│ │ │ │ -
305
│ │ │ │ -
306 static const int N = FixedDimension<POINT>::value; // 2 (Unit3) or 3 (Point3)
│ │ │ │ -
307
│ │ │ │ -
308 // Do SVD on A.
│ │ │ │ -
309 Eigen::JacobiSVD<Matrix> svd(E, Eigen::ComputeFullU);
│ │ │ │ -
310 size_t m = this->keys_.size();
│ │ │ │ -
311 Enull = svd.matrixU().block(0, N, ZDim * m, ZDim * m - N); // last ZDim*m-N columns
│ │ │ │ -
312 }
│ │ │ │ -
│ │ │ │ -
313
│ │ │ │ -
315 // TODO(dellaert): Not used/tested anywhere and not properly whitened.
│ │ │ │ -
│ │ │ │ -
316 boost::shared_ptr<RegularHessianFactor<Dim> > createHessianFactor(
│ │ │ │ -
317 const Cameras& cameras, const Point3& point, const double lambda = 0.0,
│ │ │ │ -
318 bool diagonalDamping = false) const {
│ │ │ │ -
319
│ │ │ │ -
320 Matrix E;
│ │ │ │ -
321 Vector b;
│ │ │ │ -
322 computeJacobians(Fs, E, b, cameras, point);
│ │ │ │ -
323
│ │ │ │ -
324 // build augmented hessian
│ │ │ │ -
325 SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fs, E, b);
│ │ │ │ -
326
│ │ │ │ -
327 return boost::make_shared<RegularHessianFactor<Dim> >(keys_,
│ │ │ │ -
328 augmentedHessian);
│ │ │ │ -
329 }
│ │ │ │ -
│ │ │ │ -
330
│ │ │ │ -
│ │ │ │ -
336 void updateAugmentedHessian(const Cameras& cameras, const Point3& point,
│ │ │ │ -
337 const double lambda, bool diagonalDamping,
│ │ │ │ -
338 SymmetricBlockMatrix& augmentedHessian,
│ │ │ │ -
339 const KeyVector allKeys) const {
│ │ │ │ -
340 Matrix E;
│ │ │ │ -
341 Vector b;
│ │ │ │ -
342 computeJacobians(Fs, E, b, cameras, point);
│ │ │ │ -
343 Cameras::UpdateSchurComplement(Fs, E, b, allKeys, keys_, augmentedHessian);
│ │ │ │ -
344 }
│ │ │ │ -
│ │ │ │ -
345
│ │ │ │ -
│ │ │ │ -
347 void whitenJacobians(FBlocks& F, Matrix& E, Vector& b) const {
│ │ │ │ -
348 noiseModel_->WhitenSystem(E, b);
│ │ │ │ -
349 // TODO make WhitenInPlace work with any dense matrix type
│ │ │ │ -
350 for (size_t i = 0; i < F.size(); i++)
│ │ │ │ -
351 F[i] = noiseModel_->Whiten(F[i]);
│ │ │ │ -
352 }
│ │ │ │ -
│ │ │ │ -
353
│ │ │ │ -
355 boost::shared_ptr<RegularImplicitSchurFactor<CAMERA> > //
│ │ │ │ -
│ │ │ │ -
356 createRegularImplicitSchurFactor(const Cameras& cameras, const Point3& point,
│ │ │ │ -
357 double lambda = 0.0, bool diagonalDamping = false) const {
│ │ │ │ -
358 Matrix E;
│ │ │ │ -
359 Vector b;
│ │ │ │ -
360 FBlocks F;
│ │ │ │ -
361 computeJacobians(F, E, b, cameras, point);
│ │ │ │ -
362 whitenJacobians(F, E, b);
│ │ │ │ -
363 Matrix P = Cameras::PointCov(E, lambda, diagonalDamping);
│ │ │ │ -
364 return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_, F, E,
│ │ │ │ -
365 P, b);
│ │ │ │ -
366 }
│ │ │ │ -
│ │ │ │ -
367
│ │ │ │ -
│ │ │ │ -
369 boost::shared_ptr<JacobianFactorQ<Dim, ZDim> > createJacobianQFactor(
│ │ │ │ -
370 const Cameras& cameras, const Point3& point, double lambda = 0.0,
│ │ │ │ -
371 bool diagonalDamping = false) const {
│ │ │ │ -
372 Matrix E;
│ │ │ │ -
373 Vector b;
│ │ │ │ -
374 FBlocks F;
│ │ │ │ -
375 computeJacobians(F, E, b, cameras, point);
│ │ │ │ -
376 const size_t M = b.size();
│ │ │ │ -
377 Matrix P = Cameras::PointCov(E, lambda, diagonalDamping);
│ │ │ │ -
378 SharedIsotropic n = noiseModel::Isotropic::Sigma(M, noiseModel_->sigma());
│ │ │ │ -
379 return boost::make_shared<JacobianFactorQ<Dim, ZDim> >(keys_, F, E, P, b, n);
│ │ │ │ -
380 }
│ │ │ │ -
│ │ │ │ -
381
│ │ │ │ -
│ │ │ │ -
386 boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
│ │ │ │ -
387 const Cameras& cameras, const Point3& point, double lambda = 0.0) const {
│ │ │ │ -
388 size_t m = this->keys_.size();
│ │ │ │ -
389 FBlocks F;
│ │ │ │ -
390 Vector b;
│ │ │ │ -
391 const size_t M = ZDim * m;
│ │ │ │ -
392 Matrix E0(M, M - 3);
│ │ │ │ -
393 computeJacobiansSVD(F, E0, b, cameras, point);
│ │ │ │ -
394 SharedIsotropic n = noiseModel::Isotropic::Sigma(M - 3,
│ │ │ │ -
395 noiseModel_->sigma());
│ │ │ │ -
396 return boost::make_shared<JacobianFactorSVD<Dim, ZDim> >(keys_, F, E0, b, n);
│ │ │ │ -
397 }
│ │ │ │ -
│ │ │ │ -
398
│ │ │ │ -
│ │ │ │ -
400 static void FillDiagonalF(const FBlocks& Fs, Matrix& F) {
│ │ │ │ -
401 size_t m = Fs.size();
│ │ │ │ -
402 F.resize(ZDim * m, Dim * m);
│ │ │ │ -
403 F.setZero();
│ │ │ │ -
404 for (size_t i = 0; i < m; ++i)
│ │ │ │ -
405 F.block<ZDim, Dim>(ZDim * i, Dim * i) = Fs.at(i);
│ │ │ │ -
406 }
│ │ │ │ -
│ │ │ │ -
407
│ │ │ │ -
408 // Return sensor pose.
│ │ │ │ -
409 Pose3 body_P_sensor() const{
│ │ │ │ -
410 if(body_P_sensor_)
│ │ │ │ -
411 return *body_P_sensor_;
│ │ │ │ -
412 else
│ │ │ │ -
413 return Pose3(); // if unspecified, the transformation is the identity
│ │ │ │ -
414 }
│ │ │ │ -
415
│ │ │ │ -
416private:
│ │ │ │ -
417
│ │ │ │ -
419 friend class boost::serialization::access;
│ │ │ │ -
420 template<class ARCHIVE>
│ │ │ │ -
421 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
422 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
423 ar & BOOST_SERIALIZATION_NVP(noiseModel_);
│ │ │ │ -
424 ar & BOOST_SERIALIZATION_NVP(measured_);
│ │ │ │ -
425 ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
│ │ │ │ -
426 }
│ │ │ │ -
427};
│ │ │ │ -
│ │ │ │ -
428// end class SmartFactorBase
│ │ │ │ -
429
│ │ │ │ -
430// Definitions need to avoid link errors (above are only declarations)
│ │ │ │ -
431template<class CAMERA> const int SmartFactorBase<CAMERA>::Dim;
│ │ │ │ -
432template<class CAMERA> const int SmartFactorBase<CAMERA>::ZDim;
│ │ │ │ -
433
│ │ │ │ -
434} // \ namespace gtsam
│ │ │ │ -
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ -
CameraSet.h
Base class to create smart factors on poses or cameras.
│ │ │ │ -
RegularHessianFactor.h
HessianFactor class with constant sized blocks.
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ -
RegularImplicitSchurFactor.h
A subclass of GaussianFactor specialized to structureless SFM.
│ │ │ │ +
87 // put into Values structure
│ │ │ │ +
88 Values estimate;
│ │ │ │ +
89 for (const auto& key_pose : GNresult.extract<Pose>()) {
│ │ │ │ +
90 const Key& key = key_pose.first;
│ │ │ │ +
91 if (key != kAnchorKey) {
│ │ │ │ +
92 const Pose& pose = key_pose.second;
│ │ │ │ +
93 estimate.insert(key, pose);
│ │ │ │ +
94 }
│ │ │ │ +
95 }
│ │ │ │ +
96 return estimate;
│ │ │ │ +
97}
│ │ │ │ +
98} // namespace initialize
│ │ │ │ +
99} // namespace gtsam
│ │ │ │ + │ │ │ │ +
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::svd
void svd(const Matrix &A, Matrix &U, Vector &S, Matrix &V)
SVD computes economy SVD A=U*S*V'.
Definition Matrix.cpp:560
│ │ │ │ -
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ -
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::FixedDimension
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition Manifold.h:164
│ │ │ │ -
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
│ │ │ │ -
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition CameraSet.h:36
│ │ │ │ -
gtsam::CameraSet::reprojectionError
Vector reprojectionError(const POINT &point, const ZVector &measured, boost::optional< FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const
Calculate vector [project2(point)-z] of re-projection errors.
Definition CameraSet.h:136
│ │ │ │ -
gtsam::CameraSet::PointCov
static Matrix PointCov(const Matrix &E, const double lambda=0.0, bool diagonalDamping=false)
Computes Point Covariance P, with lambda parameter, dynamic version.
Definition CameraSet.h:331
│ │ │ │ -
gtsam::CameraSet::UpdateSchurComplement
static void UpdateSchurComplement(const FBlocks &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b, const KeyVector &allKeys, const KeyVector &keys, SymmetricBlockMatrix &augmentedHessian)
Applies Schur complement (exploiting block structure) to get a smart factor on cameras,...
Definition CameraSet.h:369
│ │ │ │ -
gtsam::CameraSet::SchurComplement
static SymmetricBlockMatrix SchurComplement(const std::vector< Eigen::Matrix< double, ZDim, ND >, Eigen::aligned_allocator< Eigen::Matrix< double, ZDim, ND > > > &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector &b)
Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = F' * F - F' * E * P * ...
Definition CameraSet.h:150
│ │ │ │ -
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ -
gtsam::Pose3::inverse
Pose3 inverse() const
inverse transformation with derivatives
Definition Pose3.cpp:49
│ │ │ │ -
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
│ │ │ │ -
gtsam::noiseModel::Isotropic
An isotropic noise model corresponds to a scaled diagonal covariance To construct,...
Definition NoiseModel.h:516
│ │ │ │ -
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
An isotropic noise model created by specifying a standard devation sigma.
Definition NoiseModel.cpp:597
│ │ │ │ -
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ -
gtsam::NonlinearFactor::equals
virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const
Check if two factors are equal.
Definition NonlinearFactor.cpp:47
│ │ │ │ -
gtsam::NonlinearFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition NonlinearFactor.cpp:37
│ │ │ │ -
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
gtsam::Values::at
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
│ │ │ │ -
gtsam::SmartFactorBase
Base class for smart factors.
Definition SmartFactorBase.h:50
│ │ │ │ -
gtsam::SmartFactorBase::computeJacobians
void computeJacobians(FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras, const POINT &point) const
Compute F, E, and b (called below in both vanilla and SVD versions), where F is a vector of derivativ...
Definition SmartFactorBase.h:285
│ │ │ │ -
gtsam::SmartFactorBase::add
void add(const Z &measured, const Key &key)
Add a new measurement and pose/camera key.
Definition SmartFactorBase.h:126
│ │ │ │ -
gtsam::SmartFactorBase::~SmartFactorBase
~SmartFactorBase() override
Virtual destructor, subclasses from NonlinearFactor.
Definition SmartFactorBase.h:118
│ │ │ │ -
gtsam::SmartFactorBase::updateAugmentedHessian
void updateAugmentedHessian(const Cameras &cameras, const Point3 &point, const double lambda, bool diagonalDamping, SymmetricBlockMatrix &augmentedHessian, const KeyVector allKeys) const
Add the contribution of the smart factor to a pre-allocated Hessian, using sparse linear algebra.
Definition SmartFactorBase.h:336
│ │ │ │ -
gtsam::SmartFactorBase::createRegularImplicitSchurFactor
boost::shared_ptr< RegularImplicitSchurFactor< CAMERA > > createRegularImplicitSchurFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0, bool diagonalDamping=false) const
Return Jacobians as RegularImplicitSchurFactor with raw access.
Definition SmartFactorBase.h:356
│ │ │ │ -
gtsam::SmartFactorBase::noiseModel_
SharedIsotropic noiseModel_
As of Feb 22, 2015, the noise model is the same for all measurements and is isotropic.
Definition SmartFactorBase.h:72
│ │ │ │ -
gtsam::SmartFactorBase::Dim
static const int Dim
Camera dimension.
Definition SmartFactorBase.h:60
│ │ │ │ -
gtsam::SmartFactorBase::whitenedError
Vector whitenedError(const Cameras &cameras, const POINT &point) const
Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z], with the noise model ap...
Definition SmartFactorBase.h:251
│ │ │ │ -
gtsam::SmartFactorBase::cameras
virtual Cameras cameras(const Values &values) const
Collect all cameras: important that in key order.
Definition SmartFactorBase.h:162
│ │ │ │ -
gtsam::SmartFactorBase::FillDiagonalF
static void FillDiagonalF(const FBlocks &Fs, Matrix &F)
Create BIG block-diagonal matrix F from Fblocks.
Definition SmartFactorBase.h:400
│ │ │ │ -
gtsam::SmartFactorBase::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, const POINT &point) const
Calculate the error of the factor.
Definition SmartFactorBase.h:267
│ │ │ │ -
gtsam::SmartFactorBase::add
void add(const ZVector &measurements, const KeyVector &cameraKeys)
Add a bunch of measurements, together with the camera keys.
Definition SmartFactorBase.h:136
│ │ │ │ -
gtsam::SmartFactorBase::createHessianFactor
boost::shared_ptr< RegularHessianFactor< Dim > > createHessianFactor(const Cameras &cameras, const Point3 &point, const double lambda=0.0, bool diagonalDamping=false) const
Linearize to a Hessianfactor.
Definition SmartFactorBase.h:316
│ │ │ │ -
gtsam::SmartFactorBase::computeJacobiansSVD
void computeJacobiansSVD(FBlocks &Fs, Matrix &Enull, Vector &b, const Cameras &cameras, const POINT &point) const
SVD version that produces smaller Jacobian matrices by doing an SVD decomposition on E,...
Definition SmartFactorBase.h:300
│ │ │ │ -
gtsam::SmartFactorBase::measured_
ZVector measured_
Measurements for each of the m views.
Definition SmartFactorBase.h:79
│ │ │ │ -
gtsam::SmartFactorBase::shared_ptr
GTSAM_MAKE_ALIGNED_OPERATOR_NEW typedef boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor.
Definition SmartFactorBase.h:91
│ │ │ │ -
gtsam::SmartFactorBase::unwhitenedError
Vector unwhitenedError(const Cameras &cameras, const POINT &point, boost::optional< typename Cameras::FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const
Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and derivatives.
Definition SmartFactorBase.h:204
│ │ │ │ -
gtsam::SmartFactorBase::SmartFactorBase
SmartFactorBase()
Default Constructor, for serialization.
Definition SmartFactorBase.h:97
│ │ │ │ -
gtsam::SmartFactorBase::whitenJacobians
void whitenJacobians(FBlocks &F, Matrix &E, Vector &b) const
Whiten the Jacobians computed by computeJacobians using noiseModel_.
Definition SmartFactorBase.h:347
│ │ │ │ -
gtsam::SmartFactorBase::SmartFactorBase
SmartFactorBase(const SharedNoiseModel &sharedNoiseModel, boost::optional< Pose3 > body_P_sensor=boost::none, size_t expectedNumberCameras=10)
Construct with given noise model and optional arguments.
Definition SmartFactorBase.h:100
│ │ │ │ -
gtsam::SmartFactorBase::add
void add(const SFM_TRACK &trackToAdd)
Add an entire SfM_track (collection of cameras observing a single point).
Definition SmartFactorBase.h:148
│ │ │ │ -
gtsam::SmartFactorBase::measured
const ZVector & measured() const
Return the 2D measurements (ZDim, in general).
Definition SmartFactorBase.h:159
│ │ │ │ -
gtsam::SmartFactorBase::createJacobianSVDFactor
boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0) const
Return Jacobians as JacobianFactorSVD.
Definition SmartFactorBase.h:386
│ │ │ │ -
gtsam::SmartFactorBase::dim
size_t dim() const override
Return the dimension (number of rows!) of the factor.
Definition SmartFactorBase.h:156
│ │ │ │ -
gtsam::SmartFactorBase::createJacobianQFactor
boost::shared_ptr< JacobianFactorQ< Dim, ZDim > > createJacobianQFactor(const Cameras &cameras, const Point3 &point, double lambda=0.0, bool diagonalDamping=false) const
Return Jacobians as JacobianFactorQ.
Definition SmartFactorBase.h:369
│ │ │ │ -
gtsam::SmartFactorBase::access
friend class boost::serialization::access
Serialization function.
Definition SmartFactorBase.h:419
│ │ │ │ -
gtsam::SmartFactorBase::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition SmartFactorBase.h:174
│ │ │ │ -
gtsam::SmartFactorBase::body_P_sensor_
boost::optional< Pose3 > body_P_sensor_
Pose of the camera in the body frame.
Definition SmartFactorBase.h:82
│ │ │ │ -
gtsam::SmartFactorBase::correctForMissingMeasurements
virtual void correctForMissingMeasurements(const Cameras &cameras, Vector &ue, boost::optional< typename Cameras::FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const
This corrects the Jacobians for the case in which some 2D measurement is missing (nan).
Definition SmartFactorBase.h:241
│ │ │ │ -
gtsam::SmartFactorBase::ZDim
static const int ZDim
Measurement dimension.
Definition SmartFactorBase.h:61
│ │ │ │ -
gtsam::SmartFactorBase::PointCov
static Matrix PointCov(const Matrix &E)
Computes Point Covariance P from the "point Jacobian" E.
Definition SmartFactorBase.h:274
│ │ │ │ -
gtsam::SmartFactorBase::Cameras
CameraSet< CAMERA > Cameras
The CameraSet data structure is used to refer to a set of cameras.
Definition SmartFactorBase.h:94
│ │ │ │ -
gtsam::SmartFactorBase::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition SmartFactorBase.h:187
│ │ │ │ +
gtsam::FactorGraph::add
IsDerived< DERIVEDFACTOR > add(boost::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition FactorGraph.h:210
│ │ │ │ +
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Create a unit covariance noise model.
Definition NoiseModel.h:597
│ │ │ │ +
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │ + │ │ │ │ + │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,629 +1,124 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SmartFactorBase.h │ │ │ │ │ +InitializePose.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -22#pragma once │ │ │ │ │ -23 │ │ │ │ │ -24#include │ │ │ │ │ -25#include │ │ │ │ │ -26#include <_g_t_s_a_m_/_s_l_a_m_/_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_._h> │ │ │ │ │ -27 │ │ │ │ │ -28#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -29#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ -30#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_m_e_r_a_S_e_t_._h> │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_S_y_m_b_o_l_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_P_r_i_o_r_F_a_c_t_o_r_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_s_l_a_m_/_B_e_t_w_e_e_n_F_a_c_t_o_r_._h> │ │ │ │ │ +26 │ │ │ │ │ +27namespace _g_t_s_a_m { │ │ │ │ │ +28namespace initialize { │ │ │ │ │ +29 │ │ │ │ │ +30static constexpr _K_e_y kAnchorKey = 99999999; │ │ │ │ │ 31 │ │ │ │ │ -32#include │ │ │ │ │ -33#include │ │ │ │ │ -34#include │ │ │ │ │ -35#include │ │ │ │ │ -36 │ │ │ │ │ -37namespace _g_t_s_a_m { │ │ │ │ │ -38 │ │ │ │ │ -49template │ │ │ │ │ -_5_0class _S_m_a_r_t_F_a_c_t_o_r_B_a_s_e: public _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r { │ │ │ │ │ -51 │ │ │ │ │ -52private: │ │ │ │ │ -53 typedef _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r _B_a_s_e; │ │ │ │ │ -54 typedef _S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_<_C_A_M_E_R_A_> This; │ │ │ │ │ -55 typedef typename CAMERA::Measurement Z; │ │ │ │ │ -56 typedef typename CAMERA::MeasurementVector ZVector; │ │ │ │ │ -57 │ │ │ │ │ -58public: │ │ │ │ │ -59 │ │ │ │ │ -_6_0 static const int _D_i_m = _t_r_a_i_t_s_<_C_A_M_E_R_A_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ -_6_1 static const int _Z_D_i_m = _t_r_a_i_t_s_<_Z_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ -62 typedef Eigen::Matrix MatrixZD; // F blocks (derivatives │ │ │ │ │ -wrpt camera) │ │ │ │ │ -63 typedef std::vector > FBlocks; │ │ │ │ │ -// vector of F blocks │ │ │ │ │ -64 │ │ │ │ │ -65protected: │ │ │ │ │ -_7_2 SharedIsotropic _n_o_i_s_e_M_o_d_e_l__; │ │ │ │ │ -73 │ │ │ │ │ -_7_9 ZVector _m_e_a_s_u_r_e_d__; │ │ │ │ │ -80 │ │ │ │ │ -81 boost::optional │ │ │ │ │ -_8_2 _b_o_d_y___P___s_e_n_s_o_r__; │ │ │ │ │ -83 │ │ │ │ │ -84 // Cache for Fblocks, to avoid a malloc ever time we re-linearize │ │ │ │ │ -85 mutable FBlocks Fs; │ │ │ │ │ +36template │ │ │ │ │ +37static NonlinearFactorGraph buildPoseGraph(const NonlinearFactorGraph& graph) │ │ │ │ │ +{ │ │ │ │ │ +38 NonlinearFactorGraph poseGraph; │ │ │ │ │ +39 │ │ │ │ │ +40 for (const auto& factor : graph) { │ │ │ │ │ +41 // recast to a between on Pose │ │ │ │ │ +42 if (auto between = │ │ │ │ │ +43 boost::dynamic_pointer_cast >(factor)) │ │ │ │ │ +44 poseGraph._a_d_d(between); │ │ │ │ │ +45 │ │ │ │ │ +46 // recast PriorFactor to BetweenFactor │ │ │ │ │ +47 if (auto prior = boost::dynamic_pointer_cast >(factor)) │ │ │ │ │ +48 poseGraph.emplace_shared >( │ │ │ │ │ +49 kAnchorKey, prior->keys()[0], prior->prior(), prior->noiseModel()); │ │ │ │ │ +50 } │ │ │ │ │ +51 return poseGraph; │ │ │ │ │ +52} │ │ │ │ │ +53 │ │ │ │ │ +57template │ │ │ │ │ +58static _V_a_l_u_e_s computePoses(const _V_a_l_u_e_s& initialRot, │ │ │ │ │ +59 NonlinearFactorGraph* posegraph, │ │ │ │ │ +60 bool singleIter = true) { │ │ │ │ │ +61 const auto origin = Pose().translation(); │ │ │ │ │ +62 │ │ │ │ │ +63 // Upgrade rotations to full poses │ │ │ │ │ +64 _V_a_l_u_e_s initialPose; │ │ │ │ │ +65 for (const auto& key_rot : initialRot.extract()) { │ │ │ │ │ +66 const _K_e_y& key = key_rot.first; │ │ │ │ │ +67 const auto& rot = key_rot.second; │ │ │ │ │ +68 const Pose initializedPose(rot, origin); │ │ │ │ │ +69 initialPose.insert(key, initializedPose); │ │ │ │ │ +70 } │ │ │ │ │ +71 │ │ │ │ │ +72 // add prior on dummy node │ │ │ │ │ +73 auto priorModel = _n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_C_r_e_a_t_e(Pose::dimension); │ │ │ │ │ +74 initialPose.insert(kAnchorKey, Pose()); │ │ │ │ │ +75 posegraph->emplace_shared >(kAnchorKey, Pose(), │ │ │ │ │ +priorModel); │ │ │ │ │ +76 │ │ │ │ │ +77 // Create optimizer │ │ │ │ │ +78 GaussNewtonParams params; │ │ │ │ │ +79 if (singleIter) { │ │ │ │ │ +80 params.maxIterations = 1; │ │ │ │ │ +81 } else { │ │ │ │ │ +82 params.setVerbosity("TERMINATION"); │ │ │ │ │ +83 } │ │ │ │ │ +84 GaussNewtonOptimizer optimizer(*posegraph, initialPose, params); │ │ │ │ │ +85 const _V_a_l_u_e_s GNresult = optimizer.optimize(); │ │ │ │ │ 86 │ │ │ │ │ -87 public: │ │ │ │ │ -88 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -89 │ │ │ │ │ -_9_1 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -92 │ │ │ │ │ -_9_4 typedef _C_a_m_e_r_a_S_e_t_<_C_A_M_E_R_A_> _C_a_m_e_r_a_s; │ │ │ │ │ -95 │ │ │ │ │ -_9_7 _S_m_a_r_t_F_a_c_t_o_r_B_a_s_e() {} │ │ │ │ │ -98 │ │ │ │ │ -_1_0_0 _S_m_a_r_t_F_a_c_t_o_r_B_a_s_e(const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& sharedNoiseModel, │ │ │ │ │ -101 boost::optional body_P_sensor = boost::none, │ │ │ │ │ -102 size_t expectedNumberCameras = 10) │ │ │ │ │ -103 : _b_o_d_y___P___s_e_n_s_o_r__(body_P_sensor), Fs(expectedNumberCameras) { │ │ │ │ │ -104 │ │ │ │ │ -105 if (!sharedNoiseModel) │ │ │ │ │ -106 throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required"); │ │ │ │ │ -107 │ │ │ │ │ -108 SharedIsotropic sharedIsotropic = boost::dynamic_pointer_cast< │ │ │ │ │ -109 _n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c>(sharedNoiseModel); │ │ │ │ │ -110 │ │ │ │ │ -111 if (!sharedIsotropic) │ │ │ │ │ -112 throw std::runtime_error("SmartFactorBase: needs isotropic"); │ │ │ │ │ -113 │ │ │ │ │ -114 _n_o_i_s_e_M_o_d_e_l__ = sharedIsotropic; │ │ │ │ │ -115 } │ │ │ │ │ -116 │ │ │ │ │ -_1_1_8 _~_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e() override { │ │ │ │ │ -119 } │ │ │ │ │ -120 │ │ │ │ │ -_1_2_6 void _a_d_d(const Z& _m_e_a_s_u_r_e_d, const _K_e_y& key) { │ │ │ │ │ -127 if(std::find(_k_e_y_s__.begin(), _k_e_y_s__.end(), key) != _k_e_y_s__.end()) { │ │ │ │ │ -128 throw std::invalid_argument( │ │ │ │ │ -129 "SmartFactorBase::add: adding duplicate measurement for key."); │ │ │ │ │ -130 } │ │ │ │ │ -131 this->measured_.push_back(_m_e_a_s_u_r_e_d); │ │ │ │ │ -132 this->_k_e_y_s__.push_back(key); │ │ │ │ │ -133 } │ │ │ │ │ -134 │ │ │ │ │ -_1_3_6 void _a_d_d(const ZVector& measurements, const _K_e_y_V_e_c_t_o_r& cameraKeys) { │ │ │ │ │ -137 assert(measurements.size() == cameraKeys.size()); │ │ │ │ │ -138 for (size_t i = 0; i < measurements.size(); i++) { │ │ │ │ │ -139 this->_a_d_d(measurements[i], cameraKeys[i]); │ │ │ │ │ -140 } │ │ │ │ │ -141 } │ │ │ │ │ -142 │ │ │ │ │ -147 template │ │ │ │ │ -_1_4_8 void _a_d_d(const SFM_TRACK& trackToAdd) { │ │ │ │ │ -149 for (size_t k = 0; k < trackToAdd.numberMeasurements(); k++) { │ │ │ │ │ -150 this->measured_.push_back(trackToAdd.measurements[k].second); │ │ │ │ │ -151 this->_k_e_y_s__.push_back(trackToAdd.measurements[k].first); │ │ │ │ │ -152 } │ │ │ │ │ -153 } │ │ │ │ │ -154 │ │ │ │ │ -_1_5_6 size_t _d_i_m() const override { return _Z_D_i_m * this->measured_.size(); } │ │ │ │ │ -157 │ │ │ │ │ -_1_5_9 const ZVector& _m_e_a_s_u_r_e_d() const { return _m_e_a_s_u_r_e_d__; } │ │ │ │ │ -160 │ │ │ │ │ -_1_6_2 virtual _C_a_m_e_r_a_s _c_a_m_e_r_a_s(const _V_a_l_u_e_s& values) const { │ │ │ │ │ -163 _C_a_m_e_r_a_s _c_a_m_e_r_a_s; │ │ │ │ │ -164 for(const _K_e_y& k: this->_k_e_y_s__) │ │ │ │ │ -165 cameras.push_back(values._a_t(k)); │ │ │ │ │ -166 return _c_a_m_e_r_a_s; │ │ │ │ │ -167 } │ │ │ │ │ -168 │ │ │ │ │ -_1_7_4 void _p_r_i_n_t(const std::string& s = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ -175 DefaultKeyFormatter) const override { │ │ │ │ │ -176 std::cout << s << "SmartFactorBase, z = \n"; │ │ │ │ │ -177 for (size_t k = 0; k < _m_e_a_s_u_r_e_d__.size(); ++k) { │ │ │ │ │ -178 std::cout << "measurement " << k<<", px = \n" << _m_e_a_s_u_r_e_d__[k] << "\n"; │ │ │ │ │ -179 _n_o_i_s_e_M_o_d_e_l__->print("noise model = "); │ │ │ │ │ -180 } │ │ │ │ │ -181 if(_b_o_d_y___P___s_e_n_s_o_r__) │ │ │ │ │ -182 _b_o_d_y___P___s_e_n_s_o_r__->print("body_P_sensor_:\n"); │ │ │ │ │ -183 _B_a_s_e_:_:_p_r_i_n_t("", keyFormatter); │ │ │ │ │ -184 } │ │ │ │ │ -185 │ │ │ │ │ -_1_8_7 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& p, double tol = 1e-9) const override { │ │ │ │ │ -188 if (const This* e = dynamic_cast(&p)) { │ │ │ │ │ -189 // Check that all measurements are the same. │ │ │ │ │ -190 for (size_t i = 0; i < _m_e_a_s_u_r_e_d__.size(); i++) { │ │ │ │ │ -191 if (!_t_r_a_i_t_s_<_Z_>_:_:_E_q_u_a_l_s(this->measured_.at(i), e->measured_.at(i), tol)) │ │ │ │ │ -192 return false; │ │ │ │ │ -193 } │ │ │ │ │ -194 // If so, check base class. │ │ │ │ │ -195 return _B_a_s_e_:_:_e_q_u_a_l_s(p, tol); │ │ │ │ │ -196 } else { │ │ │ │ │ -197 return false; │ │ │ │ │ -198 } │ │ │ │ │ -199 } │ │ │ │ │ -200 │ │ │ │ │ -203 template │ │ │ │ │ -_2_0_4 Vector _u_n_w_h_i_t_e_n_e_d_E_r_r_o_r( │ │ │ │ │ -205 const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, const POINT& point, │ │ │ │ │ -206 boost::optional Fs = boost::none, // │ │ │ │ │ -207 boost::optional E = boost::none) const { │ │ │ │ │ -208 // Reproject, with optional derivatives. │ │ │ │ │ -209 Vector error = _c_a_m_e_r_a_s._r_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r(point, _m_e_a_s_u_r_e_d__, Fs, E); │ │ │ │ │ -210 │ │ │ │ │ -211 // Apply chain rule if body_P_sensor_ is given. │ │ │ │ │ -212 if (_b_o_d_y___P___s_e_n_s_o_r__ && Fs) { │ │ │ │ │ -213 const _P_o_s_e_3 sensor_P_body = _b_o_d_y___P___s_e_n_s_o_r__->_i_n_v_e_r_s_e(); │ │ │ │ │ -214 constexpr int camera_dim = _t_r_a_i_t_s_<_C_A_M_E_R_A_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ -215 constexpr int pose_dim = _t_r_a_i_t_s_<_P_o_s_e_3_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ -216 │ │ │ │ │ -217 for (size_t i = 0; i < Fs->size(); i++) { │ │ │ │ │ -218 const _P_o_s_e_3 world_P_body = _c_a_m_e_r_a_s[i].pose() * sensor_P_body; │ │ │ │ │ -219 Eigen::Matrix J; │ │ │ │ │ -220 J.setZero(); │ │ │ │ │ -221 Eigen::Matrix H; │ │ │ │ │ -222 // Call compose to compute Jacobian for camera extrinsics │ │ │ │ │ -223 world_P_body.compose(*_b_o_d_y___P___s_e_n_s_o_r__, H); │ │ │ │ │ -224 // Assign extrinsics part of the Jacobian │ │ │ │ │ -225 J.template block(0, 0) = H; │ │ │ │ │ -226 Fs->at(i) = Fs->at(i) * J; │ │ │ │ │ -227 } │ │ │ │ │ -228 } │ │ │ │ │ -229 │ │ │ │ │ -230 // Correct the Jacobians in case some measurements are missing. │ │ │ │ │ -231 _c_o_r_r_e_c_t_F_o_r_M_i_s_s_i_n_g_M_e_a_s_u_r_e_m_e_n_t_s(_c_a_m_e_r_a_s, error, Fs, E); │ │ │ │ │ -232 │ │ │ │ │ -233 return error; │ │ │ │ │ -234 } │ │ │ │ │ -235 │ │ │ │ │ -_2_4_1 virtual void _c_o_r_r_e_c_t_F_o_r_M_i_s_s_i_n_g_M_e_a_s_u_r_e_m_e_n_t_s( │ │ │ │ │ -242 const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, Vector& ue, │ │ │ │ │ -243 boost::optional Fs = boost::none, │ │ │ │ │ -244 boost::optional E = boost::none) const {} │ │ │ │ │ -245 │ │ │ │ │ -250 template │ │ │ │ │ -_2_5_1 Vector _w_h_i_t_e_n_e_d_E_r_r_o_r(const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, const POINT& point) const { │ │ │ │ │ -252 Vector error = _c_a_m_e_r_a_s._r_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r(point, _m_e_a_s_u_r_e_d__); │ │ │ │ │ -253 if (_n_o_i_s_e_M_o_d_e_l__) │ │ │ │ │ -254 _n_o_i_s_e_M_o_d_e_l__->whitenInPlace(error); │ │ │ │ │ -255 return error; │ │ │ │ │ -256 } │ │ │ │ │ -257 │ │ │ │ │ -266 template │ │ │ │ │ -_2_6_7 double _t_o_t_a_l_R_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r(const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, │ │ │ │ │ -268 const POINT& point) const { │ │ │ │ │ -269 Vector error = _w_h_i_t_e_n_e_d_E_r_r_o_r(_c_a_m_e_r_a_s, point); │ │ │ │ │ -270 return 0.5 * error.dot(error); │ │ │ │ │ -271 } │ │ │ │ │ -272 │ │ │ │ │ -_2_7_4 static Matrix _P_o_i_n_t_C_o_v(const Matrix& E) { │ │ │ │ │ -275 return (E.transpose() * E).inverse(); │ │ │ │ │ -276 } │ │ │ │ │ -277 │ │ │ │ │ -284 template │ │ │ │ │ -_2_8_5 void _c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s(FBlocks& Fs, Matrix& E, Vector& b, │ │ │ │ │ -286 const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, const POINT& point) const { │ │ │ │ │ -287 // Project into Camera set and calculate derivatives │ │ │ │ │ -288 // As in expressionFactor, RHS vector b = - (h(x_bar) - z) = z-h(x_bar) │ │ │ │ │ -289 // Indeed, nonlinear error |h(x_bar+dx)-z| ~ |h(x_bar) + A*dx - z| │ │ │ │ │ -290 // = |A*dx - (z-h(x_bar))| │ │ │ │ │ -291 b = -_u_n_w_h_i_t_e_n_e_d_E_r_r_o_r(_c_a_m_e_r_a_s, point, Fs, E); │ │ │ │ │ -292 } │ │ │ │ │ -293 │ │ │ │ │ -299 template │ │ │ │ │ -_3_0_0 void _c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s_S_V_D(FBlocks& Fs, Matrix& Enull, │ │ │ │ │ -301 Vector& b, const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, const POINT& point) const { │ │ │ │ │ -302 │ │ │ │ │ -303 Matrix E; │ │ │ │ │ -304 _c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s(Fs, E, b, _c_a_m_e_r_a_s, point); │ │ │ │ │ -305 │ │ │ │ │ -306 static const int N = _F_i_x_e_d_D_i_m_e_n_s_i_o_n_<_P_O_I_N_T_>_:_:_v_a_l_u_e; // 2 (Unit3) or 3 │ │ │ │ │ -(Point3) │ │ │ │ │ -307 │ │ │ │ │ -308 // Do SVD on A. │ │ │ │ │ -309 Eigen::JacobiSVD _s_v_d(E, Eigen::ComputeFullU); │ │ │ │ │ -310 size_t m = this->_k_e_y_s__.size(); │ │ │ │ │ -311 Enull = _s_v_d.matrixU().block(0, N, _Z_D_i_m * m, _Z_D_i_m * m - N); // last ZDim*m- │ │ │ │ │ -N columns │ │ │ │ │ -312 } │ │ │ │ │ -313 │ │ │ │ │ -315 // TODO(dellaert): Not used/tested anywhere and not properly whitened. │ │ │ │ │ -_3_1_6 boost::shared_ptr > _c_r_e_a_t_e_H_e_s_s_i_a_n_F_a_c_t_o_r( │ │ │ │ │ -317 const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, const _P_o_i_n_t_3& point, const double lambda = 0.0, │ │ │ │ │ -318 bool diagonalDamping = false) const { │ │ │ │ │ -319 │ │ │ │ │ -320 Matrix E; │ │ │ │ │ -321 Vector b; │ │ │ │ │ -322 _c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s(Fs, E, b, _c_a_m_e_r_a_s, point); │ │ │ │ │ -323 │ │ │ │ │ -324 // build augmented hessian │ │ │ │ │ -325 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x augmentedHessian = _C_a_m_e_r_a_s_:_:_S_c_h_u_r_C_o_m_p_l_e_m_e_n_t(Fs, E, b); │ │ │ │ │ -326 │ │ │ │ │ -327 return boost::make_shared >(_k_e_y_s__, │ │ │ │ │ -328 augmentedHessian); │ │ │ │ │ -329 } │ │ │ │ │ -330 │ │ │ │ │ -_3_3_6 void _u_p_d_a_t_e_A_u_g_m_e_n_t_e_d_H_e_s_s_i_a_n(const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, const _P_o_i_n_t_3& point, │ │ │ │ │ -337 const double lambda, bool diagonalDamping, │ │ │ │ │ -338 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x& augmentedHessian, │ │ │ │ │ -339 const _K_e_y_V_e_c_t_o_r allKeys) const { │ │ │ │ │ -340 Matrix E; │ │ │ │ │ -341 Vector b; │ │ │ │ │ -342 _c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s(Fs, E, b, _c_a_m_e_r_a_s, point); │ │ │ │ │ -343 _C_a_m_e_r_a_s_:_:_U_p_d_a_t_e_S_c_h_u_r_C_o_m_p_l_e_m_e_n_t(Fs, E, b, allKeys, _k_e_y_s__, augmentedHessian); │ │ │ │ │ -344 } │ │ │ │ │ -345 │ │ │ │ │ -_3_4_7 void _w_h_i_t_e_n_J_a_c_o_b_i_a_n_s(FBlocks& F, Matrix& E, Vector& b) const { │ │ │ │ │ -348 _n_o_i_s_e_M_o_d_e_l__->WhitenSystem(E, b); │ │ │ │ │ -349 // TODO make WhitenInPlace work with any dense matrix type │ │ │ │ │ -350 for (size_t i = 0; i < F.size(); i++) │ │ │ │ │ -351 F[i] = _n_o_i_s_e_M_o_d_e_l__->Whiten(F[i]); │ │ │ │ │ -352 } │ │ │ │ │ -353 │ │ │ │ │ -355 boost::shared_ptr > // │ │ │ │ │ -_3_5_6 _c_r_e_a_t_e_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r(const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, const _P_o_i_n_t_3& │ │ │ │ │ -point, │ │ │ │ │ -357 double lambda = 0.0, bool diagonalDamping = false) const { │ │ │ │ │ -358 Matrix E; │ │ │ │ │ -359 Vector b; │ │ │ │ │ -360 FBlocks F; │ │ │ │ │ -361 _c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s(F, E, b, _c_a_m_e_r_a_s, point); │ │ │ │ │ -362 _w_h_i_t_e_n_J_a_c_o_b_i_a_n_s(F, E, b); │ │ │ │ │ -363 Matrix P = _C_a_m_e_r_a_s_:_:_P_o_i_n_t_C_o_v(E, lambda, diagonalDamping); │ │ │ │ │ -364 return boost::make_shared >(_k_e_y_s__, F, E, │ │ │ │ │ -365 P, b); │ │ │ │ │ -366 } │ │ │ │ │ -367 │ │ │ │ │ -_3_6_9 boost::shared_ptr > _c_r_e_a_t_e_J_a_c_o_b_i_a_n_Q_F_a_c_t_o_r( │ │ │ │ │ -370 const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, const _P_o_i_n_t_3& point, double lambda = 0.0, │ │ │ │ │ -371 bool diagonalDamping = false) const { │ │ │ │ │ -372 Matrix E; │ │ │ │ │ -373 Vector b; │ │ │ │ │ -374 FBlocks F; │ │ │ │ │ -375 _c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s(F, E, b, _c_a_m_e_r_a_s, point); │ │ │ │ │ -376 const size_t M = b.size(); │ │ │ │ │ -377 Matrix P = _C_a_m_e_r_a_s_:_:_P_o_i_n_t_C_o_v(E, lambda, diagonalDamping); │ │ │ │ │ -378 SharedIsotropic n = _n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c_:_:_S_i_g_m_a(M, _n_o_i_s_e_M_o_d_e_l__->sigma()); │ │ │ │ │ -379 return boost::make_shared >(_k_e_y_s__, F, E, P, b, │ │ │ │ │ -n); │ │ │ │ │ -380 } │ │ │ │ │ -381 │ │ │ │ │ -_3_8_6 boost::shared_ptr _c_r_e_a_t_e_J_a_c_o_b_i_a_n_S_V_D_F_a_c_t_o_r( │ │ │ │ │ -387 const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s, const _P_o_i_n_t_3& point, double lambda = 0.0) const { │ │ │ │ │ -388 size_t m = this->_k_e_y_s__.size(); │ │ │ │ │ -389 FBlocks F; │ │ │ │ │ -390 Vector b; │ │ │ │ │ -391 const size_t M = _Z_D_i_m * m; │ │ │ │ │ -392 Matrix E0(M, M - 3); │ │ │ │ │ -393 _c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s_S_V_D(F, E0, b, _c_a_m_e_r_a_s, point); │ │ │ │ │ -394 SharedIsotropic n = _n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c_:_:_S_i_g_m_a(M - 3, │ │ │ │ │ -395 _n_o_i_s_e_M_o_d_e_l__->sigma()); │ │ │ │ │ -396 return boost::make_shared >(_k_e_y_s__, F, E0, b, │ │ │ │ │ -n); │ │ │ │ │ -397 } │ │ │ │ │ -398 │ │ │ │ │ -_4_0_0 static void _F_i_l_l_D_i_a_g_o_n_a_l_F(const FBlocks& Fs, Matrix& F) { │ │ │ │ │ -401 size_t m = Fs.size(); │ │ │ │ │ -402 F.resize(_Z_D_i_m * m, _D_i_m * m); │ │ │ │ │ -403 F.setZero(); │ │ │ │ │ -404 for (size_t i = 0; i < m; ++i) │ │ │ │ │ -405 F.block<_Z_D_i_m, _D_i_m>(_Z_D_i_m * i, _D_i_m * i) = Fs.at(i); │ │ │ │ │ -406 } │ │ │ │ │ -407 │ │ │ │ │ -408 // Return sensor pose. │ │ │ │ │ -409 _P_o_s_e_3 body_P_sensor() const{ │ │ │ │ │ -410 if(_b_o_d_y___P___s_e_n_s_o_r__) │ │ │ │ │ -411 return *_b_o_d_y___P___s_e_n_s_o_r__; │ │ │ │ │ -412 else │ │ │ │ │ -413 return _P_o_s_e_3(); // if unspecified, the transformation is the identity │ │ │ │ │ -414 } │ │ │ │ │ -415 │ │ │ │ │ -416private: │ │ │ │ │ -417 │ │ │ │ │ -_4_1_9 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -420 template │ │ │ │ │ -421 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -422 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -423 ar & BOOST_SERIALIZATION_NVP(_n_o_i_s_e_M_o_d_e_l__); │ │ │ │ │ -424 ar & BOOST_SERIALIZATION_NVP(_m_e_a_s_u_r_e_d__); │ │ │ │ │ -425 ar & BOOST_SERIALIZATION_NVP(_b_o_d_y___P___s_e_n_s_o_r__); │ │ │ │ │ -426 } │ │ │ │ │ -427}; │ │ │ │ │ -428// end class SmartFactorBase │ │ │ │ │ -429 │ │ │ │ │ -430// Definitions need to avoid link errors (above are only declarations) │ │ │ │ │ -431template const int _S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_<_C_A_M_E_R_A_>_:_:_D_i_m; │ │ │ │ │ -432template const int _S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_<_C_A_M_E_R_A_>_:_:_Z_D_i_m; │ │ │ │ │ -433 │ │ │ │ │ -434} // \ namespace gtsam │ │ │ │ │ -_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ -This marks a GTSAM object to require alignment. │ │ │ │ │ -DDeeffiinniittiioonn types.h:308 │ │ │ │ │ -_C_a_m_e_r_a_S_e_t_._h │ │ │ │ │ -Base class to create smart factors on poses or cameras. │ │ │ │ │ -_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ -HessianFactor class with constant sized blocks. │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ -Non-linear factor base classes. │ │ │ │ │ -_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_._h │ │ │ │ │ -A subclass of GaussianFactor specialized to structureless SFM. │ │ │ │ │ +87 // put into Values structure │ │ │ │ │ +88 _V_a_l_u_e_s estimate; │ │ │ │ │ +89 for (const auto& key_pose : GNresult.extract()) { │ │ │ │ │ +90 const _K_e_y& key = key_pose.first; │ │ │ │ │ +91 if (key != kAnchorKey) { │ │ │ │ │ +92 const Pose& pose = key_pose.second; │ │ │ │ │ +93 estimate.insert(key, pose); │ │ │ │ │ +94 } │ │ │ │ │ +95 } │ │ │ │ │ +96 return estimate; │ │ │ │ │ +97} │ │ │ │ │ +98} // namespace initialize │ │ │ │ │ +99} // namespace gtsam │ │ │ │ │ +_G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Factor Graph consisting of non-linear factors. │ │ │ │ │ +_B_e_t_w_e_e_n_F_a_c_t_o_r_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_s_v_d │ │ │ │ │ -void svd(const Matrix &A, Matrix &U, Vector &S, Matrix &V) │ │ │ │ │ -SVD computes economy SVD A=U*S*V'. │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:560 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ -Vector3 Point3 │ │ │ │ │ -As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point3 to Vector3... │ │ │ │ │ -DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ -Aliases. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_F_i_x_e_d_D_i_m_e_n_s_i_o_n │ │ │ │ │ -Give fixed size dimension of a type, fails at compile time if dynamic. │ │ │ │ │ -DDeeffiinniittiioonn Manifold.h:164 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ -of blocks. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t │ │ │ │ │ -A set of cameras, all with their own calibration. │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_r_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r │ │ │ │ │ -Vector reprojectionError(const POINT &point, const ZVector &measured, boost:: │ │ │ │ │ -optional< FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost:: │ │ │ │ │ -none) const │ │ │ │ │ -Calculate vector [project2(point)-z] of re-projection errors. │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:136 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_P_o_i_n_t_C_o_v │ │ │ │ │ -static Matrix PointCov(const Matrix &E, const double lambda=0.0, bool │ │ │ │ │ -diagonalDamping=false) │ │ │ │ │ -Computes Point Covariance P, with lambda parameter, dynamic version. │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:331 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_U_p_d_a_t_e_S_c_h_u_r_C_o_m_p_l_e_m_e_n_t │ │ │ │ │ -static void UpdateSchurComplement(const FBlocks &Fs, const Matrix &E, const │ │ │ │ │ -Eigen::Matrix< double, N, N > &P, const Vector &b, const KeyVector &allKeys, │ │ │ │ │ -const KeyVector &keys, SymmetricBlockMatrix &augmentedHessian) │ │ │ │ │ -Applies Schur complement (exploiting block structure) to get a smart factor on │ │ │ │ │ -cameras,... │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:369 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_S_c_h_u_r_C_o_m_p_l_e_m_e_n_t │ │ │ │ │ -static SymmetricBlockMatrix SchurComplement(const std::vector< Eigen::Matrix< │ │ │ │ │ -double, ZDim, ND >, Eigen::aligned_allocator< Eigen::Matrix< double, ZDim, ND > │ │ │ │ │ -> > &Fs, const Matrix &E, const Eigen::Matrix< double, N, N > &P, const Vector │ │ │ │ │ -&b) │ │ │ │ │ -Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix G = │ │ │ │ │ -F' * F - F' * E * P * ... │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:150 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ -A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_i_n_v_e_r_s_e │ │ │ │ │ -Pose3 inverse() const │ │ │ │ │ -inverse transformation with derivatives │ │ │ │ │ -DDeeffiinniittiioonn Pose3.cpp:49 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s__ │ │ │ │ │ -KeyVector keys_ │ │ │ │ │ -The keys involved in this factor. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c │ │ │ │ │ -An isotropic noise model corresponds to a scaled diagonal covariance To │ │ │ │ │ -construct,... │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:516 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c_:_:_S_i_g_m_a │ │ │ │ │ -static shared_ptr Sigma(size_t dim, double sigma, bool smart=true) │ │ │ │ │ -An isotropic noise model created by specifying a standard devation sigma. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.cpp:597 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ -Nonlinear factor base class. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ -virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const │ │ │ │ │ -Check if two factors are equal. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.cpp:47 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.cpp:37 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_a_t │ │ │ │ │ -const ValueType at(Key j) const │ │ │ │ │ -Retrieve a variable by key j. │ │ │ │ │ -DDeeffiinniittiioonn Values-inl.h:361 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e │ │ │ │ │ -Base class for smart factors. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s │ │ │ │ │ -void computeJacobians(FBlocks &Fs, Matrix &E, Vector &b, const Cameras │ │ │ │ │ -&cameras, const POINT &point) const │ │ │ │ │ -Compute F, E, and b (called below in both vanilla and SVD versions), where F is │ │ │ │ │ -a vector of derivativ... │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:285 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_a_d_d │ │ │ │ │ -void add(const Z &measured, const Key &key) │ │ │ │ │ -Add a new measurement and pose/camera key. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:126 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_~_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e │ │ │ │ │ -~SmartFactorBase() override │ │ │ │ │ -Virtual destructor, subclasses from NonlinearFactor. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:118 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_u_p_d_a_t_e_A_u_g_m_e_n_t_e_d_H_e_s_s_i_a_n │ │ │ │ │ -void updateAugmentedHessian(const Cameras &cameras, const Point3 &point, const │ │ │ │ │ -double lambda, bool diagonalDamping, SymmetricBlockMatrix &augmentedHessian, │ │ │ │ │ -const KeyVector allKeys) const │ │ │ │ │ -Add the contribution of the smart factor to a pre-allocated Hessian, using │ │ │ │ │ -sparse linear algebra. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:336 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_c_r_e_a_t_e_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r │ │ │ │ │ -boost::shared_ptr< RegularImplicitSchurFactor< CAMERA > > │ │ │ │ │ -createRegularImplicitSchurFactor(const Cameras &cameras, const Point3 &point, │ │ │ │ │ -double lambda=0.0, bool diagonalDamping=false) const │ │ │ │ │ -Return Jacobians as RegularImplicitSchurFactor with raw access. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:356 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_n_o_i_s_e_M_o_d_e_l__ │ │ │ │ │ -SharedIsotropic noiseModel_ │ │ │ │ │ -As of Feb 22, 2015, the noise model is the same for all measurements and is │ │ │ │ │ -isotropic. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:72 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_D_i_m │ │ │ │ │ -static const int Dim │ │ │ │ │ -Camera dimension. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:60 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_w_h_i_t_e_n_e_d_E_r_r_o_r │ │ │ │ │ -Vector whitenedError(const Cameras &cameras, const POINT &point) const │ │ │ │ │ -Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z], │ │ │ │ │ -with the noise model ap... │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:251 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_c_a_m_e_r_a_s │ │ │ │ │ -virtual Cameras cameras(const Values &values) const │ │ │ │ │ -Collect all cameras: important that in key order. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:162 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_F_i_l_l_D_i_a_g_o_n_a_l_F │ │ │ │ │ -static void FillDiagonalF(const FBlocks &Fs, Matrix &F) │ │ │ │ │ -Create BIG block-diagonal matrix F from Fblocks. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:400 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_t_o_t_a_l_R_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r │ │ │ │ │ -double totalReprojectionError(const Cameras &cameras, const POINT &point) const │ │ │ │ │ -Calculate the error of the factor. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:267 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_a_d_d │ │ │ │ │ -void add(const ZVector &measurements, const KeyVector &cameraKeys) │ │ │ │ │ -Add a bunch of measurements, together with the camera keys. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:136 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_c_r_e_a_t_e_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -boost::shared_ptr< RegularHessianFactor< Dim > > createHessianFactor(const │ │ │ │ │ -Cameras &cameras, const Point3 &point, const double lambda=0.0, bool │ │ │ │ │ -diagonalDamping=false) const │ │ │ │ │ -Linearize to a Hessianfactor. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:316 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s_S_V_D │ │ │ │ │ -void computeJacobiansSVD(FBlocks &Fs, Matrix &Enull, Vector &b, const Cameras │ │ │ │ │ -&cameras, const POINT &point) const │ │ │ │ │ -SVD version that produces smaller Jacobian matrices by doing an SVD │ │ │ │ │ -decomposition on E,... │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:300 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_m_e_a_s_u_r_e_d__ │ │ │ │ │ -ZVector measured_ │ │ │ │ │ -Measurements for each of the m views. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:79 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -GTSAM_MAKE_ALIGNED_OPERATOR_NEW typedef boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shorthand for a smart pointer to a factor. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:91 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_u_n_w_h_i_t_e_n_e_d_E_r_r_o_r │ │ │ │ │ -Vector unwhitenedError(const Cameras &cameras, const POINT &point, boost:: │ │ │ │ │ -optional< typename Cameras::FBlocks & > Fs=boost::none, boost::optional< Matrix │ │ │ │ │ -& > E=boost::none) const │ │ │ │ │ -Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and derivatives. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:204 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e │ │ │ │ │ -SmartFactorBase() │ │ │ │ │ -Default Constructor, for serialization. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:97 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_w_h_i_t_e_n_J_a_c_o_b_i_a_n_s │ │ │ │ │ -void whitenJacobians(FBlocks &F, Matrix &E, Vector &b) const │ │ │ │ │ -Whiten the Jacobians computed by computeJacobians using noiseModel_. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:347 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e │ │ │ │ │ -SmartFactorBase(const SharedNoiseModel &sharedNoiseModel, boost::optional< │ │ │ │ │ -Pose3 > body_P_sensor=boost::none, size_t expectedNumberCameras=10) │ │ │ │ │ -Construct with given noise model and optional arguments. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_a_d_d │ │ │ │ │ -void add(const SFM_TRACK &trackToAdd) │ │ │ │ │ -Add an entire SfM_track (collection of cameras observing a single point). │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:148 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_m_e_a_s_u_r_e_d │ │ │ │ │ -const ZVector & measured() const │ │ │ │ │ -Return the 2D measurements (ZDim, in general). │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:159 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_c_r_e_a_t_e_J_a_c_o_b_i_a_n_S_V_D_F_a_c_t_o_r │ │ │ │ │ -boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras │ │ │ │ │ -&cameras, const Point3 &point, double lambda=0.0) const │ │ │ │ │ -Return Jacobians as JacobianFactorSVD. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:386 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_d_i_m │ │ │ │ │ -size_t dim() const override │ │ │ │ │ -Return the dimension (number of rows!) of the factor. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:156 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_c_r_e_a_t_e_J_a_c_o_b_i_a_n_Q_F_a_c_t_o_r │ │ │ │ │ -boost::shared_ptr< JacobianFactorQ< Dim, ZDim > > createJacobianQFactor(const │ │ │ │ │ -Cameras &cameras, const Point3 &point, double lambda=0.0, bool │ │ │ │ │ -diagonalDamping=false) const │ │ │ │ │ -Return Jacobians as JacobianFactorQ. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:369 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:419 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:174 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_b_o_d_y___P___s_e_n_s_o_r__ │ │ │ │ │ -boost::optional< Pose3 > body_P_sensor_ │ │ │ │ │ -Pose of the camera in the body frame. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:82 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_c_o_r_r_e_c_t_F_o_r_M_i_s_s_i_n_g_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ -virtual void correctForMissingMeasurements(const Cameras &cameras, Vector &ue, │ │ │ │ │ -boost::optional< typename Cameras::FBlocks & > Fs=boost::none, boost::optional< │ │ │ │ │ -Matrix & > E=boost::none) const │ │ │ │ │ -This corrects the Jacobians for the case in which some 2D measurement is │ │ │ │ │ -missing (nan). │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:241 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_Z_D_i_m │ │ │ │ │ -static const int ZDim │ │ │ │ │ -Measurement dimension. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:61 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_P_o_i_n_t_C_o_v │ │ │ │ │ -static Matrix PointCov(const Matrix &E) │ │ │ │ │ -Computes Point Covariance P from the "point Jacobian" E. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:274 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_C_a_m_e_r_a_s │ │ │ │ │ -CameraSet< CAMERA > Cameras │ │ │ │ │ -The CameraSet data structure is used to refer to a set of cameras. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:94 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const NonlinearFactor &p, double tol=1e-9) const override │ │ │ │ │ -equals │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:187 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d │ │ │ │ │ +IsDerived< DERIVEDFACTOR > add(boost::shared_ptr< DERIVEDFACTOR > factor) │ │ │ │ │ +add is a synonym for push_back. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:210 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_C_r_e_a_t_e │ │ │ │ │ +static shared_ptr Create(size_t dim) │ │ │ │ │ +Create a unit covariance noise model. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:597 │ │ │ │ │ +_V_a_l_u_e_s │ │ │ │ │ +In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ +a non-linear function... │ │ │ │ │ +_S_y_m_b_o_l_._h │ │ │ │ │ +_P_r_i_o_r_F_a_c_t_o_r_._h │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_._h │ │ │ │ │ + * _I_n_i_t_i_a_l_i_z_e_P_o_s_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01271_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/EssentialMatrixFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/InitializePose3.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,407 +98,97 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
EssentialMatrixFactor.h
│ │ │ │ +
InitializePose3.h
│ │ │ │
│ │ │ │
│ │ │ │ -
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010-2014, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
12/*
│ │ │ │ -
13 * @file EssentialMatrixFactor.h
│ │ │ │ -
14 * @brief EssentialMatrixFactor class
│ │ │ │ -
15 * @author Frank Dellaert
│ │ │ │ -
16 * @author Ayush Baid
│ │ │ │ -
17 * @author Akshay Krishnan
│ │ │ │ -
18 * @date December 17, 2013
│ │ │ │ -
19 */
│ │ │ │ -
20
│ │ │ │
21#pragma once
│ │ │ │
22
│ │ │ │ -
23#include <gtsam/geometry/EssentialMatrix.h>
│ │ │ │ -
24#include <gtsam/geometry/PinholeCamera.h>
│ │ │ │ -
25#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
26
│ │ │ │ -
27#include <iostream>
│ │ │ │ +
23#include <gtsam/geometry/Rot3.h>
│ │ │ │ +
24#include <gtsam/inference/graph.h>
│ │ │ │ +
25#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ +
26#include <gtsam/linear/VectorValues.h>
│ │ │ │ +
27#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │
28
│ │ │ │ -
29namespace gtsam {
│ │ │ │ -
30
│ │ │ │ -
│ │ │ │ -
34class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
│ │ │ │ -
35 Vector3 vA_, vB_;
│ │ │ │ +
29#include <map>
│ │ │ │ +
30#include <vector>
│ │ │ │ +
31
│ │ │ │ +
32namespace gtsam {
│ │ │ │ +
33
│ │ │ │ +
34typedef std::map<Key, std::vector<size_t> > KeyVectorMap;
│ │ │ │ +
35typedef std::map<Key, Rot3> KeyRotMap;
│ │ │ │
36
│ │ │ │ -
37 typedef NoiseModelFactorN<EssentialMatrix> Base;
│ │ │ │ -
38 typedef EssentialMatrixFactor This;
│ │ │ │ -
39
│ │ │ │ -
40 public:
│ │ │ │ -
│ │ │ │ -
49 EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB,
│ │ │ │ -
50 const SharedNoiseModel& model)
│ │ │ │ -
51 : Base(model, key) {
│ │ │ │ -
52 vA_ = EssentialMatrix::Homogeneous(pA);
│ │ │ │ -
53 vB_ = EssentialMatrix::Homogeneous(pB);
│ │ │ │ -
54 }
│ │ │ │ -
│ │ │ │ +
│ │ │ │ +
37struct GTSAM_EXPORT InitializePose3 {
│ │ │ │ +
38 static GaussianFactorGraph buildLinearOrientationGraph(
│ │ │ │ +
39 const NonlinearFactorGraph& g);
│ │ │ │ +
40
│ │ │ │ +
41 static Values normalizeRelaxedRotations(const VectorValues& relaxedRot3);
│ │ │ │ +
42
│ │ │ │ +
46 static Values computeOrientationsChordal(
│ │ │ │ +
47 const NonlinearFactorGraph& pose3Graph);
│ │ │ │ +
48
│ │ │ │ +
52 static Values computeOrientationsGradient(
│ │ │ │ +
53 const NonlinearFactorGraph& pose3Graph, const Values& givenGuess,
│ │ │ │ +
54 size_t maxIter = 10000, const bool setRefFrame = true);
│ │ │ │
55
│ │ │ │ -
65 template <class CALIBRATION>
│ │ │ │ -
│ │ │ │ -
66 EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB,
│ │ │ │ -
67 const SharedNoiseModel& model,
│ │ │ │ -
68 boost::shared_ptr<CALIBRATION> K)
│ │ │ │ -
69 : Base(model, key) {
│ │ │ │ -
70 assert(K);
│ │ │ │ -
71 vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA));
│ │ │ │ -
72 vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB));
│ │ │ │ -
73 }
│ │ │ │ -
│ │ │ │ -
74
│ │ │ │ -
│ │ │ │ -
76 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
77 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ -
78 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
│ │ │ │ -
79 }
│ │ │ │ -
│ │ │ │ -
80
│ │ │ │ -
│ │ │ │ -
82 void print(
│ │ │ │ -
83 const std::string& s = "",
│ │ │ │ -
84 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ -
85 Base::print(s);
│ │ │ │ -
86 std::cout << " EssentialMatrixFactor with measurements\n ("
│ │ │ │ -
87 << vA_.transpose() << ")' and (" << vB_.transpose() << ")'"
│ │ │ │ -
88 << std::endl;
│ │ │ │ -
89 }
│ │ │ │ -
│ │ │ │ +
56 static void createSymbolicGraph(const NonlinearFactorGraph& pose3Graph,
│ │ │ │ +
57 KeyVectorMap* adjEdgesMap,
│ │ │ │ +
58 KeyRotMap* factorId2RotMap);
│ │ │ │ +
59
│ │ │ │ +
60 static Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a,
│ │ │ │ +
61 const double b);
│ │ │ │ +
62
│ │ │ │ +
67 static NonlinearFactorGraph buildPose3graph(
│ │ │ │ +
68 const NonlinearFactorGraph& graph);
│ │ │ │ +
69
│ │ │ │ +
73 static Values computePoses(const Values& initialRot,
│ │ │ │ +
74 NonlinearFactorGraph* poseGraph,
│ │ │ │ +
75 bool singleIter = true);
│ │ │ │ +
76
│ │ │ │ +
81 static Values initializeOrientations(const NonlinearFactorGraph& graph);
│ │ │ │ +
82
│ │ │ │ +
88 static Values initialize(const NonlinearFactorGraph& graph,
│ │ │ │ +
89 const Values& givenGuess, bool useGradient = false);
│ │ │ │
90
│ │ │ │ -
│ │ │ │ -
92 Vector evaluateError(
│ │ │ │ -
93 const EssentialMatrix& E,
│ │ │ │ -
94 boost::optional<Matrix&> H = boost::none) const override {
│ │ │ │ -
95 Vector error(1);
│ │ │ │ -
96 error << E.error(vA_, vB_, H);
│ │ │ │ -
97 return error;
│ │ │ │ -
98 }
│ │ │ │ -
│ │ │ │ -
99
│ │ │ │ -
100 public:
│ │ │ │ -
101 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
102};
│ │ │ │ -
│ │ │ │ -
103
│ │ │ │ -
│ │ │ │ -
108class EssentialMatrixFactor2
│ │ │ │ -
109 : public NoiseModelFactorN<EssentialMatrix, double> {
│ │ │ │ -
110 Point3 dP1_;
│ │ │ │ -
111 Point2 pn_;
│ │ │ │ -
112 double f_;
│ │ │ │ -
113
│ │ │ │ -
114 typedef NoiseModelFactorN<EssentialMatrix, double> Base;
│ │ │ │ -
115 typedef EssentialMatrixFactor2 This;
│ │ │ │ -
116
│ │ │ │ -
117 public:
│ │ │ │ -
│ │ │ │ -
126 EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
│ │ │ │ -
127 const SharedNoiseModel& model)
│ │ │ │ -
128 : Base(model, key1, key2),
│ │ │ │ -
129 dP1_(EssentialMatrix::Homogeneous(pA)),
│ │ │ │ -
130 pn_(pB) {
│ │ │ │ -
131 f_ = 1.0;
│ │ │ │ -
132 }
│ │ │ │ -
│ │ │ │ -
133
│ │ │ │ -
143 template <class CALIBRATION>
│ │ │ │ -
│ │ │ │ -
144 EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
│ │ │ │ -
145 const SharedNoiseModel& model,
│ │ │ │ -
146 boost::shared_ptr<CALIBRATION> K)
│ │ │ │ -
147 : Base(model, key1, key2),
│ │ │ │ -
148 dP1_(EssentialMatrix::Homogeneous(K->calibrate(pA))),
│ │ │ │ -
149 pn_(K->calibrate(pB)) {
│ │ │ │ -
150 f_ = 0.5 * (K->fx() + K->fy());
│ │ │ │ -
151 }
│ │ │ │ -
│ │ │ │ -
152
│ │ │ │ -
│ │ │ │ -
154 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
155 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ -
156 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
│ │ │ │ -
157 }
│ │ │ │ -
│ │ │ │ -
158
│ │ │ │ -
│ │ │ │ -
160 void print(
│ │ │ │ -
161 const std::string& s = "",
│ │ │ │ -
162 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ -
163 Base::print(s);
│ │ │ │ -
164 std::cout << " EssentialMatrixFactor2 with measurements\n ("
│ │ │ │ -
165 << dP1_.transpose() << ")' and (" << pn_.transpose() << ")'"
│ │ │ │ -
166 << std::endl;
│ │ │ │ -
167 }
│ │ │ │ -
│ │ │ │ -
168
│ │ │ │ -
169 /*
│ │ │ │ -
170 * Vector of errors returns 2D vector
│ │ │ │ -
171 * @param E essential matrix
│ │ │ │ -
172 * @param d inverse depth d
│ │ │ │ -
173 */
│ │ │ │ -
174 Vector evaluateError(
│ │ │ │ -
175 const EssentialMatrix& E, const double& d,
│ │ │ │ -
176 boost::optional<Matrix&> DE = boost::none,
│ │ │ │ -
177 boost::optional<Matrix&> Dd = boost::none) const override {
│ │ │ │ -
178 // We have point x,y in image 1
│ │ │ │ -
179 // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d
│ │ │ │ -
180 // We then convert to second camera by P2 = 1R2'*(P1-1T2)
│ │ │ │ -
181 // The homogeneous coordinates of can be written as
│ │ │ │ -
182 // 2R1*(P1-1T2) == 2R1*d*(P1-1T2) == 2R1*((x,y,1)-d*1T2)
│ │ │ │ -
183 // where we multiplied with d which yields equivalent homogeneous
│ │ │ │ -
184 // coordinates. Note that this is just the homography 2R1 for d==0 The point
│ │ │ │ -
185 // d*P1 = (x,y,1) is computed in constructor as dP1_
│ │ │ │ -
186
│ │ │ │ -
187 // Project to normalized image coordinates, then uncalibrate
│ │ │ │ -
188 Point2 pn(0, 0);
│ │ │ │ -
189 if (!DE && !Dd) {
│ │ │ │ -
190 Point3 _1T2 = E.direction().point3();
│ │ │ │ -
191 Point3 d1T2 = d * _1T2;
│ │ │ │ -
192 Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2); // 2R1*((x,y,1)-d*1T2)
│ │ │ │ -
193 pn = PinholeBase::Project(dP2);
│ │ │ │ -
194
│ │ │ │ -
195 } else {
│ │ │ │ -
196 // Calculate derivatives. TODO if slow: optimize with Mathematica
│ │ │ │ -
197 // 3*2 3*3 3*3
│ │ │ │ -
198 Matrix D_1T2_dir, DdP2_rot, DP2_point;
│ │ │ │ -
199
│ │ │ │ -
200 Point3 _1T2 = E.direction().point3(D_1T2_dir);
│ │ │ │ -
201 Point3 d1T2 = d * _1T2;
│ │ │ │ -
202 Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point);
│ │ │ │ -
203
│ │ │ │ -
204 Matrix23 Dpn_dP2;
│ │ │ │ -
205 pn = PinholeBase::Project(dP2, Dpn_dP2);
│ │ │ │ -
206
│ │ │ │ -
207 if (DE) {
│ │ │ │ -
208 Matrix DdP2_E(3, 5);
│ │ │ │ -
209 DdP2_E << DdP2_rot, -DP2_point * d * D_1T2_dir; // (3*3), (3*3) * (3*2)
│ │ │ │ -
210 *DE = f_ * Dpn_dP2 * DdP2_E; // (2*3) * (3*5)
│ │ │ │ -
211 }
│ │ │ │ -
212
│ │ │ │ -
213 if (Dd) // efficient backwards computation:
│ │ │ │ -
214 // (2*3) * (3*3) * (3*1)
│ │ │ │ -
215 *Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2));
│ │ │ │ -
216 }
│ │ │ │ -
217 Point2 reprojectionError = pn - pn_;
│ │ │ │ -
218 return f_ * reprojectionError;
│ │ │ │ -
219 }
│ │ │ │ -
220
│ │ │ │ -
221 public:
│ │ │ │ -
222 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
223};
│ │ │ │ -
│ │ │ │ -
224// EssentialMatrixFactor2
│ │ │ │ -
225
│ │ │ │ -
│ │ │ │ -
231class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
│ │ │ │ -
232 typedef EssentialMatrixFactor2 Base;
│ │ │ │ -
233 typedef EssentialMatrixFactor3 This;
│ │ │ │ -
234
│ │ │ │ -
235 Rot3 cRb_;
│ │ │ │ -
236
│ │ │ │ -
237 public:
│ │ │ │ -
│ │ │ │ -
247 EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB,
│ │ │ │ -
248 const Rot3& cRb, const SharedNoiseModel& model)
│ │ │ │ -
249 : EssentialMatrixFactor2(key1, key2, pA, pB, model), cRb_(cRb) {}
│ │ │ │ -
│ │ │ │ -
250
│ │ │ │ -
260 template <class CALIBRATION>
│ │ │ │ -
│ │ │ │ -
261 EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB,
│ │ │ │ -
262 const Rot3& cRb, const SharedNoiseModel& model,
│ │ │ │ -
263 boost::shared_ptr<CALIBRATION> K)
│ │ │ │ -
264 : EssentialMatrixFactor2(key1, key2, pA, pB, model, K), cRb_(cRb) {}
│ │ │ │ -
│ │ │ │ -
265
│ │ │ │ -
│ │ │ │ -
267 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
268 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ -
269 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
│ │ │ │ -
270 }
│ │ │ │ -
│ │ │ │ -
271
│ │ │ │ -
│ │ │ │ -
273 void print(
│ │ │ │ -
274 const std::string& s = "",
│ │ │ │ -
275 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ -
276 Base::print(s);
│ │ │ │ -
277 std::cout << " EssentialMatrixFactor3 with rotation " << cRb_ << std::endl;
│ │ │ │ -
278 }
│ │ │ │ -
│ │ │ │ -
279
│ │ │ │ -
280 /*
│ │ │ │ -
281 * Vector of errors returns 2D vector
│ │ │ │ -
282 * @param E essential matrix
│ │ │ │ -
283 * @param d inverse depth d
│ │ │ │ -
284 */
│ │ │ │ -
285 Vector evaluateError(
│ │ │ │ -
286 const EssentialMatrix& E, const double& d,
│ │ │ │ -
287 boost::optional<Matrix&> DE = boost::none,
│ │ │ │ -
288 boost::optional<Matrix&> Dd = boost::none) const override {
│ │ │ │ -
289 if (!DE) {
│ │ │ │ -
290 // Convert E from body to camera frame
│ │ │ │ -
291 EssentialMatrix cameraE = cRb_ * E;
│ │ │ │ -
292 // Evaluate error
│ │ │ │ -
293 return Base::evaluateError(cameraE, d, boost::none, Dd);
│ │ │ │ -
294 } else {
│ │ │ │ -
295 // Version with derivatives
│ │ │ │ -
296 Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5
│ │ │ │ -
297 EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E);
│ │ │ │ -
298 Vector e = Base::evaluateError(cameraE, d, D_e_cameraE, Dd);
│ │ │ │ -
299 *DE = D_e_cameraE * D_cameraE_E; // (2*5) * (5*5)
│ │ │ │ -
300 return e;
│ │ │ │ -
301 }
│ │ │ │ -
302 }
│ │ │ │ -
303
│ │ │ │ -
304 public:
│ │ │ │ -
305 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
306};
│ │ │ │ -
│ │ │ │ -
307// EssentialMatrixFactor3
│ │ │ │ -
308
│ │ │ │ -
322template <class CALIBRATION>
│ │ │ │ -
│ │ │ │ -
323class EssentialMatrixFactor4
│ │ │ │ -
324 : public NoiseModelFactorN<EssentialMatrix, CALIBRATION> {
│ │ │ │ -
325 private:
│ │ │ │ -
326 Point2 pA_, pB_;
│ │ │ │ -
327
│ │ │ │ -
328 typedef NoiseModelFactorN<EssentialMatrix, CALIBRATION> Base;
│ │ │ │ -
329 typedef EssentialMatrixFactor4 This;
│ │ │ │ -
330
│ │ │ │ -
331 static constexpr int DimK = FixedDimension<CALIBRATION>::value;
│ │ │ │ -
332 typedef Eigen::Matrix<double, 2, DimK> JacobianCalibration;
│ │ │ │ -
333
│ │ │ │ -
334 public:
│ │ │ │ -
│ │ │ │ -
344 EssentialMatrixFactor4(Key keyE, Key keyK, const Point2& pA, const Point2& pB,
│ │ │ │ -
345 const SharedNoiseModel& model)
│ │ │ │ -
346 : Base(model, keyE, keyK), pA_(pA), pB_(pB) {}
│ │ │ │ -
│ │ │ │ -
347
│ │ │ │ -
│ │ │ │ -
349 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
350 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ -
351 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
│ │ │ │ -
352 }
│ │ │ │ -
│ │ │ │ -
353
│ │ │ │ -
│ │ │ │ -
355 void print(
│ │ │ │ -
356 const std::string& s = "",
│ │ │ │ -
357 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ -
358 Base::print(s);
│ │ │ │ -
359 std::cout << " EssentialMatrixFactor4 with measurements\n ("
│ │ │ │ -
360 << pA_.transpose() << ")' and (" << pB_.transpose() << ")'"
│ │ │ │ -
361 << std::endl;
│ │ │ │ -
362 }
│ │ │ │ -
│ │ │ │ -
363
│ │ │ │ -
│ │ │ │ -
373 Vector evaluateError(
│ │ │ │ -
374 const EssentialMatrix& E, const CALIBRATION& K,
│ │ │ │ -
375 boost::optional<Matrix&> H1 = boost::none,
│ │ │ │ -
376 boost::optional<Matrix&> H2 = boost::none) const override {
│ │ │ │ -
377 // converting from pixel coordinates to normalized coordinates cA and cB
│ │ │ │ -
378 JacobianCalibration cA_H_K; // dcA/dK
│ │ │ │ -
379 JacobianCalibration cB_H_K; // dcB/dK
│ │ │ │ -
380 Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0, boost::none);
│ │ │ │ -
381 Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0, boost::none);
│ │ │ │ -
382
│ │ │ │ -
383 // convert to homogeneous coordinates
│ │ │ │ -
384 Vector3 vA = EssentialMatrix::Homogeneous(cA);
│ │ │ │ -
385 Vector3 vB = EssentialMatrix::Homogeneous(cB);
│ │ │ │ -
386
│ │ │ │ -
387 if (H2) {
│ │ │ │ -
388 // compute the jacobian of error w.r.t K
│ │ │ │ -
389
│ │ │ │ -
390 // error function f = vA.T * E * vB
│ │ │ │ -
391 // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK
│ │ │ │ -
392 // where dvA/dK = dvA/dcA * dcA/dK, dVB/dK = dvB/dcB * dcB/dK
│ │ │ │ -
393 // and dvA/dcA = dvB/dcB = [[1, 0], [0, 1], [0, 0]]
│ │ │ │ -
394 *H2 = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K +
│ │ │ │ -
395 vA.transpose() * E.matrix().leftCols<2>() * cB_H_K;
│ │ │ │ -
396 }
│ │ │ │ -
397
│ │ │ │ -
398 Vector error(1);
│ │ │ │ -
399 error << E.error(vA, vB, H1);
│ │ │ │ -
400
│ │ │ │ -
401 return error;
│ │ │ │ -
402 }
│ │ │ │ -
│ │ │ │ -
403
│ │ │ │ -
404 public:
│ │ │ │ -
405 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
406};
│ │ │ │ +
92 static Values initialize(const NonlinearFactorGraph& graph);
│ │ │ │ +
93};
│ │ │ │
│ │ │ │ -
407// EssentialMatrixFactor4
│ │ │ │ -
408
│ │ │ │ -
409} // namespace gtsam
│ │ │ │ -
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ -
PinholeCamera.h
Base class for all pinhole cameras.
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
94} // end of namespace gtsam
│ │ │ │ +
Rot3.h
3D rotation represented as a rotation matrix or quaternion
│ │ │ │ +
graph.h
Graph algorithm using boost library.
│ │ │ │ +
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ +
VectorValues.h
Factor Graph Values.
│ │ │ │ +
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │ -
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ -
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::FixedDimension
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition Manifold.h:164
│ │ │ │ -
gtsam::PinholeBase::Project
static Point2 Project(const Point3 &pc, OptionalJacobian< 2, 3 > Dpoint=boost::none)
Project from 3D point in camera coordinates into image Does not throw a CheiralityException,...
Definition CalibratedCamera.cpp:88
│ │ │ │ -
gtsam::EssentialMatrix
An essential matrix is like a Pose3, except with translation up to scale It is named after the 3*3 ma...
Definition EssentialMatrix.h:26
│ │ │ │ -
gtsam::EssentialMatrix::Homogeneous
static Vector3 Homogeneous(const Point2 &p)
Static function to convert Point2 to homogeneous coordinates.
Definition EssentialMatrix.h:34
│ │ │ │
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
│ │ │ │ -
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
│ │ │ │ -
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Calculate the error of the factor.
Definition NonlinearFactor.cpp:138
│ │ │ │ -
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ -
gtsam::NoiseModelFactorN< EssentialMatrix >::key
Key key() const
Returns a key.
Definition NonlinearFactor.h:518
│ │ │ │ -
gtsam::EssentialMatrixFactor
Factor that evaluates epipolar error p'Ep for given essential matrix.
Definition EssentialMatrixFactor.h:34
│ │ │ │ -
gtsam::EssentialMatrixFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition EssentialMatrixFactor.h:76
│ │ │ │ -
gtsam::EssentialMatrixFactor::EssentialMatrixFactor
EssentialMatrixFactor(Key key, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model)
Constructor.
Definition EssentialMatrixFactor.h:49
│ │ │ │ -
gtsam::EssentialMatrixFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition EssentialMatrixFactor.h:82
│ │ │ │ -
gtsam::EssentialMatrixFactor::EssentialMatrixFactor
EssentialMatrixFactor(Key key, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model, boost::shared_ptr< CALIBRATION > K)
Constructor.
Definition EssentialMatrixFactor.h:66
│ │ │ │ -
gtsam::EssentialMatrixFactor::evaluateError
Vector evaluateError(const EssentialMatrix &E, boost::optional< Matrix & > H=boost::none) const override
vector of errors returns 1D vector
Definition EssentialMatrixFactor.h:92
│ │ │ │ -
gtsam::EssentialMatrixFactor2
Binary factor that optimizes for E and inverse depth d: assumes measurement in image 2 is perfect,...
Definition EssentialMatrixFactor.h:109
│ │ │ │ -
gtsam::EssentialMatrixFactor2::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition EssentialMatrixFactor.h:154
│ │ │ │ -
gtsam::EssentialMatrixFactor2::EssentialMatrixFactor2
EssentialMatrixFactor2(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model, boost::shared_ptr< CALIBRATION > K)
Constructor.
Definition EssentialMatrixFactor.h:144
│ │ │ │ -
gtsam::EssentialMatrixFactor2::EssentialMatrixFactor2
EssentialMatrixFactor2(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model)
Constructor.
Definition EssentialMatrixFactor.h:126
│ │ │ │ -
gtsam::EssentialMatrixFactor2::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition EssentialMatrixFactor.h:160
│ │ │ │ -
gtsam::EssentialMatrixFactor3
Binary factor that optimizes for E and inverse depth d: assumes measurement in image 2 is perfect,...
Definition EssentialMatrixFactor.h:231
│ │ │ │ -
gtsam::EssentialMatrixFactor3::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition EssentialMatrixFactor.h:273
│ │ │ │ -
gtsam::EssentialMatrixFactor3::EssentialMatrixFactor3
EssentialMatrixFactor3(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const Rot3 &cRb, const SharedNoiseModel &model)
Constructor.
Definition EssentialMatrixFactor.h:247
│ │ │ │ -
gtsam::EssentialMatrixFactor3::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition EssentialMatrixFactor.h:267
│ │ │ │ -
gtsam::EssentialMatrixFactor3::EssentialMatrixFactor3
EssentialMatrixFactor3(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const Rot3 &cRb, const SharedNoiseModel &model, boost::shared_ptr< CALIBRATION > K)
Constructor.
Definition EssentialMatrixFactor.h:261
│ │ │ │ -
gtsam::EssentialMatrixFactor4
Binary factor that optimizes for E and calibration K using the algebraic epipolar error (K^-1 pA)'E (...
Definition EssentialMatrixFactor.h:324
│ │ │ │ -
gtsam::EssentialMatrixFactor4::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition EssentialMatrixFactor.h:355
│ │ │ │ -
gtsam::EssentialMatrixFactor4::evaluateError
Vector evaluateError(const EssentialMatrix &E, const CALIBRATION &K, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Calculate the algebraic epipolar error pA' (K^-1)' E K pB.
Definition EssentialMatrixFactor.h:373
│ │ │ │ -
gtsam::EssentialMatrixFactor4::EssentialMatrixFactor4
EssentialMatrixFactor4(Key keyE, Key keyK, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model)
Constructor.
Definition EssentialMatrixFactor.h:344
│ │ │ │ -
gtsam::EssentialMatrixFactor4::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition EssentialMatrixFactor.h:349
│ │ │ │ +
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ +
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │ +
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
gtsam::InitializePose3
Definition InitializePose3.h:37
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,488 +1,106 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -EssentialMatrixFactor.h │ │ │ │ │ +InitializePose3.h │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010-2014, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -12/* │ │ │ │ │ -13 * @file EssentialMatrixFactor.h │ │ │ │ │ -14 * @brief EssentialMatrixFactor class │ │ │ │ │ -15 * @author Frank Dellaert │ │ │ │ │ -16 * @author Ayush Baid │ │ │ │ │ -17 * @author Akshay Krishnan │ │ │ │ │ -18 * @date December 17, 2013 │ │ │ │ │ -19 */ │ │ │ │ │ -20 │ │ │ │ │ 21#pragma once │ │ │ │ │ 22 │ │ │ │ │ -23#include │ │ │ │ │ -24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_i_n_h_o_l_e_C_a_m_e_r_a_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -26 │ │ │ │ │ -27#include │ │ │ │ │ +23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_3_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_g_r_a_p_h_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ +27#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ 28 │ │ │ │ │ -29namespace _g_t_s_a_m { │ │ │ │ │ -30 │ │ │ │ │ -_3_4class _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -35 Vector3 vA_, vB_; │ │ │ │ │ +29#include │ │ │ │ │ +30#include │ │ │ │ │ +31 │ │ │ │ │ +32namespace _g_t_s_a_m { │ │ │ │ │ +33 │ │ │ │ │ +34typedef std::map > KeyVectorMap; │ │ │ │ │ +35typedef std::map KeyRotMap; │ │ │ │ │ 36 │ │ │ │ │ -37 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_> _B_a_s_e; │ │ │ │ │ -38 typedef _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ -39 │ │ │ │ │ -40 public: │ │ │ │ │ -_4_9 _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r(_K_e_y _k_e_y, const _P_o_i_n_t_2& pA, const _P_o_i_n_t_2& pB, │ │ │ │ │ -50 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model) │ │ │ │ │ -51 : _B_a_s_e(model, _k_e_y) { │ │ │ │ │ -52 vA_ = _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_:_:_H_o_m_o_g_e_n_e_o_u_s(pA); │ │ │ │ │ -53 vB_ = _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_:_:_H_o_m_o_g_e_n_e_o_u_s(pB); │ │ │ │ │ -54 } │ │ │ │ │ +_3_7struct GTSAM_EXPORT _I_n_i_t_i_a_l_i_z_e_P_o_s_e_3 { │ │ │ │ │ +38 static _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h buildLinearOrientationGraph( │ │ │ │ │ +39 const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& g); │ │ │ │ │ +40 │ │ │ │ │ +41 static _V_a_l_u_e_s normalizeRelaxedRotations(const _V_e_c_t_o_r_V_a_l_u_e_s& relaxedRot3); │ │ │ │ │ +42 │ │ │ │ │ +46 static _V_a_l_u_e_s computeOrientationsChordal( │ │ │ │ │ +47 const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& pose3Graph); │ │ │ │ │ +48 │ │ │ │ │ +52 static _V_a_l_u_e_s computeOrientationsGradient( │ │ │ │ │ +53 const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& pose3Graph, const _V_a_l_u_e_s& givenGuess, │ │ │ │ │ +54 size_t maxIter = 10000, const bool setRefFrame = true); │ │ │ │ │ 55 │ │ │ │ │ -65 template │ │ │ │ │ -_6_6 _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r(_K_e_y _k_e_y, const _P_o_i_n_t_2& pA, const _P_o_i_n_t_2& pB, │ │ │ │ │ -67 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model, │ │ │ │ │ -68 boost::shared_ptr K) │ │ │ │ │ -69 : _B_a_s_e(model, _k_e_y) { │ │ │ │ │ -70 assert(K); │ │ │ │ │ -71 vA_ = _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_:_:_H_o_m_o_g_e_n_e_o_u_s(K->calibrate(pA)); │ │ │ │ │ -72 vB_ = _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_:_:_H_o_m_o_g_e_n_e_o_u_s(K->calibrate(pB)); │ │ │ │ │ -73 } │ │ │ │ │ -74 │ │ │ │ │ -_7_6 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -77 return boost::static_pointer_cast( │ │ │ │ │ -78 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); │ │ │ │ │ -79 } │ │ │ │ │ -80 │ │ │ │ │ -_8_2 void _p_r_i_n_t( │ │ │ │ │ -83 const std::string& s = "", │ │ │ │ │ -84 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ -85 _B_a_s_e_:_:_p_r_i_n_t(s); │ │ │ │ │ -86 std::cout << " EssentialMatrixFactor with measurements\n (" │ │ │ │ │ -87 << vA_.transpose() << ")' and (" << vB_.transpose() << ")'" │ │ │ │ │ -88 << std::endl; │ │ │ │ │ -89 } │ │ │ │ │ +56 static void createSymbolicGraph(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& pose3Graph, │ │ │ │ │ +57 KeyVectorMap* adjEdgesMap, │ │ │ │ │ +58 KeyRotMap* factorId2RotMap); │ │ │ │ │ +59 │ │ │ │ │ +60 static Vector3 gradientTron(const _R_o_t_3& R1, const _R_o_t_3& R2, const double a, │ │ │ │ │ +61 const double b); │ │ │ │ │ +62 │ │ │ │ │ +67 static _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h buildPose3graph( │ │ │ │ │ +68 const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph); │ │ │ │ │ +69 │ │ │ │ │ +73 static _V_a_l_u_e_s computePoses(const _V_a_l_u_e_s& initialRot, │ │ │ │ │ +74 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h* poseGraph, │ │ │ │ │ +75 bool singleIter = true); │ │ │ │ │ +76 │ │ │ │ │ +81 static _V_a_l_u_e_s initializeOrientations(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph); │ │ │ │ │ +82 │ │ │ │ │ +88 static _V_a_l_u_e_s initialize(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, │ │ │ │ │ +89 const _V_a_l_u_e_s& givenGuess, bool useGradient = false); │ │ │ │ │ 90 │ │ │ │ │ -_9_2 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r( │ │ │ │ │ -93 const _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x& E, │ │ │ │ │ -94 boost::optional H = boost::none) const override { │ │ │ │ │ -95 Vector _e_r_r_o_r(1); │ │ │ │ │ -96 _e_r_r_o_r << E.error(vA_, vB_, H); │ │ │ │ │ -97 return _e_r_r_o_r; │ │ │ │ │ -98 } │ │ │ │ │ -99 │ │ │ │ │ -100 public: │ │ │ │ │ -101 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -102}; │ │ │ │ │ -103 │ │ │ │ │ -_1_0_8class _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2 │ │ │ │ │ -109 : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -110 _P_o_i_n_t_3 dP1_; │ │ │ │ │ -111 _P_o_i_n_t_2 pn_; │ │ │ │ │ -112 double f_; │ │ │ │ │ -113 │ │ │ │ │ -114 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_,_ _d_o_u_b_l_e_> _B_a_s_e; │ │ │ │ │ -115 typedef _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2 _T_h_i_s; │ │ │ │ │ -116 │ │ │ │ │ -117 public: │ │ │ │ │ -_1_2_6 _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2(_K_e_y key1, _K_e_y key2, const _P_o_i_n_t_2& pA, const _P_o_i_n_t_2& │ │ │ │ │ -pB, │ │ │ │ │ -127 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model) │ │ │ │ │ -128 : _B_a_s_e(model, key1, key2), │ │ │ │ │ -129 dP1_(_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x::Homogeneous(pA)), │ │ │ │ │ -130 pn_(pB) { │ │ │ │ │ -131 f_ = 1.0; │ │ │ │ │ -132 } │ │ │ │ │ -133 │ │ │ │ │ -143 template │ │ │ │ │ -_1_4_4 _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2(_K_e_y key1, _K_e_y key2, const _P_o_i_n_t_2& pA, const _P_o_i_n_t_2& │ │ │ │ │ -pB, │ │ │ │ │ -145 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model, │ │ │ │ │ -146 boost::shared_ptr K) │ │ │ │ │ -147 : _B_a_s_e(model, key1, key2), │ │ │ │ │ -148 dP1_(_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x::Homogeneous(K->calibrate(pA))), │ │ │ │ │ -149 pn_(K->calibrate(pB)) { │ │ │ │ │ -150 f_ = 0.5 * (K->fx() + K->fy()); │ │ │ │ │ -151 } │ │ │ │ │ -152 │ │ │ │ │ -_1_5_4 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -155 return boost::static_pointer_cast( │ │ │ │ │ -156 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); │ │ │ │ │ -157 } │ │ │ │ │ -158 │ │ │ │ │ -_1_6_0 void _p_r_i_n_t( │ │ │ │ │ -161 const std::string& s = "", │ │ │ │ │ -162 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ -163 _B_a_s_e_:_:_p_r_i_n_t(s); │ │ │ │ │ -164 std::cout << " EssentialMatrixFactor2 with measurements\n (" │ │ │ │ │ -165 << dP1_.transpose() << ")' and (" << pn_.transpose() << ")'" │ │ │ │ │ -166 << std::endl; │ │ │ │ │ -167 } │ │ │ │ │ -168 │ │ │ │ │ -169 /* │ │ │ │ │ -170 * Vector of errors returns 2D vector │ │ │ │ │ -171 * @param E essential matrix │ │ │ │ │ -172 * @param d inverse depth d │ │ │ │ │ -173 */ │ │ │ │ │ -174 Vector evaluateError( │ │ │ │ │ -175 const _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x& E, const double& d, │ │ │ │ │ -176 boost::optional DE = boost::none, │ │ │ │ │ -177 boost::optional Dd = boost::none) const override { │ │ │ │ │ -178 // We have point x,y in image 1 │ │ │ │ │ -179 // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d │ │ │ │ │ -180 // We then convert to second camera by P2 = 1R2'*(P1-1T2) │ │ │ │ │ -181 // The homogeneous coordinates of can be written as │ │ │ │ │ -182 // 2R1*(P1-1T2) == 2R1*d*(P1-1T2) == 2R1*((x,y,1)-d*1T2) │ │ │ │ │ -183 // where we multiplied with d which yields equivalent homogeneous │ │ │ │ │ -184 // coordinates. Note that this is just the homography 2R1 for d==0 The │ │ │ │ │ -point │ │ │ │ │ -185 // d*P1 = (x,y,1) is computed in constructor as dP1_ │ │ │ │ │ -186 │ │ │ │ │ -187 // Project to normalized image coordinates, then uncalibrate │ │ │ │ │ -188 _P_o_i_n_t_2 pn(0, 0); │ │ │ │ │ -189 if (!DE && !Dd) { │ │ │ │ │ -190 _P_o_i_n_t_3 _1T2 = E.direction().point3(); │ │ │ │ │ -191 _P_o_i_n_t_3 d1T2 = d * _1T2; │ │ │ │ │ -192 _P_o_i_n_t_3 dP2 = E.rotation().unrotate(dP1_ - d1T2); // 2R1*((x,y,1)-d*1T2) │ │ │ │ │ -193 pn = _P_i_n_h_o_l_e_B_a_s_e_:_:_P_r_o_j_e_c_t(dP2); │ │ │ │ │ -194 │ │ │ │ │ -195 } else { │ │ │ │ │ -196 // Calculate derivatives. TODO if slow: optimize with Mathematica │ │ │ │ │ -197 // 3*2 3*3 3*3 │ │ │ │ │ -198 Matrix D_1T2_dir, DdP2_rot, DP2_point; │ │ │ │ │ -199 │ │ │ │ │ -200 _P_o_i_n_t_3 _1T2 = E.direction().point3(D_1T2_dir); │ │ │ │ │ -201 _P_o_i_n_t_3 d1T2 = d * _1T2; │ │ │ │ │ -202 _P_o_i_n_t_3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point); │ │ │ │ │ -203 │ │ │ │ │ -204 Matrix23 Dpn_dP2; │ │ │ │ │ -205 pn = _P_i_n_h_o_l_e_B_a_s_e_:_:_P_r_o_j_e_c_t(dP2, Dpn_dP2); │ │ │ │ │ -206 │ │ │ │ │ -207 if (DE) { │ │ │ │ │ -208 Matrix DdP2_E(3, 5); │ │ │ │ │ -209 DdP2_E << DdP2_rot, -DP2_point * d * D_1T2_dir; // (3*3), (3*3) * (3*2) │ │ │ │ │ -210 *DE = f_ * Dpn_dP2 * DdP2_E; // (2*3) * (3*5) │ │ │ │ │ -211 } │ │ │ │ │ -212 │ │ │ │ │ -213 if (Dd) // efficient backwards computation: │ │ │ │ │ -214 // (2*3) * (3*3) * (3*1) │ │ │ │ │ -215 *Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2)); │ │ │ │ │ -216 } │ │ │ │ │ -217 _P_o_i_n_t_2 reprojectionError = pn - pn_; │ │ │ │ │ -218 return f_ * reprojectionError; │ │ │ │ │ -219 } │ │ │ │ │ -220 │ │ │ │ │ -221 public: │ │ │ │ │ -222 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -223}; │ │ │ │ │ -224// EssentialMatrixFactor2 │ │ │ │ │ -225 │ │ │ │ │ -_2_3_1class _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_3 : public _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2 { │ │ │ │ │ -232 typedef _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2 _B_a_s_e; │ │ │ │ │ -233 typedef _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_3 _T_h_i_s; │ │ │ │ │ -234 │ │ │ │ │ -235 _R_o_t_3 cRb_; │ │ │ │ │ -236 │ │ │ │ │ -237 public: │ │ │ │ │ -_2_4_7 _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_3(_K_e_y key1, _K_e_y key2, const _P_o_i_n_t_2& pA, const _P_o_i_n_t_2& │ │ │ │ │ -pB, │ │ │ │ │ -248 const _R_o_t_3& cRb, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model) │ │ │ │ │ -249 : _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2(key1, key2, pA, pB, model), cRb_(cRb) {} │ │ │ │ │ -250 │ │ │ │ │ -260 template │ │ │ │ │ -_2_6_1 _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_3(_K_e_y key1, _K_e_y key2, const _P_o_i_n_t_2& pA, const _P_o_i_n_t_2& │ │ │ │ │ -pB, │ │ │ │ │ -262 const _R_o_t_3& cRb, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model, │ │ │ │ │ -263 boost::shared_ptr K) │ │ │ │ │ -264 : _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2(key1, key2, pA, pB, model, K), cRb_(cRb) {} │ │ │ │ │ -265 │ │ │ │ │ -_2_6_7 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -268 return boost::static_pointer_cast( │ │ │ │ │ -269 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); │ │ │ │ │ -270 } │ │ │ │ │ -271 │ │ │ │ │ -_2_7_3 void _p_r_i_n_t( │ │ │ │ │ -274 const std::string& s = "", │ │ │ │ │ -275 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ -276 _B_a_s_e_:_:_p_r_i_n_t(s); │ │ │ │ │ -277 std::cout << " EssentialMatrixFactor3 with rotation " << cRb_ << std::endl; │ │ │ │ │ -278 } │ │ │ │ │ -279 │ │ │ │ │ -280 /* │ │ │ │ │ -281 * Vector of errors returns 2D vector │ │ │ │ │ -282 * @param E essential matrix │ │ │ │ │ -283 * @param d inverse depth d │ │ │ │ │ -284 */ │ │ │ │ │ -285 Vector evaluateError( │ │ │ │ │ -286 const _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x& E, const double& d, │ │ │ │ │ -287 boost::optional DE = boost::none, │ │ │ │ │ -288 boost::optional Dd = boost::none) const override { │ │ │ │ │ -289 if (!DE) { │ │ │ │ │ -290 // Convert E from body to camera frame │ │ │ │ │ -291 _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x cameraE = cRb_ * E; │ │ │ │ │ -292 // Evaluate error │ │ │ │ │ -293 return Base::evaluateError(cameraE, d, boost::none, Dd); │ │ │ │ │ -294 } else { │ │ │ │ │ -295 // Version with derivatives │ │ │ │ │ -296 Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5 │ │ │ │ │ -297 EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E); │ │ │ │ │ -298 Vector e = Base::evaluateError(cameraE, d, D_e_cameraE, Dd); │ │ │ │ │ -299 *DE = D_e_cameraE * D_cameraE_E; // (2*5) * (5*5) │ │ │ │ │ -300 return e; │ │ │ │ │ -301 } │ │ │ │ │ -302 } │ │ │ │ │ -303 │ │ │ │ │ -304 public: │ │ │ │ │ -305 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -306}; │ │ │ │ │ -307// EssentialMatrixFactor3 │ │ │ │ │ -308 │ │ │ │ │ -322template │ │ │ │ │ -_3_2_3class _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_4 │ │ │ │ │ -324 : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -325 private: │ │ │ │ │ -326 _P_o_i_n_t_2 pA_, pB_; │ │ │ │ │ -327 │ │ │ │ │ -328 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_,_ _C_A_L_I_B_R_A_T_I_O_N_> _B_a_s_e; │ │ │ │ │ -329 typedef _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_4 _T_h_i_s; │ │ │ │ │ -330 │ │ │ │ │ -331 static constexpr int DimK = _F_i_x_e_d_D_i_m_e_n_s_i_o_n_<_C_A_L_I_B_R_A_T_I_O_N_>_:_:_v_a_l_u_e; │ │ │ │ │ -332 typedef Eigen::Matrix JacobianCalibration; │ │ │ │ │ -333 │ │ │ │ │ -334 public: │ │ │ │ │ -_3_4_4 _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_4(_K_e_y keyE, _K_e_y keyK, const _P_o_i_n_t_2& pA, const _P_o_i_n_t_2& │ │ │ │ │ -pB, │ │ │ │ │ -345 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model) │ │ │ │ │ -346 : _B_a_s_e(model, keyE, keyK), pA_(pA), pB_(pB) {} │ │ │ │ │ -347 │ │ │ │ │ -_3_4_9 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -350 return boost::static_pointer_cast( │ │ │ │ │ -351 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); │ │ │ │ │ -352 } │ │ │ │ │ -353 │ │ │ │ │ -_3_5_5 void _p_r_i_n_t( │ │ │ │ │ -356 const std::string& s = "", │ │ │ │ │ -357 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ -358 _B_a_s_e_:_:_p_r_i_n_t(s); │ │ │ │ │ -359 std::cout << " EssentialMatrixFactor4 with measurements\n (" │ │ │ │ │ -360 << pA_.transpose() << ")' and (" << pB_.transpose() << ")'" │ │ │ │ │ -361 << std::endl; │ │ │ │ │ -362 } │ │ │ │ │ -363 │ │ │ │ │ -_3_7_3 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r( │ │ │ │ │ -374 const _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x& E, const CALIBRATION& K, │ │ │ │ │ -375 boost::optional H1 = boost::none, │ │ │ │ │ -376 boost::optional H2 = boost::none) const override { │ │ │ │ │ -377 // converting from pixel coordinates to normalized coordinates cA and cB │ │ │ │ │ -378 JacobianCalibration cA_H_K; // dcA/dK │ │ │ │ │ -379 JacobianCalibration cB_H_K; // dcB/dK │ │ │ │ │ -380 _P_o_i_n_t_2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0, boost::none); │ │ │ │ │ -381 _P_o_i_n_t_2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0, boost::none); │ │ │ │ │ -382 │ │ │ │ │ -383 // convert to homogeneous coordinates │ │ │ │ │ -384 Vector3 vA = _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_:_:_H_o_m_o_g_e_n_e_o_u_s(cA); │ │ │ │ │ -385 Vector3 vB = _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_:_:_H_o_m_o_g_e_n_e_o_u_s(cB); │ │ │ │ │ -386 │ │ │ │ │ -387 if (H2) { │ │ │ │ │ -388 // compute the jacobian of error w.r.t K │ │ │ │ │ -389 │ │ │ │ │ -390 // error function f = vA.T * E * vB │ │ │ │ │ -391 // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK │ │ │ │ │ -392 // where dvA/dK = dvA/dcA * dcA/dK, dVB/dK = dvB/dcB * dcB/dK │ │ │ │ │ -393 // and dvA/dcA = dvB/dcB = [[1, 0], [0, 1], [0, 0]] │ │ │ │ │ -394 *H2 = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K + │ │ │ │ │ -395 vA.transpose() * E.matrix().leftCols<2>() * cB_H_K; │ │ │ │ │ -396 } │ │ │ │ │ -397 │ │ │ │ │ -398 Vector _e_r_r_o_r(1); │ │ │ │ │ -399 _e_r_r_o_r << E.error(vA, vB, H1); │ │ │ │ │ -400 │ │ │ │ │ -401 return _e_r_r_o_r; │ │ │ │ │ -402 } │ │ │ │ │ -403 │ │ │ │ │ -404 public: │ │ │ │ │ -405 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -406}; │ │ │ │ │ -407// EssentialMatrixFactor4 │ │ │ │ │ -408 │ │ │ │ │ -409} // namespace gtsam │ │ │ │ │ -_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ -This marks a GTSAM object to require alignment. │ │ │ │ │ -DDeeffiinniittiioonn types.h:308 │ │ │ │ │ -_P_i_n_h_o_l_e_C_a_m_e_r_a_._h │ │ │ │ │ -Base class for all pinhole cameras. │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ -Non-linear factor base classes. │ │ │ │ │ +92 static _V_a_l_u_e_s initialize(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph); │ │ │ │ │ +93}; │ │ │ │ │ +94} // end of namespace gtsam │ │ │ │ │ +_R_o_t_3_._h │ │ │ │ │ +3D rotation represented as a rotation matrix or quaternion │ │ │ │ │ +_g_r_a_p_h_._h │ │ │ │ │ +Graph algorithm using boost library. │ │ │ │ │ +_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ +Factor Graph Values. │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Factor Graph consisting of non-linear factors. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ -Vector2 Point2 │ │ │ │ │ -As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point2 to Vector2... │ │ │ │ │ -DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ -Vector3 Point3 │ │ │ │ │ -As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point3 to Vector3... │ │ │ │ │ -DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ -Aliases. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_F_i_x_e_d_D_i_m_e_n_s_i_o_n │ │ │ │ │ -Give fixed size dimension of a type, fails at compile time if dynamic. │ │ │ │ │ -DDeeffiinniittiioonn Manifold.h:164 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_P_r_o_j_e_c_t │ │ │ │ │ -static Point2 Project(const Point3 &pc, OptionalJacobian< 2, 3 > Dpoint=boost:: │ │ │ │ │ -none) │ │ │ │ │ -Project from 3D point in camera coordinates into image Does not throw a │ │ │ │ │ -CheiralityException,... │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.cpp:88 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x │ │ │ │ │ -An essential matrix is like a Pose3, except with translation up to scale It is │ │ │ │ │ -named after the 3*3 ma... │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrix.h:26 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_:_:_H_o_m_o_g_e_n_e_o_u_s │ │ │ │ │ -static Vector3 Homogeneous(const Point2 &p) │ │ │ │ │ -Static function to convert Point2 to homogeneous coordinates. │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrix.h:34 │ │ │ │ │ _g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor │ │ │ │ │ symbol GTSAM_USE_QUATERNIO... │ │ │ │ │ DDeeffiinniittiioonn Rot3.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ -virtual void print(const std::string &s="Factor", const KeyFormatter │ │ │ │ │ -&formatter=DefaultKeyFormatter) const │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn Factor.cpp:29 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_e_r_r_o_r │ │ │ │ │ -double error(const Values &c) const override │ │ │ │ │ -Calculate the error of the factor. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.cpp:138 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ -A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ -variables. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_ _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_ _>_:_:_k_e_y │ │ │ │ │ -Key key() const │ │ │ │ │ -Returns a key. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:518 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r │ │ │ │ │ -Factor that evaluates epipolar error p'Ep for given essential matrix. │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrixFactor.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ -gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrixFactor.h:76 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r │ │ │ │ │ -EssentialMatrixFactor(Key key, const Point2 &pA, const Point2 &pB, const │ │ │ │ │ -SharedNoiseModel &model) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrixFactor.h:49 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrixFactor.h:82 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r │ │ │ │ │ -EssentialMatrixFactor(Key key, const Point2 &pA, const Point2 &pB, const │ │ │ │ │ -SharedNoiseModel &model, boost::shared_ptr< CALIBRATION > K) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrixFactor.h:66 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ -Vector evaluateError(const EssentialMatrix &E, boost::optional< Matrix & > │ │ │ │ │ -H=boost::none) const override │ │ │ │ │ -vector of errors returns 1D vector │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrixFactor.h:92 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2 │ │ │ │ │ -Binary factor that optimizes for E and inverse depth d: assumes measurement in │ │ │ │ │ -image 2 is perfect,... │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrixFactor.h:109 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2_:_:_c_l_o_n_e │ │ │ │ │ -gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrixFactor.h:154 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2 │ │ │ │ │ -EssentialMatrixFactor2(Key key1, Key key2, const Point2 &pA, const Point2 &pB, │ │ │ │ │ -const SharedNoiseModel &model, boost::shared_ptr< CALIBRATION > K) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrixFactor.h:144 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2 │ │ │ │ │ -EssentialMatrixFactor2(Key key1, Key key2, const Point2 &pA, const Point2 &pB, │ │ │ │ │ -const SharedNoiseModel &model) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrixFactor.h:126 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrixFactor.h:160 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_3 │ │ │ │ │ -Binary factor that optimizes for E and inverse depth d: assumes measurement in │ │ │ │ │ -image 2 is perfect,... │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrixFactor.h:231 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_3_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrixFactor.h:273 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_3_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_3 │ │ │ │ │ -EssentialMatrixFactor3(Key key1, Key key2, const Point2 &pA, const Point2 &pB, │ │ │ │ │ -const Rot3 &cRb, const SharedNoiseModel &model) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrixFactor.h:247 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_3_:_:_c_l_o_n_e │ │ │ │ │ -gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrixFactor.h:267 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_3_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_3 │ │ │ │ │ -EssentialMatrixFactor3(Key key1, Key key2, const Point2 &pA, const Point2 &pB, │ │ │ │ │ -const Rot3 &cRb, const SharedNoiseModel &model, boost::shared_ptr< CALIBRATION │ │ │ │ │ -> K) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrixFactor.h:261 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_4 │ │ │ │ │ -Binary factor that optimizes for E and calibration K using the algebraic │ │ │ │ │ -epipolar error (K^-1 pA)'E (... │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrixFactor.h:324 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_4_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrixFactor.h:355 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_4_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ -Vector evaluateError(const EssentialMatrix &E, const CALIBRATION &K, boost:: │ │ │ │ │ -optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost:: │ │ │ │ │ -none) const override │ │ │ │ │ -Calculate the algebraic epipolar error pA' (K^-1)' E K pB. │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrixFactor.h:373 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_4_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_4 │ │ │ │ │ -EssentialMatrixFactor4(Key keyE, Key keyK, const Point2 &pA, const Point2 &pB, │ │ │ │ │ -const SharedNoiseModel &model) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrixFactor.h:344 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_4_:_:_c_l_o_n_e │ │ │ │ │ -gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrixFactor.h:349 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ +with a unique integer... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_I_n_i_t_i_a_l_i_z_e_P_o_s_e_3 │ │ │ │ │ +DDeeffiinniittiioonn InitializePose3.h:37 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * EEsssseennttiiaallMMaattrriixxFFaaccttoorr..hh │ │ │ │ │ + * _I_n_i_t_i_a_l_i_z_e_P_o_s_e_3_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01274.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/BetweenFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/EssentialMatrixConstraint.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,55 +95,42 @@ │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Macros
│ │ │ │ -
BetweenFactor.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
EssentialMatrixConstraint.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::BetweenFactor< VALUE >
 A class for a measurement predicted by "between(config[key1],config[key2])". More...
 
struct  gtsam::traits< BetweenFactor< VALUE > >
 traits More...
 
class  gtsam::BetweenConstraint< VALUE >
 Binary between constraint - forces between to a given value This constraint requires the underlying type to a Lie type. More...
 
struct  gtsam::traits< BetweenConstraint< VALUE > >
 traits More...
class  gtsam::EssentialMatrixConstraint
 Binary factor between two Pose3 variables induced by an EssentialMatrix measurement. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Macros

│ │ │ │ -#define BETWEENFACTOR_VISIBILITY   GTSAM_EXPORT
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Frank Dellaert, Viorela Ila
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ +Pablo Alcantarilla
│ │ │ │ +
Date
Jan 5, 2014
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,37 +1,27 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s │ │ │ │ │ -BetweenFactor.h File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +EssentialMatrixConstraint.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_B_e_t_w_e_e_n_F_a_c_t_o_r_<_ _V_A_L_U_E_ _> │ │ │ │ │ -  A class for a measurement predicted by "between(config[key1],config │ │ │ │ │ - [key2])". _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _B_e_t_w_e_e_n_F_a_c_t_o_r_<_ _V_A_L_U_E_ _>_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_B_e_t_w_e_e_n_C_o_n_s_t_r_a_i_n_t_<_ _V_A_L_U_E_ _> │ │ │ │ │ -  Binary between constraint - forces between to a given value This │ │ │ │ │ - constraint requires the underlying type to a Lie type. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _B_e_t_w_e_e_n_C_o_n_s_t_r_a_i_n_t_<_ _V_A_L_U_E_ _>_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t │ │ │ │ │ +  Binary factor between two _P_o_s_e_3 variables induced by an _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x │ │ │ │ │ + measurement. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -MMaaccrrooss │ │ │ │ │ -#define  BBEETTWWEEEENNFFAACCTTOORR__VVIISSIIBBIILLIITTYY   GTSAM_EXPORT │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert, Viorela Ila │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Pablo Alcantarilla │ │ │ │ │ + Date │ │ │ │ │ + Jan 5, 2014 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _B_e_t_w_e_e_n_F_a_c_t_o_r_._h │ │ │ │ │ + * _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01274_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/BetweenFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/EssentialMatrixConstraint.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,215 +98,127 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
BetweenFactor.h
│ │ │ │ +
EssentialMatrixConstraint.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010-2014, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
16#pragma once
│ │ │ │ -
17
│ │ │ │ -
18#include <ostream>
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/base/Testable.h>
│ │ │ │ -
21#include <gtsam/base/Lie.h>
│ │ │ │ -
22#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
22#include <gtsam/geometry/EssentialMatrix.h>
│ │ │ │
23
│ │ │ │ -
24#ifdef _WIN32
│ │ │ │ -
25#define BETWEENFACTOR_VISIBILITY
│ │ │ │ -
26#else
│ │ │ │ -
27// This will trigger a LNKxxxx on MSVC, so disable for MSVC build
│ │ │ │ -
28// Please refer to https://github.com/borglab/gtsam/blob/develop/Using-GTSAM-EXPORT.md
│ │ │ │ -
29#define BETWEENFACTOR_VISIBILITY GTSAM_EXPORT
│ │ │ │ -
30#endif
│ │ │ │ +
24namespace gtsam {
│ │ │ │ +
25
│ │ │ │ +
│ │ │ │ +
30class GTSAM_EXPORT EssentialMatrixConstraint: public NoiseModelFactorN<Pose3, Pose3> {
│ │ │ │
31
│ │ │ │ -
32namespace gtsam {
│ │ │ │ +
32private:
│ │ │ │
33
│ │ │ │ -
39 template<class VALUE>
│ │ │ │ -
│ │ │ │ -
40 class BetweenFactor: public NoiseModelFactorN<VALUE, VALUE> {
│ │ │ │ -
41
│ │ │ │ -
42 // Check that VALUE type is a testable Lie group
│ │ │ │ -
43 BOOST_CONCEPT_ASSERT((IsTestable<VALUE>));
│ │ │ │ -
44 BOOST_CONCEPT_ASSERT((IsLieGroup<VALUE>));
│ │ │ │ -
45
│ │ │ │ -
46 public:
│ │ │ │ +
34 typedef EssentialMatrixConstraint This;
│ │ │ │ +
35 typedef NoiseModelFactorN<Pose3, Pose3> Base;
│ │ │ │ +
36
│ │ │ │ +
37 EssentialMatrix measuredE_;
│ │ │ │ +
39public:
│ │ │ │ +
40
│ │ │ │ +
41 // shorthand for a smart pointer to a factor
│ │ │ │ +
42 typedef boost::shared_ptr<EssentialMatrixConstraint> shared_ptr;
│ │ │ │ +
43
│ │ │ │ +
│ │ │ │ +
45 EssentialMatrixConstraint() {
│ │ │ │ +
46 }
│ │ │ │ +
│ │ │ │
47
│ │ │ │ -
48 typedef VALUE T;
│ │ │ │ -
49
│ │ │ │ -
50 private:
│ │ │ │ -
51
│ │ │ │ -
52 typedef BetweenFactor<VALUE> This;
│ │ │ │ -
53 typedef NoiseModelFactorN<VALUE, VALUE> Base;
│ │ │ │ -
54
│ │ │ │ -
55 VALUE measured_;
│ │ │ │ -
57 public:
│ │ │ │ -
58
│ │ │ │ -
59 // shorthand for a smart pointer to a factor
│ │ │ │ -
60 typedef typename boost::shared_ptr<BetweenFactor> shared_ptr;
│ │ │ │ -
61
│ │ │ │ -
64
│ │ │ │ -
66 BetweenFactor() {}
│ │ │ │ -
67
│ │ │ │ -
│ │ │ │ -
69 BetweenFactor(Key key1, Key key2, const VALUE& measured,
│ │ │ │ -
70 const SharedNoiseModel& model = nullptr) :
│ │ │ │ -
71 Base(model, key1, key2), measured_(measured) {
│ │ │ │ -
72 }
│ │ │ │ -
│ │ │ │ -
73
│ │ │ │ -
75
│ │ │ │ -
76 ~BetweenFactor() override {}
│ │ │ │ +
│ │ │ │ +
55 EssentialMatrixConstraint(Key key1, Key key2,
│ │ │ │ +
56 const EssentialMatrix& measuredE, const SharedNoiseModel& model) :
│ │ │ │ +
57 Base(model, key1, key2), measuredE_(measuredE) {
│ │ │ │ +
58 }
│ │ │ │ +
│ │ │ │ +
59
│ │ │ │ +
60 ~EssentialMatrixConstraint() override {
│ │ │ │ +
61 }
│ │ │ │ +
62
│ │ │ │ +
│ │ │ │ +
64 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
65 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ +
66 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
│ │ │ │ +
67 }
│ │ │ │ +
│ │ │ │ +
68
│ │ │ │ +
72 void print(const std::string& s = "",
│ │ │ │ +
73 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
│ │ │ │ +
74
│ │ │ │ +
76 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
│ │ │ │
77
│ │ │ │ -
│ │ │ │ -
79 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
80 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ -
81 gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
│ │ │ │ -
│ │ │ │ -
82
│ │ │ │ -
85
│ │ │ │ -
│ │ │ │ -
87 void print(
│ │ │ │ -
88 const std::string& s = "",
│ │ │ │ -
89 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ -
90 std::cout << s << "BetweenFactor("
│ │ │ │ -
91 << keyFormatter(this->key1()) << ","
│ │ │ │ -
92 << keyFormatter(this->key2()) << ")\n";
│ │ │ │ -
93 traits<T>::Print(measured_, " measured: ");
│ │ │ │ -
94 this->noiseModel_->print(" noise model: ");
│ │ │ │ -
95 }
│ │ │ │ -
│ │ │ │ -
96
│ │ │ │ -
│ │ │ │ -
98 bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
│ │ │ │ -
99 const This *e = dynamic_cast<const This*> (&expected);
│ │ │ │ -
100 return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(this->measured_, e->measured_, tol);
│ │ │ │ -
101 }
│ │ │ │ -
│ │ │ │ +
81 Vector evaluateError(const Pose3& p1, const Pose3& p2,
│ │ │ │ +
82 boost::optional<Matrix&> Hp1 = boost::none, //
│ │ │ │ +
83 boost::optional<Matrix&> Hp2 = boost::none) const override;
│ │ │ │ +
84
│ │ │ │ +
│ │ │ │ +
86 const EssentialMatrix& measured() const {
│ │ │ │ +
87 return measuredE_;
│ │ │ │ +
88 }
│ │ │ │ +
│ │ │ │ +
89
│ │ │ │ +
90private:
│ │ │ │ +
91
│ │ │ │ +
93 friend class boost::serialization::access;
│ │ │ │ +
94 template<class ARCHIVE>
│ │ │ │ +
95 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
96 // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ +
97 ar
│ │ │ │ +
98 & boost::serialization::make_nvp("NoiseModelFactor2",
│ │ │ │ +
99 boost::serialization::base_object<Base>(*this));
│ │ │ │ +
100 ar & BOOST_SERIALIZATION_NVP(measuredE_);
│ │ │ │ +
101 }
│ │ │ │
102
│ │ │ │ -
106
│ │ │ │ -
│ │ │ │ -
108 Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 =
│ │ │ │ -
109 boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
│ │ │ │ -
110 T hx = traits<T>::Between(p1, p2, H1, H2); // h(x)
│ │ │ │ -
111 // manifold equivalent of h(x)-z -> log(z,h(x))
│ │ │ │ -
112#ifdef GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR
│ │ │ │ -
113 typename traits<T>::ChartJacobian::Jacobian Hlocal;
│ │ │ │ -
114 Vector rval = traits<T>::Local(measured_, hx, boost::none, (H1 || H2) ? &Hlocal : 0);
│ │ │ │ -
115 if (H1) *H1 = Hlocal * (*H1);
│ │ │ │ -
116 if (H2) *H2 = Hlocal * (*H2);
│ │ │ │ -
117 return rval;
│ │ │ │ -
118#else
│ │ │ │ -
119 return traits<T>::Local(measured_, hx);
│ │ │ │ -
120#endif
│ │ │ │ -
121 }
│ │ │ │ -
│ │ │ │ -
122
│ │ │ │ -
126
│ │ │ │ -
│ │ │ │ -
128 const VALUE& measured() const {
│ │ │ │ -
129 return measured_;
│ │ │ │ -
130 }
│ │ │ │ -
│ │ │ │ -
132
│ │ │ │ -
133 private:
│ │ │ │ -
134
│ │ │ │ -
136 friend class boost::serialization::access;
│ │ │ │ -
137 template<class ARCHIVE>
│ │ │ │ -
138 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
139 // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ -
140 ar & boost::serialization::make_nvp("NoiseModelFactor2",
│ │ │ │ -
141 boost::serialization::base_object<Base>(*this));
│ │ │ │ -
142 ar & BOOST_SERIALIZATION_NVP(measured_);
│ │ │ │ -
143 }
│ │ │ │ -
144
│ │ │ │ -
145 // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
│ │ │ │ -
146 enum { NeedsToAlign = (sizeof(VALUE) % 16) == 0 };
│ │ │ │ -
147 public:
│ │ │ │ -
148 GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
│ │ │ │ -
149 }; // \class BetweenFactor
│ │ │ │ -
│ │ │ │ -
150
│ │ │ │ -
152 template<class VALUE>
│ │ │ │ -
153 struct traits<BetweenFactor<VALUE> > : public Testable<BetweenFactor<VALUE> > {};
│ │ │ │ -
154
│ │ │ │ -
160 template<class VALUE>
│ │ │ │ -
│ │ │ │ -
161 class BetweenConstraint : public BetweenFactor<VALUE> {
│ │ │ │ -
162 public:
│ │ │ │ -
163 typedef boost::shared_ptr<BetweenConstraint<VALUE> > shared_ptr;
│ │ │ │ -
164
│ │ │ │ -
│ │ │ │ -
166 BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) :
│ │ │ │ -
167 BetweenFactor<VALUE>(key1, key2, measured,
│ │ │ │ -
168 noiseModel::Constrained::All(traits<VALUE>::GetDimension(measured), std::abs(mu)))
│ │ │ │ -
169 {}
│ │ │ │ -
│ │ │ │ -
170
│ │ │ │ -
171 private:
│ │ │ │ -
172
│ │ │ │ -
174 friend class boost::serialization::access;
│ │ │ │ -
175 template<class ARCHIVE>
│ │ │ │ -
176 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
177 ar & boost::serialization::make_nvp("BetweenFactor",
│ │ │ │ -
178 boost::serialization::base_object<BetweenFactor<VALUE> >(*this));
│ │ │ │ -
179 }
│ │ │ │ -
180 }; // \class BetweenConstraint
│ │ │ │ -
│ │ │ │ -
181
│ │ │ │ -
183 template<class VALUE>
│ │ │ │ -
184 struct traits<BetweenConstraint<VALUE> > : public Testable<BetweenConstraint<VALUE> > {};
│ │ │ │ -
185
│ │ │ │ -
186}
│ │ │ │ -
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ -
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
This marks a GTSAM object to require alignment.
Definition types.h:317
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
103public:
│ │ │ │ +
104 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
105};
│ │ │ │ +
│ │ │ │ +
106// \class EssentialMatrixConstraint
│ │ │ │ +
107
│ │ │ │ +
108}
│ │ │ │ +
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::IsLieGroup
Lie Group Concept.
Definition Lie.h:260
│ │ │ │ -
gtsam::IsTestable
A testable concept check that should be placed in applicable unit tests and in generic algorithms.
Definition Testable.h:58
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::EssentialMatrix
An essential matrix is like a Pose3, except with translation up to scale It is named after the 3*3 ma...
Definition EssentialMatrix.h:26
│ │ │ │ +
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ -
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition NonlinearFactor.h:223
│ │ │ │
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ -
gtsam::BetweenFactor
A class for a measurement predicted by "between(config[key1],config[key2])".
Definition BetweenFactor.h:40
│ │ │ │ -
gtsam::BetweenFactor::BetweenFactor
BetweenFactor()
default constructor - only use for serialization
Definition BetweenFactor.h:66
│ │ │ │ -
gtsam::BetweenFactor::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
assert equality up to a tolerance
Definition BetweenFactor.h:98
│ │ │ │ -
gtsam::BetweenFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition BetweenFactor.h:79
│ │ │ │ -
gtsam::BetweenFactor::shared_ptr
boost::shared_ptr< BetweenFactor > shared_ptr
The measurement.
Definition BetweenFactor.h:60
│ │ │ │ -
gtsam::BetweenFactor::measured
const VALUE & measured() const
return the measurement
Definition BetweenFactor.h:128
│ │ │ │ -
gtsam::BetweenFactor::BetweenFactor
BetweenFactor(Key key1, Key key2, const VALUE &measured, const SharedNoiseModel &model=nullptr)
Constructor.
Definition BetweenFactor.h:69
│ │ │ │ -
gtsam::BetweenFactor::access
friend class boost::serialization::access
Serialization function.
Definition BetweenFactor.h:136
│ │ │ │ -
gtsam::BetweenFactor::evaluateError
Vector evaluateError(const T &p1, const T &p2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
evaluate error, returns vector of errors size of tangent space
Definition BetweenFactor.h:108
│ │ │ │ -
gtsam::BetweenFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print with optional string
Definition BetweenFactor.h:87
│ │ │ │ -
gtsam::BetweenConstraint
Binary between constraint - forces between to a given value This constraint requires the underlying t...
Definition BetweenFactor.h:161
│ │ │ │ -
gtsam::BetweenConstraint::BetweenConstraint
BetweenConstraint(const VALUE &measured, Key key1, Key key2, double mu=1000.0)
Syntactic sugar for constrained version.
Definition BetweenFactor.h:166
│ │ │ │ -
gtsam::BetweenConstraint::access
friend class boost::serialization::access
Serialization function.
Definition BetweenFactor.h:174
│ │ │ │ +
gtsam::EssentialMatrixConstraint
Binary factor between two Pose3 variables induced by an EssentialMatrix measurement.
Definition EssentialMatrixConstraint.h:30
│ │ │ │ +
gtsam::EssentialMatrixConstraint::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition EssentialMatrixConstraint.h:64
│ │ │ │ +
gtsam::EssentialMatrixConstraint::measured
const EssentialMatrix & measured() const
return the measured
Definition EssentialMatrixConstraint.h:86
│ │ │ │ +
gtsam::EssentialMatrixConstraint::shared_ptr
boost::shared_ptr< EssentialMatrixConstraint > shared_ptr
The measurement is an essential matrix.
Definition EssentialMatrixConstraint.h:42
│ │ │ │ +
gtsam::EssentialMatrixConstraint::EssentialMatrixConstraint
EssentialMatrixConstraint()
default constructor - only use for serialization
Definition EssentialMatrixConstraint.h:45
│ │ │ │ +
gtsam::EssentialMatrixConstraint::EssentialMatrixConstraint
EssentialMatrixConstraint(Key key1, Key key2, const EssentialMatrix &measuredE, const SharedNoiseModel &model)
Constructor.
Definition EssentialMatrixConstraint.h:55
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,278 +1,159 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -BetweenFactor.h │ │ │ │ │ +EssentialMatrixConstraint.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010-2014, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -16#pragma once │ │ │ │ │ -17 │ │ │ │ │ -18#include │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_b_a_s_e_/_L_i_e_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +22#include │ │ │ │ │ 23 │ │ │ │ │ -24#ifdef _WIN32 │ │ │ │ │ -25#define BETWEENFACTOR_VISIBILITY │ │ │ │ │ -26#else │ │ │ │ │ -27// This will trigger a LNKxxxx on MSVC, so disable for MSVC build │ │ │ │ │ -28// Please refer to https://github.com/borglab/gtsam/blob/develop/Using-GTSAM- │ │ │ │ │ -EXPORT.md │ │ │ │ │ -29#define BETWEENFACTOR_VISIBILITY GTSAM_EXPORT │ │ │ │ │ -30#endif │ │ │ │ │ +24namespace _g_t_s_a_m { │ │ │ │ │ +25 │ │ │ │ │ +_3_0class GTSAM_EXPORT _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ 31 │ │ │ │ │ -32namespace _g_t_s_a_m { │ │ │ │ │ +32private: │ │ │ │ │ 33 │ │ │ │ │ -39 template │ │ │ │ │ -_4_0 class _B_e_t_w_e_e_n_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -41 │ │ │ │ │ -42 // Check that VALUE type is a testable Lie group │ │ │ │ │ -43 BOOST_CONCEPT_ASSERT((_I_s_T_e_s_t_a_b_l_e_<_V_A_L_U_E_>)); │ │ │ │ │ -44 BOOST_CONCEPT_ASSERT((_I_s_L_i_e_G_r_o_u_p_<_V_A_L_U_E_>)); │ │ │ │ │ -45 │ │ │ │ │ -46 public: │ │ │ │ │ +34 typedef _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t _T_h_i_s; │ │ │ │ │ +35 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_P_o_s_e_3_,_ _P_o_s_e_3_> _B_a_s_e; │ │ │ │ │ +36 │ │ │ │ │ +37 _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x measuredE_; │ │ │ │ │ +39public: │ │ │ │ │ +40 │ │ │ │ │ +41 // shorthand for a smart pointer to a factor │ │ │ │ │ +_4_2 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +43 │ │ │ │ │ +_4_5 _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t() { │ │ │ │ │ +46 } │ │ │ │ │ 47 │ │ │ │ │ -48 typedef VALUE T; │ │ │ │ │ -49 │ │ │ │ │ -50 private: │ │ │ │ │ -51 │ │ │ │ │ -52 typedef _B_e_t_w_e_e_n_F_a_c_t_o_r_<_V_A_L_U_E_> This; │ │ │ │ │ -53 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_V_A_L_U_E_,_ _V_A_L_U_E_> _B_a_s_e; │ │ │ │ │ -54 │ │ │ │ │ -55 VALUE measured_; │ │ │ │ │ -57 public: │ │ │ │ │ -58 │ │ │ │ │ -59 // shorthand for a smart pointer to a factor │ │ │ │ │ -_6_0 typedef typename boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -61 │ │ │ │ │ -64 │ │ │ │ │ -_6_6 _B_e_t_w_e_e_n_F_a_c_t_o_r() {} │ │ │ │ │ -67 │ │ │ │ │ -_6_9 _B_e_t_w_e_e_n_F_a_c_t_o_r(_K_e_y key1, _K_e_y key2, const VALUE& _m_e_a_s_u_r_e_d, │ │ │ │ │ -70 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model = nullptr) : │ │ │ │ │ -71 _B_a_s_e(model, key1, key2), measured_(_m_e_a_s_u_r_e_d) { │ │ │ │ │ -72 } │ │ │ │ │ -73 │ │ │ │ │ -75 │ │ │ │ │ -76 _~_B_e_t_w_e_e_n_F_a_c_t_o_r() override {} │ │ │ │ │ +_5_5 _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t(_K_e_y key1, _K_e_y key2, │ │ │ │ │ +56 const _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x& measuredE, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model) : │ │ │ │ │ +57 _B_a_s_e(model, key1, key2), measuredE_(measuredE) { │ │ │ │ │ +58 } │ │ │ │ │ +59 │ │ │ │ │ +60 _~_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t() override { │ │ │ │ │ +61 } │ │ │ │ │ +62 │ │ │ │ │ +_6_4 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +65 return boost::static_pointer_cast( │ │ │ │ │ +66 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); │ │ │ │ │ +67 } │ │ │ │ │ +68 │ │ │ │ │ +72 void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ +73 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override; │ │ │ │ │ +74 │ │ │ │ │ +76 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& expected, double tol = 1e-9) const │ │ │ │ │ +override; │ │ │ │ │ 77 │ │ │ │ │ -_7_9 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -80 return boost::static_pointer_cast( │ │ │ │ │ -81 gtsam::NonlinearFactor::shared_ptr(new This(*this))); } │ │ │ │ │ -82 │ │ │ │ │ -85 │ │ │ │ │ -_8_7 void _p_r_i_n_t( │ │ │ │ │ -88 const std::string& s = "", │ │ │ │ │ -89 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ -90 std::cout << s << "BetweenFactor(" │ │ │ │ │ -91 << keyFormatter(this->key1()) << "," │ │ │ │ │ -92 << keyFormatter(this->key2()) << ")\n"; │ │ │ │ │ -93 _t_r_a_i_t_s_<_T_>_:_:_P_r_i_n_t(measured_, " measured: "); │ │ │ │ │ -94 this->noiseModel_->print(" noise model: "); │ │ │ │ │ -95 } │ │ │ │ │ -96 │ │ │ │ │ -_9_8 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& expected, double tol=1e-9) const override │ │ │ │ │ -{ │ │ │ │ │ -99 const This *e = dynamic_cast (&expected); │ │ │ │ │ -100 return e != nullptr && _B_a_s_e_:_:_e_q_u_a_l_s(*e, tol) && _t_r_a_i_t_s_<_T_>_:_:_E_q_u_a_l_s(this- │ │ │ │ │ ->measured_, e->measured_, tol); │ │ │ │ │ +81 Vector evaluateError(const _P_o_s_e_3& p1, const _P_o_s_e_3& p2, │ │ │ │ │ +82 boost::optional Hp1 = boost::none, // │ │ │ │ │ +83 boost::optional Hp2 = boost::none) const override; │ │ │ │ │ +84 │ │ │ │ │ +_8_6 const _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x& _m_e_a_s_u_r_e_d() const { │ │ │ │ │ +87 return measuredE_; │ │ │ │ │ +88 } │ │ │ │ │ +89 │ │ │ │ │ +90private: │ │ │ │ │ +91 │ │ │ │ │ +_9_3 friend class boost::serialization::access; │ │ │ │ │ +94 template │ │ │ │ │ +95 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +96 // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility │ │ │ │ │ +97 ar │ │ │ │ │ +98 & boost::serialization::make_nvp("NoiseModelFactor2", │ │ │ │ │ +99 boost::serialization::base_object(*this)); │ │ │ │ │ +100 ar & BOOST_SERIALIZATION_NVP(measuredE_); │ │ │ │ │ 101 } │ │ │ │ │ 102 │ │ │ │ │ -106 │ │ │ │ │ -_1_0_8 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const T& p1, const T& p2, boost::optional H1 │ │ │ │ │ -= │ │ │ │ │ -109 boost::none, boost::optional H2 = boost::none) const override { │ │ │ │ │ -110 T hx = _t_r_a_i_t_s_<_T_>_:_:_B_e_t_w_e_e_n(p1, p2, H1, H2); // h(x) │ │ │ │ │ -111 // manifold equivalent of h(x)-z -> log(z,h(x)) │ │ │ │ │ -112#ifdef GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR │ │ │ │ │ -113 typename _t_r_a_i_t_s_<_T_>_:_:_C_h_a_r_t_J_a_c_o_b_i_a_n_:_:_J_a_c_o_b_i_a_n Hlocal; │ │ │ │ │ -114 Vector rval = _t_r_a_i_t_s_<_T_>_:_:_L_o_c_a_l(measured_, hx, boost::none, (H1 || H2) ? │ │ │ │ │ -&Hlocal : 0); │ │ │ │ │ -115 if (H1) *H1 = Hlocal * (*H1); │ │ │ │ │ -116 if (H2) *H2 = Hlocal * (*H2); │ │ │ │ │ -117 return rval; │ │ │ │ │ -118#else │ │ │ │ │ -119 return _t_r_a_i_t_s_<_T_>_:_:_L_o_c_a_l(measured_, hx); │ │ │ │ │ -120#endif │ │ │ │ │ -121 } │ │ │ │ │ -122 │ │ │ │ │ -126 │ │ │ │ │ -_1_2_8 const VALUE& _m_e_a_s_u_r_e_d() const { │ │ │ │ │ -129 return measured_; │ │ │ │ │ -130 } │ │ │ │ │ -132 │ │ │ │ │ -133 private: │ │ │ │ │ -134 │ │ │ │ │ -_1_3_6 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -137 template │ │ │ │ │ -138 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -139 // NoiseModelFactor2 instead of NoiseModelFactorN for backward │ │ │ │ │ -compatibility │ │ │ │ │ -140 ar & boost::serialization::make_nvp("NoiseModelFactor2", │ │ │ │ │ -141 boost::serialization::base_object(*this)); │ │ │ │ │ -142 ar & BOOST_SERIALIZATION_NVP(measured_); │ │ │ │ │ -143 } │ │ │ │ │ -144 │ │ │ │ │ -145 // Alignment, see https://eigen.tuxfamily.org/dox/ │ │ │ │ │ -group__TopicStructHavingEigenMembers.html │ │ │ │ │ -146 enum { NeedsToAlign = (sizeof(VALUE) % 16) == 0 }; │ │ │ │ │ -147 public: │ │ │ │ │ -148 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W___I_F(NeedsToAlign) │ │ │ │ │ -149 }; // \class BetweenFactor │ │ │ │ │ -150 │ │ │ │ │ -152 template │ │ │ │ │ -_1_5_3 struct _t_r_a_i_t_s<_B_e_t_w_e_e_n_F_a_c_t_o_r > : public _T_e_s_t_a_b_l_e │ │ │ │ │ -> {}; │ │ │ │ │ -154 │ │ │ │ │ -160 template │ │ │ │ │ -_1_6_1 class _B_e_t_w_e_e_n_C_o_n_s_t_r_a_i_n_t : public _B_e_t_w_e_e_n_F_a_c_t_o_r { │ │ │ │ │ -162 public: │ │ │ │ │ -163 typedef boost::shared_ptr > shared_ptr; │ │ │ │ │ -164 │ │ │ │ │ -_1_6_6 _B_e_t_w_e_e_n_C_o_n_s_t_r_a_i_n_t(const VALUE& _m_e_a_s_u_r_e_d, _K_e_y key1, _K_e_y key2, double mu = │ │ │ │ │ -1000.0) : │ │ │ │ │ -167 _B_e_t_w_e_e_n_F_a_c_t_o_r(key1, key2, _m_e_a_s_u_r_e_d, │ │ │ │ │ -168 _n_o_i_s_e_M_o_d_e_l::Constrained::All(_t_r_a_i_t_s::GetDimension(_m_e_a_s_u_r_e_d), std:: │ │ │ │ │ -abs(mu))) │ │ │ │ │ -169 {} │ │ │ │ │ -170 │ │ │ │ │ -171 private: │ │ │ │ │ -172 │ │ │ │ │ -_1_7_4 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -175 template │ │ │ │ │ -176 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -177 ar & boost::serialization::make_nvp("BetweenFactor", │ │ │ │ │ -178 boost::serialization::base_object<_B_e_t_w_e_e_n_F_a_c_t_o_r_<_V_A_L_U_E_> >(*this)); │ │ │ │ │ -179 } │ │ │ │ │ -180 }; // \class BetweenConstraint │ │ │ │ │ -181 │ │ │ │ │ -183 template │ │ │ │ │ -_1_8_4 struct _t_r_a_i_t_s<_B_e_t_w_e_e_n_C_o_n_s_t_r_a_i_n_t > : public │ │ │ │ │ -_T_e_s_t_a_b_l_e > {}; │ │ │ │ │ -185 │ │ │ │ │ -186} │ │ │ │ │ -_L_i_e_._h │ │ │ │ │ -Base class and basic functions for Lie types. │ │ │ │ │ -_T_e_s_t_a_b_l_e_._h │ │ │ │ │ -Concept check for values that can be used in unit tests. │ │ │ │ │ -_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W___I_F │ │ │ │ │ -#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) │ │ │ │ │ +103public: │ │ │ │ │ +104 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +105}; │ │ │ │ │ +106// \class EssentialMatrixConstraint │ │ │ │ │ +107 │ │ │ │ │ +108} │ │ │ │ │ +_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ This marks a GTSAM object to require alignment. │ │ │ │ │ -DDeeffiinniittiioonn types.h:317 │ │ │ │ │ +DDeeffiinniittiioonn types.h:308 │ │ │ │ │ _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ _g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ Aliases. │ │ │ │ │ DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_I_s_L_i_e_G_r_o_u_p │ │ │ │ │ -Lie Group Concept. │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:260 │ │ │ │ │ -_g_t_s_a_m_:_:_I_s_T_e_s_t_a_b_l_e │ │ │ │ │ -A testable concept check that should be placed in applicable unit tests and in │ │ │ │ │ -generic algorithms. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const This &other, double tol=1e-9) const │ │ │ │ │ -check equality │ │ │ │ │ -DDeeffiinniittiioonn Factor.cpp:42 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x │ │ │ │ │ +An essential matrix is like a Pose3, except with translation up to scale It is │ │ │ │ │ +named after the 3*3 ma... │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrix.h:26 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ +A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ Nonlinear factor base class. │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_n_o_i_s_e_M_o_d_e_l │ │ │ │ │ -const SharedNoiseModel & noiseModel() const │ │ │ │ │ -access to the noise model │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:223 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ variables. │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ -_g_t_s_a_m_:_:_B_e_t_w_e_e_n_F_a_c_t_o_r │ │ │ │ │ -A class for a measurement predicted by "between(config[key1],config[key2])". │ │ │ │ │ -DDeeffiinniittiioonn BetweenFactor.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_B_e_t_w_e_e_n_F_a_c_t_o_r_:_:_B_e_t_w_e_e_n_F_a_c_t_o_r │ │ │ │ │ -BetweenFactor() │ │ │ │ │ -default constructor - only use for serialization │ │ │ │ │ -DDeeffiinniittiioonn BetweenFactor.h:66 │ │ │ │ │ -_g_t_s_a_m_:_:_B_e_t_w_e_e_n_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const NonlinearFactor &expected, double tol=1e-9) const override │ │ │ │ │ -assert equality up to a tolerance │ │ │ │ │ -DDeeffiinniittiioonn BetweenFactor.h:98 │ │ │ │ │ -_g_t_s_a_m_:_:_B_e_t_w_e_e_n_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t │ │ │ │ │ +Binary factor between two Pose3 variables induced by an EssentialMatrix │ │ │ │ │ +measurement. │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrixConstraint.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t_:_:_c_l_o_n_e │ │ │ │ │ gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn BetweenFactor.h:79 │ │ │ │ │ -_g_t_s_a_m_:_:_B_e_t_w_e_e_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< BetweenFactor > shared_ptr │ │ │ │ │ -The measurement. │ │ │ │ │ -DDeeffiinniittiioonn BetweenFactor.h:60 │ │ │ │ │ -_g_t_s_a_m_:_:_B_e_t_w_e_e_n_F_a_c_t_o_r_:_:_m_e_a_s_u_r_e_d │ │ │ │ │ -const VALUE & measured() const │ │ │ │ │ -return the measurement │ │ │ │ │ -DDeeffiinniittiioonn BetweenFactor.h:128 │ │ │ │ │ -_g_t_s_a_m_:_:_B_e_t_w_e_e_n_F_a_c_t_o_r_:_:_B_e_t_w_e_e_n_F_a_c_t_o_r │ │ │ │ │ -BetweenFactor(Key key1, Key key2, const VALUE &measured, const SharedNoiseModel │ │ │ │ │ -&model=nullptr) │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrixConstraint.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t_:_:_m_e_a_s_u_r_e_d │ │ │ │ │ +const EssentialMatrix & measured() const │ │ │ │ │ +return the measured │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrixConstraint.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< EssentialMatrixConstraint > shared_ptr │ │ │ │ │ +The measurement is an essential matrix. │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrixConstraint.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t │ │ │ │ │ +EssentialMatrixConstraint() │ │ │ │ │ +default constructor - only use for serialization │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrixConstraint.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t │ │ │ │ │ +EssentialMatrixConstraint(Key key1, Key key2, const EssentialMatrix &measuredE, │ │ │ │ │ +const SharedNoiseModel &model) │ │ │ │ │ Constructor. │ │ │ │ │ -DDeeffiinniittiioonn BetweenFactor.h:69 │ │ │ │ │ -_g_t_s_a_m_:_:_B_e_t_w_e_e_n_F_a_c_t_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn BetweenFactor.h:136 │ │ │ │ │ -_g_t_s_a_m_:_:_B_e_t_w_e_e_n_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ -Vector evaluateError(const T &p1, const T &p2, boost::optional< Matrix & > │ │ │ │ │ -H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override │ │ │ │ │ -evaluate error, returns vector of errors size of tangent space │ │ │ │ │ -DDeeffiinniittiioonn BetweenFactor.h:108 │ │ │ │ │ -_g_t_s_a_m_:_:_B_e_t_w_e_e_n_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -print with optional string │ │ │ │ │ -DDeeffiinniittiioonn BetweenFactor.h:87 │ │ │ │ │ -_g_t_s_a_m_:_:_B_e_t_w_e_e_n_C_o_n_s_t_r_a_i_n_t │ │ │ │ │ -Binary between constraint - forces between to a given value This constraint │ │ │ │ │ -requires the underlying t... │ │ │ │ │ -DDeeffiinniittiioonn BetweenFactor.h:161 │ │ │ │ │ -_g_t_s_a_m_:_:_B_e_t_w_e_e_n_C_o_n_s_t_r_a_i_n_t_:_:_B_e_t_w_e_e_n_C_o_n_s_t_r_a_i_n_t │ │ │ │ │ -BetweenConstraint(const VALUE &measured, Key key1, Key key2, double mu=1000.0) │ │ │ │ │ -Syntactic sugar for constrained version. │ │ │ │ │ -DDeeffiinniittiioonn BetweenFactor.h:166 │ │ │ │ │ -_g_t_s_a_m_:_:_B_e_t_w_e_e_n_C_o_n_s_t_r_a_i_n_t_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn BetweenFactor.h:174 │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrixConstraint.h:55 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _B_e_t_w_e_e_n_F_a_c_t_o_r_._h │ │ │ │ │ + * _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01280.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/GeneralSFMFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/BoundingConstraint.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,50 +96,45 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
GeneralSFMFactor.h File Reference
│ │ │ │ +
BoundingConstraint.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

a general SFM factor with an unknown calibration │ │ │ │ +

Provides partially implemented constraints to implement bounds. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::GeneralSFMFactor< CAMERA, LANDMARK >
 Non-linear factor for a constraint derived from a 2D measurement. More...
struct  gtsam::BoundingConstraint1< VALUE >
 Unary inequality constraint forcing a scalar to be greater/less than a fixed threshold. More...
 
struct  gtsam::traits< GeneralSFMFactor< CAMERA, LANDMARK > >
 
class  gtsam::GeneralSFMFactor2< CALIBRATION >
 Non-linear factor for a constraint derived from a 2D measurement. More...
 
struct  gtsam::traits< GeneralSFMFactor2< CALIBRATION > >
struct  gtsam::BoundingConstraint2< VALUE1, VALUE2 >
 Binary scalar inequality constraint, with a similar value() function to implement for specific systems. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

a general SFM factor with an unknown calibration

│ │ │ │ -
Date
Dec 15, 2010
│ │ │ │ -
Author
Kai Ni
│ │ │ │ +

Provides partially implemented constraints to implement bounds.

│ │ │ │ +
Author
Alex Cunningham
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,36 +1,30 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -GeneralSFMFactor.h File Reference │ │ │ │ │ -a general SFM factor with an unknown calibration _M_o_r_e_._._. │ │ │ │ │ +BoundingConstraint.h File Reference │ │ │ │ │ +Provides partially implemented constraints to implement bounds. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_<_ _C_A_M_E_R_A_,_ _L_A_N_D_M_A_R_K_ _> │ │ │ │ │ -  Non-linear factor for a constraint derived from a 2D measurement. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_1_<_ _V_A_L_U_E_ _> │ │ │ │ │ +  Unary inequality constraint forcing a scalar to be greater/less than a │ │ │ │ │ + fixed threshold. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_<_ _C_A_M_E_R_A_,_ _L_A_N_D_M_A_R_K_ _>_ _> │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_<_ _C_A_L_I_B_R_A_T_I_O_N_ _> │ │ │ │ │ -  Non-linear factor for a constraint derived from a 2D measurement. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_<_ _C_A_L_I_B_R_A_T_I_O_N_ _>_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_2_<_ _V_A_L_U_E_1_,_ _V_A_L_U_E_2_ _> │ │ │ │ │ +  Binary scalar inequality constraint, with a similar _v_a_l_u_e_(_) function │ │ │ │ │ + to implement for specific systems. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -a general SFM factor with an unknown calibration │ │ │ │ │ - Date │ │ │ │ │ - Dec 15, 2010 │ │ │ │ │ +Provides partially implemented constraints to implement bounds. │ │ │ │ │ Author │ │ │ │ │ - Kai Ni │ │ │ │ │ + Alex Cunningham │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_._h │ │ │ │ │ + * _B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01280.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,5 +1,3 @@ │ │ │ │ │ var a01280 = [ │ │ │ │ │ - ["gtsam::traits< GeneralSFMFactor< CAMERA, LANDMARK > >", "a04844.html", null], │ │ │ │ │ - ["gtsam::GeneralSFMFactor2< CALIBRATION >", "a04848.html", "a04848"], │ │ │ │ │ - ["gtsam::traits< GeneralSFMFactor2< CALIBRATION > >", "a04852.html", null] │ │ │ │ │ + ["gtsam::BoundingConstraint2< VALUE1, VALUE2 >", "a04784.html", "a04784"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01280_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/GeneralSFMFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/BoundingConstraint.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,346 +98,193 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
GeneralSFMFactor.h
│ │ │ │ +
BoundingConstraint.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
21#pragma once
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/base/Lie.h>
│ │ │ │ +
21#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │
22
│ │ │ │ -
23#include <gtsam/geometry/PinholeCamera.h>
│ │ │ │ -
24#include <gtsam/geometry/Point2.h>
│ │ │ │ -
25#include <gtsam/geometry/Point3.h>
│ │ │ │ -
26#include <gtsam/geometry/Pose3.h>
│ │ │ │ -
27#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
28#include <gtsam/linear/BinaryJacobianFactor.h>
│ │ │ │ -
29#include <gtsam/linear/NoiseModel.h>
│ │ │ │ -
30#include <gtsam/base/concepts.h>
│ │ │ │ -
31#include <gtsam/base/Manifold.h>
│ │ │ │ -
32#include <gtsam/base/Matrix.h>
│ │ │ │ -
33#include <gtsam/base/SymmetricBlockMatrix.h>
│ │ │ │ -
34#include <gtsam/base/types.h>
│ │ │ │ -
35#include <gtsam/base/Testable.h>
│ │ │ │ -
36#include <gtsam/base/Vector.h>
│ │ │ │ -
37#include <gtsam/base/timing.h>
│ │ │ │ -
38
│ │ │ │ -
39#include <boost/none.hpp>
│ │ │ │ -
40#include <boost/optional/optional.hpp>
│ │ │ │ -
41#include <boost/serialization/nvp.hpp>
│ │ │ │ -
42#include <boost/smart_ptr/shared_ptr.hpp>
│ │ │ │ -
43#include <iostream>
│ │ │ │ -
44#include <string>
│ │ │ │ -
45
│ │ │ │ -
46namespace boost {
│ │ │ │ -
47namespace serialization {
│ │ │ │ -
48class access;
│ │ │ │ -
49} /* namespace serialization */
│ │ │ │ -
50} /* namespace boost */
│ │ │ │ +
23namespace gtsam {
│ │ │ │ +
24
│ │ │ │ +
32template<class VALUE>
│ │ │ │ +
│ │ │ │ +
33struct BoundingConstraint1: public NoiseModelFactorN<VALUE> {
│ │ │ │ +
34 typedef VALUE X;
│ │ │ │ +
35 typedef NoiseModelFactorN<VALUE> Base;
│ │ │ │ +
36 typedef boost::shared_ptr<BoundingConstraint1<VALUE> > shared_ptr;
│ │ │ │ +
37
│ │ │ │ +
38 double threshold_;
│ │ │ │ +
39 bool isGreaterThan_;
│ │ │ │ +
40
│ │ │ │ +
│ │ │ │ +
41 BoundingConstraint1(Key key, double threshold,
│ │ │ │ +
42 bool isGreaterThan, double mu = 1000.0) :
│ │ │ │ +
43 Base(noiseModel::Constrained::All(1, mu), key),
│ │ │ │ +
44 threshold_(threshold), isGreaterThan_(isGreaterThan) {
│ │ │ │ +
45 }
│ │ │ │ +
│ │ │ │ +
46
│ │ │ │ +
47 ~BoundingConstraint1() override {}
│ │ │ │ +
48
│ │ │ │ +
49 inline double threshold() const { return threshold_; }
│ │ │ │ +
50 inline bool isGreaterThan() const { return isGreaterThan_; }
│ │ │ │
51
│ │ │ │ -
52namespace gtsam {
│ │ │ │ -
53
│ │ │ │ -
59template<class CAMERA, class LANDMARK>
│ │ │ │ -
│ │ │ │ -
60class GeneralSFMFactor: public NoiseModelFactorN<CAMERA, LANDMARK> {
│ │ │ │ -
61
│ │ │ │ -
62 GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA)
│ │ │ │ -
63 GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK)
│ │ │ │ -
64
│ │ │ │ -
65 static const int DimC = FixedDimension<CAMERA>::value;
│ │ │ │ -
66 static const int DimL = FixedDimension<LANDMARK>::value;
│ │ │ │ -
67 typedef Eigen::Matrix<double, 2, DimC> JacobianC;
│ │ │ │ -
68 typedef Eigen::Matrix<double, 2, DimL> JacobianL;
│ │ │ │ -
69
│ │ │ │ -
70protected:
│ │ │ │ -
71
│ │ │ │ -
72 Point2 measured_;
│ │ │ │ -
73
│ │ │ │ -
74public:
│ │ │ │ +
57 virtual double value(const X& x, boost::optional<Matrix&> H =
│ │ │ │ +
58 boost::none) const = 0;
│ │ │ │ +
59
│ │ │ │ +
│ │ │ │ +
61 bool active(const Values& c) const override {
│ │ │ │ +
62 // note: still active at equality to avoid zigzagging
│ │ │ │ +
63 double x = value(c.at<X>(this->key()));
│ │ │ │ +
64 return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
│ │ │ │ +
65 }
│ │ │ │ +
│ │ │ │ +
66
│ │ │ │ +
67 Vector evaluateError(const X& x, boost::optional<Matrix&> H =
│ │ │ │ +
68 boost::none) const override {
│ │ │ │ +
69 Matrix D;
│ │ │ │ +
70 double error = value(x, D) - threshold_;
│ │ │ │ +
71 if (H) {
│ │ │ │ +
72 if (isGreaterThan_) *H = D;
│ │ │ │ +
73 else *H = -1.0 * D;
│ │ │ │ +
74 }
│ │ │ │
75
│ │ │ │ -
76 typedef GeneralSFMFactor<CAMERA, LANDMARK> This;
│ │ │ │ -
77 typedef NoiseModelFactorN<CAMERA, LANDMARK> Base;
│ │ │ │ -
78
│ │ │ │ -
79 // shorthand for a smart pointer to a factor
│ │ │ │ -
80 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
76 if (isGreaterThan_)
│ │ │ │ +
77 return (Vector(1) << error).finished();
│ │ │ │ +
78 else
│ │ │ │ +
79 return -1.0 * (Vector(1) << error).finished();
│ │ │ │ +
80 }
│ │ │ │
81
│ │ │ │ -
│ │ │ │ -
89 GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model,
│ │ │ │ -
90 Key cameraKey, Key landmarkKey)
│ │ │ │ -
91 : Base(model, cameraKey, landmarkKey), measured_(measured) {}
│ │ │ │ -
│ │ │ │ -
92
│ │ │ │ -
93 GeneralSFMFactor() : measured_(0.0, 0.0) {}
│ │ │ │ -
95 GeneralSFMFactor(const Point2& p) : measured_(p) {}
│ │ │ │ -
97 GeneralSFMFactor(double x, double y) : measured_(x, y) {}
│ │ │ │ -
98
│ │ │ │ -
99 ~GeneralSFMFactor() override {}
│ │ │ │ -
100
│ │ │ │ -
102 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
103 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ -
104 gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
│ │ │ │ -
105
│ │ │ │ -
111 void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ -
112 Base::print(s, keyFormatter);
│ │ │ │ -
113 traits<Point2>::Print(measured_, s + ".z");
│ │ │ │ -
114 }
│ │ │ │ +
82private:
│ │ │ │ +
83
│ │ │ │ +
85 friend class boost::serialization::access;
│ │ │ │ +
86 template<class ARCHIVE>
│ │ │ │ +
87 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
88 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ +
89 ar & boost::serialization::make_nvp("NoiseModelFactor1",
│ │ │ │ +
90 boost::serialization::base_object<Base>(*this));
│ │ │ │ +
91 ar & BOOST_SERIALIZATION_NVP(threshold_);
│ │ │ │ +
92 ar & BOOST_SERIALIZATION_NVP(isGreaterThan_);
│ │ │ │ +
93 }
│ │ │ │ +
94};
│ │ │ │ +
│ │ │ │ +
95
│ │ │ │ +
100template<class VALUE1, class VALUE2>
│ │ │ │ +
│ │ │ │ +
101struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> {
│ │ │ │ +
102 typedef VALUE1 X1;
│ │ │ │ +
103 typedef VALUE2 X2;
│ │ │ │ +
104
│ │ │ │ +
105 typedef NoiseModelFactorN<VALUE1, VALUE2> Base;
│ │ │ │ +
106 typedef boost::shared_ptr<BoundingConstraint2<VALUE1, VALUE2> > shared_ptr;
│ │ │ │ +
107
│ │ │ │ +
108 double threshold_;
│ │ │ │ +
109 bool isGreaterThan_;
│ │ │ │ +
110
│ │ │ │ +
│ │ │ │ +
111 BoundingConstraint2(Key key1, Key key2, double threshold,
│ │ │ │ +
112 bool isGreaterThan, double mu = 1000.0)
│ │ │ │ +
113 : Base(noiseModel::Constrained::All(1, mu), key1, key2),
│ │ │ │ +
114 threshold_(threshold), isGreaterThan_(isGreaterThan) {}
│ │ │ │ +
│ │ │ │
115
│ │ │ │ -
119 bool equals(const NonlinearFactor &p, double tol = 1e-9) const override {
│ │ │ │ -
120 const This* e = dynamic_cast<const This*>(&p);
│ │ │ │ -
121 return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol);
│ │ │ │ -
122 }
│ │ │ │ -
123
│ │ │ │ -
125 Vector evaluateError(const CAMERA& camera, const LANDMARK& point,
│ │ │ │ -
126 boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const override {
│ │ │ │ -
127 try {
│ │ │ │ -
128 return camera.project2(point,H1,H2) - measured_;
│ │ │ │ -
129 }
│ │ │ │ -
130 catch( CheiralityException& e) {
│ │ │ │ -
131 if (H1) *H1 = JacobianC::Zero();
│ │ │ │ -
132 if (H2) *H2 = JacobianL::Zero();
│ │ │ │ -
133 //TODO Print the exception via logging
│ │ │ │ -
134 return Z_2x1;
│ │ │ │ -
135 }
│ │ │ │ -
136 }
│ │ │ │ -
137
│ │ │ │ -
139 boost::shared_ptr<GaussianFactor> linearize(const Values& values) const override {
│ │ │ │ -
140 // Only linearize if the factor is active
│ │ │ │ -
141 if (!this->active(values)) return boost::shared_ptr<JacobianFactor>();
│ │ │ │ -
142
│ │ │ │ -
143 const Key key1 = this->key1(), key2 = this->key2();
│ │ │ │ -
144 JacobianC H1;
│ │ │ │ -
145 JacobianL H2;
│ │ │ │ -
146 Vector2 b;
│ │ │ │ -
147 try {
│ │ │ │ -
148 const CAMERA& camera = values.at<CAMERA>(key1);
│ │ │ │ -
149 const LANDMARK& point = values.at<LANDMARK>(key2);
│ │ │ │ -
150 b = measured() - camera.project2(point, H1, H2);
│ │ │ │ -
151 } catch (CheiralityException& e) {
│ │ │ │ -
152 H1.setZero();
│ │ │ │ -
153 H2.setZero();
│ │ │ │ -
154 b.setZero();
│ │ │ │ -
155 //TODO Print the exception via logging
│ │ │ │ -
156 }
│ │ │ │ +
116 ~BoundingConstraint2() override {}
│ │ │ │ +
117
│ │ │ │ +
118 inline double threshold() const { return threshold_; }
│ │ │ │ +
119 inline bool isGreaterThan() const { return isGreaterThan_; }
│ │ │ │ +
120
│ │ │ │ +
125 virtual double value(const X1& x1, const X2& x2,
│ │ │ │ +
126 boost::optional<Matrix&> H1 = boost::none,
│ │ │ │ +
127 boost::optional<Matrix&> H2 = boost::none) const = 0;
│ │ │ │ +
128
│ │ │ │ +
│ │ │ │ +
130 bool active(const Values& c) const override {
│ │ │ │ +
131 // note: still active at equality to avoid zigzagging
│ │ │ │ +
132 double x = value(c.at<X1>(this->key1()), c.at<X2>(this->key2()));
│ │ │ │ +
133 return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
│ │ │ │ +
134 }
│ │ │ │ +
│ │ │ │ +
135
│ │ │ │ +
136 Vector evaluateError(const X1& x1, const X2& x2,
│ │ │ │ +
137 boost::optional<Matrix&> H1 = boost::none,
│ │ │ │ +
138 boost::optional<Matrix&> H2 = boost::none) const override {
│ │ │ │ +
139 Matrix D1, D2;
│ │ │ │ +
140 double error = value(x1, x2, D1, D2) - threshold_;
│ │ │ │ +
141 if (H1) {
│ │ │ │ +
142 if (isGreaterThan_) *H1 = D1;
│ │ │ │ +
143 else *H1 = -1.0 * D1;
│ │ │ │ +
144 }
│ │ │ │ +
145 if (H2) {
│ │ │ │ +
146 if (isGreaterThan_) *H2 = D2;
│ │ │ │ +
147 else *H2 = -1.0 * D2;
│ │ │ │ +
148 }
│ │ │ │ +
149
│ │ │ │ +
150 if (isGreaterThan_)
│ │ │ │ +
151 return (Vector(1) << error).finished();
│ │ │ │ +
152 else
│ │ │ │ +
153 return -1.0 * (Vector(1) << error).finished();
│ │ │ │ +
154 }
│ │ │ │ +
155
│ │ │ │ +
156private:
│ │ │ │
157
│ │ │ │ -
158 // Whiten the system if needed
│ │ │ │ -
159 const SharedNoiseModel& noiseModel = this->noiseModel();
│ │ │ │ -
160 if (noiseModel && !noiseModel->isUnit()) {
│ │ │ │ -
161 // TODO: implement WhitenSystem for fixed size matrices and include
│ │ │ │ -
162 // above
│ │ │ │ -
163 H1 = noiseModel->Whiten(H1);
│ │ │ │ -
164 H2 = noiseModel->Whiten(H2);
│ │ │ │ -
165 b = noiseModel->Whiten(b);
│ │ │ │ -
166 }
│ │ │ │ -
167
│ │ │ │ -
168 // Create new (unit) noiseModel, preserving constraints if applicable
│ │ │ │ -
169 SharedDiagonal model;
│ │ │ │ -
170 if (noiseModel && noiseModel->isConstrained()) {
│ │ │ │ -
171 model = boost::static_pointer_cast<noiseModel::Constrained>(noiseModel)->unit();
│ │ │ │ -
172 }
│ │ │ │ -
173
│ │ │ │ -
174 return boost::make_shared<BinaryJacobianFactor<2, DimC, DimL> >(key1, H1, key2, H2, b, model);
│ │ │ │ -
175 }
│ │ │ │ -
176
│ │ │ │ -
178 inline const Point2 measured() const {
│ │ │ │ -
179 return measured_;
│ │ │ │ -
180 }
│ │ │ │ -
181
│ │ │ │ -
182private:
│ │ │ │ -
184 friend class boost::serialization::access;
│ │ │ │ -
185 template<class Archive>
│ │ │ │ -
186 void serialize(Archive & ar, const unsigned int /*version*/) {
│ │ │ │ -
187 // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ -
188 ar & boost::serialization::make_nvp("NoiseModelFactor2",
│ │ │ │ -
189 boost::serialization::base_object<Base>(*this));
│ │ │ │ -
190 ar & BOOST_SERIALIZATION_NVP(measured_);
│ │ │ │ -
191 }
│ │ │ │ -
192};
│ │ │ │ -
│ │ │ │ -
193
│ │ │ │ -
194template<class CAMERA, class LANDMARK>
│ │ │ │ -
│ │ │ │ -
195struct traits<GeneralSFMFactor<CAMERA, LANDMARK> > : Testable<
│ │ │ │ -
196 GeneralSFMFactor<CAMERA, LANDMARK> > {
│ │ │ │ -
197};
│ │ │ │ -
│ │ │ │ -
198
│ │ │ │ -
203template<class CALIBRATION>
│ │ │ │ -
│ │ │ │ -
204class GeneralSFMFactor2: public NoiseModelFactorN<Pose3, Point3, CALIBRATION> {
│ │ │ │ -
205
│ │ │ │ -
206 GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
│ │ │ │ -
207 static const int DimK = FixedDimension<CALIBRATION>::value;
│ │ │ │ -
208
│ │ │ │ -
209protected:
│ │ │ │ -
210
│ │ │ │ -
211 Point2 measured_;
│ │ │ │ -
212
│ │ │ │ -
213public:
│ │ │ │ -
214
│ │ │ │ -
215 typedef GeneralSFMFactor2<CALIBRATION> This;
│ │ │ │ -
216 typedef PinholeCamera<CALIBRATION> Camera;
│ │ │ │ -
217 typedef NoiseModelFactorN<Pose3, Point3, CALIBRATION> Base;
│ │ │ │ -
218
│ │ │ │ -
219 // shorthand for a smart pointer to a factor
│ │ │ │ -
220 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
221
│ │ │ │ -
│ │ │ │ -
230 GeneralSFMFactor2(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, Key calibKey) :
│ │ │ │ -
231 Base(model, poseKey, landmarkKey, calibKey), measured_(measured) {}
│ │ │ │ -
│ │ │ │ -
232 GeneralSFMFactor2():measured_(0.0,0.0) {}
│ │ │ │ -
233
│ │ │ │ -
│ │ │ │ -
234 ~GeneralSFMFactor2() override {}
│ │ │ │ -
235
│ │ │ │ -
│ │ │ │ -
237 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
238 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ -
239 gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
│ │ │ │ -
│ │ │ │ -
240
│ │ │ │ -
│ │ │ │ -
246 void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ -
247 Base::print(s, keyFormatter);
│ │ │ │ -
248 traits<Point2>::Print(measured_, s + ".z");
│ │ │ │ -
249 }
│ │ │ │ -
│ │ │ │ -
250
│ │ │ │ -
│ │ │ │ -
254 bool equals(const NonlinearFactor &p, double tol = 1e-9) const override {
│ │ │ │ -
255 const This* e = dynamic_cast<const This*>(&p);
│ │ │ │ -
256 return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol);
│ │ │ │ -
257 }
│ │ │ │ -
│ │ │ │ -
258
│ │ │ │ -
│ │ │ │ -
260 Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib,
│ │ │ │ -
261 boost::optional<Matrix&> H1=boost::none,
│ │ │ │ -
262 boost::optional<Matrix&> H2=boost::none,
│ │ │ │ -
263 boost::optional<Matrix&> H3=boost::none) const override
│ │ │ │ -
264 {
│ │ │ │ -
265 try {
│ │ │ │ -
266 Camera camera(pose3,calib);
│ │ │ │ -
267 return camera.project(point, H1, H2, H3) - measured_;
│ │ │ │ -
268 }
│ │ │ │ -
269 catch( CheiralityException& e) {
│ │ │ │ -
270 if (H1) *H1 = Matrix::Zero(2, 6);
│ │ │ │ -
271 if (H2) *H2 = Matrix::Zero(2, 3);
│ │ │ │ -
272 if (H3) *H3 = Matrix::Zero(2, DimK);
│ │ │ │ -
273 std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
│ │ │ │ -
274 << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
│ │ │ │ -
275 }
│ │ │ │ -
276 return Z_2x1;
│ │ │ │ -
277 }
│ │ │ │ -
│ │ │ │ -
278
│ │ │ │ -
│ │ │ │ -
280 inline const Point2 measured() const {
│ │ │ │ -
281 return measured_;
│ │ │ │ -
282 }
│ │ │ │ -
│ │ │ │ -
283
│ │ │ │ -
284private:
│ │ │ │ -
286 friend class boost::serialization::access;
│ │ │ │ -
287 template<class Archive>
│ │ │ │ -
288 void serialize(Archive & ar, const unsigned int /*version*/) {
│ │ │ │ -
289 // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ -
290 ar & boost::serialization::make_nvp("NoiseModelFactor3",
│ │ │ │ -
291 boost::serialization::base_object<Base>(*this));
│ │ │ │ -
292 ar & BOOST_SERIALIZATION_NVP(measured_);
│ │ │ │ -
293 }
│ │ │ │ -
294};
│ │ │ │ -
295
│ │ │ │ -
296template<class CALIBRATION>
│ │ │ │ -
│ │ │ │ -
297struct traits<GeneralSFMFactor2<CALIBRATION> > : Testable<
│ │ │ │ -
298 GeneralSFMFactor2<CALIBRATION> > {
│ │ │ │ -
299};
│ │ │ │ -
│ │ │ │ -
300
│ │ │ │ -
301} //namespace
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ -
timing.h
Timing utilities.
│ │ │ │ -
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ -
types.h
Typedefs for easier changing of types.
│ │ │ │ -
SymmetricBlockMatrix.h
Access to matrices via blocks of pre-defined sizes.
│ │ │ │ -
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ -
PinholeCamera.h
Base class for all pinhole cameras.
│ │ │ │ -
Pose3.h
3D Pose
│ │ │ │ -
Point3.h
3D Point
│ │ │ │ -
Point2.h
2D Point
│ │ │ │ - │ │ │ │ -
BinaryJacobianFactor.h
A binary JacobianFactor specialization that uses fixed matrix math for speed.
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
159 friend class boost::serialization::access;
│ │ │ │ +
160 template<class ARCHIVE>
│ │ │ │ +
161 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
162 // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ +
163 ar & boost::serialization::make_nvp("NoiseModelFactor2",
│ │ │ │ +
164 boost::serialization::base_object<Base>(*this));
│ │ │ │ +
165 ar & BOOST_SERIALIZATION_NVP(threshold_);
│ │ │ │ +
166 ar & BOOST_SERIALIZATION_NVP(isGreaterThan_);
│ │ │ │ +
167 }
│ │ │ │ +
168};
│ │ │ │ +
│ │ │ │ +
169
│ │ │ │ +
170} // \namespace gtsam
│ │ │ │ +
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │ -
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ -
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::FixedDimension
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition Manifold.h:164
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::CheiralityException
Definition CalibratedCamera.h:32
│ │ │ │ -
gtsam::PinholeCamera
A pinhole camera class that has a Pose3 and a Calibration.
Definition PinholeCamera.h:33
│ │ │ │ -
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ -
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
│ │ │ │ -
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
│ │ │ │ -
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ -
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition NonlinearFactor.h:118
│ │ │ │ +
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Calculate the error of the factor.
Definition NonlinearFactor.cpp:138
│ │ │ │
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition NonlinearFactor.h:223
│ │ │ │
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ -
gtsam::GeneralSFMFactor
Non-linear factor for a constraint derived from a 2D measurement.
Definition GeneralSFMFactor.h:60
│ │ │ │ -
gtsam::GeneralSFMFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &values) const override
Linearize using fixed-size matrices.
Definition GeneralSFMFactor.h:138
│ │ │ │ -
gtsam::GeneralSFMFactor::evaluateError
Vector evaluateError(const CAMERA &camera, const LANDMARK &point, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
h(x)-z
Definition GeneralSFMFactor.h:124
│ │ │ │ -
gtsam::GeneralSFMFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition GeneralSFMFactor.h:101
│ │ │ │ -
gtsam::GeneralSFMFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition GeneralSFMFactor.h:118
│ │ │ │ -
gtsam::GeneralSFMFactor::measured
const Point2 measured() const
return the measured
Definition GeneralSFMFactor.h:177
│ │ │ │ -
gtsam::GeneralSFMFactor::GeneralSFMFactor
GeneralSFMFactor(const Point2 &measured, const SharedNoiseModel &model, Key cameraKey, Key landmarkKey)
Constructor.
Definition GeneralSFMFactor.h:89
│ │ │ │ -
gtsam::GeneralSFMFactor::GeneralSFMFactor
GeneralSFMFactor()
default constructor
Definition GeneralSFMFactor.h:93
│ │ │ │ -
gtsam::GeneralSFMFactor::This
GeneralSFMFactor< CAMERA, LANDMARK > This
typedef for this object
Definition GeneralSFMFactor.h:76
│ │ │ │ -
gtsam::GeneralSFMFactor::~GeneralSFMFactor
~GeneralSFMFactor() override
destructor
Definition GeneralSFMFactor.h:98
│ │ │ │ -
gtsam::GeneralSFMFactor::Base
NoiseModelFactorN< CAMERA, LANDMARK > Base
typedef for the base class
Definition GeneralSFMFactor.h:77
│ │ │ │ -
gtsam::GeneralSFMFactor::print
void print(const std::string &s="SFMFactor", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition GeneralSFMFactor.h:110
│ │ │ │ -
gtsam::GeneralSFMFactor::access
friend class boost::serialization::access
Serialization function.
Definition GeneralSFMFactor.h:183
│ │ │ │ -
gtsam::GeneralSFMFactor::measured_
Point2 measured_
the 2D measurement
Definition GeneralSFMFactor.h:72
│ │ │ │ -
gtsam::GeneralSFMFactor2
Non-linear factor for a constraint derived from a 2D measurement.
Definition GeneralSFMFactor.h:204
│ │ │ │ -
gtsam::GeneralSFMFactor2::GeneralSFMFactor2
GeneralSFMFactor2()
default constructor
Definition GeneralSFMFactor.h:232
│ │ │ │ -
gtsam::GeneralSFMFactor2::~GeneralSFMFactor2
~GeneralSFMFactor2() override
destructor
Definition GeneralSFMFactor.h:234
│ │ │ │ -
gtsam::GeneralSFMFactor2::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition GeneralSFMFactor.h:237
│ │ │ │ -
gtsam::GeneralSFMFactor2::Base
NoiseModelFactorN< Pose3, Point3, CALIBRATION > Base
typedef for the base class
Definition GeneralSFMFactor.h:217
│ │ │ │ -
gtsam::GeneralSFMFactor2::evaluateError
Vector evaluateError(const Pose3 &pose3, const Point3 &point, const CALIBRATION &calib, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none) const override
h(x)-z
Definition GeneralSFMFactor.h:260
│ │ │ │ -
gtsam::GeneralSFMFactor2::print
void print(const std::string &s="SFMFactor2", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition GeneralSFMFactor.h:246
│ │ │ │ -
gtsam::GeneralSFMFactor2::measured_
Point2 measured_
the 2D measurement
Definition GeneralSFMFactor.h:211
│ │ │ │ -
gtsam::GeneralSFMFactor2::measured
const Point2 measured() const
return the measured
Definition GeneralSFMFactor.h:280
│ │ │ │ -
gtsam::GeneralSFMFactor2::Camera
PinholeCamera< CALIBRATION > Camera
typedef for camera type
Definition GeneralSFMFactor.h:216
│ │ │ │ -
gtsam::GeneralSFMFactor2::GeneralSFMFactor2
GeneralSFMFactor2(const Point2 &measured, const SharedNoiseModel &model, Key poseKey, Key landmarkKey, Key calibKey)
Constructor.
Definition GeneralSFMFactor.h:230
│ │ │ │ -
gtsam::GeneralSFMFactor2::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition GeneralSFMFactor.h:254
│ │ │ │ -
gtsam::GeneralSFMFactor2::access
friend class boost::serialization::access
Serialization function.
Definition GeneralSFMFactor.h:286
│ │ │ │ -
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │ +
gtsam::NoiseModelFactorN< VALUE >::key
Key key() const
Returns a key.
Definition NonlinearFactor.h:518
│ │ │ │ +
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
gtsam::Values::at
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
│ │ │ │ +
gtsam::BoundingConstraint1
Unary inequality constraint forcing a scalar to be greater/less than a fixed threshold.
Definition BoundingConstraint.h:33
│ │ │ │ +
gtsam::BoundingConstraint1::active
bool active(const Values &c) const override
active when constraint NOT met
Definition BoundingConstraint.h:61
│ │ │ │ +
gtsam::BoundingConstraint1::BoundingConstraint1
BoundingConstraint1(Key key, double threshold, bool isGreaterThan, double mu=1000.0)
flag for greater/less than
Definition BoundingConstraint.h:41
│ │ │ │ +
gtsam::BoundingConstraint1::access
friend class boost::serialization::access
Serialization function.
Definition BoundingConstraint.h:85
│ │ │ │ +
gtsam::BoundingConstraint1::value
virtual double value(const X &x, boost::optional< Matrix & > H=boost::none) const =0
function producing a scalar value to compare to the threshold Must have optional argument for derivat...
│ │ │ │ +
gtsam::BoundingConstraint2
Binary scalar inequality constraint, with a similar value() function to implement for specific system...
Definition BoundingConstraint.h:101
│ │ │ │ +
gtsam::BoundingConstraint2::active
bool active(const Values &c) const override
active when constraint NOT met
Definition BoundingConstraint.h:130
│ │ │ │ +
gtsam::BoundingConstraint2::BoundingConstraint2
BoundingConstraint2(Key key1, Key key2, double threshold, bool isGreaterThan, double mu=1000.0)
flag for greater/less than
Definition BoundingConstraint.h:111
│ │ │ │ +
gtsam::BoundingConstraint2::access
friend class boost::serialization::access
Serialization function.
Definition BoundingConstraint.h:159
│ │ │ │ +
gtsam::BoundingConstraint2::value
virtual double value(const X1 &x1, const X2 &x2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const =0
function producing a scalar value to compare to the threshold Must have optional argument for derivat...
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,477 +1,232 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GeneralSFMFactor.h │ │ │ │ │ +BoundingConstraint.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -21#pragma once │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_b_a_s_e_/_L_i_e_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ 22 │ │ │ │ │ -23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_i_n_h_o_l_e_C_a_m_e_r_a_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_3_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_3_._h> │ │ │ │ │ -27#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -28#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_B_i_n_a_r_y_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ -29#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_N_o_i_s_e_M_o_d_e_l_._h> │ │ │ │ │ -30#include │ │ │ │ │ -31#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ -32#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ -33#include <_g_t_s_a_m_/_b_a_s_e_/_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_._h> │ │ │ │ │ -34#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ -35#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ -36#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_c_t_o_r_._h> │ │ │ │ │ -37#include <_g_t_s_a_m_/_b_a_s_e_/_t_i_m_i_n_g_._h> │ │ │ │ │ -38 │ │ │ │ │ -39#include │ │ │ │ │ -40#include │ │ │ │ │ -41#include │ │ │ │ │ -42#include │ │ │ │ │ -43#include │ │ │ │ │ -44#include │ │ │ │ │ -45 │ │ │ │ │ -46namespace boost { │ │ │ │ │ -47namespace serialization { │ │ │ │ │ -48class access; │ │ │ │ │ -49} /* namespace serialization */ │ │ │ │ │ -50} /* namespace boost */ │ │ │ │ │ +23namespace _g_t_s_a_m { │ │ │ │ │ +24 │ │ │ │ │ +32template │ │ │ │ │ +_3_3struct _B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_1: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +34 typedef VALUE X; │ │ │ │ │ +35 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_V_A_L_U_E_> _B_a_s_e; │ │ │ │ │ +36 typedef boost::shared_ptr > shared_ptr; │ │ │ │ │ +37 │ │ │ │ │ +38 double threshold_; │ │ │ │ │ +39 bool isGreaterThan_; │ │ │ │ │ +40 │ │ │ │ │ +_4_1 _B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_1(_K_e_y _k_e_y, double threshold, │ │ │ │ │ +42 bool isGreaterThan, double mu = 1000.0) : │ │ │ │ │ +43 _B_a_s_e(_n_o_i_s_e_M_o_d_e_l::Constrained::All(1, mu), _k_e_y), │ │ │ │ │ +44 threshold_(threshold), isGreaterThan_(isGreaterThan) { │ │ │ │ │ +45 } │ │ │ │ │ +46 │ │ │ │ │ +47 _~_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_1() override {} │ │ │ │ │ +48 │ │ │ │ │ +49 inline double threshold() const { return threshold_; } │ │ │ │ │ +50 inline bool isGreaterThan() const { return isGreaterThan_; } │ │ │ │ │ 51 │ │ │ │ │ -52namespace _g_t_s_a_m { │ │ │ │ │ -53 │ │ │ │ │ -59template │ │ │ │ │ -_6_0class _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -61 │ │ │ │ │ -62 GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA) │ │ │ │ │ -63 GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK) │ │ │ │ │ -64 │ │ │ │ │ -65 static const int DimC = _F_i_x_e_d_D_i_m_e_n_s_i_o_n_<_C_A_M_E_R_A_>_:_:_v_a_l_u_e; │ │ │ │ │ -66 static const int DimL = _F_i_x_e_d_D_i_m_e_n_s_i_o_n_<_L_A_N_D_M_A_R_K_>_:_:_v_a_l_u_e; │ │ │ │ │ -67 typedef Eigen::Matrix JacobianC; │ │ │ │ │ -68 typedef Eigen::Matrix JacobianL; │ │ │ │ │ -69 │ │ │ │ │ -70protected: │ │ │ │ │ -71 │ │ │ │ │ -_7_2 _P_o_i_n_t_2 _m_e_a_s_u_r_e_d__; │ │ │ │ │ -73 │ │ │ │ │ -74public: │ │ │ │ │ +_5_7 virtual double _v_a_l_u_e(const X& x, boost::optional H = │ │ │ │ │ +58 boost::none) const = 0; │ │ │ │ │ +59 │ │ │ │ │ +_6_1 bool _a_c_t_i_v_e(const _V_a_l_u_e_s& c) const override { │ │ │ │ │ +62 // note: still active at equality to avoid zigzagging │ │ │ │ │ +63 double x = _v_a_l_u_e(c._a_t(this->key())); │ │ │ │ │ +64 return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; │ │ │ │ │ +65 } │ │ │ │ │ +66 │ │ │ │ │ +67 Vector evaluateError(const X& x, boost::optional H = │ │ │ │ │ +68 boost::none) const override { │ │ │ │ │ +69 Matrix D; │ │ │ │ │ +70 double _e_r_r_o_r = _v_a_l_u_e(x, D) - threshold_; │ │ │ │ │ +71 if (H) { │ │ │ │ │ +72 if (isGreaterThan_) *H = D; │ │ │ │ │ +73 else *H = -1.0 * D; │ │ │ │ │ +74 } │ │ │ │ │ 75 │ │ │ │ │ -_7_6 typedef _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_<_C_A_M_E_R_A_,_ _L_A_N_D_M_A_R_K_> _T_h_i_s; │ │ │ │ │ -_7_7 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_C_A_M_E_R_A_,_ _L_A_N_D_M_A_R_K_> _B_a_s_e; │ │ │ │ │ -78 │ │ │ │ │ -79 // shorthand for a smart pointer to a factor │ │ │ │ │ -80 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +76 if (isGreaterThan_) │ │ │ │ │ +77 return (Vector(1) << _e_r_r_o_r).finished(); │ │ │ │ │ +78 else │ │ │ │ │ +79 return -1.0 * (Vector(1) << _e_r_r_o_r).finished(); │ │ │ │ │ +80 } │ │ │ │ │ 81 │ │ │ │ │ -_8_9 _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r(const _P_o_i_n_t_2& _m_e_a_s_u_r_e_d, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model, │ │ │ │ │ -90 _K_e_y cameraKey, _K_e_y landmarkKey) │ │ │ │ │ -91 : _B_a_s_e(model, cameraKey, landmarkKey), _m_e_a_s_u_r_e_d__(_m_e_a_s_u_r_e_d) {} │ │ │ │ │ -92 │ │ │ │ │ -_9_3 _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r() : _m_e_a_s_u_r_e_d__(0.0, 0.0) {} │ │ │ │ │ -95 _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r(const _P_o_i_n_t_2& p) : _m_e_a_s_u_r_e_d__(p) {} │ │ │ │ │ -97 _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r(double x, double y) : _m_e_a_s_u_r_e_d__(x, y) {} │ │ │ │ │ -_9_8 │ │ │ │ │ -99 _~_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r() override {} │ │ │ │ │ -100 │ │ │ │ │ -102 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -103 return boost::static_pointer_cast( │ │ │ │ │ -104 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this)));} │ │ │ │ │ -105 │ │ │ │ │ -111 void _p_r_i_n_t(const std::string& s = "SFMFactor", const _K_e_y_F_o_r_m_a_t_t_e_r& │ │ │ │ │ -keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ -112 _B_a_s_e_:_:_p_r_i_n_t(s, keyFormatter); │ │ │ │ │ -113 traits::Print(_m_e_a_s_u_r_e_d__, s + ".z"); │ │ │ │ │ -114 } │ │ │ │ │ +82private: │ │ │ │ │ +83 │ │ │ │ │ +_8_5 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +86 template │ │ │ │ │ +87 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +88 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility │ │ │ │ │ +89 ar & boost::serialization::make_nvp("NoiseModelFactor1", │ │ │ │ │ +90 boost::serialization::base_object(*this)); │ │ │ │ │ +91 ar & BOOST_SERIALIZATION_NVP(threshold_); │ │ │ │ │ +92 ar & BOOST_SERIALIZATION_NVP(isGreaterThan_); │ │ │ │ │ +93 } │ │ │ │ │ +94}; │ │ │ │ │ +95 │ │ │ │ │ +100template │ │ │ │ │ +_1_0_1struct _B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_2: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +102 typedef VALUE1 X1; │ │ │ │ │ +103 typedef VALUE2 X2; │ │ │ │ │ +104 │ │ │ │ │ +105 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_V_A_L_U_E_1_,_ _V_A_L_U_E_2_> _B_a_s_e; │ │ │ │ │ +106 typedef boost::shared_ptr > shared_ptr; │ │ │ │ │ +107 │ │ │ │ │ +108 double threshold_; │ │ │ │ │ +109 bool isGreaterThan_; │ │ │ │ │ +110 │ │ │ │ │ +_1_1_1 _B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_2(_K_e_y key1, _K_e_y key2, double threshold, │ │ │ │ │ +112 bool isGreaterThan, double mu = 1000.0) │ │ │ │ │ +113 : _B_a_s_e(_n_o_i_s_e_M_o_d_e_l::Constrained::All(1, mu), key1, key2), │ │ │ │ │ +114 threshold_(threshold), isGreaterThan_(isGreaterThan) {} │ │ │ │ │ 115 │ │ │ │ │ -119 bool _e_q_u_a_l_s(const NonlinearFactor &p, double tol = 1e-9) const override { │ │ │ │ │ -120 const _T_h_i_s* e = dynamic_cast(&p); │ │ │ │ │ -121 return e && _B_a_s_e_:_:_e_q_u_a_l_s(p, tol) && traits::Equals(this->measured_, │ │ │ │ │ -e->measured_, tol); │ │ │ │ │ -122 } │ │ │ │ │ -123 │ │ │ │ │ -125 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const CAMERA& camera, const LANDMARK& point, │ │ │ │ │ -126 boost::optional H1=boost::none, boost::optional │ │ │ │ │ -H2=boost::none) const override { │ │ │ │ │ -127 try { │ │ │ │ │ -128 return camera.project2(point,H1,H2) - _m_e_a_s_u_r_e_d__; │ │ │ │ │ -129 } │ │ │ │ │ -130 catch( CheiralityException& e) { │ │ │ │ │ -131 if (H1) *H1 = JacobianC::Zero(); │ │ │ │ │ -132 if (H2) *H2 = JacobianL::Zero(); │ │ │ │ │ -133 //TODO Print the exception via logging │ │ │ │ │ -134 return Z_2x1; │ │ │ │ │ -135 } │ │ │ │ │ -136 } │ │ │ │ │ -137 │ │ │ │ │ -139 boost::shared_ptr _l_i_n_e_a_r_i_z_e(const _V_a_l_u_e_s& values) const │ │ │ │ │ -override { │ │ │ │ │ -140 // Only linearize if the factor is active │ │ │ │ │ -141 if (!this->_a_c_t_i_v_e(values)) return boost::shared_ptr(); │ │ │ │ │ -142 │ │ │ │ │ -143 const _K_e_y key1 = this->key1(), key2 = this->key2(); │ │ │ │ │ -144 JacobianC H1; │ │ │ │ │ -145 JacobianL H2; │ │ │ │ │ -146 Vector2 b; │ │ │ │ │ -147 try { │ │ │ │ │ -148 const CAMERA& camera = values.at(key1); │ │ │ │ │ -149 const LANDMARK& point = values.at(key2); │ │ │ │ │ -150 b = _m_e_a_s_u_r_e_d() - camera.project2(point, H1, H2); │ │ │ │ │ -151 } catch (CheiralityException& e) { │ │ │ │ │ -152 H1.setZero(); │ │ │ │ │ -153 H2.setZero(); │ │ │ │ │ -154 b.setZero(); │ │ │ │ │ -155 //TODO Print the exception via logging │ │ │ │ │ -156 } │ │ │ │ │ +116 _~_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_2() override {} │ │ │ │ │ +117 │ │ │ │ │ +118 inline double threshold() const { return threshold_; } │ │ │ │ │ +119 inline bool isGreaterThan() const { return isGreaterThan_; } │ │ │ │ │ +120 │ │ │ │ │ +_1_2_5 virtual double _v_a_l_u_e(const X1& x1, const X2& x2, │ │ │ │ │ +126 boost::optional H1 = boost::none, │ │ │ │ │ +127 boost::optional H2 = boost::none) const = 0; │ │ │ │ │ +128 │ │ │ │ │ +_1_3_0 bool _a_c_t_i_v_e(const _V_a_l_u_e_s& c) const override { │ │ │ │ │ +131 // note: still active at equality to avoid zigzagging │ │ │ │ │ +132 double x = _v_a_l_u_e(c._a_t(this->key1()), c._a_t(this->key2())); │ │ │ │ │ +133 return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; │ │ │ │ │ +134 } │ │ │ │ │ +135 │ │ │ │ │ +136 Vector evaluateError(const X1& x1, const X2& x2, │ │ │ │ │ +137 boost::optional H1 = boost::none, │ │ │ │ │ +138 boost::optional H2 = boost::none) const override { │ │ │ │ │ +139 Matrix D1, D2; │ │ │ │ │ +140 double _e_r_r_o_r = _v_a_l_u_e(x1, x2, D1, D2) - threshold_; │ │ │ │ │ +141 if (H1) { │ │ │ │ │ +142 if (isGreaterThan_) *H1 = D1; │ │ │ │ │ +143 else *H1 = -1.0 * D1; │ │ │ │ │ +144 } │ │ │ │ │ +145 if (H2) { │ │ │ │ │ +146 if (isGreaterThan_) *H2 = D2; │ │ │ │ │ +147 else *H2 = -1.0 * D2; │ │ │ │ │ +148 } │ │ │ │ │ +149 │ │ │ │ │ +150 if (isGreaterThan_) │ │ │ │ │ +151 return (Vector(1) << _e_r_r_o_r).finished(); │ │ │ │ │ +152 else │ │ │ │ │ +153 return -1.0 * (Vector(1) << _e_r_r_o_r).finished(); │ │ │ │ │ +154 } │ │ │ │ │ +155 │ │ │ │ │ +156private: │ │ │ │ │ 157 │ │ │ │ │ -158 // Whiten the system if needed │ │ │ │ │ -159 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& _n_o_i_s_e_M_o_d_e_l = this->noiseModel(); │ │ │ │ │ -160 if (noiseModel && !noiseModel->isUnit()) { │ │ │ │ │ -161 // TODO: implement WhitenSystem for fixed size matrices and include │ │ │ │ │ -162 // above │ │ │ │ │ -163 H1 = noiseModel->Whiten(H1); │ │ │ │ │ -164 H2 = noiseModel->Whiten(H2); │ │ │ │ │ -165 b = noiseModel->Whiten(b); │ │ │ │ │ -166 } │ │ │ │ │ -167 │ │ │ │ │ -168 // Create new (unit) noiseModel, preserving constraints if applicable │ │ │ │ │ -169 SharedDiagonal model; │ │ │ │ │ -170 if (noiseModel && noiseModel->isConstrained()) { │ │ │ │ │ -171 model = boost::static_pointer_cast(noiseModel)- │ │ │ │ │ ->unit(); │ │ │ │ │ -172 } │ │ │ │ │ -173 │ │ │ │ │ -174 return boost::make_shared >(key1, H1, │ │ │ │ │ -key2, H2, b, model); │ │ │ │ │ -175 } │ │ │ │ │ -176 │ │ │ │ │ -178 inline const _P_o_i_n_t_2 _m_e_a_s_u_r_e_d() const { │ │ │ │ │ -179 return _m_e_a_s_u_r_e_d__; │ │ │ │ │ -180 } │ │ │ │ │ -181 │ │ │ │ │ -182private: │ │ │ │ │ -184 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -185 template │ │ │ │ │ -186 void serialize(Archive & ar, const unsigned int /*version*/) { │ │ │ │ │ -187 // NoiseModelFactor2 instead of NoiseModelFactorN for backward │ │ │ │ │ +_1_5_9 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +160 template │ │ │ │ │ +161 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +162 // NoiseModelFactor2 instead of NoiseModelFactorN for backward │ │ │ │ │ compatibility │ │ │ │ │ -188 ar & boost::serialization::make_nvp("NoiseModelFactor2", │ │ │ │ │ -189 boost::serialization::base_object(*this)); │ │ │ │ │ -190 ar & BOOST_SERIALIZATION_NVP(_m_e_a_s_u_r_e_d__); │ │ │ │ │ -191 } │ │ │ │ │ -192}; │ │ │ │ │ -193 │ │ │ │ │ -194template │ │ │ │ │ -_1_9_5struct _t_r_a_i_t_s<_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r > : _T_e_s_t_a_b_l_e< │ │ │ │ │ -196 GeneralSFMFactor > { │ │ │ │ │ -197}; │ │ │ │ │ -198 │ │ │ │ │ -203template │ │ │ │ │ -_2_0_4class _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -205 │ │ │ │ │ -206 GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) │ │ │ │ │ -207 static const int DimK = _F_i_x_e_d_D_i_m_e_n_s_i_o_n_<_C_A_L_I_B_R_A_T_I_O_N_>_:_:_v_a_l_u_e; │ │ │ │ │ -208 │ │ │ │ │ -209protected: │ │ │ │ │ -210 │ │ │ │ │ -_2_1_1 _P_o_i_n_t_2 _m_e_a_s_u_r_e_d__; │ │ │ │ │ -212 │ │ │ │ │ -213public: │ │ │ │ │ -214 │ │ │ │ │ -215 typedef _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_<_C_A_L_I_B_R_A_T_I_O_N_> This; │ │ │ │ │ -_2_1_6 typedef _P_i_n_h_o_l_e_C_a_m_e_r_a_<_C_A_L_I_B_R_A_T_I_O_N_> _C_a_m_e_r_a; │ │ │ │ │ -_2_1_7 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_P_o_s_e_3_,_ _P_o_i_n_t_3_,_ _C_A_L_I_B_R_A_T_I_O_N_> _B_a_s_e; │ │ │ │ │ -218 │ │ │ │ │ -219 // shorthand for a smart pointer to a factor │ │ │ │ │ -220 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -221 │ │ │ │ │ -_2_3_0 _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2(const _P_o_i_n_t_2& _m_e_a_s_u_r_e_d, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model, │ │ │ │ │ -_K_e_y poseKey, _K_e_y landmarkKey, _K_e_y calibKey) : │ │ │ │ │ -231 _B_a_s_e(model, poseKey, landmarkKey, calibKey), _m_e_a_s_u_r_e_d__(_m_e_a_s_u_r_e_d) {} │ │ │ │ │ -_2_3_2 _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2():_m_e_a_s_u_r_e_d__(0.0,0.0) {} │ │ │ │ │ -233 │ │ │ │ │ -_2_3_4 _~_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2() override {} │ │ │ │ │ -235 │ │ │ │ │ -_2_3_7 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -238 return boost::static_pointer_cast( │ │ │ │ │ -239 gtsam::NonlinearFactor::shared_ptr(new This(*this)));} │ │ │ │ │ -240 │ │ │ │ │ -_2_4_6 void _p_r_i_n_t(const std::string& s = "SFMFactor2", const _K_e_y_F_o_r_m_a_t_t_e_r& │ │ │ │ │ -keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ -247 _B_a_s_e_:_:_p_r_i_n_t(s, keyFormatter); │ │ │ │ │ -248 _t_r_a_i_t_s_<_P_o_i_n_t_2_>_:_:_P_r_i_n_t(_m_e_a_s_u_r_e_d__, s + ".z"); │ │ │ │ │ -249 } │ │ │ │ │ -250 │ │ │ │ │ -_2_5_4 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r &p, double tol = 1e-9) const override { │ │ │ │ │ -255 const This* e = dynamic_cast(&p); │ │ │ │ │ -256 return e && _B_a_s_e_:_:_e_q_u_a_l_s(p, tol) && _t_r_a_i_t_s_<_P_o_i_n_t_2_>_:_:_E_q_u_a_l_s(this->measured_, │ │ │ │ │ -e->measured_, tol); │ │ │ │ │ -257 } │ │ │ │ │ -258 │ │ │ │ │ -_2_6_0 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const _P_o_s_e_3& pose3, const _P_o_i_n_t_3& point, const │ │ │ │ │ -CALIBRATION &calib, │ │ │ │ │ -261 boost::optional H1=boost::none, │ │ │ │ │ -262 boost::optional H2=boost::none, │ │ │ │ │ -263 boost::optional H3=boost::none) const override │ │ │ │ │ -264 { │ │ │ │ │ -265 try { │ │ │ │ │ -266 _C_a_m_e_r_a camera(pose3,calib); │ │ │ │ │ -267 return camera.project(point, H1, H2, H3) - _m_e_a_s_u_r_e_d__; │ │ │ │ │ -268 } │ │ │ │ │ -269 catch( _C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n& e) { │ │ │ │ │ -270 if (H1) *H1 = Matrix::Zero(2, 6); │ │ │ │ │ -271 if (H2) *H2 = Matrix::Zero(2, 3); │ │ │ │ │ -272 if (H3) *H3 = Matrix::Zero(2, DimK); │ │ │ │ │ -273 std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) │ │ │ │ │ -274 << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl; │ │ │ │ │ -275 } │ │ │ │ │ -276 return Z_2x1; │ │ │ │ │ -277 } │ │ │ │ │ -278 │ │ │ │ │ -_2_8_0 inline const _P_o_i_n_t_2 _m_e_a_s_u_r_e_d() const { │ │ │ │ │ -281 return _m_e_a_s_u_r_e_d__; │ │ │ │ │ -282 } │ │ │ │ │ -283 │ │ │ │ │ -284private: │ │ │ │ │ -_2_8_6 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -287 template │ │ │ │ │ -288 void serialize(Archive & ar, const unsigned int /*version*/) { │ │ │ │ │ -289 // NoiseModelFactor3 instead of NoiseModelFactorN for backward │ │ │ │ │ -compatibility │ │ │ │ │ -290 ar & boost::serialization::make_nvp("NoiseModelFactor3", │ │ │ │ │ -291 boost::serialization::base_object(*this)); │ │ │ │ │ -292 ar & BOOST_SERIALIZATION_NVP(_m_e_a_s_u_r_e_d__); │ │ │ │ │ -293 } │ │ │ │ │ -294}; │ │ │ │ │ -295 │ │ │ │ │ -296template │ │ │ │ │ -_2_9_7struct _t_r_a_i_t_s<_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2 > : _T_e_s_t_a_b_l_e< │ │ │ │ │ -298 GeneralSFMFactor2 > { │ │ │ │ │ -299}; │ │ │ │ │ -300 │ │ │ │ │ -301} //namespace │ │ │ │ │ -_V_e_c_t_o_r_._h │ │ │ │ │ -typedef and functions to augment Eigen's VectorXd │ │ │ │ │ -_T_e_s_t_a_b_l_e_._h │ │ │ │ │ -Concept check for values that can be used in unit tests. │ │ │ │ │ -_t_i_m_i_n_g_._h │ │ │ │ │ -Timing utilities. │ │ │ │ │ -_M_a_n_i_f_o_l_d_._h │ │ │ │ │ -Base class and basic functions for Manifold types. │ │ │ │ │ -_t_y_p_e_s_._h │ │ │ │ │ -Typedefs for easier changing of types. │ │ │ │ │ -_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_._h │ │ │ │ │ -Access to matrices via blocks of pre-defined sizes. │ │ │ │ │ -_M_a_t_r_i_x_._h │ │ │ │ │ -typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ -_P_i_n_h_o_l_e_C_a_m_e_r_a_._h │ │ │ │ │ -Base class for all pinhole cameras. │ │ │ │ │ -_P_o_s_e_3_._h │ │ │ │ │ -3D Pose │ │ │ │ │ -_P_o_i_n_t_3_._h │ │ │ │ │ -3D Point │ │ │ │ │ -_P_o_i_n_t_2_._h │ │ │ │ │ -2D Point │ │ │ │ │ -_N_o_i_s_e_M_o_d_e_l_._h │ │ │ │ │ -_B_i_n_a_r_y_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ -A binary JacobianFactor specialization that uses fixed matrix math for speed. │ │ │ │ │ +163 ar & boost::serialization::make_nvp("NoiseModelFactor2", │ │ │ │ │ +164 boost::serialization::base_object(*this)); │ │ │ │ │ +165 ar & BOOST_SERIALIZATION_NVP(threshold_); │ │ │ │ │ +166 ar & BOOST_SERIALIZATION_NVP(isGreaterThan_); │ │ │ │ │ +167 } │ │ │ │ │ +168}; │ │ │ │ │ +169 │ │ │ │ │ +170} // \namespace gtsam │ │ │ │ │ +_L_i_e_._h │ │ │ │ │ +Base class and basic functions for Lie types. │ │ │ │ │ _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ -Vector2 Point2 │ │ │ │ │ -As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point2 to Vector2... │ │ │ │ │ -DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ -Vector3 Point3 │ │ │ │ │ -As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point3 to Vector3... │ │ │ │ │ -DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ -Aliases. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_F_i_x_e_d_D_i_m_e_n_s_i_o_n │ │ │ │ │ -Give fixed size dimension of a type, fails at compile time if dynamic. │ │ │ │ │ -DDeeffiinniittiioonn Manifold.h:164 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a │ │ │ │ │ -A pinhole camera class that has a Pose3 and a Calibration. │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:33 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ -A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ -virtual void print(const std::string &s="Factor", const KeyFormatter │ │ │ │ │ -&formatter=DefaultKeyFormatter) const │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn Factor.cpp:29 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const This &other, double tol=1e-9) const │ │ │ │ │ -check equality │ │ │ │ │ -DDeeffiinniittiioonn Factor.cpp:42 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ -Nonlinear factor base class. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_:_:_a_c_t_i_v_e │ │ │ │ │ -virtual bool active(const Values &) const │ │ │ │ │ -Checks whether a factor should be used based on a set of values. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:118 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_e_r_r_o_r │ │ │ │ │ +double error(const Values &c) const override │ │ │ │ │ +Calculate the error of the factor. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.cpp:138 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_n_o_i_s_e_M_o_d_e_l │ │ │ │ │ const SharedNoiseModel & noiseModel() const │ │ │ │ │ access to the noise model │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactor.h:223 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ variables. │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r │ │ │ │ │ -Non-linear factor for a constraint derived from a 2D measurement. │ │ │ │ │ -DDeeffiinniittiioonn GeneralSFMFactor.h:60 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e │ │ │ │ │ -boost::shared_ptr< GaussianFactor > linearize(const Values &values) const │ │ │ │ │ -override │ │ │ │ │ -Linearize using fixed-size matrices. │ │ │ │ │ -DDeeffiinniittiioonn GeneralSFMFactor.h:138 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ -Vector evaluateError(const CAMERA &camera, const LANDMARK &point, boost:: │ │ │ │ │ -optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost:: │ │ │ │ │ -none) const override │ │ │ │ │ -h(x)-z │ │ │ │ │ -DDeeffiinniittiioonn GeneralSFMFactor.h:124 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ -gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn GeneralSFMFactor.h:101 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const NonlinearFactor &p, double tol=1e-9) const override │ │ │ │ │ -equals │ │ │ │ │ -DDeeffiinniittiioonn GeneralSFMFactor.h:118 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_:_:_m_e_a_s_u_r_e_d │ │ │ │ │ -const Point2 measured() const │ │ │ │ │ -return the measured │ │ │ │ │ -DDeeffiinniittiioonn GeneralSFMFactor.h:177 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r │ │ │ │ │ -GeneralSFMFactor(const Point2 &measured, const SharedNoiseModel &model, Key │ │ │ │ │ -cameraKey, Key landmarkKey) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn GeneralSFMFactor.h:89 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r │ │ │ │ │ -GeneralSFMFactor() │ │ │ │ │ -default constructor │ │ │ │ │ -DDeeffiinniittiioonn GeneralSFMFactor.h:93 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_:_:_T_h_i_s │ │ │ │ │ -GeneralSFMFactor< CAMERA, LANDMARK > This │ │ │ │ │ -typedef for this object │ │ │ │ │ -DDeeffiinniittiioonn GeneralSFMFactor.h:76 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_:_:_~_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r │ │ │ │ │ -~GeneralSFMFactor() override │ │ │ │ │ -destructor │ │ │ │ │ -DDeeffiinniittiioonn GeneralSFMFactor.h:98 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_:_:_B_a_s_e │ │ │ │ │ -NoiseModelFactorN< CAMERA, LANDMARK > Base │ │ │ │ │ -typedef for the base class │ │ │ │ │ -DDeeffiinniittiioonn GeneralSFMFactor.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="SFMFactor", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn GeneralSFMFactor.h:110 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_ _V_A_L_U_E_ _>_:_:_k_e_y │ │ │ │ │ +Key key() const │ │ │ │ │ +Returns a key. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:518 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_a_t │ │ │ │ │ +const ValueType at(Key j) const │ │ │ │ │ +Retrieve a variable by key j. │ │ │ │ │ +DDeeffiinniittiioonn Values-inl.h:361 │ │ │ │ │ +_g_t_s_a_m_:_:_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_1 │ │ │ │ │ +Unary inequality constraint forcing a scalar to be greater/less than a fixed │ │ │ │ │ +threshold. │ │ │ │ │ +DDeeffiinniittiioonn BoundingConstraint.h:33 │ │ │ │ │ +_g_t_s_a_m_:_:_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_1_:_:_a_c_t_i_v_e │ │ │ │ │ +bool active(const Values &c) const override │ │ │ │ │ +active when constraint NOT met │ │ │ │ │ +DDeeffiinniittiioonn BoundingConstraint.h:61 │ │ │ │ │ +_g_t_s_a_m_:_:_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_1_:_:_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_1 │ │ │ │ │ +BoundingConstraint1(Key key, double threshold, bool isGreaterThan, double │ │ │ │ │ +mu=1000.0) │ │ │ │ │ +flag for greater/less than │ │ │ │ │ +DDeeffiinniittiioonn BoundingConstraint.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_1_:_:_a_c_c_e_s_s │ │ │ │ │ friend class boost::serialization::access │ │ │ │ │ Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn GeneralSFMFactor.h:183 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_:_:_m_e_a_s_u_r_e_d__ │ │ │ │ │ -Point2 measured_ │ │ │ │ │ -the 2D measurement │ │ │ │ │ -DDeeffiinniittiioonn GeneralSFMFactor.h:72 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2 │ │ │ │ │ -Non-linear factor for a constraint derived from a 2D measurement. │ │ │ │ │ -DDeeffiinniittiioonn GeneralSFMFactor.h:204 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2 │ │ │ │ │ -GeneralSFMFactor2() │ │ │ │ │ -default constructor │ │ │ │ │ -DDeeffiinniittiioonn GeneralSFMFactor.h:232 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_:_:_~_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2 │ │ │ │ │ -~GeneralSFMFactor2() override │ │ │ │ │ -destructor │ │ │ │ │ -DDeeffiinniittiioonn GeneralSFMFactor.h:234 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_:_:_c_l_o_n_e │ │ │ │ │ -gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn GeneralSFMFactor.h:237 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_:_:_B_a_s_e │ │ │ │ │ -NoiseModelFactorN< Pose3, Point3, CALIBRATION > Base │ │ │ │ │ -typedef for the base class │ │ │ │ │ -DDeeffiinniittiioonn GeneralSFMFactor.h:217 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ -Vector evaluateError(const Pose3 &pose3, const Point3 &point, const CALIBRATION │ │ │ │ │ -&calib, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > │ │ │ │ │ -H2=boost::none, boost::optional< Matrix & > H3=boost::none) const override │ │ │ │ │ -h(x)-z │ │ │ │ │ -DDeeffiinniittiioonn GeneralSFMFactor.h:260 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="SFMFactor2", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn GeneralSFMFactor.h:246 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_:_:_m_e_a_s_u_r_e_d__ │ │ │ │ │ -Point2 measured_ │ │ │ │ │ -the 2D measurement │ │ │ │ │ -DDeeffiinniittiioonn GeneralSFMFactor.h:211 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_:_:_m_e_a_s_u_r_e_d │ │ │ │ │ -const Point2 measured() const │ │ │ │ │ -return the measured │ │ │ │ │ -DDeeffiinniittiioonn GeneralSFMFactor.h:280 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_:_:_C_a_m_e_r_a │ │ │ │ │ -PinholeCamera< CALIBRATION > Camera │ │ │ │ │ -typedef for camera type │ │ │ │ │ -DDeeffiinniittiioonn GeneralSFMFactor.h:216 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2 │ │ │ │ │ -GeneralSFMFactor2(const Point2 &measured, const SharedNoiseModel &model, Key │ │ │ │ │ -poseKey, Key landmarkKey, Key calibKey) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn GeneralSFMFactor.h:230 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const NonlinearFactor &p, double tol=1e-9) const override │ │ │ │ │ -equals │ │ │ │ │ -DDeeffiinniittiioonn GeneralSFMFactor.h:254 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_2_:_:_a_c_c_e_s_s │ │ │ │ │ +DDeeffiinniittiioonn BoundingConstraint.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_1_:_:_v_a_l_u_e │ │ │ │ │ +virtual double value(const X &x, boost::optional< Matrix & > H=boost::none) │ │ │ │ │ +const =0 │ │ │ │ │ +function producing a scalar value to compare to the threshold Must have │ │ │ │ │ +optional argument for derivat... │ │ │ │ │ +_g_t_s_a_m_:_:_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_2 │ │ │ │ │ +Binary scalar inequality constraint, with a similar value() function to │ │ │ │ │ +implement for specific system... │ │ │ │ │ +DDeeffiinniittiioonn BoundingConstraint.h:101 │ │ │ │ │ +_g_t_s_a_m_:_:_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_2_:_:_a_c_t_i_v_e │ │ │ │ │ +bool active(const Values &c) const override │ │ │ │ │ +active when constraint NOT met │ │ │ │ │ +DDeeffiinniittiioonn BoundingConstraint.h:130 │ │ │ │ │ +_g_t_s_a_m_:_:_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_2_:_:_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_2 │ │ │ │ │ +BoundingConstraint2(Key key1, Key key2, double threshold, bool isGreaterThan, │ │ │ │ │ +double mu=1000.0) │ │ │ │ │ +flag for greater/less than │ │ │ │ │ +DDeeffiinniittiioonn BoundingConstraint.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_2_:_:_a_c_c_e_s_s │ │ │ │ │ friend class boost::serialization::access │ │ │ │ │ Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn GeneralSFMFactor.h:286 │ │ │ │ │ -_V_a_l_u_e_s │ │ │ │ │ -In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ -a non-linear function... │ │ │ │ │ +DDeeffiinniittiioonn BoundingConstraint.h:159 │ │ │ │ │ +_g_t_s_a_m_:_:_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_2_:_:_v_a_l_u_e │ │ │ │ │ +virtual double value(const X1 &x1, const X2 &x2, boost::optional< Matrix & > │ │ │ │ │ +H1=boost::none, boost::optional< Matrix & > H2=boost::none) const =0 │ │ │ │ │ +function producing a scalar value to compare to the threshold Must have │ │ │ │ │ +optional argument for derivat... │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _G_e_n_e_r_a_l_S_F_M_F_a_c_t_o_r_._h │ │ │ │ │ + * _B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01283.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/ProjectionFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/BetweenFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,52 +95,55 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
ProjectionFactor.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Macros
│ │ │ │ +
BetweenFactor.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Reprojection of a LANDMARK to a 2D point. │ │ │ │ -More...

│ │ │ │ - │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::GenericProjectionFactor< POSE, LANDMARK, CALIBRATION >
 Non-linear factor for a constraint derived from a 2D measurement. More...
class  gtsam::BetweenFactor< VALUE >
 A class for a measurement predicted by "between(config[key1],config[key2])". More...
 
struct  gtsam::traits< BetweenFactor< VALUE > >
 traits More...
 
struct  gtsam::traits< GenericProjectionFactor< POSE, LANDMARK, CALIBRATION > >
 traits More...
class  gtsam::BetweenConstraint< VALUE >
 Binary between constraint - forces between to a given value This constraint requires the underlying type to a Lie type. More...
 
struct  gtsam::traits< BetweenConstraint< VALUE > >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Macros

│ │ │ │ +#define BETWEENFACTOR_VISIBILITY   GTSAM_EXPORT
 
│ │ │ │

Detailed Description

│ │ │ │ -

Reprojection of a LANDMARK to a 2D point.

│ │ │ │ -
Author
Chris Beall
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ -
│ │ │ │ -Alex Cunningham
│ │ │ │ +
Author
Frank Dellaert, Viorela Ila
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,33 +1,37 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -ProjectionFactor.h File Reference │ │ │ │ │ -Reprojection of a LANDMARK to a 2D point. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s │ │ │ │ │ +BetweenFactor.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_<_ _P_O_S_E_,_ _L_A_N_D_M_A_R_K_,_ _C_A_L_I_B_R_A_T_I_O_N_ _> │ │ │ │ │ -  Non-linear factor for a constraint derived from a 2D measurement. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_B_e_t_w_e_e_n_F_a_c_t_o_r_<_ _V_A_L_U_E_ _> │ │ │ │ │ +  A class for a measurement predicted by "between(config[key1],config │ │ │ │ │ + [key2])". _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_<_ _P_O_S_E_,_ _L_A_N_D_M_A_R_K_,_ _C_A_L_I_B_R_A_T_I_O_N_ _> │ │ │ │ │ - _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _B_e_t_w_e_e_n_F_a_c_t_o_r_<_ _V_A_L_U_E_ _>_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_B_e_t_w_e_e_n_C_o_n_s_t_r_a_i_n_t_<_ _V_A_L_U_E_ _> │ │ │ │ │ +  Binary between constraint - forces between to a given value This │ │ │ │ │ + constraint requires the underlying type to a Lie type. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _B_e_t_w_e_e_n_C_o_n_s_t_r_a_i_n_t_<_ _V_A_L_U_E_ _>_ _> │ │ │ │ │   traits _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +MMaaccrrooss │ │ │ │ │ +#define  BBEETTWWEEEENNFFAACCTTOORR__VVIISSIIBBIILLIITTYY   GTSAM_EXPORT │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Reprojection of a LANDMARK to a 2D point. │ │ │ │ │ Author │ │ │ │ │ - Chris Beall │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Alex Cunningham │ │ │ │ │ + Frank Dellaert, Viorela Ila │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_._h │ │ │ │ │ + * _B_e_t_w_e_e_n_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01283.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,5 @@ │ │ │ │ │ var a01283 = [ │ │ │ │ │ - ["gtsam::traits< GenericProjectionFactor< POSE, LANDMARK, CALIBRATION > >", "a04900.html", null] │ │ │ │ │ + ["gtsam::traits< BetweenFactor< VALUE > >", "a04768.html", null], │ │ │ │ │ + ["gtsam::BetweenConstraint< VALUE >", "a04772.html", "a04772"], │ │ │ │ │ + ["gtsam::traits< BetweenConstraint< VALUE > >", "a04776.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01283_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/ProjectionFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/BetweenFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,243 +98,215 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ProjectionFactor.h
│ │ │ │ +
BetweenFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
21#pragma once
│ │ │ │ -
22
│ │ │ │ -
23#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
24#include <gtsam/geometry/PinholeCamera.h>
│ │ │ │ -
25#include <gtsam/geometry/Pose3.h>
│ │ │ │ -
26#include <gtsam/geometry/Point3.h>
│ │ │ │ -
27#include <gtsam/geometry/Cal3_S2.h>
│ │ │ │ -
28#include <boost/optional.hpp>
│ │ │ │ -
29
│ │ │ │ -
30namespace gtsam {
│ │ │ │ +
16#pragma once
│ │ │ │ +
17
│ │ │ │ +
18#include <ostream>
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/base/Testable.h>
│ │ │ │ +
21#include <gtsam/base/Lie.h>
│ │ │ │ +
22#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
23
│ │ │ │ +
24#ifdef _WIN32
│ │ │ │ +
25#define BETWEENFACTOR_VISIBILITY
│ │ │ │ +
26#else
│ │ │ │ +
27// This will trigger a LNKxxxx on MSVC, so disable for MSVC build
│ │ │ │ +
28// Please refer to https://github.com/borglab/gtsam/blob/develop/Using-GTSAM-EXPORT.md
│ │ │ │ +
29#define BETWEENFACTOR_VISIBILITY GTSAM_EXPORT
│ │ │ │ +
30#endif
│ │ │ │
31
│ │ │ │ -
38 template <class POSE = Pose3, class LANDMARK = Point3,
│ │ │ │ -
39 class CALIBRATION = Cal3_S2>
│ │ │ │ +
32namespace gtsam {
│ │ │ │ +
33
│ │ │ │ +
39 template<class VALUE>
│ │ │ │
│ │ │ │ -
40 class GenericProjectionFactor: public NoiseModelFactorN<POSE, LANDMARK> {
│ │ │ │ -
41 protected:
│ │ │ │ -
42
│ │ │ │ -
43 // Keep a copy of measurement and calibration for I/O
│ │ │ │ -
44 Point2 measured_;
│ │ │ │ -
45 boost::shared_ptr<CALIBRATION> K_;
│ │ │ │ -
46 boost::optional<POSE> body_P_sensor_;
│ │ │ │ +
40 class BetweenFactor: public NoiseModelFactorN<VALUE, VALUE> {
│ │ │ │ +
41
│ │ │ │ +
42 // Check that VALUE type is a testable Lie group
│ │ │ │ +
43 BOOST_CONCEPT_ASSERT((IsTestable<VALUE>));
│ │ │ │ +
44 BOOST_CONCEPT_ASSERT((IsLieGroup<VALUE>));
│ │ │ │ +
45
│ │ │ │ +
46 public:
│ │ │ │
47
│ │ │ │ -
48 // verbosity handling for Cheirality Exceptions
│ │ │ │ -
49 bool throwCheirality_;
│ │ │ │ -
50 bool verboseCheirality_;
│ │ │ │ +
48 typedef VALUE T;
│ │ │ │ +
49
│ │ │ │ +
50 private:
│ │ │ │
51
│ │ │ │ -
52 public:
│ │ │ │ -
53
│ │ │ │ -
55 typedef NoiseModelFactorN<POSE, LANDMARK> Base;
│ │ │ │ -
56
│ │ │ │ -
58 typedef GenericProjectionFactor<POSE, LANDMARK, CALIBRATION> This;
│ │ │ │ -
59
│ │ │ │ -
61 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
62
│ │ │ │ -
│ │ │ │ -
64 GenericProjectionFactor() :
│ │ │ │ -
65 measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) {
│ │ │ │ -
66 }
│ │ │ │ -
│ │ │ │ +
52 typedef BetweenFactor<VALUE> This;
│ │ │ │ +
53 typedef NoiseModelFactorN<VALUE, VALUE> Base;
│ │ │ │ +
54
│ │ │ │ +
55 VALUE measured_;
│ │ │ │ +
57 public:
│ │ │ │ +
58
│ │ │ │ +
59 // shorthand for a smart pointer to a factor
│ │ │ │ +
60 typedef typename boost::shared_ptr<BetweenFactor> shared_ptr;
│ │ │ │ +
61
│ │ │ │ +
64
│ │ │ │ +
66 BetweenFactor() {}
│ │ │ │
67
│ │ │ │ -
│ │ │ │ -
78 GenericProjectionFactor(const Point2& measured, const SharedNoiseModel& model,
│ │ │ │ -
79 Key poseKey, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
│ │ │ │ -
80 boost::optional<POSE> body_P_sensor = boost::none) :
│ │ │ │ -
81 Base(model, poseKey, pointKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
│ │ │ │ -
82 throwCheirality_(false), verboseCheirality_(false) {}
│ │ │ │ -
│ │ │ │ -
83
│ │ │ │ -
│ │ │ │ -
96 GenericProjectionFactor(const Point2& measured, const SharedNoiseModel& model,
│ │ │ │ -
97 Key poseKey, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
│ │ │ │ -
98 bool throwCheirality, bool verboseCheirality,
│ │ │ │ -
99 boost::optional<POSE> body_P_sensor = boost::none) :
│ │ │ │ -
100 Base(model, poseKey, pointKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
│ │ │ │ -
101 throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
│ │ │ │ +
│ │ │ │ +
69 BetweenFactor(Key key1, Key key2, const VALUE& measured,
│ │ │ │ +
70 const SharedNoiseModel& model = nullptr) :
│ │ │ │ +
71 Base(model, key1, key2), measured_(measured) {
│ │ │ │ +
72 }
│ │ │ │ +
│ │ │ │ +
73
│ │ │ │ +
75
│ │ │ │ +
76 ~BetweenFactor() override {}
│ │ │ │ +
77
│ │ │ │ +
│ │ │ │ +
79 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
80 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ +
81 gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
│ │ │ │ +
│ │ │ │ +
82
│ │ │ │ +
85
│ │ │ │ +
│ │ │ │ +
87 void print(
│ │ │ │ +
88 const std::string& s = "",
│ │ │ │ +
89 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ +
90 std::cout << s << "BetweenFactor("
│ │ │ │ +
91 << keyFormatter(this->key1()) << ","
│ │ │ │ +
92 << keyFormatter(this->key2()) << ")\n";
│ │ │ │ +
93 traits<T>::Print(measured_, " measured: ");
│ │ │ │ +
94 this->noiseModel_->print(" noise model: ");
│ │ │ │ +
95 }
│ │ │ │ +
│ │ │ │ +
96
│ │ │ │ +
│ │ │ │ +
98 bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
│ │ │ │ +
99 const This *e = dynamic_cast<const This*> (&expected);
│ │ │ │ +
100 return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(this->measured_, e->measured_, tol);
│ │ │ │ +
101 }
│ │ │ │
│ │ │ │
102
│ │ │ │ -
104 ~GenericProjectionFactor() override {}
│ │ │ │ -
105
│ │ │ │ -
│ │ │ │ -
107 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
108 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ -
109 gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
│ │ │ │ -
│ │ │ │ -
110
│ │ │ │ -
│ │ │ │ -
116 void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ -
117 std::cout << s << "GenericProjectionFactor, z = ";
│ │ │ │ -
118 traits<Point2>::Print(measured_);
│ │ │ │ -
119 if(this->body_P_sensor_)
│ │ │ │ -
120 this->body_P_sensor_->print(" sensor pose in body frame: ");
│ │ │ │ -
121 Base::print("", keyFormatter);
│ │ │ │ -
122 }
│ │ │ │ -
│ │ │ │ -
123
│ │ │ │ -
│ │ │ │ -
125 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
│ │ │ │ -
126 const This *e = dynamic_cast<const This*>(&p);
│ │ │ │ -
127 return e
│ │ │ │ -
128 && Base::equals(p, tol)
│ │ │ │ -
129 && traits<Point2>::Equals(this->measured_, e->measured_, tol)
│ │ │ │ -
130 && this->K_->equals(*e->K_, tol)
│ │ │ │ -
131 && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
│ │ │ │ -
132 }
│ │ │ │ -
│ │ │ │ -
133
│ │ │ │ -
│ │ │ │ -
135 Vector evaluateError(const Pose3& pose, const Point3& point,
│ │ │ │ -
136 boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
│ │ │ │ -
137 try {
│ │ │ │ -
138 if(body_P_sensor_) {
│ │ │ │ -
139 if(H1) {
│ │ │ │ -
140 gtsam::Matrix H0;
│ │ │ │ -
141 PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
│ │ │ │ -
142 Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_);
│ │ │ │ -
143 *H1 = *H1 * H0;
│ │ │ │ -
144 return reprojectionError;
│ │ │ │ -
145 } else {
│ │ │ │ -
146 PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
│ │ │ │ -
147 return camera.project(point, H1, H2, boost::none) - measured_;
│ │ │ │ -
148 }
│ │ │ │ -
149 } else {
│ │ │ │ -
150 PinholeCamera<CALIBRATION> camera(pose, *K_);
│ │ │ │ -
151 return camera.project(point, H1, H2, boost::none) - measured_;
│ │ │ │ -
152 }
│ │ │ │ -
153 } catch( CheiralityException& e) {
│ │ │ │ -
154 if (H1) *H1 = Matrix::Zero(2,6);
│ │ │ │ -
155 if (H2) *H2 = Matrix::Zero(2,3);
│ │ │ │ -
156 if (verboseCheirality_)
│ │ │ │ -
157 std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
│ │ │ │ -
158 " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
│ │ │ │ -
159 if (throwCheirality_)
│ │ │ │ -
160 throw CheiralityException(this->key2());
│ │ │ │ -
161 }
│ │ │ │ -
162 return Vector2::Constant(2.0 * K_->fx());
│ │ │ │ -
163 }
│ │ │ │ -
│ │ │ │ +
106
│ │ │ │ +
│ │ │ │ +
108 Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 =
│ │ │ │ +
109 boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
│ │ │ │ +
110 T hx = traits<T>::Between(p1, p2, H1, H2); // h(x)
│ │ │ │ +
111 // manifold equivalent of h(x)-z -> log(z,h(x))
│ │ │ │ +
112#ifdef GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR
│ │ │ │ +
113 typename traits<T>::ChartJacobian::Jacobian Hlocal;
│ │ │ │ +
114 Vector rval = traits<T>::Local(measured_, hx, boost::none, (H1 || H2) ? &Hlocal : 0);
│ │ │ │ +
115 if (H1) *H1 = Hlocal * (*H1);
│ │ │ │ +
116 if (H2) *H2 = Hlocal * (*H2);
│ │ │ │ +
117 return rval;
│ │ │ │ +
118#else
│ │ │ │ +
119 return traits<T>::Local(measured_, hx);
│ │ │ │ +
120#endif
│ │ │ │ +
121 }
│ │ │ │ +
│ │ │ │ +
122
│ │ │ │ +
126
│ │ │ │ +
│ │ │ │ +
128 const VALUE& measured() const {
│ │ │ │ +
129 return measured_;
│ │ │ │ +
130 }
│ │ │ │ +
│ │ │ │ +
132
│ │ │ │ +
133 private:
│ │ │ │ +
134
│ │ │ │ +
136 friend class boost::serialization::access;
│ │ │ │ +
137 template<class ARCHIVE>
│ │ │ │ +
138 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
139 // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ +
140 ar & boost::serialization::make_nvp("NoiseModelFactor2",
│ │ │ │ +
141 boost::serialization::base_object<Base>(*this));
│ │ │ │ +
142 ar & BOOST_SERIALIZATION_NVP(measured_);
│ │ │ │ +
143 }
│ │ │ │ +
144
│ │ │ │ +
145 // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
│ │ │ │ +
146 enum { NeedsToAlign = (sizeof(VALUE) % 16) == 0 };
│ │ │ │ +
147 public:
│ │ │ │ +
148 GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
│ │ │ │ +
149 }; // \class BetweenFactor
│ │ │ │ +
│ │ │ │ +
150
│ │ │ │ +
152 template<class VALUE>
│ │ │ │ +
153 struct traits<BetweenFactor<VALUE> > : public Testable<BetweenFactor<VALUE> > {};
│ │ │ │ +
154
│ │ │ │ +
160 template<class VALUE>
│ │ │ │ +
│ │ │ │ +
161 class BetweenConstraint : public BetweenFactor<VALUE> {
│ │ │ │ +
162 public:
│ │ │ │ +
163 typedef boost::shared_ptr<BetweenConstraint<VALUE> > shared_ptr;
│ │ │ │
164
│ │ │ │
│ │ │ │ -
166 const Point2& measured() const {
│ │ │ │ -
167 return measured_;
│ │ │ │ -
168 }
│ │ │ │ -
│ │ │ │ -
169
│ │ │ │ -
│ │ │ │ -
171 const boost::shared_ptr<CALIBRATION> calibration() const {
│ │ │ │ -
172 return K_;
│ │ │ │ -
173 }
│ │ │ │ -
│ │ │ │ -
174
│ │ │ │ -
│ │ │ │ -
176 const boost::optional<POSE>& body_P_sensor() const {
│ │ │ │ -
177 return body_P_sensor_;
│ │ │ │ -
178 }
│ │ │ │ -
│ │ │ │ -
179
│ │ │ │ -
181 inline bool verboseCheirality() const { return verboseCheirality_; }
│ │ │ │ -
182
│ │ │ │ -
184 inline bool throwCheirality() const { return throwCheirality_; }
│ │ │ │ +
166 BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) :
│ │ │ │ +
167 BetweenFactor<VALUE>(key1, key2, measured,
│ │ │ │ +
168 noiseModel::Constrained::All(traits<VALUE>::GetDimension(measured), std::abs(mu)))
│ │ │ │ +
169 {}
│ │ │ │ +
│ │ │ │ +
170
│ │ │ │ +
171 private:
│ │ │ │ +
172
│ │ │ │ +
174 friend class boost::serialization::access;
│ │ │ │ +
175 template<class ARCHIVE>
│ │ │ │ +
176 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
177 ar & boost::serialization::make_nvp("BetweenFactor",
│ │ │ │ +
178 boost::serialization::base_object<BetweenFactor<VALUE> >(*this));
│ │ │ │ +
179 }
│ │ │ │ +
180 }; // \class BetweenConstraint
│ │ │ │ +
│ │ │ │ +
181
│ │ │ │ +
183 template<class VALUE>
│ │ │ │ +
184 struct traits<BetweenConstraint<VALUE> > : public Testable<BetweenConstraint<VALUE> > {};
│ │ │ │
185
│ │ │ │ -
186 private:
│ │ │ │ -
187
│ │ │ │ -
189 friend class boost::serialization::access;
│ │ │ │ -
190 template<class ARCHIVE>
│ │ │ │ -
191 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
192 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
193 ar & BOOST_SERIALIZATION_NVP(measured_);
│ │ │ │ -
194 ar & BOOST_SERIALIZATION_NVP(K_);
│ │ │ │ -
195 ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
│ │ │ │ -
196 ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
│ │ │ │ -
197 ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
│ │ │ │ -
198 }
│ │ │ │ -
199
│ │ │ │ -
200 public:
│ │ │ │ -
201 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
202};
│ │ │ │ -
│ │ │ │ -
203
│ │ │ │ -
205 template<class POSE, class LANDMARK, class CALIBRATION>
│ │ │ │ -
│ │ │ │ -
206 struct traits<GenericProjectionFactor<POSE, LANDMARK, CALIBRATION> > :
│ │ │ │ -
207 public Testable<GenericProjectionFactor<POSE, LANDMARK, CALIBRATION> > {
│ │ │ │ -
208 };
│ │ │ │ -
│ │ │ │ -
209
│ │ │ │ -
210} // \ namespace gtsam
│ │ │ │ -
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ -
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
│ │ │ │ -
PinholeCamera.h
Base class for all pinhole cameras.
│ │ │ │ -
Pose3.h
3D Pose
│ │ │ │ -
Point3.h
3D Point
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
186}
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
This marks a GTSAM object to require alignment.
Definition types.h:317
│ │ │ │ +
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │ -
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::IsLieGroup
Lie Group Concept.
Definition Lie.h:260
│ │ │ │ +
gtsam::IsTestable
A testable concept check that should be placed in applicable unit tests and in generic algorithms.
Definition Testable.h:58
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::CheiralityException
Definition CalibratedCamera.h:32
│ │ │ │ -
gtsam::PinholeCamera
A pinhole camera class that has a Pose3 and a Calibration.
Definition PinholeCamera.h:33
│ │ │ │ -
gtsam::PinholeBaseK::project
Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > Dcal=boost::none) const
project a 3D point from world coordinates into the image
Definition PinholePose.h:118
│ │ │ │ -
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ -
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
│ │ │ │
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
│ │ │ │
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ +
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition NonlinearFactor.h:223
│ │ │ │
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ -
gtsam::GenericProjectionFactor
Non-linear factor for a constraint derived from a 2D measurement.
Definition ProjectionFactor.h:40
│ │ │ │ -
gtsam::GenericProjectionFactor::verboseCheirality
bool verboseCheirality() const
return verbosity
Definition ProjectionFactor.h:181
│ │ │ │ -
gtsam::GenericProjectionFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition ProjectionFactor.h:116
│ │ │ │ -
gtsam::GenericProjectionFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition ProjectionFactor.h:125
│ │ │ │ -
gtsam::GenericProjectionFactor::measured_
Point2 measured_
2D measurement
Definition ProjectionFactor.h:44
│ │ │ │ -
gtsam::GenericProjectionFactor::This
GenericProjectionFactor< POSE, LANDMARK, CALIBRATION > This
shorthand for this class
Definition ProjectionFactor.h:58
│ │ │ │ -
gtsam::GenericProjectionFactor::measured
const Point2 & measured() const
return the measurement
Definition ProjectionFactor.h:166
│ │ │ │ -
gtsam::GenericProjectionFactor::body_P_sensor_
boost::optional< POSE > body_P_sensor_
The pose of the sensor in the body frame.
Definition ProjectionFactor.h:46
│ │ │ │ -
gtsam::GenericProjectionFactor::K_
boost::shared_ptr< CALIBRATION > K_
shared pointer to calibration object
Definition ProjectionFactor.h:45
│ │ │ │ -
gtsam::GenericProjectionFactor::~GenericProjectionFactor
~GenericProjectionFactor() override
Virtual destructor.
Definition ProjectionFactor.h:104
│ │ │ │ -
gtsam::GenericProjectionFactor::evaluateError
Vector evaluateError(const Pose3 &pose, const Point3 &point, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Evaluate error h(x)-z and optionally derivatives.
Definition ProjectionFactor.h:135
│ │ │ │ -
gtsam::GenericProjectionFactor::GenericProjectionFactor
GenericProjectionFactor()
Default constructor.
Definition ProjectionFactor.h:64
│ │ │ │ -
gtsam::GenericProjectionFactor::throwCheirality_
bool throwCheirality_
If true, rethrows Cheirality exceptions (default: false)
Definition ProjectionFactor.h:49
│ │ │ │ -
gtsam::GenericProjectionFactor::Base
NoiseModelFactorN< POSE, LANDMARK > Base
shorthand for base class type
Definition ProjectionFactor.h:55
│ │ │ │ -
gtsam::GenericProjectionFactor::body_P_sensor
const boost::optional< POSE > & body_P_sensor() const
return the (optional) sensor pose with respect to the vehicle frame
Definition ProjectionFactor.h:176
│ │ │ │ -
gtsam::GenericProjectionFactor::throwCheirality
bool throwCheirality() const
return flag for throwing cheirality exceptions
Definition ProjectionFactor.h:184
│ │ │ │ -
gtsam::GenericProjectionFactor::GenericProjectionFactor
GenericProjectionFactor(const Point2 &measured, const SharedNoiseModel &model, Key poseKey, Key pointKey, const boost::shared_ptr< CALIBRATION > &K, bool throwCheirality, bool verboseCheirality, boost::optional< POSE > body_P_sensor=boost::none)
Constructor with exception-handling flags TODO: Mark argument order standard (keys,...
Definition ProjectionFactor.h:96
│ │ │ │ -
gtsam::GenericProjectionFactor::access
friend class boost::serialization::access
Serialization function.
Definition ProjectionFactor.h:189
│ │ │ │ -
gtsam::GenericProjectionFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition ProjectionFactor.h:107
│ │ │ │ -
gtsam::GenericProjectionFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition ProjectionFactor.h:61
│ │ │ │ -
gtsam::GenericProjectionFactor::verboseCheirality_
bool verboseCheirality_
If true, prints text for Cheirality exceptions (default: false)
Definition ProjectionFactor.h:50
│ │ │ │ -
gtsam::GenericProjectionFactor::GenericProjectionFactor
GenericProjectionFactor(const Point2 &measured, const SharedNoiseModel &model, Key poseKey, Key pointKey, const boost::shared_ptr< CALIBRATION > &K, boost::optional< POSE > body_P_sensor=boost::none)
Constructor TODO: Mark argument order standard (keys, measurement, parameters)
Definition ProjectionFactor.h:78
│ │ │ │ -
gtsam::GenericProjectionFactor::calibration
const boost::shared_ptr< CALIBRATION > calibration() const
return the calibration object
Definition ProjectionFactor.h:171
│ │ │ │ +
gtsam::BetweenFactor
A class for a measurement predicted by "between(config[key1],config[key2])".
Definition BetweenFactor.h:40
│ │ │ │ +
gtsam::BetweenFactor::BetweenFactor
BetweenFactor()
default constructor - only use for serialization
Definition BetweenFactor.h:66
│ │ │ │ +
gtsam::BetweenFactor::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
assert equality up to a tolerance
Definition BetweenFactor.h:98
│ │ │ │ +
gtsam::BetweenFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition BetweenFactor.h:79
│ │ │ │ +
gtsam::BetweenFactor::shared_ptr
boost::shared_ptr< BetweenFactor > shared_ptr
The measurement.
Definition BetweenFactor.h:60
│ │ │ │ +
gtsam::BetweenFactor::measured
const VALUE & measured() const
return the measurement
Definition BetweenFactor.h:128
│ │ │ │ +
gtsam::BetweenFactor::BetweenFactor
BetweenFactor(Key key1, Key key2, const VALUE &measured, const SharedNoiseModel &model=nullptr)
Constructor.
Definition BetweenFactor.h:69
│ │ │ │ +
gtsam::BetweenFactor::access
friend class boost::serialization::access
Serialization function.
Definition BetweenFactor.h:136
│ │ │ │ +
gtsam::BetweenFactor::evaluateError
Vector evaluateError(const T &p1, const T &p2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
evaluate error, returns vector of errors size of tangent space
Definition BetweenFactor.h:108
│ │ │ │ +
gtsam::BetweenFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print with optional string
Definition BetweenFactor.h:87
│ │ │ │ +
gtsam::BetweenConstraint
Binary between constraint - forces between to a given value This constraint requires the underlying t...
Definition BetweenFactor.h:161
│ │ │ │ +
gtsam::BetweenConstraint::BetweenConstraint
BetweenConstraint(const VALUE &measured, Key key1, Key key2, double mu=1000.0)
Syntactic sugar for constrained version.
Definition BetweenFactor.h:166
│ │ │ │ +
gtsam::BetweenConstraint::access
friend class boost::serialization::access
Serialization function.
Definition BetweenFactor.h:174
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,205 +1,187 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ProjectionFactor.h │ │ │ │ │ +BetweenFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -21#pragma once │ │ │ │ │ -22 │ │ │ │ │ -23#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_i_n_h_o_l_e_C_a_m_e_r_a_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_3_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_3_._h> │ │ │ │ │ -27#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3___S_2_._h> │ │ │ │ │ -28#include │ │ │ │ │ -29 │ │ │ │ │ -30namespace _g_t_s_a_m { │ │ │ │ │ +16#pragma once │ │ │ │ │ +17 │ │ │ │ │ +18#include │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_b_a_s_e_/_L_i_e_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +23 │ │ │ │ │ +24#ifdef _WIN32 │ │ │ │ │ +25#define BETWEENFACTOR_VISIBILITY │ │ │ │ │ +26#else │ │ │ │ │ +27// This will trigger a LNKxxxx on MSVC, so disable for MSVC build │ │ │ │ │ +28// Please refer to https://github.com/borglab/gtsam/blob/develop/Using-GTSAM- │ │ │ │ │ +EXPORT.md │ │ │ │ │ +29#define BETWEENFACTOR_VISIBILITY GTSAM_EXPORT │ │ │ │ │ +30#endif │ │ │ │ │ 31 │ │ │ │ │ -38 template │ │ │ │ │ -_4_0 class _G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -41 protected: │ │ │ │ │ -42 │ │ │ │ │ -43 // Keep a copy of measurement and calibration for I/O │ │ │ │ │ -_4_4 _P_o_i_n_t_2 _m_e_a_s_u_r_e_d__; │ │ │ │ │ -_4_5 boost::shared_ptr _K__; │ │ │ │ │ -_4_6 boost::optional _b_o_d_y___P___s_e_n_s_o_r__; │ │ │ │ │ +32namespace _g_t_s_a_m { │ │ │ │ │ +33 │ │ │ │ │ +39 template │ │ │ │ │ +_4_0 class _B_e_t_w_e_e_n_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +41 │ │ │ │ │ +42 // Check that VALUE type is a testable Lie group │ │ │ │ │ +43 BOOST_CONCEPT_ASSERT((_I_s_T_e_s_t_a_b_l_e_<_V_A_L_U_E_>)); │ │ │ │ │ +44 BOOST_CONCEPT_ASSERT((_I_s_L_i_e_G_r_o_u_p_<_V_A_L_U_E_>)); │ │ │ │ │ +45 │ │ │ │ │ +46 public: │ │ │ │ │ 47 │ │ │ │ │ -48 // verbosity handling for Cheirality Exceptions │ │ │ │ │ -_4_9 bool _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__; │ │ │ │ │ -_5_0 bool _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__; │ │ │ │ │ +48 typedef VALUE T; │ │ │ │ │ +49 │ │ │ │ │ +50 private: │ │ │ │ │ 51 │ │ │ │ │ -52 public: │ │ │ │ │ -53 │ │ │ │ │ -_5_5 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_P_O_S_E_,_ _L_A_N_D_M_A_R_K_> _B_a_s_e; │ │ │ │ │ -56 │ │ │ │ │ -_5_8 typedef _G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_<_P_O_S_E_,_ _L_A_N_D_M_A_R_K_,_ _C_A_L_I_B_R_A_T_I_O_N_> _T_h_i_s; │ │ │ │ │ -59 │ │ │ │ │ -_6_1 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -62 │ │ │ │ │ -_6_4 _G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r() : │ │ │ │ │ -65 _m_e_a_s_u_r_e_d__(0, 0), _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__(false), _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__(false) { │ │ │ │ │ -66 } │ │ │ │ │ +52 typedef _B_e_t_w_e_e_n_F_a_c_t_o_r_<_V_A_L_U_E_> This; │ │ │ │ │ +53 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_V_A_L_U_E_,_ _V_A_L_U_E_> _B_a_s_e; │ │ │ │ │ +54 │ │ │ │ │ +55 VALUE measured_; │ │ │ │ │ +57 public: │ │ │ │ │ +58 │ │ │ │ │ +59 // shorthand for a smart pointer to a factor │ │ │ │ │ +_6_0 typedef typename boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +61 │ │ │ │ │ +64 │ │ │ │ │ +_6_6 _B_e_t_w_e_e_n_F_a_c_t_o_r() {} │ │ │ │ │ 67 │ │ │ │ │ -_7_8 _G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r(const _P_o_i_n_t_2& _m_e_a_s_u_r_e_d, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& │ │ │ │ │ -model, │ │ │ │ │ -79 _K_e_y poseKey, _K_e_y pointKey, const boost::shared_ptr& K, │ │ │ │ │ -80 boost::optional _b_o_d_y___P___s_e_n_s_o_r = boost::none) : │ │ │ │ │ -81 _B_a_s_e(model, poseKey, pointKey), _m_e_a_s_u_r_e_d__(_m_e_a_s_u_r_e_d), _K__(K), _b_o_d_y___P___s_e_n_s_o_r__ │ │ │ │ │ -(_b_o_d_y___P___s_e_n_s_o_r), │ │ │ │ │ -82 _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__(false), _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__(false) {} │ │ │ │ │ -83 │ │ │ │ │ -_9_6 _G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r(const _P_o_i_n_t_2& _m_e_a_s_u_r_e_d, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& │ │ │ │ │ -model, │ │ │ │ │ -97 _K_e_y poseKey, _K_e_y pointKey, const boost::shared_ptr& K, │ │ │ │ │ -98 bool _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y, bool _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y, │ │ │ │ │ -99 boost::optional _b_o_d_y___P___s_e_n_s_o_r = boost::none) : │ │ │ │ │ -100 _B_a_s_e(model, poseKey, pointKey), _m_e_a_s_u_r_e_d__(_m_e_a_s_u_r_e_d), _K__(K), _b_o_d_y___P___s_e_n_s_o_r__ │ │ │ │ │ -(_b_o_d_y___P___s_e_n_s_o_r), │ │ │ │ │ -101 _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__(_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y), _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__(_v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y) {} │ │ │ │ │ +_6_9 _B_e_t_w_e_e_n_F_a_c_t_o_r(_K_e_y key1, _K_e_y key2, const VALUE& _m_e_a_s_u_r_e_d, │ │ │ │ │ +70 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model = nullptr) : │ │ │ │ │ +71 _B_a_s_e(model, key1, key2), measured_(_m_e_a_s_u_r_e_d) { │ │ │ │ │ +72 } │ │ │ │ │ +73 │ │ │ │ │ +75 │ │ │ │ │ +76 _~_B_e_t_w_e_e_n_F_a_c_t_o_r() override {} │ │ │ │ │ +77 │ │ │ │ │ +_7_9 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +80 return boost::static_pointer_cast( │ │ │ │ │ +81 gtsam::NonlinearFactor::shared_ptr(new This(*this))); } │ │ │ │ │ +82 │ │ │ │ │ +85 │ │ │ │ │ +_8_7 void _p_r_i_n_t( │ │ │ │ │ +88 const std::string& s = "", │ │ │ │ │ +89 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ +90 std::cout << s << "BetweenFactor(" │ │ │ │ │ +91 << keyFormatter(this->key1()) << "," │ │ │ │ │ +92 << keyFormatter(this->key2()) << ")\n"; │ │ │ │ │ +93 _t_r_a_i_t_s_<_T_>_:_:_P_r_i_n_t(measured_, " measured: "); │ │ │ │ │ +94 this->noiseModel_->print(" noise model: "); │ │ │ │ │ +95 } │ │ │ │ │ +96 │ │ │ │ │ +_9_8 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& expected, double tol=1e-9) const override │ │ │ │ │ +{ │ │ │ │ │ +99 const This *e = dynamic_cast (&expected); │ │ │ │ │ +100 return e != nullptr && _B_a_s_e_:_:_e_q_u_a_l_s(*e, tol) && _t_r_a_i_t_s_<_T_>_:_:_E_q_u_a_l_s(this- │ │ │ │ │ +>measured_, e->measured_, tol); │ │ │ │ │ +101 } │ │ │ │ │ 102 │ │ │ │ │ -_1_0_4 _~_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r() override {} │ │ │ │ │ -105 │ │ │ │ │ -_1_0_7 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -108 return boost::static_pointer_cast( │ │ │ │ │ -109 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); } │ │ │ │ │ -110 │ │ │ │ │ -_1_1_6 void _p_r_i_n_t(const std::string& s = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ -DefaultKeyFormatter) const override { │ │ │ │ │ -117 std::cout << s << "GenericProjectionFactor, z = "; │ │ │ │ │ -118 _t_r_a_i_t_s_<_P_o_i_n_t_2_>_:_:_P_r_i_n_t(_m_e_a_s_u_r_e_d__); │ │ │ │ │ -119 if(this->body_P_sensor_) │ │ │ │ │ -120 this->body_P_sensor_->print(" sensor pose in body frame: "); │ │ │ │ │ -121 _B_a_s_e_:_:_p_r_i_n_t("", keyFormatter); │ │ │ │ │ -122 } │ │ │ │ │ -123 │ │ │ │ │ -_1_2_5 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& p, double tol = 1e-9) const override { │ │ │ │ │ -126 const _T_h_i_s *e = dynamic_cast(&p); │ │ │ │ │ -127 return e │ │ │ │ │ -128 && _B_a_s_e_:_:_e_q_u_a_l_s(p, tol) │ │ │ │ │ -129 && _t_r_a_i_t_s_<_P_o_i_n_t_2_>_:_:_E_q_u_a_l_s(this->measured_, e->measured_, tol) │ │ │ │ │ -130 && this->K_->equals(*e->K_, tol) │ │ │ │ │ -131 && ((!_b_o_d_y___P___s_e_n_s_o_r__ && !e->body_P_sensor_) || (_b_o_d_y___P___s_e_n_s_o_r__ && e- │ │ │ │ │ ->body_P_sensor_ && _b_o_d_y___P___s_e_n_s_o_r__->equals(*e->body_P_sensor_))); │ │ │ │ │ -132 } │ │ │ │ │ -133 │ │ │ │ │ -_1_3_5 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const _P_o_s_e_3& pose, const _P_o_i_n_t_3& point, │ │ │ │ │ -136 boost::optional H1 = boost::none, boost::optional H2 = │ │ │ │ │ -boost::none) const override { │ │ │ │ │ -137 try { │ │ │ │ │ -138 if(_b_o_d_y___P___s_e_n_s_o_r__) { │ │ │ │ │ -139 if(H1) { │ │ │ │ │ -140 gtsam::Matrix H0; │ │ │ │ │ -141 _P_i_n_h_o_l_e_C_a_m_e_r_a_<_C_A_L_I_B_R_A_T_I_O_N_> camera(pose.compose(*_b_o_d_y___P___s_e_n_s_o_r__, H0), *_K__); │ │ │ │ │ -142 _P_o_i_n_t_2 reprojectionError(camera._p_r_o_j_e_c_t(point, H1, H2, boost::none) - │ │ │ │ │ -_m_e_a_s_u_r_e_d__); │ │ │ │ │ -143 *H1 = *H1 * H0; │ │ │ │ │ -144 return reprojectionError; │ │ │ │ │ -145 } else { │ │ │ │ │ -146 _P_i_n_h_o_l_e_C_a_m_e_r_a_<_C_A_L_I_B_R_A_T_I_O_N_> camera(pose.compose(*_b_o_d_y___P___s_e_n_s_o_r__), *_K__); │ │ │ │ │ -147 return camera._p_r_o_j_e_c_t(point, H1, H2, boost::none) - _m_e_a_s_u_r_e_d__; │ │ │ │ │ -148 } │ │ │ │ │ -149 } else { │ │ │ │ │ -150 _P_i_n_h_o_l_e_C_a_m_e_r_a_<_C_A_L_I_B_R_A_T_I_O_N_> camera(pose, *_K__); │ │ │ │ │ -151 return camera._p_r_o_j_e_c_t(point, H1, H2, boost::none) - _m_e_a_s_u_r_e_d__; │ │ │ │ │ -152 } │ │ │ │ │ -153 } catch( _C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n& e) { │ │ │ │ │ -154 if (H1) *H1 = Matrix::Zero(2,6); │ │ │ │ │ -155 if (H2) *H2 = Matrix::Zero(2,3); │ │ │ │ │ -156 if (_v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__) │ │ │ │ │ -157 std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) │ │ │ │ │ -<< │ │ │ │ │ -158 " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; │ │ │ │ │ -159 if (_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__) │ │ │ │ │ -160 throw _C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n(this->key2()); │ │ │ │ │ -161 } │ │ │ │ │ -162 return Vector2::Constant(2.0 * _K__->fx()); │ │ │ │ │ -163 } │ │ │ │ │ +106 │ │ │ │ │ +_1_0_8 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const T& p1, const T& p2, boost::optional H1 │ │ │ │ │ += │ │ │ │ │ +109 boost::none, boost::optional H2 = boost::none) const override { │ │ │ │ │ +110 T hx = _t_r_a_i_t_s_<_T_>_:_:_B_e_t_w_e_e_n(p1, p2, H1, H2); // h(x) │ │ │ │ │ +111 // manifold equivalent of h(x)-z -> log(z,h(x)) │ │ │ │ │ +112#ifdef GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR │ │ │ │ │ +113 typename _t_r_a_i_t_s_<_T_>_:_:_C_h_a_r_t_J_a_c_o_b_i_a_n_:_:_J_a_c_o_b_i_a_n Hlocal; │ │ │ │ │ +114 Vector rval = _t_r_a_i_t_s_<_T_>_:_:_L_o_c_a_l(measured_, hx, boost::none, (H1 || H2) ? │ │ │ │ │ +&Hlocal : 0); │ │ │ │ │ +115 if (H1) *H1 = Hlocal * (*H1); │ │ │ │ │ +116 if (H2) *H2 = Hlocal * (*H2); │ │ │ │ │ +117 return rval; │ │ │ │ │ +118#else │ │ │ │ │ +119 return _t_r_a_i_t_s_<_T_>_:_:_L_o_c_a_l(measured_, hx); │ │ │ │ │ +120#endif │ │ │ │ │ +121 } │ │ │ │ │ +122 │ │ │ │ │ +126 │ │ │ │ │ +_1_2_8 const VALUE& _m_e_a_s_u_r_e_d() const { │ │ │ │ │ +129 return measured_; │ │ │ │ │ +130 } │ │ │ │ │ +132 │ │ │ │ │ +133 private: │ │ │ │ │ +134 │ │ │ │ │ +_1_3_6 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +137 template │ │ │ │ │ +138 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +139 // NoiseModelFactor2 instead of NoiseModelFactorN for backward │ │ │ │ │ +compatibility │ │ │ │ │ +140 ar & boost::serialization::make_nvp("NoiseModelFactor2", │ │ │ │ │ +141 boost::serialization::base_object(*this)); │ │ │ │ │ +142 ar & BOOST_SERIALIZATION_NVP(measured_); │ │ │ │ │ +143 } │ │ │ │ │ +144 │ │ │ │ │ +145 // Alignment, see https://eigen.tuxfamily.org/dox/ │ │ │ │ │ +group__TopicStructHavingEigenMembers.html │ │ │ │ │ +146 enum { NeedsToAlign = (sizeof(VALUE) % 16) == 0 }; │ │ │ │ │ +147 public: │ │ │ │ │ +148 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W___I_F(NeedsToAlign) │ │ │ │ │ +149 }; // \class BetweenFactor │ │ │ │ │ +150 │ │ │ │ │ +152 template │ │ │ │ │ +_1_5_3 struct _t_r_a_i_t_s<_B_e_t_w_e_e_n_F_a_c_t_o_r > : public _T_e_s_t_a_b_l_e │ │ │ │ │ +> {}; │ │ │ │ │ +154 │ │ │ │ │ +160 template │ │ │ │ │ +_1_6_1 class _B_e_t_w_e_e_n_C_o_n_s_t_r_a_i_n_t : public _B_e_t_w_e_e_n_F_a_c_t_o_r { │ │ │ │ │ +162 public: │ │ │ │ │ +163 typedef boost::shared_ptr > shared_ptr; │ │ │ │ │ 164 │ │ │ │ │ -_1_6_6 const _P_o_i_n_t_2& _m_e_a_s_u_r_e_d() const { │ │ │ │ │ -167 return _m_e_a_s_u_r_e_d__; │ │ │ │ │ -168 } │ │ │ │ │ -169 │ │ │ │ │ -_1_7_1 const boost::shared_ptr _c_a_l_i_b_r_a_t_i_o_n() const { │ │ │ │ │ -172 return _K__; │ │ │ │ │ -173 } │ │ │ │ │ -174 │ │ │ │ │ -_1_7_6 const boost::optional& _b_o_d_y___P___s_e_n_s_o_r() const { │ │ │ │ │ -177 return _b_o_d_y___P___s_e_n_s_o_r__; │ │ │ │ │ -178 } │ │ │ │ │ -179 │ │ │ │ │ -_1_8_1 inline bool _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y() const { return _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__; } │ │ │ │ │ -182 │ │ │ │ │ -_1_8_4 inline bool _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y() const { return _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__; } │ │ │ │ │ +_1_6_6 _B_e_t_w_e_e_n_C_o_n_s_t_r_a_i_n_t(const VALUE& _m_e_a_s_u_r_e_d, _K_e_y key1, _K_e_y key2, double mu = │ │ │ │ │ +1000.0) : │ │ │ │ │ +167 _B_e_t_w_e_e_n_F_a_c_t_o_r(key1, key2, _m_e_a_s_u_r_e_d, │ │ │ │ │ +168 _n_o_i_s_e_M_o_d_e_l::Constrained::All(_t_r_a_i_t_s::GetDimension(_m_e_a_s_u_r_e_d), std:: │ │ │ │ │ +abs(mu))) │ │ │ │ │ +169 {} │ │ │ │ │ +170 │ │ │ │ │ +171 private: │ │ │ │ │ +172 │ │ │ │ │ +_1_7_4 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +175 template │ │ │ │ │ +176 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +177 ar & boost::serialization::make_nvp("BetweenFactor", │ │ │ │ │ +178 boost::serialization::base_object<_B_e_t_w_e_e_n_F_a_c_t_o_r_<_V_A_L_U_E_> >(*this)); │ │ │ │ │ +179 } │ │ │ │ │ +180 }; // \class BetweenConstraint │ │ │ │ │ +181 │ │ │ │ │ +183 template │ │ │ │ │ +_1_8_4 struct _t_r_a_i_t_s<_B_e_t_w_e_e_n_C_o_n_s_t_r_a_i_n_t > : public │ │ │ │ │ +_T_e_s_t_a_b_l_e > {}; │ │ │ │ │ 185 │ │ │ │ │ -186 private: │ │ │ │ │ -187 │ │ │ │ │ -_1_8_9 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -190 template │ │ │ │ │ -191 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -192 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -193 ar & BOOST_SERIALIZATION_NVP(_m_e_a_s_u_r_e_d__); │ │ │ │ │ -194 ar & BOOST_SERIALIZATION_NVP(_K__); │ │ │ │ │ -195 ar & BOOST_SERIALIZATION_NVP(_b_o_d_y___P___s_e_n_s_o_r__); │ │ │ │ │ -196 ar & BOOST_SERIALIZATION_NVP(_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__); │ │ │ │ │ -197 ar & BOOST_SERIALIZATION_NVP(_v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__); │ │ │ │ │ -198 } │ │ │ │ │ -199 │ │ │ │ │ -200 public: │ │ │ │ │ -201 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -202}; │ │ │ │ │ -203 │ │ │ │ │ -205 template │ │ │ │ │ -_2_0_6 struct _t_r_a_i_t_s<_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r > : │ │ │ │ │ -207 public _T_e_s_t_a_b_l_e > { │ │ │ │ │ -208 }; │ │ │ │ │ -209 │ │ │ │ │ -210} // \ namespace gtsam │ │ │ │ │ -_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ +186} │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ +_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W___I_F │ │ │ │ │ +#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) │ │ │ │ │ This marks a GTSAM object to require alignment. │ │ │ │ │ -DDeeffiinniittiioonn types.h:308 │ │ │ │ │ -_C_a_l_3___S_2_._h │ │ │ │ │ -The most common 5DOF 3D->2D calibration. │ │ │ │ │ -_P_i_n_h_o_l_e_C_a_m_e_r_a_._h │ │ │ │ │ -Base class for all pinhole cameras. │ │ │ │ │ -_P_o_s_e_3_._h │ │ │ │ │ -3D Pose │ │ │ │ │ -_P_o_i_n_t_3_._h │ │ │ │ │ -3D Point │ │ │ │ │ +DDeeffiinniittiioonn types.h:317 │ │ │ │ │ +_L_i_e_._h │ │ │ │ │ +Base class and basic functions for Lie types. │ │ │ │ │ _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ -Vector2 Point2 │ │ │ │ │ -As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point2 to Vector2... │ │ │ │ │ -DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ -Vector3 Point3 │ │ │ │ │ -As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point3 to Vector3... │ │ │ │ │ -DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ _g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ Aliases. │ │ │ │ │ DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ @@ -208,143 +190,89 @@ │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_I_s_L_i_e_G_r_o_u_p │ │ │ │ │ +Lie Group Concept. │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:260 │ │ │ │ │ +_g_t_s_a_m_:_:_I_s_T_e_s_t_a_b_l_e │ │ │ │ │ +A testable concept check that should be placed in applicable unit tests and in │ │ │ │ │ +generic algorithms. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:58 │ │ │ │ │ _g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ A helper that implements the traits interface for GTSAM types. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a │ │ │ │ │ -A pinhole camera class that has a Pose3 and a Calibration. │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:33 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_p_r_o_j_e_c_t │ │ │ │ │ -Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, │ │ │ │ │ -OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > │ │ │ │ │ -Dcal=boost::none) const │ │ │ │ │ -project a 3D point from world coordinates into the image │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:118 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ -A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ -virtual void print(const std::string &s="Factor", const KeyFormatter │ │ │ │ │ -&formatter=DefaultKeyFormatter) const │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn Factor.cpp:29 │ │ │ │ │ _g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ bool equals(const This &other, double tol=1e-9) const │ │ │ │ │ check equality │ │ │ │ │ DDeeffiinniittiioonn Factor.cpp:42 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ Nonlinear factor base class. │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_n_o_i_s_e_M_o_d_e_l │ │ │ │ │ +const SharedNoiseModel & noiseModel() const │ │ │ │ │ +access to the noise model │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:223 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ variables. │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -Non-linear factor for a constraint derived from a 2D measurement. │ │ │ │ │ -DDeeffiinniittiioonn ProjectionFactor.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y │ │ │ │ │ -bool verboseCheirality() const │ │ │ │ │ -return verbosity │ │ │ │ │ -DDeeffiinniittiioonn ProjectionFactor.h:181 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +_g_t_s_a_m_:_:_B_e_t_w_e_e_n_F_a_c_t_o_r │ │ │ │ │ +A class for a measurement predicted by "between(config[key1],config[key2])". │ │ │ │ │ +DDeeffiinniittiioonn BetweenFactor.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_B_e_t_w_e_e_n_F_a_c_t_o_r_:_:_B_e_t_w_e_e_n_F_a_c_t_o_r │ │ │ │ │ +BetweenFactor() │ │ │ │ │ +default constructor - only use for serialization │ │ │ │ │ +DDeeffiinniittiioonn BetweenFactor.h:66 │ │ │ │ │ +_g_t_s_a_m_:_:_B_e_t_w_e_e_n_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const NonlinearFactor &expected, double tol=1e-9) const override │ │ │ │ │ +assert equality up to a tolerance │ │ │ │ │ +DDeeffiinniittiioonn BetweenFactor.h:98 │ │ │ │ │ +_g_t_s_a_m_:_:_B_e_t_w_e_e_n_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ +gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn BetweenFactor.h:79 │ │ │ │ │ +_g_t_s_a_m_:_:_B_e_t_w_e_e_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< BetweenFactor > shared_ptr │ │ │ │ │ +The measurement. │ │ │ │ │ +DDeeffiinniittiioonn BetweenFactor.h:60 │ │ │ │ │ +_g_t_s_a_m_:_:_B_e_t_w_e_e_n_F_a_c_t_o_r_:_:_m_e_a_s_u_r_e_d │ │ │ │ │ +const VALUE & measured() const │ │ │ │ │ +return the measurement │ │ │ │ │ +DDeeffiinniittiioonn BetweenFactor.h:128 │ │ │ │ │ +_g_t_s_a_m_:_:_B_e_t_w_e_e_n_F_a_c_t_o_r_:_:_B_e_t_w_e_e_n_F_a_c_t_o_r │ │ │ │ │ +BetweenFactor(Key key1, Key key2, const VALUE &measured, const SharedNoiseModel │ │ │ │ │ +&model=nullptr) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn BetweenFactor.h:69 │ │ │ │ │ +_g_t_s_a_m_:_:_B_e_t_w_e_e_n_F_a_c_t_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn BetweenFactor.h:136 │ │ │ │ │ +_g_t_s_a_m_:_:_B_e_t_w_e_e_n_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ +Vector evaluateError(const T &p1, const T &p2, boost::optional< Matrix & > │ │ │ │ │ +H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override │ │ │ │ │ +evaluate error, returns vector of errors size of tangent space │ │ │ │ │ +DDeeffiinniittiioonn BetweenFactor.h:108 │ │ │ │ │ +_g_t_s_a_m_:_:_B_e_t_w_e_e_n_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ void print(const std::string &s="", const KeyFormatter │ │ │ │ │ &keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn ProjectionFactor.h:116 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const NonlinearFactor &p, double tol=1e-9) const override │ │ │ │ │ -equals │ │ │ │ │ -DDeeffiinniittiioonn ProjectionFactor.h:125 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_m_e_a_s_u_r_e_d__ │ │ │ │ │ -Point2 measured_ │ │ │ │ │ -2D measurement │ │ │ │ │ -DDeeffiinniittiioonn ProjectionFactor.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_T_h_i_s │ │ │ │ │ -GenericProjectionFactor< POSE, LANDMARK, CALIBRATION > This │ │ │ │ │ -shorthand for this class │ │ │ │ │ -DDeeffiinniittiioonn ProjectionFactor.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_m_e_a_s_u_r_e_d │ │ │ │ │ -const Point2 & measured() const │ │ │ │ │ -return the measurement │ │ │ │ │ -DDeeffiinniittiioonn ProjectionFactor.h:166 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_b_o_d_y___P___s_e_n_s_o_r__ │ │ │ │ │ -boost::optional< POSE > body_P_sensor_ │ │ │ │ │ -The pose of the sensor in the body frame. │ │ │ │ │ -DDeeffiinniittiioonn ProjectionFactor.h:46 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_K__ │ │ │ │ │ -boost::shared_ptr< CALIBRATION > K_ │ │ │ │ │ -shared pointer to calibration object │ │ │ │ │ -DDeeffiinniittiioonn ProjectionFactor.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_~_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -~GenericProjectionFactor() override │ │ │ │ │ -Virtual destructor. │ │ │ │ │ -DDeeffiinniittiioonn ProjectionFactor.h:104 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ -Vector evaluateError(const Pose3 &pose, const Point3 &point, boost::optional< │ │ │ │ │ -Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const │ │ │ │ │ -override │ │ │ │ │ -Evaluate error h(x)-z and optionally derivatives. │ │ │ │ │ -DDeeffiinniittiioonn ProjectionFactor.h:135 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -GenericProjectionFactor() │ │ │ │ │ -Default constructor. │ │ │ │ │ -DDeeffiinniittiioonn ProjectionFactor.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__ │ │ │ │ │ -bool throwCheirality_ │ │ │ │ │ -If true, rethrows Cheirality exceptions (default: false) │ │ │ │ │ -DDeeffiinniittiioonn ProjectionFactor.h:49 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_B_a_s_e │ │ │ │ │ -NoiseModelFactorN< POSE, LANDMARK > Base │ │ │ │ │ -shorthand for base class type │ │ │ │ │ -DDeeffiinniittiioonn ProjectionFactor.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_b_o_d_y___P___s_e_n_s_o_r │ │ │ │ │ -const boost::optional< POSE > & body_P_sensor() const │ │ │ │ │ -return the (optional) sensor pose with respect to the vehicle frame │ │ │ │ │ -DDeeffiinniittiioonn ProjectionFactor.h:176 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y │ │ │ │ │ -bool throwCheirality() const │ │ │ │ │ -return flag for throwing cheirality exceptions │ │ │ │ │ -DDeeffiinniittiioonn ProjectionFactor.h:184 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -GenericProjectionFactor(const Point2 &measured, const SharedNoiseModel &model, │ │ │ │ │ -Key poseKey, Key pointKey, const boost::shared_ptr< CALIBRATION > &K, bool │ │ │ │ │ -throwCheirality, bool verboseCheirality, boost::optional< POSE > │ │ │ │ │ -body_P_sensor=boost::none) │ │ │ │ │ -Constructor with exception-handling flags TODO: Mark argument order standard │ │ │ │ │ -(keys,... │ │ │ │ │ -DDeeffiinniittiioonn ProjectionFactor.h:96 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ +print with optional string │ │ │ │ │ +DDeeffiinniittiioonn BetweenFactor.h:87 │ │ │ │ │ +_g_t_s_a_m_:_:_B_e_t_w_e_e_n_C_o_n_s_t_r_a_i_n_t │ │ │ │ │ +Binary between constraint - forces between to a given value This constraint │ │ │ │ │ +requires the underlying t... │ │ │ │ │ +DDeeffiinniittiioonn BetweenFactor.h:161 │ │ │ │ │ +_g_t_s_a_m_:_:_B_e_t_w_e_e_n_C_o_n_s_t_r_a_i_n_t_:_:_B_e_t_w_e_e_n_C_o_n_s_t_r_a_i_n_t │ │ │ │ │ +BetweenConstraint(const VALUE &measured, Key key1, Key key2, double mu=1000.0) │ │ │ │ │ +Syntactic sugar for constrained version. │ │ │ │ │ +DDeeffiinniittiioonn BetweenFactor.h:166 │ │ │ │ │ +_g_t_s_a_m_:_:_B_e_t_w_e_e_n_C_o_n_s_t_r_a_i_n_t_:_:_a_c_c_e_s_s │ │ │ │ │ friend class boost::serialization::access │ │ │ │ │ Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn ProjectionFactor.h:189 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ -gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn ProjectionFactor.h:107 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shorthand for a smart pointer to a factor │ │ │ │ │ -DDeeffiinniittiioonn ProjectionFactor.h:61 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__ │ │ │ │ │ -bool verboseCheirality_ │ │ │ │ │ -If true, prints text for Cheirality exceptions (default: false) │ │ │ │ │ -DDeeffiinniittiioonn ProjectionFactor.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -GenericProjectionFactor(const Point2 &measured, const SharedNoiseModel &model, │ │ │ │ │ -Key poseKey, Key pointKey, const boost::shared_ptr< CALIBRATION > &K, boost:: │ │ │ │ │ -optional< POSE > body_P_sensor=boost::none) │ │ │ │ │ -Constructor TODO: Mark argument order standard (keys, measurement, parameters) │ │ │ │ │ -DDeeffiinniittiioonn ProjectionFactor.h:78 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_c_a_l_i_b_r_a_t_i_o_n │ │ │ │ │ -const boost::shared_ptr< CALIBRATION > calibration() const │ │ │ │ │ -return the calibration object │ │ │ │ │ -DDeeffiinniittiioonn ProjectionFactor.h:171 │ │ │ │ │ +DDeeffiinniittiioonn BetweenFactor.h:174 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_._h │ │ │ │ │ + * _B_e_t_w_e_e_n_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01286_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/ReferenceFrameFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/TriangulationFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,153 +98,245 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ReferenceFrameFactor.h
│ │ │ │ +
TriangulationFactor.h
│ │ │ │
│ │ │ │
│ │ │ │
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
12/*
│ │ │ │ -
13 * @file ReferenceFrameFactor.h
│ │ │ │ -
14 * @brief A constraint for combining graphs by common landmarks and a transform node
│ │ │ │ -
15 * @author Alex Cunningham
│ │ │ │ -
16 */
│ │ │ │ -
17
│ │ │ │
18#pragma once
│ │ │ │
19
│ │ │ │ -
20#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
21
│ │ │ │ -
22namespace gtsam {
│ │ │ │ -
23
│ │ │ │ -
29template<class T, class P>
│ │ │ │ -
│ │ │ │ -
30P transform_point(
│ │ │ │ -
31 const T& trans, const P& global,
│ │ │ │ -
32 boost::optional<Matrix&> Dtrans,
│ │ │ │ -
33 boost::optional<Matrix&> Dglobal) {
│ │ │ │ -
34 return trans.transformFrom(global, Dtrans, Dglobal);
│ │ │ │ -
35}
│ │ │ │ -
│ │ │ │ +
20#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
21#include <gtsam/geometry/CalibratedCamera.h>
│ │ │ │ +
22#include <boost/make_shared.hpp>
│ │ │ │ +
23#include <boost/lexical_cast.hpp>
│ │ │ │ +
24
│ │ │ │ +
25namespace gtsam {
│ │ │ │ +
26
│ │ │ │ +
32template<class CAMERA>
│ │ │ │ +
│ │ │ │ +
33class TriangulationFactor: public NoiseModelFactorN<Point3> {
│ │ │ │ +
34
│ │ │ │ +
35public:
│ │ │ │
36
│ │ │ │ -
56template<class POINT, class TRANSFORM>
│ │ │ │ -
│ │ │ │ -
57class ReferenceFrameFactor : public NoiseModelFactorN<POINT, TRANSFORM, POINT> {
│ │ │ │ -
58protected:
│ │ │ │ -
60 ReferenceFrameFactor() {}
│ │ │ │ +
38 using Camera = CAMERA;
│ │ │ │ +
39
│ │ │ │ +
40protected:
│ │ │ │ +
41
│ │ │ │ +
43 using Base = NoiseModelFactorN<Point3>;
│ │ │ │ +
44
│ │ │ │ +
46 using This = TriangulationFactor<CAMERA>;
│ │ │ │ +
47
│ │ │ │ +
49 using Measurement = typename CAMERA::Measurement;
│ │ │ │ +
50
│ │ │ │ +
51 // Keep a copy of measurement and calibration for I/O
│ │ │ │ +
52 const CAMERA camera_;
│ │ │ │ +
53 const Measurement measured_;
│ │ │ │ +
54
│ │ │ │ +
55 // verbosity handling for Cheirality Exceptions
│ │ │ │ +
56 const bool throwCheirality_;
│ │ │ │ +
57 const bool verboseCheirality_;
│ │ │ │ +
58
│ │ │ │ +
59public:
│ │ │ │ +
60 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │
61
│ │ │ │ -
62public:
│ │ │ │ -
63 typedef NoiseModelFactorN<POINT, TRANSFORM, POINT> Base;
│ │ │ │ -
64 typedef ReferenceFrameFactor<POINT, TRANSFORM> This;
│ │ │ │ -
65
│ │ │ │ -
66 typedef POINT Point;
│ │ │ │ -
67 typedef TRANSFORM Transform;
│ │ │ │ -
68
│ │ │ │ -
│ │ │ │ -
72 ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, const noiseModel::Base::shared_ptr& model)
│ │ │ │ -
73 : Base(model,globalKey, transKey, localKey) {}
│ │ │ │ +
63 using shared_ptr = boost::shared_ptr<This>;
│ │ │ │ +
64
│ │ │ │ +
│ │ │ │ +
66 TriangulationFactor() :
│ │ │ │ +
67 throwCheirality_(false), verboseCheirality_(false) {
│ │ │ │ +
68 }
│ │ │ │
│ │ │ │ -
74
│ │ │ │ +
69
│ │ │ │
│ │ │ │ -
79 ReferenceFrameFactor(double mu, Key globalKey, Key transKey, Key localKey)
│ │ │ │ -
80 : Base(globalKey, transKey, localKey, Point().dim(), mu) {}
│ │ │ │ +
79 TriangulationFactor(const CAMERA& camera, const Measurement& measured,
│ │ │ │ +
80 const SharedNoiseModel& model, Key pointKey, bool throwCheirality = false,
│ │ │ │ +
81 bool verboseCheirality = false) :
│ │ │ │ +
82 Base(model, pointKey), camera_(camera), measured_(measured), throwCheirality_(
│ │ │ │ +
83 throwCheirality), verboseCheirality_(verboseCheirality) {
│ │ │ │ +
84 if (model && model->dim() != traits<Measurement>::dimension)
│ │ │ │ +
85 throw std::invalid_argument(
│ │ │ │ +
86 "TriangulationFactor must be created with "
│ │ │ │ +
87 + boost::lexical_cast<std::string>((int) traits<Measurement>::dimension)
│ │ │ │ +
88 + "-dimensional noise model.");
│ │ │ │ +
89 }
│ │ │ │
│ │ │ │ -
81
│ │ │ │ -
│ │ │ │ -
86 ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, double sigma = 1e-2)
│ │ │ │ -
87 : Base(noiseModel::Isotropic::Sigma(traits<POINT>::dimension, sigma),
│ │ │ │ -
88 globalKey, transKey, localKey) {}
│ │ │ │ -
│ │ │ │ -
89
│ │ │ │ -
90 ~ReferenceFrameFactor() override{}
│ │ │ │ -
91
│ │ │ │ +
90
│ │ │ │
│ │ │ │ -
92 NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
93 return boost::static_pointer_cast<NonlinearFactor>(
│ │ │ │ -
94 NonlinearFactor::shared_ptr(new This(*this))); }
│ │ │ │ -
│ │ │ │ -
95
│ │ │ │ -
│ │ │ │ -
97 Vector evaluateError(const Point& global, const Transform& trans, const Point& local,
│ │ │ │ -
98 boost::optional<Matrix&> Dforeign = boost::none,
│ │ │ │ -
99 boost::optional<Matrix&> Dtrans = boost::none,
│ │ │ │ -
100 boost::optional<Matrix&> Dlocal = boost::none) const override {
│ │ │ │ -
101 Point newlocal = transform_point<Transform,Point>(trans, global, Dtrans, Dforeign);
│ │ │ │ -
102 if (Dlocal)
│ │ │ │ -
103 *Dlocal = -1* Matrix::Identity(traits<Point>::dimension, traits<Point>::dimension);
│ │ │ │ -
104 return traits<Point>::Local(local,newlocal);
│ │ │ │ -
105 }
│ │ │ │ -
│ │ │ │ -
106
│ │ │ │ -
│ │ │ │ -
107 void print(const std::string& s="",
│ │ │ │ -
108 const gtsam::KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ -
109 std::cout << s << ": ReferenceFrameFactor("
│ │ │ │ -
110 << "Global: " << keyFormatter(this->key1()) << ","
│ │ │ │ -
111 << " Transform: " << keyFormatter(this->key2()) << ","
│ │ │ │ -
112 << " Local: " << keyFormatter(this->key3()) << ")\n";
│ │ │ │ -
113 this->noiseModel_->print(" noise model");
│ │ │ │ -
114 }
│ │ │ │ -
│ │ │ │ -
115
│ │ │ │ -
116 // access - convenience functions
│ │ │ │ -
117 Key global_key() const { return this->key1(); }
│ │ │ │ -
118 Key transform_key() const { return this->key2(); }
│ │ │ │ -
119 Key local_key() const { return this->key3(); }
│ │ │ │ +
92 ~TriangulationFactor() override {
│ │ │ │ +
93 }
│ │ │ │ +
│ │ │ │ +
94
│ │ │ │ +
│ │ │ │ +
96 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
97 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ +
98 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
│ │ │ │ +
99 }
│ │ │ │ +
│ │ │ │ +
100
│ │ │ │ +
│ │ │ │ +
106 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
│ │ │ │ +
107 DefaultKeyFormatter) const override {
│ │ │ │ +
108 std::cout << s << "TriangulationFactor,";
│ │ │ │ +
109 camera_.print("camera");
│ │ │ │ +
110 traits<Measurement>::Print(measured_, "z");
│ │ │ │ +
111 Base::print("", keyFormatter);
│ │ │ │ +
112 }
│ │ │ │ +
│ │ │ │ +
113
│ │ │ │ +
│ │ │ │ +
115 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
│ │ │ │ +
116 const This *e = dynamic_cast<const This*>(&p);
│ │ │ │ +
117 return e && Base::equals(p, tol) && this->camera_.equals(e->camera_, tol)
│ │ │ │ +
118 && traits<Measurement>::Equals(this->measured_, e->measured_, tol);
│ │ │ │ +
119 }
│ │ │ │ +
│ │ │ │
120
│ │ │ │ -
121private:
│ │ │ │ -
123 friend class boost::serialization::access;
│ │ │ │ -
124 template<class ARCHIVE>
│ │ │ │ -
125 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
126 ar & boost::serialization::make_nvp("NonlinearFactor3",
│ │ │ │ -
127 boost::serialization::base_object<Base>(*this));
│ │ │ │ -
128 }
│ │ │ │ -
129};
│ │ │ │ -
│ │ │ │ -
130
│ │ │ │ -
132template<class T1, class T2>
│ │ │ │ -
133struct traits<ReferenceFrameFactor<T1, T2> > : public Testable<ReferenceFrameFactor<T1, T2> > {};
│ │ │ │ -
134
│ │ │ │ -
135} // \namespace gtsam
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
│ │ │ │ +
122 Vector evaluateError(const Point3& point, boost::optional<Matrix&> H2 =
│ │ │ │ +
123 boost::none) const override {
│ │ │ │ +
124 try {
│ │ │ │ +
125 return traits<Measurement>::Local(measured_, camera_.project2(point, boost::none, H2));
│ │ │ │ +
126 } catch (CheiralityException& e) {
│ │ │ │ +
127 if (H2)
│ │ │ │ +
128 *H2 = Matrix::Zero(traits<Measurement>::dimension, 3);
│ │ │ │ +
129 if (verboseCheirality_)
│ │ │ │ +
130 std::cout << e.what() << ": Landmark "
│ │ │ │ +
131 << DefaultKeyFormatter(this->key()) << " moved behind camera"
│ │ │ │ +
132 << std::endl;
│ │ │ │ +
133 if (throwCheirality_)
│ │ │ │ +
134 throw e;
│ │ │ │ +
135 return camera_.defaultErrorWhenTriangulatingBehindCamera();
│ │ │ │ +
136 }
│ │ │ │ +
137 }
│ │ │ │ +
│ │ │ │ +
138
│ │ │ │ +
140 mutable VerticalBlockMatrix Ab;
│ │ │ │ +
141 mutable Matrix A;
│ │ │ │ +
142 mutable Vector b;
│ │ │ │ +
143
│ │ │ │ +
│ │ │ │ +
149 boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override {
│ │ │ │ +
150 // Only linearize if the factor is active
│ │ │ │ +
151 if (!this->active(x))
│ │ │ │ +
152 return boost::shared_ptr<JacobianFactor>();
│ │ │ │ +
153
│ │ │ │ +
154 // Allocate memory for Jacobian factor, do only once
│ │ │ │ +
155 if (Ab.rows() == 0) {
│ │ │ │ +
156 std::vector<size_t> dimensions(1, 3);
│ │ │ │ +
157 Ab = VerticalBlockMatrix(dimensions, traits<Measurement>::dimension, true);
│ │ │ │ +
158 A.resize(traits<Measurement>::dimension,3);
│ │ │ │ +
159 b.resize(traits<Measurement>::dimension);
│ │ │ │ +
160 }
│ │ │ │ +
161
│ │ │ │ +
162 // Would be even better if we could pass blocks to project
│ │ │ │ +
163 const Point3& point = x.at<Point3>(key());
│ │ │ │ +
164 b = traits<Measurement>::Local(camera_.project2(point, boost::none, A), measured_);
│ │ │ │ +
165 if (noiseModel_)
│ │ │ │ +
166 this->noiseModel_->WhitenSystem(A, b);
│ │ │ │ +
167
│ │ │ │ +
168 Ab(0) = A;
│ │ │ │ +
169 Ab(1) = b;
│ │ │ │ +
170
│ │ │ │ +
171 return boost::make_shared<JacobianFactor>(this->keys_, Ab);
│ │ │ │ +
172 }
│ │ │ │ +
│ │ │ │ +
173
│ │ │ │ +
│ │ │ │ +
175 const Measurement& measured() const {
│ │ │ │ +
176 return measured_;
│ │ │ │ +
177 }
│ │ │ │ +
│ │ │ │ +
178
│ │ │ │ +
│ │ │ │ +
180 inline bool verboseCheirality() const {
│ │ │ │ +
181 return verboseCheirality_;
│ │ │ │ +
182 }
│ │ │ │ +
│ │ │ │ +
183
│ │ │ │ +
│ │ │ │ +
185 inline bool throwCheirality() const {
│ │ │ │ +
186 return throwCheirality_;
│ │ │ │ +
187 }
│ │ │ │ +
│ │ │ │ +
188
│ │ │ │ +
189private:
│ │ │ │ +
190
│ │ │ │ +
192 friend class boost::serialization::access;
│ │ │ │ +
193 template<class ARCHIVE>
│ │ │ │ +
194 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
195 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
196 ar & BOOST_SERIALIZATION_NVP(camera_);
│ │ │ │ +
197 ar & BOOST_SERIALIZATION_NVP(measured_);
│ │ │ │ +
198 ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
│ │ │ │ +
199 ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
│ │ │ │ +
200 }
│ │ │ │ +
201};
│ │ │ │ +
│ │ │ │ +
202} // \ namespace gtsam
│ │ │ │ +
203
│ │ │ │ +
CalibratedCamera.h
Calibrated camera for which only pose is unknown.
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::transform_point
P transform_point(const T &trans, const P &global, boost::optional< Matrix & > Dtrans, boost::optional< Matrix & > Dglobal)
Transform function that must be specialized specific domains.
Definition ReferenceFrameFactor.h:30
│ │ │ │ -
gtsam::trans
Matrix trans(const Matrix &A)
static transpose function, just calls Eigen transpose member function
Definition Matrix.h:242
│ │ │ │ +
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ +
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::NoiseModelFactor::dim
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition NonlinearFactor.h:218
│ │ │ │ -
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition NonlinearFactor.h:223
│ │ │ │ +
gtsam::VerticalBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43
│ │ │ │ +
gtsam::VerticalBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition VerticalBlockMatrix.h:115
│ │ │ │ +
gtsam::CheiralityException
Definition CalibratedCamera.h:32
│ │ │ │ +
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
│ │ │ │ +
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
│ │ │ │ +
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
│ │ │ │ +
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ +
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition NonlinearFactor.h:118
│ │ │ │
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ -
gtsam::ReferenceFrameFactor
A constraint between two landmarks in separate maps Templated on: Point : Type of landmark Transform ...
Definition ReferenceFrameFactor.h:57
│ │ │ │ -
gtsam::ReferenceFrameFactor::print
void print(const std::string &s="", const gtsam::KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition ReferenceFrameFactor.h:107
│ │ │ │ -
gtsam::ReferenceFrameFactor::ReferenceFrameFactor
ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, const noiseModel::Base::shared_ptr &model)
General constructor with arbitrary noise model (constrained or otherwise)
Definition ReferenceFrameFactor.h:72
│ │ │ │ -
gtsam::ReferenceFrameFactor::evaluateError
Vector evaluateError(const Point &global, const Transform &trans, const Point &local, boost::optional< Matrix & > Dforeign=boost::none, boost::optional< Matrix & > Dtrans=boost::none, boost::optional< Matrix & > Dlocal=boost::none) const override
Combined cost and derivative function using boost::optional.
Definition ReferenceFrameFactor.h:97
│ │ │ │ -
gtsam::ReferenceFrameFactor::clone
NonlinearFactor::shared_ptr clone() const override
Creates a shared_ptr clone of the factor - needs to be specialized to allow for subclasses.
Definition ReferenceFrameFactor.h:92
│ │ │ │ -
gtsam::ReferenceFrameFactor::ReferenceFrameFactor
ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, double sigma=1e-2)
Simple soft constraint constructor for frame of reference, with equal weighting for each degree of fr...
Definition ReferenceFrameFactor.h:86
│ │ │ │ -
gtsam::ReferenceFrameFactor::ReferenceFrameFactor
ReferenceFrameFactor(double mu, Key globalKey, Key transKey, Key localKey)
Construct a hard frame of reference reference constraint with equal mu values for each degree of free...
Definition ReferenceFrameFactor.h:79
│ │ │ │ -
gtsam::ReferenceFrameFactor::access
friend class boost::serialization::access
Serialization function.
Definition ReferenceFrameFactor.h:123
│ │ │ │ -
gtsam::ReferenceFrameFactor::ReferenceFrameFactor
ReferenceFrameFactor()
default constructor for serialization only
Definition ReferenceFrameFactor.h:60
│ │ │ │ +
gtsam::NoiseModelFactorN< Point3 >::key
Key key() const
Returns a key.
Definition NonlinearFactor.h:518
│ │ │ │ +
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
gtsam::Values::at
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
│ │ │ │ +
gtsam::TriangulationFactor
Non-linear factor for a constraint derived from a 2D measurement.
Definition TriangulationFactor.h:33
│ │ │ │ +
gtsam::TriangulationFactor::Ab
VerticalBlockMatrix Ab
thread-safe (?) scratch memory for linearize
Definition TriangulationFactor.h:140
│ │ │ │ +
gtsam::TriangulationFactor::throwCheirality_
const bool throwCheirality_
If true, rethrows Cheirality exceptions (default: false)
Definition TriangulationFactor.h:56
│ │ │ │ +
gtsam::TriangulationFactor::throwCheirality
bool throwCheirality() const
return flag for throwing cheirality exceptions
Definition TriangulationFactor.h:185
│ │ │ │ +
gtsam::TriangulationFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition TriangulationFactor.h:106
│ │ │ │ +
gtsam::TriangulationFactor::evaluateError
Vector evaluateError(const Point3 &point, boost::optional< Matrix & > H2=boost::none) const override
Evaluate error h(x)-z and optionally derivatives.
Definition TriangulationFactor.h:122
│ │ │ │ +
gtsam::TriangulationFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition TriangulationFactor.h:96
│ │ │ │ +
gtsam::TriangulationFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override
Linearize to a JacobianFactor, does not support constrained noise model ! Hence .
Definition TriangulationFactor.h:149
│ │ │ │ +
gtsam::TriangulationFactor::TriangulationFactor
TriangulationFactor(const CAMERA &camera, const Measurement &measured, const SharedNoiseModel &model, Key pointKey, bool throwCheirality=false, bool verboseCheirality=false)
Constructor with exception-handling flags.
Definition TriangulationFactor.h:79
│ │ │ │ +
gtsam::TriangulationFactor::Measurement
typename CAMERA::Measurement Measurement
shorthand for measurement type, e.g. Point2 or StereoPoint2
Definition TriangulationFactor.h:49
│ │ │ │ +
gtsam::TriangulationFactor::measured
const Measurement & measured() const
return the measurement
Definition TriangulationFactor.h:175
│ │ │ │ +
gtsam::TriangulationFactor::verboseCheirality
bool verboseCheirality() const
return verbosity
Definition TriangulationFactor.h:180
│ │ │ │ +
gtsam::TriangulationFactor::Camera
CAMERA Camera
CAMERA type.
Definition TriangulationFactor.h:38
│ │ │ │ +
gtsam::TriangulationFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition TriangulationFactor.h:115
│ │ │ │ +
gtsam::TriangulationFactor::~TriangulationFactor
~TriangulationFactor() override
Virtual destructor.
Definition TriangulationFactor.h:92
│ │ │ │ +
gtsam::TriangulationFactor::verboseCheirality_
const bool verboseCheirality_
If true, prints text for Cheirality exceptions (default: false)
Definition TriangulationFactor.h:57
│ │ │ │ +
gtsam::TriangulationFactor::access
friend class boost::serialization::access
Serialization function.
Definition TriangulationFactor.h:192
│ │ │ │ +
gtsam::TriangulationFactor::TriangulationFactor
TriangulationFactor()
Default constructor.
Definition TriangulationFactor.h:66
│ │ │ │ +
gtsam::TriangulationFactor::This
TriangulationFactor< CAMERA > This
shorthand for this class
Definition TriangulationFactor.h:46
│ │ │ │ +
gtsam::TriangulationFactor::measured_
const Measurement measured_
2D measurement
Definition TriangulationFactor.h:53
│ │ │ │ +
gtsam::TriangulationFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition TriangulationFactor.h:63
│ │ │ │ +
gtsam::TriangulationFactor::camera_
const CAMERA camera_
CAMERA in which this landmark was seen.
Definition TriangulationFactor.h:52
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,203 +1,338 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ReferenceFrameFactor.h │ │ │ │ │ +TriangulationFactor.h │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -12/* │ │ │ │ │ -13 * @file ReferenceFrameFactor.h │ │ │ │ │ -14 * @brief A constraint for combining graphs by common landmarks and a │ │ │ │ │ -transform node │ │ │ │ │ -15 * @author Alex Cunningham │ │ │ │ │ -16 */ │ │ │ │ │ -17 │ │ │ │ │ 18#pragma once │ │ │ │ │ 19 │ │ │ │ │ 20#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -21 │ │ │ │ │ -22namespace _g_t_s_a_m { │ │ │ │ │ -23 │ │ │ │ │ -29template │ │ │ │ │ -_3_0P _t_r_a_n_s_f_o_r_m___p_o_i_n_t( │ │ │ │ │ -31 const T& _t_r_a_n_s, const P& global, │ │ │ │ │ -32 boost::optional Dtrans, │ │ │ │ │ -33 boost::optional Dglobal) { │ │ │ │ │ -34 return _t_r_a_n_s.transformFrom(global, Dtrans, Dglobal); │ │ │ │ │ -35} │ │ │ │ │ +21#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_._h> │ │ │ │ │ +22#include │ │ │ │ │ +23#include │ │ │ │ │ +24 │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ +26 │ │ │ │ │ +32template │ │ │ │ │ +_3_3class _T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +34 │ │ │ │ │ +35public: │ │ │ │ │ 36 │ │ │ │ │ -56template │ │ │ │ │ -_5_7class _R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -58protected: │ │ │ │ │ -_6_0 _R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r() {} │ │ │ │ │ +_3_8 using _C_a_m_e_r_a = CAMERA; │ │ │ │ │ +39 │ │ │ │ │ +40protected: │ │ │ │ │ +41 │ │ │ │ │ +_4_3 using _B_a_s_e = _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_P_o_i_n_t_3_>; │ │ │ │ │ +44 │ │ │ │ │ +_4_6 using _T_h_i_s = _T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_<_C_A_M_E_R_A_>; │ │ │ │ │ +47 │ │ │ │ │ +_4_9 using _M_e_a_s_u_r_e_m_e_n_t = typename CAMERA::Measurement; │ │ │ │ │ +50 │ │ │ │ │ +51 // Keep a copy of measurement and calibration for I/O │ │ │ │ │ +_5_2 const CAMERA _c_a_m_e_r_a__; │ │ │ │ │ +_5_3 const _M_e_a_s_u_r_e_m_e_n_t _m_e_a_s_u_r_e_d__; │ │ │ │ │ +54 │ │ │ │ │ +55 // verbosity handling for Cheirality Exceptions │ │ │ │ │ +_5_6 const bool _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__; │ │ │ │ │ +_5_7 const bool _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__; │ │ │ │ │ +58 │ │ │ │ │ +59public: │ │ │ │ │ +60 EIGEN_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ 61 │ │ │ │ │ -62public: │ │ │ │ │ -63 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_P_O_I_N_T_,_ _T_R_A_N_S_F_O_R_M_,_ _P_O_I_N_T_> Base; │ │ │ │ │ -64 typedef _R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r_<_P_O_I_N_T_,_ _T_R_A_N_S_F_O_R_M_> This; │ │ │ │ │ -65 │ │ │ │ │ -66 typedef POINT Point; │ │ │ │ │ -67 typedef TRANSFORM Transform; │ │ │ │ │ -68 │ │ │ │ │ -_7_2 _R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r(_K_e_y globalKey, _K_e_y transKey, _K_e_y localKey, const │ │ │ │ │ -noiseModel::Base::shared_ptr& model) │ │ │ │ │ -73 : _B_a_s_e(model,globalKey, transKey, localKey) {} │ │ │ │ │ -74 │ │ │ │ │ -_7_9 _R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r(double mu, _K_e_y globalKey, _K_e_y transKey, _K_e_y localKey) │ │ │ │ │ -80 : _B_a_s_e(globalKey, transKey, localKey, Point()._d_i_m(), mu) {} │ │ │ │ │ -81 │ │ │ │ │ -_8_6 _R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r(_K_e_y globalKey, _K_e_y transKey, _K_e_y localKey, double sigma │ │ │ │ │ -= 1e-2) │ │ │ │ │ -87 : _B_a_s_e(_n_o_i_s_e_M_o_d_e_l::Isotropic::Sigma(_t_r_a_i_t_s::dimension, sigma), │ │ │ │ │ -88 globalKey, transKey, localKey) {} │ │ │ │ │ -89 │ │ │ │ │ -90 _~_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r() override{} │ │ │ │ │ -91 │ │ │ │ │ -_9_2 NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -93 return boost::static_pointer_cast( │ │ │ │ │ -94 NonlinearFactor::shared_ptr(new This(*this))); } │ │ │ │ │ -95 │ │ │ │ │ -_9_7 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const Point& global, const Transform& _t_r_a_n_s, const │ │ │ │ │ -Point& local, │ │ │ │ │ -98 boost::optional Dforeign = boost::none, │ │ │ │ │ -99 boost::optional Dtrans = boost::none, │ │ │ │ │ -100 boost::optional Dlocal = boost::none) const override { │ │ │ │ │ -101 Point newlocal = transform_point(_t_r_a_n_s, global, Dtrans, │ │ │ │ │ -Dforeign); │ │ │ │ │ -102 if (Dlocal) │ │ │ │ │ -103 *Dlocal = -1* Matrix::Identity(_t_r_a_i_t_s_<_P_o_i_n_t_>_:_:_d_i_m_e_n_s_i_o_n, _t_r_a_i_t_s_<_P_o_i_n_t_>_:_: │ │ │ │ │ -_d_i_m_e_n_s_i_o_n); │ │ │ │ │ -104 return _t_r_a_i_t_s_<_P_o_i_n_t_>_:_:_L_o_c_a_l(local,newlocal); │ │ │ │ │ -105 } │ │ │ │ │ -106 │ │ │ │ │ -_1_0_7 void _p_r_i_n_t(const std::string& s="", │ │ │ │ │ -108 const _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const │ │ │ │ │ -override { │ │ │ │ │ -109 std::cout << s << ": ReferenceFrameFactor(" │ │ │ │ │ -110 << "Global: " << keyFormatter(this->key1()) << "," │ │ │ │ │ -111 << " Transform: " << keyFormatter(this->key2()) << "," │ │ │ │ │ -112 << " Local: " << keyFormatter(this->key3()) << ")\n"; │ │ │ │ │ -113 this->noiseModel_->print(" noise model"); │ │ │ │ │ -114 } │ │ │ │ │ -115 │ │ │ │ │ -116 // access - convenience functions │ │ │ │ │ -117 _K_e_y global_key() const { return this->key1(); } │ │ │ │ │ -118 _K_e_y transform_key() const { return this->key2(); } │ │ │ │ │ -119 _K_e_y local_key() const { return this->key3(); } │ │ │ │ │ +_6_3 using _s_h_a_r_e_d___p_t_r = boost::shared_ptr; │ │ │ │ │ +64 │ │ │ │ │ +_6_6 _T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r() : │ │ │ │ │ +67 _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__(false), _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__(false) { │ │ │ │ │ +68 } │ │ │ │ │ +69 │ │ │ │ │ +_7_9 _T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r(const CAMERA& camera, const _M_e_a_s_u_r_e_m_e_n_t& _m_e_a_s_u_r_e_d, │ │ │ │ │ +80 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model, _K_e_y pointKey, bool _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y = false, │ │ │ │ │ +81 bool _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y = false) : │ │ │ │ │ +82 _B_a_s_e(model, pointKey), _c_a_m_e_r_a__(camera), _m_e_a_s_u_r_e_d__(_m_e_a_s_u_r_e_d), │ │ │ │ │ +_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__( │ │ │ │ │ +83 _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y), _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__(_v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y) { │ │ │ │ │ +84 if (model && model->dim() != _t_r_a_i_t_s_<_M_e_a_s_u_r_e_m_e_n_t_>_:_:_d_i_m_e_n_s_i_o_n) │ │ │ │ │ +85 throw std::invalid_argument( │ │ │ │ │ +86 "TriangulationFactor must be created with " │ │ │ │ │ +87 + boost::lexical_cast((int) _t_r_a_i_t_s_<_M_e_a_s_u_r_e_m_e_n_t_>_:_:_d_i_m_e_n_s_i_o_n) │ │ │ │ │ +88 + "-dimensional noise model."); │ │ │ │ │ +89 } │ │ │ │ │ +90 │ │ │ │ │ +_9_2 _~_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r() override { │ │ │ │ │ +93 } │ │ │ │ │ +94 │ │ │ │ │ +_9_6 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +97 return boost::static_pointer_cast( │ │ │ │ │ +98 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); │ │ │ │ │ +99 } │ │ │ │ │ +100 │ │ │ │ │ +_1_0_6 void _p_r_i_n_t(const std::string& s = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ +107 DefaultKeyFormatter) const override { │ │ │ │ │ +108 std::cout << s << "TriangulationFactor,"; │ │ │ │ │ +109 _c_a_m_e_r_a__.print("camera"); │ │ │ │ │ +110 _t_r_a_i_t_s_<_M_e_a_s_u_r_e_m_e_n_t_>_:_:_P_r_i_n_t(_m_e_a_s_u_r_e_d__, "z"); │ │ │ │ │ +111 _B_a_s_e_:_:_p_r_i_n_t("", keyFormatter); │ │ │ │ │ +112 } │ │ │ │ │ +113 │ │ │ │ │ +_1_1_5 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& p, double tol = 1e-9) const override { │ │ │ │ │ +116 const _T_h_i_s *e = dynamic_cast(&p); │ │ │ │ │ +117 return e && _B_a_s_e_:_:_e_q_u_a_l_s(p, tol) && this->camera_.equals(e->camera_, tol) │ │ │ │ │ +118 && _t_r_a_i_t_s_<_M_e_a_s_u_r_e_m_e_n_t_>_:_:_E_q_u_a_l_s(this->measured_, e->measured_, tol); │ │ │ │ │ +119 } │ │ │ │ │ 120 │ │ │ │ │ -121private: │ │ │ │ │ -_1_2_3 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -124 template │ │ │ │ │ -125 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -126 ar & boost::serialization::make_nvp("NonlinearFactor3", │ │ │ │ │ -127 boost::serialization::base_object(*this)); │ │ │ │ │ -128 } │ │ │ │ │ -129}; │ │ │ │ │ -130 │ │ │ │ │ -132template │ │ │ │ │ -_1_3_3struct _t_r_a_i_t_s<_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r > : public │ │ │ │ │ -_T_e_s_t_a_b_l_e > {}; │ │ │ │ │ -134 │ │ │ │ │ -135} // \namespace gtsam │ │ │ │ │ +_1_2_2 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const _P_o_i_n_t_3& point, boost::optional H2 = │ │ │ │ │ +123 boost::none) const override { │ │ │ │ │ +124 try { │ │ │ │ │ +125 return _t_r_a_i_t_s_<_M_e_a_s_u_r_e_m_e_n_t_>_:_:_L_o_c_a_l(_m_e_a_s_u_r_e_d__, _c_a_m_e_r_a__.project2(point, │ │ │ │ │ +boost::none, H2)); │ │ │ │ │ +126 } catch (_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n& e) { │ │ │ │ │ +127 if (H2) │ │ │ │ │ +128 *H2 = Matrix::Zero(_t_r_a_i_t_s_<_M_e_a_s_u_r_e_m_e_n_t_>_:_:_d_i_m_e_n_s_i_o_n, 3); │ │ │ │ │ +129 if (_v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__) │ │ │ │ │ +130 std::cout << e.what() << ": Landmark " │ │ │ │ │ +131 << DefaultKeyFormatter(this->_k_e_y()) << " moved behind camera" │ │ │ │ │ +132 << std::endl; │ │ │ │ │ +133 if (_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__) │ │ │ │ │ +134 throw e; │ │ │ │ │ +135 return _c_a_m_e_r_a__.defaultErrorWhenTriangulatingBehindCamera(); │ │ │ │ │ +136 } │ │ │ │ │ +137 } │ │ │ │ │ +138 │ │ │ │ │ +_1_4_0 mutable _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x _A_b; │ │ │ │ │ +141 mutable Matrix A; │ │ │ │ │ +142 mutable Vector b; │ │ │ │ │ +143 │ │ │ │ │ +_1_4_9 boost::shared_ptr _l_i_n_e_a_r_i_z_e(const _V_a_l_u_e_s& x) const override │ │ │ │ │ +{ │ │ │ │ │ +150 // Only linearize if the factor is active │ │ │ │ │ +151 if (!this->_a_c_t_i_v_e(x)) │ │ │ │ │ +152 return boost::shared_ptr(); │ │ │ │ │ +153 │ │ │ │ │ +154 // Allocate memory for Jacobian factor, do only once │ │ │ │ │ +155 if (_A_b._r_o_w_s() == 0) { │ │ │ │ │ +156 std::vector dimensions(1, 3); │ │ │ │ │ +157 _A_b = _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x(dimensions, _t_r_a_i_t_s_<_M_e_a_s_u_r_e_m_e_n_t_>_:_:_d_i_m_e_n_s_i_o_n, true); │ │ │ │ │ +158 A.resize(_t_r_a_i_t_s_<_M_e_a_s_u_r_e_m_e_n_t_>_:_:_d_i_m_e_n_s_i_o_n,3); │ │ │ │ │ +159 b.resize(_t_r_a_i_t_s_<_M_e_a_s_u_r_e_m_e_n_t_>_:_:_d_i_m_e_n_s_i_o_n); │ │ │ │ │ +160 } │ │ │ │ │ +161 │ │ │ │ │ +162 // Would be even better if we could pass blocks to project │ │ │ │ │ +163 const _P_o_i_n_t_3& point = x._a_t<_P_o_i_n_t_3>(_k_e_y()); │ │ │ │ │ +164 b = _t_r_a_i_t_s_<_M_e_a_s_u_r_e_m_e_n_t_>_:_:_L_o_c_a_l(_c_a_m_e_r_a__.project2(point, boost::none, A), │ │ │ │ │ +_m_e_a_s_u_r_e_d__); │ │ │ │ │ +165 if (noiseModel_) │ │ │ │ │ +166 this->noiseModel_->WhitenSystem(A, b); │ │ │ │ │ +167 │ │ │ │ │ +168 _A_b(0) = A; │ │ │ │ │ +169 _A_b(1) = b; │ │ │ │ │ +170 │ │ │ │ │ +171 return boost::make_shared(this->_k_e_y_s__, Ab); │ │ │ │ │ +172 } │ │ │ │ │ +173 │ │ │ │ │ +_1_7_5 const _M_e_a_s_u_r_e_m_e_n_t& _m_e_a_s_u_r_e_d() const { │ │ │ │ │ +176 return _m_e_a_s_u_r_e_d__; │ │ │ │ │ +177 } │ │ │ │ │ +178 │ │ │ │ │ +_1_8_0 inline bool _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y() const { │ │ │ │ │ +181 return _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__; │ │ │ │ │ +182 } │ │ │ │ │ +183 │ │ │ │ │ +_1_8_5 inline bool _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y() const { │ │ │ │ │ +186 return _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__; │ │ │ │ │ +187 } │ │ │ │ │ +188 │ │ │ │ │ +189private: │ │ │ │ │ +190 │ │ │ │ │ +_1_9_2 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +193 template │ │ │ │ │ +194 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +195 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +196 ar & BOOST_SERIALIZATION_NVP(_c_a_m_e_r_a__); │ │ │ │ │ +197 ar & BOOST_SERIALIZATION_NVP(_m_e_a_s_u_r_e_d__); │ │ │ │ │ +198 ar & BOOST_SERIALIZATION_NVP(_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__); │ │ │ │ │ +199 ar & BOOST_SERIALIZATION_NVP(_v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__); │ │ │ │ │ +200 } │ │ │ │ │ +201}; │ │ │ │ │ +202} // \ namespace gtsam │ │ │ │ │ +203 │ │ │ │ │ +_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_._h │ │ │ │ │ +Calibrated camera for which only pose is unknown. │ │ │ │ │ _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_n_s_f_o_r_m___p_o_i_n_t │ │ │ │ │ -P transform_point(const T &trans, const P &global, boost::optional< Matrix & > │ │ │ │ │ -Dtrans, boost::optional< Matrix & > Dglobal) │ │ │ │ │ -Transform function that must be specialized specific domains. │ │ │ │ │ -DDeeffiinniittiioonn ReferenceFrameFactor.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_n_s │ │ │ │ │ -Matrix trans(const Matrix &A) │ │ │ │ │ -static transpose function, just calls Eigen transpose member function │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:242 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ +Vector3 Point3 │ │ │ │ │ +As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ +typedef Point3 to Vector3... │ │ │ │ │ +DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ +Aliases. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_d_i_m │ │ │ │ │ -size_t dim() const override │ │ │ │ │ -get the dimension of the factor (number of rows on linearization) │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:218 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_n_o_i_s_e_M_o_d_e_l │ │ │ │ │ -const SharedNoiseModel & noiseModel() const │ │ │ │ │ -access to the noise model │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:223 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ +of vertical blocks. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_r_o_w_s │ │ │ │ │ +DenseIndex rows() const │ │ │ │ │ +Row size. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:115 │ │ │ │ │ +_g_t_s_a_m_:_:_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s__ │ │ │ │ │ +KeyVector keys_ │ │ │ │ │ +The keys involved in this factor. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +virtual void print(const std::string &s="Factor", const KeyFormatter │ │ │ │ │ +&formatter=DefaultKeyFormatter) const │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn Factor.cpp:29 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const This &other, double tol=1e-9) const │ │ │ │ │ +check equality │ │ │ │ │ +DDeeffiinniittiioonn Factor.cpp:42 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ +Nonlinear factor base class. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_:_:_a_c_t_i_v_e │ │ │ │ │ +virtual bool active(const Values &) const │ │ │ │ │ +Checks whether a factor should be used based on a set of values. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:118 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ variables. │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r │ │ │ │ │ -A constraint between two landmarks in separate maps Templated on: Point : Type │ │ │ │ │ -of landmark Transform ... │ │ │ │ │ -DDeeffiinniittiioonn ReferenceFrameFactor.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const gtsam::KeyFormatter │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_ _P_o_i_n_t_3_ _>_:_:_k_e_y │ │ │ │ │ +Key key() const │ │ │ │ │ +Returns a key. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:518 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_a_t │ │ │ │ │ +const ValueType at(Key j) const │ │ │ │ │ +Retrieve a variable by key j. │ │ │ │ │ +DDeeffiinniittiioonn Values-inl.h:361 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +Non-linear factor for a constraint derived from a 2D measurement. │ │ │ │ │ +DDeeffiinniittiioonn TriangulationFactor.h:33 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_A_b │ │ │ │ │ +VerticalBlockMatrix Ab │ │ │ │ │ +thread-safe (?) scratch memory for linearize │ │ │ │ │ +DDeeffiinniittiioonn TriangulationFactor.h:140 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__ │ │ │ │ │ +const bool throwCheirality_ │ │ │ │ │ +If true, rethrows Cheirality exceptions (default: false) │ │ │ │ │ +DDeeffiinniittiioonn TriangulationFactor.h:56 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y │ │ │ │ │ +bool throwCheirality() const │ │ │ │ │ +return flag for throwing cheirality exceptions │ │ │ │ │ +DDeeffiinniittiioonn TriangulationFactor.h:185 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ &keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -Print. │ │ │ │ │ -DDeeffiinniittiioonn ReferenceFrameFactor.h:107 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r_:_:_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r │ │ │ │ │ -ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, const │ │ │ │ │ -noiseModel::Base::shared_ptr &model) │ │ │ │ │ -General constructor with arbitrary noise model (constrained or otherwise) │ │ │ │ │ -DDeeffiinniittiioonn ReferenceFrameFactor.h:72 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ -Vector evaluateError(const Point &global, const Transform &trans, const Point │ │ │ │ │ -&local, boost::optional< Matrix & > Dforeign=boost::none, boost::optional< │ │ │ │ │ -Matrix & > Dtrans=boost::none, boost::optional< Matrix & > Dlocal=boost::none) │ │ │ │ │ -const override │ │ │ │ │ -Combined cost and derivative function using boost::optional. │ │ │ │ │ -DDeeffiinniittiioonn ReferenceFrameFactor.h:97 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ -NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -Creates a shared_ptr clone of the factor - needs to be specialized to allow for │ │ │ │ │ -subclasses. │ │ │ │ │ -DDeeffiinniittiioonn ReferenceFrameFactor.h:92 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r_:_:_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r │ │ │ │ │ -ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, double │ │ │ │ │ -sigma=1e-2) │ │ │ │ │ -Simple soft constraint constructor for frame of reference, with equal weighting │ │ │ │ │ -for each degree of fr... │ │ │ │ │ -DDeeffiinniittiioonn ReferenceFrameFactor.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r_:_:_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r │ │ │ │ │ -ReferenceFrameFactor(double mu, Key globalKey, Key transKey, Key localKey) │ │ │ │ │ -Construct a hard frame of reference reference constraint with equal mu values │ │ │ │ │ -for each degree of free... │ │ │ │ │ -DDeeffiinniittiioonn ReferenceFrameFactor.h:79 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn TriangulationFactor.h:106 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ +Vector evaluateError(const Point3 &point, boost::optional< Matrix & > │ │ │ │ │ +H2=boost::none) const override │ │ │ │ │ +Evaluate error h(x)-z and optionally derivatives. │ │ │ │ │ +DDeeffiinniittiioonn TriangulationFactor.h:122 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ +gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn TriangulationFactor.h:96 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e │ │ │ │ │ +boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override │ │ │ │ │ +Linearize to a JacobianFactor, does not support constrained noise model ! Hence │ │ │ │ │ +. │ │ │ │ │ +DDeeffiinniittiioonn TriangulationFactor.h:149 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +TriangulationFactor(const CAMERA &camera, const Measurement &measured, const │ │ │ │ │ +SharedNoiseModel &model, Key pointKey, bool throwCheirality=false, bool │ │ │ │ │ +verboseCheirality=false) │ │ │ │ │ +Constructor with exception-handling flags. │ │ │ │ │ +DDeeffiinniittiioonn TriangulationFactor.h:79 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_M_e_a_s_u_r_e_m_e_n_t │ │ │ │ │ +typename CAMERA::Measurement Measurement │ │ │ │ │ +shorthand for measurement type, e.g. Point2 or StereoPoint2 │ │ │ │ │ +DDeeffiinniittiioonn TriangulationFactor.h:49 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_m_e_a_s_u_r_e_d │ │ │ │ │ +const Measurement & measured() const │ │ │ │ │ +return the measurement │ │ │ │ │ +DDeeffiinniittiioonn TriangulationFactor.h:175 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y │ │ │ │ │ +bool verboseCheirality() const │ │ │ │ │ +return verbosity │ │ │ │ │ +DDeeffiinniittiioonn TriangulationFactor.h:180 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_C_a_m_e_r_a │ │ │ │ │ +CAMERA Camera │ │ │ │ │ +CAMERA type. │ │ │ │ │ +DDeeffiinniittiioonn TriangulationFactor.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const NonlinearFactor &p, double tol=1e-9) const override │ │ │ │ │ +equals │ │ │ │ │ +DDeeffiinniittiioonn TriangulationFactor.h:115 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_~_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +~TriangulationFactor() override │ │ │ │ │ +Virtual destructor. │ │ │ │ │ +DDeeffiinniittiioonn TriangulationFactor.h:92 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__ │ │ │ │ │ +const bool verboseCheirality_ │ │ │ │ │ +If true, prints text for Cheirality exceptions (default: false) │ │ │ │ │ +DDeeffiinniittiioonn TriangulationFactor.h:57 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ friend class boost::serialization::access │ │ │ │ │ Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn ReferenceFrameFactor.h:123 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r_:_:_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r │ │ │ │ │ -ReferenceFrameFactor() │ │ │ │ │ -default constructor for serialization only │ │ │ │ │ -DDeeffiinniittiioonn ReferenceFrameFactor.h:60 │ │ │ │ │ +DDeeffiinniittiioonn TriangulationFactor.h:192 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +TriangulationFactor() │ │ │ │ │ +Default constructor. │ │ │ │ │ +DDeeffiinniittiioonn TriangulationFactor.h:66 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_T_h_i_s │ │ │ │ │ +TriangulationFactor< CAMERA > This │ │ │ │ │ +shorthand for this class │ │ │ │ │ +DDeeffiinniittiioonn TriangulationFactor.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_m_e_a_s_u_r_e_d__ │ │ │ │ │ +const Measurement measured_ │ │ │ │ │ +2D measurement │ │ │ │ │ +DDeeffiinniittiioonn TriangulationFactor.h:53 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shorthand for a smart pointer to a factor │ │ │ │ │ +DDeeffiinniittiioonn TriangulationFactor.h:63 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_c_a_m_e_r_a__ │ │ │ │ │ +const CAMERA camera_ │ │ │ │ │ +CAMERA in which this landmark was seen. │ │ │ │ │ +DDeeffiinniittiioonn TriangulationFactor.h:52 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * RReeffeerreenncceeFFrraammeeFFaaccttoorr..hh │ │ │ │ │ + * TTrriiaanngguullaattiioonnFFaaccttoorr..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01289_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/lago.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/RotateFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,75 +98,170 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
lago.h
│ │ │ │ +
RotateFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ -
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ -
4 * Atlanta, Georgia 30332-0415
│ │ │ │ -
5 * All Rights Reserved
│ │ │ │ -
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ -
7
│ │ │ │ -
8 * See LICENSE for the license information
│ │ │ │ -
9
│ │ │ │ -
10 * -------------------------------------------------------------------------- */
│ │ │ │ -
11
│ │ │ │ -
35#pragma once
│ │ │ │ -
36
│ │ │ │ -
37#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ -
38#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ -
39#include <gtsam/linear/VectorValues.h>
│ │ │ │ -
40#include <gtsam/inference/graph.h>
│ │ │ │ -
41
│ │ │ │ -
42namespace gtsam {
│ │ │ │ -
43namespace lago {
│ │ │ │ -
44
│ │ │ │ -
45typedef std::map<Key, double> key2doubleMap;
│ │ │ │ -
46
│ │ │ │ -
51GTSAM_EXPORT key2doubleMap computeThetasToRoot(
│ │ │ │ -
52 const key2doubleMap& deltaThetaMap, const PredecessorMap<Key>& tree);
│ │ │ │ -
53
│ │ │ │ -
62GTSAM_EXPORT void getSymbolicGraph(
│ │ │ │ -
63/*OUTPUTS*/std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds,
│ │ │ │ -
64 key2doubleMap& deltaThetaMap,
│ │ │ │ -
65 /*INPUTS*/const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g);
│ │ │ │ -
66
│ │ │ │ -
68GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(
│ │ │ │ -
69 const std::vector<size_t>& spanningTreeIds,
│ │ │ │ -
70 const std::vector<size_t>& chordsIds, const NonlinearFactorGraph& g,
│ │ │ │ -
71 const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree);
│ │ │ │ -
72
│ │ │ │ -
74GTSAM_EXPORT VectorValues initializeOrientations(
│ │ │ │ -
75 const NonlinearFactorGraph& graph, bool useOdometricPath = true);
│ │ │ │ -
76
│ │ │ │ -
78GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph,
│ │ │ │ -
79 bool useOdometricPath = true);
│ │ │ │ -
80
│ │ │ │ -
82GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph,
│ │ │ │ -
83 const Values& initialGuess);
│ │ │ │ -
84
│ │ │ │ -
85} // end of namespace lago
│ │ │ │ -
86} // end of namespace gtsam
│ │ │ │ -
graph.h
Graph algorithm using boost library.
│ │ │ │ -
VectorValues.h
Factor Graph Values.
│ │ │ │ -
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ -
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │ +
1/*
│ │ │ │ +
2 * @file RotateFactor.cpp
│ │ │ │ +
3 * @brief RotateFactor class
│ │ │ │ +
4 * @author Frank Dellaert
│ │ │ │ +
5 * @date December 17, 2013
│ │ │ │ +
6 */
│ │ │ │ +
7
│ │ │ │ +
8#pragma once
│ │ │ │ +
9
│ │ │ │ +
10#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
11#include <gtsam/geometry/Rot3.h>
│ │ │ │ +
12
│ │ │ │ +
13namespace gtsam {
│ │ │ │ +
14
│ │ │ │ +
│ │ │ │ +
23class RotateFactor: public NoiseModelFactorN<Rot3> {
│ │ │ │ +
24
│ │ │ │ +
25 Point3 p_, z_;
│ │ │ │ +
26
│ │ │ │ +
27 typedef NoiseModelFactorN<Rot3> Base;
│ │ │ │ +
28 typedef RotateFactor This;
│ │ │ │ +
29
│ │ │ │ +
30public:
│ │ │ │ +
31
│ │ │ │ +
│ │ │ │ +
33 RotateFactor(Key key, const Rot3& P, const Rot3& Z,
│ │ │ │ +
34 const SharedNoiseModel& model) :
│ │ │ │ +
35 Base(model, key), p_(Rot3::Logmap(P)), z_(Rot3::Logmap(Z)) {
│ │ │ │ +
36 }
│ │ │ │ +
│ │ │ │ +
37
│ │ │ │ +
│ │ │ │ +
39 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
40 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ +
41 gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
│ │ │ │ +
│ │ │ │ +
42
│ │ │ │ +
│ │ │ │ +
44 void print(const std::string& s = "",
│ │ │ │ +
45 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ +
46 Base::print(s);
│ │ │ │ +
47 std::cout << "RotateFactor:]\n";
│ │ │ │ +
48 std::cout << "p: " << p_.transpose() << std::endl;
│ │ │ │ +
49 std::cout << "z: " << z_.transpose() << std::endl;
│ │ │ │ +
50 }
│ │ │ │ +
│ │ │ │ +
51
│ │ │ │ +
│ │ │ │ +
53 Vector evaluateError(const Rot3& R,
│ │ │ │ +
54 boost::optional<Matrix&> H = boost::none) const override {
│ │ │ │ +
55 // predict p_ as q = R*z_, derivative H will be filled if not none
│ │ │ │ +
56 Point3 q = R.rotate(z_,H);
│ │ │ │ +
57 // error is just difference, and note derivative of that wrpt q is I3
│ │ │ │ +
58 return (Vector(3) << q.x()-p_.x(), q.y()-p_.y(), q.z()-p_.z()).finished();
│ │ │ │ +
59 }
│ │ │ │ +
│ │ │ │ +
60
│ │ │ │ +
61};
│ │ │ │ +
│ │ │ │ +
62
│ │ │ │ +
│ │ │ │ +
67class RotateDirectionsFactor: public NoiseModelFactorN<Rot3> {
│ │ │ │ +
68
│ │ │ │ +
69 Unit3 i_p_, c_z_;
│ │ │ │ +
70
│ │ │ │ +
71 typedef NoiseModelFactorN<Rot3> Base;
│ │ │ │ +
72 typedef RotateDirectionsFactor This;
│ │ │ │ +
73
│ │ │ │ +
74public:
│ │ │ │ +
75
│ │ │ │ +
│ │ │ │ +
77 RotateDirectionsFactor(Key key, const Unit3& i_p, const Unit3& c_z,
│ │ │ │ +
78 const SharedNoiseModel& model) :
│ │ │ │ +
79 Base(model, key), i_p_(i_p), c_z_(c_z) {
│ │ │ │ +
80 }
│ │ │ │ +
│ │ │ │ +
81
│ │ │ │ +
│ │ │ │ +
83 static Rot3 Initialize(const Unit3& i_p, const Unit3& c_z) {
│ │ │ │ +
84 gtsam::Quaternion iRc;
│ │ │ │ +
85 // setFromTwoVectors sets iRc to (a) quaternion which transform c_z into i_p
│ │ │ │ +
86 iRc.setFromTwoVectors(c_z.unitVector(), i_p.unitVector());
│ │ │ │ +
87 return Rot3(iRc);
│ │ │ │ +
88 }
│ │ │ │ +
│ │ │ │ +
89
│ │ │ │ +
│ │ │ │ +
91 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
92 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ +
93 gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
│ │ │ │ +
│ │ │ │ +
94
│ │ │ │ +
│ │ │ │ +
96 void print(const std::string& s = "",
│ │ │ │ +
97 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ +
98 Base::print(s);
│ │ │ │ +
99 std::cout << "RotateDirectionsFactor:" << std::endl;
│ │ │ │ +
100 i_p_.print("p");
│ │ │ │ +
101 c_z_.print("z");
│ │ │ │ +
102 }
│ │ │ │ +
│ │ │ │ +
103
│ │ │ │ +
│ │ │ │ +
105 Vector evaluateError(const Rot3& iRc, boost::optional<Matrix&> H = boost::none) const override {
│ │ │ │ +
106 Unit3 i_q = iRc * c_z_;
│ │ │ │ +
107 Vector error = i_p_.error(i_q, H);
│ │ │ │ +
108 if (H) {
│ │ │ │ +
109 Matrix DR;
│ │ │ │ +
110 iRc.rotate(c_z_, DR);
│ │ │ │ +
111 *H = (*H) * DR;
│ │ │ │ +
112 }
│ │ │ │ +
113 return error;
│ │ │ │ +
114 }
│ │ │ │ +
│ │ │ │ +
115
│ │ │ │ +
116 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
117};
│ │ │ │ +
│ │ │ │ +
118} // namespace gtsam
│ │ │ │ +
119
│ │ │ │ +
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ +
Rot3.h
3D rotation represented as a rotation matrix or quaternion
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
VectorValues
The Factor::error simply extracts the.
│ │ │ │ -
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │ +
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ +
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
│ │ │ │ +
gtsam::Rot3::rotate
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
rotate point from rotated coordinate frame to world
Definition Rot3M.cpp:149
│ │ │ │ +
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
│ │ │ │ +
gtsam::Unit3::unitVector
Vector3 unitVector(OptionalJacobian< 3, 2 > H=boost::none) const
Return unit-norm Vector.
Definition Unit3.cpp:151
│ │ │ │ +
gtsam::Unit3::print
void print(const std::string &s=std::string()) const
The print fuction.
Definition Unit3.cpp:164
│ │ │ │ +
gtsam::Unit3::error
Vector2 error(const Unit3 &q, OptionalJacobian< 2, 2 > H_q=boost::none) const
Signed, vector-valued error between two directions.
Definition Unit3.cpp:199
│ │ │ │ +
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
│ │ │ │ +
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Calculate the error of the factor.
Definition NonlinearFactor.cpp:138
│ │ │ │ +
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ +
gtsam::NoiseModelFactorN< Rot3 >::key
Key key() const
Returns a key.
Definition NonlinearFactor.h:518
│ │ │ │ +
gtsam::RotateFactor
Factor on unknown rotation iRC that relates two incremental rotations c1Rc2 = iRc' * i1Ri2 * iRc Whic...
Definition RotateFactor.h:23
│ │ │ │ +
gtsam::RotateFactor::RotateFactor
RotateFactor(Key key, const Rot3 &P, const Rot3 &Z, const SharedNoiseModel &model)
Constructor.
Definition RotateFactor.h:33
│ │ │ │ +
gtsam::RotateFactor::evaluateError
Vector evaluateError(const Rot3 &R, boost::optional< Matrix & > H=boost::none) const override
vector of errors returns 2D vector
Definition RotateFactor.h:53
│ │ │ │ +
gtsam::RotateFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition RotateFactor.h:44
│ │ │ │ +
gtsam::RotateFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition RotateFactor.h:39
│ │ │ │ +
gtsam::RotateDirectionsFactor
Factor on unknown rotation iRc that relates two directions c Directions provide less constraints than...
Definition RotateFactor.h:67
│ │ │ │ +
gtsam::RotateDirectionsFactor::evaluateError
Vector evaluateError(const Rot3 &iRc, boost::optional< Matrix & > H=boost::none) const override
vector of errors returns 2D vector
Definition RotateFactor.h:105
│ │ │ │ +
gtsam::RotateDirectionsFactor::RotateDirectionsFactor
RotateDirectionsFactor(Key key, const Unit3 &i_p, const Unit3 &c_z, const SharedNoiseModel &model)
Constructor.
Definition RotateFactor.h:77
│ │ │ │ +
gtsam::RotateDirectionsFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition RotateFactor.h:96
│ │ │ │ +
gtsam::RotateDirectionsFactor::Initialize
static Rot3 Initialize(const Unit3 &i_p, const Unit3 &c_z)
Initialize rotation iRc such that i_p = iRc * c_z.
Definition RotateFactor.h:83
│ │ │ │ +
gtsam::RotateDirectionsFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition RotateFactor.h:91
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,77 +1,226 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -lago.h │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -1/* --------------------------------------------------------------------------- │ │ │ │ │ -- │ │ │ │ │ -2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ -4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -5 * All Rights Reserved │ │ │ │ │ -6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +RotateFactor.h │ │ │ │ │ +1/* │ │ │ │ │ +2 * @file RotateFactor.cpp │ │ │ │ │ +3 * @brief RotateFactor class │ │ │ │ │ +4 * @author Frank Dellaert │ │ │ │ │ +5 * @date December 17, 2013 │ │ │ │ │ +6 */ │ │ │ │ │ 7 │ │ │ │ │ -8 * See LICENSE for the license information │ │ │ │ │ +8#pragma once │ │ │ │ │ 9 │ │ │ │ │ -10 * ------------------------------------------------------------------------- │ │ │ │ │ -- */ │ │ │ │ │ -11 │ │ │ │ │ -35#pragma once │ │ │ │ │ -36 │ │ │ │ │ -37#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -38#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -39#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ -40#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_g_r_a_p_h_._h> │ │ │ │ │ -41 │ │ │ │ │ -42namespace _g_t_s_a_m { │ │ │ │ │ -43namespace lago { │ │ │ │ │ -44 │ │ │ │ │ -45typedef std::map key2doubleMap; │ │ │ │ │ -46 │ │ │ │ │ -51GTSAM_EXPORT key2doubleMap computeThetasToRoot( │ │ │ │ │ -52 const key2doubleMap& deltaThetaMap, const PredecessorMap& tree); │ │ │ │ │ -53 │ │ │ │ │ -62GTSAM_EXPORT void getSymbolicGraph( │ │ │ │ │ -63/*OUTPUTS*/std::vector& spanningTreeIds, std::vector& │ │ │ │ │ -chordsIds, │ │ │ │ │ -64 key2doubleMap& deltaThetaMap, │ │ │ │ │ -65 /*INPUTS*/const PredecessorMap& tree, const NonlinearFactorGraph& g); │ │ │ │ │ -66 │ │ │ │ │ -68GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph( │ │ │ │ │ -69 const std::vector& spanningTreeIds, │ │ │ │ │ -70 const std::vector& chordsIds, const NonlinearFactorGraph& g, │ │ │ │ │ -71 const key2doubleMap& orientationsToRoot, const PredecessorMap& tree); │ │ │ │ │ -72 │ │ │ │ │ -74GTSAM_EXPORT _V_e_c_t_o_r_V_a_l_u_e_s initializeOrientations( │ │ │ │ │ -75 const NonlinearFactorGraph& graph, bool useOdometricPath = true); │ │ │ │ │ -76 │ │ │ │ │ -78GTSAM_EXPORT _V_a_l_u_e_s initialize(const NonlinearFactorGraph& graph, │ │ │ │ │ -79 bool useOdometricPath = true); │ │ │ │ │ -80 │ │ │ │ │ -82GTSAM_EXPORT _V_a_l_u_e_s initialize(const NonlinearFactorGraph& graph, │ │ │ │ │ -83 const _V_a_l_u_e_s& initialGuess); │ │ │ │ │ -84 │ │ │ │ │ -85} // end of namespace lago │ │ │ │ │ -86} // end of namespace gtsam │ │ │ │ │ -_g_r_a_p_h_._h │ │ │ │ │ -Graph algorithm using boost library. │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ -Factor Graph Values. │ │ │ │ │ -_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Factor Graph consisting of non-linear factors. │ │ │ │ │ +10#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +11#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_3_._h> │ │ │ │ │ +12 │ │ │ │ │ +13namespace _g_t_s_a_m { │ │ │ │ │ +14 │ │ │ │ │ +_2_3class _R_o_t_a_t_e_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +24 │ │ │ │ │ +25 _P_o_i_n_t_3 p_, z_; │ │ │ │ │ +26 │ │ │ │ │ +27 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_R_o_t_3_> _B_a_s_e; │ │ │ │ │ +28 typedef _R_o_t_a_t_e_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ +29 │ │ │ │ │ +30public: │ │ │ │ │ +31 │ │ │ │ │ +_3_3 _R_o_t_a_t_e_F_a_c_t_o_r(_K_e_y _k_e_y, const _R_o_t_3& P, const _R_o_t_3& Z, │ │ │ │ │ +34 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model) : │ │ │ │ │ +35 _B_a_s_e(model, _k_e_y), p_(_R_o_t_3::Logmap(P)), z_(_R_o_t_3::Logmap(Z)) { │ │ │ │ │ +36 } │ │ │ │ │ +37 │ │ │ │ │ +_3_9 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +40 return boost::static_pointer_cast( │ │ │ │ │ +41 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); } │ │ │ │ │ +42 │ │ │ │ │ +_4_4 void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ +45 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ +46 _B_a_s_e_:_:_p_r_i_n_t(s); │ │ │ │ │ +47 std::cout << "RotateFactor:]\n"; │ │ │ │ │ +48 std::cout << "p: " << p_.transpose() << std::endl; │ │ │ │ │ +49 std::cout << "z: " << z_.transpose() << std::endl; │ │ │ │ │ +50 } │ │ │ │ │ +51 │ │ │ │ │ +_5_3 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const _R_o_t_3& R, │ │ │ │ │ +54 boost::optional H = boost::none) const override { │ │ │ │ │ +55 // predict p_ as q = R*z_, derivative H will be filled if not none │ │ │ │ │ +56 _P_o_i_n_t_3 q = R.rotate(z_,H); │ │ │ │ │ +57 // error is just difference, and note derivative of that wrpt q is I3 │ │ │ │ │ +58 return (Vector(3) << q.x()-p_.x(), q.y()-p_.y(), q.z()-p_.z()).finished(); │ │ │ │ │ +59 } │ │ │ │ │ +60 │ │ │ │ │ +61}; │ │ │ │ │ +62 │ │ │ │ │ +_6_7class _R_o_t_a_t_e_D_i_r_e_c_t_i_o_n_s_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +68 │ │ │ │ │ +69 _U_n_i_t_3 i_p_, c_z_; │ │ │ │ │ +70 │ │ │ │ │ +71 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_R_o_t_3_> _B_a_s_e; │ │ │ │ │ +72 typedef _R_o_t_a_t_e_D_i_r_e_c_t_i_o_n_s_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ +73 │ │ │ │ │ +74public: │ │ │ │ │ +75 │ │ │ │ │ +_7_7 _R_o_t_a_t_e_D_i_r_e_c_t_i_o_n_s_F_a_c_t_o_r(_K_e_y _k_e_y, const _U_n_i_t_3& i_p, const _U_n_i_t_3& c_z, │ │ │ │ │ +78 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model) : │ │ │ │ │ +79 _B_a_s_e(model, _k_e_y), i_p_(i_p), c_z_(c_z) { │ │ │ │ │ +80 } │ │ │ │ │ +81 │ │ │ │ │ +_8_3 static _R_o_t_3 _I_n_i_t_i_a_l_i_z_e(const _U_n_i_t_3& i_p, const _U_n_i_t_3& c_z) { │ │ │ │ │ +84 gtsam::Quaternion iRc; │ │ │ │ │ +85 // setFromTwoVectors sets iRc to (a) quaternion which transform c_z into i_p │ │ │ │ │ +86 iRc.setFromTwoVectors(c_z._u_n_i_t_V_e_c_t_o_r(), i_p._u_n_i_t_V_e_c_t_o_r()); │ │ │ │ │ +87 return _R_o_t_3(iRc); │ │ │ │ │ +88 } │ │ │ │ │ +89 │ │ │ │ │ +_9_1 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +92 return boost::static_pointer_cast( │ │ │ │ │ +93 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); } │ │ │ │ │ +94 │ │ │ │ │ +_9_6 void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ +97 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ +98 _B_a_s_e_:_:_p_r_i_n_t(s); │ │ │ │ │ +99 std::cout << "RotateDirectionsFactor:" << std::endl; │ │ │ │ │ +100 i_p_._p_r_i_n_t("p"); │ │ │ │ │ +101 c_z_._p_r_i_n_t("z"); │ │ │ │ │ +102 } │ │ │ │ │ +103 │ │ │ │ │ +_1_0_5 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const _R_o_t_3& iRc, boost::optional H = boost:: │ │ │ │ │ +none) const override { │ │ │ │ │ +106 _U_n_i_t_3 i_q = iRc * c_z_; │ │ │ │ │ +107 Vector _e_r_r_o_r = i_p_._e_r_r_o_r(i_q, H); │ │ │ │ │ +108 if (H) { │ │ │ │ │ +109 Matrix DR; │ │ │ │ │ +110 iRc._r_o_t_a_t_e(c_z_, DR); │ │ │ │ │ +111 *H = (*H) * DR; │ │ │ │ │ +112 } │ │ │ │ │ +113 return _e_r_r_o_r; │ │ │ │ │ +114 } │ │ │ │ │ +115 │ │ │ │ │ +116 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +117}; │ │ │ │ │ +118} // namespace gtsam │ │ │ │ │ +119 │ │ │ │ │ +_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ +This marks a GTSAM object to require alignment. │ │ │ │ │ +DDeeffiinniittiioonn types.h:308 │ │ │ │ │ +_R_o_t_3_._h │ │ │ │ │ +3D rotation represented as a rotation matrix or quaternion │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ +Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -The Factor::error simply extracts the. │ │ │ │ │ -_V_a_l_u_e_s │ │ │ │ │ -In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ -a non-linear function... │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ +Vector3 Point3 │ │ │ │ │ +As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ +typedef Point3 to Vector3... │ │ │ │ │ +DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ +Aliases. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ +Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor │ │ │ │ │ +symbol GTSAM_USE_QUATERNIO... │ │ │ │ │ +DDeeffiinniittiioonn Rot3.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_3_:_:_r_o_t_a_t_e │ │ │ │ │ +Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, │ │ │ │ │ +OptionalJacobian< 3, 3 > H2=boost::none) const │ │ │ │ │ +rotate point from rotated coordinate frame to world │ │ │ │ │ +DDeeffiinniittiioonn Rot3M.cpp:149 │ │ │ │ │ +_g_t_s_a_m_:_:_U_n_i_t_3 │ │ │ │ │ +Represents a 3D point on a unit sphere. │ │ │ │ │ +DDeeffiinniittiioonn Unit3.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_U_n_i_t_3_:_:_u_n_i_t_V_e_c_t_o_r │ │ │ │ │ +Vector3 unitVector(OptionalJacobian< 3, 2 > H=boost::none) const │ │ │ │ │ +Return unit-norm Vector. │ │ │ │ │ +DDeeffiinniittiioonn Unit3.cpp:151 │ │ │ │ │ +_g_t_s_a_m_:_:_U_n_i_t_3_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s=std::string()) const │ │ │ │ │ +The print fuction. │ │ │ │ │ +DDeeffiinniittiioonn Unit3.cpp:164 │ │ │ │ │ +_g_t_s_a_m_:_:_U_n_i_t_3_:_:_e_r_r_o_r │ │ │ │ │ +Vector2 error(const Unit3 &q, OptionalJacobian< 2, 2 > H_q=boost::none) const │ │ │ │ │ +Signed, vector-valued error between two directions. │ │ │ │ │ +DDeeffiinniittiioonn Unit3.cpp:199 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +virtual void print(const std::string &s="Factor", const KeyFormatter │ │ │ │ │ +&formatter=DefaultKeyFormatter) const │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn Factor.cpp:29 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_e_r_r_o_r │ │ │ │ │ +double error(const Values &c) const override │ │ │ │ │ +Calculate the error of the factor. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.cpp:138 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ +A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ +variables. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_ _R_o_t_3_ _>_:_:_k_e_y │ │ │ │ │ +Key key() const │ │ │ │ │ +Returns a key. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:518 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_a_t_e_F_a_c_t_o_r │ │ │ │ │ +Factor on unknown rotation iRC that relates two incremental rotations c1Rc2 = │ │ │ │ │ +iRc' * i1Ri2 * iRc Whic... │ │ │ │ │ +DDeeffiinniittiioonn RotateFactor.h:23 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_a_t_e_F_a_c_t_o_r_:_:_R_o_t_a_t_e_F_a_c_t_o_r │ │ │ │ │ +RotateFactor(Key key, const Rot3 &P, const Rot3 &Z, const SharedNoiseModel │ │ │ │ │ +&model) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn RotateFactor.h:33 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_a_t_e_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ +Vector evaluateError(const Rot3 &R, boost::optional< Matrix & > H=boost::none) │ │ │ │ │ +const override │ │ │ │ │ +vector of errors returns 2D vector │ │ │ │ │ +DDeeffiinniittiioonn RotateFactor.h:53 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_a_t_e_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn RotateFactor.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_a_t_e_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ +gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn RotateFactor.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_a_t_e_D_i_r_e_c_t_i_o_n_s_F_a_c_t_o_r │ │ │ │ │ +Factor on unknown rotation iRc that relates two directions c Directions provide │ │ │ │ │ +less constraints than... │ │ │ │ │ +DDeeffiinniittiioonn RotateFactor.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_a_t_e_D_i_r_e_c_t_i_o_n_s_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ +Vector evaluateError(const Rot3 &iRc, boost::optional< Matrix & > H=boost:: │ │ │ │ │ +none) const override │ │ │ │ │ +vector of errors returns 2D vector │ │ │ │ │ +DDeeffiinniittiioonn RotateFactor.h:105 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_a_t_e_D_i_r_e_c_t_i_o_n_s_F_a_c_t_o_r_:_:_R_o_t_a_t_e_D_i_r_e_c_t_i_o_n_s_F_a_c_t_o_r │ │ │ │ │ +RotateDirectionsFactor(Key key, const Unit3 &i_p, const Unit3 &c_z, const │ │ │ │ │ +SharedNoiseModel &model) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn RotateFactor.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_a_t_e_D_i_r_e_c_t_i_o_n_s_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn RotateFactor.h:96 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_a_t_e_D_i_r_e_c_t_i_o_n_s_F_a_c_t_o_r_:_:_I_n_i_t_i_a_l_i_z_e │ │ │ │ │ +static Rot3 Initialize(const Unit3 &i_p, const Unit3 &c_z) │ │ │ │ │ +Initialize rotation iRc such that i_p = iRc * c_z. │ │ │ │ │ +DDeeffiinniittiioonn RotateFactor.h:83 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_a_t_e_D_i_r_e_c_t_i_o_n_s_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ +gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn RotateFactor.h:91 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _l_a_g_o_._h │ │ │ │ │ + * RRoottaatteeFFaaccttoorr..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01292.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/InitializePose3.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/lago.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,56 +94,186 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs
│ │ │ │ -
InitializePose3.h File Reference
│ │ │ │ +Typedefs | │ │ │ │ +Functions
│ │ │ │ +
lago.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Initialize Pose3 in a factor graph. │ │ │ │ +

Initialize Pose2 in a factor graph using LAGO (Linear Approximation for Graph Optimization). │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

struct  gtsam::InitializePose3
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ Typedefs

│ │ │ │ -typedef std::map< Key, std::vector< size_t > > gtsam::KeyVectorMap
 
│ │ │ │ -typedef std::map< Key, Rot3 > gtsam::KeyRotMap
 
│ │ │ │ +typedef std::map< Key, double > gtsam::lago::key2doubleMap
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +key2doubleMap gtsam::lago::computeThetasToRoot (const key2doubleMap &deltaThetaMap, const PredecessorMap< Key > &tree)
 Compute the cumulative orientations (without wrapping) for all nodes wrt the root (root has zero orientation).
 
void gtsam::lago::getSymbolicGraph (std::vector< size_t > &spanningTreeIds, std::vector< size_t > &chordsIds, key2doubleMap &deltaThetaMap, const PredecessorMap< Key > &tree, const NonlinearFactorGraph &g)
 Given a factor graph "g", and a spanning tree "tree", select the nodes belonging to the tree and to g, and stores the factor slots corresponding to edges in the tree and to chordsIds wrt this tree.
 
GaussianFactorGraph gtsam::lago::buildLinearOrientationGraph (const std::vector< size_t > &spanningTreeIds, const std::vector< size_t > &chordsIds, const NonlinearFactorGraph &g, const key2doubleMap &orientationsToRoot, const PredecessorMap< Key > &tree)
 Linear factor graph with regularized orientation measurements.
 
│ │ │ │ +VectorValues gtsam::lago::initializeOrientations (const NonlinearFactorGraph &graph, bool useOdometricPath=true)
 LAGO: Return the orientations of the Pose2 in a generic factor graph.
 
│ │ │ │ +Values gtsam::lago::initialize (const NonlinearFactorGraph &graph, bool useOdometricPath=true)
 Return the values for the Pose2 in a generic factor graph.
 
│ │ │ │ +Values gtsam::lago::initialize (const NonlinearFactorGraph &graph, const Values &initialGuess)
 Only correct the orientation part in initialGuess.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Initialize Pose3 in a factor graph.

│ │ │ │ +

Initialize Pose2 in a factor graph using LAGO (Linear Approximation for Graph Optimization).

│ │ │ │ +
Author
Luca Carlone
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +
Date
May 14, 2014
│ │ │ │ +

see papers:

│ │ │ │ +

L. Carlone, R. Aragues, J. Castellanos, and B. Bona, A fast and accurate approximation for planar pose graph optimization, IJRR, 2014.

│ │ │ │ +

L. Carlone, R. Aragues, J.A. Castellanos, and B. Bona, A linear approximation for graph-based simultaneous localization and mapping, RSS, 2011.

│ │ │ │ +
Parameters
│ │ │ │ + │ │ │ │ + │ │ │ │ +
graphnonlinear factor graph (can include arbitrary factors but we assume that there is a subgraph involving Pose2 and betweenFactors). Also in the current version we assume that there is an odometric spanning path (x0->x1, x1->x2, etc) and a prior on x0. This assumption can be relaxed by using the extra argument useOdometricPath = false, although this part of code is not stable yet.
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
Returns
Values: initial guess from LAGO (only pose2 are initialized)
│ │ │ │
Author
Luca Carlone
│ │ │ │
│ │ │ │ Frank Dellaert
│ │ │ │ -
Date
August, 2014
│ │ │ │ -
│ │ │ │ +
Date
May 14, 2014
│ │ │ │ +

Function Documentation

│ │ │ │ + │ │ │ │ +

◆ buildLinearOrientationGraph()

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
GTSAM_EXPORT GaussianFactorGraph gtsam::lago::buildLinearOrientationGraph (const vector< size_t > & spanningTreeIds,
const vector< size_t > & chordsIds,
const NonlinearFactorGraph & g,
const key2doubleMap & orientationsToRoot,
const PredecessorMap< Key > & tree 
)
│ │ │ │ +
│ │ │ │ + │ │ │ │ +

Linear factor graph with regularized orientation measurements.

│ │ │ │ +

cout << "REG: key1= " << DefaultKeyFormatter(key1) << " key2= " << DefaultKeyFormatter(key2) << endl;

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ +

◆ getSymbolicGraph()

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
GTSAM_EXPORT void gtsam::lago::getSymbolicGraph (std::vector< size_t > & spanningTreeIds,
std::vector< size_t > & chordsIds,
key2doubleMap & deltaThetaMap,
const PredecessorMap< Key > & tree,
const NonlinearFactorGraph & g 
)
│ │ │ │ +
│ │ │ │ + │ │ │ │ +

Given a factor graph "g", and a spanning tree "tree", select the nodes belonging to the tree and to g, and stores the factor slots corresponding to edges in the tree and to chordsIds wrt this tree.

│ │ │ │ +

Also it computes deltaThetaMap which is a fast way to encode relative orientations along the tree: for a node key2, s.t. tree[key2]=key1, the value deltaThetaMap[key2] is relative orientation theta[key2]-theta[key1]

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,32 +1,114 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ -InitializePose3.h File Reference │ │ │ │ │ -Initialize Pose3 in a factor graph. _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +lago.h File Reference │ │ │ │ │ +Initialize Pose2 in a factor graph using LAGO (Linear Approximation for Graph │ │ │ │ │ +Optimization). _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_I_n_i_t_i_a_l_i_z_e_P_o_s_e_3 │ │ │ │ │ -  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ TTyyppeeddeeffss │ │ │ │ │ -typedef std::map< _K_e_y, std::vector< size_t > >  ggttssaamm::::KKeeyyVVeeccttoorrMMaapp │ │ │ │ │ +typedef std::map< _K_e_y, double >  ggttssaamm::::llaaggoo::::kkeeyy22ddoouubblleeMMaapp │ │ │ │ │ +  │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ + key2doubleMap  ggttssaamm::::llaaggoo::::ccoommppuutteeTThheettaassTTooRRoooott (const key2doubleMap │ │ │ │ │ + &deltaThetaMap, const _P_r_e_d_e_c_e_s_s_o_r_M_a_p< _K_e_y > &tree) │ │ │ │ │ +  Compute the cumulative orientations (without wrapping) for │ │ │ │ │ + all nodes wrt the root (root has zero orientation). │ │ │ │ │ +  │ │ │ │ │ + void  _g_t_s_a_m_:_:_l_a_g_o_:_:_g_e_t_S_y_m_b_o_l_i_c_G_r_a_p_h (std::vector< size_t > │ │ │ │ │ + &spanningTreeIds, std::vector< size_t > &chordsIds, │ │ │ │ │ + key2doubleMap &deltaThetaMap, const _P_r_e_d_e_c_e_s_s_o_r_M_a_p< _K_e_y > │ │ │ │ │ + &tree, const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h &g) │ │ │ │ │ + Given a factor graph "g", and a spanning tree "tree", │ │ │ │ │ +  select the nodes belonging to the tree and to g, and │ │ │ │ │ + stores the factor slots corresponding to edges in the tree │ │ │ │ │ + and to chordsIds wrt this tree. │ │ │ │ │ +  │ │ │ │ │ +_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h  _g_t_s_a_m_:_:_l_a_g_o_:_:_b_u_i_l_d_L_i_n_e_a_r_O_r_i_e_n_t_a_t_i_o_n_G_r_a_p_h (const std:: │ │ │ │ │ + vector< size_t > &spanningTreeIds, const std::vector< │ │ │ │ │ + size_t > &chordsIds, const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h &g, const │ │ │ │ │ + key2doubleMap &orientationsToRoot, const _P_r_e_d_e_c_e_s_s_o_r_M_a_p< │ │ │ │ │ + _K_e_y > &tree) │ │ │ │ │ +  Linear factor graph with regularized orientation │ │ │ │ │ + measurements. │ │ │ │ │ +  │ │ │ │ │ + _V_e_c_t_o_r_V_a_l_u_e_s  ggttssaamm::::llaaggoo::::iinniittiiaalliizzeeOOrriieennttaattiioonnss (const │ │ │ │ │ + _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h &graph, bool useOdometricPath=true) │ │ │ │ │ +  LAGO: Return the orientations of the _P_o_s_e_2 in a generic │ │ │ │ │ + factor graph. │ │ │ │ │   │ │ │ │ │ - typedef std::map< _K_e_y, _R_o_t_3 >  ggttssaamm::::KKeeyyRRoottMMaapp │ │ │ │ │ + _V_a_l_u_e_s  ggttssaamm::::llaaggoo::::iinniittiiaalliizzee (const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ + &graph, bool useOdometricPath=true) │ │ │ │ │ +  Return the values for the _P_o_s_e_2 in a generic factor graph. │ │ │ │ │ +  │ │ │ │ │ + _V_a_l_u_e_s  ggttssaamm::::llaaggoo::::iinniittiiaalliizzee (const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ + &graph, const _V_a_l_u_e_s &initialGuess) │ │ │ │ │ +  Only correct the orientation part in initialGuess. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Initialize Pose3 in a factor graph. │ │ │ │ │ +Initialize Pose2 in a factor graph using LAGO (Linear Approximation for Graph │ │ │ │ │ +Optimization). │ │ │ │ │ + Author │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + May 14, 2014 │ │ │ │ │ +see papers: │ │ │ │ │ +L. Carlone, R. Aragues, J. Castellanos, and B. Bona, A fast and accurate │ │ │ │ │ +approximation for planar pose graph optimization, IJRR, 2014. │ │ │ │ │ +L. Carlone, R. Aragues, J.A. Castellanos, and B. Bona, A linear approximation │ │ │ │ │ +for graph-based simultaneous localization and mapping, RSS, 2011. │ │ │ │ │ + Parameters │ │ │ │ │ + nonlinear factor graph (can include arbitrary factors but we assume │ │ │ │ │ + that there is a subgraph involving Pose2 and betweenFactors). Also │ │ │ │ │ + in the current version we assume that there is an odometric │ │ │ │ │ + graph spanning path (x0->x1, x1->x2, etc) and a prior on x0. This │ │ │ │ │ + assumption can be relaxed by using the extra argument │ │ │ │ │ + useOdometricPath = false, although this part of code is not stable │ │ │ │ │ + yet. │ │ │ │ │ + Returns │ │ │ │ │ + _V_a_l_u_e_s: initial guess from LAGO (only pose2 are initialized) │ │ │ │ │ Author │ │ │ │ │ Luca Carlone │ │ │ │ │ Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - August, 2014 │ │ │ │ │ + May 14, 2014 │ │ │ │ │ +********** FFuunnccttiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ +********** _?◆_? bbuuiillddLLiinneeaarrOOrriieennttaattiioonnGGrraapphh(()) ********** │ │ │ │ │ +GTSAM_EXPORT │ │ │ │ │ +GaussianFactorGraph gtsam:: ( const vector< size_t > &  ssppaannnniinnggTTrreeeeIIddss, │ │ │ │ │ +lago:: │ │ │ │ │ +buildLinearOrientationGraph │ │ │ │ │ + const vector< size_t > &  cchhoorrddssIIddss, │ │ │ │ │ + const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h &  gg, │ │ │ │ │ + const key2doubleMap &  oorriieennttaattiioonnssTTooRRoooott, │ │ │ │ │ + const _P_r_e_d_e_c_e_s_s_o_r_M_a_p< _K_e_y > ttrreeee  │ │ │ │ │ + &  │ │ │ │ │ + ) │ │ │ │ │ +Linear factor graph with regularized orientation measurements. │ │ │ │ │ +cout << "REG: key1= " << DefaultKeyFormatter(key1) << " key2= " << │ │ │ │ │ +DefaultKeyFormatter(key2) << endl; │ │ │ │ │ +********** _?◆_? ggeettSSyymmbboolliiccGGrraapphh(()) ********** │ │ │ │ │ +GTSAM_EXPORT void gtsam:: ( std::vector< size_t > &  ssppaannnniinnggTTrreeeeIIddss, │ │ │ │ │ +lago::getSymbolicGraph │ │ │ │ │ + std::vector< size_t > &  cchhoorrddssIIddss, │ │ │ │ │ + key2doubleMap &  ddeellttaaTThheettaaMMaapp, │ │ │ │ │ + const _P_r_e_d_e_c_e_s_s_o_r_M_a_p< _K_e_y > &  ttrreeee, │ │ │ │ │ + const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h &  gg  │ │ │ │ │ + ) │ │ │ │ │ +Given a factor graph "g", and a spanning tree "tree", select the nodes │ │ │ │ │ +belonging to the tree and to g, and stores the factor slots corresponding to │ │ │ │ │ +edges in the tree and to chordsIds wrt this tree. │ │ │ │ │ +Also it computes deltaThetaMap which is a fast way to encode relative │ │ │ │ │ +orientations along the tree: for a node key2, s.t. tree[key2]=key1, the value │ │ │ │ │ +deltaThetaMap[key2] is relative orientation theta[key2]-theta[key1] │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _I_n_i_t_i_a_l_i_z_e_P_o_s_e_3_._h │ │ │ │ │ + * _l_a_g_o_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01292_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/InitializePose3.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/lago.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,97 +98,75 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
InitializePose3.h
│ │ │ │ +
lago.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
21#pragma once
│ │ │ │ -
22
│ │ │ │ -
23#include <gtsam/geometry/Rot3.h>
│ │ │ │ -
24#include <gtsam/inference/graph.h>
│ │ │ │ -
25#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ -
26#include <gtsam/linear/VectorValues.h>
│ │ │ │ -
27#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ -
28
│ │ │ │ -
29#include <map>
│ │ │ │ -
30#include <vector>
│ │ │ │ -
31
│ │ │ │ -
32namespace gtsam {
│ │ │ │ -
33
│ │ │ │ -
34typedef std::map<Key, std::vector<size_t> > KeyVectorMap;
│ │ │ │ -
35typedef std::map<Key, Rot3> KeyRotMap;
│ │ │ │ +
35#pragma once
│ │ │ │
36
│ │ │ │ -
│ │ │ │ -
37struct GTSAM_EXPORT InitializePose3 {
│ │ │ │ -
38 static GaussianFactorGraph buildLinearOrientationGraph(
│ │ │ │ -
39 const NonlinearFactorGraph& g);
│ │ │ │ -
40
│ │ │ │ -
41 static Values normalizeRelaxedRotations(const VectorValues& relaxedRot3);
│ │ │ │ -
42
│ │ │ │ -
46 static Values computeOrientationsChordal(
│ │ │ │ -
47 const NonlinearFactorGraph& pose3Graph);
│ │ │ │ -
48
│ │ │ │ -
52 static Values computeOrientationsGradient(
│ │ │ │ -
53 const NonlinearFactorGraph& pose3Graph, const Values& givenGuess,
│ │ │ │ -
54 size_t maxIter = 10000, const bool setRefFrame = true);
│ │ │ │ -
55
│ │ │ │ -
56 static void createSymbolicGraph(const NonlinearFactorGraph& pose3Graph,
│ │ │ │ -
57 KeyVectorMap* adjEdgesMap,
│ │ │ │ -
58 KeyRotMap* factorId2RotMap);
│ │ │ │ -
59
│ │ │ │ -
60 static Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a,
│ │ │ │ -
61 const double b);
│ │ │ │ -
62
│ │ │ │ -
67 static NonlinearFactorGraph buildPose3graph(
│ │ │ │ -
68 const NonlinearFactorGraph& graph);
│ │ │ │ -
69
│ │ │ │ -
73 static Values computePoses(const Values& initialRot,
│ │ │ │ -
74 NonlinearFactorGraph* poseGraph,
│ │ │ │ -
75 bool singleIter = true);
│ │ │ │ +
37#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ +
38#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ +
39#include <gtsam/linear/VectorValues.h>
│ │ │ │ +
40#include <gtsam/inference/graph.h>
│ │ │ │ +
41
│ │ │ │ +
42namespace gtsam {
│ │ │ │ +
43namespace lago {
│ │ │ │ +
44
│ │ │ │ +
45typedef std::map<Key, double> key2doubleMap;
│ │ │ │ +
46
│ │ │ │ +
51GTSAM_EXPORT key2doubleMap computeThetasToRoot(
│ │ │ │ +
52 const key2doubleMap& deltaThetaMap, const PredecessorMap<Key>& tree);
│ │ │ │ +
53
│ │ │ │ +
62GTSAM_EXPORT void getSymbolicGraph(
│ │ │ │ +
63/*OUTPUTS*/std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds,
│ │ │ │ +
64 key2doubleMap& deltaThetaMap,
│ │ │ │ +
65 /*INPUTS*/const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g);
│ │ │ │ +
66
│ │ │ │ +
68GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(
│ │ │ │ +
69 const std::vector<size_t>& spanningTreeIds,
│ │ │ │ +
70 const std::vector<size_t>& chordsIds, const NonlinearFactorGraph& g,
│ │ │ │ +
71 const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree);
│ │ │ │ +
72
│ │ │ │ +
74GTSAM_EXPORT VectorValues initializeOrientations(
│ │ │ │ +
75 const NonlinearFactorGraph& graph, bool useOdometricPath = true);
│ │ │ │
76
│ │ │ │ -
81 static Values initializeOrientations(const NonlinearFactorGraph& graph);
│ │ │ │ -
82
│ │ │ │ -
88 static Values initialize(const NonlinearFactorGraph& graph,
│ │ │ │ -
89 const Values& givenGuess, bool useGradient = false);
│ │ │ │ -
90
│ │ │ │ -
92 static Values initialize(const NonlinearFactorGraph& graph);
│ │ │ │ -
93};
│ │ │ │ -
│ │ │ │ -
94} // end of namespace gtsam
│ │ │ │ -
Rot3.h
3D rotation represented as a rotation matrix or quaternion
│ │ │ │ -
graph.h
Graph algorithm using boost library.
│ │ │ │ -
VectorValues.h
Factor Graph Values.
│ │ │ │ -
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ -
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │ +
78GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph,
│ │ │ │ +
79 bool useOdometricPath = true);
│ │ │ │ +
80
│ │ │ │ +
82GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph,
│ │ │ │ +
83 const Values& initialGuess);
│ │ │ │ +
84
│ │ │ │ +
85} // end of namespace lago
│ │ │ │ +
86} // end of namespace gtsam
│ │ │ │ +
graph.h
Graph algorithm using boost library.
│ │ │ │ +
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ +
VectorValues.h
Factor Graph Values.
│ │ │ │ +
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
│ │ │ │ -
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ -
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ -
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │ -
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
gtsam::InitializePose3
Definition InitializePose3.h:37
│ │ │ │ +
VectorValues
The Factor::error simply extracts the.
│ │ │ │ +
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,106 +1,77 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -InitializePose3.h │ │ │ │ │ +lago.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -21#pragma once │ │ │ │ │ -22 │ │ │ │ │ -23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_3_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_g_r_a_p_h_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ -27#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -28 │ │ │ │ │ -29#include │ │ │ │ │ -30#include │ │ │ │ │ -31 │ │ │ │ │ -32namespace _g_t_s_a_m { │ │ │ │ │ -33 │ │ │ │ │ -34typedef std::map > KeyVectorMap; │ │ │ │ │ -35typedef std::map KeyRotMap; │ │ │ │ │ +35#pragma once │ │ │ │ │ 36 │ │ │ │ │ -_3_7struct GTSAM_EXPORT _I_n_i_t_i_a_l_i_z_e_P_o_s_e_3 { │ │ │ │ │ -38 static _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h buildLinearOrientationGraph( │ │ │ │ │ -39 const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& g); │ │ │ │ │ -40 │ │ │ │ │ -41 static _V_a_l_u_e_s normalizeRelaxedRotations(const _V_e_c_t_o_r_V_a_l_u_e_s& relaxedRot3); │ │ │ │ │ -42 │ │ │ │ │ -46 static _V_a_l_u_e_s computeOrientationsChordal( │ │ │ │ │ -47 const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& pose3Graph); │ │ │ │ │ -48 │ │ │ │ │ -52 static _V_a_l_u_e_s computeOrientationsGradient( │ │ │ │ │ -53 const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& pose3Graph, const _V_a_l_u_e_s& givenGuess, │ │ │ │ │ -54 size_t maxIter = 10000, const bool setRefFrame = true); │ │ │ │ │ -55 │ │ │ │ │ -56 static void createSymbolicGraph(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& pose3Graph, │ │ │ │ │ -57 KeyVectorMap* adjEdgesMap, │ │ │ │ │ -58 KeyRotMap* factorId2RotMap); │ │ │ │ │ -59 │ │ │ │ │ -60 static Vector3 gradientTron(const _R_o_t_3& R1, const _R_o_t_3& R2, const double a, │ │ │ │ │ -61 const double b); │ │ │ │ │ -62 │ │ │ │ │ -67 static _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h buildPose3graph( │ │ │ │ │ -68 const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph); │ │ │ │ │ -69 │ │ │ │ │ -73 static _V_a_l_u_e_s computePoses(const _V_a_l_u_e_s& initialRot, │ │ │ │ │ -74 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h* poseGraph, │ │ │ │ │ -75 bool singleIter = true); │ │ │ │ │ +37#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +38#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +39#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ +40#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_g_r_a_p_h_._h> │ │ │ │ │ +41 │ │ │ │ │ +42namespace _g_t_s_a_m { │ │ │ │ │ +43namespace lago { │ │ │ │ │ +44 │ │ │ │ │ +45typedef std::map key2doubleMap; │ │ │ │ │ +46 │ │ │ │ │ +51GTSAM_EXPORT key2doubleMap computeThetasToRoot( │ │ │ │ │ +52 const key2doubleMap& deltaThetaMap, const PredecessorMap& tree); │ │ │ │ │ +53 │ │ │ │ │ +62GTSAM_EXPORT void getSymbolicGraph( │ │ │ │ │ +63/*OUTPUTS*/std::vector& spanningTreeIds, std::vector& │ │ │ │ │ +chordsIds, │ │ │ │ │ +64 key2doubleMap& deltaThetaMap, │ │ │ │ │ +65 /*INPUTS*/const PredecessorMap& tree, const NonlinearFactorGraph& g); │ │ │ │ │ +66 │ │ │ │ │ +68GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph( │ │ │ │ │ +69 const std::vector& spanningTreeIds, │ │ │ │ │ +70 const std::vector& chordsIds, const NonlinearFactorGraph& g, │ │ │ │ │ +71 const key2doubleMap& orientationsToRoot, const PredecessorMap& tree); │ │ │ │ │ +72 │ │ │ │ │ +74GTSAM_EXPORT _V_e_c_t_o_r_V_a_l_u_e_s initializeOrientations( │ │ │ │ │ +75 const NonlinearFactorGraph& graph, bool useOdometricPath = true); │ │ │ │ │ 76 │ │ │ │ │ -81 static _V_a_l_u_e_s initializeOrientations(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph); │ │ │ │ │ -82 │ │ │ │ │ -88 static _V_a_l_u_e_s initialize(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, │ │ │ │ │ -89 const _V_a_l_u_e_s& givenGuess, bool useGradient = false); │ │ │ │ │ -90 │ │ │ │ │ -92 static _V_a_l_u_e_s initialize(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph); │ │ │ │ │ -93}; │ │ │ │ │ -94} // end of namespace gtsam │ │ │ │ │ -_R_o_t_3_._h │ │ │ │ │ -3D rotation represented as a rotation matrix or quaternion │ │ │ │ │ +78GTSAM_EXPORT _V_a_l_u_e_s initialize(const NonlinearFactorGraph& graph, │ │ │ │ │ +79 bool useOdometricPath = true); │ │ │ │ │ +80 │ │ │ │ │ +82GTSAM_EXPORT _V_a_l_u_e_s initialize(const NonlinearFactorGraph& graph, │ │ │ │ │ +83 const _V_a_l_u_e_s& initialGuess); │ │ │ │ │ +84 │ │ │ │ │ +85} // end of namespace lago │ │ │ │ │ +86} // end of namespace gtsam │ │ │ │ │ _g_r_a_p_h_._h │ │ │ │ │ Graph algorithm using boost library. │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ -Factor Graph Values. │ │ │ │ │ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ +Factor Graph Values. │ │ │ │ │ _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ Factor Graph consisting of non-linear factors. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ -Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor │ │ │ │ │ -symbol GTSAM_USE_QUATERNIO... │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ -with a unique integer... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_I_n_i_t_i_a_l_i_z_e_P_o_s_e_3 │ │ │ │ │ -DDeeffiinniittiioonn InitializePose3.h:37 │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +The Factor::error simply extracts the. │ │ │ │ │ +_V_a_l_u_e_s │ │ │ │ │ +In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ +a non-linear function... │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _I_n_i_t_i_a_l_i_z_e_P_o_s_e_3_._h │ │ │ │ │ + * _l_a_g_o_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01295.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/FrobeniusFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/SmartFactorParams.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,56 +96,62 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ -Functions
│ │ │ │ -
FrobeniusFactor.h File Reference
│ │ │ │ +Enumerations
│ │ │ │ +
SmartFactorParams.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Various factors that minimize some Frobenius norm. │ │ │ │ +

Collect common parameters for SmartProjection and SmartStereoProjection factors. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::FrobeniusPrior< Rot >
 FrobeniusPrior calculates the Frobenius norm between a given matrix and an element of SO(3) or SO(4). More...
 
class  gtsam::FrobeniusFactor< Rot >
 FrobeniusFactor calculates the Frobenius norm between rotation matrices. More...
 
class  gtsam::FrobeniusBetweenFactor< Rot >
 FrobeniusBetweenFactor is a BetweenFactor that evaluates the Frobenius norm of the rotation error between measured and predicted (rather than the Logmap of the error). More...
struct  gtsam::SmartProjectionParams
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ -Functions

SharedNoiseModel gtsam::ConvertNoiseModel (const SharedNoiseModel &model, size_t n, bool defaultToUnit=true)
 When creating (any) FrobeniusFactor we can convert a Rot/Pose BetweenFactor noise model into a n-dimensional isotropic noise model used to weight the Frobenius norm.
 

│ │ │ │ +Enumerations

enum  gtsam::LinearizationMode { HESSIAN │ │ │ │ +, IMPLICIT_SCHUR │ │ │ │ +, JACOBIAN_Q │ │ │ │ +, JACOBIAN_SVD │ │ │ │ + }
 SmartFactorParams: parameters and (linearization/degeneracy) modes for SmartProjection and SmartStereoProjection factors. More...
 
enum  gtsam::DegeneracyMode { IGNORE_DEGENERACY │ │ │ │ +, ZERO_ON_DEGENERACY │ │ │ │ +, HANDLE_INFINITY │ │ │ │ + }
 How to manage degeneracy.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Various factors that minimize some Frobenius norm.

│ │ │ │ -
Date
March 2019
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +

Collect common parameters for SmartProjection and SmartStereoProjection factors.

│ │ │ │ +
Author
Luca Carlone
│ │ │ │ +
│ │ │ │ +Zsolt Kira
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,44 +1,38 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -FrobeniusFactor.h File Reference │ │ │ │ │ -Various factors that minimize some Frobenius norm. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _E_n_u_m_e_r_a_t_i_o_n_s │ │ │ │ │ +SmartFactorParams.h File Reference │ │ │ │ │ +Collect common parameters for SmartProjection and SmartStereoProjection │ │ │ │ │ +factors. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_F_r_o_b_e_n_i_u_s_P_r_i_o_r_<_ _R_o_t_ _> │ │ │ │ │ -  _F_r_o_b_e_n_i_u_s_P_r_i_o_r calculates the Frobenius norm between a given matrix and │ │ │ │ │ - an element of SO(3) or SO(4). _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_F_r_o_b_e_n_i_u_s_F_a_c_t_o_r_<_ _R_o_t_ _> │ │ │ │ │ -  _F_r_o_b_e_n_i_u_s_F_a_c_t_o_r calculates the Frobenius norm between rotation │ │ │ │ │ - matrices. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_F_r_o_b_e_n_i_u_s_B_e_t_w_e_e_n_F_a_c_t_o_r_<_ _R_o_t_ _> │ │ │ │ │ - _F_r_o_b_e_n_i_u_s_B_e_t_w_e_e_n_F_a_c_t_o_r is a _B_e_t_w_e_e_n_F_a_c_t_o_r that evaluates the Frobenius │ │ │ │ │ -  norm of the rotation error between measured and predicted (rather than │ │ │ │ │ - the Logmap of the error). _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l  _g_t_s_a_m_:_:_C_o_n_v_e_r_t_N_o_i_s_e_M_o_d_e_l (const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, │ │ │ │ │ - size_t n, bool defaultToUnit=true) │ │ │ │ │ - When creating (any) _F_r_o_b_e_n_i_u_s_F_a_c_t_o_r we can convert a Rot/Pose │ │ │ │ │ -  _B_e_t_w_e_e_n_F_a_c_t_o_r noise model into a n-dimensional isotropic │ │ │ │ │ - noise model used to weight the Frobenius norm. │ │ │ │ │ +EEnnuummeerraattiioonnss │ │ │ │ │ +enum   _g_t_s_a_m_:_:_L_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e { HHEESSSSIIAANN , IIMMPPLLIICCIITT__SSCCHHUURR , JJAACCOOBBIIAANN__QQ , │ │ │ │ │ + JJAACCOOBBIIAANN__SSVVDD } │ │ │ │ │ +  SmartFactorParams: parameters and (linearization/degeneracy) modes for │ │ │ │ │ + SmartProjection and SmartStereoProjection factors. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +enum   _g_t_s_a_m_:_:_D_e_g_e_n_e_r_a_c_y_M_o_d_e { IIGGNNOORREE__DDEEGGEENNEERRAACCYY , ZZEERROO__OONN__DDEEGGEENNEERRAACCYY , │ │ │ │ │ + HHAANNDDLLEE__IINNFFIINNIITTYY } │ │ │ │ │ +  How to manage degeneracy. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Various factors that minimize some Frobenius norm. │ │ │ │ │ - Date │ │ │ │ │ - March 2019 │ │ │ │ │ +Collect common parameters for SmartProjection and SmartStereoProjection │ │ │ │ │ +factors. │ │ │ │ │ Author │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Zsolt Kira │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _F_r_o_b_e_n_i_u_s_F_a_c_t_o_r_._h │ │ │ │ │ + * _S_m_a_r_t_F_a_c_t_o_r_P_a_r_a_m_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01295.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,6 +1,14 @@ │ │ │ │ │ var a01295 = [ │ │ │ │ │ - ["gtsam::FrobeniusPrior< Rot >", "a04828.html", "a04828"], │ │ │ │ │ - ["gtsam::FrobeniusFactor< Rot >", "a04832.html", "a04832"], │ │ │ │ │ - ["gtsam::FrobeniusBetweenFactor< Rot >", "a04836.html", "a04836"], │ │ │ │ │ - ["ConvertNoiseModel", "a01295.html#aa43e7fd7c2b86873458a91ddafc506e4", null] │ │ │ │ │ + ["gtsam::SmartProjectionParams", "a04932.html", "a04932"], │ │ │ │ │ + ["DegeneracyMode", "a01295.html#ac3d021280c8a8b84903a8ce81f73ea0f", [ │ │ │ │ │ + ["IGNORE_DEGENERACY", "a01295.html#ac3d021280c8a8b84903a8ce81f73ea0fa7377887fa83ae502adf7f477205c0273", null], │ │ │ │ │ + ["ZERO_ON_DEGENERACY", "a01295.html#ac3d021280c8a8b84903a8ce81f73ea0fa88da97b27cb213cdf714af781ae76b99", null], │ │ │ │ │ + ["HANDLE_INFINITY", "a01295.html#ac3d021280c8a8b84903a8ce81f73ea0fa0836b9ba9572539be631139b0785a200", null] │ │ │ │ │ + ]], │ │ │ │ │ + ["LinearizationMode", "a01295.html#adf2020f3f6087064dde501c99794aac3", [ │ │ │ │ │ + ["HESSIAN", "a01295.html#adf2020f3f6087064dde501c99794aac3a94d642727a23c8b5a1b59b8f74553e80", null], │ │ │ │ │ + ["IMPLICIT_SCHUR", "a01295.html#adf2020f3f6087064dde501c99794aac3a7a3f02bec869fc563591ecb84ea2eb7a", null], │ │ │ │ │ + ["JACOBIAN_Q", "a01295.html#adf2020f3f6087064dde501c99794aac3a672276bd72f64f6b462004c89c57297b", null], │ │ │ │ │ + ["JACOBIAN_SVD", "a01295.html#adf2020f3f6087064dde501c99794aac3aca6b517d49e41094c865c321ddff5b81", null] │ │ │ │ │ + ]] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01295_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/FrobeniusFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/SmartFactorParams.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,181 +98,157 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
FrobeniusFactor.h
│ │ │ │ +
SmartFactorParams.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/geometry/Rot2.h>
│ │ │ │ -
22#include <gtsam/geometry/Rot3.h>
│ │ │ │ -
23#include <gtsam/geometry/SOn.h>
│ │ │ │ -
24#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/geometry/triangulation.h>
│ │ │ │ +
23
│ │ │ │ +
24namespace gtsam {
│ │ │ │
25
│ │ │ │ -
26namespace gtsam {
│ │ │ │ -
27
│ │ │ │ -
42GTSAM_EXPORT SharedNoiseModel
│ │ │ │ -
43ConvertNoiseModel(const SharedNoiseModel &model, size_t n,
│ │ │ │ -
44 bool defaultToUnit = true);
│ │ │ │ -
45
│ │ │ │ -
50template <class Rot>
│ │ │ │ -
│ │ │ │ -
51class FrobeniusPrior : public NoiseModelFactorN<Rot> {
│ │ │ │ -
52 enum { Dim = Rot::VectorN2::RowsAtCompileTime };
│ │ │ │ -
53 using MatrixNN = typename Rot::MatrixNN;
│ │ │ │ -
54 Eigen::Matrix<double, Dim, 1> vecM_;
│ │ │ │ -
55
│ │ │ │ -
56 public:
│ │ │ │ -
57 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
│ │ │ │ +
30enum LinearizationMode {
│ │ │ │ +
31 HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
│ │ │ │ +
32};
│ │ │ │ +
│ │ │ │ +
33
│ │ │ │ +
│ │ │ │ +
35enum DegeneracyMode {
│ │ │ │ +
36 IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY
│ │ │ │ +
37};
│ │ │ │ +
│ │ │ │ +
38
│ │ │ │ +
39/*
│ │ │ │ +
40 * Parameters for the smart (stereo) projection factors
│ │ │ │ +
41 */
│ │ │ │ +
│ │ │ │ +
42struct SmartProjectionParams {
│ │ │ │ +
43
│ │ │ │ +
44 LinearizationMode linearizationMode;
│ │ │ │ +
45 DegeneracyMode degeneracyMode;
│ │ │ │ +
46
│ │ │ │ +
49 TriangulationParameters triangulation;
│ │ │ │ +
50 double retriangulationThreshold;
│ │ │ │ +
52
│ │ │ │ +
55 bool throwCheirality;
│ │ │ │ +
56 bool verboseCheirality;
│ │ │ │
58
│ │ │ │ -
│ │ │ │ -
60 FrobeniusPrior(Key j, const MatrixNN& M,
│ │ │ │ -
61 const SharedNoiseModel& model = nullptr)
│ │ │ │ -
62 : NoiseModelFactorN<Rot>(ConvertNoiseModel(model, Dim), j) {
│ │ │ │ -
63 vecM_ << Eigen::Map<const Matrix>(M.data(), Dim, 1);
│ │ │ │ -
64 }
│ │ │ │ -
│ │ │ │ -
65
│ │ │ │ -
│ │ │ │ -
67 Vector evaluateError(const Rot& R,
│ │ │ │ -
68 boost::optional<Matrix&> H = boost::none) const override {
│ │ │ │ -
69 return R.vec(H) - vecM_; // Jacobian is computed only when needed.
│ │ │ │ -
70 }
│ │ │ │ -
│ │ │ │ -
71};
│ │ │ │ -
│ │ │ │ -
72
│ │ │ │ -
77template <class Rot>
│ │ │ │ -
│ │ │ │ -
78class FrobeniusFactor : public NoiseModelFactorN<Rot, Rot> {
│ │ │ │ -
79 enum { Dim = Rot::VectorN2::RowsAtCompileTime };
│ │ │ │ -
80
│ │ │ │ -
81 public:
│ │ │ │ -
│ │ │ │ -
83 FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr)
│ │ │ │ -
84 : NoiseModelFactorN<Rot, Rot>(ConvertNoiseModel(model, Dim), j1,
│ │ │ │ -
85 j2) {}
│ │ │ │ -
│ │ │ │ -
86
│ │ │ │ -
│ │ │ │ -
88 Vector evaluateError(const Rot& R1, const Rot& R2,
│ │ │ │ -
89 boost::optional<Matrix&> H1 = boost::none,
│ │ │ │ -
90 boost::optional<Matrix&> H2 = boost::none) const override {
│ │ │ │ -
91 Vector error = R2.vec(H2) - R1.vec(H1);
│ │ │ │ -
92 if (H1) *H1 = -*H1;
│ │ │ │ -
93 return error;
│ │ │ │ -
94 }
│ │ │ │ -
│ │ │ │ -
95};
│ │ │ │ -
│ │ │ │ -
96
│ │ │ │ -
103template <class Rot>
│ │ │ │ -
│ │ │ │ -
104class FrobeniusBetweenFactor : public NoiseModelFactorN<Rot, Rot> {
│ │ │ │ -
105 Rot R12_;
│ │ │ │ -
106 Eigen::Matrix<double, Rot::dimension, Rot::dimension>
│ │ │ │ -
107 R2hat_H_R1_;
│ │ │ │ -
108 enum { Dim = Rot::VectorN2::RowsAtCompileTime };
│ │ │ │ -
109
│ │ │ │ -
110 public:
│ │ │ │ -
111 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
112
│ │ │ │ -
115
│ │ │ │ -
│ │ │ │ -
117 FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12,
│ │ │ │ -
118 const SharedNoiseModel& model = nullptr)
│ │ │ │ -
119 : NoiseModelFactorN<Rot, Rot>(
│ │ │ │ -
120 ConvertNoiseModel(model, Dim), j1, j2),
│ │ │ │ -
121 R12_(R12),
│ │ │ │ -
122 R2hat_H_R1_(R12.inverse().AdjointMap()) {}
│ │ │ │ -
│ │ │ │ -
123
│ │ │ │ -
127
│ │ │ │ -
129 void
│ │ │ │ -
│ │ │ │ -
130 print(const std::string &s,
│ │ │ │ -
131 const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ -
132 std::cout << s << "FrobeniusBetweenFactor<" << demangle(typeid(Rot).name())
│ │ │ │ -
133 << ">(" << keyFormatter(this->key1()) << ","
│ │ │ │ -
134 << keyFormatter(this->key2()) << ")\n";
│ │ │ │ -
135 traits<Rot>::Print(R12_, " R12: ");
│ │ │ │ -
136 this->noiseModel_->print(" noise model: ");
│ │ │ │ -
137 }
│ │ │ │ -
│ │ │ │ -
138
│ │ │ │ -
│ │ │ │ -
140 bool equals(const NonlinearFactor &expected,
│ │ │ │ -
141 double tol = 1e-9) const override {
│ │ │ │ -
142 auto e = dynamic_cast<const FrobeniusBetweenFactor *>(&expected);
│ │ │ │ -
143 return e != nullptr && NoiseModelFactorN<Rot, Rot>::equals(*e, tol) &&
│ │ │ │ -
144 traits<Rot>::Equals(this->R12_, e->R12_, tol);
│ │ │ │ -
145 }
│ │ │ │ -
│ │ │ │ -
146
│ │ │ │ -
150
│ │ │ │ -
│ │ │ │ -
152 Vector evaluateError(const Rot& R1, const Rot& R2,
│ │ │ │ -
153 boost::optional<Matrix&> H1 = boost::none,
│ │ │ │ -
154 boost::optional<Matrix&> H2 = boost::none) const override {
│ │ │ │ -
155 const Rot R2hat = R1.compose(R12_);
│ │ │ │ -
156 Eigen::Matrix<double, Dim, Rot::dimension> vec_H_R2hat;
│ │ │ │ -
157 Vector error = R2.vec(H2) - R2hat.vec(H1 ? &vec_H_R2hat : nullptr);
│ │ │ │ -
158 if (H1) *H1 = -vec_H_R2hat * R2hat_H_R1_;
│ │ │ │ -
159 return error;
│ │ │ │ -
160 }
│ │ │ │ -
│ │ │ │ -
162};
│ │ │ │ -
│ │ │ │ -
163
│ │ │ │ -
164} // namespace gtsam
│ │ │ │ -
SOn.h
N*N matrix representation of SO(N).
│ │ │ │ -
Rot3.h
3D rotation represented as a rotation matrix or quaternion
│ │ │ │ -
Rot2.h
2D rotation
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
59 // Constructor
│ │ │ │ +
60 SmartProjectionParams(LinearizationMode linMode = HESSIAN,
│ │ │ │ +
61 DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false,
│ │ │ │ +
62 bool verboseCheirality = false, double retriangulationTh = 1e-5) :
│ │ │ │ +
63 linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold(
│ │ │ │ +
64 retriangulationTh), throwCheirality(throwCheirality), verboseCheirality(
│ │ │ │ +
65 verboseCheirality) {
│ │ │ │ +
66 }
│ │ │ │ +
67
│ │ │ │ +
68 virtual ~SmartProjectionParams() {
│ │ │ │ +
69 }
│ │ │ │ +
70
│ │ │ │ +
71 void print(const std::string& str = "") const {
│ │ │ │ +
72 std::cout << "linearizationMode: " << linearizationMode << "\n";
│ │ │ │ +
73 std::cout << " degeneracyMode: " << degeneracyMode << "\n";
│ │ │ │ +
74 std::cout << triangulation << std::endl;
│ │ │ │ +
75 }
│ │ │ │ +
76
│ │ │ │ +
77 // get class variables
│ │ │ │ +
78 LinearizationMode getLinearizationMode() const {
│ │ │ │ +
79 return linearizationMode;
│ │ │ │ +
80 }
│ │ │ │ +
81 DegeneracyMode getDegeneracyMode() const {
│ │ │ │ +
82 return degeneracyMode;
│ │ │ │ +
83 }
│ │ │ │ +
84 TriangulationParameters getTriangulationParameters() const {
│ │ │ │ +
85 return triangulation;
│ │ │ │ +
86 }
│ │ │ │ +
87 bool getVerboseCheirality() const {
│ │ │ │ +
88 return verboseCheirality;
│ │ │ │ +
89 }
│ │ │ │ +
90 bool getThrowCheirality() const {
│ │ │ │ +
91 return throwCheirality;
│ │ │ │ +
92 }
│ │ │ │ +
93 double getRetriangulationThreshold() const {
│ │ │ │ +
94 return retriangulationThreshold;
│ │ │ │ +
95 }
│ │ │ │ +
96 // set class variables
│ │ │ │ +
97 void setLinearizationMode(LinearizationMode linMode) {
│ │ │ │ +
98 linearizationMode = linMode;
│ │ │ │ +
99 }
│ │ │ │ +
100 void setDegeneracyMode(DegeneracyMode degMode) {
│ │ │ │ +
101 degeneracyMode = degMode;
│ │ │ │ +
102 }
│ │ │ │ +
103 void setRetriangulationThreshold(double retriangulationTh) {
│ │ │ │ +
104 retriangulationThreshold = retriangulationTh;
│ │ │ │ +
105 }
│ │ │ │ +
106 void setRankTolerance(double rankTol) {
│ │ │ │ +
107 triangulation.rankTolerance = rankTol;
│ │ │ │ +
108 }
│ │ │ │ +
109 void setEnableEPI(bool enableEPI) {
│ │ │ │ +
110 triangulation.enableEPI = enableEPI;
│ │ │ │ +
111 }
│ │ │ │ +
112 void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) {
│ │ │ │ +
113 triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold;
│ │ │ │ +
114 }
│ │ │ │ +
115 void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) {
│ │ │ │ +
116 triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold;
│ │ │ │ +
117 }
│ │ │ │ +
118
│ │ │ │ +
119private:
│ │ │ │ +
120
│ │ │ │ +
122 friend class boost::serialization::access;
│ │ │ │ +
123 template<class ARCHIVE>
│ │ │ │ +
124 void serialize(ARCHIVE & ar, const unsigned int version) {
│ │ │ │ +
125 ar & BOOST_SERIALIZATION_NVP(linearizationMode);
│ │ │ │ +
126 ar & BOOST_SERIALIZATION_NVP(degeneracyMode);
│ │ │ │ +
127 ar & BOOST_SERIALIZATION_NVP(triangulation);
│ │ │ │ +
128 ar & BOOST_SERIALIZATION_NVP(retriangulationThreshold);
│ │ │ │ +
129 ar & BOOST_SERIALIZATION_NVP(throwCheirality);
│ │ │ │ +
130 ar & BOOST_SERIALIZATION_NVP(verboseCheirality);
│ │ │ │ +
131 }
│ │ │ │ +
132};
│ │ │ │ +
│ │ │ │ +
133
│ │ │ │ +
134} // \ namespace gtsam
│ │ │ │ +
triangulation.h
Functions for triangulation.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::demangle
std::string demangle(const char *name)
Pretty print Value type name.
Definition types.cpp:37
│ │ │ │ -
gtsam::ConvertNoiseModel
SharedNoiseModel ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit)
When creating (any) FrobeniusFactor we can convert a Rot/Pose BetweenFactor noise model into a n-dime...
Definition FrobeniusFactor.cpp:27
│ │ │ │ -
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
│ │ │ │ -
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ -
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Calculate the error of the factor.
Definition NonlinearFactor.cpp:138
│ │ │ │ -
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ -
gtsam::FrobeniusPrior
FrobeniusPrior calculates the Frobenius norm between a given matrix and an element of SO(3) or SO(4).
Definition FrobeniusFactor.h:51
│ │ │ │ -
gtsam::FrobeniusPrior::FrobeniusPrior
EIGEN_MAKE_ALIGNED_OPERATOR_NEW FrobeniusPrior(Key j, const MatrixNN &M, const SharedNoiseModel &model=nullptr)
Constructor.
Definition FrobeniusFactor.h:60
│ │ │ │ -
gtsam::FrobeniusPrior::evaluateError
Vector evaluateError(const Rot &R, boost::optional< Matrix & > H=boost::none) const override
Error is just Frobenius norm between Rot element and vectorized matrix M.
Definition FrobeniusFactor.h:67
│ │ │ │ -
gtsam::FrobeniusFactor
FrobeniusFactor calculates the Frobenius norm between rotation matrices.
Definition FrobeniusFactor.h:78
│ │ │ │ -
gtsam::FrobeniusFactor::evaluateError
Vector evaluateError(const Rot &R1, const Rot &R2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Error is just Frobenius norm between rotation matrices.
Definition FrobeniusFactor.h:88
│ │ │ │ -
gtsam::FrobeniusFactor::FrobeniusFactor
FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel &model=nullptr)
Constructor.
Definition FrobeniusFactor.h:83
│ │ │ │ -
gtsam::FrobeniusBetweenFactor
FrobeniusBetweenFactor is a BetweenFactor that evaluates the Frobenius norm of the rotation error bet...
Definition FrobeniusFactor.h:104
│ │ │ │ -
gtsam::FrobeniusBetweenFactor::FrobeniusBetweenFactor
FrobeniusBetweenFactor(Key j1, Key j2, const Rot &R12, const SharedNoiseModel &model=nullptr)
Construct from two keys and measured rotation.
Definition FrobeniusFactor.h:117
│ │ │ │ -
gtsam::FrobeniusBetweenFactor::print
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print with optional string
Definition FrobeniusFactor.h:130
│ │ │ │ -
gtsam::FrobeniusBetweenFactor::evaluateError
Vector evaluateError(const Rot &R1, const Rot &R2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Error is Frobenius norm between R1*R12 and R2.
Definition FrobeniusFactor.h:152
│ │ │ │ -
gtsam::FrobeniusBetweenFactor::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
assert equality up to a tolerance
Definition FrobeniusFactor.h:140
│ │ │ │ +
gtsam::DegeneracyMode
DegeneracyMode
How to manage degeneracy.
Definition SmartFactorParams.h:35
│ │ │ │ +
gtsam::LinearizationMode
LinearizationMode
SmartFactorParams: parameters and (linearization/degeneracy) modes for SmartProjection and SmartStere...
Definition SmartFactorParams.h:30
│ │ │ │ +
gtsam::TriangulationParameters
Definition triangulation.h:556
│ │ │ │ +
gtsam::TriangulationParameters::dynamicOutlierRejectionThreshold
double dynamicOutlierRejectionThreshold
If this is nonnegative the we will check if the average reprojection error is smaller than this thres...
Definition triangulation.h:573
│ │ │ │ +
gtsam::TriangulationParameters::rankTolerance
double rankTolerance
threshold to decide whether triangulation is result.degenerate
Definition triangulation.h:558
│ │ │ │ +
gtsam::TriangulationParameters::landmarkDistanceThreshold
double landmarkDistanceThreshold
if the landmark is triangulated at distance larger than this, result is flagged as degenerate.
Definition triangulation.h:566
│ │ │ │ +
gtsam::TriangulationParameters::enableEPI
bool enableEPI
if set to true, will refine triangulation using LM
Definition triangulation.h:560
│ │ │ │ +
gtsam::SmartProjectionParams
Definition SmartFactorParams.h:42
│ │ │ │ +
gtsam::SmartProjectionParams::throwCheirality
bool throwCheirality
If true, re-throws Cheirality exceptions (default: false)
Definition SmartFactorParams.h:55
│ │ │ │ +
gtsam::SmartProjectionParams::linearizationMode
LinearizationMode linearizationMode
How to linearize the factor.
Definition SmartFactorParams.h:44
│ │ │ │ +
gtsam::SmartProjectionParams::retriangulationThreshold
double retriangulationThreshold
threshold to decide whether to re-triangulate
Definition SmartFactorParams.h:50
│ │ │ │ +
gtsam::SmartProjectionParams::degeneracyMode
DegeneracyMode degeneracyMode
How to linearize the factor.
Definition SmartFactorParams.h:45
│ │ │ │ +
gtsam::SmartProjectionParams::verboseCheirality
bool verboseCheirality
If true, prints text for Cheirality exceptions (default: false)
Definition SmartFactorParams.h:56
│ │ │ │ +
gtsam::SmartProjectionParams::access
friend class boost::serialization::access
Serialization function.
Definition SmartFactorParams.h:122
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,226 +1,188 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -FrobeniusFactor.h │ │ │ │ │ +SmartFactorParams.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_2_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_3_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_S_O_n_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_t_r_i_a_n_g_u_l_a_t_i_o_n_._h> │ │ │ │ │ +23 │ │ │ │ │ +24namespace _g_t_s_a_m { │ │ │ │ │ 25 │ │ │ │ │ -26namespace _g_t_s_a_m { │ │ │ │ │ -27 │ │ │ │ │ -42GTSAM_EXPORT _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -43_C_o_n_v_e_r_t_N_o_i_s_e_M_o_d_e_l(const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, size_t n, │ │ │ │ │ -44 bool defaultToUnit = true); │ │ │ │ │ -45 │ │ │ │ │ -50template │ │ │ │ │ -_5_1class _F_r_o_b_e_n_i_u_s_P_r_i_o_r : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -52 enum { Dim = Rot::VectorN2::RowsAtCompileTime }; │ │ │ │ │ -53 using MatrixNN = typename Rot::MatrixNN; │ │ │ │ │ -54 Eigen::Matrix vecM_; │ │ │ │ │ -55 │ │ │ │ │ -56 public: │ │ │ │ │ -57 EIGEN_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ +_3_0enum _L_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e { │ │ │ │ │ +31 HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD │ │ │ │ │ +32}; │ │ │ │ │ +33 │ │ │ │ │ +_3_5enum _D_e_g_e_n_e_r_a_c_y_M_o_d_e { │ │ │ │ │ +36 IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY │ │ │ │ │ +37}; │ │ │ │ │ +38 │ │ │ │ │ +39/* │ │ │ │ │ +40 * Parameters for the smart (stereo) projection factors │ │ │ │ │ +41 */ │ │ │ │ │ +_4_2struct _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s { │ │ │ │ │ +43 │ │ │ │ │ +_4_4 _L_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e _l_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e; │ │ │ │ │ +_4_5 _D_e_g_e_n_e_r_a_c_y_M_o_d_e _d_e_g_e_n_e_r_a_c_y_M_o_d_e; │ │ │ │ │ +46 │ │ │ │ │ +49 _T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s triangulation; │ │ │ │ │ +_5_0 double _r_e_t_r_i_a_n_g_u_l_a_t_i_o_n_T_h_r_e_s_h_o_l_d; │ │ │ │ │ +52 │ │ │ │ │ +_5_5 bool _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y; │ │ │ │ │ +_5_6 bool _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y; │ │ │ │ │ 58 │ │ │ │ │ -_6_0 _F_r_o_b_e_n_i_u_s_P_r_i_o_r(_K_e_y j, const MatrixNN& M, │ │ │ │ │ -61 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model = nullptr) │ │ │ │ │ -62 : _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N(_C_o_n_v_e_r_t_N_o_i_s_e_M_o_d_e_l(model, Dim), j) { │ │ │ │ │ -63 vecM_ << Eigen::Map(M.data(), Dim, 1); │ │ │ │ │ -64 } │ │ │ │ │ -65 │ │ │ │ │ -_6_7 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const Rot& R, │ │ │ │ │ -68 boost::optional H = boost::none) const override { │ │ │ │ │ -69 return R.vec(H) - vecM_; // Jacobian is computed only when needed. │ │ │ │ │ -70 } │ │ │ │ │ -71}; │ │ │ │ │ -72 │ │ │ │ │ -77template │ │ │ │ │ -_7_8class _F_r_o_b_e_n_i_u_s_F_a_c_t_o_r : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -79 enum { Dim = Rot::VectorN2::RowsAtCompileTime }; │ │ │ │ │ -80 │ │ │ │ │ -81 public: │ │ │ │ │ -_8_3 _F_r_o_b_e_n_i_u_s_F_a_c_t_o_r(_K_e_y j1, _K_e_y j2, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model = nullptr) │ │ │ │ │ -84 : _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N(_C_o_n_v_e_r_t_N_o_i_s_e_M_o_d_e_l(model, Dim), j1, │ │ │ │ │ -85 j2) {} │ │ │ │ │ -86 │ │ │ │ │ -_8_8 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const Rot& R1, const Rot& R2, │ │ │ │ │ -89 boost::optional H1 = boost::none, │ │ │ │ │ -90 boost::optional H2 = boost::none) const override { │ │ │ │ │ -91 Vector _e_r_r_o_r = R2.vec(H2) - R1.vec(H1); │ │ │ │ │ -92 if (H1) *H1 = -*H1; │ │ │ │ │ -93 return _e_r_r_o_r; │ │ │ │ │ -94 } │ │ │ │ │ -95}; │ │ │ │ │ -96 │ │ │ │ │ -103template │ │ │ │ │ -_1_0_4class _F_r_o_b_e_n_i_u_s_B_e_t_w_e_e_n_F_a_c_t_o_r : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -105 Rot R12_; │ │ │ │ │ -106 Eigen::Matrix │ │ │ │ │ -107 R2hat_H_R1_; │ │ │ │ │ -108 enum { Dim = Rot::VectorN2::RowsAtCompileTime }; │ │ │ │ │ -109 │ │ │ │ │ -110 public: │ │ │ │ │ -111 EIGEN_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ -112 │ │ │ │ │ -115 │ │ │ │ │ -_1_1_7 _F_r_o_b_e_n_i_u_s_B_e_t_w_e_e_n_F_a_c_t_o_r(_K_e_y j1, _K_e_y j2, const Rot& R12, │ │ │ │ │ -118 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model = nullptr) │ │ │ │ │ -119 : _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N( │ │ │ │ │ -120 _C_o_n_v_e_r_t_N_o_i_s_e_M_o_d_e_l(model, Dim), j1, j2), │ │ │ │ │ -121 R12_(R12), │ │ │ │ │ -122 R2hat_H_R1_(R12.inverse().AdjointMap()) {} │ │ │ │ │ -123 │ │ │ │ │ -127 │ │ │ │ │ -129 void │ │ │ │ │ -_1_3_0 _p_r_i_n_t(const std::string &s, │ │ │ │ │ -131 const _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ -132 std::cout << s << "FrobeniusBetweenFactor<" << _d_e_m_a_n_g_l_e(typeid(Rot).name()) │ │ │ │ │ -133 << ">(" << keyFormatter(this->key1()) << "," │ │ │ │ │ -134 << keyFormatter(this->key2()) << ")\n"; │ │ │ │ │ -135 _t_r_a_i_t_s_<_R_o_t_>_:_:_P_r_i_n_t(R12_, " R12: "); │ │ │ │ │ -136 this->noiseModel_->print(" noise model: "); │ │ │ │ │ -137 } │ │ │ │ │ -138 │ │ │ │ │ -_1_4_0 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r &expected, │ │ │ │ │ -141 double tol = 1e-9) const override { │ │ │ │ │ -142 auto e = dynamic_cast(&expected); │ │ │ │ │ -143 return e != nullptr && _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_R_o_t_,_ _R_o_t_>_:_:_e_q_u_a_l_s(*e, tol) && │ │ │ │ │ -144 _t_r_a_i_t_s_<_R_o_t_>_:_:_E_q_u_a_l_s(this->R12_, e->R12_, tol); │ │ │ │ │ -145 } │ │ │ │ │ -146 │ │ │ │ │ -150 │ │ │ │ │ -_1_5_2 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const Rot& R1, const Rot& R2, │ │ │ │ │ -153 boost::optional H1 = boost::none, │ │ │ │ │ -154 boost::optional H2 = boost::none) const override { │ │ │ │ │ -155 const Rot R2hat = R1.compose(R12_); │ │ │ │ │ -156 Eigen::Matrix vec_H_R2hat; │ │ │ │ │ -157 Vector _e_r_r_o_r = R2.vec(H2) - R2hat.vec(H1 ? &vec_H_R2hat : nullptr); │ │ │ │ │ -158 if (H1) *H1 = -vec_H_R2hat * R2hat_H_R1_; │ │ │ │ │ -159 return _e_r_r_o_r; │ │ │ │ │ -160 } │ │ │ │ │ -162}; │ │ │ │ │ -163 │ │ │ │ │ -164} // namespace gtsam │ │ │ │ │ -_S_O_n_._h │ │ │ │ │ -N*N matrix representation of SO(N). │ │ │ │ │ -_R_o_t_3_._h │ │ │ │ │ -3D rotation represented as a rotation matrix or quaternion │ │ │ │ │ -_R_o_t_2_._h │ │ │ │ │ -2D rotation │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ -Non-linear factor base classes. │ │ │ │ │ +59 // Constructor │ │ │ │ │ +60 _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s(_L_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e linMode = HESSIAN, │ │ │ │ │ +61 _D_e_g_e_n_e_r_a_c_y_M_o_d_e degMode = IGNORE_DEGENERACY, bool _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y = false, │ │ │ │ │ +62 bool _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y = false, double retriangulationTh = 1e-5) : │ │ │ │ │ +63 _l_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e(linMode), _d_e_g_e_n_e_r_a_c_y_M_o_d_e(degMode), │ │ │ │ │ +_r_e_t_r_i_a_n_g_u_l_a_t_i_o_n_T_h_r_e_s_h_o_l_d( │ │ │ │ │ +64 retriangulationTh), _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y(_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y), _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y( │ │ │ │ │ +65 _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y) { │ │ │ │ │ +66 } │ │ │ │ │ +67 │ │ │ │ │ +68 virtual _~_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s() { │ │ │ │ │ +69 } │ │ │ │ │ +70 │ │ │ │ │ +71 void print(const std::string& str = "") const { │ │ │ │ │ +72 std::cout << "linearizationMode: " << _l_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e << "\n"; │ │ │ │ │ +73 std::cout << " degeneracyMode: " << _d_e_g_e_n_e_r_a_c_y_M_o_d_e << "\n"; │ │ │ │ │ +74 std::cout << triangulation << std::endl; │ │ │ │ │ +75 } │ │ │ │ │ +76 │ │ │ │ │ +77 // get class variables │ │ │ │ │ +78 _L_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e getLinearizationMode() const { │ │ │ │ │ +79 return _l_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e; │ │ │ │ │ +80 } │ │ │ │ │ +81 _D_e_g_e_n_e_r_a_c_y_M_o_d_e getDegeneracyMode() const { │ │ │ │ │ +82 return _d_e_g_e_n_e_r_a_c_y_M_o_d_e; │ │ │ │ │ +83 } │ │ │ │ │ +84 TriangulationParameters getTriangulationParameters() const { │ │ │ │ │ +85 return triangulation; │ │ │ │ │ +86 } │ │ │ │ │ +87 bool getVerboseCheirality() const { │ │ │ │ │ +88 return _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y; │ │ │ │ │ +89 } │ │ │ │ │ +90 bool getThrowCheirality() const { │ │ │ │ │ +91 return _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y; │ │ │ │ │ +92 } │ │ │ │ │ +93 double getRetriangulationThreshold() const { │ │ │ │ │ +94 return _r_e_t_r_i_a_n_g_u_l_a_t_i_o_n_T_h_r_e_s_h_o_l_d; │ │ │ │ │ +95 } │ │ │ │ │ +96 // set class variables │ │ │ │ │ +97 void setLinearizationMode(_L_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e linMode) { │ │ │ │ │ +98 _l_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e = linMode; │ │ │ │ │ +99 } │ │ │ │ │ +100 void setDegeneracyMode(_D_e_g_e_n_e_r_a_c_y_M_o_d_e degMode) { │ │ │ │ │ +101 _d_e_g_e_n_e_r_a_c_y_M_o_d_e = degMode; │ │ │ │ │ +102 } │ │ │ │ │ +103 void setRetriangulationThreshold(double retriangulationTh) { │ │ │ │ │ +104 _r_e_t_r_i_a_n_g_u_l_a_t_i_o_n_T_h_r_e_s_h_o_l_d = retriangulationTh; │ │ │ │ │ +105 } │ │ │ │ │ +106 void setRankTolerance(double rankTol) { │ │ │ │ │ +107 triangulation._r_a_n_k_T_o_l_e_r_a_n_c_e = rankTol; │ │ │ │ │ +108 } │ │ │ │ │ +109 void setEnableEPI(bool enableEPI) { │ │ │ │ │ +110 triangulation._e_n_a_b_l_e_E_P_I = enableEPI; │ │ │ │ │ +111 } │ │ │ │ │ +112 void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) { │ │ │ │ │ +113 triangulation._l_a_n_d_m_a_r_k_D_i_s_t_a_n_c_e_T_h_r_e_s_h_o_l_d = landmarkDistanceThreshold; │ │ │ │ │ +114 } │ │ │ │ │ +115 void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) { │ │ │ │ │ +116 triangulation._d_y_n_a_m_i_c_O_u_t_l_i_e_r_R_e_j_e_c_t_i_o_n_T_h_r_e_s_h_o_l_d = dynOutRejectionThreshold; │ │ │ │ │ +117 } │ │ │ │ │ +118 │ │ │ │ │ +119private: │ │ │ │ │ +120 │ │ │ │ │ +_1_2_2 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +123 template │ │ │ │ │ +124 void serialize(ARCHIVE & ar, const unsigned int version) { │ │ │ │ │ +125 ar & BOOST_SERIALIZATION_NVP(_l_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e); │ │ │ │ │ +126 ar & BOOST_SERIALIZATION_NVP(_d_e_g_e_n_e_r_a_c_y_M_o_d_e); │ │ │ │ │ +127 ar & BOOST_SERIALIZATION_NVP(triangulation); │ │ │ │ │ +128 ar & BOOST_SERIALIZATION_NVP(_r_e_t_r_i_a_n_g_u_l_a_t_i_o_n_T_h_r_e_s_h_o_l_d); │ │ │ │ │ +129 ar & BOOST_SERIALIZATION_NVP(_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y); │ │ │ │ │ +130 ar & BOOST_SERIALIZATION_NVP(_v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y); │ │ │ │ │ +131 } │ │ │ │ │ +132}; │ │ │ │ │ +133 │ │ │ │ │ +134} // \ namespace gtsam │ │ │ │ │ +_t_r_i_a_n_g_u_l_a_t_i_o_n_._h │ │ │ │ │ +Functions for triangulation. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_d_e_m_a_n_g_l_e │ │ │ │ │ -std::string demangle(const char *name) │ │ │ │ │ -Pretty print Value type name. │ │ │ │ │ -DDeeffiinniittiioonn types.cpp:37 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_v_e_r_t_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -SharedNoiseModel ConvertNoiseModel(const SharedNoiseModel &model, size_t d, │ │ │ │ │ -bool defaultToUnit) │ │ │ │ │ -When creating (any) FrobeniusFactor we can convert a Rot/Pose BetweenFactor │ │ │ │ │ -noise model into a n-dime... │ │ │ │ │ -DDeeffiinniittiioonn FrobeniusFactor.cpp:27 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ -Aliases. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const This &other, double tol=1e-9) const │ │ │ │ │ -check equality │ │ │ │ │ -DDeeffiinniittiioonn Factor.cpp:42 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ -Nonlinear factor base class. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_e_r_r_o_r │ │ │ │ │ -double error(const Values &c) const override │ │ │ │ │ -Calculate the error of the factor. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.cpp:138 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ -A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ -variables. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ -_g_t_s_a_m_:_:_F_r_o_b_e_n_i_u_s_P_r_i_o_r │ │ │ │ │ -FrobeniusPrior calculates the Frobenius norm between a given matrix and an │ │ │ │ │ -element of SO(3) or SO(4). │ │ │ │ │ -DDeeffiinniittiioonn FrobeniusFactor.h:51 │ │ │ │ │ -_g_t_s_a_m_:_:_F_r_o_b_e_n_i_u_s_P_r_i_o_r_:_:_F_r_o_b_e_n_i_u_s_P_r_i_o_r │ │ │ │ │ -EIGEN_MAKE_ALIGNED_OPERATOR_NEW FrobeniusPrior(Key j, const MatrixNN &M, const │ │ │ │ │ -SharedNoiseModel &model=nullptr) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn FrobeniusFactor.h:60 │ │ │ │ │ -_g_t_s_a_m_:_:_F_r_o_b_e_n_i_u_s_P_r_i_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ -Vector evaluateError(const Rot &R, boost::optional< Matrix & > H=boost::none) │ │ │ │ │ -const override │ │ │ │ │ -Error is just Frobenius norm between Rot element and vectorized matrix M. │ │ │ │ │ -DDeeffiinniittiioonn FrobeniusFactor.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_F_r_o_b_e_n_i_u_s_F_a_c_t_o_r │ │ │ │ │ -FrobeniusFactor calculates the Frobenius norm between rotation matrices. │ │ │ │ │ -DDeeffiinniittiioonn FrobeniusFactor.h:78 │ │ │ │ │ -_g_t_s_a_m_:_:_F_r_o_b_e_n_i_u_s_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ -Vector evaluateError(const Rot &R1, const Rot &R2, boost::optional< Matrix & > │ │ │ │ │ -H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override │ │ │ │ │ -Error is just Frobenius norm between rotation matrices. │ │ │ │ │ -DDeeffiinniittiioonn FrobeniusFactor.h:88 │ │ │ │ │ -_g_t_s_a_m_:_:_F_r_o_b_e_n_i_u_s_F_a_c_t_o_r_:_:_F_r_o_b_e_n_i_u_s_F_a_c_t_o_r │ │ │ │ │ -FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel &model=nullptr) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn FrobeniusFactor.h:83 │ │ │ │ │ -_g_t_s_a_m_:_:_F_r_o_b_e_n_i_u_s_B_e_t_w_e_e_n_F_a_c_t_o_r │ │ │ │ │ -FrobeniusBetweenFactor is a BetweenFactor that evaluates the Frobenius norm of │ │ │ │ │ -the rotation error bet... │ │ │ │ │ -DDeeffiinniittiioonn FrobeniusFactor.h:104 │ │ │ │ │ -_g_t_s_a_m_:_:_F_r_o_b_e_n_i_u_s_B_e_t_w_e_e_n_F_a_c_t_o_r_:_:_F_r_o_b_e_n_i_u_s_B_e_t_w_e_e_n_F_a_c_t_o_r │ │ │ │ │ -FrobeniusBetweenFactor(Key j1, Key j2, const Rot &R12, const SharedNoiseModel │ │ │ │ │ -&model=nullptr) │ │ │ │ │ -Construct from two keys and measured rotation. │ │ │ │ │ -DDeeffiinniittiioonn FrobeniusFactor.h:117 │ │ │ │ │ -_g_t_s_a_m_:_:_F_r_o_b_e_n_i_u_s_B_e_t_w_e_e_n_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s, const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -print with optional string │ │ │ │ │ -DDeeffiinniittiioonn FrobeniusFactor.h:130 │ │ │ │ │ -_g_t_s_a_m_:_:_F_r_o_b_e_n_i_u_s_B_e_t_w_e_e_n_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ -Vector evaluateError(const Rot &R1, const Rot &R2, boost::optional< Matrix & > │ │ │ │ │ -H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override │ │ │ │ │ -Error is Frobenius norm between R1*R12 and R2. │ │ │ │ │ -DDeeffiinniittiioonn FrobeniusFactor.h:152 │ │ │ │ │ -_g_t_s_a_m_:_:_F_r_o_b_e_n_i_u_s_B_e_t_w_e_e_n_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const NonlinearFactor &expected, double tol=1e-9) const override │ │ │ │ │ -assert equality up to a tolerance │ │ │ │ │ -DDeeffiinniittiioonn FrobeniusFactor.h:140 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_g_e_n_e_r_a_c_y_M_o_d_e │ │ │ │ │ +DegeneracyMode │ │ │ │ │ +How to manage degeneracy. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorParams.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e │ │ │ │ │ +LinearizationMode │ │ │ │ │ +SmartFactorParams: parameters and (linearization/degeneracy) modes for │ │ │ │ │ +SmartProjection and SmartStere... │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorParams.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:556 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s_:_:_d_y_n_a_m_i_c_O_u_t_l_i_e_r_R_e_j_e_c_t_i_o_n_T_h_r_e_s_h_o_l_d │ │ │ │ │ +double dynamicOutlierRejectionThreshold │ │ │ │ │ +If this is nonnegative the we will check if the average reprojection error is │ │ │ │ │ +smaller than this thres... │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:573 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s_:_:_r_a_n_k_T_o_l_e_r_a_n_c_e │ │ │ │ │ +double rankTolerance │ │ │ │ │ +threshold to decide whether triangulation is result.degenerate │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:558 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s_:_:_l_a_n_d_m_a_r_k_D_i_s_t_a_n_c_e_T_h_r_e_s_h_o_l_d │ │ │ │ │ +double landmarkDistanceThreshold │ │ │ │ │ +if the landmark is triangulated at distance larger than this, result is flagged │ │ │ │ │ +as degenerate. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:566 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s_:_:_e_n_a_b_l_e_E_P_I │ │ │ │ │ +bool enableEPI │ │ │ │ │ +if set to true, will refine triangulation using LM │ │ │ │ │ +DDeeffiinniittiioonn triangulation.h:560 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorParams.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s_:_:_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y │ │ │ │ │ +bool throwCheirality │ │ │ │ │ +If true, re-throws Cheirality exceptions (default: false) │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorParams.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s_:_:_l_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e │ │ │ │ │ +LinearizationMode linearizationMode │ │ │ │ │ +How to linearize the factor. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorParams.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s_:_:_r_e_t_r_i_a_n_g_u_l_a_t_i_o_n_T_h_r_e_s_h_o_l_d │ │ │ │ │ +double retriangulationThreshold │ │ │ │ │ +threshold to decide whether to re-triangulate │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorParams.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s_:_:_d_e_g_e_n_e_r_a_c_y_M_o_d_e │ │ │ │ │ +DegeneracyMode degeneracyMode │ │ │ │ │ +How to linearize the factor. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorParams.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s_:_:_v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y │ │ │ │ │ +bool verboseCheirality │ │ │ │ │ +If true, prints text for Cheirality exceptions (default: false) │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorParams.h:56 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorParams.h:122 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _F_r_o_b_e_n_i_u_s_F_a_c_t_o_r_._h │ │ │ │ │ + * _S_m_a_r_t_F_a_c_t_o_r_P_a_r_a_m_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01298_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/OrientedPlane3Factor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/JacobianFactorQ.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,113 +98,119 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
OrientedPlane3Factor.h
│ │ │ │ +
JacobianFactorQ.h
│ │ │ │
│ │ │ │
│ │ │ │ -
1/*
│ │ │ │ -
2 * @file OrientedPlane3Factor.cpp
│ │ │ │ -
3 * @brief OrientedPlane3 Factor class
│ │ │ │ -
4 * @author Alex Trevor
│ │ │ │ -
5 * @date December 22, 2013
│ │ │ │ -
6 */
│ │ │ │ -
7
│ │ │ │ -
8#pragma once
│ │ │ │ -
9
│ │ │ │ -
10#include <gtsam/geometry/OrientedPlane3.h>
│ │ │ │ -
11#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
12
│ │ │ │ -
13namespace gtsam {
│ │ │ │ -
14
│ │ │ │ -
│ │ │ │ -
18class GTSAM_EXPORT OrientedPlane3Factor: public NoiseModelFactorN<Pose3, OrientedPlane3> {
│ │ │ │ -
19 protected:
│ │ │ │ -
20 OrientedPlane3 measured_p_;
│ │ │ │ -
21 typedef NoiseModelFactorN<Pose3, OrientedPlane3> Base;
│ │ │ │ -
22
│ │ │ │ -
23 public:
│ │ │ │ -
│ │ │ │ -
25 OrientedPlane3Factor() {
│ │ │ │ -
26 }
│ │ │ │ -
│ │ │ │ -
27 ~OrientedPlane3Factor() override {}
│ │ │ │ +
1/* ----------------------------------------------------------------------------
│ │ │ │ +
2
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
4 * Atlanta, Georgia 30332-0415
│ │ │ │ +
5 * All Rights Reserved
│ │ │ │ +
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
7
│ │ │ │ +
8 * See LICENSE for the license information
│ │ │ │ +
9
│ │ │ │ +
10 * -------------------------------------------------------------------------- */
│ │ │ │ +
11
│ │ │ │ +
12/*
│ │ │ │ +
13 * @file JacobianFactorQ.h
│ │ │ │ +
14 * @date Oct 27, 2013
│ │ │ │ +
15 * @uthor Frank Dellaert
│ │ │ │ +
16 */
│ │ │ │ +
17
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/linear/RegularJacobianFactor.h>
│ │ │ │ +
21
│ │ │ │ +
22namespace gtsam {
│ │ │ │ +
26template<size_t D, size_t ZDim>
│ │ │ │ +
│ │ │ │ +
27class JacobianFactorQ: public RegularJacobianFactor<D> {
│ │ │ │
28
│ │ │ │ +
29 typedef RegularJacobianFactor<D> Base;
│ │ │ │ +
30 typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
│ │ │ │ +
31 typedef std::pair<Key, Matrix> KeyMatrix;
│ │ │ │ +
32
│ │ │ │ +
33public:
│ │ │ │ +
34
│ │ │ │
│ │ │ │ -
36 OrientedPlane3Factor(const Vector4& z, const SharedGaussian& noiseModel,
│ │ │ │ -
37 Key poseKey, Key landmarkKey)
│ │ │ │ -
38 : Base(noiseModel, poseKey, landmarkKey), measured_p_(z) {}
│ │ │ │ -
│ │ │ │ -
39
│ │ │ │ -
41 void print(const std::string& s = "OrientedPlane3Factor",
│ │ │ │ -
42 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
│ │ │ │ -
43
│ │ │ │ -
45 Vector evaluateError(
│ │ │ │ -
46 const Pose3& pose, const OrientedPlane3& plane,
│ │ │ │ -
47 boost::optional<Matrix&> H1 = boost::none,
│ │ │ │ -
48 boost::optional<Matrix&> H2 = boost::none) const override;
│ │ │ │ -
49};
│ │ │ │ -
│ │ │ │ -
50
│ │ │ │ -
51// TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior
│ │ │ │ -
│ │ │ │ -
52class GTSAM_EXPORT OrientedPlane3DirectionPrior : public NoiseModelFactorN<OrientedPlane3> {
│ │ │ │ -
53 protected:
│ │ │ │ -
54 OrientedPlane3 measured_p_;
│ │ │ │ -
55 typedef NoiseModelFactorN<OrientedPlane3> Base;
│ │ │ │ -
56
│ │ │ │ -
57 public:
│ │ │ │ -
58 typedef OrientedPlane3DirectionPrior This;
│ │ │ │ -
│ │ │ │ -
60 OrientedPlane3DirectionPrior() {
│ │ │ │ -
61 }
│ │ │ │ -
│ │ │ │ -
62
│ │ │ │ -
│ │ │ │ -
64 OrientedPlane3DirectionPrior(Key key, const Vector4& z,
│ │ │ │ -
65 const SharedGaussian& noiseModel)
│ │ │ │ -
66 : Base(noiseModel, key), measured_p_(z) {}
│ │ │ │ -
│ │ │ │ -
67
│ │ │ │ -
69 void print(const std::string& s = "OrientedPlane3DirectionPrior",
│ │ │ │ -
70 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
│ │ │ │ -
71
│ │ │ │ -
73 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
│ │ │ │ -
74
│ │ │ │ -
75 Vector evaluateError(const OrientedPlane3& plane,
│ │ │ │ -
76 boost::optional<Matrix&> H = boost::none) const override;
│ │ │ │ -
77};
│ │ │ │ +
36 JacobianFactorQ() {
│ │ │ │ +
37 }
│ │ │ │ +
│ │ │ │ +
38
│ │ │ │ +
│ │ │ │ +
40 JacobianFactorQ(const KeyVector& keys, //
│ │ │ │ +
41 const SharedDiagonal& model = SharedDiagonal()) :
│ │ │ │ +
42 Base() {
│ │ │ │ +
43 Matrix zeroMatrix = Matrix::Zero(0, D);
│ │ │ │ +
44 Vector zeroVector = Vector::Zero(0);
│ │ │ │ +
45 std::vector<KeyMatrix> QF;
│ │ │ │ +
46 QF.reserve(keys.size());
│ │ │ │ +
47 for(const Key& key: keys)
│ │ │ │ +
48 QF.push_back(KeyMatrix(key, zeroMatrix));
│ │ │ │ +
49 JacobianFactor::fillTerms(QF, zeroVector, model);
│ │ │ │ +
50 }
│ │ │ │ +
│ │ │ │ +
51
│ │ │ │ +
│ │ │ │ +
53 JacobianFactorQ(const KeyVector& keys,
│ │ │ │ +
54 const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix3& P,
│ │ │ │ +
55 const Vector& b, const SharedDiagonal& model = SharedDiagonal()) :
│ │ │ │ +
56 Base() {
│ │ │ │ +
57 size_t j = 0, m2 = E.rows(), m = m2 / ZDim;
│ │ │ │ +
58 // Calculate projector Q
│ │ │ │ +
59 Matrix Q = Matrix::Identity(m2,m2) - E * P * E.transpose();
│ │ │ │ +
60 // Calculate pre-computed Jacobian matrices
│ │ │ │ +
61 // TODO: can we do better ?
│ │ │ │ +
62 std::vector<KeyMatrix> QF;
│ │ │ │ +
63 QF.reserve(m);
│ │ │ │ +
64 // Below, we compute each mZDim*D block A_j = Q_j * F_j = (mZDim*ZDim) * (Zdim*D)
│ │ │ │ +
65 for (size_t k = 0; k < FBlocks.size(); ++k) {
│ │ │ │ +
66 Key key = keys[k];
│ │ │ │ +
67 QF.push_back(
│ │ │ │ +
68 KeyMatrix(key, - Q.block(0, ZDim * j++, m2, ZDim) * FBlocks[k]));
│ │ │ │ +
69 }
│ │ │ │ +
70 // Which is then passed to the normal JacobianFactor constructor
│ │ │ │ +
71 JacobianFactor::fillTerms(QF, - Q * b, model);
│ │ │ │ +
72 }
│ │ │ │ +
│ │ │ │ +
73};
│ │ │ │ +
│ │ │ │ +
74// end class JacobianFactorQ
│ │ │ │ +
75
│ │ │ │ +
76// traits
│ │ │ │ +
│ │ │ │ +
77template<size_t D, size_t ZDim> struct traits<JacobianFactorQ<D, ZDim> > : public Testable<
│ │ │ │ +
78 JacobianFactorQ<D, ZDim> > {
│ │ │ │ +
79};
│ │ │ │
│ │ │ │ -
78
│ │ │ │ -
79} // gtsam
│ │ │ │
80
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
81}
│ │ │ │ +
RegularJacobianFactor.h
JacobianFactor class with fixed sized blcoks.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::OrientedPlane3
Represents an infinite plane in 3D, which is composed of a planar normal and its perpendicular distan...
Definition OrientedPlane3.h:36
│ │ │ │ -
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ -
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ -
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ -
gtsam::OrientedPlane3Factor
Factor to measure a planar landmark from a given pose.
Definition OrientedPlane3Factor.h:18
│ │ │ │ -
gtsam::OrientedPlane3Factor::OrientedPlane3Factor
OrientedPlane3Factor(const Vector4 &z, const SharedGaussian &noiseModel, Key poseKey, Key landmarkKey)
Constructor with measured plane (a,b,c,d) coefficients.
Definition OrientedPlane3Factor.h:36
│ │ │ │ -
gtsam::OrientedPlane3Factor::OrientedPlane3Factor
OrientedPlane3Factor()
Constructor.
Definition OrientedPlane3Factor.h:25
│ │ │ │ -
gtsam::OrientedPlane3DirectionPrior
Definition OrientedPlane3Factor.h:52
│ │ │ │ -
gtsam::OrientedPlane3DirectionPrior::OrientedPlane3DirectionPrior
OrientedPlane3DirectionPrior(Key key, const Vector4 &z, const SharedGaussian &noiseModel)
Constructor with measured plane coefficients (a,b,c,d), noise model, landmark symbol.
Definition OrientedPlane3Factor.h:64
│ │ │ │ -
gtsam::OrientedPlane3DirectionPrior::Base
NoiseModelFactorN< OrientedPlane3 > Base
measured plane parameters
Definition OrientedPlane3Factor.h:55
│ │ │ │ -
gtsam::OrientedPlane3DirectionPrior::OrientedPlane3DirectionPrior
OrientedPlane3DirectionPrior()
Constructor.
Definition OrientedPlane3Factor.h:60
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
│ │ │ │ +
gtsam::JacobianFactor::fillTerms
void fillTerms(const TERMS &terms, const Vector &b, const SharedDiagonal &noiseModel)
Internal function to fill blocks and set dimensions.
Definition JacobianFactor-inl.h:60
│ │ │ │ +
gtsam::RegularJacobianFactor
JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator.
Definition RegularJacobianFactor.h:32
│ │ │ │ +
gtsam::JacobianFactorQ
JacobianFactor for Schur complement that uses Q noise model.
Definition JacobianFactorQ.h:27
│ │ │ │ +
gtsam::JacobianFactorQ::JacobianFactorQ
JacobianFactorQ()
Default constructor.
Definition JacobianFactorQ.h:36
│ │ │ │ +
gtsam::JacobianFactorQ::JacobianFactorQ
JacobianFactorQ(const KeyVector &keys, const std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > &FBlocks, const Matrix &E, const Matrix3 &P, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Constructor.
Definition JacobianFactorQ.h:53
│ │ │ │ +
gtsam::JacobianFactorQ::JacobianFactorQ
JacobianFactorQ(const KeyVector &keys, const SharedDiagonal &model=SharedDiagonal())
Empty constructor with keys.
Definition JacobianFactorQ.h:40
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,139 +1,141 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -OrientedPlane3Factor.h │ │ │ │ │ -1/* │ │ │ │ │ -2 * @file OrientedPlane3Factor.cpp │ │ │ │ │ -3 * @brief OrientedPlane3 Factor class │ │ │ │ │ -4 * @author Alex Trevor │ │ │ │ │ -5 * @date December 22, 2013 │ │ │ │ │ -6 */ │ │ │ │ │ +JacobianFactorQ.h │ │ │ │ │ +1/* --------------------------------------------------------------------------- │ │ │ │ │ +- │ │ │ │ │ +2 │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +5 * All Rights Reserved │ │ │ │ │ +6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ -8#pragma once │ │ │ │ │ +8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ -10#include │ │ │ │ │ -11#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -12 │ │ │ │ │ -13namespace _g_t_s_a_m { │ │ │ │ │ -14 │ │ │ │ │ -_1_8class GTSAM_EXPORT _O_r_i_e_n_t_e_d_P_l_a_n_e_3_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -19 protected: │ │ │ │ │ -20 _O_r_i_e_n_t_e_d_P_l_a_n_e_3 measured_p_; │ │ │ │ │ -21 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_P_o_s_e_3_,_ _O_r_i_e_n_t_e_d_P_l_a_n_e_3_> _B_a_s_e; │ │ │ │ │ -22 │ │ │ │ │ -23 public: │ │ │ │ │ -_2_5 _O_r_i_e_n_t_e_d_P_l_a_n_e_3_F_a_c_t_o_r() { │ │ │ │ │ -26 } │ │ │ │ │ -27 _~_O_r_i_e_n_t_e_d_P_l_a_n_e_3_F_a_c_t_o_r() override {} │ │ │ │ │ +10 * ------------------------------------------------------------------------- │ │ │ │ │ +- */ │ │ │ │ │ +11 │ │ │ │ │ +12/* │ │ │ │ │ +13 * @file JacobianFactorQ.h │ │ │ │ │ +14 * @date Oct 27, 2013 │ │ │ │ │ +15 * @uthor Frank Dellaert │ │ │ │ │ +16 */ │ │ │ │ │ +17 │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ +21 │ │ │ │ │ +22namespace _g_t_s_a_m { │ │ │ │ │ +26template │ │ │ │ │ +_2_7class _J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q: public _R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r { │ │ │ │ │ 28 │ │ │ │ │ -_3_6 _O_r_i_e_n_t_e_d_P_l_a_n_e_3_F_a_c_t_o_r(const Vector4& z, const SharedGaussian& noiseModel, │ │ │ │ │ -37 _K_e_y poseKey, _K_e_y landmarkKey) │ │ │ │ │ -38 : _B_a_s_e(noiseModel, poseKey, landmarkKey), measured_p_(z) {} │ │ │ │ │ -39 │ │ │ │ │ -41 void _p_r_i_n_t(const std::string& s = "OrientedPlane3Factor", │ │ │ │ │ -42 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override; │ │ │ │ │ -43 │ │ │ │ │ -45 Vector evaluateError( │ │ │ │ │ -46 const _P_o_s_e_3& pose, const _O_r_i_e_n_t_e_d_P_l_a_n_e_3& plane, │ │ │ │ │ -47 boost::optional H1 = boost::none, │ │ │ │ │ -48 boost::optional H2 = boost::none) const override; │ │ │ │ │ -49}; │ │ │ │ │ -50 │ │ │ │ │ -51// TODO: Convert this factor to dimension two, three dimensions is redundant │ │ │ │ │ -for direction prior │ │ │ │ │ -_5_2class GTSAM_EXPORT _O_r_i_e_n_t_e_d_P_l_a_n_e_3_D_i_r_e_c_t_i_o_n_P_r_i_o_r : public │ │ │ │ │ -_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -53 protected: │ │ │ │ │ -54 _O_r_i_e_n_t_e_d_P_l_a_n_e_3 measured_p_; │ │ │ │ │ -_5_5 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_O_r_i_e_n_t_e_d_P_l_a_n_e_3_> _B_a_s_e; │ │ │ │ │ -56 │ │ │ │ │ -57 public: │ │ │ │ │ -58 typedef _O_r_i_e_n_t_e_d_P_l_a_n_e_3_D_i_r_e_c_t_i_o_n_P_r_i_o_r _T_h_i_s; │ │ │ │ │ -_6_0 _O_r_i_e_n_t_e_d_P_l_a_n_e_3_D_i_r_e_c_t_i_o_n_P_r_i_o_r() { │ │ │ │ │ -61 } │ │ │ │ │ -62 │ │ │ │ │ -_6_4 _O_r_i_e_n_t_e_d_P_l_a_n_e_3_D_i_r_e_c_t_i_o_n_P_r_i_o_r(_K_e_y key, const Vector4& z, │ │ │ │ │ -65 const SharedGaussian& noiseModel) │ │ │ │ │ -66 : _B_a_s_e(noiseModel, key), measured_p_(z) {} │ │ │ │ │ -67 │ │ │ │ │ -69 void _p_r_i_n_t(const std::string& s = "OrientedPlane3DirectionPrior", │ │ │ │ │ -70 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override; │ │ │ │ │ -71 │ │ │ │ │ -73 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& expected, double tol = 1e-9) const │ │ │ │ │ -override; │ │ │ │ │ -74 │ │ │ │ │ -75 Vector evaluateError(const _O_r_i_e_n_t_e_d_P_l_a_n_e_3& plane, │ │ │ │ │ -76 boost::optional H = boost::none) const override; │ │ │ │ │ -77}; │ │ │ │ │ -78 │ │ │ │ │ -79} // gtsam │ │ │ │ │ +29 typedef _R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_<_D_> Base; │ │ │ │ │ +30 typedef Eigen::Matrix MatrixZD; │ │ │ │ │ +31 typedef std::pair KeyMatrix; │ │ │ │ │ +32 │ │ │ │ │ +33public: │ │ │ │ │ +34 │ │ │ │ │ +_3_6 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q() { │ │ │ │ │ +37 } │ │ │ │ │ +38 │ │ │ │ │ +_4_0 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q(const _K_e_y_V_e_c_t_o_r& _k_e_y_s, // │ │ │ │ │ +41 const SharedDiagonal& model = SharedDiagonal()) : │ │ │ │ │ +42 Base() { │ │ │ │ │ +43 Matrix zeroMatrix = Matrix::Zero(0, D); │ │ │ │ │ +44 Vector zeroVector = Vector::Zero(0); │ │ │ │ │ +45 std::vector QF; │ │ │ │ │ +46 QF.reserve(_k_e_y_s.size()); │ │ │ │ │ +47 for(const _K_e_y& key: _k_e_y_s) │ │ │ │ │ +48 QF.push_back(KeyMatrix(key, zeroMatrix)); │ │ │ │ │ +49 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_f_i_l_l_T_e_r_m_s(QF, zeroVector, model); │ │ │ │ │ +50 } │ │ │ │ │ +51 │ │ │ │ │ +_5_3 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q(const _K_e_y_V_e_c_t_o_r& _k_e_y_s, │ │ │ │ │ +54 const std::vector >& FBlocks, │ │ │ │ │ +const Matrix& E, const Matrix3& P, │ │ │ │ │ +55 const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : │ │ │ │ │ +56 Base() { │ │ │ │ │ +57 size_t j = 0, m2 = E.rows(), m = m2 / ZDim; │ │ │ │ │ +58 // Calculate projector Q │ │ │ │ │ +59 Matrix Q = Matrix::Identity(m2,m2) - E * P * E.transpose(); │ │ │ │ │ +60 // Calculate pre-computed Jacobian matrices │ │ │ │ │ +61 // TODO: can we do better ? │ │ │ │ │ +62 std::vector QF; │ │ │ │ │ +63 QF.reserve(m); │ │ │ │ │ +64 // Below, we compute each mZDim*D block A_j = Q_j * F_j = (mZDim*ZDim) * │ │ │ │ │ +(Zdim*D) │ │ │ │ │ +65 for (size_t k = 0; k < FBlocks.size(); ++k) { │ │ │ │ │ +66 _K_e_y key = _k_e_y_s[k]; │ │ │ │ │ +67 QF.push_back( │ │ │ │ │ +68 KeyMatrix(key, - Q.block(0, ZDim * j++, m2, ZDim) * FBlocks[k])); │ │ │ │ │ +69 } │ │ │ │ │ +70 // Which is then passed to the normal JacobianFactor constructor │ │ │ │ │ +71 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_f_i_l_l_T_e_r_m_s(QF, - Q * b, model); │ │ │ │ │ +72 } │ │ │ │ │ +73}; │ │ │ │ │ +74// end class JacobianFactorQ │ │ │ │ │ +75 │ │ │ │ │ +76// traits │ │ │ │ │ +_7_7template struct _t_r_a_i_t_s<_J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q > : │ │ │ │ │ +public _T_e_s_t_a_b_l_e< │ │ │ │ │ +78 JacobianFactorQ > { │ │ │ │ │ +79}; │ │ │ │ │ 80 │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ -Non-linear factor base classes. │ │ │ │ │ +81} │ │ │ │ │ +_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ +JacobianFactor class with fixed sized blcoks. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3 │ │ │ │ │ -Represents an infinite plane in 3D, which is composed of a planar normal and │ │ │ │ │ -its perpendicular distan... │ │ │ │ │ -DDeeffiinniittiioonn OrientedPlane3.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ -A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ -Nonlinear factor base class. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ -A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ -variables. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_F_a_c_t_o_r │ │ │ │ │ -Factor to measure a planar landmark from a given pose. │ │ │ │ │ -DDeeffiinniittiioonn OrientedPlane3Factor.h:18 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_F_a_c_t_o_r_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_F_a_c_t_o_r │ │ │ │ │ -OrientedPlane3Factor(const Vector4 &z, const SharedGaussian &noiseModel, Key │ │ │ │ │ -poseKey, Key landmarkKey) │ │ │ │ │ -Constructor with measured plane (a,b,c,d) coefficients. │ │ │ │ │ -DDeeffiinniittiioonn OrientedPlane3Factor.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_F_a_c_t_o_r_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_F_a_c_t_o_r │ │ │ │ │ -OrientedPlane3Factor() │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn OrientedPlane3Factor.h:25 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_D_i_r_e_c_t_i_o_n_P_r_i_o_r │ │ │ │ │ -DDeeffiinniittiioonn OrientedPlane3Factor.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_D_i_r_e_c_t_i_o_n_P_r_i_o_r_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_D_i_r_e_c_t_i_o_n_P_r_i_o_r │ │ │ │ │ -OrientedPlane3DirectionPrior(Key key, const Vector4 &z, const SharedGaussian │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s │ │ │ │ │ +const KeyVector & keys() const │ │ │ │ │ +Access the factor's involved variable keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:140 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_f_i_l_l_T_e_r_m_s │ │ │ │ │ +void fillTerms(const TERMS &terms, const Vector &b, const SharedDiagonal │ │ │ │ │ &noiseModel) │ │ │ │ │ -Constructor with measured plane coefficients (a,b,c,d), noise model, landmark │ │ │ │ │ -symbol. │ │ │ │ │ -DDeeffiinniittiioonn OrientedPlane3Factor.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_D_i_r_e_c_t_i_o_n_P_r_i_o_r_:_:_B_a_s_e │ │ │ │ │ -NoiseModelFactorN< OrientedPlane3 > Base │ │ │ │ │ -measured plane parameters │ │ │ │ │ -DDeeffiinniittiioonn OrientedPlane3Factor.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_D_i_r_e_c_t_i_o_n_P_r_i_o_r_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_D_i_r_e_c_t_i_o_n_P_r_i_o_r │ │ │ │ │ -OrientedPlane3DirectionPrior() │ │ │ │ │ +Internal function to fill blocks and set dimensions. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor-inl.h:60 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ +JacobianFactor with constant sized blocks Provides raw memory access versions │ │ │ │ │ +of linear operator. │ │ │ │ │ +DDeeffiinniittiioonn RegularJacobianFactor.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q │ │ │ │ │ +JacobianFactor for Schur complement that uses Q noise model. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactorQ.h:27 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q │ │ │ │ │ +JacobianFactorQ() │ │ │ │ │ +Default constructor. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactorQ.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q │ │ │ │ │ +JacobianFactorQ(const KeyVector &keys, const std::vector< MatrixZD, Eigen:: │ │ │ │ │ +aligned_allocator< MatrixZD > > &FBlocks, const Matrix &E, const Matrix3 &P, │ │ │ │ │ +const Vector &b, const SharedDiagonal &model=SharedDiagonal()) │ │ │ │ │ Constructor. │ │ │ │ │ -DDeeffiinniittiioonn OrientedPlane3Factor.h:60 │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactorQ.h:53 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q │ │ │ │ │ +JacobianFactorQ(const KeyVector &keys, const SharedDiagonal │ │ │ │ │ +&model=SharedDiagonal()) │ │ │ │ │ +Empty constructor with keys. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactorQ.h:40 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * OOrriieenntteeddPPllaannee33FFaaccttoorr..hh │ │ │ │ │ + * JJaaccoobbiiaannFFaaccttoorrQQ..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01301.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/dataset.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/FrobeniusFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,153 +94,58 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Enumerations | │ │ │ │ Functions
│ │ │ │ -
dataset.h File Reference
│ │ │ │ +
FrobeniusFactor.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

utility functions for loading datasets │ │ │ │ +

Various factors that minimize some Frobenius norm. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::FrobeniusPrior< Rot >
 FrobeniusPrior calculates the Frobenius norm between a given matrix and an element of SO(3) or SO(4). More...
 
class  gtsam::FrobeniusFactor< Rot >
 FrobeniusFactor calculates the Frobenius norm between rotation matrices. More...
 
class  gtsam::FrobeniusBetweenFactor< Rot >
 FrobeniusBetweenFactor is a BetweenFactor that evaluates the Frobenius norm of the rotation error between measured and predicted (rather than the Logmap of the error). More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Typedefs

│ │ │ │ -typedef std::pair< size_t, Pose2 > gtsam::IndexedPose
 Return type for auxiliary functions.
 
│ │ │ │ -typedef std::pair< size_t, Point2 > gtsam::IndexedLandmark
 
│ │ │ │ -typedef std::pair< std::pair< size_t, size_t >, Pose2 > gtsam::IndexedEdge
 
using gtsam::GraphAndValues = std::pair< NonlinearFactorGraph::shared_ptr, Values::shared_ptr >
 Return type for load functions, which return a graph and initial values.
 
│ │ │ │ -using gtsam::BetweenFactorPose2s = std::vector< BetweenFactor< Pose2 >::shared_ptr >
 
│ │ │ │ -using gtsam::BetweenFactorPose3s = std::vector< BetweenFactor< Pose3 >::shared_ptr >
 
│ │ │ │ -using gtsam::BinaryMeasurementsUnit3 = std::vector< BinaryMeasurement< Unit3 > >
 
│ │ │ │ -using gtsam::BinaryMeasurementsPoint3 = std::vector< BinaryMeasurement< Point3 > >
 
│ │ │ │ -using gtsam::BinaryMeasurementsRot3 = std::vector< BinaryMeasurement< Rot3 > >
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Enumerations

enum  gtsam::NoiseFormat {
│ │ │ │ -  gtsam::NoiseFormatG2O │ │ │ │ -, gtsam::NoiseFormatTORO │ │ │ │ -, gtsam::NoiseFormatGRAPH │ │ │ │ -, gtsam::NoiseFormatCOV │ │ │ │ -,
│ │ │ │ -  gtsam::NoiseFormatAUTO │ │ │ │ -
│ │ │ │ - }
 Indicates how noise parameters are stored in file. More...
 
enum  gtsam::KernelFunctionType { KernelFunctionTypeNONE │ │ │ │ -, KernelFunctionTypeHUBER │ │ │ │ -, KernelFunctionTypeTUKEY │ │ │ │ - }
 Robust kernel type to wrap around quadratic noise model.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

string gtsam::findExampleDataFile (const std::string &name)
 Find the full path to an example dataset distributed with gtsam.
 
│ │ │ │ -string gtsam::createRewrittenFileName (const std::string &name)
 Creates a temporary file name that needs to be ignored in .gitingnore for checking read-write oprations.
 
template<typename T >
GTSAM_EXPORT std::map< size_t, T > gtsam::parseVariables (const std::string &filename, size_t maxIndex=0)
 Parse variables in a line-based text format (like g2o) into a map.
 
template<typename T >
GTSAM_EXPORT std::vector< BinaryMeasurement< T > > gtsam::parseMeasurements (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model=nullptr, size_t maxIndex=0)
 Parse binary measurements in a line-based text format (like g2o) into a vector.
 
template<typename T >
GTSAM_EXPORT std::vector< typename BetweenFactor< T >::shared_ptr > gtsam::parseFactors (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model=nullptr, size_t maxIndex=0)
 Parse BetweenFactors in a line-based text format (like g2o) into a vector of shared pointers.
 
boost::optional< IndexedPose > gtsam::parseVertexPose (std::istream &is, const std::string &tag)
 Parse TORO/G2O vertex "id x y yaw".
 
boost::optional< IndexedLandmark > gtsam::parseVertexLandmark (std::istream &is, const std::string &tag)
 Parse G2O landmark vertex "id x y".
 
boost::optional< IndexedEdge > gtsam::parseEdge (std::istream &is, const std::string &tag)
 Parse TORO/G2O edge "id1 id2 x y yaw".
 
GTSAM_EXPORT GraphAndValues gtsam::load2D (std::pair< std::string, SharedNoiseModel > dataset, size_t maxIndex=0, bool addNoise=false, bool smart=true, NoiseFormat noiseFormat=NoiseFormatAUTO, KernelFunctionType kernelFunctionType=KernelFunctionTypeNONE)
 Load TORO 2D Graph.
 
GraphAndValues gtsam::load2D (const std::string &filename, SharedNoiseModel model=SharedNoiseModel(), size_t maxIndex=0, bool addNoise=false, bool smart=true, NoiseFormat noiseFormat=NoiseFormatAUTO, KernelFunctionType kernelFunctionType=KernelFunctionTypeNONE)
 Load TORO/G2O style graph files.
 
│ │ │ │ -void gtsam::save2D (const NonlinearFactorGraph &graph, const Values &config, const noiseModel::Diagonal::shared_ptr model, const std::string &filename)
 save 2d graph
 
GraphAndValues gtsam::readG2o (const std::string &g2oFile, const bool is3D=false, KernelFunctionType kernelFunctionType=KernelFunctionTypeNONE)
 This function parses a g2o file and stores the measurements into a NonlinearFactorGraph and the initial guess in a Values structure.
 
void gtsam::writeG2o (const NonlinearFactorGraph &graph, const Values &estimate, const std::string &filename)
 This function writes a g2o file from NonlinearFactorGraph and a Values structure.
 
│ │ │ │ -GraphAndValues gtsam::load3D (const std::string &filename)
 Load TORO 3D Graph.
 
│ │ │ │ -BetweenFactorPose2s gtsam::parse2DFactors (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
 
│ │ │ │ -BetweenFactorPose3s gtsam::parse3DFactors (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
 
SharedNoiseModel gtsam::ConvertNoiseModel (const SharedNoiseModel &model, size_t n, bool defaultToUnit=true)
 When creating (any) FrobeniusFactor we can convert a Rot/Pose BetweenFactor noise model into a n-dimensional isotropic noise model used to weight the Frobenius norm.
 
│ │ │ │

Detailed Description

│ │ │ │ -

utility functions for loading datasets

│ │ │ │ -
Date
Jan 22, 2010
│ │ │ │ -
Author
Ni Kai
│ │ │ │ -
│ │ │ │ -Luca Carlone
│ │ │ │ -
│ │ │ │ -Varun Agrawal
│ │ │ │ +

Various factors that minimize some Frobenius norm.

│ │ │ │ +
Date
March 2019
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,164 +1,44 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _E_n_u_m_e_r_a_t_i_o_n_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -dataset.h File Reference │ │ │ │ │ -utility functions for loading datasets _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +FrobeniusFactor.h File Reference │ │ │ │ │ +Various factors that minimize some Frobenius norm. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_F_r_o_b_e_n_i_u_s_P_r_i_o_r_<_ _R_o_t_ _> │ │ │ │ │ +  _F_r_o_b_e_n_i_u_s_P_r_i_o_r calculates the Frobenius norm between a given matrix and │ │ │ │ │ + an element of SO(3) or SO(4). _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_F_r_o_b_e_n_i_u_s_F_a_c_t_o_r_<_ _R_o_t_ _> │ │ │ │ │ +  _F_r_o_b_e_n_i_u_s_F_a_c_t_o_r calculates the Frobenius norm between rotation │ │ │ │ │ + matrices. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +class   _g_t_s_a_m_:_:_F_r_o_b_e_n_i_u_s_B_e_t_w_e_e_n_F_a_c_t_o_r_<_ _R_o_t_ _> │ │ │ │ │ + _F_r_o_b_e_n_i_u_s_B_e_t_w_e_e_n_F_a_c_t_o_r is a _B_e_t_w_e_e_n_F_a_c_t_o_r that evaluates the Frobenius │ │ │ │ │ +  norm of the rotation error between measured and predicted (rather than │ │ │ │ │ + the Logmap of the error). _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ - typedef std::pair< size_t, _P_o_s_e_2 >  ggttssaamm::::IInnddeexxeeddPPoossee │ │ │ │ │ -  Return type for auxiliary functions. │ │ │ │ │ -  │ │ │ │ │ - typedef std::pair< size_t, _P_o_i_n_t_2 >  ggttssaamm::::IInnddeexxeeddLLaannddmmaarrkk │ │ │ │ │ -  │ │ │ │ │ -typedef std::pair< std::pair< size_t, │ │ │ │ │ - size_t >, _P_o_s_e_2 >  ggttssaamm::::IInnddeexxeeddEEddggee │ │ │ │ │ -  │ │ │ │ │ - using  _g_t_s_a_m_:_:_G_r_a_p_h_A_n_d_V_a_l_u_e_s = std::pair< │ │ │ │ │ - NonlinearFactorGraph::shared_ptr, │ │ │ │ │ - _V_a_l_u_e_s_:_:_s_h_a_r_e_d___p_t_r > │ │ │ │ │ -  Return type for load functions, which │ │ │ │ │ - return a graph and initial values. │ │ │ │ │ -  │ │ │ │ │ - using  ggttssaamm::::BBeettwweeeennFFaaccttoorrPPoossee22ss = std::vector< │ │ │ │ │ - _B_e_t_w_e_e_n_F_a_c_t_o_r< _P_o_s_e_2 >::shared_ptr > │ │ │ │ │ -  │ │ │ │ │ - using  ggttssaamm::::BBeettwweeeennFFaaccttoorrPPoossee33ss = std::vector< │ │ │ │ │ - _B_e_t_w_e_e_n_F_a_c_t_o_r< _P_o_s_e_3 >::shared_ptr > │ │ │ │ │ -  │ │ │ │ │ - using  ggttssaamm::::BBiinnaarryyMMeeaassuurreemmeennttssUUnniitt33 = std:: │ │ │ │ │ - vector< _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t< _U_n_i_t_3 > > │ │ │ │ │ -  │ │ │ │ │ - using  ggttssaamm::::BBiinnaarryyMMeeaassuurreemmeennttssPPooiinntt33 = std:: │ │ │ │ │ - vector< _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t< _P_o_i_n_t_3 > > │ │ │ │ │ -  │ │ │ │ │ - using  ggttssaamm::::BBiinnaarryyMMeeaassuurreemmeennttssRRoott33 = std:: │ │ │ │ │ - vector< _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t< _R_o_t_3 > > │ │ │ │ │ -  │ │ │ │ │ -EEnnuummeerraattiioonnss │ │ │ │ │ -enum   _g_t_s_a_m_:_:_N_o_i_s_e_F_o_r_m_a_t { │ │ │ │ │ -   _g_t_s_a_m_:_:_N_o_i_s_e_F_o_r_m_a_t_G_2_O , _g_t_s_a_m_:_:_N_o_i_s_e_F_o_r_m_a_t_T_O_R_O , _g_t_s_a_m_:_: │ │ │ │ │ - _N_o_i_s_e_F_o_r_m_a_t_G_R_A_P_H , _g_t_s_a_m_:_:_N_o_i_s_e_F_o_r_m_a_t_C_O_V , │ │ │ │ │ -   _g_t_s_a_m_:_:_N_o_i_s_e_F_o_r_m_a_t_A_U_T_O │ │ │ │ │ - } │ │ │ │ │ -  Indicates how noise parameters are stored in file. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -enum   _g_t_s_a_m_:_:_K_e_r_n_e_l_F_u_n_c_t_i_o_n_T_y_p_e { KKeerrnneellFFuunnccttiioonnTTyyppeeNNOONNEE , │ │ │ │ │ - KKeerrnneellFFuunnccttiioonnTTyyppeeHHUUBBEERR , KKeerrnneellFFuunnccttiioonnTTyyppeeTTUUKKEEYY } │ │ │ │ │ -  Robust kernel type to wrap around quadratic noise model. │ │ │ │ │ -  │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ - string  _g_t_s_a_m_:_:_f_i_n_d_E_x_a_m_p_l_e_D_a_t_a_F_i_l_e (const std:: │ │ │ │ │ - string &name) │ │ │ │ │ -  Find the full path to an example dataset │ │ │ │ │ - distributed with gtsam. │ │ │ │ │ -  │ │ │ │ │ - string  ggttssaamm::::ccrreeaatteeRReewwrriitttteennFFiilleeNNaammee (const std:: │ │ │ │ │ - string &name) │ │ │ │ │ - Creates a temporary file name that needs to │ │ │ │ │ -  be ignored in .gitingnore for checking │ │ │ │ │ - read-write oprations. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -GTSAM_EXPORT std::map< size_t, T >  _g_t_s_a_m_:_:_p_a_r_s_e_V_a_r_i_a_b_l_e_s (const std::string │ │ │ │ │ - &filename, size_t maxIndex=0) │ │ │ │ │ -  Parse variables in a line-based text format │ │ │ │ │ - (like g2o) into a map. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - GTSAM_EXPORT std::vector< _g_t_s_a_m_:_:_p_a_r_s_e_M_e_a_s_u_r_e_m_e_n_t_s (const std::string │ │ │ │ │ - _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t< T > >  &filename, const noiseModel::Diagonal:: │ │ │ │ │ - shared_ptr &model=nullptr, size_t │ │ │ │ │ - maxIndex=0) │ │ │ │ │ -  Parse binary measurements in a line-based │ │ │ │ │ - text format (like g2o) into a vector. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - GTSAM_EXPORT std::vector< typename _g_t_s_a_m_:_:_p_a_r_s_e_F_a_c_t_o_r_s (const std::string │ │ │ │ │ - _B_e_t_w_e_e_n_F_a_c_t_o_r< T >::shared_ptr >  &filename, const noiseModel::Diagonal:: │ │ │ │ │ - shared_ptr &model=nullptr, size_t │ │ │ │ │ - maxIndex=0) │ │ │ │ │ - Parse BetweenFactors in a line-based text │ │ │ │ │ -  format (like g2o) into a vector of shared │ │ │ │ │ - pointers. │ │ │ │ │ -  │ │ │ │ │ - boost::optional< _I_n_d_e_x_e_d_P_o_s_e >  _g_t_s_a_m_:_:_p_a_r_s_e_V_e_r_t_e_x_P_o_s_e (std::istream &is, │ │ │ │ │ - const std::string &tag) │ │ │ │ │ -  Parse TORO/G2O vertex "id x y yaw". │ │ │ │ │ -  │ │ │ │ │ -boost::optional< IndexedLandmark >  _g_t_s_a_m_:_:_p_a_r_s_e_V_e_r_t_e_x_L_a_n_d_m_a_r_k (std::istream │ │ │ │ │ - &is, const std::string &tag) │ │ │ │ │ -  Parse G2O landmark vertex "id x y". │ │ │ │ │ -  │ │ │ │ │ - boost::optional< IndexedEdge >  _g_t_s_a_m_:_:_p_a_r_s_e_E_d_g_e (std::istream &is, const │ │ │ │ │ - std::string &tag) │ │ │ │ │ -  Parse TORO/G2O edge "id1 id2 x y yaw". │ │ │ │ │ -  │ │ │ │ │ - GTSAM_EXPORT _G_r_a_p_h_A_n_d_V_a_l_u_e_s  _g_t_s_a_m_:_:_l_o_a_d_2_D (std::pair< std::string, │ │ │ │ │ - _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l > dataset, size_t │ │ │ │ │ - maxIndex=0, bool addNoise=false, bool │ │ │ │ │ - smart=true, _N_o_i_s_e_F_o_r_m_a_t │ │ │ │ │ - noiseFormat=_N_o_i_s_e_F_o_r_m_a_t_A_U_T_O, │ │ │ │ │ - _K_e_r_n_e_l_F_u_n_c_t_i_o_n_T_y_p_e │ │ │ │ │ - kernelFunctionType=KernelFunctionTypeNONE) │ │ │ │ │ -  Load TORO 2D Graph. │ │ │ │ │ -  │ │ │ │ │ - _G_r_a_p_h_A_n_d_V_a_l_u_e_s  _g_t_s_a_m_:_:_l_o_a_d_2_D (const std::string &filename, │ │ │ │ │ - _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l model=_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l(), │ │ │ │ │ - size_t maxIndex=0, bool addNoise=false, │ │ │ │ │ - bool smart=true, _N_o_i_s_e_F_o_r_m_a_t │ │ │ │ │ - noiseFormat=_N_o_i_s_e_F_o_r_m_a_t_A_U_T_O, │ │ │ │ │ - _K_e_r_n_e_l_F_u_n_c_t_i_o_n_T_y_p_e │ │ │ │ │ - kernelFunctionType=KernelFunctionTypeNONE) │ │ │ │ │ -  Load TORO/G2O style graph files. │ │ │ │ │ -  │ │ │ │ │ - void  ggttssaamm::::ssaavvee22DD (const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ - &graph, const _V_a_l_u_e_s &config, const │ │ │ │ │ - noiseModel::Diagonal::shared_ptr model, │ │ │ │ │ - const std::string &filename) │ │ │ │ │ -  save 2d graph │ │ │ │ │ -  │ │ │ │ │ - _G_r_a_p_h_A_n_d_V_a_l_u_e_s  _g_t_s_a_m_:_:_r_e_a_d_G_2_o (const std::string &g2oFile, │ │ │ │ │ - const bool is3D=false, _K_e_r_n_e_l_F_u_n_c_t_i_o_n_T_y_p_e │ │ │ │ │ - kernelFunctionType=KernelFunctionTypeNONE) │ │ │ │ │ - This function parses a g2o file and stores │ │ │ │ │ -  the measurements into a │ │ │ │ │ - _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h and the initial guess │ │ │ │ │ - in a _V_a_l_u_e_s structure. │ │ │ │ │ -  │ │ │ │ │ - void  _g_t_s_a_m_:_:_w_r_i_t_e_G_2_o (const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ - &graph, const _V_a_l_u_e_s &estimate, const std:: │ │ │ │ │ - string &filename) │ │ │ │ │ - This function writes a g2o file from │ │ │ │ │ -  _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h and a _V_a_l_u_e_s │ │ │ │ │ - structure. │ │ │ │ │ -  │ │ │ │ │ - _G_r_a_p_h_A_n_d_V_a_l_u_e_s  ggttssaamm::::llooaadd33DD (const std::string &filename) │ │ │ │ │ -  Load TORO 3D Graph. │ │ │ │ │ -  │ │ │ │ │ - BetweenFactorPose2s  ggttssaamm::::ppaarrssee22DDFFaaccttoorrss (const std::string │ │ │ │ │ - &filename, const noiseModel::Diagonal:: │ │ │ │ │ - shared_ptr &model, size_t maxIndex) │ │ │ │ │ -  │ │ │ │ │ - BetweenFactorPose3s  ggttssaamm::::ppaarrssee33DDFFaaccttoorrss (const std::string │ │ │ │ │ - &filename, const noiseModel::Diagonal:: │ │ │ │ │ - shared_ptr &model, size_t maxIndex) │ │ │ │ │ +_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l  _g_t_s_a_m_:_:_C_o_n_v_e_r_t_N_o_i_s_e_M_o_d_e_l (const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, │ │ │ │ │ + size_t n, bool defaultToUnit=true) │ │ │ │ │ + When creating (any) _F_r_o_b_e_n_i_u_s_F_a_c_t_o_r we can convert a Rot/Pose │ │ │ │ │ +  _B_e_t_w_e_e_n_F_a_c_t_o_r noise model into a n-dimensional isotropic │ │ │ │ │ + noise model used to weight the Frobenius norm. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -utility functions for loading datasets │ │ │ │ │ +Various factors that minimize some Frobenius norm. │ │ │ │ │ Date │ │ │ │ │ - Jan 22, 2010 │ │ │ │ │ + March 2019 │ │ │ │ │ Author │ │ │ │ │ - Ni Kai │ │ │ │ │ - Luca Carlone │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _d_a_t_a_s_e_t_._h │ │ │ │ │ + * _F_r_o_b_e_n_i_u_s_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01301.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,30 +1,6 @@ │ │ │ │ │ var a01301 = [ │ │ │ │ │ - ["GraphAndValues", "a01301.html#abcc5d1ec05ddc954d5de28929acabbdf", null], │ │ │ │ │ - ["IndexedPose", "a01301.html#a6ab764d13f9aaeafc2dc60763b05ec0e", null], │ │ │ │ │ - ["KernelFunctionType", "a01301.html#a8be373f34edc0a9d28b1bfab5dd62ba0", [ │ │ │ │ │ - ["KernelFunctionTypeNONE", "a01301.html#a8be373f34edc0a9d28b1bfab5dd62ba0a406af886b3b4b113dfe666c8ad95e38b", null], │ │ │ │ │ - ["KernelFunctionTypeHUBER", "a01301.html#a8be373f34edc0a9d28b1bfab5dd62ba0a37cef4c071c8c4f77f31fa617e91da53", null], │ │ │ │ │ - ["KernelFunctionTypeTUKEY", "a01301.html#a8be373f34edc0a9d28b1bfab5dd62ba0aacc5d659fca66977c8fe6f341802fae9", null] │ │ │ │ │ - ]], │ │ │ │ │ - ["NoiseFormat", "a01301.html#a90552b70a2da9c74595cae4b05c2ce18", [ │ │ │ │ │ - ["NoiseFormatG2O", "a01301.html#a90552b70a2da9c74595cae4b05c2ce18ad0aaa5c51a45cb7c9151aae60a603209", null], │ │ │ │ │ - ["NoiseFormatTORO", "a01301.html#a90552b70a2da9c74595cae4b05c2ce18a3790bc2c6f11cd7da84730b33173de5a", null], │ │ │ │ │ - ["NoiseFormatGRAPH", "a01301.html#a90552b70a2da9c74595cae4b05c2ce18a1f8edeb25c58a249789556c686265a4c", null], │ │ │ │ │ - ["NoiseFormatCOV", "a01301.html#a90552b70a2da9c74595cae4b05c2ce18a5450a16f3522214ccdfa23c461e7d05a", null], │ │ │ │ │ - ["NoiseFormatAUTO", "a01301.html#a90552b70a2da9c74595cae4b05c2ce18a2f9067b8f5bd2d0dd38c49326a4c070e", null] │ │ │ │ │ - ]], │ │ │ │ │ - ["createRewrittenFileName", "a01301.html#adbe09e573771fd72b08069353d537725", null], │ │ │ │ │ - ["findExampleDataFile", "a01301.html#aef971b2c69845e4fcce707306703c7b8", null], │ │ │ │ │ - ["load2D", "a01301.html#a635ec096b94adc2b05f081cd07eb2c0f", null], │ │ │ │ │ - ["load2D", "a01301.html#a6f8d49b13ccc0e007a5e7572286d5854", null], │ │ │ │ │ - ["load3D", "a01301.html#ac96511703a91071d7b2033976f9739c3", null], │ │ │ │ │ - ["parseEdge", "a01301.html#abc8bb9db0893652e8b5e7b47782ec990", null], │ │ │ │ │ - ["parseFactors", "a01301.html#a8a2136a163e31faaefe2db498a9b9b43", null], │ │ │ │ │ - ["parseMeasurements", "a01301.html#a6eb4817b97658f00e426b753c52361ed", null], │ │ │ │ │ - ["parseVariables", "a01301.html#a683ab6a83b5b3e508e734a7089986838", null], │ │ │ │ │ - ["parseVertexLandmark", "a01301.html#a961e0398bbfc2504c62fdaad70472418", null], │ │ │ │ │ - ["parseVertexPose", "a01301.html#abfc0157e4790e3b03dc601d3e86c684d", null], │ │ │ │ │ - ["readG2o", "a01301.html#a71d086efc70f4af31d3c3b00dde4f7c0", null], │ │ │ │ │ - ["save2D", "a01301.html#acfe516bc2d9ca4cc4530dd3b8460ce3d", null], │ │ │ │ │ - ["writeG2o", "a01301.html#a68bc584d72da3747666ac134a9490f92", null] │ │ │ │ │ + ["gtsam::FrobeniusPrior< Rot >", "a04828.html", "a04828"], │ │ │ │ │ + ["gtsam::FrobeniusFactor< Rot >", "a04832.html", "a04832"], │ │ │ │ │ + ["gtsam::FrobeniusBetweenFactor< Rot >", "a04836.html", "a04836"], │ │ │ │ │ + ["ConvertNoiseModel", "a01301.html#aa43e7fd7c2b86873458a91ddafc506e4", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01301_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/dataset.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/FrobeniusFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,208 +98,181 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
dataset.h
│ │ │ │ +
FrobeniusFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
21#pragma once
│ │ │ │ -
22
│ │ │ │ -
23#include <gtsam/sfm/BinaryMeasurement.h>
│ │ │ │ -
24#include <gtsam/slam/BetweenFactor.h>
│ │ │ │ -
25#include <gtsam/sfm/SfmData.h>
│ │ │ │ -
26#include <gtsam/geometry/Cal3Bundler.h>
│ │ │ │ -
27#include <gtsam/geometry/PinholeCamera.h>
│ │ │ │ -
28#include <gtsam/geometry/Pose2.h>
│ │ │ │ -
29#include <gtsam/geometry/Pose3.h>
│ │ │ │ -
30#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ -
31#include <gtsam/nonlinear/Values.h>
│ │ │ │ -
32#include <gtsam/linear/NoiseModel.h>
│ │ │ │ -
33#include <gtsam/base/serialization.h>
│ │ │ │ -
34#include <gtsam/base/Testable.h>
│ │ │ │ -
35#include <gtsam/base/types.h>
│ │ │ │ -
36
│ │ │ │ -
37#include <boost/smart_ptr/shared_ptr.hpp>
│ │ │ │ -
38#include <string>
│ │ │ │ -
39#include <utility> // for pair
│ │ │ │ -
40#include <vector>
│ │ │ │ -
41#include <iosfwd>
│ │ │ │ -
42#include <map>
│ │ │ │ -
43
│ │ │ │ -
44namespace gtsam {
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/geometry/Rot2.h>
│ │ │ │ +
22#include <gtsam/geometry/Rot3.h>
│ │ │ │ +
23#include <gtsam/geometry/SOn.h>
│ │ │ │ +
24#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
25
│ │ │ │ +
26namespace gtsam {
│ │ │ │ +
27
│ │ │ │ +
42GTSAM_EXPORT SharedNoiseModel
│ │ │ │ +
43ConvertNoiseModel(const SharedNoiseModel &model, size_t n,
│ │ │ │ +
44 bool defaultToUnit = true);
│ │ │ │
45
│ │ │ │ -
57GTSAM_EXPORT std::string findExampleDataFile(const std::string& name);
│ │ │ │ +
50template <class Rot>
│ │ │ │ +
│ │ │ │ +
51class FrobeniusPrior : public NoiseModelFactorN<Rot> {
│ │ │ │ +
52 enum { Dim = Rot::VectorN2::RowsAtCompileTime };
│ │ │ │ +
53 using MatrixNN = typename Rot::MatrixNN;
│ │ │ │ +
54 Eigen::Matrix<double, Dim, 1> vecM_;
│ │ │ │ +
55
│ │ │ │ +
56 public:
│ │ │ │ +
57 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │
58
│ │ │ │ -
63GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name);
│ │ │ │ -
64
│ │ │ │ -
│ │ │ │ -
66enum NoiseFormat {
│ │ │ │ -
67 NoiseFormatG2O,
│ │ │ │ -
68 NoiseFormatTORO,
│ │ │ │ -
69 NoiseFormatGRAPH,
│ │ │ │ -
70 NoiseFormatCOV,
│ │ │ │ -
71 NoiseFormatAUTO
│ │ │ │ -
72};
│ │ │ │ -
│ │ │ │ -
73
│ │ │ │ -
│ │ │ │ -
75enum KernelFunctionType {
│ │ │ │ -
76 KernelFunctionTypeNONE, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY
│ │ │ │ -
77};
│ │ │ │ -
│ │ │ │ -
78
│ │ │ │ -
85template <typename T>
│ │ │ │ -
86GTSAM_EXPORT std::map<size_t, T> parseVariables(const std::string &filename,
│ │ │ │ -
87 size_t maxIndex = 0);
│ │ │ │ -
88
│ │ │ │ -
95template <typename T>
│ │ │ │ -
96GTSAM_EXPORT std::vector<BinaryMeasurement<T>>
│ │ │ │ -
97parseMeasurements(const std::string &filename,
│ │ │ │ -
98 const noiseModel::Diagonal::shared_ptr &model = nullptr,
│ │ │ │ -
99 size_t maxIndex = 0);
│ │ │ │ -
100
│ │ │ │ -
105template <typename T>
│ │ │ │ -
106GTSAM_EXPORT std::vector<typename BetweenFactor<T>::shared_ptr>
│ │ │ │ -
107parseFactors(const std::string &filename,
│ │ │ │ -
108 const noiseModel::Diagonal::shared_ptr &model = nullptr,
│ │ │ │ -
109 size_t maxIndex = 0);
│ │ │ │ -
110
│ │ │ │ -
112typedef std::pair<size_t, Pose2> IndexedPose;
│ │ │ │ -
113typedef std::pair<size_t, Point2> IndexedLandmark;
│ │ │ │ -
114typedef std::pair<std::pair<size_t, size_t>, Pose2> IndexedEdge;
│ │ │ │ +
│ │ │ │ +
60 FrobeniusPrior(Key j, const MatrixNN& M,
│ │ │ │ +
61 const SharedNoiseModel& model = nullptr)
│ │ │ │ +
62 : NoiseModelFactorN<Rot>(ConvertNoiseModel(model, Dim), j) {
│ │ │ │ +
63 vecM_ << Eigen::Map<const Matrix>(M.data(), Dim, 1);
│ │ │ │ +
64 }
│ │ │ │ +
│ │ │ │ +
65
│ │ │ │ +
│ │ │ │ +
67 Vector evaluateError(const Rot& R,
│ │ │ │ +
68 boost::optional<Matrix&> H = boost::none) const override {
│ │ │ │ +
69 return R.vec(H) - vecM_; // Jacobian is computed only when needed.
│ │ │ │ +
70 }
│ │ │ │ +
│ │ │ │ +
71};
│ │ │ │ +
│ │ │ │ +
72
│ │ │ │ +
77template <class Rot>
│ │ │ │ +
│ │ │ │ +
78class FrobeniusFactor : public NoiseModelFactorN<Rot, Rot> {
│ │ │ │ +
79 enum { Dim = Rot::VectorN2::RowsAtCompileTime };
│ │ │ │ +
80
│ │ │ │ +
81 public:
│ │ │ │ +
│ │ │ │ +
83 FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel& model = nullptr)
│ │ │ │ +
84 : NoiseModelFactorN<Rot, Rot>(ConvertNoiseModel(model, Dim), j1,
│ │ │ │ +
85 j2) {}
│ │ │ │ +
│ │ │ │ +
86
│ │ │ │ +
│ │ │ │ +
88 Vector evaluateError(const Rot& R1, const Rot& R2,
│ │ │ │ +
89 boost::optional<Matrix&> H1 = boost::none,
│ │ │ │ +
90 boost::optional<Matrix&> H2 = boost::none) const override {
│ │ │ │ +
91 Vector error = R2.vec(H2) - R1.vec(H1);
│ │ │ │ +
92 if (H1) *H1 = -*H1;
│ │ │ │ +
93 return error;
│ │ │ │ +
94 }
│ │ │ │ +
│ │ │ │ +
95};
│ │ │ │ +
│ │ │ │ +
96
│ │ │ │ +
103template <class Rot>
│ │ │ │ +
│ │ │ │ +
104class FrobeniusBetweenFactor : public NoiseModelFactorN<Rot, Rot> {
│ │ │ │ +
105 Rot R12_;
│ │ │ │ +
106 Eigen::Matrix<double, Rot::dimension, Rot::dimension>
│ │ │ │ +
107 R2hat_H_R1_;
│ │ │ │ +
108 enum { Dim = Rot::VectorN2::RowsAtCompileTime };
│ │ │ │ +
109
│ │ │ │ +
110 public:
│ │ │ │ +
111 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
112
│ │ │ │
115
│ │ │ │ -
121GTSAM_EXPORT boost::optional<IndexedPose> parseVertexPose(std::istream& is,
│ │ │ │ -
122 const std::string& tag);
│ │ │ │ +
│ │ │ │ +
117 FrobeniusBetweenFactor(Key j1, Key j2, const Rot& R12,
│ │ │ │ +
118 const SharedNoiseModel& model = nullptr)
│ │ │ │ +
119 : NoiseModelFactorN<Rot, Rot>(
│ │ │ │ +
120 ConvertNoiseModel(model, Dim), j1, j2),
│ │ │ │ +
121 R12_(R12),
│ │ │ │ +
122 R2hat_H_R1_(R12.inverse().AdjointMap()) {}
│ │ │ │ +
│ │ │ │
123
│ │ │ │ -
129GTSAM_EXPORT boost::optional<IndexedLandmark> parseVertexLandmark(std::istream& is,
│ │ │ │ -
130 const std::string& tag);
│ │ │ │ -
131
│ │ │ │ -
137GTSAM_EXPORT boost::optional<IndexedEdge> parseEdge(std::istream& is,
│ │ │ │ -
138 const std::string& tag);
│ │ │ │ -
139
│ │ │ │ -
143using GraphAndValues =
│ │ │ │ -
144 std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr>;
│ │ │ │ -
145
│ │ │ │ -
153GTSAM_EXPORT GraphAndValues load2D(
│ │ │ │ -
154 std::pair<std::string, SharedNoiseModel> dataset, size_t maxIndex = 0,
│ │ │ │ -
155 bool addNoise = false,
│ │ │ │ -
156 bool smart = true, //
│ │ │ │ -
157 NoiseFormat noiseFormat = NoiseFormatAUTO,
│ │ │ │ -
158 KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
│ │ │ │ -
159
│ │ │ │ -
171GTSAM_EXPORT GraphAndValues
│ │ │ │ -
172load2D(const std::string& filename, SharedNoiseModel model = SharedNoiseModel(),
│ │ │ │ -
173 size_t maxIndex = 0, bool addNoise = false, bool smart = true,
│ │ │ │ -
174 NoiseFormat noiseFormat = NoiseFormatAUTO, //
│ │ │ │ -
175 KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
│ │ │ │ -
176
│ │ │ │ -
178GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
│ │ │ │ -
179 const Values& config, const noiseModel::Diagonal::shared_ptr model,
│ │ │ │ -
180 const std::string& filename);
│ │ │ │ -
181
│ │ │ │ -
190GTSAM_EXPORT GraphAndValues
│ │ │ │ -
191readG2o(const std::string& g2oFile, const bool is3D = false,
│ │ │ │ -
192 KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
│ │ │ │ -
193
│ │ │ │ -
206GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
│ │ │ │ -
207 const Values& estimate, const std::string& filename);
│ │ │ │ -
208
│ │ │ │ -
210GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);
│ │ │ │ -
211
│ │ │ │ -
212// Wrapper-friendly versions of parseFactors<Pose2> and parseFactors<Pose2>
│ │ │ │ -
213using BetweenFactorPose2s = std::vector<BetweenFactor<Pose2>::shared_ptr>;
│ │ │ │ -
214GTSAM_EXPORT BetweenFactorPose2s
│ │ │ │ -
215parse2DFactors(const std::string &filename,
│ │ │ │ -
216 const noiseModel::Diagonal::shared_ptr &model = nullptr,
│ │ │ │ -
217 size_t maxIndex = 0);
│ │ │ │ -
218
│ │ │ │ -
219using BetweenFactorPose3s = std::vector<BetweenFactor<Pose3>::shared_ptr>;
│ │ │ │ -
220GTSAM_EXPORT BetweenFactorPose3s
│ │ │ │ -
221parse3DFactors(const std::string &filename,
│ │ │ │ -
222 const noiseModel::Diagonal::shared_ptr &model = nullptr,
│ │ │ │ -
223 size_t maxIndex = 0);
│ │ │ │ -
224
│ │ │ │ -
225using BinaryMeasurementsUnit3 = std::vector<BinaryMeasurement<Unit3>>;
│ │ │ │ -
226using BinaryMeasurementsPoint3 = std::vector<BinaryMeasurement<Point3>>;
│ │ │ │ -
227using BinaryMeasurementsRot3 = std::vector<BinaryMeasurement<Rot3>>;
│ │ │ │ -
228
│ │ │ │ -
229#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ -
230inline boost::optional<IndexedPose> GTSAM_DEPRECATED
│ │ │ │ -
231parseVertex(std::istream& is, const std::string& tag) {
│ │ │ │ -
232 return parseVertexPose(is, tag);
│ │ │ │ -
233}
│ │ │ │ -
234
│ │ │ │ -
235GTSAM_EXPORT std::map<size_t, Pose3> GTSAM_DEPRECATED
│ │ │ │ -
236parse3DPoses(const std::string& filename, size_t maxIndex = 0);
│ │ │ │ -
237
│ │ │ │ -
238GTSAM_EXPORT std::map<size_t, Point3> GTSAM_DEPRECATED
│ │ │ │ -
239parse3DLandmarks(const std::string& filename, size_t maxIndex = 0);
│ │ │ │ -
240
│ │ │ │ -
241GTSAM_EXPORT GraphAndValues GTSAM_DEPRECATED
│ │ │ │ -
242load2D_robust(const std::string& filename,
│ │ │ │ -
243 const noiseModel::Base::shared_ptr& model, size_t maxIndex = 0);
│ │ │ │ -
244#endif
│ │ │ │ -
245} // namespace gtsam
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ -
types.h
Typedefs for easier changing of types.
│ │ │ │ -
serialization.h
Convenience functions for serializing data structures via boost.serialization.
│ │ │ │ -
Cal3Bundler.h
Calibration used by Bundler.
│ │ │ │ -
PinholeCamera.h
Base class for all pinhole cameras.
│ │ │ │ -
Pose3.h
3D Pose
│ │ │ │ -
Pose2.h
2D Pose
│ │ │ │ - │ │ │ │ -
Values.h
A non-templated config holding any types of Manifold-group elements.
│ │ │ │ -
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │ -
BinaryMeasurement.h
Binary measurement represents a measurement between two keys in a graph. A binary measurement is simi...
│ │ │ │ -
SfmData.h
Data structure for dealing with Structure from Motion data.
│ │ │ │ - │ │ │ │ +
127
│ │ │ │ +
129 void
│ │ │ │ +
│ │ │ │ +
130 print(const std::string &s,
│ │ │ │ +
131 const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ +
132 std::cout << s << "FrobeniusBetweenFactor<" << demangle(typeid(Rot).name())
│ │ │ │ +
133 << ">(" << keyFormatter(this->key1()) << ","
│ │ │ │ +
134 << keyFormatter(this->key2()) << ")\n";
│ │ │ │ +
135 traits<Rot>::Print(R12_, " R12: ");
│ │ │ │ +
136 this->noiseModel_->print(" noise model: ");
│ │ │ │ +
137 }
│ │ │ │ +
│ │ │ │ +
138
│ │ │ │ +
│ │ │ │ +
140 bool equals(const NonlinearFactor &expected,
│ │ │ │ +
141 double tol = 1e-9) const override {
│ │ │ │ +
142 auto e = dynamic_cast<const FrobeniusBetweenFactor *>(&expected);
│ │ │ │ +
143 return e != nullptr && NoiseModelFactorN<Rot, Rot>::equals(*e, tol) &&
│ │ │ │ +
144 traits<Rot>::Equals(this->R12_, e->R12_, tol);
│ │ │ │ +
145 }
│ │ │ │ +
│ │ │ │ +
146
│ │ │ │ +
150
│ │ │ │ +
│ │ │ │ +
152 Vector evaluateError(const Rot& R1, const Rot& R2,
│ │ │ │ +
153 boost::optional<Matrix&> H1 = boost::none,
│ │ │ │ +
154 boost::optional<Matrix&> H2 = boost::none) const override {
│ │ │ │ +
155 const Rot R2hat = R1.compose(R12_);
│ │ │ │ +
156 Eigen::Matrix<double, Dim, Rot::dimension> vec_H_R2hat;
│ │ │ │ +
157 Vector error = R2.vec(H2) - R2hat.vec(H1 ? &vec_H_R2hat : nullptr);
│ │ │ │ +
158 if (H1) *H1 = -vec_H_R2hat * R2hat_H_R1_;
│ │ │ │ +
159 return error;
│ │ │ │ +
160 }
│ │ │ │ +
│ │ │ │ +
162};
│ │ │ │ +
│ │ │ │ +
163
│ │ │ │ +
164} // namespace gtsam
│ │ │ │ +
Rot3.h
3D rotation represented as a rotation matrix or quaternion
│ │ │ │ +
SOn.h
N*N matrix representation of SO(N).
│ │ │ │ +
Rot2.h
2D rotation
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::load2D
GraphAndValues load2D(const string &filename, SharedNoiseModel model, size_t maxIndex, bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType)
Load TORO/G2O style graph files.
Definition dataset.cpp:502
│ │ │ │ -
gtsam::parseVariables
GTSAM_EXPORT std::map< size_t, T > parseVariables(const std::string &filename, size_t maxIndex=0)
Parse variables in a line-based text format (like g2o) into a map.
│ │ │ │ -
gtsam::writeG2o
void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, const string &filename)
This function writes a g2o file from NonlinearFactorGraph and a Values structure.
Definition dataset.cpp:632
│ │ │ │ -
gtsam::IndexedPose
std::pair< size_t, Pose2 > IndexedPose
Return type for auxiliary functions.
Definition dataset.h:112
│ │ │ │ -
gtsam::readG2o
GraphAndValues readG2o(const string &g2oFile, const bool is3D, KernelFunctionType kernelFunctionType)
This function parses a g2o file and stores the measurements into a NonlinearFactorGraph and the initi...
Definition dataset.cpp:617
│ │ │ │ -
gtsam::parseFactors
GTSAM_EXPORT std::vector< typename BetweenFactor< T >::shared_ptr > parseFactors(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model=nullptr, size_t maxIndex=0)
Parse BetweenFactors in a line-based text format (like g2o) into a vector of shared pointers.
│ │ │ │ -
gtsam::KernelFunctionType
KernelFunctionType
Robust kernel type to wrap around quadratic noise model.
Definition dataset.h:75
│ │ │ │ -
gtsam::NoiseFormat
NoiseFormat
Indicates how noise parameters are stored in file.
Definition dataset.h:66
│ │ │ │ -
gtsam::NoiseFormatGRAPH
@ NoiseFormatGRAPH
default: toro-style order, but covariance matrix !
Definition dataset.h:69
│ │ │ │ -
gtsam::NoiseFormatAUTO
@ NoiseFormatAUTO
Try to guess covariance matrix layout.
Definition dataset.h:71
│ │ │ │ -
gtsam::NoiseFormatTORO
@ NoiseFormatTORO
Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr.
Definition dataset.h:68
│ │ │ │ -
gtsam::NoiseFormatCOV
@ NoiseFormatCOV
Covariance matrix C11, C12, C13, C22, C23, C33.
Definition dataset.h:70
│ │ │ │ -
gtsam::NoiseFormatG2O
@ NoiseFormatG2O
Information matrix I11, I12, I13, I22, I23, I33.
Definition dataset.h:67
│ │ │ │ -
gtsam::parseVertexLandmark
boost::optional< IndexedLandmark > parseVertexLandmark(istream &is, const string &tag)
Parse G2O landmark vertex "id x y".
Definition dataset.cpp:187
│ │ │ │ +
gtsam::demangle
std::string demangle(const char *name)
Pretty print Value type name.
Definition types.cpp:37
│ │ │ │ +
gtsam::ConvertNoiseModel
SharedNoiseModel ConvertNoiseModel(const SharedNoiseModel &model, size_t d, bool defaultToUnit)
When creating (any) FrobeniusFactor we can convert a Rot/Pose BetweenFactor noise model into a n-dime...
Definition FrobeniusFactor.cpp:27
│ │ │ │
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ -
gtsam::parseEdge
boost::optional< IndexedEdge > parseEdge(istream &is, const string &tag)
Parse TORO/G2O edge "id1 id2 x y yaw".
Definition dataset.cpp:293
│ │ │ │ -
gtsam::GraphAndValues
std::pair< NonlinearFactorGraph::shared_ptr, Values::shared_ptr > GraphAndValues
Return type for load functions, which return a graph and initial values.
Definition dataset.h:144
│ │ │ │ -
gtsam::parseVertexPose
boost::optional< IndexedPose > parseVertexPose(istream &is, const string &tag)
Parse TORO/G2O vertex "id x y yaw".
Definition dataset.cpp:167
│ │ │ │ -
gtsam::load3D
GraphAndValues load3D(const string &filename)
Load TORO 3D Graph.
Definition dataset.cpp:918
│ │ │ │ -
gtsam::save2D
void save2D(const NonlinearFactorGraph &graph, const Values &config, const noiseModel::Diagonal::shared_ptr model, const string &filename)
save 2d graph
Definition dataset.cpp:584
│ │ │ │ -
gtsam::createRewrittenFileName
string createRewrittenFileName(const string &name)
Creates a temporary file name that needs to be ignored in .gitingnore for checking read-write opratio...
Definition dataset.cpp:99
│ │ │ │ -
gtsam::findExampleDataFile
string findExampleDataFile(const string &name)
Find the full path to an example dataset distributed with gtsam.
Definition dataset.cpp:62
│ │ │ │ -
gtsam::Pose2
A 2D pose (Point2,Rot2)
Definition Pose2.h:36
│ │ │ │ -
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │ -
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
│ │ │ │ +
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ +
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Calculate the error of the factor.
Definition NonlinearFactor.cpp:138
│ │ │ │ +
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ +
gtsam::FrobeniusPrior
FrobeniusPrior calculates the Frobenius norm between a given matrix and an element of SO(3) or SO(4).
Definition FrobeniusFactor.h:51
│ │ │ │ +
gtsam::FrobeniusPrior::FrobeniusPrior
EIGEN_MAKE_ALIGNED_OPERATOR_NEW FrobeniusPrior(Key j, const MatrixNN &M, const SharedNoiseModel &model=nullptr)
Constructor.
Definition FrobeniusFactor.h:60
│ │ │ │ +
gtsam::FrobeniusPrior::evaluateError
Vector evaluateError(const Rot &R, boost::optional< Matrix & > H=boost::none) const override
Error is just Frobenius norm between Rot element and vectorized matrix M.
Definition FrobeniusFactor.h:67
│ │ │ │ +
gtsam::FrobeniusFactor
FrobeniusFactor calculates the Frobenius norm between rotation matrices.
Definition FrobeniusFactor.h:78
│ │ │ │ +
gtsam::FrobeniusFactor::evaluateError
Vector evaluateError(const Rot &R1, const Rot &R2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Error is just Frobenius norm between rotation matrices.
Definition FrobeniusFactor.h:88
│ │ │ │ +
gtsam::FrobeniusFactor::FrobeniusFactor
FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel &model=nullptr)
Constructor.
Definition FrobeniusFactor.h:83
│ │ │ │ +
gtsam::FrobeniusBetweenFactor
FrobeniusBetweenFactor is a BetweenFactor that evaluates the Frobenius norm of the rotation error bet...
Definition FrobeniusFactor.h:104
│ │ │ │ +
gtsam::FrobeniusBetweenFactor::FrobeniusBetweenFactor
FrobeniusBetweenFactor(Key j1, Key j2, const Rot &R12, const SharedNoiseModel &model=nullptr)
Construct from two keys and measured rotation.
Definition FrobeniusFactor.h:117
│ │ │ │ +
gtsam::FrobeniusBetweenFactor::print
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print with optional string
Definition FrobeniusFactor.h:130
│ │ │ │ +
gtsam::FrobeniusBetweenFactor::evaluateError
Vector evaluateError(const Rot &R1, const Rot &R2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Error is Frobenius norm between R1*R12 and R2.
Definition FrobeniusFactor.h:152
│ │ │ │ +
gtsam::FrobeniusBetweenFactor::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
assert equality up to a tolerance
Definition FrobeniusFactor.h:140
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,295 +1,226 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -dataset.h │ │ │ │ │ +FrobeniusFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -21#pragma once │ │ │ │ │ -22 │ │ │ │ │ -23#include <_g_t_s_a_m_/_s_f_m_/_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_s_l_a_m_/_B_e_t_w_e_e_n_F_a_c_t_o_r_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_s_f_m_/_S_f_m_D_a_t_a_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3_B_u_n_d_l_e_r_._h> │ │ │ │ │ -27#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_i_n_h_o_l_e_C_a_m_e_r_a_._h> │ │ │ │ │ -28#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_2_._h> │ │ │ │ │ -29#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_3_._h> │ │ │ │ │ -30#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -31#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_V_a_l_u_e_s_._h> │ │ │ │ │ -32#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_N_o_i_s_e_M_o_d_e_l_._h> │ │ │ │ │ -33#include <_g_t_s_a_m_/_b_a_s_e_/_s_e_r_i_a_l_i_z_a_t_i_o_n_._h> │ │ │ │ │ -34#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ -35#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ -36 │ │ │ │ │ -37#include │ │ │ │ │ -38#include │ │ │ │ │ -39#include // for pair │ │ │ │ │ -40#include │ │ │ │ │ -41#include │ │ │ │ │ -42#include │ │ │ │ │ -43 │ │ │ │ │ -44namespace _g_t_s_a_m { │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_2_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_3_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_S_O_n_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +25 │ │ │ │ │ +26namespace _g_t_s_a_m { │ │ │ │ │ +27 │ │ │ │ │ +42GTSAM_EXPORT _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +43_C_o_n_v_e_r_t_N_o_i_s_e_M_o_d_e_l(const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, size_t n, │ │ │ │ │ +44 bool defaultToUnit = true); │ │ │ │ │ 45 │ │ │ │ │ -57GTSAM_EXPORT std::string _f_i_n_d_E_x_a_m_p_l_e_D_a_t_a_F_i_l_e(const std::string& name); │ │ │ │ │ +50template │ │ │ │ │ +_5_1class _F_r_o_b_e_n_i_u_s_P_r_i_o_r : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +52 enum { Dim = Rot::VectorN2::RowsAtCompileTime }; │ │ │ │ │ +53 using MatrixNN = typename Rot::MatrixNN; │ │ │ │ │ +54 Eigen::Matrix vecM_; │ │ │ │ │ +55 │ │ │ │ │ +56 public: │ │ │ │ │ +57 EIGEN_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ 58 │ │ │ │ │ -63GTSAM_EXPORT std::string _c_r_e_a_t_e_R_e_w_r_i_t_t_e_n_F_i_l_e_N_a_m_e(const std::string& name); │ │ │ │ │ -64 │ │ │ │ │ -_6_6enum _N_o_i_s_e_F_o_r_m_a_t { │ │ │ │ │ -_6_7 _N_o_i_s_e_F_o_r_m_a_t_G_2_O, │ │ │ │ │ -_6_8 _N_o_i_s_e_F_o_r_m_a_t_T_O_R_O, │ │ │ │ │ -_6_9 _N_o_i_s_e_F_o_r_m_a_t_G_R_A_P_H, │ │ │ │ │ -_7_0 _N_o_i_s_e_F_o_r_m_a_t_C_O_V, │ │ │ │ │ -71 _N_o_i_s_e_F_o_r_m_a_t_A_U_T_O │ │ │ │ │ -_7_2}; │ │ │ │ │ -73 │ │ │ │ │ -_7_5enum _K_e_r_n_e_l_F_u_n_c_t_i_o_n_T_y_p_e { │ │ │ │ │ -76 KernelFunctionTypeNONE, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY │ │ │ │ │ -77}; │ │ │ │ │ -78 │ │ │ │ │ -85template │ │ │ │ │ -_8_6GTSAM_EXPORT std::map _p_a_r_s_e_V_a_r_i_a_b_l_e_s(const std::string &filename, │ │ │ │ │ -87 size_t maxIndex = 0); │ │ │ │ │ -88 │ │ │ │ │ -95template │ │ │ │ │ -96GTSAM_EXPORT std::vector> │ │ │ │ │ -_9_7parseMeasurements(const std::string &filename, │ │ │ │ │ -98 const noiseModel::Diagonal::shared_ptr &model = nullptr, │ │ │ │ │ -99 size_t maxIndex = 0); │ │ │ │ │ -100 │ │ │ │ │ -105template │ │ │ │ │ -106GTSAM_EXPORT std::vector::shared_ptr> │ │ │ │ │ -_1_0_7_p_a_r_s_e_F_a_c_t_o_r_s(const std::string &filename, │ │ │ │ │ -108 const noiseModel::Diagonal::shared_ptr &model = nullptr, │ │ │ │ │ -109 size_t maxIndex = 0); │ │ │ │ │ -110 │ │ │ │ │ -_1_1_2typedef std::pair _I_n_d_e_x_e_d_P_o_s_e; │ │ │ │ │ -113typedef std::pair IndexedLandmark; │ │ │ │ │ -114typedef std::pair, _P_o_s_e_2> IndexedEdge; │ │ │ │ │ +_6_0 _F_r_o_b_e_n_i_u_s_P_r_i_o_r(_K_e_y j, const MatrixNN& M, │ │ │ │ │ +61 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model = nullptr) │ │ │ │ │ +62 : _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N(_C_o_n_v_e_r_t_N_o_i_s_e_M_o_d_e_l(model, Dim), j) { │ │ │ │ │ +63 vecM_ << Eigen::Map(M.data(), Dim, 1); │ │ │ │ │ +64 } │ │ │ │ │ +65 │ │ │ │ │ +_6_7 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const Rot& R, │ │ │ │ │ +68 boost::optional H = boost::none) const override { │ │ │ │ │ +69 return R.vec(H) - vecM_; // Jacobian is computed only when needed. │ │ │ │ │ +70 } │ │ │ │ │ +71}; │ │ │ │ │ +72 │ │ │ │ │ +77template │ │ │ │ │ +_7_8class _F_r_o_b_e_n_i_u_s_F_a_c_t_o_r : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +79 enum { Dim = Rot::VectorN2::RowsAtCompileTime }; │ │ │ │ │ +80 │ │ │ │ │ +81 public: │ │ │ │ │ +_8_3 _F_r_o_b_e_n_i_u_s_F_a_c_t_o_r(_K_e_y j1, _K_e_y j2, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model = nullptr) │ │ │ │ │ +84 : _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N(_C_o_n_v_e_r_t_N_o_i_s_e_M_o_d_e_l(model, Dim), j1, │ │ │ │ │ +85 j2) {} │ │ │ │ │ +86 │ │ │ │ │ +_8_8 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const Rot& R1, const Rot& R2, │ │ │ │ │ +89 boost::optional H1 = boost::none, │ │ │ │ │ +90 boost::optional H2 = boost::none) const override { │ │ │ │ │ +91 Vector _e_r_r_o_r = R2.vec(H2) - R1.vec(H1); │ │ │ │ │ +92 if (H1) *H1 = -*H1; │ │ │ │ │ +93 return _e_r_r_o_r; │ │ │ │ │ +94 } │ │ │ │ │ +95}; │ │ │ │ │ +96 │ │ │ │ │ +103template │ │ │ │ │ +_1_0_4class _F_r_o_b_e_n_i_u_s_B_e_t_w_e_e_n_F_a_c_t_o_r : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +105 Rot R12_; │ │ │ │ │ +106 Eigen::Matrix │ │ │ │ │ +107 R2hat_H_R1_; │ │ │ │ │ +108 enum { Dim = Rot::VectorN2::RowsAtCompileTime }; │ │ │ │ │ +109 │ │ │ │ │ +110 public: │ │ │ │ │ +111 EIGEN_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ +112 │ │ │ │ │ 115 │ │ │ │ │ -121GTSAM_EXPORT boost::optional _p_a_r_s_e_V_e_r_t_e_x_P_o_s_e(std::istream& is, │ │ │ │ │ -122 const std::string& tag); │ │ │ │ │ +_1_1_7 _F_r_o_b_e_n_i_u_s_B_e_t_w_e_e_n_F_a_c_t_o_r(_K_e_y j1, _K_e_y j2, const Rot& R12, │ │ │ │ │ +118 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model = nullptr) │ │ │ │ │ +119 : _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N( │ │ │ │ │ +120 _C_o_n_v_e_r_t_N_o_i_s_e_M_o_d_e_l(model, Dim), j1, j2), │ │ │ │ │ +121 R12_(R12), │ │ │ │ │ +122 R2hat_H_R1_(R12.inverse().AdjointMap()) {} │ │ │ │ │ 123 │ │ │ │ │ -129GTSAM_EXPORT boost::optional _p_a_r_s_e_V_e_r_t_e_x_L_a_n_d_m_a_r_k(std:: │ │ │ │ │ -istream& is, │ │ │ │ │ -130 const std::string& tag); │ │ │ │ │ -131 │ │ │ │ │ -137GTSAM_EXPORT boost::optional _p_a_r_s_e_E_d_g_e(std::istream& is, │ │ │ │ │ -138 const std::string& tag); │ │ │ │ │ -139 │ │ │ │ │ -_1_4_3using _G_r_a_p_h_A_n_d_V_a_l_u_e_s = │ │ │ │ │ -144 std::pair; │ │ │ │ │ -145 │ │ │ │ │ -153GTSAM_EXPORT _G_r_a_p_h_A_n_d_V_a_l_u_e_s _l_o_a_d_2_D( │ │ │ │ │ -154 std::pair dataset, size_t maxIndex = 0, │ │ │ │ │ -155 bool addNoise = false, │ │ │ │ │ -156 bool smart = true, // │ │ │ │ │ -157 _N_o_i_s_e_F_o_r_m_a_t noiseFormat = _N_o_i_s_e_F_o_r_m_a_t_A_U_T_O, │ │ │ │ │ -158 _K_e_r_n_e_l_F_u_n_c_t_i_o_n_T_y_p_e kernelFunctionType = KernelFunctionTypeNONE); │ │ │ │ │ -159 │ │ │ │ │ -171GTSAM_EXPORT _G_r_a_p_h_A_n_d_V_a_l_u_e_s │ │ │ │ │ -172_l_o_a_d_2_D(const std::string& filename, _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l model = │ │ │ │ │ -_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l(), │ │ │ │ │ -173 size_t maxIndex = 0, bool addNoise = false, bool smart = true, │ │ │ │ │ -174 _N_o_i_s_e_F_o_r_m_a_t noiseFormat = _N_o_i_s_e_F_o_r_m_a_t_A_U_T_O, // │ │ │ │ │ -175 _K_e_r_n_e_l_F_u_n_c_t_i_o_n_T_y_p_e kernelFunctionType = KernelFunctionTypeNONE); │ │ │ │ │ -176 │ │ │ │ │ -178GTSAM_EXPORT void _s_a_v_e_2_D(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, │ │ │ │ │ -179 const _V_a_l_u_e_s& config, const noiseModel::Diagonal::shared_ptr model, │ │ │ │ │ -180 const std::string& filename); │ │ │ │ │ -181 │ │ │ │ │ -190GTSAM_EXPORT _G_r_a_p_h_A_n_d_V_a_l_u_e_s │ │ │ │ │ -191_r_e_a_d_G_2_o(const std::string& g2oFile, const bool is3D = false, │ │ │ │ │ -192 _K_e_r_n_e_l_F_u_n_c_t_i_o_n_T_y_p_e kernelFunctionType = KernelFunctionTypeNONE); │ │ │ │ │ -193 │ │ │ │ │ -206GTSAM_EXPORT void _w_r_i_t_e_G_2_o(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, │ │ │ │ │ -207 const _V_a_l_u_e_s& estimate, const std::string& filename); │ │ │ │ │ -208 │ │ │ │ │ -210GTSAM_EXPORT _G_r_a_p_h_A_n_d_V_a_l_u_e_s _l_o_a_d_3_D(const std::string& filename); │ │ │ │ │ -211 │ │ │ │ │ -212// Wrapper-friendly versions of parseFactors and parseFactors │ │ │ │ │ -213using BetweenFactorPose2s = std::vector::shared_ptr>; │ │ │ │ │ -214GTSAM_EXPORT BetweenFactorPose2s │ │ │ │ │ -215parse2DFactors(const std::string &filename, │ │ │ │ │ -216 const noiseModel::Diagonal::shared_ptr &model = nullptr, │ │ │ │ │ -217 size_t maxIndex = 0); │ │ │ │ │ -218 │ │ │ │ │ -219using BetweenFactorPose3s = std::vector::shared_ptr>; │ │ │ │ │ -220GTSAM_EXPORT BetweenFactorPose3s │ │ │ │ │ -221parse3DFactors(const std::string &filename, │ │ │ │ │ -222 const noiseModel::Diagonal::shared_ptr &model = nullptr, │ │ │ │ │ -223 size_t maxIndex = 0); │ │ │ │ │ -224 │ │ │ │ │ -225using BinaryMeasurementsUnit3 = std::vector>; │ │ │ │ │ -226using BinaryMeasurementsPoint3 = std::vector>; │ │ │ │ │ -227using BinaryMeasurementsRot3 = std::vector>; │ │ │ │ │ -228 │ │ │ │ │ -229#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ -230inline boost::optional GTSAM_DEPRECATED │ │ │ │ │ -231parseVertex(std::istream& is, const std::string& tag) { │ │ │ │ │ -232 return _p_a_r_s_e_V_e_r_t_e_x_P_o_s_e(is, tag); │ │ │ │ │ -233} │ │ │ │ │ -234 │ │ │ │ │ -235GTSAM_EXPORT std::map GTSAM_DEPRECATED │ │ │ │ │ -236parse3DPoses(const std::string& filename, size_t maxIndex = 0); │ │ │ │ │ -237 │ │ │ │ │ -238GTSAM_EXPORT std::map GTSAM_DEPRECATED │ │ │ │ │ -239parse3DLandmarks(const std::string& filename, size_t maxIndex = 0); │ │ │ │ │ -240 │ │ │ │ │ -241GTSAM_EXPORT _G_r_a_p_h_A_n_d_V_a_l_u_e_s GTSAM_DEPRECATED │ │ │ │ │ -242load2D_robust(const std::string& filename, │ │ │ │ │ -243 const noiseModel::Base::shared_ptr& model, size_t maxIndex = 0); │ │ │ │ │ -244#endif │ │ │ │ │ -245} // namespace gtsam │ │ │ │ │ -_T_e_s_t_a_b_l_e_._h │ │ │ │ │ -Concept check for values that can be used in unit tests. │ │ │ │ │ -_t_y_p_e_s_._h │ │ │ │ │ -Typedefs for easier changing of types. │ │ │ │ │ -_s_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ -Convenience functions for serializing data structures via boost.serialization. │ │ │ │ │ -_C_a_l_3_B_u_n_d_l_e_r_._h │ │ │ │ │ -Calibration used by Bundler. │ │ │ │ │ -_P_i_n_h_o_l_e_C_a_m_e_r_a_._h │ │ │ │ │ -Base class for all pinhole cameras. │ │ │ │ │ -_P_o_s_e_3_._h │ │ │ │ │ -3D Pose │ │ │ │ │ -_P_o_s_e_2_._h │ │ │ │ │ -2D Pose │ │ │ │ │ -_N_o_i_s_e_M_o_d_e_l_._h │ │ │ │ │ -_V_a_l_u_e_s_._h │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Factor Graph consisting of non-linear factors. │ │ │ │ │ -_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_._h │ │ │ │ │ -Binary measurement represents a measurement between two keys in a graph. A │ │ │ │ │ -binary measurement is simi... │ │ │ │ │ -_S_f_m_D_a_t_a_._h │ │ │ │ │ -Data structure for dealing with Structure from Motion data. │ │ │ │ │ -_B_e_t_w_e_e_n_F_a_c_t_o_r_._h │ │ │ │ │ +127 │ │ │ │ │ +129 void │ │ │ │ │ +_1_3_0 _p_r_i_n_t(const std::string &s, │ │ │ │ │ +131 const _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ +132 std::cout << s << "FrobeniusBetweenFactor<" << _d_e_m_a_n_g_l_e(typeid(Rot).name()) │ │ │ │ │ +133 << ">(" << keyFormatter(this->key1()) << "," │ │ │ │ │ +134 << keyFormatter(this->key2()) << ")\n"; │ │ │ │ │ +135 _t_r_a_i_t_s_<_R_o_t_>_:_:_P_r_i_n_t(R12_, " R12: "); │ │ │ │ │ +136 this->noiseModel_->print(" noise model: "); │ │ │ │ │ +137 } │ │ │ │ │ +138 │ │ │ │ │ +_1_4_0 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r &expected, │ │ │ │ │ +141 double tol = 1e-9) const override { │ │ │ │ │ +142 auto e = dynamic_cast(&expected); │ │ │ │ │ +143 return e != nullptr && _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_R_o_t_,_ _R_o_t_>_:_:_e_q_u_a_l_s(*e, tol) && │ │ │ │ │ +144 _t_r_a_i_t_s_<_R_o_t_>_:_:_E_q_u_a_l_s(this->R12_, e->R12_, tol); │ │ │ │ │ +145 } │ │ │ │ │ +146 │ │ │ │ │ +150 │ │ │ │ │ +_1_5_2 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const Rot& R1, const Rot& R2, │ │ │ │ │ +153 boost::optional H1 = boost::none, │ │ │ │ │ +154 boost::optional H2 = boost::none) const override { │ │ │ │ │ +155 const Rot R2hat = R1.compose(R12_); │ │ │ │ │ +156 Eigen::Matrix vec_H_R2hat; │ │ │ │ │ +157 Vector _e_r_r_o_r = R2.vec(H2) - R2hat.vec(H1 ? &vec_H_R2hat : nullptr); │ │ │ │ │ +158 if (H1) *H1 = -vec_H_R2hat * R2hat_H_R1_; │ │ │ │ │ +159 return _e_r_r_o_r; │ │ │ │ │ +160 } │ │ │ │ │ +162}; │ │ │ │ │ +163 │ │ │ │ │ +164} // namespace gtsam │ │ │ │ │ +_R_o_t_3_._h │ │ │ │ │ +3D rotation represented as a rotation matrix or quaternion │ │ │ │ │ +_S_O_n_._h │ │ │ │ │ +N*N matrix representation of SO(N). │ │ │ │ │ +_R_o_t_2_._h │ │ │ │ │ +2D rotation │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ +Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_l_o_a_d_2_D │ │ │ │ │ -GraphAndValues load2D(const string &filename, SharedNoiseModel model, size_t │ │ │ │ │ -maxIndex, bool addNoise, bool smart, NoiseFormat noiseFormat, │ │ │ │ │ -KernelFunctionType kernelFunctionType) │ │ │ │ │ -Load TORO/G2O style graph files. │ │ │ │ │ -DDeeffiinniittiioonn dataset.cpp:502 │ │ │ │ │ -_g_t_s_a_m_:_:_p_a_r_s_e_V_a_r_i_a_b_l_e_s │ │ │ │ │ -GTSAM_EXPORT std::map< size_t, T > parseVariables(const std::string &filename, │ │ │ │ │ -size_t maxIndex=0) │ │ │ │ │ -Parse variables in a line-based text format (like g2o) into a map. │ │ │ │ │ -_g_t_s_a_m_:_:_w_r_i_t_e_G_2_o │ │ │ │ │ -void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, const │ │ │ │ │ -string &filename) │ │ │ │ │ -This function writes a g2o file from NonlinearFactorGraph and a Values │ │ │ │ │ -structure. │ │ │ │ │ -DDeeffiinniittiioonn dataset.cpp:632 │ │ │ │ │ -_g_t_s_a_m_:_:_I_n_d_e_x_e_d_P_o_s_e │ │ │ │ │ -std::pair< size_t, Pose2 > IndexedPose │ │ │ │ │ -Return type for auxiliary functions. │ │ │ │ │ -DDeeffiinniittiioonn dataset.h:112 │ │ │ │ │ -_g_t_s_a_m_:_:_r_e_a_d_G_2_o │ │ │ │ │ -GraphAndValues readG2o(const string &g2oFile, const bool is3D, │ │ │ │ │ -KernelFunctionType kernelFunctionType) │ │ │ │ │ -This function parses a g2o file and stores the measurements into a │ │ │ │ │ -NonlinearFactorGraph and the initi... │ │ │ │ │ -DDeeffiinniittiioonn dataset.cpp:617 │ │ │ │ │ -_g_t_s_a_m_:_:_p_a_r_s_e_F_a_c_t_o_r_s │ │ │ │ │ -GTSAM_EXPORT std::vector< typename BetweenFactor< T >::shared_ptr > │ │ │ │ │ -parseFactors(const std::string &filename, const noiseModel::Diagonal:: │ │ │ │ │ -shared_ptr &model=nullptr, size_t maxIndex=0) │ │ │ │ │ -Parse BetweenFactors in a line-based text format (like g2o) into a vector of │ │ │ │ │ -shared pointers. │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_r_n_e_l_F_u_n_c_t_i_o_n_T_y_p_e │ │ │ │ │ -KernelFunctionType │ │ │ │ │ -Robust kernel type to wrap around quadratic noise model. │ │ │ │ │ -DDeeffiinniittiioonn dataset.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_F_o_r_m_a_t │ │ │ │ │ -NoiseFormat │ │ │ │ │ -Indicates how noise parameters are stored in file. │ │ │ │ │ -DDeeffiinniittiioonn dataset.h:66 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_F_o_r_m_a_t_G_R_A_P_H │ │ │ │ │ -@ NoiseFormatGRAPH │ │ │ │ │ -default: toro-style order, but covariance matrix ! │ │ │ │ │ -DDeeffiinniittiioonn dataset.h:69 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_F_o_r_m_a_t_A_U_T_O │ │ │ │ │ -@ NoiseFormatAUTO │ │ │ │ │ -Try to guess covariance matrix layout. │ │ │ │ │ -DDeeffiinniittiioonn dataset.h:71 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_F_o_r_m_a_t_T_O_R_O │ │ │ │ │ -@ NoiseFormatTORO │ │ │ │ │ -Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr. │ │ │ │ │ -DDeeffiinniittiioonn dataset.h:68 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_F_o_r_m_a_t_C_O_V │ │ │ │ │ -@ NoiseFormatCOV │ │ │ │ │ -Covariance matrix C11, C12, C13, C22, C23, C33. │ │ │ │ │ -DDeeffiinniittiioonn dataset.h:70 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_F_o_r_m_a_t_G_2_O │ │ │ │ │ -@ NoiseFormatG2O │ │ │ │ │ -Information matrix I11, I12, I13, I22, I23, I33. │ │ │ │ │ -DDeeffiinniittiioonn dataset.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_p_a_r_s_e_V_e_r_t_e_x_L_a_n_d_m_a_r_k │ │ │ │ │ -boost::optional< IndexedLandmark > parseVertexLandmark(istream &is, const │ │ │ │ │ -string &tag) │ │ │ │ │ -Parse G2O landmark vertex "id x y". │ │ │ │ │ -DDeeffiinniittiioonn dataset.cpp:187 │ │ │ │ │ +_g_t_s_a_m_:_:_d_e_m_a_n_g_l_e │ │ │ │ │ +std::string demangle(const char *name) │ │ │ │ │ +Pretty print Value type name. │ │ │ │ │ +DDeeffiinniittiioonn types.cpp:37 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_v_e_r_t_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +SharedNoiseModel ConvertNoiseModel(const SharedNoiseModel &model, size_t d, │ │ │ │ │ +bool defaultToUnit) │ │ │ │ │ +When creating (any) FrobeniusFactor we can convert a Rot/Pose BetweenFactor │ │ │ │ │ +noise model into a n-dime... │ │ │ │ │ +DDeeffiinniittiioonn FrobeniusFactor.cpp:27 │ │ │ │ │ _g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ Aliases. │ │ │ │ │ DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ -_g_t_s_a_m_:_:_p_a_r_s_e_E_d_g_e │ │ │ │ │ -boost::optional< IndexedEdge > parseEdge(istream &is, const string &tag) │ │ │ │ │ -Parse TORO/G2O edge "id1 id2 x y yaw". │ │ │ │ │ -DDeeffiinniittiioonn dataset.cpp:293 │ │ │ │ │ -_g_t_s_a_m_:_:_G_r_a_p_h_A_n_d_V_a_l_u_e_s │ │ │ │ │ -std::pair< NonlinearFactorGraph::shared_ptr, Values::shared_ptr > │ │ │ │ │ -GraphAndValues │ │ │ │ │ -Return type for load functions, which return a graph and initial values. │ │ │ │ │ -DDeeffiinniittiioonn dataset.h:144 │ │ │ │ │ -_g_t_s_a_m_:_:_p_a_r_s_e_V_e_r_t_e_x_P_o_s_e │ │ │ │ │ -boost::optional< IndexedPose > parseVertexPose(istream &is, const string &tag) │ │ │ │ │ -Parse TORO/G2O vertex "id x y yaw". │ │ │ │ │ -DDeeffiinniittiioonn dataset.cpp:167 │ │ │ │ │ -_g_t_s_a_m_:_:_l_o_a_d_3_D │ │ │ │ │ -GraphAndValues load3D(const string &filename) │ │ │ │ │ -Load TORO 3D Graph. │ │ │ │ │ -DDeeffiinniittiioonn dataset.cpp:918 │ │ │ │ │ -_g_t_s_a_m_:_:_s_a_v_e_2_D │ │ │ │ │ -void save2D(const NonlinearFactorGraph &graph, const Values &config, const │ │ │ │ │ -noiseModel::Diagonal::shared_ptr model, const string &filename) │ │ │ │ │ -save 2d graph │ │ │ │ │ -DDeeffiinniittiioonn dataset.cpp:584 │ │ │ │ │ -_g_t_s_a_m_:_:_c_r_e_a_t_e_R_e_w_r_i_t_t_e_n_F_i_l_e_N_a_m_e │ │ │ │ │ -string createRewrittenFileName(const string &name) │ │ │ │ │ -Creates a temporary file name that needs to be ignored in .gitingnore for │ │ │ │ │ -checking read-write opratio... │ │ │ │ │ -DDeeffiinniittiioonn dataset.cpp:99 │ │ │ │ │ -_g_t_s_a_m_:_:_f_i_n_d_E_x_a_m_p_l_e_D_a_t_a_F_i_l_e │ │ │ │ │ -string findExampleDataFile(const string &name) │ │ │ │ │ -Find the full path to an example dataset distributed with gtsam. │ │ │ │ │ -DDeeffiinniittiioonn dataset.cpp:62 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2 │ │ │ │ │ -A 2D pose (Point2,Rot2) │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const This &other, double tol=1e-9) const │ │ │ │ │ +check equality │ │ │ │ │ +DDeeffiinniittiioonn Factor.cpp:42 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ +Nonlinear factor base class. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_e_r_r_o_r │ │ │ │ │ +double error(const Values &c) const override │ │ │ │ │ +Calculate the error of the factor. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.cpp:138 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ +A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ +variables. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ +_g_t_s_a_m_:_:_F_r_o_b_e_n_i_u_s_P_r_i_o_r │ │ │ │ │ +FrobeniusPrior calculates the Frobenius norm between a given matrix and an │ │ │ │ │ +element of SO(3) or SO(4). │ │ │ │ │ +DDeeffiinniittiioonn FrobeniusFactor.h:51 │ │ │ │ │ +_g_t_s_a_m_:_:_F_r_o_b_e_n_i_u_s_P_r_i_o_r_:_:_F_r_o_b_e_n_i_u_s_P_r_i_o_r │ │ │ │ │ +EIGEN_MAKE_ALIGNED_OPERATOR_NEW FrobeniusPrior(Key j, const MatrixNN &M, const │ │ │ │ │ +SharedNoiseModel &model=nullptr) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn FrobeniusFactor.h:60 │ │ │ │ │ +_g_t_s_a_m_:_:_F_r_o_b_e_n_i_u_s_P_r_i_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ +Vector evaluateError(const Rot &R, boost::optional< Matrix & > H=boost::none) │ │ │ │ │ +const override │ │ │ │ │ +Error is just Frobenius norm between Rot element and vectorized matrix M. │ │ │ │ │ +DDeeffiinniittiioonn FrobeniusFactor.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_F_r_o_b_e_n_i_u_s_F_a_c_t_o_r │ │ │ │ │ +FrobeniusFactor calculates the Frobenius norm between rotation matrices. │ │ │ │ │ +DDeeffiinniittiioonn FrobeniusFactor.h:78 │ │ │ │ │ +_g_t_s_a_m_:_:_F_r_o_b_e_n_i_u_s_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ +Vector evaluateError(const Rot &R1, const Rot &R2, boost::optional< Matrix & > │ │ │ │ │ +H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override │ │ │ │ │ +Error is just Frobenius norm between rotation matrices. │ │ │ │ │ +DDeeffiinniittiioonn FrobeniusFactor.h:88 │ │ │ │ │ +_g_t_s_a_m_:_:_F_r_o_b_e_n_i_u_s_F_a_c_t_o_r_:_:_F_r_o_b_e_n_i_u_s_F_a_c_t_o_r │ │ │ │ │ +FrobeniusFactor(Key j1, Key j2, const SharedNoiseModel &model=nullptr) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn FrobeniusFactor.h:83 │ │ │ │ │ +_g_t_s_a_m_:_:_F_r_o_b_e_n_i_u_s_B_e_t_w_e_e_n_F_a_c_t_o_r │ │ │ │ │ +FrobeniusBetweenFactor is a BetweenFactor that evaluates the Frobenius norm of │ │ │ │ │ +the rotation error bet... │ │ │ │ │ +DDeeffiinniittiioonn FrobeniusFactor.h:104 │ │ │ │ │ +_g_t_s_a_m_:_:_F_r_o_b_e_n_i_u_s_B_e_t_w_e_e_n_F_a_c_t_o_r_:_:_F_r_o_b_e_n_i_u_s_B_e_t_w_e_e_n_F_a_c_t_o_r │ │ │ │ │ +FrobeniusBetweenFactor(Key j1, Key j2, const Rot &R12, const SharedNoiseModel │ │ │ │ │ +&model=nullptr) │ │ │ │ │ +Construct from two keys and measured rotation. │ │ │ │ │ +DDeeffiinniittiioonn FrobeniusFactor.h:117 │ │ │ │ │ +_g_t_s_a_m_:_:_F_r_o_b_e_n_i_u_s_B_e_t_w_e_e_n_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s, const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +print with optional string │ │ │ │ │ +DDeeffiinniittiioonn FrobeniusFactor.h:130 │ │ │ │ │ +_g_t_s_a_m_:_:_F_r_o_b_e_n_i_u_s_B_e_t_w_e_e_n_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ +Vector evaluateError(const Rot &R1, const Rot &R2, boost::optional< Matrix & > │ │ │ │ │ +H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override │ │ │ │ │ +Error is Frobenius norm between R1*R12 and R2. │ │ │ │ │ +DDeeffiinniittiioonn FrobeniusFactor.h:152 │ │ │ │ │ +_g_t_s_a_m_:_:_F_r_o_b_e_n_i_u_s_B_e_t_w_e_e_n_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const NonlinearFactor &expected, double tol=1e-9) const override │ │ │ │ │ +assert equality up to a tolerance │ │ │ │ │ +DDeeffiinniittiioonn FrobeniusFactor.h:140 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _d_a_t_a_s_e_t_._h │ │ │ │ │ + * _F_r_o_b_e_n_i_u_s_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01307_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/KarcherMeanFactor-inl.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/JacobianFactorQR.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,111 +98,95 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
KarcherMeanFactor-inl.h
│ │ │ │ +
JacobianFactorQR.h
│ │ │ │
│ │ │ │
│ │ │ │ -
1/* ----------------------------------------------------------------------------
│ │ │ │ -
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ -
4 * Atlanta, Georgia 30332-0415
│ │ │ │ -
5 * All Rights Reserved
│ │ │ │ -
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ -
7
│ │ │ │ -
8 * See LICENSE for the license information
│ │ │ │ -
9
│ │ │ │ -
10 * -------------------------------------------------------------------------- */
│ │ │ │ -
11
│ │ │ │ -
12/*
│ │ │ │ -
13 * @file KarcherMeanFactor.cpp
│ │ │ │ -
14 * @author Frank Dellaert
│ │ │ │ -
15 * @date March 2019
│ │ │ │ -
16 */
│ │ │ │ -
17
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
│ │ │ │ -
21#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ -
22#include <gtsam/slam/KarcherMeanFactor.h>
│ │ │ │ -
23
│ │ │ │ -
24using namespace std;
│ │ │ │ +
1/*
│ │ │ │ +
2 * @file JacobianFactorQR.h
│ │ │ │ +
3 * @brief Jacobianfactor that combines and eliminates points
│ │ │ │ +
4 * @date Oct 27, 2013
│ │ │ │ +
5 * @uthor Frank Dellaert
│ │ │ │ +
6 */
│ │ │ │ +
7
│ │ │ │ +
8#pragma once
│ │ │ │ +
9#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ +
10#include <gtsam/linear/RegularJacobianFactor.h>
│ │ │ │ +
11#include <gtsam/inference/Symbol.h>
│ │ │ │ +
12
│ │ │ │ +
13namespace gtsam {
│ │ │ │ +
14
│ │ │ │ +
15class GaussianBayesNet;
│ │ │ │ +
16
│ │ │ │ +
20template<size_t D, size_t ZDim>
│ │ │ │ +
│ │ │ │ +
21class JacobianFactorQR: public RegularJacobianFactor<D> {
│ │ │ │ +
22
│ │ │ │ +
23 typedef RegularJacobianFactor<D> Base;
│ │ │ │ +
24 typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
│ │ │ │
25
│ │ │ │ -
26namespace gtsam {
│ │ │ │ +
26public:
│ │ │ │
27
│ │ │ │ -
28template <class T, class ALLOC>
│ │ │ │ -
29T FindKarcherMeanImpl(const vector<T, ALLOC>& rotations) {
│ │ │ │ -
30 // Cost function C(R) = \sum PriorFactor(R_i)::error(R)
│ │ │ │ -
31 // No closed form solution.
│ │ │ │ -
32 NonlinearFactorGraph graph;
│ │ │ │ -
33 static const Key kKey(0);
│ │ │ │ -
34 for (const auto& R : rotations) {
│ │ │ │ -
35 graph.addPrior<T>(kKey, R);
│ │ │ │ -
36 }
│ │ │ │ -
37 Values initial;
│ │ │ │ -
38 initial.insert<T>(kKey, T());
│ │ │ │ -
39 auto result = GaussNewtonOptimizer(graph, initial).optimize();
│ │ │ │ -
40 return result.at<T>(kKey);
│ │ │ │ -
41}
│ │ │ │ -
42
│ │ │ │ -
43template <class T>
│ │ │ │ -
44T FindKarcherMean(const std::vector<T>& rotations) {
│ │ │ │ -
45 return FindKarcherMeanImpl(rotations);
│ │ │ │ -
46}
│ │ │ │ -
47
│ │ │ │ -
48template <class T>
│ │ │ │ -
│ │ │ │ -
49T FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>>& rotations) {
│ │ │ │ -
50 return FindKarcherMeanImpl(rotations);
│ │ │ │ -
51}
│ │ │ │ -
│ │ │ │ -
52
│ │ │ │ -
53template <class T>
│ │ │ │ -
54T FindKarcherMean(std::initializer_list<T>&& rotations) {
│ │ │ │ -
55 return FindKarcherMeanImpl(std::vector<T, Eigen::aligned_allocator<T> >(rotations));
│ │ │ │ -
56}
│ │ │ │ -
57
│ │ │ │ -
58template <class T>
│ │ │ │ -
59template <typename CONTAINER>
│ │ │ │ -
│ │ │ │ -
60KarcherMeanFactor<T>::KarcherMeanFactor(const CONTAINER &keys, int d,
│ │ │ │ -
61 boost::optional<double> beta)
│ │ │ │ -
62 : NonlinearFactor(keys), d_(static_cast<size_t>(d)) {
│ │ │ │ -
63 if (d <= 0) {
│ │ │ │ -
64 throw std::invalid_argument(
│ │ │ │ -
65 "KarcherMeanFactor needs dimension for dynamic types.");
│ │ │ │ -
66 }
│ │ │ │ -
67 // Create the constant Jacobian made of d*d identity matrices,
│ │ │ │ -
68 // where d is the dimensionality of the manifold.
│ │ │ │ -
69 Matrix A = Matrix::Identity(d, d);
│ │ │ │ -
70 if (beta) A *= std::sqrt(*beta);
│ │ │ │ -
71 std::map<Key, Matrix> terms;
│ │ │ │ -
72 for (Key j : keys) {
│ │ │ │ -
73 terms[j] = A;
│ │ │ │ -
74 }
│ │ │ │ -
75 whitenedJacobian_ =
│ │ │ │ -
76 boost::make_shared<JacobianFactor>(terms, Vector::Zero(d));
│ │ │ │ -
77}
│ │ │ │ -
│ │ │ │ -
78} // namespace gtsam
│ │ │ │ - │ │ │ │ -
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │ +
│ │ │ │ +
31 JacobianFactorQR(const KeyVector& keys,
│ │ │ │ +
32 const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix3& P,
│ │ │ │ +
33 const Vector& b, //
│ │ │ │ +
34 const SharedDiagonal& model = SharedDiagonal()) :
│ │ │ │ +
35 Base() {
│ │ │ │ +
36 // Create a number of Jacobian factors in a factor graph
│ │ │ │ +
37 GaussianFactorGraph gfg;
│ │ │ │ +
38 Symbol pointKey('p', 0);
│ │ │ │ +
39 for (size_t k = 0; k < FBlocks.size(); ++k) {
│ │ │ │ +
40 Key key = keys[k];
│ │ │ │ +
41 gfg.add(pointKey, E.block<ZDim, 3>(ZDim * k, 0), key, FBlocks[k],
│ │ │ │ +
42 b.segment < ZDim > (ZDim * k), model);
│ │ │ │ +
43 }
│ │ │ │ +
44 //gfg.print("gfg");
│ │ │ │ +
45
│ │ │ │ +
46 // eliminate the point
│ │ │ │ +
47 boost::shared_ptr<GaussianBayesNet> bn;
│ │ │ │ +
48 GaussianFactorGraph::shared_ptr fg;
│ │ │ │ +
49 KeyVector variables;
│ │ │ │ +
50 variables.push_back(pointKey);
│ │ │ │ +
51 boost::tie(bn, fg) = gfg.eliminatePartialSequential(variables, EliminateQR);
│ │ │ │ +
52 //fg->print("fg");
│ │ │ │ +
53
│ │ │ │ +
54 JacobianFactor::operator=(JacobianFactor(*fg));
│ │ │ │ +
55 }
│ │ │ │ +
│ │ │ │ +
56};
│ │ │ │ +
│ │ │ │ +
57// end class JacobianFactorQR
│ │ │ │ +
58
│ │ │ │ +
59}// end namespace gtsam
│ │ │ │ +
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ +
RegularJacobianFactor.h
JacobianFactor class with fixed sized blcoks.
│ │ │ │ +
gtsam::JacobianFactor::EliminateQR
friend GTSAM_EXPORT std::pair< boost::shared_ptr< GaussianConditional >, shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Densely partially eliminate with QR factorization, this is usually provided as an argument to one of ...
Definition JacobianFactor.cpp:789
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::EliminateableFactorGraph::eliminatePartialSequential
std::pair< boost::shared_ptr< BayesNetType >, boost::shared_ptr< FactorGraphType > > eliminatePartialSequential(const Ordering &ordering, const Eliminate &function=EliminationTraitsType::DefaultEliminate, OptionalVariableIndex variableIndex=boost::none) const
Do sequential elimination of some variables, in ordering provided, to produce a Bayes net and a remai...
Definition EliminateableFactorGraph-inst.h:154
│ │ │ │
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
│ │ │ │ -
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ -
gtsam::KarcherMeanFactor::KarcherMeanFactor
KarcherMeanFactor(const CONTAINER &keys, int d=D, boost::optional< double > beta=boost::none)
Construct from given keys.
Definition KarcherMeanFactor-inl.h:60
│ │ │ │ -
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │ +
gtsam::Symbol
Character and index key used to refer to variables.
Definition Symbol.h:35
│ │ │ │ +
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ +
gtsam::GaussianFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactorGraph.h:81
│ │ │ │ +
gtsam::GaussianFactorGraph::add
void add(const GaussianFactor &factor)
Add a factor by value - makes a copy.
Definition GaussianFactorGraph.h:127
│ │ │ │ +
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition JacobianFactor.h:91
│ │ │ │ +
gtsam::RegularJacobianFactor
JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator.
Definition RegularJacobianFactor.h:32
│ │ │ │ +
gtsam::JacobianFactorQR
JacobianFactor for Schur complement that uses Q noise model.
Definition JacobianFactorQR.h:21
│ │ │ │ +
gtsam::JacobianFactorQR::JacobianFactorQR
JacobianFactorQR(const KeyVector &keys, const std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > &FBlocks, const Matrix &E, const Matrix3 &P, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Constructor.
Definition JacobianFactorQR.h:31
│ │ │ │ + │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,117 +1,129 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -KarcherMeanFactor-inl.h │ │ │ │ │ -1/* --------------------------------------------------------------------------- │ │ │ │ │ -- │ │ │ │ │ -2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ -4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -5 * All Rights Reserved │ │ │ │ │ -6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +JacobianFactorQR.h │ │ │ │ │ +1/* │ │ │ │ │ +2 * @file JacobianFactorQR.h │ │ │ │ │ +3 * @brief Jacobianfactor that combines and eliminates points │ │ │ │ │ +4 * @date Oct 27, 2013 │ │ │ │ │ +5 * @uthor Frank Dellaert │ │ │ │ │ +6 */ │ │ │ │ │ 7 │ │ │ │ │ -8 * See LICENSE for the license information │ │ │ │ │ -9 │ │ │ │ │ -10 * ------------------------------------------------------------------------- │ │ │ │ │ -- */ │ │ │ │ │ -11 │ │ │ │ │ -12/* │ │ │ │ │ -13 * @file KarcherMeanFactor.cpp │ │ │ │ │ -14 * @author Frank Dellaert │ │ │ │ │ -15 * @date March 2019 │ │ │ │ │ -16 */ │ │ │ │ │ -17 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -22#include │ │ │ │ │ -23 │ │ │ │ │ -24using namespace std; │ │ │ │ │ +8#pragma once │ │ │ │ │ +9#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +10#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ +11#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_S_y_m_b_o_l_._h> │ │ │ │ │ +12 │ │ │ │ │ +13namespace _g_t_s_a_m { │ │ │ │ │ +14 │ │ │ │ │ +15class GaussianBayesNet; │ │ │ │ │ +16 │ │ │ │ │ +20template │ │ │ │ │ +_2_1class _J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q_R: public _R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r { │ │ │ │ │ +22 │ │ │ │ │ +23 typedef _R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_<_D_> Base; │ │ │ │ │ +24 typedef Eigen::Matrix MatrixZD; │ │ │ │ │ 25 │ │ │ │ │ -26namespace _g_t_s_a_m { │ │ │ │ │ +26public: │ │ │ │ │ 27 │ │ │ │ │ -28template │ │ │ │ │ -29T FindKarcherMeanImpl(const vector& rotations) { │ │ │ │ │ -30 // Cost function C(R) = \sum PriorFactor(R_i)::error(R) │ │ │ │ │ -31 // No closed form solution. │ │ │ │ │ -32 NonlinearFactorGraph graph; │ │ │ │ │ -33 static const _K_e_y kKey(0); │ │ │ │ │ -34 for (const auto& R : rotations) { │ │ │ │ │ -35 graph.addPrior(kKey, R); │ │ │ │ │ -36 } │ │ │ │ │ -37 _V_a_l_u_e_s initial; │ │ │ │ │ -38 initial.insert(kKey, T()); │ │ │ │ │ -39 auto result = GaussNewtonOptimizer(graph, initial).optimize(); │ │ │ │ │ -40 return result.at(kKey); │ │ │ │ │ -41} │ │ │ │ │ -42 │ │ │ │ │ -43template │ │ │ │ │ -44T FindKarcherMean(const std::vector& rotations) { │ │ │ │ │ -45 return FindKarcherMeanImpl(rotations); │ │ │ │ │ -46} │ │ │ │ │ -47 │ │ │ │ │ -48template │ │ │ │ │ -_4_9T FindKarcherMean(const std::vector>& │ │ │ │ │ -rotations) { │ │ │ │ │ -50 return FindKarcherMeanImpl(rotations); │ │ │ │ │ -51} │ │ │ │ │ -52 │ │ │ │ │ -53template │ │ │ │ │ -54T FindKarcherMean(std::initializer_list&& rotations) { │ │ │ │ │ -55 return FindKarcherMeanImpl(std::vector > │ │ │ │ │ -(rotations)); │ │ │ │ │ -56} │ │ │ │ │ -57 │ │ │ │ │ -58template │ │ │ │ │ -59template │ │ │ │ │ -_6_0_K_a_r_c_h_e_r_M_e_a_n_F_a_c_t_o_r_<_T_>_:_:_K_a_r_c_h_e_r_M_e_a_n_F_a_c_t_o_r(const CONTAINER &keys, int d, │ │ │ │ │ -61 boost::optional beta) │ │ │ │ │ -62 : _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r(keys), d_(static_cast(d)) { │ │ │ │ │ -63 if (d <= 0) { │ │ │ │ │ -64 throw std::invalid_argument( │ │ │ │ │ -65 "KarcherMeanFactor needs dimension for dynamic types."); │ │ │ │ │ -66 } │ │ │ │ │ -67 // Create the constant Jacobian made of d*d identity matrices, │ │ │ │ │ -68 // where d is the dimensionality of the manifold. │ │ │ │ │ -69 Matrix A = Matrix::Identity(d, d); │ │ │ │ │ -70 if (beta) A *= std::sqrt(*beta); │ │ │ │ │ -71 std::map terms; │ │ │ │ │ -72 for (_K_e_y j : _k_e_y_s) { │ │ │ │ │ -73 terms[j] = A; │ │ │ │ │ -74 } │ │ │ │ │ -75 whitenedJacobian_ = │ │ │ │ │ -76 boost::make_shared(terms, Vector::Zero(d)); │ │ │ │ │ -77} │ │ │ │ │ -78} // namespace gtsam │ │ │ │ │ -_G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Factor Graph consisting of non-linear factors. │ │ │ │ │ +_3_1 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q_R(const _K_e_y_V_e_c_t_o_r& _k_e_y_s, │ │ │ │ │ +32 const std::vector >& FBlocks, │ │ │ │ │ +const Matrix& E, const Matrix3& P, │ │ │ │ │ +33 const Vector& b, // │ │ │ │ │ +34 const SharedDiagonal& model = SharedDiagonal()) : │ │ │ │ │ +35 Base() { │ │ │ │ │ +36 // Create a number of Jacobian factors in a factor graph │ │ │ │ │ +37 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h gfg; │ │ │ │ │ +38 _S_y_m_b_o_l pointKey('p', 0); │ │ │ │ │ +39 for (size_t k = 0; k < FBlocks.size(); ++k) { │ │ │ │ │ +40 _K_e_y key = _k_e_y_s[k]; │ │ │ │ │ +41 gfg._a_d_d(pointKey, E.block(ZDim * k, 0), key, FBlocks[k], │ │ │ │ │ +42 b.segment < ZDim > (ZDim * k), model); │ │ │ │ │ +43 } │ │ │ │ │ +44 //gfg.print("gfg"); │ │ │ │ │ +45 │ │ │ │ │ +46 // eliminate the point │ │ │ │ │ +47 boost::shared_ptr bn; │ │ │ │ │ +48 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r fg; │ │ │ │ │ +49 _K_e_y_V_e_c_t_o_r variables; │ │ │ │ │ +50 variables.push_back(pointKey); │ │ │ │ │ +51 boost::tie(bn, fg) = gfg._e_l_i_m_i_n_a_t_e_P_a_r_t_i_a_l_S_e_q_u_e_n_t_i_a_l(variables, _E_l_i_m_i_n_a_t_e_Q_R); │ │ │ │ │ +52 //fg->print("fg"); │ │ │ │ │ +53 │ │ │ │ │ +54 JacobianFactor::operator=(_J_a_c_o_b_i_a_n_F_a_c_t_o_r(*fg)); │ │ │ │ │ +55 } │ │ │ │ │ +56}; │ │ │ │ │ +57// end class JacobianFactorQR │ │ │ │ │ +58 │ │ │ │ │ +59}// end namespace gtsam │ │ │ │ │ +_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ +_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ +JacobianFactor class with fixed sized blcoks. │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_E_l_i_m_i_n_a_t_e_Q_R │ │ │ │ │ +friend GTSAM_EXPORT std::pair< boost::shared_ptr< GaussianConditional >, │ │ │ │ │ +shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering │ │ │ │ │ +&keys) │ │ │ │ │ +Densely partially eliminate with QR factorization, this is usually provided as │ │ │ │ │ +an argument to one of ... │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.cpp:789 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_:_:_e_l_i_m_i_n_a_t_e_P_a_r_t_i_a_l_S_e_q_u_e_n_t_i_a_l │ │ │ │ │ +std::pair< boost::shared_ptr< BayesNetType >, boost::shared_ptr< │ │ │ │ │ +FactorGraphType > > eliminatePartialSequential(const Ordering &ordering, const │ │ │ │ │ +Eliminate &function=EliminationTraitsType::DefaultEliminate, │ │ │ │ │ +OptionalVariableIndex variableIndex=boost::none) const │ │ │ │ │ +Do sequential elimination of some variables, in ordering provided, to produce a │ │ │ │ │ +Bayes net and a remai... │ │ │ │ │ +DDeeffiinniittiioonn EliminateableFactorGraph-inst.h:154 │ │ │ │ │ _g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s │ │ │ │ │ const KeyVector & keys() const │ │ │ │ │ Access the factor's involved variable keys. │ │ │ │ │ DDeeffiinniittiioonn Factor.h:140 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ -Nonlinear factor base class. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_K_a_r_c_h_e_r_M_e_a_n_F_a_c_t_o_r_:_:_K_a_r_c_h_e_r_M_e_a_n_F_a_c_t_o_r │ │ │ │ │ -KarcherMeanFactor(const CONTAINER &keys, int d=D, boost::optional< double > │ │ │ │ │ -beta=boost::none) │ │ │ │ │ -Construct from given keys. │ │ │ │ │ -DDeeffiinniittiioonn KarcherMeanFactor-inl.h:60 │ │ │ │ │ -_V_a_l_u_e_s │ │ │ │ │ -In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ -a non-linear function... │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l │ │ │ │ │ +Character and index key used to refer to variables. │ │ │ │ │ +DDeeffiinniittiioonn Symbol.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:81 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d │ │ │ │ │ +void add(const GaussianFactor &factor) │ │ │ │ │ +Add a factor by value - makes a copy. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:127 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ +A Gaussian factor in the squared-error form. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:91 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ +JacobianFactor with constant sized blocks Provides raw memory access versions │ │ │ │ │ +of linear operator. │ │ │ │ │ +DDeeffiinniittiioonn RegularJacobianFactor.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q_R │ │ │ │ │ +JacobianFactor for Schur complement that uses Q noise model. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactorQR.h:21 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q_R_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q_R │ │ │ │ │ +JacobianFactorQR(const KeyVector &keys, const std::vector< MatrixZD, Eigen:: │ │ │ │ │ +aligned_allocator< MatrixZD > > &FBlocks, const Matrix &E, const Matrix3 &P, │ │ │ │ │ +const Vector &b, const SharedDiagonal &model=SharedDiagonal()) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactorQR.h:31 │ │ │ │ │ +_S_y_m_b_o_l_._h │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * KKaarrcchheerrMMeeaannFFaaccttoorr--iinnll..hh │ │ │ │ │ + * JJaaccoobbiiaannFFaaccttoorrQQRR..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01310.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/StereoFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/RegularImplicitSchurFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,47 +96,46 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
StereoFactor.h File Reference
│ │ │ │ +
RegularImplicitSchurFactor.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

A non-linear factor for stereo measurements. │ │ │ │ +

A subclass of GaussianFactor specialized to structureless SFM. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::GenericStereoFactor< POSE, LANDMARK >
 A Generic Stereo Factor. More...
class  gtsam::RegularImplicitSchurFactor< CAMERA >
 RegularImplicitSchurFactor. More...
 
struct  gtsam::traits< GenericStereoFactor< T1, T2 > >
 traits More...
struct  gtsam::traits< RegularImplicitSchurFactor< CAMERA > >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

A non-linear factor for stereo measurements.

│ │ │ │ -
Author
Alireza Fathi
│ │ │ │ +

A subclass of GaussianFactor specialized to structureless SFM.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │ -Chris Beall
│ │ │ │ +Luca Carlone
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,29 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -StereoFactor.h File Reference │ │ │ │ │ -A non-linear factor for stereo measurements. _M_o_r_e_._._. │ │ │ │ │ +RegularImplicitSchurFactor.h File Reference │ │ │ │ │ +A subclass of GaussianFactor specialized to structureless SFM. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_<_ _P_O_S_E_,_ _L_A_N_D_M_A_R_K_ _> │ │ │ │ │ -  A Generic Stereo _F_a_c_t_o_r. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_<_ _C_A_M_E_R_A_ _> │ │ │ │ │ +  _R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_<_ _T_1_,_ _T_2_ _>_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_<_ _C_A_M_E_R_A_ _>_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A non-linear factor for stereo measurements. │ │ │ │ │ +A subclass of GaussianFactor specialized to structureless SFM. │ │ │ │ │ Author │ │ │ │ │ - Alireza Fathi │ │ │ │ │ - Chris Beall │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Luca Carlone │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _S_t_e_r_e_o_F_a_c_t_o_r_._h │ │ │ │ │ + * _R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01310.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,4 @@ │ │ │ │ │ var a01310 = [ │ │ │ │ │ - ["gtsam::traits< GenericStereoFactor< T1, T2 > >", "a04964.html", null] │ │ │ │ │ + ["gtsam::RegularImplicitSchurFactor< CAMERA >", "a04912.html", "a04912"], │ │ │ │ │ + ["gtsam::traits< RegularImplicitSchurFactor< CAMERA > >", "a04916.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01310_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/StereoFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/RegularImplicitSchurFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,218 +98,587 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
StereoFactor.h
│ │ │ │ +
RegularImplicitSchurFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ -
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ -
4 * Atlanta, Georgia 30332-0415
│ │ │ │ -
5 * All Rights Reserved
│ │ │ │ -
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ -
7
│ │ │ │ -
8 * See LICENSE for the license information
│ │ │ │ -
9
│ │ │ │ -
10 * -------------------------------------------------------------------------- */
│ │ │ │ -
11
│ │ │ │ -
19#pragma once
│ │ │ │ +Go to the documentation of this file.
1
│ │ │ │ +
8#pragma once
│ │ │ │ +
9
│ │ │ │ +
10#include <gtsam/geometry/CameraSet.h>
│ │ │ │ +
11#include <gtsam/linear/JacobianFactor.h>
│ │ │ │ +
12#include <gtsam/linear/VectorValues.h>
│ │ │ │ +
13
│ │ │ │ +
14#include <iosfwd>
│ │ │ │ +
15#include <map>
│ │ │ │ +
16#include <string>
│ │ │ │ +
17#include <vector>
│ │ │ │ +
18
│ │ │ │ +
19namespace gtsam {
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
22#include <gtsam/geometry/StereoCamera.h>
│ │ │ │ -
23
│ │ │ │ -
24namespace gtsam {
│ │ │ │ -
25
│ │ │ │ -
30template<class POSE, class LANDMARK>
│ │ │ │ -
│ │ │ │ -
31class GenericStereoFactor: public NoiseModelFactorN<POSE, LANDMARK> {
│ │ │ │ -
32private:
│ │ │ │ -
33
│ │ │ │ -
34 // Keep a copy of measurement and calibration for I/O
│ │ │ │ -
35 StereoPoint2 measured_;
│ │ │ │ -
36 Cal3_S2Stereo::shared_ptr K_;
│ │ │ │ -
37 boost::optional<POSE> body_P_sensor_;
│ │ │ │ -
38
│ │ │ │ -
39 // verbosity handling for Cheirality Exceptions
│ │ │ │ -
40 bool throwCheirality_;
│ │ │ │ -
41 bool verboseCheirality_;
│ │ │ │ -
42
│ │ │ │ -
43public:
│ │ │ │ +
38template<class CAMERA>
│ │ │ │ +
│ │ │ │ +
39class RegularImplicitSchurFactor: public GaussianFactor {
│ │ │ │ +
40
│ │ │ │ +
41public:
│ │ │ │ +
42 typedef RegularImplicitSchurFactor This;
│ │ │ │ +
43 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │
44
│ │ │ │ -
45 // shorthand for base class type
│ │ │ │ -
46 typedef NoiseModelFactorN<POSE, LANDMARK> Base;
│ │ │ │ -
47 typedef GenericStereoFactor<POSE, LANDMARK> This;
│ │ │ │ -
48 typedef boost::shared_ptr<GenericStereoFactor> shared_ptr;
│ │ │ │ -
49 typedef POSE CamPose;
│ │ │ │ -
50
│ │ │ │ -
│ │ │ │ -
54 GenericStereoFactor() : K_(new Cal3_S2Stereo(444, 555, 666, 777, 888, 1.0)),
│ │ │ │ -
55 throwCheirality_(false), verboseCheirality_(false) {}
│ │ │ │ -
│ │ │ │ -
56
│ │ │ │ +
45protected:
│ │ │ │ +
46
│ │ │ │ +
47 // This factor is closely related to a CameraSet
│ │ │ │ +
48 typedef CameraSet<CAMERA> Set;
│ │ │ │ +
49
│ │ │ │ +
50 typedef typename CAMERA::Measurement Z;
│ │ │ │ +
51 static const int D = traits<CAMERA>::dimension;
│ │ │ │ +
52 static const int ZDim = traits<Z>::dimension;
│ │ │ │ +
53
│ │ │ │ +
54 typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
│ │ │ │ +
55 typedef Eigen::Matrix<double, D, D> MatrixDD;
│ │ │ │ +
56 typedef std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> > FBlocks;
│ │ │ │ +
57
│ │ │ │ +
58 FBlocks FBlocks_;
│ │ │ │ +
59 const Matrix PointCovariance_;
│ │ │ │ +
60 const Matrix E_;
│ │ │ │ +
61 const Vector b_;
│ │ │ │ +
62
│ │ │ │ +
63public:
│ │ │ │ +
64
│ │ │ │
│ │ │ │ -
66 GenericStereoFactor(const StereoPoint2& measured, const SharedNoiseModel& model,
│ │ │ │ -
67 Key poseKey, Key landmarkKey, const Cal3_S2Stereo::shared_ptr& K,
│ │ │ │ -
68 boost::optional<POSE> body_P_sensor = boost::none) :
│ │ │ │ -
69 Base(model, poseKey, landmarkKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
│ │ │ │ -
70 throwCheirality_(false), verboseCheirality_(false) {}
│ │ │ │ -
│ │ │ │ -
71
│ │ │ │ -
│ │ │ │ -
83 GenericStereoFactor(const StereoPoint2& measured, const SharedNoiseModel& model,
│ │ │ │ -
84 Key poseKey, Key landmarkKey, const Cal3_S2Stereo::shared_ptr& K,
│ │ │ │ -
85 bool throwCheirality, bool verboseCheirality,
│ │ │ │ -
86 boost::optional<POSE> body_P_sensor = boost::none) :
│ │ │ │ -
87 Base(model, poseKey, landmarkKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
│ │ │ │ -
88 throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
│ │ │ │ -
│ │ │ │ -
89
│ │ │ │ -
91 ~GenericStereoFactor() override {}
│ │ │ │ -
92
│ │ │ │ -
│ │ │ │ -
94 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
95 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ -
96 gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
│ │ │ │ -
│ │ │ │ -
97
│ │ │ │ -
│ │ │ │ -
103 void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ -
104 Base::print(s, keyFormatter);
│ │ │ │ -
105 measured_.print(s + ".z");
│ │ │ │ -
106 if(this->body_P_sensor_)
│ │ │ │ -
107 this->body_P_sensor_->print(" sensor pose in body frame: ");
│ │ │ │ -
108 }
│ │ │ │ -
│ │ │ │ -
109
│ │ │ │ -
│ │ │ │ -
113 bool equals(const NonlinearFactor& f, double tol = 1e-9) const override {
│ │ │ │ -
114 const GenericStereoFactor* e = dynamic_cast<const GenericStereoFactor*> (&f);
│ │ │ │ -
115 return e
│ │ │ │ -
116 && Base::equals(f)
│ │ │ │ -
117 && measured_.equals(e->measured_, tol)
│ │ │ │ -
118 && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
│ │ │ │ -
119 }
│ │ │ │ -
│ │ │ │ -
120
│ │ │ │ -
│ │ │ │ -
122 Vector evaluateError(const Pose3& pose, const Point3& point,
│ │ │ │ -
123 boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
│ │ │ │ -
124 try {
│ │ │ │ -
125 if(body_P_sensor_) {
│ │ │ │ -
126 if(H1) {
│ │ │ │ -
127 gtsam::Matrix H0;
│ │ │ │ -
128 StereoCamera stereoCam(pose.compose(*body_P_sensor_, H0), K_);
│ │ │ │ -
129 StereoPoint2 reprojectionError(stereoCam.project(point, H1, H2) - measured_);
│ │ │ │ -
130 *H1 = *H1 * H0;
│ │ │ │ -
131 return reprojectionError.vector();
│ │ │ │ -
132 } else {
│ │ │ │ -
133 StereoCamera stereoCam(pose.compose(*body_P_sensor_), K_);
│ │ │ │ -
134 return (stereoCam.project(point, H1, H2) - measured_).vector();
│ │ │ │ -
135 }
│ │ │ │ -
136 } else {
│ │ │ │ -
137 StereoCamera stereoCam(pose, K_);
│ │ │ │ -
138 return (stereoCam.project(point, H1, H2) - measured_).vector();
│ │ │ │ -
139 }
│ │ │ │ -
140 } catch(StereoCheiralityException& e) {
│ │ │ │ -
141 if (H1) *H1 = Matrix::Zero(3,6);
│ │ │ │ -
142 if (H2) *H2 = Z_3x3;
│ │ │ │ -
143 if (verboseCheirality_)
│ │ │ │ -
144 std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
│ │ │ │ -
145 " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
│ │ │ │ -
146 if (throwCheirality_)
│ │ │ │ -
147 throw StereoCheiralityException(this->key2());
│ │ │ │ -
148 }
│ │ │ │ -
149 return Vector3::Constant(2.0 * K_->fx());
│ │ │ │ -
150 }
│ │ │ │ -
│ │ │ │ -
151
│ │ │ │ -
│ │ │ │ -
153 const StereoPoint2& measured() const {
│ │ │ │ -
154 return measured_;
│ │ │ │ -
155 }
│ │ │ │ -
│ │ │ │ -
156
│ │ │ │ -
│ │ │ │ -
158 inline const Cal3_S2Stereo::shared_ptr calibration() const {
│ │ │ │ -
159 return K_;
│ │ │ │ +
66 RegularImplicitSchurFactor() {
│ │ │ │ +
67 }
│ │ │ │ +
│ │ │ │ +
68
│ │ │ │ +
70
│ │ │ │ +
│ │ │ │ +
80 RegularImplicitSchurFactor(const KeyVector& keys, const FBlocks& Fs,
│ │ │ │ +
81 const Matrix& E, const Matrix& P, const Vector& b)
│ │ │ │ +
82 : GaussianFactor(keys), FBlocks_(Fs), PointCovariance_(P), E_(E), b_(b) {}
│ │ │ │ +
│ │ │ │ +
83
│ │ │ │ +
│ │ │ │ +
85 ~RegularImplicitSchurFactor() override {
│ │ │ │ +
86 }
│ │ │ │ +
│ │ │ │ +
87
│ │ │ │ +
88 const FBlocks& Fs() const {
│ │ │ │ +
89 return FBlocks_;
│ │ │ │ +
90 }
│ │ │ │ +
91
│ │ │ │ +
92 const Matrix& E() const {
│ │ │ │ +
93 return E_;
│ │ │ │ +
94 }
│ │ │ │ +
95
│ │ │ │ +
96 const Vector& b() const {
│ │ │ │ +
97 return b_;
│ │ │ │ +
98 }
│ │ │ │ +
99
│ │ │ │ +
100 const Matrix& getPointCovariance() const {
│ │ │ │ +
101 return PointCovariance_;
│ │ │ │ +
102 }
│ │ │ │ +
103
│ │ │ │ +
│ │ │ │ +
105 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
│ │ │ │ +
106 DefaultKeyFormatter) const override {
│ │ │ │ +
107 std::cout << " RegularImplicitSchurFactor " << std::endl;
│ │ │ │ +
108 Factor::print(s);
│ │ │ │ +
109 for (size_t pos = 0; pos < size(); ++pos) {
│ │ │ │ +
110 std::cout << "Fblock:\n" << FBlocks_[pos] << std::endl;
│ │ │ │ +
111 }
│ │ │ │ +
112 std::cout << "PointCovariance:\n" << PointCovariance_ << std::endl;
│ │ │ │ +
113 std::cout << "E:\n" << E_ << std::endl;
│ │ │ │ +
114 std::cout << "b:\n" << b_.transpose() << std::endl;
│ │ │ │ +
115 }
│ │ │ │ +
│ │ │ │ +
116
│ │ │ │ +
│ │ │ │ +
118 bool equals(const GaussianFactor& lf, double tol) const override {
│ │ │ │ +
119 const This* f = dynamic_cast<const This*>(&lf);
│ │ │ │ +
120 if (!f)
│ │ │ │ +
121 return false;
│ │ │ │ +
122 for (size_t k = 0; k < FBlocks_.size(); ++k) {
│ │ │ │ +
123 if (keys_[k] != f->keys_[k])
│ │ │ │ +
124 return false;
│ │ │ │ +
125 if (!equal_with_abs_tol(FBlocks_[k], f->FBlocks_[k], tol))
│ │ │ │ +
126 return false;
│ │ │ │ +
127 }
│ │ │ │ +
128 return equal_with_abs_tol(PointCovariance_, f->PointCovariance_, tol)
│ │ │ │ +
129 && equal_with_abs_tol(E_, f->E_, tol)
│ │ │ │ +
130 && equal_with_abs_tol(b_, f->b_, tol);
│ │ │ │ +
131 }
│ │ │ │ +
│ │ │ │ +
132
│ │ │ │ +
│ │ │ │ +
134 DenseIndex getDim(const_iterator variable) const override {
│ │ │ │ +
135 return D;
│ │ │ │ +
136 }
│ │ │ │ +
│ │ │ │ +
137
│ │ │ │ +
│ │ │ │ +
138 void updateHessian(const KeyVector& keys,
│ │ │ │ +
139 SymmetricBlockMatrix* info) const override {
│ │ │ │ +
140 throw std::runtime_error(
│ │ │ │ +
141 "RegularImplicitSchurFactor::updateHessian non implemented");
│ │ │ │ +
142 }
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
143 Matrix augmentedJacobian() const override {
│ │ │ │ +
144 throw std::runtime_error(
│ │ │ │ +
145 "RegularImplicitSchurFactor::augmentedJacobian non implemented");
│ │ │ │ +
146 return Matrix();
│ │ │ │ +
147 }
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
148 std::pair<Matrix, Vector> jacobian() const override {
│ │ │ │ +
149 throw std::runtime_error(
│ │ │ │ +
150 "RegularImplicitSchurFactor::jacobian non implemented");
│ │ │ │ +
151 return std::make_pair(Matrix(), Vector());
│ │ │ │ +
152 }
│ │ │ │ +
│ │ │ │ +
153
│ │ │ │ +
│ │ │ │ +
155 Matrix augmentedInformation() const override {
│ │ │ │ +
156 // Do the Schur complement
│ │ │ │ +
157 SymmetricBlockMatrix augmentedHessian =
│ │ │ │ +
158 Set::SchurComplement(FBlocks_, E_, b_);
│ │ │ │ +
159 return augmentedHessian.selfadjointView();
│ │ │ │
160 }
│ │ │ │
│ │ │ │
161
│ │ │ │ -
163 inline bool verboseCheirality() const { return verboseCheirality_; }
│ │ │ │ -
164
│ │ │ │ -
166 inline bool throwCheirality() const { return throwCheirality_; }
│ │ │ │ -
167
│ │ │ │ -
168private:
│ │ │ │ -
170 friend class boost::serialization::access;
│ │ │ │ -
171 template<class Archive>
│ │ │ │ -
172 void serialize(Archive & ar, const unsigned int /*version*/) {
│ │ │ │ -
173 // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ -
174 ar & boost::serialization::make_nvp("NoiseModelFactor2",
│ │ │ │ -
175 boost::serialization::base_object<Base>(*this));
│ │ │ │ -
176 ar & BOOST_SERIALIZATION_NVP(measured_);
│ │ │ │ -
177 ar & BOOST_SERIALIZATION_NVP(K_);
│ │ │ │ -
178 ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
│ │ │ │ -
179 ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
│ │ │ │ -
180 ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
│ │ │ │ -
181 }
│ │ │ │ -
182};
│ │ │ │ -
│ │ │ │ -
183
│ │ │ │ -
185template<class T1, class T2>
│ │ │ │ -
186struct traits<GenericStereoFactor<T1, T2> > : public Testable<GenericStereoFactor<T1, T2> > {};
│ │ │ │ -
187
│ │ │ │ -
188} // \ namespace gtsam
│ │ │ │ -
StereoCamera.h
A Stereo Camera based on two Simple Cameras.
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
│ │ │ │ +
163 Matrix information() const override {
│ │ │ │ +
164 Matrix augmented = augmentedInformation();
│ │ │ │ +
165 int m = this->keys_.size();
│ │ │ │ +
166 size_t M = D * m;
│ │ │ │ +
167 return augmented.block(0, 0, M, M);
│ │ │ │ +
168 }
│ │ │ │ +
│ │ │ │ +
169
│ │ │ │ +
171 using GaussianFactor::hessianDiagonal;
│ │ │ │ +
172
│ │ │ │ +
│ │ │ │ +
174 void hessianDiagonalAdd(VectorValues &d) const override {
│ │ │ │ +
175 // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
│ │ │ │ +
176 for (size_t k = 0; k < size(); ++k) { // for each camera
│ │ │ │ +
177 Key j = keys_[k];
│ │ │ │ +
178
│ │ │ │ +
179 // Calculate Fj'*Ej for the current camera (observing a single point)
│ │ │ │ +
180 // D x 3 = (D x ZDim) * (ZDim x 3)
│ │ │ │ +
181 const MatrixZD& Fj = FBlocks_[k];
│ │ │ │ +
182 Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
│ │ │ │ +
183 * E_.block<ZDim, 3>(ZDim * k, 0);
│ │ │ │ +
184
│ │ │ │ +
185 Eigen::Matrix<double, D, 1> dj;
│ │ │ │ +
186 for (int k = 0; k < D; ++k) { // for each diagonal element of the camera hessian
│ │ │ │ +
187 // Vector column_k_Fj = Fj.col(k);
│ │ │ │ +
188 dj(k) = Fj.col(k).squaredNorm(); // dot(column_k_Fj, column_k_Fj);
│ │ │ │ +
189 // Vector column_k_FtE = FtE.row(k);
│ │ │ │ +
190 // (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1)
│ │ │ │ +
191 dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose();
│ │ │ │ +
192 }
│ │ │ │ +
193
│ │ │ │ +
194 auto result = d.emplace(j, dj);
│ │ │ │ +
195 if(!result.second) {
│ │ │ │ +
196 result.first->second += dj;
│ │ │ │ +
197 }
│ │ │ │ +
198 }
│ │ │ │ +
199 }
│ │ │ │ +
│ │ │ │ +
200
│ │ │ │ +
│ │ │ │ +
205 void hessianDiagonal(double* d) const override {
│ │ │ │ +
206 // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
│ │ │ │ +
207 // Use eigen magic to access raw memory
│ │ │ │ +
208 typedef Eigen::Matrix<double, D, 1> DVector;
│ │ │ │ +
209 typedef Eigen::Map<DVector> DMap;
│ │ │ │ +
210
│ │ │ │ +
211 for (size_t pos = 0; pos < size(); ++pos) { // for each camera in the factor
│ │ │ │ +
212 Key j = keys_[pos];
│ │ │ │ +
213
│ │ │ │ +
214 // Calculate Fj'*Ej for the current camera (observing a single point)
│ │ │ │ +
215 // D x 3 = (D x ZDim) * (ZDim x 3)
│ │ │ │ +
216 const MatrixZD& Fj = FBlocks_[pos];
│ │ │ │ +
217 Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
│ │ │ │ +
218 * E_.block<ZDim, 3>(ZDim * pos, 0);
│ │ │ │ +
219
│ │ │ │ +
220 DVector dj;
│ │ │ │ +
221 for (int k = 0; k < D; ++k) { // for each diagonal element of the camera hessian
│ │ │ │ +
222 dj(k) = Fj.col(k).squaredNorm();
│ │ │ │ +
223 // (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1)
│ │ │ │ +
224 dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose();
│ │ │ │ +
225 }
│ │ │ │ +
226 DMap(d + D * j) += dj;
│ │ │ │ +
227 }
│ │ │ │ +
228 }
│ │ │ │ +
│ │ │ │ +
229
│ │ │ │ +
│ │ │ │ +
231 std::map<Key, Matrix> hessianBlockDiagonal() const override {
│ │ │ │ +
232 std::map<Key, Matrix> blocks;
│ │ │ │ +
233 // F'*(I - E*P*E')*F
│ │ │ │ +
234 for (size_t pos = 0; pos < size(); ++pos) {
│ │ │ │ +
235 Key j = keys_[pos];
│ │ │ │ +
236 // F'*F - F'*E*P*E'*F e.g. (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9)
│ │ │ │ +
237 const MatrixZD& Fj = FBlocks_[pos];
│ │ │ │ +
238 // Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
│ │ │ │ +
239 // * E_.block<ZDim, 3>(ZDim * pos, 0);
│ │ │ │ +
240 // blocks[j] = Fj.transpose() * Fj
│ │ │ │ +
241 // - FtE * PointCovariance_ * FtE.transpose();
│ │ │ │ +
242
│ │ │ │ +
243 const Matrix23& Ej = E_.block<ZDim, 3>(ZDim * pos, 0);
│ │ │ │ +
244 blocks[j] = Fj.transpose()
│ │ │ │ +
245 * (Fj - Ej * PointCovariance_ * Ej.transpose() * Fj);
│ │ │ │ +
246
│ │ │ │ +
247 // F'*(I - E*P*E')*F, TODO: this should work, but it does not :-(
│ │ │ │ +
248 // static const Eigen::Matrix<double, ZDim, ZDim> I2 = eye(ZDim);
│ │ │ │ +
249 // Matrix2 Q = //
│ │ │ │ +
250 // I2 - E_.block<ZDim, 3>(ZDim * pos, 0) * PointCovariance_ * E_.block<ZDim, 3>(ZDim * pos, 0).transpose();
│ │ │ │ +
251 // blocks[j] = Fj.transpose() * Q * Fj;
│ │ │ │ +
252 }
│ │ │ │ +
253 return blocks;
│ │ │ │ +
254 }
│ │ │ │ +
│ │ │ │ +
255
│ │ │ │ +
│ │ │ │ +
256 GaussianFactor::shared_ptr clone() const override {
│ │ │ │ +
257 return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
│ │ │ │ +
258 FBlocks_, PointCovariance_, E_, b_);
│ │ │ │ +
259 throw std::runtime_error(
│ │ │ │ +
260 "RegularImplicitSchurFactor::clone non implemented");
│ │ │ │ +
261 }
│ │ │ │ +
│ │ │ │ +
262
│ │ │ │ +
│ │ │ │ +
263 GaussianFactor::shared_ptr negate() const override {
│ │ │ │ +
264 return boost::make_shared<RegularImplicitSchurFactor<CAMERA> >(keys_,
│ │ │ │ +
265 FBlocks_, PointCovariance_, E_, b_);
│ │ │ │ +
266 throw std::runtime_error(
│ │ │ │ +
267 "RegularImplicitSchurFactor::negate non implemented");
│ │ │ │ +
268 }
│ │ │ │ +
│ │ │ │ +
269
│ │ │ │ +
270 // Raw Vector version of y += F'*alpha*(I - E*P*E')*F*x, for testing
│ │ │ │ +
271 static
│ │ │ │ +
272 void multiplyHessianAdd(const Matrix& F, const Matrix& E,
│ │ │ │ +
273 const Matrix& PointCovariance, double alpha, const Vector& x, Vector& y) {
│ │ │ │ +
274 Vector e1 = F * x;
│ │ │ │ +
275 Vector d1 = E.transpose() * e1;
│ │ │ │ +
276 Vector d2 = PointCovariance * d1;
│ │ │ │ +
277 Vector e2 = E * d2;
│ │ │ │ +
278 Vector e3 = alpha * (e1 - e2);
│ │ │ │ +
279 y += F.transpose() * e3;
│ │ │ │ +
280 }
│ │ │ │ +
281
│ │ │ │ +
282 typedef std::vector<Vector2, Eigen::aligned_allocator<Vector2>> Error2s;
│ │ │ │ +
283
│ │ │ │ +
│ │ │ │ +
287 void projectError2(const Error2s& e1, Error2s& e2) const {
│ │ │ │ +
288
│ │ │ │ +
289 // d1 = E.transpose() * (e1-ZDim*b) = (3*2m)*2m
│ │ │ │ +
290 Vector3 d1;
│ │ │ │ +
291 d1.setZero();
│ │ │ │ +
292 for (size_t k = 0; k < size(); k++)
│ │ │ │ +
293 d1 += E_.block<ZDim, 3>(ZDim * k, 0).transpose()
│ │ │ │ +
294 * (e1[k] - ZDim * b_.segment<ZDim>(k * ZDim));
│ │ │ │ +
295
│ │ │ │ +
296 // d2 = E.transpose() * e1 = (3*2m)*2m
│ │ │ │ +
297 Vector3 d2 = PointCovariance_ * d1;
│ │ │ │ +
298
│ │ │ │ +
299 // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
│ │ │ │ +
300 for (size_t k = 0; k < size(); k++)
│ │ │ │ +
301 e2[k] = e1[k] - ZDim * b_.segment<ZDim>(k * ZDim)
│ │ │ │ +
302 - E_.block<ZDim, 3>(ZDim * k, 0) * d2;
│ │ │ │ +
303 }
│ │ │ │ +
│ │ │ │ +
304
│ │ │ │ +
305 /*
│ │ │ │ +
306 * This definition matches the linearized error in the Hessian Factor:
│ │ │ │ +
307 * LinError(x) = x'*H*x - 2*x'*eta + f
│ │ │ │ +
308 * with:
│ │ │ │ +
309 * H = F' * (I-E'*P*E) * F = F' * Q * F
│ │ │ │ +
310 * eta = F' * (I-E'*P*E) * b = F' * Q * b
│ │ │ │ +
311 * f = nonlinear error
│ │ │ │ +
312 * (x'*H*x - 2*x'*eta + f) = x'*F'*Q*F*x - 2*x'*F'*Q *b + f = x'*F'*Q*(F*x - 2*b) + f
│ │ │ │ +
313 */
│ │ │ │ +
314 double error(const VectorValues& x) const override {
│ │ │ │ +
315
│ │ │ │ +
316 // resize does not do malloc if correct size
│ │ │ │ +
317 e1.resize(size());
│ │ │ │ +
318 e2.resize(size());
│ │ │ │ +
319
│ │ │ │ +
320 // e1 = F * x - b = (2m*dm)*dm
│ │ │ │ +
321 for (size_t k = 0; k < size(); ++k)
│ │ │ │ +
322 e1[k] = FBlocks_[k] * x.at(keys_[k]);
│ │ │ │ +
323 projectError2(e1, e2);
│ │ │ │ +
324
│ │ │ │ +
325 double result = 0;
│ │ │ │ +
326 for (size_t k = 0; k < size(); ++k)
│ │ │ │ +
327 result += dot(e1[k], e2[k]);
│ │ │ │ +
328
│ │ │ │ +
329 double f = b_.squaredNorm();
│ │ │ │ +
330 return 0.5 * (result + f);
│ │ │ │ +
331 }
│ │ │ │ +
332
│ │ │ │ +
333 // needed to be GaussianFactor - (I - E*P*E')*(F*x - b)
│ │ │ │ +
334 // This is wrong and does not match the definition in Hessian,
│ │ │ │ +
335 // but it matches the definition of the Jacobian factor (JF)
│ │ │ │ +
336 double errorJF(const VectorValues& x) const {
│ │ │ │ +
337
│ │ │ │ +
338 // resize does not do malloc if correct size
│ │ │ │ +
339 e1.resize(size());
│ │ │ │ +
340 e2.resize(size());
│ │ │ │ +
341
│ │ │ │ +
342 // e1 = F * x - b = (2m*dm)*dm
│ │ │ │ +
343 for (size_t k = 0; k < size(); ++k)
│ │ │ │ +
344 e1[k] = FBlocks_[k] * x.at(keys_[k]) - b_.segment<ZDim>(k * ZDim);
│ │ │ │ +
345 projectError(e1, e2);
│ │ │ │ +
346
│ │ │ │ +
347 double result = 0;
│ │ │ │ +
348 for (size_t k = 0; k < size(); ++k)
│ │ │ │ +
349 result += dot(e2[k], e2[k]);
│ │ │ │ +
350
│ │ │ │ +
351 // std::cout << "implicitFactor::error result " << result << std::endl;
│ │ │ │ +
352 return 0.5 * result;
│ │ │ │ +
353 }
│ │ │ │ +
│ │ │ │ +
357 void projectError(const Error2s& e1, Error2s& e2) const {
│ │ │ │ +
358
│ │ │ │ +
359 // d1 = E.transpose() * e1 = (3*2m)*2m
│ │ │ │ +
360 Vector3 d1;
│ │ │ │ +
361 d1.setZero();
│ │ │ │ +
362 for (size_t k = 0; k < size(); k++)
│ │ │ │ +
363 d1 += E_.block<ZDim, 3>(ZDim * k, 0).transpose() * e1[k];
│ │ │ │ +
364
│ │ │ │ +
365 // d2 = E.transpose() * e1 = (3*2m)*2m
│ │ │ │ +
366 Vector3 d2 = PointCovariance_ * d1;
│ │ │ │ +
367
│ │ │ │ +
368 // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
│ │ │ │ +
369 for (size_t k = 0; k < size(); k++)
│ │ │ │ +
370 e2[k] = e1[k] - E_.block<ZDim, 3>(ZDim * k, 0) * d2;
│ │ │ │ +
371 }
│ │ │ │ +
│ │ │ │ +
372
│ │ │ │ +
374 mutable Error2s e1, e2;
│ │ │ │ +
375
│ │ │ │ +
│ │ │ │ +
380 void multiplyHessianAdd(double alpha, const double* x, double* y) const {
│ │ │ │ +
381
│ │ │ │ +
382 // Use eigen magic to access raw memory
│ │ │ │ +
383 typedef Eigen::Matrix<double, D, 1> DVector;
│ │ │ │ +
384 typedef Eigen::Map<DVector> DMap;
│ │ │ │ +
385 typedef Eigen::Map<const DVector> ConstDMap;
│ │ │ │ +
386
│ │ │ │ +
387 // resize does not do malloc if correct size
│ │ │ │ +
388 e1.resize(size());
│ │ │ │ +
389 e2.resize(size());
│ │ │ │ +
390
│ │ │ │ +
391 // e1 = F * x = (2m*dm)*dm
│ │ │ │ +
392 for (size_t k = 0; k < size(); ++k) {
│ │ │ │ +
393 Key key = keys_[k];
│ │ │ │ +
394 e1[k] = FBlocks_[k] * ConstDMap(x + D * key);
│ │ │ │ +
395 }
│ │ │ │ +
396
│ │ │ │ +
397 projectError(e1, e2);
│ │ │ │ +
398
│ │ │ │ +
399 // y += F.transpose()*e2 = (2d*2m)*2m
│ │ │ │ +
400 for (size_t k = 0; k < size(); ++k) {
│ │ │ │ +
401 Key key = keys_[k];
│ │ │ │ +
402 DMap(y + D * key) += FBlocks_[k].transpose() * alpha * e2[k];
│ │ │ │ +
403 }
│ │ │ │ +
404 }
│ │ │ │ +
│ │ │ │ +
405
│ │ │ │ +
406 void multiplyHessianAdd(double alpha, const double* x, double* y,
│ │ │ │ +
407 std::vector<size_t> keys) const {
│ │ │ │ +
408 }
│ │ │ │ +
409
│ │ │ │ +
│ │ │ │ +
413 void multiplyHessianAdd(double alpha, const VectorValues& x,
│ │ │ │ +
414 VectorValues& y) const override {
│ │ │ │ +
415
│ │ │ │ +
416 // resize does not do malloc if correct size
│ │ │ │ +
417 e1.resize(size());
│ │ │ │ +
418 e2.resize(size());
│ │ │ │ +
419
│ │ │ │ +
420 // e1 = F * x = (2m*dm)*dm
│ │ │ │ +
421 for (size_t k = 0; k < size(); ++k)
│ │ │ │ +
422 e1[k] = FBlocks_[k] * x.at(keys_[k]);
│ │ │ │ +
423
│ │ │ │ +
424 projectError(e1, e2);
│ │ │ │ +
425
│ │ │ │ +
426 // y += F.transpose()*e2 = (2d*2m)*2m
│ │ │ │ +
427 for (size_t k = 0; k < size(); ++k) {
│ │ │ │ +
428 Key key = keys_[k];
│ │ │ │ +
429 static const Vector empty;
│ │ │ │ +
430 std::pair<VectorValues::iterator, bool> it = y.tryInsert(key, empty);
│ │ │ │ +
431 Vector& yi = it.first->second;
│ │ │ │ +
432 // Create the value as a zero vector if it does not exist.
│ │ │ │ +
433 if (it.second)
│ │ │ │ +
434 yi = Vector::Zero(FBlocks_[k].cols());
│ │ │ │ +
435 yi += FBlocks_[k].transpose() * alpha * e2[k];
│ │ │ │ +
436 }
│ │ │ │ +
437 }
│ │ │ │ +
│ │ │ │ +
438
│ │ │ │ +
│ │ │ │ +
442 void multiplyHessianDummy(double alpha, const VectorValues& x,
│ │ │ │ +
443 VectorValues& y) const {
│ │ │ │ +
444
│ │ │ │ +
445 for (size_t k = 0; k < size(); ++k) {
│ │ │ │ +
446 static const Vector empty;
│ │ │ │ +
447 Key key = keys_[k];
│ │ │ │ +
448 std::pair<VectorValues::iterator, bool> it = y.tryInsert(key, empty);
│ │ │ │ +
449 Vector& yi = it.first->second;
│ │ │ │ +
450 yi = x.at(key);
│ │ │ │ +
451 }
│ │ │ │ +
452 }
│ │ │ │ +
│ │ │ │ +
453
│ │ │ │ +
│ │ │ │ +
457 VectorValues gradientAtZero() const override {
│ │ │ │ +
458 // calculate Q*b
│ │ │ │ +
459 e1.resize(size());
│ │ │ │ +
460 e2.resize(size());
│ │ │ │ +
461 for (size_t k = 0; k < size(); k++)
│ │ │ │ +
462 e1[k] = b_.segment<ZDim>(ZDim * k);
│ │ │ │ +
463 projectError(e1, e2);
│ │ │ │ +
464
│ │ │ │ +
465 // g = F.transpose()*e2
│ │ │ │ +
466 VectorValues g;
│ │ │ │ +
467 for (size_t k = 0; k < size(); ++k) {
│ │ │ │ +
468 Key key = keys_[k];
│ │ │ │ +
469 g.insert(key, -FBlocks_[k].transpose() * e2[k]);
│ │ │ │ +
470 }
│ │ │ │ +
471
│ │ │ │ +
472 // return it
│ │ │ │ +
473 return g;
│ │ │ │ +
474 }
│ │ │ │ +
│ │ │ │ +
475
│ │ │ │ +
│ │ │ │ +
479 void gradientAtZero(double* d) const override {
│ │ │ │ +
480
│ │ │ │ +
481 // Use eigen magic to access raw memory
│ │ │ │ +
482 typedef Eigen::Matrix<double, D, 1> DVector;
│ │ │ │ +
483 typedef Eigen::Map<DVector> DMap;
│ │ │ │ +
484
│ │ │ │ +
485 // calculate Q*b
│ │ │ │ +
486 e1.resize(size());
│ │ │ │ +
487 e2.resize(size());
│ │ │ │ +
488 for (size_t k = 0; k < size(); k++)
│ │ │ │ +
489 e1[k] = b_.segment<ZDim>(ZDim * k);
│ │ │ │ +
490 projectError(e1, e2);
│ │ │ │ +
491
│ │ │ │ +
492 for (size_t k = 0; k < size(); ++k) { // for each camera in the factor
│ │ │ │ +
493 Key j = keys_[k];
│ │ │ │ +
494 DMap(d + D * j) += -FBlocks_[k].transpose() * e2[k];
│ │ │ │ +
495 }
│ │ │ │ +
496 }
│ │ │ │ +
│ │ │ │ +
497
│ │ │ │ +
│ │ │ │ +
499 Vector gradient(Key key, const VectorValues& x) const override {
│ │ │ │ +
500 throw std::runtime_error(
│ │ │ │ +
501 "gradient for RegularImplicitSchurFactor is not implemented yet");
│ │ │ │ +
502 }
│ │ │ │ +
│ │ │ │ +
503
│ │ │ │ +
504};
│ │ │ │ +
│ │ │ │ +
505// end class RegularImplicitSchurFactor
│ │ │ │ +
506
│ │ │ │ +
507template<class CAMERA>
│ │ │ │ +
508const int RegularImplicitSchurFactor<CAMERA>::D;
│ │ │ │ +
509
│ │ │ │ +
510template<class CAMERA>
│ │ │ │ +
511const int RegularImplicitSchurFactor<CAMERA>::ZDim;
│ │ │ │ +
512
│ │ │ │ +
513// traits
│ │ │ │ +
│ │ │ │ +
514template<class CAMERA> struct traits<RegularImplicitSchurFactor<CAMERA> > : public Testable<
│ │ │ │ +
515 RegularImplicitSchurFactor<CAMERA> > {
│ │ │ │ +
516};
│ │ │ │ +
│ │ │ │ +
517
│ │ │ │ +
518}
│ │ │ │ +
519
│ │ │ │ +
CameraSet.h
Base class to create smart factors on poses or cameras.
│ │ │ │ + │ │ │ │ +
VectorValues.h
Factor Graph Values.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ -
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │ +
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition Matrix.h:81
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
│ │ │ │ +
gtsam::SymmetricBlockMatrix::selfadjointView
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j).
Definition SymmetricBlockMatrix.h:156
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::Cal3_S2Stereo
The most common 5DOF 3D->2D calibration, stereo version.
Definition Cal3_S2Stereo.h:30
│ │ │ │ -
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ -
gtsam::StereoCheiralityException
Definition StereoCamera.h:26
│ │ │ │ -
gtsam::StereoCamera
A stereo camera class, parameterize by left camera pose and stereo calibration.
Definition StereoCamera.h:47
│ │ │ │ -
gtsam::StereoCamera::project
StereoPoint2 project(const Point3 &point) const
Project 3D point to StereoPoint2 (uL,uR,v)
Definition StereoCamera.cpp:32
│ │ │ │ -
gtsam::StereoPoint2
A 2D stereo point, v will be same for rectified images.
Definition StereoPoint2.h:32
│ │ │ │ -
gtsam::StereoPoint2::vector
Vector3 vector() const
convert to vector
Definition StereoPoint2.h:115
│ │ │ │ -
gtsam::StereoPoint2::equals
bool equals(const StereoPoint2 &q, double tol=1e-9) const
equals
Definition StereoPoint2.h:64
│ │ │ │ -
gtsam::StereoPoint2::print
void print(const std::string &s="") const
print
Definition StereoPoint2.cpp:26
│ │ │ │ +
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition CameraSet.h:36
│ │ │ │ +
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
│ │ │ │ +
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
│ │ │ │ +
gtsam::Factor::empty
bool empty() const
Whether the factor is empty (involves zero variables).
Definition Factor.h:128
│ │ │ │
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
│ │ │ │ -
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
│ │ │ │ -
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ -
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ -
gtsam::GenericStereoFactor
A Generic Stereo Factor.
Definition StereoFactor.h:31
│ │ │ │ -
gtsam::GenericStereoFactor::throwCheirality
bool throwCheirality() const
return flag for throwing cheirality exceptions
Definition StereoFactor.h:166
│ │ │ │ -
gtsam::GenericStereoFactor::evaluateError
Vector evaluateError(const Pose3 &pose, const Point3 &point, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
h(x)-z
Definition StereoFactor.h:122
│ │ │ │ -
gtsam::GenericStereoFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
equals
Definition StereoFactor.h:113
│ │ │ │ -
gtsam::GenericStereoFactor::calibration
const Cal3_S2Stereo::shared_ptr calibration() const
return the calibration object
Definition StereoFactor.h:158
│ │ │ │ -
gtsam::GenericStereoFactor::GenericStereoFactor
GenericStereoFactor(const StereoPoint2 &measured, const SharedNoiseModel &model, Key poseKey, Key landmarkKey, const Cal3_S2Stereo::shared_ptr &K, bool throwCheirality, bool verboseCheirality, boost::optional< POSE > body_P_sensor=boost::none)
Constructor with exception-handling flags.
Definition StereoFactor.h:83
│ │ │ │ -
gtsam::GenericStereoFactor::This
GenericStereoFactor< POSE, LANDMARK > This
typedef for this class (with templates)
Definition StereoFactor.h:47
│ │ │ │ -
gtsam::GenericStereoFactor::~GenericStereoFactor
~GenericStereoFactor() override
Virtual destructor.
Definition StereoFactor.h:91
│ │ │ │ -
gtsam::GenericStereoFactor::verboseCheirality
bool verboseCheirality() const
return verbosity
Definition StereoFactor.h:163
│ │ │ │ -
gtsam::GenericStereoFactor::shared_ptr
boost::shared_ptr< GenericStereoFactor > shared_ptr
typedef for shared pointer to this object
Definition StereoFactor.h:48
│ │ │ │ -
gtsam::GenericStereoFactor::measured
const StereoPoint2 & measured() const
return the measured
Definition StereoFactor.h:153
│ │ │ │ -
gtsam::GenericStereoFactor::GenericStereoFactor
GenericStereoFactor()
Default constructor.
Definition StereoFactor.h:54
│ │ │ │ -
gtsam::GenericStereoFactor::access
friend class boost::serialization::access
Serialization function.
Definition StereoFactor.h:170
│ │ │ │ -
gtsam::GenericStereoFactor::GenericStereoFactor
GenericStereoFactor(const StereoPoint2 &measured, const SharedNoiseModel &model, Key poseKey, Key landmarkKey, const Cal3_S2Stereo::shared_ptr &K, boost::optional< POSE > body_P_sensor=boost::none)
Constructor.
Definition StereoFactor.h:66
│ │ │ │ -
gtsam::GenericStereoFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition StereoFactor.h:103
│ │ │ │ -
gtsam::GenericStereoFactor::CamPose
POSE CamPose
typedef for Pose Lie Value type
Definition StereoFactor.h:49
│ │ │ │ -
gtsam::GenericStereoFactor::Base
NoiseModelFactorN< POSE, LANDMARK > Base
typedef for base class
Definition StereoFactor.h:46
│ │ │ │ -
gtsam::GenericStereoFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition StereoFactor.h:94
│ │ │ │ +
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition Factor.h:80
│ │ │ │ +
gtsam::Factor::size
size_t size() const
Definition Factor.h:157
│ │ │ │ +
gtsam::GaussianFactor
An abstract virtual base class for JacobianFactor and HessianFactor.
Definition GaussianFactor.h:39
│ │ │ │ +
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactor.h:42
│ │ │ │ +
gtsam::GaussianFactor::hessianDiagonal
VectorValues hessianDiagonal() const
Return the diagonal of the Hessian for this factor.
Definition GaussianFactor.cpp:35
│ │ │ │ +
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
gtsam::VectorValues::insert
iterator insert(const std::pair< Key, Vector > &key_value)
Insert a vector value with key j.
Definition VectorValues.cpp:91
│ │ │ │ +
gtsam::VectorValues::emplace
std::pair< VectorValues::iterator, bool > emplace(Key j, Args &&... args)
Emplace a vector value with key j.
Definition VectorValues.h:185
│ │ │ │ +
gtsam::VectorValues::at
Vector & at(Key j)
Read/write access to the vector value with key j, throws std::out_of_range if j does not exist,...
Definition VectorValues.h:139
│ │ │ │ +
gtsam::VectorValues::tryInsert
std::pair< iterator, bool > tryInsert(Key j, const Vector &value)
insert that mimics the STL map insert - if the value already exists, the map is not modified and an i...
Definition VectorValues.h:209
│ │ │ │ +
gtsam::RegularImplicitSchurFactor
RegularImplicitSchurFactor.
Definition RegularImplicitSchurFactor.h:39
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::E_
const Matrix E_
The 2m*3 E Jacobian with respect to the point.
Definition RegularImplicitSchurFactor.h:60
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::projectError
void projectError(const Error2s &e1, Error2s &e2) const
Calculate corrected error Q*e = (I - E*P*E')*e.
Definition RegularImplicitSchurFactor.h:357
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::clone
GaussianFactor::shared_ptr clone() const override
Clone a factor (make a deep copy)
Definition RegularImplicitSchurFactor.h:256
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::hessianDiagonalAdd
void hessianDiagonalAdd(VectorValues &d) const override
Add the diagonal of the Hessian for this factor to existing VectorValues.
Definition RegularImplicitSchurFactor.h:174
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::RegularImplicitSchurFactor
RegularImplicitSchurFactor(const KeyVector &keys, const FBlocks &Fs, const Matrix &E, const Matrix &P, const Vector &b)
Construct from blocks of F, E, inv(E'*E), and RHS vector b.
Definition RegularImplicitSchurFactor.h:80
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::RegularImplicitSchurFactor
RegularImplicitSchurFactor()
Constructor.
Definition RegularImplicitSchurFactor.h:66
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::b_
const Vector b_
2m-dimensional RHS vector
Definition RegularImplicitSchurFactor.h:61
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::jacobian
std::pair< Matrix, Vector > jacobian() const override
Return the dense Jacobian and right-hand-side , with the noise models baked into A and b.
Definition RegularImplicitSchurFactor.h:148
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::MatrixDD
Eigen::Matrix< double, D, D > MatrixDD
camera Hessian
Definition RegularImplicitSchurFactor.h:55
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::gradientAtZero
void gradientAtZero(double *d) const override
Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS.
Definition RegularImplicitSchurFactor.h:479
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::ZDim
static const int ZDim
Measurement dimension.
Definition RegularImplicitSchurFactor.h:52
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::information
Matrix information() const override
Compute full information matrix
Definition RegularImplicitSchurFactor.h:163
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::~RegularImplicitSchurFactor
~RegularImplicitSchurFactor() override
Destructor.
Definition RegularImplicitSchurFactor.h:85
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::updateHessian
void updateHessian(const KeyVector &keys, SymmetricBlockMatrix *info) const override
Update an information matrix by adding the information corresponding to this factor (used internally ...
Definition RegularImplicitSchurFactor.h:138
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::FBlocks_
FBlocks FBlocks_
All ZDim*D F blocks (one for each camera)
Definition RegularImplicitSchurFactor.h:58
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::gradientAtZero
VectorValues gradientAtZero() const override
Calculate gradient, which is -F'Q*b, see paper.
Definition RegularImplicitSchurFactor.h:457
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::MatrixZD
Eigen::Matrix< double, ZDim, D > MatrixZD
type of an F block
Definition RegularImplicitSchurFactor.h:54
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::PointCovariance_
const Matrix PointCovariance_
the 3*3 matrix P = inv(E'E) (2*2 if degenerate)
Definition RegularImplicitSchurFactor.h:59
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
Hessian-vector multiply, i.e.
Definition RegularImplicitSchurFactor.h:413
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::hessianBlockDiagonal
std::map< Key, Matrix > hessianBlockDiagonal() const override
Return the block diagonal of the Hessian for this factor.
Definition RegularImplicitSchurFactor.h:231
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::augmentedInformation
Matrix augmentedInformation() const override
Compute full augmented information matrix
Definition RegularImplicitSchurFactor.h:155
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::negate
GaussianFactor::shared_ptr negate() const override
Construct the corresponding anti-factor to negate information stored stored in this factor.
Definition RegularImplicitSchurFactor.h:263
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition RegularImplicitSchurFactor.h:43
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::e1
Error2s e1
Scratch space for multiplyHessianAdd.
Definition RegularImplicitSchurFactor.h:374
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition RegularImplicitSchurFactor.h:105
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::getDim
DenseIndex getDim(const_iterator variable) const override
Degrees of freedom of camera.
Definition RegularImplicitSchurFactor.h:134
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::equals
bool equals(const GaussianFactor &lf, double tol) const override
equals
Definition RegularImplicitSchurFactor.h:118
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::augmentedJacobian
Matrix augmentedJacobian() const override
Return a dense Jacobian matrix, augmented with b with the noise models baked into A and b.
Definition RegularImplicitSchurFactor.h:143
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::This
RegularImplicitSchurFactor This
Typedef to this class.
Definition RegularImplicitSchurFactor.h:42
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::D
static const int D
Camera dimension.
Definition RegularImplicitSchurFactor.h:51
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::projectError2
void projectError2(const Error2s &e1, Error2s &e2) const
Calculate corrected error Q*(e-ZDim*b) = (I - E*P*E')*(e-ZDim*b)
Definition RegularImplicitSchurFactor.h:287
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const double *x, double *y) const
double* Hessian-vector multiply, i.e.
Definition RegularImplicitSchurFactor.h:380
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::hessianDiagonal
void hessianDiagonal(double *d) const override
add the contribution of this factor to the diagonal of the hessian d(output) = d(input) + deltaHessia...
Definition RegularImplicitSchurFactor.h:205
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::multiplyHessianDummy
void multiplyHessianDummy(double alpha, const VectorValues &x, VectorValues &y) const
Dummy version to measure overhead of key access.
Definition RegularImplicitSchurFactor.h:442
│ │ │ │ +
gtsam::RegularImplicitSchurFactor::gradient
Vector gradient(Key key, const VectorValues &x) const override
Gradient wrt a key at any values.
Definition RegularImplicitSchurFactor.h:499
│ │ │ │ +
VectorValues
The Factor::error simply extracts the.
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,317 +1,729 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -StereoFactor.h │ │ │ │ │ +RegularImplicitSchurFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -1/* --------------------------------------------------------------------------- │ │ │ │ │ -- │ │ │ │ │ -2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ -4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -5 * All Rights Reserved │ │ │ │ │ -6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ -7 │ │ │ │ │ -8 * See LICENSE for the license information │ │ │ │ │ +1 │ │ │ │ │ +8#pragma once │ │ │ │ │ 9 │ │ │ │ │ -10 * ------------------------------------------------------------------------- │ │ │ │ │ -- */ │ │ │ │ │ -11 │ │ │ │ │ -19#pragma once │ │ │ │ │ +10#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_m_e_r_a_S_e_t_._h> │ │ │ │ │ +11#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ +12#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ +13 │ │ │ │ │ +14#include │ │ │ │ │ +15#include │ │ │ │ │ +16#include │ │ │ │ │ +17#include │ │ │ │ │ +18 │ │ │ │ │ +19namespace _g_t_s_a_m { │ │ │ │ │ 20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_S_t_e_r_e_o_C_a_m_e_r_a_._h> │ │ │ │ │ -23 │ │ │ │ │ -24namespace _g_t_s_a_m { │ │ │ │ │ -25 │ │ │ │ │ -30template │ │ │ │ │ -_3_1class _G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -32private: │ │ │ │ │ -33 │ │ │ │ │ -34 // Keep a copy of measurement and calibration for I/O │ │ │ │ │ -35 _S_t_e_r_e_o_P_o_i_n_t_2 measured_; │ │ │ │ │ -36 Cal3_S2Stereo::shared_ptr K_; │ │ │ │ │ -37 boost::optional body_P_sensor_; │ │ │ │ │ -38 │ │ │ │ │ -39 // verbosity handling for Cheirality Exceptions │ │ │ │ │ -40 bool throwCheirality_; │ │ │ │ │ -41 bool verboseCheirality_; │ │ │ │ │ -42 │ │ │ │ │ -43public: │ │ │ │ │ +38template │ │ │ │ │ +_3_9class _R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r: public _G_a_u_s_s_i_a_n_F_a_c_t_o_r { │ │ │ │ │ +40 │ │ │ │ │ +41public: │ │ │ │ │ +_4_2 typedef _R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ +_4_3 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ 44 │ │ │ │ │ -45 // shorthand for base class type │ │ │ │ │ -_4_6 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_P_O_S_E_,_ _L_A_N_D_M_A_R_K_> _B_a_s_e; │ │ │ │ │ -_4_7 typedef _G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_<_P_O_S_E_,_ _L_A_N_D_M_A_R_K_> _T_h_i_s; │ │ │ │ │ -_4_8 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -_4_9 typedef POSE _C_a_m_P_o_s_e; │ │ │ │ │ -50 │ │ │ │ │ -_5_4 _G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r() : K_(new _C_a_l_3___S_2_S_t_e_r_e_o(444, 555, 666, 777, 888, 1.0)), │ │ │ │ │ -55 throwCheirality_(false), verboseCheirality_(false) {} │ │ │ │ │ -56 │ │ │ │ │ -_6_6 _G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r(const _S_t_e_r_e_o_P_o_i_n_t_2& _m_e_a_s_u_r_e_d, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& │ │ │ │ │ -model, │ │ │ │ │ -67 _K_e_y poseKey, _K_e_y landmarkKey, const Cal3_S2Stereo::shared_ptr& K, │ │ │ │ │ -68 boost::optional body_P_sensor = boost::none) : │ │ │ │ │ -69 _B_a_s_e(model, poseKey, landmarkKey), measured_(_m_e_a_s_u_r_e_d), K_(K), │ │ │ │ │ -body_P_sensor_(body_P_sensor), │ │ │ │ │ -70 throwCheirality_(false), verboseCheirality_(false) {} │ │ │ │ │ -71 │ │ │ │ │ -_8_3 _G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r(const _S_t_e_r_e_o_P_o_i_n_t_2& _m_e_a_s_u_r_e_d, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& │ │ │ │ │ -model, │ │ │ │ │ -84 _K_e_y poseKey, _K_e_y landmarkKey, const Cal3_S2Stereo::shared_ptr& K, │ │ │ │ │ -85 bool _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y, bool _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y, │ │ │ │ │ -86 boost::optional body_P_sensor = boost::none) : │ │ │ │ │ -87 _B_a_s_e(model, poseKey, landmarkKey), measured_(_m_e_a_s_u_r_e_d), K_(K), │ │ │ │ │ -body_P_sensor_(body_P_sensor), │ │ │ │ │ -88 throwCheirality_(_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y), verboseCheirality_(_v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y) {} │ │ │ │ │ -89 │ │ │ │ │ -_9_1 _~_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r() override {} │ │ │ │ │ -92 │ │ │ │ │ -_9_4 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -95 return boost::static_pointer_cast( │ │ │ │ │ -96 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); } │ │ │ │ │ -97 │ │ │ │ │ -_1_0_3 void _p_r_i_n_t(const std::string& s = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ -DefaultKeyFormatter) const override { │ │ │ │ │ -104 _B_a_s_e_:_:_p_r_i_n_t(s, keyFormatter); │ │ │ │ │ -105 measured_._p_r_i_n_t(s + ".z"); │ │ │ │ │ -106 if(this->body_P_sensor_) │ │ │ │ │ -107 this->body_P_sensor_->print(" sensor pose in body frame: "); │ │ │ │ │ -108 } │ │ │ │ │ -109 │ │ │ │ │ -_1_1_3 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& f, double tol = 1e-9) const override { │ │ │ │ │ -114 const _G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r* e = dynamic_cast │ │ │ │ │ -(&f); │ │ │ │ │ -115 return e │ │ │ │ │ -116 && _B_a_s_e_:_:_e_q_u_a_l_s(f) │ │ │ │ │ -117 && measured_._e_q_u_a_l_s(e->measured_, tol) │ │ │ │ │ -118 && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e- │ │ │ │ │ ->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); │ │ │ │ │ -119 } │ │ │ │ │ -120 │ │ │ │ │ -_1_2_2 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const _P_o_s_e_3& pose, const _P_o_i_n_t_3& point, │ │ │ │ │ -123 boost::optional H1 = boost::none, boost::optional H2 = │ │ │ │ │ -boost::none) const override { │ │ │ │ │ -124 try { │ │ │ │ │ -125 if(body_P_sensor_) { │ │ │ │ │ -126 if(H1) { │ │ │ │ │ -127 gtsam::Matrix H0; │ │ │ │ │ -128 _S_t_e_r_e_o_C_a_m_e_r_a stereoCam(pose.compose(*body_P_sensor_, H0), K_); │ │ │ │ │ -129 _S_t_e_r_e_o_P_o_i_n_t_2 reprojectionError(stereoCam._p_r_o_j_e_c_t(point, H1, H2) - │ │ │ │ │ -measured_); │ │ │ │ │ -130 *H1 = *H1 * H0; │ │ │ │ │ -131 return reprojectionError._v_e_c_t_o_r(); │ │ │ │ │ -132 } else { │ │ │ │ │ -133 _S_t_e_r_e_o_C_a_m_e_r_a stereoCam(pose.compose(*body_P_sensor_), K_); │ │ │ │ │ -134 return (stereoCam._p_r_o_j_e_c_t(point, H1, H2) - measured_).vector(); │ │ │ │ │ -135 } │ │ │ │ │ -136 } else { │ │ │ │ │ -137 _S_t_e_r_e_o_C_a_m_e_r_a stereoCam(pose, K_); │ │ │ │ │ -138 return (stereoCam._p_r_o_j_e_c_t(point, H1, H2) - measured_).vector(); │ │ │ │ │ -139 } │ │ │ │ │ -140 } catch(_S_t_e_r_e_o_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n& e) { │ │ │ │ │ -141 if (H1) *H1 = Matrix::Zero(3,6); │ │ │ │ │ -142 if (H2) *H2 = Z_3x3; │ │ │ │ │ -143 if (verboseCheirality_) │ │ │ │ │ -144 std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) │ │ │ │ │ -<< │ │ │ │ │ -145 " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; │ │ │ │ │ -146 if (throwCheirality_) │ │ │ │ │ -147 throw _S_t_e_r_e_o_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n(this->key2()); │ │ │ │ │ -148 } │ │ │ │ │ -149 return Vector3::Constant(2.0 * K_->fx()); │ │ │ │ │ -150 } │ │ │ │ │ -151 │ │ │ │ │ -_1_5_3 const _S_t_e_r_e_o_P_o_i_n_t_2& _m_e_a_s_u_r_e_d() const { │ │ │ │ │ -154 return measured_; │ │ │ │ │ -155 } │ │ │ │ │ -156 │ │ │ │ │ -_1_5_8 inline const Cal3_S2Stereo::shared_ptr _c_a_l_i_b_r_a_t_i_o_n() const { │ │ │ │ │ -159 return K_; │ │ │ │ │ +45protected: │ │ │ │ │ +46 │ │ │ │ │ +47 // This factor is closely related to a CameraSet │ │ │ │ │ +48 typedef _C_a_m_e_r_a_S_e_t_<_C_A_M_E_R_A_> Set; │ │ │ │ │ +49 │ │ │ │ │ +50 typedef typename CAMERA::Measurement Z; │ │ │ │ │ +_5_1 static const int _D = _t_r_a_i_t_s_<_C_A_M_E_R_A_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ +_5_2 static const int _Z_D_i_m = _t_r_a_i_t_s_<_Z_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ +53 │ │ │ │ │ +_5_4 typedef Eigen::Matrix _M_a_t_r_i_x_Z_D; │ │ │ │ │ +_5_5 typedef Eigen::Matrix _M_a_t_r_i_x_D_D; │ │ │ │ │ +56 typedef std::vector > FBlocks; │ │ │ │ │ +57 │ │ │ │ │ +_5_8 FBlocks _F_B_l_o_c_k_s__; │ │ │ │ │ +_5_9 const Matrix _P_o_i_n_t_C_o_v_a_r_i_a_n_c_e__; │ │ │ │ │ +_6_0 const Matrix _E__; │ │ │ │ │ +_6_1 const Vector _b__; │ │ │ │ │ +62 │ │ │ │ │ +63public: │ │ │ │ │ +64 │ │ │ │ │ +_6_6 _R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r() { │ │ │ │ │ +67 } │ │ │ │ │ +68 │ │ │ │ │ +70 │ │ │ │ │ +_8_0 _R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r(const _K_e_y_V_e_c_t_o_r& _k_e_y_s, const FBlocks& Fs, │ │ │ │ │ +81 const Matrix& E, const Matrix& P, const Vector& b) │ │ │ │ │ +82 : _G_a_u_s_s_i_a_n_F_a_c_t_o_r(_k_e_y_s), _F_B_l_o_c_k_s__(Fs), _P_o_i_n_t_C_o_v_a_r_i_a_n_c_e__(P), _E__(E), _b__(b) {} │ │ │ │ │ +83 │ │ │ │ │ +_8_5 _~_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r() override { │ │ │ │ │ +86 } │ │ │ │ │ +87 │ │ │ │ │ +88 const FBlocks& Fs() const { │ │ │ │ │ +89 return _F_B_l_o_c_k_s__; │ │ │ │ │ +90 } │ │ │ │ │ +91 │ │ │ │ │ +92 const Matrix& E() const { │ │ │ │ │ +93 return _E__; │ │ │ │ │ +94 } │ │ │ │ │ +95 │ │ │ │ │ +96 const Vector& b() const { │ │ │ │ │ +97 return _b__; │ │ │ │ │ +98 } │ │ │ │ │ +99 │ │ │ │ │ +100 const Matrix& getPointCovariance() const { │ │ │ │ │ +101 return _P_o_i_n_t_C_o_v_a_r_i_a_n_c_e__; │ │ │ │ │ +102 } │ │ │ │ │ +103 │ │ │ │ │ +_1_0_5 void _p_r_i_n_t(const std::string& s = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ +106 DefaultKeyFormatter) const override { │ │ │ │ │ +107 std::cout << " RegularImplicitSchurFactor " << std::endl; │ │ │ │ │ +108 _F_a_c_t_o_r_:_:_p_r_i_n_t(s); │ │ │ │ │ +109 for (size_t pos = 0; pos < _s_i_z_e(); ++pos) { │ │ │ │ │ +110 std::cout << "Fblock:\n" << _F_B_l_o_c_k_s__[pos] << std::endl; │ │ │ │ │ +111 } │ │ │ │ │ +112 std::cout << "PointCovariance:\n" << _P_o_i_n_t_C_o_v_a_r_i_a_n_c_e__ << std::endl; │ │ │ │ │ +113 std::cout << "E:\n" << _E__ << std::endl; │ │ │ │ │ +114 std::cout << "b:\n" << _b__.transpose() << std::endl; │ │ │ │ │ +115 } │ │ │ │ │ +116 │ │ │ │ │ +_1_1_8 bool _e_q_u_a_l_s(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r& lf, double tol) const override { │ │ │ │ │ +119 const _T_h_i_s* f = dynamic_cast(&lf); │ │ │ │ │ +120 if (!f) │ │ │ │ │ +121 return false; │ │ │ │ │ +122 for (size_t k = 0; k < _F_B_l_o_c_k_s__.size(); ++k) { │ │ │ │ │ +123 if (_k_e_y_s__[k] != f->_k_e_y_s__[k]) │ │ │ │ │ +124 return false; │ │ │ │ │ +125 if (!_e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(_F_B_l_o_c_k_s__[k], f->_F_B_l_o_c_k_s__[k], tol)) │ │ │ │ │ +126 return false; │ │ │ │ │ +127 } │ │ │ │ │ +128 return _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(_P_o_i_n_t_C_o_v_a_r_i_a_n_c_e__, f->_P_o_i_n_t_C_o_v_a_r_i_a_n_c_e__, tol) │ │ │ │ │ +129 && _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(_E__, f->_E__, tol) │ │ │ │ │ +130 && _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(_b__, f->_b__, tol); │ │ │ │ │ +131 } │ │ │ │ │ +132 │ │ │ │ │ +_1_3_4 _D_e_n_s_e_I_n_d_e_x _g_e_t_D_i_m(_c_o_n_s_t___i_t_e_r_a_t_o_r variable) const override { │ │ │ │ │ +135 return _D; │ │ │ │ │ +136 } │ │ │ │ │ +137 │ │ │ │ │ +_1_3_8 void _u_p_d_a_t_e_H_e_s_s_i_a_n(const _K_e_y_V_e_c_t_o_r& _k_e_y_s, │ │ │ │ │ +139 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x* info) const override { │ │ │ │ │ +140 throw std::runtime_error( │ │ │ │ │ +141 "RegularImplicitSchurFactor::updateHessian non implemented"); │ │ │ │ │ +142 } │ │ │ │ │ +_1_4_3 Matrix _a_u_g_m_e_n_t_e_d_J_a_c_o_b_i_a_n() const override { │ │ │ │ │ +144 throw std::runtime_error( │ │ │ │ │ +145 "RegularImplicitSchurFactor::augmentedJacobian non implemented"); │ │ │ │ │ +146 return Matrix(); │ │ │ │ │ +147 } │ │ │ │ │ +_1_4_8 std::pair _j_a_c_o_b_i_a_n() const override { │ │ │ │ │ +149 throw std::runtime_error( │ │ │ │ │ +150 "RegularImplicitSchurFactor::jacobian non implemented"); │ │ │ │ │ +151 return std::make_pair(Matrix(), Vector()); │ │ │ │ │ +152 } │ │ │ │ │ +153 │ │ │ │ │ +_1_5_5 Matrix _a_u_g_m_e_n_t_e_d_I_n_f_o_r_m_a_t_i_o_n() const override { │ │ │ │ │ +156 // Do the Schur complement │ │ │ │ │ +157 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x augmentedHessian = │ │ │ │ │ +158 Set::SchurComplement(_F_B_l_o_c_k_s__, _E__, _b__); │ │ │ │ │ +159 return augmentedHessian._s_e_l_f_a_d_j_o_i_n_t_V_i_e_w(); │ │ │ │ │ 160 } │ │ │ │ │ 161 │ │ │ │ │ -_1_6_3 inline bool _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y() const { return verboseCheirality_; } │ │ │ │ │ -164 │ │ │ │ │ -_1_6_6 inline bool _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y() const { return throwCheirality_; } │ │ │ │ │ -167 │ │ │ │ │ -168private: │ │ │ │ │ -_1_7_0 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -171 template │ │ │ │ │ -172 void serialize(Archive & ar, const unsigned int /*version*/) { │ │ │ │ │ -173 // NoiseModelFactor2 instead of NoiseModelFactorN for backward │ │ │ │ │ -compatibility │ │ │ │ │ -174 ar & boost::serialization::make_nvp("NoiseModelFactor2", │ │ │ │ │ -175 boost::serialization::base_object(*this)); │ │ │ │ │ -176 ar & BOOST_SERIALIZATION_NVP(measured_); │ │ │ │ │ -177 ar & BOOST_SERIALIZATION_NVP(K_); │ │ │ │ │ -178 ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); │ │ │ │ │ -179 ar & BOOST_SERIALIZATION_NVP(throwCheirality_); │ │ │ │ │ -180 ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); │ │ │ │ │ -181 } │ │ │ │ │ -182}; │ │ │ │ │ -183 │ │ │ │ │ -185template │ │ │ │ │ -_1_8_6struct _t_r_a_i_t_s<_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r > : public │ │ │ │ │ -_T_e_s_t_a_b_l_e > {}; │ │ │ │ │ -187 │ │ │ │ │ -188} // \ namespace gtsam │ │ │ │ │ -_S_t_e_r_e_o_C_a_m_e_r_a_._h │ │ │ │ │ -A Stereo Camera based on two Simple Cameras. │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ -Non-linear factor base classes. │ │ │ │ │ +_1_6_3 Matrix _i_n_f_o_r_m_a_t_i_o_n() const override { │ │ │ │ │ +164 Matrix augmented = _a_u_g_m_e_n_t_e_d_I_n_f_o_r_m_a_t_i_o_n(); │ │ │ │ │ +165 int m = this->_k_e_y_s__.size(); │ │ │ │ │ +166 size_t M = _D * m; │ │ │ │ │ +167 return augmented.block(0, 0, M, M); │ │ │ │ │ +168 } │ │ │ │ │ +169 │ │ │ │ │ +171 using _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_h_e_s_s_i_a_n_D_i_a_g_o_n_a_l; │ │ │ │ │ +172 │ │ │ │ │ +_1_7_4 void _h_e_s_s_i_a_n_D_i_a_g_o_n_a_l_A_d_d(_V_e_c_t_o_r_V_a_l_u_e_s &d) const override { │ │ │ │ │ +175 // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F); │ │ │ │ │ +176 for (size_t k = 0; k < _s_i_z_e(); ++k) { // for each camera │ │ │ │ │ +177 _K_e_y j = _k_e_y_s__[k]; │ │ │ │ │ +178 │ │ │ │ │ +179 // Calculate Fj'*Ej for the current camera (observing a single point) │ │ │ │ │ +180 // D x 3 = (D x ZDim) * (ZDim x 3) │ │ │ │ │ +181 const _M_a_t_r_i_x_Z_D& Fj = _F_B_l_o_c_k_s__[k]; │ │ │ │ │ +182 Eigen::Matrix FtE = Fj.transpose() │ │ │ │ │ +183 * _E__.block<_Z_D_i_m, 3>(_Z_D_i_m * k, 0); │ │ │ │ │ +184 │ │ │ │ │ +185 Eigen::Matrix dj; │ │ │ │ │ +186 for (int k = 0; k < _D; ++k) { // for each diagonal element of the camera │ │ │ │ │ +hessian │ │ │ │ │ +187 // Vector column_k_Fj = Fj.col(k); │ │ │ │ │ +188 dj(k) = Fj.col(k).squaredNorm(); // dot(column_k_Fj, column_k_Fj); │ │ │ │ │ +189 // Vector column_k_FtE = FtE.row(k); │ │ │ │ │ +190 // (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1) │ │ │ │ │ +191 dj(k) -= FtE.row(k) * _P_o_i_n_t_C_o_v_a_r_i_a_n_c_e__ * FtE.row(k).transpose(); │ │ │ │ │ +192 } │ │ │ │ │ +193 │ │ │ │ │ +194 auto result = d._e_m_p_l_a_c_e(j, dj); │ │ │ │ │ +195 if(!result.second) { │ │ │ │ │ +196 result.first->second += dj; │ │ │ │ │ +197 } │ │ │ │ │ +198 } │ │ │ │ │ +199 } │ │ │ │ │ +200 │ │ │ │ │ +_2_0_5 void _h_e_s_s_i_a_n_D_i_a_g_o_n_a_l(double* d) const override { │ │ │ │ │ +206 // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F); │ │ │ │ │ +207 // Use eigen magic to access raw memory │ │ │ │ │ +208 typedef Eigen::Matrix DVector; │ │ │ │ │ +209 typedef Eigen::Map DMap; │ │ │ │ │ +210 │ │ │ │ │ +211 for (size_t pos = 0; pos < _s_i_z_e(); ++pos) { // for each camera in the │ │ │ │ │ +factor │ │ │ │ │ +212 _K_e_y j = _k_e_y_s__[pos]; │ │ │ │ │ +213 │ │ │ │ │ +214 // Calculate Fj'*Ej for the current camera (observing a single point) │ │ │ │ │ +215 // D x 3 = (D x ZDim) * (ZDim x 3) │ │ │ │ │ +216 const _M_a_t_r_i_x_Z_D& Fj = _F_B_l_o_c_k_s__[pos]; │ │ │ │ │ +217 Eigen::Matrix FtE = Fj.transpose() │ │ │ │ │ +218 * _E__.block<_Z_D_i_m, 3>(_Z_D_i_m * pos, 0); │ │ │ │ │ +219 │ │ │ │ │ +220 DVector dj; │ │ │ │ │ +221 for (int k = 0; k < _D; ++k) { // for each diagonal element of the camera │ │ │ │ │ +hessian │ │ │ │ │ +222 dj(k) = Fj.col(k).squaredNorm(); │ │ │ │ │ +223 // (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1) │ │ │ │ │ +224 dj(k) -= FtE.row(k) * _P_o_i_n_t_C_o_v_a_r_i_a_n_c_e__ * FtE.row(k).transpose(); │ │ │ │ │ +225 } │ │ │ │ │ +226 DMap(d + _D * j) += dj; │ │ │ │ │ +227 } │ │ │ │ │ +228 } │ │ │ │ │ +229 │ │ │ │ │ +_2_3_1 std::map _h_e_s_s_i_a_n_B_l_o_c_k_D_i_a_g_o_n_a_l() const override { │ │ │ │ │ +232 std::map blocks; │ │ │ │ │ +233 // F'*(I - E*P*E')*F │ │ │ │ │ +234 for (size_t pos = 0; pos < _s_i_z_e(); ++pos) { │ │ │ │ │ +235 _K_e_y j = _k_e_y_s__[pos]; │ │ │ │ │ +236 // F'*F - F'*E*P*E'*F e.g. (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9) │ │ │ │ │ +237 const _M_a_t_r_i_x_Z_D& Fj = _F_B_l_o_c_k_s__[pos]; │ │ │ │ │ +238 // Eigen::Matrix FtE = Fj.transpose() │ │ │ │ │ +239 // * E_.block(ZDim * pos, 0); │ │ │ │ │ +240 // blocks[j] = Fj.transpose() * Fj │ │ │ │ │ +241 // - FtE * PointCovariance_ * FtE.transpose(); │ │ │ │ │ +242 │ │ │ │ │ +243 const Matrix23& Ej = _E__.block<_Z_D_i_m, 3>(_Z_D_i_m * pos, 0); │ │ │ │ │ +244 blocks[j] = Fj.transpose() │ │ │ │ │ +245 * (Fj - Ej * _P_o_i_n_t_C_o_v_a_r_i_a_n_c_e__ * Ej.transpose() * Fj); │ │ │ │ │ +246 │ │ │ │ │ +247 // F'*(I - E*P*E')*F, TODO: this should work, but it does not :-( │ │ │ │ │ +248 // static const Eigen::Matrix I2 = eye(ZDim); │ │ │ │ │ +249 // Matrix2 Q = // │ │ │ │ │ +250 // I2 - E_.block(ZDim * pos, 0) * PointCovariance_ * │ │ │ │ │ +E_.block(ZDim * pos, 0).transpose(); │ │ │ │ │ +251 // blocks[j] = Fj.transpose() * Q * Fj; │ │ │ │ │ +252 } │ │ │ │ │ +253 return blocks; │ │ │ │ │ +254 } │ │ │ │ │ +255 │ │ │ │ │ +_2_5_6 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r _c_l_o_n_e() const override { │ │ │ │ │ +257 return boost::make_shared >(_k_e_y_s__, │ │ │ │ │ +258 _F_B_l_o_c_k_s__, _P_o_i_n_t_C_o_v_a_r_i_a_n_c_e__, _E__, _b__); │ │ │ │ │ +259 throw std::runtime_error( │ │ │ │ │ +260 "RegularImplicitSchurFactor::clone non implemented"); │ │ │ │ │ +261 } │ │ │ │ │ +262 │ │ │ │ │ +_2_6_3 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r _n_e_g_a_t_e() const override { │ │ │ │ │ +264 return boost::make_shared >(_k_e_y_s__, │ │ │ │ │ +265 _F_B_l_o_c_k_s__, _P_o_i_n_t_C_o_v_a_r_i_a_n_c_e__, _E__, _b__); │ │ │ │ │ +266 throw std::runtime_error( │ │ │ │ │ +267 "RegularImplicitSchurFactor::negate non implemented"); │ │ │ │ │ +268 } │ │ │ │ │ +269 │ │ │ │ │ +270 // Raw Vector version of y += F'*alpha*(I - E*P*E')*F*x, for testing │ │ │ │ │ +271 static │ │ │ │ │ +272 void multiplyHessianAdd(const Matrix& F, const Matrix& E, │ │ │ │ │ +273 const Matrix& PointCovariance, double alpha, const Vector& x, Vector& y) { │ │ │ │ │ +274 Vector _e_1 = F * x; │ │ │ │ │ +275 Vector d1 = E.transpose() * _e_1; │ │ │ │ │ +276 Vector d2 = PointCovariance * d1; │ │ │ │ │ +277 Vector e2 = E * d2; │ │ │ │ │ +278 Vector e3 = alpha * (_e_1 - e2); │ │ │ │ │ +279 y += F.transpose() * e3; │ │ │ │ │ +280 } │ │ │ │ │ +281 │ │ │ │ │ +282 typedef std::vector> Error2s; │ │ │ │ │ +283 │ │ │ │ │ +_2_8_7 void _p_r_o_j_e_c_t_E_r_r_o_r_2(const Error2s& _e_1, Error2s& e2) const { │ │ │ │ │ +288 │ │ │ │ │ +289 // d1 = E.transpose() * (e1-ZDim*b) = (3*2m)*2m │ │ │ │ │ +290 Vector3 d1; │ │ │ │ │ +291 d1.setZero(); │ │ │ │ │ +292 for (size_t k = 0; k < _s_i_z_e(); k++) │ │ │ │ │ +293 d1 += _E__.block<_Z_D_i_m, 3>(_Z_D_i_m * k, 0).transpose() │ │ │ │ │ +294 * (_e_1[k] - _Z_D_i_m * _b__.segment<_Z_D_i_m>(k * _Z_D_i_m)); │ │ │ │ │ +295 │ │ │ │ │ +296 // d2 = E.transpose() * e1 = (3*2m)*2m │ │ │ │ │ +297 Vector3 d2 = _P_o_i_n_t_C_o_v_a_r_i_a_n_c_e__ * d1; │ │ │ │ │ +298 │ │ │ │ │ +299 // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] │ │ │ │ │ +300 for (size_t k = 0; k < _s_i_z_e(); k++) │ │ │ │ │ +301 e2[k] = _e_1[k] - _Z_D_i_m * _b__.segment<_Z_D_i_m>(k * _Z_D_i_m) │ │ │ │ │ +302 - _E__.block<_Z_D_i_m, 3>(_Z_D_i_m * k, 0) * d2; │ │ │ │ │ +303 } │ │ │ │ │ +304 │ │ │ │ │ +305 /* │ │ │ │ │ +306 * This definition matches the linearized error in the Hessian Factor: │ │ │ │ │ +307 * LinError(x) = x'*H*x - 2*x'*eta + f │ │ │ │ │ +308 * with: │ │ │ │ │ +309 * H = F' * (I-E'*P*E) * F = F' * Q * F │ │ │ │ │ +310 * eta = F' * (I-E'*P*E) * b = F' * Q * b │ │ │ │ │ +311 * f = nonlinear error │ │ │ │ │ +312 * (x'*H*x - 2*x'*eta + f) = x'*F'*Q*F*x - 2*x'*F'*Q *b + f = x'*F'*Q*(F*x - │ │ │ │ │ +2*b) + f │ │ │ │ │ +313 */ │ │ │ │ │ +314 double error(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const override { │ │ │ │ │ +315 │ │ │ │ │ +316 // resize does not do malloc if correct size │ │ │ │ │ +317 _e_1.resize(_s_i_z_e()); │ │ │ │ │ +318 e2.resize(_s_i_z_e()); │ │ │ │ │ +319 │ │ │ │ │ +320 // e1 = F * x - b = (2m*dm)*dm │ │ │ │ │ +321 for (size_t k = 0; k < _s_i_z_e(); ++k) │ │ │ │ │ +322 _e_1[k] = _F_B_l_o_c_k_s__[k] * x._a_t(_k_e_y_s__[k]); │ │ │ │ │ +323 _p_r_o_j_e_c_t_E_r_r_o_r_2(_e_1, e2); │ │ │ │ │ +324 │ │ │ │ │ +325 double result = 0; │ │ │ │ │ +326 for (size_t k = 0; k < _s_i_z_e(); ++k) │ │ │ │ │ +327 result += _d_o_t(_e_1[k], e2[k]); │ │ │ │ │ +328 │ │ │ │ │ +329 double f = _b__.squaredNorm(); │ │ │ │ │ +330 return 0.5 * (result + f); │ │ │ │ │ +331 } │ │ │ │ │ +332 │ │ │ │ │ +333 // needed to be GaussianFactor - (I - E*P*E')*(F*x - b) │ │ │ │ │ +334 // This is wrong and does not match the definition in Hessian, │ │ │ │ │ +335 // but it matches the definition of the Jacobian factor (JF) │ │ │ │ │ +336 double errorJF(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const { │ │ │ │ │ +337 │ │ │ │ │ +338 // resize does not do malloc if correct size │ │ │ │ │ +339 _e_1.resize(_s_i_z_e()); │ │ │ │ │ +340 e2.resize(_s_i_z_e()); │ │ │ │ │ +341 │ │ │ │ │ +342 // e1 = F * x - b = (2m*dm)*dm │ │ │ │ │ +343 for (size_t k = 0; k < _s_i_z_e(); ++k) │ │ │ │ │ +344 _e_1[k] = _F_B_l_o_c_k_s__[k] * x.at(_k_e_y_s__[k]) - _b__.segment<_Z_D_i_m>(k * _Z_D_i_m); │ │ │ │ │ +345 _p_r_o_j_e_c_t_E_r_r_o_r(_e_1, e2); │ │ │ │ │ +346 │ │ │ │ │ +347 double result = 0; │ │ │ │ │ +348 for (size_t k = 0; k < _s_i_z_e(); ++k) │ │ │ │ │ +349 result += _d_o_t(e2[k], e2[k]); │ │ │ │ │ +350 │ │ │ │ │ +351 // std::cout << "implicitFactor::error result " << result << std::endl; │ │ │ │ │ +352 return 0.5 * result; │ │ │ │ │ +353 } │ │ │ │ │ +_3_5_7 void _p_r_o_j_e_c_t_E_r_r_o_r(const Error2s& _e_1, Error2s& e2) const { │ │ │ │ │ +358 │ │ │ │ │ +359 // d1 = E.transpose() * e1 = (3*2m)*2m │ │ │ │ │ +360 Vector3 d1; │ │ │ │ │ +361 d1.setZero(); │ │ │ │ │ +362 for (size_t k = 0; k < _s_i_z_e(); k++) │ │ │ │ │ +363 d1 += _E__.block<_Z_D_i_m, 3>(_Z_D_i_m * k, 0).transpose() * _e_1[k]; │ │ │ │ │ +364 │ │ │ │ │ +365 // d2 = E.transpose() * e1 = (3*2m)*2m │ │ │ │ │ +366 Vector3 d2 = _P_o_i_n_t_C_o_v_a_r_i_a_n_c_e__ * d1; │ │ │ │ │ +367 │ │ │ │ │ +368 // e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3] │ │ │ │ │ +369 for (size_t k = 0; k < _s_i_z_e(); k++) │ │ │ │ │ +370 e2[k] = _e_1[k] - _E__.block<_Z_D_i_m, 3>(_Z_D_i_m * k, 0) * d2; │ │ │ │ │ +371 } │ │ │ │ │ +372 │ │ │ │ │ +_3_7_4 mutable Error2s _e_1, e2; │ │ │ │ │ +375 │ │ │ │ │ +_3_8_0 void _m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d(double alpha, const double* x, double* y) const { │ │ │ │ │ +381 │ │ │ │ │ +382 // Use eigen magic to access raw memory │ │ │ │ │ +383 typedef Eigen::Matrix DVector; │ │ │ │ │ +384 typedef Eigen::Map DMap; │ │ │ │ │ +385 typedef Eigen::Map ConstDMap; │ │ │ │ │ +386 │ │ │ │ │ +387 // resize does not do malloc if correct size │ │ │ │ │ +388 _e_1.resize(_s_i_z_e()); │ │ │ │ │ +389 e2.resize(_s_i_z_e()); │ │ │ │ │ +390 │ │ │ │ │ +391 // e1 = F * x = (2m*dm)*dm │ │ │ │ │ +392 for (size_t k = 0; k < _s_i_z_e(); ++k) { │ │ │ │ │ +393 _K_e_y key = _k_e_y_s__[k]; │ │ │ │ │ +394 _e_1[k] = _F_B_l_o_c_k_s__[k] * ConstDMap(x + _D * key); │ │ │ │ │ +395 } │ │ │ │ │ +396 │ │ │ │ │ +397 _p_r_o_j_e_c_t_E_r_r_o_r(_e_1, e2); │ │ │ │ │ +398 │ │ │ │ │ +399 // y += F.transpose()*e2 = (2d*2m)*2m │ │ │ │ │ +400 for (size_t k = 0; k < _s_i_z_e(); ++k) { │ │ │ │ │ +401 _K_e_y key = _k_e_y_s__[k]; │ │ │ │ │ +402 DMap(y + _D * key) += _F_B_l_o_c_k_s__[k].transpose() * alpha * e2[k]; │ │ │ │ │ +403 } │ │ │ │ │ +404 } │ │ │ │ │ +405 │ │ │ │ │ +406 void multiplyHessianAdd(double alpha, const double* x, double* y, │ │ │ │ │ +407 std::vector _k_e_y_s) const { │ │ │ │ │ +408 } │ │ │ │ │ +409 │ │ │ │ │ +_4_1_3 void _m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d(double alpha, const _V_e_c_t_o_r_V_a_l_u_e_s& x, │ │ │ │ │ +414 _V_e_c_t_o_r_V_a_l_u_e_s& y) const override { │ │ │ │ │ +415 │ │ │ │ │ +416 // resize does not do malloc if correct size │ │ │ │ │ +417 _e_1.resize(_s_i_z_e()); │ │ │ │ │ +418 e2.resize(_s_i_z_e()); │ │ │ │ │ +419 │ │ │ │ │ +420 // e1 = F * x = (2m*dm)*dm │ │ │ │ │ +421 for (size_t k = 0; k < _s_i_z_e(); ++k) │ │ │ │ │ +422 _e_1[k] = _F_B_l_o_c_k_s__[k] * x._a_t(_k_e_y_s__[k]); │ │ │ │ │ +423 │ │ │ │ │ +424 _p_r_o_j_e_c_t_E_r_r_o_r(_e_1, e2); │ │ │ │ │ +425 │ │ │ │ │ +426 // y += F.transpose()*e2 = (2d*2m)*2m │ │ │ │ │ +427 for (size_t k = 0; k < _s_i_z_e(); ++k) { │ │ │ │ │ +428 _K_e_y key = _k_e_y_s__[k]; │ │ │ │ │ +429 static const Vector _e_m_p_t_y; │ │ │ │ │ +430 std::pair it = y._t_r_y_I_n_s_e_r_t(key, _e_m_p_t_y); │ │ │ │ │ +431 Vector& yi = it.first->second; │ │ │ │ │ +432 // Create the value as a zero vector if it does not exist. │ │ │ │ │ +433 if (it.second) │ │ │ │ │ +434 yi = Vector::Zero(_F_B_l_o_c_k_s__[k].cols()); │ │ │ │ │ +435 yi += _F_B_l_o_c_k_s__[k].transpose() * alpha * e2[k]; │ │ │ │ │ +436 } │ │ │ │ │ +437 } │ │ │ │ │ +438 │ │ │ │ │ +_4_4_2 void _m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_D_u_m_m_y(double alpha, const _V_e_c_t_o_r_V_a_l_u_e_s& x, │ │ │ │ │ +443 _V_e_c_t_o_r_V_a_l_u_e_s& y) const { │ │ │ │ │ +444 │ │ │ │ │ +445 for (size_t k = 0; k < _s_i_z_e(); ++k) { │ │ │ │ │ +446 static const Vector _e_m_p_t_y; │ │ │ │ │ +447 _K_e_y key = _k_e_y_s__[k]; │ │ │ │ │ +448 std::pair it = y._t_r_y_I_n_s_e_r_t(key, _e_m_p_t_y); │ │ │ │ │ +449 Vector& yi = it.first->second; │ │ │ │ │ +450 yi = x._a_t(key); │ │ │ │ │ +451 } │ │ │ │ │ +452 } │ │ │ │ │ +453 │ │ │ │ │ +_4_5_7 _V_e_c_t_o_r_V_a_l_u_e_s _g_r_a_d_i_e_n_t_A_t_Z_e_r_o() const override { │ │ │ │ │ +458 // calculate Q*b │ │ │ │ │ +459 _e_1.resize(_s_i_z_e()); │ │ │ │ │ +460 e2.resize(_s_i_z_e()); │ │ │ │ │ +461 for (size_t k = 0; k < _s_i_z_e(); k++) │ │ │ │ │ +462 _e_1[k] = _b__.segment<_Z_D_i_m>(_Z_D_i_m * k); │ │ │ │ │ +463 _p_r_o_j_e_c_t_E_r_r_o_r(_e_1, e2); │ │ │ │ │ +464 │ │ │ │ │ +465 // g = F.transpose()*e2 │ │ │ │ │ +466 _V_e_c_t_o_r_V_a_l_u_e_s g; │ │ │ │ │ +467 for (size_t k = 0; k < _s_i_z_e(); ++k) { │ │ │ │ │ +468 _K_e_y key = _k_e_y_s__[k]; │ │ │ │ │ +469 g._i_n_s_e_r_t(key, -_F_B_l_o_c_k_s__[k].transpose() * e2[k]); │ │ │ │ │ +470 } │ │ │ │ │ +471 │ │ │ │ │ +472 // return it │ │ │ │ │ +473 return g; │ │ │ │ │ +474 } │ │ │ │ │ +475 │ │ │ │ │ +_4_7_9 void _g_r_a_d_i_e_n_t_A_t_Z_e_r_o(double* d) const override { │ │ │ │ │ +480 │ │ │ │ │ +481 // Use eigen magic to access raw memory │ │ │ │ │ +482 typedef Eigen::Matrix DVector; │ │ │ │ │ +483 typedef Eigen::Map DMap; │ │ │ │ │ +484 │ │ │ │ │ +485 // calculate Q*b │ │ │ │ │ +486 _e_1.resize(_s_i_z_e()); │ │ │ │ │ +487 e2.resize(_s_i_z_e()); │ │ │ │ │ +488 for (size_t k = 0; k < _s_i_z_e(); k++) │ │ │ │ │ +489 _e_1[k] = _b__.segment<_Z_D_i_m>(_Z_D_i_m * k); │ │ │ │ │ +490 _p_r_o_j_e_c_t_E_r_r_o_r(_e_1, e2); │ │ │ │ │ +491 │ │ │ │ │ +492 for (size_t k = 0; k < _s_i_z_e(); ++k) { // for each camera in the factor │ │ │ │ │ +493 _K_e_y j = _k_e_y_s__[k]; │ │ │ │ │ +494 DMap(d + _D * j) += -_F_B_l_o_c_k_s__[k].transpose() * e2[k]; │ │ │ │ │ +495 } │ │ │ │ │ +496 } │ │ │ │ │ +497 │ │ │ │ │ +_4_9_9 Vector _g_r_a_d_i_e_n_t(_K_e_y key, const _V_e_c_t_o_r_V_a_l_u_e_s& x) const override { │ │ │ │ │ +500 throw std::runtime_error( │ │ │ │ │ +501 "gradient for RegularImplicitSchurFactor is not implemented yet"); │ │ │ │ │ +502 } │ │ │ │ │ +503 │ │ │ │ │ +504}; │ │ │ │ │ +505// end class RegularImplicitSchurFactor │ │ │ │ │ +506 │ │ │ │ │ +507template │ │ │ │ │ +508const int _R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_<_C_A_M_E_R_A_>_:_:_D; │ │ │ │ │ +509 │ │ │ │ │ +510template │ │ │ │ │ +511const int _R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_<_C_A_M_E_R_A_>_:_:_Z_D_i_m; │ │ │ │ │ +512 │ │ │ │ │ +513// traits │ │ │ │ │ +_5_1_4template struct _t_r_a_i_t_s<_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r > : │ │ │ │ │ +public _T_e_s_t_a_b_l_e< │ │ │ │ │ +515 RegularImplicitSchurFactor > { │ │ │ │ │ +516}; │ │ │ │ │ +517 │ │ │ │ │ +518} │ │ │ │ │ +519 │ │ │ │ │ +_C_a_m_e_r_a_S_e_t_._h │ │ │ │ │ +Base class to create smart factors on poses or cameras. │ │ │ │ │ +_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ +Factor Graph Values. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ -Vector3 Point3 │ │ │ │ │ -As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point3 to Vector3... │ │ │ │ │ -DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ -Aliases. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_n_s_e_I_n_d_e_x │ │ │ │ │ +ptrdiff_t DenseIndex │ │ │ │ │ +The index type for Eigen objects. │ │ │ │ │ +DDeeffiinniittiioonn types.h:106 │ │ │ │ │ +_g_t_s_a_m_:_:_d_o_t │ │ │ │ │ +double dot(const V1 &a, const V2 &b) │ │ │ │ │ +Dot product. │ │ │ │ │ +DDeeffiinniittiioonn Vector.h:195 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l___w_i_t_h___a_b_s___t_o_l │ │ │ │ │ +bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen:: │ │ │ │ │ +DenseBase< MATRIX > &B, double tol=1e-9) │ │ │ │ │ +equals with a tolerance │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:81 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ +of blocks. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_s_e_l_f_a_d_j_o_i_n_t_V_i_e_w │ │ │ │ │ +Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex │ │ │ │ │ +I, DenseIndex J) const │ │ │ │ │ +Return the square sub-matrix that contains blocks(i:j, i:j). │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:156 │ │ │ │ │ _g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ A helper that implements the traits interface for GTSAM types. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o │ │ │ │ │ -The most common 5DOF 3D->2D calibration, stereo version. │ │ │ │ │ -DDeeffiinniittiioonn Cal3_S2Stereo.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ -A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n │ │ │ │ │ -DDeeffiinniittiioonn StereoCamera.h:26 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a │ │ │ │ │ -A stereo camera class, parameterize by left camera pose and stereo calibration. │ │ │ │ │ -DDeeffiinniittiioonn StereoCamera.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a_:_:_p_r_o_j_e_c_t │ │ │ │ │ -StereoPoint2 project(const Point3 &point) const │ │ │ │ │ -Project 3D point to StereoPoint2 (uL,uR,v) │ │ │ │ │ -DDeeffiinniittiioonn StereoCamera.cpp:32 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2 │ │ │ │ │ -A 2D stereo point, v will be same for rectified images. │ │ │ │ │ -DDeeffiinniittiioonn StereoPoint2.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_v_e_c_t_o_r │ │ │ │ │ -Vector3 vector() const │ │ │ │ │ -convert to vector │ │ │ │ │ -DDeeffiinniittiioonn StereoPoint2.h:115 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const StereoPoint2 &q, double tol=1e-9) const │ │ │ │ │ -equals │ │ │ │ │ -DDeeffiinniittiioonn StereoPoint2.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="") const │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn StereoPoint2.cpp:26 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t │ │ │ │ │ +A set of cameras, all with their own calibration. │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s │ │ │ │ │ +const KeyVector & keys() const │ │ │ │ │ +Access the factor's involved variable keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:140 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s__ │ │ │ │ │ +KeyVector keys_ │ │ │ │ │ +The keys involved in this factor. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_m_p_t_y │ │ │ │ │ +bool empty() const │ │ │ │ │ +Whether the factor is empty (involves zero variables). │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:128 │ │ │ │ │ _g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ virtual void print(const std::string &s="Factor", const KeyFormatter │ │ │ │ │ &formatter=DefaultKeyFormatter) const │ │ │ │ │ print │ │ │ │ │ DDeeffiinniittiioonn Factor.cpp:29 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const This &other, double tol=1e-9) const │ │ │ │ │ -check equality │ │ │ │ │ -DDeeffiinniittiioonn Factor.cpp:42 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ -Nonlinear factor base class. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ -A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ -variables. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r │ │ │ │ │ -A Generic Stereo Factor. │ │ │ │ │ -DDeeffiinniittiioonn StereoFactor.h:31 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y │ │ │ │ │ -bool throwCheirality() const │ │ │ │ │ -return flag for throwing cheirality exceptions │ │ │ │ │ -DDeeffiinniittiioonn StereoFactor.h:166 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ -Vector evaluateError(const Pose3 &pose, const Point3 &point, boost::optional< │ │ │ │ │ -Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const │ │ │ │ │ -override │ │ │ │ │ -h(x)-z │ │ │ │ │ -DDeeffiinniittiioonn StereoFactor.h:122 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const NonlinearFactor &f, double tol=1e-9) const override │ │ │ │ │ -equals │ │ │ │ │ -DDeeffiinniittiioonn StereoFactor.h:113 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_c_a_l_i_b_r_a_t_i_o_n │ │ │ │ │ -const Cal3_S2Stereo::shared_ptr calibration() const │ │ │ │ │ -return the calibration object │ │ │ │ │ -DDeeffiinniittiioonn StereoFactor.h:158 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r │ │ │ │ │ -GenericStereoFactor(const StereoPoint2 &measured, const SharedNoiseModel │ │ │ │ │ -&model, Key poseKey, Key landmarkKey, const Cal3_S2Stereo::shared_ptr &K, bool │ │ │ │ │ -throwCheirality, bool verboseCheirality, boost::optional< POSE > │ │ │ │ │ -body_P_sensor=boost::none) │ │ │ │ │ -Constructor with exception-handling flags. │ │ │ │ │ -DDeeffiinniittiioonn StereoFactor.h:83 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_T_h_i_s │ │ │ │ │ -GenericStereoFactor< POSE, LANDMARK > This │ │ │ │ │ -typedef for this class (with templates) │ │ │ │ │ -DDeeffiinniittiioonn StereoFactor.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_~_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r │ │ │ │ │ -~GenericStereoFactor() override │ │ │ │ │ -Virtual destructor. │ │ │ │ │ -DDeeffiinniittiioonn StereoFactor.h:91 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y │ │ │ │ │ -bool verboseCheirality() const │ │ │ │ │ -return verbosity │ │ │ │ │ -DDeeffiinniittiioonn StereoFactor.h:163 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< GenericStereoFactor > shared_ptr │ │ │ │ │ -typedef for shared pointer to this object │ │ │ │ │ -DDeeffiinniittiioonn StereoFactor.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_m_e_a_s_u_r_e_d │ │ │ │ │ -const StereoPoint2 & measured() const │ │ │ │ │ -return the measured │ │ │ │ │ -DDeeffiinniittiioonn StereoFactor.h:153 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r │ │ │ │ │ -GenericStereoFactor() │ │ │ │ │ -Default constructor. │ │ │ │ │ -DDeeffiinniittiioonn StereoFactor.h:54 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn StereoFactor.h:170 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r │ │ │ │ │ -GenericStereoFactor(const StereoPoint2 &measured, const SharedNoiseModel │ │ │ │ │ -&model, Key poseKey, Key landmarkKey, const Cal3_S2Stereo::shared_ptr &K, │ │ │ │ │ -boost::optional< POSE > body_P_sensor=boost::none) │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_c_o_n_s_t___i_t_e_r_a_t_o_r │ │ │ │ │ +KeyVector::const_iterator const_iterator │ │ │ │ │ +Const iterator over keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:80 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_s_i_z_e │ │ │ │ │ +size_t size() const │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:157 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +An abstract virtual base class for JacobianFactor and HessianFactor. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactor.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactor.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_h_e_s_s_i_a_n_D_i_a_g_o_n_a_l │ │ │ │ │ +VectorValues hessianDiagonal() const │ │ │ │ │ +Return the diagonal of the Hessian for this factor. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactor.cpp:35 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ +with a unique integer... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_i_n_s_e_r_t │ │ │ │ │ +iterator insert(const std::pair< Key, Vector > &key_value) │ │ │ │ │ +Insert a vector value with key j. │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.cpp:91 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_e_m_p_l_a_c_e │ │ │ │ │ +std::pair< VectorValues::iterator, bool > emplace(Key j, Args &&... args) │ │ │ │ │ +Emplace a vector value with key j. │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:185 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_a_t │ │ │ │ │ +Vector & at(Key j) │ │ │ │ │ +Read/write access to the vector value with key j, throws std::out_of_range if j │ │ │ │ │ +does not exist,... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:139 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_t_r_y_I_n_s_e_r_t │ │ │ │ │ +std::pair< iterator, bool > tryInsert(Key j, const Vector &value) │ │ │ │ │ +insert that mimics the STL map insert - if the value already exists, the map is │ │ │ │ │ +not modified and an i... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:209 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r │ │ │ │ │ +RegularImplicitSchurFactor. │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_E__ │ │ │ │ │ +const Matrix E_ │ │ │ │ │ +The 2m*3 E Jacobian with respect to the point. │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:60 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_p_r_o_j_e_c_t_E_r_r_o_r │ │ │ │ │ +void projectError(const Error2s &e1, Error2s &e2) const │ │ │ │ │ +Calculate corrected error Q*e = (I - E*P*E')*e. │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:357 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ +GaussianFactor::shared_ptr clone() const override │ │ │ │ │ +Clone a factor (make a deep copy) │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:256 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_h_e_s_s_i_a_n_D_i_a_g_o_n_a_l_A_d_d │ │ │ │ │ +void hessianDiagonalAdd(VectorValues &d) const override │ │ │ │ │ +Add the diagonal of the Hessian for this factor to existing VectorValues. │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:174 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r │ │ │ │ │ +RegularImplicitSchurFactor(const KeyVector &keys, const FBlocks &Fs, const │ │ │ │ │ +Matrix &E, const Matrix &P, const Vector &b) │ │ │ │ │ +Construct from blocks of F, E, inv(E'*E), and RHS vector b. │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:80 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r │ │ │ │ │ +RegularImplicitSchurFactor() │ │ │ │ │ Constructor. │ │ │ │ │ -DDeeffiinniittiioonn StereoFactor.h:66 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:66 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_b__ │ │ │ │ │ +const Vector b_ │ │ │ │ │ +2m-dimensional RHS vector │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:61 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_j_a_c_o_b_i_a_n │ │ │ │ │ +std::pair< Matrix, Vector > jacobian() const override │ │ │ │ │ +Return the dense Jacobian and right-hand-side , with the noise models baked │ │ │ │ │ +into A and b. │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:148 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_M_a_t_r_i_x_D_D │ │ │ │ │ +Eigen::Matrix< double, D, D > MatrixDD │ │ │ │ │ +camera Hessian │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_g_r_a_d_i_e_n_t_A_t_Z_e_r_o │ │ │ │ │ +void gradientAtZero(double *d) const override │ │ │ │ │ +Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS. │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:479 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_Z_D_i_m │ │ │ │ │ +static const int ZDim │ │ │ │ │ +Measurement dimension. │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_i_n_f_o_r_m_a_t_i_o_n │ │ │ │ │ +Matrix information() const override │ │ │ │ │ +Compute full information matrix │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:163 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_~_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r │ │ │ │ │ +~RegularImplicitSchurFactor() override │ │ │ │ │ +Destructor. │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_u_p_d_a_t_e_H_e_s_s_i_a_n │ │ │ │ │ +void updateHessian(const KeyVector &keys, SymmetricBlockMatrix *info) const │ │ │ │ │ +override │ │ │ │ │ +Update an information matrix by adding the information corresponding to this │ │ │ │ │ +factor (used internally ... │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:138 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_F_B_l_o_c_k_s__ │ │ │ │ │ +FBlocks FBlocks_ │ │ │ │ │ +All ZDim*D F blocks (one for each camera) │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_g_r_a_d_i_e_n_t_A_t_Z_e_r_o │ │ │ │ │ +VectorValues gradientAtZero() const override │ │ │ │ │ +Calculate gradient, which is -F'Q*b, see paper. │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:457 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_M_a_t_r_i_x_Z_D │ │ │ │ │ +Eigen::Matrix< double, ZDim, D > MatrixZD │ │ │ │ │ +type of an F block │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:54 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_P_o_i_n_t_C_o_v_a_r_i_a_n_c_e__ │ │ │ │ │ +const Matrix PointCovariance_ │ │ │ │ │ +the 3*3 matrix P = inv(E'E) (2*2 if degenerate) │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:59 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d │ │ │ │ │ +void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) │ │ │ │ │ +const override │ │ │ │ │ +Hessian-vector multiply, i.e. │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:413 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_h_e_s_s_i_a_n_B_l_o_c_k_D_i_a_g_o_n_a_l │ │ │ │ │ +std::map< Key, Matrix > hessianBlockDiagonal() const override │ │ │ │ │ +Return the block diagonal of the Hessian for this factor. │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:231 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_a_u_g_m_e_n_t_e_d_I_n_f_o_r_m_a_t_i_o_n │ │ │ │ │ +Matrix augmentedInformation() const override │ │ │ │ │ +Compute full augmented information matrix │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:155 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_n_e_g_a_t_e │ │ │ │ │ +GaussianFactor::shared_ptr negate() const override │ │ │ │ │ +Construct the corresponding anti-factor to negate information stored stored in │ │ │ │ │ +this factor. │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:263 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_e_1 │ │ │ │ │ +Error2s e1 │ │ │ │ │ +Scratch space for multiplyHessianAdd. │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:374 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ void print(const std::string &s="", const KeyFormatter │ │ │ │ │ &keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ print │ │ │ │ │ -DDeeffiinniittiioonn StereoFactor.h:103 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_C_a_m_P_o_s_e │ │ │ │ │ -POSE CamPose │ │ │ │ │ -typedef for Pose Lie Value type │ │ │ │ │ -DDeeffiinniittiioonn StereoFactor.h:49 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_B_a_s_e │ │ │ │ │ -NoiseModelFactorN< POSE, LANDMARK > Base │ │ │ │ │ -typedef for base class │ │ │ │ │ -DDeeffiinniittiioonn StereoFactor.h:46 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ -gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn StereoFactor.h:94 │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:105 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_g_e_t_D_i_m │ │ │ │ │ +DenseIndex getDim(const_iterator variable) const override │ │ │ │ │ +Degrees of freedom of camera. │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:134 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const GaussianFactor &lf, double tol) const override │ │ │ │ │ +equals │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:118 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_a_u_g_m_e_n_t_e_d_J_a_c_o_b_i_a_n │ │ │ │ │ +Matrix augmentedJacobian() const override │ │ │ │ │ +Return a dense Jacobian matrix, augmented with b with the noise models baked │ │ │ │ │ +into A and b. │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:143 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_T_h_i_s │ │ │ │ │ +RegularImplicitSchurFactor This │ │ │ │ │ +Typedef to this class. │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_D │ │ │ │ │ +static const int D │ │ │ │ │ +Camera dimension. │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:51 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_p_r_o_j_e_c_t_E_r_r_o_r_2 │ │ │ │ │ +void projectError2(const Error2s &e1, Error2s &e2) const │ │ │ │ │ +Calculate corrected error Q*(e-ZDim*b) = (I - E*P*E')*(e-ZDim*b) │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:287 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d │ │ │ │ │ +void multiplyHessianAdd(double alpha, const double *x, double *y) const │ │ │ │ │ +double* Hessian-vector multiply, i.e. │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:380 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_h_e_s_s_i_a_n_D_i_a_g_o_n_a_l │ │ │ │ │ +void hessianDiagonal(double *d) const override │ │ │ │ │ +add the contribution of this factor to the diagonal of the hessian d(output) = │ │ │ │ │ +d(input) + deltaHessia... │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:205 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_D_u_m_m_y │ │ │ │ │ +void multiplyHessianDummy(double alpha, const VectorValues &x, VectorValues &y) │ │ │ │ │ +const │ │ │ │ │ +Dummy version to measure overhead of key access. │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:442 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_:_:_g_r_a_d_i_e_n_t │ │ │ │ │ +Vector gradient(Key key, const VectorValues &x) const override │ │ │ │ │ +Gradient wrt a key at any values. │ │ │ │ │ +DDeeffiinniittiioonn RegularImplicitSchurFactor.h:499 │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +The Factor::error simply extracts the. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _S_t_e_r_e_o_F_a_c_t_o_r_._h │ │ │ │ │ + * _R_e_g_u_l_a_r_I_m_p_l_i_c_i_t_S_c_h_u_r_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01313.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/SmartProjectionPoseFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/StereoFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,49 +96,47 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
SmartProjectionPoseFactor.h File Reference
│ │ │ │ +
StereoFactor.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Smart factor on poses, assuming camera calibration is fixed. │ │ │ │ +

A non-linear factor for stereo measurements. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::SmartProjectionPoseFactor< CALIBRATION >
 If you are using the factor, please cite: L. More...
class  gtsam::GenericStereoFactor< POSE, LANDMARK >
 A Generic Stereo Factor. More...
 
struct  gtsam::traits< SmartProjectionPoseFactor< CALIBRATION > >
 traits More...
struct  gtsam::traits< GenericStereoFactor< T1, T2 > >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Smart factor on poses, assuming camera calibration is fixed.

│ │ │ │ -
Author
Luca Carlone
│ │ │ │ +

A non-linear factor for stereo measurements.

│ │ │ │ +
Author
Alireza Fathi
│ │ │ │
│ │ │ │ -Chris Beall
│ │ │ │ -
│ │ │ │ -Zsolt Kira
│ │ │ │ +Chris Beall
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,30 +1,29 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SmartProjectionPoseFactor.h File Reference │ │ │ │ │ -Smart factor on poses, assuming camera calibration is fixed. _M_o_r_e_._._. │ │ │ │ │ +StereoFactor.h File Reference │ │ │ │ │ +A non-linear factor for stereo measurements. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_<_ _C_A_L_I_B_R_A_T_I_O_N_ _> │ │ │ │ │ -  If you are using the factor, please cite: L. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_<_ _P_O_S_E_,_ _L_A_N_D_M_A_R_K_ _> │ │ │ │ │ +  A Generic Stereo _F_a_c_t_o_r. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_<_ _C_A_L_I_B_R_A_T_I_O_N_ _>_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_<_ _T_1_,_ _T_2_ _>_ _> │ │ │ │ │   traits _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Smart factor on poses, assuming camera calibration is fixed. │ │ │ │ │ +A non-linear factor for stereo measurements. │ │ │ │ │ Author │ │ │ │ │ - Luca Carlone │ │ │ │ │ + Alireza Fathi │ │ │ │ │ Chris Beall │ │ │ │ │ - Zsolt Kira │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_._h │ │ │ │ │ + * _S_t_e_r_e_o_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01313.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a01313 = [ │ │ │ │ │ - ["gtsam::traits< SmartProjectionPoseFactor< CALIBRATION > >", "a04948.html", null] │ │ │ │ │ + ["gtsam::traits< GenericStereoFactor< T1, T2 > >", "a04964.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01313_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/SmartProjectionPoseFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/StereoFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,176 +98,218 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
SmartProjectionPoseFactor.h
│ │ │ │ +
StereoFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/slam/SmartProjectionFactor.h>
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
22#include <gtsam/geometry/StereoCamera.h>
│ │ │ │
23
│ │ │ │
24namespace gtsam {
│ │ │ │ -
44template <class CALIBRATION>
│ │ │ │ -
│ │ │ │ -
45class SmartProjectionPoseFactor
│ │ │ │ -
46 : public SmartProjectionFactor<PinholePose<CALIBRATION> > {
│ │ │ │ -
47 private:
│ │ │ │ -
48 typedef PinholePose<CALIBRATION> Camera;
│ │ │ │ -
49 typedef SmartProjectionFactor<Camera> Base;
│ │ │ │ -
50 typedef SmartProjectionPoseFactor<CALIBRATION> This;
│ │ │ │ -
51
│ │ │ │ -
52protected:
│ │ │ │ -
53
│ │ │ │ -
54 boost::shared_ptr<CALIBRATION> K_;
│ │ │ │ -
55
│ │ │ │ -
56public:
│ │ │ │ -
57
│ │ │ │ -
59 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
60
│ │ │ │ -
64 SmartProjectionPoseFactor() {}
│ │ │ │ -
65
│ │ │ │ -
│ │ │ │ -
72 SmartProjectionPoseFactor(
│ │ │ │ -
73 const SharedNoiseModel& sharedNoiseModel,
│ │ │ │ -
74 const boost::shared_ptr<CALIBRATION> K,
│ │ │ │ -
75 const SmartProjectionParams& params = SmartProjectionParams())
│ │ │ │ -
76 : Base(sharedNoiseModel, params), K_(K) {
│ │ │ │ -
77 }
│ │ │ │ -
│ │ │ │ -
78
│ │ │ │ -
│ │ │ │ -
86 SmartProjectionPoseFactor(
│ │ │ │ -
87 const SharedNoiseModel& sharedNoiseModel,
│ │ │ │ -
88 const boost::shared_ptr<CALIBRATION> K,
│ │ │ │ -
89 const boost::optional<Pose3> body_P_sensor,
│ │ │ │ -
90 const SmartProjectionParams& params = SmartProjectionParams())
│ │ │ │ -
91 : SmartProjectionPoseFactor(sharedNoiseModel, K, params) {
│ │ │ │ -
92 this->body_P_sensor_ = body_P_sensor;
│ │ │ │ -
93 }
│ │ │ │ -
│ │ │ │ -
94
│ │ │ │ -
│ │ │ │ -
96 ~SmartProjectionPoseFactor() override {
│ │ │ │ -
97 }
│ │ │ │ -
│ │ │ │ -
98
│ │ │ │ -
│ │ │ │ -
104 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
│ │ │ │ -
105 DefaultKeyFormatter) const override {
│ │ │ │ -
106 std::cout << s << "SmartProjectionPoseFactor, z = \n ";
│ │ │ │ -
107 Base::print("", keyFormatter);
│ │ │ │ +
25
│ │ │ │ +
30template<class POSE, class LANDMARK>
│ │ │ │ +
│ │ │ │ +
31class GenericStereoFactor: public NoiseModelFactorN<POSE, LANDMARK> {
│ │ │ │ +
32private:
│ │ │ │ +
33
│ │ │ │ +
34 // Keep a copy of measurement and calibration for I/O
│ │ │ │ +
35 StereoPoint2 measured_;
│ │ │ │ +
36 Cal3_S2Stereo::shared_ptr K_;
│ │ │ │ +
37 boost::optional<POSE> body_P_sensor_;
│ │ │ │ +
38
│ │ │ │ +
39 // verbosity handling for Cheirality Exceptions
│ │ │ │ +
40 bool throwCheirality_;
│ │ │ │ +
41 bool verboseCheirality_;
│ │ │ │ +
42
│ │ │ │ +
43public:
│ │ │ │ +
44
│ │ │ │ +
45 // shorthand for base class type
│ │ │ │ +
46 typedef NoiseModelFactorN<POSE, LANDMARK> Base;
│ │ │ │ +
47 typedef GenericStereoFactor<POSE, LANDMARK> This;
│ │ │ │ +
48 typedef boost::shared_ptr<GenericStereoFactor> shared_ptr;
│ │ │ │ +
49 typedef POSE CamPose;
│ │ │ │ +
50
│ │ │ │ +
│ │ │ │ +
54 GenericStereoFactor() : K_(new Cal3_S2Stereo(444, 555, 666, 777, 888, 1.0)),
│ │ │ │ +
55 throwCheirality_(false), verboseCheirality_(false) {}
│ │ │ │ +
│ │ │ │ +
56
│ │ │ │ +
│ │ │ │ +
66 GenericStereoFactor(const StereoPoint2& measured, const SharedNoiseModel& model,
│ │ │ │ +
67 Key poseKey, Key landmarkKey, const Cal3_S2Stereo::shared_ptr& K,
│ │ │ │ +
68 boost::optional<POSE> body_P_sensor = boost::none) :
│ │ │ │ +
69 Base(model, poseKey, landmarkKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
│ │ │ │ +
70 throwCheirality_(false), verboseCheirality_(false) {}
│ │ │ │ +
│ │ │ │ +
71
│ │ │ │ +
│ │ │ │ +
83 GenericStereoFactor(const StereoPoint2& measured, const SharedNoiseModel& model,
│ │ │ │ +
84 Key poseKey, Key landmarkKey, const Cal3_S2Stereo::shared_ptr& K,
│ │ │ │ +
85 bool throwCheirality, bool verboseCheirality,
│ │ │ │ +
86 boost::optional<POSE> body_P_sensor = boost::none) :
│ │ │ │ +
87 Base(model, poseKey, landmarkKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
│ │ │ │ +
88 throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
│ │ │ │ +
│ │ │ │ +
89
│ │ │ │ +
91 ~GenericStereoFactor() override {}
│ │ │ │ +
92
│ │ │ │ +
│ │ │ │ +
94 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
95 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ +
96 gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
│ │ │ │ +
│ │ │ │ +
97
│ │ │ │ +
│ │ │ │ +
103 void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ +
104 Base::print(s, keyFormatter);
│ │ │ │ +
105 measured_.print(s + ".z");
│ │ │ │ +
106 if(this->body_P_sensor_)
│ │ │ │ +
107 this->body_P_sensor_->print(" sensor pose in body frame: ");
│ │ │ │
108 }
│ │ │ │
│ │ │ │
109
│ │ │ │ -
│ │ │ │ -
111 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
│ │ │ │ -
112 const This *e = dynamic_cast<const This*>(&p);
│ │ │ │ -
113 return e && Base::equals(p, tol);
│ │ │ │ -
114 }
│ │ │ │ -
│ │ │ │ -
115
│ │ │ │ -
│ │ │ │ -
119 double error(const Values& values) const override {
│ │ │ │ -
120 if (this->active(values)) {
│ │ │ │ -
121 return this->totalReprojectionError(cameras(values));
│ │ │ │ -
122 } else { // else of active flag
│ │ │ │ -
123 return 0.0;
│ │ │ │ -
124 }
│ │ │ │ -
125 }
│ │ │ │ -
│ │ │ │ -
126
│ │ │ │ -
│ │ │ │ -
128 inline const boost::shared_ptr<CALIBRATION> calibration() const {
│ │ │ │ -
129 return K_;
│ │ │ │ -
130 }
│ │ │ │ -
│ │ │ │ -
131
│ │ │ │ -
│ │ │ │ -
138 typename Base::Cameras cameras(const Values& values) const override {
│ │ │ │ -
139 typename Base::Cameras cameras;
│ │ │ │ -
140 for (const Key& k : this->keys_) {
│ │ │ │ -
141 const Pose3 world_P_sensor_k =
│ │ │ │ -
142 Base::body_P_sensor_ ? values.at<Pose3>(k) * *Base::body_P_sensor_
│ │ │ │ -
143 : values.at<Pose3>(k);
│ │ │ │ -
144 cameras.emplace_back(world_P_sensor_k, K_);
│ │ │ │ -
145 }
│ │ │ │ -
146 return cameras;
│ │ │ │ -
147 }
│ │ │ │ -
│ │ │ │ -
148
│ │ │ │ -
149 private:
│ │ │ │ -
150
│ │ │ │ -
152 friend class boost::serialization::access;
│ │ │ │ -
153 template<class ARCHIVE>
│ │ │ │ -
154 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
155 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
156 ar & BOOST_SERIALIZATION_NVP(K_);
│ │ │ │ -
157 }
│ │ │ │ -
158};
│ │ │ │ -
│ │ │ │ -
159// end of class declaration
│ │ │ │ -
160
│ │ │ │ -
162template<class CALIBRATION>
│ │ │ │ -
│ │ │ │ -
163struct traits<SmartProjectionPoseFactor<CALIBRATION> > : public Testable<
│ │ │ │ -
164 SmartProjectionPoseFactor<CALIBRATION> > {
│ │ │ │ -
165};
│ │ │ │ -
│ │ │ │ -
166
│ │ │ │ -
167} // \ namespace gtsam
│ │ │ │ -
SmartProjectionFactor.h
Smart factor on cameras (pose + calibration)
│ │ │ │ +
│ │ │ │ +
113 bool equals(const NonlinearFactor& f, double tol = 1e-9) const override {
│ │ │ │ +
114 const GenericStereoFactor* e = dynamic_cast<const GenericStereoFactor*> (&f);
│ │ │ │ +
115 return e
│ │ │ │ +
116 && Base::equals(f)
│ │ │ │ +
117 && measured_.equals(e->measured_, tol)
│ │ │ │ +
118 && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
│ │ │ │ +
119 }
│ │ │ │ +
│ │ │ │ +
120
│ │ │ │ +
│ │ │ │ +
122 Vector evaluateError(const Pose3& pose, const Point3& point,
│ │ │ │ +
123 boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
│ │ │ │ +
124 try {
│ │ │ │ +
125 if(body_P_sensor_) {
│ │ │ │ +
126 if(H1) {
│ │ │ │ +
127 gtsam::Matrix H0;
│ │ │ │ +
128 StereoCamera stereoCam(pose.compose(*body_P_sensor_, H0), K_);
│ │ │ │ +
129 StereoPoint2 reprojectionError(stereoCam.project(point, H1, H2) - measured_);
│ │ │ │ +
130 *H1 = *H1 * H0;
│ │ │ │ +
131 return reprojectionError.vector();
│ │ │ │ +
132 } else {
│ │ │ │ +
133 StereoCamera stereoCam(pose.compose(*body_P_sensor_), K_);
│ │ │ │ +
134 return (stereoCam.project(point, H1, H2) - measured_).vector();
│ │ │ │ +
135 }
│ │ │ │ +
136 } else {
│ │ │ │ +
137 StereoCamera stereoCam(pose, K_);
│ │ │ │ +
138 return (stereoCam.project(point, H1, H2) - measured_).vector();
│ │ │ │ +
139 }
│ │ │ │ +
140 } catch(StereoCheiralityException& e) {
│ │ │ │ +
141 if (H1) *H1 = Matrix::Zero(3,6);
│ │ │ │ +
142 if (H2) *H2 = Z_3x3;
│ │ │ │ +
143 if (verboseCheirality_)
│ │ │ │ +
144 std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
│ │ │ │ +
145 " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
│ │ │ │ +
146 if (throwCheirality_)
│ │ │ │ +
147 throw StereoCheiralityException(this->key2());
│ │ │ │ +
148 }
│ │ │ │ +
149 return Vector3::Constant(2.0 * K_->fx());
│ │ │ │ +
150 }
│ │ │ │ +
│ │ │ │ +
151
│ │ │ │ +
│ │ │ │ +
153 const StereoPoint2& measured() const {
│ │ │ │ +
154 return measured_;
│ │ │ │ +
155 }
│ │ │ │ +
│ │ │ │ +
156
│ │ │ │ +
│ │ │ │ +
158 inline const Cal3_S2Stereo::shared_ptr calibration() const {
│ │ │ │ +
159 return K_;
│ │ │ │ +
160 }
│ │ │ │ +
│ │ │ │ +
161
│ │ │ │ +
163 inline bool verboseCheirality() const { return verboseCheirality_; }
│ │ │ │ +
164
│ │ │ │ +
166 inline bool throwCheirality() const { return throwCheirality_; }
│ │ │ │ +
167
│ │ │ │ +
168private:
│ │ │ │ +
170 friend class boost::serialization::access;
│ │ │ │ +
171 template<class Archive>
│ │ │ │ +
172 void serialize(Archive & ar, const unsigned int /*version*/) {
│ │ │ │ +
173 // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ +
174 ar & boost::serialization::make_nvp("NoiseModelFactor2",
│ │ │ │ +
175 boost::serialization::base_object<Base>(*this));
│ │ │ │ +
176 ar & BOOST_SERIALIZATION_NVP(measured_);
│ │ │ │ +
177 ar & BOOST_SERIALIZATION_NVP(K_);
│ │ │ │ +
178 ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
│ │ │ │ +
179 ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
│ │ │ │ +
180 ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
│ │ │ │ +
181 }
│ │ │ │ +
182};
│ │ │ │ +
│ │ │ │ +
183
│ │ │ │ +
185template<class T1, class T2>
│ │ │ │ +
186struct traits<GenericStereoFactor<T1, T2> > : public Testable<GenericStereoFactor<T1, T2> > {};
│ │ │ │ +
187
│ │ │ │ +
188} // \ namespace gtsam
│ │ │ │ +
StereoCamera.h
A Stereo Camera based on two Simple Cameras.
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::PinholePose
A pinhole camera class that has a Pose3 and a fixed Calibration.
Definition PinholePose.h:243
│ │ │ │ +
gtsam::Cal3_S2Stereo
The most common 5DOF 3D->2D calibration, stereo version.
Definition Cal3_S2Stereo.h:30
│ │ │ │
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ -
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
│ │ │ │ +
gtsam::StereoCheiralityException
Definition StereoCamera.h:26
│ │ │ │ +
gtsam::StereoCamera
A stereo camera class, parameterize by left camera pose and stereo calibration.
Definition StereoCamera.h:47
│ │ │ │ +
gtsam::StereoCamera::project
StereoPoint2 project(const Point3 &point) const
Project 3D point to StereoPoint2 (uL,uR,v)
Definition StereoCamera.cpp:32
│ │ │ │ +
gtsam::StereoPoint2
A 2D stereo point, v will be same for rectified images.
Definition StereoPoint2.h:32
│ │ │ │ +
gtsam::StereoPoint2::vector
Vector3 vector() const
convert to vector
Definition StereoPoint2.h:115
│ │ │ │ +
gtsam::StereoPoint2::equals
bool equals(const StereoPoint2 &q, double tol=1e-9) const
equals
Definition StereoPoint2.h:64
│ │ │ │ +
gtsam::StereoPoint2::print
void print(const std::string &s="") const
print
Definition StereoPoint2.cpp:26
│ │ │ │ +
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
│ │ │ │ +
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
│ │ │ │
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ -
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition NonlinearFactor.h:118
│ │ │ │ -
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
gtsam::Values::at
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
│ │ │ │ -
gtsam::SmartFactorBase::body_P_sensor_
boost::optional< Pose3 > body_P_sensor_
Pose of the camera in the body frame.
Definition SmartFactorBase.h:82
│ │ │ │ -
gtsam::SmartProjectionParams
Definition SmartFactorParams.h:42
│ │ │ │ -
gtsam::SmartProjectionFactor
SmartProjectionFactor: triangulates point and keeps an estimate of it around.
Definition SmartProjectionFactor.h:45
│ │ │ │ -
gtsam::SmartProjectionFactor< PinholePose< CALIBRATION > >::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, boost::optional< Point3 > externalPoint=boost::none) const
Calculate the error of the factor.
Definition SmartProjectionFactor.h:411
│ │ │ │ -
gtsam::SmartProjectionPoseFactor
If you are using the factor, please cite: L.
Definition SmartProjectionPoseFactor.h:46
│ │ │ │ -
gtsam::SmartProjectionPoseFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition SmartProjectionPoseFactor.h:111
│ │ │ │ -
gtsam::SmartProjectionPoseFactor::SmartProjectionPoseFactor
SmartProjectionPoseFactor(const SharedNoiseModel &sharedNoiseModel, const boost::shared_ptr< CALIBRATION > K, const SmartProjectionParams &params=SmartProjectionParams())
Constructor.
Definition SmartProjectionPoseFactor.h:72
│ │ │ │ -
gtsam::SmartProjectionPoseFactor::cameras
Base::Cameras cameras(const Values &values) const override
Collect all cameras involved in this factor.
Definition SmartProjectionPoseFactor.h:138
│ │ │ │ -
gtsam::SmartProjectionPoseFactor::error
double error(const Values &values) const override
error calculates the error of the factor.
Definition SmartProjectionPoseFactor.h:119
│ │ │ │ -
gtsam::SmartProjectionPoseFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition SmartProjectionPoseFactor.h:59
│ │ │ │ -
gtsam::SmartProjectionPoseFactor::K_
boost::shared_ptr< CALIBRATION > K_
calibration object (one for all cameras)
Definition SmartProjectionPoseFactor.h:54
│ │ │ │ -
gtsam::SmartProjectionPoseFactor::~SmartProjectionPoseFactor
~SmartProjectionPoseFactor() override
Virtual destructor.
Definition SmartProjectionPoseFactor.h:96
│ │ │ │ -
gtsam::SmartProjectionPoseFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition SmartProjectionPoseFactor.h:104
│ │ │ │ -
gtsam::SmartProjectionPoseFactor::calibration
const boost::shared_ptr< CALIBRATION > calibration() const
return calibration shared pointers
Definition SmartProjectionPoseFactor.h:128
│ │ │ │ -
gtsam::SmartProjectionPoseFactor::SmartProjectionPoseFactor
SmartProjectionPoseFactor(const SharedNoiseModel &sharedNoiseModel, const boost::shared_ptr< CALIBRATION > K, const boost::optional< Pose3 > body_P_sensor, const SmartProjectionParams &params=SmartProjectionParams())
Constructor.
Definition SmartProjectionPoseFactor.h:86
│ │ │ │ -
gtsam::SmartProjectionPoseFactor::access
friend class boost::serialization::access
Serialization function.
Definition SmartProjectionPoseFactor.h:152
│ │ │ │ -
gtsam::SmartProjectionPoseFactor::SmartProjectionPoseFactor
SmartProjectionPoseFactor()
Default constructor, only for serialization.
Definition SmartProjectionPoseFactor.h:64
│ │ │ │ +
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ +
gtsam::GenericStereoFactor
A Generic Stereo Factor.
Definition StereoFactor.h:31
│ │ │ │ +
gtsam::GenericStereoFactor::throwCheirality
bool throwCheirality() const
return flag for throwing cheirality exceptions
Definition StereoFactor.h:166
│ │ │ │ +
gtsam::GenericStereoFactor::evaluateError
Vector evaluateError(const Pose3 &pose, const Point3 &point, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
h(x)-z
Definition StereoFactor.h:122
│ │ │ │ +
gtsam::GenericStereoFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
equals
Definition StereoFactor.h:113
│ │ │ │ +
gtsam::GenericStereoFactor::calibration
const Cal3_S2Stereo::shared_ptr calibration() const
return the calibration object
Definition StereoFactor.h:158
│ │ │ │ +
gtsam::GenericStereoFactor::GenericStereoFactor
GenericStereoFactor(const StereoPoint2 &measured, const SharedNoiseModel &model, Key poseKey, Key landmarkKey, const Cal3_S2Stereo::shared_ptr &K, bool throwCheirality, bool verboseCheirality, boost::optional< POSE > body_P_sensor=boost::none)
Constructor with exception-handling flags.
Definition StereoFactor.h:83
│ │ │ │ +
gtsam::GenericStereoFactor::This
GenericStereoFactor< POSE, LANDMARK > This
typedef for this class (with templates)
Definition StereoFactor.h:47
│ │ │ │ +
gtsam::GenericStereoFactor::~GenericStereoFactor
~GenericStereoFactor() override
Virtual destructor.
Definition StereoFactor.h:91
│ │ │ │ +
gtsam::GenericStereoFactor::verboseCheirality
bool verboseCheirality() const
return verbosity
Definition StereoFactor.h:163
│ │ │ │ +
gtsam::GenericStereoFactor::shared_ptr
boost::shared_ptr< GenericStereoFactor > shared_ptr
typedef for shared pointer to this object
Definition StereoFactor.h:48
│ │ │ │ +
gtsam::GenericStereoFactor::measured
const StereoPoint2 & measured() const
return the measured
Definition StereoFactor.h:153
│ │ │ │ +
gtsam::GenericStereoFactor::GenericStereoFactor
GenericStereoFactor()
Default constructor.
Definition StereoFactor.h:54
│ │ │ │ +
gtsam::GenericStereoFactor::access
friend class boost::serialization::access
Serialization function.
Definition StereoFactor.h:170
│ │ │ │ +
gtsam::GenericStereoFactor::GenericStereoFactor
GenericStereoFactor(const StereoPoint2 &measured, const SharedNoiseModel &model, Key poseKey, Key landmarkKey, const Cal3_S2Stereo::shared_ptr &K, boost::optional< POSE > body_P_sensor=boost::none)
Constructor.
Definition StereoFactor.h:66
│ │ │ │ +
gtsam::GenericStereoFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition StereoFactor.h:103
│ │ │ │ +
gtsam::GenericStereoFactor::CamPose
POSE CamPose
typedef for Pose Lie Value type
Definition StereoFactor.h:49
│ │ │ │ +
gtsam::GenericStereoFactor::Base
NoiseModelFactorN< POSE, LANDMARK > Base
typedef for base class
Definition StereoFactor.h:46
│ │ │ │ +
gtsam::GenericStereoFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition StereoFactor.h:94
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,125 +1,177 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SmartProjectionPoseFactor.h │ │ │ │ │ +StereoFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_s_l_a_m_/_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_._h> │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_S_t_e_r_e_o_C_a_m_e_r_a_._h> │ │ │ │ │ 23 │ │ │ │ │ 24namespace _g_t_s_a_m { │ │ │ │ │ -44template │ │ │ │ │ -_4_5class _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r │ │ │ │ │ -46 : public _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r > { │ │ │ │ │ -47 private: │ │ │ │ │ -48 typedef _P_i_n_h_o_l_e_P_o_s_e_<_C_A_L_I_B_R_A_T_I_O_N_> Camera; │ │ │ │ │ -49 typedef _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_<_C_a_m_e_r_a_> Base; │ │ │ │ │ -50 typedef _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_<_C_A_L_I_B_R_A_T_I_O_N_> This; │ │ │ │ │ -51 │ │ │ │ │ -52protected: │ │ │ │ │ -53 │ │ │ │ │ -_5_4 boost::shared_ptr _K__; │ │ │ │ │ -55 │ │ │ │ │ -56public: │ │ │ │ │ -57 │ │ │ │ │ -_5_9 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -60 │ │ │ │ │ -_6_4 _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r() {} │ │ │ │ │ -65 │ │ │ │ │ -_7_2 _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r( │ │ │ │ │ -73 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& sharedNoiseModel, │ │ │ │ │ -74 const boost::shared_ptr K, │ │ │ │ │ -75 const _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s& params = _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s()) │ │ │ │ │ -76 : Base(sharedNoiseModel, params), _K__(K) { │ │ │ │ │ -77 } │ │ │ │ │ -78 │ │ │ │ │ -_8_6 _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r( │ │ │ │ │ -87 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& sharedNoiseModel, │ │ │ │ │ -88 const boost::shared_ptr K, │ │ │ │ │ -89 const boost::optional body_P_sensor, │ │ │ │ │ -90 const _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s& params = _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s()) │ │ │ │ │ -91 : _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r(sharedNoiseModel, K, params) { │ │ │ │ │ -92 this->_b_o_d_y___P___s_e_n_s_o_r__ = body_P_sensor; │ │ │ │ │ -93 } │ │ │ │ │ -94 │ │ │ │ │ -_9_6 _~_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r() override { │ │ │ │ │ -97 } │ │ │ │ │ -98 │ │ │ │ │ -_1_0_4 void _p_r_i_n_t(const std::string& s = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ -105 DefaultKeyFormatter) const override { │ │ │ │ │ -106 std::cout << s << "SmartProjectionPoseFactor, z = \n "; │ │ │ │ │ -107 Base::print("", keyFormatter); │ │ │ │ │ +25 │ │ │ │ │ +30template │ │ │ │ │ +_3_1class _G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +32private: │ │ │ │ │ +33 │ │ │ │ │ +34 // Keep a copy of measurement and calibration for I/O │ │ │ │ │ +35 _S_t_e_r_e_o_P_o_i_n_t_2 measured_; │ │ │ │ │ +36 Cal3_S2Stereo::shared_ptr K_; │ │ │ │ │ +37 boost::optional body_P_sensor_; │ │ │ │ │ +38 │ │ │ │ │ +39 // verbosity handling for Cheirality Exceptions │ │ │ │ │ +40 bool throwCheirality_; │ │ │ │ │ +41 bool verboseCheirality_; │ │ │ │ │ +42 │ │ │ │ │ +43public: │ │ │ │ │ +44 │ │ │ │ │ +45 // shorthand for base class type │ │ │ │ │ +_4_6 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_P_O_S_E_,_ _L_A_N_D_M_A_R_K_> _B_a_s_e; │ │ │ │ │ +_4_7 typedef _G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_<_P_O_S_E_,_ _L_A_N_D_M_A_R_K_> _T_h_i_s; │ │ │ │ │ +_4_8 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +_4_9 typedef POSE _C_a_m_P_o_s_e; │ │ │ │ │ +50 │ │ │ │ │ +_5_4 _G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r() : K_(new _C_a_l_3___S_2_S_t_e_r_e_o(444, 555, 666, 777, 888, 1.0)), │ │ │ │ │ +55 throwCheirality_(false), verboseCheirality_(false) {} │ │ │ │ │ +56 │ │ │ │ │ +_6_6 _G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r(const _S_t_e_r_e_o_P_o_i_n_t_2& _m_e_a_s_u_r_e_d, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& │ │ │ │ │ +model, │ │ │ │ │ +67 _K_e_y poseKey, _K_e_y landmarkKey, const Cal3_S2Stereo::shared_ptr& K, │ │ │ │ │ +68 boost::optional body_P_sensor = boost::none) : │ │ │ │ │ +69 _B_a_s_e(model, poseKey, landmarkKey), measured_(_m_e_a_s_u_r_e_d), K_(K), │ │ │ │ │ +body_P_sensor_(body_P_sensor), │ │ │ │ │ +70 throwCheirality_(false), verboseCheirality_(false) {} │ │ │ │ │ +71 │ │ │ │ │ +_8_3 _G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r(const _S_t_e_r_e_o_P_o_i_n_t_2& _m_e_a_s_u_r_e_d, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& │ │ │ │ │ +model, │ │ │ │ │ +84 _K_e_y poseKey, _K_e_y landmarkKey, const Cal3_S2Stereo::shared_ptr& K, │ │ │ │ │ +85 bool _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y, bool _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y, │ │ │ │ │ +86 boost::optional body_P_sensor = boost::none) : │ │ │ │ │ +87 _B_a_s_e(model, poseKey, landmarkKey), measured_(_m_e_a_s_u_r_e_d), K_(K), │ │ │ │ │ +body_P_sensor_(body_P_sensor), │ │ │ │ │ +88 throwCheirality_(_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y), verboseCheirality_(_v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y) {} │ │ │ │ │ +89 │ │ │ │ │ +_9_1 _~_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r() override {} │ │ │ │ │ +92 │ │ │ │ │ +_9_4 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +95 return boost::static_pointer_cast( │ │ │ │ │ +96 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); } │ │ │ │ │ +97 │ │ │ │ │ +_1_0_3 void _p_r_i_n_t(const std::string& s = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ +DefaultKeyFormatter) const override { │ │ │ │ │ +104 _B_a_s_e_:_:_p_r_i_n_t(s, keyFormatter); │ │ │ │ │ +105 measured_._p_r_i_n_t(s + ".z"); │ │ │ │ │ +106 if(this->body_P_sensor_) │ │ │ │ │ +107 this->body_P_sensor_->print(" sensor pose in body frame: "); │ │ │ │ │ 108 } │ │ │ │ │ 109 │ │ │ │ │ -_1_1_1 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& p, double tol = 1e-9) const override { │ │ │ │ │ -112 const This *e = dynamic_cast(&p); │ │ │ │ │ -113 return e && Base::equals(p, tol); │ │ │ │ │ -114 } │ │ │ │ │ -115 │ │ │ │ │ -_1_1_9 double _e_r_r_o_r(const _V_a_l_u_e_s& values) const override { │ │ │ │ │ -120 if (this->_a_c_t_i_v_e(values)) { │ │ │ │ │ -121 return this->_t_o_t_a_l_R_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r(_c_a_m_e_r_a_s(values)); │ │ │ │ │ -122 } else { // else of active flag │ │ │ │ │ -123 return 0.0; │ │ │ │ │ -124 } │ │ │ │ │ -125 } │ │ │ │ │ -126 │ │ │ │ │ -_1_2_8 inline const boost::shared_ptr _c_a_l_i_b_r_a_t_i_o_n() const { │ │ │ │ │ -129 return _K__; │ │ │ │ │ -130 } │ │ │ │ │ -131 │ │ │ │ │ -_1_3_8 typename Base::Cameras _c_a_m_e_r_a_s(const _V_a_l_u_e_s& values) const override { │ │ │ │ │ -139 typename Base::Cameras _c_a_m_e_r_a_s; │ │ │ │ │ -140 for (const _K_e_y& k : this->_k_e_y_s__) { │ │ │ │ │ -141 const _P_o_s_e_3 world_P_sensor_k = │ │ │ │ │ -142 Base::body_P_sensor_ ? values._a_t<_P_o_s_e_3>(k) * *Base::body_P_sensor_ │ │ │ │ │ -143 : values._a_t<_P_o_s_e_3>(k); │ │ │ │ │ -144 _c_a_m_e_r_a_s.emplace_back(world_P_sensor_k, _K__); │ │ │ │ │ -145 } │ │ │ │ │ -146 return _c_a_m_e_r_a_s; │ │ │ │ │ -147 } │ │ │ │ │ -148 │ │ │ │ │ -149 private: │ │ │ │ │ -150 │ │ │ │ │ -_1_5_2 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -153 template │ │ │ │ │ -154 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -155 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); │ │ │ │ │ -156 ar & BOOST_SERIALIZATION_NVP(_K__); │ │ │ │ │ -157 } │ │ │ │ │ -158}; │ │ │ │ │ -159// end of class declaration │ │ │ │ │ -160 │ │ │ │ │ -162template │ │ │ │ │ -_1_6_3struct _t_r_a_i_t_s<_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r > : public _T_e_s_t_a_b_l_e< │ │ │ │ │ -164 SmartProjectionPoseFactor > { │ │ │ │ │ -165}; │ │ │ │ │ -166 │ │ │ │ │ -167} // \ namespace gtsam │ │ │ │ │ -_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_._h │ │ │ │ │ -Smart factor on cameras (pose + calibration) │ │ │ │ │ +_1_1_3 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& f, double tol = 1e-9) const override { │ │ │ │ │ +114 const _G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r* e = dynamic_cast │ │ │ │ │ +(&f); │ │ │ │ │ +115 return e │ │ │ │ │ +116 && _B_a_s_e_:_:_e_q_u_a_l_s(f) │ │ │ │ │ +117 && measured_._e_q_u_a_l_s(e->measured_, tol) │ │ │ │ │ +118 && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e- │ │ │ │ │ +>body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); │ │ │ │ │ +119 } │ │ │ │ │ +120 │ │ │ │ │ +_1_2_2 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const _P_o_s_e_3& pose, const _P_o_i_n_t_3& point, │ │ │ │ │ +123 boost::optional H1 = boost::none, boost::optional H2 = │ │ │ │ │ +boost::none) const override { │ │ │ │ │ +124 try { │ │ │ │ │ +125 if(body_P_sensor_) { │ │ │ │ │ +126 if(H1) { │ │ │ │ │ +127 gtsam::Matrix H0; │ │ │ │ │ +128 _S_t_e_r_e_o_C_a_m_e_r_a stereoCam(pose.compose(*body_P_sensor_, H0), K_); │ │ │ │ │ +129 _S_t_e_r_e_o_P_o_i_n_t_2 reprojectionError(stereoCam._p_r_o_j_e_c_t(point, H1, H2) - │ │ │ │ │ +measured_); │ │ │ │ │ +130 *H1 = *H1 * H0; │ │ │ │ │ +131 return reprojectionError._v_e_c_t_o_r(); │ │ │ │ │ +132 } else { │ │ │ │ │ +133 _S_t_e_r_e_o_C_a_m_e_r_a stereoCam(pose.compose(*body_P_sensor_), K_); │ │ │ │ │ +134 return (stereoCam._p_r_o_j_e_c_t(point, H1, H2) - measured_).vector(); │ │ │ │ │ +135 } │ │ │ │ │ +136 } else { │ │ │ │ │ +137 _S_t_e_r_e_o_C_a_m_e_r_a stereoCam(pose, K_); │ │ │ │ │ +138 return (stereoCam._p_r_o_j_e_c_t(point, H1, H2) - measured_).vector(); │ │ │ │ │ +139 } │ │ │ │ │ +140 } catch(_S_t_e_r_e_o_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n& e) { │ │ │ │ │ +141 if (H1) *H1 = Matrix::Zero(3,6); │ │ │ │ │ +142 if (H2) *H2 = Z_3x3; │ │ │ │ │ +143 if (verboseCheirality_) │ │ │ │ │ +144 std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) │ │ │ │ │ +<< │ │ │ │ │ +145 " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; │ │ │ │ │ +146 if (throwCheirality_) │ │ │ │ │ +147 throw _S_t_e_r_e_o_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n(this->key2()); │ │ │ │ │ +148 } │ │ │ │ │ +149 return Vector3::Constant(2.0 * K_->fx()); │ │ │ │ │ +150 } │ │ │ │ │ +151 │ │ │ │ │ +_1_5_3 const _S_t_e_r_e_o_P_o_i_n_t_2& _m_e_a_s_u_r_e_d() const { │ │ │ │ │ +154 return measured_; │ │ │ │ │ +155 } │ │ │ │ │ +156 │ │ │ │ │ +_1_5_8 inline const Cal3_S2Stereo::shared_ptr _c_a_l_i_b_r_a_t_i_o_n() const { │ │ │ │ │ +159 return K_; │ │ │ │ │ +160 } │ │ │ │ │ +161 │ │ │ │ │ +_1_6_3 inline bool _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y() const { return verboseCheirality_; } │ │ │ │ │ +164 │ │ │ │ │ +_1_6_6 inline bool _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y() const { return throwCheirality_; } │ │ │ │ │ +167 │ │ │ │ │ +168private: │ │ │ │ │ +_1_7_0 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +171 template │ │ │ │ │ +172 void serialize(Archive & ar, const unsigned int /*version*/) { │ │ │ │ │ +173 // NoiseModelFactor2 instead of NoiseModelFactorN for backward │ │ │ │ │ +compatibility │ │ │ │ │ +174 ar & boost::serialization::make_nvp("NoiseModelFactor2", │ │ │ │ │ +175 boost::serialization::base_object(*this)); │ │ │ │ │ +176 ar & BOOST_SERIALIZATION_NVP(measured_); │ │ │ │ │ +177 ar & BOOST_SERIALIZATION_NVP(K_); │ │ │ │ │ +178 ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); │ │ │ │ │ +179 ar & BOOST_SERIALIZATION_NVP(throwCheirality_); │ │ │ │ │ +180 ar & BOOST_SERIALIZATION_NVP(verboseCheirality_); │ │ │ │ │ +181 } │ │ │ │ │ +182}; │ │ │ │ │ +183 │ │ │ │ │ +185template │ │ │ │ │ +_1_8_6struct _t_r_a_i_t_s<_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r > : public │ │ │ │ │ +_T_e_s_t_a_b_l_e > {}; │ │ │ │ │ +187 │ │ │ │ │ +188} // \ namespace gtsam │ │ │ │ │ +_S_t_e_r_e_o_C_a_m_e_r_a_._h │ │ │ │ │ +A Stereo Camera based on two Simple Cameras. │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ +Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ +Vector3 Point3 │ │ │ │ │ +As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ +typedef Point3 to Vector3... │ │ │ │ │ +DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ _g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ Aliases. │ │ │ │ │ DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ @@ -131,106 +183,135 @@ │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ _g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ A helper that implements the traits interface for GTSAM types. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e │ │ │ │ │ -A pinhole camera class that has a Pose3 and a fixed Calibration. │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:243 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o │ │ │ │ │ +The most common 5DOF 3D->2D calibration, stereo version. │ │ │ │ │ +DDeeffiinniittiioonn Cal3_S2Stereo.h:30 │ │ │ │ │ _g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s__ │ │ │ │ │ -KeyVector keys_ │ │ │ │ │ -The keys involved in this factor. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n │ │ │ │ │ +DDeeffiinniittiioonn StereoCamera.h:26 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a │ │ │ │ │ +A stereo camera class, parameterize by left camera pose and stereo calibration. │ │ │ │ │ +DDeeffiinniittiioonn StereoCamera.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a_:_:_p_r_o_j_e_c_t │ │ │ │ │ +StereoPoint2 project(const Point3 &point) const │ │ │ │ │ +Project 3D point to StereoPoint2 (uL,uR,v) │ │ │ │ │ +DDeeffiinniittiioonn StereoCamera.cpp:32 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2 │ │ │ │ │ +A 2D stereo point, v will be same for rectified images. │ │ │ │ │ +DDeeffiinniittiioonn StereoPoint2.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_v_e_c_t_o_r │ │ │ │ │ +Vector3 vector() const │ │ │ │ │ +convert to vector │ │ │ │ │ +DDeeffiinniittiioonn StereoPoint2.h:115 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const StereoPoint2 &q, double tol=1e-9) const │ │ │ │ │ +equals │ │ │ │ │ +DDeeffiinniittiioonn StereoPoint2.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="") const │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn StereoPoint2.cpp:26 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +virtual void print(const std::string &s="Factor", const KeyFormatter │ │ │ │ │ +&formatter=DefaultKeyFormatter) const │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn Factor.cpp:29 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const This &other, double tol=1e-9) const │ │ │ │ │ +check equality │ │ │ │ │ +DDeeffiinniittiioonn Factor.cpp:42 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ Nonlinear factor base class. │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_:_:_a_c_t_i_v_e │ │ │ │ │ -virtual bool active(const Values &) const │ │ │ │ │ -Checks whether a factor should be used based on a set of values. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:118 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_a_t │ │ │ │ │ -const ValueType at(Key j) const │ │ │ │ │ -Retrieve a variable by key j. │ │ │ │ │ -DDeeffiinniittiioonn Values-inl.h:361 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_b_o_d_y___P___s_e_n_s_o_r__ │ │ │ │ │ -boost::optional< Pose3 > body_P_sensor_ │ │ │ │ │ -Pose of the camera in the body frame. │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorBase.h:82 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ -DDeeffiinniittiioonn SmartFactorParams.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -SmartProjectionFactor: triangulates point and keeps an estimate of it around. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_<_ _P_i_n_h_o_l_e_P_o_s_e_<_ _C_A_L_I_B_R_A_T_I_O_N_ _>_ _>_:_: │ │ │ │ │ -_t_o_t_a_l_R_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r │ │ │ │ │ -double totalReprojectionError(const Cameras &cameras, boost::optional< Point3 > │ │ │ │ │ -externalPoint=boost::none) const │ │ │ │ │ -Calculate the error of the factor. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionFactor.h:411 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r │ │ │ │ │ -If you are using the factor, please cite: L. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionPoseFactor.h:46 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const NonlinearFactor &p, double tol=1e-9) const override │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ +A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ +variables. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r │ │ │ │ │ +A Generic Stereo Factor. │ │ │ │ │ +DDeeffiinniittiioonn StereoFactor.h:31 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y │ │ │ │ │ +bool throwCheirality() const │ │ │ │ │ +return flag for throwing cheirality exceptions │ │ │ │ │ +DDeeffiinniittiioonn StereoFactor.h:166 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ +Vector evaluateError(const Pose3 &pose, const Point3 &point, boost::optional< │ │ │ │ │ +Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const │ │ │ │ │ +override │ │ │ │ │ +h(x)-z │ │ │ │ │ +DDeeffiinniittiioonn StereoFactor.h:122 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const NonlinearFactor &f, double tol=1e-9) const override │ │ │ │ │ equals │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionPoseFactor.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r │ │ │ │ │ -SmartProjectionPoseFactor(const SharedNoiseModel &sharedNoiseModel, const │ │ │ │ │ -boost::shared_ptr< CALIBRATION > K, const SmartProjectionParams │ │ │ │ │ -¶ms=SmartProjectionParams()) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionPoseFactor.h:72 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_:_:_c_a_m_e_r_a_s │ │ │ │ │ -Base::Cameras cameras(const Values &values) const override │ │ │ │ │ -Collect all cameras involved in this factor. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionPoseFactor.h:138 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_:_:_e_r_r_o_r │ │ │ │ │ -double error(const Values &values) const override │ │ │ │ │ -error calculates the error of the factor. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionPoseFactor.h:119 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shorthand for a smart pointer to a factor │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionPoseFactor.h:59 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_:_:_K__ │ │ │ │ │ -boost::shared_ptr< CALIBRATION > K_ │ │ │ │ │ -calibration object (one for all cameras) │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionPoseFactor.h:54 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_:_:_~_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r │ │ │ │ │ -~SmartProjectionPoseFactor() override │ │ │ │ │ +DDeeffiinniittiioonn StereoFactor.h:113 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_c_a_l_i_b_r_a_t_i_o_n │ │ │ │ │ +const Cal3_S2Stereo::shared_ptr calibration() const │ │ │ │ │ +return the calibration object │ │ │ │ │ +DDeeffiinniittiioonn StereoFactor.h:158 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r │ │ │ │ │ +GenericStereoFactor(const StereoPoint2 &measured, const SharedNoiseModel │ │ │ │ │ +&model, Key poseKey, Key landmarkKey, const Cal3_S2Stereo::shared_ptr &K, bool │ │ │ │ │ +throwCheirality, bool verboseCheirality, boost::optional< POSE > │ │ │ │ │ +body_P_sensor=boost::none) │ │ │ │ │ +Constructor with exception-handling flags. │ │ │ │ │ +DDeeffiinniittiioonn StereoFactor.h:83 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_T_h_i_s │ │ │ │ │ +GenericStereoFactor< POSE, LANDMARK > This │ │ │ │ │ +typedef for this class (with templates) │ │ │ │ │ +DDeeffiinniittiioonn StereoFactor.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_~_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r │ │ │ │ │ +~GenericStereoFactor() override │ │ │ │ │ Virtual destructor. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionPoseFactor.h:96 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +DDeeffiinniittiioonn StereoFactor.h:91 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y │ │ │ │ │ +bool verboseCheirality() const │ │ │ │ │ +return verbosity │ │ │ │ │ +DDeeffiinniittiioonn StereoFactor.h:163 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< GenericStereoFactor > shared_ptr │ │ │ │ │ +typedef for shared pointer to this object │ │ │ │ │ +DDeeffiinniittiioonn StereoFactor.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_m_e_a_s_u_r_e_d │ │ │ │ │ +const StereoPoint2 & measured() const │ │ │ │ │ +return the measured │ │ │ │ │ +DDeeffiinniittiioonn StereoFactor.h:153 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r │ │ │ │ │ +GenericStereoFactor() │ │ │ │ │ +Default constructor. │ │ │ │ │ +DDeeffiinniittiioonn StereoFactor.h:54 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn StereoFactor.h:170 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r │ │ │ │ │ +GenericStereoFactor(const StereoPoint2 &measured, const SharedNoiseModel │ │ │ │ │ +&model, Key poseKey, Key landmarkKey, const Cal3_S2Stereo::shared_ptr &K, │ │ │ │ │ +boost::optional< POSE > body_P_sensor=boost::none) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn StereoFactor.h:66 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ void print(const std::string &s="", const KeyFormatter │ │ │ │ │ &keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ print │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionPoseFactor.h:104 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_:_:_c_a_l_i_b_r_a_t_i_o_n │ │ │ │ │ -const boost::shared_ptr< CALIBRATION > calibration() const │ │ │ │ │ -return calibration shared pointers │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionPoseFactor.h:128 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r │ │ │ │ │ -SmartProjectionPoseFactor(const SharedNoiseModel &sharedNoiseModel, const │ │ │ │ │ -boost::shared_ptr< CALIBRATION > K, const boost::optional< Pose3 > │ │ │ │ │ -body_P_sensor, const SmartProjectionParams ¶ms=SmartProjectionParams()) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionPoseFactor.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionPoseFactor.h:152 │ │ │ │ │ -_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r │ │ │ │ │ -SmartProjectionPoseFactor() │ │ │ │ │ -Default constructor, only for serialization. │ │ │ │ │ -DDeeffiinniittiioonn SmartProjectionPoseFactor.h:64 │ │ │ │ │ +DDeeffiinniittiioonn StereoFactor.h:103 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_C_a_m_P_o_s_e │ │ │ │ │ +POSE CamPose │ │ │ │ │ +typedef for Pose Lie Value type │ │ │ │ │ +DDeeffiinniittiioonn StereoFactor.h:49 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_B_a_s_e │ │ │ │ │ +NoiseModelFactorN< POSE, LANDMARK > Base │ │ │ │ │ +typedef for base class │ │ │ │ │ +DDeeffiinniittiioonn StereoFactor.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_S_t_e_r_e_o_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ +gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn StereoFactor.h:94 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_._h │ │ │ │ │ + * _S_t_e_r_e_o_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01319.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/BoundingConstraint.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/dataset.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,47 +94,153 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
BoundingConstraint.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Typedefs | │ │ │ │ +Enumerations | │ │ │ │ +Functions
│ │ │ │ +
dataset.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Provides partially implemented constraints to implement bounds. │ │ │ │ +

utility functions for loading datasets │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

struct  gtsam::BoundingConstraint1< VALUE >
 Unary inequality constraint forcing a scalar to be greater/less than a fixed threshold. More...
 
struct  gtsam::BoundingConstraint2< VALUE1, VALUE2 >
 Binary scalar inequality constraint, with a similar value() function to implement for specific systems. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Typedefs

│ │ │ │ +typedef std::pair< size_t, Pose2 > gtsam::IndexedPose
 Return type for auxiliary functions.
 
│ │ │ │ +typedef std::pair< size_t, Point2 > gtsam::IndexedLandmark
 
│ │ │ │ +typedef std::pair< std::pair< size_t, size_t >, Pose2 > gtsam::IndexedEdge
 
using gtsam::GraphAndValues = std::pair< NonlinearFactorGraph::shared_ptr, Values::shared_ptr >
 Return type for load functions, which return a graph and initial values.
 
│ │ │ │ +using gtsam::BetweenFactorPose2s = std::vector< BetweenFactor< Pose2 >::shared_ptr >
 
│ │ │ │ +using gtsam::BetweenFactorPose3s = std::vector< BetweenFactor< Pose3 >::shared_ptr >
 
│ │ │ │ +using gtsam::BinaryMeasurementsUnit3 = std::vector< BinaryMeasurement< Unit3 > >
 
│ │ │ │ +using gtsam::BinaryMeasurementsPoint3 = std::vector< BinaryMeasurement< Point3 > >
 
│ │ │ │ +using gtsam::BinaryMeasurementsRot3 = std::vector< BinaryMeasurement< Rot3 > >
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Enumerations

enum  gtsam::NoiseFormat {
│ │ │ │ +  gtsam::NoiseFormatG2O │ │ │ │ +, gtsam::NoiseFormatTORO │ │ │ │ +, gtsam::NoiseFormatGRAPH │ │ │ │ +, gtsam::NoiseFormatCOV │ │ │ │ +,
│ │ │ │ +  gtsam::NoiseFormatAUTO │ │ │ │ +
│ │ │ │ + }
 Indicates how noise parameters are stored in file. More...
 
enum  gtsam::KernelFunctionType { KernelFunctionTypeNONE │ │ │ │ +, KernelFunctionTypeHUBER │ │ │ │ +, KernelFunctionTypeTUKEY │ │ │ │ + }
 Robust kernel type to wrap around quadratic noise model.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

string gtsam::findExampleDataFile (const std::string &name)
 Find the full path to an example dataset distributed with gtsam.
 
│ │ │ │ +string gtsam::createRewrittenFileName (const std::string &name)
 Creates a temporary file name that needs to be ignored in .gitingnore for checking read-write oprations.
 
template<typename T >
GTSAM_EXPORT std::map< size_t, T > gtsam::parseVariables (const std::string &filename, size_t maxIndex=0)
 Parse variables in a line-based text format (like g2o) into a map.
 
template<typename T >
GTSAM_EXPORT std::vector< BinaryMeasurement< T > > gtsam::parseMeasurements (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model=nullptr, size_t maxIndex=0)
 Parse binary measurements in a line-based text format (like g2o) into a vector.
 
template<typename T >
GTSAM_EXPORT std::vector< typename BetweenFactor< T >::shared_ptr > gtsam::parseFactors (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model=nullptr, size_t maxIndex=0)
 Parse BetweenFactors in a line-based text format (like g2o) into a vector of shared pointers.
 
boost::optional< IndexedPose > gtsam::parseVertexPose (std::istream &is, const std::string &tag)
 Parse TORO/G2O vertex "id x y yaw".
 
boost::optional< IndexedLandmark > gtsam::parseVertexLandmark (std::istream &is, const std::string &tag)
 Parse G2O landmark vertex "id x y".
 
boost::optional< IndexedEdge > gtsam::parseEdge (std::istream &is, const std::string &tag)
 Parse TORO/G2O edge "id1 id2 x y yaw".
 
GTSAM_EXPORT GraphAndValues gtsam::load2D (std::pair< std::string, SharedNoiseModel > dataset, size_t maxIndex=0, bool addNoise=false, bool smart=true, NoiseFormat noiseFormat=NoiseFormatAUTO, KernelFunctionType kernelFunctionType=KernelFunctionTypeNONE)
 Load TORO 2D Graph.
 
GraphAndValues gtsam::load2D (const std::string &filename, SharedNoiseModel model=SharedNoiseModel(), size_t maxIndex=0, bool addNoise=false, bool smart=true, NoiseFormat noiseFormat=NoiseFormatAUTO, KernelFunctionType kernelFunctionType=KernelFunctionTypeNONE)
 Load TORO/G2O style graph files.
 
│ │ │ │ +void gtsam::save2D (const NonlinearFactorGraph &graph, const Values &config, const noiseModel::Diagonal::shared_ptr model, const std::string &filename)
 save 2d graph
 
GraphAndValues gtsam::readG2o (const std::string &g2oFile, const bool is3D=false, KernelFunctionType kernelFunctionType=KernelFunctionTypeNONE)
 This function parses a g2o file and stores the measurements into a NonlinearFactorGraph and the initial guess in a Values structure.
 
void gtsam::writeG2o (const NonlinearFactorGraph &graph, const Values &estimate, const std::string &filename)
 This function writes a g2o file from NonlinearFactorGraph and a Values structure.
 
│ │ │ │ +GraphAndValues gtsam::load3D (const std::string &filename)
 Load TORO 3D Graph.
 
│ │ │ │ +BetweenFactorPose2s gtsam::parse2DFactors (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
 
│ │ │ │ +BetweenFactorPose3s gtsam::parse3DFactors (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Provides partially implemented constraints to implement bounds.

│ │ │ │ -
Author
Alex Cunningham
│ │ │ │ +

utility functions for loading datasets

│ │ │ │ +
Date
Jan 22, 2010
│ │ │ │ +
Author
Ni Kai
│ │ │ │ +
│ │ │ │ +Luca Carlone
│ │ │ │ +
│ │ │ │ +Varun Agrawal
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,30 +1,164 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -BoundingConstraint.h File Reference │ │ │ │ │ -Provides partially implemented constraints to implement bounds. _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _E_n_u_m_e_r_a_t_i_o_n_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +dataset.h File Reference │ │ │ │ │ +utility functions for loading datasets _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_1_<_ _V_A_L_U_E_ _> │ │ │ │ │ -  Unary inequality constraint forcing a scalar to be greater/less than a │ │ │ │ │ - fixed threshold. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_2_<_ _V_A_L_U_E_1_,_ _V_A_L_U_E_2_ _> │ │ │ │ │ -  Binary scalar inequality constraint, with a similar _v_a_l_u_e_(_) function │ │ │ │ │ - to implement for specific systems. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ + typedef std::pair< size_t, _P_o_s_e_2 >  ggttssaamm::::IInnddeexxeeddPPoossee │ │ │ │ │ +  Return type for auxiliary functions. │ │ │ │ │ +  │ │ │ │ │ + typedef std::pair< size_t, _P_o_i_n_t_2 >  ggttssaamm::::IInnddeexxeeddLLaannddmmaarrkk │ │ │ │ │ +  │ │ │ │ │ +typedef std::pair< std::pair< size_t, │ │ │ │ │ + size_t >, _P_o_s_e_2 >  ggttssaamm::::IInnddeexxeeddEEddggee │ │ │ │ │ +  │ │ │ │ │ + using  _g_t_s_a_m_:_:_G_r_a_p_h_A_n_d_V_a_l_u_e_s = std::pair< │ │ │ │ │ + NonlinearFactorGraph::shared_ptr, │ │ │ │ │ + _V_a_l_u_e_s_:_:_s_h_a_r_e_d___p_t_r > │ │ │ │ │ +  Return type for load functions, which │ │ │ │ │ + return a graph and initial values. │ │ │ │ │ +  │ │ │ │ │ + using  ggttssaamm::::BBeettwweeeennFFaaccttoorrPPoossee22ss = std::vector< │ │ │ │ │ + _B_e_t_w_e_e_n_F_a_c_t_o_r< _P_o_s_e_2 >::shared_ptr > │ │ │ │ │ +  │ │ │ │ │ + using  ggttssaamm::::BBeettwweeeennFFaaccttoorrPPoossee33ss = std::vector< │ │ │ │ │ + _B_e_t_w_e_e_n_F_a_c_t_o_r< _P_o_s_e_3 >::shared_ptr > │ │ │ │ │ +  │ │ │ │ │ + using  ggttssaamm::::BBiinnaarryyMMeeaassuurreemmeennttssUUnniitt33 = std:: │ │ │ │ │ + vector< _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t< _U_n_i_t_3 > > │ │ │ │ │ +  │ │ │ │ │ + using  ggttssaamm::::BBiinnaarryyMMeeaassuurreemmeennttssPPooiinntt33 = std:: │ │ │ │ │ + vector< _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t< _P_o_i_n_t_3 > > │ │ │ │ │ +  │ │ │ │ │ + using  ggttssaamm::::BBiinnaarryyMMeeaassuurreemmeennttssRRoott33 = std:: │ │ │ │ │ + vector< _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t< _R_o_t_3 > > │ │ │ │ │ +  │ │ │ │ │ +EEnnuummeerraattiioonnss │ │ │ │ │ +enum   _g_t_s_a_m_:_:_N_o_i_s_e_F_o_r_m_a_t { │ │ │ │ │ +   _g_t_s_a_m_:_:_N_o_i_s_e_F_o_r_m_a_t_G_2_O , _g_t_s_a_m_:_:_N_o_i_s_e_F_o_r_m_a_t_T_O_R_O , _g_t_s_a_m_:_: │ │ │ │ │ + _N_o_i_s_e_F_o_r_m_a_t_G_R_A_P_H , _g_t_s_a_m_:_:_N_o_i_s_e_F_o_r_m_a_t_C_O_V , │ │ │ │ │ +   _g_t_s_a_m_:_:_N_o_i_s_e_F_o_r_m_a_t_A_U_T_O │ │ │ │ │ + } │ │ │ │ │ +  Indicates how noise parameters are stored in file. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +enum   _g_t_s_a_m_:_:_K_e_r_n_e_l_F_u_n_c_t_i_o_n_T_y_p_e { KKeerrnneellFFuunnccttiioonnTTyyppeeNNOONNEE , │ │ │ │ │ + KKeerrnneellFFuunnccttiioonnTTyyppeeHHUUBBEERR , KKeerrnneellFFuunnccttiioonnTTyyppeeTTUUKKEEYY } │ │ │ │ │ +  Robust kernel type to wrap around quadratic noise model. │ │ │ │ │ +  │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ + string  _g_t_s_a_m_:_:_f_i_n_d_E_x_a_m_p_l_e_D_a_t_a_F_i_l_e (const std:: │ │ │ │ │ + string &name) │ │ │ │ │ +  Find the full path to an example dataset │ │ │ │ │ + distributed with gtsam. │ │ │ │ │ +  │ │ │ │ │ + string  ggttssaamm::::ccrreeaatteeRReewwrriitttteennFFiilleeNNaammee (const std:: │ │ │ │ │ + string &name) │ │ │ │ │ + Creates a temporary file name that needs to │ │ │ │ │ +  be ignored in .gitingnore for checking │ │ │ │ │ + read-write oprations. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +GTSAM_EXPORT std::map< size_t, T >  _g_t_s_a_m_:_:_p_a_r_s_e_V_a_r_i_a_b_l_e_s (const std::string │ │ │ │ │ + &filename, size_t maxIndex=0) │ │ │ │ │ +  Parse variables in a line-based text format │ │ │ │ │ + (like g2o) into a map. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + GTSAM_EXPORT std::vector< _g_t_s_a_m_:_:_p_a_r_s_e_M_e_a_s_u_r_e_m_e_n_t_s (const std::string │ │ │ │ │ + _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t< T > >  &filename, const noiseModel::Diagonal:: │ │ │ │ │ + shared_ptr &model=nullptr, size_t │ │ │ │ │ + maxIndex=0) │ │ │ │ │ +  Parse binary measurements in a line-based │ │ │ │ │ + text format (like g2o) into a vector. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + GTSAM_EXPORT std::vector< typename _g_t_s_a_m_:_:_p_a_r_s_e_F_a_c_t_o_r_s (const std::string │ │ │ │ │ + _B_e_t_w_e_e_n_F_a_c_t_o_r< T >::shared_ptr >  &filename, const noiseModel::Diagonal:: │ │ │ │ │ + shared_ptr &model=nullptr, size_t │ │ │ │ │ + maxIndex=0) │ │ │ │ │ + Parse BetweenFactors in a line-based text │ │ │ │ │ +  format (like g2o) into a vector of shared │ │ │ │ │ + pointers. │ │ │ │ │ +  │ │ │ │ │ + boost::optional< _I_n_d_e_x_e_d_P_o_s_e >  _g_t_s_a_m_:_:_p_a_r_s_e_V_e_r_t_e_x_P_o_s_e (std::istream &is, │ │ │ │ │ + const std::string &tag) │ │ │ │ │ +  Parse TORO/G2O vertex "id x y yaw". │ │ │ │ │ +  │ │ │ │ │ +boost::optional< IndexedLandmark >  _g_t_s_a_m_:_:_p_a_r_s_e_V_e_r_t_e_x_L_a_n_d_m_a_r_k (std::istream │ │ │ │ │ + &is, const std::string &tag) │ │ │ │ │ +  Parse G2O landmark vertex "id x y". │ │ │ │ │ +  │ │ │ │ │ + boost::optional< IndexedEdge >  _g_t_s_a_m_:_:_p_a_r_s_e_E_d_g_e (std::istream &is, const │ │ │ │ │ + std::string &tag) │ │ │ │ │ +  Parse TORO/G2O edge "id1 id2 x y yaw". │ │ │ │ │ +  │ │ │ │ │ + GTSAM_EXPORT _G_r_a_p_h_A_n_d_V_a_l_u_e_s  _g_t_s_a_m_:_:_l_o_a_d_2_D (std::pair< std::string, │ │ │ │ │ + _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l > dataset, size_t │ │ │ │ │ + maxIndex=0, bool addNoise=false, bool │ │ │ │ │ + smart=true, _N_o_i_s_e_F_o_r_m_a_t │ │ │ │ │ + noiseFormat=_N_o_i_s_e_F_o_r_m_a_t_A_U_T_O, │ │ │ │ │ + _K_e_r_n_e_l_F_u_n_c_t_i_o_n_T_y_p_e │ │ │ │ │ + kernelFunctionType=KernelFunctionTypeNONE) │ │ │ │ │ +  Load TORO 2D Graph. │ │ │ │ │ +  │ │ │ │ │ + _G_r_a_p_h_A_n_d_V_a_l_u_e_s  _g_t_s_a_m_:_:_l_o_a_d_2_D (const std::string &filename, │ │ │ │ │ + _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l model=_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l(), │ │ │ │ │ + size_t maxIndex=0, bool addNoise=false, │ │ │ │ │ + bool smart=true, _N_o_i_s_e_F_o_r_m_a_t │ │ │ │ │ + noiseFormat=_N_o_i_s_e_F_o_r_m_a_t_A_U_T_O, │ │ │ │ │ + _K_e_r_n_e_l_F_u_n_c_t_i_o_n_T_y_p_e │ │ │ │ │ + kernelFunctionType=KernelFunctionTypeNONE) │ │ │ │ │ +  Load TORO/G2O style graph files. │ │ │ │ │ +  │ │ │ │ │ + void  ggttssaamm::::ssaavvee22DD (const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ + &graph, const _V_a_l_u_e_s &config, const │ │ │ │ │ + noiseModel::Diagonal::shared_ptr model, │ │ │ │ │ + const std::string &filename) │ │ │ │ │ +  save 2d graph │ │ │ │ │ +  │ │ │ │ │ + _G_r_a_p_h_A_n_d_V_a_l_u_e_s  _g_t_s_a_m_:_:_r_e_a_d_G_2_o (const std::string &g2oFile, │ │ │ │ │ + const bool is3D=false, _K_e_r_n_e_l_F_u_n_c_t_i_o_n_T_y_p_e │ │ │ │ │ + kernelFunctionType=KernelFunctionTypeNONE) │ │ │ │ │ + This function parses a g2o file and stores │ │ │ │ │ +  the measurements into a │ │ │ │ │ + _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h and the initial guess │ │ │ │ │ + in a _V_a_l_u_e_s structure. │ │ │ │ │ +  │ │ │ │ │ + void  _g_t_s_a_m_:_:_w_r_i_t_e_G_2_o (const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ + &graph, const _V_a_l_u_e_s &estimate, const std:: │ │ │ │ │ + string &filename) │ │ │ │ │ + This function writes a g2o file from │ │ │ │ │ +  _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h and a _V_a_l_u_e_s │ │ │ │ │ + structure. │ │ │ │ │ +  │ │ │ │ │ + _G_r_a_p_h_A_n_d_V_a_l_u_e_s  ggttssaamm::::llooaadd33DD (const std::string &filename) │ │ │ │ │ +  Load TORO 3D Graph. │ │ │ │ │ +  │ │ │ │ │ + BetweenFactorPose2s  ggttssaamm::::ppaarrssee22DDFFaaccttoorrss (const std::string │ │ │ │ │ + &filename, const noiseModel::Diagonal:: │ │ │ │ │ + shared_ptr &model, size_t maxIndex) │ │ │ │ │ +  │ │ │ │ │ + BetweenFactorPose3s  ggttssaamm::::ppaarrssee33DDFFaaccttoorrss (const std::string │ │ │ │ │ + &filename, const noiseModel::Diagonal:: │ │ │ │ │ + shared_ptr &model, size_t maxIndex) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Provides partially implemented constraints to implement bounds. │ │ │ │ │ +utility functions for loading datasets │ │ │ │ │ + Date │ │ │ │ │ + Jan 22, 2010 │ │ │ │ │ Author │ │ │ │ │ - Alex Cunningham │ │ │ │ │ + Ni Kai │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Varun Agrawal │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_._h │ │ │ │ │ + * _d_a_t_a_s_e_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01319.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,30 @@ │ │ │ │ │ var a01319 = [ │ │ │ │ │ - ["gtsam::BoundingConstraint2< VALUE1, VALUE2 >", "a04784.html", "a04784"] │ │ │ │ │ + ["GraphAndValues", "a01319.html#abcc5d1ec05ddc954d5de28929acabbdf", null], │ │ │ │ │ + ["IndexedPose", "a01319.html#a6ab764d13f9aaeafc2dc60763b05ec0e", null], │ │ │ │ │ + ["KernelFunctionType", "a01319.html#a8be373f34edc0a9d28b1bfab5dd62ba0", [ │ │ │ │ │ + ["KernelFunctionTypeNONE", "a01319.html#a8be373f34edc0a9d28b1bfab5dd62ba0a406af886b3b4b113dfe666c8ad95e38b", null], │ │ │ │ │ + ["KernelFunctionTypeHUBER", "a01319.html#a8be373f34edc0a9d28b1bfab5dd62ba0a37cef4c071c8c4f77f31fa617e91da53", null], │ │ │ │ │ + ["KernelFunctionTypeTUKEY", "a01319.html#a8be373f34edc0a9d28b1bfab5dd62ba0aacc5d659fca66977c8fe6f341802fae9", null] │ │ │ │ │ + ]], │ │ │ │ │ + ["NoiseFormat", "a01319.html#a90552b70a2da9c74595cae4b05c2ce18", [ │ │ │ │ │ + ["NoiseFormatG2O", "a01319.html#a90552b70a2da9c74595cae4b05c2ce18ad0aaa5c51a45cb7c9151aae60a603209", null], │ │ │ │ │ + ["NoiseFormatTORO", "a01319.html#a90552b70a2da9c74595cae4b05c2ce18a3790bc2c6f11cd7da84730b33173de5a", null], │ │ │ │ │ + ["NoiseFormatGRAPH", "a01319.html#a90552b70a2da9c74595cae4b05c2ce18a1f8edeb25c58a249789556c686265a4c", null], │ │ │ │ │ + ["NoiseFormatCOV", "a01319.html#a90552b70a2da9c74595cae4b05c2ce18a5450a16f3522214ccdfa23c461e7d05a", null], │ │ │ │ │ + ["NoiseFormatAUTO", "a01319.html#a90552b70a2da9c74595cae4b05c2ce18a2f9067b8f5bd2d0dd38c49326a4c070e", null] │ │ │ │ │ + ]], │ │ │ │ │ + ["createRewrittenFileName", "a01319.html#adbe09e573771fd72b08069353d537725", null], │ │ │ │ │ + ["findExampleDataFile", "a01319.html#aef971b2c69845e4fcce707306703c7b8", null], │ │ │ │ │ + ["load2D", "a01319.html#a635ec096b94adc2b05f081cd07eb2c0f", null], │ │ │ │ │ + ["load2D", "a01319.html#a6f8d49b13ccc0e007a5e7572286d5854", null], │ │ │ │ │ + ["load3D", "a01319.html#ac96511703a91071d7b2033976f9739c3", null], │ │ │ │ │ + ["parseEdge", "a01319.html#abc8bb9db0893652e8b5e7b47782ec990", null], │ │ │ │ │ + ["parseFactors", "a01319.html#a8a2136a163e31faaefe2db498a9b9b43", null], │ │ │ │ │ + ["parseMeasurements", "a01319.html#a6eb4817b97658f00e426b753c52361ed", null], │ │ │ │ │ + ["parseVariables", "a01319.html#a683ab6a83b5b3e508e734a7089986838", null], │ │ │ │ │ + ["parseVertexLandmark", "a01319.html#a961e0398bbfc2504c62fdaad70472418", null], │ │ │ │ │ + ["parseVertexPose", "a01319.html#abfc0157e4790e3b03dc601d3e86c684d", null], │ │ │ │ │ + ["readG2o", "a01319.html#a71d086efc70f4af31d3c3b00dde4f7c0", null], │ │ │ │ │ + ["save2D", "a01319.html#acfe516bc2d9ca4cc4530dd3b8460ce3d", null], │ │ │ │ │ + ["writeG2o", "a01319.html#a68bc584d72da3747666ac134a9490f92", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01319_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/BoundingConstraint.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/dataset.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,193 +98,208 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
BoundingConstraint.h
│ │ │ │ +
dataset.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/base/Lie.h>
│ │ │ │ -
21#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
21#pragma once
│ │ │ │
22
│ │ │ │ -
23namespace gtsam {
│ │ │ │ -
24
│ │ │ │ -
32template<class VALUE>
│ │ │ │ -
│ │ │ │ -
33struct BoundingConstraint1: public NoiseModelFactorN<VALUE> {
│ │ │ │ -
34 typedef VALUE X;
│ │ │ │ -
35 typedef NoiseModelFactorN<VALUE> Base;
│ │ │ │ -
36 typedef boost::shared_ptr<BoundingConstraint1<VALUE> > shared_ptr;
│ │ │ │ -
37
│ │ │ │ -
38 double threshold_;
│ │ │ │ -
39 bool isGreaterThan_;
│ │ │ │ -
40
│ │ │ │ -
│ │ │ │ -
41 BoundingConstraint1(Key key, double threshold,
│ │ │ │ -
42 bool isGreaterThan, double mu = 1000.0) :
│ │ │ │ -
43 Base(noiseModel::Constrained::All(1, mu), key),
│ │ │ │ -
44 threshold_(threshold), isGreaterThan_(isGreaterThan) {
│ │ │ │ -
45 }
│ │ │ │ -
│ │ │ │ -
46
│ │ │ │ -
47 ~BoundingConstraint1() override {}
│ │ │ │ -
48
│ │ │ │ -
49 inline double threshold() const { return threshold_; }
│ │ │ │ -
50 inline bool isGreaterThan() const { return isGreaterThan_; }
│ │ │ │ -
51
│ │ │ │ -
57 virtual double value(const X& x, boost::optional<Matrix&> H =
│ │ │ │ -
58 boost::none) const = 0;
│ │ │ │ -
59
│ │ │ │ -
│ │ │ │ -
61 bool active(const Values& c) const override {
│ │ │ │ -
62 // note: still active at equality to avoid zigzagging
│ │ │ │ -
63 double x = value(c.at<X>(this->key()));
│ │ │ │ -
64 return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
│ │ │ │ -
65 }
│ │ │ │ -
│ │ │ │ -
66
│ │ │ │ -
67 Vector evaluateError(const X& x, boost::optional<Matrix&> H =
│ │ │ │ -
68 boost::none) const override {
│ │ │ │ -
69 Matrix D;
│ │ │ │ -
70 double error = value(x, D) - threshold_;
│ │ │ │ -
71 if (H) {
│ │ │ │ -
72 if (isGreaterThan_) *H = D;
│ │ │ │ -
73 else *H = -1.0 * D;
│ │ │ │ -
74 }
│ │ │ │ -
75
│ │ │ │ -
76 if (isGreaterThan_)
│ │ │ │ -
77 return (Vector(1) << error).finished();
│ │ │ │ -
78 else
│ │ │ │ -
79 return -1.0 * (Vector(1) << error).finished();
│ │ │ │ -
80 }
│ │ │ │ -
81
│ │ │ │ -
82private:
│ │ │ │ -
83
│ │ │ │ -
85 friend class boost::serialization::access;
│ │ │ │ -
86 template<class ARCHIVE>
│ │ │ │ -
87 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
88 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ -
89 ar & boost::serialization::make_nvp("NoiseModelFactor1",
│ │ │ │ -
90 boost::serialization::base_object<Base>(*this));
│ │ │ │ -
91 ar & BOOST_SERIALIZATION_NVP(threshold_);
│ │ │ │ -
92 ar & BOOST_SERIALIZATION_NVP(isGreaterThan_);
│ │ │ │ -
93 }
│ │ │ │ -
94};
│ │ │ │ -
│ │ │ │ -
95
│ │ │ │ -
100template<class VALUE1, class VALUE2>
│ │ │ │ -
│ │ │ │ -
101struct BoundingConstraint2: public NoiseModelFactorN<VALUE1, VALUE2> {
│ │ │ │ -
102 typedef VALUE1 X1;
│ │ │ │ -
103 typedef VALUE2 X2;
│ │ │ │ -
104
│ │ │ │ -
105 typedef NoiseModelFactorN<VALUE1, VALUE2> Base;
│ │ │ │ -
106 typedef boost::shared_ptr<BoundingConstraint2<VALUE1, VALUE2> > shared_ptr;
│ │ │ │ -
107
│ │ │ │ -
108 double threshold_;
│ │ │ │ -
109 bool isGreaterThan_;
│ │ │ │ +
23#include <gtsam/sfm/BinaryMeasurement.h>
│ │ │ │ +
24#include <gtsam/slam/BetweenFactor.h>
│ │ │ │ +
25#include <gtsam/sfm/SfmData.h>
│ │ │ │ +
26#include <gtsam/geometry/Cal3Bundler.h>
│ │ │ │ +
27#include <gtsam/geometry/PinholeCamera.h>
│ │ │ │ +
28#include <gtsam/geometry/Pose2.h>
│ │ │ │ +
29#include <gtsam/geometry/Pose3.h>
│ │ │ │ +
30#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ +
31#include <gtsam/nonlinear/Values.h>
│ │ │ │ +
32#include <gtsam/linear/NoiseModel.h>
│ │ │ │ +
33#include <gtsam/base/serialization.h>
│ │ │ │ +
34#include <gtsam/base/Testable.h>
│ │ │ │ +
35#include <gtsam/base/types.h>
│ │ │ │ +
36
│ │ │ │ +
37#include <boost/smart_ptr/shared_ptr.hpp>
│ │ │ │ +
38#include <string>
│ │ │ │ +
39#include <utility> // for pair
│ │ │ │ +
40#include <vector>
│ │ │ │ +
41#include <iosfwd>
│ │ │ │ +
42#include <map>
│ │ │ │ +
43
│ │ │ │ +
44namespace gtsam {
│ │ │ │ +
45
│ │ │ │ +
57GTSAM_EXPORT std::string findExampleDataFile(const std::string& name);
│ │ │ │ +
58
│ │ │ │ +
63GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name);
│ │ │ │ +
64
│ │ │ │ +
│ │ │ │ +
66enum NoiseFormat {
│ │ │ │ +
67 NoiseFormatG2O,
│ │ │ │ +
68 NoiseFormatTORO,
│ │ │ │ +
69 NoiseFormatGRAPH,
│ │ │ │ +
70 NoiseFormatCOV,
│ │ │ │ +
71 NoiseFormatAUTO
│ │ │ │ +
72};
│ │ │ │ +
│ │ │ │ +
73
│ │ │ │ +
│ │ │ │ +
75enum KernelFunctionType {
│ │ │ │ +
76 KernelFunctionTypeNONE, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY
│ │ │ │ +
77};
│ │ │ │ +
│ │ │ │ +
78
│ │ │ │ +
85template <typename T>
│ │ │ │ +
86GTSAM_EXPORT std::map<size_t, T> parseVariables(const std::string &filename,
│ │ │ │ +
87 size_t maxIndex = 0);
│ │ │ │ +
88
│ │ │ │ +
95template <typename T>
│ │ │ │ +
96GTSAM_EXPORT std::vector<BinaryMeasurement<T>>
│ │ │ │ +
97parseMeasurements(const std::string &filename,
│ │ │ │ +
98 const noiseModel::Diagonal::shared_ptr &model = nullptr,
│ │ │ │ +
99 size_t maxIndex = 0);
│ │ │ │ +
100
│ │ │ │ +
105template <typename T>
│ │ │ │ +
106GTSAM_EXPORT std::vector<typename BetweenFactor<T>::shared_ptr>
│ │ │ │ +
107parseFactors(const std::string &filename,
│ │ │ │ +
108 const noiseModel::Diagonal::shared_ptr &model = nullptr,
│ │ │ │ +
109 size_t maxIndex = 0);
│ │ │ │
110
│ │ │ │ -
│ │ │ │ -
111 BoundingConstraint2(Key key1, Key key2, double threshold,
│ │ │ │ -
112 bool isGreaterThan, double mu = 1000.0)
│ │ │ │ -
113 : Base(noiseModel::Constrained::All(1, mu), key1, key2),
│ │ │ │ -
114 threshold_(threshold), isGreaterThan_(isGreaterThan) {}
│ │ │ │ -
│ │ │ │ +
112typedef std::pair<size_t, Pose2> IndexedPose;
│ │ │ │ +
113typedef std::pair<size_t, Point2> IndexedLandmark;
│ │ │ │ +
114typedef std::pair<std::pair<size_t, size_t>, Pose2> IndexedEdge;
│ │ │ │
115
│ │ │ │ -
116 ~BoundingConstraint2() override {}
│ │ │ │ -
117
│ │ │ │ -
118 inline double threshold() const { return threshold_; }
│ │ │ │ -
119 inline bool isGreaterThan() const { return isGreaterThan_; }
│ │ │ │ -
120
│ │ │ │ -
125 virtual double value(const X1& x1, const X2& x2,
│ │ │ │ -
126 boost::optional<Matrix&> H1 = boost::none,
│ │ │ │ -
127 boost::optional<Matrix&> H2 = boost::none) const = 0;
│ │ │ │ -
128
│ │ │ │ -
│ │ │ │ -
130 bool active(const Values& c) const override {
│ │ │ │ -
131 // note: still active at equality to avoid zigzagging
│ │ │ │ -
132 double x = value(c.at<X1>(this->key1()), c.at<X2>(this->key2()));
│ │ │ │ -
133 return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
│ │ │ │ -
134 }
│ │ │ │ -
│ │ │ │ -
135
│ │ │ │ -
136 Vector evaluateError(const X1& x1, const X2& x2,
│ │ │ │ -
137 boost::optional<Matrix&> H1 = boost::none,
│ │ │ │ -
138 boost::optional<Matrix&> H2 = boost::none) const override {
│ │ │ │ -
139 Matrix D1, D2;
│ │ │ │ -
140 double error = value(x1, x2, D1, D2) - threshold_;
│ │ │ │ -
141 if (H1) {
│ │ │ │ -
142 if (isGreaterThan_) *H1 = D1;
│ │ │ │ -
143 else *H1 = -1.0 * D1;
│ │ │ │ -
144 }
│ │ │ │ -
145 if (H2) {
│ │ │ │ -
146 if (isGreaterThan_) *H2 = D2;
│ │ │ │ -
147 else *H2 = -1.0 * D2;
│ │ │ │ -
148 }
│ │ │ │ -
149
│ │ │ │ -
150 if (isGreaterThan_)
│ │ │ │ -
151 return (Vector(1) << error).finished();
│ │ │ │ -
152 else
│ │ │ │ -
153 return -1.0 * (Vector(1) << error).finished();
│ │ │ │ -
154 }
│ │ │ │ -
155
│ │ │ │ -
156private:
│ │ │ │ -
157
│ │ │ │ -
159 friend class boost::serialization::access;
│ │ │ │ -
160 template<class ARCHIVE>
│ │ │ │ -
161 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
162 // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ -
163 ar & boost::serialization::make_nvp("NoiseModelFactor2",
│ │ │ │ -
164 boost::serialization::base_object<Base>(*this));
│ │ │ │ -
165 ar & BOOST_SERIALIZATION_NVP(threshold_);
│ │ │ │ -
166 ar & BOOST_SERIALIZATION_NVP(isGreaterThan_);
│ │ │ │ -
167 }
│ │ │ │ -
168};
│ │ │ │ -
│ │ │ │ -
169
│ │ │ │ -
170} // \namespace gtsam
│ │ │ │ -
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
121GTSAM_EXPORT boost::optional<IndexedPose> parseVertexPose(std::istream& is,
│ │ │ │ +
122 const std::string& tag);
│ │ │ │ +
123
│ │ │ │ +
129GTSAM_EXPORT boost::optional<IndexedLandmark> parseVertexLandmark(std::istream& is,
│ │ │ │ +
130 const std::string& tag);
│ │ │ │ +
131
│ │ │ │ +
137GTSAM_EXPORT boost::optional<IndexedEdge> parseEdge(std::istream& is,
│ │ │ │ +
138 const std::string& tag);
│ │ │ │ +
139
│ │ │ │ +
143using GraphAndValues =
│ │ │ │ +
144 std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr>;
│ │ │ │ +
145
│ │ │ │ +
153GTSAM_EXPORT GraphAndValues load2D(
│ │ │ │ +
154 std::pair<std::string, SharedNoiseModel> dataset, size_t maxIndex = 0,
│ │ │ │ +
155 bool addNoise = false,
│ │ │ │ +
156 bool smart = true, //
│ │ │ │ +
157 NoiseFormat noiseFormat = NoiseFormatAUTO,
│ │ │ │ +
158 KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
│ │ │ │ +
159
│ │ │ │ +
171GTSAM_EXPORT GraphAndValues
│ │ │ │ +
172load2D(const std::string& filename, SharedNoiseModel model = SharedNoiseModel(),
│ │ │ │ +
173 size_t maxIndex = 0, bool addNoise = false, bool smart = true,
│ │ │ │ +
174 NoiseFormat noiseFormat = NoiseFormatAUTO, //
│ │ │ │ +
175 KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
│ │ │ │ +
176
│ │ │ │ +
178GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
│ │ │ │ +
179 const Values& config, const noiseModel::Diagonal::shared_ptr model,
│ │ │ │ +
180 const std::string& filename);
│ │ │ │ +
181
│ │ │ │ +
190GTSAM_EXPORT GraphAndValues
│ │ │ │ +
191readG2o(const std::string& g2oFile, const bool is3D = false,
│ │ │ │ +
192 KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
│ │ │ │ +
193
│ │ │ │ +
206GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
│ │ │ │ +
207 const Values& estimate, const std::string& filename);
│ │ │ │ +
208
│ │ │ │ +
210GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);
│ │ │ │ +
211
│ │ │ │ +
212// Wrapper-friendly versions of parseFactors<Pose2> and parseFactors<Pose2>
│ │ │ │ +
213using BetweenFactorPose2s = std::vector<BetweenFactor<Pose2>::shared_ptr>;
│ │ │ │ +
214GTSAM_EXPORT BetweenFactorPose2s
│ │ │ │ +
215parse2DFactors(const std::string &filename,
│ │ │ │ +
216 const noiseModel::Diagonal::shared_ptr &model = nullptr,
│ │ │ │ +
217 size_t maxIndex = 0);
│ │ │ │ +
218
│ │ │ │ +
219using BetweenFactorPose3s = std::vector<BetweenFactor<Pose3>::shared_ptr>;
│ │ │ │ +
220GTSAM_EXPORT BetweenFactorPose3s
│ │ │ │ +
221parse3DFactors(const std::string &filename,
│ │ │ │ +
222 const noiseModel::Diagonal::shared_ptr &model = nullptr,
│ │ │ │ +
223 size_t maxIndex = 0);
│ │ │ │ +
224
│ │ │ │ +
225using BinaryMeasurementsUnit3 = std::vector<BinaryMeasurement<Unit3>>;
│ │ │ │ +
226using BinaryMeasurementsPoint3 = std::vector<BinaryMeasurement<Point3>>;
│ │ │ │ +
227using BinaryMeasurementsRot3 = std::vector<BinaryMeasurement<Rot3>>;
│ │ │ │ +
228
│ │ │ │ +
229#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ +
230inline boost::optional<IndexedPose> GTSAM_DEPRECATED
│ │ │ │ +
231parseVertex(std::istream& is, const std::string& tag) {
│ │ │ │ +
232 return parseVertexPose(is, tag);
│ │ │ │ +
233}
│ │ │ │ +
234
│ │ │ │ +
235GTSAM_EXPORT std::map<size_t, Pose3> GTSAM_DEPRECATED
│ │ │ │ +
236parse3DPoses(const std::string& filename, size_t maxIndex = 0);
│ │ │ │ +
237
│ │ │ │ +
238GTSAM_EXPORT std::map<size_t, Point3> GTSAM_DEPRECATED
│ │ │ │ +
239parse3DLandmarks(const std::string& filename, size_t maxIndex = 0);
│ │ │ │ +
240
│ │ │ │ +
241GTSAM_EXPORT GraphAndValues GTSAM_DEPRECATED
│ │ │ │ +
242load2D_robust(const std::string& filename,
│ │ │ │ +
243 const noiseModel::Base::shared_ptr& model, size_t maxIndex = 0);
│ │ │ │ +
244#endif
│ │ │ │ +
245} // namespace gtsam
│ │ │ │ +
serialization.h
Convenience functions for serializing data structures via boost.serialization.
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
types.h
Typedefs for easier changing of types.
│ │ │ │ +
Pose3.h
3D Pose
│ │ │ │ +
Pose2.h
2D Pose
│ │ │ │ +
Cal3Bundler.h
Calibration used by Bundler.
│ │ │ │ +
PinholeCamera.h
Base class for all pinhole cameras.
│ │ │ │ + │ │ │ │ +
Values.h
A non-templated config holding any types of Manifold-group elements.
│ │ │ │ +
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │ +
BinaryMeasurement.h
Binary measurement represents a measurement between two keys in a graph. A binary measurement is simi...
│ │ │ │ +
SfmData.h
Data structure for dealing with Structure from Motion data.
│ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Calculate the error of the factor.
Definition NonlinearFactor.cpp:138
│ │ │ │ -
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition NonlinearFactor.h:223
│ │ │ │ -
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ -
gtsam::NoiseModelFactorN< VALUE >::key
Key key() const
Returns a key.
Definition NonlinearFactor.h:518
│ │ │ │ +
gtsam::load2D
GraphAndValues load2D(const string &filename, SharedNoiseModel model, size_t maxIndex, bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType)
Load TORO/G2O style graph files.
Definition dataset.cpp:502
│ │ │ │ +
gtsam::parseVariables
GTSAM_EXPORT std::map< size_t, T > parseVariables(const std::string &filename, size_t maxIndex=0)
Parse variables in a line-based text format (like g2o) into a map.
│ │ │ │ +
gtsam::writeG2o
void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, const string &filename)
This function writes a g2o file from NonlinearFactorGraph and a Values structure.
Definition dataset.cpp:632
│ │ │ │ +
gtsam::IndexedPose
std::pair< size_t, Pose2 > IndexedPose
Return type for auxiliary functions.
Definition dataset.h:112
│ │ │ │ +
gtsam::readG2o
GraphAndValues readG2o(const string &g2oFile, const bool is3D, KernelFunctionType kernelFunctionType)
This function parses a g2o file and stores the measurements into a NonlinearFactorGraph and the initi...
Definition dataset.cpp:617
│ │ │ │ +
gtsam::parseFactors
GTSAM_EXPORT std::vector< typename BetweenFactor< T >::shared_ptr > parseFactors(const std::string &filename, const noiseModel::Diagonal::shared_ptr &model=nullptr, size_t maxIndex=0)
Parse BetweenFactors in a line-based text format (like g2o) into a vector of shared pointers.
│ │ │ │ +
gtsam::KernelFunctionType
KernelFunctionType
Robust kernel type to wrap around quadratic noise model.
Definition dataset.h:75
│ │ │ │ +
gtsam::NoiseFormat
NoiseFormat
Indicates how noise parameters are stored in file.
Definition dataset.h:66
│ │ │ │ +
gtsam::NoiseFormatGRAPH
@ NoiseFormatGRAPH
default: toro-style order, but covariance matrix !
Definition dataset.h:69
│ │ │ │ +
gtsam::NoiseFormatAUTO
@ NoiseFormatAUTO
Try to guess covariance matrix layout.
Definition dataset.h:71
│ │ │ │ +
gtsam::NoiseFormatTORO
@ NoiseFormatTORO
Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr.
Definition dataset.h:68
│ │ │ │ +
gtsam::NoiseFormatCOV
@ NoiseFormatCOV
Covariance matrix C11, C12, C13, C22, C23, C33.
Definition dataset.h:70
│ │ │ │ +
gtsam::NoiseFormatG2O
@ NoiseFormatG2O
Information matrix I11, I12, I13, I22, I23, I33.
Definition dataset.h:67
│ │ │ │ +
gtsam::parseVertexLandmark
boost::optional< IndexedLandmark > parseVertexLandmark(istream &is, const string &tag)
Parse G2O landmark vertex "id x y".
Definition dataset.cpp:187
│ │ │ │ +
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ +
gtsam::parseEdge
boost::optional< IndexedEdge > parseEdge(istream &is, const string &tag)
Parse TORO/G2O edge "id1 id2 x y yaw".
Definition dataset.cpp:293
│ │ │ │ +
gtsam::GraphAndValues
std::pair< NonlinearFactorGraph::shared_ptr, Values::shared_ptr > GraphAndValues
Return type for load functions, which return a graph and initial values.
Definition dataset.h:144
│ │ │ │ +
gtsam::parseVertexPose
boost::optional< IndexedPose > parseVertexPose(istream &is, const string &tag)
Parse TORO/G2O vertex "id x y yaw".
Definition dataset.cpp:167
│ │ │ │ +
gtsam::load3D
GraphAndValues load3D(const string &filename)
Load TORO 3D Graph.
Definition dataset.cpp:918
│ │ │ │ +
gtsam::save2D
void save2D(const NonlinearFactorGraph &graph, const Values &config, const noiseModel::Diagonal::shared_ptr model, const string &filename)
save 2d graph
Definition dataset.cpp:584
│ │ │ │ +
gtsam::createRewrittenFileName
string createRewrittenFileName(const string &name)
Creates a temporary file name that needs to be ignored in .gitingnore for checking read-write opratio...
Definition dataset.cpp:99
│ │ │ │ +
gtsam::findExampleDataFile
string findExampleDataFile(const string &name)
Find the full path to an example dataset distributed with gtsam.
Definition dataset.cpp:62
│ │ │ │ +
gtsam::Pose2
A 2D pose (Point2,Rot2)
Definition Pose2.h:36
│ │ │ │ +
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
gtsam::Values::at
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
│ │ │ │ -
gtsam::BoundingConstraint1
Unary inequality constraint forcing a scalar to be greater/less than a fixed threshold.
Definition BoundingConstraint.h:33
│ │ │ │ -
gtsam::BoundingConstraint1::active
bool active(const Values &c) const override
active when constraint NOT met
Definition BoundingConstraint.h:61
│ │ │ │ -
gtsam::BoundingConstraint1::BoundingConstraint1
BoundingConstraint1(Key key, double threshold, bool isGreaterThan, double mu=1000.0)
flag for greater/less than
Definition BoundingConstraint.h:41
│ │ │ │ -
gtsam::BoundingConstraint1::access
friend class boost::serialization::access
Serialization function.
Definition BoundingConstraint.h:85
│ │ │ │ -
gtsam::BoundingConstraint1::value
virtual double value(const X &x, boost::optional< Matrix & > H=boost::none) const =0
function producing a scalar value to compare to the threshold Must have optional argument for derivat...
│ │ │ │ -
gtsam::BoundingConstraint2
Binary scalar inequality constraint, with a similar value() function to implement for specific system...
Definition BoundingConstraint.h:101
│ │ │ │ -
gtsam::BoundingConstraint2::active
bool active(const Values &c) const override
active when constraint NOT met
Definition BoundingConstraint.h:130
│ │ │ │ -
gtsam::BoundingConstraint2::BoundingConstraint2
BoundingConstraint2(Key key1, Key key2, double threshold, bool isGreaterThan, double mu=1000.0)
flag for greater/less than
Definition BoundingConstraint.h:111
│ │ │ │ -
gtsam::BoundingConstraint2::access
friend class boost::serialization::access
Serialization function.
Definition BoundingConstraint.h:159
│ │ │ │ -
gtsam::BoundingConstraint2::value
virtual double value(const X1 &x1, const X2 &x2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const =0
function producing a scalar value to compare to the threshold Must have optional argument for derivat...
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,232 +1,295 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -BoundingConstraint.h │ │ │ │ │ +dataset.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_b_a_s_e_/_L_i_e_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +21#pragma once │ │ │ │ │ 22 │ │ │ │ │ -23namespace _g_t_s_a_m { │ │ │ │ │ -24 │ │ │ │ │ -32template │ │ │ │ │ -_3_3struct _B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_1: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -34 typedef VALUE X; │ │ │ │ │ -35 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_V_A_L_U_E_> _B_a_s_e; │ │ │ │ │ -36 typedef boost::shared_ptr > shared_ptr; │ │ │ │ │ -37 │ │ │ │ │ -38 double threshold_; │ │ │ │ │ -39 bool isGreaterThan_; │ │ │ │ │ -40 │ │ │ │ │ -_4_1 _B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_1(_K_e_y _k_e_y, double threshold, │ │ │ │ │ -42 bool isGreaterThan, double mu = 1000.0) : │ │ │ │ │ -43 _B_a_s_e(_n_o_i_s_e_M_o_d_e_l::Constrained::All(1, mu), _k_e_y), │ │ │ │ │ -44 threshold_(threshold), isGreaterThan_(isGreaterThan) { │ │ │ │ │ -45 } │ │ │ │ │ -46 │ │ │ │ │ -47 _~_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_1() override {} │ │ │ │ │ -48 │ │ │ │ │ -49 inline double threshold() const { return threshold_; } │ │ │ │ │ -50 inline bool isGreaterThan() const { return isGreaterThan_; } │ │ │ │ │ -51 │ │ │ │ │ -_5_7 virtual double _v_a_l_u_e(const X& x, boost::optional H = │ │ │ │ │ -58 boost::none) const = 0; │ │ │ │ │ -59 │ │ │ │ │ -_6_1 bool _a_c_t_i_v_e(const _V_a_l_u_e_s& c) const override { │ │ │ │ │ -62 // note: still active at equality to avoid zigzagging │ │ │ │ │ -63 double x = _v_a_l_u_e(c._a_t(this->key())); │ │ │ │ │ -64 return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; │ │ │ │ │ -65 } │ │ │ │ │ -66 │ │ │ │ │ -67 Vector evaluateError(const X& x, boost::optional H = │ │ │ │ │ -68 boost::none) const override { │ │ │ │ │ -69 Matrix D; │ │ │ │ │ -70 double _e_r_r_o_r = _v_a_l_u_e(x, D) - threshold_; │ │ │ │ │ -71 if (H) { │ │ │ │ │ -72 if (isGreaterThan_) *H = D; │ │ │ │ │ -73 else *H = -1.0 * D; │ │ │ │ │ -74 } │ │ │ │ │ -75 │ │ │ │ │ -76 if (isGreaterThan_) │ │ │ │ │ -77 return (Vector(1) << _e_r_r_o_r).finished(); │ │ │ │ │ -78 else │ │ │ │ │ -79 return -1.0 * (Vector(1) << _e_r_r_o_r).finished(); │ │ │ │ │ -80 } │ │ │ │ │ -81 │ │ │ │ │ -82private: │ │ │ │ │ -83 │ │ │ │ │ -_8_5 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -86 template │ │ │ │ │ -87 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -88 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility │ │ │ │ │ -89 ar & boost::serialization::make_nvp("NoiseModelFactor1", │ │ │ │ │ -90 boost::serialization::base_object(*this)); │ │ │ │ │ -91 ar & BOOST_SERIALIZATION_NVP(threshold_); │ │ │ │ │ -92 ar & BOOST_SERIALIZATION_NVP(isGreaterThan_); │ │ │ │ │ -93 } │ │ │ │ │ -94}; │ │ │ │ │ -95 │ │ │ │ │ -100template │ │ │ │ │ -_1_0_1struct _B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_2: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -102 typedef VALUE1 X1; │ │ │ │ │ -103 typedef VALUE2 X2; │ │ │ │ │ -104 │ │ │ │ │ -105 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_V_A_L_U_E_1_,_ _V_A_L_U_E_2_> _B_a_s_e; │ │ │ │ │ -106 typedef boost::shared_ptr > shared_ptr; │ │ │ │ │ -107 │ │ │ │ │ -108 double threshold_; │ │ │ │ │ -109 bool isGreaterThan_; │ │ │ │ │ +23#include <_g_t_s_a_m_/_s_f_m_/_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_s_l_a_m_/_B_e_t_w_e_e_n_F_a_c_t_o_r_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_s_f_m_/_S_f_m_D_a_t_a_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3_B_u_n_d_l_e_r_._h> │ │ │ │ │ +27#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_i_n_h_o_l_e_C_a_m_e_r_a_._h> │ │ │ │ │ +28#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_2_._h> │ │ │ │ │ +29#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_3_._h> │ │ │ │ │ +30#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +31#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_V_a_l_u_e_s_._h> │ │ │ │ │ +32#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_N_o_i_s_e_M_o_d_e_l_._h> │ │ │ │ │ +33#include <_g_t_s_a_m_/_b_a_s_e_/_s_e_r_i_a_l_i_z_a_t_i_o_n_._h> │ │ │ │ │ +34#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +35#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ +36 │ │ │ │ │ +37#include │ │ │ │ │ +38#include │ │ │ │ │ +39#include // for pair │ │ │ │ │ +40#include │ │ │ │ │ +41#include │ │ │ │ │ +42#include │ │ │ │ │ +43 │ │ │ │ │ +44namespace _g_t_s_a_m { │ │ │ │ │ +45 │ │ │ │ │ +57GTSAM_EXPORT std::string _f_i_n_d_E_x_a_m_p_l_e_D_a_t_a_F_i_l_e(const std::string& name); │ │ │ │ │ +58 │ │ │ │ │ +63GTSAM_EXPORT std::string _c_r_e_a_t_e_R_e_w_r_i_t_t_e_n_F_i_l_e_N_a_m_e(const std::string& name); │ │ │ │ │ +64 │ │ │ │ │ +_6_6enum _N_o_i_s_e_F_o_r_m_a_t { │ │ │ │ │ +_6_7 _N_o_i_s_e_F_o_r_m_a_t_G_2_O, │ │ │ │ │ +_6_8 _N_o_i_s_e_F_o_r_m_a_t_T_O_R_O, │ │ │ │ │ +_6_9 _N_o_i_s_e_F_o_r_m_a_t_G_R_A_P_H, │ │ │ │ │ +_7_0 _N_o_i_s_e_F_o_r_m_a_t_C_O_V, │ │ │ │ │ +71 _N_o_i_s_e_F_o_r_m_a_t_A_U_T_O │ │ │ │ │ +_7_2}; │ │ │ │ │ +73 │ │ │ │ │ +_7_5enum _K_e_r_n_e_l_F_u_n_c_t_i_o_n_T_y_p_e { │ │ │ │ │ +76 KernelFunctionTypeNONE, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY │ │ │ │ │ +77}; │ │ │ │ │ +78 │ │ │ │ │ +85template │ │ │ │ │ +_8_6GTSAM_EXPORT std::map _p_a_r_s_e_V_a_r_i_a_b_l_e_s(const std::string &filename, │ │ │ │ │ +87 size_t maxIndex = 0); │ │ │ │ │ +88 │ │ │ │ │ +95template │ │ │ │ │ +96GTSAM_EXPORT std::vector> │ │ │ │ │ +_9_7parseMeasurements(const std::string &filename, │ │ │ │ │ +98 const noiseModel::Diagonal::shared_ptr &model = nullptr, │ │ │ │ │ +99 size_t maxIndex = 0); │ │ │ │ │ +100 │ │ │ │ │ +105template │ │ │ │ │ +106GTSAM_EXPORT std::vector::shared_ptr> │ │ │ │ │ +_1_0_7_p_a_r_s_e_F_a_c_t_o_r_s(const std::string &filename, │ │ │ │ │ +108 const noiseModel::Diagonal::shared_ptr &model = nullptr, │ │ │ │ │ +109 size_t maxIndex = 0); │ │ │ │ │ 110 │ │ │ │ │ -_1_1_1 _B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_2(_K_e_y key1, _K_e_y key2, double threshold, │ │ │ │ │ -112 bool isGreaterThan, double mu = 1000.0) │ │ │ │ │ -113 : _B_a_s_e(_n_o_i_s_e_M_o_d_e_l::Constrained::All(1, mu), key1, key2), │ │ │ │ │ -114 threshold_(threshold), isGreaterThan_(isGreaterThan) {} │ │ │ │ │ +_1_1_2typedef std::pair _I_n_d_e_x_e_d_P_o_s_e; │ │ │ │ │ +113typedef std::pair IndexedLandmark; │ │ │ │ │ +114typedef std::pair, _P_o_s_e_2> IndexedEdge; │ │ │ │ │ 115 │ │ │ │ │ -116 _~_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_2() override {} │ │ │ │ │ -117 │ │ │ │ │ -118 inline double threshold() const { return threshold_; } │ │ │ │ │ -119 inline bool isGreaterThan() const { return isGreaterThan_; } │ │ │ │ │ -120 │ │ │ │ │ -_1_2_5 virtual double _v_a_l_u_e(const X1& x1, const X2& x2, │ │ │ │ │ -126 boost::optional H1 = boost::none, │ │ │ │ │ -127 boost::optional H2 = boost::none) const = 0; │ │ │ │ │ -128 │ │ │ │ │ -_1_3_0 bool _a_c_t_i_v_e(const _V_a_l_u_e_s& c) const override { │ │ │ │ │ -131 // note: still active at equality to avoid zigzagging │ │ │ │ │ -132 double x = _v_a_l_u_e(c._a_t(this->key1()), c._a_t(this->key2())); │ │ │ │ │ -133 return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; │ │ │ │ │ -134 } │ │ │ │ │ -135 │ │ │ │ │ -136 Vector evaluateError(const X1& x1, const X2& x2, │ │ │ │ │ -137 boost::optional H1 = boost::none, │ │ │ │ │ -138 boost::optional H2 = boost::none) const override { │ │ │ │ │ -139 Matrix D1, D2; │ │ │ │ │ -140 double _e_r_r_o_r = _v_a_l_u_e(x1, x2, D1, D2) - threshold_; │ │ │ │ │ -141 if (H1) { │ │ │ │ │ -142 if (isGreaterThan_) *H1 = D1; │ │ │ │ │ -143 else *H1 = -1.0 * D1; │ │ │ │ │ -144 } │ │ │ │ │ -145 if (H2) { │ │ │ │ │ -146 if (isGreaterThan_) *H2 = D2; │ │ │ │ │ -147 else *H2 = -1.0 * D2; │ │ │ │ │ -148 } │ │ │ │ │ -149 │ │ │ │ │ -150 if (isGreaterThan_) │ │ │ │ │ -151 return (Vector(1) << _e_r_r_o_r).finished(); │ │ │ │ │ -152 else │ │ │ │ │ -153 return -1.0 * (Vector(1) << _e_r_r_o_r).finished(); │ │ │ │ │ -154 } │ │ │ │ │ -155 │ │ │ │ │ -156private: │ │ │ │ │ -157 │ │ │ │ │ -_1_5_9 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -160 template │ │ │ │ │ -161 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -162 // NoiseModelFactor2 instead of NoiseModelFactorN for backward │ │ │ │ │ -compatibility │ │ │ │ │ -163 ar & boost::serialization::make_nvp("NoiseModelFactor2", │ │ │ │ │ -164 boost::serialization::base_object(*this)); │ │ │ │ │ -165 ar & BOOST_SERIALIZATION_NVP(threshold_); │ │ │ │ │ -166 ar & BOOST_SERIALIZATION_NVP(isGreaterThan_); │ │ │ │ │ -167 } │ │ │ │ │ -168}; │ │ │ │ │ -169 │ │ │ │ │ -170} // \namespace gtsam │ │ │ │ │ -_L_i_e_._h │ │ │ │ │ -Base class and basic functions for Lie types. │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ -Non-linear factor base classes. │ │ │ │ │ +121GTSAM_EXPORT boost::optional _p_a_r_s_e_V_e_r_t_e_x_P_o_s_e(std::istream& is, │ │ │ │ │ +122 const std::string& tag); │ │ │ │ │ +123 │ │ │ │ │ +129GTSAM_EXPORT boost::optional _p_a_r_s_e_V_e_r_t_e_x_L_a_n_d_m_a_r_k(std:: │ │ │ │ │ +istream& is, │ │ │ │ │ +130 const std::string& tag); │ │ │ │ │ +131 │ │ │ │ │ +137GTSAM_EXPORT boost::optional _p_a_r_s_e_E_d_g_e(std::istream& is, │ │ │ │ │ +138 const std::string& tag); │ │ │ │ │ +139 │ │ │ │ │ +_1_4_3using _G_r_a_p_h_A_n_d_V_a_l_u_e_s = │ │ │ │ │ +144 std::pair; │ │ │ │ │ +145 │ │ │ │ │ +153GTSAM_EXPORT _G_r_a_p_h_A_n_d_V_a_l_u_e_s _l_o_a_d_2_D( │ │ │ │ │ +154 std::pair dataset, size_t maxIndex = 0, │ │ │ │ │ +155 bool addNoise = false, │ │ │ │ │ +156 bool smart = true, // │ │ │ │ │ +157 _N_o_i_s_e_F_o_r_m_a_t noiseFormat = _N_o_i_s_e_F_o_r_m_a_t_A_U_T_O, │ │ │ │ │ +158 _K_e_r_n_e_l_F_u_n_c_t_i_o_n_T_y_p_e kernelFunctionType = KernelFunctionTypeNONE); │ │ │ │ │ +159 │ │ │ │ │ +171GTSAM_EXPORT _G_r_a_p_h_A_n_d_V_a_l_u_e_s │ │ │ │ │ +172_l_o_a_d_2_D(const std::string& filename, _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l model = │ │ │ │ │ +_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l(), │ │ │ │ │ +173 size_t maxIndex = 0, bool addNoise = false, bool smart = true, │ │ │ │ │ +174 _N_o_i_s_e_F_o_r_m_a_t noiseFormat = _N_o_i_s_e_F_o_r_m_a_t_A_U_T_O, // │ │ │ │ │ +175 _K_e_r_n_e_l_F_u_n_c_t_i_o_n_T_y_p_e kernelFunctionType = KernelFunctionTypeNONE); │ │ │ │ │ +176 │ │ │ │ │ +178GTSAM_EXPORT void _s_a_v_e_2_D(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, │ │ │ │ │ +179 const _V_a_l_u_e_s& config, const noiseModel::Diagonal::shared_ptr model, │ │ │ │ │ +180 const std::string& filename); │ │ │ │ │ +181 │ │ │ │ │ +190GTSAM_EXPORT _G_r_a_p_h_A_n_d_V_a_l_u_e_s │ │ │ │ │ +191_r_e_a_d_G_2_o(const std::string& g2oFile, const bool is3D = false, │ │ │ │ │ +192 _K_e_r_n_e_l_F_u_n_c_t_i_o_n_T_y_p_e kernelFunctionType = KernelFunctionTypeNONE); │ │ │ │ │ +193 │ │ │ │ │ +206GTSAM_EXPORT void _w_r_i_t_e_G_2_o(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, │ │ │ │ │ +207 const _V_a_l_u_e_s& estimate, const std::string& filename); │ │ │ │ │ +208 │ │ │ │ │ +210GTSAM_EXPORT _G_r_a_p_h_A_n_d_V_a_l_u_e_s _l_o_a_d_3_D(const std::string& filename); │ │ │ │ │ +211 │ │ │ │ │ +212// Wrapper-friendly versions of parseFactors and parseFactors │ │ │ │ │ +213using BetweenFactorPose2s = std::vector::shared_ptr>; │ │ │ │ │ +214GTSAM_EXPORT BetweenFactorPose2s │ │ │ │ │ +215parse2DFactors(const std::string &filename, │ │ │ │ │ +216 const noiseModel::Diagonal::shared_ptr &model = nullptr, │ │ │ │ │ +217 size_t maxIndex = 0); │ │ │ │ │ +218 │ │ │ │ │ +219using BetweenFactorPose3s = std::vector::shared_ptr>; │ │ │ │ │ +220GTSAM_EXPORT BetweenFactorPose3s │ │ │ │ │ +221parse3DFactors(const std::string &filename, │ │ │ │ │ +222 const noiseModel::Diagonal::shared_ptr &model = nullptr, │ │ │ │ │ +223 size_t maxIndex = 0); │ │ │ │ │ +224 │ │ │ │ │ +225using BinaryMeasurementsUnit3 = std::vector>; │ │ │ │ │ +226using BinaryMeasurementsPoint3 = std::vector>; │ │ │ │ │ +227using BinaryMeasurementsRot3 = std::vector>; │ │ │ │ │ +228 │ │ │ │ │ +229#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ +230inline boost::optional GTSAM_DEPRECATED │ │ │ │ │ +231parseVertex(std::istream& is, const std::string& tag) { │ │ │ │ │ +232 return _p_a_r_s_e_V_e_r_t_e_x_P_o_s_e(is, tag); │ │ │ │ │ +233} │ │ │ │ │ +234 │ │ │ │ │ +235GTSAM_EXPORT std::map GTSAM_DEPRECATED │ │ │ │ │ +236parse3DPoses(const std::string& filename, size_t maxIndex = 0); │ │ │ │ │ +237 │ │ │ │ │ +238GTSAM_EXPORT std::map GTSAM_DEPRECATED │ │ │ │ │ +239parse3DLandmarks(const std::string& filename, size_t maxIndex = 0); │ │ │ │ │ +240 │ │ │ │ │ +241GTSAM_EXPORT _G_r_a_p_h_A_n_d_V_a_l_u_e_s GTSAM_DEPRECATED │ │ │ │ │ +242load2D_robust(const std::string& filename, │ │ │ │ │ +243 const noiseModel::Base::shared_ptr& model, size_t maxIndex = 0); │ │ │ │ │ +244#endif │ │ │ │ │ +245} // namespace gtsam │ │ │ │ │ +_s_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ +Convenience functions for serializing data structures via boost.serialization. │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ +_t_y_p_e_s_._h │ │ │ │ │ +Typedefs for easier changing of types. │ │ │ │ │ +_P_o_s_e_3_._h │ │ │ │ │ +3D Pose │ │ │ │ │ +_P_o_s_e_2_._h │ │ │ │ │ +2D Pose │ │ │ │ │ +_C_a_l_3_B_u_n_d_l_e_r_._h │ │ │ │ │ +Calibration used by Bundler. │ │ │ │ │ +_P_i_n_h_o_l_e_C_a_m_e_r_a_._h │ │ │ │ │ +Base class for all pinhole cameras. │ │ │ │ │ +_N_o_i_s_e_M_o_d_e_l_._h │ │ │ │ │ +_V_a_l_u_e_s_._h │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Factor Graph consisting of non-linear factors. │ │ │ │ │ +_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_._h │ │ │ │ │ +Binary measurement represents a measurement between two keys in a graph. A │ │ │ │ │ +binary measurement is simi... │ │ │ │ │ +_S_f_m_D_a_t_a_._h │ │ │ │ │ +Data structure for dealing with Structure from Motion data. │ │ │ │ │ +_B_e_t_w_e_e_n_F_a_c_t_o_r_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_e_r_r_o_r │ │ │ │ │ -double error(const Values &c) const override │ │ │ │ │ -Calculate the error of the factor. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.cpp:138 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_n_o_i_s_e_M_o_d_e_l │ │ │ │ │ -const SharedNoiseModel & noiseModel() const │ │ │ │ │ -access to the noise model │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:223 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ -A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ -variables. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_ _V_A_L_U_E_ _>_:_:_k_e_y │ │ │ │ │ -Key key() const │ │ │ │ │ -Returns a key. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:518 │ │ │ │ │ +_g_t_s_a_m_:_:_l_o_a_d_2_D │ │ │ │ │ +GraphAndValues load2D(const string &filename, SharedNoiseModel model, size_t │ │ │ │ │ +maxIndex, bool addNoise, bool smart, NoiseFormat noiseFormat, │ │ │ │ │ +KernelFunctionType kernelFunctionType) │ │ │ │ │ +Load TORO/G2O style graph files. │ │ │ │ │ +DDeeffiinniittiioonn dataset.cpp:502 │ │ │ │ │ +_g_t_s_a_m_:_:_p_a_r_s_e_V_a_r_i_a_b_l_e_s │ │ │ │ │ +GTSAM_EXPORT std::map< size_t, T > parseVariables(const std::string &filename, │ │ │ │ │ +size_t maxIndex=0) │ │ │ │ │ +Parse variables in a line-based text format (like g2o) into a map. │ │ │ │ │ +_g_t_s_a_m_:_:_w_r_i_t_e_G_2_o │ │ │ │ │ +void writeG2o(const NonlinearFactorGraph &graph, const Values &estimate, const │ │ │ │ │ +string &filename) │ │ │ │ │ +This function writes a g2o file from NonlinearFactorGraph and a Values │ │ │ │ │ +structure. │ │ │ │ │ +DDeeffiinniittiioonn dataset.cpp:632 │ │ │ │ │ +_g_t_s_a_m_:_:_I_n_d_e_x_e_d_P_o_s_e │ │ │ │ │ +std::pair< size_t, Pose2 > IndexedPose │ │ │ │ │ +Return type for auxiliary functions. │ │ │ │ │ +DDeeffiinniittiioonn dataset.h:112 │ │ │ │ │ +_g_t_s_a_m_:_:_r_e_a_d_G_2_o │ │ │ │ │ +GraphAndValues readG2o(const string &g2oFile, const bool is3D, │ │ │ │ │ +KernelFunctionType kernelFunctionType) │ │ │ │ │ +This function parses a g2o file and stores the measurements into a │ │ │ │ │ +NonlinearFactorGraph and the initi... │ │ │ │ │ +DDeeffiinniittiioonn dataset.cpp:617 │ │ │ │ │ +_g_t_s_a_m_:_:_p_a_r_s_e_F_a_c_t_o_r_s │ │ │ │ │ +GTSAM_EXPORT std::vector< typename BetweenFactor< T >::shared_ptr > │ │ │ │ │ +parseFactors(const std::string &filename, const noiseModel::Diagonal:: │ │ │ │ │ +shared_ptr &model=nullptr, size_t maxIndex=0) │ │ │ │ │ +Parse BetweenFactors in a line-based text format (like g2o) into a vector of │ │ │ │ │ +shared pointers. │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_r_n_e_l_F_u_n_c_t_i_o_n_T_y_p_e │ │ │ │ │ +KernelFunctionType │ │ │ │ │ +Robust kernel type to wrap around quadratic noise model. │ │ │ │ │ +DDeeffiinniittiioonn dataset.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_F_o_r_m_a_t │ │ │ │ │ +NoiseFormat │ │ │ │ │ +Indicates how noise parameters are stored in file. │ │ │ │ │ +DDeeffiinniittiioonn dataset.h:66 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_F_o_r_m_a_t_G_R_A_P_H │ │ │ │ │ +@ NoiseFormatGRAPH │ │ │ │ │ +default: toro-style order, but covariance matrix ! │ │ │ │ │ +DDeeffiinniittiioonn dataset.h:69 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_F_o_r_m_a_t_A_U_T_O │ │ │ │ │ +@ NoiseFormatAUTO │ │ │ │ │ +Try to guess covariance matrix layout. │ │ │ │ │ +DDeeffiinniittiioonn dataset.h:71 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_F_o_r_m_a_t_T_O_R_O │ │ │ │ │ +@ NoiseFormatTORO │ │ │ │ │ +Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr. │ │ │ │ │ +DDeeffiinniittiioonn dataset.h:68 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_F_o_r_m_a_t_C_O_V │ │ │ │ │ +@ NoiseFormatCOV │ │ │ │ │ +Covariance matrix C11, C12, C13, C22, C23, C33. │ │ │ │ │ +DDeeffiinniittiioonn dataset.h:70 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_F_o_r_m_a_t_G_2_O │ │ │ │ │ +@ NoiseFormatG2O │ │ │ │ │ +Information matrix I11, I12, I13, I22, I23, I33. │ │ │ │ │ +DDeeffiinniittiioonn dataset.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_p_a_r_s_e_V_e_r_t_e_x_L_a_n_d_m_a_r_k │ │ │ │ │ +boost::optional< IndexedLandmark > parseVertexLandmark(istream &is, const │ │ │ │ │ +string &tag) │ │ │ │ │ +Parse G2O landmark vertex "id x y". │ │ │ │ │ +DDeeffiinniittiioonn dataset.cpp:187 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ +Aliases. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ +_g_t_s_a_m_:_:_p_a_r_s_e_E_d_g_e │ │ │ │ │ +boost::optional< IndexedEdge > parseEdge(istream &is, const string &tag) │ │ │ │ │ +Parse TORO/G2O edge "id1 id2 x y yaw". │ │ │ │ │ +DDeeffiinniittiioonn dataset.cpp:293 │ │ │ │ │ +_g_t_s_a_m_:_:_G_r_a_p_h_A_n_d_V_a_l_u_e_s │ │ │ │ │ +std::pair< NonlinearFactorGraph::shared_ptr, Values::shared_ptr > │ │ │ │ │ +GraphAndValues │ │ │ │ │ +Return type for load functions, which return a graph and initial values. │ │ │ │ │ +DDeeffiinniittiioonn dataset.h:144 │ │ │ │ │ +_g_t_s_a_m_:_:_p_a_r_s_e_V_e_r_t_e_x_P_o_s_e │ │ │ │ │ +boost::optional< IndexedPose > parseVertexPose(istream &is, const string &tag) │ │ │ │ │ +Parse TORO/G2O vertex "id x y yaw". │ │ │ │ │ +DDeeffiinniittiioonn dataset.cpp:167 │ │ │ │ │ +_g_t_s_a_m_:_:_l_o_a_d_3_D │ │ │ │ │ +GraphAndValues load3D(const string &filename) │ │ │ │ │ +Load TORO 3D Graph. │ │ │ │ │ +DDeeffiinniittiioonn dataset.cpp:918 │ │ │ │ │ +_g_t_s_a_m_:_:_s_a_v_e_2_D │ │ │ │ │ +void save2D(const NonlinearFactorGraph &graph, const Values &config, const │ │ │ │ │ +noiseModel::Diagonal::shared_ptr model, const string &filename) │ │ │ │ │ +save 2d graph │ │ │ │ │ +DDeeffiinniittiioonn dataset.cpp:584 │ │ │ │ │ +_g_t_s_a_m_:_:_c_r_e_a_t_e_R_e_w_r_i_t_t_e_n_F_i_l_e_N_a_m_e │ │ │ │ │ +string createRewrittenFileName(const string &name) │ │ │ │ │ +Creates a temporary file name that needs to be ignored in .gitingnore for │ │ │ │ │ +checking read-write opratio... │ │ │ │ │ +DDeeffiinniittiioonn dataset.cpp:99 │ │ │ │ │ +_g_t_s_a_m_:_:_f_i_n_d_E_x_a_m_p_l_e_D_a_t_a_F_i_l_e │ │ │ │ │ +string findExampleDataFile(const string &name) │ │ │ │ │ +Find the full path to an example dataset distributed with gtsam. │ │ │ │ │ +DDeeffiinniittiioonn dataset.cpp:62 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_2 │ │ │ │ │ +A 2D pose (Point2,Rot2) │ │ │ │ │ +DDeeffiinniittiioonn Pose2.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ _g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_a_t │ │ │ │ │ -const ValueType at(Key j) const │ │ │ │ │ -Retrieve a variable by key j. │ │ │ │ │ -DDeeffiinniittiioonn Values-inl.h:361 │ │ │ │ │ -_g_t_s_a_m_:_:_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_1 │ │ │ │ │ -Unary inequality constraint forcing a scalar to be greater/less than a fixed │ │ │ │ │ -threshold. │ │ │ │ │ -DDeeffiinniittiioonn BoundingConstraint.h:33 │ │ │ │ │ -_g_t_s_a_m_:_:_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_1_:_:_a_c_t_i_v_e │ │ │ │ │ -bool active(const Values &c) const override │ │ │ │ │ -active when constraint NOT met │ │ │ │ │ -DDeeffiinniittiioonn BoundingConstraint.h:61 │ │ │ │ │ -_g_t_s_a_m_:_:_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_1_:_:_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_1 │ │ │ │ │ -BoundingConstraint1(Key key, double threshold, bool isGreaterThan, double │ │ │ │ │ -mu=1000.0) │ │ │ │ │ -flag for greater/less than │ │ │ │ │ -DDeeffiinniittiioonn BoundingConstraint.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_1_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn BoundingConstraint.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_1_:_:_v_a_l_u_e │ │ │ │ │ -virtual double value(const X &x, boost::optional< Matrix & > H=boost::none) │ │ │ │ │ -const =0 │ │ │ │ │ -function producing a scalar value to compare to the threshold Must have │ │ │ │ │ -optional argument for derivat... │ │ │ │ │ -_g_t_s_a_m_:_:_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_2 │ │ │ │ │ -Binary scalar inequality constraint, with a similar value() function to │ │ │ │ │ -implement for specific system... │ │ │ │ │ -DDeeffiinniittiioonn BoundingConstraint.h:101 │ │ │ │ │ -_g_t_s_a_m_:_:_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_2_:_:_a_c_t_i_v_e │ │ │ │ │ -bool active(const Values &c) const override │ │ │ │ │ -active when constraint NOT met │ │ │ │ │ -DDeeffiinniittiioonn BoundingConstraint.h:130 │ │ │ │ │ -_g_t_s_a_m_:_:_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_2_:_:_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_2 │ │ │ │ │ -BoundingConstraint2(Key key1, Key key2, double threshold, bool isGreaterThan, │ │ │ │ │ -double mu=1000.0) │ │ │ │ │ -flag for greater/less than │ │ │ │ │ -DDeeffiinniittiioonn BoundingConstraint.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_2_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn BoundingConstraint.h:159 │ │ │ │ │ -_g_t_s_a_m_:_:_B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_2_:_:_v_a_l_u_e │ │ │ │ │ -virtual double value(const X1 &x1, const X2 &x2, boost::optional< Matrix & > │ │ │ │ │ -H1=boost::none, boost::optional< Matrix & > H2=boost::none) const =0 │ │ │ │ │ -function producing a scalar value to compare to the threshold Must have │ │ │ │ │ -optional argument for derivat... │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _B_o_u_n_d_i_n_g_C_o_n_s_t_r_a_i_n_t_._h │ │ │ │ │ + * _d_a_t_a_s_e_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01322_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/TriangulationFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/SmartProjectionPoseFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,245 +98,176 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
TriangulationFactor.h
│ │ │ │ +
SmartProjectionPoseFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ -
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
21#include <gtsam/geometry/CalibratedCamera.h>
│ │ │ │ -
22#include <boost/make_shared.hpp>
│ │ │ │ -
23#include <boost/lexical_cast.hpp>
│ │ │ │ -
24
│ │ │ │ -
25namespace gtsam {
│ │ │ │ -
26
│ │ │ │ -
32template<class CAMERA>
│ │ │ │ -
│ │ │ │ -
33class TriangulationFactor: public NoiseModelFactorN<Point3> {
│ │ │ │ -
34
│ │ │ │ -
35public:
│ │ │ │ -
36
│ │ │ │ -
38 using Camera = CAMERA;
│ │ │ │ -
39
│ │ │ │ -
40protected:
│ │ │ │ -
41
│ │ │ │ -
43 using Base = NoiseModelFactorN<Point3>;
│ │ │ │ -
44
│ │ │ │ -
46 using This = TriangulationFactor<CAMERA>;
│ │ │ │ -
47
│ │ │ │ -
49 using Measurement = typename CAMERA::Measurement;
│ │ │ │ -
50
│ │ │ │ -
51 // Keep a copy of measurement and calibration for I/O
│ │ │ │ -
52 const CAMERA camera_;
│ │ │ │ -
53 const Measurement measured_;
│ │ │ │ -
54
│ │ │ │ -
55 // verbosity handling for Cheirality Exceptions
│ │ │ │ -
56 const bool throwCheirality_;
│ │ │ │ -
57 const bool verboseCheirality_;
│ │ │ │ -
58
│ │ │ │ -
59public:
│ │ │ │ -
60 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
61
│ │ │ │ -
63 using shared_ptr = boost::shared_ptr<This>;
│ │ │ │ -
64
│ │ │ │ -
│ │ │ │ -
66 TriangulationFactor() :
│ │ │ │ -
67 throwCheirality_(false), verboseCheirality_(false) {
│ │ │ │ -
68 }
│ │ │ │ -
│ │ │ │ -
69
│ │ │ │ -
│ │ │ │ -
79 TriangulationFactor(const CAMERA& camera, const Measurement& measured,
│ │ │ │ -
80 const SharedNoiseModel& model, Key pointKey, bool throwCheirality = false,
│ │ │ │ -
81 bool verboseCheirality = false) :
│ │ │ │ -
82 Base(model, pointKey), camera_(camera), measured_(measured), throwCheirality_(
│ │ │ │ -
83 throwCheirality), verboseCheirality_(verboseCheirality) {
│ │ │ │ -
84 if (model && model->dim() != traits<Measurement>::dimension)
│ │ │ │ -
85 throw std::invalid_argument(
│ │ │ │ -
86 "TriangulationFactor must be created with "
│ │ │ │ -
87 + boost::lexical_cast<std::string>((int) traits<Measurement>::dimension)
│ │ │ │ -
88 + "-dimensional noise model.");
│ │ │ │ -
89 }
│ │ │ │ -
│ │ │ │ -
90
│ │ │ │ -
│ │ │ │ -
92 ~TriangulationFactor() override {
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/slam/SmartProjectionFactor.h>
│ │ │ │ +
23
│ │ │ │ +
24namespace gtsam {
│ │ │ │ +
44template <class CALIBRATION>
│ │ │ │ +
│ │ │ │ +
45class SmartProjectionPoseFactor
│ │ │ │ +
46 : public SmartProjectionFactor<PinholePose<CALIBRATION> > {
│ │ │ │ +
47 private:
│ │ │ │ +
48 typedef PinholePose<CALIBRATION> Camera;
│ │ │ │ +
49 typedef SmartProjectionFactor<Camera> Base;
│ │ │ │ +
50 typedef SmartProjectionPoseFactor<CALIBRATION> This;
│ │ │ │ +
51
│ │ │ │ +
52protected:
│ │ │ │ +
53
│ │ │ │ +
54 boost::shared_ptr<CALIBRATION> K_;
│ │ │ │ +
55
│ │ │ │ +
56public:
│ │ │ │ +
57
│ │ │ │ +
59 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
60
│ │ │ │ +
64 SmartProjectionPoseFactor() {}
│ │ │ │ +
65
│ │ │ │ +
│ │ │ │ +
72 SmartProjectionPoseFactor(
│ │ │ │ +
73 const SharedNoiseModel& sharedNoiseModel,
│ │ │ │ +
74 const boost::shared_ptr<CALIBRATION> K,
│ │ │ │ +
75 const SmartProjectionParams& params = SmartProjectionParams())
│ │ │ │ +
76 : Base(sharedNoiseModel, params), K_(K) {
│ │ │ │ +
77 }
│ │ │ │ +
│ │ │ │ +
78
│ │ │ │ +
│ │ │ │ +
86 SmartProjectionPoseFactor(
│ │ │ │ +
87 const SharedNoiseModel& sharedNoiseModel,
│ │ │ │ +
88 const boost::shared_ptr<CALIBRATION> K,
│ │ │ │ +
89 const boost::optional<Pose3> body_P_sensor,
│ │ │ │ +
90 const SmartProjectionParams& params = SmartProjectionParams())
│ │ │ │ +
91 : SmartProjectionPoseFactor(sharedNoiseModel, K, params) {
│ │ │ │ +
92 this->body_P_sensor_ = body_P_sensor;
│ │ │ │
93 }
│ │ │ │
│ │ │ │
94
│ │ │ │
│ │ │ │ -
96 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
97 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ -
98 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
│ │ │ │ -
99 }
│ │ │ │ -
│ │ │ │ -
100
│ │ │ │ -
│ │ │ │ -
106 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
│ │ │ │ -
107 DefaultKeyFormatter) const override {
│ │ │ │ -
108 std::cout << s << "TriangulationFactor,";
│ │ │ │ -
109 camera_.print("camera");
│ │ │ │ -
110 traits<Measurement>::Print(measured_, "z");
│ │ │ │ -
111 Base::print("", keyFormatter);
│ │ │ │ -
112 }
│ │ │ │ -
│ │ │ │ -
113
│ │ │ │ -
│ │ │ │ -
115 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
│ │ │ │ -
116 const This *e = dynamic_cast<const This*>(&p);
│ │ │ │ -
117 return e && Base::equals(p, tol) && this->camera_.equals(e->camera_, tol)
│ │ │ │ -
118 && traits<Measurement>::Equals(this->measured_, e->measured_, tol);
│ │ │ │ -
119 }
│ │ │ │ -
│ │ │ │ -
120
│ │ │ │ -
│ │ │ │ -
122 Vector evaluateError(const Point3& point, boost::optional<Matrix&> H2 =
│ │ │ │ -
123 boost::none) const override {
│ │ │ │ -
124 try {
│ │ │ │ -
125 return traits<Measurement>::Local(measured_, camera_.project2(point, boost::none, H2));
│ │ │ │ -
126 } catch (CheiralityException& e) {
│ │ │ │ -
127 if (H2)
│ │ │ │ -
128 *H2 = Matrix::Zero(traits<Measurement>::dimension, 3);
│ │ │ │ -
129 if (verboseCheirality_)
│ │ │ │ -
130 std::cout << e.what() << ": Landmark "
│ │ │ │ -
131 << DefaultKeyFormatter(this->key()) << " moved behind camera"
│ │ │ │ -
132 << std::endl;
│ │ │ │ -
133 if (throwCheirality_)
│ │ │ │ -
134 throw e;
│ │ │ │ -
135 return camera_.defaultErrorWhenTriangulatingBehindCamera();
│ │ │ │ -
136 }
│ │ │ │ -
137 }
│ │ │ │ -
│ │ │ │ -
138
│ │ │ │ -
140 mutable VerticalBlockMatrix Ab;
│ │ │ │ -
141 mutable Matrix A;
│ │ │ │ -
142 mutable Vector b;
│ │ │ │ -
143
│ │ │ │ -
│ │ │ │ -
149 boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override {
│ │ │ │ -
150 // Only linearize if the factor is active
│ │ │ │ -
151 if (!this->active(x))
│ │ │ │ -
152 return boost::shared_ptr<JacobianFactor>();
│ │ │ │ -
153
│ │ │ │ -
154 // Allocate memory for Jacobian factor, do only once
│ │ │ │ -
155 if (Ab.rows() == 0) {
│ │ │ │ -
156 std::vector<size_t> dimensions(1, 3);
│ │ │ │ -
157 Ab = VerticalBlockMatrix(dimensions, traits<Measurement>::dimension, true);
│ │ │ │ -
158 A.resize(traits<Measurement>::dimension,3);
│ │ │ │ -
159 b.resize(traits<Measurement>::dimension);
│ │ │ │ -
160 }
│ │ │ │ -
161
│ │ │ │ -
162 // Would be even better if we could pass blocks to project
│ │ │ │ -
163 const Point3& point = x.at<Point3>(key());
│ │ │ │ -
164 b = traits<Measurement>::Local(camera_.project2(point, boost::none, A), measured_);
│ │ │ │ -
165 if (noiseModel_)
│ │ │ │ -
166 this->noiseModel_->WhitenSystem(A, b);
│ │ │ │ -
167
│ │ │ │ -
168 Ab(0) = A;
│ │ │ │ -
169 Ab(1) = b;
│ │ │ │ -
170
│ │ │ │ -
171 return boost::make_shared<JacobianFactor>(this->keys_, Ab);
│ │ │ │ -
172 }
│ │ │ │ -
│ │ │ │ -
173
│ │ │ │ -
│ │ │ │ -
175 const Measurement& measured() const {
│ │ │ │ -
176 return measured_;
│ │ │ │ -
177 }
│ │ │ │ -
│ │ │ │ -
178
│ │ │ │ -
│ │ │ │ -
180 inline bool verboseCheirality() const {
│ │ │ │ -
181 return verboseCheirality_;
│ │ │ │ -
182 }
│ │ │ │ -
│ │ │ │ -
183
│ │ │ │ -
│ │ │ │ -
185 inline bool throwCheirality() const {
│ │ │ │ -
186 return throwCheirality_;
│ │ │ │ -
187 }
│ │ │ │ -
│ │ │ │ -
188
│ │ │ │ -
189private:
│ │ │ │ -
190
│ │ │ │ -
192 friend class boost::serialization::access;
│ │ │ │ -
193 template<class ARCHIVE>
│ │ │ │ -
194 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
195 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
196 ar & BOOST_SERIALIZATION_NVP(camera_);
│ │ │ │ -
197 ar & BOOST_SERIALIZATION_NVP(measured_);
│ │ │ │ -
198 ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
│ │ │ │ -
199 ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
│ │ │ │ -
200 }
│ │ │ │ -
201};
│ │ │ │ -
│ │ │ │ -
202} // \ namespace gtsam
│ │ │ │ -
203
│ │ │ │ -
CalibratedCamera.h
Calibrated camera for which only pose is unknown.
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
96 ~SmartProjectionPoseFactor() override {
│ │ │ │ +
97 }
│ │ │ │ +
│ │ │ │ +
98
│ │ │ │ +
│ │ │ │ +
104 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
│ │ │ │ +
105 DefaultKeyFormatter) const override {
│ │ │ │ +
106 std::cout << s << "SmartProjectionPoseFactor, z = \n ";
│ │ │ │ +
107 Base::print("", keyFormatter);
│ │ │ │ +
108 }
│ │ │ │ +
│ │ │ │ +
109
│ │ │ │ +
│ │ │ │ +
111 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
│ │ │ │ +
112 const This *e = dynamic_cast<const This*>(&p);
│ │ │ │ +
113 return e && Base::equals(p, tol);
│ │ │ │ +
114 }
│ │ │ │ +
│ │ │ │ +
115
│ │ │ │ +
│ │ │ │ +
119 double error(const Values& values) const override {
│ │ │ │ +
120 if (this->active(values)) {
│ │ │ │ +
121 return this->totalReprojectionError(cameras(values));
│ │ │ │ +
122 } else { // else of active flag
│ │ │ │ +
123 return 0.0;
│ │ │ │ +
124 }
│ │ │ │ +
125 }
│ │ │ │ +
│ │ │ │ +
126
│ │ │ │ +
│ │ │ │ +
128 inline const boost::shared_ptr<CALIBRATION> calibration() const {
│ │ │ │ +
129 return K_;
│ │ │ │ +
130 }
│ │ │ │ +
│ │ │ │ +
131
│ │ │ │ +
│ │ │ │ +
138 typename Base::Cameras cameras(const Values& values) const override {
│ │ │ │ +
139 typename Base::Cameras cameras;
│ │ │ │ +
140 for (const Key& k : this->keys_) {
│ │ │ │ +
141 const Pose3 world_P_sensor_k =
│ │ │ │ +
142 Base::body_P_sensor_ ? values.at<Pose3>(k) * *Base::body_P_sensor_
│ │ │ │ +
143 : values.at<Pose3>(k);
│ │ │ │ +
144 cameras.emplace_back(world_P_sensor_k, K_);
│ │ │ │ +
145 }
│ │ │ │ +
146 return cameras;
│ │ │ │ +
147 }
│ │ │ │ +
│ │ │ │ +
148
│ │ │ │ +
149 private:
│ │ │ │ +
150
│ │ │ │ +
152 friend class boost::serialization::access;
│ │ │ │ +
153 template<class ARCHIVE>
│ │ │ │ +
154 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
155 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
156 ar & BOOST_SERIALIZATION_NVP(K_);
│ │ │ │ +
157 }
│ │ │ │ +
158};
│ │ │ │ +
│ │ │ │ +
159// end of class declaration
│ │ │ │ +
160
│ │ │ │ +
162template<class CALIBRATION>
│ │ │ │ +
│ │ │ │ +
163struct traits<SmartProjectionPoseFactor<CALIBRATION> > : public Testable<
│ │ │ │ +
164 SmartProjectionPoseFactor<CALIBRATION> > {
│ │ │ │ +
165};
│ │ │ │ +
│ │ │ │ +
166
│ │ │ │ +
167} // \ namespace gtsam
│ │ │ │ +
SmartProjectionFactor.h
Smart factor on cameras (pose + calibration)
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::VerticalBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43
│ │ │ │ -
gtsam::VerticalBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition VerticalBlockMatrix.h:115
│ │ │ │ -
gtsam::CheiralityException
Definition CalibratedCamera.h:32
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::PinholePose
A pinhole camera class that has a Pose3 and a fixed Calibration.
Definition PinholePose.h:243
│ │ │ │ +
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
│ │ │ │ -
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
│ │ │ │ -
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
│ │ │ │
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition NonlinearFactor.h:118
│ │ │ │ -
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ -
gtsam::NoiseModelFactorN< Point3 >::key
Key key() const
Returns a key.
Definition NonlinearFactor.h:518
│ │ │ │
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │
gtsam::Values::at
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
│ │ │ │ -
gtsam::TriangulationFactor
Non-linear factor for a constraint derived from a 2D measurement.
Definition TriangulationFactor.h:33
│ │ │ │ -
gtsam::TriangulationFactor::Ab
VerticalBlockMatrix Ab
thread-safe (?) scratch memory for linearize
Definition TriangulationFactor.h:140
│ │ │ │ -
gtsam::TriangulationFactor::throwCheirality_
const bool throwCheirality_
If true, rethrows Cheirality exceptions (default: false)
Definition TriangulationFactor.h:56
│ │ │ │ -
gtsam::TriangulationFactor::throwCheirality
bool throwCheirality() const
return flag for throwing cheirality exceptions
Definition TriangulationFactor.h:185
│ │ │ │ -
gtsam::TriangulationFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition TriangulationFactor.h:106
│ │ │ │ -
gtsam::TriangulationFactor::evaluateError
Vector evaluateError(const Point3 &point, boost::optional< Matrix & > H2=boost::none) const override
Evaluate error h(x)-z and optionally derivatives.
Definition TriangulationFactor.h:122
│ │ │ │ -
gtsam::TriangulationFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition TriangulationFactor.h:96
│ │ │ │ -
gtsam::TriangulationFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override
Linearize to a JacobianFactor, does not support constrained noise model ! Hence .
Definition TriangulationFactor.h:149
│ │ │ │ -
gtsam::TriangulationFactor::TriangulationFactor
TriangulationFactor(const CAMERA &camera, const Measurement &measured, const SharedNoiseModel &model, Key pointKey, bool throwCheirality=false, bool verboseCheirality=false)
Constructor with exception-handling flags.
Definition TriangulationFactor.h:79
│ │ │ │ -
gtsam::TriangulationFactor::Measurement
typename CAMERA::Measurement Measurement
shorthand for measurement type, e.g. Point2 or StereoPoint2
Definition TriangulationFactor.h:49
│ │ │ │ -
gtsam::TriangulationFactor::measured
const Measurement & measured() const
return the measurement
Definition TriangulationFactor.h:175
│ │ │ │ -
gtsam::TriangulationFactor::verboseCheirality
bool verboseCheirality() const
return verbosity
Definition TriangulationFactor.h:180
│ │ │ │ -
gtsam::TriangulationFactor::Camera
CAMERA Camera
CAMERA type.
Definition TriangulationFactor.h:38
│ │ │ │ -
gtsam::TriangulationFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition TriangulationFactor.h:115
│ │ │ │ -
gtsam::TriangulationFactor::~TriangulationFactor
~TriangulationFactor() override
Virtual destructor.
Definition TriangulationFactor.h:92
│ │ │ │ -
gtsam::TriangulationFactor::verboseCheirality_
const bool verboseCheirality_
If true, prints text for Cheirality exceptions (default: false)
Definition TriangulationFactor.h:57
│ │ │ │ -
gtsam::TriangulationFactor::access
friend class boost::serialization::access
Serialization function.
Definition TriangulationFactor.h:192
│ │ │ │ -
gtsam::TriangulationFactor::TriangulationFactor
TriangulationFactor()
Default constructor.
Definition TriangulationFactor.h:66
│ │ │ │ -
gtsam::TriangulationFactor::This
TriangulationFactor< CAMERA > This
shorthand for this class
Definition TriangulationFactor.h:46
│ │ │ │ -
gtsam::TriangulationFactor::measured_
const Measurement measured_
2D measurement
Definition TriangulationFactor.h:53
│ │ │ │ -
gtsam::TriangulationFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition TriangulationFactor.h:63
│ │ │ │ -
gtsam::TriangulationFactor::camera_
const CAMERA camera_
CAMERA in which this landmark was seen.
Definition TriangulationFactor.h:52
│ │ │ │ +
gtsam::SmartFactorBase::body_P_sensor_
boost::optional< Pose3 > body_P_sensor_
Pose of the camera in the body frame.
Definition SmartFactorBase.h:82
│ │ │ │ +
gtsam::SmartProjectionParams
Definition SmartFactorParams.h:42
│ │ │ │ +
gtsam::SmartProjectionFactor
SmartProjectionFactor: triangulates point and keeps an estimate of it around.
Definition SmartProjectionFactor.h:45
│ │ │ │ +
gtsam::SmartProjectionFactor< PinholePose< CALIBRATION > >::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, boost::optional< Point3 > externalPoint=boost::none) const
Calculate the error of the factor.
Definition SmartProjectionFactor.h:411
│ │ │ │ +
gtsam::SmartProjectionPoseFactor
If you are using the factor, please cite: L.
Definition SmartProjectionPoseFactor.h:46
│ │ │ │ +
gtsam::SmartProjectionPoseFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition SmartProjectionPoseFactor.h:111
│ │ │ │ +
gtsam::SmartProjectionPoseFactor::SmartProjectionPoseFactor
SmartProjectionPoseFactor(const SharedNoiseModel &sharedNoiseModel, const boost::shared_ptr< CALIBRATION > K, const SmartProjectionParams &params=SmartProjectionParams())
Constructor.
Definition SmartProjectionPoseFactor.h:72
│ │ │ │ +
gtsam::SmartProjectionPoseFactor::cameras
Base::Cameras cameras(const Values &values) const override
Collect all cameras involved in this factor.
Definition SmartProjectionPoseFactor.h:138
│ │ │ │ +
gtsam::SmartProjectionPoseFactor::error
double error(const Values &values) const override
error calculates the error of the factor.
Definition SmartProjectionPoseFactor.h:119
│ │ │ │ +
gtsam::SmartProjectionPoseFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition SmartProjectionPoseFactor.h:59
│ │ │ │ +
gtsam::SmartProjectionPoseFactor::K_
boost::shared_ptr< CALIBRATION > K_
calibration object (one for all cameras)
Definition SmartProjectionPoseFactor.h:54
│ │ │ │ +
gtsam::SmartProjectionPoseFactor::~SmartProjectionPoseFactor
~SmartProjectionPoseFactor() override
Virtual destructor.
Definition SmartProjectionPoseFactor.h:96
│ │ │ │ +
gtsam::SmartProjectionPoseFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition SmartProjectionPoseFactor.h:104
│ │ │ │ +
gtsam::SmartProjectionPoseFactor::calibration
const boost::shared_ptr< CALIBRATION > calibration() const
return calibration shared pointers
Definition SmartProjectionPoseFactor.h:128
│ │ │ │ +
gtsam::SmartProjectionPoseFactor::SmartProjectionPoseFactor
SmartProjectionPoseFactor(const SharedNoiseModel &sharedNoiseModel, const boost::shared_ptr< CALIBRATION > K, const boost::optional< Pose3 > body_P_sensor, const SmartProjectionParams &params=SmartProjectionParams())
Constructor.
Definition SmartProjectionPoseFactor.h:86
│ │ │ │ +
gtsam::SmartProjectionPoseFactor::access
friend class boost::serialization::access
Serialization function.
Definition SmartProjectionPoseFactor.h:152
│ │ │ │ +
gtsam::SmartProjectionPoseFactor::SmartProjectionPoseFactor
SmartProjectionPoseFactor()
Default constructor, only for serialization.
Definition SmartProjectionPoseFactor.h:64
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,189 +1,125 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -TriangulationFactor.h │ │ │ │ │ +SmartProjectionPoseFactor.h │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_._h> │ │ │ │ │ -22#include │ │ │ │ │ -23#include │ │ │ │ │ -24 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ -26 │ │ │ │ │ -32template │ │ │ │ │ -_3_3class _T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -34 │ │ │ │ │ -35public: │ │ │ │ │ -36 │ │ │ │ │ -_3_8 using _C_a_m_e_r_a = CAMERA; │ │ │ │ │ -39 │ │ │ │ │ -40protected: │ │ │ │ │ -41 │ │ │ │ │ -_4_3 using _B_a_s_e = _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_P_o_i_n_t_3_>; │ │ │ │ │ -44 │ │ │ │ │ -_4_6 using _T_h_i_s = _T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_<_C_A_M_E_R_A_>; │ │ │ │ │ -47 │ │ │ │ │ -_4_9 using _M_e_a_s_u_r_e_m_e_n_t = typename CAMERA::Measurement; │ │ │ │ │ -50 │ │ │ │ │ -51 // Keep a copy of measurement and calibration for I/O │ │ │ │ │ -_5_2 const CAMERA _c_a_m_e_r_a__; │ │ │ │ │ -_5_3 const _M_e_a_s_u_r_e_m_e_n_t _m_e_a_s_u_r_e_d__; │ │ │ │ │ -54 │ │ │ │ │ -55 // verbosity handling for Cheirality Exceptions │ │ │ │ │ -_5_6 const bool _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__; │ │ │ │ │ -_5_7 const bool _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__; │ │ │ │ │ -58 │ │ │ │ │ -59public: │ │ │ │ │ -60 EIGEN_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ -61 │ │ │ │ │ -_6_3 using _s_h_a_r_e_d___p_t_r = boost::shared_ptr; │ │ │ │ │ -64 │ │ │ │ │ -_6_6 _T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r() : │ │ │ │ │ -67 _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__(false), _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__(false) { │ │ │ │ │ -68 } │ │ │ │ │ -69 │ │ │ │ │ -_7_9 _T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r(const CAMERA& camera, const _M_e_a_s_u_r_e_m_e_n_t& _m_e_a_s_u_r_e_d, │ │ │ │ │ -80 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model, _K_e_y pointKey, bool _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y = false, │ │ │ │ │ -81 bool _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y = false) : │ │ │ │ │ -82 _B_a_s_e(model, pointKey), _c_a_m_e_r_a__(camera), _m_e_a_s_u_r_e_d__(_m_e_a_s_u_r_e_d), │ │ │ │ │ -_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__( │ │ │ │ │ -83 _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y), _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__(_v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y) { │ │ │ │ │ -84 if (model && model->dim() != _t_r_a_i_t_s_<_M_e_a_s_u_r_e_m_e_n_t_>_:_:_d_i_m_e_n_s_i_o_n) │ │ │ │ │ -85 throw std::invalid_argument( │ │ │ │ │ -86 "TriangulationFactor must be created with " │ │ │ │ │ -87 + boost::lexical_cast((int) _t_r_a_i_t_s_<_M_e_a_s_u_r_e_m_e_n_t_>_:_:_d_i_m_e_n_s_i_o_n) │ │ │ │ │ -88 + "-dimensional noise model."); │ │ │ │ │ -89 } │ │ │ │ │ -90 │ │ │ │ │ -_9_2 _~_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r() override { │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include <_g_t_s_a_m_/_s_l_a_m_/_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_._h> │ │ │ │ │ +23 │ │ │ │ │ +24namespace _g_t_s_a_m { │ │ │ │ │ +44template │ │ │ │ │ +_4_5class _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r │ │ │ │ │ +46 : public _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r > { │ │ │ │ │ +47 private: │ │ │ │ │ +48 typedef _P_i_n_h_o_l_e_P_o_s_e_<_C_A_L_I_B_R_A_T_I_O_N_> Camera; │ │ │ │ │ +49 typedef _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_<_C_a_m_e_r_a_> Base; │ │ │ │ │ +50 typedef _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_<_C_A_L_I_B_R_A_T_I_O_N_> This; │ │ │ │ │ +51 │ │ │ │ │ +52protected: │ │ │ │ │ +53 │ │ │ │ │ +_5_4 boost::shared_ptr _K__; │ │ │ │ │ +55 │ │ │ │ │ +56public: │ │ │ │ │ +57 │ │ │ │ │ +_5_9 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +60 │ │ │ │ │ +_6_4 _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r() {} │ │ │ │ │ +65 │ │ │ │ │ +_7_2 _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r( │ │ │ │ │ +73 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& sharedNoiseModel, │ │ │ │ │ +74 const boost::shared_ptr K, │ │ │ │ │ +75 const _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s& params = _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s()) │ │ │ │ │ +76 : Base(sharedNoiseModel, params), _K__(K) { │ │ │ │ │ +77 } │ │ │ │ │ +78 │ │ │ │ │ +_8_6 _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r( │ │ │ │ │ +87 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& sharedNoiseModel, │ │ │ │ │ +88 const boost::shared_ptr K, │ │ │ │ │ +89 const boost::optional body_P_sensor, │ │ │ │ │ +90 const _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s& params = _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s()) │ │ │ │ │ +91 : _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r(sharedNoiseModel, K, params) { │ │ │ │ │ +92 this->_b_o_d_y___P___s_e_n_s_o_r__ = body_P_sensor; │ │ │ │ │ 93 } │ │ │ │ │ 94 │ │ │ │ │ -_9_6 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -97 return boost::static_pointer_cast( │ │ │ │ │ -98 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); │ │ │ │ │ -99 } │ │ │ │ │ -100 │ │ │ │ │ -_1_0_6 void _p_r_i_n_t(const std::string& s = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ -107 DefaultKeyFormatter) const override { │ │ │ │ │ -108 std::cout << s << "TriangulationFactor,"; │ │ │ │ │ -109 _c_a_m_e_r_a__.print("camera"); │ │ │ │ │ -110 _t_r_a_i_t_s_<_M_e_a_s_u_r_e_m_e_n_t_>_:_:_P_r_i_n_t(_m_e_a_s_u_r_e_d__, "z"); │ │ │ │ │ -111 _B_a_s_e_:_:_p_r_i_n_t("", keyFormatter); │ │ │ │ │ -112 } │ │ │ │ │ -113 │ │ │ │ │ -_1_1_5 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& p, double tol = 1e-9) const override { │ │ │ │ │ -116 const _T_h_i_s *e = dynamic_cast(&p); │ │ │ │ │ -117 return e && _B_a_s_e_:_:_e_q_u_a_l_s(p, tol) && this->camera_.equals(e->camera_, tol) │ │ │ │ │ -118 && _t_r_a_i_t_s_<_M_e_a_s_u_r_e_m_e_n_t_>_:_:_E_q_u_a_l_s(this->measured_, e->measured_, tol); │ │ │ │ │ -119 } │ │ │ │ │ -120 │ │ │ │ │ -_1_2_2 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const _P_o_i_n_t_3& point, boost::optional H2 = │ │ │ │ │ -123 boost::none) const override { │ │ │ │ │ -124 try { │ │ │ │ │ -125 return _t_r_a_i_t_s_<_M_e_a_s_u_r_e_m_e_n_t_>_:_:_L_o_c_a_l(_m_e_a_s_u_r_e_d__, _c_a_m_e_r_a__.project2(point, │ │ │ │ │ -boost::none, H2)); │ │ │ │ │ -126 } catch (_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n& e) { │ │ │ │ │ -127 if (H2) │ │ │ │ │ -128 *H2 = Matrix::Zero(_t_r_a_i_t_s_<_M_e_a_s_u_r_e_m_e_n_t_>_:_:_d_i_m_e_n_s_i_o_n, 3); │ │ │ │ │ -129 if (_v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__) │ │ │ │ │ -130 std::cout << e.what() << ": Landmark " │ │ │ │ │ -131 << DefaultKeyFormatter(this->_k_e_y()) << " moved behind camera" │ │ │ │ │ -132 << std::endl; │ │ │ │ │ -133 if (_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__) │ │ │ │ │ -134 throw e; │ │ │ │ │ -135 return _c_a_m_e_r_a__.defaultErrorWhenTriangulatingBehindCamera(); │ │ │ │ │ -136 } │ │ │ │ │ -137 } │ │ │ │ │ -138 │ │ │ │ │ -_1_4_0 mutable _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x _A_b; │ │ │ │ │ -141 mutable Matrix A; │ │ │ │ │ -142 mutable Vector b; │ │ │ │ │ -143 │ │ │ │ │ -_1_4_9 boost::shared_ptr _l_i_n_e_a_r_i_z_e(const _V_a_l_u_e_s& x) const override │ │ │ │ │ -{ │ │ │ │ │ -150 // Only linearize if the factor is active │ │ │ │ │ -151 if (!this->_a_c_t_i_v_e(x)) │ │ │ │ │ -152 return boost::shared_ptr(); │ │ │ │ │ -153 │ │ │ │ │ -154 // Allocate memory for Jacobian factor, do only once │ │ │ │ │ -155 if (_A_b._r_o_w_s() == 0) { │ │ │ │ │ -156 std::vector dimensions(1, 3); │ │ │ │ │ -157 _A_b = _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x(dimensions, _t_r_a_i_t_s_<_M_e_a_s_u_r_e_m_e_n_t_>_:_:_d_i_m_e_n_s_i_o_n, true); │ │ │ │ │ -158 A.resize(_t_r_a_i_t_s_<_M_e_a_s_u_r_e_m_e_n_t_>_:_:_d_i_m_e_n_s_i_o_n,3); │ │ │ │ │ -159 b.resize(_t_r_a_i_t_s_<_M_e_a_s_u_r_e_m_e_n_t_>_:_:_d_i_m_e_n_s_i_o_n); │ │ │ │ │ -160 } │ │ │ │ │ -161 │ │ │ │ │ -162 // Would be even better if we could pass blocks to project │ │ │ │ │ -163 const _P_o_i_n_t_3& point = x._a_t<_P_o_i_n_t_3>(_k_e_y()); │ │ │ │ │ -164 b = _t_r_a_i_t_s_<_M_e_a_s_u_r_e_m_e_n_t_>_:_:_L_o_c_a_l(_c_a_m_e_r_a__.project2(point, boost::none, A), │ │ │ │ │ -_m_e_a_s_u_r_e_d__); │ │ │ │ │ -165 if (noiseModel_) │ │ │ │ │ -166 this->noiseModel_->WhitenSystem(A, b); │ │ │ │ │ -167 │ │ │ │ │ -168 _A_b(0) = A; │ │ │ │ │ -169 _A_b(1) = b; │ │ │ │ │ -170 │ │ │ │ │ -171 return boost::make_shared(this->_k_e_y_s__, Ab); │ │ │ │ │ -172 } │ │ │ │ │ -173 │ │ │ │ │ -_1_7_5 const _M_e_a_s_u_r_e_m_e_n_t& _m_e_a_s_u_r_e_d() const { │ │ │ │ │ -176 return _m_e_a_s_u_r_e_d__; │ │ │ │ │ -177 } │ │ │ │ │ -178 │ │ │ │ │ -_1_8_0 inline bool _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y() const { │ │ │ │ │ -181 return _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__; │ │ │ │ │ -182 } │ │ │ │ │ -183 │ │ │ │ │ -_1_8_5 inline bool _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y() const { │ │ │ │ │ -186 return _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__; │ │ │ │ │ -187 } │ │ │ │ │ -188 │ │ │ │ │ -189private: │ │ │ │ │ -190 │ │ │ │ │ -_1_9_2 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -193 template │ │ │ │ │ -194 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -195 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -196 ar & BOOST_SERIALIZATION_NVP(_c_a_m_e_r_a__); │ │ │ │ │ -197 ar & BOOST_SERIALIZATION_NVP(_m_e_a_s_u_r_e_d__); │ │ │ │ │ -198 ar & BOOST_SERIALIZATION_NVP(_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__); │ │ │ │ │ -199 ar & BOOST_SERIALIZATION_NVP(_v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__); │ │ │ │ │ -200 } │ │ │ │ │ -201}; │ │ │ │ │ -202} // \ namespace gtsam │ │ │ │ │ -203 │ │ │ │ │ -_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_._h │ │ │ │ │ -Calibrated camera for which only pose is unknown. │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ -Non-linear factor base classes. │ │ │ │ │ +_9_6 _~_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r() override { │ │ │ │ │ +97 } │ │ │ │ │ +98 │ │ │ │ │ +_1_0_4 void _p_r_i_n_t(const std::string& s = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ +105 DefaultKeyFormatter) const override { │ │ │ │ │ +106 std::cout << s << "SmartProjectionPoseFactor, z = \n "; │ │ │ │ │ +107 Base::print("", keyFormatter); │ │ │ │ │ +108 } │ │ │ │ │ +109 │ │ │ │ │ +_1_1_1 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& p, double tol = 1e-9) const override { │ │ │ │ │ +112 const This *e = dynamic_cast(&p); │ │ │ │ │ +113 return e && Base::equals(p, tol); │ │ │ │ │ +114 } │ │ │ │ │ +115 │ │ │ │ │ +_1_1_9 double _e_r_r_o_r(const _V_a_l_u_e_s& values) const override { │ │ │ │ │ +120 if (this->_a_c_t_i_v_e(values)) { │ │ │ │ │ +121 return this->_t_o_t_a_l_R_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r(_c_a_m_e_r_a_s(values)); │ │ │ │ │ +122 } else { // else of active flag │ │ │ │ │ +123 return 0.0; │ │ │ │ │ +124 } │ │ │ │ │ +125 } │ │ │ │ │ +126 │ │ │ │ │ +_1_2_8 inline const boost::shared_ptr _c_a_l_i_b_r_a_t_i_o_n() const { │ │ │ │ │ +129 return _K__; │ │ │ │ │ +130 } │ │ │ │ │ +131 │ │ │ │ │ +_1_3_8 typename Base::Cameras _c_a_m_e_r_a_s(const _V_a_l_u_e_s& values) const override { │ │ │ │ │ +139 typename Base::Cameras _c_a_m_e_r_a_s; │ │ │ │ │ +140 for (const _K_e_y& k : this->_k_e_y_s__) { │ │ │ │ │ +141 const _P_o_s_e_3 world_P_sensor_k = │ │ │ │ │ +142 Base::body_P_sensor_ ? values._a_t<_P_o_s_e_3>(k) * *Base::body_P_sensor_ │ │ │ │ │ +143 : values._a_t<_P_o_s_e_3>(k); │ │ │ │ │ +144 _c_a_m_e_r_a_s.emplace_back(world_P_sensor_k, _K__); │ │ │ │ │ +145 } │ │ │ │ │ +146 return _c_a_m_e_r_a_s; │ │ │ │ │ +147 } │ │ │ │ │ +148 │ │ │ │ │ +149 private: │ │ │ │ │ +150 │ │ │ │ │ +_1_5_2 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +153 template │ │ │ │ │ +154 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +155 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); │ │ │ │ │ +156 ar & BOOST_SERIALIZATION_NVP(_K__); │ │ │ │ │ +157 } │ │ │ │ │ +158}; │ │ │ │ │ +159// end of class declaration │ │ │ │ │ +160 │ │ │ │ │ +162template │ │ │ │ │ +_1_6_3struct _t_r_a_i_t_s<_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r > : public _T_e_s_t_a_b_l_e< │ │ │ │ │ +164 SmartProjectionPoseFactor > { │ │ │ │ │ +165}; │ │ │ │ │ +166 │ │ │ │ │ +167} // \ namespace gtsam │ │ │ │ │ +_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_._h │ │ │ │ │ +Smart factor on cameras (pose + calibration) │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ -Vector3 Point3 │ │ │ │ │ -As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point3 to Vector3... │ │ │ │ │ -DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ _g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ Aliases. │ │ │ │ │ DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ @@ -192,147 +128,109 @@ │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ -of vertical blocks. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_r_o_w_s │ │ │ │ │ -DenseIndex rows() const │ │ │ │ │ -Row size. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:115 │ │ │ │ │ -_g_t_s_a_m_:_:_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e │ │ │ │ │ +A pinhole camera class that has a Pose3 and a fixed Calibration. │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:243 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ +A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ _g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s__ │ │ │ │ │ KeyVector keys_ │ │ │ │ │ The keys involved in this factor. │ │ │ │ │ DDeeffiinniittiioonn Factor.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ -virtual void print(const std::string &s="Factor", const KeyFormatter │ │ │ │ │ -&formatter=DefaultKeyFormatter) const │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn Factor.cpp:29 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const This &other, double tol=1e-9) const │ │ │ │ │ -check equality │ │ │ │ │ -DDeeffiinniittiioonn Factor.cpp:42 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ Nonlinear factor base class. │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_:_:_a_c_t_i_v_e │ │ │ │ │ virtual bool active(const Values &) const │ │ │ │ │ Checks whether a factor should be used based on a set of values. │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactor.h:118 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ -A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ -variables. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_ _P_o_i_n_t_3_ _>_:_:_k_e_y │ │ │ │ │ -Key key() const │ │ │ │ │ -Returns a key. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:518 │ │ │ │ │ _g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ _g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_a_t │ │ │ │ │ const ValueType at(Key j) const │ │ │ │ │ Retrieve a variable by key j. │ │ │ │ │ DDeeffiinniittiioonn Values-inl.h:361 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -Non-linear factor for a constraint derived from a 2D measurement. │ │ │ │ │ -DDeeffiinniittiioonn TriangulationFactor.h:33 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_A_b │ │ │ │ │ -VerticalBlockMatrix Ab │ │ │ │ │ -thread-safe (?) scratch memory for linearize │ │ │ │ │ -DDeeffiinniittiioonn TriangulationFactor.h:140 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__ │ │ │ │ │ -const bool throwCheirality_ │ │ │ │ │ -If true, rethrows Cheirality exceptions (default: false) │ │ │ │ │ -DDeeffiinniittiioonn TriangulationFactor.h:56 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y │ │ │ │ │ -bool throwCheirality() const │ │ │ │ │ -return flag for throwing cheirality exceptions │ │ │ │ │ -DDeeffiinniittiioonn TriangulationFactor.h:185 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn TriangulationFactor.h:106 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ -Vector evaluateError(const Point3 &point, boost::optional< Matrix & > │ │ │ │ │ -H2=boost::none) const override │ │ │ │ │ -Evaluate error h(x)-z and optionally derivatives. │ │ │ │ │ -DDeeffiinniittiioonn TriangulationFactor.h:122 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ -gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn TriangulationFactor.h:96 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e │ │ │ │ │ -boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override │ │ │ │ │ -Linearize to a JacobianFactor, does not support constrained noise model ! Hence │ │ │ │ │ -. │ │ │ │ │ -DDeeffiinniittiioonn TriangulationFactor.h:149 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -TriangulationFactor(const CAMERA &camera, const Measurement &measured, const │ │ │ │ │ -SharedNoiseModel &model, Key pointKey, bool throwCheirality=false, bool │ │ │ │ │ -verboseCheirality=false) │ │ │ │ │ -Constructor with exception-handling flags. │ │ │ │ │ -DDeeffiinniittiioonn TriangulationFactor.h:79 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_M_e_a_s_u_r_e_m_e_n_t │ │ │ │ │ -typename CAMERA::Measurement Measurement │ │ │ │ │ -shorthand for measurement type, e.g. Point2 or StereoPoint2 │ │ │ │ │ -DDeeffiinniittiioonn TriangulationFactor.h:49 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_m_e_a_s_u_r_e_d │ │ │ │ │ -const Measurement & measured() const │ │ │ │ │ -return the measurement │ │ │ │ │ -DDeeffiinniittiioonn TriangulationFactor.h:175 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y │ │ │ │ │ -bool verboseCheirality() const │ │ │ │ │ -return verbosity │ │ │ │ │ -DDeeffiinniittiioonn TriangulationFactor.h:180 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_C_a_m_e_r_a │ │ │ │ │ -CAMERA Camera │ │ │ │ │ -CAMERA type. │ │ │ │ │ -DDeeffiinniittiioonn TriangulationFactor.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_b_o_d_y___P___s_e_n_s_o_r__ │ │ │ │ │ +boost::optional< Pose3 > body_P_sensor_ │ │ │ │ │ +Pose of the camera in the body frame. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:82 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorParams.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +SmartProjectionFactor: triangulates point and keeps an estimate of it around. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_<_ _P_i_n_h_o_l_e_P_o_s_e_<_ _C_A_L_I_B_R_A_T_I_O_N_ _>_ _>_:_: │ │ │ │ │ +_t_o_t_a_l_R_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r │ │ │ │ │ +double totalReprojectionError(const Cameras &cameras, boost::optional< Point3 > │ │ │ │ │ +externalPoint=boost::none) const │ │ │ │ │ +Calculate the error of the factor. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:411 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r │ │ │ │ │ +If you are using the factor, please cite: L. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionPoseFactor.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ bool equals(const NonlinearFactor &p, double tol=1e-9) const override │ │ │ │ │ equals │ │ │ │ │ -DDeeffiinniittiioonn TriangulationFactor.h:115 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_~_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -~TriangulationFactor() override │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionPoseFactor.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r │ │ │ │ │ +SmartProjectionPoseFactor(const SharedNoiseModel &sharedNoiseModel, const │ │ │ │ │ +boost::shared_ptr< CALIBRATION > K, const SmartProjectionParams │ │ │ │ │ +¶ms=SmartProjectionParams()) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionPoseFactor.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_:_:_c_a_m_e_r_a_s │ │ │ │ │ +Base::Cameras cameras(const Values &values) const override │ │ │ │ │ +Collect all cameras involved in this factor. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionPoseFactor.h:138 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_:_:_e_r_r_o_r │ │ │ │ │ +double error(const Values &values) const override │ │ │ │ │ +error calculates the error of the factor. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionPoseFactor.h:119 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shorthand for a smart pointer to a factor │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionPoseFactor.h:59 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_:_:_K__ │ │ │ │ │ +boost::shared_ptr< CALIBRATION > K_ │ │ │ │ │ +calibration object (one for all cameras) │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionPoseFactor.h:54 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_:_:_~_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r │ │ │ │ │ +~SmartProjectionPoseFactor() override │ │ │ │ │ Virtual destructor. │ │ │ │ │ -DDeeffiinniittiioonn TriangulationFactor.h:92 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__ │ │ │ │ │ -const bool verboseCheirality_ │ │ │ │ │ -If true, prints text for Cheirality exceptions (default: false) │ │ │ │ │ -DDeeffiinniittiioonn TriangulationFactor.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionPoseFactor.h:96 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionPoseFactor.h:104 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_:_:_c_a_l_i_b_r_a_t_i_o_n │ │ │ │ │ +const boost::shared_ptr< CALIBRATION > calibration() const │ │ │ │ │ +return calibration shared pointers │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionPoseFactor.h:128 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r │ │ │ │ │ +SmartProjectionPoseFactor(const SharedNoiseModel &sharedNoiseModel, const │ │ │ │ │ +boost::shared_ptr< CALIBRATION > K, const boost::optional< Pose3 > │ │ │ │ │ +body_P_sensor, const SmartProjectionParams ¶ms=SmartProjectionParams()) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionPoseFactor.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ friend class boost::serialization::access │ │ │ │ │ Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn TriangulationFactor.h:192 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -TriangulationFactor() │ │ │ │ │ -Default constructor. │ │ │ │ │ -DDeeffiinniittiioonn TriangulationFactor.h:66 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_T_h_i_s │ │ │ │ │ -TriangulationFactor< CAMERA > This │ │ │ │ │ -shorthand for this class │ │ │ │ │ -DDeeffiinniittiioonn TriangulationFactor.h:46 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_m_e_a_s_u_r_e_d__ │ │ │ │ │ -const Measurement measured_ │ │ │ │ │ -2D measurement │ │ │ │ │ -DDeeffiinniittiioonn TriangulationFactor.h:53 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shorthand for a smart pointer to a factor │ │ │ │ │ -DDeeffiinniittiioonn TriangulationFactor.h:63 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_:_:_c_a_m_e_r_a__ │ │ │ │ │ -const CAMERA camera_ │ │ │ │ │ -CAMERA in which this landmark was seen. │ │ │ │ │ -DDeeffiinniittiioonn TriangulationFactor.h:52 │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionPoseFactor.h:152 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r │ │ │ │ │ +SmartProjectionPoseFactor() │ │ │ │ │ +Default constructor, only for serialization. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionPoseFactor.h:64 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * TTrriiaanngguullaattiioonnFFaaccttoorr..hh │ │ │ │ │ + * _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01325_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/PoseRotationPrior.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/KarcherMeanFactor-inl.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,146 +98,111 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
PoseRotationPrior.h
│ │ │ │ +
KarcherMeanFactor-inl.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1
│ │ │ │ -
10#pragma once
│ │ │ │ +
1/* ----------------------------------------------------------------------------
│ │ │ │ +
2
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
4 * Atlanta, Georgia 30332-0415
│ │ │ │ +
5 * All Rights Reserved
│ │ │ │ +
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
7
│ │ │ │ +
8 * See LICENSE for the license information
│ │ │ │ +
9
│ │ │ │ +
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
12#include <gtsam/geometry/concepts.h>
│ │ │ │ -
13#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
14
│ │ │ │ -
15
│ │ │ │ -
16namespace gtsam {
│ │ │ │ +
12/*
│ │ │ │ +
13 * @file KarcherMeanFactor.cpp
│ │ │ │ +
14 * @author Frank Dellaert
│ │ │ │ +
15 * @date March 2019
│ │ │ │ +
16 */
│ │ │ │
17
│ │ │ │ -
18template<class POSE>
│ │ │ │ -
│ │ │ │ -
19class PoseRotationPrior : public NoiseModelFactorN<POSE> {
│ │ │ │ -
20public:
│ │ │ │ -
21
│ │ │ │ -
22 typedef PoseRotationPrior<POSE> This;
│ │ │ │ -
23 typedef NoiseModelFactorN<POSE> Base;
│ │ │ │ -
24 typedef POSE Pose;
│ │ │ │ -
25 typedef typename POSE::Translation Translation;
│ │ │ │ -
26 typedef typename POSE::Rotation Rotation;
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
│ │ │ │ +
21#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ +
22#include <gtsam/slam/KarcherMeanFactor.h>
│ │ │ │ +
23
│ │ │ │ +
24using namespace std;
│ │ │ │ +
25
│ │ │ │ +
26namespace gtsam {
│ │ │ │
27
│ │ │ │ -
28 GTSAM_CONCEPT_POSE_TYPE(Pose)
│ │ │ │ -
29 GTSAM_CONCEPT_GROUP_TYPE(Pose)
│ │ │ │ -
30 GTSAM_CONCEPT_LIE_TYPE(Rotation)
│ │ │ │ -
31
│ │ │ │ -
32 // Get dimensions of pose and rotation type at compile time
│ │ │ │ -
33 static const int xDim = FixedDimension<POSE>::value;
│ │ │ │ -
34 static const int rDim = FixedDimension<typename POSE::Rotation>::value;
│ │ │ │ -
35
│ │ │ │ -
36protected:
│ │ │ │ -
37
│ │ │ │ -
38 Rotation measured_;
│ │ │ │ -
39
│ │ │ │ -
40public:
│ │ │ │ -
41
│ │ │ │ -
43 PoseRotationPrior() {}
│ │ │ │ -
44
│ │ │ │ -
│ │ │ │ -
46 PoseRotationPrior(Key key, const Rotation& rot_z, const SharedNoiseModel& model)
│ │ │ │ -
47 : Base(model, key), measured_(rot_z) {}
│ │ │ │ -
│ │ │ │ -
48
│ │ │ │ -
│ │ │ │ -
50 PoseRotationPrior(Key key, const POSE& pose_z, const SharedNoiseModel& model)
│ │ │ │ -
51 : Base(model, key), measured_(pose_z.rotation()) {}
│ │ │ │ +
28template <class T, class ALLOC>
│ │ │ │ +
29T FindKarcherMeanImpl(const vector<T, ALLOC>& rotations) {
│ │ │ │ +
30 // Cost function C(R) = \sum PriorFactor(R_i)::error(R)
│ │ │ │ +
31 // No closed form solution.
│ │ │ │ +
32 NonlinearFactorGraph graph;
│ │ │ │ +
33 static const Key kKey(0);
│ │ │ │ +
34 for (const auto& R : rotations) {
│ │ │ │ +
35 graph.addPrior<T>(kKey, R);
│ │ │ │ +
36 }
│ │ │ │ +
37 Values initial;
│ │ │ │ +
38 initial.insert<T>(kKey, T());
│ │ │ │ +
39 auto result = GaussNewtonOptimizer(graph, initial).optimize();
│ │ │ │ +
40 return result.at<T>(kKey);
│ │ │ │ +
41}
│ │ │ │ +
42
│ │ │ │ +
43template <class T>
│ │ │ │ +
44T FindKarcherMean(const std::vector<T>& rotations) {
│ │ │ │ +
45 return FindKarcherMeanImpl(rotations);
│ │ │ │ +
46}
│ │ │ │ +
47
│ │ │ │ +
48template <class T>
│ │ │ │ +
│ │ │ │ +
49T FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>>& rotations) {
│ │ │ │ +
50 return FindKarcherMeanImpl(rotations);
│ │ │ │ +
51}
│ │ │ │
│ │ │ │
52
│ │ │ │ -
53 ~PoseRotationPrior() override {}
│ │ │ │ -
54
│ │ │ │ -
│ │ │ │ -
56 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
57 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ -
58 gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
│ │ │ │ -
│ │ │ │ -
59
│ │ │ │ -
60 // access
│ │ │ │ -
61 const Rotation& measured() const { return measured_; }
│ │ │ │ -
62
│ │ │ │ -
63 // testable
│ │ │ │ -
64
│ │ │ │ -
│ │ │ │ -
66 bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
│ │ │ │ -
67 const This *e = dynamic_cast<const This*> (&expected);
│ │ │ │ -
68 return e != nullptr && Base::equals(*e, tol) && measured_.equals(e->measured_, tol);
│ │ │ │ -
69 }
│ │ │ │ -
│ │ │ │ -
70
│ │ │ │ -
│ │ │ │ -
72 void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ -
73 Base::print(s + "PoseRotationPrior", keyFormatter);
│ │ │ │ -
74 measured_.print("Measured Rotation");
│ │ │ │ -
75 }
│ │ │ │ -
│ │ │ │ -
76
│ │ │ │ -
│ │ │ │ -
78 Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const override {
│ │ │ │ -
79 const Rotation& newR = pose.rotation();
│ │ │ │ -
80 if (H) {
│ │ │ │ -
81 *H = Matrix::Zero(rDim, xDim);
│ │ │ │ -
82 std::pair<size_t, size_t> rotInterval = POSE::rotationInterval();
│ │ │ │ -
83 (*H).middleCols(rotInterval.first, rDim).setIdentity(rDim, rDim);
│ │ │ │ -
84 }
│ │ │ │ -
85
│ │ │ │ -
86 return measured_.localCoordinates(newR);
│ │ │ │ -
87 }
│ │ │ │ -
│ │ │ │ -
88
│ │ │ │ -
89private:
│ │ │ │ -
90
│ │ │ │ -
92 friend class boost::serialization::access;
│ │ │ │ -
93 template<class ARCHIVE>
│ │ │ │ -
94 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
95 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ -
96 ar & boost::serialization::make_nvp("NoiseModelFactor1",
│ │ │ │ -
97 boost::serialization::base_object<Base>(*this));
│ │ │ │ -
98 ar & BOOST_SERIALIZATION_NVP(measured_);
│ │ │ │ -
99 }
│ │ │ │ -
100};
│ │ │ │ -
│ │ │ │ -
101
│ │ │ │ -
102} // \namespace gtsam
│ │ │ │ -
103
│ │ │ │ -
104
│ │ │ │ -
105
│ │ │ │ -
106
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
53template <class T>
│ │ │ │ +
54T FindKarcherMean(std::initializer_list<T>&& rotations) {
│ │ │ │ +
55 return FindKarcherMeanImpl(std::vector<T, Eigen::aligned_allocator<T> >(rotations));
│ │ │ │ +
56}
│ │ │ │ +
57
│ │ │ │ +
58template <class T>
│ │ │ │ +
59template <typename CONTAINER>
│ │ │ │ +
│ │ │ │ +
60KarcherMeanFactor<T>::KarcherMeanFactor(const CONTAINER &keys, int d,
│ │ │ │ +
61 boost::optional<double> beta)
│ │ │ │ +
62 : NonlinearFactor(keys), d_(static_cast<size_t>(d)) {
│ │ │ │ +
63 if (d <= 0) {
│ │ │ │ +
64 throw std::invalid_argument(
│ │ │ │ +
65 "KarcherMeanFactor needs dimension for dynamic types.");
│ │ │ │ +
66 }
│ │ │ │ +
67 // Create the constant Jacobian made of d*d identity matrices,
│ │ │ │ +
68 // where d is the dimensionality of the manifold.
│ │ │ │ +
69 Matrix A = Matrix::Identity(d, d);
│ │ │ │ +
70 if (beta) A *= std::sqrt(*beta);
│ │ │ │ +
71 std::map<Key, Matrix> terms;
│ │ │ │ +
72 for (Key j : keys) {
│ │ │ │ +
73 terms[j] = A;
│ │ │ │ +
74 }
│ │ │ │ +
75 whitenedJacobian_ =
│ │ │ │ +
76 boost::make_shared<JacobianFactor>(terms, Vector::Zero(d));
│ │ │ │ +
77}
│ │ │ │ +
│ │ │ │ +
78} // namespace gtsam
│ │ │ │ + │ │ │ │ +
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::FixedDimension
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition Manifold.h:164
│ │ │ │ -
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
│ │ │ │ -
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
│ │ │ │ +
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
│ │ │ │
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ -
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ -
gtsam::NoiseModelFactorN< POSE >::key
Key key() const
Returns a key.
Definition NonlinearFactor.h:518
│ │ │ │ -
gtsam::PoseRotationPrior
Definition PoseRotationPrior.h:19
│ │ │ │ -
gtsam::PoseRotationPrior::PoseRotationPrior
PoseRotationPrior(Key key, const Rotation &rot_z, const SharedNoiseModel &model)
standard constructor
Definition PoseRotationPrior.h:46
│ │ │ │ -
gtsam::PoseRotationPrior::PoseRotationPrior
PoseRotationPrior(Key key, const POSE &pose_z, const SharedNoiseModel &model)
Constructor that pulls the translation from an incoming POSE.
Definition PoseRotationPrior.h:50
│ │ │ │ -
gtsam::PoseRotationPrior::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition PoseRotationPrior.h:56
│ │ │ │ -
gtsam::PoseRotationPrior::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
equals specialized to this factor
Definition PoseRotationPrior.h:66
│ │ │ │ -
gtsam::PoseRotationPrior::PoseRotationPrior
PoseRotationPrior()
default constructor - only use for serialization
Definition PoseRotationPrior.h:43
│ │ │ │ -
gtsam::PoseRotationPrior::evaluateError
Vector evaluateError(const Pose &pose, boost::optional< Matrix & > H=boost::none) const override
h(x)-z
Definition PoseRotationPrior.h:78
│ │ │ │ -
gtsam::PoseRotationPrior::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print contents
Definition PoseRotationPrior.h:72
│ │ │ │ -
gtsam::PoseRotationPrior::access
friend class boost::serialization::access
Serialization function.
Definition PoseRotationPrior.h:92
│ │ │ │ -
concepts.h
Concept-checking macros for geometric objects Each macro instantiates a concept check structure,...
│ │ │ │ +
gtsam::KarcherMeanFactor::KarcherMeanFactor
KarcherMeanFactor(const CONTAINER &keys, int d=D, boost::optional< double > beta=boost::none)
Construct from given keys.
Definition KarcherMeanFactor-inl.h:60
│ │ │ │ +
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,186 +1,117 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -PoseRotationPrior.h │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -1 │ │ │ │ │ -10#pragma once │ │ │ │ │ +KarcherMeanFactor-inl.h │ │ │ │ │ +1/* --------------------------------------------------------------------------- │ │ │ │ │ +- │ │ │ │ │ +2 │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +5 * All Rights Reserved │ │ │ │ │ +6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +7 │ │ │ │ │ +8 * See LICENSE for the license information │ │ │ │ │ +9 │ │ │ │ │ +10 * ------------------------------------------------------------------------- │ │ │ │ │ +- */ │ │ │ │ │ 11 │ │ │ │ │ -12#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_c_o_n_c_e_p_t_s_._h> │ │ │ │ │ -13#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -14 │ │ │ │ │ -15 │ │ │ │ │ -16namespace _g_t_s_a_m { │ │ │ │ │ +12/* │ │ │ │ │ +13 * @file KarcherMeanFactor.cpp │ │ │ │ │ +14 * @author Frank Dellaert │ │ │ │ │ +15 * @date March 2019 │ │ │ │ │ +16 */ │ │ │ │ │ 17 │ │ │ │ │ -18template │ │ │ │ │ -_1_9class _P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -20public: │ │ │ │ │ -21 │ │ │ │ │ -22 typedef _P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r_<_P_O_S_E_> This; │ │ │ │ │ -23 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_P_O_S_E_> _B_a_s_e; │ │ │ │ │ -24 typedef POSE Pose; │ │ │ │ │ -25 typedef typename POSE::Translation Translation; │ │ │ │ │ -26 typedef typename POSE::Rotation Rotation; │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +22#include │ │ │ │ │ +23 │ │ │ │ │ +24using namespace std; │ │ │ │ │ +25 │ │ │ │ │ +26namespace _g_t_s_a_m { │ │ │ │ │ 27 │ │ │ │ │ -28 GTSAM_CONCEPT_POSE_TYPE(Pose) │ │ │ │ │ -29 GTSAM_CONCEPT_GROUP_TYPE(Pose) │ │ │ │ │ -30 GTSAM_CONCEPT_LIE_TYPE(Rotation) │ │ │ │ │ -31 │ │ │ │ │ -32 // Get dimensions of pose and rotation type at compile time │ │ │ │ │ -33 static const int xDim = _F_i_x_e_d_D_i_m_e_n_s_i_o_n_<_P_O_S_E_>_:_:_v_a_l_u_e; │ │ │ │ │ -34 static const int rDim = _F_i_x_e_d_D_i_m_e_n_s_i_o_n_<_t_y_p_e_n_a_m_e_ _P_O_S_E_:_:_R_o_t_a_t_i_o_n_>_:_:_v_a_l_u_e; │ │ │ │ │ -35 │ │ │ │ │ -36protected: │ │ │ │ │ -37 │ │ │ │ │ -38 Rotation measured_; │ │ │ │ │ -39 │ │ │ │ │ -40public: │ │ │ │ │ -41 │ │ │ │ │ -_4_3 _P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r() {} │ │ │ │ │ -44 │ │ │ │ │ -_4_6 _P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r(_K_e_y _k_e_y, const Rotation& rot_z, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& │ │ │ │ │ -model) │ │ │ │ │ -47 : _B_a_s_e(model, _k_e_y), measured_(rot_z) {} │ │ │ │ │ -48 │ │ │ │ │ -_5_0 _P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r(_K_e_y _k_e_y, const POSE& pose_z, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& │ │ │ │ │ -model) │ │ │ │ │ -51 : _B_a_s_e(model, _k_e_y), measured_(pose_z.rotation()) {} │ │ │ │ │ +28template │ │ │ │ │ +29T FindKarcherMeanImpl(const vector& rotations) { │ │ │ │ │ +30 // Cost function C(R) = \sum PriorFactor(R_i)::error(R) │ │ │ │ │ +31 // No closed form solution. │ │ │ │ │ +32 NonlinearFactorGraph graph; │ │ │ │ │ +33 static const _K_e_y kKey(0); │ │ │ │ │ +34 for (const auto& R : rotations) { │ │ │ │ │ +35 graph.addPrior(kKey, R); │ │ │ │ │ +36 } │ │ │ │ │ +37 _V_a_l_u_e_s initial; │ │ │ │ │ +38 initial.insert(kKey, T()); │ │ │ │ │ +39 auto result = GaussNewtonOptimizer(graph, initial).optimize(); │ │ │ │ │ +40 return result.at(kKey); │ │ │ │ │ +41} │ │ │ │ │ +42 │ │ │ │ │ +43template │ │ │ │ │ +44T FindKarcherMean(const std::vector& rotations) { │ │ │ │ │ +45 return FindKarcherMeanImpl(rotations); │ │ │ │ │ +46} │ │ │ │ │ +47 │ │ │ │ │ +48template │ │ │ │ │ +_4_9T FindKarcherMean(const std::vector>& │ │ │ │ │ +rotations) { │ │ │ │ │ +50 return FindKarcherMeanImpl(rotations); │ │ │ │ │ +51} │ │ │ │ │ 52 │ │ │ │ │ -53 _~_P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r() override {} │ │ │ │ │ -54 │ │ │ │ │ -_5_6 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -57 return boost::static_pointer_cast( │ │ │ │ │ -58 gtsam::NonlinearFactor::shared_ptr(new This(*this))); } │ │ │ │ │ -59 │ │ │ │ │ -60 // access │ │ │ │ │ -61 const Rotation& measured() const { return measured_; } │ │ │ │ │ -62 │ │ │ │ │ -63 // testable │ │ │ │ │ -64 │ │ │ │ │ -_6_6 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& expected, double tol=1e-9) const override │ │ │ │ │ -{ │ │ │ │ │ -67 const This *e = dynamic_cast (&expected); │ │ │ │ │ -68 return e != nullptr && _B_a_s_e_:_:_e_q_u_a_l_s(*e, tol) && measured_.equals(e- │ │ │ │ │ ->measured_, tol); │ │ │ │ │ -69 } │ │ │ │ │ -70 │ │ │ │ │ -_7_2 void _p_r_i_n_t(const std::string& s="", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ -DefaultKeyFormatter) const override { │ │ │ │ │ -73 _B_a_s_e_:_:_p_r_i_n_t(s + "PoseRotationPrior", keyFormatter); │ │ │ │ │ -74 measured_.print("Measured Rotation"); │ │ │ │ │ -75 } │ │ │ │ │ -76 │ │ │ │ │ -_7_8 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const Pose& pose, boost::optional H = boost:: │ │ │ │ │ -none) const override { │ │ │ │ │ -79 const Rotation& newR = pose.rotation(); │ │ │ │ │ -80 if (H) { │ │ │ │ │ -81 *H = Matrix::Zero(rDim, xDim); │ │ │ │ │ -82 std::pair rotInterval = POSE::rotationInterval(); │ │ │ │ │ -83 (*H).middleCols(rotInterval.first, rDim).setIdentity(rDim, rDim); │ │ │ │ │ -84 } │ │ │ │ │ -85 │ │ │ │ │ -86 return measured_.localCoordinates(newR); │ │ │ │ │ -87 } │ │ │ │ │ -88 │ │ │ │ │ -89private: │ │ │ │ │ -90 │ │ │ │ │ -_9_2 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -93 template │ │ │ │ │ -94 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -95 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility │ │ │ │ │ -96 ar & boost::serialization::make_nvp("NoiseModelFactor1", │ │ │ │ │ -97 boost::serialization::base_object(*this)); │ │ │ │ │ -98 ar & BOOST_SERIALIZATION_NVP(measured_); │ │ │ │ │ -99 } │ │ │ │ │ -100}; │ │ │ │ │ -101 │ │ │ │ │ -102} // \namespace gtsam │ │ │ │ │ -103 │ │ │ │ │ -104 │ │ │ │ │ -105 │ │ │ │ │ -106 │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ -Non-linear factor base classes. │ │ │ │ │ +53template │ │ │ │ │ +54T FindKarcherMean(std::initializer_list&& rotations) { │ │ │ │ │ +55 return FindKarcherMeanImpl(std::vector > │ │ │ │ │ +(rotations)); │ │ │ │ │ +56} │ │ │ │ │ +57 │ │ │ │ │ +58template │ │ │ │ │ +59template │ │ │ │ │ +_6_0_K_a_r_c_h_e_r_M_e_a_n_F_a_c_t_o_r_<_T_>_:_:_K_a_r_c_h_e_r_M_e_a_n_F_a_c_t_o_r(const CONTAINER &keys, int d, │ │ │ │ │ +61 boost::optional beta) │ │ │ │ │ +62 : _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r(keys), d_(static_cast(d)) { │ │ │ │ │ +63 if (d <= 0) { │ │ │ │ │ +64 throw std::invalid_argument( │ │ │ │ │ +65 "KarcherMeanFactor needs dimension for dynamic types."); │ │ │ │ │ +66 } │ │ │ │ │ +67 // Create the constant Jacobian made of d*d identity matrices, │ │ │ │ │ +68 // where d is the dimensionality of the manifold. │ │ │ │ │ +69 Matrix A = Matrix::Identity(d, d); │ │ │ │ │ +70 if (beta) A *= std::sqrt(*beta); │ │ │ │ │ +71 std::map terms; │ │ │ │ │ +72 for (_K_e_y j : _k_e_y_s) { │ │ │ │ │ +73 terms[j] = A; │ │ │ │ │ +74 } │ │ │ │ │ +75 whitenedJacobian_ = │ │ │ │ │ +76 boost::make_shared(terms, Vector::Zero(d)); │ │ │ │ │ +77} │ │ │ │ │ +78} // namespace gtsam │ │ │ │ │ +_G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Factor Graph consisting of non-linear factors. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ -Aliases. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_F_i_x_e_d_D_i_m_e_n_s_i_o_n │ │ │ │ │ -Give fixed size dimension of a type, fails at compile time if dynamic. │ │ │ │ │ -DDeeffiinniittiioonn Manifold.h:164 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ -virtual void print(const std::string &s="Factor", const KeyFormatter │ │ │ │ │ -&formatter=DefaultKeyFormatter) const │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn Factor.cpp:29 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const This &other, double tol=1e-9) const │ │ │ │ │ -check equality │ │ │ │ │ -DDeeffiinniittiioonn Factor.cpp:42 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s │ │ │ │ │ +const KeyVector & keys() const │ │ │ │ │ +Access the factor's involved variable keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:140 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ Nonlinear factor base class. │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ -A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ -variables. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_ _P_O_S_E_ _>_:_:_k_e_y │ │ │ │ │ -Key key() const │ │ │ │ │ -Returns a key. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:518 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r │ │ │ │ │ -DDeeffiinniittiioonn PoseRotationPrior.h:19 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r_:_:_P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r │ │ │ │ │ -PoseRotationPrior(Key key, const Rotation &rot_z, const SharedNoiseModel │ │ │ │ │ -&model) │ │ │ │ │ -standard constructor │ │ │ │ │ -DDeeffiinniittiioonn PoseRotationPrior.h:46 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r_:_:_P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r │ │ │ │ │ -PoseRotationPrior(Key key, const POSE &pose_z, const SharedNoiseModel &model) │ │ │ │ │ -Constructor that pulls the translation from an incoming POSE. │ │ │ │ │ -DDeeffiinniittiioonn PoseRotationPrior.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r_:_:_c_l_o_n_e │ │ │ │ │ -gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn PoseRotationPrior.h:56 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const NonlinearFactor &expected, double tol=1e-9) const override │ │ │ │ │ -equals specialized to this factor │ │ │ │ │ -DDeeffiinniittiioonn PoseRotationPrior.h:66 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r_:_:_P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r │ │ │ │ │ -PoseRotationPrior() │ │ │ │ │ -default constructor - only use for serialization │ │ │ │ │ -DDeeffiinniittiioonn PoseRotationPrior.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ -Vector evaluateError(const Pose &pose, boost::optional< Matrix & > H=boost:: │ │ │ │ │ -none) const override │ │ │ │ │ -h(x)-z │ │ │ │ │ -DDeeffiinniittiioonn PoseRotationPrior.h:78 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -print contents │ │ │ │ │ -DDeeffiinniittiioonn PoseRotationPrior.h:72 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn PoseRotationPrior.h:92 │ │ │ │ │ -_c_o_n_c_e_p_t_s_._h │ │ │ │ │ -Concept-checking macros for geometric objects Each macro instantiates a concept │ │ │ │ │ -check structure,... │ │ │ │ │ +_g_t_s_a_m_:_:_K_a_r_c_h_e_r_M_e_a_n_F_a_c_t_o_r_:_:_K_a_r_c_h_e_r_M_e_a_n_F_a_c_t_o_r │ │ │ │ │ +KarcherMeanFactor(const CONTAINER &keys, int d=D, boost::optional< double > │ │ │ │ │ +beta=boost::none) │ │ │ │ │ +Construct from given keys. │ │ │ │ │ +DDeeffiinniittiioonn KarcherMeanFactor-inl.h:60 │ │ │ │ │ +_V_a_l_u_e_s │ │ │ │ │ +In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ +a non-linear function... │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r_._h │ │ │ │ │ + * KKaarrcchheerrMMeeaannFFaaccttoorr--iinnll..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01328.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/InitializePose.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/SmartProjectionRigFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,38 +94,54 @@ │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
InitializePose.h File Reference
│ │ │ │ +
SmartProjectionRigFactor.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

common code between lago.* (2D) and InitializePose3.* (3D) │ │ │ │ +

Smart factor on poses, assuming camera calibration is fixed. │ │ │ │ More...

│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::SmartProjectionRigFactor< CAMERA >
 If you are using the factor, please cite: L. More...
 
struct  gtsam::traits< SmartProjectionRigFactor< CAMERA > >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

common code between lago.* (2D) and InitializePose3.* (3D)

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
Date
August, 2020
│ │ │ │ +

Smart factor on poses, assuming camera calibration is fixed.

│ │ │ │ +

Same as SmartProjectionPoseFactor, except:

    │ │ │ │ +
  • it is templated on CAMERA (i.e., it allows cameras beyond pinhole)
    • │ │ │ │ +
  • it admits a different calibration for each measurement (i.e., it can model a multi-camera rig system)
    • │ │ │ │ +
  • it allows multiple observations from the same pose/key (again, to model a multi-camera system)
    Author
    Luca Carlone
    │ │ │ │ +
    │ │ │ │ +Frank Dellaert
    │ │ │ │ +
    • │ │ │ │ +
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,35 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -InitializePose.h File Reference │ │ │ │ │ -common code between lago.* (2D) and InitializePose3.* (3D) _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +SmartProjectionRigFactor.h File Reference │ │ │ │ │ +Smart factor on poses, assuming camera calibration is fixed. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_<_ _C_A_M_E_R_A_ _> │ │ │ │ │ +  If you are using the factor, please cite: L. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_<_ _C_A_M_E_R_A_ _>_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -common code between lago.* (2D) and InitializePose3.* (3D) │ │ │ │ │ - Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - August, 2020 │ │ │ │ │ +Smart factor on poses, assuming camera calibration is fixed. │ │ │ │ │ +Same as SmartProjectionPoseFactor, except: │ │ │ │ │ + * it is templated on CAMERA (i.e., it allows cameras beyond pinhole) │ │ │ │ │ + * it admits a different calibration for each measurement (i.e., it can │ │ │ │ │ + model a multi-camera rig system) │ │ │ │ │ + * it allows multiple observations from the same pose/key (again, to model a │ │ │ │ │ + multi-camera system) │ │ │ │ │ + Author │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _I_n_i_t_i_a_l_i_z_e_P_o_s_e_._h │ │ │ │ │ + * _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01328_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/InitializePose.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/SmartProjectionRigFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,116 +98,366 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
InitializePose.h
│ │ │ │ +
SmartProjectionRigFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/inference/Symbol.h>
│ │ │ │ -
22#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
│ │ │ │ -
23#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ -
24#include <gtsam/nonlinear/PriorFactor.h>
│ │ │ │ -
25#include <gtsam/slam/BetweenFactor.h>
│ │ │ │ +
25#pragma once
│ │ │ │
26
│ │ │ │ -
27namespace gtsam {
│ │ │ │ -
28namespace initialize {
│ │ │ │ -
29
│ │ │ │ -
30static constexpr Key kAnchorKey = 99999999;
│ │ │ │ -
31
│ │ │ │ -
36template <class Pose>
│ │ │ │ -
37static NonlinearFactorGraph buildPoseGraph(const NonlinearFactorGraph& graph) {
│ │ │ │ -
38 NonlinearFactorGraph poseGraph;
│ │ │ │ -
39
│ │ │ │ -
40 for (const auto& factor : graph) {
│ │ │ │ -
41 // recast to a between on Pose
│ │ │ │ -
42 if (auto between =
│ │ │ │ -
43 boost::dynamic_pointer_cast<BetweenFactor<Pose> >(factor))
│ │ │ │ -
44 poseGraph.add(between);
│ │ │ │ -
45
│ │ │ │ -
46 // recast PriorFactor<Pose> to BetweenFactor<Pose>
│ │ │ │ -
47 if (auto prior = boost::dynamic_pointer_cast<PriorFactor<Pose> >(factor))
│ │ │ │ -
48 poseGraph.emplace_shared<BetweenFactor<Pose> >(
│ │ │ │ -
49 kAnchorKey, prior->keys()[0], prior->prior(), prior->noiseModel());
│ │ │ │ -
50 }
│ │ │ │ -
51 return poseGraph;
│ │ │ │ -
52}
│ │ │ │ -
53
│ │ │ │ -
57template <class Pose>
│ │ │ │ -
58static Values computePoses(const Values& initialRot,
│ │ │ │ -
59 NonlinearFactorGraph* posegraph,
│ │ │ │ -
60 bool singleIter = true) {
│ │ │ │ -
61 const auto origin = Pose().translation();
│ │ │ │ +
27#include <gtsam/slam/SmartProjectionFactor.h>
│ │ │ │ +
28
│ │ │ │ +
29namespace gtsam {
│ │ │ │ +
51template <class CAMERA>
│ │ │ │ +
│ │ │ │ +
52class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
│ │ │ │ +
53 private:
│ │ │ │ +
54 typedef SmartProjectionFactor<CAMERA> Base;
│ │ │ │ +
55 typedef SmartProjectionRigFactor<CAMERA> This;
│ │ │ │ +
56 typedef typename CAMERA::CalibrationType CALIBRATION;
│ │ │ │ +
57 typedef typename CAMERA::Measurement MEASUREMENT;
│ │ │ │ +
58 typedef typename CAMERA::MeasurementVector MEASUREMENTS;
│ │ │ │ +
59
│ │ │ │ +
60 static const int DimPose = 6;
│ │ │ │ +
61 static const int ZDim = 2;
│ │ │ │
62
│ │ │ │ -
63 // Upgrade rotations to full poses
│ │ │ │ -
64 Values initialPose;
│ │ │ │ -
65 for (const auto& key_rot : initialRot.extract<typename Pose::Rotation>()) {
│ │ │ │ -
66 const Key& key = key_rot.first;
│ │ │ │ -
67 const auto& rot = key_rot.second;
│ │ │ │ -
68 const Pose initializedPose(rot, origin);
│ │ │ │ -
69 initialPose.insert(key, initializedPose);
│ │ │ │ -
70 }
│ │ │ │ -
71
│ │ │ │ -
72 // add prior on dummy node
│ │ │ │ -
73 auto priorModel = noiseModel::Unit::Create(Pose::dimension);
│ │ │ │ -
74 initialPose.insert(kAnchorKey, Pose());
│ │ │ │ -
75 posegraph->emplace_shared<PriorFactor<Pose> >(kAnchorKey, Pose(), priorModel);
│ │ │ │ +
63 protected:
│ │ │ │ +
65 KeyVector nonUniqueKeys_;
│ │ │ │ +
66
│ │ │ │ +
68 boost::shared_ptr<typename Base::Cameras> cameraRig_;
│ │ │ │ +
69
│ │ │ │ +
72 FastVector<size_t> cameraIds_;
│ │ │ │ +
73
│ │ │ │ +
74 public:
│ │ │ │ +
75 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │
76
│ │ │ │ -
77 // Create optimizer
│ │ │ │ -
78 GaussNewtonParams params;
│ │ │ │ -
79 if (singleIter) {
│ │ │ │ -
80 params.maxIterations = 1;
│ │ │ │ -
81 } else {
│ │ │ │ -
82 params.setVerbosity("TERMINATION");
│ │ │ │ -
83 }
│ │ │ │ -
84 GaussNewtonOptimizer optimizer(*posegraph, initialPose, params);
│ │ │ │ -
85 const Values GNresult = optimizer.optimize();
│ │ │ │ -
86
│ │ │ │ -
87 // put into Values structure
│ │ │ │ -
88 Values estimate;
│ │ │ │ -
89 for (const auto& key_pose : GNresult.extract<Pose>()) {
│ │ │ │ -
90 const Key& key = key_pose.first;
│ │ │ │ -
91 if (key != kAnchorKey) {
│ │ │ │ -
92 const Pose& pose = key_pose.second;
│ │ │ │ -
93 estimate.insert(key, pose);
│ │ │ │ -
94 }
│ │ │ │ -
95 }
│ │ │ │ -
96 return estimate;
│ │ │ │ -
97}
│ │ │ │ -
98} // namespace initialize
│ │ │ │ -
99} // namespace gtsam
│ │ │ │ - │ │ │ │ -
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │ - │ │ │ │ +
77 typedef CAMERA Camera;
│ │ │ │ +
78 typedef CameraSet<CAMERA> Cameras;
│ │ │ │ +
79
│ │ │ │ +
81 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
82
│ │ │ │ +
84 SmartProjectionRigFactor() {}
│ │ │ │ +
85
│ │ │ │ +
│ │ │ │ +
94 SmartProjectionRigFactor(
│ │ │ │ +
95 const SharedNoiseModel& sharedNoiseModel,
│ │ │ │ +
96 const boost::shared_ptr<Cameras>& cameraRig,
│ │ │ │ +
97 const SmartProjectionParams& params = SmartProjectionParams())
│ │ │ │ +
98 : Base(sharedNoiseModel, params), cameraRig_(cameraRig) {
│ │ │ │ +
99 // throw exception if configuration is not supported by this factor
│ │ │ │ +
100 if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY)
│ │ │ │ +
101 throw std::runtime_error(
│ │ │ │ +
102 "SmartProjectionRigFactor: "
│ │ │ │ +
103 "degeneracyMode must be set to ZERO_ON_DEGENERACY");
│ │ │ │ +
104 if (Base::params_.linearizationMode != gtsam::HESSIAN)
│ │ │ │ +
105 throw std::runtime_error(
│ │ │ │ +
106 "SmartProjectionRigFactor: "
│ │ │ │ +
107 "linearizationMode must be set to HESSIAN");
│ │ │ │ +
108 }
│ │ │ │ +
│ │ │ │ +
109
│ │ │ │ +
111 ~SmartProjectionRigFactor() override = default;
│ │ │ │ +
112
│ │ │ │ +
│ │ │ │ +
123 void add(const MEASUREMENT& measured, const Key& poseKey,
│ │ │ │ +
124 const size_t& cameraId = 0) {
│ │ │ │ +
125 // store measurement and key
│ │ │ │ +
126 this->measured_.push_back(measured);
│ │ │ │ +
127 this->nonUniqueKeys_.push_back(poseKey);
│ │ │ │ +
128
│ │ │ │ +
129 // also store keys in the keys_ vector: these keys are assumed to be
│ │ │ │ +
130 // unique, so we avoid duplicates here
│ │ │ │ +
131 if (std::find(this->keys_.begin(), this->keys_.end(), poseKey) ==
│ │ │ │ +
132 this->keys_.end())
│ │ │ │ +
133 this->keys_.push_back(poseKey); // add only unique keys
│ │ │ │ +
134
│ │ │ │ +
135 // store id of the camera taking the measurement
│ │ │ │ +
136 cameraIds_.push_back(cameraId);
│ │ │ │ +
137 }
│ │ │ │ +
│ │ │ │ +
138
│ │ │ │ +
│ │ │ │ +
149 void add(const MEASUREMENTS& measurements, const KeyVector& poseKeys,
│ │ │ │ +
150 const FastVector<size_t>& cameraIds = FastVector<size_t>()) {
│ │ │ │ +
151 if (poseKeys.size() != measurements.size() ||
│ │ │ │ +
152 (poseKeys.size() != cameraIds.size() && cameraIds.size() != 0)) {
│ │ │ │ +
153 throw std::runtime_error(
│ │ │ │ +
154 "SmartProjectionRigFactor: "
│ │ │ │ +
155 "trying to add inconsistent inputs");
│ │ │ │ +
156 }
│ │ │ │ +
157 if (cameraIds.size() == 0 && cameraRig_->size() > 1) {
│ │ │ │ +
158 throw std::runtime_error(
│ │ │ │ +
159 "SmartProjectionRigFactor: "
│ │ │ │ +
160 "camera rig includes multiple camera "
│ │ │ │ +
161 "but add did not input cameraIds");
│ │ │ │ +
162 }
│ │ │ │ +
163 for (size_t i = 0; i < measurements.size(); i++) {
│ │ │ │ +
164 add(measurements[i], poseKeys[i],
│ │ │ │ +
165 cameraIds.size() == 0 ? 0 : cameraIds[i]);
│ │ │ │ +
166 }
│ │ │ │ +
167 }
│ │ │ │ +
│ │ │ │ +
168
│ │ │ │ +
171 const KeyVector& nonUniqueKeys() const { return nonUniqueKeys_; }
│ │ │ │ +
172
│ │ │ │ +
174 const boost::shared_ptr<Cameras>& cameraRig() const { return cameraRig_; }
│ │ │ │ +
175
│ │ │ │ +
177 const FastVector<size_t>& cameraIds() const { return cameraIds_; }
│ │ │ │ +
178
│ │ │ │ +
│ │ │ │ +
184 void print(
│ │ │ │ +
185 const std::string& s = "",
│ │ │ │ +
186 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ +
187 std::cout << s << "SmartProjectionRigFactor: \n ";
│ │ │ │ +
188 for (size_t i = 0; i < nonUniqueKeys_.size(); i++) {
│ │ │ │ +
189 std::cout << "-- Measurement nr " << i << std::endl;
│ │ │ │ +
190 std::cout << "key: " << keyFormatter(nonUniqueKeys_[i]) << std::endl;
│ │ │ │ +
191 std::cout << "cameraId: " << cameraIds_[i] << std::endl;
│ │ │ │ +
192 (*cameraRig_)[cameraIds_[i]].print("camera in rig:\n");
│ │ │ │ +
193 }
│ │ │ │ +
194 Base::print("", keyFormatter);
│ │ │ │ +
195 }
│ │ │ │ +
│ │ │ │ +
196
│ │ │ │ +
│ │ │ │ +
198 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
│ │ │ │ +
199 const This* e = dynamic_cast<const This*>(&p);
│ │ │ │ +
200 return e && Base::equals(p, tol) && nonUniqueKeys_ == e->nonUniqueKeys() &&
│ │ │ │ +
201 cameraRig_->equals(*(e->cameraRig())) &&
│ │ │ │ +
202 std::equal(cameraIds_.begin(), cameraIds_.end(),
│ │ │ │ +
203 e->cameraIds().begin());
│ │ │ │ +
204 }
│ │ │ │ +
│ │ │ │ +
205
│ │ │ │ +
│ │ │ │ +
212 typename Base::Cameras cameras(const Values& values) const override {
│ │ │ │ +
213 typename Base::Cameras cameras;
│ │ │ │ +
214 cameras.reserve(nonUniqueKeys_.size()); // preallocate
│ │ │ │ +
215 for (size_t i = 0; i < nonUniqueKeys_.size(); i++) {
│ │ │ │ +
216 const typename Base::Camera& camera_i = (*cameraRig_)[cameraIds_[i]];
│ │ │ │ +
217 const Pose3 world_P_sensor_i =
│ │ │ │ +
218 values.at<Pose3>(nonUniqueKeys_[i]) // = world_P_body
│ │ │ │ +
219 * camera_i.pose(); // = body_P_cam_i
│ │ │ │ +
220 cameras.emplace_back(world_P_sensor_i,
│ │ │ │ +
221 make_shared<typename CAMERA::CalibrationType>(
│ │ │ │ +
222 camera_i.calibration()));
│ │ │ │ +
223 }
│ │ │ │ +
224 return cameras;
│ │ │ │ +
225 }
│ │ │ │ +
│ │ │ │ +
226
│ │ │ │ +
│ │ │ │ +
230 double error(const Values& values) const override {
│ │ │ │ +
231 if (this->active(values)) {
│ │ │ │ +
232 return this->totalReprojectionError(this->cameras(values));
│ │ │ │ +
233 } else { // else of active flag
│ │ │ │ +
234 return 0.0;
│ │ │ │ +
235 }
│ │ │ │ +
236 }
│ │ │ │ +
│ │ │ │ +
237
│ │ │ │ +
│ │ │ │ +
247 void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks& Fs,
│ │ │ │ +
248 Matrix& E, Vector& b,
│ │ │ │ +
249 const Cameras& cameras) const {
│ │ │ │ +
250 if (!this->result_) {
│ │ │ │ +
251 throw("computeJacobiansWithTriangulatedPoint");
│ │ │ │ +
252 } else { // valid result: compute jacobians
│ │ │ │ +
253 b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E);
│ │ │ │ +
254 for (size_t i = 0; i < Fs.size(); i++) {
│ │ │ │ +
255 const Pose3& body_P_sensor = (*cameraRig_)[cameraIds_[i]].pose();
│ │ │ │ +
256 const Pose3 world_P_body = cameras[i].pose() * body_P_sensor.inverse();
│ │ │ │ +
257 Eigen::Matrix<double, DimPose, DimPose> H;
│ │ │ │ +
258 world_P_body.compose(body_P_sensor, H);
│ │ │ │ +
259 Fs.at(i) = Fs.at(i) * H;
│ │ │ │ +
260 }
│ │ │ │ +
261 }
│ │ │ │ +
262 }
│ │ │ │ +
│ │ │ │ +
263
│ │ │ │ +
│ │ │ │ +
265 boost::shared_ptr<RegularHessianFactor<DimPose> > createHessianFactor(
│ │ │ │ +
266 const Values& values, const double& lambda = 0.0,
│ │ │ │ +
267 bool diagonalDamping = false) const {
│ │ │ │ +
268 // we may have multiple observation sharing the same keys (e.g., 2 cameras
│ │ │ │ +
269 // measuring from the same body pose), hence the number of unique keys may
│ │ │ │ +
270 // be smaller than nrMeasurements
│ │ │ │ +
271 size_t nrUniqueKeys =
│ │ │ │ +
272 this->keys_
│ │ │ │ +
273 .size(); // note: by construction, keys_ only contains unique keys
│ │ │ │ +
274
│ │ │ │ +
275 Cameras cameras = this->cameras(values);
│ │ │ │ +
276
│ │ │ │ +
277 // Create structures for Hessian Factors
│ │ │ │ +
278 std::vector<size_t> js;
│ │ │ │ +
279 std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2);
│ │ │ │ +
280 std::vector<Vector> gs(nrUniqueKeys);
│ │ │ │ +
281
│ │ │ │ +
282 if (this->measured_.size() != cameras.size()) // 1 observation per camera
│ │ │ │ +
283 throw std::runtime_error(
│ │ │ │ +
284 "SmartProjectionRigFactor: "
│ │ │ │ +
285 "measured_.size() inconsistent with input");
│ │ │ │ +
286
│ │ │ │ +
287 // triangulate 3D point at given linearization point
│ │ │ │ +
288 this->triangulateSafe(cameras);
│ │ │ │ +
289
│ │ │ │ +
290 if (!this->result_) { // failed: return "empty/zero" Hessian
│ │ │ │ +
291 if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) {
│ │ │ │ +
292 for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose);
│ │ │ │ +
293 for (Vector& v : gs) v = Vector::Zero(DimPose);
│ │ │ │ +
294 return boost::make_shared<RegularHessianFactor<DimPose> >(this->keys_,
│ │ │ │ +
295 Gs, gs, 0.0);
│ │ │ │ +
296 } else {
│ │ │ │ +
297 throw std::runtime_error(
│ │ │ │ +
298 "SmartProjectionRigFactor: "
│ │ │ │ +
299 "only supported degeneracy mode is ZERO_ON_DEGENERACY");
│ │ │ │ +
300 }
│ │ │ │ +
301 }
│ │ │ │ +
302
│ │ │ │ +
303 // compute Jacobian given triangulated 3D Point
│ │ │ │ +
304 typename Base::FBlocks Fs;
│ │ │ │ +
305 Matrix E;
│ │ │ │ +
306 Vector b;
│ │ │ │ +
307 this->computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras);
│ │ │ │ +
308
│ │ │ │ +
309 // Whiten using noise model
│ │ │ │ +
310 this->noiseModel_->WhitenSystem(E, b);
│ │ │ │ +
311 for (size_t i = 0; i < Fs.size(); i++) {
│ │ │ │ +
312 Fs[i] = this->noiseModel_->Whiten(Fs[i]);
│ │ │ │ +
313 }
│ │ │ │ +
314
│ │ │ │ +
315 const Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping);
│ │ │ │ +
316
│ │ │ │ +
317 // Build augmented Hessian (with last row/column being the information
│ │ │ │ +
318 // vector) Note: we need to get the augumented hessian wrt the unique keys
│ │ │ │ +
319 // in key_
│ │ │ │ +
320 SymmetricBlockMatrix augmentedHessianUniqueKeys =
│ │ │ │ +
321 Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 6, 6>(
│ │ │ │ +
322 Fs, E, P, b, nonUniqueKeys_, this->keys_);
│ │ │ │ +
323
│ │ │ │ +
324 return boost::make_shared<RegularHessianFactor<DimPose> >(
│ │ │ │ +
325 this->keys_, augmentedHessianUniqueKeys);
│ │ │ │ +
326 }
│ │ │ │ +
│ │ │ │ +
327
│ │ │ │ +
│ │ │ │ +
335 boost::shared_ptr<GaussianFactor> linearizeDamped(
│ │ │ │ +
336 const Values& values, const double& lambda = 0.0) const {
│ │ │ │ +
337 // depending on flag set on construction we may linearize to different
│ │ │ │ +
338 // linear factors
│ │ │ │ +
339 switch (this->params_.linearizationMode) {
│ │ │ │ +
340 case HESSIAN:
│ │ │ │ +
341 return this->createHessianFactor(values, lambda);
│ │ │ │ +
342 default:
│ │ │ │ +
343 throw std::runtime_error(
│ │ │ │ +
344 "SmartProjectionRigFactor: unknown linearization mode");
│ │ │ │ +
345 }
│ │ │ │ +
346 }
│ │ │ │ +
│ │ │ │ +
347
│ │ │ │ +
│ │ │ │ +
349 boost::shared_ptr<GaussianFactor> linearize(
│ │ │ │ +
350 const Values& values) const override {
│ │ │ │ +
351 return this->linearizeDamped(values);
│ │ │ │ +
352 }
│ │ │ │ +
│ │ │ │ +
353
│ │ │ │ +
354 private:
│ │ │ │ +
356 friend class boost::serialization::access;
│ │ │ │ +
357 template <class ARCHIVE>
│ │ │ │ +
358 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ +
359 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
360 // ar& BOOST_SERIALIZATION_NVP(nonUniqueKeys_);
│ │ │ │ +
361 // ar& BOOST_SERIALIZATION_NVP(cameraRig_);
│ │ │ │ +
362 // ar& BOOST_SERIALIZATION_NVP(cameraIds_);
│ │ │ │ +
363 }
│ │ │ │ +
364};
│ │ │ │ +
│ │ │ │ +
365// end of class declaration
│ │ │ │ +
366
│ │ │ │ +
368template <class CAMERA>
│ │ │ │ +
│ │ │ │ +
369struct traits<SmartProjectionRigFactor<CAMERA> >
│ │ │ │ +
370 : public Testable<SmartProjectionRigFactor<CAMERA> > {};
│ │ │ │ +
│ │ │ │ +
371
│ │ │ │ +
372} // namespace gtsam
│ │ │ │ +
SmartProjectionFactor.h
Smart factor on cameras (pose + calibration)
│ │ │ │ +
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::FactorGraph::add
IsDerived< DERIVEDFACTOR > add(boost::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
Definition FactorGraph.h:210
│ │ │ │ -
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Create a unit covariance noise model.
Definition NoiseModel.h:597
│ │ │ │ -
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │ - │ │ │ │ - │ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition CameraSet.h:36
│ │ │ │ +
gtsam::CameraSet::reprojectionError
Vector reprojectionError(const POINT &point, const ZVector &measured, boost::optional< FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost::none) const
Calculate vector [project2(point)-z] of re-projection errors.
Definition CameraSet.h:136
│ │ │ │ +
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ +
gtsam::Pose3::inverse
Pose3 inverse() const
inverse transformation with derivatives
Definition Pose3.cpp:49
│ │ │ │ +
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
│ │ │ │ +
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ +
gtsam::NonlinearFactor::active
virtual bool active(const Values &) const
Checks whether a factor should be used based on a set of values.
Definition NonlinearFactor.h:118
│ │ │ │ +
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
gtsam::Values::at
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
│ │ │ │ +
gtsam::SmartFactorBase::noiseModel_
SharedIsotropic noiseModel_
As of Feb 22, 2015, the noise model is the same for all measurements and is isotropic.
Definition SmartFactorBase.h:72
│ │ │ │ +
gtsam::SmartFactorBase::measured_
ZVector measured_
Measurements for each of the m views.
Definition SmartFactorBase.h:79
│ │ │ │ +
gtsam::SmartFactorBase::measured
const ZVector & measured() const
Return the 2D measurements (ZDim, in general).
Definition SmartFactorBase.h:159
│ │ │ │ +
gtsam::SmartFactorBase::PointCov
static Matrix PointCov(const Matrix &E)
Computes Point Covariance P from the "point Jacobian" E.
Definition SmartFactorBase.h:274
│ │ │ │ +
gtsam::SmartProjectionParams
Definition SmartFactorParams.h:42
│ │ │ │ +
gtsam::SmartProjectionParams::linearizationMode
LinearizationMode linearizationMode
How to linearize the factor.
Definition SmartFactorParams.h:44
│ │ │ │ +
gtsam::SmartProjectionParams::degeneracyMode
DegeneracyMode degeneracyMode
How to linearize the factor.
Definition SmartFactorParams.h:45
│ │ │ │ +
gtsam::SmartProjectionFactor
SmartProjectionFactor: triangulates point and keeps an estimate of it around.
Definition SmartProjectionFactor.h:45
│ │ │ │ +
gtsam::SmartProjectionFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition SmartProjectionFactor.h:103
│ │ │ │ +
gtsam::SmartProjectionFactor::result_
TriangulationResult result_
result from triangulateSafe
Definition SmartProjectionFactor.h:63
│ │ │ │ +
gtsam::SmartProjectionFactor::triangulateSafe
TriangulationResult triangulateSafe(const Cameras &cameras) const
Call gtsam::triangulateSafe iff we need to re-triangulate.
Definition SmartProjectionFactor.h:174
│ │ │ │ +
gtsam::SmartProjectionFactor::totalReprojectionError
double totalReprojectionError(const Cameras &cameras, boost::optional< Point3 > externalPoint=boost::none) const
Calculate the error of the factor.
Definition SmartProjectionFactor.h:411
│ │ │ │ +
gtsam::SmartProjectionFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition SmartProjectionFactor.h:115
│ │ │ │ +
gtsam::SmartProjectionFactor::Camera
CAMERA Camera
shorthand for a set of cameras
Definition SmartProjectionFactor.h:74
│ │ │ │ +
gtsam::SmartProjectionRigFactor
If you are using the factor, please cite: L.
Definition SmartProjectionRigFactor.h:52
│ │ │ │ +
gtsam::SmartProjectionRigFactor::nonUniqueKeys_
KeyVector nonUniqueKeys_
vector of keys (one for each observation) with potentially repeated keys
Definition SmartProjectionRigFactor.h:65
│ │ │ │ +
gtsam::SmartProjectionRigFactor::cameraIds_
FastVector< size_t > cameraIds_
vector of camera Ids (one for each observation, in the same order), identifying which camera took the...
Definition SmartProjectionRigFactor.h:72
│ │ │ │ +
gtsam::SmartProjectionRigFactor::cameraIds
const FastVector< size_t > & cameraIds() const
return the calibration object
Definition SmartProjectionRigFactor.h:177
│ │ │ │ +
gtsam::SmartProjectionRigFactor::createHessianFactor
boost::shared_ptr< RegularHessianFactor< DimPose > > createHessianFactor(const Values &values, const double &lambda=0.0, bool diagonalDamping=false) const
linearize and return a Hessianfactor that is an approximation of error(p)
Definition SmartProjectionRigFactor.h:265
│ │ │ │ +
gtsam::SmartProjectionRigFactor::add
void add(const MEASUREMENT &measured, const Key &poseKey, const size_t &cameraId=0)
add a new measurement, corresponding to an observation from pose "poseKey" and taken from the camera ...
Definition SmartProjectionRigFactor.h:123
│ │ │ │ +
gtsam::SmartProjectionRigFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition SmartProjectionRigFactor.h:81
│ │ │ │ +
gtsam::SmartProjectionRigFactor::computeJacobiansWithTriangulatedPoint
void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks &Fs, Matrix &E, Vector &b, const Cameras &cameras) const
Compute jacobian F, E and error vector at a given linearization point.
Definition SmartProjectionRigFactor.h:247
│ │ │ │ +
gtsam::SmartProjectionRigFactor::add
void add(const MEASUREMENTS &measurements, const KeyVector &poseKeys, const FastVector< size_t > &cameraIds=FastVector< size_t >())
Variant of the previous "add" function in which we include multiple measurements.
Definition SmartProjectionRigFactor.h:149
│ │ │ │ +
gtsam::SmartProjectionRigFactor::SmartProjectionRigFactor
SmartProjectionRigFactor()
Default constructor, only for serialization.
Definition SmartProjectionRigFactor.h:84
│ │ │ │ +
gtsam::SmartProjectionRigFactor::error
double error(const Values &values) const override
error calculates the error of the factor.
Definition SmartProjectionRigFactor.h:230
│ │ │ │ +
gtsam::SmartProjectionRigFactor::cameras
Base::Cameras cameras(const Values &values) const override
Collect all cameras involved in this factor.
Definition SmartProjectionRigFactor.h:212
│ │ │ │ +
gtsam::SmartProjectionRigFactor::~SmartProjectionRigFactor
~SmartProjectionRigFactor() override=default
Virtual destructor.
│ │ │ │ +
gtsam::SmartProjectionRigFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition SmartProjectionRigFactor.h:184
│ │ │ │ +
gtsam::SmartProjectionRigFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &values) const override
linearize
Definition SmartProjectionRigFactor.h:349
│ │ │ │ +
gtsam::SmartProjectionRigFactor::access
friend class boost::serialization::access
Serialization function.
Definition SmartProjectionRigFactor.h:356
│ │ │ │ +
gtsam::SmartProjectionRigFactor::nonUniqueKeys
const KeyVector & nonUniqueKeys() const
return (for each observation) the (possibly non unique) keys involved in the measurements
Definition SmartProjectionRigFactor.h:171
│ │ │ │ +
gtsam::SmartProjectionRigFactor::cameraRig_
boost::shared_ptr< typename Base::Cameras > cameraRig_
cameras in the rig (fixed poses wrt body and intrinsics, for each camera)
Definition SmartProjectionRigFactor.h:68
│ │ │ │ +
gtsam::SmartProjectionRigFactor::SmartProjectionRigFactor
SmartProjectionRigFactor(const SharedNoiseModel &sharedNoiseModel, const boost::shared_ptr< Cameras > &cameraRig, const SmartProjectionParams &params=SmartProjectionParams())
Constructor.
Definition SmartProjectionRigFactor.h:94
│ │ │ │ +
gtsam::SmartProjectionRigFactor::cameraRig
const boost::shared_ptr< Cameras > & cameraRig() const
return the calibration object
Definition SmartProjectionRigFactor.h:174
│ │ │ │ +
gtsam::SmartProjectionRigFactor::linearizeDamped
boost::shared_ptr< GaussianFactor > linearizeDamped(const Values &values, const double &lambda=0.0) const
Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM)
Definition SmartProjectionRigFactor.h:335
│ │ │ │ +
gtsam::SmartProjectionRigFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition SmartProjectionRigFactor.h:198
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,124 +1,503 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -InitializePose.h │ │ │ │ │ +SmartProjectionRigFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_S_y_m_b_o_l_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_P_r_i_o_r_F_a_c_t_o_r_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_s_l_a_m_/_B_e_t_w_e_e_n_F_a_c_t_o_r_._h> │ │ │ │ │ +25#pragma once │ │ │ │ │ 26 │ │ │ │ │ -27namespace _g_t_s_a_m { │ │ │ │ │ -28namespace initialize { │ │ │ │ │ -29 │ │ │ │ │ -30static constexpr _K_e_y kAnchorKey = 99999999; │ │ │ │ │ -31 │ │ │ │ │ -36template │ │ │ │ │ -37static NonlinearFactorGraph buildPoseGraph(const NonlinearFactorGraph& graph) │ │ │ │ │ -{ │ │ │ │ │ -38 NonlinearFactorGraph poseGraph; │ │ │ │ │ -39 │ │ │ │ │ -40 for (const auto& factor : graph) { │ │ │ │ │ -41 // recast to a between on Pose │ │ │ │ │ -42 if (auto between = │ │ │ │ │ -43 boost::dynamic_pointer_cast >(factor)) │ │ │ │ │ -44 poseGraph._a_d_d(between); │ │ │ │ │ -45 │ │ │ │ │ -46 // recast PriorFactor to BetweenFactor │ │ │ │ │ -47 if (auto prior = boost::dynamic_pointer_cast >(factor)) │ │ │ │ │ -48 poseGraph.emplace_shared >( │ │ │ │ │ -49 kAnchorKey, prior->keys()[0], prior->prior(), prior->noiseModel()); │ │ │ │ │ -50 } │ │ │ │ │ -51 return poseGraph; │ │ │ │ │ -52} │ │ │ │ │ -53 │ │ │ │ │ -57template │ │ │ │ │ -58static _V_a_l_u_e_s computePoses(const _V_a_l_u_e_s& initialRot, │ │ │ │ │ -59 NonlinearFactorGraph* posegraph, │ │ │ │ │ -60 bool singleIter = true) { │ │ │ │ │ -61 const auto origin = Pose().translation(); │ │ │ │ │ +27#include <_g_t_s_a_m_/_s_l_a_m_/_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_._h> │ │ │ │ │ +28 │ │ │ │ │ +29namespace _g_t_s_a_m { │ │ │ │ │ +51template │ │ │ │ │ +_5_2class _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r : public _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r { │ │ │ │ │ +53 private: │ │ │ │ │ +54 typedef _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_<_C_A_M_E_R_A_> _B_a_s_e; │ │ │ │ │ +55 typedef _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_<_C_A_M_E_R_A_> This; │ │ │ │ │ +56 typedef typename CAMERA::CalibrationType CALIBRATION; │ │ │ │ │ +57 typedef typename CAMERA::Measurement MEASUREMENT; │ │ │ │ │ +58 typedef typename CAMERA::MeasurementVector MEASUREMENTS; │ │ │ │ │ +59 │ │ │ │ │ +60 static const int DimPose = 6; │ │ │ │ │ +61 static const int ZDim = 2; │ │ │ │ │ 62 │ │ │ │ │ -63 // Upgrade rotations to full poses │ │ │ │ │ -64 _V_a_l_u_e_s initialPose; │ │ │ │ │ -65 for (const auto& key_rot : initialRot.extract()) { │ │ │ │ │ -66 const _K_e_y& key = key_rot.first; │ │ │ │ │ -67 const auto& rot = key_rot.second; │ │ │ │ │ -68 const Pose initializedPose(rot, origin); │ │ │ │ │ -69 initialPose.insert(key, initializedPose); │ │ │ │ │ -70 } │ │ │ │ │ -71 │ │ │ │ │ -72 // add prior on dummy node │ │ │ │ │ -73 auto priorModel = _n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_C_r_e_a_t_e(Pose::dimension); │ │ │ │ │ -74 initialPose.insert(kAnchorKey, Pose()); │ │ │ │ │ -75 posegraph->emplace_shared >(kAnchorKey, Pose(), │ │ │ │ │ -priorModel); │ │ │ │ │ +63 protected: │ │ │ │ │ +_6_5 _K_e_y_V_e_c_t_o_r _n_o_n_U_n_i_q_u_e_K_e_y_s__; │ │ │ │ │ +66 │ │ │ │ │ +_6_8 boost::shared_ptr _c_a_m_e_r_a_R_i_g__; │ │ │ │ │ +69 │ │ │ │ │ +_7_2 _F_a_s_t_V_e_c_t_o_r_<_s_i_z_e___t_> _c_a_m_e_r_a_I_d_s__; │ │ │ │ │ +73 │ │ │ │ │ +74 public: │ │ │ │ │ +75 EIGEN_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ 76 │ │ │ │ │ -77 // Create optimizer │ │ │ │ │ -78 GaussNewtonParams params; │ │ │ │ │ -79 if (singleIter) { │ │ │ │ │ -80 params.maxIterations = 1; │ │ │ │ │ -81 } else { │ │ │ │ │ -82 params.setVerbosity("TERMINATION"); │ │ │ │ │ -83 } │ │ │ │ │ -84 GaussNewtonOptimizer optimizer(*posegraph, initialPose, params); │ │ │ │ │ -85 const _V_a_l_u_e_s GNresult = optimizer.optimize(); │ │ │ │ │ -86 │ │ │ │ │ -87 // put into Values structure │ │ │ │ │ -88 _V_a_l_u_e_s estimate; │ │ │ │ │ -89 for (const auto& key_pose : GNresult.extract()) { │ │ │ │ │ -90 const _K_e_y& key = key_pose.first; │ │ │ │ │ -91 if (key != kAnchorKey) { │ │ │ │ │ -92 const Pose& pose = key_pose.second; │ │ │ │ │ -93 estimate.insert(key, pose); │ │ │ │ │ -94 } │ │ │ │ │ -95 } │ │ │ │ │ -96 return estimate; │ │ │ │ │ -97} │ │ │ │ │ -98} // namespace initialize │ │ │ │ │ -99} // namespace gtsam │ │ │ │ │ -_G_a_u_s_s_N_e_w_t_o_n_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Factor Graph consisting of non-linear factors. │ │ │ │ │ -_B_e_t_w_e_e_n_F_a_c_t_o_r_._h │ │ │ │ │ +77 typedef CAMERA _C_a_m_e_r_a; │ │ │ │ │ +78 typedef _C_a_m_e_r_a_S_e_t_<_C_A_M_E_R_A_> _C_a_m_e_r_a_s; │ │ │ │ │ +79 │ │ │ │ │ +_8_1 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +82 │ │ │ │ │ +_8_4 _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r() {} │ │ │ │ │ +85 │ │ │ │ │ +_9_4 _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r( │ │ │ │ │ +95 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& sharedNoiseModel, │ │ │ │ │ +96 const boost::shared_ptr& _c_a_m_e_r_a_R_i_g, │ │ │ │ │ +97 const _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s& params = _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s()) │ │ │ │ │ +98 : _B_a_s_e(sharedNoiseModel, params), _c_a_m_e_r_a_R_i_g__(_c_a_m_e_r_a_R_i_g) { │ │ │ │ │ +99 // throw exception if configuration is not supported by this factor │ │ │ │ │ +100 if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY) │ │ │ │ │ +101 throw std::runtime_error( │ │ │ │ │ +102 "SmartProjectionRigFactor: " │ │ │ │ │ +103 "degeneracyMode must be set to ZERO_ON_DEGENERACY"); │ │ │ │ │ +104 if (Base::params_.linearizationMode != gtsam::HESSIAN) │ │ │ │ │ +105 throw std::runtime_error( │ │ │ │ │ +106 "SmartProjectionRigFactor: " │ │ │ │ │ +107 "linearizationMode must be set to HESSIAN"); │ │ │ │ │ +108 } │ │ │ │ │ +109 │ │ │ │ │ +_1_1_1 _~_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r() override = default; │ │ │ │ │ +112 │ │ │ │ │ +_1_2_3 void _a_d_d(const MEASUREMENT& _m_e_a_s_u_r_e_d, const _K_e_y& poseKey, │ │ │ │ │ +124 const size_t& cameraId = 0) { │ │ │ │ │ +125 // store measurement and key │ │ │ │ │ +126 this->_m_e_a_s_u_r_e_d__.push_back(measured); │ │ │ │ │ +127 this->nonUniqueKeys_.push_back(poseKey); │ │ │ │ │ +128 │ │ │ │ │ +129 // also store keys in the keys_ vector: these keys are assumed to be │ │ │ │ │ +130 // unique, so we avoid duplicates here │ │ │ │ │ +131 if (std::find(this->_k_e_y_s__.begin(), this->keys_.end(), poseKey) == │ │ │ │ │ +132 this->keys_.end()) │ │ │ │ │ +133 this->_k_e_y_s__.push_back(poseKey); // add only unique keys │ │ │ │ │ +134 │ │ │ │ │ +135 // store id of the camera taking the measurement │ │ │ │ │ +136 _c_a_m_e_r_a_I_d_s__.push_back(cameraId); │ │ │ │ │ +137 } │ │ │ │ │ +138 │ │ │ │ │ +_1_4_9 void _a_d_d(const MEASUREMENTS& measurements, const _K_e_y_V_e_c_t_o_r& poseKeys, │ │ │ │ │ +150 const _F_a_s_t_V_e_c_t_o_r_<_s_i_z_e___t_>& _c_a_m_e_r_a_I_d_s = _F_a_s_t_V_e_c_t_o_r_<_s_i_z_e___t_>()) { │ │ │ │ │ +151 if (poseKeys.size() != measurements.size() || │ │ │ │ │ +152 (poseKeys.size() != _c_a_m_e_r_a_I_d_s.size() && _c_a_m_e_r_a_I_d_s.size() != 0)) { │ │ │ │ │ +153 throw std::runtime_error( │ │ │ │ │ +154 "SmartProjectionRigFactor: " │ │ │ │ │ +155 "trying to add inconsistent inputs"); │ │ │ │ │ +156 } │ │ │ │ │ +157 if (_c_a_m_e_r_a_I_d_s.size() == 0 && _c_a_m_e_r_a_R_i_g__->size() > 1) { │ │ │ │ │ +158 throw std::runtime_error( │ │ │ │ │ +159 "SmartProjectionRigFactor: " │ │ │ │ │ +160 "camera rig includes multiple camera " │ │ │ │ │ +161 "but add did not input cameraIds"); │ │ │ │ │ +162 } │ │ │ │ │ +163 for (size_t i = 0; i < measurements.size(); i++) { │ │ │ │ │ +164 _a_d_d(measurements[i], poseKeys[i], │ │ │ │ │ +165 _c_a_m_e_r_a_I_d_s.size() == 0 ? 0 : _c_a_m_e_r_a_I_d_s[i]); │ │ │ │ │ +166 } │ │ │ │ │ +167 } │ │ │ │ │ +168 │ │ │ │ │ +_1_7_1 const _K_e_y_V_e_c_t_o_r& _n_o_n_U_n_i_q_u_e_K_e_y_s() const { return _n_o_n_U_n_i_q_u_e_K_e_y_s__; } │ │ │ │ │ +172 │ │ │ │ │ +_1_7_4 const boost::shared_ptr& _c_a_m_e_r_a_R_i_g() const { return _c_a_m_e_r_a_R_i_g__; } │ │ │ │ │ +175 │ │ │ │ │ +_1_7_7 const _F_a_s_t_V_e_c_t_o_r_<_s_i_z_e___t_>& _c_a_m_e_r_a_I_d_s() const { return _c_a_m_e_r_a_I_d_s__; } │ │ │ │ │ +178 │ │ │ │ │ +_1_8_4 void _p_r_i_n_t( │ │ │ │ │ +185 const std::string& s = "", │ │ │ │ │ +186 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ +187 std::cout << s << "SmartProjectionRigFactor: \n "; │ │ │ │ │ +188 for (size_t i = 0; i < _n_o_n_U_n_i_q_u_e_K_e_y_s__.size(); i++) { │ │ │ │ │ +189 std::cout << "-- Measurement nr " << i << std::endl; │ │ │ │ │ +190 std::cout << "key: " << keyFormatter(_n_o_n_U_n_i_q_u_e_K_e_y_s__[i]) << std::endl; │ │ │ │ │ +191 std::cout << "cameraId: " << _c_a_m_e_r_a_I_d_s__[i] << std::endl; │ │ │ │ │ +192 (*cameraRig_)[_c_a_m_e_r_a_I_d_s__[i]].print("camera in rig:\n"); │ │ │ │ │ +193 } │ │ │ │ │ +194 _B_a_s_e_:_:_p_r_i_n_t("", keyFormatter); │ │ │ │ │ +195 } │ │ │ │ │ +196 │ │ │ │ │ +_1_9_8 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& p, double tol = 1e-9) const override { │ │ │ │ │ +199 const This* e = dynamic_cast(&p); │ │ │ │ │ +200 return e && _B_a_s_e_:_:_e_q_u_a_l_s(p, tol) && _n_o_n_U_n_i_q_u_e_K_e_y_s__ == e->nonUniqueKeys() && │ │ │ │ │ +201 _c_a_m_e_r_a_R_i_g__->equals(*(e->cameraRig())) && │ │ │ │ │ +202 std::equal(_c_a_m_e_r_a_I_d_s__.begin(), _c_a_m_e_r_a_I_d_s__.end(), │ │ │ │ │ +203 e->cameraIds().begin()); │ │ │ │ │ +204 } │ │ │ │ │ +205 │ │ │ │ │ +_2_1_2 typename _B_a_s_e_:_:_C_a_m_e_r_a_s _c_a_m_e_r_a_s(const _V_a_l_u_e_s& values) const override { │ │ │ │ │ +213 typename _B_a_s_e_:_:_C_a_m_e_r_a_s _c_a_m_e_r_a_s; │ │ │ │ │ +214 _c_a_m_e_r_a_s.reserve(_n_o_n_U_n_i_q_u_e_K_e_y_s__.size()); // preallocate │ │ │ │ │ +215 for (size_t i = 0; i < _n_o_n_U_n_i_q_u_e_K_e_y_s__.size(); i++) { │ │ │ │ │ +216 const typename _B_a_s_e_:_:_C_a_m_e_r_a& camera_i = (*cameraRig_)[_c_a_m_e_r_a_I_d_s__[i]]; │ │ │ │ │ +217 const _P_o_s_e_3 world_P_sensor_i = │ │ │ │ │ +218 values._a_t<_P_o_s_e_3>(_n_o_n_U_n_i_q_u_e_K_e_y_s__[i]) // = world_P_body │ │ │ │ │ +219 * camera_i.pose(); // = body_P_cam_i │ │ │ │ │ +220 _c_a_m_e_r_a_s.emplace_back(world_P_sensor_i, │ │ │ │ │ +221 make_shared( │ │ │ │ │ +222 camera_i.calibration())); │ │ │ │ │ +223 } │ │ │ │ │ +224 return _c_a_m_e_r_a_s; │ │ │ │ │ +225 } │ │ │ │ │ +226 │ │ │ │ │ +_2_3_0 double _e_r_r_o_r(const _V_a_l_u_e_s& values) const override { │ │ │ │ │ +231 if (this->_a_c_t_i_v_e(values)) { │ │ │ │ │ +232 return this->_t_o_t_a_l_R_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r(this->_c_a_m_e_r_a_s(values)); │ │ │ │ │ +233 } else { // else of active flag │ │ │ │ │ +234 return 0.0; │ │ │ │ │ +235 } │ │ │ │ │ +236 } │ │ │ │ │ +237 │ │ │ │ │ +_2_4_7 void _c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s_W_i_t_h_T_r_i_a_n_g_u_l_a_t_e_d_P_o_i_n_t(typename Base::FBlocks& Fs, │ │ │ │ │ +248 Matrix& E, Vector& b, │ │ │ │ │ +249 const _C_a_m_e_r_a_s& _c_a_m_e_r_a_s) const { │ │ │ │ │ +250 if (!this->_r_e_s_u_l_t__) { │ │ │ │ │ +251 throw("computeJacobiansWithTriangulatedPoint"); │ │ │ │ │ +252 } else { // valid result: compute jacobians │ │ │ │ │ +253 b = -_c_a_m_e_r_a_s._r_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r(*this->_r_e_s_u_l_t__, this->_m_e_a_s_u_r_e_d__, Fs, E); │ │ │ │ │ +254 for (size_t i = 0; i < Fs.size(); i++) { │ │ │ │ │ +255 const _P_o_s_e_3& body_P_sensor = (*cameraRig_)[_c_a_m_e_r_a_I_d_s__[i]].pose(); │ │ │ │ │ +256 const _P_o_s_e_3 world_P_body = _c_a_m_e_r_a_s[i].pose() * body_P_sensor._i_n_v_e_r_s_e(); │ │ │ │ │ +257 Eigen::Matrix H; │ │ │ │ │ +258 world_P_body.compose(body_P_sensor, H); │ │ │ │ │ +259 Fs.at(i) = Fs.at(i) * H; │ │ │ │ │ +260 } │ │ │ │ │ +261 } │ │ │ │ │ +262 } │ │ │ │ │ +263 │ │ │ │ │ +_2_6_5 boost::shared_ptr > _c_r_e_a_t_e_H_e_s_s_i_a_n_F_a_c_t_o_r( │ │ │ │ │ +266 const _V_a_l_u_e_s& values, const double& lambda = 0.0, │ │ │ │ │ +267 bool diagonalDamping = false) const { │ │ │ │ │ +268 // we may have multiple observation sharing the same keys (e.g., 2 cameras │ │ │ │ │ +269 // measuring from the same body pose), hence the number of unique keys may │ │ │ │ │ +270 // be smaller than nrMeasurements │ │ │ │ │ +271 size_t nrUniqueKeys = │ │ │ │ │ +272 this->_k_e_y_s__ │ │ │ │ │ +273 .size(); // note: by construction, keys_ only contains unique keys │ │ │ │ │ +274 │ │ │ │ │ +275 Cameras _c_a_m_e_r_a_s = this->_c_a_m_e_r_a_s(values); │ │ │ │ │ +276 │ │ │ │ │ +277 // Create structures for Hessian Factors │ │ │ │ │ +278 std::vector js; │ │ │ │ │ +279 std::vector Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); │ │ │ │ │ +280 std::vector gs(nrUniqueKeys); │ │ │ │ │ +281 │ │ │ │ │ +282 if (this->_m_e_a_s_u_r_e_d__.size() != cameras.size()) // 1 observation per camera │ │ │ │ │ +283 throw std::runtime_error( │ │ │ │ │ +284 "SmartProjectionRigFactor: " │ │ │ │ │ +285 "measured_.size() inconsistent with input"); │ │ │ │ │ +286 │ │ │ │ │ +287 // triangulate 3D point at given linearization point │ │ │ │ │ +288 this->_t_r_i_a_n_g_u_l_a_t_e_S_a_f_e(cameras); │ │ │ │ │ +289 │ │ │ │ │ +290 if (!this->_r_e_s_u_l_t__) { // failed: return "empty/zero" Hessian │ │ │ │ │ +291 if (this->params_._d_e_g_e_n_e_r_a_c_y_M_o_d_e == ZERO_ON_DEGENERACY) { │ │ │ │ │ +292 for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose); │ │ │ │ │ +293 for (Vector& v : gs) v = Vector::Zero(DimPose); │ │ │ │ │ +294 return boost::make_shared >(this->_k_e_y_s__, │ │ │ │ │ +295 Gs, gs, 0.0); │ │ │ │ │ +296 } else { │ │ │ │ │ +297 throw std::runtime_error( │ │ │ │ │ +298 "SmartProjectionRigFactor: " │ │ │ │ │ +299 "only supported degeneracy mode is ZERO_ON_DEGENERACY"); │ │ │ │ │ +300 } │ │ │ │ │ +301 } │ │ │ │ │ +302 │ │ │ │ │ +303 // compute Jacobian given triangulated 3D Point │ │ │ │ │ +304 typename Base::FBlocks Fs; │ │ │ │ │ +305 Matrix E; │ │ │ │ │ +306 Vector b; │ │ │ │ │ +307 this->_c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s_W_i_t_h_T_r_i_a_n_g_u_l_a_t_e_d_P_o_i_n_t(Fs, E, b, _c_a_m_e_r_a_s); │ │ │ │ │ +308 │ │ │ │ │ +309 // Whiten using noise model │ │ │ │ │ +310 this->_n_o_i_s_e_M_o_d_e_l__->WhitenSystem(E, b); │ │ │ │ │ +311 for (size_t i = 0; i < Fs.size(); i++) { │ │ │ │ │ +312 Fs[i] = this->_n_o_i_s_e_M_o_d_e_l__->Whiten(Fs[i]); │ │ │ │ │ +313 } │ │ │ │ │ +314 │ │ │ │ │ +315 const Matrix3 P = _B_a_s_e_:_:_C_a_m_e_r_a_s_:_:_P_o_i_n_t_C_o_v(E, lambda, diagonalDamping); │ │ │ │ │ +316 │ │ │ │ │ +317 // Build augmented Hessian (with last row/column being the information │ │ │ │ │ +318 // vector) Note: we need to get the augumented hessian wrt the unique keys │ │ │ │ │ +319 // in key_ │ │ │ │ │ +320 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x augmentedHessianUniqueKeys = │ │ │ │ │ +321 Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 6, 6>( │ │ │ │ │ +322 Fs, E, P, b, _n_o_n_U_n_i_q_u_e_K_e_y_s__, this->_k_e_y_s__); │ │ │ │ │ +323 │ │ │ │ │ +324 return boost::make_shared >( │ │ │ │ │ +325 this->_k_e_y_s__, augmentedHessianUniqueKeys); │ │ │ │ │ +326 } │ │ │ │ │ +327 │ │ │ │ │ +_3_3_5 boost::shared_ptr _l_i_n_e_a_r_i_z_e_D_a_m_p_e_d( │ │ │ │ │ +336 const _V_a_l_u_e_s& values, const double& lambda = 0.0) const { │ │ │ │ │ +337 // depending on flag set on construction we may linearize to different │ │ │ │ │ +338 // linear factors │ │ │ │ │ +339 switch (this->params_._l_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e) { │ │ │ │ │ +340 case HESSIAN: │ │ │ │ │ +341 return this->_c_r_e_a_t_e_H_e_s_s_i_a_n_F_a_c_t_o_r(values, lambda); │ │ │ │ │ +342 default: │ │ │ │ │ +343 throw std::runtime_error( │ │ │ │ │ +344 "SmartProjectionRigFactor: unknown linearization mode"); │ │ │ │ │ +345 } │ │ │ │ │ +346 } │ │ │ │ │ +347 │ │ │ │ │ +_3_4_9 boost::shared_ptr _l_i_n_e_a_r_i_z_e( │ │ │ │ │ +350 const _V_a_l_u_e_s& values) const override { │ │ │ │ │ +351 return this->_l_i_n_e_a_r_i_z_e_D_a_m_p_e_d(values); │ │ │ │ │ +352 } │ │ │ │ │ +353 │ │ │ │ │ +354 private: │ │ │ │ │ +_3_5_6 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +357 template │ │ │ │ │ +358 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ +359 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +360 // ar& BOOST_SERIALIZATION_NVP(nonUniqueKeys_); │ │ │ │ │ +361 // ar& BOOST_SERIALIZATION_NVP(cameraRig_); │ │ │ │ │ +362 // ar& BOOST_SERIALIZATION_NVP(cameraIds_); │ │ │ │ │ +363 } │ │ │ │ │ +364}; │ │ │ │ │ +365// end of class declaration │ │ │ │ │ +366 │ │ │ │ │ +368template │ │ │ │ │ +_3_6_9struct _t_r_a_i_t_s<_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r > │ │ │ │ │ +370 : public _T_e_s_t_a_b_l_e > {}; │ │ │ │ │ +371 │ │ │ │ │ +372} // namespace gtsam │ │ │ │ │ +_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_._h │ │ │ │ │ +Smart factor on cameras (pose + calibration) │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ +std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ +FastVector │ │ │ │ │ +FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ +DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ +Aliases. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d │ │ │ │ │ -IsDerived< DERIVEDFACTOR > add(boost::shared_ptr< DERIVEDFACTOR > factor) │ │ │ │ │ -add is a synonym for push_back. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:210 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_C_r_e_a_t_e │ │ │ │ │ -static shared_ptr Create(size_t dim) │ │ │ │ │ -Create a unit covariance noise model. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:597 │ │ │ │ │ -_V_a_l_u_e_s │ │ │ │ │ -In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ -a non-linear function... │ │ │ │ │ -_S_y_m_b_o_l_._h │ │ │ │ │ -_P_r_i_o_r_F_a_c_t_o_r_._h │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ +of blocks. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t │ │ │ │ │ +A set of cameras, all with their own calibration. │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_:_:_r_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r │ │ │ │ │ +Vector reprojectionError(const POINT &point, const ZVector &measured, boost:: │ │ │ │ │ +optional< FBlocks & > Fs=boost::none, boost::optional< Matrix & > E=boost:: │ │ │ │ │ +none) const │ │ │ │ │ +Calculate vector [project2(point)-z] of re-projection errors. │ │ │ │ │ +DDeeffiinniittiioonn CameraSet.h:136 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ +A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_i_n_v_e_r_s_e │ │ │ │ │ +Pose3 inverse() const │ │ │ │ │ +inverse transformation with derivatives │ │ │ │ │ +DDeeffiinniittiioonn Pose3.cpp:49 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s__ │ │ │ │ │ +KeyVector keys_ │ │ │ │ │ +The keys involved in this factor. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ +Nonlinear factor base class. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_:_:_a_c_t_i_v_e │ │ │ │ │ +virtual bool active(const Values &) const │ │ │ │ │ +Checks whether a factor should be used based on a set of values. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:118 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_a_t │ │ │ │ │ +const ValueType at(Key j) const │ │ │ │ │ +Retrieve a variable by key j. │ │ │ │ │ +DDeeffiinniittiioonn Values-inl.h:361 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_n_o_i_s_e_M_o_d_e_l__ │ │ │ │ │ +SharedIsotropic noiseModel_ │ │ │ │ │ +As of Feb 22, 2015, the noise model is the same for all measurements and is │ │ │ │ │ +isotropic. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_m_e_a_s_u_r_e_d__ │ │ │ │ │ +ZVector measured_ │ │ │ │ │ +Measurements for each of the m views. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:79 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_m_e_a_s_u_r_e_d │ │ │ │ │ +const ZVector & measured() const │ │ │ │ │ +Return the 2D measurements (ZDim, in general). │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:159 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_:_:_P_o_i_n_t_C_o_v │ │ │ │ │ +static Matrix PointCov(const Matrix &E) │ │ │ │ │ +Computes Point Covariance P from the "point Jacobian" E. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorBase.h:274 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorParams.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s_:_:_l_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e │ │ │ │ │ +LinearizationMode linearizationMode │ │ │ │ │ +How to linearize the factor. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorParams.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s_:_:_d_e_g_e_n_e_r_a_c_y_M_o_d_e │ │ │ │ │ +DegeneracyMode degeneracyMode │ │ │ │ │ +How to linearize the factor. │ │ │ │ │ +DDeeffiinniittiioonn SmartFactorParams.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +SmartProjectionFactor: triangulates point and keeps an estimate of it around. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:103 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_r_e_s_u_l_t__ │ │ │ │ │ +TriangulationResult result_ │ │ │ │ │ +result from triangulateSafe │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:63 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_t_r_i_a_n_g_u_l_a_t_e_S_a_f_e │ │ │ │ │ +TriangulationResult triangulateSafe(const Cameras &cameras) const │ │ │ │ │ +Call gtsam::triangulateSafe iff we need to re-triangulate. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:174 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_t_o_t_a_l_R_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r │ │ │ │ │ +double totalReprojectionError(const Cameras &cameras, boost::optional< Point3 > │ │ │ │ │ +externalPoint=boost::none) const │ │ │ │ │ +Calculate the error of the factor. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:411 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const NonlinearFactor &p, double tol=1e-9) const override │ │ │ │ │ +equals │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:115 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_C_a_m_e_r_a │ │ │ │ │ +CAMERA Camera │ │ │ │ │ +shorthand for a set of cameras │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionFactor.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r │ │ │ │ │ +If you are using the factor, please cite: L. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionRigFactor.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_n_o_n_U_n_i_q_u_e_K_e_y_s__ │ │ │ │ │ +KeyVector nonUniqueKeys_ │ │ │ │ │ +vector of keys (one for each observation) with potentially repeated keys │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionRigFactor.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_c_a_m_e_r_a_I_d_s__ │ │ │ │ │ +FastVector< size_t > cameraIds_ │ │ │ │ │ +vector of camera Ids (one for each observation, in the same order), identifying │ │ │ │ │ +which camera took the... │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionRigFactor.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_c_a_m_e_r_a_I_d_s │ │ │ │ │ +const FastVector< size_t > & cameraIds() const │ │ │ │ │ +return the calibration object │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionRigFactor.h:177 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_c_r_e_a_t_e_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +boost::shared_ptr< RegularHessianFactor< DimPose > > createHessianFactor(const │ │ │ │ │ +Values &values, const double &lambda=0.0, bool diagonalDamping=false) const │ │ │ │ │ +linearize and return a Hessianfactor that is an approximation of error(p) │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionRigFactor.h:265 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_a_d_d │ │ │ │ │ +void add(const MEASUREMENT &measured, const Key &poseKey, const size_t │ │ │ │ │ +&cameraId=0) │ │ │ │ │ +add a new measurement, corresponding to an observation from pose "poseKey" and │ │ │ │ │ +taken from the camera ... │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionRigFactor.h:123 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shorthand for a smart pointer to a factor │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionRigFactor.h:81 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_c_o_m_p_u_t_e_J_a_c_o_b_i_a_n_s_W_i_t_h_T_r_i_a_n_g_u_l_a_t_e_d_P_o_i_n_t │ │ │ │ │ +void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks &Fs, Matrix │ │ │ │ │ +&E, Vector &b, const Cameras &cameras) const │ │ │ │ │ +Compute jacobian F, E and error vector at a given linearization point. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionRigFactor.h:247 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_a_d_d │ │ │ │ │ +void add(const MEASUREMENTS &measurements, const KeyVector &poseKeys, const │ │ │ │ │ +FastVector< size_t > &cameraIds=FastVector< size_t >()) │ │ │ │ │ +Variant of the previous "add" function in which we include multiple │ │ │ │ │ +measurements. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionRigFactor.h:149 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r │ │ │ │ │ +SmartProjectionRigFactor() │ │ │ │ │ +Default constructor, only for serialization. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionRigFactor.h:84 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_e_r_r_o_r │ │ │ │ │ +double error(const Values &values) const override │ │ │ │ │ +error calculates the error of the factor. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionRigFactor.h:230 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_c_a_m_e_r_a_s │ │ │ │ │ +Base::Cameras cameras(const Values &values) const override │ │ │ │ │ +Collect all cameras involved in this factor. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionRigFactor.h:212 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_~_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r │ │ │ │ │ +~SmartProjectionRigFactor() override=default │ │ │ │ │ +Virtual destructor. │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionRigFactor.h:184 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e │ │ │ │ │ +boost::shared_ptr< GaussianFactor > linearize(const Values &values) const │ │ │ │ │ +override │ │ │ │ │ +linearize │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionRigFactor.h:349 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionRigFactor.h:356 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_n_o_n_U_n_i_q_u_e_K_e_y_s │ │ │ │ │ +const KeyVector & nonUniqueKeys() const │ │ │ │ │ +return (for each observation) the (possibly non unique) keys involved in the │ │ │ │ │ +measurements │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionRigFactor.h:171 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_c_a_m_e_r_a_R_i_g__ │ │ │ │ │ +boost::shared_ptr< typename Base::Cameras > cameraRig_ │ │ │ │ │ +cameras in the rig (fixed poses wrt body and intrinsics, for each camera) │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionRigFactor.h:68 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r │ │ │ │ │ +SmartProjectionRigFactor(const SharedNoiseModel &sharedNoiseModel, const │ │ │ │ │ +boost::shared_ptr< Cameras > &cameraRig, const SmartProjectionParams │ │ │ │ │ +¶ms=SmartProjectionParams()) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionRigFactor.h:94 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_c_a_m_e_r_a_R_i_g │ │ │ │ │ +const boost::shared_ptr< Cameras > & cameraRig() const │ │ │ │ │ +return the calibration object │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionRigFactor.h:174 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e_D_a_m_p_e_d │ │ │ │ │ +boost::shared_ptr< GaussianFactor > linearizeDamped(const Values &values, const │ │ │ │ │ +double &lambda=0.0) const │ │ │ │ │ +Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM) │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionRigFactor.h:335 │ │ │ │ │ +_g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const NonlinearFactor &p, double tol=1e-9) const override │ │ │ │ │ +equals │ │ │ │ │ +DDeeffiinniittiioonn SmartProjectionRigFactor.h:198 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _I_n_i_t_i_a_l_i_z_e_P_o_s_e_._h │ │ │ │ │ + * _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_R_i_g_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01334_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/PoseTranslationPrior.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/ReferenceFrameFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,142 +98,153 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
PoseTranslationPrior.h
│ │ │ │ +
ReferenceFrameFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1
│ │ │ │ -
10#pragma once
│ │ │ │ +
1/* ----------------------------------------------------------------------------
│ │ │ │ +
2
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
4 * Atlanta, Georgia 30332-0415
│ │ │ │ +
5 * All Rights Reserved
│ │ │ │ +
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
7
│ │ │ │ +
8 * See LICENSE for the license information
│ │ │ │ +
9
│ │ │ │ +
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
12#include <gtsam/geometry/concepts.h>
│ │ │ │ -
13#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
14
│ │ │ │ -
15namespace gtsam {
│ │ │ │ -
16
│ │ │ │ -
20template<class POSE>
│ │ │ │ -
│ │ │ │ -
21class PoseTranslationPrior : public NoiseModelFactorN<POSE> {
│ │ │ │ -
22public:
│ │ │ │ -
23 typedef PoseTranslationPrior<POSE> This;
│ │ │ │ -
24 typedef NoiseModelFactorN<POSE> Base;
│ │ │ │ -
25 typedef POSE Pose;
│ │ │ │ -
26 typedef typename POSE::Translation Translation;
│ │ │ │ -
27 typedef typename POSE::Rotation Rotation;
│ │ │ │ -
28
│ │ │ │ -
29 GTSAM_CONCEPT_POSE_TYPE(Pose)
│ │ │ │ -
30 GTSAM_CONCEPT_GROUP_TYPE(Pose)
│ │ │ │ -
31 GTSAM_CONCEPT_LIE_TYPE(Translation)
│ │ │ │ -
32
│ │ │ │ -
33protected:
│ │ │ │ -
34
│ │ │ │ -
35 Translation measured_;
│ │ │ │ +
12/*
│ │ │ │ +
13 * @file ReferenceFrameFactor.h
│ │ │ │ +
14 * @brief A constraint for combining graphs by common landmarks and a transform node
│ │ │ │ +
15 * @author Alex Cunningham
│ │ │ │ +
16 */
│ │ │ │ +
17
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
21
│ │ │ │ +
22namespace gtsam {
│ │ │ │ +
23
│ │ │ │ +
29template<class T, class P>
│ │ │ │ +
│ │ │ │ +
30P transform_point(
│ │ │ │ +
31 const T& trans, const P& global,
│ │ │ │ +
32 boost::optional<Matrix&> Dtrans,
│ │ │ │ +
33 boost::optional<Matrix&> Dglobal) {
│ │ │ │ +
34 return trans.transformFrom(global, Dtrans, Dglobal);
│ │ │ │ +
35}
│ │ │ │ +
│ │ │ │
36
│ │ │ │ -
37public:
│ │ │ │ -
38
│ │ │ │ -
40 PoseTranslationPrior() {}
│ │ │ │ -
41
│ │ │ │ -
│ │ │ │ -
43 PoseTranslationPrior(Key key, const Translation& measured, const noiseModel::Base::shared_ptr& model)
│ │ │ │ -
44 : Base(model, key), measured_(measured) {
│ │ │ │ -
45 }
│ │ │ │ -
│ │ │ │ -
46
│ │ │ │ -
│ │ │ │ -
48 PoseTranslationPrior(Key key, const POSE& pose_z, const noiseModel::Base::shared_ptr& model)
│ │ │ │ -
49 : Base(model, key), measured_(pose_z.translation()) {
│ │ │ │ -
50 }
│ │ │ │ -
│ │ │ │ -
51
│ │ │ │ -
52 ~PoseTranslationPrior() override {}
│ │ │ │ -
53
│ │ │ │ -
54 const Translation& measured() const { return measured_; }
│ │ │ │ -
55
│ │ │ │ -
│ │ │ │ -
57 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
58 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ -
59 gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
│ │ │ │ -
│ │ │ │ -
60
│ │ │ │ -
│ │ │ │ -
62 Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const override {
│ │ │ │ -
63 const Translation& newTrans = pose.translation();
│ │ │ │ -
64 const Rotation& R = pose.rotation();
│ │ │ │ -
65 const int tDim = traits<Translation>::GetDimension(newTrans);
│ │ │ │ -
66 const int xDim = traits<Pose>::GetDimension(pose);
│ │ │ │ -
67 if (H) {
│ │ │ │ -
68 *H = Matrix::Zero(tDim, xDim);
│ │ │ │ -
69 std::pair<size_t, size_t> transInterval = POSE::translationInterval();
│ │ │ │ -
70 (*H).middleCols(transInterval.first, tDim) = R.matrix();
│ │ │ │ -
71 }
│ │ │ │ -
72
│ │ │ │ -
73 return traits<Translation>::Local(measured_, newTrans);
│ │ │ │ -
74 }
│ │ │ │ -
│ │ │ │ -
75
│ │ │ │ -
│ │ │ │ -
77 bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
│ │ │ │ -
78 const This *e = dynamic_cast<const This*> (&expected);
│ │ │ │ -
79 return e != nullptr && Base::equals(*e, tol) && traits<Translation>::Equals(measured_, e->measured_, tol);
│ │ │ │ -
80 }
│ │ │ │ +
56template<class POINT, class TRANSFORM>
│ │ │ │ +
│ │ │ │ +
57class ReferenceFrameFactor : public NoiseModelFactorN<POINT, TRANSFORM, POINT> {
│ │ │ │ +
58protected:
│ │ │ │ +
60 ReferenceFrameFactor() {}
│ │ │ │ +
61
│ │ │ │ +
62public:
│ │ │ │ +
63 typedef NoiseModelFactorN<POINT, TRANSFORM, POINT> Base;
│ │ │ │ +
64 typedef ReferenceFrameFactor<POINT, TRANSFORM> This;
│ │ │ │ +
65
│ │ │ │ +
66 typedef POINT Point;
│ │ │ │ +
67 typedef TRANSFORM Transform;
│ │ │ │ +
68
│ │ │ │ +
│ │ │ │ +
72 ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, const noiseModel::Base::shared_ptr& model)
│ │ │ │ +
73 : Base(model,globalKey, transKey, localKey) {}
│ │ │ │ +
│ │ │ │ +
74
│ │ │ │ +
│ │ │ │ +
79 ReferenceFrameFactor(double mu, Key globalKey, Key transKey, Key localKey)
│ │ │ │ +
80 : Base(globalKey, transKey, localKey, Point().dim(), mu) {}
│ │ │ │
│ │ │ │
81
│ │ │ │ -
│ │ │ │ -
83 void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ -
84 Base::print(s + "PoseTranslationPrior", keyFormatter);
│ │ │ │ -
85 traits<Translation>::Print(measured_, "Measured Translation");
│ │ │ │ -
86 }
│ │ │ │ +
│ │ │ │ +
86 ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, double sigma = 1e-2)
│ │ │ │ +
87 : Base(noiseModel::Isotropic::Sigma(traits<POINT>::dimension, sigma),
│ │ │ │ +
88 globalKey, transKey, localKey) {}
│ │ │ │
│ │ │ │ -
87
│ │ │ │ -
88private:
│ │ │ │
89
│ │ │ │ -
91 friend class boost::serialization::access;
│ │ │ │ -
92 template<class ARCHIVE>
│ │ │ │ -
93 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
94 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ -
95 ar & boost::serialization::make_nvp("NoiseModelFactor1",
│ │ │ │ -
96 boost::serialization::base_object<Base>(*this));
│ │ │ │ -
97 ar & BOOST_SERIALIZATION_NVP(measured_);
│ │ │ │ -
98 }
│ │ │ │ -
99
│ │ │ │ -
100};
│ │ │ │ -
│ │ │ │ -
101
│ │ │ │ -
102} // \namespace gtsam
│ │ │ │ -
103
│ │ │ │ -
104
│ │ │ │ -
105
│ │ │ │ +
90 ~ReferenceFrameFactor() override{}
│ │ │ │ +
91
│ │ │ │ +
│ │ │ │ +
92 NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
93 return boost::static_pointer_cast<NonlinearFactor>(
│ │ │ │ +
94 NonlinearFactor::shared_ptr(new This(*this))); }
│ │ │ │ +
│ │ │ │ +
95
│ │ │ │ +
│ │ │ │ +
97 Vector evaluateError(const Point& global, const Transform& trans, const Point& local,
│ │ │ │ +
98 boost::optional<Matrix&> Dforeign = boost::none,
│ │ │ │ +
99 boost::optional<Matrix&> Dtrans = boost::none,
│ │ │ │ +
100 boost::optional<Matrix&> Dlocal = boost::none) const override {
│ │ │ │ +
101 Point newlocal = transform_point<Transform,Point>(trans, global, Dtrans, Dforeign);
│ │ │ │ +
102 if (Dlocal)
│ │ │ │ +
103 *Dlocal = -1* Matrix::Identity(traits<Point>::dimension, traits<Point>::dimension);
│ │ │ │ +
104 return traits<Point>::Local(local,newlocal);
│ │ │ │ +
105 }
│ │ │ │ +
│ │ │ │
106
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
│ │ │ │ +
107 void print(const std::string& s="",
│ │ │ │ +
108 const gtsam::KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ +
109 std::cout << s << ": ReferenceFrameFactor("
│ │ │ │ +
110 << "Global: " << keyFormatter(this->key1()) << ","
│ │ │ │ +
111 << " Transform: " << keyFormatter(this->key2()) << ","
│ │ │ │ +
112 << " Local: " << keyFormatter(this->key3()) << ")\n";
│ │ │ │ +
113 this->noiseModel_->print(" noise model");
│ │ │ │ +
114 }
│ │ │ │ +
│ │ │ │ +
115
│ │ │ │ +
116 // access - convenience functions
│ │ │ │ +
117 Key global_key() const { return this->key1(); }
│ │ │ │ +
118 Key transform_key() const { return this->key2(); }
│ │ │ │ +
119 Key local_key() const { return this->key3(); }
│ │ │ │ +
120
│ │ │ │ +
121private:
│ │ │ │ +
123 friend class boost::serialization::access;
│ │ │ │ +
124 template<class ARCHIVE>
│ │ │ │ +
125 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
126 ar & boost::serialization::make_nvp("NonlinearFactor3",
│ │ │ │ +
127 boost::serialization::base_object<Base>(*this));
│ │ │ │ +
128 }
│ │ │ │ +
129};
│ │ │ │ +
│ │ │ │ +
130
│ │ │ │ +
132template<class T1, class T2>
│ │ │ │ +
133struct traits<ReferenceFrameFactor<T1, T2> > : public Testable<ReferenceFrameFactor<T1, T2> > {};
│ │ │ │ +
134
│ │ │ │ +
135} // \namespace gtsam
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::transform_point
P transform_point(const T &trans, const P &global, boost::optional< Matrix & > Dtrans, boost::optional< Matrix & > Dglobal)
Transform function that must be specialized specific domains.
Definition ReferenceFrameFactor.h:30
│ │ │ │ +
gtsam::trans
Matrix trans(const Matrix &A)
static transpose function, just calls Eigen transpose member function
Definition Matrix.h:242
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
│ │ │ │ -
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
│ │ │ │ -
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::NoiseModelFactor::dim
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition NonlinearFactor.h:218
│ │ │ │ +
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition NonlinearFactor.h:223
│ │ │ │
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ -
gtsam::NoiseModelFactorN< POSE >::key
Key key() const
Returns a key.
Definition NonlinearFactor.h:518
│ │ │ │ -
gtsam::PoseTranslationPrior
A prior on the translation part of a pose.
Definition PoseTranslationPrior.h:21
│ │ │ │ -
gtsam::PoseTranslationPrior::PoseTranslationPrior
PoseTranslationPrior(Key key, const Translation &measured, const noiseModel::Base::shared_ptr &model)
standard constructor
Definition PoseTranslationPrior.h:43
│ │ │ │ -
gtsam::PoseTranslationPrior::PoseTranslationPrior
PoseTranslationPrior(Key key, const POSE &pose_z, const noiseModel::Base::shared_ptr &model)
Constructor that pulls the translation from an incoming POSE.
Definition PoseTranslationPrior.h:48
│ │ │ │ -
gtsam::PoseTranslationPrior::evaluateError
Vector evaluateError(const Pose &pose, boost::optional< Matrix & > H=boost::none) const override
h(x)-z
Definition PoseTranslationPrior.h:62
│ │ │ │ -
gtsam::PoseTranslationPrior::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
equals specialized to this factor
Definition PoseTranslationPrior.h:77
│ │ │ │ -
gtsam::PoseTranslationPrior::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition PoseTranslationPrior.h:57
│ │ │ │ -
gtsam::PoseTranslationPrior::access
friend class boost::serialization::access
Serialization function.
Definition PoseTranslationPrior.h:91
│ │ │ │ -
gtsam::PoseTranslationPrior::PoseTranslationPrior
PoseTranslationPrior()
default constructor - only use for serialization
Definition PoseTranslationPrior.h:40
│ │ │ │ -
gtsam::PoseTranslationPrior::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print contents
Definition PoseTranslationPrior.h:83
│ │ │ │ -
concepts.h
Concept-checking macros for geometric objects Each macro instantiates a concept check structure,...
│ │ │ │ +
gtsam::ReferenceFrameFactor
A constraint between two landmarks in separate maps Templated on: Point : Type of landmark Transform ...
Definition ReferenceFrameFactor.h:57
│ │ │ │ +
gtsam::ReferenceFrameFactor::print
void print(const std::string &s="", const gtsam::KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition ReferenceFrameFactor.h:107
│ │ │ │ +
gtsam::ReferenceFrameFactor::ReferenceFrameFactor
ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, const noiseModel::Base::shared_ptr &model)
General constructor with arbitrary noise model (constrained or otherwise)
Definition ReferenceFrameFactor.h:72
│ │ │ │ +
gtsam::ReferenceFrameFactor::evaluateError
Vector evaluateError(const Point &global, const Transform &trans, const Point &local, boost::optional< Matrix & > Dforeign=boost::none, boost::optional< Matrix & > Dtrans=boost::none, boost::optional< Matrix & > Dlocal=boost::none) const override
Combined cost and derivative function using boost::optional.
Definition ReferenceFrameFactor.h:97
│ │ │ │ +
gtsam::ReferenceFrameFactor::clone
NonlinearFactor::shared_ptr clone() const override
Creates a shared_ptr clone of the factor - needs to be specialized to allow for subclasses.
Definition ReferenceFrameFactor.h:92
│ │ │ │ +
gtsam::ReferenceFrameFactor::ReferenceFrameFactor
ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, double sigma=1e-2)
Simple soft constraint constructor for frame of reference, with equal weighting for each degree of fr...
Definition ReferenceFrameFactor.h:86
│ │ │ │ +
gtsam::ReferenceFrameFactor::ReferenceFrameFactor
ReferenceFrameFactor(double mu, Key globalKey, Key transKey, Key localKey)
Construct a hard frame of reference reference constraint with equal mu values for each degree of free...
Definition ReferenceFrameFactor.h:79
│ │ │ │ +
gtsam::ReferenceFrameFactor::access
friend class boost::serialization::access
Serialization function.
Definition ReferenceFrameFactor.h:123
│ │ │ │ +
gtsam::ReferenceFrameFactor::ReferenceFrameFactor
ReferenceFrameFactor()
default constructor for serialization only
Definition ReferenceFrameFactor.h:60
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,182 +1,203 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -PoseTranslationPrior.h │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -1 │ │ │ │ │ -10#pragma once │ │ │ │ │ +ReferenceFrameFactor.h │ │ │ │ │ +1/* --------------------------------------------------------------------------- │ │ │ │ │ +- │ │ │ │ │ +2 │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +5 * All Rights Reserved │ │ │ │ │ +6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +7 │ │ │ │ │ +8 * See LICENSE for the license information │ │ │ │ │ +9 │ │ │ │ │ +10 * ------------------------------------------------------------------------- │ │ │ │ │ +- */ │ │ │ │ │ 11 │ │ │ │ │ -12#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_c_o_n_c_e_p_t_s_._h> │ │ │ │ │ -13#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -14 │ │ │ │ │ -15namespace _g_t_s_a_m { │ │ │ │ │ -16 │ │ │ │ │ -20template │ │ │ │ │ -_2_1class _P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -22public: │ │ │ │ │ -23 typedef _P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r_<_P_O_S_E_> This; │ │ │ │ │ -24 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_P_O_S_E_> _B_a_s_e; │ │ │ │ │ -25 typedef POSE Pose; │ │ │ │ │ -26 typedef typename POSE::Translation Translation; │ │ │ │ │ -27 typedef typename POSE::Rotation Rotation; │ │ │ │ │ -28 │ │ │ │ │ -29 GTSAM_CONCEPT_POSE_TYPE(Pose) │ │ │ │ │ -30 GTSAM_CONCEPT_GROUP_TYPE(Pose) │ │ │ │ │ -31 GTSAM_CONCEPT_LIE_TYPE(Translation) │ │ │ │ │ -32 │ │ │ │ │ -33protected: │ │ │ │ │ -34 │ │ │ │ │ -35 Translation measured_; │ │ │ │ │ +12/* │ │ │ │ │ +13 * @file ReferenceFrameFactor.h │ │ │ │ │ +14 * @brief A constraint for combining graphs by common landmarks and a │ │ │ │ │ +transform node │ │ │ │ │ +15 * @author Alex Cunningham │ │ │ │ │ +16 */ │ │ │ │ │ +17 │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +21 │ │ │ │ │ +22namespace _g_t_s_a_m { │ │ │ │ │ +23 │ │ │ │ │ +29template │ │ │ │ │ +_3_0P _t_r_a_n_s_f_o_r_m___p_o_i_n_t( │ │ │ │ │ +31 const T& _t_r_a_n_s, const P& global, │ │ │ │ │ +32 boost::optional Dtrans, │ │ │ │ │ +33 boost::optional Dglobal) { │ │ │ │ │ +34 return _t_r_a_n_s.transformFrom(global, Dtrans, Dglobal); │ │ │ │ │ +35} │ │ │ │ │ 36 │ │ │ │ │ -37public: │ │ │ │ │ -38 │ │ │ │ │ -_4_0 _P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r() {} │ │ │ │ │ -41 │ │ │ │ │ -_4_3 _P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r(_K_e_y _k_e_y, const Translation& measured, const │ │ │ │ │ +56template │ │ │ │ │ +_5_7class _R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +58protected: │ │ │ │ │ +_6_0 _R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r() {} │ │ │ │ │ +61 │ │ │ │ │ +62public: │ │ │ │ │ +63 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_P_O_I_N_T_,_ _T_R_A_N_S_F_O_R_M_,_ _P_O_I_N_T_> Base; │ │ │ │ │ +64 typedef _R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r_<_P_O_I_N_T_,_ _T_R_A_N_S_F_O_R_M_> This; │ │ │ │ │ +65 │ │ │ │ │ +66 typedef POINT Point; │ │ │ │ │ +67 typedef TRANSFORM Transform; │ │ │ │ │ +68 │ │ │ │ │ +_7_2 _R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r(_K_e_y globalKey, _K_e_y transKey, _K_e_y localKey, const │ │ │ │ │ noiseModel::Base::shared_ptr& model) │ │ │ │ │ -44 : _B_a_s_e(model, _k_e_y), measured_(measured) { │ │ │ │ │ -45 } │ │ │ │ │ -46 │ │ │ │ │ -_4_8 _P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r(_K_e_y _k_e_y, const POSE& pose_z, const noiseModel::Base:: │ │ │ │ │ -shared_ptr& model) │ │ │ │ │ -49 : _B_a_s_e(model, _k_e_y), measured_(pose_z.translation()) { │ │ │ │ │ -50 } │ │ │ │ │ -51 │ │ │ │ │ -52 _~_P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r() override {} │ │ │ │ │ -53 │ │ │ │ │ -54 const Translation& measured() const { return measured_; } │ │ │ │ │ -55 │ │ │ │ │ -_5_7 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -58 return boost::static_pointer_cast( │ │ │ │ │ -59 gtsam::NonlinearFactor::shared_ptr(new This(*this))); } │ │ │ │ │ -60 │ │ │ │ │ -_6_2 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const Pose& pose, boost::optional H = boost:: │ │ │ │ │ -none) const override { │ │ │ │ │ -63 const Translation& newTrans = pose.translation(); │ │ │ │ │ -64 const Rotation& R = pose.rotation(); │ │ │ │ │ -65 const int tDim = _t_r_a_i_t_s_<_T_r_a_n_s_l_a_t_i_o_n_>_:_:_G_e_t_D_i_m_e_n_s_i_o_n(newTrans); │ │ │ │ │ -66 const int xDim = _t_r_a_i_t_s_<_P_o_s_e_>_:_:_G_e_t_D_i_m_e_n_s_i_o_n(pose); │ │ │ │ │ -67 if (H) { │ │ │ │ │ -68 *H = Matrix::Zero(tDim, xDim); │ │ │ │ │ -69 std::pair transInterval = POSE::translationInterval(); │ │ │ │ │ -70 (*H).middleCols(transInterval.first, tDim) = R.matrix(); │ │ │ │ │ -71 } │ │ │ │ │ -72 │ │ │ │ │ -73 return _t_r_a_i_t_s_<_T_r_a_n_s_l_a_t_i_o_n_>_:_:_L_o_c_a_l(measured_, newTrans); │ │ │ │ │ -74 } │ │ │ │ │ -75 │ │ │ │ │ -_7_7 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& expected, double tol=1e-9) const override │ │ │ │ │ -{ │ │ │ │ │ -78 const This *e = dynamic_cast (&expected); │ │ │ │ │ -79 return e != nullptr && _B_a_s_e_:_:_e_q_u_a_l_s(*e, tol) && _t_r_a_i_t_s_<_T_r_a_n_s_l_a_t_i_o_n_>_:_:_E_q_u_a_l_s │ │ │ │ │ -(measured_, e->measured_, tol); │ │ │ │ │ -80 } │ │ │ │ │ +73 : _B_a_s_e(model,globalKey, transKey, localKey) {} │ │ │ │ │ +74 │ │ │ │ │ +_7_9 _R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r(double mu, _K_e_y globalKey, _K_e_y transKey, _K_e_y localKey) │ │ │ │ │ +80 : _B_a_s_e(globalKey, transKey, localKey, Point()._d_i_m(), mu) {} │ │ │ │ │ 81 │ │ │ │ │ -_8_3 void _p_r_i_n_t(const std::string& s="", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ -DefaultKeyFormatter) const override { │ │ │ │ │ -84 _B_a_s_e_:_:_p_r_i_n_t(s + "PoseTranslationPrior", keyFormatter); │ │ │ │ │ -85 _t_r_a_i_t_s_<_T_r_a_n_s_l_a_t_i_o_n_>_:_:_P_r_i_n_t(measured_, "Measured Translation"); │ │ │ │ │ -86 } │ │ │ │ │ -87 │ │ │ │ │ -88private: │ │ │ │ │ +_8_6 _R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r(_K_e_y globalKey, _K_e_y transKey, _K_e_y localKey, double sigma │ │ │ │ │ += 1e-2) │ │ │ │ │ +87 : _B_a_s_e(_n_o_i_s_e_M_o_d_e_l::Isotropic::Sigma(_t_r_a_i_t_s::dimension, sigma), │ │ │ │ │ +88 globalKey, transKey, localKey) {} │ │ │ │ │ 89 │ │ │ │ │ -_9_1 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -92 template │ │ │ │ │ -93 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -94 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility │ │ │ │ │ -95 ar & boost::serialization::make_nvp("NoiseModelFactor1", │ │ │ │ │ -96 boost::serialization::base_object(*this)); │ │ │ │ │ -97 ar & BOOST_SERIALIZATION_NVP(measured_); │ │ │ │ │ -98 } │ │ │ │ │ -99 │ │ │ │ │ -100}; │ │ │ │ │ -101 │ │ │ │ │ -102} // \namespace gtsam │ │ │ │ │ -103 │ │ │ │ │ -104 │ │ │ │ │ -105 │ │ │ │ │ +90 _~_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r() override{} │ │ │ │ │ +91 │ │ │ │ │ +_9_2 NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +93 return boost::static_pointer_cast( │ │ │ │ │ +94 NonlinearFactor::shared_ptr(new This(*this))); } │ │ │ │ │ +95 │ │ │ │ │ +_9_7 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const Point& global, const Transform& _t_r_a_n_s, const │ │ │ │ │ +Point& local, │ │ │ │ │ +98 boost::optional Dforeign = boost::none, │ │ │ │ │ +99 boost::optional Dtrans = boost::none, │ │ │ │ │ +100 boost::optional Dlocal = boost::none) const override { │ │ │ │ │ +101 Point newlocal = transform_point(_t_r_a_n_s, global, Dtrans, │ │ │ │ │ +Dforeign); │ │ │ │ │ +102 if (Dlocal) │ │ │ │ │ +103 *Dlocal = -1* Matrix::Identity(_t_r_a_i_t_s_<_P_o_i_n_t_>_:_:_d_i_m_e_n_s_i_o_n, _t_r_a_i_t_s_<_P_o_i_n_t_>_:_: │ │ │ │ │ +_d_i_m_e_n_s_i_o_n); │ │ │ │ │ +104 return _t_r_a_i_t_s_<_P_o_i_n_t_>_:_:_L_o_c_a_l(local,newlocal); │ │ │ │ │ +105 } │ │ │ │ │ 106 │ │ │ │ │ +_1_0_7 void _p_r_i_n_t(const std::string& s="", │ │ │ │ │ +108 const _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const │ │ │ │ │ +override { │ │ │ │ │ +109 std::cout << s << ": ReferenceFrameFactor(" │ │ │ │ │ +110 << "Global: " << keyFormatter(this->key1()) << "," │ │ │ │ │ +111 << " Transform: " << keyFormatter(this->key2()) << "," │ │ │ │ │ +112 << " Local: " << keyFormatter(this->key3()) << ")\n"; │ │ │ │ │ +113 this->noiseModel_->print(" noise model"); │ │ │ │ │ +114 } │ │ │ │ │ +115 │ │ │ │ │ +116 // access - convenience functions │ │ │ │ │ +117 _K_e_y global_key() const { return this->key1(); } │ │ │ │ │ +118 _K_e_y transform_key() const { return this->key2(); } │ │ │ │ │ +119 _K_e_y local_key() const { return this->key3(); } │ │ │ │ │ +120 │ │ │ │ │ +121private: │ │ │ │ │ +_1_2_3 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +124 template │ │ │ │ │ +125 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +126 ar & boost::serialization::make_nvp("NonlinearFactor3", │ │ │ │ │ +127 boost::serialization::base_object(*this)); │ │ │ │ │ +128 } │ │ │ │ │ +129}; │ │ │ │ │ +130 │ │ │ │ │ +132template │ │ │ │ │ +_1_3_3struct _t_r_a_i_t_s<_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r > : public │ │ │ │ │ +_T_e_s_t_a_b_l_e > {}; │ │ │ │ │ +134 │ │ │ │ │ +135} // \namespace gtsam │ │ │ │ │ _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_n_s_f_o_r_m___p_o_i_n_t │ │ │ │ │ +P transform_point(const T &trans, const P &global, boost::optional< Matrix & > │ │ │ │ │ +Dtrans, boost::optional< Matrix & > Dglobal) │ │ │ │ │ +Transform function that must be specialized specific domains. │ │ │ │ │ +DDeeffiinniittiioonn ReferenceFrameFactor.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_n_s │ │ │ │ │ +Matrix trans(const Matrix &A) │ │ │ │ │ +static transpose function, just calls Eigen transpose member function │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:242 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ -virtual void print(const std::string &s="Factor", const KeyFormatter │ │ │ │ │ -&formatter=DefaultKeyFormatter) const │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn Factor.cpp:29 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const This &other, double tol=1e-9) const │ │ │ │ │ -check equality │ │ │ │ │ -DDeeffiinniittiioonn Factor.cpp:42 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ -Nonlinear factor base class. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_d_i_m │ │ │ │ │ +size_t dim() const override │ │ │ │ │ +get the dimension of the factor (number of rows on linearization) │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:218 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_n_o_i_s_e_M_o_d_e_l │ │ │ │ │ +const SharedNoiseModel & noiseModel() const │ │ │ │ │ +access to the noise model │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:223 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ variables. │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_ _P_O_S_E_ _>_:_:_k_e_y │ │ │ │ │ -Key key() const │ │ │ │ │ -Returns a key. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:518 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r │ │ │ │ │ -A prior on the translation part of a pose. │ │ │ │ │ -DDeeffiinniittiioonn PoseTranslationPrior.h:21 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r_:_:_P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r │ │ │ │ │ -PoseTranslationPrior(Key key, const Translation &measured, const noiseModel:: │ │ │ │ │ -Base::shared_ptr &model) │ │ │ │ │ -standard constructor │ │ │ │ │ -DDeeffiinniittiioonn PoseTranslationPrior.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r_:_:_P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r │ │ │ │ │ -PoseTranslationPrior(Key key, const POSE &pose_z, const noiseModel::Base:: │ │ │ │ │ -shared_ptr &model) │ │ │ │ │ -Constructor that pulls the translation from an incoming POSE. │ │ │ │ │ -DDeeffiinniittiioonn PoseTranslationPrior.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ -Vector evaluateError(const Pose &pose, boost::optional< Matrix & > H=boost:: │ │ │ │ │ -none) const override │ │ │ │ │ -h(x)-z │ │ │ │ │ -DDeeffiinniittiioonn PoseTranslationPrior.h:62 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const NonlinearFactor &expected, double tol=1e-9) const override │ │ │ │ │ -equals specialized to this factor │ │ │ │ │ -DDeeffiinniittiioonn PoseTranslationPrior.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r_:_:_c_l_o_n_e │ │ │ │ │ -gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn PoseTranslationPrior.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r │ │ │ │ │ +A constraint between two landmarks in separate maps Templated on: Point : Type │ │ │ │ │ +of landmark Transform ... │ │ │ │ │ +DDeeffiinniittiioonn ReferenceFrameFactor.h:57 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const gtsam::KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +Print. │ │ │ │ │ +DDeeffiinniittiioonn ReferenceFrameFactor.h:107 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r_:_:_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r │ │ │ │ │ +ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, const │ │ │ │ │ +noiseModel::Base::shared_ptr &model) │ │ │ │ │ +General constructor with arbitrary noise model (constrained or otherwise) │ │ │ │ │ +DDeeffiinniittiioonn ReferenceFrameFactor.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ +Vector evaluateError(const Point &global, const Transform &trans, const Point │ │ │ │ │ +&local, boost::optional< Matrix & > Dforeign=boost::none, boost::optional< │ │ │ │ │ +Matrix & > Dtrans=boost::none, boost::optional< Matrix & > Dlocal=boost::none) │ │ │ │ │ +const override │ │ │ │ │ +Combined cost and derivative function using boost::optional. │ │ │ │ │ +DDeeffiinniittiioonn ReferenceFrameFactor.h:97 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ +NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +Creates a shared_ptr clone of the factor - needs to be specialized to allow for │ │ │ │ │ +subclasses. │ │ │ │ │ +DDeeffiinniittiioonn ReferenceFrameFactor.h:92 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r_:_:_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r │ │ │ │ │ +ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, double │ │ │ │ │ +sigma=1e-2) │ │ │ │ │ +Simple soft constraint constructor for frame of reference, with equal weighting │ │ │ │ │ +for each degree of fr... │ │ │ │ │ +DDeeffiinniittiioonn ReferenceFrameFactor.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r_:_:_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r │ │ │ │ │ +ReferenceFrameFactor(double mu, Key globalKey, Key transKey, Key localKey) │ │ │ │ │ +Construct a hard frame of reference reference constraint with equal mu values │ │ │ │ │ +for each degree of free... │ │ │ │ │ +DDeeffiinniittiioonn ReferenceFrameFactor.h:79 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ friend class boost::serialization::access │ │ │ │ │ Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn PoseTranslationPrior.h:91 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r_:_:_P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r │ │ │ │ │ -PoseTranslationPrior() │ │ │ │ │ -default constructor - only use for serialization │ │ │ │ │ -DDeeffiinniittiioonn PoseTranslationPrior.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -print contents │ │ │ │ │ -DDeeffiinniittiioonn PoseTranslationPrior.h:83 │ │ │ │ │ -_c_o_n_c_e_p_t_s_._h │ │ │ │ │ -Concept-checking macros for geometric objects Each macro instantiates a concept │ │ │ │ │ -check structure,... │ │ │ │ │ +DDeeffiinniittiioonn ReferenceFrameFactor.h:123 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r_:_:_R_e_f_e_r_e_n_c_e_F_r_a_m_e_F_a_c_t_o_r │ │ │ │ │ +ReferenceFrameFactor() │ │ │ │ │ +default constructor for serialization only │ │ │ │ │ +DDeeffiinniittiioonn ReferenceFrameFactor.h:60 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r_._h │ │ │ │ │ + * RReeffeerreenncceeFFrraammeeFFaaccttoorr..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01337_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/EssentialMatrixConstraint.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/JacobianFactorSVD.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,127 +98,99 @@ │ │ │ │
No Matches
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
EssentialMatrixConstraint.h
│ │ │ │ +
JacobianFactorSVD.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ -
2
│ │ │ │ -
3 * GTSAM Copyright 2010-2014, Georgia Tech Research Corporation,
│ │ │ │ -
4 * Atlanta, Georgia 30332-0415
│ │ │ │ -
5 * All Rights Reserved
│ │ │ │ -
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ -
7
│ │ │ │ -
8 * See LICENSE for the license information
│ │ │ │ -
9
│ │ │ │ -
10 * -------------------------------------------------------------------------- */
│ │ │ │ -
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
22#include <gtsam/geometry/EssentialMatrix.h>
│ │ │ │ -
23
│ │ │ │ -
24namespace gtsam {
│ │ │ │ -
25
│ │ │ │ -
│ │ │ │ -
30class GTSAM_EXPORT EssentialMatrixConstraint: public NoiseModelFactorN<Pose3, Pose3> {
│ │ │ │ -
31
│ │ │ │ -
32private:
│ │ │ │ -
33
│ │ │ │ -
34 typedef EssentialMatrixConstraint This;
│ │ │ │ -
35 typedef NoiseModelFactorN<Pose3, Pose3> Base;
│ │ │ │ +
1/*
│ │ │ │ +
2 * @file JacobianFactorSVD.h
│ │ │ │ +
3 * @date Oct 27, 2013
│ │ │ │ +
4 * @uthor Frank Dellaert
│ │ │ │ +
5 */
│ │ │ │ +
6
│ │ │ │ +
7#pragma once
│ │ │ │ +
8#include <gtsam/linear/RegularJacobianFactor.h>
│ │ │ │ +
9
│ │ │ │ +
10namespace gtsam {
│ │ │ │ +
28template<size_t D, size_t ZDim>
│ │ │ │ +
│ │ │ │ +
29class JacobianFactorSVD: public RegularJacobianFactor<D> {
│ │ │ │ +
30
│ │ │ │ +
31 typedef RegularJacobianFactor<D> Base;
│ │ │ │ +
32 typedef Eigen::Matrix<double, ZDim, D> MatrixZD; // e.g 2 x 6 with Z=Point2
│ │ │ │ +
33 typedef std::pair<Key, Matrix> KeyMatrix;
│ │ │ │ +
34
│ │ │ │ +
35public:
│ │ │ │
36
│ │ │ │ -
37 EssentialMatrix measuredE_;
│ │ │ │ -
39public:
│ │ │ │ +
│ │ │ │ +
38 JacobianFactorSVD() {
│ │ │ │ +
39 }
│ │ │ │ +
│ │ │ │
40
│ │ │ │ -
41 // shorthand for a smart pointer to a factor
│ │ │ │ -
42 typedef boost::shared_ptr<EssentialMatrixConstraint> shared_ptr;
│ │ │ │ -
43
│ │ │ │ -
│ │ │ │ -
45 EssentialMatrixConstraint() {
│ │ │ │ -
46 }
│ │ │ │ -
│ │ │ │ -
47
│ │ │ │ -
│ │ │ │ -
55 EssentialMatrixConstraint(Key key1, Key key2,
│ │ │ │ -
56 const EssentialMatrix& measuredE, const SharedNoiseModel& model) :
│ │ │ │ -
57 Base(model, key1, key2), measuredE_(measuredE) {
│ │ │ │ -
58 }
│ │ │ │ -
│ │ │ │ -
59
│ │ │ │ -
60 ~EssentialMatrixConstraint() override {
│ │ │ │ -
61 }
│ │ │ │ -
62
│ │ │ │ +
│ │ │ │ +
42 JacobianFactorSVD(const KeyVector& keys,
│ │ │ │ +
43 const SharedDiagonal& model = SharedDiagonal())
│ │ │ │ +
44 : Base() {
│ │ │ │ +
45 Matrix zeroMatrix = Matrix::Zero(0, D);
│ │ │ │ +
46 Vector zeroVector = Vector::Zero(0);
│ │ │ │ +
47 std::vector<KeyMatrix> QF;
│ │ │ │ +
48 QF.reserve(keys.size());
│ │ │ │ +
49 for(const Key& key: keys)
│ │ │ │ +
50 QF.push_back(KeyMatrix(key, zeroMatrix));
│ │ │ │ +
51 JacobianFactor::fillTerms(QF, zeroVector, model);
│ │ │ │ +
52 }
│ │ │ │ +
│ │ │ │ +
53
│ │ │ │
│ │ │ │ -
64 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
65 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ -
66 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
│ │ │ │ -
67 }
│ │ │ │ -
│ │ │ │ -
68
│ │ │ │ -
72 void print(const std::string& s = "",
│ │ │ │ -
73 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
│ │ │ │ -
74
│ │ │ │ -
76 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
│ │ │ │ -
77
│ │ │ │ -
81 Vector evaluateError(const Pose3& p1, const Pose3& p2,
│ │ │ │ -
82 boost::optional<Matrix&> Hp1 = boost::none, //
│ │ │ │ -
83 boost::optional<Matrix&> Hp2 = boost::none) const override;
│ │ │ │ -
84
│ │ │ │ -
│ │ │ │ -
86 const EssentialMatrix& measured() const {
│ │ │ │ -
87 return measuredE_;
│ │ │ │ -
88 }
│ │ │ │ -
│ │ │ │ -
89
│ │ │ │ -
90private:
│ │ │ │ -
91
│ │ │ │ -
93 friend class boost::serialization::access;
│ │ │ │ -
94 template<class ARCHIVE>
│ │ │ │ -
95 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
96 // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ -
97 ar
│ │ │ │ -
98 & boost::serialization::make_nvp("NoiseModelFactor2",
│ │ │ │ -
99 boost::serialization::base_object<Base>(*this));
│ │ │ │ -
100 ar & BOOST_SERIALIZATION_NVP(measuredE_);
│ │ │ │ -
101 }
│ │ │ │ -
102
│ │ │ │ -
103public:
│ │ │ │ -
104 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
105};
│ │ │ │ -
│ │ │ │ -
106// \class EssentialMatrixConstraint
│ │ │ │ -
107
│ │ │ │ -
108}
│ │ │ │ -
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
64 JacobianFactorSVD(
│ │ │ │ +
65 const KeyVector& keys,
│ │ │ │ +
66 const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& Fblocks,
│ │ │ │ +
67 const Matrix& Enull, const Vector& b,
│ │ │ │ +
68 const SharedDiagonal& model = SharedDiagonal())
│ │ │ │ +
69 : Base() {
│ │ │ │ +
70 size_t numKeys = Enull.rows() / ZDim;
│ │ │ │ +
71 size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()?
│ │ │ │ +
72 // PLAIN nullptr SPACE TRICK
│ │ │ │ +
73 // Matrix Q = Enull * Enull.transpose();
│ │ │ │ +
74 // for(const KeyMatrixZD& it: Fblocks)
│ │ │ │ +
75 // QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second));
│ │ │ │ +
76 // JacobianFactor factor(QF, Q * b);
│ │ │ │ +
77 std::vector<KeyMatrix> QF;
│ │ │ │ +
78 QF.reserve(numKeys);
│ │ │ │ +
79 for (size_t k = 0; k < Fblocks.size(); ++k) {
│ │ │ │ +
80 Key key = keys[k];
│ │ │ │ +
81 QF.emplace_back(
│ │ │ │ +
82 key, (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k]);
│ │ │ │ +
83 }
│ │ │ │ +
84 JacobianFactor::fillTerms(QF, Enull.transpose() * b, model);
│ │ │ │ +
85 }
│ │ │ │ +
│ │ │ │ +
86};
│ │ │ │ +
│ │ │ │ +
87
│ │ │ │ +
88}
│ │ │ │ +
RegularJacobianFactor.h
JacobianFactor class with fixed sized blcoks.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::EssentialMatrix
An essential matrix is like a Pose3, except with translation up to scale It is named after the 3*3 ma...
Definition EssentialMatrix.h:26
│ │ │ │ -
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ -
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ -
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ -
gtsam::EssentialMatrixConstraint
Binary factor between two Pose3 variables induced by an EssentialMatrix measurement.
Definition EssentialMatrixConstraint.h:30
│ │ │ │ -
gtsam::EssentialMatrixConstraint::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition EssentialMatrixConstraint.h:64
│ │ │ │ -
gtsam::EssentialMatrixConstraint::measured
const EssentialMatrix & measured() const
return the measured
Definition EssentialMatrixConstraint.h:86
│ │ │ │ -
gtsam::EssentialMatrixConstraint::shared_ptr
boost::shared_ptr< EssentialMatrixConstraint > shared_ptr
The measurement is an essential matrix.
Definition EssentialMatrixConstraint.h:42
│ │ │ │ -
gtsam::EssentialMatrixConstraint::EssentialMatrixConstraint
EssentialMatrixConstraint()
default constructor - only use for serialization
Definition EssentialMatrixConstraint.h:45
│ │ │ │ -
gtsam::EssentialMatrixConstraint::EssentialMatrixConstraint
EssentialMatrixConstraint(Key key1, Key key2, const EssentialMatrix &measuredE, const SharedNoiseModel &model)
Constructor.
Definition EssentialMatrixConstraint.h:55
│ │ │ │ +
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
│ │ │ │ +
gtsam::JacobianFactor::fillTerms
void fillTerms(const TERMS &terms, const Vector &b, const SharedDiagonal &noiseModel)
Internal function to fill blocks and set dimensions.
Definition JacobianFactor-inl.h:60
│ │ │ │ +
gtsam::RegularJacobianFactor
JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator.
Definition RegularJacobianFactor.h:32
│ │ │ │ +
gtsam::JacobianFactorSVD
JacobianFactor for Schur complement that uses the "Nullspace Trick" by Mourikis et al.
Definition JacobianFactorSVD.h:29
│ │ │ │ +
gtsam::JacobianFactorSVD::JacobianFactorSVD
JacobianFactorSVD()
Default constructor.
Definition JacobianFactorSVD.h:38
│ │ │ │ +
gtsam::JacobianFactorSVD::JacobianFactorSVD
JacobianFactorSVD(const KeyVector &keys, const std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > &Fblocks, const Matrix &Enull, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Construct a new JacobianFactorSVD object, createing a reduced-rank Jacobian factor on the CameraSet.
Definition JacobianFactorSVD.h:64
│ │ │ │ +
gtsam::JacobianFactorSVD::JacobianFactorSVD
JacobianFactorSVD(const KeyVector &keys, const SharedDiagonal &model=SharedDiagonal())
Empty constructor with keys.
Definition JacobianFactorSVD.h:42
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,159 +1,116 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -EssentialMatrixConstraint.h │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -1/* --------------------------------------------------------------------------- │ │ │ │ │ -- │ │ │ │ │ -2 │ │ │ │ │ -3 * GTSAM Copyright 2010-2014, Georgia Tech Research Corporation, │ │ │ │ │ -4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -5 * All Rights Reserved │ │ │ │ │ -6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ -7 │ │ │ │ │ -8 * See LICENSE for the license information │ │ │ │ │ +JacobianFactorSVD.h │ │ │ │ │ +1/* │ │ │ │ │ +2 * @file JacobianFactorSVD.h │ │ │ │ │ +3 * @date Oct 27, 2013 │ │ │ │ │ +4 * @uthor Frank Dellaert │ │ │ │ │ +5 */ │ │ │ │ │ +6 │ │ │ │ │ +7#pragma once │ │ │ │ │ +8#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ 9 │ │ │ │ │ -10 * ------------------------------------------------------------------------- │ │ │ │ │ -- */ │ │ │ │ │ -11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -22#include │ │ │ │ │ -23 │ │ │ │ │ -24namespace _g_t_s_a_m { │ │ │ │ │ -25 │ │ │ │ │ -_3_0class GTSAM_EXPORT _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -31 │ │ │ │ │ -32private: │ │ │ │ │ -33 │ │ │ │ │ -34 typedef _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t _T_h_i_s; │ │ │ │ │ -35 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_P_o_s_e_3_,_ _P_o_s_e_3_> _B_a_s_e; │ │ │ │ │ +10namespace _g_t_s_a_m { │ │ │ │ │ +28template │ │ │ │ │ +_2_9class _J_a_c_o_b_i_a_n_F_a_c_t_o_r_S_V_D: public _R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r { │ │ │ │ │ +30 │ │ │ │ │ +31 typedef _R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_<_D_> Base; │ │ │ │ │ +32 typedef Eigen::Matrix MatrixZD; // e.g 2 x 6 with Z=Point2 │ │ │ │ │ +33 typedef std::pair KeyMatrix; │ │ │ │ │ +34 │ │ │ │ │ +35public: │ │ │ │ │ 36 │ │ │ │ │ -37 _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x measuredE_; │ │ │ │ │ -39public: │ │ │ │ │ +_3_8 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_S_V_D() { │ │ │ │ │ +39 } │ │ │ │ │ 40 │ │ │ │ │ -41 // shorthand for a smart pointer to a factor │ │ │ │ │ -_4_2 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -43 │ │ │ │ │ -_4_5 _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t() { │ │ │ │ │ -46 } │ │ │ │ │ -47 │ │ │ │ │ -_5_5 _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t(_K_e_y key1, _K_e_y key2, │ │ │ │ │ -56 const _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x& measuredE, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model) : │ │ │ │ │ -57 _B_a_s_e(model, key1, key2), measuredE_(measuredE) { │ │ │ │ │ -58 } │ │ │ │ │ -59 │ │ │ │ │ -60 _~_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t() override { │ │ │ │ │ -61 } │ │ │ │ │ -62 │ │ │ │ │ -_6_4 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -65 return boost::static_pointer_cast( │ │ │ │ │ -66 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); │ │ │ │ │ -67 } │ │ │ │ │ -68 │ │ │ │ │ -72 void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ -73 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override; │ │ │ │ │ -74 │ │ │ │ │ -76 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& expected, double tol = 1e-9) const │ │ │ │ │ -override; │ │ │ │ │ -77 │ │ │ │ │ -81 Vector evaluateError(const _P_o_s_e_3& p1, const _P_o_s_e_3& p2, │ │ │ │ │ -82 boost::optional Hp1 = boost::none, // │ │ │ │ │ -83 boost::optional Hp2 = boost::none) const override; │ │ │ │ │ -84 │ │ │ │ │ -_8_6 const _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x& _m_e_a_s_u_r_e_d() const { │ │ │ │ │ -87 return measuredE_; │ │ │ │ │ -88 } │ │ │ │ │ -89 │ │ │ │ │ -90private: │ │ │ │ │ -91 │ │ │ │ │ -_9_3 friend class boost::serialization::access; │ │ │ │ │ -94 template │ │ │ │ │ -95 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -96 // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility │ │ │ │ │ -97 ar │ │ │ │ │ -98 & boost::serialization::make_nvp("NoiseModelFactor2", │ │ │ │ │ -99 boost::serialization::base_object(*this)); │ │ │ │ │ -100 ar & BOOST_SERIALIZATION_NVP(measuredE_); │ │ │ │ │ -101 } │ │ │ │ │ -102 │ │ │ │ │ -103public: │ │ │ │ │ -104 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -105}; │ │ │ │ │ -106// \class EssentialMatrixConstraint │ │ │ │ │ -107 │ │ │ │ │ -108} │ │ │ │ │ -_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ -This marks a GTSAM object to require alignment. │ │ │ │ │ -DDeeffiinniittiioonn types.h:308 │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ -Non-linear factor base classes. │ │ │ │ │ +_4_2 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_S_V_D(const _K_e_y_V_e_c_t_o_r& _k_e_y_s, │ │ │ │ │ +43 const SharedDiagonal& model = SharedDiagonal()) │ │ │ │ │ +44 : Base() { │ │ │ │ │ +45 Matrix zeroMatrix = Matrix::Zero(0, D); │ │ │ │ │ +46 Vector zeroVector = Vector::Zero(0); │ │ │ │ │ +47 std::vector QF; │ │ │ │ │ +48 QF.reserve(_k_e_y_s.size()); │ │ │ │ │ +49 for(const _K_e_y& key: _k_e_y_s) │ │ │ │ │ +50 QF.push_back(KeyMatrix(key, zeroMatrix)); │ │ │ │ │ +51 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_f_i_l_l_T_e_r_m_s(QF, zeroVector, model); │ │ │ │ │ +52 } │ │ │ │ │ +53 │ │ │ │ │ +_6_4 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_S_V_D( │ │ │ │ │ +65 const _K_e_y_V_e_c_t_o_r& _k_e_y_s, │ │ │ │ │ +66 const std::vector >& Fblocks, │ │ │ │ │ +67 const Matrix& Enull, const Vector& b, │ │ │ │ │ +68 const SharedDiagonal& model = SharedDiagonal()) │ │ │ │ │ +69 : Base() { │ │ │ │ │ +70 size_t numKeys = Enull.rows() / ZDim; │ │ │ │ │ +71 size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()? │ │ │ │ │ +72 // PLAIN nullptr SPACE TRICK │ │ │ │ │ +73 // Matrix Q = Enull * Enull.transpose(); │ │ │ │ │ +74 // for(const KeyMatrixZD& it: Fblocks) │ │ │ │ │ +75 // QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * │ │ │ │ │ +it.second)); │ │ │ │ │ +76 // JacobianFactor factor(QF, Q * b); │ │ │ │ │ +77 std::vector QF; │ │ │ │ │ +78 QF.reserve(numKeys); │ │ │ │ │ +79 for (size_t k = 0; k < Fblocks.size(); ++k) { │ │ │ │ │ +80 _K_e_y key = _k_e_y_s[k]; │ │ │ │ │ +81 QF.emplace_back( │ │ │ │ │ +82 key, (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k]); │ │ │ │ │ +83 } │ │ │ │ │ +84 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_f_i_l_l_T_e_r_m_s(QF, Enull.transpose() * b, model); │ │ │ │ │ +85 } │ │ │ │ │ +86}; │ │ │ │ │ +87 │ │ │ │ │ +88} │ │ │ │ │ +_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ +JacobianFactor class with fixed sized blcoks. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ -Aliases. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x │ │ │ │ │ -An essential matrix is like a Pose3, except with translation up to scale It is │ │ │ │ │ -named after the 3*3 ma... │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrix.h:26 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ -A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ -Nonlinear factor base class. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ -A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ -variables. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t │ │ │ │ │ -Binary factor between two Pose3 variables induced by an EssentialMatrix │ │ │ │ │ -measurement. │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrixConstraint.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t_:_:_c_l_o_n_e │ │ │ │ │ -gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrixConstraint.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t_:_:_m_e_a_s_u_r_e_d │ │ │ │ │ -const EssentialMatrix & measured() const │ │ │ │ │ -return the measured │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrixConstraint.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< EssentialMatrixConstraint > shared_ptr │ │ │ │ │ -The measurement is an essential matrix. │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrixConstraint.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t │ │ │ │ │ -EssentialMatrixConstraint() │ │ │ │ │ -default constructor - only use for serialization │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrixConstraint.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t │ │ │ │ │ -EssentialMatrixConstraint(Key key1, Key key2, const EssentialMatrix &measuredE, │ │ │ │ │ -const SharedNoiseModel &model) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn EssentialMatrixConstraint.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s │ │ │ │ │ +const KeyVector & keys() const │ │ │ │ │ +Access the factor's involved variable keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:140 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_f_i_l_l_T_e_r_m_s │ │ │ │ │ +void fillTerms(const TERMS &terms, const Vector &b, const SharedDiagonal │ │ │ │ │ +&noiseModel) │ │ │ │ │ +Internal function to fill blocks and set dimensions. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor-inl.h:60 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ +JacobianFactor with constant sized blocks Provides raw memory access versions │ │ │ │ │ +of linear operator. │ │ │ │ │ +DDeeffiinniittiioonn RegularJacobianFactor.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_S_V_D │ │ │ │ │ +JacobianFactor for Schur complement that uses the "Nullspace Trick" by Mourikis │ │ │ │ │ +et al. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactorSVD.h:29 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_S_V_D_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_S_V_D │ │ │ │ │ +JacobianFactorSVD() │ │ │ │ │ +Default constructor. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactorSVD.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_S_V_D_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_S_V_D │ │ │ │ │ +JacobianFactorSVD(const KeyVector &keys, const std::vector< MatrixZD, Eigen:: │ │ │ │ │ +aligned_allocator< MatrixZD > > &Fblocks, const Matrix &Enull, const Vector &b, │ │ │ │ │ +const SharedDiagonal &model=SharedDiagonal()) │ │ │ │ │ +Construct a new JacobianFactorSVD object, createing a reduced-rank Jacobian │ │ │ │ │ +factor on the CameraSet. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactorSVD.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_S_V_D_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_S_V_D │ │ │ │ │ +JacobianFactorSVD(const KeyVector &keys, const SharedDiagonal │ │ │ │ │ +&model=SharedDiagonal()) │ │ │ │ │ +Empty constructor with keys. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactorSVD.h:42 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_l_a_m │ │ │ │ │ - * _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t_._h │ │ │ │ │ + * JJaaccoobbiiaannFFaaccttoorrSSVVDD..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01343.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicFactor-inst.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicFactorGraph.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,76 +94,32 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
SymbolicFactor-inst.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
SymbolicFactorGraph.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

template<class FACTOR >
std::pair< boost::shared_ptr< SymbolicConditional >, boost::shared_ptr< SymbolicFactor > > gtsam::internal::EliminateSymbolic (const FactorGraph< FACTOR > &factors, const Ordering &keys)
 Implementation of dense elimination function for symbolic factors.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Oct 17, 2010
│ │ │ │ -

Function Documentation

│ │ │ │ - │ │ │ │ -

◆ EliminateSymbolic()

│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -template<class FACTOR >
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
std::pair< boost::shared_ptr< SymbolicConditional >, boost::shared_ptr< SymbolicFactor > > gtsam::internal::EliminateSymbolic (const FactorGraph< FACTOR > & factors,
const Ordering & keys 
)
│ │ │ │ -
│ │ │ │ - │ │ │ │ -

Implementation of dense elimination function for symbolic factors.

│ │ │ │ -

This is a templated version for internally doing symbolic elimination on any factor.

│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ +
Date
Oct 29, 2009
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,41 +1,20 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -SymbolicFactor-inst.h File Reference │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +SymbolicFactorGraph.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ - std::pair< boost::shared_ptr< _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_E_l_i_m_i_n_a_t_e_S_y_m_b_o_l_i_c (const │ │ │ │ │ - _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l >, boost:: _F_a_c_t_o_r_G_r_a_p_h< FACTOR > &factors, const │ │ │ │ │ -shared_ptr< _S_y_m_b_o_l_i_c_F_a_c_t_o_r > >  _O_r_d_e_r_i_n_g &keys) │ │ │ │ │ -  Implementation of dense elimination │ │ │ │ │ - function for symbolic factors. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ Date │ │ │ │ │ - Oct 17, 2010 │ │ │ │ │ -********** FFuunnccttiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ -********** _?◆_? EElliimmiinnaatteeSSyymmbboolliicc(()) ********** │ │ │ │ │ -template │ │ │ │ │ -std::pair< boost::shared_ptr< │ │ │ │ │ -SymbolicConditional >, boost:: ( const _F_a_c_t_o_r_G_r_a_p_h< FACTOR > &  ffaaccttoorrss, │ │ │ │ │ -shared_ptr< SymbolicFactor > > │ │ │ │ │ -gtsam::internal::EliminateSymbolic │ │ │ │ │ - const _O_r_d_e_r_i_n_g &  kkeeyyss  │ │ │ │ │ - ) │ │ │ │ │ -Implementation of dense elimination function for symbolic factors. │ │ │ │ │ -This is a templated version for internally doing symbolic elimination on any │ │ │ │ │ -factor. │ │ │ │ │ + Oct 29, 2009 │ │ │ │ │ + Author │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_y_m_b_o_l_i_c │ │ │ │ │ - * _S_y_m_b_o_l_i_c_F_a_c_t_o_r_-_i_n_s_t_._h │ │ │ │ │ + * _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01346.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicISAM.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicConditional.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -96,40 +96,42 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
SymbolicISAM.h File Reference
│ │ │ │ +
SymbolicConditional.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::SymbolicISAM
class  gtsam::SymbolicConditional
 SymbolicConditional is a conditional with keys but no probability data, produced by symbolic elimination of SymbolicFactor. More...
 
struct  gtsam::traits< SymbolicConditional >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
July 29, 2013
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Oct 17, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,25 +1,29 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SymbolicISAM.h File Reference │ │ │ │ │ +SymbolicConditional.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_I_S_A_M │ │ │ │ │ + class   _g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +  _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l is a conditional with keys but no probability │ │ │ │ │ + data, produced by symbolic elimination of _S_y_m_b_o_l_i_c_F_a_c_t_o_r. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - July 29, 2013 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Oct 17, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_y_m_b_o_l_i_c │ │ │ │ │ - * _S_y_m_b_o_l_i_c_I_S_A_M_._h │ │ │ │ │ + * _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01346.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,4 @@ │ │ │ │ │ var a01346 = [ │ │ │ │ │ - ["gtsam::SymbolicISAM", "a05032.html", "a05032"] │ │ │ │ │ + ["gtsam::SymbolicConditional", "a04996.html", "a04996"], │ │ │ │ │ + ["gtsam::traits< SymbolicConditional >", "a05000.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01346_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicISAM.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicConditional.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,63 +98,168 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
SymbolicISAM.h
│ │ │ │ +
SymbolicConditional.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/symbolic/SymbolicBayesTree.h>
│ │ │ │ -
22#include <gtsam/inference/ISAM.h>
│ │ │ │ -
23
│ │ │ │ -
24namespace gtsam {
│ │ │ │ -
25
│ │ │ │ -
│ │ │ │ -
26 class GTSAM_EXPORT SymbolicISAM : public ISAM<SymbolicBayesTree>
│ │ │ │ -
27 {
│ │ │ │ -
28 public:
│ │ │ │ -
29 typedef ISAM<SymbolicBayesTree> Base;
│ │ │ │ -
30 typedef SymbolicISAM This;
│ │ │ │ -
31 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
32
│ │ │ │ -
35
│ │ │ │ -
37 SymbolicISAM();
│ │ │ │ -
38
│ │ │ │ -
40 SymbolicISAM(const SymbolicBayesTree& bayesTree);
│ │ │ │ -
41
│ │ │ │ -
43
│ │ │ │ -
44 };
│ │ │ │ -
│ │ │ │ -
45
│ │ │ │ -
46}
│ │ │ │ -
ISAM.h
Incremental update functionality (iSAM) for BayesTree.
│ │ │ │ - │ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/base/Testable.h>
│ │ │ │ +
21#include <gtsam/base/types.h>
│ │ │ │ +
22#include <gtsam/inference/Conditional-inst.h>
│ │ │ │ +
23#include <gtsam/symbolic/SymbolicFactor.h>
│ │ │ │ +
24
│ │ │ │ +
25namespace gtsam {
│ │ │ │ +
26
│ │ │ │ +
│ │ │ │ +
36 class GTSAM_EXPORT SymbolicConditional :
│ │ │ │ +
37 public SymbolicFactor,
│ │ │ │ +
38 public Conditional<SymbolicFactor, SymbolicConditional> {
│ │ │ │ +
39
│ │ │ │ +
40 public:
│ │ │ │ +
41 typedef SymbolicConditional This;
│ │ │ │ +
42 typedef SymbolicFactor BaseFactor;
│ │ │ │ +
43 typedef Conditional<BaseFactor, This> BaseConditional;
│ │ │ │ +
44 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
45 typedef BaseFactor::iterator iterator;
│ │ │ │ +
46 typedef BaseFactor::const_iterator const_iterator;
│ │ │ │ +
47
│ │ │ │ +
50
│ │ │ │ +
52 SymbolicConditional() {}
│ │ │ │ +
53
│ │ │ │ +
55 SymbolicConditional(Key j) : BaseFactor(j), BaseConditional(1) {}
│ │ │ │ +
56
│ │ │ │ +
58 SymbolicConditional(Key j, Key parent) : BaseFactor(j, parent), BaseConditional(1) {}
│ │ │ │ +
59
│ │ │ │ +
61 SymbolicConditional(Key j, Key parent1, Key parent2) : BaseFactor(j, parent1, parent2), BaseConditional(1) {}
│ │ │ │ +
62
│ │ │ │ +
64 SymbolicConditional(Key j, Key parent1, Key parent2, Key parent3) : BaseFactor(j, parent1, parent2, parent3), BaseConditional(1) {}
│ │ │ │ +
65
│ │ │ │ +
67 template<typename ITERATOR>
│ │ │ │ +
│ │ │ │ +
68 static SymbolicConditional FromIterators(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals)
│ │ │ │ +
69 {
│ │ │ │ +
70 SymbolicConditional result;
│ │ │ │ +
71 (BaseFactor&)result = BaseFactor::FromIterators(firstKey, lastKey);
│ │ │ │ +
72 result.nrFrontals_ = nrFrontals;
│ │ │ │ +
73 return result;
│ │ │ │ +
74 }
│ │ │ │ +
│ │ │ │ +
75
│ │ │ │ +
77 template<typename ITERATOR>
│ │ │ │ +
│ │ │ │ +
78 static SymbolicConditional::shared_ptr FromIteratorsShared(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals)
│ │ │ │ +
79 {
│ │ │ │ +
80 SymbolicConditional::shared_ptr result = boost::make_shared<SymbolicConditional>();
│ │ │ │ +
81 result->keys_.assign(firstKey, lastKey);
│ │ │ │ +
82 result->nrFrontals_ = nrFrontals;
│ │ │ │ +
83 return result;
│ │ │ │ +
84 }
│ │ │ │ +
│ │ │ │ +
85
│ │ │ │ +
87 template<class CONTAINER>
│ │ │ │ +
│ │ │ │ +
88 static SymbolicConditional FromKeys(const CONTAINER& keys, size_t nrFrontals) {
│ │ │ │ +
89 return FromIterators(keys.begin(), keys.end(), nrFrontals);
│ │ │ │ +
90 }
│ │ │ │ +
│ │ │ │ +
91
│ │ │ │ +
93 template<class CONTAINER>
│ │ │ │ +
│ │ │ │ +
94 static SymbolicConditional::shared_ptr FromKeysShared(const CONTAINER& keys, size_t nrFrontals) {
│ │ │ │ +
95 return FromIteratorsShared(keys.begin(), keys.end(), nrFrontals);
│ │ │ │ +
96 }
│ │ │ │ +
│ │ │ │ +
97
│ │ │ │ +
99 SymbolicFactor::shared_ptr clone() const { return boost::make_shared<This>(*this); }
│ │ │ │ +
100
│ │ │ │ +
104
│ │ │ │ +
106 void print(
│ │ │ │ +
107 const std::string& str = "",
│ │ │ │ +
108 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
│ │ │ │ +
109
│ │ │ │ +
111 bool equals(const This& c, double tol = 1e-9) const;
│ │ │ │ +
112
│ │ │ │ +
116
│ │ │ │ +
118 double logProbability(const HybridValues& x) const override;
│ │ │ │ +
119
│ │ │ │ +
121 double evaluate(const HybridValues& x) const override;
│ │ │ │ +
122
│ │ │ │ +
123 using Conditional::operator(); // Expose evaluate(const HybridValues&) method..
│ │ │ │ +
124 using SymbolicFactor::error; // Expose error(const HybridValues&) method..
│ │ │ │ +
125
│ │ │ │ +
127
│ │ │ │ +
128 private:
│ │ │ │ +
130 friend class boost::serialization::access;
│ │ │ │ +
131 template<class Archive>
│ │ │ │ +
132 void serialize(Archive & ar, const unsigned int /*version*/) {
│ │ │ │ +
133 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseFactor);
│ │ │ │ +
134 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseConditional);
│ │ │ │ +
135 }
│ │ │ │ +
136 };
│ │ │ │ +
│ │ │ │ +
137
│ │ │ │ +
139template<>
│ │ │ │ +
│ │ │ │ +
140struct traits<SymbolicConditional> : public Testable<SymbolicConditional> {
│ │ │ │ +
141};
│ │ │ │ +
│ │ │ │ +
142
│ │ │ │ +
143} //\ namespace gtsam
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
types.h
Typedefs for easier changing of types.
│ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::ISAM
A Bayes tree with an update methods that implements the iSAM algorithm.
Definition ISAM.h:31
│ │ │ │ -
gtsam::SymbolicBayesTree
A Bayes tree that represents the connectivity between variables but is not associated with any probab...
Definition SymbolicBayesTree.h:51
│ │ │ │ -
gtsam::SymbolicISAM
Definition SymbolicISAM.h:27
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ +
gtsam::Conditional
Definition Conditional.h:64
│ │ │ │ +
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition Factor.h:80
│ │ │ │ +
gtsam::Factor::iterator
KeyVector::iterator iterator
Iterator over keys.
Definition Factor.h:77
│ │ │ │ +
gtsam::SymbolicConditional
SymbolicConditional is a conditional with keys but no probability data, produced by symbolic eliminat...
Definition SymbolicConditional.h:38
│ │ │ │ +
gtsam::SymbolicConditional::SymbolicConditional
SymbolicConditional(Key j, Key parent1, Key parent2, Key parent3)
Three parents.
Definition SymbolicConditional.h:64
│ │ │ │ +
gtsam::SymbolicConditional::SymbolicConditional
SymbolicConditional()
Empty Constructor to make serialization possible.
Definition SymbolicConditional.h:52
│ │ │ │ +
gtsam::SymbolicConditional::FromKeys
static SymbolicConditional FromKeys(const CONTAINER &keys, size_t nrFrontals)
Named constructor from an arbitrary number of keys and frontals.
Definition SymbolicConditional.h:88
│ │ │ │ +
gtsam::SymbolicConditional::const_iterator
BaseFactor::const_iterator const_iterator
iterator to keys
Definition SymbolicConditional.h:46
│ │ │ │ +
gtsam::SymbolicConditional::shared_ptr
boost::shared_ptr< This > shared_ptr
Typedef to the conditional base class.
Definition SymbolicConditional.h:44
│ │ │ │ +
gtsam::SymbolicConditional::BaseConditional
Conditional< BaseFactor, This > BaseConditional
Typedef to the factor base class.
Definition SymbolicConditional.h:43
│ │ │ │ +
gtsam::SymbolicConditional::FromIteratorsShared
static SymbolicConditional::shared_ptr FromIteratorsShared(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals)
Named constructor from an arbitrary number of keys and frontals.
Definition SymbolicConditional.h:78
│ │ │ │ +
gtsam::SymbolicConditional::FromIterators
static SymbolicConditional FromIterators(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals)
Named constructor from an arbitrary number of keys and frontals.
Definition SymbolicConditional.h:68
│ │ │ │ +
gtsam::SymbolicConditional::SymbolicConditional
SymbolicConditional(Key j, Key parent)
Single parent.
Definition SymbolicConditional.h:58
│ │ │ │ +
gtsam::SymbolicConditional::SymbolicConditional
SymbolicConditional(Key j)
No parents.
Definition SymbolicConditional.h:55
│ │ │ │ +
gtsam::SymbolicConditional::SymbolicConditional
SymbolicConditional(Key j, Key parent1, Key parent2)
Two parents.
Definition SymbolicConditional.h:61
│ │ │ │ +
gtsam::SymbolicConditional::clone
SymbolicFactor::shared_ptr clone() const
Copy this object as its actual derived type.
Definition SymbolicConditional.h:99
│ │ │ │ +
gtsam::SymbolicConditional::iterator
BaseFactor::iterator iterator
Boost shared_ptr to this class.
Definition SymbolicConditional.h:45
│ │ │ │ +
gtsam::SymbolicConditional::BaseFactor
SymbolicFactor BaseFactor
Typedef to this class.
Definition SymbolicConditional.h:42
│ │ │ │ +
gtsam::SymbolicConditional::FromKeysShared
static SymbolicConditional::shared_ptr FromKeysShared(const CONTAINER &keys, size_t nrFrontals)
Named constructor from an arbitrary number of keys and frontals.
Definition SymbolicConditional.h:94
│ │ │ │ +
gtsam::SymbolicFactor
SymbolicFactor represents a symbolic factor that specifies graph topology but is not associated with ...
Definition SymbolicFactor.h:39
│ │ │ │ +
gtsam::SymbolicFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
Overriding the shared_ptr typedef.
Definition SymbolicFactor.h:48
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,62 +1,244 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SymbolicISAM.h │ │ │ │ │ +SymbolicConditional.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_s_y_m_b_o_l_i_c_/_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_I_S_A_M_._h> │ │ │ │ │ -23 │ │ │ │ │ -24namespace _g_t_s_a_m { │ │ │ │ │ -25 │ │ │ │ │ -_2_6 class GTSAM_EXPORT _S_y_m_b_o_l_i_c_I_S_A_M : public _I_S_A_M │ │ │ │ │ -27 { │ │ │ │ │ -28 public: │ │ │ │ │ -29 typedef _I_S_A_M_<_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_> _B_a_s_e; │ │ │ │ │ -30 typedef _S_y_m_b_o_l_i_c_I_S_A_M _T_h_i_s; │ │ │ │ │ -31 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -32 │ │ │ │ │ -35 │ │ │ │ │ -37 _S_y_m_b_o_l_i_c_I_S_A_M(); │ │ │ │ │ -38 │ │ │ │ │ -40 _S_y_m_b_o_l_i_c_I_S_A_M(const _S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e& bayesTree); │ │ │ │ │ -41 │ │ │ │ │ -43 │ │ │ │ │ -44 }; │ │ │ │ │ -45 │ │ │ │ │ -46} │ │ │ │ │ -_I_S_A_M_._h │ │ │ │ │ -Incremental update functionality (iSAM) for BayesTree. │ │ │ │ │ -_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ +22#include │ │ │ │ │ +23#include <_g_t_s_a_m_/_s_y_m_b_o_l_i_c_/_S_y_m_b_o_l_i_c_F_a_c_t_o_r_._h> │ │ │ │ │ +24 │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ +26 │ │ │ │ │ +_3_6 class GTSAM_EXPORT _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l : │ │ │ │ │ +37 public _S_y_m_b_o_l_i_c_F_a_c_t_o_r, │ │ │ │ │ +38 public _C_o_n_d_i_t_i_o_n_a_l { │ │ │ │ │ +39 │ │ │ │ │ +40 public: │ │ │ │ │ +41 typedef _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l _T_h_i_s; │ │ │ │ │ +_4_2 typedef _S_y_m_b_o_l_i_c_F_a_c_t_o_r _B_a_s_e_F_a_c_t_o_r; │ │ │ │ │ +_4_3 typedef _C_o_n_d_i_t_i_o_n_a_l_<_B_a_s_e_F_a_c_t_o_r_,_ _T_h_i_s_> _B_a_s_e_C_o_n_d_i_t_i_o_n_a_l; │ │ │ │ │ +_4_4 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +_4_5 typedef _B_a_s_e_F_a_c_t_o_r_:_:_i_t_e_r_a_t_o_r _i_t_e_r_a_t_o_r; │ │ │ │ │ +_4_6 typedef _B_a_s_e_F_a_c_t_o_r_:_:_c_o_n_s_t___i_t_e_r_a_t_o_r _c_o_n_s_t___i_t_e_r_a_t_o_r; │ │ │ │ │ +47 │ │ │ │ │ +50 │ │ │ │ │ +_5_2 _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l() {} │ │ │ │ │ +53 │ │ │ │ │ +_5_5 _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l(_K_e_y j) : _B_a_s_e_F_a_c_t_o_r(j), _B_a_s_e_C_o_n_d_i_t_i_o_n_a_l(1) {} │ │ │ │ │ +56 │ │ │ │ │ +_5_8 _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l(_K_e_y j, _K_e_y parent) : _B_a_s_e_F_a_c_t_o_r(j, parent), │ │ │ │ │ +_B_a_s_e_C_o_n_d_i_t_i_o_n_a_l(1) {} │ │ │ │ │ +59 │ │ │ │ │ +_6_1 _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l(_K_e_y j, _K_e_y parent1, _K_e_y parent2) : _B_a_s_e_F_a_c_t_o_r(j, │ │ │ │ │ +parent1, parent2), _B_a_s_e_C_o_n_d_i_t_i_o_n_a_l(1) {} │ │ │ │ │ +62 │ │ │ │ │ +_6_4 _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l(_K_e_y j, _K_e_y parent1, _K_e_y parent2, _K_e_y parent3) : │ │ │ │ │ +_B_a_s_e_F_a_c_t_o_r(j, parent1, parent2, parent3), _B_a_s_e_C_o_n_d_i_t_i_o_n_a_l(1) {} │ │ │ │ │ +65 │ │ │ │ │ +67 template │ │ │ │ │ +_6_8 static _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l _F_r_o_m_I_t_e_r_a_t_o_r_s(ITERATOR firstKey, ITERATOR │ │ │ │ │ +lastKey, size_t nrFrontals) │ │ │ │ │ +69 { │ │ │ │ │ +70 _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l result; │ │ │ │ │ +71 (_B_a_s_e_F_a_c_t_o_r&)result = BaseFactor::FromIterators(firstKey, lastKey); │ │ │ │ │ +72 result.nrFrontals_ = nrFrontals; │ │ │ │ │ +73 return result; │ │ │ │ │ +74 } │ │ │ │ │ +75 │ │ │ │ │ +77 template │ │ │ │ │ +_7_8 static _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r _F_r_o_m_I_t_e_r_a_t_o_r_s_S_h_a_r_e_d(ITERATOR │ │ │ │ │ +firstKey, ITERATOR lastKey, size_t nrFrontals) │ │ │ │ │ +79 { │ │ │ │ │ +80 _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r result = boost:: │ │ │ │ │ +make_shared(); │ │ │ │ │ +81 result->keys_.assign(firstKey, lastKey); │ │ │ │ │ +82 result->nrFrontals_ = nrFrontals; │ │ │ │ │ +83 return result; │ │ │ │ │ +84 } │ │ │ │ │ +85 │ │ │ │ │ +87 template │ │ │ │ │ +_8_8 static _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l _F_r_o_m_K_e_y_s(const CONTAINER& keys, size_t │ │ │ │ │ +nrFrontals) { │ │ │ │ │ +89 return FromIterators(keys.begin(), keys.end(), nrFrontals); │ │ │ │ │ +90 } │ │ │ │ │ +91 │ │ │ │ │ +93 template │ │ │ │ │ +_9_4 static _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r _F_r_o_m_K_e_y_s_S_h_a_r_e_d(const CONTAINER& keys, │ │ │ │ │ +size_t nrFrontals) { │ │ │ │ │ +95 return FromIteratorsShared(keys.begin(), keys.end(), nrFrontals); │ │ │ │ │ +96 } │ │ │ │ │ +97 │ │ │ │ │ +_9_9 _S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r _c_l_o_n_e() const { return boost::make_shared │ │ │ │ │ +(*this); } │ │ │ │ │ +100 │ │ │ │ │ +104 │ │ │ │ │ +106 void _p_r_i_n_t( │ │ │ │ │ +107 const std::string& str = "", │ │ │ │ │ +108 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override; │ │ │ │ │ +109 │ │ │ │ │ +111 bool _e_q_u_a_l_s(const This& c, double tol = 1e-9) const; │ │ │ │ │ +112 │ │ │ │ │ +116 │ │ │ │ │ +118 double logProbability(const _H_y_b_r_i_d_V_a_l_u_e_s& x) const override; │ │ │ │ │ +119 │ │ │ │ │ +121 double evaluate(const _H_y_b_r_i_d_V_a_l_u_e_s& x) const override; │ │ │ │ │ +122 │ │ │ │ │ +123 using Conditional::operator(); // Expose evaluate(const HybridValues&) │ │ │ │ │ +method.. │ │ │ │ │ +124 using SymbolicFactor::error; // Expose error(const HybridValues&) method.. │ │ │ │ │ +125 │ │ │ │ │ +127 │ │ │ │ │ +128 private: │ │ │ │ │ +_1_3_0 friend class boost::serialization::access; │ │ │ │ │ +131 template │ │ │ │ │ +132 void serialize(Archive & ar, const unsigned int /*version*/) { │ │ │ │ │ +133 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e_F_a_c_t_o_r); │ │ │ │ │ +134 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e_C_o_n_d_i_t_i_o_n_a_l); │ │ │ │ │ +135 } │ │ │ │ │ +136 }; │ │ │ │ │ +137 │ │ │ │ │ +139template<> │ │ │ │ │ +_1_4_0struct _t_r_a_i_t_s<_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l> : public _T_e_s_t_a_b_l_e { │ │ │ │ │ +141}; │ │ │ │ │ +142 │ │ │ │ │ +143} //\ namespace gtsam │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ +_t_y_p_e_s_._h │ │ │ │ │ +Typedefs for easier changing of types. │ │ │ │ │ +_S_y_m_b_o_l_i_c_F_a_c_t_o_r_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M │ │ │ │ │ -A Bayes tree with an update methods that implements the iSAM algorithm. │ │ │ │ │ -DDeeffiinniittiioonn ISAM.h:31 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e │ │ │ │ │ -A Bayes tree that represents the connectivity between variables but is not │ │ │ │ │ -associated with any probab... │ │ │ │ │ -DDeeffiinniittiioonn SymbolicBayesTree.h:51 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_I_S_A_M │ │ │ │ │ -DDeeffiinniittiioonn SymbolicISAM.h:27 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_c_o_n_s_t___i_t_e_r_a_t_o_r │ │ │ │ │ +KeyVector::const_iterator const_iterator │ │ │ │ │ +Const iterator over keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:80 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_i_t_e_r_a_t_o_r │ │ │ │ │ +KeyVector::iterator iterator │ │ │ │ │ +Iterator over keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +SymbolicConditional is a conditional with keys but no probability data, │ │ │ │ │ +produced by symbolic eliminat... │ │ │ │ │ +DDeeffiinniittiioonn SymbolicConditional.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +SymbolicConditional(Key j, Key parent1, Key parent2, Key parent3) │ │ │ │ │ +Three parents. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicConditional.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +SymbolicConditional() │ │ │ │ │ +Empty Constructor to make serialization possible. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicConditional.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_F_r_o_m_K_e_y_s │ │ │ │ │ +static SymbolicConditional FromKeys(const CONTAINER &keys, size_t nrFrontals) │ │ │ │ │ +Named constructor from an arbitrary number of keys and frontals. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicConditional.h:88 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_c_o_n_s_t___i_t_e_r_a_t_o_r │ │ │ │ │ +BaseFactor::const_iterator const_iterator │ │ │ │ │ +iterator to keys │ │ │ │ │ +DDeeffiinniittiioonn SymbolicConditional.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +Typedef to the conditional base class. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicConditional.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_B_a_s_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +Conditional< BaseFactor, This > BaseConditional │ │ │ │ │ +Typedef to the factor base class. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicConditional.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_F_r_o_m_I_t_e_r_a_t_o_r_s_S_h_a_r_e_d │ │ │ │ │ +static SymbolicConditional::shared_ptr FromIteratorsShared(ITERATOR firstKey, │ │ │ │ │ +ITERATOR lastKey, size_t nrFrontals) │ │ │ │ │ +Named constructor from an arbitrary number of keys and frontals. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicConditional.h:78 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_F_r_o_m_I_t_e_r_a_t_o_r_s │ │ │ │ │ +static SymbolicConditional FromIterators(ITERATOR firstKey, ITERATOR lastKey, │ │ │ │ │ +size_t nrFrontals) │ │ │ │ │ +Named constructor from an arbitrary number of keys and frontals. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicConditional.h:68 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +SymbolicConditional(Key j, Key parent) │ │ │ │ │ +Single parent. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicConditional.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +SymbolicConditional(Key j) │ │ │ │ │ +No parents. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicConditional.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +SymbolicConditional(Key j, Key parent1, Key parent2) │ │ │ │ │ +Two parents. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicConditional.h:61 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_c_l_o_n_e │ │ │ │ │ +SymbolicFactor::shared_ptr clone() const │ │ │ │ │ +Copy this object as its actual derived type. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicConditional.h:99 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_i_t_e_r_a_t_o_r │ │ │ │ │ +BaseFactor::iterator iterator │ │ │ │ │ +Boost shared_ptr to this class. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicConditional.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_B_a_s_e_F_a_c_t_o_r │ │ │ │ │ +SymbolicFactor BaseFactor │ │ │ │ │ +Typedef to this class. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicConditional.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_F_r_o_m_K_e_y_s_S_h_a_r_e_d │ │ │ │ │ +static SymbolicConditional::shared_ptr FromKeysShared(const CONTAINER &keys, │ │ │ │ │ +size_t nrFrontals) │ │ │ │ │ +Named constructor from an arbitrary number of keys and frontals. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicConditional.h:94 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r │ │ │ │ │ +SymbolicFactor represents a symbolic factor that specifies graph topology but │ │ │ │ │ +is not associated with ... │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactor.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +Overriding the shared_ptr typedef. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactor.h:48 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_y_m_b_o_l_i_c │ │ │ │ │ - * _S_y_m_b_o_l_i_c_I_S_A_M_._h │ │ │ │ │ + * _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01349.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicJunctionTree.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicEliminationTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,19 +94,30 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
SymbolicJunctionTree.cpp File Reference
│ │ │ │ +
SymbolicEliminationTree.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::SymbolicEliminationTree
 
struct  gtsam::traits< SymbolicEliminationTree >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ @@ -115,13 +126,13 @@ │ │ │ │
│ │ │ │ Richard Roberts
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SymbolicJunctionTree.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +SymbolicEliminationTree.h File Reference │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Date │ │ │ │ │ Mar 29, 2013 │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_y_m_b_o_l_i_c │ │ │ │ │ - * _S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e_._c_p_p │ │ │ │ │ + * _S_y_m_b_o_l_i_c_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01352.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicConditional.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicEliminationTree.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,44 +94,34 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
SymbolicConditional.h File Reference
│ │ │ │ +
SymbolicEliminationTree.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::SymbolicConditional
 SymbolicConditional is a conditional with keys but no probability data, produced by symbolic elimination of SymbolicFactor. More...
 
struct  gtsam::traits< SymbolicConditional >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Oct 17, 2010
│ │ │ │ +
Date
Mar 29, 2013
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,29 +1,21 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SymbolicConditional.h File Reference │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -  _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l is a conditional with keys but no probability │ │ │ │ │ - data, produced by symbolic elimination of _S_y_m_b_o_l_i_c_F_a_c_t_o_r. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +SymbolicEliminationTree.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ + Date │ │ │ │ │ + Mar 29, 2013 │ │ │ │ │ Author │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ - Date │ │ │ │ │ - Oct 17, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_y_m_b_o_l_i_c │ │ │ │ │ - * _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ + * _S_y_m_b_o_l_i_c_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01355.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicJunctionTree.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicBayesTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,50 +95,50 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Typedefs
│ │ │ │ -
SymbolicJunctionTree.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
SymbolicBayesTree.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::SymbolicJunctionTree
 A EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree, with the additional property that it represents the clique tree associated with a Bayes net. More...
class  gtsam::SymbolicBayesTreeClique
 A clique in a SymbolicBayesTree. More...
 
class  gtsam::SymbolicBayesTree
 A Bayes tree that represents the connectivity between variables but is not associated with any probability functions. More...
 
struct  gtsam::traits< SymbolicBayesTreeClique >
 traits More...
 
struct  gtsam::traits< SymbolicBayesTree >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Typedefs

│ │ │ │ -using gtsam::SymbolicCluster = SymbolicJunctionTree::Cluster
 typedef for wrapper:
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Mar 29, 2013
│ │ │ │ +
Date
Oct 29, 2009
│ │ │ │
Author
Frank Dellaert
│ │ │ │
│ │ │ │ Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,32 +1,35 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ -SymbolicJunctionTree.h File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +SymbolicBayesTree.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ - A _E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e, i.e., a set of variable clusters with │ │ │ │ │ -  factors, arranged in a tree, with the additional property that it │ │ │ │ │ - represents the clique tree associated with a Bayes net. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e │ │ │ │ │ +  A clique in a _S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e │ │ │ │ │ +  A Bayes tree that represents the connectivity between variables but is │ │ │ │ │ + not associated with any probability functions. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -using  ggttssaamm::::SSyymmbboolliiccCClluusstteerr = _S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e_:_:_C_l_u_s_t_e_r │ │ │ │ │ -  typedef for wrapper: │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Date │ │ │ │ │ - Mar 29, 2013 │ │ │ │ │ + Oct 29, 2009 │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_y_m_b_o_l_i_c │ │ │ │ │ - * _S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e_._h │ │ │ │ │ + * _S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01355.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,6 @@ │ │ │ │ │ var a01355 = [ │ │ │ │ │ - ["SymbolicCluster", "a01355.html#ad78d0a5ba773ceac7ff253525032a15e", null] │ │ │ │ │ + ["gtsam::SymbolicBayesTreeClique", "a04980.html", null], │ │ │ │ │ + ["gtsam::SymbolicBayesTree", "a04984.html", "a04984"], │ │ │ │ │ + ["gtsam::traits< SymbolicBayesTreeClique >", "a04988.html", null], │ │ │ │ │ + ["gtsam::traits< SymbolicBayesTree >", "a04992.html", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01355_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicJunctionTree.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicBayesTree.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,15 +98,15 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
SymbolicJunctionTree.h
│ │ │ │ +
SymbolicBayesTree.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │ @@ -114,52 +114,87 @@ │ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/symbolic/SymbolicFactorGraph.h>
│ │ │ │ -
22#include <gtsam/symbolic/SymbolicBayesTree.h>
│ │ │ │ -
23#include <gtsam/inference/JunctionTree.h>
│ │ │ │ -
24
│ │ │ │ -
25namespace gtsam {
│ │ │ │ -
26
│ │ │ │ -
27 // Forward declarations
│ │ │ │ -
28 class SymbolicEliminationTree;
│ │ │ │ -
29
│ │ │ │ -
│ │ │ │ -
50 class GTSAM_EXPORT SymbolicJunctionTree :
│ │ │ │ -
51 public JunctionTree<SymbolicBayesTree, SymbolicFactorGraph> {
│ │ │ │ -
52 public:
│ │ │ │ -
53 typedef JunctionTree<SymbolicBayesTree, SymbolicFactorGraph> Base;
│ │ │ │ -
54 typedef SymbolicJunctionTree This;
│ │ │ │ -
55 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
56
│ │ │ │ -
65 SymbolicJunctionTree(const SymbolicEliminationTree& eliminationTree);
│ │ │ │ -
66 };
│ │ │ │ -
│ │ │ │ -
67
│ │ │ │ -
69 using SymbolicCluster = SymbolicJunctionTree::Cluster;
│ │ │ │ -
70}
│ │ │ │ -
JunctionTree.h
The junction tree.
│ │ │ │ - │ │ │ │ - │ │ │ │ +
21#include <gtsam/symbolic/SymbolicBayesNet.h>
│ │ │ │ +
22#include <gtsam/symbolic/SymbolicFactorGraph.h>
│ │ │ │ +
23#include <gtsam/inference/BayesTree.h>
│ │ │ │ +
24#include <gtsam/inference/BayesTreeCliqueBase.h>
│ │ │ │ +
25
│ │ │ │ +
26namespace gtsam {
│ │ │ │ +
27
│ │ │ │ +
28 // Forward declarations
│ │ │ │ +
29 class SymbolicConditional;
│ │ │ │ +
30
│ │ │ │ +
31 /* ************************************************************************* */
│ │ │ │ +
│ │ │ │ +
33 class GTSAM_EXPORT SymbolicBayesTreeClique :
│ │ │ │ +
34 public BayesTreeCliqueBase<SymbolicBayesTreeClique, SymbolicFactorGraph>
│ │ │ │ +
35 {
│ │ │ │ +
36 public:
│ │ │ │ +
37 typedef SymbolicBayesTreeClique This;
│ │ │ │ +
38 typedef BayesTreeCliqueBase<SymbolicBayesTreeClique, SymbolicFactorGraph> Base;
│ │ │ │ +
39 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
40 typedef boost::weak_ptr<This> weak_ptr;
│ │ │ │ +
41 SymbolicBayesTreeClique() {}
│ │ │ │ +
42 virtual ~SymbolicBayesTreeClique() {}
│ │ │ │ +
43 SymbolicBayesTreeClique(const boost::shared_ptr<SymbolicConditional>& conditional) : Base(conditional) {}
│ │ │ │ +
44 };
│ │ │ │ +
│ │ │ │ +
45
│ │ │ │ +
46 /* ************************************************************************* */
│ │ │ │ +
│ │ │ │ +
49 class GTSAM_EXPORT SymbolicBayesTree :
│ │ │ │ +
50 public BayesTree<SymbolicBayesTreeClique>
│ │ │ │ +
51 {
│ │ │ │ +
52 private:
│ │ │ │ +
53 typedef BayesTree<SymbolicBayesTreeClique> Base;
│ │ │ │ +
54
│ │ │ │ +
55 public:
│ │ │ │ +
56 typedef SymbolicBayesTree This;
│ │ │ │ +
57 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
58
│ │ │ │ +
60 SymbolicBayesTree() {}
│ │ │ │ +
61
│ │ │ │ +
63 bool equals(const This& other, double tol = 1e-9) const;
│ │ │ │ +
64
│ │ │ │ +
65 private:
│ │ │ │ +
67 friend class boost::serialization::access;
│ │ │ │ +
68 template<class ARCHIVE>
│ │ │ │ +
69 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
70 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
71 }
│ │ │ │ +
72 };
│ │ │ │ +
│ │ │ │ +
73
│ │ │ │ +
75template<> struct traits<SymbolicBayesTreeClique> : public Testable<SymbolicBayesTreeClique> {};
│ │ │ │ +
76template<> struct traits<SymbolicBayesTree> : public Testable<SymbolicBayesTree> {};
│ │ │ │ +
77
│ │ │ │ +
78} //\ namespace gtsam
│ │ │ │ +
79
│ │ │ │ +
BayesTreeCliqueBase.h
Base class for cliques of a BayesTree.
│ │ │ │ +
BayesTree.h
Bayes Tree is a tree of cliques of a Bayes Chain.
│ │ │ │ + │ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::ClusterTree::Cluster
A Cluster is just a collection of factors.
Definition ClusterTree.h:36
│ │ │ │ -
gtsam::JunctionTree
A JunctionTree is a cluster tree, a set of variable clusters with factors, arranged in a tree,...
Definition JunctionTree.h:50
│ │ │ │ -
gtsam::SymbolicEliminationTree
Definition SymbolicEliminationTree.h:29
│ │ │ │ -
gtsam::SymbolicJunctionTree
A EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree,...
Definition SymbolicJunctionTree.h:51
│ │ │ │ -
gtsam::SymbolicJunctionTree::shared_ptr
boost::shared_ptr< This > shared_ptr
Shared pointer to this class.
Definition SymbolicJunctionTree.h:55
│ │ │ │ -
gtsam::SymbolicJunctionTree::Base
JunctionTree< SymbolicBayesTree, SymbolicFactorGraph > Base
Base class.
Definition SymbolicJunctionTree.h:53
│ │ │ │ -
gtsam::SymbolicJunctionTree::This
SymbolicJunctionTree This
This class.
Definition SymbolicJunctionTree.h:54
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::BayesTree
Bayes tree.
Definition BayesTree.h:67
│ │ │ │ +
gtsam::BayesTreeCliqueBase
This is the base class for BayesTree cliques.
Definition BayesTreeCliqueBase.h:50
│ │ │ │ +
gtsam::SymbolicBayesTreeClique
A clique in a SymbolicBayesTree.
Definition SymbolicBayesTree.h:35
│ │ │ │ +
gtsam::SymbolicBayesTree
A Bayes tree that represents the connectivity between variables but is not associated with any probab...
Definition SymbolicBayesTree.h:51
│ │ │ │ +
gtsam::SymbolicBayesTree::SymbolicBayesTree
SymbolicBayesTree()
Default constructor, creates an empty Bayes tree.
Definition SymbolicBayesTree.h:60
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SymbolicJunctionTree.h │ │ │ │ │ +SymbolicBayesTree.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ @@ -16,64 +16,105 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 19#pragma once │ │ │ │ │ 20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_s_y_m_b_o_l_i_c_/_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_s_y_m_b_o_l_i_c_/_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_J_u_n_c_t_i_o_n_T_r_e_e_._h> │ │ │ │ │ -24 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ -26 │ │ │ │ │ -27 // Forward declarations │ │ │ │ │ -28 class SymbolicEliminationTree; │ │ │ │ │ -29 │ │ │ │ │ -_5_0 class GTSAM_EXPORT _S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e : │ │ │ │ │ -51 public _J_u_n_c_t_i_o_n_T_r_e_e { │ │ │ │ │ -52 public: │ │ │ │ │ -_5_3 typedef _J_u_n_c_t_i_o_n_T_r_e_e_<_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_,_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_> _B_a_s_e; │ │ │ │ │ -_5_4 typedef _S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e _T_h_i_s; │ │ │ │ │ -_5_5 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -56 │ │ │ │ │ -65 _S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e(const _S_y_m_b_o_l_i_c_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e& eliminationTree); │ │ │ │ │ -66 }; │ │ │ │ │ -67 │ │ │ │ │ -_6_9 using _S_y_m_b_o_l_i_c_C_l_u_s_t_e_r = _S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e_:_:_C_l_u_s_t_e_r; │ │ │ │ │ -70} │ │ │ │ │ -_J_u_n_c_t_i_o_n_T_r_e_e_._h │ │ │ │ │ -The junction tree. │ │ │ │ │ +21#include <_g_t_s_a_m_/_s_y_m_b_o_l_i_c_/_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_s_y_m_b_o_l_i_c_/_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._h> │ │ │ │ │ +25 │ │ │ │ │ +26namespace _g_t_s_a_m { │ │ │ │ │ +27 │ │ │ │ │ +28 // Forward declarations │ │ │ │ │ +29 class SymbolicConditional; │ │ │ │ │ +30 │ │ │ │ │ +31 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +_3_3 class GTSAM_EXPORT _S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e : │ │ │ │ │ +34 public _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e │ │ │ │ │ +35 { │ │ │ │ │ +36 public: │ │ │ │ │ +37 typedef _S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e _T_h_i_s; │ │ │ │ │ +38 typedef _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_,_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_> │ │ │ │ │ +_B_a_s_e; │ │ │ │ │ +39 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +40 typedef boost::weak_ptr weak_ptr; │ │ │ │ │ +41 _S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e() {} │ │ │ │ │ +42 virtual _~_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e() {} │ │ │ │ │ +43 _S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e(const boost::shared_ptr& │ │ │ │ │ +conditional) : _B_a_s_e(conditional) {} │ │ │ │ │ +44 }; │ │ │ │ │ +45 │ │ │ │ │ +46 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +_4_9 class GTSAM_EXPORT _S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e : │ │ │ │ │ +50 public _B_a_y_e_s_T_r_e_e │ │ │ │ │ +51 { │ │ │ │ │ +52 private: │ │ │ │ │ +53 typedef _B_a_y_e_s_T_r_e_e_<_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_> _B_a_s_e; │ │ │ │ │ +54 │ │ │ │ │ +55 public: │ │ │ │ │ +56 typedef _S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e _T_h_i_s; │ │ │ │ │ +57 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +58 │ │ │ │ │ +_6_0 _S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e() {} │ │ │ │ │ +61 │ │ │ │ │ +63 bool _e_q_u_a_l_s(const This& other, double tol = 1e-9) const; │ │ │ │ │ +64 │ │ │ │ │ +65 private: │ │ │ │ │ +_6_7 friend class boost::serialization::access; │ │ │ │ │ +68 template │ │ │ │ │ +69 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +70 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +71 } │ │ │ │ │ +72 }; │ │ │ │ │ +73 │ │ │ │ │ +_7_5template<> struct _t_r_a_i_t_s<_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e> : public │ │ │ │ │ +_T_e_s_t_a_b_l_e {}; │ │ │ │ │ +_7_6template<> struct _t_r_a_i_t_s<_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e> : public │ │ │ │ │ +_T_e_s_t_a_b_l_e {}; │ │ │ │ │ +77 │ │ │ │ │ +78} //\ namespace gtsam │ │ │ │ │ +79 │ │ │ │ │ +_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._h │ │ │ │ │ +Base class for cliques of a BayesTree. │ │ │ │ │ +_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ +Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ │ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ +_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r │ │ │ │ │ -A Cluster is just a collection of factors. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ -A JunctionTree is a cluster tree, a set of variable clusters with factors, │ │ │ │ │ -arranged in a tree,... │ │ │ │ │ -DDeeffiinniittiioonn JunctionTree.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ -DDeeffiinniittiioonn SymbolicEliminationTree.h:29 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ -A EliminatableClusterTree, i.e., a set of variable clusters with factors, │ │ │ │ │ -arranged in a tree,... │ │ │ │ │ -DDeeffiinniittiioonn SymbolicJunctionTree.h:51 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -Shared pointer to this class. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicJunctionTree.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e_:_:_B_a_s_e │ │ │ │ │ -JunctionTree< SymbolicBayesTree, SymbolicFactorGraph > Base │ │ │ │ │ -Base class. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicJunctionTree.h:53 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e_:_:_T_h_i_s │ │ │ │ │ -SymbolicJunctionTree This │ │ │ │ │ -This class. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicJunctionTree.h:54 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e │ │ │ │ │ +Bayes tree. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e │ │ │ │ │ +This is the base class for BayesTree cliques. │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e │ │ │ │ │ +A clique in a SymbolicBayesTree. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicBayesTree.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e │ │ │ │ │ +A Bayes tree that represents the connectivity between variables but is not │ │ │ │ │ +associated with any probab... │ │ │ │ │ +DDeeffiinniittiioonn SymbolicBayesTree.h:51 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e │ │ │ │ │ +SymbolicBayesTree() │ │ │ │ │ +Default constructor, creates an empty Bayes tree. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicBayesTree.h:60 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_y_m_b_o_l_i_c │ │ │ │ │ - * _S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e_._h │ │ │ │ │ + * _S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01358.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicBayesNet.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,34 +94,39 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces
│ │ │ │ -
SymbolicBayesNet.cpp File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
SymbolicFactor.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

std::pair< boost::shared_ptr< SymbolicConditional >, boost::shared_ptr< SymbolicFactor > > gtsam::EliminateSymbolic (const SymbolicFactorGraph &factors, const Ordering &keys)
 Dense elimination function for symbolic factors.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Oct 29, 2009
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Oct 17, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,27 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SymbolicBayesNet.cpp File Reference │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +SymbolicFactor.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ + std::pair< boost::shared_ptr< _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_S_y_m_b_o_l_i_c (const │ │ │ │ │ + _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l >, boost:: _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h &factors, const │ │ │ │ │ +shared_ptr< _S_y_m_b_o_l_i_c_F_a_c_t_o_r > >  _O_r_d_e_r_i_n_g &keys) │ │ │ │ │ +  Dense elimination function for symbolic │ │ │ │ │ + factors. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Oct 29, 2009 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Oct 17, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_y_m_b_o_l_i_c │ │ │ │ │ - * _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_._c_p_p │ │ │ │ │ + * _S_y_m_b_o_l_i_c_F_a_c_t_o_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01361.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicEliminationTree.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicConditional.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,45 +94,32 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
SymbolicEliminationTree.h File Reference
│ │ │ │ +
SymbolicConditional.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::SymbolicEliminationTree
 
struct  gtsam::traits< SymbolicEliminationTree >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Mar 29, 2013
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Oct 17, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,20 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SymbolicEliminationTree.h File Reference │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +SymbolicConditional.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Mar 29, 2013 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Oct 17, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_y_m_b_o_l_i_c │ │ │ │ │ - * _S_y_m_b_o_l_i_c_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h │ │ │ │ │ + * _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01364.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicBayesNet.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicFactor-inst.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,46 +94,76 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
SymbolicBayesNet.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
SymbolicFactor-inst.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::SymbolicBayesNet
 A SymbolicBayesNet is a Bayes Net of purely symbolic conditionals. More...
 
struct  gtsam::traits< SymbolicBayesNet >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

template<class FACTOR >
std::pair< boost::shared_ptr< SymbolicConditional >, boost::shared_ptr< SymbolicFactor > > gtsam::internal::EliminateSymbolic (const FactorGraph< FACTOR > &factors, const Ordering &keys)
 Implementation of dense elimination function for symbolic factors.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Oct 29, 2009
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ -
│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Oct 17, 2010
│ │ │ │ +

Function Documentation

│ │ │ │ + │ │ │ │ +

◆ EliminateSymbolic()

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +template<class FACTOR >
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
std::pair< boost::shared_ptr< SymbolicConditional >, boost::shared_ptr< SymbolicFactor > > gtsam::internal::EliminateSymbolic (const FactorGraph< FACTOR > & factors,
const Ordering & keys 
)
│ │ │ │ +
│ │ │ │ + │ │ │ │ +

Implementation of dense elimination function for symbolic factors.

│ │ │ │ +

This is a templated version for internally doing symbolic elimination on any factor.

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,30 +1,41 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SymbolicBayesNet.h File Reference │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +SymbolicFactor-inst.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t │ │ │ │ │ -  A _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t is a Bayes Net of purely symbolic conditionals. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +template │ │ │ │ │ + std::pair< boost::shared_ptr< _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_E_l_i_m_i_n_a_t_e_S_y_m_b_o_l_i_c (const │ │ │ │ │ + _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l >, boost:: _F_a_c_t_o_r_G_r_a_p_h< FACTOR > &factors, const │ │ │ │ │ +shared_ptr< _S_y_m_b_o_l_i_c_F_a_c_t_o_r > >  _O_r_d_e_r_i_n_g &keys) │ │ │ │ │ +  Implementation of dense elimination │ │ │ │ │ + function for symbolic factors. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Oct 29, 2009 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Oct 17, 2010 │ │ │ │ │ +********** FFuunnccttiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ +********** _?◆_? EElliimmiinnaatteeSSyymmbboolliicc(()) ********** │ │ │ │ │ +template │ │ │ │ │ +std::pair< boost::shared_ptr< │ │ │ │ │ +SymbolicConditional >, boost:: ( const _F_a_c_t_o_r_G_r_a_p_h< FACTOR > &  ffaaccttoorrss, │ │ │ │ │ +shared_ptr< SymbolicFactor > > │ │ │ │ │ +gtsam::internal::EliminateSymbolic │ │ │ │ │ + const _O_r_d_e_r_i_n_g &  kkeeyyss  │ │ │ │ │ + ) │ │ │ │ │ +Implementation of dense elimination function for symbolic factors. │ │ │ │ │ +This is a templated version for internally doing symbolic elimination on any │ │ │ │ │ +factor. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_y_m_b_o_l_i_c │ │ │ │ │ - * _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_._h │ │ │ │ │ + * _S_y_m_b_o_l_i_c_F_a_c_t_o_r_-_i_n_s_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01364.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,3 +1,3 @@ │ │ │ │ │ var a01364 = [ │ │ │ │ │ - ["gtsam::traits< SymbolicBayesNet >", "a04976.html", null] │ │ │ │ │ + ["EliminateSymbolic", "a01364.html#ae394348e1a503834f713405afa9afad8", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01364_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicBayesNet.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicFactor-inst.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,146 +98,99 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
SymbolicBayesNet.h
│ │ │ │ +
SymbolicFactor-inst.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/symbolic/SymbolicConditional.h>
│ │ │ │ -
22#include <gtsam/inference/BayesNet.h>
│ │ │ │ -
23#include <gtsam/inference/FactorGraph.h>
│ │ │ │ -
24#include <gtsam/base/types.h>
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/symbolic/SymbolicFactor.h>
│ │ │ │ +
21#include <gtsam/symbolic/SymbolicConditional.h>
│ │ │ │ +
22#include <gtsam/inference/Factor.h>
│ │ │ │ +
23#include <gtsam/inference/Key.h>
│ │ │ │ +
24#include <gtsam/base/timing.h>
│ │ │ │
25
│ │ │ │ -
26namespace gtsam {
│ │ │ │ -
27
│ │ │ │ -
│ │ │ │ -
32 class SymbolicBayesNet : public BayesNet<SymbolicConditional> {
│ │ │ │ -
33 public:
│ │ │ │ -
34 typedef BayesNet<SymbolicConditional> Base;
│ │ │ │ -
35 typedef SymbolicBayesNet This;
│ │ │ │ -
36 typedef SymbolicConditional ConditionalType;
│ │ │ │ -
37 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
38 typedef boost::shared_ptr<ConditionalType> sharedConditional;
│ │ │ │ -
39
│ │ │ │ +
26#include <boost/shared_ptr.hpp>
│ │ │ │ +
27#include <boost/make_shared.hpp>
│ │ │ │ +
28
│ │ │ │ +
29#include <utility>
│ │ │ │ +
30
│ │ │ │ +
31namespace gtsam
│ │ │ │ +
32{
│ │ │ │ +
33 namespace internal
│ │ │ │ +
34 {
│ │ │ │ +
37 template<class FACTOR>
│ │ │ │ +
38 std::pair<boost::shared_ptr<SymbolicConditional>, boost::shared_ptr<SymbolicFactor> >
│ │ │ │ +
│ │ │ │ +
39 EliminateSymbolic(const FactorGraph<FACTOR>& factors, const Ordering& keys)
│ │ │ │ +
40 {
│ │ │ │ +
41 gttic(EliminateSymbolic);
│ │ │ │
42
│ │ │ │ -
44 SymbolicBayesNet() {}
│ │ │ │ -
45
│ │ │ │ -
47 template <typename ITERATOR>
│ │ │ │ -
│ │ │ │ -
48 SymbolicBayesNet(ITERATOR firstConditional, ITERATOR lastConditional)
│ │ │ │ -
49 : Base(firstConditional, lastConditional) {}
│ │ │ │ -
│ │ │ │ -
50
│ │ │ │ -
52 template <class CONTAINER>
│ │ │ │ -
│ │ │ │ -
53 explicit SymbolicBayesNet(const CONTAINER& conditionals) {
│ │ │ │ -
54 push_back(conditionals);
│ │ │ │ -
55 }
│ │ │ │ -
│ │ │ │ -
56
│ │ │ │ -
59 template <class DERIVEDCONDITIONAL>
│ │ │ │ -
│ │ │ │ -
60 explicit SymbolicBayesNet(const FactorGraph<DERIVEDCONDITIONAL>& graph)
│ │ │ │ -
61 : Base(graph) {}
│ │ │ │ -
│ │ │ │ -
62
│ │ │ │ -
│ │ │ │ -
67 SymbolicBayesNet(std::initializer_list<boost::shared_ptr<SymbolicConditional>> conditionals)
│ │ │ │ -
68 : Base(conditionals) {}
│ │ │ │ -
│ │ │ │ -
69
│ │ │ │ -
│ │ │ │ -
71 SymbolicBayesNet(SymbolicConditional&& c) {
│ │ │ │ -
72 push_back(boost::make_shared<SymbolicConditional>(c));
│ │ │ │ -
73 }
│ │ │ │ -
│ │ │ │ -
74
│ │ │ │ -
│ │ │ │ -
81 SymbolicBayesNet& operator()(SymbolicConditional&& c) {
│ │ │ │ -
82 push_back(boost::make_shared<SymbolicConditional>(c));
│ │ │ │ -
83 return *this;
│ │ │ │ -
84 }
│ │ │ │ -
│ │ │ │ -
85
│ │ │ │ -
87 virtual ~SymbolicBayesNet() {}
│ │ │ │ -
88
│ │ │ │ -
90
│ │ │ │ -
93
│ │ │ │ -
95 GTSAM_EXPORT bool equals(const This& bn, double tol = 1e-9) const;
│ │ │ │ -
96
│ │ │ │ -
│ │ │ │ -
98 GTSAM_EXPORT void print(
│ │ │ │ -
99 const std::string& s = "SymbolicBayesNet",
│ │ │ │ -
100 const KeyFormatter& formatter = DefaultKeyFormatter) const override {
│ │ │ │ -
101 Base::print(s, formatter);
│ │ │ │ -
102 }
│ │ │ │ -
│ │ │ │ -
103
│ │ │ │ -
105
│ │ │ │ -
106 private:
│ │ │ │ -
108 friend class boost::serialization::access;
│ │ │ │ -
109 template<class ARCHIVE>
│ │ │ │ -
110 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
111 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
112 }
│ │ │ │ -
113};
│ │ │ │ -
│ │ │ │ -
114
│ │ │ │ -
116 template<>
│ │ │ │ -
│ │ │ │ -
117 struct traits<SymbolicBayesNet> : public Testable<SymbolicBayesNet> {
│ │ │ │ -
118 };
│ │ │ │ -
│ │ │ │ -
119
│ │ │ │ -
120} //\ namespace gtsam
│ │ │ │ -
types.h
Typedefs for easier changing of types.
│ │ │ │ -
FactorGraph.h
Factor Graph Base Class.
│ │ │ │ -
BayesNet.h
Bayes network.
│ │ │ │ - │ │ │ │ +
43 // Gather all keys
│ │ │ │ +
44 KeySet allKeys;
│ │ │ │ +
45 for(const boost::shared_ptr<FACTOR>& factor: factors) {
│ │ │ │ +
46 allKeys.insert(factor->begin(), factor->end());
│ │ │ │ +
47 }
│ │ │ │ +
48
│ │ │ │ +
49 // Check keys
│ │ │ │ +
50 for(Key key: keys) {
│ │ │ │ +
51 if(allKeys.find(key) == allKeys.end())
│ │ │ │ +
52 throw std::runtime_error("Requested to eliminate a key that is not in the factors");
│ │ │ │ +
53 }
│ │ │ │ +
54
│ │ │ │ +
55 // Sort frontal keys
│ │ │ │ +
56 KeySet frontals(keys);
│ │ │ │ +
57 const size_t nFrontals = keys.size();
│ │ │ │ +
58
│ │ │ │ +
59 // Build a key vector with the frontals followed by the separator
│ │ │ │ +
60 KeyVector orderedKeys(allKeys.size());
│ │ │ │ +
61 std::copy(keys.begin(), keys.end(), orderedKeys.begin());
│ │ │ │ +
62 std::set_difference(allKeys.begin(), allKeys.end(), frontals.begin(), frontals.end(), orderedKeys.begin() + nFrontals);
│ │ │ │ +
63
│ │ │ │ +
64 // Return resulting conditional and factor
│ │ │ │ +
65 return std::make_pair(
│ │ │ │ +
66 SymbolicConditional::FromKeysShared(orderedKeys, nFrontals),
│ │ │ │ +
67 SymbolicFactor::FromIteratorsShared(orderedKeys.begin() + nFrontals, orderedKeys.end()));
│ │ │ │ +
68 }
│ │ │ │ +
│ │ │ │ +
69 }
│ │ │ │ +
70}
│ │ │ │ +
timing.h
Timing utilities.
│ │ │ │ +
Factor.h
The base class for all factors.
│ │ │ │ + │ │ │ │ + │ │ │ │ +
gtsam::internal::EliminateSymbolic
std::pair< boost::shared_ptr< SymbolicConditional >, boost::shared_ptr< SymbolicFactor > > EliminateSymbolic(const FactorGraph< FACTOR > &factors, const Ordering &keys)
Implementation of dense elimination function for symbolic factors.
Definition SymbolicFactor-inst.h:39
│ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::BayesNet
A BayesNet is a tree of conditionals, stored in elimination order.
Definition BayesNet.h:35
│ │ │ │ -
gtsam::BayesNet< SymbolicConditional >::print
void print(const std::string &s="BayesNet", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print out graph
Definition BayesNet-inst.h:32
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ + │ │ │ │
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ -
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition FactorGraph.h:186
│ │ │ │ -
gtsam::SymbolicBayesNet
A SymbolicBayesNet is a Bayes Net of purely symbolic conditionals.
Definition SymbolicBayesNet.h:32
│ │ │ │ -
gtsam::SymbolicBayesNet::~SymbolicBayesNet
virtual ~SymbolicBayesNet()
Destructor.
Definition SymbolicBayesNet.h:87
│ │ │ │ -
gtsam::SymbolicBayesNet::SymbolicBayesNet
SymbolicBayesNet(SymbolicConditional &&c)
Construct from a single conditional.
Definition SymbolicBayesNet.h:71
│ │ │ │ -
gtsam::SymbolicBayesNet::print
GTSAM_EXPORT void print(const std::string &s="SymbolicBayesNet", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print
Definition SymbolicBayesNet.h:98
│ │ │ │ -
gtsam::SymbolicBayesNet::SymbolicBayesNet
SymbolicBayesNet(ITERATOR firstConditional, ITERATOR lastConditional)
Construct from iterator over conditionals.
Definition SymbolicBayesNet.h:48
│ │ │ │ -
gtsam::SymbolicBayesNet::SymbolicBayesNet
SymbolicBayesNet(std::initializer_list< boost::shared_ptr< SymbolicConditional > > conditionals)
Constructor that takes an initializer list of shared pointers.
Definition SymbolicBayesNet.h:67
│ │ │ │ -
gtsam::SymbolicBayesNet::SymbolicBayesNet
SymbolicBayesNet(const CONTAINER &conditionals)
Construct from container of factors (shared_ptr or plain objects)
Definition SymbolicBayesNet.h:53
│ │ │ │ -
gtsam::SymbolicBayesNet::SymbolicBayesNet
SymbolicBayesNet()
Construct empty factor graph.
Definition SymbolicBayesNet.h:44
│ │ │ │ -
gtsam::SymbolicBayesNet::access
friend class boost::serialization::access
Serialization function.
Definition SymbolicBayesNet.h:108
│ │ │ │ -
gtsam::SymbolicBayesNet::operator()
SymbolicBayesNet & operator()(SymbolicConditional &&c)
Add a single conditional and return a reference.
Definition SymbolicBayesNet.h:81
│ │ │ │ -
gtsam::SymbolicBayesNet::SymbolicBayesNet
SymbolicBayesNet(const FactorGraph< DERIVEDCONDITIONAL > &graph)
Implicit copy/downcast constructor to override explicit template container constructor.
Definition SymbolicBayesNet.h:60
│ │ │ │ -
gtsam::SymbolicConditional
SymbolicConditional is a conditional with keys but no probability data, produced by symbolic eliminat...
Definition SymbolicConditional.h:38
│ │ │ │ +
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
gtsam::SymbolicConditional::FromKeysShared
static SymbolicConditional::shared_ptr FromKeysShared(const CONTAINER &keys, size_t nrFrontals)
Named constructor from an arbitrary number of keys and frontals.
Definition SymbolicConditional.h:94
│ │ │ │ +
gtsam::SymbolicFactor::FromIteratorsShared
static SymbolicFactor::shared_ptr FromIteratorsShared(KEYITERATOR beginKey, KEYITERATOR endKey)
Constructor from a collection of keys.
Definition SymbolicFactor.h:121
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,186 +1,120 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SymbolicBayesNet.h │ │ │ │ │ +SymbolicFactor-inst.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_s_y_m_b_o_l_i_c_/_S_y_m_b_o_l_i_c_F_a_c_t_o_r_._h> │ │ │ │ │ 21#include <_g_t_s_a_m_/_s_y_m_b_o_l_i_c_/_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_b_a_s_e_/_t_i_m_i_n_g_._h> │ │ │ │ │ 25 │ │ │ │ │ -26namespace _g_t_s_a_m { │ │ │ │ │ -27 │ │ │ │ │ -_3_2 class _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t : public _B_a_y_e_s_N_e_t { │ │ │ │ │ -33 public: │ │ │ │ │ -34 typedef _B_a_y_e_s_N_e_t_<_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_> _B_a_s_e; │ │ │ │ │ -35 typedef _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t _T_h_i_s; │ │ │ │ │ -36 typedef _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l _C_o_n_d_i_t_i_o_n_a_l_T_y_p_e; │ │ │ │ │ -37 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -38 typedef boost::shared_ptr sharedConditional; │ │ │ │ │ -39 │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28 │ │ │ │ │ +29#include │ │ │ │ │ +30 │ │ │ │ │ +31namespace _g_t_s_a_m │ │ │ │ │ +32{ │ │ │ │ │ +33 namespace internal │ │ │ │ │ +34 { │ │ │ │ │ +37 template │ │ │ │ │ +38 std::pair, boost:: │ │ │ │ │ +shared_ptr > │ │ │ │ │ +_3_9 _E_l_i_m_i_n_a_t_e_S_y_m_b_o_l_i_c(const _F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_>& factors, const _O_r_d_e_r_i_n_g& keys) │ │ │ │ │ +40 { │ │ │ │ │ +41 gttic(_E_l_i_m_i_n_a_t_e_S_y_m_b_o_l_i_c); │ │ │ │ │ 42 │ │ │ │ │ -_4_4 _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t() {} │ │ │ │ │ -45 │ │ │ │ │ -47 template │ │ │ │ │ -_4_8 _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t(ITERATOR firstConditional, ITERATOR lastConditional) │ │ │ │ │ -49 : _B_a_s_e(firstConditional, lastConditional) {} │ │ │ │ │ -50 │ │ │ │ │ -52 template │ │ │ │ │ -_5_3 explicit _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t(const CONTAINER& conditionals) { │ │ │ │ │ -54 _p_u_s_h___b_a_c_k(conditionals); │ │ │ │ │ -55 } │ │ │ │ │ -56 │ │ │ │ │ -59 template │ │ │ │ │ -_6_0 explicit _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t(const _F_a_c_t_o_r_G_r_a_p_h_<_D_E_R_I_V_E_D_C_O_N_D_I_T_I_O_N_A_L_>& graph) │ │ │ │ │ -61 : _B_a_s_e(graph) {} │ │ │ │ │ -62 │ │ │ │ │ -_6_7 _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t(std::initializer_list> conditionals) │ │ │ │ │ -68 : _B_a_s_e(conditionals) {} │ │ │ │ │ -69 │ │ │ │ │ -_7_1 _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t(_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l&& c) { │ │ │ │ │ -72 _p_u_s_h___b_a_c_k(boost::make_shared(c)); │ │ │ │ │ -73 } │ │ │ │ │ -74 │ │ │ │ │ -_8_1 _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t& _o_p_e_r_a_t_o_r_(_)(_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l&& c) { │ │ │ │ │ -82 _p_u_s_h___b_a_c_k(boost::make_shared(c)); │ │ │ │ │ -83 return *this; │ │ │ │ │ -84 } │ │ │ │ │ -85 │ │ │ │ │ -_8_7 virtual _~_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t() {} │ │ │ │ │ -88 │ │ │ │ │ -90 │ │ │ │ │ -93 │ │ │ │ │ -95 GTSAM_EXPORT bool _e_q_u_a_l_s(const This& bn, double tol = 1e-9) const; │ │ │ │ │ -96 │ │ │ │ │ -_9_8 GTSAM_EXPORT void _p_r_i_n_t( │ │ │ │ │ -99 const std::string& s = "SymbolicBayesNet", │ │ │ │ │ -100 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override { │ │ │ │ │ -101 _B_a_s_e_:_:_p_r_i_n_t(s, formatter); │ │ │ │ │ -102 } │ │ │ │ │ -103 │ │ │ │ │ -105 │ │ │ │ │ -106 private: │ │ │ │ │ -_1_0_8 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -109 template │ │ │ │ │ -110 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -111 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -112 } │ │ │ │ │ -113}; │ │ │ │ │ -114 │ │ │ │ │ -116 template<> │ │ │ │ │ -_1_1_7 struct _t_r_a_i_t_s<_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t> : public _T_e_s_t_a_b_l_e { │ │ │ │ │ -118 }; │ │ │ │ │ -119 │ │ │ │ │ -120} //\ namespace gtsam │ │ │ │ │ -_t_y_p_e_s_._h │ │ │ │ │ -Typedefs for easier changing of types. │ │ │ │ │ -_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Factor Graph Base Class. │ │ │ │ │ -_B_a_y_e_s_N_e_t_._h │ │ │ │ │ -Bayes network. │ │ │ │ │ +43 // Gather all keys │ │ │ │ │ +44 _K_e_y_S_e_t allKeys; │ │ │ │ │ +45 for(const boost::shared_ptr& factor: factors) { │ │ │ │ │ +46 allKeys.insert(factor->begin(), factor->end()); │ │ │ │ │ +47 } │ │ │ │ │ +48 │ │ │ │ │ +49 // Check keys │ │ │ │ │ +50 for(_K_e_y key: keys) { │ │ │ │ │ +51 if(allKeys.find(key) == allKeys.end()) │ │ │ │ │ +52 throw std::runtime_error("Requested to eliminate a key that is not in the │ │ │ │ │ +factors"); │ │ │ │ │ +53 } │ │ │ │ │ +54 │ │ │ │ │ +55 // Sort frontal keys │ │ │ │ │ +56 _K_e_y_S_e_t frontals(keys); │ │ │ │ │ +57 const size_t nFrontals = keys.size(); │ │ │ │ │ +58 │ │ │ │ │ +59 // Build a key vector with the frontals followed by the separator │ │ │ │ │ +60 _K_e_y_V_e_c_t_o_r orderedKeys(allKeys.size()); │ │ │ │ │ +61 std::copy(keys.begin(), keys.end(), orderedKeys.begin()); │ │ │ │ │ +62 std::set_difference(allKeys.begin(), allKeys.end(), frontals.begin(), │ │ │ │ │ +frontals.end(), orderedKeys.begin() + nFrontals); │ │ │ │ │ +63 │ │ │ │ │ +64 // Return resulting conditional and factor │ │ │ │ │ +65 return std::make_pair( │ │ │ │ │ +66 _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_F_r_o_m_K_e_y_s_S_h_a_r_e_d(orderedKeys, nFrontals), │ │ │ │ │ +67 _S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_F_r_o_m_I_t_e_r_a_t_o_r_s_S_h_a_r_e_d(orderedKeys.begin() + nFrontals, │ │ │ │ │ +orderedKeys.end())); │ │ │ │ │ +68 } │ │ │ │ │ +69 } │ │ │ │ │ +70} │ │ │ │ │ +_t_i_m_i_n_g_._h │ │ │ │ │ +Timing utilities. │ │ │ │ │ +_F_a_c_t_o_r_._h │ │ │ │ │ +The base class for all factors. │ │ │ │ │ +_K_e_y_._h │ │ │ │ │ _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_E_l_i_m_i_n_a_t_e_S_y_m_b_o_l_i_c │ │ │ │ │ +std::pair< boost::shared_ptr< SymbolicConditional >, boost::shared_ptr< │ │ │ │ │ +SymbolicFactor > > EliminateSymbolic(const FactorGraph< FACTOR > &factors, │ │ │ │ │ +const Ordering &keys) │ │ │ │ │ +Implementation of dense elimination function for symbolic factors. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactor-inst.h:39 │ │ │ │ │ +_S_y_m_b_o_l_i_c_F_a_c_t_o_r_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t │ │ │ │ │ -A BayesNet is a tree of conditionals, stored in elimination order. │ │ │ │ │ -DDeeffiinniittiioonn BayesNet.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t_<_ _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_ _>_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="BayesNet", const KeyFormatter │ │ │ │ │ -&formatter=DefaultKeyFormatter) const override │ │ │ │ │ -print out graph │ │ │ │ │ -DDeeffiinniittiioonn BayesNet-inst.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ _g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ nodes. │ │ │ │ │ DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_p_u_s_h___b_a_c_k │ │ │ │ │ -IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor) │ │ │ │ │ -Add a factor directly using a shared_ptr. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:186 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t │ │ │ │ │ -A SymbolicBayesNet is a Bayes Net of purely symbolic conditionals. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicBayesNet.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_:_:_~_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t │ │ │ │ │ -virtual ~SymbolicBayesNet() │ │ │ │ │ -Destructor. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicBayesNet.h:87 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t │ │ │ │ │ -SymbolicBayesNet(SymbolicConditional &&c) │ │ │ │ │ -Construct from a single conditional. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicBayesNet.h:71 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_:_:_p_r_i_n_t │ │ │ │ │ -GTSAM_EXPORT void print(const std::string &s="SymbolicBayesNet", const │ │ │ │ │ -KeyFormatter &formatter=DefaultKeyFormatter) const override │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn SymbolicBayesNet.h:98 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t │ │ │ │ │ -SymbolicBayesNet(ITERATOR firstConditional, ITERATOR lastConditional) │ │ │ │ │ -Construct from iterator over conditionals. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicBayesNet.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t │ │ │ │ │ -SymbolicBayesNet(std::initializer_list< boost::shared_ptr< SymbolicConditional │ │ │ │ │ -> > conditionals) │ │ │ │ │ -Constructor that takes an initializer list of shared pointers. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicBayesNet.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t │ │ │ │ │ -SymbolicBayesNet(const CONTAINER &conditionals) │ │ │ │ │ -Construct from container of factors (shared_ptr or plain objects) │ │ │ │ │ -DDeeffiinniittiioonn SymbolicBayesNet.h:53 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t │ │ │ │ │ -SymbolicBayesNet() │ │ │ │ │ -Construct empty factor graph. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicBayesNet.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicBayesNet.h:108 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ -SymbolicBayesNet & operator()(SymbolicConditional &&c) │ │ │ │ │ -Add a single conditional and return a reference. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicBayesNet.h:81 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t │ │ │ │ │ -SymbolicBayesNet(const FactorGraph< DERIVEDCONDITIONAL > &graph) │ │ │ │ │ -Implicit copy/downcast constructor to override explicit template container │ │ │ │ │ -constructor. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicBayesNet.h:60 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -SymbolicConditional is a conditional with keys but no probability data, │ │ │ │ │ -produced by symbolic eliminat... │ │ │ │ │ -DDeeffiinniittiioonn SymbolicConditional.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_F_r_o_m_K_e_y_s_S_h_a_r_e_d │ │ │ │ │ +static SymbolicConditional::shared_ptr FromKeysShared(const CONTAINER &keys, │ │ │ │ │ +size_t nrFrontals) │ │ │ │ │ +Named constructor from an arbitrary number of keys and frontals. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicConditional.h:94 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_F_r_o_m_I_t_e_r_a_t_o_r_s_S_h_a_r_e_d │ │ │ │ │ +static SymbolicFactor::shared_ptr FromIteratorsShared(KEYITERATOR beginKey, │ │ │ │ │ +KEYITERATOR endKey) │ │ │ │ │ +Constructor from a collection of keys. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactor.h:121 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_y_m_b_o_l_i_c │ │ │ │ │ - * _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_._h │ │ │ │ │ + * _S_y_m_b_o_l_i_c_F_a_c_t_o_r_-_i_n_s_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01367.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicEliminationTree.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicFactorGraph.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,34 +94,48 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
SymbolicEliminationTree.cpp File Reference
│ │ │ │ +
SymbolicFactorGraph.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

struct  gtsam::EliminationTraits< SymbolicFactorGraph >
 
class  gtsam::SymbolicFactorGraph
 Symbolic Factor Graph. More...
 
struct  gtsam::traits< SymbolicFactorGraph >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Mar 29, 2013
│ │ │ │ +
Date
Oct 29, 2009
│ │ │ │
Author
Frank Dellaert
│ │ │ │
│ │ │ │ Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,31 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SymbolicEliminationTree.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +SymbolicFactorGraph.h File Reference │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +struct   _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_ _> │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +  Symbolic _F_a_c_t_o_r Graph. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Date │ │ │ │ │ - Mar 29, 2013 │ │ │ │ │ + Oct 29, 2009 │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_y_m_b_o_l_i_c │ │ │ │ │ - * _S_y_m_b_o_l_i_c_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._c_p_p │ │ │ │ │ + * _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01370.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicFactorGraph.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicBayesNet.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,32 +94,46 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
SymbolicFactorGraph.cpp File Reference
│ │ │ │ +
SymbolicBayesNet.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::SymbolicBayesNet
 A SymbolicBayesNet is a Bayes Net of purely symbolic conditionals. More...
 
struct  gtsam::traits< SymbolicBayesNet >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │
Date
Oct 29, 2009
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,30 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SymbolicFactorGraph.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +SymbolicBayesNet.h File Reference │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t │ │ │ │ │ +  A _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t is a Bayes Net of purely symbolic conditionals. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Date │ │ │ │ │ Oct 29, 2009 │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_y_m_b_o_l_i_c │ │ │ │ │ - * _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_._c_p_p │ │ │ │ │ + * _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01373.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicConditional.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicJunctionTree.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,31 +95,33 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces
│ │ │ │ -
SymbolicConditional.cpp File Reference
│ │ │ │ +
SymbolicJunctionTree.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Oct 17, 2010
│ │ │ │ +
Date
Mar 29, 2013
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,21 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SymbolicConditional.cpp File Reference │ │ │ │ │ +SymbolicJunctionTree.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ + Date │ │ │ │ │ + Mar 29, 2013 │ │ │ │ │ Author │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ - Date │ │ │ │ │ - Oct 17, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_y_m_b_o_l_i_c │ │ │ │ │ - * _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_._c_p_p │ │ │ │ │ + * _S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01379.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicFactorGraph.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicISAM.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,48 +94,34 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
SymbolicFactorGraph.h File Reference
│ │ │ │ +
SymbolicISAM.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

struct  gtsam::EliminationTraits< SymbolicFactorGraph >
 
class  gtsam::SymbolicFactorGraph
 Symbolic Factor Graph. More...
 
struct  gtsam::traits< SymbolicFactorGraph >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Oct 29, 2009
│ │ │ │ +
Date
July 29, 2013
│ │ │ │
Author
Frank Dellaert
│ │ │ │
│ │ │ │ Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,31 +1,21 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SymbolicFactorGraph.h File Reference │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_ _> │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -  Symbolic _F_a_c_t_o_r Graph. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +SymbolicISAM.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Date │ │ │ │ │ - Oct 29, 2009 │ │ │ │ │ + July 29, 2013 │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_y_m_b_o_l_i_c │ │ │ │ │ - * _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ + * _S_y_m_b_o_l_i_c_I_S_A_M_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01382.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicFactor.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicBayesNet.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,39 +94,34 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
SymbolicFactor.cpp File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
SymbolicBayesNet.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

std::pair< boost::shared_ptr< SymbolicConditional >, boost::shared_ptr< SymbolicFactor > > gtsam::EliminateSymbolic (const SymbolicFactorGraph &factors, const Ordering &keys)
 Dense elimination function for symbolic factors.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Oct 17, 2010
│ │ │ │ +
Date
Oct 29, 2009
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,27 +1,21 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -SymbolicFactor.cpp File Reference │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +SymbolicBayesNet.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ - std::pair< boost::shared_ptr< _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_S_y_m_b_o_l_i_c (const │ │ │ │ │ - _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l >, boost:: _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h &factors, const │ │ │ │ │ -shared_ptr< _S_y_m_b_o_l_i_c_F_a_c_t_o_r > >  _O_r_d_e_r_i_n_g &keys) │ │ │ │ │ -  Dense elimination function for symbolic │ │ │ │ │ - factors. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ + Date │ │ │ │ │ + Oct 29, 2009 │ │ │ │ │ Author │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ - Date │ │ │ │ │ - Oct 17, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_y_m_b_o_l_i_c │ │ │ │ │ - * _S_y_m_b_o_l_i_c_F_a_c_t_o_r_._c_p_p │ │ │ │ │ + * _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01385.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicISAM.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,50 +95,41 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
SymbolicFactor.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
SymbolicISAM.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::SymbolicFactor
 SymbolicFactor represents a symbolic factor that specifies graph topology but is not associated with any numerical function. More...
 
struct  gtsam::traits< SymbolicFactor >
 traits More...
class  gtsam::SymbolicISAM
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

std::pair< boost::shared_ptr< SymbolicConditional >, boost::shared_ptr< SymbolicFactor > > gtsam::EliminateSymbolic (const SymbolicFactorGraph &factors, const Ordering &keys)
 Dense elimination function for symbolic factors.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Oct 17, 2010
│ │ │ │ +
Date
July 29, 2013
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,36 +1,25 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -SymbolicFactor.h File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +SymbolicISAM.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r │ │ │ │ │ -  _S_y_m_b_o_l_i_c_F_a_c_t_o_r represents a symbolic factor that specifies graph │ │ │ │ │ - topology but is not associated with any numerical function. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_I_S_A_M │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ - std::pair< boost::shared_ptr< _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_S_y_m_b_o_l_i_c (const │ │ │ │ │ - _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l >, boost:: _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h &factors, const │ │ │ │ │ -shared_ptr< _S_y_m_b_o_l_i_c_F_a_c_t_o_r > >  _O_r_d_e_r_i_n_g &keys) │ │ │ │ │ -  Dense elimination function for symbolic │ │ │ │ │ - factors. │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ + Date │ │ │ │ │ + July 29, 2013 │ │ │ │ │ Author │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ - Date │ │ │ │ │ - Oct 17, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_y_m_b_o_l_i_c │ │ │ │ │ - * _S_y_m_b_o_l_i_c_F_a_c_t_o_r_._h │ │ │ │ │ + * _S_y_m_b_o_l_i_c_I_S_A_M_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01385.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,5 +1,3 @@ │ │ │ │ │ var a01385 = [ │ │ │ │ │ - ["gtsam::SymbolicFactor", "a05012.html", "a05012"], │ │ │ │ │ - ["gtsam::traits< SymbolicFactor >", "a05016.html", null], │ │ │ │ │ - ["EliminateSymbolic", "a01385.html#a659f20993995258b1405737e375efd10", null] │ │ │ │ │ + ["gtsam::SymbolicISAM", "a05032.html", "a05032"] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01385_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicISAM.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,216 +98,63 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
SymbolicFactor.h
│ │ │ │ +
SymbolicISAM.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/inference/Factor.h>
│ │ │ │ -
21#include <gtsam/inference/Key.h>
│ │ │ │ -
22#include <gtsam/base/Testable.h>
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/symbolic/SymbolicBayesTree.h>
│ │ │ │ +
22#include <gtsam/inference/ISAM.h>
│ │ │ │
23
│ │ │ │ -
24#include <boost/shared_ptr.hpp>
│ │ │ │ -
25#include <boost/make_shared.hpp>
│ │ │ │ -
26
│ │ │ │ -
27#include <utility>
│ │ │ │ -
28
│ │ │ │ -
29namespace gtsam {
│ │ │ │ -
30
│ │ │ │ -
31 // Forward declarations
│ │ │ │ -
32 class SymbolicConditional;
│ │ │ │ -
33 class HybridValues;
│ │ │ │ -
34 class Ordering;
│ │ │ │ +
24namespace gtsam {
│ │ │ │ +
25
│ │ │ │ +
│ │ │ │ +
26 class GTSAM_EXPORT SymbolicISAM : public ISAM<SymbolicBayesTree>
│ │ │ │ +
27 {
│ │ │ │ +
28 public:
│ │ │ │ +
29 typedef ISAM<SymbolicBayesTree> Base;
│ │ │ │ +
30 typedef SymbolicISAM This;
│ │ │ │ +
31 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
32
│ │ │ │
35
│ │ │ │ -
│ │ │ │ -
39 class GTSAM_EXPORT SymbolicFactor : public Factor {
│ │ │ │ -
40
│ │ │ │ -
41 public:
│ │ │ │ -
42
│ │ │ │ -
43 typedef SymbolicFactor This;
│ │ │ │ -
44 typedef Factor Base;
│ │ │ │ -
45 typedef SymbolicConditional ConditionalType;
│ │ │ │ -
46
│ │ │ │ -
48 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
49
│ │ │ │ -
52
│ │ │ │ -
54 SymbolicFactor() {}
│ │ │ │ -
55
│ │ │ │ -
│ │ │ │ -
57 explicit SymbolicFactor(Key j) :
│ │ │ │ -
58 Base(KeyVector{j}) {}
│ │ │ │ -
│ │ │ │ -
59
│ │ │ │ -
│ │ │ │ -
61 SymbolicFactor(Key j1, Key j2) :
│ │ │ │ -
62 Base(KeyVector{j1, j2}) {}
│ │ │ │ -
│ │ │ │ -
63
│ │ │ │ -
│ │ │ │ -
65 SymbolicFactor(Key j1, Key j2, Key j3) :
│ │ │ │ -
66 Base(KeyVector{j1, j2, j3}) {}
│ │ │ │ -
│ │ │ │ -
67
│ │ │ │ -
│ │ │ │ -
69 SymbolicFactor(Key j1, Key j2, Key j3, Key j4) :
│ │ │ │ -
70 Base(KeyVector{j1, j2, j3, j4}) {}
│ │ │ │ -
│ │ │ │ -
71
│ │ │ │ -
│ │ │ │ -
73 SymbolicFactor(Key j1, Key j2, Key j3, Key j4, Key j5) :
│ │ │ │ -
74 Base(KeyVector{j1, j2, j3, j4, j5}) {}
│ │ │ │ -
│ │ │ │ -
75
│ │ │ │ -
│ │ │ │ -
77 SymbolicFactor(Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) :
│ │ │ │ -
78 Base(KeyVector{j1, j2, j3, j4, j5, j6}) {}
│ │ │ │ -
│ │ │ │ -
79
│ │ │ │ -
81 explicit SymbolicFactor(const Factor& factor) : Base(factor.keys()) {}
│ │ │ │ -
82
│ │ │ │ -
83 virtual ~SymbolicFactor() {}
│ │ │ │ -
84
│ │ │ │ -
86 SymbolicFactor::shared_ptr clone() const { return boost::make_shared<This>(*this); }
│ │ │ │ -
87
│ │ │ │ -
89
│ │ │ │ -
92
│ │ │ │ -
93 bool equals(const This& other, double tol = 1e-9) const;
│ │ │ │ -
94
│ │ │ │ -
│ │ │ │ -
96 void print(
│ │ │ │ -
97 const std::string& s = "SymbolicFactor",
│ │ │ │ -
98 const KeyFormatter& formatter = DefaultKeyFormatter) const override {
│ │ │ │ -
99 Base::print(s, formatter);
│ │ │ │ -
100 }
│ │ │ │ -
│ │ │ │ -
101
│ │ │ │ -
│ │ │ │ -
103 void printKeys(
│ │ │ │ -
104 const std::string& s = "SymbolicFactor",
│ │ │ │ -
105 const KeyFormatter& formatter = DefaultKeyFormatter) const override {
│ │ │ │ -
106 Base::printKeys(s, formatter);
│ │ │ │ -
107 }
│ │ │ │ -
│ │ │ │ -
108
│ │ │ │ -
112
│ │ │ │ -
114 template<typename KEYITERATOR>
│ │ │ │ -
│ │ │ │ -
115 static SymbolicFactor FromIterators(KEYITERATOR beginKey, KEYITERATOR endKey) {
│ │ │ │ -
116 return SymbolicFactor(Base::FromIterators(beginKey, endKey));
│ │ │ │ -
117 }
│ │ │ │ -
│ │ │ │ -
118
│ │ │ │ -
120 template<typename KEYITERATOR>
│ │ │ │ -
│ │ │ │ -
121 static SymbolicFactor::shared_ptr FromIteratorsShared(KEYITERATOR beginKey, KEYITERATOR endKey) {
│ │ │ │ -
122 SymbolicFactor::shared_ptr result = boost::make_shared<SymbolicFactor>();
│ │ │ │ -
123 result->keys_.assign(beginKey, endKey);
│ │ │ │ -
124 return result;
│ │ │ │ -
125 }
│ │ │ │ -
│ │ │ │ -
126
│ │ │ │ -
129 template<class CONTAINER>
│ │ │ │ -
│ │ │ │ -
130 static SymbolicFactor FromKeys(const CONTAINER& keys) {
│ │ │ │ -
131 return SymbolicFactor(Base::FromKeys(keys));
│ │ │ │ -
132 }
│ │ │ │ -
│ │ │ │ -
133
│ │ │ │ -
136 template<class CONTAINER>
│ │ │ │ -
│ │ │ │ -
137 static SymbolicFactor::shared_ptr FromKeysShared(const CONTAINER& keys) {
│ │ │ │ -
138 return FromIteratorsShared(keys.begin(), keys.end());
│ │ │ │ -
139 }
│ │ │ │ -
│ │ │ │ -
140
│ │ │ │ -
142
│ │ │ │ -
145
│ │ │ │ -
147 double error(const HybridValues& c) const override;
│ │ │ │ -
148
│ │ │ │ -
151 std::pair<boost::shared_ptr<SymbolicConditional>, boost::shared_ptr<SymbolicFactor> >
│ │ │ │ -
152 eliminate(const Ordering& keys) const;
│ │ │ │ -
153
│ │ │ │ -
155
│ │ │ │ -
156 private:
│ │ │ │ -
158 friend class boost::serialization::access;
│ │ │ │ -
159 template<class ARCHIVE>
│ │ │ │ -
160 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
161 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
162 }
│ │ │ │ -
163 }; // IndexFactor
│ │ │ │ -
│ │ │ │ -
164
│ │ │ │ -
165 // Forward declarations
│ │ │ │ -
166 class SymbolicFactorGraph;
│ │ │ │ -
167 class Ordering;
│ │ │ │ -
168
│ │ │ │ -
173 GTSAM_EXPORT std::pair<boost::shared_ptr<SymbolicConditional>, boost::shared_ptr<SymbolicFactor> >
│ │ │ │ -
174 EliminateSymbolic(const SymbolicFactorGraph& factors, const Ordering& keys);
│ │ │ │ -
175
│ │ │ │ -
177 template<>
│ │ │ │ -
│ │ │ │ -
178 struct traits<SymbolicFactor> : public Testable<SymbolicFactor> {
│ │ │ │ -
179 };
│ │ │ │ -
│ │ │ │ -
180
│ │ │ │ -
181} //\ namespace gtsam
│ │ │ │ -
182
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ - │ │ │ │ -
Factor.h
The base class for all factors.
│ │ │ │ +
37 SymbolicISAM();
│ │ │ │ +
38
│ │ │ │ +
40 SymbolicISAM(const SymbolicBayesTree& bayesTree);
│ │ │ │ +
41
│ │ │ │ +
43
│ │ │ │ +
44 };
│ │ │ │ +
│ │ │ │ +
45
│ │ │ │ +
46}
│ │ │ │ +
ISAM.h
Incremental update functionality (iSAM) for BayesTree.
│ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::EliminateSymbolic
std::pair< boost::shared_ptr< SymbolicConditional >, boost::shared_ptr< SymbolicFactor > > EliminateSymbolic(const SymbolicFactorGraph &factors, const Ordering &keys)
Dense elimination function for symbolic factors.
Definition SymbolicFactor.cpp:36
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ -
gtsam::Factor
Definition Factor.h:68
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ -
gtsam::SymbolicConditional
SymbolicConditional is a conditional with keys but no probability data, produced by symbolic eliminat...
Definition SymbolicConditional.h:38
│ │ │ │ -
gtsam::SymbolicFactor
SymbolicFactor represents a symbolic factor that specifies graph topology but is not associated with ...
Definition SymbolicFactor.h:39
│ │ │ │ -
gtsam::SymbolicFactor::print
void print(const std::string &s="SymbolicFactor", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print
Definition SymbolicFactor.h:96
│ │ │ │ -
gtsam::SymbolicFactor::SymbolicFactor
SymbolicFactor(Key j1, Key j2, Key j3, Key j4, Key j5, Key j6)
Construct 6-way factor.
Definition SymbolicFactor.h:77
│ │ │ │ -
gtsam::SymbolicFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
Overriding the shared_ptr typedef.
Definition SymbolicFactor.h:48
│ │ │ │ -
gtsam::SymbolicFactor::FromKeys
static SymbolicFactor FromKeys(const CONTAINER &keys)
Constructor from a collection of keys - compatible with boost assign::list_of and boost assign::cref_...
Definition SymbolicFactor.h:130
│ │ │ │ -
gtsam::SymbolicFactor::SymbolicFactor
SymbolicFactor(const Factor &factor)
Create symbolic version of any factor.
Definition SymbolicFactor.h:81
│ │ │ │ -
gtsam::SymbolicFactor::SymbolicFactor
SymbolicFactor(Key j1, Key j2, Key j3, Key j4)
Construct 4-way factor.
Definition SymbolicFactor.h:69
│ │ │ │ -
gtsam::SymbolicFactor::SymbolicFactor
SymbolicFactor(Key j1, Key j2)
Construct binary factor.
Definition SymbolicFactor.h:61
│ │ │ │ -
gtsam::SymbolicFactor::FromIterators
static SymbolicFactor FromIterators(KEYITERATOR beginKey, KEYITERATOR endKey)
Constructor from a collection of keys.
Definition SymbolicFactor.h:115
│ │ │ │ -
gtsam::SymbolicFactor::printKeys
void printKeys(const std::string &s="SymbolicFactor", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print only keys
Definition SymbolicFactor.h:103
│ │ │ │ -
gtsam::SymbolicFactor::FromKeysShared
static SymbolicFactor::shared_ptr FromKeysShared(const CONTAINER &keys)
Constructor from a collection of keys - compatible with boost assign::list_of and boost assign::cref_...
Definition SymbolicFactor.h:137
│ │ │ │ -
gtsam::SymbolicFactor::SymbolicFactor
SymbolicFactor(Key j1, Key j2, Key j3)
Construct ternary factor.
Definition SymbolicFactor.h:65
│ │ │ │ -
gtsam::SymbolicFactor::SymbolicFactor
SymbolicFactor(Key j)
Construct unary factor.
Definition SymbolicFactor.h:57
│ │ │ │ -
gtsam::SymbolicFactor::SymbolicFactor
SymbolicFactor(Key j1, Key j2, Key j3, Key j4, Key j5)
Construct 5-way factor.
Definition SymbolicFactor.h:73
│ │ │ │ -
gtsam::SymbolicFactor::FromIteratorsShared
static SymbolicFactor::shared_ptr FromIteratorsShared(KEYITERATOR beginKey, KEYITERATOR endKey)
Constructor from a collection of keys.
Definition SymbolicFactor.h:121
│ │ │ │ -
gtsam::SymbolicFactor::SymbolicFactor
SymbolicFactor()
Default constructor for I/O.
Definition SymbolicFactor.h:54
│ │ │ │ -
gtsam::SymbolicFactor::clone
SymbolicFactor::shared_ptr clone() const
Copy this object as its actual derived type.
Definition SymbolicFactor.h:86
│ │ │ │ -
HybridValues
the error.
│ │ │ │ +
gtsam::ISAM
A Bayes tree with an update methods that implements the iSAM algorithm.
Definition ISAM.h:31
│ │ │ │ +
gtsam::SymbolicBayesTree
A Bayes tree that represents the connectivity between variables but is not associated with any probab...
Definition SymbolicBayesTree.h:51
│ │ │ │ +
gtsam::SymbolicISAM
Definition SymbolicISAM.h:27
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,275 +1,62 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SymbolicFactor.h │ │ │ │ │ +SymbolicISAM.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_s_y_m_b_o_l_i_c_/_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_I_S_A_M_._h> │ │ │ │ │ 23 │ │ │ │ │ -24#include │ │ │ │ │ -25#include │ │ │ │ │ -26 │ │ │ │ │ -27#include │ │ │ │ │ -28 │ │ │ │ │ -29namespace _g_t_s_a_m { │ │ │ │ │ -30 │ │ │ │ │ -31 // Forward declarations │ │ │ │ │ -32 class SymbolicConditional; │ │ │ │ │ -33 class _H_y_b_r_i_d_V_a_l_u_e_s; │ │ │ │ │ -34 class Ordering; │ │ │ │ │ +24namespace _g_t_s_a_m { │ │ │ │ │ +25 │ │ │ │ │ +_2_6 class GTSAM_EXPORT _S_y_m_b_o_l_i_c_I_S_A_M : public _I_S_A_M │ │ │ │ │ +27 { │ │ │ │ │ +28 public: │ │ │ │ │ +29 typedef _I_S_A_M_<_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_> _B_a_s_e; │ │ │ │ │ +30 typedef _S_y_m_b_o_l_i_c_I_S_A_M _T_h_i_s; │ │ │ │ │ +31 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +32 │ │ │ │ │ 35 │ │ │ │ │ -_3_9 class GTSAM_EXPORT _S_y_m_b_o_l_i_c_F_a_c_t_o_r : public _F_a_c_t_o_r { │ │ │ │ │ -40 │ │ │ │ │ -41 public: │ │ │ │ │ -42 │ │ │ │ │ -43 typedef _S_y_m_b_o_l_i_c_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ -44 typedef _F_a_c_t_o_r _B_a_s_e; │ │ │ │ │ -45 typedef _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l _C_o_n_d_i_t_i_o_n_a_l_T_y_p_e; │ │ │ │ │ -46 │ │ │ │ │ -_4_8 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -49 │ │ │ │ │ -52 │ │ │ │ │ -_5_4 _S_y_m_b_o_l_i_c_F_a_c_t_o_r() {} │ │ │ │ │ -55 │ │ │ │ │ -_5_7 explicit _S_y_m_b_o_l_i_c_F_a_c_t_o_r(_K_e_y j) : │ │ │ │ │ -58 _B_a_s_e(_K_e_y_V_e_c_t_o_r{j}) {} │ │ │ │ │ -59 │ │ │ │ │ -_6_1 _S_y_m_b_o_l_i_c_F_a_c_t_o_r(_K_e_y j1, _K_e_y j2) : │ │ │ │ │ -62 _B_a_s_e(_K_e_y_V_e_c_t_o_r{j1, j2}) {} │ │ │ │ │ -63 │ │ │ │ │ -_6_5 _S_y_m_b_o_l_i_c_F_a_c_t_o_r(_K_e_y j1, _K_e_y j2, _K_e_y j3) : │ │ │ │ │ -66 _B_a_s_e(_K_e_y_V_e_c_t_o_r{j1, j2, j3}) {} │ │ │ │ │ -67 │ │ │ │ │ -_6_9 _S_y_m_b_o_l_i_c_F_a_c_t_o_r(_K_e_y j1, _K_e_y j2, _K_e_y j3, _K_e_y j4) : │ │ │ │ │ -70 _B_a_s_e(_K_e_y_V_e_c_t_o_r{j1, j2, j3, j4}) {} │ │ │ │ │ -71 │ │ │ │ │ -_7_3 _S_y_m_b_o_l_i_c_F_a_c_t_o_r(_K_e_y j1, _K_e_y j2, _K_e_y j3, _K_e_y j4, _K_e_y j5) : │ │ │ │ │ -74 _B_a_s_e(_K_e_y_V_e_c_t_o_r{j1, j2, j3, j4, j5}) {} │ │ │ │ │ -75 │ │ │ │ │ -_7_7 _S_y_m_b_o_l_i_c_F_a_c_t_o_r(_K_e_y j1, _K_e_y j2, _K_e_y j3, _K_e_y j4, _K_e_y j5, _K_e_y j6) : │ │ │ │ │ -78 _B_a_s_e(_K_e_y_V_e_c_t_o_r{j1, j2, j3, j4, j5, j6}) {} │ │ │ │ │ -79 │ │ │ │ │ -_8_1 explicit _S_y_m_b_o_l_i_c_F_a_c_t_o_r(const _F_a_c_t_o_r& factor) : _B_a_s_e(factor.keys()) {} │ │ │ │ │ -82 │ │ │ │ │ -83 virtual _~_S_y_m_b_o_l_i_c_F_a_c_t_o_r() {} │ │ │ │ │ -84 │ │ │ │ │ -_8_6 _S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r _c_l_o_n_e() const { return boost::make_shared │ │ │ │ │ -(*this); } │ │ │ │ │ -87 │ │ │ │ │ -89 │ │ │ │ │ -92 │ │ │ │ │ -93 bool _e_q_u_a_l_s(const This& other, double tol = 1e-9) const; │ │ │ │ │ -94 │ │ │ │ │ -_9_6 void _p_r_i_n_t( │ │ │ │ │ -97 const std::string& s = "SymbolicFactor", │ │ │ │ │ -98 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override { │ │ │ │ │ -99 Base::print(s, formatter); │ │ │ │ │ -100 } │ │ │ │ │ -101 │ │ │ │ │ -_1_0_3 void _p_r_i_n_t_K_e_y_s( │ │ │ │ │ -104 const std::string& s = "SymbolicFactor", │ │ │ │ │ -105 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override { │ │ │ │ │ -106 Base::printKeys(s, formatter); │ │ │ │ │ -107 } │ │ │ │ │ -108 │ │ │ │ │ -112 │ │ │ │ │ -114 template │ │ │ │ │ -_1_1_5 static _S_y_m_b_o_l_i_c_F_a_c_t_o_r _F_r_o_m_I_t_e_r_a_t_o_r_s(KEYITERATOR beginKey, KEYITERATOR │ │ │ │ │ -endKey) { │ │ │ │ │ -116 return _S_y_m_b_o_l_i_c_F_a_c_t_o_r(Base::FromIterators(beginKey, endKey)); │ │ │ │ │ -117 } │ │ │ │ │ -118 │ │ │ │ │ -120 template │ │ │ │ │ -_1_2_1 static _S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r _F_r_o_m_I_t_e_r_a_t_o_r_s_S_h_a_r_e_d(KEYITERATOR beginKey, │ │ │ │ │ -KEYITERATOR endKey) { │ │ │ │ │ -122 _S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r result = boost::make_shared(); │ │ │ │ │ -123 result->keys_.assign(beginKey, endKey); │ │ │ │ │ -124 return result; │ │ │ │ │ -125 } │ │ │ │ │ -126 │ │ │ │ │ -129 template │ │ │ │ │ -_1_3_0 static _S_y_m_b_o_l_i_c_F_a_c_t_o_r _F_r_o_m_K_e_y_s(const CONTAINER& keys) { │ │ │ │ │ -131 return _S_y_m_b_o_l_i_c_F_a_c_t_o_r(Base::FromKeys(keys)); │ │ │ │ │ -132 } │ │ │ │ │ -133 │ │ │ │ │ -136 template │ │ │ │ │ -_1_3_7 static _S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r _F_r_o_m_K_e_y_s_S_h_a_r_e_d(const CONTAINER& keys) { │ │ │ │ │ -138 return FromIteratorsShared(keys.begin(), keys.end()); │ │ │ │ │ -139 } │ │ │ │ │ -140 │ │ │ │ │ -142 │ │ │ │ │ -145 │ │ │ │ │ -147 double error(const _H_y_b_r_i_d_V_a_l_u_e_s& c) const override; │ │ │ │ │ -148 │ │ │ │ │ -151 std::pair, boost:: │ │ │ │ │ -shared_ptr > │ │ │ │ │ -152 eliminate(const _O_r_d_e_r_i_n_g& keys) const; │ │ │ │ │ -153 │ │ │ │ │ -155 │ │ │ │ │ -156 private: │ │ │ │ │ -_1_5_8 friend class boost::serialization::access; │ │ │ │ │ -159 template │ │ │ │ │ -160 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -161 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -162 } │ │ │ │ │ -163 }; // IndexFactor │ │ │ │ │ -164 │ │ │ │ │ -165 // Forward declarations │ │ │ │ │ -166 class SymbolicFactorGraph; │ │ │ │ │ -167 class Ordering; │ │ │ │ │ -168 │ │ │ │ │ -173 GTSAM_EXPORT std::pair, boost:: │ │ │ │ │ -shared_ptr > │ │ │ │ │ -174 _E_l_i_m_i_n_a_t_e_S_y_m_b_o_l_i_c(const SymbolicFactorGraph& factors, const Ordering& │ │ │ │ │ -keys); │ │ │ │ │ -175 │ │ │ │ │ -177 template<> │ │ │ │ │ -_1_7_8 struct _t_r_a_i_t_s<_S_y_m_b_o_l_i_c_F_a_c_t_o_r> : public _T_e_s_t_a_b_l_e { │ │ │ │ │ -179 }; │ │ │ │ │ -180 │ │ │ │ │ -181} //\ namespace gtsam │ │ │ │ │ -182 │ │ │ │ │ -_T_e_s_t_a_b_l_e_._h │ │ │ │ │ -Concept check for values that can be used in unit tests. │ │ │ │ │ -_K_e_y_._h │ │ │ │ │ -_F_a_c_t_o_r_._h │ │ │ │ │ -The base class for all factors. │ │ │ │ │ +37 _S_y_m_b_o_l_i_c_I_S_A_M(); │ │ │ │ │ +38 │ │ │ │ │ +40 _S_y_m_b_o_l_i_c_I_S_A_M(const _S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e& bayesTree); │ │ │ │ │ +41 │ │ │ │ │ +43 │ │ │ │ │ +44 }; │ │ │ │ │ +45 │ │ │ │ │ +46} │ │ │ │ │ +_I_S_A_M_._h │ │ │ │ │ +Incremental update functionality (iSAM) for BayesTree. │ │ │ │ │ +_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_S_y_m_b_o_l_i_c │ │ │ │ │ -std::pair< boost::shared_ptr< SymbolicConditional >, boost::shared_ptr< │ │ │ │ │ -SymbolicFactor > > EliminateSymbolic(const SymbolicFactorGraph &factors, const │ │ │ │ │ -Ordering &keys) │ │ │ │ │ -Dense elimination function for symbolic factors. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactor.cpp:36 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:68 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -SymbolicConditional is a conditional with keys but no probability data, │ │ │ │ │ -produced by symbolic eliminat... │ │ │ │ │ -DDeeffiinniittiioonn SymbolicConditional.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r │ │ │ │ │ -SymbolicFactor represents a symbolic factor that specifies graph topology but │ │ │ │ │ -is not associated with ... │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactor.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="SymbolicFactor", const KeyFormatter │ │ │ │ │ -&formatter=DefaultKeyFormatter) const override │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactor.h:96 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r │ │ │ │ │ -SymbolicFactor(Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) │ │ │ │ │ -Construct 6-way factor. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactor.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -Overriding the shared_ptr typedef. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactor.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_F_r_o_m_K_e_y_s │ │ │ │ │ -static SymbolicFactor FromKeys(const CONTAINER &keys) │ │ │ │ │ -Constructor from a collection of keys - compatible with boost assign::list_of │ │ │ │ │ -and boost assign::cref_... │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactor.h:130 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r │ │ │ │ │ -SymbolicFactor(const Factor &factor) │ │ │ │ │ -Create symbolic version of any factor. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactor.h:81 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r │ │ │ │ │ -SymbolicFactor(Key j1, Key j2, Key j3, Key j4) │ │ │ │ │ -Construct 4-way factor. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactor.h:69 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r │ │ │ │ │ -SymbolicFactor(Key j1, Key j2) │ │ │ │ │ -Construct binary factor. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactor.h:61 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_F_r_o_m_I_t_e_r_a_t_o_r_s │ │ │ │ │ -static SymbolicFactor FromIterators(KEYITERATOR beginKey, KEYITERATOR endKey) │ │ │ │ │ -Constructor from a collection of keys. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactor.h:115 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_p_r_i_n_t_K_e_y_s │ │ │ │ │ -void printKeys(const std::string &s="SymbolicFactor", const KeyFormatter │ │ │ │ │ -&formatter=DefaultKeyFormatter) const override │ │ │ │ │ -print only keys │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactor.h:103 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_F_r_o_m_K_e_y_s_S_h_a_r_e_d │ │ │ │ │ -static SymbolicFactor::shared_ptr FromKeysShared(const CONTAINER &keys) │ │ │ │ │ -Constructor from a collection of keys - compatible with boost assign::list_of │ │ │ │ │ -and boost assign::cref_... │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactor.h:137 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r │ │ │ │ │ -SymbolicFactor(Key j1, Key j2, Key j3) │ │ │ │ │ -Construct ternary factor. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactor.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r │ │ │ │ │ -SymbolicFactor(Key j) │ │ │ │ │ -Construct unary factor. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactor.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r │ │ │ │ │ -SymbolicFactor(Key j1, Key j2, Key j3, Key j4, Key j5) │ │ │ │ │ -Construct 5-way factor. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactor.h:73 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_F_r_o_m_I_t_e_r_a_t_o_r_s_S_h_a_r_e_d │ │ │ │ │ -static SymbolicFactor::shared_ptr FromIteratorsShared(KEYITERATOR beginKey, │ │ │ │ │ -KEYITERATOR endKey) │ │ │ │ │ -Constructor from a collection of keys. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactor.h:121 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r │ │ │ │ │ -SymbolicFactor() │ │ │ │ │ -Default constructor for I/O. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactor.h:54 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ -SymbolicFactor::shared_ptr clone() const │ │ │ │ │ -Copy this object as its actual derived type. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactor.h:86 │ │ │ │ │ -_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -the error. │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M │ │ │ │ │ +A Bayes tree with an update methods that implements the iSAM algorithm. │ │ │ │ │ +DDeeffiinniittiioonn ISAM.h:31 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e │ │ │ │ │ +A Bayes tree that represents the connectivity between variables but is not │ │ │ │ │ +associated with any probab... │ │ │ │ │ +DDeeffiinniittiioonn SymbolicBayesTree.h:51 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_I_S_A_M │ │ │ │ │ +DDeeffiinniittiioonn SymbolicISAM.h:27 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_y_m_b_o_l_i_c │ │ │ │ │ - * _S_y_m_b_o_l_i_c_F_a_c_t_o_r_._h │ │ │ │ │ + * _S_y_m_b_o_l_i_c_I_S_A_M_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01388.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicISAM.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -94,34 +94,51 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces
│ │ │ │ -
SymbolicISAM.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
SymbolicFactor.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::SymbolicFactor
 SymbolicFactor represents a symbolic factor that specifies graph topology but is not associated with any numerical function. More...
 
struct  gtsam::traits< SymbolicFactor >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

std::pair< boost::shared_ptr< SymbolicConditional >, boost::shared_ptr< SymbolicFactor > > gtsam::EliminateSymbolic (const SymbolicFactorGraph &factors, const Ordering &keys)
 Dense elimination function for symbolic factors.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
July 29, 2013
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Oct 17, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,36 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SymbolicISAM.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +SymbolicFactor.h File Reference │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r │ │ │ │ │ +  _S_y_m_b_o_l_i_c_F_a_c_t_o_r represents a symbolic factor that specifies graph │ │ │ │ │ + topology but is not associated with any numerical function. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ + std::pair< boost::shared_ptr< _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_S_y_m_b_o_l_i_c (const │ │ │ │ │ + _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l >, boost:: _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h &factors, const │ │ │ │ │ +shared_ptr< _S_y_m_b_o_l_i_c_F_a_c_t_o_r > >  _O_r_d_e_r_i_n_g &keys) │ │ │ │ │ +  Dense elimination function for symbolic │ │ │ │ │ + factors. │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - July 29, 2013 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Oct 17, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_y_m_b_o_l_i_c │ │ │ │ │ - * _S_y_m_b_o_l_i_c_I_S_A_M_._c_p_p │ │ │ │ │ + * _S_y_m_b_o_l_i_c_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01391.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicBayesTree.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicJunctionTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -95,50 +95,50 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
SymbolicBayesTree.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Typedefs
│ │ │ │ +
SymbolicJunctionTree.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::SymbolicBayesTreeClique
 A clique in a SymbolicBayesTree. More...
 
class  gtsam::SymbolicBayesTree
 A Bayes tree that represents the connectivity between variables but is not associated with any probability functions. More...
 
struct  gtsam::traits< SymbolicBayesTreeClique >
 traits More...
 
struct  gtsam::traits< SymbolicBayesTree >
class  gtsam::SymbolicJunctionTree
 A EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree, with the additional property that it represents the clique tree associated with a Bayes net. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Typedefs

│ │ │ │ +using gtsam::SymbolicCluster = SymbolicJunctionTree::Cluster
 typedef for wrapper:
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Oct 29, 2009
│ │ │ │ +
Date
Mar 29, 2013
│ │ │ │
Author
Frank Dellaert
│ │ │ │
│ │ │ │ Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,35 +1,32 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -SymbolicBayesTree.h File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ +SymbolicJunctionTree.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e │ │ │ │ │ -  A clique in a _S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e │ │ │ │ │ -  A Bayes tree that represents the connectivity between variables but is │ │ │ │ │ - not associated with any probability functions. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_ _> │ │ │ │ │ -  traits _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_ _> │ │ │ │ │ +class   _g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ + A _E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e, i.e., a set of variable clusters with │ │ │ │ │ +  factors, arranged in a tree, with the additional property that it │ │ │ │ │ + represents the clique tree associated with a Bayes net. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ +using  ggttssaamm::::SSyymmbboolliiccCClluusstteerr = _S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e_:_:_C_l_u_s_t_e_r │ │ │ │ │ +  typedef for wrapper: │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Date │ │ │ │ │ - Oct 29, 2009 │ │ │ │ │ + Mar 29, 2013 │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_y_m_b_o_l_i_c │ │ │ │ │ - * _S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ + * _S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01391.js │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,6 +1,3 @@ │ │ │ │ │ var a01391 = [ │ │ │ │ │ - ["gtsam::SymbolicBayesTreeClique", "a04980.html", null], │ │ │ │ │ - ["gtsam::SymbolicBayesTree", "a04984.html", "a04984"], │ │ │ │ │ - ["gtsam::traits< SymbolicBayesTreeClique >", "a04988.html", null], │ │ │ │ │ - ["gtsam::traits< SymbolicBayesTree >", "a04992.html", null] │ │ │ │ │ + ["SymbolicCluster", "a01391.html#ad78d0a5ba773ceac7ff253525032a15e", null] │ │ │ │ │ ]; │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01391_source.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicBayesTree.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicJunctionTree.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -98,15 +98,15 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
SymbolicBayesTree.h
│ │ │ │ +
SymbolicJunctionTree.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │ @@ -114,87 +114,52 @@ │ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/symbolic/SymbolicBayesNet.h>
│ │ │ │ -
22#include <gtsam/symbolic/SymbolicFactorGraph.h>
│ │ │ │ -
23#include <gtsam/inference/BayesTree.h>
│ │ │ │ -
24#include <gtsam/inference/BayesTreeCliqueBase.h>
│ │ │ │ -
25
│ │ │ │ -
26namespace gtsam {
│ │ │ │ -
27
│ │ │ │ -
28 // Forward declarations
│ │ │ │ -
29 class SymbolicConditional;
│ │ │ │ -
30
│ │ │ │ -
31 /* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
33 class GTSAM_EXPORT SymbolicBayesTreeClique :
│ │ │ │ -
34 public BayesTreeCliqueBase<SymbolicBayesTreeClique, SymbolicFactorGraph>
│ │ │ │ -
35 {
│ │ │ │ -
36 public:
│ │ │ │ -
37 typedef SymbolicBayesTreeClique This;
│ │ │ │ -
38 typedef BayesTreeCliqueBase<SymbolicBayesTreeClique, SymbolicFactorGraph> Base;
│ │ │ │ -
39 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
40 typedef boost::weak_ptr<This> weak_ptr;
│ │ │ │ -
41 SymbolicBayesTreeClique() {}
│ │ │ │ -
42 virtual ~SymbolicBayesTreeClique() {}
│ │ │ │ -
43 SymbolicBayesTreeClique(const boost::shared_ptr<SymbolicConditional>& conditional) : Base(conditional) {}
│ │ │ │ -
44 };
│ │ │ │ -
│ │ │ │ -
45
│ │ │ │ -
46 /* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
49 class GTSAM_EXPORT SymbolicBayesTree :
│ │ │ │ -
50 public BayesTree<SymbolicBayesTreeClique>
│ │ │ │ -
51 {
│ │ │ │ -
52 private:
│ │ │ │ -
53 typedef BayesTree<SymbolicBayesTreeClique> Base;
│ │ │ │ -
54
│ │ │ │ -
55 public:
│ │ │ │ -
56 typedef SymbolicBayesTree This;
│ │ │ │ -
57 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
58
│ │ │ │ -
60 SymbolicBayesTree() {}
│ │ │ │ -
61
│ │ │ │ -
63 bool equals(const This& other, double tol = 1e-9) const;
│ │ │ │ -
64
│ │ │ │ -
65 private:
│ │ │ │ -
67 friend class boost::serialization::access;
│ │ │ │ -
68 template<class ARCHIVE>
│ │ │ │ -
69 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
70 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
71 }
│ │ │ │ -
72 };
│ │ │ │ -
│ │ │ │ -
73
│ │ │ │ -
75template<> struct traits<SymbolicBayesTreeClique> : public Testable<SymbolicBayesTreeClique> {};
│ │ │ │ -
76template<> struct traits<SymbolicBayesTree> : public Testable<SymbolicBayesTree> {};
│ │ │ │ -
77
│ │ │ │ -
78} //\ namespace gtsam
│ │ │ │ -
79
│ │ │ │ -
BayesTree.h
Bayes Tree is a tree of cliques of a Bayes Chain.
│ │ │ │ -
BayesTreeCliqueBase.h
Base class for cliques of a BayesTree.
│ │ │ │ - │ │ │ │ - │ │ │ │ +
21#include <gtsam/symbolic/SymbolicFactorGraph.h>
│ │ │ │ +
22#include <gtsam/symbolic/SymbolicBayesTree.h>
│ │ │ │ +
23#include <gtsam/inference/JunctionTree.h>
│ │ │ │ +
24
│ │ │ │ +
25namespace gtsam {
│ │ │ │ +
26
│ │ │ │ +
27 // Forward declarations
│ │ │ │ +
28 class SymbolicEliminationTree;
│ │ │ │ +
29
│ │ │ │ +
│ │ │ │ +
50 class GTSAM_EXPORT SymbolicJunctionTree :
│ │ │ │ +
51 public JunctionTree<SymbolicBayesTree, SymbolicFactorGraph> {
│ │ │ │ +
52 public:
│ │ │ │ +
53 typedef JunctionTree<SymbolicBayesTree, SymbolicFactorGraph> Base;
│ │ │ │ +
54 typedef SymbolicJunctionTree This;
│ │ │ │ +
55 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
56
│ │ │ │ +
65 SymbolicJunctionTree(const SymbolicEliminationTree& eliminationTree);
│ │ │ │ +
66 };
│ │ │ │ +
│ │ │ │ +
67
│ │ │ │ +
69 using SymbolicCluster = SymbolicJunctionTree::Cluster;
│ │ │ │ +
70}
│ │ │ │ +
JunctionTree.h
The junction tree.
│ │ │ │ + │ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::BayesTree
Bayes tree.
Definition BayesTree.h:67
│ │ │ │ -
gtsam::BayesTreeCliqueBase
This is the base class for BayesTree cliques.
Definition BayesTreeCliqueBase.h:50
│ │ │ │ -
gtsam::SymbolicBayesTreeClique
A clique in a SymbolicBayesTree.
Definition SymbolicBayesTree.h:35
│ │ │ │ -
gtsam::SymbolicBayesTree
A Bayes tree that represents the connectivity between variables but is not associated with any probab...
Definition SymbolicBayesTree.h:51
│ │ │ │ -
gtsam::SymbolicBayesTree::SymbolicBayesTree
SymbolicBayesTree()
Default constructor, creates an empty Bayes tree.
Definition SymbolicBayesTree.h:60
│ │ │ │ +
gtsam::ClusterTree::Cluster
A Cluster is just a collection of factors.
Definition ClusterTree.h:36
│ │ │ │ +
gtsam::JunctionTree
A JunctionTree is a cluster tree, a set of variable clusters with factors, arranged in a tree,...
Definition JunctionTree.h:50
│ │ │ │ +
gtsam::SymbolicEliminationTree
Definition SymbolicEliminationTree.h:29
│ │ │ │ +
gtsam::SymbolicJunctionTree
A EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree,...
Definition SymbolicJunctionTree.h:51
│ │ │ │ +
gtsam::SymbolicJunctionTree::shared_ptr
boost::shared_ptr< This > shared_ptr
Shared pointer to this class.
Definition SymbolicJunctionTree.h:55
│ │ │ │ +
gtsam::SymbolicJunctionTree::Base
JunctionTree< SymbolicBayesTree, SymbolicFactorGraph > Base
Base class.
Definition SymbolicJunctionTree.h:53
│ │ │ │ +
gtsam::SymbolicJunctionTree::This
SymbolicJunctionTree This
This class.
Definition SymbolicJunctionTree.h:54
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SymbolicBayesTree.h │ │ │ │ │ +SymbolicJunctionTree.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ @@ -16,105 +16,64 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 19#pragma once │ │ │ │ │ 20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_s_y_m_b_o_l_i_c_/_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_s_y_m_b_o_l_i_c_/_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._h> │ │ │ │ │ -25 │ │ │ │ │ -26namespace _g_t_s_a_m { │ │ │ │ │ -27 │ │ │ │ │ -28 // Forward declarations │ │ │ │ │ -29 class SymbolicConditional; │ │ │ │ │ -30 │ │ │ │ │ -31 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -_3_3 class GTSAM_EXPORT _S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e : │ │ │ │ │ -34 public _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e │ │ │ │ │ -35 { │ │ │ │ │ -36 public: │ │ │ │ │ -37 typedef _S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e _T_h_i_s; │ │ │ │ │ -38 typedef _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_,_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_> │ │ │ │ │ -_B_a_s_e; │ │ │ │ │ -39 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -40 typedef boost::weak_ptr weak_ptr; │ │ │ │ │ -41 _S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e() {} │ │ │ │ │ -42 virtual _~_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e() {} │ │ │ │ │ -43 _S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e(const boost::shared_ptr& │ │ │ │ │ -conditional) : _B_a_s_e(conditional) {} │ │ │ │ │ -44 }; │ │ │ │ │ -45 │ │ │ │ │ -46 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -_4_9 class GTSAM_EXPORT _S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e : │ │ │ │ │ -50 public _B_a_y_e_s_T_r_e_e │ │ │ │ │ -51 { │ │ │ │ │ -52 private: │ │ │ │ │ -53 typedef _B_a_y_e_s_T_r_e_e_<_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_> _B_a_s_e; │ │ │ │ │ -54 │ │ │ │ │ -55 public: │ │ │ │ │ -56 typedef _S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e _T_h_i_s; │ │ │ │ │ -57 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -58 │ │ │ │ │ -_6_0 _S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e() {} │ │ │ │ │ -61 │ │ │ │ │ -63 bool _e_q_u_a_l_s(const This& other, double tol = 1e-9) const; │ │ │ │ │ -64 │ │ │ │ │ -65 private: │ │ │ │ │ -_6_7 friend class boost::serialization::access; │ │ │ │ │ -68 template │ │ │ │ │ -69 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -70 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -71 } │ │ │ │ │ -72 }; │ │ │ │ │ -73 │ │ │ │ │ -_7_5template<> struct _t_r_a_i_t_s<_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e> : public │ │ │ │ │ -_T_e_s_t_a_b_l_e {}; │ │ │ │ │ -_7_6template<> struct _t_r_a_i_t_s<_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e> : public │ │ │ │ │ -_T_e_s_t_a_b_l_e {}; │ │ │ │ │ -77 │ │ │ │ │ -78} //\ namespace gtsam │ │ │ │ │ -79 │ │ │ │ │ -_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ -Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ │ -_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._h │ │ │ │ │ -Base class for cliques of a BayesTree. │ │ │ │ │ -_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_._h │ │ │ │ │ +21#include <_g_t_s_a_m_/_s_y_m_b_o_l_i_c_/_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_s_y_m_b_o_l_i_c_/_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_J_u_n_c_t_i_o_n_T_r_e_e_._h> │ │ │ │ │ +24 │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ +26 │ │ │ │ │ +27 // Forward declarations │ │ │ │ │ +28 class SymbolicEliminationTree; │ │ │ │ │ +29 │ │ │ │ │ +_5_0 class GTSAM_EXPORT _S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e : │ │ │ │ │ +51 public _J_u_n_c_t_i_o_n_T_r_e_e { │ │ │ │ │ +52 public: │ │ │ │ │ +_5_3 typedef _J_u_n_c_t_i_o_n_T_r_e_e_<_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_,_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_> _B_a_s_e; │ │ │ │ │ +_5_4 typedef _S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e _T_h_i_s; │ │ │ │ │ +_5_5 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +56 │ │ │ │ │ +65 _S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e(const _S_y_m_b_o_l_i_c_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e& eliminationTree); │ │ │ │ │ +66 }; │ │ │ │ │ +67 │ │ │ │ │ +_6_9 using _S_y_m_b_o_l_i_c_C_l_u_s_t_e_r = _S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e_:_:_C_l_u_s_t_e_r; │ │ │ │ │ +70} │ │ │ │ │ +_J_u_n_c_t_i_o_n_T_r_e_e_._h │ │ │ │ │ +The junction tree. │ │ │ │ │ +_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e │ │ │ │ │ -Bayes tree. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e │ │ │ │ │ -This is the base class for BayesTree cliques. │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e │ │ │ │ │ -A clique in a SymbolicBayesTree. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicBayesTree.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e │ │ │ │ │ -A Bayes tree that represents the connectivity between variables but is not │ │ │ │ │ -associated with any probab... │ │ │ │ │ -DDeeffiinniittiioonn SymbolicBayesTree.h:51 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e │ │ │ │ │ -SymbolicBayesTree() │ │ │ │ │ -Default constructor, creates an empty Bayes tree. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicBayesTree.h:60 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r │ │ │ │ │ +A Cluster is just a collection of factors. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ +A JunctionTree is a cluster tree, a set of variable clusters with factors, │ │ │ │ │ +arranged in a tree,... │ │ │ │ │ +DDeeffiinniittiioonn JunctionTree.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ +DDeeffiinniittiioonn SymbolicEliminationTree.h:29 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ +A EliminatableClusterTree, i.e., a set of variable clusters with factors, │ │ │ │ │ +arranged in a tree,... │ │ │ │ │ +DDeeffiinniittiioonn SymbolicJunctionTree.h:51 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +Shared pointer to this class. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicJunctionTree.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e_:_:_B_a_s_e │ │ │ │ │ +JunctionTree< SymbolicBayesTree, SymbolicFactorGraph > Base │ │ │ │ │ +Base class. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicJunctionTree.h:53 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e_:_:_T_h_i_s │ │ │ │ │ +SymbolicJunctionTree This │ │ │ │ │ +This class. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicJunctionTree.h:54 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _s_y_m_b_o_l_i_c │ │ │ │ │ - * _S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ + * _S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01397.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/precompiled_header.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/global_includes.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -93,28 +93,29 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
precompiled_header.cpp File Reference
│ │ │ │ +
global_includes.h File Reference
Base
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

We need exactly one compilation unit that includes the precompiled headers. │ │ │ │ +

Included from all GTSAM files. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │

Detailed Description

│ │ │ │ -

We need exactly one compilation unit that includes the precompiled headers.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
Date
November 2018
│ │ │ │ +

Included from all GTSAM files.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,17 +1,16 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -precompiled_header.cpp File Reference │ │ │ │ │ -We need exactly one compilation unit that includes the precompiled headers. │ │ │ │ │ -_M_o_r_e_._._. │ │ │ │ │ +global_includes.h File Reference │ │ │ │ │ +_B_a_s_e │ │ │ │ │ +Included from all GTSAM files. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -We need exactly one compilation unit that includes the precompiled headers. │ │ │ │ │ +Included from all GTSAM files. │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - November 2018 │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _p_r_e_c_o_m_p_i_l_e_d___h_e_a_d_e_r_._c_p_p │ │ │ │ │ + * _g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01400.html │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/global_includes.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/precompiled_header.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -93,29 +93,28 @@ │ │ │ │
No Matches
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
global_includes.h File Reference
Base
│ │ │ │ +
precompiled_header.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Included from all GTSAM files. │ │ │ │ +

We need exactly one compilation unit that includes the precompiled headers. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │

Detailed Description

│ │ │ │ -

Included from all GTSAM files.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ +

We need exactly one compilation unit that includes the precompiled headers.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
Date
November 2018
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,16 +1,17 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -global_includes.h File Reference │ │ │ │ │ -_B_a_s_e │ │ │ │ │ -Included from all GTSAM files. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +precompiled_header.cpp File Reference │ │ │ │ │ +We need exactly one compilation unit that includes the precompiled headers. │ │ │ │ │ +_M_o_r_e_._._. │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Included from all GTSAM files. │ │ │ │ │ +We need exactly one compilation unit that includes the precompiled headers. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + November 2018 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ + * _p_r_e_c_o_m_p_i_l_e_d___h_e_a_d_e_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a01409.html │ │ │ │ @@ -133,15 +133,15 @@ │ │ │ │
│ │ │ │
Member gtsam::SphericalCamera::Dim ()
│ │ │ │
│ │ │ │
Member gtsam::StereoCamera::project (const Point3 &point, OptionalJacobian< 3, 6 > H1, OptionalJacobian< 3, 3 > H2=boost::none, OptionalJacobian< 3, 0 > H3=boost::none) const
│ │ │ │
, use project2 - this class has fixed calibration
│ │ │ │
Member gtsam::Unit3::error (const Unit3 &q, OptionalJacobian< 2, 2 > H_q=boost::none) const
│ │ │ │
, errorVector has the proper derivatives, this confusingly has only the second.
│ │ │ │ -
Member GTSAM_CONCEPT_TESTABLE_INST (T)
│ │ │ │ +
Member GTSAM_CONCEPT_TESTABLE_INST (T)
│ │ │ │
please use BOOST_CONCEPT_ASSERT and
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/GenericValue.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/GenericValue.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Group.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Group.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
gtsam::additive_group_tag Struct Reference
│ │ │ │
│ │ │ │
│ │ │ │
The documentation for this struct was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Group.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Group.h
    • │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Lie.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Lie.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Manifold.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Manifold.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/OptionalJacobian.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/OptionalJacobian.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/OptionalJacobian.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/OptionalJacobian.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │

Detailed Description

│ │ │ │
template<typename TEST_TYPE, typename BASIC_TYPE, typename AS_NON_CONST, typename AS_CONST>
│ │ │ │ struct gtsam::const_selector< TEST_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST >

Helper class that uses templates to select between two types based on whether TEST_TYPE is const or not.

│ │ │ │
The documentation for this struct was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/types.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/types.h
    • │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │

Detailed Description

│ │ │ │

An object whose scope defines a block where TBB and OpenMP parallelism are mixed.

│ │ │ │

In such a block, we use default threads for TBB, and p/2 threads for OpenMP. If GTSAM is not compiled to use both TBB and OpenMP, this has no effect.

│ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/types.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/types.h
    • │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Value.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/GenericValue.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Value.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/GenericValue.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/VectorSpace.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/VectorSpace.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Basis.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Basis.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Basis.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Basis.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Basis.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Basis.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/BasisFactors.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/BasisFactors.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/BasisFactors.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/BasisFactors.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/BasisFactors.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/BasisFactors.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/BasisFactors.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/BasisFactors.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/BasisFactors.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/BasisFactors.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/BasisFactors.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/BasisFactors.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/BasisFactors.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/BasisFactors.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Chebyshev.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Chebyshev.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Chebyshev.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Chebyshev.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Chebyshev.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Chebyshev.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Chebyshev.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Chebyshev.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Chebyshev2.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Chebyshev2.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Chebyshev2.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Chebyshev2.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/FitBasis.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/FitBasis.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Fourier.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Fourier.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/ParameterMatrix.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/ParameterMatrix.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/Assignment.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/Assignment.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DecisionTree-inl.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DecisionTree-inl.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DecisionTree-inl.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DecisionTree-inl.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DecisionTree.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DecisionTree-inl.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DecisionTree.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DecisionTree-inl.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │

Implemented in gtsam::DecisionTree< L, Y >::Leaf< L, Y >, and gtsam::DecisionTree< L, Y >::Choice< L, Y >.

│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
The documentation for this struct was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DecisionTree.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DecisionTree.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ .. │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteBayesNet.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteBayesNet.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteBayesNet.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteBayesNet.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ 00029830: 3c21 2d2d 2063 6f6e 7465 6e74 7320 2d2d .Print (const DiscreteConditional &m, const std::string &str="") │ │ │ │   │ │ │ │ │ │ │ │ static bool Equals (const DiscreteConditional &m1, const DiscreteConditional &m2, double tol=1e-8) │ │ │ │   │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following file: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │

Implemented in gtsam::DecisionTreeFactor.

│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteFactor.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteFactor.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteFactor.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ . │ │ │ │ 00026730: 3c21 2d2d 2064 6f63 2d63 6f6e 7465 6e74 .. │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a02888.html │ │ │ │ @@ -452,16 +452,16 @@ │ │ │ │

argmax by back-substitution, optionally given certain variables.

│ │ │ │

Assumes the DAG is reverse topologically sorted, i.e. last conditional will be optimized first and that the DAG does not contain any conditionals for the given variables. If the DAG resulted from eliminating a factor graph, this is true for the elimination ordering.

│ │ │ │
Returns
given assignment extended w. optimal assignment for all variables.
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteMarginals.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteMarginals.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteValues.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteValues.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteValues.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteValues.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/Signature.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/Signature.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/Signature.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/Signature.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
gtsam::Bearing< A1, A2 > Struct Template Reference
│ │ │ │
│ │ │ │
│ │ │ │
The documentation for this struct was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/BearingRange.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/BearingRange.h
    • │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
gtsam::Range< A1, A2 > Struct Template Reference
│ │ │ │
│ │ │ │
│ │ │ │
The documentation for this struct was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/BearingRange.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/BearingRange.h
    • │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3_S2.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3_S2.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3_S2.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3_S2.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3_S2Stereo.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3_S2Stereo.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3_S2Stereo.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3_S2Stereo.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Bundler.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Bundler.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Bundler.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Bundler.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3DS2.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3DS2.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3DS2.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3DS2.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3DS2_Base.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3DS2_Base.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3DS2_Base.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3DS2_Base.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Fisheye.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Fisheye.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Fisheye.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Fisheye.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Unified.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Unified.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Unified.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Unified.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/CalibratedCamera.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/CalibratedCamera.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/CalibratedCamera.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/CalibratedCamera.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/CalibratedCamera.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/CalibratedCamera.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/CalibratedCamera.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/CalibratedCamera.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/CameraSet.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/CameraSet.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/EssentialMatrix.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/EssentialMatrix.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/EssentialMatrix.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Line3.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Line3.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Line3.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/OrientedPlane3.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/OrientedPlane3.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/OrientedPlane3.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/PinholeCamera.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/PinholeCamera.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/PinholePose.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/PinholePose.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/PinholePose.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/PinholePose.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/PinholeSet.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/PinholeSet.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Pose2.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Pose2.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Pose2.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Pose2.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │

Interpolate between two poses via individual rotation and translation interpolation.

│ │ │ │ -

The default "interpolate" method defined in Lie.h minimizes the geodesic distance on the manifold, leading to a screw motion interpolation in Cartesian space, which might not be what is expected. In contrast, this method executes a straight line interpolation for the translation, while still using interpolate (aka "slerp") for the rotational component. This might be more intuitive in many applications.

│ │ │ │ +

The default "interpolate" method defined in Lie.h minimizes the geodesic distance on the manifold, leading to a screw motion interpolation in Cartesian space, which might not be what is expected. In contrast, this method executes a straight line interpolation for the translation, while still using interpolate (aka "slerp") for the rotational component. This might be more intuitive in many applications.

│ │ │ │
Parameters
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
TEnd point of interpolation.
tA value in [0, 1].
│ │ │ │
│ │ │ │
│ │ │ │ @@ -1158,16 +1158,16 @@ │ │ │ │ │ │ │ │ │ │ │ │
Returns
xihat, 4*4 element of Lie algebra that can be exponentiated
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Pose3.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Pose3.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Pose3.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Pose3.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Rot2.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Rot2.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Rot2.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Rot2.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ...
Rot3 Retract (const Vector3 &v, OptionalJacobian< 3, 3 > H=boost::none) │ │ │ │   │ │ │ │ │ │ │ │ static Vector3 Local (const Rot3 &r, OptionalJacobian< 3, 3 > H=boost::none) │ │ │ │   │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Rot3.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Rot3M.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Rot3.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Rot3M.cpp
    • │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Similarity2.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Similarity2.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Similarity2.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Similarity3.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Similarity3.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Similarity3.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Similarity3.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SphericalCamera.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SphericalCamera.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SphericalCamera.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/StereoCamera.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/StereoCamera.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/StereoCamera.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/triangulation.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/triangulation.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Unit3.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Unit3.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Unit3.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │

Compute the error of this Gaussian Mixture.

│ │ │ │

This requires some care, as different mixture components may have different normalization constants. Let's consider p(x|y,m), where m is discrete. We need the error to satisfy the invariant:

│ │ │ │

error(x;y,m) = K - log(probability(x;y,m))

│ │ │ │ -

For all x,y,m. But note that K, the (log) normalization constant defined in Conditional.h, should not depend on x, y, or m, only on the parameters of the density. Hence, we delegate to the underlying Gaussian conditionals, indexed by m, which do satisfy:

│ │ │ │ +

For all x,y,m. But note that K, the (log) normalization constant defined in Conditional.h, should not depend on x, y, or m, only on the parameters of the density. Hence, we delegate to the underlying Gaussian conditionals, indexed by m, which do satisfy:

│ │ │ │

log(probability_m(x;y)) = K_m - error_m(x;y)

│ │ │ │

We resolve by having K == max(K_m) and

│ │ │ │

error(x;y,m) = error_m(x;y) + K - K_m

│ │ │ │

which also makes error(x;y,m) >= 0 for all x,y,m.

│ │ │ │
Parameters
│ │ │ │ │ │ │ │ │ │ │ │ @@ -962,16 +962,16 @@ │ │ │ │
valuesContinuous values and discrete assignment.
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/GaussianMixture.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/GaussianMixture.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/GaussianMixture.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/GaussianMixture.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridBayesNet.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridBayesNet.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridBayesNet.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridBayesNet.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridBayesTree.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridBayesTree.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridBayesTree.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridBayesTree.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridBayesTree.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridBayesTree.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridFactor.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridFactor.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridFactor.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following file: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridGaussianISAM.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridGaussianISAM.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridGaussianISAM.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridSmoother.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridSmoother.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridSmoother.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridSmoother.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridValues.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridValues.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/MixtureFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/MixtureFactor.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesNet.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesNet-inst.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesNet.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesNet-inst.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTree.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/FactorGraph.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/FactorGraph-inst.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTree.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/FactorGraph.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/FactorGraph-inst.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │

Eliminate the factors to a Bayes tree and remaining factor graph.

│ │ │ │
Parameters
│ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │
functionThe function to use to eliminate, see the namespace functions in GaussianFactorGraph.h
functionThe function to use to eliminate, see the namespace functions in GaussianFactorGraph.h
│ │ │ │
│ │ │ │
│ │ │ │
Returns
The Bayes tree and factor graph resulting from elimination
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTree.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/ClusterTree.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/ClusterTree-inst.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTree.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/ClusterTree.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/ClusterTree-inst.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTree.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTree-inst.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTree.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTree-inst.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTree.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTree.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │

Detailed Description

│ │ │ │
template<class GRAPH>
│ │ │ │ struct gtsam::EliminationTraits< GRAPH >

Traits class for eliminateable factor graphs, specifies the types that result from elimination, etc.

│ │ │ │

This must be defined for each factor graph that inherits from EliminateableFactorGraph.

│ │ │ │
The documentation for this struct was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTreeCliqueBase.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesTreeCliqueBase.h
    • │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Conditional.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Conditional-inst.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Conditional.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Conditional-inst.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/DotWriter.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/DotWriter.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/DotWriter.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/DotWriter.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │

Eliminate the factors to a Bayes net and remaining factor graph.

│ │ │ │
Parameters
│ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │
functionThe function to use to eliminate, see the namespace functions in GaussianFactorGraph.h
functionThe function to use to eliminate, see the namespace functions in GaussianFactorGraph.h
│ │ │ │
│ │ │ │
│ │ │ │
Returns
The Bayes net and factor graph resulting from elimination
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/EliminationTree.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/EliminationTree-inst.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/EliminationTree.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/EliminationTree-inst.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
Returns
the number of variables involved in this factor
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Factor.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Factor.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Factor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Factor.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/ISAM.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/ISAM-inst.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/ISAM.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/ISAM-inst.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/LabeledSymbol.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/LabeledSymbol.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/LabeledSymbol.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/MetisIndex.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/MetisIndex-inl.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/MetisIndex.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/MetisIndex-inl.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Ordering.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Ordering.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Ordering.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Ordering.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Symbol.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Symbol.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Symbol.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/VariableIndex.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/VariableIndex-inl.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/VariableIndex.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/VariableIndex.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/VariableIndex-inl.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/VariableIndex.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/VariableSlots.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/VariableSlots.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/VariableSlots.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/VariableSlots.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following file: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │

Reimplemented from gtsam::IterativeOptimizationParameters.

│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/ConjugateGradientSolver.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/ConjugateGradientSolver.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/ConjugateGradientSolver.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianBayesNet.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianBayesNet.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianBayesNet.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianBayesNet.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianDensity.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianDensity.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianDensity.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianDensity.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianFactor.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianFactor.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianFactor.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/HessianFactor.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/HessianFactor-inl.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/HessianFactor.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/HessianFactor-inl.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/HessianFactor.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │

Implemented in gtsam::SubgraphSolver.

│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/IterativeSolver.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/IterativeSolver.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/IterativeSolver.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/IterativeSolver.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/JacobianFactor.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/JacobianFactor-inl.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/JacobianFactor.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/JacobianFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/JacobianFactor-inl.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/JacobianFactor.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/KalmanFilter.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/KalmanFilter.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/KalmanFilter.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/KalmanFilter.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/LossFunctions.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │

Implemented in gtsam::noiseModel::Gaussian.

│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │

Implements gtsam::noiseModel::Base.

│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │

Reimplemented from gtsam::ConjugateGradientParameters.

│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
The documentation for this struct was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/PCGSolver.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/PCGSolver.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/PCGSolver.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │

Implements gtsam::IterativeSolver.

│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/PCGSolver.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/PCGSolver.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/PCGSolver.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/PowerMethod.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/PowerMethod.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Preconditioner.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Preconditioner.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Preconditioner.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Preconditioner.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Preconditioner.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Preconditioner.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Preconditioner.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Sampler.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Sampler.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Sampler.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Sampler.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │

Reimplemented from gtsam::ConjugateGradientParameters.

│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
The documentation for this struct was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/SubgraphSolver.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/SubgraphSolver.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/SubgraphSolver.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/SubgraphSolver.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/SubgraphSolver.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/SubgraphSolver.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/VectorValues.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/VectorValues.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/VectorValues.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/VectorValues.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AHRSFactor.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AHRSFactor.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AHRSFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AHRSFactor.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AHRSFactor.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AHRSFactor.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AHRSFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AHRSFactor.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AttitudeFactor.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AttitudeFactor.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AttitudeFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AttitudeFactor.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AttitudeFactor.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AttitudeFactor.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AttitudeFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AttitudeFactor.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AttitudeFactor.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AttitudeFactor.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AttitudeFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AttitudeFactor.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/BarometricFactor.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/BarometricFactor.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/BarometricFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/BarometricFactor.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │

Reimplemented from gtsam::PreintegratedRotationParams.

│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
The documentation for this struct was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/CombinedImuFactor.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/CombinedImuFactor.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/CombinedImuFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/CombinedImuFactor.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/CombinedImuFactor.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/CombinedImuFactor.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/CombinedImuFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/CombinedImuFactor.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/CombinedImuFactor.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/CombinedImuFactor.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/CombinedImuFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/CombinedImuFactor.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/GPSFactor.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/GPSFactor.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/GPSFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/GPSFactor.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/GPSFactor.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/GPSFactor.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/GPSFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/GPSFactor.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ImuFactor.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ImuFactor.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ImuFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ImuFactor.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ImuFactor.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ImuFactor.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ImuFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ImuFactor.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ImuFactor.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ImuFactor.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ImuFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ImuFactor.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/MagFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/MagFactor.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/MagFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/MagFactor.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/MagFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/MagFactor.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/MagFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/MagFactor.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/MagPoseFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/MagPoseFactor.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │

serialization

│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/NavState.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/NavState.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/NavState.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/PreintegrationBase.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/PreintegrationBase.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/PreintegrationBase.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │

Reimplemented from gtsam::PreintegratedRotationParams.

│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
The documentation for this struct was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/PreintegrationParams.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/PreintegrationParams.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/PreintegrationParams.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/Scenario.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/Scenario.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/Scenario.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/Scenario.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/Scenario.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/Scenario.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/CustomFactor.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/CustomFactor.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/CustomFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/CustomFactor.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │

Reimplemented from gtsam::NonlinearOptimizerParams.

│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/DoglegOptimizer.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/DoglegOptimizer.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/DoglegOptimizer.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/DoglegOptimizer.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/DoglegOptimizer.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/DoglegOptimizer.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/DoglegOptimizer.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/DoglegOptimizer.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/DoglegOptimizerImpl.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/DoglegOptimizerImpl.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/DoglegOptimizerImpl.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Expression.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ExpressionFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Expression.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ExpressionFactor.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
gtsam::internal::ExecutionTrace< T > Class Template Reference
│ │ │ │
│ │ │ │
│ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Expression.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Expression.h
    • │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
gtsam::internal::ExpressionNode< T > Class Template Reference
│ │ │ │
│ │ │ │
│ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Expression.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Expression.h
    • │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Expression.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Expression-inl.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Expression.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Expression-inl.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/FunctorizedFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/FunctorizedFactor.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/FunctorizedFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/FunctorizedFactor.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/GncOptimizer.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/GncOptimizer.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/GncParams.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/GncParams.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2-impl.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2-impl.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2-impl.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2-impl.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2-impl.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2-impl.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Clique.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Clique.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Clique.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Clique.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Params.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Params.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Params.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Params.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Params.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Params.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Params.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Params.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Params.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Params.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Result.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Result.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Result.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Result.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2UpdateParams.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2UpdateParams.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │

Reimplemented from gtsam::NonlinearOptimizerParams.

│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Marginals.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Marginals.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Marginals.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Marginals.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Marginals.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Marginals.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Marginals.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Marginals.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearEquality.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearEquality.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearEquality.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearEquality.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearEquality.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearEquality.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearFactor.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearFactor.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearFactor.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearFactor.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearFactor.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearFactor.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearFactor.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearISAM.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearISAM.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearISAM.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearISAM.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

or to the result of a properly-formed std::bind call.

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
gtsam::internal::handle_matrix< MatrixType, isDynamic > Struct Template Reference
│ │ │ │
│ │ │ │
│ │ │ │
The documentation for this struct was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Values-inl.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Values-inl.h
    • │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Values.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Values-inl.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Values.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Values-inl.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Values.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/WhiteNoiseFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/WhiteNoiseFactor.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/BinaryMeasurement.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/BinaryMeasurement.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/MFAS.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/MFAS.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/MFAS.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/MFAS.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/SfmData.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/SfmData.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/SfmData.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/SfmData.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/SfmTrack.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/SfmTrack.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/ShonanAveraging.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/ShonanAveraging.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/ShonanAveraging.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/ShonanAveraging.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/ShonanFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/ShonanFactor.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/ShonanFactor.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/ShonanGaugeFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/ShonanGaugeFactor.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/TranslationFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/TranslationFactor.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/AntiFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/AntiFactor.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/BetweenFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/BetweenFactor.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following file: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following file: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/FrobeniusFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/FrobeniusFactor.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/GeneralSFMFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/GeneralSFMFactor.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/GeneralSFMFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/GeneralSFMFactor.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this struct was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/InitializePose3.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/InitializePose3.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/InitializePose3.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/InitializePose3.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/JacobianFactorSVD.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/JacobianFactorSVD.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/OrientedPlane3Factor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/OrientedPlane3Factor.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/OrientedPlane3Factor.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/OrientedPlane3Factor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/OrientedPlane3Factor.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/OrientedPlane3Factor.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/PoseRotationPrior.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/PoseRotationPrior.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/ProjectionFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/ProjectionFactor.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/RotateFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/RotateFactor.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/RotateFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/RotateFactor.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/StereoFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/StereoFactor.h
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following file: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicBayesNet.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicBayesNet.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicBayesNet.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicBayesNet.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicFactor.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicFactor.cpp
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicFactor.cpp
    • │ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
The documentation for this class was generated from the following files: │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │

Detailed Description

│ │ │ │

symbolic elimination etc.

│ │ │ │

Derived classes must redefine the Factor and shared_ptr typedefs to refer to the associated factor type and shared_ptr type of the derived class. See SymbolicConditional and GaussianConditional for examples.

│ │ │ │
The documentation for this class was generated from the following file:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Conditional.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Conditional.h
    • │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │

Detailed Description

│ │ │ │

The Factor::error simply extracts the.

│ │ │ │

(A*x-b)/sigma

│ │ │ │

HybridValues simply extracts the error.

│ │ │ │
The documentation for this class was generated from the following files:
    │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianFactor.h
    • │ │ │ │
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/HessianFactor.h
    • │ │ │ │ -
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/JacobianFactor.h
    • │ │ │ │ +
  • /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/JacobianFactor.h
    • │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
346
│ │ │ │
347} // namespace utilities
│ │ │ │
348
│ │ │ │
349}
│ │ │ │
350
│ │ │ │ -
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
│ │ │ │ -
PinholeCamera.h
Base class for all pinhole cameras.
│ │ │ │ -
Pose3.h
3D Pose
│ │ │ │ -
Point3.h
3D Point
│ │ │ │ -
Pose2.h
2D Pose
│ │ │ │ -
Point2.h
2D Point
│ │ │ │ -
VectorValues.h
Factor Graph Values.
│ │ │ │ -
Sampler.h
sampling from a NoiseModel
│ │ │ │ -
Values.h
A non-templated config holding any types of Manifold-group elements.
│ │ │ │ -
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ -
ProjectionFactor.h
Reprojection of a LANDMARK to a 2D point.
│ │ │ │ +
Point2.h
2D Point
│ │ │ │ +
Pose3.h
3D Pose
│ │ │ │ +
Pose2.h
2D Pose
│ │ │ │ +
Point3.h
3D Point
│ │ │ │ +
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
│ │ │ │ +
PinholeCamera.h
Base class for all pinhole cameras.
│ │ │ │ +
Sampler.h
sampling from a NoiseModel
│ │ │ │ +
VectorValues.h
Factor Graph Values.
│ │ │ │ +
Values.h
A non-templated config holding any types of Manifold-group elements.
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │ +
ProjectionFactor.h
Reprojection of a LANDMARK to a 2D point.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │
gtsam::symbol
Key symbol(unsigned char c, std::uint64_t j)
Create a symbol key from a character and index, i.e.
Definition Symbol.h:135
│ │ │ │
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ ├── html2text {} │ │ │ │ │ @@ -314,36 +314,36 @@ │ │ │ │ │ 344 return world; │ │ │ │ │ 345} │ │ │ │ │ 346 │ │ │ │ │ 347} // namespace utilities │ │ │ │ │ 348 │ │ │ │ │ 349} │ │ │ │ │ 350 │ │ │ │ │ -_C_a_l_3___S_2_._h │ │ │ │ │ -The most common 5DOF 3D->2D calibration. │ │ │ │ │ -_P_i_n_h_o_l_e_C_a_m_e_r_a_._h │ │ │ │ │ -Base class for all pinhole cameras. │ │ │ │ │ +_P_o_i_n_t_2_._h │ │ │ │ │ +2D Point │ │ │ │ │ _P_o_s_e_3_._h │ │ │ │ │ 3D Pose │ │ │ │ │ -_P_o_i_n_t_3_._h │ │ │ │ │ -3D Point │ │ │ │ │ _P_o_s_e_2_._h │ │ │ │ │ 2D Pose │ │ │ │ │ -_P_o_i_n_t_2_._h │ │ │ │ │ -2D Point │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ -Factor Graph Values. │ │ │ │ │ +_P_o_i_n_t_3_._h │ │ │ │ │ +3D Point │ │ │ │ │ +_C_a_l_3___S_2_._h │ │ │ │ │ +The most common 5DOF 3D->2D calibration. │ │ │ │ │ +_P_i_n_h_o_l_e_C_a_m_e_r_a_._h │ │ │ │ │ +Base class for all pinhole cameras. │ │ │ │ │ _S_a_m_p_l_e_r_._h │ │ │ │ │ sampling from a NoiseModel │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ +Factor Graph Values. │ │ │ │ │ _V_a_l_u_e_s_._h │ │ │ │ │ A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Factor Graph consisting of non-linear factors. │ │ │ │ │ _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ Non-linear factor base classes. │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Factor Graph consisting of non-linear factors. │ │ │ │ │ _P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_._h │ │ │ │ │ Reprojection of a LANDMARK to a 2D point. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ FastVector< Key > KeyVector │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a08137_source.html │ │ │ │ @@ -114,16 +114,16 @@ │ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/base/Testable.h>
│ │ │ │ -
22#include <gtsam/inference/Key.h>
│ │ │ │ +
21#include <gtsam/base/Testable.h>
│ │ │ │ +
22#include <gtsam/inference/Key.h>
│ │ │ │
23
│ │ │ │
24#include <boost/serialization/nvp.hpp>
│ │ │ │
25#include <cstdint>
│ │ │ │
26#include <functional>
│ │ │ │
27
│ │ │ │
28namespace gtsam {
│ │ │ │
29
│ │ │ │ @@ -268,16 +268,16 @@ │ │ │ │
179 constexpr unsigned char chr() const { return c_; }
│ │ │ │
180};
│ │ │ │ │ │ │ │
181
│ │ │ │
183template<> struct traits<Symbol> : public Testable<Symbol> {};
│ │ │ │
184
│ │ │ │
185} // \ namespace gtsam
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ - │ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::symbolIndex
std::uint64_t symbolIndex(Key key)
Return the index portion of a symbol key.
Definition Symbol.h:141
│ │ │ │
gtsam::symbol
Key symbol(unsigned char c, std::uint64_t j)
Create a symbol key from a character and index, i.e.
Definition Symbol.h:135
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │
gtsam::symbolChr
unsigned char symbolChr(Key key)
Return the character portion of a symbol key.
Definition Symbol.h:138
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a08143_source.html │ │ │ │ @@ -104,16 +104,16 @@ │ │ │ │
│ │ │ │
expressions.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1
│ │ │ │
8#pragma once
│ │ │ │
9
│ │ │ │ -
10#include <gtsam/geometry/Rot3.h>
│ │ │ │ -
11#include <gtsam/navigation/NavState.h>
│ │ │ │ +
10#include <gtsam/geometry/Rot3.h>
│ │ │ │ +
11#include <gtsam/navigation/NavState.h>
│ │ │ │
12#include <gtsam/nonlinear/expressions.h>
│ │ │ │
13#include <gtsam/slam/expressions.h>
│ │ │ │
14
│ │ │ │
15namespace gtsam {
│ │ │ │
16
│ │ │ │
17typedef Expression<NavState> NavState_;
│ │ │ │
18typedef Expression<Velocity3> Velocity3_;
│ │ │ │ @@ -139,16 +139,16 @@ │ │ │ │
38 return Point3_(internal::position, X);
│ │ │ │
39}
│ │ │ │
40inline Velocity3_ velocity(const NavState_& X) {
│ │ │ │
41 return Velocity3_(internal::velocity, X);
│ │ │ │
42}
│ │ │ │
43
│ │ │ │
44} // namespace gtsam
│ │ │ │ -
Rot3.h
3D rotation represented as a rotation matrix or quaternion
│ │ │ │ -
NavState.h
Navigation state composing of attitude, position, and velocity.
│ │ │ │ +
Rot3.h
3D rotation represented as a rotation matrix or quaternion
│ │ │ │ +
NavState.h
Navigation state composing of attitude, position, and velocity.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │
gtsam::Velocity3
Vector3 Velocity3
Velocity is currently typedef'd to Vector3.
Definition NavState.h:28
│ │ │ │
expressions.h
Common expressions, both linear and non-linear.
│ │ │ │
expressions.h
Common expressions for solving geometry/slam/sfm problems.
│ │ │ │
│ │ │ │ │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a08146_source.html │ │ │ │ @@ -104,22 +104,22 @@ │ │ │ │
│ │ │ │
expressions.h
│ │ │ │
│ │ │ │
│ │ │ │ Go to the documentation of this file.
1
│ │ │ │
8#pragma once
│ │ │ │
9
│ │ │ │ -
10#include <gtsam/nonlinear/Expression.h>
│ │ │ │ -
11#include <gtsam/base/Lie.h>
│ │ │ │ +
10#include <gtsam/nonlinear/Expression.h>
│ │ │ │ +
11#include <gtsam/base/Lie.h>
│ │ │ │
12
│ │ │ │
13namespace gtsam {
│ │ │ │
14
│ │ │ │
15// Generic between, assumes existence of traits<T>::Between
│ │ │ │
16template <typename T>
│ │ │ │ -
17Expression<T> between(const Expression<T>& t1, const Expression<T>& t2) {
│ │ │ │ +
17Expression<T> between(const Expression<T>& t1, const Expression<T>& t2) {
│ │ │ │
18 return Expression<T>(traits<T>::Between, t1, t2);
│ │ │ │
19}
│ │ │ │
20
│ │ │ │
21// Generic compose, assumes existence of traits<T>::Compose
│ │ │ │
22template <typename T>
│ │ │ │
23Expression<T> compose(const Expression<T>& t1, const Expression<T>& t2) {
│ │ │ │
24 return Expression<T>(traits<T>::Compose, t1, t2);
│ │ │ │ @@ -134,17 +134,17 @@ │ │ │ │
33typedef Expression<Vector5> Vector5_;
│ │ │ │
34typedef Expression<Vector6> Vector6_;
│ │ │ │
35typedef Expression<Vector7> Vector7_;
│ │ │ │
36typedef Expression<Vector8> Vector8_;
│ │ │ │
37typedef Expression<Vector9> Vector9_;
│ │ │ │
38
│ │ │ │
39} // \namespace gtsam
│ │ │ │ -
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ -
gtsam::testing::between
T between(const T &t1, const T &t2)
binary functions
Definition lieProxies.h:36
│ │ │ │ -
Expression.h
Expressions for Block Automatic Differentiation.
│ │ │ │ +
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ +
gtsam::testing::between
T between(const T &t1, const T &t2)
binary functions
Definition lieProxies.h:36
│ │ │ │ +
Expression.h
Expressions for Block Automatic Differentiation.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ Go to the documentation of this file.
1
│ │ │ │
8#pragma once
│ │ │ │
9
│ │ │ │
10#include <gtsam/nonlinear/expressions.h>
│ │ │ │ -
11#include <gtsam/geometry/Pose2.h>
│ │ │ │ -
12#include <gtsam/geometry/Cal3_S2.h>
│ │ │ │ -
13#include <gtsam/geometry/Cal3Bundler.h>
│ │ │ │ -
14#include <gtsam/geometry/Line3.h>
│ │ │ │ +
11#include <gtsam/geometry/Pose2.h>
│ │ │ │ +
12#include <gtsam/geometry/Cal3_S2.h>
│ │ │ │ +
13#include <gtsam/geometry/Cal3Bundler.h>
│ │ │ │ +
14#include <gtsam/geometry/Line3.h>
│ │ │ │
15#include <gtsam/geometry/OrientedPlane3.h>
│ │ │ │ -
16#include <gtsam/geometry/PinholeCamera.h>
│ │ │ │ +
16#include <gtsam/geometry/PinholeCamera.h>
│ │ │ │
17
│ │ │ │
18namespace gtsam {
│ │ │ │
19
│ │ │ │
20// 2D Geometry
│ │ │ │
21
│ │ │ │
22typedef Expression<Point2> Point2_;
│ │ │ │
23typedef Expression<Rot2> Rot2_;
│ │ │ │ @@ -191,23 +191,23 @@ │ │ │ │
90 return Rot3_(internal::rotation, pose);
│ │ │ │
91}
│ │ │ │
92
│ │ │ │
93inline Point3_ translation(const Pose3_& pose) {
│ │ │ │
94 return Point3_(internal::translation, pose);
│ │ │ │
95}
│ │ │ │
96
│ │ │ │ -
97inline Point3_ rotate(const Rot3_& x, const Point3_& p) {
│ │ │ │ +
97inline Point3_ rotate(const Rot3_& x, const Point3_& p) {
│ │ │ │
98 return Point3_(x, &Rot3::rotate, p);
│ │ │ │
99}
│ │ │ │
100
│ │ │ │
101inline Point3_ point3(const Unit3_& v) {
│ │ │ │
102 return Point3_(&Unit3::point3, v);
│ │ │ │
103}
│ │ │ │
104
│ │ │ │ -
105inline Unit3_ rotate(const Rot3_& x, const Unit3_& p) {
│ │ │ │ +
105inline Unit3_ rotate(const Rot3_& x, const Unit3_& p) {
│ │ │ │
106 return Unit3_(x, &Rot3::rotate, p);
│ │ │ │
107}
│ │ │ │
108
│ │ │ │
109inline Point3_ unrotate(const Rot3_& x, const Point3_& p) {
│ │ │ │
110 return Point3_(x, &Rot3::unrotate, p);
│ │ │ │
111}
│ │ │ │
112
│ │ │ │ @@ -295,20 +295,20 @@ │ │ │ │
193 const gtsam::Expression<T> &x1, const gtsam::Expression<T> &x2) {
│ │ │ │
194 return Expression<typename gtsam::traits<T>::TangentVector>(
│ │ │ │
195 gtsam::traits<T>::Logmap, between(x1, x2));
│ │ │ │
196}
│ │ │ │
│ │ │ │
197
│ │ │ │
198} // \namespace gtsam
│ │ │ │ -
gtsam::testing::rotate
P rotate(const T &r, const P &pt)
rotation functions
Definition lieProxies.h:47
│ │ │ │ -
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
│ │ │ │ -
Cal3Bundler.h
Calibration used by Bundler.
│ │ │ │ -
PinholeCamera.h
Base class for all pinhole cameras.
│ │ │ │ -
Pose2.h
2D Pose
│ │ │ │ -
Line3.h
4 dimensional manifold of 3D lines
│ │ │ │ +
gtsam::testing::rotate
P rotate(const T &r, const P &pt)
rotation functions
Definition lieProxies.h:47
│ │ │ │ +
Line3.h
4 dimensional manifold of 3D lines
│ │ │ │ +
Pose2.h
2D Pose
│ │ │ │ +
Cal3Bundler.h
Calibration used by Bundler.
│ │ │ │ +
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
│ │ │ │ +
PinholeCamera.h
Base class for all pinhole cameras.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::logmap
gtsam::Expression< typename gtsam::traits< T >::TangentVector > logmap(const gtsam::Expression< T > &x1, const gtsam::Expression< T > &x2)
logmap
Definition expressions.h:192
│ │ │ │
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │
gtsam::cross
Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
cross product
Definition Point3.cpp:64
│ │ │ │
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │
gtsam::transformTo
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Transform a line from world to camera frame.
Definition Line3.cpp:94
│ │ │ │
gtsam::normalize
Point3 normalize(const Point3 &p, OptionalJacobian< 3, 3 > H)
normalize, with optional Jacobian
Definition Point3.cpp:52
│ │ │ │ ├── html2text {} │ │ │ │ │ @@ -199,24 +199,24 @@ │ │ │ │ │ 196} │ │ │ │ │ 197 │ │ │ │ │ 198} // \namespace gtsam │ │ │ │ │ _g_t_s_a_m_:_:_t_e_s_t_i_n_g_:_:_r_o_t_a_t_e │ │ │ │ │ P rotate(const T &r, const P &pt) │ │ │ │ │ rotation functions │ │ │ │ │ DDeeffiinniittiioonn lieProxies.h:47 │ │ │ │ │ -_C_a_l_3___S_2_._h │ │ │ │ │ -The most common 5DOF 3D->2D calibration. │ │ │ │ │ +_L_i_n_e_3_._h │ │ │ │ │ +4 dimensional manifold of 3D lines │ │ │ │ │ +_P_o_s_e_2_._h │ │ │ │ │ +2D Pose │ │ │ │ │ _C_a_l_3_B_u_n_d_l_e_r_._h │ │ │ │ │ Calibration used by Bundler. │ │ │ │ │ +_C_a_l_3___S_2_._h │ │ │ │ │ +The most common 5DOF 3D->2D calibration. │ │ │ │ │ _P_i_n_h_o_l_e_C_a_m_e_r_a_._h │ │ │ │ │ Base class for all pinhole cameras. │ │ │ │ │ -_P_o_s_e_2_._h │ │ │ │ │ -2D Pose │ │ │ │ │ -_L_i_n_e_3_._h │ │ │ │ │ -4 dimensional manifold of 3D lines │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_l_o_g_m_a_p │ │ │ │ │ gtsam::Expression< typename gtsam::traits< T >::TangentVector > logmap(const │ │ │ │ │ gtsam::Expression< T > &x1, const gtsam::Expression< T > &x2) │ │ │ │ │ logmap │ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a08152_source.html │ │ │ │ @@ -114,16 +114,16 @@ │ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
16#pragma once
│ │ │ │
17
│ │ │ │ -
18#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
19#include <gtsam/base/Testable.h>
│ │ │ │ +
18#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
19#include <gtsam/base/Testable.h>
│ │ │ │
20
│ │ │ │
21#include <string>
│ │ │ │
22
│ │ │ │
23namespace gtsam {
│ │ │ │
24
│ │ │ │
29 template<class VALUE>
│ │ │ │
│ │ │ │ @@ -206,26 +206,26 @@ │ │ │ │
110 boost::serialization::base_object<Base>(*this));
│ │ │ │
111 ar & BOOST_SERIALIZATION_NVP(prior_);
│ │ │ │
112 }
│ │ │ │
113
│ │ │ │
114 // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
│ │ │ │
115 enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
│ │ │ │
116 public:
│ │ │ │ -
117 GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
│ │ │ │ +
117 GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
│ │ │ │
118 };
│ │ │ │
│ │ │ │
119
│ │ │ │
121 template<class VALUE>
│ │ │ │
122 struct traits<PriorFactor<VALUE> > : public Testable<PriorFactor<VALUE> > {};
│ │ │ │
123
│ │ │ │
124
│ │ │ │
125}
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ -
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
This marks a GTSAM object to require alignment.
Definition types.h:317
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
This marks a GTSAM object to require alignment.
Definition types.h:317
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
│ │ │ ├── ./usr/share/doc/libgtsam-dev/html/a08164_source.html │ │ │ │ @@ -115,15 +115,15 @@ │ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
20#pragma once
│ │ │ │
21
│ │ │ │
22#include <gtsam/nonlinear/ExpressionFactor.h>
│ │ │ │ -
23#include <gtsam/geometry/BearingRange.h>
│ │ │ │ +
23#include <gtsam/geometry/BearingRange.h>
│ │ │ │
24#include <boost/concept/assert.hpp>
│ │ │ │
25
│ │ │ │
26namespace gtsam {
│ │ │ │
27
│ │ │ │
32template <typename A1, typename A2,
│ │ │ │
33 typename B = typename Bearing<A1, A2>::result_type,
│ │ │ │
34 typename R = typename Range<A1, A2>::result_type>
│ │ │ │ @@ -206,15 +206,15 @@ │ │ │ │
107template <typename A1, typename A2, typename B, typename R>
│ │ │ │
│ │ │ │
108struct traits<BearingRangeFactor<A1, A2, B, R> >
│ │ │ │
109 : public Testable<BearingRangeFactor<A1, A2, B, R> > {};
│ │ │ │
│ │ │ │
110
│ │ │ │
111} // namespace gtsam
│ │ │ │ -
BearingRange.h
Bearing-Range product.
│ │ │ │ +
BearingRange.h
Bearing-Range product.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::genericValue
GenericValue< T > genericValue(const T &v)
Functional constructor of GenericValue<T> so T can be automatically deduced.
Definition GenericValue.h:212
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ ├── ./usr/share/doc/libgtsam-dev/html/dir_0e8eb460b7f775f5eda250a12ea9d76a.html │ │ │ │ @@ -99,45 +99,45 @@ │ │ │ │
│ │ │ │
symbolic Directory Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Files

 SymbolicBayesNet.cpp
 SymbolicBayesNet.cpp
 
 SymbolicBayesNet.h
 SymbolicBayesNet.h
 
 SymbolicBayesTree.h
 SymbolicBayesTree.h
 
 SymbolicConditional.cpp
 SymbolicConditional.cpp
 
 SymbolicConditional.h
 SymbolicConditional.h
 
 SymbolicEliminationTree.cpp
 SymbolicEliminationTree.cpp
 
 SymbolicEliminationTree.h
 SymbolicEliminationTree.h
 
 SymbolicFactor-inst.h
 SymbolicFactor-inst.h
 
 SymbolicFactor.cpp
 SymbolicFactor.cpp
 
 SymbolicFactor.h
 SymbolicFactor.h
 
 SymbolicFactorGraph.cpp
 SymbolicFactorGraph.cpp
 
 SymbolicFactorGraph.h
 SymbolicFactorGraph.h
 
 SymbolicISAM.cpp
 SymbolicISAM.cpp
 
 SymbolicISAM.h
 SymbolicISAM.h
 
 SymbolicJunctionTree.cpp
 SymbolicJunctionTree.cpp
 
 SymbolicJunctionTree.h
 SymbolicJunctionTree.h
 
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
Here is a list of all documented files with brief descriptions:
│ │ │ │
[detail level 123]
│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │
 ▼ gtsam
 ► base
 ► basis
 ► discrete
 ► geometry
 ► hybrid
 ► inference
 ► linear
 ► navigation
 ► nonlinear
 ► sam
 ► sfm
 ► slam
 ► symbolic
 global_includes.hIncluded from all GTSAM files
 precompiled_header.cppWe need exactly one compilation unit that includes the precompiled headers
 precompiled_header.h>
 global_includes.hIncluded from all GTSAM files
 precompiled_header.cppWe need exactly one compilation unit that includes the precompiled headers
 precompiled_header.h>
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
Here is a list of all documented file members with links to the documentation:
    │ │ │ │ -
  • assert_throw : types.h
    • │ │ │ │ -
  • EXPECT_CORRECT_EXPRESSION_JACOBIANS : expressionTesting.h
    • │ │ │ │ -
  • EXPECT_CORRECT_FACTOR_JACOBIANS : factorTesting.h
    • │ │ │ │ -
  • GTSAM_CONCEPT_GROUP_INST : Group.h
    • │ │ │ │ -
  • GTSAM_CONCEPT_LIE_INST : Lie.h
    • │ │ │ │ -
  • GTSAM_CONCEPT_MANIFOLD_INST : Manifold.h
    • │ │ │ │ +
  • assert_throw : types.h
    • │ │ │ │ +
  • EXPECT_CORRECT_EXPRESSION_JACOBIANS : expressionTesting.h
    • │ │ │ │ +
  • EXPECT_CORRECT_FACTOR_JACOBIANS : factorTesting.h
    • │ │ │ │ +
  • GTSAM_CONCEPT_GROUP_INST : Group.h
    • │ │ │ │ +
  • GTSAM_CONCEPT_LIE_INST : Lie.h
    • │ │ │ │ +
  • GTSAM_CONCEPT_MANIFOLD_INST : Manifold.h
    • │ │ │ │
  • GTSAM_CONCEPT_POSE_INST : concepts.h
    • │ │ │ │ -
  • GTSAM_CONCEPT_TESTABLE_INST : Testable.h
    • │ │ │ │ -
  • GTSAM_MAKE_ALIGNED_OPERATOR_NEW : types.h
    • │ │ │ │ -
  • GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF : types.h
    • │ │ │ │ +
  • GTSAM_CONCEPT_TESTABLE_INST : Testable.h
    • │ │ │ │ +
  • GTSAM_MAKE_ALIGNED_OPERATOR_NEW : types.h
    • │ │ │ │ +
  • GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF : types.h
    • │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
Here is a list of all documented macros with links to the documentation:
    │ │ │ │ -
  • assert_throw : types.h
    • │ │ │ │ -
  • EXPECT_CORRECT_EXPRESSION_JACOBIANS : expressionTesting.h
    • │ │ │ │ -
  • EXPECT_CORRECT_FACTOR_JACOBIANS : factorTesting.h
    • │ │ │ │ -
  • GTSAM_CONCEPT_GROUP_INST : Group.h
    • │ │ │ │ -
  • GTSAM_CONCEPT_LIE_INST : Lie.h
    • │ │ │ │ -
  • GTSAM_CONCEPT_MANIFOLD_INST : Manifold.h
    • │ │ │ │ +
  • assert_throw : types.h
    • │ │ │ │ +
  • EXPECT_CORRECT_EXPRESSION_JACOBIANS : expressionTesting.h
    • │ │ │ │ +
  • EXPECT_CORRECT_FACTOR_JACOBIANS : factorTesting.h
    • │ │ │ │ +
  • GTSAM_CONCEPT_GROUP_INST : Group.h
    • │ │ │ │ +
  • GTSAM_CONCEPT_LIE_INST : Lie.h
    • │ │ │ │ +
  • GTSAM_CONCEPT_MANIFOLD_INST : Manifold.h
    • │ │ │ │
  • GTSAM_CONCEPT_POSE_INST : concepts.h
    • │ │ │ │ -
  • GTSAM_CONCEPT_TESTABLE_INST : Testable.h
    • │ │ │ │ -
  • GTSAM_MAKE_ALIGNED_OPERATOR_NEW : types.h
    • │ │ │ │ -
  • GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF : types.h
    • │ │ │ │ +
  • GTSAM_CONCEPT_TESTABLE_INST : Testable.h
    • │ │ │ │ +
  • GTSAM_MAKE_ALIGNED_OPERATOR_NEW : types.h
    • │ │ │ │ +
  • GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF : types.h
    • │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
VectorSerialization.h
│ │ │ │ +
JunctionTree.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/base/Vector.h>
│ │ │ │ +
21#pragma once
│ │ │ │
22
│ │ │ │ -
23#include <boost/serialization/array.hpp>
│ │ │ │ -
24#include <boost/serialization/nvp.hpp>
│ │ │ │ -
25#include <boost/serialization/split_free.hpp>
│ │ │ │ +
23#include <gtsam/inference/ClusterTree.h>
│ │ │ │ +
24
│ │ │ │ +
25namespace gtsam {
│ │ │ │
26
│ │ │ │ -
27namespace boost {
│ │ │ │ -
28namespace serialization {
│ │ │ │ +
27 // Forward declarations
│ │ │ │ +
28 template<class BAYESNET, class GRAPH> class EliminationTree;
│ │ │ │
29
│ │ │ │ -
30// split version - copies into an STL vector for serialization
│ │ │ │ -
31template <class Archive>
│ │ │ │ -
32void save(Archive& ar, const gtsam::Vector& v, unsigned int /*version*/) {
│ │ │ │ -
33 const size_t size = v.size();
│ │ │ │ -
34 ar << BOOST_SERIALIZATION_NVP(size);
│ │ │ │ -
35 ar << make_nvp("data", make_array(v.data(), v.size()));
│ │ │ │ -
36}
│ │ │ │ -
37
│ │ │ │ -
38template <class Archive>
│ │ │ │ -
39void load(Archive& ar, gtsam::Vector& v, unsigned int /*version*/) {
│ │ │ │ -
40 size_t size;
│ │ │ │ -
41 ar >> BOOST_SERIALIZATION_NVP(size);
│ │ │ │ -
42 v.resize(size);
│ │ │ │ -
43 ar >> make_nvp("data", make_array(v.data(), v.size()));
│ │ │ │ -
44}
│ │ │ │ -
45
│ │ │ │ -
46// split version - copies into an STL vector for serialization
│ │ │ │ -
47template <class Archive, int D>
│ │ │ │ -
48void save(Archive& ar, const Eigen::Matrix<double, D, 1>& v,
│ │ │ │ -
49 unsigned int /*version*/) {
│ │ │ │ -
50 ar << make_nvp("data", make_array(v.data(), v.RowsAtCompileTime));
│ │ │ │ -
51}
│ │ │ │ -
52
│ │ │ │ -
53template <class Archive, int D>
│ │ │ │ -
54void load(Archive& ar, Eigen::Matrix<double, D, 1>& v,
│ │ │ │ -
55 unsigned int /*version*/) {
│ │ │ │ -
56 ar >> make_nvp("data", make_array(v.data(), v.RowsAtCompileTime));
│ │ │ │ -
57}
│ │ │ │ -
58
│ │ │ │ -
59} // namespace serialization
│ │ │ │ -
60} // namespace boost
│ │ │ │ -
61
│ │ │ │ -
62BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector)
│ │ │ │ -
63BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector2)
│ │ │ │ -
64BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector3)
│ │ │ │ -
65BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector6)
│ │ │ │ -
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ -
gtsam::save
void save(const Matrix &A, const string &s, const string &filename)
save a matrix to file, which can be loaded by matlab
Definition Matrix.cpp:167
│ │ │ │ +
49 template<class BAYESTREE, class GRAPH>
│ │ │ │ +
│ │ │ │ +
50 class JunctionTree : public EliminatableClusterTree<BAYESTREE, GRAPH> {
│ │ │ │ +
51
│ │ │ │ +
52 public:
│ │ │ │ +
53
│ │ │ │ +
54 typedef JunctionTree<BAYESTREE, GRAPH> This;
│ │ │ │ +
55 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
56 typedef EliminatableClusterTree<BAYESTREE, GRAPH> Base;
│ │ │ │ +
57
│ │ │ │ +
58 protected:
│ │ │ │ +
59
│ │ │ │ +
62
│ │ │ │ +
64 template<class ETREE>
│ │ │ │ +
65 static This FromEliminationTree(const ETREE& eliminationTree) { return This(eliminationTree); }
│ │ │ │ +
66
│ │ │ │ +
68 template<class ETREE_BAYESNET, class ETREE_GRAPH>
│ │ │ │ +
69 JunctionTree(const EliminationTree<ETREE_BAYESNET, ETREE_GRAPH>& eliminationTree);
│ │ │ │ +
70
│ │ │ │ +
72
│ │ │ │ +
73 protected:
│ │ │ │ +
74
│ │ │ │ +
75 // Private default constructor (used in static construction methods)
│ │ │ │ +
76 JunctionTree() {}
│ │ │ │ +
77
│ │ │ │ +
78 };
│ │ │ │ +
│ │ │ │ +
79
│ │ │ │ +
80}
│ │ │ │ +
ClusterTree.h
Collects factorgraph fragments defined on variable clusters, arranged in a tree.
│ │ │ │ +
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::EliminatableClusterTree
A cluster-tree that eliminates to a Bayes tree.
Definition ClusterTree.h:184
│ │ │ │ +
gtsam::EliminationTree
An elimination tree is a data structure used intermediately during elimination.
Definition EliminationTree.h:52
│ │ │ │ +
gtsam::JunctionTree
A JunctionTree is a cluster tree, a set of variable clusters with factors, arranged in a tree,...
Definition JunctionTree.h:50
│ │ │ │ +
gtsam::JunctionTree::FromEliminationTree
static This FromEliminationTree(const ETREE &eliminationTree)
Build the junction tree from an elimination tree.
Definition JunctionTree.h:65
│ │ │ │ +
gtsam::JunctionTree::This
JunctionTree< BAYESTREE, GRAPH > This
This class.
Definition JunctionTree.h:54
│ │ │ │ +
gtsam::JunctionTree::Base
EliminatableClusterTree< BAYESTREE, GRAPH > Base
Our base class.
Definition JunctionTree.h:56
│ │ │ │ +
gtsam::JunctionTree::shared_ptr
boost::shared_ptr< This > shared_ptr
Shared pointer to this class.
Definition JunctionTree.h:55
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,77 +1,94 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -VectorSerialization.h │ │ │ │ │ +JunctionTree.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_c_t_o_r_._h> │ │ │ │ │ +21#pragma once │ │ │ │ │ 22 │ │ │ │ │ -23#include │ │ │ │ │ -24#include │ │ │ │ │ -25#include │ │ │ │ │ +23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_C_l_u_s_t_e_r_T_r_e_e_._h> │ │ │ │ │ +24 │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ 26 │ │ │ │ │ -27namespace boost { │ │ │ │ │ -28namespace serialization { │ │ │ │ │ +27 // Forward declarations │ │ │ │ │ +28 template class EliminationTree; │ │ │ │ │ 29 │ │ │ │ │ -30// split version - copies into an STL vector for serialization │ │ │ │ │ -31template │ │ │ │ │ -32void _s_a_v_e(Archive& ar, const gtsam::Vector& v, unsigned int /*version*/) { │ │ │ │ │ -33 const size_t size = v.size(); │ │ │ │ │ -34 ar << BOOST_SERIALIZATION_NVP(size); │ │ │ │ │ -35 ar << make_nvp("data", make_array(v.data(), v.size())); │ │ │ │ │ -36} │ │ │ │ │ -37 │ │ │ │ │ -38template │ │ │ │ │ -39void load(Archive& ar, gtsam::Vector& v, unsigned int /*version*/) { │ │ │ │ │ -40 size_t size; │ │ │ │ │ -41 ar >> BOOST_SERIALIZATION_NVP(size); │ │ │ │ │ -42 v.resize(size); │ │ │ │ │ -43 ar >> make_nvp("data", make_array(v.data(), v.size())); │ │ │ │ │ -44} │ │ │ │ │ -45 │ │ │ │ │ -46// split version - copies into an STL vector for serialization │ │ │ │ │ -47template │ │ │ │ │ -48void _s_a_v_e(Archive& ar, const Eigen::Matrix& v, │ │ │ │ │ -49 unsigned int /*version*/) { │ │ │ │ │ -50 ar << make_nvp("data", make_array(v.data(), v.RowsAtCompileTime)); │ │ │ │ │ -51} │ │ │ │ │ -52 │ │ │ │ │ -53template │ │ │ │ │ -54void load(Archive& ar, Eigen::Matrix& v, │ │ │ │ │ -55 unsigned int /*version*/) { │ │ │ │ │ -56 ar >> make_nvp("data", make_array(v.data(), v.RowsAtCompileTime)); │ │ │ │ │ -57} │ │ │ │ │ -58 │ │ │ │ │ -59} // namespace serialization │ │ │ │ │ -60} // namespace boost │ │ │ │ │ -61 │ │ │ │ │ -62BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector) │ │ │ │ │ -63BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector2) │ │ │ │ │ -64BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector3) │ │ │ │ │ -65BOOST_SERIALIZATION_SPLIT_FREE(gtsam::Vector6) │ │ │ │ │ -_V_e_c_t_o_r_._h │ │ │ │ │ -typedef and functions to augment Eigen's VectorXd │ │ │ │ │ -_g_t_s_a_m_:_:_s_a_v_e │ │ │ │ │ -void save(const Matrix &A, const string &s, const string &filename) │ │ │ │ │ -save a matrix to file, which can be loaded by matlab │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:167 │ │ │ │ │ +49 template │ │ │ │ │ +_5_0 class _J_u_n_c_t_i_o_n_T_r_e_e : public _E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e { │ │ │ │ │ +51 │ │ │ │ │ +52 public: │ │ │ │ │ +53 │ │ │ │ │ +_5_4 typedef _J_u_n_c_t_i_o_n_T_r_e_e_<_B_A_Y_E_S_T_R_E_E_,_ _G_R_A_P_H_> _T_h_i_s; │ │ │ │ │ +_5_5 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +_5_6 typedef _E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_<_B_A_Y_E_S_T_R_E_E_,_ _G_R_A_P_H_> _B_a_s_e; │ │ │ │ │ +57 │ │ │ │ │ +58 protected: │ │ │ │ │ +59 │ │ │ │ │ +62 │ │ │ │ │ +64 template │ │ │ │ │ +_6_5 static _T_h_i_s _F_r_o_m_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e(const ETREE& eliminationTree) { return _T_h_i_s │ │ │ │ │ +(eliminationTree); } │ │ │ │ │ +66 │ │ │ │ │ +68 template │ │ │ │ │ +69 _J_u_n_c_t_i_o_n_T_r_e_e(const _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_E_T_R_E_E___B_A_Y_E_S_N_E_T_,_ _E_T_R_E_E___G_R_A_P_H_>& │ │ │ │ │ +eliminationTree); │ │ │ │ │ +70 │ │ │ │ │ +72 │ │ │ │ │ +73 protected: │ │ │ │ │ +74 │ │ │ │ │ +75 // Private default constructor (used in static construction methods) │ │ │ │ │ +76 _J_u_n_c_t_i_o_n_T_r_e_e() {} │ │ │ │ │ +77 │ │ │ │ │ +78 }; │ │ │ │ │ +79 │ │ │ │ │ +80} │ │ │ │ │ +_C_l_u_s_t_e_r_T_r_e_e_._h │ │ │ │ │ +Collects factorgraph fragments defined on variable clusters, arranged in a │ │ │ │ │ +tree. │ │ │ │ │ +_g_t_s_a_m │ │ │ │ │ +Global functions in a separate testing namespace. │ │ │ │ │ +DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e │ │ │ │ │ +A cluster-tree that eliminates to a Bayes tree. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:184 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ +An elimination tree is a data structure used intermediately during elimination. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ +A JunctionTree is a cluster tree, a set of variable clusters with factors, │ │ │ │ │ +arranged in a tree,... │ │ │ │ │ +DDeeffiinniittiioonn JunctionTree.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_J_u_n_c_t_i_o_n_T_r_e_e_:_:_F_r_o_m_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ +static This FromEliminationTree(const ETREE &eliminationTree) │ │ │ │ │ +Build the junction tree from an elimination tree. │ │ │ │ │ +DDeeffiinniittiioonn JunctionTree.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_J_u_n_c_t_i_o_n_T_r_e_e_:_:_T_h_i_s │ │ │ │ │ +JunctionTree< BAYESTREE, GRAPH > This │ │ │ │ │ +This class. │ │ │ │ │ +DDeeffiinniittiioonn JunctionTree.h:54 │ │ │ │ │ +_g_t_s_a_m_:_:_J_u_n_c_t_i_o_n_T_r_e_e_:_:_B_a_s_e │ │ │ │ │ +EliminatableClusterTree< BAYESTREE, GRAPH > Base │ │ │ │ │ +Our base class. │ │ │ │ │ +DDeeffiinniittiioonn JunctionTree.h:56 │ │ │ │ │ +_g_t_s_a_m_:_:_J_u_n_c_t_i_o_n_T_r_e_e_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +Shared pointer to this class. │ │ │ │ │ +DDeeffiinniittiioonn JunctionTree.h:55 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _b_a_s_e │ │ │ │ │ - * _V_e_c_t_o_r_S_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ + * _i_n_f_e_r_e_n_c_e │ │ │ │ │ + * _J_u_n_c_t_i_o_n_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00005.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00947.html │ │ │ │┄ Files 88% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/debug.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/Scenario.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions | │ │ │ │ -Variables
│ │ │ │ -
debug.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
Scenario.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Global debugging flags. │ │ │ │ +

Simple class to test navigation scenarios. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::Scenario
 Simple trajectory simulator. More...
 
class  gtsam::ConstantTwistScenario
 Scenario with constant twist 3D trajectory. More...
 
class  gtsam::AcceleratingScenario
 Accelerating from an arbitrary initial state, with optional rotation. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Functions

│ │ │ │ -bool gtsam::guardedIsDebug (const std::string &s)
 
│ │ │ │ -void gtsam::guardedSetDebug (const std::string &s, const bool v)
 
│ │ │ │ -bool gtsam::isDebugVersion ()
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Variables

│ │ │ │ -GTSAM_EXPORT FastMap< std::string, ValueWithDefault< bool, false > > gtsam::debugFlags
 
│ │ │ │

Detailed Description

│ │ │ │ -

Global debugging flags.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Feb 1, 2011
│ │ │ │ +

Simple class to test navigation scenarios.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,33 +1,32 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s | _V_a_r_i_a_b_l_e_s │ │ │ │ │ -debug.cpp File Reference │ │ │ │ │ -Global debugging flags. _M_o_r_e_._._. │ │ │ │ │ -NNaammeessppaacceess │ │ │ │ │ -namespace   _g_t_s_a_m │ │ │ │ │ -  Global functions in a separate testing namespace. │ │ │ │ │ -  │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -bool  ggttssaamm::::gguuaarrddeeddIIssDDeebbuugg (const std::string &s) │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +Scenario.h File Reference │ │ │ │ │ +Simple class to test navigation scenarios. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_S_c_e_n_a_r_i_o │ │ │ │ │ +  Simple trajectory simulator. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -void  ggttssaamm::::gguuaarrddeeddSSeettDDeebbuugg (const std::string &s, const bool v) │ │ │ │ │ +class   _g_t_s_a_m_:_:_C_o_n_s_t_a_n_t_T_w_i_s_t_S_c_e_n_a_r_i_o │ │ │ │ │ +  _S_c_e_n_a_r_i_o with constant twist 3D trajectory. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -bool  ggttssaamm::::iissDDeebbuuggVVeerrssiioonn () │ │ │ │ │ +class   _g_t_s_a_m_:_:_A_c_c_e_l_e_r_a_t_i_n_g_S_c_e_n_a_r_i_o │ │ │ │ │ +  Accelerating from an arbitrary initial state, with optional rotation. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -VVaarriiaabblleess │ │ │ │ │ -GTSAM_EXPORT _F_a_s_t_M_a_p< std::string, _V_a_l_u_e_W_i_t_h_D_e_f_a_u_l_t< bool, │ │ │ │ │ - false > >  ggttssaamm::::ddeebbuuggFFllaaggss │ │ │ │ │ +NNaammeessppaacceess │ │ │ │ │ +namespace   _g_t_s_a_m │ │ │ │ │ +  Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Global debugging flags. │ │ │ │ │ +Simple class to test navigation scenarios. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Date │ │ │ │ │ - Feb 1, 2011 │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _b_a_s_e │ │ │ │ │ - * _d_e_b_u_g_._c_p_p │ │ │ │ │ + * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ + * _S_c_e_n_a_r_i_o_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00011_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01136_source.html │ │ │ │┄ Files 81% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Lie.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Values-inl.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Lie.h
│ │ │ │ +
Values-inl.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
24#pragma once
│ │ │ │ -
25
│ │ │ │ -
26#include <gtsam/base/Manifold.h>
│ │ │ │ -
27#include <gtsam/base/Group.h>
│ │ │ │ -
28
│ │ │ │ -
29namespace gtsam {
│ │ │ │ +
25#pragma once
│ │ │ │ +
26
│ │ │ │ +
27#include <utility>
│ │ │ │ +
28#include <boost/bind/bind.hpp>
│ │ │ │ +
29#include <gtsam/nonlinear/Values.h>
│ │ │ │
30
│ │ │ │ -
36template <class Class, int N>
│ │ │ │ -
│ │ │ │ -
37struct LieGroup {
│ │ │ │ -
38
│ │ │ │ -
39 enum { dimension = N };
│ │ │ │ -
40 typedef OptionalJacobian<N, N> ChartJacobian;
│ │ │ │ -
41 typedef Eigen::Matrix<double, N, N> Jacobian;
│ │ │ │ -
42 typedef Eigen::Matrix<double, N, 1> TangentVector;
│ │ │ │ -
43
│ │ │ │ -
44 const Class & derived() const {
│ │ │ │ -
45 return static_cast<const Class&>(*this);
│ │ │ │ -
46 }
│ │ │ │ -
47
│ │ │ │ -
48 Class compose(const Class& g) const {
│ │ │ │ -
49 return derived() * g;
│ │ │ │ -
50 }
│ │ │ │ -
51
│ │ │ │ -
52 Class between(const Class& g) const {
│ │ │ │ -
53 return derived().inverse() * g;
│ │ │ │ -
54 }
│ │ │ │ -
55
│ │ │ │ -
56 Class compose(const Class& g, ChartJacobian H1,
│ │ │ │ -
57 ChartJacobian H2 = boost::none) const {
│ │ │ │ -
58 if (H1) *H1 = g.inverse().AdjointMap();
│ │ │ │ -
59 if (H2) *H2 = Eigen::Matrix<double, N, N>::Identity();
│ │ │ │ -
60 return derived() * g;
│ │ │ │ -
61 }
│ │ │ │ -
62
│ │ │ │ -
63 Class between(const Class& g, ChartJacobian H1,
│ │ │ │ -
64 ChartJacobian H2 = boost::none) const {
│ │ │ │ -
65 Class result = derived().inverse() * g;
│ │ │ │ -
66 if (H1) *H1 = - result.inverse().AdjointMap();
│ │ │ │ -
67 if (H2) *H2 = Eigen::Matrix<double, N, N>::Identity();
│ │ │ │ -
68 return result;
│ │ │ │ -
69 }
│ │ │ │ -
70
│ │ │ │ -
71 Class inverse(ChartJacobian H) const {
│ │ │ │ -
72 if (H) *H = - derived().AdjointMap();
│ │ │ │ -
73 return derived().inverse();
│ │ │ │ -
74 }
│ │ │ │ -
75
│ │ │ │ -
│ │ │ │ -
78 Class expmap(const TangentVector& v) const {
│ │ │ │ -
79 return compose(Class::Expmap(v));
│ │ │ │ -
80 }
│ │ │ │ -
│ │ │ │ -
81
│ │ │ │ -
│ │ │ │ -
84 TangentVector logmap(const Class& g) const {
│ │ │ │ -
85 return Class::Logmap(between(g));
│ │ │ │ -
86 }
│ │ │ │ -
│ │ │ │ -
87
│ │ │ │ -
│ │ │ │ -
89 Class expmap(const TangentVector& v, //
│ │ │ │ -
90 ChartJacobian H1, ChartJacobian H2 = boost::none) const {
│ │ │ │ -
91 Jacobian D_g_v;
│ │ │ │ -
92 Class g = Class::Expmap(v,H2 ? &D_g_v : 0);
│ │ │ │ -
93 Class h = compose(g); // derivatives inlined below
│ │ │ │ -
94 if (H1) *H1 = g.inverse().AdjointMap();
│ │ │ │ -
95 if (H2) *H2 = D_g_v;
│ │ │ │ -
96 return h;
│ │ │ │ -
97 }
│ │ │ │ -
│ │ │ │ -
98
│ │ │ │ -
│ │ │ │ -
100 TangentVector logmap(const Class& g, //
│ │ │ │ -
101 ChartJacobian H1, ChartJacobian H2 = boost::none) const {
│ │ │ │ -
102 Class h = between(g); // derivatives inlined below
│ │ │ │ -
103 Jacobian D_v_h;
│ │ │ │ -
104 TangentVector v = Class::Logmap(h, (H1 || H2) ? &D_v_h : 0);
│ │ │ │ -
105 if (H1) *H1 = - D_v_h * h.inverse().AdjointMap();
│ │ │ │ -
106 if (H2) *H2 = D_v_h;
│ │ │ │ -
107 return v;
│ │ │ │ -
108 }
│ │ │ │ -
│ │ │ │ -
109
│ │ │ │ -
│ │ │ │ -
111 static Class Retract(const TangentVector& v) {
│ │ │ │ -
112 return Class::ChartAtOrigin::Retract(v);
│ │ │ │ -
113 }
│ │ │ │ -
│ │ │ │ -
114
│ │ │ │ -
│ │ │ │ -
116 static TangentVector LocalCoordinates(const Class& g) {
│ │ │ │ -
117 return Class::ChartAtOrigin::Local(g);
│ │ │ │ -
118 }
│ │ │ │ -
│ │ │ │ +
31namespace gtsam {
│ │ │ │ +
32
│ │ │ │ +
33
│ │ │ │ +
34 /* ************************************************************************* */
│ │ │ │ +
35 template<class ValueType>
│ │ │ │ +
│ │ │ │ +
36 struct _ValuesKeyValuePair {
│ │ │ │ +
37 const Key key;
│ │ │ │ +
38 ValueType& value;
│ │ │ │ +
39
│ │ │ │ +
40 _ValuesKeyValuePair(Key _key, ValueType& _value) : key(_key), value(_value) {}
│ │ │ │ +
41 };
│ │ │ │ +
│ │ │ │ +
42
│ │ │ │ +
43 /* ************************************************************************* */
│ │ │ │ +
44 template<class ValueType>
│ │ │ │ +
│ │ │ │ +
45 struct _ValuesConstKeyValuePair {
│ │ │ │ +
46 const Key key;
│ │ │ │ +
47 const ValueType& value;
│ │ │ │ +
48
│ │ │ │ +
49 _ValuesConstKeyValuePair(Key _key, const ValueType& _value) :
│ │ │ │ +
50 key(_key), value(_value) {
│ │ │ │ +
51 }
│ │ │ │ +
52 _ValuesConstKeyValuePair(const _ValuesKeyValuePair<ValueType>& rhs) :
│ │ │ │ +
53 key(rhs.key), value(rhs.value) {
│ │ │ │ +
54 }
│ │ │ │ +
55 };
│ │ │ │ +
│ │ │ │ +
56
│ │ │ │ +
57 /* ************************************************************************* */
│ │ │ │ +
58
│ │ │ │ +
59 // Cast helpers for making _Values[Const]KeyValuePair's from Values::[Const]KeyValuePair
│ │ │ │ +
60 // need to use a struct here for later partial specialization
│ │ │ │ +
61 template<class ValueType, class CastedKeyValuePairType, class KeyValuePairType>
│ │ │ │ +
│ │ │ │ +
62 struct ValuesCastHelper {
│ │ │ │ +
63 static CastedKeyValuePairType cast(KeyValuePairType key_value) {
│ │ │ │ +
64 // Static cast because we already checked the type during filtering
│ │ │ │ +
65 return CastedKeyValuePairType(key_value.key,
│ │ │ │ +
66 const_cast<GenericValue<ValueType>&>(static_cast<const GenericValue<
│ │ │ │ +
67 ValueType>&>(key_value.value)).value());
│ │ │ │ +
68 }
│ │ │ │ +
69 };
│ │ │ │ +
│ │ │ │ +
70 // partial specialized version for ValueType == Value
│ │ │ │ +
71 template<class CastedKeyValuePairType, class KeyValuePairType>
│ │ │ │ +
│ │ │ │ +
72 struct ValuesCastHelper<Value, CastedKeyValuePairType, KeyValuePairType> {
│ │ │ │ +
73 static CastedKeyValuePairType cast(KeyValuePairType key_value) {
│ │ │ │ +
74 // Static cast because we already checked the type during filtering
│ │ │ │ +
75 // in this case the casted and keyvalue pair are essentially the same type
│ │ │ │ +
76 // (key, Value&) so perhaps this could be done with just a cast of the key_value?
│ │ │ │ +
77 return CastedKeyValuePairType(key_value.key, key_value.value);
│ │ │ │ +
78 }
│ │ │ │ +
79 };
│ │ │ │ +
│ │ │ │ +
80 // partial specialized version for ValueType == Value
│ │ │ │ +
81 template<class CastedKeyValuePairType, class KeyValuePairType>
│ │ │ │ +
│ │ │ │ +
82 struct ValuesCastHelper<const Value, CastedKeyValuePairType, KeyValuePairType> {
│ │ │ │ +
83 static CastedKeyValuePairType cast(KeyValuePairType key_value) {
│ │ │ │ +
84 // Static cast because we already checked the type during filtering
│ │ │ │ +
85 // in this case the casted and keyvalue pair are essentially the same type
│ │ │ │ +
86 // (key, Value&) so perhaps this could be done with just a cast of the key_value?
│ │ │ │ +
87 return CastedKeyValuePairType(key_value.key, key_value.value);
│ │ │ │ +
88 }
│ │ │ │ +
89 };
│ │ │ │ +
│ │ │ │ +
90
│ │ │ │ +
91/* ************************************************************************* */
│ │ │ │ +
92 template <class ValueType>
│ │ │ │ +
93 std::map<Key, ValueType>
│ │ │ │ +
│ │ │ │ +
94 Values::extract(const std::function<bool(Key)>& filterFcn) const {
│ │ │ │ +
95 std::map<Key, ValueType> result;
│ │ │ │ +
96 for (const auto& key_value : values_) {
│ │ │ │ +
97 // Check if key matches
│ │ │ │ +
98 if (filterFcn(key_value.first)) {
│ │ │ │ +
99 // Check if type matches (typically does as symbols matched with types)
│ │ │ │ +
100 if (auto t =
│ │ │ │ +
101 dynamic_cast<const GenericValue<ValueType>*>(key_value.second))
│ │ │ │ +
102 result[key_value.first] = t->value();
│ │ │ │ +
103 }
│ │ │ │ +
104 }
│ │ │ │ +
105 return result;
│ │ │ │ +
106 }
│ │ │ │ +
│ │ │ │ +
107
│ │ │ │ +
108/* ************************************************************************* */
│ │ │ │ +
109#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ +
110#include <boost/bind/bind.hpp>
│ │ │ │ +
111
│ │ │ │ +
112 template<class ValueType>
│ │ │ │ +
113 class Values::Filtered {
│ │ │ │ +
114 public:
│ │ │ │ +
116 typedef _ValuesKeyValuePair<ValueType> KeyValuePair;
│ │ │ │ +
117 typedef _ValuesConstKeyValuePair<ValueType> ConstKeyValuePair;
│ │ │ │ +
118 typedef KeyValuePair value_type;
│ │ │ │
119
│ │ │ │ -
│ │ │ │ -
121 static Class Retract(const TangentVector& v, ChartJacobian H) {
│ │ │ │ -
122 return Class::ChartAtOrigin::Retract(v,H);
│ │ │ │ -
123 }
│ │ │ │ -
│ │ │ │ -
124
│ │ │ │ -
│ │ │ │ -
126 static TangentVector LocalCoordinates(const Class& g, ChartJacobian H) {
│ │ │ │ -
127 return Class::ChartAtOrigin::Local(g,H);
│ │ │ │ -
128 }
│ │ │ │ -
│ │ │ │ +
120 typedef
│ │ │ │ +
121 boost::transform_iterator<
│ │ │ │ +
122 KeyValuePair(*)(Values::KeyValuePair),
│ │ │ │ +
123 boost::filter_iterator<
│ │ │ │ +
124 std::function<bool(const Values::ConstKeyValuePair&)>,
│ │ │ │ +
125 Values::iterator> >
│ │ │ │ +
126 iterator;
│ │ │ │ +
127
│ │ │ │ +
128 typedef iterator const_iterator;
│ │ │ │
129
│ │ │ │ -
│ │ │ │ -
131 Class retract(const TangentVector& v) const {
│ │ │ │ -
132 return compose(Class::ChartAtOrigin::Retract(v));
│ │ │ │ -
133 }
│ │ │ │ -
│ │ │ │ -
134
│ │ │ │ -
│ │ │ │ -
136 TangentVector localCoordinates(const Class& g) const {
│ │ │ │ -
137 return Class::ChartAtOrigin::Local(between(g));
│ │ │ │ -
138 }
│ │ │ │ -
│ │ │ │ -
139
│ │ │ │ -
│ │ │ │ -
141 Class retract(const TangentVector& v, //
│ │ │ │ -
142 ChartJacobian H1, ChartJacobian H2 = boost::none) const {
│ │ │ │ -
143 Jacobian D_g_v;
│ │ │ │ -
144 Class g = Class::ChartAtOrigin::Retract(v, H2 ? &D_g_v : 0);
│ │ │ │ -
145 Class h = compose(g); // derivatives inlined below
│ │ │ │ -
146 if (H1) *H1 = g.inverse().AdjointMap();
│ │ │ │ -
147 if (H2) *H2 = D_g_v;
│ │ │ │ -
148 return h;
│ │ │ │ -
149 }
│ │ │ │ -
│ │ │ │ -
150
│ │ │ │ -
│ │ │ │ -
152 TangentVector localCoordinates(const Class& g, //
│ │ │ │ -
153 ChartJacobian H1, ChartJacobian H2 = boost::none) const {
│ │ │ │ -
154 Class h = between(g); // derivatives inlined below
│ │ │ │ -
155 Jacobian D_v_h;
│ │ │ │ -
156 TangentVector v = Class::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0);
│ │ │ │ -
157 if (H1) *H1 = - D_v_h * h.inverse().AdjointMap();
│ │ │ │ -
158 if (H2) *H2 = D_v_h;
│ │ │ │ -
159 return v;
│ │ │ │ -
160 }
│ │ │ │ -
│ │ │ │ -
161};
│ │ │ │ -
│ │ │ │ -
162
│ │ │ │ -
164struct lie_group_tag: public manifold_tag, public group_tag {};
│ │ │ │ -
165
│ │ │ │ -
166namespace internal {
│ │ │ │ -
167
│ │ │ │ -
173template<class Class>
│ │ │ │ -
│ │ │ │ -
174struct LieGroupTraits: GetDimensionImpl<Class, Class::dimension> {
│ │ │ │ -
175 typedef lie_group_tag structure_category;
│ │ │ │ -
176
│ │ │ │ -
179 typedef multiplicative_group_tag group_flavor;
│ │ │ │ -
180 static Class Identity() { return Class::Identity();}
│ │ │ │ -
182
│ │ │ │ -
185 typedef Class ManifoldType;
│ │ │ │ -
186 enum { dimension = Class::dimension };
│ │ │ │ -
187 typedef Eigen::Matrix<double, dimension, 1> TangentVector;
│ │ │ │ -
188 typedef OptionalJacobian<dimension, dimension> ChartJacobian;
│ │ │ │ -
189
│ │ │ │ -
190 static TangentVector Local(const Class& origin, const Class& other,
│ │ │ │ -
191 ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
│ │ │ │ -
192 return origin.localCoordinates(other, Horigin, Hother);
│ │ │ │ -
193 }
│ │ │ │ +
130 typedef
│ │ │ │ +
131 boost::transform_iterator<
│ │ │ │ +
132 ConstKeyValuePair(*)(Values::ConstKeyValuePair),
│ │ │ │ +
133 boost::filter_iterator<
│ │ │ │ +
134 std::function<bool(const Values::ConstKeyValuePair&)>,
│ │ │ │ +
135 Values::const_iterator> >
│ │ │ │ +
136 const_const_iterator;
│ │ │ │ +
137
│ │ │ │ +
138 iterator begin() { return begin_; }
│ │ │ │ +
139 iterator end() { return end_; }
│ │ │ │ +
140 const_iterator begin() const { return begin_; }
│ │ │ │ +
141 const_iterator end() const { return end_; }
│ │ │ │ +
142 const_const_iterator beginConst() const { return constBegin_; }
│ │ │ │ +
143 const_const_iterator endConst() const { return constEnd_; }
│ │ │ │ +
144
│ │ │ │ +
146 size_t size() const {
│ │ │ │ +
147 size_t i = 0;
│ │ │ │ +
148 for (const_const_iterator it = beginConst(); it != endConst(); ++it)
│ │ │ │ +
149 ++i;
│ │ │ │ +
150 return i;
│ │ │ │ +
151 }
│ │ │ │ +
152
│ │ │ │ +
153 private:
│ │ │ │ +
154 Filtered(
│ │ │ │ +
155 const std::function<bool(const Values::ConstKeyValuePair&)>& filter,
│ │ │ │ +
156 Values& values) :
│ │ │ │ +
157 begin_(
│ │ │ │ +
158 boost::make_transform_iterator(
│ │ │ │ +
159 boost::make_filter_iterator(filter, values.begin(), values.end()),
│ │ │ │ +
160 &ValuesCastHelper<ValueType, KeyValuePair, Values::KeyValuePair>::cast)), end_(
│ │ │ │ +
161 boost::make_transform_iterator(
│ │ │ │ +
162 boost::make_filter_iterator(filter, values.end(), values.end()),
│ │ │ │ +
163 &ValuesCastHelper<ValueType, KeyValuePair, Values::KeyValuePair>::cast)), constBegin_(
│ │ │ │ +
164 boost::make_transform_iterator(
│ │ │ │ +
165 boost::make_filter_iterator(filter,
│ │ │ │ +
166 values._begin(),
│ │ │ │ +
167 values._end()),
│ │ │ │ +
168 &ValuesCastHelper<ValueType, ConstKeyValuePair,
│ │ │ │ +
169 Values::ConstKeyValuePair>::cast)), constEnd_(
│ │ │ │ +
170 boost::make_transform_iterator(
│ │ │ │ +
171 boost::make_filter_iterator(filter,
│ │ │ │ +
172 values._end(),
│ │ │ │ +
173 values._end()),
│ │ │ │ +
174 &ValuesCastHelper<ValueType, ConstKeyValuePair,
│ │ │ │ +
175 Values::ConstKeyValuePair>::cast)) {
│ │ │ │ +
176 }
│ │ │ │ +
177
│ │ │ │ +
178 friend class Values;
│ │ │ │ +
179 iterator begin_;
│ │ │ │ +
180 iterator end_;
│ │ │ │ +
181 const_const_iterator constBegin_;
│ │ │ │ +
182 const_const_iterator constEnd_;
│ │ │ │ +
183 };
│ │ │ │ +
184
│ │ │ │ +
185 template<class ValueType>
│ │ │ │ +
186 class Values::ConstFiltered {
│ │ │ │ +
187 public:
│ │ │ │ +
189 typedef _ValuesConstKeyValuePair<ValueType> KeyValuePair;
│ │ │ │ +
190 typedef KeyValuePair value_type;
│ │ │ │ +
191
│ │ │ │ +
192 typedef typename Filtered<ValueType>::const_const_iterator iterator;
│ │ │ │ +
193 typedef typename Filtered<ValueType>::const_const_iterator const_iterator;
│ │ │ │
194
│ │ │ │ -
195 static Class Retract(const Class& origin, const TangentVector& v,
│ │ │ │ -
196 ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) {
│ │ │ │ -
197 return origin.retract(v, Horigin, Hv);
│ │ │ │ -
198 }
│ │ │ │ -
200
│ │ │ │ -
203 static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) {
│ │ │ │ -
204 return Class::Logmap(m, Hm);
│ │ │ │ -
205 }
│ │ │ │ -
206
│ │ │ │ -
207 static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) {
│ │ │ │ -
208 return Class::Expmap(v, Hv);
│ │ │ │ -
209 }
│ │ │ │ -
210
│ │ │ │ -
211 static Class Compose(const Class& m1, const Class& m2, //
│ │ │ │ -
212 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ -
213 return m1.compose(m2, H1, H2);
│ │ │ │ -
214 }
│ │ │ │ -
215
│ │ │ │ -
216 static Class Between(const Class& m1, const Class& m2, //
│ │ │ │ -
217 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
│ │ │ │ -
218 return m1.between(m2, H1, H2);
│ │ │ │ -
219 }
│ │ │ │ -
220
│ │ │ │ -
221 static Class Inverse(const Class& m, //
│ │ │ │ -
222 ChartJacobian H = boost::none) {
│ │ │ │ -
223 return m.inverse(H);
│ │ │ │ -
224 }
│ │ │ │ -
226};
│ │ │ │ -
│ │ │ │ -
227
│ │ │ │ -
229template<class Class> struct LieGroup: LieGroupTraits<Class>, Testable<Class> {};
│ │ │ │ -
230
│ │ │ │ -
231} // \ namepsace internal
│ │ │ │ -
232
│ │ │ │ -
239template<class Class>
│ │ │ │ -
│ │ │ │ -
240inline Class between_default(const Class& l1, const Class& l2) {
│ │ │ │ -
241 return l1.inverse().compose(l2);
│ │ │ │ -
242}
│ │ │ │ -
│ │ │ │ -
243
│ │ │ │ -
245template<class Class>
│ │ │ │ -
│ │ │ │ -
246inline Vector logmap_default(const Class& l0, const Class& lp) {
│ │ │ │ -
247 return Class::Logmap(l0.between(lp));
│ │ │ │ -
248}
│ │ │ │ -
│ │ │ │ -
249
│ │ │ │ -
251template<class Class>
│ │ │ │ -
│ │ │ │ -
252inline Class expmap_default(const Class& t, const Vector& d) {
│ │ │ │ -
253 return t.compose(Class::Expmap(d));
│ │ │ │ -
254}
│ │ │ │ -
│ │ │ │ +
196 ConstFiltered(const Filtered<ValueType>& rhs) :
│ │ │ │ +
197 begin_(rhs.beginConst()),
│ │ │ │ +
198 end_(rhs.endConst()) {}
│ │ │ │ +
199
│ │ │ │ +
200 iterator begin() { return begin_; }
│ │ │ │ +
201 iterator end() { return end_; }
│ │ │ │ +
202 const_iterator begin() const { return begin_; }
│ │ │ │ +
203 const_iterator end() const { return end_; }
│ │ │ │ +
204
│ │ │ │ +
206 size_t size() const {
│ │ │ │ +
207 size_t i = 0;
│ │ │ │ +
208 for (const_iterator it = begin(); it != end(); ++it)
│ │ │ │ +
209 ++i;
│ │ │ │ +
210 return i;
│ │ │ │ +
211 }
│ │ │ │ +
212
│ │ │ │ +
213 FastList<Key> keys() const {
│ │ │ │ +
214 FastList<Key> result;
│ │ │ │ +
215 for(const_iterator it = begin(); it != end(); ++it)
│ │ │ │ +
216 result.push_back(it->key);
│ │ │ │ +
217 return result;
│ │ │ │ +
218 }
│ │ │ │ +
219
│ │ │ │ +
220 private:
│ │ │ │ +
221 friend class Values;
│ │ │ │ +
222 const_iterator begin_;
│ │ │ │ +
223 const_iterator end_;
│ │ │ │ +
224 ConstFiltered(
│ │ │ │ +
225 const std::function<bool(const Values::ConstKeyValuePair&)>& filter,
│ │ │ │ +
226 const Values& values) {
│ │ │ │ +
227 // We remove the const from values to create a non-const Filtered
│ │ │ │ +
228 // view, then pull the const_iterators out of it.
│ │ │ │ +
229 const Filtered<ValueType> filtered(filter, const_cast<Values&>(values));
│ │ │ │ +
230 begin_ = filtered.beginConst();
│ │ │ │ +
231 end_ = filtered.endConst();
│ │ │ │ +
232 }
│ │ │ │ +
233 };
│ │ │ │ +
234
│ │ │ │ +
235 template<class ValueType>
│ │ │ │ +
236 Values::Values(const Values::Filtered<ValueType>& view) {
│ │ │ │ +
237 for(const auto key_value: view) {
│ │ │ │ +
238 Key key = key_value.key;
│ │ │ │ +
239 insert(key, static_cast<const ValueType&>(key_value.value));
│ │ │ │ +
240 }
│ │ │ │ +
241 }
│ │ │ │ +
242
│ │ │ │ +
243 template<class ValueType>
│ │ │ │ +
244 Values::Values(const Values::ConstFiltered<ValueType>& view) {
│ │ │ │ +
245 for(const auto key_value: view) {
│ │ │ │ +
246 Key key = key_value.key;
│ │ │ │ +
247 insert(key, static_cast<const ValueType&>(key_value.value));
│ │ │ │ +
248 }
│ │ │ │ +
249 }
│ │ │ │ +
250
│ │ │ │ +
251 Values::Filtered<Value>
│ │ │ │ +
252 inline Values::filter(const std::function<bool(Key)>& filterFcn) {
│ │ │ │ +
253 return filter<Value>(filterFcn);
│ │ │ │ +
254 }
│ │ │ │
255
│ │ │ │ -
259template<typename T>
│ │ │ │ -
│ │ │ │ -
260class IsLieGroup: public IsGroup<T>, public IsManifold<T> {
│ │ │ │ -
261public:
│ │ │ │ -
262 typedef typename traits<T>::structure_category structure_category_tag;
│ │ │ │ -
263 typedef typename traits<T>::ManifoldType ManifoldType;
│ │ │ │ -
264 typedef typename traits<T>::TangentVector TangentVector;
│ │ │ │ -
265 typedef typename traits<T>::ChartJacobian ChartJacobian;
│ │ │ │ -
266
│ │ │ │ -
267 BOOST_CONCEPT_USAGE(IsLieGroup) {
│ │ │ │ -
268 BOOST_STATIC_ASSERT_MSG(
│ │ │ │ -
269 (boost::is_base_of<lie_group_tag, structure_category_tag>::value),
│ │ │ │ -
270 "This type's trait does not assert it is a Lie group (or derived)");
│ │ │ │ -
271
│ │ │ │ -
272 // group opertations with Jacobians
│ │ │ │ -
273 g = traits<T>::Compose(g, h, Hg, Hh);
│ │ │ │ -
274 g = traits<T>::Between(g, h, Hg, Hh);
│ │ │ │ -
275 g = traits<T>::Inverse(g, Hg);
│ │ │ │ -
276 // log and exp map without Jacobians
│ │ │ │ -
277 g = traits<T>::Expmap(v);
│ │ │ │ -
278 v = traits<T>::Logmap(g);
│ │ │ │ -
279 // log and exponential map with Jacobians
│ │ │ │ -
280 g = traits<T>::Expmap(v, Hg);
│ │ │ │ -
281 v = traits<T>::Logmap(g, Hg);
│ │ │ │ -
282 }
│ │ │ │ -
283private:
│ │ │ │ -
284 T g, h;
│ │ │ │ -
285 TangentVector v;
│ │ │ │ -
286 ChartJacobian Hg, Hh;
│ │ │ │ -
287};
│ │ │ │ -
│ │ │ │ -
288
│ │ │ │ -
297template<class T>
│ │ │ │ -
│ │ │ │ -
298T BCH(const T& X, const T& Y) {
│ │ │ │ -
299 static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.;
│ │ │ │ -
300 T X_Y = bracket(X, Y);
│ │ │ │ -
301 return T(X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, bracket(X, X_Y)));
│ │ │ │ -
302}
│ │ │ │ -
│ │ │ │ -
303
│ │ │ │ -
308template <class T> Matrix wedge(const Vector& x);
│ │ │ │ -
309
│ │ │ │ -
316template <class T>
│ │ │ │ -
│ │ │ │ -
317T expm(const Vector& x, int K=7) {
│ │ │ │ -
318 Matrix xhat = wedge<T>(x);
│ │ │ │ -
319 return T(expm(xhat,K));
│ │ │ │ -
320}
│ │ │ │ -
│ │ │ │ -
321
│ │ │ │ -
326template <typename T>
│ │ │ │ -
│ │ │ │ -
327T interpolate(const T& X, const T& Y, double t,
│ │ │ │ -
328 typename MakeOptionalJacobian<T, T>::type Hx = boost::none,
│ │ │ │ -
329 typename MakeOptionalJacobian<T, T>::type Hy = boost::none) {
│ │ │ │ -
330 if (Hx || Hy) {
│ │ │ │ -
331 typename MakeJacobian<T, T>::type between_H_x, log_H, exp_H, compose_H_x;
│ │ │ │ -
332 const T between =
│ │ │ │ -
333 traits<T>::Between(X, Y, between_H_x); // between_H_y = identity
│ │ │ │ -
334 typename traits<T>::TangentVector delta = traits<T>::Logmap(between, log_H);
│ │ │ │ -
335 const T Delta = traits<T>::Expmap(t * delta, exp_H);
│ │ │ │ -
336 const T result = traits<T>::Compose(
│ │ │ │ -
337 X, Delta, compose_H_x); // compose_H_xinv_y = identity
│ │ │ │ -
338
│ │ │ │ -
339 if (Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x;
│ │ │ │ -
340 if (Hy) *Hy = t * exp_H * log_H;
│ │ │ │ -
341 return result;
│ │ │ │ -
342 }
│ │ │ │ -
343 return traits<T>::Compose(
│ │ │ │ -
344 X, traits<T>::Expmap(t * traits<T>::Logmap(traits<T>::Between(X, Y))));
│ │ │ │ -
345}
│ │ │ │ -
│ │ │ │ -
346
│ │ │ │ -
351template<class T>
│ │ │ │ -
│ │ │ │ -
352class TransformCovariance
│ │ │ │ -
353{
│ │ │ │ -
354private:
│ │ │ │ -
355 typename T::Jacobian adjointMap_;
│ │ │ │ -
356public:
│ │ │ │ -
357 explicit TransformCovariance(const T &X) : adjointMap_{X.AdjointMap()} {}
│ │ │ │ -
358 typename T::Jacobian operator()(const typename T::Jacobian &covariance)
│ │ │ │ -
359 { return adjointMap_ * covariance * adjointMap_.transpose(); }
│ │ │ │ -
360};
│ │ │ │ -
│ │ │ │ -
361
│ │ │ │ -
362} // namespace gtsam
│ │ │ │ -
363
│ │ │ │ -
372#define GTSAM_CONCEPT_LIE_INST(T) template class gtsam::IsLieGroup<T>;
│ │ │ │ -
373#define GTSAM_CONCEPT_LIE_TYPE(T) using _gtsam_IsLieGroup_##T = gtsam::IsLieGroup<T>;
│ │ │ │ -
Group.h
Concept check class for variable types with Group properties.
│ │ │ │ -
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ +
256 template<class ValueType>
│ │ │ │ +
257 Values::Filtered<ValueType>
│ │ │ │ +
258 Values::filter(const std::function<bool(Key)>& filterFcn) {
│ │ │ │ +
259 return Filtered<ValueType>(std::bind(&filterHelper<ValueType>, filterFcn,
│ │ │ │ +
260 std::placeholders::_1), *this);
│ │ │ │ +
261 }
│ │ │ │ +
262
│ │ │ │ +
263 Values::ConstFiltered<Value>
│ │ │ │ +
264 inline Values::filter(const std::function<bool(Key)>& filterFcn) const {
│ │ │ │ +
265 return filter<Value>(filterFcn);
│ │ │ │ +
266 }
│ │ │ │ +
267
│ │ │ │ +
268 template<class ValueType>
│ │ │ │ +
269 Values::ConstFiltered<ValueType>
│ │ │ │ +
270 Values::filter(const std::function<bool(Key)>& filterFcn) const {
│ │ │ │ +
271 return ConstFiltered<ValueType>(std::bind(&filterHelper<ValueType>,
│ │ │ │ +
272 filterFcn, std::placeholders::_1), *this);
│ │ │ │ +
273 }
│ │ │ │ +
274#endif
│ │ │ │ +
275
│ │ │ │ +
276 /* ************************************************************************* */
│ │ │ │ +
277 template<>
│ │ │ │ +
278 inline bool Values::filterHelper<Value>(const std::function<bool(Key)> filter,
│ │ │ │ +
279 const ConstKeyValuePair& key_value) {
│ │ │ │ +
280 // Filter and check the type
│ │ │ │ +
281 return filter(key_value.key);
│ │ │ │ +
282 }
│ │ │ │ +
283
│ │ │ │ +
284 /* ************************************************************************* */
│ │ │ │ +
285
│ │ │ │ +
286 namespace internal {
│ │ │ │ +
287
│ │ │ │ +
288 // Check the type and throw exception if incorrect
│ │ │ │ +
289 // Generic version, partially specialized below for various Eigen Matrix types
│ │ │ │ +
290 template <typename ValueType>
│ │ │ │ +
│ │ │ │ +
291 struct handle {
│ │ │ │ +
292 ValueType operator()(Key j, const Value* const pointer) {
│ │ │ │ +
293 auto ptr = dynamic_cast<const GenericValue<ValueType>*>(pointer);
│ │ │ │ +
294 if (ptr) {
│ │ │ │ +
295 // value returns a const ValueType&, and the return makes a copy !!!!!
│ │ │ │ +
296 return ptr->value();
│ │ │ │ +
297 } else {
│ │ │ │ +
298 throw ValuesIncorrectType(j, typeid(*pointer), typeid(ValueType));
│ │ │ │ +
299 }
│ │ │ │ +
300 }
│ │ │ │ +
301 };
│ │ │ │ +
│ │ │ │ +
302
│ │ │ │ +
303 template <typename MatrixType, bool isDynamic>
│ │ │ │ +
304 struct handle_matrix;
│ │ │ │ +
305
│ │ │ │ +
306 // Handle dynamic matrices
│ │ │ │ +
307 template <int M, int N>
│ │ │ │ +
│ │ │ │ +
308 struct handle_matrix<Eigen::Matrix<double, M, N>, true> {
│ │ │ │ +
309 inline Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
│ │ │ │ +
310 auto ptr = dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>*>(pointer);
│ │ │ │ +
311 if (ptr) {
│ │ │ │ +
312 // value returns a const Matrix&, and the return makes a copy !!!!!
│ │ │ │ +
313 return ptr->value();
│ │ │ │ +
314 } else {
│ │ │ │ +
315 // If a fixed matrix was stored, we end up here as well.
│ │ │ │ +
316 throw ValuesIncorrectType(j, typeid(*pointer), typeid(Eigen::Matrix<double, M, N>));
│ │ │ │ +
317 }
│ │ │ │ +
318 }
│ │ │ │ +
319 };
│ │ │ │ +
│ │ │ │ +
320
│ │ │ │ +
321 // Handle fixed matrices
│ │ │ │ +
322 template <int M, int N>
│ │ │ │ +
│ │ │ │ +
323 struct handle_matrix<Eigen::Matrix<double, M, N>, false> {
│ │ │ │ +
324 inline Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
│ │ │ │ +
325 auto ptr = dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>*>(pointer);
│ │ │ │ +
326 if (ptr) {
│ │ │ │ +
327 // value returns a const MatrixMN&, and the return makes a copy !!!!!
│ │ │ │ +
328 return ptr->value();
│ │ │ │ +
329 } else {
│ │ │ │ +
330 Matrix A;
│ │ │ │ +
331 // Check if a dynamic matrix was stored
│ │ │ │ +
332 auto ptr = dynamic_cast<const GenericValue<Eigen::MatrixXd>*>(pointer);
│ │ │ │ +
333 if (ptr) {
│ │ │ │ +
334 A = ptr->value();
│ │ │ │ +
335 } else {
│ │ │ │ +
336 // Or a dynamic vector
│ │ │ │ +
337 A = handle_matrix<Eigen::VectorXd, true>()(j, pointer); // will throw if not....
│ │ │ │ +
338 }
│ │ │ │ +
339 // Yes: check size, and throw if not a match
│ │ │ │ +
340 if (A.rows() != M || A.cols() != N)
│ │ │ │ +
341 throw NoMatchFoundForFixed(M, N, A.rows(), A.cols());
│ │ │ │ +
342 else
│ │ │ │ +
343 return A; // copy but not malloc
│ │ │ │ +
344 }
│ │ │ │ +
345 }
│ │ │ │ +
346 };
│ │ │ │ +
│ │ │ │ +
347
│ │ │ │ +
348 // Handle matrices
│ │ │ │ +
349 template <int M, int N>
│ │ │ │ +
│ │ │ │ +
350 struct handle<Eigen::Matrix<double, M, N>> {
│ │ │ │ +
351 Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
│ │ │ │ +
352 return handle_matrix<Eigen::Matrix<double, M, N>,
│ │ │ │ +
353 (M == Eigen::Dynamic || N == Eigen::Dynamic)>()(j, pointer);
│ │ │ │ +
354 }
│ │ │ │ +
355 };
│ │ │ │ +
│ │ │ │ +
356
│ │ │ │ +
357 } // internal
│ │ │ │ +
358
│ │ │ │ +
359 /* ************************************************************************* */
│ │ │ │ +
360 template <typename ValueType>
│ │ │ │ +
│ │ │ │ +
361 const ValueType Values::at(Key j) const {
│ │ │ │ +
362 // Find the item
│ │ │ │ +
363 KeyValueMap::const_iterator item = values_.find(j);
│ │ │ │ +
364
│ │ │ │ +
365 // Throw exception if it does not exist
│ │ │ │ +
366 if (item == values_.end()) throw ValuesKeyDoesNotExist("at", j);
│ │ │ │ +
367
│ │ │ │ +
368 // Check the type and throw exception if incorrect
│ │ │ │ +
369 // h() split in two lines to avoid internal compiler error (MSVC2017)
│ │ │ │ +
370 auto h = internal::handle<ValueType>();
│ │ │ │ +
371 return h(j, item->second);
│ │ │ │ +
372 }
│ │ │ │ +
│ │ │ │ +
373
│ │ │ │ +
374 /* ************************************************************************* */
│ │ │ │ +
375 template<typename ValueType>
│ │ │ │ +
│ │ │ │ +
376 boost::optional<const ValueType&> Values::exists(Key j) const {
│ │ │ │ +
377 // Find the item
│ │ │ │ +
378 KeyValueMap::const_iterator item = values_.find(j);
│ │ │ │ +
379
│ │ │ │ +
380 if(item != values_.end()) {
│ │ │ │ +
381 // dynamic cast the type and throw exception if incorrect
│ │ │ │ +
382 auto ptr = dynamic_cast<const GenericValue<ValueType>*>(item->second);
│ │ │ │ +
383 if (ptr) {
│ │ │ │ +
384 return ptr->value();
│ │ │ │ +
385 } else {
│ │ │ │ +
386 // NOTE(abe): clang warns about potential side effects if done in typeid
│ │ │ │ +
387 const Value* value = item->second;
│ │ │ │ +
388 throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType));
│ │ │ │ +
389 }
│ │ │ │ +
390 } else {
│ │ │ │ +
391 return boost::none;
│ │ │ │ +
392 }
│ │ │ │ +
393 }
│ │ │ │ +
│ │ │ │ +
394
│ │ │ │ +
395 /* ************************************************************************* */
│ │ │ │ +
396
│ │ │ │ +
397 // insert a templated value
│ │ │ │ +
398 template<typename ValueType>
│ │ │ │ +
│ │ │ │ +
399 void Values::insert(Key j, const ValueType& val) {
│ │ │ │ +
400 insert(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
│ │ │ │ +
401 }
│ │ │ │ +
│ │ │ │ +
402
│ │ │ │ +
403 // update with templated value
│ │ │ │ +
404 template <typename ValueType>
│ │ │ │ +
405 void Values::update(Key j, const ValueType& val) {
│ │ │ │ +
406 update(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
│ │ │ │ +
407 }
│ │ │ │ +
408
│ │ │ │ +
409 // insert_or_assign with templated value
│ │ │ │ +
410 template <typename ValueType>
│ │ │ │ +
│ │ │ │ +
411 void Values::insert_or_assign(Key j, const ValueType& val) {
│ │ │ │ +
412 insert_or_assign(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
│ │ │ │ +
413 }
│ │ │ │ +
│ │ │ │ +
414
│ │ │ │ +
415}
│ │ │ │ +
Values.h
A non-templated config holding any types of Manifold-group elements.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::expm
T expm(const Vector &x, int K=7)
Exponential map given exponential coordinates class T needs a wedge<> function and a constructor from...
Definition Lie.h:317
│ │ │ │ -
gtsam::logmap_default
Vector logmap_default(const Class &l0, const Class &lp)
Log map centered at l0, s.t.
Definition Lie.h:246
│ │ │ │ -
gtsam::between_default
Class between_default(const Class &l1, const Class &l2)
These core global functions can be specialized by new Lie types for better performance.
Definition Lie.h:240
│ │ │ │ -
gtsam::interpolate
T interpolate(const T &X, const T &Y, double t, typename MakeOptionalJacobian< T, T >::type Hx=boost::none, typename MakeOptionalJacobian< T, T >::type Hy=boost::none)
Linear interpolation between X and Y by coefficient t.
Definition Lie.h:327
│ │ │ │ -
gtsam::BCH
T BCH(const T &X, const T &Y)
Three term approximation of the Baker-Campbell-Hausdorff formula In non-commutative Lie groups,...
Definition Lie.h:298
│ │ │ │ -
gtsam::expmap_default
Class expmap_default(const Class &t, const Vector &d)
Exponential map centered at l0, s.t.
Definition Lie.h:252
│ │ │ │ -
gtsam::wedge
Matrix wedge(const Vector &x)
Declaration of wedge (see Murray94book) used to convert from n exponential coordinates to n*n element...
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::group_tag
tag to assert a type is a group
Definition Group.h:34
│ │ │ │ -
gtsam::multiplicative_group_tag
Group operator syntax flavors.
Definition Group.h:37
│ │ │ │ -
gtsam::IsGroup
Group Concept.
Definition Group.h:46
│ │ │ │ -
gtsam::LieGroup
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition Lie.h:37
│ │ │ │ -
gtsam::LieGroup::Retract
static Class Retract(const TangentVector &v, ChartJacobian H)
Retract at origin with optional derivative.
Definition Lie.h:121
│ │ │ │ -
gtsam::LieGroup::localCoordinates
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g
Definition Lie.h:136
│ │ │ │ -
gtsam::LieGroup::expmap
Class expmap(const TangentVector &v, ChartJacobian H1, ChartJacobian H2=boost::none) const
expmap with optional derivatives
Definition Lie.h:89
│ │ │ │ -
gtsam::LieGroup::localCoordinates
TangentVector localCoordinates(const Class &g, ChartJacobian H1, ChartJacobian H2=boost::none) const
localCoordinates with optional derivatives
Definition Lie.h:152
│ │ │ │ -
gtsam::LieGroup::logmap
TangentVector logmap(const Class &g) const
logmap as required by manifold concept Applies logarithmic map to group element that takes *this to g
Definition Lie.h:84
│ │ │ │ -
gtsam::LieGroup::logmap
TangentVector logmap(const Class &g, ChartJacobian H1, ChartJacobian H2=boost::none) const
logmap with optional derivatives
Definition Lie.h:100
│ │ │ │ -
gtsam::LieGroup::LocalCoordinates
static TangentVector LocalCoordinates(const Class &g)
LocalCoordinates at origin: possible in Lie group because it has an identity.
Definition Lie.h:116
│ │ │ │ -
gtsam::LieGroup::retract
Class retract(const TangentVector &v, ChartJacobian H1, ChartJacobian H2=boost::none) const
retract with optional derivatives
Definition Lie.h:141
│ │ │ │ -
gtsam::LieGroup::LocalCoordinates
static TangentVector LocalCoordinates(const Class &g, ChartJacobian H)
LocalCoordinates at origin with optional derivative.
Definition Lie.h:126
│ │ │ │ -
gtsam::LieGroup::retract
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition Lie.h:131
│ │ │ │ -
gtsam::LieGroup::expmap
Class expmap(const TangentVector &v) const
expmap as required by manifold concept Applies exponential map to v and composes with *this
Definition Lie.h:78
│ │ │ │ -
gtsam::LieGroup::Retract
static Class Retract(const TangentVector &v)
Retract at origin: possible in Lie group because it has an identity.
Definition Lie.h:111
│ │ │ │ -
gtsam::lie_group_tag
tag to assert a type is a Lie group
Definition Lie.h:164
│ │ │ │ -
gtsam::internal::LieGroupTraits
A helper class that implements the traits interface for GTSAM lie groups.
Definition Lie.h:174
│ │ │ │ -
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition Lie.h:229
│ │ │ │ -
gtsam::IsLieGroup
Lie Group Concept.
Definition Lie.h:260
│ │ │ │ -
gtsam::TransformCovariance
Functor for transforming covariance of T.
Definition Lie.h:353
│ │ │ │ -
gtsam::manifold_tag
tag to assert a type is a manifold
Definition Manifold.h:33
│ │ │ │ -
gtsam::internal::GetDimensionImpl
Extra manifold traits for fixed-dimension types.
Definition Manifold.h:75
│ │ │ │ -
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::GenericValue
Wraps any type T so it can play as a Value.
Definition GenericValue.h:47
│ │ │ │ +
gtsam::GenericValue::value
const T & value() const
Return a constant value.
Definition GenericValue.h:67
│ │ │ │ +
gtsam::Value
This is the base class for any type to be stored in Values.
Definition Value.h:37
│ │ │ │ +
gtsam::_ValuesKeyValuePair
Definition Values-inl.h:36
│ │ │ │ +
gtsam::_ValuesKeyValuePair::key
const Key key
The key.
Definition Values-inl.h:37
│ │ │ │ +
gtsam::_ValuesKeyValuePair::value
ValueType & value
The value.
Definition Values-inl.h:38
│ │ │ │ +
gtsam::_ValuesConstKeyValuePair
Definition Values-inl.h:45
│ │ │ │ +
gtsam::_ValuesConstKeyValuePair::key
const Key key
The key.
Definition Values-inl.h:46
│ │ │ │ +
gtsam::_ValuesConstKeyValuePair::value
const ValueType & value
The value.
Definition Values-inl.h:47
│ │ │ │ +
gtsam::ValuesCastHelper
Definition Values-inl.h:62
│ │ │ │ +
gtsam::internal::handle
Definition Values-inl.h:291
│ │ │ │ +
gtsam::internal::handle_matrix
Definition Values-inl.h:304
│ │ │ │ +
gtsam::Values::update
void update(Key j, const Value &val)
single element change of existing element
Definition Values.cpp:180
│ │ │ │ +
gtsam::Values::extract
std::map< Key, ValueType > extract(const std::function< bool(Key)> &filterFcn=&_truePredicate< Key >) const
Extract a subset of values of the given type ValueType.
Definition Values-inl.h:94
│ │ │ │ +
gtsam::Values::at
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
│ │ │ │ +
gtsam::Values::insert
void insert(Key j, const Value &val)
Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present.
Definition Values.cpp:157
│ │ │ │ +
gtsam::Values::insert_or_assign
void insert_or_assign(Key j, const Value &val)
If key j exists, update value, else perform an insert.
Definition Values.cpp:203
│ │ │ │ +
gtsam::Values::Values
Values()=default
Default constructor creates an empty Values class.
│ │ │ │ +
gtsam::Values::exists
bool exists(Key j) const
Check if a value exists with key j.
Definition Values.cpp:94
│ │ │ │ +
gtsam::Values::KeyValuePair
A key-value pair, which you get by dereferencing iterators.
Definition Values.h:93
│ │ │ │ +
gtsam::Values::ConstKeyValuePair
A key-value pair, which you get by dereferencing iterators.
Definition Values.h:101
│ │ │ │ +
gtsam::ValuesKeyDoesNotExist
Definition Values.h:475
│ │ │ │ +
gtsam::ValuesIncorrectType
Definition Values.h:498
│ │ │ │ +
gtsam::NoMatchFoundForFixed
Definition Values.h:542
│ │ │ │ +
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,440 +1,525 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Lie.h │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +Values-inl.h │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -24#pragma once │ │ │ │ │ -25 │ │ │ │ │ -26#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ -27#include <_g_t_s_a_m_/_b_a_s_e_/_G_r_o_u_p_._h> │ │ │ │ │ -28 │ │ │ │ │ -29namespace _g_t_s_a_m { │ │ │ │ │ +25#pragma once │ │ │ │ │ +26 │ │ │ │ │ +27#include │ │ │ │ │ +28#include │ │ │ │ │ +29#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_V_a_l_u_e_s_._h> │ │ │ │ │ 30 │ │ │ │ │ -36template │ │ │ │ │ -_3_7struct _L_i_e_G_r_o_u_p { │ │ │ │ │ -38 │ │ │ │ │ -39 enum { dimension = N }; │ │ │ │ │ -40 typedef _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_N_,_ _N_> ChartJacobian; │ │ │ │ │ -41 typedef Eigen::Matrix Jacobian; │ │ │ │ │ -42 typedef Eigen::Matrix TangentVector; │ │ │ │ │ -43 │ │ │ │ │ -44 const Class & derived() const { │ │ │ │ │ -45 return static_cast(*this); │ │ │ │ │ -46 } │ │ │ │ │ -47 │ │ │ │ │ -48 Class compose(const Class& g) const { │ │ │ │ │ -49 return derived() * g; │ │ │ │ │ -50 } │ │ │ │ │ -51 │ │ │ │ │ -52 Class between(const Class& g) const { │ │ │ │ │ -53 return derived().inverse() * g; │ │ │ │ │ +31namespace _g_t_s_a_m { │ │ │ │ │ +32 │ │ │ │ │ +33 │ │ │ │ │ +34 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +35 template │ │ │ │ │ +_3_6 struct ___V_a_l_u_e_s_K_e_y_V_a_l_u_e_P_a_i_r { │ │ │ │ │ +_3_7 const _K_e_y _k_e_y; │ │ │ │ │ +_3_8 ValueType& _v_a_l_u_e; │ │ │ │ │ +39 │ │ │ │ │ +40 ___V_a_l_u_e_s_K_e_y_V_a_l_u_e_P_a_i_r(_K_e_y _key, ValueType& _value) : _k_e_y(_key), _v_a_l_u_e(_value) │ │ │ │ │ +{} │ │ │ │ │ +41 }; │ │ │ │ │ +42 │ │ │ │ │ +43 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +44 template │ │ │ │ │ +_4_5 struct ___V_a_l_u_e_s_C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r { │ │ │ │ │ +_4_6 const _K_e_y _k_e_y; │ │ │ │ │ +_4_7 const ValueType& _v_a_l_u_e; │ │ │ │ │ +48 │ │ │ │ │ +49 ___V_a_l_u_e_s_C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r(_K_e_y _key, const ValueType& _value) : │ │ │ │ │ +50 _k_e_y(_key), _v_a_l_u_e(_value) { │ │ │ │ │ +51 } │ │ │ │ │ +52 ___V_a_l_u_e_s_C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r(const ___V_a_l_u_e_s_K_e_y_V_a_l_u_e_P_a_i_r_<_V_a_l_u_e_T_y_p_e_>& rhs) : │ │ │ │ │ +53 _k_e_y(rhs._k_e_y), _v_a_l_u_e(rhs._v_a_l_u_e) { │ │ │ │ │ 54 } │ │ │ │ │ -55 │ │ │ │ │ -56 Class compose(const Class& g, ChartJacobian H1, │ │ │ │ │ -57 ChartJacobian H2 = boost::none) const { │ │ │ │ │ -58 if (H1) *H1 = g.inverse().AdjointMap(); │ │ │ │ │ -59 if (H2) *H2 = Eigen::Matrix::Identity(); │ │ │ │ │ -60 return derived() * g; │ │ │ │ │ -61 } │ │ │ │ │ -62 │ │ │ │ │ -63 Class between(const Class& g, ChartJacobian H1, │ │ │ │ │ -64 ChartJacobian H2 = boost::none) const { │ │ │ │ │ -65 Class result = derived().inverse() * g; │ │ │ │ │ -66 if (H1) *H1 = - result.inverse().AdjointMap(); │ │ │ │ │ -67 if (H2) *H2 = Eigen::Matrix::Identity(); │ │ │ │ │ -68 return result; │ │ │ │ │ -69 } │ │ │ │ │ -70 │ │ │ │ │ -71 Class inverse(ChartJacobian H) const { │ │ │ │ │ -72 if (H) *H = - derived().AdjointMap(); │ │ │ │ │ -73 return derived().inverse(); │ │ │ │ │ -74 } │ │ │ │ │ -75 │ │ │ │ │ -_7_8 Class _e_x_p_m_a_p(const TangentVector& v) const { │ │ │ │ │ -79 return compose(Class::Expmap(v)); │ │ │ │ │ -80 } │ │ │ │ │ -81 │ │ │ │ │ -_8_4 TangentVector _l_o_g_m_a_p(const Class& g) const { │ │ │ │ │ -85 return Class::Logmap(between(g)); │ │ │ │ │ -86 } │ │ │ │ │ -87 │ │ │ │ │ -_8_9 Class _e_x_p_m_a_p(const TangentVector& v, // │ │ │ │ │ -90 ChartJacobian H1, ChartJacobian H2 = boost::none) const { │ │ │ │ │ -91 Jacobian D_g_v; │ │ │ │ │ -92 Class g = Class::Expmap(v,H2 ? &D_g_v : 0); │ │ │ │ │ -93 Class h = compose(g); // derivatives inlined below │ │ │ │ │ -94 if (H1) *H1 = g.inverse().AdjointMap(); │ │ │ │ │ -95 if (H2) *H2 = D_g_v; │ │ │ │ │ -96 return h; │ │ │ │ │ -97 } │ │ │ │ │ -98 │ │ │ │ │ -_1_0_0 TangentVector _l_o_g_m_a_p(const Class& g, // │ │ │ │ │ -101 ChartJacobian H1, ChartJacobian H2 = boost::none) const { │ │ │ │ │ -102 Class h = between(g); // derivatives inlined below │ │ │ │ │ -103 Jacobian D_v_h; │ │ │ │ │ -104 TangentVector v = Class::Logmap(h, (H1 || H2) ? &D_v_h : 0); │ │ │ │ │ -105 if (H1) *H1 = - D_v_h * h.inverse().AdjointMap(); │ │ │ │ │ -106 if (H2) *H2 = D_v_h; │ │ │ │ │ -107 return v; │ │ │ │ │ -108 } │ │ │ │ │ -109 │ │ │ │ │ -_1_1_1 static Class _R_e_t_r_a_c_t(const TangentVector& v) { │ │ │ │ │ -112 return Class::ChartAtOrigin::Retract(v); │ │ │ │ │ -113 } │ │ │ │ │ -114 │ │ │ │ │ -_1_1_6 static TangentVector _L_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(const Class& g) { │ │ │ │ │ -117 return Class::ChartAtOrigin::Local(g); │ │ │ │ │ -118 } │ │ │ │ │ +55 }; │ │ │ │ │ +56 │ │ │ │ │ +57 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +58 │ │ │ │ │ +59 // Cast helpers for making _Values[Const]KeyValuePair's from Values:: │ │ │ │ │ +[Const]KeyValuePair │ │ │ │ │ +60 // need to use a struct here for later partial specialization │ │ │ │ │ +61 template │ │ │ │ │ +_6_2 struct _V_a_l_u_e_s_C_a_s_t_H_e_l_p_e_r { │ │ │ │ │ +63 static CastedKeyValuePairType cast(KeyValuePairType key_value) { │ │ │ │ │ +64 // Static cast because we already checked the type during filtering │ │ │ │ │ +65 return CastedKeyValuePairType(key_value.key, │ │ │ │ │ +66 const_cast<_G_e_n_e_r_i_c_V_a_l_u_e_<_V_a_l_u_e_T_y_p_e_>&>(static_cast&>(key_value.value))._v_a_l_u_e()); │ │ │ │ │ +68 } │ │ │ │ │ +69 }; │ │ │ │ │ +70 // partial specialized version for ValueType == Value │ │ │ │ │ +71 template │ │ │ │ │ +_7_2 struct _V_a_l_u_e_s_C_a_s_t_H_e_l_p_e_r<_V_a_l_u_e, CastedKeyValuePairType, KeyValuePairType> { │ │ │ │ │ +73 static CastedKeyValuePairType cast(KeyValuePairType key_value) { │ │ │ │ │ +74 // Static cast because we already checked the type during filtering │ │ │ │ │ +75 // in this case the casted and keyvalue pair are essentially the same type │ │ │ │ │ +76 // (key, Value&) so perhaps this could be done with just a cast of the │ │ │ │ │ +key_value? │ │ │ │ │ +77 return CastedKeyValuePairType(key_value.key, key_value.value); │ │ │ │ │ +78 } │ │ │ │ │ +79 }; │ │ │ │ │ +80 // partial specialized version for ValueType == Value │ │ │ │ │ +81 template │ │ │ │ │ +_8_2 struct _V_a_l_u_e_s_C_a_s_t_H_e_l_p_e_r { │ │ │ │ │ +83 static CastedKeyValuePairType cast(KeyValuePairType key_value) { │ │ │ │ │ +84 // Static cast because we already checked the type during filtering │ │ │ │ │ +85 // in this case the casted and keyvalue pair are essentially the same type │ │ │ │ │ +86 // (key, Value&) so perhaps this could be done with just a cast of the │ │ │ │ │ +key_value? │ │ │ │ │ +87 return CastedKeyValuePairType(key_value.key, key_value.value); │ │ │ │ │ +88 } │ │ │ │ │ +89 }; │ │ │ │ │ +90 │ │ │ │ │ +91/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +92 template │ │ │ │ │ +93 std::map │ │ │ │ │ +_9_4 _V_a_l_u_e_s_:_:_e_x_t_r_a_c_t(const std::function& filterFcn) const { │ │ │ │ │ +95 std::map result; │ │ │ │ │ +96 for (const auto& key_value : values_) { │ │ │ │ │ +97 // Check if key matches │ │ │ │ │ +98 if (filterFcn(key_value.first)) { │ │ │ │ │ +99 // Check if type matches (typically does as symbols matched with types) │ │ │ │ │ +100 if (auto t = │ │ │ │ │ +101 dynamic_cast*>(key_value.second)) │ │ │ │ │ +102 result[key_value.first] = t->value(); │ │ │ │ │ +103 } │ │ │ │ │ +104 } │ │ │ │ │ +105 return result; │ │ │ │ │ +106 } │ │ │ │ │ +107 │ │ │ │ │ +108/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +109#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ +110#include │ │ │ │ │ +111 │ │ │ │ │ +112 template │ │ │ │ │ +113 class Values::Filtered { │ │ │ │ │ +114 public: │ │ │ │ │ +116 typedef ___V_a_l_u_e_s_K_e_y_V_a_l_u_e_P_a_i_r_<_V_a_l_u_e_T_y_p_e_> KeyValuePair; │ │ │ │ │ +117 typedef ___V_a_l_u_e_s_C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r_<_V_a_l_u_e_T_y_p_e_> ConstKeyValuePair; │ │ │ │ │ +118 typedef KeyValuePair value_type; │ │ │ │ │ 119 │ │ │ │ │ -_1_2_1 static Class _R_e_t_r_a_c_t(const TangentVector& v, ChartJacobian H) { │ │ │ │ │ -122 return Class::ChartAtOrigin::Retract(v,H); │ │ │ │ │ -123 } │ │ │ │ │ -124 │ │ │ │ │ -_1_2_6 static TangentVector _L_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(const Class& g, ChartJacobian H) { │ │ │ │ │ -127 return Class::ChartAtOrigin::Local(g,H); │ │ │ │ │ -128 } │ │ │ │ │ +120 typedef │ │ │ │ │ +121 boost::transform_iterator< │ │ │ │ │ +122 KeyValuePair(*)(_V_a_l_u_e_s_:_:_K_e_y_V_a_l_u_e_P_a_i_r), │ │ │ │ │ +123 boost::filter_iterator< │ │ │ │ │ +124 std::function, │ │ │ │ │ +125 Values::iterator> > │ │ │ │ │ +126 iterator; │ │ │ │ │ +127 │ │ │ │ │ +128 typedef iterator const_iterator; │ │ │ │ │ 129 │ │ │ │ │ -_1_3_1 Class _r_e_t_r_a_c_t(const TangentVector& v) const { │ │ │ │ │ -132 return compose(Class::ChartAtOrigin::Retract(v)); │ │ │ │ │ -133 } │ │ │ │ │ -134 │ │ │ │ │ -_1_3_6 TangentVector _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(const Class& g) const { │ │ │ │ │ -137 return Class::ChartAtOrigin::Local(between(g)); │ │ │ │ │ -138 } │ │ │ │ │ -139 │ │ │ │ │ -_1_4_1 Class _r_e_t_r_a_c_t(const TangentVector& v, // │ │ │ │ │ -142 ChartJacobian H1, ChartJacobian H2 = boost::none) const { │ │ │ │ │ -143 Jacobian D_g_v; │ │ │ │ │ -144 Class g = Class::ChartAtOrigin::Retract(v, H2 ? &D_g_v : 0); │ │ │ │ │ -145 Class h = compose(g); // derivatives inlined below │ │ │ │ │ -146 if (H1) *H1 = g.inverse().AdjointMap(); │ │ │ │ │ -147 if (H2) *H2 = D_g_v; │ │ │ │ │ -148 return h; │ │ │ │ │ -149 } │ │ │ │ │ -150 │ │ │ │ │ -_1_5_2 TangentVector _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(const Class& g, // │ │ │ │ │ -153 ChartJacobian H1, ChartJacobian H2 = boost::none) const { │ │ │ │ │ -154 Class h = between(g); // derivatives inlined below │ │ │ │ │ -155 Jacobian D_v_h; │ │ │ │ │ -156 TangentVector v = Class::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0); │ │ │ │ │ -157 if (H1) *H1 = - D_v_h * h.inverse().AdjointMap(); │ │ │ │ │ -158 if (H2) *H2 = D_v_h; │ │ │ │ │ -159 return v; │ │ │ │ │ -160 } │ │ │ │ │ -161}; │ │ │ │ │ -162 │ │ │ │ │ -_1_6_4struct _l_i_e___g_r_o_u_p___t_a_g: public _m_a_n_i_f_o_l_d___t_a_g, public _g_r_o_u_p___t_a_g {}; │ │ │ │ │ -165 │ │ │ │ │ -166namespace internal { │ │ │ │ │ -167 │ │ │ │ │ -173template │ │ │ │ │ -_1_7_4struct _L_i_e_G_r_o_u_p_T_r_a_i_t_s: _G_e_t_D_i_m_e_n_s_i_o_n_I_m_p_l { │ │ │ │ │ -175 typedef _l_i_e___g_r_o_u_p___t_a_g _s_t_r_u_c_t_u_r_e___c_a_t_e_g_o_r_y; │ │ │ │ │ -176 │ │ │ │ │ -179 typedef _m_u_l_t_i_p_l_i_c_a_t_i_v_e___g_r_o_u_p___t_a_g _g_r_o_u_p___f_l_a_v_o_r; │ │ │ │ │ -180 static Class Identity() { return Class::Identity();} │ │ │ │ │ -182 │ │ │ │ │ -185 typedef Class ManifoldType; │ │ │ │ │ -186 enum { dimension = Class::dimension }; │ │ │ │ │ -187 typedef Eigen::Matrix TangentVector; │ │ │ │ │ -188 typedef _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_d_i_m_e_n_s_i_o_n_,_ _d_i_m_e_n_s_i_o_n_> ChartJacobian; │ │ │ │ │ -189 │ │ │ │ │ -190 static TangentVector Local(const Class& origin, const Class& other, │ │ │ │ │ -191 ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { │ │ │ │ │ -192 return origin.localCoordinates(other, Horigin, Hother); │ │ │ │ │ -193 } │ │ │ │ │ +130 typedef │ │ │ │ │ +131 boost::transform_iterator< │ │ │ │ │ +132 ConstKeyValuePair(*)(_V_a_l_u_e_s_:_:_C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r), │ │ │ │ │ +133 boost::filter_iterator< │ │ │ │ │ +134 std::function, │ │ │ │ │ +135 Values::const_iterator> > │ │ │ │ │ +136 const_const_iterator; │ │ │ │ │ +137 │ │ │ │ │ +138 iterator begin() { return begin_; } │ │ │ │ │ +139 iterator end() { return end_; } │ │ │ │ │ +140 const_iterator begin() const { return begin_; } │ │ │ │ │ +141 const_iterator end() const { return end_; } │ │ │ │ │ +142 const_const_iterator beginConst() const { return constBegin_; } │ │ │ │ │ +143 const_const_iterator endConst() const { return constEnd_; } │ │ │ │ │ +144 │ │ │ │ │ +146 size_t size() const { │ │ │ │ │ +147 size_t i = 0; │ │ │ │ │ +148 for (const_const_iterator it = beginConst(); it != endConst(); ++it) │ │ │ │ │ +149 ++i; │ │ │ │ │ +150 return i; │ │ │ │ │ +151 } │ │ │ │ │ +152 │ │ │ │ │ +153 private: │ │ │ │ │ +154 Filtered( │ │ │ │ │ +155 const std::function& filter, │ │ │ │ │ +156 _V_a_l_u_e_s& values) : │ │ │ │ │ +157 begin_( │ │ │ │ │ +158 boost::make_transform_iterator( │ │ │ │ │ +159 boost::make_filter_iterator(filter, values.begin(), values.end()), │ │ │ │ │ +160 &ValuesCastHelper::cast)), │ │ │ │ │ +end_( │ │ │ │ │ +161 boost::make_transform_iterator( │ │ │ │ │ +162 boost::make_filter_iterator(filter, values.end(), values.end()), │ │ │ │ │ +163 &ValuesCastHelper::cast)), │ │ │ │ │ +constBegin_( │ │ │ │ │ +164 boost::make_transform_iterator( │ │ │ │ │ +165 boost::make_filter_iterator(filter, │ │ │ │ │ +166 values._begin(), │ │ │ │ │ +167 values._end()), │ │ │ │ │ +168 &ValuesCastHelper::cast)), constEnd_( │ │ │ │ │ +170 boost::make_transform_iterator( │ │ │ │ │ +171 boost::make_filter_iterator(filter, │ │ │ │ │ +172 values._end(), │ │ │ │ │ +173 values._end()), │ │ │ │ │ +174 &ValuesCastHelper::cast)) { │ │ │ │ │ +176 } │ │ │ │ │ +177 │ │ │ │ │ +178 friend class _V_a_l_u_e_s; │ │ │ │ │ +179 iterator begin_; │ │ │ │ │ +180 iterator end_; │ │ │ │ │ +181 const_const_iterator constBegin_; │ │ │ │ │ +182 const_const_iterator constEnd_; │ │ │ │ │ +183 }; │ │ │ │ │ +184 │ │ │ │ │ +185 template │ │ │ │ │ +186 class Values::ConstFiltered { │ │ │ │ │ +187 public: │ │ │ │ │ +189 typedef _ValuesConstKeyValuePair KeyValuePair; │ │ │ │ │ +190 typedef KeyValuePair value_type; │ │ │ │ │ +191 │ │ │ │ │ +192 typedef typename Filtered::const_const_iterator iterator; │ │ │ │ │ +193 typedef typename Filtered::const_const_iterator const_iterator; │ │ │ │ │ 194 │ │ │ │ │ -195 static Class Retract(const Class& origin, const TangentVector& v, │ │ │ │ │ -196 ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) { │ │ │ │ │ -197 return origin.retract(v, Horigin, Hv); │ │ │ │ │ -198 } │ │ │ │ │ -200 │ │ │ │ │ -203 static TangentVector Logmap(const Class& m, ChartJacobian Hm = boost::none) │ │ │ │ │ -{ │ │ │ │ │ -204 return Class::Logmap(m, Hm); │ │ │ │ │ -205 } │ │ │ │ │ -206 │ │ │ │ │ -207 static Class Expmap(const TangentVector& v, ChartJacobian Hv = boost::none) │ │ │ │ │ -{ │ │ │ │ │ -208 return Class::Expmap(v, Hv); │ │ │ │ │ -209 } │ │ │ │ │ -210 │ │ │ │ │ -211 static Class Compose(const Class& m1, const Class& m2, // │ │ │ │ │ -212 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { │ │ │ │ │ -213 return m1.compose(m2, H1, H2); │ │ │ │ │ -214 } │ │ │ │ │ -215 │ │ │ │ │ -216 static Class Between(const Class& m1, const Class& m2, // │ │ │ │ │ -217 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { │ │ │ │ │ -218 return m1.between(m2, H1, H2); │ │ │ │ │ -219 } │ │ │ │ │ -220 │ │ │ │ │ -221 static Class Inverse(const Class& m, // │ │ │ │ │ -222 ChartJacobian H = boost::none) { │ │ │ │ │ -223 return m.inverse(H); │ │ │ │ │ -224 } │ │ │ │ │ -226}; │ │ │ │ │ -227 │ │ │ │ │ -_2_2_9template struct _L_i_e_G_r_o_u_p: _L_i_e_G_r_o_u_p_T_r_a_i_t_s, │ │ │ │ │ -_T_e_s_t_a_b_l_e {}; │ │ │ │ │ -230 │ │ │ │ │ -231} // \ namepsace internal │ │ │ │ │ -232 │ │ │ │ │ -239template │ │ │ │ │ -_2_4_0inline Class _b_e_t_w_e_e_n___d_e_f_a_u_l_t(const Class& l1, const Class& l2) { │ │ │ │ │ -241 return l1.inverse().compose(l2); │ │ │ │ │ -242} │ │ │ │ │ -243 │ │ │ │ │ -245template │ │ │ │ │ -_2_4_6inline Vector _l_o_g_m_a_p___d_e_f_a_u_l_t(const Class& l0, const Class& lp) { │ │ │ │ │ -247 return Class::Logmap(l0.between(lp)); │ │ │ │ │ -248} │ │ │ │ │ -249 │ │ │ │ │ -251template │ │ │ │ │ -_2_5_2inline Class _e_x_p_m_a_p___d_e_f_a_u_l_t(const Class& t, const Vector& d) { │ │ │ │ │ -253 return t.compose(Class::Expmap(d)); │ │ │ │ │ -254} │ │ │ │ │ +196 ConstFiltered(const Filtered& rhs) : │ │ │ │ │ +197 begin_(rhs.beginConst()), │ │ │ │ │ +198 end_(rhs.endConst()) {} │ │ │ │ │ +199 │ │ │ │ │ +200 iterator begin() { return begin_; } │ │ │ │ │ +201 iterator end() { return end_; } │ │ │ │ │ +202 const_iterator begin() const { return begin_; } │ │ │ │ │ +203 const_iterator end() const { return end_; } │ │ │ │ │ +204 │ │ │ │ │ +206 size_t size() const { │ │ │ │ │ +207 size_t i = 0; │ │ │ │ │ +208 for (const_iterator it = begin(); it != end(); ++it) │ │ │ │ │ +209 ++i; │ │ │ │ │ +210 return i; │ │ │ │ │ +211 } │ │ │ │ │ +212 │ │ │ │ │ +213 FastList keys() const { │ │ │ │ │ +214 FastList result; │ │ │ │ │ +215 for(const_iterator it = begin(); it != end(); ++it) │ │ │ │ │ +216 result.push_back(it->key); │ │ │ │ │ +217 return result; │ │ │ │ │ +218 } │ │ │ │ │ +219 │ │ │ │ │ +220 private: │ │ │ │ │ +221 friend class _V_a_l_u_e_s; │ │ │ │ │ +222 const_iterator begin_; │ │ │ │ │ +223 const_iterator end_; │ │ │ │ │ +224 ConstFiltered( │ │ │ │ │ +225 const std::function& filter, │ │ │ │ │ +226 const _V_a_l_u_e_s& values) { │ │ │ │ │ +227 // We remove the const from values to create a non-const Filtered │ │ │ │ │ +228 // view, then pull the const_iterators out of it. │ │ │ │ │ +229 const Filtered filtered(filter, const_cast<_V_a_l_u_e_s&>(values)); │ │ │ │ │ +230 begin_ = filtered.beginConst(); │ │ │ │ │ +231 end_ = filtered.endConst(); │ │ │ │ │ +232 } │ │ │ │ │ +233 }; │ │ │ │ │ +234 │ │ │ │ │ +235 template │ │ │ │ │ +236 _V_a_l_u_e_s_:_:_V_a_l_u_e_s(const Values::Filtered& view) { │ │ │ │ │ +237 for(const auto key_value: view) { │ │ │ │ │ +238 _K_e_y key = key_value.key; │ │ │ │ │ +239 _i_n_s_e_r_t(key, static_cast(key_value.value)); │ │ │ │ │ +240 } │ │ │ │ │ +241 } │ │ │ │ │ +242 │ │ │ │ │ +243 template │ │ │ │ │ +244 _V_a_l_u_e_s_:_:_V_a_l_u_e_s(const Values::ConstFiltered& view) { │ │ │ │ │ +245 for(const auto key_value: view) { │ │ │ │ │ +246 _K_e_y key = key_value.key; │ │ │ │ │ +247 _i_n_s_e_r_t(key, static_cast(key_value.value)); │ │ │ │ │ +248 } │ │ │ │ │ +249 } │ │ │ │ │ +250 │ │ │ │ │ +251 Values::Filtered │ │ │ │ │ +252 inline Values::filter(const std::function& filterFcn) { │ │ │ │ │ +253 return filter(filterFcn); │ │ │ │ │ +254 } │ │ │ │ │ 255 │ │ │ │ │ -259template │ │ │ │ │ -_2_6_0class _I_s_L_i_e_G_r_o_u_p: public _I_s_G_r_o_u_p, public IsManifold { │ │ │ │ │ -261public: │ │ │ │ │ -262 typedef typename _t_r_a_i_t_s_<_T_>_:_:_s_t_r_u_c_t_u_r_e___c_a_t_e_g_o_r_y structure_category_tag; │ │ │ │ │ -263 typedef typename _t_r_a_i_t_s_<_T_>_:_:_M_a_n_i_f_o_l_d_T_y_p_e ManifoldType; │ │ │ │ │ -264 typedef typename _t_r_a_i_t_s_<_T_>_:_:_T_a_n_g_e_n_t_V_e_c_t_o_r TangentVector; │ │ │ │ │ -265 typedef typename _t_r_a_i_t_s_<_T_>_:_:_C_h_a_r_t_J_a_c_o_b_i_a_n ChartJacobian; │ │ │ │ │ -266 │ │ │ │ │ -267 BOOST_CONCEPT_USAGE(_I_s_L_i_e_G_r_o_u_p) { │ │ │ │ │ -268 BOOST_STATIC_ASSERT_MSG( │ │ │ │ │ -269 (boost::is_base_of::value), │ │ │ │ │ -270 "This type's trait does not assert it is a Lie group (or derived)"); │ │ │ │ │ -271 │ │ │ │ │ -272 // group opertations with Jacobians │ │ │ │ │ -273 g = _t_r_a_i_t_s_<_T_>_:_:_C_o_m_p_o_s_e(g, h, Hg, Hh); │ │ │ │ │ -274 g = _t_r_a_i_t_s_<_T_>_:_:_B_e_t_w_e_e_n(g, h, Hg, Hh); │ │ │ │ │ -275 g = _t_r_a_i_t_s_<_T_>_:_:_I_n_v_e_r_s_e(g, Hg); │ │ │ │ │ -276 // log and exp map without Jacobians │ │ │ │ │ -277 g = _t_r_a_i_t_s_<_T_>_:_:_E_x_p_m_a_p(v); │ │ │ │ │ -278 v = _t_r_a_i_t_s_<_T_>_:_:_L_o_g_m_a_p(g); │ │ │ │ │ -279 // log and exponential map with Jacobians │ │ │ │ │ -280 g = _t_r_a_i_t_s_<_T_>_:_:_E_x_p_m_a_p(v, Hg); │ │ │ │ │ -281 v = _t_r_a_i_t_s_<_T_>_:_:_L_o_g_m_a_p(g, Hg); │ │ │ │ │ +256 template │ │ │ │ │ +257 Values::Filtered │ │ │ │ │ +258 Values::filter(const std::function& filterFcn) { │ │ │ │ │ +259 return Filtered(std::bind(&filterHelper, filterFcn, │ │ │ │ │ +260 std::placeholders::_1), *this); │ │ │ │ │ +261 } │ │ │ │ │ +262 │ │ │ │ │ +263 Values::ConstFiltered │ │ │ │ │ +264 inline Values::filter(const std::function& filterFcn) const { │ │ │ │ │ +265 return filter(filterFcn); │ │ │ │ │ +266 } │ │ │ │ │ +267 │ │ │ │ │ +268 template │ │ │ │ │ +269 Values::ConstFiltered │ │ │ │ │ +270 Values::filter(const std::function& filterFcn) const { │ │ │ │ │ +271 return ConstFiltered(std::bind(&filterHelper, │ │ │ │ │ +272 filterFcn, std::placeholders::_1), *this); │ │ │ │ │ +273 } │ │ │ │ │ +274#endif │ │ │ │ │ +275 │ │ │ │ │ +276 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +277 template<> │ │ │ │ │ +278 inline bool Values::filterHelper(const std::function │ │ │ │ │ +filter, │ │ │ │ │ +279 const ConstKeyValuePair& key_value) { │ │ │ │ │ +280 // Filter and check the type │ │ │ │ │ +281 return filter(key_value.key); │ │ │ │ │ 282 } │ │ │ │ │ -283private: │ │ │ │ │ -284 T g, h; │ │ │ │ │ -285 TangentVector v; │ │ │ │ │ -286 ChartJacobian Hg, Hh; │ │ │ │ │ -287}; │ │ │ │ │ -288 │ │ │ │ │ -297template │ │ │ │ │ -_2_9_8T _B_C_H(const T& X, const T& Y) { │ │ │ │ │ -299 static const double _2 = 1. / 2., _12 = 1. / 12., _24 = 1. / 24.; │ │ │ │ │ -300 T X_Y = bracket(X, Y); │ │ │ │ │ -301 return T(X + Y + _2 * X_Y + _12 * bracket(X - Y, X_Y) - _24 * bracket(Y, │ │ │ │ │ -bracket(X, X_Y))); │ │ │ │ │ -302} │ │ │ │ │ -303 │ │ │ │ │ -_3_0_8template Matrix _w_e_d_g_e(const Vector& x); │ │ │ │ │ -309 │ │ │ │ │ -316template │ │ │ │ │ -_3_1_7T _e_x_p_m(const Vector& x, int K=7) { │ │ │ │ │ -318 Matrix xhat = wedge(x); │ │ │ │ │ -319 return T(_e_x_p_m(xhat,K)); │ │ │ │ │ -320} │ │ │ │ │ -321 │ │ │ │ │ -326template │ │ │ │ │ -_3_2_7T _i_n_t_e_r_p_o_l_a_t_e(const T& X, const T& Y, double t, │ │ │ │ │ -328 typename MakeOptionalJacobian::type Hx = boost::none, │ │ │ │ │ -329 typename MakeOptionalJacobian::type Hy = boost::none) { │ │ │ │ │ -330 if (Hx || Hy) { │ │ │ │ │ -331 typename MakeJacobian::type between_H_x, log_H, exp_H, compose_H_x; │ │ │ │ │ -332 const T between = │ │ │ │ │ -333 _t_r_a_i_t_s_<_T_>_:_:_B_e_t_w_e_e_n(X, Y, between_H_x); // between_H_y = identity │ │ │ │ │ -334 typename _t_r_a_i_t_s_<_T_>_:_:_T_a_n_g_e_n_t_V_e_c_t_o_r delta = _t_r_a_i_t_s_<_T_>_:_:_L_o_g_m_a_p(between, │ │ │ │ │ -log_H); │ │ │ │ │ -335 const T Delta = _t_r_a_i_t_s_<_T_>_:_:_E_x_p_m_a_p(t * delta, exp_H); │ │ │ │ │ -336 const T result = _t_r_a_i_t_s_<_T_>_:_:_C_o_m_p_o_s_e( │ │ │ │ │ -337 X, Delta, compose_H_x); // compose_H_xinv_y = identity │ │ │ │ │ -338 │ │ │ │ │ -339 if (Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x; │ │ │ │ │ -340 if (Hy) *Hy = t * exp_H * log_H; │ │ │ │ │ -341 return result; │ │ │ │ │ -342 } │ │ │ │ │ -343 return _t_r_a_i_t_s_<_T_>_:_:_C_o_m_p_o_s_e( │ │ │ │ │ -344 X, _t_r_a_i_t_s_<_T_>_:_:_E_x_p_m_a_p(t * _t_r_a_i_t_s_<_T_>_:_:_L_o_g_m_a_p(_t_r_a_i_t_s_<_T_>_:_:_B_e_t_w_e_e_n(X, Y)))); │ │ │ │ │ -345} │ │ │ │ │ -346 │ │ │ │ │ -351template │ │ │ │ │ -_3_5_2class _T_r_a_n_s_f_o_r_m_C_o_v_a_r_i_a_n_c_e │ │ │ │ │ -353{ │ │ │ │ │ -354private: │ │ │ │ │ -355 typename T::Jacobian adjointMap_; │ │ │ │ │ -356public: │ │ │ │ │ -357 explicit _T_r_a_n_s_f_o_r_m_C_o_v_a_r_i_a_n_c_e(const T &X) : adjointMap_{X.AdjointMap()} {} │ │ │ │ │ -358 typename T::Jacobian operator()(const typename T::Jacobian &covariance) │ │ │ │ │ -359 { return adjointMap_ * covariance * adjointMap_.transpose(); } │ │ │ │ │ -360}; │ │ │ │ │ -361 │ │ │ │ │ -362} // namespace gtsam │ │ │ │ │ -363 │ │ │ │ │ -_3_7_2#define GTSAM_CONCEPT_LIE_INST(T) template class gtsam::IsLieGroup; │ │ │ │ │ -373#define GTSAM_CONCEPT_LIE_TYPE(T) using _gtsam_IsLieGroup_##T = gtsam:: │ │ │ │ │ -IsLieGroup; │ │ │ │ │ -_G_r_o_u_p_._h │ │ │ │ │ -Concept check class for variable types with Group properties. │ │ │ │ │ -_M_a_n_i_f_o_l_d_._h │ │ │ │ │ -Base class and basic functions for Manifold types. │ │ │ │ │ +283 │ │ │ │ │ +284 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +285 │ │ │ │ │ +286 namespace internal { │ │ │ │ │ +287 │ │ │ │ │ +288 // Check the type and throw exception if incorrect │ │ │ │ │ +289 // Generic version, partially specialized below for various Eigen Matrix │ │ │ │ │ +types │ │ │ │ │ +290 template │ │ │ │ │ +_2_9_1 struct _h_a_n_d_l_e { │ │ │ │ │ +292 ValueType operator()(_K_e_y j, const _V_a_l_u_e* const pointer) { │ │ │ │ │ +293 auto ptr = dynamic_cast*>(pointer); │ │ │ │ │ +294 if (ptr) { │ │ │ │ │ +295 // value returns a const ValueType&, and the return makes a copy !!!!! │ │ │ │ │ +296 return ptr->_v_a_l_u_e(); │ │ │ │ │ +297 } else { │ │ │ │ │ +298 throw _V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e(j, typeid(*pointer), typeid(ValueType)); │ │ │ │ │ +299 } │ │ │ │ │ +300 } │ │ │ │ │ +301 }; │ │ │ │ │ +302 │ │ │ │ │ +303 template │ │ │ │ │ +_3_0_4 struct _h_a_n_d_l_e___m_a_t_r_i_x; │ │ │ │ │ +305 │ │ │ │ │ +306 // Handle dynamic matrices │ │ │ │ │ +307 template │ │ │ │ │ +_3_0_8 struct _h_a_n_d_l_e___m_a_t_r_i_x, true> { │ │ │ │ │ +309 inline Eigen::Matrix operator()(_K_e_y j, const _V_a_l_u_e* const │ │ │ │ │ +pointer) { │ │ │ │ │ +310 auto ptr = dynamic_cast>*> │ │ │ │ │ +(pointer); │ │ │ │ │ +311 if (ptr) { │ │ │ │ │ +312 // value returns a const Matrix&, and the return makes a copy !!!!! │ │ │ │ │ +313 return ptr->_v_a_l_u_e(); │ │ │ │ │ +314 } else { │ │ │ │ │ +315 // If a fixed matrix was stored, we end up here as well. │ │ │ │ │ +316 throw _V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e(j, typeid(*pointer), typeid(Eigen::Matrix)); │ │ │ │ │ +317 } │ │ │ │ │ +318 } │ │ │ │ │ +319 }; │ │ │ │ │ +320 │ │ │ │ │ +321 // Handle fixed matrices │ │ │ │ │ +322 template │ │ │ │ │ +_3_2_3 struct _h_a_n_d_l_e___m_a_t_r_i_x, false> { │ │ │ │ │ +324 inline Eigen::Matrix operator()(_K_e_y j, const _V_a_l_u_e* const │ │ │ │ │ +pointer) { │ │ │ │ │ +325 auto ptr = dynamic_cast>*> │ │ │ │ │ +(pointer); │ │ │ │ │ +326 if (ptr) { │ │ │ │ │ +327 // value returns a const MatrixMN&, and the return makes a copy !!!!! │ │ │ │ │ +328 return ptr->_v_a_l_u_e(); │ │ │ │ │ +329 } else { │ │ │ │ │ +330 Matrix A; │ │ │ │ │ +331 // Check if a dynamic matrix was stored │ │ │ │ │ +332 auto ptr = dynamic_cast*>(pointer); │ │ │ │ │ +333 if (ptr) { │ │ │ │ │ +334 A = ptr->value(); │ │ │ │ │ +335 } else { │ │ │ │ │ +336 // Or a dynamic vector │ │ │ │ │ +337 A = _h_a_n_d_l_e___m_a_t_r_i_x_<_E_i_g_e_n_:_:_V_e_c_t_o_r_X_d_,_ _t_r_u_e_>()(j, pointer); // will throw if │ │ │ │ │ +not.... │ │ │ │ │ +338 } │ │ │ │ │ +339 // Yes: check size, and throw if not a match │ │ │ │ │ +340 if (A.rows() != M || A.cols() != N) │ │ │ │ │ +341 throw _N_o_M_a_t_c_h_F_o_u_n_d_F_o_r_F_i_x_e_d(M, N, A.rows(), A.cols()); │ │ │ │ │ +342 else │ │ │ │ │ +343 return A; // copy but not malloc │ │ │ │ │ +344 } │ │ │ │ │ +345 } │ │ │ │ │ +346 }; │ │ │ │ │ +347 │ │ │ │ │ +348 // Handle matrices │ │ │ │ │ +349 template │ │ │ │ │ +_3_5_0 struct _h_a_n_d_l_e> { │ │ │ │ │ +351 Eigen::Matrix operator()(_K_e_y j, const _V_a_l_u_e* const pointer) { │ │ │ │ │ +352 return _h_a_n_d_l_e___m_a_t_r_i_x_<_E_i_g_e_n_:_:_M_a_t_r_i_x_<_d_o_u_b_l_e_,_ _M_,_ _N_>, │ │ │ │ │ +353 (M == Eigen::Dynamic || N == Eigen::Dynamic)>()(j, pointer); │ │ │ │ │ +354 } │ │ │ │ │ +355 }; │ │ │ │ │ +356 │ │ │ │ │ +357 } // internal │ │ │ │ │ +358 │ │ │ │ │ +359 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +360 template │ │ │ │ │ +_3_6_1 const ValueType _V_a_l_u_e_s_:_:_a_t(_K_e_y j) const { │ │ │ │ │ +362 // Find the item │ │ │ │ │ +363 KeyValueMap::const_iterator item = values_.find(j); │ │ │ │ │ +364 │ │ │ │ │ +365 // Throw exception if it does not exist │ │ │ │ │ +366 if (item == values_.end()) throw _V_a_l_u_e_s_K_e_y_D_o_e_s_N_o_t_E_x_i_s_t("at", j); │ │ │ │ │ +367 │ │ │ │ │ +368 // Check the type and throw exception if incorrect │ │ │ │ │ +369 // h() split in two lines to avoid internal compiler error (MSVC2017) │ │ │ │ │ +370 auto h = _i_n_t_e_r_n_a_l_:_:_h_a_n_d_l_e_<_V_a_l_u_e_T_y_p_e_>(); │ │ │ │ │ +371 return h(j, item->second); │ │ │ │ │ +372 } │ │ │ │ │ +373 │ │ │ │ │ +374 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +375 template │ │ │ │ │ +_3_7_6 boost::optional _V_a_l_u_e_s_:_:_e_x_i_s_t_s(_K_e_y j) const { │ │ │ │ │ +377 // Find the item │ │ │ │ │ +378 KeyValueMap::const_iterator item = values_.find(j); │ │ │ │ │ +379 │ │ │ │ │ +380 if(item != values_.end()) { │ │ │ │ │ +381 // dynamic cast the type and throw exception if incorrect │ │ │ │ │ +382 auto ptr = dynamic_cast*>(item->second); │ │ │ │ │ +383 if (ptr) { │ │ │ │ │ +384 return ptr->_v_a_l_u_e(); │ │ │ │ │ +385 } else { │ │ │ │ │ +386 // NOTE(abe): clang warns about potential side effects if done in typeid │ │ │ │ │ +387 const _V_a_l_u_e* value = item->second; │ │ │ │ │ +388 throw _V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e(j, typeid(*value), typeid(ValueType)); │ │ │ │ │ +389 } │ │ │ │ │ +390 } else { │ │ │ │ │ +391 return boost::none; │ │ │ │ │ +392 } │ │ │ │ │ +393 } │ │ │ │ │ +394 │ │ │ │ │ +395 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +396 │ │ │ │ │ +397 // insert a templated value │ │ │ │ │ +398 template │ │ │ │ │ +_3_9_9 void _V_a_l_u_e_s_:_:_i_n_s_e_r_t(_K_e_y j, const ValueType& val) { │ │ │ │ │ +400 _i_n_s_e_r_t(j, static_cast(_G_e_n_e_r_i_c_V_a_l_u_e_<_V_a_l_u_e_T_y_p_e_>(val))); │ │ │ │ │ +401 } │ │ │ │ │ +402 │ │ │ │ │ +403 // update with templated value │ │ │ │ │ +404 template │ │ │ │ │ +405 void _V_a_l_u_e_s_:_:_u_p_d_a_t_e(_K_e_y j, const ValueType& val) { │ │ │ │ │ +406 _u_p_d_a_t_e(j, static_cast(_G_e_n_e_r_i_c_V_a_l_u_e_<_V_a_l_u_e_T_y_p_e_>(val))); │ │ │ │ │ +407 } │ │ │ │ │ +408 │ │ │ │ │ +409 // insert_or_assign with templated value │ │ │ │ │ +410 template │ │ │ │ │ +_4_1_1 void _V_a_l_u_e_s_:_:_i_n_s_e_r_t___o_r___a_s_s_i_g_n(_K_e_y j, const ValueType& val) { │ │ │ │ │ +412 _i_n_s_e_r_t___o_r___a_s_s_i_g_n(j, static_cast(_G_e_n_e_r_i_c_V_a_l_u_e_<_V_a_l_u_e_T_y_p_e_> │ │ │ │ │ +(val))); │ │ │ │ │ +413 } │ │ │ │ │ +414 │ │ │ │ │ +415} │ │ │ │ │ +_V_a_l_u_e_s_._h │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_e_x_p_m │ │ │ │ │ -T expm(const Vector &x, int K=7) │ │ │ │ │ -Exponential map given exponential coordinates class T needs a wedge<> function │ │ │ │ │ -and a constructor from... │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:317 │ │ │ │ │ -_g_t_s_a_m_:_:_l_o_g_m_a_p___d_e_f_a_u_l_t │ │ │ │ │ -Vector logmap_default(const Class &l0, const Class &lp) │ │ │ │ │ -Log map centered at l0, s.t. │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:246 │ │ │ │ │ -_g_t_s_a_m_:_:_b_e_t_w_e_e_n___d_e_f_a_u_l_t │ │ │ │ │ -Class between_default(const Class &l1, const Class &l2) │ │ │ │ │ -These core global functions can be specialized by new Lie types for better │ │ │ │ │ -performance. │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:240 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_p_o_l_a_t_e │ │ │ │ │ -T interpolate(const T &X, const T &Y, double t, typename MakeOptionalJacobian< │ │ │ │ │ -T, T >::type Hx=boost::none, typename MakeOptionalJacobian< T, T >::type │ │ │ │ │ -Hy=boost::none) │ │ │ │ │ -Linear interpolation between X and Y by coefficient t. │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:327 │ │ │ │ │ -_g_t_s_a_m_:_:_B_C_H │ │ │ │ │ -T BCH(const T &X, const T &Y) │ │ │ │ │ -Three term approximation of the Baker-Campbell-Hausdorff formula In non- │ │ │ │ │ -commutative Lie groups,... │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:298 │ │ │ │ │ -_g_t_s_a_m_:_:_e_x_p_m_a_p___d_e_f_a_u_l_t │ │ │ │ │ -Class expmap_default(const Class &t, const Vector &d) │ │ │ │ │ -Exponential map centered at l0, s.t. │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:252 │ │ │ │ │ -_g_t_s_a_m_:_:_w_e_d_g_e │ │ │ │ │ -Matrix wedge(const Vector &x) │ │ │ │ │ -Declaration of wedge (see Murray94book) used to convert from n exponential │ │ │ │ │ -coordinates to n*n element... │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_g_r_o_u_p___t_a_g │ │ │ │ │ -tag to assert a type is a group │ │ │ │ │ -DDeeffiinniittiioonn Group.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_m_u_l_t_i_p_l_i_c_a_t_i_v_e___g_r_o_u_p___t_a_g │ │ │ │ │ -Group operator syntax flavors. │ │ │ │ │ -DDeeffiinniittiioonn Group.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_I_s_G_r_o_u_p │ │ │ │ │ -Group Concept. │ │ │ │ │ -DDeeffiinniittiioonn Group.h:46 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ -A CRTP helper class that implements Lie group methods Prerequisites: methods │ │ │ │ │ -operator*,... │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_R_e_t_r_a_c_t │ │ │ │ │ -static Class Retract(const TangentVector &v, ChartJacobian H) │ │ │ │ │ -Retract at origin with optional derivative. │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:121 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ -TangentVector localCoordinates(const Class &g) const │ │ │ │ │ -localCoordinates as required by manifold concept: finds tangent vector between │ │ │ │ │ -*this and g │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:136 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_e_x_p_m_a_p │ │ │ │ │ -Class expmap(const TangentVector &v, ChartJacobian H1, ChartJacobian H2=boost:: │ │ │ │ │ -none) const │ │ │ │ │ -expmap with optional derivatives │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:89 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ -TangentVector localCoordinates(const Class &g, ChartJacobian H1, ChartJacobian │ │ │ │ │ -H2=boost::none) const │ │ │ │ │ -localCoordinates with optional derivatives │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:152 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_l_o_g_m_a_p │ │ │ │ │ -TangentVector logmap(const Class &g) const │ │ │ │ │ -logmap as required by manifold concept Applies logarithmic map to group element │ │ │ │ │ -that takes *this to g │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:84 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_l_o_g_m_a_p │ │ │ │ │ -TangentVector logmap(const Class &g, ChartJacobian H1, ChartJacobian H2=boost:: │ │ │ │ │ -none) const │ │ │ │ │ -logmap with optional derivatives │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_L_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ -static TangentVector LocalCoordinates(const Class &g) │ │ │ │ │ -LocalCoordinates at origin: possible in Lie group because it has an identity. │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:116 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_r_e_t_r_a_c_t │ │ │ │ │ -Class retract(const TangentVector &v, ChartJacobian H1, ChartJacobian │ │ │ │ │ -H2=boost::none) const │ │ │ │ │ -retract with optional derivatives │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:141 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_L_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ -static TangentVector LocalCoordinates(const Class &g, ChartJacobian H) │ │ │ │ │ -LocalCoordinates at origin with optional derivative. │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:126 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_r_e_t_r_a_c_t │ │ │ │ │ -Class retract(const TangentVector &v) const │ │ │ │ │ -retract as required by manifold concept: applies v at *this │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:131 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_e_x_p_m_a_p │ │ │ │ │ -Class expmap(const TangentVector &v) const │ │ │ │ │ -expmap as required by manifold concept Applies exponential map to v and │ │ │ │ │ -composes with *this │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:78 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_R_e_t_r_a_c_t │ │ │ │ │ -static Class Retract(const TangentVector &v) │ │ │ │ │ -Retract at origin: possible in Lie group because it has an identity. │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_l_i_e___g_r_o_u_p___t_a_g │ │ │ │ │ -tag to assert a type is a Lie group │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:164 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p_T_r_a_i_t_s │ │ │ │ │ -A helper class that implements the traits interface for GTSAM lie groups. │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:174 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ -Both LieGroupTraits and Testable. │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:229 │ │ │ │ │ -_g_t_s_a_m_:_:_I_s_L_i_e_G_r_o_u_p │ │ │ │ │ -Lie Group Concept. │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:260 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_a_n_s_f_o_r_m_C_o_v_a_r_i_a_n_c_e │ │ │ │ │ -Functor for transforming covariance of T. │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:353 │ │ │ │ │ -_g_t_s_a_m_:_:_m_a_n_i_f_o_l_d___t_a_g │ │ │ │ │ -tag to assert a type is a manifold │ │ │ │ │ -DDeeffiinniittiioonn Manifold.h:33 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_G_e_t_D_i_m_e_n_s_i_o_n_I_m_p_l │ │ │ │ │ -Extra manifold traits for fixed-dimension types. │ │ │ │ │ -DDeeffiinniittiioonn Manifold.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ -either a fixed size o... │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e │ │ │ │ │ +Wraps any type T so it can play as a Value. │ │ │ │ │ +DDeeffiinniittiioonn GenericValue.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_v_a_l_u_e │ │ │ │ │ +const T & value() const │ │ │ │ │ +Return a constant value. │ │ │ │ │ +DDeeffiinniittiioonn GenericValue.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e │ │ │ │ │ +This is the base class for any type to be stored in Values. │ │ │ │ │ +DDeeffiinniittiioonn Value.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:___V_a_l_u_e_s_K_e_y_V_a_l_u_e_P_a_i_r │ │ │ │ │ +DDeeffiinniittiioonn Values-inl.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:___V_a_l_u_e_s_K_e_y_V_a_l_u_e_P_a_i_r_:_:_k_e_y │ │ │ │ │ +const Key key │ │ │ │ │ +The key. │ │ │ │ │ +DDeeffiinniittiioonn Values-inl.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:___V_a_l_u_e_s_K_e_y_V_a_l_u_e_P_a_i_r_:_:_v_a_l_u_e │ │ │ │ │ +ValueType & value │ │ │ │ │ +The value. │ │ │ │ │ +DDeeffiinniittiioonn Values-inl.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:___V_a_l_u_e_s_C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r │ │ │ │ │ +DDeeffiinniittiioonn Values-inl.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:___V_a_l_u_e_s_C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r_:_:_k_e_y │ │ │ │ │ +const Key key │ │ │ │ │ +The key. │ │ │ │ │ +DDeeffiinniittiioonn Values-inl.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:___V_a_l_u_e_s_C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r_:_:_v_a_l_u_e │ │ │ │ │ +const ValueType & value │ │ │ │ │ +The value. │ │ │ │ │ +DDeeffiinniittiioonn Values-inl.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_C_a_s_t_H_e_l_p_e_r │ │ │ │ │ +DDeeffiinniittiioonn Values-inl.h:62 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_h_a_n_d_l_e │ │ │ │ │ +DDeeffiinniittiioonn Values-inl.h:291 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_h_a_n_d_l_e___m_a_t_r_i_x │ │ │ │ │ +DDeeffiinniittiioonn Values-inl.h:304 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_u_p_d_a_t_e │ │ │ │ │ +void update(Key j, const Value &val) │ │ │ │ │ +single element change of existing element │ │ │ │ │ +DDeeffiinniittiioonn Values.cpp:180 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_e_x_t_r_a_c_t │ │ │ │ │ +std::map< Key, ValueType > extract(const std::function< bool(Key)> │ │ │ │ │ +&filterFcn=&_truePredicate< Key >) const │ │ │ │ │ +Extract a subset of values of the given type ValueType. │ │ │ │ │ +DDeeffiinniittiioonn Values-inl.h:94 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_a_t │ │ │ │ │ +const ValueType at(Key j) const │ │ │ │ │ +Retrieve a variable by key j. │ │ │ │ │ +DDeeffiinniittiioonn Values-inl.h:361 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_i_n_s_e_r_t │ │ │ │ │ +void insert(Key j, const Value &val) │ │ │ │ │ +Add a variable with the given j, throws KeyAlreadyExists if j is already │ │ │ │ │ +present. │ │ │ │ │ +DDeeffiinniittiioonn Values.cpp:157 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_i_n_s_e_r_t___o_r___a_s_s_i_g_n │ │ │ │ │ +void insert_or_assign(Key j, const Value &val) │ │ │ │ │ +If key j exists, update value, else perform an insert. │ │ │ │ │ +DDeeffiinniittiioonn Values.cpp:203 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_V_a_l_u_e_s │ │ │ │ │ +Values()=default │ │ │ │ │ +Default constructor creates an empty Values class. │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_e_x_i_s_t_s │ │ │ │ │ +bool exists(Key j) const │ │ │ │ │ +Check if a value exists with key j. │ │ │ │ │ +DDeeffiinniittiioonn Values.cpp:94 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_K_e_y_V_a_l_u_e_P_a_i_r │ │ │ │ │ +A key-value pair, which you get by dereferencing iterators. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:93 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r │ │ │ │ │ +A key-value pair, which you get by dereferencing iterators. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:101 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_K_e_y_D_o_e_s_N_o_t_E_x_i_s_t │ │ │ │ │ +DDeeffiinniittiioonn Values.h:475 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e │ │ │ │ │ +DDeeffiinniittiioonn Values.h:498 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_M_a_t_c_h_F_o_u_n_d_F_o_r_F_i_x_e_d │ │ │ │ │ +DDeeffiinniittiioonn Values.h:542 │ │ │ │ │ +_V_a_l_u_e_s │ │ │ │ │ +In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ +a non-linear function... │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _b_a_s_e │ │ │ │ │ - * _L_i_e_._h │ │ │ │ │ + * _n_o_n_l_i_n_e_a_r │ │ │ │ │ + * VVaalluueess--iinnll..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00032_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00017_source.html │ │ │ │┄ Files 99% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
numericalDerivative.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │ @@ -117,18 +117,18 @@ │ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
18// \callgraph
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │
21#include <functional>
│ │ │ │
22
│ │ │ │ -
23#include <gtsam/linear/VectorValues.h>
│ │ │ │ -
24#include <gtsam/linear/JacobianFactor.h>
│ │ │ │ -
25#include <gtsam/nonlinear/Values.h>
│ │ │ │ -
26#include <gtsam/base/Lie.h>
│ │ │ │ +
23#include <gtsam/linear/VectorValues.h>
│ │ │ │ +
24#include <gtsam/linear/JacobianFactor.h>
│ │ │ │ +
25#include <gtsam/nonlinear/Values.h>
│ │ │ │ +
26#include <gtsam/base/Lie.h>
│ │ │ │
27
│ │ │ │
28namespace gtsam {
│ │ │ │
29
│ │ │ │
30/*
│ │ │ │
31 * Note that all of these functions have two versions, a boost.function version and a
│ │ │ │
32 * standard C++ function pointer version. This allows reformulating the arguments of
│ │ │ │
33 * a function to fit the correct structure, which is useful for situations like
│ │ │ │ @@ -988,18 +988,18 @@ │ │ │ │
1099 return numericalHessian323(
│ │ │ │
1100 std::function<double(const X1&, const X2&, const X3&)>(f), x1, x2, x3,
│ │ │ │
1101 delta);
│ │ │ │
1102}
│ │ │ │
1103
│ │ │ │
1104} // namespace gtsam
│ │ │ │
1105
│ │ │ │ -
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ - │ │ │ │ -
VectorValues.h
Factor Graph Values.
│ │ │ │ -
Values.h
A non-templated config holding any types of Manifold-group elements.
│ │ │ │ +
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ + │ │ │ │ +
VectorValues.h
Factor Graph Values.
│ │ │ │ +
Values.h
A non-templated config holding any types of Manifold-group elements.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::numericalDerivative61
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative61(std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
Compute numerical derivative in argument 1 of 6-argument function.
Definition numericalDerivative.h:643
│ │ │ │
gtsam::numericalHessian311
internal::FixedSizeMatrix< X1, X1 >::type numericalHessian311(std::function< double(const X1 &, const X2 &, const X3 &)> f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
Numerical Hessian for tenary functions.
Definition numericalDerivative.h:972
│ │ │ │
gtsam::numericalHessian
internal::FixedSizeMatrix< X, X >::type numericalHessian(std::function< double(const X &)> f, const X &x, double delta=1e-5)
Compute numerical Hessian matrix.
Definition numericalDerivative.h:861
│ │ │ │
gtsam::numericalDerivative21
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative21(const std::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5)
Compute numerical derivative in argument 1 of binary function.
Definition numericalDerivative.h:166
│ │ │ │
gtsam::numericalDerivative63
internal::FixedSizeMatrix< Y, X3 >::type numericalDerivative63(std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
Compute numerical derivative in argument 3 of 6-argument function.
Definition numericalDerivative.h:717
│ │ │ │
gtsam::numericalDerivative51
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative51(std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
Compute numerical derivative in argument 1 of 5-argument function.
Definition numericalDerivative.h:462
│ │ │ │ @@ -1026,13 +1026,13 @@ │ │ │ │
gtsam::internal::FixedSizeMatrix
Definition numericalDerivative.h:56
│ │ │ │
gtsam::G_x1
Helper class that computes the derivative of f w.r.t.
Definition numericalDerivative.h:883
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00035.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00731.html │ │ │ │┄ Files 85% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/ThreadsafeException.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/JacobianFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces
│ │ │ │ -
ThreadsafeException.h File Reference
Base
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
JacobianFactor.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Base exception type that uses tbb_allocator if GTSAM is compiled with TBB. │ │ │ │ -More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::ThreadsafeException< DERIVED >
 Base exception type that uses tbb_allocator if GTSAM is compiled with TBB. More...
 
class  gtsam::RuntimeErrorThreadsafe
 Thread-safe runtime error exception. More...
 
class  gtsam::OutOfRangeThreadsafe
 Thread-safe out of range exception. More...
class  gtsam::JacobianFactor
 A Gaussian factor in the squared-error form. More...
 
class  gtsam::InvalidArgumentThreadsafe
 Thread-safe invalid argument exception. More...
 
class  gtsam::CholeskyFailed
 Indicate Cholesky factorization failure. More...
struct  gtsam::traits< JacobianFactor >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > gtsam::EliminateQR (const GaussianFactorGraph &factors, const Ordering &keys)
 Multiply all factors and eliminate the given keys from the resulting factor using a QR variant that handles constraints (zero sigmas).
 
│ │ │ │

Detailed Description

│ │ │ │ -

Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Aug 21, 2010
│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
│ │ │ │ +Christian Potthast
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +
Date
Dec 8, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,42 +1,39 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -ThreadsafeException.h File Reference │ │ │ │ │ -_B_a_s_e │ │ │ │ │ -Base exception type that uses tbb_allocator if GTSAM is compiled with TBB. │ │ │ │ │ -_M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +JacobianFactor.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_<_ _D_E_R_I_V_E_D_ _> │ │ │ │ │ -  Base exception type that uses tbb_allocator if GTSAM is compiled with │ │ │ │ │ - TBB. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ +  A Gaussian factor in the squared-error form. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -class   _g_t_s_a_m_:_:_R_u_n_t_i_m_e_E_r_r_o_r_T_h_r_e_a_d_s_a_f_e │ │ │ │ │ -  Thread-safe runtime error exception. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_O_u_t_O_f_R_a_n_g_e_T_h_r_e_a_d_s_a_f_e │ │ │ │ │ -  Thread-safe out of range exception. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_I_n_v_a_l_i_d_A_r_g_u_m_e_n_t_T_h_r_e_a_d_s_a_f_e │ │ │ │ │ -  Thread-safe invalid argument exception. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_C_h_o_l_e_s_k_y_F_a_i_l_e_d │ │ │ │ │ -  Indicate Cholesky factorization failure. _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _J_a_c_o_b_i_a_n_F_a_c_t_o_r_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ + std::pair< _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_: _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_Q_R (const │ │ │ │ │ +_s_h_a_r_e_d___p_t_r, _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &factors, const │ │ │ │ │ + >  _O_r_d_e_r_i_n_g &keys) │ │ │ │ │ + Multiply all factors and eliminate the │ │ │ │ │ +  given keys from the resulting factor │ │ │ │ │ + using a QR variant that handles │ │ │ │ │ + constraints (zero sigmas). │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Base exception type that uses tbb_allocator if GTSAM is compiled with TBB. │ │ │ │ │ Author │ │ │ │ │ Richard Roberts │ │ │ │ │ + Christian Potthast │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - Aug 21, 2010 │ │ │ │ │ + Dec 8, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _b_a_s_e │ │ │ │ │ - * _T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_._h │ │ │ │ │ + * _l_i_n_e_a_r │ │ │ │ │ + * _J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00035_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00455_source.html │ │ │ │┄ Files 95% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/ThreadsafeException.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3DS2_Base.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ThreadsafeException.h
│ │ │ │ +
Cal3DS2_Base.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
20#pragma once
│ │ │ │
21
│ │ │ │ -
22#include <gtsam/config.h> // for GTSAM_USE_TBB
│ │ │ │ -
23
│ │ │ │ -
24#include <boost/optional/optional.hpp>
│ │ │ │ -
25#include <gtsam/dllexport.h>
│ │ │ │ -
26#include <string>
│ │ │ │ -
27#include <typeinfo>
│ │ │ │ -
28#include <exception>
│ │ │ │ -
29
│ │ │ │ -
30#ifdef GTSAM_USE_TBB
│ │ │ │ -
31#include <tbb/tbb_allocator.h>
│ │ │ │ -
32#include <tbb/scalable_allocator.h>
│ │ │ │ -
33#include <iostream>
│ │ │ │ -
34#endif
│ │ │ │ -
35
│ │ │ │ -
36namespace gtsam {
│ │ │ │ -
37
│ │ │ │ -
39template<class DERIVED>
│ │ │ │ -
│ │ │ │ -
40class ThreadsafeException:
│ │ │ │ -
41public std::exception
│ │ │ │ -
42{
│ │ │ │ -
43private:
│ │ │ │ -
44 typedef std::exception Base;
│ │ │ │ -
45#ifdef GTSAM_USE_TBB
│ │ │ │ -
46protected:
│ │ │ │ -
47 typedef std::basic_string<char, std::char_traits<char>,
│ │ │ │ -
48 tbb::tbb_allocator<char> > String;
│ │ │ │ -
49#else
│ │ │ │ -
50protected:
│ │ │ │ -
51 typedef std::string String;
│ │ │ │ -
52#endif
│ │ │ │ +
22#include <gtsam/geometry/Cal3.h>
│ │ │ │ +
23#include <gtsam/geometry/Point2.h>
│ │ │ │ +
24#include <boost/shared_ptr.hpp>
│ │ │ │ +
25
│ │ │ │ +
26namespace gtsam {
│ │ │ │ +
27
│ │ │ │ +
│ │ │ │ +
42class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
│ │ │ │ +
43 protected:
│ │ │ │ +
44 double k1_ = 0.0f, k2_ = 0.0f;
│ │ │ │ +
45 double p1_ = 0.0f, p2_ = 0.0f;
│ │ │ │ +
46 double tol_ = 1e-5;
│ │ │ │ +
47
│ │ │ │ +
48 public:
│ │ │ │ +
49 enum { dimension = 9 };
│ │ │ │ +
50
│ │ │ │ +
52 using shared_ptr = boost::shared_ptr<Cal3DS2_Base>;
│ │ │ │
53
│ │ │ │ -
54protected:
│ │ │ │ -
55 bool dynamic_;
│ │ │ │ -
56 mutable boost::optional<String> description_;
│ │ │ │ -
57
│ │ │ │ -
│ │ │ │ -
59 ThreadsafeException() :
│ │ │ │ -
60 dynamic_(false) {
│ │ │ │ -
61 }
│ │ │ │ -
│ │ │ │ -
62
│ │ │ │ -
│ │ │ │ -
64 ThreadsafeException(const ThreadsafeException& other) :
│ │ │ │ -
65 Base(other), dynamic_(false) {
│ │ │ │ -
66 }
│ │ │ │ -
│ │ │ │ -
67
│ │ │ │ -
│ │ │ │ -
69 ThreadsafeException(const std::string& description) :
│ │ │ │ -
70 dynamic_(false), description_(
│ │ │ │ -
71 String(description.begin(), description.end())) {
│ │ │ │ -
72 }
│ │ │ │ -
│ │ │ │ -
73
│ │ │ │ -
│ │ │ │ -
75 ~ThreadsafeException() noexcept override {
│ │ │ │ -
76 }
│ │ │ │ -
│ │ │ │ -
77
│ │ │ │ -
78public:
│ │ │ │ -
79 const char* what() const noexcept override {
│ │ │ │ -
80 return description_ ? description_->c_str() : "";
│ │ │ │ -
81 }
│ │ │ │ -
82};
│ │ │ │ -
│ │ │ │ -
83
│ │ │ │ -
│ │ │ │ -
85class GTSAM_EXPORT RuntimeErrorThreadsafe: public ThreadsafeException<RuntimeErrorThreadsafe> {
│ │ │ │ -
86public:
│ │ │ │ -
│ │ │ │ -
88 RuntimeErrorThreadsafe(const std::string& description) :
│ │ │ │ -
89 ThreadsafeException<RuntimeErrorThreadsafe>(description) {
│ │ │ │ -
90 }
│ │ │ │ -
│ │ │ │ -
91};
│ │ │ │ -
│ │ │ │ +
56
│ │ │ │ +
58 Cal3DS2_Base() = default;
│ │ │ │ +
59
│ │ │ │ +
60 Cal3DS2_Base(double fx, double fy, double s, double u0, double v0, double k1,
│ │ │ │ +
61 double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5)
│ │ │ │ +
62 : Cal3(fx, fy, s, u0, v0),
│ │ │ │ +
63 k1_(k1),
│ │ │ │ +
64 k2_(k2),
│ │ │ │ +
65 p1_(p1),
│ │ │ │ +
66 p2_(p2),
│ │ │ │ +
67 tol_(tol) {}
│ │ │ │ +
68
│ │ │ │ +
69 ~Cal3DS2_Base() override {}
│ │ │ │ +
70
│ │ │ │ +
74
│ │ │ │ +
75 Cal3DS2_Base(const Vector9& v)
│ │ │ │ +
76 : Cal3(v(0), v(1), v(2), v(3), v(4)),
│ │ │ │ +
77 k1_(v(5)),
│ │ │ │ +
78 k2_(v(6)),
│ │ │ │ +
79 p1_(v(7)),
│ │ │ │ +
80 p2_(v(8)) {}
│ │ │ │ +
81
│ │ │ │ +
85
│ │ │ │ +
87 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
│ │ │ │ +
88 const Cal3DS2_Base& cal);
│ │ │ │ +
89
│ │ │ │ +
91 void print(const std::string& s = "") const override;
│ │ │ │
92
│ │ │ │ -
│ │ │ │ -
94class OutOfRangeThreadsafe: public ThreadsafeException<OutOfRangeThreadsafe> {
│ │ │ │ -
95public:
│ │ │ │ -
│ │ │ │ -
97 OutOfRangeThreadsafe(const std::string& description) :
│ │ │ │ -
98 ThreadsafeException<OutOfRangeThreadsafe>(description) {
│ │ │ │ -
99 }
│ │ │ │ -
│ │ │ │ -
100};
│ │ │ │ -
│ │ │ │ -
101
│ │ │ │ -
│ │ │ │ -
103class InvalidArgumentThreadsafe: public ThreadsafeException<
│ │ │ │ -
104 InvalidArgumentThreadsafe> {
│ │ │ │ -
105public:
│ │ │ │ -
│ │ │ │ -
107 InvalidArgumentThreadsafe(const std::string& description) :
│ │ │ │ -
108 ThreadsafeException<InvalidArgumentThreadsafe>(description) {
│ │ │ │ -
109 }
│ │ │ │ -
│ │ │ │ -
110};
│ │ │ │ -
│ │ │ │ +
94 bool equals(const Cal3DS2_Base& K, double tol = 1e-8) const;
│ │ │ │ +
95
│ │ │ │ +
99
│ │ │ │ +
101 inline double k1() const { return k1_; }
│ │ │ │ +
102
│ │ │ │ +
104 inline double k2() const { return k2_; }
│ │ │ │ +
105
│ │ │ │ +
107 inline double p1() const { return p1_; }
│ │ │ │ +
108
│ │ │ │ +
110 inline double p2() const { return p2_; }
│ │ │ │
111
│ │ │ │ -
│ │ │ │ -
113class CholeskyFailed : public gtsam::ThreadsafeException<CholeskyFailed>
│ │ │ │ -
114{
│ │ │ │ -
115public:
│ │ │ │ -
116 CholeskyFailed() noexcept {}
│ │ │ │ -
117 ~CholeskyFailed() noexcept override {}
│ │ │ │ -
118};
│ │ │ │ -
│ │ │ │ -
119
│ │ │ │ -
120} // namespace gtsam
│ │ │ │ +
113 Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); }
│ │ │ │ +
114
│ │ │ │ +
116 Vector9 vector() const;
│ │ │ │ +
117
│ │ │ │ +
125 Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
│ │ │ │ +
126 OptionalJacobian<2, 2> Dp = boost::none) const;
│ │ │ │ +
127
│ │ │ │ +
129 Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
│ │ │ │ +
130 OptionalJacobian<2, 2> Dp = boost::none) const;
│ │ │ │ +
131
│ │ │ │ +
133 Matrix2 D2d_intrinsic(const Point2& p) const;
│ │ │ │ +
134
│ │ │ │ +
136 Matrix29 D2d_calibration(const Point2& p) const;
│ │ │ │ +
137
│ │ │ │ +
139 size_t dim() const override { return Dim(); }
│ │ │ │ +
140
│ │ │ │ +
142 inline static size_t Dim() { return dimension; }
│ │ │ │ +
143
│ │ │ │ +
147
│ │ │ │ +
│ │ │ │ +
149 virtual boost::shared_ptr<Cal3DS2_Base> clone() const {
│ │ │ │ +
150 return boost::shared_ptr<Cal3DS2_Base>(new Cal3DS2_Base(*this));
│ │ │ │ +
151 }
│ │ │ │ +
│ │ │ │ +
152
│ │ │ │ +
154
│ │ │ │ +
155 private:
│ │ │ │ +
158
│ │ │ │ +
160 friend class boost::serialization::access;
│ │ │ │ +
161 template <class Archive>
│ │ │ │ +
162 void serialize(Archive& ar, const unsigned int /*version*/) {
│ │ │ │ +
163 ar& boost::serialization::make_nvp(
│ │ │ │ +
164 "Cal3DS2_Base", boost::serialization::base_object<Cal3>(*this));
│ │ │ │ +
165 ar& BOOST_SERIALIZATION_NVP(k1_);
│ │ │ │ +
166 ar& BOOST_SERIALIZATION_NVP(k2_);
│ │ │ │ +
167 ar& BOOST_SERIALIZATION_NVP(p1_);
│ │ │ │ +
168 ar& BOOST_SERIALIZATION_NVP(p2_);
│ │ │ │ +
169 ar& BOOST_SERIALIZATION_NVP(tol_);
│ │ │ │ +
170 }
│ │ │ │ +
171
│ │ │ │ +
173};
│ │ │ │ +
│ │ │ │ +
174}
│ │ │ │ +
Point2.h
2D Point
│ │ │ │ +
Cal3.h
Common code for all Calibration models.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::ThreadsafeException
Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
Definition ThreadsafeException.h:42
│ │ │ │ -
gtsam::ThreadsafeException::~ThreadsafeException
~ThreadsafeException() noexcept override
Default destructor doesn't have the noexcept.
Definition ThreadsafeException.h:75
│ │ │ │ -
gtsam::ThreadsafeException::dynamic_
bool dynamic_
Whether this object was moved.
Definition ThreadsafeException.h:55
│ │ │ │ -
gtsam::ThreadsafeException::ThreadsafeException
ThreadsafeException()
Default constructor is protected - may only be created from derived classes.
Definition ThreadsafeException.h:59
│ │ │ │ -
gtsam::ThreadsafeException::ThreadsafeException
ThreadsafeException(const ThreadsafeException &other)
Copy constructor is protected - may only be created from derived classes.
Definition ThreadsafeException.h:64
│ │ │ │ -
gtsam::ThreadsafeException::ThreadsafeException
ThreadsafeException(const std::string &description)
Construct with description string.
Definition ThreadsafeException.h:69
│ │ │ │ -
gtsam::ThreadsafeException::description_
boost::optional< String > description_
Optional description.
Definition ThreadsafeException.h:56
│ │ │ │ -
gtsam::RuntimeErrorThreadsafe
Thread-safe runtime error exception.
Definition ThreadsafeException.h:85
│ │ │ │ -
gtsam::RuntimeErrorThreadsafe::RuntimeErrorThreadsafe
RuntimeErrorThreadsafe(const std::string &description)
Construct with a string describing the exception.
Definition ThreadsafeException.h:88
│ │ │ │ -
gtsam::OutOfRangeThreadsafe
Thread-safe out of range exception.
Definition ThreadsafeException.h:94
│ │ │ │ -
gtsam::OutOfRangeThreadsafe::OutOfRangeThreadsafe
OutOfRangeThreadsafe(const std::string &description)
Construct with a string describing the exception.
Definition ThreadsafeException.h:97
│ │ │ │ -
gtsam::InvalidArgumentThreadsafe
Thread-safe invalid argument exception.
Definition ThreadsafeException.h:104
│ │ │ │ -
gtsam::InvalidArgumentThreadsafe::InvalidArgumentThreadsafe
InvalidArgumentThreadsafe(const std::string &description)
Construct with a string describing the exception.
Definition ThreadsafeException.h:107
│ │ │ │ -
gtsam::CholeskyFailed
Indicate Cholesky factorization failure.
Definition ThreadsafeException.h:114
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │ +
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ +
gtsam::Cal3
Common base class for all calibration models.
Definition Cal3.h:69
│ │ │ │ +
gtsam::Cal3DS2_Base
Calibration of a camera with radial distortion.
Definition Cal3DS2_Base.h:42
│ │ │ │ +
gtsam::Cal3DS2_Base::Cal3DS2_Base
Cal3DS2_Base()=default
Default Constructor with only unit focal length.
│ │ │ │ +
gtsam::Cal3DS2_Base::p2
double p2() const
Second tangential distortion coefficient.
Definition Cal3DS2_Base.h:110
│ │ │ │ +
gtsam::Cal3DS2_Base::Dim
static size_t Dim()
return DOF, dimensionality of tangent space
Definition Cal3DS2_Base.h:142
│ │ │ │ +
gtsam::Cal3DS2_Base::clone
virtual boost::shared_ptr< Cal3DS2_Base > clone() const
Definition Cal3DS2_Base.h:149
│ │ │ │ +
gtsam::Cal3DS2_Base::k2
double k2() const
Second distortion coefficient.
Definition Cal3DS2_Base.h:104
│ │ │ │ +
gtsam::Cal3DS2_Base::k1
double k1() const
First distortion coefficient.
Definition Cal3DS2_Base.h:101
│ │ │ │ +
gtsam::Cal3DS2_Base::dim
size_t dim() const override
return DOF, dimensionality of tangent space
Definition Cal3DS2_Base.h:139
│ │ │ │ +
gtsam::Cal3DS2_Base::p1
double p1() const
First tangential distortion coefficient.
Definition Cal3DS2_Base.h:107
│ │ │ │ +
gtsam::Cal3DS2_Base::k
Vector4 k() const
return distortion parameter vector
Definition Cal3DS2_Base.h:113
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,14 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ThreadsafeException.h │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +Cal3DS2_Base.h │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ @@ -16,154 +15,170 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 20#pragma once │ │ │ │ │ 21 │ │ │ │ │ -22#include // for GTSAM_USE_TBB │ │ │ │ │ -23 │ │ │ │ │ -24#include │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ -28#include │ │ │ │ │ -29 │ │ │ │ │ -30#ifdef GTSAM_USE_TBB │ │ │ │ │ -31#include │ │ │ │ │ -32#include │ │ │ │ │ -33#include │ │ │ │ │ -34#endif │ │ │ │ │ -35 │ │ │ │ │ -36namespace _g_t_s_a_m { │ │ │ │ │ -37 │ │ │ │ │ -39template │ │ │ │ │ -_4_0class _T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n: │ │ │ │ │ -41public std::exception │ │ │ │ │ -42{ │ │ │ │ │ -43private: │ │ │ │ │ -44 typedef std::exception Base; │ │ │ │ │ -45#ifdef GTSAM_USE_TBB │ │ │ │ │ -46protected: │ │ │ │ │ -47 typedef std::basic_string, │ │ │ │ │ -48 tbb::tbb_allocator > String; │ │ │ │ │ -49#else │ │ │ │ │ -50protected: │ │ │ │ │ -51 typedef std::string String; │ │ │ │ │ -52#endif │ │ │ │ │ +22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ +24#include │ │ │ │ │ +25 │ │ │ │ │ +26namespace _g_t_s_a_m { │ │ │ │ │ +27 │ │ │ │ │ +_4_2class GTSAM_EXPORT _C_a_l_3_D_S_2___B_a_s_e : public _C_a_l_3 { │ │ │ │ │ +43 protected: │ │ │ │ │ +_4_4 double k1_ = 0.0f, k2_ = 0.0f; │ │ │ │ │ +_4_5 double p1_ = 0.0f, p2_ = 0.0f; │ │ │ │ │ +_4_6 double tol_ = 1e-5; │ │ │ │ │ +47 │ │ │ │ │ +48 public: │ │ │ │ │ +49 enum { dimension = 9 }; │ │ │ │ │ +50 │ │ │ │ │ +52 using shared_ptr = boost::shared_ptr; │ │ │ │ │ 53 │ │ │ │ │ -54protected: │ │ │ │ │ -_5_5 bool _d_y_n_a_m_i_c__; │ │ │ │ │ -_5_6 mutable boost::optional _d_e_s_c_r_i_p_t_i_o_n__; │ │ │ │ │ -57 │ │ │ │ │ -_5_9 _T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n() : │ │ │ │ │ -60 _d_y_n_a_m_i_c__(false) { │ │ │ │ │ -61 } │ │ │ │ │ -62 │ │ │ │ │ -_6_4 _T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n(const _T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n& other) : │ │ │ │ │ -65 Base(other), _d_y_n_a_m_i_c__(false) { │ │ │ │ │ -66 } │ │ │ │ │ -67 │ │ │ │ │ -_6_9 _T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n(const std::string& description) : │ │ │ │ │ -70 _d_y_n_a_m_i_c__(false), _d_e_s_c_r_i_p_t_i_o_n__( │ │ │ │ │ -71 String(description.begin(), description.end())) { │ │ │ │ │ -72 } │ │ │ │ │ -73 │ │ │ │ │ -_7_5 _~_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n() noexcept override { │ │ │ │ │ -76 } │ │ │ │ │ -77 │ │ │ │ │ -78public: │ │ │ │ │ -79 const char* what() const noexcept override { │ │ │ │ │ -80 return _d_e_s_c_r_i_p_t_i_o_n__ ? _d_e_s_c_r_i_p_t_i_o_n__->c_str() : ""; │ │ │ │ │ -81 } │ │ │ │ │ -82}; │ │ │ │ │ -83 │ │ │ │ │ -_8_5class GTSAM_EXPORT _R_u_n_t_i_m_e_E_r_r_o_r_T_h_r_e_a_d_s_a_f_e: public │ │ │ │ │ -_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n { │ │ │ │ │ -86public: │ │ │ │ │ -_8_8 _R_u_n_t_i_m_e_E_r_r_o_r_T_h_r_e_a_d_s_a_f_e(const std::string& description) : │ │ │ │ │ -89 _T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n<_R_u_n_t_i_m_e_E_r_r_o_r_T_h_r_e_a_d_s_a_f_e>(description) { │ │ │ │ │ -90 } │ │ │ │ │ -91}; │ │ │ │ │ +56 │ │ │ │ │ +_5_8 _C_a_l_3_D_S_2___B_a_s_e() = default; │ │ │ │ │ +59 │ │ │ │ │ +60 _C_a_l_3_D_S_2___B_a_s_e(double fx, double fy, double s, double u0, double v0, double │ │ │ │ │ +k1, │ │ │ │ │ +61 double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5) │ │ │ │ │ +62 : _C_a_l_3(fx, fy, s, u0, v0), │ │ │ │ │ +63 k1_(k1), │ │ │ │ │ +64 k2_(k2), │ │ │ │ │ +65 p1_(p1), │ │ │ │ │ +66 p2_(p2), │ │ │ │ │ +67 tol_(tol) {} │ │ │ │ │ +68 │ │ │ │ │ +69 _~_C_a_l_3_D_S_2___B_a_s_e() override {} │ │ │ │ │ +70 │ │ │ │ │ +74 │ │ │ │ │ +75 Cal3DS2_Base(const Vector9& v) │ │ │ │ │ +76 : Cal3(v(0), v(1), v(2), v(3), v(4)), │ │ │ │ │ +77 k1_(v(5)), │ │ │ │ │ +78 k2_(v(6)), │ │ │ │ │ +79 p1_(v(7)), │ │ │ │ │ +80 p2_(v(8)) {} │ │ │ │ │ +81 │ │ │ │ │ +85 │ │ │ │ │ +87 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, │ │ │ │ │ +88 const Cal3DS2_Base& cal); │ │ │ │ │ +89 │ │ │ │ │ +91 void _p_r_i_n_t(const std::string& s = "") const override; │ │ │ │ │ 92 │ │ │ │ │ -_9_4class _O_u_t_O_f_R_a_n_g_e_T_h_r_e_a_d_s_a_f_e: public _T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n │ │ │ │ │ -{ │ │ │ │ │ -95public: │ │ │ │ │ -_9_7 _O_u_t_O_f_R_a_n_g_e_T_h_r_e_a_d_s_a_f_e(const std::string& description) : │ │ │ │ │ -98 _T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n<_O_u_t_O_f_R_a_n_g_e_T_h_r_e_a_d_s_a_f_e>(description) { │ │ │ │ │ -99 } │ │ │ │ │ -100}; │ │ │ │ │ -101 │ │ │ │ │ -_1_0_3class _I_n_v_a_l_i_d_A_r_g_u_m_e_n_t_T_h_r_e_a_d_s_a_f_e: public _T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n< │ │ │ │ │ -104 InvalidArgumentThreadsafe> { │ │ │ │ │ -105public: │ │ │ │ │ -_1_0_7 _I_n_v_a_l_i_d_A_r_g_u_m_e_n_t_T_h_r_e_a_d_s_a_f_e(const std::string& description) : │ │ │ │ │ -108 _T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n<_I_n_v_a_l_i_d_A_r_g_u_m_e_n_t_T_h_r_e_a_d_s_a_f_e>(description) { │ │ │ │ │ -109 } │ │ │ │ │ -110}; │ │ │ │ │ +94 bool equals(const Cal3DS2_Base& K, double tol = 1e-8) const; │ │ │ │ │ +95 │ │ │ │ │ +99 │ │ │ │ │ +_1_0_1 inline double _k_1() const { return k1_; } │ │ │ │ │ +102 │ │ │ │ │ +_1_0_4 inline double _k_2() const { return k2_; } │ │ │ │ │ +105 │ │ │ │ │ +_1_0_7 inline double _p_1() const { return p1_; } │ │ │ │ │ +108 │ │ │ │ │ +_1_1_0 inline double _p_2() const { return p2_; } │ │ │ │ │ 111 │ │ │ │ │ -_1_1_3class _C_h_o_l_e_s_k_y_F_a_i_l_e_d : public _g_t_s_a_m_:_:_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n │ │ │ │ │ -114{ │ │ │ │ │ -115public: │ │ │ │ │ -116 _C_h_o_l_e_s_k_y_F_a_i_l_e_d() noexcept {} │ │ │ │ │ -117 _~_C_h_o_l_e_s_k_y_F_a_i_l_e_d() noexcept override {} │ │ │ │ │ -118}; │ │ │ │ │ -119 │ │ │ │ │ -120} // namespace gtsam │ │ │ │ │ +_1_1_3 Vector4 _k() const { return Vector4(k1_, k2_, p1_, p2_); } │ │ │ │ │ +114 │ │ │ │ │ +116 Vector9 vector() const; │ │ │ │ │ +117 │ │ │ │ │ +125 _P_o_i_n_t_2 uncalibrate(const _P_o_i_n_t_2& p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _9_> Dcal = boost:: │ │ │ │ │ +none, │ │ │ │ │ +126 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dp = boost::none) const; │ │ │ │ │ +127 │ │ │ │ │ +129 _P_o_i_n_t_2 calibrate(const _P_o_i_n_t_2& p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _9_> Dcal = boost:: │ │ │ │ │ +none, │ │ │ │ │ +130 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dp = boost::none) const; │ │ │ │ │ +131 │ │ │ │ │ +133 Matrix2 D2d_intrinsic(const _P_o_i_n_t_2& p) const; │ │ │ │ │ +134 │ │ │ │ │ +136 Matrix29 D2d_calibration(const _P_o_i_n_t_2& p) const; │ │ │ │ │ +137 │ │ │ │ │ +_1_3_9 size_t _d_i_m() const override { return Dim(); } │ │ │ │ │ +140 │ │ │ │ │ +_1_4_2 inline static size_t _D_i_m() { return dimension; } │ │ │ │ │ +143 │ │ │ │ │ +147 │ │ │ │ │ +_1_4_9 virtual boost::shared_ptr _c_l_o_n_e() const { │ │ │ │ │ +150 return boost::shared_ptr(new _C_a_l_3_D_S_2___B_a_s_e(*this)); │ │ │ │ │ +151 } │ │ │ │ │ +152 │ │ │ │ │ +154 │ │ │ │ │ +155 private: │ │ │ │ │ +158 │ │ │ │ │ +_1_6_0 friend class boost::serialization::access; │ │ │ │ │ +161 template │ │ │ │ │ +162 void serialize(Archive& ar, const unsigned int /*version*/) { │ │ │ │ │ +163 ar& boost::serialization::make_nvp( │ │ │ │ │ +164 "Cal3DS2_Base", boost::serialization::base_object(*this)); │ │ │ │ │ +165 ar& BOOST_SERIALIZATION_NVP(k1_); │ │ │ │ │ +166 ar& BOOST_SERIALIZATION_NVP(k2_); │ │ │ │ │ +167 ar& BOOST_SERIALIZATION_NVP(p1_); │ │ │ │ │ +168 ar& BOOST_SERIALIZATION_NVP(p2_); │ │ │ │ │ +169 ar& BOOST_SERIALIZATION_NVP(tol_); │ │ │ │ │ +170 } │ │ │ │ │ +171 │ │ │ │ │ +173}; │ │ │ │ │ +174} │ │ │ │ │ +_P_o_i_n_t_2_._h │ │ │ │ │ +2D Point │ │ │ │ │ +_C_a_l_3_._h │ │ │ │ │ +Common code for all Calibration models. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n │ │ │ │ │ -Base exception type that uses tbb_allocator if GTSAM is compiled with TBB. │ │ │ │ │ -DDeeffiinniittiioonn ThreadsafeException.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_:_:_~_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n │ │ │ │ │ -~ThreadsafeException() noexcept override │ │ │ │ │ -Default destructor doesn't have the noexcept. │ │ │ │ │ -DDeeffiinniittiioonn ThreadsafeException.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_:_:_d_y_n_a_m_i_c__ │ │ │ │ │ -bool dynamic_ │ │ │ │ │ -Whether this object was moved. │ │ │ │ │ -DDeeffiinniittiioonn ThreadsafeException.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_:_:_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n │ │ │ │ │ -ThreadsafeException() │ │ │ │ │ -Default constructor is protected - may only be created from derived classes. │ │ │ │ │ -DDeeffiinniittiioonn ThreadsafeException.h:59 │ │ │ │ │ -_g_t_s_a_m_:_:_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_:_:_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n │ │ │ │ │ -ThreadsafeException(const ThreadsafeException &other) │ │ │ │ │ -Copy constructor is protected - may only be created from derived classes. │ │ │ │ │ -DDeeffiinniittiioonn ThreadsafeException.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_:_:_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n │ │ │ │ │ -ThreadsafeException(const std::string &description) │ │ │ │ │ -Construct with description string. │ │ │ │ │ -DDeeffiinniittiioonn ThreadsafeException.h:69 │ │ │ │ │ -_g_t_s_a_m_:_:_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_:_:_d_e_s_c_r_i_p_t_i_o_n__ │ │ │ │ │ -boost::optional< String > description_ │ │ │ │ │ -Optional description. │ │ │ │ │ -DDeeffiinniittiioonn ThreadsafeException.h:56 │ │ │ │ │ -_g_t_s_a_m_:_:_R_u_n_t_i_m_e_E_r_r_o_r_T_h_r_e_a_d_s_a_f_e │ │ │ │ │ -Thread-safe runtime error exception. │ │ │ │ │ -DDeeffiinniittiioonn ThreadsafeException.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_R_u_n_t_i_m_e_E_r_r_o_r_T_h_r_e_a_d_s_a_f_e_:_:_R_u_n_t_i_m_e_E_r_r_o_r_T_h_r_e_a_d_s_a_f_e │ │ │ │ │ -RuntimeErrorThreadsafe(const std::string &description) │ │ │ │ │ -Construct with a string describing the exception. │ │ │ │ │ -DDeeffiinniittiioonn ThreadsafeException.h:88 │ │ │ │ │ -_g_t_s_a_m_:_:_O_u_t_O_f_R_a_n_g_e_T_h_r_e_a_d_s_a_f_e │ │ │ │ │ -Thread-safe out of range exception. │ │ │ │ │ -DDeeffiinniittiioonn ThreadsafeException.h:94 │ │ │ │ │ -_g_t_s_a_m_:_:_O_u_t_O_f_R_a_n_g_e_T_h_r_e_a_d_s_a_f_e_:_:_O_u_t_O_f_R_a_n_g_e_T_h_r_e_a_d_s_a_f_e │ │ │ │ │ -OutOfRangeThreadsafe(const std::string &description) │ │ │ │ │ -Construct with a string describing the exception. │ │ │ │ │ -DDeeffiinniittiioonn ThreadsafeException.h:97 │ │ │ │ │ -_g_t_s_a_m_:_:_I_n_v_a_l_i_d_A_r_g_u_m_e_n_t_T_h_r_e_a_d_s_a_f_e │ │ │ │ │ -Thread-safe invalid argument exception. │ │ │ │ │ -DDeeffiinniittiioonn ThreadsafeException.h:104 │ │ │ │ │ -_g_t_s_a_m_:_:_I_n_v_a_l_i_d_A_r_g_u_m_e_n_t_T_h_r_e_a_d_s_a_f_e_:_:_I_n_v_a_l_i_d_A_r_g_u_m_e_n_t_T_h_r_e_a_d_s_a_f_e │ │ │ │ │ -InvalidArgumentThreadsafe(const std::string &description) │ │ │ │ │ -Construct with a string describing the exception. │ │ │ │ │ -DDeeffiinniittiioonn ThreadsafeException.h:107 │ │ │ │ │ -_g_t_s_a_m_:_:_C_h_o_l_e_s_k_y_F_a_i_l_e_d │ │ │ │ │ -Indicate Cholesky factorization failure. │ │ │ │ │ -DDeeffiinniittiioonn ThreadsafeException.h:114 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ +Vector2 Point2 │ │ │ │ │ +As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ +typedef Point2 to Vector2... │ │ │ │ │ +DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ +either a fixed size o... │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3 │ │ │ │ │ +Common base class for all calibration models. │ │ │ │ │ +DDeeffiinniittiioonn Cal3.h:69 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_D_S_2___B_a_s_e │ │ │ │ │ +Calibration of a camera with radial distortion. │ │ │ │ │ +DDeeffiinniittiioonn Cal3DS2_Base.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_D_S_2___B_a_s_e_:_:_C_a_l_3_D_S_2___B_a_s_e │ │ │ │ │ +Cal3DS2_Base()=default │ │ │ │ │ +Default Constructor with only unit focal length. │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_D_S_2___B_a_s_e_:_:_p_2 │ │ │ │ │ +double p2() const │ │ │ │ │ +Second tangential distortion coefficient. │ │ │ │ │ +DDeeffiinniittiioonn Cal3DS2_Base.h:110 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_D_S_2___B_a_s_e_:_:_D_i_m │ │ │ │ │ +static size_t Dim() │ │ │ │ │ +return DOF, dimensionality of tangent space │ │ │ │ │ +DDeeffiinniittiioonn Cal3DS2_Base.h:142 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_D_S_2___B_a_s_e_:_:_c_l_o_n_e │ │ │ │ │ +virtual boost::shared_ptr< Cal3DS2_Base > clone() const │ │ │ │ │ +DDeeffiinniittiioonn Cal3DS2_Base.h:149 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_D_S_2___B_a_s_e_:_:_k_2 │ │ │ │ │ +double k2() const │ │ │ │ │ +Second distortion coefficient. │ │ │ │ │ +DDeeffiinniittiioonn Cal3DS2_Base.h:104 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_D_S_2___B_a_s_e_:_:_k_1 │ │ │ │ │ +double k1() const │ │ │ │ │ +First distortion coefficient. │ │ │ │ │ +DDeeffiinniittiioonn Cal3DS2_Base.h:101 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_D_S_2___B_a_s_e_:_:_d_i_m │ │ │ │ │ +size_t dim() const override │ │ │ │ │ +return DOF, dimensionality of tangent space │ │ │ │ │ +DDeeffiinniittiioonn Cal3DS2_Base.h:139 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_D_S_2___B_a_s_e_:_:_p_1 │ │ │ │ │ +double p1() const │ │ │ │ │ +First tangential distortion coefficient. │ │ │ │ │ +DDeeffiinniittiioonn Cal3DS2_Base.h:107 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_D_S_2___B_a_s_e_:_:_k │ │ │ │ │ +Vector4 k() const │ │ │ │ │ +return distortion parameter vector │ │ │ │ │ +DDeeffiinniittiioonn Cal3DS2_Base.h:113 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _b_a_s_e │ │ │ │ │ - * _T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_._h │ │ │ │ │ + * _g_e_o_m_e_t_r_y │ │ │ │ │ + * CCaall33DDSS22__BBaassee..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00041.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00143.html │ │ │ │┄ Files 96% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Value.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/VerticalBlockMatrix.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
Value.h File Reference
│ │ │ │ +
VerticalBlockMatrix.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

The base class for any variable that can be optimized or used in a factor. │ │ │ │ +

A matrix with column blocks of pre-defined sizes. │ │ │ │ More...

│ │ │ │ │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::Value
 This is the base class for any type to be stored in Values. More...
class  gtsam::VerticalBlockMatrix
 This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

The base class for any variable that can be optimized or used in a factor.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Jan 14, 2012
│ │ │ │ +

A matrix with column blocks of pre-defined sizes.

│ │ │ │ +

Used in JacobianFactor and GaussianConditional.

Author
Richard Roberts
│ │ │ │ +
Date
Sep 18, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,29 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Value.h File Reference │ │ │ │ │ -The base class for any variable that can be optimized or used in a factor. │ │ │ │ │ -_M_o_r_e_._._. │ │ │ │ │ +VerticalBlockMatrix.h File Reference │ │ │ │ │ +A matrix with column blocks of pre-defined sizes. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_V_a_l_u_e │ │ │ │ │ -  This is the base class for any type to be stored in _V_a_l_u_e_s. _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +  This class stores a dense matrix and allows it to be accessed as a │ │ │ │ │ + collection of vertical blocks. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -The base class for any variable that can be optimized or used in a factor. │ │ │ │ │ +A matrix with column blocks of pre-defined sizes. │ │ │ │ │ +Used in _J_a_c_o_b_i_a_n_F_a_c_t_o_r and _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l. │ │ │ │ │ Author │ │ │ │ │ Richard Roberts │ │ │ │ │ Date │ │ │ │ │ - Jan 14, 2012 │ │ │ │ │ + Sep 18, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _V_a_l_u_e_._h │ │ │ │ │ + * _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00050_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00650_source.html │ │ │ │┄ Files 92% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/serializationTestHelpers.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/JunctionTree-inst.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
serializationTestHelpers.h
│ │ │ │ +
JunctionTree-inst.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <iostream>
│ │ │ │ -
23#include <sstream>
│ │ │ │ -
24#include <string>
│ │ │ │ -
25
│ │ │ │ -
26#include <gtsam/base/serialization.h>
│ │ │ │ +
21#pragma once
│ │ │ │ +
22
│ │ │ │ +
23#include <gtsam/inference/JunctionTree.h>
│ │ │ │ +
24#include <gtsam/inference/ClusterTree-inst.h>
│ │ │ │ +
25#include <gtsam/symbolic/SymbolicConditional.h>
│ │ │ │ +
26#include <gtsam/symbolic/SymbolicFactor-inst.h>
│ │ │ │
27
│ │ │ │ -
28#include <boost/serialization/serialization.hpp>
│ │ │ │ -
29#include <boost/filesystem.hpp>
│ │ │ │ -
30
│ │ │ │ -
31
│ │ │ │ -
32// whether to print the serialized text to stdout
│ │ │ │ -
33const bool verbose = false;
│ │ │ │ +
28namespace gtsam {
│ │ │ │ +
29
│ │ │ │ +
30template<class BAYESTREE, class GRAPH, class ETREE_NODE>
│ │ │ │ +
│ │ │ │ +
31struct ConstructorTraversalData {
│ │ │ │ +
32 typedef typename JunctionTree<BAYESTREE, GRAPH>::Node Node;
│ │ │ │ +
33 typedef typename JunctionTree<BAYESTREE, GRAPH>::sharedNode sharedNode;
│ │ │ │
34
│ │ │ │ -
35namespace gtsam {
│ │ │ │ -
36namespace serializationTestHelpers {
│ │ │ │ -
37
│ │ │ │ -
38// templated default object creation so we only need to declare one friend (if applicable)
│ │ │ │ -
39template<class T>
│ │ │ │ -
40T create() {
│ │ │ │ -
41 return T();
│ │ │ │ -
42}
│ │ │ │ +
35 ConstructorTraversalData* const parentData;
│ │ │ │ +
36 sharedNode junctionTreeNode;
│ │ │ │ +
37 FastVector<SymbolicConditional::shared_ptr> childSymbolicConditionals;
│ │ │ │ +
38 FastVector<SymbolicFactor::shared_ptr> childSymbolicFactors;
│ │ │ │ +
39
│ │ │ │ +
40 // Small inner class to store symbolic factors
│ │ │ │ +
│ │ │ │ +
41 class SymbolicFactors: public FactorGraph<Factor> {
│ │ │ │ +
42 };
│ │ │ │ +
│ │ │ │
43
│ │ │ │ -
44// Creates or empties a folder in the build folder and returns the relative path
│ │ │ │ -
45inline boost::filesystem::path resetFilesystem(
│ │ │ │ -
46 boost::filesystem::path folder = "actual") {
│ │ │ │ -
47 boost::filesystem::remove_all(folder);
│ │ │ │ -
48 boost::filesystem::create_directory(folder);
│ │ │ │ -
49 return folder;
│ │ │ │ -
50}
│ │ │ │ -
51
│ │ │ │ -
52// Templated round-trip serialization
│ │ │ │ -
53template<class T>
│ │ │ │ -
54void roundtrip(const T& input, T& output) {
│ │ │ │ -
55 std::string serialized = serialize(input);
│ │ │ │ -
56 if (verbose) std::cout << serialized << std::endl << std::endl;
│ │ │ │ -
57 deserialize(serialized, output);
│ │ │ │ -
58}
│ │ │ │ -
59
│ │ │ │ -
60// Templated round-trip serialization using a file
│ │ │ │ -
61template<class T>
│ │ │ │ -
62void roundtripFile(const T& input, T& output) {
│ │ │ │ -
63 boost::filesystem::path path = resetFilesystem()/"graph.dat";
│ │ │ │ -
64 serializeToFile(input, path.string());
│ │ │ │ -
65 deserializeFromFile(path.string(), output);
│ │ │ │ -
66}
│ │ │ │ -
67
│ │ │ │ -
68// This version requires equality operator and uses string and file round-trips
│ │ │ │ -
69template<class T>
│ │ │ │ -
70bool equality(const T& input = T()) {
│ │ │ │ -
71 T output = create<T>(), outputf = create<T>();
│ │ │ │ -
72 roundtrip<T>(input,output);
│ │ │ │ -
73 roundtripFile<T>(input,outputf);
│ │ │ │ -
74 return (input==output) && (input==outputf);
│ │ │ │ -
75}
│ │ │ │ -
76
│ │ │ │ -
77// This version requires Testable
│ │ │ │ -
78template<class T>
│ │ │ │ -
79bool equalsObj(const T& input = T()) {
│ │ │ │ -
80 T output = create<T>();
│ │ │ │ -
81 roundtrip<T>(input,output);
│ │ │ │ -
82 return assert_equal(input, output);
│ │ │ │ -
83}
│ │ │ │ -
84
│ │ │ │ -
85// De-referenced version for pointers, requires equals method
│ │ │ │ -
86template<class T>
│ │ │ │ -
87bool equalsDereferenced(const T& input) {
│ │ │ │ -
88 T output = create<T>();
│ │ │ │ -
89 roundtrip<T>(input,output);
│ │ │ │ -
90 return input->equals(*output);
│ │ │ │ -
91}
│ │ │ │ -
92
│ │ │ │ -
93// Templated round-trip serialization using XML
│ │ │ │ -
94template<class T>
│ │ │ │ -
95void roundtripXML(const T& input, T& output) {
│ │ │ │ -
96 std::string serialized = serializeXML<T>(input);
│ │ │ │ -
97 if (verbose) std::cout << serialized << std::endl << std::endl;
│ │ │ │ -
98 deserializeXML(serialized, output);
│ │ │ │ -
99}
│ │ │ │ -
100
│ │ │ │ -
101// Templated round-trip serialization using XML File
│ │ │ │ -
102template<class T>
│ │ │ │ -
103void roundtripXMLFile(const T& input, T& output) {
│ │ │ │ -
104 boost::filesystem::path path = resetFilesystem()/"graph.xml";
│ │ │ │ -
105 serializeToXMLFile(input, path.string());
│ │ │ │ -
106 deserializeFromXMLFile(path.string(), output);
│ │ │ │ -
107}
│ │ │ │ -
108
│ │ │ │ -
109// This version requires equality operator
│ │ │ │ -
110template<class T>
│ │ │ │ -
111bool equalityXML(const T& input = T()) {
│ │ │ │ -
112 T output = create<T>(), outputf = create<T>();
│ │ │ │ -
113 roundtripXML<T>(input,output);
│ │ │ │ -
114 roundtripXMLFile<T>(input,outputf);
│ │ │ │ -
115 return (input==output) && (input==outputf);
│ │ │ │ -
116}
│ │ │ │ -
117
│ │ │ │ -
118// This version requires Testable
│ │ │ │ -
119template<class T>
│ │ │ │ -
120bool equalsXML(const T& input = T()) {
│ │ │ │ -
121 T output = create<T>();
│ │ │ │ -
122 roundtripXML<T>(input,output);
│ │ │ │ -
123 return assert_equal(input, output);
│ │ │ │ -
124}
│ │ │ │ -
125
│ │ │ │ -
126// This version is for pointers, requires equals method
│ │ │ │ -
127template<class T>
│ │ │ │ -
128bool equalsDereferencedXML(const T& input = T()) {
│ │ │ │ -
129 T output = create<T>();
│ │ │ │ -
130 roundtripXML<T>(input,output);
│ │ │ │ -
131 return input->equals(*output);
│ │ │ │ -
132}
│ │ │ │ -
133
│ │ │ │ -
134// Templated round-trip serialization using XML
│ │ │ │ -
135template<class T>
│ │ │ │ -
136void roundtripBinary(const T& input, T& output) {
│ │ │ │ -
137 std::string serialized = serializeBinary<T>(input);
│ │ │ │ -
138 if (verbose) std::cout << serialized << std::endl << std::endl;
│ │ │ │ -
139 deserializeBinary(serialized, output);
│ │ │ │ -
140}
│ │ │ │ -
141
│ │ │ │ -
142// Templated round-trip serialization using Binary file
│ │ │ │ -
143template<class T>
│ │ │ │ -
144void roundtripBinaryFile(const T& input, T& output) {
│ │ │ │ -
145 boost::filesystem::path path = resetFilesystem()/"graph.bin";
│ │ │ │ -
146 serializeToBinaryFile(input, path.string());
│ │ │ │ -
147 deserializeFromBinaryFile(path.string(), output);
│ │ │ │ -
148}
│ │ │ │ -
149
│ │ │ │ -
150// This version requires equality operator
│ │ │ │ -
151template<class T>
│ │ │ │ -
152bool equalityBinary(const T& input = T()) {
│ │ │ │ -
153 T output = create<T>(), outputf = create<T>();
│ │ │ │ -
154 roundtripBinary<T>(input,output);
│ │ │ │ -
155 roundtripBinaryFile<T>(input,outputf);
│ │ │ │ -
156 return (input==output) && (input==outputf);
│ │ │ │ -
157}
│ │ │ │ -
158
│ │ │ │ -
159// This version requires Testable
│ │ │ │ -
160template<class T>
│ │ │ │ -
161bool equalsBinary(const T& input = T()) {
│ │ │ │ -
162 T output = create<T>();
│ │ │ │ -
163 roundtripBinary<T>(input,output);
│ │ │ │ -
164 return assert_equal(input, output);
│ │ │ │ -
165}
│ │ │ │ -
166
│ │ │ │ -
167// This version is for pointers, requires equals method
│ │ │ │ -
168template<class T>
│ │ │ │ -
169bool equalsDereferencedBinary(const T& input = T()) {
│ │ │ │ -
170 T output = create<T>();
│ │ │ │ -
171 roundtripBinary<T>(input,output);
│ │ │ │ -
172 return input->equals(*output);
│ │ │ │ -
173}
│ │ │ │ -
174
│ │ │ │ -
175} // \namespace serializationTestHelpers
│ │ │ │ -
176} // \namespace gtsam
│ │ │ │ -
serialization.h
Convenience functions for serializing data structures via boost.serialization.
│ │ │ │ +
44 ConstructorTraversalData(ConstructorTraversalData* _parentData) :
│ │ │ │ +
45 parentData(_parentData) {
│ │ │ │ +
46 }
│ │ │ │ +
47
│ │ │ │ +
48 // Pre-order visitor function
│ │ │ │ +
49 static ConstructorTraversalData ConstructorTraversalVisitorPre(
│ │ │ │ +
50 const boost::shared_ptr<ETREE_NODE>& node,
│ │ │ │ +
51 ConstructorTraversalData& parentData) {
│ │ │ │ +
52 // On the pre-order pass, before children have been visited, we just set up
│ │ │ │ +
53 // a traversal data structure with its own JT node, and create a child
│ │ │ │ +
54 // pointer in its parent.
│ │ │ │ +
55 ConstructorTraversalData myData = ConstructorTraversalData(&parentData);
│ │ │ │ +
56 myData.junctionTreeNode =
│ │ │ │ +
57 boost::make_shared<Node>(node->key, node->factors);
│ │ │ │ +
58 parentData.junctionTreeNode->addChild(myData.junctionTreeNode);
│ │ │ │ +
59 return myData;
│ │ │ │ +
60 }
│ │ │ │ +
61
│ │ │ │ +
62 // Post-order visitor function
│ │ │ │ +
63 static void ConstructorTraversalVisitorPostAlg2(
│ │ │ │ +
64 const boost::shared_ptr<ETREE_NODE>& ETreeNode,
│ │ │ │ +
65 const ConstructorTraversalData& myData) {
│ │ │ │ +
66 // In this post-order visitor, we combine the symbolic elimination results
│ │ │ │ +
67 // from the elimination tree children and symbolically eliminate the current
│ │ │ │ +
68 // elimination tree node. We then check whether each of our elimination
│ │ │ │ +
│ │ │ │ +
69 // tree child nodes should be merged with us. The check for this is that
│ │ │ │ +
70 // our number of symbolic elimination parents is exactly 1 less than
│ │ │ │ +
71 // our child's symbolic elimination parents - this condition indicates that
│ │ │ │ +
72 // eliminating the current node did not introduce any parents beyond those
│ │ │ │ +
73 // already in the child->
│ │ │ │ +
74
│ │ │ │ +
75 // Do symbolic elimination for this node
│ │ │ │ +
76 SymbolicFactors symbolicFactors;
│ │ │ │ +
77 symbolicFactors.reserve(
│ │ │ │ +
78 ETreeNode->factors.size() + myData.childSymbolicFactors.size());
│ │ │ │ +
79 // Add ETree node factors
│ │ │ │ +
80 symbolicFactors += ETreeNode->factors;
│ │ │ │ +
81 // Add symbolic factors passed up from children
│ │ │ │ +
82 symbolicFactors += myData.childSymbolicFactors;
│ │ │ │ +
83
│ │ │ │ +
84 Ordering keyAsOrdering;
│ │ │ │ +
85 keyAsOrdering.push_back(ETreeNode->key);
│ │ │ │ +
86 SymbolicConditional::shared_ptr myConditional;
│ │ │ │ +
87 SymbolicFactor::shared_ptr mySeparatorFactor;
│ │ │ │ +
88 boost::tie(myConditional, mySeparatorFactor) = internal::EliminateSymbolic(
│ │ │ │ +
89 symbolicFactors, keyAsOrdering);
│ │ │ │ +
90
│ │ │ │ +
91 // Store symbolic elimination results in the parent
│ │ │ │ +
92 myData.parentData->childSymbolicConditionals.push_back(myConditional);
│ │ │ │ +
93 myData.parentData->childSymbolicFactors.push_back(mySeparatorFactor);
│ │ │ │ +
94
│ │ │ │ +
95 sharedNode node = myData.junctionTreeNode;
│ │ │ │ +
96 const FastVector<SymbolicConditional::shared_ptr>& childConditionals =
│ │ │ │ +
97 myData.childSymbolicConditionals;
│ │ │ │ +
98 node->problemSize_ = (int) (myConditional->size() * symbolicFactors.size());
│ │ │ │ +
99
│ │ │ │ +
100 // Merge our children if they are in our clique - if our conditional has
│ │ │ │ +
101 // exactly one fewer parent than our child's conditional.
│ │ │ │ +
102 const size_t myNrParents = myConditional->nrParents();
│ │ │ │ +
103 const size_t nrChildren = node->nrChildren();
│ │ │ │ +
104 assert(childConditionals.size() == nrChildren);
│ │ │ │ +
105
│ │ │ │ +
106 // decide which children to merge, as index into children
│ │ │ │ +
107 std::vector<size_t> nrFrontals = node->nrFrontalsOfChildren();
│ │ │ │ +
108 std::vector<bool> merge(nrChildren, false);
│ │ │ │ +
109 size_t myNrFrontals = 1;
│ │ │ │ +
110 for (size_t i = 0;i<nrChildren;i++){
│ │ │ │ +
111 // Check if we should merge the i^th child
│ │ │ │ +
112 if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) {
│ │ │ │ +
113 // Increment number of frontal variables
│ │ │ │ +
114 myNrFrontals += nrFrontals[i];
│ │ │ │ +
115 merge[i] = true;
│ │ │ │ +
116 }
│ │ │ │ +
117 }
│ │ │ │ +
118
│ │ │ │ +
119 // now really merge
│ │ │ │ +
120 node->mergeChildren(merge);
│ │ │ │ +
121 }
│ │ │ │ +
122};
│ │ │ │ +
123
│ │ │ │ +
124/* ************************************************************************* */
│ │ │ │ +
125template<class BAYESTREE, class GRAPH>
│ │ │ │ +
126template<class ETREE_BAYESNET, class ETREE_GRAPH>
│ │ │ │ +
│ │ │ │ +
127JunctionTree<BAYESTREE, GRAPH>::JunctionTree(
│ │ │ │ +
128 const EliminationTree<ETREE_BAYESNET, ETREE_GRAPH>& eliminationTree) {
│ │ │ │ +
129 gttic(JunctionTree_FromEliminationTree);
│ │ │ │ +
130 // Here we rely on the BayesNet having been produced by this elimination tree,
│ │ │ │ +
131 // such that the conditionals are arranged in DFS post-order. We traverse the
│ │ │ │ +
132 // elimination tree, and inspect the symbolic conditional corresponding to
│ │ │ │ +
133 // each node. The elimination tree node is added to the same clique with its
│ │ │ │ +
134 // parent if it has exactly one more Bayes net conditional parent than
│ │ │ │ +
135 // does its elimination tree parent.
│ │ │ │ +
136
│ │ │ │ +
137 // Traverse the elimination tree, doing symbolic elimination and merging nodes
│ │ │ │ +
138 // as we go. Gather the created junction tree roots in a dummy Node.
│ │ │ │ +
139 typedef typename EliminationTree<ETREE_BAYESNET, ETREE_GRAPH>::Node ETreeNode;
│ │ │ │ +
140 typedef ConstructorTraversalData<BAYESTREE, GRAPH, ETreeNode> Data;
│ │ │ │ +
141 Data rootData(0);
│ │ │ │ +
142 // Make a dummy node to gather the junction tree roots
│ │ │ │ +
143 rootData.junctionTreeNode = boost::make_shared<typename Base::Node>();
│ │ │ │ +
144 treeTraversal::DepthFirstForest(eliminationTree, rootData,
│ │ │ │ +
145 Data::ConstructorTraversalVisitorPre,
│ │ │ │ +
146 Data::ConstructorTraversalVisitorPostAlg2);
│ │ │ │ +
147
│ │ │ │ +
148 // Assign roots from the dummy node
│ │ │ │ +
149 this->addChildrenAsRoots(rootData.junctionTreeNode);
│ │ │ │ +
150
│ │ │ │ +
151 // Transfer remaining factors from elimination tree
│ │ │ │ +
152 Base::remainingFactors_ = eliminationTree.remainingFactors();
│ │ │ │ +
153}
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
154
│ │ │ │ +
155} // namespace gtsam
│ │ │ │ +
│ │ │ │ +
JunctionTree.h
The junction tree.
│ │ │ │ +
ClusterTree-inst.h
Collects factorgraph fragments defined on variable clusters, arranged in a tree.
│ │ │ │ + │ │ │ │ + │ │ │ │ +
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::deserializeFromXMLFile
bool deserializeFromXMLFile(const std::string &filename, T &output, const std::string &name="data")
deserializes from an XML file
Definition serialization.h:174
│ │ │ │ -
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition serialization.h:113
│ │ │ │ -
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
equals with an tolerance, prints out message if unequal
Definition Matrix.cpp:43
│ │ │ │ -
gtsam::deserializeBinary
void deserializeBinary(const std::string &serialized, T &output, const std::string &name="data")
deserializes from a string in binary
Definition serialization.h:266
│ │ │ │ -
gtsam::deserializeXML
void deserializeXML(const std::string &serialized, T &output, const std::string &name="data")
deserializes from a string in XML
Definition serialization.h:192
│ │ │ │ -
gtsam::serializeToXMLFile
bool serializeToXMLFile(const T &input, const std::string &filename, const std::string &name="data")
serializes to an XML file
Definition serialization.h:163
│ │ │ │ -
gtsam::serializeToFile
bool serializeToFile(const T &input, const std::string &filename)
serializes to a file
Definition serialization.h:93
│ │ │ │ -
gtsam::deserialize
void deserialize(const std::string &serialized, T &output)
deserializes from a string
Definition serialization.h:119
│ │ │ │ -
gtsam::serializeToBinaryFile
bool serializeToBinaryFile(const T &input, const std::string &filename, const std::string &name="data")
serializes to a binary file
Definition serialization.h:237
│ │ │ │ -
gtsam::deserializeFromFile
bool deserializeFromFile(const std::string &filename, T &output)
deserializes from a file
Definition serialization.h:103
│ │ │ │ -
gtsam::deserializeFromBinaryFile
bool deserializeFromBinaryFile(const std::string &filename, T &output, const std::string &name="data")
deserializes from a binary file
Definition serialization.h:248
│ │ │ │ +
gtsam::treeTraversal::DepthFirstForest
void DepthFirstForest(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, VISITOR_POST &visitorPost)
Traverse a forest depth-first with pre-order and post-order visits.
Definition treeTraversal-inst.h:77
│ │ │ │ +
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ +
gtsam::ClusterTree::Cluster
A Cluster is just a collection of factors.
Definition ClusterTree.h:36
│ │ │ │ +
gtsam::EliminationTree
An elimination tree is a data structure used intermediately during elimination.
Definition EliminationTree.h:52
│ │ │ │ +
gtsam::EliminationTree::remainingFactors
const FastVector< sharedFactor > & remainingFactors() const
Return the remaining factors that are not pulled into elimination.
Definition EliminationTree.h:154
│ │ │ │ +
gtsam::EliminationTree::Node
Definition EliminationTree.h:66
│ │ │ │ +
gtsam::ConstructorTraversalData
Definition JunctionTree-inst.h:31
│ │ │ │ +
gtsam::ConstructorTraversalData::SymbolicFactors
Definition JunctionTree-inst.h:41
│ │ │ │ +
gtsam::JunctionTree
A JunctionTree is a cluster tree, a set of variable clusters with factors, arranged in a tree,...
Definition JunctionTree.h:50
│ │ │ │ +
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
gtsam::SymbolicConditional::shared_ptr
boost::shared_ptr< This > shared_ptr
Typedef to the conditional base class.
Definition SymbolicConditional.h:44
│ │ │ │ +
gtsam::SymbolicFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
Overriding the shared_ptr typedef.
Definition SymbolicFactor.h:48
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,239 +1,219 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -serializationTestHelpers.h │ │ │ │ │ +JunctionTree-inst.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include │ │ │ │ │ -23#include │ │ │ │ │ -24#include │ │ │ │ │ -25 │ │ │ │ │ -26#include <_g_t_s_a_m_/_b_a_s_e_/_s_e_r_i_a_l_i_z_a_t_i_o_n_._h> │ │ │ │ │ +21#pragma once │ │ │ │ │ +22 │ │ │ │ │ +23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_J_u_n_c_t_i_o_n_T_r_e_e_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_C_l_u_s_t_e_r_T_r_e_e_-_i_n_s_t_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_s_y_m_b_o_l_i_c_/_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_s_y_m_b_o_l_i_c_/_S_y_m_b_o_l_i_c_F_a_c_t_o_r_-_i_n_s_t_._h> │ │ │ │ │ 27 │ │ │ │ │ -28#include │ │ │ │ │ -29#include │ │ │ │ │ -30 │ │ │ │ │ -31 │ │ │ │ │ -32// whether to print the serialized text to stdout │ │ │ │ │ -33const bool verbose = false; │ │ │ │ │ +28namespace _g_t_s_a_m { │ │ │ │ │ +29 │ │ │ │ │ +30template │ │ │ │ │ +_3_1struct _C_o_n_s_t_r_u_c_t_o_r_T_r_a_v_e_r_s_a_l_D_a_t_a { │ │ │ │ │ +32 typedef typename _J_u_n_c_t_i_o_n_T_r_e_e_<_B_A_Y_E_S_T_R_E_E_,_ _G_R_A_P_H_>_:_:_N_o_d_e _N_o_d_e; │ │ │ │ │ +33 typedef typename JunctionTree::sharedNode sharedNode; │ │ │ │ │ 34 │ │ │ │ │ -35namespace _g_t_s_a_m { │ │ │ │ │ -36namespace serializationTestHelpers { │ │ │ │ │ -37 │ │ │ │ │ -38// templated default object creation so we only need to declare one friend │ │ │ │ │ -(if applicable) │ │ │ │ │ -39template │ │ │ │ │ -40T create() { │ │ │ │ │ -41 return T(); │ │ │ │ │ -42} │ │ │ │ │ +35 _C_o_n_s_t_r_u_c_t_o_r_T_r_a_v_e_r_s_a_l_D_a_t_a* const parentData; │ │ │ │ │ +36 sharedNode junctionTreeNode; │ │ │ │ │ +37 _F_a_s_t_V_e_c_t_o_r_<_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r_> childSymbolicConditionals; │ │ │ │ │ +38 _F_a_s_t_V_e_c_t_o_r_<_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r_> childSymbolicFactors; │ │ │ │ │ +39 │ │ │ │ │ +40 // Small inner class to store symbolic factors │ │ │ │ │ +_4_1 class _S_y_m_b_o_l_i_c_F_a_c_t_o_r_s: public _F_a_c_t_o_r_G_r_a_p_h { │ │ │ │ │ +42 }; │ │ │ │ │ 43 │ │ │ │ │ -44// Creates or empties a folder in the build folder and returns the relative │ │ │ │ │ -path │ │ │ │ │ -45inline boost::filesystem::path resetFilesystem( │ │ │ │ │ -46 boost::filesystem::path folder = "actual") { │ │ │ │ │ -47 boost::filesystem::remove_all(folder); │ │ │ │ │ -48 boost::filesystem::create_directory(folder); │ │ │ │ │ -49 return folder; │ │ │ │ │ -50} │ │ │ │ │ -51 │ │ │ │ │ -52// Templated round-trip serialization │ │ │ │ │ -53template │ │ │ │ │ -54void roundtrip(const T& input, T& output) { │ │ │ │ │ -55 std::string serialized = _s_e_r_i_a_l_i_z_e(input); │ │ │ │ │ -56 if (verbose) std::cout << serialized << std::endl << std::endl; │ │ │ │ │ -57 _d_e_s_e_r_i_a_l_i_z_e(serialized, output); │ │ │ │ │ -58} │ │ │ │ │ -59 │ │ │ │ │ -60// Templated round-trip serialization using a file │ │ │ │ │ -61template │ │ │ │ │ -62void roundtripFile(const T& input, T& output) { │ │ │ │ │ -63 boost::filesystem::path path = resetFilesystem()/"graph.dat"; │ │ │ │ │ -64 _s_e_r_i_a_l_i_z_e_T_o_F_i_l_e(input, path.string()); │ │ │ │ │ -65 _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_F_i_l_e(path.string(), output); │ │ │ │ │ -66} │ │ │ │ │ -67 │ │ │ │ │ -68// This version requires equality operator and uses string and file round- │ │ │ │ │ -trips │ │ │ │ │ -69template │ │ │ │ │ -70bool equality(const T& input = T()) { │ │ │ │ │ -71 T output = create(), outputf = create(); │ │ │ │ │ -72 roundtrip(input,output); │ │ │ │ │ -73 roundtripFile(input,outputf); │ │ │ │ │ -74 return (input==output) && (input==outputf); │ │ │ │ │ -75} │ │ │ │ │ -76 │ │ │ │ │ -77// This version requires Testable │ │ │ │ │ -78template │ │ │ │ │ -79bool equalsObj(const T& input = T()) { │ │ │ │ │ -80 T output = create(); │ │ │ │ │ -81 roundtrip(input,output); │ │ │ │ │ -82 return _a_s_s_e_r_t___e_q_u_a_l(input, output); │ │ │ │ │ -83} │ │ │ │ │ -84 │ │ │ │ │ -85// De-referenced version for pointers, requires equals method │ │ │ │ │ -86template │ │ │ │ │ -87bool equalsDereferenced(const T& input) { │ │ │ │ │ -88 T output = create(); │ │ │ │ │ -89 roundtrip(input,output); │ │ │ │ │ -90 return input->equals(*output); │ │ │ │ │ -91} │ │ │ │ │ -92 │ │ │ │ │ -93// Templated round-trip serialization using XML │ │ │ │ │ -94template │ │ │ │ │ -95void roundtripXML(const T& input, T& output) { │ │ │ │ │ -96 std::string serialized = serializeXML(input); │ │ │ │ │ -97 if (verbose) std::cout << serialized << std::endl << std::endl; │ │ │ │ │ -98 _d_e_s_e_r_i_a_l_i_z_e_X_M_L(serialized, output); │ │ │ │ │ -99} │ │ │ │ │ -100 │ │ │ │ │ -101// Templated round-trip serialization using XML File │ │ │ │ │ -102template │ │ │ │ │ -103void roundtripXMLFile(const T& input, T& output) { │ │ │ │ │ -104 boost::filesystem::path path = resetFilesystem()/"graph.xml"; │ │ │ │ │ -105 _s_e_r_i_a_l_i_z_e_T_o_X_M_L_F_i_l_e(input, path.string()); │ │ │ │ │ -106 _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_X_M_L_F_i_l_e(path.string(), output); │ │ │ │ │ -107} │ │ │ │ │ -108 │ │ │ │ │ -109// This version requires equality operator │ │ │ │ │ -110template │ │ │ │ │ -111bool equalityXML(const T& input = T()) { │ │ │ │ │ -112 T output = create(), outputf = create(); │ │ │ │ │ -113 roundtripXML(input,output); │ │ │ │ │ -114 roundtripXMLFile(input,outputf); │ │ │ │ │ -115 return (input==output) && (input==outputf); │ │ │ │ │ -116} │ │ │ │ │ -117 │ │ │ │ │ -118// This version requires Testable │ │ │ │ │ -119template │ │ │ │ │ -120bool equalsXML(const T& input = T()) { │ │ │ │ │ -121 T output = create(); │ │ │ │ │ -122 roundtripXML(input,output); │ │ │ │ │ -123 return _a_s_s_e_r_t___e_q_u_a_l(input, output); │ │ │ │ │ -124} │ │ │ │ │ -125 │ │ │ │ │ -126// This version is for pointers, requires equals method │ │ │ │ │ -127template │ │ │ │ │ -128bool equalsDereferencedXML(const T& input = T()) { │ │ │ │ │ -129 T output = create(); │ │ │ │ │ -130 roundtripXML(input,output); │ │ │ │ │ -131 return input->equals(*output); │ │ │ │ │ -132} │ │ │ │ │ -133 │ │ │ │ │ -134// Templated round-trip serialization using XML │ │ │ │ │ -135template │ │ │ │ │ -136void roundtripBinary(const T& input, T& output) { │ │ │ │ │ -137 std::string serialized = serializeBinary(input); │ │ │ │ │ -138 if (verbose) std::cout << serialized << std::endl << std::endl; │ │ │ │ │ -139 _d_e_s_e_r_i_a_l_i_z_e_B_i_n_a_r_y(serialized, output); │ │ │ │ │ -140} │ │ │ │ │ -141 │ │ │ │ │ -142// Templated round-trip serialization using Binary file │ │ │ │ │ -143template │ │ │ │ │ -144void roundtripBinaryFile(const T& input, T& output) { │ │ │ │ │ -145 boost::filesystem::path path = resetFilesystem()/"graph.bin"; │ │ │ │ │ -146 _s_e_r_i_a_l_i_z_e_T_o_B_i_n_a_r_y_F_i_l_e(input, path.string()); │ │ │ │ │ -147 _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_B_i_n_a_r_y_F_i_l_e(path.string(), output); │ │ │ │ │ -148} │ │ │ │ │ -149 │ │ │ │ │ -150// This version requires equality operator │ │ │ │ │ -151template │ │ │ │ │ -152bool equalityBinary(const T& input = T()) { │ │ │ │ │ -153 T output = create(), outputf = create(); │ │ │ │ │ -154 roundtripBinary(input,output); │ │ │ │ │ -155 roundtripBinaryFile(input,outputf); │ │ │ │ │ -156 return (input==output) && (input==outputf); │ │ │ │ │ -157} │ │ │ │ │ -158 │ │ │ │ │ -159// This version requires Testable │ │ │ │ │ -160template │ │ │ │ │ -161bool equalsBinary(const T& input = T()) { │ │ │ │ │ -162 T output = create(); │ │ │ │ │ -163 roundtripBinary(input,output); │ │ │ │ │ -164 return _a_s_s_e_r_t___e_q_u_a_l(input, output); │ │ │ │ │ -165} │ │ │ │ │ -166 │ │ │ │ │ -167// This version is for pointers, requires equals method │ │ │ │ │ -168template │ │ │ │ │ -169bool equalsDereferencedBinary(const T& input = T()) { │ │ │ │ │ -170 T output = create(); │ │ │ │ │ -171 roundtripBinary(input,output); │ │ │ │ │ -172 return input->equals(*output); │ │ │ │ │ -173} │ │ │ │ │ -174 │ │ │ │ │ -175} // \namespace serializationTestHelpers │ │ │ │ │ -176} // \namespace gtsam │ │ │ │ │ -_s_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ -Convenience functions for serializing data structures via boost.serialization. │ │ │ │ │ +44 _C_o_n_s_t_r_u_c_t_o_r_T_r_a_v_e_r_s_a_l_D_a_t_a(_C_o_n_s_t_r_u_c_t_o_r_T_r_a_v_e_r_s_a_l_D_a_t_a* _parentData) : │ │ │ │ │ +45 parentData(_parentData) { │ │ │ │ │ +46 } │ │ │ │ │ +47 │ │ │ │ │ +48 // Pre-order visitor function │ │ │ │ │ +49 static _C_o_n_s_t_r_u_c_t_o_r_T_r_a_v_e_r_s_a_l_D_a_t_a ConstructorTraversalVisitorPre( │ │ │ │ │ +50 const boost::shared_ptr& node, │ │ │ │ │ +51 _C_o_n_s_t_r_u_c_t_o_r_T_r_a_v_e_r_s_a_l_D_a_t_a& parentData) { │ │ │ │ │ +52 // On the pre-order pass, before children have been visited, we just set up │ │ │ │ │ +53 // a traversal data structure with its own JT node, and create a child │ │ │ │ │ +54 // pointer in its parent. │ │ │ │ │ +55 _C_o_n_s_t_r_u_c_t_o_r_T_r_a_v_e_r_s_a_l_D_a_t_a myData = _C_o_n_s_t_r_u_c_t_o_r_T_r_a_v_e_r_s_a_l_D_a_t_a(&parentData); │ │ │ │ │ +56 myData.junctionTreeNode = │ │ │ │ │ +57 boost::make_shared(node->key, node->factors); │ │ │ │ │ +58 parentData.junctionTreeNode->addChild(myData.junctionTreeNode); │ │ │ │ │ +59 return myData; │ │ │ │ │ +60 } │ │ │ │ │ +61 │ │ │ │ │ +62 // Post-order visitor function │ │ │ │ │ +63 static void ConstructorTraversalVisitorPostAlg2( │ │ │ │ │ +64 const boost::shared_ptr& ETreeNode, │ │ │ │ │ +65 const ConstructorTraversalData& myData) { │ │ │ │ │ +66 // In this post-order visitor, we combine the symbolic elimination results │ │ │ │ │ +67 // from the elimination tree children and symbolically eliminate the current │ │ │ │ │ +68 // elimination tree node. We then check whether each of our elimination │ │ │ │ │ +_6_9 // tree child nodes should be merged with us. The check for this is that │ │ │ │ │ +70 // our number of symbolic elimination parents is exactly 1 less than │ │ │ │ │ +71 // our child's symbolic elimination parents - this condition indicates that │ │ │ │ │ +72 // eliminating the current node did not introduce any parents beyond those │ │ │ │ │ +73 // already in the child-> │ │ │ │ │ +74 │ │ │ │ │ +75 // Do symbolic elimination for this node │ │ │ │ │ +76 SymbolicFactors symbolicFactors; │ │ │ │ │ +77 symbolicFactors.reserve( │ │ │ │ │ +78 ETreeNode->factors.size() + myData.childSymbolicFactors.size()); │ │ │ │ │ +79 // Add ETree node factors │ │ │ │ │ +80 symbolicFactors += ETreeNode->factors; │ │ │ │ │ +81 // Add symbolic factors passed up from children │ │ │ │ │ +82 symbolicFactors += myData.childSymbolicFactors; │ │ │ │ │ +83 │ │ │ │ │ +84 _O_r_d_e_r_i_n_g keyAsOrdering; │ │ │ │ │ +85 keyAsOrdering.push_back(ETreeNode->key); │ │ │ │ │ +86 _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r myConditional; │ │ │ │ │ +87 _S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r mySeparatorFactor; │ │ │ │ │ +88 boost::tie(myConditional, mySeparatorFactor) = internal::EliminateSymbolic( │ │ │ │ │ +89 symbolicFactors, keyAsOrdering); │ │ │ │ │ +90 │ │ │ │ │ +91 // Store symbolic elimination results in the parent │ │ │ │ │ +92 myData.parentData->childSymbolicConditionals.push_back(myConditional); │ │ │ │ │ +93 myData.parentData->childSymbolicFactors.push_back(mySeparatorFactor); │ │ │ │ │ +94 │ │ │ │ │ +95 sharedNode node = myData.junctionTreeNode; │ │ │ │ │ +96 const _F_a_s_t_V_e_c_t_o_r_<_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r_>& childConditionals = │ │ │ │ │ +97 myData.childSymbolicConditionals; │ │ │ │ │ +98 node->problemSize_ = (int) (myConditional->size() * symbolicFactors.size()); │ │ │ │ │ +99 │ │ │ │ │ +100 // Merge our children if they are in our clique - if our conditional has │ │ │ │ │ +101 // exactly one fewer parent than our child's conditional. │ │ │ │ │ +102 const size_t myNrParents = myConditional->nrParents(); │ │ │ │ │ +103 const size_t nrChildren = node->nrChildren(); │ │ │ │ │ +104 assert(childConditionals.size() == nrChildren); │ │ │ │ │ +105 │ │ │ │ │ +106 // decide which children to merge, as index into children │ │ │ │ │ +107 std::vector nrFrontals = node->nrFrontalsOfChildren(); │ │ │ │ │ +108 std::vector merge(nrChildren, false); │ │ │ │ │ +109 size_t myNrFrontals = 1; │ │ │ │ │ +110 for (size_t i = 0;inrParents()) { │ │ │ │ │ +113 // Increment number of frontal variables │ │ │ │ │ +114 myNrFrontals += nrFrontals[i]; │ │ │ │ │ +115 merge[i] = true; │ │ │ │ │ +116 } │ │ │ │ │ +117 } │ │ │ │ │ +118 │ │ │ │ │ +119 // now really merge │ │ │ │ │ +120 node->mergeChildren(merge); │ │ │ │ │ +121 } │ │ │ │ │ +122}; │ │ │ │ │ +123 │ │ │ │ │ +124/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +125template │ │ │ │ │ +126template │ │ │ │ │ +_1_2_7_J_u_n_c_t_i_o_n_T_r_e_e_<_B_A_Y_E_S_T_R_E_E_,_ _G_R_A_P_H_>_:_:_J_u_n_c_t_i_o_n_T_r_e_e( │ │ │ │ │ +128 const _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_E_T_R_E_E___B_A_Y_E_S_N_E_T_,_ _E_T_R_E_E___G_R_A_P_H_>& eliminationTree) { │ │ │ │ │ +129 gttic(JunctionTree_FromEliminationTree); │ │ │ │ │ +130 // Here we rely on the BayesNet having been produced by this elimination │ │ │ │ │ +tree, │ │ │ │ │ +131 // such that the conditionals are arranged in DFS post-order. We traverse │ │ │ │ │ +the │ │ │ │ │ +132 // elimination tree, and inspect the symbolic conditional corresponding to │ │ │ │ │ +133 // each node. The elimination tree node is added to the same clique with │ │ │ │ │ +its │ │ │ │ │ +134 // parent if it has exactly one more Bayes net conditional parent than │ │ │ │ │ +135 // does its elimination tree parent. │ │ │ │ │ +136 │ │ │ │ │ +137 // Traverse the elimination tree, doing symbolic elimination and merging │ │ │ │ │ +nodes │ │ │ │ │ +138 // as we go. Gather the created junction tree roots in a dummy Node. │ │ │ │ │ +139 typedef typename _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_E_T_R_E_E___B_A_Y_E_S_N_E_T_,_ _E_T_R_E_E___G_R_A_P_H_>_:_:_N_o_d_e │ │ │ │ │ +ETreeNode; │ │ │ │ │ +140 typedef _C_o_n_s_t_r_u_c_t_o_r_T_r_a_v_e_r_s_a_l_D_a_t_a_<_B_A_Y_E_S_T_R_E_E_,_ _G_R_A_P_H_,_ _E_T_r_e_e_N_o_d_e_> Data; │ │ │ │ │ +141 Data rootData(0); │ │ │ │ │ +142 // Make a dummy node to gather the junction tree roots │ │ │ │ │ +143 rootData.junctionTreeNode = boost::make_shared(); │ │ │ │ │ +144 _t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t(eliminationTree, rootData, │ │ │ │ │ +145 Data::ConstructorTraversalVisitorPre, │ │ │ │ │ +146 Data::ConstructorTraversalVisitorPostAlg2); │ │ │ │ │ +147 │ │ │ │ │ +148 // Assign roots from the dummy node │ │ │ │ │ +149 this->addChildrenAsRoots(rootData.junctionTreeNode); │ │ │ │ │ +150 │ │ │ │ │ +151 // Transfer remaining factors from elimination tree │ │ │ │ │ +152 Base::remainingFactors_ = eliminationTree._r_e_m_a_i_n_i_n_g_F_a_c_t_o_r_s(); │ │ │ │ │ +153} │ │ │ │ │ +154 │ │ │ │ │ +155} // namespace gtsam │ │ │ │ │ +_J_u_n_c_t_i_o_n_T_r_e_e_._h │ │ │ │ │ +The junction tree. │ │ │ │ │ +_C_l_u_s_t_e_r_T_r_e_e_-_i_n_s_t_._h │ │ │ │ │ +Collects factorgraph fragments defined on variable clusters, arranged in a │ │ │ │ │ +tree. │ │ │ │ │ +_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ +_S_y_m_b_o_l_i_c_F_a_c_t_o_r_-_i_n_s_t_._h │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ +std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ +FastVector │ │ │ │ │ +FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ +DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_X_M_L_F_i_l_e │ │ │ │ │ -bool deserializeFromXMLFile(const std::string &filename, T &output, const std:: │ │ │ │ │ -string &name="data") │ │ │ │ │ -deserializes from an XML file │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:174 │ │ │ │ │ -_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e │ │ │ │ │ -std::string serialize(const T &input) │ │ │ │ │ -serializes to a string │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:113 │ │ │ │ │ -_g_t_s_a_m_:_:_a_s_s_e_r_t___e_q_u_a_l │ │ │ │ │ -bool assert_equal(const Matrix &expected, const Matrix &actual, double tol) │ │ │ │ │ -equals with an tolerance, prints out message if unequal │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:43 │ │ │ │ │ -_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_B_i_n_a_r_y │ │ │ │ │ -void deserializeBinary(const std::string &serialized, T &output, const std:: │ │ │ │ │ -string &name="data") │ │ │ │ │ -deserializes from a string in binary │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:266 │ │ │ │ │ -_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_X_M_L │ │ │ │ │ -void deserializeXML(const std::string &serialized, T &output, const std::string │ │ │ │ │ -&name="data") │ │ │ │ │ -deserializes from a string in XML │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:192 │ │ │ │ │ -_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_X_M_L_F_i_l_e │ │ │ │ │ -bool serializeToXMLFile(const T &input, const std::string &filename, const │ │ │ │ │ -std::string &name="data") │ │ │ │ │ -serializes to an XML file │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:163 │ │ │ │ │ -_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_F_i_l_e │ │ │ │ │ -bool serializeToFile(const T &input, const std::string &filename) │ │ │ │ │ -serializes to a file │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:93 │ │ │ │ │ -_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e │ │ │ │ │ -void deserialize(const std::string &serialized, T &output) │ │ │ │ │ -deserializes from a string │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:119 │ │ │ │ │ -_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_B_i_n_a_r_y_F_i_l_e │ │ │ │ │ -bool serializeToBinaryFile(const T &input, const std::string &filename, const │ │ │ │ │ -std::string &name="data") │ │ │ │ │ -serializes to a binary file │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:237 │ │ │ │ │ -_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_F_i_l_e │ │ │ │ │ -bool deserializeFromFile(const std::string &filename, T &output) │ │ │ │ │ -deserializes from a file │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:103 │ │ │ │ │ -_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_B_i_n_a_r_y_F_i_l_e │ │ │ │ │ -bool deserializeFromBinaryFile(const std::string &filename, T &output, const │ │ │ │ │ -std::string &name="data") │ │ │ │ │ -deserializes from a binary file │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:248 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t │ │ │ │ │ +void DepthFirstForest(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, │ │ │ │ │ +VISITOR_POST &visitorPost) │ │ │ │ │ +Traverse a forest depth-first with pre-order and post-order visits. │ │ │ │ │ +DDeeffiinniittiioonn treeTraversal-inst.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ +nodes. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ +_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r │ │ │ │ │ +A Cluster is just a collection of factors. │ │ │ │ │ +DDeeffiinniittiioonn ClusterTree.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ +An elimination tree is a data structure used intermediately during elimination. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_r_e_m_a_i_n_i_n_g_F_a_c_t_o_r_s │ │ │ │ │ +const FastVector< sharedFactor > & remainingFactors() const │ │ │ │ │ +Return the remaining factors that are not pulled into elimination. │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:154 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_N_o_d_e │ │ │ │ │ +DDeeffiinniittiioonn EliminationTree.h:66 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_s_t_r_u_c_t_o_r_T_r_a_v_e_r_s_a_l_D_a_t_a │ │ │ │ │ +DDeeffiinniittiioonn JunctionTree-inst.h:31 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_s_t_r_u_c_t_o_r_T_r_a_v_e_r_s_a_l_D_a_t_a_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_s │ │ │ │ │ +DDeeffiinniittiioonn JunctionTree-inst.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ +A JunctionTree is a cluster tree, a set of variable clusters with factors, │ │ │ │ │ +arranged in a tree,... │ │ │ │ │ +DDeeffiinniittiioonn JunctionTree.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +Typedef to the conditional base class. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicConditional.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +Overriding the shared_ptr typedef. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactor.h:48 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _b_a_s_e │ │ │ │ │ - * _s_e_r_i_a_l_i_z_a_t_i_o_n_T_e_s_t_H_e_l_p_e_r_s_._h │ │ │ │ │ + * _i_n_f_e_r_e_n_c_e │ │ │ │ │ + * _J_u_n_c_t_i_o_n_T_r_e_e_-_i_n_s_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00065.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01277.html │ │ │ │┄ Files 74% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Vector.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/dataset.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces | │ │ │ │ Macros | │ │ │ │ Typedefs | │ │ │ │ Functions
│ │ │ │ -
Vector.h File Reference
│ │ │ │ +
dataset.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

typedef and functions to augment Eigen's VectorXd │ │ │ │ +

utility functions for loading datasets │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Macros

│ │ │ │ -#define MKL_BLAS   MKL_DOMAIN_BLAS
 
#define GTSAM_MAKE_VECTOR_DEFS(N)
 
│ │ │ │ +#define LINESIZE   81920
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Typedefs

│ │ │ │ -typedef Eigen::VectorXd gtsam::Vector
 
│ │ │ │ -typedef Eigen::Matrix< double, 1, 1 > gtsam::Vector1
 
│ │ │ │ -typedef Eigen::Vector2d gtsam::Vector2
 
│ │ │ │ -typedef Eigen::Vector3d gtsam::Vector3
 
│ │ │ │ -typedef Eigen::VectorBlock< Vector > gtsam::SubVector
 
│ │ │ │ -typedef Eigen::VectorBlock< const Vector > gtsam::ConstSubVector
 
│ │ │ │ +template<typename T >
using gtsam::Parser = std::function< boost::optional< T >(istream &is, const string &tag)>
 
│ │ │ │ +using gtsam::BearingRange2D = BearingRange< Pose2, Point2 >
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

bool gtsam::fpEqual (double a, double b, double tol, bool check_relative_also=true)
 Ensure we are not including a different version of Eigen in user code than while compiling gtsam, since it can lead to hard-to-understand runtime crashes.
 
│ │ │ │ -void gtsam::print (const Vector &v, const std::string &s, std::ostream &stream)
 print without optional string, must specify cout yourself
 
│ │ │ │ -void gtsam::print (const Vector &v, const std::string &s="")
 print with optional string to cout
 
│ │ │ │ -void gtsam::save (const Vector &A, const std::string &s, const std::string &filename)
 save a vector to file, which can be loaded by matlab
 
│ │ │ │ -bool gtsam::operator== (const Vector &vec1, const Vector &vec2)
 operator==()
 
│ │ │ │ -bool gtsam::greaterThanOrEqual (const Vector &v1, const Vector &v2)
 Greater than or equal to operation returns true if all elements in v1 are greater than corresponding elements in v2.
 
│ │ │ │ -bool gtsam::equal_with_abs_tol (const Vector &vec1, const Vector &vec2, double tol=1e-9)
 VecA == VecB up to tolerance.
 
│ │ │ │ -bool gtsam::equal_with_abs_tol (const SubVector &vec1, const SubVector &vec2, double tol)
 
│ │ │ │ -bool gtsam::equal (const Vector &vec1, const Vector &vec2, double tol)
 Override of equal in Lie.h.
 
│ │ │ │ -bool gtsam::equal (const Vector &vec1, const Vector &vec2)
 Override of equal in Lie.h.
 
bool gtsam::assert_equal (const Vector &vec1, const Vector &vec2, double tol=1e-9)
 Same, prints if error.
 
bool gtsam::assert_inequal (const Vector &vec1, const Vector &vec2, double tol=1e-9)
 Not the same, prints if error.
 
bool gtsam::assert_equal (const SubVector &vec1, const SubVector &vec2, double tol=1e-9)
 Same, prints if error.
 
│ │ │ │ -bool gtsam::assert_equal (const ConstSubVector &expected, const ConstSubVector &actual, double tol)
 
bool gtsam::linear_dependent (const Vector &vec1, const Vector &vec2, double tol=1e-9)
 check whether two vectors are linearly dependent
 
Vector gtsam::ediv_ (const Vector &a, const Vector &b)
 elementwise division, but 0/0 = 0, not inf
 
│ │ │ │ -template<class V1 , class V2 >
double gtsam::dot (const V1 &a, const V2 &b)
 Dot product.
 
│ │ │ │ -template<class V1 , class V2 >
double gtsam::inner_prod (const V1 &a, const V2 &b)
 compatibility version for ublas' inner_prod()
 
pair< double, Vector > gtsam::house (const Vector &x)
 house(x,j) computes HouseHolder vector v and scaling factor beta from x, such that the corresponding Householder reflection zeroes out all but x.
 
│ │ │ │ -double gtsam::houseInPlace (Vector &x)
 beta = house(x) computes the HouseHolder vector in place
 
pair< Vector, double > gtsam::weightedPseudoinverse (const Vector &v, const Vector &weights)
 Weighted Householder solution vector, a.k.a., the pseudoinverse of the column NOTE: if any sigmas are zero (indicating a constraint) the pseudoinverse will be a selection vector, and the variance will be zero.
 
│ │ │ │ -double gtsam::weightedPseudoinverse (const Vector &a, const Vector &weights, Vector &pseudo)
 
│ │ │ │ -Vector gtsam::concatVectors (const std::list< Vector > &vs)
 concatenate Vectors
 
│ │ │ │ -Vector gtsam::concatVectors (size_t nrVectors,...)
 concatenate Vectors
 
string gtsam::findExampleDataFile (const std::string &name)
 Find the full path to an example dataset distributed with gtsam.
 
│ │ │ │ +string gtsam::createRewrittenFileName (const std::string &name)
 Creates a temporary file name that needs to be ignored in .gitingnore for checking read-write oprations.
 
│ │ │ │ +template<typename T >
map< size_t, T > gtsam::parseToMap (const string &filename, Parser< pair< size_t, T > > parse, size_t maxIndex)
 
boost::optional< IndexedPose > gtsam::parseVertexPose (std::istream &is, const std::string &tag)
 Parse TORO/G2O vertex "id x y yaw".
 
│ │ │ │ +template<>
GTSAM_EXPORT std::map< size_t, Pose2 > gtsam::parseVariables< Pose2 > (const std::string &filename, size_t maxIndex)
 
boost::optional< IndexedLandmark > gtsam::parseVertexLandmark (std::istream &is, const std::string &tag)
 Parse G2O landmark vertex "id x y".
 
│ │ │ │ +template<>
GTSAM_EXPORT std::map< size_t, Point2 > gtsam::parseVariables< Point2 > (const std::string &filename, size_t maxIndex)
 
boost::optional< IndexedEdge > gtsam::parseEdge (std::istream &is, const std::string &tag)
 Parse TORO/G2O edge "id1 id2 x y yaw".
 
│ │ │ │ +boost::shared_ptr< Sampler > gtsam::createSampler (const SharedNoiseModel &model)
 
│ │ │ │ +template<>
GTSAM_EXPORT std::vector< BinaryMeasurement< Pose2 > > gtsam::parseMeasurements (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
 
│ │ │ │ +template<>
GTSAM_EXPORT std::vector< BinaryMeasurement< Rot2 > > gtsam::parseMeasurements (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
 
│ │ │ │ +template<>
GTSAM_EXPORT std::vector< BetweenFactor< Pose2 >::shared_ptr > gtsam::parseFactors< Pose2 > (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
 
GraphAndValues gtsam::load2D (const std::string &filename, SharedNoiseModel model=SharedNoiseModel(), size_t maxIndex=0, bool addNoise=false, bool smart=true, NoiseFormat noiseFormat=NoiseFormatAUTO, KernelFunctionType kernelFunctionType=KernelFunctionTypeNONE)
 Load TORO/G2O style graph files.
 
GraphAndValues gtsam::load2D (pair< string, SharedNoiseModel > dataset, size_t maxIndex, bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType)
 Load TORO 2D Graph.
 
│ │ │ │ +GraphAndValues gtsam::load2D_robust (const string &filename, const noiseModel::Base::shared_ptr &model, size_t maxIndex)
 
│ │ │ │ +void gtsam::save2D (const NonlinearFactorGraph &graph, const Values &config, const noiseModel::Diagonal::shared_ptr model, const std::string &filename)
 save 2d graph
 
GraphAndValues gtsam::readG2o (const std::string &g2oFile, const bool is3D=false, KernelFunctionType kernelFunctionType=KernelFunctionTypeNONE)
 This function parses a g2o file and stores the measurements into a NonlinearFactorGraph and the initial guess in a Values structure.
 
void gtsam::writeG2o (const NonlinearFactorGraph &graph, const Values &estimate, const std::string &filename)
 This function writes a g2o file from NonlinearFactorGraph and a Values structure.
 
│ │ │ │ +istream & gtsam::operator>> (istream &is, Quaternion &q)
 
│ │ │ │ +istream & gtsam::operator>> (istream &is, Rot3 &R)
 
│ │ │ │ +boost::optional< pair< size_t, Pose3 > > gtsam::parseVertexPose3 (istream &is, const string &tag)
 
│ │ │ │ +template<>
GTSAM_EXPORT std::map< size_t, Pose3 > gtsam::parseVariables< Pose3 > (const std::string &filename, size_t maxIndex)
 
│ │ │ │ +boost::optional< pair< size_t, Point3 > > gtsam::parseVertexPoint3 (istream &is, const string &tag)
 
│ │ │ │ +template<>
GTSAM_EXPORT std::map< size_t, Point3 > gtsam::parseVariables< Point3 > (const std::string &filename, size_t maxIndex)
 
│ │ │ │ +istream & gtsam::operator>> (istream &is, Matrix6 &m)
 
│ │ │ │ +template<>
GTSAM_EXPORT std::vector< BinaryMeasurement< Pose3 > > gtsam::parseMeasurements (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
 
│ │ │ │ +template<>
GTSAM_EXPORT std::vector< BinaryMeasurement< Rot3 > > gtsam::parseMeasurements (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
 
│ │ │ │ +template<>
GTSAM_EXPORT std::vector< BetweenFactor< Pose3 >::shared_ptr > gtsam::parseFactors< Pose3 > (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
 
│ │ │ │ +GraphAndValues gtsam::load3D (const std::string &filename)
 Load TORO 3D Graph.
 
│ │ │ │ +BetweenFactorPose2s gtsam::parse2DFactors (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
 
│ │ │ │ +BetweenFactorPose3s gtsam::parse3DFactors (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
 
│ │ │ │

Detailed Description

│ │ │ │ -

typedef and functions to augment Eigen's VectorXd

│ │ │ │ +

utility functions for loading datasets

│ │ │ │ +
Date
Jan 22, 2010
│ │ │ │
Author
Kai Ni
│ │ │ │
│ │ │ │ -Frank Dellaert
│ │ │ │ +Luca Carlone │ │ │ │
│ │ │ │ -Alex Hagiopol
│ │ │ │ +Frank Dellaert │ │ │ │
│ │ │ │ Varun Agrawal
│ │ │ │ -

Macro Definition Documentation

│ │ │ │ - │ │ │ │ -

◆ GTSAM_MAKE_VECTOR_DEFS

│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
#define GTSAM_MAKE_VECTOR_DEFS( N)
│ │ │ │ -
│ │ │ │ -Value:
using Vector##N = Eigen::Matrix<double, N, 1>; \
│ │ │ │ -
static const Eigen::MatrixBase<Vector##N>::ConstantReturnType Z_##N##x1 = Vector##N::Zero();
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ +
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,147 +1,188 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Vector.h File Reference │ │ │ │ │ -typedef and functions to augment Eigen's VectorXd _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +dataset.cpp File Reference │ │ │ │ │ +utility functions for loading datasets _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ MMaaccrrooss │ │ │ │ │ -#define  MMKKLL__BBLLAASS   MKL_DOMAIN_BLAS │ │ │ │ │ -  │ │ │ │ │ -#define  _G_T_S_A_M___M_A_K_E___V_E_C_T_O_R___D_E_F_S(N) │ │ │ │ │ +#define  LLIINNEESSIIZZEE   81920 │ │ │ │ │   │ │ │ │ │ TTyyppeeddeeffss │ │ │ │ │ - typedef Eigen::VectorXd  ggttssaamm::::VVeeccttoorr │ │ │ │ │ -  │ │ │ │ │ - typedef Eigen::Matrix< double, 1, 1 >  ggttssaamm::::VVeeccttoorr11 │ │ │ │ │ -  │ │ │ │ │ - typedef Eigen::Vector2d  ggttssaamm::::VVeeccttoorr22 │ │ │ │ │ -  │ │ │ │ │ - typedef Eigen::Vector3d  ggttssaamm::::VVeeccttoorr33 │ │ │ │ │ +template │ │ │ │ │ +using  ggttssaamm::::PPaarrsseerr = std::function< boost::optional< T >(istream &is, const │ │ │ │ │ + string &tag)> │ │ │ │ │   │ │ │ │ │ - typedef Eigen::VectorBlock< Vector >  ggttssaamm::::SSuubbVVeeccttoorr │ │ │ │ │ -  │ │ │ │ │ -typedef Eigen::VectorBlock< const Vector >  ggttssaamm::::CCoonnssttSSuubbVVeeccttoorr │ │ │ │ │ +using  ggttssaamm::::BBeeaarriinnggRRaannggee22DD = _B_e_a_r_i_n_g_R_a_n_g_e< _P_o_s_e_2, _P_o_i_n_t_2 > │ │ │ │ │   │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ - bool  _g_t_s_a_m_:_:_f_p_E_q_u_a_l (double a, double b, double tol, bool │ │ │ │ │ - check_relative_also=true) │ │ │ │ │ - Ensure we are not including a different version of │ │ │ │ │ -  Eigen in user code than while compiling gtsam, since it │ │ │ │ │ - can lead to hard-to-understand runtime crashes. │ │ │ │ │ -  │ │ │ │ │ - void  ggttssaamm::::pprriinntt (const Vector &v, const std::string &s, │ │ │ │ │ - std::ostream &stream) │ │ │ │ │ -  print without optional string, must specify cout │ │ │ │ │ - yourself │ │ │ │ │ -  │ │ │ │ │ - void  ggttssaamm::::pprriinntt (const Vector &v, const std::string &s="") │ │ │ │ │ -  print with optional string to cout │ │ │ │ │ -  │ │ │ │ │ - void  ggttssaamm::::ssaavvee (const Vector &A, const std::string &s, │ │ │ │ │ - const std::string &filename) │ │ │ │ │ -  save a vector to file, which can be loaded by matlab │ │ │ │ │ -  │ │ │ │ │ - bool  ggttssaamm::::ooppeerraattoorr==== (const Vector &vec1, const Vector │ │ │ │ │ - &vec2) │ │ │ │ │ -  _o_p_e_r_a_t_o_r_=_=_(_) │ │ │ │ │ -  │ │ │ │ │ - bool  ggttssaamm::::ggrreeaatteerrTThhaannOOrrEEqquuaall (const Vector &v1, const │ │ │ │ │ - Vector &v2) │ │ │ │ │ - Greater than or equal to operation returns true if all │ │ │ │ │ -  elements in v1 are greater than corresponding elements │ │ │ │ │ - in v2. │ │ │ │ │ -  │ │ │ │ │ - bool  ggttssaamm::::eeqquuaall__wwiitthh__aabbss__ttooll (const Vector &vec1, const │ │ │ │ │ - Vector &vec2, double tol=1e-9) │ │ │ │ │ -  VecA == VecB up to tolerance. │ │ │ │ │ -  │ │ │ │ │ - bool  ggttssaamm::::eeqquuaall__wwiitthh__aabbss__ttooll (const SubVector &vec1, const │ │ │ │ │ - SubVector &vec2, double tol) │ │ │ │ │ -  │ │ │ │ │ - bool  ggttssaamm::::eeqquuaall (const Vector &vec1, const Vector &vec2, │ │ │ │ │ - double tol) │ │ │ │ │ -  Override of equal in _L_i_e_._h. │ │ │ │ │ -  │ │ │ │ │ - bool  ggttssaamm::::eeqquuaall (const Vector &vec1, const Vector &vec2) │ │ │ │ │ -  Override of equal in _L_i_e_._h. │ │ │ │ │ -  │ │ │ │ │ - bool  _g_t_s_a_m_:_:_a_s_s_e_r_t___e_q_u_a_l (const Vector &vec1, const Vector │ │ │ │ │ - &vec2, double tol=1e-9) │ │ │ │ │ -  Same, prints if error. │ │ │ │ │ -  │ │ │ │ │ - bool  _g_t_s_a_m_:_:_a_s_s_e_r_t___i_n_e_q_u_a_l (const Vector &vec1, const Vector │ │ │ │ │ - &vec2, double tol=1e-9) │ │ │ │ │ -  Not the same, prints if error. │ │ │ │ │ -  │ │ │ │ │ - bool  _g_t_s_a_m_:_:_a_s_s_e_r_t___e_q_u_a_l (const SubVector &vec1, const │ │ │ │ │ - SubVector &vec2, double tol=1e-9) │ │ │ │ │ -  Same, prints if error. │ │ │ │ │ -  │ │ │ │ │ - bool  ggttssaamm::::aasssseerrtt__eeqquuaall (const ConstSubVector &expected, │ │ │ │ │ - const ConstSubVector &actual, double tol) │ │ │ │ │ -  │ │ │ │ │ - bool  _g_t_s_a_m_:_:_l_i_n_e_a_r___d_e_p_e_n_d_e_n_t (const Vector &vec1, const │ │ │ │ │ - Vector &vec2, double tol=1e-9) │ │ │ │ │ -  check whether two vectors are linearly dependent │ │ │ │ │ -  │ │ │ │ │ - Vector  _g_t_s_a_m_:_:_e_d_i_v__ (const Vector &a, const Vector &b) │ │ │ │ │ -  elementwise division, but 0/0 = 0, not inf │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - double  ggttssaamm::::ddoott (const V1 &a, const V2 &b) │ │ │ │ │ -  Dot product. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - double  ggttssaamm::::iinnnneerr__pprroodd (const V1 &a, const V2 &b) │ │ │ │ │ -  compatibility version for ublas' _i_n_n_e_r___p_r_o_d_(_) │ │ │ │ │ -  │ │ │ │ │ -pair< double, Vector >  _g_t_s_a_m_:_:_h_o_u_s_e (const Vector &x) │ │ │ │ │ - house(x,j) computes HouseHolder vector v and scaling │ │ │ │ │ -  factor beta from x, such that the corresponding │ │ │ │ │ - Householder reflection zeroes out all but x. │ │ │ │ │ -  │ │ │ │ │ - double  ggttssaamm::::hhoouusseeIInnPPllaaccee (Vector &x) │ │ │ │ │ -  beta = house(x) computes the HouseHolder vector in │ │ │ │ │ - place │ │ │ │ │ -  │ │ │ │ │ -pair< Vector, double >  _g_t_s_a_m_:_:_w_e_i_g_h_t_e_d_P_s_e_u_d_o_i_n_v_e_r_s_e (const Vector &v, const │ │ │ │ │ - Vector &weights) │ │ │ │ │ - Weighted Householder solution vector, a.k.a., the │ │ │ │ │ -  pseudoinverse of the column NOTE: if any sigmas are │ │ │ │ │ - zero (indicating a constraint) the pseudoinverse will │ │ │ │ │ - be a selection vector, and the variance will be zero. │ │ │ │ │ -  │ │ │ │ │ - double  ggttssaamm::::wweeiigghhtteeddPPsseeuuddooiinnvveerrssee (const Vector &a, const │ │ │ │ │ - Vector &weights, Vector &pseudo) │ │ │ │ │ -  │ │ │ │ │ - Vector  ggttssaamm::::ccoonnccaattVVeeccttoorrss (const std::list< Vector > &vs) │ │ │ │ │ -  concatenate Vectors │ │ │ │ │ -  │ │ │ │ │ - Vector  ggttssaamm::::ccoonnccaattVVeeccttoorrss (size_t nrVectors,...) │ │ │ │ │ -  concatenate Vectors │ │ │ │ │ + string  _g_t_s_a_m_:_:_f_i_n_d_E_x_a_m_p_l_e_D_a_t_a_F_i_l_e (const std:: │ │ │ │ │ + string &name) │ │ │ │ │ +  Find the full path to an example dataset │ │ │ │ │ + distributed with gtsam. │ │ │ │ │ +  │ │ │ │ │ + string  ggttssaamm::::ccrreeaatteeRReewwrriitttteennFFiilleeNNaammee (const │ │ │ │ │ + std::string &name) │ │ │ │ │ + Creates a temporary file name that needs │ │ │ │ │ +  to be ignored in .gitingnore for checking │ │ │ │ │ + read-write oprations. │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ + map< size_t, T >  ggttssaamm::::ppaarrsseeTTooMMaapp (const string &filename, │ │ │ │ │ + Parser< pair< size_t, T > > parse, size_t │ │ │ │ │ + maxIndex) │ │ │ │ │ +  │ │ │ │ │ + boost::optional< _I_n_d_e_x_e_d_P_o_s_e >  _g_t_s_a_m_:_:_p_a_r_s_e_V_e_r_t_e_x_P_o_s_e (std::istream &is, │ │ │ │ │ + const std::string &tag) │ │ │ │ │ +  Parse TORO/G2O vertex "id x y yaw". │ │ │ │ │ +  │ │ │ │ │ + template<> │ │ │ │ │ +GTSAM_EXPORT std::map< size_t, _P_o_s_e_2 ggttssaamm::::ppaarrsseeVVaarriiaabblleess<< PPoossee22 >> (const │ │ │ │ │ + >  std::string &filename, size_t maxIndex) │ │ │ │ │ +  │ │ │ │ │ + boost::optional< IndexedLandmark >  _g_t_s_a_m_:_:_p_a_r_s_e_V_e_r_t_e_x_L_a_n_d_m_a_r_k (std::istream │ │ │ │ │ + &is, const std::string &tag) │ │ │ │ │ +  Parse G2O landmark vertex "id x y". │ │ │ │ │ +  │ │ │ │ │ + template<> │ │ │ │ │ + GTSAM_EXPORT std::map< size_t, ggttssaamm::::ppaarrsseeVVaarriiaabblleess<< PPooiinntt22 >> (const │ │ │ │ │ + _P_o_i_n_t_2 >  std::string &filename, size_t maxIndex) │ │ │ │ │ +  │ │ │ │ │ + boost::optional< IndexedEdge >  _g_t_s_a_m_:_:_p_a_r_s_e_E_d_g_e (std::istream &is, const │ │ │ │ │ + std::string &tag) │ │ │ │ │ +  Parse TORO/G2O edge "id1 id2 x y yaw". │ │ │ │ │ +  │ │ │ │ │ + boost::shared_ptr< _S_a_m_p_l_e_r >  ggttssaamm::::ccrreeaatteeSSaammpplleerr (const │ │ │ │ │ + _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model) │ │ │ │ │ +  │ │ │ │ │ + template<> │ │ │ │ │ + GTSAM_EXPORT std::vector< ggttssaamm::::ppaarrsseeMMeeaassuurreemmeennttss (const std:: │ │ │ │ │ + _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t< _P_o_s_e_2 > >  string &filename, const noiseModel:: │ │ │ │ │ + Diagonal::shared_ptr &model, size_t │ │ │ │ │ + maxIndex) │ │ │ │ │ +  │ │ │ │ │ + template<> │ │ │ │ │ + GTSAM_EXPORT std::vector< ggttssaamm::::ppaarrsseeMMeeaassuurreemmeennttss (const std:: │ │ │ │ │ + _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t< _R_o_t_2 > >  string &filename, const noiseModel:: │ │ │ │ │ + Diagonal::shared_ptr &model, size_t │ │ │ │ │ + maxIndex) │ │ │ │ │ +  │ │ │ │ │ + template<> │ │ │ │ │ + GTSAM_EXPORT std::vector< ggttssaamm::::ppaarrsseeFFaaccttoorrss<< PPoossee22 >> (const std:: │ │ │ │ │ + _B_e_t_w_e_e_n_F_a_c_t_o_r< _P_o_s_e_2 >::shared_ptr string &filename, const noiseModel:: │ │ │ │ │ + >  Diagonal::shared_ptr &model, size_t │ │ │ │ │ + maxIndex) │ │ │ │ │ +  │ │ │ │ │ + _G_r_a_p_h_A_n_d_V_a_l_u_e_s  _g_t_s_a_m_:_:_l_o_a_d_2_D (const std::string │ │ │ │ │ + &filename, _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ + model=_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l(), size_t │ │ │ │ │ + maxIndex=0, bool addNoise=false, bool │ │ │ │ │ + smart=true, _N_o_i_s_e_F_o_r_m_a_t │ │ │ │ │ + noiseFormat=_N_o_i_s_e_F_o_r_m_a_t_A_U_T_O, │ │ │ │ │ + _K_e_r_n_e_l_F_u_n_c_t_i_o_n_T_y_p_e │ │ │ │ │ + kernelFunctionType=KernelFunctionTypeNONE) │ │ │ │ │ +  Load TORO/G2O style graph files. │ │ │ │ │ +  │ │ │ │ │ + _G_r_a_p_h_A_n_d_V_a_l_u_e_s  _g_t_s_a_m_:_:_l_o_a_d_2_D (pair< string, │ │ │ │ │ + _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l > dataset, size_t │ │ │ │ │ + maxIndex, bool addNoise, bool smart, │ │ │ │ │ + _N_o_i_s_e_F_o_r_m_a_t noiseFormat, │ │ │ │ │ + _K_e_r_n_e_l_F_u_n_c_t_i_o_n_T_y_p_e kernelFunctionType) │ │ │ │ │ +  Load TORO 2D Graph. │ │ │ │ │ +  │ │ │ │ │ + _G_r_a_p_h_A_n_d_V_a_l_u_e_s  ggttssaamm::::llooaadd22DD__rroobbuusstt (const string │ │ │ │ │ + &filename, const noiseModel::Base:: │ │ │ │ │ + shared_ptr &model, size_t maxIndex) │ │ │ │ │ +  │ │ │ │ │ + void  ggttssaamm::::ssaavvee22DD (const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ + &graph, const _V_a_l_u_e_s &config, const │ │ │ │ │ + noiseModel::Diagonal::shared_ptr model, │ │ │ │ │ + const std::string &filename) │ │ │ │ │ +  save 2d graph │ │ │ │ │ +  │ │ │ │ │ + _G_r_a_p_h_A_n_d_V_a_l_u_e_s  _g_t_s_a_m_:_:_r_e_a_d_G_2_o (const std::string │ │ │ │ │ + &g2oFile, const bool is3D=false, │ │ │ │ │ + _K_e_r_n_e_l_F_u_n_c_t_i_o_n_T_y_p_e │ │ │ │ │ + kernelFunctionType=KernelFunctionTypeNONE) │ │ │ │ │ + This function parses a g2o file and stores │ │ │ │ │ +  the measurements into a │ │ │ │ │ + _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h and the initial guess │ │ │ │ │ + in a _V_a_l_u_e_s structure. │ │ │ │ │ +  │ │ │ │ │ + void  _g_t_s_a_m_:_:_w_r_i_t_e_G_2_o (const │ │ │ │ │ + _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h &graph, const _V_a_l_u_e_s │ │ │ │ │ + &estimate, const std::string &filename) │ │ │ │ │ + This function writes a g2o file from │ │ │ │ │ +  _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h and a _V_a_l_u_e_s │ │ │ │ │ + structure. │ │ │ │ │ +  │ │ │ │ │ + istream &  ggttssaamm::::ooppeerraattoorr>>>> (istream &is, Quaternion │ │ │ │ │ + &q) │ │ │ │ │ +  │ │ │ │ │ + istream &  ggttssaamm::::ooppeerraattoorr>>>> (istream &is, _R_o_t_3 &R) │ │ │ │ │ +  │ │ │ │ │ +boost::optional< pair< size_t, _P_o_s_e_3 ggttssaamm::::ppaarrsseeVVeerrtteexxPPoossee33 (istream &is, │ │ │ │ │ + > >  const string &tag) │ │ │ │ │ +  │ │ │ │ │ + template<> │ │ │ │ │ +GTSAM_EXPORT std::map< size_t, _P_o_s_e_3 ggttssaamm::::ppaarrsseeVVaarriiaabblleess<< PPoossee33 >> (const │ │ │ │ │ + >  std::string &filename, size_t maxIndex) │ │ │ │ │ +  │ │ │ │ │ + boost::optional< pair< size_t, ggttssaamm::::ppaarrsseeVVeerrtteexxPPooiinntt33 (istream &is, │ │ │ │ │ + _P_o_i_n_t_3 > >  const string &tag) │ │ │ │ │ +  │ │ │ │ │ + template<> │ │ │ │ │ + GTSAM_EXPORT std::map< size_t, ggttssaamm::::ppaarrsseeVVaarriiaabblleess<< PPooiinntt33 >> (const │ │ │ │ │ + _P_o_i_n_t_3 >  std::string &filename, size_t maxIndex) │ │ │ │ │ +  │ │ │ │ │ + istream &  ggttssaamm::::ooppeerraattoorr>>>> (istream &is, Matrix6 │ │ │ │ │ + &m) │ │ │ │ │ +  │ │ │ │ │ + template<> │ │ │ │ │ + GTSAM_EXPORT std::vector< ggttssaamm::::ppaarrsseeMMeeaassuurreemmeennttss (const std:: │ │ │ │ │ + _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t< _P_o_s_e_3 > >  string &filename, const noiseModel:: │ │ │ │ │ + Diagonal::shared_ptr &model, size_t │ │ │ │ │ + maxIndex) │ │ │ │ │ +  │ │ │ │ │ + template<> │ │ │ │ │ + GTSAM_EXPORT std::vector< ggttssaamm::::ppaarrsseeMMeeaassuurreemmeennttss (const std:: │ │ │ │ │ + _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t< _R_o_t_3 > >  string &filename, const noiseModel:: │ │ │ │ │ + Diagonal::shared_ptr &model, size_t │ │ │ │ │ + maxIndex) │ │ │ │ │ +  │ │ │ │ │ + template<> │ │ │ │ │ + GTSAM_EXPORT std::vector< ggttssaamm::::ppaarrsseeFFaaccttoorrss<< PPoossee33 >> (const std:: │ │ │ │ │ + _B_e_t_w_e_e_n_F_a_c_t_o_r< _P_o_s_e_3 >::shared_ptr string &filename, const noiseModel:: │ │ │ │ │ + >  Diagonal::shared_ptr &model, size_t │ │ │ │ │ + maxIndex) │ │ │ │ │ +  │ │ │ │ │ + _G_r_a_p_h_A_n_d_V_a_l_u_e_s  ggttssaamm::::llooaadd33DD (const std::string │ │ │ │ │ + &filename) │ │ │ │ │ +  Load TORO 3D Graph. │ │ │ │ │ +  │ │ │ │ │ + BetweenFactorPose2s  ggttssaamm::::ppaarrssee22DDFFaaccttoorrss (const std::string │ │ │ │ │ + &filename, const noiseModel::Diagonal:: │ │ │ │ │ + shared_ptr &model, size_t maxIndex) │ │ │ │ │ +  │ │ │ │ │ + BetweenFactorPose3s  ggttssaamm::::ppaarrssee33DDFFaaccttoorrss (const std::string │ │ │ │ │ + &filename, const noiseModel::Diagonal:: │ │ │ │ │ + shared_ptr &model, size_t maxIndex) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -typedef and functions to augment Eigen's VectorXd │ │ │ │ │ +utility functions for loading datasets │ │ │ │ │ + Date │ │ │ │ │ + Jan 22, 2010 │ │ │ │ │ Author │ │ │ │ │ Kai Ni │ │ │ │ │ + Luca Carlone │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Alex Hagiopol │ │ │ │ │ Varun Agrawal │ │ │ │ │ -********** MMaaccrroo DDeeffiinniittiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ -********** _?◆_? GGTTSSAAMM__MMAAKKEE__VVEECCTTOORR__DDEEFFSS ********** │ │ │ │ │ -#define GTSAM_MAKE_VECTOR_DEFS (   N ) │ │ │ │ │ -VVaalluuee:: │ │ │ │ │ -using Vector##N = Eigen::Matrix; \ │ │ │ │ │ -static const Eigen::MatrixBase::ConstantReturnType Z_##N##x1 = │ │ │ │ │ -Vector##N::Zero(); │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _b_a_s_e │ │ │ │ │ - * _V_e_c_t_o_r_._h │ │ │ │ │ + * _s_l_a_m │ │ │ │ │ + * _d_a_t_a_s_e_t_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00077_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00005_source.html │ │ │ │┄ Files 99% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
14 * @brief
│ │ │ │
15 * @date November, 2014
│ │ │ │
16 * @author Paul Furgale
│ │ │ │
17 */
│ │ │ │
18
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/base/Matrix.h>
│ │ │ │ -
22#include <gtsam/base/Manifold.h>
│ │ │ │ -
23#include <gtsam/base/Testable.h>
│ │ │ │ +
21#include <gtsam/base/Matrix.h>
│ │ │ │ +
22#include <gtsam/base/Manifold.h>
│ │ │ │ +
23#include <gtsam/base/Testable.h>
│ │ │ │
24#include <CppUnitLite/TestResult.h>
│ │ │ │
25#include <CppUnitLite/Test.h>
│ │ │ │
26#include <CppUnitLite/Failure.h>
│ │ │ │
27
│ │ │ │
│ │ │ │
28namespace gtsam {
│ │ │ │
29// Do a full concept check and test the invertibility of local() vs. retract().
│ │ │ │ @@ -170,17 +170,17 @@ │ │ │ │
62 EXPECT(assert_equal(Matrix(dx), Matrix(invdx), 1e-9));
│ │ │ │
63}
│ │ │ │
64} // namespace gtsam
│ │ │ │
│ │ │ │
65
│ │ │ │
68#define CHECK_CHART_CONCEPT(value) \
│ │ │ │
69 { gtsam::testDefaultChart(result_, name_, value); }
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ -
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ -
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
equals with an tolerance, prints out message if unequal
Definition Matrix.cpp:43
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
Used in GaussianFactor and GaussianConditional.

Author
Richard Roberts
│ │ │ │
Date
Sep 18, 2010
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00098.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00107.html │ │ │ │┄ Files 91% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Group.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Manifold.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ Macros | │ │ │ │ -Functions | │ │ │ │ -Variables
│ │ │ │ -
Group.h File Reference
│ │ │ │ +Functions
│ │ │ │ +
Manifold.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Concept check class for variable types with Group properties. │ │ │ │ +

Base class and basic functions for Manifold types. │ │ │ │ More...

│ │ │ │ │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

struct  gtsam::group_tag
 tag to assert a type is a group More...
struct  gtsam::manifold_tag
 tag to assert a type is a manifold More...
 
struct  gtsam::multiplicative_group_tag
 Group operator syntax flavors. More...
struct  gtsam::internal::HasManifoldPrereqs< Class >
 Requirements on type to pass it to Manifold template below. More...
 
struct  gtsam::additive_group_tag
struct  gtsam::internal::GetDimensionImpl< Class, N >
 Extra manifold traits for fixed-dimension types. More...
 
class  gtsam::IsGroup< G >
 Group Concept. More...
struct  gtsam::internal::GetDimensionImpl< Class, Eigen::Dynamic >
 Extra manifold traits for variable-dimension types. More...
 
class  gtsam::DirectProduct
struct  gtsam::internal::ManifoldTraits< Class >
 A helper that implements the traits interface for GTSAM manifolds. More...
 
struct  gtsam::traits< DirectProduct< G, H > >
struct  gtsam::internal::Manifold< Class >
 Both ManifoldTraits and Testable. More...
 
class  gtsam::DirectSum< G, H >
 Template to construct the direct sum of two additive groups Assumes existence of three additive operators for both groups. More...
 
struct  gtsam::traits< DirectSum< G, H > >
struct  gtsam::FixedDimension< T >
 Give fixed size dimension of a type, fails at compile time if dynamic. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Macros

#define GTSAM_CONCEPT_GROUP_INST(T)   template class gtsam::IsGroup<T>;
 Macros for using the IsGroup.
 
│ │ │ │ -#define GTSAM_CONCEPT_GROUP_TYPE(T)   typedef gtsam::IsGroup<T> _gtsam_IsGroup_##T;
 
│ │ │ │ +#define GTSAM_CONCEPT_MANIFOLD_INST(T)   template class gtsam::IsManifold<T>;
 ‍**
 
│ │ │ │ +#define GTSAM_CONCEPT_MANIFOLD_TYPE(T)   using _gtsam_IsManifold_##T = gtsam::IsManifold<T>;
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ Functions

│ │ │ │ -template<typename G >
 gtsam::BOOST_CONCEPT_REQUIRES (((IsGroup< G >)),(bool)) check_group_invariants(const G &a
 Check invariants.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ -Variables

│ │ │ │ -const G & gtsam::b
 
│ │ │ │ -const G double gtsam::tol
 
│ │ │ │ +template<typename T >
 gtsam::BOOST_CONCEPT_REQUIRES (((IsTestable< T >)),(bool)) check_manifold_invariants(const T &a
 Check invariants for Manifold type.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Concept check class for variable types with Group properties.

│ │ │ │ -
Date
November, 2011
│ │ │ │ +

Base class and basic functions for Manifold types.

│ │ │ │
Author
Alex Cunningham
│ │ │ │
│ │ │ │ -Frank Dellaert
│ │ │ │ -

Macro Definition Documentation

│ │ │ │ - │ │ │ │ -

◆ GTSAM_CONCEPT_GROUP_INST

│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
#define GTSAM_CONCEPT_GROUP_INST( T)   template class gtsam::IsGroup<T>;
│ │ │ │ -
│ │ │ │ - │ │ │ │ -

Macros for using the IsGroup.

│ │ │ │ -
    │ │ │ │ -
  • An instantiation for use inside unit tests
    • │ │ │ │ -
  • A typedef for use inside generic algorithms
    • │ │ │ │ -
│ │ │ │ -

NOTE: intentionally not in the gtsam namespace to allow for classes not in the gtsam namespace to be more easily enforced as testable

│ │ │ │ - │ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ +Frank Dellaert │ │ │ │ +
│ │ │ │ +Mike Bosse
│ │ │ │ +
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,72 +1,60 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _F_u_n_c_t_i_o_n_s | _V_a_r_i_a_b_l_e_s │ │ │ │ │ -Group.h File Reference │ │ │ │ │ -Concept check class for variable types with Group properties. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Manifold.h File Reference │ │ │ │ │ +Base class and basic functions for Manifold types. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_g_r_o_u_p___t_a_g │ │ │ │ │ -  tag to assert a type is a group _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_m_a_n_i_f_o_l_d___t_a_g │ │ │ │ │ +  tag to assert a type is a manifold _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_m_u_l_t_i_p_l_i_c_a_t_i_v_e___g_r_o_u_p___t_a_g │ │ │ │ │ -  Group operator syntax flavors. _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_H_a_s_M_a_n_i_f_o_l_d_P_r_e_r_e_q_s_<_ _C_l_a_s_s_ _> │ │ │ │ │ +  Requirements on type to pass it to _M_a_n_i_f_o_l_d template below. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_a_d_d_i_t_i_v_e___g_r_o_u_p___t_a_g │ │ │ │ │ +struct   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_G_e_t_D_i_m_e_n_s_i_o_n_I_m_p_l_<_ _C_l_a_s_s_,_ _N_ _> │ │ │ │ │ +  Extra manifold traits for fixed-dimension types. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ - class   _g_t_s_a_m_:_:_I_s_G_r_o_u_p_<_ _G_ _> │ │ │ │ │ -  Group Concept. _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_G_e_t_D_i_m_e_n_s_i_o_n_I_m_p_l_<_ _C_l_a_s_s_,_ _E_i_g_e_n_:_:_D_y_n_a_m_i_c_ _> │ │ │ │ │ +  Extra manifold traits for variable-dimension types. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ - class   _g_t_s_a_m_:_:_D_i_r_e_c_t_P_r_o_d_u_c_t │ │ │ │ │ +struct   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d_T_r_a_i_t_s_<_ _C_l_a_s_s_ _> │ │ │ │ │ +  A helper that implements the traits interface for GTSAM manifolds. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _D_i_r_e_c_t_P_r_o_d_u_c_t_<_ _G_,_ _H_ _>_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d_<_ _C_l_a_s_s_ _> │ │ │ │ │ +  Both _M_a_n_i_f_o_l_d_T_r_a_i_t_s and _T_e_s_t_a_b_l_e. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ - class   _g_t_s_a_m_:_:_D_i_r_e_c_t_S_u_m_<_ _G_,_ _H_ _> │ │ │ │ │ -  Template to construct the direct sum of two additive groups Assumes │ │ │ │ │ - existence of three additive operators for both groups. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _D_i_r_e_c_t_S_u_m_<_ _G_,_ _H_ _>_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_F_i_x_e_d_D_i_m_e_n_s_i_o_n_<_ _T_ _> │ │ │ │ │ +  Give fixed size dimension of a type, fails at compile time if dynamic. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ MMaaccrrooss │ │ │ │ │ -#define  _G_T_S_A_M___C_O_N_C_E_P_T___G_R_O_U_P___I_N_S_T(T)   template class _g_t_s_a_m_:_:_I_s_G_r_o_u_p; │ │ │ │ │ -  Macros for using the IsGroup. │ │ │ │ │ +#define  GGTTSSAAMM__CCOONNCCEEPPTT__MMAANNIIFFOOLLDD__IINNSSTT(T)   template class gtsam::IsManifold; │ │ │ │ │ +  ‍** │ │ │ │ │   │ │ │ │ │ -#define  GGTTSSAAMM__CCOONNCCEEPPTT__GGRROOUUPP__TTYYPPEE(T)   typedef _g_t_s_a_m_:_:_I_s_G_r_o_u_p │ │ │ │ │ - _gtsam_IsGroup_##T; │ │ │ │ │ +#define  GGTTSSAAMM__CCOONNCCEEPPTT__MMAANNIIFFOOLLDD__TTYYPPEE(T)   using _gtsam_IsManifold_##T = gtsam:: │ │ │ │ │ + IsManifold; │ │ │ │ │   │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ -  ggttssaamm::::BBOOOOSSTT__CCOONNCCEEPPTT__RREEQQUUIIRREESS (((_I_s_G_r_o_u_p< G >)),(bool)) │ │ │ │ │ - check_group_invariants(const G &a │ │ │ │ │ -  Check invariants. │ │ │ │ │ -  │ │ │ │ │ -VVaarriiaabblleess │ │ │ │ │ - const G &  ggttssaamm::::bb │ │ │ │ │ -  │ │ │ │ │ -const G double  ggttssaamm::::ttooll │ │ │ │ │ +template │ │ │ │ │ +  ggttssaamm::::BBOOOOSSTT__CCOONNCCEEPPTT__RREEQQUUIIRREESS (((_I_s_T_e_s_t_a_b_l_e< T >)),(bool)) │ │ │ │ │ + check_manifold_invariants(const T &a │ │ │ │ │ +  Check invariants for Manifold type. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Concept check class for variable types with Group properties. │ │ │ │ │ - Date │ │ │ │ │ - November, 2011 │ │ │ │ │ +Base class and basic functions for Manifold types. │ │ │ │ │ Author │ │ │ │ │ Alex Cunningham │ │ │ │ │ Frank Dellaert │ │ │ │ │ -********** MMaaccrroo DDeeffiinniittiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ -********** _?◆_? GGTTSSAAMM__CCOONNCCEEPPTT__GGRROOUUPP__IINNSSTT ********** │ │ │ │ │ -#define GTSAM_CONCEPT_GROUP_INST (   T )    template class _g_t_s_a_m_:_:_I_s_G_r_o_u_p; │ │ │ │ │ -Macros for using the IsGroup. │ │ │ │ │ - * An instantiation for use inside unit tests │ │ │ │ │ - * A typedef for use inside generic algorithms │ │ │ │ │ -NOTE: intentionally not in the gtsam namespace to allow for classes not in the │ │ │ │ │ -gtsam namespace to be more easily enforced as testable │ │ │ │ │ + Mike Bosse │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _G_r_o_u_p_._h │ │ │ │ │ + * _M_a_n_i_f_o_l_d_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00098.js │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01073.js │ │ │ │┄ Files 75% similar despite different names │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,12 +1,13 @@ │ │ │ │ │ -var a00098 = [ │ │ │ │ │ - ["gtsam::group_tag", "a02332.html", null], │ │ │ │ │ - ["gtsam::multiplicative_group_tag", "a02336.html", null], │ │ │ │ │ - ["gtsam::additive_group_tag", "a02340.html", null], │ │ │ │ │ - ["gtsam::IsGroup< G >", "a02344.html", null], │ │ │ │ │ - ["gtsam::DirectProduct", "a02348.html", "a02348"], │ │ │ │ │ - ["gtsam::traits< DirectProduct< G, H > >", "a02352.html", null], │ │ │ │ │ - ["gtsam::DirectSum< G, H >", "a02356.html", "a02356"], │ │ │ │ │ - ["gtsam::traits< DirectSum< G, H > >", "a02360.html", null], │ │ │ │ │ - ["GTSAM_CONCEPT_GROUP_INST", "a00098.html#aac1ea136475ea605715c809f4b3f0156", null], │ │ │ │ │ - ["BOOST_CONCEPT_REQUIRES", "a00098.html#a2e95b5b5508cf3284107e979401bd1a4", null] │ │ │ │ │ +var a01073 = [ │ │ │ │ │ + ["gtsam::ValueCloneAllocator", "a04608.html", null], │ │ │ │ │ + ["gtsam::Values", "a04612.html", "a04612"], │ │ │ │ │ + ["gtsam::Values::KeyValuePair", "a04616.html", "a04616"], │ │ │ │ │ + ["gtsam::Values::ConstKeyValuePair", "a04620.html", "a04620"], │ │ │ │ │ + ["gtsam::Values::deref_iterator", "a04624.html", null], │ │ │ │ │ + ["gtsam::ValuesKeyAlreadyExists", "a04628.html", "a04628"], │ │ │ │ │ + ["gtsam::ValuesKeyDoesNotExist", "a04632.html", "a04632"], │ │ │ │ │ + ["gtsam::ValuesIncorrectType", "a04636.html", "a04636"], │ │ │ │ │ + ["gtsam::DynamicValuesMismatched", "a04640.html", null], │ │ │ │ │ + ["gtsam::NoMatchFoundForFixed", "a04644.html", null], │ │ │ │ │ + ["gtsam::traits< Values >", "a04648.html", null] │ │ │ │ │ ]; │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00101_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01397_source.html │ │ │ │┄ Files 83% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/FastVector.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/global_includes.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
FastVector.h
│ │ │ │ +
global_includes.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/base/FastDefaultAllocator.h>
│ │ │ │ -
23#include <vector>
│ │ │ │ -
24
│ │ │ │ -
25namespace gtsam {
│ │ │ │ -
26
│ │ │ │ -
32template <typename T>
│ │ │ │ -
33using FastVector =
│ │ │ │ -
34 std::vector<T, typename internal::FastDefaultVectorAllocator<T>::type>;
│ │ │ │ -
35
│ │ │ │ -
36} // namespace gtsam
│ │ │ │ -
FastDefaultAllocator.h
An easy way to control which allocator is used for Fast* collections.
│ │ │ │ -
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │ -
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/config.h> // Configuration from CMake
│ │ │ │ +
22#include <gtsam/base/types.h> // Basic types, constants, and compatibility functions
│ │ │ │ +
23// types.h includes dllexport.h, which contains macros for dllspec tags for Windows DLLs
│ │ │ │ +
types.h
Typedefs for easier changing of types.
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,46 +1,32 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -FastVector.h │ │ │ │ │ +global_includes.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_._h> │ │ │ │ │ -23#include │ │ │ │ │ -24 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ -26 │ │ │ │ │ -32template │ │ │ │ │ -_3_3using _F_a_s_t_V_e_c_t_o_r = │ │ │ │ │ -34 std::vector::type>; │ │ │ │ │ -35 │ │ │ │ │ -36} // namespace gtsam │ │ │ │ │ -_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_._h │ │ │ │ │ -An easy way to control which allocator is used for Fast* collections. │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ -std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ -FastVector │ │ │ │ │ -FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ -DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ -_g_t_s_a_m │ │ │ │ │ -Global functions in a separate testing namespace. │ │ │ │ │ -DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include // Configuration from CMake │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> // Basic types, constants, and compatibility │ │ │ │ │ +functions │ │ │ │ │ +23// types.h includes dllexport.h, which contains macros for dllspec tags for │ │ │ │ │ +Windows DLLs │ │ │ │ │ +_t_y_p_e_s_._h │ │ │ │ │ +Typedefs for easier changing of types. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _b_a_s_e │ │ │ │ │ - * _F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ + * _g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00131.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00149.html │ │ │ │┄ Files 88% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/debug.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/lieProxies.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces | │ │ │ │ -Macros | │ │ │ │ Functions
│ │ │ │ -
debug.h File Reference
│ │ │ │ +
lieProxies.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Global debugging flags. │ │ │ │ +

Provides convenient mappings of common member functions for testing. │ │ │ │ More...

│ │ │ │ │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Macros

│ │ │ │ -#define ISDEBUG(S)   (gtsam::guardedIsDebug(S))
 
│ │ │ │ -#define SETDEBUG(S, V)   ((void)(gtsam::guardedSetDebug(S,V)))
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

│ │ │ │ -bool gtsam::guardedIsDebug (const std::string &s)
 
│ │ │ │ -void gtsam::guardedSetDebug (const std::string &s, const bool v)
 
│ │ │ │ -bool gtsam::isDebugVersion ()
 
│ │ │ │ +template<class T >
T gtsam::testing::between (const T &t1, const T &t2)
 binary functions
 
│ │ │ │ +template<class T >
T gtsam::testing::compose (const T &t1, const T &t2)
 
│ │ │ │ +template<class T >
T gtsam::testing::inverse (const T &t)
 unary functions
 
│ │ │ │ +template<class T , class P >
P gtsam::testing::rotate (const T &r, const P &pt)
 rotation functions
 
│ │ │ │ +template<class T , class P >
P gtsam::testing::unrotate (const T &r, const P &pt)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Global debugging flags.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Feb 1, 2011
│ │ │ │ +

Provides convenient mappings of common member functions for testing.

│ │ │ │ +
Author
Alex Cunningham
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,35 +1,40 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -debug.h File Reference │ │ │ │ │ -Global debugging flags. _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +lieProxies.h File Reference │ │ │ │ │ +Provides convenient mappings of common member functions for testing. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -MMaaccrrooss │ │ │ │ │ -#define  IISSDDEEBBUUGG(S)   (gtsam::guardedIsDebug(S)) │ │ │ │ │ -  │ │ │ │ │ -#define  SSEETTDDEEBBUUGG(S, V)   ((void)(gtsam::guardedSetDebug(S,V))) │ │ │ │ │ -  │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -bool  ggttssaamm::::gguuaarrddeeddIIssDDeebbuugg (const std::string &s) │ │ │ │ │ -  │ │ │ │ │ -void  ggttssaamm::::gguuaarrddeeddSSeettDDeebbuugg (const std::string &s, const bool v) │ │ │ │ │ +template │ │ │ │ │ +T  ggttssaamm::::tteessttiinngg::::bbeettwweeeenn (const T &t1, const T &t2) │ │ │ │ │ +  binary functions │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +T  ggttssaamm::::tteessttiinngg::::ccoommppoossee (const T &t1, const T &t2) │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +T  ggttssaamm::::tteessttiinngg::::iinnvveerrssee (const T &t) │ │ │ │ │ +  unary functions │ │ │ │ │ +  │ │ │ │ │ +template │ │ │ │ │ +P  ggttssaamm::::tteessttiinngg::::rroottaattee (const T &r, const P &pt) │ │ │ │ │ +  rotation functions │ │ │ │ │   │ │ │ │ │ -bool  ggttssaamm::::iissDDeebbuuggVVeerrssiioonn () │ │ │ │ │ +template │ │ │ │ │ +P  ggttssaamm::::tteessttiinngg::::uunnrroottaattee (const T &r, const P &pt) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Global debugging flags. │ │ │ │ │ +Provides convenient mappings of common member functions for testing. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Date │ │ │ │ │ - Feb 1, 2011 │ │ │ │ │ + Alex Cunningham │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _b_a_s_e │ │ │ │ │ - * _d_e_b_u_g_._h │ │ │ │ │ + * _l_i_e_P_r_o_x_i_e_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00131_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00689_source.html │ │ │ │┄ Files 87% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/debug.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/MetisIndex-inl.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
debug.h
│ │ │ │ +
MetisIndex-inl.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#include <gtsam/base/FastMap.h>
│ │ │ │ -
20#include <gtsam/global_includes.h>
│ │ │ │ -
21#include <string>
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <map>
│ │ │ │ +
21#include <vector>
│ │ │ │
22
│ │ │ │ -
23
│ │ │ │ -
24// This file defines granular debugging flags that may be switched on and off
│ │ │ │ -
25// at run time. Typical usage is 'if(ISDEBUG("myFunction"))' to check if the
│ │ │ │ -
26// 'myFunction' flag is enabled, and SETDEBUG("myFunction", true) to enable
│ │ │ │ -
27// this flag, or SETDEBUG("myFunction", false) to disable it.
│ │ │ │ -
28//
│ │ │ │ -
29// Debug flags are created automatically as they are accessed, so they can be
│ │ │ │ -
30// used immediately without explicitly creating them. Each flag defaults to
│ │ │ │ -
31// 'false', i.e. disabled.
│ │ │ │ -
32//
│ │ │ │ -
33// For these macro to have any effect, granular debugging must be enabled by
│ │ │ │ -
34// defining GTSAM_ENABLE_DEBUG. If NDEBUG is not defined, then
│ │ │ │ -
35// GTSAM_ENABLE_DEBUG will be automatically defined and thus granular
│ │ │ │ -
36// debugging enabled.
│ │ │ │ -
37
│ │ │ │ -
38#ifndef NDEBUG
│ │ │ │ -
39#ifndef GTSAM_ENABLE_DEBUG
│ │ │ │ -
40#define GTSAM_ENABLE_DEBUG
│ │ │ │ -
41#endif
│ │ │ │ -
42#endif
│ │ │ │ -
43
│ │ │ │ -
44namespace gtsam {
│ │ │ │ -
45 GTSAM_EXTERN_EXPORT FastMap<std::string, ValueWithDefault<bool,false> > debugFlags;
│ │ │ │ -
46
│ │ │ │ -
47 // Non-guarded use led to crashes, and solved in commit cd35db2
│ │ │ │ -
48 bool GTSAM_EXPORT guardedIsDebug(const std::string& s);
│ │ │ │ -
49 void GTSAM_EXPORT guardedSetDebug(const std::string& s, const bool v);
│ │ │ │ -
50
│ │ │ │ -
51 // function to check if compiled version has debug information
│ │ │ │ -
52 bool GTSAM_EXPORT isDebugVersion();
│ │ │ │ -
53}
│ │ │ │ -
54
│ │ │ │ -
55#undef ISDEBUG
│ │ │ │ -
56#undef SETDEBUG
│ │ │ │ -
57
│ │ │ │ -
58#ifdef GTSAM_ENABLE_DEBUG
│ │ │ │ -
59
│ │ │ │ -
60#define ISDEBUG(S) (gtsam::guardedIsDebug(S))
│ │ │ │ -
61#define SETDEBUG(S,V) ((void)(gtsam::guardedSetDebug(S,V)))
│ │ │ │ -
62
│ │ │ │ -
63#else
│ │ │ │ -
64
│ │ │ │ -
65#define ISDEBUG(S) (false)
│ │ │ │ -
66#define SETDEBUG(S,V) ((void)false)
│ │ │ │ +
23namespace gtsam {
│ │ │ │ +
24
│ │ │ │ +
25/* ************************************************************************* */
│ │ │ │ +
26template<class FACTORGRAPH>
│ │ │ │ +
│ │ │ │ +
27void MetisIndex::augment(const FACTORGRAPH& factors) {
│ │ │ │ +
28 std::map<int32_t, std::set<int32_t> > iAdjMap; // Stores a set of keys that are adjacent to key x, with adjMap.first
│ │ │ │ +
29 std::map<int32_t, std::set<int32_t> >::iterator iAdjMapIt;
│ │ │ │ +
30 std::set<Key> keySet;
│ │ │ │ +
31
│ │ │ │ +
32 /* ********** Convert to CSR format ********** */
│ │ │ │ +
33 // Assuming that vertex numbering starts from 0 (C style),
│ │ │ │ +
34 // then the adjacency list of vertex i is stored in array adjncy
│ │ │ │ +
35 // starting at index xadj[i] and ending at(but not including)
│ │ │ │ +
36 // index xadj[i + 1](i.e., adjncy[xadj[i]] through
│ │ │ │ +
37 // and including adjncy[xadj[i + 1] - 1]).
│ │ │ │ +
38 int32_t keyCounter = 0;
│ │ │ │ +
39
│ │ │ │ +
40 // First: Record a copy of each key inside the factorgraph and create a
│ │ │ │ +
41 // key to integer mapping. This is referenced during the adjaceny step
│ │ │ │ +
42 for (size_t i = 0; i < factors.size(); i++) {
│ │ │ │ +
43 if (factors[i]) {
│ │ │ │ +
44 for(const Key& key: *factors[i]) {
│ │ │ │ +
45 keySet.insert(keySet.end(), key); // Keep a track of all unique keys
│ │ │ │ +
46 if (intKeyBMap_.left.find(key) == intKeyBMap_.left.end()) {
│ │ │ │ +
47 intKeyBMap_.insert(bm_type::value_type(key, keyCounter));
│ │ │ │ +
48 keyCounter++;
│ │ │ │ +
49 }
│ │ │ │ +
50 }
│ │ │ │ +
51 }
│ │ │ │ +
52 }
│ │ │ │ +
53
│ │ │ │ +
54 // Create an adjacency mapping that stores the set of all adjacent keys for every key
│ │ │ │ +
55 for (size_t i = 0; i < factors.size(); i++) {
│ │ │ │ +
56 if (factors[i]) {
│ │ │ │ +
57 for(const Key& k1: *factors[i])
│ │ │ │ +
58 for(const Key& k2: *factors[i])
│ │ │ │ +
59 if (k1 != k2) {
│ │ │ │ +
60 // Store both in Key and int32_t format
│ │ │ │ +
61 int i = intKeyBMap_.left.at(k1);
│ │ │ │ +
62 int j = intKeyBMap_.left.at(k2);
│ │ │ │ +
63 iAdjMap[i].insert(iAdjMap[i].end(), j);
│ │ │ │ +
64 }
│ │ │ │ +
65 }
│ │ │ │ +
66 }
│ │ │ │
67
│ │ │ │ -
68#endif
│ │ │ │ -
69
│ │ │ │ -
FastMap.h
A thin wrapper around std::map that uses boost's fast_pool_allocator.
│ │ │ │ -
global_includes.h
Included from all GTSAM files.
│ │ │ │ +
68 // Number of keys referenced in this factor graph
│ │ │ │ +
69 nKeys_ = keySet.size();
│ │ │ │ +
70
│ │ │ │ +
71 xadj_.push_back(0); // Always set the first index to zero
│ │ │ │ +
72 for (iAdjMapIt = iAdjMap.begin(); iAdjMapIt != iAdjMap.end(); ++iAdjMapIt) {
│ │ │ │ +
73 std::vector<int32_t> temp;
│ │ │ │ +
74 // Copy from the FastSet into a temporary vector
│ │ │ │ +
75 std::copy(iAdjMapIt->second.begin(), iAdjMapIt->second.end(),
│ │ │ │ +
76 std::back_inserter(temp));
│ │ │ │ +
77 // Insert each index's set in order by appending them to the end of adj_
│ │ │ │ +
78 adj_.insert(adj_.end(), temp.begin(), temp.end());
│ │ │ │ +
79 //adj_.push_back(temp);
│ │ │ │ +
80 xadj_.push_back((int32_t) adj_.size());
│ │ │ │ +
81 }
│ │ │ │ +
82}
│ │ │ │ +
│ │ │ │ +
83
│ │ │ │ +
84} // \ gtsam
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::MetisIndex::augment
void augment(const FACTORGRAPH &factors)
Augment the variable index with new factors.
Definition MetisIndex-inl.h:27
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,83 +1,105 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -debug.h │ │ │ │ │ +MetisIndex-inl.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_M_a_p_._h> │ │ │ │ │ -20#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ -21#include │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include │ │ │ │ │ +21#include │ │ │ │ │ 22 │ │ │ │ │ -23 │ │ │ │ │ -24// This file defines granular debugging flags that may be switched on and off │ │ │ │ │ -25// at run time. Typical usage is 'if(ISDEBUG("myFunction"))' to check if the │ │ │ │ │ -26// 'myFunction' flag is enabled, and SETDEBUG("myFunction", true) to enable │ │ │ │ │ -27// this flag, or SETDEBUG("myFunction", false) to disable it. │ │ │ │ │ -28// │ │ │ │ │ -29// Debug flags are created automatically as they are accessed, so they can be │ │ │ │ │ -30// used immediately without explicitly creating them. Each flag defaults to │ │ │ │ │ -31// 'false', i.e. disabled. │ │ │ │ │ -32// │ │ │ │ │ -33// For these macro to have any effect, granular debugging must be enabled by │ │ │ │ │ -34// defining GTSAM_ENABLE_DEBUG. If NDEBUG is not defined, then │ │ │ │ │ -35// GTSAM_ENABLE_DEBUG will be automatically defined and thus granular │ │ │ │ │ -36// debugging enabled. │ │ │ │ │ -37 │ │ │ │ │ -38#ifndef NDEBUG │ │ │ │ │ -39#ifndef GTSAM_ENABLE_DEBUG │ │ │ │ │ -40#define GTSAM_ENABLE_DEBUG │ │ │ │ │ -41#endif │ │ │ │ │ -42#endif │ │ │ │ │ -43 │ │ │ │ │ -44namespace _g_t_s_a_m { │ │ │ │ │ -45 GTSAM_EXTERN_EXPORT FastMap > │ │ │ │ │ -debugFlags; │ │ │ │ │ -46 │ │ │ │ │ -47 // Non-guarded use led to crashes, and solved in commit cd35db2 │ │ │ │ │ -48 bool GTSAM_EXPORT guardedIsDebug(const std::string& s); │ │ │ │ │ -49 void GTSAM_EXPORT guardedSetDebug(const std::string& s, const bool v); │ │ │ │ │ -50 │ │ │ │ │ -51 // function to check if compiled version has debug information │ │ │ │ │ -52 bool GTSAM_EXPORT isDebugVersion(); │ │ │ │ │ -53} │ │ │ │ │ -54 │ │ │ │ │ -55#undef ISDEBUG │ │ │ │ │ -56#undef SETDEBUG │ │ │ │ │ -57 │ │ │ │ │ -58#ifdef GTSAM_ENABLE_DEBUG │ │ │ │ │ -59 │ │ │ │ │ -60#define ISDEBUG(S) (gtsam::guardedIsDebug(S)) │ │ │ │ │ -61#define SETDEBUG(S,V) ((void)(gtsam::guardedSetDebug(S,V))) │ │ │ │ │ -62 │ │ │ │ │ -63#else │ │ │ │ │ -64 │ │ │ │ │ -65#define ISDEBUG(S) (false) │ │ │ │ │ -66#define SETDEBUG(S,V) ((void)false) │ │ │ │ │ +23namespace _g_t_s_a_m { │ │ │ │ │ +24 │ │ │ │ │ +25/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +26template │ │ │ │ │ +_2_7void _M_e_t_i_s_I_n_d_e_x_:_:_a_u_g_m_e_n_t(const FACTORGRAPH& factors) { │ │ │ │ │ +28 std::map > iAdjMap; // Stores a set of keys that │ │ │ │ │ +are adjacent to key x, with adjMap.first │ │ │ │ │ +29 std::map >::iterator iAdjMapIt; │ │ │ │ │ +30 std::set keySet; │ │ │ │ │ +31 │ │ │ │ │ +32 /* ********** Convert to CSR format ********** */ │ │ │ │ │ +33 // Assuming that vertex numbering starts from 0 (C style), │ │ │ │ │ +34 // then the adjacency list of vertex i is stored in array adjncy │ │ │ │ │ +35 // starting at index xadj[i] and ending at(but not including) │ │ │ │ │ +36 // index xadj[i + 1](i.e., adjncy[xadj[i]] through │ │ │ │ │ +37 // and including adjncy[xadj[i + 1] - 1]). │ │ │ │ │ +38 int32_t keyCounter = 0; │ │ │ │ │ +39 │ │ │ │ │ +40 // First: Record a copy of each key inside the factorgraph and create a │ │ │ │ │ +41 // key to integer mapping. This is referenced during the adjaceny step │ │ │ │ │ +42 for (size_t i = 0; i < factors.size(); i++) { │ │ │ │ │ +43 if (factors[i]) { │ │ │ │ │ +44 for(const _K_e_y& key: *factors[i]) { │ │ │ │ │ +45 keySet.insert(keySet.end(), key); // Keep a track of all unique keys │ │ │ │ │ +46 if (intKeyBMap_.left.find(key) == intKeyBMap_.left.end()) { │ │ │ │ │ +47 intKeyBMap_.insert(bm_type::value_type(key, keyCounter)); │ │ │ │ │ +48 keyCounter++; │ │ │ │ │ +49 } │ │ │ │ │ +50 } │ │ │ │ │ +51 } │ │ │ │ │ +52 } │ │ │ │ │ +53 │ │ │ │ │ +54 // Create an adjacency mapping that stores the set of all adjacent keys for │ │ │ │ │ +every key │ │ │ │ │ +55 for (size_t i = 0; i < factors.size(); i++) { │ │ │ │ │ +56 if (factors[i]) { │ │ │ │ │ +57 for(const _K_e_y& k1: *factors[i]) │ │ │ │ │ +58 for(const _K_e_y& k2: *factors[i]) │ │ │ │ │ +59 if (k1 != k2) { │ │ │ │ │ +60 // Store both in Key and int32_t format │ │ │ │ │ +61 int i = intKeyBMap_.left.at(k1); │ │ │ │ │ +62 int j = intKeyBMap_.left.at(k2); │ │ │ │ │ +63 iAdjMap[i].insert(iAdjMap[i].end(), j); │ │ │ │ │ +64 } │ │ │ │ │ +65 } │ │ │ │ │ +66 } │ │ │ │ │ 67 │ │ │ │ │ -68#endif │ │ │ │ │ -69 │ │ │ │ │ -_F_a_s_t_M_a_p_._h │ │ │ │ │ -A thin wrapper around std::map that uses boost's fast_pool_allocator. │ │ │ │ │ -_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ -Included from all GTSAM files. │ │ │ │ │ +68 // Number of keys referenced in this factor graph │ │ │ │ │ +69 nKeys_ = keySet.size(); │ │ │ │ │ +70 │ │ │ │ │ +71 xadj_.push_back(0); // Always set the first index to zero │ │ │ │ │ +72 for (iAdjMapIt = iAdjMap.begin(); iAdjMapIt != iAdjMap.end(); ++iAdjMapIt) { │ │ │ │ │ +73 std::vector temp; │ │ │ │ │ +74 // Copy from the FastSet into a temporary vector │ │ │ │ │ +75 std::copy(iAdjMapIt->second.begin(), iAdjMapIt->second.end(), │ │ │ │ │ +76 std::back_inserter(temp)); │ │ │ │ │ +77 // Insert each index's set in order by appending them to the end of adj_ │ │ │ │ │ +78 adj_.insert(adj_.end(), temp.begin(), temp.end()); │ │ │ │ │ +79 //adj_.push_back(temp); │ │ │ │ │ +80 xadj_.push_back((int32_t) adj_.size()); │ │ │ │ │ +81 } │ │ │ │ │ +82} │ │ │ │ │ +83 │ │ │ │ │ +84} // \ gtsam │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_M_e_t_i_s_I_n_d_e_x_:_:_a_u_g_m_e_n_t │ │ │ │ │ +void augment(const FACTORGRAPH &factors) │ │ │ │ │ +Augment the variable index with new factors. │ │ │ │ │ +DDeeffiinniittiioonn MetisIndex-inl.h:27 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _b_a_s_e │ │ │ │ │ - * _d_e_b_u_g_._h │ │ │ │ │ + * _i_n_f_e_r_e_n_c_e │ │ │ │ │ + * _M_e_t_i_s_I_n_d_e_x_-_i_n_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00140_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00182_source.html │ │ │ │┄ Files 88% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/WeightedSampler.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteBayesTree.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
WeightedSampler.h
│ │ │ │ +
DiscreteBayesTree.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <cmath>
│ │ │ │ -
22#include <queue>
│ │ │ │ -
23#include <random>
│ │ │ │ -
24#include <stdexcept>
│ │ │ │ -
25#include <utility>
│ │ │ │ -
26#include <vector>
│ │ │ │ -
27
│ │ │ │ -
28namespace gtsam {
│ │ │ │ -
29/*
│ │ │ │ -
30 * Fast sampling without replacement.
│ │ │ │ -
31 * Example usage:
│ │ │ │ -
32 * std::mt19937 rng(42);
│ │ │ │ -
33 * WeightedSampler<std::mt19937> sampler(&rng);
│ │ │ │ -
34 * auto samples = sampler.sampleWithoutReplacement(5, weights);
│ │ │ │ -
35 */
│ │ │ │ -
36template <class Engine = std::mt19937>
│ │ │ │ -
│ │ │ │ -
37class WeightedSampler {
│ │ │ │ -
38 private:
│ │ │ │ -
39 Engine* engine_; // random number generation engine
│ │ │ │ -
40
│ │ │ │ +
21#pragma once
│ │ │ │ +
22
│ │ │ │ +
23#include <gtsam/discrete/DiscreteBayesNet.h>
│ │ │ │ +
24#include <gtsam/discrete/DiscreteFactorGraph.h>
│ │ │ │ +
25#include <gtsam/inference/BayesTree.h>
│ │ │ │ +
26#include <gtsam/inference/Conditional.h>
│ │ │ │ +
27#include <gtsam/inference/BayesTreeCliqueBase.h>
│ │ │ │ +
28
│ │ │ │ +
29#include <string>
│ │ │ │ +
30
│ │ │ │ +
31namespace gtsam {
│ │ │ │ +
32
│ │ │ │ +
33// Forward declarations
│ │ │ │ +
34class DiscreteConditional;
│ │ │ │ +
35class VectorValues;
│ │ │ │ +
36
│ │ │ │ +
37/* ************************************************************************* */
│ │ │ │ +
│ │ │ │ +
39class GTSAM_EXPORT DiscreteBayesTreeClique
│ │ │ │ +
40 : public BayesTreeCliqueBase<DiscreteBayesTreeClique, DiscreteFactorGraph> {
│ │ │ │
41 public:
│ │ │ │ -
46 explicit WeightedSampler(Engine* engine) : engine_(engine) {}
│ │ │ │ -
47
│ │ │ │ -
48 std::vector<size_t> sampleWithoutReplacement(
│ │ │ │ -
49 size_t numSamples, const std::vector<double>& weights) {
│ │ │ │ -
50 // Implementation adapted from code accompanying paper at
│ │ │ │ -
51 // https://www.ethz.ch/content/dam/ethz/special-interest/baug/ivt/ivt-dam/vpl/reports/1101-1200/ab1141.pdf
│ │ │ │ -
52 const size_t n = weights.size();
│ │ │ │ -
53 if (n < numSamples) {
│ │ │ │ -
54 throw std::runtime_error(
│ │ │ │ -
55 "numSamples must be smaller than weights.size()");
│ │ │ │ -
56 }
│ │ │ │ -
57
│ │ │ │ -
58 // Return empty array if numSamples==0
│ │ │ │ -
59 std::vector<size_t> result(numSamples);
│ │ │ │ -
60 if (numSamples == 0) return result;
│ │ │ │ -
61
│ │ │ │ -
62 // Step 1: The first m items of V are inserted into reservoir
│ │ │ │ -
63 // Step 2: For each item v_i ∈ reservoir: Calculate a key k_i = u_i^(1/w),
│ │ │ │ -
64 // where u_i = random(0, 1)
│ │ │ │ -
65 // (Modification: Calculate and store -log k_i = e_i / w where e_i = exp(1),
│ │ │ │ -
66 // reservoir is a priority queue that pops the *maximum* elements)
│ │ │ │ -
67 std::priority_queue<std::pair<double, size_t> > reservoir;
│ │ │ │ +
42 typedef DiscreteBayesTreeClique This;
│ │ │ │ +
43 typedef BayesTreeCliqueBase<DiscreteBayesTreeClique, DiscreteFactorGraph>
│ │ │ │ +
44 Base;
│ │ │ │ +
45 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
46 typedef boost::weak_ptr<This> weak_ptr;
│ │ │ │ +
47 DiscreteBayesTreeClique() {}
│ │ │ │ +
48 virtual ~DiscreteBayesTreeClique() {}
│ │ │ │ +
49 DiscreteBayesTreeClique(
│ │ │ │ +
50 const boost::shared_ptr<DiscreteConditional>& conditional)
│ │ │ │ +
51 : Base(conditional) {}
│ │ │ │ +
52
│ │ │ │ +
│ │ │ │ +
54 void printSignature(
│ │ │ │ +
55 const std::string& s = "Clique: ",
│ │ │ │ +
56 const KeyFormatter& formatter = DefaultKeyFormatter) const {
│ │ │ │ +
57 conditional_->printSignature(s, formatter);
│ │ │ │ +
58 }
│ │ │ │ +
│ │ │ │ +
59
│ │ │ │ +
60 //** evaluate conditional probability of subtree for given DiscreteValues */
│ │ │ │ +
61 double evaluate(const DiscreteValues& values) const;
│ │ │ │ +
62
│ │ │ │ +
63 //** (Preferred) sugar for the above for given DiscreteValues */
│ │ │ │ +
64 double operator()(const DiscreteValues& values) const {
│ │ │ │ +
65 return evaluate(values);
│ │ │ │ +
66 }
│ │ │ │ +
67};
│ │ │ │ +
│ │ │ │
68
│ │ │ │ -
69 static const double kexp1 = std::exp(1.0);
│ │ │ │ -
70 for (auto it = weights.begin(); it != weights.begin() + numSamples; ++it) {
│ │ │ │ -
71 const double k_i = kexp1 / *it;
│ │ │ │ -
72 reservoir.push(std::make_pair(k_i, it - weights.begin() + 1));
│ │ │ │ -
73 }
│ │ │ │ -
74
│ │ │ │ -
75 // Step 4: Repeat Steps 5–10 until the population is exhausted
│ │ │ │ -
76 {
│ │ │ │ -
77 // Step 3: The threshold T_w is the minimum key of reservoir
│ │ │ │ -
78 // (Modification: This is now the logarithm)
│ │ │ │ -
79 // Step 10: The new threshold T w is the new minimum key of reservoir
│ │ │ │ -
80 const std::pair<double, size_t>& T_w = reservoir.top();
│ │ │ │ -
81
│ │ │ │ -
82 // Incrementing it is part of Step 7
│ │ │ │ -
83 for (auto it = weights.begin() + numSamples; it != weights.end(); ++it) {
│ │ │ │ -
84 // Step 5: Let r = random(0, 1) and X_w = log(r) / log(T_w)
│ │ │ │ -
85 // (Modification: Use e = -exp(1) instead of log(r))
│ │ │ │ -
86 const double X_w = kexp1 / T_w.first;
│ │ │ │ +
69/* ************************************************************************* */
│ │ │ │ +
│ │ │ │ +
74class GTSAM_EXPORT DiscreteBayesTree
│ │ │ │ +
75 : public BayesTree<DiscreteBayesTreeClique> {
│ │ │ │ +
76 private:
│ │ │ │ +
77 typedef BayesTree<DiscreteBayesTreeClique> Base;
│ │ │ │ +
78
│ │ │ │ +
79 public:
│ │ │ │ +
80 typedef DiscreteBayesTree This;
│ │ │ │ +
81 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
82
│ │ │ │ +
85
│ │ │ │ +
86 DiscreteBayesTree() {}
│ │ │ │
87
│ │ │ │ -
88 // Step 6: From the current item v_c skip items until item v_i, such
│ │ │ │ -
89 // that:
│ │ │ │ -
90 double w = 0.0;
│ │ │ │ -
91
│ │ │ │ -
92 // Step 7: w_c + w_{c+1} + ··· + w_{i−1} < X_w <= w_c + w_{c+1} + ··· +
│ │ │ │ -
93 // w_{i−1} + w_i
│ │ │ │ -
94 for (; it != weights.end(); ++it) {
│ │ │ │ -
95 w += *it;
│ │ │ │ -
96 if (X_w <= w) break;
│ │ │ │ -
97 }
│ │ │ │ +
89 bool equals(const This& other, double tol = 1e-9) const;
│ │ │ │ +
90
│ │ │ │ +
91 //** evaluate probability for given DiscreteValues */
│ │ │ │ +
92 double evaluate(const DiscreteValues& values) const;
│ │ │ │ +
93
│ │ │ │ +
94 //** (Preferred) sugar for the above for given DiscreteValues */
│ │ │ │ +
95 double operator()(const DiscreteValues& values) const {
│ │ │ │ +
96 return evaluate(values);
│ │ │ │ +
97 }
│ │ │ │
98
│ │ │ │ -
99 // Step 7: No such item, terminate
│ │ │ │ -
100 if (it == weights.end()) break;
│ │ │ │ -
101
│ │ │ │ -
102 // Step 9: Let t_w = T_w^{w_i}, r_2 = random(t_w, 1) and v_i’s key: k_i
│ │ │ │ -
103 // = (r_2)^{1/w_i} (Mod: Let t_w = log(T_w) * {w_i}, e_2 =
│ │ │ │ -
104 // log(random(e^{t_w}, 1)) and v_i’s key: k_i = -e_2 / w_i)
│ │ │ │ -
105 const double t_w = -T_w.first * *it;
│ │ │ │ -
106 std::uniform_real_distribution<double> randomAngle(std::exp(t_w), 1.0);
│ │ │ │ -
107 const double e_2 = std::log(randomAngle(*engine_));
│ │ │ │ -
108 const double k_i = -e_2 / *it;
│ │ │ │ -
109
│ │ │ │ -
110 // Step 8: The item in reservoir with the minimum key is replaced by
│ │ │ │ -
111 // item v_i
│ │ │ │ -
112 reservoir.pop();
│ │ │ │ -
113 reservoir.push(std::make_pair(k_i, it - weights.begin() + 1));
│ │ │ │ -
114 }
│ │ │ │ -
115 }
│ │ │ │ -
116
│ │ │ │ -
117 for (auto iret = result.end(); iret != result.begin();) {
│ │ │ │ -
118 --iret;
│ │ │ │ -
119
│ │ │ │ -
120 if (reservoir.empty()) {
│ │ │ │ -
121 throw std::runtime_error(
│ │ │ │ -
122 "Reservoir empty before all elements have been filled");
│ │ │ │ -
123 }
│ │ │ │ -
124
│ │ │ │ -
125 *iret = reservoir.top().second - 1;
│ │ │ │ -
126 reservoir.pop();
│ │ │ │ -
127 }
│ │ │ │ -
128
│ │ │ │ -
129 if (!reservoir.empty()) {
│ │ │ │ -
130 throw std::runtime_error(
│ │ │ │ -
131 "Reservoir not empty after all elements have been filled");
│ │ │ │ -
132 }
│ │ │ │ -
133
│ │ │ │ -
134 return result;
│ │ │ │ -
135 }
│ │ │ │ -
136}; // namespace gtsam
│ │ │ │ -
│ │ │ │ -
137} // namespace gtsam
│ │ │ │ +
102
│ │ │ │ +
104 std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
105 const DiscreteFactor::Names& names = {}) const;
│ │ │ │ +
106
│ │ │ │ +
108 std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
109 const DiscreteFactor::Names& names = {}) const;
│ │ │ │ +
110
│ │ │ │ +
112};
│ │ │ │ +
│ │ │ │ +
113
│ │ │ │ +
114} // namespace gtsam
│ │ │ │ + │ │ │ │ + │ │ │ │ +
BayesTreeCliqueBase.h
Base class for cliques of a BayesTree.
│ │ │ │ +
Conditional.h
Base class for conditional densities.
│ │ │ │ +
BayesTree.h
Bayes Tree is a tree of cliques of a Bayes Chain.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::WeightedSampler
Definition WeightedSampler.h:37
│ │ │ │ -
gtsam::WeightedSampler::WeightedSampler
WeightedSampler(Engine *engine)
Construct from random number generation engine We only store a pointer to it.
Definition WeightedSampler.h:46
│ │ │ │ +
gtsam::html
string html(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
Free version of html.
Definition DiscreteValues.cpp:134
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::DiscreteBayesTreeClique
A clique in a DiscreteBayesTree.
Definition DiscreteBayesTree.h:40
│ │ │ │ +
gtsam::DiscreteBayesTreeClique::printSignature
void printSignature(const std::string &s="Clique: ", const KeyFormatter &formatter=DefaultKeyFormatter) const
print index signature only
Definition DiscreteBayesTree.h:54
│ │ │ │ +
gtsam::DiscreteBayesTree
A Bayes tree representing a Discrete density.
Definition DiscreteBayesTree.h:75
│ │ │ │ +
gtsam::DiscreteBayesTree::DiscreteBayesTree
DiscreteBayesTree()
Default constructor, creates an empty Bayes tree.
Definition DiscreteBayesTree.h:86
│ │ │ │ +
gtsam::DiscreteValues
A map from keys to values.
Definition DiscreteValues.h:34
│ │ │ │ +
gtsam::BayesTree
Bayes tree.
Definition BayesTree.h:67
│ │ │ │ +
gtsam::BayesTreeCliqueBase
This is the base class for BayesTree cliques.
Definition BayesTreeCliqueBase.h:50
│ │ │ │ +
VectorValues
The Factor::error simply extracts the.
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── encoding │ │ │ │ │ @@ -1 +1 @@ │ │ │ │ │ -utf-8 │ │ │ │ │ +us-ascii │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,150 +1,155 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -WeightedSampler.h │ │ │ │ │ +DiscreteBayesTree.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include │ │ │ │ │ -22#include │ │ │ │ │ -23#include │ │ │ │ │ -24#include │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ -27 │ │ │ │ │ -28namespace _g_t_s_a_m { │ │ │ │ │ -29/* │ │ │ │ │ -30 * Fast sampling without replacement. │ │ │ │ │ -31 * Example usage: │ │ │ │ │ -32 * std::mt19937 rng(42); │ │ │ │ │ -33 * WeightedSampler sampler(&rng); │ │ │ │ │ -34 * auto samples = sampler.sampleWithoutReplacement(5, weights); │ │ │ │ │ -35 */ │ │ │ │ │ -36template │ │ │ │ │ -_3_7class _W_e_i_g_h_t_e_d_S_a_m_p_l_e_r { │ │ │ │ │ -38 private: │ │ │ │ │ -39 Engine* engine_; // random number generation engine │ │ │ │ │ -40 │ │ │ │ │ +21#pragma once │ │ │ │ │ +22 │ │ │ │ │ +23#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ +27#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._h> │ │ │ │ │ +28 │ │ │ │ │ +29#include │ │ │ │ │ +30 │ │ │ │ │ +31namespace _g_t_s_a_m { │ │ │ │ │ +32 │ │ │ │ │ +33// Forward declarations │ │ │ │ │ +34class DiscreteConditional; │ │ │ │ │ +35class _V_e_c_t_o_r_V_a_l_u_e_s; │ │ │ │ │ +36 │ │ │ │ │ +37/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +_3_9class GTSAM_EXPORT _D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e │ │ │ │ │ +40 : public _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e { │ │ │ │ │ 41 public: │ │ │ │ │ -_4_6 explicit _W_e_i_g_h_t_e_d_S_a_m_p_l_e_r(Engine* engine) : engine_(engine) {} │ │ │ │ │ -47 │ │ │ │ │ -48 std::vector sampleWithoutReplacement( │ │ │ │ │ -49 size_t numSamples, const std::vector& weights) { │ │ │ │ │ -50 // Implementation adapted from code accompanying paper at │ │ │ │ │ -51 // https://www.ethz.ch/content/dam/ethz/special-interest/baug/ivt/ivt-dam/ │ │ │ │ │ -vpl/reports/1101-1200/ab1141.pdf │ │ │ │ │ -52 const size_t n = weights.size(); │ │ │ │ │ -53 if (n < numSamples) { │ │ │ │ │ -54 throw std::runtime_error( │ │ │ │ │ -55 "numSamples must be smaller than weights.size()"); │ │ │ │ │ -56 } │ │ │ │ │ -57 │ │ │ │ │ -58 // Return empty array if numSamples==0 │ │ │ │ │ -59 std::vector result(numSamples); │ │ │ │ │ -60 if (numSamples == 0) return result; │ │ │ │ │ -61 │ │ │ │ │ -62 // Step 1: The first m items of V are inserted into reservoir │ │ │ │ │ -63 // Step 2: For each item v_i ∈ reservoir: Calculate a key k_i = u_i^(1/w), │ │ │ │ │ -64 // where u_i = random(0, 1) │ │ │ │ │ -65 // (Modification: Calculate and store -log k_i = e_i / w where e_i = exp(1), │ │ │ │ │ -66 // reservoir is a priority queue that pops the *maximum* elements) │ │ │ │ │ -67 std::priority_queue > reservoir; │ │ │ │ │ +42 typedef _D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e _T_h_i_s; │ │ │ │ │ +43 typedef _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_,_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_> │ │ │ │ │ +44 _B_a_s_e; │ │ │ │ │ +45 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +46 typedef boost::weak_ptr weak_ptr; │ │ │ │ │ +47 _D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e() {} │ │ │ │ │ +48 virtual _~_D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e() {} │ │ │ │ │ +49 _D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e( │ │ │ │ │ +50 const boost::shared_ptr& conditional) │ │ │ │ │ +51 : _B_a_s_e(conditional) {} │ │ │ │ │ +52 │ │ │ │ │ +_5_4 void _p_r_i_n_t_S_i_g_n_a_t_u_r_e( │ │ │ │ │ +55 const std::string& s = "Clique: ", │ │ │ │ │ +56 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const { │ │ │ │ │ +57 conditional_->printSignature(s, formatter); │ │ │ │ │ +58 } │ │ │ │ │ +59 │ │ │ │ │ +60 //** evaluate conditional probability of subtree for given DiscreteValues */ │ │ │ │ │ +61 double evaluate(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values) const; │ │ │ │ │ +62 │ │ │ │ │ +63 //** (Preferred) sugar for the above for given DiscreteValues */ │ │ │ │ │ +64 double operator()(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values) const { │ │ │ │ │ +65 return evaluate(values); │ │ │ │ │ +66 } │ │ │ │ │ +67}; │ │ │ │ │ 68 │ │ │ │ │ -69 static const double kexp1 = std::exp(1.0); │ │ │ │ │ -70 for (auto it = weights.begin(); it != weights.begin() + numSamples; ++it) { │ │ │ │ │ -71 const double k_i = kexp1 / *it; │ │ │ │ │ -72 reservoir.push(std::make_pair(k_i, it - weights.begin() + 1)); │ │ │ │ │ -73 } │ │ │ │ │ -74 │ │ │ │ │ -75 // Step 4: Repeat Steps 5–10 until the population is exhausted │ │ │ │ │ -76 { │ │ │ │ │ -77 // Step 3: The threshold T_w is the minimum key of reservoir │ │ │ │ │ -78 // (Modification: This is now the logarithm) │ │ │ │ │ -79 // Step 10: The new threshold T w is the new minimum key of reservoir │ │ │ │ │ -80 const std::pair& T_w = reservoir.top(); │ │ │ │ │ -81 │ │ │ │ │ -82 // Incrementing it is part of Step 7 │ │ │ │ │ -83 for (auto it = weights.begin() + numSamples; it != weights.end(); ++it) { │ │ │ │ │ -84 // Step 5: Let r = random(0, 1) and X_w = log(r) / log(T_w) │ │ │ │ │ -85 // (Modification: Use e = -exp(1) instead of log(r)) │ │ │ │ │ -86 const double X_w = kexp1 / T_w.first; │ │ │ │ │ +69/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +_7_4class GTSAM_EXPORT _D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e │ │ │ │ │ +75 : public _B_a_y_e_s_T_r_e_e { │ │ │ │ │ +76 private: │ │ │ │ │ +77 typedef _B_a_y_e_s_T_r_e_e_<_D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_> _B_a_s_e; │ │ │ │ │ +78 │ │ │ │ │ +79 public: │ │ │ │ │ +80 typedef _D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e _T_h_i_s; │ │ │ │ │ +81 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +82 │ │ │ │ │ +85 │ │ │ │ │ +_8_6 _D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e() {} │ │ │ │ │ 87 │ │ │ │ │ -88 // Step 6: From the current item v_c skip items until item v_i, such │ │ │ │ │ -89 // that: │ │ │ │ │ -90 double w = 0.0; │ │ │ │ │ -91 │ │ │ │ │ -92 // Step 7: w_c + w_{c+1} + ··· + w_{i−1} < X_w <= w_c + w_{c+1} + │ │ │ │ │ -··· + │ │ │ │ │ -93 // w_{i−1} + w_i │ │ │ │ │ -94 for (; it != weights.end(); ++it) { │ │ │ │ │ -95 w += *it; │ │ │ │ │ -96 if (X_w <= w) break; │ │ │ │ │ +89 bool _e_q_u_a_l_s(const This& other, double tol = 1e-9) const; │ │ │ │ │ +90 │ │ │ │ │ +91 //** evaluate probability for given DiscreteValues */ │ │ │ │ │ +92 double evaluate(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values) const; │ │ │ │ │ +93 │ │ │ │ │ +94 //** (Preferred) sugar for the above for given DiscreteValues */ │ │ │ │ │ +95 double operator()(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values) const { │ │ │ │ │ +96 return evaluate(values); │ │ │ │ │ 97 } │ │ │ │ │ 98 │ │ │ │ │ -99 // Step 7: No such item, terminate │ │ │ │ │ -100 if (it == weights.end()) break; │ │ │ │ │ -101 │ │ │ │ │ -102 // Step 9: Let t_w = T_w^{w_i}, r_2 = random(t_w, 1) and v_i’s key: k_i │ │ │ │ │ -103 // = (r_2)^{1/w_i} (Mod: Let t_w = log(T_w) * {w_i}, e_2 = │ │ │ │ │ -104 // log(random(e^{t_w}, 1)) and v_i’s key: k_i = -e_2 / w_i) │ │ │ │ │ -105 const double t_w = -T_w.first * *it; │ │ │ │ │ -106 std::uniform_real_distribution randomAngle(std::exp(t_w), 1.0); │ │ │ │ │ -107 const double e_2 = std::log(randomAngle(*engine_)); │ │ │ │ │ -108 const double k_i = -e_2 / *it; │ │ │ │ │ -109 │ │ │ │ │ -110 // Step 8: The item in reservoir with the minimum key is replaced by │ │ │ │ │ -111 // item v_i │ │ │ │ │ -112 reservoir.pop(); │ │ │ │ │ -113 reservoir.push(std::make_pair(k_i, it - weights.begin() + 1)); │ │ │ │ │ -114 } │ │ │ │ │ -115 } │ │ │ │ │ -116 │ │ │ │ │ -117 for (auto iret = result.end(); iret != result.begin();) { │ │ │ │ │ -118 --iret; │ │ │ │ │ -119 │ │ │ │ │ -120 if (reservoir.empty()) { │ │ │ │ │ -121 throw std::runtime_error( │ │ │ │ │ -122 "Reservoir empty before all elements have been filled"); │ │ │ │ │ -123 } │ │ │ │ │ -124 │ │ │ │ │ -125 *iret = reservoir.top().second - 1; │ │ │ │ │ -126 reservoir.pop(); │ │ │ │ │ -127 } │ │ │ │ │ -128 │ │ │ │ │ -129 if (!reservoir.empty()) { │ │ │ │ │ -130 throw std::runtime_error( │ │ │ │ │ -131 "Reservoir not empty after all elements have been filled"); │ │ │ │ │ -132 } │ │ │ │ │ -133 │ │ │ │ │ -134 return result; │ │ │ │ │ -135 } │ │ │ │ │ -136}; // namespace gtsam │ │ │ │ │ -137} // namespace gtsam │ │ │ │ │ +102 │ │ │ │ │ +104 std::string markdown(const KeyFormatter& keyFormatter = │ │ │ │ │ +DefaultKeyFormatter, │ │ │ │ │ +105 const DiscreteFactor::Names& names = {}) const; │ │ │ │ │ +106 │ │ │ │ │ +108 std::string _h_t_m_l(const KeyFormatter& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ +109 const DiscreteFactor::Names& names = {}) const; │ │ │ │ │ +110 │ │ │ │ │ +112}; │ │ │ │ │ +113 │ │ │ │ │ +114} // namespace gtsam │ │ │ │ │ +_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_._h │ │ │ │ │ +_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._h │ │ │ │ │ +Base class for cliques of a BayesTree. │ │ │ │ │ +_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ +Base class for conditional densities. │ │ │ │ │ +_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ +Bayes Tree is a tree of cliques of a Bayes Chain. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_W_e_i_g_h_t_e_d_S_a_m_p_l_e_r │ │ │ │ │ -DDeeffiinniittiioonn WeightedSampler.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_W_e_i_g_h_t_e_d_S_a_m_p_l_e_r_:_:_W_e_i_g_h_t_e_d_S_a_m_p_l_e_r │ │ │ │ │ -WeightedSampler(Engine *engine) │ │ │ │ │ -Construct from random number generation engine We only store a pointer to it. │ │ │ │ │ -DDeeffiinniittiioonn WeightedSampler.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_h_t_m_l │ │ │ │ │ +string html(const DiscreteValues &values, const KeyFormatter &keyFormatter, │ │ │ │ │ +const DiscreteValues::Names &names) │ │ │ │ │ +Free version of html. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteValues.cpp:134 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e │ │ │ │ │ +A clique in a DiscreteBayesTree. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteBayesTree.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_:_:_p_r_i_n_t_S_i_g_n_a_t_u_r_e │ │ │ │ │ +void printSignature(const std::string &s="Clique: ", const KeyFormatter │ │ │ │ │ +&formatter=DefaultKeyFormatter) const │ │ │ │ │ +print index signature only │ │ │ │ │ +DDeeffiinniittiioonn DiscreteBayesTree.h:54 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e │ │ │ │ │ +A Bayes tree representing a Discrete density. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteBayesTree.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e │ │ │ │ │ +DiscreteBayesTree() │ │ │ │ │ +Default constructor, creates an empty Bayes tree. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteBayesTree.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ +A map from keys to values. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteValues.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e │ │ │ │ │ +Bayes tree. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e │ │ │ │ │ +This is the base class for BayesTree cliques. │ │ │ │ │ +DDeeffiinniittiioonn BayesTreeCliqueBase.h:50 │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +The Factor::error simply extracts the. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _b_a_s_e │ │ │ │ │ - * _W_e_i_g_h_t_e_d_S_a_m_p_l_e_r_._h │ │ │ │ │ + * _d_i_s_c_r_e_t_e │ │ │ │ │ + * _D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00152_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00149_source.html │ │ │ │┄ Files 88% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Chebyshev.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/lieProxies.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Chebyshev.h
│ │ │ │ +
lieProxies.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/base/Manifold.h>
│ │ │ │ -
22#include <gtsam/basis/Basis.h>
│ │ │ │ -
23
│ │ │ │ -
24namespace gtsam {
│ │ │ │ -
25
│ │ │ │ -
│ │ │ │ -
32struct GTSAM_EXPORT Chebyshev1Basis : Basis<Chebyshev1Basis> {
│ │ │ │ -
33 using Parameters = Eigen::Matrix<double, -1, 1 /*Nx1*/>;
│ │ │ │ -
34
│ │ │ │ -
35 Parameters parameters_;
│ │ │ │ -
36
│ │ │ │ -
45 static Weights CalculateWeights(size_t N, double x, double a = -1,
│ │ │ │ -
46 double b = 1);
│ │ │ │ -
47
│ │ │ │ -
66 static Weights DerivativeWeights(size_t N, double x, double a = -1,
│ │ │ │ -
67 double b = 1);
│ │ │ │ -
68}; // Chebyshev1Basis
│ │ │ │ -
│ │ │ │ -
69
│ │ │ │ -
│ │ │ │ -
80struct GTSAM_EXPORT Chebyshev2Basis : Basis<Chebyshev2Basis> {
│ │ │ │ -
81 using Parameters = Eigen::Matrix<double, -1, 1 /*Nx1*/>;
│ │ │ │ -
82
│ │ │ │ -
91 static Weights CalculateWeights(size_t N, double x, double a = -1,
│ │ │ │ -
92 double b = 1);
│ │ │ │ -
93
│ │ │ │ -
103 static Weights DerivativeWeights(size_t N, double x, double a = -1,
│ │ │ │ -
104 double b = 1);
│ │ │ │ -
105}; // Chebyshev2Basis
│ │ │ │ -
│ │ │ │ -
106
│ │ │ │ -
107} // namespace gtsam
│ │ │ │ -
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ -
Basis.h
Compute an interpolating basis.
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/global_includes.h>
│ │ │ │ +
21
│ │ │ │ +
31namespace gtsam {
│ │ │ │ +
32namespace testing {
│ │ │ │ +
33
│ │ │ │ +
35 template<class T>
│ │ │ │ +
36 T between(const T& t1, const T& t2) { return t1.between(t2); }
│ │ │ │ +
37
│ │ │ │ +
38 template<class T>
│ │ │ │ +
39 T compose(const T& t1, const T& t2) { return t1.compose(t2); }
│ │ │ │ +
40
│ │ │ │ +
42 template<class T>
│ │ │ │ +
43 T inverse(const T& t) { return t.inverse(); }
│ │ │ │ +
44
│ │ │ │ +
46 template<class T, class P>
│ │ │ │ +
47 P rotate(const T& r, const P& pt) { return r.rotate(pt); }
│ │ │ │ +
48
│ │ │ │ +
49 template<class T, class P>
│ │ │ │ +
50 P unrotate(const T& r, const P& pt) { return r.unrotate(pt); }
│ │ │ │ +
51
│ │ │ │ +
52} // \namespace testing
│ │ │ │ +
53} // \namespace gtsam
│ │ │ │ +
54
│ │ │ │ +
55
│ │ │ │ +
gtsam::testing::rotate
P rotate(const T &r, const P &pt)
rotation functions
Definition lieProxies.h:47
│ │ │ │ +
gtsam::testing::between
T between(const T &t1, const T &t2)
binary functions
Definition lieProxies.h:36
│ │ │ │ +
gtsam::testing::inverse
T inverse(const T &t)
unary functions
Definition lieProxies.h:43
│ │ │ │ +
global_includes.h
Included from all GTSAM files.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::Basis
CRTP Base class for function bases.
Definition Basis.h:100
│ │ │ │ -
gtsam::Chebyshev1Basis
Basis of Chebyshev polynomials of the first kind https://en.wikipedia.org/wiki/Chebyshev_polynomials#...
Definition Chebyshev.h:32
│ │ │ │ -
gtsam::Chebyshev2Basis
Basis of Chebyshev polynomials of the second kind.
Definition Chebyshev.h:80
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,71 +1,67 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Chebyshev.h │ │ │ │ │ +lieProxies.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_i_s_/_B_a_s_i_s_._h> │ │ │ │ │ -23 │ │ │ │ │ -24namespace _g_t_s_a_m { │ │ │ │ │ -25 │ │ │ │ │ -_3_2struct GTSAM_EXPORT _C_h_e_b_y_s_h_e_v_1_B_a_s_i_s : _B_a_s_i_s { │ │ │ │ │ -33 using Parameters = Eigen::Matrix; │ │ │ │ │ -34 │ │ │ │ │ -35 Parameters parameters_; │ │ │ │ │ -36 │ │ │ │ │ -45 static Weights CalculateWeights(size_t N, double x, double a = -1, │ │ │ │ │ -46 double b = 1); │ │ │ │ │ -47 │ │ │ │ │ -66 static Weights DerivativeWeights(size_t N, double x, double a = -1, │ │ │ │ │ -67 double b = 1); │ │ │ │ │ -68}; // Chebyshev1Basis │ │ │ │ │ -69 │ │ │ │ │ -_8_0struct GTSAM_EXPORT _C_h_e_b_y_s_h_e_v_2_B_a_s_i_s : _B_a_s_i_s { │ │ │ │ │ -81 using Parameters = Eigen::Matrix; │ │ │ │ │ -82 │ │ │ │ │ -91 static Weights CalculateWeights(size_t N, double x, double a = -1, │ │ │ │ │ -92 double b = 1); │ │ │ │ │ -93 │ │ │ │ │ -103 static Weights DerivativeWeights(size_t N, double x, double a = -1, │ │ │ │ │ -104 double b = 1); │ │ │ │ │ -105}; // Chebyshev2Basis │ │ │ │ │ -106 │ │ │ │ │ -107} // namespace gtsam │ │ │ │ │ -_M_a_n_i_f_o_l_d_._h │ │ │ │ │ -Base class and basic functions for Manifold types. │ │ │ │ │ -_B_a_s_i_s_._h │ │ │ │ │ -Compute an interpolating basis. │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ +21 │ │ │ │ │ +31namespace _g_t_s_a_m { │ │ │ │ │ +32namespace testing { │ │ │ │ │ +33 │ │ │ │ │ +35 template │ │ │ │ │ +_3_6 T _b_e_t_w_e_e_n(const T& t1, const T& t2) { return t1.between(t2); } │ │ │ │ │ +37 │ │ │ │ │ +38 template │ │ │ │ │ +39 T compose(const T& t1, const T& t2) { return t1.compose(t2); } │ │ │ │ │ +40 │ │ │ │ │ +42 template │ │ │ │ │ +_4_3 T _i_n_v_e_r_s_e(const T& t) { return t.inverse(); } │ │ │ │ │ +44 │ │ │ │ │ +46 template │ │ │ │ │ +_4_7 P _r_o_t_a_t_e(const T& r, const P& pt) { return r.rotate(pt); } │ │ │ │ │ +48 │ │ │ │ │ +49 template │ │ │ │ │ +50 P unrotate(const T& r, const P& pt) { return r.unrotate(pt); } │ │ │ │ │ +51 │ │ │ │ │ +52} // \namespace testing │ │ │ │ │ +53} // \namespace gtsam │ │ │ │ │ +54 │ │ │ │ │ +55 │ │ │ │ │ +_g_t_s_a_m_:_:_t_e_s_t_i_n_g_:_:_r_o_t_a_t_e │ │ │ │ │ +P rotate(const T &r, const P &pt) │ │ │ │ │ +rotation functions │ │ │ │ │ +DDeeffiinniittiioonn lieProxies.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_t_e_s_t_i_n_g_:_:_b_e_t_w_e_e_n │ │ │ │ │ +T between(const T &t1, const T &t2) │ │ │ │ │ +binary functions │ │ │ │ │ +DDeeffiinniittiioonn lieProxies.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_t_e_s_t_i_n_g_:_:_i_n_v_e_r_s_e │ │ │ │ │ +T inverse(const T &t) │ │ │ │ │ +unary functions │ │ │ │ │ +DDeeffiinniittiioonn lieProxies.h:43 │ │ │ │ │ +_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ +Included from all GTSAM files. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_s_i_s │ │ │ │ │ -CRTP Base class for function bases. │ │ │ │ │ -DDeeffiinniittiioonn Basis.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_C_h_e_b_y_s_h_e_v_1_B_a_s_i_s │ │ │ │ │ -Basis of Chebyshev polynomials of the first kind https://en.wikipedia.org/wiki/ │ │ │ │ │ -Chebyshev_polynomials#... │ │ │ │ │ -DDeeffiinniittiioonn Chebyshev.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_C_h_e_b_y_s_h_e_v_2_B_a_s_i_s │ │ │ │ │ -Basis of Chebyshev polynomials of the second kind. │ │ │ │ │ -DDeeffiinniittiioonn Chebyshev.h:80 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _b_a_s_i_s │ │ │ │ │ - * _C_h_e_b_y_s_h_e_v_._h │ │ │ │ │ + * _b_a_s_e │ │ │ │ │ + * _l_i_e_P_r_o_x_i_e_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00176_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01043_source.html │ │ │ │┄ Files 94% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/FitBasis.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearOptimizer.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
FitBasis.h
│ │ │ │ +
NonlinearOptimizer.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19/*
│ │ │ │ -
20 * Concept needed for LS. Parameters = Coefficients | Values
│ │ │ │ -
21 * - Parameters, Jacobian
│ │ │ │ -
22 * - PredictFactor(double x)(Parameters p, OptionalJacobian<1,N> H)
│ │ │ │ -
23 */
│ │ │ │ -
24
│ │ │ │ -
25#pragma once
│ │ │ │ -
26
│ │ │ │ -
27#include <gtsam/basis/Basis.h>
│ │ │ │ -
28#include <gtsam/basis/BasisFactors.h>
│ │ │ │ -
29#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ -
30#include <gtsam/linear/VectorValues.h>
│ │ │ │ -
31#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ -
32
│ │ │ │ -
33namespace gtsam {
│ │ │ │ -
34
│ │ │ │ -
36using Sequence = std::map<double, double>;
│ │ │ │ -
38using Sample = std::pair<double, double>;
│ │ │ │ -
39
│ │ │ │ -
51template <class Basis>
│ │ │ │ -
│ │ │ │ -
52class FitBasis {
│ │ │ │ -
53 public:
│ │ │ │ -
54 using Parameters = typename Basis::Parameters;
│ │ │ │ -
55
│ │ │ │ -
56 private:
│ │ │ │ -
57 Parameters parameters_;
│ │ │ │ -
58
│ │ │ │ -
59 public:
│ │ │ │ -
│ │ │ │ -
61 static NonlinearFactorGraph NonlinearGraph(const Sequence& sequence,
│ │ │ │ -
62 const SharedNoiseModel& model,
│ │ │ │ -
63 size_t N) {
│ │ │ │ -
64 NonlinearFactorGraph graph;
│ │ │ │ -
65 for (const Sample sample : sequence) {
│ │ │ │ -
66 graph.emplace_shared<EvaluationFactor<Basis>>(0, sample.second, model, N,
│ │ │ │ -
67 sample.first);
│ │ │ │ -
68 }
│ │ │ │ -
69 return graph;
│ │ │ │ -
70 }
│ │ │ │ -
│ │ │ │ -
71
│ │ │ │ -
│ │ │ │ -
73 static GaussianFactorGraph::shared_ptr LinearGraph(
│ │ │ │ -
74 const Sequence& sequence, const SharedNoiseModel& model, size_t N) {
│ │ │ │ -
75 NonlinearFactorGraph graph = NonlinearGraph(sequence, model, N);
│ │ │ │ -
76 Values values;
│ │ │ │ -
77 values.insert<Parameters>(0, Parameters::Zero(N));
│ │ │ │ -
78 GaussianFactorGraph::shared_ptr gfg = graph.linearize(values);
│ │ │ │ -
79 return gfg;
│ │ │ │ -
80 }
│ │ │ │ -
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ +
22#include <gtsam/nonlinear/NonlinearOptimizerParams.h>
│ │ │ │ +
23
│ │ │ │ +
24namespace gtsam {
│ │ │ │ +
25
│ │ │ │ +
26namespace internal { struct NonlinearOptimizerState; }
│ │ │ │ +
27
│ │ │ │ +
│ │ │ │ +
75class GTSAM_EXPORT NonlinearOptimizer {
│ │ │ │ +
76
│ │ │ │ +
77protected:
│ │ │ │ +
78 NonlinearFactorGraph graph_;
│ │ │ │ +
79
│ │ │ │ +
80 std::unique_ptr<internal::NonlinearOptimizerState> state_;
│ │ │ │
81
│ │ │ │ -
│ │ │ │ -
89 FitBasis(const Sequence& sequence, const SharedNoiseModel& model, size_t N) {
│ │ │ │ -
90 GaussianFactorGraph::shared_ptr gfg = LinearGraph(sequence, model, N);
│ │ │ │ -
91 VectorValues solution = gfg->optimize();
│ │ │ │ -
92 parameters_ = solution.at(0);
│ │ │ │ -
93 }
│ │ │ │ -
│ │ │ │ -
94
│ │ │ │ -
96 Parameters parameters() const { return parameters_; }
│ │ │ │ -
97};
│ │ │ │ -
│ │ │ │ -
98
│ │ │ │ -
99} // namespace gtsam
│ │ │ │ -
BasisFactors.h
Factor definitions for various Basis functors.
│ │ │ │ -
Basis.h
Compute an interpolating basis.
│ │ │ │ -
VectorValues.h
Factor Graph Values.
│ │ │ │ -
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ -
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │ +
82public:
│ │ │ │ +
84 using shared_ptr = boost::shared_ptr<const NonlinearOptimizer>;
│ │ │ │ +
85
│ │ │ │ +
88
│ │ │ │ +
98 virtual const Values& optimize() { defaultOptimize(); return values(); }
│ │ │ │ +
99
│ │ │ │ +
106 const Values& optimizeSafely();
│ │ │ │ +
107
│ │ │ │ +
109 double error() const;
│ │ │ │ +
110
│ │ │ │ +
112 size_t iterations() const;
│ │ │ │ +
113
│ │ │ │ +
115 const Values &values() const;
│ │ │ │ +
116
│ │ │ │ +
118 const NonlinearFactorGraph &graph() const { return graph_; }
│ │ │ │ +
119
│ │ │ │ +
121
│ │ │ │ +
124
│ │ │ │ +
126 virtual ~NonlinearOptimizer();
│ │ │ │ +
127
│ │ │ │ +
129 virtual VectorValues solve(const GaussianFactorGraph &gfg,
│ │ │ │ +
130 const NonlinearOptimizerParams& params) const;
│ │ │ │ +
131
│ │ │ │ +
136 virtual GaussianFactorGraph::shared_ptr iterate() = 0;
│ │ │ │ +
137
│ │ │ │ +
139
│ │ │ │ +
140protected:
│ │ │ │ +
144 void defaultOptimize();
│ │ │ │ +
145
│ │ │ │ +
146 virtual const NonlinearOptimizerParams& _params() const = 0;
│ │ │ │ +
147
│ │ │ │ +
149 NonlinearOptimizer(const NonlinearFactorGraph& graph,
│ │ │ │ +
150 std::unique_ptr<internal::NonlinearOptimizerState> state);
│ │ │ │ +
151};
│ │ │ │ +
│ │ │ │ +
152
│ │ │ │ +
157GTSAM_EXPORT bool checkConvergence(double relativeErrorTreshold,
│ │ │ │ +
158 double absoluteErrorTreshold, double errorThreshold,
│ │ │ │ +
159 double currentError, double newError, NonlinearOptimizerParams::Verbosity verbosity = NonlinearOptimizerParams::SILENT);
│ │ │ │ +
160
│ │ │ │ +
161GTSAM_EXPORT bool checkConvergence(const NonlinearOptimizerParams& params, double currentError,
│ │ │ │ +
162 double newError);
│ │ │ │ +
163
│ │ │ │ +
164} // gtsam
│ │ │ │ +
NonlinearOptimizerParams.h
Parameters for nonlinear optimization.
│ │ │ │ +
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::Sequence
std::map< double, double > Sequence
Our sequence representation is a map of {x: y} values where y = f(x)
Definition FitBasis.h:36
│ │ │ │ -
gtsam::Sample
std::pair< double, double > Sample
A sample is a key-value pair from a sequence.
Definition FitBasis.h:38
│ │ │ │ -
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ -
gtsam::EvaluationFactor
Factor for enforcing the scalar value of the polynomial BASIS representation at x is the same as the ...
Definition BasisFactors.h:39
│ │ │ │ -
gtsam::FitBasis
Class that does regression via least squares Example usage: size_t N = 3; auto fit = FitBasis<Chebysh...
Definition FitBasis.h:52
│ │ │ │ -
gtsam::FitBasis::NonlinearGraph
static NonlinearFactorGraph NonlinearGraph(const Sequence &sequence, const SharedNoiseModel &model, size_t N)
Create nonlinear FG from Sequence.
Definition FitBasis.h:61
│ │ │ │ -
gtsam::FitBasis::parameters
Parameters parameters() const
Return Fourier coefficients.
Definition FitBasis.h:96
│ │ │ │ -
gtsam::FitBasis::FitBasis
FitBasis(const Sequence &sequence, const SharedNoiseModel &model, size_t N)
Construct a new FitBasis object.
Definition FitBasis.h:89
│ │ │ │ -
gtsam::FitBasis::LinearGraph
static GaussianFactorGraph::shared_ptr LinearGraph(const Sequence &sequence, const SharedNoiseModel &model, size_t N)
Create linear FG from Sequence.
Definition FitBasis.h:73
│ │ │ │ +
gtsam::checkConvergence
bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold, double errorThreshold, double currentError, double newError, NonlinearOptimizerParams::Verbosity verbosity)
Check whether the relative error decrease is less than relativeErrorTreshold, the absolute error decr...
Definition NonlinearOptimizer.cpp:185
│ │ │ │ +
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │
gtsam::GaussianFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactorGraph.h:81
│ │ │ │
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ -
gtsam::VectorValues::at
Vector & at(Key j)
Read/write access to the vector value with key j, throws std::out_of_range if j does not exist,...
Definition VectorValues.h:139
│ │ │ │
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │ +
gtsam::NonlinearOptimizer
This is the abstract interface for classes that can optimize for the maximum-likelihood estimate of a...
Definition NonlinearOptimizer.h:75
│ │ │ │ +
gtsam::NonlinearOptimizer::graph
const NonlinearFactorGraph & graph() const
return the graph with nonlinear factors
Definition NonlinearOptimizer.h:118
│ │ │ │ +
gtsam::NonlinearOptimizer::graph_
NonlinearFactorGraph graph_
The graph with nonlinear factors.
Definition NonlinearOptimizer.h:78
│ │ │ │ +
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Optimize for the maximum-likelihood estimate, returning a the optimized variable assignments.
Definition NonlinearOptimizer.h:98
│ │ │ │ +
gtsam::NonlinearOptimizer::state_
std::unique_ptr< internal::NonlinearOptimizerState > state_
PIMPL'd state.
Definition NonlinearOptimizer.h:80
│ │ │ │ +
gtsam::NonlinearOptimizer::shared_ptr
boost::shared_ptr< const NonlinearOptimizer > shared_ptr
A shared pointer to this class.
Definition NonlinearOptimizer.h:84
│ │ │ │ +
gtsam::NonlinearOptimizer::iterate
virtual GaussianFactorGraph::shared_ptr iterate()=0
Perform a single iteration, returning GaussianFactorGraph corresponding to the linearized factor grap...
│ │ │ │ +
gtsam::NonlinearOptimizerParams
The common parameters for Nonlinear optimizers.
Definition NonlinearOptimizerParams.h:34
│ │ │ │ +
gtsam::NonlinearOptimizerParams::Verbosity
Verbosity
See NonlinearOptimizerParams::verbosity.
Definition NonlinearOptimizerParams.h:37
│ │ │ │
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
gtsam::Values::insert
void insert(Key j, const Value &val)
Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present.
Definition Values.cpp:157
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,157 +1,151 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -FitBasis.h │ │ │ │ │ +NonlinearOptimizer.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19/* │ │ │ │ │ -20 * Concept needed for LS. Parameters = Coefficients | Values │ │ │ │ │ -21 * - Parameters, Jacobian │ │ │ │ │ -22 * - PredictFactor(double x)(Parameters p, OptionalJacobian<1,N> H) │ │ │ │ │ -23 */ │ │ │ │ │ -24 │ │ │ │ │ -25#pragma once │ │ │ │ │ -26 │ │ │ │ │ -27#include <_g_t_s_a_m_/_b_a_s_i_s_/_B_a_s_i_s_._h> │ │ │ │ │ -28#include <_g_t_s_a_m_/_b_a_s_i_s_/_B_a_s_i_s_F_a_c_t_o_r_s_._h> │ │ │ │ │ -29#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -30#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ -31#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -32 │ │ │ │ │ -33namespace _g_t_s_a_m { │ │ │ │ │ -34 │ │ │ │ │ -_3_6using _S_e_q_u_e_n_c_e = std::map; │ │ │ │ │ -_3_8using _S_a_m_p_l_e = std::pair; │ │ │ │ │ -39 │ │ │ │ │ -51template │ │ │ │ │ -_5_2class _F_i_t_B_a_s_i_s { │ │ │ │ │ -53 public: │ │ │ │ │ -54 using Parameters = typename Basis::Parameters; │ │ │ │ │ -55 │ │ │ │ │ -56 private: │ │ │ │ │ -57 Parameters parameters_; │ │ │ │ │ -58 │ │ │ │ │ -59 public: │ │ │ │ │ -_6_1 static _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h _N_o_n_l_i_n_e_a_r_G_r_a_p_h(const _S_e_q_u_e_n_c_e& sequence, │ │ │ │ │ -62 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model, │ │ │ │ │ -63 size_t N) { │ │ │ │ │ -64 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h graph; │ │ │ │ │ -65 for (const _S_a_m_p_l_e sample : sequence) { │ │ │ │ │ -66 graph.emplace_shared<_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r_<_B_a_s_i_s_>>(0, sample.second, model, N, │ │ │ │ │ -67 sample.first); │ │ │ │ │ -68 } │ │ │ │ │ -69 return graph; │ │ │ │ │ -70 } │ │ │ │ │ -71 │ │ │ │ │ -_7_3 static _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r _L_i_n_e_a_r_G_r_a_p_h( │ │ │ │ │ -74 const _S_e_q_u_e_n_c_e& sequence, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model, size_t N) { │ │ │ │ │ -75 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h graph = _N_o_n_l_i_n_e_a_r_G_r_a_p_h(sequence, model, N); │ │ │ │ │ -76 _V_a_l_u_e_s values; │ │ │ │ │ -77 values._i_n_s_e_r_t(0, Parameters::Zero(N)); │ │ │ │ │ -78 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r gfg = graph.linearize(values); │ │ │ │ │ -79 return gfg; │ │ │ │ │ -80 } │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_._h> │ │ │ │ │ +23 │ │ │ │ │ +24namespace _g_t_s_a_m { │ │ │ │ │ +25 │ │ │ │ │ +26namespace internal { struct NonlinearOptimizerState; } │ │ │ │ │ +27 │ │ │ │ │ +_7_5class GTSAM_EXPORT _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r { │ │ │ │ │ +76 │ │ │ │ │ +77protected: │ │ │ │ │ +_7_8 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h _g_r_a_p_h__; │ │ │ │ │ +79 │ │ │ │ │ +_8_0 std::unique_ptr _s_t_a_t_e__; │ │ │ │ │ 81 │ │ │ │ │ -_8_9 _F_i_t_B_a_s_i_s(const _S_e_q_u_e_n_c_e& sequence, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model, size_t N) │ │ │ │ │ -{ │ │ │ │ │ -90 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r gfg = _L_i_n_e_a_r_G_r_a_p_h(sequence, model, N); │ │ │ │ │ -91 _V_e_c_t_o_r_V_a_l_u_e_s solution = gfg->optimize(); │ │ │ │ │ -92 parameters_ = solution._a_t(0); │ │ │ │ │ -93 } │ │ │ │ │ -94 │ │ │ │ │ -_9_6 Parameters _p_a_r_a_m_e_t_e_r_s() const { return parameters_; } │ │ │ │ │ -97}; │ │ │ │ │ -98 │ │ │ │ │ -99} // namespace gtsam │ │ │ │ │ -_B_a_s_i_s_F_a_c_t_o_r_s_._h │ │ │ │ │ -Factor definitions for various Basis functors. │ │ │ │ │ -_B_a_s_i_s_._h │ │ │ │ │ -Compute an interpolating basis. │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ -Factor Graph Values. │ │ │ │ │ -_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ +82public: │ │ │ │ │ +_8_4 using _s_h_a_r_e_d___p_t_r = boost::shared_ptr; │ │ │ │ │ +85 │ │ │ │ │ +88 │ │ │ │ │ +_9_8 virtual const _V_a_l_u_e_s& _o_p_t_i_m_i_z_e() { defaultOptimize(); return values(); } │ │ │ │ │ +99 │ │ │ │ │ +106 const _V_a_l_u_e_s& optimizeSafely(); │ │ │ │ │ +107 │ │ │ │ │ +109 double error() const; │ │ │ │ │ +110 │ │ │ │ │ +112 size_t iterations() const; │ │ │ │ │ +113 │ │ │ │ │ +115 const _V_a_l_u_e_s &values() const; │ │ │ │ │ +116 │ │ │ │ │ +_1_1_8 const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h &_g_r_a_p_h() const { return graph_; } │ │ │ │ │ +119 │ │ │ │ │ +121 │ │ │ │ │ +124 │ │ │ │ │ +126 virtual _~_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r(); │ │ │ │ │ +127 │ │ │ │ │ +129 virtual _V_e_c_t_o_r_V_a_l_u_e_s solve(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, │ │ │ │ │ +130 const _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s& params) const; │ │ │ │ │ +131 │ │ │ │ │ +_1_3_6 virtual _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r _i_t_e_r_a_t_e() = 0; │ │ │ │ │ +137 │ │ │ │ │ +139 │ │ │ │ │ +140protected: │ │ │ │ │ +144 void defaultOptimize(); │ │ │ │ │ +145 │ │ │ │ │ +146 virtual const _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s& _params() const = 0; │ │ │ │ │ +147 │ │ │ │ │ +149 _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, │ │ │ │ │ +150 std::unique_ptr state); │ │ │ │ │ +151}; │ │ │ │ │ +152 │ │ │ │ │ +157GTSAM_EXPORT bool _c_h_e_c_k_C_o_n_v_e_r_g_e_n_c_e(double relativeErrorTreshold, │ │ │ │ │ +158 double absoluteErrorTreshold, double errorThreshold, │ │ │ │ │ +159 double currentError, double newError, _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_V_e_r_b_o_s_i_t_y │ │ │ │ │ +verbosity = NonlinearOptimizerParams::SILENT); │ │ │ │ │ +160 │ │ │ │ │ +161GTSAM_EXPORT bool _c_h_e_c_k_C_o_n_v_e_r_g_e_n_c_e(const _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s& params, │ │ │ │ │ +double currentError, │ │ │ │ │ +162 double newError); │ │ │ │ │ +163 │ │ │ │ │ +164} // gtsam │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_._h │ │ │ │ │ +Parameters for nonlinear optimization. │ │ │ │ │ _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ Factor Graph consisting of non-linear factors. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_S_e_q_u_e_n_c_e │ │ │ │ │ -std::map< double, double > Sequence │ │ │ │ │ -Our sequence representation is a map of {x: y} values where y = f(x) │ │ │ │ │ -DDeeffiinniittiioonn FitBasis.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_S_a_m_p_l_e │ │ │ │ │ -std::pair< double, double > Sample │ │ │ │ │ -A sample is a key-value pair from a sequence. │ │ │ │ │ -DDeeffiinniittiioonn FitBasis.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ -Aliases. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ -_g_t_s_a_m_:_:_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -Factor for enforcing the scalar value of the polynomial BASIS representation at │ │ │ │ │ -x is the same as the ... │ │ │ │ │ -DDeeffiinniittiioonn BasisFactors.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_F_i_t_B_a_s_i_s │ │ │ │ │ -Class that does regression via least squares Example usage: size_t N = 3; auto │ │ │ │ │ -fit = FitBasis shared_ptr │ │ │ │ │ shared_ptr to this class │ │ │ │ │ DDeeffiinniittiioonn GaussianFactorGraph.h:81 │ │ │ │ │ _g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ with a unique integer... │ │ │ │ │ DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_a_t │ │ │ │ │ -Vector & at(Key j) │ │ │ │ │ -Read/write access to the vector value with key j, throws std::out_of_range if j │ │ │ │ │ -does not exist,... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:139 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r │ │ │ │ │ +This is the abstract interface for classes that can optimize for the maximum- │ │ │ │ │ +likelihood estimate of a... │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizer.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_:_:_g_r_a_p_h │ │ │ │ │ +const NonlinearFactorGraph & graph() const │ │ │ │ │ +return the graph with nonlinear factors │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizer.h:118 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_:_:_g_r_a_p_h__ │ │ │ │ │ +NonlinearFactorGraph graph_ │ │ │ │ │ +The graph with nonlinear factors. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizer.h:78 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ +virtual const Values & optimize() │ │ │ │ │ +Optimize for the maximum-likelihood estimate, returning a the optimized │ │ │ │ │ +variable assignments. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizer.h:98 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_:_:_s_t_a_t_e__ │ │ │ │ │ +std::unique_ptr< internal::NonlinearOptimizerState > state_ │ │ │ │ │ +PIMPL'd state. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizer.h:80 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< const NonlinearOptimizer > shared_ptr │ │ │ │ │ +A shared pointer to this class. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizer.h:84 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_:_:_i_t_e_r_a_t_e │ │ │ │ │ +virtual GaussianFactorGraph::shared_ptr iterate()=0 │ │ │ │ │ +Perform a single iteration, returning GaussianFactorGraph corresponding to the │ │ │ │ │ +linearized factor grap... │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s │ │ │ │ │ +The common parameters for Nonlinear optimizers. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizerParams.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_V_e_r_b_o_s_i_t_y │ │ │ │ │ +Verbosity │ │ │ │ │ +See NonlinearOptimizerParams::verbosity. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizerParams.h:37 │ │ │ │ │ _g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_i_n_s_e_r_t │ │ │ │ │ -void insert(Key j, const Value &val) │ │ │ │ │ -Add a variable with the given j, throws KeyAlreadyExists if j is already │ │ │ │ │ -present. │ │ │ │ │ -DDeeffiinniittiioonn Values.cpp:157 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _b_a_s_i_s │ │ │ │ │ - * _F_i_t_B_a_s_i_s_._h │ │ │ │ │ + * _n_o_n_l_i_n_e_a_r │ │ │ │ │ + * _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00185_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00209_source.html │ │ │ │┄ Files 99% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
DiscreteDistribution.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
18#pragma once
│ │ │ │
19
│ │ │ │ -
20#include <gtsam/discrete/DiscreteConditional.h>
│ │ │ │ +
20#include <gtsam/discrete/DiscreteConditional.h>
│ │ │ │
21
│ │ │ │
22#include <string>
│ │ │ │
23#include <vector>
│ │ │ │
24
│ │ │ │
25namespace gtsam {
│ │ │ │
26
│ │ │ │
│ │ │ │ @@ -171,15 +171,15 @@ │ │ │ │
103// DiscreteDistribution
│ │ │ │
104
│ │ │ │
105// traits
│ │ │ │
106template <>
│ │ │ │
107struct traits<DiscreteDistribution> : public Testable<DiscreteDistribution> {};
│ │ │ │
108
│ │ │ │
109} // namespace gtsam
│ │ │ │ - │ │ │ │ + │ │ │ │
gtsam::DiscreteKey
std::pair< Key, size_t > DiscreteKey
Key type for discrete variables.
Definition DiscreteKey.h:36
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │
gtsam::DecisionTreeFactor
A discrete probabilistic factor.
Definition DecisionTreeFactor.h:45
│ │ │ │
gtsam::DiscreteConditional
Discrete Conditional Density Derives from DecisionTreeFactor.
Definition DiscreteConditional.h:40
│ │ │ │
gtsam::DiscreteDistribution
A prior probability on a set of discrete variables.
Definition DiscreteDistribution.h:33
│ │ │ │ @@ -190,13 +190,13 @@ │ │ │ │
gtsam::DiscreteDistribution::DiscreteDistribution
DiscreteDistribution(const DecisionTreeFactor &f)
Constructor from factor.
Definition DiscreteDistribution.h:44
│ │ │ │
gtsam::Signature
Signature for a discrete conditional density, used to construct conditionals.
Definition Signature.h:54
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00197.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00452.html │ │ │ │┄ Files 95% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DecisionTreeFactor.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/CalibratedCamera.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces
│ │ │ │ -
DecisionTreeFactor.cpp File Reference
│ │ │ │ +
CalibratedCamera.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

discrete factor │ │ │ │ +

Calibrated camera for which only pose is unknown. │ │ │ │ More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

discrete factor

│ │ │ │ -
Date
Feb 14, 2011
│ │ │ │ -
Author
Duy-Nguyen Ta
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ +

Calibrated camera for which only pose is unknown.

│ │ │ │ +
Date
Aug 17, 2009
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,22 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -DecisionTreeFactor.cpp File Reference │ │ │ │ │ -discrete factor _M_o_r_e_._._. │ │ │ │ │ +CalibratedCamera.cpp File Reference │ │ │ │ │ +Calibrated camera for which only pose is unknown. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -discrete factor │ │ │ │ │ +Calibrated camera for which only pose is unknown. │ │ │ │ │ Date │ │ │ │ │ - Feb 14, 2011 │ │ │ │ │ + Aug 17, 2009 │ │ │ │ │ Author │ │ │ │ │ - Duy-Nguyen Ta │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_._c_p_p │ │ │ │ │ + * _g_e_o_m_e_t_r_y │ │ │ │ │ + * _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00215_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00371_source.html │ │ │ │┄ Files 95% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteConditional.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Rot2.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
DiscreteConditional.h
│ │ │ │ +
Rot2.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/inference/Conditional-inst.h>
│ │ │ │ -
22#include <gtsam/discrete/DecisionTreeFactor.h>
│ │ │ │ -
23#include <gtsam/discrete/Signature.h>
│ │ │ │ -
24
│ │ │ │ -
25#include <boost/make_shared.hpp>
│ │ │ │ -
26#include <boost/shared_ptr.hpp>
│ │ │ │ -
27#include <string>
│ │ │ │ -
28#include <vector>
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/geometry/Point2.h>
│ │ │ │ +
23#include <gtsam/base/Lie.h>
│ │ │ │ +
24#include <boost/optional.hpp>
│ │ │ │ +
25
│ │ │ │ +
26#include <random>
│ │ │ │ +
27
│ │ │ │ +
28namespace gtsam {
│ │ │ │
29
│ │ │ │ -
30namespace gtsam {
│ │ │ │ -
31
│ │ │ │ -
│ │ │ │ -
38class GTSAM_EXPORT DiscreteConditional
│ │ │ │ -
39 : public DecisionTreeFactor,
│ │ │ │ -
40 public Conditional<DecisionTreeFactor, DiscreteConditional> {
│ │ │ │ -
41 public:
│ │ │ │ -
42 // typedefs needed to play nice with gtsam
│ │ │ │ -
43 typedef DiscreteConditional This;
│ │ │ │ -
44 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
45 typedef DecisionTreeFactor BaseFactor;
│ │ │ │ -
46 typedef Conditional<BaseFactor, This>
│ │ │ │ -
47 BaseConditional;
│ │ │ │ +
│ │ │ │ +
36 class GTSAM_EXPORT Rot2 : public LieGroup<Rot2, 1> {
│ │ │ │ +
37
│ │ │ │ +
39 double c_, s_;
│ │ │ │ +
40
│ │ │ │ +
42 Rot2& normalize();
│ │ │ │ +
43
│ │ │ │ +
45 inline Rot2(double c, double s) : c_(c), s_(s) {}
│ │ │ │ +
46
│ │ │ │ +
47 public:
│ │ │ │
48
│ │ │ │ -
49 using Values = DiscreteValues;
│ │ │ │ -
50
│ │ │ │ -
53
│ │ │ │ -
55 DiscreteConditional() {}
│ │ │ │ -
56
│ │ │ │ -
58 DiscreteConditional(size_t nFrontals, const DecisionTreeFactor& f);
│ │ │ │ -
59
│ │ │ │ -
64 DiscreteConditional(size_t nFrontals, const DiscreteKeys& keys,
│ │ │ │ -
65 const ADT& potentials);
│ │ │ │ -
66
│ │ │ │ -
68 explicit DiscreteConditional(const Signature& signature);
│ │ │ │ -
69
│ │ │ │ -
│ │ │ │ -
77 DiscreteConditional(const DiscreteKey& key, const DiscreteKeys& parents,
│ │ │ │ -
78 const Signature::Table& table)
│ │ │ │ -
79 : DiscreteConditional(Signature(key, parents, table)) {}
│ │ │ │ -
│ │ │ │ -
80
│ │ │ │ -
│ │ │ │ -
90 DiscreteConditional(const DiscreteKey& key, const DiscreteKeys& parents,
│ │ │ │ -
91 const std::string& spec)
│ │ │ │ -
92 : DiscreteConditional(Signature(key, parents, spec)) {}
│ │ │ │ -
│ │ │ │ -
93
│ │ │ │ -
│ │ │ │ -
95 DiscreteConditional(const DiscreteKey& key, const std::string& spec)
│ │ │ │ -
96 : DiscreteConditional(Signature(key, {}, spec)) {}
│ │ │ │ -
│ │ │ │ -
97
│ │ │ │ -
102 DiscreteConditional(const DecisionTreeFactor& joint,
│ │ │ │ -
103 const DecisionTreeFactor& marginal);
│ │ │ │ -
104
│ │ │ │ -
110 DiscreteConditional(const DecisionTreeFactor& joint,
│ │ │ │ -
111 const DecisionTreeFactor& marginal,
│ │ │ │ -
112 const Ordering& orderedKeys);
│ │ │ │ -
113
│ │ │ │ -
129 DiscreteConditional operator*(const DiscreteConditional& other) const;
│ │ │ │ +
51
│ │ │ │ +
53 Rot2() : c_(1.0), s_(0.0) {}
│ │ │ │ +
54
│ │ │ │ +
56 Rot2(const Rot2& r) : Rot2(r.c_, r.s_) {}
│ │ │ │ +
57
│ │ │ │ +
59 Rot2(double theta) : c_(cos(theta)), s_(sin(theta)) {}
│ │ │ │ +
60
│ │ │ │ +
│ │ │ │ +
62 static Rot2 fromAngle(double theta) {
│ │ │ │ +
63 return Rot2(theta);
│ │ │ │ +
64 }
│ │ │ │ +
│ │ │ │ +
65
│ │ │ │ +
│ │ │ │ +
67 static Rot2 fromDegrees(double theta) {
│ │ │ │ +
68 static const double degree = M_PI / 180;
│ │ │ │ +
69 return fromAngle(theta * degree);
│ │ │ │ +
70 }
│ │ │ │ +
│ │ │ │ +
71
│ │ │ │ +
73 static Rot2 fromCosSin(double c, double s);
│ │ │ │ +
74
│ │ │ │ +
82 static Rot2 relativeBearing(const Point2& d, OptionalJacobian<1,2> H =
│ │ │ │ +
83 boost::none);
│ │ │ │ +
84
│ │ │ │ +
86 static Rot2 atan2(double y, double x);
│ │ │ │ +
87
│ │ │ │ +
94 static Rot2 Random(std::mt19937 & rng);
│ │ │ │ +
95
│ │ │ │ +
99
│ │ │ │ +
101 void print(const std::string& s = "theta") const;
│ │ │ │ +
102
│ │ │ │ +
104 bool equals(const Rot2& R, double tol = 1e-9) const;
│ │ │ │ +
105
│ │ │ │ +
109
│ │ │ │ +
111 inline static Rot2 Identity() { return Rot2(); }
│ │ │ │ +
112
│ │ │ │ +
114 Rot2 inverse() const { return Rot2(c_, -s_);}
│ │ │ │ +
115
│ │ │ │ +
│ │ │ │ +
117 Rot2 operator*(const Rot2& R) const {
│ │ │ │ +
118 return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
│ │ │ │ +
119 }
│ │ │ │ +
│ │ │ │ +
120
│ │ │ │ +
124
│ │ │ │ +
126 static Rot2 Expmap(const Vector1& v, ChartJacobian H = boost::none);
│ │ │ │ +
127
│ │ │ │ +
129 static Vector1 Logmap(const Rot2& r, ChartJacobian H = boost::none);
│ │ │ │
130
│ │ │ │ -
132 DiscreteConditional marginal(Key key) const;
│ │ │ │ +
132 Matrix1 AdjointMap() const { return I_1x1; }
│ │ │ │
133
│ │ │ │ -
137
│ │ │ │ -
139 void print(
│ │ │ │ -
140 const std::string& s = "Discrete Conditional: ",
│ │ │ │ -
141 const KeyFormatter& formatter = DefaultKeyFormatter) const override;
│ │ │ │ -
142
│ │ │ │ -
144 bool equals(const DiscreteFactor& other, double tol = 1e-9) const override;
│ │ │ │ -
145
│ │ │ │ -
149
│ │ │ │ -
│ │ │ │ -
151 double logProbability(const DiscreteValues& x) const {
│ │ │ │ -
152 return -error(x);
│ │ │ │ -
153 }
│ │ │ │ -
│ │ │ │ -
154
│ │ │ │ -
│ │ │ │ -
156 void printSignature(
│ │ │ │ -
157 const std::string& s = "Discrete Conditional: ",
│ │ │ │ -
158 const KeyFormatter& formatter = DefaultKeyFormatter) const {
│ │ │ │ -
159 static_cast<const BaseConditional*>(this)->print(s, formatter);
│ │ │ │ -
160 }
│ │ │ │ -
│ │ │ │ -
161
│ │ │ │ -
│ │ │ │ -
163 double evaluate(const DiscreteValues& values) const {
│ │ │ │ -
164 return ADT::operator()(values);
│ │ │ │ -
165 }
│ │ │ │ -
│ │ │ │ -
166
│ │ │ │ -
167 using DecisionTreeFactor::error;
│ │ │ │ -
168 using DecisionTreeFactor::operator();
│ │ │ │ -
169
│ │ │ │ -
183 shared_ptr choose(const DiscreteValues& given) const;
│ │ │ │ -
184
│ │ │ │ -
186 DecisionTreeFactor::shared_ptr likelihood(
│ │ │ │ -
187 const DiscreteValues& frontalValues) const;
│ │ │ │ -
188
│ │ │ │ -
190 DecisionTreeFactor::shared_ptr likelihood(size_t frontal) const;
│ │ │ │ -
191
│ │ │ │ -
197 size_t sample(const DiscreteValues& parentsValues) const;
│ │ │ │ -
198
│ │ │ │ -
200 size_t sample(size_t parent_value) const;
│ │ │ │ +
│ │ │ │ +
135 static Matrix ExpmapDerivative(const Vector& /*v*/) {
│ │ │ │ +
136 return I_1x1;
│ │ │ │ +
137 }
│ │ │ │ +
│ │ │ │ +
138
│ │ │ │ +
│ │ │ │ +
140 static Matrix LogmapDerivative(const Vector& /*v*/) {
│ │ │ │ +
141 return I_1x1;
│ │ │ │ +
142 }
│ │ │ │ +
│ │ │ │ +
143
│ │ │ │ +
144 // Chart at origin simply uses exponential map and its inverse
│ │ │ │ +
│ │ │ │ +
145 struct ChartAtOrigin {
│ │ │ │ +
146 static Rot2 Retract(const Vector1& v, ChartJacobian H = boost::none) {
│ │ │ │ +
147 return Expmap(v, H);
│ │ │ │ +
148 }
│ │ │ │ +
149 static Vector1 Local(const Rot2& r, ChartJacobian H = boost::none) {
│ │ │ │ +
150 return Logmap(r, H);
│ │ │ │ +
151 }
│ │ │ │ +
152 };
│ │ │ │ +
│ │ │ │ +
153
│ │ │ │ +
154 using LieGroup<Rot2, 1>::inverse; // version with derivative
│ │ │ │ +
155
│ │ │ │ +
159
│ │ │ │ +
163 Point2 rotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none,
│ │ │ │ +
164 OptionalJacobian<2, 2> H2 = boost::none) const;
│ │ │ │ +
165
│ │ │ │ +
│ │ │ │ +
167 inline Point2 operator*(const Point2& p) const {
│ │ │ │ +
168 return rotate(p);
│ │ │ │ +
169 }
│ │ │ │ +
│ │ │ │ +
170
│ │ │ │ +
174 Point2 unrotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none,
│ │ │ │ +
175 OptionalJacobian<2, 2> H2 = boost::none) const;
│ │ │ │ +
176
│ │ │ │ +
180
│ │ │ │ +
│ │ │ │ +
182 inline Point2 unit() const {
│ │ │ │ +
183 return Point2(c_, s_);
│ │ │ │ +
184 }
│ │ │ │ +
│ │ │ │ +
185
│ │ │ │ +
│ │ │ │ +
187 double theta() const {
│ │ │ │ +
188 return ::atan2(s_, c_);
│ │ │ │ +
189 }
│ │ │ │ +
│ │ │ │ +
190
│ │ │ │ +
│ │ │ │ +
192 double degrees() const {
│ │ │ │ +
193 const double degree = M_PI / 180;
│ │ │ │ +
194 return theta() / degree;
│ │ │ │ +
195 }
│ │ │ │ +
│ │ │ │ +
196
│ │ │ │ +
│ │ │ │ +
198 inline double c() const {
│ │ │ │ +
199 return c_;
│ │ │ │ +
200 }
│ │ │ │ +
│ │ │ │
201
│ │ │ │ -
203 size_t sample() const;
│ │ │ │ -
204
│ │ │ │ -
209 size_t argmax() const;
│ │ │ │ -
210
│ │ │ │ -
214
│ │ │ │ -
216 void sampleInPlace(DiscreteValues* parentsValues) const;
│ │ │ │ -
217
│ │ │ │ -
219 std::vector<DiscreteValues> frontalAssignments() const;
│ │ │ │ -
220
│ │ │ │ -
222 std::vector<DiscreteValues> allAssignments() const;
│ │ │ │ -
223
│ │ │ │ -
227
│ │ │ │ -
229 std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
230 const Names& names = {}) const override;
│ │ │ │ -
231
│ │ │ │ -
233 std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
234 const Names& names = {}) const override;
│ │ │ │ -
235
│ │ │ │ -
236
│ │ │ │ -
240
│ │ │ │ -
245 double evaluate(const HybridValues& x) const override;
│ │ │ │ -
246
│ │ │ │ -
247 using BaseConditional::operator();
│ │ │ │ -
248
│ │ │ │ -
│ │ │ │ -
253 double logProbability(const HybridValues& x) const override {
│ │ │ │ -
254 return -error(x);
│ │ │ │ -
255 }
│ │ │ │ -
│ │ │ │ -
256
│ │ │ │ -
262 double logNormalizationConstant() const override { return 0.0; }
│ │ │ │ -
263
│ │ │ │ -
265
│ │ │ │ -
266#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ -
269 size_t GTSAM_DEPRECATED solve(const DiscreteValues& parentsValues) const;
│ │ │ │ -
270 void GTSAM_DEPRECATED solveInPlace(DiscreteValues* parentsValues) const;
│ │ │ │ -
272#endif
│ │ │ │ -
273
│ │ │ │ -
274 protected:
│ │ │ │ -
276 DiscreteConditional::ADT choose(const DiscreteValues& given,
│ │ │ │ -
277 bool forceComplete) const;
│ │ │ │ -
278
│ │ │ │ -
279 private:
│ │ │ │ -
281 friend class boost::serialization::access;
│ │ │ │ -
282 template <class Archive>
│ │ │ │ -
283 void serialize(Archive& ar, const unsigned int /*version*/) {
│ │ │ │ -
284 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseFactor);
│ │ │ │ -
285 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseConditional);
│ │ │ │ -
286 }
│ │ │ │ -
287};
│ │ │ │ -
│ │ │ │ -
288// DiscreteConditional
│ │ │ │ -
289
│ │ │ │ -
290// traits
│ │ │ │ -
291template <>
│ │ │ │ -
292struct traits<DiscreteConditional> : public Testable<DiscreteConditional> {};
│ │ │ │ -
293
│ │ │ │ -
294} // namespace gtsam
│ │ │ │ -
Signature.h
signatures for conditional densities
│ │ │ │ - │ │ │ │ -
gtsam::DiscreteKey
std::pair< Key, size_t > DiscreteKey
Key type for discrete variables.
Definition DiscreteKey.h:36
│ │ │ │ +
│ │ │ │ +
203 inline double s() const {
│ │ │ │ +
204 return s_;
│ │ │ │ +
205 }
│ │ │ │ +
│ │ │ │ +
206
│ │ │ │ +
208 Matrix2 matrix() const;
│ │ │ │ +
209
│ │ │ │ +
211 Matrix2 transpose() const;
│ │ │ │ +
212
│ │ │ │ +
214 static Rot2 ClosestTo(const Matrix2& M);
│ │ │ │ +
215
│ │ │ │ +
216 private:
│ │ │ │ +
218 friend class boost::serialization::access;
│ │ │ │ +
219 template<class ARCHIVE>
│ │ │ │ +
220 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
221 ar & BOOST_SERIALIZATION_NVP(c_);
│ │ │ │ +
222 ar & BOOST_SERIALIZATION_NVP(s_);
│ │ │ │ +
223 }
│ │ │ │ +
224
│ │ │ │ +
225 };
│ │ │ │ +
│ │ │ │ +
226
│ │ │ │ +
227 template<>
│ │ │ │ +
228 struct traits<Rot2> : public internal::LieGroup<Rot2> {};
│ │ │ │ +
229
│ │ │ │ +
230 template<>
│ │ │ │ +
231 struct traits<const Rot2> : public internal::LieGroup<Rot2> {};
│ │ │ │ +
232
│ │ │ │ +
233} // gtsam
│ │ │ │ +
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ +
Point2.h
2D Point
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::markdown
string markdown(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
Free version of markdown.
Definition DiscreteValues.cpp:129
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │ +
gtsam::normalize
Point3 normalize(const Point3 &p, OptionalJacobian< 3, 3 > H)
normalize, with optional Jacobian
Definition Point3.cpp:52
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ - │ │ │ │ -
gtsam::DecisionTreeFactor
A discrete probabilistic factor.
Definition DecisionTreeFactor.h:45
│ │ │ │ -
gtsam::DiscreteConditional
Discrete Conditional Density Derives from DecisionTreeFactor.
Definition DiscreteConditional.h:40
│ │ │ │ -
gtsam::DiscreteConditional::DiscreteConditional
DiscreteConditional(const DiscreteKey &key, const std::string &spec)
No-parent specialization; can also use DiscreteDistribution.
Definition DiscreteConditional.h:95
│ │ │ │ -
gtsam::DiscreteConditional::DiscreteConditional
DiscreteConditional(const DiscreteKey &key, const DiscreteKeys &parents, const std::string &spec)
Construct from key, parents, and a string specifying the conditional probability table (CPT) in 00 01...
Definition DiscreteConditional.h:90
│ │ │ │ -
gtsam::DiscreteConditional::evaluate
double evaluate(const DiscreteValues &values) const
Evaluate, just look up in AlgebraicDecisonTree.
Definition DiscreteConditional.h:163
│ │ │ │ -
gtsam::DiscreteConditional::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition DiscreteConditional.h:44
│ │ │ │ -
gtsam::DiscreteConditional::DiscreteConditional
DiscreteConditional(const DiscreteKey &key, const DiscreteKeys &parents, const Signature::Table &table)
Construct from key, parents, and a Signature::Table specifying the conditional probability table (CPT...
Definition DiscreteConditional.h:77
│ │ │ │ -
gtsam::DiscreteConditional::BaseConditional
Conditional< BaseFactor, This > BaseConditional
Typedef to our conditional base class.
Definition DiscreteConditional.h:47
│ │ │ │ -
gtsam::DiscreteConditional::logProbability
double logProbability(const HybridValues &x) const override
< HybridValues version
Definition DiscreteConditional.h:253
│ │ │ │ -
gtsam::DiscreteConditional::This
DiscreteConditional This
Typedef to this class.
Definition DiscreteConditional.h:43
│ │ │ │ -
gtsam::DiscreteConditional::BaseFactor
DecisionTreeFactor BaseFactor
Typedef to our factor base class.
Definition DiscreteConditional.h:45
│ │ │ │ -
gtsam::DiscreteConditional::DiscreteConditional
DiscreteConditional()
Default constructor needed for serialization.
Definition DiscreteConditional.h:55
│ │ │ │ -
gtsam::DiscreteConditional::logProbability
double logProbability(const DiscreteValues &x) const
Log-probability is just -error(x).
Definition DiscreteConditional.h:151
│ │ │ │ -
gtsam::DiscreteConditional::printSignature
void printSignature(const std::string &s="Discrete Conditional: ", const KeyFormatter &formatter=DefaultKeyFormatter) const
print index signature only
Definition DiscreteConditional.h:156
│ │ │ │ -
gtsam::DiscreteConditional::logNormalizationConstant
double logNormalizationConstant() const override
logNormalizationConstant K is just zero, such that logProbability(x) = log(evaluate(x)) = - error(x) ...
Definition DiscreteConditional.h:262
│ │ │ │ -
gtsam::DiscreteKeys
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition DiscreteKey.h:39
│ │ │ │ -
gtsam::DiscreteValues
A map from keys to values.
Definition DiscreteValues.h:34
│ │ │ │ -
gtsam::Signature
Signature for a discrete conditional density, used to construct conditionals.
Definition Signature.h:54
│ │ │ │ -
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ -
gtsam::Conditional
Definition Conditional.h:64
│ │ │ │ -
HybridValues
the error.
│ │ │ │ +
gtsam::LieGroup
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition Lie.h:37
│ │ │ │ +
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition Lie.h:229
│ │ │ │ +
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::Rot2
Rotation matrix NOTE: the angle theta is in radians unless explicitly stated.
Definition Rot2.h:36
│ │ │ │ +
gtsam::Rot2::operator*
Rot2 operator*(const Rot2 &R) const
Compose - make a new rotation by adding angles.
Definition Rot2.h:117
│ │ │ │ +
gtsam::Rot2::c
double c() const
return cos
Definition Rot2.h:198
│ │ │ │ +
gtsam::Rot2::ExpmapDerivative
static Matrix ExpmapDerivative(const Vector &)
Left-trivialized derivative of the exponential map.
Definition Rot2.h:135
│ │ │ │ +
gtsam::Rot2::unit
Point2 unit() const
Creates a unit vector as a Point2.
Definition Rot2.h:182
│ │ │ │ +
gtsam::Rot2::theta
double theta() const
return angle (RADIANS)
Definition Rot2.h:187
│ │ │ │ +
gtsam::Rot2::inverse
Rot2 inverse() const
The inverse rotation - negative angle.
Definition Rot2.h:114
│ │ │ │ +
gtsam::Rot2::operator*
Point2 operator*(const Point2 &p) const
syntactic sugar for rotate
Definition Rot2.h:167
│ │ │ │ +
gtsam::Rot2::s
double s() const
return sin
Definition Rot2.h:203
│ │ │ │ +
gtsam::Rot2::Identity
static Rot2 Identity()
Identity.
Definition Rot2.h:111
│ │ │ │ +
gtsam::Rot2::degrees
double degrees() const
return angle (DEGREES)
Definition Rot2.h:192
│ │ │ │ +
gtsam::Rot2::LogmapDerivative
static Matrix LogmapDerivative(const Vector &)
Left-trivialized derivative inverse of the exponential map.
Definition Rot2.h:140
│ │ │ │ +
gtsam::Rot2::AdjointMap
Matrix1 AdjointMap() const
Calculate Adjoint map.
Definition Rot2.h:132
│ │ │ │ +
gtsam::Rot2::Rot2
Rot2(double theta)
Constructor from angle in radians == exponential map at identity.
Definition Rot2.h:59
│ │ │ │ +
gtsam::Rot2::fromDegrees
static Rot2 fromDegrees(double theta)
Named constructor from angle in degrees.
Definition Rot2.h:67
│ │ │ │ +
gtsam::Rot2::Rot2
Rot2()
default constructor, zero rotation
Definition Rot2.h:53
│ │ │ │ +
gtsam::Rot2::Rot2
Rot2(const Rot2 &r)
copy constructor
Definition Rot2.h:56
│ │ │ │ +
gtsam::Rot2::fromAngle
static Rot2 fromAngle(double theta)
Named constructor from angle in radians.
Definition Rot2.h:62
│ │ │ │ +
gtsam::Rot2::ChartAtOrigin
Definition Rot2.h:145
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,286 +1,277 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DiscreteConditional.h │ │ │ │ │ +Rot2.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include │ │ │ │ │ -22#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_S_i_g_n_a_t_u_r_e_._h> │ │ │ │ │ -24 │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ -28#include │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_b_a_s_e_/_L_i_e_._h> │ │ │ │ │ +24#include │ │ │ │ │ +25 │ │ │ │ │ +26#include │ │ │ │ │ +27 │ │ │ │ │ +28namespace _g_t_s_a_m { │ │ │ │ │ 29 │ │ │ │ │ -30namespace _g_t_s_a_m { │ │ │ │ │ -31 │ │ │ │ │ -_3_8class GTSAM_EXPORT _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -39 : public _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r, │ │ │ │ │ -40 public _C_o_n_d_i_t_i_o_n_a_l { │ │ │ │ │ -41 public: │ │ │ │ │ -42 // typedefs needed to play nice with gtsam │ │ │ │ │ -_4_3 typedef _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l _T_h_i_s; │ │ │ │ │ -_4_4 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -_4_5 typedef _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r _B_a_s_e_F_a_c_t_o_r; │ │ │ │ │ -46 typedef _C_o_n_d_i_t_i_o_n_a_l_<_B_a_s_e_F_a_c_t_o_r_,_ _T_h_i_s_> │ │ │ │ │ -_4_7 _B_a_s_e_C_o_n_d_i_t_i_o_n_a_l; │ │ │ │ │ +_3_6 class GTSAM_EXPORT _R_o_t_2 : public _L_i_e_G_r_o_u_p { │ │ │ │ │ +37 │ │ │ │ │ +39 double c_, s_; │ │ │ │ │ +40 │ │ │ │ │ +42 _R_o_t_2& _n_o_r_m_a_l_i_z_e(); │ │ │ │ │ +43 │ │ │ │ │ +45 inline _R_o_t_2(double c, double s) : c_(c), s_(s) {} │ │ │ │ │ +46 │ │ │ │ │ +47 public: │ │ │ │ │ 48 │ │ │ │ │ -_4_9 using _V_a_l_u_e_s = _D_i_s_c_r_e_t_e_V_a_l_u_e_s; │ │ │ │ │ -50 │ │ │ │ │ -53 │ │ │ │ │ -_5_5 _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l() {} │ │ │ │ │ -56 │ │ │ │ │ -58 _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l(size_t nFrontals, const _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r& f); │ │ │ │ │ -59 │ │ │ │ │ -64 _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l(size_t nFrontals, const _D_i_s_c_r_e_t_e_K_e_y_s& keys, │ │ │ │ │ -65 const ADT& potentials); │ │ │ │ │ -66 │ │ │ │ │ -68 explicit _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l(const _S_i_g_n_a_t_u_r_e& signature); │ │ │ │ │ -69 │ │ │ │ │ -_7_7 _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l(const _D_i_s_c_r_e_t_e_K_e_y& key, const _D_i_s_c_r_e_t_e_K_e_y_s& parents, │ │ │ │ │ -78 const Signature::Table& table) │ │ │ │ │ -79 : _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l(_S_i_g_n_a_t_u_r_e(key, parents, table)) {} │ │ │ │ │ -80 │ │ │ │ │ -_9_0 _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l(const _D_i_s_c_r_e_t_e_K_e_y& key, const _D_i_s_c_r_e_t_e_K_e_y_s& parents, │ │ │ │ │ -91 const std::string& spec) │ │ │ │ │ -92 : _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l(_S_i_g_n_a_t_u_r_e(key, parents, spec)) {} │ │ │ │ │ -93 │ │ │ │ │ -_9_5 _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l(const _D_i_s_c_r_e_t_e_K_e_y& key, const std::string& spec) │ │ │ │ │ -96 : _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l(_S_i_g_n_a_t_u_r_e(key, {}, spec)) {} │ │ │ │ │ -97 │ │ │ │ │ -102 DiscreteConditional(const DecisionTreeFactor& joint, │ │ │ │ │ -103 const DecisionTreeFactor& marginal); │ │ │ │ │ -104 │ │ │ │ │ -110 DiscreteConditional(const DecisionTreeFactor& joint, │ │ │ │ │ -111 const DecisionTreeFactor& marginal, │ │ │ │ │ -112 const Ordering& orderedKeys); │ │ │ │ │ -113 │ │ │ │ │ -129 DiscreteConditional operator*(const DiscreteConditional& other) const; │ │ │ │ │ +51 │ │ │ │ │ +_5_3 _R_o_t_2() : c_(1.0), s_(0.0) {} │ │ │ │ │ +54 │ │ │ │ │ +_5_6 _R_o_t_2(const _R_o_t_2& r) : _R_o_t_2(r.c_, r.s_) {} │ │ │ │ │ +57 │ │ │ │ │ +_5_9 _R_o_t_2(double theta) : c_(cos(theta)), s_(sin(theta)) {} │ │ │ │ │ +60 │ │ │ │ │ +_6_2 static _R_o_t_2 _f_r_o_m_A_n_g_l_e(double theta) { │ │ │ │ │ +63 return _R_o_t_2(theta); │ │ │ │ │ +64 } │ │ │ │ │ +65 │ │ │ │ │ +_6_7 static _R_o_t_2 _f_r_o_m_D_e_g_r_e_e_s(double theta) { │ │ │ │ │ +68 static const double degree = M_PI / 180; │ │ │ │ │ +69 return fromAngle(theta * degree); │ │ │ │ │ +70 } │ │ │ │ │ +71 │ │ │ │ │ +73 static _R_o_t_2 fromCosSin(double c, double s); │ │ │ │ │ +74 │ │ │ │ │ +82 static _R_o_t_2 relativeBearing(const _P_o_i_n_t_2& d, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_2_> H = │ │ │ │ │ +83 boost::none); │ │ │ │ │ +84 │ │ │ │ │ +86 static _R_o_t_2 atan2(double y, double x); │ │ │ │ │ +87 │ │ │ │ │ +94 static _R_o_t_2 Random(std::mt19937 & rng); │ │ │ │ │ +95 │ │ │ │ │ +99 │ │ │ │ │ +101 void _p_r_i_n_t(const std::string& s = "theta") const; │ │ │ │ │ +102 │ │ │ │ │ +104 bool _e_q_u_a_l_s(const _R_o_t_2& R, double tol = 1e-9) const; │ │ │ │ │ +105 │ │ │ │ │ +109 │ │ │ │ │ +_1_1_1 inline static _R_o_t_2 _I_d_e_n_t_i_t_y() { return _R_o_t_2(); } │ │ │ │ │ +112 │ │ │ │ │ +_1_1_4 _R_o_t_2 _i_n_v_e_r_s_e() const { return _R_o_t_2(c_, -s_);} │ │ │ │ │ +115 │ │ │ │ │ +_1_1_7 _R_o_t_2 _o_p_e_r_a_t_o_r_*(const _R_o_t_2& R) const { │ │ │ │ │ +118 return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_); │ │ │ │ │ +119 } │ │ │ │ │ +120 │ │ │ │ │ +124 │ │ │ │ │ +126 static _R_o_t_2 Expmap(const Vector1& v, ChartJacobian H = boost::none); │ │ │ │ │ +127 │ │ │ │ │ +129 static Vector1 Logmap(const _R_o_t_2& r, ChartJacobian H = boost::none); │ │ │ │ │ 130 │ │ │ │ │ -132 DiscreteConditional marginal(Key key) const; │ │ │ │ │ +_1_3_2 Matrix1 _A_d_j_o_i_n_t_M_a_p() const { return I_1x1; } │ │ │ │ │ 133 │ │ │ │ │ -137 │ │ │ │ │ -139 void print( │ │ │ │ │ -140 const std::string& s = "Discrete Conditional: ", │ │ │ │ │ -141 const KeyFormatter& formatter = DefaultKeyFormatter) const override; │ │ │ │ │ -142 │ │ │ │ │ -144 bool equals(const DiscreteFactor& other, double tol = 1e-9) const override; │ │ │ │ │ -145 │ │ │ │ │ -149 │ │ │ │ │ -_1_5_1 double _l_o_g_P_r_o_b_a_b_i_l_i_t_y(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& x) const { │ │ │ │ │ -152 return -error(x); │ │ │ │ │ -153 } │ │ │ │ │ -154 │ │ │ │ │ -_1_5_6 void _p_r_i_n_t_S_i_g_n_a_t_u_r_e( │ │ │ │ │ -157 const std::string& s = "Discrete Conditional: ", │ │ │ │ │ -158 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const { │ │ │ │ │ -159 static_cast(this)->_p_r_i_n_t(s, formatter); │ │ │ │ │ -160 } │ │ │ │ │ -161 │ │ │ │ │ -_1_6_3 double _e_v_a_l_u_a_t_e(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values) const { │ │ │ │ │ -164 return ADT::operator()(values); │ │ │ │ │ -165 } │ │ │ │ │ -166 │ │ │ │ │ -167 using DecisionTreeFactor::error; │ │ │ │ │ -168 using DecisionTreeFactor::operator(); │ │ │ │ │ -169 │ │ │ │ │ -183 shared_ptr choose(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& given) const; │ │ │ │ │ -184 │ │ │ │ │ -186 DecisionTreeFactor::shared_ptr likelihood( │ │ │ │ │ -187 const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& frontalValues) const; │ │ │ │ │ -188 │ │ │ │ │ -190 DecisionTreeFactor::shared_ptr likelihood(size_t frontal) const; │ │ │ │ │ -191 │ │ │ │ │ -197 size_t sample(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& parentsValues) const; │ │ │ │ │ -198 │ │ │ │ │ -200 size_t sample(size_t parent_value) const; │ │ │ │ │ +_1_3_5 static Matrix _E_x_p_m_a_p_D_e_r_i_v_a_t_i_v_e(const Vector& /*v*/) { │ │ │ │ │ +136 return I_1x1; │ │ │ │ │ +137 } │ │ │ │ │ +138 │ │ │ │ │ +_1_4_0 static Matrix _L_o_g_m_a_p_D_e_r_i_v_a_t_i_v_e(const Vector& /*v*/) { │ │ │ │ │ +141 return I_1x1; │ │ │ │ │ +142 } │ │ │ │ │ +143 │ │ │ │ │ +144 // Chart at origin simply uses exponential map and its inverse │ │ │ │ │ +_1_4_5 struct _C_h_a_r_t_A_t_O_r_i_g_i_n { │ │ │ │ │ +146 static _R_o_t_2 Retract(const Vector1& v, ChartJacobian H = boost::none) { │ │ │ │ │ +147 return Expmap(v, H); │ │ │ │ │ +148 } │ │ │ │ │ +149 static Vector1 Local(const _R_o_t_2& r, ChartJacobian H = boost::none) { │ │ │ │ │ +150 return Logmap(r, H); │ │ │ │ │ +151 } │ │ │ │ │ +152 }; │ │ │ │ │ +153 │ │ │ │ │ +154 using _L_i_e_G_r_o_u_p<_R_o_t_2, 1>::inverse; // version with derivative │ │ │ │ │ +155 │ │ │ │ │ +159 │ │ │ │ │ +163 _P_o_i_n_t_2 rotate(const _P_o_i_n_t_2& p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _1_> H1 = boost::none, │ │ │ │ │ +164 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> H2 = boost::none) const; │ │ │ │ │ +165 │ │ │ │ │ +_1_6_7 inline _P_o_i_n_t_2 _o_p_e_r_a_t_o_r_*(const _P_o_i_n_t_2& p) const { │ │ │ │ │ +168 return rotate(p); │ │ │ │ │ +169 } │ │ │ │ │ +170 │ │ │ │ │ +174 _P_o_i_n_t_2 unrotate(const _P_o_i_n_t_2& p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _1_> H1 = boost::none, │ │ │ │ │ +175 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> H2 = boost::none) const; │ │ │ │ │ +176 │ │ │ │ │ +180 │ │ │ │ │ +_1_8_2 inline _P_o_i_n_t_2 _u_n_i_t() const { │ │ │ │ │ +183 return _P_o_i_n_t_2(c_, s_); │ │ │ │ │ +184 } │ │ │ │ │ +185 │ │ │ │ │ +_1_8_7 double _t_h_e_t_a() const { │ │ │ │ │ +188 return ::atan2(s_, c_); │ │ │ │ │ +189 } │ │ │ │ │ +190 │ │ │ │ │ +_1_9_2 double _d_e_g_r_e_e_s() const { │ │ │ │ │ +193 const double degree = M_PI / 180; │ │ │ │ │ +194 return theta() / degree; │ │ │ │ │ +195 } │ │ │ │ │ +196 │ │ │ │ │ +_1_9_8 inline double _c() const { │ │ │ │ │ +199 return c_; │ │ │ │ │ +200 } │ │ │ │ │ 201 │ │ │ │ │ -203 size_t sample() const; │ │ │ │ │ -204 │ │ │ │ │ -209 size_t argmax() const; │ │ │ │ │ -210 │ │ │ │ │ -214 │ │ │ │ │ -216 void sampleInPlace(_D_i_s_c_r_e_t_e_V_a_l_u_e_s* parentsValues) const; │ │ │ │ │ -217 │ │ │ │ │ -219 std::vector frontalAssignments() const; │ │ │ │ │ -220 │ │ │ │ │ -222 std::vector allAssignments() const; │ │ │ │ │ -223 │ │ │ │ │ -227 │ │ │ │ │ -229 std::string _m_a_r_k_d_o_w_n(const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ -DefaultKeyFormatter, │ │ │ │ │ -230 const Names& names = {}) const override; │ │ │ │ │ -231 │ │ │ │ │ -233 std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ -234 const Names& names = {}) const override; │ │ │ │ │ -235 │ │ │ │ │ -236 │ │ │ │ │ -240 │ │ │ │ │ -245 double evaluate(const _H_y_b_r_i_d_V_a_l_u_e_s& x) const override; │ │ │ │ │ -246 │ │ │ │ │ -247 using BaseConditional::operator(); │ │ │ │ │ -248 │ │ │ │ │ -_2_5_3 double _l_o_g_P_r_o_b_a_b_i_l_i_t_y(const _H_y_b_r_i_d_V_a_l_u_e_s& x) const override { │ │ │ │ │ -254 return -error(x); │ │ │ │ │ -255 } │ │ │ │ │ -256 │ │ │ │ │ -_2_6_2 double _l_o_g_N_o_r_m_a_l_i_z_a_t_i_o_n_C_o_n_s_t_a_n_t() const override { return 0.0; } │ │ │ │ │ -263 │ │ │ │ │ -265 │ │ │ │ │ -266#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ -269 size_t GTSAM_DEPRECATED solve(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& parentsValues) const; │ │ │ │ │ -270 void GTSAM_DEPRECATED solveInPlace(_D_i_s_c_r_e_t_e_V_a_l_u_e_s* parentsValues) const; │ │ │ │ │ -272#endif │ │ │ │ │ -273 │ │ │ │ │ -274 protected: │ │ │ │ │ -276 _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_A_D_T choose(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& given, │ │ │ │ │ -277 bool forceComplete) const; │ │ │ │ │ -278 │ │ │ │ │ -279 private: │ │ │ │ │ -_2_8_1 friend class boost::serialization::access; │ │ │ │ │ -282 template │ │ │ │ │ -283 void serialize(Archive& ar, const unsigned int /*version*/) { │ │ │ │ │ -284 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e_F_a_c_t_o_r); │ │ │ │ │ -285 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e_C_o_n_d_i_t_i_o_n_a_l); │ │ │ │ │ -286 } │ │ │ │ │ -287}; │ │ │ │ │ -288// DiscreteConditional │ │ │ │ │ -289 │ │ │ │ │ -290// traits │ │ │ │ │ -291template <> │ │ │ │ │ -_2_9_2struct _t_r_a_i_t_s<_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l> : public _T_e_s_t_a_b_l_e │ │ │ │ │ -{}; │ │ │ │ │ -293 │ │ │ │ │ -294} // namespace gtsam │ │ │ │ │ -_S_i_g_n_a_t_u_r_e_._h │ │ │ │ │ -signatures for conditional densities │ │ │ │ │ -_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_._h │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y │ │ │ │ │ -std::pair< Key, size_t > DiscreteKey │ │ │ │ │ -Key type for discrete variables. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteKey.h:36 │ │ │ │ │ +_2_0_3 inline double _s() const { │ │ │ │ │ +204 return s_; │ │ │ │ │ +205 } │ │ │ │ │ +206 │ │ │ │ │ +208 Matrix2 matrix() const; │ │ │ │ │ +209 │ │ │ │ │ +211 Matrix2 transpose() const; │ │ │ │ │ +212 │ │ │ │ │ +214 static _R_o_t_2 ClosestTo(const Matrix2& M); │ │ │ │ │ +215 │ │ │ │ │ +216 private: │ │ │ │ │ +_2_1_8 friend class boost::serialization::access; │ │ │ │ │ +219 template │ │ │ │ │ +220 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +221 ar & BOOST_SERIALIZATION_NVP(c_); │ │ │ │ │ +222 ar & BOOST_SERIALIZATION_NVP(s_); │ │ │ │ │ +223 } │ │ │ │ │ +224 │ │ │ │ │ +225 }; │ │ │ │ │ +226 │ │ │ │ │ +227 template<> │ │ │ │ │ +_2_2_8 struct _t_r_a_i_t_s<_R_o_t_2> : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ +229 │ │ │ │ │ +230 template<> │ │ │ │ │ +_2_3_1 struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ +232 │ │ │ │ │ +233} // gtsam │ │ │ │ │ +_L_i_e_._h │ │ │ │ │ +Base class and basic functions for Lie types. │ │ │ │ │ +_P_o_i_n_t_2_._h │ │ │ │ │ +2D Point │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_m_a_r_k_d_o_w_n │ │ │ │ │ -string markdown(const DiscreteValues &values, const KeyFormatter &keyFormatter, │ │ │ │ │ -const DiscreteValues::Names &names) │ │ │ │ │ -Free version of markdown. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteValues.cpp:129 │ │ │ │ │ _g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ print without optional string, must specify cout yourself │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ +Vector2 Point2 │ │ │ │ │ +As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ +typedef Point2 to Vector2... │ │ │ │ │ +DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_r_m_a_l_i_z_e │ │ │ │ │ +Point3 normalize(const Point3 &p, OptionalJacobian< 3, 3 > H) │ │ │ │ │ +normalize, with optional Jacobian │ │ │ │ │ +DDeeffiinniittiioonn Point3.cpp:52 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_<_ _K_e_y_ _> │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r │ │ │ │ │ -A discrete probabilistic factor. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTreeFactor.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -Discrete Conditional Density Derives from DecisionTreeFactor. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteConditional.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -DiscreteConditional(const DiscreteKey &key, const std::string &spec) │ │ │ │ │ -No-parent specialization; can also use DiscreteDistribution. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteConditional.h:95 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -DiscreteConditional(const DiscreteKey &key, const DiscreteKeys &parents, const │ │ │ │ │ -std::string &spec) │ │ │ │ │ -Construct from key, parents, and a string specifying the conditional │ │ │ │ │ -probability table (CPT) in 00 01... │ │ │ │ │ -DDeeffiinniittiioonn DiscreteConditional.h:90 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_e_v_a_l_u_a_t_e │ │ │ │ │ -double evaluate(const DiscreteValues &values) const │ │ │ │ │ -Evaluate, just look up in AlgebraicDecisonTree. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteConditional.h:163 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn DiscreteConditional.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -DiscreteConditional(const DiscreteKey &key, const DiscreteKeys &parents, const │ │ │ │ │ -Signature::Table &table) │ │ │ │ │ -Construct from key, parents, and a Signature::Table specifying the conditional │ │ │ │ │ -probability table (CPT... │ │ │ │ │ -DDeeffiinniittiioonn DiscreteConditional.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_B_a_s_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -Conditional< BaseFactor, This > BaseConditional │ │ │ │ │ -Typedef to our conditional base class. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteConditional.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_l_o_g_P_r_o_b_a_b_i_l_i_t_y │ │ │ │ │ -double logProbability(const HybridValues &x) const override │ │ │ │ │ -< HybridValues version │ │ │ │ │ -DDeeffiinniittiioonn DiscreteConditional.h:253 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_T_h_i_s │ │ │ │ │ -DiscreteConditional This │ │ │ │ │ -Typedef to this class. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteConditional.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_B_a_s_e_F_a_c_t_o_r │ │ │ │ │ -DecisionTreeFactor BaseFactor │ │ │ │ │ -Typedef to our factor base class. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteConditional.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -DiscreteConditional() │ │ │ │ │ -Default constructor needed for serialization. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteConditional.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_l_o_g_P_r_o_b_a_b_i_l_i_t_y │ │ │ │ │ -double logProbability(const DiscreteValues &x) const │ │ │ │ │ -Log-probability is just -error(x). │ │ │ │ │ -DDeeffiinniittiioonn DiscreteConditional.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_p_r_i_n_t_S_i_g_n_a_t_u_r_e │ │ │ │ │ -void printSignature(const std::string &s="Discrete Conditional: ", const │ │ │ │ │ -KeyFormatter &formatter=DefaultKeyFormatter) const │ │ │ │ │ -print index signature only │ │ │ │ │ -DDeeffiinniittiioonn DiscreteConditional.h:156 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_l_o_g_N_o_r_m_a_l_i_z_a_t_i_o_n_C_o_n_s_t_a_n_t │ │ │ │ │ -double logNormalizationConstant() const override │ │ │ │ │ -logNormalizationConstant K is just zero, such that logProbability(x) = log │ │ │ │ │ -(evaluate(x)) = - error(x) ... │ │ │ │ │ -DDeeffiinniittiioonn DiscreteConditional.h:262 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ -DiscreteKeys is a set of keys that can be assembled using the & operator. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteKey.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ -A map from keys to values. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteValues.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_S_i_g_n_a_t_u_r_e │ │ │ │ │ -Signature for a discrete conditional density, used to construct conditionals. │ │ │ │ │ -DDeeffiinniittiioonn Signature.h:54 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:64 │ │ │ │ │ -_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -the error. │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ +A CRTP helper class that implements Lie group methods Prerequisites: methods │ │ │ │ │ +operator*,... │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ +Both LieGroupTraits and Testable. │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:229 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ +either a fixed size o... │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_2 │ │ │ │ │ +Rotation matrix NOTE: the angle theta is in radians unless explicitly stated. │ │ │ │ │ +DDeeffiinniittiioonn Rot2.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_2_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ +Rot2 operator*(const Rot2 &R) const │ │ │ │ │ +Compose - make a new rotation by adding angles. │ │ │ │ │ +DDeeffiinniittiioonn Rot2.h:117 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_2_:_:_c │ │ │ │ │ +double c() const │ │ │ │ │ +return cos │ │ │ │ │ +DDeeffiinniittiioonn Rot2.h:198 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_2_:_:_E_x_p_m_a_p_D_e_r_i_v_a_t_i_v_e │ │ │ │ │ +static Matrix ExpmapDerivative(const Vector &) │ │ │ │ │ +Left-trivialized derivative of the exponential map. │ │ │ │ │ +DDeeffiinniittiioonn Rot2.h:135 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_2_:_:_u_n_i_t │ │ │ │ │ +Point2 unit() const │ │ │ │ │ +Creates a unit vector as a Point2. │ │ │ │ │ +DDeeffiinniittiioonn Rot2.h:182 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_2_:_:_t_h_e_t_a │ │ │ │ │ +double theta() const │ │ │ │ │ +return angle (RADIANS) │ │ │ │ │ +DDeeffiinniittiioonn Rot2.h:187 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_2_:_:_i_n_v_e_r_s_e │ │ │ │ │ +Rot2 inverse() const │ │ │ │ │ +The inverse rotation - negative angle. │ │ │ │ │ +DDeeffiinniittiioonn Rot2.h:114 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_2_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ +Point2 operator*(const Point2 &p) const │ │ │ │ │ +syntactic sugar for rotate │ │ │ │ │ +DDeeffiinniittiioonn Rot2.h:167 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_2_:_:_s │ │ │ │ │ +double s() const │ │ │ │ │ +return sin │ │ │ │ │ +DDeeffiinniittiioonn Rot2.h:203 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_2_:_:_I_d_e_n_t_i_t_y │ │ │ │ │ +static Rot2 Identity() │ │ │ │ │ +Identity. │ │ │ │ │ +DDeeffiinniittiioonn Rot2.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_2_:_:_d_e_g_r_e_e_s │ │ │ │ │ +double degrees() const │ │ │ │ │ +return angle (DEGREES) │ │ │ │ │ +DDeeffiinniittiioonn Rot2.h:192 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_2_:_:_L_o_g_m_a_p_D_e_r_i_v_a_t_i_v_e │ │ │ │ │ +static Matrix LogmapDerivative(const Vector &) │ │ │ │ │ +Left-trivialized derivative inverse of the exponential map. │ │ │ │ │ +DDeeffiinniittiioonn Rot2.h:140 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_2_:_:_A_d_j_o_i_n_t_M_a_p │ │ │ │ │ +Matrix1 AdjointMap() const │ │ │ │ │ +Calculate Adjoint map. │ │ │ │ │ +DDeeffiinniittiioonn Rot2.h:132 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_2_:_:_R_o_t_2 │ │ │ │ │ +Rot2(double theta) │ │ │ │ │ +Constructor from angle in radians == exponential map at identity. │ │ │ │ │ +DDeeffiinniittiioonn Rot2.h:59 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_2_:_:_f_r_o_m_D_e_g_r_e_e_s │ │ │ │ │ +static Rot2 fromDegrees(double theta) │ │ │ │ │ +Named constructor from angle in degrees. │ │ │ │ │ +DDeeffiinniittiioonn Rot2.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_2_:_:_R_o_t_2 │ │ │ │ │ +Rot2() │ │ │ │ │ +default constructor, zero rotation │ │ │ │ │ +DDeeffiinniittiioonn Rot2.h:53 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_2_:_:_R_o_t_2 │ │ │ │ │ +Rot2(const Rot2 &r) │ │ │ │ │ +copy constructor │ │ │ │ │ +DDeeffiinniittiioonn Rot2.h:56 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_2_:_:_f_r_o_m_A_n_g_l_e │ │ │ │ │ +static Rot2 fromAngle(double theta) │ │ │ │ │ +Named constructor from angle in radians. │ │ │ │ │ +DDeeffiinniittiioonn Rot2.h:62 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_2_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n │ │ │ │ │ +DDeeffiinniittiioonn Rot2.h:145 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ + * _g_e_o_m_e_t_r_y │ │ │ │ │ + * _R_o_t_2_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00227_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00242_source.html │ │ │ │┄ Files 99% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
AlgebraicDecisionTree.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/base/Testable.h>
│ │ │ │ +
21#include <gtsam/base/Testable.h>
│ │ │ │
22#include <gtsam/discrete/DecisionTree-inl.h>
│ │ │ │
23
│ │ │ │
24#include <algorithm>
│ │ │ │
25#include <map>
│ │ │ │
26#include <string>
│ │ │ │
27#include <vector>
│ │ │ │
28namespace gtsam {
│ │ │ │ @@ -276,15 +276,15 @@ │ │ │ │
227
│ │ │ │
228template <typename T>
│ │ │ │
│ │ │ │
229struct traits<AlgebraicDecisionTree<T>>
│ │ │ │
230 : public Testable<AlgebraicDecisionTree<T>> {};
│ │ │ │
│ │ │ │
231} // namespace gtsam
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::apply
DecisionTree< L, Y > apply(const DecisionTree< L, Y > &f, const typename DecisionTree< L, Y >::Unary &op)
free versions of apply
Definition DecisionTree.h:413
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │
gtsam::AlgebraicDecisionTree
An algebraic decision tree fixes the range of a DecisionTree to double.
Definition AlgebraicDecisionTree.h:38
│ │ │ │
gtsam::AlgebraicDecisionTree::operator/
AlgebraicDecisionTree operator/(const AlgebraicDecisionTree &g) const
division
Definition AlgebraicDecisionTree.h:194
│ │ │ │
gtsam::AlgebraicDecisionTree::sum
AlgebraicDecisionTree sum(const L &label, size_t cardinality) const
sum out variable
Definition AlgebraicDecisionTree.h:199
│ │ │ │ @@ -302,13 +302,13 @@ │ │ │ │
gtsam::DecisionTree::root_
NodePtr root_
A DecisionTree just contains the root. TODO(dellaert): make protected.
Definition DecisionTree.h:146
│ │ │ │
gtsam::DecisionTree< L, double >::LabelC
std::pair< L, size_t > LabelC
A label annotated with cardinality.
Definition DecisionTree.h:79
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00230_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00170_source.html │ │ │ │┄ Files 90% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteFactorGraph.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/BasisFactors.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
DiscreteFactorGraph.h
│ │ │ │ +
BasisFactors.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/discrete/DecisionTreeFactor.h>
│ │ │ │ -
22#include <gtsam/discrete/DiscreteLookupDAG.h>
│ │ │ │ -
23#include <gtsam/inference/EliminateableFactorGraph.h>
│ │ │ │ -
24#include <gtsam/inference/FactorGraph.h>
│ │ │ │ -
25#include <gtsam/inference/Ordering.h>
│ │ │ │ -
26#include <gtsam/base/FastSet.h>
│ │ │ │ -
27
│ │ │ │ -
28#include <boost/make_shared.hpp>
│ │ │ │ -
29#include <string>
│ │ │ │ -
30#include <utility>
│ │ │ │ -
31#include <vector>
│ │ │ │ -
32
│ │ │ │ -
33namespace gtsam {
│ │ │ │ -
34
│ │ │ │ -
35// Forward declarations
│ │ │ │ -
36class DiscreteFactorGraph;
│ │ │ │ -
37class DiscreteConditional;
│ │ │ │ -
38class DiscreteBayesNet;
│ │ │ │ -
39class DiscreteEliminationTree;
│ │ │ │ -
40class DiscreteBayesTree;
│ │ │ │ -
41class DiscreteJunctionTree;
│ │ │ │ +
21#include <gtsam/basis/Basis.h>
│ │ │ │ +
22#include <gtsam/nonlinear/FunctorizedFactor.h>
│ │ │ │ +
23
│ │ │ │ +
24namespace gtsam {
│ │ │ │ +
25
│ │ │ │ +
38template <class BASIS>
│ │ │ │ +
│ │ │ │ +
39class EvaluationFactor : public FunctorizedFactor<double, Vector> {
│ │ │ │ +
40 private:
│ │ │ │ +
41 using Base = FunctorizedFactor<double, Vector>;
│ │ │ │
42
│ │ │ │ -
51GTSAM_EXPORT
│ │ │ │ -
52std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>
│ │ │ │ -
53EliminateDiscrete(const DiscreteFactorGraph& factors,
│ │ │ │ -
54 const Ordering& frontalKeys);
│ │ │ │ -
55
│ │ │ │ -
64GTSAM_EXPORT
│ │ │ │ -
65std::pair<DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr>
│ │ │ │ -
66EliminateForMPE(const DiscreteFactorGraph& factors,
│ │ │ │ -
67 const Ordering& frontalKeys);
│ │ │ │ -
68
│ │ │ │ -
│ │ │ │ -
69template<> struct EliminationTraits<DiscreteFactorGraph>
│ │ │ │ -
70{
│ │ │ │ -
71 typedef DiscreteFactor FactorType;
│ │ │ │ -
72 typedef DiscreteFactorGraph FactorGraphType;
│ │ │ │ -
73 typedef DiscreteConditional ConditionalType;
│ │ │ │ -
74 typedef DiscreteBayesNet BayesNetType;
│ │ │ │ -
75 typedef DiscreteEliminationTree EliminationTreeType;
│ │ │ │ -
76 typedef DiscreteBayesTree BayesTreeType;
│ │ │ │ -
77 typedef DiscreteJunctionTree JunctionTreeType;
│ │ │ │ -
78
│ │ │ │ -
80 static std::pair<boost::shared_ptr<ConditionalType>,
│ │ │ │ -
81 boost::shared_ptr<FactorType> >
│ │ │ │ -
│ │ │ │ -
82 DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) {
│ │ │ │ -
83 return EliminateDiscrete(factors, keys);
│ │ │ │ -
84 }
│ │ │ │ -
│ │ │ │ -
85
│ │ │ │ -
│ │ │ │ -
87 static Ordering DefaultOrderingFunc(
│ │ │ │ -
88 const FactorGraphType& graph,
│ │ │ │ -
89 boost::optional<const VariableIndex&> variableIndex) {
│ │ │ │ -
90 return Ordering::Colamd(*variableIndex);
│ │ │ │ -
91 }
│ │ │ │ -
│ │ │ │ -
92};
│ │ │ │ -
│ │ │ │ -
93
│ │ │ │ -
│ │ │ │ -
99class GTSAM_EXPORT DiscreteFactorGraph
│ │ │ │ -
100 : public FactorGraph<DiscreteFactor>,
│ │ │ │ -
101 public EliminateableFactorGraph<DiscreteFactorGraph> {
│ │ │ │ -
102 public:
│ │ │ │ -
103 using This = DiscreteFactorGraph;
│ │ │ │ -
104 using Base = FactorGraph<DiscreteFactor>;
│ │ │ │ -
105 using BaseEliminateable =
│ │ │ │ -
106 EliminateableFactorGraph<This>;
│ │ │ │ -
107 using shared_ptr = boost::shared_ptr<This>;
│ │ │ │ -
108
│ │ │ │ -
109 using Values = DiscreteValues;
│ │ │ │ -
110
│ │ │ │ -
111 using Indices = KeyVector;
│ │ │ │ -
112
│ │ │ │ -
114 DiscreteFactorGraph() {}
│ │ │ │ -
115
│ │ │ │ -
117 template <typename ITERATOR>
│ │ │ │ -
│ │ │ │ -
118 DiscreteFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
│ │ │ │ -
119 : Base(firstFactor, lastFactor) {}
│ │ │ │ -
│ │ │ │ -
120
│ │ │ │ -
122 template <class CONTAINER>
│ │ │ │ -
123 explicit DiscreteFactorGraph(const CONTAINER& factors) : Base(factors) {}
│ │ │ │ -
124
│ │ │ │ -
127 template <class DERIVED_FACTOR>
│ │ │ │ -
128 DiscreteFactorGraph(const FactorGraph<DERIVED_FACTOR>& graph) : Base(graph) {}
│ │ │ │ -
129
│ │ │ │ -
131 virtual ~DiscreteFactorGraph() {}
│ │ │ │ -
132
│ │ │ │ -
135
│ │ │ │ -
136 bool equals(const This& fg, double tol = 1e-9) const;
│ │ │ │ -
137
│ │ │ │ +
43 public:
│ │ │ │ +
44 EvaluationFactor() {}
│ │ │ │ +
45
│ │ │ │ +
│ │ │ │ +
55 EvaluationFactor(Key key, double z, const SharedNoiseModel &model,
│ │ │ │ +
56 const size_t N, double x)
│ │ │ │ +
57 : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x)) {}
│ │ │ │ +
│ │ │ │ +
58
│ │ │ │ +
│ │ │ │ +
70 EvaluationFactor(Key key, double z, const SharedNoiseModel &model,
│ │ │ │ +
71 const size_t N, double x, double a, double b)
│ │ │ │ +
72 : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x, a, b)) {}
│ │ │ │ +
│ │ │ │ +
73
│ │ │ │ +
74 virtual ~EvaluationFactor() {}
│ │ │ │ +
75};
│ │ │ │ +
│ │ │ │ +
76
│ │ │ │ +
94template <class BASIS, int M>
│ │ │ │ +
│ │ │ │ +
95class VectorEvaluationFactor
│ │ │ │ +
96 : public FunctorizedFactor<Vector, ParameterMatrix<M>> {
│ │ │ │ +
97 private:
│ │ │ │ +
98 using Base = FunctorizedFactor<Vector, ParameterMatrix<M>>;
│ │ │ │ +
99
│ │ │ │ +
100 public:
│ │ │ │ +
101 VectorEvaluationFactor() {}
│ │ │ │ +
102
│ │ │ │ +
│ │ │ │ +
113 VectorEvaluationFactor(Key key, const Vector &z,
│ │ │ │ +
114 const SharedNoiseModel &model, const size_t N,
│ │ │ │ +
115 double x)
│ │ │ │ +
116 : Base(key, z, model,
│ │ │ │ +
117 typename BASIS::template VectorEvaluationFunctor<M>(N, x)) {}
│ │ │ │ +
│ │ │ │ +
118
│ │ │ │ +
│ │ │ │ +
131 VectorEvaluationFactor(Key key, const Vector &z,
│ │ │ │ +
132 const SharedNoiseModel &model, const size_t N,
│ │ │ │ +
133 double x, double a, double b)
│ │ │ │ +
134 : Base(key, z, model,
│ │ │ │ +
135 typename BASIS::template VectorEvaluationFunctor<M>(N, x, a, b)) {}
│ │ │ │ +
│ │ │ │ +
136
│ │ │ │ +
137 virtual ~VectorEvaluationFactor() {}
│ │ │ │ +
138};
│ │ │ │ +
│ │ │ │
139
│ │ │ │ -
141 template <typename... Args>
│ │ │ │ -
│ │ │ │ -
142 void add(Args&&... args) {
│ │ │ │ -
143 emplace_shared<DecisionTreeFactor>(std::forward<Args>(args)...);
│ │ │ │ -
144 }
│ │ │ │ -
│ │ │ │ -
145
│ │ │ │ -
147 KeySet keys() const;
│ │ │ │ -
148
│ │ │ │ -
150 DiscreteKeys discreteKeys() const;
│ │ │ │ -
151
│ │ │ │ -
153 DecisionTreeFactor product() const;
│ │ │ │ -
154
│ │ │ │ -
159 double operator()(const DiscreteValues& values) const;
│ │ │ │ -
160
│ │ │ │ -
162 void print(
│ │ │ │ -
163 const std::string& s = "DiscreteFactorGraph",
│ │ │ │ -
164 const KeyFormatter& formatter = DefaultKeyFormatter) const override;
│ │ │ │ -
165
│ │ │ │ -
172 DiscreteBayesNet sumProduct(
│ │ │ │ -
173 OptionalOrderingType orderingType = boost::none) const;
│ │ │ │ -
174
│ │ │ │ -
181 DiscreteBayesNet sumProduct(const Ordering& ordering) const;
│ │ │ │ -
182
│ │ │ │ -
189 DiscreteLookupDAG maxProduct(
│ │ │ │ -
190 OptionalOrderingType orderingType = boost::none) const;
│ │ │ │ -
191
│ │ │ │ -
198 DiscreteLookupDAG maxProduct(const Ordering& ordering) const;
│ │ │ │ -
199
│ │ │ │ -
206 DiscreteValues optimize(
│ │ │ │ -
207 OptionalOrderingType orderingType = boost::none) const;
│ │ │ │ -
208
│ │ │ │ -
215 DiscreteValues optimize(const Ordering& ordering) const;
│ │ │ │ -
216
│ │ │ │ -
219
│ │ │ │ -
227 std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
228 const DiscreteFactor::Names& names = {}) const;
│ │ │ │ -
229
│ │ │ │ -
237 std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
238 const DiscreteFactor::Names& names = {}) const;
│ │ │ │ -
239
│ │ │ │ -
243
│ │ │ │ -
244 using Base::error; // Expose error(const HybridValues&) method..
│ │ │ │ -
245
│ │ │ │ -
247}; // \ DiscreteFactorGraph
│ │ │ │ -
│ │ │ │ -
248
│ │ │ │ -
250template <>
│ │ │ │ -
251struct traits<DiscreteFactorGraph> : public Testable<DiscreteFactorGraph> {};
│ │ │ │ -
252
│ │ │ │ -
253} // namespace gtsam
│ │ │ │ -
FastSet.h
A thin wrapper around std::set that uses boost's fast_pool_allocator.
│ │ │ │ - │ │ │ │ - │ │ │ │ -
EliminateableFactorGraph.h
Variable elimination algorithms for factor graphs.
│ │ │ │ -
FactorGraph.h
Factor Graph Base Class.
│ │ │ │ -
Ordering.h
Variable ordering for the elimination algorithm.
│ │ │ │ -
gtsam::EliminateDiscrete
std::pair< DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr > EliminateDiscrete(const DiscreteFactorGraph &factors, const Ordering &frontalKeys)
Main elimination function for DiscreteFactorGraph.
Definition DiscreteFactorGraph.cpp:200
│ │ │ │ -
gtsam::EliminateForMPE
std::pair< DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr > EliminateForMPE(const DiscreteFactorGraph &factors, const Ordering &frontalKeys)
Alternate elimination function for that creates non-normalized lookup tables.
Definition DiscreteFactorGraph.cpp:116
│ │ │ │ +
159template <class BASIS, size_t P>
│ │ │ │ +
│ │ │ │ +
160class VectorComponentFactor
│ │ │ │ +
161 : public FunctorizedFactor<double, ParameterMatrix<P>> {
│ │ │ │ +
162 private:
│ │ │ │ +
163 using Base = FunctorizedFactor<double, ParameterMatrix<P>>;
│ │ │ │ +
164
│ │ │ │ +
165 public:
│ │ │ │ +
166 VectorComponentFactor() {}
│ │ │ │ +
167
│ │ │ │ +
│ │ │ │ +
181 VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model,
│ │ │ │ +
182 const size_t N, size_t i, double x)
│ │ │ │ +
183 : Base(key, z, model,
│ │ │ │ +
184 typename BASIS::template VectorComponentFunctor<P>(N, i, x)) {}
│ │ │ │ +
│ │ │ │ +
185
│ │ │ │ +
│ │ │ │ +
201 VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model,
│ │ │ │ +
202 const size_t N, size_t i, double x, double a, double b)
│ │ │ │ +
203 : Base(
│ │ │ │ +
204 key, z, model,
│ │ │ │ +
205 typename BASIS::template VectorComponentFunctor<P>(N, i, x, a, b)) {
│ │ │ │ +
206 }
│ │ │ │ +
│ │ │ │ +
207
│ │ │ │ +
208 virtual ~VectorComponentFactor() {}
│ │ │ │ +
209};
│ │ │ │ +
│ │ │ │ +
210
│ │ │ │ +
228template <class BASIS, typename T>
│ │ │ │ +
│ │ │ │ +
229class ManifoldEvaluationFactor
│ │ │ │ +
230 : public FunctorizedFactor<T, ParameterMatrix<traits<T>::dimension>> {
│ │ │ │ +
231 private:
│ │ │ │ +
232 using Base = FunctorizedFactor<T, ParameterMatrix<traits<T>::dimension>>;
│ │ │ │ +
233
│ │ │ │ +
234 public:
│ │ │ │ +
235 ManifoldEvaluationFactor() {}
│ │ │ │ +
236
│ │ │ │ +
│ │ │ │ +
247 ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model,
│ │ │ │ +
248 const size_t N, double x)
│ │ │ │ +
249 : Base(key, z, model,
│ │ │ │ +
250 typename BASIS::template ManifoldEvaluationFunctor<T>(N, x)) {}
│ │ │ │ +
│ │ │ │ +
251
│ │ │ │ +
│ │ │ │ +
264 ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model,
│ │ │ │ +
265 const size_t N, double x, double a, double b)
│ │ │ │ +
266 : Base(
│ │ │ │ +
267 key, z, model,
│ │ │ │ +
268 typename BASIS::template ManifoldEvaluationFunctor<T>(N, x, a, b)) {
│ │ │ │ +
269 }
│ │ │ │ +
│ │ │ │ +
270
│ │ │ │ +
271 virtual ~ManifoldEvaluationFactor() {}
│ │ │ │ +
272};
│ │ │ │ +
│ │ │ │ +
273
│ │ │ │ +
280template <class BASIS>
│ │ │ │ +
│ │ │ │ +
281class DerivativeFactor
│ │ │ │ +
282 : public FunctorizedFactor<double, typename BASIS::Parameters> {
│ │ │ │ +
283 private:
│ │ │ │ +
284 using Base = FunctorizedFactor<double, typename BASIS::Parameters>;
│ │ │ │ +
285
│ │ │ │ +
286 public:
│ │ │ │ +
287 DerivativeFactor() {}
│ │ │ │ +
288
│ │ │ │ +
│ │ │ │ +
299 DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model,
│ │ │ │ +
300 const size_t N, double x)
│ │ │ │ +
301 : Base(key, z, model, typename BASIS::DerivativeFunctor(N, x)) {}
│ │ │ │ +
│ │ │ │ +
302
│ │ │ │ +
│ │ │ │ +
315 DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model,
│ │ │ │ +
316 const size_t N, double x, double a, double b)
│ │ │ │ +
317 : Base(key, z, model, typename BASIS::DerivativeFunctor(N, x, a, b)) {}
│ │ │ │ +
│ │ │ │ +
318
│ │ │ │ +
319 virtual ~DerivativeFactor() {}
│ │ │ │ +
320};
│ │ │ │ +
│ │ │ │ +
321
│ │ │ │ +
329template <class BASIS, int M>
│ │ │ │ +
│ │ │ │ +
330class VectorDerivativeFactor
│ │ │ │ +
331 : public FunctorizedFactor<Vector, ParameterMatrix<M>> {
│ │ │ │ +
332 private:
│ │ │ │ +
333 using Base = FunctorizedFactor<Vector, ParameterMatrix<M>>;
│ │ │ │ +
334 using Func = typename BASIS::template VectorDerivativeFunctor<M>;
│ │ │ │ +
335
│ │ │ │ +
336 public:
│ │ │ │ +
337 VectorDerivativeFactor() {}
│ │ │ │ +
338
│ │ │ │ +
│ │ │ │ +
349 VectorDerivativeFactor(Key key, const Vector &z,
│ │ │ │ +
350 const SharedNoiseModel &model, const size_t N,
│ │ │ │ +
351 double x)
│ │ │ │ +
352 : Base(key, z, model, Func(N, x)) {}
│ │ │ │ +
│ │ │ │ +
353
│ │ │ │ +
│ │ │ │ +
366 VectorDerivativeFactor(Key key, const Vector &z,
│ │ │ │ +
367 const SharedNoiseModel &model, const size_t N,
│ │ │ │ +
368 double x, double a, double b)
│ │ │ │ +
369 : Base(key, z, model, Func(N, x, a, b)) {}
│ │ │ │ +
│ │ │ │ +
370
│ │ │ │ +
371 virtual ~VectorDerivativeFactor() {}
│ │ │ │ +
372};
│ │ │ │ +
│ │ │ │ +
373
│ │ │ │ +
382template <class BASIS, int P>
│ │ │ │ +
│ │ │ │ +
383class ComponentDerivativeFactor
│ │ │ │ +
384 : public FunctorizedFactor<double, ParameterMatrix<P>> {
│ │ │ │ +
385 private:
│ │ │ │ +
386 using Base = FunctorizedFactor<double, ParameterMatrix<P>>;
│ │ │ │ +
387 using Func = typename BASIS::template ComponentDerivativeFunctor<P>;
│ │ │ │ +
388
│ │ │ │ +
389 public:
│ │ │ │ +
390 ComponentDerivativeFactor() {}
│ │ │ │ +
391
│ │ │ │ +
│ │ │ │ +
405 ComponentDerivativeFactor(Key key, const double &z,
│ │ │ │ +
406 const SharedNoiseModel &model, const size_t N,
│ │ │ │ +
407 size_t i, double x)
│ │ │ │ +
408 : Base(key, z, model, Func(N, i, x)) {}
│ │ │ │ +
│ │ │ │ +
409
│ │ │ │ +
│ │ │ │ +
425 ComponentDerivativeFactor(Key key, const double &z,
│ │ │ │ +
426 const SharedNoiseModel &model, const size_t N,
│ │ │ │ +
427 size_t i, double x, double a, double b)
│ │ │ │ +
428 : Base(key, z, model, Func(N, i, x, a, b)) {}
│ │ │ │ +
│ │ │ │ +
429
│ │ │ │ +
430 virtual ~ComponentDerivativeFactor() {}
│ │ │ │ +
431};
│ │ │ │ +
│ │ │ │ +
432
│ │ │ │ +
433} // namespace gtsam
│ │ │ │ +
Basis.h
Compute an interpolating basis.
│ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::markdown
string markdown(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
Free version of markdown.
Definition DiscreteValues.cpp:129
│ │ │ │ -
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ - │ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::DecisionTreeFactor
A discrete probabilistic factor.
Definition DecisionTreeFactor.h:45
│ │ │ │ -
gtsam::DiscreteBayesNet
A Bayes net made from discrete conditional distributions.
Definition DiscreteBayesNet.h:38
│ │ │ │ -
gtsam::DiscreteBayesTree
A Bayes tree representing a Discrete density.
Definition DiscreteBayesTree.h:75
│ │ │ │ -
gtsam::DiscreteConditional
Discrete Conditional Density Derives from DecisionTreeFactor.
Definition DiscreteConditional.h:40
│ │ │ │ -
gtsam::DiscreteEliminationTree
Elimination tree for discrete factors.
Definition DiscreteEliminationTree.h:33
│ │ │ │ -
gtsam::DiscreteFactor
Base class for discrete probabilistic factors The most general one is the derived DecisionTreeFactor.
Definition DiscreteFactor.h:38
│ │ │ │ -
gtsam::DiscreteFactor::Names
DiscreteValues::Names Names
Translation table from values to strings.
Definition DiscreteFactor.h:106
│ │ │ │ -
gtsam::EliminationTraits< DiscreteFactorGraph >::FactorGraphType
DiscreteFactorGraph FactorGraphType
Type of the factor graph (e.g. DiscreteFactorGraph)
Definition DiscreteFactorGraph.h:72
│ │ │ │ -
gtsam::EliminationTraits< DiscreteFactorGraph >::JunctionTreeType
DiscreteJunctionTree JunctionTreeType
Type of Junction tree.
Definition DiscreteFactorGraph.h:77
│ │ │ │ -
gtsam::EliminationTraits< DiscreteFactorGraph >::FactorType
DiscreteFactor FactorType
Type of factors in factor graph.
Definition DiscreteFactorGraph.h:71
│ │ │ │ -
gtsam::EliminationTraits< DiscreteFactorGraph >::DefaultEliminate
static std::pair< boost::shared_ptr< ConditionalType >, boost::shared_ptr< FactorType > > DefaultEliminate(const FactorGraphType &factors, const Ordering &keys)
The default dense elimination function.
Definition DiscreteFactorGraph.h:82
│ │ │ │ -
gtsam::EliminationTraits< DiscreteFactorGraph >::BayesTreeType
DiscreteBayesTree BayesTreeType
Type of Bayes tree.
Definition DiscreteFactorGraph.h:76
│ │ │ │ -
gtsam::EliminationTraits< DiscreteFactorGraph >::BayesNetType
DiscreteBayesNet BayesNetType
Type of Bayes net from sequential elimination.
Definition DiscreteFactorGraph.h:74
│ │ │ │ -
gtsam::EliminationTraits< DiscreteFactorGraph >::ConditionalType
DiscreteConditional ConditionalType
Type of conditionals from elimination.
Definition DiscreteFactorGraph.h:73
│ │ │ │ -
gtsam::EliminationTraits< DiscreteFactorGraph >::DefaultOrderingFunc
static Ordering DefaultOrderingFunc(const FactorGraphType &graph, boost::optional< const VariableIndex & > variableIndex)
The default ordering generation function.
Definition DiscreteFactorGraph.h:87
│ │ │ │ -
gtsam::EliminationTraits< DiscreteFactorGraph >::EliminationTreeType
DiscreteEliminationTree EliminationTreeType
Type of elimination tree.
Definition DiscreteFactorGraph.h:75
│ │ │ │ -
gtsam::DiscreteFactorGraph
A Discrete Factor Graph is a factor graph where all factors are Discrete, i.e.
Definition DiscreteFactorGraph.h:101
│ │ │ │ -
gtsam::DiscreteFactorGraph::DiscreteFactorGraph
DiscreteFactorGraph(const CONTAINER &factors)
Construct from container of factors (shared_ptr or plain objects)
Definition DiscreteFactorGraph.h:123
│ │ │ │ -
gtsam::DiscreteFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to This
Definition DiscreteFactorGraph.h:107
│ │ │ │ -
gtsam::DiscreteFactorGraph::DiscreteFactorGraph
DiscreteFactorGraph()
‍map from keys to values
Definition DiscreteFactorGraph.h:114
│ │ │ │ -
gtsam::DiscreteFactorGraph::DiscreteFactorGraph
DiscreteFactorGraph(const FactorGraph< DERIVED_FACTOR > &graph)
Implicit copy/downcast constructor to override explicit template container constructor.
Definition DiscreteFactorGraph.h:128
│ │ │ │ -
gtsam::DiscreteFactorGraph::DiscreteFactorGraph
DiscreteFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
Construct from iterator over factors.
Definition DiscreteFactorGraph.h:118
│ │ │ │ -
gtsam::DiscreteFactorGraph::~DiscreteFactorGraph
virtual ~DiscreteFactorGraph()
Destructor.
Definition DiscreteFactorGraph.h:131
│ │ │ │ -
gtsam::DiscreteFactorGraph::BaseEliminateable
EliminateableFactorGraph< This > BaseEliminateable
for elimination
Definition DiscreteFactorGraph.h:106
│ │ │ │ -
gtsam::DiscreteFactorGraph::add
void add(Args &&... args)
Add a decision-tree factor.
Definition DiscreteFactorGraph.h:142
│ │ │ │ -
gtsam::DiscreteJunctionTree
An EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree,...
Definition DiscreteJunctionTree.h:52
│ │ │ │ -
gtsam::DiscreteKeys
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition DiscreteKey.h:39
│ │ │ │ -
gtsam::DiscreteLookupDAG
A DAG made from lookup tables, as defined above.
Definition DiscreteLookupDAG.h:77
│ │ │ │ -
gtsam::DiscreteValues
A map from keys to values.
Definition DiscreteValues.h:34
│ │ │ │ -
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ -
gtsam::EliminationTraits
Traits class for eliminateable factor graphs, specifies the types that result from elimination,...
Definition EliminateableFactorGraph.h:36
│ │ │ │ -
gtsam::EliminateableFactorGraph
EliminateableFactorGraph is a base class for factor graphs that contains elimination algorithms.
Definition EliminateableFactorGraph.h:57
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ -
gtsam::Ordering::Colamd
static Ordering Colamd(const FACTOR_GRAPH &graph)
Compute a fill-reducing ordering using COLAMD from a factor graph (see details for note on performanc...
Definition Ordering.h:95
│ │ │ │ +
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::EvaluationFactor
Factor for enforcing the scalar value of the polynomial BASIS representation at x is the same as the ...
Definition BasisFactors.h:39
│ │ │ │ +
gtsam::EvaluationFactor::EvaluationFactor
EvaluationFactor(Key key, double z, const SharedNoiseModel &model, const size_t N, double x)
Construct a new EvaluationFactor object.
Definition BasisFactors.h:55
│ │ │ │ +
gtsam::EvaluationFactor::EvaluationFactor
EvaluationFactor(Key key, double z, const SharedNoiseModel &model, const size_t N, double x, double a, double b)
Construct a new EvaluationFactor object.
Definition BasisFactors.h:70
│ │ │ │ +
gtsam::VectorEvaluationFactor
Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix of size (M,...
Definition BasisFactors.h:96
│ │ │ │ +
gtsam::VectorEvaluationFactor::VectorEvaluationFactor
VectorEvaluationFactor(Key key, const Vector &z, const SharedNoiseModel &model, const size_t N, double x, double a, double b)
Construct a new VectorEvaluationFactor object.
Definition BasisFactors.h:131
│ │ │ │ +
gtsam::VectorEvaluationFactor::VectorEvaluationFactor
VectorEvaluationFactor(Key key, const Vector &z, const SharedNoiseModel &model, const size_t N, double x)
Construct a new VectorEvaluationFactor object.
Definition BasisFactors.h:113
│ │ │ │ +
gtsam::VectorComponentFactor
Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix of size (P,...
Definition BasisFactors.h:161
│ │ │ │ +
gtsam::VectorComponentFactor::VectorComponentFactor
VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model, const size_t N, size_t i, double x, double a, double b)
Construct a new VectorComponentFactor object.
Definition BasisFactors.h:201
│ │ │ │ +
gtsam::VectorComponentFactor::VectorComponentFactor
VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model, const size_t N, size_t i, double x)
Construct a new VectorComponentFactor object.
Definition BasisFactors.h:181
│ │ │ │ +
gtsam::ManifoldEvaluationFactor
For a measurement value of type T i.e.
Definition BasisFactors.h:230
│ │ │ │ +
gtsam::ManifoldEvaluationFactor::ManifoldEvaluationFactor
ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model, const size_t N, double x, double a, double b)
Construct a new ManifoldEvaluationFactor object.
Definition BasisFactors.h:264
│ │ │ │ +
gtsam::ManifoldEvaluationFactor::ManifoldEvaluationFactor
ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model, const size_t N, double x)
Construct a new ManifoldEvaluationFactor object.
Definition BasisFactors.h:247
│ │ │ │ +
gtsam::DerivativeFactor
A unary factor which enforces the evaluation of the derivative of a BASIS polynomial at a specified p...
Definition BasisFactors.h:282
│ │ │ │ +
gtsam::DerivativeFactor::DerivativeFactor
DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model, const size_t N, double x, double a, double b)
Construct a new DerivativeFactor object.
Definition BasisFactors.h:315
│ │ │ │ +
gtsam::DerivativeFactor::DerivativeFactor
DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model, const size_t N, double x)
Construct a new DerivativeFactor object.
Definition BasisFactors.h:299
│ │ │ │ +
gtsam::VectorDerivativeFactor
A unary factor which enforces the evaluation of the derivative of a BASIS polynomial at a specified p...
Definition BasisFactors.h:331
│ │ │ │ +
gtsam::VectorDerivativeFactor::VectorDerivativeFactor
VectorDerivativeFactor(Key key, const Vector &z, const SharedNoiseModel &model, const size_t N, double x)
Construct a new VectorDerivativeFactor object.
Definition BasisFactors.h:349
│ │ │ │ +
gtsam::VectorDerivativeFactor::VectorDerivativeFactor
VectorDerivativeFactor(Key key, const Vector &z, const SharedNoiseModel &model, const size_t N, double x, double a, double b)
Construct a new VectorDerivativeFactor object.
Definition BasisFactors.h:366
│ │ │ │ +
gtsam::ComponentDerivativeFactor
A unary factor which enforces the evaluation of the derivative of a BASIS polynomial is equal to the ...
Definition BasisFactors.h:384
│ │ │ │ +
gtsam::ComponentDerivativeFactor::ComponentDerivativeFactor
ComponentDerivativeFactor(Key key, const double &z, const SharedNoiseModel &model, const size_t N, size_t i, double x, double a, double b)
Construct a new ComponentDerivativeFactor object.
Definition BasisFactors.h:425
│ │ │ │ +
gtsam::ComponentDerivativeFactor::ComponentDerivativeFactor
ComponentDerivativeFactor(Key key, const double &z, const SharedNoiseModel &model, const size_t N, size_t i, double x)
Construct a new ComponentDerivativeFactor object.
Definition BasisFactors.h:405
│ │ │ │ +
gtsam::FunctorizedFactor
Factor which evaluates provided unary functor and uses the result to compute error with respect to th...
Definition FunctorizedFactor.h:59
│ │ │ │ +
gtsam::NoiseModelFactorN::key
Key key() const
Returns a key.
Definition NonlinearFactor.h:518
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── encoding │ │ │ │ │ @@ -1 +1 @@ │ │ │ │ │ -utf-8 │ │ │ │ │ +us-ascii │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DiscreteFactorGraph.h │ │ │ │ │ +BasisFactors.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ @@ -16,337 +16,297 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 19#pragma once │ │ │ │ │ 20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_O_r_d_e_r_i_n_g_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_S_e_t_._h> │ │ │ │ │ -27 │ │ │ │ │ -28#include │ │ │ │ │ -29#include │ │ │ │ │ -30#include │ │ │ │ │ -31#include │ │ │ │ │ -32 │ │ │ │ │ -33namespace _g_t_s_a_m { │ │ │ │ │ -34 │ │ │ │ │ -35// Forward declarations │ │ │ │ │ -36class DiscreteFactorGraph; │ │ │ │ │ -37class DiscreteConditional; │ │ │ │ │ -38class DiscreteBayesNet; │ │ │ │ │ -39class DiscreteEliminationTree; │ │ │ │ │ -40class DiscreteBayesTree; │ │ │ │ │ -41class DiscreteJunctionTree; │ │ │ │ │ +21#include <_g_t_s_a_m_/_b_a_s_i_s_/_B_a_s_i_s_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_._h> │ │ │ │ │ +23 │ │ │ │ │ +24namespace _g_t_s_a_m { │ │ │ │ │ +25 │ │ │ │ │ +38template │ │ │ │ │ +_3_9class _E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r : public _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r { │ │ │ │ │ +40 private: │ │ │ │ │ +41 using _B_a_s_e = _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_<_d_o_u_b_l_e_,_ _V_e_c_t_o_r_>; │ │ │ │ │ 42 │ │ │ │ │ -51GTSAM_EXPORT │ │ │ │ │ -52std::pair │ │ │ │ │ -53_E_l_i_m_i_n_a_t_e_D_i_s_c_r_e_t_e(const DiscreteFactorGraph& factors, │ │ │ │ │ -54 const Ordering& frontalKeys); │ │ │ │ │ -55 │ │ │ │ │ -64GTSAM_EXPORT │ │ │ │ │ -65std::pair │ │ │ │ │ -66_E_l_i_m_i_n_a_t_e_F_o_r_M_P_E(const DiscreteFactorGraph& factors, │ │ │ │ │ -67 const Ordering& frontalKeys); │ │ │ │ │ -68 │ │ │ │ │ -_6_9template<> struct _E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s<_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h> │ │ │ │ │ -70{ │ │ │ │ │ -_7_1 typedef _D_i_s_c_r_e_t_e_F_a_c_t_o_r _F_a_c_t_o_r_T_y_p_e; │ │ │ │ │ -_7_2 typedef _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e; │ │ │ │ │ -_7_3 typedef _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l _C_o_n_d_i_t_i_o_n_a_l_T_y_p_e; │ │ │ │ │ -_7_4 typedef _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t _B_a_y_e_s_N_e_t_T_y_p_e; │ │ │ │ │ -_7_5 typedef _D_i_s_c_r_e_t_e_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_T_y_p_e; │ │ │ │ │ -_7_6 typedef _D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e _B_a_y_e_s_T_r_e_e_T_y_p_e; │ │ │ │ │ -_7_7 typedef _D_i_s_c_r_e_t_e_J_u_n_c_t_i_o_n_T_r_e_e _J_u_n_c_t_i_o_n_T_r_e_e_T_y_p_e; │ │ │ │ │ -78 │ │ │ │ │ -80 static std::pair, │ │ │ │ │ -81 boost::shared_ptr > │ │ │ │ │ -_8_2 _D_e_f_a_u_l_t_E_l_i_m_i_n_a_t_e(const _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e& factors, const _O_r_d_e_r_i_n_g& keys) { │ │ │ │ │ -83 return _E_l_i_m_i_n_a_t_e_D_i_s_c_r_e_t_e(factors, keys); │ │ │ │ │ -84 } │ │ │ │ │ -85 │ │ │ │ │ -_8_7 static _O_r_d_e_r_i_n_g _D_e_f_a_u_l_t_O_r_d_e_r_i_n_g_F_u_n_c( │ │ │ │ │ -88 const _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e& graph, │ │ │ │ │ -89 boost::optional variableIndex) { │ │ │ │ │ -90 return _O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d(*variableIndex); │ │ │ │ │ -91 } │ │ │ │ │ -92}; │ │ │ │ │ -93 │ │ │ │ │ -_9_9class GTSAM_EXPORT _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -100 : public _F_a_c_t_o_r_G_r_a_p_h, │ │ │ │ │ -101 public _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h { │ │ │ │ │ -102 public: │ │ │ │ │ -_1_0_3 using _T_h_i_s = _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h; │ │ │ │ │ -_1_0_4 using _B_a_s_e = _F_a_c_t_o_r_G_r_a_p_h_<_D_i_s_c_r_e_t_e_F_a_c_t_o_r_>; │ │ │ │ │ -_1_0_5 using _B_a_s_e_E_l_i_m_i_n_a_t_e_a_b_l_e = │ │ │ │ │ -106 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_T_h_i_s_>; │ │ │ │ │ -_1_0_7 using _s_h_a_r_e_d___p_t_r = boost::shared_ptr; │ │ │ │ │ -108 │ │ │ │ │ -_1_0_9 using _V_a_l_u_e_s = _D_i_s_c_r_e_t_e_V_a_l_u_e_s; │ │ │ │ │ -110 │ │ │ │ │ -111 using Indices = _K_e_y_V_e_c_t_o_r; │ │ │ │ │ -112 │ │ │ │ │ -_1_1_4 _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h() {} │ │ │ │ │ -115 │ │ │ │ │ -117 template │ │ │ │ │ -_1_1_8 _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h(ITERATOR firstFactor, ITERATOR lastFactor) │ │ │ │ │ -119 : _B_a_s_e(firstFactor, lastFactor) {} │ │ │ │ │ -120 │ │ │ │ │ -122 template │ │ │ │ │ -_1_2_3 explicit _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h(const CONTAINER& factors) : _B_a_s_e(factors) {} │ │ │ │ │ -124 │ │ │ │ │ -127 template │ │ │ │ │ -_1_2_8 _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h(const _F_a_c_t_o_r_G_r_a_p_h_<_D_E_R_I_V_E_D___F_A_C_T_O_R_>& graph) : _B_a_s_e(graph) │ │ │ │ │ -{} │ │ │ │ │ -129 │ │ │ │ │ -_1_3_1 virtual _~_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h() {} │ │ │ │ │ -132 │ │ │ │ │ -135 │ │ │ │ │ -136 bool _e_q_u_a_l_s(const This& fg, double tol = 1e-9) const; │ │ │ │ │ -137 │ │ │ │ │ +43 public: │ │ │ │ │ +44 _E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r() {} │ │ │ │ │ +45 │ │ │ │ │ +_5_5 _E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r(_K_e_y _k_e_y, double z, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, │ │ │ │ │ +56 const size_t N, double x) │ │ │ │ │ +57 : _B_a_s_e(_k_e_y, z, model, typename BASIS::EvaluationFunctor(N, x)) {} │ │ │ │ │ +58 │ │ │ │ │ +_7_0 _E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r(_K_e_y _k_e_y, double z, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, │ │ │ │ │ +71 const size_t N, double x, double a, double b) │ │ │ │ │ +72 : _B_a_s_e(_k_e_y, z, model, typename BASIS::EvaluationFunctor(N, x, a, b)) {} │ │ │ │ │ +73 │ │ │ │ │ +74 virtual _~_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r() {} │ │ │ │ │ +75}; │ │ │ │ │ +76 │ │ │ │ │ +94template │ │ │ │ │ +_9_5class _V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +96 : public _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r> { │ │ │ │ │ +97 private: │ │ │ │ │ +98 using _B_a_s_e = _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_<_V_e_c_t_o_r_,_ _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>>; │ │ │ │ │ +99 │ │ │ │ │ +100 public: │ │ │ │ │ +101 _V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r() {} │ │ │ │ │ +102 │ │ │ │ │ +_1_1_3 _V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r(_K_e_y _k_e_y, const Vector &z, │ │ │ │ │ +114 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, const size_t N, │ │ │ │ │ +115 double x) │ │ │ │ │ +116 : _B_a_s_e(_k_e_y, z, model, │ │ │ │ │ +117 typename BASIS::template VectorEvaluationFunctor(N, x)) {} │ │ │ │ │ +118 │ │ │ │ │ +_1_3_1 _V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r(_K_e_y _k_e_y, const Vector &z, │ │ │ │ │ +132 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, const size_t N, │ │ │ │ │ +133 double x, double a, double b) │ │ │ │ │ +134 : _B_a_s_e(_k_e_y, z, model, │ │ │ │ │ +135 typename BASIS::template VectorEvaluationFunctor(N, x, a, b)) {} │ │ │ │ │ +136 │ │ │ │ │ +137 virtual _~_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r() {} │ │ │ │ │ +138}; │ │ │ │ │ 139 │ │ │ │ │ -141 template │ │ │ │ │ -_1_4_2 void _a_d_d(Args&&... args) { │ │ │ │ │ -143 emplace_shared(std::forward(args)...); │ │ │ │ │ -144 } │ │ │ │ │ -145 │ │ │ │ │ -147 _K_e_y_S_e_t keys() const; │ │ │ │ │ -148 │ │ │ │ │ -150 _D_i_s_c_r_e_t_e_K_e_y_s discreteKeys() const; │ │ │ │ │ -151 │ │ │ │ │ -153 _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r product() const; │ │ │ │ │ -154 │ │ │ │ │ -159 double operator()(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values) const; │ │ │ │ │ -160 │ │ │ │ │ -162 void _p_r_i_n_t( │ │ │ │ │ -163 const std::string& s = "DiscreteFactorGraph", │ │ │ │ │ -164 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override; │ │ │ │ │ -165 │ │ │ │ │ -172 _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t sumProduct( │ │ │ │ │ -173 OptionalOrderingType orderingType = boost::none) const; │ │ │ │ │ -174 │ │ │ │ │ -181 _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t sumProduct(const _O_r_d_e_r_i_n_g& ordering) const; │ │ │ │ │ -182 │ │ │ │ │ -189 _D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G maxProduct( │ │ │ │ │ -190 OptionalOrderingType orderingType = boost::none) const; │ │ │ │ │ -191 │ │ │ │ │ -198 _D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G maxProduct(const _O_r_d_e_r_i_n_g& ordering) const; │ │ │ │ │ -199 │ │ │ │ │ -206 _D_i_s_c_r_e_t_e_V_a_l_u_e_s _o_p_t_i_m_i_z_e( │ │ │ │ │ -207 OptionalOrderingType orderingType = boost::none) const; │ │ │ │ │ -208 │ │ │ │ │ -215 _D_i_s_c_r_e_t_e_V_a_l_u_e_s _o_p_t_i_m_i_z_e(const _O_r_d_e_r_i_n_g& ordering) const; │ │ │ │ │ -216 │ │ │ │ │ -219 │ │ │ │ │ -227 std::string _m_a_r_k_d_o_w_n(const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ -DefaultKeyFormatter, │ │ │ │ │ -228 const _D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_N_a_m_e_s& names = {}) const; │ │ │ │ │ -229 │ │ │ │ │ -237 std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ -238 const DiscreteFactor::Names& names = {}) const; │ │ │ │ │ -239 │ │ │ │ │ -243 │ │ │ │ │ -244 using Base::error; // Expose error(const HybridValues&) method.. │ │ │ │ │ -245 │ │ │ │ │ -247}; // \ DiscreteFactorGraph │ │ │ │ │ -248 │ │ │ │ │ -250template <> │ │ │ │ │ -_2_5_1struct _t_r_a_i_t_s<_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h> : public _T_e_s_t_a_b_l_e │ │ │ │ │ -{}; │ │ │ │ │ -252 │ │ │ │ │ -253} // namespace gtsam │ │ │ │ │ -_F_a_s_t_S_e_t_._h │ │ │ │ │ -A thin wrapper around std::set that uses boost's fast_pool_allocator. │ │ │ │ │ -_D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G_._h │ │ │ │ │ -_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_._h │ │ │ │ │ -_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Variable elimination algorithms for factor graphs. │ │ │ │ │ -_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Factor Graph Base Class. │ │ │ │ │ -_O_r_d_e_r_i_n_g_._h │ │ │ │ │ -Variable ordering for the elimination algorithm. │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_D_i_s_c_r_e_t_e │ │ │ │ │ -std::pair< DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr > │ │ │ │ │ -EliminateDiscrete(const DiscreteFactorGraph &factors, const Ordering │ │ │ │ │ -&frontalKeys) │ │ │ │ │ -Main elimination function for DiscreteFactorGraph. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactorGraph.cpp:200 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_F_o_r_M_P_E │ │ │ │ │ -std::pair< DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr > │ │ │ │ │ -EliminateForMPE(const DiscreteFactorGraph &factors, const Ordering │ │ │ │ │ -&frontalKeys) │ │ │ │ │ -Alternate elimination function for that creates non-normalized lookup tables. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactorGraph.cpp:116 │ │ │ │ │ +159template │ │ │ │ │ +_1_6_0class _V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_a_c_t_o_r │ │ │ │ │ +161 : public _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r> { │ │ │ │ │ +162 private: │ │ │ │ │ +163 using _B_a_s_e = _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_<_d_o_u_b_l_e_,_ _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_P_>>; │ │ │ │ │ +164 │ │ │ │ │ +165 public: │ │ │ │ │ +166 _V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_a_c_t_o_r() {} │ │ │ │ │ +167 │ │ │ │ │ +_1_8_1 _V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_a_c_t_o_r(_K_e_y _k_e_y, const double &z, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +&model, │ │ │ │ │ +182 const size_t N, size_t i, double x) │ │ │ │ │ +183 : _B_a_s_e(_k_e_y, z, model, │ │ │ │ │ +184 typename BASIS::template VectorComponentFunctor

(N, i, x)) {} │ │ │ │ │ +185 │ │ │ │ │ +_2_0_1 _V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_a_c_t_o_r(_K_e_y _k_e_y, const double &z, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +&model, │ │ │ │ │ +202 const size_t N, size_t i, double x, double a, double b) │ │ │ │ │ +203 : _B_a_s_e( │ │ │ │ │ +204 _k_e_y, z, model, │ │ │ │ │ +205 typename BASIS::template VectorComponentFunctor

(N, i, x, a, b)) { │ │ │ │ │ +206 } │ │ │ │ │ +207 │ │ │ │ │ +208 virtual _~_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_a_c_t_o_r() {} │ │ │ │ │ +209}; │ │ │ │ │ +210 │ │ │ │ │ +228template │ │ │ │ │ +_2_2_9class _M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +230 : public _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r::dimension>> { │ │ │ │ │ +231 private: │ │ │ │ │ +232 using _B_a_s_e = _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_<_T_,_ _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_t_r_a_i_t_s_<_T_>_:_:_d_i_m_e_n_s_i_o_n>>; │ │ │ │ │ +233 │ │ │ │ │ +234 public: │ │ │ │ │ +235 _M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r() {} │ │ │ │ │ +236 │ │ │ │ │ +_2_4_7 _M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r(_K_e_y _k_e_y, const T &z, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +&model, │ │ │ │ │ +248 const size_t N, double x) │ │ │ │ │ +249 : _B_a_s_e(_k_e_y, z, model, │ │ │ │ │ +250 typename BASIS::template ManifoldEvaluationFunctor(N, x)) {} │ │ │ │ │ +251 │ │ │ │ │ +_2_6_4 _M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r(_K_e_y _k_e_y, const T &z, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +&model, │ │ │ │ │ +265 const size_t N, double x, double a, double b) │ │ │ │ │ +266 : _B_a_s_e( │ │ │ │ │ +267 _k_e_y, z, model, │ │ │ │ │ +268 typename BASIS::template ManifoldEvaluationFunctor(N, x, a, b)) { │ │ │ │ │ +269 } │ │ │ │ │ +270 │ │ │ │ │ +271 virtual _~_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r() {} │ │ │ │ │ +272}; │ │ │ │ │ +273 │ │ │ │ │ +280template │ │ │ │ │ +_2_8_1class _D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r │ │ │ │ │ +282 : public _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r { │ │ │ │ │ +283 private: │ │ │ │ │ +284 using Base = _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_<_d_o_u_b_l_e_,_ _t_y_p_e_n_a_m_e_ _B_A_S_I_S_:_:_P_a_r_a_m_e_t_e_r_s_>; │ │ │ │ │ +285 │ │ │ │ │ +286 public: │ │ │ │ │ +287 _D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r() {} │ │ │ │ │ +288 │ │ │ │ │ +_2_9_9 _D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r(_K_e_y _k_e_y, const double &z, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, │ │ │ │ │ +300 const size_t N, double x) │ │ │ │ │ +301 : Base(_k_e_y, z, model, typename BASIS::DerivativeFunctor(N, x)) {} │ │ │ │ │ +302 │ │ │ │ │ +_3_1_5 _D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r(_K_e_y _k_e_y, const double &z, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, │ │ │ │ │ +316 const size_t N, double x, double a, double b) │ │ │ │ │ +317 : Base(_k_e_y, z, model, typename BASIS::DerivativeFunctor(N, x, a, b)) {} │ │ │ │ │ +318 │ │ │ │ │ +319 virtual _~_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r() {} │ │ │ │ │ +320}; │ │ │ │ │ +321 │ │ │ │ │ +329template │ │ │ │ │ +_3_3_0class _V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r │ │ │ │ │ +331 : public _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r> { │ │ │ │ │ +332 private: │ │ │ │ │ +333 using _B_a_s_e = _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_<_V_e_c_t_o_r_,_ _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>>; │ │ │ │ │ +334 using Func = typename BASIS::template VectorDerivativeFunctor; │ │ │ │ │ +335 │ │ │ │ │ +336 public: │ │ │ │ │ +337 _V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r() {} │ │ │ │ │ +338 │ │ │ │ │ +_3_4_9 _V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r(_K_e_y _k_e_y, const Vector &z, │ │ │ │ │ +350 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, const size_t N, │ │ │ │ │ +351 double x) │ │ │ │ │ +352 : _B_a_s_e(_k_e_y, z, model, Func(N, x)) {} │ │ │ │ │ +353 │ │ │ │ │ +_3_6_6 _V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r(_K_e_y _k_e_y, const Vector &z, │ │ │ │ │ +367 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, const size_t N, │ │ │ │ │ +368 double x, double a, double b) │ │ │ │ │ +369 : _B_a_s_e(_k_e_y, z, model, Func(N, x, a, b)) {} │ │ │ │ │ +370 │ │ │ │ │ +371 virtual _~_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r() {} │ │ │ │ │ +372}; │ │ │ │ │ +373 │ │ │ │ │ +382template │ │ │ │ │ +_3_8_3class _C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r │ │ │ │ │ +384 : public _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r> { │ │ │ │ │ +385 private: │ │ │ │ │ +386 using _B_a_s_e = _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_<_d_o_u_b_l_e_,_ _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_P_>>; │ │ │ │ │ +387 using Func = typename BASIS::template ComponentDerivativeFunctor

; │ │ │ │ │ +388 │ │ │ │ │ +389 public: │ │ │ │ │ +390 _C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r() {} │ │ │ │ │ +391 │ │ │ │ │ +_4_0_5 _C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r(_K_e_y _k_e_y, const double &z, │ │ │ │ │ +406 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, const size_t N, │ │ │ │ │ +407 size_t i, double x) │ │ │ │ │ +408 : _B_a_s_e(_k_e_y, z, model, Func(N, i, x)) {} │ │ │ │ │ +409 │ │ │ │ │ +_4_2_5 _C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r(_K_e_y _k_e_y, const double &z, │ │ │ │ │ +426 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, const size_t N, │ │ │ │ │ +427 size_t i, double x, double a, double b) │ │ │ │ │ +428 : _B_a_s_e(_k_e_y, z, model, Func(N, i, x, a, b)) {} │ │ │ │ │ +429 │ │ │ │ │ +430 virtual _~_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r() {} │ │ │ │ │ +431}; │ │ │ │ │ +432 │ │ │ │ │ +433} // namespace gtsam │ │ │ │ │ +_B_a_s_i_s_._h │ │ │ │ │ +Compute an interpolating basis. │ │ │ │ │ +_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_m_a_r_k_d_o_w_n │ │ │ │ │ -string markdown(const DiscreteValues &values, const KeyFormatter &keyFormatter, │ │ │ │ │ -const DiscreteValues::Names &names) │ │ │ │ │ -Free version of markdown. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteValues.cpp:129 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ -Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key │ │ │ │ │ -landmarkKey) │ │ │ │ │ -Optimize for triangulation. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.cpp:155 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r │ │ │ │ │ -A discrete probabilistic factor. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTreeFactor.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t │ │ │ │ │ -A Bayes net made from discrete conditional distributions. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteBayesNet.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e │ │ │ │ │ -A Bayes tree representing a Discrete density. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteBayesTree.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -Discrete Conditional Density Derives from DecisionTreeFactor. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteConditional.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ -Elimination tree for discrete factors. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteEliminationTree.h:33 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r │ │ │ │ │ -Base class for discrete probabilistic factors The most general one is the │ │ │ │ │ -derived DecisionTreeFactor. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactor.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_N_a_m_e_s │ │ │ │ │ -DiscreteValues::Names Names │ │ │ │ │ -Translation table from values to strings. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactor.h:106 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_F_a_c_t_o_r_G_r_a_p_h_T_y_p_e │ │ │ │ │ -DiscreteFactorGraph FactorGraphType │ │ │ │ │ -Type of the factor graph (e.g. DiscreteFactorGraph) │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactorGraph.h:72 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_J_u_n_c_t_i_o_n_T_r_e_e_T_y_p_e │ │ │ │ │ -DiscreteJunctionTree JunctionTreeType │ │ │ │ │ -Type of Junction tree. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactorGraph.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_F_a_c_t_o_r_T_y_p_e │ │ │ │ │ -DiscreteFactor FactorType │ │ │ │ │ -Type of factors in factor graph. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactorGraph.h:71 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_D_e_f_a_u_l_t_E_l_i_m_i_n_a_t_e │ │ │ │ │ -static std::pair< boost::shared_ptr< ConditionalType >, boost::shared_ptr< │ │ │ │ │ -FactorType > > DefaultEliminate(const FactorGraphType &factors, const Ordering │ │ │ │ │ -&keys) │ │ │ │ │ -The default dense elimination function. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactorGraph.h:82 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_B_a_y_e_s_T_r_e_e_T_y_p_e │ │ │ │ │ -DiscreteBayesTree BayesTreeType │ │ │ │ │ -Type of Bayes tree. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactorGraph.h:76 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_B_a_y_e_s_N_e_t_T_y_p_e │ │ │ │ │ -DiscreteBayesNet BayesNetType │ │ │ │ │ -Type of Bayes net from sequential elimination. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactorGraph.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_C_o_n_d_i_t_i_o_n_a_l_T_y_p_e │ │ │ │ │ -DiscreteConditional ConditionalType │ │ │ │ │ -Type of conditionals from elimination. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactorGraph.h:73 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_D_e_f_a_u_l_t_O_r_d_e_r_i_n_g_F_u_n_c │ │ │ │ │ -static Ordering DefaultOrderingFunc(const FactorGraphType &graph, boost:: │ │ │ │ │ -optional< const VariableIndex & > variableIndex) │ │ │ │ │ -The default ordering generation function. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactorGraph.h:87 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_T_y_p_e │ │ │ │ │ -DiscreteEliminationTree EliminationTreeType │ │ │ │ │ -Type of elimination tree. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactorGraph.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A Discrete Factor Graph is a factor graph where all factors are Discrete, i.e. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactorGraph.h:101 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -DiscreteFactorGraph(const CONTAINER &factors) │ │ │ │ │ -Construct from container of factors (shared_ptr or plain objects) │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactorGraph.h:123 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to This │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactorGraph.h:107 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -DiscreteFactorGraph() │ │ │ │ │ -‍map from keys to values │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactorGraph.h:114 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -DiscreteFactorGraph(const FactorGraph< DERIVED_FACTOR > &graph) │ │ │ │ │ -Implicit copy/downcast constructor to override explicit template container │ │ │ │ │ -constructor. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactorGraph.h:128 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -DiscreteFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) │ │ │ │ │ -Construct from iterator over factors. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactorGraph.h:118 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_:_:_~_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -virtual ~DiscreteFactorGraph() │ │ │ │ │ -Destructor. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactorGraph.h:131 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_:_:_B_a_s_e_E_l_i_m_i_n_a_t_e_a_b_l_e │ │ │ │ │ -EliminateableFactorGraph< This > BaseEliminateable │ │ │ │ │ -for elimination │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactorGraph.h:106 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d │ │ │ │ │ -void add(Args &&... args) │ │ │ │ │ -Add a decision-tree factor. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactorGraph.h:142 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ -An EliminatableClusterTree, i.e., a set of variable clusters with factors, │ │ │ │ │ -arranged in a tree,... │ │ │ │ │ -DDeeffiinniittiioonn DiscreteJunctionTree.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ -DiscreteKeys is a set of keys that can be assembled using the & operator. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteKey.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G │ │ │ │ │ -A DAG made from lookup tables, as defined above. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteLookupDAG.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ -A map from keys to values. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteValues.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ -nodes. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s │ │ │ │ │ -Traits class for eliminateable factor graphs, specifies the types that result │ │ │ │ │ -from elimination,... │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -EliminateableFactorGraph is a base class for factor graphs that contains │ │ │ │ │ -elimination algorithms. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d │ │ │ │ │ -static Ordering Colamd(const FACTOR_GRAPH &graph) │ │ │ │ │ -Compute a fill-reducing ordering using COLAMD from a factor graph (see details │ │ │ │ │ -for note on performanc... │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:95 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ +Aliases. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +Factor for enforcing the scalar value of the polynomial BASIS representation at │ │ │ │ │ +x is the same as the ... │ │ │ │ │ +DDeeffiinniittiioonn BasisFactors.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r_:_:_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +EvaluationFactor(Key key, double z, const SharedNoiseModel &model, const size_t │ │ │ │ │ +N, double x) │ │ │ │ │ +Construct a new EvaluationFactor object. │ │ │ │ │ +DDeeffiinniittiioonn BasisFactors.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r_:_:_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +EvaluationFactor(Key key, double z, const SharedNoiseModel &model, const size_t │ │ │ │ │ +N, double x, double a, double b) │ │ │ │ │ +Construct a new EvaluationFactor object. │ │ │ │ │ +DDeeffiinniittiioonn BasisFactors.h:70 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix of │ │ │ │ │ +size (M,... │ │ │ │ │ +DDeeffiinniittiioonn BasisFactors.h:96 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +VectorEvaluationFactor(Key key, const Vector &z, const SharedNoiseModel &model, │ │ │ │ │ +const size_t N, double x, double a, double b) │ │ │ │ │ +Construct a new VectorEvaluationFactor object. │ │ │ │ │ +DDeeffiinniittiioonn BasisFactors.h:131 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +VectorEvaluationFactor(Key key, const Vector &z, const SharedNoiseModel &model, │ │ │ │ │ +const size_t N, double x) │ │ │ │ │ +Construct a new VectorEvaluationFactor object. │ │ │ │ │ +DDeeffiinniittiioonn BasisFactors.h:113 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_a_c_t_o_r │ │ │ │ │ +Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix of │ │ │ │ │ +size (P,... │ │ │ │ │ +DDeeffiinniittiioonn BasisFactors.h:161 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_a_c_t_o_r_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_a_c_t_o_r │ │ │ │ │ +VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model, │ │ │ │ │ +const size_t N, size_t i, double x, double a, double b) │ │ │ │ │ +Construct a new VectorComponentFactor object. │ │ │ │ │ +DDeeffiinniittiioonn BasisFactors.h:201 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_a_c_t_o_r_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_a_c_t_o_r │ │ │ │ │ +VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model, │ │ │ │ │ +const size_t N, size_t i, double x) │ │ │ │ │ +Construct a new VectorComponentFactor object. │ │ │ │ │ +DDeeffiinniittiioonn BasisFactors.h:181 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +For a measurement value of type T i.e. │ │ │ │ │ +DDeeffiinniittiioonn BasisFactors.h:230 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model, │ │ │ │ │ +const size_t N, double x, double a, double b) │ │ │ │ │ +Construct a new ManifoldEvaluationFactor object. │ │ │ │ │ +DDeeffiinniittiioonn BasisFactors.h:264 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model, │ │ │ │ │ +const size_t N, double x) │ │ │ │ │ +Construct a new ManifoldEvaluationFactor object. │ │ │ │ │ +DDeeffiinniittiioonn BasisFactors.h:247 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r │ │ │ │ │ +A unary factor which enforces the evaluation of the derivative of a BASIS │ │ │ │ │ +polynomial at a specified p... │ │ │ │ │ +DDeeffiinniittiioonn BasisFactors.h:282 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r_:_:_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r │ │ │ │ │ +DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model, const │ │ │ │ │ +size_t N, double x, double a, double b) │ │ │ │ │ +Construct a new DerivativeFactor object. │ │ │ │ │ +DDeeffiinniittiioonn BasisFactors.h:315 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r_:_:_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r │ │ │ │ │ +DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model, const │ │ │ │ │ +size_t N, double x) │ │ │ │ │ +Construct a new DerivativeFactor object. │ │ │ │ │ +DDeeffiinniittiioonn BasisFactors.h:299 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r │ │ │ │ │ +A unary factor which enforces the evaluation of the derivative of a BASIS │ │ │ │ │ +polynomial at a specified p... │ │ │ │ │ +DDeeffiinniittiioonn BasisFactors.h:331 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r │ │ │ │ │ +VectorDerivativeFactor(Key key, const Vector &z, const SharedNoiseModel &model, │ │ │ │ │ +const size_t N, double x) │ │ │ │ │ +Construct a new VectorDerivativeFactor object. │ │ │ │ │ +DDeeffiinniittiioonn BasisFactors.h:349 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r │ │ │ │ │ +VectorDerivativeFactor(Key key, const Vector &z, const SharedNoiseModel &model, │ │ │ │ │ +const size_t N, double x, double a, double b) │ │ │ │ │ +Construct a new VectorDerivativeFactor object. │ │ │ │ │ +DDeeffiinniittiioonn BasisFactors.h:366 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r │ │ │ │ │ +A unary factor which enforces the evaluation of the derivative of a BASIS │ │ │ │ │ +polynomial is equal to the ... │ │ │ │ │ +DDeeffiinniittiioonn BasisFactors.h:384 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r │ │ │ │ │ +ComponentDerivativeFactor(Key key, const double &z, const SharedNoiseModel │ │ │ │ │ +&model, const size_t N, size_t i, double x, double a, double b) │ │ │ │ │ +Construct a new ComponentDerivativeFactor object. │ │ │ │ │ +DDeeffiinniittiioonn BasisFactors.h:425 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_a_c_t_o_r │ │ │ │ │ +ComponentDerivativeFactor(Key key, const double &z, const SharedNoiseModel │ │ │ │ │ +&model, const size_t N, size_t i, double x) │ │ │ │ │ +Construct a new ComponentDerivativeFactor object. │ │ │ │ │ +DDeeffiinniittiioonn BasisFactors.h:405 │ │ │ │ │ +_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r │ │ │ │ │ +Factor which evaluates provided unary functor and uses the result to compute │ │ │ │ │ +error with respect to th... │ │ │ │ │ +DDeeffiinniittiioonn FunctorizedFactor.h:59 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_:_:_k_e_y │ │ │ │ │ +Key key() const │ │ │ │ │ +Returns a key. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:518 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ + * _b_a_s_i_s │ │ │ │ │ + * _B_a_s_i_s_F_a_c_t_o_r_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00239_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00245_source.html │ │ │ │┄ Files 94% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteKey.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteLookupDAG.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │

│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
DiscreteKey.h
│ │ │ │ +
DiscreteLookupDAG.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/global_includes.h>
│ │ │ │ -
22#include <gtsam/inference/Key.h>
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/discrete/DiscreteDistribution.h>
│ │ │ │ +
21#include <gtsam/inference/BayesNet.h>
│ │ │ │ +
22#include <gtsam/inference/FactorGraph.h>
│ │ │ │
23
│ │ │ │ -
24#include <boost/serialization/vector.hpp>
│ │ │ │ -
25#include <map>
│ │ │ │ -
26#include <string>
│ │ │ │ +
24#include <boost/shared_ptr.hpp>
│ │ │ │ +
25#include <string>
│ │ │ │ +
26#include <utility>
│ │ │ │
27#include <vector>
│ │ │ │
28
│ │ │ │
29namespace gtsam {
│ │ │ │
30
│ │ │ │ -
36 using DiscreteKey = std::pair<Key,size_t>;
│ │ │ │ -
37
│ │ │ │ -
│ │ │ │ -
39 struct GTSAM_EXPORT DiscreteKeys: public std::vector<DiscreteKey> {
│ │ │ │ -
40
│ │ │ │ -
41 // Forward all constructors.
│ │ │ │ -
42 using std::vector<DiscreteKey>::vector;
│ │ │ │ -
43
│ │ │ │ -
45 DiscreteKeys() : std::vector<DiscreteKey>::vector() {}
│ │ │ │ -
46
│ │ │ │ -
48 explicit DiscreteKeys(const DiscreteKey& key) { push_back(key); }
│ │ │ │ -
49
│ │ │ │ -
│ │ │ │ -
51 explicit DiscreteKeys(std::map<Key, size_t> cardinalities) {
│ │ │ │ -
52 for (auto&& kv : cardinalities) emplace_back(kv);
│ │ │ │ -
53 }
│ │ │ │ -
│ │ │ │ -
54
│ │ │ │ -
│ │ │ │ -
56 DiscreteKeys(const std::vector<DiscreteKey>& keys) :
│ │ │ │ -
57 std::vector<DiscreteKey>(keys) {
│ │ │ │ -
58 }
│ │ │ │ -
│ │ │ │ -
59
│ │ │ │ -
61 DiscreteKeys(const std::vector<int>& cs);
│ │ │ │ -
62
│ │ │ │ -
64 KeyVector indices() const;
│ │ │ │ -
65
│ │ │ │ -
67 std::map<Key,size_t> cardinalities() const;
│ │ │ │ +
31class DiscreteBayesNet;
│ │ │ │ +
32
│ │ │ │ +
│ │ │ │ +
40class GTSAM_EXPORT DiscreteLookupTable : public DiscreteConditional {
│ │ │ │ +
41 public:
│ │ │ │ +
42 using This = DiscreteLookupTable;
│ │ │ │ +
43 using shared_ptr = boost::shared_ptr<This>;
│ │ │ │ +
44 using BaseConditional = Conditional<DecisionTreeFactor, This>;
│ │ │ │ +
45
│ │ │ │ +
│ │ │ │ +
53 DiscreteLookupTable(size_t nFrontals, const DiscreteKeys& keys,
│ │ │ │ +
54 const ADT& potentials)
│ │ │ │ +
55 : DiscreteConditional(nFrontals, keys, potentials) {}
│ │ │ │ +
│ │ │ │ +
56
│ │ │ │ +
58 void print(
│ │ │ │ +
59 const std::string& s = "Discrete Lookup Table: ",
│ │ │ │ +
60 const KeyFormatter& formatter = DefaultKeyFormatter) const override;
│ │ │ │ +
61
│ │ │ │ +
67 size_t argmax(const DiscreteValues& parentsValues) const;
│ │ │ │
68
│ │ │ │ -
│ │ │ │ -
70 DiscreteKeys& operator&(const DiscreteKey& key) {
│ │ │ │ -
71 push_back(key);
│ │ │ │ -
72 return *this;
│ │ │ │ -
73 }
│ │ │ │ -
│ │ │ │ -
74
│ │ │ │ -
76 void print(const std::string& s = "",
│ │ │ │ -
77 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ -
78
│ │ │ │ -
80 bool equals(const DiscreteKeys& other, double tol = 0) const;
│ │ │ │ -
81
│ │ │ │ -
83 friend class boost::serialization::access;
│ │ │ │ -
84 template <class ARCHIVE>
│ │ │ │ -
85 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ -
86 ar& boost::serialization::make_nvp(
│ │ │ │ -
87 "DiscreteKeys",
│ │ │ │ -
88 boost::serialization::base_object<std::vector<DiscreteKey>>(*this));
│ │ │ │ -
89 }
│ │ │ │ -
90
│ │ │ │ -
91 }; // DiscreteKeys
│ │ │ │ -
│ │ │ │ -
92
│ │ │ │ -
94 GTSAM_EXPORT DiscreteKeys operator&(const DiscreteKey& key1, const DiscreteKey& key2);
│ │ │ │ -
95
│ │ │ │ -
96 // traits
│ │ │ │ -
97 template <>
│ │ │ │ -
98 struct traits<DiscreteKeys> : public Testable<DiscreteKeys> {};
│ │ │ │ +
73 void argmaxInPlace(DiscreteValues* parentsValues) const;
│ │ │ │ +
74};
│ │ │ │ +
│ │ │ │ +
75
│ │ │ │ +
│ │ │ │ +
77class GTSAM_EXPORT DiscreteLookupDAG : public BayesNet<DiscreteLookupTable> {
│ │ │ │ +
78 public:
│ │ │ │ +
79 using Base = BayesNet<DiscreteLookupTable>;
│ │ │ │ +
80 using This = DiscreteLookupDAG;
│ │ │ │ +
81 using shared_ptr = boost::shared_ptr<This>;
│ │ │ │ +
82
│ │ │ │ +
85
│ │ │ │ +
87 DiscreteLookupDAG() {}
│ │ │ │ +
88
│ │ │ │ +
90 static DiscreteLookupDAG FromBayesNet(const DiscreteBayesNet& bayesNet);
│ │ │ │ +
91
│ │ │ │ +
93 virtual ~DiscreteLookupDAG() {}
│ │ │ │ +
94
│ │ │ │ +
96
│ │ │ │
99
│ │ │ │ -
100 } // namespace gtsam
│ │ │ │ - │ │ │ │ -
global_includes.h
Included from all GTSAM files.
│ │ │ │ -
gtsam::DiscreteKey
std::pair< Key, size_t > DiscreteKey
Key type for discrete variables.
Definition DiscreteKey.h:36
│ │ │ │ +
101 bool equals(const This& bn, double tol = 1e-9) const;
│ │ │ │ +
102
│ │ │ │ +
104
│ │ │ │ +
107
│ │ │ │ +
109 template <typename... Args>
│ │ │ │ +
│ │ │ │ +
110 void add(Args&&... args) {
│ │ │ │ +
111 emplace_shared<DiscreteLookupTable>(std::forward<Args>(args)...);
│ │ │ │ +
112 }
│ │ │ │ +
│ │ │ │ +
113
│ │ │ │ +
125 DiscreteValues argmax(DiscreteValues given = DiscreteValues()) const;
│ │ │ │ +
127
│ │ │ │ +
128 private:
│ │ │ │ +
130 friend class boost::serialization::access;
│ │ │ │ +
131 template <class ARCHIVE>
│ │ │ │ +
132 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ +
133 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
134 }
│ │ │ │ +
135};
│ │ │ │ +
│ │ │ │ +
136
│ │ │ │ +
137// traits
│ │ │ │ +
138template <>
│ │ │ │ +
139struct traits<DiscreteLookupDAG> : public Testable<DiscreteLookupDAG> {};
│ │ │ │ +
140
│ │ │ │ +
141} // namespace gtsam
│ │ │ │ + │ │ │ │ +
FactorGraph.h
Factor Graph Base Class.
│ │ │ │ +
BayesNet.h
Bayes network.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::operator&
DiscreteKeys operator&(const DiscreteKey &key1, const DiscreteKey &key2)
Create a list from two keys.
Definition DiscreteKey.cpp:46
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ + │ │ │ │ +
gtsam::DiscreteBayesNet
A Bayes net made from discrete conditional distributions.
Definition DiscreteBayesNet.h:38
│ │ │ │ +
gtsam::DiscreteConditional
Discrete Conditional Density Derives from DecisionTreeFactor.
Definition DiscreteConditional.h:40
│ │ │ │
gtsam::DiscreteKeys
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition DiscreteKey.h:39
│ │ │ │ -
gtsam::DiscreteKeys::operator&
DiscreteKeys & operator&(const DiscreteKey &key)
Add a key (non-const!)
Definition DiscreteKey.h:70
│ │ │ │ -
gtsam::DiscreteKeys::DiscreteKeys
DiscreteKeys()
Constructor for serialization.
Definition DiscreteKey.h:45
│ │ │ │ -
gtsam::DiscreteKeys::DiscreteKeys
DiscreteKeys(const DiscreteKey &key)
Construct from a key.
Definition DiscreteKey.h:48
│ │ │ │ -
gtsam::DiscreteKeys::DiscreteKeys
DiscreteKeys(const std::vector< DiscreteKey > &keys)
Construct from a vector of keys.
Definition DiscreteKey.h:56
│ │ │ │ -
gtsam::DiscreteKeys::DiscreteKeys
DiscreteKeys(std::map< Key, size_t > cardinalities)
Construct from cardinalities.
Definition DiscreteKey.h:51
│ │ │ │ +
gtsam::DiscreteLookupTable
DiscreteLookupTable table for max-product.
Definition DiscreteLookupDAG.h:40
│ │ │ │ +
gtsam::DiscreteLookupTable::DiscreteLookupTable
DiscreteLookupTable(size_t nFrontals, const DiscreteKeys &keys, const ADT &potentials)
Construct a new Discrete Lookup Table object.
Definition DiscreteLookupDAG.h:53
│ │ │ │ +
gtsam::DiscreteLookupDAG
A DAG made from lookup tables, as defined above.
Definition DiscreteLookupDAG.h:77
│ │ │ │ +
gtsam::DiscreteLookupDAG::equals
bool equals(const This &bn, double tol=1e-9) const
Check equality.
│ │ │ │ +
gtsam::DiscreteLookupDAG::DiscreteLookupDAG
DiscreteLookupDAG()
Construct empty DAG.
Definition DiscreteLookupDAG.h:87
│ │ │ │ +
gtsam::DiscreteLookupDAG::add
void add(Args &&... args)
Add a DiscreteLookupTable.
Definition DiscreteLookupDAG.h:110
│ │ │ │ +
gtsam::DiscreteLookupDAG::~DiscreteLookupDAG
virtual ~DiscreteLookupDAG()
Destructor.
Definition DiscreteLookupDAG.h:93
│ │ │ │ +
gtsam::DiscreteValues
A map from keys to values.
Definition DiscreteValues.h:34
│ │ │ │ +
gtsam::BayesNet
A BayesNet is a tree of conditionals, stored in elimination order.
Definition BayesNet.h:35
│ │ │ │ +
gtsam::Conditional
Definition Conditional.h:64
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,148 +1,164 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DiscreteKey.h │ │ │ │ │ +DiscreteLookupDAG.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ 23 │ │ │ │ │ -24#include │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ +24#include │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ 27#include │ │ │ │ │ 28 │ │ │ │ │ 29namespace _g_t_s_a_m { │ │ │ │ │ 30 │ │ │ │ │ -_3_6 using _D_i_s_c_r_e_t_e_K_e_y = std::pair; │ │ │ │ │ -37 │ │ │ │ │ -_3_9 struct GTSAM_EXPORT _D_i_s_c_r_e_t_e_K_e_y_s: public std::vector { │ │ │ │ │ -40 │ │ │ │ │ -41 // Forward all constructors. │ │ │ │ │ -42 using std::vector<_D_i_s_c_r_e_t_e_K_e_y>::vector; │ │ │ │ │ -43 │ │ │ │ │ -_4_5 _D_i_s_c_r_e_t_e_K_e_y_s() : std::vector<_D_i_s_c_r_e_t_e_K_e_y>::vector() {} │ │ │ │ │ -46 │ │ │ │ │ -_4_8 explicit _D_i_s_c_r_e_t_e_K_e_y_s(const _D_i_s_c_r_e_t_e_K_e_y& key) { push_back(key); } │ │ │ │ │ -49 │ │ │ │ │ -_5_1 explicit _D_i_s_c_r_e_t_e_K_e_y_s(std::map cardinalities) { │ │ │ │ │ -52 for (auto&& kv : cardinalities) emplace_back(kv); │ │ │ │ │ -53 } │ │ │ │ │ -54 │ │ │ │ │ -_5_6 _D_i_s_c_r_e_t_e_K_e_y_s(const std::vector& keys) : │ │ │ │ │ -57 std::vector<_D_i_s_c_r_e_t_e_K_e_y>(keys) { │ │ │ │ │ -58 } │ │ │ │ │ -59 │ │ │ │ │ -61 _D_i_s_c_r_e_t_e_K_e_y_s(const std::vector& cs); │ │ │ │ │ -62 │ │ │ │ │ -64 _K_e_y_V_e_c_t_o_r indices() const; │ │ │ │ │ -65 │ │ │ │ │ -67 std::map cardinalities() const; │ │ │ │ │ +31class DiscreteBayesNet; │ │ │ │ │ +32 │ │ │ │ │ +_4_0class GTSAM_EXPORT _D_i_s_c_r_e_t_e_L_o_o_k_u_p_T_a_b_l_e : public _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l { │ │ │ │ │ +41 public: │ │ │ │ │ +42 using _T_h_i_s = _D_i_s_c_r_e_t_e_L_o_o_k_u_p_T_a_b_l_e; │ │ │ │ │ +43 using shared_ptr = boost::shared_ptr; │ │ │ │ │ +44 using BaseConditional = _C_o_n_d_i_t_i_o_n_a_l_<_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_,_ _T_h_i_s_>; │ │ │ │ │ +45 │ │ │ │ │ +_5_3 _D_i_s_c_r_e_t_e_L_o_o_k_u_p_T_a_b_l_e(size_t nFrontals, const _D_i_s_c_r_e_t_e_K_e_y_s& keys, │ │ │ │ │ +54 const _A_D_T& potentials) │ │ │ │ │ +55 : _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l(nFrontals, keys, potentials) {} │ │ │ │ │ +56 │ │ │ │ │ +58 void _p_r_i_n_t( │ │ │ │ │ +59 const std::string& s = "Discrete Lookup Table: ", │ │ │ │ │ +60 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override; │ │ │ │ │ +61 │ │ │ │ │ +67 size_t argmax(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& parentsValues) const; │ │ │ │ │ 68 │ │ │ │ │ -_7_0 _D_i_s_c_r_e_t_e_K_e_y_s& _o_p_e_r_a_t_o_r_&(const _D_i_s_c_r_e_t_e_K_e_y& key) { │ │ │ │ │ -71 push_back(key); │ │ │ │ │ -72 return *this; │ │ │ │ │ -73 } │ │ │ │ │ -74 │ │ │ │ │ -76 void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ -77 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const; │ │ │ │ │ -78 │ │ │ │ │ -80 bool _e_q_u_a_l_s(const _D_i_s_c_r_e_t_e_K_e_y_s& other, double tol = 0) const; │ │ │ │ │ -81 │ │ │ │ │ -_8_3 friend class boost::serialization::access; │ │ │ │ │ -84 template │ │ │ │ │ -85 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ -86 ar& boost::serialization::make_nvp( │ │ │ │ │ -87 "DiscreteKeys", │ │ │ │ │ -88 boost::serialization::base_object>(*this)); │ │ │ │ │ -89 } │ │ │ │ │ -90 │ │ │ │ │ -91 }; // DiscreteKeys │ │ │ │ │ -92 │ │ │ │ │ -94 GTSAM_EXPORT DiscreteKeys _o_p_e_r_a_t_o_r_&(const _D_i_s_c_r_e_t_e_K_e_y& key1, const │ │ │ │ │ -_D_i_s_c_r_e_t_e_K_e_y& key2); │ │ │ │ │ -95 │ │ │ │ │ -96 // traits │ │ │ │ │ -97 template <> │ │ │ │ │ -_9_8 struct _t_r_a_i_t_s<_D_i_s_c_r_e_t_e_K_e_y_s> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ +73 void argmaxInPlace(_D_i_s_c_r_e_t_e_V_a_l_u_e_s* parentsValues) const; │ │ │ │ │ +74}; │ │ │ │ │ +75 │ │ │ │ │ +_7_7class GTSAM_EXPORT _D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G : public _B_a_y_e_s_N_e_t { │ │ │ │ │ +78 public: │ │ │ │ │ +79 using _B_a_s_e = _B_a_y_e_s_N_e_t_<_D_i_s_c_r_e_t_e_L_o_o_k_u_p_T_a_b_l_e_>; │ │ │ │ │ +80 using _T_h_i_s = _D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G; │ │ │ │ │ +81 using shared_ptr = boost::shared_ptr; │ │ │ │ │ +82 │ │ │ │ │ +85 │ │ │ │ │ +_8_7 _D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G() {} │ │ │ │ │ +88 │ │ │ │ │ +90 static _D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G FromBayesNet(const _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t& bayesNet); │ │ │ │ │ +91 │ │ │ │ │ +_9_3 virtual _~_D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G() {} │ │ │ │ │ +94 │ │ │ │ │ +96 │ │ │ │ │ 99 │ │ │ │ │ -100 } // namespace gtsam │ │ │ │ │ -_K_e_y_._h │ │ │ │ │ -_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ -Included from all GTSAM files. │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y │ │ │ │ │ -std::pair< Key, size_t > DiscreteKey │ │ │ │ │ -Key type for discrete variables. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteKey.h:36 │ │ │ │ │ +_1_0_1 bool _e_q_u_a_l_s(const _T_h_i_s& bn, double tol = 1e-9) const; │ │ │ │ │ +102 │ │ │ │ │ +104 │ │ │ │ │ +107 │ │ │ │ │ +109 template │ │ │ │ │ +_1_1_0 void _a_d_d(Args&&... args) { │ │ │ │ │ +111 emplace_shared(std::forward(args)...); │ │ │ │ │ +112 } │ │ │ │ │ +113 │ │ │ │ │ +125 _D_i_s_c_r_e_t_e_V_a_l_u_e_s argmax(_D_i_s_c_r_e_t_e_V_a_l_u_e_s given = _D_i_s_c_r_e_t_e_V_a_l_u_e_s()) const; │ │ │ │ │ +127 │ │ │ │ │ +128 private: │ │ │ │ │ +_1_3_0 friend class boost::serialization::access; │ │ │ │ │ +131 template │ │ │ │ │ +132 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ +133 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +134 } │ │ │ │ │ +135}; │ │ │ │ │ +136 │ │ │ │ │ +137// traits │ │ │ │ │ +138template <> │ │ │ │ │ +_1_3_9struct _t_r_a_i_t_s<_D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ +140 │ │ │ │ │ +141} // namespace gtsam │ │ │ │ │ +_D_i_s_c_r_e_t_e_D_i_s_t_r_i_b_u_t_i_o_n_._h │ │ │ │ │ +_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Factor Graph Base Class. │ │ │ │ │ +_B_a_y_e_s_N_e_t_._h │ │ │ │ │ +Bayes network. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_& │ │ │ │ │ -DiscreteKeys operator&(const DiscreteKey &key1, const DiscreteKey &key2) │ │ │ │ │ -Create a list from two keys. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteKey.cpp:46 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ _g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ print without optional string, must specify cout yourself │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ _g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ A helper that implements the traits interface for GTSAM types. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_<_ _K_e_y_ _> │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t │ │ │ │ │ +A Bayes net made from discrete conditional distributions. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteBayesNet.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +Discrete Conditional Density Derives from DecisionTreeFactor. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteConditional.h:40 │ │ │ │ │ _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ DiscreteKeys is a set of keys that can be assembled using the & operator. │ │ │ │ │ DDeeffiinniittiioonn DiscreteKey.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s_:_:_o_p_e_r_a_t_o_r_& │ │ │ │ │ -DiscreteKeys & operator&(const DiscreteKey &key) │ │ │ │ │ -Add a key (non-const!) │ │ │ │ │ -DDeeffiinniittiioonn DiscreteKey.h:70 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ -DiscreteKeys() │ │ │ │ │ -Constructor for serialization. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteKey.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ -DiscreteKeys(const DiscreteKey &key) │ │ │ │ │ -Construct from a key. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteKey.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ -DiscreteKeys(const std::vector< DiscreteKey > &keys) │ │ │ │ │ -Construct from a vector of keys. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteKey.h:56 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ -DiscreteKeys(std::map< Key, size_t > cardinalities) │ │ │ │ │ -Construct from cardinalities. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteKey.h:51 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_L_o_o_k_u_p_T_a_b_l_e │ │ │ │ │ +DiscreteLookupTable table for max-product. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteLookupDAG.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_L_o_o_k_u_p_T_a_b_l_e_:_:_D_i_s_c_r_e_t_e_L_o_o_k_u_p_T_a_b_l_e │ │ │ │ │ +DiscreteLookupTable(size_t nFrontals, const DiscreteKeys &keys, const ADT │ │ │ │ │ +&potentials) │ │ │ │ │ +Construct a new Discrete Lookup Table object. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteLookupDAG.h:53 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G │ │ │ │ │ +A DAG made from lookup tables, as defined above. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteLookupDAG.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const This &bn, double tol=1e-9) const │ │ │ │ │ +Check equality. │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G_:_:_D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G │ │ │ │ │ +DiscreteLookupDAG() │ │ │ │ │ +Construct empty DAG. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteLookupDAG.h:87 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G_:_:_a_d_d │ │ │ │ │ +void add(Args &&... args) │ │ │ │ │ +Add a DiscreteLookupTable. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteLookupDAG.h:110 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G_:_:_~_D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G │ │ │ │ │ +virtual ~DiscreteLookupDAG() │ │ │ │ │ +Destructor. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteLookupDAG.h:93 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ +A map from keys to values. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteValues.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t │ │ │ │ │ +A BayesNet is a tree of conditionals, stored in elimination order. │ │ │ │ │ +DDeeffiinniittiioonn BayesNet.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:64 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_K_e_y_._h │ │ │ │ │ + * _D_i_s_c_r_e_t_e_L_o_o_k_u_p_D_A_G_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00251_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01145_source.html │ │ │ │┄ Files 94% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DecisionTreeFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearFactorGraph.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
DecisionTreeFactor.h
│ │ │ │ +
NonlinearFactorGraph.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/discrete/AlgebraicDecisionTree.h>
│ │ │ │ -
22#include <gtsam/discrete/DiscreteFactor.h>
│ │ │ │ -
23#include <gtsam/discrete/DiscreteKey.h>
│ │ │ │ -
24#include <gtsam/inference/Ordering.h>
│ │ │ │ -
25
│ │ │ │ -
26#include <algorithm>
│ │ │ │ -
27#include <boost/shared_ptr.hpp>
│ │ │ │ -
28#include <map>
│ │ │ │ -
29#include <stdexcept>
│ │ │ │ -
30#include <string>
│ │ │ │ -
31#include <utility>
│ │ │ │ -
32#include <vector>
│ │ │ │ -
33
│ │ │ │ -
34namespace gtsam {
│ │ │ │ -
35
│ │ │ │ -
36 class DiscreteConditional;
│ │ │ │ -
37 class HybridValues;
│ │ │ │ -
38
│ │ │ │ -
│ │ │ │ -
44 class GTSAM_EXPORT DecisionTreeFactor : public DiscreteFactor,
│ │ │ │ -
45 public AlgebraicDecisionTree<Key> {
│ │ │ │ -
46 public:
│ │ │ │ -
47 // typedefs needed to play nice with gtsam
│ │ │ │ -
48 typedef DecisionTreeFactor This;
│ │ │ │ -
49 typedef DiscreteFactor Base;
│ │ │ │ -
50 typedef boost::shared_ptr<DecisionTreeFactor> shared_ptr;
│ │ │ │ -
51 typedef AlgebraicDecisionTree<Key> ADT;
│ │ │ │ -
52
│ │ │ │ -
53 protected:
│ │ │ │ -
54 std::map<Key, size_t> cardinalities_;
│ │ │ │ -
55
│ │ │ │ -
56 public:
│ │ │ │ -
59
│ │ │ │ -
61 DecisionTreeFactor();
│ │ │ │ +
20// \callgraph
│ │ │ │ +
21
│ │ │ │ +
22#pragma once
│ │ │ │ +
23
│ │ │ │ +
24#include <gtsam/geometry/Point2.h>
│ │ │ │ +
25#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
26#include <gtsam/nonlinear/GraphvizFormatting.h>
│ │ │ │ +
27#include <gtsam/inference/FactorGraph.h>
│ │ │ │ +
28#include <gtsam/nonlinear/PriorFactor.h>
│ │ │ │ +
29
│ │ │ │ +
30#include <boost/shared_ptr.hpp>
│ │ │ │ +
31#include <functional>
│ │ │ │ +
32
│ │ │ │ +
33namespace gtsam {
│ │ │ │ +
34
│ │ │ │ +
35 // Forward declarations
│ │ │ │ +
36 class Values;
│ │ │ │ +
37 class Ordering;
│ │ │ │ +
38 class GaussianFactorGraph;
│ │ │ │ +
39 class SymbolicFactorGraph;
│ │ │ │ +
40 template<typename T>
│ │ │ │ +
41 class Expression;
│ │ │ │ +
42 template<typename T>
│ │ │ │ +
43 class ExpressionFactor;
│ │ │ │ +
44
│ │ │ │ +
│ │ │ │ +
55 class GTSAM_EXPORT NonlinearFactorGraph: public FactorGraph<NonlinearFactor> {
│ │ │ │ +
56
│ │ │ │ +
57 public:
│ │ │ │ +
58
│ │ │ │ +
59 typedef FactorGraph<NonlinearFactor> Base;
│ │ │ │ +
60 typedef NonlinearFactorGraph This;
│ │ │ │ +
61 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │
62
│ │ │ │ -
64 DecisionTreeFactor(const DiscreteKeys& keys, const ADT& potentials);
│ │ │ │
65
│ │ │ │ -
85 DecisionTreeFactor(const DiscreteKeys& keys,
│ │ │ │ -
86 const std::vector<double>& table);
│ │ │ │ +
67 NonlinearFactorGraph() {}
│ │ │ │ +
68
│ │ │ │ +
70 template<typename ITERATOR>
│ │ │ │ +
71 NonlinearFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {}
│ │ │ │ +
72
│ │ │ │ +
74 template<class CONTAINER>
│ │ │ │ +
75 explicit NonlinearFactorGraph(const CONTAINER& factors) : Base(factors) {}
│ │ │ │ +
76
│ │ │ │ +
78 template<class DERIVEDFACTOR>
│ │ │ │ +
79 NonlinearFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
│ │ │ │ +
80
│ │ │ │ +
82 virtual ~NonlinearFactorGraph() {}
│ │ │ │ +
83
│ │ │ │
87
│ │ │ │ -
106 DecisionTreeFactor(const DiscreteKeys& keys, const std::string& table);
│ │ │ │ -
107
│ │ │ │ -
109 template <class SOURCE>
│ │ │ │ -
│ │ │ │ -
110 DecisionTreeFactor(const DiscreteKey& key, SOURCE table)
│ │ │ │ -
111 : DecisionTreeFactor(DiscreteKeys{key}, table) {}
│ │ │ │ -
│ │ │ │ -
112
│ │ │ │ -
│ │ │ │ -
114 DecisionTreeFactor(const DiscreteKey& key, const std::vector<double>& row)
│ │ │ │ -
115 : DecisionTreeFactor(DiscreteKeys{key}, row) {}
│ │ │ │ -
│ │ │ │ +
89 void print(
│ │ │ │ +
90 const std::string& str = "NonlinearFactorGraph: ",
│ │ │ │ +
91 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
│ │ │ │ +
92
│ │ │ │ +
94 void printErrors(const Values& values, const std::string& str = "NonlinearFactorGraph: ",
│ │ │ │ +
95 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
96 const std::function<bool(const Factor* /*factor*/, double /*whitenedError*/, size_t /*index*/)>&
│ │ │ │ +
97 printCondition = [](const Factor *,double, size_t) {return true;}) const;
│ │ │ │ +
98
│ │ │ │ +
100 bool equals(const NonlinearFactorGraph& other, double tol = 1e-9) const;
│ │ │ │ +
101
│ │ │ │ +
105
│ │ │ │ +
107 double error(const Values& values) const;
│ │ │ │ +
108
│ │ │ │ +
110 double probPrime(const Values& values) const;
│ │ │ │ +
111
│ │ │ │ +
115 boost::shared_ptr<SymbolicFactorGraph> symbolic() const;
│ │ │ │
116
│ │ │ │ -
118 explicit DecisionTreeFactor(const DiscreteConditional& c);
│ │ │ │ -
119
│ │ │ │ -
123
│ │ │ │ -
125 bool equals(const DiscreteFactor& other, double tol = 1e-9) const override;
│ │ │ │ -
126
│ │ │ │ -
127 // print
│ │ │ │ -
128 void print(
│ │ │ │ -
129 const std::string& s = "DecisionTreeFactor:\n",
│ │ │ │ -
130 const KeyFormatter& formatter = DefaultKeyFormatter) const override;
│ │ │ │ +
120 Ordering orderingCOLAMD() const;
│ │ │ │ +
121
│ │ │ │ +
130 Ordering orderingCOLAMDConstrained(const FastMap<Key, int>& constraints) const;
│ │ │ │
131
│ │ │ │ -
135
│ │ │ │ -
│ │ │ │ -
138 double evaluate(const DiscreteValues& values) const {
│ │ │ │ -
139 return ADT::operator()(values);
│ │ │ │ -
140 }
│ │ │ │ -
│ │ │ │ -
141
│ │ │ │ -
│ │ │ │ -
143 double operator()(const DiscreteValues& values) const override {
│ │ │ │ -
144 return ADT::operator()(values);
│ │ │ │ -
145 }
│ │ │ │ -
│ │ │ │ -
146
│ │ │ │ -
148 double error(const DiscreteValues& values) const;
│ │ │ │ -
149
│ │ │ │ -
│ │ │ │ -
151 DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override {
│ │ │ │ -
152 return apply(f, ADT::Ring::mul);
│ │ │ │ -
153 }
│ │ │ │ -
│ │ │ │ -
154
│ │ │ │ -
155 static double safe_div(const double& a, const double& b);
│ │ │ │ -
156
│ │ │ │ -
157 size_t cardinality(Key j) const { return cardinalities_.at(j); }
│ │ │ │ +
133 boost::shared_ptr<GaussianFactorGraph> linearize(const Values& linearizationPoint) const;
│ │ │ │ +
134
│ │ │ │ +
136 typedef std::function<void(const boost::shared_ptr<HessianFactor>& hessianFactor)> Dampen;
│ │ │ │ +
137
│ │ │ │ +
145 boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
│ │ │ │ +
146 const Values& values, const Dampen& dampen = nullptr) const;
│ │ │ │ +
147
│ │ │ │ +
156 boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
│ │ │ │ +
157 const Values& values, const Ordering& ordering, const Dampen& dampen = nullptr) const;
│ │ │ │
158
│ │ │ │ -
│ │ │ │ -
160 DecisionTreeFactor operator/(const DecisionTreeFactor& f) const {
│ │ │ │ -
161 return apply(f, safe_div);
│ │ │ │ -
162 }
│ │ │ │ -
│ │ │ │ +
161 Values updateCholesky(const Values& values,
│ │ │ │ +
162 const Dampen& dampen = nullptr) const;
│ │ │ │
163
│ │ │ │ -
165 DecisionTreeFactor toDecisionTreeFactor() const override { return *this; }
│ │ │ │ -
166
│ │ │ │ -
│ │ │ │ -
168 shared_ptr sum(size_t nrFrontals) const {
│ │ │ │ -
169 return combine(nrFrontals, ADT::Ring::add);
│ │ │ │ -
170 }
│ │ │ │ -
│ │ │ │ +
166 Values updateCholesky(const Values& values, const Ordering& ordering,
│ │ │ │ +
167 const Dampen& dampen = nullptr) const;
│ │ │ │ +
168
│ │ │ │ +
170 NonlinearFactorGraph clone() const;
│ │ │ │
171
│ │ │ │ -
│ │ │ │ -
173 shared_ptr sum(const Ordering& keys) const {
│ │ │ │ -
174 return combine(keys, ADT::Ring::add);
│ │ │ │ -
175 }
│ │ │ │ -
│ │ │ │ -
176
│ │ │ │ -
│ │ │ │ -
178 shared_ptr max(size_t nrFrontals) const {
│ │ │ │ -
179 return combine(nrFrontals, ADT::Ring::max);
│ │ │ │ -
180 }
│ │ │ │ -
│ │ │ │ -
181
│ │ │ │ -
│ │ │ │ -
183 shared_ptr max(const Ordering& keys) const {
│ │ │ │ -
184 return combine(keys, ADT::Ring::max);
│ │ │ │ -
185 }
│ │ │ │ -
│ │ │ │ -
186
│ │ │ │ -
190
│ │ │ │ -
196 DecisionTreeFactor apply(const DecisionTreeFactor& f, ADT::Binary op) const;
│ │ │ │ -
197
│ │ │ │ -
204 shared_ptr combine(size_t nrFrontals, ADT::Binary op) const;
│ │ │ │ -
205
│ │ │ │ -
212 shared_ptr combine(const Ordering& keys, ADT::Binary op) const;
│ │ │ │ -
213
│ │ │ │ -
215 std::vector<std::pair<DiscreteValues, double>> enumerate() const;
│ │ │ │ -
216
│ │ │ │ -
218 DiscreteKeys discreteKeys() const;
│ │ │ │ -
219
│ │ │ │ -
238 DecisionTreeFactor prune(size_t maxNrAssignments) const;
│ │ │ │ +
181 NonlinearFactorGraph rekey(const std::map<Key,Key>& rekey_mapping) const;
│ │ │ │ +
182
│ │ │ │ +
189 template<typename T>
│ │ │ │ +
│ │ │ │ +
190 void addExpressionFactor(const SharedNoiseModel& R, const T& z,
│ │ │ │ +
191 const Expression<T>& h) {
│ │ │ │ +
192 push_back(boost::make_shared<ExpressionFactor<T> >(R, z, h));
│ │ │ │ +
193 }
│ │ │ │ +
│ │ │ │ +
194
│ │ │ │ +
201 template<typename T>
│ │ │ │ +
│ │ │ │ +
202 void addPrior(Key key, const T& prior,
│ │ │ │ +
203 const SharedNoiseModel& model = nullptr) {
│ │ │ │ +
204 emplace_shared<PriorFactor<T>>(key, prior, model);
│ │ │ │ +
205 }
│ │ │ │ +
│ │ │ │ +
206
│ │ │ │ +
217 template<typename T>
│ │ │ │ +
│ │ │ │ +
218 void addPrior(Key key, const T& prior, const Matrix& covariance) {
│ │ │ │ +
219 emplace_shared<PriorFactor<T>>(key, prior, covariance);
│ │ │ │ +
220 }
│ │ │ │ +
│ │ │ │ +
221
│ │ │ │ +
225
│ │ │ │ +
226 using FactorGraph::dot;
│ │ │ │ +
227 using FactorGraph::saveGraph;
│ │ │ │ +
228
│ │ │ │ +
230 void dot(std::ostream& os, const Values& values,
│ │ │ │ +
231 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
232 const GraphvizFormatting& writer = GraphvizFormatting()) const;
│ │ │ │ +
233
│ │ │ │ +
235 std::string dot(
│ │ │ │ +
236 const Values& values,
│ │ │ │ +
237 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
238 const GraphvizFormatting& writer = GraphvizFormatting()) const;
│ │ │ │
239
│ │ │ │ -
243
│ │ │ │ -
245 void dot(std::ostream& os,
│ │ │ │ -
246 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
247 bool showZero = true) const;
│ │ │ │ +
241 void saveGraph(
│ │ │ │ +
242 const std::string& filename, const Values& values,
│ │ │ │ +
243 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
244 const GraphvizFormatting& writer = GraphvizFormatting()) const;
│ │ │ │ +
246
│ │ │ │ +
247 private:
│ │ │ │
248
│ │ │ │ -
250 void dot(const std::string& name,
│ │ │ │ -
251 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
252 bool showZero = true) const;
│ │ │ │ -
253
│ │ │ │ -
255 std::string dot(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
256 bool showZero = true) const;
│ │ │ │ -
257
│ │ │ │ -
265 std::string markdown(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
266 const Names& names = {}) const override;
│ │ │ │ -
267
│ │ │ │ -
275 std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
276 const Names& names = {}) const override;
│ │ │ │ -
277
│ │ │ │ -
281
│ │ │ │ -
286 double error(const HybridValues& values) const override;
│ │ │ │ -
287
│ │ │ │ -
289
│ │ │ │ -
290 private:
│ │ │ │ -
292 friend class boost::serialization::access;
│ │ │ │ -
293 template <class ARCHIVE>
│ │ │ │ -
294 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ -
295 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
296 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(ADT);
│ │ │ │ -
297 ar& BOOST_SERIALIZATION_NVP(cardinalities_);
│ │ │ │ -
298 }
│ │ │ │ -
299 };
│ │ │ │ -
│ │ │ │ -
300
│ │ │ │ -
301// traits
│ │ │ │ -
302template <>
│ │ │ │ -
303struct traits<DecisionTreeFactor> : public Testable<DecisionTreeFactor> {};
│ │ │ │ +
253 boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
│ │ │ │ +
254 const Values& values, const Scatter& scatter, const Dampen& dampen = nullptr) const;
│ │ │ │ +
255
│ │ │ │ +
257 friend class boost::serialization::access;
│ │ │ │ +
258 template<class ARCHIVE>
│ │ │ │ +
259 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
260 ar & boost::serialization::make_nvp("NonlinearFactorGraph",
│ │ │ │ +
261 boost::serialization::base_object<Base>(*this));
│ │ │ │ +
262 }
│ │ │ │ +
263
│ │ │ │ +
264 public:
│ │ │ │ +
265
│ │ │ │ +
266#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ +
269
│ │ │ │ +
270 boost::shared_ptr<HessianFactor> GTSAM_DEPRECATED linearizeToHessianFactor(
│ │ │ │ +
271 const Values& values, boost::none_t, const Dampen& dampen = nullptr) const
│ │ │ │ +
272 {return linearizeToHessianFactor(values, dampen);}
│ │ │ │ +
273
│ │ │ │ +
275 Values GTSAM_DEPRECATED updateCholesky(const Values& values, boost::none_t,
│ │ │ │ +
276 const Dampen& dampen = nullptr) const
│ │ │ │ +
277 {return updateCholesky(values, dampen);}
│ │ │ │ +
278
│ │ │ │ +
280 void GTSAM_DEPRECATED saveGraph(
│ │ │ │ +
281 std::ostream& os, const Values& values = Values(),
│ │ │ │ +
282 const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(),
│ │ │ │ +
283 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
│ │ │ │ +
284 dot(os, values, keyFormatter, graphvizFormatting);
│ │ │ │ +
285 }
│ │ │ │ +
287 void GTSAM_DEPRECATED
│ │ │ │ +
288 saveGraph(const std::string& filename, const Values& values,
│ │ │ │ +
289 const GraphvizFormatting& graphvizFormatting,
│ │ │ │ +
290 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
│ │ │ │ +
291 saveGraph(filename, values, keyFormatter, graphvizFormatting);
│ │ │ │ +
292 }
│ │ │ │ +
294#endif
│ │ │ │ +
295
│ │ │ │ +
296 };
│ │ │ │ +
│ │ │ │ +
297
│ │ │ │ +
299template<>
│ │ │ │ +
│ │ │ │ +
300struct traits<NonlinearFactorGraph> : public Testable<NonlinearFactorGraph> {
│ │ │ │ +
301};
│ │ │ │ +
│ │ │ │ +
302
│ │ │ │ +
303} //\ namespace gtsam
│ │ │ │
304
│ │ │ │ -
305} // namespace gtsam
│ │ │ │ -
AlgebraicDecisionTree.h
Algebraic Decision Trees.
│ │ │ │ -
DiscreteKey.h
specialized key for discrete variables
│ │ │ │ - │ │ │ │ -
Ordering.h
Variable ordering for the elimination algorithm.
│ │ │ │ -
gtsam::DiscreteKey
std::pair< Key, size_t > DiscreteKey
Key type for discrete variables.
Definition DiscreteKey.h:36
│ │ │ │ +
Point2.h
2D Point
│ │ │ │ +
FactorGraph.h
Factor Graph Base Class.
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
GraphvizFormatting.h
Graphviz formatter for NonlinearFactorGraph.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::row
const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j)
Extracts a row view from a matrix that avoids a copy.
Definition Matrix.h:222
│ │ │ │ -
gtsam::markdown
string markdown(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
Free version of markdown.
Definition DiscreteValues.cpp:129
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::apply
DecisionTree< L, Y > apply(const DecisionTree< L, Y > &f, const typename DecisionTree< L, Y >::Unary &op)
free versions of apply
Definition DecisionTree.h:413
│ │ │ │ +
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::AlgebraicDecisionTree
An algebraic decision tree fixes the range of a DecisionTree to double.
Definition AlgebraicDecisionTree.h:38
│ │ │ │ -
gtsam::DecisionTreeFactor
A discrete probabilistic factor.
Definition DecisionTreeFactor.h:45
│ │ │ │ -
gtsam::DecisionTreeFactor::sum
shared_ptr sum(const Ordering &keys) const
Create new factor by summing all values with the same separator values.
Definition DecisionTreeFactor.h:173
│ │ │ │ -
gtsam::DecisionTreeFactor::operator()
double operator()(const DiscreteValues &values) const override
Evaluate probability density, sugar.
Definition DecisionTreeFactor.h:143
│ │ │ │ -
gtsam::DecisionTreeFactor::operator*
DecisionTreeFactor operator*(const DecisionTreeFactor &f) const override
multiply two factors
Definition DecisionTreeFactor.h:151
│ │ │ │ -
gtsam::DecisionTreeFactor::max
shared_ptr max(size_t nrFrontals) const
Create new factor by maximizing over all values with the same separator.
Definition DecisionTreeFactor.h:178
│ │ │ │ -
gtsam::DecisionTreeFactor::max
shared_ptr max(const Ordering &keys) const
Create new factor by maximizing over all values with the same separator.
Definition DecisionTreeFactor.h:183
│ │ │ │ -
gtsam::DecisionTreeFactor::evaluate
double evaluate(const DiscreteValues &values) const
Calculate probability for given values x, is just look up in AlgebraicDecisionTree.
Definition DecisionTreeFactor.h:138
│ │ │ │ -
gtsam::DecisionTreeFactor::Base
DiscreteFactor Base
Typedef to base class.
Definition DecisionTreeFactor.h:49
│ │ │ │ -
gtsam::DecisionTreeFactor::sum
shared_ptr sum(size_t nrFrontals) const
Create new factor by summing all values with the same separator values.
Definition DecisionTreeFactor.h:168
│ │ │ │ -
gtsam::DecisionTreeFactor::DecisionTreeFactor
DecisionTreeFactor(const DiscreteKey &key, SOURCE table)
Single-key specialization.
Definition DecisionTreeFactor.h:110
│ │ │ │ -
gtsam::DecisionTreeFactor::toDecisionTreeFactor
DecisionTreeFactor toDecisionTreeFactor() const override
Convert into a decisiontree.
Definition DecisionTreeFactor.h:165
│ │ │ │ -
gtsam::DecisionTreeFactor::operator/
DecisionTreeFactor operator/(const DecisionTreeFactor &f) const
divide by factor f (safely)
Definition DecisionTreeFactor.h:160
│ │ │ │ -
gtsam::DecisionTreeFactor::DecisionTreeFactor
DecisionTreeFactor(const DiscreteKey &key, const std::vector< double > &row)
Single-key specialization, with vector of doubles.
Definition DecisionTreeFactor.h:114
│ │ │ │ -
gtsam::DiscreteConditional
Discrete Conditional Density Derives from DecisionTreeFactor.
Definition DiscreteConditional.h:40
│ │ │ │ -
gtsam::DiscreteFactor
Base class for discrete probabilistic factors The most general one is the derived DecisionTreeFactor.
Definition DiscreteFactor.h:38
│ │ │ │ -
gtsam::DiscreteKeys
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition DiscreteKey.h:39
│ │ │ │ -
gtsam::DiscreteValues
A map from keys to values.
Definition DiscreteValues.h:34
│ │ │ │ +
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ +
gtsam::Factor
Definition Factor.h:68
│ │ │ │
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ -
HybridValues
the error.
│ │ │ │ +
gtsam::Scatter
Scatter is an intermediate data structure used when building a HessianFactor incrementally,...
Definition Scatter.h:49
│ │ │ │ +
gtsam::ExpressionFactor
Factor that supports arbitrary expressions via AD.
Definition ExpressionFactor.h:44
│ │ │ │ +
gtsam::Expression
Expression class that supports automatic differentiation.
Definition Expression.h:48
│ │ │ │ +
gtsam::GraphvizFormatting
Formatting options and functions for saving a NonlinearFactorGraph instance in GraphViz format.
Definition GraphvizFormatting.h:32
│ │ │ │ +
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │ +
gtsam::NonlinearFactorGraph::NonlinearFactorGraph
NonlinearFactorGraph()
Default constructor.
Definition NonlinearFactorGraph.h:67
│ │ │ │ +
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Convenience method which adds a PriorFactor to the factor graph.
Definition NonlinearFactorGraph.h:202
│ │ │ │ +
gtsam::NonlinearFactorGraph::NonlinearFactorGraph
NonlinearFactorGraph(const CONTAINER &factors)
Construct from container of factors (shared_ptr or plain objects)
Definition NonlinearFactorGraph.h:75
│ │ │ │ +
gtsam::NonlinearFactorGraph::addExpressionFactor
void addExpressionFactor(const SharedNoiseModel &R, const T &z, const Expression< T > &h)
Directly add ExpressionFactor that implements |h(x)-z|^2_R.
Definition NonlinearFactorGraph.h:190
│ │ │ │ +
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const Matrix &covariance)
Convenience method which adds a PriorFactor to the factor graph.
Definition NonlinearFactorGraph.h:218
│ │ │ │ +
gtsam::NonlinearFactorGraph::NonlinearFactorGraph
NonlinearFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
Construct from iterator over factors.
Definition NonlinearFactorGraph.h:71
│ │ │ │ +
gtsam::NonlinearFactorGraph::Dampen
std::function< void(const boost::shared_ptr< HessianFactor > &hessianFactor)> Dampen
typdef for dampen functions used below
Definition NonlinearFactorGraph.h:136
│ │ │ │ +
gtsam::NonlinearFactorGraph::NonlinearFactorGraph
NonlinearFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
Implicit copy/downcast constructor to override explicit template container constructor.
Definition NonlinearFactorGraph.h:79
│ │ │ │ +
gtsam::NonlinearFactorGraph::~NonlinearFactorGraph
virtual ~NonlinearFactorGraph()
Destructor.
Definition NonlinearFactorGraph.h:82
│ │ │ │ +
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │ + │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,212 +1,228 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DecisionTreeFactor.h │ │ │ │ │ +NonlinearFactorGraph.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_F_a_c_t_o_r_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_K_e_y_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_O_r_d_e_r_i_n_g_._h> │ │ │ │ │ -25 │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ -28#include │ │ │ │ │ -29#include │ │ │ │ │ -30#include │ │ │ │ │ -31#include │ │ │ │ │ -32#include │ │ │ │ │ -33 │ │ │ │ │ -34namespace _g_t_s_a_m { │ │ │ │ │ -35 │ │ │ │ │ -36 class DiscreteConditional; │ │ │ │ │ -37 class _H_y_b_r_i_d_V_a_l_u_e_s; │ │ │ │ │ -38 │ │ │ │ │ -_4_4 class GTSAM_EXPORT _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r : public _D_i_s_c_r_e_t_e_F_a_c_t_o_r, │ │ │ │ │ -45 public _A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e { │ │ │ │ │ -46 public: │ │ │ │ │ -47 // typedefs needed to play nice with gtsam │ │ │ │ │ -48 typedef _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ -_4_9 typedef _D_i_s_c_r_e_t_e_F_a_c_t_o_r _B_a_s_e; │ │ │ │ │ -50 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -51 typedef _A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_<_K_e_y_> _A_D_T; │ │ │ │ │ -52 │ │ │ │ │ -53 protected: │ │ │ │ │ -54 std::map cardinalities_; │ │ │ │ │ -55 │ │ │ │ │ -56 public: │ │ │ │ │ -59 │ │ │ │ │ -61 _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r(); │ │ │ │ │ +20// \callgraph │ │ │ │ │ +21 │ │ │ │ │ +22#pragma once │ │ │ │ │ +23 │ │ │ │ │ +24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g_._h> │ │ │ │ │ +27#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +28#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_P_r_i_o_r_F_a_c_t_o_r_._h> │ │ │ │ │ +29 │ │ │ │ │ +30#include │ │ │ │ │ +31#include │ │ │ │ │ +32 │ │ │ │ │ +33namespace _g_t_s_a_m { │ │ │ │ │ +34 │ │ │ │ │ +35 // Forward declarations │ │ │ │ │ +36 class _V_a_l_u_e_s; │ │ │ │ │ +37 class Ordering; │ │ │ │ │ +38 class GaussianFactorGraph; │ │ │ │ │ +39 class SymbolicFactorGraph; │ │ │ │ │ +40 template │ │ │ │ │ +41 class Expression; │ │ │ │ │ +42 template │ │ │ │ │ +43 class ExpressionFactor; │ │ │ │ │ +44 │ │ │ │ │ +_5_5 class GTSAM_EXPORT _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h: public _F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +{ │ │ │ │ │ +56 │ │ │ │ │ +57 public: │ │ │ │ │ +58 │ │ │ │ │ +59 typedef _F_a_c_t_o_r_G_r_a_p_h_<_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_> _B_a_s_e; │ │ │ │ │ +60 typedef _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h _T_h_i_s; │ │ │ │ │ +61 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ 62 │ │ │ │ │ -64 _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r(const _D_i_s_c_r_e_t_e_K_e_y_s& keys, const _A_D_T& potentials); │ │ │ │ │ 65 │ │ │ │ │ -85 _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r(const _D_i_s_c_r_e_t_e_K_e_y_s& keys, │ │ │ │ │ -86 const std::vector& table); │ │ │ │ │ +_6_7 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h() {} │ │ │ │ │ +68 │ │ │ │ │ +70 template │ │ │ │ │ +_7_1 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h(ITERATOR firstFactor, ITERATOR lastFactor) : _B_a_s_e │ │ │ │ │ +(firstFactor, lastFactor) {} │ │ │ │ │ +72 │ │ │ │ │ +74 template │ │ │ │ │ +_7_5 explicit _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h(const CONTAINER& factors) : _B_a_s_e(factors) {} │ │ │ │ │ +76 │ │ │ │ │ +78 template │ │ │ │ │ +_7_9 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h(const _F_a_c_t_o_r_G_r_a_p_h_<_D_E_R_I_V_E_D_F_A_C_T_O_R_>& graph) : _B_a_s_e(graph) │ │ │ │ │ +{} │ │ │ │ │ +80 │ │ │ │ │ +_8_2 virtual _~_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h() {} │ │ │ │ │ +83 │ │ │ │ │ 87 │ │ │ │ │ -106 _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r(const _D_i_s_c_r_e_t_e_K_e_y_s& keys, const std::string& table); │ │ │ │ │ -107 │ │ │ │ │ -109 template │ │ │ │ │ -_1_1_0 _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r(const _D_i_s_c_r_e_t_e_K_e_y& key, SOURCE table) │ │ │ │ │ -111 : _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r(_D_i_s_c_r_e_t_e_K_e_y_s{key}, table) {} │ │ │ │ │ -112 │ │ │ │ │ -_1_1_4 _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r(const _D_i_s_c_r_e_t_e_K_e_y& key, const std::vector& row) │ │ │ │ │ -115 : _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r(_D_i_s_c_r_e_t_e_K_e_y_s{key}, _r_o_w) {} │ │ │ │ │ +89 void _p_r_i_n_t( │ │ │ │ │ +90 const std::string& str = "NonlinearFactorGraph: ", │ │ │ │ │ +91 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override; │ │ │ │ │ +92 │ │ │ │ │ +94 void printErrors(const _V_a_l_u_e_s& values, const std::string& str = │ │ │ │ │ +"NonlinearFactorGraph: ", │ │ │ │ │ +95 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ +96 const std::function& │ │ │ │ │ +97 printCondition = [](const _F_a_c_t_o_r *,double, size_t) {return true;}) const; │ │ │ │ │ +98 │ │ │ │ │ +100 bool equals(const NonlinearFactorGraph& other, double tol = 1e-9) const; │ │ │ │ │ +101 │ │ │ │ │ +105 │ │ │ │ │ +107 double error(const _V_a_l_u_e_s& values) const; │ │ │ │ │ +108 │ │ │ │ │ +110 double probPrime(const _V_a_l_u_e_s& values) const; │ │ │ │ │ +111 │ │ │ │ │ +115 boost::shared_ptr symbolic() const; │ │ │ │ │ 116 │ │ │ │ │ -118 explicit _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r(const _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l& c); │ │ │ │ │ -119 │ │ │ │ │ -123 │ │ │ │ │ -125 bool _e_q_u_a_l_s(const _D_i_s_c_r_e_t_e_F_a_c_t_o_r& other, double tol = 1e-9) const override; │ │ │ │ │ -126 │ │ │ │ │ -127 // print │ │ │ │ │ -128 void _p_r_i_n_t( │ │ │ │ │ -129 const std::string& s = "DecisionTreeFactor:\n", │ │ │ │ │ -130 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override; │ │ │ │ │ +120 Ordering orderingCOLAMD() const; │ │ │ │ │ +121 │ │ │ │ │ +130 Ordering orderingCOLAMDConstrained(const FastMap& constraints) │ │ │ │ │ +const; │ │ │ │ │ 131 │ │ │ │ │ -135 │ │ │ │ │ -_1_3_8 double _e_v_a_l_u_a_t_e(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values) const { │ │ │ │ │ -139 return ADT::operator()(values); │ │ │ │ │ -140 } │ │ │ │ │ -141 │ │ │ │ │ -_1_4_3 double _o_p_e_r_a_t_o_r_(_)(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values) const override { │ │ │ │ │ -144 return ADT::operator()(values); │ │ │ │ │ -145 } │ │ │ │ │ -146 │ │ │ │ │ -148 double error(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values) const; │ │ │ │ │ -149 │ │ │ │ │ -_1_5_1 _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r _o_p_e_r_a_t_o_r_*(const _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r& f) const override { │ │ │ │ │ -152 return _a_p_p_l_y(f, ADT::Ring::mul); │ │ │ │ │ -153 } │ │ │ │ │ -154 │ │ │ │ │ -155 static double safe_div(const double& a, const double& b); │ │ │ │ │ -156 │ │ │ │ │ -157 size_t cardinality(_K_e_y j) const { return cardinalities_.at(j); } │ │ │ │ │ +133 boost::shared_ptr linearize(const _V_a_l_u_e_s& │ │ │ │ │ +linearizationPoint) const; │ │ │ │ │ +134 │ │ │ │ │ +_1_3_6 typedef std::function& │ │ │ │ │ +hessianFactor)> _D_a_m_p_e_n; │ │ │ │ │ +137 │ │ │ │ │ +145 boost::shared_ptr linearizeToHessianFactor( │ │ │ │ │ +146 const _V_a_l_u_e_s& values, const _D_a_m_p_e_n& dampen = nullptr) const; │ │ │ │ │ +147 │ │ │ │ │ +156 boost::shared_ptr linearizeToHessianFactor( │ │ │ │ │ +157 const _V_a_l_u_e_s& values, const _O_r_d_e_r_i_n_g& ordering, const _D_a_m_p_e_n& dampen = │ │ │ │ │ +nullptr) const; │ │ │ │ │ 158 │ │ │ │ │ -_1_6_0 _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r _o_p_e_r_a_t_o_r_/(const _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r& f) const { │ │ │ │ │ -161 return _a_p_p_l_y(f, safe_div); │ │ │ │ │ -162 } │ │ │ │ │ +161 _V_a_l_u_e_s updateCholesky(const _V_a_l_u_e_s& values, │ │ │ │ │ +162 const _D_a_m_p_e_n& dampen = nullptr) const; │ │ │ │ │ 163 │ │ │ │ │ -_1_6_5 _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r _t_o_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r() const override { return *this; } │ │ │ │ │ -166 │ │ │ │ │ -_1_6_8 shared_ptr _s_u_m(size_t nrFrontals) const { │ │ │ │ │ -169 return combine(nrFrontals, ADT::Ring::add); │ │ │ │ │ -170 } │ │ │ │ │ +166 _V_a_l_u_e_s updateCholesky(const _V_a_l_u_e_s& values, const _O_r_d_e_r_i_n_g& ordering, │ │ │ │ │ +167 const _D_a_m_p_e_n& dampen = nullptr) const; │ │ │ │ │ +168 │ │ │ │ │ +170 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h clone() const; │ │ │ │ │ 171 │ │ │ │ │ -_1_7_3 shared_ptr _s_u_m(const _O_r_d_e_r_i_n_g& keys) const { │ │ │ │ │ -174 return combine(keys, ADT::Ring::add); │ │ │ │ │ -175 } │ │ │ │ │ -176 │ │ │ │ │ -_1_7_8 shared_ptr _m_a_x(size_t nrFrontals) const { │ │ │ │ │ -179 return combine(nrFrontals, ADT::Ring::max); │ │ │ │ │ -180 } │ │ │ │ │ -181 │ │ │ │ │ -_1_8_3 shared_ptr _m_a_x(const _O_r_d_e_r_i_n_g& keys) const { │ │ │ │ │ -184 return combine(keys, ADT::Ring::max); │ │ │ │ │ -185 } │ │ │ │ │ -186 │ │ │ │ │ -190 │ │ │ │ │ -196 _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r _a_p_p_l_y(const _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r& f, ADT::Binary op) │ │ │ │ │ -const; │ │ │ │ │ -197 │ │ │ │ │ -204 shared_ptr combine(size_t nrFrontals, ADT::Binary op) const; │ │ │ │ │ -205 │ │ │ │ │ -212 shared_ptr combine(const _O_r_d_e_r_i_n_g& keys, ADT::Binary op) const; │ │ │ │ │ -213 │ │ │ │ │ -215 std::vector> enumerate() const; │ │ │ │ │ -216 │ │ │ │ │ -218 _D_i_s_c_r_e_t_e_K_e_y_s discreteKeys() const; │ │ │ │ │ -219 │ │ │ │ │ -238 _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r prune(size_t maxNrAssignments) const; │ │ │ │ │ +181 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h rekey(const std::map& rekey_mapping) const; │ │ │ │ │ +182 │ │ │ │ │ +189 template │ │ │ │ │ +_1_9_0 void _a_d_d_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r(const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& R, const T& z, │ │ │ │ │ +191 const _E_x_p_r_e_s_s_i_o_n_<_T_>& h) { │ │ │ │ │ +192 push_back(boost::make_shared<_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_<_T_> >(R, z, h)); │ │ │ │ │ +193 } │ │ │ │ │ +194 │ │ │ │ │ +201 template │ │ │ │ │ +_2_0_2 void _a_d_d_P_r_i_o_r(_K_e_y key, const T& prior, │ │ │ │ │ +203 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model = nullptr) { │ │ │ │ │ +204 emplace_shared>(key, prior, model); │ │ │ │ │ +205 } │ │ │ │ │ +206 │ │ │ │ │ +217 template │ │ │ │ │ +_2_1_8 void _a_d_d_P_r_i_o_r(_K_e_y key, const T& prior, const Matrix& covariance) { │ │ │ │ │ +219 emplace_shared>(key, prior, covariance); │ │ │ │ │ +220 } │ │ │ │ │ +221 │ │ │ │ │ +225 │ │ │ │ │ +226 using FactorGraph::dot; │ │ │ │ │ +227 using FactorGraph::saveGraph; │ │ │ │ │ +228 │ │ │ │ │ +230 void _d_o_t(std::ostream& os, const _V_a_l_u_e_s& values, │ │ │ │ │ +231 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ +232 const _G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g& writer = _G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g()) const; │ │ │ │ │ +233 │ │ │ │ │ +235 std::string _d_o_t( │ │ │ │ │ +236 const _V_a_l_u_e_s& values, │ │ │ │ │ +237 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ +238 const _G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g& writer = _G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g()) const; │ │ │ │ │ 239 │ │ │ │ │ -243 │ │ │ │ │ -245 void _d_o_t(std::ostream& os, │ │ │ │ │ -246 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ -247 bool showZero = true) const; │ │ │ │ │ +241 void saveGraph( │ │ │ │ │ +242 const std::string& filename, const _V_a_l_u_e_s& values, │ │ │ │ │ +243 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ +244 const _G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g& writer = _G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g()) const; │ │ │ │ │ +246 │ │ │ │ │ +247 private: │ │ │ │ │ 248 │ │ │ │ │ -250 void _d_o_t(const std::string& name, │ │ │ │ │ -251 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ -252 bool showZero = true) const; │ │ │ │ │ -253 │ │ │ │ │ -255 std::string _d_o_t(const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ -256 bool showZero = true) const; │ │ │ │ │ -257 │ │ │ │ │ -265 std::string _m_a_r_k_d_o_w_n(const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ -DefaultKeyFormatter, │ │ │ │ │ -266 const Names& names = {}) const override; │ │ │ │ │ -267 │ │ │ │ │ -275 std::string html(const KeyFormatter& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ -276 const Names& names = {}) const override; │ │ │ │ │ -277 │ │ │ │ │ -281 │ │ │ │ │ -286 double error(const _H_y_b_r_i_d_V_a_l_u_e_s& values) const override; │ │ │ │ │ -287 │ │ │ │ │ -289 │ │ │ │ │ -290 private: │ │ │ │ │ -_2_9_2 friend class boost::serialization::access; │ │ │ │ │ -293 template │ │ │ │ │ -294 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ -295 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -296 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(_A_D_T); │ │ │ │ │ -297 ar& BOOST_SERIALIZATION_NVP(cardinalities_); │ │ │ │ │ -298 } │ │ │ │ │ -299 }; │ │ │ │ │ -300 │ │ │ │ │ -301// traits │ │ │ │ │ -302template <> │ │ │ │ │ -_3_0_3struct _t_r_a_i_t_s<_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ +253 boost::shared_ptr linearizeToHessianFactor( │ │ │ │ │ +254 const _V_a_l_u_e_s& values, const _S_c_a_t_t_e_r& scatter, const Dampen& dampen = │ │ │ │ │ +nullptr) const; │ │ │ │ │ +255 │ │ │ │ │ +_2_5_7 friend class boost::serialization::access; │ │ │ │ │ +258 template │ │ │ │ │ +259 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +260 ar & boost::serialization::make_nvp("NonlinearFactorGraph", │ │ │ │ │ +261 boost::serialization::base_object(*this)); │ │ │ │ │ +262 } │ │ │ │ │ +263 │ │ │ │ │ +264 public: │ │ │ │ │ +265 │ │ │ │ │ +266#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ +269 │ │ │ │ │ +270 boost::shared_ptr GTSAM_DEPRECATED linearizeToHessianFactor( │ │ │ │ │ +271 const _V_a_l_u_e_s& values, boost::none_t, const Dampen& dampen = nullptr) const │ │ │ │ │ +272 {return linearizeToHessianFactor(values, dampen);} │ │ │ │ │ +273 │ │ │ │ │ +275 _V_a_l_u_e_s GTSAM_DEPRECATED updateCholesky(const _V_a_l_u_e_s& values, boost::none_t, │ │ │ │ │ +276 const Dampen& dampen = nullptr) const │ │ │ │ │ +277 {return updateCholesky(values, dampen);} │ │ │ │ │ +278 │ │ │ │ │ +280 void GTSAM_DEPRECATED saveGraph( │ │ │ │ │ +281 std::ostream& os, const _V_a_l_u_e_s& values = _V_a_l_u_e_s(), │ │ │ │ │ +282 const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(), │ │ │ │ │ +283 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { │ │ │ │ │ +284 _d_o_t(os, values, keyFormatter, graphvizFormatting); │ │ │ │ │ +285 } │ │ │ │ │ +287 void GTSAM_DEPRECATED │ │ │ │ │ +288 saveGraph(const std::string& filename, const _V_a_l_u_e_s& values, │ │ │ │ │ +289 const GraphvizFormatting& graphvizFormatting, │ │ │ │ │ +290 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { │ │ │ │ │ +291 saveGraph(filename, values, keyFormatter, graphvizFormatting); │ │ │ │ │ +292 } │ │ │ │ │ +294#endif │ │ │ │ │ +295 │ │ │ │ │ +296 }; │ │ │ │ │ +297 │ │ │ │ │ +299template<> │ │ │ │ │ +_3_0_0struct _t_r_a_i_t_s<_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h> : public _T_e_s_t_a_b_l_e │ │ │ │ │ +{ │ │ │ │ │ +301}; │ │ │ │ │ +302 │ │ │ │ │ +303} //\ namespace gtsam │ │ │ │ │ 304 │ │ │ │ │ -305} // namespace gtsam │ │ │ │ │ -_A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_._h │ │ │ │ │ -Algebraic Decision Trees. │ │ │ │ │ -_D_i_s_c_r_e_t_e_K_e_y_._h │ │ │ │ │ -specialized key for discrete variables │ │ │ │ │ -_D_i_s_c_r_e_t_e_F_a_c_t_o_r_._h │ │ │ │ │ -_O_r_d_e_r_i_n_g_._h │ │ │ │ │ -Variable ordering for the elimination algorithm. │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y │ │ │ │ │ -std::pair< Key, size_t > DiscreteKey │ │ │ │ │ -Key type for discrete variables. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteKey.h:36 │ │ │ │ │ +_P_o_i_n_t_2_._h │ │ │ │ │ +2D Point │ │ │ │ │ +_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Factor Graph Base Class. │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ +Non-linear factor base classes. │ │ │ │ │ +_G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g_._h │ │ │ │ │ +Graphviz formatter for NonlinearFactorGraph. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_r_o_w │ │ │ │ │ -const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j) │ │ │ │ │ -Extracts a row view from a matrix that avoids a copy. │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:222 │ │ │ │ │ -_g_t_s_a_m_:_:_m_a_r_k_d_o_w_n │ │ │ │ │ -string markdown(const DiscreteValues &values, const KeyFormatter &keyFormatter, │ │ │ │ │ -const DiscreteValues::Names &names) │ │ │ │ │ -Free version of markdown. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteValues.cpp:129 │ │ │ │ │ _g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ print without optional string, must specify cout yourself │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_a_p_p_l_y │ │ │ │ │ -DecisionTree< L, Y > apply(const DecisionTree< L, Y > &f, const typename │ │ │ │ │ -DecisionTree< L, Y >::Unary &op) │ │ │ │ │ -free versions of apply │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree.h:413 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ +Aliases. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ _g_t_s_a_m_:_:_d_o_t │ │ │ │ │ double dot(const V1 &a, const V2 &b) │ │ │ │ │ Dot product. │ │ │ │ │ DDeeffiinniittiioonn Vector.h:195 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ @@ -215,89 +231,84 @@ │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ _g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ A helper that implements the traits interface for GTSAM types. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e │ │ │ │ │ -An algebraic decision tree fixes the range of a DecisionTree to double. │ │ │ │ │ -DDeeffiinniittiioonn AlgebraicDecisionTree.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r │ │ │ │ │ -A discrete probabilistic factor. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTreeFactor.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_:_:_s_u_m │ │ │ │ │ -shared_ptr sum(const Ordering &keys) const │ │ │ │ │ -Create new factor by summing all values with the same separator values. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTreeFactor.h:173 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ -double operator()(const DiscreteValues &values) const override │ │ │ │ │ -Evaluate probability density, sugar. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTreeFactor.h:143 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ -DecisionTreeFactor operator*(const DecisionTreeFactor &f) const override │ │ │ │ │ -multiply two factors │ │ │ │ │ -DDeeffiinniittiioonn DecisionTreeFactor.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_:_:_m_a_x │ │ │ │ │ -shared_ptr max(size_t nrFrontals) const │ │ │ │ │ -Create new factor by maximizing over all values with the same separator. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTreeFactor.h:178 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_:_:_m_a_x │ │ │ │ │ -shared_ptr max(const Ordering &keys) const │ │ │ │ │ -Create new factor by maximizing over all values with the same separator. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTreeFactor.h:183 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e │ │ │ │ │ -double evaluate(const DiscreteValues &values) const │ │ │ │ │ -Calculate probability for given values x, is just look up in │ │ │ │ │ -AlgebraicDecisionTree. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTreeFactor.h:138 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_:_:_B_a_s_e │ │ │ │ │ -DiscreteFactor Base │ │ │ │ │ -Typedef to base class. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTreeFactor.h:49 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_:_:_s_u_m │ │ │ │ │ -shared_ptr sum(size_t nrFrontals) const │ │ │ │ │ -Create new factor by summing all values with the same separator values. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTreeFactor.h:168 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r │ │ │ │ │ -DecisionTreeFactor(const DiscreteKey &key, SOURCE table) │ │ │ │ │ -Single-key specialization. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTreeFactor.h:110 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_:_:_t_o_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r │ │ │ │ │ -DecisionTreeFactor toDecisionTreeFactor() const override │ │ │ │ │ -Convert into a decisiontree. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTreeFactor.h:165 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_:_:_o_p_e_r_a_t_o_r_/ │ │ │ │ │ -DecisionTreeFactor operator/(const DecisionTreeFactor &f) const │ │ │ │ │ -divide by factor f (safely) │ │ │ │ │ -DDeeffiinniittiioonn DecisionTreeFactor.h:160 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r │ │ │ │ │ -DecisionTreeFactor(const DiscreteKey &key, const std::vector< double > &row) │ │ │ │ │ -Single-key specialization, with vector of doubles. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTreeFactor.h:114 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -Discrete Conditional Density Derives from DecisionTreeFactor. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteConditional.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r │ │ │ │ │ -Base class for discrete probabilistic factors The most general one is the │ │ │ │ │ -derived DecisionTreeFactor. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactor.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ -DiscreteKeys is a set of keys that can be assembled using the & operator. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteKey.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ -A map from keys to values. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteValues.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ +nodes. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:68 │ │ │ │ │ _g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ -_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -the error. │ │ │ │ │ +_g_t_s_a_m_:_:_S_c_a_t_t_e_r │ │ │ │ │ +Scatter is an intermediate data structure used when building a HessianFactor │ │ │ │ │ +incrementally,... │ │ │ │ │ +DDeeffiinniittiioonn Scatter.h:49 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r │ │ │ │ │ +Factor that supports arbitrary expressions via AD. │ │ │ │ │ +DDeeffiinniittiioonn ExpressionFactor.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n │ │ │ │ │ +Expression class that supports automatic differentiation. │ │ │ │ │ +DDeeffiinniittiioonn Expression.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g │ │ │ │ │ +Formatting options and functions for saving a NonlinearFactorGraph instance in │ │ │ │ │ +GraphViz format. │ │ │ │ │ +DDeeffiinniittiioonn GraphvizFormatting.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +NonlinearFactorGraph() │ │ │ │ │ +Default constructor. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactorGraph.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d_P_r_i_o_r │ │ │ │ │ +void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr) │ │ │ │ │ +Convenience method which adds a PriorFactor to the factor graph. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactorGraph.h:202 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +NonlinearFactorGraph(const CONTAINER &factors) │ │ │ │ │ +Construct from container of factors (shared_ptr or plain objects) │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactorGraph.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r │ │ │ │ │ +void addExpressionFactor(const SharedNoiseModel &R, const T &z, const │ │ │ │ │ +Expression< T > &h) │ │ │ │ │ +Directly add ExpressionFactor that implements |h(x)-z|^2_R. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactorGraph.h:190 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d_P_r_i_o_r │ │ │ │ │ +void addPrior(Key key, const T &prior, const Matrix &covariance) │ │ │ │ │ +Convenience method which adds a PriorFactor to the factor graph. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactorGraph.h:218 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +NonlinearFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) │ │ │ │ │ +Construct from iterator over factors. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactorGraph.h:71 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_D_a_m_p_e_n │ │ │ │ │ +std::function< void(const boost::shared_ptr< HessianFactor > &hessianFactor)> │ │ │ │ │ +Dampen │ │ │ │ │ +typdef for dampen functions used below │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactorGraph.h:136 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +NonlinearFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph) │ │ │ │ │ +Implicit copy/downcast constructor to override explicit template container │ │ │ │ │ +constructor. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactorGraph.h:79 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_~_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +virtual ~NonlinearFactorGraph() │ │ │ │ │ +Destructor. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactorGraph.h:82 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +_V_a_l_u_e_s │ │ │ │ │ +In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ +a non-linear function... │ │ │ │ │ +_P_r_i_o_r_F_a_c_t_o_r_._h │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_._h │ │ │ │ │ + * _n_o_n_l_i_n_e_a_r │ │ │ │ │ + * _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00254.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00275.html │ │ │ │┄ Files 76% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/Signature.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Point2.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions | │ │ │ │ -Variables
│ │ │ │ -
Signature.cpp File Reference
│ │ │ │ +Functions
│ │ │ │ +
Point2.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

signatures for conditional densities │ │ │ │ +

2D Point │ │ │ │ More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Typedefs

│ │ │ │ -typedef string::const_iterator gtsam::parser::It
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ Functions

│ │ │ │ -Signature::Table gtsam::parser::logic (bool ff, bool ft, bool tf, bool tt)
 
│ │ │ │ -ostream & gtsam::operator<< (ostream &os, const Signature::Row &row)
 
│ │ │ │ -ostream & gtsam::operator<< (ostream &os, const Signature::Table &table)
 
│ │ │ │ -ostream & gtsam::operator<< (ostream &os, const Signature &s)
 
│ │ │ │ -Signature gtsam::operator| (const DiscreteKey &key, const DiscreteKey &parent)
 Helper function to create Signature objects example: Signature s = D | E;.
 
│ │ │ │ -Signature gtsam::operator% (const DiscreteKey &key, const std::string &parent)
 Helper function to create Signature objects example: Signature s(D % "99/1");.
 
│ │ │ │ -Signature gtsam::operator% (const DiscreteKey &key, const Signature::Table &parent)
 Helper function to create Signature objects, using table construction directly example: Signature s(D % table);.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ -Variables

│ │ │ │ -Signature::Row gtsam::parser::F {1, 0}
 
│ │ │ │ -Signature::Row gtsam::parser::T {0, 1}
 
│ │ │ │ -struct gtsam::parser::Grammar gtsam::parser::grammar
 
│ │ │ │ +double gtsam::norm2 (const Point2 &p, OptionalJacobian< 1, 2 > H=boost::none)
 Distance of the point from the origin, with Jacobian.
 
│ │ │ │ +double gtsam::distance2 (const Point2 &p1, const Point2 &q, OptionalJacobian< 1, 2 > H1=boost::none, OptionalJacobian< 1, 2 > H2=boost::none)
 distance between two points
 
│ │ │ │ +boost::optional< Point2 > gtsam::circleCircleIntersection (double R_d, double r_d, double tol)
 
│ │ │ │ +list< Point2 > gtsam::circleCircleIntersection (Point2 c1, Point2 c2, boost::optional< Point2 > fh)
 
list< Point2 > gtsam::circleCircleIntersection (Point2 c1, double r1, Point2 c2, double r2, double tol=1e-9)
 Intersect 2 circles.
 
│ │ │ │ +Point2Pair gtsam::means (const std::vector< Point2Pair > &abPointPairs)
 Calculate the two means of a set of Point2 pairs.
 
│ │ │ │ +ostream & gtsam::operator<< (ostream &os, const gtsam::Point2Pair &p)
 
│ │ │ │

Detailed Description

│ │ │ │ -

signatures for conditional densities

│ │ │ │ +

2D Point

│ │ │ │
Author
Frank Dellaert
│ │ │ │ -
Date
Feb 27, 2011
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,57 +1,48 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s | _V_a_r_i_a_b_l_e_s │ │ │ │ │ -Signature.cpp File Reference │ │ │ │ │ -signatures for conditional densities _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Point2.cpp File Reference │ │ │ │ │ +2D Point _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -typedef string::const_iterator  ggttssaamm::::ppaarrsseerr::::IItt │ │ │ │ │ -  │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -Signature::Table  ggttssaamm::::ppaarrsseerr::::llooggiicc (bool ff, bool ft, bool tf, bool tt) │ │ │ │ │ -  │ │ │ │ │ - ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const _S_i_g_n_a_t_u_r_e_:_:_R_o_w &_r_o_w) │ │ │ │ │ -  │ │ │ │ │ - ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const Signature::Table │ │ │ │ │ - &table) │ │ │ │ │ -  │ │ │ │ │ - ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const _S_i_g_n_a_t_u_r_e &s) │ │ │ │ │ -  │ │ │ │ │ - _S_i_g_n_a_t_u_r_e  ggttssaamm::::ooppeerraattoorr|| (const _D_i_s_c_r_e_t_e_K_e_y &key, const _D_i_s_c_r_e_t_e_K_e_y │ │ │ │ │ - &parent) │ │ │ │ │ -  Helper function to create _S_i_g_n_a_t_u_r_e objects example: │ │ │ │ │ - _S_i_g_n_a_t_u_r_e s = D | E;. │ │ │ │ │ -  │ │ │ │ │ - _S_i_g_n_a_t_u_r_e  ggttssaamm::::ooppeerraattoorr%% (const _D_i_s_c_r_e_t_e_K_e_y &key, const std::string │ │ │ │ │ - &parent) │ │ │ │ │ -  Helper function to create _S_i_g_n_a_t_u_r_e objects example: │ │ │ │ │ - _S_i_g_n_a_t_u_r_e s(D % "99/1");. │ │ │ │ │ -  │ │ │ │ │ - _S_i_g_n_a_t_u_r_e  ggttssaamm::::ooppeerraattoorr%% (const _D_i_s_c_r_e_t_e_K_e_y &key, const Signature:: │ │ │ │ │ - Table &parent) │ │ │ │ │ -  Helper function to create _S_i_g_n_a_t_u_r_e objects, using table │ │ │ │ │ - construction directly example: _S_i_g_n_a_t_u_r_e s(D % table);. │ │ │ │ │ -  │ │ │ │ │ -VVaarriiaabblleess │ │ │ │ │ - _S_i_g_n_a_t_u_r_e_:_:_R_o_w  ggttssaamm::::ppaarrsseerr::::FF {1, 0} │ │ │ │ │ -  │ │ │ │ │ - _S_i_g_n_a_t_u_r_e_:_:_R_o_w  ggttssaamm::::ppaarrsseerr::::TT {0, 1} │ │ │ │ │ + double  ggttssaamm::::nnoorrmm22 (const _P_o_i_n_t_2 &p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 1, │ │ │ │ │ + 2 > H=boost::none) │ │ │ │ │ +  Distance of the point from the origin, with │ │ │ │ │ + Jacobian. │ │ │ │ │ +  │ │ │ │ │ + double  ggttssaamm::::ddiissttaannccee22 (const _P_o_i_n_t_2 &p1, const _P_o_i_n_t_2 &q, │ │ │ │ │ + _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 1, 2 > H1=boost::none, │ │ │ │ │ + _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 1, 2 > H2=boost::none) │ │ │ │ │ +  distance between two points │ │ │ │ │ +  │ │ │ │ │ +boost::optional< _P_o_i_n_t_2 >  ggttssaamm::::cciirrcclleeCCiirrcclleeIInntteerrsseeccttiioonn (double R_d, double │ │ │ │ │ + r_d, double tol) │ │ │ │ │ +  │ │ │ │ │ + list< _P_o_i_n_t_2 >  ggttssaamm::::cciirrcclleeCCiirrcclleeIInntteerrsseeccttiioonn (_P_o_i_n_t_2 c1, _P_o_i_n_t_2 │ │ │ │ │ + c2, boost::optional< _P_o_i_n_t_2 > fh) │ │ │ │ │ +  │ │ │ │ │ + list< _P_o_i_n_t_2 >  _g_t_s_a_m_:_:_c_i_r_c_l_e_C_i_r_c_l_e_I_n_t_e_r_s_e_c_t_i_o_n (_P_o_i_n_t_2 c1, double │ │ │ │ │ + r1, _P_o_i_n_t_2 c2, double r2, double tol=1e-9) │ │ │ │ │ +  Intersect 2 circles. │ │ │ │ │ +  │ │ │ │ │ + Point2Pair  ggttssaamm::::mmeeaannss (const std::vector< Point2Pair > │ │ │ │ │ + &abPointPairs) │ │ │ │ │ +  Calculate the two means of a set of Point2 pairs. │ │ │ │ │   │ │ │ │ │ -struct gtsam::parser::Grammar  ggttssaamm::::ppaarrsseerr::::ggrraammmmaarr │ │ │ │ │ + ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const gtsam:: │ │ │ │ │ + Point2Pair &p) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -signatures for conditional densities │ │ │ │ │ +2D Point │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Date │ │ │ │ │ - Feb 27, 2011 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _S_i_g_n_a_t_u_r_e_._c_p_p │ │ │ │ │ + * _g_e_o_m_e_t_r_y │ │ │ │ │ + * _P_o_i_n_t_2_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00263_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00533_source.html │ │ │ │┄ Files 93% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/GaussianMixtureFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
DiscreteFactor.h
│ │ │ │ +
GaussianMixtureFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/discrete/DiscreteValues.h>
│ │ │ │ -
22#include <gtsam/inference/Factor.h>
│ │ │ │ -
23#include <gtsam/base/Testable.h>
│ │ │ │ -
24
│ │ │ │ -
25#include <string>
│ │ │ │ -
26namespace gtsam {
│ │ │ │ -
27
│ │ │ │ -
28class DecisionTreeFactor;
│ │ │ │ -
29class DiscreteConditional;
│ │ │ │ -
30class HybridValues;
│ │ │ │ +
21#pragma once
│ │ │ │ +
22
│ │ │ │ +
23#include <gtsam/discrete/AlgebraicDecisionTree.h>
│ │ │ │ +
24#include <gtsam/discrete/DecisionTree.h>
│ │ │ │ +
25#include <gtsam/discrete/DiscreteKey.h>
│ │ │ │ +
26#include <gtsam/hybrid/HybridFactor.h>
│ │ │ │ +
27#include <gtsam/linear/GaussianFactor.h>
│ │ │ │ +
28#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ +
29
│ │ │ │ +
30namespace gtsam {
│ │ │ │
31
│ │ │ │ -
│ │ │ │ -
38class GTSAM_EXPORT DiscreteFactor: public Factor {
│ │ │ │ -
39
│ │ │ │ -
40public:
│ │ │ │ -
41
│ │ │ │ -
42 // typedefs needed to play nice with gtsam
│ │ │ │ -
43 typedef DiscreteFactor This;
│ │ │ │ -
44 typedef boost::shared_ptr<DiscreteFactor> shared_ptr;
│ │ │ │ -
45 typedef Factor Base;
│ │ │ │ -
46
│ │ │ │ -
47 using Values = DiscreteValues;
│ │ │ │ -
48
│ │ │ │ -
49public:
│ │ │ │ -
50
│ │ │ │ -
53
│ │ │ │ -
55 DiscreteFactor() {}
│ │ │ │ -
56
│ │ │ │ -
59 template<typename CONTAINER>
│ │ │ │ -
60 DiscreteFactor(const CONTAINER& keys) : Base(keys) {}
│ │ │ │ +
32class HybridValues;
│ │ │ │ +
33class DiscreteValues;
│ │ │ │ +
34class VectorValues;
│ │ │ │ +
35
│ │ │ │ +
│ │ │ │ +
47class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor {
│ │ │ │ +
48 public:
│ │ │ │ +
49 using Base = HybridFactor;
│ │ │ │ +
50 using This = GaussianMixtureFactor;
│ │ │ │ +
51 using shared_ptr = boost::shared_ptr<This>;
│ │ │ │ +
52
│ │ │ │ +
53 using sharedFactor = boost::shared_ptr<GaussianFactor>;
│ │ │ │ +
54
│ │ │ │ +
56 using Factors = DecisionTree<Key, sharedFactor>;
│ │ │ │ +
57
│ │ │ │ +
58 private:
│ │ │ │ +
60 Factors factors_;
│ │ │ │
61
│ │ │ │ -
│ │ │ │ -
63 virtual ~DiscreteFactor() {
│ │ │ │ -
64 }
│ │ │ │ -
│ │ │ │ -
65
│ │ │ │ +
68 GaussianFactorGraphTree asGaussianFactorGraphTree() const;
│ │ │ │
69
│ │ │ │ -
71 virtual bool equals(const DiscreteFactor& lf, double tol = 1e-9) const = 0;
│ │ │ │ -
72
│ │ │ │ -
│ │ │ │ -
74 void print(
│ │ │ │ -
75 const std::string& s = "DiscreteFactor\n",
│ │ │ │ -
76 const KeyFormatter& formatter = DefaultKeyFormatter) const override {
│ │ │ │ -
77 Base::print(s, formatter);
│ │ │ │ -
78 }
│ │ │ │ -
│ │ │ │ -
79
│ │ │ │ -
83
│ │ │ │ -
85 virtual double operator()(const DiscreteValues&) const = 0;
│ │ │ │ -
86
│ │ │ │ -
88 double error(const DiscreteValues& values) const;
│ │ │ │ +
70 public:
│ │ │ │ +
73
│ │ │ │ +
75 GaussianMixtureFactor() = default;
│ │ │ │ +
76
│ │ │ │ +
86 GaussianMixtureFactor(const KeyVector &continuousKeys,
│ │ │ │ +
87 const DiscreteKeys &discreteKeys,
│ │ │ │ +
88 const Factors &factors);
│ │ │ │
89
│ │ │ │ -
94 double error(const HybridValues& c) const override;
│ │ │ │ -
95
│ │ │ │ -
97 virtual DecisionTreeFactor operator*(const DecisionTreeFactor&) const = 0;
│ │ │ │ -
98
│ │ │ │ -
99 virtual DecisionTreeFactor toDecisionTreeFactor() const = 0;
│ │ │ │ -
100
│ │ │ │ -
104
│ │ │ │ -
106 using Names = DiscreteValues::Names;
│ │ │ │ +
│ │ │ │ +
98 GaussianMixtureFactor(const KeyVector &continuousKeys,
│ │ │ │ +
99 const DiscreteKeys &discreteKeys,
│ │ │ │ +
100 const std::vector<sharedFactor> &factors)
│ │ │ │ +
101 : GaussianMixtureFactor(continuousKeys, discreteKeys,
│ │ │ │ +
102 Factors(discreteKeys, factors)) {}
│ │ │ │ +
│ │ │ │ +
103
│ │ │ │
107
│ │ │ │ -
115 virtual std::string markdown(
│ │ │ │ -
116 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
117 const Names& names = {}) const = 0;
│ │ │ │ -
118
│ │ │ │ -
126 virtual std::string html(
│ │ │ │ -
127 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
128 const Names& names = {}) const = 0;
│ │ │ │ -
129
│ │ │ │ -
131};
│ │ │ │ -
│ │ │ │ -
132// DiscreteFactor
│ │ │ │ -
133
│ │ │ │ -
134// traits
│ │ │ │ -
135template<> struct traits<DiscreteFactor> : public Testable<DiscreteFactor> {};
│ │ │ │ -
136
│ │ │ │ -
137
│ │ │ │ -
154std::vector<double> expNormalize(const std::vector<double> &logProbs);
│ │ │ │ -
155
│ │ │ │ -
156
│ │ │ │ -
157}// namespace gtsam
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ - │ │ │ │ -
Factor.h
The base class for all factors.
│ │ │ │ +
108 bool equals(const HybridFactor &lf, double tol = 1e-9) const override;
│ │ │ │ +
109
│ │ │ │ +
110 void print(
│ │ │ │ +
111 const std::string &s = "GaussianMixtureFactor\n",
│ │ │ │ +
112 const KeyFormatter &formatter = DefaultKeyFormatter) const override;
│ │ │ │ +
113
│ │ │ │ +
117
│ │ │ │ +
119 sharedFactor operator()(const DiscreteValues &assignment) const;
│ │ │ │ +
120
│ │ │ │ +
129 GaussianFactorGraphTree add(const GaussianFactorGraphTree &sum) const;
│ │ │ │ +
130
│ │ │ │ +
138 AlgebraicDecisionTree<Key> error(const VectorValues &continuousValues) const;
│ │ │ │ +
139
│ │ │ │ +
144 double error(const HybridValues &values) const override;
│ │ │ │ +
145
│ │ │ │ +
│ │ │ │ +
147 friend GaussianFactorGraphTree &operator+=(
│ │ │ │ +
148 GaussianFactorGraphTree &sum, const GaussianMixtureFactor &factor) {
│ │ │ │ +
149 sum = factor.add(sum);
│ │ │ │ +
150 return sum;
│ │ │ │ +
151 }
│ │ │ │ +
│ │ │ │ +
153
│ │ │ │ +
154 private:
│ │ │ │ +
156 friend class boost::serialization::access;
│ │ │ │ +
157 template <class ARCHIVE>
│ │ │ │ +
158 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ +
159 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
160 ar &BOOST_SERIALIZATION_NVP(factors_);
│ │ │ │ +
161 }
│ │ │ │ +
162};
│ │ │ │ +
│ │ │ │ +
163
│ │ │ │ +
164// traits
│ │ │ │ +
165template <>
│ │ │ │ +
│ │ │ │ +
166struct traits<GaussianMixtureFactor> : public Testable<GaussianMixtureFactor> {
│ │ │ │ +
167};
│ │ │ │ +
│ │ │ │ +
168
│ │ │ │ +
169} // namespace gtsam
│ │ │ │ +
AlgebraicDecisionTree.h
Algebraic Decision Trees.
│ │ │ │ +
DecisionTree.h
Decision Tree for use in DiscreteFactors.
│ │ │ │ +
DiscreteKey.h
specialized key for discrete variables
│ │ │ │ + │ │ │ │ +
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ +
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::expNormalize
std::vector< double > expNormalize(const std::vector< double > &logProbs)
Normalize a set of log probabilities.
Definition DiscreteFactor.cpp:42
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::DecisionTreeFactor
A discrete probabilistic factor.
Definition DecisionTreeFactor.h:45
│ │ │ │ -
gtsam::DiscreteFactor
Base class for discrete probabilistic factors The most general one is the derived DecisionTreeFactor.
Definition DiscreteFactor.h:38
│ │ │ │ -
gtsam::DiscreteFactor::html
virtual std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter, const Names &names={}) const =0
Render as html table.
│ │ │ │ -
gtsam::DiscreteFactor::Base
Factor Base
Our base class.
Definition DiscreteFactor.h:45
│ │ │ │ -
gtsam::DiscreteFactor::print
void print(const std::string &s="DiscreteFactor\n", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print
Definition DiscreteFactor.h:74
│ │ │ │ -
gtsam::DiscreteFactor::shared_ptr
boost::shared_ptr< DiscreteFactor > shared_ptr
shared_ptr to this class
Definition DiscreteFactor.h:44
│ │ │ │ -
gtsam::DiscreteFactor::~DiscreteFactor
virtual ~DiscreteFactor()
Virtual destructor.
Definition DiscreteFactor.h:63
│ │ │ │ -
gtsam::DiscreteFactor::Names
DiscreteValues::Names Names
Translation table from values to strings.
Definition DiscreteFactor.h:106
│ │ │ │ -
gtsam::DiscreteFactor::equals
virtual bool equals(const DiscreteFactor &lf, double tol=1e-9) const =0
equals
│ │ │ │ -
gtsam::DiscreteFactor::This
DiscreteFactor This
This class.
Definition DiscreteFactor.h:43
│ │ │ │ -
gtsam::DiscreteFactor::operator*
virtual DecisionTreeFactor operator*(const DecisionTreeFactor &) const =0
Multiply in a DecisionTreeFactor and return the result as DecisionTreeFactor.
│ │ │ │ -
gtsam::DiscreteFactor::operator()
virtual double operator()(const DiscreteValues &) const =0
Find value for given assignment of values to variables.
│ │ │ │ -
gtsam::DiscreteFactor::markdown
virtual std::string markdown(const KeyFormatter &keyFormatter=DefaultKeyFormatter, const Names &names={}) const =0
Render as markdown table.
│ │ │ │ -
gtsam::DiscreteFactor::DiscreteFactor
DiscreteFactor(const CONTAINER &keys)
Construct from container of keys.
Definition DiscreteFactor.h:60
│ │ │ │ -
gtsam::DiscreteFactor::DiscreteFactor
DiscreteFactor()
Default constructor creates empty factor.
Definition DiscreteFactor.h:55
│ │ │ │ +
gtsam::AlgebraicDecisionTree
An algebraic decision tree fixes the range of a DecisionTree to double.
Definition AlgebraicDecisionTree.h:38
│ │ │ │ + │ │ │ │ +
gtsam::DiscreteKeys
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition DiscreteKey.h:39
│ │ │ │
gtsam::DiscreteValues
A map from keys to values.
Definition DiscreteValues.h:34
│ │ │ │ -
gtsam::DiscreteValues::Names
std::map< Key, std::vector< std::string > > Names
Translation table from values to strings.
Definition DiscreteValues.h:95
│ │ │ │ +
gtsam::GaussianMixtureFactor
Implementation of a discrete conditional mixture factor.
Definition GaussianMixtureFactor.h:47
│ │ │ │ +
gtsam::GaussianMixtureFactor::GaussianMixtureFactor
GaussianMixtureFactor()=default
Default constructor, mainly for serialization.
│ │ │ │ +
gtsam::GaussianMixtureFactor::GaussianMixtureFactor
GaussianMixtureFactor(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, const std::vector< sharedFactor > &factors)
Construct a new GaussianMixtureFactor object using a vector of GaussianFactor shared pointers.
Definition GaussianMixtureFactor.h:98
│ │ │ │ +
gtsam::GaussianMixtureFactor::add
GaussianFactorGraphTree add(const GaussianFactorGraphTree &sum) const
Combine the Gaussian Factor Graphs in sum and this while maintaining the original tree structure.
Definition GaussianMixtureFactor.cpp:85
│ │ │ │ +
gtsam::GaussianMixtureFactor::operator+=
friend GaussianFactorGraphTree & operator+=(GaussianFactorGraphTree &sum, const GaussianMixtureFactor &factor)
Add MixtureFactor to a Sum, syntactic sugar.
Definition GaussianMixtureFactor.h:147
│ │ │ │ +
gtsam::HybridFactor
Base class for truly hybrid probabilistic factors.
Definition HybridFactor.h:52
│ │ │ │
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ -
gtsam::Factor
Definition Factor.h:68
│ │ │ │ +
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
DiscreteValues
The Factor::error simply extracts the.
│ │ │ │
HybridValues
the error.
│ │ │ │ +
VectorValues
The Factor::error simply extracts the.
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,196 +1,191 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DiscreteFactor.h │ │ │ │ │ +GaussianMixtureFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_V_a_l_u_e_s_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ -24 │ │ │ │ │ -25#include │ │ │ │ │ -26namespace _g_t_s_a_m { │ │ │ │ │ -27 │ │ │ │ │ -28class DecisionTreeFactor; │ │ │ │ │ -29class DiscreteConditional; │ │ │ │ │ -30class _H_y_b_r_i_d_V_a_l_u_e_s; │ │ │ │ │ +21#pragma once │ │ │ │ │ +22 │ │ │ │ │ +23#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_e_c_i_s_i_o_n_T_r_e_e_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_K_e_y_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_F_a_c_t_o_r_._h> │ │ │ │ │ +27#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ +28#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +29 │ │ │ │ │ +30namespace _g_t_s_a_m { │ │ │ │ │ 31 │ │ │ │ │ -_3_8class GTSAM_EXPORT _D_i_s_c_r_e_t_e_F_a_c_t_o_r: public _F_a_c_t_o_r { │ │ │ │ │ -39 │ │ │ │ │ -40public: │ │ │ │ │ -41 │ │ │ │ │ -42 // typedefs needed to play nice with gtsam │ │ │ │ │ -_4_3 typedef _D_i_s_c_r_e_t_e_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ -_4_4 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -_4_5 typedef _F_a_c_t_o_r _B_a_s_e; │ │ │ │ │ -46 │ │ │ │ │ -_4_7 using _V_a_l_u_e_s = _D_i_s_c_r_e_t_e_V_a_l_u_e_s; │ │ │ │ │ -48 │ │ │ │ │ -49public: │ │ │ │ │ -50 │ │ │ │ │ -53 │ │ │ │ │ -_5_5 _D_i_s_c_r_e_t_e_F_a_c_t_o_r() {} │ │ │ │ │ -56 │ │ │ │ │ -59 template │ │ │ │ │ -_6_0 _D_i_s_c_r_e_t_e_F_a_c_t_o_r(const CONTAINER& keys) : _B_a_s_e(keys) {} │ │ │ │ │ +32class _H_y_b_r_i_d_V_a_l_u_e_s; │ │ │ │ │ +33class _D_i_s_c_r_e_t_e_V_a_l_u_e_s; │ │ │ │ │ +34class _V_e_c_t_o_r_V_a_l_u_e_s; │ │ │ │ │ +35 │ │ │ │ │ +_4_7class GTSAM_EXPORT _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r : public _H_y_b_r_i_d_F_a_c_t_o_r { │ │ │ │ │ +48 public: │ │ │ │ │ +49 using _B_a_s_e = _H_y_b_r_i_d_F_a_c_t_o_r; │ │ │ │ │ +50 using _T_h_i_s = _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r; │ │ │ │ │ +51 using shared_ptr = boost::shared_ptr; │ │ │ │ │ +52 │ │ │ │ │ +53 using sharedFactor = boost::shared_ptr; │ │ │ │ │ +54 │ │ │ │ │ +_5_6 using _F_a_c_t_o_r_s = _D_e_c_i_s_i_o_n_T_r_e_e_<_K_e_y_,_ _s_h_a_r_e_d_F_a_c_t_o_r_>; │ │ │ │ │ +57 │ │ │ │ │ +58 private: │ │ │ │ │ +60 _F_a_c_t_o_r_s factors_; │ │ │ │ │ 61 │ │ │ │ │ -_6_3 virtual _~_D_i_s_c_r_e_t_e_F_a_c_t_o_r() { │ │ │ │ │ -64 } │ │ │ │ │ -65 │ │ │ │ │ +68 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_T_r_e_e asGaussianFactorGraphTree() const; │ │ │ │ │ 69 │ │ │ │ │ -_7_1 virtual bool _e_q_u_a_l_s(const _D_i_s_c_r_e_t_e_F_a_c_t_o_r& lf, double tol = 1e-9) const = 0; │ │ │ │ │ -72 │ │ │ │ │ -_7_4 void _p_r_i_n_t( │ │ │ │ │ -75 const std::string& s = "DiscreteFactor\n", │ │ │ │ │ -76 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override { │ │ │ │ │ -77 Base::print(s, formatter); │ │ │ │ │ -78 } │ │ │ │ │ -79 │ │ │ │ │ -83 │ │ │ │ │ -_8_5 virtual double _o_p_e_r_a_t_o_r_(_)(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s&) const = 0; │ │ │ │ │ -86 │ │ │ │ │ -88 double error(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values) const; │ │ │ │ │ +70 public: │ │ │ │ │ +73 │ │ │ │ │ +_7_5 _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r() = default; │ │ │ │ │ +76 │ │ │ │ │ +86 _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r(const _K_e_y_V_e_c_t_o_r &continuousKeys, │ │ │ │ │ +87 const _D_i_s_c_r_e_t_e_K_e_y_s &discreteKeys, │ │ │ │ │ +88 const _F_a_c_t_o_r_s &factors); │ │ │ │ │ 89 │ │ │ │ │ -94 double error(const _H_y_b_r_i_d_V_a_l_u_e_s& c) const override; │ │ │ │ │ -95 │ │ │ │ │ -_9_7 virtual _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r _o_p_e_r_a_t_o_r_*(const _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r&) const = 0; │ │ │ │ │ -98 │ │ │ │ │ -99 virtual _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r toDecisionTreeFactor() const = 0; │ │ │ │ │ -100 │ │ │ │ │ -104 │ │ │ │ │ -_1_0_6 using _N_a_m_e_s = _D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_N_a_m_e_s; │ │ │ │ │ +_9_8 _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r(const _K_e_y_V_e_c_t_o_r &continuousKeys, │ │ │ │ │ +99 const _D_i_s_c_r_e_t_e_K_e_y_s &discreteKeys, │ │ │ │ │ +100 const std::vector &factors) │ │ │ │ │ +101 : _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r(continuousKeys, discreteKeys, │ │ │ │ │ +102 _F_a_c_t_o_r_s(discreteKeys, factors)) {} │ │ │ │ │ +103 │ │ │ │ │ 107 │ │ │ │ │ -_1_1_5 virtual std::string _m_a_r_k_d_o_w_n( │ │ │ │ │ -116 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ -117 const _N_a_m_e_s& names = {}) const = 0; │ │ │ │ │ -118 │ │ │ │ │ -_1_2_6 virtual std::string _h_t_m_l( │ │ │ │ │ -127 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ -128 const _N_a_m_e_s& names = {}) const = 0; │ │ │ │ │ -129 │ │ │ │ │ -131}; │ │ │ │ │ -132// DiscreteFactor │ │ │ │ │ -133 │ │ │ │ │ -134// traits │ │ │ │ │ -_1_3_5template<> struct _t_r_a_i_t_s<_D_i_s_c_r_e_t_e_F_a_c_t_o_r> : public _T_e_s_t_a_b_l_e │ │ │ │ │ -{}; │ │ │ │ │ -136 │ │ │ │ │ -137 │ │ │ │ │ -154std::vector _e_x_p_N_o_r_m_a_l_i_z_e(const std::vector &logProbs); │ │ │ │ │ -155 │ │ │ │ │ -156 │ │ │ │ │ -157}// namespace gtsam │ │ │ │ │ -_T_e_s_t_a_b_l_e_._h │ │ │ │ │ -Concept check for values that can be used in unit tests. │ │ │ │ │ -_D_i_s_c_r_e_t_e_V_a_l_u_e_s_._h │ │ │ │ │ -_F_a_c_t_o_r_._h │ │ │ │ │ -The base class for all factors. │ │ │ │ │ +108 bool _e_q_u_a_l_s(const _H_y_b_r_i_d_F_a_c_t_o_r &lf, double tol = 1e-9) const override; │ │ │ │ │ +109 │ │ │ │ │ +110 void _p_r_i_n_t( │ │ │ │ │ +111 const std::string &s = "GaussianMixtureFactor\n", │ │ │ │ │ +112 const _K_e_y_F_o_r_m_a_t_t_e_r &formatter = DefaultKeyFormatter) const override; │ │ │ │ │ +113 │ │ │ │ │ +117 │ │ │ │ │ +119 sharedFactor operator()(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s &assignment) const; │ │ │ │ │ +120 │ │ │ │ │ +129 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_T_r_e_e add(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_T_r_e_e &sum) const; │ │ │ │ │ +130 │ │ │ │ │ +138 _A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_<_K_e_y_> error(const _V_e_c_t_o_r_V_a_l_u_e_s &continuousValues) │ │ │ │ │ +const; │ │ │ │ │ +139 │ │ │ │ │ +144 double error(const _H_y_b_r_i_d_V_a_l_u_e_s &values) const override; │ │ │ │ │ +145 │ │ │ │ │ +_1_4_7 friend _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_T_r_e_e &_o_p_e_r_a_t_o_r_+_=( │ │ │ │ │ +148 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_T_r_e_e &sum, const _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r &factor) { │ │ │ │ │ +149 sum = factor._a_d_d(sum); │ │ │ │ │ +150 return sum; │ │ │ │ │ +151 } │ │ │ │ │ +153 │ │ │ │ │ +154 private: │ │ │ │ │ +_1_5_6 friend class boost::serialization::access; │ │ │ │ │ +157 template │ │ │ │ │ +158 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ +159 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +160 ar &BOOST_SERIALIZATION_NVP(factors_); │ │ │ │ │ +161 } │ │ │ │ │ +162}; │ │ │ │ │ +163 │ │ │ │ │ +164// traits │ │ │ │ │ +165template <> │ │ │ │ │ +_1_6_6struct _t_r_a_i_t_s<_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r> : public │ │ │ │ │ +_T_e_s_t_a_b_l_e { │ │ │ │ │ +167}; │ │ │ │ │ +168 │ │ │ │ │ +169} // namespace gtsam │ │ │ │ │ +_A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_._h │ │ │ │ │ +Algebraic Decision Trees. │ │ │ │ │ +_D_e_c_i_s_i_o_n_T_r_e_e_._h │ │ │ │ │ +Decision Tree for use in DiscreteFactors. │ │ │ │ │ +_D_i_s_c_r_e_t_e_K_e_y_._h │ │ │ │ │ +specialized key for discrete variables │ │ │ │ │ +_H_y_b_r_i_d_F_a_c_t_o_r_._h │ │ │ │ │ +_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ +_G_a_u_s_s_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ +A factor with a quadratic error function - a Gaussian. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_e_x_p_N_o_r_m_a_l_i_z_e │ │ │ │ │ -std::vector< double > expNormalize(const std::vector< double > &logProbs) │ │ │ │ │ -Normalize a set of log probabilities. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactor.cpp:42 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ _g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ A helper that implements the traits interface for GTSAM types. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r │ │ │ │ │ -A discrete probabilistic factor. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTreeFactor.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r │ │ │ │ │ -Base class for discrete probabilistic factors The most general one is the │ │ │ │ │ -derived DecisionTreeFactor. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactor.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_h_t_m_l │ │ │ │ │ -virtual std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter, │ │ │ │ │ -const Names &names={}) const =0 │ │ │ │ │ -Render as html table. │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_B_a_s_e │ │ │ │ │ -Factor Base │ │ │ │ │ -Our base class. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactor.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="DiscreteFactor\n", const KeyFormatter │ │ │ │ │ -&formatter=DefaultKeyFormatter) const override │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactor.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< DiscreteFactor > shared_ptr │ │ │ │ │ -shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactor.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_~_D_i_s_c_r_e_t_e_F_a_c_t_o_r │ │ │ │ │ -virtual ~DiscreteFactor() │ │ │ │ │ -Virtual destructor. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactor.h:63 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_N_a_m_e_s │ │ │ │ │ -DiscreteValues::Names Names │ │ │ │ │ -Translation table from values to strings. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactor.h:106 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ -virtual bool equals(const DiscreteFactor &lf, double tol=1e-9) const =0 │ │ │ │ │ -equals │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_T_h_i_s │ │ │ │ │ -DiscreteFactor This │ │ │ │ │ -This class. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactor.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ -virtual DecisionTreeFactor operator*(const DecisionTreeFactor &) const =0 │ │ │ │ │ -Multiply in a DecisionTreeFactor and return the result as DecisionTreeFactor. │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ -virtual double operator()(const DiscreteValues &) const =0 │ │ │ │ │ -Find value for given assignment of values to variables. │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_m_a_r_k_d_o_w_n │ │ │ │ │ -virtual std::string markdown(const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter, const Names &names={}) const =0 │ │ │ │ │ -Render as markdown table. │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r │ │ │ │ │ -DiscreteFactor(const CONTAINER &keys) │ │ │ │ │ -Construct from container of keys. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactor.h:60 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r_:_:_D_i_s_c_r_e_t_e_F_a_c_t_o_r │ │ │ │ │ -DiscreteFactor() │ │ │ │ │ -Default constructor creates empty factor. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteFactor.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e │ │ │ │ │ +An algebraic decision tree fixes the range of a DecisionTree to double. │ │ │ │ │ +DDeeffiinniittiioonn AlgebraicDecisionTree.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_<_ _K_e_y_,_ _s_h_a_r_e_d_F_a_c_t_o_r_ _> │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ +DiscreteKeys is a set of keys that can be assembled using the & operator. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteKey.h:39 │ │ │ │ │ _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ A map from keys to values. │ │ │ │ │ DDeeffiinniittiioonn DiscreteValues.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_N_a_m_e_s │ │ │ │ │ -std::map< Key, std::vector< std::string > > Names │ │ │ │ │ -Translation table from values to strings. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteValues.h:95 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r │ │ │ │ │ +Implementation of a discrete conditional mixture factor. │ │ │ │ │ +DDeeffiinniittiioonn GaussianMixtureFactor.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r_:_:_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r │ │ │ │ │ +GaussianMixtureFactor()=default │ │ │ │ │ +Default constructor, mainly for serialization. │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r_:_:_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r │ │ │ │ │ +GaussianMixtureFactor(const KeyVector &continuousKeys, const DiscreteKeys │ │ │ │ │ +&discreteKeys, const std::vector< sharedFactor > &factors) │ │ │ │ │ +Construct a new GaussianMixtureFactor object using a vector of GaussianFactor │ │ │ │ │ +shared pointers. │ │ │ │ │ +DDeeffiinniittiioonn GaussianMixtureFactor.h:98 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r_:_:_a_d_d │ │ │ │ │ +GaussianFactorGraphTree add(const GaussianFactorGraphTree &sum) const │ │ │ │ │ +Combine the Gaussian Factor Graphs in sum and this while maintaining the │ │ │ │ │ +original tree structure. │ │ │ │ │ +DDeeffiinniittiioonn GaussianMixtureFactor.cpp:85 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r_:_:_o_p_e_r_a_t_o_r_+_= │ │ │ │ │ +friend GaussianFactorGraphTree & operator+=(GaussianFactorGraphTree &sum, const │ │ │ │ │ +GaussianMixtureFactor &factor) │ │ │ │ │ +Add MixtureFactor to a Sum, syntactic sugar. │ │ │ │ │ +DDeeffiinniittiioonn GaussianMixtureFactor.h:147 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r │ │ │ │ │ +Base class for truly hybrid probabilistic factors. │ │ │ │ │ +DDeeffiinniittiioonn HybridFactor.h:52 │ │ │ │ │ _g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:68 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ +with a unique integer... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ +The Factor::error simply extracts the. │ │ │ │ │ _H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ the error. │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +The Factor::error simply extracts the. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _d_i_s_c_r_e_t_e │ │ │ │ │ - * _D_i_s_c_r_e_t_e_F_a_c_t_o_r_._h │ │ │ │ │ + * _h_y_b_r_i_d │ │ │ │ │ + * _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00269_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00224_source.html │ │ │ │┄ Files 99% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
DiscreteMarginals.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
21#pragma once
│ │ │ │
22
│ │ │ │ -
23#include <gtsam/discrete/DiscreteFactorGraph.h>
│ │ │ │ -
24#include <gtsam/discrete/DiscreteBayesTree.h>
│ │ │ │ -
25#include <gtsam/base/Vector.h>
│ │ │ │ +
23#include <gtsam/discrete/DiscreteFactorGraph.h>
│ │ │ │ +
24#include <gtsam/discrete/DiscreteBayesTree.h>
│ │ │ │ +
25#include <gtsam/base/Vector.h>
│ │ │ │
26
│ │ │ │
27namespace gtsam {
│ │ │ │
28
│ │ │ │
│ │ │ │
33class DiscreteMarginals {
│ │ │ │
34
│ │ │ │
35 protected:
│ │ │ │ @@ -167,17 +167,17 @@ │ │ │ │
75 }
│ │ │ │
│ │ │ │
76
│ │ │ │
77 };
│ │ │ │
│ │ │ │
78
│ │ │ │
79} /* namespace gtsam */
│ │ │ │ -
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ -
DiscreteBayesTree.h
Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree.
│ │ │ │ - │ │ │ │ +
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ +
DiscreteBayesTree.h
Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree.
│ │ │ │ + │ │ │ │
gtsam::EliminateDiscrete
std::pair< DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr > EliminateDiscrete(const DiscreteFactorGraph &factors, const Ordering &frontalKeys)
Main elimination function for DiscreteFactorGraph.
Definition DiscreteFactorGraph.cpp:200
│ │ │ │
gtsam::DiscreteKey
std::pair< Key, size_t > DiscreteKey
Key type for discrete variables.
Definition DiscreteKey.h:36
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::DiscreteFactor::shared_ptr
boost::shared_ptr< DiscreteFactor > shared_ptr
shared_ptr to this class
Definition DiscreteFactor.h:44
│ │ │ │
gtsam::DiscreteFactorGraph
A Discrete Factor Graph is a factor graph where all factors are Discrete, i.e.
Definition DiscreteFactorGraph.h:101
│ │ │ │
gtsam::DiscreteMarginals
A class for computing marginals of variables in a DiscreteFactorGraph.
Definition DiscreteMarginals.h:33
│ │ │ │ @@ -186,13 +186,13 @@ │ │ │ │
gtsam::DiscreteMarginals::operator()
DiscreteFactor::shared_ptr operator()(Key variable) const
Compute the marginal of a single variable.
Definition DiscreteMarginals.h:51
│ │ │ │
gtsam::DiscreteValues
A map from keys to values.
Definition DiscreteValues.h:34
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00275_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00392_source.html │ │ │ │┄ Files 99% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
3 * @brief EssentialMatrix class
│ │ │ │
4 * @author Frank Dellaert
│ │ │ │
5 * @date December 17, 2013
│ │ │ │
6 */
│ │ │ │
7
│ │ │ │
8#pragma once
│ │ │ │
9
│ │ │ │ -
10#include <gtsam/geometry/Pose3.h>
│ │ │ │ +
10#include <gtsam/geometry/Pose3.h>
│ │ │ │
11#include <gtsam/geometry/Unit3.h>
│ │ │ │ -
12#include <gtsam/geometry/Point2.h>
│ │ │ │ -
13#include <gtsam/base/Manifold.h>
│ │ │ │ +
12#include <gtsam/geometry/Point2.h>
│ │ │ │ +
13#include <gtsam/base/Manifold.h>
│ │ │ │
14
│ │ │ │
15#include <iosfwd>
│ │ │ │
16#include <string>
│ │ │ │
17
│ │ │ │
18namespace gtsam {
│ │ │ │
19
│ │ │ │
│ │ │ │ @@ -271,30 +271,30 @@ │ │ │ │
196 ar & boost::serialization::make_nvp("E31", E_(2, 0));
│ │ │ │
197 ar & boost::serialization::make_nvp("E32", E_(2, 1));
│ │ │ │
198 ar & boost::serialization::make_nvp("E33", E_(2, 2));
│ │ │ │
199 }
│ │ │ │
200
│ │ │ │
202
│ │ │ │
203 public:
│ │ │ │ -
204 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
204 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │
205};
│ │ │ │
│ │ │ │
206
│ │ │ │
207template<>
│ │ │ │
208struct traits<EssentialMatrix> : public internal::Manifold<EssentialMatrix> {};
│ │ │ │
209
│ │ │ │
210template<>
│ │ │ │
211struct traits<const EssentialMatrix> : public internal::Manifold<EssentialMatrix> {};
│ │ │ │
212
│ │ │ │
213} // namespace gtsam
│ │ │ │
214
│ │ │ │ -
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ -
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ -
Pose3.h
3D Pose
│ │ │ │ -
Point2.h
2D Point
│ │ │ │ +
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ +
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ +
Point2.h
2D Point
│ │ │ │ +
Pose3.h
3D Pose
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │
gtsam::LieGroup::localCoordinates
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g
Definition Lie.h:136
│ │ │ │
gtsam::LieGroup::retract
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Definition Lie.h:131
│ │ │ │
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
│ │ │ │ ├── html2text {} │ │ │ │ │ @@ -167,18 +167,18 @@ │ │ │ │ │ 214 │ │ │ │ │ _M_a_n_i_f_o_l_d_._h │ │ │ │ │ Base class and basic functions for Manifold types. │ │ │ │ │ _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ This marks a GTSAM object to require alignment. │ │ │ │ │ DDeeffiinniittiioonn types.h:308 │ │ │ │ │ -_P_o_s_e_3_._h │ │ │ │ │ -3D Pose │ │ │ │ │ _P_o_i_n_t_2_._h │ │ │ │ │ 2D Point │ │ │ │ │ +_P_o_s_e_3_._h │ │ │ │ │ +3D Pose │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ Vector2 Point2 │ │ │ │ │ As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ typedef Point2 to Vector2... │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00287.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00578.html │ │ │ │┄ Files 88% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SO4.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/LabeledSymbol.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs | │ │ │ │ Functions
│ │ │ │ -
SO4.h File Reference
│ │ │ │ +
LabeledSymbol.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

4*4 matrix representation of SO(4) │ │ │ │ -More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

struct  gtsam::traits< SO4 >
class  gtsam::LabeledSymbol
 Customized version of gtsam::Symbol for multi-robot use. More...
 
struct  gtsam::traits< const SO4 >
struct  gtsam::traits< LabeledSymbol >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Typedefs

│ │ │ │ -using gtsam::SO4 = SO< 4 >
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

GTSAM_EXPORT Matrix3 gtsam::topLeft (const SO4 &Q, OptionalJacobian< 9, 6 > H=boost::none)
 Project to top-left 3*3 matrix.
 
│ │ │ │ -GTSAM_EXPORT Matrix43 gtsam::stiefel (const SO4 &Q, OptionalJacobian< 12, 6 > H=boost::none)
 Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q) -> \( S \in St(3,4) \).
 
│ │ │ │ -template<class Archive >
void gtsam::serialize (Archive &ar, SO4 &Q, const unsigned int)
 Serialization function.
 
Key gtsam::mrsymbol (unsigned char c, unsigned char label, size_t j)
 Create a symbol key from a character, label and index, i.e.
 
│ │ │ │ +unsigned char gtsam::mrsymbolChr (Key key)
 Return the character portion of a symbol key.
 
│ │ │ │ +unsigned char gtsam::mrsymbolLabel (Key key)
 Return the label portion of a symbol key.
 
│ │ │ │ +size_t gtsam::mrsymbolIndex (Key key)
 Return the index portion of a symbol key.
 
│ │ │ │

Detailed Description

│ │ │ │ -

4*4 matrix representation of SO(4)

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +
Date
Jan 12, 2010
│ │ │ │ +
Author
: Alex Cunningham
│ │ │ │ +
Date
Jan 12, 2010
│ │ │ │ +
Author
: Alex Cunningham
│ │ │ │ +
│ │ │ │ +: Frank Dellaert
│ │ │ │
│ │ │ │ -Luca Carlone
│ │ │ │ -
Date
March 2019
│ │ │ │ +: Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,47 +1,47 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -SO4.h File Reference │ │ │ │ │ -4*4 matrix representation of SO(4) _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +LabeledSymbol.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_O_4_ _> │ │ │ │ │ + class   _g_t_s_a_m_:_:_L_a_b_e_l_e_d_S_y_m_b_o_l │ │ │ │ │ +  Customized version of _g_t_s_a_m_:_:_S_y_m_b_o_l for multi-robot use. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _S_O_4_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _L_a_b_e_l_e_d_S_y_m_b_o_l_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -using  ggttssaamm::::SSOO44 = _S_O< 4 > │ │ │ │ │ -  │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ - GTSAM_EXPORT Matrix3  _g_t_s_a_m_:_:_t_o_p_L_e_f_t (const _S_O_4 &Q, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 9, 6 > │ │ │ │ │ - H=boost::none) │ │ │ │ │ -  Project to top-left 3*3 matrix. │ │ │ │ │ -  │ │ │ │ │ -GTSAM_EXPORT Matrix43  ggttssaamm::::ssttiieeffeell (const _S_O_4 &Q, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 12, 6 > │ │ │ │ │ - H=boost::none) │ │ │ │ │ -  Project to Stiefel manifold of 4*3 orthonormal 3-frames │ │ │ │ │ - in R^4, i.e., pi(Q) -> \( S \in St(3,4) \). │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - void  ggttssaamm::::sseerriiaalliizzee (Archive &ar, _S_O_4 &Q, const unsigned │ │ │ │ │ - int) │ │ │ │ │ -  Serialization function. │ │ │ │ │ + _K_e_y  _g_t_s_a_m_:_:_m_r_s_y_m_b_o_l (unsigned char c, unsigned char label, size_t j) │ │ │ │ │ +  Create a symbol key from a character, label and index, i.e. │ │ │ │ │ +  │ │ │ │ │ +unsigned char  ggttssaamm::::mmrrssyymmbboollCChhrr (_K_e_y key) │ │ │ │ │ +  Return the character portion of a symbol key. │ │ │ │ │ +  │ │ │ │ │ +unsigned char  ggttssaamm::::mmrrssyymmbboollLLaabbeell (_K_e_y key) │ │ │ │ │ +  Return the label portion of a symbol key. │ │ │ │ │ +  │ │ │ │ │ + size_t  ggttssaamm::::mmrrssyymmbboollIInnddeexx (_K_e_y key) │ │ │ │ │ +  Return the index portion of a symbol key. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -4*4 matrix representation of SO(4) │ │ │ │ │ + Date │ │ │ │ │ + Jan 12, 2010 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Luca Carlone │ │ │ │ │ + : Alex Cunningham │ │ │ │ │ Date │ │ │ │ │ - March 2019 │ │ │ │ │ + Jan 12, 2010 │ │ │ │ │ + Author │ │ │ │ │ + : Alex Cunningham │ │ │ │ │ + : Frank Dellaert │ │ │ │ │ + : Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_O_4_._h │ │ │ │ │ + * _i_n_f_e_r_e_n_c_e │ │ │ │ │ + * _L_a_b_e_l_e_d_S_y_m_b_o_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00287_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00365_source.html │ │ │ │┄ Files 94% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SO4.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Unified.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
SO4.h
│ │ │ │ +
Cal3Unified.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/geometry/SOn.h>
│ │ │ │ -
23
│ │ │ │ -
24#include <gtsam/base/Group.h>
│ │ │ │ -
25#include <gtsam/base/Lie.h>
│ │ │ │ -
26#include <gtsam/base/Manifold.h>
│ │ │ │ -
27#include <gtsam/base/Matrix.h>
│ │ │ │ -
28#include <gtsam/dllexport.h>
│ │ │ │ +
24#pragma once
│ │ │ │ +
25
│ │ │ │ +
26#include <gtsam/geometry/Cal3DS2_Base.h>
│ │ │ │ +
27
│ │ │ │ +
28namespace gtsam {
│ │ │ │
29
│ │ │ │ -
30#include <string>
│ │ │ │ -
31
│ │ │ │ -
32namespace gtsam {
│ │ │ │ -
33
│ │ │ │ -
34using SO4 = SO<4>;
│ │ │ │ -
35
│ │ │ │ -
36// /// Random SO(4) element (no big claims about uniformity)
│ │ │ │ -
37// static SO4 Random(std::mt19937 &rng);
│ │ │ │ -
38
│ │ │ │ -
39// Below are all declarations of SO<4> specializations.
│ │ │ │ -
40// They are *defined* in SO4.cpp.
│ │ │ │ -
41
│ │ │ │ -
42template <>
│ │ │ │ -
43GTSAM_EXPORT
│ │ │ │ -
44Matrix4 SO4::Hat(const TangentVector &xi);
│ │ │ │ -
45
│ │ │ │ -
46template <>
│ │ │ │ -
47GTSAM_EXPORT
│ │ │ │ -
48Vector6 SO4::Vee(const Matrix4 &X);
│ │ │ │ -
49
│ │ │ │ -
50template <>
│ │ │ │ -
51GTSAM_EXPORT
│ │ │ │ -
52SO4 SO4::Expmap(const Vector6 &xi, ChartJacobian H);
│ │ │ │ -
53
│ │ │ │ -
54template <>
│ │ │ │ -
55GTSAM_EXPORT
│ │ │ │ -
56Matrix6 SO4::AdjointMap() const;
│ │ │ │ +
│ │ │ │ +
45class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
│ │ │ │ +
46 using This = Cal3Unified;
│ │ │ │ +
47 using Base = Cal3DS2_Base;
│ │ │ │ +
48
│ │ │ │ +
49 private:
│ │ │ │ +
50 double xi_ = 0.0f;
│ │ │ │ +
51
│ │ │ │ +
52 public:
│ │ │ │ +
53 enum { dimension = 10 };
│ │ │ │ +
54
│ │ │ │ +
56 using shared_ptr = boost::shared_ptr<Cal3Unified>;
│ │ │ │
57
│ │ │ │ -
58template <>
│ │ │ │ -
59GTSAM_EXPORT
│ │ │ │ -
60SO4::VectorN2 SO4::vec(OptionalJacobian<16, 6> H) const;
│ │ │ │ -
61
│ │ │ │ -
62template <>
│ │ │ │ -
63GTSAM_EXPORT
│ │ │ │ -
64SO4 SO4::ChartAtOrigin::Retract(const Vector6 &omega, ChartJacobian H);
│ │ │ │ -
65
│ │ │ │ -
66template <>
│ │ │ │ -
67GTSAM_EXPORT
│ │ │ │ -
68Vector6 SO4::ChartAtOrigin::Local(const SO4 &Q, ChartJacobian H);
│ │ │ │ +
60
│ │ │ │ +
62 Cal3Unified() = default;
│ │ │ │ +
63
│ │ │ │ +
64 Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1,
│ │ │ │ +
65 double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0)
│ │ │ │ +
66 : Base(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {}
│ │ │ │ +
67
│ │ │ │ +
68 ~Cal3Unified() override {}
│ │ │ │
69
│ │ │ │ -
73GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian<9, 6> H = boost::none);
│ │ │ │ -
74
│ │ │ │ -
79GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian<12, 6> H = boost::none);
│ │ │ │ +
73
│ │ │ │ +
74 Cal3Unified(const Vector10& v)
│ │ │ │ +
75 : Base(v(0), v(1), v(2), v(3), v(4), v(5), v(6), v(7), v(8)), xi_(v(9)) {}
│ │ │ │ +
76
│ │ │ │
80
│ │ │ │ -
82template <class Archive>
│ │ │ │ -
│ │ │ │ -
83void serialize(Archive &ar, SO4 &Q, const unsigned int /*version*/) {
│ │ │ │ -
84 Matrix4 &M = Q.matrix_;
│ │ │ │ -
85 ar &boost::serialization::make_nvp("Q11", M(0, 0));
│ │ │ │ -
86 ar &boost::serialization::make_nvp("Q12", M(0, 1));
│ │ │ │ -
87 ar &boost::serialization::make_nvp("Q13", M(0, 2));
│ │ │ │ -
88 ar &boost::serialization::make_nvp("Q14", M(0, 3));
│ │ │ │ -
89
│ │ │ │ -
90 ar &boost::serialization::make_nvp("Q21", M(1, 0));
│ │ │ │ -
91 ar &boost::serialization::make_nvp("Q22", M(1, 1));
│ │ │ │ -
92 ar &boost::serialization::make_nvp("Q23", M(1, 2));
│ │ │ │ -
93 ar &boost::serialization::make_nvp("Q24", M(1, 3));
│ │ │ │ +
82 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
│ │ │ │ +
83 const Cal3Unified& cal);
│ │ │ │ +
84
│ │ │ │ +
86 void print(const std::string& s = "") const override;
│ │ │ │ +
87
│ │ │ │ +
89 bool equals(const Cal3Unified& K, double tol = 10e-9) const;
│ │ │ │ +
90
│ │ │ │
94
│ │ │ │ -
95 ar &boost::serialization::make_nvp("Q31", M(2, 0));
│ │ │ │ -
96 ar &boost::serialization::make_nvp("Q32", M(2, 1));
│ │ │ │ -
97 ar &boost::serialization::make_nvp("Q33", M(2, 2));
│ │ │ │ -
98 ar &boost::serialization::make_nvp("Q34", M(2, 3));
│ │ │ │ -
99
│ │ │ │ -
100 ar &boost::serialization::make_nvp("Q41", M(3, 0));
│ │ │ │ -
101 ar &boost::serialization::make_nvp("Q42", M(3, 1));
│ │ │ │ -
102 ar &boost::serialization::make_nvp("Q43", M(3, 2));
│ │ │ │ -
103 ar &boost::serialization::make_nvp("Q44", M(3, 3));
│ │ │ │ -
104}
│ │ │ │ -
│ │ │ │ -
105
│ │ │ │ -
106/*
│ │ │ │ -
107 * Define the traits. internal::LieGroup provides both Lie group and Testable
│ │ │ │ -
108 */
│ │ │ │ -
109
│ │ │ │ -
110template <>
│ │ │ │ -
111struct traits<SO4> : public internal::LieGroup<SO4> {};
│ │ │ │ -
112
│ │ │ │ -
113template <>
│ │ │ │ -
114struct traits<const SO4> : public internal::LieGroup<SO4> {};
│ │ │ │ +
96 inline double xi() const { return xi_; }
│ │ │ │ +
97
│ │ │ │ +
99 Vector10 vector() const;
│ │ │ │ +
100
│ │ │ │ +
108 Point2 uncalibrate(const Point2& p,
│ │ │ │ +
109 OptionalJacobian<2, 10> Dcal = boost::none,
│ │ │ │ +
110 OptionalJacobian<2, 2> Dp = boost::none) const;
│ │ │ │ +
111
│ │ │ │ +
113 Point2 calibrate(const Point2& p, OptionalJacobian<2, 10> Dcal = boost::none,
│ │ │ │ +
114 OptionalJacobian<2, 2> Dp = boost::none) const;
│ │ │ │
115
│ │ │ │ -
116} // end namespace gtsam
│ │ │ │ -
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ -
Group.h
Concept check class for variable types with Group properties.
│ │ │ │ -
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ -
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ -
SOn.h
N*N matrix representation of SO(N).
│ │ │ │ +
117 Point2 spaceToNPlane(const Point2& p) const;
│ │ │ │ +
118
│ │ │ │ +
120 Point2 nPlaneToSpace(const Point2& p) const;
│ │ │ │ +
121
│ │ │ │ +
125
│ │ │ │ +
127 Cal3Unified retract(const Vector& d) const;
│ │ │ │ +
128
│ │ │ │ +
130 Vector localCoordinates(const Cal3Unified& T2) const;
│ │ │ │ +
131
│ │ │ │ +
133 size_t dim() const override { return Dim(); }
│ │ │ │ +
134
│ │ │ │ +
136 inline static size_t Dim() { return dimension; }
│ │ │ │ +
137
│ │ │ │ +
139
│ │ │ │ +
140 private:
│ │ │ │ +
142 friend class boost::serialization::access;
│ │ │ │ +
143 template <class Archive>
│ │ │ │ +
144 void serialize(Archive& ar, const unsigned int /*version*/) {
│ │ │ │ +
145 ar& boost::serialization::make_nvp(
│ │ │ │ +
146 "Cal3Unified", boost::serialization::base_object<Cal3DS2_Base>(*this));
│ │ │ │ +
147 ar& BOOST_SERIALIZATION_NVP(xi_);
│ │ │ │ +
148 }
│ │ │ │ +
149};
│ │ │ │ +
│ │ │ │ +
150
│ │ │ │ +
151template <>
│ │ │ │ +
152struct traits<Cal3Unified> : public internal::Manifold<Cal3Unified> {};
│ │ │ │ +
153
│ │ │ │ +
154template <>
│ │ │ │ +
155struct traits<const Cal3Unified> : public internal::Manifold<Cal3Unified> {};
│ │ │ │ +
156}
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition serialization.h:113
│ │ │ │ -
gtsam::stiefel
GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian< 12, 6 > H)
Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q) -> .
Definition SO4.cpp:220
│ │ │ │ -
gtsam::topLeft
GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian< 9, 6 > H)
Project to top-left 3*3 matrix.
Definition SO4.cpp:206
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::LieGroup< SO< N >, internal::DimensionSO(N)>::Retract
static SO< N > Retract(const TangentVector &v)
Retract at origin: possible in Lie group because it has an identity.
Definition Lie.h:111
│ │ │ │ -
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition Lie.h:229
│ │ │ │ -
gtsam::SO
Manifold of special orthogonal rotation matrices SO<N>.
Definition SOn.h:52
│ │ │ │ -
gtsam::SO::Expmap
static SO Expmap(const TangentVector &omega, ChartJacobian H=boost::none)
Exponential map at identity - create a rotation from canonical coordinates.
Definition SOn-inl.h:67
│ │ │ │ -
gtsam::SO::vec
VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost::none) const
Return vectorized rotation matrix in column order.
Definition SOn-inl.h:88
│ │ │ │ -
gtsam::SO::Vee
static TangentVector Vee(const MatrixNN &X)
Inverse of Hat. See note about xi element order in Hat.
Definition SOn-inl.h:35
│ │ │ │ -
gtsam::SO::AdjointMap
MatrixDD AdjointMap() const
Adjoint map.
Definition SO4.cpp:159
│ │ │ │ -
gtsam::SO::Hat
static MatrixNN Hat(const TangentVector &xi)
Hat operator creates Lie algebra element corresponding to d-vector, where d is the dimensionality of ...
Definition SOn-inl.h:29
│ │ │ │ +
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
│ │ │ │ +
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ +
gtsam::Cal3DS2_Base
Calibration of a camera with radial distortion.
Definition Cal3DS2_Base.h:42
│ │ │ │ +
gtsam::Cal3Unified
Calibration of a omni-directional camera with mirror + lens radial distortion.
Definition Cal3Unified.h:45
│ │ │ │ +
gtsam::Cal3Unified::dim
size_t dim() const override
Return dimensions of calibration manifold object.
Definition Cal3Unified.h:133
│ │ │ │ +
gtsam::Cal3Unified::Cal3Unified
Cal3Unified()=default
Default Constructor with only unit focal length.
│ │ │ │ +
gtsam::Cal3Unified::xi
double xi() const
mirror parameter
Definition Cal3Unified.h:96
│ │ │ │ +
gtsam::Cal3Unified::Dim
static size_t Dim()
Return dimensions of calibration manifold object.
Definition Cal3Unified.h:136
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,178 +1,151 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SO4.h │ │ │ │ │ +Cal3Unified.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_S_O_n_._h> │ │ │ │ │ -23 │ │ │ │ │ -24#include <_g_t_s_a_m_/_b_a_s_e_/_G_r_o_u_p_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_b_a_s_e_/_L_i_e_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ -27#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ -28#include │ │ │ │ │ +24#pragma once │ │ │ │ │ +25 │ │ │ │ │ +26#include │ │ │ │ │ +27 │ │ │ │ │ +28namespace _g_t_s_a_m { │ │ │ │ │ 29 │ │ │ │ │ -30#include │ │ │ │ │ -31 │ │ │ │ │ -32namespace _g_t_s_a_m { │ │ │ │ │ -33 │ │ │ │ │ -34using SO4 = SO<4>; │ │ │ │ │ -35 │ │ │ │ │ -36// /// Random SO(4) element (no big claims about uniformity) │ │ │ │ │ -37// static SO4 Random(std::mt19937 &rng); │ │ │ │ │ -38 │ │ │ │ │ -39// Below are all declarations of SO<4> specializations. │ │ │ │ │ -40// They are *defined* in SO4.cpp. │ │ │ │ │ -41 │ │ │ │ │ -42template <> │ │ │ │ │ -43GTSAM_EXPORT │ │ │ │ │ -44Matrix4 _S_O_4_:_:_H_a_t(const TangentVector &xi); │ │ │ │ │ -45 │ │ │ │ │ -46template <> │ │ │ │ │ -47GTSAM_EXPORT │ │ │ │ │ -48Vector6 _S_O_4_:_:_V_e_e(const Matrix4 &X); │ │ │ │ │ -49 │ │ │ │ │ -50template <> │ │ │ │ │ -51GTSAM_EXPORT │ │ │ │ │ -52SO4 _S_O_4_:_:_E_x_p_m_a_p(const Vector6 &xi, ChartJacobian H); │ │ │ │ │ -53 │ │ │ │ │ -54template <> │ │ │ │ │ -55GTSAM_EXPORT │ │ │ │ │ -56Matrix6 _S_O_4_:_:_A_d_j_o_i_n_t_M_a_p() const; │ │ │ │ │ +_4_5class GTSAM_EXPORT _C_a_l_3_U_n_i_f_i_e_d : public _C_a_l_3_D_S_2___B_a_s_e { │ │ │ │ │ +46 using _T_h_i_s = _C_a_l_3_U_n_i_f_i_e_d; │ │ │ │ │ +47 using _B_a_s_e = _C_a_l_3_D_S_2___B_a_s_e; │ │ │ │ │ +48 │ │ │ │ │ +49 private: │ │ │ │ │ +50 double xi_ = 0.0f; │ │ │ │ │ +51 │ │ │ │ │ +52 public: │ │ │ │ │ +53 enum { dimension = 10 }; │ │ │ │ │ +54 │ │ │ │ │ +56 using shared_ptr = boost::shared_ptr; │ │ │ │ │ 57 │ │ │ │ │ -58template <> │ │ │ │ │ -59GTSAM_EXPORT │ │ │ │ │ -60SO4::VectorN2 _S_O_4_:_:_v_e_c(OptionalJacobian<16, 6> H) const; │ │ │ │ │ -61 │ │ │ │ │ -62template <> │ │ │ │ │ -63GTSAM_EXPORT │ │ │ │ │ -64SO4 _S_O_4_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n_:_:_R_e_t_r_a_c_t(const Vector6 &omega, ChartJacobian H); │ │ │ │ │ -65 │ │ │ │ │ -66template <> │ │ │ │ │ -67GTSAM_EXPORT │ │ │ │ │ -68Vector6 SO4::ChartAtOrigin::Local(const SO4 &Q, ChartJacobian H); │ │ │ │ │ +60 │ │ │ │ │ +_6_2 _C_a_l_3_U_n_i_f_i_e_d() = default; │ │ │ │ │ +63 │ │ │ │ │ +64 _C_a_l_3_U_n_i_f_i_e_d(double fx, double fy, double s, double u0, double v0, double k1, │ │ │ │ │ +65 double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0) │ │ │ │ │ +66 : _B_a_s_e(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {} │ │ │ │ │ +67 │ │ │ │ │ +68 _~_C_a_l_3_U_n_i_f_i_e_d() override {} │ │ │ │ │ 69 │ │ │ │ │ -73GTSAM_EXPORT Matrix3 _t_o_p_L_e_f_t(const SO4 &Q, OptionalJacobian<9, 6> H = boost:: │ │ │ │ │ -none); │ │ │ │ │ -74 │ │ │ │ │ -79GTSAM_EXPORT Matrix43 _s_t_i_e_f_e_l(const SO4 &Q, OptionalJacobian<12, 6> H = │ │ │ │ │ -boost::none); │ │ │ │ │ +73 │ │ │ │ │ +74 Cal3Unified(const Vector10& v) │ │ │ │ │ +75 : Base(v(0), v(1), v(2), v(3), v(4), v(5), v(6), v(7), v(8)), xi_(v(9)) {} │ │ │ │ │ +76 │ │ │ │ │ 80 │ │ │ │ │ -82template │ │ │ │ │ -_8_3void _s_e_r_i_a_l_i_z_e(Archive &ar, _S_O_4 &Q, const unsigned int /*version*/) { │ │ │ │ │ -84 Matrix4 &M = Q.matrix_; │ │ │ │ │ -85 ar &boost::serialization::make_nvp("Q11", M(0, 0)); │ │ │ │ │ -86 ar &boost::serialization::make_nvp("Q12", M(0, 1)); │ │ │ │ │ -87 ar &boost::serialization::make_nvp("Q13", M(0, 2)); │ │ │ │ │ -88 ar &boost::serialization::make_nvp("Q14", M(0, 3)); │ │ │ │ │ -89 │ │ │ │ │ -90 ar &boost::serialization::make_nvp("Q21", M(1, 0)); │ │ │ │ │ -91 ar &boost::serialization::make_nvp("Q22", M(1, 1)); │ │ │ │ │ -92 ar &boost::serialization::make_nvp("Q23", M(1, 2)); │ │ │ │ │ -93 ar &boost::serialization::make_nvp("Q24", M(1, 3)); │ │ │ │ │ +82 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, │ │ │ │ │ +83 const Cal3Unified& cal); │ │ │ │ │ +84 │ │ │ │ │ +86 void _p_r_i_n_t(const std::string& s = "") const override; │ │ │ │ │ +87 │ │ │ │ │ +89 bool equals(const Cal3Unified& K, double tol = 10e-9) const; │ │ │ │ │ +90 │ │ │ │ │ 94 │ │ │ │ │ -95 ar &boost::serialization::make_nvp("Q31", M(2, 0)); │ │ │ │ │ -96 ar &boost::serialization::make_nvp("Q32", M(2, 1)); │ │ │ │ │ -97 ar &boost::serialization::make_nvp("Q33", M(2, 2)); │ │ │ │ │ -98 ar &boost::serialization::make_nvp("Q34", M(2, 3)); │ │ │ │ │ -99 │ │ │ │ │ -100 ar &boost::serialization::make_nvp("Q41", M(3, 0)); │ │ │ │ │ -101 ar &boost::serialization::make_nvp("Q42", M(3, 1)); │ │ │ │ │ -102 ar &boost::serialization::make_nvp("Q43", M(3, 2)); │ │ │ │ │ -103 ar &boost::serialization::make_nvp("Q44", M(3, 3)); │ │ │ │ │ -104} │ │ │ │ │ -105 │ │ │ │ │ -106/* │ │ │ │ │ -107 * Define the traits. internal::LieGroup provides both Lie group and │ │ │ │ │ -Testable │ │ │ │ │ -108 */ │ │ │ │ │ -109 │ │ │ │ │ -110template <> │ │ │ │ │ -_1_1_1struct _t_r_a_i_t_s<_S_O_4> : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ -112 │ │ │ │ │ -113template <> │ │ │ │ │ -_1_1_4struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ +_9_6 inline double _x_i() const { return xi_; } │ │ │ │ │ +97 │ │ │ │ │ +99 Vector10 vector() const; │ │ │ │ │ +100 │ │ │ │ │ +108 _P_o_i_n_t_2 uncalibrate(const _P_o_i_n_t_2& p, │ │ │ │ │ +109 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _1_0_> Dcal = boost::none, │ │ │ │ │ +110 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dp = boost::none) const; │ │ │ │ │ +111 │ │ │ │ │ +113 _P_o_i_n_t_2 calibrate(const _P_o_i_n_t_2& p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _1_0_> Dcal = boost:: │ │ │ │ │ +none, │ │ │ │ │ +114 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dp = boost::none) const; │ │ │ │ │ 115 │ │ │ │ │ -116} // end namespace gtsam │ │ │ │ │ -_L_i_e_._h │ │ │ │ │ -Base class and basic functions for Lie types. │ │ │ │ │ -_G_r_o_u_p_._h │ │ │ │ │ -Concept check class for variable types with Group properties. │ │ │ │ │ -_M_a_n_i_f_o_l_d_._h │ │ │ │ │ -Base class and basic functions for Manifold types. │ │ │ │ │ -_M_a_t_r_i_x_._h │ │ │ │ │ -typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ -_S_O_n_._h │ │ │ │ │ -N*N matrix representation of SO(N). │ │ │ │ │ +117 _P_o_i_n_t_2 spaceToNPlane(const _P_o_i_n_t_2& p) const; │ │ │ │ │ +118 │ │ │ │ │ +120 _P_o_i_n_t_2 nPlaneToSpace(const _P_o_i_n_t_2& p) const; │ │ │ │ │ +121 │ │ │ │ │ +125 │ │ │ │ │ +127 _C_a_l_3_U_n_i_f_i_e_d retract(const Vector& d) const; │ │ │ │ │ +128 │ │ │ │ │ +130 Vector localCoordinates(const _C_a_l_3_U_n_i_f_i_e_d& T2) const; │ │ │ │ │ +131 │ │ │ │ │ +_1_3_3 size_t _d_i_m() const override { return Dim(); } │ │ │ │ │ +134 │ │ │ │ │ +_1_3_6 inline static size_t _D_i_m() { return dimension; } │ │ │ │ │ +137 │ │ │ │ │ +139 │ │ │ │ │ +140 private: │ │ │ │ │ +_1_4_2 friend class boost::serialization::access; │ │ │ │ │ +143 template │ │ │ │ │ +144 void serialize(Archive& ar, const unsigned int /*version*/) { │ │ │ │ │ +145 ar& boost::serialization::make_nvp( │ │ │ │ │ +146 "Cal3Unified", boost::serialization::base_object(*this)); │ │ │ │ │ +147 ar& BOOST_SERIALIZATION_NVP(xi_); │ │ │ │ │ +148 } │ │ │ │ │ +149}; │ │ │ │ │ +150 │ │ │ │ │ +151template <> │ │ │ │ │ +_1_5_2struct _t_r_a_i_t_s<_C_a_l_3_U_n_i_f_i_e_d> : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d {}; │ │ │ │ │ +153 │ │ │ │ │ +154template <> │ │ │ │ │ +_1_5_5struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ +{}; │ │ │ │ │ +156} │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e │ │ │ │ │ -std::string serialize(const T &input) │ │ │ │ │ -serializes to a string │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:113 │ │ │ │ │ -_g_t_s_a_m_:_:_s_t_i_e_f_e_l │ │ │ │ │ -GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian< 12, 6 > H) │ │ │ │ │ -Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q) - │ │ │ │ │ -> . │ │ │ │ │ -DDeeffiinniittiioonn SO4.cpp:220 │ │ │ │ │ -_g_t_s_a_m_:_:_t_o_p_L_e_f_t │ │ │ │ │ -GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian< 9, 6 > H) │ │ │ │ │ -Project to top-left 3*3 matrix. │ │ │ │ │ -DDeeffiinniittiioonn SO4.cpp:206 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ +Vector2 Point2 │ │ │ │ │ +As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ +typedef Point2 to Vector2... │ │ │ │ │ +DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_<_ _S_O_<_ _N_ _>_,_ _i_n_t_e_r_n_a_l_:_:_D_i_m_e_n_s_i_o_n_S_O_(_N_)_>_:_:_R_e_t_r_a_c_t │ │ │ │ │ -static SO< N > Retract(const TangentVector &v) │ │ │ │ │ -Retract at origin: possible in Lie group because it has an identity. │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ -Both LieGroupTraits and Testable. │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:229 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O │ │ │ │ │ -Manifold of special orthogonal rotation matrices SO. │ │ │ │ │ -DDeeffiinniittiioonn SOn.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_E_x_p_m_a_p │ │ │ │ │ -static SO Expmap(const TangentVector &omega, ChartJacobian H=boost::none) │ │ │ │ │ -Exponential map at identity - create a rotation from canonical coordinates. │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_v_e_c │ │ │ │ │ -VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost:: │ │ │ │ │ -none) const │ │ │ │ │ -Return vectorized rotation matrix in column order. │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:88 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_V_e_e │ │ │ │ │ -static TangentVector Vee(const MatrixNN &X) │ │ │ │ │ -Inverse of Hat. See note about xi element order in Hat. │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_A_d_j_o_i_n_t_M_a_p │ │ │ │ │ -MatrixDD AdjointMap() const │ │ │ │ │ -Adjoint map. │ │ │ │ │ -DDeeffiinniittiioonn SO4.cpp:159 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_:_:_H_a_t │ │ │ │ │ -static MatrixNN Hat(const TangentVector &xi) │ │ │ │ │ -Hat operator creates Lie algebra element corresponding to d-vector, where d is │ │ │ │ │ -the dimensionality of ... │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:29 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ +Both ManifoldTraits and Testable. │ │ │ │ │ +DDeeffiinniittiioonn Manifold.h:120 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ +either a fixed size o... │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_D_S_2___B_a_s_e │ │ │ │ │ +Calibration of a camera with radial distortion. │ │ │ │ │ +DDeeffiinniittiioonn Cal3DS2_Base.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_U_n_i_f_i_e_d │ │ │ │ │ +Calibration of a omni-directional camera with mirror + lens radial distortion. │ │ │ │ │ +DDeeffiinniittiioonn Cal3Unified.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_U_n_i_f_i_e_d_:_:_d_i_m │ │ │ │ │ +size_t dim() const override │ │ │ │ │ +Return dimensions of calibration manifold object. │ │ │ │ │ +DDeeffiinniittiioonn Cal3Unified.h:133 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_U_n_i_f_i_e_d_:_:_C_a_l_3_U_n_i_f_i_e_d │ │ │ │ │ +Cal3Unified()=default │ │ │ │ │ +Default Constructor with only unit focal length. │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_U_n_i_f_i_e_d_:_:_x_i │ │ │ │ │ +double xi() const │ │ │ │ │ +mirror parameter │ │ │ │ │ +DDeeffiinniittiioonn Cal3Unified.h:96 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3_U_n_i_f_i_e_d_:_:_D_i_m │ │ │ │ │ +static size_t Dim() │ │ │ │ │ +Return dimensions of calibration manifold object. │ │ │ │ │ +DDeeffiinniittiioonn Cal3Unified.h:136 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_O_4_._h │ │ │ │ │ + * _C_a_l_3_U_n_i_f_i_e_d_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00299.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00782.html │ │ │ │┄ Files 88% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SimpleCamera.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/IterativeSolver.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Typedefs
│ │ │ │ -
SimpleCamera.h File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
IterativeSolver.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

A simple camera class with a Cal3_S2 calibration. │ │ │ │ +

Some support classes for iterative solvers. │ │ │ │ More...

│ │ │ │ │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::IterativeOptimizationParameters
 parameters for iterative linear solvers More...
 
class  gtsam::IterativeSolver
 Base class for Iterative Solvers like SubgraphSolver. More...
 
struct  gtsam::KeyInfoEntry
 Handy data structure for iterative solvers key to (index, dimension, start) More...
 
class  gtsam::KeyInfo
 Handy data structure for iterative solvers. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Typedefs

using gtsam::PinholeCameraCal3_S2 = gtsam::PinholeCamera< gtsam::Cal3_S2 >
 Convenient aliases for Pinhole camera classes with different calibrations.
 
│ │ │ │ -using gtsam::PinholeCameraCal3Bundler = gtsam::PinholeCamera< gtsam::Cal3Bundler >
 
│ │ │ │ -using gtsam::PinholeCameraCal3DS2 = gtsam::PinholeCamera< gtsam::Cal3DS2 >
 
│ │ │ │ -using gtsam::PinholeCameraCal3Unified = gtsam::PinholeCamera< gtsam::Cal3Unified >
 
│ │ │ │ -using gtsam::PinholeCameraCal3Fisheye = gtsam::PinholeCamera< gtsam::Cal3Fisheye >
 
│ │ │ │

Detailed Description

│ │ │ │ -

A simple camera class with a Cal3_S2 calibration.

│ │ │ │ -
Date
Aug 16, 2009
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +

Some support classes for iterative solvers.

│ │ │ │ +
Date
2010
│ │ │ │ +
Author
Yong-Dian Jian
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,39 +1,37 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ -SimpleCamera.h File Reference │ │ │ │ │ -A simple camera class with a Cal3_S2 calibration. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +IterativeSolver.h File Reference │ │ │ │ │ +Some support classes for iterative solvers. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_I_t_e_r_a_t_i_v_e_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ +  parameters for iterative linear solvers _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_I_t_e_r_a_t_i_v_e_S_o_l_v_e_r │ │ │ │ │ +  Base class for Iterative Solvers like _S_u_b_g_r_a_p_h_S_o_l_v_e_r. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_K_e_y_I_n_f_o_E_n_t_r_y │ │ │ │ │ +  Handy data structure for iterative solvers key to (index, dimension, │ │ │ │ │ + start) _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_K_e_y_I_n_f_o │ │ │ │ │ +  Handy data structure for iterative solvers. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -using  _g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_C_a_l_3___S_2 = _g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a< _g_t_s_a_m_:_:_C_a_l_3___S_2 > │ │ │ │ │ -  Convenient aliases for Pinhole camera classes with different │ │ │ │ │ - calibrations. │ │ │ │ │ -  │ │ │ │ │ -using  ggttssaamm::::PPiinnhhoolleeCCaammeerraaCCaall33BBuunnddlleerr = _g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a< _g_t_s_a_m_:_: │ │ │ │ │ - _C_a_l_3_B_u_n_d_l_e_r > │ │ │ │ │ -  │ │ │ │ │ -using  ggttssaamm::::PPiinnhhoolleeCCaammeerraaCCaall33DDSS22 = _g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a< _g_t_s_a_m_:_:_C_a_l_3_D_S_2 > │ │ │ │ │ -  │ │ │ │ │ -using  ggttssaamm::::PPiinnhhoolleeCCaammeerraaCCaall33UUnniiffiieedd = _g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a< _g_t_s_a_m_:_: │ │ │ │ │ - _C_a_l_3_U_n_i_f_i_e_d > │ │ │ │ │ -  │ │ │ │ │ -using  ggttssaamm::::PPiinnhhoolleeCCaammeerraaCCaall33FFiisshheeyyee = _g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a< _g_t_s_a_m_:_: │ │ │ │ │ - _C_a_l_3_F_i_s_h_e_y_e > │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A simple camera class with a Cal3_S2 calibration. │ │ │ │ │ +Some support classes for iterative solvers. │ │ │ │ │ Date │ │ │ │ │ - Aug 16, 2009 │ │ │ │ │ + 2010 │ │ │ │ │ Author │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Yong-Dian Jian │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_i_m_p_l_e_C_a_m_e_r_a_._h │ │ │ │ │ + * _l_i_n_e_a_r │ │ │ │ │ + * _I_t_e_r_a_t_i_v_e_S_o_l_v_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00299_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00434_source.html │ │ │ │┄ Files 99% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
SimpleCamera.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/geometry/BearingRange.h>
│ │ │ │ -
22#include <gtsam/geometry/Cal3Bundler.h>
│ │ │ │ -
23#include <gtsam/geometry/Cal3DS2.h>
│ │ │ │ -
24#include <gtsam/geometry/Cal3Fisheye.h>
│ │ │ │ -
25#include <gtsam/geometry/Cal3Unified.h>
│ │ │ │ -
26#include <gtsam/geometry/Cal3Fisheye.h>
│ │ │ │ -
27#include <gtsam/geometry/Cal3_S2.h>
│ │ │ │ -
28#include <gtsam/geometry/PinholeCamera.h>
│ │ │ │ +
21#include <gtsam/geometry/BearingRange.h>
│ │ │ │ +
22#include <gtsam/geometry/Cal3Bundler.h>
│ │ │ │ +
23#include <gtsam/geometry/Cal3DS2.h>
│ │ │ │ +
24#include <gtsam/geometry/Cal3Fisheye.h>
│ │ │ │ +
25#include <gtsam/geometry/Cal3Unified.h>
│ │ │ │ +
26#include <gtsam/geometry/Cal3Fisheye.h>
│ │ │ │ +
27#include <gtsam/geometry/Cal3_S2.h>
│ │ │ │ +
28#include <gtsam/geometry/PinholeCamera.h>
│ │ │ │
29
│ │ │ │
30namespace gtsam {
│ │ │ │
31
│ │ │ │
34 using PinholeCameraCal3_S2 = gtsam::PinholeCamera<gtsam::Cal3_S2>;
│ │ │ │
35 using PinholeCameraCal3Bundler = gtsam::PinholeCamera<gtsam::Cal3Bundler>;
│ │ │ │
36 using PinholeCameraCal3DS2 = gtsam::PinholeCamera<gtsam::Cal3DS2>;
│ │ │ │
37 using PinholeCameraCal3Unified = gtsam::PinholeCamera<gtsam::Cal3Unified>;
│ │ │ │ @@ -207,32 +207,32 @@ │ │ │ │
150// range traits, used in RangeFactor
│ │ │ │
151template <typename T>
│ │ │ │
152struct Range<SimpleCamera, T> : HasRange<SimpleCamera, T, double> {};
│ │ │ │
153
│ │ │ │
154#endif
│ │ │ │
155
│ │ │ │
156} // namespace gtsam
│ │ │ │ -
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
│ │ │ │ -
Cal3Unified.h
Unified Calibration Model, see Mei07icra for details.
│ │ │ │ -
Cal3Bundler.h
Calibration used by Bundler.
│ │ │ │ -
PinholeCamera.h
Base class for all pinhole cameras.
│ │ │ │ -
Cal3Fisheye.h
Calibration of a fisheye camera.
│ │ │ │ -
BearingRange.h
Bearing-Range product.
│ │ │ │ -
Cal3DS2.h
Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base.
│ │ │ │ +
Cal3DS2.h
Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base.
│ │ │ │ +
Cal3Unified.h
Unified Calibration Model, see Mei07icra for details.
│ │ │ │ +
Cal3Bundler.h
Calibration used by Bundler.
│ │ │ │ +
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
│ │ │ │ +
Cal3Fisheye.h
Calibration of a fisheye camera.
│ │ │ │ +
PinholeCamera.h
Base class for all pinhole cameras.
│ │ │ │ +
BearingRange.h
Bearing-Range product.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::PinholeCameraCal3_S2
gtsam::PinholeCamera< gtsam::Cal3_S2 > PinholeCameraCal3_S2
Convenient aliases for Pinhole camera classes with different calibrations.
Definition SimpleCamera.h:34
│ │ │ │
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition Cal3_S2.h:34
│ │ │ │
gtsam::PinholeCamera
A pinhole camera class that has a Pose3 and a Calibration.
Definition PinholeCamera.h:33
│ │ │ │
gtsam::Pose2
A 2D pose (Point2,Rot2)
Definition Pose2.h:36
│ │ │ │
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -111,29 +111,29 @@ │ │ │ │ │ 150// range traits, used in RangeFactor │ │ │ │ │ 151template │ │ │ │ │ 152struct Range : HasRange {}; │ │ │ │ │ 153 │ │ │ │ │ 154#endif │ │ │ │ │ 155 │ │ │ │ │ 156} // namespace gtsam │ │ │ │ │ -_C_a_l_3___S_2_._h │ │ │ │ │ -The most common 5DOF 3D->2D calibration. │ │ │ │ │ +_C_a_l_3_D_S_2_._h │ │ │ │ │ +Calibration of a camera with radial distortion, calculations in base class │ │ │ │ │ +Cal3DS2_Base. │ │ │ │ │ _C_a_l_3_U_n_i_f_i_e_d_._h │ │ │ │ │ Unified Calibration Model, see Mei07icra for details. │ │ │ │ │ _C_a_l_3_B_u_n_d_l_e_r_._h │ │ │ │ │ Calibration used by Bundler. │ │ │ │ │ -_P_i_n_h_o_l_e_C_a_m_e_r_a_._h │ │ │ │ │ -Base class for all pinhole cameras. │ │ │ │ │ +_C_a_l_3___S_2_._h │ │ │ │ │ +The most common 5DOF 3D->2D calibration. │ │ │ │ │ _C_a_l_3_F_i_s_h_e_y_e_._h │ │ │ │ │ Calibration of a fisheye camera. │ │ │ │ │ +_P_i_n_h_o_l_e_C_a_m_e_r_a_._h │ │ │ │ │ +Base class for all pinhole cameras. │ │ │ │ │ _B_e_a_r_i_n_g_R_a_n_g_e_._h │ │ │ │ │ Bearing-Range product. │ │ │ │ │ -_C_a_l_3_D_S_2_._h │ │ │ │ │ -Calibration of a camera with radial distortion, calculations in base class │ │ │ │ │ -Cal3DS2_Base. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_C_a_l_3___S_2 │ │ │ │ │ gtsam::PinholeCamera< gtsam::Cal3_S2 > PinholeCameraCal3_S2 │ │ │ │ │ Convenient aliases for Pinhole camera classes with different calibrations. │ │ │ │ │ DDeeffiinniittiioonn SimpleCamera.h:34 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00305_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00716_source.html │ │ │ │┄ Files 94% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Unified.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/PowerMethod.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Cal3Unified.h
│ │ │ │ +
PowerMethod.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
24#pragma once
│ │ │ │ -
25
│ │ │ │ -
26#include <gtsam/geometry/Cal3DS2_Base.h>
│ │ │ │ -
27
│ │ │ │ -
28namespace gtsam {
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/base/Matrix.h>
│ │ │ │ +
23#include <gtsam/base/Vector.h>
│ │ │ │ +
24
│ │ │ │ +
25#include <Eigen/Core>
│ │ │ │ +
26#include <Eigen/Sparse>
│ │ │ │ +
27#include <random>
│ │ │ │ +
28#include <vector>
│ │ │ │
29
│ │ │ │ -
│ │ │ │ -
45class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
│ │ │ │ -
46 using This = Cal3Unified;
│ │ │ │ -
47 using Base = Cal3DS2_Base;
│ │ │ │ -
48
│ │ │ │ -
49 private:
│ │ │ │ -
50 double xi_ = 0.0f;
│ │ │ │ -
51
│ │ │ │ -
52 public:
│ │ │ │ -
53 enum { dimension = 10 };
│ │ │ │ -
54
│ │ │ │ -
56 using shared_ptr = boost::shared_ptr<Cal3Unified>;
│ │ │ │ -
57
│ │ │ │ -
60
│ │ │ │ -
62 Cal3Unified() = default;
│ │ │ │ -
63
│ │ │ │ -
64 Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1,
│ │ │ │ -
65 double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0)
│ │ │ │ -
66 : Base(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {}
│ │ │ │ -
67
│ │ │ │ -
68 ~Cal3Unified() override {}
│ │ │ │ -
69
│ │ │ │ -
73
│ │ │ │ -
74 Cal3Unified(const Vector10& v)
│ │ │ │ -
75 : Base(v(0), v(1), v(2), v(3), v(4), v(5), v(6), v(7), v(8)), xi_(v(9)) {}
│ │ │ │ -
76
│ │ │ │ -
80
│ │ │ │ -
82 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
│ │ │ │ -
83 const Cal3Unified& cal);
│ │ │ │ -
84
│ │ │ │ -
86 void print(const std::string& s = "") const override;
│ │ │ │ -
87
│ │ │ │ -
89 bool equals(const Cal3Unified& K, double tol = 10e-9) const;
│ │ │ │ +
30namespace gtsam {
│ │ │ │ +
31
│ │ │ │ +
32using Sparse = Eigen::SparseMatrix<double>;
│ │ │ │ +
33
│ │ │ │ +
56template <class Operator>
│ │ │ │ +
│ │ │ │ +
57class PowerMethod {
│ │ │ │ +
58 protected:
│ │ │ │ +
63 const Operator &A_;
│ │ │ │ +
64
│ │ │ │ +
65 const int dim_; // dimension of Matrix A
│ │ │ │ +
66
│ │ │ │ +
67 size_t nrIterations_; // number of iterations
│ │ │ │ +
68
│ │ │ │ +
69 double ritzValue_; // Ritz eigenvalue
│ │ │ │ +
70 Vector ritzVector_; // Ritz eigenvector
│ │ │ │ +
71
│ │ │ │ +
72 public:
│ │ │ │ +
75
│ │ │ │ +
│ │ │ │ +
77 explicit PowerMethod(const Operator &A,
│ │ │ │ +
78 const boost::optional<Vector> initial = boost::none)
│ │ │ │ +
79 : A_(A), dim_(A.rows()), nrIterations_(0) {
│ │ │ │ +
80 Vector x0;
│ │ │ │ +
81 x0 = initial ? initial.get() : Vector::Random(dim_);
│ │ │ │ +
82 x0.normalize();
│ │ │ │ +
83
│ │ │ │ +
84 // initialize Ritz eigen value
│ │ │ │ +
85 ritzValue_ = 0.0;
│ │ │ │ +
86
│ │ │ │ +
87 // initialize Ritz eigen vector
│ │ │ │ +
88 ritzVector_ = powerIteration(x0);
│ │ │ │ +
89 }
│ │ │ │ +
│ │ │ │
90
│ │ │ │ -
94
│ │ │ │ -
96 inline double xi() const { return xi_; }
│ │ │ │ -
97
│ │ │ │ -
99 Vector10 vector() const;
│ │ │ │ +
│ │ │ │ +
95 Vector powerIteration(const Vector &x) const {
│ │ │ │ +
96 Vector y = A_ * x;
│ │ │ │ +
97 y.normalize();
│ │ │ │ +
98 return y;
│ │ │ │ +
99 }
│ │ │ │ +
│ │ │ │
100
│ │ │ │ -
108 Point2 uncalibrate(const Point2& p,
│ │ │ │ -
109 OptionalJacobian<2, 10> Dcal = boost::none,
│ │ │ │ -
110 OptionalJacobian<2, 2> Dp = boost::none) const;
│ │ │ │ -
111
│ │ │ │ -
113 Point2 calibrate(const Point2& p, OptionalJacobian<2, 10> Dcal = boost::none,
│ │ │ │ -
114 OptionalJacobian<2, 2> Dp = boost::none) const;
│ │ │ │ -
115
│ │ │ │ -
117 Point2 spaceToNPlane(const Point2& p) const;
│ │ │ │ -
118
│ │ │ │ -
120 Point2 nPlaneToSpace(const Point2& p) const;
│ │ │ │ -
121
│ │ │ │ -
125
│ │ │ │ -
127 Cal3Unified retract(const Vector& d) const;
│ │ │ │ -
128
│ │ │ │ -
130 Vector localCoordinates(const Cal3Unified& T2) const;
│ │ │ │ -
131
│ │ │ │ -
133 size_t dim() const override { return Dim(); }
│ │ │ │ -
134
│ │ │ │ -
136 inline static size_t Dim() { return dimension; }
│ │ │ │ -
137
│ │ │ │ -
139
│ │ │ │ -
140 private:
│ │ │ │ -
142 friend class boost::serialization::access;
│ │ │ │ -
143 template <class Archive>
│ │ │ │ -
144 void serialize(Archive& ar, const unsigned int /*version*/) {
│ │ │ │ -
145 ar& boost::serialization::make_nvp(
│ │ │ │ -
146 "Cal3Unified", boost::serialization::base_object<Cal3DS2_Base>(*this));
│ │ │ │ -
147 ar& BOOST_SERIALIZATION_NVP(xi_);
│ │ │ │ -
148 }
│ │ │ │ -
149};
│ │ │ │ -
│ │ │ │ -
150
│ │ │ │ -
151template <>
│ │ │ │ -
152struct traits<Cal3Unified> : public internal::Manifold<Cal3Unified> {};
│ │ │ │ -
153
│ │ │ │ -
154template <>
│ │ │ │ -
155struct traits<const Cal3Unified> : public internal::Manifold<Cal3Unified> {};
│ │ │ │ -
156}
│ │ │ │ +
105 Vector powerIteration() const { return powerIteration(ritzVector_); }
│ │ │ │ +
106
│ │ │ │ +
│ │ │ │ +
112 bool converged(double tol) const {
│ │ │ │ +
113 const Vector x = ritzVector_;
│ │ │ │ +
114 // store the Ritz eigen value
│ │ │ │ +
115 const double ritzValue = x.dot(A_ * x);
│ │ │ │ +
116 const double error = (A_ * x - ritzValue * x).norm();
│ │ │ │ +
117 return error < tol;
│ │ │ │ +
118 }
│ │ │ │ +
│ │ │ │ +
119
│ │ │ │ +
121 size_t nrIterations() const { return nrIterations_; }
│ │ │ │ +
122
│ │ │ │ +
│ │ │ │ +
129 bool compute(size_t maxIterations, double tol) {
│ │ │ │ +
130 // Starting
│ │ │ │ +
131 bool isConverged = false;
│ │ │ │ +
132
│ │ │ │ +
133 for (size_t i = 0; i < maxIterations && !isConverged; i++) {
│ │ │ │ +
134 ++nrIterations_;
│ │ │ │ +
135 // update the ritzVector after power iteration
│ │ │ │ +
136 ritzVector_ = powerIteration();
│ │ │ │ +
137 // update the ritzValue
│ │ │ │ +
138 ritzValue_ = ritzVector_.dot(A_ * ritzVector_);
│ │ │ │ +
139 isConverged = converged(tol);
│ │ │ │ +
140 }
│ │ │ │ +
141
│ │ │ │ +
142 return isConverged;
│ │ │ │ +
143 }
│ │ │ │ +
│ │ │ │ +
144
│ │ │ │ +
146 double eigenvalue() const { return ritzValue_; }
│ │ │ │ +
147
│ │ │ │ +
149 Vector eigenvector() const { return ritzVector_; }
│ │ │ │ +
150};
│ │ │ │ +
│ │ │ │ +
151
│ │ │ │ +
152} // namespace gtsam
│ │ │ │ +
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ +
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
│ │ │ │ -
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
gtsam::Cal3DS2_Base
Calibration of a camera with radial distortion.
Definition Cal3DS2_Base.h:42
│ │ │ │ -
gtsam::Cal3Unified
Calibration of a omni-directional camera with mirror + lens radial distortion.
Definition Cal3Unified.h:45
│ │ │ │ -
gtsam::Cal3Unified::dim
size_t dim() const override
Return dimensions of calibration manifold object.
Definition Cal3Unified.h:133
│ │ │ │ -
gtsam::Cal3Unified::Cal3Unified
Cal3Unified()=default
Default Constructor with only unit focal length.
│ │ │ │ -
gtsam::Cal3Unified::xi
double xi() const
mirror parameter
Definition Cal3Unified.h:96
│ │ │ │ -
gtsam::Cal3Unified::Dim
static size_t Dim()
Return dimensions of calibration manifold object.
Definition Cal3Unified.h:136
│ │ │ │ +
gtsam::PowerMethod
Compute maximum Eigenpair with power method.
Definition PowerMethod.h:57
│ │ │ │ +
gtsam::PowerMethod::powerIteration
Vector powerIteration(const Vector &x) const
Run power iteration to get ritzVector with previous ritzVector x, and return A * x / || A * x ||.
Definition PowerMethod.h:95
│ │ │ │ +
gtsam::PowerMethod::eigenvector
Vector eigenvector() const
Return the eigenvector.
Definition PowerMethod.h:149
│ │ │ │ +
gtsam::PowerMethod::A_
const Operator & A_
Const reference to an externally-held matrix whose minimum-eigenvalue we want to compute.
Definition PowerMethod.h:63
│ │ │ │ +
gtsam::PowerMethod::eigenvalue
double eigenvalue() const
Return the eigenvalue.
Definition PowerMethod.h:146
│ │ │ │ +
gtsam::PowerMethod::PowerMethod
PowerMethod(const Operator &A, const boost::optional< Vector > initial=boost::none)
Construct from the aim matrix and intial ritz vector.
Definition PowerMethod.h:77
│ │ │ │ +
gtsam::PowerMethod::powerIteration
Vector powerIteration() const
Run power iteration to get ritzVector with previous ritzVector x, and return A * x / || A * x ||.
Definition PowerMethod.h:105
│ │ │ │ +
gtsam::PowerMethod::converged
bool converged(double tol) const
After Perform power iteration on a single Ritz value, check if the Ritz residual for the current Ritz...
Definition PowerMethod.h:112
│ │ │ │ +
gtsam::PowerMethod::compute
bool compute(size_t maxIterations, double tol)
Start the power/accelerated iteration, after performing the power/accelerated iteration,...
Definition PowerMethod.h:129
│ │ │ │ +
gtsam::PowerMethod::nrIterations
size_t nrIterations() const
Return the number of iterations.
Definition PowerMethod.h:121
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,151 +1,158 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Cal3Unified.h │ │ │ │ │ +PowerMethod.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -24#pragma once │ │ │ │ │ -25 │ │ │ │ │ -26#include │ │ │ │ │ -27 │ │ │ │ │ -28namespace _g_t_s_a_m { │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_c_t_o_r_._h> │ │ │ │ │ +24 │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28#include │ │ │ │ │ 29 │ │ │ │ │ -_4_5class GTSAM_EXPORT _C_a_l_3_U_n_i_f_i_e_d : public _C_a_l_3_D_S_2___B_a_s_e { │ │ │ │ │ -46 using _T_h_i_s = _C_a_l_3_U_n_i_f_i_e_d; │ │ │ │ │ -47 using _B_a_s_e = _C_a_l_3_D_S_2___B_a_s_e; │ │ │ │ │ -48 │ │ │ │ │ -49 private: │ │ │ │ │ -50 double xi_ = 0.0f; │ │ │ │ │ -51 │ │ │ │ │ -52 public: │ │ │ │ │ -53 enum { dimension = 10 }; │ │ │ │ │ -54 │ │ │ │ │ -56 using shared_ptr = boost::shared_ptr; │ │ │ │ │ -57 │ │ │ │ │ -60 │ │ │ │ │ -_6_2 _C_a_l_3_U_n_i_f_i_e_d() = default; │ │ │ │ │ -63 │ │ │ │ │ -64 _C_a_l_3_U_n_i_f_i_e_d(double fx, double fy, double s, double u0, double v0, double k1, │ │ │ │ │ -65 double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0) │ │ │ │ │ -66 : _B_a_s_e(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {} │ │ │ │ │ -67 │ │ │ │ │ -68 _~_C_a_l_3_U_n_i_f_i_e_d() override {} │ │ │ │ │ -69 │ │ │ │ │ -73 │ │ │ │ │ -74 Cal3Unified(const Vector10& v) │ │ │ │ │ -75 : Base(v(0), v(1), v(2), v(3), v(4), v(5), v(6), v(7), v(8)), xi_(v(9)) {} │ │ │ │ │ -76 │ │ │ │ │ -80 │ │ │ │ │ -82 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, │ │ │ │ │ -83 const Cal3Unified& cal); │ │ │ │ │ -84 │ │ │ │ │ -86 void _p_r_i_n_t(const std::string& s = "") const override; │ │ │ │ │ -87 │ │ │ │ │ -89 bool equals(const Cal3Unified& K, double tol = 10e-9) const; │ │ │ │ │ +30namespace _g_t_s_a_m { │ │ │ │ │ +31 │ │ │ │ │ +32using Sparse = Eigen::SparseMatrix; │ │ │ │ │ +33 │ │ │ │ │ +56template │ │ │ │ │ +_5_7class _P_o_w_e_r_M_e_t_h_o_d { │ │ │ │ │ +58 protected: │ │ │ │ │ +_6_3 const Operator &_A__; │ │ │ │ │ +64 │ │ │ │ │ +65 const int dim_; // dimension of Matrix A │ │ │ │ │ +66 │ │ │ │ │ +67 size_t nrIterations_; // number of iterations │ │ │ │ │ +68 │ │ │ │ │ +69 double ritzValue_; // Ritz eigenvalue │ │ │ │ │ +70 Vector ritzVector_; // Ritz eigenvector │ │ │ │ │ +71 │ │ │ │ │ +72 public: │ │ │ │ │ +75 │ │ │ │ │ +_7_7 explicit _P_o_w_e_r_M_e_t_h_o_d(const Operator &A, │ │ │ │ │ +78 const boost::optional initial = boost::none) │ │ │ │ │ +79 : _A__(A), dim_(A.rows()), nrIterations_(0) { │ │ │ │ │ +80 Vector x0; │ │ │ │ │ +81 x0 = initial ? initial.get() : Vector::Random(dim_); │ │ │ │ │ +82 x0.normalize(); │ │ │ │ │ +83 │ │ │ │ │ +84 // initialize Ritz eigen value │ │ │ │ │ +85 ritzValue_ = 0.0; │ │ │ │ │ +86 │ │ │ │ │ +87 // initialize Ritz eigen vector │ │ │ │ │ +88 ritzVector_ = _p_o_w_e_r_I_t_e_r_a_t_i_o_n(x0); │ │ │ │ │ +89 } │ │ │ │ │ 90 │ │ │ │ │ -94 │ │ │ │ │ -_9_6 inline double _x_i() const { return xi_; } │ │ │ │ │ -97 │ │ │ │ │ -99 Vector10 vector() const; │ │ │ │ │ +_9_5 Vector _p_o_w_e_r_I_t_e_r_a_t_i_o_n(const Vector &x) const { │ │ │ │ │ +96 Vector y = _A__ * x; │ │ │ │ │ +97 y.normalize(); │ │ │ │ │ +98 return y; │ │ │ │ │ +99 } │ │ │ │ │ 100 │ │ │ │ │ -108 _P_o_i_n_t_2 uncalibrate(const _P_o_i_n_t_2& p, │ │ │ │ │ -109 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _1_0_> Dcal = boost::none, │ │ │ │ │ -110 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dp = boost::none) const; │ │ │ │ │ -111 │ │ │ │ │ -113 _P_o_i_n_t_2 calibrate(const _P_o_i_n_t_2& p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _1_0_> Dcal = boost:: │ │ │ │ │ -none, │ │ │ │ │ -114 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dp = boost::none) const; │ │ │ │ │ -115 │ │ │ │ │ -117 _P_o_i_n_t_2 spaceToNPlane(const _P_o_i_n_t_2& p) const; │ │ │ │ │ -118 │ │ │ │ │ -120 _P_o_i_n_t_2 nPlaneToSpace(const _P_o_i_n_t_2& p) const; │ │ │ │ │ -121 │ │ │ │ │ -125 │ │ │ │ │ -127 _C_a_l_3_U_n_i_f_i_e_d retract(const Vector& d) const; │ │ │ │ │ -128 │ │ │ │ │ -130 Vector localCoordinates(const _C_a_l_3_U_n_i_f_i_e_d& T2) const; │ │ │ │ │ -131 │ │ │ │ │ -_1_3_3 size_t _d_i_m() const override { return Dim(); } │ │ │ │ │ -134 │ │ │ │ │ -_1_3_6 inline static size_t _D_i_m() { return dimension; } │ │ │ │ │ -137 │ │ │ │ │ -139 │ │ │ │ │ -140 private: │ │ │ │ │ -_1_4_2 friend class boost::serialization::access; │ │ │ │ │ -143 template │ │ │ │ │ -144 void serialize(Archive& ar, const unsigned int /*version*/) { │ │ │ │ │ -145 ar& boost::serialization::make_nvp( │ │ │ │ │ -146 "Cal3Unified", boost::serialization::base_object(*this)); │ │ │ │ │ -147 ar& BOOST_SERIALIZATION_NVP(xi_); │ │ │ │ │ -148 } │ │ │ │ │ -149}; │ │ │ │ │ -150 │ │ │ │ │ -151template <> │ │ │ │ │ -_1_5_2struct _t_r_a_i_t_s<_C_a_l_3_U_n_i_f_i_e_d> : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d {}; │ │ │ │ │ -153 │ │ │ │ │ -154template <> │ │ │ │ │ -_1_5_5struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ -{}; │ │ │ │ │ -156} │ │ │ │ │ +_1_0_5 Vector _p_o_w_e_r_I_t_e_r_a_t_i_o_n() const { return _p_o_w_e_r_I_t_e_r_a_t_i_o_n(ritzVector_); } │ │ │ │ │ +106 │ │ │ │ │ +_1_1_2 bool _c_o_n_v_e_r_g_e_d(double tol) const { │ │ │ │ │ +113 const Vector x = ritzVector_; │ │ │ │ │ +114 // store the Ritz eigen value │ │ │ │ │ +115 const double ritzValue = x.dot(_A__ * x); │ │ │ │ │ +116 const double error = (_A__ * x - ritzValue * x).norm(); │ │ │ │ │ +117 return error < tol; │ │ │ │ │ +118 } │ │ │ │ │ +119 │ │ │ │ │ +_1_2_1 size_t _n_r_I_t_e_r_a_t_i_o_n_s() const { return nrIterations_; } │ │ │ │ │ +122 │ │ │ │ │ +_1_2_9 bool _c_o_m_p_u_t_e(size_t maxIterations, double tol) { │ │ │ │ │ +130 // Starting │ │ │ │ │ +131 bool isConverged = false; │ │ │ │ │ +132 │ │ │ │ │ +133 for (size_t i = 0; i < maxIterations && !isConverged; i++) { │ │ │ │ │ +134 ++nrIterations_; │ │ │ │ │ +135 // update the ritzVector after power iteration │ │ │ │ │ +136 ritzVector_ = _p_o_w_e_r_I_t_e_r_a_t_i_o_n(); │ │ │ │ │ +137 // update the ritzValue │ │ │ │ │ +138 ritzValue_ = ritzVector_.dot(_A__ * ritzVector_); │ │ │ │ │ +139 isConverged = _c_o_n_v_e_r_g_e_d(tol); │ │ │ │ │ +140 } │ │ │ │ │ +141 │ │ │ │ │ +142 return isConverged; │ │ │ │ │ +143 } │ │ │ │ │ +144 │ │ │ │ │ +_1_4_6 double _e_i_g_e_n_v_a_l_u_e() const { return ritzValue_; } │ │ │ │ │ +147 │ │ │ │ │ +_1_4_9 Vector _e_i_g_e_n_v_e_c_t_o_r() const { return ritzVector_; } │ │ │ │ │ +150}; │ │ │ │ │ +151 │ │ │ │ │ +152} // namespace gtsam │ │ │ │ │ +_V_e_c_t_o_r_._h │ │ │ │ │ +typedef and functions to augment Eigen's VectorXd │ │ │ │ │ +_M_a_t_r_i_x_._h │ │ │ │ │ +typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ -Vector2 Point2 │ │ │ │ │ -As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point2 to Vector2... │ │ │ │ │ -DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ -Both ManifoldTraits and Testable. │ │ │ │ │ -DDeeffiinniittiioonn Manifold.h:120 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ -either a fixed size o... │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_D_S_2___B_a_s_e │ │ │ │ │ -Calibration of a camera with radial distortion. │ │ │ │ │ -DDeeffiinniittiioonn Cal3DS2_Base.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_U_n_i_f_i_e_d │ │ │ │ │ -Calibration of a omni-directional camera with mirror + lens radial distortion. │ │ │ │ │ -DDeeffiinniittiioonn Cal3Unified.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_U_n_i_f_i_e_d_:_:_d_i_m │ │ │ │ │ -size_t dim() const override │ │ │ │ │ -Return dimensions of calibration manifold object. │ │ │ │ │ -DDeeffiinniittiioonn Cal3Unified.h:133 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_U_n_i_f_i_e_d_:_:_C_a_l_3_U_n_i_f_i_e_d │ │ │ │ │ -Cal3Unified()=default │ │ │ │ │ -Default Constructor with only unit focal length. │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_U_n_i_f_i_e_d_:_:_x_i │ │ │ │ │ -double xi() const │ │ │ │ │ -mirror parameter │ │ │ │ │ -DDeeffiinniittiioonn Cal3Unified.h:96 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_U_n_i_f_i_e_d_:_:_D_i_m │ │ │ │ │ -static size_t Dim() │ │ │ │ │ -Return dimensions of calibration manifold object. │ │ │ │ │ -DDeeffiinniittiioonn Cal3Unified.h:136 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_w_e_r_M_e_t_h_o_d │ │ │ │ │ +Compute maximum Eigenpair with power method. │ │ │ │ │ +DDeeffiinniittiioonn PowerMethod.h:57 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_w_e_r_M_e_t_h_o_d_:_:_p_o_w_e_r_I_t_e_r_a_t_i_o_n │ │ │ │ │ +Vector powerIteration(const Vector &x) const │ │ │ │ │ +Run power iteration to get ritzVector with previous ritzVector x, and return A │ │ │ │ │ +* x / || A * x ||. │ │ │ │ │ +DDeeffiinniittiioonn PowerMethod.h:95 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_w_e_r_M_e_t_h_o_d_:_:_e_i_g_e_n_v_e_c_t_o_r │ │ │ │ │ +Vector eigenvector() const │ │ │ │ │ +Return the eigenvector. │ │ │ │ │ +DDeeffiinniittiioonn PowerMethod.h:149 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_w_e_r_M_e_t_h_o_d_:_:_A__ │ │ │ │ │ +const Operator & A_ │ │ │ │ │ +Const reference to an externally-held matrix whose minimum-eigenvalue we want │ │ │ │ │ +to compute. │ │ │ │ │ +DDeeffiinniittiioonn PowerMethod.h:63 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_w_e_r_M_e_t_h_o_d_:_:_e_i_g_e_n_v_a_l_u_e │ │ │ │ │ +double eigenvalue() const │ │ │ │ │ +Return the eigenvalue. │ │ │ │ │ +DDeeffiinniittiioonn PowerMethod.h:146 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_w_e_r_M_e_t_h_o_d_:_:_P_o_w_e_r_M_e_t_h_o_d │ │ │ │ │ +PowerMethod(const Operator &A, const boost::optional< Vector > initial=boost:: │ │ │ │ │ +none) │ │ │ │ │ +Construct from the aim matrix and intial ritz vector. │ │ │ │ │ +DDeeffiinniittiioonn PowerMethod.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_w_e_r_M_e_t_h_o_d_:_:_p_o_w_e_r_I_t_e_r_a_t_i_o_n │ │ │ │ │ +Vector powerIteration() const │ │ │ │ │ +Run power iteration to get ritzVector with previous ritzVector x, and return A │ │ │ │ │ +* x / || A * x ||. │ │ │ │ │ +DDeeffiinniittiioonn PowerMethod.h:105 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_w_e_r_M_e_t_h_o_d_:_:_c_o_n_v_e_r_g_e_d │ │ │ │ │ +bool converged(double tol) const │ │ │ │ │ +After Perform power iteration on a single Ritz value, check if the Ritz │ │ │ │ │ +residual for the current Ritz... │ │ │ │ │ +DDeeffiinniittiioonn PowerMethod.h:112 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_w_e_r_M_e_t_h_o_d_:_:_c_o_m_p_u_t_e │ │ │ │ │ +bool compute(size_t maxIterations, double tol) │ │ │ │ │ +Start the power/accelerated iteration, after performing the power/accelerated │ │ │ │ │ +iteration,... │ │ │ │ │ +DDeeffiinniittiioonn PowerMethod.h:129 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_w_e_r_M_e_t_h_o_d_:_:_n_r_I_t_e_r_a_t_i_o_n_s │ │ │ │ │ +size_t nrIterations() const │ │ │ │ │ +Return the number of iterations. │ │ │ │ │ +DDeeffiinniittiioonn PowerMethod.h:121 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3_U_n_i_f_i_e_d_._h │ │ │ │ │ + * _l_i_n_e_a_r │ │ │ │ │ + * _P_o_w_e_r_M_e_t_h_o_d_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00335_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00770_source.html │ │ │ │┄ Files 92% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/PinholeCamera.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianFactorGraph.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
PinholeCamera.h
│ │ │ │ +
GaussianFactorGraph.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/geometry/PinholePose.h>
│ │ │ │ -
22#include <gtsam/geometry/BearingRange.h>
│ │ │ │ +
22#pragma once
│ │ │ │
23
│ │ │ │ -
24namespace gtsam {
│ │ │ │ -
25
│ │ │ │ -
32template<typename Calibration>
│ │ │ │ -
│ │ │ │ -
33class PinholeCamera: public PinholeBaseK<Calibration> {
│ │ │ │ -
34
│ │ │ │ -
35public:
│ │ │ │ -
36
│ │ │ │ -
41 typedef Point2 Measurement;
│ │ │ │ -
42 typedef Point2Vector MeasurementVector;
│ │ │ │ -
43
│ │ │ │ -
44private:
│ │ │ │ -
45
│ │ │ │ -
46 typedef PinholeBaseK<Calibration> Base;
│ │ │ │ -
47 Calibration K_;
│ │ │ │ -
48
│ │ │ │ -
49 // Get dimensions of calibration type at compile time
│ │ │ │ -
50 static const int DimK = FixedDimension<Calibration>::value;
│ │ │ │ -
51
│ │ │ │ -
52public:
│ │ │ │ -
53
│ │ │ │ -
54 enum {
│ │ │ │ -
55 dimension = 6 + DimK
│ │ │ │ -
56 };
│ │ │ │ -
57
│ │ │ │ -
60
│ │ │ │ -
│ │ │ │ -
62 PinholeCamera() {
│ │ │ │ -
63 }
│ │ │ │ +
24#include <gtsam/inference/EliminateableFactorGraph.h>
│ │ │ │ +
25#include <gtsam/inference/FactorGraph.h>
│ │ │ │ +
26#include <gtsam/linear/Errors.h> // Included here instead of fw-declared so we can use Errors::iterator
│ │ │ │ +
27#include <gtsam/linear/GaussianFactor.h>
│ │ │ │ +
28#include <gtsam/linear/HessianFactor.h>
│ │ │ │ +
29#include <gtsam/linear/JacobianFactor.h>
│ │ │ │ +
30#include <gtsam/linear/VectorValues.h>
│ │ │ │ +
31
│ │ │ │ +
32namespace gtsam {
│ │ │ │ +
33
│ │ │ │ +
34 // Forward declarations
│ │ │ │ +
35 class GaussianFactorGraph;
│ │ │ │ +
36 class GaussianFactor;
│ │ │ │ +
37 class GaussianConditional;
│ │ │ │ +
38 class GaussianBayesNet;
│ │ │ │ +
39 class GaussianEliminationTree;
│ │ │ │ +
40 class GaussianBayesTree;
│ │ │ │ +
41 class GaussianJunctionTree;
│ │ │ │ +
42
│ │ │ │ +
43 /* ************************************************************************* */
│ │ │ │ +
│ │ │ │ +
44 template<> struct EliminationTraits<GaussianFactorGraph>
│ │ │ │ +
45 {
│ │ │ │ +
46 typedef GaussianFactor FactorType;
│ │ │ │ +
47 typedef GaussianFactorGraph FactorGraphType;
│ │ │ │ +
48 typedef GaussianConditional ConditionalType;
│ │ │ │ +
49 typedef GaussianBayesNet BayesNetType;
│ │ │ │ +
50 typedef GaussianEliminationTree EliminationTreeType;
│ │ │ │ +
51 typedef GaussianBayesTree BayesTreeType;
│ │ │ │ +
52 typedef GaussianJunctionTree JunctionTreeType;
│ │ │ │ +
54 static std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> >
│ │ │ │ +
│ │ │ │ +
55 DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) {
│ │ │ │ +
56 return EliminatePreferCholesky(factors, keys); }
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
58 static Ordering DefaultOrderingFunc(
│ │ │ │ +
59 const FactorGraphType& graph,
│ │ │ │ +
60 boost::optional<const VariableIndex&> variableIndex) {
│ │ │ │ +
61 return Ordering::Colamd(*variableIndex);
│ │ │ │ +
62 }
│ │ │ │
│ │ │ │ -
64
│ │ │ │ -
│ │ │ │ -
66 explicit PinholeCamera(const Pose3& pose) :
│ │ │ │ -
67 Base(pose) {
│ │ │ │ -
68 }
│ │ │ │ -
│ │ │ │ -
69
│ │ │ │ -
│ │ │ │ -
71 PinholeCamera(const Pose3& pose, const Calibration& K) :
│ │ │ │ -
72 Base(pose), K_(K) {
│ │ │ │ -
73 }
│ │ │ │ -
│ │ │ │ -
74
│ │ │ │ -
78
│ │ │ │ -
│ │ │ │ -
86 static PinholeCamera Level(const Calibration &K, const Pose2& pose2,
│ │ │ │ -
87 double height) {
│ │ │ │ -
88 return PinholeCamera(Base::LevelPose(pose2, height), K);
│ │ │ │ -
89 }
│ │ │ │ -
│ │ │ │ -
90
│ │ │ │ -
│ │ │ │ -
92 static PinholeCamera Level(const Pose2& pose2, double height) {
│ │ │ │ -
93 return PinholeCamera::Level(Calibration(), pose2, height);
│ │ │ │ -
94 }
│ │ │ │ -
│ │ │ │ -
95
│ │ │ │ -
│ │ │ │ -
105 static PinholeCamera Lookat(const Point3& eye, const Point3& target,
│ │ │ │ -
106 const Point3& upVector, const Calibration& K = Calibration()) {
│ │ │ │ -
107 return PinholeCamera(Base::LookatPose(eye, target, upVector), K);
│ │ │ │ -
108 }
│ │ │ │ -
│ │ │ │ -
109
│ │ │ │ -
110 // Create PinholeCamera, with derivatives
│ │ │ │ -
111 static PinholeCamera Create(const Pose3& pose, const Calibration &K,
│ │ │ │ -
112 OptionalJacobian<dimension, 6> H1 = boost::none, //
│ │ │ │ -
113 OptionalJacobian<dimension, DimK> H2 = boost::none) {
│ │ │ │ -
114 typedef Eigen::Matrix<double, DimK, 6> MatrixK6;
│ │ │ │ -
115 if (H1)
│ │ │ │ -
116 *H1 << I_6x6, MatrixK6::Zero();
│ │ │ │ -
117 typedef Eigen::Matrix<double, 6, DimK> Matrix6K;
│ │ │ │ -
118 typedef Eigen::Matrix<double, DimK, DimK> MatrixK;
│ │ │ │ -
119 if (H2)
│ │ │ │ -
120 *H2 << Matrix6K::Zero(), MatrixK::Identity();
│ │ │ │ -
121 return PinholeCamera(pose,K);
│ │ │ │ -
122 }
│ │ │ │ -
123
│ │ │ │ -
127
│ │ │ │ -
│ │ │ │ -
129 explicit PinholeCamera(const Vector &v) :
│ │ │ │ -
130 Base(v.head<6>()) {
│ │ │ │ -
131 if (v.size() > 6)
│ │ │ │ -
132 K_ = Calibration(v.tail<DimK>());
│ │ │ │ -
133 }
│ │ │ │ -
│ │ │ │ -
134
│ │ │ │ -
│ │ │ │ -
136 PinholeCamera(const Vector &v, const Vector &K) :
│ │ │ │ -
137 Base(v), K_(K) {
│ │ │ │ -
138 }
│ │ │ │ -
│ │ │ │ -
139
│ │ │ │ -
143
│ │ │ │ -
│ │ │ │ -
145 bool equals(const Base &camera, double tol = 1e-9) const {
│ │ │ │ -
146 const PinholeCamera* e = dynamic_cast<const PinholeCamera*>(&camera);
│ │ │ │ -
147 return Base::equals(camera, tol) && K_.equals(e->calibration(), tol);
│ │ │ │ -
148 }
│ │ │ │ -
│ │ │ │ -
149
│ │ │ │ -
│ │ │ │ -
151 void print(const std::string& s = "PinholeCamera") const override {
│ │ │ │ -
152 Base::print(s);
│ │ │ │ -
153 K_.print(s + ".calibration");
│ │ │ │ -
154 }
│ │ │ │ -
│ │ │ │ -
155
│ │ │ │ -
159
│ │ │ │ -
160 ~PinholeCamera() override {
│ │ │ │ -
161 }
│ │ │ │ -
162
│ │ │ │ -
│ │ │ │ -
164 const Pose3& pose() const {
│ │ │ │ -
165 return Base::pose();
│ │ │ │ -
166 }
│ │ │ │ -
│ │ │ │ -
167
│ │ │ │ -
│ │ │ │ -
169 const Pose3& getPose(OptionalJacobian<6, dimension> H) const {
│ │ │ │ -
170 if (H) {
│ │ │ │ -
171 H->setZero();
│ │ │ │ -
172 H->template block<6, 6>(0, 0) = I_6x6;
│ │ │ │ -
173 }
│ │ │ │ -
174 return Base::pose();
│ │ │ │ -
175 }
│ │ │ │ -
│ │ │ │ -
176
│ │ │ │ -
│ │ │ │ -
178 const Calibration& calibration() const override {
│ │ │ │ -
179 return K_;
│ │ │ │ -
180 }
│ │ │ │ +
63 };
│ │ │ │
│ │ │ │ +
64
│ │ │ │ +
65 /* ************************************************************************* */
│ │ │ │ +
│ │ │ │ +
72 class GTSAM_EXPORT GaussianFactorGraph :
│ │ │ │ +
73 public FactorGraph<GaussianFactor>,
│ │ │ │ +
74 public EliminateableFactorGraph<GaussianFactorGraph>
│ │ │ │ +
75 {
│ │ │ │ +
76 public:
│ │ │ │ +
77
│ │ │ │ +
78 typedef GaussianFactorGraph This;
│ │ │ │ +
79 typedef FactorGraph<GaussianFactor> Base;
│ │ │ │ +
80 typedef EliminateableFactorGraph<This> BaseEliminateable;
│ │ │ │ +
81 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
82
│ │ │ │ +
85
│ │ │ │ +
87 GaussianFactorGraph() {}
│ │ │ │ +
88
│ │ │ │ +
94 GaussianFactorGraph(std::initializer_list<sharedFactor> factors) : Base(factors) {}
│ │ │ │ +
95
│ │ │ │ +
96
│ │ │ │ +
98 template<typename ITERATOR>
│ │ │ │ +
99 GaussianFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {}
│ │ │ │ +
100
│ │ │ │ +
102 template<class CONTAINER>
│ │ │ │ +
103 explicit GaussianFactorGraph(const CONTAINER& factors) : Base(factors) {}
│ │ │ │ +
104
│ │ │ │ +
106 template<class DERIVEDFACTOR>
│ │ │ │ +
107 GaussianFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
│ │ │ │ +
108
│ │ │ │ +
110 virtual ~GaussianFactorGraph() {}
│ │ │ │ +
111
│ │ │ │ +
115
│ │ │ │ +
116 bool equals(const This& fg, double tol = 1e-9) const;
│ │ │ │ +
117
│ │ │ │ +
119
│ │ │ │ +
│ │ │ │ +
121 friend bool operator==(const GaussianFactorGraph& lhs,
│ │ │ │ +
122 const GaussianFactorGraph& rhs) {
│ │ │ │ +
123 return lhs.isEqual(rhs);
│ │ │ │ +
124 }
│ │ │ │ +
│ │ │ │ +
125
│ │ │ │ +
127 void add(const GaussianFactor& factor) { push_back(factor.clone()); }
│ │ │ │ +
128
│ │ │ │ +
130 void add(const sharedFactor& factor) { push_back(factor); }
│ │ │ │ +
131
│ │ │ │ +
│ │ │ │ +
133 void add(const Vector& b) {
│ │ │ │ +
134 add(JacobianFactor(b)); }
│ │ │ │ +
│ │ │ │ +
135
│ │ │ │ +
│ │ │ │ +
137 void add(Key key1, const Matrix& A1,
│ │ │ │ +
138 const Vector& b, const SharedDiagonal& model = SharedDiagonal()) {
│ │ │ │ +
139 add(JacobianFactor(key1,A1,b,model)); }
│ │ │ │ +
│ │ │ │ +
140
│ │ │ │ +
│ │ │ │ +
142 void add(Key key1, const Matrix& A1,
│ │ │ │ +
143 Key key2, const Matrix& A2,
│ │ │ │ +
144 const Vector& b, const SharedDiagonal& model = SharedDiagonal()) {
│ │ │ │ +
145 add(JacobianFactor(key1,A1,key2,A2,b,model)); }
│ │ │ │ +
│ │ │ │ +
146
│ │ │ │ +
│ │ │ │ +
148 void add(Key key1, const Matrix& A1,
│ │ │ │ +
149 Key key2, const Matrix& A2,
│ │ │ │ +
150 Key key3, const Matrix& A3,
│ │ │ │ +
151 const Vector& b, const SharedDiagonal& model = SharedDiagonal()) {
│ │ │ │ +
152 add(JacobianFactor(key1,A1,key2,A2,key3,A3,b,model)); }
│ │ │ │ +
│ │ │ │ +
153
│ │ │ │ +
155 template<class TERMS>
│ │ │ │ +
│ │ │ │ +
156 void add(const TERMS& terms, const Vector &b, const SharedDiagonal& model = SharedDiagonal()) {
│ │ │ │ +
157 add(JacobianFactor(terms,b,model)); }
│ │ │ │ +
│ │ │ │ +
158
│ │ │ │ +
163 typedef KeySet Keys;
│ │ │ │ +
164 Keys keys() const;
│ │ │ │ +
165
│ │ │ │ +
166 /* return a map of (Key, dimension) */
│ │ │ │ +
167 std::map<Key, size_t> getKeyDimMap() const;
│ │ │ │ +
168
│ │ │ │ +
170 double error(const VectorValues& x) const;
│ │ │ │ +
171
│ │ │ │ +
173 double probPrime(const VectorValues& c) const;
│ │ │ │ +
174
│ │ │ │ +
180 virtual GaussianFactorGraph clone() const;
│ │ │ │
181
│ │ │ │ -
185
│ │ │ │ -
│ │ │ │ -
187 size_t dim() const {
│ │ │ │ -
188 return dimension;
│ │ │ │ -
189 }
│ │ │ │ -
│ │ │ │ -
190
│ │ │ │ -
│ │ │ │ -
192 static size_t Dim() {
│ │ │ │ -
193 return dimension;
│ │ │ │ -
194 }
│ │ │ │ -
│ │ │ │ +
186 virtual GaussianFactorGraph::shared_ptr cloneToPtr() const;
│ │ │ │ +
187
│ │ │ │ +
194 GaussianFactorGraph negate() const;
│ │ │ │
195
│ │ │ │ -
196 typedef Eigen::Matrix<double, dimension, 1> VectorK6;
│ │ │ │ -
197
│ │ │ │ -
│ │ │ │ -
199 PinholeCamera retract(const Vector& d) const {
│ │ │ │ -
200 if ((size_t) d.size() == 6)
│ │ │ │ -
201 return PinholeCamera(this->pose().retract(d), calibration());
│ │ │ │ -
202 else
│ │ │ │ -
203 return PinholeCamera(this->pose().retract(d.head<6>()),
│ │ │ │ -
204 calibration().retract(d.tail(calibration().dim())));
│ │ │ │ -
205 }
│ │ │ │ -
│ │ │ │ -
206
│ │ │ │ -
│ │ │ │ -
208 VectorK6 localCoordinates(const PinholeCamera& T2) const {
│ │ │ │ -
209 VectorK6 d;
│ │ │ │ -
210 d.template head<6>() = this->pose().localCoordinates(T2.pose());
│ │ │ │ -
211 d.template tail<DimK>() = calibration().localCoordinates(T2.calibration());
│ │ │ │ -
212 return d;
│ │ │ │ -
213 }
│ │ │ │ -
│ │ │ │ +
198
│ │ │ │ +
209 std::vector<std::tuple<int, int, double> > sparseJacobian(
│ │ │ │ +
210 const Ordering& ordering, size_t& nrows, size_t& ncols) const;
│ │ │ │ +
211
│ │ │ │ +
213 std::vector<std::tuple<int, int, double> > sparseJacobian() const;
│ │ │ │
214
│ │ │ │ -
│ │ │ │ -
216 static PinholeCamera Identity() {
│ │ │ │ -
217 return PinholeCamera(); // assumes that the default constructor is valid
│ │ │ │ -
218 }
│ │ │ │ -
│ │ │ │ -
219
│ │ │ │ -
223
│ │ │ │ -
224 typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
│ │ │ │ -
225
│ │ │ │ -
229 template<class POINT>
│ │ │ │ -
│ │ │ │ -
230 Point2 _project2(const POINT& pw, OptionalJacobian<2, dimension> Dcamera,
│ │ │ │ -
231 OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint) const {
│ │ │ │ -
232 // We just call 3-derivative version in Base
│ │ │ │ -
233 if (Dcamera){
│ │ │ │ -
234 Matrix26 Dpose;
│ │ │ │ -
235 Eigen::Matrix<double, 2, DimK> Dcal;
│ │ │ │ -
236 const Point2 pi = Base::project(pw, Dpose, Dpoint, Dcal);
│ │ │ │ -
237 *Dcamera << Dpose, Dcal;
│ │ │ │ -
238 return pi;
│ │ │ │ -
239 } else {
│ │ │ │ -
240 return Base::project(pw, boost::none, Dpoint, boost::none);
│ │ │ │ -
241 }
│ │ │ │ -
242 }
│ │ │ │ -
│ │ │ │ -
243
│ │ │ │ -
│ │ │ │ -
245 Point2 project2(const Point3& pw, OptionalJacobian<2, dimension> Dcamera =
│ │ │ │ -
246 boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const {
│ │ │ │ -
247 return _project2(pw, Dcamera, Dpoint);
│ │ │ │ -
248 }
│ │ │ │ -
│ │ │ │ +
221 Matrix sparseJacobian_() const;
│ │ │ │ +
222
│ │ │ │ +
230 Matrix augmentedJacobian(const Ordering& ordering) const;
│ │ │ │ +
231
│ │ │ │ +
239 Matrix augmentedJacobian() const;
│ │ │ │ +
240
│ │ │ │ +
248 std::pair<Matrix,Vector> jacobian(const Ordering& ordering) const;
│ │ │ │
249
│ │ │ │ -
│ │ │ │ -
251 Point2 project2(const Unit3& pw, OptionalJacobian<2, dimension> Dcamera =
│ │ │ │ -
252 boost::none, OptionalJacobian<2, 2> Dpoint = boost::none) const {
│ │ │ │ -
253 return _project2(pw, Dcamera, Dpoint);
│ │ │ │ -
254 }
│ │ │ │ -
│ │ │ │ -
255
│ │ │ │ -
│ │ │ │ -
261 double range(const Point3& point, OptionalJacobian<1, dimension> Dcamera =
│ │ │ │ -
262 boost::none, OptionalJacobian<1, 3> Dpoint = boost::none) const {
│ │ │ │ -
263 Matrix16 Dpose_;
│ │ │ │ -
264 double result = this->pose().range(point, Dcamera ? &Dpose_ : 0, Dpoint);
│ │ │ │ -
265 if (Dcamera)
│ │ │ │ -
266 *Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
│ │ │ │ -
267 return result;
│ │ │ │ -
268 }
│ │ │ │ -
│ │ │ │ -
269
│ │ │ │ -
│ │ │ │ -
275 double range(const Pose3& pose, OptionalJacobian<1, dimension> Dcamera =
│ │ │ │ -
276 boost::none, OptionalJacobian<1, 6> Dpose = boost::none) const {
│ │ │ │ -
277 Matrix16 Dpose_;
│ │ │ │ -
278 double result = this->pose().range(pose, Dcamera ? &Dpose_ : 0, Dpose);
│ │ │ │ -
279 if (Dcamera)
│ │ │ │ -
280 *Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
│ │ │ │ -
281 return result;
│ │ │ │ -
282 }
│ │ │ │ -
│ │ │ │ -
283
│ │ │ │ -
289 template<class CalibrationB>
│ │ │ │ -
│ │ │ │ -
290 double range(const PinholeCamera<CalibrationB>& camera,
│ │ │ │ -
291 OptionalJacobian<1, dimension> Dcamera = boost::none,
│ │ │ │ -
292 OptionalJacobian<1, 6 + CalibrationB::dimension> Dother = boost::none) const {
│ │ │ │ -
293 Matrix16 Dcamera_, Dother_;
│ │ │ │ -
294 double result = this->pose().range(camera.pose(), Dcamera ? &Dcamera_ : 0,
│ │ │ │ -
295 Dother ? &Dother_ : 0);
│ │ │ │ -
296 if (Dcamera) {
│ │ │ │ -
297 *Dcamera << Dcamera_, Eigen::Matrix<double, 1, DimK>::Zero();
│ │ │ │ -
298 }
│ │ │ │ -
299 if (Dother) {
│ │ │ │ -
300 Dother->setZero();
│ │ │ │ -
301 Dother->template block<1, 6>(0, 0) = Dother_;
│ │ │ │ -
302 }
│ │ │ │ -
303 return result;
│ │ │ │ -
304 }
│ │ │ │ -
│ │ │ │ -
305
│ │ │ │ -
│ │ │ │ -
311 double range(const CalibratedCamera& camera,
│ │ │ │ -
312 OptionalJacobian<1, dimension> Dcamera = boost::none,
│ │ │ │ -
313 OptionalJacobian<1, 6> Dother = boost::none) const {
│ │ │ │ -
314 return range(camera.pose(), Dcamera, Dother);
│ │ │ │ -
315 }
│ │ │ │ -
│ │ │ │ -
316
│ │ │ │ -
│ │ │ │ -
318 Matrix34 cameraProjectionMatrix() const {
│ │ │ │ -
319 return K_.K() * PinholeBase::pose().inverse().matrix().block(0, 0, 3, 4);
│ │ │ │ -
320 }
│ │ │ │ -
│ │ │ │ -
321
│ │ │ │ -
│ │ │ │ -
323 Vector defaultErrorWhenTriangulatingBehindCamera() const {
│ │ │ │ -
324 return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_.fx());;
│ │ │ │ -
325 }
│ │ │ │ -
│ │ │ │ -
326
│ │ │ │ -
327private:
│ │ │ │ -
328
│ │ │ │ -
330 friend class boost::serialization::access;
│ │ │ │ -
331 template<class Archive>
│ │ │ │ -
332 void serialize(Archive & ar, const unsigned int /*version*/) {
│ │ │ │ -
333 ar
│ │ │ │ -
334 & boost::serialization::make_nvp("PinholeBaseK",
│ │ │ │ -
335 boost::serialization::base_object<Base>(*this));
│ │ │ │ -
336 ar & BOOST_SERIALIZATION_NVP(K_);
│ │ │ │ -
337 }
│ │ │ │ -
338
│ │ │ │ -
339public:
│ │ │ │ -
340 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
341};
│ │ │ │ -
│ │ │ │ -
342
│ │ │ │ -
343// manifold traits
│ │ │ │ -
344
│ │ │ │ -
345template <typename Calibration>
│ │ │ │ -
│ │ │ │ -
346struct traits<PinholeCamera<Calibration> >
│ │ │ │ -
347 : public internal::Manifold<PinholeCamera<Calibration> > {};
│ │ │ │ -
│ │ │ │ -
348
│ │ │ │ -
349template <typename Calibration>
│ │ │ │ -
│ │ │ │ -
350struct traits<const PinholeCamera<Calibration> >
│ │ │ │ -
351 : public internal::Manifold<PinholeCamera<Calibration> > {};
│ │ │ │ -
│ │ │ │ -
352
│ │ │ │ -
353// range traits, used in RangeFactor
│ │ │ │ -
354template <typename Calibration, typename T>
│ │ │ │ -
355struct Range<PinholeCamera<Calibration>, T> : HasRange<PinholeCamera<Calibration>, T, double> {};
│ │ │ │ -
356
│ │ │ │ -
357} // \ gtsam
│ │ │ │ -
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ -
BearingRange.h
Bearing-Range product.
│ │ │ │ -
PinholePose.h
Pinhole camera with known calibration.
│ │ │ │ +
257 std::pair<Matrix,Vector> jacobian() const;
│ │ │ │ +
258
│ │ │ │ +
270 Matrix augmentedHessian(const Ordering& ordering) const;
│ │ │ │ +
271
│ │ │ │ +
283 Matrix augmentedHessian() const;
│ │ │ │ +
284
│ │ │ │ +
291 std::pair<Matrix,Vector> hessian(const Ordering& ordering) const;
│ │ │ │ +
292
│ │ │ │ +
299 std::pair<Matrix,Vector> hessian() const;
│ │ │ │ +
300
│ │ │ │ +
302 virtual VectorValues hessianDiagonal() const;
│ │ │ │ +
303
│ │ │ │ +
305 virtual std::map<Key,Matrix> hessianBlockDiagonal() const;
│ │ │ │ +
306
│ │ │ │ +
311 VectorValues optimize(
│ │ │ │ +
312 const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
│ │ │ │ +
313
│ │ │ │ +
318 VectorValues optimize(const Ordering&,
│ │ │ │ +
319 const Eliminate& function = EliminationTraitsType::DefaultEliminate) const;
│ │ │ │ +
320
│ │ │ │ +
324 VectorValues optimizeDensely() const;
│ │ │ │ +
325
│ │ │ │ +
335 VectorValues gradient(const VectorValues& x0) const;
│ │ │ │ +
336
│ │ │ │ +
344 virtual VectorValues gradientAtZero() const;
│ │ │ │ +
345
│ │ │ │ +
370 VectorValues optimizeGradientSearch() const;
│ │ │ │ +
371
│ │ │ │ +
373 VectorValues transposeMultiply(const Errors& e) const;
│ │ │ │ +
374
│ │ │ │ +
376 void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& x) const;
│ │ │ │ +
377
│ │ │ │ +
379 Errors gaussianErrors(const VectorValues& x) const;
│ │ │ │ +
380
│ │ │ │ +
382 Errors operator*(const VectorValues& x) const;
│ │ │ │ +
383
│ │ │ │ +
385 void multiplyHessianAdd(double alpha, const VectorValues& x,
│ │ │ │ +
386 VectorValues& y) const;
│ │ │ │ +
387
│ │ │ │ +
389 void multiplyInPlace(const VectorValues& x, Errors& e) const;
│ │ │ │ +
390
│ │ │ │ +
392 void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const;
│ │ │ │ +
393
│ │ │ │ +
394 void printErrors(
│ │ │ │ +
395 const VectorValues& x,
│ │ │ │ +
396 const std::string& str = "GaussianFactorGraph: ",
│ │ │ │ +
397 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
398 const std::function<bool(const Factor* /*factor*/,
│ │ │ │ +
399 double /*whitenedError*/, size_t /*index*/)>&
│ │ │ │ +
400 printCondition =
│ │ │ │ +
401 [](const Factor*, double, size_t) { return true; }) const;
│ │ │ │ +
403
│ │ │ │ +
404 private:
│ │ │ │ +
406 friend class boost::serialization::access;
│ │ │ │ +
407 template<class ARCHIVE>
│ │ │ │ +
408 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
409 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
410 }
│ │ │ │ +
411
│ │ │ │ +
412 public:
│ │ │ │ +
413
│ │ │ │ +
414#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ +
416 VectorValues GTSAM_DEPRECATED
│ │ │ │ +
417 optimize(boost::none_t, const Eliminate& function =
│ │ │ │ +
418 EliminationTraitsType::DefaultEliminate) const {
│ │ │ │ +
419 return optimize(function);
│ │ │ │ +
420 }
│ │ │ │ +
421#endif
│ │ │ │ +
422
│ │ │ │ +
423 };
│ │ │ │ +
│ │ │ │ +
424
│ │ │ │ +
429 GTSAM_EXPORT bool hasConstraints(const GaussianFactorGraph& factors);
│ │ │ │ +
430
│ │ │ │ +
431 /****** Linear Algebra Operations ******/
│ │ │ │ +
432
│ │ │ │ +
434 //GTSAM_EXPORT void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
│ │ │ │ +
435 //GTSAM_EXPORT void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
│ │ │ │ +
436
│ │ │ │ +
438template<>
│ │ │ │ +
│ │ │ │ +
439struct traits<GaussianFactorGraph> : public Testable<GaussianFactorGraph> {
│ │ │ │ +
440};
│ │ │ │ +
│ │ │ │ +
441
│ │ │ │ +
442} // \ namespace gtsam
│ │ │ │ +
FactorGraph.h
Factor Graph Base Class.
│ │ │ │ +
EliminateableFactorGraph.h
Variable elimination algorithms for factor graphs.
│ │ │ │ + │ │ │ │ +
HessianFactor.h
Contains the HessianFactor class, a general quadratic factor.
│ │ │ │ +
Errors.h
vector of errors
│ │ │ │ +
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
│ │ │ │ +
VectorValues.h
Factor Graph Values.
│ │ │ │ +
gtsam::EliminatePreferCholesky
std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< GaussianFactor > > EliminatePreferCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
Densely partially eliminate with Cholesky factorization.
Definition HessianFactor.cpp:548
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │ -
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ +
gtsam::hasConstraints
bool hasConstraints(const GaussianFactorGraph &factors)
Evaluates whether linear factors have any constrained noise models.
Definition GaussianFactorGraph.cpp:442
│ │ │ │ +
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
│ │ │ │ +
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::LieGroup::localCoordinates
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g
Definition Lie.h:136
│ │ │ │ -
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
│ │ │ │ -
gtsam::FixedDimension
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition Manifold.h:164
│ │ │ │ -
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
gtsam::Range
Definition BearingRange.h:40
│ │ │ │ -
gtsam::HasRange
Definition BearingRange.h:194
│ │ │ │ -
gtsam::PinholeBase::Dpose
static Matrix26 Dpose(const Point2 &pn, double d)
Calculate Jacobian with respect to pose.
Definition CalibratedCamera.cpp:27
│ │ │ │ -
gtsam::PinholeBase::print
virtual void print(const std::string &s="PinholeBase") const
print
Definition CalibratedCamera.cpp:74
│ │ │ │ -
gtsam::PinholeBase::pose
const Pose3 & pose() const
return pose, constant version
Definition CalibratedCamera.h:152
│ │ │ │ -
gtsam::PinholeBase::LevelPose
static Pose3 LevelPose(const Pose2 &pose2, double height)
Create a level pose at the given 2D pose and height.
Definition CalibratedCamera.cpp:49
│ │ │ │ -
gtsam::PinholeBase::equals
bool equals(const PinholeBase &camera, double tol=1e-9) const
assert equality up to a tolerance
Definition CalibratedCamera.cpp:69
│ │ │ │ -
gtsam::PinholeBase::Dpoint
static Matrix23 Dpoint(const Point2 &pn, double d, const Matrix3 &Rt)
Calculate Jacobian with respect to point.
Definition CalibratedCamera.cpp:37
│ │ │ │ -
gtsam::PinholeBase::LookatPose
static Pose3 LookatPose(const Point3 &eye, const Point3 &target, const Point3 &upVector)
Create a camera pose at the given eye position looking at a target point in the scene with the specif...
Definition CalibratedCamera.cpp:58
│ │ │ │ -
gtsam::CalibratedCamera
A Calibrated camera class [R|-R't], calibration K=I.
Definition CalibratedCamera.h:247
│ │ │ │ -
gtsam::PinholeCamera
A pinhole camera class that has a Pose3 and a Calibration.
Definition PinholeCamera.h:33
│ │ │ │ -
gtsam::PinholeCamera::print
void print(const std::string &s="PinholeCamera") const override
print
Definition PinholeCamera.h:151
│ │ │ │ -
gtsam::PinholeCamera::_project2
Point2 _project2(const POINT &pw, OptionalJacobian< 2, dimension > Dcamera, OptionalJacobian< 2, FixedDimension< POINT >::value > Dpoint) const
Templated projection of a 3D point or a point at infinity into the image.
Definition PinholeCamera.h:230
│ │ │ │ -
gtsam::PinholeCamera::defaultErrorWhenTriangulatingBehindCamera
Vector defaultErrorWhenTriangulatingBehindCamera() const
for Nonlinear Triangulation
Definition PinholeCamera.h:323
│ │ │ │ -
gtsam::PinholeCamera::calibration
const Calibration & calibration() const override
return calibration
Definition PinholeCamera.h:178
│ │ │ │ -
gtsam::PinholeCamera::getPose
const Pose3 & getPose(OptionalJacobian< 6, dimension > H) const
return pose, with derivative
Definition PinholeCamera.h:169
│ │ │ │ -
gtsam::PinholeCamera::project2
Point2 project2(const Point3 &pw, OptionalJacobian< 2, dimension > Dcamera=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none) const
project a 3D point from world coordinates into the image
Definition PinholeCamera.h:245
│ │ │ │ -
gtsam::PinholeCamera::PinholeCamera
PinholeCamera(const Pose3 &pose)
constructor with pose
Definition PinholeCamera.h:66
│ │ │ │ -
gtsam::PinholeCamera::Level
static PinholeCamera Level(const Pose2 &pose2, double height)
PinholeCamera::level with default calibration.
Definition PinholeCamera.h:92
│ │ │ │ -
gtsam::PinholeCamera::PinholeCamera
PinholeCamera(const Pose3 &pose, const Calibration &K)
constructor with pose and calibration
Definition PinholeCamera.h:71
│ │ │ │ -
gtsam::PinholeCamera::Level
static PinholeCamera Level(const Calibration &K, const Pose2 &pose2, double height)
Create a level camera at the given 2D pose and height.
Definition PinholeCamera.h:86
│ │ │ │ -
gtsam::PinholeCamera::dim
size_t dim() const
Definition PinholeCamera.h:187
│ │ │ │ -
gtsam::PinholeCamera::Identity
static PinholeCamera Identity()
for Canonical
Definition PinholeCamera.h:216
│ │ │ │ -
gtsam::PinholeCamera::Dim
static size_t Dim()
Definition PinholeCamera.h:192
│ │ │ │ -
gtsam::PinholeCamera::equals
bool equals(const Base &camera, double tol=1e-9) const
assert equality up to a tolerance
Definition PinholeCamera.h:145
│ │ │ │ -
gtsam::PinholeCamera::range
double range(const Pose3 &pose, OptionalJacobian< 1, dimension > Dcamera=boost::none, OptionalJacobian< 1, 6 > Dpose=boost::none) const
Calculate range to another pose.
Definition PinholeCamera.h:275
│ │ │ │ -
gtsam::PinholeCamera::range
double range(const PinholeCamera< CalibrationB > &camera, OptionalJacobian< 1, dimension > Dcamera=boost::none, OptionalJacobian< 1, 6+CalibrationB::dimension > Dother=boost::none) const
Calculate range to another camera.
Definition PinholeCamera.h:290
│ │ │ │ -
gtsam::PinholeCamera::PinholeCamera
PinholeCamera(const Vector &v, const Vector &K)
Init from Vector and calibration.
Definition PinholeCamera.h:136
│ │ │ │ -
gtsam::PinholeCamera::Lookat
static PinholeCamera Lookat(const Point3 &eye, const Point3 &target, const Point3 &upVector, const Calibration &K=Calibration())
Create a camera at the given eye position looking at a target point in the scene with the specified u...
Definition PinholeCamera.h:105
│ │ │ │ -
gtsam::PinholeCamera::cameraProjectionMatrix
Matrix34 cameraProjectionMatrix() const
for Linear Triangulation
Definition PinholeCamera.h:318
│ │ │ │ -
gtsam::PinholeCamera::localCoordinates
VectorK6 localCoordinates(const PinholeCamera &T2) const
return canonical coordinate
Definition PinholeCamera.h:208
│ │ │ │ -
gtsam::PinholeCamera::retract
PinholeCamera retract(const Vector &d) const
move a cameras according to d
Definition PinholeCamera.h:199
│ │ │ │ -
gtsam::PinholeCamera::access
friend class boost::serialization::access
Serialization function.
Definition PinholeCamera.h:330
│ │ │ │ -
gtsam::PinholeCamera::project2
Point2 project2(const Unit3 &pw, OptionalJacobian< 2, dimension > Dcamera=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const
project a point at infinity from world coordinates into the image
Definition PinholeCamera.h:251
│ │ │ │ -
gtsam::PinholeCamera::pose
const Pose3 & pose() const
return pose
Definition PinholeCamera.h:164
│ │ │ │ -
gtsam::PinholeCamera::PinholeCamera
PinholeCamera(const Vector &v)
Init from vector, can be 6D (default calibration) or dim.
Definition PinholeCamera.h:129
│ │ │ │ -
gtsam::PinholeCamera::PinholeCamera
PinholeCamera()
default constructor
Definition PinholeCamera.h:62
│ │ │ │ -
gtsam::PinholeCamera::Measurement
Point2 Measurement
Some classes template on either PinholeCamera or StereoCamera, and this typedef informs those classes...
Definition PinholeCamera.h:41
│ │ │ │ -
gtsam::PinholeCamera::range
double range(const CalibratedCamera &camera, OptionalJacobian< 1, dimension > Dcamera=boost::none, OptionalJacobian< 1, 6 > Dother=boost::none) const
Calculate range to a calibrated camera.
Definition PinholeCamera.h:311
│ │ │ │ -
gtsam::PinholeCamera::range
double range(const Point3 &point, OptionalJacobian< 1, dimension > Dcamera=boost::none, OptionalJacobian< 1, 3 > Dpoint=boost::none) const
Calculate range to a landmark.
Definition PinholeCamera.h:261
│ │ │ │ -
gtsam::PinholeBaseK
A pinhole camera class that has a Pose3 and a fixed Calibration.
Definition PinholePose.h:34
│ │ │ │ -
gtsam::PinholeBaseK< Calibration >::project
Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > Dcal=boost::none) const
project a 3D point from world coordinates into the image
Definition PinholePose.h:118
│ │ │ │ -
gtsam::Pose2
A 2D pose (Point2,Rot2)
Definition Pose2.h:36
│ │ │ │ -
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ -
gtsam::Pose3::range
double range(const Point3 &point, OptionalJacobian< 1, 6 > Hself=boost::none, OptionalJacobian< 1, 3 > Hpoint=boost::none) const
Calculate range to a landmark.
Definition Pose3.cpp:399
│ │ │ │ -
gtsam::Pose3::matrix
Matrix4 matrix() const
convert to 4*4 matrix
Definition Pose3.cpp:323
│ │ │ │ -
gtsam::Pose3::inverse
Pose3 inverse() const
inverse transformation with derivatives
Definition Pose3.cpp:49
│ │ │ │ -
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
│ │ │ │ + │ │ │ │ + │ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ +
gtsam::FactorGraph::isEqual
bool isEqual(const FactorGraph &other) const
Check exact equality of the factor pointers. Useful for derived ==.
Definition FactorGraph.h:134
│ │ │ │ +
gtsam::FactorGraph< GaussianFactor >::sharedFactor
boost::shared_ptr< GaussianFactor > sharedFactor
Shared pointer to a factor.
Definition FactorGraph.h:101
│ │ │ │ +
gtsam::EliminationTraits
Traits class for eliminateable factor graphs, specifies the types that result from elimination,...
Definition EliminateableFactorGraph.h:36
│ │ │ │ +
gtsam::EliminateableFactorGraph
EliminateableFactorGraph is a base class for factor graphs that contains elimination algorithms.
Definition EliminateableFactorGraph.h:57
│ │ │ │ +
gtsam::Factor
Definition Factor.h:68
│ │ │ │ +
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
gtsam::Ordering::Colamd
static Ordering Colamd(const FACTOR_GRAPH &graph)
Compute a fill-reducing ordering using COLAMD from a factor graph (see details for note on performanc...
Definition Ordering.h:95
│ │ │ │ +
gtsam::GaussianBayesNet
GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals.
Definition GaussianBayesNet.h:36
│ │ │ │ +
gtsam::GaussianBayesTree
A Bayes tree representing a Gaussian density.
Definition GaussianBayesTree.h:52
│ │ │ │ +
gtsam::GaussianConditional
A GaussianConditional functions as the node in a Bayes network.
Definition GaussianConditional.h:43
│ │ │ │ +
gtsam::GaussianEliminationTree
Definition GaussianEliminationTree.h:29
│ │ │ │ +
gtsam::GaussianFactor
An abstract virtual base class for JacobianFactor and HessianFactor.
Definition GaussianFactor.h:39
│ │ │ │ +
gtsam::GaussianFactor::clone
virtual GaussianFactor::shared_ptr clone() const =0
Clone a factor (make a deep copy)
│ │ │ │ +
gtsam::EliminationTraits< GaussianFactorGraph >::DefaultOrderingFunc
static Ordering DefaultOrderingFunc(const FactorGraphType &graph, boost::optional< const VariableIndex & > variableIndex)
The default ordering generation function.
Definition GaussianFactorGraph.h:58
│ │ │ │ +
gtsam::EliminationTraits< GaussianFactorGraph >::BayesTreeType
GaussianBayesTree BayesTreeType
Type of Bayes tree.
Definition GaussianFactorGraph.h:51
│ │ │ │ +
gtsam::EliminationTraits< GaussianFactorGraph >::ConditionalType
GaussianConditional ConditionalType
Type of conditionals from elimination.
Definition GaussianFactorGraph.h:48
│ │ │ │ +
gtsam::EliminationTraits< GaussianFactorGraph >::FactorType
GaussianFactor FactorType
Type of factors in factor graph.
Definition GaussianFactorGraph.h:46
│ │ │ │ +
gtsam::EliminationTraits< GaussianFactorGraph >::EliminationTreeType
GaussianEliminationTree EliminationTreeType
Type of elimination tree.
Definition GaussianFactorGraph.h:50
│ │ │ │ +
gtsam::EliminationTraits< GaussianFactorGraph >::FactorGraphType
GaussianFactorGraph FactorGraphType
Type of the factor graph (e.g. GaussianFactorGraph)
Definition GaussianFactorGraph.h:47
│ │ │ │ +
gtsam::EliminationTraits< GaussianFactorGraph >::BayesNetType
GaussianBayesNet BayesNetType
Type of Bayes net from sequential elimination.
Definition GaussianFactorGraph.h:49
│ │ │ │ +
gtsam::EliminationTraits< GaussianFactorGraph >::JunctionTreeType
GaussianJunctionTree JunctionTreeType
Type of Junction tree.
Definition GaussianFactorGraph.h:52
│ │ │ │ +
gtsam::EliminationTraits< GaussianFactorGraph >::DefaultEliminate
static std::pair< boost::shared_ptr< ConditionalType >, boost::shared_ptr< FactorType > > DefaultEliminate(const FactorGraphType &factors, const Ordering &keys)
The default dense elimination function.
Definition GaussianFactorGraph.h:55
│ │ │ │ +
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ +
gtsam::GaussianFactorGraph::BaseEliminateable
EliminateableFactorGraph< This > BaseEliminateable
Typedef to base elimination class.
Definition GaussianFactorGraph.h:80
│ │ │ │ +
gtsam::GaussianFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactorGraph.h:81
│ │ │ │ +
gtsam::GaussianFactorGraph::add
void add(const TERMS &terms, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Add an n-ary factor.
Definition GaussianFactorGraph.h:156
│ │ │ │ +
gtsam::GaussianFactorGraph::GaussianFactorGraph
GaussianFactorGraph(std::initializer_list< sharedFactor > factors)
Construct from an initializer lists of GaussianFactor shared pointers.
Definition GaussianFactorGraph.h:94
│ │ │ │ +
gtsam::GaussianFactorGraph::GaussianFactorGraph
GaussianFactorGraph()
Default constructor.
Definition GaussianFactorGraph.h:87
│ │ │ │ +
gtsam::GaussianFactorGraph::add
void add(const GaussianFactor &factor)
Add a factor by value - makes a copy.
Definition GaussianFactorGraph.h:127
│ │ │ │ +
gtsam::GaussianFactorGraph::add
void add(Key key1, const Matrix &A1, Key key2, const Matrix &A2, Key key3, const Matrix &A3, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Add a ternary factor.
Definition GaussianFactorGraph.h:148
│ │ │ │ +
gtsam::GaussianFactorGraph::add
void add(const sharedFactor &factor)
Add a factor by pointer - stores pointer without copying the factor.
Definition GaussianFactorGraph.h:130
│ │ │ │ +
gtsam::GaussianFactorGraph::operator==
friend bool operator==(const GaussianFactorGraph &lhs, const GaussianFactorGraph &rhs)
Check exact equality.
Definition GaussianFactorGraph.h:121
│ │ │ │ +
gtsam::GaussianFactorGraph::This
GaussianFactorGraph This
Typedef to this class.
Definition GaussianFactorGraph.h:78
│ │ │ │ +
gtsam::GaussianFactorGraph::Keys
KeySet Keys
Return the set of variables involved in the factors (computes a set union).
Definition GaussianFactorGraph.h:163
│ │ │ │ +
gtsam::GaussianFactorGraph::add
void add(const Vector &b)
Add a null factor.
Definition GaussianFactorGraph.h:133
│ │ │ │ +
gtsam::GaussianFactorGraph::add
void add(Key key1, const Matrix &A1, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Add a unary factor.
Definition GaussianFactorGraph.h:137
│ │ │ │ +
gtsam::GaussianFactorGraph::~GaussianFactorGraph
virtual ~GaussianFactorGraph()
Virtual destructor.
Definition GaussianFactorGraph.h:110
│ │ │ │ +
gtsam::GaussianFactorGraph::add
void add(Key key1, const Matrix &A1, Key key2, const Matrix &A2, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Add a binary factor.
Definition GaussianFactorGraph.h:142
│ │ │ │ +
gtsam::GaussianFactorGraph::GaussianFactorGraph
GaussianFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
Construct from iterator over factors.
Definition GaussianFactorGraph.h:99
│ │ │ │ +
gtsam::GaussianFactorGraph::Base
FactorGraph< GaussianFactor > Base
Typedef to base factor graph type.
Definition GaussianFactorGraph.h:79
│ │ │ │ +
gtsam::GaussianFactorGraph::GaussianFactorGraph
GaussianFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
Implicit copy/downcast constructor to override explicit template container constructor.
Definition GaussianFactorGraph.h:107
│ │ │ │ +
gtsam::GaussianFactorGraph::GaussianFactorGraph
GaussianFactorGraph(const CONTAINER &factors)
Construct from container of factors (shared_ptr or plain objects)
Definition GaussianFactorGraph.h:103
│ │ │ │ +
gtsam::GaussianJunctionTree
A junction tree specialized to Gaussian factors, i.e., it is a cluster tree with Gaussian factors sto...
Definition GaussianJunctionTree.h:39
│ │ │ │ +
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition JacobianFactor.h:91
│ │ │ │ +
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
GaussianConditional
is the normalization constant.
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,513 +1,493 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -PinholeCamera.h │ │ │ │ │ +GaussianFactorGraph.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_i_n_h_o_l_e_P_o_s_e_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_B_e_a_r_i_n_g_R_a_n_g_e_._h> │ │ │ │ │ +22#pragma once │ │ │ │ │ 23 │ │ │ │ │ -24namespace _g_t_s_a_m { │ │ │ │ │ -25 │ │ │ │ │ -32template │ │ │ │ │ -_3_3class _P_i_n_h_o_l_e_C_a_m_e_r_a: public _P_i_n_h_o_l_e_B_a_s_e_K { │ │ │ │ │ -34 │ │ │ │ │ -35public: │ │ │ │ │ -36 │ │ │ │ │ -_4_1 typedef _P_o_i_n_t_2 _M_e_a_s_u_r_e_m_e_n_t; │ │ │ │ │ -42 typedef Point2Vector MeasurementVector; │ │ │ │ │ -43 │ │ │ │ │ -44private: │ │ │ │ │ -45 │ │ │ │ │ -46 typedef _P_i_n_h_o_l_e_B_a_s_e_K_<_C_a_l_i_b_r_a_t_i_o_n_> _B_a_s_e; │ │ │ │ │ -47 Calibration K_; │ │ │ │ │ -48 │ │ │ │ │ -49 // Get dimensions of calibration type at compile time │ │ │ │ │ -50 static const int DimK = _F_i_x_e_d_D_i_m_e_n_s_i_o_n_<_C_a_l_i_b_r_a_t_i_o_n_>_:_:_v_a_l_u_e; │ │ │ │ │ -51 │ │ │ │ │ -52public: │ │ │ │ │ -53 │ │ │ │ │ -54 enum { │ │ │ │ │ -55 dimension = 6 + DimK │ │ │ │ │ -56 }; │ │ │ │ │ -57 │ │ │ │ │ -60 │ │ │ │ │ -_6_2 _P_i_n_h_o_l_e_C_a_m_e_r_a() { │ │ │ │ │ -63 } │ │ │ │ │ +24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_E_r_r_o_r_s_._h> // Included here instead of fw-declared so │ │ │ │ │ +we can use Errors::iterator │ │ │ │ │ +27#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ +28#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_H_e_s_s_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ +29#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ +30#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ +31 │ │ │ │ │ +32namespace _g_t_s_a_m { │ │ │ │ │ +33 │ │ │ │ │ +34 // Forward declarations │ │ │ │ │ +35 class GaussianFactorGraph; │ │ │ │ │ +36 class GaussianFactor; │ │ │ │ │ +37 class _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l; │ │ │ │ │ +38 class GaussianBayesNet; │ │ │ │ │ +39 class GaussianEliminationTree; │ │ │ │ │ +40 class GaussianBayesTree; │ │ │ │ │ +41 class GaussianJunctionTree; │ │ │ │ │ +42 │ │ │ │ │ +43 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +_4_4 template<> struct _E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s<_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h> │ │ │ │ │ +45 { │ │ │ │ │ +_4_6 typedef _G_a_u_s_s_i_a_n_F_a_c_t_o_r _F_a_c_t_o_r_T_y_p_e; │ │ │ │ │ +_4_7 typedef _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e; │ │ │ │ │ +_4_8 typedef _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l _C_o_n_d_i_t_i_o_n_a_l_T_y_p_e; │ │ │ │ │ +_4_9 typedef _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t _B_a_y_e_s_N_e_t_T_y_p_e; │ │ │ │ │ +_5_0 typedef _G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_T_y_p_e; │ │ │ │ │ +_5_1 typedef _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e _B_a_y_e_s_T_r_e_e_T_y_p_e; │ │ │ │ │ +_5_2 typedef _G_a_u_s_s_i_a_n_J_u_n_c_t_i_o_n_T_r_e_e _J_u_n_c_t_i_o_n_T_r_e_e_T_y_p_e; │ │ │ │ │ +54 static std::pair, boost:: │ │ │ │ │ +shared_ptr > │ │ │ │ │ +_5_5 _D_e_f_a_u_l_t_E_l_i_m_i_n_a_t_e(const _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e& factors, const _O_r_d_e_r_i_n_g& keys) { │ │ │ │ │ +56 return _E_l_i_m_i_n_a_t_e_P_r_e_f_e_r_C_h_o_l_e_s_k_y(factors, keys); } │ │ │ │ │ +_5_8 static _O_r_d_e_r_i_n_g _D_e_f_a_u_l_t_O_r_d_e_r_i_n_g_F_u_n_c( │ │ │ │ │ +59 const _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e& graph, │ │ │ │ │ +60 boost::optional variableIndex) { │ │ │ │ │ +61 return _O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d(*variableIndex); │ │ │ │ │ +62 } │ │ │ │ │ +63 }; │ │ │ │ │ 64 │ │ │ │ │ -_6_6 explicit _P_i_n_h_o_l_e_C_a_m_e_r_a(const _P_o_s_e_3& _p_o_s_e) : │ │ │ │ │ -67 _B_a_s_e(_p_o_s_e) { │ │ │ │ │ -68 } │ │ │ │ │ -69 │ │ │ │ │ -_7_1 _P_i_n_h_o_l_e_C_a_m_e_r_a(const _P_o_s_e_3& _p_o_s_e, const Calibration& K) : │ │ │ │ │ -72 _B_a_s_e(_p_o_s_e), K_(K) { │ │ │ │ │ -73 } │ │ │ │ │ -74 │ │ │ │ │ -78 │ │ │ │ │ -_8_6 static _P_i_n_h_o_l_e_C_a_m_e_r_a _L_e_v_e_l(const Calibration &K, const _P_o_s_e_2& pose2, │ │ │ │ │ -87 double height) { │ │ │ │ │ -88 return _P_i_n_h_o_l_e_C_a_m_e_r_a(_B_a_s_e_:_:_L_e_v_e_l_P_o_s_e(pose2, height), K); │ │ │ │ │ -89 } │ │ │ │ │ -90 │ │ │ │ │ -_9_2 static _P_i_n_h_o_l_e_C_a_m_e_r_a _L_e_v_e_l(const _P_o_s_e_2& pose2, double height) { │ │ │ │ │ -93 return _P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_L_e_v_e_l(Calibration(), pose2, height); │ │ │ │ │ -94 } │ │ │ │ │ +65 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +_7_2 class GTSAM_EXPORT _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h : │ │ │ │ │ +73 public _F_a_c_t_o_r_G_r_a_p_h, │ │ │ │ │ +74 public _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +75 { │ │ │ │ │ +76 public: │ │ │ │ │ +77 │ │ │ │ │ +_7_8 typedef _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h _T_h_i_s; │ │ │ │ │ +_7_9 typedef _F_a_c_t_o_r_G_r_a_p_h_<_G_a_u_s_s_i_a_n_F_a_c_t_o_r_> _B_a_s_e; │ │ │ │ │ +_8_0 typedef _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_T_h_i_s_> _B_a_s_e_E_l_i_m_i_n_a_t_e_a_b_l_e; │ │ │ │ │ +_8_1 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +82 │ │ │ │ │ +85 │ │ │ │ │ +_8_7 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h() {} │ │ │ │ │ +88 │ │ │ │ │ +_9_4 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h(std::initializer_list factors) : _B_a_s_e │ │ │ │ │ +(factors) {} │ │ │ │ │ 95 │ │ │ │ │ -_1_0_5 static _P_i_n_h_o_l_e_C_a_m_e_r_a _L_o_o_k_a_t(const _P_o_i_n_t_3& eye, const _P_o_i_n_t_3& target, │ │ │ │ │ -106 const _P_o_i_n_t_3& upVector, const Calibration& K = Calibration()) { │ │ │ │ │ -107 return _P_i_n_h_o_l_e_C_a_m_e_r_a(_B_a_s_e_:_:_L_o_o_k_a_t_P_o_s_e(eye, target, upVector), K); │ │ │ │ │ -108 } │ │ │ │ │ -109 │ │ │ │ │ -110 // Create PinholeCamera, with derivatives │ │ │ │ │ -111 static _P_i_n_h_o_l_e_C_a_m_e_r_a Create(const _P_o_s_e_3& _p_o_s_e, const Calibration &K, │ │ │ │ │ -112 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_d_i_m_e_n_s_i_o_n_,_ _6_> H1 = boost::none, // │ │ │ │ │ -113 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_d_i_m_e_n_s_i_o_n_,_ _D_i_m_K_> H2 = boost::none) { │ │ │ │ │ -114 typedef Eigen::Matrix MatrixK6; │ │ │ │ │ -115 if (H1) │ │ │ │ │ -116 *H1 << I_6x6, MatrixK6::Zero(); │ │ │ │ │ -117 typedef Eigen::Matrix Matrix6K; │ │ │ │ │ -118 typedef Eigen::Matrix MatrixK; │ │ │ │ │ -119 if (H2) │ │ │ │ │ -120 *H2 << Matrix6K::Zero(), MatrixK::Identity(); │ │ │ │ │ -121 return _P_i_n_h_o_l_e_C_a_m_e_r_a(_p_o_s_e,K); │ │ │ │ │ -122 } │ │ │ │ │ -123 │ │ │ │ │ -127 │ │ │ │ │ -_1_2_9 explicit _P_i_n_h_o_l_e_C_a_m_e_r_a(const Vector &v) : │ │ │ │ │ -130 _B_a_s_e(v.head<6>()) { │ │ │ │ │ -131 if (v.size() > 6) │ │ │ │ │ -132 K_ = Calibration(v.tail()); │ │ │ │ │ -133 } │ │ │ │ │ -134 │ │ │ │ │ -_1_3_6 _P_i_n_h_o_l_e_C_a_m_e_r_a(const Vector &v, const Vector &K) : │ │ │ │ │ -137 _B_a_s_e(v), K_(K) { │ │ │ │ │ -138 } │ │ │ │ │ -139 │ │ │ │ │ -143 │ │ │ │ │ -_1_4_5 bool _e_q_u_a_l_s(const _B_a_s_e &camera, double tol = 1e-9) const { │ │ │ │ │ -146 const _P_i_n_h_o_l_e_C_a_m_e_r_a* e = dynamic_cast(&camera); │ │ │ │ │ -147 return _B_a_s_e_:_:_e_q_u_a_l_s(camera, tol) && K_._e_q_u_a_l_s(e->_c_a_l_i_b_r_a_t_i_o_n(), tol); │ │ │ │ │ -148 } │ │ │ │ │ -149 │ │ │ │ │ -_1_5_1 void _p_r_i_n_t(const std::string& s = "PinholeCamera") const override { │ │ │ │ │ -152 _B_a_s_e_:_:_p_r_i_n_t(s); │ │ │ │ │ -153 K_.print(s + ".calibration"); │ │ │ │ │ -154 } │ │ │ │ │ -155 │ │ │ │ │ -159 │ │ │ │ │ -160 _~_P_i_n_h_o_l_e_C_a_m_e_r_a() override { │ │ │ │ │ -161 } │ │ │ │ │ -162 │ │ │ │ │ -_1_6_4 const _P_o_s_e_3& _p_o_s_e() const { │ │ │ │ │ -165 return _B_a_s_e_:_:_p_o_s_e(); │ │ │ │ │ -166 } │ │ │ │ │ -167 │ │ │ │ │ -_1_6_9 const _P_o_s_e_3& _g_e_t_P_o_s_e(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _d_i_m_e_n_s_i_o_n_> H) const { │ │ │ │ │ -170 if (H) { │ │ │ │ │ -171 H->setZero(); │ │ │ │ │ -172 H->template block<6, 6>(0, 0) = I_6x6; │ │ │ │ │ -173 } │ │ │ │ │ -174 return _B_a_s_e_:_:_p_o_s_e(); │ │ │ │ │ -175 } │ │ │ │ │ -176 │ │ │ │ │ -_1_7_8 const Calibration& _c_a_l_i_b_r_a_t_i_o_n() const override { │ │ │ │ │ -179 return K_; │ │ │ │ │ -180 } │ │ │ │ │ +96 │ │ │ │ │ +98 template │ │ │ │ │ +_9_9 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h(ITERATOR firstFactor, ITERATOR lastFactor) : _B_a_s_e │ │ │ │ │ +(firstFactor, lastFactor) {} │ │ │ │ │ +100 │ │ │ │ │ +102 template │ │ │ │ │ +_1_0_3 explicit _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h(const CONTAINER& factors) : _B_a_s_e(factors) {} │ │ │ │ │ +104 │ │ │ │ │ +106 template │ │ │ │ │ +_1_0_7 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h(const _F_a_c_t_o_r_G_r_a_p_h_<_D_E_R_I_V_E_D_F_A_C_T_O_R_>& graph) : _B_a_s_e(graph) │ │ │ │ │ +{} │ │ │ │ │ +108 │ │ │ │ │ +_1_1_0 virtual _~_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h() {} │ │ │ │ │ +111 │ │ │ │ │ +115 │ │ │ │ │ +116 bool _e_q_u_a_l_s(const This& fg, double tol = 1e-9) const; │ │ │ │ │ +117 │ │ │ │ │ +119 │ │ │ │ │ +_1_2_1 friend bool _o_p_e_r_a_t_o_r_=_=(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& lhs, │ │ │ │ │ +122 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& rhs) { │ │ │ │ │ +123 return lhs._i_s_E_q_u_a_l(rhs); │ │ │ │ │ +124 } │ │ │ │ │ +125 │ │ │ │ │ +_1_2_7 void _a_d_d(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r& factor) { push_back(factor._c_l_o_n_e()); } │ │ │ │ │ +128 │ │ │ │ │ +_1_3_0 void _a_d_d(const _s_h_a_r_e_d_F_a_c_t_o_r& factor) { push_back(factor); } │ │ │ │ │ +131 │ │ │ │ │ +_1_3_3 void _a_d_d(const Vector& b) { │ │ │ │ │ +134 add(_J_a_c_o_b_i_a_n_F_a_c_t_o_r(b)); } │ │ │ │ │ +135 │ │ │ │ │ +_1_3_7 void _a_d_d(_K_e_y key1, const Matrix& A1, │ │ │ │ │ +138 const Vector& b, const SharedDiagonal& model = SharedDiagonal()) { │ │ │ │ │ +139 add(_J_a_c_o_b_i_a_n_F_a_c_t_o_r(key1,A1,b,model)); } │ │ │ │ │ +140 │ │ │ │ │ +_1_4_2 void _a_d_d(_K_e_y key1, const Matrix& A1, │ │ │ │ │ +143 _K_e_y key2, const Matrix& A2, │ │ │ │ │ +144 const Vector& b, const SharedDiagonal& model = SharedDiagonal()) { │ │ │ │ │ +145 add(_J_a_c_o_b_i_a_n_F_a_c_t_o_r(key1,A1,key2,A2,b,model)); } │ │ │ │ │ +146 │ │ │ │ │ +_1_4_8 void _a_d_d(_K_e_y key1, const Matrix& A1, │ │ │ │ │ +149 _K_e_y key2, const Matrix& A2, │ │ │ │ │ +150 _K_e_y key3, const Matrix& A3, │ │ │ │ │ +151 const Vector& b, const SharedDiagonal& model = SharedDiagonal()) { │ │ │ │ │ +152 add(_J_a_c_o_b_i_a_n_F_a_c_t_o_r(key1,A1,key2,A2,key3,A3,b,model)); } │ │ │ │ │ +153 │ │ │ │ │ +155 template │ │ │ │ │ +_1_5_6 void _a_d_d(const TERMS& terms, const Vector &b, const SharedDiagonal& model = │ │ │ │ │ +SharedDiagonal()) { │ │ │ │ │ +157 add(_J_a_c_o_b_i_a_n_F_a_c_t_o_r(terms,b,model)); } │ │ │ │ │ +158 │ │ │ │ │ +_1_6_3 typedef _K_e_y_S_e_t _K_e_y_s; │ │ │ │ │ +164 _K_e_y_s keys() const; │ │ │ │ │ +165 │ │ │ │ │ +166 /* return a map of (Key, dimension) */ │ │ │ │ │ +167 std::map getKeyDimMap() const; │ │ │ │ │ +168 │ │ │ │ │ +170 double error(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const; │ │ │ │ │ +171 │ │ │ │ │ +173 double probPrime(const _V_e_c_t_o_r_V_a_l_u_e_s& c) const; │ │ │ │ │ +174 │ │ │ │ │ +180 virtual _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h clone() const; │ │ │ │ │ 181 │ │ │ │ │ -185 │ │ │ │ │ -_1_8_7 size_t _d_i_m() const { │ │ │ │ │ -188 return dimension; │ │ │ │ │ -189 } │ │ │ │ │ -190 │ │ │ │ │ -_1_9_2 static size_t _D_i_m() { │ │ │ │ │ -193 return dimension; │ │ │ │ │ -194 } │ │ │ │ │ +186 virtual _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r cloneToPtr() const; │ │ │ │ │ +187 │ │ │ │ │ +194 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h negate() const; │ │ │ │ │ 195 │ │ │ │ │ -196 typedef Eigen::Matrix VectorK6; │ │ │ │ │ -197 │ │ │ │ │ -_1_9_9 _P_i_n_h_o_l_e_C_a_m_e_r_a _r_e_t_r_a_c_t(const Vector& d) const { │ │ │ │ │ -200 if ((size_t) d.size() == 6) │ │ │ │ │ -201 return _P_i_n_h_o_l_e_C_a_m_e_r_a(this->_p_o_s_e()._r_e_t_r_a_c_t(d), _c_a_l_i_b_r_a_t_i_o_n()); │ │ │ │ │ -202 else │ │ │ │ │ -203 return _P_i_n_h_o_l_e_C_a_m_e_r_a(this->_p_o_s_e()._r_e_t_r_a_c_t(d.head<6>()), │ │ │ │ │ -204 _c_a_l_i_b_r_a_t_i_o_n()._r_e_t_r_a_c_t(d.tail(_c_a_l_i_b_r_a_t_i_o_n()._d_i_m()))); │ │ │ │ │ -205 } │ │ │ │ │ -206 │ │ │ │ │ -_2_0_8 VectorK6 _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(const _P_i_n_h_o_l_e_C_a_m_e_r_a& T2) const { │ │ │ │ │ -209 VectorK6 d; │ │ │ │ │ -210 d.template head<6>() = this->_p_o_s_e()._l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(T2._p_o_s_e()); │ │ │ │ │ -211 d.template tail() = _c_a_l_i_b_r_a_t_i_o_n().localCoordinates(T2._c_a_l_i_b_r_a_t_i_o_n()); │ │ │ │ │ -212 return d; │ │ │ │ │ -213 } │ │ │ │ │ +198 │ │ │ │ │ +209 std::vector > sparseJacobian( │ │ │ │ │ +210 const _O_r_d_e_r_i_n_g& ordering, size_t& nrows, size_t& ncols) const; │ │ │ │ │ +211 │ │ │ │ │ +213 std::vector > sparseJacobian() const; │ │ │ │ │ 214 │ │ │ │ │ -_2_1_6 static _P_i_n_h_o_l_e_C_a_m_e_r_a _I_d_e_n_t_i_t_y() { │ │ │ │ │ -217 return _P_i_n_h_o_l_e_C_a_m_e_r_a(); // assumes that the default constructor is valid │ │ │ │ │ -218 } │ │ │ │ │ -219 │ │ │ │ │ -223 │ │ │ │ │ -224 typedef Eigen::Matrix Matrix2K; │ │ │ │ │ -225 │ │ │ │ │ -229 template │ │ │ │ │ -_2_3_0 _P_o_i_n_t_2 ___p_r_o_j_e_c_t_2(const POINT& pw, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _d_i_m_e_n_s_i_o_n_> Dcamera, │ │ │ │ │ -231 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n<2, _F_i_x_e_d_D_i_m_e_n_s_i_o_n_<_P_O_I_N_T_>_:_:_v_a_l_u_e> _D_p_o_i_n_t) const { │ │ │ │ │ -232 // We just call 3-derivative version in Base │ │ │ │ │ -233 if (Dcamera){ │ │ │ │ │ -234 Matrix26 _D_p_o_s_e; │ │ │ │ │ -235 Eigen::Matrix Dcal; │ │ │ │ │ -236 const _P_o_i_n_t_2 pi = _B_a_s_e_:_:_p_r_o_j_e_c_t(pw, _D_p_o_s_e, _D_p_o_i_n_t, Dcal); │ │ │ │ │ -237 *Dcamera << _D_p_o_s_e, Dcal; │ │ │ │ │ -238 return pi; │ │ │ │ │ -239 } else { │ │ │ │ │ -240 return _B_a_s_e_:_:_p_r_o_j_e_c_t(pw, boost::none, _D_p_o_i_n_t, boost::none); │ │ │ │ │ -241 } │ │ │ │ │ -242 } │ │ │ │ │ -243 │ │ │ │ │ -_2_4_5 _P_o_i_n_t_2 _p_r_o_j_e_c_t_2(const _P_o_i_n_t_3& pw, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _d_i_m_e_n_s_i_o_n_> Dcamera = │ │ │ │ │ -246 boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> _D_p_o_i_n_t = boost::none) const { │ │ │ │ │ -247 return ___p_r_o_j_e_c_t_2(pw, Dcamera, _D_p_o_i_n_t); │ │ │ │ │ -248 } │ │ │ │ │ +221 Matrix sparseJacobian_() const; │ │ │ │ │ +222 │ │ │ │ │ +230 Matrix augmentedJacobian(const _O_r_d_e_r_i_n_g& ordering) const; │ │ │ │ │ +231 │ │ │ │ │ +239 Matrix augmentedJacobian() const; │ │ │ │ │ +240 │ │ │ │ │ +248 std::pair jacobian(const _O_r_d_e_r_i_n_g& ordering) const; │ │ │ │ │ 249 │ │ │ │ │ -_2_5_1 _P_o_i_n_t_2 _p_r_o_j_e_c_t_2(const _U_n_i_t_3& pw, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _d_i_m_e_n_s_i_o_n_> Dcamera = │ │ │ │ │ -252 boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> _D_p_o_i_n_t = boost::none) const { │ │ │ │ │ -253 return ___p_r_o_j_e_c_t_2(pw, Dcamera, _D_p_o_i_n_t); │ │ │ │ │ -254 } │ │ │ │ │ -255 │ │ │ │ │ -_2_6_1 double _r_a_n_g_e(const _P_o_i_n_t_3& point, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _d_i_m_e_n_s_i_o_n_> Dcamera = │ │ │ │ │ -262 boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> _D_p_o_i_n_t = boost::none) const { │ │ │ │ │ -263 Matrix16 Dpose_; │ │ │ │ │ -264 double result = this->_p_o_s_e()._r_a_n_g_e(point, Dcamera ? &Dpose_ : 0, _D_p_o_i_n_t); │ │ │ │ │ -265 if (Dcamera) │ │ │ │ │ -266 *Dcamera << Dpose_, Eigen::Matrix::Zero(); │ │ │ │ │ -267 return result; │ │ │ │ │ -268 } │ │ │ │ │ -269 │ │ │ │ │ -_2_7_5 double _r_a_n_g_e(const _P_o_s_e_3& _p_o_s_e, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _d_i_m_e_n_s_i_o_n_> Dcamera = │ │ │ │ │ -276 boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> _D_p_o_s_e = boost::none) const { │ │ │ │ │ -277 Matrix16 Dpose_; │ │ │ │ │ -278 double result = this->_p_o_s_e()._r_a_n_g_e(pose, Dcamera ? &Dpose_ : 0, _D_p_o_s_e); │ │ │ │ │ -279 if (Dcamera) │ │ │ │ │ -280 *Dcamera << Dpose_, Eigen::Matrix::Zero(); │ │ │ │ │ -281 return result; │ │ │ │ │ -282 } │ │ │ │ │ -283 │ │ │ │ │ -289 template │ │ │ │ │ -_2_9_0 double _r_a_n_g_e(const _P_i_n_h_o_l_e_C_a_m_e_r_a_<_C_a_l_i_b_r_a_t_i_o_n_B_>& camera, │ │ │ │ │ -291 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _d_i_m_e_n_s_i_o_n_> Dcamera = boost::none, │ │ │ │ │ -292 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_ _+_ _C_a_l_i_b_r_a_t_i_o_n_B_:_:_d_i_m_e_n_s_i_o_n_> Dother = boost::none) │ │ │ │ │ -const { │ │ │ │ │ -293 Matrix16 Dcamera_, Dother_; │ │ │ │ │ -294 double result = this->_p_o_s_e()._r_a_n_g_e(camera._p_o_s_e(), Dcamera ? &Dcamera_ : 0, │ │ │ │ │ -295 Dother ? &Dother_ : 0); │ │ │ │ │ -296 if (Dcamera) { │ │ │ │ │ -297 *Dcamera << Dcamera_, Eigen::Matrix::Zero(); │ │ │ │ │ -298 } │ │ │ │ │ -299 if (Dother) { │ │ │ │ │ -300 Dother->setZero(); │ │ │ │ │ -301 Dother->template block<1, 6>(0, 0) = Dother_; │ │ │ │ │ -302 } │ │ │ │ │ -303 return result; │ │ │ │ │ -304 } │ │ │ │ │ -305 │ │ │ │ │ -_3_1_1 double _r_a_n_g_e(const _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a& camera, │ │ │ │ │ -312 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _d_i_m_e_n_s_i_o_n_> Dcamera = boost::none, │ │ │ │ │ -313 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> Dother = boost::none) const { │ │ │ │ │ -314 return _r_a_n_g_e(camera._p_o_s_e(), Dcamera, Dother); │ │ │ │ │ -315 } │ │ │ │ │ -316 │ │ │ │ │ -_3_1_8 Matrix34 _c_a_m_e_r_a_P_r_o_j_e_c_t_i_o_n_M_a_t_r_i_x() const { │ │ │ │ │ -319 return K_.K() * _P_i_n_h_o_l_e_B_a_s_e_:_:_p_o_s_e()._i_n_v_e_r_s_e()._m_a_t_r_i_x().block(0, 0, 3, 4); │ │ │ │ │ -320 } │ │ │ │ │ -321 │ │ │ │ │ -_3_2_3 Vector _d_e_f_a_u_l_t_E_r_r_o_r_W_h_e_n_T_r_i_a_n_g_u_l_a_t_i_n_g_B_e_h_i_n_d_C_a_m_e_r_a() const { │ │ │ │ │ -324 return Eigen::Matrix::dimension,1>::Constant(2.0 * │ │ │ │ │ -K_.fx());; │ │ │ │ │ -325 } │ │ │ │ │ -326 │ │ │ │ │ -327private: │ │ │ │ │ -328 │ │ │ │ │ -_3_3_0 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -331 template │ │ │ │ │ -332 void serialize(Archive & ar, const unsigned int /*version*/) { │ │ │ │ │ -333 ar │ │ │ │ │ -334 & boost::serialization::make_nvp("PinholeBaseK", │ │ │ │ │ -335 boost::serialization::base_object(*this)); │ │ │ │ │ -336 ar & BOOST_SERIALIZATION_NVP(K_); │ │ │ │ │ -337 } │ │ │ │ │ -338 │ │ │ │ │ -339public: │ │ │ │ │ -340 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -341}; │ │ │ │ │ -342 │ │ │ │ │ -343// manifold traits │ │ │ │ │ -344 │ │ │ │ │ -345template │ │ │ │ │ -_3_4_6struct _t_r_a_i_t_s<_P_i_n_h_o_l_e_C_a_m_e_r_a > │ │ │ │ │ -347 : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d > {}; │ │ │ │ │ -348 │ │ │ │ │ -349template │ │ │ │ │ -_3_5_0struct _t_r_a_i_t_s > │ │ │ │ │ -351 : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d > {}; │ │ │ │ │ -352 │ │ │ │ │ -353// range traits, used in RangeFactor │ │ │ │ │ -354template │ │ │ │ │ -_3_5_5struct _R_a_n_g_e<_P_i_n_h_o_l_e_C_a_m_e_r_a, T> : │ │ │ │ │ -_H_a_s_R_a_n_g_e, T, double> {}; │ │ │ │ │ -356 │ │ │ │ │ -357} // \ gtsam │ │ │ │ │ -_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ -This marks a GTSAM object to require alignment. │ │ │ │ │ -DDeeffiinniittiioonn types.h:308 │ │ │ │ │ -_B_e_a_r_i_n_g_R_a_n_g_e_._h │ │ │ │ │ -Bearing-Range product. │ │ │ │ │ -_P_i_n_h_o_l_e_P_o_s_e_._h │ │ │ │ │ -Pinhole camera with known calibration. │ │ │ │ │ +257 std::pair jacobian() const; │ │ │ │ │ +258 │ │ │ │ │ +270 Matrix augmentedHessian(const _O_r_d_e_r_i_n_g& ordering) const; │ │ │ │ │ +271 │ │ │ │ │ +283 Matrix augmentedHessian() const; │ │ │ │ │ +284 │ │ │ │ │ +291 std::pair hessian(const _O_r_d_e_r_i_n_g& ordering) const; │ │ │ │ │ +292 │ │ │ │ │ +299 std::pair hessian() const; │ │ │ │ │ +300 │ │ │ │ │ +302 virtual _V_e_c_t_o_r_V_a_l_u_e_s hessianDiagonal() const; │ │ │ │ │ +303 │ │ │ │ │ +305 virtual std::map hessianBlockDiagonal() const; │ │ │ │ │ +306 │ │ │ │ │ +311 _V_e_c_t_o_r_V_a_l_u_e_s _o_p_t_i_m_i_z_e( │ │ │ │ │ +312 const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; │ │ │ │ │ +313 │ │ │ │ │ +318 _V_e_c_t_o_r_V_a_l_u_e_s _o_p_t_i_m_i_z_e(const _O_r_d_e_r_i_n_g&, │ │ │ │ │ +319 const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; │ │ │ │ │ +320 │ │ │ │ │ +324 _V_e_c_t_o_r_V_a_l_u_e_s optimizeDensely() const; │ │ │ │ │ +325 │ │ │ │ │ +335 _V_e_c_t_o_r_V_a_l_u_e_s gradient(const _V_e_c_t_o_r_V_a_l_u_e_s& x0) const; │ │ │ │ │ +336 │ │ │ │ │ +344 virtual _V_e_c_t_o_r_V_a_l_u_e_s gradientAtZero() const; │ │ │ │ │ +345 │ │ │ │ │ +370 _V_e_c_t_o_r_V_a_l_u_e_s optimizeGradientSearch() const; │ │ │ │ │ +371 │ │ │ │ │ +373 _V_e_c_t_o_r_V_a_l_u_e_s transposeMultiply(const _E_r_r_o_r_s& e) const; │ │ │ │ │ +374 │ │ │ │ │ +376 void transposeMultiplyAdd(double alpha, const _E_r_r_o_r_s& e, _V_e_c_t_o_r_V_a_l_u_e_s& x) │ │ │ │ │ +const; │ │ │ │ │ +377 │ │ │ │ │ +379 _E_r_r_o_r_s gaussianErrors(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const; │ │ │ │ │ +380 │ │ │ │ │ +382 _E_r_r_o_r_s _o_p_e_r_a_t_o_r_*(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const; │ │ │ │ │ +383 │ │ │ │ │ +385 void multiplyHessianAdd(double alpha, const _V_e_c_t_o_r_V_a_l_u_e_s& x, │ │ │ │ │ +386 _V_e_c_t_o_r_V_a_l_u_e_s& y) const; │ │ │ │ │ +387 │ │ │ │ │ +389 void multiplyInPlace(const _V_e_c_t_o_r_V_a_l_u_e_s& x, _E_r_r_o_r_s& e) const; │ │ │ │ │ +390 │ │ │ │ │ +392 void multiplyInPlace(const _V_e_c_t_o_r_V_a_l_u_e_s& x, const Errors::iterator& e) │ │ │ │ │ +const; │ │ │ │ │ +393 │ │ │ │ │ +394 void printErrors( │ │ │ │ │ +395 const _V_e_c_t_o_r_V_a_l_u_e_s& x, │ │ │ │ │ +396 const std::string& str = "GaussianFactorGraph: ", │ │ │ │ │ +397 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ +398 const std::function& │ │ │ │ │ +400 printCondition = │ │ │ │ │ +401 [](const _F_a_c_t_o_r*, double, size_t) { return true; }) const; │ │ │ │ │ +403 │ │ │ │ │ +404 private: │ │ │ │ │ +_4_0_6 friend class boost::serialization::access; │ │ │ │ │ +407 template │ │ │ │ │ +408 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +409 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +410 } │ │ │ │ │ +411 │ │ │ │ │ +412 public: │ │ │ │ │ +413 │ │ │ │ │ +414#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ +416 _V_e_c_t_o_r_V_a_l_u_e_s GTSAM_DEPRECATED │ │ │ │ │ +417 _o_p_t_i_m_i_z_e(boost::none_t, const Eliminate& function = │ │ │ │ │ +418 EliminationTraitsType::DefaultEliminate) const { │ │ │ │ │ +419 return _o_p_t_i_m_i_z_e(function); │ │ │ │ │ +420 } │ │ │ │ │ +421#endif │ │ │ │ │ +422 │ │ │ │ │ +423 }; │ │ │ │ │ +424 │ │ │ │ │ +429 GTSAM_EXPORT bool _h_a_s_C_o_n_s_t_r_a_i_n_t_s(const GaussianFactorGraph& factors); │ │ │ │ │ +430 │ │ │ │ │ +431 /****** Linear Algebra Operations ******/ │ │ │ │ │ +432 │ │ │ │ │ +434 //GTSAM_EXPORT void residual(const GaussianFactorGraph& fg, const │ │ │ │ │ +VectorValues &x, VectorValues &r); │ │ │ │ │ +435 //GTSAM_EXPORT void multiply(const GaussianFactorGraph& fg, const │ │ │ │ │ +VectorValues &x, VectorValues &r); │ │ │ │ │ +436 │ │ │ │ │ +438template<> │ │ │ │ │ +_4_3_9struct _t_r_a_i_t_s<_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h> : public _T_e_s_t_a_b_l_e { │ │ │ │ │ +440}; │ │ │ │ │ +441 │ │ │ │ │ +442} // \ namespace gtsam │ │ │ │ │ +_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Factor Graph Base Class. │ │ │ │ │ +_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Variable elimination algorithms for factor graphs. │ │ │ │ │ +_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ +_H_e_s_s_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ +Contains the HessianFactor class, a general quadratic factor. │ │ │ │ │ +_E_r_r_o_r_s_._h │ │ │ │ │ +vector of errors │ │ │ │ │ +_G_a_u_s_s_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ +A factor with a quadratic error function - a Gaussian. │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ +Factor Graph Values. │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_P_r_e_f_e_r_C_h_o_l_e_s_k_y │ │ │ │ │ +std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< │ │ │ │ │ +GaussianFactor > > EliminatePreferCholesky(const GaussianFactorGraph &factors, │ │ │ │ │ +const Ordering &keys) │ │ │ │ │ +Densely partially eliminate with Cholesky factorization. │ │ │ │ │ +DDeeffiinniittiioonn HessianFactor.cpp:548 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ -Vector2 Point2 │ │ │ │ │ -As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point2 to Vector2... │ │ │ │ │ -DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ -Vector3 Point3 │ │ │ │ │ -As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point3 to Vector3... │ │ │ │ │ -DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_h_a_s_C_o_n_s_t_r_a_i_n_t_s │ │ │ │ │ +bool hasConstraints(const GaussianFactorGraph &factors) │ │ │ │ │ +Evaluates whether linear factors have any constrained noise models. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.cpp:442 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ +Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key │ │ │ │ │ +landmarkKey) │ │ │ │ │ +Optimize for triangulation. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.cpp:155 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ +Point2 operator*(double s, const Point2 &p) │ │ │ │ │ +multiply with scalar │ │ │ │ │ +DDeeffiinniittiioonn Point2.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ -TangentVector localCoordinates(const Class &g) const │ │ │ │ │ -localCoordinates as required by manifold concept: finds tangent vector between │ │ │ │ │ -*this and g │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:136 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ -Both ManifoldTraits and Testable. │ │ │ │ │ -DDeeffiinniittiioonn Manifold.h:120 │ │ │ │ │ -_g_t_s_a_m_:_:_F_i_x_e_d_D_i_m_e_n_s_i_o_n │ │ │ │ │ -Give fixed size dimension of a type, fails at compile time if dynamic. │ │ │ │ │ -DDeeffiinniittiioonn Manifold.h:164 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ -either a fixed size o... │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_R_a_n_g_e │ │ │ │ │ -DDeeffiinniittiioonn BearingRange.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_H_a_s_R_a_n_g_e │ │ │ │ │ -DDeeffiinniittiioonn BearingRange.h:194 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_D_p_o_s_e │ │ │ │ │ -static Matrix26 Dpose(const Point2 &pn, double d) │ │ │ │ │ -Calculate Jacobian with respect to pose. │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.cpp:27 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_p_r_i_n_t │ │ │ │ │ -virtual void print(const std::string &s="PinholeBase") const │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.cpp:74 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_p_o_s_e │ │ │ │ │ -const Pose3 & pose() const │ │ │ │ │ -return pose, constant version │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:152 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_L_e_v_e_l_P_o_s_e │ │ │ │ │ -static Pose3 LevelPose(const Pose2 &pose2, double height) │ │ │ │ │ -Create a level pose at the given 2D pose and height. │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.cpp:49 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const PinholeBase &camera, double tol=1e-9) const │ │ │ │ │ -assert equality up to a tolerance │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.cpp:69 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_D_p_o_i_n_t │ │ │ │ │ -static Matrix23 Dpoint(const Point2 &pn, double d, const Matrix3 &Rt) │ │ │ │ │ -Calculate Jacobian with respect to point. │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.cpp:37 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_L_o_o_k_a_t_P_o_s_e │ │ │ │ │ -static Pose3 LookatPose(const Point3 &eye, const Point3 &target, const Point3 │ │ │ │ │ -&upVector) │ │ │ │ │ -Create a camera pose at the given eye position looking at a target point in the │ │ │ │ │ -scene with the specif... │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.cpp:58 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a │ │ │ │ │ -A Calibrated camera class [R|-R't], calibration K=I. │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:247 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a │ │ │ │ │ -A pinhole camera class that has a Pose3 and a Calibration. │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:33 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="PinholeCamera") const override │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:___p_r_o_j_e_c_t_2 │ │ │ │ │ -Point2 _project2(const POINT &pw, OptionalJacobian< 2, dimension > Dcamera, │ │ │ │ │ -OptionalJacobian< 2, FixedDimension< POINT >::value > Dpoint) const │ │ │ │ │ -Templated projection of a 3D point or a point at infinity into the image. │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:230 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_d_e_f_a_u_l_t_E_r_r_o_r_W_h_e_n_T_r_i_a_n_g_u_l_a_t_i_n_g_B_e_h_i_n_d_C_a_m_e_r_a │ │ │ │ │ -Vector defaultErrorWhenTriangulatingBehindCamera() const │ │ │ │ │ -for Nonlinear Triangulation │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:323 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_c_a_l_i_b_r_a_t_i_o_n │ │ │ │ │ -const Calibration & calibration() const override │ │ │ │ │ -return calibration │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:178 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_g_e_t_P_o_s_e │ │ │ │ │ -const Pose3 & getPose(OptionalJacobian< 6, dimension > H) const │ │ │ │ │ -return pose, with derivative │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:169 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_p_r_o_j_e_c_t_2 │ │ │ │ │ -Point2 project2(const Point3 &pw, OptionalJacobian< 2, dimension > │ │ │ │ │ -Dcamera=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none) const │ │ │ │ │ -project a 3D point from world coordinates into the image │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:245 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a │ │ │ │ │ -PinholeCamera(const Pose3 &pose) │ │ │ │ │ -constructor with pose │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:66 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_L_e_v_e_l │ │ │ │ │ -static PinholeCamera Level(const Pose2 &pose2, double height) │ │ │ │ │ -PinholeCamera::level with default calibration. │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:92 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a │ │ │ │ │ -PinholeCamera(const Pose3 &pose, const Calibration &K) │ │ │ │ │ -constructor with pose and calibration │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:71 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_L_e_v_e_l │ │ │ │ │ -static PinholeCamera Level(const Calibration &K, const Pose2 &pose2, double │ │ │ │ │ -height) │ │ │ │ │ -Create a level camera at the given 2D pose and height. │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_d_i_m │ │ │ │ │ -size_t dim() const │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:187 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_I_d_e_n_t_i_t_y │ │ │ │ │ -static PinholeCamera Identity() │ │ │ │ │ -for Canonical │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:216 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_D_i_m │ │ │ │ │ -static size_t Dim() │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:192 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const Base &camera, double tol=1e-9) const │ │ │ │ │ -assert equality up to a tolerance │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:145 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_r_a_n_g_e │ │ │ │ │ -double range(const Pose3 &pose, OptionalJacobian< 1, dimension > │ │ │ │ │ -Dcamera=boost::none, OptionalJacobian< 1, 6 > Dpose=boost::none) const │ │ │ │ │ -Calculate range to another pose. │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:275 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_r_a_n_g_e │ │ │ │ │ -double range(const PinholeCamera< CalibrationB > &camera, OptionalJacobian< 1, │ │ │ │ │ -dimension > Dcamera=boost::none, OptionalJacobian< 1, 6+CalibrationB::dimension │ │ │ │ │ -> Dother=boost::none) const │ │ │ │ │ -Calculate range to another camera. │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:290 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a │ │ │ │ │ -PinholeCamera(const Vector &v, const Vector &K) │ │ │ │ │ -Init from Vector and calibration. │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:136 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_L_o_o_k_a_t │ │ │ │ │ -static PinholeCamera Lookat(const Point3 &eye, const Point3 &target, const │ │ │ │ │ -Point3 &upVector, const Calibration &K=Calibration()) │ │ │ │ │ -Create a camera at the given eye position looking at a target point in the │ │ │ │ │ -scene with the specified u... │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:105 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_c_a_m_e_r_a_P_r_o_j_e_c_t_i_o_n_M_a_t_r_i_x │ │ │ │ │ -Matrix34 cameraProjectionMatrix() const │ │ │ │ │ -for Linear Triangulation │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:318 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ -VectorK6 localCoordinates(const PinholeCamera &T2) const │ │ │ │ │ -return canonical coordinate │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:208 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_r_e_t_r_a_c_t │ │ │ │ │ -PinholeCamera retract(const Vector &d) const │ │ │ │ │ -move a cameras according to d │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:199 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:330 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_p_r_o_j_e_c_t_2 │ │ │ │ │ -Point2 project2(const Unit3 &pw, OptionalJacobian< 2, dimension > │ │ │ │ │ -Dcamera=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const │ │ │ │ │ -project a point at infinity from world coordinates into the image │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:251 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_p_o_s_e │ │ │ │ │ -const Pose3 & pose() const │ │ │ │ │ -return pose │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:164 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a │ │ │ │ │ -PinholeCamera(const Vector &v) │ │ │ │ │ -Init from vector, can be 6D (default calibration) or dim. │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:129 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a │ │ │ │ │ -PinholeCamera() │ │ │ │ │ -default constructor │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:62 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_M_e_a_s_u_r_e_m_e_n_t │ │ │ │ │ -Point2 Measurement │ │ │ │ │ -Some classes template on either PinholeCamera or StereoCamera, and this typedef │ │ │ │ │ -informs those classes... │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_r_a_n_g_e │ │ │ │ │ -double range(const CalibratedCamera &camera, OptionalJacobian< 1, dimension > │ │ │ │ │ -Dcamera=boost::none, OptionalJacobian< 1, 6 > Dother=boost::none) const │ │ │ │ │ -Calculate range to a calibrated camera. │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:311 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a_:_:_r_a_n_g_e │ │ │ │ │ -double range(const Point3 &point, OptionalJacobian< 1, dimension > │ │ │ │ │ -Dcamera=boost::none, OptionalJacobian< 1, 3 > Dpoint=boost::none) const │ │ │ │ │ -Calculate range to a landmark. │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:261 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K │ │ │ │ │ -A pinhole camera class that has a Pose3 and a fixed Calibration. │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_<_ _C_a_l_i_b_r_a_t_i_o_n_ _>_:_:_p_r_o_j_e_c_t │ │ │ │ │ -Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, │ │ │ │ │ -OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > │ │ │ │ │ -Dcal=boost::none) const │ │ │ │ │ -project a 3D point from world coordinates into the image │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:118 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2 │ │ │ │ │ -A 2D pose (Point2,Rot2) │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ -A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_r_a_n_g_e │ │ │ │ │ -double range(const Point3 &point, OptionalJacobian< 1, 6 > Hself=boost::none, │ │ │ │ │ -OptionalJacobian< 1, 3 > Hpoint=boost::none) const │ │ │ │ │ -Calculate range to a landmark. │ │ │ │ │ -DDeeffiinniittiioonn Pose3.cpp:399 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_m_a_t_r_i_x │ │ │ │ │ -Matrix4 matrix() const │ │ │ │ │ -convert to 4*4 matrix │ │ │ │ │ -DDeeffiinniittiioonn Pose3.cpp:323 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_i_n_v_e_r_s_e │ │ │ │ │ -Pose3 inverse() const │ │ │ │ │ -inverse transformation with derivatives │ │ │ │ │ -DDeeffiinniittiioonn Pose3.cpp:49 │ │ │ │ │ -_g_t_s_a_m_:_:_U_n_i_t_3 │ │ │ │ │ -Represents a 3D point on a unit sphere. │ │ │ │ │ -DDeeffiinniittiioonn Unit3.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t_<_ _V_e_c_t_o_r_ _> │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ +nodes. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_i_s_E_q_u_a_l │ │ │ │ │ +bool isEqual(const FactorGraph &other) const │ │ │ │ │ +Check exact equality of the factor pointers. Useful for derived ==. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:134 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_ _>_:_:_s_h_a_r_e_d_F_a_c_t_o_r │ │ │ │ │ +boost::shared_ptr< GaussianFactor > sharedFactor │ │ │ │ │ +Shared pointer to a factor. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:101 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s │ │ │ │ │ +Traits class for eliminateable factor graphs, specifies the types that result │ │ │ │ │ +from elimination,... │ │ │ │ │ +DDeeffiinniittiioonn EliminateableFactorGraph.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +EliminateableFactorGraph is a base class for factor graphs that contains │ │ │ │ │ +elimination algorithms. │ │ │ │ │ +DDeeffiinniittiioonn EliminateableFactorGraph.h:57 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:68 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d │ │ │ │ │ +static Ordering Colamd(const FACTOR_GRAPH &graph) │ │ │ │ │ +Compute a fill-reducing ordering using COLAMD from a factor graph (see details │ │ │ │ │ +for note on performanc... │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:95 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t │ │ │ │ │ +GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals. │ │ │ │ │ +DDeeffiinniittiioonn GaussianBayesNet.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e │ │ │ │ │ +A Bayes tree representing a Gaussian density. │ │ │ │ │ +DDeeffiinniittiioonn GaussianBayesTree.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +A GaussianConditional functions as the node in a Bayes network. │ │ │ │ │ +DDeeffiinniittiioonn GaussianConditional.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ +DDeeffiinniittiioonn GaussianEliminationTree.h:29 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +An abstract virtual base class for JacobianFactor and HessianFactor. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactor.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ +virtual GaussianFactor::shared_ptr clone() const =0 │ │ │ │ │ +Clone a factor (make a deep copy) │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_D_e_f_a_u_l_t_O_r_d_e_r_i_n_g_F_u_n_c │ │ │ │ │ +static Ordering DefaultOrderingFunc(const FactorGraphType &graph, boost:: │ │ │ │ │ +optional< const VariableIndex & > variableIndex) │ │ │ │ │ +The default ordering generation function. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_B_a_y_e_s_T_r_e_e_T_y_p_e │ │ │ │ │ +GaussianBayesTree BayesTreeType │ │ │ │ │ +Type of Bayes tree. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:51 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_C_o_n_d_i_t_i_o_n_a_l_T_y_p_e │ │ │ │ │ +GaussianConditional ConditionalType │ │ │ │ │ +Type of conditionals from elimination. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_F_a_c_t_o_r_T_y_p_e │ │ │ │ │ +GaussianFactor FactorType │ │ │ │ │ +Type of factors in factor graph. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_T_y_p_e │ │ │ │ │ +GaussianEliminationTree EliminationTreeType │ │ │ │ │ +Type of elimination tree. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_F_a_c_t_o_r_G_r_a_p_h_T_y_p_e │ │ │ │ │ +GaussianFactorGraph FactorGraphType │ │ │ │ │ +Type of the factor graph (e.g. GaussianFactorGraph) │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_B_a_y_e_s_N_e_t_T_y_p_e │ │ │ │ │ +GaussianBayesNet BayesNetType │ │ │ │ │ +Type of Bayes net from sequential elimination. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:49 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_J_u_n_c_t_i_o_n_T_r_e_e_T_y_p_e │ │ │ │ │ +GaussianJunctionTree JunctionTreeType │ │ │ │ │ +Type of Junction tree. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_D_e_f_a_u_l_t_E_l_i_m_i_n_a_t_e │ │ │ │ │ +static std::pair< boost::shared_ptr< ConditionalType >, boost::shared_ptr< │ │ │ │ │ +FactorType > > DefaultEliminate(const FactorGraphType &factors, const Ordering │ │ │ │ │ +&keys) │ │ │ │ │ +The default dense elimination function. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_B_a_s_e_E_l_i_m_i_n_a_t_e_a_b_l_e │ │ │ │ │ +EliminateableFactorGraph< This > BaseEliminateable │ │ │ │ │ +Typedef to base elimination class. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:80 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:81 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d │ │ │ │ │ +void add(const TERMS &terms, const Vector &b, const SharedDiagonal │ │ │ │ │ +&model=SharedDiagonal()) │ │ │ │ │ +Add an n-ary factor. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:156 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +GaussianFactorGraph(std::initializer_list< sharedFactor > factors) │ │ │ │ │ +Construct from an initializer lists of GaussianFactor shared pointers. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:94 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +GaussianFactorGraph() │ │ │ │ │ +Default constructor. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:87 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d │ │ │ │ │ +void add(const GaussianFactor &factor) │ │ │ │ │ +Add a factor by value - makes a copy. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:127 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d │ │ │ │ │ +void add(Key key1, const Matrix &A1, Key key2, const Matrix &A2, Key key3, │ │ │ │ │ +const Matrix &A3, const Vector &b, const SharedDiagonal &model=SharedDiagonal │ │ │ │ │ +()) │ │ │ │ │ +Add a ternary factor. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:148 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d │ │ │ │ │ +void add(const sharedFactor &factor) │ │ │ │ │ +Add a factor by pointer - stores pointer without copying the factor. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:130 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_o_p_e_r_a_t_o_r_=_= │ │ │ │ │ +friend bool operator==(const GaussianFactorGraph &lhs, const │ │ │ │ │ +GaussianFactorGraph &rhs) │ │ │ │ │ +Check exact equality. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:121 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_T_h_i_s │ │ │ │ │ +GaussianFactorGraph This │ │ │ │ │ +Typedef to this class. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:78 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_K_e_y_s │ │ │ │ │ +KeySet Keys │ │ │ │ │ +Return the set of variables involved in the factors (computes a set union). │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:163 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d │ │ │ │ │ +void add(const Vector &b) │ │ │ │ │ +Add a null factor. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:133 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d │ │ │ │ │ +void add(Key key1, const Matrix &A1, const Vector &b, const SharedDiagonal │ │ │ │ │ +&model=SharedDiagonal()) │ │ │ │ │ +Add a unary factor. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:137 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_~_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +virtual ~GaussianFactorGraph() │ │ │ │ │ +Virtual destructor. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:110 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d │ │ │ │ │ +void add(Key key1, const Matrix &A1, Key key2, const Matrix &A2, const Vector │ │ │ │ │ +&b, const SharedDiagonal &model=SharedDiagonal()) │ │ │ │ │ +Add a binary factor. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:142 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +GaussianFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) │ │ │ │ │ +Construct from iterator over factors. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:99 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_B_a_s_e │ │ │ │ │ +FactorGraph< GaussianFactor > Base │ │ │ │ │ +Typedef to base factor graph type. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:79 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +GaussianFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph) │ │ │ │ │ +Implicit copy/downcast constructor to override explicit template container │ │ │ │ │ +constructor. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:107 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +GaussianFactorGraph(const CONTAINER &factors) │ │ │ │ │ +Construct from container of factors (shared_ptr or plain objects) │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:103 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ +A junction tree specialized to Gaussian factors, i.e., it is a cluster tree │ │ │ │ │ +with Gaussian factors sto... │ │ │ │ │ +DDeeffiinniittiioonn GaussianJunctionTree.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ +A Gaussian factor in the squared-error form. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:91 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ +with a unique integer... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +is the normalization constant. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _P_i_n_h_o_l_e_C_a_m_e_r_a_._h │ │ │ │ │ + * _l_i_n_e_a_r │ │ │ │ │ + * _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00341_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00443_source.html │ │ │ │┄ Files 93% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Pose3.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/CalibratedCamera.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Pose3.h
│ │ │ │ +
CalibratedCamera.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
17// \callgraph
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/config.h>
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/geometry/BearingRange.h>
│ │ │ │ -
23#include <gtsam/geometry/Point3.h>
│ │ │ │ -
24#include <gtsam/geometry/Rot3.h>
│ │ │ │ -
25#include <gtsam/base/Lie.h>
│ │ │ │ -
26
│ │ │ │ -
27namespace gtsam {
│ │ │ │ -
28
│ │ │ │ -
29class Pose2;
│ │ │ │ -
30// forward declare
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/geometry/BearingRange.h>
│ │ │ │ +
22#include <gtsam/geometry/Point2.h>
│ │ │ │ +
23#include <gtsam/geometry/Pose3.h>
│ │ │ │ +
24#include <gtsam/base/concepts.h>
│ │ │ │ +
25#include <gtsam/base/Manifold.h>
│ │ │ │ +
26#include <gtsam/base/ThreadsafeException.h>
│ │ │ │ +
27#include <gtsam/dllexport.h>
│ │ │ │ +
28#include <boost/serialization/nvp.hpp>
│ │ │ │ +
29
│ │ │ │ +
30namespace gtsam {
│ │ │ │
31
│ │ │ │ -
│ │ │ │ -
37class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
│ │ │ │ -
38public:
│ │ │ │ -
39
│ │ │ │ -
41 typedef Rot3 Rotation;
│ │ │ │ -
42 typedef Point3 Translation;
│ │ │ │ -
43
│ │ │ │ -
44private:
│ │ │ │ -
45
│ │ │ │ -
46 Rot3 R_;
│ │ │ │ -
47 Point3 t_;
│ │ │ │ -
48
│ │ │ │ -
49public:
│ │ │ │ -
50
│ │ │ │ +
│ │ │ │ +
32class GTSAM_EXPORT CheiralityException: public ThreadsafeException<CheiralityException> {
│ │ │ │ +
33public:
│ │ │ │ +
34 CheiralityException()
│ │ │ │ +
35 : CheiralityException(std::numeric_limits<Key>::max()) {}
│ │ │ │ +
36
│ │ │ │ +
37 CheiralityException(Key j)
│ │ │ │ +
38 : ThreadsafeException<CheiralityException>("CheiralityException"),
│ │ │ │ +
39 j_(j) {}
│ │ │ │ +
40
│ │ │ │ +
41 Key nearbyVariable() const {return j_;}
│ │ │ │ +
42
│ │ │ │ +
43private:
│ │ │ │ +
44 Key j_;
│ │ │ │ +
45};
│ │ │ │ +
│ │ │ │ +
46
│ │ │ │ +
│ │ │ │ +
52class GTSAM_EXPORT PinholeBase {
│ │ │ │
53
│ │ │ │ -
55 Pose3() : R_(traits<Rot3>::Identity()), t_(traits<Point3>::Identity()) {}
│ │ │ │ -
56
│ │ │ │ -
│ │ │ │ -
58 Pose3(const Pose3& pose) :
│ │ │ │ -
59 R_(pose.R_), t_(pose.t_) {
│ │ │ │ -
60 }
│ │ │ │ -
│ │ │ │ -
61
│ │ │ │ -
│ │ │ │ -
63 Pose3(const Rot3& R, const Point3& t) :
│ │ │ │ -
64 R_(R), t_(t) {
│ │ │ │ -
65 }
│ │ │ │ -
│ │ │ │ +
54public:
│ │ │ │ +
55
│ │ │ │ +
57 typedef Rot3 Rotation;
│ │ │ │ +
58 typedef Point3 Translation;
│ │ │ │ +
59
│ │ │ │ +
64 typedef Point2 Measurement;
│ │ │ │ +
65 typedef Point2Vector MeasurementVector;
│ │ │ │
66
│ │ │ │ -
68 explicit Pose3(const Pose2& pose2);
│ │ │ │ -
69
│ │ │ │ -
│ │ │ │ -
71 Pose3(const Matrix &T) :
│ │ │ │ -
72 R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), T(2, 1),
│ │ │ │ -
73 T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {
│ │ │ │ -
74 }
│ │ │ │ -
│ │ │ │ +
67private:
│ │ │ │ +
68
│ │ │ │ +
69 Pose3 pose_;
│ │ │ │ +
70
│ │ │ │ +
71protected:
│ │ │ │ +
72
│ │ │ │
75
│ │ │ │ -
77 static Pose3 Create(const Rot3& R, const Point3& t,
│ │ │ │ -
78 OptionalJacobian<6, 3> HR = boost::none,
│ │ │ │ -
79 OptionalJacobian<6, 3> Ht = boost::none);
│ │ │ │ -
80
│ │ │ │ -
86 static boost::optional<Pose3> Align(const Point3Pairs& abPointPairs);
│ │ │ │ -
87
│ │ │ │ -
88 // Version of Pose3::Align that takes 2 matrices.
│ │ │ │ -
89 static boost::optional<Pose3> Align(const Matrix& a, const Matrix& b);
│ │ │ │ +
81 static Matrix26 Dpose(const Point2& pn, double d);
│ │ │ │ +
82
│ │ │ │ +
89 static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt);
│ │ │ │
90
│ │ │ │ +
92
│ │ │ │ +
93public:
│ │ │ │
94
│ │ │ │ -
96 void print(const std::string& s = "") const;
│ │ │ │
97
│ │ │ │ -
99 bool equals(const Pose3& pose, double tol = 1e-9) const;
│ │ │ │ -
100
│ │ │ │ -
104
│ │ │ │ -
│ │ │ │ -
106 static Pose3 Identity() {
│ │ │ │ -
107 return Pose3();
│ │ │ │ -
108 }
│ │ │ │ -
│ │ │ │ -
109
│ │ │ │ -
111 Pose3 inverse() const;
│ │ │ │ -
112
│ │ │ │ -
│ │ │ │ -
114 Pose3 operator*(const Pose3& T) const {
│ │ │ │ -
115 return Pose3(R_ * T.R_, t_ + R_ * T.t_);
│ │ │ │ -
116 }
│ │ │ │ -
│ │ │ │ +
105 static Pose3 LevelPose(const Pose2& pose2, double height);
│ │ │ │ +
106
│ │ │ │ +
115 static Pose3 LookatPose(const Point3& eye, const Point3& target,
│ │ │ │ +
116 const Point3& upVector);
│ │ │ │
117
│ │ │ │ -
132 Pose3 interpolateRt(const Pose3& T, double t) const;
│ │ │ │ +
121
│ │ │ │ +
123 PinholeBase() {}
│ │ │ │ +
124
│ │ │ │ +
126 explicit PinholeBase(const Pose3& pose) : pose_(pose) {}
│ │ │ │ +
127
│ │ │ │ +
131
│ │ │ │ +
132 explicit PinholeBase(const Vector& v) : pose_(Pose3::Expmap(v)) {}
│ │ │ │
133
│ │ │ │ -
137
│ │ │ │ -
139 static Pose3 Expmap(const Vector6& xi, OptionalJacobian<6, 6> Hxi = boost::none);
│ │ │ │ +
135 virtual ~PinholeBase() = default;
│ │ │ │ +
136
│ │ │ │
140
│ │ │ │ -
142 static Vector6 Logmap(const Pose3& pose, OptionalJacobian<6, 6> Hpose = boost::none);
│ │ │ │ +
142 bool equals(const PinholeBase &camera, double tol = 1e-9) const;
│ │ │ │
143
│ │ │ │ -
148 Matrix6 AdjointMap() const;
│ │ │ │ -
149
│ │ │ │ -
156 Vector6 Adjoint(const Vector6& xi_b,
│ │ │ │ -
157 OptionalJacobian<6, 6> H_this = boost::none,
│ │ │ │ -
158 OptionalJacobian<6, 6> H_xib = boost::none) const;
│ │ │ │ -
159
│ │ │ │ -
161 Vector6 AdjointTranspose(const Vector6& x,
│ │ │ │ -
162 OptionalJacobian<6, 6> H_this = boost::none,
│ │ │ │ -
163 OptionalJacobian<6, 6> H_x = boost::none) const;
│ │ │ │ -
164
│ │ │ │ -
180 static Matrix6 adjointMap(const Vector6& xi);
│ │ │ │ -
181
│ │ │ │ -
185 static Vector6 adjoint(const Vector6& xi, const Vector6& y,
│ │ │ │ -
186 OptionalJacobian<6, 6> Hxi = boost::none,
│ │ │ │ -
187 OptionalJacobian<6, 6> H_y = boost::none);
│ │ │ │ +
145 virtual void print(const std::string& s = "PinholeBase") const;
│ │ │ │ +
146
│ │ │ │ +
150
│ │ │ │ +
│ │ │ │ +
152 const Pose3& pose() const {
│ │ │ │ +
153 return pose_;
│ │ │ │ +
154 }
│ │ │ │ +
│ │ │ │ +
155
│ │ │ │ +
│ │ │ │ +
157 const Rot3& rotation() const {
│ │ │ │ +
158 return pose_.rotation();
│ │ │ │ +
159 }
│ │ │ │ +
│ │ │ │ +
160
│ │ │ │ +
│ │ │ │ +
162 const Point3& translation() const {
│ │ │ │ +
163 return pose_.translation();
│ │ │ │ +
164 }
│ │ │ │ +
│ │ │ │ +
165
│ │ │ │ +
167 const Pose3& getPose(OptionalJacobian<6, 6> H) const;
│ │ │ │ +
168
│ │ │ │ +
172
│ │ │ │ +
178 static Point2 Project(const Point3& pc, //
│ │ │ │ +
179 OptionalJacobian<2, 3> Dpoint = boost::none);
│ │ │ │ +
180
│ │ │ │ +
186 static Point2 Project(const Unit3& pc, //
│ │ │ │ +
187 OptionalJacobian<2, 2> Dpoint = boost::none);
│ │ │ │
188
│ │ │ │ -
189 // temporary fix for wrappers until case issue is resolved
│ │ │ │ -
190 static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);}
│ │ │ │ -
191 static Vector6 adjoint_(const Vector6 &xi, const Vector6 &y) { return adjoint(xi, y);}
│ │ │ │ -
192
│ │ │ │ -
196 static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y,
│ │ │ │ -
197 OptionalJacobian<6, 6> Hxi = boost::none,
│ │ │ │ -
198 OptionalJacobian<6, 6> H_y = boost::none);
│ │ │ │ +
190 std::pair<Point2, bool> projectSafe(const Point3& pw) const;
│ │ │ │ +
191
│ │ │ │ +
197 Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose =
│ │ │ │ +
198 boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
│ │ │ │
199
│ │ │ │ -
201 static Matrix6 ExpmapDerivative(const Vector6& xi);
│ │ │ │ -
202
│ │ │ │ -
204 static Matrix6 LogmapDerivative(const Pose3& xi);
│ │ │ │ -
205
│ │ │ │ -
206 // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP
│ │ │ │ -
│ │ │ │ -
207 struct ChartAtOrigin {
│ │ │ │ -
208 static Pose3 Retract(const Vector6& xi, ChartJacobian Hxi = boost::none);
│ │ │ │ -
209 static Vector6 Local(const Pose3& pose, ChartJacobian Hpose = boost::none);
│ │ │ │ -
210 };
│ │ │ │ -
│ │ │ │ -
211
│ │ │ │ -
221 static Matrix3 ComputeQforExpmapDerivative(
│ │ │ │ -
222 const Vector6& xi, double nearZeroThreshold = 1e-5);
│ │ │ │ -
223
│ │ │ │ -
224 using LieGroup<Pose3, 6>::inverse; // version with derivative
│ │ │ │ -
225
│ │ │ │ -
│ │ │ │ -
233 static Matrix wedge(double wx, double wy, double wz, double vx, double vy,
│ │ │ │ -
234 double vz) {
│ │ │ │ -
235 return (Matrix(4, 4) << 0., -wz, wy, vx, wz, 0., -wx, vy, -wy, wx, 0., vz, 0., 0., 0., 0.).finished();
│ │ │ │ +
205 Point2 project2(const Unit3& point,
│ │ │ │ +
206 OptionalJacobian<2, 6> Dpose = boost::none,
│ │ │ │ +
207 OptionalJacobian<2, 2> Dpoint = boost::none) const;
│ │ │ │ +
208
│ │ │ │ +
210 static Point3 BackprojectFromCamera(const Point2& p, const double depth,
│ │ │ │ +
211 OptionalJacobian<3, 2> Dpoint = boost::none,
│ │ │ │ +
212 OptionalJacobian<3, 1> Ddepth = boost::none);
│ │ │ │ +
213
│ │ │ │ +
217
│ │ │ │ +
│ │ │ │ +
223 inline static std::pair<size_t, size_t> translationInterval() {
│ │ │ │ +
224 return std::make_pair(3, 5);
│ │ │ │ +
225 }
│ │ │ │ +
│ │ │ │ +
226
│ │ │ │ +
228
│ │ │ │ +
229private:
│ │ │ │ +
230
│ │ │ │ +
232 friend class boost::serialization::access;
│ │ │ │ +
233 template<class Archive>
│ │ │ │ +
234 void serialize(Archive & ar, const unsigned int /*version*/) {
│ │ │ │ +
235 ar & BOOST_SERIALIZATION_NVP(pose_);
│ │ │ │
236 }
│ │ │ │ +
237};
│ │ │ │ +
│ │ │ │ +
238// end of class PinholeBase
│ │ │ │ +
239
│ │ │ │ +
│ │ │ │ +
247class GTSAM_EXPORT CalibratedCamera: public PinholeBase {
│ │ │ │ +
248
│ │ │ │ +
249public:
│ │ │ │ +
250
│ │ │ │ +
251 enum {
│ │ │ │ +
252 dimension = 6
│ │ │ │ +
253 };
│ │ │ │ +
254
│ │ │ │ +
257
│ │ │ │ +
│ │ │ │ +
259 CalibratedCamera() {
│ │ │ │ +
260 }
│ │ │ │ +
│ │ │ │ +
261
│ │ │ │ +
│ │ │ │ +
263 explicit CalibratedCamera(const Pose3& pose) :
│ │ │ │ +
264 PinholeBase(pose) {
│ │ │ │ +
265 }
│ │ │ │ +
│ │ │ │ +
266
│ │ │ │ +
270
│ │ │ │ +
271 // Create CalibratedCamera, with derivatives
│ │ │ │ +
272 static CalibratedCamera Create(const Pose3& pose,
│ │ │ │ +
273 OptionalJacobian<dimension, 6> H1 = boost::none) {
│ │ │ │ +
274 if (H1)
│ │ │ │ +
275 *H1 << I_6x6;
│ │ │ │ +
276 return CalibratedCamera(pose);
│ │ │ │ +
277 }
│ │ │ │ +
278
│ │ │ │ +
285 static CalibratedCamera Level(const Pose2& pose2, double height);
│ │ │ │ +
286
│ │ │ │ +
295 static CalibratedCamera Lookat(const Point3& eye, const Point3& target,
│ │ │ │ +
296 const Point3& upVector);
│ │ │ │ +
297
│ │ │ │ +
301
│ │ │ │ +
│ │ │ │ +
303 explicit CalibratedCamera(const Vector &v) :
│ │ │ │ +
304 PinholeBase(v) {
│ │ │ │ +
305 }
│ │ │ │ +
│ │ │ │ +
306
│ │ │ │ +
310
│ │ │ │ +
│ │ │ │ +
312 virtual ~CalibratedCamera() {
│ │ │ │ +
313 }
│ │ │ │ +
│ │ │ │ +
314
│ │ │ │ +
318
│ │ │ │ +
320 CalibratedCamera retract(const Vector& d) const;
│ │ │ │ +
321
│ │ │ │ +
323 Vector localCoordinates(const CalibratedCamera& T2) const;
│ │ │ │ +
324
│ │ │ │ +
│ │ │ │ +
326 void print(const std::string& s = "CalibratedCamera") const override {
│ │ │ │ +
327 PinholeBase::print(s);
│ │ │ │ +
328 }
│ │ │ │ +
│ │ │ │ +
329
│ │ │ │ +
│ │ │ │ +
331 inline size_t dim() const {
│ │ │ │ +
332 return dimension;
│ │ │ │ +
333 }
│ │ │ │ +
│ │ │ │ +
334
│ │ │ │ +
│ │ │ │ +
336 inline static size_t Dim() {
│ │ │ │ +
337 return dimension;
│ │ │ │ +
338 }
│ │ │ │
│ │ │ │ -
237
│ │ │ │ -
241
│ │ │ │ -
249 Point3 transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself =
│ │ │ │ -
250 boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
│ │ │ │ -
251
│ │ │ │ -
257 Matrix transformFrom(const Matrix& points) const;
│ │ │ │ -
258
│ │ │ │ -
│ │ │ │ -
260 inline Point3 operator*(const Point3& point) const {
│ │ │ │ -
261 return transformFrom(point);
│ │ │ │ -
262 }
│ │ │ │ -
│ │ │ │ -
263
│ │ │ │ -
271 Point3 transformTo(const Point3& point, OptionalJacobian<3, 6> Hself =
│ │ │ │ -
272 boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
│ │ │ │ -
273
│ │ │ │ -
279 Matrix transformTo(const Matrix& points) const;
│ │ │ │ -
280
│ │ │ │ -
284
│ │ │ │ -
286 const Rot3& rotation(OptionalJacobian<3, 6> Hself = boost::none) const;
│ │ │ │ -
287
│ │ │ │ -
289 const Point3& translation(OptionalJacobian<3, 6> Hself = boost::none) const;
│ │ │ │ -
290
│ │ │ │ -
│ │ │ │ -
292 double x() const {
│ │ │ │ -
293 return t_.x();
│ │ │ │ -
294 }
│ │ │ │ -
│ │ │ │ -
295
│ │ │ │ -
│ │ │ │ -
297 double y() const {
│ │ │ │ -
298 return t_.y();
│ │ │ │ -
299 }
│ │ │ │ -
│ │ │ │ -
300
│ │ │ │ -
│ │ │ │ -
302 double z() const {
│ │ │ │ -
303 return t_.z();
│ │ │ │ -
304 }
│ │ │ │ -
│ │ │ │ -
305
│ │ │ │ -
307 Matrix4 matrix() const;
│ │ │ │ -
308
│ │ │ │ -
314 Pose3 transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself = boost::none,
│ │ │ │ -
315 OptionalJacobian<6, 6> HaTb = boost::none) const;
│ │ │ │ -
316
│ │ │ │ -
321 Pose3 transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself = boost::none,
│ │ │ │ -
322 OptionalJacobian<6, 6> HwTb = boost::none) const;
│ │ │ │ -
323
│ │ │ │ -
329 double range(const Point3& point, OptionalJacobian<1, 6> Hself = boost::none,
│ │ │ │ -
330 OptionalJacobian<1, 3> Hpoint = boost::none) const;
│ │ │ │ -
331
│ │ │ │ -
337 double range(const Pose3& pose, OptionalJacobian<1, 6> Hself = boost::none,
│ │ │ │ -
338 OptionalJacobian<1, 6> Hpose = boost::none) const;
│ │ │ │
339
│ │ │ │ -
345 Unit3 bearing(const Point3& point, OptionalJacobian<2, 6> Hself = boost::none,
│ │ │ │ -
346 OptionalJacobian<2, 3> Hpoint = boost::none) const;
│ │ │ │ -
347
│ │ │ │ -
354 Unit3 bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself = boost::none,
│ │ │ │ -
355 OptionalJacobian<2, 6> Hpose = boost::none) const;
│ │ │ │ +
343
│ │ │ │ +
348 Point2 project(const Point3& point, OptionalJacobian<2, 6> Dcamera =
│ │ │ │ +
349 boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
│ │ │ │ +
350
│ │ │ │ +
│ │ │ │ +
352 Point3 backproject(const Point2& pn, double depth,
│ │ │ │ +
353 OptionalJacobian<3, 6> Dresult_dpose = boost::none,
│ │ │ │ +
354 OptionalJacobian<3, 2> Dresult_dp = boost::none,
│ │ │ │ +
355 OptionalJacobian<3, 1> Dresult_ddepth = boost::none) const {
│ │ │ │
356
│ │ │ │ -
360
│ │ │ │ -
│ │ │ │ -
366 inline static std::pair<size_t, size_t> translationInterval() {
│ │ │ │ -
367 return std::make_pair(3, 5);
│ │ │ │ -
368 }
│ │ │ │ -
│ │ │ │ -
369
│ │ │ │ -
│ │ │ │ -
375 static std::pair<size_t, size_t> rotationInterval() {
│ │ │ │ -
376 return std::make_pair(0, 2);
│ │ │ │ -
377 }
│ │ │ │ -
│ │ │ │ -
378
│ │ │ │ -
384 Pose3 slerp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none,
│ │ │ │ -
385 OptionalJacobian<6, 6> Hy = boost::none) const;
│ │ │ │ +
357 Matrix32 Dpoint_dpn;
│ │ │ │ +
358 Matrix31 Dpoint_ddepth;
│ │ │ │ +
359 const Point3 point = BackprojectFromCamera(pn, depth,
│ │ │ │ +
360 Dresult_dp ? &Dpoint_dpn : 0,
│ │ │ │ +
361 Dresult_ddepth ? &Dpoint_ddepth : 0);
│ │ │ │ +
362
│ │ │ │ +
363 Matrix33 Dresult_dpoint;
│ │ │ │ +
364 const Point3 result = pose().transformFrom(point, Dresult_dpose,
│ │ │ │ +
365 (Dresult_ddepth ||
│ │ │ │ +
366 Dresult_dp) ? &Dresult_dpoint : 0);
│ │ │ │ +
367
│ │ │ │ +
368 if (Dresult_dp)
│ │ │ │ +
369 *Dresult_dp = Dresult_dpoint * Dpoint_dpn;
│ │ │ │ +
370 if (Dresult_ddepth)
│ │ │ │ +
371 *Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth;
│ │ │ │ +
372
│ │ │ │ +
373 return result;
│ │ │ │ +
374 }
│ │ │ │ +
│ │ │ │ +
375
│ │ │ │ +
│ │ │ │ +
381 double range(const Point3& point,
│ │ │ │ +
382 OptionalJacobian<1, 6> Dcamera = boost::none,
│ │ │ │ +
383 OptionalJacobian<1, 3> Dpoint = boost::none) const {
│ │ │ │ +
384 return pose().range(point, Dcamera, Dpoint);
│ │ │ │ +
385 }
│ │ │ │ +
│ │ │ │
386
│ │ │ │ -
388 GTSAM_EXPORT
│ │ │ │ -
389 friend std::ostream &operator<<(std::ostream &os, const Pose3& p);
│ │ │ │ -
390
│ │ │ │ -
391 private:
│ │ │ │ -
393 friend class boost::serialization::access;
│ │ │ │ -
394 template<class Archive>
│ │ │ │ -
395 void serialize(Archive & ar, const unsigned int /*version*/) {
│ │ │ │ -
396 ar & BOOST_SERIALIZATION_NVP(R_);
│ │ │ │ -
397 ar & BOOST_SERIALIZATION_NVP(t_);
│ │ │ │ -
398 }
│ │ │ │ -
400
│ │ │ │ -
401#ifdef GTSAM_USE_QUATERNIONS
│ │ │ │ -
402 // Align if we are using Quaternions
│ │ │ │ -
403 public:
│ │ │ │ -
404 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
405#endif
│ │ │ │ -
406};
│ │ │ │ -
│ │ │ │ -
407// Pose3 class
│ │ │ │ -
408
│ │ │ │ -
416template<>
│ │ │ │ -
│ │ │ │ -
417inline Matrix wedge<Pose3>(const Vector& xi) {
│ │ │ │ -
418 return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5));
│ │ │ │ -
419}
│ │ │ │ -
│ │ │ │ -
420
│ │ │ │ -
421// Convenience typedef
│ │ │ │ -
422using Pose3Pair = std::pair<Pose3, Pose3>;
│ │ │ │ -
423using Pose3Pairs = std::vector<std::pair<Pose3, Pose3> >;
│ │ │ │ -
424
│ │ │ │ -
425// For MATLAB wrapper
│ │ │ │ -
426typedef std::vector<Pose3> Pose3Vector;
│ │ │ │ -
427
│ │ │ │ +
│ │ │ │ +
392 double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = boost::none,
│ │ │ │ +
393 OptionalJacobian<1, 6> Dpose = boost::none) const {
│ │ │ │ +
394 return this->pose().range(pose, Dcamera, Dpose);
│ │ │ │ +
395 }
│ │ │ │ +
│ │ │ │ +
396
│ │ │ │ +
│ │ │ │ +
402 double range(const CalibratedCamera& camera, //
│ │ │ │ +
403 OptionalJacobian<1, 6> H1 = boost::none, //
│ │ │ │ +
404 OptionalJacobian<1, 6> H2 = boost::none) const {
│ │ │ │ +
405 return pose().range(camera.pose(), H1, H2);
│ │ │ │ +
406 }
│ │ │ │ +
│ │ │ │ +
407
│ │ │ │ +
409
│ │ │ │ +
410private:
│ │ │ │ +
411
│ │ │ │ +
414
│ │ │ │ +
416 friend class boost::serialization::access;
│ │ │ │ +
417 template<class Archive>
│ │ │ │ +
418 void serialize(Archive & ar, const unsigned int /*version*/) {
│ │ │ │ +
419 ar
│ │ │ │ +
420 & boost::serialization::make_nvp("PinholeBase",
│ │ │ │ +
421 boost::serialization::base_object<PinholeBase>(*this));
│ │ │ │ +
422 }
│ │ │ │ +
423
│ │ │ │ +
425};
│ │ │ │ +
│ │ │ │ +
426
│ │ │ │ +
427// manifold traits
│ │ │ │
428template <>
│ │ │ │ -
429struct traits<Pose3> : public internal::LieGroup<Pose3> {};
│ │ │ │ +
429struct traits<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
│ │ │ │
430
│ │ │ │
431template <>
│ │ │ │ -
432struct traits<const Pose3> : public internal::LieGroup<Pose3> {};
│ │ │ │ +
432struct traits<const CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
│ │ │ │
433
│ │ │ │ -
434// bearing and range traits, used in RangeFactor
│ │ │ │ -
435template <>
│ │ │ │ -
436struct Bearing<Pose3, Point3> : HasBearing<Pose3, Point3, Unit3> {};
│ │ │ │ +
434// range traits, used in RangeFactor
│ │ │ │ +
435template <typename T>
│ │ │ │ +
436struct Range<CalibratedCamera, T> : HasRange<CalibratedCamera, T, double> {};
│ │ │ │
437
│ │ │ │ -
438template<>
│ │ │ │ -
439struct Bearing<Pose3, Pose3> : HasBearing<Pose3, Pose3, Unit3> {};
│ │ │ │ -
440
│ │ │ │ -
441template <typename T>
│ │ │ │ -
442struct Range<Pose3, T> : HasRange<Pose3, T, double> {};
│ │ │ │ -
443
│ │ │ │ -
444} // namespace gtsam
│ │ │ │ -
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ -
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ -
Point3.h
3D Point
│ │ │ │ -
BearingRange.h
Bearing-Range product.
│ │ │ │ -
Rot3.h
3D rotation represented as a rotation matrix or quaternion
│ │ │ │ +
438} // namespace gtsam
│ │ │ │ +
ThreadsafeException.h
Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
│ │ │ │ +
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ +
Point2.h
2D Point
│ │ │ │ +
Pose3.h
3D Pose
│ │ │ │ +
BearingRange.h
Bearing-Range product.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ -
gtsam::transformTo
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Transform a line from world to camera frame.
Definition Line3.cpp:94
│ │ │ │ -
gtsam::wedge< Pose3 >
Matrix wedge< Pose3 >(const Vector &xi)
wedge for Pose3:
Definition Pose3.h:417
│ │ │ │ +
gtsam::project
Point2_ project(const Point3_ &p_cam)
Expression version of PinholeBase::Project.
Definition expressions.h:131
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::LieGroup
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition Lie.h:37
│ │ │ │ -
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition Lie.h:229
│ │ │ │ +
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
│ │ │ │
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Bearing
Definition BearingRange.h:34
│ │ │ │ +
gtsam::ThreadsafeException
Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
Definition ThreadsafeException.h:42
│ │ │ │
gtsam::Range
Definition BearingRange.h:40
│ │ │ │ -
gtsam::HasBearing
Definition BearingRange.h:180
│ │ │ │
gtsam::HasRange
Definition BearingRange.h:194
│ │ │ │ +
gtsam::CheiralityException
Definition CalibratedCamera.h:32
│ │ │ │ +
gtsam::PinholeBase
A pinhole camera class that has a Pose3, functions as base class for all pinhole cameras.
Definition CalibratedCamera.h:52
│ │ │ │ +
gtsam::PinholeBase::PinholeBase
PinholeBase()
Default constructor.
Definition CalibratedCamera.h:123
│ │ │ │ +
gtsam::PinholeBase::PinholeBase
PinholeBase(const Pose3 &pose)
Constructor with pose.
Definition CalibratedCamera.h:126
│ │ │ │ +
gtsam::PinholeBase::translation
const Point3 & translation() const
get translation
Definition CalibratedCamera.h:162
│ │ │ │ +
gtsam::PinholeBase::rotation
const Rot3 & rotation() const
get rotation
Definition CalibratedCamera.h:157
│ │ │ │ +
gtsam::PinholeBase::Measurement
Point2 Measurement
Some classes template on either PinholeCamera or StereoCamera, and this typedef informs those classes...
Definition CalibratedCamera.h:64
│ │ │ │ +
gtsam::PinholeBase::pose
const Pose3 & pose() const
return pose, constant version
Definition CalibratedCamera.h:152
│ │ │ │ +
gtsam::PinholeBase::translationInterval
static std::pair< size_t, size_t > translationInterval()
Return the start and end indices (inclusive) of the translation component of the exponential map para...
Definition CalibratedCamera.h:223
│ │ │ │ +
gtsam::PinholeBase::Rotation
Rot3 Rotation
Pose Concept requirements.
Definition CalibratedCamera.h:57
│ │ │ │ +
gtsam::PinholeBase::~PinholeBase
virtual ~PinholeBase()=default
Default destructor.
│ │ │ │ +
gtsam::CalibratedCamera
A Calibrated camera class [R|-R't], calibration K=I.
Definition CalibratedCamera.h:247
│ │ │ │ +
gtsam::CalibratedCamera::backproject
Point3 backproject(const Point2 &pn, double depth, OptionalJacobian< 3, 6 > Dresult_dpose=boost::none, OptionalJacobian< 3, 2 > Dresult_dp=boost::none, OptionalJacobian< 3, 1 > Dresult_ddepth=boost::none) const
backproject a 2-dimensional point to a 3-dimensional point at given depth
Definition CalibratedCamera.h:352
│ │ │ │ +
gtsam::CalibratedCamera::CalibratedCamera
CalibratedCamera()
default constructor
Definition CalibratedCamera.h:259
│ │ │ │ +
gtsam::CalibratedCamera::range
double range(const Pose3 &pose, OptionalJacobian< 1, 6 > Dcamera=boost::none, OptionalJacobian< 1, 6 > Dpose=boost::none) const
Calculate range to another pose.
Definition CalibratedCamera.h:392
│ │ │ │ +
gtsam::CalibratedCamera::dim
size_t dim() const
Definition CalibratedCamera.h:331
│ │ │ │ +
gtsam::CalibratedCamera::CalibratedCamera
CalibratedCamera(const Vector &v)
construct from vector
Definition CalibratedCamera.h:303
│ │ │ │ +
gtsam::CalibratedCamera::~CalibratedCamera
virtual ~CalibratedCamera()
destructor
Definition CalibratedCamera.h:312
│ │ │ │ +
gtsam::CalibratedCamera::Dim
static size_t Dim()
Definition CalibratedCamera.h:336
│ │ │ │ +
gtsam::CalibratedCamera::range
double range(const CalibratedCamera &camera, OptionalJacobian< 1, 6 > H1=boost::none, OptionalJacobian< 1, 6 > H2=boost::none) const
Calculate range to another camera.
Definition CalibratedCamera.h:402
│ │ │ │ +
gtsam::CalibratedCamera::print
void print(const std::string &s="CalibratedCamera") const override
print
Definition CalibratedCamera.h:326
│ │ │ │ +
gtsam::CalibratedCamera::range
double range(const Point3 &point, OptionalJacobian< 1, 6 > Dcamera=boost::none, OptionalJacobian< 1, 3 > Dpoint=boost::none) const
Calculate range to a landmark.
Definition CalibratedCamera.h:381
│ │ │ │ +
gtsam::CalibratedCamera::CalibratedCamera
CalibratedCamera(const Pose3 &pose)
construct with pose
Definition CalibratedCamera.h:263
│ │ │ │
gtsam::Pose2
A 2D pose (Point2,Rot2)
Definition Pose2.h:36
│ │ │ │
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ -
gtsam::Pose3::Identity
static Pose3 Identity()
identity for group operation
Definition Pose3.h:106
│ │ │ │ -
gtsam::Pose3::Pose3
Pose3(const Pose3 &pose)
Copy constructor.
Definition Pose3.h:58
│ │ │ │ -
gtsam::Pose3::Pose3
Pose3()
Default constructor is origin.
Definition Pose3.h:55
│ │ │ │ -
gtsam::Pose3::wedge
static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz)
wedge for Pose3:
Definition Pose3.h:233
│ │ │ │ -
gtsam::Pose3::Pose3
Pose3(const Rot3 &R, const Point3 &t)
Construct from R,t.
Definition Pose3.h:63
│ │ │ │ -
gtsam::Pose3::Pose3
Pose3(const Matrix &T)
Constructor from 4*4 matrix.
Definition Pose3.h:71
│ │ │ │ -
gtsam::Pose3::z
double z() const
get z
Definition Pose3.h:302
│ │ │ │ -
gtsam::Pose3::rotationInterval
static std::pair< size_t, size_t > rotationInterval()
Return the start and end indices (inclusive) of the rotation component of the exponential map paramet...
Definition Pose3.h:375
│ │ │ │ -
gtsam::Pose3::operator*
Pose3 operator*(const Pose3 &T) const
compose syntactic sugar
Definition Pose3.h:114
│ │ │ │ -
gtsam::Pose3::Rotation
Rot3 Rotation
Pose Concept requirements.
Definition Pose3.h:41
│ │ │ │ -
gtsam::Pose3::y
double y() const
get y
Definition Pose3.h:297
│ │ │ │ -
gtsam::Pose3::translationInterval
static std::pair< size_t, size_t > translationInterval()
Return the start and end indices (inclusive) of the translation component of the exponential map para...
Definition Pose3.h:366
│ │ │ │ -
gtsam::Pose3::operator*
Point3 operator*(const Point3 &point) const
syntactic sugar for transformFrom
Definition Pose3.h:260
│ │ │ │ -
gtsam::Pose3::x
double x() const
get x
Definition Pose3.h:292
│ │ │ │ -
gtsam::Pose3::ChartAtOrigin
Definition Pose3.h:207
│ │ │ │ +
gtsam::Pose3::transformFrom
Point3 transformFrom(const Point3 &point, OptionalJacobian< 3, 6 > Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const
takes point in Pose coordinates and transforms it to world coordinates
Definition Pose3.cpp:347
│ │ │ │ +
gtsam::Pose3::range
double range(const Point3 &point, OptionalJacobian< 1, 6 > Hself=boost::none, OptionalJacobian< 1, 3 > Hpoint=boost::none) const
Calculate range to a landmark.
Definition Pose3.cpp:399
│ │ │ │ +
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get translation
Definition Pose3.cpp:308
│ │ │ │ +
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get rotation
Definition Pose3.cpp:315
│ │ │ │
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
│ │ │ │
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,402 +1,460 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Pose3.h │ │ │ │ │ +CalibratedCamera.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -17// \callgraph │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include │ │ │ │ │ -21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_B_e_a_r_i_n_g_R_a_n_g_e_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_3_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_3_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_b_a_s_e_/_L_i_e_._h> │ │ │ │ │ -26 │ │ │ │ │ -27namespace _g_t_s_a_m { │ │ │ │ │ -28 │ │ │ │ │ -29class Pose2; │ │ │ │ │ -30// forward declare │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_B_e_a_r_i_n_g_R_a_n_g_e_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_3_._h> │ │ │ │ │ +24#include │ │ │ │ │ +25#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_b_a_s_e_/_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_._h> │ │ │ │ │ +27#include │ │ │ │ │ +28#include │ │ │ │ │ +29 │ │ │ │ │ +30namespace _g_t_s_a_m { │ │ │ │ │ 31 │ │ │ │ │ -_3_7class GTSAM_EXPORT _P_o_s_e_3: public _L_i_e_G_r_o_u_p { │ │ │ │ │ -38public: │ │ │ │ │ -39 │ │ │ │ │ -_4_1 typedef _R_o_t_3 _R_o_t_a_t_i_o_n; │ │ │ │ │ -42 typedef _P_o_i_n_t_3 Translation; │ │ │ │ │ -43 │ │ │ │ │ -44private: │ │ │ │ │ -45 │ │ │ │ │ -46 _R_o_t_3 R_; │ │ │ │ │ -47 _P_o_i_n_t_3 t_; │ │ │ │ │ -48 │ │ │ │ │ -49public: │ │ │ │ │ -50 │ │ │ │ │ +_3_2class GTSAM_EXPORT _C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n: public │ │ │ │ │ +_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n { │ │ │ │ │ +33public: │ │ │ │ │ +34 _C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n() │ │ │ │ │ +35 : _C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n(std::numeric_limits::max()) {} │ │ │ │ │ +36 │ │ │ │ │ +37 _C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n(_K_e_y j) │ │ │ │ │ +38 : _T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_<_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n_>("CheiralityException"), │ │ │ │ │ +39 j_(j) {} │ │ │ │ │ +40 │ │ │ │ │ +41 _K_e_y nearbyVariable() const {return j_;} │ │ │ │ │ +42 │ │ │ │ │ +43private: │ │ │ │ │ +44 _K_e_y j_; │ │ │ │ │ +45}; │ │ │ │ │ +46 │ │ │ │ │ +_5_2class GTSAM_EXPORT _P_i_n_h_o_l_e_B_a_s_e { │ │ │ │ │ 53 │ │ │ │ │ -_5_5 _P_o_s_e_3() : R_(_t_r_a_i_t_s<_R_o_t_3>::Identity()), t_(_t_r_a_i_t_s<_P_o_i_n_t_3>::Identity()) {} │ │ │ │ │ -56 │ │ │ │ │ -_5_8 _P_o_s_e_3(const _P_o_s_e_3& pose) : │ │ │ │ │ -59 R_(pose.R_), t_(pose.t_) { │ │ │ │ │ -60 } │ │ │ │ │ -61 │ │ │ │ │ -_6_3 _P_o_s_e_3(const _R_o_t_3& R, const _P_o_i_n_t_3& t) : │ │ │ │ │ -64 R_(R), t_(t) { │ │ │ │ │ -65 } │ │ │ │ │ +54public: │ │ │ │ │ +55 │ │ │ │ │ +_5_7 typedef _R_o_t_3 _R_o_t_a_t_i_o_n; │ │ │ │ │ +58 typedef _P_o_i_n_t_3 Translation; │ │ │ │ │ +59 │ │ │ │ │ +_6_4 typedef _P_o_i_n_t_2 _M_e_a_s_u_r_e_m_e_n_t; │ │ │ │ │ +65 typedef Point2Vector MeasurementVector; │ │ │ │ │ 66 │ │ │ │ │ -68 explicit _P_o_s_e_3(const _P_o_s_e_2& pose2); │ │ │ │ │ -69 │ │ │ │ │ -_7_1 _P_o_s_e_3(const Matrix &T) : │ │ │ │ │ -72 R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), T(2, 1), │ │ │ │ │ -73 T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) { │ │ │ │ │ -74 } │ │ │ │ │ +67private: │ │ │ │ │ +68 │ │ │ │ │ +69 _P_o_s_e_3 pose_; │ │ │ │ │ +70 │ │ │ │ │ +71protected: │ │ │ │ │ +72 │ │ │ │ │ 75 │ │ │ │ │ -77 static _P_o_s_e_3 Create(const _R_o_t_3& R, const _P_o_i_n_t_3& t, │ │ │ │ │ -78 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _3_> HR = boost::none, │ │ │ │ │ -79 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _3_> Ht = boost::none); │ │ │ │ │ -80 │ │ │ │ │ -86 static boost::optional Align(const Point3Pairs& abPointPairs); │ │ │ │ │ -87 │ │ │ │ │ -88 // Version of Pose3::Align that takes 2 matrices. │ │ │ │ │ -89 static boost::optional Align(const Matrix& a, const Matrix& b); │ │ │ │ │ +81 static Matrix26 Dpose(const _P_o_i_n_t_2& pn, double d); │ │ │ │ │ +82 │ │ │ │ │ +89 static Matrix23 Dpoint(const _P_o_i_n_t_2& pn, double d, const Matrix3& Rt); │ │ │ │ │ 90 │ │ │ │ │ +92 │ │ │ │ │ +93public: │ │ │ │ │ 94 │ │ │ │ │ -96 void _p_r_i_n_t(const std::string& s = "") const; │ │ │ │ │ 97 │ │ │ │ │ -99 bool _e_q_u_a_l_s(const _P_o_s_e_3& pose, double tol = 1e-9) const; │ │ │ │ │ -100 │ │ │ │ │ -104 │ │ │ │ │ -_1_0_6 static _P_o_s_e_3 _I_d_e_n_t_i_t_y() { │ │ │ │ │ -107 return _P_o_s_e_3(); │ │ │ │ │ -108 } │ │ │ │ │ -109 │ │ │ │ │ -111 _P_o_s_e_3 inverse() const; │ │ │ │ │ -112 │ │ │ │ │ -_1_1_4 _P_o_s_e_3 _o_p_e_r_a_t_o_r_*(const _P_o_s_e_3& T) const { │ │ │ │ │ -115 return _P_o_s_e_3(R_ * T.R_, t_ + R_ * T.t_); │ │ │ │ │ -116 } │ │ │ │ │ +105 static _P_o_s_e_3 LevelPose(const _P_o_s_e_2& pose2, double height); │ │ │ │ │ +106 │ │ │ │ │ +115 static _P_o_s_e_3 LookatPose(const _P_o_i_n_t_3& eye, const _P_o_i_n_t_3& target, │ │ │ │ │ +116 const _P_o_i_n_t_3& upVector); │ │ │ │ │ 117 │ │ │ │ │ -132 _P_o_s_e_3 interpolateRt(const _P_o_s_e_3& T, double t) const; │ │ │ │ │ +121 │ │ │ │ │ +_1_2_3 _P_i_n_h_o_l_e_B_a_s_e() {} │ │ │ │ │ +124 │ │ │ │ │ +_1_2_6 explicit _P_i_n_h_o_l_e_B_a_s_e(const _P_o_s_e_3& pose) : pose_(pose) {} │ │ │ │ │ +127 │ │ │ │ │ +131 │ │ │ │ │ +132 explicit _P_i_n_h_o_l_e_B_a_s_e(const Vector& v) : pose_(_P_o_s_e_3::Expmap(v)) {} │ │ │ │ │ 133 │ │ │ │ │ -137 │ │ │ │ │ -139 static _P_o_s_e_3 Expmap(const Vector6& xi, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> Hxi = boost:: │ │ │ │ │ -none); │ │ │ │ │ +_1_3_5 virtual _~_P_i_n_h_o_l_e_B_a_s_e() = default; │ │ │ │ │ +136 │ │ │ │ │ 140 │ │ │ │ │ -142 static Vector6 Logmap(const _P_o_s_e_3& pose, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> Hpose = │ │ │ │ │ -boost::none); │ │ │ │ │ +142 bool _e_q_u_a_l_s(const _P_i_n_h_o_l_e_B_a_s_e &camera, double tol = 1e-9) const; │ │ │ │ │ 143 │ │ │ │ │ -148 Matrix6 AdjointMap() const; │ │ │ │ │ -149 │ │ │ │ │ -156 Vector6 Adjoint(const Vector6& xi_b, │ │ │ │ │ -157 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> H_this = boost::none, │ │ │ │ │ -158 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> H_xib = boost::none) const; │ │ │ │ │ -159 │ │ │ │ │ -161 Vector6 AdjointTranspose(const Vector6& x, │ │ │ │ │ -162 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> H_this = boost::none, │ │ │ │ │ -163 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> H_x = boost::none) const; │ │ │ │ │ -164 │ │ │ │ │ -180 static Matrix6 adjointMap(const Vector6& xi); │ │ │ │ │ -181 │ │ │ │ │ -185 static Vector6 adjoint(const Vector6& xi, const Vector6& y, │ │ │ │ │ -186 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> Hxi = boost::none, │ │ │ │ │ -187 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> H_y = boost::none); │ │ │ │ │ +145 virtual void _p_r_i_n_t(const std::string& s = "PinholeBase") const; │ │ │ │ │ +146 │ │ │ │ │ +150 │ │ │ │ │ +_1_5_2 const _P_o_s_e_3& _p_o_s_e() const { │ │ │ │ │ +153 return pose_; │ │ │ │ │ +154 } │ │ │ │ │ +155 │ │ │ │ │ +_1_5_7 const _R_o_t_3& _r_o_t_a_t_i_o_n() const { │ │ │ │ │ +158 return pose_._r_o_t_a_t_i_o_n(); │ │ │ │ │ +159 } │ │ │ │ │ +160 │ │ │ │ │ +_1_6_2 const _P_o_i_n_t_3& _t_r_a_n_s_l_a_t_i_o_n() const { │ │ │ │ │ +163 return pose_._t_r_a_n_s_l_a_t_i_o_n(); │ │ │ │ │ +164 } │ │ │ │ │ +165 │ │ │ │ │ +167 const _P_o_s_e_3& getPose(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> H) const; │ │ │ │ │ +168 │ │ │ │ │ +172 │ │ │ │ │ +178 static _P_o_i_n_t_2 Project(const _P_o_i_n_t_3& pc, // │ │ │ │ │ +179 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> Dpoint = boost::none); │ │ │ │ │ +180 │ │ │ │ │ +186 static _P_o_i_n_t_2 Project(const _U_n_i_t_3& pc, // │ │ │ │ │ +187 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dpoint = boost::none); │ │ │ │ │ 188 │ │ │ │ │ -189 // temporary fix for wrappers until case issue is resolved │ │ │ │ │ -190 static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);} │ │ │ │ │ -191 static Vector6 adjoint_(const Vector6 &xi, const Vector6 &y) { return │ │ │ │ │ -adjoint(xi, y);} │ │ │ │ │ -192 │ │ │ │ │ -196 static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, │ │ │ │ │ -197 OptionalJacobian<6, 6> Hxi = boost::none, │ │ │ │ │ -198 OptionalJacobian<6, 6> H_y = boost::none); │ │ │ │ │ +190 std::pair projectSafe(const _P_o_i_n_t_3& pw) const; │ │ │ │ │ +191 │ │ │ │ │ +197 _P_o_i_n_t_2 project2(const _P_o_i_n_t_3& point, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> Dpose = │ │ │ │ │ +198 boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> Dpoint = boost::none) const; │ │ │ │ │ 199 │ │ │ │ │ -201 static Matrix6 ExpmapDerivative(const Vector6& xi); │ │ │ │ │ -202 │ │ │ │ │ -204 static Matrix6 LogmapDerivative(const Pose3& xi); │ │ │ │ │ -205 │ │ │ │ │ -206 // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP │ │ │ │ │ -_2_0_7 struct _C_h_a_r_t_A_t_O_r_i_g_i_n { │ │ │ │ │ -208 static _P_o_s_e_3 Retract(const Vector6& xi, ChartJacobian Hxi = boost::none); │ │ │ │ │ -209 static Vector6 Local(const _P_o_s_e_3& pose, ChartJacobian Hpose = boost::none); │ │ │ │ │ -210 }; │ │ │ │ │ -211 │ │ │ │ │ -221 static Matrix3 ComputeQforExpmapDerivative( │ │ │ │ │ -222 const Vector6& xi, double nearZeroThreshold = 1e-5); │ │ │ │ │ -223 │ │ │ │ │ -224 using _L_i_e_G_r_o_u_p<_P_o_s_e_3, 6>::inverse; // version with derivative │ │ │ │ │ -225 │ │ │ │ │ -_2_3_3 static Matrix _w_e_d_g_e(double wx, double wy, double wz, double vx, double vy, │ │ │ │ │ -234 double vz) { │ │ │ │ │ -235 return (Matrix(4, 4) << 0., -wz, wy, vx, wz, 0., -wx, vy, -wy, wx, 0., vz, │ │ │ │ │ -0., 0., 0., 0.).finished(); │ │ │ │ │ +205 _P_o_i_n_t_2 project2(const _U_n_i_t_3& point, │ │ │ │ │ +206 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> Dpose = boost::none, │ │ │ │ │ +207 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dpoint = boost::none) const; │ │ │ │ │ +208 │ │ │ │ │ +210 static _P_o_i_n_t_3 BackprojectFromCamera(const _P_o_i_n_t_2& p, const double depth, │ │ │ │ │ +211 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _2_> Dpoint = boost::none, │ │ │ │ │ +212 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _1_> Ddepth = boost::none); │ │ │ │ │ +213 │ │ │ │ │ +217 │ │ │ │ │ +_2_2_3 inline static std::pair _t_r_a_n_s_l_a_t_i_o_n_I_n_t_e_r_v_a_l() { │ │ │ │ │ +224 return std::make_pair(3, 5); │ │ │ │ │ +225 } │ │ │ │ │ +226 │ │ │ │ │ +228 │ │ │ │ │ +229private: │ │ │ │ │ +230 │ │ │ │ │ +_2_3_2 friend class boost::serialization::access; │ │ │ │ │ +233 template │ │ │ │ │ +234 void serialize(Archive & ar, const unsigned int /*version*/) { │ │ │ │ │ +235 ar & BOOST_SERIALIZATION_NVP(pose_); │ │ │ │ │ 236 } │ │ │ │ │ -237 │ │ │ │ │ -241 │ │ │ │ │ -249 _P_o_i_n_t_3 transformFrom(const _P_o_i_n_t_3& point, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _6_> Hself = │ │ │ │ │ -250 boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> Hpoint = boost::none) const; │ │ │ │ │ -251 │ │ │ │ │ -257 Matrix transformFrom(const Matrix& points) const; │ │ │ │ │ -258 │ │ │ │ │ -_2_6_0 inline _P_o_i_n_t_3 _o_p_e_r_a_t_o_r_*(const _P_o_i_n_t_3& point) const { │ │ │ │ │ -261 return transformFrom(point); │ │ │ │ │ -262 } │ │ │ │ │ -263 │ │ │ │ │ -271 _P_o_i_n_t_3 _t_r_a_n_s_f_o_r_m_T_o(const _P_o_i_n_t_3& point, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _6_> Hself = │ │ │ │ │ -272 boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> Hpoint = boost::none) const; │ │ │ │ │ -273 │ │ │ │ │ -279 Matrix _t_r_a_n_s_f_o_r_m_T_o(const Matrix& points) const; │ │ │ │ │ -280 │ │ │ │ │ -284 │ │ │ │ │ -286 const _R_o_t_3& rotation(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _6_> Hself = boost::none) const; │ │ │ │ │ -287 │ │ │ │ │ -289 const _P_o_i_n_t_3& translation(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _6_> Hself = boost::none) │ │ │ │ │ -const; │ │ │ │ │ -290 │ │ │ │ │ -_2_9_2 double _x() const { │ │ │ │ │ -293 return t_.x(); │ │ │ │ │ -294 } │ │ │ │ │ -295 │ │ │ │ │ -_2_9_7 double _y() const { │ │ │ │ │ -298 return t_.y(); │ │ │ │ │ -299 } │ │ │ │ │ -300 │ │ │ │ │ -_3_0_2 double _z() const { │ │ │ │ │ -303 return t_.z(); │ │ │ │ │ -304 } │ │ │ │ │ -305 │ │ │ │ │ -307 Matrix4 matrix() const; │ │ │ │ │ -308 │ │ │ │ │ -314 _P_o_s_e_3 transformPoseFrom(const _P_o_s_e_3& aTb, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> Hself = │ │ │ │ │ -boost::none, │ │ │ │ │ -315 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> HaTb = boost::none) const; │ │ │ │ │ -316 │ │ │ │ │ -321 _P_o_s_e_3 transformPoseTo(const _P_o_s_e_3& wTb, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> Hself = │ │ │ │ │ -boost::none, │ │ │ │ │ -322 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> HwTb = boost::none) const; │ │ │ │ │ -323 │ │ │ │ │ -329 double range(const _P_o_i_n_t_3& point, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> Hself = boost:: │ │ │ │ │ -none, │ │ │ │ │ -330 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> Hpoint = boost::none) const; │ │ │ │ │ -331 │ │ │ │ │ -337 double range(const _P_o_s_e_3& pose, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> Hself = boost::none, │ │ │ │ │ -338 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> Hpose = boost::none) const; │ │ │ │ │ +237}; │ │ │ │ │ +238// end of class PinholeBase │ │ │ │ │ +239 │ │ │ │ │ +_2_4_7class GTSAM_EXPORT _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a: public _P_i_n_h_o_l_e_B_a_s_e { │ │ │ │ │ +248 │ │ │ │ │ +249public: │ │ │ │ │ +250 │ │ │ │ │ +251 enum { │ │ │ │ │ +252 dimension = 6 │ │ │ │ │ +253 }; │ │ │ │ │ +254 │ │ │ │ │ +257 │ │ │ │ │ +_2_5_9 _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a() { │ │ │ │ │ +260 } │ │ │ │ │ +261 │ │ │ │ │ +_2_6_3 explicit _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a(const _P_o_s_e_3& pose) : │ │ │ │ │ +264 _P_i_n_h_o_l_e_B_a_s_e(pose) { │ │ │ │ │ +265 } │ │ │ │ │ +266 │ │ │ │ │ +270 │ │ │ │ │ +271 // Create CalibratedCamera, with derivatives │ │ │ │ │ +272 static _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a Create(const _P_o_s_e_3& pose, │ │ │ │ │ +273 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_d_i_m_e_n_s_i_o_n_,_ _6_> H1 = boost::none) { │ │ │ │ │ +274 if (H1) │ │ │ │ │ +275 *H1 << I_6x6; │ │ │ │ │ +276 return _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a(pose); │ │ │ │ │ +277 } │ │ │ │ │ +278 │ │ │ │ │ +285 static CalibratedCamera Level(const Pose2& pose2, double height); │ │ │ │ │ +286 │ │ │ │ │ +295 static CalibratedCamera Lookat(const Point3& eye, const Point3& target, │ │ │ │ │ +296 const Point3& upVector); │ │ │ │ │ +297 │ │ │ │ │ +301 │ │ │ │ │ +_3_0_3 explicit _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a(const Vector &v) : │ │ │ │ │ +304 _P_i_n_h_o_l_e_B_a_s_e(v) { │ │ │ │ │ +305 } │ │ │ │ │ +306 │ │ │ │ │ +310 │ │ │ │ │ +_3_1_2 virtual _~_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a() { │ │ │ │ │ +313 } │ │ │ │ │ +314 │ │ │ │ │ +318 │ │ │ │ │ +320 _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a retract(const Vector& d) const; │ │ │ │ │ +321 │ │ │ │ │ +323 Vector localCoordinates(const _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a& T2) const; │ │ │ │ │ +324 │ │ │ │ │ +_3_2_6 void _p_r_i_n_t(const std::string& s = "CalibratedCamera") const override { │ │ │ │ │ +327 PinholeBase::print(s); │ │ │ │ │ +328 } │ │ │ │ │ +329 │ │ │ │ │ +_3_3_1 inline size_t _d_i_m() const { │ │ │ │ │ +332 return dimension; │ │ │ │ │ +333 } │ │ │ │ │ +334 │ │ │ │ │ +_3_3_6 inline static size_t _D_i_m() { │ │ │ │ │ +337 return dimension; │ │ │ │ │ +338 } │ │ │ │ │ 339 │ │ │ │ │ -345 _U_n_i_t_3 bearing(const _P_o_i_n_t_3& point, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> Hself = boost:: │ │ │ │ │ -none, │ │ │ │ │ -346 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> Hpoint = boost::none) const; │ │ │ │ │ -347 │ │ │ │ │ -354 _U_n_i_t_3 bearing(const _P_o_s_e_3& pose, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> Hself = boost:: │ │ │ │ │ -none, │ │ │ │ │ -355 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> Hpose = boost::none) const; │ │ │ │ │ +343 │ │ │ │ │ +348 _P_o_i_n_t_2 _p_r_o_j_e_c_t(const _P_o_i_n_t_3& point, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> Dcamera = │ │ │ │ │ +349 boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> Dpoint = boost::none) const; │ │ │ │ │ +350 │ │ │ │ │ +_3_5_2 _P_o_i_n_t_3 _b_a_c_k_p_r_o_j_e_c_t(const _P_o_i_n_t_2& pn, double depth, │ │ │ │ │ +353 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _6_> Dresult_dpose = boost::none, │ │ │ │ │ +354 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _2_> Dresult_dp = boost::none, │ │ │ │ │ +355 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _1_> Dresult_ddepth = boost::none) const { │ │ │ │ │ 356 │ │ │ │ │ -360 │ │ │ │ │ -_3_6_6 inline static std::pair _t_r_a_n_s_l_a_t_i_o_n_I_n_t_e_r_v_a_l() { │ │ │ │ │ -367 return std::make_pair(3, 5); │ │ │ │ │ -368 } │ │ │ │ │ -369 │ │ │ │ │ -_3_7_5 static std::pair _r_o_t_a_t_i_o_n_I_n_t_e_r_v_a_l() { │ │ │ │ │ -376 return std::make_pair(0, 2); │ │ │ │ │ -377 } │ │ │ │ │ -378 │ │ │ │ │ -384 _P_o_s_e_3 slerp(double t, const _P_o_s_e_3& other, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> Hx = │ │ │ │ │ -boost::none, │ │ │ │ │ -385 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_6_,_ _6_> Hy = boost::none) const; │ │ │ │ │ +357 Matrix32 Dpoint_dpn; │ │ │ │ │ +358 Matrix31 Dpoint_ddepth; │ │ │ │ │ +359 const _P_o_i_n_t_3 point = BackprojectFromCamera(pn, depth, │ │ │ │ │ +360 Dresult_dp ? &Dpoint_dpn : 0, │ │ │ │ │ +361 Dresult_ddepth ? &Dpoint_ddepth : 0); │ │ │ │ │ +362 │ │ │ │ │ +363 Matrix33 Dresult_dpoint; │ │ │ │ │ +364 const _P_o_i_n_t_3 result = pose()._t_r_a_n_s_f_o_r_m_F_r_o_m(point, Dresult_dpose, │ │ │ │ │ +365 (Dresult_ddepth || │ │ │ │ │ +366 Dresult_dp) ? &Dresult_dpoint : 0); │ │ │ │ │ +367 │ │ │ │ │ +368 if (Dresult_dp) │ │ │ │ │ +369 *Dresult_dp = Dresult_dpoint * Dpoint_dpn; │ │ │ │ │ +370 if (Dresult_ddepth) │ │ │ │ │ +371 *Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth; │ │ │ │ │ +372 │ │ │ │ │ +373 return result; │ │ │ │ │ +374 } │ │ │ │ │ +375 │ │ │ │ │ +_3_8_1 double _r_a_n_g_e(const _P_o_i_n_t_3& point, │ │ │ │ │ +382 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> Dcamera = boost::none, │ │ │ │ │ +383 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> Dpoint = boost::none) const { │ │ │ │ │ +384 return pose()._r_a_n_g_e(point, Dcamera, Dpoint); │ │ │ │ │ +385 } │ │ │ │ │ 386 │ │ │ │ │ -388 GTSAM_EXPORT │ │ │ │ │ -389 friend std::ostream &operator<<(std::ostream &os, const _P_o_s_e_3& p); │ │ │ │ │ -390 │ │ │ │ │ -391 private: │ │ │ │ │ -_3_9_3 friend class boost::serialization::access; │ │ │ │ │ -394 template │ │ │ │ │ -395 void serialize(Archive & ar, const unsigned int /*version*/) { │ │ │ │ │ -396 ar & BOOST_SERIALIZATION_NVP(R_); │ │ │ │ │ -397 ar & BOOST_SERIALIZATION_NVP(t_); │ │ │ │ │ -398 } │ │ │ │ │ -400 │ │ │ │ │ -401#ifdef GTSAM_USE_QUATERNIONS │ │ │ │ │ -402 // Align if we are using Quaternions │ │ │ │ │ -403 public: │ │ │ │ │ -404 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -405#endif │ │ │ │ │ -406}; │ │ │ │ │ -407// Pose3 class │ │ │ │ │ -408 │ │ │ │ │ -416template<> │ │ │ │ │ -_4_1_7inline Matrix _w_e_d_g_e_<_P_o_s_e_3_>(const Vector& xi) { │ │ │ │ │ -418 return _P_o_s_e_3_:_:_w_e_d_g_e(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5)); │ │ │ │ │ -419} │ │ │ │ │ -420 │ │ │ │ │ -421// Convenience typedef │ │ │ │ │ -422using Pose3Pair = std::pair; │ │ │ │ │ -423using Pose3Pairs = std::vector >; │ │ │ │ │ -424 │ │ │ │ │ -425// For MATLAB wrapper │ │ │ │ │ -426typedef std::vector Pose3Vector; │ │ │ │ │ -427 │ │ │ │ │ +_3_9_2 double _r_a_n_g_e(const _P_o_s_e_3& pose, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> Dcamera = boost:: │ │ │ │ │ +none, │ │ │ │ │ +393 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> Dpose = boost::none) const { │ │ │ │ │ +394 return this->pose()._r_a_n_g_e(pose, Dcamera, Dpose); │ │ │ │ │ +395 } │ │ │ │ │ +396 │ │ │ │ │ +_4_0_2 double _r_a_n_g_e(const _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a& camera, // │ │ │ │ │ +403 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> H1 = boost::none, // │ │ │ │ │ +404 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> H2 = boost::none) const { │ │ │ │ │ +405 return pose()._r_a_n_g_e(camera._p_o_s_e(), H1, H2); │ │ │ │ │ +406 } │ │ │ │ │ +407 │ │ │ │ │ +409 │ │ │ │ │ +410private: │ │ │ │ │ +411 │ │ │ │ │ +414 │ │ │ │ │ +_4_1_6 friend class boost::serialization::access; │ │ │ │ │ +417 template │ │ │ │ │ +418 void serialize(Archive & ar, const unsigned int /*version*/) { │ │ │ │ │ +419 ar │ │ │ │ │ +420 & boost::serialization::make_nvp("PinholeBase", │ │ │ │ │ +421 boost::serialization::base_object(*this)); │ │ │ │ │ +422 } │ │ │ │ │ +423 │ │ │ │ │ +425}; │ │ │ │ │ +426 │ │ │ │ │ +427// manifold traits │ │ │ │ │ 428template <> │ │ │ │ │ -_4_2_9struct _t_r_a_i_t_s<_P_o_s_e_3> : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ +_4_2_9struct _t_r_a_i_t_s<_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a> : public _i_n_t_e_r_n_a_l_:_: │ │ │ │ │ +_M_a_n_i_f_o_l_d {}; │ │ │ │ │ 430 │ │ │ │ │ 431template <> │ │ │ │ │ -_4_3_2struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ +_4_3_2struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_: │ │ │ │ │ +_M_a_n_i_f_o_l_d {}; │ │ │ │ │ 433 │ │ │ │ │ -434// bearing and range traits, used in RangeFactor │ │ │ │ │ -435template <> │ │ │ │ │ -_4_3_6struct _B_e_a_r_i_n_g<_P_o_s_e_3, _P_o_i_n_t_3> : _H_a_s_B_e_a_r_i_n_g {}; │ │ │ │ │ +434// range traits, used in RangeFactor │ │ │ │ │ +435template │ │ │ │ │ +_4_3_6struct _R_a_n_g_e<_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a, T> : _H_a_s_R_a_n_g_e │ │ │ │ │ +{}; │ │ │ │ │ 437 │ │ │ │ │ -438template<> │ │ │ │ │ -_4_3_9struct _B_e_a_r_i_n_g<_P_o_s_e_3, _P_o_s_e_3> : _H_a_s_B_e_a_r_i_n_g {}; │ │ │ │ │ -440 │ │ │ │ │ -441template │ │ │ │ │ -_4_4_2struct _R_a_n_g_e<_P_o_s_e_3, T> : _H_a_s_R_a_n_g_e {}; │ │ │ │ │ -443 │ │ │ │ │ -444} // namespace gtsam │ │ │ │ │ -_L_i_e_._h │ │ │ │ │ -Base class and basic functions for Lie types. │ │ │ │ │ -_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ -This marks a GTSAM object to require alignment. │ │ │ │ │ -DDeeffiinniittiioonn types.h:308 │ │ │ │ │ -_P_o_i_n_t_3_._h │ │ │ │ │ -3D Point │ │ │ │ │ +438} // namespace gtsam │ │ │ │ │ +_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_._h │ │ │ │ │ +Base exception type that uses tbb_allocator if GTSAM is compiled with TBB. │ │ │ │ │ +_M_a_n_i_f_o_l_d_._h │ │ │ │ │ +Base class and basic functions for Manifold types. │ │ │ │ │ +_P_o_i_n_t_2_._h │ │ │ │ │ +2D Point │ │ │ │ │ +_P_o_s_e_3_._h │ │ │ │ │ +3D Pose │ │ │ │ │ _B_e_a_r_i_n_g_R_a_n_g_e_._h │ │ │ │ │ Bearing-Range product. │ │ │ │ │ -_R_o_t_3_._h │ │ │ │ │ -3D rotation represented as a rotation matrix or quaternion │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ print without optional string, must specify cout yourself │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ +Vector2 Point2 │ │ │ │ │ +As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ +typedef Point2 to Vector2... │ │ │ │ │ +DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ _g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ Vector3 Point3 │ │ │ │ │ As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ typedef Point3 to Vector3... │ │ │ │ │ DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_n_s_f_o_r_m_T_o │ │ │ │ │ -Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > │ │ │ │ │ -Dpose, OptionalJacobian< 4, 4 > Dline) │ │ │ │ │ -Transform a line from world to camera frame. │ │ │ │ │ -DDeeffiinniittiioonn Line3.cpp:94 │ │ │ │ │ -_g_t_s_a_m_:_:_w_e_d_g_e_<_ _P_o_s_e_3_ _> │ │ │ │ │ -Matrix wedge< Pose3 >(const Vector &xi) │ │ │ │ │ -wedge for Pose3: │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:417 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_o_j_e_c_t │ │ │ │ │ +Point2_ project(const Point3_ &p_cam) │ │ │ │ │ +Expression version of PinholeBase::Project. │ │ │ │ │ +DDeeffiinniittiioonn expressions.h:131 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ -A CRTP helper class that implements Lie group methods Prerequisites: methods │ │ │ │ │ -operator*,... │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ -Both LieGroupTraits and Testable. │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:229 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ +Both ManifoldTraits and Testable. │ │ │ │ │ +DDeeffiinniittiioonn Manifold.h:120 │ │ │ │ │ _g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ either a fixed size o... │ │ │ │ │ DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ _g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ Template to create a binary predicate. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_B_e_a_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn BearingRange.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n │ │ │ │ │ +Base exception type that uses tbb_allocator if GTSAM is compiled with TBB. │ │ │ │ │ +DDeeffiinniittiioonn ThreadsafeException.h:42 │ │ │ │ │ _g_t_s_a_m_:_:_R_a_n_g_e │ │ │ │ │ DDeeffiinniittiioonn BearingRange.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_H_a_s_B_e_a_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn BearingRange.h:180 │ │ │ │ │ _g_t_s_a_m_:_:_H_a_s_R_a_n_g_e │ │ │ │ │ DDeeffiinniittiioonn BearingRange.h:194 │ │ │ │ │ +_g_t_s_a_m_:_:_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e │ │ │ │ │ +A pinhole camera class that has a Pose3, functions as base class for all │ │ │ │ │ +pinhole cameras. │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_P_i_n_h_o_l_e_B_a_s_e │ │ │ │ │ +PinholeBase() │ │ │ │ │ +Default constructor. │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:123 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_P_i_n_h_o_l_e_B_a_s_e │ │ │ │ │ +PinholeBase(const Pose3 &pose) │ │ │ │ │ +Constructor with pose. │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:126 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_t_r_a_n_s_l_a_t_i_o_n │ │ │ │ │ +const Point3 & translation() const │ │ │ │ │ +get translation │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:162 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_r_o_t_a_t_i_o_n │ │ │ │ │ +const Rot3 & rotation() const │ │ │ │ │ +get rotation │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:157 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_M_e_a_s_u_r_e_m_e_n_t │ │ │ │ │ +Point2 Measurement │ │ │ │ │ +Some classes template on either PinholeCamera or StereoCamera, and this typedef │ │ │ │ │ +informs those classes... │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_p_o_s_e │ │ │ │ │ +const Pose3 & pose() const │ │ │ │ │ +return pose, constant version │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:152 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_t_r_a_n_s_l_a_t_i_o_n_I_n_t_e_r_v_a_l │ │ │ │ │ +static std::pair< size_t, size_t > translationInterval() │ │ │ │ │ +Return the start and end indices (inclusive) of the translation component of │ │ │ │ │ +the exponential map para... │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:223 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_R_o_t_a_t_i_o_n │ │ │ │ │ +Rot3 Rotation │ │ │ │ │ +Pose Concept requirements. │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:57 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_~_P_i_n_h_o_l_e_B_a_s_e │ │ │ │ │ +virtual ~PinholeBase()=default │ │ │ │ │ +Default destructor. │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a │ │ │ │ │ +A Calibrated camera class [R|-R't], calibration K=I. │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:247 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_:_:_b_a_c_k_p_r_o_j_e_c_t │ │ │ │ │ +Point3 backproject(const Point2 &pn, double depth, OptionalJacobian< 3, 6 > │ │ │ │ │ +Dresult_dpose=boost::none, OptionalJacobian< 3, 2 > Dresult_dp=boost::none, │ │ │ │ │ +OptionalJacobian< 3, 1 > Dresult_ddepth=boost::none) const │ │ │ │ │ +backproject a 2-dimensional point to a 3-dimensional point at given depth │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:352 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a │ │ │ │ │ +CalibratedCamera() │ │ │ │ │ +default constructor │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:259 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_:_:_r_a_n_g_e │ │ │ │ │ +double range(const Pose3 &pose, OptionalJacobian< 1, 6 > Dcamera=boost::none, │ │ │ │ │ +OptionalJacobian< 1, 6 > Dpose=boost::none) const │ │ │ │ │ +Calculate range to another pose. │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:392 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_:_:_d_i_m │ │ │ │ │ +size_t dim() const │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:331 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a │ │ │ │ │ +CalibratedCamera(const Vector &v) │ │ │ │ │ +construct from vector │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:303 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_:_:_~_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a │ │ │ │ │ +virtual ~CalibratedCamera() │ │ │ │ │ +destructor │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:312 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_:_:_D_i_m │ │ │ │ │ +static size_t Dim() │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:336 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_:_:_r_a_n_g_e │ │ │ │ │ +double range(const CalibratedCamera &camera, OptionalJacobian< 1, 6 > │ │ │ │ │ +H1=boost::none, OptionalJacobian< 1, 6 > H2=boost::none) const │ │ │ │ │ +Calculate range to another camera. │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:402 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="CalibratedCamera") const override │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:326 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_:_:_r_a_n_g_e │ │ │ │ │ +double range(const Point3 &point, OptionalJacobian< 1, 6 > Dcamera=boost::none, │ │ │ │ │ +OptionalJacobian< 1, 3 > Dpoint=boost::none) const │ │ │ │ │ +Calculate range to a landmark. │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:381 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a │ │ │ │ │ +CalibratedCamera(const Pose3 &pose) │ │ │ │ │ +construct with pose │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:263 │ │ │ │ │ _g_t_s_a_m_:_:_P_o_s_e_2 │ │ │ │ │ A 2D pose (Point2,Rot2) │ │ │ │ │ DDeeffiinniittiioonn Pose2.h:36 │ │ │ │ │ _g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_I_d_e_n_t_i_t_y │ │ │ │ │ -static Pose3 Identity() │ │ │ │ │ -identity for group operation │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:106 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_P_o_s_e_3 │ │ │ │ │ -Pose3(const Pose3 &pose) │ │ │ │ │ -Copy constructor. │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_P_o_s_e_3 │ │ │ │ │ -Pose3() │ │ │ │ │ -Default constructor is origin. │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_w_e_d_g_e │ │ │ │ │ -static Matrix wedge(double wx, double wy, double wz, double vx, double vy, │ │ │ │ │ -double vz) │ │ │ │ │ -wedge for Pose3: │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:233 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_P_o_s_e_3 │ │ │ │ │ -Pose3(const Rot3 &R, const Point3 &t) │ │ │ │ │ -Construct from R,t. │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:63 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_P_o_s_e_3 │ │ │ │ │ -Pose3(const Matrix &T) │ │ │ │ │ -Constructor from 4*4 matrix. │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:71 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_z │ │ │ │ │ -double z() const │ │ │ │ │ -get z │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:302 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_r_o_t_a_t_i_o_n_I_n_t_e_r_v_a_l │ │ │ │ │ -static std::pair< size_t, size_t > rotationInterval() │ │ │ │ │ -Return the start and end indices (inclusive) of the rotation component of the │ │ │ │ │ -exponential map paramet... │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:375 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ -Pose3 operator*(const Pose3 &T) const │ │ │ │ │ -compose syntactic sugar │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:114 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_R_o_t_a_t_i_o_n │ │ │ │ │ -Rot3 Rotation │ │ │ │ │ -Pose Concept requirements. │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_y │ │ │ │ │ -double y() const │ │ │ │ │ -get y │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:297 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_t_r_a_n_s_l_a_t_i_o_n_I_n_t_e_r_v_a_l │ │ │ │ │ -static std::pair< size_t, size_t > translationInterval() │ │ │ │ │ -Return the start and end indices (inclusive) of the translation component of │ │ │ │ │ -the exponential map para... │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:366 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ -Point3 operator*(const Point3 &point) const │ │ │ │ │ -syntactic sugar for transformFrom │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:260 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_x │ │ │ │ │ -double x() const │ │ │ │ │ -get x │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:292 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:207 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_t_r_a_n_s_f_o_r_m_F_r_o_m │ │ │ │ │ +Point3 transformFrom(const Point3 &point, OptionalJacobian< 3, 6 > │ │ │ │ │ +Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const │ │ │ │ │ +takes point in Pose coordinates and transforms it to world coordinates │ │ │ │ │ +DDeeffiinniittiioonn Pose3.cpp:347 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_r_a_n_g_e │ │ │ │ │ +double range(const Point3 &point, OptionalJacobian< 1, 6 > Hself=boost::none, │ │ │ │ │ +OptionalJacobian< 1, 3 > Hpoint=boost::none) const │ │ │ │ │ +Calculate range to a landmark. │ │ │ │ │ +DDeeffiinniittiioonn Pose3.cpp:399 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_t_r_a_n_s_l_a_t_i_o_n │ │ │ │ │ +const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const │ │ │ │ │ +get translation │ │ │ │ │ +DDeeffiinniittiioonn Pose3.cpp:308 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3_:_:_r_o_t_a_t_i_o_n │ │ │ │ │ +const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const │ │ │ │ │ +get rotation │ │ │ │ │ +DDeeffiinniittiioonn Pose3.cpp:315 │ │ │ │ │ _g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor │ │ │ │ │ symbol GTSAM_USE_QUATERNIO... │ │ │ │ │ DDeeffiinniittiioonn Rot3.h:58 │ │ │ │ │ _g_t_s_a_m_:_:_U_n_i_t_3 │ │ │ │ │ Represents a 3D point on a unit sphere. │ │ │ │ │ DDeeffiinniittiioonn Unit3.h:43 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _P_o_s_e_3_._h │ │ │ │ │ + * _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00344.js │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00071.js │ │ │ │┄ Files 79% similar despite different names │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,11 +1,10 @@ │ │ │ │ │ -var a00344 = [ │ │ │ │ │ - ["gtsam::Range< Point3, Point3 >", "a03152.html", null], │ │ │ │ │ - ["Point3", "a00344.html#aaa8ed89fd60ea4601d9de63c4811525b", null], │ │ │ │ │ - ["cross", "a00344.html#a86bf6ed2ffb0ecc42a7deb8922f4ca4f", null], │ │ │ │ │ - ["distance3", "a00344.html#aa42f7ec660b7353de39f9c86a26711cc", null], │ │ │ │ │ - ["dot", "a00344.html#a7dc820e4e47218768f104b43a184a1bd", null], │ │ │ │ │ - ["mean", "a00344.html#a300205092710091b8745c0a145da20a1", null], │ │ │ │ │ - ["means", "a00344.html#a57f778b84bc6106471006421d289b832", null], │ │ │ │ │ - ["norm3", "a00344.html#ac37b6f807985ffd25217e33f6136fe58", null], │ │ │ │ │ - ["normalize", "a00344.html#abb52bb00c68909fc1147e5d112e8c2ae", null] │ │ │ │ │ +var a00071 = [ │ │ │ │ │ + ["gtsam::equals< V >", "a02480.html", null], │ │ │ │ │ + ["gtsam::equals_star< V >", "a02484.html", null], │ │ │ │ │ + ["gtsam::HasTestablePrereqs< T >", "a02488.html", null], │ │ │ │ │ + ["gtsam::Testable< T >", "a02492.html", null], │ │ │ │ │ + ["GTSAM_CONCEPT_TESTABLE_INST", "a00071.html#abe0a238cdd07d484ff0be56ae945b182", null], │ │ │ │ │ + ["assert_equal", "a00071.html#a659619cca082d1c10b07c033d48c54da", null], │ │ │ │ │ + ["equal", "a00071.html#a1f3dc67ad88b799c469088e428c583b1", null], │ │ │ │ │ + ["equal", "a00071.html#ad2dbee4a72127938c79162cc8b6d5152", null] │ │ │ │ │ ]; │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00344_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00332_source.html │ │ │ │┄ Files 99% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
Point3.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │ @@ -118,17 +118,17 @@ │ │ │ │
11
│ │ │ │
20// \callgraph
│ │ │ │
21
│ │ │ │
22#pragma once
│ │ │ │
23
│ │ │ │
24#include <gtsam/config.h>
│ │ │ │
25#include <gtsam/base/VectorSpace.h>
│ │ │ │ -
26#include <gtsam/base/Vector.h>
│ │ │ │ +
26#include <gtsam/base/Vector.h>
│ │ │ │
27#include <gtsam/dllexport.h>
│ │ │ │ -
28#include <gtsam/base/VectorSerialization.h>
│ │ │ │ +
28#include <gtsam/base/VectorSerialization.h>
│ │ │ │
29#include <boost/serialization/nvp.hpp>
│ │ │ │
30#include <numeric>
│ │ │ │
31
│ │ │ │
32namespace gtsam {
│ │ │ │
33
│ │ │ │
36typedef Vector3 Point3;
│ │ │ │
37typedef std::vector<Point3, Eigen::aligned_allocator<Point3> > Point3Vector;
│ │ │ │ @@ -180,16 +180,16 @@ │ │ │ │
87 return distance3(p, q, H1, H2);
│ │ │ │
88 }
│ │ │ │
89};
│ │ │ │
│ │ │ │
90
│ │ │ │
91} // namespace gtsam
│ │ │ │
92
│ │ │ │ -
VectorSerialization.h
serialization for Vectors
│ │ │ │ -
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ +
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ +
VectorSerialization.h
serialization for Vectors
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::mean
Point3 mean(const CONTAINER &points)
mean
Definition Point3.h:68
│ │ │ │
gtsam::cross
Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
cross product
Definition Point3.cpp:64
│ │ │ │
gtsam::means
Point2Pair means(const std::vector< Point2Pair > &abPointPairs)
Calculate the two means of a set of Point2 pairs.
Definition Point2.cpp:116
│ │ │ │
gtsam::distance3
double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian< 1, 3 > H1, OptionalJacobian< 1, 3 > H2)
distance between two points
Definition Point3.cpp:27
│ │ │ │
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │
gtsam::normalize
Point3 normalize(const Point3 &p, OptionalJacobian< 3, 3 > H)
normalize, with optional Jacobian
Definition Point3.cpp:52
│ │ │ │ @@ -198,13 +198,13 @@ │ │ │ │
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │
gtsam::Range
Definition BearingRange.h:40
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -82,18 +82,18 @@ │ │ │ │ │ 86 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> H2 = boost::none) { │ │ │ │ │ 87 return _d_i_s_t_a_n_c_e_3(p, q, H1, H2); │ │ │ │ │ 88 } │ │ │ │ │ 89}; │ │ │ │ │ 90 │ │ │ │ │ 91} // namespace gtsam │ │ │ │ │ 92 │ │ │ │ │ -_V_e_c_t_o_r_S_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ -serialization for Vectors │ │ │ │ │ _V_e_c_t_o_r_._h │ │ │ │ │ typedef and functions to augment Eigen's VectorXd │ │ │ │ │ +_V_e_c_t_o_r_S_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ +serialization for Vectors │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_m_e_a_n │ │ │ │ │ Point3 mean(const CONTAINER &points) │ │ │ │ │ mean │ │ │ │ │ DDeeffiinniittiioonn Point3.h:68 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00353_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00395_source.html │ │ │ │┄ Files 94% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/OrientedPlane3.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Cal3.h
│ │ │ │ +
OrientedPlane3.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ -
4 * Atlanta, Georgia 30332-0415
│ │ │ │ -
5 * All Rights Reserved
│ │ │ │ +
3 * Atlanta, Georgia 30332-0415
│ │ │ │ +
4 * All Rights Reserved
│ │ │ │ +
5 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
22#pragma once
│ │ │ │ -
23
│ │ │ │ -
24#include <gtsam/geometry/Point2.h>
│ │ │ │ -
25
│ │ │ │ -
26namespace gtsam {
│ │ │ │ -
27
│ │ │ │ -
46template <typename Cal, size_t Dim>
│ │ │ │ -
│ │ │ │ -
47void calibrateJacobians(const Cal& calibration, const Point2& pn,
│ │ │ │ -
48 OptionalJacobian<2, Dim> Dcal = boost::none,
│ │ │ │ -
49 OptionalJacobian<2, 2> Dp = boost::none) {
│ │ │ │ -
50 if (Dcal || Dp) {
│ │ │ │ -
51 Eigen::Matrix<double, 2, Dim> H_uncal_K;
│ │ │ │ -
52 Matrix22 H_uncal_pn, H_uncal_pn_inv;
│ │ │ │ +
12/*
│ │ │ │ +
13 * @file OrientedPlane3.h
│ │ │ │ +
14 * @date Dec 19, 2013
│ │ │ │ +
15 * @author Alex Trevor
│ │ │ │ +
16 * @author Frank Dellaert
│ │ │ │ +
17 * @author Zhaoyang Lv
│ │ │ │ +
18 * @brief An infinite plane, represented by a normal direction and perpendicular distance
│ │ │ │ +
19 */
│ │ │ │ +
20
│ │ │ │ +
21#pragma once
│ │ │ │ +
22
│ │ │ │ +
23#include <gtsam/geometry/Unit3.h>
│ │ │ │ +
24#include <gtsam/geometry/Pose3.h>
│ │ │ │ +
25#include <string>
│ │ │ │ +
26
│ │ │ │ +
27namespace gtsam {
│ │ │ │ +
28
│ │ │ │ +
│ │ │ │ +
36class GTSAM_EXPORT OrientedPlane3 {
│ │ │ │ +
37private:
│ │ │ │ +
38 Unit3 n_;
│ │ │ │ +
39 double d_;
│ │ │ │ +
40
│ │ │ │ +
41public:
│ │ │ │ +
42 enum {
│ │ │ │ +
43 dimension = 3
│ │ │ │ +
44 };
│ │ │ │ +
45
│ │ │ │ +
48
│ │ │ │ +
│ │ │ │ +
50 OrientedPlane3() :
│ │ │ │ +
51 n_(), d_(0.0) {
│ │ │ │ +
52 }
│ │ │ │ +
│ │ │ │
53
│ │ │ │ -
54 // Compute uncalibrate Jacobians
│ │ │ │ -
55 calibration.uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
│ │ │ │ -
56
│ │ │ │ -
57 H_uncal_pn_inv = H_uncal_pn.inverse();
│ │ │ │ +
│ │ │ │ +
55 OrientedPlane3(const Unit3& n, double d) :
│ │ │ │ +
56 n_(n), d_(d) {
│ │ │ │ +
57 }
│ │ │ │ +
│ │ │ │
58
│ │ │ │ -
59 if (Dp) *Dp = H_uncal_pn_inv;
│ │ │ │ -
60 if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
│ │ │ │ -
61 }
│ │ │ │ -
62}
│ │ │ │ -
│ │ │ │ -
63
│ │ │ │ -
│ │ │ │ -
69class GTSAM_EXPORT Cal3 {
│ │ │ │ -
70 protected:
│ │ │ │ -
71 double fx_ = 1.0f, fy_ = 1.0f;
│ │ │ │ -
72 double s_ = 0.0f;
│ │ │ │ -
73 double u0_ = 0.0f, v0_ = 0.0f;
│ │ │ │ -
74
│ │ │ │ -
75 public:
│ │ │ │ -
76 enum { dimension = 5 };
│ │ │ │ -
78 using shared_ptr = boost::shared_ptr<Cal3>;
│ │ │ │ -
79
│ │ │ │ +
│ │ │ │ +
60 explicit OrientedPlane3(const Vector4& vec)
│ │ │ │ +
61 : n_(vec(0), vec(1), vec(2)), d_(vec(3)) {}
│ │ │ │ +
│ │ │ │ +
62
│ │ │ │ +
│ │ │ │ +
64 OrientedPlane3(double a, double b, double c, double d) {
│ │ │ │ +
65 n_ = Unit3(a, b, c);
│ │ │ │ +
66 d_ = d;
│ │ │ │ +
67 }
│ │ │ │ +
│ │ │ │ +
68
│ │ │ │ +
72
│ │ │ │ +
74 void print(const std::string& s = std::string()) const;
│ │ │ │ +
75
│ │ │ │ +
│ │ │ │ +
77 bool equals(const OrientedPlane3& s, double tol = 1e-9) const {
│ │ │ │ +
78 return (n_.equals(s.n_, tol) && (std::abs(d_ - s.d_) < tol));
│ │ │ │ +
79 }
│ │ │ │ +
│ │ │ │ +
80
│ │ │ │
82
│ │ │ │ -
84 Cal3() = default;
│ │ │ │ -
85
│ │ │ │ -
│ │ │ │ -
87 Cal3(double fx, double fy, double s, double u0, double v0)
│ │ │ │ -
88 : fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {}
│ │ │ │ -
│ │ │ │ -
89
│ │ │ │ -
│ │ │ │ -
91 Cal3(const Vector5& d)
│ │ │ │ -
92 : fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) {}
│ │ │ │ -
│ │ │ │ -
93
│ │ │ │ -
100 Cal3(double fov, int w, int h);
│ │ │ │ -
101
│ │ │ │ -
103 virtual ~Cal3() {}
│ │ │ │ -
104
│ │ │ │ +
89 OrientedPlane3 transform(const Pose3& xr,
│ │ │ │ +
90 OptionalJacobian<3, 3> Hp = boost::none,
│ │ │ │ +
91 OptionalJacobian<3, 6> Hr = boost::none) const;
│ │ │ │ +
92
│ │ │ │ +
100 Vector3 errorVector(const OrientedPlane3& other,
│ │ │ │ +
101 OptionalJacobian<3, 3> H1 = boost::none,
│ │ │ │ +
102 OptionalJacobian<3, 3> H2 = boost::none) const;
│ │ │ │ +
103
│ │ │ │ +
│ │ │ │ +
105 inline static size_t Dim() {
│ │ │ │ +
106 return 3;
│ │ │ │ +
107 }
│ │ │ │ +
│ │ │ │
108
│ │ │ │ -
118 Cal3(const std::string& path);
│ │ │ │ -
119
│ │ │ │ -
123
│ │ │ │ -
125 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
│ │ │ │ -
126 const Cal3& cal);
│ │ │ │ -
127
│ │ │ │ -
129 virtual void print(const std::string& s = "") const;
│ │ │ │ -
130
│ │ │ │ -
132 bool equals(const Cal3& K, double tol = 10e-9) const;
│ │ │ │ -
133
│ │ │ │ -
137
│ │ │ │ -
139 inline double fx() const { return fx_; }
│ │ │ │ -
140
│ │ │ │ -
142 inline double fy() const { return fy_; }
│ │ │ │ +
│ │ │ │ +
110 inline size_t dim() const {
│ │ │ │ +
111 return 3;
│ │ │ │ +
112 }
│ │ │ │ +
│ │ │ │ +
113
│ │ │ │ +
115 OrientedPlane3 retract(const Vector3& v,
│ │ │ │ +
116 OptionalJacobian<3, 3> H = boost::none) const;
│ │ │ │ +
117
│ │ │ │ +
119 Vector3 localCoordinates(const OrientedPlane3& s) const;
│ │ │ │ +
120
│ │ │ │ +
│ │ │ │ +
122 inline Vector4 planeCoefficients() const {
│ │ │ │ +
123 Vector3 unit_vec = n_.unitVector();
│ │ │ │ +
124 return Vector4(unit_vec[0], unit_vec[1], unit_vec[2], d_);
│ │ │ │ +
125 }
│ │ │ │ +
│ │ │ │ +
126
│ │ │ │ +
│ │ │ │ +
128 inline Unit3 normal(OptionalJacobian<2, 3> H = boost::none) const {
│ │ │ │ +
129 if (H) *H << I_2x2, Z_2x1;
│ │ │ │ +
130 return n_;
│ │ │ │ +
131 }
│ │ │ │ +
│ │ │ │ +
132
│ │ │ │ +
│ │ │ │ +
134 inline double distance(OptionalJacobian<1, 3> H = boost::none) const {
│ │ │ │ +
135 if (H) *H << 0,0,1;
│ │ │ │ +
136 return d_;
│ │ │ │ +
137 }
│ │ │ │ +
│ │ │ │ +
138};
│ │ │ │ +
│ │ │ │ +
139
│ │ │ │ +
│ │ │ │ +
140template<> struct traits<OrientedPlane3> : public internal::Manifold<
│ │ │ │ +
141OrientedPlane3> {
│ │ │ │ +
142};
│ │ │ │ +
│ │ │ │
143
│ │ │ │ -
145 inline double aspectRatio() const { return fx_ / fy_; }
│ │ │ │ -
146
│ │ │ │ -
148 inline double skew() const { return s_; }
│ │ │ │ +
│ │ │ │ +
144template<> struct traits<const OrientedPlane3> : public internal::Manifold<
│ │ │ │ +
145OrientedPlane3> {
│ │ │ │ +
146};
│ │ │ │ +
│ │ │ │ +
147
│ │ │ │ +
148} // namespace gtsam
│ │ │ │
149
│ │ │ │ -
151 inline double px() const { return u0_; }
│ │ │ │ -
152
│ │ │ │ -
154 inline double py() const { return v0_; }
│ │ │ │ -
155
│ │ │ │ -
157 Point2 principalPoint() const { return Point2(u0_, v0_); }
│ │ │ │ -
158
│ │ │ │ -
│ │ │ │ -
160 Vector5 vector() const {
│ │ │ │ -
161 Vector5 v;
│ │ │ │ -
162 v << fx_, fy_, s_, u0_, v0_;
│ │ │ │ -
163 return v;
│ │ │ │ -
164 }
│ │ │ │ -
│ │ │ │ -
165
│ │ │ │ -
│ │ │ │ -
167 virtual Matrix3 K() const {
│ │ │ │ -
168 Matrix3 K;
│ │ │ │ -
169 K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
│ │ │ │ -
170 return K;
│ │ │ │ -
171 }
│ │ │ │ -
│ │ │ │ -
172
│ │ │ │ -
173#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ -
175 Matrix3 GTSAM_DEPRECATED matrix() const { return K(); }
│ │ │ │ -
176#endif
│ │ │ │ -
177
│ │ │ │ -
179 Matrix3 inverse() const;
│ │ │ │ -
180
│ │ │ │ -
182 inline virtual size_t dim() const { return Dim(); }
│ │ │ │ -
183
│ │ │ │ -
185 inline static size_t Dim() { return dimension; }
│ │ │ │ -
186
│ │ │ │ -
190
│ │ │ │ -
191 private:
│ │ │ │ -
193 friend class boost::serialization::access;
│ │ │ │ -
194 template <class Archive>
│ │ │ │ -
195 void serialize(Archive& ar, const unsigned int /*version*/) {
│ │ │ │ -
196 ar& BOOST_SERIALIZATION_NVP(fx_);
│ │ │ │ -
197 ar& BOOST_SERIALIZATION_NVP(fy_);
│ │ │ │ -
198 ar& BOOST_SERIALIZATION_NVP(s_);
│ │ │ │ -
199 ar& BOOST_SERIALIZATION_NVP(u0_);
│ │ │ │ -
200 ar& BOOST_SERIALIZATION_NVP(v0_);
│ │ │ │ -
201 }
│ │ │ │ -
202
│ │ │ │ -
204};
│ │ │ │ -
│ │ │ │ -
205
│ │ │ │ -
206} // \ namespace gtsam
│ │ │ │ -
Point2.h
2D Point
│ │ │ │ +
Pose3.h
3D Pose
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │ -
gtsam::calibrateJacobians
void calibrateJacobians(const Cal &calibration, const Point2 &pn, OptionalJacobian< 2, Dim > Dcal=boost::none, OptionalJacobian< 2, 2 > Dp=boost::none)
Function which makes use of the Implicit Function Theorem to compute the Jacobians of calibrate using...
Definition Cal3.h:47
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
│ │ │ │
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Cal3
Common base class for all calibration models.
Definition Cal3.h:69
│ │ │ │ -
gtsam::Cal3::Cal3
Cal3(double fx, double fy, double s, double u0, double v0)
constructor from doubles
Definition Cal3.h:87
│ │ │ │ -
gtsam::Cal3::K
virtual Matrix3 K() const
return calibration matrix K
Definition Cal3.h:167
│ │ │ │ -
gtsam::Cal3::Cal3
Cal3(const Vector5 &d)
constructor from vector
Definition Cal3.h:91
│ │ │ │ -
gtsam::Cal3::~Cal3
virtual ~Cal3()
Virtual destructor.
Definition Cal3.h:103
│ │ │ │ -
gtsam::Cal3::vector
Vector5 vector() const
vectorized form (column-wise)
Definition Cal3.h:160
│ │ │ │ -
gtsam::Cal3::Cal3
Cal3()=default
Create a default calibration that leaves coordinates unchanged.
│ │ │ │ -
gtsam::Cal3::px
double px() const
image center in x
Definition Cal3.h:151
│ │ │ │ -
gtsam::Cal3::Dim
static size_t Dim()
return DOF, dimensionality of tangent space
Definition Cal3.h:185
│ │ │ │ -
gtsam::Cal3::principalPoint
Point2 principalPoint() const
return the principal point
Definition Cal3.h:157
│ │ │ │ -
gtsam::Cal3::fx
double fx() const
focal length x
Definition Cal3.h:139
│ │ │ │ -
gtsam::Cal3::py
double py() const
image center in y
Definition Cal3.h:154
│ │ │ │ -
gtsam::Cal3::skew
double skew() const
skew
Definition Cal3.h:148
│ │ │ │ -
gtsam::Cal3::dim
virtual size_t dim() const
return DOF, dimensionality of tangent space
Definition Cal3.h:182
│ │ │ │ -
gtsam::Cal3::aspectRatio
double aspectRatio() const
aspect ratio
Definition Cal3.h:145
│ │ │ │ -
gtsam::Cal3::fy
double fy() const
focal length y
Definition Cal3.h:142
│ │ │ │ +
gtsam::OrientedPlane3
Represents an infinite plane in 3D, which is composed of a planar normal and its perpendicular distan...
Definition OrientedPlane3.h:36
│ │ │ │ +
gtsam::OrientedPlane3::OrientedPlane3
OrientedPlane3(double a, double b, double c, double d)
Construct from four numbers of plane coeffcients (a, b, c, d)
Definition OrientedPlane3.h:64
│ │ │ │ +
gtsam::OrientedPlane3::planeCoefficients
Vector4 planeCoefficients() const
Returns the plane coefficients.
Definition OrientedPlane3.h:122
│ │ │ │ +
gtsam::OrientedPlane3::OrientedPlane3
OrientedPlane3(const Unit3 &n, double d)
Construct from a Unit3 and a distance.
Definition OrientedPlane3.h:55
│ │ │ │ +
gtsam::OrientedPlane3::dim
size_t dim() const
Dimensionality of tangent space = 3 DOF.
Definition OrientedPlane3.h:110
│ │ │ │ +
gtsam::OrientedPlane3::equals
bool equals(const OrientedPlane3 &s, double tol=1e-9) const
The equals function with tolerance.
Definition OrientedPlane3.h:77
│ │ │ │ +
gtsam::OrientedPlane3::OrientedPlane3
OrientedPlane3(const Vector4 &vec)
Construct from a vector of plane coefficients.
Definition OrientedPlane3.h:60
│ │ │ │ +
gtsam::OrientedPlane3::Dim
static size_t Dim()
Dimensionality of tangent space = 3 DOF.
Definition OrientedPlane3.h:105
│ │ │ │ +
gtsam::OrientedPlane3::OrientedPlane3
OrientedPlane3()
Default constructor.
Definition OrientedPlane3.h:50
│ │ │ │ +
gtsam::OrientedPlane3::distance
double distance(OptionalJacobian< 1, 3 > H=boost::none) const
Return the perpendicular distance to the origin.
Definition OrientedPlane3.h:134
│ │ │ │ +
gtsam::OrientedPlane3::normal
Unit3 normal(OptionalJacobian< 2, 3 > H=boost::none) const
Return the normal.
Definition OrientedPlane3.h:128
│ │ │ │ +
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ +
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
│ │ │ │ +
gtsam::Unit3::unitVector
Vector3 unitVector(OptionalJacobian< 3, 2 > H=boost::none) const
Return unit-norm Vector.
Definition Unit3.cpp:151
│ │ │ │ +
gtsam::Unit3::equals
bool equals(const Unit3 &s, double tol=1e-9) const
The equals function with tolerance.
Definition Unit3.h:115
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,224 +1,201 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Cal3.h │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +OrientedPlane3.h │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ -4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -5 * All Rights Reserved │ │ │ │ │ +3 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +4 * All Rights Reserved │ │ │ │ │ +5 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -22#pragma once │ │ │ │ │ -23 │ │ │ │ │ -24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ -25 │ │ │ │ │ -26namespace _g_t_s_a_m { │ │ │ │ │ -27 │ │ │ │ │ -46template │ │ │ │ │ -_4_7void _c_a_l_i_b_r_a_t_e_J_a_c_o_b_i_a_n_s(const Cal& calibration, const _P_o_i_n_t_2& pn, │ │ │ │ │ -48 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _D_i_m_> Dcal = boost::none, │ │ │ │ │ -49 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dp = boost::none) { │ │ │ │ │ -50 if (Dcal || Dp) { │ │ │ │ │ -51 Eigen::Matrix H_uncal_K; │ │ │ │ │ -52 Matrix22 H_uncal_pn, H_uncal_pn_inv; │ │ │ │ │ +12/* │ │ │ │ │ +13 * @file OrientedPlane3.h │ │ │ │ │ +14 * @date Dec 19, 2013 │ │ │ │ │ +15 * @author Alex Trevor │ │ │ │ │ +16 * @author Frank Dellaert │ │ │ │ │ +17 * @author Zhaoyang Lv │ │ │ │ │ +18 * @brief An infinite plane, represented by a normal direction and │ │ │ │ │ +perpendicular distance │ │ │ │ │ +19 */ │ │ │ │ │ +20 │ │ │ │ │ +21#pragma once │ │ │ │ │ +22 │ │ │ │ │ +23#include │ │ │ │ │ +24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_3_._h> │ │ │ │ │ +25#include │ │ │ │ │ +26 │ │ │ │ │ +27namespace _g_t_s_a_m { │ │ │ │ │ +28 │ │ │ │ │ +_3_6class GTSAM_EXPORT _O_r_i_e_n_t_e_d_P_l_a_n_e_3 { │ │ │ │ │ +37private: │ │ │ │ │ +38 _U_n_i_t_3 n_; │ │ │ │ │ +39 double d_; │ │ │ │ │ +40 │ │ │ │ │ +41public: │ │ │ │ │ +42 enum { │ │ │ │ │ +43 dimension = 3 │ │ │ │ │ +44 }; │ │ │ │ │ +45 │ │ │ │ │ +48 │ │ │ │ │ +_5_0 _O_r_i_e_n_t_e_d_P_l_a_n_e_3() : │ │ │ │ │ +51 n_(), d_(0.0) { │ │ │ │ │ +52 } │ │ │ │ │ 53 │ │ │ │ │ -54 // Compute uncalibrate Jacobians │ │ │ │ │ -55 calibration.uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn); │ │ │ │ │ -56 │ │ │ │ │ -57 H_uncal_pn_inv = H_uncal_pn.inverse(); │ │ │ │ │ +_5_5 _O_r_i_e_n_t_e_d_P_l_a_n_e_3(const _U_n_i_t_3& n, double d) : │ │ │ │ │ +56 n_(n), d_(d) { │ │ │ │ │ +57 } │ │ │ │ │ 58 │ │ │ │ │ -59 if (Dp) *Dp = H_uncal_pn_inv; │ │ │ │ │ -60 if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K; │ │ │ │ │ -61 } │ │ │ │ │ -62} │ │ │ │ │ -63 │ │ │ │ │ -_6_9class GTSAM_EXPORT _C_a_l_3 { │ │ │ │ │ -70 protected: │ │ │ │ │ -_7_1 double fx_ = 1.0f, fy_ = 1.0f; │ │ │ │ │ -_7_2 double s_ = 0.0f; │ │ │ │ │ -_7_3 double u0_ = 0.0f, v0_ = 0.0f; │ │ │ │ │ -74 │ │ │ │ │ -75 public: │ │ │ │ │ -76 enum { dimension = 5 }; │ │ │ │ │ -78 using shared_ptr = boost::shared_ptr; │ │ │ │ │ -79 │ │ │ │ │ +_6_0 explicit _O_r_i_e_n_t_e_d_P_l_a_n_e_3(const Vector4& vec) │ │ │ │ │ +61 : n_(vec(0), vec(1), vec(2)), d_(vec(3)) {} │ │ │ │ │ +62 │ │ │ │ │ +_6_4 _O_r_i_e_n_t_e_d_P_l_a_n_e_3(double a, double b, double c, double d) { │ │ │ │ │ +65 n_ = _U_n_i_t_3(a, b, c); │ │ │ │ │ +66 d_ = d; │ │ │ │ │ +67 } │ │ │ │ │ +68 │ │ │ │ │ +72 │ │ │ │ │ +74 void _p_r_i_n_t(const std::string& s = std::string()) const; │ │ │ │ │ +75 │ │ │ │ │ +_7_7 bool _e_q_u_a_l_s(const _O_r_i_e_n_t_e_d_P_l_a_n_e_3& s, double tol = 1e-9) const { │ │ │ │ │ +78 return (n_._e_q_u_a_l_s(s.n_, tol) && (std::abs(d_ - s.d_) < tol)); │ │ │ │ │ +79 } │ │ │ │ │ +80 │ │ │ │ │ 82 │ │ │ │ │ -_8_4 _C_a_l_3() = default; │ │ │ │ │ -85 │ │ │ │ │ -_8_7 _C_a_l_3(double fx, double fy, double s, double u0, double v0) │ │ │ │ │ -88 : fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) {} │ │ │ │ │ -89 │ │ │ │ │ -_9_1 _C_a_l_3(const Vector5& d) │ │ │ │ │ -92 : fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) {} │ │ │ │ │ -93 │ │ │ │ │ -100 _C_a_l_3(double fov, int w, int h); │ │ │ │ │ -101 │ │ │ │ │ -_1_0_3 virtual _~_C_a_l_3() {} │ │ │ │ │ -104 │ │ │ │ │ +89 _O_r_i_e_n_t_e_d_P_l_a_n_e_3 transform(const _P_o_s_e_3& xr, │ │ │ │ │ +90 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> Hp = boost::none, │ │ │ │ │ +91 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _6_> Hr = boost::none) const; │ │ │ │ │ +92 │ │ │ │ │ +100 Vector3 errorVector(const _O_r_i_e_n_t_e_d_P_l_a_n_e_3& other, │ │ │ │ │ +101 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H1 = boost::none, │ │ │ │ │ +102 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H2 = boost::none) const; │ │ │ │ │ +103 │ │ │ │ │ +_1_0_5 inline static size_t _D_i_m() { │ │ │ │ │ +106 return 3; │ │ │ │ │ +107 } │ │ │ │ │ 108 │ │ │ │ │ -118 _C_a_l_3(const std::string& path); │ │ │ │ │ -119 │ │ │ │ │ -123 │ │ │ │ │ -125 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, │ │ │ │ │ -126 const _C_a_l_3& cal); │ │ │ │ │ -127 │ │ │ │ │ -129 virtual void _p_r_i_n_t(const std::string& s = "") const; │ │ │ │ │ -130 │ │ │ │ │ -132 bool _e_q_u_a_l_s(const _C_a_l_3& K, double tol = 10e-9) const; │ │ │ │ │ -133 │ │ │ │ │ -137 │ │ │ │ │ -_1_3_9 inline double _f_x() const { return fx_; } │ │ │ │ │ -140 │ │ │ │ │ -_1_4_2 inline double _f_y() const { return fy_; } │ │ │ │ │ +_1_1_0 inline size_t _d_i_m() const { │ │ │ │ │ +111 return 3; │ │ │ │ │ +112 } │ │ │ │ │ +113 │ │ │ │ │ +115 _O_r_i_e_n_t_e_d_P_l_a_n_e_3 retract(const Vector3& v, │ │ │ │ │ +116 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H = boost::none) const; │ │ │ │ │ +117 │ │ │ │ │ +119 Vector3 localCoordinates(const _O_r_i_e_n_t_e_d_P_l_a_n_e_3& s) const; │ │ │ │ │ +120 │ │ │ │ │ +_1_2_2 inline Vector4 _p_l_a_n_e_C_o_e_f_f_i_c_i_e_n_t_s() const { │ │ │ │ │ +123 Vector3 unit_vec = n_._u_n_i_t_V_e_c_t_o_r(); │ │ │ │ │ +124 return Vector4(unit_vec[0], unit_vec[1], unit_vec[2], d_); │ │ │ │ │ +125 } │ │ │ │ │ +126 │ │ │ │ │ +_1_2_8 inline _U_n_i_t_3 _n_o_r_m_a_l(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> H = boost::none) const { │ │ │ │ │ +129 if (H) *H << I_2x2, Z_2x1; │ │ │ │ │ +130 return n_; │ │ │ │ │ +131 } │ │ │ │ │ +132 │ │ │ │ │ +_1_3_4 inline double _d_i_s_t_a_n_c_e(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> H = boost::none) const { │ │ │ │ │ +135 if (H) *H << 0,0,1; │ │ │ │ │ +136 return d_; │ │ │ │ │ +137 } │ │ │ │ │ +138}; │ │ │ │ │ +139 │ │ │ │ │ +_1_4_0template<> struct _t_r_a_i_t_s<_O_r_i_e_n_t_e_d_P_l_a_n_e_3> : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d< │ │ │ │ │ +141OrientedPlane3> { │ │ │ │ │ +142}; │ │ │ │ │ 143 │ │ │ │ │ -_1_4_5 inline double _a_s_p_e_c_t_R_a_t_i_o() const { return fx_ / fy_; } │ │ │ │ │ -146 │ │ │ │ │ -_1_4_8 inline double _s_k_e_w() const { return s_; } │ │ │ │ │ +_1_4_4template<> struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d< │ │ │ │ │ +145OrientedPlane3> { │ │ │ │ │ +146}; │ │ │ │ │ +147 │ │ │ │ │ +148} // namespace gtsam │ │ │ │ │ 149 │ │ │ │ │ -_1_5_1 inline double _p_x() const { return u0_; } │ │ │ │ │ -152 │ │ │ │ │ -_1_5_4 inline double _p_y() const { return v0_; } │ │ │ │ │ -155 │ │ │ │ │ -_1_5_7 _P_o_i_n_t_2 _p_r_i_n_c_i_p_a_l_P_o_i_n_t() const { return _P_o_i_n_t_2(u0_, v0_); } │ │ │ │ │ -158 │ │ │ │ │ -_1_6_0 Vector5 _v_e_c_t_o_r() const { │ │ │ │ │ -161 Vector5 v; │ │ │ │ │ -162 v << fx_, fy_, s_, u0_, v0_; │ │ │ │ │ -163 return v; │ │ │ │ │ -164 } │ │ │ │ │ -165 │ │ │ │ │ -_1_6_7 virtual Matrix3 _K() const { │ │ │ │ │ -168 Matrix3 K; │ │ │ │ │ -169 K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0; │ │ │ │ │ -170 return K; │ │ │ │ │ -171 } │ │ │ │ │ -172 │ │ │ │ │ -173#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ -175 Matrix3 GTSAM_DEPRECATED matrix() const { return K(); } │ │ │ │ │ -176#endif │ │ │ │ │ -177 │ │ │ │ │ -179 Matrix3 inverse() const; │ │ │ │ │ -180 │ │ │ │ │ -_1_8_2 inline virtual size_t _d_i_m() const { return Dim(); } │ │ │ │ │ -183 │ │ │ │ │ -_1_8_5 inline static size_t _D_i_m() { return dimension; } │ │ │ │ │ -186 │ │ │ │ │ -190 │ │ │ │ │ -191 private: │ │ │ │ │ -_1_9_3 friend class boost::serialization::access; │ │ │ │ │ -194 template │ │ │ │ │ -195 void serialize(Archive& ar, const unsigned int /*version*/) { │ │ │ │ │ -196 ar& BOOST_SERIALIZATION_NVP(fx_); │ │ │ │ │ -197 ar& BOOST_SERIALIZATION_NVP(fy_); │ │ │ │ │ -198 ar& BOOST_SERIALIZATION_NVP(s_); │ │ │ │ │ -199 ar& BOOST_SERIALIZATION_NVP(u0_); │ │ │ │ │ -200 ar& BOOST_SERIALIZATION_NVP(v0_); │ │ │ │ │ -201 } │ │ │ │ │ -202 │ │ │ │ │ -204}; │ │ │ │ │ -205 │ │ │ │ │ -206} // \ namespace gtsam │ │ │ │ │ -_P_o_i_n_t_2_._h │ │ │ │ │ -2D Point │ │ │ │ │ +_P_o_s_e_3_._h │ │ │ │ │ +3D Pose │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ print without optional string, must specify cout yourself │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ -Vector2 Point2 │ │ │ │ │ -As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point2 to Vector2... │ │ │ │ │ -DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ -_g_t_s_a_m_:_:_c_a_l_i_b_r_a_t_e_J_a_c_o_b_i_a_n_s │ │ │ │ │ -void calibrateJacobians(const Cal &calibration, const Point2 &pn, │ │ │ │ │ -OptionalJacobian< 2, Dim > Dcal=boost::none, OptionalJacobian< 2, 2 > │ │ │ │ │ -Dp=boost::none) │ │ │ │ │ -Function which makes use of the Implicit Function Theorem to compute the │ │ │ │ │ -Jacobians of calibrate using... │ │ │ │ │ -DDeeffiinniittiioonn Cal3.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ +Both ManifoldTraits and Testable. │ │ │ │ │ +DDeeffiinniittiioonn Manifold.h:120 │ │ │ │ │ _g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ either a fixed size o... │ │ │ │ │ DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3 │ │ │ │ │ -Common base class for all calibration models. │ │ │ │ │ -DDeeffiinniittiioonn Cal3.h:69 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_:_:_C_a_l_3 │ │ │ │ │ -Cal3(double fx, double fy, double s, double u0, double v0) │ │ │ │ │ -constructor from doubles │ │ │ │ │ -DDeeffiinniittiioonn Cal3.h:87 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_:_:_K │ │ │ │ │ -virtual Matrix3 K() const │ │ │ │ │ -return calibration matrix K │ │ │ │ │ -DDeeffiinniittiioonn Cal3.h:167 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_:_:_C_a_l_3 │ │ │ │ │ -Cal3(const Vector5 &d) │ │ │ │ │ -constructor from vector │ │ │ │ │ -DDeeffiinniittiioonn Cal3.h:91 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_:_:_~_C_a_l_3 │ │ │ │ │ -virtual ~Cal3() │ │ │ │ │ -Virtual destructor. │ │ │ │ │ -DDeeffiinniittiioonn Cal3.h:103 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_:_:_v_e_c_t_o_r │ │ │ │ │ -Vector5 vector() const │ │ │ │ │ -vectorized form (column-wise) │ │ │ │ │ -DDeeffiinniittiioonn Cal3.h:160 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_:_:_C_a_l_3 │ │ │ │ │ -Cal3()=default │ │ │ │ │ -Create a default calibration that leaves coordinates unchanged. │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_:_:_p_x │ │ │ │ │ -double px() const │ │ │ │ │ -image center in x │ │ │ │ │ -DDeeffiinniittiioonn Cal3.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_:_:_D_i_m │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3 │ │ │ │ │ +Represents an infinite plane in 3D, which is composed of a planar normal and │ │ │ │ │ +its perpendicular distan... │ │ │ │ │ +DDeeffiinniittiioonn OrientedPlane3.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3 │ │ │ │ │ +OrientedPlane3(double a, double b, double c, double d) │ │ │ │ │ +Construct from four numbers of plane coeffcients (a, b, c, d) │ │ │ │ │ +DDeeffiinniittiioonn OrientedPlane3.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_:_:_p_l_a_n_e_C_o_e_f_f_i_c_i_e_n_t_s │ │ │ │ │ +Vector4 planeCoefficients() const │ │ │ │ │ +Returns the plane coefficients. │ │ │ │ │ +DDeeffiinniittiioonn OrientedPlane3.h:122 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3 │ │ │ │ │ +OrientedPlane3(const Unit3 &n, double d) │ │ │ │ │ +Construct from a Unit3 and a distance. │ │ │ │ │ +DDeeffiinniittiioonn OrientedPlane3.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_:_:_d_i_m │ │ │ │ │ +size_t dim() const │ │ │ │ │ +Dimensionality of tangent space = 3 DOF. │ │ │ │ │ +DDeeffiinniittiioonn OrientedPlane3.h:110 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const OrientedPlane3 &s, double tol=1e-9) const │ │ │ │ │ +The equals function with tolerance. │ │ │ │ │ +DDeeffiinniittiioonn OrientedPlane3.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3 │ │ │ │ │ +OrientedPlane3(const Vector4 &vec) │ │ │ │ │ +Construct from a vector of plane coefficients. │ │ │ │ │ +DDeeffiinniittiioonn OrientedPlane3.h:60 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_:_:_D_i_m │ │ │ │ │ static size_t Dim() │ │ │ │ │ -return DOF, dimensionality of tangent space │ │ │ │ │ -DDeeffiinniittiioonn Cal3.h:185 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_:_:_p_r_i_n_c_i_p_a_l_P_o_i_n_t │ │ │ │ │ -Point2 principalPoint() const │ │ │ │ │ -return the principal point │ │ │ │ │ -DDeeffiinniittiioonn Cal3.h:157 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_:_:_f_x │ │ │ │ │ -double fx() const │ │ │ │ │ -focal length x │ │ │ │ │ -DDeeffiinniittiioonn Cal3.h:139 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_:_:_p_y │ │ │ │ │ -double py() const │ │ │ │ │ -image center in y │ │ │ │ │ -DDeeffiinniittiioonn Cal3.h:154 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_:_:_s_k_e_w │ │ │ │ │ -double skew() const │ │ │ │ │ -skew │ │ │ │ │ -DDeeffiinniittiioonn Cal3.h:148 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_:_:_d_i_m │ │ │ │ │ -virtual size_t dim() const │ │ │ │ │ -return DOF, dimensionality of tangent space │ │ │ │ │ -DDeeffiinniittiioonn Cal3.h:182 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_:_:_a_s_p_e_c_t_R_a_t_i_o │ │ │ │ │ -double aspectRatio() const │ │ │ │ │ -aspect ratio │ │ │ │ │ -DDeeffiinniittiioonn Cal3.h:145 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_:_:_f_y │ │ │ │ │ -double fy() const │ │ │ │ │ -focal length y │ │ │ │ │ -DDeeffiinniittiioonn Cal3.h:142 │ │ │ │ │ +Dimensionality of tangent space = 3 DOF. │ │ │ │ │ +DDeeffiinniittiioonn OrientedPlane3.h:105 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3 │ │ │ │ │ +OrientedPlane3() │ │ │ │ │ +Default constructor. │ │ │ │ │ +DDeeffiinniittiioonn OrientedPlane3.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_:_:_d_i_s_t_a_n_c_e │ │ │ │ │ +double distance(OptionalJacobian< 1, 3 > H=boost::none) const │ │ │ │ │ +Return the perpendicular distance to the origin. │ │ │ │ │ +DDeeffiinniittiioonn OrientedPlane3.h:134 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_i_e_n_t_e_d_P_l_a_n_e_3_:_:_n_o_r_m_a_l │ │ │ │ │ +Unit3 normal(OptionalJacobian< 2, 3 > H=boost::none) const │ │ │ │ │ +Return the normal. │ │ │ │ │ +DDeeffiinniittiioonn OrientedPlane3.h:128 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ +A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_U_n_i_t_3 │ │ │ │ │ +Represents a 3D point on a unit sphere. │ │ │ │ │ +DDeeffiinniittiioonn Unit3.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_U_n_i_t_3_:_:_u_n_i_t_V_e_c_t_o_r │ │ │ │ │ +Vector3 unitVector(OptionalJacobian< 3, 2 > H=boost::none) const │ │ │ │ │ +Return unit-norm Vector. │ │ │ │ │ +DDeeffiinniittiioonn Unit3.cpp:151 │ │ │ │ │ +_g_t_s_a_m_:_:_U_n_i_t_3_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const Unit3 &s, double tol=1e-9) const │ │ │ │ │ +The equals function with tolerance. │ │ │ │ │ +DDeeffiinniittiioonn Unit3.h:115 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3_._h │ │ │ │ │ + * OOrriieenntteeddPPllaannee33..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00368_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00425_source.html │ │ │ │┄ Files 99% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
BearingRange.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/base/Manifold.h>
│ │ │ │ -
22#include <gtsam/base/Testable.h>
│ │ │ │ -
23#include <gtsam/base/OptionalJacobian.h>
│ │ │ │ +
21#include <gtsam/base/Manifold.h>
│ │ │ │ +
22#include <gtsam/base/Testable.h>
│ │ │ │ +
23#include <gtsam/base/OptionalJacobian.h>
│ │ │ │
24#include <boost/concept/assert.hpp>
│ │ │ │
25#include <boost/serialization/nvp.hpp>
│ │ │ │
26#include <iostream>
│ │ │ │
27
│ │ │ │
28namespace gtsam {
│ │ │ │
29
│ │ │ │
30// Forward declaration of Bearing functor which should be of A1*A2 -> return_type
│ │ │ │ @@ -241,15 +241,15 @@ │ │ │ │
158
│ │ │ │
160
│ │ │ │
161 // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
│ │ │ │
162 enum {
│ │ │ │
163 NeedsToAlign = (sizeof(B) % 16) == 0 || (sizeof(R) % 16) == 0
│ │ │ │
164 };
│ │ │ │
165public:
│ │ │ │ -
166 GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
│ │ │ │ +
166 GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
│ │ │ │
167};
│ │ │ │
│ │ │ │
168
│ │ │ │
169// Declare this to be both Testable and a Manifold
│ │ │ │
170template <typename A1, typename A2>
│ │ │ │
│ │ │ │
171struct traits<BearingRange<A1, A2> >
│ │ │ │ @@ -287,18 +287,18 @@ │ │ │ │
199 OptionalJacobian<traits<RT>::dimension, traits<A2>::dimension> H2=boost::none) {
│ │ │ │
200 return a1.range(a2, H1, H2);
│ │ │ │
201 }
│ │ │ │
202};
│ │ │ │
│ │ │ │
203
│ │ │ │
204} // namespace gtsam
│ │ │ │ -
OptionalJacobian.h
Special class for optional Jacobian arguments.
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ -
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ -
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
This marks a GTSAM object to require alignment.
Definition types.h:317
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
OptionalJacobian.h
Special class for optional Jacobian arguments.
│ │ │ │ +
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ +
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
This marks a GTSAM object to require alignment.
Definition types.h:317
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │
gtsam::internal::ManifoldTraits
A helper that implements the traits interface for GTSAM manifolds.
Definition Manifold.h:95
│ │ │ │
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │
gtsam::Bearing
Definition BearingRange.h:34
│ │ │ │
gtsam::Range
Definition BearingRange.h:40
│ │ │ │ @@ -313,13 +313,13 @@ │ │ │ │
gtsam::HasBearing
Definition BearingRange.h:180
│ │ │ │
gtsam::HasRange
Definition BearingRange.h:194
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -189,18 +189,18 @@ │ │ │ │ │ 199 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n<_t_r_a_i_t_s_<_R_T_>_:_:_d_i_m_e_n_s_i_o_n, _t_r_a_i_t_s_<_A_2_>_:_:_d_i_m_e_n_s_i_o_n> H2=boost:: │ │ │ │ │ none) { │ │ │ │ │ 200 return a1.range(a2, H1, H2); │ │ │ │ │ 201 } │ │ │ │ │ 202}; │ │ │ │ │ 203 │ │ │ │ │ 204} // namespace gtsam │ │ │ │ │ -_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h │ │ │ │ │ -Special class for optional Jacobian arguments. │ │ │ │ │ _T_e_s_t_a_b_l_e_._h │ │ │ │ │ Concept check for values that can be used in unit tests. │ │ │ │ │ +_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h │ │ │ │ │ +Special class for optional Jacobian arguments. │ │ │ │ │ _M_a_n_i_f_o_l_d_._h │ │ │ │ │ Base class and basic functions for Manifold types. │ │ │ │ │ _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W___I_F │ │ │ │ │ #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) │ │ │ │ │ This marks a GTSAM object to require alignment. │ │ │ │ │ DDeeffiinniittiioonn types.h:317 │ │ │ │ │ _g_t_s_a_m │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00377_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00326_source.html │ │ │ │┄ Files 95% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/StereoCamera.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/StereoPoint2.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
StereoCamera.h
│ │ │ │ +
StereoPoint2.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/geometry/Cal3_S2Stereo.h>
│ │ │ │ -
21#include <gtsam/geometry/Pose3.h>
│ │ │ │ -
22#include <gtsam/geometry/StereoPoint2.h>
│ │ │ │ -
23
│ │ │ │ -
24namespace gtsam {
│ │ │ │ -
25
│ │ │ │ -
│ │ │ │ -
26class GTSAM_EXPORT StereoCheiralityException: public std::runtime_error {
│ │ │ │ -
27public:
│ │ │ │ -
28 StereoCheiralityException()
│ │ │ │ -
29 : StereoCheiralityException(std::numeric_limits<Key>::max()) {}
│ │ │ │ -
30
│ │ │ │ -
31 StereoCheiralityException(Key j)
│ │ │ │ -
32 : std::runtime_error("Stereo Cheirality Exception"),
│ │ │ │ -
33 j_(j) {}
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/geometry/Point2.h>
│ │ │ │ +
22#include <gtsam/base/VectorSpace.h>
│ │ │ │ +
23#include <boost/serialization/nvp.hpp>
│ │ │ │ +
24
│ │ │ │ +
25namespace gtsam {
│ │ │ │ +
26
│ │ │ │ +
│ │ │ │ +
32class GTSAM_EXPORT StereoPoint2 {
│ │ │ │ +
33private:
│ │ │ │
34
│ │ │ │ -
35 Key nearbyVariable() const {
│ │ │ │ -
36 return j_;
│ │ │ │ -
37 }
│ │ │ │ -
38
│ │ │ │ -
39private:
│ │ │ │ -
40 Key j_;
│ │ │ │ -
41};
│ │ │ │ -
│ │ │ │ -
42
│ │ │ │ -
│ │ │ │ -
47class GTSAM_EXPORT StereoCamera {
│ │ │ │ -
48
│ │ │ │ -
49public:
│ │ │ │ -
50
│ │ │ │ -
55 typedef StereoPoint2 Measurement;
│ │ │ │ -
56 typedef StereoPoint2Vector MeasurementVector;
│ │ │ │ -
57
│ │ │ │ -
58private:
│ │ │ │ -
59 Pose3 leftCamPose_;
│ │ │ │ -
60 Cal3_S2Stereo::shared_ptr K_;
│ │ │ │ -
61
│ │ │ │ -
62public:
│ │ │ │ -
63
│ │ │ │ -
64 enum {
│ │ │ │ -
65 dimension = 6
│ │ │ │ -
66 };
│ │ │ │ -
67
│ │ │ │ -
70
│ │ │ │ -
│ │ │ │ -
72 StereoCamera() :
│ │ │ │ -
73 K_(new Cal3_S2Stereo()) {
│ │ │ │ -
74 }
│ │ │ │ -
│ │ │ │ -
75
│ │ │ │ -
77 StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K);
│ │ │ │ -
78
│ │ │ │ -
│ │ │ │ -
80 const Cal3_S2Stereo& calibration() const {
│ │ │ │ -
81 return *K_;
│ │ │ │ -
82 }
│ │ │ │ +
35 double uL_, uR_, v_;
│ │ │ │ +
36
│ │ │ │ +
37public:
│ │ │ │ +
38 enum { dimension = 3 };
│ │ │ │ +
41
│ │ │ │ +
│ │ │ │ +
43 StereoPoint2() :
│ │ │ │ +
44 uL_(0), uR_(0), v_(0) {
│ │ │ │ +
45 }
│ │ │ │ +
│ │ │ │ +
46
│ │ │ │ +
│ │ │ │ +
48 StereoPoint2(double uL, double uR, double v) :
│ │ │ │ +
49 uL_(uL), uR_(uR), v_(v) {
│ │ │ │ +
50 }
│ │ │ │ +
│ │ │ │ +
51
│ │ │ │ +
│ │ │ │ +
53 explicit StereoPoint2(const Vector3& v) :
│ │ │ │ +
54 uL_(v(0)), uR_(v(1)), v_(v(2)) {}
│ │ │ │ +
│ │ │ │ +
55
│ │ │ │ +
59
│ │ │ │ +
61 void print(const std::string& s = "") const;
│ │ │ │ +
62
│ │ │ │ +
│ │ │ │ +
64 bool equals(const StereoPoint2& q, double tol = 1e-9) const {
│ │ │ │ +
65 return (std::abs(uL_ - q.uL_) < tol && std::abs(uR_ - q.uR_) < tol
│ │ │ │ +
66 && std::abs(v_ - q.v_) < tol);
│ │ │ │ +
67 }
│ │ │ │ +
│ │ │ │ +
68
│ │ │ │ +
72
│ │ │ │ +
│ │ │ │ +
74 inline static StereoPoint2 Identity() {
│ │ │ │ +
75 return StereoPoint2();
│ │ │ │ +
76 }
│ │ │ │ +
│ │ │ │ +
77
│ │ │ │ +
│ │ │ │ +
79 StereoPoint2 operator-() const {
│ │ │ │ +
80 return StereoPoint2(-uL_, -uR_, -v_);
│ │ │ │ +
81 }
│ │ │ │ +
│ │ │ │ +
82
│ │ │ │ +
│ │ │ │ +
84 inline StereoPoint2 operator +(const Vector3& v) const {
│ │ │ │ +
85 return StereoPoint2(uL_ + v[0], uR_ + v[1], v_ + v[2]);
│ │ │ │ +
86 }
│ │ │ │
│ │ │ │ -
83
│ │ │ │
87
│ │ │ │
│ │ │ │ -
89 void print(const std::string& s = "") const {
│ │ │ │ -
90 leftCamPose_.print(s + ".camera.");
│ │ │ │ -
91 K_->print(s + ".calibration.");
│ │ │ │ -
92 }
│ │ │ │ -
│ │ │ │ -
93
│ │ │ │ -
│ │ │ │ -
95 bool equals(const StereoCamera &camera, double tol = 1e-9) const {
│ │ │ │ -
96 return leftCamPose_.equals(camera.leftCamPose_, tol)
│ │ │ │ -
97 && K_->equals(*camera.K_, tol);
│ │ │ │ -
98 }
│ │ │ │ -
│ │ │ │ -
99
│ │ │ │ -
103
│ │ │ │ -
│ │ │ │ -
105 inline size_t dim() const {
│ │ │ │ -
106 return 6;
│ │ │ │ -
107 }
│ │ │ │ -
│ │ │ │ -
108
│ │ │ │ -
│ │ │ │ -
110 static inline size_t Dim() {
│ │ │ │ -
111 return 6;
│ │ │ │ -
112 }
│ │ │ │ -
│ │ │ │ +
89 inline StereoPoint2 operator +(const StereoPoint2& b) const {
│ │ │ │ +
90 return StereoPoint2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_);
│ │ │ │ +
91 }
│ │ │ │ +
│ │ │ │ +
92
│ │ │ │ +
│ │ │ │ +
94 inline StereoPoint2 operator -(const StereoPoint2& b) const {
│ │ │ │ +
95 return StereoPoint2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_);
│ │ │ │ +
96 }
│ │ │ │ +
│ │ │ │ +
97
│ │ │ │ +
101
│ │ │ │ +
103 inline bool operator ==(const StereoPoint2& q) const {return uL_== q.uL_ && uR_==q.uR_ && v_ == q.v_;}
│ │ │ │ +
104
│ │ │ │ +
106 inline double uL() const {return uL_;}
│ │ │ │ +
107
│ │ │ │ +
109 inline double uR() const {return uR_;}
│ │ │ │ +
110
│ │ │ │ +
112 inline double v() const {return v_;}
│ │ │ │
113
│ │ │ │
│ │ │ │ -
115 inline StereoCamera retract(const Vector& v) const {
│ │ │ │ -
116 return StereoCamera(pose().retract(v), K_);
│ │ │ │ +
115 Vector3 vector() const {
│ │ │ │ +
116 return Vector3(uL_, uR_, v_);
│ │ │ │
117 }
│ │ │ │
│ │ │ │
118
│ │ │ │
│ │ │ │ -
120 inline Vector6 localCoordinates(const StereoCamera& t2) const {
│ │ │ │ -
121 return leftCamPose_.localCoordinates(t2.leftCamPose_);
│ │ │ │ +
120 Point2 point2() const {
│ │ │ │ +
121 return Point2(uL_, v_);
│ │ │ │
122 }
│ │ │ │
│ │ │ │
123
│ │ │ │ -
127
│ │ │ │ -
│ │ │ │ -
129 const Pose3& pose() const {
│ │ │ │ -
130 return leftCamPose_;
│ │ │ │ -
131 }
│ │ │ │ -
│ │ │ │ -
132
│ │ │ │ -
│ │ │ │ -
134 double baseline() const {
│ │ │ │ -
135 return K_->baseline();
│ │ │ │ -
136 }
│ │ │ │ -
│ │ │ │ -
137
│ │ │ │ -
139 StereoPoint2 project(const Point3& point) const;
│ │ │ │ -
140
│ │ │ │ -
145 StereoPoint2 project2(const Point3& point, OptionalJacobian<3, 6> H1 =
│ │ │ │ -
146 boost::none, OptionalJacobian<3, 3> H2 = boost::none) const;
│ │ │ │ -
147
│ │ │ │ -
149 Point3 backproject(const StereoPoint2& z) const;
│ │ │ │ -
150
│ │ │ │ -
155 Point3 backproject2(const StereoPoint2& z,
│ │ │ │ -
156 OptionalJacobian<3, 6> H1 = boost::none,
│ │ │ │ -
157 OptionalJacobian<3, 3> H2 = boost::none) const;
│ │ │ │ -
158
│ │ │ │ -
162
│ │ │ │ -
169 StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1,
│ │ │ │ -
170 OptionalJacobian<3, 3> H2 = boost::none, OptionalJacobian<3, 0> H3 =
│ │ │ │ -
171 boost::none) const;
│ │ │ │ -
172
│ │ │ │ -
│ │ │ │ -
174 Vector defaultErrorWhenTriangulatingBehindCamera() const {
│ │ │ │ -
175 return Eigen::Matrix<double,traits<Measurement>::dimension,1>::Constant(2.0 * K_->fx());;
│ │ │ │ -
176 }
│ │ │ │ -
│ │ │ │ -
177
│ │ │ │ -
179
│ │ │ │ -
180private:
│ │ │ │ -
181
│ │ │ │ -
182 friend class boost::serialization::access;
│ │ │ │ -
183 template<class Archive>
│ │ │ │ -
184 void serialize(Archive & ar, const unsigned int /*version*/) {
│ │ │ │ -
185 ar & BOOST_SERIALIZATION_NVP(leftCamPose_);
│ │ │ │ -
186 ar & BOOST_SERIALIZATION_NVP(K_);
│ │ │ │ -
187 }
│ │ │ │ -
188
│ │ │ │ -
189};
│ │ │ │ -
│ │ │ │ -
190
│ │ │ │ -
191template<>
│ │ │ │ -
│ │ │ │ -
192struct traits<StereoCamera> : public internal::Manifold<StereoCamera> {
│ │ │ │ -
193};
│ │ │ │ -
│ │ │ │ -
194
│ │ │ │ -
195template<>
│ │ │ │ -
│ │ │ │ -
196struct traits<const StereoCamera> : public internal::Manifold<StereoCamera> {
│ │ │ │ -
197};
│ │ │ │ -
│ │ │ │ -
198}
│ │ │ │ -
Cal3_S2Stereo.h
The most common 5DOF 3D->2D calibration + Stereo baseline.
│ │ │ │ -
Pose3.h
3D Pose
│ │ │ │ -
StereoPoint2.h
A 2D stereo point (uL,uR,v)
│ │ │ │ +
│ │ │ │ +
125 Point2 right() const {
│ │ │ │ +
126 return Point2(uR_, v_);
│ │ │ │ +
127 }
│ │ │ │ +
│ │ │ │ +
128
│ │ │ │ +
131 inline StereoPoint2 inverse() const { return StereoPoint2()- (*this);}
│ │ │ │ +
132 inline StereoPoint2 compose(const StereoPoint2& p1) const { return *this + p1;}
│ │ │ │ +
133 inline StereoPoint2 between(const StereoPoint2& p2) const { return p2 - *this; }
│ │ │ │ +
134 inline Vector localCoordinates(const StereoPoint2& t2) const { return Logmap(between(t2)); }
│ │ │ │ +
135 inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap(v)); }
│ │ │ │ +
136 static inline Vector Logmap(const StereoPoint2& p) { return p.vector(); }
│ │ │ │ +
137 static inline StereoPoint2 Expmap(const Vector& d) { return StereoPoint2(d(0), d(1), d(2)); }
│ │ │ │ +
139
│ │ │ │ +
141 GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const StereoPoint2& p);
│ │ │ │ +
142
│ │ │ │ +
143private:
│ │ │ │ +
144
│ │ │ │ +
148
│ │ │ │ +
150 friend class boost::serialization::access;
│ │ │ │ +
151 template<class ARCHIVE>
│ │ │ │ +
152 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
153 ar & BOOST_SERIALIZATION_NVP(uL_);
│ │ │ │ +
154 ar & BOOST_SERIALIZATION_NVP(uR_);
│ │ │ │ +
155 ar & BOOST_SERIALIZATION_NVP(v_);
│ │ │ │ +
156 }
│ │ │ │ +
157
│ │ │ │ +
159
│ │ │ │ +
160};
│ │ │ │ +
│ │ │ │ +
161
│ │ │ │ +
162typedef std::vector<StereoPoint2> StereoPoint2Vector;
│ │ │ │ +
163
│ │ │ │ +
164template<>
│ │ │ │ +
165struct traits<StereoPoint2> : public internal::VectorSpace<StereoPoint2> {};
│ │ │ │ +
166
│ │ │ │ +
167template<>
│ │ │ │ +
168struct traits<const StereoPoint2> : public internal::VectorSpace<StereoPoint2> {};
│ │ │ │ +
169}
│ │ │ │ +
gtsam::testing::between
T between(const T &t1, const T &t2)
binary functions
Definition lieProxies.h:36
│ │ │ │ +
Point2.h
2D Point
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ -
gtsam::project
Point2_ project(const Point3_ &p_cam)
Expression version of PinholeBase::Project.
Definition expressions.h:131
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::LieGroup::localCoordinates
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g
Definition Lie.h:136
│ │ │ │ -
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
│ │ │ │ -
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
gtsam::Cal3_S2Stereo
The most common 5DOF 3D->2D calibration, stereo version.
Definition Cal3_S2Stereo.h:30
│ │ │ │ -
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ -
gtsam::Pose3::equals
bool equals(const Pose3 &pose, double tol=1e-9) const
assert equality up to a tolerance
Definition Pose3.cpp:157
│ │ │ │ -
gtsam::Pose3::print
void print(const std::string &s="") const
print with optional string
Definition Pose3.cpp:152
│ │ │ │ -
gtsam::StereoCheiralityException
Definition StereoCamera.h:26
│ │ │ │ -
gtsam::StereoCamera
A stereo camera class, parameterize by left camera pose and stereo calibration.
Definition StereoCamera.h:47
│ │ │ │ -
gtsam::StereoCamera::StereoCamera
StereoCamera()
Default constructor allocates a calibration!
Definition StereoCamera.h:72
│ │ │ │ -
gtsam::StereoCamera::Dim
static size_t Dim()
Dimensionality of the tangent space.
Definition StereoCamera.h:110
│ │ │ │ -
gtsam::StereoCamera::print
void print(const std::string &s="") const
print
Definition StereoCamera.h:89
│ │ │ │ -
gtsam::StereoCamera::localCoordinates
Vector6 localCoordinates(const StereoCamera &t2) const
Local coordinates of manifold neighborhood around current value.
Definition StereoCamera.h:120
│ │ │ │ -
gtsam::StereoCamera::equals
bool equals(const StereoCamera &camera, double tol=1e-9) const
equals
Definition StereoCamera.h:95
│ │ │ │ -
gtsam::StereoCamera::dim
size_t dim() const
Dimensionality of the tangent space.
Definition StereoCamera.h:105
│ │ │ │ -
gtsam::StereoCamera::retract
StereoCamera retract(const Vector &v) const
Updates a with tangent space delta.
Definition StereoCamera.h:115
│ │ │ │ -
gtsam::StereoCamera::Measurement
StereoPoint2 Measurement
Some classes template on either PinholeCamera or StereoCamera, and this typedef informs those classes...
Definition StereoCamera.h:55
│ │ │ │ -
gtsam::StereoCamera::calibration
const Cal3_S2Stereo & calibration() const
Return shared pointer to calibration.
Definition StereoCamera.h:80
│ │ │ │ -
gtsam::StereoCamera::baseline
double baseline() const
baseline
Definition StereoCamera.h:134
│ │ │ │ -
gtsam::StereoCamera::pose
const Pose3 & pose() const
pose
Definition StereoCamera.h:129
│ │ │ │ -
gtsam::StereoCamera::defaultErrorWhenTriangulatingBehindCamera
Vector defaultErrorWhenTriangulatingBehindCamera() const
for Nonlinear Triangulation
Definition StereoCamera.h:174
│ │ │ │ +
gtsam::internal::VectorSpace
VectorSpace provides both Testable and VectorSpaceTraits.
Definition VectorSpace.h:207
│ │ │ │
gtsam::StereoPoint2
A 2D stereo point, v will be same for rectified images.
Definition StereoPoint2.h:32
│ │ │ │ +
gtsam::StereoPoint2::uR
double uR() const
get uR
Definition StereoPoint2.h:109
│ │ │ │ +
gtsam::StereoPoint2::StereoPoint2
StereoPoint2(const Vector3 &v)
construct from 3D vector
Definition StereoPoint2.h:53
│ │ │ │ +
gtsam::StereoPoint2::right
Point2 right() const
convenient function to get a Point2 from the right image
Definition StereoPoint2.h:125
│ │ │ │ +
gtsam::StereoPoint2::vector
Vector3 vector() const
convert to vector
Definition StereoPoint2.h:115
│ │ │ │ +
gtsam::StereoPoint2::uL
double uL() const
get uL
Definition StereoPoint2.h:106
│ │ │ │ +
gtsam::StereoPoint2::StereoPoint2
StereoPoint2(double uL, double uR, double v)
constructor
Definition StereoPoint2.h:48
│ │ │ │ +
gtsam::StereoPoint2::equals
bool equals(const StereoPoint2 &q, double tol=1e-9) const
equals
Definition StereoPoint2.h:64
│ │ │ │ +
gtsam::StereoPoint2::operator-
StereoPoint2 operator-() const
inverse
Definition StereoPoint2.h:79
│ │ │ │ +
gtsam::StereoPoint2::v
double v() const
get v
Definition StereoPoint2.h:112
│ │ │ │ +
gtsam::StereoPoint2::point2
Point2 point2() const
convenient function to get a Point2 from the left image
Definition StereoPoint2.h:120
│ │ │ │ +
gtsam::StereoPoint2::Identity
static StereoPoint2 Identity()
identity
Definition StereoPoint2.h:74
│ │ │ │ +
gtsam::StereoPoint2::StereoPoint2
StereoPoint2()
default constructor
Definition StereoPoint2.h:43
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,265 +1,217 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -StereoCamera.h │ │ │ │ │ +StereoPoint2.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3___S_2_S_t_e_r_e_o_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_3_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_S_t_e_r_e_o_P_o_i_n_t_2_._h> │ │ │ │ │ -23 │ │ │ │ │ -24namespace _g_t_s_a_m { │ │ │ │ │ -25 │ │ │ │ │ -_2_6class GTSAM_EXPORT _S_t_e_r_e_o_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n: public std::runtime_error { │ │ │ │ │ -27public: │ │ │ │ │ -28 _S_t_e_r_e_o_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n() │ │ │ │ │ -29 : _S_t_e_r_e_o_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n(std::numeric_limits::max()) {} │ │ │ │ │ -30 │ │ │ │ │ -31 _S_t_e_r_e_o_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n(_K_e_y j) │ │ │ │ │ -32 : std::runtime_error("Stereo Cheirality Exception"), │ │ │ │ │ -33 j_(j) {} │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ +22#include │ │ │ │ │ +23#include │ │ │ │ │ +24 │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ +26 │ │ │ │ │ +_3_2class GTSAM_EXPORT _S_t_e_r_e_o_P_o_i_n_t_2 { │ │ │ │ │ +33private: │ │ │ │ │ 34 │ │ │ │ │ -35 _K_e_y nearbyVariable() const { │ │ │ │ │ -36 return j_; │ │ │ │ │ -37 } │ │ │ │ │ -38 │ │ │ │ │ -39private: │ │ │ │ │ -40 _K_e_y j_; │ │ │ │ │ -41}; │ │ │ │ │ -42 │ │ │ │ │ -_4_7class GTSAM_EXPORT _S_t_e_r_e_o_C_a_m_e_r_a { │ │ │ │ │ -48 │ │ │ │ │ -49public: │ │ │ │ │ -50 │ │ │ │ │ -_5_5 typedef _S_t_e_r_e_o_P_o_i_n_t_2 _M_e_a_s_u_r_e_m_e_n_t; │ │ │ │ │ -56 typedef StereoPoint2Vector MeasurementVector; │ │ │ │ │ -57 │ │ │ │ │ -58private: │ │ │ │ │ -59 _P_o_s_e_3 leftCamPose_; │ │ │ │ │ -60 Cal3_S2Stereo::shared_ptr K_; │ │ │ │ │ -61 │ │ │ │ │ -62public: │ │ │ │ │ -63 │ │ │ │ │ -64 enum { │ │ │ │ │ -65 dimension = 6 │ │ │ │ │ -66 }; │ │ │ │ │ -67 │ │ │ │ │ -70 │ │ │ │ │ -_7_2 _S_t_e_r_e_o_C_a_m_e_r_a() : │ │ │ │ │ -73 K_(new _C_a_l_3___S_2_S_t_e_r_e_o()) { │ │ │ │ │ -74 } │ │ │ │ │ -75 │ │ │ │ │ -77 _S_t_e_r_e_o_C_a_m_e_r_a(const _P_o_s_e_3& leftCamPose, const Cal3_S2Stereo::shared_ptr K); │ │ │ │ │ -78 │ │ │ │ │ -_8_0 const _C_a_l_3___S_2_S_t_e_r_e_o& _c_a_l_i_b_r_a_t_i_o_n() const { │ │ │ │ │ -81 return *K_; │ │ │ │ │ -82 } │ │ │ │ │ -83 │ │ │ │ │ +35 double uL_, uR_, v_; │ │ │ │ │ +36 │ │ │ │ │ +37public: │ │ │ │ │ +38 enum { dimension = 3 }; │ │ │ │ │ +41 │ │ │ │ │ +_4_3 _S_t_e_r_e_o_P_o_i_n_t_2() : │ │ │ │ │ +44 uL_(0), uR_(0), v_(0) { │ │ │ │ │ +45 } │ │ │ │ │ +46 │ │ │ │ │ +_4_8 _S_t_e_r_e_o_P_o_i_n_t_2(double uL, double uR, double v) : │ │ │ │ │ +49 uL_(uL), uR_(uR), v_(v) { │ │ │ │ │ +50 } │ │ │ │ │ +51 │ │ │ │ │ +_5_3 explicit _S_t_e_r_e_o_P_o_i_n_t_2(const Vector3& v) : │ │ │ │ │ +54 uL_(v(0)), uR_(v(1)), v_(v(2)) {} │ │ │ │ │ +55 │ │ │ │ │ +59 │ │ │ │ │ +61 void _p_r_i_n_t(const std::string& s = "") const; │ │ │ │ │ +62 │ │ │ │ │ +_6_4 bool _e_q_u_a_l_s(const _S_t_e_r_e_o_P_o_i_n_t_2& q, double tol = 1e-9) const { │ │ │ │ │ +65 return (std::abs(uL_ - q.uL_) < tol && std::abs(uR_ - q.uR_) < tol │ │ │ │ │ +66 && std::abs(v_ - q.v_) < tol); │ │ │ │ │ +67 } │ │ │ │ │ +68 │ │ │ │ │ +72 │ │ │ │ │ +_7_4 inline static _S_t_e_r_e_o_P_o_i_n_t_2 _I_d_e_n_t_i_t_y() { │ │ │ │ │ +75 return _S_t_e_r_e_o_P_o_i_n_t_2(); │ │ │ │ │ +76 } │ │ │ │ │ +77 │ │ │ │ │ +_7_9 _S_t_e_r_e_o_P_o_i_n_t_2 _o_p_e_r_a_t_o_r_-() const { │ │ │ │ │ +80 return _S_t_e_r_e_o_P_o_i_n_t_2(-uL_, -uR_, -v_); │ │ │ │ │ +81 } │ │ │ │ │ +82 │ │ │ │ │ +_8_4 inline _S_t_e_r_e_o_P_o_i_n_t_2 operator +(const Vector3& v) const { │ │ │ │ │ +85 return _S_t_e_r_e_o_P_o_i_n_t_2(uL_ + v[0], uR_ + v[1], v_ + v[2]); │ │ │ │ │ +86 } │ │ │ │ │ 87 │ │ │ │ │ -_8_9 void _p_r_i_n_t(const std::string& s = "") const { │ │ │ │ │ -90 leftCamPose_._p_r_i_n_t(s + ".camera."); │ │ │ │ │ -91 K_->print(s + ".calibration."); │ │ │ │ │ -92 } │ │ │ │ │ -93 │ │ │ │ │ -_9_5 bool _e_q_u_a_l_s(const _S_t_e_r_e_o_C_a_m_e_r_a &camera, double tol = 1e-9) const { │ │ │ │ │ -96 return leftCamPose_._e_q_u_a_l_s(camera.leftCamPose_, tol) │ │ │ │ │ -97 && K_->equals(*camera.K_, tol); │ │ │ │ │ -98 } │ │ │ │ │ -99 │ │ │ │ │ -103 │ │ │ │ │ -_1_0_5 inline size_t _d_i_m() const { │ │ │ │ │ -106 return 6; │ │ │ │ │ -107 } │ │ │ │ │ -108 │ │ │ │ │ -_1_1_0 static inline size_t _D_i_m() { │ │ │ │ │ -111 return 6; │ │ │ │ │ -112 } │ │ │ │ │ +_8_9 inline _S_t_e_r_e_o_P_o_i_n_t_2 operator +(const _S_t_e_r_e_o_P_o_i_n_t_2& b) const { │ │ │ │ │ +90 return _S_t_e_r_e_o_P_o_i_n_t_2(uL_ + b.uL_, uR_ + b.uR_, v_ + b.v_); │ │ │ │ │ +91 } │ │ │ │ │ +92 │ │ │ │ │ +_9_4 inline _S_t_e_r_e_o_P_o_i_n_t_2 operator -(const _S_t_e_r_e_o_P_o_i_n_t_2& b) const { │ │ │ │ │ +95 return _S_t_e_r_e_o_P_o_i_n_t_2(uL_ - b.uL_, uR_ - b.uR_, v_ - b.v_); │ │ │ │ │ +96 } │ │ │ │ │ +97 │ │ │ │ │ +101 │ │ │ │ │ +_1_0_3 inline bool operator ==(const _S_t_e_r_e_o_P_o_i_n_t_2& q) const {return uL_== q.uL_ && │ │ │ │ │ +uR_==q.uR_ && v_ == q.v_;} │ │ │ │ │ +104 │ │ │ │ │ +_1_0_6 inline double _u_L() const {return uL_;} │ │ │ │ │ +107 │ │ │ │ │ +_1_0_9 inline double _u_R() const {return uR_;} │ │ │ │ │ +110 │ │ │ │ │ +_1_1_2 inline double _v() const {return v_;} │ │ │ │ │ 113 │ │ │ │ │ -_1_1_5 inline _S_t_e_r_e_o_C_a_m_e_r_a _r_e_t_r_a_c_t(const Vector& v) const { │ │ │ │ │ -116 return _S_t_e_r_e_o_C_a_m_e_r_a(pose().retract(v), K_); │ │ │ │ │ +_1_1_5 Vector3 _v_e_c_t_o_r() const { │ │ │ │ │ +116 return Vector3(uL_, uR_, v_); │ │ │ │ │ 117 } │ │ │ │ │ 118 │ │ │ │ │ -_1_2_0 inline Vector6 _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(const _S_t_e_r_e_o_C_a_m_e_r_a& t2) const { │ │ │ │ │ -121 return leftCamPose_._l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(t2.leftCamPose_); │ │ │ │ │ +_1_2_0 _P_o_i_n_t_2 _p_o_i_n_t_2() const { │ │ │ │ │ +121 return _P_o_i_n_t_2(uL_, v_); │ │ │ │ │ 122 } │ │ │ │ │ 123 │ │ │ │ │ -127 │ │ │ │ │ -_1_2_9 const _P_o_s_e_3& _p_o_s_e() const { │ │ │ │ │ -130 return leftCamPose_; │ │ │ │ │ -131 } │ │ │ │ │ -132 │ │ │ │ │ -_1_3_4 double _b_a_s_e_l_i_n_e() const { │ │ │ │ │ -135 return K_->baseline(); │ │ │ │ │ -136 } │ │ │ │ │ -137 │ │ │ │ │ -139 _S_t_e_r_e_o_P_o_i_n_t_2 _p_r_o_j_e_c_t(const _P_o_i_n_t_3& point) const; │ │ │ │ │ -140 │ │ │ │ │ -145 _S_t_e_r_e_o_P_o_i_n_t_2 project2(const _P_o_i_n_t_3& point, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _6_> H1 = │ │ │ │ │ -146 boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H2 = boost::none) const; │ │ │ │ │ -147 │ │ │ │ │ -149 _P_o_i_n_t_3 backproject(const _S_t_e_r_e_o_P_o_i_n_t_2& z) const; │ │ │ │ │ -150 │ │ │ │ │ -155 _P_o_i_n_t_3 backproject2(const _S_t_e_r_e_o_P_o_i_n_t_2& z, │ │ │ │ │ -156 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _6_> H1 = boost::none, │ │ │ │ │ -157 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H2 = boost::none) const; │ │ │ │ │ -158 │ │ │ │ │ -162 │ │ │ │ │ -169 _S_t_e_r_e_o_P_o_i_n_t_2 _p_r_o_j_e_c_t(const _P_o_i_n_t_3& point, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _6_> H1, │ │ │ │ │ -170 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H2 = boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _0_> H3 = │ │ │ │ │ -171 boost::none) const; │ │ │ │ │ -172 │ │ │ │ │ -_1_7_4 Vector _d_e_f_a_u_l_t_E_r_r_o_r_W_h_e_n_T_r_i_a_n_g_u_l_a_t_i_n_g_B_e_h_i_n_d_C_a_m_e_r_a() const { │ │ │ │ │ -175 return Eigen::Matrix::dimension,1>::Constant(2.0 │ │ │ │ │ -* K_->fx());; │ │ │ │ │ -176 } │ │ │ │ │ -177 │ │ │ │ │ -179 │ │ │ │ │ -180private: │ │ │ │ │ -181 │ │ │ │ │ -182 friend class boost::serialization::access; │ │ │ │ │ -183 template │ │ │ │ │ -184 void serialize(Archive & ar, const unsigned int /*version*/) { │ │ │ │ │ -185 ar & BOOST_SERIALIZATION_NVP(leftCamPose_); │ │ │ │ │ -186 ar & BOOST_SERIALIZATION_NVP(K_); │ │ │ │ │ -187 } │ │ │ │ │ -188 │ │ │ │ │ -189}; │ │ │ │ │ -190 │ │ │ │ │ -191template<> │ │ │ │ │ -_1_9_2struct _t_r_a_i_t_s<_S_t_e_r_e_o_C_a_m_e_r_a> : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d { │ │ │ │ │ -193}; │ │ │ │ │ -194 │ │ │ │ │ -195template<> │ │ │ │ │ -_1_9_6struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ -{ │ │ │ │ │ -197}; │ │ │ │ │ -198} │ │ │ │ │ -_C_a_l_3___S_2_S_t_e_r_e_o_._h │ │ │ │ │ -The most common 5DOF 3D->2D calibration + Stereo baseline. │ │ │ │ │ -_P_o_s_e_3_._h │ │ │ │ │ -3D Pose │ │ │ │ │ -_S_t_e_r_e_o_P_o_i_n_t_2_._h │ │ │ │ │ -A 2D stereo point (uL,uR,v) │ │ │ │ │ +_1_2_5 _P_o_i_n_t_2 _r_i_g_h_t() const { │ │ │ │ │ +126 return _P_o_i_n_t_2(uR_, v_); │ │ │ │ │ +127 } │ │ │ │ │ +128 │ │ │ │ │ +131 inline _S_t_e_r_e_o_P_o_i_n_t_2 inverse() const { return _S_t_e_r_e_o_P_o_i_n_t_2()- (*this);} │ │ │ │ │ +132 inline StereoPoint2 compose(const StereoPoint2& p1) const { return *this + │ │ │ │ │ +p1;} │ │ │ │ │ +133 inline StereoPoint2 _b_e_t_w_e_e_n(const StereoPoint2& p2) const { return p2 - │ │ │ │ │ +*this; } │ │ │ │ │ +134 inline Vector localCoordinates(const StereoPoint2& t2) const { return │ │ │ │ │ +Logmap(_b_e_t_w_e_e_n(t2)); } │ │ │ │ │ +135 inline StereoPoint2 retract(const Vector& v) const { return compose(Expmap │ │ │ │ │ +(v)); } │ │ │ │ │ +136 static inline Vector Logmap(const StereoPoint2& p) { return p.vector(); } │ │ │ │ │ +137 static inline StereoPoint2 Expmap(const Vector& d) { return StereoPoint2(d │ │ │ │ │ +(0), d(1), d(2)); } │ │ │ │ │ +139 │ │ │ │ │ +141 GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const │ │ │ │ │ +StereoPoint2& p); │ │ │ │ │ +142 │ │ │ │ │ +143private: │ │ │ │ │ +144 │ │ │ │ │ +148 │ │ │ │ │ +_1_5_0 friend class boost::serialization::access; │ │ │ │ │ +151 template │ │ │ │ │ +152 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +153 ar & BOOST_SERIALIZATION_NVP(uL_); │ │ │ │ │ +154 ar & BOOST_SERIALIZATION_NVP(uR_); │ │ │ │ │ +155 ar & BOOST_SERIALIZATION_NVP(v_); │ │ │ │ │ +156 } │ │ │ │ │ +157 │ │ │ │ │ +159 │ │ │ │ │ +160}; │ │ │ │ │ +161 │ │ │ │ │ +162typedef std::vector StereoPoint2Vector; │ │ │ │ │ +163 │ │ │ │ │ +164template<> │ │ │ │ │ +_1_6_5struct _t_r_a_i_t_s<_S_t_e_r_e_o_P_o_i_n_t_2> : public _i_n_t_e_r_n_a_l_:_:_V_e_c_t_o_r_S_p_a_c_e {}; │ │ │ │ │ +166 │ │ │ │ │ +167template<> │ │ │ │ │ +_1_6_8struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_: │ │ │ │ │ +_V_e_c_t_o_r_S_p_a_c_e {}; │ │ │ │ │ +169} │ │ │ │ │ +_g_t_s_a_m_:_:_t_e_s_t_i_n_g_:_:_b_e_t_w_e_e_n │ │ │ │ │ +T between(const T &t1, const T &t2) │ │ │ │ │ +binary functions │ │ │ │ │ +DDeeffiinniittiioonn lieProxies.h:36 │ │ │ │ │ +_P_o_i_n_t_2_._h │ │ │ │ │ +2D Point │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ -Vector3 Point3 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ +Vector2 Point2 │ │ │ │ │ As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point3 to Vector3... │ │ │ │ │ -DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_o_j_e_c_t │ │ │ │ │ -Point2_ project(const Point3_ &p_cam) │ │ │ │ │ -Expression version of PinholeBase::Project. │ │ │ │ │ -DDeeffiinniittiioonn expressions.h:131 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +typedef Point2 to Vector2... │ │ │ │ │ +DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ -TangentVector localCoordinates(const Class &g) const │ │ │ │ │ -localCoordinates as required by manifold concept: finds tangent vector between │ │ │ │ │ -*this and g │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:136 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ -Both ManifoldTraits and Testable. │ │ │ │ │ -DDeeffiinniittiioonn Manifold.h:120 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ -either a fixed size o... │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o │ │ │ │ │ -The most common 5DOF 3D->2D calibration, stereo version. │ │ │ │ │ -DDeeffiinniittiioonn Cal3_S2Stereo.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ -A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const Pose3 &pose, double tol=1e-9) const │ │ │ │ │ -assert equality up to a tolerance │ │ │ │ │ -DDeeffiinniittiioonn Pose3.cpp:157 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="") const │ │ │ │ │ -print with optional string │ │ │ │ │ -DDeeffiinniittiioonn Pose3.cpp:152 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n │ │ │ │ │ -DDeeffiinniittiioonn StereoCamera.h:26 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a │ │ │ │ │ -A stereo camera class, parameterize by left camera pose and stereo calibration. │ │ │ │ │ -DDeeffiinniittiioonn StereoCamera.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a_:_:_S_t_e_r_e_o_C_a_m_e_r_a │ │ │ │ │ -StereoCamera() │ │ │ │ │ -Default constructor allocates a calibration! │ │ │ │ │ -DDeeffiinniittiioonn StereoCamera.h:72 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a_:_:_D_i_m │ │ │ │ │ -static size_t Dim() │ │ │ │ │ -Dimensionality of the tangent space. │ │ │ │ │ -DDeeffiinniittiioonn StereoCamera.h:110 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="") const │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn StereoCamera.h:89 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ -Vector6 localCoordinates(const StereoCamera &t2) const │ │ │ │ │ -Local coordinates of manifold neighborhood around current value. │ │ │ │ │ -DDeeffiinniittiioonn StereoCamera.h:120 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const StereoCamera &camera, double tol=1e-9) const │ │ │ │ │ -equals │ │ │ │ │ -DDeeffiinniittiioonn StereoCamera.h:95 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a_:_:_d_i_m │ │ │ │ │ -size_t dim() const │ │ │ │ │ -Dimensionality of the tangent space. │ │ │ │ │ -DDeeffiinniittiioonn StereoCamera.h:105 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a_:_:_r_e_t_r_a_c_t │ │ │ │ │ -StereoCamera retract(const Vector &v) const │ │ │ │ │ -Updates a with tangent space delta. │ │ │ │ │ -DDeeffiinniittiioonn StereoCamera.h:115 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a_:_:_M_e_a_s_u_r_e_m_e_n_t │ │ │ │ │ -StereoPoint2 Measurement │ │ │ │ │ -Some classes template on either PinholeCamera or StereoCamera, and this typedef │ │ │ │ │ -informs those classes... │ │ │ │ │ -DDeeffiinniittiioonn StereoCamera.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a_:_:_c_a_l_i_b_r_a_t_i_o_n │ │ │ │ │ -const Cal3_S2Stereo & calibration() const │ │ │ │ │ -Return shared pointer to calibration. │ │ │ │ │ -DDeeffiinniittiioonn StereoCamera.h:80 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a_:_:_b_a_s_e_l_i_n_e │ │ │ │ │ -double baseline() const │ │ │ │ │ -baseline │ │ │ │ │ -DDeeffiinniittiioonn StereoCamera.h:134 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a_:_:_p_o_s_e │ │ │ │ │ -const Pose3 & pose() const │ │ │ │ │ -pose │ │ │ │ │ -DDeeffiinniittiioonn StereoCamera.h:129 │ │ │ │ │ -_g_t_s_a_m_:_:_S_t_e_r_e_o_C_a_m_e_r_a_:_:_d_e_f_a_u_l_t_E_r_r_o_r_W_h_e_n_T_r_i_a_n_g_u_l_a_t_i_n_g_B_e_h_i_n_d_C_a_m_e_r_a │ │ │ │ │ -Vector defaultErrorWhenTriangulatingBehindCamera() const │ │ │ │ │ -for Nonlinear Triangulation │ │ │ │ │ -DDeeffiinniittiioonn StereoCamera.h:174 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_V_e_c_t_o_r_S_p_a_c_e │ │ │ │ │ +VectorSpace provides both Testable and VectorSpaceTraits. │ │ │ │ │ +DDeeffiinniittiioonn VectorSpace.h:207 │ │ │ │ │ _g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2 │ │ │ │ │ A 2D stereo point, v will be same for rectified images. │ │ │ │ │ DDeeffiinniittiioonn StereoPoint2.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_u_R │ │ │ │ │ +double uR() const │ │ │ │ │ +get uR │ │ │ │ │ +DDeeffiinniittiioonn StereoPoint2.h:109 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_S_t_e_r_e_o_P_o_i_n_t_2 │ │ │ │ │ +StereoPoint2(const Vector3 &v) │ │ │ │ │ +construct from 3D vector │ │ │ │ │ +DDeeffiinniittiioonn StereoPoint2.h:53 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_r_i_g_h_t │ │ │ │ │ +Point2 right() const │ │ │ │ │ +convenient function to get a Point2 from the right image │ │ │ │ │ +DDeeffiinniittiioonn StereoPoint2.h:125 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_v_e_c_t_o_r │ │ │ │ │ +Vector3 vector() const │ │ │ │ │ +convert to vector │ │ │ │ │ +DDeeffiinniittiioonn StereoPoint2.h:115 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_u_L │ │ │ │ │ +double uL() const │ │ │ │ │ +get uL │ │ │ │ │ +DDeeffiinniittiioonn StereoPoint2.h:106 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_S_t_e_r_e_o_P_o_i_n_t_2 │ │ │ │ │ +StereoPoint2(double uL, double uR, double v) │ │ │ │ │ +constructor │ │ │ │ │ +DDeeffiinniittiioonn StereoPoint2.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const StereoPoint2 &q, double tol=1e-9) const │ │ │ │ │ +equals │ │ │ │ │ +DDeeffiinniittiioonn StereoPoint2.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_o_p_e_r_a_t_o_r_- │ │ │ │ │ +StereoPoint2 operator-() const │ │ │ │ │ +inverse │ │ │ │ │ +DDeeffiinniittiioonn StereoPoint2.h:79 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_v │ │ │ │ │ +double v() const │ │ │ │ │ +get v │ │ │ │ │ +DDeeffiinniittiioonn StereoPoint2.h:112 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_p_o_i_n_t_2 │ │ │ │ │ +Point2 point2() const │ │ │ │ │ +convenient function to get a Point2 from the left image │ │ │ │ │ +DDeeffiinniittiioonn StereoPoint2.h:120 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_I_d_e_n_t_i_t_y │ │ │ │ │ +static StereoPoint2 Identity() │ │ │ │ │ +identity │ │ │ │ │ +DDeeffiinniittiioonn StereoPoint2.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_S_t_e_r_e_o_P_o_i_n_t_2_:_:_S_t_e_r_e_o_P_o_i_n_t_2 │ │ │ │ │ +StereoPoint2() │ │ │ │ │ +default constructor │ │ │ │ │ +DDeeffiinniittiioonn StereoPoint2.h:43 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_t_e_r_e_o_C_a_m_e_r_a_._h │ │ │ │ │ + * _S_t_e_r_e_o_P_o_i_n_t_2_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00383_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00290_source.html │ │ │ │┄ Files 99% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
PinholeSet.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
18#pragma once
│ │ │ │
19
│ │ │ │ -
20#include <gtsam/geometry/CameraSet.h>
│ │ │ │ -
21#include <gtsam/geometry/triangulation.h>
│ │ │ │ +
20#include <gtsam/geometry/CameraSet.h>
│ │ │ │ +
21#include <gtsam/geometry/triangulation.h>
│ │ │ │
22#include <boost/optional.hpp>
│ │ │ │
23
│ │ │ │
24namespace gtsam {
│ │ │ │
25
│ │ │ │
29template<class CAMERA>
│ │ │ │
│ │ │ │
30class PinholeSet: public CameraSet<CAMERA> {
│ │ │ │ @@ -182,16 +182,16 @@ │ │ │ │
80template<class CAMERA>
│ │ │ │
│ │ │ │
81struct traits<const PinholeSet<CAMERA> > : public Testable<PinholeSet<CAMERA> > {
│ │ │ │
82};
│ │ │ │
│ │ │ │
83
│ │ │ │
84} // \ namespace gtsam
│ │ │ │ -
CameraSet.h
Base class to create smart factors on poses or cameras.
│ │ │ │ -
triangulation.h
Functions for triangulation.
│ │ │ │ +
triangulation.h
Functions for triangulation.
│ │ │ │ +
CameraSet.h
Base class to create smart factors on poses or cameras.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::triangulateSafe
TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters &params)
triangulateSafe: extensive checking of the outcome
Definition triangulation.h:680
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition CameraSet.h:36
│ │ │ │
gtsam::PinholeSet
PinholeSet: triangulates point and keeps an estimate of it around.
Definition PinholeSet.h:30
│ │ │ │
gtsam::PinholeSet::triangulateSafe
TriangulationResult triangulateSafe(const typename CAMERA::MeasurementVector &measured, const TriangulationParameters &params) const
triangulateSafe
Definition PinholeSet.h:60
│ │ │ │ @@ -202,13 +202,13 @@ │ │ │ │
gtsam::TriangulationParameters
Definition triangulation.h:556
│ │ │ │
gtsam::TriangulationResult
TriangulationResult is an optional point, along with the reasons why it is invalid.
Definition triangulation.h:626
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -71,18 +71,18 @@ │ │ │ │ │ 79 │ │ │ │ │ 80template │ │ │ │ │ _8_1struct _t_r_a_i_t_s > : public _T_e_s_t_a_b_l_e │ │ │ │ │ > { │ │ │ │ │ 82}; │ │ │ │ │ 83 │ │ │ │ │ 84} // \ namespace gtsam │ │ │ │ │ -_C_a_m_e_r_a_S_e_t_._h │ │ │ │ │ -Base class to create smart factors on poses or cameras. │ │ │ │ │ _t_r_i_a_n_g_u_l_a_t_i_o_n_._h │ │ │ │ │ Functions for triangulation. │ │ │ │ │ +_C_a_m_e_r_a_S_e_t_._h │ │ │ │ │ +Base class to create smart factors on poses or cameras. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_S_a_f_e │ │ │ │ │ TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const │ │ │ │ │ typename CAMERA::MeasurementVector &measured, const TriangulationParameters │ │ │ │ │ ¶ms) │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00389_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00470_source.html │ │ │ │┄ Files 99% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
16 * @author Frank Dellaert
│ │ │ │
17 * @author Alex Trevor
│ │ │ │
18 * @brief Develop a Unit3 class - basically a point on a unit sphere
│ │ │ │
19 */
│ │ │ │
20
│ │ │ │
21#pragma once
│ │ │ │
22
│ │ │ │ -
23#include <gtsam/geometry/Point2.h>
│ │ │ │ -
24#include <gtsam/geometry/Point3.h>
│ │ │ │ -
25#include <gtsam/base/Manifold.h>
│ │ │ │ -
26#include <gtsam/base/Vector.h>
│ │ │ │ -
27#include <gtsam/base/VectorSerialization.h>
│ │ │ │ -
28#include <gtsam/base/Matrix.h>
│ │ │ │ +
23#include <gtsam/geometry/Point2.h>
│ │ │ │ +
24#include <gtsam/geometry/Point3.h>
│ │ │ │ +
25#include <gtsam/base/Manifold.h>
│ │ │ │ +
26#include <gtsam/base/Vector.h>
│ │ │ │ +
27#include <gtsam/base/VectorSerialization.h>
│ │ │ │ +
28#include <gtsam/base/Matrix.h>
│ │ │ │
29#include <gtsam/dllexport.h>
│ │ │ │
30
│ │ │ │
31#include <boost/optional.hpp>
│ │ │ │
32
│ │ │ │
33#include <random>
│ │ │ │
34#include <string>
│ │ │ │
35
│ │ │ │ @@ -280,15 +280,15 @@ │ │ │ │
205 template<class ARCHIVE>
│ │ │ │
206 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │
207 ar & BOOST_SERIALIZATION_NVP(p_);
│ │ │ │
208 }
│ │ │ │
209
│ │ │ │
211
│ │ │ │
212public:
│ │ │ │ -
213 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
213 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │
214};
│ │ │ │
│ │ │ │
215
│ │ │ │
216// Define GTSAM traits
│ │ │ │
│ │ │ │
217template<> struct traits<Unit3> : public internal::Manifold<Unit3> {
│ │ │ │
218};
│ │ │ │ @@ -297,21 +297,21 @@ │ │ │ │
│ │ │ │
220template<> struct traits<const Unit3> : public internal::Manifold<Unit3> {
│ │ │ │
221};
│ │ │ │
│ │ │ │
222
│ │ │ │
223} // namespace gtsam
│ │ │ │
224
│ │ │ │ -
VectorSerialization.h
serialization for Vectors
│ │ │ │ -
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ -
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ -
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ -
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ -
Point3.h
3D Point
│ │ │ │ -
Point2.h
2D Point
│ │ │ │ +
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ +
VectorSerialization.h
serialization for Vectors
│ │ │ │ +
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ +
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ +
Point2.h
2D Point
│ │ │ │ +
Point3.h
3D Point
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
│ │ │ │
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition Matrix.h:81
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ ├── html2text {} │ │ │ │ │ @@ -176,30 +176,30 @@ │ │ │ │ │ 218}; │ │ │ │ │ 219 │ │ │ │ │ _2_2_0template<> struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d { │ │ │ │ │ 221}; │ │ │ │ │ 222 │ │ │ │ │ 223} // namespace gtsam │ │ │ │ │ 224 │ │ │ │ │ -_V_e_c_t_o_r_S_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ -serialization for Vectors │ │ │ │ │ _V_e_c_t_o_r_._h │ │ │ │ │ typedef and functions to augment Eigen's VectorXd │ │ │ │ │ +_V_e_c_t_o_r_S_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ +serialization for Vectors │ │ │ │ │ +_M_a_t_r_i_x_._h │ │ │ │ │ +typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ _M_a_n_i_f_o_l_d_._h │ │ │ │ │ Base class and basic functions for Manifold types. │ │ │ │ │ _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ #define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ This marks a GTSAM object to require alignment. │ │ │ │ │ DDeeffiinniittiioonn types.h:308 │ │ │ │ │ -_M_a_t_r_i_x_._h │ │ │ │ │ -typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ -_P_o_i_n_t_3_._h │ │ │ │ │ -3D Point │ │ │ │ │ _P_o_i_n_t_2_._h │ │ │ │ │ 2D Point │ │ │ │ │ +_P_o_i_n_t_3_._h │ │ │ │ │ +3D Point │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ print without optional string, must specify cout yourself │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00392.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01112.html │ │ │ │┄ Files 92% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3Unified.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
Cal3Unified.cpp File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
LevenbergMarquardtOptimizer.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ + │ │ │ │ +

A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme. │ │ │ │ +More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -std::ostream & gtsam::operator<< (std::ostream &os, const Cal3Unified &cal)
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Mar 8, 2014
│ │ │ │ -
Author
Jing Dong
│ │ │ │ +

A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │
│ │ │ │ -Varun Agrawal
│ │ │ │ +Luca Carlone
│ │ │ │ +
Date
Feb 26, 2012
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,24 +1,25 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Cal3Unified.cpp File Reference │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +LevenbergMarquardtOptimizer.cpp File Reference │ │ │ │ │ +A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme. │ │ │ │ │ +_M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _C_a_l_3_U_n_i_f_i_e_d &cal) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Mar 8, 2014 │ │ │ │ │ +A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme. │ │ │ │ │ Author │ │ │ │ │ - Jing Dong │ │ │ │ │ - Varun Agrawal │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Date │ │ │ │ │ + Feb 26, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_3_U_n_i_f_i_e_d_._c_p_p │ │ │ │ │ + * _n_o_n_l_i_n_e_a_r │ │ │ │ │ + * _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00395.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00890.html │ │ │ │┄ Files 87% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Point3.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Errors.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces | │ │ │ │ Functions
│ │ │ │ -
Point3.cpp File Reference
│ │ │ │ +
Errors.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

3D Point │ │ │ │ +

Factor Graph Values. │ │ │ │ More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

│ │ │ │ -double gtsam::distance3 (const Point3 &p1, const Point3 &q, OptionalJacobian< 1, 3 > H1=boost::none, OptionalJacobian< 1, 3 > H2=boost::none)
 distance between two points
 
│ │ │ │ -double gtsam::norm3 (const Point3 &p, OptionalJacobian< 1, 3 > H=boost::none)
 Distance of the point from the origin, with Jacobian.
 
│ │ │ │ -Point3 gtsam::normalize (const Point3 &p, OptionalJacobian< 3, 3 > H=boost::none)
 normalize, with optional Jacobian
 
Point3 gtsam::cross (const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H_p=boost::none, OptionalJacobian< 3, 3 > H_q=boost::none)
 cross product
 
│ │ │ │ -double gtsam::dot (const Point3 &p, const Point3 &q, OptionalJacobian< 1, 3 > H_p=boost::none, OptionalJacobian< 1, 3 > H_q=boost::none)
 dot product
 
│ │ │ │ -Point3Pair gtsam::means (const std::vector< Point3Pair > &abPointPairs)
 Calculate the two means of a set of Point3 pairs.
 
│ │ │ │ -ostream & gtsam::operator<< (ostream &os, const gtsam::Point3Pair &p)
 
│ │ │ │ +Errors gtsam::createErrors (const VectorValues &V)
 Break V into pieces according to its start indices.
 
│ │ │ │ +void gtsam::print (const Errors &e, const std::string &s="Errors")
 Print an Errors instance.
 
│ │ │ │ +bool gtsam::equality (const Errors &actual, const Errors &expected, double tol)
 
│ │ │ │ +Errors gtsam::operator+ (const Errors &a, const Errors &b)
 Addition.
 
│ │ │ │ +Errors gtsam::operator- (const Errors &a, const Errors &b)
 Subtraction.
 
│ │ │ │ +Errors gtsam::operator- (const Errors &a)
 Negation.
 
│ │ │ │ +double gtsam::dot (const Errors &a, const Errors &b)
 Dot product.
 
│ │ │ │ +void gtsam::axpy (double alpha, const Errors &x, Errors &y)
 BLAS level 2 style AXPY, y := alpha*x + y
 
│ │ │ │

Detailed Description

│ │ │ │ -

3D Point

│ │ │ │ +

Factor Graph Values.

│ │ │ │ +

Errors

Author
Carlos Nieto
│ │ │ │ +
│ │ │ │ +Christian Potthast
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,45 +1,47 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Point3.cpp File Reference │ │ │ │ │ -3D Point _M_o_r_e_._._. │ │ │ │ │ +Errors.cpp File Reference │ │ │ │ │ +Factor Graph _V_a_l_u_e_s. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ - double  ggttssaamm::::ddiissttaannccee33 (const _P_o_i_n_t_3 &p1, const _P_o_i_n_t_3 &q, │ │ │ │ │ - _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 1, 3 > H1=boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 1, 3 > │ │ │ │ │ - H2=boost::none) │ │ │ │ │ -  distance between two points │ │ │ │ │ -  │ │ │ │ │ - double  ggttssaamm::::nnoorrmm33 (const _P_o_i_n_t_3 &p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 1, 3 > H=boost:: │ │ │ │ │ - none) │ │ │ │ │ -  Distance of the point from the origin, with Jacobian. │ │ │ │ │ -  │ │ │ │ │ - _P_o_i_n_t_3  ggttssaamm::::nnoorrmmaalliizzee (const _P_o_i_n_t_3 &p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 3, 3 > │ │ │ │ │ - H=boost::none) │ │ │ │ │ -  normalize, with optional Jacobian │ │ │ │ │ -  │ │ │ │ │ - _P_o_i_n_t_3  _g_t_s_a_m_:_:_c_r_o_s_s (const _P_o_i_n_t_3 &p, const _P_o_i_n_t_3 &q, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< │ │ │ │ │ - 3, 3 > H_p=boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 3, 3 > H_q=boost::none) │ │ │ │ │ -  cross product │ │ │ │ │ -  │ │ │ │ │ - double  ggttssaamm::::ddoott (const _P_o_i_n_t_3 &p, const _P_o_i_n_t_3 &q, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 1, │ │ │ │ │ - 3 > H_p=boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 1, 3 > H_q=boost::none) │ │ │ │ │ -  dot product │ │ │ │ │ +_E_r_r_o_r_s  ggttssaamm::::ccrreeaatteeEErrrroorrss (const _V_e_c_t_o_r_V_a_l_u_e_s &V) │ │ │ │ │ +  Break V into pieces according to its start indices. │ │ │ │ │   │ │ │ │ │ -Point3Pair  ggttssaamm::::mmeeaannss (const std::vector< Point3Pair > &abPointPairs) │ │ │ │ │ -  Calculate the two means of a set of Point3 pairs. │ │ │ │ │ + void  ggttssaamm::::pprriinntt (const _E_r_r_o_r_s &e, const std::string &s="Errors") │ │ │ │ │ +  Print an Errors instance. │ │ │ │ │   │ │ │ │ │ - ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const gtsam::Point3Pair &p) │ │ │ │ │ + bool  ggttssaamm::::eeqquuaalliittyy (const _E_r_r_o_r_s &actual, const _E_r_r_o_r_s &expected, double │ │ │ │ │ + tol) │ │ │ │ │ +  │ │ │ │ │ +_E_r_r_o_r_s  ggttssaamm::::ooppeerraattoorr++ (const _E_r_r_o_r_s &a, const _E_r_r_o_r_s &b) │ │ │ │ │ +  Addition. │ │ │ │ │ +  │ │ │ │ │ +_E_r_r_o_r_s  ggttssaamm::::ooppeerraattoorr-- (const _E_r_r_o_r_s &a, const _E_r_r_o_r_s &b) │ │ │ │ │ +  Subtraction. │ │ │ │ │ +  │ │ │ │ │ +_E_r_r_o_r_s  ggttssaamm::::ooppeerraattoorr-- (const _E_r_r_o_r_s &a) │ │ │ │ │ +  Negation. │ │ │ │ │ +  │ │ │ │ │ +double  ggttssaamm::::ddoott (const _E_r_r_o_r_s &a, const _E_r_r_o_r_s &b) │ │ │ │ │ +  Dot product. │ │ │ │ │ +  │ │ │ │ │ + void  ggttssaamm::::aaxxppyy (double alpha, const _E_r_r_o_r_s &x, _E_r_r_o_r_s &y) │ │ │ │ │ +  BLAS level 2 style AXPY, y := alpha*x + y │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -3D Point │ │ │ │ │ +Factor Graph _V_a_l_u_e_s. │ │ │ │ │ +Errors │ │ │ │ │ + Author │ │ │ │ │ + Carlos Nieto │ │ │ │ │ + Christian Potthast │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _P_o_i_n_t_3_._c_p_p │ │ │ │ │ + * _l_i_n_e_a_r │ │ │ │ │ + * _E_r_r_o_r_s_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00416.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00428.html │ │ │ │┄ Files 96% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/CalibratedCamera.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Rot2.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces
│ │ │ │ -
CalibratedCamera.cpp File Reference
│ │ │ │ +
Rot2.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Calibrated camera for which only pose is unknown. │ │ │ │ +

2D Rotations │ │ │ │ More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Calibrated camera for which only pose is unknown.

│ │ │ │ -
Date
Aug 17, 2009
│ │ │ │ +

2D Rotations

│ │ │ │ +
Date
Dec 9, 2009
│ │ │ │
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,22 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -CalibratedCamera.cpp File Reference │ │ │ │ │ -Calibrated camera for which only pose is unknown. _M_o_r_e_._._. │ │ │ │ │ +Rot2.cpp File Reference │ │ │ │ │ +2D Rotations _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Calibrated camera for which only pose is unknown. │ │ │ │ │ +2D Rotations │ │ │ │ │ Date │ │ │ │ │ - Aug 17, 2009 │ │ │ │ │ + Dec 9, 2009 │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_._c_p_p │ │ │ │ │ + * _R_o_t_2_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00437_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00776_source.html │ │ │ │┄ Files 95% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SO3.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianBayesNet.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
SO3.h
│ │ │ │ +
GaussianBayesNet.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ +
19// \callgraph
│ │ │ │ +
20
│ │ │ │
21#pragma once
│ │ │ │
22
│ │ │ │ -
23#include <gtsam/geometry/SOn.h>
│ │ │ │ -
24
│ │ │ │ -
25#include <gtsam/base/Lie.h>
│ │ │ │ -
26#include <gtsam/base/Matrix.h>
│ │ │ │ -
27#include <gtsam/dllexport.h>
│ │ │ │ -
28
│ │ │ │ -
29#include <cmath>
│ │ │ │ -
30#include <vector>
│ │ │ │ -
31
│ │ │ │ -
32namespace gtsam {
│ │ │ │ -
33
│ │ │ │ -
34using SO3 = SO<3>;
│ │ │ │ -
35
│ │ │ │ -
36// Below are all declarations of SO<3> specializations.
│ │ │ │ -
37// They are *defined* in SO3.cpp.
│ │ │ │ +
23#include <gtsam/linear/GaussianConditional.h>
│ │ │ │ +
24#include <gtsam/inference/BayesNet.h>
│ │ │ │ +
25#include <gtsam/inference/FactorGraph.h>
│ │ │ │ +
26#include <gtsam/global_includes.h>
│ │ │ │ +
27
│ │ │ │ +
28#include <utility>
│ │ │ │ +
29namespace gtsam {
│ │ │ │ +
30
│ │ │ │ +
│ │ │ │ +
35 class GTSAM_EXPORT GaussianBayesNet: public BayesNet<GaussianConditional>
│ │ │ │ +
36 {
│ │ │ │ +
37 public:
│ │ │ │
38
│ │ │ │ -
39template <>
│ │ │ │ -
40GTSAM_EXPORT
│ │ │ │ -
41SO3 SO3::AxisAngle(const Vector3& axis, double theta);
│ │ │ │ -
42
│ │ │ │ -
43template <>
│ │ │ │ -
44GTSAM_EXPORT
│ │ │ │ -
45SO3 SO3::ClosestTo(const Matrix3& M);
│ │ │ │ -
46
│ │ │ │ -
47template <>
│ │ │ │ -
48GTSAM_EXPORT
│ │ │ │ -
49SO3 SO3::ChordalMean(const std::vector<SO3>& rotations);
│ │ │ │ +
39 typedef BayesNet<GaussianConditional> Base;
│ │ │ │ +
40 typedef GaussianBayesNet This;
│ │ │ │ +
41 typedef GaussianConditional ConditionalType;
│ │ │ │ +
42 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
43 typedef boost::shared_ptr<ConditionalType> sharedConditional;
│ │ │ │ +
44
│ │ │ │ +
47
│ │ │ │ +
49 GaussianBayesNet() {}
│ │ │ │
50
│ │ │ │ -
51template <>
│ │ │ │ -
52GTSAM_EXPORT
│ │ │ │ -
53Matrix3 SO3::Hat(const Vector3& xi);
│ │ │ │ -
54
│ │ │ │ -
55template <>
│ │ │ │ -
56GTSAM_EXPORT
│ │ │ │ -
57Vector3 SO3::Vee(const Matrix3& X);
│ │ │ │ -
58
│ │ │ │ -
60template <>
│ │ │ │ -
61Matrix3 SO3::AdjointMap() const;
│ │ │ │ -
62
│ │ │ │ -
67template <>
│ │ │ │ -
68GTSAM_EXPORT
│ │ │ │ -
69SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H);
│ │ │ │ -
70
│ │ │ │ -
72template <>
│ │ │ │ -
73GTSAM_EXPORT
│ │ │ │ -
74Matrix3 SO3::ExpmapDerivative(const Vector3& omega);
│ │ │ │ -
75
│ │ │ │ -
80template <>
│ │ │ │ -
81GTSAM_EXPORT
│ │ │ │ -
82Vector3 SO3::Logmap(const SO3& R, ChartJacobian H);
│ │ │ │ -
83
│ │ │ │ -
85template <>
│ │ │ │ -
86GTSAM_EXPORT
│ │ │ │ -
87Matrix3 SO3::LogmapDerivative(const Vector3& omega);
│ │ │ │ -
88
│ │ │ │ -
89// Chart at origin for SO3 is *not* Cayley but actual Expmap/Logmap
│ │ │ │ -
90template <>
│ │ │ │ -
91GTSAM_EXPORT
│ │ │ │ -
92SO3 SO3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H);
│ │ │ │ -
93
│ │ │ │ -
94template <>
│ │ │ │ -
95GTSAM_EXPORT
│ │ │ │ -
96Vector3 SO3::ChartAtOrigin::Local(const SO3& R, ChartJacobian H);
│ │ │ │ -
97
│ │ │ │ -
98template <>
│ │ │ │ -
99GTSAM_EXPORT
│ │ │ │ -
100Vector9 SO3::vec(OptionalJacobian<9, 3> H) const;
│ │ │ │ -
101
│ │ │ │ -
103template <class Archive>
│ │ │ │ -
│ │ │ │ -
104void serialize(Archive& ar, SO3& R, const unsigned int /*version*/) {
│ │ │ │ -
105 Matrix3& M = R.matrix_;
│ │ │ │ -
106 ar& boost::serialization::make_nvp("R11", M(0, 0));
│ │ │ │ -
107 ar& boost::serialization::make_nvp("R12", M(0, 1));
│ │ │ │ -
108 ar& boost::serialization::make_nvp("R13", M(0, 2));
│ │ │ │ -
109 ar& boost::serialization::make_nvp("R21", M(1, 0));
│ │ │ │ -
110 ar& boost::serialization::make_nvp("R22", M(1, 1));
│ │ │ │ -
111 ar& boost::serialization::make_nvp("R23", M(1, 2));
│ │ │ │ -
112 ar& boost::serialization::make_nvp("R31", M(2, 0));
│ │ │ │ -
113 ar& boost::serialization::make_nvp("R32", M(2, 1));
│ │ │ │ -
114 ar& boost::serialization::make_nvp("R33", M(2, 2));
│ │ │ │ -
115}
│ │ │ │ -
│ │ │ │ -
116
│ │ │ │ -
117namespace so3 {
│ │ │ │ -
118
│ │ │ │ -
123GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R,
│ │ │ │ -
124 OptionalJacobian<9, 9> H = boost::none);
│ │ │ │ -
125
│ │ │ │ -
127GTSAM_EXPORT Matrix99 Dcompose(const SO3& R);
│ │ │ │ -
128
│ │ │ │ -
129// Below are two functors that allow for saving computation when exponential map
│ │ │ │ -
130// and its derivatives are needed at the same location in so<3>. The second
│ │ │ │ -
131// functor also implements dedicated methods to apply dexp and/or inv(dexp).
│ │ │ │ +
52 template <typename ITERATOR>
│ │ │ │ +
│ │ │ │ +
53 GaussianBayesNet(ITERATOR firstConditional, ITERATOR lastConditional)
│ │ │ │ +
54 : Base(firstConditional, lastConditional) {}
│ │ │ │ +
│ │ │ │ +
55
│ │ │ │ +
57 template <class CONTAINER>
│ │ │ │ +
│ │ │ │ +
58 explicit GaussianBayesNet(const CONTAINER& conditionals) {
│ │ │ │ +
59 push_back(conditionals);
│ │ │ │ +
60 }
│ │ │ │ +
│ │ │ │ +
61
│ │ │ │ +
64 template <class DERIVEDCONDITIONAL>
│ │ │ │ +
│ │ │ │ +
65 explicit GaussianBayesNet(const FactorGraph<DERIVEDCONDITIONAL>& graph)
│ │ │ │ +
66 : Base(graph) {}
│ │ │ │ +
│ │ │ │ +
67
│ │ │ │ +
72 template <class DERIVEDCONDITIONAL>
│ │ │ │ +
│ │ │ │ +
73 GaussianBayesNet(
│ │ │ │ +
74 std::initializer_list<boost::shared_ptr<DERIVEDCONDITIONAL> > conditionals)
│ │ │ │ +
75 : Base(conditionals) {}
│ │ │ │ +
│ │ │ │ +
76
│ │ │ │ +
78 virtual ~GaussianBayesNet() = default;
│ │ │ │ +
79
│ │ │ │ +
81
│ │ │ │ +
84
│ │ │ │ +
86 bool equals(const This& bn, double tol = 1e-9) const;
│ │ │ │ +
87
│ │ │ │ +
│ │ │ │ +
89 void print(
│ │ │ │ +
90 const std::string& s = "",
│ │ │ │ +
91 const KeyFormatter& formatter = DefaultKeyFormatter) const override {
│ │ │ │ +
92 Base::print(s, formatter);
│ │ │ │ +
93 }
│ │ │ │ +
│ │ │ │ +
94
│ │ │ │ +
96
│ │ │ │ +
99
│ │ │ │ +
101 double error(const VectorValues& x) const;
│ │ │ │ +
102
│ │ │ │ +
104 double logProbability(const VectorValues& x) const;
│ │ │ │ +
105
│ │ │ │ +
111 double evaluate(const VectorValues& x) const;
│ │ │ │ +
112
│ │ │ │ +
│ │ │ │ +
114 double operator()(const VectorValues& x) const {
│ │ │ │ +
115 return evaluate(x);
│ │ │ │ +
116 }
│ │ │ │ +
│ │ │ │ +
117
│ │ │ │ +
120 VectorValues optimize() const;
│ │ │ │ +
121
│ │ │ │ +
123 VectorValues optimize(const VectorValues& given) const;
│ │ │ │ +
124
│ │ │ │ +
131 VectorValues sample(std::mt19937_64* rng) const;
│ │ │ │
132
│ │ │ │ -
│ │ │ │ -
134class GTSAM_EXPORT ExpmapFunctor {
│ │ │ │ -
135 protected:
│ │ │ │ -
136 const double theta2;
│ │ │ │ -
137 Matrix3 W, K, KK;
│ │ │ │ -
138 bool nearZero;
│ │ │ │ -
139 double theta, sin_theta, one_minus_cos; // only defined if !nearZero
│ │ │ │ -
140
│ │ │ │ -
141 void init(bool nearZeroApprox = false);
│ │ │ │ -
142
│ │ │ │ -
143 public:
│ │ │ │ -
145 explicit ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false);
│ │ │ │ -
146
│ │ │ │ -
148 ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false);
│ │ │ │ -
149
│ │ │ │ -
151 SO3 expmap() const;
│ │ │ │ -
152};
│ │ │ │ -
│ │ │ │ -
153
│ │ │ │ -
│ │ │ │ -
155class DexpFunctor : public ExpmapFunctor {
│ │ │ │ -
156 const Vector3 omega;
│ │ │ │ -
157 double a, b;
│ │ │ │ -
158 Matrix3 dexp_;
│ │ │ │ -
159
│ │ │ │ -
160 public:
│ │ │ │ -
162 GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false);
│ │ │ │ -
163
│ │ │ │ -
164 // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation
│ │ │ │ -
165 // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models,
│ │ │ │ -
166 // Information Theory, and Lie Groups", Volume 2, 2008.
│ │ │ │ -
167 // expmap(omega + v) \approx expmap(omega) * expmap(dexp * v)
│ │ │ │ -
168 // This maps a perturbation v in the tangent space to
│ │ │ │ -
169 // a perturbation on the manifold Expmap(dexp * v) */
│ │ │ │ -
170 const Matrix3& dexp() const { return dexp_; }
│ │ │ │ -
171
│ │ │ │ -
173 GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none,
│ │ │ │ -
174 OptionalJacobian<3, 3> H2 = boost::none) const;
│ │ │ │ -
175
│ │ │ │ -
177 GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v,
│ │ │ │ -
178 OptionalJacobian<3, 3> H1 = boost::none,
│ │ │ │ -
179 OptionalJacobian<3, 3> H2 = boost::none) const;
│ │ │ │ -
180};
│ │ │ │ -
│ │ │ │ -
181} // namespace so3
│ │ │ │ -
182
│ │ │ │ -
183/*
│ │ │ │ -
184 * Define the traits. internal::LieGroup provides both Lie group and Testable
│ │ │ │ -
185 */
│ │ │ │ -
186
│ │ │ │ -
187template <>
│ │ │ │ -
188struct traits<SO3> : public internal::LieGroup<SO3> {};
│ │ │ │ -
189
│ │ │ │ -
190template <>
│ │ │ │ -
191struct traits<const SO3> : public internal::LieGroup<SO3> {};
│ │ │ │ -
192
│ │ │ │ -
193} // end namespace gtsam
│ │ │ │ -
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ -
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ -
SOn.h
N*N matrix representation of SO(N).
│ │ │ │ +
140 VectorValues sample(const VectorValues& given, std::mt19937_64* rng) const;
│ │ │ │ +
141
│ │ │ │ +
143 VectorValues sample() const;
│ │ │ │ +
144
│ │ │ │ +
146 VectorValues sample(const VectorValues& given) const;
│ │ │ │ +
147
│ │ │ │ +
154 Ordering ordering() const;
│ │ │ │ +
155
│ │ │ │ +
157
│ │ │ │ +
160
│ │ │ │ +
166 std::pair<Matrix, Vector> matrix(const Ordering& ordering) const;
│ │ │ │ +
167
│ │ │ │ +
173 std::pair<Matrix, Vector> matrix() const;
│ │ │ │ +
174
│ │ │ │ +
200 VectorValues optimizeGradientSearch() const;
│ │ │ │ +
201
│ │ │ │ +
207 VectorValues gradient(const VectorValues& x0) const;
│ │ │ │ +
208
│ │ │ │ +
215 VectorValues gradientAtZero() const;
│ │ │ │ +
216
│ │ │ │ +
224 double determinant() const;
│ │ │ │ +
225
│ │ │ │ +
232 double logDeterminant() const;
│ │ │ │ +
233
│ │ │ │ +
238 VectorValues backSubstitute(const VectorValues& gx) const;
│ │ │ │ +
239
│ │ │ │ +
246 VectorValues backSubstituteTranspose(const VectorValues& gx) const;
│ │ │ │ +
247
│ │ │ │ +
251
│ │ │ │ +
252 using Base::evaluate; // Expose evaluate(const HybridValues&) method..
│ │ │ │ +
253 using Base::logProbability; // Expose logProbability(const HybridValues&) method..
│ │ │ │ +
254 using Base::error; // Expose error(const HybridValues&) method..
│ │ │ │ +
255
│ │ │ │ +
257
│ │ │ │ +
258 private:
│ │ │ │ +
260 friend class boost::serialization::access;
│ │ │ │ +
261 template<class ARCHIVE>
│ │ │ │ +
262 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
263 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
264 }
│ │ │ │ +
265 };
│ │ │ │ +
│ │ │ │ +
266
│ │ │ │ +
268 template<>
│ │ │ │ +
│ │ │ │ +
269 struct traits<GaussianBayesNet> : public Testable<GaussianBayesNet> {
│ │ │ │ +
270 };
│ │ │ │ +
│ │ │ │ +
271
│ │ │ │ +
272} //\ namespace gtsam
│ │ │ │ +
FactorGraph.h
Factor Graph Base Class.
│ │ │ │ +
BayesNet.h
Bayes network.
│ │ │ │ +
GaussianConditional.h
Conditional Gaussian Base class.
│ │ │ │ +
global_includes.h
Included from all GTSAM files.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition serialization.h:113
│ │ │ │ +
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::LieGroup< SO< N >, internal::DimensionSO(N)>::Retract
static SO< N > Retract(const TangentVector &v)
Retract at origin: possible in Lie group because it has an identity.
Definition Lie.h:111
│ │ │ │ -
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition Lie.h:229
│ │ │ │ -
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
gtsam::so3::ExpmapFunctor
Functor implementing Exponential map.
Definition SO3.h:134
│ │ │ │ -
gtsam::so3::DexpFunctor
Functor that implements Exponential map and its derivatives.
Definition SO3.h:155
│ │ │ │ -
gtsam::so3::DexpFunctor::applyDexp
GTSAM_EXPORT Vector3 applyDexp(const Vector3 &v, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
Multiplies with dexp(), with optional derivatives.
Definition SO3.cpp:101
│ │ │ │ -
gtsam::so3::DexpFunctor::applyInvDexp
GTSAM_EXPORT Vector3 applyInvDexp(const Vector3 &v, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
Multiplies with dexp().inverse(), with optional derivatives.
Definition SO3.cpp:120
│ │ │ │ - │ │ │ │ -
gtsam::SO< 3 >::Expmap
static SO Expmap(const TangentVector &omega, ChartJacobian H=boost::none)
Exponential map at identity - create a rotation from canonical coordinates.
Definition SOn-inl.h:67
│ │ │ │ -
gtsam::SO< 3 >::vec
VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost::none) const
Return vectorized rotation matrix in column order.
Definition SOn-inl.h:88
│ │ │ │ -
gtsam::SO< 3 >::ChordalMean
static SO ChordalMean(const std::vector< SO > &rotations)
Named constructor that finds chordal mean , currently only defined for SO3.
│ │ │ │ -
gtsam::SO< 3 >::Vee
static TangentVector Vee(const MatrixNN &X)
Inverse of Hat. See note about xi element order in Hat.
Definition SOn-inl.h:35
│ │ │ │ -
gtsam::SO< 3 >::Logmap
static TangentVector Logmap(const SO &R, ChartJacobian H=boost::none)
Log map at identity - returns the canonical coordinates of this rotation.
Definition SOn-inl.h:77
│ │ │ │ -
gtsam::SO< 3 >::AxisAngle
static SO AxisAngle(const Vector3 &axis, double theta)
Constructor from axis and angle. Only defined for SO3.
│ │ │ │ -
gtsam::SO< 3 >::AdjointMap
MatrixDD AdjointMap() const
Adjoint map.
Definition SO4.cpp:159
│ │ │ │ -
gtsam::SO< 3 >::Hat
static MatrixNN Hat(const TangentVector &xi)
Hat operator creates Lie algebra element corresponding to d-vector, where d is the dimensionality of ...
Definition SOn-inl.h:29
│ │ │ │ -
gtsam::SO< 3 >::ExpmapDerivative
static MatrixDD ExpmapDerivative(const TangentVector &omega)
Derivative of Expmap, currently only defined for SO3.
Definition SOn-inl.h:72
│ │ │ │ -
gtsam::SO< 3 >::LogmapDerivative
static MatrixDD LogmapDerivative(const TangentVector &omega)
Derivative of Logmap, currently only defined for SO3.
Definition SOn-inl.h:82
│ │ │ │ -
gtsam::SO< 3 >::ClosestTo
static SO ClosestTo(const MatrixNN &M)
Named constructor that finds SO(n) matrix closest to M in Frobenius norm, currently only defined for ...
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::BayesNet
A BayesNet is a tree of conditionals, stored in elimination order.
Definition BayesNet.h:35
│ │ │ │ +
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ +
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
gtsam::GaussianBayesNet
GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals.
Definition GaussianBayesNet.h:36
│ │ │ │ +
gtsam::GaussianBayesNet::operator()
double operator()(const VectorValues &x) const
Evaluate probability density, sugar.
Definition GaussianBayesNet.h:114
│ │ │ │ +
gtsam::GaussianBayesNet::print
void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print graph
Definition GaussianBayesNet.h:89
│ │ │ │ +
gtsam::GaussianBayesNet::GaussianBayesNet
GaussianBayesNet(std::initializer_list< boost::shared_ptr< DERIVEDCONDITIONAL > > conditionals)
Constructor that takes an initializer list of shared pointers.
Definition GaussianBayesNet.h:73
│ │ │ │ +
gtsam::GaussianBayesNet::GaussianBayesNet
GaussianBayesNet(const FactorGraph< DERIVEDCONDITIONAL > &graph)
Implicit copy/downcast constructor to override explicit template container constructor.
Definition GaussianBayesNet.h:65
│ │ │ │ +
gtsam::GaussianBayesNet::~GaussianBayesNet
virtual ~GaussianBayesNet()=default
Destructor.
│ │ │ │ +
gtsam::GaussianBayesNet::GaussianBayesNet
GaussianBayesNet(const CONTAINER &conditionals)
Construct from container of factors (shared_ptr or plain objects)
Definition GaussianBayesNet.h:58
│ │ │ │ +
gtsam::GaussianBayesNet::GaussianBayesNet
GaussianBayesNet()
Construct empty bayes net.
Definition GaussianBayesNet.h:49
│ │ │ │ +
gtsam::GaussianBayesNet::GaussianBayesNet
GaussianBayesNet(ITERATOR firstConditional, ITERATOR lastConditional)
Construct from iterator over conditionals.
Definition GaussianBayesNet.h:53
│ │ │ │ +
gtsam::GaussianConditional
A GaussianConditional functions as the node in a Bayes network.
Definition GaussianConditional.h:43
│ │ │ │ +
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,266 +1,228 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SO3.h │ │ │ │ │ +GaussianBayesNet.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ +19// \callgraph │ │ │ │ │ +20 │ │ │ │ │ 21#pragma once │ │ │ │ │ 22 │ │ │ │ │ -23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_S_O_n_._h> │ │ │ │ │ -24 │ │ │ │ │ -25#include <_g_t_s_a_m_/_b_a_s_e_/_L_i_e_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ -27#include │ │ │ │ │ -28 │ │ │ │ │ -29#include │ │ │ │ │ -30#include │ │ │ │ │ -31 │ │ │ │ │ -32namespace _g_t_s_a_m { │ │ │ │ │ -33 │ │ │ │ │ -34using SO3 = SO<3>; │ │ │ │ │ -35 │ │ │ │ │ -36// Below are all declarations of SO<3> specializations. │ │ │ │ │ -37// They are *defined* in SO3.cpp. │ │ │ │ │ +23#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ +27 │ │ │ │ │ +28#include │ │ │ │ │ +29namespace _g_t_s_a_m { │ │ │ │ │ +30 │ │ │ │ │ +_3_5 class GTSAM_EXPORT _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t: public _B_a_y_e_s_N_e_t │ │ │ │ │ +36 { │ │ │ │ │ +37 public: │ │ │ │ │ 38 │ │ │ │ │ -39template <> │ │ │ │ │ -40GTSAM_EXPORT │ │ │ │ │ -41SO3 _S_O_3_:_:_A_x_i_s_A_n_g_l_e(const Vector3& axis, double theta); │ │ │ │ │ -42 │ │ │ │ │ -43template <> │ │ │ │ │ -44GTSAM_EXPORT │ │ │ │ │ -45SO3 _S_O_3_:_:_C_l_o_s_e_s_t_T_o(const Matrix3& M); │ │ │ │ │ -46 │ │ │ │ │ -47template <> │ │ │ │ │ -48GTSAM_EXPORT │ │ │ │ │ -49SO3 _S_O_3_:_:_C_h_o_r_d_a_l_M_e_a_n(const std::vector& rotations); │ │ │ │ │ +39 typedef _B_a_y_e_s_N_e_t_<_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_> _B_a_s_e; │ │ │ │ │ +40 typedef _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t _T_h_i_s; │ │ │ │ │ +41 typedef _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l _C_o_n_d_i_t_i_o_n_a_l_T_y_p_e; │ │ │ │ │ +42 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +43 typedef boost::shared_ptr sharedConditional; │ │ │ │ │ +44 │ │ │ │ │ +47 │ │ │ │ │ +_4_9 _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t() {} │ │ │ │ │ 50 │ │ │ │ │ -51template <> │ │ │ │ │ -52GTSAM_EXPORT │ │ │ │ │ -53Matrix3 _S_O_3_:_:_H_a_t(const Vector3& xi); │ │ │ │ │ -54 │ │ │ │ │ -55template <> │ │ │ │ │ -56GTSAM_EXPORT │ │ │ │ │ -57Vector3 _S_O_3_:_:_V_e_e(const Matrix3& X); │ │ │ │ │ -58 │ │ │ │ │ -60template <> │ │ │ │ │ -61Matrix3 _S_O_3_:_:_A_d_j_o_i_n_t_M_a_p() const; │ │ │ │ │ -62 │ │ │ │ │ -67template <> │ │ │ │ │ -68GTSAM_EXPORT │ │ │ │ │ -69SO3 _S_O_3_:_:_E_x_p_m_a_p(const Vector3& omega, ChartJacobian H); │ │ │ │ │ -70 │ │ │ │ │ -72template <> │ │ │ │ │ -73GTSAM_EXPORT │ │ │ │ │ -74Matrix3 _S_O_3_:_:_E_x_p_m_a_p_D_e_r_i_v_a_t_i_v_e(const Vector3& omega); │ │ │ │ │ -75 │ │ │ │ │ -80template <> │ │ │ │ │ -81GTSAM_EXPORT │ │ │ │ │ -82Vector3 _S_O_3_:_:_L_o_g_m_a_p(const SO3& R, ChartJacobian H); │ │ │ │ │ -83 │ │ │ │ │ -85template <> │ │ │ │ │ -86GTSAM_EXPORT │ │ │ │ │ -87Matrix3 _S_O_3_:_:_L_o_g_m_a_p_D_e_r_i_v_a_t_i_v_e(const Vector3& omega); │ │ │ │ │ -88 │ │ │ │ │ -89// Chart at origin for SO3 is *not* Cayley but actual Expmap/Logmap │ │ │ │ │ -90template <> │ │ │ │ │ -91GTSAM_EXPORT │ │ │ │ │ -92SO3 _S_O_3_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n_:_:_R_e_t_r_a_c_t(const Vector3& omega, ChartJacobian H); │ │ │ │ │ -93 │ │ │ │ │ -94template <> │ │ │ │ │ -95GTSAM_EXPORT │ │ │ │ │ -96Vector3 SO3::ChartAtOrigin::Local(const SO3& R, ChartJacobian H); │ │ │ │ │ -97 │ │ │ │ │ -98template <> │ │ │ │ │ -99GTSAM_EXPORT │ │ │ │ │ -100Vector9 _S_O_3_:_:_v_e_c(OptionalJacobian<9, 3> H) const; │ │ │ │ │ -101 │ │ │ │ │ -103template │ │ │ │ │ -_1_0_4void _s_e_r_i_a_l_i_z_e(Archive& ar, _S_O_3& R, const unsigned int /*version*/) { │ │ │ │ │ -105 Matrix3& M = R.matrix_; │ │ │ │ │ -106 ar& boost::serialization::make_nvp("R11", M(0, 0)); │ │ │ │ │ -107 ar& boost::serialization::make_nvp("R12", M(0, 1)); │ │ │ │ │ -108 ar& boost::serialization::make_nvp("R13", M(0, 2)); │ │ │ │ │ -109 ar& boost::serialization::make_nvp("R21", M(1, 0)); │ │ │ │ │ -110 ar& boost::serialization::make_nvp("R22", M(1, 1)); │ │ │ │ │ -111 ar& boost::serialization::make_nvp("R23", M(1, 2)); │ │ │ │ │ -112 ar& boost::serialization::make_nvp("R31", M(2, 0)); │ │ │ │ │ -113 ar& boost::serialization::make_nvp("R32", M(2, 1)); │ │ │ │ │ -114 ar& boost::serialization::make_nvp("R33", M(2, 2)); │ │ │ │ │ -115} │ │ │ │ │ -116 │ │ │ │ │ -117namespace so3 { │ │ │ │ │ -118 │ │ │ │ │ -123GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, │ │ │ │ │ -124 OptionalJacobian<9, 9> H = boost::none); │ │ │ │ │ -125 │ │ │ │ │ -127GTSAM_EXPORT Matrix99 Dcompose(const SO3& R); │ │ │ │ │ -128 │ │ │ │ │ -129// Below are two functors that allow for saving computation when exponential │ │ │ │ │ -map │ │ │ │ │ -130// and its derivatives are needed at the same location in so<3>. The second │ │ │ │ │ -131// functor also implements dedicated methods to apply dexp and/or inv(dexp). │ │ │ │ │ +52 template │ │ │ │ │ +_5_3 _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t(ITERATOR firstConditional, ITERATOR lastConditional) │ │ │ │ │ +54 : _B_a_s_e(firstConditional, lastConditional) {} │ │ │ │ │ +55 │ │ │ │ │ +57 template │ │ │ │ │ +_5_8 explicit _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t(const CONTAINER& conditionals) { │ │ │ │ │ +59 push_back(conditionals); │ │ │ │ │ +60 } │ │ │ │ │ +61 │ │ │ │ │ +64 template │ │ │ │ │ +_6_5 explicit _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t(const _F_a_c_t_o_r_G_r_a_p_h_<_D_E_R_I_V_E_D_C_O_N_D_I_T_I_O_N_A_L_>& graph) │ │ │ │ │ +66 : _B_a_s_e(graph) {} │ │ │ │ │ +67 │ │ │ │ │ +72 template │ │ │ │ │ +_7_3 _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t( │ │ │ │ │ +74 std::initializer_list > conditionals) │ │ │ │ │ +75 : _B_a_s_e(conditionals) {} │ │ │ │ │ +76 │ │ │ │ │ +_7_8 virtual _~_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t() = default; │ │ │ │ │ +79 │ │ │ │ │ +81 │ │ │ │ │ +84 │ │ │ │ │ +86 bool _e_q_u_a_l_s(const _T_h_i_s& bn, double tol = 1e-9) const; │ │ │ │ │ +87 │ │ │ │ │ +_8_9 void _p_r_i_n_t( │ │ │ │ │ +90 const std::string& s = "", │ │ │ │ │ +91 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override { │ │ │ │ │ +92 Base::print(s, formatter); │ │ │ │ │ +93 } │ │ │ │ │ +94 │ │ │ │ │ +96 │ │ │ │ │ +99 │ │ │ │ │ +101 double error(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const; │ │ │ │ │ +102 │ │ │ │ │ +104 double logProbability(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const; │ │ │ │ │ +105 │ │ │ │ │ +111 double evaluate(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const; │ │ │ │ │ +112 │ │ │ │ │ +_1_1_4 double _o_p_e_r_a_t_o_r_(_)(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const { │ │ │ │ │ +115 return evaluate(x); │ │ │ │ │ +116 } │ │ │ │ │ +117 │ │ │ │ │ +120 _V_e_c_t_o_r_V_a_l_u_e_s _o_p_t_i_m_i_z_e() const; │ │ │ │ │ +121 │ │ │ │ │ +123 _V_e_c_t_o_r_V_a_l_u_e_s _o_p_t_i_m_i_z_e(const _V_e_c_t_o_r_V_a_l_u_e_s& given) const; │ │ │ │ │ +124 │ │ │ │ │ +131 _V_e_c_t_o_r_V_a_l_u_e_s sample(std::mt19937_64* rng) const; │ │ │ │ │ 132 │ │ │ │ │ -_1_3_4class GTSAM_EXPORT _E_x_p_m_a_p_F_u_n_c_t_o_r { │ │ │ │ │ -135 protected: │ │ │ │ │ -136 const double theta2; │ │ │ │ │ -137 Matrix3 W, K, KK; │ │ │ │ │ -138 bool nearZero; │ │ │ │ │ -139 double theta, sin_theta, one_minus_cos; // only defined if !nearZero │ │ │ │ │ -140 │ │ │ │ │ -141 void init(bool nearZeroApprox = false); │ │ │ │ │ -142 │ │ │ │ │ -143 public: │ │ │ │ │ -145 explicit _E_x_p_m_a_p_F_u_n_c_t_o_r(const Vector3& omega, bool nearZeroApprox = false); │ │ │ │ │ -146 │ │ │ │ │ -148 _E_x_p_m_a_p_F_u_n_c_t_o_r(const Vector3& axis, double angle, bool nearZeroApprox = │ │ │ │ │ -false); │ │ │ │ │ -149 │ │ │ │ │ -151 _S_O_3 expmap() const; │ │ │ │ │ -152}; │ │ │ │ │ -153 │ │ │ │ │ -_1_5_5class _D_e_x_p_F_u_n_c_t_o_r : public _E_x_p_m_a_p_F_u_n_c_t_o_r { │ │ │ │ │ -156 const Vector3 omega; │ │ │ │ │ -157 double a, b; │ │ │ │ │ -158 Matrix3 dexp_; │ │ │ │ │ -159 │ │ │ │ │ -160 public: │ │ │ │ │ -162 GTSAM_EXPORT explicit _D_e_x_p_F_u_n_c_t_o_r(const Vector3& omega, bool nearZeroApprox │ │ │ │ │ -= false); │ │ │ │ │ -163 │ │ │ │ │ -164 // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation │ │ │ │ │ -165 // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models, │ │ │ │ │ -166 // Information Theory, and Lie Groups", Volume 2, 2008. │ │ │ │ │ -167 // expmap(omega + v) \approx expmap(omega) * expmap(dexp * v) │ │ │ │ │ -168 // This maps a perturbation v in the tangent space to │ │ │ │ │ -169 // a perturbation on the manifold Expmap(dexp * v) */ │ │ │ │ │ -170 const Matrix3& dexp() const { return dexp_; } │ │ │ │ │ -171 │ │ │ │ │ -173 GTSAM_EXPORT Vector3 _a_p_p_l_y_D_e_x_p(const Vector3& v, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H1 │ │ │ │ │ -= boost::none, │ │ │ │ │ -174 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H2 = boost::none) const; │ │ │ │ │ -175 │ │ │ │ │ -177 GTSAM_EXPORT Vector3 _a_p_p_l_y_I_n_v_D_e_x_p(const Vector3& v, │ │ │ │ │ -178 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H1 = boost::none, │ │ │ │ │ -179 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H2 = boost::none) const; │ │ │ │ │ -180}; │ │ │ │ │ -181} // namespace so3 │ │ │ │ │ -182 │ │ │ │ │ -183/* │ │ │ │ │ -184 * Define the traits. internal::LieGroup provides both Lie group and │ │ │ │ │ -Testable │ │ │ │ │ -185 */ │ │ │ │ │ -186 │ │ │ │ │ -187template <> │ │ │ │ │ -_1_8_8struct _t_r_a_i_t_s<_S_O_3> : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ -189 │ │ │ │ │ -190template <> │ │ │ │ │ -_1_9_1struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ -192 │ │ │ │ │ -193} // end namespace gtsam │ │ │ │ │ -_L_i_e_._h │ │ │ │ │ -Base class and basic functions for Lie types. │ │ │ │ │ -_M_a_t_r_i_x_._h │ │ │ │ │ -typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ -_S_O_n_._h │ │ │ │ │ -N*N matrix representation of SO(N). │ │ │ │ │ +140 _V_e_c_t_o_r_V_a_l_u_e_s sample(const _V_e_c_t_o_r_V_a_l_u_e_s& given, std::mt19937_64* rng) const; │ │ │ │ │ +141 │ │ │ │ │ +143 _V_e_c_t_o_r_V_a_l_u_e_s sample() const; │ │ │ │ │ +144 │ │ │ │ │ +146 _V_e_c_t_o_r_V_a_l_u_e_s sample(const _V_e_c_t_o_r_V_a_l_u_e_s& given) const; │ │ │ │ │ +147 │ │ │ │ │ +154 _O_r_d_e_r_i_n_g ordering() const; │ │ │ │ │ +155 │ │ │ │ │ +157 │ │ │ │ │ +160 │ │ │ │ │ +166 std::pair matrix(const _O_r_d_e_r_i_n_g& ordering) const; │ │ │ │ │ +167 │ │ │ │ │ +173 std::pair matrix() const; │ │ │ │ │ +174 │ │ │ │ │ +200 _V_e_c_t_o_r_V_a_l_u_e_s optimizeGradientSearch() const; │ │ │ │ │ +201 │ │ │ │ │ +207 _V_e_c_t_o_r_V_a_l_u_e_s gradient(const _V_e_c_t_o_r_V_a_l_u_e_s& x0) const; │ │ │ │ │ +208 │ │ │ │ │ +215 _V_e_c_t_o_r_V_a_l_u_e_s gradientAtZero() const; │ │ │ │ │ +216 │ │ │ │ │ +224 double determinant() const; │ │ │ │ │ +225 │ │ │ │ │ +232 double logDeterminant() const; │ │ │ │ │ +233 │ │ │ │ │ +238 _V_e_c_t_o_r_V_a_l_u_e_s backSubstitute(const _V_e_c_t_o_r_V_a_l_u_e_s& gx) const; │ │ │ │ │ +239 │ │ │ │ │ +246 _V_e_c_t_o_r_V_a_l_u_e_s backSubstituteTranspose(const _V_e_c_t_o_r_V_a_l_u_e_s& gx) const; │ │ │ │ │ +247 │ │ │ │ │ +251 │ │ │ │ │ +252 using Base::evaluate; // Expose evaluate(const HybridValues&) method.. │ │ │ │ │ +253 using Base::logProbability; // Expose logProbability(const HybridValues&) │ │ │ │ │ +method.. │ │ │ │ │ +254 using Base::error; // Expose error(const HybridValues&) method.. │ │ │ │ │ +255 │ │ │ │ │ +257 │ │ │ │ │ +258 private: │ │ │ │ │ +_2_6_0 friend class boost::serialization::access; │ │ │ │ │ +261 template │ │ │ │ │ +262 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +263 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +264 } │ │ │ │ │ +265 }; │ │ │ │ │ +266 │ │ │ │ │ +268 template<> │ │ │ │ │ +_2_6_9 struct _t_r_a_i_t_s<_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t> : public _T_e_s_t_a_b_l_e { │ │ │ │ │ +270 }; │ │ │ │ │ +271 │ │ │ │ │ +272} //\ namespace gtsam │ │ │ │ │ +_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Factor Graph Base Class. │ │ │ │ │ +_B_a_y_e_s_N_e_t_._h │ │ │ │ │ +Bayes network. │ │ │ │ │ +_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ +Conditional Gaussian Base class. │ │ │ │ │ +_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ +Included from all GTSAM files. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e │ │ │ │ │ -std::string serialize(const T &input) │ │ │ │ │ -serializes to a string │ │ │ │ │ -DDeeffiinniittiioonn serialization.h:113 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ +Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key │ │ │ │ │ +landmarkKey) │ │ │ │ │ +Optimize for triangulation. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.cpp:155 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_<_ _S_O_<_ _N_ _>_,_ _i_n_t_e_r_n_a_l_:_:_D_i_m_e_n_s_i_o_n_S_O_(_N_)_>_:_:_R_e_t_r_a_c_t │ │ │ │ │ -static SO< N > Retract(const TangentVector &v) │ │ │ │ │ -Retract at origin: possible in Lie group because it has an identity. │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ -Both LieGroupTraits and Testable. │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:229 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ -either a fixed size o... │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_s_o_3_:_:_E_x_p_m_a_p_F_u_n_c_t_o_r │ │ │ │ │ -Functor implementing Exponential map. │ │ │ │ │ -DDeeffiinniittiioonn SO3.h:134 │ │ │ │ │ -_g_t_s_a_m_:_:_s_o_3_:_:_D_e_x_p_F_u_n_c_t_o_r │ │ │ │ │ -Functor that implements Exponential map and its derivatives. │ │ │ │ │ -DDeeffiinniittiioonn SO3.h:155 │ │ │ │ │ -_g_t_s_a_m_:_:_s_o_3_:_:_D_e_x_p_F_u_n_c_t_o_r_:_:_a_p_p_l_y_D_e_x_p │ │ │ │ │ -GTSAM_EXPORT Vector3 applyDexp(const Vector3 &v, OptionalJacobian< 3, 3 > │ │ │ │ │ -H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const │ │ │ │ │ -Multiplies with dexp(), with optional derivatives. │ │ │ │ │ -DDeeffiinniittiioonn SO3.cpp:101 │ │ │ │ │ -_g_t_s_a_m_:_:_s_o_3_:_:_D_e_x_p_F_u_n_c_t_o_r_:_:_a_p_p_l_y_I_n_v_D_e_x_p │ │ │ │ │ -GTSAM_EXPORT Vector3 applyInvDexp(const Vector3 &v, OptionalJacobian< 3, 3 > │ │ │ │ │ -H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const │ │ │ │ │ -Multiplies with dexp().inverse(), with optional derivatives. │ │ │ │ │ -DDeeffiinniittiioonn SO3.cpp:120 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_<_ _3_ _> │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_<_ _3_ _>_:_:_E_x_p_m_a_p │ │ │ │ │ -static SO Expmap(const TangentVector &omega, ChartJacobian H=boost::none) │ │ │ │ │ -Exponential map at identity - create a rotation from canonical coordinates. │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_<_ _3_ _>_:_:_v_e_c │ │ │ │ │ -VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost:: │ │ │ │ │ -none) const │ │ │ │ │ -Return vectorized rotation matrix in column order. │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:88 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_<_ _3_ _>_:_:_C_h_o_r_d_a_l_M_e_a_n │ │ │ │ │ -static SO ChordalMean(const std::vector< SO > &rotations) │ │ │ │ │ -Named constructor that finds chordal mean , currently only defined for SO3. │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_<_ _3_ _>_:_:_V_e_e │ │ │ │ │ -static TangentVector Vee(const MatrixNN &X) │ │ │ │ │ -Inverse of Hat. See note about xi element order in Hat. │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_<_ _3_ _>_:_:_L_o_g_m_a_p │ │ │ │ │ -static TangentVector Logmap(const SO &R, ChartJacobian H=boost::none) │ │ │ │ │ -Log map at identity - returns the canonical coordinates of this rotation. │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_<_ _3_ _>_:_:_A_x_i_s_A_n_g_l_e │ │ │ │ │ -static SO AxisAngle(const Vector3 &axis, double theta) │ │ │ │ │ -Constructor from axis and angle. Only defined for SO3. │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_<_ _3_ _>_:_:_A_d_j_o_i_n_t_M_a_p │ │ │ │ │ -MatrixDD AdjointMap() const │ │ │ │ │ -Adjoint map. │ │ │ │ │ -DDeeffiinniittiioonn SO4.cpp:159 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_<_ _3_ _>_:_:_H_a_t │ │ │ │ │ -static MatrixNN Hat(const TangentVector &xi) │ │ │ │ │ -Hat operator creates Lie algebra element corresponding to d-vector, where d is │ │ │ │ │ -the dimensionality of ... │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:29 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_<_ _3_ _>_:_:_E_x_p_m_a_p_D_e_r_i_v_a_t_i_v_e │ │ │ │ │ -static MatrixDD ExpmapDerivative(const TangentVector &omega) │ │ │ │ │ -Derivative of Expmap, currently only defined for SO3. │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:72 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_<_ _3_ _>_:_:_L_o_g_m_a_p_D_e_r_i_v_a_t_i_v_e │ │ │ │ │ -static MatrixDD LogmapDerivative(const TangentVector &omega) │ │ │ │ │ -Derivative of Logmap, currently only defined for SO3. │ │ │ │ │ -DDeeffiinniittiioonn SOn-inl.h:82 │ │ │ │ │ -_g_t_s_a_m_:_:_S_O_<_ _3_ _>_:_:_C_l_o_s_e_s_t_T_o │ │ │ │ │ -static SO ClosestTo(const MatrixNN &M) │ │ │ │ │ -Named constructor that finds SO(n) matrix closest to M in Frobenius norm, │ │ │ │ │ -currently only defined for ... │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t │ │ │ │ │ +A BayesNet is a tree of conditionals, stored in elimination order. │ │ │ │ │ +DDeeffiinniittiioonn BayesNet.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ +nodes. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t │ │ │ │ │ +GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals. │ │ │ │ │ +DDeeffiinniittiioonn GaussianBayesNet.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ +double operator()(const VectorValues &x) const │ │ │ │ │ +Evaluate probability density, sugar. │ │ │ │ │ +DDeeffiinniittiioonn GaussianBayesNet.h:114 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&formatter=DefaultKeyFormatter) const override │ │ │ │ │ +print graph │ │ │ │ │ +DDeeffiinniittiioonn GaussianBayesNet.h:89 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t │ │ │ │ │ +GaussianBayesNet(std::initializer_list< boost::shared_ptr< DERIVEDCONDITIONAL > │ │ │ │ │ +> conditionals) │ │ │ │ │ +Constructor that takes an initializer list of shared pointers. │ │ │ │ │ +DDeeffiinniittiioonn GaussianBayesNet.h:73 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t │ │ │ │ │ +GaussianBayesNet(const FactorGraph< DERIVEDCONDITIONAL > &graph) │ │ │ │ │ +Implicit copy/downcast constructor to override explicit template container │ │ │ │ │ +constructor. │ │ │ │ │ +DDeeffiinniittiioonn GaussianBayesNet.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_:_:_~_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t │ │ │ │ │ +virtual ~GaussianBayesNet()=default │ │ │ │ │ +Destructor. │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t │ │ │ │ │ +GaussianBayesNet(const CONTAINER &conditionals) │ │ │ │ │ +Construct from container of factors (shared_ptr or plain objects) │ │ │ │ │ +DDeeffiinniittiioonn GaussianBayesNet.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t │ │ │ │ │ +GaussianBayesNet() │ │ │ │ │ +Construct empty bayes net. │ │ │ │ │ +DDeeffiinniittiioonn GaussianBayesNet.h:49 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t │ │ │ │ │ +GaussianBayesNet(ITERATOR firstConditional, ITERATOR lastConditional) │ │ │ │ │ +Construct from iterator over conditionals. │ │ │ │ │ +DDeeffiinniittiioonn GaussianBayesNet.h:53 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +A GaussianConditional functions as the node in a Bayes network. │ │ │ │ │ +DDeeffiinniittiioonn GaussianConditional.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ +with a unique integer... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_O_3_._h │ │ │ │ │ + * _l_i_n_e_a_r │ │ │ │ │ + * _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00440_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00386_source.html │ │ │ │┄ Files 93% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3DS2_Base.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SOn-inl.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Cal3DS2_Base.h
│ │ │ │ +
SOn-inl.h
│ │ │ │
│ │ │ │
│ │ │ │ -
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/geometry/Cal3.h>
│ │ │ │ -
23#include <gtsam/geometry/Point2.h>
│ │ │ │ -
24#include <boost/shared_ptr.hpp>
│ │ │ │ -
25
│ │ │ │ -
26namespace gtsam {
│ │ │ │ -
27
│ │ │ │ -
│ │ │ │ -
42class GTSAM_EXPORT Cal3DS2_Base : public Cal3 {
│ │ │ │ -
43 protected:
│ │ │ │ -
44 double k1_ = 0.0f, k2_ = 0.0f;
│ │ │ │ -
45 double p1_ = 0.0f, p2_ = 0.0f;
│ │ │ │ -
46 double tol_ = 1e-5;
│ │ │ │ -
47
│ │ │ │ -
48 public:
│ │ │ │ -
49 enum { dimension = 9 };
│ │ │ │ -
50
│ │ │ │ -
52 using shared_ptr = boost::shared_ptr<Cal3DS2_Base>;
│ │ │ │ -
53
│ │ │ │ -
56
│ │ │ │ -
58 Cal3DS2_Base() = default;
│ │ │ │ -
59
│ │ │ │ -
60 Cal3DS2_Base(double fx, double fy, double s, double u0, double v0, double k1,
│ │ │ │ -
61 double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5)
│ │ │ │ -
62 : Cal3(fx, fy, s, u0, v0),
│ │ │ │ -
63 k1_(k1),
│ │ │ │ -
64 k2_(k2),
│ │ │ │ -
65 p1_(p1),
│ │ │ │ -
66 p2_(p2),
│ │ │ │ -
67 tol_(tol) {}
│ │ │ │ -
68
│ │ │ │ -
69 ~Cal3DS2_Base() override {}
│ │ │ │ +
12#pragma once
│ │ │ │ +
13
│ │ │ │ +
21#include <gtsam/base/Matrix.h>
│ │ │ │ +
22
│ │ │ │ +
23#include <iostream>
│ │ │ │ +
24
│ │ │ │ +
25namespace gtsam {
│ │ │ │ +
26
│ │ │ │ +
27// Implementation for N>=5 just uses dynamic version
│ │ │ │ +
28template <int N>
│ │ │ │ +
│ │ │ │ +
29typename SO<N>::MatrixNN SO<N>::Hat(const TangentVector& xi) {
│ │ │ │ +
30 return SOn::Hat(xi);
│ │ │ │ +
31}
│ │ │ │ +
│ │ │ │ +
32
│ │ │ │ +
33// Implementation for N>=5 just uses dynamic version
│ │ │ │ +
34template <int N>
│ │ │ │ +
│ │ │ │ +
35typename SO<N>::TangentVector SO<N>::Vee(const MatrixNN& X) {
│ │ │ │ +
36 return SOn::Vee(X);
│ │ │ │ +
37}
│ │ │ │ +
│ │ │ │ +
38
│ │ │ │ +
39template <int N>
│ │ │ │ +
│ │ │ │ +
40SO<N> SO<N>::ChartAtOrigin::Retract(const TangentVector& xi, ChartJacobian H) {
│ │ │ │ +
41 if (H) throw std::runtime_error("SO<N>::Retract jacobian not implemented.");
│ │ │ │ +
42 const Matrix X = Hat(xi / 2.0);
│ │ │ │ +
43 size_t n = AmbientDim(xi.size());
│ │ │ │ +
44 const auto I = Eigen::MatrixXd::Identity(n, n);
│ │ │ │ +
45 // https://pdfs.semanticscholar.org/6165/0347b2ccac34b5f423081d1ce4dbc4d09475.pdf
│ │ │ │ +
46 return SO((I + X) * (I - X).inverse());
│ │ │ │ +
47}
│ │ │ │ +
│ │ │ │ +
48
│ │ │ │ +
49template <int N>
│ │ │ │ +
│ │ │ │ +
50typename SO<N>::TangentVector SO<N>::ChartAtOrigin::Local(const SO& R,
│ │ │ │ +
51 ChartJacobian H) {
│ │ │ │ +
52 if (H) throw std::runtime_error("SO<N>::Local jacobian not implemented.");
│ │ │ │ +
53 const size_t n = R.rows();
│ │ │ │ +
54 const auto I = Eigen::MatrixXd::Identity(n, n);
│ │ │ │ +
55 const Matrix X = (I - R.matrix_) * (I + R.matrix_).inverse();
│ │ │ │ +
56 return -2 * Vee(X);
│ │ │ │ +
57}
│ │ │ │ +
│ │ │ │ +
58
│ │ │ │ +
59template <int N>
│ │ │ │ +
60typename SO<N>::MatrixDD SO<N>::AdjointMap() const {
│ │ │ │ +
61 if (N==2) return I_1x1; // SO(2) case
│ │ │ │ +
62 throw std::runtime_error(
│ │ │ │ +
63 "SO<N>::AdjointMap only implemented for SO2, SO3 and SO4.");
│ │ │ │ +
64}
│ │ │ │ +
65
│ │ │ │ +
66template <int N>
│ │ │ │ +
│ │ │ │ +
67SO<N> SO<N>::Expmap(const TangentVector& omega, ChartJacobian H) {
│ │ │ │ +
68 throw std::runtime_error("SO<N>::Expmap only implemented for SO3 and SO4.");
│ │ │ │ +
69}
│ │ │ │ +
│ │ │ │
70
│ │ │ │ -
74
│ │ │ │ -
75 Cal3DS2_Base(const Vector9& v)
│ │ │ │ -
76 : Cal3(v(0), v(1), v(2), v(3), v(4)),
│ │ │ │ -
77 k1_(v(5)),
│ │ │ │ -
78 k2_(v(6)),
│ │ │ │ -
79 p1_(v(7)),
│ │ │ │ -
80 p2_(v(8)) {}
│ │ │ │ -
81
│ │ │ │ +
71template <int N>
│ │ │ │ +
│ │ │ │ +
72typename SO<N>::MatrixDD SO<N>::ExpmapDerivative(const TangentVector& omega) {
│ │ │ │ +
73 throw std::runtime_error("SO<N>::ExpmapDerivative only implemented for SO3.");
│ │ │ │ +
74}
│ │ │ │ +
│ │ │ │ +
75
│ │ │ │ +
76template <int N>
│ │ │ │ +
│ │ │ │ +
77typename SO<N>::TangentVector SO<N>::Logmap(const SO& R, ChartJacobian H) {
│ │ │ │ +
78 throw std::runtime_error("SO<N>::Logmap only implemented for SO3.");
│ │ │ │ +
79}
│ │ │ │ +
│ │ │ │ +
80
│ │ │ │ +
81template <int N>
│ │ │ │ +
│ │ │ │ +
82typename SO<N>::MatrixDD SO<N>::LogmapDerivative(const TangentVector& omega) {
│ │ │ │ +
83 throw std::runtime_error("O<N>::LogmapDerivative only implemented for SO3.");
│ │ │ │ +
84}
│ │ │ │ +
│ │ │ │
85
│ │ │ │ -
87 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
│ │ │ │ -
88 const Cal3DS2_Base& cal);
│ │ │ │ -
89
│ │ │ │ -
91 void print(const std::string& s = "") const override;
│ │ │ │ +
86// Default fixed size version (but specialized elsewehere for N=2,3,4)
│ │ │ │ +
87template <int N>
│ │ │ │ +
│ │ │ │ +
88typename SO<N>::VectorN2 SO<N>::vec(
│ │ │ │ +
89 OptionalJacobian<internal::NSquaredSO(N), dimension> H) const {
│ │ │ │ +
90 // Vectorize
│ │ │ │ +
91 VectorN2 X = Eigen::Map<const VectorN2>(matrix_.data());
│ │ │ │
92
│ │ │ │ -
94 bool equals(const Cal3DS2_Base& K, double tol = 1e-8) const;
│ │ │ │ -
95
│ │ │ │ -
99
│ │ │ │ -
101 inline double k1() const { return k1_; }
│ │ │ │ -
102
│ │ │ │ -
104 inline double k2() const { return k2_; }
│ │ │ │ -
105
│ │ │ │ -
107 inline double p1() const { return p1_; }
│ │ │ │ -
108
│ │ │ │ -
110 inline double p2() const { return p2_; }
│ │ │ │ +
93 // If requested, calculate H as (I \oplus Q) * P,
│ │ │ │ +
94 // where Q is the N*N rotation matrix, and P is calculated below.
│ │ │ │ +
95 if (H) {
│ │ │ │ +
96 // Calculate P matrix of vectorized generators
│ │ │ │ +
97 // TODO(duy): Should we refactor this as the jacobian of Hat?
│ │ │ │ +
98 Matrix P = SO<N>::VectorizedGenerators();
│ │ │ │ +
99 for (size_t i = 0; i < N; i++) {
│ │ │ │ +
100 H->block(i * N, 0, N, dimension) =
│ │ │ │ +
101 matrix_ * P.block(i * N, 0, N, dimension);
│ │ │ │ +
102 }
│ │ │ │ +
103 }
│ │ │ │ +
104 return X;
│ │ │ │ +
105}
│ │ │ │ +
│ │ │ │ +
106
│ │ │ │ +
107template <int N>
│ │ │ │ +
108void SO<N>::print(const std::string& s) const {
│ │ │ │ +
109 std::cout << s << matrix_ << std::endl;
│ │ │ │ +
110}
│ │ │ │
111
│ │ │ │ -
113 Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); }
│ │ │ │ -
114
│ │ │ │ -
116 Vector9 vector() const;
│ │ │ │ -
117
│ │ │ │ -
125 Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
│ │ │ │ -
126 OptionalJacobian<2, 2> Dp = boost::none) const;
│ │ │ │ -
127
│ │ │ │ -
129 Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
│ │ │ │ -
130 OptionalJacobian<2, 2> Dp = boost::none) const;
│ │ │ │ -
131
│ │ │ │ -
133 Matrix2 D2d_intrinsic(const Point2& p) const;
│ │ │ │ -
134
│ │ │ │ -
136 Matrix29 D2d_calibration(const Point2& p) const;
│ │ │ │ -
137
│ │ │ │ -
139 size_t dim() const override { return Dim(); }
│ │ │ │ -
140
│ │ │ │ -
142 inline static size_t Dim() { return dimension; }
│ │ │ │ -
143
│ │ │ │ -
147
│ │ │ │ -
│ │ │ │ -
149 virtual boost::shared_ptr<Cal3DS2_Base> clone() const {
│ │ │ │ -
150 return boost::shared_ptr<Cal3DS2_Base>(new Cal3DS2_Base(*this));
│ │ │ │ -
151 }
│ │ │ │ -
│ │ │ │ -
152
│ │ │ │ -
154
│ │ │ │ -
155 private:
│ │ │ │ -
158
│ │ │ │ -
160 friend class boost::serialization::access;
│ │ │ │ -
161 template <class Archive>
│ │ │ │ -
162 void serialize(Archive& ar, const unsigned int /*version*/) {
│ │ │ │ -
163 ar& boost::serialization::make_nvp(
│ │ │ │ -
164 "Cal3DS2_Base", boost::serialization::base_object<Cal3>(*this));
│ │ │ │ -
165 ar& BOOST_SERIALIZATION_NVP(k1_);
│ │ │ │ -
166 ar& BOOST_SERIALIZATION_NVP(k2_);
│ │ │ │ -
167 ar& BOOST_SERIALIZATION_NVP(p1_);
│ │ │ │ -
168 ar& BOOST_SERIALIZATION_NVP(p2_);
│ │ │ │ -
169 ar& BOOST_SERIALIZATION_NVP(tol_);
│ │ │ │ -
170 }
│ │ │ │ -
171
│ │ │ │ -
173};
│ │ │ │ -
│ │ │ │ -
174}
│ │ │ │ -
Cal3.h
Common code for all Calibration models.
│ │ │ │ -
Point2.h
2D Point
│ │ │ │ +
112} // namespace gtsam
│ │ │ │ +
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
gtsam::Cal3
Common base class for all calibration models.
Definition Cal3.h:69
│ │ │ │ -
gtsam::Cal3DS2_Base
Calibration of a camera with radial distortion.
Definition Cal3DS2_Base.h:42
│ │ │ │ -
gtsam::Cal3DS2_Base::Cal3DS2_Base
Cal3DS2_Base()=default
Default Constructor with only unit focal length.
│ │ │ │ -
gtsam::Cal3DS2_Base::p2
double p2() const
Second tangential distortion coefficient.
Definition Cal3DS2_Base.h:110
│ │ │ │ -
gtsam::Cal3DS2_Base::Dim
static size_t Dim()
return DOF, dimensionality of tangent space
Definition Cal3DS2_Base.h:142
│ │ │ │ -
gtsam::Cal3DS2_Base::clone
virtual boost::shared_ptr< Cal3DS2_Base > clone() const
Definition Cal3DS2_Base.h:149
│ │ │ │ -
gtsam::Cal3DS2_Base::k2
double k2() const
Second distortion coefficient.
Definition Cal3DS2_Base.h:104
│ │ │ │ -
gtsam::Cal3DS2_Base::k1
double k1() const
First distortion coefficient.
Definition Cal3DS2_Base.h:101
│ │ │ │ -
gtsam::Cal3DS2_Base::dim
size_t dim() const override
return DOF, dimensionality of tangent space
Definition Cal3DS2_Base.h:139
│ │ │ │ -
gtsam::Cal3DS2_Base::p1
double p1() const
First tangential distortion coefficient.
Definition Cal3DS2_Base.h:107
│ │ │ │ -
gtsam::Cal3DS2_Base::k
Vector4 k() const
return distortion parameter vector
Definition Cal3DS2_Base.h:113
│ │ │ │ +
gtsam::SO
Manifold of special orthogonal rotation matrices SO<N>.
Definition SOn.h:52
│ │ │ │ +
gtsam::SO::Expmap
static SO Expmap(const TangentVector &omega, ChartJacobian H=boost::none)
Exponential map at identity - create a rotation from canonical coordinates.
Definition SOn-inl.h:67
│ │ │ │ +
gtsam::SO::VectorizedGenerators
static Matrix VectorizedGenerators()
Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N)
Definition SOn.h:300
│ │ │ │ +
gtsam::SO::inverse
SO inverse() const
inverse of a rotation = transpose
Definition SOn.h:193
│ │ │ │ +
gtsam::SO::vec
VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost::none) const
Return vectorized rotation matrix in column order.
Definition SOn-inl.h:88
│ │ │ │ +
gtsam::SO::Vee
static TangentVector Vee(const MatrixNN &X)
Inverse of Hat. See note about xi element order in Hat.
Definition SOn-inl.h:35
│ │ │ │ +
gtsam::SO::matrix_
MatrixNN matrix_
Rotation matrix.
Definition SOn.h:62
│ │ │ │ +
gtsam::SO::Logmap
static TangentVector Logmap(const SO &R, ChartJacobian H=boost::none)
Log map at identity - returns the canonical coordinates of this rotation.
Definition SOn-inl.h:77
│ │ │ │ +
gtsam::SO::AdjointMap
MatrixDD AdjointMap() const
Adjoint map.
Definition SO4.cpp:159
│ │ │ │ +
gtsam::SO::Hat
static MatrixNN Hat(const TangentVector &xi)
Hat operator creates Lie algebra element corresponding to d-vector, where d is the dimensionality of ...
Definition SOn-inl.h:29
│ │ │ │ +
gtsam::SO::ExpmapDerivative
static MatrixDD ExpmapDerivative(const TangentVector &omega)
Derivative of Expmap, currently only defined for SO3.
Definition SOn-inl.h:72
│ │ │ │ +
gtsam::SO::LogmapDerivative
static MatrixDD LogmapDerivative(const TangentVector &omega)
Derivative of Logmap, currently only defined for SO3.
Definition SOn-inl.h:82
│ │ │ │ +
gtsam::SO::SO
SO()
Construct SO<N> identity for N >= 2.
Definition SOn.h:79
│ │ │ │ +
gtsam::SO::ChartAtOrigin::Local
static TangentVector Local(const SO &R, ChartJacobian H=boost::none)
Inverse of Retract.
Definition SOn-inl.h:50
│ │ │ │ +
gtsam::SO::ChartAtOrigin::Retract
static SO Retract(const TangentVector &xi, ChartJacobian H=boost::none)
Retract uses Cayley map.
Definition SOn-inl.h:40
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,184 +1,194 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Cal3DS2_Base.h │ │ │ │ │ +SOn-inl.h │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ -24#include │ │ │ │ │ -25 │ │ │ │ │ -26namespace _g_t_s_a_m { │ │ │ │ │ -27 │ │ │ │ │ -_4_2class GTSAM_EXPORT _C_a_l_3_D_S_2___B_a_s_e : public _C_a_l_3 { │ │ │ │ │ -43 protected: │ │ │ │ │ -_4_4 double k1_ = 0.0f, k2_ = 0.0f; │ │ │ │ │ -_4_5 double p1_ = 0.0f, p2_ = 0.0f; │ │ │ │ │ -_4_6 double tol_ = 1e-5; │ │ │ │ │ -47 │ │ │ │ │ -48 public: │ │ │ │ │ -49 enum { dimension = 9 }; │ │ │ │ │ -50 │ │ │ │ │ -52 using shared_ptr = boost::shared_ptr; │ │ │ │ │ -53 │ │ │ │ │ -56 │ │ │ │ │ -_5_8 _C_a_l_3_D_S_2___B_a_s_e() = default; │ │ │ │ │ -59 │ │ │ │ │ -60 _C_a_l_3_D_S_2___B_a_s_e(double fx, double fy, double s, double u0, double v0, double │ │ │ │ │ -k1, │ │ │ │ │ -61 double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5) │ │ │ │ │ -62 : _C_a_l_3(fx, fy, s, u0, v0), │ │ │ │ │ -63 k1_(k1), │ │ │ │ │ -64 k2_(k2), │ │ │ │ │ -65 p1_(p1), │ │ │ │ │ -66 p2_(p2), │ │ │ │ │ -67 tol_(tol) {} │ │ │ │ │ -68 │ │ │ │ │ -69 _~_C_a_l_3_D_S_2___B_a_s_e() override {} │ │ │ │ │ +12#pragma once │ │ │ │ │ +13 │ │ │ │ │ +21#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ +22 │ │ │ │ │ +23#include │ │ │ │ │ +24 │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ +26 │ │ │ │ │ +27// Implementation for N>=5 just uses dynamic version │ │ │ │ │ +28template │ │ │ │ │ +_2_9typename SO::MatrixNN _S_O_<_N_>_:_:_H_a_t(const TangentVector& xi) { │ │ │ │ │ +30 return _S_O_n_:_:_H_a_t(xi); │ │ │ │ │ +31} │ │ │ │ │ +32 │ │ │ │ │ +33// Implementation for N>=5 just uses dynamic version │ │ │ │ │ +34template │ │ │ │ │ +_3_5typename SO::TangentVector _S_O_<_N_>_:_:_V_e_e(const MatrixNN& X) { │ │ │ │ │ +36 return _S_O_n_:_:_V_e_e(X); │ │ │ │ │ +37} │ │ │ │ │ +38 │ │ │ │ │ +39template │ │ │ │ │ +_4_0_S_O_<_N_> _S_O_<_N_>_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n_:_:_R_e_t_r_a_c_t(const TangentVector& xi, _C_h_a_r_t_J_a_c_o_b_i_a_n H) │ │ │ │ │ +{ │ │ │ │ │ +41 if (H) throw std::runtime_error("SO::Retract jacobian not implemented."); │ │ │ │ │ +42 const Matrix X = _H_a_t(xi / 2.0); │ │ │ │ │ +43 size_t n = AmbientDim(xi.size()); │ │ │ │ │ +44 const auto I = Eigen::MatrixXd::Identity(n, n); │ │ │ │ │ +45 // https://pdfs.semanticscholar.org/6165/ │ │ │ │ │ +0347b2ccac34b5f423081d1ce4dbc4d09475.pdf │ │ │ │ │ +46 return _S_O((I + X) * (I - X)._i_n_v_e_r_s_e()); │ │ │ │ │ +47} │ │ │ │ │ +48 │ │ │ │ │ +49template │ │ │ │ │ +_5_0typename SO::TangentVector _S_O_<_N_>_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n_:_:_L_o_c_a_l(const _S_O& R, │ │ │ │ │ +51 _C_h_a_r_t_J_a_c_o_b_i_a_n H) { │ │ │ │ │ +52 if (H) throw std::runtime_error("SO::Local jacobian not implemented."); │ │ │ │ │ +53 const size_t n = R.rows(); │ │ │ │ │ +54 const auto I = Eigen::MatrixXd::Identity(n, n); │ │ │ │ │ +55 const Matrix X = (I - R.matrix_) * (I + R.matrix_).inverse(); │ │ │ │ │ +56 return -2 * _V_e_e(X); │ │ │ │ │ +57} │ │ │ │ │ +58 │ │ │ │ │ +59template │ │ │ │ │ +60typename SO::MatrixDD _S_O_<_N_>_:_:_A_d_j_o_i_n_t_M_a_p() const { │ │ │ │ │ +61 if (N==2) return I_1x1; // SO(2) case │ │ │ │ │ +62 throw std::runtime_error( │ │ │ │ │ +63 "SO::AdjointMap only implemented for SO2, SO3 and SO4."); │ │ │ │ │ +64} │ │ │ │ │ +65 │ │ │ │ │ +66template │ │ │ │ │ +_6_7_S_O_<_N_> _S_O_<_N_>_:_:_E_x_p_m_a_p(const TangentVector& omega, _C_h_a_r_t_J_a_c_o_b_i_a_n H) { │ │ │ │ │ +68 throw std::runtime_error("SO::Expmap only implemented for SO3 and SO4."); │ │ │ │ │ +69} │ │ │ │ │ 70 │ │ │ │ │ -74 │ │ │ │ │ -75 Cal3DS2_Base(const Vector9& v) │ │ │ │ │ -76 : Cal3(v(0), v(1), v(2), v(3), v(4)), │ │ │ │ │ -77 k1_(v(5)), │ │ │ │ │ -78 k2_(v(6)), │ │ │ │ │ -79 p1_(v(7)), │ │ │ │ │ -80 p2_(v(8)) {} │ │ │ │ │ -81 │ │ │ │ │ +71template │ │ │ │ │ +_7_2typename SO::MatrixDD _S_O_<_N_>_:_:_E_x_p_m_a_p_D_e_r_i_v_a_t_i_v_e(const TangentVector& omega) │ │ │ │ │ +{ │ │ │ │ │ +73 throw std::runtime_error("SO::ExpmapDerivative only implemented for │ │ │ │ │ +SO3."); │ │ │ │ │ +74} │ │ │ │ │ +75 │ │ │ │ │ +76template │ │ │ │ │ +_7_7typename SO::TangentVector _S_O_<_N_>_:_:_L_o_g_m_a_p(const _S_O& R, _C_h_a_r_t_J_a_c_o_b_i_a_n H) { │ │ │ │ │ +78 throw std::runtime_error("SO::Logmap only implemented for SO3."); │ │ │ │ │ +79} │ │ │ │ │ +80 │ │ │ │ │ +81template │ │ │ │ │ +_8_2typename SO::MatrixDD _S_O_<_N_>_:_:_L_o_g_m_a_p_D_e_r_i_v_a_t_i_v_e(const TangentVector& omega) │ │ │ │ │ +{ │ │ │ │ │ +83 throw std::runtime_error("O::LogmapDerivative only implemented for │ │ │ │ │ +SO3."); │ │ │ │ │ +84} │ │ │ │ │ 85 │ │ │ │ │ -87 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, │ │ │ │ │ -88 const Cal3DS2_Base& cal); │ │ │ │ │ -89 │ │ │ │ │ -91 void _p_r_i_n_t(const std::string& s = "") const override; │ │ │ │ │ +86// Default fixed size version (but specialized elsewehere for N=2,3,4) │ │ │ │ │ +87template │ │ │ │ │ +_8_8typename SO::VectorN2 _S_O_<_N_>_:_:_v_e_c( │ │ │ │ │ +89 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n H) const { │ │ │ │ │ +90 // Vectorize │ │ │ │ │ +91 VectorN2 X = Eigen::Map(_m_a_t_r_i_x__.data()); │ │ │ │ │ 92 │ │ │ │ │ -94 bool equals(const Cal3DS2_Base& K, double tol = 1e-8) const; │ │ │ │ │ -95 │ │ │ │ │ -99 │ │ │ │ │ -_1_0_1 inline double _k_1() const { return k1_; } │ │ │ │ │ -102 │ │ │ │ │ -_1_0_4 inline double _k_2() const { return k2_; } │ │ │ │ │ -105 │ │ │ │ │ -_1_0_7 inline double _p_1() const { return p1_; } │ │ │ │ │ -108 │ │ │ │ │ -_1_1_0 inline double _p_2() const { return p2_; } │ │ │ │ │ +93 // If requested, calculate H as (I \oplus Q) * P, │ │ │ │ │ +94 // where Q is the N*N rotation matrix, and P is calculated below. │ │ │ │ │ +95 if (H) { │ │ │ │ │ +96 // Calculate P matrix of vectorized generators │ │ │ │ │ +97 // TODO(duy): Should we refactor this as the jacobian of Hat? │ │ │ │ │ +98 Matrix P = _S_O_<_N_>_:_:_V_e_c_t_o_r_i_z_e_d_G_e_n_e_r_a_t_o_r_s(); │ │ │ │ │ +99 for (size_t i = 0; i < N; i++) { │ │ │ │ │ +100 H->block(i * N, 0, N, dimension) = │ │ │ │ │ +101 _m_a_t_r_i_x__ * P.block(i * N, 0, N, dimension); │ │ │ │ │ +102 } │ │ │ │ │ +103 } │ │ │ │ │ +104 return X; │ │ │ │ │ +105} │ │ │ │ │ +106 │ │ │ │ │ +107template │ │ │ │ │ +108void _S_O_<_N_>_:_:_p_r_i_n_t(const std::string& s) const { │ │ │ │ │ +109 std::cout << s << _m_a_t_r_i_x__ << std::endl; │ │ │ │ │ +110} │ │ │ │ │ 111 │ │ │ │ │ -_1_1_3 Vector4 _k() const { return Vector4(k1_, k2_, p1_, p2_); } │ │ │ │ │ -114 │ │ │ │ │ -116 Vector9 vector() const; │ │ │ │ │ -117 │ │ │ │ │ -125 _P_o_i_n_t_2 uncalibrate(const _P_o_i_n_t_2& p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _9_> Dcal = boost:: │ │ │ │ │ -none, │ │ │ │ │ -126 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dp = boost::none) const; │ │ │ │ │ -127 │ │ │ │ │ -129 _P_o_i_n_t_2 calibrate(const _P_o_i_n_t_2& p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _9_> Dcal = boost:: │ │ │ │ │ -none, │ │ │ │ │ -130 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dp = boost::none) const; │ │ │ │ │ -131 │ │ │ │ │ -133 Matrix2 D2d_intrinsic(const _P_o_i_n_t_2& p) const; │ │ │ │ │ -134 │ │ │ │ │ -136 Matrix29 D2d_calibration(const _P_o_i_n_t_2& p) const; │ │ │ │ │ -137 │ │ │ │ │ -_1_3_9 size_t _d_i_m() const override { return Dim(); } │ │ │ │ │ -140 │ │ │ │ │ -_1_4_2 inline static size_t _D_i_m() { return dimension; } │ │ │ │ │ -143 │ │ │ │ │ -147 │ │ │ │ │ -_1_4_9 virtual boost::shared_ptr _c_l_o_n_e() const { │ │ │ │ │ -150 return boost::shared_ptr(new _C_a_l_3_D_S_2___B_a_s_e(*this)); │ │ │ │ │ -151 } │ │ │ │ │ -152 │ │ │ │ │ -154 │ │ │ │ │ -155 private: │ │ │ │ │ -158 │ │ │ │ │ -_1_6_0 friend class boost::serialization::access; │ │ │ │ │ -161 template │ │ │ │ │ -162 void serialize(Archive& ar, const unsigned int /*version*/) { │ │ │ │ │ -163 ar& boost::serialization::make_nvp( │ │ │ │ │ -164 "Cal3DS2_Base", boost::serialization::base_object(*this)); │ │ │ │ │ -165 ar& BOOST_SERIALIZATION_NVP(k1_); │ │ │ │ │ -166 ar& BOOST_SERIALIZATION_NVP(k2_); │ │ │ │ │ -167 ar& BOOST_SERIALIZATION_NVP(p1_); │ │ │ │ │ -168 ar& BOOST_SERIALIZATION_NVP(p2_); │ │ │ │ │ -169 ar& BOOST_SERIALIZATION_NVP(tol_); │ │ │ │ │ -170 } │ │ │ │ │ -171 │ │ │ │ │ -173}; │ │ │ │ │ -174} │ │ │ │ │ -_C_a_l_3_._h │ │ │ │ │ -Common code for all Calibration models. │ │ │ │ │ -_P_o_i_n_t_2_._h │ │ │ │ │ -2D Point │ │ │ │ │ +112} // namespace gtsam │ │ │ │ │ +_M_a_t_r_i_x_._h │ │ │ │ │ +typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ -Vector2 Point2 │ │ │ │ │ -As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point2 to Vector2... │ │ │ │ │ -DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ _g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ either a fixed size o... │ │ │ │ │ DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3 │ │ │ │ │ -Common base class for all calibration models. │ │ │ │ │ -DDeeffiinniittiioonn Cal3.h:69 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_D_S_2___B_a_s_e │ │ │ │ │ -Calibration of a camera with radial distortion. │ │ │ │ │ -DDeeffiinniittiioonn Cal3DS2_Base.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_D_S_2___B_a_s_e_:_:_C_a_l_3_D_S_2___B_a_s_e │ │ │ │ │ -Cal3DS2_Base()=default │ │ │ │ │ -Default Constructor with only unit focal length. │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_D_S_2___B_a_s_e_:_:_p_2 │ │ │ │ │ -double p2() const │ │ │ │ │ -Second tangential distortion coefficient. │ │ │ │ │ -DDeeffiinniittiioonn Cal3DS2_Base.h:110 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_D_S_2___B_a_s_e_:_:_D_i_m │ │ │ │ │ -static size_t Dim() │ │ │ │ │ -return DOF, dimensionality of tangent space │ │ │ │ │ -DDeeffiinniittiioonn Cal3DS2_Base.h:142 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_D_S_2___B_a_s_e_:_:_c_l_o_n_e │ │ │ │ │ -virtual boost::shared_ptr< Cal3DS2_Base > clone() const │ │ │ │ │ -DDeeffiinniittiioonn Cal3DS2_Base.h:149 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_D_S_2___B_a_s_e_:_:_k_2 │ │ │ │ │ -double k2() const │ │ │ │ │ -Second distortion coefficient. │ │ │ │ │ -DDeeffiinniittiioonn Cal3DS2_Base.h:104 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_D_S_2___B_a_s_e_:_:_k_1 │ │ │ │ │ -double k1() const │ │ │ │ │ -First distortion coefficient. │ │ │ │ │ -DDeeffiinniittiioonn Cal3DS2_Base.h:101 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_D_S_2___B_a_s_e_:_:_d_i_m │ │ │ │ │ -size_t dim() const override │ │ │ │ │ -return DOF, dimensionality of tangent space │ │ │ │ │ -DDeeffiinniittiioonn Cal3DS2_Base.h:139 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_D_S_2___B_a_s_e_:_:_p_1 │ │ │ │ │ -double p1() const │ │ │ │ │ -First tangential distortion coefficient. │ │ │ │ │ -DDeeffiinniittiioonn Cal3DS2_Base.h:107 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3_D_S_2___B_a_s_e_:_:_k │ │ │ │ │ -Vector4 k() const │ │ │ │ │ -return distortion parameter vector │ │ │ │ │ -DDeeffiinniittiioonn Cal3DS2_Base.h:113 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O │ │ │ │ │ +Manifold of special orthogonal rotation matrices SO. │ │ │ │ │ +DDeeffiinniittiioonn SOn.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_E_x_p_m_a_p │ │ │ │ │ +static SO Expmap(const TangentVector &omega, ChartJacobian H=boost::none) │ │ │ │ │ +Exponential map at identity - create a rotation from canonical coordinates. │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_V_e_c_t_o_r_i_z_e_d_G_e_n_e_r_a_t_o_r_s │ │ │ │ │ +static Matrix VectorizedGenerators() │ │ │ │ │ +Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N) │ │ │ │ │ +DDeeffiinniittiioonn SOn.h:300 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_i_n_v_e_r_s_e │ │ │ │ │ +SO inverse() const │ │ │ │ │ +inverse of a rotation = transpose │ │ │ │ │ +DDeeffiinniittiioonn SOn.h:193 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_v_e_c │ │ │ │ │ +VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost:: │ │ │ │ │ +none) const │ │ │ │ │ +Return vectorized rotation matrix in column order. │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:88 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_V_e_e │ │ │ │ │ +static TangentVector Vee(const MatrixNN &X) │ │ │ │ │ +Inverse of Hat. See note about xi element order in Hat. │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_m_a_t_r_i_x__ │ │ │ │ │ +MatrixNN matrix_ │ │ │ │ │ +Rotation matrix. │ │ │ │ │ +DDeeffiinniittiioonn SOn.h:62 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_L_o_g_m_a_p │ │ │ │ │ +static TangentVector Logmap(const SO &R, ChartJacobian H=boost::none) │ │ │ │ │ +Log map at identity - returns the canonical coordinates of this rotation. │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_A_d_j_o_i_n_t_M_a_p │ │ │ │ │ +MatrixDD AdjointMap() const │ │ │ │ │ +Adjoint map. │ │ │ │ │ +DDeeffiinniittiioonn SO4.cpp:159 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_H_a_t │ │ │ │ │ +static MatrixNN Hat(const TangentVector &xi) │ │ │ │ │ +Hat operator creates Lie algebra element corresponding to d-vector, where d is │ │ │ │ │ +the dimensionality of ... │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:29 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_E_x_p_m_a_p_D_e_r_i_v_a_t_i_v_e │ │ │ │ │ +static MatrixDD ExpmapDerivative(const TangentVector &omega) │ │ │ │ │ +Derivative of Expmap, currently only defined for SO3. │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_L_o_g_m_a_p_D_e_r_i_v_a_t_i_v_e │ │ │ │ │ +static MatrixDD LogmapDerivative(const TangentVector &omega) │ │ │ │ │ +Derivative of Logmap, currently only defined for SO3. │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:82 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_S_O │ │ │ │ │ +SO() │ │ │ │ │ +Construct SO identity for N >= 2. │ │ │ │ │ +DDeeffiinniittiioonn SOn.h:79 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n_:_:_L_o_c_a_l │ │ │ │ │ +static TangentVector Local(const SO &R, ChartJacobian H=boost::none) │ │ │ │ │ +Inverse of Retract. │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n_:_:_R_e_t_r_a_c_t │ │ │ │ │ +static SO Retract(const TangentVector &xi, ChartJacobian H=boost::none) │ │ │ │ │ +Retract uses Cayley map. │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:40 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * CCaall33DDSS22__BBaassee..hh │ │ │ │ │ + * _S_O_n_-_i_n_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00455.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00419.html │ │ │ │┄ Files 98% similar despite different names │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │

Detailed Description

│ │ │ │

3D Pose

│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00458.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00464.html │ │ │ │┄ Files 92% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Similarity3.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/CameraSet.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
Similarity3.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
CameraSet.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Implementation of Similarity3 transform. │ │ │ │ +

Base class to create smart factors on poses or cameras. │ │ │ │ More...

│ │ │ │ │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::Similarity3
 3D similarity transform More...
class  gtsam::CameraSet< CAMERA >
 A set of cameras, all with their own calibration. More...
 
struct  gtsam::Similarity3::ChartAtOrigin
 Chart at the origin. More...
struct  gtsam::traits< CameraSet< CAMERA > >
 
struct  gtsam::traits< Similarity3 >
 
struct  gtsam::traits< const Similarity3 >
struct  gtsam::traits< const CameraSet< CAMERA > >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -template<>
Matrix gtsam::wedge< Similarity3 > (const Vector &xi)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Implementation of Similarity3 transform.

│ │ │ │ -
Author
Paul Drews
│ │ │ │ -
│ │ │ │ -John Lambert
│ │ │ │ +

Base class to create smart factors on poses or cameras.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
Date
Feb 19, 2015
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,37 +1,31 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Similarity3.h File Reference │ │ │ │ │ -Implementation of Similarity3 transform. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +CameraSet.h File Reference │ │ │ │ │ +Base class to create smart factors on poses or cameras. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_3 │ │ │ │ │ -  3D similarity transform _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t_<_ _C_A_M_E_R_A_ _> │ │ │ │ │ +  A set of cameras, all with their own calibration. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_3_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n │ │ │ │ │ -  Chart at the origin. _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _C_a_m_e_r_a_S_e_t_<_ _C_A_M_E_R_A_ _>_ _> │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_i_m_i_l_a_r_i_t_y_3_ _> │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _S_i_m_i_l_a_r_i_t_y_3_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _C_a_m_e_r_a_S_e_t_<_ _C_A_M_E_R_A_ _>_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -template<> │ │ │ │ │ - Matrix  ggttssaamm::::wweeddggee<< SSiimmiillaarriittyy33 >> (const Vector &xi) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Implementation of Similarity3 transform. │ │ │ │ │ +Base class to create smart factors on poses or cameras. │ │ │ │ │ Author │ │ │ │ │ - Paul Drews │ │ │ │ │ - John Lambert │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + Feb 19, 2015 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_i_m_i_l_a_r_i_t_y_3_._h │ │ │ │ │ + * _C_a_m_e_r_a_S_e_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00458_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00569_source.html │ │ │ │┄ Files 92% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Similarity3.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridBayesNet.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Similarity3.h
│ │ │ │ +
HybridBayesNet.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ -
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ -
4 * Atlanta, Georgia 30332-0415
│ │ │ │ -
5 * All Rights Reserved
│ │ │ │ -
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ -
7
│ │ │ │ -
8 * See LICENSE for the license information
│ │ │ │ -
9
│ │ │ │ -
10 * -------------------------------------------------------------------------- */
│ │ │ │ -
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/base/Lie.h>
│ │ │ │ -
22#include <gtsam/base/Manifold.h>
│ │ │ │ -
23#include <gtsam/dllexport.h>
│ │ │ │ -
24#include <gtsam/geometry/Point3.h>
│ │ │ │ -
25#include <gtsam/geometry/Pose3.h>
│ │ │ │ -
26#include <gtsam/geometry/Rot3.h>
│ │ │ │ -
27
│ │ │ │ -
28namespace gtsam {
│ │ │ │ -
29
│ │ │ │ -
30// Forward declarations
│ │ │ │ -
31class Pose3;
│ │ │ │ -
32
│ │ │ │ -
│ │ │ │ -
36class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
│ │ │ │ -
39 typedef Rot3 Rotation;
│ │ │ │ -
40 typedef Point3 Translation;
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +
2 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
3 * Atlanta, Georgia 30332-0415
│ │ │ │ +
4 * All Rights Reserved
│ │ │ │ +
5 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
6 * See LICENSE for the license information
│ │ │ │ +
7 * -------------------------------------------------------------------------- */
│ │ │ │ +
8
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/discrete/DecisionTreeFactor.h>
│ │ │ │ +
21#include <gtsam/global_includes.h>
│ │ │ │ +
22#include <gtsam/hybrid/HybridConditional.h>
│ │ │ │ +
23#include <gtsam/hybrid/HybridValues.h>
│ │ │ │ +
24#include <gtsam/inference/BayesNet.h>
│ │ │ │ +
25#include <gtsam/linear/GaussianBayesNet.h>
│ │ │ │ +
26
│ │ │ │ +
27namespace gtsam {
│ │ │ │ +
28
│ │ │ │ +
│ │ │ │ +
35class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
│ │ │ │ +
36 public:
│ │ │ │ +
37 using Base = BayesNet<HybridConditional>;
│ │ │ │ +
38 using This = HybridBayesNet;
│ │ │ │ +
39 using ConditionalType = HybridConditional;
│ │ │ │ +
40 using shared_ptr = boost::shared_ptr<HybridBayesNet>;
│ │ │ │ +
41 using sharedConditional = boost::shared_ptr<ConditionalType>;
│ │ │ │
42
│ │ │ │ -
43 private:
│ │ │ │ -
44 Rot3 R_;
│ │ │ │ -
45 Point3 t_;
│ │ │ │ -
46 double s_;
│ │ │ │ -
47
│ │ │ │ -
48 public:
│ │ │ │ -
51
│ │ │ │ -
53 Similarity3();
│ │ │ │ -
54
│ │ │ │ -
56 Similarity3(double s);
│ │ │ │ -
57
│ │ │ │ -
59 Similarity3(const Rot3& R, const Point3& t, double s);
│ │ │ │ -
60
│ │ │ │ -
62 Similarity3(const Matrix3& R, const Vector3& t, double s);
│ │ │ │ +
45
│ │ │ │ +
47 HybridBayesNet() = default;
│ │ │ │ +
48
│ │ │ │ +
52
│ │ │ │ +
54 void print(const std::string &s = "", const KeyFormatter &formatter =
│ │ │ │ +
55 DefaultKeyFormatter) const override;
│ │ │ │ +
56
│ │ │ │ +
58 bool equals(const This &fg, double tol = 1e-9) const;
│ │ │ │ +
59
│ │ │ │
63
│ │ │ │ -
65 Similarity3(const Matrix4& T);
│ │ │ │ -
66
│ │ │ │ -
70
│ │ │ │ -
72 bool equals(const Similarity3& sim, double tol) const;
│ │ │ │ -
73
│ │ │ │ -
75 bool operator==(const Similarity3& other) const;
│ │ │ │ -
76
│ │ │ │ -
78 void print(const std::string& s) const;
│ │ │ │ -
79
│ │ │ │ -
80 friend std::ostream& operator<<(std::ostream& os, const Similarity3& p);
│ │ │ │ -
81
│ │ │ │ -
85
│ │ │ │ -
87 static Similarity3 Identity();
│ │ │ │ -
88
│ │ │ │ -
90 Similarity3 operator*(const Similarity3& S) const;
│ │ │ │ -
91
│ │ │ │ -
93 Similarity3 inverse() const;
│ │ │ │ -
94
│ │ │ │ -
98
│ │ │ │ -
100 Point3 transformFrom(const Point3& p, //
│ │ │ │ -
101 OptionalJacobian<3, 7> H1 = boost::none, //
│ │ │ │ -
102 OptionalJacobian<3, 3> H2 = boost::none) const;
│ │ │ │ +
│ │ │ │ +
69 void push_back(boost::shared_ptr<HybridConditional> conditional) {
│ │ │ │ +
70 factors_.push_back(conditional);
│ │ │ │ +
71 }
│ │ │ │ +
│ │ │ │ +
72
│ │ │ │ +
81 template <class Conditional>
│ │ │ │ +
│ │ │ │ +
82 void emplace_back(Conditional *conditional) {
│ │ │ │ +
83 factors_.push_back(boost::make_shared<HybridConditional>(
│ │ │ │ +
84 boost::shared_ptr<Conditional>(conditional)));
│ │ │ │ +
85 }
│ │ │ │ +
│ │ │ │ +
86
│ │ │ │ +
│ │ │ │ +
99 void push_back(HybridConditional &&conditional) {
│ │ │ │ +
100 factors_.push_back(
│ │ │ │ +
101 boost::make_shared<HybridConditional>(std::move(conditional)));
│ │ │ │ +
102 }
│ │ │ │ +
│ │ │ │
103
│ │ │ │ -
115 Pose3 transformFrom(const Pose3& T) const;
│ │ │ │ -
116
│ │ │ │ -
118 Point3 operator*(const Point3& p) const;
│ │ │ │ -
119
│ │ │ │ -
123 static Similarity3 Align(const Point3Pairs& abPointPairs);
│ │ │ │ -
124
│ │ │ │ -
135 static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);
│ │ │ │ -
136
│ │ │ │ -
140
│ │ │ │ -
144 static Vector7 Logmap(const Similarity3& s, //
│ │ │ │ -
145 OptionalJacobian<7, 7> Hm = boost::none);
│ │ │ │ -
146
│ │ │ │ -
149 static Similarity3 Expmap(const Vector7& v, //
│ │ │ │ -
150 OptionalJacobian<7, 7> Hm = boost::none);
│ │ │ │ -
151
│ │ │ │ -
│ │ │ │ -
153 struct ChartAtOrigin {
│ │ │ │ -
154 static Similarity3 Retract(const Vector7& v,
│ │ │ │ -
155 ChartJacobian H = boost::none) {
│ │ │ │ -
156 return Similarity3::Expmap(v, H);
│ │ │ │ -
157 }
│ │ │ │ -
158 static Vector7 Local(const Similarity3& other,
│ │ │ │ -
159 ChartJacobian H = boost::none) {
│ │ │ │ -
160 return Similarity3::Logmap(other, H);
│ │ │ │ -
161 }
│ │ │ │ -
162 };
│ │ │ │ -
│ │ │ │ -
163
│ │ │ │ -
164 using LieGroup<Similarity3, 7>::inverse;
│ │ │ │ -
165
│ │ │ │ -
172 static Matrix4 wedge(const Vector7& xi);
│ │ │ │ -
173
│ │ │ │ -
175 Matrix7 AdjointMap() const;
│ │ │ │ -
176
│ │ │ │ -
180
│ │ │ │ -
182 Matrix4 matrix() const;
│ │ │ │ -
183
│ │ │ │ -
185 Rot3 rotation() const { return R_; }
│ │ │ │ +
111 GaussianBayesNet choose(const DiscreteValues &assignment) const;
│ │ │ │ +
112
│ │ │ │ +
114 double evaluate(const HybridValues &values) const;
│ │ │ │ +
115
│ │ │ │ +
│ │ │ │ +
117 double operator()(const HybridValues &values) const {
│ │ │ │ +
118 return evaluate(values);
│ │ │ │ +
119 }
│ │ │ │ +
│ │ │ │ +
120
│ │ │ │ +
128 HybridValues optimize() const;
│ │ │ │ +
129
│ │ │ │ +
137 VectorValues optimize(const DiscreteValues &assignment) const;
│ │ │ │ +
138
│ │ │ │ +
144 DecisionTreeFactor::shared_ptr discreteConditionals() const;
│ │ │ │ +
145
│ │ │ │ +
158 HybridValues sample(const HybridValues &given, std::mt19937_64 *rng) const;
│ │ │ │ +
159
│ │ │ │ +
170 HybridValues sample(std::mt19937_64 *rng) const;
│ │ │ │ +
171
│ │ │ │ +
178 HybridValues sample(const HybridValues &given) const;
│ │ │ │ +
179
│ │ │ │ +
185 HybridValues sample() const;
│ │ │ │
186
│ │ │ │ -
188 Point3 translation() const { return t_; }
│ │ │ │ +
188 HybridBayesNet prune(size_t maxNrLeaves);
│ │ │ │
189
│ │ │ │ -
191 double scale() const { return s_; }
│ │ │ │ -
192
│ │ │ │ -
194 inline static size_t Dim() { return 7; }
│ │ │ │ -
195
│ │ │ │ -
197 inline size_t dim() const { return 7; }
│ │ │ │ -
198
│ │ │ │ -
202
│ │ │ │ -
203 private:
│ │ │ │ -
205 static Matrix3 GetV(Vector3 w, double lambda);
│ │ │ │ -
206
│ │ │ │ -
208};
│ │ │ │ -
│ │ │ │ -
209
│ │ │ │ -
210template <>
│ │ │ │ -
211inline Matrix wedge<Similarity3>(const Vector& xi) {
│ │ │ │ -
212 return Similarity3::wedge(xi);
│ │ │ │ -
213}
│ │ │ │ +
197 AlgebraicDecisionTree<Key> logProbability(
│ │ │ │ +
198 const VectorValues &continuousValues) const;
│ │ │ │ +
199
│ │ │ │ +
200 using BayesNet::logProbability; // expose HybridValues version
│ │ │ │ +
201
│ │ │ │ +
212 AlgebraicDecisionTree<Key> evaluate(
│ │ │ │ +
213 const VectorValues &continuousValues) const;
│ │ │ │
214
│ │ │ │ -
215template <>
│ │ │ │ -
216struct traits<Similarity3> : public internal::LieGroup<Similarity3> {};
│ │ │ │ -
217
│ │ │ │ -
218template <>
│ │ │ │ -
219struct traits<const Similarity3> : public internal::LieGroup<Similarity3> {};
│ │ │ │ -
220
│ │ │ │ -
221} // namespace gtsam
│ │ │ │ -
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ -
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ -
Pose3.h
3D Pose
│ │ │ │ -
Point3.h
3D Point
│ │ │ │ -
Rot3.h
3D rotation represented as a rotation matrix or quaternion
│ │ │ │ +
219 HybridGaussianFactorGraph toFactorGraph(
│ │ │ │ +
220 const VectorValues &measurements) const;
│ │ │ │ +
222
│ │ │ │ +
223 private:
│ │ │ │ +
229 void updateDiscreteConditionals(const DecisionTreeFactor &prunedDecisionTree);
│ │ │ │ +
230
│ │ │ │ +
232 friend class boost::serialization::access;
│ │ │ │ +
233 template <class ARCHIVE>
│ │ │ │ +
234 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ +
235 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
236 }
│ │ │ │ +
237};
│ │ │ │ +
│ │ │ │ +
238
│ │ │ │ +
240template <>
│ │ │ │ +
241struct traits<HybridBayesNet> : public Testable<HybridBayesNet> {};
│ │ │ │ +
242
│ │ │ │ +
243} // namespace gtsam
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
BayesNet.h
Bayes network.
│ │ │ │ +
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
│ │ │ │ +
global_includes.h
Included from all GTSAM files.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
│ │ │ │ -
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ -
gtsam::operator==
bool operator==(const Matrix &A, const Matrix &B)
equality is just equal_with_abs_tol 1e-9
Definition Matrix.h:100
│ │ │ │ -
gtsam::wedge
Matrix wedge(const Vector &x)
Declaration of wedge (see Murray94book) used to convert from n exponential coordinates to n*n element...
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::LieGroup
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition Lie.h:37
│ │ │ │ -
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition Lie.h:229
│ │ │ │ -
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ -
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
│ │ │ │ -
gtsam::Similarity3
3D similarity transform
Definition Similarity3.h:36
│ │ │ │ -
gtsam::Similarity3::Dim
static size_t Dim()
Dimensionality of tangent space = 7 DOF - used to autodetect sizes.
Definition Similarity3.h:194
│ │ │ │ -
gtsam::Similarity3::translation
Point3 translation() const
Return a GTSAM translation.
Definition Similarity3.h:188
│ │ │ │ -
gtsam::Similarity3::rotation
Rot3 rotation() const
Return a GTSAM rotation.
Definition Similarity3.h:185
│ │ │ │ -
gtsam::Similarity3::dim
size_t dim() const
Dimensionality of tangent space = 7 DOF.
Definition Similarity3.h:197
│ │ │ │ -
gtsam::Similarity3::wedge
static Matrix4 wedge(const Vector7 &xi)
wedge for Similarity3:
Definition Similarity3.cpp:198
│ │ │ │ -
gtsam::Similarity3::scale
double scale() const
Return the scale.
Definition Similarity3.h:191
│ │ │ │ -
gtsam::Similarity3::ChartAtOrigin
Chart at the origin.
Definition Similarity3.h:153
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::AlgebraicDecisionTree
An algebraic decision tree fixes the range of a DecisionTree to double.
Definition AlgebraicDecisionTree.h:38
│ │ │ │ +
gtsam::DecisionTreeFactor
A discrete probabilistic factor.
Definition DecisionTreeFactor.h:45
│ │ │ │ +
gtsam::DiscreteValues
A map from keys to values.
Definition DiscreteValues.h:34
│ │ │ │ +
gtsam::HybridBayesNet
A hybrid Bayes net is a collection of HybridConditionals, which can have discrete conditionals,...
Definition HybridBayesNet.h:35
│ │ │ │ +
gtsam::HybridBayesNet::push_back
void push_back(boost::shared_ptr< HybridConditional > conditional)
Add a hybrid conditional using a shared_ptr.
Definition HybridBayesNet.h:69
│ │ │ │ +
gtsam::HybridBayesNet::emplace_back
void emplace_back(Conditional *conditional)
Preferred: add a conditional directly using a pointer.
Definition HybridBayesNet.h:82
│ │ │ │ +
gtsam::HybridBayesNet::push_back
void push_back(HybridConditional &&conditional)
Add a conditional using a shared_ptr, using implicit conversion to a HybridConditional.
Definition HybridBayesNet.h:99
│ │ │ │ +
gtsam::HybridBayesNet::HybridBayesNet
HybridBayesNet()=default
Construct empty Bayes net.
│ │ │ │ +
gtsam::HybridBayesNet::operator()
double operator()(const HybridValues &values) const
Evaluate hybrid probability density for given HybridValues, sugar.
Definition HybridBayesNet.h:117
│ │ │ │ +
gtsam::HybridConditional
Hybrid Conditional Density.
Definition HybridConditional.h:62
│ │ │ │ +
gtsam::HybridGaussianFactorGraph
Definition HybridGaussianFactorGraph.h:102
│ │ │ │ +
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ +
gtsam::BayesNet
A BayesNet is a tree of conditionals, stored in elimination order.
Definition BayesNet.h:35
│ │ │ │ +
gtsam::Conditional
Definition Conditional.h:64
│ │ │ │ +
gtsam::GaussianBayesNet
GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals.
Definition GaussianBayesNet.h:36
│ │ │ │ +
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,233 +1,203 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Similarity3.h │ │ │ │ │ +HybridBayesNet.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ -2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ -4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -5 * All Rights Reserved │ │ │ │ │ -6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ -7 │ │ │ │ │ -8 * See LICENSE for the license information │ │ │ │ │ -9 │ │ │ │ │ -10 * ------------------------------------------------------------------------- │ │ │ │ │ +2 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +3 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +4 * All Rights Reserved │ │ │ │ │ +5 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +6 * See LICENSE for the license information │ │ │ │ │ +7 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ -11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_b_a_s_e_/_L_i_e_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ -23#include │ │ │ │ │ -24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_3_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_3_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_3_._h> │ │ │ │ │ -27 │ │ │ │ │ -28namespace _g_t_s_a_m { │ │ │ │ │ -29 │ │ │ │ │ -30// Forward declarations │ │ │ │ │ -31class Pose3; │ │ │ │ │ -32 │ │ │ │ │ -_3_6class GTSAM_EXPORT _S_i_m_i_l_a_r_i_t_y_3 : public _L_i_e_G_r_o_u_p { │ │ │ │ │ -39 typedef _R_o_t_3 _R_o_t_a_t_i_o_n; │ │ │ │ │ -40 typedef _P_o_i_n_t_3 Translation; │ │ │ │ │ +8 │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_V_a_l_u_e_s_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ +26 │ │ │ │ │ +27namespace _g_t_s_a_m { │ │ │ │ │ +28 │ │ │ │ │ +_3_5class GTSAM_EXPORT _H_y_b_r_i_d_B_a_y_e_s_N_e_t : public _B_a_y_e_s_N_e_t { │ │ │ │ │ +36 public: │ │ │ │ │ +37 using _B_a_s_e = _B_a_y_e_s_N_e_t_<_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_>; │ │ │ │ │ +38 using _T_h_i_s = _H_y_b_r_i_d_B_a_y_e_s_N_e_t; │ │ │ │ │ +39 using _C_o_n_d_i_t_i_o_n_a_l_T_y_p_e = _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l; │ │ │ │ │ +40 using shared_ptr = boost::shared_ptr; │ │ │ │ │ +41 using sharedConditional = boost::shared_ptr; │ │ │ │ │ 42 │ │ │ │ │ -43 private: │ │ │ │ │ -44 _R_o_t_3 R_; │ │ │ │ │ -45 _P_o_i_n_t_3 t_; │ │ │ │ │ -46 double s_; │ │ │ │ │ -47 │ │ │ │ │ -48 public: │ │ │ │ │ -51 │ │ │ │ │ -53 _S_i_m_i_l_a_r_i_t_y_3(); │ │ │ │ │ -54 │ │ │ │ │ -56 _S_i_m_i_l_a_r_i_t_y_3(double s); │ │ │ │ │ -57 │ │ │ │ │ -59 _S_i_m_i_l_a_r_i_t_y_3(const _R_o_t_3& R, const _P_o_i_n_t_3& t, double s); │ │ │ │ │ -60 │ │ │ │ │ -62 _S_i_m_i_l_a_r_i_t_y_3(const Matrix3& R, const Vector3& t, double s); │ │ │ │ │ +45 │ │ │ │ │ +_4_7 _H_y_b_r_i_d_B_a_y_e_s_N_e_t() = default; │ │ │ │ │ +48 │ │ │ │ │ +52 │ │ │ │ │ +54 void _p_r_i_n_t(const std::string &s = "", const _K_e_y_F_o_r_m_a_t_t_e_r &formatter = │ │ │ │ │ +55 DefaultKeyFormatter) const override; │ │ │ │ │ +56 │ │ │ │ │ +58 bool _e_q_u_a_l_s(const _T_h_i_s &fg, double tol = 1e-9) const; │ │ │ │ │ +59 │ │ │ │ │ 63 │ │ │ │ │ -65 _S_i_m_i_l_a_r_i_t_y_3(const Matrix4& T); │ │ │ │ │ -66 │ │ │ │ │ -70 │ │ │ │ │ -72 bool _e_q_u_a_l_s(const _S_i_m_i_l_a_r_i_t_y_3& sim, double tol) const; │ │ │ │ │ -73 │ │ │ │ │ -75 bool _o_p_e_r_a_t_o_r_=_=(const _S_i_m_i_l_a_r_i_t_y_3& other) const; │ │ │ │ │ -76 │ │ │ │ │ -78 void _p_r_i_n_t(const std::string& s) const; │ │ │ │ │ -79 │ │ │ │ │ -80 friend std::ostream& operator<<(std::ostream& os, const _S_i_m_i_l_a_r_i_t_y_3& p); │ │ │ │ │ -81 │ │ │ │ │ -85 │ │ │ │ │ -87 static _S_i_m_i_l_a_r_i_t_y_3 Identity(); │ │ │ │ │ -88 │ │ │ │ │ -90 _S_i_m_i_l_a_r_i_t_y_3 _o_p_e_r_a_t_o_r_*(const _S_i_m_i_l_a_r_i_t_y_3& S) const; │ │ │ │ │ -91 │ │ │ │ │ -93 _S_i_m_i_l_a_r_i_t_y_3 inverse() const; │ │ │ │ │ -94 │ │ │ │ │ -98 │ │ │ │ │ -100 _P_o_i_n_t_3 transformFrom(const _P_o_i_n_t_3& p, // │ │ │ │ │ -101 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _7_> H1 = boost::none, // │ │ │ │ │ -102 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H2 = boost::none) const; │ │ │ │ │ +_6_9 void _p_u_s_h___b_a_c_k(boost::shared_ptr conditional) { │ │ │ │ │ +70 factors_.push_back(conditional); │ │ │ │ │ +71 } │ │ │ │ │ +72 │ │ │ │ │ +81 template │ │ │ │ │ +_8_2 void _e_m_p_l_a_c_e___b_a_c_k(_C_o_n_d_i_t_i_o_n_a_l *conditional) { │ │ │ │ │ +83 factors_.push_back(boost::make_shared( │ │ │ │ │ +84 boost::shared_ptr(conditional))); │ │ │ │ │ +85 } │ │ │ │ │ +86 │ │ │ │ │ +_9_9 void _p_u_s_h___b_a_c_k(_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l &&conditional) { │ │ │ │ │ +100 factors_.push_back( │ │ │ │ │ +101 boost::make_shared(std::move(conditional))); │ │ │ │ │ +102 } │ │ │ │ │ 103 │ │ │ │ │ -115 _P_o_s_e_3 transformFrom(const _P_o_s_e_3& T) const; │ │ │ │ │ -116 │ │ │ │ │ -118 _P_o_i_n_t_3 _o_p_e_r_a_t_o_r_*(const _P_o_i_n_t_3& p) const; │ │ │ │ │ -119 │ │ │ │ │ -123 static _S_i_m_i_l_a_r_i_t_y_3 Align(const Point3Pairs& abPointPairs); │ │ │ │ │ -124 │ │ │ │ │ -135 static _S_i_m_i_l_a_r_i_t_y_3 Align(const std::vector& abPosePairs); │ │ │ │ │ -136 │ │ │ │ │ -140 │ │ │ │ │ -144 static Vector7 Logmap(const _S_i_m_i_l_a_r_i_t_y_3& s, // │ │ │ │ │ -145 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_7_,_ _7_> Hm = boost::none); │ │ │ │ │ -146 │ │ │ │ │ -149 static _S_i_m_i_l_a_r_i_t_y_3 Expmap(const Vector7& v, // │ │ │ │ │ -150 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_7_,_ _7_> Hm = boost::none); │ │ │ │ │ -151 │ │ │ │ │ -_1_5_3 struct _C_h_a_r_t_A_t_O_r_i_g_i_n { │ │ │ │ │ -154 static _S_i_m_i_l_a_r_i_t_y_3 Retract(const Vector7& v, │ │ │ │ │ -155 ChartJacobian H = boost::none) { │ │ │ │ │ -156 return Similarity3::Expmap(v, H); │ │ │ │ │ -157 } │ │ │ │ │ -158 static Vector7 Local(const _S_i_m_i_l_a_r_i_t_y_3& other, │ │ │ │ │ -159 ChartJacobian H = boost::none) { │ │ │ │ │ -160 return Similarity3::Logmap(other, H); │ │ │ │ │ -161 } │ │ │ │ │ -162 }; │ │ │ │ │ -163 │ │ │ │ │ -164 using _L_i_e_G_r_o_u_p<_S_i_m_i_l_a_r_i_t_y_3, 7>::inverse; │ │ │ │ │ -165 │ │ │ │ │ -172 static Matrix4 _w_e_d_g_e(const Vector7& xi); │ │ │ │ │ -173 │ │ │ │ │ -175 Matrix7 AdjointMap() const; │ │ │ │ │ -176 │ │ │ │ │ -180 │ │ │ │ │ -182 Matrix4 matrix() const; │ │ │ │ │ -183 │ │ │ │ │ -_1_8_5 _R_o_t_3 _r_o_t_a_t_i_o_n() const { return R_; } │ │ │ │ │ +111 _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t choose(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s &assignment) const; │ │ │ │ │ +112 │ │ │ │ │ +114 double evaluate(const _H_y_b_r_i_d_V_a_l_u_e_s &values) const; │ │ │ │ │ +115 │ │ │ │ │ +_1_1_7 double _o_p_e_r_a_t_o_r_(_)(const _H_y_b_r_i_d_V_a_l_u_e_s &values) const { │ │ │ │ │ +118 return evaluate(values); │ │ │ │ │ +119 } │ │ │ │ │ +120 │ │ │ │ │ +128 _H_y_b_r_i_d_V_a_l_u_e_s _o_p_t_i_m_i_z_e() const; │ │ │ │ │ +129 │ │ │ │ │ +137 _V_e_c_t_o_r_V_a_l_u_e_s _o_p_t_i_m_i_z_e(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s &assignment) const; │ │ │ │ │ +138 │ │ │ │ │ +144 DecisionTreeFactor::shared_ptr discreteConditionals() const; │ │ │ │ │ +145 │ │ │ │ │ +158 _H_y_b_r_i_d_V_a_l_u_e_s sample(const _H_y_b_r_i_d_V_a_l_u_e_s &given, std::mt19937_64 *rng) const; │ │ │ │ │ +159 │ │ │ │ │ +170 _H_y_b_r_i_d_V_a_l_u_e_s sample(std::mt19937_64 *rng) const; │ │ │ │ │ +171 │ │ │ │ │ +178 _H_y_b_r_i_d_V_a_l_u_e_s sample(const _H_y_b_r_i_d_V_a_l_u_e_s &given) const; │ │ │ │ │ +179 │ │ │ │ │ +185 _H_y_b_r_i_d_V_a_l_u_e_s sample() const; │ │ │ │ │ 186 │ │ │ │ │ -_1_8_8 _P_o_i_n_t_3 _t_r_a_n_s_l_a_t_i_o_n() const { return t_; } │ │ │ │ │ +188 _H_y_b_r_i_d_B_a_y_e_s_N_e_t prune(size_t maxNrLeaves); │ │ │ │ │ 189 │ │ │ │ │ -_1_9_1 double _s_c_a_l_e() const { return s_; } │ │ │ │ │ -192 │ │ │ │ │ -_1_9_4 inline static size_t _D_i_m() { return 7; } │ │ │ │ │ -195 │ │ │ │ │ -_1_9_7 inline size_t _d_i_m() const { return 7; } │ │ │ │ │ -198 │ │ │ │ │ -202 │ │ │ │ │ -203 private: │ │ │ │ │ -205 static Matrix3 GetV(Vector3 w, double lambda); │ │ │ │ │ -206 │ │ │ │ │ -208}; │ │ │ │ │ -209 │ │ │ │ │ -210template <> │ │ │ │ │ -211inline Matrix wedge(const Vector& xi) { │ │ │ │ │ -212 return _S_i_m_i_l_a_r_i_t_y_3_:_:_w_e_d_g_e(xi); │ │ │ │ │ -213} │ │ │ │ │ +197 _A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_<_K_e_y_> logProbability( │ │ │ │ │ +198 const _V_e_c_t_o_r_V_a_l_u_e_s &continuousValues) const; │ │ │ │ │ +199 │ │ │ │ │ +200 using BayesNet::logProbability; // expose HybridValues version │ │ │ │ │ +201 │ │ │ │ │ +212 _A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_<_K_e_y_> evaluate( │ │ │ │ │ +213 const _V_e_c_t_o_r_V_a_l_u_e_s &continuousValues) const; │ │ │ │ │ 214 │ │ │ │ │ -215template <> │ │ │ │ │ -_2_1_6struct _t_r_a_i_t_s<_S_i_m_i_l_a_r_i_t_y_3> : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p {}; │ │ │ │ │ -217 │ │ │ │ │ -218template <> │ │ │ │ │ -_2_1_9struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ -{}; │ │ │ │ │ -220 │ │ │ │ │ -221} // namespace gtsam │ │ │ │ │ -_L_i_e_._h │ │ │ │ │ -Base class and basic functions for Lie types. │ │ │ │ │ -_M_a_n_i_f_o_l_d_._h │ │ │ │ │ -Base class and basic functions for Manifold types. │ │ │ │ │ -_P_o_s_e_3_._h │ │ │ │ │ -3D Pose │ │ │ │ │ -_P_o_i_n_t_3_._h │ │ │ │ │ -3D Point │ │ │ │ │ -_R_o_t_3_._h │ │ │ │ │ -3D rotation represented as a rotation matrix or quaternion │ │ │ │ │ +219 _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h toFactorGraph( │ │ │ │ │ +220 const _V_e_c_t_o_r_V_a_l_u_e_s &measurements) const; │ │ │ │ │ +222 │ │ │ │ │ +223 private: │ │ │ │ │ +229 void updateDiscreteConditionals(const _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r │ │ │ │ │ +&prunedDecisionTree); │ │ │ │ │ +230 │ │ │ │ │ +_2_3_2 friend class boost::serialization::access; │ │ │ │ │ +233 template │ │ │ │ │ +234 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ +235 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +236 } │ │ │ │ │ +237}; │ │ │ │ │ +238 │ │ │ │ │ +240template <> │ │ │ │ │ +_2_4_1struct _t_r_a_i_t_s<_H_y_b_r_i_d_B_a_y_e_s_N_e_t> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ +242 │ │ │ │ │ +243} // namespace gtsam │ │ │ │ │ +_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_._h │ │ │ │ │ +_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ +_H_y_b_r_i_d_V_a_l_u_e_s_._h │ │ │ │ │ +_B_a_y_e_s_N_e_t_._h │ │ │ │ │ +Bayes network. │ │ │ │ │ +_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h │ │ │ │ │ +Chordal Bayes Net, the result of eliminating a factor graph. │ │ │ │ │ +_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ +Included from all GTSAM files. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ +Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key │ │ │ │ │ +landmarkKey) │ │ │ │ │ +Optimize for triangulation. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.cpp:155 │ │ │ │ │ _g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ print without optional string, must specify cout yourself │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ -Point2 operator*(double s, const Point2 &p) │ │ │ │ │ -multiply with scalar │ │ │ │ │ -DDeeffiinniittiioonn Point2.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ -Vector3 Point3 │ │ │ │ │ -As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point3 to Vector3... │ │ │ │ │ -DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_=_= │ │ │ │ │ -bool operator==(const Matrix &A, const Matrix &B) │ │ │ │ │ -equality is just equal_with_abs_tol 1e-9 │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_w_e_d_g_e │ │ │ │ │ -Matrix wedge(const Vector &x) │ │ │ │ │ -Declaration of wedge (see Murray94book) used to convert from n exponential │ │ │ │ │ -coordinates to n*n element... │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ -A CRTP helper class that implements Lie group methods Prerequisites: methods │ │ │ │ │ -operator*,... │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ -Both LieGroupTraits and Testable. │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:229 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ -either a fixed size o... │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ _g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ Template to create a binary predicate. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ -A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ -Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor │ │ │ │ │ -symbol GTSAM_USE_QUATERNIO... │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_3 │ │ │ │ │ -3D similarity transform │ │ │ │ │ -DDeeffiinniittiioonn Similarity3.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_3_:_:_D_i_m │ │ │ │ │ -static size_t Dim() │ │ │ │ │ -Dimensionality of tangent space = 7 DOF - used to autodetect sizes. │ │ │ │ │ -DDeeffiinniittiioonn Similarity3.h:194 │ │ │ │ │ -_g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_3_:_:_t_r_a_n_s_l_a_t_i_o_n │ │ │ │ │ -Point3 translation() const │ │ │ │ │ -Return a GTSAM translation. │ │ │ │ │ -DDeeffiinniittiioonn Similarity3.h:188 │ │ │ │ │ -_g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_3_:_:_r_o_t_a_t_i_o_n │ │ │ │ │ -Rot3 rotation() const │ │ │ │ │ -Return a GTSAM rotation. │ │ │ │ │ -DDeeffiinniittiioonn Similarity3.h:185 │ │ │ │ │ -_g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_3_:_:_d_i_m │ │ │ │ │ -size_t dim() const │ │ │ │ │ -Dimensionality of tangent space = 7 DOF. │ │ │ │ │ -DDeeffiinniittiioonn Similarity3.h:197 │ │ │ │ │ -_g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_3_:_:_w_e_d_g_e │ │ │ │ │ -static Matrix4 wedge(const Vector7 &xi) │ │ │ │ │ -wedge for Similarity3: │ │ │ │ │ -DDeeffiinniittiioonn Similarity3.cpp:198 │ │ │ │ │ -_g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_3_:_:_s_c_a_l_e │ │ │ │ │ -double scale() const │ │ │ │ │ -Return the scale. │ │ │ │ │ -DDeeffiinniittiioonn Similarity3.h:191 │ │ │ │ │ -_g_t_s_a_m_:_:_S_i_m_i_l_a_r_i_t_y_3_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n │ │ │ │ │ -Chart at the origin. │ │ │ │ │ -DDeeffiinniittiioonn Similarity3.h:153 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e │ │ │ │ │ +An algebraic decision tree fixes the range of a DecisionTree to double. │ │ │ │ │ +DDeeffiinniittiioonn AlgebraicDecisionTree.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r │ │ │ │ │ +A discrete probabilistic factor. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTreeFactor.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ +A map from keys to values. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteValues.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_N_e_t │ │ │ │ │ +A hybrid Bayes net is a collection of HybridConditionals, which can have │ │ │ │ │ +discrete conditionals,... │ │ │ │ │ +DDeeffiinniittiioonn HybridBayesNet.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_N_e_t_:_:_p_u_s_h___b_a_c_k │ │ │ │ │ +void push_back(boost::shared_ptr< HybridConditional > conditional) │ │ │ │ │ +Add a hybrid conditional using a shared_ptr. │ │ │ │ │ +DDeeffiinniittiioonn HybridBayesNet.h:69 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_N_e_t_:_:_e_m_p_l_a_c_e___b_a_c_k │ │ │ │ │ +void emplace_back(Conditional *conditional) │ │ │ │ │ +Preferred: add a conditional directly using a pointer. │ │ │ │ │ +DDeeffiinniittiioonn HybridBayesNet.h:82 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_N_e_t_:_:_p_u_s_h___b_a_c_k │ │ │ │ │ +void push_back(HybridConditional &&conditional) │ │ │ │ │ +Add a conditional using a shared_ptr, using implicit conversion to a │ │ │ │ │ +HybridConditional. │ │ │ │ │ +DDeeffiinniittiioonn HybridBayesNet.h:99 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_N_e_t_:_:_H_y_b_r_i_d_B_a_y_e_s_N_e_t │ │ │ │ │ +HybridBayesNet()=default │ │ │ │ │ +Construct empty Bayes net. │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_N_e_t_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ +double operator()(const HybridValues &values) const │ │ │ │ │ +Evaluate hybrid probability density for given HybridValues, sugar. │ │ │ │ │ +DDeeffiinniittiioonn HybridBayesNet.h:117 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +Hybrid Conditional Density. │ │ │ │ │ +DDeeffiinniittiioonn HybridConditional.h:62 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +DDeeffiinniittiioonn HybridGaussianFactorGraph.h:102 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t │ │ │ │ │ +A BayesNet is a tree of conditionals, stored in elimination order. │ │ │ │ │ +DDeeffiinniittiioonn BayesNet.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +DDeeffiinniittiioonn Conditional.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t │ │ │ │ │ +GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals. │ │ │ │ │ +DDeeffiinniittiioonn GaussianBayesNet.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ +with a unique integer... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _S_i_m_i_l_a_r_i_t_y_3_._h │ │ │ │ │ + * _h_y_b_r_i_d │ │ │ │ │ + * _H_y_b_r_i_d_B_a_y_e_s_N_e_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00461_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01130_source.html │ │ │ │┄ Files 86% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/triangulation.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/GncOptimizer.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
triangulation.h
│ │ │ │ +
GncOptimizer.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
21#pragma once
│ │ │ │ -
22
│ │ │ │ -
23#include <gtsam/geometry/Cal3Bundler.h>
│ │ │ │ -
24#include <gtsam/geometry/Cal3Fisheye.h>
│ │ │ │ -
25#include <gtsam/geometry/Cal3Unified.h>
│ │ │ │ -
26#include <gtsam/geometry/Cal3_S2.h>
│ │ │ │ -
27#include <gtsam/geometry/Cal3DS2.h>
│ │ │ │ -
28#include <gtsam/geometry/CameraSet.h>
│ │ │ │ -
29#include <gtsam/geometry/PinholeCamera.h>
│ │ │ │ -
30#include <gtsam/geometry/SphericalCamera.h>
│ │ │ │ -
31#include <gtsam/geometry/Pose2.h>
│ │ │ │ -
32#include <gtsam/inference/Symbol.h>
│ │ │ │ -
33#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ -
34#include <gtsam/slam/TriangulationFactor.h>
│ │ │ │ -
35
│ │ │ │ -
36namespace gtsam {
│ │ │ │ -
37
│ │ │ │ -
│ │ │ │ -
39class GTSAM_EXPORT TriangulationUnderconstrainedException: public std::runtime_error {
│ │ │ │ -
40public:
│ │ │ │ -
41 TriangulationUnderconstrainedException() :
│ │ │ │ -
42 std::runtime_error("Triangulation Underconstrained Exception.") {
│ │ │ │ -
43 }
│ │ │ │ -
44};
│ │ │ │ -
│ │ │ │ -
45
│ │ │ │ -
│ │ │ │ -
47class GTSAM_EXPORT TriangulationCheiralityException: public std::runtime_error {
│ │ │ │ -
48public:
│ │ │ │ -
49 TriangulationCheiralityException() :
│ │ │ │ -
50 std::runtime_error(
│ │ │ │ -
51 "Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") {
│ │ │ │ -
52 }
│ │ │ │ -
53};
│ │ │ │ -
│ │ │ │ -
54
│ │ │ │ -
62GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
│ │ │ │ -
63 const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
│ │ │ │ -
64 const Point2Vector& measurements, double rank_tol = 1e-9);
│ │ │ │ -
65
│ │ │ │ -
74GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
│ │ │ │ -
75 const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
│ │ │ │ -
76 const std::vector<Unit3>& measurements, double rank_tol = 1e-9);
│ │ │ │ -
77
│ │ │ │ -
85GTSAM_EXPORT Point3 triangulateDLT(
│ │ │ │ -
86 const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
│ │ │ │ -
87 const Point2Vector& measurements,
│ │ │ │ -
88 double rank_tol = 1e-9);
│ │ │ │ -
89
│ │ │ │ -
93GTSAM_EXPORT Point3 triangulateDLT(
│ │ │ │ -
94 const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
│ │ │ │ -
95 const std::vector<Unit3>& measurements,
│ │ │ │ -
96 double rank_tol = 1e-9);
│ │ │ │ -
97
│ │ │ │ -
108GTSAM_EXPORT Point3 triangulateLOST(const std::vector<Pose3>& poses,
│ │ │ │ -
109 const Point3Vector& calibratedMeasurements,
│ │ │ │ -
110 const SharedIsotropic& measurementNoise);
│ │ │ │ -
111
│ │ │ │ -
121template<class CALIBRATION>
│ │ │ │ -
│ │ │ │ -
122std::pair<NonlinearFactorGraph, Values> triangulationGraph(
│ │ │ │ -
123 const std::vector<Pose3>& poses, boost::shared_ptr<CALIBRATION> sharedCal,
│ │ │ │ -
124 const Point2Vector& measurements, Key landmarkKey,
│ │ │ │ -
125 const Point3& initialEstimate,
│ │ │ │ -
126 const SharedNoiseModel& model = noiseModel::Unit::Create(2)) {
│ │ │ │ -
127 Values values;
│ │ │ │ -
128 values.insert(landmarkKey, initialEstimate); // Initial landmark value
│ │ │ │ -
129 NonlinearFactorGraph graph;
│ │ │ │ -
130 for (size_t i = 0; i < measurements.size(); i++) {
│ │ │ │ -
131 const Pose3& pose_i = poses[i];
│ │ │ │ -
132 typedef PinholePose<CALIBRATION> Camera;
│ │ │ │ -
133 Camera camera_i(pose_i, sharedCal);
│ │ │ │ -
134 graph.emplace_shared<TriangulationFactor<Camera> > //
│ │ │ │ -
135 (camera_i, measurements[i], model, landmarkKey);
│ │ │ │ -
136 }
│ │ │ │ -
137 return std::make_pair(graph, values);
│ │ │ │ -
138}
│ │ │ │ -
│ │ │ │ -
139
│ │ │ │ -
149template<class CAMERA>
│ │ │ │ -
│ │ │ │ -
150std::pair<NonlinearFactorGraph, Values> triangulationGraph(
│ │ │ │ -
151 const CameraSet<CAMERA>& cameras,
│ │ │ │ -
152 const typename CAMERA::MeasurementVector& measurements, Key landmarkKey,
│ │ │ │ -
153 const Point3& initialEstimate,
│ │ │ │ -
154 const SharedNoiseModel& model = nullptr) {
│ │ │ │ -
155 Values values;
│ │ │ │ -
156 values.insert(landmarkKey, initialEstimate); // Initial landmark value
│ │ │ │ -
157 NonlinearFactorGraph graph;
│ │ │ │ -
158 static SharedNoiseModel unit(noiseModel::Unit::Create(
│ │ │ │ -
159 traits<typename CAMERA::Measurement>::dimension));
│ │ │ │ -
160 for (size_t i = 0; i < measurements.size(); i++) {
│ │ │ │ -
161 const CAMERA& camera_i = cameras[i];
│ │ │ │ -
162 graph.emplace_shared<TriangulationFactor<CAMERA> > //
│ │ │ │ -
163 (camera_i, measurements[i], model? model : unit, landmarkKey);
│ │ │ │ -
164 }
│ │ │ │ -
165 return std::make_pair(graph, values);
│ │ │ │ -
166}
│ │ │ │ -
│ │ │ │ +
27#pragma once
│ │ │ │ +
28
│ │ │ │ +
29#include <gtsam/nonlinear/GncParams.h>
│ │ │ │ +
30#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ +
31#include <boost/math/distributions/chi_squared.hpp>
│ │ │ │ +
32
│ │ │ │ +
33namespace gtsam {
│ │ │ │ +
34/*
│ │ │ │ +
35 * Quantile of chi-squared distribution with given degrees of freedom at probability alpha.
│ │ │ │ +
36 * Equivalent to chi2inv in Matlab.
│ │ │ │ +
37 */
│ │ │ │ +
38static double Chi2inv(const double alpha, const size_t dofs) {
│ │ │ │ +
39 boost::math::chi_squared_distribution<double> chi2(dofs);
│ │ │ │ +
40 return boost::math::quantile(chi2, alpha);
│ │ │ │ +
41}
│ │ │ │ +
42
│ │ │ │ +
43/* ************************************************************************* */
│ │ │ │ +
44template<class GncParameters>
│ │ │ │ +
│ │ │ │ +
45class GncOptimizer {
│ │ │ │ +
46 public:
│ │ │ │ +
48 typedef typename GncParameters::OptimizerType BaseOptimizer;
│ │ │ │ +
49
│ │ │ │ +
50 private:
│ │ │ │ +
51 NonlinearFactorGraph nfg_;
│ │ │ │ +
52 Values state_;
│ │ │ │ +
53 GncParameters params_;
│ │ │ │ +
54 Vector weights_;
│ │ │ │ +
55 Vector barcSq_;
│ │ │ │ +
56
│ │ │ │ +
57 public:
│ │ │ │ +
│ │ │ │ +
59 GncOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
│ │ │ │ +
60 const GncParameters& params = GncParameters())
│ │ │ │ +
61 : state_(initialValues),
│ │ │ │ +
62 params_(params) {
│ │ │ │ +
63
│ │ │ │ +
64 // make sure all noiseModels are Gaussian or convert to Gaussian
│ │ │ │ +
65 nfg_.resize(graph.size());
│ │ │ │ +
66 for (size_t i = 0; i < graph.size(); i++) {
│ │ │ │ +
67 if (graph[i]) {
│ │ │ │ +
68 NoiseModelFactor::shared_ptr factor = boost::dynamic_pointer_cast<
│ │ │ │ +
69 NoiseModelFactor>(graph[i]);
│ │ │ │ +
70 auto robust = boost::dynamic_pointer_cast<
│ │ │ │ +
71 noiseModel::Robust>(factor->noiseModel());
│ │ │ │ +
72 // if the factor has a robust loss, we remove the robust loss
│ │ │ │ +
73 nfg_[i] = robust ? factor-> cloneWithNewNoiseModel(robust->noise()) : factor;
│ │ │ │ +
74 }
│ │ │ │ +
75 }
│ │ │ │ +
76
│ │ │ │ +
77 // check that known inliers and outliers make sense:
│ │ │ │ +
78 std::vector<size_t> inconsistentlySpecifiedWeights; // measurements the user has incorrectly specified
│ │ │ │ +
79 // to be BOTH known inliers and known outliers
│ │ │ │ +
80 std::set_intersection(params.knownInliers.begin(),params.knownInliers.end(),
│ │ │ │ +
81 params.knownOutliers.begin(),params.knownOutliers.end(),
│ │ │ │ +
82 std::inserter(inconsistentlySpecifiedWeights, inconsistentlySpecifiedWeights.begin()));
│ │ │ │ +
83 if(inconsistentlySpecifiedWeights.size() > 0){ // if we have inconsistently specified weights, we throw an exception
│ │ │ │ +
84 params.print("params\n");
│ │ │ │ +
85 throw std::runtime_error("GncOptimizer::constructor: the user has selected one or more measurements"
│ │ │ │ +
86 " to be BOTH a known inlier and a known outlier.");
│ │ │ │ +
87 }
│ │ │ │ +
88 // check that known inliers are in the graph
│ │ │ │ +
89 for (size_t i = 0; i < params.knownInliers.size(); i++){
│ │ │ │ +
90 if( params.knownInliers[i] > nfg_.size()-1 ){ // outside graph
│ │ │ │ +
91 throw std::runtime_error("GncOptimizer::constructor: the user has selected one or more measurements"
│ │ │ │ +
92 "that are not in the factor graph to be known inliers.");
│ │ │ │ +
93 }
│ │ │ │ +
94 }
│ │ │ │ +
95 // check that known outliers are in the graph
│ │ │ │ +
96 for (size_t i = 0; i < params.knownOutliers.size(); i++){
│ │ │ │ +
97 if( params.knownOutliers[i] > nfg_.size()-1 ){ // outside graph
│ │ │ │ +
98 throw std::runtime_error("GncOptimizer::constructor: the user has selected one or more measurements"
│ │ │ │ +
99 "that are not in the factor graph to be known outliers.");
│ │ │ │ +
100 }
│ │ │ │ +
101 }
│ │ │ │ +
102 // initialize weights (if we don't have prior knowledge of inliers/outliers
│ │ │ │ +
103 // the weights are all initialized to 1.
│ │ │ │ +
104 weights_ = initializeWeightsFromKnownInliersAndOutliers();
│ │ │ │ +
105
│ │ │ │ +
106 // set default barcSq_ (inlier threshold)
│ │ │ │ +
107 double alpha = 0.99; // with this (default) probability, inlier residuals are smaller than barcSq_
│ │ │ │ +
108 setInlierCostThresholdsAtProbability(alpha);
│ │ │ │ +
109 }
│ │ │ │ +
│ │ │ │ +
110
│ │ │ │ +
│ │ │ │ +
117 void setInlierCostThresholds(const double inth) {
│ │ │ │ +
118 barcSq_ = inth * Vector::Ones(nfg_.size());
│ │ │ │ +
119 }
│ │ │ │ +
│ │ │ │ +
120
│ │ │ │ +
│ │ │ │ +
125 void setInlierCostThresholds(const Vector& inthVec) {
│ │ │ │ +
126 barcSq_ = inthVec;
│ │ │ │ +
127 }
│ │ │ │ +
│ │ │ │ +
128
│ │ │ │ +
│ │ │ │ +
132 void setInlierCostThresholdsAtProbability(const double alpha) {
│ │ │ │ +
133 barcSq_ = Vector::Ones(nfg_.size()); // initialize
│ │ │ │ +
134 for (size_t k = 0; k < nfg_.size(); k++) {
│ │ │ │ +
135 if (nfg_[k]) {
│ │ │ │ +
136 barcSq_[k] = 0.5 * Chi2inv(alpha, nfg_[k]->dim()); // 0.5 derives from the error definition in gtsam
│ │ │ │ +
137 }
│ │ │ │ +
138 }
│ │ │ │ +
139 }
│ │ │ │ +
│ │ │ │ +
140
│ │ │ │ +
│ │ │ │ +
144 void setWeights(const Vector w) {
│ │ │ │ +
145 if (size_t(w.size()) != nfg_.size()) {
│ │ │ │ +
146 throw std::runtime_error(
│ │ │ │ +
147 "GncOptimizer::setWeights: the number of specified weights"
│ │ │ │ +
148 " does not match the size of the factor graph.");
│ │ │ │ +
149 }
│ │ │ │ +
150 weights_ = w;
│ │ │ │ +
151 }
│ │ │ │ +
│ │ │ │ +
152
│ │ │ │ +
154 const NonlinearFactorGraph& getFactors() const { return nfg_; }
│ │ │ │ +
155
│ │ │ │ +
157 const Values& getState() const { return state_; }
│ │ │ │ +
158
│ │ │ │ +
160 const GncParameters& getParams() const { return params_;}
│ │ │ │ +
161
│ │ │ │ +
163 const Vector& getWeights() const { return weights_;}
│ │ │ │ +
164
│ │ │ │ +
166 const Vector& getInlierCostThresholds() const {return barcSq_;}
│ │ │ │
167
│ │ │ │ -
175GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph,
│ │ │ │ -
176 const Values& values, Key landmarkKey);
│ │ │ │ -
177
│ │ │ │ -
186template<class CALIBRATION>
│ │ │ │ -
│ │ │ │ -
187Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
│ │ │ │ -
188 boost::shared_ptr<CALIBRATION> sharedCal,
│ │ │ │ -
189 const Point2Vector& measurements, const Point3& initialEstimate,
│ │ │ │ -
190 const SharedNoiseModel& model = nullptr) {
│ │ │ │ -
191
│ │ │ │ -
192 // Create a factor graph and initial values
│ │ │ │ -
193 Values values;
│ │ │ │ -
194 NonlinearFactorGraph graph;
│ │ │ │ -
195 boost::tie(graph, values) = triangulationGraph<CALIBRATION> //
│ │ │ │ -
196 (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate, model);
│ │ │ │ -
197
│ │ │ │ -
198 return optimize(graph, values, Symbol('p', 0));
│ │ │ │ -
199}
│ │ │ │ -
│ │ │ │ -
200
│ │ │ │ -
208template<class CAMERA>
│ │ │ │ -
│ │ │ │ -
209Point3 triangulateNonlinear(
│ │ │ │ -
210 const CameraSet<CAMERA>& cameras,
│ │ │ │ -
211 const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate,
│ │ │ │ -
212 const SharedNoiseModel& model = nullptr) {
│ │ │ │ -
213
│ │ │ │ -
214 // Create a factor graph and initial values
│ │ │ │ -
215 Values values;
│ │ │ │ -
216 NonlinearFactorGraph graph;
│ │ │ │ -
217 boost::tie(graph, values) = triangulationGraph<CAMERA> //
│ │ │ │ -
218 (cameras, measurements, Symbol('p', 0), initialEstimate, model);
│ │ │ │ -
219
│ │ │ │ -
220 return optimize(graph, values, Symbol('p', 0));
│ │ │ │ -
221}
│ │ │ │ -
│ │ │ │ -
222
│ │ │ │ -
223template<class CAMERA>
│ │ │ │ -
224std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>
│ │ │ │ -
225projectionMatricesFromCameras(const CameraSet<CAMERA> &cameras) {
│ │ │ │ -
226 std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
│ │ │ │ -
227 for (const CAMERA &camera: cameras) {
│ │ │ │ -
228 projection_matrices.push_back(camera.cameraProjectionMatrix());
│ │ │ │ -
229 }
│ │ │ │ -
230 return projection_matrices;
│ │ │ │ -
231}
│ │ │ │ -
232
│ │ │ │ -
233// overload, assuming pinholePose
│ │ │ │ -
234template<class CALIBRATION>
│ │ │ │ -
235std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projectionMatricesFromPoses(
│ │ │ │ -
236 const std::vector<Pose3> &poses, boost::shared_ptr<CALIBRATION> sharedCal) {
│ │ │ │ -
237 std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
│ │ │ │ -
238 for (size_t i = 0; i < poses.size(); i++) {
│ │ │ │ -
239 PinholePose<CALIBRATION> camera(poses.at(i), sharedCal);
│ │ │ │ -
240 projection_matrices.push_back(camera.cameraProjectionMatrix());
│ │ │ │ -
241 }
│ │ │ │ -
242 return projection_matrices;
│ │ │ │ -
243}
│ │ │ │ -
244
│ │ │ │ -
252template <class CALIBRATION>
│ │ │ │ -
│ │ │ │ -
253Cal3_S2 createPinholeCalibration(const CALIBRATION& cal) {
│ │ │ │ -
254 const auto& K = cal.K();
│ │ │ │ -
255 return Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2));
│ │ │ │ -
256}
│ │ │ │ -
│ │ │ │ -
257
│ │ │ │ -
260template <class CALIBRATION, class MEASUREMENT>
│ │ │ │ -
│ │ │ │ -
261MEASUREMENT undistortMeasurementInternal(
│ │ │ │ -
262 const CALIBRATION& cal, const MEASUREMENT& measurement,
│ │ │ │ -
263 boost::optional<Cal3_S2> pinholeCal = boost::none) {
│ │ │ │ -
264 if (!pinholeCal) {
│ │ │ │ -
265 pinholeCal = createPinholeCalibration(cal);
│ │ │ │ -
266 }
│ │ │ │ -
267 return pinholeCal->uncalibrate(cal.calibrate(measurement));
│ │ │ │ -
268}
│ │ │ │ -
│ │ │ │ -
269
│ │ │ │ -
281template <class CALIBRATION>
│ │ │ │ -
│ │ │ │ -
282Point2Vector undistortMeasurements(const CALIBRATION& cal,
│ │ │ │ -
283 const Point2Vector& measurements) {
│ │ │ │ -
284 Cal3_S2 pinholeCalibration = createPinholeCalibration(cal);
│ │ │ │ -
285 Point2Vector undistortedMeasurements;
│ │ │ │ -
286 // Calibrate with cal and uncalibrate with pinhole version of cal so that
│ │ │ │ -
287 // measurements are undistorted.
│ │ │ │ -
288 std::transform(measurements.begin(), measurements.end(),
│ │ │ │ -
289 std::back_inserter(undistortedMeasurements),
│ │ │ │ -
290 [&cal, &pinholeCalibration](const Point2& measurement) {
│ │ │ │ -
291 return undistortMeasurementInternal<CALIBRATION>(
│ │ │ │ -
292 cal, measurement, pinholeCalibration);
│ │ │ │ -
293 });
│ │ │ │ -
294 return undistortedMeasurements;
│ │ │ │ -
295}
│ │ │ │ -
│ │ │ │ -
296
│ │ │ │ -
298template <>
│ │ │ │ -
│ │ │ │ -
299inline Point2Vector undistortMeasurements(const Cal3_S2& cal,
│ │ │ │ -
300 const Point2Vector& measurements) {
│ │ │ │ -
301 return measurements;
│ │ │ │ -
302}
│ │ │ │ -
│ │ │ │ -
303
│ │ │ │ -
315template <class CAMERA>
│ │ │ │ -
│ │ │ │ -
316typename CAMERA::MeasurementVector undistortMeasurements(
│ │ │ │ -
317 const CameraSet<CAMERA>& cameras,
│ │ │ │ -
318 const typename CAMERA::MeasurementVector& measurements) {
│ │ │ │ -
319 const size_t nrMeasurements = measurements.size();
│ │ │ │ -
320 assert(nrMeasurements == cameras.size());
│ │ │ │ -
321 typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements);
│ │ │ │ -
322 for (size_t ii = 0; ii < nrMeasurements; ++ii) {
│ │ │ │ -
323 // Calibrate with cal and uncalibrate with pinhole version of cal so that
│ │ │ │ -
324 // measurements are undistorted.
│ │ │ │ -
325 undistortedMeasurements[ii] =
│ │ │ │ -
326 undistortMeasurementInternal<typename CAMERA::CalibrationType>(
│ │ │ │ -
327 cameras[ii].calibration(), measurements[ii]);
│ │ │ │ -
328 }
│ │ │ │ -
329 return undistortedMeasurements;
│ │ │ │ -
330}
│ │ │ │ +
│ │ │ │ +
169 bool equals(const GncOptimizer& other, double tol = 1e-9) const {
│ │ │ │ +
170 return nfg_.equals(other.getFactors())
│ │ │ │ +
171 && equal(weights_, other.getWeights())
│ │ │ │ +
172 && params_.equals(other.getParams())
│ │ │ │ +
173 && equal(barcSq_, other.getInlierCostThresholds());
│ │ │ │ +
174 }
│ │ │ │ +
│ │ │ │ +
175
│ │ │ │ +
176 Vector initializeWeightsFromKnownInliersAndOutliers() const{
│ │ │ │ +
177 Vector weights = Vector::Ones(nfg_.size());
│ │ │ │ +
178 for (size_t i = 0; i < params_.knownOutliers.size(); i++){
│ │ │ │ +
179 weights[ params_.knownOutliers[i] ] = 0.0; // known to be outliers
│ │ │ │ +
180 }
│ │ │ │ +
181 return weights;
│ │ │ │ +
182 }
│ │ │ │ +
183
│ │ │ │ +
│ │ │ │ +
185 Values optimize() {
│ │ │ │ +
186 NonlinearFactorGraph graph_initial = this->makeWeightedGraph(weights_);
│ │ │ │ +
187 BaseOptimizer baseOptimizer(
│ │ │ │ +
188 graph_initial, state_, params_.baseOptimizerParams);
│ │ │ │ +
189 Values result = baseOptimizer.optimize();
│ │ │ │ +
190 double mu = initializeMu();
│ │ │ │ +
191 double prev_cost = graph_initial.error(result);
│ │ │ │ +
192 double cost = 0.0; // this will be updated in the main loop
│ │ │ │ +
193
│ │ │ │ +
194 // handle the degenerate case that corresponds to small
│ │ │ │ +
195 // maximum residual errors at initialization
│ │ │ │ +
196 // For GM: if residual error is small, mu -> 0
│ │ │ │ +
197 // For TLS: if residual error is small, mu -> -1
│ │ │ │ +
198 int nrUnknownInOrOut = nfg_.size() - ( params_.knownInliers.size() + params_.knownOutliers.size() );
│ │ │ │ +
199 // ^^ number of measurements that are not known to be inliers or outliers (GNC will need to figure them out)
│ │ │ │ +
200 if (mu <= 0 || nrUnknownInOrOut == 0) { // no need to even call GNC in this case
│ │ │ │ +
201 if (mu <= 0 && params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
│ │ │ │ +
202 std::cout << "GNC Optimizer stopped because maximum residual at "
│ │ │ │ +
203 "initialization is small."
│ │ │ │ +
204 << std::endl;
│ │ │ │ +
205 }
│ │ │ │ +
206 if (nrUnknownInOrOut==0 && params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
│ │ │ │ +
207 std::cout << "GNC Optimizer stopped because all measurements are already known to be inliers or outliers"
│ │ │ │ +
208 << std::endl;
│ │ │ │ +
209 }
│ │ │ │ +
210 if (params_.verbosity >= GncParameters::Verbosity::MU) {
│ │ │ │ +
211 std::cout << "mu: " << mu << std::endl;
│ │ │ │ +
212 }
│ │ │ │ +
213 if (params_.verbosity >= GncParameters::Verbosity::VALUES) {
│ │ │ │ +
214 result.print("result\n");
│ │ │ │ +
215 }
│ │ │ │ +
216 return result;
│ │ │ │ +
217 }
│ │ │ │ +
218
│ │ │ │ +
219 size_t iter;
│ │ │ │ +
220 for (iter = 0; iter < params_.maxIterations; iter++) {
│ │ │ │ +
221
│ │ │ │ +
222 // display info
│ │ │ │ +
223 if (params_.verbosity >= GncParameters::Verbosity::MU) {
│ │ │ │ +
224 std::cout << "iter: " << iter << std::endl;
│ │ │ │ +
225 std::cout << "mu: " << mu << std::endl;
│ │ │ │ +
226 }
│ │ │ │ +
227 if (params_.verbosity >= GncParameters::Verbosity::WEIGHTS) {
│ │ │ │ +
228 std::cout << "weights: " << weights_ << std::endl;
│ │ │ │ +
229 }
│ │ │ │ +
230 if (params_.verbosity >= GncParameters::Verbosity::VALUES) {
│ │ │ │ +
231 result.print("result\n");
│ │ │ │ +
232 }
│ │ │ │ +
233 // weights update
│ │ │ │ +
234 weights_ = calculateWeights(result, mu);
│ │ │ │ +
235
│ │ │ │ +
236 // variable/values update
│ │ │ │ +
237 NonlinearFactorGraph graph_iter = this->makeWeightedGraph(weights_);
│ │ │ │ +
238 BaseOptimizer baseOptimizer_iter(
│ │ │ │ +
239 graph_iter, state_, params_.baseOptimizerParams);
│ │ │ │ +
240 result = baseOptimizer_iter.optimize();
│ │ │ │ +
241
│ │ │ │ +
242 // stopping condition
│ │ │ │ +
243 cost = graph_iter.error(result);
│ │ │ │ +
244 if (checkConvergence(mu, weights_, cost, prev_cost)) {
│ │ │ │ +
245 break;
│ │ │ │ +
246 }
│ │ │ │ +
247
│ │ │ │ +
248 // update mu
│ │ │ │ +
249 mu = updateMu(mu);
│ │ │ │ +
250
│ │ │ │ +
251 // get ready for next iteration
│ │ │ │ +
252 prev_cost = cost;
│ │ │ │ +
253
│ │ │ │ +
254 // display info
│ │ │ │ +
255 if (params_.verbosity >= GncParameters::Verbosity::VALUES) {
│ │ │ │ +
256 std::cout << "previous cost: " << prev_cost << std::endl;
│ │ │ │ +
257 std::cout << "current cost: " << cost << std::endl;
│ │ │ │ +
258 }
│ │ │ │ +
259 }
│ │ │ │ +
260 // display info
│ │ │ │ +
261 if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
│ │ │ │ +
262 std::cout << "final iterations: " << iter << std::endl;
│ │ │ │ +
263 std::cout << "final mu: " << mu << std::endl;
│ │ │ │ +
264 std::cout << "previous cost: " << prev_cost << std::endl;
│ │ │ │ +
265 std::cout << "current cost: " << cost << std::endl;
│ │ │ │ +
266 }
│ │ │ │ +
267 if (params_.verbosity >= GncParameters::Verbosity::WEIGHTS) {
│ │ │ │ +
268 std::cout << "final weights: " << weights_ << std::endl;
│ │ │ │ +
269 }
│ │ │ │ +
270 return result;
│ │ │ │ +
271 }
│ │ │ │ +
│ │ │ │ +
272
│ │ │ │ +
│ │ │ │ +
274 double initializeMu() const {
│ │ │ │ +
275
│ │ │ │ +
276 double mu_init = 0.0;
│ │ │ │ +
277 // initialize mu to the value specified in Remark 5 in GNC paper.
│ │ │ │ +
278 switch (params_.lossType) {
│ │ │ │ +
279 case GncLossType::GM:
│ │ │ │ +
280 /* surrogate cost is convex for large mu. initialize as in remark 5 in GNC paper.
│ │ │ │ +
281 Since barcSq_ can be different for each factor, we compute the max of the quantity in remark 5 in GNC paper
│ │ │ │ +
282 */
│ │ │ │ +
283 for (size_t k = 0; k < nfg_.size(); k++) {
│ │ │ │ +
284 if (nfg_[k]) {
│ │ │ │ +
285 mu_init = std::max(mu_init, 2 * nfg_[k]->error(state_) / barcSq_[k]);
│ │ │ │ +
286 }
│ │ │ │ +
287 }
│ │ │ │ +
288 return mu_init; // initial mu
│ │ │ │ +
289 case GncLossType::TLS:
│ │ │ │ +
290 /* surrogate cost is convex for mu close to zero. initialize as in remark 5 in GNC paper.
│ │ │ │ +
291 degenerate case: 2 * rmax_sq - params_.barcSq < 0 (handled in the main loop)
│ │ │ │ +
292 according to remark mu = params_.barcSq / (2 * rmax_sq - params_.barcSq) = params_.barcSq/ excessResidual
│ │ │ │ +
293 however, if the denominator is 0 or negative, we return mu = -1 which leads to termination of the main GNC loop.
│ │ │ │ +
294 Since barcSq_ can be different for each factor, we look for the minimimum (positive) quantity in remark 5 in GNC paper
│ │ │ │ +
295 */
│ │ │ │ +
296 mu_init = std::numeric_limits<double>::infinity();
│ │ │ │ +
297 for (size_t k = 0; k < nfg_.size(); k++) {
│ │ │ │ +
298 if (nfg_[k]) {
│ │ │ │ +
299 double rk = nfg_[k]->error(state_);
│ │ │ │ +
300 mu_init = (2 * rk - barcSq_[k]) > 0 ? // if positive, update mu, otherwise keep same
│ │ │ │ +
301 std::min(mu_init, barcSq_[k] / (2 * rk - barcSq_[k]) ) : mu_init;
│ │ │ │ +
302 }
│ │ │ │ +
303 }
│ │ │ │ +
304 if (mu_init >= 0 && mu_init < 1e-6){
│ │ │ │ +
305 mu_init = 1e-6; // if mu ~ 0 (but positive), that means we have measurements with large errors,
│ │ │ │ +
306 // i.e., rk > barcSq_[k] and rk very large, hence we threshold to 1e-6 to avoid mu = 0
│ │ │ │ +
307 }
│ │ │ │ +
308
│ │ │ │ +
309 return mu_init > 0 && !std::isinf(mu_init) ? mu_init : -1; // if mu <= 0 or mu = inf, return -1,
│ │ │ │ +
310 // which leads to termination of the main gnc loop. In this case, all residuals are already below the threshold
│ │ │ │ +
311 // and there is no need to robustify (TLS = least squares)
│ │ │ │ +
312 default:
│ │ │ │ +
313 throw std::runtime_error(
│ │ │ │ +
314 "GncOptimizer::initializeMu: called with unknown loss type.");
│ │ │ │ +
315 }
│ │ │ │ +
316 }
│ │ │ │ +
│ │ │ │ +
317
│ │ │ │ +
│ │ │ │ +
319 double updateMu(const double mu) const {
│ │ │ │ +
320 switch (params_.lossType) {
│ │ │ │ +
321 case GncLossType::GM:
│ │ │ │ +
322 // reduce mu, but saturate at 1 (original cost is recovered for mu -> 1)
│ │ │ │ +
323 return std::max(1.0, mu / params_.muStep);
│ │ │ │ +
324 case GncLossType::TLS:
│ │ │ │ +
325 // increases mu at each iteration (original cost is recovered for mu -> inf)
│ │ │ │ +
326 return mu * params_.muStep;
│ │ │ │ +
327 default:
│ │ │ │ +
328 throw std::runtime_error(
│ │ │ │ +
329 "GncOptimizer::updateMu: called with unknown loss type.");
│ │ │ │ +
330 }
│ │ │ │ +
331 }
│ │ │ │
│ │ │ │ -
331
│ │ │ │ -
333template <class CAMERA = PinholeCamera<Cal3_S2>>
│ │ │ │ +
332
│ │ │ │
│ │ │ │ -
334inline PinholeCamera<Cal3_S2>::MeasurementVector undistortMeasurements(
│ │ │ │ -
335 const CameraSet<PinholeCamera<Cal3_S2>>& cameras,
│ │ │ │ -
336 const PinholeCamera<Cal3_S2>::MeasurementVector& measurements) {
│ │ │ │ -
337 return measurements;
│ │ │ │ -
338}
│ │ │ │ -
│ │ │ │ -
339
│ │ │ │ -
341template <class CAMERA = SphericalCamera>
│ │ │ │ -
│ │ │ │ -
342inline SphericalCamera::MeasurementVector undistortMeasurements(
│ │ │ │ -
343 const CameraSet<SphericalCamera>& cameras,
│ │ │ │ -
344 const SphericalCamera::MeasurementVector& measurements) {
│ │ │ │ -
345 return measurements;
│ │ │ │ -
346}
│ │ │ │ -
│ │ │ │ -
347
│ │ │ │ -
356template <class CALIBRATION>
│ │ │ │ -
│ │ │ │ -
357inline Point3Vector calibrateMeasurementsShared(
│ │ │ │ -
358 const CALIBRATION& cal, const Point2Vector& measurements) {
│ │ │ │ -
359 Point3Vector calibratedMeasurements;
│ │ │ │ -
360 // Calibrate with cal and uncalibrate with pinhole version of cal so that
│ │ │ │ -
361 // measurements are undistorted.
│ │ │ │ -
362 std::transform(measurements.begin(), measurements.end(),
│ │ │ │ -
363 std::back_inserter(calibratedMeasurements),
│ │ │ │ -
364 [&cal](const Point2& measurement) {
│ │ │ │ -
365 Point3 p;
│ │ │ │ -
366 p << cal.calibrate(measurement), 1.0;
│ │ │ │ -
367 return p;
│ │ │ │ -
368 });
│ │ │ │ -
369 return calibratedMeasurements;
│ │ │ │ -
370}
│ │ │ │ -
│ │ │ │ -
371
│ │ │ │ -
380template <class CAMERA>
│ │ │ │ -
│ │ │ │ -
381inline Point3Vector calibrateMeasurements(
│ │ │ │ -
382 const CameraSet<CAMERA>& cameras,
│ │ │ │ -
383 const typename CAMERA::MeasurementVector& measurements) {
│ │ │ │ -
384 const size_t nrMeasurements = measurements.size();
│ │ │ │ -
385 assert(nrMeasurements == cameras.size());
│ │ │ │ -
386 Point3Vector calibratedMeasurements(nrMeasurements);
│ │ │ │ -
387 for (size_t ii = 0; ii < nrMeasurements; ++ii) {
│ │ │ │ -
388 calibratedMeasurements[ii]
│ │ │ │ -
389 << cameras[ii].calibration().calibrate(measurements[ii]),
│ │ │ │ -
390 1.0;
│ │ │ │ -
391 }
│ │ │ │ -
392 return calibratedMeasurements;
│ │ │ │ -
393}
│ │ │ │ -
│ │ │ │ -
394
│ │ │ │ -
396template <class CAMERA = SphericalCamera>
│ │ │ │ -
│ │ │ │ -
397inline Point3Vector calibrateMeasurements(
│ │ │ │ -
398 const CameraSet<SphericalCamera>& cameras,
│ │ │ │ -
399 const SphericalCamera::MeasurementVector& measurements) {
│ │ │ │ -
400 Point3Vector calibratedMeasurements(measurements.size());
│ │ │ │ -
401 for (size_t ii = 0; ii < measurements.size(); ++ii) {
│ │ │ │ -
402 calibratedMeasurements[ii] << measurements[ii].point3();
│ │ │ │ -
403 }
│ │ │ │ -
404 return calibratedMeasurements;
│ │ │ │ -
405}
│ │ │ │ -
│ │ │ │ -
406
│ │ │ │ -
420template <class CALIBRATION>
│ │ │ │ -
│ │ │ │ -
421Point3 triangulatePoint3(const std::vector<Pose3>& poses,
│ │ │ │ -
422 boost::shared_ptr<CALIBRATION> sharedCal,
│ │ │ │ -
423 const Point2Vector& measurements,
│ │ │ │ -
424 double rank_tol = 1e-9, bool optimize = false,
│ │ │ │ -
425 const SharedNoiseModel& model = nullptr,
│ │ │ │ -
426 const bool useLOST = false) {
│ │ │ │ -
427 assert(poses.size() == measurements.size());
│ │ │ │ -
428 if (poses.size() < 2) throw(TriangulationUnderconstrainedException());
│ │ │ │ -
429
│ │ │ │ -
430 // Triangulate linearly
│ │ │ │ -
431 Point3 point;
│ │ │ │ -
432 if (useLOST) {
│ │ │ │ -
433 // Reduce input noise model to an isotropic noise model using the mean of
│ │ │ │ -
434 // the diagonal.
│ │ │ │ -
435 const double measurementSigma = model ? model->sigmas().mean() : 1e-4;
│ │ │ │ -
436 SharedIsotropic measurementNoise =
│ │ │ │ -
437 noiseModel::Isotropic::Sigma(2, measurementSigma);
│ │ │ │ -
438 // calibrate the measurements to obtain homogenous coordinates in image
│ │ │ │ -
439 // plane.
│ │ │ │ -
440 auto calibratedMeasurements =
│ │ │ │ -
441 calibrateMeasurementsShared<CALIBRATION>(*sharedCal, measurements);
│ │ │ │ -
442
│ │ │ │ -
443 point = triangulateLOST(poses, calibratedMeasurements, measurementNoise);
│ │ │ │ -
444 } else {
│ │ │ │ -
445 // construct projection matrices from poses & calibration
│ │ │ │ -
446 auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal);
│ │ │ │ -
447
│ │ │ │ -
448 // Undistort the measurements, leaving only the pinhole elements in effect.
│ │ │ │ -
449 auto undistortedMeasurements =
│ │ │ │ -
450 undistortMeasurements<CALIBRATION>(*sharedCal, measurements);
│ │ │ │ -
451
│ │ │ │ -
452 point =
│ │ │ │ -
453 triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
│ │ │ │ -
454 }
│ │ │ │ -
455
│ │ │ │ -
456 // Then refine using non-linear optimization
│ │ │ │ -
457 if (optimize)
│ │ │ │ -
458 point = triangulateNonlinear<CALIBRATION> //
│ │ │ │ -
459 (poses, sharedCal, measurements, point, model);
│ │ │ │ -
460
│ │ │ │ -
461#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
│ │ │ │ -
462 // verify that the triangulated point lies in front of all cameras
│ │ │ │ -
463 for (const Pose3& pose : poses) {
│ │ │ │ -
464 const Point3& p_local = pose.transformTo(point);
│ │ │ │ -
465 if (p_local.z() <= 0) throw(TriangulationCheiralityException());
│ │ │ │ -
466 }
│ │ │ │ -
467#endif
│ │ │ │ -
468
│ │ │ │ -
469 return point;
│ │ │ │ -
470}
│ │ │ │ -
│ │ │ │ -
471
│ │ │ │ -
486template <class CAMERA>
│ │ │ │ -
│ │ │ │ -
487Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
│ │ │ │ -
488 const typename CAMERA::MeasurementVector& measurements,
│ │ │ │ -
489 double rank_tol = 1e-9, bool optimize = false,
│ │ │ │ -
490 const SharedNoiseModel& model = nullptr,
│ │ │ │ -
491 const bool useLOST = false) {
│ │ │ │ -
492 size_t m = cameras.size();
│ │ │ │ -
493 assert(measurements.size() == m);
│ │ │ │ -
494
│ │ │ │ -
495 if (m < 2) throw(TriangulationUnderconstrainedException());
│ │ │ │ -
496
│ │ │ │ -
497 // Triangulate linearly
│ │ │ │ -
498 Point3 point;
│ │ │ │ -
499 if (useLOST) {
│ │ │ │ -
500 // Reduce input noise model to an isotropic noise model using the mean of
│ │ │ │ -
501 // the diagonal.
│ │ │ │ -
502 const double measurementSigma = model ? model->sigmas().mean() : 1e-4;
│ │ │ │ -
503 SharedIsotropic measurementNoise =
│ │ │ │ -
504 noiseModel::Isotropic::Sigma(2, measurementSigma);
│ │ │ │ -
505
│ │ │ │ -
506 // construct poses from cameras.
│ │ │ │ -
507 std::vector<Pose3> poses;
│ │ │ │ -
508 poses.reserve(cameras.size());
│ │ │ │ -
509 for (const auto& camera : cameras) poses.push_back(camera.pose());
│ │ │ │ -
510
│ │ │ │ -
511 // calibrate the measurements to obtain homogenous coordinates in image
│ │ │ │ -
512 // plane.
│ │ │ │ -
513 auto calibratedMeasurements =
│ │ │ │ -
514 calibrateMeasurements<CAMERA>(cameras, measurements);
│ │ │ │ -
515
│ │ │ │ -
516 point = triangulateLOST(poses, calibratedMeasurements, measurementNoise);
│ │ │ │ -
517 } else {
│ │ │ │ -
518 // construct projection matrices from poses & calibration
│ │ │ │ -
519 auto projection_matrices = projectionMatricesFromCameras(cameras);
│ │ │ │ -
520
│ │ │ │ -
521 // Undistort the measurements, leaving only the pinhole elements in effect.
│ │ │ │ -
522 auto undistortedMeasurements =
│ │ │ │ -
523 undistortMeasurements<CAMERA>(cameras, measurements);
│ │ │ │ -
524
│ │ │ │ -
525 point =
│ │ │ │ -
526 triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
│ │ │ │ -
527 }
│ │ │ │ -
528
│ │ │ │ -
529 // Then refine using non-linear optimization
│ │ │ │ -
530 if (optimize) {
│ │ │ │ -
531 point = triangulateNonlinear<CAMERA>(cameras, measurements, point, model);
│ │ │ │ -
532 }
│ │ │ │ -
533
│ │ │ │ -
534#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
│ │ │ │ -
535 // verify that the triangulated point lies in front of all cameras
│ │ │ │ -
536 for (const CAMERA& camera : cameras) {
│ │ │ │ -
537 const Point3& p_local = camera.pose().transformTo(point);
│ │ │ │ -
538 if (p_local.z() <= 0) throw(TriangulationCheiralityException());
│ │ │ │ -
539 }
│ │ │ │ -
540#endif
│ │ │ │ -
541
│ │ │ │ -
542 return point;
│ │ │ │ -
543}
│ │ │ │ -
│ │ │ │ -
544
│ │ │ │ -
546template <class CALIBRATION>
│ │ │ │ -
│ │ │ │ -
547Point3 triangulatePoint3(const CameraSet<PinholeCamera<CALIBRATION>>& cameras,
│ │ │ │ -
548 const Point2Vector& measurements,
│ │ │ │ -
549 double rank_tol = 1e-9, bool optimize = false,
│ │ │ │ -
550 const SharedNoiseModel& model = nullptr,
│ │ │ │ -
551 const bool useLOST = false) {
│ │ │ │ -
552 return triangulatePoint3<PinholeCamera<CALIBRATION>> //
│ │ │ │ -
553 (cameras, measurements, rank_tol, optimize, model, useLOST);
│ │ │ │ -
554}
│ │ │ │ -
│ │ │ │ -
555
│ │ │ │ -
│ │ │ │ -
556struct GTSAM_EXPORT TriangulationParameters {
│ │ │ │ -
557
│ │ │ │ -
558 double rankTolerance;
│ │ │ │ -
560 bool enableEPI;
│ │ │ │ -
561
│ │ │ │ -
566 double landmarkDistanceThreshold; //
│ │ │ │ -
567
│ │ │ │ -
573 double dynamicOutlierRejectionThreshold;
│ │ │ │ -
574
│ │ │ │ -
575 SharedNoiseModel noiseModel;
│ │ │ │ -
576
│ │ │ │ -
│ │ │ │ -
586 TriangulationParameters(const double _rankTolerance = 1.0,
│ │ │ │ -
587 const bool _enableEPI = false, double _landmarkDistanceThreshold = -1,
│ │ │ │ -
588 double _dynamicOutlierRejectionThreshold = -1,
│ │ │ │ -
589 const SharedNoiseModel& _noiseModel = nullptr) :
│ │ │ │ -
590 rankTolerance(_rankTolerance), enableEPI(_enableEPI), //
│ │ │ │ -
591 landmarkDistanceThreshold(_landmarkDistanceThreshold), //
│ │ │ │ -
592 dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold),
│ │ │ │ -
593 noiseModel(_noiseModel){
│ │ │ │ -
594 }
│ │ │ │ -
│ │ │ │ -
595
│ │ │ │ -
596 // stream to output
│ │ │ │ -
597 friend std::ostream &operator<<(std::ostream &os,
│ │ │ │ -
598 const TriangulationParameters& p) {
│ │ │ │ -
599 os << "rankTolerance = " << p.rankTolerance << std::endl;
│ │ │ │ -
600 os << "enableEPI = " << p.enableEPI << std::endl;
│ │ │ │ -
601 os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold
│ │ │ │ -
602 << std::endl;
│ │ │ │ -
603 os << "dynamicOutlierRejectionThreshold = "
│ │ │ │ -
604 << p.dynamicOutlierRejectionThreshold << std::endl;
│ │ │ │ -
605 os << "noise model" << std::endl;
│ │ │ │ -
606 return os;
│ │ │ │ -
607 }
│ │ │ │ -
608
│ │ │ │ -
609private:
│ │ │ │ -
610
│ │ │ │ -
612 friend class boost::serialization::access;
│ │ │ │ -
613 template<class ARCHIVE>
│ │ │ │ -
614 void serialize(ARCHIVE & ar, const unsigned int version) {
│ │ │ │ -
615 ar & BOOST_SERIALIZATION_NVP(rankTolerance);
│ │ │ │ -
616 ar & BOOST_SERIALIZATION_NVP(enableEPI);
│ │ │ │ -
617 ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold);
│ │ │ │ -
618 ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold);
│ │ │ │ -
619 }
│ │ │ │ -
620};
│ │ │ │ -
│ │ │ │ -
621
│ │ │ │ -
│ │ │ │ -
626class TriangulationResult : public boost::optional<Point3> {
│ │ │ │ -
627 public:
│ │ │ │ -
628 enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT };
│ │ │ │ -
629 Status status;
│ │ │ │ -
630
│ │ │ │ -
631 private:
│ │ │ │ -
632 TriangulationResult(Status s) : status(s) {}
│ │ │ │ -
633
│ │ │ │ -
634 public:
│ │ │ │ -
638 TriangulationResult() {}
│ │ │ │ -
639
│ │ │ │ -
643 TriangulationResult(const Point3& p) : status(VALID) { reset(p); }
│ │ │ │ -
644 static TriangulationResult Degenerate() {
│ │ │ │ -
645 return TriangulationResult(DEGENERATE);
│ │ │ │ -
646 }
│ │ │ │ -
647 static TriangulationResult Outlier() { return TriangulationResult(OUTLIER); }
│ │ │ │ -
648 static TriangulationResult FarPoint() {
│ │ │ │ -
649 return TriangulationResult(FAR_POINT);
│ │ │ │ -
650 }
│ │ │ │ -
651 static TriangulationResult BehindCamera() {
│ │ │ │ -
652 return TriangulationResult(BEHIND_CAMERA);
│ │ │ │ -
653 }
│ │ │ │ -
654 bool valid() const { return status == VALID; }
│ │ │ │ -
655 bool degenerate() const { return status == DEGENERATE; }
│ │ │ │ -
656 bool outlier() const { return status == OUTLIER; }
│ │ │ │ -
657 bool farPoint() const { return status == FAR_POINT; }
│ │ │ │ -
658 bool behindCamera() const { return status == BEHIND_CAMERA; }
│ │ │ │ -
659 // stream to output
│ │ │ │ -
660 friend std::ostream& operator<<(std::ostream& os,
│ │ │ │ -
661 const TriangulationResult& result) {
│ │ │ │ -
662 if (result)
│ │ │ │ -
663 os << "point = " << *result << std::endl;
│ │ │ │ -
664 else
│ │ │ │ -
665 os << "no point, status = " << result.status << std::endl;
│ │ │ │ -
666 return os;
│ │ │ │ -
667 }
│ │ │ │ -
668
│ │ │ │ -
669 private:
│ │ │ │ -
671 friend class boost::serialization::access;
│ │ │ │ -
672 template <class ARCHIVE>
│ │ │ │ -
673 void serialize(ARCHIVE& ar, const unsigned int version) {
│ │ │ │ -
674 ar& BOOST_SERIALIZATION_NVP(status);
│ │ │ │ -
675 }
│ │ │ │ -
676};
│ │ │ │ -
│ │ │ │ -
677
│ │ │ │ -
679template<class CAMERA>
│ │ │ │ -
│ │ │ │ -
680TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
│ │ │ │ -
681 const typename CAMERA::MeasurementVector& measured,
│ │ │ │ -
682 const TriangulationParameters& params) {
│ │ │ │ -
683
│ │ │ │ -
684 size_t m = cameras.size();
│ │ │ │ -
685
│ │ │ │ -
686 // if we have a single pose the corresponding factor is uninformative
│ │ │ │ -
687 if (m < 2)
│ │ │ │ -
688 return TriangulationResult::Degenerate();
│ │ │ │ -
689 else
│ │ │ │ -
690 // We triangulate the 3D position of the landmark
│ │ │ │ -
691 try {
│ │ │ │ -
692 Point3 point =
│ │ │ │ -
693 triangulatePoint3<CAMERA>(cameras, measured, params.rankTolerance,
│ │ │ │ -
694 params.enableEPI, params.noiseModel);
│ │ │ │ -
695
│ │ │ │ -
696 // Check landmark distance and re-projection errors to avoid outliers
│ │ │ │ -
697 size_t i = 0;
│ │ │ │ -
698 double maxReprojError = 0.0;
│ │ │ │ -
699 for(const CAMERA& camera: cameras) {
│ │ │ │ -
700 const Pose3& pose = camera.pose();
│ │ │ │ -
701 if (params.landmarkDistanceThreshold > 0
│ │ │ │ -
702 && distance3(pose.translation(), point)
│ │ │ │ -
703 > params.landmarkDistanceThreshold)
│ │ │ │ -
704 return TriangulationResult::FarPoint();
│ │ │ │ -
705#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
│ │ │ │ -
706 // verify that the triangulated point lies in front of all cameras
│ │ │ │ -
707 // Only needed if this was not yet handled by exception
│ │ │ │ -
708 const Point3& p_local = pose.transformTo(point);
│ │ │ │ -
709 if (p_local.z() <= 0)
│ │ │ │ -
710 return TriangulationResult::BehindCamera();
│ │ │ │ -
711#endif
│ │ │ │ -
712 // Check reprojection error
│ │ │ │ -
713 if (params.dynamicOutlierRejectionThreshold > 0) {
│ │ │ │ -
714 const typename CAMERA::Measurement& zi = measured.at(i);
│ │ │ │ -
715 Point2 reprojectionError = camera.reprojectionError(point, zi);
│ │ │ │ -
716 maxReprojError = std::max(maxReprojError, reprojectionError.norm());
│ │ │ │ -
717 }
│ │ │ │ -
718 i += 1;
│ │ │ │ -
719 }
│ │ │ │ -
720 // Flag as degenerate if average reprojection error is too large
│ │ │ │ -
721 if (params.dynamicOutlierRejectionThreshold > 0
│ │ │ │ -
722 && maxReprojError > params.dynamicOutlierRejectionThreshold)
│ │ │ │ -
723 return TriangulationResult::Outlier();
│ │ │ │ -
724
│ │ │ │ -
725 // all good!
│ │ │ │ -
726 return TriangulationResult(point);
│ │ │ │ -
727 } catch (TriangulationUnderconstrainedException&) {
│ │ │ │ -
728 // This exception is thrown if
│ │ │ │ -
729 // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
│ │ │ │ -
730 // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
│ │ │ │ -
731 return TriangulationResult::Degenerate();
│ │ │ │ -
732 } catch (TriangulationCheiralityException&) {
│ │ │ │ -
733 // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
│ │ │ │ -
734 return TriangulationResult::BehindCamera();
│ │ │ │ -
735 }
│ │ │ │ -
736}
│ │ │ │ -
│ │ │ │ -
737
│ │ │ │ -
738// Vector of Cameras - used by the Python/MATLAB wrapper
│ │ │ │ -
739using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
│ │ │ │ -
740using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
│ │ │ │ -
741using CameraSetCal3DS2 = CameraSet<PinholeCamera<Cal3DS2>>;
│ │ │ │ -
742using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
│ │ │ │ -
743using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
│ │ │ │ -
744using CameraSetSpherical = CameraSet<SphericalCamera>;
│ │ │ │ -
745} // \namespace gtsam
│ │ │ │ -
746
│ │ │ │ -
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
│ │ │ │ -
CameraSet.h
Base class to create smart factors on poses or cameras.
│ │ │ │ -
Cal3Unified.h
Unified Calibration Model, see Mei07icra for details.
│ │ │ │ -
Cal3Bundler.h
Calibration used by Bundler.
│ │ │ │ -
SphericalCamera.h
Calibrated camera with spherical projection.
│ │ │ │ -
PinholeCamera.h
Base class for all pinhole cameras.
│ │ │ │ -
Cal3Fisheye.h
Calibration of a fisheye camera.
│ │ │ │ -
Pose2.h
2D Pose
│ │ │ │ -
Cal3DS2.h
Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base.
│ │ │ │ -
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │ +
334 bool checkMuConvergence(const double mu) const {
│ │ │ │ +
335 bool muConverged = false;
│ │ │ │ +
336 switch (params_.lossType) {
│ │ │ │ +
337 case GncLossType::GM:
│ │ │ │ +
338 muConverged = std::fabs(mu - 1.0) < 1e-9; // mu=1 recovers the original GM function
│ │ │ │ +
339 break;
│ │ │ │ +
340 case GncLossType::TLS:
│ │ │ │ +
341 muConverged = false; // for TLS there is no stopping condition on mu (it must tend to infinity)
│ │ │ │ +
342 break;
│ │ │ │ +
343 default:
│ │ │ │ +
344 throw std::runtime_error(
│ │ │ │ +
345 "GncOptimizer::checkMuConvergence: called with unknown loss type.");
│ │ │ │ +
346 }
│ │ │ │ +
347 if (muConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY)
│ │ │ │ +
348 std::cout << "muConverged = true " << std::endl;
│ │ │ │ +
349 return muConverged;
│ │ │ │ +
350 }
│ │ │ │ +
│ │ │ │ +
351
│ │ │ │ +
│ │ │ │ +
353 bool checkCostConvergence(const double cost, const double prev_cost) const {
│ │ │ │ +
354 bool costConverged = std::fabs(cost - prev_cost) / std::max(prev_cost, 1e-7)
│ │ │ │ +
355 < params_.relativeCostTol;
│ │ │ │ +
356 if (costConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY){
│ │ │ │ +
357 std::cout << "checkCostConvergence = true (prev. cost = " << prev_cost
│ │ │ │ +
358 << ", curr. cost = " << cost << ")" << std::endl;
│ │ │ │ +
359 }
│ │ │ │ +
360 return costConverged;
│ │ │ │ +
361 }
│ │ │ │ +
│ │ │ │ +
362
│ │ │ │ +
│ │ │ │ +
364 bool checkWeightsConvergence(const Vector& weights) const {
│ │ │ │ +
365 bool weightsConverged = false;
│ │ │ │ +
366 switch (params_.lossType) {
│ │ │ │ +
367 case GncLossType::GM:
│ │ │ │ +
368 weightsConverged = false; // for GM, there is no clear binary convergence for the weights
│ │ │ │ +
369 break;
│ │ │ │ +
370 case GncLossType::TLS:
│ │ │ │ +
371 weightsConverged = true;
│ │ │ │ +
372 for (int i = 0; i < weights.size(); i++) {
│ │ │ │ +
373 if (std::fabs(weights[i] - std::round(weights[i]))
│ │ │ │ +
374 > params_.weightsTol) {
│ │ │ │ +
375 weightsConverged = false;
│ │ │ │ +
376 break;
│ │ │ │ +
377 }
│ │ │ │ +
378 }
│ │ │ │ +
379 break;
│ │ │ │ +
380 default:
│ │ │ │ +
381 throw std::runtime_error(
│ │ │ │ +
382 "GncOptimizer::checkWeightsConvergence: called with unknown loss type.");
│ │ │ │ +
383 }
│ │ │ │ +
384 if (weightsConverged
│ │ │ │ +
385 && params_.verbosity >= GncParameters::Verbosity::SUMMARY)
│ │ │ │ +
386 std::cout << "weightsConverged = true " << std::endl;
│ │ │ │ +
387 return weightsConverged;
│ │ │ │ +
388 }
│ │ │ │ +
│ │ │ │ +
389
│ │ │ │ +
│ │ │ │ +
391 bool checkConvergence(const double mu, const Vector& weights,
│ │ │ │ +
392 const double cost, const double prev_cost) const {
│ │ │ │ +
393 return checkCostConvergence(cost, prev_cost)
│ │ │ │ +
394 || checkWeightsConvergence(weights) || checkMuConvergence(mu);
│ │ │ │ +
395 }
│ │ │ │ +
│ │ │ │ +
396
│ │ │ │ +
│ │ │ │ +
398 NonlinearFactorGraph makeWeightedGraph(const Vector& weights) const {
│ │ │ │ +
399 // make sure all noiseModels are Gaussian or convert to Gaussian
│ │ │ │ +
400 NonlinearFactorGraph newGraph;
│ │ │ │ +
401 newGraph.resize(nfg_.size());
│ │ │ │ +
402 for (size_t i = 0; i < nfg_.size(); i++) {
│ │ │ │ +
403 if (nfg_[i]) {
│ │ │ │ +
404 auto factor = boost::dynamic_pointer_cast<
│ │ │ │ +
405 NoiseModelFactor>(nfg_[i]);
│ │ │ │ +
406 auto noiseModel =
│ │ │ │ +
407 boost::dynamic_pointer_cast<noiseModel::Gaussian>(
│ │ │ │ +
408 factor->noiseModel());
│ │ │ │ +
409 if (noiseModel) {
│ │ │ │ +
410 Matrix newInfo = weights[i] * noiseModel->information();
│ │ │ │ +
411 auto newNoiseModel = noiseModel::Gaussian::Information(newInfo);
│ │ │ │ +
412 newGraph[i] = factor->cloneWithNewNoiseModel(newNoiseModel);
│ │ │ │ +
413 } else {
│ │ │ │ +
414 throw std::runtime_error(
│ │ │ │ +
415 "GncOptimizer::makeWeightedGraph: unexpected non-Gaussian noise model.");
│ │ │ │ +
416 }
│ │ │ │ +
417 }
│ │ │ │ +
418 }
│ │ │ │ +
419 return newGraph;
│ │ │ │ +
420 }
│ │ │ │ +
│ │ │ │ +
421
│ │ │ │ +
│ │ │ │ +
423 Vector calculateWeights(const Values& currentEstimate, const double mu) {
│ │ │ │ +
424 Vector weights = initializeWeightsFromKnownInliersAndOutliers();
│ │ │ │ +
425
│ │ │ │ +
426 // do not update the weights that the user has decided are known inliers
│ │ │ │ +
427 std::vector<size_t> allWeights;
│ │ │ │ +
428 for (size_t k = 0; k < nfg_.size(); k++) {
│ │ │ │ +
429 allWeights.push_back(k);
│ │ │ │ +
430 }
│ │ │ │ +
431 std::vector<size_t> knownWeights;
│ │ │ │ +
432 std::set_union(params_.knownInliers.begin(), params_.knownInliers.end(),
│ │ │ │ +
433 params_.knownOutliers.begin(), params_.knownOutliers.end(),
│ │ │ │ +
434 std::inserter(knownWeights, knownWeights.begin()));
│ │ │ │ +
435
│ │ │ │ +
436 std::vector<size_t> unknownWeights;
│ │ │ │ +
437 std::set_difference(allWeights.begin(), allWeights.end(),
│ │ │ │ +
438 knownWeights.begin(), knownWeights.end(),
│ │ │ │ +
439 std::inserter(unknownWeights, unknownWeights.begin()));
│ │ │ │ +
440
│ │ │ │ +
441 // update weights of known inlier/outlier measurements
│ │ │ │ +
442 switch (params_.lossType) {
│ │ │ │ +
443 case GncLossType::GM: { // use eq (12) in GNC paper
│ │ │ │ +
444 for (size_t k : unknownWeights) {
│ │ │ │ +
445 if (nfg_[k]) {
│ │ │ │ +
446 double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual
│ │ │ │ +
447 weights[k] = std::pow(
│ │ │ │ +
448 (mu * barcSq_[k]) / (u2_k + mu * barcSq_[k]), 2);
│ │ │ │ +
449 }
│ │ │ │ +
450 }
│ │ │ │ +
451 return weights;
│ │ │ │ +
452 }
│ │ │ │ +
453 case GncLossType::TLS: { // use eq (14) in GNC paper
│ │ │ │ +
454 for (size_t k : unknownWeights) {
│ │ │ │ +
455 if (nfg_[k]) {
│ │ │ │ +
456 double u2_k = nfg_[k]->error(currentEstimate); // squared (and whitened) residual
│ │ │ │ +
457 double upperbound = (mu + 1) / mu * barcSq_[k];
│ │ │ │ +
458 double lowerbound = mu / (mu + 1) * barcSq_[k];
│ │ │ │ +
459 weights[k] = std::sqrt(barcSq_[k] * mu * (mu + 1) / u2_k) - mu;
│ │ │ │ +
460 if (u2_k >= upperbound || weights[k] < 0) {
│ │ │ │ +
461 weights[k] = 0;
│ │ │ │ +
462 } else if (u2_k <= lowerbound || weights[k] > 1) {
│ │ │ │ +
463 weights[k] = 1;
│ │ │ │ +
464 }
│ │ │ │ +
465 }
│ │ │ │ +
466 }
│ │ │ │ +
467 return weights;
│ │ │ │ +
468 }
│ │ │ │ +
469 default:
│ │ │ │ +
470 throw std::runtime_error(
│ │ │ │ +
471 "GncOptimizer::calculateWeights: called with unknown loss type.");
│ │ │ │ +
472 }
│ │ │ │ +
473 }
│ │ │ │ +
│ │ │ │ +
474};
│ │ │ │ +
│ │ │ │ +
475
│ │ │ │ +
476}
│ │ │ │ +
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::calibrateMeasurementsShared
Point3Vector calibrateMeasurementsShared(const CALIBRATION &cal, const Point2Vector &measurements)
Convert pixel measurements in image to homogeneous measurements in the image plane using shared camer...
Definition triangulation.h:357
│ │ │ │ -
gtsam::triangulateLOST
Point3 triangulateLOST(const std::vector< Pose3 > &poses, const Point3Vector &calibratedMeasurements, const SharedIsotropic &measurementNoise)
Triangulation using the LOST (Linear Optimal Sine Triangulation) algorithm proposed in https://arxiv....
Definition triangulation.cpp:92
│ │ │ │ -
gtsam::undistortMeasurements
Point2Vector undistortMeasurements(const CALIBRATION &cal, const Point2Vector &measurements)
Remove distortion for measurements so as if the measurements came from a pinhole camera.
Definition triangulation.h:282
│ │ │ │ -
gtsam::createPinholeCalibration
Cal3_S2 createPinholeCalibration(const CALIBRATION &cal)
Create a pinhole calibration from a different Cal3 object, removing distortion.
Definition triangulation.h:253
│ │ │ │ -
gtsam::undistortMeasurementInternal
MEASUREMENT undistortMeasurementInternal(const CALIBRATION &cal, const MEASUREMENT &measurement, boost::optional< Cal3_S2 > pinholeCal=boost::none)
Internal undistortMeasurement to be used by undistortMeasurement and undistortMeasurements.
Definition triangulation.h:261
│ │ │ │ -
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
│ │ │ │ -
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │ -
gtsam::triangulateSafe
TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters &params)
triangulateSafe: extensive checking of the outcome
Definition triangulation.h:680
│ │ │ │ -
gtsam::triangulateNonlinear
Point3 triangulateNonlinear(const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, const Point3 &initialEstimate, const SharedNoiseModel &model=nullptr)
Given an initial estimate , refine a point using measurements in several cameras.
Definition triangulation.h:187
│ │ │ │ -
gtsam::distance3
double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian< 1, 3 > H1, OptionalJacobian< 1, 3 > H2)
distance between two points
Definition Point3.cpp:27
│ │ │ │ -
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ -
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ -
gtsam::triangulatePoint3
Point3 triangulatePoint3(const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, double rank_tol=1e-9, bool optimize=false, const SharedNoiseModel &model=nullptr, const bool useLOST=false)
Function to triangulate 3D landmark point from an arbitrary number of poses (at least 2) using the DL...
Definition triangulation.h:421
│ │ │ │ -
gtsam::triangulationGraph
std::pair< NonlinearFactorGraph, Values > triangulationGraph(const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, Key landmarkKey, const Point3 &initialEstimate, const SharedNoiseModel &model=noiseModel::Unit::Create(2))
Create a factor graph with projection factors from poses and one calibration.
Definition triangulation.h:122
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::calibrateMeasurements
Point3Vector calibrateMeasurements(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements)
Convert pixel measurements in image to homogeneous measurements in the image plane using camera intri...
Definition triangulation.h:381
│ │ │ │ -
gtsam::triangulateDLT
Point3 triangulateDLT(const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > &projection_matrices, const Point2Vector &measurements, double rank_tol)
DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312.
Definition triangulation.cpp:127
│ │ │ │ -
gtsam::triangulateHomogeneousDLT
Vector4 triangulateHomogeneousDLT(const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > &projection_matrices, const Point2Vector &measurements, double rank_tol)
DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312.
Definition triangulation.cpp:27
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition Cal3_S2.h:34
│ │ │ │ -
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition CameraSet.h:36
│ │ │ │ -
gtsam::PinholeCamera
A pinhole camera class that has a Pose3 and a Calibration.
Definition PinholeCamera.h:33
│ │ │ │ -
gtsam::PinholePose
A pinhole camera class that has a Pose3 and a fixed Calibration.
Definition PinholePose.h:243
│ │ │ │ -
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ -
gtsam::Pose3::transformTo
Point3 transformTo(const Point3 &point, OptionalJacobian< 3, 6 > Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const
takes point in world coordinates and transforms it to Pose coordinates
Definition Pose3.cpp:371
│ │ │ │ -
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get translation
Definition Pose3.cpp:308
│ │ │ │ -
gtsam::TriangulationUnderconstrainedException
Exception thrown by triangulateDLT when SVD returns rank < 3.
Definition triangulation.h:39
│ │ │ │ -
gtsam::TriangulationCheiralityException
Exception thrown by triangulateDLT when landmark is behind one or more of the cameras.
Definition triangulation.h:47
│ │ │ │ -
gtsam::TriangulationParameters
Definition triangulation.h:556
│ │ │ │ -
gtsam::TriangulationParameters::TriangulationParameters
TriangulationParameters(const double _rankTolerance=1.0, const bool _enableEPI=false, double _landmarkDistanceThreshold=-1, double _dynamicOutlierRejectionThreshold=-1, const SharedNoiseModel &_noiseModel=nullptr)
Constructor.
Definition triangulation.h:586
│ │ │ │ -
gtsam::TriangulationParameters::dynamicOutlierRejectionThreshold
double dynamicOutlierRejectionThreshold
If this is nonnegative the we will check if the average reprojection error is smaller than this thres...
Definition triangulation.h:573
│ │ │ │ -
gtsam::TriangulationParameters::rankTolerance
double rankTolerance
threshold to decide whether triangulation is result.degenerate
Definition triangulation.h:558
│ │ │ │ -
gtsam::TriangulationParameters::landmarkDistanceThreshold
double landmarkDistanceThreshold
if the landmark is triangulated at distance larger than this, result is flagged as degenerate.
Definition triangulation.h:566
│ │ │ │ -
gtsam::TriangulationParameters::enableEPI
bool enableEPI
if set to true, will refine triangulation using LM
Definition triangulation.h:560
│ │ │ │ -
gtsam::TriangulationParameters::noiseModel
SharedNoiseModel noiseModel
used in the nonlinear triangulation
Definition triangulation.h:575
│ │ │ │ -
gtsam::TriangulationResult
TriangulationResult is an optional point, along with the reasons why it is invalid.
Definition triangulation.h:626
│ │ │ │ -
gtsam::TriangulationResult::TriangulationResult
TriangulationResult()
Default constructor, only for serialization.
Definition triangulation.h:638
│ │ │ │ -
gtsam::TriangulationResult::TriangulationResult
TriangulationResult(const Point3 &p)
Constructor.
Definition triangulation.h:643
│ │ │ │ -
gtsam::TriangulationResult::access
friend class boost::serialization::access
Serialization function.
Definition triangulation.h:671
│ │ │ │ -
gtsam::Symbol
Character and index key used to refer to variables.
Definition Symbol.h:35
│ │ │ │ -
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
An isotropic noise model created by specifying a standard devation sigma.
Definition NoiseModel.cpp:597
│ │ │ │ -
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Create a unit covariance noise model.
Definition NoiseModel.h:597
│ │ │ │ +
gtsam::equal
bool equal(const T &obj1, const T &obj2, double tol)
Call equal on the object.
Definition Testable.h:84
│ │ │ │ +
gtsam::FactorGraph::resize
virtual void resize(size_t size)
Directly resize the number of factors in the graph.
Definition FactorGraph.h:381
│ │ │ │ +
gtsam::FactorGraph::size
size_t size() const
return the number of factors (including any null factors set by remove() ).
Definition FactorGraph.h:326
│ │ │ │ +
gtsam::noiseModel::Gaussian::Information
static shared_ptr Information(const Matrix &M, bool smart=true)
A Gaussian noise model created by specifying an information matrix.
Definition NoiseModel.cpp:100
│ │ │ │ +
gtsam::noiseModel::Robust
Base class for robust error models The robust M-estimators above simply tell us how to re-weight the ...
Definition NoiseModel.h:642
│ │ │ │ +
gtsam::GncOptimizer
Definition GncOptimizer.h:45
│ │ │ │ +
gtsam::GncOptimizer::checkWeightsConvergence
bool checkWeightsConvergence(const Vector &weights) const
Check convergence of weights to binary values.
Definition GncOptimizer.h:364
│ │ │ │ +
gtsam::GncOptimizer::setWeights
void setWeights(const Vector w)
Set weights for each factor.
Definition GncOptimizer.h:144
│ │ │ │ +
gtsam::GncOptimizer::checkMuConvergence
bool checkMuConvergence(const double mu) const
Check if we have reached the value of mu for which the surrogate loss matches the original loss.
Definition GncOptimizer.h:334
│ │ │ │ +
gtsam::GncOptimizer::BaseOptimizer
GncParameters::OptimizerType BaseOptimizer
For each parameter, specify the corresponding optimizer: e.g., GaussNewtonParams -> GaussNewtonOptimi...
Definition GncOptimizer.h:48
│ │ │ │ +
gtsam::GncOptimizer::optimize
Values optimize()
Compute optimal solution using graduated non-convexity.
Definition GncOptimizer.h:185
│ │ │ │ +
gtsam::GncOptimizer::getInlierCostThresholds
const Vector & getInlierCostThresholds() const
Get the inlier threshold.
Definition GncOptimizer.h:166
│ │ │ │ +
gtsam::GncOptimizer::checkCostConvergence
bool checkCostConvergence(const double cost, const double prev_cost) const
Check convergence of relative cost differences.
Definition GncOptimizer.h:353
│ │ │ │ +
gtsam::GncOptimizer::getState
const Values & getState() const
Access a copy of the internal values.
Definition GncOptimizer.h:157
│ │ │ │ +
gtsam::GncOptimizer::setInlierCostThresholds
void setInlierCostThresholds(const Vector &inthVec)
Set the maximum weighted residual error for an inlier (one for each factor).
Definition GncOptimizer.h:125
│ │ │ │ +
gtsam::GncOptimizer::calculateWeights
Vector calculateWeights(const Values &currentEstimate, const double mu)
Calculate gnc weights.
Definition GncOptimizer.h:423
│ │ │ │ +
gtsam::GncOptimizer::initializeMu
double initializeMu() const
Initialize the gnc parameter mu such that loss is approximately convex (remark 5 in GNC paper).
Definition GncOptimizer.h:274
│ │ │ │ +
gtsam::GncOptimizer::updateMu
double updateMu(const double mu) const
Update the gnc parameter mu to gradually increase nonconvexity.
Definition GncOptimizer.h:319
│ │ │ │ +
gtsam::GncOptimizer::equals
bool equals(const GncOptimizer &other, double tol=1e-9) const
Equals.
Definition GncOptimizer.h:169
│ │ │ │ +
gtsam::GncOptimizer::checkConvergence
bool checkConvergence(const double mu, const Vector &weights, const double cost, const double prev_cost) const
Check for convergence between consecutive GNC iterations.
Definition GncOptimizer.h:391
│ │ │ │ +
gtsam::GncOptimizer::setInlierCostThresholds
void setInlierCostThresholds(const double inth)
Set the maximum weighted residual error for an inlier (same for all factors).
Definition GncOptimizer.h:117
│ │ │ │ +
gtsam::GncOptimizer::setInlierCostThresholdsAtProbability
void setInlierCostThresholdsAtProbability(const double alpha)
Set the maximum weighted residual error threshold by specifying the probability alpha that the inlier...
Definition GncOptimizer.h:132
│ │ │ │ +
gtsam::GncOptimizer::makeWeightedGraph
NonlinearFactorGraph makeWeightedGraph(const Vector &weights) const
Create a graph where each factor is weighted by the gnc weights.
Definition GncOptimizer.h:398
│ │ │ │ +
gtsam::GncOptimizer::getFactors
const NonlinearFactorGraph & getFactors() const
Access a copy of the internal factor graph.
Definition GncOptimizer.h:154
│ │ │ │ +
gtsam::GncOptimizer::getWeights
const Vector & getWeights() const
Access a copy of the GNC weights.
Definition GncOptimizer.h:163
│ │ │ │ +
gtsam::GncOptimizer::getParams
const GncParameters & getParams() const
Access a copy of the parameters.
Definition GncOptimizer.h:160
│ │ │ │ +
gtsam::GncOptimizer::GncOptimizer
GncOptimizer(const NonlinearFactorGraph &graph, const Values &initialValues, const GncParameters &params=GncParameters())
Constructor.
Definition GncOptimizer.h:59
│ │ │ │ +
gtsam::NoiseModelFactor
A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
Definition NonlinearFactor.h:174
│ │ │ │ +
gtsam::NoiseModelFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
Noise model.
Definition NonlinearFactor.h:186
│ │ │ │
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │ +
gtsam::NonlinearFactorGraph::equals
bool equals(const NonlinearFactorGraph &other, double tol=1e-9) const
Test equality.
Definition NonlinearFactorGraph.cpp:97
│ │ │ │ +
gtsam::NonlinearFactorGraph::error
double error(const Values &values) const
unnormalized error, in the most common case
Definition NonlinearFactorGraph.cpp:170
│ │ │ │
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
gtsam::Values::insert
void insert(Key j, const Value &val)
Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present.
Definition Values.cpp:157
│ │ │ │ -
gtsam::TriangulationFactor
Non-linear factor for a constraint derived from a 2D measurement.
Definition TriangulationFactor.h:33
│ │ │ │ -
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │ - │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,815 +1,616 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -triangulation.h │ │ │ │ │ +GncOptimizer.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -21#pragma once │ │ │ │ │ -22 │ │ │ │ │ -23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3_B_u_n_d_l_e_r_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3_F_i_s_h_e_y_e_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3_U_n_i_f_i_e_d_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3___S_2_._h> │ │ │ │ │ -27#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3_D_S_2_._h> │ │ │ │ │ -28#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_m_e_r_a_S_e_t_._h> │ │ │ │ │ -29#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_i_n_h_o_l_e_C_a_m_e_r_a_._h> │ │ │ │ │ -30#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_._h> │ │ │ │ │ -31#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_2_._h> │ │ │ │ │ -32#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_S_y_m_b_o_l_._h> │ │ │ │ │ -33#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -34#include │ │ │ │ │ -35 │ │ │ │ │ -36namespace _g_t_s_a_m { │ │ │ │ │ -37 │ │ │ │ │ -_3_9class GTSAM_EXPORT _T_r_i_a_n_g_u_l_a_t_i_o_n_U_n_d_e_r_c_o_n_s_t_r_a_i_n_e_d_E_x_c_e_p_t_i_o_n: public std:: │ │ │ │ │ -runtime_error { │ │ │ │ │ -40public: │ │ │ │ │ -41 _T_r_i_a_n_g_u_l_a_t_i_o_n_U_n_d_e_r_c_o_n_s_t_r_a_i_n_e_d_E_x_c_e_p_t_i_o_n() : │ │ │ │ │ -42 std::runtime_error("Triangulation Underconstrained Exception.") { │ │ │ │ │ -43 } │ │ │ │ │ -44}; │ │ │ │ │ -45 │ │ │ │ │ -_4_7class GTSAM_EXPORT _T_r_i_a_n_g_u_l_a_t_i_o_n_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n: public std:: │ │ │ │ │ -runtime_error { │ │ │ │ │ -48public: │ │ │ │ │ -49 _T_r_i_a_n_g_u_l_a_t_i_o_n_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n() : │ │ │ │ │ -50 std::runtime_error( │ │ │ │ │ -51 "Triangulation Cheirality Exception: The resulting landmark is behind one or │ │ │ │ │ -more cameras.") { │ │ │ │ │ -52 } │ │ │ │ │ -53}; │ │ │ │ │ -54 │ │ │ │ │ -62GTSAM_EXPORT Vector4 _t_r_i_a_n_g_u_l_a_t_e_H_o_m_o_g_e_n_e_o_u_s_D_L_T( │ │ │ │ │ -63 const std::vector>& │ │ │ │ │ -projection_matrices, │ │ │ │ │ -64 const Point2Vector& measurements, double rank_tol = 1e-9); │ │ │ │ │ -65 │ │ │ │ │ -74GTSAM_EXPORT Vector4 _t_r_i_a_n_g_u_l_a_t_e_H_o_m_o_g_e_n_e_o_u_s_D_L_T( │ │ │ │ │ -75 const std::vector>& │ │ │ │ │ -projection_matrices, │ │ │ │ │ -76 const std::vector& measurements, double rank_tol = 1e-9); │ │ │ │ │ -77 │ │ │ │ │ -85GTSAM_EXPORT _P_o_i_n_t_3 _t_r_i_a_n_g_u_l_a_t_e_D_L_T( │ │ │ │ │ -86 const std::vector>& │ │ │ │ │ -projection_matrices, │ │ │ │ │ -87 const Point2Vector& measurements, │ │ │ │ │ -88 double rank_tol = 1e-9); │ │ │ │ │ -89 │ │ │ │ │ -93GTSAM_EXPORT _P_o_i_n_t_3 _t_r_i_a_n_g_u_l_a_t_e_D_L_T( │ │ │ │ │ -94 const std::vector>& │ │ │ │ │ -projection_matrices, │ │ │ │ │ -95 const std::vector& measurements, │ │ │ │ │ -96 double rank_tol = 1e-9); │ │ │ │ │ -97 │ │ │ │ │ -108GTSAM_EXPORT _P_o_i_n_t_3 _t_r_i_a_n_g_u_l_a_t_e_L_O_S_T(const std::vector& poses, │ │ │ │ │ -109 const Point3Vector& calibratedMeasurements, │ │ │ │ │ -110 const SharedIsotropic& measurementNoise); │ │ │ │ │ -111 │ │ │ │ │ -121template │ │ │ │ │ -_1_2_2std::pair _t_r_i_a_n_g_u_l_a_t_i_o_n_G_r_a_p_h( │ │ │ │ │ -123 const std::vector& poses, boost::shared_ptr sharedCal, │ │ │ │ │ -124 const Point2Vector& measurements, _K_e_y landmarkKey, │ │ │ │ │ -125 const _P_o_i_n_t_3& initialEstimate, │ │ │ │ │ -126 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model = _n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_C_r_e_a_t_e(2)) { │ │ │ │ │ -127 _V_a_l_u_e_s values; │ │ │ │ │ -128 values._i_n_s_e_r_t(landmarkKey, initialEstimate); // Initial landmark value │ │ │ │ │ -129 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h graph; │ │ │ │ │ -130 for (size_t i = 0; i < measurements.size(); i++) { │ │ │ │ │ -131 const _P_o_s_e_3& pose_i = poses[i]; │ │ │ │ │ -132 typedef _P_i_n_h_o_l_e_P_o_s_e_<_C_A_L_I_B_R_A_T_I_O_N_> Camera; │ │ │ │ │ -133 Camera camera_i(pose_i, sharedCal); │ │ │ │ │ -134 graph.emplace_shared<_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_<_C_a_m_e_r_a_> > // │ │ │ │ │ -135 (camera_i, measurements[i], model, landmarkKey); │ │ │ │ │ -136 } │ │ │ │ │ -137 return std::make_pair(graph, values); │ │ │ │ │ -138} │ │ │ │ │ -139 │ │ │ │ │ -149template │ │ │ │ │ -_1_5_0std::pair _t_r_i_a_n_g_u_l_a_t_i_o_n_G_r_a_p_h( │ │ │ │ │ -151 const _C_a_m_e_r_a_S_e_t_<_C_A_M_E_R_A_>& cameras, │ │ │ │ │ -152 const typename CAMERA::MeasurementVector& measurements, _K_e_y landmarkKey, │ │ │ │ │ -153 const _P_o_i_n_t_3& initialEstimate, │ │ │ │ │ -154 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model = nullptr) { │ │ │ │ │ -155 _V_a_l_u_e_s values; │ │ │ │ │ -156 values._i_n_s_e_r_t(landmarkKey, initialEstimate); // Initial landmark value │ │ │ │ │ -157 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h graph; │ │ │ │ │ -158 static _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l unit(_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_C_r_e_a_t_e( │ │ │ │ │ -159 _t_r_a_i_t_s_<_t_y_p_e_n_a_m_e_ _C_A_M_E_R_A_:_:_M_e_a_s_u_r_e_m_e_n_t_>_:_:_d_i_m_e_n_s_i_o_n)); │ │ │ │ │ -160 for (size_t i = 0; i < measurements.size(); i++) { │ │ │ │ │ -161 const CAMERA& camera_i = cameras[i]; │ │ │ │ │ -162 graph.emplace_shared<_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r_<_C_A_M_E_R_A_> > // │ │ │ │ │ -163 (camera_i, measurements[i], model? model : unit, landmarkKey); │ │ │ │ │ -164 } │ │ │ │ │ -165 return std::make_pair(graph, values); │ │ │ │ │ -166} │ │ │ │ │ +27#pragma once │ │ │ │ │ +28 │ │ │ │ │ +29#include │ │ │ │ │ +30#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +31#include │ │ │ │ │ +32 │ │ │ │ │ +33namespace _g_t_s_a_m { │ │ │ │ │ +34/* │ │ │ │ │ +35 * Quantile of chi-squared distribution with given degrees of freedom at │ │ │ │ │ +probability alpha. │ │ │ │ │ +36 * Equivalent to chi2inv in Matlab. │ │ │ │ │ +37 */ │ │ │ │ │ +38static double Chi2inv(const double alpha, const size_t dofs) { │ │ │ │ │ +39 boost::math::chi_squared_distribution chi2(dofs); │ │ │ │ │ +40 return boost::math::quantile(chi2, alpha); │ │ │ │ │ +41} │ │ │ │ │ +42 │ │ │ │ │ +43/* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +44template │ │ │ │ │ +_4_5class _G_n_c_O_p_t_i_m_i_z_e_r { │ │ │ │ │ +46 public: │ │ │ │ │ +_4_8 typedef typename GncParameters::OptimizerType _B_a_s_e_O_p_t_i_m_i_z_e_r; │ │ │ │ │ +49 │ │ │ │ │ +50 private: │ │ │ │ │ +51 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h nfg_; │ │ │ │ │ +52 _V_a_l_u_e_s state_; │ │ │ │ │ +53 GncParameters params_; │ │ │ │ │ +54 Vector weights_; │ │ │ │ │ +55 Vector barcSq_; │ │ │ │ │ +56 │ │ │ │ │ +57 public: │ │ │ │ │ +_5_9 _G_n_c_O_p_t_i_m_i_z_e_r(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, const _V_a_l_u_e_s& initialValues, │ │ │ │ │ +60 const GncParameters& params = GncParameters()) │ │ │ │ │ +61 : state_(initialValues), │ │ │ │ │ +62 params_(params) { │ │ │ │ │ +63 │ │ │ │ │ +64 // make sure all noiseModels are Gaussian or convert to Gaussian │ │ │ │ │ +65 nfg_._r_e_s_i_z_e(graph.size()); │ │ │ │ │ +66 for (size_t i = 0; i < graph.size(); i++) { │ │ │ │ │ +67 if (graph[i]) { │ │ │ │ │ +68 _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r factor = boost::dynamic_pointer_cast< │ │ │ │ │ +69 _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r>(graph[i]); │ │ │ │ │ +70 auto robust = boost::dynamic_pointer_cast< │ │ │ │ │ +71 _n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t>(factor->noiseModel()); │ │ │ │ │ +72 // if the factor has a robust loss, we remove the robust loss │ │ │ │ │ +73 nfg_[i] = robust ? factor-> cloneWithNewNoiseModel(robust->noise()) : │ │ │ │ │ +factor; │ │ │ │ │ +74 } │ │ │ │ │ +75 } │ │ │ │ │ +76 │ │ │ │ │ +77 // check that known inliers and outliers make sense: │ │ │ │ │ +78 std::vector inconsistentlySpecifiedWeights; // measurements the user │ │ │ │ │ +has incorrectly specified │ │ │ │ │ +79 // to be BOTH known inliers and known outliers │ │ │ │ │ +80 std::set_intersection(params.knownInliers.begin(),params.knownInliers.end(), │ │ │ │ │ +81 params.knownOutliers.begin(),params.knownOutliers.end(), │ │ │ │ │ +82 std::inserter(inconsistentlySpecifiedWeights, │ │ │ │ │ +inconsistentlySpecifiedWeights.begin())); │ │ │ │ │ +83 if(inconsistentlySpecifiedWeights.size() > 0){ // if we have inconsistently │ │ │ │ │ +specified weights, we throw an exception │ │ │ │ │ +84 params.print("params\n"); │ │ │ │ │ +85 throw std::runtime_error("GncOptimizer::constructor: the user has selected │ │ │ │ │ +one or more measurements" │ │ │ │ │ +86 " to be BOTH a known inlier and a known outlier."); │ │ │ │ │ +87 } │ │ │ │ │ +88 // check that known inliers are in the graph │ │ │ │ │ +89 for (size_t i = 0; i < params.knownInliers.size(); i++){ │ │ │ │ │ +90 if( params.knownInliers[i] > nfg_._s_i_z_e()-1 ){ // outside graph │ │ │ │ │ +91 throw std::runtime_error("GncOptimizer::constructor: the user has selected │ │ │ │ │ +one or more measurements" │ │ │ │ │ +92 "that are not in the factor graph to be known inliers."); │ │ │ │ │ +93 } │ │ │ │ │ +94 } │ │ │ │ │ +95 // check that known outliers are in the graph │ │ │ │ │ +96 for (size_t i = 0; i < params.knownOutliers.size(); i++){ │ │ │ │ │ +97 if( params.knownOutliers[i] > nfg_._s_i_z_e()-1 ){ // outside graph │ │ │ │ │ +98 throw std::runtime_error("GncOptimizer::constructor: the user has selected │ │ │ │ │ +one or more measurements" │ │ │ │ │ +99 "that are not in the factor graph to be known outliers."); │ │ │ │ │ +100 } │ │ │ │ │ +101 } │ │ │ │ │ +102 // initialize weights (if we don't have prior knowledge of inliers/outliers │ │ │ │ │ +103 // the weights are all initialized to 1. │ │ │ │ │ +104 weights_ = initializeWeightsFromKnownInliersAndOutliers(); │ │ │ │ │ +105 │ │ │ │ │ +106 // set default barcSq_ (inlier threshold) │ │ │ │ │ +107 double alpha = 0.99; // with this (default) probability, inlier residuals │ │ │ │ │ +are smaller than barcSq_ │ │ │ │ │ +108 _s_e_t_I_n_l_i_e_r_C_o_s_t_T_h_r_e_s_h_o_l_d_s_A_t_P_r_o_b_a_b_i_l_i_t_y(alpha); │ │ │ │ │ +109 } │ │ │ │ │ +110 │ │ │ │ │ +_1_1_7 void _s_e_t_I_n_l_i_e_r_C_o_s_t_T_h_r_e_s_h_o_l_d_s(const double inth) { │ │ │ │ │ +118 barcSq_ = inth * Vector::Ones(nfg_._s_i_z_e()); │ │ │ │ │ +119 } │ │ │ │ │ +120 │ │ │ │ │ +_1_2_5 void _s_e_t_I_n_l_i_e_r_C_o_s_t_T_h_r_e_s_h_o_l_d_s(const Vector& inthVec) { │ │ │ │ │ +126 barcSq_ = inthVec; │ │ │ │ │ +127 } │ │ │ │ │ +128 │ │ │ │ │ +_1_3_2 void _s_e_t_I_n_l_i_e_r_C_o_s_t_T_h_r_e_s_h_o_l_d_s_A_t_P_r_o_b_a_b_i_l_i_t_y(const double alpha) { │ │ │ │ │ +133 barcSq_ = Vector::Ones(nfg_._s_i_z_e()); // initialize │ │ │ │ │ +134 for (size_t k = 0; k < nfg_._s_i_z_e(); k++) { │ │ │ │ │ +135 if (nfg_[k]) { │ │ │ │ │ +136 barcSq_[k] = 0.5 * Chi2inv(alpha, nfg_[k]->dim()); // 0.5 derives from the │ │ │ │ │ +error definition in gtsam │ │ │ │ │ +137 } │ │ │ │ │ +138 } │ │ │ │ │ +139 } │ │ │ │ │ +140 │ │ │ │ │ +_1_4_4 void _s_e_t_W_e_i_g_h_t_s(const Vector w) { │ │ │ │ │ +145 if (size_t(w.size()) != nfg_._s_i_z_e()) { │ │ │ │ │ +146 throw std::runtime_error( │ │ │ │ │ +147 "GncOptimizer::setWeights: the number of specified weights" │ │ │ │ │ +148 " does not match the size of the factor graph."); │ │ │ │ │ +149 } │ │ │ │ │ +150 weights_ = w; │ │ │ │ │ +151 } │ │ │ │ │ +152 │ │ │ │ │ +_1_5_4 const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& _g_e_t_F_a_c_t_o_r_s() const { return nfg_; } │ │ │ │ │ +155 │ │ │ │ │ +_1_5_7 const _V_a_l_u_e_s& _g_e_t_S_t_a_t_e() const { return state_; } │ │ │ │ │ +158 │ │ │ │ │ +_1_6_0 const GncParameters& _g_e_t_P_a_r_a_m_s() const { return params_;} │ │ │ │ │ +161 │ │ │ │ │ +_1_6_3 const Vector& _g_e_t_W_e_i_g_h_t_s() const { return weights_;} │ │ │ │ │ +164 │ │ │ │ │ +_1_6_6 const Vector& _g_e_t_I_n_l_i_e_r_C_o_s_t_T_h_r_e_s_h_o_l_d_s() const {return barcSq_;} │ │ │ │ │ 167 │ │ │ │ │ -175GTSAM_EXPORT _P_o_i_n_t_3 _o_p_t_i_m_i_z_e(const NonlinearFactorGraph& graph, │ │ │ │ │ -176 const _V_a_l_u_e_s& values, _K_e_y landmarkKey); │ │ │ │ │ -177 │ │ │ │ │ -186template │ │ │ │ │ -_1_8_7_P_o_i_n_t_3 _t_r_i_a_n_g_u_l_a_t_e_N_o_n_l_i_n_e_a_r(const std::vector& poses, │ │ │ │ │ -188 boost::shared_ptr sharedCal, │ │ │ │ │ -189 const Point2Vector& measurements, const _P_o_i_n_t_3& initialEstimate, │ │ │ │ │ -190 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model = nullptr) { │ │ │ │ │ -191 │ │ │ │ │ -192 // Create a factor graph and initial values │ │ │ │ │ -193 _V_a_l_u_e_s values; │ │ │ │ │ -194 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h graph; │ │ │ │ │ -195 boost::tie(graph, values) = triangulationGraph // │ │ │ │ │ -196 (poses, sharedCal, measurements, _S_y_m_b_o_l('p', 0), initialEstimate, model); │ │ │ │ │ -197 │ │ │ │ │ -198 return _o_p_t_i_m_i_z_e(graph, values, _S_y_m_b_o_l('p', 0)); │ │ │ │ │ -199} │ │ │ │ │ -200 │ │ │ │ │ -208template │ │ │ │ │ -_2_0_9_P_o_i_n_t_3 _t_r_i_a_n_g_u_l_a_t_e_N_o_n_l_i_n_e_a_r( │ │ │ │ │ -210 const _C_a_m_e_r_a_S_e_t_<_C_A_M_E_R_A_>& cameras, │ │ │ │ │ -211 const typename CAMERA::MeasurementVector& measurements, const _P_o_i_n_t_3& │ │ │ │ │ -initialEstimate, │ │ │ │ │ -212 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model = nullptr) { │ │ │ │ │ -213 │ │ │ │ │ -214 // Create a factor graph and initial values │ │ │ │ │ -215 _V_a_l_u_e_s values; │ │ │ │ │ -216 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h graph; │ │ │ │ │ -217 boost::tie(graph, values) = triangulationGraph // │ │ │ │ │ -218 (cameras, measurements, _S_y_m_b_o_l('p', 0), initialEstimate, model); │ │ │ │ │ -219 │ │ │ │ │ -220 return _o_p_t_i_m_i_z_e(graph, values, _S_y_m_b_o_l('p', 0)); │ │ │ │ │ -221} │ │ │ │ │ -222 │ │ │ │ │ -223template │ │ │ │ │ -224std::vector> │ │ │ │ │ -225projectionMatricesFromCameras(const CameraSet &cameras) { │ │ │ │ │ -226 std::vector> │ │ │ │ │ -projection_matrices; │ │ │ │ │ -227 for (const CAMERA &camera: cameras) { │ │ │ │ │ -228 projection_matrices.push_back(camera.cameraProjectionMatrix()); │ │ │ │ │ +_1_6_9 bool _e_q_u_a_l_s(const _G_n_c_O_p_t_i_m_i_z_e_r& other, double tol = 1e-9) const { │ │ │ │ │ +170 return nfg_._e_q_u_a_l_s(other._g_e_t_F_a_c_t_o_r_s()) │ │ │ │ │ +171 && _e_q_u_a_l(weights_, other._g_e_t_W_e_i_g_h_t_s()) │ │ │ │ │ +172 && params_.equals(other._g_e_t_P_a_r_a_m_s()) │ │ │ │ │ +173 && _e_q_u_a_l(barcSq_, other._g_e_t_I_n_l_i_e_r_C_o_s_t_T_h_r_e_s_h_o_l_d_s()); │ │ │ │ │ +174 } │ │ │ │ │ +175 │ │ │ │ │ +176 Vector initializeWeightsFromKnownInliersAndOutliers() const{ │ │ │ │ │ +177 Vector weights = Vector::Ones(nfg_._s_i_z_e()); │ │ │ │ │ +178 for (size_t i = 0; i < params_.knownOutliers.size(); i++){ │ │ │ │ │ +179 weights[ params_.knownOutliers[i] ] = 0.0; // known to be outliers │ │ │ │ │ +180 } │ │ │ │ │ +181 return weights; │ │ │ │ │ +182 } │ │ │ │ │ +183 │ │ │ │ │ +_1_8_5 _V_a_l_u_e_s _o_p_t_i_m_i_z_e() { │ │ │ │ │ +186 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h graph_initial = this->_m_a_k_e_W_e_i_g_h_t_e_d_G_r_a_p_h(weights_); │ │ │ │ │ +187 _B_a_s_e_O_p_t_i_m_i_z_e_r baseOptimizer( │ │ │ │ │ +188 graph_initial, state_, params_.baseOptimizerParams); │ │ │ │ │ +189 _V_a_l_u_e_s result = baseOptimizer.optimize(); │ │ │ │ │ +190 double mu = _i_n_i_t_i_a_l_i_z_e_M_u(); │ │ │ │ │ +191 double prev_cost = graph_initial._e_r_r_o_r(result); │ │ │ │ │ +192 double cost = 0.0; // this will be updated in the main loop │ │ │ │ │ +193 │ │ │ │ │ +194 // handle the degenerate case that corresponds to small │ │ │ │ │ +195 // maximum residual errors at initialization │ │ │ │ │ +196 // For GM: if residual error is small, mu -> 0 │ │ │ │ │ +197 // For TLS: if residual error is small, mu -> -1 │ │ │ │ │ +198 int nrUnknownInOrOut = nfg_._s_i_z_e() - ( params_.knownInliers.size() + │ │ │ │ │ +params_.knownOutliers.size() ); │ │ │ │ │ +199 // ^^ number of measurements that are not known to be inliers or outliers │ │ │ │ │ +(GNC will need to figure them out) │ │ │ │ │ +200 if (mu <= 0 || nrUnknownInOrOut == 0) { // no need to even call GNC in this │ │ │ │ │ +case │ │ │ │ │ +201 if (mu <= 0 && params_.verbosity >= GncParameters::Verbosity::SUMMARY) { │ │ │ │ │ +202 std::cout << "GNC Optimizer stopped because maximum residual at " │ │ │ │ │ +203 "initialization is small." │ │ │ │ │ +204 << std::endl; │ │ │ │ │ +205 } │ │ │ │ │ +206 if (nrUnknownInOrOut==0 && params_.verbosity >= GncParameters::Verbosity:: │ │ │ │ │ +SUMMARY) { │ │ │ │ │ +207 std::cout << "GNC Optimizer stopped because all measurements are already │ │ │ │ │ +known to be inliers or outliers" │ │ │ │ │ +208 << std::endl; │ │ │ │ │ +209 } │ │ │ │ │ +210 if (params_.verbosity >= GncParameters::Verbosity::MU) { │ │ │ │ │ +211 std::cout << "mu: " << mu << std::endl; │ │ │ │ │ +212 } │ │ │ │ │ +213 if (params_.verbosity >= GncParameters::Verbosity::VALUES) { │ │ │ │ │ +214 result.print("result\n"); │ │ │ │ │ +215 } │ │ │ │ │ +216 return result; │ │ │ │ │ +217 } │ │ │ │ │ +218 │ │ │ │ │ +219 size_t iter; │ │ │ │ │ +220 for (iter = 0; iter < params_.maxIterations; iter++) { │ │ │ │ │ +221 │ │ │ │ │ +222 // display info │ │ │ │ │ +223 if (params_.verbosity >= GncParameters::Verbosity::MU) { │ │ │ │ │ +224 std::cout << "iter: " << iter << std::endl; │ │ │ │ │ +225 std::cout << "mu: " << mu << std::endl; │ │ │ │ │ +226 } │ │ │ │ │ +227 if (params_.verbosity >= GncParameters::Verbosity::WEIGHTS) { │ │ │ │ │ +228 std::cout << "weights: " << weights_ << std::endl; │ │ │ │ │ 229 } │ │ │ │ │ -230 return projection_matrices; │ │ │ │ │ -231} │ │ │ │ │ -232 │ │ │ │ │ -233// overload, assuming pinholePose │ │ │ │ │ -234template │ │ │ │ │ -235std::vector> │ │ │ │ │ -projectionMatricesFromPoses( │ │ │ │ │ -236 const std::vector &poses, boost::shared_ptr sharedCal) │ │ │ │ │ -{ │ │ │ │ │ -237 std::vector> │ │ │ │ │ -projection_matrices; │ │ │ │ │ -238 for (size_t i = 0; i < poses.size(); i++) { │ │ │ │ │ -239 PinholePose camera(poses.at(i), sharedCal); │ │ │ │ │ -240 projection_matrices.push_back(camera.cameraProjectionMatrix()); │ │ │ │ │ -241 } │ │ │ │ │ -242 return projection_matrices; │ │ │ │ │ -243} │ │ │ │ │ -244 │ │ │ │ │ -252template │ │ │ │ │ -_2_5_3_C_a_l_3___S_2 _c_r_e_a_t_e_P_i_n_h_o_l_e_C_a_l_i_b_r_a_t_i_o_n(const CALIBRATION& cal) { │ │ │ │ │ -254 const auto& K = cal.K(); │ │ │ │ │ -255 return _C_a_l_3___S_2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2)); │ │ │ │ │ -256} │ │ │ │ │ -257 │ │ │ │ │ -260template │ │ │ │ │ -_2_6_1MEASUREMENT _u_n_d_i_s_t_o_r_t_M_e_a_s_u_r_e_m_e_n_t_I_n_t_e_r_n_a_l( │ │ │ │ │ -262 const CALIBRATION& cal, const MEASUREMENT& measurement, │ │ │ │ │ -263 boost::optional pinholeCal = boost::none) { │ │ │ │ │ -264 if (!pinholeCal) { │ │ │ │ │ -265 pinholeCal = _c_r_e_a_t_e_P_i_n_h_o_l_e_C_a_l_i_b_r_a_t_i_o_n(cal); │ │ │ │ │ +230 if (params_.verbosity >= GncParameters::Verbosity::VALUES) { │ │ │ │ │ +231 result.print("result\n"); │ │ │ │ │ +232 } │ │ │ │ │ +233 // weights update │ │ │ │ │ +234 weights_ = _c_a_l_c_u_l_a_t_e_W_e_i_g_h_t_s(result, mu); │ │ │ │ │ +235 │ │ │ │ │ +236 // variable/values update │ │ │ │ │ +237 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h graph_iter = this->_m_a_k_e_W_e_i_g_h_t_e_d_G_r_a_p_h(weights_); │ │ │ │ │ +238 _B_a_s_e_O_p_t_i_m_i_z_e_r baseOptimizer_iter( │ │ │ │ │ +239 graph_iter, state_, params_.baseOptimizerParams); │ │ │ │ │ +240 result = baseOptimizer_iter.optimize(); │ │ │ │ │ +241 │ │ │ │ │ +242 // stopping condition │ │ │ │ │ +243 cost = graph_iter._e_r_r_o_r(result); │ │ │ │ │ +244 if (_c_h_e_c_k_C_o_n_v_e_r_g_e_n_c_e(mu, weights_, cost, prev_cost)) { │ │ │ │ │ +245 break; │ │ │ │ │ +246 } │ │ │ │ │ +247 │ │ │ │ │ +248 // update mu │ │ │ │ │ +249 mu = _u_p_d_a_t_e_M_u(mu); │ │ │ │ │ +250 │ │ │ │ │ +251 // get ready for next iteration │ │ │ │ │ +252 prev_cost = cost; │ │ │ │ │ +253 │ │ │ │ │ +254 // display info │ │ │ │ │ +255 if (params_.verbosity >= GncParameters::Verbosity::VALUES) { │ │ │ │ │ +256 std::cout << "previous cost: " << prev_cost << std::endl; │ │ │ │ │ +257 std::cout << "current cost: " << cost << std::endl; │ │ │ │ │ +258 } │ │ │ │ │ +259 } │ │ │ │ │ +260 // display info │ │ │ │ │ +261 if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) { │ │ │ │ │ +262 std::cout << "final iterations: " << iter << std::endl; │ │ │ │ │ +263 std::cout << "final mu: " << mu << std::endl; │ │ │ │ │ +264 std::cout << "previous cost: " << prev_cost << std::endl; │ │ │ │ │ +265 std::cout << "current cost: " << cost << std::endl; │ │ │ │ │ 266 } │ │ │ │ │ -267 return pinholeCal->uncalibrate(cal.calibrate(measurement)); │ │ │ │ │ -268} │ │ │ │ │ -269 │ │ │ │ │ -281template │ │ │ │ │ -_2_8_2Point2Vector _u_n_d_i_s_t_o_r_t_M_e_a_s_u_r_e_m_e_n_t_s(const CALIBRATION& cal, │ │ │ │ │ -283 const Point2Vector& measurements) { │ │ │ │ │ -284 _C_a_l_3___S_2 pinholeCalibration = _c_r_e_a_t_e_P_i_n_h_o_l_e_C_a_l_i_b_r_a_t_i_o_n(cal); │ │ │ │ │ -285 Point2Vector undistortedMeasurements; │ │ │ │ │ -286 // Calibrate with cal and uncalibrate with pinhole version of cal so that │ │ │ │ │ -287 // measurements are undistorted. │ │ │ │ │ -288 std::transform(measurements.begin(), measurements.end(), │ │ │ │ │ -289 std::back_inserter(undistortedMeasurements), │ │ │ │ │ -290 [&cal, &pinholeCalibration](const _P_o_i_n_t_2& measurement) { │ │ │ │ │ -291 return undistortMeasurementInternal( │ │ │ │ │ -292 cal, measurement, pinholeCalibration); │ │ │ │ │ -293 }); │ │ │ │ │ -294 return undistortedMeasurements; │ │ │ │ │ -295} │ │ │ │ │ -296 │ │ │ │ │ -298template <> │ │ │ │ │ -_2_9_9inline Point2Vector _u_n_d_i_s_t_o_r_t_M_e_a_s_u_r_e_m_e_n_t_s(const _C_a_l_3___S_2& cal, │ │ │ │ │ -300 const Point2Vector& measurements) { │ │ │ │ │ -301 return measurements; │ │ │ │ │ -302} │ │ │ │ │ -303 │ │ │ │ │ -315template │ │ │ │ │ -_3_1_6typename CAMERA::MeasurementVector _u_n_d_i_s_t_o_r_t_M_e_a_s_u_r_e_m_e_n_t_s( │ │ │ │ │ -317 const _C_a_m_e_r_a_S_e_t_<_C_A_M_E_R_A_>& cameras, │ │ │ │ │ -318 const typename CAMERA::MeasurementVector& measurements) { │ │ │ │ │ -319 const size_t nrMeasurements = measurements.size(); │ │ │ │ │ -320 assert(nrMeasurements == cameras.size()); │ │ │ │ │ -321 typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements); │ │ │ │ │ -322 for (size_t ii = 0; ii < nrMeasurements; ++ii) { │ │ │ │ │ -323 // Calibrate with cal and uncalibrate with pinhole version of cal so that │ │ │ │ │ -324 // measurements are undistorted. │ │ │ │ │ -325 undistortedMeasurements[ii] = │ │ │ │ │ -326 undistortMeasurementInternal( │ │ │ │ │ -327 cameras[ii].calibration(), measurements[ii]); │ │ │ │ │ -328 } │ │ │ │ │ -329 return undistortedMeasurements; │ │ │ │ │ -330} │ │ │ │ │ -331 │ │ │ │ │ -333template > │ │ │ │ │ -_3_3_4inline PinholeCamera::MeasurementVector _u_n_d_i_s_t_o_r_t_M_e_a_s_u_r_e_m_e_n_t_s( │ │ │ │ │ -335 const _C_a_m_e_r_a_S_e_t<_P_i_n_h_o_l_e_C_a_m_e_r_a_<_C_a_l_3___S_2_>>& cameras, │ │ │ │ │ -336 const PinholeCamera::MeasurementVector& measurements) { │ │ │ │ │ -337 return measurements; │ │ │ │ │ -338} │ │ │ │ │ -339 │ │ │ │ │ -341template │ │ │ │ │ -_3_4_2inline SphericalCamera::MeasurementVector _u_n_d_i_s_t_o_r_t_M_e_a_s_u_r_e_m_e_n_t_s( │ │ │ │ │ -343 const _C_a_m_e_r_a_S_e_t_<_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_>& cameras, │ │ │ │ │ -344 const SphericalCamera::MeasurementVector& measurements) { │ │ │ │ │ -345 return measurements; │ │ │ │ │ -346} │ │ │ │ │ -347 │ │ │ │ │ -356template │ │ │ │ │ -_3_5_7inline Point3Vector _c_a_l_i_b_r_a_t_e_M_e_a_s_u_r_e_m_e_n_t_s_S_h_a_r_e_d( │ │ │ │ │ -358 const CALIBRATION& cal, const Point2Vector& measurements) { │ │ │ │ │ -359 Point3Vector calibratedMeasurements; │ │ │ │ │ -360 // Calibrate with cal and uncalibrate with pinhole version of cal so that │ │ │ │ │ -361 // measurements are undistorted. │ │ │ │ │ -362 std::transform(measurements.begin(), measurements.end(), │ │ │ │ │ -363 std::back_inserter(calibratedMeasurements), │ │ │ │ │ -364 [&cal](const _P_o_i_n_t_2& measurement) { │ │ │ │ │ -365 Point3 p; │ │ │ │ │ -366 p << cal.calibrate(measurement), 1.0; │ │ │ │ │ -367 return p; │ │ │ │ │ -368 }); │ │ │ │ │ -369 return calibratedMeasurements; │ │ │ │ │ -370} │ │ │ │ │ -371 │ │ │ │ │ -380template │ │ │ │ │ -_3_8_1inline Point3Vector _c_a_l_i_b_r_a_t_e_M_e_a_s_u_r_e_m_e_n_t_s( │ │ │ │ │ -382 const _C_a_m_e_r_a_S_e_t_<_C_A_M_E_R_A_>& cameras, │ │ │ │ │ -383 const typename CAMERA::MeasurementVector& measurements) { │ │ │ │ │ -384 const size_t nrMeasurements = measurements.size(); │ │ │ │ │ -385 assert(nrMeasurements == cameras.size()); │ │ │ │ │ -386 Point3Vector calibratedMeasurements(nrMeasurements); │ │ │ │ │ -387 for (size_t ii = 0; ii < nrMeasurements; ++ii) { │ │ │ │ │ -388 calibratedMeasurements[ii] │ │ │ │ │ -389 << cameras[ii].calibration().calibrate(measurements[ii]), │ │ │ │ │ -390 1.0; │ │ │ │ │ -391 } │ │ │ │ │ -392 return calibratedMeasurements; │ │ │ │ │ -393} │ │ │ │ │ -394 │ │ │ │ │ -396template │ │ │ │ │ -_3_9_7inline Point3Vector _c_a_l_i_b_r_a_t_e_M_e_a_s_u_r_e_m_e_n_t_s( │ │ │ │ │ -398 const _C_a_m_e_r_a_S_e_t_<_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_>& cameras, │ │ │ │ │ -399 const SphericalCamera::MeasurementVector& measurements) { │ │ │ │ │ -400 Point3Vector calibratedMeasurements(measurements.size()); │ │ │ │ │ -401 for (size_t ii = 0; ii < measurements.size(); ++ii) { │ │ │ │ │ -402 calibratedMeasurements[ii] << measurements[ii].point3(); │ │ │ │ │ -403 } │ │ │ │ │ -404 return calibratedMeasurements; │ │ │ │ │ -405} │ │ │ │ │ -406 │ │ │ │ │ -420template │ │ │ │ │ -_4_2_1_P_o_i_n_t_3 _t_r_i_a_n_g_u_l_a_t_e_P_o_i_n_t_3(const std::vector& poses, │ │ │ │ │ -422 boost::shared_ptr sharedCal, │ │ │ │ │ -423 const Point2Vector& measurements, │ │ │ │ │ -424 double rank_tol = 1e-9, bool _o_p_t_i_m_i_z_e = false, │ │ │ │ │ -425 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model = nullptr, │ │ │ │ │ -426 const bool useLOST = false) { │ │ │ │ │ -427 assert(poses.size() == measurements.size()); │ │ │ │ │ -428 if (poses.size() < 2) throw(_T_r_i_a_n_g_u_l_a_t_i_o_n_U_n_d_e_r_c_o_n_s_t_r_a_i_n_e_d_E_x_c_e_p_t_i_o_n()); │ │ │ │ │ -429 │ │ │ │ │ -430 // Triangulate linearly │ │ │ │ │ -431 _P_o_i_n_t_3 point; │ │ │ │ │ -432 if (useLOST) { │ │ │ │ │ -433 // Reduce input noise model to an isotropic noise model using the mean of │ │ │ │ │ -434 // the diagonal. │ │ │ │ │ -435 const double measurementSigma = model ? model->sigmas().mean() : 1e-4; │ │ │ │ │ -436 SharedIsotropic measurementNoise = │ │ │ │ │ -437 _n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c_:_:_S_i_g_m_a(2, measurementSigma); │ │ │ │ │ -438 // calibrate the measurements to obtain homogenous coordinates in image │ │ │ │ │ -439 // plane. │ │ │ │ │ -440 auto calibratedMeasurements = │ │ │ │ │ -441 calibrateMeasurementsShared(*sharedCal, measurements); │ │ │ │ │ -442 │ │ │ │ │ -443 point = _t_r_i_a_n_g_u_l_a_t_e_L_O_S_T(poses, calibratedMeasurements, measurementNoise); │ │ │ │ │ -444 } else { │ │ │ │ │ -445 // construct projection matrices from poses & calibration │ │ │ │ │ -446 auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal); │ │ │ │ │ -447 │ │ │ │ │ -448 // Undistort the measurements, leaving only the pinhole elements in effect. │ │ │ │ │ -449 auto undistortedMeasurements = │ │ │ │ │ -450 undistortMeasurements(*sharedCal, measurements); │ │ │ │ │ -451 │ │ │ │ │ -452 point = │ │ │ │ │ -453 _t_r_i_a_n_g_u_l_a_t_e_D_L_T(projection_matrices, undistortedMeasurements, rank_tol); │ │ │ │ │ -454 } │ │ │ │ │ -455 │ │ │ │ │ -456 // Then refine using non-linear optimization │ │ │ │ │ -457 if (_o_p_t_i_m_i_z_e) │ │ │ │ │ -458 point = triangulateNonlinear // │ │ │ │ │ -459 (poses, sharedCal, measurements, point, model); │ │ │ │ │ -460 │ │ │ │ │ -461#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION │ │ │ │ │ -462 // verify that the triangulated point lies in front of all cameras │ │ │ │ │ -463 for (const _P_o_s_e_3& pose : poses) { │ │ │ │ │ -464 const _P_o_i_n_t_3& p_local = pose.transformTo(point); │ │ │ │ │ -465 if (p_local.z() <= 0) throw(_T_r_i_a_n_g_u_l_a_t_i_o_n_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n()); │ │ │ │ │ +267 if (params_.verbosity >= GncParameters::Verbosity::WEIGHTS) { │ │ │ │ │ +268 std::cout << "final weights: " << weights_ << std::endl; │ │ │ │ │ +269 } │ │ │ │ │ +270 return result; │ │ │ │ │ +271 } │ │ │ │ │ +272 │ │ │ │ │ +_2_7_4 double _i_n_i_t_i_a_l_i_z_e_M_u() const { │ │ │ │ │ +275 │ │ │ │ │ +276 double mu_init = 0.0; │ │ │ │ │ +277 // initialize mu to the value specified in Remark 5 in GNC paper. │ │ │ │ │ +278 switch (params_.lossType) { │ │ │ │ │ +279 case GncLossType::GM: │ │ │ │ │ +280 /* surrogate cost is convex for large mu. initialize as in remark 5 in GNC │ │ │ │ │ +paper. │ │ │ │ │ +281 Since barcSq_ can be different for each factor, we compute the max of the │ │ │ │ │ +quantity in remark 5 in GNC paper │ │ │ │ │ +282 */ │ │ │ │ │ +283 for (size_t k = 0; k < nfg_._s_i_z_e(); k++) { │ │ │ │ │ +284 if (nfg_[k]) { │ │ │ │ │ +285 mu_init = std::max(mu_init, 2 * nfg_[k]->error(state_) / barcSq_[k]); │ │ │ │ │ +286 } │ │ │ │ │ +287 } │ │ │ │ │ +288 return mu_init; // initial mu │ │ │ │ │ +289 case GncLossType::TLS: │ │ │ │ │ +290 /* surrogate cost is convex for mu close to zero. initialize as in remark 5 │ │ │ │ │ +in GNC paper. │ │ │ │ │ +291 degenerate case: 2 * rmax_sq - params_.barcSq < 0 (handled in the main │ │ │ │ │ +loop) │ │ │ │ │ +292 according to remark mu = params_.barcSq / (2 * rmax_sq - params_.barcSq) = │ │ │ │ │ +params_.barcSq/ excessResidual │ │ │ │ │ +293 however, if the denominator is 0 or negative, we return mu = -1 which leads │ │ │ │ │ +to termination of the main GNC loop. │ │ │ │ │ +294 Since barcSq_ can be different for each factor, we look for the minimimum │ │ │ │ │ +(positive) quantity in remark 5 in GNC paper │ │ │ │ │ +295 */ │ │ │ │ │ +296 mu_init = std::numeric_limits::infinity(); │ │ │ │ │ +297 for (size_t k = 0; k < nfg_._s_i_z_e(); k++) { │ │ │ │ │ +298 if (nfg_[k]) { │ │ │ │ │ +299 double rk = nfg_[k]->_e_r_r_o_r(state_); │ │ │ │ │ +300 mu_init = (2 * rk - barcSq_[k]) > 0 ? // if positive, update mu, otherwise │ │ │ │ │ +keep same │ │ │ │ │ +301 std::min(mu_init, barcSq_[k] / (2 * rk - barcSq_[k]) ) : mu_init; │ │ │ │ │ +302 } │ │ │ │ │ +303 } │ │ │ │ │ +304 if (mu_init >= 0 && mu_init < 1e-6){ │ │ │ │ │ +305 mu_init = 1e-6; // if mu ~ 0 (but positive), that means we have │ │ │ │ │ +measurements with large errors, │ │ │ │ │ +306 // i.e., rk > barcSq_[k] and rk very large, hence we threshold to 1e-6 to │ │ │ │ │ +avoid mu = 0 │ │ │ │ │ +307 } │ │ │ │ │ +308 │ │ │ │ │ +309 return mu_init > 0 && !std::isinf(mu_init) ? mu_init : -1; // if mu <= 0 or │ │ │ │ │ +mu = inf, return -1, │ │ │ │ │ +310 // which leads to termination of the main gnc loop. In this case, all │ │ │ │ │ +residuals are already below the threshold │ │ │ │ │ +311 // and there is no need to robustify (TLS = least squares) │ │ │ │ │ +312 default: │ │ │ │ │ +313 throw std::runtime_error( │ │ │ │ │ +314 "GncOptimizer::initializeMu: called with unknown loss type."); │ │ │ │ │ +315 } │ │ │ │ │ +316 } │ │ │ │ │ +317 │ │ │ │ │ +_3_1_9 double _u_p_d_a_t_e_M_u(const double mu) const { │ │ │ │ │ +320 switch (params_.lossType) { │ │ │ │ │ +321 case GncLossType::GM: │ │ │ │ │ +322 // reduce mu, but saturate at 1 (original cost is recovered for mu -> 1) │ │ │ │ │ +323 return std::max(1.0, mu / params_.muStep); │ │ │ │ │ +324 case GncLossType::TLS: │ │ │ │ │ +325 // increases mu at each iteration (original cost is recovered for mu - │ │ │ │ │ +> inf) │ │ │ │ │ +326 return mu * params_.muStep; │ │ │ │ │ +327 default: │ │ │ │ │ +328 throw std::runtime_error( │ │ │ │ │ +329 "GncOptimizer::updateMu: called with unknown loss type."); │ │ │ │ │ +330 } │ │ │ │ │ +331 } │ │ │ │ │ +332 │ │ │ │ │ +_3_3_4 bool _c_h_e_c_k_M_u_C_o_n_v_e_r_g_e_n_c_e(const double mu) const { │ │ │ │ │ +335 bool muConverged = false; │ │ │ │ │ +336 switch (params_.lossType) { │ │ │ │ │ +337 case GncLossType::GM: │ │ │ │ │ +338 muConverged = std::fabs(mu - 1.0) < 1e-9; // mu=1 recovers the original GM │ │ │ │ │ +function │ │ │ │ │ +339 break; │ │ │ │ │ +340 case GncLossType::TLS: │ │ │ │ │ +341 muConverged = false; // for TLS there is no stopping condition on mu (it │ │ │ │ │ +must tend to infinity) │ │ │ │ │ +342 break; │ │ │ │ │ +343 default: │ │ │ │ │ +344 throw std::runtime_error( │ │ │ │ │ +345 "GncOptimizer::checkMuConvergence: called with unknown loss type."); │ │ │ │ │ +346 } │ │ │ │ │ +347 if (muConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY) │ │ │ │ │ +348 std::cout << "muConverged = true " << std::endl; │ │ │ │ │ +349 return muConverged; │ │ │ │ │ +350 } │ │ │ │ │ +351 │ │ │ │ │ +_3_5_3 bool _c_h_e_c_k_C_o_s_t_C_o_n_v_e_r_g_e_n_c_e(const double cost, const double prev_cost) const │ │ │ │ │ +{ │ │ │ │ │ +354 bool costConverged = std::fabs(cost - prev_cost) / std::max(prev_cost, 1e- │ │ │ │ │ +7) │ │ │ │ │ +355 < params_.relativeCostTol; │ │ │ │ │ +356 if (costConverged && params_.verbosity >= GncParameters::Verbosity:: │ │ │ │ │ +SUMMARY){ │ │ │ │ │ +357 std::cout << "checkCostConvergence = true (prev. cost = " << prev_cost │ │ │ │ │ +358 << ", curr. cost = " << cost << ")" << std::endl; │ │ │ │ │ +359 } │ │ │ │ │ +360 return costConverged; │ │ │ │ │ +361 } │ │ │ │ │ +362 │ │ │ │ │ +_3_6_4 bool _c_h_e_c_k_W_e_i_g_h_t_s_C_o_n_v_e_r_g_e_n_c_e(const Vector& weights) const { │ │ │ │ │ +365 bool weightsConverged = false; │ │ │ │ │ +366 switch (params_.lossType) { │ │ │ │ │ +367 case GncLossType::GM: │ │ │ │ │ +368 weightsConverged = false; // for GM, there is no clear binary convergence │ │ │ │ │ +for the weights │ │ │ │ │ +369 break; │ │ │ │ │ +370 case GncLossType::TLS: │ │ │ │ │ +371 weightsConverged = true; │ │ │ │ │ +372 for (int i = 0; i < weights.size(); i++) { │ │ │ │ │ +373 if (std::fabs(weights[i] - std::round(weights[i])) │ │ │ │ │ +374 > params_.weightsTol) { │ │ │ │ │ +375 weightsConverged = false; │ │ │ │ │ +376 break; │ │ │ │ │ +377 } │ │ │ │ │ +378 } │ │ │ │ │ +379 break; │ │ │ │ │ +380 default: │ │ │ │ │ +381 throw std::runtime_error( │ │ │ │ │ +382 "GncOptimizer::checkWeightsConvergence: called with unknown loss type."); │ │ │ │ │ +383 } │ │ │ │ │ +384 if (weightsConverged │ │ │ │ │ +385 && params_.verbosity >= GncParameters::Verbosity::SUMMARY) │ │ │ │ │ +386 std::cout << "weightsConverged = true " << std::endl; │ │ │ │ │ +387 return weightsConverged; │ │ │ │ │ +388 } │ │ │ │ │ +389 │ │ │ │ │ +_3_9_1 bool _c_h_e_c_k_C_o_n_v_e_r_g_e_n_c_e(const double mu, const Vector& weights, │ │ │ │ │ +392 const double cost, const double prev_cost) const { │ │ │ │ │ +393 return _c_h_e_c_k_C_o_s_t_C_o_n_v_e_r_g_e_n_c_e(cost, prev_cost) │ │ │ │ │ +394 || _c_h_e_c_k_W_e_i_g_h_t_s_C_o_n_v_e_r_g_e_n_c_e(weights) || _c_h_e_c_k_M_u_C_o_n_v_e_r_g_e_n_c_e(mu); │ │ │ │ │ +395 } │ │ │ │ │ +396 │ │ │ │ │ +_3_9_8 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h _m_a_k_e_W_e_i_g_h_t_e_d_G_r_a_p_h(const Vector& weights) const { │ │ │ │ │ +399 // make sure all noiseModels are Gaussian or convert to Gaussian │ │ │ │ │ +400 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h newGraph; │ │ │ │ │ +401 newGraph._r_e_s_i_z_e(nfg_._s_i_z_e()); │ │ │ │ │ +402 for (size_t i = 0; i < nfg_._s_i_z_e(); i++) { │ │ │ │ │ +403 if (nfg_[i]) { │ │ │ │ │ +404 auto factor = boost::dynamic_pointer_cast< │ │ │ │ │ +405 _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r>(nfg_[i]); │ │ │ │ │ +406 auto noiseModel = │ │ │ │ │ +407 boost::dynamic_pointer_cast( │ │ │ │ │ +408 factor->noiseModel()); │ │ │ │ │ +409 if (noiseModel) { │ │ │ │ │ +410 Matrix newInfo = weights[i] * noiseModel->information(); │ │ │ │ │ +411 auto newNoiseModel = _n_o_i_s_e_M_o_d_e_l_:_:_G_a_u_s_s_i_a_n_:_:_I_n_f_o_r_m_a_t_i_o_n(newInfo); │ │ │ │ │ +412 newGraph[i] = factor->cloneWithNewNoiseModel(newNoiseModel); │ │ │ │ │ +413 } else { │ │ │ │ │ +414 throw std::runtime_error( │ │ │ │ │ +415 "GncOptimizer::makeWeightedGraph: unexpected non-Gaussian noise model."); │ │ │ │ │ +416 } │ │ │ │ │ +417 } │ │ │ │ │ +418 } │ │ │ │ │ +419 return newGraph; │ │ │ │ │ +420 } │ │ │ │ │ +421 │ │ │ │ │ +_4_2_3 Vector _c_a_l_c_u_l_a_t_e_W_e_i_g_h_t_s(const _V_a_l_u_e_s& currentEstimate, const double mu) { │ │ │ │ │ +424 Vector weights = initializeWeightsFromKnownInliersAndOutliers(); │ │ │ │ │ +425 │ │ │ │ │ +426 // do not update the weights that the user has decided are known inliers │ │ │ │ │ +427 std::vector allWeights; │ │ │ │ │ +428 for (size_t k = 0; k < nfg_._s_i_z_e(); k++) { │ │ │ │ │ +429 allWeights.push_back(k); │ │ │ │ │ +430 } │ │ │ │ │ +431 std::vector knownWeights; │ │ │ │ │ +432 std::set_union(params_.knownInliers.begin(), params_.knownInliers.end(), │ │ │ │ │ +433 params_.knownOutliers.begin(), params_.knownOutliers.end(), │ │ │ │ │ +434 std::inserter(knownWeights, knownWeights.begin())); │ │ │ │ │ +435 │ │ │ │ │ +436 std::vector unknownWeights; │ │ │ │ │ +437 std::set_difference(allWeights.begin(), allWeights.end(), │ │ │ │ │ +438 knownWeights.begin(), knownWeights.end(), │ │ │ │ │ +439 std::inserter(unknownWeights, unknownWeights.begin())); │ │ │ │ │ +440 │ │ │ │ │ +441 // update weights of known inlier/outlier measurements │ │ │ │ │ +442 switch (params_.lossType) { │ │ │ │ │ +443 case GncLossType::GM: { // use eq (12) in GNC paper │ │ │ │ │ +444 for (size_t k : unknownWeights) { │ │ │ │ │ +445 if (nfg_[k]) { │ │ │ │ │ +446 double u2_k = nfg_[k]->_e_r_r_o_r(currentEstimate); // squared (and whitened) │ │ │ │ │ +residual │ │ │ │ │ +447 weights[k] = std::pow( │ │ │ │ │ +448 (mu * barcSq_[k]) / (u2_k + mu * barcSq_[k]), 2); │ │ │ │ │ +449 } │ │ │ │ │ +450 } │ │ │ │ │ +451 return weights; │ │ │ │ │ +452 } │ │ │ │ │ +453 case GncLossType::TLS: { // use eq (14) in GNC paper │ │ │ │ │ +454 for (size_t k : unknownWeights) { │ │ │ │ │ +455 if (nfg_[k]) { │ │ │ │ │ +456 double u2_k = nfg_[k]->_e_r_r_o_r(currentEstimate); // squared (and whitened) │ │ │ │ │ +residual │ │ │ │ │ +457 double upperbound = (mu + 1) / mu * barcSq_[k]; │ │ │ │ │ +458 double lowerbound = mu / (mu + 1) * barcSq_[k]; │ │ │ │ │ +459 weights[k] = std::sqrt(barcSq_[k] * mu * (mu + 1) / u2_k) - mu; │ │ │ │ │ +460 if (u2_k >= upperbound || weights[k] < 0) { │ │ │ │ │ +461 weights[k] = 0; │ │ │ │ │ +462 } else if (u2_k <= lowerbound || weights[k] > 1) { │ │ │ │ │ +463 weights[k] = 1; │ │ │ │ │ +464 } │ │ │ │ │ +465 } │ │ │ │ │ 466 } │ │ │ │ │ -467#endif │ │ │ │ │ -468 │ │ │ │ │ -469 return point; │ │ │ │ │ -470} │ │ │ │ │ -471 │ │ │ │ │ -486template │ │ │ │ │ -_4_8_7_P_o_i_n_t_3 _t_r_i_a_n_g_u_l_a_t_e_P_o_i_n_t_3(const _C_a_m_e_r_a_S_e_t_<_C_A_M_E_R_A_>& cameras, │ │ │ │ │ -488 const typename CAMERA::MeasurementVector& measurements, │ │ │ │ │ -489 double rank_tol = 1e-9, bool _o_p_t_i_m_i_z_e = false, │ │ │ │ │ -490 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model = nullptr, │ │ │ │ │ -491 const bool useLOST = false) { │ │ │ │ │ -492 size_t m = cameras.size(); │ │ │ │ │ -493 assert(measurements.size() == m); │ │ │ │ │ -494 │ │ │ │ │ -495 if (m < 2) throw(_T_r_i_a_n_g_u_l_a_t_i_o_n_U_n_d_e_r_c_o_n_s_t_r_a_i_n_e_d_E_x_c_e_p_t_i_o_n()); │ │ │ │ │ -496 │ │ │ │ │ -497 // Triangulate linearly │ │ │ │ │ -498 _P_o_i_n_t_3 point; │ │ │ │ │ -499 if (useLOST) { │ │ │ │ │ -500 // Reduce input noise model to an isotropic noise model using the mean of │ │ │ │ │ -501 // the diagonal. │ │ │ │ │ -502 const double measurementSigma = model ? model->sigmas().mean() : 1e-4; │ │ │ │ │ -503 SharedIsotropic measurementNoise = │ │ │ │ │ -504 _n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c_:_:_S_i_g_m_a(2, measurementSigma); │ │ │ │ │ -505 │ │ │ │ │ -506 // construct poses from cameras. │ │ │ │ │ -507 std::vector poses; │ │ │ │ │ -508 poses.reserve(cameras.size()); │ │ │ │ │ -509 for (const auto& camera : cameras) poses.push_back(camera.pose()); │ │ │ │ │ -510 │ │ │ │ │ -511 // calibrate the measurements to obtain homogenous coordinates in image │ │ │ │ │ -512 // plane. │ │ │ │ │ -513 auto calibratedMeasurements = │ │ │ │ │ -514 calibrateMeasurements(cameras, measurements); │ │ │ │ │ -515 │ │ │ │ │ -516 point = _t_r_i_a_n_g_u_l_a_t_e_L_O_S_T(poses, calibratedMeasurements, measurementNoise); │ │ │ │ │ -517 } else { │ │ │ │ │ -518 // construct projection matrices from poses & calibration │ │ │ │ │ -519 auto projection_matrices = projectionMatricesFromCameras(cameras); │ │ │ │ │ -520 │ │ │ │ │ -521 // Undistort the measurements, leaving only the pinhole elements in effect. │ │ │ │ │ -522 auto undistortedMeasurements = │ │ │ │ │ -523 undistortMeasurements(cameras, measurements); │ │ │ │ │ -524 │ │ │ │ │ -525 point = │ │ │ │ │ -526 _t_r_i_a_n_g_u_l_a_t_e_D_L_T(projection_matrices, undistortedMeasurements, rank_tol); │ │ │ │ │ -527 } │ │ │ │ │ -528 │ │ │ │ │ -529 // Then refine using non-linear optimization │ │ │ │ │ -530 if (_o_p_t_i_m_i_z_e) { │ │ │ │ │ -531 point = triangulateNonlinear(cameras, measurements, point, model); │ │ │ │ │ -532 } │ │ │ │ │ -533 │ │ │ │ │ -534#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION │ │ │ │ │ -535 // verify that the triangulated point lies in front of all cameras │ │ │ │ │ -536 for (const CAMERA& camera : cameras) { │ │ │ │ │ -537 const _P_o_i_n_t_3& p_local = camera.pose().transformTo(point); │ │ │ │ │ -538 if (p_local.z() <= 0) throw(_T_r_i_a_n_g_u_l_a_t_i_o_n_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n()); │ │ │ │ │ -539 } │ │ │ │ │ -540#endif │ │ │ │ │ -541 │ │ │ │ │ -542 return point; │ │ │ │ │ -543} │ │ │ │ │ -544 │ │ │ │ │ -546template │ │ │ │ │ -_5_4_7_P_o_i_n_t_3 _t_r_i_a_n_g_u_l_a_t_e_P_o_i_n_t_3(const _C_a_m_e_r_a_S_e_t<_P_i_n_h_o_l_e_C_a_m_e_r_a_<_C_A_L_I_B_R_A_T_I_O_N_>>& │ │ │ │ │ -cameras, │ │ │ │ │ -548 const Point2Vector& measurements, │ │ │ │ │ -549 double rank_tol = 1e-9, bool _o_p_t_i_m_i_z_e = false, │ │ │ │ │ -550 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model = nullptr, │ │ │ │ │ -551 const bool useLOST = false) { │ │ │ │ │ -552 return triangulatePoint3> // │ │ │ │ │ -553 (cameras, measurements, rank_tol, _o_p_t_i_m_i_z_e, model, useLOST); │ │ │ │ │ -554} │ │ │ │ │ -555 │ │ │ │ │ -_5_5_6struct GTSAM_EXPORT _T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s { │ │ │ │ │ -557 │ │ │ │ │ -_5_5_8 double _r_a_n_k_T_o_l_e_r_a_n_c_e; │ │ │ │ │ -_5_6_0 bool _e_n_a_b_l_e_E_P_I; │ │ │ │ │ -561 │ │ │ │ │ -_5_6_6 double _l_a_n_d_m_a_r_k_D_i_s_t_a_n_c_e_T_h_r_e_s_h_o_l_d; // │ │ │ │ │ -567 │ │ │ │ │ -_5_7_3 double _d_y_n_a_m_i_c_O_u_t_l_i_e_r_R_e_j_e_c_t_i_o_n_T_h_r_e_s_h_o_l_d; │ │ │ │ │ -574 │ │ │ │ │ -_5_7_5 _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l _n_o_i_s_e_M_o_d_e_l; │ │ │ │ │ -576 │ │ │ │ │ -_5_8_6 _T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s(const double _rankTolerance = 1.0, │ │ │ │ │ -587 const bool _enableEPI = false, double _landmarkDistanceThreshold = -1, │ │ │ │ │ -588 double _dynamicOutlierRejectionThreshold = -1, │ │ │ │ │ -589 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& _noiseModel = nullptr) : │ │ │ │ │ -590 rankTolerance(_rankTolerance), enableEPI(_enableEPI), // │ │ │ │ │ -591 landmarkDistanceThreshold(_landmarkDistanceThreshold), // │ │ │ │ │ -592 dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold), │ │ │ │ │ -593 noiseModel(_noiseModel){ │ │ │ │ │ -594 } │ │ │ │ │ -595 │ │ │ │ │ -596 // stream to output │ │ │ │ │ -597 friend std::ostream &operator<<(std::ostream &os, │ │ │ │ │ -598 const _T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s& p) { │ │ │ │ │ -599 os << "rankTolerance = " << p._r_a_n_k_T_o_l_e_r_a_n_c_e << std::endl; │ │ │ │ │ -600 os << "enableEPI = " << p._e_n_a_b_l_e_E_P_I << std::endl; │ │ │ │ │ -601 os << "landmarkDistanceThreshold = " << p._l_a_n_d_m_a_r_k_D_i_s_t_a_n_c_e_T_h_r_e_s_h_o_l_d │ │ │ │ │ -602 << std::endl; │ │ │ │ │ -603 os << "dynamicOutlierRejectionThreshold = " │ │ │ │ │ -604 << p._d_y_n_a_m_i_c_O_u_t_l_i_e_r_R_e_j_e_c_t_i_o_n_T_h_r_e_s_h_o_l_d << std::endl; │ │ │ │ │ -605 os << "noise model" << std::endl; │ │ │ │ │ -606 return os; │ │ │ │ │ -607 } │ │ │ │ │ -608 │ │ │ │ │ -609private: │ │ │ │ │ -610 │ │ │ │ │ -_6_1_2 friend class boost::serialization::access; │ │ │ │ │ -613 template │ │ │ │ │ -614 void serialize(ARCHIVE & ar, const unsigned int version) { │ │ │ │ │ -615 ar & BOOST_SERIALIZATION_NVP(rankTolerance); │ │ │ │ │ -616 ar & BOOST_SERIALIZATION_NVP(enableEPI); │ │ │ │ │ -617 ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold); │ │ │ │ │ -618 ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold); │ │ │ │ │ -619 } │ │ │ │ │ -620}; │ │ │ │ │ -621 │ │ │ │ │ -_6_2_6class _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t : public boost::optional { │ │ │ │ │ -627 public: │ │ │ │ │ -628 enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT }; │ │ │ │ │ -629 Status status; │ │ │ │ │ -630 │ │ │ │ │ -631 private: │ │ │ │ │ -632 _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t(Status s) : status(s) {} │ │ │ │ │ -633 │ │ │ │ │ -634 public: │ │ │ │ │ -_6_3_8 _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t() {} │ │ │ │ │ -639 │ │ │ │ │ -_6_4_3 _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t(const _P_o_i_n_t_3& p) : status(VALID) { reset(p); } │ │ │ │ │ -644 static _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t Degenerate() { │ │ │ │ │ -645 return _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t(DEGENERATE); │ │ │ │ │ -646 } │ │ │ │ │ -647 static _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t Outlier() { return _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t(OUTLIER); │ │ │ │ │ -} │ │ │ │ │ -648 static _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t FarPoint() { │ │ │ │ │ -649 return _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t(FAR_POINT); │ │ │ │ │ -650 } │ │ │ │ │ -651 static _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t BehindCamera() { │ │ │ │ │ -652 return _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t(BEHIND_CAMERA); │ │ │ │ │ -653 } │ │ │ │ │ -654 bool valid() const { return status == VALID; } │ │ │ │ │ -655 bool degenerate() const { return status == DEGENERATE; } │ │ │ │ │ -656 bool outlier() const { return status == OUTLIER; } │ │ │ │ │ -657 bool farPoint() const { return status == FAR_POINT; } │ │ │ │ │ -658 bool behindCamera() const { return status == BEHIND_CAMERA; } │ │ │ │ │ -659 // stream to output │ │ │ │ │ -660 friend std::ostream& operator<<(std::ostream& os, │ │ │ │ │ -661 const _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t& result) { │ │ │ │ │ -662 if (result) │ │ │ │ │ -663 os << "point = " << *result << std::endl; │ │ │ │ │ -664 else │ │ │ │ │ -665 os << "no point, status = " << result.status << std::endl; │ │ │ │ │ -666 return os; │ │ │ │ │ -667 } │ │ │ │ │ -668 │ │ │ │ │ -669 private: │ │ │ │ │ -_6_7_1 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -672 template │ │ │ │ │ -673 void serialize(ARCHIVE& ar, const unsigned int version) { │ │ │ │ │ -674 ar& BOOST_SERIALIZATION_NVP(status); │ │ │ │ │ -675 } │ │ │ │ │ -676}; │ │ │ │ │ -677 │ │ │ │ │ -679template │ │ │ │ │ -_6_8_0_T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t _t_r_i_a_n_g_u_l_a_t_e_S_a_f_e(const _C_a_m_e_r_a_S_e_t_<_C_A_M_E_R_A_>& cameras, │ │ │ │ │ -681 const typename CAMERA::MeasurementVector& measured, │ │ │ │ │ -682 const _T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s& params) { │ │ │ │ │ -683 │ │ │ │ │ -684 size_t m = cameras.size(); │ │ │ │ │ -685 │ │ │ │ │ -686 // if we have a single pose the corresponding factor is uninformative │ │ │ │ │ -687 if (m < 2) │ │ │ │ │ -688 return TriangulationResult::Degenerate(); │ │ │ │ │ -689 else │ │ │ │ │ -690 // We triangulate the 3D position of the landmark │ │ │ │ │ -691 try { │ │ │ │ │ -692 _P_o_i_n_t_3 point = │ │ │ │ │ -693 triangulatePoint3(cameras, measured, params._r_a_n_k_T_o_l_e_r_a_n_c_e, │ │ │ │ │ -694 params._e_n_a_b_l_e_E_P_I, params._n_o_i_s_e_M_o_d_e_l); │ │ │ │ │ -695 │ │ │ │ │ -696 // Check landmark distance and re-projection errors to avoid outliers │ │ │ │ │ -697 size_t i = 0; │ │ │ │ │ -698 double maxReprojError = 0.0; │ │ │ │ │ -699 for(const CAMERA& camera: cameras) { │ │ │ │ │ -700 const _P_o_s_e_3& pose = camera.pose(); │ │ │ │ │ -701 if (params._l_a_n_d_m_a_r_k_D_i_s_t_a_n_c_e_T_h_r_e_s_h_o_l_d > 0 │ │ │ │ │ -702 && _d_i_s_t_a_n_c_e_3(pose._t_r_a_n_s_l_a_t_i_o_n(), point) │ │ │ │ │ -703 > params._l_a_n_d_m_a_r_k_D_i_s_t_a_n_c_e_T_h_r_e_s_h_o_l_d) │ │ │ │ │ -704 return TriangulationResult::FarPoint(); │ │ │ │ │ -705#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION │ │ │ │ │ -706 // verify that the triangulated point lies in front of all cameras │ │ │ │ │ -707 // Only needed if this was not yet handled by exception │ │ │ │ │ -708 const _P_o_i_n_t_3& p_local = pose._t_r_a_n_s_f_o_r_m_T_o(point); │ │ │ │ │ -709 if (p_local.z() <= 0) │ │ │ │ │ -710 return TriangulationResult::BehindCamera(); │ │ │ │ │ -711#endif │ │ │ │ │ -712 // Check reprojection error │ │ │ │ │ -713 if (params._d_y_n_a_m_i_c_O_u_t_l_i_e_r_R_e_j_e_c_t_i_o_n_T_h_r_e_s_h_o_l_d > 0) { │ │ │ │ │ -714 const typename CAMERA::Measurement& zi = measured.at(i); │ │ │ │ │ -715 _P_o_i_n_t_2 reprojectionError = camera.reprojectionError(point, zi); │ │ │ │ │ -716 maxReprojError = std::max(maxReprojError, reprojectionError.norm()); │ │ │ │ │ -717 } │ │ │ │ │ -718 i += 1; │ │ │ │ │ -719 } │ │ │ │ │ -720 // Flag as degenerate if average reprojection error is too large │ │ │ │ │ -721 if (params._d_y_n_a_m_i_c_O_u_t_l_i_e_r_R_e_j_e_c_t_i_o_n_T_h_r_e_s_h_o_l_d > 0 │ │ │ │ │ -722 && maxReprojError > params._d_y_n_a_m_i_c_O_u_t_l_i_e_r_R_e_j_e_c_t_i_o_n_T_h_r_e_s_h_o_l_d) │ │ │ │ │ -723 return TriangulationResult::Outlier(); │ │ │ │ │ -724 │ │ │ │ │ -725 // all good! │ │ │ │ │ -726 return _T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t(point); │ │ │ │ │ -727 } catch (_T_r_i_a_n_g_u_l_a_t_i_o_n_U_n_d_e_r_c_o_n_s_t_r_a_i_n_e_d_E_x_c_e_p_t_i_o_n&) { │ │ │ │ │ -728 // This exception is thrown if │ │ │ │ │ -729 // 1) There is a single pose for triangulation - this should not happen │ │ │ │ │ -because we checked the number of poses before │ │ │ │ │ -730 // 2) The rank of the matrix used for triangulation is < 3: rotation-only, │ │ │ │ │ -parallel cameras (or motion towards the landmark) │ │ │ │ │ -731 return TriangulationResult::Degenerate(); │ │ │ │ │ -732 } catch (_T_r_i_a_n_g_u_l_a_t_i_o_n_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n&) { │ │ │ │ │ -733 // point is behind one of the cameras: can be the case of close-to-parallel │ │ │ │ │ -cameras or may depend on outliers │ │ │ │ │ -734 return TriangulationResult::BehindCamera(); │ │ │ │ │ -735 } │ │ │ │ │ -736} │ │ │ │ │ -737 │ │ │ │ │ -738// Vector of Cameras - used by the Python/MATLAB wrapper │ │ │ │ │ -739using CameraSetCal3Bundler = CameraSet>; │ │ │ │ │ -740using CameraSetCal3_S2 = CameraSet>; │ │ │ │ │ -741using CameraSetCal3DS2 = CameraSet>; │ │ │ │ │ -742using CameraSetCal3Fisheye = CameraSet>; │ │ │ │ │ -743using CameraSetCal3Unified = CameraSet>; │ │ │ │ │ -744using CameraSetSpherical = CameraSet; │ │ │ │ │ -745} // \namespace gtsam │ │ │ │ │ -746 │ │ │ │ │ -_C_a_l_3___S_2_._h │ │ │ │ │ -The most common 5DOF 3D->2D calibration. │ │ │ │ │ -_C_a_m_e_r_a_S_e_t_._h │ │ │ │ │ -Base class to create smart factors on poses or cameras. │ │ │ │ │ -_C_a_l_3_U_n_i_f_i_e_d_._h │ │ │ │ │ -Unified Calibration Model, see Mei07icra for details. │ │ │ │ │ -_C_a_l_3_B_u_n_d_l_e_r_._h │ │ │ │ │ -Calibration used by Bundler. │ │ │ │ │ -_S_p_h_e_r_i_c_a_l_C_a_m_e_r_a_._h │ │ │ │ │ -Calibrated camera with spherical projection. │ │ │ │ │ -_P_i_n_h_o_l_e_C_a_m_e_r_a_._h │ │ │ │ │ -Base class for all pinhole cameras. │ │ │ │ │ -_C_a_l_3_F_i_s_h_e_y_e_._h │ │ │ │ │ -Calibration of a fisheye camera. │ │ │ │ │ -_P_o_s_e_2_._h │ │ │ │ │ -2D Pose │ │ │ │ │ -_C_a_l_3_D_S_2_._h │ │ │ │ │ -Calibration of a camera with radial distortion, calculations in base class │ │ │ │ │ -Cal3DS2_Base. │ │ │ │ │ +467 return weights; │ │ │ │ │ +468 } │ │ │ │ │ +469 default: │ │ │ │ │ +470 throw std::runtime_error( │ │ │ │ │ +471 "GncOptimizer::calculateWeights: called with unknown loss type."); │ │ │ │ │ +472 } │ │ │ │ │ +473 } │ │ │ │ │ +474}; │ │ │ │ │ +475 │ │ │ │ │ +476} │ │ │ │ │ _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ Factor Graph consisting of non-linear factors. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_c_a_l_i_b_r_a_t_e_M_e_a_s_u_r_e_m_e_n_t_s_S_h_a_r_e_d │ │ │ │ │ -Point3Vector calibrateMeasurementsShared(const CALIBRATION &cal, const │ │ │ │ │ -Point2Vector &measurements) │ │ │ │ │ -Convert pixel measurements in image to homogeneous measurements in the image │ │ │ │ │ -plane using shared camer... │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:357 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_L_O_S_T │ │ │ │ │ -Point3 triangulateLOST(const std::vector< Pose3 > &poses, const Point3Vector │ │ │ │ │ -&calibratedMeasurements, const SharedIsotropic &measurementNoise) │ │ │ │ │ -Triangulation using the LOST (Linear Optimal Sine Triangulation) algorithm │ │ │ │ │ -proposed in https://arxiv.... │ │ │ │ │ -DDeeffiinniittiioonn triangulation.cpp:92 │ │ │ │ │ -_g_t_s_a_m_:_:_u_n_d_i_s_t_o_r_t_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ -Point2Vector undistortMeasurements(const CALIBRATION &cal, const Point2Vector │ │ │ │ │ -&measurements) │ │ │ │ │ -Remove distortion for measurements so as if the measurements came from a │ │ │ │ │ -pinhole camera. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:282 │ │ │ │ │ -_g_t_s_a_m_:_:_c_r_e_a_t_e_P_i_n_h_o_l_e_C_a_l_i_b_r_a_t_i_o_n │ │ │ │ │ -Cal3_S2 createPinholeCalibration(const CALIBRATION &cal) │ │ │ │ │ -Create a pinhole calibration from a different Cal3 object, removing distortion. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:253 │ │ │ │ │ -_g_t_s_a_m_:_:_u_n_d_i_s_t_o_r_t_M_e_a_s_u_r_e_m_e_n_t_I_n_t_e_r_n_a_l │ │ │ │ │ -MEASUREMENT undistortMeasurementInternal(const CALIBRATION &cal, const │ │ │ │ │ -MEASUREMENT &measurement, boost::optional< Cal3_S2 > pinholeCal=boost::none) │ │ │ │ │ -Internal undistortMeasurement to be used by undistortMeasurement and │ │ │ │ │ -undistortMeasurements. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:261 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ -Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key │ │ │ │ │ -landmarkKey) │ │ │ │ │ -Optimize for triangulation. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.cpp:155 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ -Vector2 Point2 │ │ │ │ │ -As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point2 to Vector2... │ │ │ │ │ -DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_S_a_f_e │ │ │ │ │ -TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const │ │ │ │ │ -typename CAMERA::MeasurementVector &measured, const TriangulationParameters │ │ │ │ │ -¶ms) │ │ │ │ │ -triangulateSafe: extensive checking of the outcome │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:680 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_N_o_n_l_i_n_e_a_r │ │ │ │ │ -Point3 triangulateNonlinear(const std::vector< Pose3 > &poses, boost:: │ │ │ │ │ -shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, const │ │ │ │ │ -Point3 &initialEstimate, const SharedNoiseModel &model=nullptr) │ │ │ │ │ -Given an initial estimate , refine a point using measurements in several │ │ │ │ │ -cameras. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:187 │ │ │ │ │ -_g_t_s_a_m_:_:_d_i_s_t_a_n_c_e_3 │ │ │ │ │ -double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian< 1, 3 > │ │ │ │ │ -H1, OptionalJacobian< 1, 3 > H2) │ │ │ │ │ -distance between two points │ │ │ │ │ -DDeeffiinniittiioonn Point3.cpp:27 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ -Vector3 Point3 │ │ │ │ │ -As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point3 to Vector3... │ │ │ │ │ -DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ -Aliases. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_P_o_i_n_t_3 │ │ │ │ │ -Point3 triangulatePoint3(const std::vector< Pose3 > &poses, boost::shared_ptr< │ │ │ │ │ -CALIBRATION > sharedCal, const Point2Vector &measurements, double rank_tol=1e- │ │ │ │ │ -9, bool optimize=false, const SharedNoiseModel &model=nullptr, const bool │ │ │ │ │ -useLOST=false) │ │ │ │ │ -Function to triangulate 3D landmark point from an arbitrary number of poses (at │ │ │ │ │ -least 2) using the DL... │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:421 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_i_o_n_G_r_a_p_h │ │ │ │ │ -std::pair< NonlinearFactorGraph, Values > triangulationGraph(const std::vector< │ │ │ │ │ -Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector │ │ │ │ │ -&measurements, Key landmarkKey, const Point3 &initialEstimate, const │ │ │ │ │ -SharedNoiseModel &model=noiseModel::Unit::Create(2)) │ │ │ │ │ -Create a factor graph with projection factors from poses and one calibration. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:122 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_c_a_l_i_b_r_a_t_e_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ -Point3Vector calibrateMeasurements(const CameraSet< CAMERA > &cameras, const │ │ │ │ │ -typename CAMERA::MeasurementVector &measurements) │ │ │ │ │ -Convert pixel measurements in image to homogeneous measurements in the image │ │ │ │ │ -plane using camera intri... │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:381 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_D_L_T │ │ │ │ │ -Point3 triangulateDLT(const std::vector< Matrix34, Eigen::aligned_allocator< │ │ │ │ │ -Matrix34 > > &projection_matrices, const Point2Vector &measurements, double │ │ │ │ │ -rank_tol) │ │ │ │ │ -DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.cpp:127 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_i_a_n_g_u_l_a_t_e_H_o_m_o_g_e_n_e_o_u_s_D_L_T │ │ │ │ │ -Vector4 triangulateHomogeneousDLT(const std::vector< Matrix34, Eigen:: │ │ │ │ │ -aligned_allocator< Matrix34 > > &projection_matrices, const Point2Vector │ │ │ │ │ -&measurements, double rank_tol) │ │ │ │ │ -DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.cpp:27 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_3___S_2 │ │ │ │ │ -The most common 5DOF 3D->2D calibration. │ │ │ │ │ -DDeeffiinniittiioonn Cal3_S2.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_m_e_r_a_S_e_t │ │ │ │ │ -A set of cameras, all with their own calibration. │ │ │ │ │ -DDeeffiinniittiioonn CameraSet.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a │ │ │ │ │ -A pinhole camera class that has a Pose3 and a Calibration. │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:33 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e │ │ │ │ │ -A pinhole camera class that has a Pose3 and a fixed Calibration. │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:243 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ -A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_t_r_a_n_s_f_o_r_m_T_o │ │ │ │ │ -Point3 transformTo(const Point3 &point, OptionalJacobian< 3, 6 > Hself=boost:: │ │ │ │ │ -none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const │ │ │ │ │ -takes point in world coordinates and transforms it to Pose coordinates │ │ │ │ │ -DDeeffiinniittiioonn Pose3.cpp:371 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_t_r_a_n_s_l_a_t_i_o_n │ │ │ │ │ -const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const │ │ │ │ │ -get translation │ │ │ │ │ -DDeeffiinniittiioonn Pose3.cpp:308 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_U_n_d_e_r_c_o_n_s_t_r_a_i_n_e_d_E_x_c_e_p_t_i_o_n │ │ │ │ │ -Exception thrown by triangulateDLT when SVD returns rank < 3. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n │ │ │ │ │ -Exception thrown by triangulateDLT when landmark is behind one or more of the │ │ │ │ │ -cameras. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:556 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ -TriangulationParameters(const double _rankTolerance=1.0, const bool │ │ │ │ │ -_enableEPI=false, double _landmarkDistanceThreshold=-1, double │ │ │ │ │ -_dynamicOutlierRejectionThreshold=-1, const SharedNoiseModel │ │ │ │ │ -&_noiseModel=nullptr) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:586 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s_:_:_d_y_n_a_m_i_c_O_u_t_l_i_e_r_R_e_j_e_c_t_i_o_n_T_h_r_e_s_h_o_l_d │ │ │ │ │ -double dynamicOutlierRejectionThreshold │ │ │ │ │ -If this is nonnegative the we will check if the average reprojection error is │ │ │ │ │ -smaller than this thres... │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:573 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s_:_:_r_a_n_k_T_o_l_e_r_a_n_c_e │ │ │ │ │ -double rankTolerance │ │ │ │ │ -threshold to decide whether triangulation is result.degenerate │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:558 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s_:_:_l_a_n_d_m_a_r_k_D_i_s_t_a_n_c_e_T_h_r_e_s_h_o_l_d │ │ │ │ │ -double landmarkDistanceThreshold │ │ │ │ │ -if the landmark is triangulated at distance larger than this, result is flagged │ │ │ │ │ -as degenerate. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:566 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s_:_:_e_n_a_b_l_e_E_P_I │ │ │ │ │ -bool enableEPI │ │ │ │ │ -if set to true, will refine triangulation using LM │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:560 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s_:_:_n_o_i_s_e_M_o_d_e_l │ │ │ │ │ -SharedNoiseModel noiseModel │ │ │ │ │ -used in the nonlinear triangulation │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:575 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t │ │ │ │ │ -TriangulationResult is an optional point, along with the reasons why it is │ │ │ │ │ -invalid. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:626 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t │ │ │ │ │ -TriangulationResult() │ │ │ │ │ -Default constructor, only for serialization. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:638 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t │ │ │ │ │ -TriangulationResult(const Point3 &p) │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l │ │ │ │ │ +bool equal(const T &obj1, const T &obj2, double tol) │ │ │ │ │ +Call equal on the object. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:84 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_r_e_s_i_z_e │ │ │ │ │ +virtual void resize(size_t size) │ │ │ │ │ +Directly resize the number of factors in the graph. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:381 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_s_i_z_e │ │ │ │ │ +size_t size() const │ │ │ │ │ +return the number of factors (including any null factors set by remove() ). │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:326 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_G_a_u_s_s_i_a_n_:_:_I_n_f_o_r_m_a_t_i_o_n │ │ │ │ │ +static shared_ptr Information(const Matrix &M, bool smart=true) │ │ │ │ │ +A Gaussian noise model created by specifying an information matrix. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.cpp:100 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t │ │ │ │ │ +Base class for robust error models The robust M-estimators above simply tell us │ │ │ │ │ +how to re-weight the ... │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:642 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r │ │ │ │ │ +DDeeffiinniittiioonn GncOptimizer.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_c_h_e_c_k_W_e_i_g_h_t_s_C_o_n_v_e_r_g_e_n_c_e │ │ │ │ │ +bool checkWeightsConvergence(const Vector &weights) const │ │ │ │ │ +Check convergence of weights to binary values. │ │ │ │ │ +DDeeffiinniittiioonn GncOptimizer.h:364 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_s_e_t_W_e_i_g_h_t_s │ │ │ │ │ +void setWeights(const Vector w) │ │ │ │ │ +Set weights for each factor. │ │ │ │ │ +DDeeffiinniittiioonn GncOptimizer.h:144 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_c_h_e_c_k_M_u_C_o_n_v_e_r_g_e_n_c_e │ │ │ │ │ +bool checkMuConvergence(const double mu) const │ │ │ │ │ +Check if we have reached the value of mu for which the surrogate loss matches │ │ │ │ │ +the original loss. │ │ │ │ │ +DDeeffiinniittiioonn GncOptimizer.h:334 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_B_a_s_e_O_p_t_i_m_i_z_e_r │ │ │ │ │ +GncParameters::OptimizerType BaseOptimizer │ │ │ │ │ +For each parameter, specify the corresponding optimizer: e.g., │ │ │ │ │ +GaussNewtonParams -> GaussNewtonOptimi... │ │ │ │ │ +DDeeffiinniittiioonn GncOptimizer.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ +Values optimize() │ │ │ │ │ +Compute optimal solution using graduated non-convexity. │ │ │ │ │ +DDeeffiinniittiioonn GncOptimizer.h:185 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_g_e_t_I_n_l_i_e_r_C_o_s_t_T_h_r_e_s_h_o_l_d_s │ │ │ │ │ +const Vector & getInlierCostThresholds() const │ │ │ │ │ +Get the inlier threshold. │ │ │ │ │ +DDeeffiinniittiioonn GncOptimizer.h:166 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_c_h_e_c_k_C_o_s_t_C_o_n_v_e_r_g_e_n_c_e │ │ │ │ │ +bool checkCostConvergence(const double cost, const double prev_cost) const │ │ │ │ │ +Check convergence of relative cost differences. │ │ │ │ │ +DDeeffiinniittiioonn GncOptimizer.h:353 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_g_e_t_S_t_a_t_e │ │ │ │ │ +const Values & getState() const │ │ │ │ │ +Access a copy of the internal values. │ │ │ │ │ +DDeeffiinniittiioonn GncOptimizer.h:157 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_s_e_t_I_n_l_i_e_r_C_o_s_t_T_h_r_e_s_h_o_l_d_s │ │ │ │ │ +void setInlierCostThresholds(const Vector &inthVec) │ │ │ │ │ +Set the maximum weighted residual error for an inlier (one for each factor). │ │ │ │ │ +DDeeffiinniittiioonn GncOptimizer.h:125 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_c_a_l_c_u_l_a_t_e_W_e_i_g_h_t_s │ │ │ │ │ +Vector calculateWeights(const Values ¤tEstimate, const double mu) │ │ │ │ │ +Calculate gnc weights. │ │ │ │ │ +DDeeffiinniittiioonn GncOptimizer.h:423 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_i_n_i_t_i_a_l_i_z_e_M_u │ │ │ │ │ +double initializeMu() const │ │ │ │ │ +Initialize the gnc parameter mu such that loss is approximately convex (remark │ │ │ │ │ +5 in GNC paper). │ │ │ │ │ +DDeeffiinniittiioonn GncOptimizer.h:274 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_u_p_d_a_t_e_M_u │ │ │ │ │ +double updateMu(const double mu) const │ │ │ │ │ +Update the gnc parameter mu to gradually increase nonconvexity. │ │ │ │ │ +DDeeffiinniittiioonn GncOptimizer.h:319 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const GncOptimizer &other, double tol=1e-9) const │ │ │ │ │ +Equals. │ │ │ │ │ +DDeeffiinniittiioonn GncOptimizer.h:169 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_c_h_e_c_k_C_o_n_v_e_r_g_e_n_c_e │ │ │ │ │ +bool checkConvergence(const double mu, const Vector &weights, const double │ │ │ │ │ +cost, const double prev_cost) const │ │ │ │ │ +Check for convergence between consecutive GNC iterations. │ │ │ │ │ +DDeeffiinniittiioonn GncOptimizer.h:391 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_s_e_t_I_n_l_i_e_r_C_o_s_t_T_h_r_e_s_h_o_l_d_s │ │ │ │ │ +void setInlierCostThresholds(const double inth) │ │ │ │ │ +Set the maximum weighted residual error for an inlier (same for all factors). │ │ │ │ │ +DDeeffiinniittiioonn GncOptimizer.h:117 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_s_e_t_I_n_l_i_e_r_C_o_s_t_T_h_r_e_s_h_o_l_d_s_A_t_P_r_o_b_a_b_i_l_i_t_y │ │ │ │ │ +void setInlierCostThresholdsAtProbability(const double alpha) │ │ │ │ │ +Set the maximum weighted residual error threshold by specifying the probability │ │ │ │ │ +alpha that the inlier... │ │ │ │ │ +DDeeffiinniittiioonn GncOptimizer.h:132 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_m_a_k_e_W_e_i_g_h_t_e_d_G_r_a_p_h │ │ │ │ │ +NonlinearFactorGraph makeWeightedGraph(const Vector &weights) const │ │ │ │ │ +Create a graph where each factor is weighted by the gnc weights. │ │ │ │ │ +DDeeffiinniittiioonn GncOptimizer.h:398 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_g_e_t_F_a_c_t_o_r_s │ │ │ │ │ +const NonlinearFactorGraph & getFactors() const │ │ │ │ │ +Access a copy of the internal factor graph. │ │ │ │ │ +DDeeffiinniittiioonn GncOptimizer.h:154 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_g_e_t_W_e_i_g_h_t_s │ │ │ │ │ +const Vector & getWeights() const │ │ │ │ │ +Access a copy of the GNC weights. │ │ │ │ │ +DDeeffiinniittiioonn GncOptimizer.h:163 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_g_e_t_P_a_r_a_m_s │ │ │ │ │ +const GncParameters & getParams() const │ │ │ │ │ +Access a copy of the parameters. │ │ │ │ │ +DDeeffiinniittiioonn GncOptimizer.h:160 │ │ │ │ │ +_g_t_s_a_m_:_:_G_n_c_O_p_t_i_m_i_z_e_r_:_:_G_n_c_O_p_t_i_m_i_z_e_r │ │ │ │ │ +GncOptimizer(const NonlinearFactorGraph &graph, const Values &initialValues, │ │ │ │ │ +const GncParameters ¶ms=GncParameters()) │ │ │ │ │ Constructor. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:643 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_R_e_s_u_l_t_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.h:671 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l │ │ │ │ │ -Character and index key used to refer to variables. │ │ │ │ │ -DDeeffiinniittiioonn Symbol.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c_:_:_S_i_g_m_a │ │ │ │ │ -static shared_ptr Sigma(size_t dim, double sigma, bool smart=true) │ │ │ │ │ -An isotropic noise model created by specifying a standard devation sigma. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.cpp:597 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_C_r_e_a_t_e │ │ │ │ │ -static shared_ptr Create(size_t dim) │ │ │ │ │ -Create a unit covariance noise model. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:597 │ │ │ │ │ +DDeeffiinniittiioonn GncOptimizer.h:59 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r │ │ │ │ │ +A nonlinear sum-of-squares factor with a zero-mean noise model implementing the │ │ │ │ │ +density Templated on... │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:174 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +Noise model. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:186 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const NonlinearFactorGraph &other, double tol=1e-9) const │ │ │ │ │ +Test equality. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactorGraph.cpp:97 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_e_r_r_o_r │ │ │ │ │ +double error(const Values &values) const │ │ │ │ │ +unnormalized error, in the most common case │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactorGraph.cpp:170 │ │ │ │ │ _g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_i_n_s_e_r_t │ │ │ │ │ -void insert(Key j, const Value &val) │ │ │ │ │ -Add a variable with the given j, throws KeyAlreadyExists if j is already │ │ │ │ │ -present. │ │ │ │ │ -DDeeffiinniittiioonn Values.cpp:157 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_i_a_n_g_u_l_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -Non-linear factor for a constraint derived from a 2D measurement. │ │ │ │ │ -DDeeffiinniittiioonn TriangulationFactor.h:33 │ │ │ │ │ -_V_a_l_u_e_s │ │ │ │ │ -In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ -a non-linear function... │ │ │ │ │ -_S_y_m_b_o_l_._h │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _t_r_i_a_n_g_u_l_a_t_i_o_n_._h │ │ │ │ │ + * _n_o_n_l_i_n_e_a_r │ │ │ │ │ + * _G_n_c_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00467_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01073_source.html │ │ │ │┄ Files 85% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/PinholePose.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Values.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
PinholePose.h
│ │ │ │ +
Values.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/geometry/CalibratedCamera.h>
│ │ │ │ -
23#include <gtsam/geometry/Point2.h>
│ │ │ │ -
24#include <boost/make_shared.hpp>
│ │ │ │ -
25
│ │ │ │ -
26namespace gtsam {
│ │ │ │ -
27
│ │ │ │ -
33template<typename CALIBRATION>
│ │ │ │ -
│ │ │ │ -
34class PinholeBaseK: public PinholeBase {
│ │ │ │ -
35
│ │ │ │ -
36private:
│ │ │ │ +
25#pragma once
│ │ │ │ +
26
│ │ │ │ +
27#include <gtsam/base/FastDefaultAllocator.h>
│ │ │ │ +
28#include <gtsam/base/GenericValue.h>
│ │ │ │ +
29#include <gtsam/base/VectorSpace.h>
│ │ │ │ +
30#include <gtsam/inference/Key.h>
│ │ │ │ +
31#include <boost/ptr_container/serialize_ptr_map.hpp>
│ │ │ │ +
32#include <boost/shared_ptr.hpp>
│ │ │ │ +
33#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ +
34#include <boost/iterator/transform_iterator.hpp>
│ │ │ │ +
35#include <boost/iterator/filter_iterator.hpp>
│ │ │ │ +
36#endif
│ │ │ │
37
│ │ │ │ -
38 GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
│ │ │ │ -
39
│ │ │ │ -
40 // Get dimensions of calibration type at compile time
│ │ │ │ -
41 static const int DimK = FixedDimension<CALIBRATION>::value;
│ │ │ │ +
38#include <string>
│ │ │ │ +
39#include <utility>
│ │ │ │ +
40
│ │ │ │ +
41namespace gtsam {
│ │ │ │
42
│ │ │ │ -
43public:
│ │ │ │ -
44
│ │ │ │ -
45 typedef CALIBRATION CalibrationType;
│ │ │ │ -
46
│ │ │ │ -
49
│ │ │ │ -
│ │ │ │ -
51 PinholeBaseK() {
│ │ │ │ -
52 }
│ │ │ │ -
│ │ │ │ -
53
│ │ │ │ -
│ │ │ │ -
55 explicit PinholeBaseK(const Pose3& pose) :
│ │ │ │ -
56 PinholeBase(pose) {
│ │ │ │ -
57 }
│ │ │ │ -
│ │ │ │ -
58
│ │ │ │ -
62
│ │ │ │ -
63 explicit PinholeBaseK(const Vector &v) :
│ │ │ │ -
64 PinholeBase(v) {
│ │ │ │ -
65 }
│ │ │ │ +
43 // Forward declarations / utilities
│ │ │ │ +
44 class VectorValues;
│ │ │ │ +
45 class ValueAutomaticCasting;
│ │ │ │ +
46 template<typename T> static bool _truePredicate(const T&) { return true; }
│ │ │ │ +
47
│ │ │ │ +
48 /* ************************************************************************* */
│ │ │ │ +
│ │ │ │ +
49 class GTSAM_EXPORT ValueCloneAllocator {
│ │ │ │ +
50 public:
│ │ │ │ +
51 static Value* allocate_clone(const Value& a) { return a.clone_(); }
│ │ │ │ +
52 static void deallocate_clone(const Value* a) { a->deallocate_(); }
│ │ │ │ +
53 ValueCloneAllocator() {}
│ │ │ │ +
54 };
│ │ │ │ +
│ │ │ │ +
55
│ │ │ │ +
│ │ │ │ +
65 class GTSAM_EXPORT Values {
│ │ │ │
66
│ │ │ │ -
70
│ │ │ │ -
71 virtual ~PinholeBaseK() {
│ │ │ │ -
72 }
│ │ │ │ -
73
│ │ │ │ -
75 virtual const CALIBRATION& calibration() const = 0;
│ │ │ │ -
76
│ │ │ │ +
67 private:
│ │ │ │ +
68 // Internally we store a boost ptr_map, with a ValueCloneAllocator (defined
│ │ │ │ +
69 // below) to clone and deallocate the Value objects, and our compile-flag-
│ │ │ │ +
70 // dependent FastDefaultAllocator to allocate map nodes. In this way, the
│ │ │ │ +
71 // user defines the allocation details (i.e. optimize for memory pool/arenas
│ │ │ │ +
72 // concurrency).
│ │ │ │ +
73 typedef internal::FastDefaultAllocator<typename std::pair<const Key, void*>>::type KeyValuePtrPairAllocator;
│ │ │ │ +
74 typedef boost::ptr_map<
│ │ │ │ +
75 Key,
│ │ │ │ +
76 Value,
│ │ │ │ +
77 std::less<Key>,
│ │ │ │ +
78 ValueCloneAllocator,
│ │ │ │ +
79 KeyValuePtrPairAllocator > KeyValueMap;
│ │ │ │
80
│ │ │ │ -
│ │ │ │ -
82 std::pair<Point2, bool> projectSafe(const Point3& pw) const {
│ │ │ │ -
83 std::pair<Point2, bool> pn = PinholeBase::projectSafe(pw);
│ │ │ │ -
84 pn.first = calibration().uncalibrate(pn.first);
│ │ │ │ -
85 return pn;
│ │ │ │ -
86 }
│ │ │ │ -
│ │ │ │ -
87
│ │ │ │ +
81 // The member to store the values, see just above
│ │ │ │ +
82 KeyValueMap values_;
│ │ │ │ +
83
│ │ │ │ +
84 public:
│ │ │ │ +
85
│ │ │ │ +
87 typedef boost::shared_ptr<Values> shared_ptr;
│ │ │ │
88
│ │ │ │ -
95 template <class POINT>
│ │ │ │ -
│ │ │ │ -
96 Point2 _project(const POINT& pw, OptionalJacobian<2, 6> Dpose,
│ │ │ │ -
97 OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint,
│ │ │ │ -
98 OptionalJacobian<2, DimK> Dcal) const {
│ │ │ │ +
90 typedef boost::shared_ptr<const Values> const_shared_ptr;
│ │ │ │ +
91
│ │ │ │ +
│ │ │ │ +
93 struct GTSAM_EXPORT KeyValuePair {
│ │ │ │ +
94 const Key key;
│ │ │ │ +
95 Value& value;
│ │ │ │ +
96
│ │ │ │ +
97 KeyValuePair(Key _key, Value& _value) : key(_key), value(_value) {}
│ │ │ │ +
98 };
│ │ │ │ +
│ │ │ │
99
│ │ │ │ -
100 // project to normalized coordinates
│ │ │ │ -
101 const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
│ │ │ │ -
102
│ │ │ │ -
103 // uncalibrate to pixel coordinates
│ │ │ │ -
104 Matrix2 Dpi_pn;
│ │ │ │ -
105 const Point2 pi = calibration().uncalibrate(pn, Dcal,
│ │ │ │ -
106 Dpose || Dpoint ? &Dpi_pn : 0);
│ │ │ │ -
107
│ │ │ │ -
108 // If needed, apply chain rule
│ │ │ │ -
109 if (Dpose)
│ │ │ │ -
110 *Dpose = Dpi_pn * *Dpose;
│ │ │ │ -
111 if (Dpoint)
│ │ │ │ -
112 *Dpoint = Dpi_pn * *Dpoint;
│ │ │ │ +
│ │ │ │ +
101 struct GTSAM_EXPORT ConstKeyValuePair {
│ │ │ │ +
102 const Key key;
│ │ │ │ +
103 const Value& value;
│ │ │ │ +
104
│ │ │ │ +
105 ConstKeyValuePair(Key _key, const Value& _value) : key(_key), value(_value) {}
│ │ │ │ +
106 ConstKeyValuePair(const KeyValuePair& kv) : key(kv.key), value(kv.value) {}
│ │ │ │ +
107 };
│ │ │ │ +
│ │ │ │ +
108
│ │ │ │ +
109 typedef KeyValuePair value_type;
│ │ │ │ +
110
│ │ │ │
113
│ │ │ │ -
114 return pi;
│ │ │ │ -
115 }
│ │ │ │ -
│ │ │ │ +
115 Values() = default;
│ │ │ │
116
│ │ │ │ -
│ │ │ │ -
118 Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
│ │ │ │ -
119 OptionalJacobian<2, 3> Dpoint = boost::none,
│ │ │ │ -
120 OptionalJacobian<2, DimK> Dcal = boost::none) const {
│ │ │ │ -
121 return _project(pw, Dpose, Dpoint, Dcal);
│ │ │ │ -
122 }
│ │ │ │ -
│ │ │ │ -
123
│ │ │ │ -
│ │ │ │ -
125 Point2 reprojectionError(const Point3& pw, const Point2& measured, OptionalJacobian<2, 6> Dpose = boost::none,
│ │ │ │ -
126 OptionalJacobian<2, 3> Dpoint = boost::none,
│ │ │ │ -
127 OptionalJacobian<2, DimK> Dcal = boost::none) const {
│ │ │ │ -
128 return Point2(_project(pw, Dpose, Dpoint, Dcal) - measured);
│ │ │ │ -
129 }
│ │ │ │ -
│ │ │ │ -
130
│ │ │ │ -
│ │ │ │ -
132 Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
│ │ │ │ -
133 OptionalJacobian<2, 2> Dpoint = boost::none,
│ │ │ │ -
134 OptionalJacobian<2, DimK> Dcal = boost::none) const {
│ │ │ │ -
135 return _project(pw, Dpose, Dpoint, Dcal);
│ │ │ │ -
136 }
│ │ │ │ -
│ │ │ │ -
137
│ │ │ │ -
│ │ │ │ -
139 Point3 backproject(const Point2& p, double depth,
│ │ │ │ -
140 OptionalJacobian<3, 6> Dresult_dpose = boost::none,
│ │ │ │ -
141 OptionalJacobian<3, 2> Dresult_dp = boost::none,
│ │ │ │ -
142 OptionalJacobian<3, 1> Dresult_ddepth = boost::none,
│ │ │ │ -
143 OptionalJacobian<3, DimK> Dresult_dcal = boost::none) const {
│ │ │ │ -
144 typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
│ │ │ │ -
145 Matrix2K Dpn_dcal;
│ │ │ │ -
146 Matrix22 Dpn_dp;
│ │ │ │ -
147 const Point2 pn = calibration().calibrate(p, Dresult_dcal ? &Dpn_dcal : 0,
│ │ │ │ -
148 Dresult_dp ? &Dpn_dp : 0);
│ │ │ │ -
149 Matrix32 Dpoint_dpn;
│ │ │ │ -
150 Matrix31 Dpoint_ddepth;
│ │ │ │ -
151 const Point3 point = BackprojectFromCamera(pn, depth,
│ │ │ │ -
152 (Dresult_dp || Dresult_dcal) ? &Dpoint_dpn : 0,
│ │ │ │ -
153 Dresult_ddepth ? &Dpoint_ddepth : 0);
│ │ │ │ -
154 Matrix33 Dresult_dpoint;
│ │ │ │ -
155 const Point3 result = pose().transformFrom(point, Dresult_dpose,
│ │ │ │ -
156 (Dresult_ddepth ||
│ │ │ │ -
157 Dresult_dp ||
│ │ │ │ -
158 Dresult_dcal) ? &Dresult_dpoint : 0);
│ │ │ │ -
159 if (Dresult_dcal)
│ │ │ │ -
160 *Dresult_dcal = Dresult_dpoint * Dpoint_dpn * Dpn_dcal; // (3x3)*(3x2)*(2xDimK)
│ │ │ │ -
161 if (Dresult_dp)
│ │ │ │ -
162 *Dresult_dp = Dresult_dpoint * Dpoint_dpn * Dpn_dp; // (3x3)*(3x2)*(2x2)
│ │ │ │ -
163 if (Dresult_ddepth)
│ │ │ │ -
164 *Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth; // (3x3)*(3x1)
│ │ │ │ -
165
│ │ │ │ -
166 return result;
│ │ │ │ -
167 }
│ │ │ │ -
│ │ │ │ -
168
│ │ │ │ -
│ │ │ │ -
170 Unit3 backprojectPointAtInfinity(const Point2& p) const {
│ │ │ │ -
171 const Point2 pn = calibration().calibrate(p);
│ │ │ │ -
172 const Unit3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1
│ │ │ │ -
173 return pose().rotation().rotate(pc);
│ │ │ │ -
174 }
│ │ │ │ -
│ │ │ │ -
175
│ │ │ │ -
│ │ │ │ -
181 double range(const Point3& point,
│ │ │ │ -
182 OptionalJacobian<1, 6> Dcamera = boost::none,
│ │ │ │ -
183 OptionalJacobian<1, 3> Dpoint = boost::none) const {
│ │ │ │ -
184 return pose().range(point, Dcamera, Dpoint);
│ │ │ │ -
185 }
│ │ │ │ -
│ │ │ │ -
186
│ │ │ │ -
│ │ │ │ -
192 double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = boost::none,
│ │ │ │ -
193 OptionalJacobian<1, 6> Dpose = boost::none) const {
│ │ │ │ -
194 return this->pose().range(pose, Dcamera, Dpose);
│ │ │ │ -
195 }
│ │ │ │ -
│ │ │ │ -
196
│ │ │ │ -
│ │ │ │ -
202 double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> Dcamera =
│ │ │ │ -
203 boost::none, OptionalJacobian<1, 6> Dother = boost::none) const {
│ │ │ │ -
204 return pose().range(camera.pose(), Dcamera, Dother);
│ │ │ │ -
205 }
│ │ │ │ -
│ │ │ │ -
206
│ │ │ │ -
212 template<class CalibrationB>
│ │ │ │ -
│ │ │ │ -
213 double range(const PinholeBaseK<CalibrationB>& camera,
│ │ │ │ -
214 OptionalJacobian<1, 6> Dcamera = boost::none,
│ │ │ │ -
215 OptionalJacobian<1, 6> Dother = boost::none) const {
│ │ │ │ -
216 return pose().range(camera.pose(), Dcamera, Dother);
│ │ │ │ -
217 }
│ │ │ │ -
│ │ │ │ -
218
│ │ │ │ -
219private:
│ │ │ │ +
118 Values(const Values& other);
│ │ │ │ +
119
│ │ │ │ +
121 Values(Values&& other);
│ │ │ │ +
122
│ │ │ │ +
128 Values(std::initializer_list<ConstKeyValuePair> init);
│ │ │ │ +
129
│ │ │ │ +
131 Values(const Values& other, const VectorValues& delta);
│ │ │ │ +
132
│ │ │ │ +
136
│ │ │ │ +
138 void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ +
139
│ │ │ │ +
141 bool equals(const Values& other, double tol=1e-9) const;
│ │ │ │ +
142
│ │ │ │ +
146
│ │ │ │ +
155 template <typename ValueType>
│ │ │ │ +
156 const ValueType at(Key j) const;
│ │ │ │ +
157
│ │ │ │ +
159 double atDouble(size_t key) const { return at<double>(key);}
│ │ │ │ +
160
│ │ │ │ +
166 const Value& at(Key j) const;
│ │ │ │ +
167
│ │ │ │ +
171 bool exists(Key j) const;
│ │ │ │ +
172
│ │ │ │ +
177 template<typename ValueType>
│ │ │ │ +
178 boost::optional<const ValueType&> exists(Key j) const;
│ │ │ │ +
179
│ │ │ │ +
181 size_t size() const { return values_.size(); }
│ │ │ │ +
182
│ │ │ │ +
184 bool empty() const { return values_.empty(); }
│ │ │ │ +
185
│ │ │ │ +
189
│ │ │ │ +
│ │ │ │ +
190 struct deref_iterator {
│ │ │ │ +
191 using const_iterator_type = typename KeyValueMap::const_iterator;
│ │ │ │ +
192 const_iterator_type it_;
│ │ │ │ +
193 deref_iterator(const_iterator_type it) : it_(it) {}
│ │ │ │ +
194 ConstKeyValuePair operator*() const { return {it_->first, *(it_->second)}; }
│ │ │ │ +
195 boost::shared_ptr<ConstKeyValuePair> operator->() {
│ │ │ │ +
196 return boost::make_shared<ConstKeyValuePair>(it_->first, *(it_->second));
│ │ │ │ +
197 }
│ │ │ │ +
198 bool operator==(const deref_iterator& other) const {
│ │ │ │ +
199 return it_ == other.it_;
│ │ │ │ +
200 }
│ │ │ │ +
201 bool operator!=(const deref_iterator& other) const { return it_ != other.it_; }
│ │ │ │ +
202 deref_iterator& operator++() {
│ │ │ │ +
203 ++it_;
│ │ │ │ +
204 return *this;
│ │ │ │ +
205 }
│ │ │ │ +
206 };
│ │ │ │ +
│ │ │ │ +
207
│ │ │ │ +
208 deref_iterator begin() const { return deref_iterator(values_.begin()); }
│ │ │ │ +
209 deref_iterator end() const { return deref_iterator(values_.end()); }
│ │ │ │ +
210
│ │ │ │ +
213 deref_iterator find(Key j) const { return deref_iterator(values_.find(j)); }
│ │ │ │ +
214
│ │ │ │ +
216 deref_iterator lower_bound(Key j) const { return deref_iterator(values_.lower_bound(j)); }
│ │ │ │ +
217
│ │ │ │ +
219 deref_iterator upper_bound(Key j) const { return deref_iterator(values_.upper_bound(j)); }
│ │ │ │
220
│ │ │ │ -
222 friend class boost::serialization::access;
│ │ │ │ -
223 template<class Archive>
│ │ │ │ -
224 void serialize(Archive & ar, const unsigned int /*version*/) {
│ │ │ │ -
225 ar
│ │ │ │ -
226 & boost::serialization::make_nvp("PinholeBase",
│ │ │ │ -
227 boost::serialization::base_object<PinholeBase>(*this));
│ │ │ │ -
228 }
│ │ │ │ -
229
│ │ │ │ -
230public:
│ │ │ │ -
231 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
232};
│ │ │ │ -
│ │ │ │ -
233// end of class PinholeBaseK
│ │ │ │ -
234
│ │ │ │ -
242template<typename CALIBRATION>
│ │ │ │ -
│ │ │ │ -
243class PinholePose: public PinholeBaseK<CALIBRATION> {
│ │ │ │ +
224
│ │ │ │ +
226 Values retract(const VectorValues& delta) const;
│ │ │ │ +
227
│ │ │ │ +
232 void retractMasked(const VectorValues& delta, const KeySet& mask);
│ │ │ │ +
233
│ │ │ │ +
235 VectorValues localCoordinates(const Values& cp) const;
│ │ │ │ +
236
│ │ │ │ +
238
│ │ │ │ +
240 void insert(Key j, const Value& val);
│ │ │ │ +
241
│ │ │ │ +
243 void insert(const Values& values);
│ │ │ │
244
│ │ │ │ -
245private:
│ │ │ │ -
246
│ │ │ │ -
247 typedef PinholeBaseK<CALIBRATION> Base;
│ │ │ │ -
248 boost::shared_ptr<CALIBRATION> K_;
│ │ │ │ -
249
│ │ │ │ -
250public:
│ │ │ │ -
251
│ │ │ │ -
252 enum {
│ │ │ │ -
253 dimension = 6
│ │ │ │ -
254 };
│ │ │ │ -
255
│ │ │ │ -
258
│ │ │ │ -
│ │ │ │ -
260 PinholePose() {
│ │ │ │ -
261 }
│ │ │ │ -
│ │ │ │ -
262
│ │ │ │ -
│ │ │ │ -
264 explicit PinholePose(const Pose3& pose) :
│ │ │ │ -
265 Base(pose), K_(new CALIBRATION()) {
│ │ │ │ -
266 }
│ │ │ │ -
│ │ │ │ -
267
│ │ │ │ -
│ │ │ │ -
269 PinholePose(const Pose3& pose, const boost::shared_ptr<CALIBRATION>& K) :
│ │ │ │ -
270 Base(pose), K_(K) {
│ │ │ │ -
271 }
│ │ │ │ -
│ │ │ │ -
272
│ │ │ │ -
276
│ │ │ │ -
│ │ │ │ -
284 static PinholePose Level(const boost::shared_ptr<CALIBRATION>& K,
│ │ │ │ -
285 const Pose2& pose2, double height) {
│ │ │ │ -
286 return PinholePose(Base::LevelPose(pose2, height), K);
│ │ │ │ -
287 }
│ │ │ │ -
│ │ │ │ +
248 template <typename ValueType>
│ │ │ │ +
249 void insert(Key j, const ValueType& val);
│ │ │ │ +
250
│ │ │ │ +
252 void insertDouble(Key j, double c) { insert<double>(j,c); }
│ │ │ │ +
253
│ │ │ │ +
255 void update(Key j, const Value& val);
│ │ │ │ +
256
│ │ │ │ +
261 template <typename T>
│ │ │ │ +
262 void update(Key j, const T& val);
│ │ │ │ +
263
│ │ │ │ +
265 void update(const Values& values);
│ │ │ │ +
266
│ │ │ │ +
268 void insert_or_assign(Key j, const Value& val);
│ │ │ │ +
269
│ │ │ │ +
274 void insert_or_assign(const Values& values);
│ │ │ │ +
275
│ │ │ │ +
277 template <typename ValueType>
│ │ │ │ +
278 void insert_or_assign(Key j, const ValueType& val);
│ │ │ │ +
279
│ │ │ │ +
281 void erase(Key j);
│ │ │ │ +
282
│ │ │ │ +
287 KeyVector keys() const;
│ │ │ │
288
│ │ │ │ -
│ │ │ │ -
290 static PinholePose Level(const Pose2& pose2, double height) {
│ │ │ │ -
291 return PinholePose::Level(boost::make_shared<CALIBRATION>(), pose2, height);
│ │ │ │ -
292 }
│ │ │ │ -
│ │ │ │ +
292 KeySet keySet() const;
│ │ │ │
293
│ │ │ │ -
│ │ │ │ -
303 static PinholePose Lookat(const Point3& eye, const Point3& target,
│ │ │ │ -
304 const Point3& upVector, const boost::shared_ptr<CALIBRATION>& K =
│ │ │ │ -
305 boost::make_shared<CALIBRATION>()) {
│ │ │ │ -
306 return PinholePose(Base::LookatPose(eye, target, upVector), K);
│ │ │ │ -
307 }
│ │ │ │ -
│ │ │ │ +
295 Values& operator=(const Values& rhs);
│ │ │ │ +
296
│ │ │ │ +
298 void swap(Values& other) { values_.swap(other.values_); }
│ │ │ │ +
299
│ │ │ │ +
301 void clear() { values_.clear(); }
│ │ │ │ +
302
│ │ │ │ +
304 size_t dim() const;
│ │ │ │ +
305
│ │ │ │ +
307 std::map<Key,size_t> dims() const;
│ │ │ │
308
│ │ │ │ -
312
│ │ │ │ -
│ │ │ │ -
314 explicit PinholePose(const Vector &v) :
│ │ │ │ -
315 Base(v), K_(new CALIBRATION()) {
│ │ │ │ -
316 }
│ │ │ │ -
│ │ │ │ -
317
│ │ │ │ -
│ │ │ │ -
319 PinholePose(const Vector &v, const Vector &K) :
│ │ │ │ -
320 Base(v), K_(new CALIBRATION(K)) {
│ │ │ │ -
321 }
│ │ │ │ -
│ │ │ │ +
310 VectorValues zeroVectors() const;
│ │ │ │ +
311
│ │ │ │ +
312 // Count values of given type \c ValueType
│ │ │ │ +
313 template<class ValueType>
│ │ │ │ +
314 size_t count() const {
│ │ │ │ +
315 size_t i = 0;
│ │ │ │ +
316 for (const auto key_value : values_) {
│ │ │ │ +
317 if (dynamic_cast<const GenericValue<ValueType>*>(key_value.second))
│ │ │ │ +
318 ++i;
│ │ │ │ +
319 }
│ │ │ │ +
320 return i;
│ │ │ │ +
321 }
│ │ │ │
322
│ │ │ │ -
323 // Init from Pose3 and calibration
│ │ │ │ -
324 PinholePose(const Pose3 &pose, const Vector &K) :
│ │ │ │ -
325 Base(pose), K_(new CALIBRATION(K)) {
│ │ │ │ -
326 }
│ │ │ │ -
327
│ │ │ │ -
331
│ │ │ │ -
│ │ │ │ -
333 bool equals(const Base &camera, double tol = 1e-9) const {
│ │ │ │ -
334 const PinholePose* e = dynamic_cast<const PinholePose*>(&camera);
│ │ │ │ -
335 return Base::equals(camera, tol) && K_->equals(e->calibration(), tol);
│ │ │ │ -
336 }
│ │ │ │ -
│ │ │ │ -
337
│ │ │ │ -
│ │ │ │ -
339 friend std::ostream& operator<<(std::ostream &os, const PinholePose& camera) {
│ │ │ │ -
340 os << "{R: " << camera.pose().rotation().rpy().transpose();
│ │ │ │ -
341 os << ", t: " << camera.pose().translation().transpose();
│ │ │ │ -
342 if (!camera.K_) os << ", K: none";
│ │ │ │ -
343 else os << ", K: " << *camera.K_;
│ │ │ │ -
344 os << "}";
│ │ │ │ -
345 return os;
│ │ │ │ -
346 }
│ │ │ │ -
│ │ │ │ -
347
│ │ │ │ -
│ │ │ │ -
349 void print(const std::string& s = "PinholePose") const override {
│ │ │ │ -
350 Base::print(s);
│ │ │ │ -
351 if (!K_)
│ │ │ │ -
352 std::cout << "s No calibration given" << std::endl;
│ │ │ │ -
353 else
│ │ │ │ -
354 K_->print(s + ".calibration");
│ │ │ │ -
355 }
│ │ │ │ -
│ │ │ │ -
356
│ │ │ │ -
360
│ │ │ │ -
361 ~PinholePose() override {
│ │ │ │ -
362 }
│ │ │ │ -
363
│ │ │ │ -
│ │ │ │ -
365 const boost::shared_ptr<CALIBRATION>& sharedCalibration() const {
│ │ │ │ -
366 return K_;
│ │ │ │ -
367 }
│ │ │ │ -
│ │ │ │ -
368
│ │ │ │ -
│ │ │ │ -
370 const CALIBRATION& calibration() const override {
│ │ │ │ -
371 return *K_;
│ │ │ │ -
372 }
│ │ │ │ -
│ │ │ │ -
373
│ │ │ │ -
│ │ │ │ -
379 Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
│ │ │ │ -
380 OptionalJacobian<2, 3> Dpoint = boost::none) const {
│ │ │ │ -
381 return Base::project(pw, Dpose, Dpoint);
│ │ │ │ -
382 }
│ │ │ │ -
│ │ │ │ -
383
│ │ │ │ -
│ │ │ │ -
385 Point2 project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
│ │ │ │ -
386 OptionalJacobian<2, 2> Dpoint = boost::none) const {
│ │ │ │ -
387 return Base::project(pw, Dpose, Dpoint);
│ │ │ │ -
388 }
│ │ │ │ -
│ │ │ │ -
389
│ │ │ │ -
393
│ │ │ │ -
│ │ │ │ -
395 size_t dim() const {
│ │ │ │ -
396 return 6;
│ │ │ │ -
397 }
│ │ │ │ -
│ │ │ │ -
398
│ │ │ │ -
│ │ │ │ -
400 static size_t Dim() {
│ │ │ │ -
401 return 6;
│ │ │ │ -
402 }
│ │ │ │ -
│ │ │ │ -
403
│ │ │ │ -
│ │ │ │ -
405 PinholePose retract(const Vector6& d) const {
│ │ │ │ -
406 return PinholePose(Base::pose().retract(d), K_);
│ │ │ │ -
407 }
│ │ │ │ -
│ │ │ │ -
408
│ │ │ │ -
│ │ │ │ -
410 Vector6 localCoordinates(const PinholePose& p) const {
│ │ │ │ -
411 return Base::pose().localCoordinates(p.Base::pose());
│ │ │ │ -
412 }
│ │ │ │ -
│ │ │ │ +
342 template <class ValueType>
│ │ │ │ +
343 std::map<Key, ValueType> // , std::less<Key>, Eigen::aligned_allocator<ValueType>
│ │ │ │ +
344 extract(const std::function<bool(Key)>& filterFcn = &_truePredicate<Key>) const;
│ │ │ │ +
345
│ │ │ │ +
346#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ +
347 // Types obtained by iterating
│ │ │ │ +
348 typedef KeyValueMap::const_iterator::value_type ConstKeyValuePtrPair;
│ │ │ │ +
349 typedef KeyValueMap::iterator::value_type KeyValuePtrPair;
│ │ │ │ +
350
│ │ │ │ +
352 typedef boost::transform_iterator<
│ │ │ │ +
353 std::function<KeyValuePair(const KeyValuePtrPair&)>, KeyValueMap::iterator> iterator;
│ │ │ │ +
354
│ │ │ │ +
356 typedef boost::transform_iterator<
│ │ │ │ +
357 std::function<ConstKeyValuePair(const ConstKeyValuePtrPair&)>, KeyValueMap::const_iterator> const_iterator;
│ │ │ │ +
358
│ │ │ │ +
360 typedef boost::transform_iterator<
│ │ │ │ +
361 std::function<KeyValuePair(const KeyValuePtrPair&)>, KeyValueMap::reverse_iterator> reverse_iterator;
│ │ │ │ +
362
│ │ │ │ +
364 typedef boost::transform_iterator<
│ │ │ │ +
365 std::function<ConstKeyValuePair(const ConstKeyValuePtrPair&)>, KeyValueMap::const_reverse_iterator> const_reverse_iterator;
│ │ │ │ +
366
│ │ │ │ +
371 std::pair<iterator, bool> tryInsert(Key j, const Value& value);
│ │ │ │ +
372
│ │ │ │ +
373 static ConstKeyValuePair make_const_deref_pair(const KeyValueMap::const_iterator::value_type& key_value) {
│ │ │ │ +
374 return ConstKeyValuePair(key_value.first, *key_value.second); }
│ │ │ │ +
375
│ │ │ │ +
376 static KeyValuePair make_deref_pair(const KeyValueMap::iterator::value_type& key_value) {
│ │ │ │ +
377 return KeyValuePair(key_value.first, *key_value.second); }
│ │ │ │ +
378
│ │ │ │ +
379 const_iterator _begin() const { return boost::make_transform_iterator(values_.begin(), &make_const_deref_pair); }
│ │ │ │ +
380 const_iterator _end() const { return boost::make_transform_iterator(values_.end(), &make_const_deref_pair); }
│ │ │ │ +
381 iterator begin() { return boost::make_transform_iterator(values_.begin(), &make_deref_pair); }
│ │ │ │ +
382 iterator end() { return boost::make_transform_iterator(values_.end(), &make_deref_pair); }
│ │ │ │ +
383 const_reverse_iterator rbegin() const { return boost::make_transform_iterator(values_.rbegin(), &make_const_deref_pair); }
│ │ │ │ +
384 const_reverse_iterator rend() const { return boost::make_transform_iterator(values_.rend(), &make_const_deref_pair); }
│ │ │ │ +
385 reverse_iterator rbegin() { return boost::make_transform_iterator(values_.rbegin(), &make_deref_pair); }
│ │ │ │ +
386 reverse_iterator rend() { return boost::make_transform_iterator(values_.rend(), &make_deref_pair); }
│ │ │ │ +
387
│ │ │ │ +
390 iterator find(Key j) { return boost::make_transform_iterator(values_.find(j), &make_deref_pair); }
│ │ │ │ +
391
│ │ │ │ +
393 iterator lower_bound(Key j) { return boost::make_transform_iterator(values_.lower_bound(j), &make_deref_pair); }
│ │ │ │ +
394
│ │ │ │ +
396 iterator upper_bound(Key j) { return boost::make_transform_iterator(values_.upper_bound(j), &make_deref_pair); }
│ │ │ │ +
397
│ │ │ │ +
399 template <class ValueType = Value>
│ │ │ │ +
400 class Filtered;
│ │ │ │ +
401
│ │ │ │ +
403 template <class ValueType = Value>
│ │ │ │ +
404 class ConstFiltered;
│ │ │ │ +
405
│ │ │ │ +
407 template <class ValueType>
│ │ │ │ +
408 Values(const Filtered<ValueType>& view);
│ │ │ │ +
409
│ │ │ │ +
411 template <class ValueType>
│ │ │ │ +
412 Values(const ConstFiltered<ValueType>& view);
│ │ │ │
413
│ │ │ │ -
│ │ │ │ -
415 static PinholePose Identity() {
│ │ │ │ -
416 return PinholePose(); // assumes that the default constructor is valid
│ │ │ │ -
417 }
│ │ │ │ -
│ │ │ │ -
418
│ │ │ │ -
│ │ │ │ -
420 Matrix34 cameraProjectionMatrix() const {
│ │ │ │ -
421 Matrix34 P = Matrix34(PinholeBase::pose().inverse().matrix().block(0, 0, 3, 4));
│ │ │ │ -
422 return K_->K() * P;
│ │ │ │ -
423 }
│ │ │ │ -
│ │ │ │ -
424
│ │ │ │ -
│ │ │ │ -
426 Vector defaultErrorWhenTriangulatingBehindCamera() const {
│ │ │ │ -
427 return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_->fx());;
│ │ │ │ -
428 }
│ │ │ │ -
│ │ │ │ -
430
│ │ │ │ -
431private:
│ │ │ │ +
415 Filtered<Value> GTSAM_DEPRECATED
│ │ │ │ +
416 filter(const std::function<bool(Key)>& filterFcn);
│ │ │ │ +
417
│ │ │ │ +
419 template <class ValueType>
│ │ │ │ +
420 Filtered<ValueType> GTSAM_DEPRECATED
│ │ │ │ +
421 filter(const std::function<bool(Key)>& filterFcn = &_truePredicate<Key>);
│ │ │ │ +
422
│ │ │ │ +
424 ConstFiltered<Value> GTSAM_DEPRECATED
│ │ │ │ +
425 filter(const std::function<bool(Key)>& filterFcn) const;
│ │ │ │ +
426
│ │ │ │ +
428 template <class ValueType>
│ │ │ │ +
429 ConstFiltered<ValueType> GTSAM_DEPRECATED filter(
│ │ │ │ +
430 const std::function<bool(Key)>& filterFcn = &_truePredicate<Key>) const;
│ │ │ │ +
431#endif
│ │ │ │
432
│ │ │ │ -
434 friend class boost::serialization::access;
│ │ │ │ -
435 template<class Archive>
│ │ │ │ -
436 void serialize(Archive & ar, const unsigned int /*version*/) {
│ │ │ │ -
437 ar
│ │ │ │ -
438 & boost::serialization::make_nvp("PinholeBaseK",
│ │ │ │ -
439 boost::serialization::base_object<Base>(*this));
│ │ │ │ -
440 ar & BOOST_SERIALIZATION_NVP(K_);
│ │ │ │ -
441 }
│ │ │ │ +
433 private:
│ │ │ │ +
434 // Filters based on ValueType (if not Value) and also based on the user-
│ │ │ │ +
435 // supplied \c filter function.
│ │ │ │ +
436 template<class ValueType>
│ │ │ │ +
437 static bool filterHelper(const std::function<bool(Key)> filter, const ConstKeyValuePair& key_value) {
│ │ │ │ +
438 BOOST_STATIC_ASSERT((!boost::is_same<ValueType, Value>::value));
│ │ │ │ +
439 // Filter and check the type
│ │ │ │ +
440 return filter(key_value.key) && (dynamic_cast<const GenericValue<ValueType>*>(&key_value.value));
│ │ │ │ +
441 }
│ │ │ │
442
│ │ │ │ -
443public:
│ │ │ │ -
444 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
445};
│ │ │ │ -
│ │ │ │ -
446// end of class PinholePose
│ │ │ │ -
447
│ │ │ │ -
448template<typename CALIBRATION>
│ │ │ │ -
│ │ │ │ -
449struct traits<PinholePose<CALIBRATION> > : public internal::Manifold<
│ │ │ │ -
450 PinholePose<CALIBRATION> > {
│ │ │ │ -
451};
│ │ │ │ -
│ │ │ │ -
452
│ │ │ │ -
453template<typename CALIBRATION>
│ │ │ │ -
│ │ │ │ -
454struct traits<const PinholePose<CALIBRATION> > : public internal::Manifold<
│ │ │ │ -
455 PinholePose<CALIBRATION> > {
│ │ │ │ -
456};
│ │ │ │ -
│ │ │ │ -
457
│ │ │ │ -
458} // \ gtsam
│ │ │ │ -
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ -
CalibratedCamera.h
Calibrated camera for which only pose is unknown.
│ │ │ │ -
Point2.h
2D Point
│ │ │ │ +
444 friend class boost::serialization::access;
│ │ │ │ +
445 template<class ARCHIVE>
│ │ │ │ +
446 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
447 ar & BOOST_SERIALIZATION_NVP(values_);
│ │ │ │ +
448 }
│ │ │ │ +
449
│ │ │ │ +
450 };
│ │ │ │ +
│ │ │ │ +
451
│ │ │ │ +
452 /* ************************************************************************* */
│ │ │ │ +
│ │ │ │ +
453 class ValuesKeyAlreadyExists : public std::exception {
│ │ │ │ +
454 protected:
│ │ │ │ +
455 const Key key_;
│ │ │ │ +
456
│ │ │ │ +
457 private:
│ │ │ │ +
458 mutable std::string message_;
│ │ │ │ +
459
│ │ │ │ +
460 public:
│ │ │ │ +
│ │ │ │ +
462 ValuesKeyAlreadyExists(Key key) noexcept :
│ │ │ │ +
463 key_(key) {}
│ │ │ │ +
│ │ │ │ +
464
│ │ │ │ +
465 ~ValuesKeyAlreadyExists() noexcept override {}
│ │ │ │ +
466
│ │ │ │ +
468 Key key() const noexcept { return key_; }
│ │ │ │ +
469
│ │ │ │ +
471 GTSAM_EXPORT const char* what() const noexcept override;
│ │ │ │ +
472 };
│ │ │ │ +
│ │ │ │ +
473
│ │ │ │ +
474 /* ************************************************************************* */
│ │ │ │ +
│ │ │ │ +
475 class ValuesKeyDoesNotExist : public std::exception {
│ │ │ │ +
476 protected:
│ │ │ │ +
477 const char* operation_;
│ │ │ │ +
478 const Key key_;
│ │ │ │ +
479
│ │ │ │ +
480 private:
│ │ │ │ +
481 mutable std::string message_;
│ │ │ │ +
482
│ │ │ │ +
483 public:
│ │ │ │ +
│ │ │ │ +
485 ValuesKeyDoesNotExist(const char* operation, Key key) noexcept :
│ │ │ │ +
486 operation_(operation), key_(key) {}
│ │ │ │ +
│ │ │ │ +
487
│ │ │ │ +
488 ~ValuesKeyDoesNotExist() noexcept override {}
│ │ │ │ +
489
│ │ │ │ +
491 Key key() const noexcept { return key_; }
│ │ │ │ +
492
│ │ │ │ +
494 GTSAM_EXPORT const char* what() const noexcept override;
│ │ │ │ +
495 };
│ │ │ │ +
│ │ │ │ +
496
│ │ │ │ +
497 /* ************************************************************************* */
│ │ │ │ +
│ │ │ │ +
498 class ValuesIncorrectType : public std::exception {
│ │ │ │ +
499 protected:
│ │ │ │ +
500 const Key key_;
│ │ │ │ +
501 const std::type_info& storedTypeId_;
│ │ │ │ +
502 const std::type_info& requestedTypeId_;
│ │ │ │ +
503
│ │ │ │ +
504 private:
│ │ │ │ +
505 mutable std::string message_;
│ │ │ │ +
506
│ │ │ │ +
507 public:
│ │ │ │ +
│ │ │ │ +
509 ValuesIncorrectType(Key key,
│ │ │ │ +
510 const std::type_info& storedTypeId, const std::type_info& requestedTypeId) noexcept :
│ │ │ │ +
511 key_(key), storedTypeId_(storedTypeId), requestedTypeId_(requestedTypeId) {}
│ │ │ │ +
│ │ │ │ +
512
│ │ │ │ +
513 ~ValuesIncorrectType() noexcept override {}
│ │ │ │ +
514
│ │ │ │ +
516 Key key() const noexcept { return key_; }
│ │ │ │ +
517
│ │ │ │ +
519 const std::type_info& storedTypeId() const { return storedTypeId_; }
│ │ │ │ +
520
│ │ │ │ +
522 const std::type_info& requestedTypeId() const { return requestedTypeId_; }
│ │ │ │ +
523
│ │ │ │ +
525 GTSAM_EXPORT const char* what() const noexcept override;
│ │ │ │ +
526 };
│ │ │ │ +
│ │ │ │ +
527
│ │ │ │ +
528 /* ************************************************************************* */
│ │ │ │ +
│ │ │ │ +
529 class DynamicValuesMismatched : public std::exception {
│ │ │ │ +
530
│ │ │ │ +
531 public:
│ │ │ │ +
532 DynamicValuesMismatched() noexcept {}
│ │ │ │ +
533
│ │ │ │ +
534 ~DynamicValuesMismatched() noexcept override {}
│ │ │ │ +
535
│ │ │ │ +
536 const char* what() const noexcept override {
│ │ │ │ +
537 return "The Values 'this' and the argument passed to Values::localCoordinates have mismatched keys and values";
│ │ │ │ +
538 }
│ │ │ │ +
539 };
│ │ │ │ +
│ │ │ │ +
540
│ │ │ │ +
541 /* ************************************************************************* */
│ │ │ │ +
│ │ │ │ +
542 class NoMatchFoundForFixed: public std::exception {
│ │ │ │ +
543
│ │ │ │ +
544 protected:
│ │ │ │ +
545 const size_t M1_, N1_;
│ │ │ │ +
546 const size_t M2_, N2_;
│ │ │ │ +
547
│ │ │ │ +
548 private:
│ │ │ │ +
549 mutable std::string message_;
│ │ │ │ +
550
│ │ │ │ +
551 public:
│ │ │ │ +
552 NoMatchFoundForFixed(size_t M1, size_t N1, size_t M2, size_t N2) noexcept :
│ │ │ │ +
553 M1_(M1), N1_(N1), M2_(M2), N2_(N2) {
│ │ │ │ +
554 }
│ │ │ │ +
555
│ │ │ │ +
556 ~NoMatchFoundForFixed() noexcept override {
│ │ │ │ +
557 }
│ │ │ │ +
558
│ │ │ │ +
559 GTSAM_EXPORT const char* what() const noexcept override;
│ │ │ │ +
560 };
│ │ │ │ +
│ │ │ │ +
561
│ │ │ │ +
562 /* ************************************************************************* */
│ │ │ │ +
564 template<>
│ │ │ │ +
│ │ │ │ +
565 struct traits<Values> : public Testable<Values> {
│ │ │ │ +
566 };
│ │ │ │ +
│ │ │ │ +
567
│ │ │ │ +
568} //\ namespace gtsam
│ │ │ │ +
569
│ │ │ │ +
570
│ │ │ │ +
571#include <gtsam/nonlinear/Values-inl.h>
│ │ │ │ +
FastDefaultAllocator.h
An easy way to control which allocator is used for Fast* collections.
│ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │ -
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
│ │ │ │ +
gtsam::operator!=
bool operator!=(const Matrix &A, const Matrix &B)
inequality
Definition Matrix.h:107
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::operator==
bool operator==(const Matrix &A, const Matrix &B)
equality is just equal_with_abs_tol 1e-9
Definition Matrix.h:100
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::LieGroup::localCoordinates
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g
Definition Lie.h:136
│ │ │ │ -
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
│ │ │ │ -
gtsam::FixedDimension
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition Manifold.h:164
│ │ │ │ -
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
gtsam::PinholeBase
A pinhole camera class that has a Pose3, functions as base class for all pinhole cameras.
Definition CalibratedCamera.h:52
│ │ │ │ -
gtsam::PinholeBase::Dpose
static Matrix26 Dpose(const Point2 &pn, double d)
Calculate Jacobian with respect to pose.
Definition CalibratedCamera.cpp:27
│ │ │ │ -
gtsam::PinholeBase::print
virtual void print(const std::string &s="PinholeBase") const
print
Definition CalibratedCamera.cpp:74
│ │ │ │ -
gtsam::PinholeBase::projectSafe
std::pair< Point2, bool > projectSafe(const Point3 &pw) const
Project a point into the image and check depth.
Definition CalibratedCamera.cpp:109
│ │ │ │ -
gtsam::PinholeBase::pose
const Pose3 & pose() const
return pose, constant version
Definition CalibratedCamera.h:152
│ │ │ │ -
gtsam::PinholeBase::LevelPose
static Pose3 LevelPose(const Pose2 &pose2, double height)
Create a level pose at the given 2D pose and height.
Definition CalibratedCamera.cpp:49
│ │ │ │ -
gtsam::PinholeBase::equals
bool equals(const PinholeBase &camera, double tol=1e-9) const
assert equality up to a tolerance
Definition CalibratedCamera.cpp:69
│ │ │ │ -
gtsam::PinholeBase::Dpoint
static Matrix23 Dpoint(const Point2 &pn, double d, const Matrix3 &Rt)
Calculate Jacobian with respect to point.
Definition CalibratedCamera.cpp:37
│ │ │ │ -
gtsam::PinholeBase::BackprojectFromCamera
static Point3 BackprojectFromCamera(const Point2 &p, const double depth, OptionalJacobian< 3, 2 > Dpoint=boost::none, OptionalJacobian< 3, 1 > Ddepth=boost::none)
backproject a 2-dimensional point to a 3-dimensional point at given depth
Definition CalibratedCamera.cpp:167
│ │ │ │ -
gtsam::PinholeBase::LookatPose
static Pose3 LookatPose(const Point3 &eye, const Point3 &target, const Point3 &upVector)
Create a camera pose at the given eye position looking at a target point in the scene with the specif...
Definition CalibratedCamera.cpp:58
│ │ │ │ -
gtsam::PinholeBase::project2
Point2 project2(const Point3 &point, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none) const
Project point into the image Throws a CheiralityException if point behind image plane iff GTSAM_THROW...
Definition CalibratedCamera.cpp:116
│ │ │ │ -
gtsam::CalibratedCamera
A Calibrated camera class [R|-R't], calibration K=I.
Definition CalibratedCamera.h:247
│ │ │ │ -
gtsam::PinholeBaseK
A pinhole camera class that has a Pose3 and a fixed Calibration.
Definition PinholePose.h:34
│ │ │ │ -
gtsam::PinholeBaseK::range
double range(const CalibratedCamera &camera, OptionalJacobian< 1, 6 > Dcamera=boost::none, OptionalJacobian< 1, 6 > Dother=boost::none) const
Calculate range to a CalibratedCamera.
Definition PinholePose.h:202
│ │ │ │ -
gtsam::PinholeBaseK::project
Point2 project(const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none, OptionalJacobian< 2, DimK > Dcal=boost::none) const
project a point at infinity from world coordinates into the image
Definition PinholePose.h:132
│ │ │ │ -
gtsam::PinholeBaseK::project
Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > Dcal=boost::none) const
project a 3D point from world coordinates into the image
Definition PinholePose.h:118
│ │ │ │ -
gtsam::PinholeBaseK::projectSafe
std::pair< Point2, bool > projectSafe(const Point3 &pw) const
Project a point into the image and check depth.
Definition PinholePose.h:82
│ │ │ │ -
gtsam::PinholeBaseK::PinholeBaseK
PinholeBaseK()
default constructor
Definition PinholePose.h:51
│ │ │ │ -
gtsam::PinholeBaseK::range
double range(const Point3 &point, OptionalJacobian< 1, 6 > Dcamera=boost::none, OptionalJacobian< 1, 3 > Dpoint=boost::none) const
Calculate range to a landmark.
Definition PinholePose.h:181
│ │ │ │ -
gtsam::PinholeBaseK::calibration
virtual const CALIBRATION & calibration() const =0
return calibration
│ │ │ │ -
gtsam::PinholeBaseK::reprojectionError
Point2 reprojectionError(const Point3 &pw, const Point2 &measured, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > Dcal=boost::none) const
project a 3D point from world coordinates into the image
Definition PinholePose.h:125
│ │ │ │ -
gtsam::PinholeBaseK::PinholeBaseK
PinholeBaseK(const Pose3 &pose)
constructor with pose
Definition PinholePose.h:55
│ │ │ │ -
gtsam::PinholeBaseK::backproject
Point3 backproject(const Point2 &p, double depth, OptionalJacobian< 3, 6 > Dresult_dpose=boost::none, OptionalJacobian< 3, 2 > Dresult_dp=boost::none, OptionalJacobian< 3, 1 > Dresult_ddepth=boost::none, OptionalJacobian< 3, DimK > Dresult_dcal=boost::none) const
backproject a 2-dimensional point to a 3-dimensional point at given depth
Definition PinholePose.h:139
│ │ │ │ -
gtsam::PinholeBaseK::range
double range(const PinholeBaseK< CalibrationB > &camera, OptionalJacobian< 1, 6 > Dcamera=boost::none, OptionalJacobian< 1, 6 > Dother=boost::none) const
Calculate range to a PinholePoseK derived class.
Definition PinholePose.h:213
│ │ │ │ -
gtsam::PinholeBaseK::backprojectPointAtInfinity
Unit3 backprojectPointAtInfinity(const Point2 &p) const
backproject a 2-dimensional point to a 3-dimensional point at infinity
Definition PinholePose.h:170
│ │ │ │ -
gtsam::PinholeBaseK::access
friend class boost::serialization::access
Serialization function.
Definition PinholePose.h:222
│ │ │ │ -
gtsam::PinholeBaseK::_project
Point2 _project(const POINT &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, FixedDimension< POINT >::value > Dpoint, OptionalJacobian< 2, DimK > Dcal) const
Templated projection of a point (possibly at infinity) from world coordinate to the image.
Definition PinholePose.h:96
│ │ │ │ -
gtsam::PinholeBaseK::range
double range(const Pose3 &pose, OptionalJacobian< 1, 6 > Dcamera=boost::none, OptionalJacobian< 1, 6 > Dpose=boost::none) const
Calculate range to another pose.
Definition PinholePose.h:192
│ │ │ │ -
gtsam::PinholePose
A pinhole camera class that has a Pose3 and a fixed Calibration.
Definition PinholePose.h:243
│ │ │ │ -
gtsam::PinholePose::Lookat
static PinholePose Lookat(const Point3 &eye, const Point3 &target, const Point3 &upVector, const boost::shared_ptr< CALIBRATION > &K=boost::make_shared< CALIBRATION >())
Create a camera at the given eye position looking at a target point in the scene with the specified u...
Definition PinholePose.h:303
│ │ │ │ -
gtsam::PinholePose::PinholePose
PinholePose()
default constructor
Definition PinholePose.h:260
│ │ │ │ -
gtsam::PinholePose::cameraProjectionMatrix
Matrix34 cameraProjectionMatrix() const
for Linear Triangulation
Definition PinholePose.h:420
│ │ │ │ -
gtsam::PinholePose::project2
Point2 project2(const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const
project2 version for point at infinity
Definition PinholePose.h:385
│ │ │ │ -
gtsam::PinholePose::calibration
const CALIBRATION & calibration() const override
return calibration
Definition PinholePose.h:370
│ │ │ │ -
gtsam::PinholePose::defaultErrorWhenTriangulatingBehindCamera
Vector defaultErrorWhenTriangulatingBehindCamera() const
for Nonlinear Triangulation
Definition PinholePose.h:426
│ │ │ │ -
gtsam::PinholePose::operator<<
friend std::ostream & operator<<(std::ostream &os, const PinholePose &camera)
stream operator
Definition PinholePose.h:339
│ │ │ │ -
gtsam::PinholePose::Identity
static PinholePose Identity()
for Canonical
Definition PinholePose.h:415
│ │ │ │ -
gtsam::PinholePose::PinholePose
PinholePose(const Vector &v, const Vector &K)
Init from Vector and calibration.
Definition PinholePose.h:319
│ │ │ │ -
gtsam::PinholePose::sharedCalibration
const boost::shared_ptr< CALIBRATION > & sharedCalibration() const
return shared pointer to calibration
Definition PinholePose.h:365
│ │ │ │ -
gtsam::PinholePose::dim
size_t dim() const
Definition PinholePose.h:395
│ │ │ │ -
gtsam::PinholePose::project2
Point2 project2(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none) const
project a point from world coordinate to the image, 2 derivatives only
Definition PinholePose.h:379
│ │ │ │ -
gtsam::PinholePose::retract
PinholePose retract(const Vector6 &d) const
move a cameras according to d
Definition PinholePose.h:405
│ │ │ │ -
gtsam::PinholePose::PinholePose
PinholePose(const Vector &v)
Init from 6D vector.
Definition PinholePose.h:314
│ │ │ │ -
gtsam::PinholePose::PinholePose
PinholePose(const Pose3 &pose, const boost::shared_ptr< CALIBRATION > &K)
constructor with pose and calibration
Definition PinholePose.h:269
│ │ │ │ -
gtsam::PinholePose::Dim
static size_t Dim()
Definition PinholePose.h:400
│ │ │ │ -
gtsam::PinholePose::access
friend class boost::serialization::access
Serialization function.
Definition PinholePose.h:434
│ │ │ │ -
gtsam::PinholePose::equals
bool equals(const Base &camera, double tol=1e-9) const
assert equality up to a tolerance
Definition PinholePose.h:333
│ │ │ │ -
gtsam::PinholePose::print
void print(const std::string &s="PinholePose") const override
print
Definition PinholePose.h:349
│ │ │ │ -
gtsam::PinholePose::Level
static PinholePose Level(const boost::shared_ptr< CALIBRATION > &K, const Pose2 &pose2, double height)
Create a level camera at the given 2D pose and height.
Definition PinholePose.h:284
│ │ │ │ -
gtsam::PinholePose::PinholePose
PinholePose(const Pose3 &pose)
constructor with pose, uses default calibration
Definition PinholePose.h:264
│ │ │ │ -
gtsam::PinholePose::Level
static PinholePose Level(const Pose2 &pose2, double height)
PinholePose::level with default calibration.
Definition PinholePose.h:290
│ │ │ │ -
gtsam::PinholePose::localCoordinates
Vector6 localCoordinates(const PinholePose &p) const
return canonical coordinate
Definition PinholePose.h:410
│ │ │ │ -
gtsam::Pose2
A 2D pose (Point2,Rot2)
Definition Pose2.h:36
│ │ │ │ -
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ -
gtsam::Pose3::transformFrom
Point3 transformFrom(const Point3 &point, OptionalJacobian< 3, 6 > Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const
takes point in Pose coordinates and transforms it to world coordinates
Definition Pose3.cpp:347
│ │ │ │ -
gtsam::Pose3::range
double range(const Point3 &point, OptionalJacobian< 1, 6 > Hself=boost::none, OptionalJacobian< 1, 3 > Hpoint=boost::none) const
Calculate range to a landmark.
Definition Pose3.cpp:399
│ │ │ │ -
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get translation
Definition Pose3.cpp:308
│ │ │ │ -
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get rotation
Definition Pose3.cpp:315
│ │ │ │ -
gtsam::Rot3::rotate
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
rotate point from rotated coordinate frame to world
Definition Rot3M.cpp:149
│ │ │ │ -
gtsam::Rot3::rpy
Vector3 rpy(OptionalJacobian< 3, 3 > H=boost::none) const
Use RQ to calculate roll-pitch-yaw angle representation.
Definition Rot3.cpp:192
│ │ │ │ -
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
│ │ │ │ +
gtsam::internal::FastDefaultAllocator
Default allocator for list, map, and set types.
Definition FastDefaultAllocator.h:50
│ │ │ │ + │ │ │ │ +
gtsam::GenericValue
Wraps any type T so it can play as a Value.
Definition GenericValue.h:47
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::Value
This is the base class for any type to be stored in Values.
Definition Value.h:37
│ │ │ │ +
gtsam::Value::deallocate_
virtual void deallocate_() const =0
Deallocate a raw pointer of this value.
│ │ │ │ +
gtsam::Value::clone_
virtual Value * clone_() const =0
Clone this value in a special memory pool, must be deleted with Value::deallocate_,...
│ │ │ │ +
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
gtsam::ValueCloneAllocator
Definition Values.h:49
│ │ │ │ +
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
gtsam::Values::update
void update(Key j, const T &val)
Templated version to update a variable with the given j, throws KeyDoesNotExist<J> if j is not presen...
│ │ │ │ +
gtsam::Values::empty
bool empty() const
whether the config is empty
Definition Values.h:184
│ │ │ │ +
gtsam::Values::upper_bound
deref_iterator upper_bound(Key j) const
Find the lowest-ordered element greater than the specified key.
Definition Values.h:219
│ │ │ │ +
gtsam::Values::shared_ptr
boost::shared_ptr< Values > shared_ptr
A shared_ptr to this class.
Definition Values.h:87
│ │ │ │ +
gtsam::Values::clear
void clear()
Remove all variables from the config.
Definition Values.h:301
│ │ │ │ +
gtsam::Values::swap
void swap(Values &other)
Swap the contents of two Values without copying data.
Definition Values.h:298
│ │ │ │ +
gtsam::Values::insertDouble
void insertDouble(Key j, double c)
version for double
Definition Values.h:252
│ │ │ │ +
gtsam::Values::size
size_t size() const
The number of variables in this config.
Definition Values.h:181
│ │ │ │ +
gtsam::Values::find
deref_iterator find(Key j) const
Find an element by key, returning an iterator, or end() if the key was not found.
Definition Values.h:213
│ │ │ │ +
gtsam::Values::const_shared_ptr
boost::shared_ptr< const Values > const_shared_ptr
A const shared_ptr to this class.
Definition Values.h:90
│ │ │ │ +
gtsam::Values::Values
Values()=default
Default constructor creates an empty Values class.
│ │ │ │ +
gtsam::Values::lower_bound
deref_iterator lower_bound(Key j) const
Find the element greater than or equal to the specified key.
Definition Values.h:216
│ │ │ │ +
gtsam::Values::atDouble
double atDouble(size_t key) const
version for double
Definition Values.h:159
│ │ │ │ +
gtsam::Values::KeyValuePair
A key-value pair, which you get by dereferencing iterators.
Definition Values.h:93
│ │ │ │ +
gtsam::Values::KeyValuePair::value
Value & value
The value.
Definition Values.h:95
│ │ │ │ +
gtsam::Values::KeyValuePair::key
const Key key
The key.
Definition Values.h:94
│ │ │ │ +
gtsam::Values::ConstKeyValuePair
A key-value pair, which you get by dereferencing iterators.
Definition Values.h:101
│ │ │ │ +
gtsam::Values::ConstKeyValuePair::key
const Key key
The key.
Definition Values.h:102
│ │ │ │ +
gtsam::Values::ConstKeyValuePair::value
const Value & value
The value.
Definition Values.h:103
│ │ │ │ +
gtsam::Values::deref_iterator
Definition Values.h:190
│ │ │ │ +
gtsam::ValuesKeyAlreadyExists
Definition Values.h:453
│ │ │ │ +
gtsam::ValuesKeyAlreadyExists::key_
const Key key_
The key that already existed.
Definition Values.h:455
│ │ │ │ +
gtsam::ValuesKeyAlreadyExists::what
GTSAM_EXPORT const char * what() const noexcept override
The message to be displayed to the user.
Definition Values.cpp:280
│ │ │ │ +
gtsam::ValuesKeyAlreadyExists::ValuesKeyAlreadyExists
ValuesKeyAlreadyExists(Key key) noexcept
Construct with the key-value pair attempted to be added.
Definition Values.h:462
│ │ │ │ +
gtsam::ValuesKeyAlreadyExists::key
Key key() const noexcept
The duplicate key that was attempted to be added.
Definition Values.h:468
│ │ │ │ +
gtsam::ValuesKeyDoesNotExist
Definition Values.h:475
│ │ │ │ +
gtsam::ValuesKeyDoesNotExist::ValuesKeyDoesNotExist
ValuesKeyDoesNotExist(const char *operation, Key key) noexcept
Construct with the key that does not exist in the values.
Definition Values.h:485
│ │ │ │ +
gtsam::ValuesKeyDoesNotExist::key_
const Key key_
The key that does not exist.
Definition Values.h:478
│ │ │ │ +
gtsam::ValuesKeyDoesNotExist::key
Key key() const noexcept
The key that was attempted to be accessed that does not exist.
Definition Values.h:491
│ │ │ │ +
gtsam::ValuesKeyDoesNotExist::operation_
const char * operation_
The operation that attempted to access the key.
Definition Values.h:477
│ │ │ │ +
gtsam::ValuesIncorrectType
Definition Values.h:498
│ │ │ │ +
gtsam::ValuesIncorrectType::storedTypeId
const std::type_info & storedTypeId() const
The typeid of the value stores in the Values.
Definition Values.h:519
│ │ │ │ +
gtsam::ValuesIncorrectType::key
Key key() const noexcept
The key that was attempted to be accessed that does not exist.
Definition Values.h:516
│ │ │ │ +
gtsam::ValuesIncorrectType::key_
const Key key_
The key requested.
Definition Values.h:500
│ │ │ │ +
gtsam::ValuesIncorrectType::ValuesIncorrectType
ValuesIncorrectType(Key key, const std::type_info &storedTypeId, const std::type_info &requestedTypeId) noexcept
Construct with the key that does not exist in the values.
Definition Values.h:509
│ │ │ │ +
gtsam::ValuesIncorrectType::requestedTypeId
const std::type_info & requestedTypeId() const
The requested typeid.
Definition Values.h:522
│ │ │ │ +
gtsam::DynamicValuesMismatched
Definition Values.h:529
│ │ │ │ +
gtsam::NoMatchFoundForFixed
Definition Values.h:542
│ │ │ │ +
VectorValues
The Factor::error simply extracts the.
│ │ │ │ +
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,662 +1,668 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -PinholePose.h │ │ │ │ │ +Values.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ -24#include │ │ │ │ │ -25 │ │ │ │ │ -26namespace _g_t_s_a_m { │ │ │ │ │ -27 │ │ │ │ │ -33template │ │ │ │ │ -_3_4class _P_i_n_h_o_l_e_B_a_s_e_K: public _P_i_n_h_o_l_e_B_a_s_e { │ │ │ │ │ -35 │ │ │ │ │ -36private: │ │ │ │ │ +25#pragma once │ │ │ │ │ +26 │ │ │ │ │ +27#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_._h> │ │ │ │ │ +28#include │ │ │ │ │ +29#include │ │ │ │ │ +30#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ +31#include │ │ │ │ │ +32#include │ │ │ │ │ +33#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ +34#include │ │ │ │ │ +35#include │ │ │ │ │ +36#endif │ │ │ │ │ 37 │ │ │ │ │ -38 GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) │ │ │ │ │ -39 │ │ │ │ │ -40 // Get dimensions of calibration type at compile time │ │ │ │ │ -41 static const int DimK = _F_i_x_e_d_D_i_m_e_n_s_i_o_n_<_C_A_L_I_B_R_A_T_I_O_N_>_:_:_v_a_l_u_e; │ │ │ │ │ +38#include │ │ │ │ │ +39#include │ │ │ │ │ +40 │ │ │ │ │ +41namespace _g_t_s_a_m { │ │ │ │ │ 42 │ │ │ │ │ -43public: │ │ │ │ │ -44 │ │ │ │ │ -45 typedef CALIBRATION CalibrationType; │ │ │ │ │ -46 │ │ │ │ │ -49 │ │ │ │ │ -_5_1 _P_i_n_h_o_l_e_B_a_s_e_K() { │ │ │ │ │ -52 } │ │ │ │ │ -53 │ │ │ │ │ -_5_5 explicit _P_i_n_h_o_l_e_B_a_s_e_K(const _P_o_s_e_3& _p_o_s_e) : │ │ │ │ │ -56 _P_i_n_h_o_l_e_B_a_s_e(_p_o_s_e) { │ │ │ │ │ -57 } │ │ │ │ │ -58 │ │ │ │ │ -62 │ │ │ │ │ -63 explicit _P_i_n_h_o_l_e_B_a_s_e_K(const Vector &v) : │ │ │ │ │ -64 _P_i_n_h_o_l_e_B_a_s_e(v) { │ │ │ │ │ -65 } │ │ │ │ │ +43 // Forward declarations / utilities │ │ │ │ │ +44 class _V_e_c_t_o_r_V_a_l_u_e_s; │ │ │ │ │ +45 class ValueAutomaticCasting; │ │ │ │ │ +46 template static bool _truePredicate(const T&) { return true; } │ │ │ │ │ +47 │ │ │ │ │ +48 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +_4_9 class GTSAM_EXPORT _V_a_l_u_e_C_l_o_n_e_A_l_l_o_c_a_t_o_r { │ │ │ │ │ +50 public: │ │ │ │ │ +51 static _V_a_l_u_e* allocate_clone(const _V_a_l_u_e& a) { return a._c_l_o_n_e__(); } │ │ │ │ │ +52 static void deallocate_clone(const _V_a_l_u_e* a) { a->_d_e_a_l_l_o_c_a_t_e__(); } │ │ │ │ │ +53 _V_a_l_u_e_C_l_o_n_e_A_l_l_o_c_a_t_o_r() {} │ │ │ │ │ +54 }; │ │ │ │ │ +55 │ │ │ │ │ +_6_5 class GTSAM_EXPORT _V_a_l_u_e_s { │ │ │ │ │ 66 │ │ │ │ │ -70 │ │ │ │ │ -71 virtual ~PinholeBaseK() { │ │ │ │ │ -72 } │ │ │ │ │ -73 │ │ │ │ │ -_7_5 virtual const CALIBRATION& _c_a_l_i_b_r_a_t_i_o_n() const = 0; │ │ │ │ │ -76 │ │ │ │ │ +67 private: │ │ │ │ │ +68 // Internally we store a boost ptr_map, with a ValueCloneAllocator (defined │ │ │ │ │ +69 // below) to clone and deallocate the Value objects, and our compile-flag- │ │ │ │ │ +70 // dependent FastDefaultAllocator to allocate map nodes. In this way, the │ │ │ │ │ +71 // user defines the allocation details (i.e. optimize for memory pool/arenas │ │ │ │ │ +72 // concurrency). │ │ │ │ │ +73 typedef _i_n_t_e_r_n_a_l_:_:_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_<_t_y_p_e_n_a_m_e_ _s_t_d_:_:_p_a_i_r_<_c_o_n_s_t_ _K_e_y_, │ │ │ │ │ +_v_o_i_d_*_>>::type KeyValuePtrPairAllocator; │ │ │ │ │ +74 typedef boost::ptr_map< │ │ │ │ │ +75 _K_e_y, │ │ │ │ │ +76 _V_a_l_u_e, │ │ │ │ │ +77 std::less, │ │ │ │ │ +78 _V_a_l_u_e_C_l_o_n_e_A_l_l_o_c_a_t_o_r, │ │ │ │ │ +79 KeyValuePtrPairAllocator > KeyValueMap; │ │ │ │ │ 80 │ │ │ │ │ -_8_2 std::pair _p_r_o_j_e_c_t_S_a_f_e(const _P_o_i_n_t_3& pw) const { │ │ │ │ │ -83 std::pair pn = _P_i_n_h_o_l_e_B_a_s_e_:_:_p_r_o_j_e_c_t_S_a_f_e(pw); │ │ │ │ │ -84 pn.first = _c_a_l_i_b_r_a_t_i_o_n().uncalibrate(pn.first); │ │ │ │ │ -85 return pn; │ │ │ │ │ -86 } │ │ │ │ │ -87 │ │ │ │ │ +81 // The member to store the values, see just above │ │ │ │ │ +82 KeyValueMap values_; │ │ │ │ │ +83 │ │ │ │ │ +84 public: │ │ │ │ │ +85 │ │ │ │ │ +_8_7 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ 88 │ │ │ │ │ -95 template │ │ │ │ │ -_9_6 _P_o_i_n_t_2 ___p_r_o_j_e_c_t(const POINT& pw, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> _D_p_o_s_e, │ │ │ │ │ -97 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n<2, _F_i_x_e_d_D_i_m_e_n_s_i_o_n_<_P_O_I_N_T_>_:_:_v_a_l_u_e> _D_p_o_i_n_t, │ │ │ │ │ -98 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _D_i_m_K_> Dcal) const { │ │ │ │ │ +_9_0 typedef boost::shared_ptr _c_o_n_s_t___s_h_a_r_e_d___p_t_r; │ │ │ │ │ +91 │ │ │ │ │ +_9_3 struct GTSAM_EXPORT _K_e_y_V_a_l_u_e_P_a_i_r { │ │ │ │ │ +_9_4 const _K_e_y _k_e_y; │ │ │ │ │ +_9_5 _V_a_l_u_e& _v_a_l_u_e; │ │ │ │ │ +96 │ │ │ │ │ +97 _K_e_y_V_a_l_u_e_P_a_i_r(_K_e_y _key, _V_a_l_u_e& _value) : key(_key), value(_value) {} │ │ │ │ │ +98 }; │ │ │ │ │ 99 │ │ │ │ │ -100 // project to normalized coordinates │ │ │ │ │ -101 const _P_o_i_n_t_2 pn = _P_i_n_h_o_l_e_B_a_s_e_:_:_p_r_o_j_e_c_t_2(pw, _D_p_o_s_e, _D_p_o_i_n_t); │ │ │ │ │ -102 │ │ │ │ │ -103 // uncalibrate to pixel coordinates │ │ │ │ │ -104 Matrix2 Dpi_pn; │ │ │ │ │ -105 const _P_o_i_n_t_2 pi = _c_a_l_i_b_r_a_t_i_o_n().uncalibrate(pn, Dcal, │ │ │ │ │ -106 _D_p_o_s_e || _D_p_o_i_n_t ? &Dpi_pn : 0); │ │ │ │ │ -107 │ │ │ │ │ -108 // If needed, apply chain rule │ │ │ │ │ -109 if (_D_p_o_s_e) │ │ │ │ │ -110 *_D_p_o_s_e = Dpi_pn * *_D_p_o_s_e; │ │ │ │ │ -111 if (_D_p_o_i_n_t) │ │ │ │ │ -112 *_D_p_o_i_n_t = Dpi_pn * *_D_p_o_i_n_t; │ │ │ │ │ +_1_0_1 struct GTSAM_EXPORT _C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r { │ │ │ │ │ +_1_0_2 const _K_e_y _k_e_y; │ │ │ │ │ +_1_0_3 const _V_a_l_u_e& _v_a_l_u_e; │ │ │ │ │ +104 │ │ │ │ │ +105 _C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r(_K_e_y _key, const _V_a_l_u_e& _value) : key(_key), value(_value) │ │ │ │ │ +{} │ │ │ │ │ +106 _C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r(const _K_e_y_V_a_l_u_e_P_a_i_r& kv) : key(kv.key), value(kv.value) {} │ │ │ │ │ +107 }; │ │ │ │ │ +108 │ │ │ │ │ +109 typedef KeyValuePair value_type; │ │ │ │ │ +110 │ │ │ │ │ 113 │ │ │ │ │ -114 return pi; │ │ │ │ │ -115 } │ │ │ │ │ +_1_1_5 _V_a_l_u_e_s() = default; │ │ │ │ │ 116 │ │ │ │ │ -_1_1_8 _P_o_i_n_t_2 _p_r_o_j_e_c_t(const _P_o_i_n_t_3& pw, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> _D_p_o_s_e = boost:: │ │ │ │ │ -none, │ │ │ │ │ -119 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> _D_p_o_i_n_t = boost::none, │ │ │ │ │ -120 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _D_i_m_K_> Dcal = boost::none) const { │ │ │ │ │ -121 return ___p_r_o_j_e_c_t(pw, _D_p_o_s_e, _D_p_o_i_n_t, Dcal); │ │ │ │ │ -122 } │ │ │ │ │ -123 │ │ │ │ │ -_1_2_5 _P_o_i_n_t_2 _r_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r(const _P_o_i_n_t_3& pw, const _P_o_i_n_t_2& measured, │ │ │ │ │ -_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> _D_p_o_s_e = boost::none, │ │ │ │ │ -126 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> _D_p_o_i_n_t = boost::none, │ │ │ │ │ -127 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _D_i_m_K_> Dcal = boost::none) const { │ │ │ │ │ -128 return _P_o_i_n_t_2(___p_r_o_j_e_c_t(pw, _D_p_o_s_e, _D_p_o_i_n_t, Dcal) - measured); │ │ │ │ │ -129 } │ │ │ │ │ -130 │ │ │ │ │ -_1_3_2 _P_o_i_n_t_2 _p_r_o_j_e_c_t(const _U_n_i_t_3& pw, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> _D_p_o_s_e = boost::none, │ │ │ │ │ -133 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> _D_p_o_i_n_t = boost::none, │ │ │ │ │ -134 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _D_i_m_K_> Dcal = boost::none) const { │ │ │ │ │ -135 return ___p_r_o_j_e_c_t(pw, _D_p_o_s_e, _D_p_o_i_n_t, Dcal); │ │ │ │ │ -136 } │ │ │ │ │ -137 │ │ │ │ │ -_1_3_9 _P_o_i_n_t_3 _b_a_c_k_p_r_o_j_e_c_t(const _P_o_i_n_t_2& p, double depth, │ │ │ │ │ -140 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _6_> Dresult_dpose = boost::none, │ │ │ │ │ -141 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _2_> Dresult_dp = boost::none, │ │ │ │ │ -142 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _1_> Dresult_ddepth = boost::none, │ │ │ │ │ -143 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _D_i_m_K_> Dresult_dcal = boost::none) const { │ │ │ │ │ -144 typedef Eigen::Matrix Matrix2K; │ │ │ │ │ -145 Matrix2K Dpn_dcal; │ │ │ │ │ -146 Matrix22 Dpn_dp; │ │ │ │ │ -147 const _P_o_i_n_t_2 pn = _c_a_l_i_b_r_a_t_i_o_n().calibrate(p, Dresult_dcal ? &Dpn_dcal : 0, │ │ │ │ │ -148 Dresult_dp ? &Dpn_dp : 0); │ │ │ │ │ -149 Matrix32 Dpoint_dpn; │ │ │ │ │ -150 Matrix31 Dpoint_ddepth; │ │ │ │ │ -151 const _P_o_i_n_t_3 point = _B_a_c_k_p_r_o_j_e_c_t_F_r_o_m_C_a_m_e_r_a(pn, depth, │ │ │ │ │ -152 (Dresult_dp || Dresult_dcal) ? &Dpoint_dpn : 0, │ │ │ │ │ -153 Dresult_ddepth ? &Dpoint_ddepth : 0); │ │ │ │ │ -154 Matrix33 Dresult_dpoint; │ │ │ │ │ -155 const _P_o_i_n_t_3 result = _p_o_s_e()._t_r_a_n_s_f_o_r_m_F_r_o_m(point, Dresult_dpose, │ │ │ │ │ -156 (Dresult_ddepth || │ │ │ │ │ -157 Dresult_dp || │ │ │ │ │ -158 Dresult_dcal) ? &Dresult_dpoint : 0); │ │ │ │ │ -159 if (Dresult_dcal) │ │ │ │ │ -160 *Dresult_dcal = Dresult_dpoint * Dpoint_dpn * Dpn_dcal; // (3x3)*(3x2)* │ │ │ │ │ -(2xDimK) │ │ │ │ │ -161 if (Dresult_dp) │ │ │ │ │ -162 *Dresult_dp = Dresult_dpoint * Dpoint_dpn * Dpn_dp; // (3x3)*(3x2)*(2x2) │ │ │ │ │ -163 if (Dresult_ddepth) │ │ │ │ │ -164 *Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth; // (3x3)*(3x1) │ │ │ │ │ -165 │ │ │ │ │ -166 return result; │ │ │ │ │ -167 } │ │ │ │ │ -168 │ │ │ │ │ -_1_7_0 _U_n_i_t_3 _b_a_c_k_p_r_o_j_e_c_t_P_o_i_n_t_A_t_I_n_f_i_n_i_t_y(const _P_o_i_n_t_2& p) const { │ │ │ │ │ -171 const _P_o_i_n_t_2 pn = _c_a_l_i_b_r_a_t_i_o_n().calibrate(p); │ │ │ │ │ -172 const _U_n_i_t_3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1 │ │ │ │ │ -173 return _p_o_s_e()._r_o_t_a_t_i_o_n()._r_o_t_a_t_e(pc); │ │ │ │ │ -174 } │ │ │ │ │ -175 │ │ │ │ │ -_1_8_1 double _r_a_n_g_e(const _P_o_i_n_t_3& point, │ │ │ │ │ -182 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> Dcamera = boost::none, │ │ │ │ │ -183 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _3_> _D_p_o_i_n_t = boost::none) const { │ │ │ │ │ -184 return _p_o_s_e()._r_a_n_g_e(point, Dcamera, _D_p_o_i_n_t); │ │ │ │ │ -185 } │ │ │ │ │ -186 │ │ │ │ │ -_1_9_2 double _r_a_n_g_e(const _P_o_s_e_3& _p_o_s_e, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> Dcamera = boost:: │ │ │ │ │ -none, │ │ │ │ │ -193 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> _D_p_o_s_e = boost::none) const { │ │ │ │ │ -194 return this->_p_o_s_e()._r_a_n_g_e(pose, Dcamera, _D_p_o_s_e); │ │ │ │ │ -195 } │ │ │ │ │ -196 │ │ │ │ │ -_2_0_2 double _r_a_n_g_e(const _C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a& camera, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> Dcamera │ │ │ │ │ -= │ │ │ │ │ -203 boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> Dother = boost::none) const { │ │ │ │ │ -204 return _p_o_s_e()._r_a_n_g_e(camera._p_o_s_e(), Dcamera, Dother); │ │ │ │ │ +118 _V_a_l_u_e_s(const _V_a_l_u_e_s& other); │ │ │ │ │ +119 │ │ │ │ │ +121 _V_a_l_u_e_s(_V_a_l_u_e_s&& other); │ │ │ │ │ +122 │ │ │ │ │ +128 _V_a_l_u_e_s(std::initializer_list init); │ │ │ │ │ +129 │ │ │ │ │ +131 _V_a_l_u_e_s(const _V_a_l_u_e_s& other, const _V_e_c_t_o_r_V_a_l_u_e_s& delta); │ │ │ │ │ +132 │ │ │ │ │ +136 │ │ │ │ │ +138 void _p_r_i_n_t(const std::string& str = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ +DefaultKeyFormatter) const; │ │ │ │ │ +139 │ │ │ │ │ +141 bool _e_q_u_a_l_s(const _V_a_l_u_e_s& other, double tol=1e-9) const; │ │ │ │ │ +142 │ │ │ │ │ +146 │ │ │ │ │ +155 template │ │ │ │ │ +156 const ValueType at(_K_e_y j) const; │ │ │ │ │ +157 │ │ │ │ │ +_1_5_9 double _a_t_D_o_u_b_l_e(size_t key) const { return at(key);} │ │ │ │ │ +160 │ │ │ │ │ +166 const _V_a_l_u_e& at(_K_e_y j) const; │ │ │ │ │ +167 │ │ │ │ │ +171 bool exists(_K_e_y j) const; │ │ │ │ │ +172 │ │ │ │ │ +177 template │ │ │ │ │ +178 boost::optional exists(_K_e_y j) const; │ │ │ │ │ +179 │ │ │ │ │ +_1_8_1 size_t _s_i_z_e() const { return values_.size(); } │ │ │ │ │ +182 │ │ │ │ │ +_1_8_4 bool _e_m_p_t_y() const { return values_.empty(); } │ │ │ │ │ +185 │ │ │ │ │ +189 │ │ │ │ │ +_1_9_0 struct _d_e_r_e_f___i_t_e_r_a_t_o_r { │ │ │ │ │ +191 using const_iterator_type = typename KeyValueMap::const_iterator; │ │ │ │ │ +192 const_iterator_type it_; │ │ │ │ │ +193 _d_e_r_e_f___i_t_e_r_a_t_o_r(const_iterator_type it) : it_(it) {} │ │ │ │ │ +194 _C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r _o_p_e_r_a_t_o_r_*() const { return {it_->first, *(it_->second)}; │ │ │ │ │ +} │ │ │ │ │ +195 boost::shared_ptr operator->() { │ │ │ │ │ +196 return boost::make_shared(it_->first, *(it_->second)); │ │ │ │ │ +197 } │ │ │ │ │ +198 bool _o_p_e_r_a_t_o_r_=_=(const _d_e_r_e_f___i_t_e_r_a_t_o_r& other) const { │ │ │ │ │ +199 return it_ == other.it_; │ │ │ │ │ +200 } │ │ │ │ │ +201 bool _o_p_e_r_a_t_o_r_!_=(const _d_e_r_e_f___i_t_e_r_a_t_o_r& other) const { return it_ != │ │ │ │ │ +other.it_; } │ │ │ │ │ +202 _d_e_r_e_f___i_t_e_r_a_t_o_r& operator++() { │ │ │ │ │ +203 ++it_; │ │ │ │ │ +204 return *this; │ │ │ │ │ 205 } │ │ │ │ │ -206 │ │ │ │ │ -212 template │ │ │ │ │ -_2_1_3 double _r_a_n_g_e(const _P_i_n_h_o_l_e_B_a_s_e_K_<_C_a_l_i_b_r_a_t_i_o_n_B_>& camera, │ │ │ │ │ -214 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> Dcamera = boost::none, │ │ │ │ │ -215 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_1_,_ _6_> Dother = boost::none) const { │ │ │ │ │ -216 return _p_o_s_e()._r_a_n_g_e(camera._p_o_s_e(), Dcamera, Dother); │ │ │ │ │ -217 } │ │ │ │ │ -218 │ │ │ │ │ -219private: │ │ │ │ │ +206 }; │ │ │ │ │ +207 │ │ │ │ │ +208 _d_e_r_e_f___i_t_e_r_a_t_o_r begin() const { return _d_e_r_e_f___i_t_e_r_a_t_o_r(values_.begin()); } │ │ │ │ │ +209 deref_iterator end() const { return deref_iterator(values_.end()); } │ │ │ │ │ +210 │ │ │ │ │ +_2_1_3 _d_e_r_e_f___i_t_e_r_a_t_o_r _f_i_n_d(_K_e_y j) const { return _d_e_r_e_f___i_t_e_r_a_t_o_r(values_.find(j)); │ │ │ │ │ +} │ │ │ │ │ +214 │ │ │ │ │ +_2_1_6 _d_e_r_e_f___i_t_e_r_a_t_o_r _l_o_w_e_r___b_o_u_n_d(_K_e_y j) const { return _d_e_r_e_f___i_t_e_r_a_t_o_r │ │ │ │ │ +(values_.lower_bound(j)); } │ │ │ │ │ +217 │ │ │ │ │ +_2_1_9 _d_e_r_e_f___i_t_e_r_a_t_o_r _u_p_p_e_r___b_o_u_n_d(_K_e_y j) const { return _d_e_r_e_f___i_t_e_r_a_t_o_r │ │ │ │ │ +(values_.upper_bound(j)); } │ │ │ │ │ 220 │ │ │ │ │ -_2_2_2 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -223 template │ │ │ │ │ -224 void serialize(Archive & ar, const unsigned int /*version*/) { │ │ │ │ │ -225 ar │ │ │ │ │ -226 & boost::serialization::make_nvp("PinholeBase", │ │ │ │ │ -227 boost::serialization::base_object(*this)); │ │ │ │ │ -228 } │ │ │ │ │ -229 │ │ │ │ │ -230public: │ │ │ │ │ -231 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -232}; │ │ │ │ │ -233// end of class PinholeBaseK │ │ │ │ │ -234 │ │ │ │ │ -242template │ │ │ │ │ -_2_4_3class _P_i_n_h_o_l_e_P_o_s_e: public _P_i_n_h_o_l_e_B_a_s_e_K { │ │ │ │ │ +224 │ │ │ │ │ +226 _V_a_l_u_e_s retract(const _V_e_c_t_o_r_V_a_l_u_e_s& delta) const; │ │ │ │ │ +227 │ │ │ │ │ +232 void retractMasked(const _V_e_c_t_o_r_V_a_l_u_e_s& delta, const _K_e_y_S_e_t& mask); │ │ │ │ │ +233 │ │ │ │ │ +235 _V_e_c_t_o_r_V_a_l_u_e_s localCoordinates(const _V_a_l_u_e_s& cp) const; │ │ │ │ │ +236 │ │ │ │ │ +238 │ │ │ │ │ +240 void insert(_K_e_y j, const _V_a_l_u_e& val); │ │ │ │ │ +241 │ │ │ │ │ +243 void insert(const _V_a_l_u_e_s& values); │ │ │ │ │ 244 │ │ │ │ │ -245private: │ │ │ │ │ -246 │ │ │ │ │ -247 typedef _P_i_n_h_o_l_e_B_a_s_e_K_<_C_A_L_I_B_R_A_T_I_O_N_> _B_a_s_e; │ │ │ │ │ -248 boost::shared_ptr K_; │ │ │ │ │ -249 │ │ │ │ │ -250public: │ │ │ │ │ -251 │ │ │ │ │ -252 enum { │ │ │ │ │ -253 dimension = 6 │ │ │ │ │ -254 }; │ │ │ │ │ -255 │ │ │ │ │ -258 │ │ │ │ │ -_2_6_0 _P_i_n_h_o_l_e_P_o_s_e() { │ │ │ │ │ -261 } │ │ │ │ │ -262 │ │ │ │ │ -_2_6_4 explicit _P_i_n_h_o_l_e_P_o_s_e(const _P_o_s_e_3& _p_o_s_e) : │ │ │ │ │ -265 _B_a_s_e(_p_o_s_e), K_(new CALIBRATION()) { │ │ │ │ │ -266 } │ │ │ │ │ -267 │ │ │ │ │ -_2_6_9 _P_i_n_h_o_l_e_P_o_s_e(const _P_o_s_e_3& _p_o_s_e, const boost::shared_ptr& K) : │ │ │ │ │ -270 _B_a_s_e(_p_o_s_e), K_(K) { │ │ │ │ │ -271 } │ │ │ │ │ -272 │ │ │ │ │ -276 │ │ │ │ │ -_2_8_4 static _P_i_n_h_o_l_e_P_o_s_e _L_e_v_e_l(const boost::shared_ptr& K, │ │ │ │ │ -285 const _P_o_s_e_2& pose2, double height) { │ │ │ │ │ -286 return _P_i_n_h_o_l_e_P_o_s_e(_B_a_s_e_:_:_L_e_v_e_l_P_o_s_e(pose2, height), K); │ │ │ │ │ -287 } │ │ │ │ │ +248 template │ │ │ │ │ +249 void insert(_K_e_y j, const ValueType& val); │ │ │ │ │ +250 │ │ │ │ │ +_2_5_2 void _i_n_s_e_r_t_D_o_u_b_l_e(_K_e_y j, double c) { insert(j,c); } │ │ │ │ │ +253 │ │ │ │ │ +255 void update(_K_e_y j, const _V_a_l_u_e& val); │ │ │ │ │ +256 │ │ │ │ │ +261 template │ │ │ │ │ +_2_6_2 void _u_p_d_a_t_e(_K_e_y j, const T& val); │ │ │ │ │ +263 │ │ │ │ │ +265 void update(const _V_a_l_u_e_s& values); │ │ │ │ │ +266 │ │ │ │ │ +268 void insert_or_assign(_K_e_y j, const _V_a_l_u_e& val); │ │ │ │ │ +269 │ │ │ │ │ +274 void insert_or_assign(const _V_a_l_u_e_s& values); │ │ │ │ │ +275 │ │ │ │ │ +277 template │ │ │ │ │ +278 void insert_or_assign(_K_e_y j, const ValueType& val); │ │ │ │ │ +279 │ │ │ │ │ +281 void erase(_K_e_y j); │ │ │ │ │ +282 │ │ │ │ │ +287 _K_e_y_V_e_c_t_o_r keys() const; │ │ │ │ │ 288 │ │ │ │ │ -_2_9_0 static _P_i_n_h_o_l_e_P_o_s_e _L_e_v_e_l(const _P_o_s_e_2& pose2, double height) { │ │ │ │ │ -291 return _P_i_n_h_o_l_e_P_o_s_e_:_:_L_e_v_e_l(boost::make_shared(), pose2, │ │ │ │ │ -height); │ │ │ │ │ -292 } │ │ │ │ │ +292 _K_e_y_S_e_t keySet() const; │ │ │ │ │ 293 │ │ │ │ │ -_3_0_3 static _P_i_n_h_o_l_e_P_o_s_e _L_o_o_k_a_t(const _P_o_i_n_t_3& eye, const _P_o_i_n_t_3& target, │ │ │ │ │ -304 const _P_o_i_n_t_3& upVector, const boost::shared_ptr& K = │ │ │ │ │ -305 boost::make_shared()) { │ │ │ │ │ -306 return _P_i_n_h_o_l_e_P_o_s_e(_B_a_s_e_:_:_L_o_o_k_a_t_P_o_s_e(eye, target, upVector), K); │ │ │ │ │ -307 } │ │ │ │ │ +295 _V_a_l_u_e_s& operator=(const _V_a_l_u_e_s& rhs); │ │ │ │ │ +296 │ │ │ │ │ +_2_9_8 void _s_w_a_p(_V_a_l_u_e_s& other) { values_.swap(other.values_); } │ │ │ │ │ +299 │ │ │ │ │ +_3_0_1 void _c_l_e_a_r() { values_.clear(); } │ │ │ │ │ +302 │ │ │ │ │ +304 size_t dim() const; │ │ │ │ │ +305 │ │ │ │ │ +307 std::map dims() const; │ │ │ │ │ 308 │ │ │ │ │ -312 │ │ │ │ │ -_3_1_4 explicit _P_i_n_h_o_l_e_P_o_s_e(const Vector &v) : │ │ │ │ │ -315 _B_a_s_e(v), K_(new CALIBRATION()) { │ │ │ │ │ -316 } │ │ │ │ │ -317 │ │ │ │ │ -_3_1_9 _P_i_n_h_o_l_e_P_o_s_e(const Vector &v, const Vector &K) : │ │ │ │ │ -320 _B_a_s_e(v), K_(new CALIBRATION(K)) { │ │ │ │ │ +310 _V_e_c_t_o_r_V_a_l_u_e_s zeroVectors() const; │ │ │ │ │ +311 │ │ │ │ │ +312 // Count values of given type \c ValueType │ │ │ │ │ +313 template │ │ │ │ │ +314 size_t count() const { │ │ │ │ │ +315 size_t i = 0; │ │ │ │ │ +316 for (const auto key_value : values_) { │ │ │ │ │ +317 if (dynamic_cast*>(key_value.second)) │ │ │ │ │ +318 ++i; │ │ │ │ │ +319 } │ │ │ │ │ +320 return i; │ │ │ │ │ 321 } │ │ │ │ │ 322 │ │ │ │ │ -323 // Init from Pose3 and calibration │ │ │ │ │ -324 _P_i_n_h_o_l_e_P_o_s_e(const _P_o_s_e_3 &_p_o_s_e, const Vector &K) : │ │ │ │ │ -325 Base(_p_o_s_e), K_(new CALIBRATION(K)) { │ │ │ │ │ -326 } │ │ │ │ │ -327 │ │ │ │ │ -331 │ │ │ │ │ -_3_3_3 bool _e_q_u_a_l_s(const _B_a_s_e &camera, double tol = 1e-9) const { │ │ │ │ │ -334 const _P_i_n_h_o_l_e_P_o_s_e* e = dynamic_cast(&camera); │ │ │ │ │ -335 return _B_a_s_e_:_:_e_q_u_a_l_s(camera, tol) && K_->_e_q_u_a_l_s(e->_c_a_l_i_b_r_a_t_i_o_n(), tol); │ │ │ │ │ -336 } │ │ │ │ │ -337 │ │ │ │ │ -_3_3_9 friend std::ostream& _o_p_e_r_a_t_o_r_<_<(std::ostream &os, const _P_i_n_h_o_l_e_P_o_s_e& │ │ │ │ │ -camera) { │ │ │ │ │ -340 os << "{R: " << camera._p_o_s_e()._r_o_t_a_t_i_o_n()._r_p_y().transpose(); │ │ │ │ │ -341 os << ", t: " << camera._p_o_s_e()._t_r_a_n_s_l_a_t_i_o_n().transpose(); │ │ │ │ │ -342 if (!camera.K_) os << ", K: none"; │ │ │ │ │ -343 else os << ", K: " << *camera.K_; │ │ │ │ │ -344 os << "}"; │ │ │ │ │ -345 return os; │ │ │ │ │ -346 } │ │ │ │ │ -347 │ │ │ │ │ -_3_4_9 void _p_r_i_n_t(const std::string& s = "PinholePose") const override { │ │ │ │ │ -350 _B_a_s_e_:_:_p_r_i_n_t(s); │ │ │ │ │ -351 if (!K_) │ │ │ │ │ -352 std::cout << "s No calibration given" << std::endl; │ │ │ │ │ -353 else │ │ │ │ │ -354 K_->print(s + ".calibration"); │ │ │ │ │ -355 } │ │ │ │ │ -356 │ │ │ │ │ -360 │ │ │ │ │ -361 _~_P_i_n_h_o_l_e_P_o_s_e() override { │ │ │ │ │ -362 } │ │ │ │ │ -363 │ │ │ │ │ -_3_6_5 const boost::shared_ptr& _s_h_a_r_e_d_C_a_l_i_b_r_a_t_i_o_n() const { │ │ │ │ │ -366 return K_; │ │ │ │ │ -367 } │ │ │ │ │ -368 │ │ │ │ │ -_3_7_0 const CALIBRATION& _c_a_l_i_b_r_a_t_i_o_n() const override { │ │ │ │ │ -371 return *K_; │ │ │ │ │ -372 } │ │ │ │ │ -373 │ │ │ │ │ -_3_7_9 _P_o_i_n_t_2 _p_r_o_j_e_c_t_2(const _P_o_i_n_t_3& pw, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> _D_p_o_s_e = boost:: │ │ │ │ │ -none, │ │ │ │ │ -380 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _3_> _D_p_o_i_n_t = boost::none) const { │ │ │ │ │ -381 return _B_a_s_e_:_:_p_r_o_j_e_c_t(pw, _D_p_o_s_e, _D_p_o_i_n_t); │ │ │ │ │ -382 } │ │ │ │ │ -383 │ │ │ │ │ -_3_8_5 _P_o_i_n_t_2 _p_r_o_j_e_c_t_2(const _U_n_i_t_3& pw, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> _D_p_o_s_e = boost:: │ │ │ │ │ -none, │ │ │ │ │ -386 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> _D_p_o_i_n_t = boost::none) const { │ │ │ │ │ -387 return _B_a_s_e_:_:_p_r_o_j_e_c_t(pw, _D_p_o_s_e, _D_p_o_i_n_t); │ │ │ │ │ -388 } │ │ │ │ │ -389 │ │ │ │ │ -393 │ │ │ │ │ -_3_9_5 size_t _d_i_m() const { │ │ │ │ │ -396 return 6; │ │ │ │ │ -397 } │ │ │ │ │ -398 │ │ │ │ │ -_4_0_0 static size_t _D_i_m() { │ │ │ │ │ -401 return 6; │ │ │ │ │ -402 } │ │ │ │ │ -403 │ │ │ │ │ -_4_0_5 _P_i_n_h_o_l_e_P_o_s_e _r_e_t_r_a_c_t(const Vector6& d) const { │ │ │ │ │ -406 return _P_i_n_h_o_l_e_P_o_s_e(_B_a_s_e_:_:_p_o_s_e()._r_e_t_r_a_c_t(d), K_); │ │ │ │ │ -407 } │ │ │ │ │ -408 │ │ │ │ │ -_4_1_0 Vector6 _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(const _P_i_n_h_o_l_e_P_o_s_e& p) const { │ │ │ │ │ -411 return _B_a_s_e_:_:_p_o_s_e()._l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(p.Base::pose()); │ │ │ │ │ -412 } │ │ │ │ │ +342 template │ │ │ │ │ +343 std::map // , std::less, Eigen:: │ │ │ │ │ +aligned_allocator │ │ │ │ │ +344 extract(const std::function& filterFcn = &_truePredicate) │ │ │ │ │ +const; │ │ │ │ │ +345 │ │ │ │ │ +346#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ +347 // Types obtained by iterating │ │ │ │ │ +348 typedef KeyValueMap::const_iterator::value_type ConstKeyValuePtrPair; │ │ │ │ │ +349 typedef KeyValueMap::iterator::value_type KeyValuePtrPair; │ │ │ │ │ +350 │ │ │ │ │ +352 typedef boost::transform_iterator< │ │ │ │ │ +353 std::function, KeyValueMap::iterator> │ │ │ │ │ +iterator; │ │ │ │ │ +354 │ │ │ │ │ +356 typedef boost::transform_iterator< │ │ │ │ │ +357 std::function, │ │ │ │ │ +KeyValueMap::const_iterator> const_iterator; │ │ │ │ │ +358 │ │ │ │ │ +360 typedef boost::transform_iterator< │ │ │ │ │ +361 std::function, KeyValueMap:: │ │ │ │ │ +reverse_iterator> reverse_iterator; │ │ │ │ │ +362 │ │ │ │ │ +364 typedef boost::transform_iterator< │ │ │ │ │ +365 std::function, │ │ │ │ │ +KeyValueMap::const_reverse_iterator> const_reverse_iterator; │ │ │ │ │ +366 │ │ │ │ │ +371 std::pair tryInsert(Key j, const Value& value); │ │ │ │ │ +372 │ │ │ │ │ +373 static ConstKeyValuePair make_const_deref_pair(const KeyValueMap:: │ │ │ │ │ +const_iterator::value_type& key_value) { │ │ │ │ │ +374 return ConstKeyValuePair(key_value.first, *key_value.second); } │ │ │ │ │ +375 │ │ │ │ │ +376 static KeyValuePair make_deref_pair(const KeyValueMap::iterator:: │ │ │ │ │ +value_type& key_value) { │ │ │ │ │ +377 return KeyValuePair(key_value.first, *key_value.second); } │ │ │ │ │ +378 │ │ │ │ │ +379 const_iterator _begin() const { return boost::make_transform_iterator │ │ │ │ │ +(values_.begin(), &make_const_deref_pair); } │ │ │ │ │ +380 const_iterator _end() const { return boost::make_transform_iterator │ │ │ │ │ +(values_.end(), &make_const_deref_pair); } │ │ │ │ │ +381 iterator begin() { return boost::make_transform_iterator(values_.begin(), │ │ │ │ │ +&make_deref_pair); } │ │ │ │ │ +382 iterator end() { return boost::make_transform_iterator(values_.end(), │ │ │ │ │ +&make_deref_pair); } │ │ │ │ │ +383 const_reverse_iterator rbegin() const { return boost:: │ │ │ │ │ +make_transform_iterator(values_.rbegin(), &make_const_deref_pair); } │ │ │ │ │ +384 const_reverse_iterator rend() const { return boost::make_transform_iterator │ │ │ │ │ +(values_.rend(), &make_const_deref_pair); } │ │ │ │ │ +385 reverse_iterator rbegin() { return boost::make_transform_iterator │ │ │ │ │ +(values_.rbegin(), &make_deref_pair); } │ │ │ │ │ +386 reverse_iterator rend() { return boost::make_transform_iterator │ │ │ │ │ +(values_.rend(), &make_deref_pair); } │ │ │ │ │ +387 │ │ │ │ │ +390 iterator find(Key j) { return boost::make_transform_iterator(values_.find │ │ │ │ │ +(j), &make_deref_pair); } │ │ │ │ │ +391 │ │ │ │ │ +393 iterator lower_bound(Key j) { return boost::make_transform_iterator │ │ │ │ │ +(values_.lower_bound(j), &make_deref_pair); } │ │ │ │ │ +394 │ │ │ │ │ +396 iterator upper_bound(Key j) { return boost::make_transform_iterator │ │ │ │ │ +(values_.upper_bound(j), &make_deref_pair); } │ │ │ │ │ +397 │ │ │ │ │ +399 template │ │ │ │ │ +400 class Filtered; │ │ │ │ │ +401 │ │ │ │ │ +403 template │ │ │ │ │ +404 class ConstFiltered; │ │ │ │ │ +405 │ │ │ │ │ +407 template │ │ │ │ │ +408 _V_a_l_u_e_s(const Filtered& view); │ │ │ │ │ +409 │ │ │ │ │ +411 template │ │ │ │ │ +412 _V_a_l_u_e_s(const ConstFiltered& view); │ │ │ │ │ 413 │ │ │ │ │ -_4_1_5 static _P_i_n_h_o_l_e_P_o_s_e _I_d_e_n_t_i_t_y() { │ │ │ │ │ -416 return _P_i_n_h_o_l_e_P_o_s_e(); // assumes that the default constructor is valid │ │ │ │ │ -417 } │ │ │ │ │ -418 │ │ │ │ │ -_4_2_0 Matrix34 _c_a_m_e_r_a_P_r_o_j_e_c_t_i_o_n_M_a_t_r_i_x() const { │ │ │ │ │ -421 Matrix34 P = Matrix34(_P_i_n_h_o_l_e_B_a_s_e_:_:_p_o_s_e().inverse().matrix().block(0, 0, 3, │ │ │ │ │ -4)); │ │ │ │ │ -422 return K_->K() * P; │ │ │ │ │ -423 } │ │ │ │ │ -424 │ │ │ │ │ -_4_2_6 Vector _d_e_f_a_u_l_t_E_r_r_o_r_W_h_e_n_T_r_i_a_n_g_u_l_a_t_i_n_g_B_e_h_i_n_d_C_a_m_e_r_a() const { │ │ │ │ │ -427 return Eigen::Matrix::dimension,1>::Constant(2.0 * │ │ │ │ │ -K_->fx());; │ │ │ │ │ -428 } │ │ │ │ │ -430 │ │ │ │ │ -431private: │ │ │ │ │ +415 Filtered GTSAM_DEPRECATED │ │ │ │ │ +416 filter(const std::function& filterFcn); │ │ │ │ │ +417 │ │ │ │ │ +419 template │ │ │ │ │ +420 Filtered GTSAM_DEPRECATED │ │ │ │ │ +421 filter(const std::function& filterFcn = &_truePredicate); │ │ │ │ │ +422 │ │ │ │ │ +424 ConstFiltered GTSAM_DEPRECATED │ │ │ │ │ +425 filter(const std::function& filterFcn) const; │ │ │ │ │ +426 │ │ │ │ │ +428 template │ │ │ │ │ +429 ConstFiltered GTSAM_DEPRECATED filter( │ │ │ │ │ +430 const std::function& filterFcn = &_truePredicate) const; │ │ │ │ │ +431#endif │ │ │ │ │ 432 │ │ │ │ │ -_4_3_4 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -435 template │ │ │ │ │ -436 void serialize(Archive & ar, const unsigned int /*version*/) { │ │ │ │ │ -437 ar │ │ │ │ │ -438 & boost::serialization::make_nvp("PinholeBaseK", │ │ │ │ │ -439 boost::serialization::base_object(*this)); │ │ │ │ │ -440 ar & BOOST_SERIALIZATION_NVP(K_); │ │ │ │ │ +433 private: │ │ │ │ │ +434 // Filters based on ValueType (if not Value) and also based on the user- │ │ │ │ │ +435 // supplied \c filter function. │ │ │ │ │ +436 template │ │ │ │ │ +437 static bool filterHelper(const std::function filter, const │ │ │ │ │ +ConstKeyValuePair& key_value) { │ │ │ │ │ +438 BOOST_STATIC_ASSERT((!boost::is_same::value)); │ │ │ │ │ +439 // Filter and check the type │ │ │ │ │ +440 return filter(key_value.key) && (dynamic_cast*>(&key_value.value)); │ │ │ │ │ 441 } │ │ │ │ │ 442 │ │ │ │ │ -443public: │ │ │ │ │ -444 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -445}; │ │ │ │ │ -446// end of class PinholePose │ │ │ │ │ -447 │ │ │ │ │ -448template │ │ │ │ │ -_4_4_9struct _t_r_a_i_t_s<_P_i_n_h_o_l_e_P_o_s_e > : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d< │ │ │ │ │ -450 PinholePose > { │ │ │ │ │ -451}; │ │ │ │ │ -452 │ │ │ │ │ -453template │ │ │ │ │ -_4_5_4struct _t_r_a_i_t_s > : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d< │ │ │ │ │ -455 PinholePose > { │ │ │ │ │ -456}; │ │ │ │ │ -457 │ │ │ │ │ -458} // \ gtsam │ │ │ │ │ -_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ -This marks a GTSAM object to require alignment. │ │ │ │ │ -DDeeffiinniittiioonn types.h:308 │ │ │ │ │ -_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a_._h │ │ │ │ │ -Calibrated camera for which only pose is unknown. │ │ │ │ │ -_P_o_i_n_t_2_._h │ │ │ │ │ -2D Point │ │ │ │ │ +_4_4_4 friend class boost::serialization::access; │ │ │ │ │ +445 template │ │ │ │ │ +446 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +447 ar & BOOST_SERIALIZATION_NVP(values_); │ │ │ │ │ +448 } │ │ │ │ │ +449 │ │ │ │ │ +450 }; │ │ │ │ │ +451 │ │ │ │ │ +452 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +_4_5_3 class _V_a_l_u_e_s_K_e_y_A_l_r_e_a_d_y_E_x_i_s_t_s : public std::exception { │ │ │ │ │ +454 protected: │ │ │ │ │ +_4_5_5 const _K_e_y _k_e_y__; │ │ │ │ │ +456 │ │ │ │ │ +457 private: │ │ │ │ │ +458 mutable std::string message_; │ │ │ │ │ +459 │ │ │ │ │ +460 public: │ │ │ │ │ +_4_6_2 _V_a_l_u_e_s_K_e_y_A_l_r_e_a_d_y_E_x_i_s_t_s(_K_e_y _k_e_y) noexcept : │ │ │ │ │ +463 _k_e_y__(_k_e_y) {} │ │ │ │ │ +464 │ │ │ │ │ +465 _~_V_a_l_u_e_s_K_e_y_A_l_r_e_a_d_y_E_x_i_s_t_s() noexcept override {} │ │ │ │ │ +466 │ │ │ │ │ +_4_6_8 _K_e_y _k_e_y() const noexcept { return _k_e_y__; } │ │ │ │ │ +469 │ │ │ │ │ +471 GTSAM_EXPORT const char* _w_h_a_t() const noexcept override; │ │ │ │ │ +472 }; │ │ │ │ │ +473 │ │ │ │ │ +474 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +_4_7_5 class _V_a_l_u_e_s_K_e_y_D_o_e_s_N_o_t_E_x_i_s_t : public std::exception { │ │ │ │ │ +476 protected: │ │ │ │ │ +_4_7_7 const char* _o_p_e_r_a_t_i_o_n__; │ │ │ │ │ +_4_7_8 const _K_e_y _k_e_y__; │ │ │ │ │ +479 │ │ │ │ │ +480 private: │ │ │ │ │ +481 mutable std::string message_; │ │ │ │ │ +482 │ │ │ │ │ +483 public: │ │ │ │ │ +_4_8_5 _V_a_l_u_e_s_K_e_y_D_o_e_s_N_o_t_E_x_i_s_t(const char* operation, _K_e_y _k_e_y) noexcept : │ │ │ │ │ +486 operation_(operation), _k_e_y__(_k_e_y) {} │ │ │ │ │ +487 │ │ │ │ │ +488 _~_V_a_l_u_e_s_K_e_y_D_o_e_s_N_o_t_E_x_i_s_t() noexcept override {} │ │ │ │ │ +489 │ │ │ │ │ +_4_9_1 _K_e_y _k_e_y() const noexcept { return _k_e_y__; } │ │ │ │ │ +492 │ │ │ │ │ +494 GTSAM_EXPORT const char* _w_h_a_t() const noexcept override; │ │ │ │ │ +495 }; │ │ │ │ │ +496 │ │ │ │ │ +497 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +_4_9_8 class _V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e : public std::exception { │ │ │ │ │ +499 protected: │ │ │ │ │ +_5_0_0 const _K_e_y _k_e_y__; │ │ │ │ │ +501 const std::type_info& storedTypeId_; │ │ │ │ │ +502 const std::type_info& requestedTypeId_; │ │ │ │ │ +503 │ │ │ │ │ +504 private: │ │ │ │ │ +505 mutable std::string message_; │ │ │ │ │ +506 │ │ │ │ │ +507 public: │ │ │ │ │ +_5_0_9 _V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e(_K_e_y _k_e_y, │ │ │ │ │ +510 const std::type_info& storedTypeId, const std::type_info& requestedTypeId) │ │ │ │ │ +noexcept : │ │ │ │ │ +511 _k_e_y__(_k_e_y), storedTypeId_(storedTypeId), requestedTypeId_(requestedTypeId) │ │ │ │ │ +{} │ │ │ │ │ +512 │ │ │ │ │ +513 _~_V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e() noexcept override {} │ │ │ │ │ +514 │ │ │ │ │ +_5_1_6 _K_e_y _k_e_y() const noexcept { return _k_e_y__; } │ │ │ │ │ +517 │ │ │ │ │ +_5_1_9 const std::type_info& _s_t_o_r_e_d_T_y_p_e_I_d() const { return storedTypeId_; } │ │ │ │ │ +520 │ │ │ │ │ +_5_2_2 const std::type_info& _r_e_q_u_e_s_t_e_d_T_y_p_e_I_d() const { return requestedTypeId_; } │ │ │ │ │ +523 │ │ │ │ │ +525 GTSAM_EXPORT const char* _w_h_a_t() const noexcept override; │ │ │ │ │ +526 }; │ │ │ │ │ +527 │ │ │ │ │ +528 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +_5_2_9 class _D_y_n_a_m_i_c_V_a_l_u_e_s_M_i_s_m_a_t_c_h_e_d : public std::exception { │ │ │ │ │ +530 │ │ │ │ │ +531 public: │ │ │ │ │ +532 _D_y_n_a_m_i_c_V_a_l_u_e_s_M_i_s_m_a_t_c_h_e_d() noexcept {} │ │ │ │ │ +533 │ │ │ │ │ +534 _~_D_y_n_a_m_i_c_V_a_l_u_e_s_M_i_s_m_a_t_c_h_e_d() noexcept override {} │ │ │ │ │ +535 │ │ │ │ │ +536 const char* _w_h_a_t() const noexcept override { │ │ │ │ │ +537 return "The Values 'this' and the argument passed to Values:: │ │ │ │ │ +localCoordinates have mismatched keys and values"; │ │ │ │ │ +538 } │ │ │ │ │ +539 }; │ │ │ │ │ +540 │ │ │ │ │ +541 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +_5_4_2 class _N_o_M_a_t_c_h_F_o_u_n_d_F_o_r_F_i_x_e_d: public std::exception { │ │ │ │ │ +543 │ │ │ │ │ +544 protected: │ │ │ │ │ +545 const size_t M1_, N1_; │ │ │ │ │ +546 const size_t M2_, N2_; │ │ │ │ │ +547 │ │ │ │ │ +548 private: │ │ │ │ │ +549 mutable std::string message_; │ │ │ │ │ +550 │ │ │ │ │ +551 public: │ │ │ │ │ +552 _N_o_M_a_t_c_h_F_o_u_n_d_F_o_r_F_i_x_e_d(size_t M1, size_t N1, size_t M2, size_t N2) noexcept : │ │ │ │ │ +553 M1_(M1), N1_(N1), M2_(M2), N2_(N2) { │ │ │ │ │ +554 } │ │ │ │ │ +555 │ │ │ │ │ +556 _~_N_o_M_a_t_c_h_F_o_u_n_d_F_o_r_F_i_x_e_d() noexcept override { │ │ │ │ │ +557 } │ │ │ │ │ +558 │ │ │ │ │ +559 GTSAM_EXPORT const char* what() const noexcept override; │ │ │ │ │ +560 }; │ │ │ │ │ +561 │ │ │ │ │ +562 / │ │ │ │ │ +* ************************************************************************* */ │ │ │ │ │ +564 template<> │ │ │ │ │ +_5_6_5 struct _t_r_a_i_t_s<_V_a_l_u_e_s> : public _T_e_s_t_a_b_l_e { │ │ │ │ │ +566 }; │ │ │ │ │ +567 │ │ │ │ │ +568} //\ namespace gtsam │ │ │ │ │ +569 │ │ │ │ │ +570 │ │ │ │ │ +571#include │ │ │ │ │ +_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_._h │ │ │ │ │ +An easy way to control which allocator is used for Fast* collections. │ │ │ │ │ +_K_e_y_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ -Vector2 Point2 │ │ │ │ │ -As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point2 to Vector2... │ │ │ │ │ -DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ -Vector3 Point3 │ │ │ │ │ -As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point3 to Vector3... │ │ │ │ │ -DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ +Point2 operator*(double s, const Point2 &p) │ │ │ │ │ +multiply with scalar │ │ │ │ │ +DDeeffiinniittiioonn Point2.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_!_= │ │ │ │ │ +bool operator!=(const Matrix &A, const Matrix &B) │ │ │ │ │ +inequality │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:107 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_=_= │ │ │ │ │ +bool operator==(const Matrix &A, const Matrix &B) │ │ │ │ │ +equality is just equal_with_abs_tol 1e-9 │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ -TangentVector localCoordinates(const Class &g) const │ │ │ │ │ -localCoordinates as required by manifold concept: finds tangent vector between │ │ │ │ │ -*this and g │ │ │ │ │ -DDeeffiinniittiioonn Lie.h:136 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ -Both ManifoldTraits and Testable. │ │ │ │ │ -DDeeffiinniittiioonn Manifold.h:120 │ │ │ │ │ -_g_t_s_a_m_:_:_F_i_x_e_d_D_i_m_e_n_s_i_o_n │ │ │ │ │ -Give fixed size dimension of a type, fails at compile time if dynamic. │ │ │ │ │ -DDeeffiinniittiioonn Manifold.h:164 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ -either a fixed size o... │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e │ │ │ │ │ -A pinhole camera class that has a Pose3, functions as base class for all │ │ │ │ │ -pinhole cameras. │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_D_p_o_s_e │ │ │ │ │ -static Matrix26 Dpose(const Point2 &pn, double d) │ │ │ │ │ -Calculate Jacobian with respect to pose. │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.cpp:27 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_p_r_i_n_t │ │ │ │ │ -virtual void print(const std::string &s="PinholeBase") const │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.cpp:74 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_p_r_o_j_e_c_t_S_a_f_e │ │ │ │ │ -std::pair< Point2, bool > projectSafe(const Point3 &pw) const │ │ │ │ │ -Project a point into the image and check depth. │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.cpp:109 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_p_o_s_e │ │ │ │ │ -const Pose3 & pose() const │ │ │ │ │ -return pose, constant version │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:152 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_L_e_v_e_l_P_o_s_e │ │ │ │ │ -static Pose3 LevelPose(const Pose2 &pose2, double height) │ │ │ │ │ -Create a level pose at the given 2D pose and height. │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.cpp:49 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const PinholeBase &camera, double tol=1e-9) const │ │ │ │ │ -assert equality up to a tolerance │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.cpp:69 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_D_p_o_i_n_t │ │ │ │ │ -static Matrix23 Dpoint(const Point2 &pn, double d, const Matrix3 &Rt) │ │ │ │ │ -Calculate Jacobian with respect to point. │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.cpp:37 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_B_a_c_k_p_r_o_j_e_c_t_F_r_o_m_C_a_m_e_r_a │ │ │ │ │ -static Point3 BackprojectFromCamera(const Point2 &p, const double depth, │ │ │ │ │ -OptionalJacobian< 3, 2 > Dpoint=boost::none, OptionalJacobian< 3, 1 > │ │ │ │ │ -Ddepth=boost::none) │ │ │ │ │ -backproject a 2-dimensional point to a 3-dimensional point at given depth │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.cpp:167 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_L_o_o_k_a_t_P_o_s_e │ │ │ │ │ -static Pose3 LookatPose(const Point3 &eye, const Point3 &target, const Point3 │ │ │ │ │ -&upVector) │ │ │ │ │ -Create a camera pose at the given eye position looking at a target point in the │ │ │ │ │ -scene with the specif... │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.cpp:58 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_p_r_o_j_e_c_t_2 │ │ │ │ │ -Point2 project2(const Point3 &point, OptionalJacobian< 2, 6 > Dpose=boost:: │ │ │ │ │ -none, OptionalJacobian< 2, 3 > Dpoint=boost::none) const │ │ │ │ │ -Project point into the image Throws a CheiralityException if point behind image │ │ │ │ │ -plane iff GTSAM_THROW... │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.cpp:116 │ │ │ │ │ -_g_t_s_a_m_:_:_C_a_l_i_b_r_a_t_e_d_C_a_m_e_r_a │ │ │ │ │ -A Calibrated camera class [R|-R't], calibration K=I. │ │ │ │ │ -DDeeffiinniittiioonn CalibratedCamera.h:247 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K │ │ │ │ │ -A pinhole camera class that has a Pose3 and a fixed Calibration. │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_r_a_n_g_e │ │ │ │ │ -double range(const CalibratedCamera &camera, OptionalJacobian< 1, 6 > │ │ │ │ │ -Dcamera=boost::none, OptionalJacobian< 1, 6 > Dother=boost::none) const │ │ │ │ │ -Calculate range to a CalibratedCamera. │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:202 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_p_r_o_j_e_c_t │ │ │ │ │ -Point2 project(const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, │ │ │ │ │ -OptionalJacobian< 2, 2 > Dpoint=boost::none, OptionalJacobian< 2, DimK > │ │ │ │ │ -Dcal=boost::none) const │ │ │ │ │ -project a point at infinity from world coordinates into the image │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:132 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_p_r_o_j_e_c_t │ │ │ │ │ -Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, │ │ │ │ │ -OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > │ │ │ │ │ -Dcal=boost::none) const │ │ │ │ │ -project a 3D point from world coordinates into the image │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:118 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_p_r_o_j_e_c_t_S_a_f_e │ │ │ │ │ -std::pair< Point2, bool > projectSafe(const Point3 &pw) const │ │ │ │ │ -Project a point into the image and check depth. │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:82 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_P_i_n_h_o_l_e_B_a_s_e_K │ │ │ │ │ -PinholeBaseK() │ │ │ │ │ -default constructor │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:51 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_r_a_n_g_e │ │ │ │ │ -double range(const Point3 &point, OptionalJacobian< 1, 6 > Dcamera=boost::none, │ │ │ │ │ -OptionalJacobian< 1, 3 > Dpoint=boost::none) const │ │ │ │ │ -Calculate range to a landmark. │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:181 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_c_a_l_i_b_r_a_t_i_o_n │ │ │ │ │ -virtual const CALIBRATION & calibration() const =0 │ │ │ │ │ -return calibration │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_r_e_p_r_o_j_e_c_t_i_o_n_E_r_r_o_r │ │ │ │ │ -Point2 reprojectionError(const Point3 &pw, const Point2 &measured, │ │ │ │ │ -OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > │ │ │ │ │ -Dpoint=boost::none, OptionalJacobian< 2, DimK > Dcal=boost::none) const │ │ │ │ │ -project a 3D point from world coordinates into the image │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:125 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_P_i_n_h_o_l_e_B_a_s_e_K │ │ │ │ │ -PinholeBaseK(const Pose3 &pose) │ │ │ │ │ -constructor with pose │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_b_a_c_k_p_r_o_j_e_c_t │ │ │ │ │ -Point3 backproject(const Point2 &p, double depth, OptionalJacobian< 3, 6 > │ │ │ │ │ -Dresult_dpose=boost::none, OptionalJacobian< 3, 2 > Dresult_dp=boost::none, │ │ │ │ │ -OptionalJacobian< 3, 1 > Dresult_ddepth=boost::none, OptionalJacobian< 3, DimK │ │ │ │ │ -> Dresult_dcal=boost::none) const │ │ │ │ │ -backproject a 2-dimensional point to a 3-dimensional point at given depth │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:139 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_r_a_n_g_e │ │ │ │ │ -double range(const PinholeBaseK< CalibrationB > &camera, OptionalJacobian< 1, 6 │ │ │ │ │ -> Dcamera=boost::none, OptionalJacobian< 1, 6 > Dother=boost::none) const │ │ │ │ │ -Calculate range to a PinholePoseK derived class. │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:213 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_b_a_c_k_p_r_o_j_e_c_t_P_o_i_n_t_A_t_I_n_f_i_n_i_t_y │ │ │ │ │ -Unit3 backprojectPointAtInfinity(const Point2 &p) const │ │ │ │ │ -backproject a 2-dimensional point to a 3-dimensional point at infinity │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:170 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:222 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:___p_r_o_j_e_c_t │ │ │ │ │ -Point2 _project(const POINT &pw, OptionalJacobian< 2, 6 > Dpose, │ │ │ │ │ -OptionalJacobian< 2, FixedDimension< POINT >::value > Dpoint, OptionalJacobian< │ │ │ │ │ -2, DimK > Dcal) const │ │ │ │ │ -Templated projection of a point (possibly at infinity) from world coordinate to │ │ │ │ │ -the image. │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:96 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_r_a_n_g_e │ │ │ │ │ -double range(const Pose3 &pose, OptionalJacobian< 1, 6 > Dcamera=boost::none, │ │ │ │ │ -OptionalJacobian< 1, 6 > Dpose=boost::none) const │ │ │ │ │ -Calculate range to another pose. │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:192 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e │ │ │ │ │ -A pinhole camera class that has a Pose3 and a fixed Calibration. │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:243 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_L_o_o_k_a_t │ │ │ │ │ -static PinholePose Lookat(const Point3 &eye, const Point3 &target, const Point3 │ │ │ │ │ -&upVector, const boost::shared_ptr< CALIBRATION > &K=boost::make_shared< │ │ │ │ │ -CALIBRATION >()) │ │ │ │ │ -Create a camera at the given eye position looking at a target point in the │ │ │ │ │ -scene with the specified u... │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:303 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_P_i_n_h_o_l_e_P_o_s_e │ │ │ │ │ -PinholePose() │ │ │ │ │ -default constructor │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:260 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_c_a_m_e_r_a_P_r_o_j_e_c_t_i_o_n_M_a_t_r_i_x │ │ │ │ │ -Matrix34 cameraProjectionMatrix() const │ │ │ │ │ -for Linear Triangulation │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:420 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_p_r_o_j_e_c_t_2 │ │ │ │ │ -Point2 project2(const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, │ │ │ │ │ -OptionalJacobian< 2, 2 > Dpoint=boost::none) const │ │ │ │ │ -project2 version for point at infinity │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:385 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_c_a_l_i_b_r_a_t_i_o_n │ │ │ │ │ -const CALIBRATION & calibration() const override │ │ │ │ │ -return calibration │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:370 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_d_e_f_a_u_l_t_E_r_r_o_r_W_h_e_n_T_r_i_a_n_g_u_l_a_t_i_n_g_B_e_h_i_n_d_C_a_m_e_r_a │ │ │ │ │ -Vector defaultErrorWhenTriangulatingBehindCamera() const │ │ │ │ │ -for Nonlinear Triangulation │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:426 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_o_p_e_r_a_t_o_r_<_< │ │ │ │ │ -friend std::ostream & operator<<(std::ostream &os, const PinholePose &camera) │ │ │ │ │ -stream operator │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:339 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_I_d_e_n_t_i_t_y │ │ │ │ │ -static PinholePose Identity() │ │ │ │ │ -for Canonical │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:415 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_P_i_n_h_o_l_e_P_o_s_e │ │ │ │ │ -PinholePose(const Vector &v, const Vector &K) │ │ │ │ │ -Init from Vector and calibration. │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:319 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_s_h_a_r_e_d_C_a_l_i_b_r_a_t_i_o_n │ │ │ │ │ -const boost::shared_ptr< CALIBRATION > & sharedCalibration() const │ │ │ │ │ -return shared pointer to calibration │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:365 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_d_i_m │ │ │ │ │ -size_t dim() const │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:395 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_p_r_o_j_e_c_t_2 │ │ │ │ │ -Point2 project2(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, │ │ │ │ │ -OptionalJacobian< 2, 3 > Dpoint=boost::none) const │ │ │ │ │ -project a point from world coordinate to the image, 2 derivatives only │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:379 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_r_e_t_r_a_c_t │ │ │ │ │ -PinholePose retract(const Vector6 &d) const │ │ │ │ │ -move a cameras according to d │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:405 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_P_i_n_h_o_l_e_P_o_s_e │ │ │ │ │ -PinholePose(const Vector &v) │ │ │ │ │ -Init from 6D vector. │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:314 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_P_i_n_h_o_l_e_P_o_s_e │ │ │ │ │ -PinholePose(const Pose3 &pose, const boost::shared_ptr< CALIBRATION > &K) │ │ │ │ │ -constructor with pose and calibration │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:269 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_D_i_m │ │ │ │ │ -static size_t Dim() │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:400 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:434 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const Base &camera, double tol=1e-9) const │ │ │ │ │ -assert equality up to a tolerance │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:333 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="PinholePose") const override │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:349 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_L_e_v_e_l │ │ │ │ │ -static PinholePose Level(const boost::shared_ptr< CALIBRATION > &K, const Pose2 │ │ │ │ │ -&pose2, double height) │ │ │ │ │ -Create a level camera at the given 2D pose and height. │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:284 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_P_i_n_h_o_l_e_P_o_s_e │ │ │ │ │ -PinholePose(const Pose3 &pose) │ │ │ │ │ -constructor with pose, uses default calibration │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:264 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_L_e_v_e_l │ │ │ │ │ -static PinholePose Level(const Pose2 &pose2, double height) │ │ │ │ │ -PinholePose::level with default calibration. │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:290 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_P_o_s_e_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ -Vector6 localCoordinates(const PinholePose &p) const │ │ │ │ │ -return canonical coordinate │ │ │ │ │ -DDeeffiinniittiioonn PinholePose.h:410 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_2 │ │ │ │ │ -A 2D pose (Point2,Rot2) │ │ │ │ │ -DDeeffiinniittiioonn Pose2.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ -A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_t_r_a_n_s_f_o_r_m_F_r_o_m │ │ │ │ │ -Point3 transformFrom(const Point3 &point, OptionalJacobian< 3, 6 > │ │ │ │ │ -Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const │ │ │ │ │ -takes point in Pose coordinates and transforms it to world coordinates │ │ │ │ │ -DDeeffiinniittiioonn Pose3.cpp:347 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_r_a_n_g_e │ │ │ │ │ -double range(const Point3 &point, OptionalJacobian< 1, 6 > Hself=boost::none, │ │ │ │ │ -OptionalJacobian< 1, 3 > Hpoint=boost::none) const │ │ │ │ │ -Calculate range to a landmark. │ │ │ │ │ -DDeeffiinniittiioonn Pose3.cpp:399 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_t_r_a_n_s_l_a_t_i_o_n │ │ │ │ │ -const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const │ │ │ │ │ -get translation │ │ │ │ │ -DDeeffiinniittiioonn Pose3.cpp:308 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3_:_:_r_o_t_a_t_i_o_n │ │ │ │ │ -const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const │ │ │ │ │ -get rotation │ │ │ │ │ -DDeeffiinniittiioonn Pose3.cpp:315 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_r_o_t_a_t_e │ │ │ │ │ -Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, │ │ │ │ │ -OptionalJacobian< 3, 3 > H2=boost::none) const │ │ │ │ │ -rotate point from rotated coordinate frame to world │ │ │ │ │ -DDeeffiinniittiioonn Rot3M.cpp:149 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_r_p_y │ │ │ │ │ -Vector3 rpy(OptionalJacobian< 3, 3 > H=boost::none) const │ │ │ │ │ -Use RQ to calculate roll-pitch-yaw angle representation. │ │ │ │ │ -DDeeffiinniittiioonn Rot3.cpp:192 │ │ │ │ │ -_g_t_s_a_m_:_:_U_n_i_t_3 │ │ │ │ │ -Represents a 3D point on a unit sphere. │ │ │ │ │ -DDeeffiinniittiioonn Unit3.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r │ │ │ │ │ +Default allocator for list, map, and set types. │ │ │ │ │ +DDeeffiinniittiioonn FastDefaultAllocator.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e │ │ │ │ │ +Wraps any type T so it can play as a Value. │ │ │ │ │ +DDeeffiinniittiioonn GenericValue.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e │ │ │ │ │ +This is the base class for any type to be stored in Values. │ │ │ │ │ +DDeeffiinniittiioonn Value.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_:_:_d_e_a_l_l_o_c_a_t_e__ │ │ │ │ │ +virtual void deallocate_() const =0 │ │ │ │ │ +Deallocate a raw pointer of this value. │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_:_:_c_l_o_n_e__ │ │ │ │ │ +virtual Value * clone_() const =0 │ │ │ │ │ +Clone this value in a special memory pool, must be deleted with Value:: │ │ │ │ │ +deallocate_,... │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ +with a unique integer... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_C_l_o_n_e_A_l_l_o_c_a_t_o_r │ │ │ │ │ +DDeeffiinniittiioonn Values.h:49 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_u_p_d_a_t_e │ │ │ │ │ +void update(Key j, const T &val) │ │ │ │ │ +Templated version to update a variable with the given j, throws │ │ │ │ │ +KeyDoesNotExist if j is not presen... │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_e_m_p_t_y │ │ │ │ │ +bool empty() const │ │ │ │ │ +whether the config is empty │ │ │ │ │ +DDeeffiinniittiioonn Values.h:184 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_u_p_p_e_r___b_o_u_n_d │ │ │ │ │ +deref_iterator upper_bound(Key j) const │ │ │ │ │ +Find the lowest-ordered element greater than the specified key. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:219 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< Values > shared_ptr │ │ │ │ │ +A shared_ptr to this class. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:87 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_c_l_e_a_r │ │ │ │ │ +void clear() │ │ │ │ │ +Remove all variables from the config. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:301 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_s_w_a_p │ │ │ │ │ +void swap(Values &other) │ │ │ │ │ +Swap the contents of two Values without copying data. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:298 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_i_n_s_e_r_t_D_o_u_b_l_e │ │ │ │ │ +void insertDouble(Key j, double c) │ │ │ │ │ +version for double │ │ │ │ │ +DDeeffiinniittiioonn Values.h:252 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_s_i_z_e │ │ │ │ │ +size_t size() const │ │ │ │ │ +The number of variables in this config. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:181 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_f_i_n_d │ │ │ │ │ +deref_iterator find(Key j) const │ │ │ │ │ +Find an element by key, returning an iterator, or end() if the key was not │ │ │ │ │ +found. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:213 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_c_o_n_s_t___s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< const Values > const_shared_ptr │ │ │ │ │ +A const shared_ptr to this class. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:90 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_V_a_l_u_e_s │ │ │ │ │ +Values()=default │ │ │ │ │ +Default constructor creates an empty Values class. │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_l_o_w_e_r___b_o_u_n_d │ │ │ │ │ +deref_iterator lower_bound(Key j) const │ │ │ │ │ +Find the element greater than or equal to the specified key. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:216 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_a_t_D_o_u_b_l_e │ │ │ │ │ +double atDouble(size_t key) const │ │ │ │ │ +version for double │ │ │ │ │ +DDeeffiinniittiioonn Values.h:159 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_K_e_y_V_a_l_u_e_P_a_i_r │ │ │ │ │ +A key-value pair, which you get by dereferencing iterators. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:93 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_K_e_y_V_a_l_u_e_P_a_i_r_:_:_v_a_l_u_e │ │ │ │ │ +Value & value │ │ │ │ │ +The value. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:95 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_K_e_y_V_a_l_u_e_P_a_i_r_:_:_k_e_y │ │ │ │ │ +const Key key │ │ │ │ │ +The key. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:94 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r │ │ │ │ │ +A key-value pair, which you get by dereferencing iterators. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:101 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r_:_:_k_e_y │ │ │ │ │ +const Key key │ │ │ │ │ +The key. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:102 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r_:_:_v_a_l_u_e │ │ │ │ │ +const Value & value │ │ │ │ │ +The value. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:103 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_d_e_r_e_f___i_t_e_r_a_t_o_r │ │ │ │ │ +DDeeffiinniittiioonn Values.h:190 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_K_e_y_A_l_r_e_a_d_y_E_x_i_s_t_s │ │ │ │ │ +DDeeffiinniittiioonn Values.h:453 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_K_e_y_A_l_r_e_a_d_y_E_x_i_s_t_s_:_:_k_e_y__ │ │ │ │ │ +const Key key_ │ │ │ │ │ +The key that already existed. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:455 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_K_e_y_A_l_r_e_a_d_y_E_x_i_s_t_s_:_:_w_h_a_t │ │ │ │ │ +GTSAM_EXPORT const char * what() const noexcept override │ │ │ │ │ +The message to be displayed to the user. │ │ │ │ │ +DDeeffiinniittiioonn Values.cpp:280 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_K_e_y_A_l_r_e_a_d_y_E_x_i_s_t_s_:_:_V_a_l_u_e_s_K_e_y_A_l_r_e_a_d_y_E_x_i_s_t_s │ │ │ │ │ +ValuesKeyAlreadyExists(Key key) noexcept │ │ │ │ │ +Construct with the key-value pair attempted to be added. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:462 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_K_e_y_A_l_r_e_a_d_y_E_x_i_s_t_s_:_:_k_e_y │ │ │ │ │ +Key key() const noexcept │ │ │ │ │ +The duplicate key that was attempted to be added. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:468 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_K_e_y_D_o_e_s_N_o_t_E_x_i_s_t │ │ │ │ │ +DDeeffiinniittiioonn Values.h:475 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_K_e_y_D_o_e_s_N_o_t_E_x_i_s_t_:_:_V_a_l_u_e_s_K_e_y_D_o_e_s_N_o_t_E_x_i_s_t │ │ │ │ │ +ValuesKeyDoesNotExist(const char *operation, Key key) noexcept │ │ │ │ │ +Construct with the key that does not exist in the values. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:485 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_K_e_y_D_o_e_s_N_o_t_E_x_i_s_t_:_:_k_e_y__ │ │ │ │ │ +const Key key_ │ │ │ │ │ +The key that does not exist. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:478 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_K_e_y_D_o_e_s_N_o_t_E_x_i_s_t_:_:_k_e_y │ │ │ │ │ +Key key() const noexcept │ │ │ │ │ +The key that was attempted to be accessed that does not exist. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:491 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_K_e_y_D_o_e_s_N_o_t_E_x_i_s_t_:_:_o_p_e_r_a_t_i_o_n__ │ │ │ │ │ +const char * operation_ │ │ │ │ │ +The operation that attempted to access the key. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:477 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e │ │ │ │ │ +DDeeffiinniittiioonn Values.h:498 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e_:_:_s_t_o_r_e_d_T_y_p_e_I_d │ │ │ │ │ +const std::type_info & storedTypeId() const │ │ │ │ │ +The typeid of the value stores in the Values. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:519 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e_:_:_k_e_y │ │ │ │ │ +Key key() const noexcept │ │ │ │ │ +The key that was attempted to be accessed that does not exist. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:516 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e_:_:_k_e_y__ │ │ │ │ │ +const Key key_ │ │ │ │ │ +The key requested. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:500 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e_:_:_V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e │ │ │ │ │ +ValuesIncorrectType(Key key, const std::type_info &storedTypeId, const std:: │ │ │ │ │ +type_info &requestedTypeId) noexcept │ │ │ │ │ +Construct with the key that does not exist in the values. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:509 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e_:_:_r_e_q_u_e_s_t_e_d_T_y_p_e_I_d │ │ │ │ │ +const std::type_info & requestedTypeId() const │ │ │ │ │ +The requested typeid. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:522 │ │ │ │ │ +_g_t_s_a_m_:_:_D_y_n_a_m_i_c_V_a_l_u_e_s_M_i_s_m_a_t_c_h_e_d │ │ │ │ │ +DDeeffiinniittiioonn Values.h:529 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_M_a_t_c_h_F_o_u_n_d_F_o_r_F_i_x_e_d │ │ │ │ │ +DDeeffiinniittiioonn Values.h:542 │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +The Factor::error simply extracts the. │ │ │ │ │ +_V_a_l_u_e_s │ │ │ │ │ +In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ +a non-linear function... │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _P_i_n_h_o_l_e_P_o_s_e_._h │ │ │ │ │ + * _n_o_n_l_i_n_e_a_r │ │ │ │ │ + * _V_a_l_u_e_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00470.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01238.html │ │ │ │┄ Files 96% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Rot3M.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/InitializePose3.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces
│ │ │ │ -
Rot3M.cpp File Reference
│ │ │ │ +
InitializePose3.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -

Rotation (internal: 3*3 matrix representation*) │ │ │ │ -More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Rotation (internal: 3*3 matrix representation*)

│ │ │ │ -
Author
Alireza Fathi
│ │ │ │ -
│ │ │ │ -Christian Potthast
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ +
Author
Luca Carlone
│ │ │ │
│ │ │ │ -Richard Roberts
│ │ │ │ +Frank Dellaert
│ │ │ │ +
Date
August, 2014
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,21 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -Rot3M.cpp File Reference │ │ │ │ │ -Rotation (internal: 3*3 matrix representation*) _M_o_r_e_._._. │ │ │ │ │ +InitializePose3.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Rotation (internal: 3*3 matrix representation*) │ │ │ │ │ Author │ │ │ │ │ - Alireza Fathi │ │ │ │ │ - Christian Potthast │ │ │ │ │ + Luca Carlone │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + August, 2014 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _R_o_t_3_M_._c_p_p │ │ │ │ │ + * _s_l_a_m │ │ │ │ │ + * _I_n_i_t_i_a_l_i_z_e_P_o_s_e_3_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00476.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00962.html │ │ │ │┄ Files 79% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Point2.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/TangentPreintegration.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces | │ │ │ │ -Functions
│ │ │ │ -
Point2.cpp File Reference
│ │ │ │ +Macros
│ │ │ │ +
TangentPreintegration.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ - │ │ │ │ -

2D Point │ │ │ │ -More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -double gtsam::norm2 (const Point2 &p, OptionalJacobian< 1, 2 > H=boost::none)
 Distance of the point from the origin, with Jacobian.
 
│ │ │ │ -double gtsam::distance2 (const Point2 &p1, const Point2 &q, OptionalJacobian< 1, 2 > H1=boost::none, OptionalJacobian< 1, 2 > H2=boost::none)
 distance between two points
 
│ │ │ │ -boost::optional< Point2 > gtsam::circleCircleIntersection (double R_d, double r_d, double tol)
 
│ │ │ │ -list< Point2 > gtsam::circleCircleIntersection (Point2 c1, Point2 c2, boost::optional< Point2 > fh)
 
list< Point2 > gtsam::circleCircleIntersection (Point2 c1, double r1, Point2 c2, double r2, double tol=1e-9)
 Intersect 2 circles.
 
│ │ │ │ -Point2Pair gtsam::means (const std::vector< Point2Pair > &abPointPairs)
 Calculate the two means of a set of Point2 pairs.
 
│ │ │ │ -ostream & gtsam::operator<< (ostream &os, const gtsam::Point2Pair &p)
 

│ │ │ │ +Macros

│ │ │ │ +#define D_R_R(H)   (H)->block<3,3>(0,0)
 
│ │ │ │ +#define D_R_t(H)   (H)->block<3,3>(0,3)
 
│ │ │ │ +#define D_R_v(H)   (H)->block<3,3>(0,6)
 
│ │ │ │ +#define D_t_R(H)   (H)->block<3,3>(3,0)
 
│ │ │ │ +#define D_t_t(H)   (H)->block<3,3>(3,3)
 
│ │ │ │ +#define D_t_v(H)   (H)->block<3,3>(3,6)
 
│ │ │ │ +#define D_v_R(H)   (H)->block<3,3>(6,0)
 
│ │ │ │ +#define D_v_t(H)   (H)->block<3,3>(6,3)
 
│ │ │ │ +#define D_v_v(H)   (H)->block<3,3>(6,6)
 
│ │ │ │

Detailed Description

│ │ │ │ -

2D Point

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ +Adam Bry
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,48 +1,38 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -Point2.cpp File Reference │ │ │ │ │ -2D Point _M_o_r_e_._._. │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s │ │ │ │ │ +TangentPreintegration.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ - double  ggttssaamm::::nnoorrmm22 (const _P_o_i_n_t_2 &p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 1, │ │ │ │ │ - 2 > H=boost::none) │ │ │ │ │ -  Distance of the point from the origin, with │ │ │ │ │ - Jacobian. │ │ │ │ │ -  │ │ │ │ │ - double  ggttssaamm::::ddiissttaannccee22 (const _P_o_i_n_t_2 &p1, const _P_o_i_n_t_2 &q, │ │ │ │ │ - _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 1, 2 > H1=boost::none, │ │ │ │ │ - _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 1, 2 > H2=boost::none) │ │ │ │ │ -  distance between two points │ │ │ │ │ -  │ │ │ │ │ -boost::optional< _P_o_i_n_t_2 >  ggttssaamm::::cciirrcclleeCCiirrcclleeIInntteerrsseeccttiioonn (double R_d, double │ │ │ │ │ - r_d, double tol) │ │ │ │ │ -  │ │ │ │ │ - list< _P_o_i_n_t_2 >  ggttssaamm::::cciirrcclleeCCiirrcclleeIInntteerrsseeccttiioonn (_P_o_i_n_t_2 c1, _P_o_i_n_t_2 │ │ │ │ │ - c2, boost::optional< _P_o_i_n_t_2 > fh) │ │ │ │ │ -  │ │ │ │ │ - list< _P_o_i_n_t_2 >  _g_t_s_a_m_:_:_c_i_r_c_l_e_C_i_r_c_l_e_I_n_t_e_r_s_e_c_t_i_o_n (_P_o_i_n_t_2 c1, double │ │ │ │ │ - r1, _P_o_i_n_t_2 c2, double r2, double tol=1e-9) │ │ │ │ │ -  Intersect 2 circles. │ │ │ │ │ -  │ │ │ │ │ - Point2Pair  ggttssaamm::::mmeeaannss (const std::vector< Point2Pair > │ │ │ │ │ - &abPointPairs) │ │ │ │ │ -  Calculate the two means of a set of Point2 pairs. │ │ │ │ │ +MMaaccrrooss │ │ │ │ │ +#define  DD__RR__RR(H)   (H)->block<3,3>(0,0) │ │ │ │ │   │ │ │ │ │ - ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const gtsam:: │ │ │ │ │ - Point2Pair &p) │ │ │ │ │ +#define  DD__RR__tt(H)   (H)->block<3,3>(0,3) │ │ │ │ │ +  │ │ │ │ │ +#define  DD__RR__vv(H)   (H)->block<3,3>(0,6) │ │ │ │ │ +  │ │ │ │ │ +#define  DD__tt__RR(H)   (H)->block<3,3>(3,0) │ │ │ │ │ +  │ │ │ │ │ +#define  DD__tt__tt(H)   (H)->block<3,3>(3,3) │ │ │ │ │ +  │ │ │ │ │ +#define  DD__tt__vv(H)   (H)->block<3,3>(3,6) │ │ │ │ │ +  │ │ │ │ │ +#define  DD__vv__RR(H)   (H)->block<3,3>(6,0) │ │ │ │ │ +  │ │ │ │ │ +#define  DD__vv__tt(H)   (H)->block<3,3>(6,3) │ │ │ │ │ +  │ │ │ │ │ +#define  DD__vv__vv(H)   (H)->block<3,3>(6,6) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -2D Point │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Adam Bry │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _g_e_o_m_e_t_r_y │ │ │ │ │ - * _P_o_i_n_t_2_._c_p_p │ │ │ │ │ + * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ + * _T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00503_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01193_source.html │ │ │ │┄ Files 89% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridNonlinearISAM.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/TranslationRecovery.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
HybridNonlinearISAM.h
│ │ │ │ +
TranslationRecovery.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/hybrid/HybridGaussianISAM.h>
│ │ │ │ -
21#include <gtsam/hybrid/HybridNonlinearFactorGraph.h>
│ │ │ │ -
22
│ │ │ │ -
23namespace gtsam {
│ │ │ │ -
│ │ │ │ -
27class GTSAM_EXPORT HybridNonlinearISAM {
│ │ │ │ -
28 protected:
│ │ │ │ -
30 gtsam::HybridGaussianISAM isam_;
│ │ │ │ -
31
│ │ │ │ -
33 Values linPoint_;
│ │ │ │ -
34
│ │ │ │ -
36 DiscreteValues assignment_;
│ │ │ │ -
37
│ │ │ │ -
39 HybridNonlinearFactorGraph factors_;
│ │ │ │ -
40
│ │ │ │ -
42 int reorderInterval_;
│ │ │ │ -
43 int reorderCounter_;
│ │ │ │ -
44
│ │ │ │ -
46 HybridGaussianFactorGraph::Eliminate eliminationFunction_;
│ │ │ │ -
47
│ │ │ │ -
48 public:
│ │ │ │ -
51
│ │ │ │ -
│ │ │ │ -
59 HybridNonlinearISAM(
│ │ │ │ -
60 int reorderInterval = 1,
│ │ │ │ -
61 const HybridGaussianFactorGraph::Eliminate& eliminationFunction =
│ │ │ │ -
62 HybridGaussianFactorGraph::EliminationTraitsType::DefaultEliminate)
│ │ │ │ -
63 : reorderInterval_(reorderInterval),
│ │ │ │ -
64 reorderCounter_(0),
│ │ │ │ -
65 eliminationFunction_(eliminationFunction) {}
│ │ │ │ -
│ │ │ │ -
66
│ │ │ │ -
70
│ │ │ │ -
72 Values estimate();
│ │ │ │ -
73
│ │ │ │ -
74 // /** find the marginal covariance for a single variable */
│ │ │ │ -
75 // Matrix marginalCovariance(Key key) const;
│ │ │ │ -
76
│ │ │ │ -
77 // access
│ │ │ │ -
78
│ │ │ │ -
80 const HybridGaussianISAM& bayesTree() const { return isam_; }
│ │ │ │ -
81
│ │ │ │ -
87 void prune(const size_t maxNumberLeaves) { isam_.prune(maxNumberLeaves); }
│ │ │ │ -
88
│ │ │ │ -
90 const Values& getLinearizationPoint() const { return linPoint_; }
│ │ │ │ -
91
│ │ │ │ -
93 const DiscreteValues& assignment() const { return assignment_; }
│ │ │ │ -
94
│ │ │ │ -
│ │ │ │ -
96 const HybridNonlinearFactorGraph& getFactorsUnsafe() const {
│ │ │ │ -
97 return factors_;
│ │ │ │ -
98 }
│ │ │ │ -
│ │ │ │ -
99
│ │ │ │ -
101 int reorderInterval() const { return reorderInterval_; }
│ │ │ │ -
│ │ │ │ -
102 int reorderCounter() const { return reorderCounter_; }
│ │ │ │ -
103
│ │ │ │ -
105 void print(const std::string& s = "",
│ │ │ │ -
106 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ -
107
│ │ │ │ -
109 void printStats() const;
│ │ │ │ -
110
│ │ │ │ -
112 void saveGraph(const std::string& s,
│ │ │ │ -
113 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ -
114
│ │ │ │ -
118
│ │ │ │ -
120 void update(const HybridNonlinearFactorGraph& newFactors,
│ │ │ │ -
121 const Values& initialValues,
│ │ │ │ -
122 const boost::optional<size_t>& maxNrLeaves = boost::none,
│ │ │ │ -
123 const boost::optional<Ordering>& ordering = boost::none);
│ │ │ │ -
124
│ │ │ │ -
126 void reorder_relinearize();
│ │ │ │ -
127
│ │ │ │ -
129};
│ │ │ │ -
130
│ │ │ │ -
131} // namespace gtsam
│ │ │ │ -
│ │ │ │ +
19#include <gtsam/geometry/Unit3.h>
│ │ │ │ +
20#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
│ │ │ │ +
21#include <gtsam/nonlinear/Values.h>
│ │ │ │ +
22#include <gtsam/sfm/BinaryMeasurement.h>
│ │ │ │ +
23
│ │ │ │ +
24#include <map>
│ │ │ │ +
25#include <set>
│ │ │ │ +
26#include <utility>
│ │ │ │ +
27#include <vector>
│ │ │ │ +
28
│ │ │ │ +
29namespace gtsam {
│ │ │ │ +
30
│ │ │ │ +
31// Set up an optimization problem for the unknown translations Ti in the world
│ │ │ │ +
32// coordinate frame, given the known camera attitudes wRi with respect to the
│ │ │ │ +
33// world frame, and a set of (noisy) translation directions of type Unit3,
│ │ │ │ +
34// w_aZb. The measurement equation is
│ │ │ │ +
35// w_aZb = Unit3(Tb - Ta) (1)
│ │ │ │ +
36// i.e., w_aZb is the translation direction from frame A to B, in world
│ │ │ │ +
37// coordinates. Although Unit3 instances live on a manifold, following
│ │ │ │ +
38// Wilson14eccv_1DSfM.pdf error we compute the *chordal distance* in the
│ │ │ │ +
39// ambient world coordinate frame.
│ │ │ │ +
40//
│ │ │ │ +
41// It is clear that we cannot recover the scale, nor the absolute position,
│ │ │ │ +
42// so the gauge freedom in this case is 3 + 1 = 4. We fix these by taking fixing
│ │ │ │ +
43// the translations Ta and Tb associated with the first measurement w_aZb,
│ │ │ │ +
44// clamping them to their initial values as given to this method. If no initial
│ │ │ │ +
45// values are given, we use the origin for Tb and set Tb to make (1) come
│ │ │ │ +
46// through, i.e.,
│ │ │ │ +
47// Tb = s * wRa * Point3(w_aZb) (2)
│ │ │ │ +
48// where s is an arbitrary scale that can be supplied, default 1.0. Hence, two
│ │ │ │ +
49// versions are supplied below corresponding to whether we have initial values
│ │ │ │ +
50// or not.
│ │ │ │ +
│ │ │ │ +
51class TranslationRecovery {
│ │ │ │ +
52 public:
│ │ │ │ +
53 using KeyPair = std::pair<Key, Key>;
│ │ │ │ +
54 using TranslationEdges = std::vector<BinaryMeasurement<Unit3>>;
│ │ │ │ +
55
│ │ │ │ +
56 private:
│ │ │ │ +
57 // Translation directions between camera pairs.
│ │ │ │ +
58 TranslationEdges relativeTranslations_;
│ │ │ │ +
59
│ │ │ │ +
60 // Parameters.
│ │ │ │ +
61 LevenbergMarquardtParams lmParams_;
│ │ │ │ +
62
│ │ │ │ +
63 public:
│ │ │ │ +
│ │ │ │ +
69 TranslationRecovery(const LevenbergMarquardtParams &lmParams)
│ │ │ │ +
70 : lmParams_(lmParams) {}
│ │ │ │
│ │ │ │ - │ │ │ │ -
HybridNonlinearFactorGraph.h
Nonlinear hybrid factor graph that uses type erasure.
│ │ │ │ +
71
│ │ │ │ +
75 TranslationRecovery() = default;
│ │ │ │ +
76
│ │ │ │ +
84 NonlinearFactorGraph buildGraph(
│ │ │ │ +
85 const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations) const;
│ │ │ │ +
86
│ │ │ │ +
102 void addPrior(
│ │ │ │ +
103 const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
│ │ │ │ +
104 const double scale,
│ │ │ │ +
105 const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
│ │ │ │ +
106 NonlinearFactorGraph *graph,
│ │ │ │ +
107 const SharedNoiseModel &priorNoiseModel =
│ │ │ │ +
108 noiseModel::Isotropic::Sigma(3, 0.01)) const;
│ │ │ │ +
109
│ │ │ │ +
121 Values initializeRandomly(
│ │ │ │ +
122 const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
│ │ │ │ +
123 const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
│ │ │ │ +
124 std::mt19937 *rng, const Values &initialValues = Values()) const;
│ │ │ │ +
125
│ │ │ │ +
136 Values initializeRandomly(
│ │ │ │ +
137 const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
│ │ │ │ +
138 const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
│ │ │ │ +
139 const Values &initialValues = Values()) const;
│ │ │ │ +
140
│ │ │ │ +
159 Values run(
│ │ │ │ +
160 const TranslationEdges &relativeTranslations, const double scale = 1.0,
│ │ │ │ +
161 const std::vector<BinaryMeasurement<Point3>> &betweenTranslations = {},
│ │ │ │ +
162 const Values &initialValues = Values()) const;
│ │ │ │ +
163
│ │ │ │ +
173 static TranslationEdges SimulateMeasurements(
│ │ │ │ +
174 const Values &poses, const std::vector<KeyPair> &edges);
│ │ │ │ +
175};
│ │ │ │ +
│ │ │ │ +
176} // namespace gtsam
│ │ │ │ +
Values.h
A non-templated config holding any types of Manifold-group elements.
│ │ │ │ +
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
│ │ │ │ +
BinaryMeasurement.h
Binary measurement represents a measurement between two keys in a graph. A binary measurement is simi...
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::DiscreteValues
A map from keys to values.
Definition DiscreteValues.h:34
│ │ │ │ -
gtsam::HybridBayesTree::prune
void prune(const size_t maxNumberLeaves)
Prune the underlying Bayes tree.
Definition HybridBayesTree.cpp:175
│ │ │ │ -
gtsam::HybridGaussianISAM
Definition HybridGaussianISAM.h:34
│ │ │ │ -
gtsam::HybridNonlinearFactorGraph
Definition HybridNonlinearFactorGraph.h:33
│ │ │ │ -
gtsam::HybridNonlinearISAM
Wrapper class to manage ISAM in a nonlinear context.
Definition HybridNonlinearISAM.h:27
│ │ │ │ -
gtsam::HybridNonlinearISAM::getFactorsUnsafe
const HybridNonlinearFactorGraph & getFactorsUnsafe() const
get underlying nonlinear graph
Definition HybridNonlinearISAM.h:96
│ │ │ │ -
gtsam::HybridNonlinearISAM::assignment_
DiscreteValues assignment_
The discrete assignment.
Definition HybridNonlinearISAM.h:36
│ │ │ │ -
gtsam::HybridNonlinearISAM::getLinearizationPoint
const Values & getLinearizationPoint() const
Return the current linearization point.
Definition HybridNonlinearISAM.h:90
│ │ │ │ -
gtsam::HybridNonlinearISAM::HybridNonlinearISAM
HybridNonlinearISAM(int reorderInterval=1, const HybridGaussianFactorGraph::Eliminate &eliminationFunction=HybridGaussianFactorGraph::EliminationTraitsType::DefaultEliminate)
Periodically reorder and relinearize.
Definition HybridNonlinearISAM.h:59
│ │ │ │ -
gtsam::HybridNonlinearISAM::factors_
HybridNonlinearFactorGraph factors_
The original factors, used when relinearizing.
Definition HybridNonlinearISAM.h:39
│ │ │ │ -
gtsam::HybridNonlinearISAM::assignment
const DiscreteValues & assignment() const
Return the current discrete assignment.
Definition HybridNonlinearISAM.h:93
│ │ │ │ -
gtsam::HybridNonlinearISAM::reorderCounter
int reorderCounter() const
TODO: comment.
Definition HybridNonlinearISAM.h:102
│ │ │ │ -
gtsam::HybridNonlinearISAM::prune
void prune(const size_t maxNumberLeaves)
Prune the underlying Bayes tree.
Definition HybridNonlinearISAM.h:87
│ │ │ │ -
gtsam::HybridNonlinearISAM::reorderInterval_
int reorderInterval_
The reordering interval and counter.
Definition HybridNonlinearISAM.h:42
│ │ │ │ -
gtsam::HybridNonlinearISAM::reorderInterval
int reorderInterval() const
get counters
Definition HybridNonlinearISAM.h:101
│ │ │ │ -
gtsam::HybridNonlinearISAM::linPoint_
Values linPoint_
The current linearization point.
Definition HybridNonlinearISAM.h:33
│ │ │ │ -
gtsam::HybridNonlinearISAM::isam_
gtsam::HybridGaussianISAM isam_
The internal iSAM object.
Definition HybridNonlinearISAM.h:30
│ │ │ │ -
gtsam::HybridNonlinearISAM::eliminationFunction_
HybridGaussianFactorGraph::Eliminate eliminationFunction_
The elimination function.
Definition HybridNonlinearISAM.h:46
│ │ │ │ -
gtsam::HybridNonlinearISAM::bayesTree
const HybridGaussianISAM & bayesTree() const
access the underlying bayes tree
Definition HybridNonlinearISAM.h:80
│ │ │ │ -
gtsam::EliminateableFactorGraph< HybridGaussianFactorGraph >::Eliminate
std::function< EliminationResult(const FactorGraphType &, const Ordering &)> Eliminate
The function type that does a single dense elimination step on a subgraph.
Definition EliminateableFactorGraph.h:89
│ │ │ │ +
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ +
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
An isotropic noise model created by specifying a standard devation sigma.
Definition NoiseModel.cpp:597
│ │ │ │ +
gtsam::LevenbergMarquardtParams
Parameters for Levenberg-Marquardt optimization.
Definition LevenbergMarquardtParams.h:35
│ │ │ │ +
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
gtsam::BinaryMeasurement
Definition BinaryMeasurement.h:36
│ │ │ │ +
gtsam::TranslationRecovery
Definition TranslationRecovery.h:51
│ │ │ │ +
gtsam::TranslationRecovery::buildGraph
NonlinearFactorGraph buildGraph(const std::vector< BinaryMeasurement< Unit3 > > &relativeTranslations) const
Build the factor graph to do the optimization.
Definition TranslationRecovery.cpp:99
│ │ │ │ +
gtsam::TranslationRecovery::SimulateMeasurements
static TranslationEdges SimulateMeasurements(const Values &poses, const std::vector< KeyPair > &edges)
Simulate translation direction measurements.
Definition TranslationRecovery.cpp:216
│ │ │ │ +
gtsam::TranslationRecovery::TranslationRecovery
TranslationRecovery()=default
Default constructor.
│ │ │ │ +
gtsam::TranslationRecovery::run
Values run(const TranslationEdges &relativeTranslations, const double scale=1.0, const std::vector< BinaryMeasurement< Point3 > > &betweenTranslations={}, const Values &initialValues=Values()) const
Build and optimize factor graph.
Definition TranslationRecovery.cpp:177
│ │ │ │ +
gtsam::TranslationRecovery::addPrior
void addPrior(const std::vector< BinaryMeasurement< Unit3 > > &relativeTranslations, const double scale, const std::vector< BinaryMeasurement< Point3 > > &betweenTranslations, NonlinearFactorGraph *graph, const SharedNoiseModel &priorNoiseModel=noiseModel::Isotropic::Sigma(3, 0.01)) const
Add 3 factors to the graph:
Definition TranslationRecovery.cpp:111
│ │ │ │ +
gtsam::TranslationRecovery::TranslationRecovery
TranslationRecovery(const LevenbergMarquardtParams &lmParams)
Construct a new Translation Recovery object.
Definition TranslationRecovery.h:69
│ │ │ │ +
gtsam::TranslationRecovery::initializeRandomly
Values initializeRandomly(const std::vector< BinaryMeasurement< Unit3 > > &relativeTranslations, const std::vector< BinaryMeasurement< Point3 > > &betweenTranslations, std::mt19937 *rng, const Values &initialValues=Values()) const
Create random initial translations.
Definition TranslationRecovery.cpp:137
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,191 +1,177 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -HybridNonlinearISAM.h │ │ │ │ │ +TranslationRecovery.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -22 │ │ │ │ │ -23namespace _g_t_s_a_m { │ │ │ │ │ -_2_7class GTSAM_EXPORT _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M { │ │ │ │ │ -28 protected: │ │ │ │ │ -_3_0 _g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M _i_s_a_m__; │ │ │ │ │ -31 │ │ │ │ │ -_3_3 _V_a_l_u_e_s _l_i_n_P_o_i_n_t__; │ │ │ │ │ -34 │ │ │ │ │ -_3_6 _D_i_s_c_r_e_t_e_V_a_l_u_e_s _a_s_s_i_g_n_m_e_n_t__; │ │ │ │ │ -37 │ │ │ │ │ -_3_9 _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h _f_a_c_t_o_r_s__; │ │ │ │ │ -40 │ │ │ │ │ -_4_2 int _r_e_o_r_d_e_r_I_n_t_e_r_v_a_l__; │ │ │ │ │ -43 int reorderCounter_; │ │ │ │ │ -44 │ │ │ │ │ -_4_6 _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_E_l_i_m_i_n_a_t_e _e_l_i_m_i_n_a_t_i_o_n_F_u_n_c_t_i_o_n__; │ │ │ │ │ -47 │ │ │ │ │ -48 public: │ │ │ │ │ -51 │ │ │ │ │ -_5_9 _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M( │ │ │ │ │ -60 int reorderInterval = 1, │ │ │ │ │ -61 const _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_E_l_i_m_i_n_a_t_e& eliminationFunction = │ │ │ │ │ -62 HybridGaussianFactorGraph::EliminationTraitsType::DefaultEliminate) │ │ │ │ │ -63 : reorderInterval_(reorderInterval), │ │ │ │ │ -64 reorderCounter_(0), │ │ │ │ │ -65 eliminationFunction_(eliminationFunction) {} │ │ │ │ │ -66 │ │ │ │ │ -70 │ │ │ │ │ -72 _V_a_l_u_e_s estimate(); │ │ │ │ │ -73 │ │ │ │ │ -74 // /** find the marginal covariance for a single variable */ │ │ │ │ │ -75 // Matrix marginalCovariance(Key key) const; │ │ │ │ │ +19#include │ │ │ │ │ +20#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_V_a_l_u_e_s_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_s_f_m_/_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_._h> │ │ │ │ │ +23 │ │ │ │ │ +24#include │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28 │ │ │ │ │ +29namespace _g_t_s_a_m { │ │ │ │ │ +30 │ │ │ │ │ +31// Set up an optimization problem for the unknown translations Ti in the │ │ │ │ │ +world │ │ │ │ │ +32// coordinate frame, given the known camera attitudes wRi with respect to the │ │ │ │ │ +33// world frame, and a set of (noisy) translation directions of type Unit3, │ │ │ │ │ +34// w_aZb. The measurement equation is │ │ │ │ │ +35// w_aZb = Unit3(Tb - Ta) (1) │ │ │ │ │ +36// i.e., w_aZb is the translation direction from frame A to B, in world │ │ │ │ │ +37// coordinates. Although Unit3 instances live on a manifold, following │ │ │ │ │ +38// Wilson14eccv_1DSfM.pdf error we compute the *chordal distance* in the │ │ │ │ │ +39// ambient world coordinate frame. │ │ │ │ │ +40// │ │ │ │ │ +41// It is clear that we cannot recover the scale, nor the absolute position, │ │ │ │ │ +42// so the gauge freedom in this case is 3 + 1 = 4. We fix these by taking │ │ │ │ │ +fixing │ │ │ │ │ +43// the translations Ta and Tb associated with the first measurement w_aZb, │ │ │ │ │ +44// clamping them to their initial values as given to this method. If no │ │ │ │ │ +initial │ │ │ │ │ +45// values are given, we use the origin for Tb and set Tb to make (1) come │ │ │ │ │ +46// through, i.e., │ │ │ │ │ +47// Tb = s * wRa * Point3(w_aZb) (2) │ │ │ │ │ +48// where s is an arbitrary scale that can be supplied, default 1.0. Hence, │ │ │ │ │ +two │ │ │ │ │ +49// versions are supplied below corresponding to whether we have initial │ │ │ │ │ +values │ │ │ │ │ +50// or not. │ │ │ │ │ +_5_1class _T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y { │ │ │ │ │ +52 public: │ │ │ │ │ +53 using KeyPair = std::pair; │ │ │ │ │ +54 using TranslationEdges = std::vector>; │ │ │ │ │ +55 │ │ │ │ │ +56 private: │ │ │ │ │ +57 // Translation directions between camera pairs. │ │ │ │ │ +58 TranslationEdges relativeTranslations_; │ │ │ │ │ +59 │ │ │ │ │ +60 // Parameters. │ │ │ │ │ +61 _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s lmParams_; │ │ │ │ │ +62 │ │ │ │ │ +63 public: │ │ │ │ │ +_6_9 _T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y(const _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s &lmParams) │ │ │ │ │ +70 : lmParams_(lmParams) {} │ │ │ │ │ +71 │ │ │ │ │ +_7_5 _T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y() = default; │ │ │ │ │ 76 │ │ │ │ │ -77 // access │ │ │ │ │ -78 │ │ │ │ │ -_8_0 const _H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M& _b_a_y_e_s_T_r_e_e() const { return isam_; } │ │ │ │ │ -81 │ │ │ │ │ -_8_7 void _p_r_u_n_e(const size_t maxNumberLeaves) { isam_._p_r_u_n_e(maxNumberLeaves); } │ │ │ │ │ -88 │ │ │ │ │ -_9_0 const _V_a_l_u_e_s& _g_e_t_L_i_n_e_a_r_i_z_a_t_i_o_n_P_o_i_n_t() const { return linPoint_; } │ │ │ │ │ -91 │ │ │ │ │ -_9_3 const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& _a_s_s_i_g_n_m_e_n_t() const { return assignment_; } │ │ │ │ │ -94 │ │ │ │ │ -_9_6 const _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& _g_e_t_F_a_c_t_o_r_s_U_n_s_a_f_e() const { │ │ │ │ │ -97 return factors_; │ │ │ │ │ -98 } │ │ │ │ │ -99 │ │ │ │ │ -_1_0_1 int _r_e_o_r_d_e_r_I_n_t_e_r_v_a_l() const { return reorderInterval_; } │ │ │ │ │ -_1_0_2 int _r_e_o_r_d_e_r_C_o_u_n_t_e_r() const { return reorderCounter_; } │ │ │ │ │ -103 │ │ │ │ │ -105 void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ -106 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const; │ │ │ │ │ -107 │ │ │ │ │ -109 void printStats() const; │ │ │ │ │ -110 │ │ │ │ │ -112 void saveGraph(const std::string& s, │ │ │ │ │ -113 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const; │ │ │ │ │ -114 │ │ │ │ │ -118 │ │ │ │ │ -120 void update(const _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& newFactors, │ │ │ │ │ -121 const _V_a_l_u_e_s& initialValues, │ │ │ │ │ -122 const boost::optional& maxNrLeaves = boost::none, │ │ │ │ │ -123 const boost::optional& ordering = boost::none); │ │ │ │ │ -124 │ │ │ │ │ -126 void reorder_relinearize(); │ │ │ │ │ -127 │ │ │ │ │ -129}; │ │ │ │ │ -130 │ │ │ │ │ -131} // namespace gtsam │ │ │ │ │ -_H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M_._h │ │ │ │ │ -_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Nonlinear hybrid factor graph that uses type erasure. │ │ │ │ │ +84 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h _b_u_i_l_d_G_r_a_p_h( │ │ │ │ │ +85 const std::vector<_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_<_U_n_i_t_3_>> &relativeTranslations) const; │ │ │ │ │ +86 │ │ │ │ │ +102 void _a_d_d_P_r_i_o_r( │ │ │ │ │ +103 const std::vector<_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_<_U_n_i_t_3_>> &relativeTranslations, │ │ │ │ │ +104 const double scale, │ │ │ │ │ +105 const std::vector<_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_<_P_o_i_n_t_3_>> &betweenTranslations, │ │ │ │ │ +106 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h *graph, │ │ │ │ │ +107 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &priorNoiseModel = │ │ │ │ │ +108 _n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c_:_:_S_i_g_m_a(3, 0.01)) const; │ │ │ │ │ +109 │ │ │ │ │ +121 _V_a_l_u_e_s _i_n_i_t_i_a_l_i_z_e_R_a_n_d_o_m_l_y( │ │ │ │ │ +122 const std::vector<_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_<_U_n_i_t_3_>> &relativeTranslations, │ │ │ │ │ +123 const std::vector<_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_<_P_o_i_n_t_3_>> &betweenTranslations, │ │ │ │ │ +124 std::mt19937 *rng, const _V_a_l_u_e_s &initialValues = _V_a_l_u_e_s()) const; │ │ │ │ │ +125 │ │ │ │ │ +136 _V_a_l_u_e_s _i_n_i_t_i_a_l_i_z_e_R_a_n_d_o_m_l_y( │ │ │ │ │ +137 const std::vector<_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_<_U_n_i_t_3_>> &relativeTranslations, │ │ │ │ │ +138 const std::vector<_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_<_P_o_i_n_t_3_>> &betweenTranslations, │ │ │ │ │ +139 const _V_a_l_u_e_s &initialValues = _V_a_l_u_e_s()) const; │ │ │ │ │ +140 │ │ │ │ │ +159 _V_a_l_u_e_s _r_u_n( │ │ │ │ │ +160 const TranslationEdges &relativeTranslations, const double scale = 1.0, │ │ │ │ │ +161 const std::vector<_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_<_P_o_i_n_t_3_>> &betweenTranslations = {}, │ │ │ │ │ +162 const _V_a_l_u_e_s &initialValues = _V_a_l_u_e_s()) const; │ │ │ │ │ +163 │ │ │ │ │ +173 static TranslationEdges _S_i_m_u_l_a_t_e_M_e_a_s_u_r_e_m_e_n_t_s( │ │ │ │ │ +174 const _V_a_l_u_e_s &poses, const std::vector &edges); │ │ │ │ │ +175}; │ │ │ │ │ +176} // namespace gtsam │ │ │ │ │ +_V_a_l_u_e_s_._h │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ +A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme. │ │ │ │ │ +_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_._h │ │ │ │ │ +Binary measurement represents a measurement between two keys in a graph. A │ │ │ │ │ +binary measurement is simi... │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ -A map from keys to values. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteValues.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_:_:_p_r_u_n_e │ │ │ │ │ -void prune(const size_t maxNumberLeaves) │ │ │ │ │ -Prune the underlying Bayes tree. │ │ │ │ │ -DDeeffiinniittiioonn HybridBayesTree.cpp:175 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M │ │ │ │ │ -DDeeffiinniittiioonn HybridGaussianISAM.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -DDeeffiinniittiioonn HybridNonlinearFactorGraph.h:33 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M │ │ │ │ │ -Wrapper class to manage ISAM in a nonlinear context. │ │ │ │ │ -DDeeffiinniittiioonn HybridNonlinearISAM.h:27 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_g_e_t_F_a_c_t_o_r_s_U_n_s_a_f_e │ │ │ │ │ -const HybridNonlinearFactorGraph & getFactorsUnsafe() const │ │ │ │ │ -get underlying nonlinear graph │ │ │ │ │ -DDeeffiinniittiioonn HybridNonlinearISAM.h:96 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_a_s_s_i_g_n_m_e_n_t__ │ │ │ │ │ -DiscreteValues assignment_ │ │ │ │ │ -The discrete assignment. │ │ │ │ │ -DDeeffiinniittiioonn HybridNonlinearISAM.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_g_e_t_L_i_n_e_a_r_i_z_a_t_i_o_n_P_o_i_n_t │ │ │ │ │ -const Values & getLinearizationPoint() const │ │ │ │ │ -Return the current linearization point. │ │ │ │ │ -DDeeffiinniittiioonn HybridNonlinearISAM.h:90 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M │ │ │ │ │ -HybridNonlinearISAM(int reorderInterval=1, const HybridGaussianFactorGraph:: │ │ │ │ │ -Eliminate &eliminationFunction=HybridGaussianFactorGraph:: │ │ │ │ │ -EliminationTraitsType::DefaultEliminate) │ │ │ │ │ -Periodically reorder and relinearize. │ │ │ │ │ -DDeeffiinniittiioonn HybridNonlinearISAM.h:59 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_f_a_c_t_o_r_s__ │ │ │ │ │ -HybridNonlinearFactorGraph factors_ │ │ │ │ │ -The original factors, used when relinearizing. │ │ │ │ │ -DDeeffiinniittiioonn HybridNonlinearISAM.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_a_s_s_i_g_n_m_e_n_t │ │ │ │ │ -const DiscreteValues & assignment() const │ │ │ │ │ -Return the current discrete assignment. │ │ │ │ │ -DDeeffiinniittiioonn HybridNonlinearISAM.h:93 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_r_e_o_r_d_e_r_C_o_u_n_t_e_r │ │ │ │ │ -int reorderCounter() const │ │ │ │ │ -TODO: comment. │ │ │ │ │ -DDeeffiinniittiioonn HybridNonlinearISAM.h:102 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_p_r_u_n_e │ │ │ │ │ -void prune(const size_t maxNumberLeaves) │ │ │ │ │ -Prune the underlying Bayes tree. │ │ │ │ │ -DDeeffiinniittiioonn HybridNonlinearISAM.h:87 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_r_e_o_r_d_e_r_I_n_t_e_r_v_a_l__ │ │ │ │ │ -int reorderInterval_ │ │ │ │ │ -The reordering interval and counter. │ │ │ │ │ -DDeeffiinniittiioonn HybridNonlinearISAM.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_r_e_o_r_d_e_r_I_n_t_e_r_v_a_l │ │ │ │ │ -int reorderInterval() const │ │ │ │ │ -get counters │ │ │ │ │ -DDeeffiinniittiioonn HybridNonlinearISAM.h:101 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_l_i_n_P_o_i_n_t__ │ │ │ │ │ -Values linPoint_ │ │ │ │ │ -The current linearization point. │ │ │ │ │ -DDeeffiinniittiioonn HybridNonlinearISAM.h:33 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_i_s_a_m__ │ │ │ │ │ -gtsam::HybridGaussianISAM isam_ │ │ │ │ │ -The internal iSAM object. │ │ │ │ │ -DDeeffiinniittiioonn HybridNonlinearISAM.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_e_l_i_m_i_n_a_t_i_o_n_F_u_n_c_t_i_o_n__ │ │ │ │ │ -HybridGaussianFactorGraph::Eliminate eliminationFunction_ │ │ │ │ │ -The elimination function. │ │ │ │ │ -DDeeffiinniittiioonn HybridNonlinearISAM.h:46 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_b_a_y_e_s_T_r_e_e │ │ │ │ │ -const HybridGaussianISAM & bayesTree() const │ │ │ │ │ -access the underlying bayes tree │ │ │ │ │ -DDeeffiinniittiioonn HybridNonlinearISAM.h:80 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_E_l_i_m_i_n_a_t_e │ │ │ │ │ -std::function< EliminationResult(const FactorGraphType &, const Ordering &)> │ │ │ │ │ -Eliminate │ │ │ │ │ -The function type that does a single dense elimination step on a subgraph. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph.h:89 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ +Aliases. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c_:_:_S_i_g_m_a │ │ │ │ │ +static shared_ptr Sigma(size_t dim, double sigma, bool smart=true) │ │ │ │ │ +An isotropic noise model created by specifying a standard devation sigma. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.cpp:597 │ │ │ │ │ +_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s │ │ │ │ │ +Parameters for Levenberg-Marquardt optimization. │ │ │ │ │ +DDeeffiinniittiioonn LevenbergMarquardtParams.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ _g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t │ │ │ │ │ +DDeeffiinniittiioonn BinaryMeasurement.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y │ │ │ │ │ +DDeeffiinniittiioonn TranslationRecovery.h:51 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y_:_:_b_u_i_l_d_G_r_a_p_h │ │ │ │ │ +NonlinearFactorGraph buildGraph(const std::vector< BinaryMeasurement< Unit3 > > │ │ │ │ │ +&relativeTranslations) const │ │ │ │ │ +Build the factor graph to do the optimization. │ │ │ │ │ +DDeeffiinniittiioonn TranslationRecovery.cpp:99 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y_:_:_S_i_m_u_l_a_t_e_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ +static TranslationEdges SimulateMeasurements(const Values &poses, const std:: │ │ │ │ │ +vector< KeyPair > &edges) │ │ │ │ │ +Simulate translation direction measurements. │ │ │ │ │ +DDeeffiinniittiioonn TranslationRecovery.cpp:216 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y_:_:_T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y │ │ │ │ │ +TranslationRecovery()=default │ │ │ │ │ +Default constructor. │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y_:_:_r_u_n │ │ │ │ │ +Values run(const TranslationEdges &relativeTranslations, const double │ │ │ │ │ +scale=1.0, const std::vector< BinaryMeasurement< Point3 > > │ │ │ │ │ +&betweenTranslations={}, const Values &initialValues=Values()) const │ │ │ │ │ +Build and optimize factor graph. │ │ │ │ │ +DDeeffiinniittiioonn TranslationRecovery.cpp:177 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y_:_:_a_d_d_P_r_i_o_r │ │ │ │ │ +void addPrior(const std::vector< BinaryMeasurement< Unit3 > > │ │ │ │ │ +&relativeTranslations, const double scale, const std::vector< │ │ │ │ │ +BinaryMeasurement< Point3 > > &betweenTranslations, NonlinearFactorGraph │ │ │ │ │ +*graph, const SharedNoiseModel &priorNoiseModel=noiseModel::Isotropic::Sigma(3, │ │ │ │ │ +0.01)) const │ │ │ │ │ +Add 3 factors to the graph: │ │ │ │ │ +DDeeffiinniittiioonn TranslationRecovery.cpp:111 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y_:_:_T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y │ │ │ │ │ +TranslationRecovery(const LevenbergMarquardtParams &lmParams) │ │ │ │ │ +Construct a new Translation Recovery object. │ │ │ │ │ +DDeeffiinniittiioonn TranslationRecovery.h:69 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y_:_:_i_n_i_t_i_a_l_i_z_e_R_a_n_d_o_m_l_y │ │ │ │ │ +Values initializeRandomly(const std::vector< BinaryMeasurement< Unit3 > > │ │ │ │ │ +&relativeTranslations, const std::vector< BinaryMeasurement< Point3 > > │ │ │ │ │ +&betweenTranslations, std::mt19937 *rng, const Values &initialValues=Values()) │ │ │ │ │ +const │ │ │ │ │ +Create random initial translations. │ │ │ │ │ +DDeeffiinniittiioonn TranslationRecovery.cpp:137 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_N_o_n_l_i_n_e_a_r_I_S_A_M_._h │ │ │ │ │ + * _s_f_m │ │ │ │ │ + * _T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00509.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01109.html │ │ │ │┄ Files 97% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridConditional.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Params.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces
│ │ │ │ -
HybridConditional.cpp File Reference
│ │ │ │ +
ISAM2Params.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ + │ │ │ │ +

Parameters for iSAM 2. │ │ │ │ +More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Mar 11, 2022
│ │ │ │ -
Author
Fan Jiang
│ │ │ │ +

Parameters for iSAM 2.

│ │ │ │ +
Author
Michael Kaess, Richard Roberts, Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,20 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -HybridConditional.cpp File Reference │ │ │ │ │ +ISAM2Params.cpp File Reference │ │ │ │ │ +Parameters for iSAM 2. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Mar 11, 2022 │ │ │ │ │ +Parameters for iSAM 2. │ │ │ │ │ Author │ │ │ │ │ - Fan Jiang │ │ │ │ │ + Michael Kaess, Richard Roberts, Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_._c_p_p │ │ │ │ │ + * _n_o_n_l_i_n_e_a_r │ │ │ │ │ + * _I_S_A_M_2_P_a_r_a_m_s_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00518_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01205_source.html │ │ │ │┄ Files 95% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridFactorGraph.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/TranslationFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
HybridFactorGraph.h
│ │ │ │ +
TranslationFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/hybrid/HybridFactor.h>
│ │ │ │ -
23#include <gtsam/inference/FactorGraph.h>
│ │ │ │ -
24
│ │ │ │ -
25#include <boost/format.hpp>
│ │ │ │ -
26#include <unordered_map>
│ │ │ │ +
12#pragma once
│ │ │ │ +
13
│ │ │ │ +
21#include <gtsam/geometry/Point3.h>
│ │ │ │ +
22#include <gtsam/geometry/Unit3.h>
│ │ │ │ +
23#include <gtsam/linear/NoiseModel.h>
│ │ │ │ +
24#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
25
│ │ │ │ +
26namespace gtsam {
│ │ │ │
27
│ │ │ │ -
28namespace gtsam {
│ │ │ │ -
29
│ │ │ │ -
30class DiscreteFactor;
│ │ │ │ -
31class Ordering;
│ │ │ │ -
32
│ │ │ │ -
33using SharedFactor = boost::shared_ptr<Factor>;
│ │ │ │ -
34
│ │ │ │ -
│ │ │ │ -
39class HybridFactorGraph : public FactorGraph<Factor> {
│ │ │ │ -
40 public:
│ │ │ │ -
41 using Base = FactorGraph<Factor>;
│ │ │ │ -
42 using This = HybridFactorGraph;
│ │ │ │ -
43 using shared_ptr = boost::shared_ptr<This>;
│ │ │ │ -
44
│ │ │ │ -
45 using Values = gtsam::Values;
│ │ │ │ -
46 using Indices = KeyVector;
│ │ │ │ -
47
│ │ │ │ -
48 public:
│ │ │ │ -
51
│ │ │ │ -
53 HybridFactorGraph() = default;
│ │ │ │ +
│ │ │ │ +
42class TranslationFactor : public NoiseModelFactorN<Point3, Point3> {
│ │ │ │ +
43 private:
│ │ │ │ +
44 typedef NoiseModelFactorN<Point3, Point3> Base;
│ │ │ │ +
45 Point3 measured_w_aZb_;
│ │ │ │ +
46
│ │ │ │ +
47 public:
│ │ │ │ +
49 TranslationFactor() {}
│ │ │ │ +
50
│ │ │ │ +
51 TranslationFactor(Key a, Key b, const Unit3& w_aZb,
│ │ │ │ +
52 const SharedNoiseModel& noiseModel)
│ │ │ │ +
53 : Base(noiseModel, a, b), measured_w_aZb_(w_aZb.point3()) {}
│ │ │ │
54
│ │ │ │ -
60 template <class DERIVEDFACTOR>
│ │ │ │ -
61 HybridFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
│ │ │ │ -
62
│ │ │ │ -
66
│ │ │ │ -
68 std::set<DiscreteKey> discreteKeys() const;
│ │ │ │ -
69
│ │ │ │ -
71 KeySet discreteKeySet() const;
│ │ │ │ -
72
│ │ │ │ -
74 std::unordered_map<Key, DiscreteKey> discreteKeyMap() const;
│ │ │ │ -
75
│ │ │ │ -
77 const KeySet continuousKeySet() const;
│ │ │ │ -
78
│ │ │ │ -
80};
│ │ │ │ -
│ │ │ │ -
81
│ │ │ │ -
82} // namespace gtsam
│ │ │ │ - │ │ │ │ -
FactorGraph.h
Factor Graph Base Class.
│ │ │ │ +
│ │ │ │ +
66 Vector evaluateError(
│ │ │ │ +
67 const Point3& Ta, const Point3& Tb,
│ │ │ │ +
68 boost::optional<Matrix&> H1 = boost::none,
│ │ │ │ +
69 boost::optional<Matrix&> H2 = boost::none) const override {
│ │ │ │ +
70 const Point3 dir = Tb - Ta;
│ │ │ │ +
71 Matrix33 H_predicted_dir;
│ │ │ │ +
72 const Point3 predicted = normalize(dir, H1 || H2 ? &H_predicted_dir : nullptr);
│ │ │ │ +
73 if (H1) *H1 = -H_predicted_dir;
│ │ │ │ +
74 if (H2) *H2 = H_predicted_dir;
│ │ │ │ +
75 return predicted - measured_w_aZb_;
│ │ │ │ +
76 }
│ │ │ │ +
│ │ │ │ +
77
│ │ │ │ +
78 private:
│ │ │ │ +
79 friend class boost::serialization::access;
│ │ │ │ +
80 template <class ARCHIVE>
│ │ │ │ +
81 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ +
82 ar& boost::serialization::make_nvp(
│ │ │ │ +
83 "Base", boost::serialization::base_object<Base>(*this));
│ │ │ │ +
84 }
│ │ │ │ +
85}; // \ TranslationFactor
│ │ │ │ +
│ │ │ │ +
86} // namespace gtsam
│ │ │ │ +
Point3.h
3D Point
│ │ │ │ + │ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ - │ │ │ │ -
gtsam::HybridFactorGraph
Hybrid Factor Graph Factor graph with utilities for hybrid factors.
Definition HybridFactorGraph.h:39
│ │ │ │ -
gtsam::HybridFactorGraph::HybridFactorGraph
HybridFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
Implicit copy/downcast constructor to override explicit template container constructor.
Definition HybridFactorGraph.h:61
│ │ │ │ -
gtsam::HybridFactorGraph::discreteKeySet
KeySet discreteKeySet() const
Get all the discrete keys in the factor graph, as a set.
Definition HybridFactorGraph.cpp:46
│ │ │ │ -
gtsam::HybridFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to This
Definition HybridFactorGraph.h:43
│ │ │ │ -
gtsam::HybridFactorGraph::discreteKeyMap
std::unordered_map< Key, DiscreteKey > discreteKeyMap() const
Get a map from Key to corresponding DiscreteKey.
Definition HybridFactorGraph.cpp:56
│ │ │ │ -
gtsam::HybridFactorGraph::continuousKeySet
const KeySet continuousKeySet() const
Get all the continuous keys in the factor graph.
Definition HybridFactorGraph.cpp:65
│ │ │ │ -
gtsam::HybridFactorGraph::HybridFactorGraph
HybridFactorGraph()=default
Default constructor.
│ │ │ │ -
gtsam::HybridFactorGraph::discreteKeys
std::set< DiscreteKey > discreteKeys() const
Get all the discrete keys in the factor graph.
Definition HybridFactorGraph.cpp:28
│ │ │ │ -
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ -
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ +
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ +
gtsam::normalize
Point3 normalize(const Point3 &p, OptionalJacobian< 3, 3 > H)
normalize, with optional Jacobian
Definition Point3.cpp:52
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
│ │ │ │ +
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ +
gtsam::TranslationFactor
Binary factor for a relative translation direction measurement w_aZb.
Definition TranslationFactor.h:42
│ │ │ │ +
gtsam::TranslationFactor::TranslationFactor
TranslationFactor()
default constructor
Definition TranslationFactor.h:49
│ │ │ │ +
gtsam::TranslationFactor::evaluateError
Vector evaluateError(const Point3 &Ta, const Point3 &Tb, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Caclulate error: (norm(Tb - Ta) - measurement) where Tb and Ta are Point3 translations and measuremen...
Definition TranslationFactor.h:66
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,116 +1,113 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -HybridFactorGraph.h │ │ │ │ │ +TranslationFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_F_a_c_t_o_r_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -24 │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ +12#pragma once │ │ │ │ │ +13 │ │ │ │ │ +21#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_3_._h> │ │ │ │ │ +22#include │ │ │ │ │ +23#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_N_o_i_s_e_M_o_d_e_l_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +25 │ │ │ │ │ +26namespace _g_t_s_a_m { │ │ │ │ │ 27 │ │ │ │ │ -28namespace _g_t_s_a_m { │ │ │ │ │ -29 │ │ │ │ │ -30class DiscreteFactor; │ │ │ │ │ -31class Ordering; │ │ │ │ │ -32 │ │ │ │ │ -33using SharedFactor = boost::shared_ptr; │ │ │ │ │ -34 │ │ │ │ │ -_3_9class _H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h : public _F_a_c_t_o_r_G_r_a_p_h { │ │ │ │ │ -40 public: │ │ │ │ │ -41 using _B_a_s_e = _F_a_c_t_o_r_G_r_a_p_h_<_F_a_c_t_o_r_>; │ │ │ │ │ -_4_2 using _T_h_i_s = _H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h; │ │ │ │ │ -_4_3 using _s_h_a_r_e_d___p_t_r = boost::shared_ptr; │ │ │ │ │ -44 │ │ │ │ │ -_4_5 using _V_a_l_u_e_s = _g_t_s_a_m_:_:_V_a_l_u_e_s; │ │ │ │ │ -46 using Indices = _K_e_y_V_e_c_t_o_r; │ │ │ │ │ -47 │ │ │ │ │ -48 public: │ │ │ │ │ -51 │ │ │ │ │ -_5_3 _H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h() = default; │ │ │ │ │ +_4_2class _T_r_a_n_s_l_a_t_i_o_n_F_a_c_t_o_r : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +43 private: │ │ │ │ │ +44 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_P_o_i_n_t_3_,_ _P_o_i_n_t_3_> _B_a_s_e; │ │ │ │ │ +45 _P_o_i_n_t_3 measured_w_aZb_; │ │ │ │ │ +46 │ │ │ │ │ +47 public: │ │ │ │ │ +_4_9 _T_r_a_n_s_l_a_t_i_o_n_F_a_c_t_o_r() {} │ │ │ │ │ +50 │ │ │ │ │ +51 _T_r_a_n_s_l_a_t_i_o_n_F_a_c_t_o_r(_K_e_y a, _K_e_y b, const _U_n_i_t_3& w_aZb, │ │ │ │ │ +52 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& noiseModel) │ │ │ │ │ +53 : Base(noiseModel, a, b), measured_w_aZb_(w_aZb.point3()) {} │ │ │ │ │ 54 │ │ │ │ │ -60 template │ │ │ │ │ -_6_1 _H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h(const _F_a_c_t_o_r_G_r_a_p_h_<_D_E_R_I_V_E_D_F_A_C_T_O_R_>& graph) : _B_a_s_e(graph) {} │ │ │ │ │ -62 │ │ │ │ │ -66 │ │ │ │ │ -68 std::set _d_i_s_c_r_e_t_e_K_e_y_s() const; │ │ │ │ │ -69 │ │ │ │ │ -71 _K_e_y_S_e_t _d_i_s_c_r_e_t_e_K_e_y_S_e_t() const; │ │ │ │ │ -72 │ │ │ │ │ -74 std::unordered_map _d_i_s_c_r_e_t_e_K_e_y_M_a_p() const; │ │ │ │ │ -75 │ │ │ │ │ -77 const _K_e_y_S_e_t _c_o_n_t_i_n_u_o_u_s_K_e_y_S_e_t() const; │ │ │ │ │ -78 │ │ │ │ │ -80}; │ │ │ │ │ -81 │ │ │ │ │ -82} // namespace gtsam │ │ │ │ │ -_H_y_b_r_i_d_F_a_c_t_o_r_._h │ │ │ │ │ -_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Factor Graph Base Class. │ │ │ │ │ +_6_6 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r( │ │ │ │ │ +67 const _P_o_i_n_t_3& Ta, const _P_o_i_n_t_3& Tb, │ │ │ │ │ +68 boost::optional H1 = boost::none, │ │ │ │ │ +69 boost::optional H2 = boost::none) const override { │ │ │ │ │ +70 const _P_o_i_n_t_3 dir = Tb - Ta; │ │ │ │ │ +71 Matrix33 H_predicted_dir; │ │ │ │ │ +72 const _P_o_i_n_t_3 predicted = _n_o_r_m_a_l_i_z_e(dir, H1 || H2 ? &H_predicted_dir : │ │ │ │ │ +nullptr); │ │ │ │ │ +73 if (H1) *H1 = -H_predicted_dir; │ │ │ │ │ +74 if (H2) *H2 = H_predicted_dir; │ │ │ │ │ +75 return predicted - measured_w_aZb_; │ │ │ │ │ +76 } │ │ │ │ │ +77 │ │ │ │ │ +78 private: │ │ │ │ │ +79 friend class boost::serialization::access; │ │ │ │ │ +80 template │ │ │ │ │ +81 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ +82 ar& boost::serialization::make_nvp( │ │ │ │ │ +83 "Base", boost::serialization::base_object(*this)); │ │ │ │ │ +84 } │ │ │ │ │ +85}; // \ TranslationFactor │ │ │ │ │ +86} // namespace gtsam │ │ │ │ │ +_P_o_i_n_t_3_._h │ │ │ │ │ +3D Point │ │ │ │ │ +_N_o_i_s_e_M_o_d_e_l_._h │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ +Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -Hybrid Factor Graph Factor graph with utilities for hybrid factors. │ │ │ │ │ -DDeeffiinniittiioonn HybridFactorGraph.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h_:_:_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -HybridFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph) │ │ │ │ │ -Implicit copy/downcast constructor to override explicit template container │ │ │ │ │ -constructor. │ │ │ │ │ -DDeeffiinniittiioonn HybridFactorGraph.h:61 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h_:_:_d_i_s_c_r_e_t_e_K_e_y_S_e_t │ │ │ │ │ -KeySet discreteKeySet() const │ │ │ │ │ -Get all the discrete keys in the factor graph, as a set. │ │ │ │ │ -DDeeffiinniittiioonn HybridFactorGraph.cpp:46 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to This │ │ │ │ │ -DDeeffiinniittiioonn HybridFactorGraph.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h_:_:_d_i_s_c_r_e_t_e_K_e_y_M_a_p │ │ │ │ │ -std::unordered_map< Key, DiscreteKey > discreteKeyMap() const │ │ │ │ │ -Get a map from Key to corresponding DiscreteKey. │ │ │ │ │ -DDeeffiinniittiioonn HybridFactorGraph.cpp:56 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h_:_:_c_o_n_t_i_n_u_o_u_s_K_e_y_S_e_t │ │ │ │ │ -const KeySet continuousKeySet() const │ │ │ │ │ -Get all the continuous keys in the factor graph. │ │ │ │ │ -DDeeffiinniittiioonn HybridFactorGraph.cpp:65 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h_:_:_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -HybridFactorGraph()=default │ │ │ │ │ -Default constructor. │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h_:_:_d_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ -std::set< DiscreteKey > discreteKeys() const │ │ │ │ │ -Get all the discrete keys in the factor graph. │ │ │ │ │ -DDeeffiinniittiioonn HybridFactorGraph.cpp:28 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ -nodes. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ +Vector3 Point3 │ │ │ │ │ +As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ +typedef Point3 to Vector3... │ │ │ │ │ +DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ +Aliases. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_r_m_a_l_i_z_e │ │ │ │ │ +Point3 normalize(const Point3 &p, OptionalJacobian< 3, 3 > H) │ │ │ │ │ +normalize, with optional Jacobian │ │ │ │ │ +DDeeffiinniittiioonn Point3.cpp:52 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_U_n_i_t_3 │ │ │ │ │ +Represents a 3D point on a unit sphere. │ │ │ │ │ +DDeeffiinniittiioonn Unit3.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ +A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ +variables. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_a_n_s_l_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +Binary factor for a relative translation direction measurement w_aZb. │ │ │ │ │ +DDeeffiinniittiioonn TranslationFactor.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_a_n_s_l_a_t_i_o_n_F_a_c_t_o_r_:_:_T_r_a_n_s_l_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +TranslationFactor() │ │ │ │ │ +default constructor │ │ │ │ │ +DDeeffiinniittiioonn TranslationFactor.h:49 │ │ │ │ │ +_g_t_s_a_m_:_:_T_r_a_n_s_l_a_t_i_o_n_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ +Vector evaluateError(const Point3 &Ta, const Point3 &Tb, boost::optional< │ │ │ │ │ +Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const │ │ │ │ │ +override │ │ │ │ │ +Caclulate error: (norm(Tb - Ta) - measurement) where Tb and Ta are Point3 │ │ │ │ │ +translations and measuremen... │ │ │ │ │ +DDeeffiinniittiioonn TranslationFactor.h:66 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ + * _s_f_m │ │ │ │ │ + * _T_r_a_n_s_l_a_t_i_o_n_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00524_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00374_source.html │ │ │ │┄ Files 93% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridBayesNet.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3_S2Stereo.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
HybridBayesNet.h
│ │ │ │ +
Cal3_S2Stereo.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ -
2 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ -
3 * Atlanta, Georgia 30332-0415
│ │ │ │ -
4 * All Rights Reserved
│ │ │ │ -
5 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ -
6 * See LICENSE for the license information
│ │ │ │ -
7 * -------------------------------------------------------------------------- */
│ │ │ │ -
8
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +
2
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
4 * Atlanta, Georgia 30332-0415
│ │ │ │ +
5 * All Rights Reserved
│ │ │ │ +
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
7
│ │ │ │ +
8 * See LICENSE for the license information
│ │ │ │ +
9
│ │ │ │ +
10 * -------------------------------------------------------------------------- */
│ │ │ │ +
11
│ │ │ │
18#pragma once
│ │ │ │
19
│ │ │ │ -
20#include <gtsam/discrete/DecisionTreeFactor.h>
│ │ │ │ -
21#include <gtsam/global_includes.h>
│ │ │ │ -
22#include <gtsam/hybrid/HybridConditional.h>
│ │ │ │ -
23#include <gtsam/hybrid/HybridValues.h>
│ │ │ │ -
24#include <gtsam/inference/BayesNet.h>
│ │ │ │ -
25#include <gtsam/linear/GaussianBayesNet.h>
│ │ │ │ -
26
│ │ │ │ -
27namespace gtsam {
│ │ │ │ -
28
│ │ │ │ -
│ │ │ │ -
35class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
│ │ │ │ -
36 public:
│ │ │ │ -
37 using Base = BayesNet<HybridConditional>;
│ │ │ │ -
38 using This = HybridBayesNet;
│ │ │ │ -
39 using ConditionalType = HybridConditional;
│ │ │ │ -
40 using shared_ptr = boost::shared_ptr<HybridBayesNet>;
│ │ │ │ -
41 using sharedConditional = boost::shared_ptr<ConditionalType>;
│ │ │ │ +
20#include <gtsam/geometry/Cal3_S2.h>
│ │ │ │ +
21#include <iosfwd>
│ │ │ │ +
22
│ │ │ │ +
23namespace gtsam {
│ │ │ │ +
24
│ │ │ │ +
│ │ │ │ +
30class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
│ │ │ │ +
31 private:
│ │ │ │ +
32 double b_ = 1.0f;
│ │ │ │ +
33
│ │ │ │ +
34 public:
│ │ │ │ +
35 enum { dimension = 6 };
│ │ │ │ +
36
│ │ │ │ +
38 using shared_ptr = boost::shared_ptr<Cal3_S2Stereo>;
│ │ │ │ +
39
│ │ │ │
42
│ │ │ │ +
44 Cal3_S2Stereo() = default;
│ │ │ │
45
│ │ │ │ -
47 HybridBayesNet() = default;
│ │ │ │ -
48
│ │ │ │ -
52
│ │ │ │ -
54 void print(const std::string &s = "", const KeyFormatter &formatter =
│ │ │ │ -
55 DefaultKeyFormatter) const override;
│ │ │ │ -
56
│ │ │ │ -
58 bool equals(const This &fg, double tol = 1e-9) const;
│ │ │ │ -
59
│ │ │ │ -
63
│ │ │ │ -
│ │ │ │ -
69 void push_back(boost::shared_ptr<HybridConditional> conditional) {
│ │ │ │ -
70 factors_.push_back(conditional);
│ │ │ │ -
71 }
│ │ │ │ -
│ │ │ │ -
72
│ │ │ │ -
81 template <class Conditional>
│ │ │ │ -
│ │ │ │ -
82 void emplace_back(Conditional *conditional) {
│ │ │ │ -
83 factors_.push_back(boost::make_shared<HybridConditional>(
│ │ │ │ -
84 boost::shared_ptr<Conditional>(conditional)));
│ │ │ │ -
85 }
│ │ │ │ +
│ │ │ │ +
47 Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b)
│ │ │ │ +
48 : Cal3_S2(fx, fy, s, u0, v0), b_(b) {}
│ │ │ │ +
│ │ │ │ +
49
│ │ │ │ +
│ │ │ │ +
51 Cal3_S2Stereo(const Vector6& d)
│ │ │ │ +
52 : Cal3_S2(d(0), d(1), d(2), d(3), d(4)), b_(d(5)) {}
│ │ │ │ +
│ │ │ │ +
53
│ │ │ │ +
│ │ │ │ +
55 Cal3_S2Stereo(double fov, int w, int h, double b)
│ │ │ │ +
56 : Cal3_S2(fov, w, h), b_(b) {}
│ │ │ │
│ │ │ │ +
57
│ │ │ │ +
59
│ │ │ │ +
67 Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none,
│ │ │ │ +
68 OptionalJacobian<2, 2> Dp = boost::none) const;
│ │ │ │ +
69
│ │ │ │ +
77 Point2 calibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none,
│ │ │ │ +
78 OptionalJacobian<2, 2> Dp = boost::none) const;
│ │ │ │ +
79
│ │ │ │ +
85 Vector3 calibrate(const Vector3& p) const { return Cal3_S2::calibrate(p); }
│ │ │ │
86
│ │ │ │ -
│ │ │ │ -
99 void push_back(HybridConditional &&conditional) {
│ │ │ │ -
100 factors_.push_back(
│ │ │ │ -
101 boost::make_shared<HybridConditional>(std::move(conditional)));
│ │ │ │ -
102 }
│ │ │ │ -
│ │ │ │ +
89
│ │ │ │ +
91 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
│ │ │ │ +
92 const Cal3_S2Stereo& cal);
│ │ │ │ +
93
│ │ │ │ +
95 void print(const std::string& s = "") const override;
│ │ │ │ +
96
│ │ │ │ +
98 bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const;
│ │ │ │ +
99
│ │ │ │
103
│ │ │ │ -
111 GaussianBayesNet choose(const DiscreteValues &assignment) const;
│ │ │ │ +
105 const Cal3_S2& calibration() const { return *this; }
│ │ │ │ +
106
│ │ │ │ +
108 Matrix3 K() const override { return Cal3_S2::K(); }
│ │ │ │ +
109
│ │ │ │ +
111 inline double baseline() const { return b_; }
│ │ │ │
112
│ │ │ │ -
114 double evaluate(const HybridValues &values) const;
│ │ │ │ -
115
│ │ │ │ -
│ │ │ │ -
117 double operator()(const HybridValues &values) const {
│ │ │ │ -
118 return evaluate(values);
│ │ │ │ -
119 }
│ │ │ │ -
│ │ │ │ -
120
│ │ │ │ -
128 HybridValues optimize() const;
│ │ │ │ +
│ │ │ │ +
114 Vector6 vector() const {
│ │ │ │ +
115 Vector6 v;
│ │ │ │ +
116 v << Cal3_S2::vector(), b_;
│ │ │ │ +
117 return v;
│ │ │ │ +
118 }
│ │ │ │ +
│ │ │ │ +
119
│ │ │ │ +
123
│ │ │ │ +
125 inline size_t dim() const override { return Dim(); }
│ │ │ │ +
126
│ │ │ │ +
128 inline static size_t Dim() { return dimension; }
│ │ │ │
129
│ │ │ │ -
137 VectorValues optimize(const DiscreteValues &assignment) const;
│ │ │ │ -
138
│ │ │ │ -
144 DecisionTreeFactor::shared_ptr discreteConditionals() const;
│ │ │ │ -
145
│ │ │ │ -
158 HybridValues sample(const HybridValues &given, std::mt19937_64 *rng) const;
│ │ │ │ -
159
│ │ │ │ -
170 HybridValues sample(std::mt19937_64 *rng) const;
│ │ │ │ -
171
│ │ │ │ -
178 HybridValues sample(const HybridValues &given) const;
│ │ │ │ -
179
│ │ │ │ -
185 HybridValues sample() const;
│ │ │ │ -
186
│ │ │ │ -
188 HybridBayesNet prune(size_t maxNrLeaves);
│ │ │ │ -
189
│ │ │ │ -
197 AlgebraicDecisionTree<Key> logProbability(
│ │ │ │ -
198 const VectorValues &continuousValues) const;
│ │ │ │ -
199
│ │ │ │ -
200 using BayesNet::logProbability; // expose HybridValues version
│ │ │ │ -
201
│ │ │ │ -
212 AlgebraicDecisionTree<Key> evaluate(
│ │ │ │ -
213 const VectorValues &continuousValues) const;
│ │ │ │ -
214
│ │ │ │ -
219 HybridGaussianFactorGraph toFactorGraph(
│ │ │ │ -
220 const VectorValues &measurements) const;
│ │ │ │ -
222
│ │ │ │ -
223 private:
│ │ │ │ -
229 void updateDiscreteConditionals(const DecisionTreeFactor &prunedDecisionTree);
│ │ │ │ -
230
│ │ │ │ -
232 friend class boost::serialization::access;
│ │ │ │ -
233 template <class ARCHIVE>
│ │ │ │ -
234 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ -
235 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
236 }
│ │ │ │ -
237};
│ │ │ │ -
│ │ │ │ -
238
│ │ │ │ -
240template <>
│ │ │ │ -
241struct traits<HybridBayesNet> : public Testable<HybridBayesNet> {};
│ │ │ │ -
242
│ │ │ │ -
243} // namespace gtsam
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
BayesNet.h
Bayes network.
│ │ │ │ -
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
│ │ │ │ -
global_includes.h
Included from all GTSAM files.
│ │ │ │ +
│ │ │ │ +
131 inline Cal3_S2Stereo retract(const Vector& d) const {
│ │ │ │ +
132 return Cal3_S2Stereo(fx() + d(0), fy() + d(1), skew() + d(2), px() + d(3),
│ │ │ │ +
133 py() + d(4), b_ + d(5));
│ │ │ │ +
134 }
│ │ │ │ +
│ │ │ │ +
135
│ │ │ │ +
│ │ │ │ +
137 Vector6 localCoordinates(const Cal3_S2Stereo& T2) const {
│ │ │ │ +
138 return T2.vector() - vector();
│ │ │ │ +
139 }
│ │ │ │ +
│ │ │ │ +
140
│ │ │ │ +
144
│ │ │ │ +
145 private:
│ │ │ │ +
147 friend class boost::serialization::access;
│ │ │ │ +
148 template <class Archive>
│ │ │ │ +
149 void serialize(Archive& ar, const unsigned int /*version*/) {
│ │ │ │ +
150 ar& boost::serialization::make_nvp(
│ │ │ │ +
151 "Cal3_S2", boost::serialization::base_object<Cal3_S2>(*this));
│ │ │ │ +
152 ar& BOOST_SERIALIZATION_NVP(b_);
│ │ │ │ +
153 }
│ │ │ │ +
155};
│ │ │ │ +
│ │ │ │ +
156
│ │ │ │ +
157// Define GTSAM traits
│ │ │ │ +
158template <>
│ │ │ │ +
159struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {};
│ │ │ │ +
160
│ │ │ │ +
161template <>
│ │ │ │ +
│ │ │ │ +
162struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
│ │ │ │ +
163};
│ │ │ │ +
│ │ │ │ +
164
│ │ │ │ +
165} // \ namespace gtsam
│ │ │ │ +
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
│ │ │ │ +
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::AlgebraicDecisionTree
An algebraic decision tree fixes the range of a DecisionTree to double.
Definition AlgebraicDecisionTree.h:38
│ │ │ │ -
gtsam::DecisionTreeFactor
A discrete probabilistic factor.
Definition DecisionTreeFactor.h:45
│ │ │ │ -
gtsam::DiscreteValues
A map from keys to values.
Definition DiscreteValues.h:34
│ │ │ │ -
gtsam::HybridBayesNet
A hybrid Bayes net is a collection of HybridConditionals, which can have discrete conditionals,...
Definition HybridBayesNet.h:35
│ │ │ │ -
gtsam::HybridBayesNet::push_back
void push_back(boost::shared_ptr< HybridConditional > conditional)
Add a hybrid conditional using a shared_ptr.
Definition HybridBayesNet.h:69
│ │ │ │ -
gtsam::HybridBayesNet::emplace_back
void emplace_back(Conditional *conditional)
Preferred: add a conditional directly using a pointer.
Definition HybridBayesNet.h:82
│ │ │ │ -
gtsam::HybridBayesNet::push_back
void push_back(HybridConditional &&conditional)
Add a conditional using a shared_ptr, using implicit conversion to a HybridConditional.
Definition HybridBayesNet.h:99
│ │ │ │ -
gtsam::HybridBayesNet::HybridBayesNet
HybridBayesNet()=default
Construct empty Bayes net.
│ │ │ │ -
gtsam::HybridBayesNet::operator()
double operator()(const HybridValues &values) const
Evaluate hybrid probability density for given HybridValues, sugar.
Definition HybridBayesNet.h:117
│ │ │ │ -
gtsam::HybridConditional
Hybrid Conditional Density.
Definition HybridConditional.h:62
│ │ │ │ -
gtsam::HybridGaussianFactorGraph
Definition HybridGaussianFactorGraph.h:102
│ │ │ │ -
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ -
gtsam::BayesNet
A BayesNet is a tree of conditionals, stored in elimination order.
Definition BayesNet.h:35
│ │ │ │ -
gtsam::Conditional
Definition Conditional.h:64
│ │ │ │ -
gtsam::GaussianBayesNet
GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals.
Definition GaussianBayesNet.h:36
│ │ │ │ -
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition Cal3_S2.h:34
│ │ │ │ +
gtsam::Cal3_S2Stereo
The most common 5DOF 3D->2D calibration, stereo version.
Definition Cal3_S2Stereo.h:30
│ │ │ │ +
gtsam::Cal3_S2Stereo::vector
Vector6 vector() const
vectorized form (column-wise)
Definition Cal3_S2Stereo.h:114
│ │ │ │ +
gtsam::Cal3_S2Stereo::Cal3_S2Stereo
Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b)
constructor from doubles
Definition Cal3_S2Stereo.h:47
│ │ │ │ +
gtsam::Cal3_S2Stereo::localCoordinates
Vector6 localCoordinates(const Cal3_S2Stereo &T2) const
Unretraction for the calibration.
Definition Cal3_S2Stereo.h:137
│ │ │ │ +
gtsam::Cal3_S2Stereo::calibration
const Cal3_S2 & calibration() const
return calibration, same for left and right
Definition Cal3_S2Stereo.h:105
│ │ │ │ +
gtsam::Cal3_S2Stereo::dim
size_t dim() const override
return DOF, dimensionality of tangent space
Definition Cal3_S2Stereo.h:125
│ │ │ │ +
gtsam::Cal3_S2Stereo::calibrate
Vector3 calibrate(const Vector3 &p) const
Convert homogeneous image coordinates to intrinsic coordinates.
Definition Cal3_S2Stereo.h:85
│ │ │ │ +
gtsam::Cal3_S2Stereo::retract
Cal3_S2Stereo retract(const Vector &d) const
Given 6-dim tangent vector, create new calibration.
Definition Cal3_S2Stereo.h:131
│ │ │ │ +
gtsam::Cal3_S2Stereo::K
Matrix3 K() const override
return calibration matrix K, same for left and right
Definition Cal3_S2Stereo.h:108
│ │ │ │ +
gtsam::Cal3_S2Stereo::Dim
static size_t Dim()
return DOF, dimensionality of tangent space
Definition Cal3_S2Stereo.h:128
│ │ │ │ +
gtsam::Cal3_S2Stereo::Cal3_S2Stereo
Cal3_S2Stereo(double fov, int w, int h, double b)
easy constructor; field-of-view in degrees, assumes zero skew
Definition Cal3_S2Stereo.h:55
│ │ │ │ +
gtsam::Cal3_S2Stereo::baseline
double baseline() const
return baseline
Definition Cal3_S2Stereo.h:111
│ │ │ │ +
gtsam::Cal3_S2Stereo::Cal3_S2Stereo
Cal3_S2Stereo(const Vector6 &d)
constructor from vector
Definition Cal3_S2Stereo.h:51
│ │ │ │ +
gtsam::Cal3_S2Stereo::Cal3_S2Stereo
Cal3_S2Stereo()=default
default calibration leaves coordinates unchanged
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,203 +1,204 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -HybridBayesNet.h │ │ │ │ │ +Cal3_S2Stereo.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ -2 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ -3 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -4 * All Rights Reserved │ │ │ │ │ -5 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ -6 * See LICENSE for the license information │ │ │ │ │ -7 * ------------------------------------------------------------------------- │ │ │ │ │ +2 │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +5 * All Rights Reserved │ │ │ │ │ +6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +7 │ │ │ │ │ +8 * See LICENSE for the license information │ │ │ │ │ +9 │ │ │ │ │ +10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ -8 │ │ │ │ │ +11 │ │ │ │ │ 18#pragma once │ │ │ │ │ 19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_V_a_l_u_e_s_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ -26 │ │ │ │ │ -27namespace _g_t_s_a_m { │ │ │ │ │ -28 │ │ │ │ │ -_3_5class GTSAM_EXPORT _H_y_b_r_i_d_B_a_y_e_s_N_e_t : public _B_a_y_e_s_N_e_t { │ │ │ │ │ -36 public: │ │ │ │ │ -37 using _B_a_s_e = _B_a_y_e_s_N_e_t_<_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_>; │ │ │ │ │ -38 using _T_h_i_s = _H_y_b_r_i_d_B_a_y_e_s_N_e_t; │ │ │ │ │ -39 using _C_o_n_d_i_t_i_o_n_a_l_T_y_p_e = _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l; │ │ │ │ │ -40 using shared_ptr = boost::shared_ptr; │ │ │ │ │ -41 using sharedConditional = boost::shared_ptr; │ │ │ │ │ +20#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3___S_2_._h> │ │ │ │ │ +21#include │ │ │ │ │ +22 │ │ │ │ │ +23namespace _g_t_s_a_m { │ │ │ │ │ +24 │ │ │ │ │ +_3_0class GTSAM_EXPORT _C_a_l_3___S_2_S_t_e_r_e_o : public _C_a_l_3___S_2 { │ │ │ │ │ +31 private: │ │ │ │ │ +32 double b_ = 1.0f; │ │ │ │ │ +33 │ │ │ │ │ +34 public: │ │ │ │ │ +35 enum { dimension = 6 }; │ │ │ │ │ +36 │ │ │ │ │ +38 using shared_ptr = boost::shared_ptr; │ │ │ │ │ +39 │ │ │ │ │ 42 │ │ │ │ │ +_4_4 _C_a_l_3___S_2_S_t_e_r_e_o() = default; │ │ │ │ │ 45 │ │ │ │ │ -_4_7 _H_y_b_r_i_d_B_a_y_e_s_N_e_t() = default; │ │ │ │ │ -48 │ │ │ │ │ -52 │ │ │ │ │ -54 void _p_r_i_n_t(const std::string &s = "", const _K_e_y_F_o_r_m_a_t_t_e_r &formatter = │ │ │ │ │ -55 DefaultKeyFormatter) const override; │ │ │ │ │ -56 │ │ │ │ │ -58 bool _e_q_u_a_l_s(const _T_h_i_s &fg, double tol = 1e-9) const; │ │ │ │ │ +_4_7 _C_a_l_3___S_2_S_t_e_r_e_o(double fx, double fy, double s, double u0, double v0, double │ │ │ │ │ +b) │ │ │ │ │ +48 : _C_a_l_3___S_2(fx, fy, s, u0, v0), b_(b) {} │ │ │ │ │ +49 │ │ │ │ │ +_5_1 _C_a_l_3___S_2_S_t_e_r_e_o(const Vector6& d) │ │ │ │ │ +52 : _C_a_l_3___S_2(d(0), d(1), d(2), d(3), d(4)), b_(d(5)) {} │ │ │ │ │ +53 │ │ │ │ │ +_5_5 _C_a_l_3___S_2_S_t_e_r_e_o(double fov, int w, int h, double b) │ │ │ │ │ +56 : _C_a_l_3___S_2(fov, w, h), b_(b) {} │ │ │ │ │ +57 │ │ │ │ │ 59 │ │ │ │ │ -63 │ │ │ │ │ -_6_9 void _p_u_s_h___b_a_c_k(boost::shared_ptr conditional) { │ │ │ │ │ -70 factors_.push_back(conditional); │ │ │ │ │ -71 } │ │ │ │ │ -72 │ │ │ │ │ -81 template │ │ │ │ │ -_8_2 void _e_m_p_l_a_c_e___b_a_c_k(_C_o_n_d_i_t_i_o_n_a_l *conditional) { │ │ │ │ │ -83 factors_.push_back(boost::make_shared( │ │ │ │ │ -84 boost::shared_ptr(conditional))); │ │ │ │ │ -85 } │ │ │ │ │ +67 _P_o_i_n_t_2 uncalibrate(const _P_o_i_n_t_2& p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> Dcal = boost:: │ │ │ │ │ +none, │ │ │ │ │ +68 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dp = boost::none) const; │ │ │ │ │ +69 │ │ │ │ │ +77 _P_o_i_n_t_2 calibrate(const _P_o_i_n_t_2& p, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _6_> Dcal = boost::none, │ │ │ │ │ +78 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_2_,_ _2_> Dp = boost::none) const; │ │ │ │ │ +79 │ │ │ │ │ +_8_5 Vector3 _c_a_l_i_b_r_a_t_e(const Vector3& p) const { return Cal3_S2::calibrate(p); } │ │ │ │ │ 86 │ │ │ │ │ -_9_9 void _p_u_s_h___b_a_c_k(_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l &&conditional) { │ │ │ │ │ -100 factors_.push_back( │ │ │ │ │ -101 boost::make_shared(std::move(conditional))); │ │ │ │ │ -102 } │ │ │ │ │ +89 │ │ │ │ │ +91 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, │ │ │ │ │ +92 const _C_a_l_3___S_2_S_t_e_r_e_o& cal); │ │ │ │ │ +93 │ │ │ │ │ +95 void _p_r_i_n_t(const std::string& s = "") const override; │ │ │ │ │ +96 │ │ │ │ │ +98 bool _e_q_u_a_l_s(const _C_a_l_3___S_2_S_t_e_r_e_o& other, double tol = 10e-9) const; │ │ │ │ │ +99 │ │ │ │ │ 103 │ │ │ │ │ -111 _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t choose(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s &assignment) const; │ │ │ │ │ +_1_0_5 const _C_a_l_3___S_2& _c_a_l_i_b_r_a_t_i_o_n() const { return *this; } │ │ │ │ │ +106 │ │ │ │ │ +_1_0_8 Matrix3 _K() const override { return Cal3_S2::K(); } │ │ │ │ │ +109 │ │ │ │ │ +_1_1_1 inline double _b_a_s_e_l_i_n_e() const { return b_; } │ │ │ │ │ 112 │ │ │ │ │ -114 double evaluate(const _H_y_b_r_i_d_V_a_l_u_e_s &values) const; │ │ │ │ │ -115 │ │ │ │ │ -_1_1_7 double _o_p_e_r_a_t_o_r_(_)(const _H_y_b_r_i_d_V_a_l_u_e_s &values) const { │ │ │ │ │ -118 return evaluate(values); │ │ │ │ │ -119 } │ │ │ │ │ -120 │ │ │ │ │ -128 _H_y_b_r_i_d_V_a_l_u_e_s _o_p_t_i_m_i_z_e() const; │ │ │ │ │ +_1_1_4 Vector6 _v_e_c_t_o_r() const { │ │ │ │ │ +115 Vector6 v; │ │ │ │ │ +116 v << Cal3_S2::vector(), b_; │ │ │ │ │ +117 return v; │ │ │ │ │ +118 } │ │ │ │ │ +119 │ │ │ │ │ +123 │ │ │ │ │ +_1_2_5 inline size_t _d_i_m() const override { return Dim(); } │ │ │ │ │ +126 │ │ │ │ │ +_1_2_8 inline static size_t _D_i_m() { return dimension; } │ │ │ │ │ 129 │ │ │ │ │ -137 _V_e_c_t_o_r_V_a_l_u_e_s _o_p_t_i_m_i_z_e(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s &assignment) const; │ │ │ │ │ -138 │ │ │ │ │ -144 DecisionTreeFactor::shared_ptr discreteConditionals() const; │ │ │ │ │ -145 │ │ │ │ │ -158 _H_y_b_r_i_d_V_a_l_u_e_s sample(const _H_y_b_r_i_d_V_a_l_u_e_s &given, std::mt19937_64 *rng) const; │ │ │ │ │ -159 │ │ │ │ │ -170 _H_y_b_r_i_d_V_a_l_u_e_s sample(std::mt19937_64 *rng) const; │ │ │ │ │ -171 │ │ │ │ │ -178 _H_y_b_r_i_d_V_a_l_u_e_s sample(const _H_y_b_r_i_d_V_a_l_u_e_s &given) const; │ │ │ │ │ -179 │ │ │ │ │ -185 _H_y_b_r_i_d_V_a_l_u_e_s sample() const; │ │ │ │ │ -186 │ │ │ │ │ -188 _H_y_b_r_i_d_B_a_y_e_s_N_e_t prune(size_t maxNrLeaves); │ │ │ │ │ -189 │ │ │ │ │ -197 _A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_<_K_e_y_> logProbability( │ │ │ │ │ -198 const _V_e_c_t_o_r_V_a_l_u_e_s &continuousValues) const; │ │ │ │ │ -199 │ │ │ │ │ -200 using BayesNet::logProbability; // expose HybridValues version │ │ │ │ │ -201 │ │ │ │ │ -212 _A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_<_K_e_y_> evaluate( │ │ │ │ │ -213 const _V_e_c_t_o_r_V_a_l_u_e_s &continuousValues) const; │ │ │ │ │ -214 │ │ │ │ │ -219 _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h toFactorGraph( │ │ │ │ │ -220 const _V_e_c_t_o_r_V_a_l_u_e_s &measurements) const; │ │ │ │ │ -222 │ │ │ │ │ -223 private: │ │ │ │ │ -229 void updateDiscreteConditionals(const _D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r │ │ │ │ │ -&prunedDecisionTree); │ │ │ │ │ -230 │ │ │ │ │ -_2_3_2 friend class boost::serialization::access; │ │ │ │ │ -233 template │ │ │ │ │ -234 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ -235 ar &BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -236 } │ │ │ │ │ -237}; │ │ │ │ │ -238 │ │ │ │ │ -240template <> │ │ │ │ │ -_2_4_1struct _t_r_a_i_t_s<_H_y_b_r_i_d_B_a_y_e_s_N_e_t> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ -242 │ │ │ │ │ -243} // namespace gtsam │ │ │ │ │ -_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r_._h │ │ │ │ │ -_H_y_b_r_i_d_V_a_l_u_e_s_._h │ │ │ │ │ -_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ -_B_a_y_e_s_N_e_t_._h │ │ │ │ │ -Bayes network. │ │ │ │ │ -_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h │ │ │ │ │ -Chordal Bayes Net, the result of eliminating a factor graph. │ │ │ │ │ -_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ -Included from all GTSAM files. │ │ │ │ │ +_1_3_1 inline _C_a_l_3___S_2_S_t_e_r_e_o _r_e_t_r_a_c_t(const Vector& d) const { │ │ │ │ │ +132 return _C_a_l_3___S_2_S_t_e_r_e_o(fx() + d(0), fy() + d(1), skew() + d(2), px() + d(3), │ │ │ │ │ +133 py() + d(4), b_ + d(5)); │ │ │ │ │ +134 } │ │ │ │ │ +135 │ │ │ │ │ +_1_3_7 Vector6 _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(const _C_a_l_3___S_2_S_t_e_r_e_o& T2) const { │ │ │ │ │ +138 return T2._v_e_c_t_o_r() - vector(); │ │ │ │ │ +139 } │ │ │ │ │ +140 │ │ │ │ │ +144 │ │ │ │ │ +145 private: │ │ │ │ │ +_1_4_7 friend class boost::serialization::access; │ │ │ │ │ +148 template │ │ │ │ │ +149 void serialize(Archive& ar, const unsigned int /*version*/) { │ │ │ │ │ +150 ar& boost::serialization::make_nvp( │ │ │ │ │ +151 "Cal3_S2", boost::serialization::base_object(*this)); │ │ │ │ │ +152 ar& BOOST_SERIALIZATION_NVP(b_); │ │ │ │ │ +153 } │ │ │ │ │ +155}; │ │ │ │ │ +156 │ │ │ │ │ +157// Define GTSAM traits │ │ │ │ │ +158template <> │ │ │ │ │ +_1_5_9struct _t_r_a_i_t_s<_C_a_l_3___S_2_S_t_e_r_e_o> : public _i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d {}; │ │ │ │ │ +160 │ │ │ │ │ +161template <> │ │ │ │ │ +_1_6_2struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_: │ │ │ │ │ +_M_a_n_i_f_o_l_d { │ │ │ │ │ +163}; │ │ │ │ │ +164 │ │ │ │ │ +165} // \ namespace gtsam │ │ │ │ │ +_C_a_l_3___S_2_._h │ │ │ │ │ +The most common 5DOF 3D->2D calibration. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ -Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key │ │ │ │ │ -landmarkKey) │ │ │ │ │ -Optimize for triangulation. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.cpp:155 │ │ │ │ │ _g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ print without optional string, must specify cout yourself │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ +Vector2 Point2 │ │ │ │ │ +As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ +typedef Point2 to Vector2... │ │ │ │ │ +DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_M_a_n_i_f_o_l_d │ │ │ │ │ +Both ManifoldTraits and Testable. │ │ │ │ │ +DDeeffiinniittiioonn Manifold.h:120 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ +either a fixed size o... │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ _g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ Template to create a binary predicate. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e │ │ │ │ │ -An algebraic decision tree fixes the range of a DecisionTree to double. │ │ │ │ │ -DDeeffiinniittiioonn AlgebraicDecisionTree.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_F_a_c_t_o_r │ │ │ │ │ -A discrete probabilistic factor. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTreeFactor.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ -A map from keys to values. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteValues.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_N_e_t │ │ │ │ │ -A hybrid Bayes net is a collection of HybridConditionals, which can have │ │ │ │ │ -discrete conditionals,... │ │ │ │ │ -DDeeffiinniittiioonn HybridBayesNet.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_N_e_t_:_:_p_u_s_h___b_a_c_k │ │ │ │ │ -void push_back(boost::shared_ptr< HybridConditional > conditional) │ │ │ │ │ -Add a hybrid conditional using a shared_ptr. │ │ │ │ │ -DDeeffiinniittiioonn HybridBayesNet.h:69 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_N_e_t_:_:_e_m_p_l_a_c_e___b_a_c_k │ │ │ │ │ -void emplace_back(Conditional *conditional) │ │ │ │ │ -Preferred: add a conditional directly using a pointer. │ │ │ │ │ -DDeeffiinniittiioonn HybridBayesNet.h:82 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_N_e_t_:_:_p_u_s_h___b_a_c_k │ │ │ │ │ -void push_back(HybridConditional &&conditional) │ │ │ │ │ -Add a conditional using a shared_ptr, using implicit conversion to a │ │ │ │ │ -HybridConditional. │ │ │ │ │ -DDeeffiinniittiioonn HybridBayesNet.h:99 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_N_e_t_:_:_H_y_b_r_i_d_B_a_y_e_s_N_e_t │ │ │ │ │ -HybridBayesNet()=default │ │ │ │ │ -Construct empty Bayes net. │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_N_e_t_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ -double operator()(const HybridValues &values) const │ │ │ │ │ -Evaluate hybrid probability density for given HybridValues, sugar. │ │ │ │ │ -DDeeffiinniittiioonn HybridBayesNet.h:117 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -Hybrid Conditional Density. │ │ │ │ │ -DDeeffiinniittiioonn HybridConditional.h:62 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -DDeeffiinniittiioonn HybridGaussianFactorGraph.h:102 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t │ │ │ │ │ -A BayesNet is a tree of conditionals, stored in elimination order. │ │ │ │ │ -DDeeffiinniittiioonn BayesNet.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t │ │ │ │ │ -GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals. │ │ │ │ │ -DDeeffiinniittiioonn GaussianBayesNet.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ -with a unique integer... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3___S_2 │ │ │ │ │ +The most common 5DOF 3D->2D calibration. │ │ │ │ │ +DDeeffiinniittiioonn Cal3_S2.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o │ │ │ │ │ +The most common 5DOF 3D->2D calibration, stereo version. │ │ │ │ │ +DDeeffiinniittiioonn Cal3_S2Stereo.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o_:_:_v_e_c_t_o_r │ │ │ │ │ +Vector6 vector() const │ │ │ │ │ +vectorized form (column-wise) │ │ │ │ │ +DDeeffiinniittiioonn Cal3_S2Stereo.h:114 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o_:_:_C_a_l_3___S_2_S_t_e_r_e_o │ │ │ │ │ +Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b) │ │ │ │ │ +constructor from doubles │ │ │ │ │ +DDeeffiinniittiioonn Cal3_S2Stereo.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ +Vector6 localCoordinates(const Cal3_S2Stereo &T2) const │ │ │ │ │ +Unretraction for the calibration. │ │ │ │ │ +DDeeffiinniittiioonn Cal3_S2Stereo.h:137 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o_:_:_c_a_l_i_b_r_a_t_i_o_n │ │ │ │ │ +const Cal3_S2 & calibration() const │ │ │ │ │ +return calibration, same for left and right │ │ │ │ │ +DDeeffiinniittiioonn Cal3_S2Stereo.h:105 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o_:_:_d_i_m │ │ │ │ │ +size_t dim() const override │ │ │ │ │ +return DOF, dimensionality of tangent space │ │ │ │ │ +DDeeffiinniittiioonn Cal3_S2Stereo.h:125 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o_:_:_c_a_l_i_b_r_a_t_e │ │ │ │ │ +Vector3 calibrate(const Vector3 &p) const │ │ │ │ │ +Convert homogeneous image coordinates to intrinsic coordinates. │ │ │ │ │ +DDeeffiinniittiioonn Cal3_S2Stereo.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o_:_:_r_e_t_r_a_c_t │ │ │ │ │ +Cal3_S2Stereo retract(const Vector &d) const │ │ │ │ │ +Given 6-dim tangent vector, create new calibration. │ │ │ │ │ +DDeeffiinniittiioonn Cal3_S2Stereo.h:131 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o_:_:_K │ │ │ │ │ +Matrix3 K() const override │ │ │ │ │ +return calibration matrix K, same for left and right │ │ │ │ │ +DDeeffiinniittiioonn Cal3_S2Stereo.h:108 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o_:_:_D_i_m │ │ │ │ │ +static size_t Dim() │ │ │ │ │ +return DOF, dimensionality of tangent space │ │ │ │ │ +DDeeffiinniittiioonn Cal3_S2Stereo.h:128 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o_:_:_C_a_l_3___S_2_S_t_e_r_e_o │ │ │ │ │ +Cal3_S2Stereo(double fov, int w, int h, double b) │ │ │ │ │ +easy constructor; field-of-view in degrees, assumes zero skew │ │ │ │ │ +DDeeffiinniittiioonn Cal3_S2Stereo.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o_:_:_b_a_s_e_l_i_n_e │ │ │ │ │ +double baseline() const │ │ │ │ │ +return baseline │ │ │ │ │ +DDeeffiinniittiioonn Cal3_S2Stereo.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o_:_:_C_a_l_3___S_2_S_t_e_r_e_o │ │ │ │ │ +Cal3_S2Stereo(const Vector6 &d) │ │ │ │ │ +constructor from vector │ │ │ │ │ +DDeeffiinniittiioonn Cal3_S2Stereo.h:51 │ │ │ │ │ +_g_t_s_a_m_:_:_C_a_l_3___S_2_S_t_e_r_e_o_:_:_C_a_l_3___S_2_S_t_e_r_e_o │ │ │ │ │ +Cal3_S2Stereo()=default │ │ │ │ │ +default calibration leaves coordinates unchanged │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_B_a_y_e_s_N_e_t_._h │ │ │ │ │ + * _g_e_o_m_e_t_r_y │ │ │ │ │ + * _C_a_l_3___S_2_S_t_e_r_e_o_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00527_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00551_source.html │ │ │ │┄ Files 99% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
MixtureFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
20#pragma once
│ │ │ │
21
│ │ │ │ -
22#include <gtsam/discrete/DiscreteValues.h>
│ │ │ │ -
23#include <gtsam/hybrid/GaussianMixtureFactor.h>
│ │ │ │ -
24#include <gtsam/hybrid/HybridValues.h>
│ │ │ │ -
25#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
26#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ +
22#include <gtsam/discrete/DiscreteValues.h>
│ │ │ │ +
23#include <gtsam/hybrid/GaussianMixtureFactor.h>
│ │ │ │ +
24#include <gtsam/hybrid/HybridValues.h>
│ │ │ │ +
25#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
26#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │
27#include <gtsam/nonlinear/Symbol.h>
│ │ │ │
28
│ │ │ │
29#include <algorithm>
│ │ │ │
30#include <boost/format.hpp>
│ │ │ │
31#include <cmath>
│ │ │ │
32#include <limits>
│ │ │ │
33#include <vector>
│ │ │ │ @@ -330,19 +330,19 @@ │ │ │ │
291 (log(infoMat.determinant()) / 2.0);
│ │ │ │
292 }
│ │ │ │
│ │ │ │
293};
│ │ │ │
│ │ │ │
294
│ │ │ │
295} // namespace gtsam
│ │ │ │ - │ │ │ │ - │ │ │ │ -
GaussianMixtureFactor.h
A set of GaussianFactors, indexed by a set of discrete keys.
│ │ │ │ -
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ + │ │ │ │ +
GaussianMixtureFactor.h
A set of GaussianFactors, indexed by a set of discrete keys.
│ │ │ │ + │ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ │ │ │ │
gtsam::AlgebraicDecisionTree
An algebraic decision tree fixes the range of a DecisionTree to double.
Definition AlgebraicDecisionTree.h:38
│ │ │ │ │ │ │ │ @@ -377,13 +377,13 @@ │ │ │ │
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │
Symbol.h
Symbol.h was moved to inference directory, this header was retained for compatibility.
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -209,21 +209,21 @@ │ │ │ │ │ 290 return -(factor->dim() * log(2.0 * M_PI) / 2.0) - │ │ │ │ │ 291 (log(infoMat.determinant()) / 2.0); │ │ │ │ │ 292 } │ │ │ │ │ 293}; │ │ │ │ │ 294 │ │ │ │ │ 295} // namespace gtsam │ │ │ │ │ _D_i_s_c_r_e_t_e_V_a_l_u_e_s_._h │ │ │ │ │ -_H_y_b_r_i_d_V_a_l_u_e_s_._h │ │ │ │ │ _G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r_._h │ │ │ │ │ A set of GaussianFactors, indexed by a set of discrete keys. │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Factor Graph consisting of non-linear factors. │ │ │ │ │ +_H_y_b_r_i_d_V_a_l_u_e_s_._h │ │ │ │ │ _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ Non-linear factor base classes. │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Factor Graph consisting of non-linear factors. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ FastVector< Key > KeyVector │ │ │ │ │ Define collection type once and for all - also used in wrappers. │ │ │ │ │ DDeeffiinniittiioonn Key.h:86 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00542_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00836_source.html │ │ │ │┄ Files 95% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridEliminationTree.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianEliminationTree.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
HybridEliminationTree.h
│ │ │ │ +
GaussianEliminationTree.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/hybrid/HybridBayesNet.h>
│ │ │ │ -
21#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
│ │ │ │ -
22#include <gtsam/inference/EliminationTree.h>
│ │ │ │ -
23
│ │ │ │ -
24namespace gtsam {
│ │ │ │ -
25
│ │ │ │ -
│ │ │ │ -
31class GTSAM_EXPORT HybridEliminationTree
│ │ │ │ -
32 : public EliminationTree<HybridBayesNet, HybridGaussianFactorGraph> {
│ │ │ │ -
33 private:
│ │ │ │ -
34 friend class ::EliminationTreeTester;
│ │ │ │ -
35
│ │ │ │ -
36 public:
│ │ │ │ -
37 typedef EliminationTree<HybridBayesNet, HybridGaussianFactorGraph>
│ │ │ │ -
38 Base;
│ │ │ │ -
39 typedef HybridEliminationTree This;
│ │ │ │ -
40 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
41
│ │ │ │ -
44
│ │ │ │ -
54 HybridEliminationTree(const HybridGaussianFactorGraph& factorGraph,
│ │ │ │ -
55 const VariableIndex& structure, const Ordering& order);
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/linear/GaussianBayesNet.h>
│ │ │ │ +
22#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ +
23#include <gtsam/inference/EliminationTree.h>
│ │ │ │ +
24
│ │ │ │ +
25namespace gtsam {
│ │ │ │ +
26
│ │ │ │ +
│ │ │ │ +
27 class GTSAM_EXPORT GaussianEliminationTree :
│ │ │ │ +
28 public EliminationTree<GaussianBayesNet, GaussianFactorGraph>
│ │ │ │ +
29 {
│ │ │ │ +
30 public:
│ │ │ │ +
31 typedef EliminationTree<GaussianBayesNet, GaussianFactorGraph> Base;
│ │ │ │ +
32 typedef GaussianEliminationTree This;
│ │ │ │ +
33 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
34
│ │ │ │ +
43 GaussianEliminationTree(const GaussianFactorGraph& factorGraph,
│ │ │ │ +
44 const VariableIndex& structure, const Ordering& order);
│ │ │ │ +
45
│ │ │ │ +
51 GaussianEliminationTree(const GaussianFactorGraph& factorGraph,
│ │ │ │ +
52 const Ordering& order);
│ │ │ │ +
53
│ │ │ │ +
55 bool equals(const This& other, double tol = 1e-9) const;
│ │ │ │
56
│ │ │ │ -
62 HybridEliminationTree(const HybridGaussianFactorGraph& factorGraph,
│ │ │ │ -
63 const Ordering& order);
│ │ │ │ -
64
│ │ │ │ -
66
│ │ │ │ -
68 bool equals(const This& other, double tol = 1e-9) const;
│ │ │ │ -
69};
│ │ │ │ -
│ │ │ │ -
70
│ │ │ │ -
71} // namespace gtsam
│ │ │ │ -
HybridBayesNet.h
A Bayes net of Gaussian Conditionals indexed by discrete keys.
│ │ │ │ -
HybridGaussianFactorGraph.h
Linearized Hybrid factor graph that uses type erasure.
│ │ │ │ - │ │ │ │ +
57 private:
│ │ │ │ +
58
│ │ │ │ +
59 friend class ::EliminationTreeTester;
│ │ │ │ +
60
│ │ │ │ +
61 };
│ │ │ │ +
│ │ │ │ +
62
│ │ │ │ +
63}
│ │ │ │ + │ │ │ │ +
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ +
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::HybridEliminationTree
Elimination Tree type for Hybrid Factor Graphs.
Definition HybridEliminationTree.h:32
│ │ │ │ -
gtsam::HybridEliminationTree::This
HybridEliminationTree This
This class.
Definition HybridEliminationTree.h:39
│ │ │ │ -
gtsam::HybridEliminationTree::shared_ptr
boost::shared_ptr< This > shared_ptr
Shared pointer to this class.
Definition HybridEliminationTree.h:40
│ │ │ │ -
gtsam::HybridEliminationTree::Base
EliminationTree< HybridBayesNet, HybridGaussianFactorGraph > Base
Base class.
Definition HybridEliminationTree.h:38
│ │ │ │ -
gtsam::HybridGaussianFactorGraph
Definition HybridGaussianFactorGraph.h:102
│ │ │ │
gtsam::EliminationTree
An elimination tree is a data structure used intermediately during elimination.
Definition EliminationTree.h:52
│ │ │ │
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition VariableIndex.h:43
│ │ │ │ +
gtsam::GaussianEliminationTree
Definition GaussianEliminationTree.h:29
│ │ │ │ +
gtsam::GaussianEliminationTree::This
GaussianEliminationTree This
This class.
Definition GaussianEliminationTree.h:32
│ │ │ │ +
gtsam::GaussianEliminationTree::Base
EliminationTree< GaussianBayesNet, GaussianFactorGraph > Base
Base class.
Definition GaussianEliminationTree.h:31
│ │ │ │ +
gtsam::GaussianEliminationTree::shared_ptr
boost::shared_ptr< This > shared_ptr
Shared pointer to this class.
Definition GaussianEliminationTree.h:33
│ │ │ │ +
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,92 +1,92 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -HybridEliminationTree.h │ │ │ │ │ +GaussianEliminationTree.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h> │ │ │ │ │ -23 │ │ │ │ │ -24namespace _g_t_s_a_m { │ │ │ │ │ -25 │ │ │ │ │ -_3_1class GTSAM_EXPORT _H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ -32 : public _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e { │ │ │ │ │ -33 private: │ │ │ │ │ -34 friend class ::EliminationTreeTester; │ │ │ │ │ -35 │ │ │ │ │ -36 public: │ │ │ │ │ -37 typedef _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_H_y_b_r_i_d_B_a_y_e_s_N_e_t_,_ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_> │ │ │ │ │ -_3_8 _B_a_s_e; │ │ │ │ │ -_3_9 typedef _H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e _T_h_i_s; │ │ │ │ │ -_4_0 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -41 │ │ │ │ │ -44 │ │ │ │ │ -54 _H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e(const _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& factorGraph, │ │ │ │ │ -55 const _V_a_r_i_a_b_l_e_I_n_d_e_x& structure, const _O_r_d_e_r_i_n_g& order); │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h> │ │ │ │ │ +24 │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ +26 │ │ │ │ │ +_2_7 class GTSAM_EXPORT _G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e : │ │ │ │ │ +28 public _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ +29 { │ │ │ │ │ +30 public: │ │ │ │ │ +_3_1 typedef _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_,_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_> _B_a_s_e; │ │ │ │ │ +_3_2 typedef _G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e _T_h_i_s; │ │ │ │ │ +_3_3 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +34 │ │ │ │ │ +43 _G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& factorGraph, │ │ │ │ │ +44 const _V_a_r_i_a_b_l_e_I_n_d_e_x& structure, const _O_r_d_e_r_i_n_g& order); │ │ │ │ │ +45 │ │ │ │ │ +51 _G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& factorGraph, │ │ │ │ │ +52 const _O_r_d_e_r_i_n_g& order); │ │ │ │ │ +53 │ │ │ │ │ +55 bool _e_q_u_a_l_s(const _T_h_i_s& other, double tol = 1e-9) const; │ │ │ │ │ 56 │ │ │ │ │ -62 _H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e(const _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& factorGraph, │ │ │ │ │ -63 const _O_r_d_e_r_i_n_g& order); │ │ │ │ │ -64 │ │ │ │ │ -66 │ │ │ │ │ -68 bool _e_q_u_a_l_s(const _T_h_i_s& other, double tol = 1e-9) const; │ │ │ │ │ -69}; │ │ │ │ │ -70 │ │ │ │ │ -71} // namespace gtsam │ │ │ │ │ -_H_y_b_r_i_d_B_a_y_e_s_N_e_t_._h │ │ │ │ │ -A Bayes net of Gaussian Conditionals indexed by discrete keys. │ │ │ │ │ -_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Linearized Hybrid factor graph that uses type erasure. │ │ │ │ │ +57 private: │ │ │ │ │ +58 │ │ │ │ │ +59 friend class ::EliminationTreeTester; │ │ │ │ │ +60 │ │ │ │ │ +61 }; │ │ │ │ │ +62 │ │ │ │ │ +63} │ │ │ │ │ _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h │ │ │ │ │ +_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ +_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h │ │ │ │ │ +Chordal Bayes Net, the result of eliminating a factor graph. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ Template to create a binary predicate. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ -Elimination Tree type for Hybrid Factor Graphs. │ │ │ │ │ -DDeeffiinniittiioonn HybridEliminationTree.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_T_h_i_s │ │ │ │ │ -HybridEliminationTree This │ │ │ │ │ -This class. │ │ │ │ │ -DDeeffiinniittiioonn HybridEliminationTree.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -Shared pointer to this class. │ │ │ │ │ -DDeeffiinniittiioonn HybridEliminationTree.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_B_a_s_e │ │ │ │ │ -EliminationTree< HybridBayesNet, HybridGaussianFactorGraph > Base │ │ │ │ │ -Base class. │ │ │ │ │ -DDeeffiinniittiioonn HybridEliminationTree.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -DDeeffiinniittiioonn HybridGaussianFactorGraph.h:102 │ │ │ │ │ _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ An elimination tree is a data structure used intermediately during elimination. │ │ │ │ │ DDeeffiinniittiioonn EliminationTree.h:52 │ │ │ │ │ _g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ _g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ The VariableIndex class computes and stores the block column structure of a │ │ │ │ │ factor graph. │ │ │ │ │ DDeeffiinniittiioonn VariableIndex.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ +DDeeffiinniittiioonn GaussianEliminationTree.h:29 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_T_h_i_s │ │ │ │ │ +GaussianEliminationTree This │ │ │ │ │ +This class. │ │ │ │ │ +DDeeffiinniittiioonn GaussianEliminationTree.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_B_a_s_e │ │ │ │ │ +EliminationTree< GaussianBayesNet, GaussianFactorGraph > Base │ │ │ │ │ +Base class. │ │ │ │ │ +DDeeffiinniittiioonn GaussianEliminationTree.h:31 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +Shared pointer to this class. │ │ │ │ │ +DDeeffiinniittiioonn GaussianEliminationTree.h:33 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h │ │ │ │ │ + * _l_i_n_e_a_r │ │ │ │ │ + * _G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00548_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01367_source.html │ │ │ │┄ Files 94% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridGaussianFactorGraph.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicFactorGraph.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
HybridGaussianFactorGraph.h
│ │ │ │ +
SymbolicFactorGraph.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/hybrid/GaussianMixtureFactor.h>
│ │ │ │ -
22#include <gtsam/hybrid/HybridFactor.h>
│ │ │ │ -
23#include <gtsam/hybrid/HybridFactorGraph.h>
│ │ │ │ -
24#include <gtsam/inference/EliminateableFactorGraph.h>
│ │ │ │ -
25#include <gtsam/inference/FactorGraph.h>
│ │ │ │ -
26#include <gtsam/inference/Ordering.h>
│ │ │ │ -
27#include <gtsam/linear/GaussianFactor.h>
│ │ │ │ -
28#include <gtsam/linear/VectorValues.h>
│ │ │ │ -
29
│ │ │ │ -
30namespace gtsam {
│ │ │ │ -
31
│ │ │ │ -
32// Forward declarations
│ │ │ │ -
33class HybridGaussianFactorGraph;
│ │ │ │ -
34class HybridConditional;
│ │ │ │ -
35class HybridBayesNet;
│ │ │ │ -
36class HybridEliminationTree;
│ │ │ │ -
37class HybridBayesTree;
│ │ │ │ -
38class HybridJunctionTree;
│ │ │ │ -
39class DecisionTreeFactor;
│ │ │ │ -
40class JacobianFactor;
│ │ │ │ -
41class HybridValues;
│ │ │ │ -
42
│ │ │ │ -
51GTSAM_EXPORT
│ │ │ │ -
52std::pair<boost::shared_ptr<HybridConditional>, boost::shared_ptr<Factor>>
│ │ │ │ -
53EliminateHybrid(const HybridGaussianFactorGraph& factors, const Ordering& keys);
│ │ │ │ -
54
│ │ │ │ -
61GTSAM_EXPORT const Ordering
│ │ │ │ -
62HybridOrdering(const HybridGaussianFactorGraph& graph);
│ │ │ │ -
63
│ │ │ │ -
64/* ************************************************************************* */
│ │ │ │ -
65template <>
│ │ │ │ -
│ │ │ │ -
66struct EliminationTraits<HybridGaussianFactorGraph> {
│ │ │ │ -
67 typedef Factor FactorType;
│ │ │ │ -
68 typedef HybridGaussianFactorGraph
│ │ │ │ -
69 FactorGraphType;
│ │ │ │ -
71 typedef HybridConditional
│ │ │ │ -
72 ConditionalType;
│ │ │ │ -
73 typedef HybridBayesNet
│ │ │ │ -
74 BayesNetType;
│ │ │ │ -
75 typedef HybridEliminationTree
│ │ │ │ -
76 EliminationTreeType;
│ │ │ │ -
77 typedef HybridBayesTree BayesTreeType;
│ │ │ │ -
78 typedef HybridJunctionTree JunctionTreeType;
│ │ │ │ -
80 static std::pair<boost::shared_ptr<ConditionalType>,
│ │ │ │ -
81 boost::shared_ptr<FactorType>>
│ │ │ │ -
│ │ │ │ -
82 DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) {
│ │ │ │ -
83 return EliminateHybrid(factors, keys);
│ │ │ │ -
84 }
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
86 static Ordering DefaultOrderingFunc(
│ │ │ │ -
87 const FactorGraphType& graph,
│ │ │ │ -
88 boost::optional<const VariableIndex&> variableIndex) {
│ │ │ │ -
89 return HybridOrdering(graph);
│ │ │ │ -
90 }
│ │ │ │ -
│ │ │ │ -
91};
│ │ │ │ -
│ │ │ │ -
92
│ │ │ │ -
│ │ │ │ -
100class GTSAM_EXPORT HybridGaussianFactorGraph
│ │ │ │ -
101 : public HybridFactorGraph,
│ │ │ │ -
102 public EliminateableFactorGraph<HybridGaussianFactorGraph> {
│ │ │ │ -
103 protected:
│ │ │ │ -
105 template <typename FACTOR>
│ │ │ │ -
106 using IsGaussian = typename std::enable_if<
│ │ │ │ -
107 std::is_base_of<GaussianFactor, FACTOR>::value>::type;
│ │ │ │ -
108
│ │ │ │ -
109 public:
│ │ │ │ -
110 using Base = HybridFactorGraph;
│ │ │ │ -
111 using This = HybridGaussianFactorGraph;
│ │ │ │ -
112 using BaseEliminateable =
│ │ │ │ -
113 EliminateableFactorGraph<This>;
│ │ │ │ -
114 using shared_ptr = boost::shared_ptr<This>;
│ │ │ │ -
115
│ │ │ │ -
116 using Values = gtsam::Values;
│ │ │ │ -
117 using Indices = KeyVector;
│ │ │ │ +
21#include <gtsam/symbolic/SymbolicFactor.h>
│ │ │ │ +
22#include <gtsam/inference/FactorGraph.h>
│ │ │ │ +
23#include <gtsam/inference/EliminateableFactorGraph.h>
│ │ │ │ +
24#include <gtsam/base/types.h>
│ │ │ │ +
25
│ │ │ │ +
26namespace gtsam {
│ │ │ │ +
27
│ │ │ │ +
28 class SymbolicFactorGraph;
│ │ │ │ +
29 class SymbolicConditional;
│ │ │ │ +
30 class SymbolicBayesNet;
│ │ │ │ +
31 class SymbolicEliminationTree;
│ │ │ │ +
32 class SymbolicBayesTree;
│ │ │ │ +
33 class SymbolicJunctionTree;
│ │ │ │ +
34
│ │ │ │ +
35 /* ************************************************************************* */
│ │ │ │ +
│ │ │ │ +
36 template<> struct EliminationTraits<SymbolicFactorGraph>
│ │ │ │ +
37 {
│ │ │ │ +
38 typedef SymbolicFactor FactorType;
│ │ │ │ +
39 typedef SymbolicFactorGraph FactorGraphType;
│ │ │ │ +
40 typedef SymbolicConditional ConditionalType;
│ │ │ │ +
41 typedef SymbolicBayesNet BayesNetType;
│ │ │ │ +
42 typedef SymbolicEliminationTree EliminationTreeType;
│ │ │ │ +
43 typedef SymbolicBayesTree BayesTreeType;
│ │ │ │ +
44 typedef SymbolicJunctionTree JunctionTreeType;
│ │ │ │ +
46 static std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> >
│ │ │ │ +
│ │ │ │ +
47 DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) {
│ │ │ │ +
48 return EliminateSymbolic(factors, keys); }
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
50 static Ordering DefaultOrderingFunc(
│ │ │ │ +
51 const FactorGraphType& graph,
│ │ │ │ +
52 boost::optional<const VariableIndex&> variableIndex) {
│ │ │ │ +
53 return Ordering::Colamd(*variableIndex);
│ │ │ │ +
54 }
│ │ │ │ +
│ │ │ │ +
55 };
│ │ │ │ +
│ │ │ │ +
56
│ │ │ │ +
57 /* ************************************************************************* */
│ │ │ │ +
│ │ │ │ +
61 class GTSAM_EXPORT SymbolicFactorGraph :
│ │ │ │ +
62 public FactorGraph<SymbolicFactor>,
│ │ │ │ +
63 public EliminateableFactorGraph<SymbolicFactorGraph>
│ │ │ │ +
64 {
│ │ │ │ +
65 public:
│ │ │ │ +
66
│ │ │ │ +
67 typedef SymbolicFactorGraph This;
│ │ │ │ +
68 typedef FactorGraph<SymbolicFactor> Base;
│ │ │ │ +
69 typedef EliminateableFactorGraph<This> BaseEliminateable;
│ │ │ │ +
70 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
71
│ │ │ │ +
74
│ │ │ │ +
76 SymbolicFactorGraph() {}
│ │ │ │ +
77
│ │ │ │ +
79 template<typename ITERATOR>
│ │ │ │ +
80 SymbolicFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {}
│ │ │ │ +
81
│ │ │ │ +
83 template<class CONTAINER>
│ │ │ │ +
84 explicit SymbolicFactorGraph(const CONTAINER& factors) : Base(factors) {}
│ │ │ │ +
85
│ │ │ │ +
87 template<class DERIVEDFACTOR>
│ │ │ │ +
88 SymbolicFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
│ │ │ │ +
89
│ │ │ │ +
│ │ │ │ +
94 SymbolicFactorGraph(
│ │ │ │ +
95 std::initializer_list<boost::shared_ptr<SymbolicFactor>> sharedFactors)
│ │ │ │ +
96 : Base(sharedFactors) {}
│ │ │ │ +
│ │ │ │ +
97
│ │ │ │ +
│ │ │ │ +
99 SymbolicFactorGraph(SymbolicFactor&& c) {
│ │ │ │ +
100 push_back(boost::make_shared<SymbolicFactor>(c));
│ │ │ │ +
101 }
│ │ │ │ +
│ │ │ │ +
102
│ │ │ │ +
│ │ │ │ +
109 SymbolicFactorGraph& operator()(SymbolicFactor&& c) {
│ │ │ │ +
110 push_back(boost::make_shared<SymbolicFactor>(c));
│ │ │ │ +
111 return *this;
│ │ │ │ +
112 }
│ │ │ │ +
│ │ │ │ +
113
│ │ │ │ +
115 virtual ~SymbolicFactorGraph() {}
│ │ │ │ +
116
│ │ │ │
118
│ │ │ │
121
│ │ │ │ -
123 HybridGaussianFactorGraph() = default;
│ │ │ │ -
124
│ │ │ │ -
130 template <class DERIVEDFACTOR>
│ │ │ │ -
│ │ │ │ -
131 HybridGaussianFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph)
│ │ │ │ -
132 : Base(graph) {}
│ │ │ │ -
│ │ │ │ -
133
│ │ │ │ -
137
│ │ │ │ -
138 // TODO(dellaert): customize print and equals.
│ │ │ │ -
139 // void print(const std::string& s = "HybridGaussianFactorGraph",
│ │ │ │ -
140 // const KeyFormatter& keyFormatter = DefaultKeyFormatter) const
│ │ │ │ -
141 // override;
│ │ │ │ -
142 // bool equals(const This& fg, double tol = 1e-9) const override;
│ │ │ │ -
143
│ │ │ │ +
122 bool equals(const This& fg, double tol = 1e-9) const;
│ │ │ │ +
123
│ │ │ │ +
│ │ │ │ +
125 void print(
│ │ │ │ +
126 const std::string& s = "SymbolicFactorGraph",
│ │ │ │ +
127 const KeyFormatter& formatter = DefaultKeyFormatter) const override {
│ │ │ │ +
128 Base::print(s, formatter);
│ │ │ │ +
129 }
│ │ │ │ +
│ │ │ │ +
130
│ │ │ │ +
132
│ │ │ │ +
135
│ │ │ │ +
137 void push_factor(Key key);
│ │ │ │ +
138
│ │ │ │ +
140 void push_factor(Key key1, Key key2);
│ │ │ │ +
141
│ │ │ │ +
143 void push_factor(Key key1, Key key2, Key key3);
│ │ │ │ +
144
│ │ │ │ +
146 void push_factor(Key key1, Key key2, Key key3, Key key4);
│ │ │ │
147
│ │ │ │ -
148 using Base::error; // Expose error(const HybridValues&) method..
│ │ │ │
149
│ │ │ │ -
159 AlgebraicDecisionTree<Key> error(const VectorValues& continuousValues) const;
│ │ │ │ -
160
│ │ │ │ -
169 AlgebraicDecisionTree<Key> probPrime(
│ │ │ │ -
170 const VectorValues& continuousValues) const;
│ │ │ │ -
171
│ │ │ │ -
178 double probPrime(const HybridValues& values) const;
│ │ │ │ -
179
│ │ │ │ -
189 GaussianFactorGraphTree assembleGraphTree() const;
│ │ │ │ -
190
│ │ │ │ -
192};
│ │ │ │ -
│ │ │ │ -
193
│ │ │ │ -
194} // namespace gtsam
│ │ │ │ -
HybridFactorGraph.h
Factor graph with utilities for hybrid factors.
│ │ │ │ - │ │ │ │ -
GaussianMixtureFactor.h
A set of GaussianFactors, indexed by a set of discrete keys.
│ │ │ │ -
EliminateableFactorGraph.h
Variable elimination algorithms for factor graphs.
│ │ │ │ -
FactorGraph.h
Factor Graph Base Class.
│ │ │ │ -
Ordering.h
Variable ordering for the elimination algorithm.
│ │ │ │ -
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
│ │ │ │ -
VectorValues.h
Factor Graph Values.
│ │ │ │ -
gtsam::EliminateHybrid
std::pair< HybridConditional::shared_ptr, boost::shared_ptr< Factor > > EliminateHybrid(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys)
Main elimination function for HybridGaussianFactorGraph.
Definition HybridGaussianFactorGraph.cpp:320
│ │ │ │ +
150 private:
│ │ │ │ +
152 friend class boost::serialization::access;
│ │ │ │ +
153 template<class ARCHIVE>
│ │ │ │ +
154 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
155 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
156 }
│ │ │ │ +
157 };
│ │ │ │ +
│ │ │ │ +
158
│ │ │ │ +
160template<>
│ │ │ │ +
│ │ │ │ +
161struct traits<SymbolicFactorGraph> : public Testable<SymbolicFactorGraph> {
│ │ │ │ +
162};
│ │ │ │ +
│ │ │ │ +
163
│ │ │ │ +
164} //\ namespace gtsam
│ │ │ │ +
types.h
Typedefs for easier changing of types.
│ │ │ │ +
FactorGraph.h
Factor Graph Base Class.
│ │ │ │ +
EliminateableFactorGraph.h
Variable elimination algorithms for factor graphs.
│ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::HybridOrdering
const Ordering HybridOrdering(const HybridGaussianFactorGraph &graph)
Return a Colamd constrained ordering where the discrete keys are eliminated after the continuous keys...
Definition HybridGaussianFactorGraph.cpp:72
│ │ │ │ -
gtsam::AlgebraicDecisionTree
An algebraic decision tree fixes the range of a DecisionTree to double.
Definition AlgebraicDecisionTree.h:38
│ │ │ │ -
gtsam::DecisionTree
a decision tree is a function from assignments to values.
Definition DecisionTree.h:61
│ │ │ │ -
gtsam::HybridBayesNet
A hybrid Bayes net is a collection of HybridConditionals, which can have discrete conditionals,...
Definition HybridBayesNet.h:35
│ │ │ │ -
gtsam::HybridBayesTree
A Bayes tree representing a Hybrid density.
Definition HybridBayesTree.h:64
│ │ │ │ -
gtsam::HybridConditional
Hybrid Conditional Density.
Definition HybridConditional.h:62
│ │ │ │ -
gtsam::HybridEliminationTree
Elimination Tree type for Hybrid Factor Graphs.
Definition HybridEliminationTree.h:32
│ │ │ │ -
gtsam::HybridFactorGraph
Hybrid Factor Graph Factor graph with utilities for hybrid factors.
Definition HybridFactorGraph.h:39
│ │ │ │ -
gtsam::EliminationTraits< HybridGaussianFactorGraph >::DefaultEliminate
static std::pair< boost::shared_ptr< ConditionalType >, boost::shared_ptr< FactorType > > DefaultEliminate(const FactorGraphType &factors, const Ordering &keys)
The default dense elimination function.
Definition HybridGaussianFactorGraph.h:82
│ │ │ │ -
gtsam::EliminationTraits< HybridGaussianFactorGraph >::ConditionalType
HybridConditional ConditionalType
Type of conditionals from elimination.
Definition HybridGaussianFactorGraph.h:72
│ │ │ │ -
gtsam::EliminationTraits< HybridGaussianFactorGraph >::FactorGraphType
HybridGaussianFactorGraph FactorGraphType
Type of the factor graph (e.g.
Definition HybridGaussianFactorGraph.h:69
│ │ │ │ -
gtsam::EliminationTraits< HybridGaussianFactorGraph >::JunctionTreeType
HybridJunctionTree JunctionTreeType
Type of Junction tree.
Definition HybridGaussianFactorGraph.h:78
│ │ │ │ -
gtsam::EliminationTraits< HybridGaussianFactorGraph >::EliminationTreeType
HybridEliminationTree EliminationTreeType
Type of elimination tree.
Definition HybridGaussianFactorGraph.h:76
│ │ │ │ -
gtsam::EliminationTraits< HybridGaussianFactorGraph >::DefaultOrderingFunc
static Ordering DefaultOrderingFunc(const FactorGraphType &graph, boost::optional< const VariableIndex & > variableIndex)
The default ordering generation function.
Definition HybridGaussianFactorGraph.h:86
│ │ │ │ -
gtsam::EliminationTraits< HybridGaussianFactorGraph >::FactorType
Factor FactorType
Type of factors in factor graph.
Definition HybridGaussianFactorGraph.h:67
│ │ │ │ -
gtsam::EliminationTraits< HybridGaussianFactorGraph >::BayesTreeType
HybridBayesTree BayesTreeType
Type of Bayes tree.
Definition HybridGaussianFactorGraph.h:77
│ │ │ │ -
gtsam::EliminationTraits< HybridGaussianFactorGraph >::BayesNetType
HybridBayesNet BayesNetType
Type of Bayes net from sequential elimination.
Definition HybridGaussianFactorGraph.h:74
│ │ │ │ -
gtsam::HybridGaussianFactorGraph
Definition HybridGaussianFactorGraph.h:102
│ │ │ │ -
gtsam::HybridGaussianFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to This
Definition HybridGaussianFactorGraph.h:114
│ │ │ │ -
gtsam::HybridGaussianFactorGraph::IsGaussian
typename std::enable_if< std::is_base_of< GaussianFactor, FACTOR >::value >::type IsGaussian
Check if FACTOR type is derived from GaussianFactor.
Definition HybridGaussianFactorGraph.h:107
│ │ │ │ -
gtsam::HybridGaussianFactorGraph::Indices
KeyVector Indices
map from keys to values
Definition HybridGaussianFactorGraph.h:117
│ │ │ │ -
gtsam::HybridGaussianFactorGraph::BaseEliminateable
EliminateableFactorGraph< This > BaseEliminateable
for elimination
Definition HybridGaussianFactorGraph.h:113
│ │ │ │ -
gtsam::HybridGaussianFactorGraph::HybridGaussianFactorGraph
HybridGaussianFactorGraph()=default
Default constructor.
│ │ │ │ -
gtsam::HybridGaussianFactorGraph::HybridGaussianFactorGraph
HybridGaussianFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
Implicit copy/downcast constructor to override explicit template container constructor.
Definition HybridGaussianFactorGraph.h:131
│ │ │ │ -
gtsam::HybridJunctionTree
An EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree,...
Definition HybridJunctionTree.h:53
│ │ │ │ -
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ +
gtsam::EliminateSymbolic
std::pair< boost::shared_ptr< SymbolicConditional >, boost::shared_ptr< SymbolicFactor > > EliminateSymbolic(const SymbolicFactorGraph &factors, const Ordering &keys)
Dense elimination function for symbolic factors.
Definition SymbolicFactor.cpp:36
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │
gtsam::EliminationTraits
Traits class for eliminateable factor graphs, specifies the types that result from elimination,...
Definition EliminateableFactorGraph.h:36
│ │ │ │
gtsam::EliminateableFactorGraph
EliminateableFactorGraph is a base class for factor graphs that contains elimination algorithms.
Definition EliminateableFactorGraph.h:57
│ │ │ │ -
gtsam::Factor
Definition Factor.h:68
│ │ │ │
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ -
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ -
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
HybridValues
the error.
│ │ │ │ -
JacobianFactor
In Gaussian factors, the error function returns either the negative log-likelihood,...
│ │ │ │ +
gtsam::Ordering::Colamd
static Ordering Colamd(const FACTOR_GRAPH &graph)
Compute a fill-reducing ordering using COLAMD from a factor graph (see details for note on performanc...
Definition Ordering.h:95
│ │ │ │ +
gtsam::SymbolicBayesNet
A SymbolicBayesNet is a Bayes Net of purely symbolic conditionals.
Definition SymbolicBayesNet.h:32
│ │ │ │ +
gtsam::SymbolicBayesTree
A Bayes tree that represents the connectivity between variables but is not associated with any probab...
Definition SymbolicBayesTree.h:51
│ │ │ │ +
gtsam::SymbolicConditional
SymbolicConditional is a conditional with keys but no probability data, produced by symbolic eliminat...
Definition SymbolicConditional.h:38
│ │ │ │ +
gtsam::SymbolicEliminationTree
Definition SymbolicEliminationTree.h:29
│ │ │ │ +
gtsam::SymbolicFactor
SymbolicFactor represents a symbolic factor that specifies graph topology but is not associated with ...
Definition SymbolicFactor.h:39
│ │ │ │ +
gtsam::EliminationTraits< SymbolicFactorGraph >::FactorType
SymbolicFactor FactorType
Type of factors in factor graph.
Definition SymbolicFactorGraph.h:38
│ │ │ │ +
gtsam::EliminationTraits< SymbolicFactorGraph >::FactorGraphType
SymbolicFactorGraph FactorGraphType
Type of the factor graph (e.g. GaussianFactorGraph)
Definition SymbolicFactorGraph.h:39
│ │ │ │ +
gtsam::EliminationTraits< SymbolicFactorGraph >::EliminationTreeType
SymbolicEliminationTree EliminationTreeType
Type of elimination tree.
Definition SymbolicFactorGraph.h:42
│ │ │ │ +
gtsam::EliminationTraits< SymbolicFactorGraph >::BayesNetType
SymbolicBayesNet BayesNetType
Type of Bayes net from sequential elimination.
Definition SymbolicFactorGraph.h:41
│ │ │ │ +
gtsam::EliminationTraits< SymbolicFactorGraph >::DefaultEliminate
static std::pair< boost::shared_ptr< ConditionalType >, boost::shared_ptr< FactorType > > DefaultEliminate(const FactorGraphType &factors, const Ordering &keys)
The default dense elimination function.
Definition SymbolicFactorGraph.h:47
│ │ │ │ +
gtsam::EliminationTraits< SymbolicFactorGraph >::DefaultOrderingFunc
static Ordering DefaultOrderingFunc(const FactorGraphType &graph, boost::optional< const VariableIndex & > variableIndex)
The default ordering generation function.
Definition SymbolicFactorGraph.h:50
│ │ │ │ +
gtsam::EliminationTraits< SymbolicFactorGraph >::JunctionTreeType
SymbolicJunctionTree JunctionTreeType
Type of Junction tree.
Definition SymbolicFactorGraph.h:44
│ │ │ │ +
gtsam::EliminationTraits< SymbolicFactorGraph >::BayesTreeType
SymbolicBayesTree BayesTreeType
Type of Bayes tree.
Definition SymbolicFactorGraph.h:43
│ │ │ │ +
gtsam::EliminationTraits< SymbolicFactorGraph >::ConditionalType
SymbolicConditional ConditionalType
Type of conditionals from elimination.
Definition SymbolicFactorGraph.h:40
│ │ │ │ +
gtsam::SymbolicFactorGraph
Symbolic Factor Graph.
Definition SymbolicFactorGraph.h:64
│ │ │ │ +
gtsam::SymbolicFactorGraph::SymbolicFactorGraph
SymbolicFactorGraph(SymbolicFactor &&c)
Construct from a single factor.
Definition SymbolicFactorGraph.h:99
│ │ │ │ +
gtsam::SymbolicFactorGraph::operator()
SymbolicFactorGraph & operator()(SymbolicFactor &&c)
Add a single factor and return a reference.
Definition SymbolicFactorGraph.h:109
│ │ │ │ +
gtsam::SymbolicFactorGraph::SymbolicFactorGraph
SymbolicFactorGraph()
Construct empty factor graph.
Definition SymbolicFactorGraph.h:76
│ │ │ │ +
gtsam::SymbolicFactorGraph::SymbolicFactorGraph
SymbolicFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
Implicit copy/downcast constructor to override explicit template container constructor.
Definition SymbolicFactorGraph.h:88
│ │ │ │ +
gtsam::SymbolicFactorGraph::SymbolicFactorGraph
SymbolicFactorGraph(std::initializer_list< boost::shared_ptr< SymbolicFactor > > sharedFactors)
Constructor that takes an initializer list of shared pointers.
Definition SymbolicFactorGraph.h:94
│ │ │ │ +
gtsam::SymbolicFactorGraph::print
void print(const std::string &s="SymbolicFactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print
Definition SymbolicFactorGraph.h:125
│ │ │ │ +
gtsam::SymbolicFactorGraph::BaseEliminateable
EliminateableFactorGraph< This > BaseEliminateable
Typedef to base elimination class.
Definition SymbolicFactorGraph.h:69
│ │ │ │ +
gtsam::SymbolicFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition SymbolicFactorGraph.h:70
│ │ │ │ +
gtsam::SymbolicFactorGraph::This
SymbolicFactorGraph This
Typedef to this class.
Definition SymbolicFactorGraph.h:67
│ │ │ │ +
gtsam::SymbolicFactorGraph::SymbolicFactorGraph
SymbolicFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
Construct from iterator over factors.
Definition SymbolicFactorGraph.h:80
│ │ │ │ +
gtsam::SymbolicFactorGraph::~SymbolicFactorGraph
virtual ~SymbolicFactorGraph()
Destructor.
Definition SymbolicFactorGraph.h:115
│ │ │ │ +
gtsam::SymbolicFactorGraph::Base
FactorGraph< SymbolicFactor > Base
Typedef to base factor graph type.
Definition SymbolicFactorGraph.h:68
│ │ │ │ +
gtsam::SymbolicFactorGraph::SymbolicFactorGraph
SymbolicFactorGraph(const CONTAINER &factors)
Construct from container of factors (shared_ptr or plain objects)
Definition SymbolicFactorGraph.h:84
│ │ │ │ +
gtsam::SymbolicJunctionTree
A EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree,...
Definition SymbolicJunctionTree.h:51
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -HybridGaussianFactorGraph.h │ │ │ │ │ +SymbolicFactorGraph.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ @@ -16,272 +16,294 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 19#pragma once │ │ │ │ │ 20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_F_a_c_t_o_r_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_O_r_d_e_r_i_n_g_._h> │ │ │ │ │ -27#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ -28#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ -29 │ │ │ │ │ -30namespace _g_t_s_a_m { │ │ │ │ │ -31 │ │ │ │ │ -32// Forward declarations │ │ │ │ │ -33class HybridGaussianFactorGraph; │ │ │ │ │ -34class HybridConditional; │ │ │ │ │ -35class HybridBayesNet; │ │ │ │ │ -36class HybridEliminationTree; │ │ │ │ │ -37class HybridBayesTree; │ │ │ │ │ -38class HybridJunctionTree; │ │ │ │ │ -39class DecisionTreeFactor; │ │ │ │ │ -40class _J_a_c_o_b_i_a_n_F_a_c_t_o_r; │ │ │ │ │ -41class _H_y_b_r_i_d_V_a_l_u_e_s; │ │ │ │ │ -42 │ │ │ │ │ -51GTSAM_EXPORT │ │ │ │ │ -52std::pair, boost::shared_ptr> │ │ │ │ │ -53_E_l_i_m_i_n_a_t_e_H_y_b_r_i_d(const HybridGaussianFactorGraph& factors, const Ordering& │ │ │ │ │ -keys); │ │ │ │ │ -54 │ │ │ │ │ -61GTSAM_EXPORT const Ordering │ │ │ │ │ -62_H_y_b_r_i_d_O_r_d_e_r_i_n_g(const HybridGaussianFactorGraph& graph); │ │ │ │ │ -63 │ │ │ │ │ -64/* ************************************************************************* │ │ │ │ │ +21#include <_g_t_s_a_m_/_s_y_m_b_o_l_i_c_/_S_y_m_b_o_l_i_c_F_a_c_t_o_r_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ +25 │ │ │ │ │ +26namespace _g_t_s_a_m { │ │ │ │ │ +27 │ │ │ │ │ +28 class SymbolicFactorGraph; │ │ │ │ │ +29 class SymbolicConditional; │ │ │ │ │ +30 class SymbolicBayesNet; │ │ │ │ │ +31 class SymbolicEliminationTree; │ │ │ │ │ +32 class SymbolicBayesTree; │ │ │ │ │ +33 class SymbolicJunctionTree; │ │ │ │ │ +34 │ │ │ │ │ +35 /* ************************************************************************* │ │ │ │ │ */ │ │ │ │ │ -65template <> │ │ │ │ │ -_6_6struct _E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s<_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h> { │ │ │ │ │ -_6_7 typedef _F_a_c_t_o_r _F_a_c_t_o_r_T_y_p_e; │ │ │ │ │ -68 typedef _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -_6_9 _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e; │ │ │ │ │ -71 typedef _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -_7_2 _C_o_n_d_i_t_i_o_n_a_l_T_y_p_e; │ │ │ │ │ -73 typedef _H_y_b_r_i_d_B_a_y_e_s_N_e_t │ │ │ │ │ -_7_4 _B_a_y_e_s_N_e_t_T_y_p_e; │ │ │ │ │ -75 typedef _H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ -_7_6 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_T_y_p_e; │ │ │ │ │ -_7_7 typedef _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e _B_a_y_e_s_T_r_e_e_T_y_p_e; │ │ │ │ │ -_7_8 typedef _H_y_b_r_i_d_J_u_n_c_t_i_o_n_T_r_e_e _J_u_n_c_t_i_o_n_T_r_e_e_T_y_p_e; │ │ │ │ │ -80 static std::pair, │ │ │ │ │ -81 boost::shared_ptr> │ │ │ │ │ -_8_2 _D_e_f_a_u_l_t_E_l_i_m_i_n_a_t_e(const _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e& factors, const _O_r_d_e_r_i_n_g& keys) { │ │ │ │ │ -83 return _E_l_i_m_i_n_a_t_e_H_y_b_r_i_d(factors, keys); │ │ │ │ │ -84 } │ │ │ │ │ -_8_6 static _O_r_d_e_r_i_n_g _D_e_f_a_u_l_t_O_r_d_e_r_i_n_g_F_u_n_c( │ │ │ │ │ -87 const _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e& graph, │ │ │ │ │ -88 boost::optional variableIndex) { │ │ │ │ │ -89 return _H_y_b_r_i_d_O_r_d_e_r_i_n_g(graph); │ │ │ │ │ -90 } │ │ │ │ │ -91}; │ │ │ │ │ -92 │ │ │ │ │ -_1_0_0class GTSAM_EXPORT _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -101 : public _H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h, │ │ │ │ │ -102 public _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h { │ │ │ │ │ -103 protected: │ │ │ │ │ -105 template │ │ │ │ │ -_1_0_6 using _I_s_G_a_u_s_s_i_a_n = typename std::enable_if< │ │ │ │ │ -107 std::is_base_of::value>::type; │ │ │ │ │ -108 │ │ │ │ │ -109 public: │ │ │ │ │ -110 using _B_a_s_e = _H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h; │ │ │ │ │ -_1_1_1 using _T_h_i_s = _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h; │ │ │ │ │ -_1_1_2 using _B_a_s_e_E_l_i_m_i_n_a_t_e_a_b_l_e = │ │ │ │ │ -113 _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_T_h_i_s_>; │ │ │ │ │ -_1_1_4 using _s_h_a_r_e_d___p_t_r = boost::shared_ptr; │ │ │ │ │ -115 │ │ │ │ │ -_1_1_6 using _V_a_l_u_e_s = _g_t_s_a_m_:_:_V_a_l_u_e_s; │ │ │ │ │ -_1_1_7 using _I_n_d_i_c_e_s = _K_e_y_V_e_c_t_o_r; │ │ │ │ │ +_3_6 template<> struct _E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s<_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h> │ │ │ │ │ +37 { │ │ │ │ │ +_3_8 typedef _S_y_m_b_o_l_i_c_F_a_c_t_o_r _F_a_c_t_o_r_T_y_p_e; │ │ │ │ │ +_3_9 typedef _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e; │ │ │ │ │ +_4_0 typedef _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l _C_o_n_d_i_t_i_o_n_a_l_T_y_p_e; │ │ │ │ │ +_4_1 typedef _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t _B_a_y_e_s_N_e_t_T_y_p_e; │ │ │ │ │ +_4_2 typedef _S_y_m_b_o_l_i_c_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_T_y_p_e; │ │ │ │ │ +_4_3 typedef _S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e _B_a_y_e_s_T_r_e_e_T_y_p_e; │ │ │ │ │ +_4_4 typedef _S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e _J_u_n_c_t_i_o_n_T_r_e_e_T_y_p_e; │ │ │ │ │ +46 static std::pair, boost:: │ │ │ │ │ +shared_ptr > │ │ │ │ │ +_4_7 _D_e_f_a_u_l_t_E_l_i_m_i_n_a_t_e(const _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e& factors, const _O_r_d_e_r_i_n_g& keys) { │ │ │ │ │ +48 return _E_l_i_m_i_n_a_t_e_S_y_m_b_o_l_i_c(factors, keys); } │ │ │ │ │ +_5_0 static _O_r_d_e_r_i_n_g _D_e_f_a_u_l_t_O_r_d_e_r_i_n_g_F_u_n_c( │ │ │ │ │ +51 const _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e& graph, │ │ │ │ │ +52 boost::optional variableIndex) { │ │ │ │ │ +53 return _O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d(*variableIndex); │ │ │ │ │ +54 } │ │ │ │ │ +55 }; │ │ │ │ │ +56 │ │ │ │ │ +57 /* ************************************************************************* │ │ │ │ │ +*/ │ │ │ │ │ +_6_1 class GTSAM_EXPORT _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h : │ │ │ │ │ +62 public _F_a_c_t_o_r_G_r_a_p_h, │ │ │ │ │ +63 public _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +64 { │ │ │ │ │ +65 public: │ │ │ │ │ +66 │ │ │ │ │ +_6_7 typedef _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h _T_h_i_s; │ │ │ │ │ +_6_8 typedef _F_a_c_t_o_r_G_r_a_p_h_<_S_y_m_b_o_l_i_c_F_a_c_t_o_r_> _B_a_s_e; │ │ │ │ │ +_6_9 typedef _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_T_h_i_s_> _B_a_s_e_E_l_i_m_i_n_a_t_e_a_b_l_e; │ │ │ │ │ +_7_0 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +71 │ │ │ │ │ +74 │ │ │ │ │ +_7_6 _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h() {} │ │ │ │ │ +77 │ │ │ │ │ +79 template │ │ │ │ │ +_8_0 _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h(ITERATOR firstFactor, ITERATOR lastFactor) : _B_a_s_e │ │ │ │ │ +(firstFactor, lastFactor) {} │ │ │ │ │ +81 │ │ │ │ │ +83 template │ │ │ │ │ +_8_4 explicit _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h(const CONTAINER& factors) : _B_a_s_e(factors) {} │ │ │ │ │ +85 │ │ │ │ │ +87 template │ │ │ │ │ +_8_8 _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h(const _F_a_c_t_o_r_G_r_a_p_h_<_D_E_R_I_V_E_D_F_A_C_T_O_R_>& graph) : _B_a_s_e(graph) │ │ │ │ │ +{} │ │ │ │ │ +89 │ │ │ │ │ +_9_4 _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h( │ │ │ │ │ +95 std::initializer_list> sharedFactors) │ │ │ │ │ +96 : _B_a_s_e(sharedFactors) {} │ │ │ │ │ +97 │ │ │ │ │ +_9_9 _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h(_S_y_m_b_o_l_i_c_F_a_c_t_o_r&& c) { │ │ │ │ │ +100 push_back(boost::make_shared(c)); │ │ │ │ │ +101 } │ │ │ │ │ +102 │ │ │ │ │ +_1_0_9 _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h& _o_p_e_r_a_t_o_r_(_)(_S_y_m_b_o_l_i_c_F_a_c_t_o_r&& c) { │ │ │ │ │ +110 push_back(boost::make_shared(c)); │ │ │ │ │ +111 return *this; │ │ │ │ │ +112 } │ │ │ │ │ +113 │ │ │ │ │ +_1_1_5 virtual _~_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h() {} │ │ │ │ │ +116 │ │ │ │ │ 118 │ │ │ │ │ 121 │ │ │ │ │ -_1_2_3 _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h() = default; │ │ │ │ │ -124 │ │ │ │ │ -130 template │ │ │ │ │ -_1_3_1 _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h(const _F_a_c_t_o_r_G_r_a_p_h_<_D_E_R_I_V_E_D_F_A_C_T_O_R_>& graph) │ │ │ │ │ -132 : _B_a_s_e(graph) {} │ │ │ │ │ -133 │ │ │ │ │ -137 │ │ │ │ │ -138 // TODO(dellaert): customize print and equals. │ │ │ │ │ -139 // void print(const std::string& s = "HybridGaussianFactorGraph", │ │ │ │ │ -140 // const KeyFormatter& keyFormatter = DefaultKeyFormatter) const │ │ │ │ │ -141 // override; │ │ │ │ │ -142 // bool equals(const This& fg, double tol = 1e-9) const override; │ │ │ │ │ -143 │ │ │ │ │ +122 bool _e_q_u_a_l_s(const This& fg, double tol = 1e-9) const; │ │ │ │ │ +123 │ │ │ │ │ +_1_2_5 void _p_r_i_n_t( │ │ │ │ │ +126 const std::string& s = "SymbolicFactorGraph", │ │ │ │ │ +127 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override { │ │ │ │ │ +128 Base::print(s, formatter); │ │ │ │ │ +129 } │ │ │ │ │ +130 │ │ │ │ │ +132 │ │ │ │ │ +135 │ │ │ │ │ +137 void push_factor(_K_e_y key); │ │ │ │ │ +138 │ │ │ │ │ +140 void push_factor(_K_e_y key1, _K_e_y key2); │ │ │ │ │ +141 │ │ │ │ │ +143 void push_factor(_K_e_y key1, _K_e_y key2, _K_e_y key3); │ │ │ │ │ +144 │ │ │ │ │ +146 void push_factor(_K_e_y key1, _K_e_y key2, _K_e_y key3, _K_e_y key4); │ │ │ │ │ 147 │ │ │ │ │ -148 using Base::error; // Expose error(const HybridValues&) method.. │ │ │ │ │ 149 │ │ │ │ │ -159 _A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_<_K_e_y_> error(const _V_e_c_t_o_r_V_a_l_u_e_s& continuousValues) │ │ │ │ │ -const; │ │ │ │ │ -160 │ │ │ │ │ -169 _A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e_<_K_e_y_> probPrime( │ │ │ │ │ -170 const _V_e_c_t_o_r_V_a_l_u_e_s& continuousValues) const; │ │ │ │ │ -171 │ │ │ │ │ -178 double probPrime(const _H_y_b_r_i_d_V_a_l_u_e_s& values) const; │ │ │ │ │ -179 │ │ │ │ │ -189 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_T_r_e_e assembleGraphTree() const; │ │ │ │ │ -190 │ │ │ │ │ -192}; │ │ │ │ │ -193 │ │ │ │ │ -194} // namespace gtsam │ │ │ │ │ -_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Factor graph with utilities for hybrid factors. │ │ │ │ │ -_H_y_b_r_i_d_F_a_c_t_o_r_._h │ │ │ │ │ -_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_F_a_c_t_o_r_._h │ │ │ │ │ -A set of GaussianFactors, indexed by a set of discrete keys. │ │ │ │ │ -_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Variable elimination algorithms for factor graphs. │ │ │ │ │ +150 private: │ │ │ │ │ +_1_5_2 friend class boost::serialization::access; │ │ │ │ │ +153 template │ │ │ │ │ +154 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +155 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +156 } │ │ │ │ │ +157 }; │ │ │ │ │ +158 │ │ │ │ │ +160template<> │ │ │ │ │ +_1_6_1struct _t_r_a_i_t_s<_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h> : public _T_e_s_t_a_b_l_e { │ │ │ │ │ +162}; │ │ │ │ │ +163 │ │ │ │ │ +164} //\ namespace gtsam │ │ │ │ │ +_t_y_p_e_s_._h │ │ │ │ │ +Typedefs for easier changing of types. │ │ │ │ │ _F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ Factor Graph Base Class. │ │ │ │ │ -_O_r_d_e_r_i_n_g_._h │ │ │ │ │ -Variable ordering for the elimination algorithm. │ │ │ │ │ -_G_a_u_s_s_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ -A factor with a quadratic error function - a Gaussian. │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ -Factor Graph Values. │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_H_y_b_r_i_d │ │ │ │ │ -std::pair< HybridConditional::shared_ptr, boost::shared_ptr< Factor > > │ │ │ │ │ -EliminateHybrid(const HybridGaussianFactorGraph &factors, const Ordering │ │ │ │ │ -&frontalKeys) │ │ │ │ │ -Main elimination function for HybridGaussianFactorGraph. │ │ │ │ │ -DDeeffiinniittiioonn HybridGaussianFactorGraph.cpp:320 │ │ │ │ │ +_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Variable elimination algorithms for factor graphs. │ │ │ │ │ +_S_y_m_b_o_l_i_c_F_a_c_t_o_r_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_O_r_d_e_r_i_n_g │ │ │ │ │ -const Ordering HybridOrdering(const HybridGaussianFactorGraph &graph) │ │ │ │ │ -Return a Colamd constrained ordering where the discrete keys are eliminated │ │ │ │ │ -after the continuous keys... │ │ │ │ │ -DDeeffiinniittiioonn HybridGaussianFactorGraph.cpp:72 │ │ │ │ │ -_g_t_s_a_m_:_:_A_l_g_e_b_r_a_i_c_D_e_c_i_s_i_o_n_T_r_e_e │ │ │ │ │ -An algebraic decision tree fixes the range of a DecisionTree to double. │ │ │ │ │ -DDeeffiinniittiioonn AlgebraicDecisionTree.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e │ │ │ │ │ -a decision tree is a function from assignments to values. │ │ │ │ │ -DDeeffiinniittiioonn DecisionTree.h:61 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_N_e_t │ │ │ │ │ -A hybrid Bayes net is a collection of HybridConditionals, which can have │ │ │ │ │ -discrete conditionals,... │ │ │ │ │ -DDeeffiinniittiioonn HybridBayesNet.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e │ │ │ │ │ -A Bayes tree representing a Hybrid density. │ │ │ │ │ -DDeeffiinniittiioonn HybridBayesTree.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -Hybrid Conditional Density. │ │ │ │ │ -DDeeffiinniittiioonn HybridConditional.h:62 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ -Elimination Tree type for Hybrid Factor Graphs. │ │ │ │ │ -DDeeffiinniittiioonn HybridEliminationTree.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -Hybrid Factor Graph Factor graph with utilities for hybrid factors. │ │ │ │ │ -DDeeffiinniittiioonn HybridFactorGraph.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_D_e_f_a_u_l_t_E_l_i_m_i_n_a_t_e │ │ │ │ │ -static std::pair< boost::shared_ptr< ConditionalType >, boost::shared_ptr< │ │ │ │ │ -FactorType > > DefaultEliminate(const FactorGraphType &factors, const Ordering │ │ │ │ │ -&keys) │ │ │ │ │ -The default dense elimination function. │ │ │ │ │ -DDeeffiinniittiioonn HybridGaussianFactorGraph.h:82 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_C_o_n_d_i_t_i_o_n_a_l_T_y_p_e │ │ │ │ │ -HybridConditional ConditionalType │ │ │ │ │ -Type of conditionals from elimination. │ │ │ │ │ -DDeeffiinniittiioonn HybridGaussianFactorGraph.h:72 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_F_a_c_t_o_r_G_r_a_p_h_T_y_p_e │ │ │ │ │ -HybridGaussianFactorGraph FactorGraphType │ │ │ │ │ -Type of the factor graph (e.g. │ │ │ │ │ -DDeeffiinniittiioonn HybridGaussianFactorGraph.h:69 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_J_u_n_c_t_i_o_n_T_r_e_e_T_y_p_e │ │ │ │ │ -HybridJunctionTree JunctionTreeType │ │ │ │ │ -Type of Junction tree. │ │ │ │ │ -DDeeffiinniittiioonn HybridGaussianFactorGraph.h:78 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_T_y_p_e │ │ │ │ │ -HybridEliminationTree EliminationTreeType │ │ │ │ │ -Type of elimination tree. │ │ │ │ │ -DDeeffiinniittiioonn HybridGaussianFactorGraph.h:76 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_D_e_f_a_u_l_t_O_r_d_e_r_i_n_g_F_u_n_c │ │ │ │ │ -static Ordering DefaultOrderingFunc(const FactorGraphType &graph, boost:: │ │ │ │ │ -optional< const VariableIndex & > variableIndex) │ │ │ │ │ -The default ordering generation function. │ │ │ │ │ -DDeeffiinniittiioonn HybridGaussianFactorGraph.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_F_a_c_t_o_r_T_y_p_e │ │ │ │ │ -Factor FactorType │ │ │ │ │ -Type of factors in factor graph. │ │ │ │ │ -DDeeffiinniittiioonn HybridGaussianFactorGraph.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_B_a_y_e_s_T_r_e_e_T_y_p_e │ │ │ │ │ -HybridBayesTree BayesTreeType │ │ │ │ │ -Type of Bayes tree. │ │ │ │ │ -DDeeffiinniittiioonn HybridGaussianFactorGraph.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_B_a_y_e_s_N_e_t_T_y_p_e │ │ │ │ │ -HybridBayesNet BayesNetType │ │ │ │ │ -Type of Bayes net from sequential elimination. │ │ │ │ │ -DDeeffiinniittiioonn HybridGaussianFactorGraph.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -DDeeffiinniittiioonn HybridGaussianFactorGraph.h:102 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to This │ │ │ │ │ -DDeeffiinniittiioonn HybridGaussianFactorGraph.h:114 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_I_s_G_a_u_s_s_i_a_n │ │ │ │ │ -typename std::enable_if< std::is_base_of< GaussianFactor, FACTOR >::value >:: │ │ │ │ │ -type IsGaussian │ │ │ │ │ -Check if FACTOR type is derived from GaussianFactor. │ │ │ │ │ -DDeeffiinniittiioonn HybridGaussianFactorGraph.h:107 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_I_n_d_i_c_e_s │ │ │ │ │ -KeyVector Indices │ │ │ │ │ -map from keys to values │ │ │ │ │ -DDeeffiinniittiioonn HybridGaussianFactorGraph.h:117 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_B_a_s_e_E_l_i_m_i_n_a_t_e_a_b_l_e │ │ │ │ │ -EliminateableFactorGraph< This > BaseEliminateable │ │ │ │ │ -for elimination │ │ │ │ │ -DDeeffiinniittiioonn HybridGaussianFactorGraph.h:113 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -HybridGaussianFactorGraph()=default │ │ │ │ │ -Default constructor. │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -HybridGaussianFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph) │ │ │ │ │ -Implicit copy/downcast constructor to override explicit template container │ │ │ │ │ -constructor. │ │ │ │ │ -DDeeffiinniittiioonn HybridGaussianFactorGraph.h:131 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ -An EliminatableClusterTree, i.e., a set of variable clusters with factors, │ │ │ │ │ -arranged in a tree,... │ │ │ │ │ -DDeeffiinniittiioonn HybridJunctionTree.h:53 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_S_y_m_b_o_l_i_c │ │ │ │ │ +std::pair< boost::shared_ptr< SymbolicConditional >, boost::shared_ptr< │ │ │ │ │ +SymbolicFactor > > EliminateSymbolic(const SymbolicFactorGraph &factors, const │ │ │ │ │ +Ordering &keys) │ │ │ │ │ +Dense elimination function for symbolic factors. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactor.cpp:36 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ _g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ nodes. │ │ │ │ │ DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s │ │ │ │ │ Traits class for eliminateable factor graphs, specifies the types that result │ │ │ │ │ from elimination,... │ │ │ │ │ DDeeffiinniittiioonn EliminateableFactorGraph.h:36 │ │ │ │ │ _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ EliminateableFactorGraph is a base class for factor graphs that contains │ │ │ │ │ elimination algorithms. │ │ │ │ │ DDeeffiinniittiioonn EliminateableFactorGraph.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:68 │ │ │ │ │ _g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ -with a unique integer... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -the error. │ │ │ │ │ -_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ -In Gaussian factors, the error function returns either the negative log- │ │ │ │ │ -likelihood,... │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d │ │ │ │ │ +static Ordering Colamd(const FACTOR_GRAPH &graph) │ │ │ │ │ +Compute a fill-reducing ordering using COLAMD from a factor graph (see details │ │ │ │ │ +for note on performanc... │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:95 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t │ │ │ │ │ +A SymbolicBayesNet is a Bayes Net of purely symbolic conditionals. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicBayesNet.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e │ │ │ │ │ +A Bayes tree that represents the connectivity between variables but is not │ │ │ │ │ +associated with any probab... │ │ │ │ │ +DDeeffiinniittiioonn SymbolicBayesTree.h:51 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +SymbolicConditional is a conditional with keys but no probability data, │ │ │ │ │ +produced by symbolic eliminat... │ │ │ │ │ +DDeeffiinniittiioonn SymbolicConditional.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ +DDeeffiinniittiioonn SymbolicEliminationTree.h:29 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r │ │ │ │ │ +SymbolicFactor represents a symbolic factor that specifies graph topology but │ │ │ │ │ +is not associated with ... │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactor.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_F_a_c_t_o_r_T_y_p_e │ │ │ │ │ +SymbolicFactor FactorType │ │ │ │ │ +Type of factors in factor graph. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactorGraph.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_F_a_c_t_o_r_G_r_a_p_h_T_y_p_e │ │ │ │ │ +SymbolicFactorGraph FactorGraphType │ │ │ │ │ +Type of the factor graph (e.g. GaussianFactorGraph) │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactorGraph.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_T_y_p_e │ │ │ │ │ +SymbolicEliminationTree EliminationTreeType │ │ │ │ │ +Type of elimination tree. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactorGraph.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_B_a_y_e_s_N_e_t_T_y_p_e │ │ │ │ │ +SymbolicBayesNet BayesNetType │ │ │ │ │ +Type of Bayes net from sequential elimination. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactorGraph.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_D_e_f_a_u_l_t_E_l_i_m_i_n_a_t_e │ │ │ │ │ +static std::pair< boost::shared_ptr< ConditionalType >, boost::shared_ptr< │ │ │ │ │ +FactorType > > DefaultEliminate(const FactorGraphType &factors, const Ordering │ │ │ │ │ +&keys) │ │ │ │ │ +The default dense elimination function. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactorGraph.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_D_e_f_a_u_l_t_O_r_d_e_r_i_n_g_F_u_n_c │ │ │ │ │ +static Ordering DefaultOrderingFunc(const FactorGraphType &graph, boost:: │ │ │ │ │ +optional< const VariableIndex & > variableIndex) │ │ │ │ │ +The default ordering generation function. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactorGraph.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_J_u_n_c_t_i_o_n_T_r_e_e_T_y_p_e │ │ │ │ │ +SymbolicJunctionTree JunctionTreeType │ │ │ │ │ +Type of Junction tree. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactorGraph.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_B_a_y_e_s_T_r_e_e_T_y_p_e │ │ │ │ │ +SymbolicBayesTree BayesTreeType │ │ │ │ │ +Type of Bayes tree. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactorGraph.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_C_o_n_d_i_t_i_o_n_a_l_T_y_p_e │ │ │ │ │ +SymbolicConditional ConditionalType │ │ │ │ │ +Type of conditionals from elimination. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactorGraph.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +Symbolic Factor Graph. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactorGraph.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +SymbolicFactorGraph(SymbolicFactor &&c) │ │ │ │ │ +Construct from a single factor. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactorGraph.h:99 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ +SymbolicFactorGraph & operator()(SymbolicFactor &&c) │ │ │ │ │ +Add a single factor and return a reference. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactorGraph.h:109 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +SymbolicFactorGraph() │ │ │ │ │ +Construct empty factor graph. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactorGraph.h:76 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +SymbolicFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph) │ │ │ │ │ +Implicit copy/downcast constructor to override explicit template container │ │ │ │ │ +constructor. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactorGraph.h:88 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +SymbolicFactorGraph(std::initializer_list< boost::shared_ptr< SymbolicFactor > │ │ │ │ │ +> sharedFactors) │ │ │ │ │ +Constructor that takes an initializer list of shared pointers. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactorGraph.h:94 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="SymbolicFactorGraph", const KeyFormatter │ │ │ │ │ +&formatter=DefaultKeyFormatter) const override │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactorGraph.h:125 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_:_:_B_a_s_e_E_l_i_m_i_n_a_t_e_a_b_l_e │ │ │ │ │ +EliminateableFactorGraph< This > BaseEliminateable │ │ │ │ │ +Typedef to base elimination class. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactorGraph.h:69 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactorGraph.h:70 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_:_:_T_h_i_s │ │ │ │ │ +SymbolicFactorGraph This │ │ │ │ │ +Typedef to this class. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactorGraph.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +SymbolicFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) │ │ │ │ │ +Construct from iterator over factors. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactorGraph.h:80 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_:_:_~_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +virtual ~SymbolicFactorGraph() │ │ │ │ │ +Destructor. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactorGraph.h:115 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_:_:_B_a_s_e │ │ │ │ │ +FactorGraph< SymbolicFactor > Base │ │ │ │ │ +Typedef to base factor graph type. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactorGraph.h:68 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +SymbolicFactorGraph(const CONTAINER &factors) │ │ │ │ │ +Construct from container of factors (shared_ptr or plain objects) │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactorGraph.h:84 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ +A EliminatableClusterTree, i.e., a set of variable clusters with factors, │ │ │ │ │ +arranged in a tree,... │ │ │ │ │ +DDeeffiinniittiioonn SymbolicJunctionTree.h:51 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ + * _s_y_m_b_o_l_i_c │ │ │ │ │ + * _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00560_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00914_source.html │ │ │ │┄ Files 90% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridConditional.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AHRSFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
HybridConditional.h
│ │ │ │ +
AHRSFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/discrete/DiscreteConditional.h>
│ │ │ │ -
21#include <gtsam/hybrid/GaussianMixture.h>
│ │ │ │ -
22#include <gtsam/hybrid/HybridFactor.h>
│ │ │ │ -
23#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
│ │ │ │ -
24#include <gtsam/inference/Conditional.h>
│ │ │ │ -
25#include <gtsam/inference/Key.h>
│ │ │ │ -
26#include <gtsam/linear/GaussianConditional.h>
│ │ │ │ -
27
│ │ │ │ -
28#include <boost/make_shared.hpp>
│ │ │ │ -
29#include <boost/shared_ptr.hpp>
│ │ │ │ -
30#include <stdexcept>
│ │ │ │ -
31#include <string>
│ │ │ │ -
32#include <typeinfo>
│ │ │ │ -
33#include <vector>
│ │ │ │ -
34
│ │ │ │ -
35namespace gtsam {
│ │ │ │ -
36
│ │ │ │ -
│ │ │ │ -
60class GTSAM_EXPORT HybridConditional
│ │ │ │ -
61 : public HybridFactor,
│ │ │ │ -
62 public Conditional<HybridFactor, HybridConditional> {
│ │ │ │ -
63 public:
│ │ │ │ -
64 // typedefs needed to play nice with gtsam
│ │ │ │ -
65 typedef HybridConditional This;
│ │ │ │ -
66 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
67 typedef HybridFactor BaseFactor;
│ │ │ │ -
68 typedef Conditional<BaseFactor, This>
│ │ │ │ -
69 BaseConditional;
│ │ │ │ -
70
│ │ │ │ -
71 protected:
│ │ │ │ -
73 boost::shared_ptr<Factor> inner_;
│ │ │ │ -
74
│ │ │ │ -
75 public:
│ │ │ │ -
78
│ │ │ │ -
80 HybridConditional() = default;
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22/* GTSAM includes */
│ │ │ │ +
23#include <gtsam/navigation/PreintegratedRotation.h>
│ │ │ │ +
24#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
25#include <gtsam/geometry/Pose3.h>
│ │ │ │ +
26
│ │ │ │ +
27namespace gtsam {
│ │ │ │ +
28
│ │ │ │ +
│ │ │ │ +
34class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation {
│ │ │ │ +
35
│ │ │ │ +
36 protected:
│ │ │ │ +
37
│ │ │ │ +
38 Vector3 biasHat_;
│ │ │ │ +
39 Matrix3 preintMeasCov_;
│ │ │ │ +
40
│ │ │ │ +
41 friend class AHRSFactor;
│ │ │ │ +
42
│ │ │ │ +
43 public:
│ │ │ │ +
44
│ │ │ │ +
46 PreintegratedAhrsMeasurements() {}
│ │ │ │ +
47
│ │ │ │ +
│ │ │ │ +
52 PreintegratedAhrsMeasurements(const boost::shared_ptr<Params>& p,
│ │ │ │ +
53 const Vector3& biasHat) :
│ │ │ │ +
54 PreintegratedRotation(p), biasHat_(biasHat) {
│ │ │ │ +
55 resetIntegration();
│ │ │ │ +
56 }
│ │ │ │ +
│ │ │ │ +
57
│ │ │ │ +
│ │ │ │ +
67 PreintegratedAhrsMeasurements(
│ │ │ │ +
68 const boost::shared_ptr<Params>& p,
│ │ │ │ +
69 const Vector3& bias_hat,
│ │ │ │ +
70 double deltaTij,
│ │ │ │ +
71 const Rot3& deltaRij,
│ │ │ │ +
72 const Matrix3& delRdelBiasOmega,
│ │ │ │ +
73 const Matrix3& preint_meas_cov) :
│ │ │ │ +
74 PreintegratedRotation(p, deltaTij, deltaRij, delRdelBiasOmega),
│ │ │ │ +
75 biasHat_(bias_hat),
│ │ │ │ +
76 preintMeasCov_(preint_meas_cov) {}
│ │ │ │ +
│ │ │ │ +
77
│ │ │ │ +
78 Params& p() const { return *boost::static_pointer_cast<Params>(p_);}
│ │ │ │ +
79 const Vector3& biasHat() const { return biasHat_; }
│ │ │ │ +
80 const Matrix3& preintMeasCov() const { return preintMeasCov_; }
│ │ │ │
81
│ │ │ │ -
│ │ │ │ -
89 HybridConditional(const KeyVector& continuousKeys,
│ │ │ │ -
90 const DiscreteKeys& discreteKeys, size_t nFrontals)
│ │ │ │ -
91 : BaseFactor(continuousKeys, discreteKeys), BaseConditional(nFrontals) {}
│ │ │ │ -
│ │ │ │ -
92
│ │ │ │ -
102 HybridConditional(const KeyVector& continuousFrontals,
│ │ │ │ -
103 const DiscreteKeys& discreteFrontals,
│ │ │ │ -
104 const KeyVector& continuousParents,
│ │ │ │ -
105 const DiscreteKeys& discreteParents);
│ │ │ │ -
106
│ │ │ │ -
113 HybridConditional(
│ │ │ │ -
114 const boost::shared_ptr<GaussianConditional>& continuousConditional);
│ │ │ │ -
115
│ │ │ │ -
122 HybridConditional(
│ │ │ │ -
123 const boost::shared_ptr<DiscreteConditional>& discreteConditional);
│ │ │ │ -
124
│ │ │ │ -
131 HybridConditional(const boost::shared_ptr<GaussianMixture>& gaussianMixture);
│ │ │ │ +
83 void print(const std::string& s = "Preintegrated Measurements: ") const;
│ │ │ │ +
84
│ │ │ │ +
86 bool equals(const PreintegratedAhrsMeasurements&, double tol = 1e-9) const;
│ │ │ │ +
87
│ │ │ │ +
89 void resetIntegration();
│ │ │ │ +
90
│ │ │ │ +
100 void integrateMeasurement(const Vector3& measuredOmega, double deltaT);
│ │ │ │ +
101
│ │ │ │ +
104 Vector3 predict(const Vector3& bias, OptionalJacobian<3,3> H = boost::none) const;
│ │ │ │ +
105
│ │ │ │ +
106 // This function is only used for test purposes
│ │ │ │ +
107 // (compare numerical derivatives wrt analytic ones)
│ │ │ │ +
108 static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt,
│ │ │ │ +
109 const Vector3& delta_angles);
│ │ │ │ +
110
│ │ │ │ +
│ │ │ │ +
112 PreintegratedAhrsMeasurements(const Vector3& biasHat,
│ │ │ │ +
113 const Matrix3& measuredOmegaCovariance)
│ │ │ │ +
114 : PreintegratedRotation(boost::make_shared<Params>()), biasHat_(biasHat) {
│ │ │ │ +
115 p_->gyroscopeCovariance = measuredOmegaCovariance;
│ │ │ │ +
116 resetIntegration();
│ │ │ │ +
117 }
│ │ │ │ +
│ │ │ │ +
118
│ │ │ │ +
119private:
│ │ │ │ +
120
│ │ │ │ +
122 friend class boost::serialization::access;
│ │ │ │ +
123 template<class ARCHIVE>
│ │ │ │ +
124 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
125 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation);
│ │ │ │ +
126 ar & BOOST_SERIALIZATION_NVP(p_);
│ │ │ │ +
127 ar & BOOST_SERIALIZATION_NVP(biasHat_);
│ │ │ │ +
128 }
│ │ │ │ +
129};
│ │ │ │ +
│ │ │ │ +
130
│ │ │ │ +
│ │ │ │ +
131class GTSAM_EXPORT AHRSFactor: public NoiseModelFactorN<Rot3, Rot3, Vector3> {
│ │ │ │
132
│ │ │ │ -
136
│ │ │ │ -
138 void print(
│ │ │ │ -
139 const std::string& s = "Hybrid Conditional: ",
│ │ │ │ -
140 const KeyFormatter& formatter = DefaultKeyFormatter) const override;
│ │ │ │ -
141
│ │ │ │ -
143 bool equals(const HybridFactor& other, double tol = 1e-9) const override;
│ │ │ │ -
144
│ │ │ │ -
148
│ │ │ │ -
│ │ │ │ -
154 GaussianMixture::shared_ptr asMixture() const {
│ │ │ │ -
155 return boost::dynamic_pointer_cast<GaussianMixture>(inner_);
│ │ │ │ -
156 }
│ │ │ │ -
│ │ │ │ -
157
│ │ │ │ -
│ │ │ │ -
163 GaussianConditional::shared_ptr asGaussian() const {
│ │ │ │ -
164 return boost::dynamic_pointer_cast<GaussianConditional>(inner_);
│ │ │ │ -
165 }
│ │ │ │ -
│ │ │ │ -
166
│ │ │ │ -
│ │ │ │ -
172 DiscreteConditional::shared_ptr asDiscrete() const {
│ │ │ │ -
173 return boost::dynamic_pointer_cast<DiscreteConditional>(inner_);
│ │ │ │ -
174 }
│ │ │ │ -
│ │ │ │ -
175
│ │ │ │ -
177 boost::shared_ptr<Factor> inner() const { return inner_; }
│ │ │ │ -
178
│ │ │ │ -
180 double error(const HybridValues& values) const override;
│ │ │ │ -
181
│ │ │ │ -
183 double logProbability(const HybridValues& values) const override;
│ │ │ │ -
184
│ │ │ │ -
190 double logNormalizationConstant() const override;
│ │ │ │ -
191
│ │ │ │ -
193 double evaluate(const HybridValues& values) const override;
│ │ │ │ -
194
│ │ │ │ -
│ │ │ │ -
196 bool frontalsIn(const VectorValues& measurements) const {
│ │ │ │ -
197 for (Key key : frontals()) {
│ │ │ │ -
198 if (!measurements.exists(key)) {
│ │ │ │ -
199 return false;
│ │ │ │ -
200 }
│ │ │ │ -
201 }
│ │ │ │ -
202 return true;
│ │ │ │ -
203 }
│ │ │ │ -
│ │ │ │ -
204
│ │ │ │ +
133 typedef AHRSFactor This;
│ │ │ │ +
134 typedef NoiseModelFactorN<Rot3, Rot3, Vector3> Base;
│ │ │ │ +
135
│ │ │ │ +
136 PreintegratedAhrsMeasurements _PIM_;
│ │ │ │ +
137
│ │ │ │ +
139 AHRSFactor() {}
│ │ │ │ +
140
│ │ │ │ +
141public:
│ │ │ │ +
142
│ │ │ │ +
144#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
│ │ │ │ +
145 typedef typename boost::shared_ptr<AHRSFactor> shared_ptr;
│ │ │ │ +
146#else
│ │ │ │ +
147 typedef boost::shared_ptr<AHRSFactor> shared_ptr;
│ │ │ │ +
148#endif
│ │ │ │ +
149
│ │ │ │ +
157 AHRSFactor(Key rot_i, Key rot_j, Key bias,
│ │ │ │ +
158 const PreintegratedAhrsMeasurements& preintegratedMeasurements);
│ │ │ │ +
159
│ │ │ │ +
160 ~AHRSFactor() override {
│ │ │ │ +
161 }
│ │ │ │ +
162
│ │ │ │ +
164 gtsam::NonlinearFactor::shared_ptr clone() const override;
│ │ │ │ +
165
│ │ │ │ +
167 void print(const std::string& s, const KeyFormatter& keyFormatter =
│ │ │ │ +
168 DefaultKeyFormatter) const override;
│ │ │ │ +
169
│ │ │ │ +
171 bool equals(const NonlinearFactor&, double tol = 1e-9) const override;
│ │ │ │ +
172
│ │ │ │ +
│ │ │ │ +
174 const PreintegratedAhrsMeasurements& preintegratedMeasurements() const {
│ │ │ │ +
175 return _PIM_;
│ │ │ │ +
176 }
│ │ │ │ +
│ │ │ │ +
177
│ │ │ │ +
181 Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j,
│ │ │ │ +
182 const Vector3& bias, boost::optional<Matrix&> H1 = boost::none,
│ │ │ │ +
183 boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
│ │ │ │ +
184 boost::none) const override;
│ │ │ │ +
185
│ │ │ │ +
188 static Rot3 Predict(const Rot3& rot_i, const Vector3& bias,
│ │ │ │ +
189 const PreintegratedAhrsMeasurements& pim);
│ │ │ │ +
190
│ │ │ │ +
192 AHRSFactor(Key rot_i, Key rot_j, Key bias,
│ │ │ │ +
193 const PreintegratedAhrsMeasurements& pim,
│ │ │ │ +
194 const Vector3& omegaCoriolis,
│ │ │ │ +
195 const boost::optional<Pose3>& body_P_sensor = boost::none);
│ │ │ │ +
196
│ │ │ │ +
198 static Rot3 predict(
│ │ │ │ +
199 const Rot3& rot_i, const Vector3& bias,
│ │ │ │ +
200 const PreintegratedAhrsMeasurements& pim, const Vector3& omegaCoriolis,
│ │ │ │ +
201 const boost::optional<Pose3>& body_P_sensor = boost::none);
│ │ │ │ +
202
│ │ │ │ +
203#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ +
205 typedef PreintegratedAhrsMeasurements PreintegratedMeasurements;
│ │ │ │
206
│ │ │ │ -
207 private:
│ │ │ │ -
209 friend class boost::serialization::access;
│ │ │ │ -
210 template <class Archive>
│ │ │ │ -
211 void serialize(Archive& ar, const unsigned int /*version*/) {
│ │ │ │ -
212 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseFactor);
│ │ │ │ -
213 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(BaseConditional);
│ │ │ │ -
214 ar& BOOST_SERIALIZATION_NVP(inner_);
│ │ │ │ -
215
│ │ │ │ -
216 // register the various casts based on the type of inner_
│ │ │ │ -
217 // https://www.boost.org/doc/libs/1_80_0/libs/serialization/doc/serialization.html#runtimecasting
│ │ │ │ -
218 if (isDiscrete()) {
│ │ │ │ -
219 boost::serialization::void_cast_register<DiscreteConditional, Factor>(
│ │ │ │ -
220 static_cast<DiscreteConditional*>(NULL), static_cast<Factor*>(NULL));
│ │ │ │ -
221 } else if (isContinuous()) {
│ │ │ │ -
222 boost::serialization::void_cast_register<GaussianConditional, Factor>(
│ │ │ │ -
223 static_cast<GaussianConditional*>(NULL), static_cast<Factor*>(NULL));
│ │ │ │ -
224 } else {
│ │ │ │ -
225 boost::serialization::void_cast_register<GaussianMixture, Factor>(
│ │ │ │ -
226 static_cast<GaussianMixture*>(NULL), static_cast<Factor*>(NULL));
│ │ │ │ -
227 }
│ │ │ │ -
228 }
│ │ │ │ -
229
│ │ │ │ -
230}; // HybridConditional
│ │ │ │ -
│ │ │ │ -
231
│ │ │ │ -
232// traits
│ │ │ │ -
233template <>
│ │ │ │ -
234struct traits<HybridConditional> : public Testable<HybridConditional> {};
│ │ │ │ -
235
│ │ │ │ -
236} // namespace gtsam
│ │ │ │ - │ │ │ │ -
GaussianMixture.h
A hybrid conditional in the Conditional Linear Gaussian scheme.
│ │ │ │ -
HybridGaussianFactorGraph.h
Linearized Hybrid factor graph that uses type erasure.
│ │ │ │ - │ │ │ │ - │ │ │ │ -
Conditional.h
Base class for conditional densities.
│ │ │ │ -
GaussianConditional.h
Conditional Gaussian Base class.
│ │ │ │ +
207#endif
│ │ │ │ +
208
│ │ │ │ +
209private:
│ │ │ │ +
210
│ │ │ │ +
212 friend class boost::serialization::access;
│ │ │ │ +
213 template<class ARCHIVE>
│ │ │ │ +
214 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
215 // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ +
216 ar
│ │ │ │ +
217 & boost::serialization::make_nvp("NoiseModelFactor3",
│ │ │ │ +
218 boost::serialization::base_object<Base>(*this));
│ │ │ │ +
219 ar & BOOST_SERIALIZATION_NVP(_PIM_);
│ │ │ │ +
220 }
│ │ │ │ +
221
│ │ │ │ +
222};
│ │ │ │ +
│ │ │ │ +
223// AHRSFactor
│ │ │ │ +
224
│ │ │ │ +
225} //namespace gtsam
│ │ │ │ +
Pose3.h
3D Pose
│ │ │ │ + │ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::make_shared
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost::shared_ptr< T > > make_shared(Args &&... args)
Add our own make_shared as a layer of wrapping on boost::make_shared This solves the problem with the...
Definition make_shared.h:57
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::DiscreteConditional
Discrete Conditional Density Derives from DecisionTreeFactor.
Definition DiscreteConditional.h:40
│ │ │ │ -
gtsam::DiscreteConditional::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition DiscreteConditional.h:44
│ │ │ │ -
gtsam::DiscreteKeys
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition DiscreteKey.h:39
│ │ │ │ -
gtsam::HybridConditional
Hybrid Conditional Density.
Definition HybridConditional.h:62
│ │ │ │ -
gtsam::HybridConditional::BaseConditional
Conditional< BaseFactor, This > BaseConditional
Typedef to our conditional base class.
Definition HybridConditional.h:69
│ │ │ │ -
gtsam::HybridConditional::BaseFactor
HybridFactor BaseFactor
Typedef to our factor base class.
Definition HybridConditional.h:67
│ │ │ │ -
gtsam::HybridConditional::inner_
boost::shared_ptr< Factor > inner_
Type-erased pointer to the inner type.
Definition HybridConditional.h:73
│ │ │ │ -
gtsam::HybridConditional::asDiscrete
DiscreteConditional::shared_ptr asDiscrete() const
Return conditional as a DiscreteConditional.
Definition HybridConditional.h:172
│ │ │ │ -
gtsam::HybridConditional::asMixture
GaussianMixture::shared_ptr asMixture() const
Return HybridConditional as a GaussianMixture.
Definition HybridConditional.h:154
│ │ │ │ -
gtsam::HybridConditional::inner
boost::shared_ptr< Factor > inner() const
Get the type-erased pointer to the inner type.
Definition HybridConditional.h:177
│ │ │ │ -
gtsam::HybridConditional::frontalsIn
bool frontalsIn(const VectorValues &measurements) const
Check if VectorValues measurements contains all frontal keys.
Definition HybridConditional.h:196
│ │ │ │ -
gtsam::HybridConditional::HybridConditional
HybridConditional(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys, size_t nFrontals)
Construct a new Hybrid Conditional object.
Definition HybridConditional.h:89
│ │ │ │ -
gtsam::HybridConditional::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition HybridConditional.h:66
│ │ │ │ -
gtsam::HybridConditional::asGaussian
GaussianConditional::shared_ptr asGaussian() const
Return HybridConditional as a GaussianConditional.
Definition HybridConditional.h:163
│ │ │ │ -
gtsam::HybridConditional::This
HybridConditional This
Typedef to this class.
Definition HybridConditional.h:65
│ │ │ │ -
gtsam::HybridConditional::HybridConditional
HybridConditional()=default
Default constructor needed for serialization.
│ │ │ │ -
gtsam::HybridFactor
Base class for truly hybrid probabilistic factors.
Definition HybridFactor.h:52
│ │ │ │ -
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ -
gtsam::Conditional
Definition Conditional.h:64
│ │ │ │ -
gtsam::Factor
Definition Factor.h:68
│ │ │ │ -
gtsam::GaussianConditional
A GaussianConditional functions as the node in a Bayes network.
Definition GaussianConditional.h:43
│ │ │ │ -
gtsam::GaussianConditional::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianConditional.h:46
│ │ │ │ -
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ -
gtsam::VectorValues::exists
bool exists(Key j) const
Check whether a variable with key j exists.
Definition VectorValues.h:133
│ │ │ │ +
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
│ │ │ │ +
gtsam::PreintegratedAhrsMeasurements
PreintegratedAHRSMeasurements accumulates (integrates) the Gyroscope measurements (rotation rates) an...
Definition AHRSFactor.h:34
│ │ │ │ +
gtsam::PreintegratedAhrsMeasurements::biasHat_
Vector3 biasHat_
Angular rate bias values used during preintegration.
Definition AHRSFactor.h:38
│ │ │ │ +
gtsam::PreintegratedAhrsMeasurements::PreintegratedAhrsMeasurements
PreintegratedAhrsMeasurements(const Vector3 &biasHat, const Matrix3 &measuredOmegaCovariance)
Definition AHRSFactor.h:112
│ │ │ │ +
gtsam::PreintegratedAhrsMeasurements::PreintegratedAhrsMeasurements
PreintegratedAhrsMeasurements(const boost::shared_ptr< Params > &p, const Vector3 &biasHat)
Default constructor, initialize with no measurements.
Definition AHRSFactor.h:52
│ │ │ │ +
gtsam::PreintegratedAhrsMeasurements::preintMeasCov_
Matrix3 preintMeasCov_
Covariance matrix of the preintegrated measurements (first-order propagation from measurementCovarian...
Definition AHRSFactor.h:39
│ │ │ │ +
gtsam::PreintegratedAhrsMeasurements::PreintegratedAhrsMeasurements
PreintegratedAhrsMeasurements(const boost::shared_ptr< Params > &p, const Vector3 &bias_hat, double deltaTij, const Rot3 &deltaRij, const Matrix3 &delRdelBiasOmega, const Matrix3 &preint_meas_cov)
Non-Default constructor, initialize with measurements.
Definition AHRSFactor.h:67
│ │ │ │ +
gtsam::PreintegratedAhrsMeasurements::PreintegratedAhrsMeasurements
PreintegratedAhrsMeasurements()
Default constructor, only for serialization and wrappers.
Definition AHRSFactor.h:46
│ │ │ │ +
gtsam::AHRSFactor
Definition AHRSFactor.h:131
│ │ │ │ +
gtsam::AHRSFactor::preintegratedMeasurements
const PreintegratedAhrsMeasurements & preintegratedMeasurements() const
Access the preintegrated measurements.
Definition AHRSFactor.h:174
│ │ │ │ +
gtsam::AHRSFactor::shared_ptr
boost::shared_ptr< AHRSFactor > shared_ptr
Shorthand for a smart pointer to a factor.
Definition AHRSFactor.h:147
│ │ │ │ +
gtsam::PreintegratedRotationParams
Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
Definition PreintegratedRotation.h:31
│ │ │ │ +
gtsam::PreintegratedRotation
PreintegratedRotation is the base class for all PreintegratedMeasurements classes (in AHRSFactor,...
Definition PreintegratedRotation.h:89
│ │ │ │ +
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ +
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,275 +1,267 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -HybridConditional.h │ │ │ │ │ +AHRSFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_F_a_c_t_o_r_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ -27 │ │ │ │ │ -28#include │ │ │ │ │ -29#include │ │ │ │ │ -30#include │ │ │ │ │ -31#include │ │ │ │ │ -32#include │ │ │ │ │ -33#include │ │ │ │ │ -34 │ │ │ │ │ -35namespace _g_t_s_a_m { │ │ │ │ │ -36 │ │ │ │ │ -_6_0class GTSAM_EXPORT _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -61 : public _H_y_b_r_i_d_F_a_c_t_o_r, │ │ │ │ │ -62 public _C_o_n_d_i_t_i_o_n_a_l { │ │ │ │ │ -63 public: │ │ │ │ │ -64 // typedefs needed to play nice with gtsam │ │ │ │ │ -_6_5 typedef _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l _T_h_i_s; │ │ │ │ │ -_6_6 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -_6_7 typedef _H_y_b_r_i_d_F_a_c_t_o_r _B_a_s_e_F_a_c_t_o_r; │ │ │ │ │ -68 typedef _C_o_n_d_i_t_i_o_n_a_l_<_B_a_s_e_F_a_c_t_o_r_,_ _T_h_i_s_> │ │ │ │ │ -_6_9 _B_a_s_e_C_o_n_d_i_t_i_o_n_a_l; │ │ │ │ │ -70 │ │ │ │ │ -71 protected: │ │ │ │ │ -_7_3 boost::shared_ptr _i_n_n_e_r__; │ │ │ │ │ -74 │ │ │ │ │ -75 public: │ │ │ │ │ -78 │ │ │ │ │ -_8_0 _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l() = default; │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22/* GTSAM includes */ │ │ │ │ │ +23#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_3_._h> │ │ │ │ │ +26 │ │ │ │ │ +27namespace _g_t_s_a_m { │ │ │ │ │ +28 │ │ │ │ │ +_3_4class GTSAM_EXPORT _P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s : public │ │ │ │ │ +_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n { │ │ │ │ │ +35 │ │ │ │ │ +36 protected: │ │ │ │ │ +37 │ │ │ │ │ +_3_8 Vector3 _b_i_a_s_H_a_t__; │ │ │ │ │ +_3_9 Matrix3 _p_r_e_i_n_t_M_e_a_s_C_o_v__; │ │ │ │ │ +40 │ │ │ │ │ +41 friend class _A_H_R_S_F_a_c_t_o_r; │ │ │ │ │ +42 │ │ │ │ │ +43 public: │ │ │ │ │ +44 │ │ │ │ │ +_4_6 _P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s() {} │ │ │ │ │ +47 │ │ │ │ │ +_5_2 _P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s(const boost::shared_ptr& p, │ │ │ │ │ +53 const Vector3& biasHat) : │ │ │ │ │ +54 _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n(p), biasHat_(biasHat) { │ │ │ │ │ +55 resetIntegration(); │ │ │ │ │ +56 } │ │ │ │ │ +57 │ │ │ │ │ +_6_7 _P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s( │ │ │ │ │ +68 const boost::shared_ptr& p, │ │ │ │ │ +69 const Vector3& bias_hat, │ │ │ │ │ +70 double deltaTij, │ │ │ │ │ +71 const _R_o_t_3& deltaRij, │ │ │ │ │ +72 const Matrix3& delRdelBiasOmega, │ │ │ │ │ +73 const Matrix3& preint_meas_cov) : │ │ │ │ │ +74 _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n(p, deltaTij, deltaRij, delRdelBiasOmega), │ │ │ │ │ +75 biasHat_(bias_hat), │ │ │ │ │ +76 preintMeasCov_(preint_meas_cov) {} │ │ │ │ │ +77 │ │ │ │ │ +78 Params& p() const { return *boost::static_pointer_cast(p_);} │ │ │ │ │ +79 const Vector3& biasHat() const { return biasHat_; } │ │ │ │ │ +80 const Matrix3& preintMeasCov() const { return preintMeasCov_; } │ │ │ │ │ 81 │ │ │ │ │ -_8_9 _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l(const _K_e_y_V_e_c_t_o_r& continuousKeys, │ │ │ │ │ -90 const _D_i_s_c_r_e_t_e_K_e_y_s& discreteKeys, size_t nFrontals) │ │ │ │ │ -91 : _B_a_s_e_F_a_c_t_o_r(continuousKeys, discreteKeys), _B_a_s_e_C_o_n_d_i_t_i_o_n_a_l(nFrontals) {} │ │ │ │ │ -92 │ │ │ │ │ -102 _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l(const _K_e_y_V_e_c_t_o_r& continuousFrontals, │ │ │ │ │ -103 const _D_i_s_c_r_e_t_e_K_e_y_s& discreteFrontals, │ │ │ │ │ -104 const _K_e_y_V_e_c_t_o_r& continuousParents, │ │ │ │ │ -105 const _D_i_s_c_r_e_t_e_K_e_y_s& discreteParents); │ │ │ │ │ -106 │ │ │ │ │ -113 _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l( │ │ │ │ │ -114 const boost::shared_ptr& continuousConditional); │ │ │ │ │ -115 │ │ │ │ │ -122 _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l( │ │ │ │ │ -123 const boost::shared_ptr& discreteConditional); │ │ │ │ │ -124 │ │ │ │ │ -131 _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l(const boost::shared_ptr& │ │ │ │ │ -gaussianMixture); │ │ │ │ │ +83 void _p_r_i_n_t(const std::string& s = "Preintegrated Measurements: ") const; │ │ │ │ │ +84 │ │ │ │ │ +86 bool equals(const PreintegratedAhrsMeasurements&, double tol = 1e-9) const; │ │ │ │ │ +87 │ │ │ │ │ +89 void resetIntegration(); │ │ │ │ │ +90 │ │ │ │ │ +100 void integrateMeasurement(const Vector3& measuredOmega, double deltaT); │ │ │ │ │ +101 │ │ │ │ │ +104 Vector3 predict(const Vector3& bias, OptionalJacobian<3,3> H = boost::none) │ │ │ │ │ +const; │ │ │ │ │ +105 │ │ │ │ │ +106 // This function is only used for test purposes │ │ │ │ │ +107 // (compare numerical derivatives wrt analytic ones) │ │ │ │ │ +108 static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt, │ │ │ │ │ +109 const Vector3& delta_angles); │ │ │ │ │ +110 │ │ │ │ │ +_1_1_2 _P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s(const Vector3& biasHat, │ │ │ │ │ +113 const Matrix3& measuredOmegaCovariance) │ │ │ │ │ +114 : _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n(boost::_m_a_k_e___s_h_a_r_e_d<_P_a_r_a_m_s>()), biasHat_(biasHat) { │ │ │ │ │ +115 p_->gyroscopeCovariance = measuredOmegaCovariance; │ │ │ │ │ +116 resetIntegration(); │ │ │ │ │ +117 } │ │ │ │ │ +118 │ │ │ │ │ +119private: │ │ │ │ │ +120 │ │ │ │ │ +_1_2_2 friend class boost::serialization::access; │ │ │ │ │ +123 template │ │ │ │ │ +124 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +125 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n); │ │ │ │ │ +126 ar & BOOST_SERIALIZATION_NVP(p_); │ │ │ │ │ +127 ar & BOOST_SERIALIZATION_NVP(biasHat_); │ │ │ │ │ +128 } │ │ │ │ │ +129}; │ │ │ │ │ +130 │ │ │ │ │ +_1_3_1class GTSAM_EXPORT _A_H_R_S_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ +{ │ │ │ │ │ 132 │ │ │ │ │ -136 │ │ │ │ │ -138 void _p_r_i_n_t( │ │ │ │ │ -139 const std::string& s = "Hybrid Conditional: ", │ │ │ │ │ -140 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override; │ │ │ │ │ -141 │ │ │ │ │ -143 bool _e_q_u_a_l_s(const _H_y_b_r_i_d_F_a_c_t_o_r& other, double tol = 1e-9) const override; │ │ │ │ │ -144 │ │ │ │ │ -148 │ │ │ │ │ -_1_5_4 GaussianMixture::shared_ptr _a_s_M_i_x_t_u_r_e() const { │ │ │ │ │ -155 return boost::dynamic_pointer_cast(inner_); │ │ │ │ │ -156 } │ │ │ │ │ -157 │ │ │ │ │ -_1_6_3 _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r _a_s_G_a_u_s_s_i_a_n() const { │ │ │ │ │ -164 return boost::dynamic_pointer_cast(inner_); │ │ │ │ │ -165 } │ │ │ │ │ -166 │ │ │ │ │ -_1_7_2 _D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r _a_s_D_i_s_c_r_e_t_e() const { │ │ │ │ │ -173 return boost::dynamic_pointer_cast(inner_); │ │ │ │ │ -174 } │ │ │ │ │ -175 │ │ │ │ │ -_1_7_7 boost::shared_ptr _i_n_n_e_r() const { return inner_; } │ │ │ │ │ -178 │ │ │ │ │ -180 double error(const _H_y_b_r_i_d_V_a_l_u_e_s& values) const override; │ │ │ │ │ -181 │ │ │ │ │ -183 double logProbability(const _H_y_b_r_i_d_V_a_l_u_e_s& values) const override; │ │ │ │ │ -184 │ │ │ │ │ -190 double logNormalizationConstant() const override; │ │ │ │ │ -191 │ │ │ │ │ -193 double evaluate(const _H_y_b_r_i_d_V_a_l_u_e_s& values) const override; │ │ │ │ │ -194 │ │ │ │ │ -_1_9_6 bool _f_r_o_n_t_a_l_s_I_n(const _V_e_c_t_o_r_V_a_l_u_e_s& measurements) const { │ │ │ │ │ -197 for (_K_e_y key : frontals()) { │ │ │ │ │ -198 if (!measurements._e_x_i_s_t_s(key)) { │ │ │ │ │ -199 return false; │ │ │ │ │ -200 } │ │ │ │ │ -201 } │ │ │ │ │ -202 return true; │ │ │ │ │ -203 } │ │ │ │ │ -204 │ │ │ │ │ +133 typedef _A_H_R_S_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ +134 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_R_o_t_3_,_ _R_o_t_3_,_ _V_e_c_t_o_r_3_> _B_a_s_e; │ │ │ │ │ +135 │ │ │ │ │ +136 _P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s _PIM_; │ │ │ │ │ +137 │ │ │ │ │ +139 _A_H_R_S_F_a_c_t_o_r() {} │ │ │ │ │ +140 │ │ │ │ │ +141public: │ │ │ │ │ +142 │ │ │ │ │ +144#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 │ │ │ │ │ +145 typedef typename boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +146#else │ │ │ │ │ +_1_4_7 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +148#endif │ │ │ │ │ +149 │ │ │ │ │ +157 _A_H_R_S_F_a_c_t_o_r(_K_e_y rot_i, _K_e_y rot_j, _K_e_y bias, │ │ │ │ │ +158 const _P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s& preintegratedMeasurements); │ │ │ │ │ +159 │ │ │ │ │ +160 _~_A_H_R_S_F_a_c_t_o_r() override { │ │ │ │ │ +161 } │ │ │ │ │ +162 │ │ │ │ │ +164 gtsam::NonlinearFactor::shared_ptr clone() const override; │ │ │ │ │ +165 │ │ │ │ │ +167 void _p_r_i_n_t(const std::string& s, const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ +168 DefaultKeyFormatter) const override; │ │ │ │ │ +169 │ │ │ │ │ +171 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r&, double tol = 1e-9) const override; │ │ │ │ │ +172 │ │ │ │ │ +_1_7_4 const _P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s& _p_r_e_i_n_t_e_g_r_a_t_e_d_M_e_a_s_u_r_e_m_e_n_t_s() const { │ │ │ │ │ +175 return _PIM_; │ │ │ │ │ +176 } │ │ │ │ │ +177 │ │ │ │ │ +181 Vector evaluateError(const _R_o_t_3& rot_i, const _R_o_t_3& rot_j, │ │ │ │ │ +182 const Vector3& bias, boost::optional H1 = boost::none, │ │ │ │ │ +183 boost::optional H2 = boost::none, boost::optional H3 = │ │ │ │ │ +184 boost::none) const override; │ │ │ │ │ +185 │ │ │ │ │ +188 static _R_o_t_3 Predict(const _R_o_t_3& rot_i, const Vector3& bias, │ │ │ │ │ +189 const _P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s& pim); │ │ │ │ │ +190 │ │ │ │ │ +192 _A_H_R_S_F_a_c_t_o_r(_K_e_y rot_i, _K_e_y rot_j, _K_e_y bias, │ │ │ │ │ +193 const _P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s& pim, │ │ │ │ │ +194 const Vector3& omegaCoriolis, │ │ │ │ │ +195 const boost::optional& body_P_sensor = boost::none); │ │ │ │ │ +196 │ │ │ │ │ +198 static _R_o_t_3 predict( │ │ │ │ │ +199 const _R_o_t_3& rot_i, const Vector3& bias, │ │ │ │ │ +200 const _P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s& pim, const Vector3& omegaCoriolis, │ │ │ │ │ +201 const boost::optional& body_P_sensor = boost::none); │ │ │ │ │ +202 │ │ │ │ │ +203#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ +205 typedef _P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s PreintegratedMeasurements; │ │ │ │ │ 206 │ │ │ │ │ -207 private: │ │ │ │ │ -_2_0_9 friend class boost::serialization::access; │ │ │ │ │ -210 template │ │ │ │ │ -211 void serialize(Archive& ar, const unsigned int /*version*/) { │ │ │ │ │ -212 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e_F_a_c_t_o_r); │ │ │ │ │ -213 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e_C_o_n_d_i_t_i_o_n_a_l); │ │ │ │ │ -214 ar& BOOST_SERIALIZATION_NVP(inner_); │ │ │ │ │ -215 │ │ │ │ │ -216 // register the various casts based on the type of inner_ │ │ │ │ │ -217 // https://www.boost.org/doc/libs/1_80_0/libs/serialization/doc/ │ │ │ │ │ -serialization.html#runtimecasting │ │ │ │ │ -218 if (isDiscrete()) { │ │ │ │ │ -219 boost::serialization::void_cast_register( │ │ │ │ │ -220 static_cast<_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l*>(NULL), static_cast<_F_a_c_t_o_r*>(NULL)); │ │ │ │ │ -221 } else if (isContinuous()) { │ │ │ │ │ -222 boost::serialization::void_cast_register( │ │ │ │ │ -223 static_cast<_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l*>(NULL), static_cast<_F_a_c_t_o_r*>(NULL)); │ │ │ │ │ -224 } else { │ │ │ │ │ -225 boost::serialization::void_cast_register( │ │ │ │ │ -226 static_cast(NULL), static_cast(NULL)); │ │ │ │ │ -227 } │ │ │ │ │ -228 } │ │ │ │ │ -229 │ │ │ │ │ -230}; // HybridConditional │ │ │ │ │ -231 │ │ │ │ │ -232// traits │ │ │ │ │ -233template <> │ │ │ │ │ -_2_3_4struct _t_r_a_i_t_s<_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ -235 │ │ │ │ │ -236} // namespace gtsam │ │ │ │ │ -_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ -_G_a_u_s_s_i_a_n_M_i_x_t_u_r_e_._h │ │ │ │ │ -A hybrid conditional in the Conditional Linear Gaussian scheme. │ │ │ │ │ -_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Linearized Hybrid factor graph that uses type erasure. │ │ │ │ │ -_H_y_b_r_i_d_F_a_c_t_o_r_._h │ │ │ │ │ -_K_e_y_._h │ │ │ │ │ -_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ -Base class for conditional densities. │ │ │ │ │ -_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ -Conditional Gaussian Base class. │ │ │ │ │ +207#endif │ │ │ │ │ +208 │ │ │ │ │ +209private: │ │ │ │ │ +210 │ │ │ │ │ +_2_1_2 friend class boost::serialization::access; │ │ │ │ │ +213 template │ │ │ │ │ +214 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +215 // NoiseModelFactor3 instead of NoiseModelFactorN for backward │ │ │ │ │ +compatibility │ │ │ │ │ +216 ar │ │ │ │ │ +217 & boost::serialization::make_nvp("NoiseModelFactor3", │ │ │ │ │ +218 boost::serialization::base_object(*this)); │ │ │ │ │ +219 ar & BOOST_SERIALIZATION_NVP(_PIM_); │ │ │ │ │ +220 } │ │ │ │ │ +221 │ │ │ │ │ +222}; │ │ │ │ │ +223// AHRSFactor │ │ │ │ │ +224 │ │ │ │ │ +225} //namespace gtsam │ │ │ │ │ +_P_o_s_e_3_._h │ │ │ │ │ +3D Pose │ │ │ │ │ +_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_._h │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ +Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ _g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ print without optional string, must specify cout yourself │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_m_a_k_e___s_h_a_r_e_d │ │ │ │ │ +gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost:: │ │ │ │ │ +shared_ptr< T > > make_shared(Args &&... args) │ │ │ │ │ +Add our own make_shared as a layer of wrapping on boost::make_shared This │ │ │ │ │ +solves the problem with the... │ │ │ │ │ +DDeeffiinniittiioonn make_shared.h:57 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ _g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ Template to create a binary predicate. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -Discrete Conditional Density Derives from DecisionTreeFactor. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteConditional.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn DiscreteConditional.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ -DiscreteKeys is a set of keys that can be assembled using the & operator. │ │ │ │ │ -DDeeffiinniittiioonn DiscreteKey.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -Hybrid Conditional Density. │ │ │ │ │ -DDeeffiinniittiioonn HybridConditional.h:62 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_:_:_B_a_s_e_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -Conditional< BaseFactor, This > BaseConditional │ │ │ │ │ -Typedef to our conditional base class. │ │ │ │ │ -DDeeffiinniittiioonn HybridConditional.h:69 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_:_:_B_a_s_e_F_a_c_t_o_r │ │ │ │ │ -HybridFactor BaseFactor │ │ │ │ │ -Typedef to our factor base class. │ │ │ │ │ -DDeeffiinniittiioonn HybridConditional.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_:_:_i_n_n_e_r__ │ │ │ │ │ -boost::shared_ptr< Factor > inner_ │ │ │ │ │ -Type-erased pointer to the inner type. │ │ │ │ │ -DDeeffiinniittiioonn HybridConditional.h:73 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_:_:_a_s_D_i_s_c_r_e_t_e │ │ │ │ │ -DiscreteConditional::shared_ptr asDiscrete() const │ │ │ │ │ -Return conditional as a DiscreteConditional. │ │ │ │ │ -DDeeffiinniittiioonn HybridConditional.h:172 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_:_:_a_s_M_i_x_t_u_r_e │ │ │ │ │ -GaussianMixture::shared_ptr asMixture() const │ │ │ │ │ -Return HybridConditional as a GaussianMixture. │ │ │ │ │ -DDeeffiinniittiioonn HybridConditional.h:154 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_:_:_i_n_n_e_r │ │ │ │ │ -boost::shared_ptr< Factor > inner() const │ │ │ │ │ -Get the type-erased pointer to the inner type. │ │ │ │ │ -DDeeffiinniittiioonn HybridConditional.h:177 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_:_:_f_r_o_n_t_a_l_s_I_n │ │ │ │ │ -bool frontalsIn(const VectorValues &measurements) const │ │ │ │ │ -Check if VectorValues measurements contains all frontal keys. │ │ │ │ │ -DDeeffiinniittiioonn HybridConditional.h:196 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -HybridConditional(const KeyVector &continuousKeys, const DiscreteKeys │ │ │ │ │ -&discreteKeys, size_t nFrontals) │ │ │ │ │ -Construct a new Hybrid Conditional object. │ │ │ │ │ -DDeeffiinniittiioonn HybridConditional.h:89 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn HybridConditional.h:66 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_:_:_a_s_G_a_u_s_s_i_a_n │ │ │ │ │ -GaussianConditional::shared_ptr asGaussian() const │ │ │ │ │ -Return HybridConditional as a GaussianConditional. │ │ │ │ │ -DDeeffiinniittiioonn HybridConditional.h:163 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_:_:_T_h_i_s │ │ │ │ │ -HybridConditional This │ │ │ │ │ -Typedef to this class. │ │ │ │ │ -DDeeffiinniittiioonn HybridConditional.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_:_:_H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -HybridConditional()=default │ │ │ │ │ -Default constructor needed for serialization. │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_F_a_c_t_o_r │ │ │ │ │ -Base class for truly hybrid probabilistic factors. │ │ │ │ │ -DDeeffiinniittiioonn HybridFactor.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -DDeeffiinniittiioonn Conditional.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:68 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -A GaussianConditional functions as the node in a Bayes network. │ │ │ │ │ -DDeeffiinniittiioonn GaussianConditional.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn GaussianConditional.h:46 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ -with a unique integer... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_e_x_i_s_t_s │ │ │ │ │ -bool exists(Key j) const │ │ │ │ │ -Check whether a variable with key j exists. │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:133 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ +Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor │ │ │ │ │ +symbol GTSAM_USE_QUATERNIO... │ │ │ │ │ +DDeeffiinniittiioonn Rot3.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ +PreintegratedAHRSMeasurements accumulates (integrates) the Gyroscope │ │ │ │ │ +measurements (rotation rates) an... │ │ │ │ │ +DDeeffiinniittiioonn AHRSFactor.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s_:_:_b_i_a_s_H_a_t__ │ │ │ │ │ +Vector3 biasHat_ │ │ │ │ │ +Angular rate bias values used during preintegration. │ │ │ │ │ +DDeeffiinniittiioonn AHRSFactor.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ +PreintegratedAhrsMeasurements(const Vector3 &biasHat, const Matrix3 │ │ │ │ │ +&measuredOmegaCovariance) │ │ │ │ │ +DDeeffiinniittiioonn AHRSFactor.h:112 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ +PreintegratedAhrsMeasurements(const boost::shared_ptr< Params > &p, const │ │ │ │ │ +Vector3 &biasHat) │ │ │ │ │ +Default constructor, initialize with no measurements. │ │ │ │ │ +DDeeffiinniittiioonn AHRSFactor.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s_:_:_p_r_e_i_n_t_M_e_a_s_C_o_v__ │ │ │ │ │ +Matrix3 preintMeasCov_ │ │ │ │ │ +Covariance matrix of the preintegrated measurements (first-order propagation │ │ │ │ │ +from measurementCovarian... │ │ │ │ │ +DDeeffiinniittiioonn AHRSFactor.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ +PreintegratedAhrsMeasurements(const boost::shared_ptr< Params > &p, const │ │ │ │ │ +Vector3 &bias_hat, double deltaTij, const Rot3 &deltaRij, const Matrix3 │ │ │ │ │ +&delRdelBiasOmega, const Matrix3 &preint_meas_cov) │ │ │ │ │ +Non-Default constructor, initialize with measurements. │ │ │ │ │ +DDeeffiinniittiioonn AHRSFactor.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ +PreintegratedAhrsMeasurements() │ │ │ │ │ +Default constructor, only for serialization and wrappers. │ │ │ │ │ +DDeeffiinniittiioonn AHRSFactor.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_A_H_R_S_F_a_c_t_o_r │ │ │ │ │ +DDeeffiinniittiioonn AHRSFactor.h:131 │ │ │ │ │ +_g_t_s_a_m_:_:_A_H_R_S_F_a_c_t_o_r_:_:_p_r_e_i_n_t_e_g_r_a_t_e_d_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ +const PreintegratedAhrsMeasurements & preintegratedMeasurements() const │ │ │ │ │ +Access the preintegrated measurements. │ │ │ │ │ +DDeeffiinniittiioonn AHRSFactor.h:174 │ │ │ │ │ +_g_t_s_a_m_:_:_A_H_R_S_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< AHRSFactor > shared_ptr │ │ │ │ │ +Shorthand for a smart pointer to a factor. │ │ │ │ │ +DDeeffiinniittiioonn AHRSFactor.h:147 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ +Parameters for pre-integration: Usage: Create just a single Params and pass a │ │ │ │ │ +shared pointer to the c... │ │ │ │ │ +DDeeffiinniittiioonn PreintegratedRotation.h:31 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n │ │ │ │ │ +PreintegratedRotation is the base class for all PreintegratedMeasurements │ │ │ │ │ +classes (in AHRSFactor,... │ │ │ │ │ +DDeeffiinniittiioonn PreintegratedRotation.h:89 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ +Nonlinear factor base class. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ +A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ +variables. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _h_y_b_r_i_d │ │ │ │ │ - * _H_y_b_r_i_d_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ + * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ + * _A_H_R_S_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00572.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00062.html │ │ │ │┄ Files 75% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/graph.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/DSFMap.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ -Macros | │ │ │ │ +Typedefs | │ │ │ │ Functions
│ │ │ │ -
graph.h File Reference
│ │ │ │ +
DSFMap.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Graph algorithm using boost library. │ │ │ │ +

Allow for arbitrary type in DSF. │ │ │ │ More...

│ │ │ │ │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::SDGraph< KEY >
 SDGraph is undirected graph with variable keys and double edge weights. More...
class  gtsam::DSFMap< KEY >
 Disjoint set forest using an STL map data structure underneath Uses rank compression and union by rank, iterator version. More...
 
class  gtsam::SGraph< KEY >
struct  gtsam::DSFMap< KEY >::Entry
 We store the forest in an STL map, but parents are done with pointers. More...
 
class  gtsam::PredecessorMap< KEY >
 Map from variable key to parent key. More...
class  gtsam::IndexPair
 Small utility class for representing a wrappable pairs of ints. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Typedefs

│ │ │ │ +typedef std::vector< IndexPair > gtsam::IndexPairVector
 
│ │ │ │ +typedef std::set< IndexPair > gtsam::IndexPairSet
 
│ │ │ │ +typedef std::map< IndexPair, IndexPairSet > gtsam::IndexPairSetMap
 
│ │ │ │ +typedef DSFMap< IndexPair > gtsam::DSFMapIndexPair
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ Functions

│ │ │ │ -template<class KEY >
std::list< KEY > gtsam::predecessorMap2Keys (const PredecessorMap< KEY > &p_map)
 Generate a list of keys from a spanning tree represented by its predecessor map.
 
│ │ │ │ -template<class G , class F , class KEY >
SDGraph< KEY > gtsam::toBoostGraph (const G &graph)
 Convert the factor graph to an SDGraph G = Graph type F = Factor type Key = Key type.
 
template<class G , class V , class KEY >
boost::tuple< G, V, std::map< KEY, V > > gtsam::predecessorMap2Graph (const PredecessorMap< KEY > &p_map)
 Build takes a predecessor map, and builds a directed graph corresponding to the tree.
 
│ │ │ │ -template<class G , class Factor , class POSE , class KEY >
boost::shared_ptr< Values > gtsam::composePoses (const G &graph, const PredecessorMap< KEY > &tree, const POSE &rootPose)
 Compose the poses by following the chain specified by the spanning tree.
 
│ │ │ │ -template<class G , class KEY , class FACTOR2 >
PredecessorMap< KEY > gtsam::findMinimumSpanningTree (const G &g)
 find the minimum spanning tree using boost graph library
 
│ │ │ │ -template<class G , class KEY , class FACTOR2 >
void gtsam::split (const G &g, const PredecessorMap< KEY > &tree, G &Ab1, G &Ab2)
 Split the graph into two parts: one corresponds to the given spanning tree, and the other corresponds to the rest of the factors.
 
│ │ │ │ +IndexPairVector gtsam::IndexPairSetAsArray (IndexPairSet &set)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Graph algorithm using boost library.

│ │ │ │ -
Author
: Kai Ni
│ │ │ │ -
Date
Jan 11, 2010
│ │ │ │ +

Allow for arbitrary type in DSF.

│ │ │ │ +
Date
Oct 26, 2013
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,75 +1,48 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -graph.h File Reference │ │ │ │ │ -Graph algorithm using boost library. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +DSFMap.h File Reference │ │ │ │ │ +Allow for arbitrary type in DSF. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_S_D_G_r_a_p_h_<_ _K_E_Y_ _> │ │ │ │ │ -  _S_D_G_r_a_p_h is undirected graph with variable keys and double edge weights. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_S_G_r_a_p_h_<_ _K_E_Y_ _> │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_P_r_e_d_e_c_e_s_s_o_r_M_a_p_<_ _K_E_Y_ _> │ │ │ │ │ -  Map from variable key to parent key. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_D_S_F_M_a_p_<_ _K_E_Y_ _> │ │ │ │ │ +  Disjoint set forest using an STL map data structure underneath Uses │ │ │ │ │ + rank compression and union by rank, iterator version. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_D_S_F_M_a_p_<_ _K_E_Y_ _>_:_:_E_n_t_r_y │ │ │ │ │ +  We store the forest in an STL map, but parents are done with pointers. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_I_n_d_e_x_P_a_i_r │ │ │ │ │ +  Small utility class for representing a wrappable pairs of ints. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +TTyyppeeddeeffss │ │ │ │ │ + typedef std::vector< _I_n_d_e_x_P_a_i_r >  ggttssaamm::::IInnddeexxPPaaiirrVVeeccttoorr │ │ │ │ │ +  │ │ │ │ │ + typedef std::set< _I_n_d_e_x_P_a_i_r >  ggttssaamm::::IInnddeexxPPaaiirrSSeett │ │ │ │ │ +  │ │ │ │ │ +typedef std::map< _I_n_d_e_x_P_a_i_r, IndexPairSet >  ggttssaamm::::IInnddeexxPPaaiirrSSeettMMaapp │ │ │ │ │ +  │ │ │ │ │ + typedef _D_S_F_M_a_p< _I_n_d_e_x_P_a_i_r >  ggttssaamm::::DDSSFFMMaappIInnddeexxPPaaiirr │ │ │ │ │ +  │ │ │ │ │ FFuunnccttiioonnss │ │ │ │ │ -template │ │ │ │ │ - std::list< KEY >  ggttssaamm::::pprreeddeecceessssoorrMMaapp22KKeeyyss (const │ │ │ │ │ - _P_r_e_d_e_c_e_s_s_o_r_M_a_p< KEY > &p_map) │ │ │ │ │ -  Generate a list of keys from a spanning │ │ │ │ │ - tree represented by its predecessor map. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - _S_D_G_r_a_p_h< KEY >  ggttssaamm::::ttooBBoooossttGGrraapphh (const G &graph) │ │ │ │ │ - Convert the factor graph to an _S_D_G_r_a_p_h G │ │ │ │ │ -  = Graph type F = _F_a_c_t_o_r type Key = Key │ │ │ │ │ - type. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ -boost::tuple< G, V, std::map< KEY, V > _g_t_s_a_m_:_:_p_r_e_d_e_c_e_s_s_o_r_M_a_p_2_G_r_a_p_h (const │ │ │ │ │ - >  _P_r_e_d_e_c_e_s_s_o_r_M_a_p< KEY > &p_map) │ │ │ │ │ - Build takes a predecessor map, and │ │ │ │ │ -  builds a directed graph corresponding to │ │ │ │ │ - the tree. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - boost::shared_ptr< _V_a_l_u_e_s >  ggttssaamm::::ccoommppoosseePPoosseess (const G &graph, │ │ │ │ │ - const _P_r_e_d_e_c_e_s_s_o_r_M_a_p< KEY > &tree, const │ │ │ │ │ - POSE &rootPose) │ │ │ │ │ -  Compose the poses by following the chain │ │ │ │ │ - specified by the spanning tree. │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - _P_r_e_d_e_c_e_s_s_o_r_M_a_p< KEY >  ggttssaamm::::ffiinnddMMiinniimmuummSSppaannnniinnggTTrreeee (const G │ │ │ │ │ - &g) │ │ │ │ │ -  find the minimum spanning tree using │ │ │ │ │ - boost graph library │ │ │ │ │ -  │ │ │ │ │ -template │ │ │ │ │ - void  ggttssaamm::::sspplliitt (const G &g, const │ │ │ │ │ - _P_r_e_d_e_c_e_s_s_o_r_M_a_p< KEY > &tree, G &Ab1, G │ │ │ │ │ - &Ab2) │ │ │ │ │ - Split the graph into two parts: one │ │ │ │ │ -  corresponds to the given spanning tree, │ │ │ │ │ - and the other corresponds to the rest of │ │ │ │ │ - the factors. │ │ │ │ │ +IndexPairVector  ggttssaamm::::IInnddeexxPPaaiirrSSeettAAssAArrrraayy (IndexPairSet &set) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Graph algorithm using boost library. │ │ │ │ │ - Author │ │ │ │ │ - : Kai Ni │ │ │ │ │ +Allow for arbitrary type in DSF. │ │ │ │ │ Date │ │ │ │ │ - Jan 11, 2010 │ │ │ │ │ + Oct 26, 2013 │ │ │ │ │ + Author │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _g_r_a_p_h_._h │ │ │ │ │ + * _b_a_s_e │ │ │ │ │ + * _D_S_F_M_a_p_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00593_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01091_source.html │ │ │ │┄ Files 94% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/FactorGraph-inst.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
FactorGraph-inst.h
│ │ │ │ +
NonlinearConjugateGradientOptimizer.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
22#pragma once
│ │ │ │ -
23
│ │ │ │ -
24#include <gtsam/inference/FactorGraph.h>
│ │ │ │ -
25
│ │ │ │ -
26#include <stdio.h>
│ │ │ │ -
27#include <algorithm>
│ │ │ │ -
28#include <iostream> // for cout :-(
│ │ │ │ -
29#include <fstream>
│ │ │ │ -
30#include <sstream>
│ │ │ │ -
31#include <string>
│ │ │ │ -
32
│ │ │ │ -
33namespace gtsam {
│ │ │ │ -
34
│ │ │ │ -
35/* ************************************************************************* */
│ │ │ │ -
36template <class FACTOR>
│ │ │ │ -
│ │ │ │ -
37void FactorGraph<FACTOR>::print(const std::string& s,
│ │ │ │ -
38 const KeyFormatter& formatter) const {
│ │ │ │ -
39 std::cout << (s.empty() ? "" : s + " ") << std::endl;
│ │ │ │ -
40 std::cout << "size: " << size() << std::endl;
│ │ │ │ -
41 for (size_t i = 0; i < factors_.size(); i++) {
│ │ │ │ -
42 std::stringstream ss;
│ │ │ │ -
43 ss << "factor " << i << ": ";
│ │ │ │ -
44 if (factors_[i]) factors_[i]->print(ss.str(), formatter);
│ │ │ │ -
45 }
│ │ │ │ -
46}
│ │ │ │ -
│ │ │ │ -
47
│ │ │ │ -
48/* ************************************************************************* */
│ │ │ │ -
49template <class FACTOR>
│ │ │ │ -
│ │ │ │ -
50bool FactorGraph<FACTOR>::equals(const This& fg, double tol) const {
│ │ │ │ -
51 // check whether the two factor graphs have the same number of factors.
│ │ │ │ -
52 if (factors_.size() != fg.size()) return false;
│ │ │ │ -
53
│ │ │ │ -
54 // check whether the factors are the same, in same order.
│ │ │ │ -
55 for (size_t i = 0; i < factors_.size(); i++) {
│ │ │ │ -
56 sharedFactor f1 = factors_[i], f2 = fg.factors_[i];
│ │ │ │ -
57 if (f1 == nullptr && f2 == nullptr) continue;
│ │ │ │ -
58 if (f1 == nullptr || f2 == nullptr) return false;
│ │ │ │ -
59 if (!f1->equals(*f2, tol)) return false;
│ │ │ │ -
60 }
│ │ │ │ -
61 return true;
│ │ │ │ -
62}
│ │ │ │ -
│ │ │ │ -
63
│ │ │ │ -
64/* ************************************************************************ */
│ │ │ │ -
65template <class FACTOR>
│ │ │ │ -
│ │ │ │ -
66double FactorGraph<FACTOR>::error(const HybridValues &values) const {
│ │ │ │ -
67 double error = 0.0;
│ │ │ │ -
68 for (auto &f : factors_) {
│ │ │ │ -
69 error += f->error(values);
│ │ │ │ -
70 }
│ │ │ │ -
71 return error;
│ │ │ │ -
72}
│ │ │ │ -
│ │ │ │ -
73
│ │ │ │ -
74/* ************************************************************************* */
│ │ │ │ -
75template <class FACTOR>
│ │ │ │ -
│ │ │ │ -
76size_t FactorGraph<FACTOR>::nrFactors() const {
│ │ │ │ -
77 size_t size_ = 0;
│ │ │ │ -
78 for (const sharedFactor& factor : factors_)
│ │ │ │ -
79 if (factor) size_++;
│ │ │ │ -
80 return size_;
│ │ │ │ -
81}
│ │ │ │ -
│ │ │ │ -
82
│ │ │ │ -
83/* ************************************************************************* */
│ │ │ │ -
84template <class FACTOR>
│ │ │ │ -
│ │ │ │ -
85KeySet FactorGraph<FACTOR>::keys() const {
│ │ │ │ -
86 KeySet keys;
│ │ │ │ -
87 for (const sharedFactor& factor : this->factors_) {
│ │ │ │ -
88 if (factor) keys.insert(factor->begin(), factor->end());
│ │ │ │ -
89 }
│ │ │ │ -
90 return keys;
│ │ │ │ -
91}
│ │ │ │ -
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/base/Manifold.h>
│ │ │ │ +
22#include <gtsam/nonlinear/NonlinearOptimizer.h>
│ │ │ │ +
23#include <boost/tuple/tuple.hpp>
│ │ │ │ +
24
│ │ │ │ +
25namespace gtsam {
│ │ │ │ +
26
│ │ │ │ +
│ │ │ │ +
28class GTSAM_EXPORT NonlinearConjugateGradientOptimizer : public NonlinearOptimizer {
│ │ │ │ +
29
│ │ │ │ +
30 /* a class for the nonlinearConjugateGradient template */
│ │ │ │ +
31 class System {
│ │ │ │ +
32 public:
│ │ │ │ +
33 typedef Values State;
│ │ │ │ +
34 typedef VectorValues Gradient;
│ │ │ │ +
35 typedef NonlinearOptimizerParams Parameters;
│ │ │ │ +
36
│ │ │ │ +
37 protected:
│ │ │ │ +
38 const NonlinearFactorGraph &graph_;
│ │ │ │ +
39
│ │ │ │ +
40 public:
│ │ │ │ +
41 System(const NonlinearFactorGraph &graph) :
│ │ │ │ +
42 graph_(graph) {
│ │ │ │ +
43 }
│ │ │ │ +
44 double error(const State &state) const;
│ │ │ │ +
45 Gradient gradient(const State &state) const;
│ │ │ │ +
46 State advance(const State &current, const double alpha,
│ │ │ │ +
47 const Gradient &g) const;
│ │ │ │ +
48 };
│ │ │ │ +
49
│ │ │ │ +
50public:
│ │ │ │ +
51
│ │ │ │ +
52 typedef NonlinearOptimizer Base;
│ │ │ │ +
53 typedef NonlinearOptimizerParams Parameters;
│ │ │ │ +
54 typedef boost::shared_ptr<NonlinearConjugateGradientOptimizer> shared_ptr;
│ │ │ │ +
55
│ │ │ │ +
56protected:
│ │ │ │ +
57 Parameters params_;
│ │ │ │ +
58
│ │ │ │ +
59 const NonlinearOptimizerParams& _params() const override {
│ │ │ │ +
60 return params_;
│ │ │ │ +
61 }
│ │ │ │ +
62
│ │ │ │ +
63public:
│ │ │ │ +
64
│ │ │ │ +
66 NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph& graph,
│ │ │ │ +
67 const Values& initialValues, const Parameters& params = Parameters());
│ │ │ │ +
68
│ │ │ │ +
│ │ │ │ +
70 ~NonlinearConjugateGradientOptimizer() override {
│ │ │ │ +
71 }
│ │ │ │ +
│ │ │ │ +
72
│ │ │ │ +
77 GaussianFactorGraph::shared_ptr iterate() override;
│ │ │ │ +
78
│ │ │ │ +
83 const Values& optimize() override;
│ │ │ │ +
84};
│ │ │ │ +
│ │ │ │ +
85
│ │ │ │ +
87template<class S, class V, class W>
│ │ │ │ +
│ │ │ │ +
88double lineSearch(const S &system, const V currentValues, const W &gradient) {
│ │ │ │ +
89
│ │ │ │ +
90 /* normalize it such that it becomes a unit vector */
│ │ │ │ +
91 const double g = gradient.norm();
│ │ │ │
92
│ │ │ │ -
93/* ************************************************************************* */
│ │ │ │ -
94template <class FACTOR>
│ │ │ │ -
│ │ │ │ -
95KeyVector FactorGraph<FACTOR>::keyVector() const {
│ │ │ │ -
96 KeyVector keys;
│ │ │ │ -
97 keys.reserve(2 * size()); // guess at size
│ │ │ │ -
98 for (const sharedFactor& factor : factors_)
│ │ │ │ -
99 if (factor) keys.insert(keys.end(), factor->begin(), factor->end());
│ │ │ │ -
100 std::sort(keys.begin(), keys.end());
│ │ │ │ -
101 auto last = std::unique(keys.begin(), keys.end());
│ │ │ │ -
102 keys.erase(last, keys.end());
│ │ │ │ -
103 return keys;
│ │ │ │ -
104}
│ │ │ │ -
│ │ │ │ -
105
│ │ │ │ -
106/* ************************************************************************* */
│ │ │ │ -
107template <class FACTOR>
│ │ │ │ -
108template <typename CONTAINER, typename>
│ │ │ │ -
│ │ │ │ -
109FactorIndices FactorGraph<FACTOR>::add_factors(const CONTAINER& factors,
│ │ │ │ -
110 bool useEmptySlots) {
│ │ │ │ -
111 const size_t num_factors = factors.size();
│ │ │ │ -
112 FactorIndices newFactorIndices(num_factors);
│ │ │ │ -
113 if (useEmptySlots) {
│ │ │ │ -
114 size_t i = 0;
│ │ │ │ -
115 for (size_t j = 0; j < num_factors; ++j) {
│ │ │ │ -
116 // Loop to find the next available factor slot
│ │ │ │ -
117 do {
│ │ │ │ -
118 if (i >= size())
│ │ │ │ -
119 // Make room for remaining factors, happens only once.
│ │ │ │ -
120 resize(size() + num_factors - j);
│ │ │ │ -
121 else if (at(i))
│ │ │ │ -
122 ++i; // Move on to the next slot or past end.
│ │ │ │ -
123 else
│ │ │ │ -
124 break; // We found an empty slot, break to fill it.
│ │ │ │ -
125 } while (true);
│ │ │ │ -
126
│ │ │ │ -
127 // Use the current slot, updating graph and newFactorSlots.
│ │ │ │ -
128 at(i) = factors[j];
│ │ │ │ -
129 newFactorIndices[j] = i;
│ │ │ │ -
130 }
│ │ │ │ -
131 } else {
│ │ │ │ -
132 // We're not looking for unused slots, so just add the factors at the end.
│ │ │ │ -
133 for (size_t i = 0; i < num_factors; ++i) newFactorIndices[i] = i + size();
│ │ │ │ -
134 push_back(factors);
│ │ │ │ -
135 }
│ │ │ │ -
136 return newFactorIndices;
│ │ │ │ -
137}
│ │ │ │ -
│ │ │ │ -
138
│ │ │ │ -
139/* ************************************************************************* */
│ │ │ │ -
140template <class FACTOR>
│ │ │ │ -
│ │ │ │ -
141void FactorGraph<FACTOR>::dot(std::ostream& os,
│ │ │ │ -
142 const KeyFormatter& keyFormatter,
│ │ │ │ -
143 const DotWriter& writer) const {
│ │ │ │ -
144 writer.graphPreamble(&os);
│ │ │ │ -
145
│ │ │ │ -
146 // Create nodes for each variable in the graph
│ │ │ │ -
147 for (Key key : keys()) {
│ │ │ │ -
148 auto position = writer.variablePos(key);
│ │ │ │ -
149 writer.drawVariable(key, keyFormatter, position, &os);
│ │ │ │ -
150 }
│ │ │ │ -
151 os << "\n";
│ │ │ │ -
152
│ │ │ │ -
153 // Create factors and variable connections
│ │ │ │ -
154 for (size_t i = 0; i < size(); ++i) {
│ │ │ │ -
155 const auto& factor = at(i);
│ │ │ │ -
156 if (factor) {
│ │ │ │ -
157 const KeyVector& factorKeys = factor->keys();
│ │ │ │ -
158 writer.processFactor(i, factorKeys, keyFormatter, boost::none, &os);
│ │ │ │ -
159 }
│ │ │ │ -
160 }
│ │ │ │ -
161
│ │ │ │ -
162 os << "}\n";
│ │ │ │ -
163 std::flush(os);
│ │ │ │ -
164}
│ │ │ │ -
│ │ │ │ +
93 // perform the golden section search algorithm to decide the the optimal step size
│ │ │ │ +
94 // detail refer to http://en.wikipedia.org/wiki/Golden_section_search
│ │ │ │ +
95 const double phi = 0.5 * (1.0 + std::sqrt(5.0)), resphi = 2.0 - phi, tau =
│ │ │ │ +
96 1e-5;
│ │ │ │ +
97 double minStep = -1.0 / g, maxStep = 0, newStep = minStep
│ │ │ │ +
98 + (maxStep - minStep) / (phi + 1.0);
│ │ │ │ +
99
│ │ │ │ +
100 V newValues = system.advance(currentValues, newStep, gradient);
│ │ │ │ +
101 double newError = system.error(newValues);
│ │ │ │ +
102
│ │ │ │ +
103 while (true) {
│ │ │ │ +
104 const bool flag = (maxStep - newStep > newStep - minStep) ? true : false;
│ │ │ │ +
105 const double testStep =
│ │ │ │ +
106 flag ? newStep + resphi * (maxStep - newStep) :
│ │ │ │ +
107 newStep - resphi * (newStep - minStep);
│ │ │ │ +
108
│ │ │ │ +
109 if ((maxStep - minStep)
│ │ │ │ +
110 < tau * (std::abs(testStep) + std::abs(newStep))) {
│ │ │ │ +
111 return 0.5 * (minStep + maxStep);
│ │ │ │ +
112 }
│ │ │ │ +
113
│ │ │ │ +
114 const V testValues = system.advance(currentValues, testStep, gradient);
│ │ │ │ +
115 const double testError = system.error(testValues);
│ │ │ │ +
116
│ │ │ │ +
117 // update the working range
│ │ │ │ +
118 if (testError >= newError) {
│ │ │ │ +
119 if (flag)
│ │ │ │ +
120 maxStep = testStep;
│ │ │ │ +
121 else
│ │ │ │ +
122 minStep = testStep;
│ │ │ │ +
123 } else {
│ │ │ │ +
124 if (flag) {
│ │ │ │ +
125 minStep = newStep;
│ │ │ │ +
126 newStep = testStep;
│ │ │ │ +
127 newError = testError;
│ │ │ │ +
128 } else {
│ │ │ │ +
129 maxStep = newStep;
│ │ │ │ +
130 newStep = testStep;
│ │ │ │ +
131 newError = testError;
│ │ │ │ +
132 }
│ │ │ │ +
133 }
│ │ │ │ +
134 }
│ │ │ │ +
135 return 0.0;
│ │ │ │ +
136}
│ │ │ │ +
│ │ │ │ +
137
│ │ │ │ +
147template<class S, class V>
│ │ │ │ +
│ │ │ │ +
148boost::tuple<V, int> nonlinearConjugateGradient(const S &system,
│ │ │ │ +
149 const V &initial, const NonlinearOptimizerParams &params,
│ │ │ │ +
150 const bool singleIteration, const bool gradientDescent = false) {
│ │ │ │ +
151
│ │ │ │ +
152 // GTSAM_CONCEPT_MANIFOLD_TYPE(V)
│ │ │ │ +
153
│ │ │ │ +
154 size_t iteration = 0;
│ │ │ │ +
155
│ │ │ │ +
156 // check if we're already close enough
│ │ │ │ +
157 double currentError = system.error(initial);
│ │ │ │ +
158 if (currentError <= params.errorTol) {
│ │ │ │ +
159 if (params.verbosity >= NonlinearOptimizerParams::ERROR) {
│ │ │ │ +
160 std::cout << "Exiting, as error = " << currentError << " < "
│ │ │ │ +
161 << params.errorTol << std::endl;
│ │ │ │ +
162 }
│ │ │ │ +
163 return boost::tie(initial, iteration);
│ │ │ │ +
164 }
│ │ │ │
165
│ │ │ │ -
166/* ************************************************************************* */
│ │ │ │ -
167template <class FACTOR>
│ │ │ │ -
│ │ │ │ -
168std::string FactorGraph<FACTOR>::dot(const KeyFormatter& keyFormatter,
│ │ │ │ -
169 const DotWriter& writer) const {
│ │ │ │ -
170 std::stringstream ss;
│ │ │ │ -
171 dot(ss, keyFormatter, writer);
│ │ │ │ -
172 return ss.str();
│ │ │ │ -
173}
│ │ │ │ -
│ │ │ │ -
174
│ │ │ │ -
175/* ************************************************************************* */
│ │ │ │ -
176template <class FACTOR>
│ │ │ │ -
│ │ │ │ -
177void FactorGraph<FACTOR>::saveGraph(const std::string& filename,
│ │ │ │ -
178 const KeyFormatter& keyFormatter,
│ │ │ │ -
179 const DotWriter& writer) const {
│ │ │ │ -
180 std::ofstream of(filename.c_str());
│ │ │ │ -
181 dot(of, keyFormatter, writer);
│ │ │ │ -
182 of.close();
│ │ │ │ -
183}
│ │ │ │ -
│ │ │ │ -
184
│ │ │ │ -
185} // namespace gtsam
│ │ │ │ -
FactorGraph.h
Factor Graph Base Class.
│ │ │ │ +
166 V currentValues = initial;
│ │ │ │ +
167 typename S::Gradient currentGradient = system.gradient(currentValues),
│ │ │ │ +
168 prevGradient, direction = currentGradient;
│ │ │ │ +
169
│ │ │ │ +
170 /* do one step of gradient descent */
│ │ │ │ +
171 V prevValues = currentValues;
│ │ │ │ +
172 double prevError = currentError;
│ │ │ │ +
173 double alpha = lineSearch(system, currentValues, direction);
│ │ │ │ +
174 currentValues = system.advance(prevValues, alpha, direction);
│ │ │ │ +
175 currentError = system.error(currentValues);
│ │ │ │ +
176
│ │ │ │ +
177 // Maybe show output
│ │ │ │ +
178 if (params.verbosity >= NonlinearOptimizerParams::ERROR)
│ │ │ │ +
179 std::cout << "Initial error: " << currentError << std::endl;
│ │ │ │ +
180
│ │ │ │ +
181 // Iterative loop
│ │ │ │ +
182 do {
│ │ │ │ +
183 if (gradientDescent == true) {
│ │ │ │ +
184 direction = system.gradient(currentValues);
│ │ │ │ +
185 } else {
│ │ │ │ +
186 prevGradient = currentGradient;
│ │ │ │ +
187 currentGradient = system.gradient(currentValues);
│ │ │ │ +
188 // Polak-Ribiere: beta = g'*(g_n-g_n-1)/g_n-1'*g_n-1
│ │ │ │ +
189 const double beta = std::max(0.0,
│ │ │ │ +
190 currentGradient.dot(currentGradient - prevGradient)
│ │ │ │ +
191 / prevGradient.dot(prevGradient));
│ │ │ │ +
192 direction = currentGradient + (beta * direction);
│ │ │ │ +
193 }
│ │ │ │ +
194
│ │ │ │ +
195 alpha = lineSearch(system, currentValues, direction);
│ │ │ │ +
196
│ │ │ │ +
197 prevValues = currentValues;
│ │ │ │ +
198 prevError = currentError;
│ │ │ │ +
199
│ │ │ │ +
200 currentValues = system.advance(prevValues, alpha, direction);
│ │ │ │ +
201 currentError = system.error(currentValues);
│ │ │ │ +
202
│ │ │ │ +
203 // User hook:
│ │ │ │ +
204 if (params.iterationHook)
│ │ │ │ +
205 params.iterationHook(iteration, prevError, currentError);
│ │ │ │ +
206
│ │ │ │ +
207 // Maybe show output
│ │ │ │ +
208 if (params.verbosity >= NonlinearOptimizerParams::ERROR)
│ │ │ │ +
209 std::cout << "iteration: " << iteration << ", currentError: " << currentError << std::endl;
│ │ │ │ +
210 } while (++iteration < params.maxIterations && !singleIteration
│ │ │ │ +
211 && !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol,
│ │ │ │ +
212 params.errorTol, prevError, currentError, params.verbosity));
│ │ │ │ +
213
│ │ │ │ +
214 // Printing if verbose
│ │ │ │ +
215 if (params.verbosity >= NonlinearOptimizerParams::ERROR
│ │ │ │ +
216 && iteration >= params.maxIterations)
│ │ │ │ +
217 std::cout
│ │ │ │ +
218 << "nonlinearConjugateGradient: Terminating because reached maximum iterations"
│ │ │ │ +
219 << std::endl;
│ │ │ │ +
220
│ │ │ │ +
221 return boost::tie(currentValues, iteration);
│ │ │ │ +
222}
│ │ │ │ +
│ │ │ │ +
223
│ │ │ │ +
224} // \ namespace gtsam
│ │ │ │ +
225
│ │ │ │ +
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ +
NonlinearOptimizer.h
Base class and parameters for nonlinear optimization algorithms.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition Factor.h:34
│ │ │ │ -
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ - │ │ │ │ -
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ -
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ -
gtsam::FactorGraph::print
virtual void print(const std::string &s="FactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const
Print out graph to std::cout, with optional key formatter.
Definition FactorGraph-inst.h:37
│ │ │ │ -
gtsam::FactorGraph::keys
KeySet keys() const
Potentially slow function to return all keys involved, sorted, as a set.
Definition FactorGraph-inst.h:85
│ │ │ │ -
gtsam::FactorGraph::add_factors
FactorIndices add_factors(const CONTAINER &factors, bool useEmptySlots=false)
Add new factors to a factor graph and returns a list of new factor indices, optionally finding and re...
Definition FactorGraph-inst.h:109
│ │ │ │ -
gtsam::FactorGraph::dot
void dot(std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
Output to graphviz format, stream version.
Definition FactorGraph-inst.h:141
│ │ │ │ -
gtsam::FactorGraph::keyVector
KeyVector keyVector() const
Potentially slow function to return all keys involved, sorted, as a vector.
Definition FactorGraph-inst.h:95
│ │ │ │ -
gtsam::FactorGraph::error
double error(const HybridValues &values) const
Add error for all factors.
Definition FactorGraph-inst.h:66
│ │ │ │ -
gtsam::FactorGraph::nrFactors
size_t nrFactors() const
return the number of non-null factors
Definition FactorGraph-inst.h:76
│ │ │ │ -
gtsam::FactorGraph::size
size_t size() const
return the number of factors (including any null factors set by remove() ).
Definition FactorGraph.h:326
│ │ │ │ -
gtsam::FactorGraph::equals
bool equals(const This &fg, double tol=1e-9) const
Check equality up to tolerance.
Definition FactorGraph-inst.h:50
│ │ │ │ -
gtsam::FactorGraph::sharedFactor
boost::shared_ptr< FACTOR > sharedFactor
Shared pointer to a factor.
Definition FactorGraph.h:101
│ │ │ │ -
gtsam::FactorGraph::factors_
FastVector< sharedFactor > factors_
concept check, makes sure FACTOR defines print and equals
Definition FactorGraph.h:131
│ │ │ │ -
gtsam::FactorGraph::saveGraph
void saveGraph(const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
output to file with graphviz format.
Definition FactorGraph-inst.h:177
│ │ │ │ -
gtsam::DotWriter
DotWriter is a helper class for writing graphviz .dot files.
Definition DotWriter.h:35
│ │ │ │ -
gtsam::DotWriter::drawVariable
void drawVariable(Key key, const KeyFormatter &keyFormatter, const boost::optional< Vector2 > &position, std::ostream *os) const
Create a variable dot fragment.
Definition DotWriter.cpp:42
│ │ │ │ -
gtsam::DotWriter::processFactor
void processFactor(size_t i, const KeyVector &keys, const KeyFormatter &keyFormatter, const boost::optional< Vector2 > &position, std::ostream *os) const
Draw a single factor, specified by its index i and its variable keys.
Definition DotWriter.cpp:96
│ │ │ │ -
gtsam::DotWriter::variablePos
boost::optional< Vector2 > variablePos(Key key) const
Return variable position or none.
Definition DotWriter.cpp:79
│ │ │ │ -
gtsam::DotWriter::graphPreamble
void graphPreamble(std::ostream *os) const
Write out preamble for graph, including size.
Definition DotWriter.cpp:30
│ │ │ │ +
gtsam::lineSearch
double lineSearch(const S &system, const V currentValues, const W &gradient)
Implement the golden-section line search algorithm.
Definition NonlinearConjugateGradientOptimizer.h:88
│ │ │ │ +
gtsam::checkConvergence
bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold, double errorThreshold, double currentError, double newError, NonlinearOptimizerParams::Verbosity verbosity)
Check whether the relative error decrease is less than relativeErrorTreshold, the absolute error decr...
Definition NonlinearOptimizer.cpp:185
│ │ │ │ +
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
│ │ │ │ +
gtsam::nonlinearConjugateGradient
boost::tuple< V, int > nonlinearConjugateGradient(const S &system, const V &initial, const NonlinearOptimizerParams &params, const bool singleIteration, const bool gradientDescent=false)
Implement the nonlinear conjugate gradient method using the Polak-Ribiere formula suggested in http:/...
Definition NonlinearConjugateGradientOptimizer.h:148
│ │ │ │ +
gtsam::GaussianFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactorGraph.h:81
│ │ │ │ +
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
gtsam::NonlinearConjugateGradientOptimizer
An implementation of the nonlinear CG method using the template below.
Definition NonlinearConjugateGradientOptimizer.h:28
│ │ │ │ +
gtsam::NonlinearConjugateGradientOptimizer::~NonlinearConjugateGradientOptimizer
~NonlinearConjugateGradientOptimizer() override
Destructor.
Definition NonlinearConjugateGradientOptimizer.h:70
│ │ │ │ +
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │ +
gtsam::NonlinearOptimizer
This is the abstract interface for classes that can optimize for the maximum-likelihood estimate of a...
Definition NonlinearOptimizer.h:75
│ │ │ │ +
gtsam::NonlinearOptimizerParams
The common parameters for Nonlinear optimizers.
Definition NonlinearOptimizerParams.h:34
│ │ │ │ +
gtsam::NonlinearOptimizerParams::absoluteErrorTol
double absoluteErrorTol
The maximum absolute error decrease to stop iterating (default 1e-5)
Definition NonlinearOptimizerParams.h:43
│ │ │ │ +
gtsam::NonlinearOptimizerParams::iterationHook
IterationHook iterationHook
Optional user-provided iteration hook to be called after each optimization iteration (Default: none).
Definition NonlinearOptimizerParams.h:94
│ │ │ │ +
gtsam::NonlinearOptimizerParams::maxIterations
size_t maxIterations
The maximum iterations to stop iterating (default 100)
Definition NonlinearOptimizerParams.h:41
│ │ │ │ +
gtsam::NonlinearOptimizerParams::verbosity
Verbosity verbosity
The printing verbosity during optimization (default SILENT)
Definition NonlinearOptimizerParams.h:45
│ │ │ │ +
gtsam::NonlinearOptimizerParams::relativeErrorTol
double relativeErrorTol
The maximum relative error decrease to stop iterating (default 1e-5)
Definition NonlinearOptimizerParams.h:42
│ │ │ │ +
gtsam::NonlinearOptimizerParams::errorTol
double errorTol
The maximum total error to stop iterating (default 0.0)
Definition NonlinearOptimizerParams.h:44
│ │ │ │ +
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,305 +1,298 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -FactorGraph-inst.h │ │ │ │ │ +NonlinearConjugateGradientOptimizer.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -22#pragma once │ │ │ │ │ -23 │ │ │ │ │ -24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -25 │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ -28#include // for cout :-( │ │ │ │ │ -29#include │ │ │ │ │ -30#include │ │ │ │ │ -31#include │ │ │ │ │ -32 │ │ │ │ │ -33namespace _g_t_s_a_m { │ │ │ │ │ -34 │ │ │ │ │ -35/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -36template │ │ │ │ │ -_3_7void _F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_>_:_:_p_r_i_n_t(const std::string& s, │ │ │ │ │ -38 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter) const { │ │ │ │ │ -39 std::cout << (s.empty() ? "" : s + " ") << std::endl; │ │ │ │ │ -40 std::cout << "size: " << size() << std::endl; │ │ │ │ │ -41 for (size_t i = 0; i < factors_.size(); i++) { │ │ │ │ │ -42 std::stringstream ss; │ │ │ │ │ -43 ss << "factor " << i << ": "; │ │ │ │ │ -44 if (factors_[i]) factors_[i]->print(ss.str(), formatter); │ │ │ │ │ -45 } │ │ │ │ │ -46} │ │ │ │ │ -47 │ │ │ │ │ -48/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -49template │ │ │ │ │ -_5_0bool _F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_>_:_:_e_q_u_a_l_s(const _T_h_i_s& fg, double tol) const { │ │ │ │ │ -51 // check whether the two factor graphs have the same number of factors. │ │ │ │ │ -52 if (factors_.size() != fg._s_i_z_e()) return false; │ │ │ │ │ -53 │ │ │ │ │ -54 // check whether the factors are the same, in same order. │ │ │ │ │ -55 for (size_t i = 0; i < factors_.size(); i++) { │ │ │ │ │ -56 _s_h_a_r_e_d_F_a_c_t_o_r f1 = factors_[i], f2 = fg._f_a_c_t_o_r_s__[i]; │ │ │ │ │ -57 if (f1 == nullptr && f2 == nullptr) continue; │ │ │ │ │ -58 if (f1 == nullptr || f2 == nullptr) return false; │ │ │ │ │ -59 if (!f1->equals(*f2, tol)) return false; │ │ │ │ │ -60 } │ │ │ │ │ -61 return true; │ │ │ │ │ -62} │ │ │ │ │ -63 │ │ │ │ │ -64/* ************************************************************************ │ │ │ │ │ -*/ │ │ │ │ │ -65template │ │ │ │ │ -_6_6double _F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_>_:_:_e_r_r_o_r(const _H_y_b_r_i_d_V_a_l_u_e_s &values) const { │ │ │ │ │ -67 double error = 0.0; │ │ │ │ │ -68 for (auto &f : factors_) { │ │ │ │ │ -69 error += f->error(values); │ │ │ │ │ -70 } │ │ │ │ │ -71 return error; │ │ │ │ │ -72} │ │ │ │ │ -73 │ │ │ │ │ -74/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -75template │ │ │ │ │ -_7_6size_t _F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_>_:_:_n_r_F_a_c_t_o_r_s() const { │ │ │ │ │ -77 size_t size_ = 0; │ │ │ │ │ -78 for (const _s_h_a_r_e_d_F_a_c_t_o_r& factor : factors_) │ │ │ │ │ -79 if (factor) size_++; │ │ │ │ │ -80 return size_; │ │ │ │ │ -81} │ │ │ │ │ -82 │ │ │ │ │ -83/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -84template │ │ │ │ │ -_8_5_K_e_y_S_e_t _F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_>_:_:_k_e_y_s() const { │ │ │ │ │ -86 _K_e_y_S_e_t keys; │ │ │ │ │ -87 for (const _s_h_a_r_e_d_F_a_c_t_o_r& factor : this->factors_) { │ │ │ │ │ -88 if (factor) keys.insert(factor->begin(), factor->end()); │ │ │ │ │ -89 } │ │ │ │ │ -90 return keys; │ │ │ │ │ -91} │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_._h> │ │ │ │ │ +23#include │ │ │ │ │ +24 │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ +26 │ │ │ │ │ +_2_8class GTSAM_EXPORT _N_o_n_l_i_n_e_a_r_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_O_p_t_i_m_i_z_e_r : public │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r { │ │ │ │ │ +29 │ │ │ │ │ +30 /* a class for the nonlinearConjugateGradient template */ │ │ │ │ │ +31 class System { │ │ │ │ │ +32 public: │ │ │ │ │ +33 typedef _V_a_l_u_e_s State; │ │ │ │ │ +34 typedef _V_e_c_t_o_r_V_a_l_u_e_s Gradient; │ │ │ │ │ +35 typedef _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s _P_a_r_a_m_e_t_e_r_s; │ │ │ │ │ +36 │ │ │ │ │ +37 protected: │ │ │ │ │ +38 const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h &graph_; │ │ │ │ │ +39 │ │ │ │ │ +40 public: │ │ │ │ │ +41 System(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h &graph) : │ │ │ │ │ +42 graph_(graph) { │ │ │ │ │ +43 } │ │ │ │ │ +44 double error(const State &state) const; │ │ │ │ │ +45 Gradient gradient(const State &state) const; │ │ │ │ │ +46 State advance(const State ¤t, const double alpha, │ │ │ │ │ +47 const Gradient &g) const; │ │ │ │ │ +48 }; │ │ │ │ │ +49 │ │ │ │ │ +50public: │ │ │ │ │ +51 │ │ │ │ │ +52 typedef _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r _B_a_s_e; │ │ │ │ │ +53 typedef _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s _P_a_r_a_m_e_t_e_r_s; │ │ │ │ │ +54 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +55 │ │ │ │ │ +56protected: │ │ │ │ │ +57 _P_a_r_a_m_e_t_e_r_s params_; │ │ │ │ │ +58 │ │ │ │ │ +59 const _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s& _params() const override { │ │ │ │ │ +60 return params_; │ │ │ │ │ +61 } │ │ │ │ │ +62 │ │ │ │ │ +63public: │ │ │ │ │ +64 │ │ │ │ │ +66 _N_o_n_l_i_n_e_a_r_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_O_p_t_i_m_i_z_e_r(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, │ │ │ │ │ +67 const _V_a_l_u_e_s& initialValues, const _P_a_r_a_m_e_t_e_r_s& params = _P_a_r_a_m_e_t_e_r_s()); │ │ │ │ │ +68 │ │ │ │ │ +_7_0 _~_N_o_n_l_i_n_e_a_r_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_O_p_t_i_m_i_z_e_r() override { │ │ │ │ │ +71 } │ │ │ │ │ +72 │ │ │ │ │ +77 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r iterate() override; │ │ │ │ │ +78 │ │ │ │ │ +83 const _V_a_l_u_e_s& _o_p_t_i_m_i_z_e() override; │ │ │ │ │ +84}; │ │ │ │ │ +85 │ │ │ │ │ +87template │ │ │ │ │ +_8_8double _l_i_n_e_S_e_a_r_c_h(const S &system, const V currentValues, const W &gradient) │ │ │ │ │ +{ │ │ │ │ │ +89 │ │ │ │ │ +90 /* normalize it such that it becomes a unit vector */ │ │ │ │ │ +91 const double g = gradient.norm(); │ │ │ │ │ 92 │ │ │ │ │ -93/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -94template │ │ │ │ │ -_9_5_K_e_y_V_e_c_t_o_r _F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_>_:_:_k_e_y_V_e_c_t_o_r() const { │ │ │ │ │ -96 _K_e_y_V_e_c_t_o_r keys; │ │ │ │ │ -97 keys.reserve(2 * size()); // guess at size │ │ │ │ │ -98 for (const _s_h_a_r_e_d_F_a_c_t_o_r& factor : factors_) │ │ │ │ │ -99 if (factor) keys.insert(keys.end(), factor->begin(), factor->end()); │ │ │ │ │ -100 std::sort(keys.begin(), keys.end()); │ │ │ │ │ -101 auto last = std::unique(keys.begin(), keys.end()); │ │ │ │ │ -102 keys.erase(last, keys.end()); │ │ │ │ │ -103 return keys; │ │ │ │ │ -104} │ │ │ │ │ -105 │ │ │ │ │ -106/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -107template │ │ │ │ │ -108template │ │ │ │ │ -_1_0_9_F_a_c_t_o_r_I_n_d_i_c_e_s _F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_>_:_:_a_d_d___f_a_c_t_o_r_s(const CONTAINER& factors, │ │ │ │ │ -110 bool useEmptySlots) { │ │ │ │ │ -111 const size_t num_factors = factors.size(); │ │ │ │ │ -112 _F_a_c_t_o_r_I_n_d_i_c_e_s newFactorIndices(num_factors); │ │ │ │ │ -113 if (useEmptySlots) { │ │ │ │ │ -114 size_t i = 0; │ │ │ │ │ -115 for (size_t j = 0; j < num_factors; ++j) { │ │ │ │ │ -116 // Loop to find the next available factor slot │ │ │ │ │ -117 do { │ │ │ │ │ -118 if (i >= size()) │ │ │ │ │ -119 // Make room for remaining factors, happens only once. │ │ │ │ │ -120 resize(size() + num_factors - j); │ │ │ │ │ -121 else if (at(i)) │ │ │ │ │ -122 ++i; // Move on to the next slot or past end. │ │ │ │ │ -123 else │ │ │ │ │ -124 break; // We found an empty slot, break to fill it. │ │ │ │ │ -125 } while (true); │ │ │ │ │ -126 │ │ │ │ │ -127 // Use the current slot, updating graph and newFactorSlots. │ │ │ │ │ -128 at(i) = factors[j]; │ │ │ │ │ -129 newFactorIndices[j] = i; │ │ │ │ │ -130 } │ │ │ │ │ -131 } else { │ │ │ │ │ -132 // We're not looking for unused slots, so just add the factors at the end. │ │ │ │ │ -133 for (size_t i = 0; i < num_factors; ++i) newFactorIndices[i] = i + size(); │ │ │ │ │ -134 push_back(factors); │ │ │ │ │ -135 } │ │ │ │ │ -136 return newFactorIndices; │ │ │ │ │ -137} │ │ │ │ │ -138 │ │ │ │ │ -139/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -140template │ │ │ │ │ -_1_4_1void _F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_>_:_:_d_o_t(std::ostream& os, │ │ │ │ │ -142 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter, │ │ │ │ │ -143 const _D_o_t_W_r_i_t_e_r& writer) const { │ │ │ │ │ -144 writer._g_r_a_p_h_P_r_e_a_m_b_l_e(&os); │ │ │ │ │ -145 │ │ │ │ │ -146 // Create nodes for each variable in the graph │ │ │ │ │ -147 for (_K_e_y key : keys()) { │ │ │ │ │ -148 auto position = writer._v_a_r_i_a_b_l_e_P_o_s(key); │ │ │ │ │ -149 writer._d_r_a_w_V_a_r_i_a_b_l_e(key, keyFormatter, position, &os); │ │ │ │ │ -150 } │ │ │ │ │ -151 os << "\n"; │ │ │ │ │ -152 │ │ │ │ │ -153 // Create factors and variable connections │ │ │ │ │ -154 for (size_t i = 0; i < size(); ++i) { │ │ │ │ │ -155 const auto& factor = at(i); │ │ │ │ │ -156 if (factor) { │ │ │ │ │ -157 const _K_e_y_V_e_c_t_o_r& factorKeys = factor->keys(); │ │ │ │ │ -158 writer._p_r_o_c_e_s_s_F_a_c_t_o_r(i, factorKeys, keyFormatter, boost::none, &os); │ │ │ │ │ -159 } │ │ │ │ │ -160 } │ │ │ │ │ -161 │ │ │ │ │ -162 os << "}\n"; │ │ │ │ │ -163 std::flush(os); │ │ │ │ │ -164} │ │ │ │ │ +93 // perform the golden section search algorithm to decide the the optimal │ │ │ │ │ +step size │ │ │ │ │ +94 // detail refer to http://en.wikipedia.org/wiki/Golden_section_search │ │ │ │ │ +95 const double phi = 0.5 * (1.0 + std::sqrt(5.0)), resphi = 2.0 - phi, tau = │ │ │ │ │ +96 1e-5; │ │ │ │ │ +97 double minStep = -1.0 / g, maxStep = 0, newStep = minStep │ │ │ │ │ +98 + (maxStep - minStep) / (phi + 1.0); │ │ │ │ │ +99 │ │ │ │ │ +100 V newValues = system.advance(currentValues, newStep, gradient); │ │ │ │ │ +101 double newError = system.error(newValues); │ │ │ │ │ +102 │ │ │ │ │ +103 while (true) { │ │ │ │ │ +104 const bool flag = (maxStep - newStep > newStep - minStep) ? true : false; │ │ │ │ │ +105 const double testStep = │ │ │ │ │ +106 flag ? newStep + resphi * (maxStep - newStep) : │ │ │ │ │ +107 newStep - resphi * (newStep - minStep); │ │ │ │ │ +108 │ │ │ │ │ +109 if ((maxStep - minStep) │ │ │ │ │ +110 < tau * (std::abs(testStep) + std::abs(newStep))) { │ │ │ │ │ +111 return 0.5 * (minStep + maxStep); │ │ │ │ │ +112 } │ │ │ │ │ +113 │ │ │ │ │ +114 const V testValues = system.advance(currentValues, testStep, gradient); │ │ │ │ │ +115 const double testError = system.error(testValues); │ │ │ │ │ +116 │ │ │ │ │ +117 // update the working range │ │ │ │ │ +118 if (testError >= newError) { │ │ │ │ │ +119 if (flag) │ │ │ │ │ +120 maxStep = testStep; │ │ │ │ │ +121 else │ │ │ │ │ +122 minStep = testStep; │ │ │ │ │ +123 } else { │ │ │ │ │ +124 if (flag) { │ │ │ │ │ +125 minStep = newStep; │ │ │ │ │ +126 newStep = testStep; │ │ │ │ │ +127 newError = testError; │ │ │ │ │ +128 } else { │ │ │ │ │ +129 maxStep = newStep; │ │ │ │ │ +130 newStep = testStep; │ │ │ │ │ +131 newError = testError; │ │ │ │ │ +132 } │ │ │ │ │ +133 } │ │ │ │ │ +134 } │ │ │ │ │ +135 return 0.0; │ │ │ │ │ +136} │ │ │ │ │ +137 │ │ │ │ │ +147template │ │ │ │ │ +_1_4_8boost::tuple _n_o_n_l_i_n_e_a_r_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t(const S &system, │ │ │ │ │ +149 const V &initial, const _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s ¶ms, │ │ │ │ │ +150 const bool singleIteration, const bool gradientDescent = false) { │ │ │ │ │ +151 │ │ │ │ │ +152 // GTSAM_CONCEPT_MANIFOLD_TYPE(V) │ │ │ │ │ +153 │ │ │ │ │ +154 size_t iteration = 0; │ │ │ │ │ +155 │ │ │ │ │ +156 // check if we're already close enough │ │ │ │ │ +157 double currentError = system.error(initial); │ │ │ │ │ +158 if (currentError <= params._e_r_r_o_r_T_o_l) { │ │ │ │ │ +159 if (params._v_e_r_b_o_s_i_t_y >= NonlinearOptimizerParams::ERROR) { │ │ │ │ │ +160 std::cout << "Exiting, as error = " << currentError << " < " │ │ │ │ │ +161 << params._e_r_r_o_r_T_o_l << std::endl; │ │ │ │ │ +162 } │ │ │ │ │ +163 return boost::tie(initial, iteration); │ │ │ │ │ +164 } │ │ │ │ │ 165 │ │ │ │ │ -166/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -167template │ │ │ │ │ -_1_6_8std::string _F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_>_:_:_d_o_t(const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter, │ │ │ │ │ -169 const _D_o_t_W_r_i_t_e_r& writer) const { │ │ │ │ │ -170 std::stringstream ss; │ │ │ │ │ -171 _d_o_t(ss, keyFormatter, writer); │ │ │ │ │ -172 return ss.str(); │ │ │ │ │ -173} │ │ │ │ │ -174 │ │ │ │ │ -175/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -176template │ │ │ │ │ -_1_7_7void _F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_>_:_:_s_a_v_e_G_r_a_p_h(const std::string& filename, │ │ │ │ │ -178 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter, │ │ │ │ │ -179 const _D_o_t_W_r_i_t_e_r& writer) const { │ │ │ │ │ -180 std::ofstream of(filename.c_str()); │ │ │ │ │ -181 _d_o_t(of, keyFormatter, writer); │ │ │ │ │ -182 of.close(); │ │ │ │ │ -183} │ │ │ │ │ -184 │ │ │ │ │ -185} // namespace gtsam │ │ │ │ │ -_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Factor Graph Base Class. │ │ │ │ │ +166 V currentValues = initial; │ │ │ │ │ +167 typename S::Gradient currentGradient = system.gradient(currentValues), │ │ │ │ │ +168 prevGradient, direction = currentGradient; │ │ │ │ │ +169 │ │ │ │ │ +170 /* do one step of gradient descent */ │ │ │ │ │ +171 V prevValues = currentValues; │ │ │ │ │ +172 double prevError = currentError; │ │ │ │ │ +173 double alpha = _l_i_n_e_S_e_a_r_c_h(system, currentValues, direction); │ │ │ │ │ +174 currentValues = system.advance(prevValues, alpha, direction); │ │ │ │ │ +175 currentError = system.error(currentValues); │ │ │ │ │ +176 │ │ │ │ │ +177 // Maybe show output │ │ │ │ │ +178 if (params._v_e_r_b_o_s_i_t_y >= NonlinearOptimizerParams::ERROR) │ │ │ │ │ +179 std::cout << "Initial error: " << currentError << std::endl; │ │ │ │ │ +180 │ │ │ │ │ +181 // Iterative loop │ │ │ │ │ +182 do { │ │ │ │ │ +183 if (gradientDescent == true) { │ │ │ │ │ +184 direction = system.gradient(currentValues); │ │ │ │ │ +185 } else { │ │ │ │ │ +186 prevGradient = currentGradient; │ │ │ │ │ +187 currentGradient = system.gradient(currentValues); │ │ │ │ │ +188 // Polak-Ribiere: beta = g'*(g_n-g_n-1)/g_n-1'*g_n-1 │ │ │ │ │ +189 const double beta = std::max(0.0, │ │ │ │ │ +190 currentGradient.dot(currentGradient - prevGradient) │ │ │ │ │ +191 / prevGradient.dot(prevGradient)); │ │ │ │ │ +192 direction = currentGradient + (beta * direction); │ │ │ │ │ +193 } │ │ │ │ │ +194 │ │ │ │ │ +195 alpha = _l_i_n_e_S_e_a_r_c_h(system, currentValues, direction); │ │ │ │ │ +196 │ │ │ │ │ +197 prevValues = currentValues; │ │ │ │ │ +198 prevError = currentError; │ │ │ │ │ +199 │ │ │ │ │ +200 currentValues = system.advance(prevValues, alpha, direction); │ │ │ │ │ +201 currentError = system.error(currentValues); │ │ │ │ │ +202 │ │ │ │ │ +203 // User hook: │ │ │ │ │ +204 if (params._i_t_e_r_a_t_i_o_n_H_o_o_k) │ │ │ │ │ +205 params._i_t_e_r_a_t_i_o_n_H_o_o_k(iteration, prevError, currentError); │ │ │ │ │ +206 │ │ │ │ │ +207 // Maybe show output │ │ │ │ │ +208 if (params._v_e_r_b_o_s_i_t_y >= NonlinearOptimizerParams::ERROR) │ │ │ │ │ +209 std::cout << "iteration: " << iteration << ", currentError: " << │ │ │ │ │ +currentError << std::endl; │ │ │ │ │ +210 } while (++iteration < params._m_a_x_I_t_e_r_a_t_i_o_n_s && !singleIteration │ │ │ │ │ +211 && !_c_h_e_c_k_C_o_n_v_e_r_g_e_n_c_e(params._r_e_l_a_t_i_v_e_E_r_r_o_r_T_o_l, params._a_b_s_o_l_u_t_e_E_r_r_o_r_T_o_l, │ │ │ │ │ +212 params._e_r_r_o_r_T_o_l, prevError, currentError, params._v_e_r_b_o_s_i_t_y)); │ │ │ │ │ +213 │ │ │ │ │ +214 // Printing if verbose │ │ │ │ │ +215 if (params._v_e_r_b_o_s_i_t_y >= NonlinearOptimizerParams::ERROR │ │ │ │ │ +216 && iteration >= params._m_a_x_I_t_e_r_a_t_i_o_n_s) │ │ │ │ │ +217 std::cout │ │ │ │ │ +218 << "nonlinearConjugateGradient: Terminating because reached maximum │ │ │ │ │ +iterations" │ │ │ │ │ +219 << std::endl; │ │ │ │ │ +220 │ │ │ │ │ +221 return boost::tie(currentValues, iteration); │ │ │ │ │ +222} │ │ │ │ │ +223 │ │ │ │ │ +224} // \ namespace gtsam │ │ │ │ │ +225 │ │ │ │ │ +_M_a_n_i_f_o_l_d_._h │ │ │ │ │ +Base class and basic functions for Manifold types. │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ +Base class and parameters for nonlinear optimization algorithms. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_I_n_d_i_c_e_s │ │ │ │ │ -FastVector< FactorIndex > FactorIndices │ │ │ │ │ -Define collection types: │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_d_o_t │ │ │ │ │ -double dot(const V1 &a, const V2 &b) │ │ │ │ │ -Dot product. │ │ │ │ │ -DDeeffiinniittiioonn Vector.h:195 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ -nodes. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_p_r_i_n_t │ │ │ │ │ -virtual void print(const std::string &s="FactorGraph", const KeyFormatter │ │ │ │ │ -&formatter=DefaultKeyFormatter) const │ │ │ │ │ -Print out graph to std::cout, with optional key formatter. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph-inst.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_k_e_y_s │ │ │ │ │ -KeySet keys() const │ │ │ │ │ -Potentially slow function to return all keys involved, sorted, as a set. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph-inst.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d___f_a_c_t_o_r_s │ │ │ │ │ -FactorIndices add_factors(const CONTAINER &factors, bool useEmptySlots=false) │ │ │ │ │ -Add new factors to a factor graph and returns a list of new factor indices, │ │ │ │ │ -optionally finding and re... │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph-inst.h:109 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_d_o_t │ │ │ │ │ -void dot(std::ostream &os, const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const │ │ │ │ │ -Output to graphviz format, stream version. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph-inst.h:141 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_k_e_y_V_e_c_t_o_r │ │ │ │ │ -KeyVector keyVector() const │ │ │ │ │ -Potentially slow function to return all keys involved, sorted, as a vector. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph-inst.h:95 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_e_r_r_o_r │ │ │ │ │ -double error(const HybridValues &values) const │ │ │ │ │ -Add error for all factors. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph-inst.h:66 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_n_r_F_a_c_t_o_r_s │ │ │ │ │ -size_t nrFactors() const │ │ │ │ │ -return the number of non-null factors │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph-inst.h:76 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_s_i_z_e │ │ │ │ │ -size_t size() const │ │ │ │ │ -return the number of factors (including any null factors set by remove() ). │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:326 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const This &fg, double tol=1e-9) const │ │ │ │ │ -Check equality up to tolerance. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph-inst.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d_F_a_c_t_o_r │ │ │ │ │ -boost::shared_ptr< FACTOR > sharedFactor │ │ │ │ │ -Shared pointer to a factor. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:101 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_f_a_c_t_o_r_s__ │ │ │ │ │ -FastVector< sharedFactor > factors_ │ │ │ │ │ -concept check, makes sure FACTOR defines print and equals │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:131 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_s_a_v_e_G_r_a_p_h │ │ │ │ │ -void saveGraph(const std::string &filename, const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const │ │ │ │ │ -output to file with graphviz format. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph-inst.h:177 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r │ │ │ │ │ -DotWriter is a helper class for writing graphviz .dot files. │ │ │ │ │ -DDeeffiinniittiioonn DotWriter.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_d_r_a_w_V_a_r_i_a_b_l_e │ │ │ │ │ -void drawVariable(Key key, const KeyFormatter &keyFormatter, const boost:: │ │ │ │ │ -optional< Vector2 > &position, std::ostream *os) const │ │ │ │ │ -Create a variable dot fragment. │ │ │ │ │ -DDeeffiinniittiioonn DotWriter.cpp:42 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_p_r_o_c_e_s_s_F_a_c_t_o_r │ │ │ │ │ -void processFactor(size_t i, const KeyVector &keys, const KeyFormatter │ │ │ │ │ -&keyFormatter, const boost::optional< Vector2 > &position, std::ostream *os) │ │ │ │ │ -const │ │ │ │ │ -Draw a single factor, specified by its index i and its variable keys. │ │ │ │ │ -DDeeffiinniittiioonn DotWriter.cpp:96 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_v_a_r_i_a_b_l_e_P_o_s │ │ │ │ │ -boost::optional< Vector2 > variablePos(Key key) const │ │ │ │ │ -Return variable position or none. │ │ │ │ │ -DDeeffiinniittiioonn DotWriter.cpp:79 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_g_r_a_p_h_P_r_e_a_m_b_l_e │ │ │ │ │ -void graphPreamble(std::ostream *os) const │ │ │ │ │ -Write out preamble for graph, including size. │ │ │ │ │ -DDeeffiinniittiioonn DotWriter.cpp:30 │ │ │ │ │ +_g_t_s_a_m_:_:_l_i_n_e_S_e_a_r_c_h │ │ │ │ │ +double lineSearch(const S &system, const V currentValues, const W &gradient) │ │ │ │ │ +Implement the golden-section line search algorithm. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearConjugateGradientOptimizer.h:88 │ │ │ │ │ +_g_t_s_a_m_:_:_c_h_e_c_k_C_o_n_v_e_r_g_e_n_c_e │ │ │ │ │ +bool checkConvergence(double relativeErrorTreshold, double │ │ │ │ │ +absoluteErrorTreshold, double errorThreshold, double currentError, double │ │ │ │ │ +newError, NonlinearOptimizerParams::Verbosity verbosity) │ │ │ │ │ +Check whether the relative error decrease is less than relativeErrorTreshold, │ │ │ │ │ +the absolute error decr... │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizer.cpp:185 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ +Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key │ │ │ │ │ +landmarkKey) │ │ │ │ │ +Optimize for triangulation. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.cpp:155 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_n_l_i_n_e_a_r_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t │ │ │ │ │ +boost::tuple< V, int > nonlinearConjugateGradient(const S &system, const V │ │ │ │ │ +&initial, const NonlinearOptimizerParams ¶ms, const bool singleIteration, │ │ │ │ │ +const bool gradientDescent=false) │ │ │ │ │ +Implement the nonlinear conjugate gradient method using the Polak-Ribiere │ │ │ │ │ +formula suggested in http:/... │ │ │ │ │ +DDeeffiinniittiioonn NonlinearConjugateGradientOptimizer.h:148 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shared_ptr to this class │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:81 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ +with a unique integer... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_O_p_t_i_m_i_z_e_r │ │ │ │ │ +An implementation of the nonlinear CG method using the template below. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearConjugateGradientOptimizer.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_O_p_t_i_m_i_z_e_r_:_: │ │ │ │ │ +_~_N_o_n_l_i_n_e_a_r_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_O_p_t_i_m_i_z_e_r │ │ │ │ │ +~NonlinearConjugateGradientOptimizer() override │ │ │ │ │ +Destructor. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearConjugateGradientOptimizer.h:70 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r │ │ │ │ │ +This is the abstract interface for classes that can optimize for the maximum- │ │ │ │ │ +likelihood estimate of a... │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizer.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s │ │ │ │ │ +The common parameters for Nonlinear optimizers. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizerParams.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_a_b_s_o_l_u_t_e_E_r_r_o_r_T_o_l │ │ │ │ │ +double absoluteErrorTol │ │ │ │ │ +The maximum absolute error decrease to stop iterating (default 1e-5) │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizerParams.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_i_t_e_r_a_t_i_o_n_H_o_o_k │ │ │ │ │ +IterationHook iterationHook │ │ │ │ │ +Optional user-provided iteration hook to be called after each optimization │ │ │ │ │ +iteration (Default: none). │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizerParams.h:94 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_m_a_x_I_t_e_r_a_t_i_o_n_s │ │ │ │ │ +size_t maxIterations │ │ │ │ │ +The maximum iterations to stop iterating (default 100) │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizerParams.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_v_e_r_b_o_s_i_t_y │ │ │ │ │ +Verbosity verbosity │ │ │ │ │ +The printing verbosity during optimization (default SILENT) │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizerParams.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_r_e_l_a_t_i_v_e_E_r_r_o_r_T_o_l │ │ │ │ │ +double relativeErrorTol │ │ │ │ │ +The maximum relative error decrease to stop iterating (default 1e-5) │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizerParams.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_e_r_r_o_r_T_o_l │ │ │ │ │ +double errorTol │ │ │ │ │ +The maximum total error to stop iterating (default 0.0) │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizerParams.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _F_a_c_t_o_r_G_r_a_p_h_-_i_n_s_t_._h │ │ │ │ │ + * _n_o_n_l_i_n_e_a_r │ │ │ │ │ + * _N_o_n_l_i_n_e_a_r_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00614.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00689.html │ │ │ │┄ Files 98% similar despite different names │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces
│ │ │ │
MetisIndex-inl.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ @@ -115,13 +115,13 @@ │ │ │ │
Author
Andrew Melim
│ │ │ │
Date
Oct. 10, 2014
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00620_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01046_source.html │ │ │ │┄ Files 94% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/JunctionTree-inst.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/DoglegOptimizerImpl.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
JunctionTree-inst.h
│ │ │ │ +
DoglegOptimizerImpl.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
21#pragma once
│ │ │ │ -
22
│ │ │ │ -
23#include <gtsam/inference/JunctionTree.h>
│ │ │ │ -
24#include <gtsam/inference/ClusterTree-inst.h>
│ │ │ │ -
25#include <gtsam/symbolic/SymbolicConditional.h>
│ │ │ │ -
26#include <gtsam/symbolic/SymbolicFactor-inst.h>
│ │ │ │ -
27
│ │ │ │ -
28namespace gtsam {
│ │ │ │ -
29
│ │ │ │ -
30template<class BAYESTREE, class GRAPH, class ETREE_NODE>
│ │ │ │ -
│ │ │ │ -
31struct ConstructorTraversalData {
│ │ │ │ -
32 typedef typename JunctionTree<BAYESTREE, GRAPH>::Node Node;
│ │ │ │ -
33 typedef typename JunctionTree<BAYESTREE, GRAPH>::sharedNode sharedNode;
│ │ │ │ -
34
│ │ │ │ -
35 ConstructorTraversalData* const parentData;
│ │ │ │ -
36 sharedNode junctionTreeNode;
│ │ │ │ -
37 FastVector<SymbolicConditional::shared_ptr> childSymbolicConditionals;
│ │ │ │ -
38 FastVector<SymbolicFactor::shared_ptr> childSymbolicFactors;
│ │ │ │ +
17#pragma once
│ │ │ │ +
18
│ │ │ │ +
19#include <iomanip>
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/linear/VectorValues.h>
│ │ │ │ +
22#include <gtsam/inference/Ordering.h>
│ │ │ │ +
23
│ │ │ │ +
24namespace gtsam {
│ │ │ │ +
25
│ │ │ │ +
│ │ │ │ +
32struct GTSAM_EXPORT DoglegOptimizerImpl {
│ │ │ │ +
33
│ │ │ │ +
│ │ │ │ +
34 struct GTSAM_EXPORT IterationResult {
│ │ │ │ +
35 double delta;
│ │ │ │ +
36 VectorValues dx_d;
│ │ │ │ +
37 double f_error;
│ │ │ │ +
38 };
│ │ │ │ +
│ │ │ │
39
│ │ │ │ -
40 // Small inner class to store symbolic factors
│ │ │ │ -
│ │ │ │ -
41 class SymbolicFactors: public FactorGraph<Factor> {
│ │ │ │ -
42 };
│ │ │ │ -
│ │ │ │ -
43
│ │ │ │ -
44 ConstructorTraversalData(ConstructorTraversalData* _parentData) :
│ │ │ │ -
45 parentData(_parentData) {
│ │ │ │ -
46 }
│ │ │ │ -
47
│ │ │ │ -
48 // Pre-order visitor function
│ │ │ │ -
49 static ConstructorTraversalData ConstructorTraversalVisitorPre(
│ │ │ │ -
50 const boost::shared_ptr<ETREE_NODE>& node,
│ │ │ │ -
51 ConstructorTraversalData& parentData) {
│ │ │ │ -
52 // On the pre-order pass, before children have been visited, we just set up
│ │ │ │ -
53 // a traversal data structure with its own JT node, and create a child
│ │ │ │ -
54 // pointer in its parent.
│ │ │ │ -
55 ConstructorTraversalData myData = ConstructorTraversalData(&parentData);
│ │ │ │ -
56 myData.junctionTreeNode =
│ │ │ │ -
57 boost::make_shared<Node>(node->key, node->factors);
│ │ │ │ -
58 parentData.junctionTreeNode->addChild(myData.junctionTreeNode);
│ │ │ │ -
59 return myData;
│ │ │ │ -
60 }
│ │ │ │ -
61
│ │ │ │ -
62 // Post-order visitor function
│ │ │ │ -
63 static void ConstructorTraversalVisitorPostAlg2(
│ │ │ │ -
64 const boost::shared_ptr<ETREE_NODE>& ETreeNode,
│ │ │ │ -
65 const ConstructorTraversalData& myData) {
│ │ │ │ -
66 // In this post-order visitor, we combine the symbolic elimination results
│ │ │ │ -
67 // from the elimination tree children and symbolically eliminate the current
│ │ │ │ -
68 // elimination tree node. We then check whether each of our elimination
│ │ │ │ -
│ │ │ │ -
69 // tree child nodes should be merged with us. The check for this is that
│ │ │ │ -
70 // our number of symbolic elimination parents is exactly 1 less than
│ │ │ │ -
71 // our child's symbolic elimination parents - this condition indicates that
│ │ │ │ -
72 // eliminating the current node did not introduce any parents beyond those
│ │ │ │ -
73 // already in the child->
│ │ │ │ -
74
│ │ │ │ -
75 // Do symbolic elimination for this node
│ │ │ │ -
76 SymbolicFactors symbolicFactors;
│ │ │ │ -
77 symbolicFactors.reserve(
│ │ │ │ -
78 ETreeNode->factors.size() + myData.childSymbolicFactors.size());
│ │ │ │ -
79 // Add ETree node factors
│ │ │ │ -
80 symbolicFactors += ETreeNode->factors;
│ │ │ │ -
81 // Add symbolic factors passed up from children
│ │ │ │ -
82 symbolicFactors += myData.childSymbolicFactors;
│ │ │ │ -
83
│ │ │ │ -
84 Ordering keyAsOrdering;
│ │ │ │ -
85 keyAsOrdering.push_back(ETreeNode->key);
│ │ │ │ -
86 SymbolicConditional::shared_ptr myConditional;
│ │ │ │ -
87 SymbolicFactor::shared_ptr mySeparatorFactor;
│ │ │ │ -
88 boost::tie(myConditional, mySeparatorFactor) = internal::EliminateSymbolic(
│ │ │ │ -
89 symbolicFactors, keyAsOrdering);
│ │ │ │ -
90
│ │ │ │ -
91 // Store symbolic elimination results in the parent
│ │ │ │ -
92 myData.parentData->childSymbolicConditionals.push_back(myConditional);
│ │ │ │ -
93 myData.parentData->childSymbolicFactors.push_back(mySeparatorFactor);
│ │ │ │ -
94
│ │ │ │ -
95 sharedNode node = myData.junctionTreeNode;
│ │ │ │ -
96 const FastVector<SymbolicConditional::shared_ptr>& childConditionals =
│ │ │ │ -
97 myData.childSymbolicConditionals;
│ │ │ │ -
98 node->problemSize_ = (int) (myConditional->size() * symbolicFactors.size());
│ │ │ │ -
99
│ │ │ │ -
100 // Merge our children if they are in our clique - if our conditional has
│ │ │ │ -
101 // exactly one fewer parent than our child's conditional.
│ │ │ │ -
102 const size_t myNrParents = myConditional->nrParents();
│ │ │ │ -
103 const size_t nrChildren = node->nrChildren();
│ │ │ │ -
104 assert(childConditionals.size() == nrChildren);
│ │ │ │ -
105
│ │ │ │ -
106 // decide which children to merge, as index into children
│ │ │ │ -
107 std::vector<size_t> nrFrontals = node->nrFrontalsOfChildren();
│ │ │ │ -
108 std::vector<bool> merge(nrChildren, false);
│ │ │ │ -
109 size_t myNrFrontals = 1;
│ │ │ │ -
110 for (size_t i = 0;i<nrChildren;i++){
│ │ │ │ -
111 // Check if we should merge the i^th child
│ │ │ │ -
112 if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) {
│ │ │ │ -
113 // Increment number of frontal variables
│ │ │ │ -
114 myNrFrontals += nrFrontals[i];
│ │ │ │ -
115 merge[i] = true;
│ │ │ │ -
116 }
│ │ │ │ -
117 }
│ │ │ │ -
118
│ │ │ │ -
119 // now really merge
│ │ │ │ -
120 node->mergeChildren(merge);
│ │ │ │ -
121 }
│ │ │ │ -
122};
│ │ │ │ -
123
│ │ │ │ -
124/* ************************************************************************* */
│ │ │ │ -
125template<class BAYESTREE, class GRAPH>
│ │ │ │ -
126template<class ETREE_BAYESNET, class ETREE_GRAPH>
│ │ │ │ -
│ │ │ │ -
127JunctionTree<BAYESTREE, GRAPH>::JunctionTree(
│ │ │ │ -
128 const EliminationTree<ETREE_BAYESNET, ETREE_GRAPH>& eliminationTree) {
│ │ │ │ -
129 gttic(JunctionTree_FromEliminationTree);
│ │ │ │ -
130 // Here we rely on the BayesNet having been produced by this elimination tree,
│ │ │ │ -
131 // such that the conditionals are arranged in DFS post-order. We traverse the
│ │ │ │ -
132 // elimination tree, and inspect the symbolic conditional corresponding to
│ │ │ │ -
133 // each node. The elimination tree node is added to the same clique with its
│ │ │ │ -
134 // parent if it has exactly one more Bayes net conditional parent than
│ │ │ │ -
135 // does its elimination tree parent.
│ │ │ │ -
136
│ │ │ │ -
137 // Traverse the elimination tree, doing symbolic elimination and merging nodes
│ │ │ │ -
138 // as we go. Gather the created junction tree roots in a dummy Node.
│ │ │ │ -
139 typedef typename EliminationTree<ETREE_BAYESNET, ETREE_GRAPH>::Node ETreeNode;
│ │ │ │ -
140 typedef ConstructorTraversalData<BAYESTREE, GRAPH, ETreeNode> Data;
│ │ │ │ -
141 Data rootData(0);
│ │ │ │ -
142 // Make a dummy node to gather the junction tree roots
│ │ │ │ -
143 rootData.junctionTreeNode = boost::make_shared<typename Base::Node>();
│ │ │ │ -
144 treeTraversal::DepthFirstForest(eliminationTree, rootData,
│ │ │ │ -
145 Data::ConstructorTraversalVisitorPre,
│ │ │ │ -
146 Data::ConstructorTraversalVisitorPostAlg2);
│ │ │ │ -
147
│ │ │ │ -
148 // Assign roots from the dummy node
│ │ │ │ -
149 this->addChildrenAsRoots(rootData.junctionTreeNode);
│ │ │ │ -
150
│ │ │ │ -
151 // Transfer remaining factors from elimination tree
│ │ │ │ -
152 Base::remainingFactors_ = eliminationTree.remainingFactors();
│ │ │ │ -
153}
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
154
│ │ │ │ -
155} // namespace gtsam
│ │ │ │ -
│ │ │ │ -
JunctionTree.h
The junction tree.
│ │ │ │ -
ClusterTree-inst.h
Collects factorgraph fragments defined on variable clusters, arranged in a tree.
│ │ │ │ - │ │ │ │ - │ │ │ │ -
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │ +
│ │ │ │ +
53 enum TrustRegionAdaptationMode {
│ │ │ │ +
54 SEARCH_EACH_ITERATION,
│ │ │ │ +
55 SEARCH_REDUCE_ONLY,
│ │ │ │ +
56 ONE_STEP_PER_ITERATION
│ │ │ │ +
57 };
│ │ │ │ +
│ │ │ │ +
58
│ │ │ │ +
94 template<class M, class F, class VALUES>
│ │ │ │ +
95 static IterationResult Iterate(
│ │ │ │ +
96 double delta, TrustRegionAdaptationMode mode, const VectorValues& dx_u, const VectorValues& dx_n,
│ │ │ │ +
97 const M& Rd, const F& f, const VALUES& x0, const double f_error, const bool verbose=false);
│ │ │ │ +
98
│ │ │ │ +
121 static VectorValues ComputeDoglegPoint(double delta, const VectorValues& dx_u, const VectorValues& dx_n, const bool verbose=false);
│ │ │ │ +
122
│ │ │ │ +
132 static VectorValues ComputeBlend(double delta, const VectorValues& x_u, const VectorValues& x_n, const bool verbose=false);
│ │ │ │ +
133};
│ │ │ │ +
│ │ │ │ +
134
│ │ │ │ +
135
│ │ │ │ +
136/* ************************************************************************* */
│ │ │ │ +
137template<class M, class F, class VALUES>
│ │ │ │ +
│ │ │ │ +
138typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
│ │ │ │ +
139 double delta, TrustRegionAdaptationMode mode, const VectorValues& dx_u, const VectorValues& dx_n,
│ │ │ │ +
140 const M& Rd, const F& f, const VALUES& x0, const double f_error, const bool verbose)
│ │ │ │ +
141{
│ │ │ │ +
142 gttic(M_error);
│ │ │ │ +
143 const double M_error = Rd.error(VectorValues::Zero(dx_u));
│ │ │ │ +
144 gttoc(M_error);
│ │ │ │ +
145
│ │ │ │ +
146 // Result to return
│ │ │ │ +
147 IterationResult result;
│ │ │ │ +
148
│ │ │ │ +
149 bool stay = true;
│ │ │ │ +
150 enum { NONE, INCREASED_DELTA, DECREASED_DELTA } lastAction = NONE; // Used to prevent alternating between increasing and decreasing in one iteration
│ │ │ │ +
151 while(stay) {
│ │ │ │ +
152 gttic(Dog_leg_point);
│ │ │ │ +
153 // Compute dog leg point
│ │ │ │ +
154 result.dx_d = ComputeDoglegPoint(delta, dx_u, dx_n, verbose);
│ │ │ │ +
155 gttoc(Dog_leg_point);
│ │ │ │ +
156
│ │ │ │ +
157 if(verbose) std::cout << "delta = " << delta << ", dx_d_norm = " << result.dx_d.norm() << std::endl;
│ │ │ │ +
158
│ │ │ │ +
159 gttic(retract);
│ │ │ │ +
160 // Compute expmapped solution
│ │ │ │ +
161 const VALUES x_d(x0.retract(result.dx_d));
│ │ │ │ +
162 gttoc(retract);
│ │ │ │ +
163
│ │ │ │ +
164 gttic(decrease_in_f);
│ │ │ │ +
165 // Compute decrease in f
│ │ │ │ +
166 result.f_error = f.error(x_d);
│ │ │ │ +
167 gttoc(decrease_in_f);
│ │ │ │ +
168
│ │ │ │ +
169 gttic(new_M_error);
│ │ │ │ +
170 // Compute decrease in M
│ │ │ │ +
171 const double new_M_error = Rd.error(result.dx_d);
│ │ │ │ +
172 gttoc(new_M_error);
│ │ │ │ +
173
│ │ │ │ +
174 if(verbose) std::cout << std::setprecision(15) << "f error: " << f_error << " -> " << result.f_error << std::endl;
│ │ │ │ +
175 if(verbose) std::cout << std::setprecision(15) << "M error: " << M_error << " -> " << new_M_error << std::endl;
│ │ │ │ +
176
│ │ │ │ +
177 gttic(adjust_delta);
│ │ │ │ +
178 // Compute gain ratio. Here we take advantage of the invariant that the
│ │ │ │ +
179 // Bayes' net error at zero is equal to the nonlinear error
│ │ │ │ +
180 const double rho = std::abs(f_error - result.f_error) < 1e-15 || std::abs(M_error - new_M_error) < 1e-15 ?
│ │ │ │ +
181 0.5 :
│ │ │ │ +
182 (f_error - result.f_error) / (M_error - new_M_error);
│ │ │ │ +
183
│ │ │ │ +
184 if(verbose) std::cout << std::setprecision(15) << "rho = " << rho << std::endl;
│ │ │ │ +
185
│ │ │ │ +
186 if(rho >= 0.75) {
│ │ │ │ +
187 // M agrees very well with f, so try to increase lambda
│ │ │ │ +
188 const double dx_d_norm = result.dx_d.norm();
│ │ │ │ +
189 const double newDelta = std::max(delta, 3.0 * dx_d_norm); // Compute new delta
│ │ │ │ +
190
│ │ │ │ +
191 if(mode == ONE_STEP_PER_ITERATION || mode == SEARCH_REDUCE_ONLY)
│ │ │ │ +
192 stay = false; // If not searching, just return with the new delta
│ │ │ │ +
193 else if(mode == SEARCH_EACH_ITERATION) {
│ │ │ │ +
194 if(std::abs(newDelta - delta) < 1e-15 || lastAction == DECREASED_DELTA)
│ │ │ │ +
195 stay = false; // Searching, but Newton's solution is within trust region so keep the same trust region
│ │ │ │ +
196 else {
│ │ │ │ +
197 stay = true; // Searching and increased delta, so try again to increase delta
│ │ │ │ +
198 lastAction = INCREASED_DELTA;
│ │ │ │ +
199 }
│ │ │ │ +
200 } else {
│ │ │ │ +
201 assert(false); }
│ │ │ │ +
202
│ │ │ │ +
203 delta = newDelta; // Update delta from new delta
│ │ │ │ +
204
│ │ │ │ +
205 } else if(0.75 > rho && rho >= 0.25) {
│ │ │ │ +
206 // M agrees so-so with f, keep the same delta
│ │ │ │ +
207 stay = false;
│ │ │ │ +
208
│ │ │ │ +
209 } else if(0.25 > rho && rho >= 0.0) {
│ │ │ │ +
210 // M does not agree well with f, decrease delta until it does
│ │ │ │ +
211 double newDelta;
│ │ │ │ +
212 bool hitMinimumDelta;
│ │ │ │ +
213 if(delta > 1e-5) {
│ │ │ │ +
214 newDelta = 0.5 * delta;
│ │ │ │ +
215 hitMinimumDelta = false;
│ │ │ │ +
216 } else {
│ │ │ │ +
217 newDelta = delta;
│ │ │ │ +
218 hitMinimumDelta = true;
│ │ │ │ +
219 }
│ │ │ │ +
220 if(mode == ONE_STEP_PER_ITERATION || /* mode == SEARCH_EACH_ITERATION && */ lastAction == INCREASED_DELTA || hitMinimumDelta)
│ │ │ │ +
221 stay = false; // If not searching, just return with the new smaller delta
│ │ │ │ +
222 else if(mode == SEARCH_EACH_ITERATION || mode == SEARCH_REDUCE_ONLY) {
│ │ │ │ +
223 stay = true;
│ │ │ │ +
224 lastAction = DECREASED_DELTA;
│ │ │ │ +
225 } else {
│ │ │ │ +
226 assert(false); }
│ │ │ │ +
227
│ │ │ │ +
228 delta = newDelta; // Update delta from new delta
│ │ │ │ +
229
│ │ │ │ +
230 } else {
│ │ │ │ +
231 // f actually increased, so keep decreasing delta until f does not decrease.
│ │ │ │ +
232 // NOTE: NaN and Inf solutions also will fall into this case, so that we
│ │ │ │ +
233 // decrease delta if the solution becomes undetermined.
│ │ │ │ +
234 assert(0.0 > rho);
│ │ │ │ +
235 if(delta > 1e-5) {
│ │ │ │ +
236 delta *= 0.5;
│ │ │ │ +
237 stay = true;
│ │ │ │ +
238 lastAction = DECREASED_DELTA;
│ │ │ │ +
239 } else {
│ │ │ │ +
240 if(verbose) std::cout << "Warning: Dog leg stopping because cannot decrease error with minimum delta" << std::endl;
│ │ │ │ +
241 result.dx_d.setZero(); // Set delta to zero - don't allow error to increase
│ │ │ │ +
242 result.f_error = f_error;
│ │ │ │ +
243 stay = false;
│ │ │ │ +
244 }
│ │ │ │ +
245 }
│ │ │ │ +
246 gttoc(adjust_delta);
│ │ │ │ +
247 }
│ │ │ │ +
248
│ │ │ │ +
249 // dx_d and f_error have already been filled in during the loop
│ │ │ │ +
250 result.delta = delta;
│ │ │ │ +
251 return result;
│ │ │ │ +
252}
│ │ │ │ +
│ │ │ │ +
253
│ │ │ │ +
254}
│ │ │ │ +
Ordering.h
Variable ordering for the elimination algorithm.
│ │ │ │ +
VectorValues.h
Factor Graph Values.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::treeTraversal::DepthFirstForest
void DepthFirstForest(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, VISITOR_POST &visitorPost)
Traverse a forest depth-first with pre-order and post-order visits.
Definition treeTraversal-inst.h:77
│ │ │ │ -
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ -
gtsam::ClusterTree::Cluster
A Cluster is just a collection of factors.
Definition ClusterTree.h:36
│ │ │ │ -
gtsam::EliminationTree
An elimination tree is a data structure used intermediately during elimination.
Definition EliminationTree.h:52
│ │ │ │ -
gtsam::EliminationTree::remainingFactors
const FastVector< sharedFactor > & remainingFactors() const
Return the remaining factors that are not pulled into elimination.
Definition EliminationTree.h:154
│ │ │ │ -
gtsam::EliminationTree::Node
Definition EliminationTree.h:66
│ │ │ │ -
gtsam::ConstructorTraversalData
Definition JunctionTree-inst.h:31
│ │ │ │ -
gtsam::ConstructorTraversalData::SymbolicFactors
Definition JunctionTree-inst.h:41
│ │ │ │ -
gtsam::JunctionTree
A JunctionTree is a cluster tree, a set of variable clusters with factors, arranged in a tree,...
Definition JunctionTree.h:50
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ -
gtsam::SymbolicConditional::shared_ptr
boost::shared_ptr< This > shared_ptr
Typedef to the conditional base class.
Definition SymbolicConditional.h:44
│ │ │ │ -
gtsam::SymbolicFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
Overriding the shared_ptr typedef.
Definition SymbolicFactor.h:48
│ │ │ │ +
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
gtsam::VectorValues::Zero
static VectorValues Zero(const VectorValues &other)
Create a VectorValues with the same structure as other, but filled with zeros.
Definition VectorValues.cpp:78
│ │ │ │ +
gtsam::DoglegOptimizerImpl
This class contains the implementation of the Dogleg algorithm.
Definition DoglegOptimizerImpl.h:32
│ │ │ │ +
gtsam::DoglegOptimizerImpl::TrustRegionAdaptationMode
TrustRegionAdaptationMode
Specifies how the trust region is adapted at each Dogleg iteration.
Definition DoglegOptimizerImpl.h:53
│ │ │ │ +
gtsam::DoglegOptimizerImpl::Iterate
static IterationResult Iterate(double delta, TrustRegionAdaptationMode mode, const VectorValues &dx_u, const VectorValues &dx_n, const M &Rd, const F &f, const VALUES &x0, const double f_error, const bool verbose=false)
Compute the update point for one iteration of the Dogleg algorithm, given an initial trust region rad...
Definition DoglegOptimizerImpl.h:138
│ │ │ │ +
gtsam::DoglegOptimizerImpl::ComputeDoglegPoint
static VectorValues ComputeDoglegPoint(double delta, const VectorValues &dx_u, const VectorValues &dx_n, const bool verbose=false)
Compute the dogleg point given a trust region radius .
Definition DoglegOptimizerImpl.cpp:25
│ │ │ │ +
gtsam::DoglegOptimizerImpl::IterationResult
Definition DoglegOptimizerImpl.h:34
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,219 +1,232 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -JunctionTree-inst.h │ │ │ │ │ +DoglegOptimizerImpl.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -21#pragma once │ │ │ │ │ -22 │ │ │ │ │ -23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_J_u_n_c_t_i_o_n_T_r_e_e_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_C_l_u_s_t_e_r_T_r_e_e_-_i_n_s_t_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_s_y_m_b_o_l_i_c_/_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_s_y_m_b_o_l_i_c_/_S_y_m_b_o_l_i_c_F_a_c_t_o_r_-_i_n_s_t_._h> │ │ │ │ │ -27 │ │ │ │ │ -28namespace _g_t_s_a_m { │ │ │ │ │ -29 │ │ │ │ │ -30template │ │ │ │ │ -_3_1struct _C_o_n_s_t_r_u_c_t_o_r_T_r_a_v_e_r_s_a_l_D_a_t_a { │ │ │ │ │ -32 typedef typename _J_u_n_c_t_i_o_n_T_r_e_e_<_B_A_Y_E_S_T_R_E_E_,_ _G_R_A_P_H_>_:_:_N_o_d_e _N_o_d_e; │ │ │ │ │ -33 typedef typename JunctionTree::sharedNode sharedNode; │ │ │ │ │ -34 │ │ │ │ │ -35 _C_o_n_s_t_r_u_c_t_o_r_T_r_a_v_e_r_s_a_l_D_a_t_a* const parentData; │ │ │ │ │ -36 sharedNode junctionTreeNode; │ │ │ │ │ -37 _F_a_s_t_V_e_c_t_o_r_<_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r_> childSymbolicConditionals; │ │ │ │ │ -38 _F_a_s_t_V_e_c_t_o_r_<_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r_> childSymbolicFactors; │ │ │ │ │ +17#pragma once │ │ │ │ │ +18 │ │ │ │ │ +19#include │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_O_r_d_e_r_i_n_g_._h> │ │ │ │ │ +23 │ │ │ │ │ +24namespace _g_t_s_a_m { │ │ │ │ │ +25 │ │ │ │ │ +_3_2struct GTSAM_EXPORT _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l { │ │ │ │ │ +33 │ │ │ │ │ +_3_4 struct GTSAM_EXPORT _I_t_e_r_a_t_i_o_n_R_e_s_u_l_t { │ │ │ │ │ +35 double delta; │ │ │ │ │ +36 _V_e_c_t_o_r_V_a_l_u_e_s dx_d; │ │ │ │ │ +37 double f_error; │ │ │ │ │ +38 }; │ │ │ │ │ 39 │ │ │ │ │ -40 // Small inner class to store symbolic factors │ │ │ │ │ -_4_1 class _S_y_m_b_o_l_i_c_F_a_c_t_o_r_s: public _F_a_c_t_o_r_G_r_a_p_h { │ │ │ │ │ -42 }; │ │ │ │ │ -43 │ │ │ │ │ -44 _C_o_n_s_t_r_u_c_t_o_r_T_r_a_v_e_r_s_a_l_D_a_t_a(_C_o_n_s_t_r_u_c_t_o_r_T_r_a_v_e_r_s_a_l_D_a_t_a* _parentData) : │ │ │ │ │ -45 parentData(_parentData) { │ │ │ │ │ -46 } │ │ │ │ │ -47 │ │ │ │ │ -48 // Pre-order visitor function │ │ │ │ │ -49 static _C_o_n_s_t_r_u_c_t_o_r_T_r_a_v_e_r_s_a_l_D_a_t_a ConstructorTraversalVisitorPre( │ │ │ │ │ -50 const boost::shared_ptr& node, │ │ │ │ │ -51 _C_o_n_s_t_r_u_c_t_o_r_T_r_a_v_e_r_s_a_l_D_a_t_a& parentData) { │ │ │ │ │ -52 // On the pre-order pass, before children have been visited, we just set up │ │ │ │ │ -53 // a traversal data structure with its own JT node, and create a child │ │ │ │ │ -54 // pointer in its parent. │ │ │ │ │ -55 _C_o_n_s_t_r_u_c_t_o_r_T_r_a_v_e_r_s_a_l_D_a_t_a myData = _C_o_n_s_t_r_u_c_t_o_r_T_r_a_v_e_r_s_a_l_D_a_t_a(&parentData); │ │ │ │ │ -56 myData.junctionTreeNode = │ │ │ │ │ -57 boost::make_shared(node->key, node->factors); │ │ │ │ │ -58 parentData.junctionTreeNode->addChild(myData.junctionTreeNode); │ │ │ │ │ -59 return myData; │ │ │ │ │ -60 } │ │ │ │ │ -61 │ │ │ │ │ -62 // Post-order visitor function │ │ │ │ │ -63 static void ConstructorTraversalVisitorPostAlg2( │ │ │ │ │ -64 const boost::shared_ptr& ETreeNode, │ │ │ │ │ -65 const ConstructorTraversalData& myData) { │ │ │ │ │ -66 // In this post-order visitor, we combine the symbolic elimination results │ │ │ │ │ -67 // from the elimination tree children and symbolically eliminate the current │ │ │ │ │ -68 // elimination tree node. We then check whether each of our elimination │ │ │ │ │ -_6_9 // tree child nodes should be merged with us. The check for this is that │ │ │ │ │ -70 // our number of symbolic elimination parents is exactly 1 less than │ │ │ │ │ -71 // our child's symbolic elimination parents - this condition indicates that │ │ │ │ │ -72 // eliminating the current node did not introduce any parents beyond those │ │ │ │ │ -73 // already in the child-> │ │ │ │ │ -74 │ │ │ │ │ -75 // Do symbolic elimination for this node │ │ │ │ │ -76 SymbolicFactors symbolicFactors; │ │ │ │ │ -77 symbolicFactors.reserve( │ │ │ │ │ -78 ETreeNode->factors.size() + myData.childSymbolicFactors.size()); │ │ │ │ │ -79 // Add ETree node factors │ │ │ │ │ -80 symbolicFactors += ETreeNode->factors; │ │ │ │ │ -81 // Add symbolic factors passed up from children │ │ │ │ │ -82 symbolicFactors += myData.childSymbolicFactors; │ │ │ │ │ -83 │ │ │ │ │ -84 _O_r_d_e_r_i_n_g keyAsOrdering; │ │ │ │ │ -85 keyAsOrdering.push_back(ETreeNode->key); │ │ │ │ │ -86 _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r myConditional; │ │ │ │ │ -87 _S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r mySeparatorFactor; │ │ │ │ │ -88 boost::tie(myConditional, mySeparatorFactor) = internal::EliminateSymbolic( │ │ │ │ │ -89 symbolicFactors, keyAsOrdering); │ │ │ │ │ -90 │ │ │ │ │ -91 // Store symbolic elimination results in the parent │ │ │ │ │ -92 myData.parentData->childSymbolicConditionals.push_back(myConditional); │ │ │ │ │ -93 myData.parentData->childSymbolicFactors.push_back(mySeparatorFactor); │ │ │ │ │ -94 │ │ │ │ │ -95 sharedNode node = myData.junctionTreeNode; │ │ │ │ │ -96 const _F_a_s_t_V_e_c_t_o_r_<_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r_>& childConditionals = │ │ │ │ │ -97 myData.childSymbolicConditionals; │ │ │ │ │ -98 node->problemSize_ = (int) (myConditional->size() * symbolicFactors.size()); │ │ │ │ │ -99 │ │ │ │ │ -100 // Merge our children if they are in our clique - if our conditional has │ │ │ │ │ -101 // exactly one fewer parent than our child's conditional. │ │ │ │ │ -102 const size_t myNrParents = myConditional->nrParents(); │ │ │ │ │ -103 const size_t nrChildren = node->nrChildren(); │ │ │ │ │ -104 assert(childConditionals.size() == nrChildren); │ │ │ │ │ -105 │ │ │ │ │ -106 // decide which children to merge, as index into children │ │ │ │ │ -107 std::vector nrFrontals = node->nrFrontalsOfChildren(); │ │ │ │ │ -108 std::vector merge(nrChildren, false); │ │ │ │ │ -109 size_t myNrFrontals = 1; │ │ │ │ │ -110 for (size_t i = 0;inrParents()) { │ │ │ │ │ -113 // Increment number of frontal variables │ │ │ │ │ -114 myNrFrontals += nrFrontals[i]; │ │ │ │ │ -115 merge[i] = true; │ │ │ │ │ -116 } │ │ │ │ │ -117 } │ │ │ │ │ -118 │ │ │ │ │ -119 // now really merge │ │ │ │ │ -120 node->mergeChildren(merge); │ │ │ │ │ -121 } │ │ │ │ │ -122}; │ │ │ │ │ -123 │ │ │ │ │ -124/* ************************************************************************* │ │ │ │ │ +_5_3 enum _T_r_u_s_t_R_e_g_i_o_n_A_d_a_p_t_a_t_i_o_n_M_o_d_e { │ │ │ │ │ +54 SEARCH_EACH_ITERATION, │ │ │ │ │ +55 SEARCH_REDUCE_ONLY, │ │ │ │ │ +56 ONE_STEP_PER_ITERATION │ │ │ │ │ +57 }; │ │ │ │ │ +58 │ │ │ │ │ +94 template │ │ │ │ │ +95 static IterationResult Iterate( │ │ │ │ │ +96 double delta, TrustRegionAdaptationMode mode, const _V_e_c_t_o_r_V_a_l_u_e_s& dx_u, │ │ │ │ │ +const _V_e_c_t_o_r_V_a_l_u_e_s& dx_n, │ │ │ │ │ +97 const M& Rd, const F& f, const VALUES& x0, const double f_error, const bool │ │ │ │ │ +verbose=false); │ │ │ │ │ +98 │ │ │ │ │ +121 static _V_e_c_t_o_r_V_a_l_u_e_s ComputeDoglegPoint(double delta, const _V_e_c_t_o_r_V_a_l_u_e_s& │ │ │ │ │ +dx_u, const _V_e_c_t_o_r_V_a_l_u_e_s& dx_n, const bool verbose=false); │ │ │ │ │ +122 │ │ │ │ │ +132 static _V_e_c_t_o_r_V_a_l_u_e_s ComputeBlend(double delta, const _V_e_c_t_o_r_V_a_l_u_e_s& x_u, │ │ │ │ │ +const _V_e_c_t_o_r_V_a_l_u_e_s& x_n, const bool verbose=false); │ │ │ │ │ +133}; │ │ │ │ │ +134 │ │ │ │ │ +135 │ │ │ │ │ +136/* ************************************************************************* │ │ │ │ │ */ │ │ │ │ │ -125template │ │ │ │ │ -126template │ │ │ │ │ -_1_2_7_J_u_n_c_t_i_o_n_T_r_e_e_<_B_A_Y_E_S_T_R_E_E_,_ _G_R_A_P_H_>_:_:_J_u_n_c_t_i_o_n_T_r_e_e( │ │ │ │ │ -128 const _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_E_T_R_E_E___B_A_Y_E_S_N_E_T_,_ _E_T_R_E_E___G_R_A_P_H_>& eliminationTree) { │ │ │ │ │ -129 gttic(JunctionTree_FromEliminationTree); │ │ │ │ │ -130 // Here we rely on the BayesNet having been produced by this elimination │ │ │ │ │ -tree, │ │ │ │ │ -131 // such that the conditionals are arranged in DFS post-order. We traverse │ │ │ │ │ -the │ │ │ │ │ -132 // elimination tree, and inspect the symbolic conditional corresponding to │ │ │ │ │ -133 // each node. The elimination tree node is added to the same clique with │ │ │ │ │ -its │ │ │ │ │ -134 // parent if it has exactly one more Bayes net conditional parent than │ │ │ │ │ -135 // does its elimination tree parent. │ │ │ │ │ -136 │ │ │ │ │ -137 // Traverse the elimination tree, doing symbolic elimination and merging │ │ │ │ │ -nodes │ │ │ │ │ -138 // as we go. Gather the created junction tree roots in a dummy Node. │ │ │ │ │ -139 typedef typename _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_E_T_R_E_E___B_A_Y_E_S_N_E_T_,_ _E_T_R_E_E___G_R_A_P_H_>_:_:_N_o_d_e │ │ │ │ │ -ETreeNode; │ │ │ │ │ -140 typedef _C_o_n_s_t_r_u_c_t_o_r_T_r_a_v_e_r_s_a_l_D_a_t_a_<_B_A_Y_E_S_T_R_E_E_,_ _G_R_A_P_H_,_ _E_T_r_e_e_N_o_d_e_> Data; │ │ │ │ │ -141 Data rootData(0); │ │ │ │ │ -142 // Make a dummy node to gather the junction tree roots │ │ │ │ │ -143 rootData.junctionTreeNode = boost::make_shared(); │ │ │ │ │ -144 _t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t(eliminationTree, rootData, │ │ │ │ │ -145 Data::ConstructorTraversalVisitorPre, │ │ │ │ │ -146 Data::ConstructorTraversalVisitorPostAlg2); │ │ │ │ │ -147 │ │ │ │ │ -148 // Assign roots from the dummy node │ │ │ │ │ -149 this->addChildrenAsRoots(rootData.junctionTreeNode); │ │ │ │ │ -150 │ │ │ │ │ -151 // Transfer remaining factors from elimination tree │ │ │ │ │ -152 Base::remainingFactors_ = eliminationTree._r_e_m_a_i_n_i_n_g_F_a_c_t_o_r_s(); │ │ │ │ │ -153} │ │ │ │ │ -154 │ │ │ │ │ -155} // namespace gtsam │ │ │ │ │ -_J_u_n_c_t_i_o_n_T_r_e_e_._h │ │ │ │ │ -The junction tree. │ │ │ │ │ -_C_l_u_s_t_e_r_T_r_e_e_-_i_n_s_t_._h │ │ │ │ │ -Collects factorgraph fragments defined on variable clusters, arranged in a │ │ │ │ │ -tree. │ │ │ │ │ -_S_y_m_b_o_l_i_c_F_a_c_t_o_r_-_i_n_s_t_._h │ │ │ │ │ -_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ -std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ -FastVector │ │ │ │ │ -FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ -DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ +137template │ │ │ │ │ +_1_3_8typename _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_:_:_I_t_e_r_a_t_i_o_n_R_e_s_u_l_t _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_:_:_I_t_e_r_a_t_e( │ │ │ │ │ +139 double delta, _T_r_u_s_t_R_e_g_i_o_n_A_d_a_p_t_a_t_i_o_n_M_o_d_e mode, const _V_e_c_t_o_r_V_a_l_u_e_s& dx_u, │ │ │ │ │ +const _V_e_c_t_o_r_V_a_l_u_e_s& dx_n, │ │ │ │ │ +140 const M& Rd, const F& f, const VALUES& x0, const double f_error, const bool │ │ │ │ │ +verbose) │ │ │ │ │ +141{ │ │ │ │ │ +142 gttic(M_error); │ │ │ │ │ +143 const double M_error = Rd.error(_V_e_c_t_o_r_V_a_l_u_e_s_:_:_Z_e_r_o(dx_u)); │ │ │ │ │ +144 gttoc(M_error); │ │ │ │ │ +145 │ │ │ │ │ +146 // Result to return │ │ │ │ │ +147 _I_t_e_r_a_t_i_o_n_R_e_s_u_l_t result; │ │ │ │ │ +148 │ │ │ │ │ +149 bool stay = true; │ │ │ │ │ +150 enum { NONE, INCREASED_DELTA, DECREASED_DELTA } lastAction = NONE; // Used │ │ │ │ │ +to prevent alternating between increasing and decreasing in one iteration │ │ │ │ │ +151 while(stay) { │ │ │ │ │ +152 gttic(Dog_leg_point); │ │ │ │ │ +153 // Compute dog leg point │ │ │ │ │ +154 result.dx_d = _C_o_m_p_u_t_e_D_o_g_l_e_g_P_o_i_n_t(delta, dx_u, dx_n, verbose); │ │ │ │ │ +155 gttoc(Dog_leg_point); │ │ │ │ │ +156 │ │ │ │ │ +157 if(verbose) std::cout << "delta = " << delta << ", dx_d_norm = " << │ │ │ │ │ +result.dx_d.norm() << std::endl; │ │ │ │ │ +158 │ │ │ │ │ +159 gttic(retract); │ │ │ │ │ +160 // Compute expmapped solution │ │ │ │ │ +161 const VALUES x_d(x0.retract(result.dx_d)); │ │ │ │ │ +162 gttoc(retract); │ │ │ │ │ +163 │ │ │ │ │ +164 gttic(decrease_in_f); │ │ │ │ │ +165 // Compute decrease in f │ │ │ │ │ +166 result.f_error = f.error(x_d); │ │ │ │ │ +167 gttoc(decrease_in_f); │ │ │ │ │ +168 │ │ │ │ │ +169 gttic(new_M_error); │ │ │ │ │ +170 // Compute decrease in M │ │ │ │ │ +171 const double new_M_error = Rd.error(result.dx_d); │ │ │ │ │ +172 gttoc(new_M_error); │ │ │ │ │ +173 │ │ │ │ │ +174 if(verbose) std::cout << std::setprecision(15) << "f error: " << f_error << │ │ │ │ │ +" -> " << result.f_error << std::endl; │ │ │ │ │ +175 if(verbose) std::cout << std::setprecision(15) << "M error: " << M_error << │ │ │ │ │ +" -> " << new_M_error << std::endl; │ │ │ │ │ +176 │ │ │ │ │ +177 gttic(adjust_delta); │ │ │ │ │ +178 // Compute gain ratio. Here we take advantage of the invariant that the │ │ │ │ │ +179 // Bayes' net error at zero is equal to the nonlinear error │ │ │ │ │ +180 const double rho = std::abs(f_error - result.f_error) < 1e-15 || std::abs │ │ │ │ │ +(M_error - new_M_error) < 1e-15 ? │ │ │ │ │ +181 0.5 : │ │ │ │ │ +182 (f_error - result.f_error) / (M_error - new_M_error); │ │ │ │ │ +183 │ │ │ │ │ +184 if(verbose) std::cout << std::setprecision(15) << "rho = " << rho << std:: │ │ │ │ │ +endl; │ │ │ │ │ +185 │ │ │ │ │ +186 if(rho >= 0.75) { │ │ │ │ │ +187 // M agrees very well with f, so try to increase lambda │ │ │ │ │ +188 const double dx_d_norm = result.dx_d.norm(); │ │ │ │ │ +189 const double newDelta = std::max(delta, 3.0 * dx_d_norm); // Compute new │ │ │ │ │ +delta │ │ │ │ │ +190 │ │ │ │ │ +191 if(mode == ONE_STEP_PER_ITERATION || mode == SEARCH_REDUCE_ONLY) │ │ │ │ │ +192 stay = false; // If not searching, just return with the new delta │ │ │ │ │ +193 else if(mode == SEARCH_EACH_ITERATION) { │ │ │ │ │ +194 if(std::abs(newDelta - delta) < 1e-15 || lastAction == DECREASED_DELTA) │ │ │ │ │ +195 stay = false; // Searching, but Newton's solution is within trust region so │ │ │ │ │ +keep the same trust region │ │ │ │ │ +196 else { │ │ │ │ │ +197 stay = true; // Searching and increased delta, so try again to increase │ │ │ │ │ +delta │ │ │ │ │ +198 lastAction = INCREASED_DELTA; │ │ │ │ │ +199 } │ │ │ │ │ +200 } else { │ │ │ │ │ +201 assert(false); } │ │ │ │ │ +202 │ │ │ │ │ +203 delta = newDelta; // Update delta from new delta │ │ │ │ │ +204 │ │ │ │ │ +205 } else if(0.75 > rho && rho >= 0.25) { │ │ │ │ │ +206 // M agrees so-so with f, keep the same delta │ │ │ │ │ +207 stay = false; │ │ │ │ │ +208 │ │ │ │ │ +209 } else if(0.25 > rho && rho >= 0.0) { │ │ │ │ │ +210 // M does not agree well with f, decrease delta until it does │ │ │ │ │ +211 double newDelta; │ │ │ │ │ +212 bool hitMinimumDelta; │ │ │ │ │ +213 if(delta > 1e-5) { │ │ │ │ │ +214 newDelta = 0.5 * delta; │ │ │ │ │ +215 hitMinimumDelta = false; │ │ │ │ │ +216 } else { │ │ │ │ │ +217 newDelta = delta; │ │ │ │ │ +218 hitMinimumDelta = true; │ │ │ │ │ +219 } │ │ │ │ │ +220 if(mode == ONE_STEP_PER_ITERATION || /* mode == SEARCH_EACH_ITERATION && */ │ │ │ │ │ +lastAction == INCREASED_DELTA || hitMinimumDelta) │ │ │ │ │ +221 stay = false; // If not searching, just return with the new smaller delta │ │ │ │ │ +222 else if(mode == SEARCH_EACH_ITERATION || mode == SEARCH_REDUCE_ONLY) { │ │ │ │ │ +223 stay = true; │ │ │ │ │ +224 lastAction = DECREASED_DELTA; │ │ │ │ │ +225 } else { │ │ │ │ │ +226 assert(false); } │ │ │ │ │ +227 │ │ │ │ │ +228 delta = newDelta; // Update delta from new delta │ │ │ │ │ +229 │ │ │ │ │ +230 } else { │ │ │ │ │ +231 // f actually increased, so keep decreasing delta until f does not │ │ │ │ │ +decrease. │ │ │ │ │ +232 // NOTE: NaN and Inf solutions also will fall into this case, so that we │ │ │ │ │ +233 // decrease delta if the solution becomes undetermined. │ │ │ │ │ +234 assert(0.0 > rho); │ │ │ │ │ +235 if(delta > 1e-5) { │ │ │ │ │ +236 delta *= 0.5; │ │ │ │ │ +237 stay = true; │ │ │ │ │ +238 lastAction = DECREASED_DELTA; │ │ │ │ │ +239 } else { │ │ │ │ │ +240 if(verbose) std::cout << "Warning: Dog leg stopping because cannot decrease │ │ │ │ │ +error with minimum delta" << std::endl; │ │ │ │ │ +241 result.dx_d.setZero(); // Set delta to zero - don't allow error to increase │ │ │ │ │ +242 result.f_error = f_error; │ │ │ │ │ +243 stay = false; │ │ │ │ │ +244 } │ │ │ │ │ +245 } │ │ │ │ │ +246 gttoc(adjust_delta); │ │ │ │ │ +247 } │ │ │ │ │ +248 │ │ │ │ │ +249 // dx_d and f_error have already been filled in during the loop │ │ │ │ │ +250 result.delta = delta; │ │ │ │ │ +251 return result; │ │ │ │ │ +252} │ │ │ │ │ +253 │ │ │ │ │ +254} │ │ │ │ │ +_O_r_d_e_r_i_n_g_._h │ │ │ │ │ +Variable ordering for the elimination algorithm. │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ +Factor Graph Values. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_D_e_p_t_h_F_i_r_s_t_F_o_r_e_s_t │ │ │ │ │ -void DepthFirstForest(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, │ │ │ │ │ -VISITOR_POST &visitorPost) │ │ │ │ │ -Traverse a forest depth-first with pre-order and post-order visits. │ │ │ │ │ -DDeeffiinniittiioonn treeTraversal-inst.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ -nodes. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ -_g_t_s_a_m_:_:_C_l_u_s_t_e_r_T_r_e_e_:_:_C_l_u_s_t_e_r │ │ │ │ │ -A Cluster is just a collection of factors. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ -An elimination tree is a data structure used intermediately during elimination. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_r_e_m_a_i_n_i_n_g_F_a_c_t_o_r_s │ │ │ │ │ -const FastVector< sharedFactor > & remainingFactors() const │ │ │ │ │ -Return the remaining factors that are not pulled into elimination. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:154 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_N_o_d_e │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:66 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_s_t_r_u_c_t_o_r_T_r_a_v_e_r_s_a_l_D_a_t_a │ │ │ │ │ -DDeeffiinniittiioonn JunctionTree-inst.h:31 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_s_t_r_u_c_t_o_r_T_r_a_v_e_r_s_a_l_D_a_t_a_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_s │ │ │ │ │ -DDeeffiinniittiioonn JunctionTree-inst.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ -A JunctionTree is a cluster tree, a set of variable clusters with factors, │ │ │ │ │ -arranged in a tree,... │ │ │ │ │ -DDeeffiinniittiioonn JunctionTree.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -Typedef to the conditional base class. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicConditional.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -Overriding the shared_ptr typedef. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactor.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ +with a unique integer... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_Z_e_r_o │ │ │ │ │ +static VectorValues Zero(const VectorValues &other) │ │ │ │ │ +Create a VectorValues with the same structure as other, but filled with zeros. │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.cpp:78 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l │ │ │ │ │ +This class contains the implementation of the Dogleg algorithm. │ │ │ │ │ +DDeeffiinniittiioonn DoglegOptimizerImpl.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_:_:_T_r_u_s_t_R_e_g_i_o_n_A_d_a_p_t_a_t_i_o_n_M_o_d_e │ │ │ │ │ +TrustRegionAdaptationMode │ │ │ │ │ +Specifies how the trust region is adapted at each Dogleg iteration. │ │ │ │ │ +DDeeffiinniittiioonn DoglegOptimizerImpl.h:53 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_:_:_I_t_e_r_a_t_e │ │ │ │ │ +static IterationResult Iterate(double delta, TrustRegionAdaptationMode mode, │ │ │ │ │ +const VectorValues &dx_u, const VectorValues &dx_n, const M &Rd, const F &f, │ │ │ │ │ +const VALUES &x0, const double f_error, const bool verbose=false) │ │ │ │ │ +Compute the update point for one iteration of the Dogleg algorithm, given an │ │ │ │ │ +initial trust region rad... │ │ │ │ │ +DDeeffiinniittiioonn DoglegOptimizerImpl.h:138 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_:_:_C_o_m_p_u_t_e_D_o_g_l_e_g_P_o_i_n_t │ │ │ │ │ +static VectorValues ComputeDoglegPoint(double delta, const VectorValues &dx_u, │ │ │ │ │ +const VectorValues &dx_n, const bool verbose=false) │ │ │ │ │ +Compute the dogleg point given a trust region radius . │ │ │ │ │ +DDeeffiinniittiioonn DoglegOptimizerImpl.cpp:25 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_:_:_I_t_e_r_a_t_i_o_n_R_e_s_u_l_t │ │ │ │ │ +DDeeffiinniittiioonn DoglegOptimizerImpl.h:34 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _J_u_n_c_t_i_o_n_T_r_e_e_-_i_n_s_t_._h │ │ │ │ │ + * _n_o_n_l_i_n_e_a_r │ │ │ │ │ + * _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00632_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01349_source.html │ │ │ │┄ Files 94% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/JunctionTree.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicEliminationTree.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
JunctionTree.h
│ │ │ │ +
SymbolicEliminationTree.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
21#pragma once
│ │ │ │ -
22
│ │ │ │ -
23#include <gtsam/inference/ClusterTree.h>
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/symbolic/SymbolicBayesNet.h>
│ │ │ │ +
22#include <gtsam/symbolic/SymbolicFactorGraph.h>
│ │ │ │ +
23#include <gtsam/inference/EliminationTree.h>
│ │ │ │
24
│ │ │ │
25namespace gtsam {
│ │ │ │
26
│ │ │ │ -
27 // Forward declarations
│ │ │ │ -
28 template<class BAYESNET, class GRAPH> class EliminationTree;
│ │ │ │ -
29
│ │ │ │ -
49 template<class BAYESTREE, class GRAPH>
│ │ │ │ -
│ │ │ │ -
50 class JunctionTree : public EliminatableClusterTree<BAYESTREE, GRAPH> {
│ │ │ │ -
51
│ │ │ │ -
52 public:
│ │ │ │ -
53
│ │ │ │ -
54 typedef JunctionTree<BAYESTREE, GRAPH> This;
│ │ │ │ -
55 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
56 typedef EliminatableClusterTree<BAYESTREE, GRAPH> Base;
│ │ │ │ +
│ │ │ │ +
27 class GTSAM_EXPORT SymbolicEliminationTree :
│ │ │ │ +
28 public EliminationTree<SymbolicBayesNet, SymbolicFactorGraph>
│ │ │ │ +
29 {
│ │ │ │ +
30 public:
│ │ │ │ +
31 typedef EliminationTree<SymbolicBayesNet, SymbolicFactorGraph> Base;
│ │ │ │ +
32 typedef SymbolicEliminationTree This;
│ │ │ │ +
33 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
34
│ │ │ │ +
40 SymbolicEliminationTree(const SymbolicFactorGraph& factorGraph,
│ │ │ │ +
41 const VariableIndex& structure, const Ordering& order);
│ │ │ │ +
42
│ │ │ │ +
47 SymbolicEliminationTree(const SymbolicFactorGraph& factorGraph,
│ │ │ │ +
48 const Ordering& order);
│ │ │ │ +
49
│ │ │ │ +
51 bool equals(const This& other, double tol = 1e-9) const;
│ │ │ │ +
52
│ │ │ │ +
53 private:
│ │ │ │ +
54
│ │ │ │ +
56 SymbolicEliminationTree() {}
│ │ │ │
57
│ │ │ │ -
58 protected:
│ │ │ │ +
58 friend class ::EliminationTreeTester;
│ │ │ │
59
│ │ │ │ -
62
│ │ │ │ -
64 template<class ETREE>
│ │ │ │ -
65 static This FromEliminationTree(const ETREE& eliminationTree) { return This(eliminationTree); }
│ │ │ │ +
60 };
│ │ │ │ +
│ │ │ │ +
61
│ │ │ │ +
63template<>
│ │ │ │ +
│ │ │ │ +
64struct traits<SymbolicEliminationTree> : public Testable<SymbolicEliminationTree> {
│ │ │ │ +
65};
│ │ │ │ +
│ │ │ │
66
│ │ │ │ -
68 template<class ETREE_BAYESNET, class ETREE_GRAPH>
│ │ │ │ -
69 JunctionTree(const EliminationTree<ETREE_BAYESNET, ETREE_GRAPH>& eliminationTree);
│ │ │ │ -
70
│ │ │ │ -
72
│ │ │ │ -
73 protected:
│ │ │ │ -
74
│ │ │ │ -
75 // Private default constructor (used in static construction methods)
│ │ │ │ -
76 JunctionTree() {}
│ │ │ │ -
77
│ │ │ │ -
78 };
│ │ │ │ -
│ │ │ │ -
79
│ │ │ │ -
80}
│ │ │ │ -
ClusterTree.h
Collects factorgraph fragments defined on variable clusters, arranged in a tree.
│ │ │ │ +
67} //\ namespace gtsam
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::EliminatableClusterTree
A cluster-tree that eliminates to a Bayes tree.
Definition ClusterTree.h:184
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │
gtsam::EliminationTree
An elimination tree is a data structure used intermediately during elimination.
Definition EliminationTree.h:52
│ │ │ │ -
gtsam::JunctionTree
A JunctionTree is a cluster tree, a set of variable clusters with factors, arranged in a tree,...
Definition JunctionTree.h:50
│ │ │ │ -
gtsam::JunctionTree::FromEliminationTree
static This FromEliminationTree(const ETREE &eliminationTree)
Build the junction tree from an elimination tree.
Definition JunctionTree.h:65
│ │ │ │ -
gtsam::JunctionTree::This
JunctionTree< BAYESTREE, GRAPH > This
This class.
Definition JunctionTree.h:54
│ │ │ │ -
gtsam::JunctionTree::Base
EliminatableClusterTree< BAYESTREE, GRAPH > Base
Our base class.
Definition JunctionTree.h:56
│ │ │ │ -
gtsam::JunctionTree::shared_ptr
boost::shared_ptr< This > shared_ptr
Shared pointer to this class.
Definition JunctionTree.h:55
│ │ │ │ +
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition VariableIndex.h:43
│ │ │ │ +
gtsam::SymbolicEliminationTree
Definition SymbolicEliminationTree.h:29
│ │ │ │ +
gtsam::SymbolicEliminationTree::shared_ptr
boost::shared_ptr< This > shared_ptr
Shared pointer to this class.
Definition SymbolicEliminationTree.h:33
│ │ │ │ +
gtsam::SymbolicEliminationTree::Base
EliminationTree< SymbolicBayesNet, SymbolicFactorGraph > Base
Base class.
Definition SymbolicEliminationTree.h:31
│ │ │ │ +
gtsam::SymbolicEliminationTree::This
SymbolicEliminationTree This
This class.
Definition SymbolicEliminationTree.h:32
│ │ │ │ +
gtsam::SymbolicFactorGraph
Symbolic Factor Graph.
Definition SymbolicFactorGraph.h:64
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,94 +1,104 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -JunctionTree.h │ │ │ │ │ +SymbolicEliminationTree.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -21#pragma once │ │ │ │ │ -22 │ │ │ │ │ -23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_C_l_u_s_t_e_r_T_r_e_e_._h> │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_s_y_m_b_o_l_i_c_/_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_s_y_m_b_o_l_i_c_/_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h> │ │ │ │ │ 24 │ │ │ │ │ 25namespace _g_t_s_a_m { │ │ │ │ │ 26 │ │ │ │ │ -27 // Forward declarations │ │ │ │ │ -28 template class EliminationTree; │ │ │ │ │ -29 │ │ │ │ │ -49 template │ │ │ │ │ -_5_0 class _J_u_n_c_t_i_o_n_T_r_e_e : public _E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e { │ │ │ │ │ -51 │ │ │ │ │ -52 public: │ │ │ │ │ -53 │ │ │ │ │ -_5_4 typedef _J_u_n_c_t_i_o_n_T_r_e_e_<_B_A_Y_E_S_T_R_E_E_,_ _G_R_A_P_H_> _T_h_i_s; │ │ │ │ │ -_5_5 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -_5_6 typedef _E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e_<_B_A_Y_E_S_T_R_E_E_,_ _G_R_A_P_H_> _B_a_s_e; │ │ │ │ │ +_2_7 class GTSAM_EXPORT _S_y_m_b_o_l_i_c_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e : │ │ │ │ │ +28 public _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ +29 { │ │ │ │ │ +30 public: │ │ │ │ │ +_3_1 typedef _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_,_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_> _B_a_s_e; │ │ │ │ │ +_3_2 typedef _S_y_m_b_o_l_i_c_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e _T_h_i_s; │ │ │ │ │ +_3_3 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +34 │ │ │ │ │ +40 _S_y_m_b_o_l_i_c_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e(const _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h& factorGraph, │ │ │ │ │ +41 const _V_a_r_i_a_b_l_e_I_n_d_e_x& structure, const _O_r_d_e_r_i_n_g& order); │ │ │ │ │ +42 │ │ │ │ │ +47 _S_y_m_b_o_l_i_c_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e(const _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h& factorGraph, │ │ │ │ │ +48 const _O_r_d_e_r_i_n_g& order); │ │ │ │ │ +49 │ │ │ │ │ +51 bool _e_q_u_a_l_s(const _T_h_i_s& other, double tol = 1e-9) const; │ │ │ │ │ +52 │ │ │ │ │ +53 private: │ │ │ │ │ +54 │ │ │ │ │ +56 _S_y_m_b_o_l_i_c_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e() {} │ │ │ │ │ 57 │ │ │ │ │ -58 protected: │ │ │ │ │ +58 friend class ::EliminationTreeTester; │ │ │ │ │ 59 │ │ │ │ │ -62 │ │ │ │ │ -64 template │ │ │ │ │ -_6_5 static _T_h_i_s _F_r_o_m_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e(const ETREE& eliminationTree) { return _T_h_i_s │ │ │ │ │ -(eliminationTree); } │ │ │ │ │ +60 }; │ │ │ │ │ +61 │ │ │ │ │ +63template<> │ │ │ │ │ +_6_4struct _t_r_a_i_t_s<_S_y_m_b_o_l_i_c_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e> : public │ │ │ │ │ +_T_e_s_t_a_b_l_e { │ │ │ │ │ +65}; │ │ │ │ │ 66 │ │ │ │ │ -68 template │ │ │ │ │ -69 _J_u_n_c_t_i_o_n_T_r_e_e(const _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_E_T_R_E_E___B_A_Y_E_S_N_E_T_,_ _E_T_R_E_E___G_R_A_P_H_>& │ │ │ │ │ -eliminationTree); │ │ │ │ │ -70 │ │ │ │ │ -72 │ │ │ │ │ -73 protected: │ │ │ │ │ -74 │ │ │ │ │ -75 // Private default constructor (used in static construction methods) │ │ │ │ │ -76 _J_u_n_c_t_i_o_n_T_r_e_e() {} │ │ │ │ │ -77 │ │ │ │ │ -78 }; │ │ │ │ │ -79 │ │ │ │ │ -80} │ │ │ │ │ -_C_l_u_s_t_e_r_T_r_e_e_._h │ │ │ │ │ -Collects factorgraph fragments defined on variable clusters, arranged in a │ │ │ │ │ -tree. │ │ │ │ │ +67} //\ namespace gtsam │ │ │ │ │ +_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h │ │ │ │ │ +_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e │ │ │ │ │ -A cluster-tree that eliminates to a Bayes tree. │ │ │ │ │ -DDeeffiinniittiioonn ClusterTree.h:184 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ An elimination tree is a data structure used intermediately during elimination. │ │ │ │ │ DDeeffiinniittiioonn EliminationTree.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ -A JunctionTree is a cluster tree, a set of variable clusters with factors, │ │ │ │ │ -arranged in a tree,... │ │ │ │ │ -DDeeffiinniittiioonn JunctionTree.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_J_u_n_c_t_i_o_n_T_r_e_e_:_:_F_r_o_m_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ -static This FromEliminationTree(const ETREE &eliminationTree) │ │ │ │ │ -Build the junction tree from an elimination tree. │ │ │ │ │ -DDeeffiinniittiioonn JunctionTree.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_J_u_n_c_t_i_o_n_T_r_e_e_:_:_T_h_i_s │ │ │ │ │ -JunctionTree< BAYESTREE, GRAPH > This │ │ │ │ │ -This class. │ │ │ │ │ -DDeeffiinniittiioonn JunctionTree.h:54 │ │ │ │ │ -_g_t_s_a_m_:_:_J_u_n_c_t_i_o_n_T_r_e_e_:_:_B_a_s_e │ │ │ │ │ -EliminatableClusterTree< BAYESTREE, GRAPH > Base │ │ │ │ │ -Our base class. │ │ │ │ │ -DDeeffiinniittiioonn JunctionTree.h:56 │ │ │ │ │ -_g_t_s_a_m_:_:_J_u_n_c_t_i_o_n_T_r_e_e_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ +The VariableIndex class computes and stores the block column structure of a │ │ │ │ │ +factor graph. │ │ │ │ │ +DDeeffiinniittiioonn VariableIndex.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ +DDeeffiinniittiioonn SymbolicEliminationTree.h:29 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ boost::shared_ptr< This > shared_ptr │ │ │ │ │ Shared pointer to this class. │ │ │ │ │ -DDeeffiinniittiioonn JunctionTree.h:55 │ │ │ │ │ +DDeeffiinniittiioonn SymbolicEliminationTree.h:33 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_B_a_s_e │ │ │ │ │ +EliminationTree< SymbolicBayesNet, SymbolicFactorGraph > Base │ │ │ │ │ +Base class. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicEliminationTree.h:31 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_T_h_i_s │ │ │ │ │ +SymbolicEliminationTree This │ │ │ │ │ +This class. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicEliminationTree.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +Symbolic Factor Graph. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactorGraph.h:64 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _J_u_n_c_t_i_o_n_T_r_e_e_._h │ │ │ │ │ + * _s_y_m_b_o_l_i_c │ │ │ │ │ + * _S_y_m_b_o_l_i_c_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00641.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00770.html │ │ │ │┄ Files 90% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Factor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianFactorGraph.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs
│ │ │ │ -
Factor.h File Reference
│ │ │ │ +Functions
│ │ │ │ +
GaussianFactorGraph.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

The base class for all factors. │ │ │ │ +

Linear Factor Graph where all factors are Gaussians. │ │ │ │ More...

│ │ │ │ │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::Factor
struct  gtsam::EliminationTraits< GaussianFactorGraph >
 
class  gtsam::GaussianFactorGraph
 A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. More...
 
struct  gtsam::traits< GaussianFactorGraph >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ -Typedefs

typedef FastVector< FactorIndex > gtsam::FactorIndices
 Define collection types:
 
│ │ │ │ -typedef FastSet< FactorIndex > gtsam::FactorIndexSet
 

│ │ │ │ +Functions

bool gtsam::hasConstraints (const GaussianFactorGraph &factors)
 Evaluates whether linear factors have any constrained noise models.
 
│ │ │ │

Detailed Description

│ │ │ │ -

The base class for all factors.

│ │ │ │ +

Linear Factor Graph where all factors are Gaussians.

│ │ │ │
Author
Kai Ni
│ │ │ │
│ │ │ │ -Frank Dellaert
│ │ │ │ +Christian Potthast │ │ │ │ +
│ │ │ │ +Alireza Fathi
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │ -Richard Roberts
│ │ │ │ +Frank Dellaert │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,32 +1,39 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ -Factor.h File Reference │ │ │ │ │ -The base class for all factors. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +GaussianFactorGraph.h File Reference │ │ │ │ │ +Linear Factor Graph where all factors are Gaussians. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_F_a_c_t_o_r │ │ │ │ │ +struct   _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _> │ │ │ │ │ +  │ │ │ │ │ + class   _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +  A Linear _F_a_c_t_o_r Graph is a factor graph where all factors are │ │ │ │ │ + Gaussian, i.e. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -typedef _F_a_s_t_V_e_c_t_o_r< _F_a_c_t_o_r_I_n_d_e_x >  _g_t_s_a_m_:_:_F_a_c_t_o_r_I_n_d_i_c_e_s │ │ │ │ │ -  Define collection types: │ │ │ │ │ -  │ │ │ │ │ - typedef _F_a_s_t_S_e_t< _F_a_c_t_o_r_I_n_d_e_x >  ggttssaamm::::FFaaccttoorrIInnddeexxSSeett │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +bool  _g_t_s_a_m_:_:_h_a_s_C_o_n_s_t_r_a_i_n_t_s (const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &factors) │ │ │ │ │ +  Evaluates whether linear factors have any constrained noise models. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -The base class for all factors. │ │ │ │ │ +Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ Author │ │ │ │ │ Kai Ni │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Christian Potthast │ │ │ │ │ + Alireza Fathi │ │ │ │ │ Richard Roberts │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _F_a_c_t_o_r_._h │ │ │ │ │ + * _l_i_n_e_a_r │ │ │ │ │ + * _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00641_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01214_source.html │ │ │ │┄ Files 91% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Factor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/SfmTrack.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Factor.h
│ │ │ │ +
SfmTrack.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20// \callgraph
│ │ │ │ -
21
│ │ │ │ -
22#pragma once
│ │ │ │ -
23
│ │ │ │ -
24#include <boost/serialization/nvp.hpp>
│ │ │ │ -
25#include <boost/shared_ptr.hpp>
│ │ │ │ -
26
│ │ │ │ -
27#include <gtsam/base/types.h>
│ │ │ │ -
28#include <gtsam/base/FastVector.h>
│ │ │ │ -
29#include <gtsam/inference/Key.h>
│ │ │ │ -
30
│ │ │ │ -
31namespace gtsam {
│ │ │ │ -
32
│ │ │ │ -
34 typedef FastVector<FactorIndex> FactorIndices;
│ │ │ │ -
35 typedef FastSet<FactorIndex> FactorIndexSet;
│ │ │ │ -
36
│ │ │ │ -
37 class HybridValues; // forward declaration of a Value type for error.
│ │ │ │ -
38
│ │ │ │ -
│ │ │ │ -
67 class GTSAM_EXPORT Factor
│ │ │ │ -
68 {
│ │ │ │ -
69
│ │ │ │ -
70 private:
│ │ │ │ -
71 // These typedefs are private because they must be overridden in derived classes.
│ │ │ │ -
72 typedef Factor This;
│ │ │ │ -
73 typedef boost::shared_ptr<Factor> shared_ptr;
│ │ │ │ -
74
│ │ │ │ -
75 public:
│ │ │ │ -
77 typedef KeyVector::iterator iterator;
│ │ │ │ -
78
│ │ │ │ -
80 typedef KeyVector::const_iterator const_iterator;
│ │ │ │ -
81
│ │ │ │ -
82 protected:
│ │ │ │ -
83
│ │ │ │ -
85 KeyVector keys_;
│ │ │ │ -
86
│ │ │ │ -
89
│ │ │ │ -
91 Factor() {}
│ │ │ │ -
92
│ │ │ │ -
95 template<typename CONTAINER>
│ │ │ │ -
96 explicit Factor(const CONTAINER& keys) : keys_(keys.begin(), keys.end()) {}
│ │ │ │ -
97
│ │ │ │ -
100 template<typename ITERATOR>
│ │ │ │ -
101 Factor(ITERATOR first, ITERATOR last) : keys_(first, last) {}
│ │ │ │ -
102
│ │ │ │ -
105 template<typename CONTAINER>
│ │ │ │ -
│ │ │ │ -
106 static Factor FromKeys(const CONTAINER& keys) {
│ │ │ │ -
107 return Factor(keys.begin(), keys.end()); }
│ │ │ │ -
│ │ │ │ -
108
│ │ │ │ -
111 template<typename ITERATOR>
│ │ │ │ -
│ │ │ │ -
112 static Factor FromIterators(ITERATOR first, ITERATOR last) {
│ │ │ │ -
113 return Factor(first, last); }
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/base/serialization.h>
│ │ │ │ +
22#include <gtsam/geometry/Point2.h>
│ │ │ │ +
23#include <gtsam/geometry/Point3.h>
│ │ │ │ +
24
│ │ │ │ +
25#include <Eigen/Core>
│ │ │ │ +
26#include <string>
│ │ │ │ +
27#include <utility>
│ │ │ │ +
28#include <vector>
│ │ │ │ +
29
│ │ │ │ +
30namespace gtsam {
│ │ │ │ +
31
│ │ │ │ +
33typedef std::pair<size_t, Point2> SfmMeasurement;
│ │ │ │ +
34
│ │ │ │ +
36typedef std::pair<size_t, size_t> SiftIndex;
│ │ │ │ +
37
│ │ │ │ +
│ │ │ │ +
43struct GTSAM_EXPORT SfmTrack2d {
│ │ │ │ +
45 std::vector<SfmMeasurement> measurements;
│ │ │ │ +
46
│ │ │ │ +
48 std::vector<SiftIndex> siftIndices;
│ │ │ │ +
49
│ │ │ │ +
52
│ │ │ │ +
53 // Default constructor.
│ │ │ │ +
54 SfmTrack2d() = default;
│ │ │ │ +
55
│ │ │ │ +
56 // Constructor from measurements.
│ │ │ │ +
57 explicit SfmTrack2d(const std::vector<SfmMeasurement>& measurements)
│ │ │ │ +
58 : measurements(measurements) {}
│ │ │ │ +
59
│ │ │ │ +
63
│ │ │ │ +
│ │ │ │ +
65 void addMeasurement(size_t idx, const gtsam::Point2& m) {
│ │ │ │ +
66 measurements.emplace_back(idx, m);
│ │ │ │ +
67 }
│ │ │ │ +
│ │ │ │ +
68
│ │ │ │ +
70 size_t numberMeasurements() const { return measurements.size(); }
│ │ │ │ +
71
│ │ │ │ +
│ │ │ │ +
73 const SfmMeasurement& measurement(size_t idx) const {
│ │ │ │ +
74 return measurements[idx];
│ │ │ │ +
75 }
│ │ │ │ +
│ │ │ │ +
76
│ │ │ │ +
78 const SiftIndex& siftIndex(size_t idx) const { return siftIndices[idx]; }
│ │ │ │ +
79
│ │ │ │ +
│ │ │ │ +
84 bool hasUniqueCameras() const {
│ │ │ │ +
85 std::vector<int> track_cam_indices;
│ │ │ │ +
86 for (auto& measurement : measurements) {
│ │ │ │ +
87 track_cam_indices.emplace_back(measurement.first);
│ │ │ │ +
88 }
│ │ │ │ +
89 auto i =
│ │ │ │ +
90 std::adjacent_find(track_cam_indices.begin(), track_cam_indices.end());
│ │ │ │ +
91 bool all_cameras_unique = (i == track_cam_indices.end());
│ │ │ │ +
92 return all_cameras_unique;
│ │ │ │ +
93 }
│ │ │ │ +
│ │ │ │ +
94
│ │ │ │ +
98
│ │ │ │ +
│ │ │ │ +
100 Eigen::MatrixX2d measurementMatrix() const {
│ │ │ │ +
101 Eigen::MatrixX2d m(numberMeasurements(), 2);
│ │ │ │ +
102 for (size_t i = 0; i < numberMeasurements(); i++) {
│ │ │ │ +
103 m.row(i) = measurement(i).second;
│ │ │ │ +
104 }
│ │ │ │ +
105 return m;
│ │ │ │ +
106 }
│ │ │ │ +
│ │ │ │ +
107
│ │ │ │ +
│ │ │ │ +
109 Eigen::VectorXi indexVector() const {
│ │ │ │ +
110 Eigen::VectorXi v(numberMeasurements());
│ │ │ │ +
111 for (size_t i = 0; i < numberMeasurements(); i++) {
│ │ │ │ +
112 v(i) = measurement(i).first;
│ │ │ │ +
113 }
│ │ │ │ +
114 return v;
│ │ │ │ +
115 }
│ │ │ │
│ │ │ │ -
114
│ │ │ │
116
│ │ │ │ -
117 public:
│ │ │ │ -
119 // public since it is required for boost serialization and static methods.
│ │ │ │ -
120 // virtual since it is public.
│ │ │ │ -
121 // http://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#Rc-dtor-virtual
│ │ │ │ -
122 virtual ~Factor() = default;
│ │ │ │ -
123
│ │ │ │ -
126
│ │ │ │ -
128 bool empty() const { return keys_.empty(); }
│ │ │ │ +
118};
│ │ │ │ +
│ │ │ │ +
119
│ │ │ │ +
120using SfmTrack2dVector = std::vector<SfmTrack2d>;
│ │ │ │ +
121
│ │ │ │ +
│ │ │ │ +
126struct GTSAM_EXPORT SfmTrack : SfmTrack2d {
│ │ │ │ +
127 Point3 p;
│ │ │ │ +
128 float r, g, b;
│ │ │ │
129
│ │ │ │ -
131 Key front() const { return keys_.front(); }
│ │ │ │
132
│ │ │ │ -
134 Key back() const { return keys_.back(); }
│ │ │ │ +
133 explicit SfmTrack(float r = 0, float g = 0, float b = 0)
│ │ │ │ +
134 : p(0, 0, 0), r(r), g(g), b(b) {}
│ │ │ │
135
│ │ │ │ -
137 const_iterator find(Key key) const { return std::find(begin(), end(), key); }
│ │ │ │ -
138
│ │ │ │ -
140 const KeyVector& keys() const { return keys_; }
│ │ │ │ -
141
│ │ │ │ -
143 const_iterator begin() const { return keys_.begin(); }
│ │ │ │ -
144
│ │ │ │ -
146 const_iterator end() const { return keys_.end(); }
│ │ │ │ -
147
│ │ │ │ -
152 virtual double error(const HybridValues& c) const;
│ │ │ │ +
136 explicit SfmTrack(const gtsam::Point3& pt, float r = 0, float g = 0,
│ │ │ │ +
137 float b = 0)
│ │ │ │ +
138 : p(pt), r(r), g(g), b(b) {}
│ │ │ │ +
139
│ │ │ │ +
143
│ │ │ │ +
145 const Point3& point3() const { return p; }
│ │ │ │ +
146
│ │ │ │ +
148 Point3 rgb() const { return Point3(r, g, b); }
│ │ │ │ +
149
│ │ │ │
153
│ │ │ │ -
157 size_t size() const { return keys_.size(); }
│ │ │ │ -
158
│ │ │ │ -
160
│ │ │ │ -
163
│ │ │ │ -
165 virtual void print(
│ │ │ │ -
166 const std::string& s = "Factor",
│ │ │ │ -
167 const KeyFormatter& formatter = DefaultKeyFormatter) const;
│ │ │ │ -
168
│ │ │ │ -
170 virtual void printKeys(
│ │ │ │ -
171 const std::string& s = "Factor",
│ │ │ │ -
172 const KeyFormatter& formatter = DefaultKeyFormatter) const;
│ │ │ │ -
173
│ │ │ │ -
175 bool equals(const This& other, double tol = 1e-9) const;
│ │ │ │ -
176
│ │ │ │ -
180
│ │ │ │ -
182 KeyVector& keys() { return keys_; }
│ │ │ │ -
183
│ │ │ │ -
185 iterator begin() { return keys_.begin(); }
│ │ │ │ -
186
│ │ │ │ -
188 iterator end() { return keys_.end(); }
│ │ │ │ +
155 void print(const std::string& s = "") const;
│ │ │ │ +
156
│ │ │ │ +
158 bool equals(const SfmTrack& sfmTrack, double tol = 1e-9) const;
│ │ │ │ +
159
│ │ │ │ +
161#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ +
164 void GTSAM_DEPRECATED add_measurement(size_t idx, const gtsam::Point2& m) {
│ │ │ │ +
165 measurements.emplace_back(idx, m);
│ │ │ │ +
166 }
│ │ │ │ +
167
│ │ │ │ +
168 size_t GTSAM_DEPRECATED number_measurements() const {
│ │ │ │ +
169 return measurements.size();
│ │ │ │ +
170 }
│ │ │ │ +
172#endif
│ │ │ │ +
175
│ │ │ │ +
177 friend class boost::serialization::access;
│ │ │ │ +
178 template <class ARCHIVE>
│ │ │ │ +
179 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ +
180 ar& BOOST_SERIALIZATION_NVP(p);
│ │ │ │ +
181 ar& BOOST_SERIALIZATION_NVP(r);
│ │ │ │ +
182 ar& BOOST_SERIALIZATION_NVP(g);
│ │ │ │ +
183 ar& BOOST_SERIALIZATION_NVP(b);
│ │ │ │ +
184 ar& BOOST_SERIALIZATION_NVP(measurements);
│ │ │ │ +
185 ar& BOOST_SERIALIZATION_NVP(siftIndices);
│ │ │ │ +
186 }
│ │ │ │ +
188};
│ │ │ │ +
│ │ │ │
189
│ │ │ │ -
191
│ │ │ │ -
192 private:
│ │ │ │ -
193
│ │ │ │ -
196
│ │ │ │ -
198 friend class boost::serialization::access;
│ │ │ │ -
199 template<class Archive>
│ │ │ │ -
200 void serialize(Archive & ar, const unsigned int /*version*/) {
│ │ │ │ -
201 ar & BOOST_SERIALIZATION_NVP(keys_);
│ │ │ │ -
202 }
│ │ │ │ -
203
│ │ │ │ -
205
│ │ │ │ -
206 };
│ │ │ │ -
│ │ │ │ -
207
│ │ │ │ -
208} // \namespace gtsam
│ │ │ │ -
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ -
types.h
Typedefs for easier changing of types.
│ │ │ │ - │ │ │ │ -
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │ +
190template <typename T>
│ │ │ │ +
191struct traits;
│ │ │ │ +
192
│ │ │ │ +
193template <>
│ │ │ │ +
194struct traits<SfmTrack> : public Testable<SfmTrack> {};
│ │ │ │ +
195
│ │ │ │ +
196} // namespace gtsam
│ │ │ │ +
serialization.h
Convenience functions for serializing data structures via boost.serialization.
│ │ │ │ +
Point2.h
2D Point
│ │ │ │ +
Point3.h
3D Point
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::SiftIndex
std::pair< size_t, size_t > SiftIndex
Sift index for SfmTrack.
Definition SfmTrack.h:36
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition Factor.h:34
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::FastSet
FastSet is a thin wrapper around std::set that uses the boost fast_pool_allocator instead of the defa...
Definition FastSet.h:50
│ │ │ │ +
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │ +
gtsam::SfmMeasurement
std::pair< size_t, Point2 > SfmMeasurement
A measurement with its camera index.
Definition SfmTrack.h:33
│ │ │ │ +
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ -
gtsam::Factor
Definition Factor.h:68
│ │ │ │ -
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
│ │ │ │ -
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
│ │ │ │ -
gtsam::Factor::find
const_iterator find(Key key) const
find
Definition Factor.h:137
│ │ │ │ -
gtsam::Factor::begin
const_iterator begin() const
Iterator at beginning of involved variable keys.
Definition Factor.h:143
│ │ │ │ -
gtsam::Factor::Factor
Factor()
Default constructor for I/O.
Definition Factor.h:91
│ │ │ │ -
gtsam::Factor::end
iterator end()
Iterator at end of involved variable keys.
Definition Factor.h:188
│ │ │ │ -
gtsam::Factor::empty
bool empty() const
Whether the factor is empty (involves zero variables).
Definition Factor.h:128
│ │ │ │ -
gtsam::Factor::keys
KeyVector & keys()
Definition Factor.h:182
│ │ │ │ -
gtsam::Factor::FromIterators
static Factor FromIterators(ITERATOR first, ITERATOR last)
Construct factor from iterator keys.
Definition Factor.h:112
│ │ │ │ -
gtsam::Factor::FromKeys
static Factor FromKeys(const CONTAINER &keys)
Construct factor from container of keys.
Definition Factor.h:106
│ │ │ │ -
gtsam::Factor::begin
iterator begin()
Iterator at beginning of involved variable keys.
Definition Factor.h:185
│ │ │ │ -
gtsam::Factor::~Factor
virtual ~Factor()=default
Default destructor.
│ │ │ │ -
gtsam::Factor::Factor
Factor(ITERATOR first, ITERATOR last)
Construct factor from iterator keys.
Definition Factor.h:101
│ │ │ │ -
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition Factor.h:80
│ │ │ │ -
gtsam::Factor::end
const_iterator end() const
Iterator at end of involved variable keys.
Definition Factor.h:146
│ │ │ │ -
gtsam::Factor::Factor
Factor(const CONTAINER &keys)
Construct factor from container of keys.
Definition Factor.h:96
│ │ │ │ -
gtsam::Factor::iterator
KeyVector::iterator iterator
Iterator over keys.
Definition Factor.h:77
│ │ │ │ -
gtsam::Factor::back
Key back() const
Last key.
Definition Factor.h:134
│ │ │ │ -
gtsam::Factor::front
Key front() const
First key.
Definition Factor.h:131
│ │ │ │ -
gtsam::Factor::size
size_t size() const
Definition Factor.h:157
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::SfmTrack2d
Track containing 2D measurements associated with a single 3D point.
Definition SfmTrack.h:43
│ │ │ │ +
gtsam::SfmTrack2d::addMeasurement
void addMeasurement(size_t idx, const gtsam::Point2 &m)
Add measurement (camera_idx, Point2) to track.
Definition SfmTrack.h:65
│ │ │ │ +
gtsam::SfmTrack2d::measurement
const SfmMeasurement & measurement(size_t idx) const
Get the measurement (camera index, Point2) at pose index idx
Definition SfmTrack.h:73
│ │ │ │ +
gtsam::SfmTrack2d::siftIndices
std::vector< SiftIndex > siftIndices
The feature descriptors (optional)
Definition SfmTrack.h:48
│ │ │ │ +
gtsam::SfmTrack2d::siftIndex
const SiftIndex & siftIndex(size_t idx) const
Get the SIFT feature index corresponding to the measurement at idx
Definition SfmTrack.h:78
│ │ │ │ +
gtsam::SfmTrack2d::hasUniqueCameras
bool hasUniqueCameras() const
Check that no two measurements are from the same camera.
Definition SfmTrack.h:84
│ │ │ │ +
gtsam::SfmTrack2d::measurementMatrix
Eigen::MatrixX2d measurementMatrix() const
Return the measurements as a 2D matrix.
Definition SfmTrack.h:100
│ │ │ │ +
gtsam::SfmTrack2d::numberMeasurements
size_t numberMeasurements() const
Total number of measurements in this track.
Definition SfmTrack.h:70
│ │ │ │ +
gtsam::SfmTrack2d::measurements
std::vector< SfmMeasurement > measurements
The 2D image projections (id,(u,v))
Definition SfmTrack.h:45
│ │ │ │ +
gtsam::SfmTrack2d::indexVector
Eigen::VectorXi indexVector() const
Return the camera indices of the measurements.
Definition SfmTrack.h:109
│ │ │ │ +
gtsam::SfmTrack
Definition SfmTrack.h:126
│ │ │ │ +
gtsam::SfmTrack::rgb
Point3 rgb() const
Get RGB values describing 3d point.
Definition SfmTrack.h:148
│ │ │ │ +
gtsam::SfmTrack::b
float b
RGB color of the 3D point.
Definition SfmTrack.h:128
│ │ │ │ +
gtsam::SfmTrack::point3
const Point3 & point3() const
Get 3D point.
Definition SfmTrack.h:145
│ │ │ │ +
gtsam::SfmTrack::p
Point3 p
3D position of the point
Definition SfmTrack.h:127
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,260 +1,248 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Factor.h │ │ │ │ │ +SfmTrack.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20// \callgraph │ │ │ │ │ -21 │ │ │ │ │ -22#pragma once │ │ │ │ │ -23 │ │ │ │ │ -24#include │ │ │ │ │ -25#include │ │ │ │ │ -26 │ │ │ │ │ -27#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ -28#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ -29#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ -30 │ │ │ │ │ -31namespace _g_t_s_a_m { │ │ │ │ │ -32 │ │ │ │ │ -_3_4 typedef _F_a_s_t_V_e_c_t_o_r_<_F_a_c_t_o_r_I_n_d_e_x_> _F_a_c_t_o_r_I_n_d_i_c_e_s; │ │ │ │ │ -35 typedef _F_a_s_t_S_e_t_<_F_a_c_t_o_r_I_n_d_e_x_> FactorIndexSet; │ │ │ │ │ -36 │ │ │ │ │ -37 class _H_y_b_r_i_d_V_a_l_u_e_s; // forward declaration of a Value type for error. │ │ │ │ │ -38 │ │ │ │ │ -_6_7 class GTSAM_EXPORT _F_a_c_t_o_r │ │ │ │ │ -68 { │ │ │ │ │ -69 │ │ │ │ │ -70 private: │ │ │ │ │ -71 // These typedefs are private because they must be overridden in derived │ │ │ │ │ -classes. │ │ │ │ │ -72 typedef _F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ -73 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -74 │ │ │ │ │ -75 public: │ │ │ │ │ -_7_7 typedef KeyVector::iterator _i_t_e_r_a_t_o_r; │ │ │ │ │ -78 │ │ │ │ │ -_8_0 typedef KeyVector::const_iterator _c_o_n_s_t___i_t_e_r_a_t_o_r; │ │ │ │ │ -81 │ │ │ │ │ -82 protected: │ │ │ │ │ -83 │ │ │ │ │ -_8_5 _K_e_y_V_e_c_t_o_r _k_e_y_s__; │ │ │ │ │ -86 │ │ │ │ │ -89 │ │ │ │ │ -_9_1 _F_a_c_t_o_r() {} │ │ │ │ │ -92 │ │ │ │ │ -95 template │ │ │ │ │ -_9_6 explicit _F_a_c_t_o_r(const CONTAINER& keys) : keys_(keys.begin(), keys.end()) {} │ │ │ │ │ -97 │ │ │ │ │ -100 template │ │ │ │ │ -_1_0_1 _F_a_c_t_o_r(ITERATOR first, ITERATOR last) : keys_(first, last) {} │ │ │ │ │ -102 │ │ │ │ │ -105 template │ │ │ │ │ -_1_0_6 static _F_a_c_t_o_r _F_r_o_m_K_e_y_s(const CONTAINER& keys) { │ │ │ │ │ -107 return _F_a_c_t_o_r(keys.begin(), keys.end()); } │ │ │ │ │ -108 │ │ │ │ │ -111 template │ │ │ │ │ -_1_1_2 static _F_a_c_t_o_r _F_r_o_m_I_t_e_r_a_t_o_r_s(ITERATOR first, ITERATOR last) { │ │ │ │ │ -113 return _F_a_c_t_o_r(first, last); } │ │ │ │ │ -114 │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_b_a_s_e_/_s_e_r_i_a_l_i_z_a_t_i_o_n_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_3_._h> │ │ │ │ │ +24 │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28#include │ │ │ │ │ +29 │ │ │ │ │ +30namespace _g_t_s_a_m { │ │ │ │ │ +31 │ │ │ │ │ +_3_3typedef std::pair _S_f_m_M_e_a_s_u_r_e_m_e_n_t; │ │ │ │ │ +34 │ │ │ │ │ +_3_6typedef std::pair _S_i_f_t_I_n_d_e_x; │ │ │ │ │ +37 │ │ │ │ │ +_4_3struct GTSAM_EXPORT _S_f_m_T_r_a_c_k_2_d { │ │ │ │ │ +_4_5 std::vector _m_e_a_s_u_r_e_m_e_n_t_s; │ │ │ │ │ +46 │ │ │ │ │ +_4_8 std::vector _s_i_f_t_I_n_d_i_c_e_s; │ │ │ │ │ +49 │ │ │ │ │ +52 │ │ │ │ │ +53 // Default constructor. │ │ │ │ │ +54 _S_f_m_T_r_a_c_k_2_d() = default; │ │ │ │ │ +55 │ │ │ │ │ +56 // Constructor from measurements. │ │ │ │ │ +57 explicit _S_f_m_T_r_a_c_k_2_d(const std::vector& measurements) │ │ │ │ │ +58 : measurements(measurements) {} │ │ │ │ │ +59 │ │ │ │ │ +63 │ │ │ │ │ +_6_5 void _a_d_d_M_e_a_s_u_r_e_m_e_n_t(size_t idx, const _g_t_s_a_m_:_:_P_o_i_n_t_2& m) { │ │ │ │ │ +66 measurements.emplace_back(idx, m); │ │ │ │ │ +67 } │ │ │ │ │ +68 │ │ │ │ │ +_7_0 size_t _n_u_m_b_e_r_M_e_a_s_u_r_e_m_e_n_t_s() const { return measurements.size(); } │ │ │ │ │ +71 │ │ │ │ │ +_7_3 const _S_f_m_M_e_a_s_u_r_e_m_e_n_t& _m_e_a_s_u_r_e_m_e_n_t(size_t idx) const { │ │ │ │ │ +74 return measurements[idx]; │ │ │ │ │ +75 } │ │ │ │ │ +76 │ │ │ │ │ +_7_8 const _S_i_f_t_I_n_d_e_x& _s_i_f_t_I_n_d_e_x(size_t idx) const { return siftIndices[idx]; } │ │ │ │ │ +79 │ │ │ │ │ +_8_4 bool _h_a_s_U_n_i_q_u_e_C_a_m_e_r_a_s() const { │ │ │ │ │ +85 std::vector track_cam_indices; │ │ │ │ │ +86 for (auto& measurement : measurements) { │ │ │ │ │ +87 track_cam_indices.emplace_back(measurement.first); │ │ │ │ │ +88 } │ │ │ │ │ +89 auto i = │ │ │ │ │ +90 std::adjacent_find(track_cam_indices.begin(), track_cam_indices.end()); │ │ │ │ │ +91 bool all_cameras_unique = (i == track_cam_indices.end()); │ │ │ │ │ +92 return all_cameras_unique; │ │ │ │ │ +93 } │ │ │ │ │ +94 │ │ │ │ │ +98 │ │ │ │ │ +_1_0_0 Eigen::MatrixX2d _m_e_a_s_u_r_e_m_e_n_t_M_a_t_r_i_x() const { │ │ │ │ │ +101 Eigen::MatrixX2d m(numberMeasurements(), 2); │ │ │ │ │ +102 for (size_t i = 0; i < numberMeasurements(); i++) { │ │ │ │ │ +103 m.row(i) = measurement(i).second; │ │ │ │ │ +104 } │ │ │ │ │ +105 return m; │ │ │ │ │ +106 } │ │ │ │ │ +107 │ │ │ │ │ +_1_0_9 Eigen::VectorXi _i_n_d_e_x_V_e_c_t_o_r() const { │ │ │ │ │ +110 Eigen::VectorXi v(numberMeasurements()); │ │ │ │ │ +111 for (size_t i = 0; i < numberMeasurements(); i++) { │ │ │ │ │ +112 v(i) = measurement(i).first; │ │ │ │ │ +113 } │ │ │ │ │ +114 return v; │ │ │ │ │ +115 } │ │ │ │ │ 116 │ │ │ │ │ -117 public: │ │ │ │ │ -119 // public since it is required for boost serialization and static methods. │ │ │ │ │ -120 // virtual since it is public. │ │ │ │ │ -121 // http://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#Rc-dtor- │ │ │ │ │ -virtual │ │ │ │ │ -_1_2_2 virtual _~_F_a_c_t_o_r() = default; │ │ │ │ │ -123 │ │ │ │ │ -126 │ │ │ │ │ -_1_2_8 bool _e_m_p_t_y() const { return keys_.empty(); } │ │ │ │ │ +118}; │ │ │ │ │ +119 │ │ │ │ │ +120using SfmTrack2dVector = std::vector; │ │ │ │ │ +121 │ │ │ │ │ +_1_2_6struct GTSAM_EXPORT _S_f_m_T_r_a_c_k : _S_f_m_T_r_a_c_k_2_d { │ │ │ │ │ +_1_2_7 _P_o_i_n_t_3 _p; │ │ │ │ │ +_1_2_8 float r, g, _b; │ │ │ │ │ 129 │ │ │ │ │ -_1_3_1 _K_e_y _f_r_o_n_t() const { return keys_.front(); } │ │ │ │ │ 132 │ │ │ │ │ -_1_3_4 _K_e_y _b_a_c_k() const { return keys_.back(); } │ │ │ │ │ +133 explicit _S_f_m_T_r_a_c_k(float r = 0, float g = 0, float b = 0) │ │ │ │ │ +134 : p(0, 0, 0), r(r), g(g), b(b) {} │ │ │ │ │ 135 │ │ │ │ │ -_1_3_7 _c_o_n_s_t___i_t_e_r_a_t_o_r _f_i_n_d(_K_e_y key) const { return std::find(begin(), end(), key); │ │ │ │ │ -} │ │ │ │ │ -138 │ │ │ │ │ -_1_4_0 const _K_e_y_V_e_c_t_o_r& _k_e_y_s() const { return keys_; } │ │ │ │ │ -141 │ │ │ │ │ -_1_4_3 _c_o_n_s_t___i_t_e_r_a_t_o_r _b_e_g_i_n() const { return keys_.begin(); } │ │ │ │ │ -144 │ │ │ │ │ -_1_4_6 _c_o_n_s_t___i_t_e_r_a_t_o_r _e_n_d() const { return keys_.end(); } │ │ │ │ │ -147 │ │ │ │ │ -152 virtual double error(const _H_y_b_r_i_d_V_a_l_u_e_s& c) const; │ │ │ │ │ +136 explicit _S_f_m_T_r_a_c_k(const _g_t_s_a_m_:_:_P_o_i_n_t_3& pt, float r = 0, float g = 0, │ │ │ │ │ +137 float b = 0) │ │ │ │ │ +138 : p(pt), r(r), g(g), b(b) {} │ │ │ │ │ +139 │ │ │ │ │ +143 │ │ │ │ │ +_1_4_5 const _P_o_i_n_t_3& _p_o_i_n_t_3() const { return p; } │ │ │ │ │ +146 │ │ │ │ │ +_1_4_8 _P_o_i_n_t_3 _r_g_b() const { return _P_o_i_n_t_3(r, g, b); } │ │ │ │ │ +149 │ │ │ │ │ 153 │ │ │ │ │ -_1_5_7 size_t _s_i_z_e() const { return keys_.size(); } │ │ │ │ │ -158 │ │ │ │ │ -160 │ │ │ │ │ -163 │ │ │ │ │ -165 virtual void _p_r_i_n_t( │ │ │ │ │ -166 const std::string& s = "Factor", │ │ │ │ │ -167 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const; │ │ │ │ │ -168 │ │ │ │ │ -170 virtual void printKeys( │ │ │ │ │ -171 const std::string& s = "Factor", │ │ │ │ │ -172 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const; │ │ │ │ │ -173 │ │ │ │ │ -175 bool _e_q_u_a_l_s(const This& other, double tol = 1e-9) const; │ │ │ │ │ -176 │ │ │ │ │ -180 │ │ │ │ │ -_1_8_2 _K_e_y_V_e_c_t_o_r& _k_e_y_s() { return keys_; } │ │ │ │ │ -183 │ │ │ │ │ -_1_8_5 _i_t_e_r_a_t_o_r _b_e_g_i_n() { return keys_.begin(); } │ │ │ │ │ -186 │ │ │ │ │ -_1_8_8 _i_t_e_r_a_t_o_r _e_n_d() { return keys_.end(); } │ │ │ │ │ +155 void _p_r_i_n_t(const std::string& s = "") const; │ │ │ │ │ +156 │ │ │ │ │ +158 bool _e_q_u_a_l_s(const _S_f_m_T_r_a_c_k& sfmTrack, double tol = 1e-9) const; │ │ │ │ │ +159 │ │ │ │ │ +161#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ +164 void GTSAM_DEPRECATED add_measurement(size_t idx, const _g_t_s_a_m_:_:_P_o_i_n_t_2& m) { │ │ │ │ │ +165 measurements.emplace_back(idx, m); │ │ │ │ │ +166 } │ │ │ │ │ +167 │ │ │ │ │ +168 size_t GTSAM_DEPRECATED number_measurements() const { │ │ │ │ │ +169 return measurements.size(); │ │ │ │ │ +170 } │ │ │ │ │ +172#endif │ │ │ │ │ +175 │ │ │ │ │ +_1_7_7 friend class boost::serialization::access; │ │ │ │ │ +178 template │ │ │ │ │ +179 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ +180 ar& BOOST_SERIALIZATION_NVP(p); │ │ │ │ │ +181 ar& BOOST_SERIALIZATION_NVP(r); │ │ │ │ │ +182 ar& BOOST_SERIALIZATION_NVP(g); │ │ │ │ │ +183 ar& BOOST_SERIALIZATION_NVP(b); │ │ │ │ │ +184 ar& BOOST_SERIALIZATION_NVP(measurements); │ │ │ │ │ +185 ar& BOOST_SERIALIZATION_NVP(siftIndices); │ │ │ │ │ +186 } │ │ │ │ │ +188}; │ │ │ │ │ 189 │ │ │ │ │ -191 │ │ │ │ │ -192 private: │ │ │ │ │ -193 │ │ │ │ │ -196 │ │ │ │ │ -_1_9_8 friend class boost::serialization::access; │ │ │ │ │ -199 template │ │ │ │ │ -200 void serialize(Archive & ar, const unsigned int /*version*/) { │ │ │ │ │ -201 ar & BOOST_SERIALIZATION_NVP(keys_); │ │ │ │ │ -202 } │ │ │ │ │ -203 │ │ │ │ │ -205 │ │ │ │ │ -206 }; │ │ │ │ │ -207 │ │ │ │ │ -208} // \namespace gtsam │ │ │ │ │ -_F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ -A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ -_t_y_p_e_s_._h │ │ │ │ │ -Typedefs for easier changing of types. │ │ │ │ │ -_K_e_y_._h │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ -std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ -FastVector │ │ │ │ │ -FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ -DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ +190template │ │ │ │ │ +191struct traits; │ │ │ │ │ +192 │ │ │ │ │ +193template <> │ │ │ │ │ +_1_9_4struct _t_r_a_i_t_s<_S_f_m_T_r_a_c_k> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ +195 │ │ │ │ │ +196} // namespace gtsam │ │ │ │ │ +_s_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ +Convenience functions for serializing data structures via boost.serialization. │ │ │ │ │ +_P_o_i_n_t_2_._h │ │ │ │ │ +2D Point │ │ │ │ │ +_P_o_i_n_t_3_._h │ │ │ │ │ +3D Point │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_S_i_f_t_I_n_d_e_x │ │ │ │ │ +std::pair< size_t, size_t > SiftIndex │ │ │ │ │ +Sift index for SfmTrack. │ │ │ │ │ +DDeeffiinniittiioonn SfmTrack.h:36 │ │ │ │ │ _g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ print without optional string, must specify cout yourself │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_I_n_d_i_c_e_s │ │ │ │ │ -FastVector< FactorIndex > FactorIndices │ │ │ │ │ -Define collection types: │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_S_e_t │ │ │ │ │ -FastSet is a thin wrapper around std::set that uses the boost │ │ │ │ │ -fast_pool_allocator instead of the defa... │ │ │ │ │ -DDeeffiinniittiioonn FastSet.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ +Vector2 Point2 │ │ │ │ │ +As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ +typedef Point2 to Vector2... │ │ │ │ │ +DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ +_g_t_s_a_m_:_:_S_f_m_M_e_a_s_u_r_e_m_e_n_t │ │ │ │ │ +std::pair< size_t, Point2 > SfmMeasurement │ │ │ │ │ +A measurement with its camera index. │ │ │ │ │ +DDeeffiinniittiioonn SfmTrack.h:33 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ +Vector3 Point3 │ │ │ │ │ +As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ +typedef Point3 to Vector3... │ │ │ │ │ +DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ _g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ Template to create a binary predicate. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ -HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ -DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:68 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s │ │ │ │ │ -const KeyVector & keys() const │ │ │ │ │ -Access the factor's involved variable keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:140 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s__ │ │ │ │ │ -KeyVector keys_ │ │ │ │ │ -The keys involved in this factor. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_f_i_n_d │ │ │ │ │ -const_iterator find(Key key) const │ │ │ │ │ -find │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:137 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_b_e_g_i_n │ │ │ │ │ -const_iterator begin() const │ │ │ │ │ -Iterator at beginning of involved variable keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:143 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_F_a_c_t_o_r │ │ │ │ │ -Factor() │ │ │ │ │ -Default constructor for I/O. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:91 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_n_d │ │ │ │ │ -iterator end() │ │ │ │ │ -Iterator at end of involved variable keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:188 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_m_p_t_y │ │ │ │ │ -bool empty() const │ │ │ │ │ -Whether the factor is empty (involves zero variables). │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:128 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s │ │ │ │ │ -KeyVector & keys() │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:182 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_F_r_o_m_I_t_e_r_a_t_o_r_s │ │ │ │ │ -static Factor FromIterators(ITERATOR first, ITERATOR last) │ │ │ │ │ -Construct factor from iterator keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:112 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_F_r_o_m_K_e_y_s │ │ │ │ │ -static Factor FromKeys(const CONTAINER &keys) │ │ │ │ │ -Construct factor from container of keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:106 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_b_e_g_i_n │ │ │ │ │ -iterator begin() │ │ │ │ │ -Iterator at beginning of involved variable keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:185 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_~_F_a_c_t_o_r │ │ │ │ │ -virtual ~Factor()=default │ │ │ │ │ -Default destructor. │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_F_a_c_t_o_r │ │ │ │ │ -Factor(ITERATOR first, ITERATOR last) │ │ │ │ │ -Construct factor from iterator keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:101 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_c_o_n_s_t___i_t_e_r_a_t_o_r │ │ │ │ │ -KeyVector::const_iterator const_iterator │ │ │ │ │ -Const iterator over keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:80 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_n_d │ │ │ │ │ -const_iterator end() const │ │ │ │ │ -Iterator at end of involved variable keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:146 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_F_a_c_t_o_r │ │ │ │ │ -Factor(const CONTAINER &keys) │ │ │ │ │ -Construct factor from container of keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:96 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_i_t_e_r_a_t_o_r │ │ │ │ │ -KeyVector::iterator iterator │ │ │ │ │ -Iterator over keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_b_a_c_k │ │ │ │ │ -Key back() const │ │ │ │ │ -Last key. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:134 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_f_r_o_n_t │ │ │ │ │ -Key front() const │ │ │ │ │ -First key. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:131 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_s_i_z_e │ │ │ │ │ -size_t size() const │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:157 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_2_d │ │ │ │ │ +Track containing 2D measurements associated with a single 3D point. │ │ │ │ │ +DDeeffiinniittiioonn SfmTrack.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_2_d_:_:_a_d_d_M_e_a_s_u_r_e_m_e_n_t │ │ │ │ │ +void addMeasurement(size_t idx, const gtsam::Point2 &m) │ │ │ │ │ +Add measurement (camera_idx, Point2) to track. │ │ │ │ │ +DDeeffiinniittiioonn SfmTrack.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_2_d_:_:_m_e_a_s_u_r_e_m_e_n_t │ │ │ │ │ +const SfmMeasurement & measurement(size_t idx) const │ │ │ │ │ +Get the measurement (camera index, Point2) at pose index idx │ │ │ │ │ +DDeeffiinniittiioonn SfmTrack.h:73 │ │ │ │ │ +_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_2_d_:_:_s_i_f_t_I_n_d_i_c_e_s │ │ │ │ │ +std::vector< SiftIndex > siftIndices │ │ │ │ │ +The feature descriptors (optional) │ │ │ │ │ +DDeeffiinniittiioonn SfmTrack.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_2_d_:_:_s_i_f_t_I_n_d_e_x │ │ │ │ │ +const SiftIndex & siftIndex(size_t idx) const │ │ │ │ │ +Get the SIFT feature index corresponding to the measurement at idx │ │ │ │ │ +DDeeffiinniittiioonn SfmTrack.h:78 │ │ │ │ │ +_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_2_d_:_:_h_a_s_U_n_i_q_u_e_C_a_m_e_r_a_s │ │ │ │ │ +bool hasUniqueCameras() const │ │ │ │ │ +Check that no two measurements are from the same camera. │ │ │ │ │ +DDeeffiinniittiioonn SfmTrack.h:84 │ │ │ │ │ +_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_2_d_:_:_m_e_a_s_u_r_e_m_e_n_t_M_a_t_r_i_x │ │ │ │ │ +Eigen::MatrixX2d measurementMatrix() const │ │ │ │ │ +Return the measurements as a 2D matrix. │ │ │ │ │ +DDeeffiinniittiioonn SfmTrack.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_2_d_:_:_n_u_m_b_e_r_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ +size_t numberMeasurements() const │ │ │ │ │ +Total number of measurements in this track. │ │ │ │ │ +DDeeffiinniittiioonn SfmTrack.h:70 │ │ │ │ │ +_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_2_d_:_:_m_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ +std::vector< SfmMeasurement > measurements │ │ │ │ │ +The 2D image projections (id,(u,v)) │ │ │ │ │ +DDeeffiinniittiioonn SfmTrack.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_2_d_:_:_i_n_d_e_x_V_e_c_t_o_r │ │ │ │ │ +Eigen::VectorXi indexVector() const │ │ │ │ │ +Return the camera indices of the measurements. │ │ │ │ │ +DDeeffiinniittiioonn SfmTrack.h:109 │ │ │ │ │ +_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k │ │ │ │ │ +DDeeffiinniittiioonn SfmTrack.h:126 │ │ │ │ │ +_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_:_:_r_g_b │ │ │ │ │ +Point3 rgb() const │ │ │ │ │ +Get RGB values describing 3d point. │ │ │ │ │ +DDeeffiinniittiioonn SfmTrack.h:148 │ │ │ │ │ +_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_:_:_b │ │ │ │ │ +float b │ │ │ │ │ +RGB color of the 3D point. │ │ │ │ │ +DDeeffiinniittiioonn SfmTrack.h:128 │ │ │ │ │ +_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_:_:_p_o_i_n_t_3 │ │ │ │ │ +const Point3 & point3() const │ │ │ │ │ +Get 3D point. │ │ │ │ │ +DDeeffiinniittiioonn SfmTrack.h:145 │ │ │ │ │ +_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_:_:_p │ │ │ │ │ +Point3 p │ │ │ │ │ +3D position of the point │ │ │ │ │ +DDeeffiinniittiioonn SfmTrack.h:127 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _F_a_c_t_o_r_._h │ │ │ │ │ + * _s_f_m │ │ │ │ │ + * _S_f_m_T_r_a_c_k_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00647.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00605.html │ │ │ │┄ Files 94% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/ISAM-inst.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/Conditional.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
ISAM-inst.h File Reference
│ │ │ │ +
Conditional.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Incremental update functionality (iSAM) for BayesTree. │ │ │ │ +

Base class for conditional densities. │ │ │ │ More...

│ │ │ │ │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::Conditional< FACTOR, DERIVEDCONDITIONAL >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Incremental update functionality (iSAM) for BayesTree.

│ │ │ │ -
Author
Michael Kaess
│ │ │ │ +

Base class for conditional densities.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,24 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -ISAM-inst.h File Reference │ │ │ │ │ -Incremental update functionality (iSAM) for BayesTree. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +Conditional.h File Reference │ │ │ │ │ +Base class for conditional densities. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_C_o_n_d_i_t_i_o_n_a_l_<_ _F_A_C_T_O_R_,_ _D_E_R_I_V_E_D_C_O_N_D_I_T_I_O_N_A_L_ _> │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Incremental update functionality (iSAM) for BayesTree. │ │ │ │ │ +Base class for conditional densities. │ │ │ │ │ Author │ │ │ │ │ - Michael Kaess │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _I_S_A_M_-_i_n_s_t_._h │ │ │ │ │ + * _C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00647_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00068_source.html │ │ │ │┄ Files 89% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/ISAM-inst.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Value.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ISAM-inst.h
│ │ │ │ +
Value.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/inference/ISAM.h>
│ │ │ │ -
21#include <gtsam/inference/VariableIndex.h>
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/config.h> // Configuration from CMake
│ │ │ │
22
│ │ │ │ -
23namespace gtsam {
│ │ │ │ -
24
│ │ │ │ -
25/* ************************************************************************* */
│ │ │ │ -
26template<class BAYESTREE>
│ │ │ │ -
│ │ │ │ -
27void ISAM<BAYESTREE>::updateInternal(const FactorGraphType& newFactors,
│ │ │ │ -
28 Cliques* orphans, const Eliminate& function) {
│ │ │ │ -
29 // Remove the contaminated part of the Bayes tree
│ │ │ │ -
30 BayesNetType bn;
│ │ │ │ -
31 const KeySet newFactorKeys = newFactors.keys();
│ │ │ │ -
32 if (!this->empty()) {
│ │ │ │ -
33 KeyVector keyVector(newFactorKeys.begin(), newFactorKeys.end());
│ │ │ │ -
34 this->removeTop(keyVector, &bn, orphans);
│ │ │ │ -
35 }
│ │ │ │ -
36
│ │ │ │ -
37 // Add the removed top and the new factors
│ │ │ │ -
38 FactorGraphType factors;
│ │ │ │ -
39 factors += bn;
│ │ │ │ -
40 factors += newFactors;
│ │ │ │ -
41
│ │ │ │ -
42 // Add the orphaned subtrees
│ │ │ │ -
43 for (const sharedClique& orphan : *orphans)
│ │ │ │ -
44 factors += boost::make_shared<BayesTreeOrphanWrapper<Clique> >(orphan);
│ │ │ │ +
23#include <gtsam/base/Vector.h>
│ │ │ │ +
24#include <boost/serialization/nvp.hpp>
│ │ │ │ +
25#include <boost/serialization/assume_abstract.hpp>
│ │ │ │ +
26#include <memory>
│ │ │ │ +
27
│ │ │ │ +
28namespace gtsam {
│ │ │ │ +
29
│ │ │ │ +
│ │ │ │ +
37 class GTSAM_EXPORT Value {
│ │ │ │ +
38 public:
│ │ │ │ +
39
│ │ │ │ +
41 virtual Value* clone_() const = 0;
│ │ │ │ +
42
│ │ │ │ +
44 virtual void deallocate_() const = 0;
│ │ │ │
45
│ │ │ │ -
46 // Get an ordering where the new keys are eliminated last
│ │ │ │ -
47 const VariableIndex index(factors);
│ │ │ │ -
48 const Ordering ordering = Ordering::ColamdConstrainedLast(index,
│ │ │ │ -
49 KeyVector(newFactorKeys.begin(), newFactorKeys.end()));
│ │ │ │ -
50
│ │ │ │ -
51 // eliminate all factors (top, added, orphans) into a new Bayes tree
│ │ │ │ -
52 auto bayesTree = factors.eliminateMultifrontal(ordering, function, index);
│ │ │ │ -
53
│ │ │ │ -
54 // Re-add into Bayes tree data structures
│ │ │ │ -
55 this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(),
│ │ │ │ -
56 bayesTree->roots().end());
│ │ │ │ -
57 this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end());
│ │ │ │ -
58}
│ │ │ │ -
│ │ │ │ -
59
│ │ │ │ -
60/* ************************************************************************* */
│ │ │ │ -
61template<class BAYESTREE>
│ │ │ │ -
│ │ │ │ -
62void ISAM<BAYESTREE>::update(const FactorGraphType& newFactors,
│ │ │ │ -
63 const Eliminate& function) {
│ │ │ │ -
│ │ │ │ -
64 Cliques orphans;
│ │ │ │ -
65 this->updateInternal(newFactors, &orphans, function);
│ │ │ │ -
66}
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
67
│ │ │ │ -
68}
│ │ │ │ -
ISAM.h
Incremental update functionality (iSAM) for BayesTree.
│ │ │ │ - │ │ │ │ +
47 virtual boost::shared_ptr<Value> clone() const = 0;
│ │ │ │ +
48
│ │ │ │ +
50 virtual bool equals_(const Value& other, double tol = 1e-9) const = 0;
│ │ │ │ +
51
│ │ │ │ +
53 virtual void print(const std::string& str = "") const = 0;
│ │ │ │ +
54
│ │ │ │ +
60 virtual size_t dim() const = 0;
│ │ │ │ +
61
│ │ │ │ +
68 virtual Value* retract_(const Vector& delta) const = 0;
│ │ │ │ +
69
│ │ │ │ +
76 virtual Vector localCoordinates_(const Value& value) const = 0;
│ │ │ │ +
77
│ │ │ │ +
│ │ │ │ +
79 virtual Value& operator=(const Value& /*rhs*/) {
│ │ │ │ +
80 //needs a empty definition so recursion in implicit derived assignment operators work
│ │ │ │ +
81 return *this;
│ │ │ │ +
82 }
│ │ │ │ +
│ │ │ │ +
83
│ │ │ │ +
85 template<typename ValueType>
│ │ │ │ +
86 const ValueType& cast() const;
│ │ │ │ +
87
│ │ │ │ +
89 virtual ~Value() {}
│ │ │ │ +
90
│ │ │ │ +
91 private:
│ │ │ │ +
122 friend class boost::serialization::access;
│ │ │ │ +
123 template<class ARCHIVE>
│ │ │ │ +
124 void serialize(ARCHIVE & /*ar*/, const unsigned int /*version*/) {
│ │ │ │ +
125 }
│ │ │ │ +
126
│ │ │ │ +
127 };
│ │ │ │ +
│ │ │ │ +
128
│ │ │ │ +
129} /* namespace gtsam */
│ │ │ │ +
130
│ │ │ │ +
131BOOST_SERIALIZATION_ASSUME_ABSTRACT(gtsam::Value)
│ │ │ │ +
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ - │ │ │ │ -
gtsam::BayesTreeOrphanWrapper
Definition BayesTree.h:276
│ │ │ │ -
gtsam::ISAM::update
void update(const FactorGraphType &newFactors, const Eliminate &function=EliminationTraitsType::DefaultEliminate)
update the Bayes tree with a set of new factors, typically derived from measurements
Definition ISAM-inst.h:62
│ │ │ │ -
gtsam::ISAM::updateInternal
void updateInternal(const FactorGraphType &newFactors, Cliques *orphans, const Eliminate &function=EliminationTraitsType::DefaultEliminate)
updateInternal provides access to list of orphans for drawing purposes
Definition ISAM-inst.h:27
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ -
gtsam::Ordering::ColamdConstrainedLast
static Ordering ColamdConstrainedLast(const FACTOR_GRAPH &graph, const KeyVector &constrainLast, bool forceOrder=false)
Compute a fill-reducing ordering using constrained COLAMD from a factor graph (see details for note o...
Definition Ordering.h:114
│ │ │ │ -
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition VariableIndex.h:43
│ │ │ │ +
gtsam::Value
This is the base class for any type to be stored in Values.
Definition Value.h:37
│ │ │ │ +
gtsam::Value::dim
virtual size_t dim() const =0
Return the dimensionality of the tangent space of this value.
│ │ │ │ +
gtsam::Value::retract_
virtual Value * retract_(const Vector &delta) const =0
Increment the value, by mapping from the vector delta in the tangent space of the current value back ...
│ │ │ │ +
gtsam::Value::deallocate_
virtual void deallocate_() const =0
Deallocate a raw pointer of this value.
│ │ │ │ +
gtsam::Value::~Value
virtual ~Value()
Virutal destructor.
Definition Value.h:89
│ │ │ │ +
gtsam::Value::equals_
virtual bool equals_(const Value &other, double tol=1e-9) const =0
Compare this Value with another for equality.
│ │ │ │ +
gtsam::Value::localCoordinates_
virtual Vector localCoordinates_(const Value &value) const =0
Compute the coordinates in the tangent space of this value that retract() would map to value.
│ │ │ │ +
gtsam::Value::operator=
virtual Value & operator=(const Value &)
Assignment operator.
Definition Value.h:79
│ │ │ │ +
gtsam::Value::print
virtual void print(const std::string &str="") const =0
Print this value, for debugging and unit tests.
│ │ │ │ +
gtsam::Value::clone_
virtual Value * clone_() const =0
Clone this value in a special memory pool, must be deleted with Value::deallocate_,...
│ │ │ │ +
gtsam::Value::clone
virtual boost::shared_ptr< Value > clone() const =0
Clone this value (normal clone on the heap, delete with 'delete' operator)
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,113 +1,119 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ISAM-inst.h │ │ │ │ │ +Value.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_I_S_A_M_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_V_a_r_i_a_b_l_e_I_n_d_e_x_._h> │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include // Configuration from CMake │ │ │ │ │ 22 │ │ │ │ │ -23namespace _g_t_s_a_m { │ │ │ │ │ -24 │ │ │ │ │ -25/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -26template │ │ │ │ │ -_2_7void _I_S_A_M_<_B_A_Y_E_S_T_R_E_E_>_:_:_u_p_d_a_t_e_I_n_t_e_r_n_a_l(const FactorGraphType& newFactors, │ │ │ │ │ -28 Cliques* orphans, const Eliminate& function) { │ │ │ │ │ -29 // Remove the contaminated part of the Bayes tree │ │ │ │ │ -30 BayesNetType bn; │ │ │ │ │ -31 const _K_e_y_S_e_t newFactorKeys = newFactors.keys(); │ │ │ │ │ -32 if (!this->empty()) { │ │ │ │ │ -33 _K_e_y_V_e_c_t_o_r keyVector(newFactorKeys.begin(), newFactorKeys.end()); │ │ │ │ │ -34 this->removeTop(keyVector, &bn, orphans); │ │ │ │ │ -35 } │ │ │ │ │ -36 │ │ │ │ │ -37 // Add the removed top and the new factors │ │ │ │ │ -38 FactorGraphType factors; │ │ │ │ │ -39 factors += bn; │ │ │ │ │ -40 factors += newFactors; │ │ │ │ │ -41 │ │ │ │ │ -42 // Add the orphaned subtrees │ │ │ │ │ -43 for (const sharedClique& orphan : *orphans) │ │ │ │ │ -44 factors += boost::make_shared<_B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r_<_C_l_i_q_u_e_> >(orphan); │ │ │ │ │ +23#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_c_t_o_r_._h> │ │ │ │ │ +24#include │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ +27 │ │ │ │ │ +28namespace _g_t_s_a_m { │ │ │ │ │ +29 │ │ │ │ │ +_3_7 class GTSAM_EXPORT _V_a_l_u_e { │ │ │ │ │ +38 public: │ │ │ │ │ +39 │ │ │ │ │ +_4_1 virtual _V_a_l_u_e* _c_l_o_n_e__() const = 0; │ │ │ │ │ +42 │ │ │ │ │ +_4_4 virtual void _d_e_a_l_l_o_c_a_t_e__() const = 0; │ │ │ │ │ 45 │ │ │ │ │ -46 // Get an ordering where the new keys are eliminated last │ │ │ │ │ -47 const _V_a_r_i_a_b_l_e_I_n_d_e_x index(factors); │ │ │ │ │ -48 const _O_r_d_e_r_i_n_g ordering = _O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_L_a_s_t(index, │ │ │ │ │ -49 _K_e_y_V_e_c_t_o_r(newFactorKeys.begin(), newFactorKeys.end())); │ │ │ │ │ -50 │ │ │ │ │ -51 // eliminate all factors (top, added, orphans) into a new Bayes tree │ │ │ │ │ -52 auto bayesTree = factors.eliminateMultifrontal(ordering, function, index); │ │ │ │ │ -53 │ │ │ │ │ -54 // Re-add into Bayes tree data structures │ │ │ │ │ -55 this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), │ │ │ │ │ -56 bayesTree->roots().end()); │ │ │ │ │ -57 this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); │ │ │ │ │ -58} │ │ │ │ │ -59 │ │ │ │ │ -60/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -61template │ │ │ │ │ -_6_2void _I_S_A_M_<_B_A_Y_E_S_T_R_E_E_>_:_:_u_p_d_a_t_e(const FactorGraphType& newFactors, │ │ │ │ │ -63 const Eliminate& function) { │ │ │ │ │ -_6_4 Cliques orphans; │ │ │ │ │ -65 this->updateInternal(newFactors, &orphans, function); │ │ │ │ │ -66} │ │ │ │ │ -67 │ │ │ │ │ -68} │ │ │ │ │ -_I_S_A_M_._h │ │ │ │ │ -Incremental update functionality (iSAM) for BayesTree. │ │ │ │ │ -_V_a_r_i_a_b_l_e_I_n_d_e_x_._h │ │ │ │ │ +_4_7 virtual boost::shared_ptr _c_l_o_n_e() const = 0; │ │ │ │ │ +48 │ │ │ │ │ +_5_0 virtual bool _e_q_u_a_l_s__(const _V_a_l_u_e& other, double tol = 1e-9) const = 0; │ │ │ │ │ +51 │ │ │ │ │ +_5_3 virtual void _p_r_i_n_t(const std::string& str = "") const = 0; │ │ │ │ │ +54 │ │ │ │ │ +_6_0 virtual size_t _d_i_m() const = 0; │ │ │ │ │ +61 │ │ │ │ │ +_6_8 virtual _V_a_l_u_e* _r_e_t_r_a_c_t__(const Vector& delta) const = 0; │ │ │ │ │ +69 │ │ │ │ │ +_7_6 virtual Vector _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s__(const _V_a_l_u_e& value) const = 0; │ │ │ │ │ +77 │ │ │ │ │ +_7_9 virtual _V_a_l_u_e& _o_p_e_r_a_t_o_r_=(const _V_a_l_u_e& /*rhs*/) { │ │ │ │ │ +80 //needs a empty definition so recursion in implicit derived assignment │ │ │ │ │ +operators work │ │ │ │ │ +81 return *this; │ │ │ │ │ +82 } │ │ │ │ │ +83 │ │ │ │ │ +85 template │ │ │ │ │ +86 const ValueType& cast() const; │ │ │ │ │ +87 │ │ │ │ │ +_8_9 virtual _~_V_a_l_u_e() {} │ │ │ │ │ +90 │ │ │ │ │ +91 private: │ │ │ │ │ +_1_2_2 friend class boost::serialization::access; │ │ │ │ │ +123 template │ │ │ │ │ +124 void serialize(ARCHIVE & /*ar*/, const unsigned int /*version*/) { │ │ │ │ │ +125 } │ │ │ │ │ +126 │ │ │ │ │ +127 }; │ │ │ │ │ +128 │ │ │ │ │ +129} /* namespace gtsam */ │ │ │ │ │ +130 │ │ │ │ │ +131BOOST_SERIALIZATION_ASSUME_ABSTRACT(_g_t_s_a_m_:_:_V_a_l_u_e) │ │ │ │ │ +_V_e_c_t_o_r_._h │ │ │ │ │ +typedef and functions to augment Eigen's VectorXd │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_O_r_p_h_a_n_W_r_a_p_p_e_r │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:276 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_:_:_u_p_d_a_t_e │ │ │ │ │ -void update(const FactorGraphType &newFactors, const Eliminate │ │ │ │ │ -&function=EliminationTraitsType::DefaultEliminate) │ │ │ │ │ -update the Bayes tree with a set of new factors, typically derived from │ │ │ │ │ -measurements │ │ │ │ │ -DDeeffiinniittiioonn ISAM-inst.h:62 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_:_:_u_p_d_a_t_e_I_n_t_e_r_n_a_l │ │ │ │ │ -void updateInternal(const FactorGraphType &newFactors, Cliques *orphans, const │ │ │ │ │ -Eliminate &function=EliminationTraitsType::DefaultEliminate) │ │ │ │ │ -updateInternal provides access to list of orphans for drawing purposes │ │ │ │ │ -DDeeffiinniittiioonn ISAM-inst.h:27 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d_C_o_n_s_t_r_a_i_n_e_d_L_a_s_t │ │ │ │ │ -static Ordering ColamdConstrainedLast(const FACTOR_GRAPH &graph, const │ │ │ │ │ -KeyVector &constrainLast, bool forceOrder=false) │ │ │ │ │ -Compute a fill-reducing ordering using constrained COLAMD from a factor graph │ │ │ │ │ -(see details for note o... │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:114 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ -The VariableIndex class computes and stores the block column structure of a │ │ │ │ │ -factor graph. │ │ │ │ │ -DDeeffiinniittiioonn VariableIndex.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e │ │ │ │ │ +This is the base class for any type to be stored in Values. │ │ │ │ │ +DDeeffiinniittiioonn Value.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_:_:_d_i_m │ │ │ │ │ +virtual size_t dim() const =0 │ │ │ │ │ +Return the dimensionality of the tangent space of this value. │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_:_:_r_e_t_r_a_c_t__ │ │ │ │ │ +virtual Value * retract_(const Vector &delta) const =0 │ │ │ │ │ +Increment the value, by mapping from the vector delta in the tangent space of │ │ │ │ │ +the current value back ... │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_:_:_d_e_a_l_l_o_c_a_t_e__ │ │ │ │ │ +virtual void deallocate_() const =0 │ │ │ │ │ +Deallocate a raw pointer of this value. │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_:_:_~_V_a_l_u_e │ │ │ │ │ +virtual ~Value() │ │ │ │ │ +Virutal destructor. │ │ │ │ │ +DDeeffiinniittiioonn Value.h:89 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_:_:_e_q_u_a_l_s__ │ │ │ │ │ +virtual bool equals_(const Value &other, double tol=1e-9) const =0 │ │ │ │ │ +Compare this Value with another for equality. │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s__ │ │ │ │ │ +virtual Vector localCoordinates_(const Value &value) const =0 │ │ │ │ │ +Compute the coordinates in the tangent space of this value that retract() would │ │ │ │ │ +map to value. │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ +virtual Value & operator=(const Value &) │ │ │ │ │ +Assignment operator. │ │ │ │ │ +DDeeffiinniittiioonn Value.h:79 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_:_:_p_r_i_n_t │ │ │ │ │ +virtual void print(const std::string &str="") const =0 │ │ │ │ │ +Print this value, for debugging and unit tests. │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_:_:_c_l_o_n_e__ │ │ │ │ │ +virtual Value * clone_() const =0 │ │ │ │ │ +Clone this value in a special memory pool, must be deleted with Value:: │ │ │ │ │ +deallocate_,... │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_:_:_c_l_o_n_e │ │ │ │ │ +virtual boost::shared_ptr< Value > clone() const =0 │ │ │ │ │ +Clone this value (normal clone on the heap, delete with 'delete' operator) │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _I_S_A_M_-_i_n_s_t_._h │ │ │ │ │ + * _b_a_s_e │ │ │ │ │ + * _V_a_l_u_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00659.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01316.html │ │ │ │┄ Files 95% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/EliminationTree-inst.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/EssentialMatrixConstraint.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces
│ │ │ │ -
EliminationTree-inst.h File Reference
│ │ │ │ +
EssentialMatrixConstraint.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │
Author
Frank Dellaert
│ │ │ │
│ │ │ │ -Richard Roberts
│ │ │ │ -
Date
Oct 13, 2010
│ │ │ │ +Pablo Alcantarilla │ │ │ │ +
Date
Jan 5, 2014
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,22 +1,21 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -EliminationTree-inst.h File Reference │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +EssentialMatrixConstraint.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Pablo Alcantarilla │ │ │ │ │ Date │ │ │ │ │ - Oct 13, 2010 │ │ │ │ │ + Jan 5, 2014 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_-_i_n_s_t_._h │ │ │ │ │ + * _s_l_a_m │ │ │ │ │ + * _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_C_o_n_s_t_r_a_i_n_t_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00659_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00257_source.html │ │ │ │┄ Files 89% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/EliminationTree-inst.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DecisionTree.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
EliminationTree-inst.h
│ │ │ │ +
DecisionTree.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3* GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ -
4* Atlanta, Georgia 30332-0415
│ │ │ │ -
5* All Rights Reserved
│ │ │ │ -
6* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
4 * Atlanta, Georgia 30332-0415
│ │ │ │ +
5 * All Rights Reserved
│ │ │ │ +
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │ -
8* See LICENSE for the license information
│ │ │ │ +
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │ -
10* -------------------------------------------------------------------------- */
│ │ │ │ +
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <boost/make_shared.hpp>
│ │ │ │ -
21#include <stack>
│ │ │ │ -
22
│ │ │ │ -
23#include <gtsam/base/timing.h>
│ │ │ │ -
24#include <gtsam/base/treeTraversal-inst.h>
│ │ │ │ -
25#include <gtsam/inference/EliminationTree.h>
│ │ │ │ -
26#include <gtsam/inference/VariableIndex.h>
│ │ │ │ -
27#include <gtsam/inference/Ordering.h>
│ │ │ │ -
28#include <gtsam/inference/inference-inst.h>
│ │ │ │ -
29
│ │ │ │ -
30namespace gtsam {
│ │ │ │ -
31
│ │ │ │ -
32 /* ************************************************************************* */
│ │ │ │ -
33 template<class BAYESNET, class GRAPH>
│ │ │ │ -
34 typename EliminationTree<BAYESNET,GRAPH>::sharedFactor
│ │ │ │ -
35 EliminationTree<BAYESNET,GRAPH>::Node::eliminate(
│ │ │ │ -
36 const boost::shared_ptr<BayesNetType>& output,
│ │ │ │ -
37 const Eliminate& function, const FastVector<sharedFactor>& childrenResults) const
│ │ │ │ -
38 {
│ │ │ │ -
39 // This function eliminates one node (Node::eliminate) - see below eliminate for the whole tree.
│ │ │ │ -
40
│ │ │ │ -
41 assert(childrenResults.size() == children.size());
│ │ │ │ -
42
│ │ │ │ -
43 // Gather factors
│ │ │ │ -
44 FactorGraphType gatheredFactors;
│ │ │ │ -
45 gatheredFactors.reserve(factors.size() + children.size());
│ │ │ │ -
46 gatheredFactors.push_back(factors.begin(), factors.end());
│ │ │ │ -
47 gatheredFactors.push_back(childrenResults.begin(), childrenResults.end());
│ │ │ │ -
48
│ │ │ │ -
49 // Do dense elimination step
│ │ │ │ -
50 KeyVector keyAsVector(1); keyAsVector[0] = key;
│ │ │ │ -
51 auto eliminationResult = function(gatheredFactors, Ordering(keyAsVector));
│ │ │ │ -
52
│ │ │ │ -
53 // Add conditional to BayesNet
│ │ │ │ -
54 output->push_back(eliminationResult.first);
│ │ │ │ -
55
│ │ │ │ -
56 // Return result
│ │ │ │ -
57 return eliminationResult.second;
│ │ │ │ -
58 }
│ │ │ │ -
59
│ │ │ │ -
60 /* ************************************************************************* */
│ │ │ │ -
61 template<class BAYESNET, class GRAPH>
│ │ │ │ -
62 void EliminationTree<BAYESNET,GRAPH>::Node::print(
│ │ │ │ -
63 const std::string& str, const KeyFormatter& keyFormatter) const
│ │ │ │ -
64 {
│ │ │ │ -
65 std::cout << str << "(" << keyFormatter(key) << ")\n";
│ │ │ │ -
66 for(const sharedFactor& factor: factors) {
│ │ │ │ -
67 if(factor)
│ │ │ │ -
68 factor->print(str);
│ │ │ │ -
69 else
│ │ │ │ -
70 std::cout << str << "null factor\n";
│ │ │ │ -
71 }
│ │ │ │ -
72 }
│ │ │ │ -
73
│ │ │ │ -
74
│ │ │ │ -
75 /* ************************************************************************* */
│ │ │ │ -
76 template<class BAYESNET, class GRAPH>
│ │ │ │ -
│ │ │ │ -
77 EliminationTree<BAYESNET,GRAPH>::EliminationTree(const FactorGraphType& graph,
│ │ │ │ -
78 const VariableIndex& structure, const Ordering& order)
│ │ │ │ -
79 {
│ │ │ │ -
80 gttic(EliminationTree_Contructor);
│ │ │ │ -
81
│ │ │ │ -
82 // Number of factors and variables - NOTE in the case of partial elimination, n here may
│ │ │ │ -
83 // be fewer variables than are actually present in the graph.
│ │ │ │ -
84 const size_t m = graph.size();
│ │ │ │ -
85 const size_t n = order.size();
│ │ │ │ -
86
│ │ │ │ -
87 static const size_t none = std::numeric_limits<size_t>::max();
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/base/Testable.h>
│ │ │ │ +
23#include <gtsam/base/types.h>
│ │ │ │ +
24#include <gtsam/discrete/Assignment.h>
│ │ │ │ +
25
│ │ │ │ +
26#include <boost/serialization/nvp.hpp>
│ │ │ │ +
27#include <boost/shared_ptr.hpp>
│ │ │ │ +
28#include <functional>
│ │ │ │ +
29#include <iostream>
│ │ │ │ +
30#include <map>
│ │ │ │ +
31#include <set>
│ │ │ │ +
32#include <sstream>
│ │ │ │ +
33#include <string>
│ │ │ │ +
34#include <utility>
│ │ │ │ +
35#include <vector>
│ │ │ │ +
36
│ │ │ │ +
37namespace gtsam {
│ │ │ │ +
38
│ │ │ │ +
60 template<typename L, typename Y>
│ │ │ │ +
│ │ │ │ +
61 class DecisionTree {
│ │ │ │ +
62 protected:
│ │ │ │ +
│ │ │ │ +
64 static bool DefaultCompare(const Y& a, const Y& b) {
│ │ │ │ +
65 return a == b;
│ │ │ │ +
66 }
│ │ │ │ +
│ │ │ │ +
67
│ │ │ │ +
68 public:
│ │ │ │ +
69 using LabelFormatter = std::function<std::string(L)>;
│ │ │ │ +
70 using ValueFormatter = std::function<std::string(Y)>;
│ │ │ │ +
71 using CompareFunc = std::function<bool(const Y&, const Y&)>;
│ │ │ │ +
72
│ │ │ │ +
74 using Unary = std::function<Y(const Y&)>;
│ │ │ │ +
75 using UnaryAssignment = std::function<Y(const Assignment<L>&, const Y&)>;
│ │ │ │ +
76 using Binary = std::function<Y(const Y&, const Y&)>;
│ │ │ │ +
77
│ │ │ │ +
79 using LabelC = std::pair<L, size_t>;
│ │ │ │ +
80
│ │ │ │ +
82 struct Leaf;
│ │ │ │ +
83 struct Choice;
│ │ │ │ +
84
│ │ │ │ +
│ │ │ │ +
86 struct Node {
│ │ │ │ +
87 using Ptr = boost::shared_ptr<const Node>;
│ │ │ │
88
│ │ │ │ -
89 // Allocate result parent vector and vector of last factor columns
│ │ │ │ -
90 FastVector<sharedNode> nodes(n);
│ │ │ │ -
91 FastVector<size_t> parents(n, none);
│ │ │ │ -
92 FastVector<size_t> prevCol(m, none);
│ │ │ │ -
93 FastVector<bool> factorUsed(m, false);
│ │ │ │ -
94
│ │ │ │ -
95 try {
│ │ │ │ -
96 // for column j \in 1 to n do
│ │ │ │ -
97 for (size_t j = 0; j < n; j++)
│ │ │ │ -
98 {
│ │ │ │ -
99 // Retrieve the factors involving this variable and create the current node
│ │ │ │ -
│ │ │ │ -
100 const FactorIndices& factors = structure[order[j]];
│ │ │ │ -
101 const sharedNode node = boost::make_shared<Node>();
│ │ │ │ -
102 node->key = order[j];
│ │ │ │ -
103
│ │ │ │ -
104 // for row i \in Struct[A*j] do
│ │ │ │ -
105 node->children.reserve(factors.size());
│ │ │ │ -
106 node->factors.reserve(factors.size());
│ │ │ │ -
107 for(const size_t i: factors) {
│ │ │ │ -
│ │ │ │ -
108 // If we already hit a variable in this factor, make the subtree containing the previous
│ │ │ │ -
109 // variable in this factor a child of the current node. This means that the variables
│ │ │ │ -
110 // eliminated earlier in the factor depend on the later variables in the factor. If we
│ │ │ │ -
111 // haven't yet hit a variable in this factor, we add the factor to the current node.
│ │ │ │ -
112 // TODO: Store root shortcuts instead of parents.
│ │ │ │ -
113 if (prevCol[i] != none) {
│ │ │ │ -
114 size_t k = prevCol[i];
│ │ │ │ -
115 // Find root r of the current tree that contains k. Use raw pointers in computing the
│ │ │ │ -
│ │ │ │ -
116 // parents to avoid changing the reference counts while traversing up the tree.
│ │ │ │ -
117 size_t r = k;
│ │ │ │ -
118 while (parents[r] != none)
│ │ │ │ -
119 r = parents[r];
│ │ │ │ -
120 // If the root of the subtree involving this node is actually the current node,
│ │ │ │ -
121 // TODO: what does this mean? forest?
│ │ │ │ -
122 if (r != j) {
│ │ │ │ -
123 // Now that we found the root, hook up parent and child pointers in the nodes.
│ │ │ │ -
124 parents[r] = j;
│ │ │ │ -
125 node->children.push_back(nodes[r]);
│ │ │ │ -
126 }
│ │ │ │ -
127 } else {
│ │ │ │ -
128 // Add the factor to the current node since we are at the first variable in this factor.
│ │ │ │ -
129 node->factors.push_back(graph[i]);
│ │ │ │ -
│ │ │ │ -
130 factorUsed[i] = true;
│ │ │ │ -
131 }
│ │ │ │ -
132 prevCol[i] = j;
│ │ │ │ -
133 }
│ │ │ │ -
134 nodes[j] = node;
│ │ │ │ -
135 }
│ │ │ │ -
136 } catch(std::invalid_argument& e) {
│ │ │ │ -
│ │ │ │ -
137 // If this is thrown from structure[order[j]] above, it means that it was requested to
│ │ │ │ -
138 // eliminate a variable not present in the graph, so throw a more informative error message.
│ │ │ │ -
139 (void)e; // Prevent unused variable warning
│ │ │ │ -
140 throw std::invalid_argument("EliminationTree: given ordering contains variables that are not involved in the factor graph");
│ │ │ │ -
141 } catch(...) {
│ │ │ │ -
│ │ │ │ -
142 throw;
│ │ │ │ -
143 }
│ │ │ │ +
89#ifdef DT_DEBUG_MEMORY
│ │ │ │ +
90 static int nrNodes;
│ │ │ │ +
91#endif
│ │ │ │ +
92
│ │ │ │ +
93 // Constructor
│ │ │ │ +
94 Node() {
│ │ │ │ +
95#ifdef DT_DEBUG_MEMORY
│ │ │ │ +
96 std::cout << ++nrNodes << " constructed " << id() << std::endl;
│ │ │ │ +
97 std::cout.flush();
│ │ │ │ +
98#endif
│ │ │ │ +
99 }
│ │ │ │ +
100
│ │ │ │ +
101 // Destructor
│ │ │ │ +
102 virtual ~Node() {
│ │ │ │ +
103#ifdef DT_DEBUG_MEMORY
│ │ │ │ +
104 std::cout << --nrNodes << " destructed " << id() << std::endl;
│ │ │ │ +
105 std::cout.flush();
│ │ │ │ +
106#endif
│ │ │ │ +
107 }
│ │ │ │ +
108
│ │ │ │ +
109 // Unique ID for dot files
│ │ │ │ +
110 const void* id() const { return this; }
│ │ │ │ +
111
│ │ │ │ +
112 // everything else is virtual, no documentation here as internal
│ │ │ │ +
113 virtual void print(const std::string& s,
│ │ │ │ +
114 const LabelFormatter& labelFormatter,
│ │ │ │ +
115 const ValueFormatter& valueFormatter) const = 0;
│ │ │ │ +
116 virtual void dot(std::ostream& os, const LabelFormatter& labelFormatter,
│ │ │ │ +
117 const ValueFormatter& valueFormatter,
│ │ │ │ +
118 bool showZero) const = 0;
│ │ │ │ +
119 virtual bool sameLeaf(const Leaf& q) const = 0;
│ │ │ │ +
120 virtual bool sameLeaf(const Node& q) const = 0;
│ │ │ │ +
121 virtual bool equals(const Node& other, const CompareFunc& compare =
│ │ │ │ +
122 &DefaultCompare) const = 0;
│ │ │ │ +
123 virtual const Y& operator()(const Assignment<L>& x) const = 0;
│ │ │ │ +
124 virtual Ptr apply(const Unary& op) const = 0;
│ │ │ │ +
125 virtual Ptr apply(const UnaryAssignment& op,
│ │ │ │ +
126 const Assignment<L>& assignment) const = 0;
│ │ │ │ +
127 virtual Ptr apply_f_op_g(const Node&, const Binary&) const = 0;
│ │ │ │ +
128 virtual Ptr apply_g_op_fL(const Leaf&, const Binary&) const = 0;
│ │ │ │ +
129 virtual Ptr apply_g_op_fC(const Choice&, const Binary&) const = 0;
│ │ │ │ +
130 virtual Ptr choose(const L& label, size_t index) const = 0;
│ │ │ │ +
131 virtual bool isLeaf() const = 0;
│ │ │ │ +
132
│ │ │ │ +
133 private:
│ │ │ │ +
135 friend class boost::serialization::access;
│ │ │ │ +
136 template <class ARCHIVE>
│ │ │ │ +
137 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {}
│ │ │ │ +
138 };
│ │ │ │ +
│ │ │ │ +
141 public:
│ │ │ │ +
143 using NodePtr = typename Node::Ptr;
│ │ │ │
144
│ │ │ │ -
145 // Find roots
│ │ │ │ -
146 assert(parents.empty() || parents.back() == none); // We expect the last-eliminated node to be a root no matter what
│ │ │ │ -
147 for(size_t j = 0; j < n; ++j)
│ │ │ │ -
148 if(parents[j] == none)
│ │ │ │ -
149 roots_.push_back(nodes[j]);
│ │ │ │ -
150
│ │ │ │ -
151 // Gather remaining factors (exclude null factors)
│ │ │ │ -
152 for(size_t i = 0; i < m; ++i)
│ │ │ │ -
153 if(!factorUsed[i] && graph[i])
│ │ │ │ -
154 remainingFactors_.push_back(graph[i]);
│ │ │ │ -
155 }
│ │ │ │ -
156
│ │ │ │ -
│ │ │ │ -
157 /* ************************************************************************* */
│ │ │ │ -
158 template<class BAYESNET, class GRAPH>
│ │ │ │ -
│ │ │ │ -
159 EliminationTree<BAYESNET,GRAPH>::EliminationTree(
│ │ │ │ -
160 const FactorGraphType& factorGraph, const Ordering& order)
│ │ │ │ -
161 {
│ │ │ │ -
162 gttic(ET_Create2);
│ │ │ │ -
163 // Build variable index first
│ │ │ │ -
164 const VariableIndex variableIndex(factorGraph);
│ │ │ │ -
165 This temp(factorGraph, variableIndex, order);
│ │ │ │ -
166 this->swap(temp); // Swap in the tree, and temp will be deleted
│ │ │ │ -
167 }
│ │ │ │ -
│ │ │ │ -
168
│ │ │ │ -
169 /* ************************************************************************* */
│ │ │ │ -
170 template<class BAYESNET, class GRAPH>
│ │ │ │ -
171 EliminationTree<BAYESNET,GRAPH>&
│ │ │ │ -
│ │ │ │ -
172 EliminationTree<BAYESNET,GRAPH>::operator=(const EliminationTree<BAYESNET,GRAPH>& other)
│ │ │ │ -
173 {
│ │ │ │ -
174 // Start by duplicating the tree.
│ │ │ │ -
175 roots_ = treeTraversal::CloneForest(other);
│ │ │ │ -
176
│ │ │ │ -
177 // Assign the remaining factors - these are pointers to factors in the original factor graph and
│ │ │ │ -
178 // we do not clone them.
│ │ │ │ -
179 remainingFactors_ = other.remainingFactors_;
│ │ │ │ +
146 NodePtr root_;
│ │ │ │ +
147
│ │ │ │ +
148 protected:
│ │ │ │ +
153 template<typename It, typename ValueIt>
│ │ │ │ +
154 NodePtr create(It begin, It end, ValueIt beginY, ValueIt endY) const;
│ │ │ │ +
155
│ │ │ │ +
166 template <typename M, typename X>
│ │ │ │ +
167 NodePtr convertFrom(const typename DecisionTree<M, X>::NodePtr& f,
│ │ │ │ +
168 std::function<L(const M&)> L_of_M,
│ │ │ │ +
169 std::function<Y(const X&)> Y_of_X) const;
│ │ │ │ +
170
│ │ │ │ +
171 public:
│ │ │ │ +
174
│ │ │ │ +
176 DecisionTree();
│ │ │ │ +
177
│ │ │ │ +
179 explicit DecisionTree(const Y& y);
│ │ │ │
180
│ │ │ │ -
181 return *this;
│ │ │ │ -
182 }
│ │ │ │ -
│ │ │ │ -
183
│ │ │ │ -
184 /* ************************************************************************* */
│ │ │ │ -
185 template<class BAYESNET, class GRAPH>
│ │ │ │ -
186 std::pair<boost::shared_ptr<BAYESNET>, boost::shared_ptr<GRAPH> >
│ │ │ │ -
│ │ │ │ -
187 EliminationTree<BAYESNET,GRAPH>::eliminate(Eliminate function) const
│ │ │ │ -
188 {
│ │ │ │ -
189 gttic(EliminationTree_eliminate);
│ │ │ │ -
190 // Allocate result
│ │ │ │ -
191 auto result = boost::make_shared<BayesNetType>();
│ │ │ │ +
188 DecisionTree(const L& label, const Y& y1, const Y& y2);
│ │ │ │ +
189
│ │ │ │ +
191 DecisionTree(const LabelC& label, const Y& y1, const Y& y2);
│ │ │ │
192
│ │ │ │ -
193 // Run tree elimination algorithm
│ │ │ │ -
194 FastVector<sharedFactor> remainingFactors = inference::EliminateTree(result, *this, function);
│ │ │ │ +
194 DecisionTree(const std::vector<LabelC>& labelCs, const std::vector<Y>& ys);
│ │ │ │
195
│ │ │ │ -
196 // Add remaining factors that were not involved with eliminated variables
│ │ │ │ -
197 auto allRemainingFactors = boost::make_shared<FactorGraphType>();
│ │ │ │ -
198 allRemainingFactors->push_back(remainingFactors_.begin(), remainingFactors_.end());
│ │ │ │ -
199 allRemainingFactors->push_back(remainingFactors.begin(), remainingFactors.end());
│ │ │ │ -
200
│ │ │ │ -
201 // Return result
│ │ │ │ -
202 return std::make_pair(result, allRemainingFactors);
│ │ │ │ -
203 }
│ │ │ │ -
│ │ │ │ -
204
│ │ │ │ -
205 /* ************************************************************************* */
│ │ │ │ -
206 template<class BAYESNET, class GRAPH>
│ │ │ │ -
│ │ │ │ -
207 void EliminationTree<BAYESNET,GRAPH>::print(const std::string& name, const KeyFormatter& formatter) const
│ │ │ │ -
208 {
│ │ │ │ -
209 treeTraversal::PrintForest(*this, name, formatter);
│ │ │ │ -
210 }
│ │ │ │ -
│ │ │ │ -
211
│ │ │ │ -
212 /* ************************************************************************* */
│ │ │ │ -
213 template<class BAYESNET, class GRAPH>
│ │ │ │ -
│ │ │ │ -
214 bool EliminationTree<BAYESNET,GRAPH>::equals(const This& expected, double tol) const
│ │ │ │ -
215 {
│ │ │ │ -
216 // Depth-first-traversal stacks
│ │ │ │ -
217 std::stack<sharedNode, FastVector<sharedNode> > stack1, stack2;
│ │ │ │ -
218
│ │ │ │ -
219 // Add roots in sorted order
│ │ │ │ -
220 {
│ │ │ │ -
221 FastMap<Key,sharedNode> keys;
│ │ │ │ -
222 for(const sharedNode& root: this->roots_) { keys.insert(std::make_pair(root->key, root)); }
│ │ │ │ -
223 typedef typename FastMap<Key,sharedNode>::value_type Key_Node;
│ │ │ │ -
224 for(const Key_Node& key_node: keys) { stack1.push(key_node.second); }
│ │ │ │ -
225 }
│ │ │ │ -
226 {
│ │ │ │ -
227 FastMap<Key,sharedNode> keys;
│ │ │ │ -
228 for(const sharedNode& root: expected.roots_) { keys.insert(std::make_pair(root->key, root)); }
│ │ │ │ -
229 typedef typename FastMap<Key,sharedNode>::value_type Key_Node;
│ │ │ │ -
230 for(const Key_Node& key_node: keys) { stack2.push(key_node.second); }
│ │ │ │ -
231 }
│ │ │ │ -
232
│ │ │ │ -
233 // Traverse, adding children in sorted order
│ │ │ │ -
234 while(!stack1.empty() && !stack2.empty()) {
│ │ │ │ -
235 // Pop nodes
│ │ │ │ -
236 sharedNode node1 = stack1.top();
│ │ │ │ -
237 stack1.pop();
│ │ │ │ -
238 sharedNode node2 = stack2.top();
│ │ │ │ -
239 stack2.pop();
│ │ │ │ -
240
│ │ │ │ -
241 // Compare nodes
│ │ │ │ -
242 if(node1->key != node2->key)
│ │ │ │ -
243 return false;
│ │ │ │ -
244 if(node1->factors.size() != node2->factors.size()) {
│ │ │ │ -
245 return false;
│ │ │ │ -
246 } else {
│ │ │ │ -
247 for(typename Node::Factors::const_iterator it1 = node1->factors.begin(), it2 = node2->factors.begin();
│ │ │ │ -
248 it1 != node1->factors.end(); ++it1, ++it2) // Only check it1 == end because we already returned false for different counts
│ │ │ │ -
249 {
│ │ │ │ -
250 if(*it1 && *it2) {
│ │ │ │ -
251 if(!(*it1)->equals(**it2, tol))
│ │ │ │ -
252 return false;
│ │ │ │ -
253 } else if((*it1 && !*it2) || (*it2 && !*it1)) {
│ │ │ │ -
254 return false;
│ │ │ │ -
255 }
│ │ │ │ -
256 }
│ │ │ │ -
257 }
│ │ │ │ +
197 DecisionTree(const std::vector<LabelC>& labelCs, const std::string& table);
│ │ │ │ +
198
│ │ │ │ +
200 template<typename Iterator>
│ │ │ │ +
201 DecisionTree(Iterator begin, Iterator end, const L& label);
│ │ │ │ +
202
│ │ │ │ +
204 DecisionTree(const L& label, const DecisionTree& f0,
│ │ │ │ +
205 const DecisionTree& f1);
│ │ │ │ +
206
│ │ │ │ +
214 template <typename X, typename Func>
│ │ │ │ +
215 DecisionTree(const DecisionTree<L, X>& other, Func Y_of_X);
│ │ │ │ +
216
│ │ │ │ +
227 template <typename M, typename X, typename Func>
│ │ │ │ +
228 DecisionTree(const DecisionTree<M, X>& other, const std::map<M, L>& map,
│ │ │ │ +
229 Func Y_of_X);
│ │ │ │ +
230
│ │ │ │ +
234
│ │ │ │ +
242 void print(const std::string& s, const LabelFormatter& labelFormatter,
│ │ │ │ +
243 const ValueFormatter& valueFormatter) const;
│ │ │ │ +
244
│ │ │ │ +
245 // Testable
│ │ │ │ +
246 bool equals(const DecisionTree& other,
│ │ │ │ +
247 const CompareFunc& compare = &DefaultCompare) const;
│ │ │ │ +
248
│ │ │ │ +
252
│ │ │ │ +
254 virtual ~DecisionTree() = default;
│ │ │ │ +
255
│ │ │ │ +
257 bool empty() const { return !root_; }
│ │ │ │
258
│ │ │ │ -
259 // Add children in sorted order
│ │ │ │ -
260 {
│ │ │ │ -
261 FastMap<Key,sharedNode> keys;
│ │ │ │ -
262 for(const sharedNode& node: node1->children) { keys.insert(std::make_pair(node->key, node)); }
│ │ │ │ -
263 typedef typename FastMap<Key,sharedNode>::value_type Key_Node;
│ │ │ │ -
264 for(const Key_Node& key_node: keys) { stack1.push(key_node.second); }
│ │ │ │ -
265 }
│ │ │ │ -
266 {
│ │ │ │ -
267 FastMap<Key,sharedNode> keys;
│ │ │ │ -
268 for(const sharedNode& node: node2->children) { keys.insert(std::make_pair(node->key, node)); }
│ │ │ │ -
269 typedef typename FastMap<Key,sharedNode>::value_type Key_Node;
│ │ │ │ -
270 for(const Key_Node& key_node: keys) { stack2.push(key_node.second); }
│ │ │ │ -
271 }
│ │ │ │ -
272 }
│ │ │ │ -
273
│ │ │ │ -
274 // If either stack is not empty, the number of nodes differed
│ │ │ │ -
275 if(!stack1.empty() || !stack2.empty())
│ │ │ │ -
276 return false;
│ │ │ │ -
277
│ │ │ │ -
278 return true;
│ │ │ │ -
279 }
│ │ │ │ -
│ │ │ │ -
280
│ │ │ │ -
281 /* ************************************************************************* */
│ │ │ │ -
282 template<class BAYESNET, class GRAPH>
│ │ │ │ -
│ │ │ │ -
283 void EliminationTree<BAYESNET,GRAPH>::swap(This& other) {
│ │ │ │ -
284 roots_.swap(other.roots_);
│ │ │ │ -
285 remainingFactors_.swap(other.remainingFactors_);
│ │ │ │ -
286 }
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
287
│ │ │ │ -
288
│ │ │ │ -
289}
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ - │ │ │ │ -
timing.h
Timing utilities.
│ │ │ │ -
inference-inst.h
Contains generic inference algorithms that convert between templated graphical models,...
│ │ │ │ - │ │ │ │ -
Ordering.h
Variable ordering for the elimination algorithm.
│ │ │ │ - │ │ │ │ -
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │ +
260 bool operator==(const DecisionTree& q) const;
│ │ │ │ +
261
│ │ │ │ +
263 const Y& operator()(const Assignment<L>& x) const;
│ │ │ │ +
264
│ │ │ │ +
279 template <typename Func>
│ │ │ │ +
280 void visit(Func f) const;
│ │ │ │ +
281
│ │ │ │ +
296 template <typename Func>
│ │ │ │ +
297 void visitLeaf(Func f) const;
│ │ │ │ +
298
│ │ │ │ +
313 template <typename Func>
│ │ │ │ +
314 void visitWith(Func f) const;
│ │ │ │ +
315
│ │ │ │ +
317 size_t nrLeaves() const;
│ │ │ │ +
318
│ │ │ │ +
334 template <typename Func, typename X>
│ │ │ │ +
335 X fold(Func f, X x0) const;
│ │ │ │ +
336
│ │ │ │ +
338 std::set<L> labels() const;
│ │ │ │ +
339
│ │ │ │ +
341 DecisionTree apply(const Unary& op) const;
│ │ │ │ +
342
│ │ │ │ +
351 DecisionTree apply(const UnaryAssignment& op) const;
│ │ │ │ +
352
│ │ │ │ +
354 DecisionTree apply(const DecisionTree& g, const Binary& op) const;
│ │ │ │ +
355
│ │ │ │ +
│ │ │ │ +
358 DecisionTree choose(const L& label, size_t index) const {
│ │ │ │ +
359 NodePtr newRoot = root_->choose(label, index);
│ │ │ │ +
360 return DecisionTree(newRoot);
│ │ │ │ +
361 }
│ │ │ │ +
│ │ │ │ +
362
│ │ │ │ +
364 DecisionTree combine(const L& label, size_t cardinality,
│ │ │ │ +
365 const Binary& op) const;
│ │ │ │ +
366
│ │ │ │ +
│ │ │ │ +
368 DecisionTree combine(const LabelC& labelC, const Binary& op) const {
│ │ │ │ +
369 return combine(labelC.first, labelC.second, op);
│ │ │ │ +
370 }
│ │ │ │ +
│ │ │ │ +
371
│ │ │ │ +
373 void dot(std::ostream& os, const LabelFormatter& labelFormatter,
│ │ │ │ +
374 const ValueFormatter& valueFormatter, bool showZero = true) const;
│ │ │ │ +
375
│ │ │ │ +
377 void dot(const std::string& name, const LabelFormatter& labelFormatter,
│ │ │ │ +
378 const ValueFormatter& valueFormatter, bool showZero = true) const;
│ │ │ │ +
379
│ │ │ │ +
381 std::string dot(const LabelFormatter& labelFormatter,
│ │ │ │ +
382 const ValueFormatter& valueFormatter,
│ │ │ │ +
383 bool showZero = true) const;
│ │ │ │ +
384
│ │ │ │ +
387
│ │ │ │ +
388 // internal use only
│ │ │ │ +
389 explicit DecisionTree(const NodePtr& root);
│ │ │ │ +
390
│ │ │ │ +
391 // internal use only
│ │ │ │ +
392 template<typename Iterator> NodePtr
│ │ │ │ +
393 compose(Iterator begin, Iterator end, const L& label) const;
│ │ │ │ +
394
│ │ │ │ +
396
│ │ │ │ +
397 private:
│ │ │ │ +
399 friend class boost::serialization::access;
│ │ │ │ +
400 template <class ARCHIVE>
│ │ │ │ +
401 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ +
402 ar& BOOST_SERIALIZATION_NVP(root_);
│ │ │ │ +
403 }
│ │ │ │ +
404 }; // DecisionTree
│ │ │ │ +
│ │ │ │ +
405
│ │ │ │ +
406 template <class L, class Y>
│ │ │ │ +
407 struct traits<DecisionTree<L, Y>> : public Testable<DecisionTree<L, Y>> {};
│ │ │ │ +
408
│ │ │ │ +
412 template<typename L, typename Y>
│ │ │ │ +
│ │ │ │ +
413 DecisionTree<L, Y> apply(const DecisionTree<L, Y>& f,
│ │ │ │ +
414 const typename DecisionTree<L, Y>::Unary& op) {
│ │ │ │ +
415 return f.apply(op);
│ │ │ │ +
416 }
│ │ │ │ +
│ │ │ │ +
417
│ │ │ │ +
419 template<typename L, typename Y>
│ │ │ │ +
│ │ │ │ +
420 DecisionTree<L, Y> apply(const DecisionTree<L, Y>& f,
│ │ │ │ +
421 const typename DecisionTree<L, Y>::UnaryAssignment& op) {
│ │ │ │ +
422 return f.apply(op);
│ │ │ │ +
423 }
│ │ │ │ +
│ │ │ │ +
424
│ │ │ │ +
426 template<typename L, typename Y>
│ │ │ │ +
│ │ │ │ +
427 DecisionTree<L, Y> apply(const DecisionTree<L, Y>& f,
│ │ │ │ +
428 const DecisionTree<L, Y>& g,
│ │ │ │ +
429 const typename DecisionTree<L, Y>::Binary& op) {
│ │ │ │ +
430 return f.apply(g, op);
│ │ │ │ +
431 }
│ │ │ │ +
│ │ │ │ +
432
│ │ │ │ +
439 template <typename L, typename T1, typename T2>
│ │ │ │ +
│ │ │ │ +
440 std::pair<DecisionTree<L, T1>, DecisionTree<L, T2> > unzip(
│ │ │ │ +
441 const DecisionTree<L, std::pair<T1, T2> >& input) {
│ │ │ │ +
442 return std::make_pair(
│ │ │ │ +
443 DecisionTree<L, T1>(input, [](std::pair<T1, T2> i) { return i.first; }),
│ │ │ │ +
444 DecisionTree<L, T2>(input,
│ │ │ │ +
445 [](std::pair<T1, T2> i) { return i.second; }));
│ │ │ │ +
446 }
│ │ │ │ +
│ │ │ │ +
447
│ │ │ │ +
448} // namespace gtsam
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
types.h
Typedefs for easier changing of types.
│ │ │ │ +
Assignment.h
An assignment from labels to a discrete value index (size_t)
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition Factor.h:34
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::treeTraversal::CloneForest
FastVector< boost::shared_ptr< typename FOREST::Node > > CloneForest(const FOREST &forest)
Clone a tree, copy-constructing new nodes (calling boost::make_shared) and setting up child pointers ...
Definition treeTraversal-inst.h:189
│ │ │ │ -
gtsam::treeTraversal::PrintForest
void PrintForest(const FOREST &forest, std::string str, const KeyFormatter &keyFormatter)
Print a tree, prefixing each line with str, and formatting keys using keyFormatter.
Definition treeTraversal-inst.h:219
│ │ │ │ -
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ -
gtsam::EliminationTree
An elimination tree is a data structure used intermediately during elimination.
Definition EliminationTree.h:52
│ │ │ │ -
gtsam::EliminationTree::equals
bool equals(const This &other, double tol=1e-9) const
Test whether the tree is equal to another.
Definition EliminationTree-inst.h:214
│ │ │ │ -
gtsam::EliminationTree::print
void print(const std::string &name="EliminationTree: ", const KeyFormatter &formatter=DefaultKeyFormatter) const
Print the tree to cout.
Definition EliminationTree-inst.h:207
│ │ │ │ -
gtsam::EliminationTree::eliminate
std::pair< boost::shared_ptr< BayesNetType >, boost::shared_ptr< FactorGraphType > > eliminate(Eliminate function) const
Eliminate the factors to a Bayes net and remaining factor graph.
Definition EliminationTree-inst.h:187
│ │ │ │ -
gtsam::EliminationTree::operator=
This & operator=(const This &other)
Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are copie...
Definition EliminationTree-inst.h:172
│ │ │ │ -
gtsam::EliminationTree::roots_
FastVector< sharedNode > roots_
concept check
Definition EliminationTree.h:86
│ │ │ │ -
gtsam::EliminationTree::sharedFactor
boost::shared_ptr< FactorType > sharedFactor
Shared pointer to a factor.
Definition EliminationTree.h:60
│ │ │ │ -
gtsam::EliminationTree::FactorGraphType
GRAPH FactorGraphType
The factor graph type.
Definition EliminationTree.h:58
│ │ │ │ -
gtsam::EliminationTree::swap
void swap(This &other)
Swap the data of this tree with another one, this operation is very fast.
Definition EliminationTree-inst.h:283
│ │ │ │ -
gtsam::EliminationTree::EliminationTree
EliminationTree()
Protected default constructor.
Definition EliminationTree.h:161
│ │ │ │ -
gtsam::EliminationTree< DiscreteBayesNet, DiscreteFactorGraph >::sharedNode
boost::shared_ptr< Node > sharedNode
Shared pointer to Node.
Definition EliminationTree.h:80
│ │ │ │ -
gtsam::EliminationTree::remainingFactors
const FastVector< sharedFactor > & remainingFactors() const
Return the remaining factors that are not pulled into elimination.
Definition EliminationTree.h:154
│ │ │ │ -
gtsam::EliminationTree::Node::key
Key key
key associated with root
Definition EliminationTree.h:70
│ │ │ │ -
gtsam::EliminationTree::Node::children
Children children
sub-trees
Definition EliminationTree.h:72
│ │ │ │ -
gtsam::EliminationTree::Node::factors
Factors factors
factors associated with root
Definition EliminationTree.h:71
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ -
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition VariableIndex.h:43
│ │ │ │ +
gtsam::unzip
std::pair< DecisionTree< L, T1 >, DecisionTree< L, T2 > > unzip(const DecisionTree< L, std::pair< T1, T2 > > &input)
unzip a DecisionTree with std::pair values.
Definition DecisionTree.h:440
│ │ │ │ +
gtsam::apply
DecisionTree< L, Y > apply(const DecisionTree< L, Y > &f, const typename DecisionTree< L, Y >::Unary &op)
free versions of apply
Definition DecisionTree.h:413
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::Assignment
An assignment from labels to value index (size_t).
Definition Assignment.h:37
│ │ │ │ +
gtsam::DecisionTree::Leaf
Definition DecisionTree-inl.h:52
│ │ │ │ +
gtsam::DecisionTree::Choice
Definition DecisionTree-inl.h:172
│ │ │ │ +
gtsam::DecisionTree
a decision tree is a function from assignments to values.
Definition DecisionTree.h:61
│ │ │ │ +
gtsam::DecisionTree::apply
DecisionTree apply(const Unary &op) const
apply Unary operation "op" to f
Definition DecisionTree-inl.h:889
│ │ │ │ +
gtsam::DecisionTree::choose
DecisionTree choose(const L &label, size_t index) const
create a new function where value(label)==index It's like "restrict" in Darwiche09book pg329,...
Definition DecisionTree.h:358
│ │ │ │ +
gtsam::DecisionTree::convertFrom
NodePtr convertFrom(const typename DecisionTree< M, X >::NodePtr &f, std::function< L(const M &)> L_of_M, std::function< Y(const X &)> Y_of_X) const
Convert from a DecisionTree<M, X> to DecisionTree<L, Y>.
Definition DecisionTree-inl.h:671
│ │ │ │ +
gtsam::DecisionTree::combine
DecisionTree combine(const LabelC &labelC, const Binary &op) const
combine with LabelC for convenience
Definition DecisionTree.h:368
│ │ │ │ +
gtsam::DecisionTree::create
NodePtr create(It begin, It end, ValueIt beginY, ValueIt endY) const
Internal recursive function to create from keys, cardinalities, and Y values.
Definition DecisionTree-inl.h:630
│ │ │ │ +
gtsam::DecisionTree::~DecisionTree
virtual ~DecisionTree()=default
Make virtual.
│ │ │ │ +
gtsam::DecisionTree::DefaultCompare
static bool DefaultCompare(const Y &a, const Y &b)
Default method for comparison of two objects of type Y.
Definition DecisionTree.h:64
│ │ │ │ +
gtsam::DecisionTree::NodePtr
typename Node::Ptr NodePtr
---------------------— Node base class ------------------------—
Definition DecisionTree.h:143
│ │ │ │ +
gtsam::DecisionTree::labels
std::set< L > labels() const
Retrieve all unique labels as a set.
Definition DecisionTree-inl.h:853
│ │ │ │ +
gtsam::DecisionTree::empty
bool empty() const
Check if tree is empty.
Definition DecisionTree.h:257
│ │ │ │ +
gtsam::DecisionTree::visit
void visit(Func f) const
Visit all leaves in depth-first fashion.
Definition DecisionTree-inl.h:736
│ │ │ │ +
gtsam::DecisionTree::visitLeaf
void visitLeaf(Func f) const
Visit all leaves in depth-first fashion.
Definition DecisionTree-inl.h:773
│ │ │ │ +
gtsam::DecisionTree::Unary
std::function< Y(const Y &)> Unary
Handy typedefs for unary and binary function types.
Definition DecisionTree.h:74
│ │ │ │ +
gtsam::DecisionTree::fold
X fold(Func f, X x0) const
Fold a binary function over the tree, returning accumulator.
Definition DecisionTree-inl.h:833
│ │ │ │ +
gtsam::DecisionTree::root_
NodePtr root_
A DecisionTree just contains the root. TODO(dellaert): make protected.
Definition DecisionTree.h:146
│ │ │ │ +
gtsam::DecisionTree::print
void print(const std::string &s, const LabelFormatter &labelFormatter, const ValueFormatter &valueFormatter) const
GTSAM-style print.
Definition DecisionTree-inl.h:872
│ │ │ │ +
gtsam::DecisionTree::combine
DecisionTree combine(const L &label, size_t cardinality, const Binary &op) const
combine subtrees on key with binary operation "op"
Definition DecisionTree-inl.h:937
│ │ │ │ +
gtsam::DecisionTree::visitWith
void visitWith(Func f) const
Visit all leaves in depth-first fashion.
Definition DecisionTree-inl.h:816
│ │ │ │ +
gtsam::DecisionTree::operator()
const Y & operator()(const Assignment< L > &x) const
evaluate
Definition DecisionTree-inl.h:884
│ │ │ │ +
gtsam::DecisionTree::dot
void dot(std::ostream &os, const LabelFormatter &labelFormatter, const ValueFormatter &valueFormatter, bool showZero=true) const
output to graphviz format, stream version
Definition DecisionTree-inl.h:949
│ │ │ │ +
gtsam::DecisionTree::access
friend class boost::serialization::access
Serialization function.
Definition DecisionTree.h:399
│ │ │ │ +
gtsam::DecisionTree::operator==
bool operator==(const DecisionTree &q) const
equality
Definition DecisionTree-inl.h:879
│ │ │ │ +
gtsam::DecisionTree::LabelC
std::pair< L, size_t > LabelC
A label annotated with cardinality.
Definition DecisionTree.h:79
│ │ │ │ +
gtsam::DecisionTree::nrLeaves
size_t nrLeaves() const
Return the number of leaves in the tree.
Definition DecisionTree-inl.h:823
│ │ │ │ +
gtsam::DecisionTree::DecisionTree
DecisionTree()
Default constructor (for serialization)
Definition DecisionTree-inl.h:462
│ │ │ │ +
gtsam::DecisionTree::Node
---------------------— Node base class ------------------------—
Definition DecisionTree.h:86
│ │ │ │ +
gtsam::DecisionTree::Node::access
friend class boost::serialization::access
Serialization function.
Definition DecisionTree.h:135
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── encoding │ │ │ │ │ @@ -1 +1 @@ │ │ │ │ │ -us-ascii │ │ │ │ │ +utf-8 │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,449 +1,417 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -EliminationTree-inst.h │ │ │ │ │ +DecisionTree.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3* GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ -4* Atlanta, Georgia 30332-0415 │ │ │ │ │ -5* All Rights Reserved │ │ │ │ │ -6* Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +5 * All Rights Reserved │ │ │ │ │ +6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ -8* See LICENSE for the license information │ │ │ │ │ +8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ -10* ------------------------------------------------------------------------- │ │ │ │ │ +10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include │ │ │ │ │ -21#include │ │ │ │ │ -22 │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_e_/_t_i_m_i_n_g_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_b_a_s_e_/_t_r_e_e_T_r_a_v_e_r_s_a_l_-_i_n_s_t_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_V_a_r_i_a_b_l_e_I_n_d_e_x_._h> │ │ │ │ │ -27#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_O_r_d_e_r_i_n_g_._h> │ │ │ │ │ -28#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_i_n_f_e_r_e_n_c_e_-_i_n_s_t_._h> │ │ │ │ │ -29 │ │ │ │ │ -30namespace _g_t_s_a_m { │ │ │ │ │ -31 │ │ │ │ │ -32 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -33 template │ │ │ │ │ -34 typename _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_B_A_Y_E_S_N_E_T_,_G_R_A_P_H_>_:_:_s_h_a_r_e_d_F_a_c_t_o_r │ │ │ │ │ -35 EliminationTree::Node::eliminate( │ │ │ │ │ -36 const boost::shared_ptr& output, │ │ │ │ │ -37 const Eliminate& function, const FastVector& childrenResults) │ │ │ │ │ -const │ │ │ │ │ -38 { │ │ │ │ │ -39 // This function eliminates one node (Node::eliminate) - see below eliminate │ │ │ │ │ -for the whole tree. │ │ │ │ │ -40 │ │ │ │ │ -41 assert(childrenResults.size() == _c_h_i_l_d_r_e_n.size()); │ │ │ │ │ -42 │ │ │ │ │ -43 // Gather factors │ │ │ │ │ -44 _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e gatheredFactors; │ │ │ │ │ -45 gatheredFactors.reserve(_f_a_c_t_o_r_s.size() + _c_h_i_l_d_r_e_n.size()); │ │ │ │ │ -46 gatheredFactors.push_back(_f_a_c_t_o_r_s.begin(), _f_a_c_t_o_r_s.end()); │ │ │ │ │ -47 gatheredFactors.push_back(childrenResults.begin(), childrenResults.end()); │ │ │ │ │ -48 │ │ │ │ │ -49 // Do dense elimination step │ │ │ │ │ -50 _K_e_y_V_e_c_t_o_r keyAsVector(1); keyAsVector[0] = _k_e_y; │ │ │ │ │ -51 auto eliminationResult = function(gatheredFactors, Ordering(keyAsVector)); │ │ │ │ │ -52 │ │ │ │ │ -53 // Add conditional to BayesNet │ │ │ │ │ -54 output->push_back(eliminationResult.first); │ │ │ │ │ -55 │ │ │ │ │ -56 // Return result │ │ │ │ │ -57 return eliminationResult.second; │ │ │ │ │ -58 } │ │ │ │ │ -59 │ │ │ │ │ -60 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -61 template │ │ │ │ │ -62 void EliminationTree::Node::print( │ │ │ │ │ -63 const std::string& str, const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter) const │ │ │ │ │ -64 { │ │ │ │ │ -65 std::cout << str << "(" << keyFormatter(key) << ")\n"; │ │ │ │ │ -66 for(const _s_h_a_r_e_d_F_a_c_t_o_r& factor: factors) { │ │ │ │ │ -67 if(factor) │ │ │ │ │ -68 factor->print(str); │ │ │ │ │ -69 else │ │ │ │ │ -70 std::cout << str << "null factor\n"; │ │ │ │ │ -71 } │ │ │ │ │ -72 } │ │ │ │ │ -73 │ │ │ │ │ -74 │ │ │ │ │ -75 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -76 template │ │ │ │ │ -_7_7 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_B_A_Y_E_S_N_E_T_,_G_R_A_P_H_>_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e(const _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e& │ │ │ │ │ -graph, │ │ │ │ │ -78 const _V_a_r_i_a_b_l_e_I_n_d_e_x& structure, const _O_r_d_e_r_i_n_g& order) │ │ │ │ │ -79 { │ │ │ │ │ -80 gttic(EliminationTree_Contructor); │ │ │ │ │ -81 │ │ │ │ │ -82 // Number of factors and variables - NOTE in the case of partial │ │ │ │ │ -elimination, n here may │ │ │ │ │ -83 // be fewer variables than are actually present in the graph. │ │ │ │ │ -84 const size_t m = graph.size(); │ │ │ │ │ -85 const size_t n = order.size(); │ │ │ │ │ -86 │ │ │ │ │ -87 static const size_t none = std::numeric_limits::max(); │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_A_s_s_i_g_n_m_e_n_t_._h> │ │ │ │ │ +25 │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28#include │ │ │ │ │ +29#include │ │ │ │ │ +30#include │ │ │ │ │ +31#include │ │ │ │ │ +32#include │ │ │ │ │ +33#include │ │ │ │ │ +34#include │ │ │ │ │ +35#include │ │ │ │ │ +36 │ │ │ │ │ +37namespace _g_t_s_a_m { │ │ │ │ │ +38 │ │ │ │ │ +60 template │ │ │ │ │ +_6_1 class _D_e_c_i_s_i_o_n_T_r_e_e { │ │ │ │ │ +62 protected: │ │ │ │ │ +_6_4 static bool _D_e_f_a_u_l_t_C_o_m_p_a_r_e(const Y& a, const Y& b) { │ │ │ │ │ +65 return a == b; │ │ │ │ │ +66 } │ │ │ │ │ +67 │ │ │ │ │ +68 public: │ │ │ │ │ +69 using LabelFormatter = std::function; │ │ │ │ │ +70 using ValueFormatter = std::function; │ │ │ │ │ +71 using CompareFunc = std::function; │ │ │ │ │ +72 │ │ │ │ │ +_7_4 using _U_n_a_r_y = std::function; │ │ │ │ │ +75 using UnaryAssignment = std::function&, const Y&)>; │ │ │ │ │ +76 using Binary = std::function; │ │ │ │ │ +77 │ │ │ │ │ +_7_9 using _L_a_b_e_l_C = std::pair; │ │ │ │ │ +80 │ │ │ │ │ +82 struct _L_e_a_f; │ │ │ │ │ +83 struct _C_h_o_i_c_e; │ │ │ │ │ +84 │ │ │ │ │ +_8_6 struct _N_o_d_e { │ │ │ │ │ +87 using Ptr = boost::shared_ptr; │ │ │ │ │ 88 │ │ │ │ │ -89 // Allocate result parent vector and vector of last factor columns │ │ │ │ │ -90 _F_a_s_t_V_e_c_t_o_r_<_s_h_a_r_e_d_N_o_d_e_> nodes(n); │ │ │ │ │ -91 _F_a_s_t_V_e_c_t_o_r_<_s_i_z_e___t_> parents(n, none); │ │ │ │ │ -92 _F_a_s_t_V_e_c_t_o_r_<_s_i_z_e___t_> prevCol(m, none); │ │ │ │ │ -93 _F_a_s_t_V_e_c_t_o_r_<_b_o_o_l_> factorUsed(m, false); │ │ │ │ │ -94 │ │ │ │ │ -95 try { │ │ │ │ │ -96 // for column j \in 1 to n do │ │ │ │ │ -97 for (size_t j = 0; j < n; j++) │ │ │ │ │ -98 { │ │ │ │ │ -99 // Retrieve the factors involving this variable and create the current node │ │ │ │ │ -_1_0_0 const _F_a_c_t_o_r_I_n_d_i_c_e_s& factors = structure[order[j]]; │ │ │ │ │ -101 const _s_h_a_r_e_d_N_o_d_e node = boost::make_shared(); │ │ │ │ │ -102 node->key = order[j]; │ │ │ │ │ -103 │ │ │ │ │ -104 // for row i \in Struct[A*j] do │ │ │ │ │ -105 node->children.reserve(factors.size()); │ │ │ │ │ -106 node->factors.reserve(factors.size()); │ │ │ │ │ -107 for(const size_t i: factors) { │ │ │ │ │ -_1_0_8 // If we already hit a variable in this factor, make the subtree containing │ │ │ │ │ -the previous │ │ │ │ │ -109 // variable in this factor a child of the current node. This means that the │ │ │ │ │ -variables │ │ │ │ │ -110 // eliminated earlier in the factor depend on the later variables in the │ │ │ │ │ -factor. If we │ │ │ │ │ -111 // haven't yet hit a variable in this factor, we add the factor to the │ │ │ │ │ -current node. │ │ │ │ │ -112 // TODO: Store root shortcuts instead of parents. │ │ │ │ │ -113 if (prevCol[i] != none) { │ │ │ │ │ -114 size_t k = prevCol[i]; │ │ │ │ │ -115 // Find root r of the current tree that contains k. Use raw pointers in │ │ │ │ │ -computing the │ │ │ │ │ -_1_1_6 // parents to avoid changing the reference counts while traversing up the │ │ │ │ │ -tree. │ │ │ │ │ -117 size_t r = k; │ │ │ │ │ -118 while (parents[r] != none) │ │ │ │ │ -119 r = parents[r]; │ │ │ │ │ -120 // If the root of the subtree involving this node is actually the current │ │ │ │ │ -node, │ │ │ │ │ -121 // TODO: what does this mean? forest? │ │ │ │ │ -122 if (r != j) { │ │ │ │ │ -123 // Now that we found the root, hook up parent and child pointers in the │ │ │ │ │ -nodes. │ │ │ │ │ -124 parents[r] = j; │ │ │ │ │ -125 node->children.push_back(nodes[r]); │ │ │ │ │ -126 } │ │ │ │ │ -127 } else { │ │ │ │ │ -128 // Add the factor to the current node since we are at the first variable in │ │ │ │ │ -this factor. │ │ │ │ │ -129 node->factors.push_back(graph[i]); │ │ │ │ │ -_1_3_0 factorUsed[i] = true; │ │ │ │ │ -131 } │ │ │ │ │ -132 prevCol[i] = j; │ │ │ │ │ -133 } │ │ │ │ │ -134 nodes[j] = node; │ │ │ │ │ -135 } │ │ │ │ │ -136 } catch(std::invalid_argument& e) { │ │ │ │ │ -_1_3_7 // If this is thrown from structure[order[j]] above, it means that it was │ │ │ │ │ -requested to │ │ │ │ │ -138 // eliminate a variable not present in the graph, so throw a more │ │ │ │ │ -informative error message. │ │ │ │ │ -139 (void)e; // Prevent unused variable warning │ │ │ │ │ -140 throw std::invalid_argument("EliminationTree: given ordering contains │ │ │ │ │ -variables that are not involved in the factor graph"); │ │ │ │ │ -141 } catch(...) { │ │ │ │ │ -_1_4_2 throw; │ │ │ │ │ -143 } │ │ │ │ │ +89#ifdef DT_DEBUG_MEMORY │ │ │ │ │ +90 static int nrNodes; │ │ │ │ │ +91#endif │ │ │ │ │ +92 │ │ │ │ │ +93 // Constructor │ │ │ │ │ +94 _N_o_d_e() { │ │ │ │ │ +95#ifdef DT_DEBUG_MEMORY │ │ │ │ │ +96 std::cout << ++nrNodes << " constructed " << id() << std::endl; │ │ │ │ │ +97 std::cout.flush(); │ │ │ │ │ +98#endif │ │ │ │ │ +99 } │ │ │ │ │ +100 │ │ │ │ │ +101 // Destructor │ │ │ │ │ +102 virtual _~_N_o_d_e() { │ │ │ │ │ +103#ifdef DT_DEBUG_MEMORY │ │ │ │ │ +104 std::cout << --nrNodes << " destructed " << id() << std::endl; │ │ │ │ │ +105 std::cout.flush(); │ │ │ │ │ +106#endif │ │ │ │ │ +107 } │ │ │ │ │ +108 │ │ │ │ │ +109 // Unique ID for dot files │ │ │ │ │ +110 const void* id() const { return this; } │ │ │ │ │ +111 │ │ │ │ │ +112 // everything else is virtual, no documentation here as internal │ │ │ │ │ +113 virtual void print(const std::string& s, │ │ │ │ │ +114 const LabelFormatter& labelFormatter, │ │ │ │ │ +115 const ValueFormatter& valueFormatter) const = 0; │ │ │ │ │ +116 virtual void dot(std::ostream& os, const LabelFormatter& labelFormatter, │ │ │ │ │ +117 const ValueFormatter& valueFormatter, │ │ │ │ │ +118 bool showZero) const = 0; │ │ │ │ │ +119 virtual bool sameLeaf(const _L_e_a_f& q) const = 0; │ │ │ │ │ +120 virtual bool sameLeaf(const _N_o_d_e& q) const = 0; │ │ │ │ │ +121 virtual bool _e_q_u_a_l_s(const _N_o_d_e& other, const CompareFunc& compare = │ │ │ │ │ +122 &_D_e_f_a_u_l_t_C_o_m_p_a_r_e) const = 0; │ │ │ │ │ +123 virtual const Y& operator()(const _A_s_s_i_g_n_m_e_n_t_<_L_>& x) const = 0; │ │ │ │ │ +124 virtual Ptr apply(const _U_n_a_r_y& op) const = 0; │ │ │ │ │ +125 virtual Ptr apply(const UnaryAssignment& op, │ │ │ │ │ +126 const _A_s_s_i_g_n_m_e_n_t_<_L_>& assignment) const = 0; │ │ │ │ │ +127 virtual Ptr apply_f_op_g(const _N_o_d_e&, const Binary&) const = 0; │ │ │ │ │ +128 virtual Ptr apply_g_op_fL(const _L_e_a_f&, const Binary&) const = 0; │ │ │ │ │ +129 virtual Ptr apply_g_op_fC(const _C_h_o_i_c_e&, const Binary&) const = 0; │ │ │ │ │ +130 virtual Ptr choose(const L& label, size_t index) const = 0; │ │ │ │ │ +131 virtual bool isLeaf() const = 0; │ │ │ │ │ +132 │ │ │ │ │ +133 private: │ │ │ │ │ +_1_3_5 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +136 template │ │ │ │ │ +137 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {} │ │ │ │ │ +138 }; │ │ │ │ │ +141 public: │ │ │ │ │ +_1_4_3 using _N_o_d_e_P_t_r = typename Node::Ptr; │ │ │ │ │ 144 │ │ │ │ │ -145 // Find roots │ │ │ │ │ -146 assert(parents.empty() || parents.back() == none); // We expect the last- │ │ │ │ │ -eliminated node to be a root no matter what │ │ │ │ │ -147 for(size_t j = 0; j < n; ++j) │ │ │ │ │ -148 if(parents[j] == none) │ │ │ │ │ -149 _r_o_o_t_s__.push_back(nodes[j]); │ │ │ │ │ -150 │ │ │ │ │ -151 // Gather remaining factors (exclude null factors) │ │ │ │ │ -152 for(size_t i = 0; i < m; ++i) │ │ │ │ │ -153 if(!factorUsed[i] && graph[i]) │ │ │ │ │ -154 remainingFactors_.push_back(graph[i]); │ │ │ │ │ -155 } │ │ │ │ │ -156 │ │ │ │ │ -_1_5_7 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -158 template │ │ │ │ │ -_1_5_9 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_B_A_Y_E_S_N_E_T_,_G_R_A_P_H_>_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e( │ │ │ │ │ -160 const _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e& factorGraph, const _O_r_d_e_r_i_n_g& order) │ │ │ │ │ -161 { │ │ │ │ │ -162 gttic(ET_Create2); │ │ │ │ │ -163 // Build variable index first │ │ │ │ │ -164 const _V_a_r_i_a_b_l_e_I_n_d_e_x variableIndex(factorGraph); │ │ │ │ │ -165 _T_h_i_s temp(factorGraph, variableIndex, order); │ │ │ │ │ -166 this->_s_w_a_p(temp); // Swap in the tree, and temp will be deleted │ │ │ │ │ -167 } │ │ │ │ │ -168 │ │ │ │ │ -169 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -170 template │ │ │ │ │ -171 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_B_A_Y_E_S_N_E_T_,_G_R_A_P_H_>& │ │ │ │ │ -_1_7_2 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_B_A_Y_E_S_N_E_T_,_G_R_A_P_H_>_:_:_o_p_e_r_a_t_o_r_=(const │ │ │ │ │ -_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_B_A_Y_E_S_N_E_T_,_G_R_A_P_H_>& other) │ │ │ │ │ -173 { │ │ │ │ │ -174 // Start by duplicating the tree. │ │ │ │ │ -175 _r_o_o_t_s__ = _t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_C_l_o_n_e_F_o_r_e_s_t(other); │ │ │ │ │ -176 │ │ │ │ │ -177 // Assign the remaining factors - these are pointers to factors in the │ │ │ │ │ -original factor graph and │ │ │ │ │ -178 // we do not clone them. │ │ │ │ │ -179 remainingFactors_ = other.remainingFactors_; │ │ │ │ │ +_1_4_6 _N_o_d_e_P_t_r _r_o_o_t__; │ │ │ │ │ +147 │ │ │ │ │ +148 protected: │ │ │ │ │ +153 template │ │ │ │ │ +154 _N_o_d_e_P_t_r _c_r_e_a_t_e(It begin, It end, ValueIt beginY, ValueIt endY) const; │ │ │ │ │ +155 │ │ │ │ │ +166 template │ │ │ │ │ +167 _N_o_d_e_P_t_r _c_o_n_v_e_r_t_F_r_o_m(const typename _D_e_c_i_s_i_o_n_T_r_e_e_<_M_,_ _X_>_:_:_N_o_d_e_P_t_r& f, │ │ │ │ │ +168 std::function L_of_M, │ │ │ │ │ +169 std::function Y_of_X) const; │ │ │ │ │ +170 │ │ │ │ │ +171 public: │ │ │ │ │ +174 │ │ │ │ │ +176 _D_e_c_i_s_i_o_n_T_r_e_e(); │ │ │ │ │ +177 │ │ │ │ │ +179 explicit _D_e_c_i_s_i_o_n_T_r_e_e(const Y& y); │ │ │ │ │ 180 │ │ │ │ │ -181 return *this; │ │ │ │ │ -182 } │ │ │ │ │ -183 │ │ │ │ │ -184 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -185 template │ │ │ │ │ -186 std::pair, boost::shared_ptr > │ │ │ │ │ -_1_8_7 _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_B_A_Y_E_S_N_E_T_,_G_R_A_P_H_>_:_:_e_l_i_m_i_n_a_t_e(Eliminate function) const │ │ │ │ │ -188 { │ │ │ │ │ -189 gttic(EliminationTree_eliminate); │ │ │ │ │ -190 // Allocate result │ │ │ │ │ -191 auto result = boost::make_shared(); │ │ │ │ │ +188 _D_e_c_i_s_i_o_n_T_r_e_e(const L& label, const Y& y1, const Y& y2); │ │ │ │ │ +189 │ │ │ │ │ +191 _D_e_c_i_s_i_o_n_T_r_e_e(const _L_a_b_e_l_C& label, const Y& y1, const Y& y2); │ │ │ │ │ 192 │ │ │ │ │ -193 // Run tree elimination algorithm │ │ │ │ │ -194 _F_a_s_t_V_e_c_t_o_r_<_s_h_a_r_e_d_F_a_c_t_o_r_> _r_e_m_a_i_n_i_n_g_F_a_c_t_o_r_s = inference::EliminateTree │ │ │ │ │ -(result, *this, function); │ │ │ │ │ +194 _D_e_c_i_s_i_o_n_T_r_e_e(const std::vector& labelCs, const std::vector& ys); │ │ │ │ │ 195 │ │ │ │ │ -196 // Add remaining factors that were not involved with eliminated variables │ │ │ │ │ -197 auto allRemainingFactors = boost::make_shared(); │ │ │ │ │ -198 allRemainingFactors->push_back(remainingFactors_.begin(), │ │ │ │ │ -remainingFactors_.end()); │ │ │ │ │ -199 allRemainingFactors->push_back(_r_e_m_a_i_n_i_n_g_F_a_c_t_o_r_s.begin(), │ │ │ │ │ -_r_e_m_a_i_n_i_n_g_F_a_c_t_o_r_s.end()); │ │ │ │ │ -200 │ │ │ │ │ -201 // Return result │ │ │ │ │ -202 return std::make_pair(result, allRemainingFactors); │ │ │ │ │ -203 } │ │ │ │ │ -204 │ │ │ │ │ -205 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -206 template │ │ │ │ │ -_2_0_7 void _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_B_A_Y_E_S_N_E_T_,_G_R_A_P_H_>_:_:_p_r_i_n_t(const std::string& name, const │ │ │ │ │ -_K_e_y_F_o_r_m_a_t_t_e_r& formatter) const │ │ │ │ │ -208 { │ │ │ │ │ -209 _t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_P_r_i_n_t_F_o_r_e_s_t(*this, name, formatter); │ │ │ │ │ -210 } │ │ │ │ │ -211 │ │ │ │ │ -212 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -213 template │ │ │ │ │ -_2_1_4 bool _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_B_A_Y_E_S_N_E_T_,_G_R_A_P_H_>_:_:_e_q_u_a_l_s(const _T_h_i_s& expected, double │ │ │ │ │ -tol) const │ │ │ │ │ -215 { │ │ │ │ │ -216 // Depth-first-traversal stacks │ │ │ │ │ -217 std::stack > stack1, stack2; │ │ │ │ │ -218 │ │ │ │ │ -219 // Add roots in sorted order │ │ │ │ │ -220 { │ │ │ │ │ -221 _F_a_s_t_M_a_p_<_K_e_y_,_s_h_a_r_e_d_N_o_d_e_> keys; │ │ │ │ │ -222 for(const _s_h_a_r_e_d_N_o_d_e& root: this->_r_o_o_t_s__) { keys.insert(std::make_pair │ │ │ │ │ -(root->key, root)); } │ │ │ │ │ -223 typedef typename _F_a_s_t_M_a_p_<_K_e_y_,_s_h_a_r_e_d_N_o_d_e_>_:_:_v_a_l_u_e___t_y_p_e Key_Node; │ │ │ │ │ -224 for(const Key_Node& key_node: keys) { stack1.push(key_node.second); } │ │ │ │ │ -225 } │ │ │ │ │ -226 { │ │ │ │ │ -227 _F_a_s_t_M_a_p_<_K_e_y_,_s_h_a_r_e_d_N_o_d_e_> keys; │ │ │ │ │ -228 for(const _s_h_a_r_e_d_N_o_d_e& root: expected._r_o_o_t_s__) { keys.insert(std::make_pair │ │ │ │ │ -(root->key, root)); } │ │ │ │ │ -229 typedef typename _F_a_s_t_M_a_p_<_K_e_y_,_s_h_a_r_e_d_N_o_d_e_>_:_:_v_a_l_u_e___t_y_p_e Key_Node; │ │ │ │ │ -230 for(const Key_Node& key_node: keys) { stack2.push(key_node.second); } │ │ │ │ │ -231 } │ │ │ │ │ -232 │ │ │ │ │ -233 // Traverse, adding children in sorted order │ │ │ │ │ -234 while(!stack1.empty() && !stack2.empty()) { │ │ │ │ │ -235 // Pop nodes │ │ │ │ │ -236 _s_h_a_r_e_d_N_o_d_e node1 = stack1.top(); │ │ │ │ │ -237 stack1.pop(); │ │ │ │ │ -238 _s_h_a_r_e_d_N_o_d_e node2 = stack2.top(); │ │ │ │ │ -239 stack2.pop(); │ │ │ │ │ -240 │ │ │ │ │ -241 // Compare nodes │ │ │ │ │ -242 if(node1->key != node2->key) │ │ │ │ │ -243 return false; │ │ │ │ │ -244 if(node1->factors.size() != node2->factors.size()) { │ │ │ │ │ -245 return false; │ │ │ │ │ -246 } else { │ │ │ │ │ -247 for(typename Node::Factors::const_iterator it1 = node1->factors.begin(), │ │ │ │ │ -it2 = node2->factors.begin(); │ │ │ │ │ -248 it1 != node1->factors.end(); ++it1, ++it2) // Only check it1 == end because │ │ │ │ │ -we already returned false for different counts │ │ │ │ │ -249 { │ │ │ │ │ -250 if(*it1 && *it2) { │ │ │ │ │ -251 if(!(*it1)->equals(**it2, tol)) │ │ │ │ │ -252 return false; │ │ │ │ │ -253 } else if((*it1 && !*it2) || (*it2 && !*it1)) { │ │ │ │ │ -254 return false; │ │ │ │ │ -255 } │ │ │ │ │ -256 } │ │ │ │ │ -257 } │ │ │ │ │ +197 _D_e_c_i_s_i_o_n_T_r_e_e(const std::vector& labelCs, const std::string& table); │ │ │ │ │ +198 │ │ │ │ │ +200 template │ │ │ │ │ +201 _D_e_c_i_s_i_o_n_T_r_e_e(Iterator begin, Iterator end, const L& label); │ │ │ │ │ +202 │ │ │ │ │ +204 _D_e_c_i_s_i_o_n_T_r_e_e(const L& label, const _D_e_c_i_s_i_o_n_T_r_e_e& f0, │ │ │ │ │ +205 const _D_e_c_i_s_i_o_n_T_r_e_e& f1); │ │ │ │ │ +206 │ │ │ │ │ +214 template │ │ │ │ │ +215 _D_e_c_i_s_i_o_n_T_r_e_e(const _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _X_>& other, Func Y_of_X); │ │ │ │ │ +216 │ │ │ │ │ +227 template │ │ │ │ │ +228 _D_e_c_i_s_i_o_n_T_r_e_e(const _D_e_c_i_s_i_o_n_T_r_e_e_<_M_,_ _X_>& other, const std::map& map, │ │ │ │ │ +229 Func Y_of_X); │ │ │ │ │ +230 │ │ │ │ │ +234 │ │ │ │ │ +242 void _p_r_i_n_t(const std::string& s, const LabelFormatter& labelFormatter, │ │ │ │ │ +243 const ValueFormatter& valueFormatter) const; │ │ │ │ │ +244 │ │ │ │ │ +245 // Testable │ │ │ │ │ +246 bool _e_q_u_a_l_s(const _D_e_c_i_s_i_o_n_T_r_e_e& other, │ │ │ │ │ +247 const CompareFunc& compare = &_D_e_f_a_u_l_t_C_o_m_p_a_r_e) const; │ │ │ │ │ +248 │ │ │ │ │ +252 │ │ │ │ │ +_2_5_4 virtual _~_D_e_c_i_s_i_o_n_T_r_e_e() = default; │ │ │ │ │ +255 │ │ │ │ │ +_2_5_7 bool _e_m_p_t_y() const { return !_r_o_o_t__; } │ │ │ │ │ 258 │ │ │ │ │ -259 // Add children in sorted order │ │ │ │ │ -260 { │ │ │ │ │ -261 _F_a_s_t_M_a_p_<_K_e_y_,_s_h_a_r_e_d_N_o_d_e_> keys; │ │ │ │ │ -262 for(const _s_h_a_r_e_d_N_o_d_e& node: node1->children) { keys.insert(std::make_pair │ │ │ │ │ -(node->key, node)); } │ │ │ │ │ -263 typedef typename _F_a_s_t_M_a_p_<_K_e_y_,_s_h_a_r_e_d_N_o_d_e_>_:_:_v_a_l_u_e___t_y_p_e Key_Node; │ │ │ │ │ -264 for(const Key_Node& key_node: keys) { stack1.push(key_node.second); } │ │ │ │ │ -265 } │ │ │ │ │ -266 { │ │ │ │ │ -267 _F_a_s_t_M_a_p_<_K_e_y_,_s_h_a_r_e_d_N_o_d_e_> keys; │ │ │ │ │ -268 for(const _s_h_a_r_e_d_N_o_d_e& node: node2->children) { keys.insert(std::make_pair │ │ │ │ │ -(node->key, node)); } │ │ │ │ │ -269 typedef typename _F_a_s_t_M_a_p_<_K_e_y_,_s_h_a_r_e_d_N_o_d_e_>_:_:_v_a_l_u_e___t_y_p_e Key_Node; │ │ │ │ │ -270 for(const Key_Node& key_node: keys) { stack2.push(key_node.second); } │ │ │ │ │ -271 } │ │ │ │ │ -272 } │ │ │ │ │ -273 │ │ │ │ │ -274 // If either stack is not empty, the number of nodes differed │ │ │ │ │ -275 if(!stack1.empty() || !stack2.empty()) │ │ │ │ │ -276 return false; │ │ │ │ │ -277 │ │ │ │ │ -278 return true; │ │ │ │ │ -279 } │ │ │ │ │ -280 │ │ │ │ │ -281 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -282 template │ │ │ │ │ -_2_8_3 void _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_B_A_Y_E_S_N_E_T_,_G_R_A_P_H_>_:_:_s_w_a_p(_T_h_i_s& other) { │ │ │ │ │ -284 _r_o_o_t_s__.swap(other._r_o_o_t_s__); │ │ │ │ │ -285 remainingFactors_.swap(other.remainingFactors_); │ │ │ │ │ -286 } │ │ │ │ │ -287 │ │ │ │ │ -288 │ │ │ │ │ -289} │ │ │ │ │ -_t_r_e_e_T_r_a_v_e_r_s_a_l_-_i_n_s_t_._h │ │ │ │ │ -_t_i_m_i_n_g_._h │ │ │ │ │ -Timing utilities. │ │ │ │ │ -_i_n_f_e_r_e_n_c_e_-_i_n_s_t_._h │ │ │ │ │ -Contains generic inference algorithms that convert between templated graphical │ │ │ │ │ -models,... │ │ │ │ │ -_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h │ │ │ │ │ -_O_r_d_e_r_i_n_g_._h │ │ │ │ │ -Variable ordering for the elimination algorithm. │ │ │ │ │ -_V_a_r_i_a_b_l_e_I_n_d_e_x_._h │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ -std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ -FastVector │ │ │ │ │ -FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ -DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ +260 bool _o_p_e_r_a_t_o_r_=_=(const _D_e_c_i_s_i_o_n_T_r_e_e& q) const; │ │ │ │ │ +261 │ │ │ │ │ +263 const Y& _o_p_e_r_a_t_o_r_(_)(const _A_s_s_i_g_n_m_e_n_t_<_L_>& x) const; │ │ │ │ │ +264 │ │ │ │ │ +279 template │ │ │ │ │ +280 void _v_i_s_i_t(Func f) const; │ │ │ │ │ +281 │ │ │ │ │ +296 template │ │ │ │ │ +297 void _v_i_s_i_t_L_e_a_f(Func f) const; │ │ │ │ │ +298 │ │ │ │ │ +313 template │ │ │ │ │ +314 void _v_i_s_i_t_W_i_t_h(Func f) const; │ │ │ │ │ +315 │ │ │ │ │ +317 size_t _n_r_L_e_a_v_e_s() const; │ │ │ │ │ +318 │ │ │ │ │ +334 template │ │ │ │ │ +335 X _f_o_l_d(Func f, X x0) const; │ │ │ │ │ +336 │ │ │ │ │ +338 std::set _l_a_b_e_l_s() const; │ │ │ │ │ +339 │ │ │ │ │ +341 _D_e_c_i_s_i_o_n_T_r_e_e _a_p_p_l_y(const _U_n_a_r_y& op) const; │ │ │ │ │ +342 │ │ │ │ │ +351 _D_e_c_i_s_i_o_n_T_r_e_e _a_p_p_l_y(const UnaryAssignment& op) const; │ │ │ │ │ +352 │ │ │ │ │ +354 _D_e_c_i_s_i_o_n_T_r_e_e _a_p_p_l_y(const _D_e_c_i_s_i_o_n_T_r_e_e& g, const Binary& op) const; │ │ │ │ │ +355 │ │ │ │ │ +_3_5_8 _D_e_c_i_s_i_o_n_T_r_e_e _c_h_o_o_s_e(const L& label, size_t index) const { │ │ │ │ │ +359 _N_o_d_e_P_t_r newRoot = _r_o_o_t__->_c_h_o_o_s_e(label, index); │ │ │ │ │ +360 return _D_e_c_i_s_i_o_n_T_r_e_e(newRoot); │ │ │ │ │ +361 } │ │ │ │ │ +362 │ │ │ │ │ +364 _D_e_c_i_s_i_o_n_T_r_e_e combine(const L& label, size_t cardinality, │ │ │ │ │ +365 const Binary& op) const; │ │ │ │ │ +366 │ │ │ │ │ +_3_6_8 _D_e_c_i_s_i_o_n_T_r_e_e _c_o_m_b_i_n_e(const _L_a_b_e_l_C& labelC, const Binary& op) const { │ │ │ │ │ +369 return _c_o_m_b_i_n_e(labelC.first, labelC.second, op); │ │ │ │ │ +370 } │ │ │ │ │ +371 │ │ │ │ │ +373 void _d_o_t(std::ostream& os, const LabelFormatter& labelFormatter, │ │ │ │ │ +374 const ValueFormatter& valueFormatter, bool showZero = true) const; │ │ │ │ │ +375 │ │ │ │ │ +377 void _d_o_t(const std::string& name, const LabelFormatter& labelFormatter, │ │ │ │ │ +378 const ValueFormatter& valueFormatter, bool showZero = true) const; │ │ │ │ │ +379 │ │ │ │ │ +381 std::string _d_o_t(const LabelFormatter& labelFormatter, │ │ │ │ │ +382 const ValueFormatter& valueFormatter, │ │ │ │ │ +383 bool showZero = true) const; │ │ │ │ │ +384 │ │ │ │ │ +387 │ │ │ │ │ +388 // internal use only │ │ │ │ │ +389 explicit _D_e_c_i_s_i_o_n_T_r_e_e(const _N_o_d_e_P_t_r& root); │ │ │ │ │ +390 │ │ │ │ │ +391 // internal use only │ │ │ │ │ +392 template _N_o_d_e_P_t_r │ │ │ │ │ +393 compose(Iterator begin, Iterator end, const L& label) const; │ │ │ │ │ +394 │ │ │ │ │ +396 │ │ │ │ │ +397 private: │ │ │ │ │ +_3_9_9 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +400 template │ │ │ │ │ +401 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ +402 ar& BOOST_SERIALIZATION_NVP(_r_o_o_t__); │ │ │ │ │ +403 } │ │ │ │ │ +404 }; // DecisionTree │ │ │ │ │ +405 │ │ │ │ │ +406 template │ │ │ │ │ +_4_0_7 struct _t_r_a_i_t_s<_D_e_c_i_s_i_o_n_T_r_e_e> : public _T_e_s_t_a_b_l_e> {}; │ │ │ │ │ +408 │ │ │ │ │ +412 template │ │ │ │ │ +_4_1_3 _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_> _a_p_p_l_y(const _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>& f, │ │ │ │ │ +414 const typename _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>_:_:_U_n_a_r_y& op) { │ │ │ │ │ +415 return f._a_p_p_l_y(op); │ │ │ │ │ +416 } │ │ │ │ │ +417 │ │ │ │ │ +419 template │ │ │ │ │ +_4_2_0 _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_> _a_p_p_l_y(const _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>& f, │ │ │ │ │ +421 const typename DecisionTree::UnaryAssignment& op) { │ │ │ │ │ +422 return f._a_p_p_l_y(op); │ │ │ │ │ +423 } │ │ │ │ │ +424 │ │ │ │ │ +426 template │ │ │ │ │ +_4_2_7 _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_> _a_p_p_l_y(const _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>& f, │ │ │ │ │ +428 const _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _Y_>& g, │ │ │ │ │ +429 const typename DecisionTree::Binary& op) { │ │ │ │ │ +430 return f._a_p_p_l_y(g, op); │ │ │ │ │ +431 } │ │ │ │ │ +432 │ │ │ │ │ +439 template │ │ │ │ │ +_4_4_0 std::pair, _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _T_2_> > _u_n_z_i_p( │ │ │ │ │ +441 const _D_e_c_i_s_i_o_n_T_r_e_e >& input) { │ │ │ │ │ +442 return std::make_pair( │ │ │ │ │ +443 _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _T_1_>(input, [](std::pair i) { return i.first; }), │ │ │ │ │ +444 _D_e_c_i_s_i_o_n_T_r_e_e_<_L_,_ _T_2_>(input, │ │ │ │ │ +445 [](std::pair i) { return i.second; })); │ │ │ │ │ +446 } │ │ │ │ │ +447 │ │ │ │ │ +448} // namespace gtsam │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ +_t_y_p_e_s_._h │ │ │ │ │ +Typedefs for easier changing of types. │ │ │ │ │ +_A_s_s_i_g_n_m_e_n_t_._h │ │ │ │ │ +An assignment from labels to a discrete value index (size_t) │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_I_n_d_i_c_e_s │ │ │ │ │ -FastVector< FactorIndex > FactorIndices │ │ │ │ │ -Define collection types: │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_C_l_o_n_e_F_o_r_e_s_t │ │ │ │ │ -FastVector< boost::shared_ptr< typename FOREST::Node > > CloneForest(const │ │ │ │ │ -FOREST &forest) │ │ │ │ │ -Clone a tree, copy-constructing new nodes (calling boost::make_shared) and │ │ │ │ │ -setting up child pointers ... │ │ │ │ │ -DDeeffiinniittiioonn treeTraversal-inst.h:189 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_e_e_T_r_a_v_e_r_s_a_l_:_:_P_r_i_n_t_F_o_r_e_s_t │ │ │ │ │ -void PrintForest(const FOREST &forest, std::string str, const KeyFormatter │ │ │ │ │ -&keyFormatter) │ │ │ │ │ -Print a tree, prefixing each line with str, and formatting keys using │ │ │ │ │ -keyFormatter. │ │ │ │ │ -DDeeffiinniittiioonn treeTraversal-inst.h:219 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ -FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ -fast_pool_allocator instead of the defa... │ │ │ │ │ -DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ -An elimination tree is a data structure used intermediately during elimination. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const This &other, double tol=1e-9) const │ │ │ │ │ -Test whether the tree is equal to another. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree-inst.h:214 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &name="EliminationTree: ", const KeyFormatter │ │ │ │ │ -&formatter=DefaultKeyFormatter) const │ │ │ │ │ -Print the tree to cout. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree-inst.h:207 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_e_l_i_m_i_n_a_t_e │ │ │ │ │ -std::pair< boost::shared_ptr< BayesNetType >, boost::shared_ptr< │ │ │ │ │ -FactorGraphType > > eliminate(Eliminate function) const │ │ │ │ │ -Eliminate the factors to a Bayes net and remaining factor graph. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree-inst.h:187 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ -This & operator=(const This &other) │ │ │ │ │ -Assignment operator - makes a deep copy of the tree structure, but only │ │ │ │ │ -pointers to factors are copie... │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree-inst.h:172 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_r_o_o_t_s__ │ │ │ │ │ -FastVector< sharedNode > roots_ │ │ │ │ │ -concept check │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_s_h_a_r_e_d_F_a_c_t_o_r │ │ │ │ │ -boost::shared_ptr< FactorType > sharedFactor │ │ │ │ │ -Shared pointer to a factor. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:60 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_F_a_c_t_o_r_G_r_a_p_h_T_y_p_e │ │ │ │ │ -GRAPH FactorGraphType │ │ │ │ │ -The factor graph type. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_s_w_a_p │ │ │ │ │ -void swap(This &other) │ │ │ │ │ -Swap the data of this tree with another one, this operation is very fast. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree-inst.h:283 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ -EliminationTree() │ │ │ │ │ -Protected default constructor. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:161 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_ _D_i_s_c_r_e_t_e_B_a_y_e_s_N_e_t_,_ _D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_s_h_a_r_e_d_N_o_d_e │ │ │ │ │ -boost::shared_ptr< Node > sharedNode │ │ │ │ │ -Shared pointer to Node. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:80 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_r_e_m_a_i_n_i_n_g_F_a_c_t_o_r_s │ │ │ │ │ -const FastVector< sharedFactor > & remainingFactors() const │ │ │ │ │ -Return the remaining factors that are not pulled into elimination. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:154 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_N_o_d_e_:_:_k_e_y │ │ │ │ │ -Key key │ │ │ │ │ -key associated with root │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:70 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_N_o_d_e_:_:_c_h_i_l_d_r_e_n │ │ │ │ │ -Children children │ │ │ │ │ -sub-trees │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:72 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_N_o_d_e_:_:_f_a_c_t_o_r_s │ │ │ │ │ -Factors factors │ │ │ │ │ -factors associated with root │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:71 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ -The VariableIndex class computes and stores the block column structure of a │ │ │ │ │ -factor graph. │ │ │ │ │ -DDeeffiinniittiioonn VariableIndex.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_u_n_z_i_p │ │ │ │ │ +std::pair< DecisionTree< L, T1 >, DecisionTree< L, T2 > > unzip(const │ │ │ │ │ +DecisionTree< L, std::pair< T1, T2 > > &input) │ │ │ │ │ +unzip a DecisionTree with std::pair values. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree.h:440 │ │ │ │ │ +_g_t_s_a_m_:_:_a_p_p_l_y │ │ │ │ │ +DecisionTree< L, Y > apply(const DecisionTree< L, Y > &f, const typename │ │ │ │ │ +DecisionTree< L, Y >::Unary &op) │ │ │ │ │ +free versions of apply │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree.h:413 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_A_s_s_i_g_n_m_e_n_t │ │ │ │ │ +An assignment from labels to value index (size_t). │ │ │ │ │ +DDeeffiinniittiioonn Assignment.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_L_e_a_f │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree-inl.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_C_h_o_i_c_e │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree-inl.h:172 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e │ │ │ │ │ +a decision tree is a function from assignments to values. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree.h:61 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_a_p_p_l_y │ │ │ │ │ +DecisionTree apply(const Unary &op) const │ │ │ │ │ +apply Unary operation "op" to f │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree-inl.h:889 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_c_h_o_o_s_e │ │ │ │ │ +DecisionTree choose(const L &label, size_t index) const │ │ │ │ │ +create a new function where value(label)==index It's like "restrict" in │ │ │ │ │ +Darwiche09book pg329,... │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree.h:358 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_c_o_n_v_e_r_t_F_r_o_m │ │ │ │ │ +NodePtr convertFrom(const typename DecisionTree< M, X >::NodePtr &f, std:: │ │ │ │ │ +function< L(const M &)> L_of_M, std::function< Y(const X &)> Y_of_X) const │ │ │ │ │ +Convert from a DecisionTree to DecisionTree. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree-inl.h:671 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_c_o_m_b_i_n_e │ │ │ │ │ +DecisionTree combine(const LabelC &labelC, const Binary &op) const │ │ │ │ │ +combine with LabelC for convenience │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree.h:368 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_c_r_e_a_t_e │ │ │ │ │ +NodePtr create(It begin, It end, ValueIt beginY, ValueIt endY) const │ │ │ │ │ +Internal recursive function to create from keys, cardinalities, and Y values. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree-inl.h:630 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_~_D_e_c_i_s_i_o_n_T_r_e_e │ │ │ │ │ +virtual ~DecisionTree()=default │ │ │ │ │ +Make virtual. │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_D_e_f_a_u_l_t_C_o_m_p_a_r_e │ │ │ │ │ +static bool DefaultCompare(const Y &a, const Y &b) │ │ │ │ │ +Default method for comparison of two objects of type Y. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_N_o_d_e_P_t_r │ │ │ │ │ +typename Node::Ptr NodePtr │ │ │ │ │ +---------------------— Node base class ------------------------— │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree.h:143 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_l_a_b_e_l_s │ │ │ │ │ +std::set< L > labels() const │ │ │ │ │ +Retrieve all unique labels as a set. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree-inl.h:853 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_e_m_p_t_y │ │ │ │ │ +bool empty() const │ │ │ │ │ +Check if tree is empty. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree.h:257 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_v_i_s_i_t │ │ │ │ │ +void visit(Func f) const │ │ │ │ │ +Visit all leaves in depth-first fashion. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree-inl.h:736 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_v_i_s_i_t_L_e_a_f │ │ │ │ │ +void visitLeaf(Func f) const │ │ │ │ │ +Visit all leaves in depth-first fashion. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree-inl.h:773 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_U_n_a_r_y │ │ │ │ │ +std::function< Y(const Y &)> Unary │ │ │ │ │ +Handy typedefs for unary and binary function types. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_f_o_l_d │ │ │ │ │ +X fold(Func f, X x0) const │ │ │ │ │ +Fold a binary function over the tree, returning accumulator. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree-inl.h:833 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_r_o_o_t__ │ │ │ │ │ +NodePtr root_ │ │ │ │ │ +A DecisionTree just contains the root. TODO(dellaert): make protected. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree.h:146 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s, const LabelFormatter &labelFormatter, const │ │ │ │ │ +ValueFormatter &valueFormatter) const │ │ │ │ │ +GTSAM-style print. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree-inl.h:872 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_c_o_m_b_i_n_e │ │ │ │ │ +DecisionTree combine(const L &label, size_t cardinality, const Binary &op) │ │ │ │ │ +const │ │ │ │ │ +combine subtrees on key with binary operation "op" │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree-inl.h:937 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_v_i_s_i_t_W_i_t_h │ │ │ │ │ +void visitWith(Func f) const │ │ │ │ │ +Visit all leaves in depth-first fashion. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree-inl.h:816 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ +const Y & operator()(const Assignment< L > &x) const │ │ │ │ │ +evaluate │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree-inl.h:884 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_d_o_t │ │ │ │ │ +void dot(std::ostream &os, const LabelFormatter &labelFormatter, const │ │ │ │ │ +ValueFormatter &valueFormatter, bool showZero=true) const │ │ │ │ │ +output to graphviz format, stream version │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree-inl.h:949 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree.h:399 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_o_p_e_r_a_t_o_r_=_= │ │ │ │ │ +bool operator==(const DecisionTree &q) const │ │ │ │ │ +equality │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree-inl.h:879 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_L_a_b_e_l_C │ │ │ │ │ +std::pair< L, size_t > LabelC │ │ │ │ │ +A label annotated with cardinality. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree.h:79 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_n_r_L_e_a_v_e_s │ │ │ │ │ +size_t nrLeaves() const │ │ │ │ │ +Return the number of leaves in the tree. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree-inl.h:823 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_D_e_c_i_s_i_o_n_T_r_e_e │ │ │ │ │ +DecisionTree() │ │ │ │ │ +Default constructor (for serialization) │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree-inl.h:462 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_N_o_d_e │ │ │ │ │ +---------------------— Node base class ------------------------— │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_c_i_s_i_o_n_T_r_e_e_:_:_N_o_d_e_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn DecisionTree.h:135 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_-_i_n_s_t_._h │ │ │ │ │ + * _d_i_s_c_r_e_t_e │ │ │ │ │ + * _D_e_c_i_s_i_o_n_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00662_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00500_source.html │ │ │ │┄ Files 94% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/ISAM.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridGaussianISAM.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ISAM.h
│ │ │ │ +
HybridGaussianISAM.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18// \callgraph
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/global_includes.h>
│ │ │ │ -
22
│ │ │ │ -
23namespace gtsam {
│ │ │ │ -
24
│ │ │ │ -
30template <class BAYESTREE>
│ │ │ │ -
│ │ │ │ -
31class ISAM : public BAYESTREE {
│ │ │ │ -
32 public:
│ │ │ │ -
33 typedef BAYESTREE Base;
│ │ │ │ -
34 typedef typename Base::BayesNetType BayesNetType;
│ │ │ │ -
35 typedef typename Base::FactorGraphType FactorGraphType;
│ │ │ │ -
36 typedef typename Base::Clique Clique;
│ │ │ │ -
37 typedef typename Base::sharedClique sharedClique;
│ │ │ │ -
38 typedef typename Base::Cliques Cliques;
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/base/Testable.h>
│ │ │ │ +
23#include <gtsam/hybrid/HybridBayesTree.h>
│ │ │ │ +
24#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
│ │ │ │ +
25#include <gtsam/inference/ISAM.h>
│ │ │ │ +
26
│ │ │ │ +
27namespace gtsam {
│ │ │ │ +
28
│ │ │ │ +
│ │ │ │ +
34class GTSAM_EXPORT HybridGaussianISAM : public ISAM<HybridBayesTree> {
│ │ │ │ +
35 public:
│ │ │ │ +
36 typedef ISAM<HybridBayesTree> Base;
│ │ │ │ +
37 typedef HybridGaussianISAM This;
│ │ │ │ +
38 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │
39
│ │ │ │ -
40 private:
│ │ │ │ -
41 typedef typename Base::Eliminate Eliminate;
│ │ │ │ -
42 typedef typename Base::EliminationTraitsType EliminationTraitsType;
│ │ │ │ -
43
│ │ │ │ -
44 public:
│ │ │ │ -
47
│ │ │ │ -
49 ISAM() {}
│ │ │ │ +
42
│ │ │ │ +
44 HybridGaussianISAM();
│ │ │ │ +
45
│ │ │ │ +
47 HybridGaussianISAM(const HybridBayesTree& bayesTree);
│ │ │ │ +
48
│ │ │ │
50
│ │ │ │ -
52 explicit ISAM(const Base& bayesTree) : Base(bayesTree) {}
│ │ │ │ -
53
│ │ │ │ -
57
│ │ │ │ -
64 void update(
│ │ │ │ -
65 const FactorGraphType& newFactors,
│ │ │ │ -
66 const Eliminate& function = EliminationTraitsType::DefaultEliminate);
│ │ │ │ -
67
│ │ │ │ -
70 void updateInternal(
│ │ │ │ -
71 const FactorGraphType& newFactors, Cliques* orphans,
│ │ │ │ -
72 const Eliminate& function = EliminationTraitsType::DefaultEliminate);
│ │ │ │ -
73
│ │ │ │ -
75};
│ │ │ │ -
│ │ │ │ -
76
│ │ │ │ -
77} // namespace gtsam
│ │ │ │ -
global_includes.h
Included from all GTSAM files.
│ │ │ │ +
51 private:
│ │ │ │ +
53 void updateInternal(
│ │ │ │ +
54 const HybridGaussianFactorGraph& newFactors,
│ │ │ │ +
55 HybridBayesTree::Cliques* orphans,
│ │ │ │ +
56 const boost::optional<size_t>& maxNrLeaves = boost::none,
│ │ │ │ +
57 const boost::optional<Ordering>& ordering = boost::none,
│ │ │ │ +
58 const HybridBayesTree::Eliminate& function =
│ │ │ │ +
59 HybridBayesTree::EliminationTraitsType::DefaultEliminate);
│ │ │ │ +
60
│ │ │ │ +
61 public:
│ │ │ │ +
70 void update(const HybridGaussianFactorGraph& newFactors,
│ │ │ │ +
71 const boost::optional<size_t>& maxNrLeaves = boost::none,
│ │ │ │ +
72 const boost::optional<Ordering>& ordering = boost::none,
│ │ │ │ +
73 const HybridBayesTree::Eliminate& function =
│ │ │ │ +
74 HybridBayesTree::EliminationTraitsType::DefaultEliminate);
│ │ │ │ +
75
│ │ │ │ +
84 static Ordering GetOrdering(HybridGaussianFactorGraph& factors,
│ │ │ │ +
85 const HybridGaussianFactorGraph& newFactors);
│ │ │ │ +
86};
│ │ │ │ +
│ │ │ │ +
87
│ │ │ │ +
89template <>
│ │ │ │ +
90struct traits<HybridGaussianISAM> : public Testable<HybridGaussianISAM> {};
│ │ │ │ +
91
│ │ │ │ +
92} // namespace gtsam
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
HybridBayesTree.h
Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree.
│ │ │ │ +
HybridGaussianFactorGraph.h
Linearized Hybrid factor graph that uses type erasure.
│ │ │ │ +
ISAM.h
Incremental update functionality (iSAM) for BayesTree.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::HybridBayesTree
A Bayes tree representing a Hybrid density.
Definition HybridBayesTree.h:64
│ │ │ │ +
gtsam::HybridGaussianFactorGraph
Definition HybridGaussianFactorGraph.h:102
│ │ │ │ +
gtsam::HybridGaussianISAM
Definition HybridGaussianISAM.h:34
│ │ │ │
gtsam::ISAM
A Bayes tree with an update methods that implements the iSAM algorithm.
Definition ISAM.h:31
│ │ │ │ -
gtsam::ISAM::update
void update(const FactorGraphType &newFactors, const Eliminate &function=EliminationTraitsType::DefaultEliminate)
update the Bayes tree with a set of new factors, typically derived from measurements
Definition ISAM-inst.h:62
│ │ │ │ -
gtsam::ISAM::ISAM
ISAM(const Base &bayesTree)
Copy constructor.
Definition ISAM.h:52
│ │ │ │ -
gtsam::ISAM::ISAM
ISAM()
Create an empty Bayes Tree.
Definition ISAM.h:49
│ │ │ │ -
gtsam::ISAM::updateInternal
void updateInternal(const FactorGraphType &newFactors, Cliques *orphans, const Eliminate &function=EliminationTraitsType::DefaultEliminate)
updateInternal provides access to list of orphans for drawing purposes
Definition ISAM-inst.h:27
│ │ │ │ +
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,90 +1,99 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ISAM.h │ │ │ │ │ +HybridGaussianISAM.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18// \callgraph │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ -22 │ │ │ │ │ -23namespace _g_t_s_a_m { │ │ │ │ │ -24 │ │ │ │ │ -30template │ │ │ │ │ -_3_1class _I_S_A_M : public BAYESTREE { │ │ │ │ │ -32 public: │ │ │ │ │ -33 typedef BAYESTREE Base; │ │ │ │ │ -34 typedef typename Base::BayesNetType BayesNetType; │ │ │ │ │ -35 typedef typename Base::FactorGraphType FactorGraphType; │ │ │ │ │ -36 typedef typename Base::Clique Clique; │ │ │ │ │ -37 typedef typename Base::sharedClique sharedClique; │ │ │ │ │ -38 typedef typename Base::Cliques Cliques; │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_I_S_A_M_._h> │ │ │ │ │ +26 │ │ │ │ │ +27namespace _g_t_s_a_m { │ │ │ │ │ +28 │ │ │ │ │ +_3_4class GTSAM_EXPORT _H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M : public _I_S_A_M { │ │ │ │ │ +35 public: │ │ │ │ │ +36 typedef _I_S_A_M_<_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_> _B_a_s_e; │ │ │ │ │ +37 typedef _H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M _T_h_i_s; │ │ │ │ │ +38 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ 39 │ │ │ │ │ -40 private: │ │ │ │ │ -41 typedef typename Base::Eliminate Eliminate; │ │ │ │ │ -42 typedef typename Base::EliminationTraitsType EliminationTraitsType; │ │ │ │ │ -43 │ │ │ │ │ -44 public: │ │ │ │ │ -47 │ │ │ │ │ -_4_9 _I_S_A_M() {} │ │ │ │ │ +42 │ │ │ │ │ +44 _H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M(); │ │ │ │ │ +45 │ │ │ │ │ +47 _H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M(const _H_y_b_r_i_d_B_a_y_e_s_T_r_e_e& bayesTree); │ │ │ │ │ +48 │ │ │ │ │ 50 │ │ │ │ │ -_5_2 explicit _I_S_A_M(const Base& bayesTree) : Base(bayesTree) {} │ │ │ │ │ -53 │ │ │ │ │ -57 │ │ │ │ │ -64 void _u_p_d_a_t_e( │ │ │ │ │ -65 const FactorGraphType& newFactors, │ │ │ │ │ -66 const Eliminate& function = EliminationTraitsType::DefaultEliminate); │ │ │ │ │ -67 │ │ │ │ │ -70 void _u_p_d_a_t_e_I_n_t_e_r_n_a_l( │ │ │ │ │ -71 const FactorGraphType& newFactors, Cliques* orphans, │ │ │ │ │ -72 const Eliminate& function = EliminationTraitsType::DefaultEliminate); │ │ │ │ │ -73 │ │ │ │ │ -75}; │ │ │ │ │ -76 │ │ │ │ │ -77} // namespace gtsam │ │ │ │ │ -_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ -Included from all GTSAM files. │ │ │ │ │ +51 private: │ │ │ │ │ +53 void updateInternal( │ │ │ │ │ +54 const _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& newFactors, │ │ │ │ │ +55 HybridBayesTree::Cliques* orphans, │ │ │ │ │ +56 const boost::optional& maxNrLeaves = boost::none, │ │ │ │ │ +57 const boost::optional& ordering = boost::none, │ │ │ │ │ +58 const HybridBayesTree::Eliminate& function = │ │ │ │ │ +59 HybridBayesTree::EliminationTraitsType::DefaultEliminate); │ │ │ │ │ +60 │ │ │ │ │ +61 public: │ │ │ │ │ +70 void update(const _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& newFactors, │ │ │ │ │ +71 const boost::optional& maxNrLeaves = boost::none, │ │ │ │ │ +72 const boost::optional& ordering = boost::none, │ │ │ │ │ +73 const HybridBayesTree::Eliminate& function = │ │ │ │ │ +74 HybridBayesTree::EliminationTraitsType::DefaultEliminate); │ │ │ │ │ +75 │ │ │ │ │ +84 static _O_r_d_e_r_i_n_g GetOrdering(_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& factors, │ │ │ │ │ +85 const _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& newFactors); │ │ │ │ │ +86}; │ │ │ │ │ +87 │ │ │ │ │ +89template <> │ │ │ │ │ +_9_0struct _t_r_a_i_t_s<_H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ +91 │ │ │ │ │ +92} // namespace gtsam │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ +_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ +Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree. │ │ │ │ │ +_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Linearized Hybrid factor graph that uses type erasure. │ │ │ │ │ +_I_S_A_M_._h │ │ │ │ │ +Incremental update functionality (iSAM) for BayesTree. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e │ │ │ │ │ +A Bayes tree representing a Hybrid density. │ │ │ │ │ +DDeeffiinniittiioonn HybridBayesTree.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +DDeeffiinniittiioonn HybridGaussianFactorGraph.h:102 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M │ │ │ │ │ +DDeeffiinniittiioonn HybridGaussianISAM.h:34 │ │ │ │ │ _g_t_s_a_m_:_:_I_S_A_M │ │ │ │ │ A Bayes tree with an update methods that implements the iSAM algorithm. │ │ │ │ │ DDeeffiinniittiioonn ISAM.h:31 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_:_:_u_p_d_a_t_e │ │ │ │ │ -void update(const FactorGraphType &newFactors, const Eliminate │ │ │ │ │ -&function=EliminationTraitsType::DefaultEliminate) │ │ │ │ │ -update the Bayes tree with a set of new factors, typically derived from │ │ │ │ │ -measurements │ │ │ │ │ -DDeeffiinniittiioonn ISAM-inst.h:62 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_:_:_I_S_A_M │ │ │ │ │ -ISAM(const Base &bayesTree) │ │ │ │ │ -Copy constructor. │ │ │ │ │ -DDeeffiinniittiioonn ISAM.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_:_:_I_S_A_M │ │ │ │ │ -ISAM() │ │ │ │ │ -Create an empty Bayes Tree. │ │ │ │ │ -DDeeffiinniittiioonn ISAM.h:49 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_:_:_u_p_d_a_t_e_I_n_t_e_r_n_a_l │ │ │ │ │ -void updateInternal(const FactorGraphType &newFactors, Cliques *orphans, const │ │ │ │ │ -Eliminate &function=EliminationTraitsType::DefaultEliminate) │ │ │ │ │ -updateInternal provides access to list of orphans for drawing purposes │ │ │ │ │ -DDeeffiinniittiioonn ISAM-inst.h:27 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _I_S_A_M_._h │ │ │ │ │ + * _h_y_b_r_i_d │ │ │ │ │ + * _H_y_b_r_i_d_G_a_u_s_s_i_a_n_I_S_A_M_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00695.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00686.html │ │ │ │┄ Files 97% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/ClusterTree-inst.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesNet.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
ClusterTree-inst.h File Reference
│ │ │ │ +
BayesNet.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Collects factorgraph fragments defined on variable clusters, arranged in a tree. │ │ │ │ +

Bayes network. │ │ │ │ More...

│ │ │ │ │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

struct  gtsam::EliminationData< CLUSTERTREE >
 
class  gtsam::EliminationData< CLUSTERTREE >::EliminationPostOrderVisitor
class  gtsam::BayesNet< CONDITIONAL >
 A BayesNet is a tree of conditionals, stored in elimination order. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Collects factorgraph fragments defined on variable clusters, arranged in a tree.

│ │ │ │ -
Date
Oct 8, 2013
│ │ │ │ -
Author
Kai Ni
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ +

Bayes network.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │ -Frank Dellaert
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,32 +1,27 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -ClusterTree-inst.h File Reference │ │ │ │ │ -Collects factorgraph fragments defined on variable clusters, arranged in a │ │ │ │ │ -tree. _M_o_r_e_._._. │ │ │ │ │ +BayesNet.h File Reference │ │ │ │ │ +Bayes network. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_D_a_t_a_<_ _C_L_U_S_T_E_R_T_R_E_E_ _> │ │ │ │ │ -  │ │ │ │ │ - class   _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_D_a_t_a_<_ _C_L_U_S_T_E_R_T_R_E_E_ _>_:_:_E_l_i_m_i_n_a_t_i_o_n_P_o_s_t_O_r_d_e_r_V_i_s_i_t_o_r │ │ │ │ │ +class   _g_t_s_a_m_:_:_B_a_y_e_s_N_e_t_<_ _C_O_N_D_I_T_I_O_N_A_L_ _> │ │ │ │ │ +  A _B_a_y_e_s_N_e_t is a tree of conditionals, stored in elimination order. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Collects factorgraph fragments defined on variable clusters, arranged in a │ │ │ │ │ -tree. │ │ │ │ │ - Date │ │ │ │ │ - Oct 8, 2013 │ │ │ │ │ +Bayes network. │ │ │ │ │ Author │ │ │ │ │ - Kai Ni │ │ │ │ │ - Richard Roberts │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _i_n_f_e_r_e_n_c_e │ │ │ │ │ - * _C_l_u_s_t_e_r_T_r_e_e_-_i_n_s_t_._h │ │ │ │ │ + * _B_a_y_e_s_N_e_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00704_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00854_source.html │ │ │ │┄ Files 99% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
GaussianBayesTree-inl.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
20#pragma once
│ │ │ │
21
│ │ │ │ -
22#include <gtsam/linear/GaussianBayesTree.h> // Only to help Eclipse
│ │ │ │ +
22#include <gtsam/linear/GaussianBayesTree.h> // Only to help Eclipse
│ │ │ │
23
│ │ │ │
24#include <stdarg.h>
│ │ │ │
25
│ │ │ │
26namespace gtsam {
│ │ │ │
27
│ │ │ │
28/* ************************************************************************* */
│ │ │ │
29namespace internal {
│ │ │ │ @@ -150,21 +150,21 @@ │ │ │ │
51
│ │ │ │
52 return result;
│ │ │ │
53}
│ │ │ │
54
│ │ │ │
55/* ************************************************************************* */
│ │ │ │
56} // \namespace internal
│ │ │ │
57} // \namespace gtsam
│ │ │ │ -
GaussianBayesTree.h
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
│ │ │ │ +
GaussianBayesTree.h
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
VectorValues
The Factor::error simply extracts the.
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00707.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01271.html │ │ │ │┄ Files 89% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/HessianFactor.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/InitializePose3.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs | │ │ │ │ -Functions
│ │ │ │ -
HessianFactor.cpp File Reference
│ │ │ │ +Typedefs
│ │ │ │ +
InitializePose3.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ + │ │ │ │ +

Initialize Pose3 in a factor graph. │ │ │ │ +More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

struct  gtsam::InitializePose3
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ Typedefs

│ │ │ │ -using gtsam::Dims = std::vector< Key >
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ -Functions

std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< HessianFactor > > gtsam::EliminateCholesky (const GaussianFactorGraph &factors, const Ordering &keys)
 Densely partially eliminate with Cholesky factorization.
 
std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< GaussianFactor > > gtsam::EliminatePreferCholesky (const GaussianFactorGraph &factors, const Ordering &keys)
 Densely partially eliminate with Cholesky factorization.
 
│ │ │ │ +typedef std::map< Key, std::vector< size_t > > gtsam::KeyVectorMap
 
│ │ │ │ +typedef std::map< Key, Rot3 > gtsam::KeyRotMap
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Dec 8, 2010
│ │ │ │ +

Initialize Pose3 in a factor graph.

│ │ │ │ +
Author
Luca Carlone
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +
Date
August, 2014
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,36 +1,32 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -HessianFactor.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ +InitializePose3.h File Reference │ │ │ │ │ +Initialize Pose3 in a factor graph. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +struct   _g_t_s_a_m_:_:_I_n_i_t_i_a_l_i_z_e_P_o_s_e_3 │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ TTyyppeeddeeffss │ │ │ │ │ -using  ggttssaamm::::DDiimmss = std::vector< _K_e_y > │ │ │ │ │ -  │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ - std::pair< boost::shared_ptr< _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_C_h_o_l_e_s_k_y (const │ │ │ │ │ - _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l >, boost:: _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &factors, const │ │ │ │ │ - shared_ptr< _H_e_s_s_i_a_n_F_a_c_t_o_r > >  _O_r_d_e_r_i_n_g &keys) │ │ │ │ │ -  Densely partially eliminate with Cholesky │ │ │ │ │ - factorization. │ │ │ │ │ +typedef std::map< _K_e_y, std::vector< size_t > >  ggttssaamm::::KKeeyyVVeeccttoorrMMaapp │ │ │ │ │   │ │ │ │ │ - std::pair< boost::shared_ptr< _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_P_r_e_f_e_r_C_h_o_l_e_s_k_y (const │ │ │ │ │ - _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l >, boost:: _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &factors, const │ │ │ │ │ -shared_ptr< _G_a_u_s_s_i_a_n_F_a_c_t_o_r > >  _O_r_d_e_r_i_n_g &keys) │ │ │ │ │ -  Densely partially eliminate with Cholesky │ │ │ │ │ - factorization. │ │ │ │ │ + typedef std::map< _K_e_y, _R_o_t_3 >  ggttssaamm::::KKeeyyRRoottMMaapp │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Initialize Pose3 in a factor graph. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - Dec 8, 2010 │ │ │ │ │ + August, 2014 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _l_i_n_e_a_r │ │ │ │ │ - * _H_e_s_s_i_a_n_F_a_c_t_o_r_._c_p_p │ │ │ │ │ + * _s_l_a_m │ │ │ │ │ + * _I_n_i_t_i_a_l_i_z_e_P_o_s_e_3_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00713_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01406_source.html │ │ │ │┄ Files 84% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianEliminationTree.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/precompiled_header.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
GaussianEliminationTree.h
│ │ │ │ +
precompiled_header.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/linear/GaussianBayesNet.h>
│ │ │ │ -
22#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ -
23#include <gtsam/inference/EliminationTree.h>
│ │ │ │ -
24
│ │ │ │ -
25namespace gtsam {
│ │ │ │ -
26
│ │ │ │ -
│ │ │ │ -
27 class GTSAM_EXPORT GaussianEliminationTree :
│ │ │ │ -
28 public EliminationTree<GaussianBayesNet, GaussianFactorGraph>
│ │ │ │ -
29 {
│ │ │ │ -
30 public:
│ │ │ │ -
31 typedef EliminationTree<GaussianBayesNet, GaussianFactorGraph> Base;
│ │ │ │ -
32 typedef GaussianEliminationTree This;
│ │ │ │ -
33 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
34
│ │ │ │ -
43 GaussianEliminationTree(const GaussianFactorGraph& factorGraph,
│ │ │ │ -
44 const VariableIndex& structure, const Ordering& order);
│ │ │ │ -
45
│ │ │ │ -
51 GaussianEliminationTree(const GaussianFactorGraph& factorGraph,
│ │ │ │ -
52 const Ordering& order);
│ │ │ │ -
53
│ │ │ │ -
55 bool equals(const This& other, double tol = 1e-9) const;
│ │ │ │ -
56
│ │ │ │ -
57 private:
│ │ │ │ +
21// All headers in base, except:
│ │ │ │ +
22// treeTraversal-inst.h: very specific to only a few compilation units
│ │ │ │ +
23// numericalDerivative.h : includes things in linear, nonlinear :-(
│ │ │ │ +
24// testLie.h: includes numericalDerivative
│ │ │ │ +
25#include <gtsam/base/Lie.h>
│ │ │ │ +
26#include <gtsam/base/chartTesting.h>
│ │ │ │ +
27#include <gtsam/base/cholesky.h>
│ │ │ │ +
28#include <gtsam/base/concepts.h>
│ │ │ │ +
29#include <gtsam/base/ConcurrentMap.h>
│ │ │ │ +
30#include <gtsam/base/debug.h>
│ │ │ │ +
31#include <gtsam/base/DSFVector.h>
│ │ │ │ +
32#include <gtsam/base/FastDefaultAllocator.h>
│ │ │ │ +
33#include <gtsam/base/FastList.h>
│ │ │ │ +
34#include <gtsam/base/FastMap.h>
│ │ │ │ +
35#include <gtsam/base/FastSet.h>
│ │ │ │ +
36#include <gtsam/base/FastVector.h>
│ │ │ │ +
37#include <gtsam/base/GenericValue.h>
│ │ │ │ +
38#include <gtsam/base/Group.h>
│ │ │ │ +
39#include <gtsam/base/Lie.h>
│ │ │ │ +
40#include <gtsam/base/lieProxies.h>
│ │ │ │ +
41#include <gtsam/base/Manifold.h>
│ │ │ │ +
42#include <gtsam/base/Matrix.h>
│ │ │ │ +
43#include <gtsam/base/OptionalJacobian.h>
│ │ │ │ +
44#include <gtsam/base/ProductLieGroup.h>
│ │ │ │ +
45#include <gtsam/base/serialization.h>
│ │ │ │ +
46#include <gtsam/base/serializationTestHelpers.h>
│ │ │ │ +
47#include <gtsam/base/SymmetricBlockMatrix.h>
│ │ │ │ +
48#include <gtsam/base/Testable.h>
│ │ │ │ +
49#include <gtsam/base/TestableAssertions.h>
│ │ │ │ +
50#include <gtsam/base/ThreadsafeException.h>
│ │ │ │ +
51#include <gtsam/base/timing.h>
│ │ │ │ +
52#include <gtsam/base/types.h>
│ │ │ │ +
53#include <gtsam/base/Value.h>
│ │ │ │ +
54#include <gtsam/base/Vector.h>
│ │ │ │ +
55#include <gtsam/base/VectorSpace.h>
│ │ │ │ +
56#include <gtsam/base/VerticalBlockMatrix.h>
│ │ │ │ +
57
│ │ │ │
58
│ │ │ │ -
59 friend class ::EliminationTreeTester;
│ │ │ │ -
60
│ │ │ │ -
61 };
│ │ │ │ -
│ │ │ │ -
62
│ │ │ │ -
63}
│ │ │ │ - │ │ │ │ -
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
│ │ │ │ -
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ -
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::EliminationTree
An elimination tree is a data structure used intermediately during elimination.
Definition EliminationTree.h:52
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ -
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition VariableIndex.h:43
│ │ │ │ -
gtsam::GaussianEliminationTree
Definition GaussianEliminationTree.h:29
│ │ │ │ -
gtsam::GaussianEliminationTree::This
GaussianEliminationTree This
This class.
Definition GaussianEliminationTree.h:32
│ │ │ │ -
gtsam::GaussianEliminationTree::Base
EliminationTree< GaussianBayesNet, GaussianFactorGraph > Base
Base class.
Definition GaussianEliminationTree.h:31
│ │ │ │ -
gtsam::GaussianEliminationTree::shared_ptr
boost::shared_ptr< This > shared_ptr
Shared pointer to this class.
Definition GaussianEliminationTree.h:33
│ │ │ │ -
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ +
FastDefaultAllocator.h
An easy way to control which allocator is used for Fast* collections.
│ │ │ │ +
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ +
serialization.h
Convenience functions for serializing data structures via boost.serialization.
│ │ │ │ +
debug.h
Global debugging flags.
│ │ │ │ + │ │ │ │ +
FastMap.h
A thin wrapper around std::map that uses boost's fast_pool_allocator.
│ │ │ │ +
FastSet.h
A thin wrapper around std::set that uses boost's fast_pool_allocator.
│ │ │ │ +
FastList.h
A thin wrapper around std::list that uses boost's fast_pool_allocator.
│ │ │ │ +
Value.h
The base class for any variable that can be optimized or used in a factor.
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ +
Group.h
Concept check class for variable types with Group properties.
│ │ │ │ +
OptionalJacobian.h
Special class for optional Jacobian arguments.
│ │ │ │ +
ThreadsafeException.h
Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
│ │ │ │ +
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
cholesky.h
Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained Cholesky.
│ │ │ │ +
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ +
ProductLieGroup.h
Group product of two Lie Groups.
│ │ │ │ +
SymmetricBlockMatrix.h
Access to matrices via blocks of pre-defined sizes.
│ │ │ │ +
timing.h
Timing utilities.
│ │ │ │ +
types.h
Typedefs for easier changing of types.
│ │ │ │ +
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ +
DSFVector.h
A faster implementation for DSF, which uses vector rather than btree.
│ │ │ │ +
VerticalBlockMatrix.h
A matrix with column blocks of pre-defined sizes.
│ │ │ │ +
TestableAssertions.h
Provides additional testing facilities for common data structures.
│ │ │ │ +
lieProxies.h
Provides convenient mappings of common member functions for testing.
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -GaussianEliminationTree.h │ │ │ │ │ +precompiled_header.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ @@ -16,77 +16,100 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 19#pragma once │ │ │ │ │ 20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h> │ │ │ │ │ -24 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ -26 │ │ │ │ │ -_2_7 class GTSAM_EXPORT _G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e : │ │ │ │ │ -28 public _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ -29 { │ │ │ │ │ -30 public: │ │ │ │ │ -_3_1 typedef _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_,_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_> _B_a_s_e; │ │ │ │ │ -_3_2 typedef _G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e _T_h_i_s; │ │ │ │ │ -_3_3 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -34 │ │ │ │ │ -43 _G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& factorGraph, │ │ │ │ │ -44 const _V_a_r_i_a_b_l_e_I_n_d_e_x& structure, const _O_r_d_e_r_i_n_g& order); │ │ │ │ │ -45 │ │ │ │ │ -51 _G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& factorGraph, │ │ │ │ │ -52 const _O_r_d_e_r_i_n_g& order); │ │ │ │ │ -53 │ │ │ │ │ -55 bool _e_q_u_a_l_s(const _T_h_i_s& other, double tol = 1e-9) const; │ │ │ │ │ -56 │ │ │ │ │ -57 private: │ │ │ │ │ +21// All headers in base, except: │ │ │ │ │ +22// treeTraversal-inst.h: very specific to only a few compilation units │ │ │ │ │ +23// numericalDerivative.h : includes things in linear, nonlinear :-( │ │ │ │ │ +24// testLie.h: includes numericalDerivative │ │ │ │ │ +25#include <_g_t_s_a_m_/_b_a_s_e_/_L_i_e_._h> │ │ │ │ │ +26#include │ │ │ │ │ +27#include <_g_t_s_a_m_/_b_a_s_e_/_c_h_o_l_e_s_k_y_._h> │ │ │ │ │ +28#include │ │ │ │ │ +29#include │ │ │ │ │ +30#include <_g_t_s_a_m_/_b_a_s_e_/_d_e_b_u_g_._h> │ │ │ │ │ +31#include <_g_t_s_a_m_/_b_a_s_e_/_D_S_F_V_e_c_t_o_r_._h> │ │ │ │ │ +32#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_._h> │ │ │ │ │ +33#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_L_i_s_t_._h> │ │ │ │ │ +34#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_M_a_p_._h> │ │ │ │ │ +35#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_S_e_t_._h> │ │ │ │ │ +36#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ +37#include │ │ │ │ │ +38#include <_g_t_s_a_m_/_b_a_s_e_/_G_r_o_u_p_._h> │ │ │ │ │ +39#include <_g_t_s_a_m_/_b_a_s_e_/_L_i_e_._h> │ │ │ │ │ +40#include <_g_t_s_a_m_/_b_a_s_e_/_l_i_e_P_r_o_x_i_e_s_._h> │ │ │ │ │ +41#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ +42#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ +43#include <_g_t_s_a_m_/_b_a_s_e_/_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h> │ │ │ │ │ +44#include <_g_t_s_a_m_/_b_a_s_e_/_P_r_o_d_u_c_t_L_i_e_G_r_o_u_p_._h> │ │ │ │ │ +45#include <_g_t_s_a_m_/_b_a_s_e_/_s_e_r_i_a_l_i_z_a_t_i_o_n_._h> │ │ │ │ │ +46#include <_g_t_s_a_m_/_b_a_s_e_/_s_e_r_i_a_l_i_z_a_t_i_o_n_T_e_s_t_H_e_l_p_e_r_s_._h> │ │ │ │ │ +47#include <_g_t_s_a_m_/_b_a_s_e_/_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_._h> │ │ │ │ │ +48#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +49#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_A_s_s_e_r_t_i_o_n_s_._h> │ │ │ │ │ +50#include <_g_t_s_a_m_/_b_a_s_e_/_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_._h> │ │ │ │ │ +51#include <_g_t_s_a_m_/_b_a_s_e_/_t_i_m_i_n_g_._h> │ │ │ │ │ +52#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ +53#include <_g_t_s_a_m_/_b_a_s_e_/_V_a_l_u_e_._h> │ │ │ │ │ +54#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_c_t_o_r_._h> │ │ │ │ │ +55#include │ │ │ │ │ +56#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_._h> │ │ │ │ │ +57 │ │ │ │ │ 58 │ │ │ │ │ -59 friend class ::EliminationTreeTester; │ │ │ │ │ -60 │ │ │ │ │ -61 }; │ │ │ │ │ -62 │ │ │ │ │ -63} │ │ │ │ │ -_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h │ │ │ │ │ -_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h │ │ │ │ │ -Chordal Bayes Net, the result of eliminating a factor graph. │ │ │ │ │ -_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ -_g_t_s_a_m │ │ │ │ │ -Global functions in a separate testing namespace. │ │ │ │ │ -DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ -An elimination tree is a data structure used intermediately during elimination. │ │ │ │ │ -DDeeffiinniittiioonn EliminationTree.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ -The VariableIndex class computes and stores the block column structure of a │ │ │ │ │ -factor graph. │ │ │ │ │ -DDeeffiinniittiioonn VariableIndex.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ -DDeeffiinniittiioonn GaussianEliminationTree.h:29 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_T_h_i_s │ │ │ │ │ -GaussianEliminationTree This │ │ │ │ │ -This class. │ │ │ │ │ -DDeeffiinniittiioonn GaussianEliminationTree.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_B_a_s_e │ │ │ │ │ -EliminationTree< GaussianBayesNet, GaussianFactorGraph > Base │ │ │ │ │ -Base class. │ │ │ │ │ -DDeeffiinniittiioonn GaussianEliminationTree.h:31 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -Shared pointer to this class. │ │ │ │ │ -DDeeffiinniittiioonn GaussianEliminationTree.h:33 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ +_F_a_s_t_D_e_f_a_u_l_t_A_l_l_o_c_a_t_o_r_._h │ │ │ │ │ +An easy way to control which allocator is used for Fast* collections. │ │ │ │ │ +_V_e_c_t_o_r_._h │ │ │ │ │ +typedef and functions to augment Eigen's VectorXd │ │ │ │ │ +_s_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ +Convenience functions for serializing data structures via boost.serialization. │ │ │ │ │ +_d_e_b_u_g_._h │ │ │ │ │ +Global debugging flags. │ │ │ │ │ +_s_e_r_i_a_l_i_z_a_t_i_o_n_T_e_s_t_H_e_l_p_e_r_s_._h │ │ │ │ │ +_F_a_s_t_M_a_p_._h │ │ │ │ │ +A thin wrapper around std::map that uses boost's fast_pool_allocator. │ │ │ │ │ +_F_a_s_t_S_e_t_._h │ │ │ │ │ +A thin wrapper around std::set that uses boost's fast_pool_allocator. │ │ │ │ │ +_F_a_s_t_L_i_s_t_._h │ │ │ │ │ +A thin wrapper around std::list that uses boost's fast_pool_allocator. │ │ │ │ │ +_V_a_l_u_e_._h │ │ │ │ │ +The base class for any variable that can be optimized or used in a factor. │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ +_F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ +A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ +_G_r_o_u_p_._h │ │ │ │ │ +Concept check class for variable types with Group properties. │ │ │ │ │ +_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h │ │ │ │ │ +Special class for optional Jacobian arguments. │ │ │ │ │ +_T_h_r_e_a_d_s_a_f_e_E_x_c_e_p_t_i_o_n_._h │ │ │ │ │ +Base exception type that uses tbb_allocator if GTSAM is compiled with TBB. │ │ │ │ │ +_M_a_t_r_i_x_._h │ │ │ │ │ +typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +_c_h_o_l_e_s_k_y_._h │ │ │ │ │ +Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained │ │ │ │ │ +Cholesky. │ │ │ │ │ +_M_a_n_i_f_o_l_d_._h │ │ │ │ │ +Base class and basic functions for Manifold types. │ │ │ │ │ +_P_r_o_d_u_c_t_L_i_e_G_r_o_u_p_._h │ │ │ │ │ +Group product of two Lie Groups. │ │ │ │ │ +_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_._h │ │ │ │ │ +Access to matrices via blocks of pre-defined sizes. │ │ │ │ │ +_t_i_m_i_n_g_._h │ │ │ │ │ +Timing utilities. │ │ │ │ │ +_t_y_p_e_s_._h │ │ │ │ │ +Typedefs for easier changing of types. │ │ │ │ │ +_L_i_e_._h │ │ │ │ │ +Base class and basic functions for Lie types. │ │ │ │ │ +_D_S_F_V_e_c_t_o_r_._h │ │ │ │ │ +A faster implementation for DSF, which uses vector rather than btree. │ │ │ │ │ +_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_._h │ │ │ │ │ +A matrix with column blocks of pre-defined sizes. │ │ │ │ │ +_T_e_s_t_a_b_l_e_A_s_s_e_r_t_i_o_n_s_._h │ │ │ │ │ +Provides additional testing facilities for common data structures. │ │ │ │ │ +_l_i_e_P_r_o_x_i_e_s_._h │ │ │ │ │ +Provides convenient mappings of common member functions for testing. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h │ │ │ │ │ + * _p_r_e_c_o_m_p_i_l_e_d___h_e_a_d_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00725.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00545.html │ │ │ │┄ Files 95% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/RegularJacobianFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridJunctionTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
RegularJacobianFactor.h File Reference
│ │ │ │ +
HybridJunctionTree.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

JacobianFactor class with fixed sized blcoks. │ │ │ │ -More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::RegularJacobianFactor< D >
 JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator. More...
class  gtsam::HybridJunctionTree
 An EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree, with the additional property that it represents the clique tree associated with a Bayes net. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

JacobianFactor class with fixed sized blcoks.

│ │ │ │ -
Author
Sungtae An
│ │ │ │ -
Date
Nov 11, 2014
│ │ │ │ +
Date
Mar 11, 2022
│ │ │ │ +
Author
Fan Jiang
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,27 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -RegularJacobianFactor.h File Reference │ │ │ │ │ -_J_a_c_o_b_i_a_n_F_a_c_t_o_r class with fixed sized blcoks. _M_o_r_e_._._. │ │ │ │ │ +HybridJunctionTree.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_<_ _D_ _> │ │ │ │ │ -  _J_a_c_o_b_i_a_n_F_a_c_t_o_r with constant sized blocks Provides raw memory access │ │ │ │ │ - versions of linear operator. _M_o_r_e_._._. │ │ │ │ │ +class   _g_t_s_a_m_:_:_H_y_b_r_i_d_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ + An _E_l_i_m_i_n_a_t_a_b_l_e_C_l_u_s_t_e_r_T_r_e_e, i.e., a set of variable clusters with │ │ │ │ │ +  factors, arranged in a tree, with the additional property that it │ │ │ │ │ + represents the clique tree associated with a Bayes net. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -_J_a_c_o_b_i_a_n_F_a_c_t_o_r class with fixed sized blcoks. │ │ │ │ │ - Author │ │ │ │ │ - Sungtae An │ │ │ │ │ Date │ │ │ │ │ - Nov 11, 2014 │ │ │ │ │ + Mar 11, 2022 │ │ │ │ │ + Author │ │ │ │ │ + Fan Jiang │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _l_i_n_e_a_r │ │ │ │ │ - * _R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ + * _h_y_b_r_i_d │ │ │ │ │ + * _H_y_b_r_i_d_J_u_n_c_t_i_o_n_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00734.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00200.html │ │ │ │┄ Files 94% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianBayesTree.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteEliminationTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
GaussianBayesTree.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
DiscreteEliminationTree.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ -More...

│ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::DiscreteEliminationTree
 Elimination tree for discrete factors. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -LogDeterminantData & gtsam::internal::logDeterminant (const GaussianBayesTreeClique::shared_ptr &clique, LogDeterminantData &parentSum)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.

│ │ │ │ -

GaussianBayesTree

Author
Frank Dellaert
│ │ │ │ +
Date
Mar 29, 2013
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │ Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,27 +1,26 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -GaussianBayesTree.cpp File Reference │ │ │ │ │ -Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +DiscreteEliminationTree.h File Reference │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ +  Elimination tree for discrete factors. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -LogDeterminantData &  ggttssaamm::::iinntteerrnnaall::::llooggDDeetteerrmmiinnaanntt (const │ │ │ │ │ - GaussianBayesTreeClique::shared_ptr &clique, │ │ │ │ │ - LogDeterminantData &parentSum) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ │ -GaussianBayesTree │ │ │ │ │ + Date │ │ │ │ │ + Mar 29, 2013 │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._c_p_p │ │ │ │ │ + * _d_i_s_c_r_e_t_e │ │ │ │ │ + * _D_i_s_c_r_e_t_e_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00737.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00266.html │ │ │ │┄ Files 94% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianConditional-inl.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/DiscreteBayesTree.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces
│ │ │ │ -
GaussianConditional-inl.h File Reference
│ │ │ │ +
DiscreteBayesTree.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Conditional Gaussian Base class. │ │ │ │ +

Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Conditional Gaussian Base class.

│ │ │ │ -
Author
Christian Potthast
│ │ │ │ +

Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree.

│ │ │ │ +

DiscreteBayesTree

Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,22 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -GaussianConditional-inl.h File Reference │ │ │ │ │ -Conditional Gaussian Base class. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +DiscreteBayesTree.cpp File Reference │ │ │ │ │ +Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Conditional Gaussian Base class. │ │ │ │ │ +Discrete Bayes Tree, the result of eliminating a DiscreteJunctionTree. │ │ │ │ │ +DiscreteBayesTree │ │ │ │ │ Author │ │ │ │ │ - Christian Potthast │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_-_i_n_l_._h │ │ │ │ │ + * _d_i_s_c_r_e_t_e │ │ │ │ │ + * _D_i_s_c_r_e_t_e_B_a_y_e_s_T_r_e_e_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00749.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00290.html │ │ │ │┄ Files 93% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/linearExceptions.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/PinholeSet.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
linearExceptions.h File Reference
│ │ │ │ +
PinholeSet.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Exceptions that may be thrown by linear solver components. │ │ │ │ +

A CameraSet of either CalibratedCamera, PinholePose, or PinholeCamera. │ │ │ │ More...

│ │ │ │ │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::IndeterminantLinearSystemException
 Thrown when a linear system is ill-posed. More...
class  gtsam::PinholeSet< CAMERA >
 PinholeSet: triangulates point and keeps an estimate of it around. More...
 
class  gtsam::InvalidNoiseModel
 An exception indicating that the noise model dimension passed into a JacobianFactor has a different dimensionality than the factor. More...
struct  gtsam::traits< PinholeSet< CAMERA > >
 
class  gtsam::InvalidMatrixBlock
 An exception indicating that a matrix block passed into a JacobianFactor has a different dimensionality than the factor. More...
 
class  gtsam::InvalidDenseElimination
struct  gtsam::traits< const PinholeSet< CAMERA > >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Exceptions that may be thrown by linear solver components.

│ │ │ │ -
Author
Richard Roberts
│ │ │ │ -
Date
Aug 17, 2012
│ │ │ │ +

A CameraSet of either CalibratedCamera, PinholePose, or PinholeCamera.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,37 +1,30 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -linearExceptions.h File Reference │ │ │ │ │ -Exceptions that may be thrown by linear solver components. _M_o_r_e_._._. │ │ │ │ │ +PinholeSet.h File Reference │ │ │ │ │ +A CameraSet of either CalibratedCamera, PinholePose, or PinholeCamera. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_I_n_d_e_t_e_r_m_i_n_a_n_t_L_i_n_e_a_r_S_y_s_t_e_m_E_x_c_e_p_t_i_o_n │ │ │ │ │ -  Thrown when a linear system is ill-posed. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_P_i_n_h_o_l_e_S_e_t_<_ _C_A_M_E_R_A_ _> │ │ │ │ │ +  _P_i_n_h_o_l_e_S_e_t: triangulates point and keeps an estimate of it around. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -class   _g_t_s_a_m_:_:_I_n_v_a_l_i_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -  An exception indicating that the noise model dimension passed into a │ │ │ │ │ - _J_a_c_o_b_i_a_n_F_a_c_t_o_r has a different dimensionality than the factor. _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _P_i_n_h_o_l_e_S_e_t_<_ _C_A_M_E_R_A_ _>_ _> │ │ │ │ │   │ │ │ │ │ -class   _g_t_s_a_m_:_:_I_n_v_a_l_i_d_M_a_t_r_i_x_B_l_o_c_k │ │ │ │ │ -  An exception indicating that a matrix block passed into a │ │ │ │ │ - _J_a_c_o_b_i_a_n_F_a_c_t_o_r has a different dimensionality than the factor. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_I_n_v_a_l_i_d_D_e_n_s_e_E_l_i_m_i_n_a_t_i_o_n │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _c_o_n_s_t_ _P_i_n_h_o_l_e_S_e_t_<_ _C_A_M_E_R_A_ _>_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Exceptions that may be thrown by linear solver components. │ │ │ │ │ +A CameraSet of either CalibratedCamera, PinholePose, or PinholeCamera. │ │ │ │ │ Author │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Date │ │ │ │ │ - Aug 17, 2012 │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _l_i_n_e_a_r │ │ │ │ │ - * _l_i_n_e_a_r_E_x_c_e_p_t_i_o_n_s_._h │ │ │ │ │ + * _g_e_o_m_e_t_r_y │ │ │ │ │ + * _P_i_n_h_o_l_e_S_e_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00749_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00887_source.html │ │ │ │┄ Files 99% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
linearExceptions.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
18#pragma once
│ │ │ │
19
│ │ │ │ -
20#include <gtsam/base/ThreadsafeException.h>
│ │ │ │ -
21#include <gtsam/base/types.h>
│ │ │ │ +
20#include <gtsam/base/ThreadsafeException.h>
│ │ │ │ +
21#include <gtsam/base/types.h>
│ │ │ │
22
│ │ │ │
23namespace gtsam {
│ │ │ │
24
│ │ │ │
│ │ │ │
94 class GTSAM_EXPORT IndeterminantLinearSystemException : public ThreadsafeException<IndeterminantLinearSystemException> {
│ │ │ │
95 Key j_;
│ │ │ │
96 public:
│ │ │ │ @@ -175,16 +175,16 @@ │ │ │ │
140 class InvalidDenseElimination : public ThreadsafeException<InvalidDenseElimination> {
│ │ │ │
141 public:
│ │ │ │
142 InvalidDenseElimination(const char *message) : ThreadsafeException<InvalidDenseElimination>(message) {}
│ │ │ │
143 };
│ │ │ │
│ │ │ │
144
│ │ │ │
145 }
│ │ │ │ -
ThreadsafeException.h
Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
│ │ │ │ -
types.h
Typedefs for easier changing of types.
│ │ │ │ +
ThreadsafeException.h
Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
│ │ │ │ +
types.h
Typedefs for easier changing of types.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::ThreadsafeException
Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
Definition ThreadsafeException.h:42
│ │ │ │
gtsam::IndeterminantLinearSystemException
Thrown when a linear system is ill-posed.
Definition linearExceptions.h:94
│ │ │ │
gtsam::InvalidNoiseModel
An exception indicating that the noise model dimension passed into a JacobianFactor has a different d...
Definition linearExceptions.h:106
│ │ │ │
gtsam::InvalidNoiseModel::factorDims
const DenseIndex factorDims
The dimensionality of the factor.
Definition linearExceptions.h:108
│ │ │ │ @@ -194,13 +194,13 @@ │ │ │ │
gtsam::InvalidMatrixBlock::blockRows
const DenseIndex blockRows
The dimensionality of the noise model.
Definition linearExceptions.h:127
│ │ │ │
gtsam::InvalidDenseElimination
Definition linearExceptions.h:140
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00755.js │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00824.js │ │ │ │┄ Files 95% similar despite different names │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ -var a00755 = [ │ │ │ │ │ +var a00824 = [ │ │ │ │ │ ["gtsam::noiseModel::Base", "a03920.html", "a03920"], │ │ │ │ │ ["gtsam::noiseModel::Gaussian", "a03924.html", "a03924"], │ │ │ │ │ ["gtsam::noiseModel::Diagonal", "a03928.html", "a03928"], │ │ │ │ │ ["gtsam::noiseModel::Constrained", "a03932.html", "a03932"], │ │ │ │ │ ["gtsam::noiseModel::Isotropic", "a03936.html", "a03936"], │ │ │ │ │ ["gtsam::noiseModel::Unit", "a03940.html", "a03940"], │ │ │ │ │ ["gtsam::noiseModel::Robust", "a03944.html", "a03944"], │ │ │ │ │ ["gtsam::traits< noiseModel::Gaussian >", "a03948.html", null], │ │ │ │ │ ["gtsam::traits< noiseModel::Diagonal >", "a03952.html", null], │ │ │ │ │ ["gtsam::traits< noiseModel::Constrained >", "a03956.html", null], │ │ │ │ │ ["gtsam::traits< noiseModel::Isotropic >", "a03960.html", null], │ │ │ │ │ ["gtsam::traits< noiseModel::Unit >", "a03964.html", null], │ │ │ │ │ - ["SharedNoiseModel", "a00755.html#ab6e5a4884342656e0837ef07008ec03f", null] │ │ │ │ │ + ["SharedNoiseModel", "a00824.html#ab6e5a4884342656e0837ef07008ec03f", null] │ │ │ │ │ ]; │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00755_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00338_source.html │ │ │ │┄ Files 80% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/NoiseModel.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SOn.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
NoiseModel.h
│ │ │ │ +
SOn.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/base/Testable.h>
│ │ │ │ -
22#include <gtsam/base/Matrix.h>
│ │ │ │ -
23#include <gtsam/dllexport.h>
│ │ │ │ -
24#include <gtsam/linear/LossFunctions.h>
│ │ │ │ -
25
│ │ │ │ -
26#include <boost/serialization/nvp.hpp>
│ │ │ │ -
27#include <boost/serialization/extended_type_info.hpp>
│ │ │ │ -
28#include <boost/serialization/singleton.hpp>
│ │ │ │ -
29#include <boost/serialization/shared_ptr.hpp>
│ │ │ │ -
30#include <boost/serialization/optional.hpp>
│ │ │ │ -
31
│ │ │ │ -
32namespace gtsam {
│ │ │ │ -
33
│ │ │ │ -
35 namespace noiseModel {
│ │ │ │ +
21#include <gtsam/base/Lie.h>
│ │ │ │ +
22#include <gtsam/base/Manifold.h>
│ │ │ │ +
23#include <gtsam/base/make_shared.h>
│ │ │ │ +
24#include <gtsam/dllexport.h>
│ │ │ │ +
25#include <Eigen/Core>
│ │ │ │ +
26
│ │ │ │ +
27#include <boost/serialization/nvp.hpp>
│ │ │ │ +
28
│ │ │ │ +
29#include <iostream> // TODO(frank): how to avoid?
│ │ │ │ +
30#include <string>
│ │ │ │ +
31#include <type_traits>
│ │ │ │ +
32#include <vector>
│ │ │ │ +
33#include <random>
│ │ │ │ +
34
│ │ │ │ +
35namespace gtsam {
│ │ │ │
36
│ │ │ │ -
37 // Forward declaration
│ │ │ │ -
38 class Gaussian;
│ │ │ │ -
39 class Diagonal;
│ │ │ │ -
40 class Constrained;
│ │ │ │ -
41 class Isotropic;
│ │ │ │ -
42 class Unit;
│ │ │ │ -
43 class RobustModel;
│ │ │ │ -
44
│ │ │ │ -
45 //---------------------------------------------------------------------------------------
│ │ │ │ +
37namespace internal {
│ │ │ │ +
│ │ │ │ +
39constexpr int DimensionSO(int N) {
│ │ │ │ +
40 return (N < 0) ? Eigen::Dynamic : N * (N - 1) / 2;
│ │ │ │ +
41}
│ │ │ │ +
│ │ │ │ +
42
│ │ │ │ +
43// Calculate N^2 at compile time, or return Dynamic if so
│ │ │ │ +
44constexpr int NSquaredSO(int N) { return (N < 0) ? Eigen::Dynamic : N * N; }
│ │ │ │ +
45} // namespace internal
│ │ │ │
46
│ │ │ │ -
│ │ │ │ -
53 class GTSAM_EXPORT Base {
│ │ │ │ -
54
│ │ │ │ -
55 public:
│ │ │ │ -
56 typedef boost::shared_ptr<Base> shared_ptr;
│ │ │ │ -
57
│ │ │ │ -
58 protected:
│ │ │ │ -
59
│ │ │ │ -
60 size_t dim_;
│ │ │ │ -
61
│ │ │ │ -
62 public:
│ │ │ │ +
51template <int N>
│ │ │ │ +
│ │ │ │ +
52class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
│ │ │ │ +
53 public:
│ │ │ │ +
54 enum { dimension = internal::DimensionSO(N) };
│ │ │ │ +
55 using MatrixNN = Eigen::Matrix<double, N, N>;
│ │ │ │ +
56 using VectorN2 = Eigen::Matrix<double, internal::NSquaredSO(N), 1>;
│ │ │ │ +
57 using MatrixDD = Eigen::Matrix<double, dimension, dimension>;
│ │ │ │ +
58
│ │ │ │ +
59 GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(true)
│ │ │ │ +
60
│ │ │ │ +
61 protected:
│ │ │ │ +
62 MatrixNN matrix_;
│ │ │ │
63
│ │ │ │ -
65 Base(size_t dim = 1):dim_(dim) {}
│ │ │ │ -
66 virtual ~Base() {}
│ │ │ │ -
67
│ │ │ │ -
69 virtual bool isConstrained() const { return false; } // default false
│ │ │ │ -
70
│ │ │ │ -
72 virtual bool isUnit() const { return false; } // default false
│ │ │ │ -
73
│ │ │ │ -
75 inline size_t dim() const { return dim_;}
│ │ │ │ +
64 // enable_if_t aliases, used to specialize constructors/methods, see
│ │ │ │ +
65 // https://www.fluentcpp.com/2018/05/18/make-sfinae-pretty-2-hidden-beauty-sfinae/
│ │ │ │ +
66 template <int N_>
│ │ │ │ +
67 using IsDynamic = typename std::enable_if<N_ == Eigen::Dynamic, void>::type;
│ │ │ │ +
68 template <int N_>
│ │ │ │ +
69 using IsFixed = typename std::enable_if<N_ >= 2, void>::type;
│ │ │ │ +
70 template <int N_>
│ │ │ │ +
71 using IsSO3 = typename std::enable_if<N_ == 3, void>::type;
│ │ │ │ +
72
│ │ │ │ +
73 public:
│ │ │ │
76
│ │ │ │ -
77 virtual void print(const std::string& name = "") const = 0;
│ │ │ │ -
78
│ │ │ │ -
79 virtual bool equals(const Base& expected, double tol=1e-9) const = 0;
│ │ │ │ +
78 template <int N_ = N, typename = IsFixed<N_>>
│ │ │ │ +
79 SO() : matrix_(MatrixNN::Identity()) {}
│ │ │ │
80
│ │ │ │ -
82 virtual Vector sigmas() const;
│ │ │ │ -
83
│ │ │ │ -
85 virtual Vector whiten(const Vector& v) const = 0;
│ │ │ │ -
86
│ │ │ │ -
88 virtual Matrix Whiten(const Matrix& H) const = 0;
│ │ │ │ -
89
│ │ │ │ -
91 virtual Vector unwhiten(const Vector& v) const = 0;
│ │ │ │ +
82 template <int N_ = N, typename = IsDynamic<N_>>
│ │ │ │ +
│ │ │ │ +
83 explicit SO(size_t n = 0) {
│ │ │ │ +
84 // We allow for n=0 as the default constructor, needed for serialization,
│ │ │ │ +
85 // wrappers etc.
│ │ │ │ +
86 matrix_ = Eigen::MatrixXd::Identity(n, n);
│ │ │ │ +
87 }
│ │ │ │ +
│ │ │ │ +
88
│ │ │ │ +
90 template <typename Derived>
│ │ │ │ +
91 explicit SO(const Eigen::MatrixBase<Derived>& R) : matrix_(R.eval()) {}
│ │ │ │
92
│ │ │ │ -
94 virtual double squaredMahalanobisDistance(const Vector& v) const;
│ │ │ │ -
95
│ │ │ │ -
│ │ │ │ -
97 virtual double mahalanobisDistance(const Vector& v) const {
│ │ │ │ -
98 return std::sqrt(squaredMahalanobisDistance(v));
│ │ │ │ -
99 }
│ │ │ │ -
│ │ │ │ -
100
│ │ │ │ -
│ │ │ │ -
102 virtual double loss(const double squared_distance) const {
│ │ │ │ -
103 return 0.5 * squared_distance;
│ │ │ │ -
104 }
│ │ │ │ -
│ │ │ │ -
105
│ │ │ │ -
106 virtual void WhitenSystem(std::vector<Matrix>& A, Vector& b) const = 0;
│ │ │ │ -
107 virtual void WhitenSystem(Matrix& A, Vector& b) const = 0;
│ │ │ │ -
108 virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const = 0;
│ │ │ │ -
109 virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const = 0;
│ │ │ │ -
110
│ │ │ │ -
│ │ │ │ -
112 virtual void whitenInPlace(Vector& v) const {
│ │ │ │ -
113 v = whiten(v);
│ │ │ │ -
114 }
│ │ │ │ -
│ │ │ │ -
115
│ │ │ │ -
│ │ │ │ -
117 virtual void unwhitenInPlace(Vector& v) const {
│ │ │ │ -
118 v = unwhiten(v);
│ │ │ │ -
119 }
│ │ │ │ -
│ │ │ │ -
120
│ │ │ │ -
│ │ │ │ -
122 virtual void whitenInPlace(Eigen::Block<Vector>& v) const {
│ │ │ │ -
123 v = whiten(v);
│ │ │ │ -
124 }
│ │ │ │ -
│ │ │ │ -
125
│ │ │ │ -
│ │ │ │ -
127 virtual void unwhitenInPlace(Eigen::Block<Vector>& v) const {
│ │ │ │ -
128 v = unwhiten(v);
│ │ │ │ -
129 }
│ │ │ │ -
│ │ │ │ -
130
│ │ │ │ -
│ │ │ │ -
132 virtual Vector unweightedWhiten(const Vector& v) const {
│ │ │ │ -
133 return whiten(v);
│ │ │ │ -
134 }
│ │ │ │ -
│ │ │ │ -
135
│ │ │ │ -
137 virtual double weight(const Vector& v) const { return 1.0; }
│ │ │ │ -
138
│ │ │ │ -
139 private:
│ │ │ │ -
141 friend class boost::serialization::access;
│ │ │ │ -
142 template<class ARCHIVE>
│ │ │ │ -
143 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
144 ar & BOOST_SERIALIZATION_NVP(dim_);
│ │ │ │ -
145 }
│ │ │ │ -
146 };
│ │ │ │ +
94 template <typename Derived>
│ │ │ │ +
│ │ │ │ +
95 static SO FromMatrix(const Eigen::MatrixBase<Derived>& R) {
│ │ │ │ +
96 return SO(R);
│ │ │ │ +
97 }
│ │ │ │ +
│ │ │ │ +
98
│ │ │ │ +
100 template <typename Derived, int N_ = N, typename = IsDynamic<N_>>
│ │ │ │ +
│ │ │ │ +
101 static SO Lift(size_t n, const Eigen::MatrixBase<Derived> &R) {
│ │ │ │ +
102 Matrix Q = Matrix::Identity(n, n);
│ │ │ │ +
103 const int p = R.rows();
│ │ │ │ +
104 assert(p >= 0 && p <= static_cast<int>(n) && R.cols() == p);
│ │ │ │ +
105 Q.topLeftCorner(p, p) = R;
│ │ │ │ +
106 return SO(Q);
│ │ │ │ +
107 }
│ │ │ │ +
│ │ │ │ +
108
│ │ │ │ +
110 template <int M, int N_ = N, typename = IsDynamic<N_>>
│ │ │ │ +
111 explicit SO(const SO<M>& R) : matrix_(R.matrix()) {}
│ │ │ │ +
112
│ │ │ │ +
114 template <int N_ = N, typename = IsSO3<N_>>
│ │ │ │ +
115 explicit SO(const Eigen::AngleAxisd& angleAxis) : matrix_(angleAxis) {}
│ │ │ │ +
116
│ │ │ │ +
118 static SO AxisAngle(const Vector3& axis, double theta);
│ │ │ │ +
119
│ │ │ │ +
122 static SO ClosestTo(const MatrixNN& M);
│ │ │ │ +
123
│ │ │ │ +
127 static SO ChordalMean(const std::vector<SO>& rotations);
│ │ │ │ +
128
│ │ │ │ +
130 template <int N_ = N, typename = IsDynamic<N_>>
│ │ │ │ +
│ │ │ │ +
131 static SO Random(std::mt19937& rng, size_t n = 0) {
│ │ │ │ +
132 if (n == 0) throw std::runtime_error("SO: Dimensionality not known.");
│ │ │ │ +
133 // TODO(frank): this might need to be re-thought
│ │ │ │ +
134 static std::uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
│ │ │ │ +
135 const size_t d = SO::Dimension(n);
│ │ │ │ +
136 Vector xi(d);
│ │ │ │ +
137 for (size_t j = 0; j < d; j++) {
│ │ │ │ +
138 xi(j) = randomAngle(rng);
│ │ │ │ +
139 }
│ │ │ │ +
140 return SO::Retract(xi);
│ │ │ │ +
141 }
│ │ │ │ +
│ │ │ │ +
142
│ │ │ │ +
144 template <int N_ = N, typename = IsFixed<N_>>
│ │ │ │ +
│ │ │ │ +
145 static SO Random(std::mt19937& rng) {
│ │ │ │ +
146 // By default, use dynamic implementation above. Specialized for SO(3).
│ │ │ │ +
147 return SO(SO<Eigen::Dynamic>::Random(rng, N).matrix());
│ │ │ │ +
148 }
│ │ │ │
│ │ │ │ -
147
│ │ │ │ -
148 //---------------------------------------------------------------------------------------
│ │ │ │
149
│ │ │ │ -
│ │ │ │ -
162 class GTSAM_EXPORT Gaussian: public Base {
│ │ │ │ +
153
│ │ │ │ +
155 const MatrixNN& matrix() const { return matrix_; }
│ │ │ │ +
156
│ │ │ │ +
157 size_t rows() const { return matrix_.rows(); }
│ │ │ │ +
158 size_t cols() const { return matrix_.cols(); }
│ │ │ │ +
159
│ │ │ │
163
│ │ │ │ -
164 protected:
│ │ │ │ +
164 void print(const std::string& s = std::string()) const;
│ │ │ │
165
│ │ │ │ -
167 boost::optional<Matrix> sqrt_information_;
│ │ │ │ -
168
│ │ │ │ -
169 private:
│ │ │ │ -
170
│ │ │ │ -
174 const Matrix& thisR() const {
│ │ │ │ -
175 // should never happen
│ │ │ │ -
176 if (!sqrt_information_) throw std::runtime_error("Gaussian: has no R matrix");
│ │ │ │ -
177 return *sqrt_information_;
│ │ │ │ -
178 }
│ │ │ │ +
166 bool equals(const SO& other, double tol) const {
│ │ │ │ +
167 return equal_with_abs_tol(matrix_, other.matrix_, tol);
│ │ │ │ +
168 }
│ │ │ │ +
169
│ │ │ │ +
173
│ │ │ │ +
│ │ │ │ +
175 SO operator*(const SO& other) const {
│ │ │ │ +
176 assert(dim() == other.dim());
│ │ │ │ +
177 return SO(matrix_ * other.matrix_);
│ │ │ │ +
178 }
│ │ │ │ +
│ │ │ │
179
│ │ │ │ -
180
│ │ │ │ -
181 public:
│ │ │ │ -
182
│ │ │ │ -
183 typedef boost::shared_ptr<Gaussian> shared_ptr;
│ │ │ │ -
184
│ │ │ │ -
│ │ │ │ -
186 Gaussian(size_t dim = 1,
│ │ │ │ -
187 const boost::optional<Matrix>& sqrt_information = boost::none)
│ │ │ │ -
188 : Base(dim), sqrt_information_(sqrt_information) {}
│ │ │ │ +
181 template <int N_ = N, typename = IsFixed<N_>>
│ │ │ │ +
│ │ │ │ +
182 static SO Identity() {
│ │ │ │ +
183 return SO();
│ │ │ │ +
184 }
│ │ │ │ +
│ │ │ │ +
185
│ │ │ │ +
187 template <int N_ = N, typename = IsDynamic<N_>>
│ │ │ │ +
│ │ │ │ +
188 static SO Identity(size_t n = 0) {
│ │ │ │ +
189 return SO(n);
│ │ │ │ +
190 }
│ │ │ │
│ │ │ │ -
189
│ │ │ │ -
190 ~Gaussian() override {}
│ │ │ │
191
│ │ │ │ -
197 static shared_ptr SqrtInformation(const Matrix& R, bool smart = true);
│ │ │ │ +
193 SO inverse() const { return SO(matrix_.transpose()); }
│ │ │ │ +
194
│ │ │ │
198
│ │ │ │ -
204 static shared_ptr Information(const Matrix& M, bool smart = true);
│ │ │ │ -
205
│ │ │ │ -
211 static shared_ptr Covariance(const Matrix& covariance, bool smart = true);
│ │ │ │ -
212
│ │ │ │ -
213 void print(const std::string& name) const override;
│ │ │ │ -
214 bool equals(const Base& expected, double tol=1e-9) const override;
│ │ │ │ -
215 Vector sigmas() const override;
│ │ │ │ -
216 Vector whiten(const Vector& v) const override;
│ │ │ │ -
217 Vector unwhiten(const Vector& v) const override;
│ │ │ │ -
218
│ │ │ │ -
223 Matrix Whiten(const Matrix& H) const override;
│ │ │ │ -
224
│ │ │ │ -
228 virtual void WhitenInPlace(Matrix& H) const;
│ │ │ │ -
229
│ │ │ │ -
233 virtual void WhitenInPlace(Eigen::Block<Matrix> H) const;
│ │ │ │ -
234
│ │ │ │ -
238 void WhitenSystem(std::vector<Matrix>& A, Vector& b) const override;
│ │ │ │ -
239 void WhitenSystem(Matrix& A, Vector& b) const override;
│ │ │ │ -
240 void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const override;
│ │ │ │ -
241 void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const override;
│ │ │ │ -
242
│ │ │ │ -
252 virtual boost::shared_ptr<Diagonal> QR(Matrix& Ab) const;
│ │ │ │ -
253
│ │ │ │ -
255 virtual Matrix R() const { return thisR();}
│ │ │ │ -
256
│ │ │ │ -
258 virtual Matrix information() const;
│ │ │ │ +
199 using TangentVector = Eigen::Matrix<double, dimension, 1>;
│ │ │ │ +
200 using ChartJacobian = OptionalJacobian<dimension, dimension>;
│ │ │ │ +
201
│ │ │ │ +
203 static int Dim() { return dimension; }
│ │ │ │ +
204
│ │ │ │ +
205 // Calculate manifold dimensionality for SO(n).
│ │ │ │ +
206 // Available as dimension or Dim() for fixed N.
│ │ │ │ +
207 static size_t Dimension(size_t n) { return n * (n - 1) / 2; }
│ │ │ │ +
208
│ │ │ │ +
209 // Calculate ambient dimension n from manifold dimensionality d.
│ │ │ │ +
210 static size_t AmbientDim(size_t d) { return (1 + std::sqrt(1 + 8 * d)) / 2; }
│ │ │ │ +
211
│ │ │ │ +
212 // Calculate run-time dimensionality of manifold.
│ │ │ │ +
213 // Available as dimension or Dim() for fixed N.
│ │ │ │ +
214 size_t dim() const { return Dimension(static_cast<size_t>(matrix_.rows())); }
│ │ │ │ +
215
│ │ │ │ +
231 static MatrixNN Hat(const TangentVector& xi);
│ │ │ │ +
232
│ │ │ │ +
234 static void Hat(const Vector &xi, Eigen::Ref<MatrixNN> X);
│ │ │ │ +
235
│ │ │ │ +
237 static TangentVector Vee(const MatrixNN& X);
│ │ │ │ +
238
│ │ │ │ +
239 // Chart at origin
│ │ │ │ +
│ │ │ │ +
240 struct ChartAtOrigin {
│ │ │ │ +
245 static SO Retract(const TangentVector& xi, ChartJacobian H = boost::none);
│ │ │ │ +
246
│ │ │ │ +
250 static TangentVector Local(const SO& R, ChartJacobian H = boost::none);
│ │ │ │ +
251 };
│ │ │ │ +
│ │ │ │ +
252
│ │ │ │ +
253 // Return dynamic identity DxD Jacobian for given SO(n)
│ │ │ │ +
254 template <int N_ = N, typename = IsDynamic<N_>>
│ │ │ │ +
255 static MatrixDD IdentityJacobian(size_t n) {
│ │ │ │ +
256 const size_t d = Dimension(n);
│ │ │ │ +
257 return MatrixDD::Identity(d, d);
│ │ │ │ +
258 }
│ │ │ │
259
│ │ │ │ -
261 virtual Matrix covariance() const;
│ │ │ │ -
262
│ │ │ │ -
263 private:
│ │ │ │ -
265 friend class boost::serialization::access;
│ │ │ │ -
266 template<class ARCHIVE>
│ │ │ │ -
267 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
268 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
269 ar & BOOST_SERIALIZATION_NVP(sqrt_information_);
│ │ │ │ -
270 }
│ │ │ │ +
263
│ │ │ │ +
265 MatrixDD AdjointMap() const;
│ │ │ │ +
266
│ │ │ │ +
270 static SO Expmap(const TangentVector& omega, ChartJacobian H = boost::none);
│ │ │ │
271
│ │ │ │ -
272 }; // Gaussian
│ │ │ │ +
273 static MatrixDD ExpmapDerivative(const TangentVector& omega);
│ │ │ │ +
274
│ │ │ │ +
278 static TangentVector Logmap(const SO& R, ChartJacobian H = boost::none);
│ │ │ │ +
279
│ │ │ │ +
281 static MatrixDD LogmapDerivative(const TangentVector& omega);
│ │ │ │ +
282
│ │ │ │ +
283 // inverse with optional derivative
│ │ │ │ +
284 using LieGroup<SO<N>, internal::DimensionSO(N)>::inverse;
│ │ │ │ +
285
│ │ │ │ +
289
│ │ │ │ +
295 VectorN2 vec(OptionalJacobian<internal::NSquaredSO(N), dimension> H =
│ │ │ │ +
296 boost::none) const;
│ │ │ │ +
297
│ │ │ │ +
299 template <int N_ = N, typename = IsFixed<N_>>
│ │ │ │ +
│ │ │ │ +
300 static Matrix VectorizedGenerators() {
│ │ │ │ +
301 constexpr size_t N2 = static_cast<size_t>(N * N);
│ │ │ │ +
302 Eigen::Matrix<double, N2, dimension> G;
│ │ │ │ +
303 for (size_t j = 0; j < dimension; j++) {
│ │ │ │ +
304 const auto X = Hat(Vector::Unit(dimension, j));
│ │ │ │ +
305 G.col(j) = Eigen::Map<const VectorN2>(X.data());
│ │ │ │ +
306 }
│ │ │ │ +
307 return G;
│ │ │ │ +
308 }
│ │ │ │
│ │ │ │ -
273
│ │ │ │ -
274 //---------------------------------------------------------------------------------------
│ │ │ │ -
275
│ │ │ │ -
│ │ │ │ -
281 class GTSAM_EXPORT Diagonal : public Gaussian {
│ │ │ │ -
282 protected:
│ │ │ │ -
283
│ │ │ │ -
289 Vector sigmas_, invsigmas_, precisions_;
│ │ │ │ -
290
│ │ │ │ -
291 protected:
│ │ │ │ -
292
│ │ │ │ -
294 Diagonal(const Vector& sigmas);
│ │ │ │ -
295
│ │ │ │ -
296 public:
│ │ │ │ -
298 Diagonal();
│ │ │ │ -
299
│ │ │ │ -
300 typedef boost::shared_ptr<Diagonal> shared_ptr;
│ │ │ │ -
301
│ │ │ │ -
302 ~Diagonal() override {}
│ │ │ │ -
303
│ │ │ │ -
308 static shared_ptr Sigmas(const Vector& sigmas, bool smart = true);
│ │ │ │
309
│ │ │ │ -
316 static shared_ptr Variances(const Vector& variances, bool smart = true);
│ │ │ │ -
317
│ │ │ │ -
322 static shared_ptr Precisions(const Vector& precisions, bool smart = true);
│ │ │ │ -
323
│ │ │ │ -
324 void print(const std::string& name) const override;
│ │ │ │ -
325 Vector sigmas() const override { return sigmas_; }
│ │ │ │ -
326 Vector whiten(const Vector& v) const override;
│ │ │ │ -
327 Vector unwhiten(const Vector& v) const override;
│ │ │ │ -
328 Matrix Whiten(const Matrix& H) const override;
│ │ │ │ -
329 void WhitenInPlace(Matrix& H) const override;
│ │ │ │ -
330 void WhitenInPlace(Eigen::Block<Matrix> H) const override;
│ │ │ │ -
331
│ │ │ │ -
335 inline double sigma(size_t i) const { return sigmas_(i); }
│ │ │ │ -
336
│ │ │ │ -
340 inline const Vector& invsigmas() const { return invsigmas_; }
│ │ │ │ -
341 inline double invsigma(size_t i) const {return invsigmas_(i);}
│ │ │ │ -
342
│ │ │ │ -
346 inline const Vector& precisions() const { return precisions_; }
│ │ │ │ -
347 inline double precision(size_t i) const {return precisions_(i);}
│ │ │ │ -
348
│ │ │ │ -
│ │ │ │ -
352 Matrix R() const override {
│ │ │ │ -
353 return invsigmas().asDiagonal();
│ │ │ │ -
354 }
│ │ │ │ -
│ │ │ │ -
355
│ │ │ │ -
356 private:
│ │ │ │ -
358 friend class boost::serialization::access;
│ │ │ │ -
359 template<class ARCHIVE>
│ │ │ │ -
360 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
361 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Gaussian);
│ │ │ │ -
362 ar & BOOST_SERIALIZATION_NVP(sigmas_);
│ │ │ │ -
363 ar & BOOST_SERIALIZATION_NVP(invsigmas_);
│ │ │ │ -
364 }
│ │ │ │ -
365 }; // Diagonal
│ │ │ │ -
│ │ │ │ -
366
│ │ │ │ -
367 //---------------------------------------------------------------------------------------
│ │ │ │ -
368
│ │ │ │ -
│ │ │ │ -
381 class GTSAM_EXPORT Constrained : public Diagonal {
│ │ │ │ -
382 protected:
│ │ │ │ -
383
│ │ │ │ -
384 // Sigmas are contained in the base class
│ │ │ │ -
385 Vector mu_;
│ │ │ │ -
386
│ │ │ │ -
392 Constrained(const Vector& mu, const Vector& sigmas);
│ │ │ │ -
393
│ │ │ │ -
394 public:
│ │ │ │ -
395
│ │ │ │ -
396 typedef boost::shared_ptr<Constrained> shared_ptr;
│ │ │ │ +
311 template <int N_ = N, typename = IsDynamic<N_>>
│ │ │ │ +
│ │ │ │ +
312 static Matrix VectorizedGenerators(size_t n = 0) {
│ │ │ │ +
313 const size_t n2 = n * n, dim = Dimension(n);
│ │ │ │ +
314 Matrix G(n2, dim);
│ │ │ │ +
315 for (size_t j = 0; j < dim; j++) {
│ │ │ │ +
316 const auto X = Hat(Vector::Unit(dim, j));
│ │ │ │ +
317 G.col(j) = Eigen::Map<const Matrix>(X.data(), n2, 1);
│ │ │ │ +
318 }
│ │ │ │ +
319 return G;
│ │ │ │ +
320 }
│ │ │ │ +
│ │ │ │ +
321
│ │ │ │ +
325
│ │ │ │ +
326 template <class Archive>
│ │ │ │ +
327 friend void save(Archive&, SO&, const unsigned int);
│ │ │ │ +
328 template <class Archive>
│ │ │ │ +
329 friend void load(Archive&, SO&, const unsigned int);
│ │ │ │ +
330 template <class Archive>
│ │ │ │ +
331 friend void serialize(Archive&, SO&, const unsigned int);
│ │ │ │ +
332 friend class boost::serialization::access;
│ │ │ │ +
333 friend class Rot3; // for serialize
│ │ │ │ +
334
│ │ │ │ +
336};
│ │ │ │ +
│ │ │ │ +
337
│ │ │ │ +
338using SOn = SO<Eigen::Dynamic>;
│ │ │ │ +
339
│ │ │ │ +
340/*
│ │ │ │ +
341 * Specialize dynamic Hat and Vee, because recursion depends on dynamic nature.
│ │ │ │ +
342 * The definition is in SOn.cpp. Fixed-size SO3 and SO4 have their own version,
│ │ │ │ +
343 * and implementation for other fixed N is in SOn-inl.h.
│ │ │ │ +
344 */
│ │ │ │ +
345
│ │ │ │ +
346template <>
│ │ │ │ +
347GTSAM_EXPORT
│ │ │ │ +
348Matrix SOn::Hat(const Vector& xi);
│ │ │ │ +
349
│ │ │ │ +
350template <>
│ │ │ │ +
351GTSAM_EXPORT
│ │ │ │ +
352Vector SOn::Vee(const Matrix& X);
│ │ │ │ +
353
│ │ │ │ +
354/*
│ │ │ │ +
355 * Specialize dynamic compose and between, because the derivative is unknowable
│ │ │ │ +
356 * by the LieGroup implementations, who return a fixed-size matrix for H2.
│ │ │ │ +
357 */
│ │ │ │ +
358
│ │ │ │ +
359using DynamicJacobian = OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic>;
│ │ │ │ +
360
│ │ │ │ +
361template <>
│ │ │ │ +
362GTSAM_EXPORT
│ │ │ │ +
363SOn LieGroup<SOn, Eigen::Dynamic>::compose(const SOn& g, DynamicJacobian H1,
│ │ │ │ +
364 DynamicJacobian H2) const;
│ │ │ │ +
365
│ │ │ │ +
366template <>
│ │ │ │ +
367GTSAM_EXPORT
│ │ │ │ +
368SOn LieGroup<SOn, Eigen::Dynamic>::between(const SOn& g, DynamicJacobian H1,
│ │ │ │ +
369 DynamicJacobian H2) const;
│ │ │ │ +
370
│ │ │ │ +
371/*
│ │ │ │ +
372 * Specialize dynamic vec.
│ │ │ │ +
373 */
│ │ │ │ +
374template <>
│ │ │ │ +
375GTSAM_EXPORT
│ │ │ │ +
376typename SOn::VectorN2 SOn::vec(DynamicJacobian H) const;
│ │ │ │ +
377
│ │ │ │ +
379template<class Archive>
│ │ │ │ +
│ │ │ │ +
380void serialize(
│ │ │ │ +
381 Archive& ar, SOn& Q,
│ │ │ │ +
382 const unsigned int file_version
│ │ │ │ +
383) {
│ │ │ │ +
384 Matrix& M = Q.matrix_;
│ │ │ │ +
385 ar& BOOST_SERIALIZATION_NVP(M);
│ │ │ │ +
386}
│ │ │ │ +
│ │ │ │ +
387
│ │ │ │ +
388/*
│ │ │ │ +
389 * Define the traits. internal::LieGroup provides both Lie group and Testable
│ │ │ │ +
390 */
│ │ │ │ +
391
│ │ │ │ +
392template <int N>
│ │ │ │ +
393struct traits<SO<N>> : public internal::LieGroup<SO<N>> {};
│ │ │ │ +
394
│ │ │ │ +
395template <int N>
│ │ │ │ +
396struct traits<const SO<N>> : public internal::LieGroup<SO<N>> {};
│ │ │ │
397
│ │ │ │ -
404 Constrained(const Vector& sigmas = Z_1x1);
│ │ │ │ -
405
│ │ │ │ -
406 ~Constrained() override {}
│ │ │ │ -
407
│ │ │ │ -
409 bool isConstrained() const override { return true; }
│ │ │ │ -
410
│ │ │ │ -
412 bool constrained(size_t i) const;
│ │ │ │ -
413
│ │ │ │ -
415 const Vector& mu() const { return mu_; }
│ │ │ │ -
416
│ │ │ │ -
421 static shared_ptr MixedSigmas(const Vector& mu, const Vector& sigmas);
│ │ │ │ -
422
│ │ │ │ -
427 static shared_ptr MixedSigmas(const Vector& sigmas);
│ │ │ │ -
428
│ │ │ │ -
433 static shared_ptr MixedSigmas(double m, const Vector& sigmas);
│ │ │ │ -
434
│ │ │ │ -
439 static shared_ptr MixedVariances(const Vector& mu, const Vector& variances);
│ │ │ │ -
440 static shared_ptr MixedVariances(const Vector& variances);
│ │ │ │ -
441
│ │ │ │ -
446 static shared_ptr MixedPrecisions(const Vector& mu, const Vector& precisions);
│ │ │ │ -
447 static shared_ptr MixedPrecisions(const Vector& precisions);
│ │ │ │ -
448
│ │ │ │ -
454 double squaredMahalanobisDistance(const Vector& v) const override;
│ │ │ │ -
455
│ │ │ │ -
│ │ │ │ -
457 static shared_ptr All(size_t dim) {
│ │ │ │ -
458 return shared_ptr(new Constrained(Vector::Constant(dim, 1000.0), Vector::Constant(dim,0)));
│ │ │ │ -
459 }
│ │ │ │ -
│ │ │ │ -
460
│ │ │ │ -
│ │ │ │ -
462 static shared_ptr All(size_t dim, const Vector& mu) {
│ │ │ │ -
463 return shared_ptr(new Constrained(mu, Vector::Constant(dim,0)));
│ │ │ │ -
464 }
│ │ │ │ -
│ │ │ │ -
465
│ │ │ │ -
│ │ │ │ -
467 static shared_ptr All(size_t dim, double mu) {
│ │ │ │ -
468 return shared_ptr(new Constrained(Vector::Constant(dim, mu), Vector::Constant(dim,0)));
│ │ │ │ -
469 }
│ │ │ │ -
│ │ │ │ -
470
│ │ │ │ -
471 void print(const std::string& name) const override;
│ │ │ │ -
472
│ │ │ │ -
474 Vector whiten(const Vector& v) const override;
│ │ │ │ -
475
│ │ │ │ -
478 Matrix Whiten(const Matrix& H) const override;
│ │ │ │ -
479 void WhitenInPlace(Matrix& H) const override;
│ │ │ │ -
480 void WhitenInPlace(Eigen::Block<Matrix> H) const override;
│ │ │ │ -
481
│ │ │ │ -
491 Diagonal::shared_ptr QR(Matrix& Ab) const override;
│ │ │ │ -
492
│ │ │ │ -
497 shared_ptr unit() const;
│ │ │ │ -
498
│ │ │ │ -
499 private:
│ │ │ │ -
501 friend class boost::serialization::access;
│ │ │ │ -
502 template<class ARCHIVE>
│ │ │ │ -
503 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
504 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Diagonal);
│ │ │ │ -
505 ar & BOOST_SERIALIZATION_NVP(mu_);
│ │ │ │ -
506 }
│ │ │ │ -
507
│ │ │ │ -
508 }; // Constrained
│ │ │ │ -
│ │ │ │ -
509
│ │ │ │ -
510 //---------------------------------------------------------------------------------------
│ │ │ │ -
511
│ │ │ │ -
│ │ │ │ -
516 class GTSAM_EXPORT Isotropic : public Diagonal {
│ │ │ │ -
517 protected:
│ │ │ │ -
518 double sigma_, invsigma_;
│ │ │ │ -
519
│ │ │ │ -
│ │ │ │ -
521 Isotropic(size_t dim, double sigma) :
│ │ │ │ -
522 Diagonal(Vector::Constant(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {}
│ │ │ │ -
│ │ │ │ -
523
│ │ │ │ -
524 public:
│ │ │ │ -
525
│ │ │ │ -
526 /* dummy constructor to allow for serialization */
│ │ │ │ -
527 Isotropic() : Diagonal(Vector1::Constant(1.0)),sigma_(1.0),invsigma_(1.0) {}
│ │ │ │ -
528
│ │ │ │ -
529 ~Isotropic() override {}
│ │ │ │ -
530
│ │ │ │ -
531 typedef boost::shared_ptr<Isotropic> shared_ptr;
│ │ │ │ -
532
│ │ │ │ -
536 static shared_ptr Sigma(size_t dim, double sigma, bool smart = true);
│ │ │ │ -
537
│ │ │ │ -
544 static shared_ptr Variance(size_t dim, double variance, bool smart = true);
│ │ │ │ -
545
│ │ │ │ -
│ │ │ │ -
549 static shared_ptr Precision(size_t dim, double precision, bool smart = true) {
│ │ │ │ -
550 return Variance(dim, 1.0/precision, smart);
│ │ │ │ -
551 }
│ │ │ │ -
│ │ │ │ -
552
│ │ │ │ -
553 void print(const std::string& name) const override;
│ │ │ │ -
554 double squaredMahalanobisDistance(const Vector& v) const override;
│ │ │ │ -
555 Vector whiten(const Vector& v) const override;
│ │ │ │ -
556 Vector unwhiten(const Vector& v) const override;
│ │ │ │ -
557 Matrix Whiten(const Matrix& H) const override;
│ │ │ │ -
558 void WhitenInPlace(Matrix& H) const override;
│ │ │ │ -
559 void whitenInPlace(Vector& v) const override;
│ │ │ │ -
560 void WhitenInPlace(Eigen::Block<Matrix> H) const override;
│ │ │ │ -
561
│ │ │ │ -
565 inline double sigma() const { return sigma_; }
│ │ │ │ -
566
│ │ │ │ -
567 private:
│ │ │ │ -
569 friend class boost::serialization::access;
│ │ │ │ -
570 template<class ARCHIVE>
│ │ │ │ -
571 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
572 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Diagonal);
│ │ │ │ -
573 ar & BOOST_SERIALIZATION_NVP(sigma_);
│ │ │ │ -
574 ar & BOOST_SERIALIZATION_NVP(invsigma_);
│ │ │ │ -
575 }
│ │ │ │ -
576
│ │ │ │ -
577 };
│ │ │ │ -
│ │ │ │ -
578
│ │ │ │ -
579 //---------------------------------------------------------------------------------------
│ │ │ │ -
580
│ │ │ │ -
│ │ │ │ -
584 class GTSAM_EXPORT Unit : public Isotropic {
│ │ │ │ -
585 public:
│ │ │ │ -
586
│ │ │ │ -
587 typedef boost::shared_ptr<Unit> shared_ptr;
│ │ │ │ -
588
│ │ │ │ -
590 Unit(size_t dim=1): Isotropic(dim,1.0) {}
│ │ │ │ -
591
│ │ │ │ -
592 ~Unit() override {}
│ │ │ │ -
593
│ │ │ │ -
│ │ │ │ -
597 static shared_ptr Create(size_t dim) {
│ │ │ │ -
598 return shared_ptr(new Unit(dim));
│ │ │ │ -
599 }
│ │ │ │ -
│ │ │ │ -
600
│ │ │ │ -
602 bool isUnit() const override { return true; }
│ │ │ │ -
603
│ │ │ │ -
604 void print(const std::string& name) const override;
│ │ │ │ -
605 double squaredMahalanobisDistance(const Vector& v) const override;
│ │ │ │ -
606 Vector whiten(const Vector& v) const override { return v; }
│ │ │ │ -
607 Vector unwhiten(const Vector& v) const override { return v; }
│ │ │ │ -
608 Matrix Whiten(const Matrix& H) const override { return H; }
│ │ │ │ -
609 void WhitenInPlace(Matrix& /*H*/) const override {}
│ │ │ │ -
610 void WhitenInPlace(Eigen::Block<Matrix> /*H*/) const override {}
│ │ │ │ -
611 void whitenInPlace(Vector& /*v*/) const override {}
│ │ │ │ -
612 void unwhitenInPlace(Vector& /*v*/) const override {}
│ │ │ │ -
613 void whitenInPlace(Eigen::Block<Vector>& /*v*/) const override {}
│ │ │ │ -
614 void unwhitenInPlace(Eigen::Block<Vector>& /*v*/) const override {}
│ │ │ │ -
615
│ │ │ │ -
616 private:
│ │ │ │ -
618 friend class boost::serialization::access;
│ │ │ │ -
619 template<class ARCHIVE>
│ │ │ │ -
620 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
621 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Isotropic);
│ │ │ │ -
622 }
│ │ │ │ -
623 };
│ │ │ │ -
│ │ │ │ -
624
│ │ │ │ -
│ │ │ │ -
642 class GTSAM_EXPORT Robust : public Base {
│ │ │ │ -
643 public:
│ │ │ │ -
644 typedef boost::shared_ptr<Robust> shared_ptr;
│ │ │ │ -
645
│ │ │ │ -
646 protected:
│ │ │ │ -
647 typedef mEstimator::Base RobustModel;
│ │ │ │ -
648 typedef noiseModel::Base NoiseModel;
│ │ │ │ -
649
│ │ │ │ -
650 const RobustModel::shared_ptr robust_;
│ │ │ │ -
651 const NoiseModel::shared_ptr noise_;
│ │ │ │ -
652
│ │ │ │ -
653 public:
│ │ │ │ -
654
│ │ │ │ -
656 Robust() {};
│ │ │ │ -
657
│ │ │ │ -
│ │ │ │ -
659 Robust(const RobustModel::shared_ptr robust, const NoiseModel::shared_ptr noise)
│ │ │ │ -
660 : Base(noise->dim()), robust_(robust), noise_(noise) {}
│ │ │ │ -
│ │ │ │ -
661
│ │ │ │ -
663 ~Robust() override {}
│ │ │ │ -
664
│ │ │ │ -
665 void print(const std::string& name) const override;
│ │ │ │ -
666 bool equals(const Base& expected, double tol=1e-9) const override;
│ │ │ │ -
667
│ │ │ │ -
669 const RobustModel::shared_ptr& robust() const { return robust_; }
│ │ │ │ -
670
│ │ │ │ -
672 const NoiseModel::shared_ptr& noise() const { return noise_; }
│ │ │ │ -
673
│ │ │ │ -
674 // Functions below are dummy but necessary for the noiseModel::Base
│ │ │ │ -
│ │ │ │ -
675 inline Vector whiten(const Vector& v) const override
│ │ │ │ -
676 { Vector r = v; this->WhitenSystem(r); return r; }
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
677 inline Matrix Whiten(const Matrix& A) const override
│ │ │ │ -
678 { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; }
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
679 inline Vector unwhiten(const Vector& /*v*/) const override
│ │ │ │ -
680 { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); }
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
682 double loss(const double squared_distance) const override {
│ │ │ │ -
683 return robust_->loss(std::sqrt(squared_distance));
│ │ │ │ -
684 }
│ │ │ │ -
│ │ │ │ -
685
│ │ │ │ -
686 // NOTE: This is special because in whiten the base version will do the reweighting
│ │ │ │ -
687 // which is incorrect!
│ │ │ │ -
│ │ │ │ -
688 double squaredMahalanobisDistance(const Vector& v) const override {
│ │ │ │ -
689 return noise_->squaredMahalanobisDistance(v);
│ │ │ │ -
690 }
│ │ │ │ -
│ │ │ │ -
691
│ │ │ │ -
692 // These are really robust iterated re-weighting support functions
│ │ │ │ -
693 virtual void WhitenSystem(Vector& b) const;
│ │ │ │ -
694 void WhitenSystem(std::vector<Matrix>& A, Vector& b) const override;
│ │ │ │ -
695 void WhitenSystem(Matrix& A, Vector& b) const override;
│ │ │ │ -
696 void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const override;
│ │ │ │ -
697 void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const override;
│ │ │ │ -
698
│ │ │ │ -
699 Vector unweightedWhiten(const Vector& v) const override;
│ │ │ │ -
700 double weight(const Vector& v) const override;
│ │ │ │ -
701
│ │ │ │ -
702 static shared_ptr Create(
│ │ │ │ -
703 const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise);
│ │ │ │ -
704
│ │ │ │ -
705 private:
│ │ │ │ -
707 friend class boost::serialization::access;
│ │ │ │ -
708 template<class ARCHIVE>
│ │ │ │ -
709 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
710 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
711 ar & boost::serialization::make_nvp("robust_", const_cast<RobustModel::shared_ptr&>(robust_));
│ │ │ │ -
712 ar & boost::serialization::make_nvp("noise_", const_cast<NoiseModel::shared_ptr&>(noise_));
│ │ │ │ -
713 }
│ │ │ │ -
714 };
│ │ │ │ -
│ │ │ │ -
715
│ │ │ │ -
716 // Helper function
│ │ │ │ -
717 GTSAM_EXPORT boost::optional<Vector> checkIfDiagonal(const Matrix& M);
│ │ │ │ -
718
│ │ │ │ -
719 } // namespace noiseModel
│ │ │ │ -
720
│ │ │ │ -
724 typedef noiseModel::Base::shared_ptr SharedNoiseModel;
│ │ │ │ -
725 typedef noiseModel::Gaussian::shared_ptr SharedGaussian;
│ │ │ │ -
726 typedef noiseModel::Diagonal::shared_ptr SharedDiagonal;
│ │ │ │ -
727 typedef noiseModel::Constrained::shared_ptr SharedConstrained;
│ │ │ │ -
728 typedef noiseModel::Isotropic::shared_ptr SharedIsotropic;
│ │ │ │ -
729
│ │ │ │ -
731 template<> struct traits<noiseModel::Gaussian> : public Testable<noiseModel::Gaussian> {};
│ │ │ │ -
732 template<> struct traits<noiseModel::Diagonal> : public Testable<noiseModel::Diagonal> {};
│ │ │ │ -
733 template<> struct traits<noiseModel::Constrained> : public Testable<noiseModel::Constrained> {};
│ │ │ │ -
734 template<> struct traits<noiseModel::Isotropic> : public Testable<noiseModel::Isotropic> {};
│ │ │ │ -
735 template<> struct traits<noiseModel::Unit> : public Testable<noiseModel::Unit> {};
│ │ │ │ -
736
│ │ │ │ -
737} //\ namespace gtsam
│ │ │ │ -
738
│ │ │ │ -
739
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ -
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
398} // namespace gtsam
│ │ │ │ +
399
│ │ │ │ +
400#include "SOn-inl.h"
│ │ │ │ +
make_shared.h
make_shared trampoline function to ensure proper alignment
│ │ │ │ +
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ +
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
This marks a GTSAM object to require alignment.
Definition types.h:317
│ │ │ │ +
Lie.h
Base class and basic functions for Lie types.
│ │ │ │ +
gtsam::internal::DimensionSO
constexpr int DimensionSO(int N)
Calculate dimensionality of SO<N> manifold, or return Dynamic if so.
Definition SOn.h:39
│ │ │ │ +
SOn-inl.h
Template implementations for SO(n)
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ +
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition serialization.h:113
│ │ │ │ +
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition Matrix.h:81
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::noiseModel::mEstimator::Base
Pure virtual class for all robust error function classes.
Definition LossFunctions.h:63
│ │ │ │ -
gtsam::noiseModel::Base
noiseModel::Base is the abstract base class for all noise models.
Definition NoiseModel.h:53
│ │ │ │ -
gtsam::noiseModel::Base::isConstrained
virtual bool isConstrained() const
true if a constrained noise model, saves slow/clumsy dynamic casting
Definition NoiseModel.h:69
│ │ │ │ -
gtsam::noiseModel::Base::whitenInPlace
virtual void whitenInPlace(Vector &v) const
in-place whiten, override if can be done more efficiently
Definition NoiseModel.h:112
│ │ │ │ -
gtsam::noiseModel::Base::dim
size_t dim() const
Dimensionality.
Definition NoiseModel.h:75
│ │ │ │ -
gtsam::noiseModel::Base::unwhitenInPlace
virtual void unwhitenInPlace(Eigen::Block< Vector > &v) const
in-place unwhiten, override if can be done more efficiently
Definition NoiseModel.h:127
│ │ │ │ -
gtsam::noiseModel::Base::whitenInPlace
virtual void whitenInPlace(Eigen::Block< Vector > &v) const
in-place whiten, override if can be done more efficiently
Definition NoiseModel.h:122
│ │ │ │ -
gtsam::noiseModel::Base::whiten
virtual Vector whiten(const Vector &v) const =0
Whiten an error vector.
│ │ │ │ -
gtsam::noiseModel::Base::mahalanobisDistance
virtual double mahalanobisDistance(const Vector &v) const
Mahalanobis distance.
Definition NoiseModel.h:97
│ │ │ │ -
gtsam::noiseModel::Base::isUnit
virtual bool isUnit() const
true if a unit noise model, saves slow/clumsy dynamic casting
Definition NoiseModel.h:72
│ │ │ │ -
gtsam::noiseModel::Base::weight
virtual double weight(const Vector &v) const
get the weight from the effective loss function on residual vector v
Definition NoiseModel.h:137
│ │ │ │ -
gtsam::noiseModel::Base::unweightedWhiten
virtual Vector unweightedWhiten(const Vector &v) const
Useful function for robust noise models to get the unweighted but whitened error.
Definition NoiseModel.h:132
│ │ │ │ -
gtsam::noiseModel::Base::unwhiten
virtual Vector unwhiten(const Vector &v) const =0
Unwhiten an error vector.
│ │ │ │ -
gtsam::noiseModel::Base::loss
virtual double loss(const double squared_distance) const
loss function, input is Mahalanobis distance
Definition NoiseModel.h:102
│ │ │ │ -
gtsam::noiseModel::Base::unwhitenInPlace
virtual void unwhitenInPlace(Vector &v) const
in-place unwhiten, override if can be done more efficiently
Definition NoiseModel.h:117
│ │ │ │ -
gtsam::noiseModel::Base::Whiten
virtual Matrix Whiten(const Matrix &H) const =0
Whiten a matrix.
│ │ │ │ -
gtsam::noiseModel::Base::Base
Base(size_t dim=1)
primary constructor
Definition NoiseModel.h:65
│ │ │ │ -
gtsam::noiseModel::Gaussian
Gaussian implements the mathematical model |R*x|^2 = |y|^2 with R'*R=inv(Sigma) where y = whiten(x) =...
Definition NoiseModel.h:162
│ │ │ │ -
gtsam::noiseModel::Gaussian::R
virtual Matrix R() const
Return R itself, but note that Whiten(H) is cheaper than R*H.
Definition NoiseModel.h:255
│ │ │ │ -
gtsam::noiseModel::Gaussian::Gaussian
Gaussian(size_t dim=1, const boost::optional< Matrix > &sqrt_information=boost::none)
constructor takes square root information matrix
Definition NoiseModel.h:186
│ │ │ │ -
gtsam::noiseModel::Gaussian::sqrt_information_
boost::optional< Matrix > sqrt_information_
Matrix square root of information matrix (R)
Definition NoiseModel.h:167
│ │ │ │ -
gtsam::noiseModel::Diagonal
A diagonal noise model implements a diagonal covariance matrix, with the elements of the diagonal spe...
Definition NoiseModel.h:281
│ │ │ │ -
gtsam::noiseModel::Diagonal::R
Matrix R() const override
Return R itself, but note that Whiten(H) is cheaper than R*H.
Definition NoiseModel.h:352
│ │ │ │ -
gtsam::noiseModel::Diagonal::sigmas_
Vector sigmas_
Standard deviations (sigmas), their inverse and inverse square (weights/precisions) These are all com...
Definition NoiseModel.h:289
│ │ │ │ -
gtsam::noiseModel::Diagonal::sigma
double sigma(size_t i) const
Return standard deviations (sqrt of diagonal)
Definition NoiseModel.h:335
│ │ │ │ -
gtsam::noiseModel::Diagonal::invsigmas
const Vector & invsigmas() const
Return sqrt precisions.
Definition NoiseModel.h:340
│ │ │ │ -
gtsam::noiseModel::Diagonal::sigmas
Vector sigmas() const override
Calculate standard deviations.
Definition NoiseModel.h:325
│ │ │ │ -
gtsam::noiseModel::Diagonal::precisions
const Vector & precisions() const
Return precisions.
Definition NoiseModel.h:346
│ │ │ │ -
gtsam::noiseModel::Constrained
A Constrained constrained model is a specialization of Diagonal which allows some or all of the sigma...
Definition NoiseModel.h:381
│ │ │ │ -
gtsam::noiseModel::Constrained::isConstrained
bool isConstrained() const override
true if a constrained noise mode, saves slow/clumsy dynamic casting
Definition NoiseModel.h:409
│ │ │ │ -
gtsam::noiseModel::Constrained::All
static shared_ptr All(size_t dim, const Vector &mu)
Fully constrained variations.
Definition NoiseModel.h:462
│ │ │ │ -
gtsam::noiseModel::Constrained::All
static shared_ptr All(size_t dim, double mu)
Fully constrained variations with a mu parameter.
Definition NoiseModel.h:467
│ │ │ │ -
gtsam::noiseModel::Constrained::All
static shared_ptr All(size_t dim)
Fully constrained variations.
Definition NoiseModel.h:457
│ │ │ │ -
gtsam::noiseModel::Constrained::mu
const Vector & mu() const
Access mu as a vector.
Definition NoiseModel.h:415
│ │ │ │ -
gtsam::noiseModel::Constrained::mu_
Vector mu_
Penalty function weight - needs to be large enough to dominate soft constraints.
Definition NoiseModel.h:385
│ │ │ │ -
gtsam::noiseModel::Isotropic
An isotropic noise model corresponds to a scaled diagonal covariance To construct,...
Definition NoiseModel.h:516
│ │ │ │ -
gtsam::noiseModel::Isotropic::sigma
double sigma() const
Return standard deviation.
Definition NoiseModel.h:565
│ │ │ │ -
gtsam::noiseModel::Isotropic::Precision
static shared_ptr Precision(size_t dim, double precision, bool smart=true)
An isotropic noise model created by specifying a precision.
Definition NoiseModel.h:549
│ │ │ │ -
gtsam::noiseModel::Isotropic::Isotropic
Isotropic(size_t dim, double sigma)
protected constructor takes sigma
Definition NoiseModel.h:521
│ │ │ │ -
gtsam::noiseModel::Unit
Unit: i.i.d.
Definition NoiseModel.h:584
│ │ │ │ -
gtsam::noiseModel::Unit::WhitenInPlace
void WhitenInPlace(Eigen::Block< Matrix >) const override
In-place version.
Definition NoiseModel.h:610
│ │ │ │ -
gtsam::noiseModel::Unit::unwhiten
Vector unwhiten(const Vector &v) const override
Unwhiten an error vector.
Definition NoiseModel.h:607
│ │ │ │ -
gtsam::noiseModel::Unit::whitenInPlace
void whitenInPlace(Eigen::Block< Vector > &) const override
in-place whiten, override if can be done more efficiently
Definition NoiseModel.h:613
│ │ │ │ -
gtsam::noiseModel::Unit::isUnit
bool isUnit() const override
true if a unit noise model, saves slow/clumsy dynamic casting
Definition NoiseModel.h:602
│ │ │ │ -
gtsam::noiseModel::Unit::Unit
Unit(size_t dim=1)
constructor for serialization
Definition NoiseModel.h:590
│ │ │ │ -
gtsam::noiseModel::Unit::whiten
Vector whiten(const Vector &v) const override
Whiten an error vector.
Definition NoiseModel.h:606
│ │ │ │ -
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Create a unit covariance noise model.
Definition NoiseModel.h:597
│ │ │ │ -
gtsam::noiseModel::Unit::unwhitenInPlace
void unwhitenInPlace(Vector &) const override
in-place unwhiten, override if can be done more efficiently
Definition NoiseModel.h:612
│ │ │ │ -
gtsam::noiseModel::Unit::unwhitenInPlace
void unwhitenInPlace(Eigen::Block< Vector > &) const override
in-place unwhiten, override if can be done more efficiently
Definition NoiseModel.h:614
│ │ │ │ -
gtsam::noiseModel::Unit::whitenInPlace
void whitenInPlace(Vector &) const override
in-place whiten, override if can be done more efficiently
Definition NoiseModel.h:611
│ │ │ │ -
gtsam::noiseModel::Unit::WhitenInPlace
void WhitenInPlace(Matrix &) const override
In-place version.
Definition NoiseModel.h:609
│ │ │ │ -
gtsam::noiseModel::Unit::Whiten
Matrix Whiten(const Matrix &H) const override
Whiten a matrix.
Definition NoiseModel.h:608
│ │ │ │ -
gtsam::noiseModel::Robust
Base class for robust error models The robust M-estimators above simply tell us how to re-weight the ...
Definition NoiseModel.h:642
│ │ │ │ -
gtsam::noiseModel::Robust::robust
const RobustModel::shared_ptr & robust() const
Return the contained robust error function.
Definition NoiseModel.h:669
│ │ │ │ -
gtsam::noiseModel::Robust::Robust
Robust()
Default Constructor for serialization.
Definition NoiseModel.h:656
│ │ │ │ -
gtsam::noiseModel::Robust::Robust
Robust(const RobustModel::shared_ptr robust, const NoiseModel::shared_ptr noise)
Constructor.
Definition NoiseModel.h:659
│ │ │ │ -
gtsam::noiseModel::Robust::squaredMahalanobisDistance
double squaredMahalanobisDistance(const Vector &v) const override
Squared Mahalanobis distance v'*R'*R*v = <R*v,R*v>
Definition NoiseModel.h:688
│ │ │ │ -
gtsam::noiseModel::Robust::noise_
const NoiseModel::shared_ptr noise_
noise model used
Definition NoiseModel.h:651
│ │ │ │ -
gtsam::noiseModel::Robust::loss
double loss(const double squared_distance) const override
Compute loss from the m-estimator using the Mahalanobis distance.
Definition NoiseModel.h:682
│ │ │ │ -
gtsam::noiseModel::Robust::unwhiten
Vector unwhiten(const Vector &) const override
Unwhiten an error vector.
Definition NoiseModel.h:679
│ │ │ │ -
gtsam::noiseModel::Robust::Whiten
Matrix Whiten(const Matrix &A) const override
Whiten a matrix.
Definition NoiseModel.h:677
│ │ │ │ -
gtsam::noiseModel::Robust::robust_
const RobustModel::shared_ptr robust_
robust error function used
Definition NoiseModel.h:650
│ │ │ │ -
gtsam::noiseModel::Robust::~Robust
~Robust() override
Destructor.
Definition NoiseModel.h:663
│ │ │ │ -
gtsam::noiseModel::Robust::noise
const NoiseModel::shared_ptr & noise() const
Return the contained noise model.
Definition NoiseModel.h:672
│ │ │ │ -
gtsam::noiseModel::Robust::whiten
Vector whiten(const Vector &v) const override
Whiten an error vector.
Definition NoiseModel.h:675
│ │ │ │ +
gtsam::LieGroup
A CRTP helper class that implements Lie group methods Prerequisites: methods operator*,...
Definition Lie.h:37
│ │ │ │ +
gtsam::LieGroup< SO< N >, internal::DimensionSO(N)>::Retract
static SO< N > Retract(const TangentVector &v)
Retract at origin: possible in Lie group because it has an identity.
Definition Lie.h:111
│ │ │ │ +
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition Lie.h:229
│ │ │ │ +
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ +
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
│ │ │ │ +
gtsam::SO
Manifold of special orthogonal rotation matrices SO<N>.
Definition SOn.h:52
│ │ │ │ +
gtsam::SO::FromMatrix
static SO FromMatrix(const Eigen::MatrixBase< Derived > &R)
Named constructor from Eigen Matrix.
Definition SOn.h:95
│ │ │ │ +
gtsam::SO::Expmap
static SO Expmap(const TangentVector &omega, ChartJacobian H=boost::none)
Exponential map at identity - create a rotation from canonical coordinates.
Definition SOn-inl.h:67
│ │ │ │ +
gtsam::SO::VectorizedGenerators
static Matrix VectorizedGenerators()
Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N)
Definition SOn.h:300
│ │ │ │ +
gtsam::SO::inverse
SO inverse() const
inverse of a rotation = transpose
Definition SOn.h:193
│ │ │ │ +
gtsam::SO::vec
VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost::none) const
Return vectorized rotation matrix in column order.
Definition SOn-inl.h:88
│ │ │ │ +
gtsam::SO::ChordalMean
static SO ChordalMean(const std::vector< SO > &rotations)
Named constructor that finds chordal mean , currently only defined for SO3.
│ │ │ │ +
gtsam::SO::operator*
SO operator*(const SO &other) const
Multiplication.
Definition SOn.h:175
│ │ │ │ +
gtsam::SO::Vee
static TangentVector Vee(const MatrixNN &X)
Inverse of Hat. See note about xi element order in Hat.
Definition SOn-inl.h:35
│ │ │ │ +
gtsam::SO::matrix_
MatrixNN matrix_
Rotation matrix.
Definition SOn.h:62
│ │ │ │ +
gtsam::SO::Logmap
static TangentVector Logmap(const SO &R, ChartJacobian H=boost::none)
Log map at identity - returns the canonical coordinates of this rotation.
Definition SOn-inl.h:77
│ │ │ │ +
gtsam::SO::AxisAngle
static SO AxisAngle(const Vector3 &axis, double theta)
Constructor from axis and angle. Only defined for SO3.
│ │ │ │ +
gtsam::SO::AdjointMap
MatrixDD AdjointMap() const
Adjoint map.
Definition SO4.cpp:159
│ │ │ │ +
gtsam::SO::Hat
static void Hat(const Vector &xi, Eigen::Ref< MatrixNN > X)
In-place version of Hat (see details there), implements recursion.
│ │ │ │ +
gtsam::SO::Identity
static SO Identity()
SO<N> identity for N >= 2.
Definition SOn.h:182
│ │ │ │ +
gtsam::SO::Dim
static int Dim()
Return compile-time dimensionality: fixed size N or Eigen::Dynamic.
Definition SOn.h:203
│ │ │ │ +
gtsam::SO::Hat
static MatrixNN Hat(const TangentVector &xi)
Hat operator creates Lie algebra element corresponding to d-vector, where d is the dimensionality of ...
Definition SOn-inl.h:29
│ │ │ │ +
gtsam::SO::ExpmapDerivative
static MatrixDD ExpmapDerivative(const TangentVector &omega)
Derivative of Expmap, currently only defined for SO3.
Definition SOn-inl.h:72
│ │ │ │ +
gtsam::SO::LogmapDerivative
static MatrixDD LogmapDerivative(const TangentVector &omega)
Derivative of Logmap, currently only defined for SO3.
Definition SOn-inl.h:82
│ │ │ │ +
gtsam::SO::SO
SO(const SO< M > &R)
Construct dynamic SO(n) from Fixed SO<M>
Definition SOn.h:111
│ │ │ │ +
gtsam::SO::SO
SO(size_t n=0)
Construct SO<N> identity for N == Eigen::Dynamic.
Definition SOn.h:83
│ │ │ │ +
gtsam::SO::SO
SO()
Construct SO<N> identity for N >= 2.
Definition SOn.h:79
│ │ │ │ +
gtsam::SO::Lift
static SO Lift(size_t n, const Eigen::MatrixBase< Derived > &R)
Named constructor from lower dimensional matrix.
Definition SOn.h:101
│ │ │ │ +
gtsam::SO::Random
static SO Random(std::mt19937 &rng, size_t n=0)
Random SO(n) element (no big claims about uniformity). SO(3) is specialized in SO3....
Definition SOn.h:131
│ │ │ │ +
gtsam::SO::Identity
static SO Identity(size_t n=0)
SO<N> identity for N == Eigen::Dynamic.
Definition SOn.h:188
│ │ │ │ +
gtsam::SO::matrix
const MatrixNN & matrix() const
Return matrix.
Definition SOn.h:155
│ │ │ │ +
gtsam::SO::Random
static SO Random(std::mt19937 &rng)
Random SO(N) element (no big claims about uniformity)
Definition SOn.h:145
│ │ │ │ +
gtsam::SO::ClosestTo
static SO ClosestTo(const MatrixNN &M)
Named constructor that finds SO(n) matrix closest to M in Frobenius norm, currently only defined for ...
│ │ │ │ +
gtsam::SO::SO
SO(const Eigen::AngleAxisd &angleAxis)
Constructor from AngleAxisd.
Definition SOn.h:115
│ │ │ │ +
gtsam::SO::VectorizedGenerators
static Matrix VectorizedGenerators(size_t n=0)
Calculate n^2 x dim matrix of vectorized Lie algebra generators for SO(n)
Definition SOn.h:312
│ │ │ │ +
gtsam::SO::SO
SO(const Eigen::MatrixBase< Derived > &R)
Constructor from Eigen Matrix, dynamic version.
Definition SOn.h:91
│ │ │ │ +
gtsam::SO::ChartAtOrigin
Definition SOn.h:240
│ │ │ │ +
gtsam::SO::ChartAtOrigin::Local
static TangentVector Local(const SO &R, ChartJacobian H=boost::none)
Inverse of Retract.
Definition SOn-inl.h:50
│ │ │ │ +
gtsam::SO::ChartAtOrigin::Retract
static SO Retract(const TangentVector &xi, ChartJacobian H=boost::none)
Retract uses Cayley map.
Definition SOn-inl.h:40
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,805 +1,507 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -NoiseModel.h │ │ │ │ │ +SOn.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 19#pragma once │ │ │ │ │ 20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ -23#include │ │ │ │ │ -24#include │ │ │ │ │ -25 │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ -28#include │ │ │ │ │ -29#include │ │ │ │ │ -30#include │ │ │ │ │ -31 │ │ │ │ │ -32namespace _g_t_s_a_m { │ │ │ │ │ -33 │ │ │ │ │ -35 namespace noiseModel { │ │ │ │ │ +21#include <_g_t_s_a_m_/_b_a_s_e_/_L_i_e_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_b_a_s_e_/_m_a_k_e___s_h_a_r_e_d_._h> │ │ │ │ │ +24#include │ │ │ │ │ +25#include │ │ │ │ │ +26 │ │ │ │ │ +27#include │ │ │ │ │ +28 │ │ │ │ │ +29#include // TODO(frank): how to avoid? │ │ │ │ │ +30#include │ │ │ │ │ +31#include │ │ │ │ │ +32#include │ │ │ │ │ +33#include │ │ │ │ │ +34 │ │ │ │ │ +35namespace _g_t_s_a_m { │ │ │ │ │ 36 │ │ │ │ │ -37 // Forward declaration │ │ │ │ │ -38 class Gaussian; │ │ │ │ │ -39 class Diagonal; │ │ │ │ │ -40 class Constrained; │ │ │ │ │ -41 class Isotropic; │ │ │ │ │ -42 class Unit; │ │ │ │ │ -43 class RobustModel; │ │ │ │ │ -44 │ │ │ │ │ -45 //-------------------------------------------------------------------------- │ │ │ │ │ -------------- │ │ │ │ │ +37namespace internal { │ │ │ │ │ +_3_9constexpr int _D_i_m_e_n_s_i_o_n_S_O(int N) { │ │ │ │ │ +40 return (N < 0) ? Eigen::Dynamic : N * (N - 1) / 2; │ │ │ │ │ +41} │ │ │ │ │ +42 │ │ │ │ │ +43// Calculate N^2 at compile time, or return Dynamic if so │ │ │ │ │ +44constexpr int NSquaredSO(int N) { return (N < 0) ? Eigen::Dynamic : N * N; } │ │ │ │ │ +45} // namespace internal │ │ │ │ │ 46 │ │ │ │ │ -_5_3 class GTSAM_EXPORT _B_a_s_e { │ │ │ │ │ -54 │ │ │ │ │ -55 public: │ │ │ │ │ -56 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -57 │ │ │ │ │ -58 protected: │ │ │ │ │ -59 │ │ │ │ │ -60 size_t dim_; │ │ │ │ │ -61 │ │ │ │ │ -62 public: │ │ │ │ │ +51template │ │ │ │ │ +_5_2class _S_O : public _L_i_e_G_r_o_u_p, internal::DimensionSO(N)> { │ │ │ │ │ +53 public: │ │ │ │ │ +54 enum { dimension = internal::DimensionSO(N) }; │ │ │ │ │ +55 using MatrixNN = Eigen::Matrix; │ │ │ │ │ +56 using VectorN2 = Eigen::Matrix; │ │ │ │ │ +57 using MatrixDD = Eigen::Matrix; │ │ │ │ │ +58 │ │ │ │ │ +59 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W___I_F(true) │ │ │ │ │ +60 │ │ │ │ │ +61 protected: │ │ │ │ │ +_6_2 MatrixNN _m_a_t_r_i_x__; │ │ │ │ │ 63 │ │ │ │ │ -_6_5 _B_a_s_e(size_t dim = 1):dim_(dim) {} │ │ │ │ │ -66 virtual _~_B_a_s_e() {} │ │ │ │ │ -67 │ │ │ │ │ -_6_9 virtual bool _i_s_C_o_n_s_t_r_a_i_n_e_d() const { return false; } // default false │ │ │ │ │ -70 │ │ │ │ │ -_7_2 virtual bool _i_s_U_n_i_t() const { return false; } // default false │ │ │ │ │ -73 │ │ │ │ │ -_7_5 inline size_t _d_i_m() const { return dim_;} │ │ │ │ │ +64 // enable_if_t aliases, used to specialize constructors/methods, see │ │ │ │ │ +65 // https://www.fluentcpp.com/2018/05/18/make-sfinae-pretty-2-hidden-beauty- │ │ │ │ │ +sfinae/ │ │ │ │ │ +66 template │ │ │ │ │ +67 using IsDynamic = typename std::enable_if::type; │ │ │ │ │ +68 template │ │ │ │ │ +69 using IsFixed = typename std::enable_if= 2, void>::type; │ │ │ │ │ +70 template │ │ │ │ │ +71 using IsSO3 = typename std::enable_if::type; │ │ │ │ │ +72 │ │ │ │ │ +73 public: │ │ │ │ │ 76 │ │ │ │ │ -77 virtual void _p_r_i_n_t(const std::string& name = "") const = 0; │ │ │ │ │ -78 │ │ │ │ │ -79 virtual bool _e_q_u_a_l_s(const _B_a_s_e& expected, double tol=1e-9) const = 0; │ │ │ │ │ +78 template > │ │ │ │ │ +_7_9 _S_O() : _m_a_t_r_i_x__(MatrixNN::_I_d_e_n_t_i_t_y()) {} │ │ │ │ │ 80 │ │ │ │ │ -82 virtual Vector sigmas() const; │ │ │ │ │ -83 │ │ │ │ │ -_8_5 virtual Vector _w_h_i_t_e_n(const Vector& v) const = 0; │ │ │ │ │ -86 │ │ │ │ │ -_8_8 virtual Matrix _W_h_i_t_e_n(const Matrix& H) const = 0; │ │ │ │ │ -89 │ │ │ │ │ -_9_1 virtual Vector _u_n_w_h_i_t_e_n(const Vector& v) const = 0; │ │ │ │ │ +82 template > │ │ │ │ │ +_8_3 explicit _S_O(size_t n = 0) { │ │ │ │ │ +84 // We allow for n=0 as the default constructor, needed for serialization, │ │ │ │ │ +85 // wrappers etc. │ │ │ │ │ +86 _m_a_t_r_i_x__ = Eigen::MatrixXd::Identity(n, n); │ │ │ │ │ +87 } │ │ │ │ │ +88 │ │ │ │ │ +90 template │ │ │ │ │ +_9_1 explicit _S_O(const Eigen::MatrixBase& R) : _m_a_t_r_i_x__(R.eval()) {} │ │ │ │ │ 92 │ │ │ │ │ -94 virtual double squaredMahalanobisDistance(const Vector& v) const; │ │ │ │ │ -95 │ │ │ │ │ -_9_7 virtual double _m_a_h_a_l_a_n_o_b_i_s_D_i_s_t_a_n_c_e(const Vector& v) const { │ │ │ │ │ -98 return std::sqrt(squaredMahalanobisDistance(v)); │ │ │ │ │ -99 } │ │ │ │ │ -100 │ │ │ │ │ -_1_0_2 virtual double _l_o_s_s(const double squared_distance) const { │ │ │ │ │ -103 return 0.5 * squared_distance; │ │ │ │ │ -104 } │ │ │ │ │ -105 │ │ │ │ │ -106 virtual void WhitenSystem(std::vector& A, Vector& b) const = 0; │ │ │ │ │ -107 virtual void WhitenSystem(Matrix& A, Vector& b) const = 0; │ │ │ │ │ -108 virtual void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const = 0; │ │ │ │ │ -109 virtual void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) │ │ │ │ │ -const = 0; │ │ │ │ │ -110 │ │ │ │ │ -_1_1_2 virtual void _w_h_i_t_e_n_I_n_P_l_a_c_e(Vector& v) const { │ │ │ │ │ -113 v = whiten(v); │ │ │ │ │ -114 } │ │ │ │ │ -115 │ │ │ │ │ -_1_1_7 virtual void _u_n_w_h_i_t_e_n_I_n_P_l_a_c_e(Vector& v) const { │ │ │ │ │ -118 v = unwhiten(v); │ │ │ │ │ -119 } │ │ │ │ │ -120 │ │ │ │ │ -_1_2_2 virtual void _w_h_i_t_e_n_I_n_P_l_a_c_e(Eigen::Block& v) const { │ │ │ │ │ -123 v = whiten(v); │ │ │ │ │ -124 } │ │ │ │ │ -125 │ │ │ │ │ -_1_2_7 virtual void _u_n_w_h_i_t_e_n_I_n_P_l_a_c_e(Eigen::Block& v) const { │ │ │ │ │ -128 v = unwhiten(v); │ │ │ │ │ -129 } │ │ │ │ │ -130 │ │ │ │ │ -_1_3_2 virtual Vector _u_n_w_e_i_g_h_t_e_d_W_h_i_t_e_n(const Vector& v) const { │ │ │ │ │ -133 return whiten(v); │ │ │ │ │ -134 } │ │ │ │ │ -135 │ │ │ │ │ -_1_3_7 virtual double _w_e_i_g_h_t(const Vector& v) const { return 1.0; } │ │ │ │ │ -138 │ │ │ │ │ -139 private: │ │ │ │ │ -_1_4_1 friend class boost::serialization::access; │ │ │ │ │ -142 template │ │ │ │ │ -143 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -144 ar & BOOST_SERIALIZATION_NVP(dim_); │ │ │ │ │ -145 } │ │ │ │ │ -146 }; │ │ │ │ │ -147 │ │ │ │ │ -148 //------------------------------------------------------------------------- │ │ │ │ │ --------------- │ │ │ │ │ +94 template │ │ │ │ │ +_9_5 static _S_O _F_r_o_m_M_a_t_r_i_x(const Eigen::MatrixBase& R) { │ │ │ │ │ +96 return _S_O(R); │ │ │ │ │ +97 } │ │ │ │ │ +98 │ │ │ │ │ +100 template > │ │ │ │ │ +_1_0_1 static _S_O _L_i_f_t(size_t n, const Eigen::MatrixBase &R) { │ │ │ │ │ +102 Matrix Q = Matrix::Identity(n, n); │ │ │ │ │ +103 const int p = R.rows(); │ │ │ │ │ +104 assert(p >= 0 && p <= static_cast(n) && R.cols() == p); │ │ │ │ │ +105 Q.topLeftCorner(p, p) = R; │ │ │ │ │ +106 return _S_O(Q); │ │ │ │ │ +107 } │ │ │ │ │ +108 │ │ │ │ │ +110 template > │ │ │ │ │ +_1_1_1 explicit _S_O(const _S_O_<_M_>& R) : _m_a_t_r_i_x__(R._m_a_t_r_i_x()) {} │ │ │ │ │ +112 │ │ │ │ │ +114 template > │ │ │ │ │ +_1_1_5 explicit _S_O(const Eigen::AngleAxisd& angleAxis) : _m_a_t_r_i_x__(angleAxis) {} │ │ │ │ │ +116 │ │ │ │ │ +_1_1_8 static _S_O _A_x_i_s_A_n_g_l_e(const Vector3& axis, double theta); │ │ │ │ │ +119 │ │ │ │ │ +_1_2_2 static _S_O _C_l_o_s_e_s_t_T_o(const MatrixNN& M); │ │ │ │ │ +123 │ │ │ │ │ +_1_2_7 static _S_O _C_h_o_r_d_a_l_M_e_a_n(const std::vector& rotations); │ │ │ │ │ +128 │ │ │ │ │ +130 template > │ │ │ │ │ +_1_3_1 static _S_O _R_a_n_d_o_m(std::mt19937& rng, size_t n = 0) { │ │ │ │ │ +132 if (n == 0) throw std::runtime_error("SO: Dimensionality not known."); │ │ │ │ │ +133 // TODO(frank): this might need to be re-thought │ │ │ │ │ +134 static std::uniform_real_distribution randomAngle(-M_PI, M_PI); │ │ │ │ │ +135 const size_t d = SO::Dimension(n); │ │ │ │ │ +136 Vector xi(d); │ │ │ │ │ +137 for (size_t j = 0; j < d; j++) { │ │ │ │ │ +138 xi(j) = randomAngle(rng); │ │ │ │ │ +139 } │ │ │ │ │ +140 return _S_O_:_:_R_e_t_r_a_c_t(xi); │ │ │ │ │ +141 } │ │ │ │ │ +142 │ │ │ │ │ +144 template > │ │ │ │ │ +_1_4_5 static _S_O _R_a_n_d_o_m(std::mt19937& rng) { │ │ │ │ │ +146 // By default, use dynamic implementation above. Specialized for SO(3). │ │ │ │ │ +147 return _S_O(_S_O_<_E_i_g_e_n_:_:_D_y_n_a_m_i_c_>_:_:_R_a_n_d_o_m(rng, N)._m_a_t_r_i_x()); │ │ │ │ │ +148 } │ │ │ │ │ 149 │ │ │ │ │ -_1_6_2 class GTSAM_EXPORT _G_a_u_s_s_i_a_n: public _B_a_s_e { │ │ │ │ │ +153 │ │ │ │ │ +_1_5_5 const MatrixNN& _m_a_t_r_i_x() const { return _m_a_t_r_i_x__; } │ │ │ │ │ +156 │ │ │ │ │ +157 size_t rows() const { return _m_a_t_r_i_x__.rows(); } │ │ │ │ │ +158 size_t cols() const { return _m_a_t_r_i_x__.cols(); } │ │ │ │ │ +159 │ │ │ │ │ 163 │ │ │ │ │ -164 protected: │ │ │ │ │ +164 void print(const std::string& s = std::string()) const; │ │ │ │ │ 165 │ │ │ │ │ -_1_6_7 boost::optional _s_q_r_t___i_n_f_o_r_m_a_t_i_o_n__; │ │ │ │ │ -168 │ │ │ │ │ -169 private: │ │ │ │ │ -170 │ │ │ │ │ -174 const Matrix& thisR() const { │ │ │ │ │ -175 // should never happen │ │ │ │ │ -176 if (!sqrt_information_) throw std::runtime_error("Gaussian: has no R │ │ │ │ │ -matrix"); │ │ │ │ │ -177 return *sqrt_information_; │ │ │ │ │ +166 bool equals(const _S_O& other, double tol) const { │ │ │ │ │ +167 return _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(_m_a_t_r_i_x__, other.matrix_, tol); │ │ │ │ │ +168 } │ │ │ │ │ +169 │ │ │ │ │ +173 │ │ │ │ │ +_1_7_5 _S_O _o_p_e_r_a_t_o_r_*(const _S_O& other) const { │ │ │ │ │ +176 assert(dim() == other.dim()); │ │ │ │ │ +177 return _S_O(_m_a_t_r_i_x__ * other._m_a_t_r_i_x__); │ │ │ │ │ 178 } │ │ │ │ │ 179 │ │ │ │ │ -180 │ │ │ │ │ -181 public: │ │ │ │ │ -182 │ │ │ │ │ -183 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -184 │ │ │ │ │ -_1_8_6 _G_a_u_s_s_i_a_n(size_t dim = 1, │ │ │ │ │ -187 const boost::optional& sqrt_information = boost::none) │ │ │ │ │ -188 : _B_a_s_e(dim), sqrt_information_(sqrt_information) {} │ │ │ │ │ -189 │ │ │ │ │ -190 _~_G_a_u_s_s_i_a_n() override {} │ │ │ │ │ +181 template > │ │ │ │ │ +_1_8_2 static _S_O _I_d_e_n_t_i_t_y() { │ │ │ │ │ +183 return _S_O(); │ │ │ │ │ +184 } │ │ │ │ │ +185 │ │ │ │ │ +187 template > │ │ │ │ │ +_1_8_8 static _S_O _I_d_e_n_t_i_t_y(size_t n = 0) { │ │ │ │ │ +189 return _S_O(n); │ │ │ │ │ +190 } │ │ │ │ │ 191 │ │ │ │ │ -197 static shared_ptr SqrtInformation(const Matrix& R, bool smart = true); │ │ │ │ │ +_1_9_3 _S_O _i_n_v_e_r_s_e() const { return _S_O(_m_a_t_r_i_x__.transpose()); } │ │ │ │ │ +194 │ │ │ │ │ 198 │ │ │ │ │ -204 static shared_ptr Information(const Matrix& M, bool smart = true); │ │ │ │ │ -205 │ │ │ │ │ -211 static shared_ptr Covariance(const Matrix& covariance, bool smart = true); │ │ │ │ │ -212 │ │ │ │ │ -213 void _p_r_i_n_t(const std::string& name) const override; │ │ │ │ │ -214 bool _e_q_u_a_l_s(const Base& expected, double tol=1e-9) const override; │ │ │ │ │ -215 Vector sigmas() const override; │ │ │ │ │ -216 Vector whiten(const Vector& v) const override; │ │ │ │ │ -217 Vector unwhiten(const Vector& v) const override; │ │ │ │ │ -218 │ │ │ │ │ -223 Matrix Whiten(const Matrix& H) const override; │ │ │ │ │ -224 │ │ │ │ │ -228 virtual void WhitenInPlace(Matrix& H) const; │ │ │ │ │ -229 │ │ │ │ │ -233 virtual void WhitenInPlace(Eigen::Block H) const; │ │ │ │ │ -234 │ │ │ │ │ -238 void WhitenSystem(std::vector& A, Vector& b) const override; │ │ │ │ │ -239 void WhitenSystem(Matrix& A, Vector& b) const override; │ │ │ │ │ -240 void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const override; │ │ │ │ │ -241 void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const │ │ │ │ │ -override; │ │ │ │ │ -242 │ │ │ │ │ -252 virtual boost::shared_ptr QR(Matrix& Ab) const; │ │ │ │ │ -253 │ │ │ │ │ -_2_5_5 virtual Matrix _R() const { return thisR();} │ │ │ │ │ -256 │ │ │ │ │ -258 virtual Matrix information() const; │ │ │ │ │ +199 using TangentVector = Eigen::Matrix; │ │ │ │ │ +200 using ChartJacobian = _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_d_i_m_e_n_s_i_o_n_,_ _d_i_m_e_n_s_i_o_n_>; │ │ │ │ │ +201 │ │ │ │ │ +_2_0_3 static int _D_i_m() { return dimension; } │ │ │ │ │ +204 │ │ │ │ │ +205 // Calculate manifold dimensionality for SO(n). │ │ │ │ │ +206 // Available as dimension or Dim() for fixed N. │ │ │ │ │ +207 static size_t Dimension(size_t n) { return n * (n - 1) / 2; } │ │ │ │ │ +208 │ │ │ │ │ +209 // Calculate ambient dimension n from manifold dimensionality d. │ │ │ │ │ +210 static size_t AmbientDim(size_t d) { return (1 + std::sqrt(1 + 8 * d)) / 2; │ │ │ │ │ +} │ │ │ │ │ +211 │ │ │ │ │ +212 // Calculate run-time dimensionality of manifold. │ │ │ │ │ +213 // Available as dimension or Dim() for fixed N. │ │ │ │ │ +214 size_t dim() const { return Dimension(static_cast(_m_a_t_r_i_x__.rows())); │ │ │ │ │ +} │ │ │ │ │ +215 │ │ │ │ │ +231 static MatrixNN _H_a_t(const TangentVector& xi); │ │ │ │ │ +232 │ │ │ │ │ +_2_3_4 static void _H_a_t(const Vector &xi, Eigen::Ref X); │ │ │ │ │ +235 │ │ │ │ │ +237 static TangentVector _V_e_e(const MatrixNN& X); │ │ │ │ │ +238 │ │ │ │ │ +239 // Chart at origin │ │ │ │ │ +_2_4_0 struct _C_h_a_r_t_A_t_O_r_i_g_i_n { │ │ │ │ │ +245 static _S_O _R_e_t_r_a_c_t(const TangentVector& xi, _C_h_a_r_t_J_a_c_o_b_i_a_n H = boost::none); │ │ │ │ │ +246 │ │ │ │ │ +250 static TangentVector _L_o_c_a_l(const _S_O& R, _C_h_a_r_t_J_a_c_o_b_i_a_n H = boost::none); │ │ │ │ │ +251 }; │ │ │ │ │ +252 │ │ │ │ │ +253 // Return dynamic identity DxD Jacobian for given SO(n) │ │ │ │ │ +254 template > │ │ │ │ │ +255 static MatrixDD IdentityJacobian(size_t n) { │ │ │ │ │ +256 const size_t d = Dimension(n); │ │ │ │ │ +257 return MatrixDD::Identity(d, d); │ │ │ │ │ +258 } │ │ │ │ │ 259 │ │ │ │ │ -261 virtual Matrix covariance() const; │ │ │ │ │ -262 │ │ │ │ │ -263 private: │ │ │ │ │ -_2_6_5 friend class boost::serialization::access; │ │ │ │ │ -266 template │ │ │ │ │ -267 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -268 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -269 ar & BOOST_SERIALIZATION_NVP(sqrt_information_); │ │ │ │ │ -270 } │ │ │ │ │ +263 │ │ │ │ │ +265 MatrixDD _A_d_j_o_i_n_t_M_a_p() const; │ │ │ │ │ +266 │ │ │ │ │ +270 static _S_O _E_x_p_m_a_p(const TangentVector& omega, ChartJacobian H = boost:: │ │ │ │ │ +none); │ │ │ │ │ 271 │ │ │ │ │ -272 }; // Gaussian │ │ │ │ │ -273 │ │ │ │ │ -274 //------------------------------------------------------------------------- │ │ │ │ │ --------------- │ │ │ │ │ -275 │ │ │ │ │ -_2_8_1 class GTSAM_EXPORT _D_i_a_g_o_n_a_l : public _G_a_u_s_s_i_a_n { │ │ │ │ │ -282 protected: │ │ │ │ │ -283 │ │ │ │ │ -_2_8_9 Vector _s_i_g_m_a_s__, invsigmas_, precisions_; │ │ │ │ │ -290 │ │ │ │ │ -291 protected: │ │ │ │ │ -292 │ │ │ │ │ -294 _D_i_a_g_o_n_a_l(const Vector& sigmas); │ │ │ │ │ -295 │ │ │ │ │ -296 public: │ │ │ │ │ -298 _D_i_a_g_o_n_a_l(); │ │ │ │ │ -299 │ │ │ │ │ -300 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -301 │ │ │ │ │ -302 _~_D_i_a_g_o_n_a_l() override {} │ │ │ │ │ -303 │ │ │ │ │ -308 static shared_ptr Sigmas(const Vector& sigmas, bool smart = true); │ │ │ │ │ +273 static MatrixDD _E_x_p_m_a_p_D_e_r_i_v_a_t_i_v_e(const TangentVector& omega); │ │ │ │ │ +274 │ │ │ │ │ +278 static TangentVector _L_o_g_m_a_p(const _S_O& R, ChartJacobian H = boost::none); │ │ │ │ │ +279 │ │ │ │ │ +281 static MatrixDD _L_o_g_m_a_p_D_e_r_i_v_a_t_i_v_e(const TangentVector& omega); │ │ │ │ │ +282 │ │ │ │ │ +283 // inverse with optional derivative │ │ │ │ │ +284 using _L_i_e_G_r_o_u_p<_S_O_<_N_>, internal::DimensionSO(N)>_:_:_i_n_v_e_r_s_e; │ │ │ │ │ +285 │ │ │ │ │ +289 │ │ │ │ │ +295 VectorN2 _v_e_c(_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n H = │ │ │ │ │ +296 boost::none) const; │ │ │ │ │ +297 │ │ │ │ │ +299 template > │ │ │ │ │ +_3_0_0 static Matrix _V_e_c_t_o_r_i_z_e_d_G_e_n_e_r_a_t_o_r_s() { │ │ │ │ │ +301 constexpr size_t N2 = static_cast(N * N); │ │ │ │ │ +302 Eigen::Matrix G; │ │ │ │ │ +303 for (size_t j = 0; j < dimension; j++) { │ │ │ │ │ +304 const auto X = _H_a_t(Vector::Unit(dimension, j)); │ │ │ │ │ +305 G.col(j) = Eigen::Map(X.data()); │ │ │ │ │ +306 } │ │ │ │ │ +307 return G; │ │ │ │ │ +308 } │ │ │ │ │ 309 │ │ │ │ │ -316 static shared_ptr Variances(const Vector& variances, bool smart = true); │ │ │ │ │ -317 │ │ │ │ │ -322 static shared_ptr Precisions(const Vector& precisions, bool smart = true); │ │ │ │ │ -323 │ │ │ │ │ -324 void _p_r_i_n_t(const std::string& name) const override; │ │ │ │ │ -_3_2_5 Vector _s_i_g_m_a_s() const override { return sigmas_; } │ │ │ │ │ -326 Vector whiten(const Vector& v) const override; │ │ │ │ │ -327 Vector unwhiten(const Vector& v) const override; │ │ │ │ │ -328 Matrix Whiten(const Matrix& H) const override; │ │ │ │ │ -329 void WhitenInPlace(Matrix& H) const override; │ │ │ │ │ -330 void WhitenInPlace(Eigen::Block H) const override; │ │ │ │ │ -331 │ │ │ │ │ -_3_3_5 inline double _s_i_g_m_a(size_t i) const { return sigmas_(i); } │ │ │ │ │ -336 │ │ │ │ │ -_3_4_0 inline const Vector& _i_n_v_s_i_g_m_a_s() const { return invsigmas_; } │ │ │ │ │ -341 inline double invsigma(size_t i) const {return invsigmas_(i);} │ │ │ │ │ -342 │ │ │ │ │ -_3_4_6 inline const Vector& _p_r_e_c_i_s_i_o_n_s() const { return precisions_; } │ │ │ │ │ -347 inline double precision(size_t i) const {return precisions_(i);} │ │ │ │ │ -348 │ │ │ │ │ -_3_5_2 Matrix _R() const override { │ │ │ │ │ -353 return invsigmas().asDiagonal(); │ │ │ │ │ -354 } │ │ │ │ │ -355 │ │ │ │ │ -356 private: │ │ │ │ │ -_3_5_8 friend class boost::serialization::access; │ │ │ │ │ -359 template │ │ │ │ │ -360 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -361 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_G_a_u_s_s_i_a_n); │ │ │ │ │ -362 ar & BOOST_SERIALIZATION_NVP(sigmas_); │ │ │ │ │ -363 ar & BOOST_SERIALIZATION_NVP(invsigmas_); │ │ │ │ │ -364 } │ │ │ │ │ -365 }; // Diagonal │ │ │ │ │ -366 │ │ │ │ │ -367 //------------------------------------------------------------------------- │ │ │ │ │ --------------- │ │ │ │ │ -368 │ │ │ │ │ -_3_8_1 class GTSAM_EXPORT _C_o_n_s_t_r_a_i_n_e_d : public _D_i_a_g_o_n_a_l { │ │ │ │ │ -382 protected: │ │ │ │ │ -383 │ │ │ │ │ -384 // Sigmas are contained in the base class │ │ │ │ │ -_3_8_5 Vector _m_u__; │ │ │ │ │ -386 │ │ │ │ │ -392 _C_o_n_s_t_r_a_i_n_e_d(const Vector& mu, const Vector& sigmas); │ │ │ │ │ -393 │ │ │ │ │ -394 public: │ │ │ │ │ -395 │ │ │ │ │ -396 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +311 template > │ │ │ │ │ +_3_1_2 static Matrix _V_e_c_t_o_r_i_z_e_d_G_e_n_e_r_a_t_o_r_s(size_t n = 0) { │ │ │ │ │ +313 const size_t n2 = n * n, dim = Dimension(n); │ │ │ │ │ +314 Matrix G(n2, dim); │ │ │ │ │ +315 for (size_t j = 0; j < dim; j++) { │ │ │ │ │ +316 const auto X = _H_a_t(Vector::Unit(dim, j)); │ │ │ │ │ +317 G.col(j) = Eigen::Map(X.data(), n2, 1); │ │ │ │ │ +318 } │ │ │ │ │ +319 return G; │ │ │ │ │ +320 } │ │ │ │ │ +321 │ │ │ │ │ +325 │ │ │ │ │ +326 template │ │ │ │ │ +327 friend void save(Archive&, _S_O&, const unsigned int); │ │ │ │ │ +328 template │ │ │ │ │ +329 friend void load(Archive&, _S_O&, const unsigned int); │ │ │ │ │ +330 template │ │ │ │ │ +331 friend void serialize(Archive&, _S_O&, const unsigned int); │ │ │ │ │ +332 friend class boost::serialization::access; │ │ │ │ │ +333 friend class _R_o_t_3; // for serialize │ │ │ │ │ +334 │ │ │ │ │ +336}; │ │ │ │ │ +337 │ │ │ │ │ +338using SOn = SO; │ │ │ │ │ +339 │ │ │ │ │ +340/* │ │ │ │ │ +341 * Specialize dynamic Hat and Vee, because recursion depends on dynamic │ │ │ │ │ +nature. │ │ │ │ │ +342 * The definition is in SOn.cpp. Fixed-size SO3 and SO4 have their own │ │ │ │ │ +version, │ │ │ │ │ +343 * and implementation for other fixed N is in SOn-inl.h. │ │ │ │ │ +344 */ │ │ │ │ │ +345 │ │ │ │ │ +346template <> │ │ │ │ │ +347GTSAM_EXPORT │ │ │ │ │ +348Matrix _S_O_n_:_:_H_a_t(const Vector& xi); │ │ │ │ │ +349 │ │ │ │ │ +350template <> │ │ │ │ │ +351GTSAM_EXPORT │ │ │ │ │ +352Vector _S_O_n_:_:_V_e_e(const Matrix& X); │ │ │ │ │ +353 │ │ │ │ │ +354/* │ │ │ │ │ +355 * Specialize dynamic compose and between, because the derivative is │ │ │ │ │ +unknowable │ │ │ │ │ +356 * by the LieGroup implementations, who return a fixed-size matrix for H2. │ │ │ │ │ +357 */ │ │ │ │ │ +358 │ │ │ │ │ +359using DynamicJacobian = OptionalJacobian; │ │ │ │ │ +360 │ │ │ │ │ +361template <> │ │ │ │ │ +362GTSAM_EXPORT │ │ │ │ │ +363SOn LieGroup::compose(const SOn& g, DynamicJacobian H1, │ │ │ │ │ +364 DynamicJacobian H2) const; │ │ │ │ │ +365 │ │ │ │ │ +366template <> │ │ │ │ │ +367GTSAM_EXPORT │ │ │ │ │ +368SOn LieGroup::between(const SOn& g, DynamicJacobian H1, │ │ │ │ │ +369 DynamicJacobian H2) const; │ │ │ │ │ +370 │ │ │ │ │ +371/* │ │ │ │ │ +372 * Specialize dynamic vec. │ │ │ │ │ +373 */ │ │ │ │ │ +374template <> │ │ │ │ │ +375GTSAM_EXPORT │ │ │ │ │ +376typename SOn::VectorN2 _S_O_n_:_:_v_e_c(DynamicJacobian H) const; │ │ │ │ │ +377 │ │ │ │ │ +379template │ │ │ │ │ +_3_8_0void _s_e_r_i_a_l_i_z_e( │ │ │ │ │ +381 Archive& ar, _S_O_n& Q, │ │ │ │ │ +382 const unsigned int file_version │ │ │ │ │ +383) { │ │ │ │ │ +384 Matrix& M = Q.matrix_; │ │ │ │ │ +385 ar& BOOST_SERIALIZATION_NVP(M); │ │ │ │ │ +386} │ │ │ │ │ +387 │ │ │ │ │ +388/* │ │ │ │ │ +389 * Define the traits. internal::LieGroup provides both Lie group and │ │ │ │ │ +Testable │ │ │ │ │ +390 */ │ │ │ │ │ +391 │ │ │ │ │ +392template │ │ │ │ │ +_3_9_3struct _t_r_a_i_t_s<_S_O> : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p> {}; │ │ │ │ │ +394 │ │ │ │ │ +395template │ │ │ │ │ +_3_9_6struct _t_r_a_i_t_s> : public _i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p> {}; │ │ │ │ │ 397 │ │ │ │ │ -404 _C_o_n_s_t_r_a_i_n_e_d(const Vector& sigmas = Z_1x1); │ │ │ │ │ -405 │ │ │ │ │ -406 _~_C_o_n_s_t_r_a_i_n_e_d() override {} │ │ │ │ │ -407 │ │ │ │ │ -_4_0_9 bool _i_s_C_o_n_s_t_r_a_i_n_e_d() const override { return true; } │ │ │ │ │ -410 │ │ │ │ │ -412 bool constrained(size_t i) const; │ │ │ │ │ -413 │ │ │ │ │ -_4_1_5 const Vector& _m_u() const { return mu_; } │ │ │ │ │ -416 │ │ │ │ │ -421 static shared_ptr MixedSigmas(const Vector& mu, const Vector& sigmas); │ │ │ │ │ -422 │ │ │ │ │ -427 static shared_ptr MixedSigmas(const Vector& sigmas); │ │ │ │ │ -428 │ │ │ │ │ -433 static shared_ptr MixedSigmas(double m, const Vector& sigmas); │ │ │ │ │ -434 │ │ │ │ │ -439 static shared_ptr MixedVariances(const Vector& mu, const Vector& │ │ │ │ │ -variances); │ │ │ │ │ -440 static shared_ptr MixedVariances(const Vector& variances); │ │ │ │ │ -441 │ │ │ │ │ -446 static shared_ptr MixedPrecisions(const Vector& mu, const Vector& │ │ │ │ │ -precisions); │ │ │ │ │ -447 static shared_ptr MixedPrecisions(const Vector& precisions); │ │ │ │ │ -448 │ │ │ │ │ -454 double squaredMahalanobisDistance(const Vector& v) const override; │ │ │ │ │ -455 │ │ │ │ │ -_4_5_7 static shared_ptr _A_l_l(size_t dim) { │ │ │ │ │ -458 return shared_ptr(new _C_o_n_s_t_r_a_i_n_e_d(Vector::Constant(dim, 1000.0), Vector:: │ │ │ │ │ -Constant(dim,0))); │ │ │ │ │ -459 } │ │ │ │ │ -460 │ │ │ │ │ -_4_6_2 static shared_ptr _A_l_l(size_t dim, const Vector& mu) { │ │ │ │ │ -463 return shared_ptr(new _C_o_n_s_t_r_a_i_n_e_d(mu, Vector::Constant(dim,0))); │ │ │ │ │ -464 } │ │ │ │ │ -465 │ │ │ │ │ -_4_6_7 static shared_ptr _A_l_l(size_t dim, double mu) { │ │ │ │ │ -468 return shared_ptr(new _C_o_n_s_t_r_a_i_n_e_d(Vector::Constant(dim, mu), Vector:: │ │ │ │ │ -Constant(dim,0))); │ │ │ │ │ -469 } │ │ │ │ │ -470 │ │ │ │ │ -471 void _p_r_i_n_t(const std::string& name) const override; │ │ │ │ │ -472 │ │ │ │ │ -474 Vector whiten(const Vector& v) const override; │ │ │ │ │ -475 │ │ │ │ │ -478 Matrix Whiten(const Matrix& H) const override; │ │ │ │ │ -479 void WhitenInPlace(Matrix& H) const override; │ │ │ │ │ -480 void WhitenInPlace(Eigen::Block H) const override; │ │ │ │ │ -481 │ │ │ │ │ -491 Diagonal::shared_ptr QR(Matrix& Ab) const override; │ │ │ │ │ -492 │ │ │ │ │ -497 shared_ptr unit() const; │ │ │ │ │ -498 │ │ │ │ │ -499 private: │ │ │ │ │ -_5_0_1 friend class boost::serialization::access; │ │ │ │ │ -502 template │ │ │ │ │ -503 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -504 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_D_i_a_g_o_n_a_l); │ │ │ │ │ -505 ar & BOOST_SERIALIZATION_NVP(mu_); │ │ │ │ │ -506 } │ │ │ │ │ -507 │ │ │ │ │ -508 }; // Constrained │ │ │ │ │ -509 │ │ │ │ │ -510 //------------------------------------------------------------------------- │ │ │ │ │ --------------- │ │ │ │ │ -511 │ │ │ │ │ -_5_1_6 class GTSAM_EXPORT _I_s_o_t_r_o_p_i_c : public _D_i_a_g_o_n_a_l { │ │ │ │ │ -517 protected: │ │ │ │ │ -518 double sigma_, invsigma_; │ │ │ │ │ -519 │ │ │ │ │ -_5_2_1 _I_s_o_t_r_o_p_i_c(size_t dim, double sigma) : │ │ │ │ │ -522 _D_i_a_g_o_n_a_l(Vector::Constant(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) │ │ │ │ │ -{} │ │ │ │ │ -523 │ │ │ │ │ -524 public: │ │ │ │ │ -525 │ │ │ │ │ -526 /* dummy constructor to allow for serialization */ │ │ │ │ │ -527 _I_s_o_t_r_o_p_i_c() : _D_i_a_g_o_n_a_l(Vector1::Constant(1.0)),sigma_(1.0),invsigma_(1.0) │ │ │ │ │ -{} │ │ │ │ │ -528 │ │ │ │ │ -529 ~Isotropic() override {} │ │ │ │ │ -530 │ │ │ │ │ -531 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -532 │ │ │ │ │ -536 static shared_ptr Sigma(size_t dim, double sigma, bool smart = true); │ │ │ │ │ -537 │ │ │ │ │ -544 static shared_ptr Variance(size_t dim, double variance, bool smart = true); │ │ │ │ │ -545 │ │ │ │ │ -_5_4_9 static shared_ptr _P_r_e_c_i_s_i_o_n(size_t dim, double precision, bool smart = │ │ │ │ │ -true) { │ │ │ │ │ -550 return Variance(dim, 1.0/precision, smart); │ │ │ │ │ -551 } │ │ │ │ │ -552 │ │ │ │ │ -553 void _p_r_i_n_t(const std::string& name) const override; │ │ │ │ │ -554 double squaredMahalanobisDistance(const Vector& v) const override; │ │ │ │ │ -555 Vector whiten(const Vector& v) const override; │ │ │ │ │ -556 Vector unwhiten(const Vector& v) const override; │ │ │ │ │ -557 Matrix Whiten(const Matrix& H) const override; │ │ │ │ │ -558 void WhitenInPlace(Matrix& H) const override; │ │ │ │ │ -559 void whitenInPlace(Vector& v) const override; │ │ │ │ │ -560 void WhitenInPlace(Eigen::Block H) const override; │ │ │ │ │ -561 │ │ │ │ │ -_5_6_5 inline double _s_i_g_m_a() const { return sigma_; } │ │ │ │ │ -566 │ │ │ │ │ -567 private: │ │ │ │ │ -_5_6_9 friend class boost::serialization::access; │ │ │ │ │ -570 template │ │ │ │ │ -571 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -572 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_D_i_a_g_o_n_a_l); │ │ │ │ │ -573 ar & BOOST_SERIALIZATION_NVP(sigma_); │ │ │ │ │ -574 ar & BOOST_SERIALIZATION_NVP(invsigma_); │ │ │ │ │ -575 } │ │ │ │ │ -576 │ │ │ │ │ -577 }; │ │ │ │ │ -578 │ │ │ │ │ -579 //------------------------------------------------------------------------- │ │ │ │ │ --------------- │ │ │ │ │ -580 │ │ │ │ │ -_5_8_4 class GTSAM_EXPORT _U_n_i_t : public _I_s_o_t_r_o_p_i_c { │ │ │ │ │ -585 public: │ │ │ │ │ -586 │ │ │ │ │ -587 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -588 │ │ │ │ │ -_5_9_0 _U_n_i_t(size_t dim=1): _I_s_o_t_r_o_p_i_c(dim,1.0) {} │ │ │ │ │ -591 │ │ │ │ │ -592 _~_U_n_i_t() override {} │ │ │ │ │ -593 │ │ │ │ │ -_5_9_7 static shared_ptr _C_r_e_a_t_e(size_t dim) { │ │ │ │ │ -598 return shared_ptr(new _U_n_i_t(dim)); │ │ │ │ │ -599 } │ │ │ │ │ -600 │ │ │ │ │ -_6_0_2 bool _i_s_U_n_i_t() const override { return true; } │ │ │ │ │ -603 │ │ │ │ │ -604 void _p_r_i_n_t(const std::string& name) const override; │ │ │ │ │ -605 double squaredMahalanobisDistance(const Vector& v) const override; │ │ │ │ │ -_6_0_6 Vector _w_h_i_t_e_n(const Vector& v) const override { return v; } │ │ │ │ │ -_6_0_7 Vector _u_n_w_h_i_t_e_n(const Vector& v) const override { return v; } │ │ │ │ │ -_6_0_8 Matrix _W_h_i_t_e_n(const Matrix& H) const override { return H; } │ │ │ │ │ -_6_0_9 void _W_h_i_t_e_n_I_n_P_l_a_c_e(Matrix& /*H*/) const override {} │ │ │ │ │ -_6_1_0 void _W_h_i_t_e_n_I_n_P_l_a_c_e(Eigen::Block /*H*/) const override {} │ │ │ │ │ -_6_1_1 void _w_h_i_t_e_n_I_n_P_l_a_c_e(Vector& /*v*/) const override {} │ │ │ │ │ -_6_1_2 void _u_n_w_h_i_t_e_n_I_n_P_l_a_c_e(Vector& /*v*/) const override {} │ │ │ │ │ -_6_1_3 void _w_h_i_t_e_n_I_n_P_l_a_c_e(Eigen::Block& /*v*/) const override {} │ │ │ │ │ -_6_1_4 void _u_n_w_h_i_t_e_n_I_n_P_l_a_c_e(Eigen::Block& /*v*/) const override {} │ │ │ │ │ -615 │ │ │ │ │ -616 private: │ │ │ │ │ -_6_1_8 friend class boost::serialization::access; │ │ │ │ │ -619 template │ │ │ │ │ -620 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -621 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_I_s_o_t_r_o_p_i_c); │ │ │ │ │ -622 } │ │ │ │ │ -623 }; │ │ │ │ │ -624 │ │ │ │ │ -_6_4_2 class GTSAM_EXPORT _R_o_b_u_s_t : public _B_a_s_e { │ │ │ │ │ -643 public: │ │ │ │ │ -644 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -645 │ │ │ │ │ -646 protected: │ │ │ │ │ -647 typedef _m_E_s_t_i_m_a_t_o_r_:_:_B_a_s_e _R_o_b_u_s_t_M_o_d_e_l; │ │ │ │ │ -648 typedef _n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e _N_o_i_s_e_M_o_d_e_l; │ │ │ │ │ -649 │ │ │ │ │ -_6_5_0 const RobustModel::shared_ptr _r_o_b_u_s_t__; │ │ │ │ │ -_6_5_1 const NoiseModel::shared_ptr _n_o_i_s_e__; │ │ │ │ │ -652 │ │ │ │ │ -653 public: │ │ │ │ │ -654 │ │ │ │ │ -_6_5_6 _R_o_b_u_s_t() {}; │ │ │ │ │ -657 │ │ │ │ │ -_6_5_9 _R_o_b_u_s_t(const RobustModel::shared_ptr robust, const NoiseModel::shared_ptr │ │ │ │ │ -noise) │ │ │ │ │ -660 : _B_a_s_e(noise->dim()), robust_(robust), noise_(noise) {} │ │ │ │ │ -661 │ │ │ │ │ -_6_6_3 _~_R_o_b_u_s_t() override {} │ │ │ │ │ -664 │ │ │ │ │ -665 void _p_r_i_n_t(const std::string& name) const override; │ │ │ │ │ -666 bool _e_q_u_a_l_s(const _B_a_s_e& expected, double tol=1e-9) const override; │ │ │ │ │ -667 │ │ │ │ │ -_6_6_9 const RobustModel::shared_ptr& _r_o_b_u_s_t() const { return robust_; } │ │ │ │ │ -670 │ │ │ │ │ -_6_7_2 const NoiseModel::shared_ptr& _n_o_i_s_e() const { return noise_; } │ │ │ │ │ -673 │ │ │ │ │ -674 // Functions below are dummy but necessary for the noiseModel::Base │ │ │ │ │ -_6_7_5 inline Vector _w_h_i_t_e_n(const Vector& v) const override │ │ │ │ │ -676 { Vector r = v; this->WhitenSystem(r); return r; } │ │ │ │ │ -_6_7_7 inline Matrix _W_h_i_t_e_n(const Matrix& A) const override │ │ │ │ │ -678 { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; } │ │ │ │ │ -_6_7_9 inline Vector _u_n_w_h_i_t_e_n(const Vector& /*v*/) const override │ │ │ │ │ -680 { throw std::invalid_argument("unwhiten is not currently supported for │ │ │ │ │ -robust noise models."); } │ │ │ │ │ -_6_8_2 double _l_o_s_s(const double squared_distance) const override { │ │ │ │ │ -683 return robust_->loss(std::sqrt(squared_distance)); │ │ │ │ │ -684 } │ │ │ │ │ -685 │ │ │ │ │ -686 // NOTE: This is special because in whiten the base version will do the │ │ │ │ │ -reweighting │ │ │ │ │ -687 // which is incorrect! │ │ │ │ │ -_6_8_8 double _s_q_u_a_r_e_d_M_a_h_a_l_a_n_o_b_i_s_D_i_s_t_a_n_c_e(const Vector& v) const override { │ │ │ │ │ -689 return noise_->squaredMahalanobisDistance(v); │ │ │ │ │ -690 } │ │ │ │ │ -691 │ │ │ │ │ -692 // These are really robust iterated re-weighting support functions │ │ │ │ │ -693 virtual void WhitenSystem(Vector& b) const; │ │ │ │ │ -694 void WhitenSystem(std::vector& A, Vector& b) const override; │ │ │ │ │ -695 void WhitenSystem(Matrix& A, Vector& b) const override; │ │ │ │ │ -696 void WhitenSystem(Matrix& A1, Matrix& A2, Vector& b) const override; │ │ │ │ │ -697 void WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const │ │ │ │ │ -override; │ │ │ │ │ -698 │ │ │ │ │ -699 Vector unweightedWhiten(const Vector& v) const override; │ │ │ │ │ -700 double weight(const Vector& v) const override; │ │ │ │ │ -701 │ │ │ │ │ -702 static shared_ptr Create( │ │ │ │ │ -703 const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise); │ │ │ │ │ -704 │ │ │ │ │ -705 private: │ │ │ │ │ -_7_0_7 friend class boost::serialization::access; │ │ │ │ │ -708 template │ │ │ │ │ -709 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -710 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -711 ar & boost::serialization::make_nvp("robust_", const_cast(robust_)); │ │ │ │ │ -712 ar & boost::serialization::make_nvp("noise_", const_cast(noise_)); │ │ │ │ │ -713 } │ │ │ │ │ -714 }; │ │ │ │ │ -715 │ │ │ │ │ -716 // Helper function │ │ │ │ │ -717 GTSAM_EXPORT boost::optional checkIfDiagonal(const Matrix& M); │ │ │ │ │ -718 │ │ │ │ │ -719 } // namespace noiseModel │ │ │ │ │ -720 │ │ │ │ │ -_7_2_4 typedef noiseModel::Base::shared_ptr _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l; │ │ │ │ │ -725 typedef noiseModel::Gaussian::shared_ptr SharedGaussian; │ │ │ │ │ -726 typedef noiseModel::Diagonal::shared_ptr SharedDiagonal; │ │ │ │ │ -727 typedef noiseModel::Constrained::shared_ptr SharedConstrained; │ │ │ │ │ -728 typedef noiseModel::Isotropic::shared_ptr SharedIsotropic; │ │ │ │ │ -729 │ │ │ │ │ -_7_3_1 template<> struct _t_r_a_i_t_s : public │ │ │ │ │ -_T_e_s_t_a_b_l_e {}; │ │ │ │ │ -_7_3_2 template<> struct _t_r_a_i_t_s : public │ │ │ │ │ -_T_e_s_t_a_b_l_e {}; │ │ │ │ │ -_7_3_3 template<> struct _t_r_a_i_t_s : public │ │ │ │ │ -_T_e_s_t_a_b_l_e {}; │ │ │ │ │ -_7_3_4 template<> struct _t_r_a_i_t_s : public │ │ │ │ │ -_T_e_s_t_a_b_l_e {}; │ │ │ │ │ -_7_3_5 template<> struct _t_r_a_i_t_s : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ -736 │ │ │ │ │ -737} //\ namespace gtsam │ │ │ │ │ -738 │ │ │ │ │ -739 │ │ │ │ │ -_T_e_s_t_a_b_l_e_._h │ │ │ │ │ -Concept check for values that can be used in unit tests. │ │ │ │ │ -_M_a_t_r_i_x_._h │ │ │ │ │ -typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +398} // namespace gtsam │ │ │ │ │ +399 │ │ │ │ │ +400#include "_S_O_n_-_i_n_l_._h" │ │ │ │ │ +_m_a_k_e___s_h_a_r_e_d_._h │ │ │ │ │ +make_shared trampoline function to ensure proper alignment │ │ │ │ │ +_M_a_n_i_f_o_l_d_._h │ │ │ │ │ +Base class and basic functions for Manifold types. │ │ │ │ │ +_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W___I_F │ │ │ │ │ +#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) │ │ │ │ │ +This marks a GTSAM object to require alignment. │ │ │ │ │ +DDeeffiinniittiioonn types.h:317 │ │ │ │ │ +_L_i_e_._h │ │ │ │ │ +Base class and basic functions for Lie types. │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_D_i_m_e_n_s_i_o_n_S_O │ │ │ │ │ +constexpr int DimensionSO(int N) │ │ │ │ │ +Calculate dimensionality of SO manifold, or return Dynamic if so. │ │ │ │ │ +DDeeffiinniittiioonn SOn.h:39 │ │ │ │ │ +_S_O_n_-_i_n_l_._h │ │ │ │ │ +Template implementations for SO(n) │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ -Aliases. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ +_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e │ │ │ │ │ +std::string serialize(const T &input) │ │ │ │ │ +serializes to a string │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:113 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l___w_i_t_h___a_b_s___t_o_l │ │ │ │ │ +bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen:: │ │ │ │ │ +DenseBase< MATRIX > &B, double tol=1e-9) │ │ │ │ │ +equals with a tolerance │ │ │ │ │ +DDeeffiinniittiioonn Matrix.h:81 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_m_E_s_t_i_m_a_t_o_r_:_:_B_a_s_e │ │ │ │ │ -Pure virtual class for all robust error function classes. │ │ │ │ │ -DDeeffiinniittiioonn LossFunctions.h:63 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e │ │ │ │ │ -noiseModel::Base is the abstract base class for all noise models. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:53 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_i_s_C_o_n_s_t_r_a_i_n_e_d │ │ │ │ │ -virtual bool isConstrained() const │ │ │ │ │ -true if a constrained noise model, saves slow/clumsy dynamic casting │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:69 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_w_h_i_t_e_n_I_n_P_l_a_c_e │ │ │ │ │ -virtual void whitenInPlace(Vector &v) const │ │ │ │ │ -in-place whiten, override if can be done more efficiently │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:112 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_d_i_m │ │ │ │ │ -size_t dim() const │ │ │ │ │ -Dimensionality. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_u_n_w_h_i_t_e_n_I_n_P_l_a_c_e │ │ │ │ │ -virtual void unwhitenInPlace(Eigen::Block< Vector > &v) const │ │ │ │ │ -in-place unwhiten, override if can be done more efficiently │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:127 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_w_h_i_t_e_n_I_n_P_l_a_c_e │ │ │ │ │ -virtual void whitenInPlace(Eigen::Block< Vector > &v) const │ │ │ │ │ -in-place whiten, override if can be done more efficiently │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:122 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_w_h_i_t_e_n │ │ │ │ │ -virtual Vector whiten(const Vector &v) const =0 │ │ │ │ │ -Whiten an error vector. │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_m_a_h_a_l_a_n_o_b_i_s_D_i_s_t_a_n_c_e │ │ │ │ │ -virtual double mahalanobisDistance(const Vector &v) const │ │ │ │ │ -Mahalanobis distance. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:97 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_i_s_U_n_i_t │ │ │ │ │ -virtual bool isUnit() const │ │ │ │ │ -true if a unit noise model, saves slow/clumsy dynamic casting │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:72 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_w_e_i_g_h_t │ │ │ │ │ -virtual double weight(const Vector &v) const │ │ │ │ │ -get the weight from the effective loss function on residual vector v │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:137 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_u_n_w_e_i_g_h_t_e_d_W_h_i_t_e_n │ │ │ │ │ -virtual Vector unweightedWhiten(const Vector &v) const │ │ │ │ │ -Useful function for robust noise models to get the unweighted but whitened │ │ │ │ │ -error. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:132 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_u_n_w_h_i_t_e_n │ │ │ │ │ -virtual Vector unwhiten(const Vector &v) const =0 │ │ │ │ │ -Unwhiten an error vector. │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_l_o_s_s │ │ │ │ │ -virtual double loss(const double squared_distance) const │ │ │ │ │ -loss function, input is Mahalanobis distance │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:102 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_u_n_w_h_i_t_e_n_I_n_P_l_a_c_e │ │ │ │ │ -virtual void unwhitenInPlace(Vector &v) const │ │ │ │ │ -in-place unwhiten, override if can be done more efficiently │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:117 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_W_h_i_t_e_n │ │ │ │ │ -virtual Matrix Whiten(const Matrix &H) const =0 │ │ │ │ │ -Whiten a matrix. │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_B_a_s_e_:_:_B_a_s_e │ │ │ │ │ -Base(size_t dim=1) │ │ │ │ │ -primary constructor │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_G_a_u_s_s_i_a_n │ │ │ │ │ -Gaussian implements the mathematical model |R*x|^2 = |y|^2 with R'*R=inv(Sigma) │ │ │ │ │ -where y = whiten(x) =... │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:162 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_G_a_u_s_s_i_a_n_:_:_R │ │ │ │ │ -virtual Matrix R() const │ │ │ │ │ -Return R itself, but note that Whiten(H) is cheaper than R*H. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:255 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_G_a_u_s_s_i_a_n_:_:_G_a_u_s_s_i_a_n │ │ │ │ │ -Gaussian(size_t dim=1, const boost::optional< Matrix > │ │ │ │ │ -&sqrt_information=boost::none) │ │ │ │ │ -constructor takes square root information matrix │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:186 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_G_a_u_s_s_i_a_n_:_:_s_q_r_t___i_n_f_o_r_m_a_t_i_o_n__ │ │ │ │ │ -boost::optional< Matrix > sqrt_information_ │ │ │ │ │ -Matrix square root of information matrix (R) │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:167 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_D_i_a_g_o_n_a_l │ │ │ │ │ -A diagonal noise model implements a diagonal covariance matrix, with the │ │ │ │ │ -elements of the diagonal spe... │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:281 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_D_i_a_g_o_n_a_l_:_:_R │ │ │ │ │ -Matrix R() const override │ │ │ │ │ -Return R itself, but note that Whiten(H) is cheaper than R*H. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:352 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_D_i_a_g_o_n_a_l_:_:_s_i_g_m_a_s__ │ │ │ │ │ -Vector sigmas_ │ │ │ │ │ -Standard deviations (sigmas), their inverse and inverse square (weights/ │ │ │ │ │ -precisions) These are all com... │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:289 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_D_i_a_g_o_n_a_l_:_:_s_i_g_m_a │ │ │ │ │ -double sigma(size_t i) const │ │ │ │ │ -Return standard deviations (sqrt of diagonal) │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:335 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_D_i_a_g_o_n_a_l_:_:_i_n_v_s_i_g_m_a_s │ │ │ │ │ -const Vector & invsigmas() const │ │ │ │ │ -Return sqrt precisions. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:340 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_D_i_a_g_o_n_a_l_:_:_s_i_g_m_a_s │ │ │ │ │ -Vector sigmas() const override │ │ │ │ │ -Calculate standard deviations. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:325 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_D_i_a_g_o_n_a_l_:_:_p_r_e_c_i_s_i_o_n_s │ │ │ │ │ -const Vector & precisions() const │ │ │ │ │ -Return precisions. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:346 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_C_o_n_s_t_r_a_i_n_e_d │ │ │ │ │ -A Constrained constrained model is a specialization of Diagonal which allows │ │ │ │ │ -some or all of the sigma... │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:381 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_C_o_n_s_t_r_a_i_n_e_d_:_:_i_s_C_o_n_s_t_r_a_i_n_e_d │ │ │ │ │ -bool isConstrained() const override │ │ │ │ │ -true if a constrained noise mode, saves slow/clumsy dynamic casting │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:409 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_C_o_n_s_t_r_a_i_n_e_d_:_:_A_l_l │ │ │ │ │ -static shared_ptr All(size_t dim, const Vector &mu) │ │ │ │ │ -Fully constrained variations. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:462 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_C_o_n_s_t_r_a_i_n_e_d_:_:_A_l_l │ │ │ │ │ -static shared_ptr All(size_t dim, double mu) │ │ │ │ │ -Fully constrained variations with a mu parameter. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:467 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_C_o_n_s_t_r_a_i_n_e_d_:_:_A_l_l │ │ │ │ │ -static shared_ptr All(size_t dim) │ │ │ │ │ -Fully constrained variations. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:457 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_C_o_n_s_t_r_a_i_n_e_d_:_:_m_u │ │ │ │ │ -const Vector & mu() const │ │ │ │ │ -Access mu as a vector. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:415 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_C_o_n_s_t_r_a_i_n_e_d_:_:_m_u__ │ │ │ │ │ -Vector mu_ │ │ │ │ │ -Penalty function weight - needs to be large enough to dominate soft │ │ │ │ │ -constraints. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:385 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c │ │ │ │ │ -An isotropic noise model corresponds to a scaled diagonal covariance To │ │ │ │ │ -construct,... │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:516 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c_:_:_s_i_g_m_a │ │ │ │ │ -double sigma() const │ │ │ │ │ -Return standard deviation. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:565 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c_:_:_P_r_e_c_i_s_i_o_n │ │ │ │ │ -static shared_ptr Precision(size_t dim, double precision, bool smart=true) │ │ │ │ │ -An isotropic noise model created by specifying a precision. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:549 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c_:_:_I_s_o_t_r_o_p_i_c │ │ │ │ │ -Isotropic(size_t dim, double sigma) │ │ │ │ │ -protected constructor takes sigma │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:521 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t │ │ │ │ │ -Unit: i.i.d. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:584 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_W_h_i_t_e_n_I_n_P_l_a_c_e │ │ │ │ │ -void WhitenInPlace(Eigen::Block< Matrix >) const override │ │ │ │ │ -In-place version. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:610 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_u_n_w_h_i_t_e_n │ │ │ │ │ -Vector unwhiten(const Vector &v) const override │ │ │ │ │ -Unwhiten an error vector. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:607 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_w_h_i_t_e_n_I_n_P_l_a_c_e │ │ │ │ │ -void whitenInPlace(Eigen::Block< Vector > &) const override │ │ │ │ │ -in-place whiten, override if can be done more efficiently │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:613 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_i_s_U_n_i_t │ │ │ │ │ -bool isUnit() const override │ │ │ │ │ -true if a unit noise model, saves slow/clumsy dynamic casting │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:602 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_U_n_i_t │ │ │ │ │ -Unit(size_t dim=1) │ │ │ │ │ -constructor for serialization │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:590 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_w_h_i_t_e_n │ │ │ │ │ -Vector whiten(const Vector &v) const override │ │ │ │ │ -Whiten an error vector. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:606 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_C_r_e_a_t_e │ │ │ │ │ -static shared_ptr Create(size_t dim) │ │ │ │ │ -Create a unit covariance noise model. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:597 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_u_n_w_h_i_t_e_n_I_n_P_l_a_c_e │ │ │ │ │ -void unwhitenInPlace(Vector &) const override │ │ │ │ │ -in-place unwhiten, override if can be done more efficiently │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:612 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_u_n_w_h_i_t_e_n_I_n_P_l_a_c_e │ │ │ │ │ -void unwhitenInPlace(Eigen::Block< Vector > &) const override │ │ │ │ │ -in-place unwhiten, override if can be done more efficiently │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:614 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_w_h_i_t_e_n_I_n_P_l_a_c_e │ │ │ │ │ -void whitenInPlace(Vector &) const override │ │ │ │ │ -in-place whiten, override if can be done more efficiently │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:611 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_W_h_i_t_e_n_I_n_P_l_a_c_e │ │ │ │ │ -void WhitenInPlace(Matrix &) const override │ │ │ │ │ -In-place version. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:609 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_W_h_i_t_e_n │ │ │ │ │ -Matrix Whiten(const Matrix &H) const override │ │ │ │ │ -Whiten a matrix. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:608 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t │ │ │ │ │ -Base class for robust error models The robust M-estimators above simply tell us │ │ │ │ │ -how to re-weight the ... │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:642 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t_:_:_r_o_b_u_s_t │ │ │ │ │ -const RobustModel::shared_ptr & robust() const │ │ │ │ │ -Return the contained robust error function. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:669 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t_:_:_R_o_b_u_s_t │ │ │ │ │ -Robust() │ │ │ │ │ -Default Constructor for serialization. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:656 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t_:_:_R_o_b_u_s_t │ │ │ │ │ -Robust(const RobustModel::shared_ptr robust, const NoiseModel::shared_ptr │ │ │ │ │ -noise) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:659 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t_:_:_s_q_u_a_r_e_d_M_a_h_a_l_a_n_o_b_i_s_D_i_s_t_a_n_c_e │ │ │ │ │ -double squaredMahalanobisDistance(const Vector &v) const override │ │ │ │ │ -Squared Mahalanobis distance v'*R'*R*v = │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:688 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t_:_:_n_o_i_s_e__ │ │ │ │ │ -const NoiseModel::shared_ptr noise_ │ │ │ │ │ -noise model used │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:651 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t_:_:_l_o_s_s │ │ │ │ │ -double loss(const double squared_distance) const override │ │ │ │ │ -Compute loss from the m-estimator using the Mahalanobis distance. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:682 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t_:_:_u_n_w_h_i_t_e_n │ │ │ │ │ -Vector unwhiten(const Vector &) const override │ │ │ │ │ -Unwhiten an error vector. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:679 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t_:_:_W_h_i_t_e_n │ │ │ │ │ -Matrix Whiten(const Matrix &A) const override │ │ │ │ │ -Whiten a matrix. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:677 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t_:_:_r_o_b_u_s_t__ │ │ │ │ │ -const RobustModel::shared_ptr robust_ │ │ │ │ │ -robust error function used │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:650 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t_:_:_~_R_o_b_u_s_t │ │ │ │ │ -~Robust() override │ │ │ │ │ -Destructor. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:663 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t_:_:_n_o_i_s_e │ │ │ │ │ -const NoiseModel::shared_ptr & noise() const │ │ │ │ │ -Return the contained noise model. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:672 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_R_o_b_u_s_t_:_:_w_h_i_t_e_n │ │ │ │ │ -Vector whiten(const Vector &v) const override │ │ │ │ │ -Whiten an error vector. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:675 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ +A CRTP helper class that implements Lie group methods Prerequisites: methods │ │ │ │ │ +operator*,... │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_e_G_r_o_u_p_<_ _S_O_<_ _N_ _>_,_ _i_n_t_e_r_n_a_l_:_:_D_i_m_e_n_s_i_o_n_S_O_(_N_)_>_:_:_R_e_t_r_a_c_t │ │ │ │ │ +static SO< N > Retract(const TangentVector &v) │ │ │ │ │ +Retract at origin: possible in Lie group because it has an identity. │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_L_i_e_G_r_o_u_p │ │ │ │ │ +Both LieGroupTraits and Testable. │ │ │ │ │ +DDeeffiinniittiioonn Lie.h:229 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ +either a fixed size o... │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ +Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor │ │ │ │ │ +symbol GTSAM_USE_QUATERNIO... │ │ │ │ │ +DDeeffiinniittiioonn Rot3.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O │ │ │ │ │ +Manifold of special orthogonal rotation matrices SO. │ │ │ │ │ +DDeeffiinniittiioonn SOn.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_F_r_o_m_M_a_t_r_i_x │ │ │ │ │ +static SO FromMatrix(const Eigen::MatrixBase< Derived > &R) │ │ │ │ │ +Named constructor from Eigen Matrix. │ │ │ │ │ +DDeeffiinniittiioonn SOn.h:95 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_E_x_p_m_a_p │ │ │ │ │ +static SO Expmap(const TangentVector &omega, ChartJacobian H=boost::none) │ │ │ │ │ +Exponential map at identity - create a rotation from canonical coordinates. │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_V_e_c_t_o_r_i_z_e_d_G_e_n_e_r_a_t_o_r_s │ │ │ │ │ +static Matrix VectorizedGenerators() │ │ │ │ │ +Calculate N^2 x dim matrix of vectorized Lie algebra generators for SO(N) │ │ │ │ │ +DDeeffiinniittiioonn SOn.h:300 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_i_n_v_e_r_s_e │ │ │ │ │ +SO inverse() const │ │ │ │ │ +inverse of a rotation = transpose │ │ │ │ │ +DDeeffiinniittiioonn SOn.h:193 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_v_e_c │ │ │ │ │ +VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H=boost:: │ │ │ │ │ +none) const │ │ │ │ │ +Return vectorized rotation matrix in column order. │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:88 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_C_h_o_r_d_a_l_M_e_a_n │ │ │ │ │ +static SO ChordalMean(const std::vector< SO > &rotations) │ │ │ │ │ +Named constructor that finds chordal mean , currently only defined for SO3. │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ +SO operator*(const SO &other) const │ │ │ │ │ +Multiplication. │ │ │ │ │ +DDeeffiinniittiioonn SOn.h:175 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_V_e_e │ │ │ │ │ +static TangentVector Vee(const MatrixNN &X) │ │ │ │ │ +Inverse of Hat. See note about xi element order in Hat. │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_m_a_t_r_i_x__ │ │ │ │ │ +MatrixNN matrix_ │ │ │ │ │ +Rotation matrix. │ │ │ │ │ +DDeeffiinniittiioonn SOn.h:62 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_L_o_g_m_a_p │ │ │ │ │ +static TangentVector Logmap(const SO &R, ChartJacobian H=boost::none) │ │ │ │ │ +Log map at identity - returns the canonical coordinates of this rotation. │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_A_x_i_s_A_n_g_l_e │ │ │ │ │ +static SO AxisAngle(const Vector3 &axis, double theta) │ │ │ │ │ +Constructor from axis and angle. Only defined for SO3. │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_A_d_j_o_i_n_t_M_a_p │ │ │ │ │ +MatrixDD AdjointMap() const │ │ │ │ │ +Adjoint map. │ │ │ │ │ +DDeeffiinniittiioonn SO4.cpp:159 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_H_a_t │ │ │ │ │ +static void Hat(const Vector &xi, Eigen::Ref< MatrixNN > X) │ │ │ │ │ +In-place version of Hat (see details there), implements recursion. │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_I_d_e_n_t_i_t_y │ │ │ │ │ +static SO Identity() │ │ │ │ │ +SO identity for N >= 2. │ │ │ │ │ +DDeeffiinniittiioonn SOn.h:182 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_D_i_m │ │ │ │ │ +static int Dim() │ │ │ │ │ +Return compile-time dimensionality: fixed size N or Eigen::Dynamic. │ │ │ │ │ +DDeeffiinniittiioonn SOn.h:203 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_H_a_t │ │ │ │ │ +static MatrixNN Hat(const TangentVector &xi) │ │ │ │ │ +Hat operator creates Lie algebra element corresponding to d-vector, where d is │ │ │ │ │ +the dimensionality of ... │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:29 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_E_x_p_m_a_p_D_e_r_i_v_a_t_i_v_e │ │ │ │ │ +static MatrixDD ExpmapDerivative(const TangentVector &omega) │ │ │ │ │ +Derivative of Expmap, currently only defined for SO3. │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_L_o_g_m_a_p_D_e_r_i_v_a_t_i_v_e │ │ │ │ │ +static MatrixDD LogmapDerivative(const TangentVector &omega) │ │ │ │ │ +Derivative of Logmap, currently only defined for SO3. │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:82 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_S_O │ │ │ │ │ +SO(const SO< M > &R) │ │ │ │ │ +Construct dynamic SO(n) from Fixed SO │ │ │ │ │ +DDeeffiinniittiioonn SOn.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_S_O │ │ │ │ │ +SO(size_t n=0) │ │ │ │ │ +Construct SO identity for N == Eigen::Dynamic. │ │ │ │ │ +DDeeffiinniittiioonn SOn.h:83 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_S_O │ │ │ │ │ +SO() │ │ │ │ │ +Construct SO identity for N >= 2. │ │ │ │ │ +DDeeffiinniittiioonn SOn.h:79 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_L_i_f_t │ │ │ │ │ +static SO Lift(size_t n, const Eigen::MatrixBase< Derived > &R) │ │ │ │ │ +Named constructor from lower dimensional matrix. │ │ │ │ │ +DDeeffiinniittiioonn SOn.h:101 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_R_a_n_d_o_m │ │ │ │ │ +static SO Random(std::mt19937 &rng, size_t n=0) │ │ │ │ │ +Random SO(n) element (no big claims about uniformity). SO(3) is specialized in │ │ │ │ │ +SO3.... │ │ │ │ │ +DDeeffiinniittiioonn SOn.h:131 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_I_d_e_n_t_i_t_y │ │ │ │ │ +static SO Identity(size_t n=0) │ │ │ │ │ +SO identity for N == Eigen::Dynamic. │ │ │ │ │ +DDeeffiinniittiioonn SOn.h:188 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_m_a_t_r_i_x │ │ │ │ │ +const MatrixNN & matrix() const │ │ │ │ │ +Return matrix. │ │ │ │ │ +DDeeffiinniittiioonn SOn.h:155 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_R_a_n_d_o_m │ │ │ │ │ +static SO Random(std::mt19937 &rng) │ │ │ │ │ +Random SO(N) element (no big claims about uniformity) │ │ │ │ │ +DDeeffiinniittiioonn SOn.h:145 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_C_l_o_s_e_s_t_T_o │ │ │ │ │ +static SO ClosestTo(const MatrixNN &M) │ │ │ │ │ +Named constructor that finds SO(n) matrix closest to M in Frobenius norm, │ │ │ │ │ +currently only defined for ... │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_S_O │ │ │ │ │ +SO(const Eigen::AngleAxisd &angleAxis) │ │ │ │ │ +Constructor from AngleAxisd. │ │ │ │ │ +DDeeffiinniittiioonn SOn.h:115 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_V_e_c_t_o_r_i_z_e_d_G_e_n_e_r_a_t_o_r_s │ │ │ │ │ +static Matrix VectorizedGenerators(size_t n=0) │ │ │ │ │ +Calculate n^2 x dim matrix of vectorized Lie algebra generators for SO(n) │ │ │ │ │ +DDeeffiinniittiioonn SOn.h:312 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_S_O │ │ │ │ │ +SO(const Eigen::MatrixBase< Derived > &R) │ │ │ │ │ +Constructor from Eigen Matrix, dynamic version. │ │ │ │ │ +DDeeffiinniittiioonn SOn.h:91 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n │ │ │ │ │ +DDeeffiinniittiioonn SOn.h:240 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n_:_:_L_o_c_a_l │ │ │ │ │ +static TangentVector Local(const SO &R, ChartJacobian H=boost::none) │ │ │ │ │ +Inverse of Retract. │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_S_O_:_:_C_h_a_r_t_A_t_O_r_i_g_i_n_:_:_R_e_t_r_a_c_t │ │ │ │ │ +static SO Retract(const TangentVector &xi, ChartJacobian H=boost::none) │ │ │ │ │ +Retract uses Cayley map. │ │ │ │ │ +DDeeffiinniittiioonn SOn-inl.h:40 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _l_i_n_e_a_r │ │ │ │ │ - * _N_o_i_s_e_M_o_d_e_l_._h │ │ │ │ │ + * _g_e_o_m_e_t_r_y │ │ │ │ │ + * _S_O_n_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00758_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00686_source.html │ │ │ │┄ Files 95% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/PowerMethod.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/BayesNet.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
PowerMethod.h
│ │ │ │ +
BayesNet.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
│ │ │ │ -
4 * Atlanta, Georgia 30332-0415
│ │ │ │ -
5 * All Rights Reserved
│ │ │ │ -
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
3* GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
4* Atlanta, Georgia 30332-0415
│ │ │ │ +
5* All Rights Reserved
│ │ │ │ +
6* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │ -
8 * See LICENSE for the license information
│ │ │ │ +
8* See LICENSE for the license information
│ │ │ │
9
│ │ │ │ -
10 * -------------------------------------------------------------------------- */
│ │ │ │ +
10* -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/base/Matrix.h>
│ │ │ │ -
23#include <gtsam/base/Vector.h>
│ │ │ │ -
24
│ │ │ │ -
25#include <Eigen/Core>
│ │ │ │ -
26#include <Eigen/Sparse>
│ │ │ │ -
27#include <random>
│ │ │ │ -
28#include <vector>
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/inference/FactorGraph.h>
│ │ │ │ +
22
│ │ │ │ +
23#include <boost/shared_ptr.hpp>
│ │ │ │ +
24#include <string>
│ │ │ │ +
25
│ │ │ │ +
26namespace gtsam {
│ │ │ │ +
27
│ │ │ │ +
28class HybridValues;
│ │ │ │
29
│ │ │ │ -
30namespace gtsam {
│ │ │ │ -
31
│ │ │ │ -
32using Sparse = Eigen::SparseMatrix<double>;
│ │ │ │ -
33
│ │ │ │ -
56template <class Operator>
│ │ │ │ -
│ │ │ │ -
57class PowerMethod {
│ │ │ │ -
58 protected:
│ │ │ │ -
63 const Operator &A_;
│ │ │ │ +
34template <class CONDITIONAL>
│ │ │ │ +
│ │ │ │ +
35class BayesNet : public FactorGraph<CONDITIONAL> {
│ │ │ │ +
36 private:
│ │ │ │ +
37 typedef FactorGraph<CONDITIONAL> Base;
│ │ │ │ +
38
│ │ │ │ +
39 public:
│ │ │ │ +
40 typedef typename boost::shared_ptr<CONDITIONAL>
│ │ │ │ +
41 sharedConditional;
│ │ │ │ +
42
│ │ │ │ +
43 protected:
│ │ │ │ +
46
│ │ │ │ +
48 BayesNet() {}
│ │ │ │ +
49
│ │ │ │ +
51 template <typename ITERATOR>
│ │ │ │ +
│ │ │ │ +
52 BayesNet(ITERATOR firstConditional, ITERATOR lastConditional)
│ │ │ │ +
53 : Base(firstConditional, lastConditional) {}
│ │ │ │ +
│ │ │ │ +
54
│ │ │ │ +
│ │ │ │ +
60 BayesNet(std::initializer_list<sharedConditional> conditionals)
│ │ │ │ +
61 : Base(conditionals) {}
│ │ │ │ +
│ │ │ │ +
62
│ │ │ │
64
│ │ │ │ -
65 const int dim_; // dimension of Matrix A
│ │ │ │ -
66
│ │ │ │ -
67 size_t nrIterations_; // number of iterations
│ │ │ │ +
65 public:
│ │ │ │
68
│ │ │ │ -
69 double ritzValue_; // Ritz eigenvalue
│ │ │ │ -
70 Vector ritzVector_; // Ritz eigenvector
│ │ │ │ -
71
│ │ │ │ -
72 public:
│ │ │ │ -
75
│ │ │ │ -
│ │ │ │ -
77 explicit PowerMethod(const Operator &A,
│ │ │ │ -
78 const boost::optional<Vector> initial = boost::none)
│ │ │ │ -
79 : A_(A), dim_(A.rows()), nrIterations_(0) {
│ │ │ │ -
80 Vector x0;
│ │ │ │ -
81 x0 = initial ? initial.get() : Vector::Random(dim_);
│ │ │ │ -
82 x0.normalize();
│ │ │ │ -
83
│ │ │ │ -
84 // initialize Ritz eigen value
│ │ │ │ -
85 ritzValue_ = 0.0;
│ │ │ │ +
70 void print(
│ │ │ │ +
71 const std::string& s = "BayesNet",
│ │ │ │ +
72 const KeyFormatter& formatter = DefaultKeyFormatter) const override;
│ │ │ │ +
73
│ │ │ │ +
77
│ │ │ │ +
79 void dot(std::ostream& os,
│ │ │ │ +
80 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
81 const DotWriter& writer = DotWriter()) const;
│ │ │ │ +
82
│ │ │ │ +
84 std::string dot(const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
85 const DotWriter& writer = DotWriter()) const;
│ │ │ │
86
│ │ │ │ -
87 // initialize Ritz eigen vector
│ │ │ │ -
88 ritzVector_ = powerIteration(x0);
│ │ │ │ -
89 }
│ │ │ │ -
│ │ │ │ -
90
│ │ │ │ -
│ │ │ │ -
95 Vector powerIteration(const Vector &x) const {
│ │ │ │ -
96 Vector y = A_ * x;
│ │ │ │ -
97 y.normalize();
│ │ │ │ -
98 return y;
│ │ │ │ -
99 }
│ │ │ │ +
88 void saveGraph(const std::string& filename,
│ │ │ │ +
89 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ +
90 const DotWriter& writer = DotWriter()) const;
│ │ │ │ +
91
│ │ │ │ +
95
│ │ │ │ +
96 // Expose HybridValues version of logProbability.
│ │ │ │ +
97 double logProbability(const HybridValues& x) const;
│ │ │ │ +
98
│ │ │ │ +
99 // Expose HybridValues version of evaluate.
│ │ │ │ +
100 double evaluate(const HybridValues& c) const;
│ │ │ │ +
101
│ │ │ │ +
103};
│ │ │ │
│ │ │ │ -
100
│ │ │ │ -
105 Vector powerIteration() const { return powerIteration(ritzVector_); }
│ │ │ │ +
104
│ │ │ │ +
105} // namespace gtsam
│ │ │ │
106
│ │ │ │ -
│ │ │ │ -
112 bool converged(double tol) const {
│ │ │ │ -
113 const Vector x = ritzVector_;
│ │ │ │ -
114 // store the Ritz eigen value
│ │ │ │ -
115 const double ritzValue = x.dot(A_ * x);
│ │ │ │ -
116 const double error = (A_ * x - ritzValue * x).norm();
│ │ │ │ -
117 return error < tol;
│ │ │ │ -
118 }
│ │ │ │ -
│ │ │ │ -
119
│ │ │ │ -
121 size_t nrIterations() const { return nrIterations_; }
│ │ │ │ -
122
│ │ │ │ -
│ │ │ │ -
129 bool compute(size_t maxIterations, double tol) {
│ │ │ │ -
130 // Starting
│ │ │ │ -
131 bool isConverged = false;
│ │ │ │ -
132
│ │ │ │ -
133 for (size_t i = 0; i < maxIterations && !isConverged; i++) {
│ │ │ │ -
134 ++nrIterations_;
│ │ │ │ -
135 // update the ritzVector after power iteration
│ │ │ │ -
136 ritzVector_ = powerIteration();
│ │ │ │ -
137 // update the ritzValue
│ │ │ │ -
138 ritzValue_ = ritzVector_.dot(A_ * ritzVector_);
│ │ │ │ -
139 isConverged = converged(tol);
│ │ │ │ -
140 }
│ │ │ │ -
141
│ │ │ │ -
142 return isConverged;
│ │ │ │ -
143 }
│ │ │ │ -
│ │ │ │ -
144
│ │ │ │ -
146 double eigenvalue() const { return ritzValue_; }
│ │ │ │ -
147
│ │ │ │ -
149 Vector eigenvector() const { return ritzVector_; }
│ │ │ │ -
150};
│ │ │ │ -
│ │ │ │ -
151
│ │ │ │ -
152} // namespace gtsam
│ │ │ │ -
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ -
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
107#include <gtsam/inference/BayesNet-inst.h>
│ │ │ │ +
FactorGraph.h
Factor Graph Base Class.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::PowerMethod
Compute maximum Eigenpair with power method.
Definition PowerMethod.h:57
│ │ │ │ -
gtsam::PowerMethod::powerIteration
Vector powerIteration(const Vector &x) const
Run power iteration to get ritzVector with previous ritzVector x, and return A * x / || A * x ||.
Definition PowerMethod.h:95
│ │ │ │ -
gtsam::PowerMethod::eigenvector
Vector eigenvector() const
Return the eigenvector.
Definition PowerMethod.h:149
│ │ │ │ -
gtsam::PowerMethod::A_
const Operator & A_
Const reference to an externally-held matrix whose minimum-eigenvalue we want to compute.
Definition PowerMethod.h:63
│ │ │ │ -
gtsam::PowerMethod::eigenvalue
double eigenvalue() const
Return the eigenvalue.
Definition PowerMethod.h:146
│ │ │ │ -
gtsam::PowerMethod::PowerMethod
PowerMethod(const Operator &A, const boost::optional< Vector > initial=boost::none)
Construct from the aim matrix and intial ritz vector.
Definition PowerMethod.h:77
│ │ │ │ -
gtsam::PowerMethod::powerIteration
Vector powerIteration() const
Run power iteration to get ritzVector with previous ritzVector x, and return A * x / || A * x ||.
Definition PowerMethod.h:105
│ │ │ │ -
gtsam::PowerMethod::converged
bool converged(double tol) const
After Perform power iteration on a single Ritz value, check if the Ritz residual for the current Ritz...
Definition PowerMethod.h:112
│ │ │ │ -
gtsam::PowerMethod::compute
bool compute(size_t maxIterations, double tol)
Start the power/accelerated iteration, after performing the power/accelerated iteration,...
Definition PowerMethod.h:129
│ │ │ │ -
gtsam::PowerMethod::nrIterations
size_t nrIterations() const
Return the number of iterations.
Definition PowerMethod.h:121
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ +
gtsam::BayesNet
A BayesNet is a tree of conditionals, stored in elimination order.
Definition BayesNet.h:35
│ │ │ │ +
gtsam::BayesNet::BayesNet
BayesNet(std::initializer_list< sharedConditional > conditionals)
Constructor that takes an initializer list of shared pointers.
Definition BayesNet.h:60
│ │ │ │ +
gtsam::BayesNet::print
void print(const std::string &s="BayesNet", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print out graph
Definition BayesNet-inst.h:32
│ │ │ │ +
gtsam::BayesNet::dot
void dot(std::ostream &os, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
Output to graphviz format, stream version.
Definition BayesNet-inst.h:46
│ │ │ │ +
gtsam::BayesNet::sharedConditional
boost::shared_ptr< CONDITIONAL > sharedConditional
A shared pointer to a conditional.
Definition BayesNet.h:41
│ │ │ │ +
gtsam::BayesNet::BayesNet
BayesNet(ITERATOR firstConditional, ITERATOR lastConditional)
Construct from iterator over conditionals.
Definition BayesNet.h:52
│ │ │ │ +
gtsam::BayesNet::saveGraph
void saveGraph(const std::string &filename, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const
output to file with graphviz format.
Definition BayesNet-inst.h:83
│ │ │ │ +
gtsam::BayesNet::BayesNet
BayesNet()
Default constructor as an empty BayesNet.
Definition BayesNet.h:48
│ │ │ │ +
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ +
gtsam::DotWriter
DotWriter is a helper class for writing graphviz .dot files.
Definition DotWriter.h:35
│ │ │ │ +
HybridValues
the error.
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,158 +1,141 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -PowerMethod.h │ │ │ │ │ +BayesNet.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, │ │ │ │ │ -4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -5 * All Rights Reserved │ │ │ │ │ -6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +3* GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4* Atlanta, Georgia 30332-0415 │ │ │ │ │ +5* All Rights Reserved │ │ │ │ │ +6* Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ -8 * See LICENSE for the license information │ │ │ │ │ +8* See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ -10 * ------------------------------------------------------------------------- │ │ │ │ │ +10* ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_c_t_o_r_._h> │ │ │ │ │ -24 │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ -28#include │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +22 │ │ │ │ │ +23#include │ │ │ │ │ +24#include │ │ │ │ │ +25 │ │ │ │ │ +26namespace _g_t_s_a_m { │ │ │ │ │ +27 │ │ │ │ │ +28class _H_y_b_r_i_d_V_a_l_u_e_s; │ │ │ │ │ 29 │ │ │ │ │ -30namespace _g_t_s_a_m { │ │ │ │ │ -31 │ │ │ │ │ -32using Sparse = Eigen::SparseMatrix; │ │ │ │ │ -33 │ │ │ │ │ -56template │ │ │ │ │ -_5_7class _P_o_w_e_r_M_e_t_h_o_d { │ │ │ │ │ -58 protected: │ │ │ │ │ -_6_3 const Operator &_A__; │ │ │ │ │ +34template │ │ │ │ │ +_3_5class _B_a_y_e_s_N_e_t : public _F_a_c_t_o_r_G_r_a_p_h { │ │ │ │ │ +36 private: │ │ │ │ │ +37 typedef _F_a_c_t_o_r_G_r_a_p_h_<_C_O_N_D_I_T_I_O_N_A_L_> _B_a_s_e; │ │ │ │ │ +38 │ │ │ │ │ +39 public: │ │ │ │ │ +40 typedef typename boost::shared_ptr │ │ │ │ │ +_4_1 _s_h_a_r_e_d_C_o_n_d_i_t_i_o_n_a_l; │ │ │ │ │ +42 │ │ │ │ │ +43 protected: │ │ │ │ │ +46 │ │ │ │ │ +_4_8 _B_a_y_e_s_N_e_t() {} │ │ │ │ │ +49 │ │ │ │ │ +51 template │ │ │ │ │ +_5_2 _B_a_y_e_s_N_e_t(ITERATOR firstConditional, ITERATOR lastConditional) │ │ │ │ │ +53 : _B_a_s_e(firstConditional, lastConditional) {} │ │ │ │ │ +54 │ │ │ │ │ +_6_0 _B_a_y_e_s_N_e_t(std::initializer_list conditionals) │ │ │ │ │ +61 : _B_a_s_e(conditionals) {} │ │ │ │ │ +62 │ │ │ │ │ 64 │ │ │ │ │ -65 const int dim_; // dimension of Matrix A │ │ │ │ │ -66 │ │ │ │ │ -67 size_t nrIterations_; // number of iterations │ │ │ │ │ +65 public: │ │ │ │ │ 68 │ │ │ │ │ -69 double ritzValue_; // Ritz eigenvalue │ │ │ │ │ -70 Vector ritzVector_; // Ritz eigenvector │ │ │ │ │ -71 │ │ │ │ │ -72 public: │ │ │ │ │ -75 │ │ │ │ │ -_7_7 explicit _P_o_w_e_r_M_e_t_h_o_d(const Operator &A, │ │ │ │ │ -78 const boost::optional initial = boost::none) │ │ │ │ │ -79 : _A__(A), dim_(A.rows()), nrIterations_(0) { │ │ │ │ │ -80 Vector x0; │ │ │ │ │ -81 x0 = initial ? initial.get() : Vector::Random(dim_); │ │ │ │ │ -82 x0.normalize(); │ │ │ │ │ -83 │ │ │ │ │ -84 // initialize Ritz eigen value │ │ │ │ │ -85 ritzValue_ = 0.0; │ │ │ │ │ +70 void _p_r_i_n_t( │ │ │ │ │ +71 const std::string& s = "BayesNet", │ │ │ │ │ +72 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override; │ │ │ │ │ +73 │ │ │ │ │ +77 │ │ │ │ │ +79 void _d_o_t(std::ostream& os, │ │ │ │ │ +80 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ +81 const _D_o_t_W_r_i_t_e_r& writer = _D_o_t_W_r_i_t_e_r()) const; │ │ │ │ │ +82 │ │ │ │ │ +84 std::string _d_o_t(const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ +85 const _D_o_t_W_r_i_t_e_r& writer = _D_o_t_W_r_i_t_e_r()) const; │ │ │ │ │ 86 │ │ │ │ │ -87 // initialize Ritz eigen vector │ │ │ │ │ -88 ritzVector_ = _p_o_w_e_r_I_t_e_r_a_t_i_o_n(x0); │ │ │ │ │ -89 } │ │ │ │ │ -90 │ │ │ │ │ -_9_5 Vector _p_o_w_e_r_I_t_e_r_a_t_i_o_n(const Vector &x) const { │ │ │ │ │ -96 Vector y = _A__ * x; │ │ │ │ │ -97 y.normalize(); │ │ │ │ │ -98 return y; │ │ │ │ │ -99 } │ │ │ │ │ -100 │ │ │ │ │ -_1_0_5 Vector _p_o_w_e_r_I_t_e_r_a_t_i_o_n() const { return _p_o_w_e_r_I_t_e_r_a_t_i_o_n(ritzVector_); } │ │ │ │ │ +88 void _s_a_v_e_G_r_a_p_h(const std::string& filename, │ │ │ │ │ +89 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ +90 const _D_o_t_W_r_i_t_e_r& writer = _D_o_t_W_r_i_t_e_r()) const; │ │ │ │ │ +91 │ │ │ │ │ +95 │ │ │ │ │ +96 // Expose HybridValues version of logProbability. │ │ │ │ │ +97 double logProbability(const _H_y_b_r_i_d_V_a_l_u_e_s& x) const; │ │ │ │ │ +98 │ │ │ │ │ +99 // Expose HybridValues version of evaluate. │ │ │ │ │ +100 double evaluate(const _H_y_b_r_i_d_V_a_l_u_e_s& c) const; │ │ │ │ │ +101 │ │ │ │ │ +103}; │ │ │ │ │ +104 │ │ │ │ │ +105} // namespace gtsam │ │ │ │ │ 106 │ │ │ │ │ -_1_1_2 bool _c_o_n_v_e_r_g_e_d(double tol) const { │ │ │ │ │ -113 const Vector x = ritzVector_; │ │ │ │ │ -114 // store the Ritz eigen value │ │ │ │ │ -115 const double ritzValue = x.dot(_A__ * x); │ │ │ │ │ -116 const double error = (_A__ * x - ritzValue * x).norm(); │ │ │ │ │ -117 return error < tol; │ │ │ │ │ -118 } │ │ │ │ │ -119 │ │ │ │ │ -_1_2_1 size_t _n_r_I_t_e_r_a_t_i_o_n_s() const { return nrIterations_; } │ │ │ │ │ -122 │ │ │ │ │ -_1_2_9 bool _c_o_m_p_u_t_e(size_t maxIterations, double tol) { │ │ │ │ │ -130 // Starting │ │ │ │ │ -131 bool isConverged = false; │ │ │ │ │ -132 │ │ │ │ │ -133 for (size_t i = 0; i < maxIterations && !isConverged; i++) { │ │ │ │ │ -134 ++nrIterations_; │ │ │ │ │ -135 // update the ritzVector after power iteration │ │ │ │ │ -136 ritzVector_ = _p_o_w_e_r_I_t_e_r_a_t_i_o_n(); │ │ │ │ │ -137 // update the ritzValue │ │ │ │ │ -138 ritzValue_ = ritzVector_.dot(_A__ * ritzVector_); │ │ │ │ │ -139 isConverged = _c_o_n_v_e_r_g_e_d(tol); │ │ │ │ │ -140 } │ │ │ │ │ -141 │ │ │ │ │ -142 return isConverged; │ │ │ │ │ -143 } │ │ │ │ │ -144 │ │ │ │ │ -_1_4_6 double _e_i_g_e_n_v_a_l_u_e() const { return ritzValue_; } │ │ │ │ │ -147 │ │ │ │ │ -_1_4_9 Vector _e_i_g_e_n_v_e_c_t_o_r() const { return ritzVector_; } │ │ │ │ │ -150}; │ │ │ │ │ -151 │ │ │ │ │ -152} // namespace gtsam │ │ │ │ │ -_V_e_c_t_o_r_._h │ │ │ │ │ -typedef and functions to augment Eigen's VectorXd │ │ │ │ │ -_M_a_t_r_i_x_._h │ │ │ │ │ -typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +107#include │ │ │ │ │ +_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Factor Graph Base Class. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_w_e_r_M_e_t_h_o_d │ │ │ │ │ -Compute maximum Eigenpair with power method. │ │ │ │ │ -DDeeffiinniittiioonn PowerMethod.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_w_e_r_M_e_t_h_o_d_:_:_p_o_w_e_r_I_t_e_r_a_t_i_o_n │ │ │ │ │ -Vector powerIteration(const Vector &x) const │ │ │ │ │ -Run power iteration to get ritzVector with previous ritzVector x, and return A │ │ │ │ │ -* x / || A * x ||. │ │ │ │ │ -DDeeffiinniittiioonn PowerMethod.h:95 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_w_e_r_M_e_t_h_o_d_:_:_e_i_g_e_n_v_e_c_t_o_r │ │ │ │ │ -Vector eigenvector() const │ │ │ │ │ -Return the eigenvector. │ │ │ │ │ -DDeeffiinniittiioonn PowerMethod.h:149 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_w_e_r_M_e_t_h_o_d_:_:_A__ │ │ │ │ │ -const Operator & A_ │ │ │ │ │ -Const reference to an externally-held matrix whose minimum-eigenvalue we want │ │ │ │ │ -to compute. │ │ │ │ │ -DDeeffiinniittiioonn PowerMethod.h:63 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_w_e_r_M_e_t_h_o_d_:_:_e_i_g_e_n_v_a_l_u_e │ │ │ │ │ -double eigenvalue() const │ │ │ │ │ -Return the eigenvalue. │ │ │ │ │ -DDeeffiinniittiioonn PowerMethod.h:146 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_w_e_r_M_e_t_h_o_d_:_:_P_o_w_e_r_M_e_t_h_o_d │ │ │ │ │ -PowerMethod(const Operator &A, const boost::optional< Vector > initial=boost:: │ │ │ │ │ -none) │ │ │ │ │ -Construct from the aim matrix and intial ritz vector. │ │ │ │ │ -DDeeffiinniittiioonn PowerMethod.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_w_e_r_M_e_t_h_o_d_:_:_p_o_w_e_r_I_t_e_r_a_t_i_o_n │ │ │ │ │ -Vector powerIteration() const │ │ │ │ │ -Run power iteration to get ritzVector with previous ritzVector x, and return A │ │ │ │ │ -* x / || A * x ||. │ │ │ │ │ -DDeeffiinniittiioonn PowerMethod.h:105 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_w_e_r_M_e_t_h_o_d_:_:_c_o_n_v_e_r_g_e_d │ │ │ │ │ -bool converged(double tol) const │ │ │ │ │ -After Perform power iteration on a single Ritz value, check if the Ritz │ │ │ │ │ -residual for the current Ritz... │ │ │ │ │ -DDeeffiinniittiioonn PowerMethod.h:112 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_w_e_r_M_e_t_h_o_d_:_:_c_o_m_p_u_t_e │ │ │ │ │ -bool compute(size_t maxIterations, double tol) │ │ │ │ │ -Start the power/accelerated iteration, after performing the power/accelerated │ │ │ │ │ -iteration,... │ │ │ │ │ -DDeeffiinniittiioonn PowerMethod.h:129 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_w_e_r_M_e_t_h_o_d_:_:_n_r_I_t_e_r_a_t_i_o_n_s │ │ │ │ │ -size_t nrIterations() const │ │ │ │ │ -Return the number of iterations. │ │ │ │ │ -DDeeffiinniittiioonn PowerMethod.h:121 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t │ │ │ │ │ +A BayesNet is a tree of conditionals, stored in elimination order. │ │ │ │ │ +DDeeffiinniittiioonn BayesNet.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t_:_:_B_a_y_e_s_N_e_t │ │ │ │ │ +BayesNet(std::initializer_list< sharedConditional > conditionals) │ │ │ │ │ +Constructor that takes an initializer list of shared pointers. │ │ │ │ │ +DDeeffiinniittiioonn BayesNet.h:60 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="BayesNet", const KeyFormatter │ │ │ │ │ +&formatter=DefaultKeyFormatter) const override │ │ │ │ │ +print out graph │ │ │ │ │ +DDeeffiinniittiioonn BayesNet-inst.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t_:_:_d_o_t │ │ │ │ │ +void dot(std::ostream &os, const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const │ │ │ │ │ +Output to graphviz format, stream version. │ │ │ │ │ +DDeeffiinniittiioonn BayesNet-inst.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t_:_:_s_h_a_r_e_d_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +boost::shared_ptr< CONDITIONAL > sharedConditional │ │ │ │ │ +A shared pointer to a conditional. │ │ │ │ │ +DDeeffiinniittiioonn BayesNet.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t_:_:_B_a_y_e_s_N_e_t │ │ │ │ │ +BayesNet(ITERATOR firstConditional, ITERATOR lastConditional) │ │ │ │ │ +Construct from iterator over conditionals. │ │ │ │ │ +DDeeffiinniittiioonn BayesNet.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t_:_:_s_a_v_e_G_r_a_p_h │ │ │ │ │ +void saveGraph(const std::string &filename, const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter, const DotWriter &writer=DotWriter()) const │ │ │ │ │ +output to file with graphviz format. │ │ │ │ │ +DDeeffiinniittiioonn BayesNet-inst.h:83 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t_:_:_B_a_y_e_s_N_e_t │ │ │ │ │ +BayesNet() │ │ │ │ │ +Default constructor as an empty BayesNet. │ │ │ │ │ +DDeeffiinniittiioonn BayesNet.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ +nodes. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r │ │ │ │ │ +DotWriter is a helper class for writing graphviz .dot files. │ │ │ │ │ +DDeeffiinniittiioonn DotWriter.h:35 │ │ │ │ │ +_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +the error. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _l_i_n_e_a_r │ │ │ │ │ - * _P_o_w_e_r_M_e_t_h_o_d_._h │ │ │ │ │ + * _i_n_f_e_r_e_n_c_e │ │ │ │ │ + * _B_a_y_e_s_N_e_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00761_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01244_source.html │ │ │ │┄ Files 92% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/JacobianFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/EssentialMatrixFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
JacobianFactor.h
│ │ │ │ +
EssentialMatrixFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010-2014, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ +
12/*
│ │ │ │ +
13 * @file EssentialMatrixFactor.h
│ │ │ │ +
14 * @brief EssentialMatrixFactor class
│ │ │ │ +
15 * @author Frank Dellaert
│ │ │ │ +
16 * @author Ayush Baid
│ │ │ │ +
17 * @author Akshay Krishnan
│ │ │ │ +
18 * @date December 17, 2013
│ │ │ │ +
19 */
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/linear/GaussianFactor.h>
│ │ │ │ -
22#include <gtsam/linear/NoiseModel.h>
│ │ │ │ -
23#include <gtsam/base/VerticalBlockMatrix.h>
│ │ │ │ -
24#include <gtsam/global_includes.h>
│ │ │ │ -
25#include <gtsam/inference/VariableSlots.h>
│ │ │ │ +
21#pragma once
│ │ │ │ +
22
│ │ │ │ +
23#include <gtsam/geometry/EssentialMatrix.h>
│ │ │ │ +
24#include <gtsam/geometry/PinholeCamera.h>
│ │ │ │ +
25#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │
26
│ │ │ │ -
27#include <boost/make_shared.hpp>
│ │ │ │ -
28#include <boost/serialization/version.hpp>
│ │ │ │ -
29#include <boost/serialization/split_member.hpp>
│ │ │ │ +
27#include <iostream>
│ │ │ │ +
28
│ │ │ │ +
29namespace gtsam {
│ │ │ │
30
│ │ │ │ -
31namespace gtsam {
│ │ │ │ -
32
│ │ │ │ -
33 // Forward declarations
│ │ │ │ -
34 class HessianFactor;
│ │ │ │ -
35 class VariableSlots;
│ │ │ │ -
36 class GaussianFactorGraph;
│ │ │ │ -
37 class GaussianConditional;
│ │ │ │ -
38 class HessianFactor;
│ │ │ │ -
39 class VectorValues;
│ │ │ │ -
40 class Ordering;
│ │ │ │ -
41 class JacobianFactor;
│ │ │ │ -
42
│ │ │ │ -
48 GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<JacobianFactor> >
│ │ │ │ -
49 EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys);
│ │ │ │ -
50
│ │ │ │ -
│ │ │ │ -
90 class GTSAM_EXPORT JacobianFactor : public GaussianFactor
│ │ │ │ -
91 {
│ │ │ │ -
92 public:
│ │ │ │ -
93
│ │ │ │ -
94 typedef JacobianFactor This;
│ │ │ │ -
95 typedef GaussianFactor Base;
│ │ │ │ -
96 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
97
│ │ │ │ -
98 typedef VerticalBlockMatrix::Block ABlock;
│ │ │ │ -
99 typedef VerticalBlockMatrix::constBlock constABlock;
│ │ │ │ -
100 typedef ABlock::ColXpr BVector;
│ │ │ │ -
101 typedef constABlock::ConstColXpr constBVector;
│ │ │ │ -
102
│ │ │ │ -
103 protected:
│ │ │ │ -
104
│ │ │ │ -
105 VerticalBlockMatrix Ab_; // the block view of the full matrix
│ │ │ │ -
106 noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix
│ │ │ │ -
107
│ │ │ │ -
108 public:
│ │ │ │ -
109
│ │ │ │ -
111 explicit JacobianFactor(const GaussianFactor& gf);
│ │ │ │ -
112
│ │ │ │ -
114 JacobianFactor(const JacobianFactor& jf) : Base(jf), Ab_(jf.Ab_), model_(jf.model_) {}
│ │ │ │ -
115
│ │ │ │ -
117 explicit JacobianFactor(const HessianFactor& hf);
│ │ │ │ -
118
│ │ │ │ -
120 JacobianFactor();
│ │ │ │ -
121
│ │ │ │ -
123 explicit JacobianFactor(const Vector& b_in);
│ │ │ │ -
124
│ │ │ │ -
126 JacobianFactor(Key i1, const Matrix& A1,
│ │ │ │ -
127 const Vector& b, const SharedDiagonal& model = SharedDiagonal());
│ │ │ │ -
128
│ │ │ │ -
130 JacobianFactor(Key i1, const Matrix& A1,
│ │ │ │ -
131 Key i2, const Matrix& A2,
│ │ │ │ -
132 const Vector& b, const SharedDiagonal& model = SharedDiagonal());
│ │ │ │ +
│ │ │ │ +
34class EssentialMatrixFactor : public NoiseModelFactorN<EssentialMatrix> {
│ │ │ │ +
35 Vector3 vA_, vB_;
│ │ │ │ +
36
│ │ │ │ +
37 typedef NoiseModelFactorN<EssentialMatrix> Base;
│ │ │ │ +
38 typedef EssentialMatrixFactor This;
│ │ │ │ +
39
│ │ │ │ +
40 public:
│ │ │ │ +
│ │ │ │ +
49 EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB,
│ │ │ │ +
50 const SharedNoiseModel& model)
│ │ │ │ +
51 : Base(model, key) {
│ │ │ │ +
52 vA_ = EssentialMatrix::Homogeneous(pA);
│ │ │ │ +
53 vB_ = EssentialMatrix::Homogeneous(pB);
│ │ │ │ +
54 }
│ │ │ │ +
│ │ │ │ +
55
│ │ │ │ +
65 template <class CALIBRATION>
│ │ │ │ +
│ │ │ │ +
66 EssentialMatrixFactor(Key key, const Point2& pA, const Point2& pB,
│ │ │ │ +
67 const SharedNoiseModel& model,
│ │ │ │ +
68 boost::shared_ptr<CALIBRATION> K)
│ │ │ │ +
69 : Base(model, key) {
│ │ │ │ +
70 assert(K);
│ │ │ │ +
71 vA_ = EssentialMatrix::Homogeneous(K->calibrate(pA));
│ │ │ │ +
72 vB_ = EssentialMatrix::Homogeneous(K->calibrate(pB));
│ │ │ │ +
73 }
│ │ │ │ +
│ │ │ │ +
74
│ │ │ │ +
│ │ │ │ +
76 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
77 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ +
78 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
│ │ │ │ +
79 }
│ │ │ │ +
│ │ │ │ +
80
│ │ │ │ +
│ │ │ │ +
82 void print(
│ │ │ │ +
83 const std::string& s = "",
│ │ │ │ +
84 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ +
85 Base::print(s);
│ │ │ │ +
86 std::cout << " EssentialMatrixFactor with measurements\n ("
│ │ │ │ +
87 << vA_.transpose() << ")' and (" << vB_.transpose() << ")'"
│ │ │ │ +
88 << std::endl;
│ │ │ │ +
89 }
│ │ │ │ +
│ │ │ │ +
90
│ │ │ │ +
│ │ │ │ +
92 Vector evaluateError(
│ │ │ │ +
93 const EssentialMatrix& E,
│ │ │ │ +
94 boost::optional<Matrix&> H = boost::none) const override {
│ │ │ │ +
95 Vector error(1);
│ │ │ │ +
96 error << E.error(vA_, vB_, H);
│ │ │ │ +
97 return error;
│ │ │ │ +
98 }
│ │ │ │ +
│ │ │ │ +
99
│ │ │ │ +
100 public:
│ │ │ │ +
101 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
102};
│ │ │ │ +
│ │ │ │ +
103
│ │ │ │ +
│ │ │ │ +
108class EssentialMatrixFactor2
│ │ │ │ +
109 : public NoiseModelFactorN<EssentialMatrix, double> {
│ │ │ │ +
110 Point3 dP1_;
│ │ │ │ +
111 Point2 pn_;
│ │ │ │ +
112 double f_;
│ │ │ │ +
113
│ │ │ │ +
114 typedef NoiseModelFactorN<EssentialMatrix, double> Base;
│ │ │ │ +
115 typedef EssentialMatrixFactor2 This;
│ │ │ │ +
116
│ │ │ │ +
117 public:
│ │ │ │ +
│ │ │ │ +
126 EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
│ │ │ │ +
127 const SharedNoiseModel& model)
│ │ │ │ +
128 : Base(model, key1, key2),
│ │ │ │ +
129 dP1_(EssentialMatrix::Homogeneous(pA)),
│ │ │ │ +
130 pn_(pB) {
│ │ │ │ +
131 f_ = 1.0;
│ │ │ │ +
132 }
│ │ │ │ +
│ │ │ │
133
│ │ │ │ -
135 JacobianFactor(Key i1, const Matrix& A1, Key i2,
│ │ │ │ -
136 const Matrix& A2, Key i3, const Matrix& A3,
│ │ │ │ -
137 const Vector& b, const SharedDiagonal& model = SharedDiagonal());
│ │ │ │ -
138
│ │ │ │ -
142 template<typename TERMS>
│ │ │ │ -
143 JacobianFactor(const TERMS& terms, const Vector& b, const SharedDiagonal& model = SharedDiagonal());
│ │ │ │ -
144
│ │ │ │ -
149 template<typename KEYS>
│ │ │ │ -
150 JacobianFactor(
│ │ │ │ -
151 const KEYS& keys, const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas = SharedDiagonal());
│ │ │ │ +
143 template <class CALIBRATION>
│ │ │ │ +
│ │ │ │ +
144 EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
│ │ │ │ +
145 const SharedNoiseModel& model,
│ │ │ │ +
146 boost::shared_ptr<CALIBRATION> K)
│ │ │ │ +
147 : Base(model, key1, key2),
│ │ │ │ +
148 dP1_(EssentialMatrix::Homogeneous(K->calibrate(pA))),
│ │ │ │ +
149 pn_(K->calibrate(pB)) {
│ │ │ │ +
150 f_ = 0.5 * (K->fx() + K->fy());
│ │ │ │ +
151 }
│ │ │ │ +
│ │ │ │
152
│ │ │ │ -
157 explicit JacobianFactor(
│ │ │ │ -
158 const GaussianFactorGraph& graph);
│ │ │ │ -
159
│ │ │ │ -
164 explicit JacobianFactor(
│ │ │ │ -
165 const GaussianFactorGraph& graph,
│ │ │ │ -
166 const VariableSlots& p_variableSlots);
│ │ │ │ -
167
│ │ │ │ -
172 explicit JacobianFactor(
│ │ │ │ -
173 const GaussianFactorGraph& graph,
│ │ │ │ -
174 const Ordering& ordering);
│ │ │ │ -
175
│ │ │ │ -
180 explicit JacobianFactor(
│ │ │ │ -
181 const GaussianFactorGraph& graph,
│ │ │ │ -
182 const Ordering& ordering,
│ │ │ │ -
183 const VariableSlots& p_variableSlots);
│ │ │ │ -
184
│ │ │ │ -
186 ~JacobianFactor() override {}
│ │ │ │ -
187
│ │ │ │ -
│ │ │ │ -
189 GaussianFactor::shared_ptr clone() const override {
│ │ │ │ -
190 return boost::static_pointer_cast<GaussianFactor>(
│ │ │ │ -
191 boost::make_shared<JacobianFactor>(*this));
│ │ │ │ -
192 }
│ │ │ │ -
│ │ │ │ -
193
│ │ │ │ -
194 // Implementing Testable interface
│ │ │ │ -
195 void print(const std::string& s = "",
│ │ │ │ -
196 const KeyFormatter& formatter = DefaultKeyFormatter) const override;
│ │ │ │ -
197 bool equals(const GaussianFactor& lf, double tol = 1e-9) const override;
│ │ │ │ -
198
│ │ │ │ -
199 Vector unweighted_error(const VectorValues& c) const;
│ │ │ │ -
200 Vector error_vector(const VectorValues& c) const;
│ │ │ │ -
203 using GaussianFactor::error;
│ │ │ │ -
204
│ │ │ │ -
206 double error(const VectorValues& c) const override;
│ │ │ │ -
207
│ │ │ │ -
216 Matrix augmentedInformation() const override;
│ │ │ │ -
217
│ │ │ │ -
221 Matrix information() const override;
│ │ │ │ -
222
│ │ │ │ -
224 using Base::hessianDiagonal;
│ │ │ │ +
│ │ │ │ +
154 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
155 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ +
156 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
│ │ │ │ +
157 }
│ │ │ │ +
│ │ │ │ +
158
│ │ │ │ +
│ │ │ │ +
160 void print(
│ │ │ │ +
161 const std::string& s = "",
│ │ │ │ +
162 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ +
163 Base::print(s);
│ │ │ │ +
164 std::cout << " EssentialMatrixFactor2 with measurements\n ("
│ │ │ │ +
165 << dP1_.transpose() << ")' and (" << pn_.transpose() << ")'"
│ │ │ │ +
166 << std::endl;
│ │ │ │ +
167 }
│ │ │ │ +
│ │ │ │ +
168
│ │ │ │ +
169 /*
│ │ │ │ +
170 * Vector of errors returns 2D vector
│ │ │ │ +
171 * @param E essential matrix
│ │ │ │ +
172 * @param d inverse depth d
│ │ │ │ +
173 */
│ │ │ │ +
174 Vector evaluateError(
│ │ │ │ +
175 const EssentialMatrix& E, const double& d,
│ │ │ │ +
176 boost::optional<Matrix&> DE = boost::none,
│ │ │ │ +
177 boost::optional<Matrix&> Dd = boost::none) const override {
│ │ │ │ +
178 // We have point x,y in image 1
│ │ │ │ +
179 // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d
│ │ │ │ +
180 // We then convert to second camera by P2 = 1R2'*(P1-1T2)
│ │ │ │ +
181 // The homogeneous coordinates of can be written as
│ │ │ │ +
182 // 2R1*(P1-1T2) == 2R1*d*(P1-1T2) == 2R1*((x,y,1)-d*1T2)
│ │ │ │ +
183 // where we multiplied with d which yields equivalent homogeneous
│ │ │ │ +
184 // coordinates. Note that this is just the homography 2R1 for d==0 The point
│ │ │ │ +
185 // d*P1 = (x,y,1) is computed in constructor as dP1_
│ │ │ │ +
186
│ │ │ │ +
187 // Project to normalized image coordinates, then uncalibrate
│ │ │ │ +
188 Point2 pn(0, 0);
│ │ │ │ +
189 if (!DE && !Dd) {
│ │ │ │ +
190 Point3 _1T2 = E.direction().point3();
│ │ │ │ +
191 Point3 d1T2 = d * _1T2;
│ │ │ │ +
192 Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2); // 2R1*((x,y,1)-d*1T2)
│ │ │ │ +
193 pn = PinholeBase::Project(dP2);
│ │ │ │ +
194
│ │ │ │ +
195 } else {
│ │ │ │ +
196 // Calculate derivatives. TODO if slow: optimize with Mathematica
│ │ │ │ +
197 // 3*2 3*3 3*3
│ │ │ │ +
198 Matrix D_1T2_dir, DdP2_rot, DP2_point;
│ │ │ │ +
199
│ │ │ │ +
200 Point3 _1T2 = E.direction().point3(D_1T2_dir);
│ │ │ │ +
201 Point3 d1T2 = d * _1T2;
│ │ │ │ +
202 Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point);
│ │ │ │ +
203
│ │ │ │ +
204 Matrix23 Dpn_dP2;
│ │ │ │ +
205 pn = PinholeBase::Project(dP2, Dpn_dP2);
│ │ │ │ +
206
│ │ │ │ +
207 if (DE) {
│ │ │ │ +
208 Matrix DdP2_E(3, 5);
│ │ │ │ +
209 DdP2_E << DdP2_rot, -DP2_point * d * D_1T2_dir; // (3*3), (3*3) * (3*2)
│ │ │ │ +
210 *DE = f_ * Dpn_dP2 * DdP2_E; // (2*3) * (3*5)
│ │ │ │ +
211 }
│ │ │ │ +
212
│ │ │ │ +
213 if (Dd) // efficient backwards computation:
│ │ │ │ +
214 // (2*3) * (3*3) * (3*1)
│ │ │ │ +
215 *Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2));
│ │ │ │ +
216 }
│ │ │ │ +
217 Point2 reprojectionError = pn - pn_;
│ │ │ │ +
218 return f_ * reprojectionError;
│ │ │ │ +
219 }
│ │ │ │ +
220
│ │ │ │ +
221 public:
│ │ │ │ +
222 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
223};
│ │ │ │ +
│ │ │ │ +
224// EssentialMatrixFactor2
│ │ │ │
225
│ │ │ │ -
227 void hessianDiagonalAdd(VectorValues& d) const override;
│ │ │ │ -
228
│ │ │ │ -
230 void hessianDiagonal(double* d) const override;
│ │ │ │ -
231
│ │ │ │ -
233 std::map<Key,Matrix> hessianBlockDiagonal() const override;
│ │ │ │ +
│ │ │ │ +
231class EssentialMatrixFactor3 : public EssentialMatrixFactor2 {
│ │ │ │ +
232 typedef EssentialMatrixFactor2 Base;
│ │ │ │ +
233 typedef EssentialMatrixFactor3 This;
│ │ │ │
234
│ │ │ │ -
238 std::pair<Matrix, Vector> jacobian() const override;
│ │ │ │ -
239
│ │ │ │ -
243 std::pair<Matrix, Vector> jacobianUnweighted() const;
│ │ │ │ -
244
│ │ │ │ -
248 Matrix augmentedJacobian() const override;
│ │ │ │ -
249
│ │ │ │ -
253 Matrix augmentedJacobianUnweighted() const;
│ │ │ │ -
254
│ │ │ │ -
256 const VerticalBlockMatrix& matrixObject() const { return Ab_; }
│ │ │ │ -
257
│ │ │ │ -
259 VerticalBlockMatrix& matrixObject() { return Ab_; }
│ │ │ │ -
260
│ │ │ │ -
266 GaussianFactor::shared_ptr negate() const override;
│ │ │ │ -
267
│ │ │ │ -
│ │ │ │ -
269 bool isConstrained() const {
│ │ │ │ -
270 return model_ && model_->isConstrained();
│ │ │ │ -
271 }
│ │ │ │ -
│ │ │ │ -
272
│ │ │ │ -
│ │ │ │ -
276 DenseIndex getDim(const_iterator variable) const override {
│ │ │ │ -
277 return Ab_(variable - begin()).cols();
│ │ │ │ -
278 }
│ │ │ │ +
235 Rot3 cRb_;
│ │ │ │ +
236
│ │ │ │ +
237 public:
│ │ │ │ +
│ │ │ │ +
247 EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB,
│ │ │ │ +
248 const Rot3& cRb, const SharedNoiseModel& model)
│ │ │ │ +
249 : EssentialMatrixFactor2(key1, key2, pA, pB, model), cRb_(cRb) {}
│ │ │ │ +
│ │ │ │ +
250
│ │ │ │ +
260 template <class CALIBRATION>
│ │ │ │ +
│ │ │ │ +
261 EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB,
│ │ │ │ +
262 const Rot3& cRb, const SharedNoiseModel& model,
│ │ │ │ +
263 boost::shared_ptr<CALIBRATION> K)
│ │ │ │ +
264 : EssentialMatrixFactor2(key1, key2, pA, pB, model, K), cRb_(cRb) {}
│ │ │ │ +
│ │ │ │ +
265
│ │ │ │ +
│ │ │ │ +
267 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
268 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ +
269 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
│ │ │ │ +
270 }
│ │ │ │ +
│ │ │ │ +
271
│ │ │ │ +
│ │ │ │ +
273 void print(
│ │ │ │ +
274 const std::string& s = "",
│ │ │ │ +
275 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ +
276 Base::print(s);
│ │ │ │ +
277 std::cout << " EssentialMatrixFactor3 with rotation " << cRb_ << std::endl;
│ │ │ │ +
278 }
│ │ │ │
│ │ │ │
279
│ │ │ │ -
283 size_t rows() const { return Ab_.rows(); }
│ │ │ │ -
284
│ │ │ │ -
288 size_t cols() const { return Ab_.cols(); }
│ │ │ │ -
289
│ │ │ │ -
291 const SharedDiagonal& get_model() const { return model_; }
│ │ │ │ -
292
│ │ │ │ -
294 SharedDiagonal& get_model() { return model_; }
│ │ │ │ -
295
│ │ │ │ -
297 const constBVector getb() const { return Ab_(size()).col(0); }
│ │ │ │ -
298
│ │ │ │ -
300 constABlock getA(const_iterator variable) const { return Ab_(variable - begin()); }
│ │ │ │ -
301
│ │ │ │ -
303 constABlock getA() const { return Ab_.range(0, size()); }
│ │ │ │ -
304
│ │ │ │ -
306 BVector getb() { return Ab_(size()).col(0); }
│ │ │ │ -
307
│ │ │ │ -
309 ABlock getA(iterator variable) { return Ab_(variable - begin()); }
│ │ │ │ -
310
│ │ │ │ -
312 ABlock getA() { return Ab_.range(0, size()); }
│ │ │ │ -
313
│ │ │ │ -
319 void updateHessian(const KeyVector& keys, SymmetricBlockMatrix* info) const override;
│ │ │ │ -
320
│ │ │ │ -
322 Vector operator*(const VectorValues& x) const;
│ │ │ │ -
323
│ │ │ │ -
326 void transposeMultiplyAdd(double alpha, const Vector& e,
│ │ │ │ -
327 VectorValues& x) const;
│ │ │ │ -
328
│ │ │ │ -
330 void multiplyHessianAdd(double alpha, const VectorValues& x,
│ │ │ │ -
331 VectorValues& y) const override;
│ │ │ │ -
332
│ │ │ │ -
341 void multiplyHessianAdd(double alpha, const double* x, double* y,
│ │ │ │ -
342 const std::vector<size_t>& accumulatedDims) const;
│ │ │ │ -
343
│ │ │ │ -
345 VectorValues gradientAtZero() const override;
│ │ │ │ -
346
│ │ │ │ -
348 void gradientAtZero(double* d) const override;
│ │ │ │ -
349
│ │ │ │ -
351 Vector gradient(Key key, const VectorValues& x) const override;
│ │ │ │ -
352
│ │ │ │ -
354 JacobianFactor whiten() const;
│ │ │ │ -
355
│ │ │ │ -
357 std::pair<boost::shared_ptr<GaussianConditional>, shared_ptr>
│ │ │ │ -
358 eliminate(const Ordering& keys);
│ │ │ │ -
359
│ │ │ │ -
361 void setModel(bool anyConstrained, const Vector& sigmas);
│ │ │ │ -
362
│ │ │ │ -
374 friend GTSAM_EXPORT std::pair<boost::shared_ptr<GaussianConditional>, shared_ptr>
│ │ │ │ -
375 EliminateQR(const GaussianFactorGraph& factors, const Ordering& keys);
│ │ │ │ -
376
│ │ │ │ -
384 boost::shared_ptr<GaussianConditional> splitConditional(size_t nrFrontals);
│ │ │ │ -
385
│ │ │ │ -
386 protected:
│ │ │ │ -
387
│ │ │ │ -
389 template<typename TERMS>
│ │ │ │ -
390 void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel);
│ │ │ │ -
391
│ │ │ │ -
392 private:
│ │ │ │ -
393
│ │ │ │ -
398 void JacobianFactorHelper(
│ │ │ │ -
399 const GaussianFactorGraph& graph,
│ │ │ │ -
400 const FastVector<VariableSlots::const_iterator>& orderedSlots);
│ │ │ │ -
401
│ │ │ │ -
408 template<class KEYS, class DIMENSIONS>
│ │ │ │ -
409 JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m,
│ │ │ │ -
410 const SharedDiagonal& model = SharedDiagonal()) :
│ │ │ │ -
411 Base(keys), Ab_(dims.begin(), dims.end(), m, true), model_(model) {
│ │ │ │ -
412 }
│ │ │ │ -
413
│ │ │ │ -
414 // be very selective on who can access these private methods:
│ │ │ │ -
415 template<typename T> friend class ExpressionFactor;
│ │ │ │ -
416
│ │ │ │ -
418 friend class boost::serialization::access;
│ │ │ │ -
419 template<class ARCHIVE>
│ │ │ │ -
420 void save(ARCHIVE & ar, const unsigned int version) const {
│ │ │ │ -
421 // TODO(fan): This is a hack for Boost < 1.66
│ │ │ │ -
422 // We really need to introduce proper versioning in the archives
│ │ │ │ -
423 // As otherwise this will not read objects serialized by older
│ │ │ │ -
424 // versions of GTSAM
│ │ │ │ -
425 ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
426 ar << BOOST_SERIALIZATION_NVP(Ab_);
│ │ │ │ -
427 bool model_null = false;
│ │ │ │ -
428 if(model_.get() == nullptr) {
│ │ │ │ -
429 model_null = true;
│ │ │ │ -
430 ar << boost::serialization::make_nvp("model_null", model_null);
│ │ │ │ -
431 } else {
│ │ │ │ -
432 ar << boost::serialization::make_nvp("model_null", model_null);
│ │ │ │ -
433 ar << BOOST_SERIALIZATION_NVP(model_);
│ │ │ │ -
434 }
│ │ │ │ -
435 }
│ │ │ │ -
436
│ │ │ │ -
437 template<class ARCHIVE>
│ │ │ │ -
438 void load(ARCHIVE & ar, const unsigned int version) {
│ │ │ │ -
439 // invoke serialization of the base class
│ │ │ │ -
440 ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
441 ar >> BOOST_SERIALIZATION_NVP(Ab_);
│ │ │ │ -
442 if (version < 1) {
│ │ │ │ -
443 ar >> BOOST_SERIALIZATION_NVP(model_);
│ │ │ │ -
444 } else {
│ │ │ │ -
445 bool model_null;
│ │ │ │ -
446 ar >> BOOST_SERIALIZATION_NVP(model_null);
│ │ │ │ -
447 if (!model_null) {
│ │ │ │ -
448 ar >> BOOST_SERIALIZATION_NVP(model_);
│ │ │ │ -
449 }
│ │ │ │ -
450 }
│ │ │ │ -
451 }
│ │ │ │ -
452
│ │ │ │ -
453 BOOST_SERIALIZATION_SPLIT_MEMBER()
│ │ │ │ -
454 }; // JacobianFactor
│ │ │ │ -
│ │ │ │ -
456template<>
│ │ │ │ -
│ │ │ │ -
457struct traits<JacobianFactor> : public Testable<JacobianFactor> {
│ │ │ │ -
458};
│ │ │ │ -
│ │ │ │ -
459
│ │ │ │ -
460} // \ namespace gtsam
│ │ │ │ -
461
│ │ │ │ -
462BOOST_CLASS_VERSION(gtsam::JacobianFactor, 1)
│ │ │ │ -
463
│ │ │ │ -
464#include <gtsam/linear/JacobianFactor-inl.h>
│ │ │ │ -
465
│ │ │ │ -
466
│ │ │ │ -
VerticalBlockMatrix.h
A matrix with column blocks of pre-defined sizes.
│ │ │ │ -
VariableSlots.h
VariableSlots describes the structure of a combined factor in terms of where each block comes from in...
│ │ │ │ - │ │ │ │ -
GaussianFactor.h
A factor with a quadratic error function - a Gaussian.
│ │ │ │ -
global_includes.h
Included from all GTSAM files.
│ │ │ │ -
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │ +
280 /*
│ │ │ │ +
281 * Vector of errors returns 2D vector
│ │ │ │ +
282 * @param E essential matrix
│ │ │ │ +
283 * @param d inverse depth d
│ │ │ │ +
284 */
│ │ │ │ +
285 Vector evaluateError(
│ │ │ │ +
286 const EssentialMatrix& E, const double& d,
│ │ │ │ +
287 boost::optional<Matrix&> DE = boost::none,
│ │ │ │ +
288 boost::optional<Matrix&> Dd = boost::none) const override {
│ │ │ │ +
289 if (!DE) {
│ │ │ │ +
290 // Convert E from body to camera frame
│ │ │ │ +
291 EssentialMatrix cameraE = cRb_ * E;
│ │ │ │ +
292 // Evaluate error
│ │ │ │ +
293 return Base::evaluateError(cameraE, d, boost::none, Dd);
│ │ │ │ +
294 } else {
│ │ │ │ +
295 // Version with derivatives
│ │ │ │ +
296 Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5
│ │ │ │ +
297 EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E);
│ │ │ │ +
298 Vector e = Base::evaluateError(cameraE, d, D_e_cameraE, Dd);
│ │ │ │ +
299 *DE = D_e_cameraE * D_cameraE_E; // (2*5) * (5*5)
│ │ │ │ +
300 return e;
│ │ │ │ +
301 }
│ │ │ │ +
302 }
│ │ │ │ +
303
│ │ │ │ +
304 public:
│ │ │ │ +
305 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
306};
│ │ │ │ +
│ │ │ │ +
307// EssentialMatrixFactor3
│ │ │ │ +
308
│ │ │ │ +
322template <class CALIBRATION>
│ │ │ │ +
│ │ │ │ +
323class EssentialMatrixFactor4
│ │ │ │ +
324 : public NoiseModelFactorN<EssentialMatrix, CALIBRATION> {
│ │ │ │ +
325 private:
│ │ │ │ +
326 Point2 pA_, pB_;
│ │ │ │ +
327
│ │ │ │ +
328 typedef NoiseModelFactorN<EssentialMatrix, CALIBRATION> Base;
│ │ │ │ +
329 typedef EssentialMatrixFactor4 This;
│ │ │ │ +
330
│ │ │ │ +
331 static constexpr int DimK = FixedDimension<CALIBRATION>::value;
│ │ │ │ +
332 typedef Eigen::Matrix<double, 2, DimK> JacobianCalibration;
│ │ │ │ +
333
│ │ │ │ +
334 public:
│ │ │ │ +
│ │ │ │ +
344 EssentialMatrixFactor4(Key keyE, Key keyK, const Point2& pA, const Point2& pB,
│ │ │ │ +
345 const SharedNoiseModel& model)
│ │ │ │ +
346 : Base(model, keyE, keyK), pA_(pA), pB_(pB) {}
│ │ │ │ +
│ │ │ │ +
347
│ │ │ │ +
│ │ │ │ +
349 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
350 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ +
351 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
│ │ │ │ +
352 }
│ │ │ │ +
│ │ │ │ +
353
│ │ │ │ +
│ │ │ │ +
355 void print(
│ │ │ │ +
356 const std::string& s = "",
│ │ │ │ +
357 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ +
358 Base::print(s);
│ │ │ │ +
359 std::cout << " EssentialMatrixFactor4 with measurements\n ("
│ │ │ │ +
360 << pA_.transpose() << ")' and (" << pB_.transpose() << ")'"
│ │ │ │ +
361 << std::endl;
│ │ │ │ +
362 }
│ │ │ │ +
│ │ │ │ +
363
│ │ │ │ +
│ │ │ │ +
373 Vector evaluateError(
│ │ │ │ +
374 const EssentialMatrix& E, const CALIBRATION& K,
│ │ │ │ +
375 boost::optional<Matrix&> H1 = boost::none,
│ │ │ │ +
376 boost::optional<Matrix&> H2 = boost::none) const override {
│ │ │ │ +
377 // converting from pixel coordinates to normalized coordinates cA and cB
│ │ │ │ +
378 JacobianCalibration cA_H_K; // dcA/dK
│ │ │ │ +
379 JacobianCalibration cB_H_K; // dcB/dK
│ │ │ │ +
380 Point2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0, boost::none);
│ │ │ │ +
381 Point2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0, boost::none);
│ │ │ │ +
382
│ │ │ │ +
383 // convert to homogeneous coordinates
│ │ │ │ +
384 Vector3 vA = EssentialMatrix::Homogeneous(cA);
│ │ │ │ +
385 Vector3 vB = EssentialMatrix::Homogeneous(cB);
│ │ │ │ +
386
│ │ │ │ +
387 if (H2) {
│ │ │ │ +
388 // compute the jacobian of error w.r.t K
│ │ │ │ +
389
│ │ │ │ +
390 // error function f = vA.T * E * vB
│ │ │ │ +
391 // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK
│ │ │ │ +
392 // where dvA/dK = dvA/dcA * dcA/dK, dVB/dK = dvB/dcB * dcB/dK
│ │ │ │ +
393 // and dvA/dcA = dvB/dcB = [[1, 0], [0, 1], [0, 0]]
│ │ │ │ +
394 *H2 = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K +
│ │ │ │ +
395 vA.transpose() * E.matrix().leftCols<2>() * cB_H_K;
│ │ │ │ +
396 }
│ │ │ │ +
397
│ │ │ │ +
398 Vector error(1);
│ │ │ │ +
399 error << E.error(vA, vB, H1);
│ │ │ │ +
400
│ │ │ │ +
401 return error;
│ │ │ │ +
402 }
│ │ │ │ +
│ │ │ │ +
403
│ │ │ │ +
404 public:
│ │ │ │ +
405 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
406};
│ │ │ │ +
│ │ │ │ +
407// EssentialMatrixFactor4
│ │ │ │ +
408
│ │ │ │ +
409} // namespace gtsam
│ │ │ │ +
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ +
PinholeCamera.h
Base class for all pinhole cameras.
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │ -
gtsam::save
void save(const Matrix &A, const string &s, const string &filename)
save a matrix to file, which can be loaded by matlab
Definition Matrix.cpp:167
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
│ │ │ │ -
gtsam::EliminateQR
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Multiply all factors and eliminate the given keys from the resulting factor using a QR variant that h...
Definition JacobianFactor.cpp:789
│ │ │ │ +
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │ +
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ +
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::VerticalBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43
│ │ │ │ -
gtsam::VerticalBlockMatrix::range
Block range(DenseIndex startBlock, DenseIndex endBlock)
access ranges of blocks at a time
Definition VerticalBlockMatrix.h:130
│ │ │ │ -
gtsam::VerticalBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition VerticalBlockMatrix.h:115
│ │ │ │ -
gtsam::VerticalBlockMatrix::cols
DenseIndex cols() const
Column size.
Definition VerticalBlockMatrix.h:118
│ │ │ │ -
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition Factor.h:80
│ │ │ │ -
gtsam::Factor::iterator
KeyVector::iterator iterator
Iterator over keys.
Definition Factor.h:77
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ -
gtsam::VariableSlots
A combined factor is assembled as one block of rows for each component factor.
Definition VariableSlots.h:52
│ │ │ │ -
gtsam::GaussianFactor
An abstract virtual base class for JacobianFactor and HessianFactor.
Definition GaussianFactor.h:39
│ │ │ │ -
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactor.h:42
│ │ │ │ -
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ -
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition HessianFactor.h:101
│ │ │ │ -
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition JacobianFactor.h:91
│ │ │ │ -
gtsam::JacobianFactor::getb
const constBVector getb() const
Get a view of the r.h.s.
Definition JacobianFactor.h:297
│ │ │ │ -
gtsam::JacobianFactor::getb
BVector getb()
Get a view of the r.h.s.
Definition JacobianFactor.h:306
│ │ │ │ -
gtsam::JacobianFactor::get_model
const SharedDiagonal & get_model() const
get a copy of model
Definition JacobianFactor.h:291
│ │ │ │ -
gtsam::JacobianFactor::JacobianFactor
JacobianFactor(const JacobianFactor &jf)
Copy constructor.
Definition JacobianFactor.h:114
│ │ │ │ -
gtsam::JacobianFactor::This
JacobianFactor This
Typedef to this class.
Definition JacobianFactor.h:94
│ │ │ │ -
gtsam::JacobianFactor::isConstrained
bool isConstrained() const
is noise model constrained ?
Definition JacobianFactor.h:269
│ │ │ │ -
gtsam::JacobianFactor::getA
constABlock getA() const
Get a view of the A matrix, not weighted by noise.
Definition JacobianFactor.h:303
│ │ │ │ -
gtsam::JacobianFactor::clone
GaussianFactor::shared_ptr clone() const override
Clone this JacobianFactor.
Definition JacobianFactor.h:189
│ │ │ │ -
gtsam::JacobianFactor::getA
ABlock getA(iterator variable)
Get a view of the A matrix for the variable pointed to by the given key iterator (non-const version)
Definition JacobianFactor.h:309
│ │ │ │ -
gtsam::JacobianFactor::get_model
SharedDiagonal & get_model()
get a copy of model (non-const version)
Definition JacobianFactor.h:294
│ │ │ │ -
gtsam::JacobianFactor::getDim
DenseIndex getDim(const_iterator variable) const override
Return the dimension of the variable pointed to by the given key iterator todo: Remove this in favor ...
Definition JacobianFactor.h:276
│ │ │ │ -
gtsam::JacobianFactor::matrixObject
VerticalBlockMatrix & matrixObject()
Mutable access to the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object.
Definition JacobianFactor.h:259
│ │ │ │ -
gtsam::JacobianFactor::getA
ABlock getA()
Get a view of the A matrix.
Definition JacobianFactor.h:312
│ │ │ │ -
gtsam::JacobianFactor::Base
GaussianFactor Base
Typedef to base class.
Definition JacobianFactor.h:95
│ │ │ │ -
gtsam::JacobianFactor::~JacobianFactor
~JacobianFactor() override
Virtual destructor.
Definition JacobianFactor.h:186
│ │ │ │ -
gtsam::JacobianFactor::matrixObject
const VerticalBlockMatrix & matrixObject() const
Return the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object.
Definition JacobianFactor.h:256
│ │ │ │ -
gtsam::JacobianFactor::rows
size_t rows() const
return the number of rows in the corresponding linear system
Definition JacobianFactor.h:283
│ │ │ │ -
gtsam::JacobianFactor::cols
size_t cols() const
return the number of columns in the corresponding linear system
Definition JacobianFactor.h:288
│ │ │ │ -
gtsam::JacobianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition JacobianFactor.h:96
│ │ │ │ -
gtsam::JacobianFactor::getA
constABlock getA(const_iterator variable) const
Get a view of the A matrix for the variable pointed to by the given key iterator.
Definition JacobianFactor.h:300
│ │ │ │ -
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ -
JacobianFactor
In Gaussian factors, the error function returns either the negative log-likelihood,...
│ │ │ │ -
GaussianConditional
is the normalization constant.
│ │ │ │ -
VectorValues
The Factor::error simply extracts the.
│ │ │ │ +
gtsam::FixedDimension
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition Manifold.h:164
│ │ │ │ +
gtsam::PinholeBase::Project
static Point2 Project(const Point3 &pc, OptionalJacobian< 2, 3 > Dpoint=boost::none)
Project from 3D point in camera coordinates into image Does not throw a CheiralityException,...
Definition CalibratedCamera.cpp:88
│ │ │ │ +
gtsam::EssentialMatrix
An essential matrix is like a Pose3, except with translation up to scale It is named after the 3*3 ma...
Definition EssentialMatrix.h:26
│ │ │ │ +
gtsam::EssentialMatrix::Homogeneous
static Vector3 Homogeneous(const Point2 &p)
Static function to convert Point2 to homogeneous coordinates.
Definition EssentialMatrix.h:34
│ │ │ │ +
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
│ │ │ │ +
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
│ │ │ │ +
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Calculate the error of the factor.
Definition NonlinearFactor.cpp:138
│ │ │ │ +
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ +
gtsam::NoiseModelFactorN< EssentialMatrix >::key
Key key() const
Returns a key.
Definition NonlinearFactor.h:518
│ │ │ │ +
gtsam::EssentialMatrixFactor
Factor that evaluates epipolar error p'Ep for given essential matrix.
Definition EssentialMatrixFactor.h:34
│ │ │ │ +
gtsam::EssentialMatrixFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition EssentialMatrixFactor.h:76
│ │ │ │ +
gtsam::EssentialMatrixFactor::EssentialMatrixFactor
EssentialMatrixFactor(Key key, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model)
Constructor.
Definition EssentialMatrixFactor.h:49
│ │ │ │ +
gtsam::EssentialMatrixFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition EssentialMatrixFactor.h:82
│ │ │ │ +
gtsam::EssentialMatrixFactor::EssentialMatrixFactor
EssentialMatrixFactor(Key key, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model, boost::shared_ptr< CALIBRATION > K)
Constructor.
Definition EssentialMatrixFactor.h:66
│ │ │ │ +
gtsam::EssentialMatrixFactor::evaluateError
Vector evaluateError(const EssentialMatrix &E, boost::optional< Matrix & > H=boost::none) const override
vector of errors returns 1D vector
Definition EssentialMatrixFactor.h:92
│ │ │ │ +
gtsam::EssentialMatrixFactor2
Binary factor that optimizes for E and inverse depth d: assumes measurement in image 2 is perfect,...
Definition EssentialMatrixFactor.h:109
│ │ │ │ +
gtsam::EssentialMatrixFactor2::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition EssentialMatrixFactor.h:154
│ │ │ │ +
gtsam::EssentialMatrixFactor2::EssentialMatrixFactor2
EssentialMatrixFactor2(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model, boost::shared_ptr< CALIBRATION > K)
Constructor.
Definition EssentialMatrixFactor.h:144
│ │ │ │ +
gtsam::EssentialMatrixFactor2::EssentialMatrixFactor2
EssentialMatrixFactor2(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model)
Constructor.
Definition EssentialMatrixFactor.h:126
│ │ │ │ +
gtsam::EssentialMatrixFactor2::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition EssentialMatrixFactor.h:160
│ │ │ │ +
gtsam::EssentialMatrixFactor3
Binary factor that optimizes for E and inverse depth d: assumes measurement in image 2 is perfect,...
Definition EssentialMatrixFactor.h:231
│ │ │ │ +
gtsam::EssentialMatrixFactor3::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition EssentialMatrixFactor.h:273
│ │ │ │ +
gtsam::EssentialMatrixFactor3::EssentialMatrixFactor3
EssentialMatrixFactor3(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const Rot3 &cRb, const SharedNoiseModel &model)
Constructor.
Definition EssentialMatrixFactor.h:247
│ │ │ │ +
gtsam::EssentialMatrixFactor3::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition EssentialMatrixFactor.h:267
│ │ │ │ +
gtsam::EssentialMatrixFactor3::EssentialMatrixFactor3
EssentialMatrixFactor3(Key key1, Key key2, const Point2 &pA, const Point2 &pB, const Rot3 &cRb, const SharedNoiseModel &model, boost::shared_ptr< CALIBRATION > K)
Constructor.
Definition EssentialMatrixFactor.h:261
│ │ │ │ +
gtsam::EssentialMatrixFactor4
Binary factor that optimizes for E and calibration K using the algebraic epipolar error (K^-1 pA)'E (...
Definition EssentialMatrixFactor.h:324
│ │ │ │ +
gtsam::EssentialMatrixFactor4::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition EssentialMatrixFactor.h:355
│ │ │ │ +
gtsam::EssentialMatrixFactor4::evaluateError
Vector evaluateError(const EssentialMatrix &E, const CALIBRATION &K, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Calculate the algebraic epipolar error pA' (K^-1)' E K pB.
Definition EssentialMatrixFactor.h:373
│ │ │ │ +
gtsam::EssentialMatrixFactor4::EssentialMatrixFactor4
EssentialMatrixFactor4(Key keyE, Key keyK, const Point2 &pA, const Point2 &pB, const SharedNoiseModel &model)
Constructor.
Definition EssentialMatrixFactor.h:344
│ │ │ │ +
gtsam::EssentialMatrixFactor4::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition EssentialMatrixFactor.h:349
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,502 +1,488 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -JacobianFactor.h │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +EssentialMatrixFactor.h │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010-2014, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ +12/* │ │ │ │ │ +13 * @file EssentialMatrixFactor.h │ │ │ │ │ +14 * @brief EssentialMatrixFactor class │ │ │ │ │ +15 * @author Frank Dellaert │ │ │ │ │ +16 * @author Ayush Baid │ │ │ │ │ +17 * @author Akshay Krishnan │ │ │ │ │ +18 * @date December 17, 2013 │ │ │ │ │ +19 */ │ │ │ │ │ 20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_N_o_i_s_e_M_o_d_e_l_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_V_a_r_i_a_b_l_e_S_l_o_t_s_._h> │ │ │ │ │ +21#pragma once │ │ │ │ │ +22 │ │ │ │ │ +23#include │ │ │ │ │ +24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_i_n_h_o_l_e_C_a_m_e_r_a_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ 26 │ │ │ │ │ -27#include │ │ │ │ │ -28#include │ │ │ │ │ -29#include │ │ │ │ │ +27#include │ │ │ │ │ +28 │ │ │ │ │ +29namespace _g_t_s_a_m { │ │ │ │ │ 30 │ │ │ │ │ -31namespace _g_t_s_a_m { │ │ │ │ │ -32 │ │ │ │ │ -33 // Forward declarations │ │ │ │ │ -34 class HessianFactor; │ │ │ │ │ -35 class VariableSlots; │ │ │ │ │ -36 class GaussianFactorGraph; │ │ │ │ │ -37 class _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l; │ │ │ │ │ -38 class HessianFactor; │ │ │ │ │ -39 class _V_e_c_t_o_r_V_a_l_u_e_s; │ │ │ │ │ -40 class Ordering; │ │ │ │ │ -41 class _J_a_c_o_b_i_a_n_F_a_c_t_o_r; │ │ │ │ │ -42 │ │ │ │ │ -48 GTSAM_EXPORT std::pair, boost:: │ │ │ │ │ -shared_ptr > │ │ │ │ │ -49 _E_l_i_m_i_n_a_t_e_Q_R(const GaussianFactorGraph& factors, const Ordering& keys); │ │ │ │ │ -50 │ │ │ │ │ -_9_0 class GTSAM_EXPORT _J_a_c_o_b_i_a_n_F_a_c_t_o_r : public _G_a_u_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -91 { │ │ │ │ │ -92 public: │ │ │ │ │ -93 │ │ │ │ │ -_9_4 typedef _J_a_c_o_b_i_a_n_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ -_9_5 typedef _G_a_u_s_s_i_a_n_F_a_c_t_o_r _B_a_s_e; │ │ │ │ │ -_9_6 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -97 │ │ │ │ │ -98 typedef VerticalBlockMatrix::Block ABlock; │ │ │ │ │ -99 typedef VerticalBlockMatrix::constBlock constABlock; │ │ │ │ │ -100 typedef ABlock::ColXpr BVector; │ │ │ │ │ -101 typedef constABlock::ConstColXpr constBVector; │ │ │ │ │ -102 │ │ │ │ │ -103 protected: │ │ │ │ │ -104 │ │ │ │ │ -105 _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x Ab_; // the block view of the full matrix │ │ │ │ │ -106 noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with │ │ │ │ │ -diagonal covariance matrix │ │ │ │ │ -107 │ │ │ │ │ -108 public: │ │ │ │ │ -109 │ │ │ │ │ -111 explicit _J_a_c_o_b_i_a_n_F_a_c_t_o_r(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r& gf); │ │ │ │ │ -112 │ │ │ │ │ -_1_1_4 _J_a_c_o_b_i_a_n_F_a_c_t_o_r(const _J_a_c_o_b_i_a_n_F_a_c_t_o_r& jf) : _B_a_s_e(jf), Ab_(jf.Ab_), model_ │ │ │ │ │ -(jf.model_) {} │ │ │ │ │ -115 │ │ │ │ │ -117 explicit _J_a_c_o_b_i_a_n_F_a_c_t_o_r(const _H_e_s_s_i_a_n_F_a_c_t_o_r& hf); │ │ │ │ │ -118 │ │ │ │ │ -120 _J_a_c_o_b_i_a_n_F_a_c_t_o_r(); │ │ │ │ │ -121 │ │ │ │ │ -123 explicit _J_a_c_o_b_i_a_n_F_a_c_t_o_r(const Vector& b_in); │ │ │ │ │ -124 │ │ │ │ │ -126 _J_a_c_o_b_i_a_n_F_a_c_t_o_r(_K_e_y i1, const Matrix& A1, │ │ │ │ │ -127 const Vector& b, const SharedDiagonal& model = SharedDiagonal()); │ │ │ │ │ -128 │ │ │ │ │ -130 _J_a_c_o_b_i_a_n_F_a_c_t_o_r(_K_e_y i1, const Matrix& A1, │ │ │ │ │ -131 _K_e_y i2, const Matrix& A2, │ │ │ │ │ -132 const Vector& b, const SharedDiagonal& model = SharedDiagonal()); │ │ │ │ │ +_3_4class _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +35 Vector3 vA_, vB_; │ │ │ │ │ +36 │ │ │ │ │ +37 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_> _B_a_s_e; │ │ │ │ │ +38 typedef _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ +39 │ │ │ │ │ +40 public: │ │ │ │ │ +_4_9 _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r(_K_e_y _k_e_y, const _P_o_i_n_t_2& pA, const _P_o_i_n_t_2& pB, │ │ │ │ │ +50 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model) │ │ │ │ │ +51 : _B_a_s_e(model, _k_e_y) { │ │ │ │ │ +52 vA_ = _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_:_:_H_o_m_o_g_e_n_e_o_u_s(pA); │ │ │ │ │ +53 vB_ = _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_:_:_H_o_m_o_g_e_n_e_o_u_s(pB); │ │ │ │ │ +54 } │ │ │ │ │ +55 │ │ │ │ │ +65 template │ │ │ │ │ +_6_6 _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r(_K_e_y _k_e_y, const _P_o_i_n_t_2& pA, const _P_o_i_n_t_2& pB, │ │ │ │ │ +67 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model, │ │ │ │ │ +68 boost::shared_ptr K) │ │ │ │ │ +69 : _B_a_s_e(model, _k_e_y) { │ │ │ │ │ +70 assert(K); │ │ │ │ │ +71 vA_ = _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_:_:_H_o_m_o_g_e_n_e_o_u_s(K->calibrate(pA)); │ │ │ │ │ +72 vB_ = _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_:_:_H_o_m_o_g_e_n_e_o_u_s(K->calibrate(pB)); │ │ │ │ │ +73 } │ │ │ │ │ +74 │ │ │ │ │ +_7_6 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +77 return boost::static_pointer_cast( │ │ │ │ │ +78 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); │ │ │ │ │ +79 } │ │ │ │ │ +80 │ │ │ │ │ +_8_2 void _p_r_i_n_t( │ │ │ │ │ +83 const std::string& s = "", │ │ │ │ │ +84 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ +85 _B_a_s_e_:_:_p_r_i_n_t(s); │ │ │ │ │ +86 std::cout << " EssentialMatrixFactor with measurements\n (" │ │ │ │ │ +87 << vA_.transpose() << ")' and (" << vB_.transpose() << ")'" │ │ │ │ │ +88 << std::endl; │ │ │ │ │ +89 } │ │ │ │ │ +90 │ │ │ │ │ +_9_2 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r( │ │ │ │ │ +93 const _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x& E, │ │ │ │ │ +94 boost::optional H = boost::none) const override { │ │ │ │ │ +95 Vector _e_r_r_o_r(1); │ │ │ │ │ +96 _e_r_r_o_r << E.error(vA_, vB_, H); │ │ │ │ │ +97 return _e_r_r_o_r; │ │ │ │ │ +98 } │ │ │ │ │ +99 │ │ │ │ │ +100 public: │ │ │ │ │ +101 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +102}; │ │ │ │ │ +103 │ │ │ │ │ +_1_0_8class _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2 │ │ │ │ │ +109 : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +110 _P_o_i_n_t_3 dP1_; │ │ │ │ │ +111 _P_o_i_n_t_2 pn_; │ │ │ │ │ +112 double f_; │ │ │ │ │ +113 │ │ │ │ │ +114 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_,_ _d_o_u_b_l_e_> _B_a_s_e; │ │ │ │ │ +115 typedef _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2 _T_h_i_s; │ │ │ │ │ +116 │ │ │ │ │ +117 public: │ │ │ │ │ +_1_2_6 _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2(_K_e_y key1, _K_e_y key2, const _P_o_i_n_t_2& pA, const _P_o_i_n_t_2& │ │ │ │ │ +pB, │ │ │ │ │ +127 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model) │ │ │ │ │ +128 : _B_a_s_e(model, key1, key2), │ │ │ │ │ +129 dP1_(_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x::Homogeneous(pA)), │ │ │ │ │ +130 pn_(pB) { │ │ │ │ │ +131 f_ = 1.0; │ │ │ │ │ +132 } │ │ │ │ │ 133 │ │ │ │ │ -135 _J_a_c_o_b_i_a_n_F_a_c_t_o_r(_K_e_y i1, const Matrix& A1, _K_e_y i2, │ │ │ │ │ -136 const Matrix& A2, _K_e_y i3, const Matrix& A3, │ │ │ │ │ -137 const Vector& b, const SharedDiagonal& model = SharedDiagonal()); │ │ │ │ │ -138 │ │ │ │ │ -142 template │ │ │ │ │ -143 _J_a_c_o_b_i_a_n_F_a_c_t_o_r(const TERMS& terms, const Vector& b, const SharedDiagonal& │ │ │ │ │ -model = SharedDiagonal()); │ │ │ │ │ -144 │ │ │ │ │ -149 template │ │ │ │ │ -150 _J_a_c_o_b_i_a_n_F_a_c_t_o_r( │ │ │ │ │ -151 const KEYS& keys, const _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x& augmentedMatrix, const │ │ │ │ │ -SharedDiagonal& sigmas = SharedDiagonal()); │ │ │ │ │ +143 template │ │ │ │ │ +_1_4_4 _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2(_K_e_y key1, _K_e_y key2, const _P_o_i_n_t_2& pA, const _P_o_i_n_t_2& │ │ │ │ │ +pB, │ │ │ │ │ +145 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model, │ │ │ │ │ +146 boost::shared_ptr K) │ │ │ │ │ +147 : _B_a_s_e(model, key1, key2), │ │ │ │ │ +148 dP1_(_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x::Homogeneous(K->calibrate(pA))), │ │ │ │ │ +149 pn_(K->calibrate(pB)) { │ │ │ │ │ +150 f_ = 0.5 * (K->fx() + K->fy()); │ │ │ │ │ +151 } │ │ │ │ │ 152 │ │ │ │ │ -157 explicit _J_a_c_o_b_i_a_n_F_a_c_t_o_r( │ │ │ │ │ -158 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& graph); │ │ │ │ │ -159 │ │ │ │ │ -164 explicit _J_a_c_o_b_i_a_n_F_a_c_t_o_r( │ │ │ │ │ -165 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& graph, │ │ │ │ │ -166 const _V_a_r_i_a_b_l_e_S_l_o_t_s& p_variableSlots); │ │ │ │ │ -167 │ │ │ │ │ -172 explicit _J_a_c_o_b_i_a_n_F_a_c_t_o_r( │ │ │ │ │ -173 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& graph, │ │ │ │ │ -174 const _O_r_d_e_r_i_n_g& ordering); │ │ │ │ │ -175 │ │ │ │ │ -180 explicit _J_a_c_o_b_i_a_n_F_a_c_t_o_r( │ │ │ │ │ -181 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& graph, │ │ │ │ │ -182 const _O_r_d_e_r_i_n_g& ordering, │ │ │ │ │ -183 const _V_a_r_i_a_b_l_e_S_l_o_t_s& p_variableSlots); │ │ │ │ │ -184 │ │ │ │ │ -_1_8_6 _~_J_a_c_o_b_i_a_n_F_a_c_t_o_r() override {} │ │ │ │ │ -187 │ │ │ │ │ -_1_8_9 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r _c_l_o_n_e() const override { │ │ │ │ │ -190 return boost::static_pointer_cast( │ │ │ │ │ -191 boost::make_shared(*this)); │ │ │ │ │ -192 } │ │ │ │ │ -193 │ │ │ │ │ -194 // Implementing Testable interface │ │ │ │ │ -195 void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ -196 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override; │ │ │ │ │ -197 bool _e_q_u_a_l_s(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r& lf, double tol = 1e-9) const override; │ │ │ │ │ -198 │ │ │ │ │ -199 Vector unweighted_error(const _V_e_c_t_o_r_V_a_l_u_e_s& c) const; │ │ │ │ │ -200 Vector error_vector(const _V_e_c_t_o_r_V_a_l_u_e_s& c) const; │ │ │ │ │ -203 using GaussianFactor::error; │ │ │ │ │ -204 │ │ │ │ │ -206 double error(const _V_e_c_t_o_r_V_a_l_u_e_s& c) const override; │ │ │ │ │ -207 │ │ │ │ │ -216 Matrix augmentedInformation() const override; │ │ │ │ │ -217 │ │ │ │ │ -221 Matrix information() const override; │ │ │ │ │ -222 │ │ │ │ │ -224 using Base::hessianDiagonal; │ │ │ │ │ +_1_5_4 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +155 return boost::static_pointer_cast( │ │ │ │ │ +156 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); │ │ │ │ │ +157 } │ │ │ │ │ +158 │ │ │ │ │ +_1_6_0 void _p_r_i_n_t( │ │ │ │ │ +161 const std::string& s = "", │ │ │ │ │ +162 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ +163 _B_a_s_e_:_:_p_r_i_n_t(s); │ │ │ │ │ +164 std::cout << " EssentialMatrixFactor2 with measurements\n (" │ │ │ │ │ +165 << dP1_.transpose() << ")' and (" << pn_.transpose() << ")'" │ │ │ │ │ +166 << std::endl; │ │ │ │ │ +167 } │ │ │ │ │ +168 │ │ │ │ │ +169 /* │ │ │ │ │ +170 * Vector of errors returns 2D vector │ │ │ │ │ +171 * @param E essential matrix │ │ │ │ │ +172 * @param d inverse depth d │ │ │ │ │ +173 */ │ │ │ │ │ +174 Vector evaluateError( │ │ │ │ │ +175 const _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x& E, const double& d, │ │ │ │ │ +176 boost::optional DE = boost::none, │ │ │ │ │ +177 boost::optional Dd = boost::none) const override { │ │ │ │ │ +178 // We have point x,y in image 1 │ │ │ │ │ +179 // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d │ │ │ │ │ +180 // We then convert to second camera by P2 = 1R2'*(P1-1T2) │ │ │ │ │ +181 // The homogeneous coordinates of can be written as │ │ │ │ │ +182 // 2R1*(P1-1T2) == 2R1*d*(P1-1T2) == 2R1*((x,y,1)-d*1T2) │ │ │ │ │ +183 // where we multiplied with d which yields equivalent homogeneous │ │ │ │ │ +184 // coordinates. Note that this is just the homography 2R1 for d==0 The │ │ │ │ │ +point │ │ │ │ │ +185 // d*P1 = (x,y,1) is computed in constructor as dP1_ │ │ │ │ │ +186 │ │ │ │ │ +187 // Project to normalized image coordinates, then uncalibrate │ │ │ │ │ +188 _P_o_i_n_t_2 pn(0, 0); │ │ │ │ │ +189 if (!DE && !Dd) { │ │ │ │ │ +190 _P_o_i_n_t_3 _1T2 = E.direction().point3(); │ │ │ │ │ +191 _P_o_i_n_t_3 d1T2 = d * _1T2; │ │ │ │ │ +192 _P_o_i_n_t_3 dP2 = E.rotation().unrotate(dP1_ - d1T2); // 2R1*((x,y,1)-d*1T2) │ │ │ │ │ +193 pn = _P_i_n_h_o_l_e_B_a_s_e_:_:_P_r_o_j_e_c_t(dP2); │ │ │ │ │ +194 │ │ │ │ │ +195 } else { │ │ │ │ │ +196 // Calculate derivatives. TODO if slow: optimize with Mathematica │ │ │ │ │ +197 // 3*2 3*3 3*3 │ │ │ │ │ +198 Matrix D_1T2_dir, DdP2_rot, DP2_point; │ │ │ │ │ +199 │ │ │ │ │ +200 _P_o_i_n_t_3 _1T2 = E.direction().point3(D_1T2_dir); │ │ │ │ │ +201 _P_o_i_n_t_3 d1T2 = d * _1T2; │ │ │ │ │ +202 _P_o_i_n_t_3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point); │ │ │ │ │ +203 │ │ │ │ │ +204 Matrix23 Dpn_dP2; │ │ │ │ │ +205 pn = _P_i_n_h_o_l_e_B_a_s_e_:_:_P_r_o_j_e_c_t(dP2, Dpn_dP2); │ │ │ │ │ +206 │ │ │ │ │ +207 if (DE) { │ │ │ │ │ +208 Matrix DdP2_E(3, 5); │ │ │ │ │ +209 DdP2_E << DdP2_rot, -DP2_point * d * D_1T2_dir; // (3*3), (3*3) * (3*2) │ │ │ │ │ +210 *DE = f_ * Dpn_dP2 * DdP2_E; // (2*3) * (3*5) │ │ │ │ │ +211 } │ │ │ │ │ +212 │ │ │ │ │ +213 if (Dd) // efficient backwards computation: │ │ │ │ │ +214 // (2*3) * (3*3) * (3*1) │ │ │ │ │ +215 *Dd = -f_ * (Dpn_dP2 * (DP2_point * _1T2)); │ │ │ │ │ +216 } │ │ │ │ │ +217 _P_o_i_n_t_2 reprojectionError = pn - pn_; │ │ │ │ │ +218 return f_ * reprojectionError; │ │ │ │ │ +219 } │ │ │ │ │ +220 │ │ │ │ │ +221 public: │ │ │ │ │ +222 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +223}; │ │ │ │ │ +224// EssentialMatrixFactor2 │ │ │ │ │ 225 │ │ │ │ │ -227 void hessianDiagonalAdd(_V_e_c_t_o_r_V_a_l_u_e_s& d) const override; │ │ │ │ │ -228 │ │ │ │ │ -230 void hessianDiagonal(double* d) const override; │ │ │ │ │ -231 │ │ │ │ │ -233 std::map hessianBlockDiagonal() const override; │ │ │ │ │ +_2_3_1class _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_3 : public _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2 { │ │ │ │ │ +232 typedef _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2 _B_a_s_e; │ │ │ │ │ +233 typedef _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_3 _T_h_i_s; │ │ │ │ │ 234 │ │ │ │ │ -238 std::pair jacobian() const override; │ │ │ │ │ -239 │ │ │ │ │ -243 std::pair jacobianUnweighted() const; │ │ │ │ │ -244 │ │ │ │ │ -248 Matrix augmentedJacobian() const override; │ │ │ │ │ -249 │ │ │ │ │ -253 Matrix augmentedJacobianUnweighted() const; │ │ │ │ │ -254 │ │ │ │ │ -_2_5_6 const _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x& _m_a_t_r_i_x_O_b_j_e_c_t() const { return Ab_; } │ │ │ │ │ -257 │ │ │ │ │ -_2_5_9 _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x& _m_a_t_r_i_x_O_b_j_e_c_t() { return Ab_; } │ │ │ │ │ -260 │ │ │ │ │ -266 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r negate() const override; │ │ │ │ │ -267 │ │ │ │ │ -_2_6_9 bool _i_s_C_o_n_s_t_r_a_i_n_e_d() const { │ │ │ │ │ -270 return model_ && model_->isConstrained(); │ │ │ │ │ -271 } │ │ │ │ │ -272 │ │ │ │ │ -_2_7_6 _D_e_n_s_e_I_n_d_e_x _g_e_t_D_i_m(_c_o_n_s_t___i_t_e_r_a_t_o_r variable) const override { │ │ │ │ │ -277 return Ab_(variable - begin())._c_o_l_s(); │ │ │ │ │ +235 _R_o_t_3 cRb_; │ │ │ │ │ +236 │ │ │ │ │ +237 public: │ │ │ │ │ +_2_4_7 _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_3(_K_e_y key1, _K_e_y key2, const _P_o_i_n_t_2& pA, const _P_o_i_n_t_2& │ │ │ │ │ +pB, │ │ │ │ │ +248 const _R_o_t_3& cRb, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model) │ │ │ │ │ +249 : _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2(key1, key2, pA, pB, model), cRb_(cRb) {} │ │ │ │ │ +250 │ │ │ │ │ +260 template │ │ │ │ │ +_2_6_1 _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_3(_K_e_y key1, _K_e_y key2, const _P_o_i_n_t_2& pA, const _P_o_i_n_t_2& │ │ │ │ │ +pB, │ │ │ │ │ +262 const _R_o_t_3& cRb, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model, │ │ │ │ │ +263 boost::shared_ptr K) │ │ │ │ │ +264 : _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2(key1, key2, pA, pB, model, K), cRb_(cRb) {} │ │ │ │ │ +265 │ │ │ │ │ +_2_6_7 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +268 return boost::static_pointer_cast( │ │ │ │ │ +269 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); │ │ │ │ │ +270 } │ │ │ │ │ +271 │ │ │ │ │ +_2_7_3 void _p_r_i_n_t( │ │ │ │ │ +274 const std::string& s = "", │ │ │ │ │ +275 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ +276 _B_a_s_e_:_:_p_r_i_n_t(s); │ │ │ │ │ +277 std::cout << " EssentialMatrixFactor3 with rotation " << cRb_ << std::endl; │ │ │ │ │ 278 } │ │ │ │ │ 279 │ │ │ │ │ -_2_8_3 size_t _r_o_w_s() const { return Ab_._r_o_w_s(); } │ │ │ │ │ -284 │ │ │ │ │ -_2_8_8 size_t _c_o_l_s() const { return Ab_._c_o_l_s(); } │ │ │ │ │ -289 │ │ │ │ │ -_2_9_1 const SharedDiagonal& _g_e_t___m_o_d_e_l() const { return model_; } │ │ │ │ │ -292 │ │ │ │ │ -_2_9_4 SharedDiagonal& _g_e_t___m_o_d_e_l() { return model_; } │ │ │ │ │ -295 │ │ │ │ │ -_2_9_7 const constBVector _g_e_t_b() const { return Ab_(size()).col(0); } │ │ │ │ │ -298 │ │ │ │ │ -_3_0_0 constABlock _g_e_t_A(_c_o_n_s_t___i_t_e_r_a_t_o_r variable) const { return Ab_(variable - │ │ │ │ │ -begin()); } │ │ │ │ │ -301 │ │ │ │ │ -_3_0_3 constABlock _g_e_t_A() const { return Ab_._r_a_n_g_e(0, size()); } │ │ │ │ │ -304 │ │ │ │ │ -_3_0_6 BVector _g_e_t_b() { return Ab_(size()).col(0); } │ │ │ │ │ -307 │ │ │ │ │ -_3_0_9 ABlock _g_e_t_A(_i_t_e_r_a_t_o_r variable) { return Ab_(variable - begin()); } │ │ │ │ │ -310 │ │ │ │ │ -_3_1_2 ABlock _g_e_t_A() { return Ab_._r_a_n_g_e(0, size()); } │ │ │ │ │ -313 │ │ │ │ │ -319 void updateHessian(const _K_e_y_V_e_c_t_o_r& keys, _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x* info) const │ │ │ │ │ -override; │ │ │ │ │ -320 │ │ │ │ │ -322 Vector _o_p_e_r_a_t_o_r_*(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const; │ │ │ │ │ -323 │ │ │ │ │ -326 void transposeMultiplyAdd(double alpha, const Vector& e, │ │ │ │ │ -327 _V_e_c_t_o_r_V_a_l_u_e_s& x) const; │ │ │ │ │ -328 │ │ │ │ │ -330 void multiplyHessianAdd(double alpha, const _V_e_c_t_o_r_V_a_l_u_e_s& x, │ │ │ │ │ -331 _V_e_c_t_o_r_V_a_l_u_e_s& y) const override; │ │ │ │ │ -332 │ │ │ │ │ -341 void multiplyHessianAdd(double alpha, const double* x, double* y, │ │ │ │ │ -342 const std::vector& accumulatedDims) const; │ │ │ │ │ -343 │ │ │ │ │ -345 _V_e_c_t_o_r_V_a_l_u_e_s gradientAtZero() const override; │ │ │ │ │ -346 │ │ │ │ │ -348 void gradientAtZero(double* d) const override; │ │ │ │ │ -349 │ │ │ │ │ -351 Vector gradient(_K_e_y key, const _V_e_c_t_o_r_V_a_l_u_e_s& x) const override; │ │ │ │ │ -352 │ │ │ │ │ -354 _J_a_c_o_b_i_a_n_F_a_c_t_o_r whiten() const; │ │ │ │ │ -355 │ │ │ │ │ -357 std::pair, shared_ptr> │ │ │ │ │ -358 eliminate(const _O_r_d_e_r_i_n_g& keys); │ │ │ │ │ -359 │ │ │ │ │ -361 void setModel(bool anyConstrained, const Vector& sigmas); │ │ │ │ │ -362 │ │ │ │ │ -374 friend GTSAM_EXPORT std::pair, │ │ │ │ │ -shared_ptr> │ │ │ │ │ -375 _E_l_i_m_i_n_a_t_e_Q_R(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& factors, const _O_r_d_e_r_i_n_g& keys); │ │ │ │ │ -376 │ │ │ │ │ -384 boost::shared_ptr splitConditional(size_t nrFrontals); │ │ │ │ │ -385 │ │ │ │ │ -386 protected: │ │ │ │ │ -387 │ │ │ │ │ -389 template │ │ │ │ │ -390 void fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& │ │ │ │ │ -noiseModel); │ │ │ │ │ -391 │ │ │ │ │ -392 private: │ │ │ │ │ -393 │ │ │ │ │ -398 void JacobianFactorHelper( │ │ │ │ │ -399 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& graph, │ │ │ │ │ -400 const _F_a_s_t_V_e_c_t_o_r_<_V_a_r_i_a_b_l_e_S_l_o_t_s_:_:_c_o_n_s_t___i_t_e_r_a_t_o_r_>& orderedSlots); │ │ │ │ │ -401 │ │ │ │ │ -408 template │ │ │ │ │ -409 _J_a_c_o_b_i_a_n_F_a_c_t_o_r(const KEYS& keys, const DIMENSIONS& dims, _D_e_n_s_e_I_n_d_e_x m, │ │ │ │ │ -410 const SharedDiagonal& model = SharedDiagonal()) : │ │ │ │ │ -411 Base(keys), Ab_(dims.begin(), dims.end(), m, true), model_(model) { │ │ │ │ │ -412 } │ │ │ │ │ -413 │ │ │ │ │ -414 // be very selective on who can access these private methods: │ │ │ │ │ -415 template friend class ExpressionFactor; │ │ │ │ │ -416 │ │ │ │ │ -_4_1_8 friend class boost::serialization::access; │ │ │ │ │ -419 template │ │ │ │ │ -420 void _s_a_v_e(ARCHIVE & ar, const unsigned int version) const { │ │ │ │ │ -421 // TODO(fan): This is a hack for Boost < 1.66 │ │ │ │ │ -422 // We really need to introduce proper versioning in the archives │ │ │ │ │ -423 // As otherwise this will not read objects serialized by older │ │ │ │ │ -424 // versions of GTSAM │ │ │ │ │ -425 ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -426 ar << BOOST_SERIALIZATION_NVP(Ab_); │ │ │ │ │ -427 bool model_null = false; │ │ │ │ │ -428 if(model_.get() == nullptr) { │ │ │ │ │ -429 model_null = true; │ │ │ │ │ -430 ar << boost::serialization::make_nvp("model_null", model_null); │ │ │ │ │ -431 } else { │ │ │ │ │ -432 ar << boost::serialization::make_nvp("model_null", model_null); │ │ │ │ │ -433 ar << BOOST_SERIALIZATION_NVP(model_); │ │ │ │ │ -434 } │ │ │ │ │ -435 } │ │ │ │ │ -436 │ │ │ │ │ -437 template │ │ │ │ │ -438 void load(ARCHIVE & ar, const unsigned int version) { │ │ │ │ │ -439 // invoke serialization of the base class │ │ │ │ │ -440 ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); │ │ │ │ │ -441 ar >> BOOST_SERIALIZATION_NVP(Ab_); │ │ │ │ │ -442 if (version < 1) { │ │ │ │ │ -443 ar >> BOOST_SERIALIZATION_NVP(model_); │ │ │ │ │ -444 } else { │ │ │ │ │ -445 bool model_null; │ │ │ │ │ -446 ar >> BOOST_SERIALIZATION_NVP(model_null); │ │ │ │ │ -447 if (!model_null) { │ │ │ │ │ -448 ar >> BOOST_SERIALIZATION_NVP(model_); │ │ │ │ │ -449 } │ │ │ │ │ -450 } │ │ │ │ │ -451 } │ │ │ │ │ -452 │ │ │ │ │ -453 BOOST_SERIALIZATION_SPLIT_MEMBER() │ │ │ │ │ -454 }; // JacobianFactor │ │ │ │ │ -456template<> │ │ │ │ │ -_4_5_7struct _t_r_a_i_t_s<_J_a_c_o_b_i_a_n_F_a_c_t_o_r> : public _T_e_s_t_a_b_l_e { │ │ │ │ │ -458}; │ │ │ │ │ -459 │ │ │ │ │ -460} // \ namespace gtsam │ │ │ │ │ -461 │ │ │ │ │ -462BOOST_CLASS_VERSION(_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r, 1) │ │ │ │ │ -463 │ │ │ │ │ -464#include │ │ │ │ │ -465 │ │ │ │ │ -466 │ │ │ │ │ -_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_._h │ │ │ │ │ -A matrix with column blocks of pre-defined sizes. │ │ │ │ │ -_V_a_r_i_a_b_l_e_S_l_o_t_s_._h │ │ │ │ │ -VariableSlots describes the structure of a combined factor in terms of where │ │ │ │ │ -each block comes from in... │ │ │ │ │ -_N_o_i_s_e_M_o_d_e_l_._h │ │ │ │ │ -_G_a_u_s_s_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ -A factor with a quadratic error function - a Gaussian. │ │ │ │ │ -_g_l_o_b_a_l___i_n_c_l_u_d_e_s_._h │ │ │ │ │ -Included from all GTSAM files. │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ -std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ -FastVector │ │ │ │ │ -FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ -DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ +280 /* │ │ │ │ │ +281 * Vector of errors returns 2D vector │ │ │ │ │ +282 * @param E essential matrix │ │ │ │ │ +283 * @param d inverse depth d │ │ │ │ │ +284 */ │ │ │ │ │ +285 Vector evaluateError( │ │ │ │ │ +286 const _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x& E, const double& d, │ │ │ │ │ +287 boost::optional DE = boost::none, │ │ │ │ │ +288 boost::optional Dd = boost::none) const override { │ │ │ │ │ +289 if (!DE) { │ │ │ │ │ +290 // Convert E from body to camera frame │ │ │ │ │ +291 _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x cameraE = cRb_ * E; │ │ │ │ │ +292 // Evaluate error │ │ │ │ │ +293 return Base::evaluateError(cameraE, d, boost::none, Dd); │ │ │ │ │ +294 } else { │ │ │ │ │ +295 // Version with derivatives │ │ │ │ │ +296 Matrix D_e_cameraE, D_cameraE_E; // 2*5, 5*5 │ │ │ │ │ +297 EssentialMatrix cameraE = E.rotate(cRb_, D_cameraE_E); │ │ │ │ │ +298 Vector e = Base::evaluateError(cameraE, d, D_e_cameraE, Dd); │ │ │ │ │ +299 *DE = D_e_cameraE * D_cameraE_E; // (2*5) * (5*5) │ │ │ │ │ +300 return e; │ │ │ │ │ +301 } │ │ │ │ │ +302 } │ │ │ │ │ +303 │ │ │ │ │ +304 public: │ │ │ │ │ +305 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +306}; │ │ │ │ │ +307// EssentialMatrixFactor3 │ │ │ │ │ +308 │ │ │ │ │ +322template │ │ │ │ │ +_3_2_3class _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_4 │ │ │ │ │ +324 : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +325 private: │ │ │ │ │ +326 _P_o_i_n_t_2 pA_, pB_; │ │ │ │ │ +327 │ │ │ │ │ +328 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_,_ _C_A_L_I_B_R_A_T_I_O_N_> _B_a_s_e; │ │ │ │ │ +329 typedef _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_4 _T_h_i_s; │ │ │ │ │ +330 │ │ │ │ │ +331 static constexpr int DimK = _F_i_x_e_d_D_i_m_e_n_s_i_o_n_<_C_A_L_I_B_R_A_T_I_O_N_>_:_:_v_a_l_u_e; │ │ │ │ │ +332 typedef Eigen::Matrix JacobianCalibration; │ │ │ │ │ +333 │ │ │ │ │ +334 public: │ │ │ │ │ +_3_4_4 _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_4(_K_e_y keyE, _K_e_y keyK, const _P_o_i_n_t_2& pA, const _P_o_i_n_t_2& │ │ │ │ │ +pB, │ │ │ │ │ +345 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model) │ │ │ │ │ +346 : _B_a_s_e(model, keyE, keyK), pA_(pA), pB_(pB) {} │ │ │ │ │ +347 │ │ │ │ │ +_3_4_9 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +350 return boost::static_pointer_cast( │ │ │ │ │ +351 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); │ │ │ │ │ +352 } │ │ │ │ │ +353 │ │ │ │ │ +_3_5_5 void _p_r_i_n_t( │ │ │ │ │ +356 const std::string& s = "", │ │ │ │ │ +357 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ +358 _B_a_s_e_:_:_p_r_i_n_t(s); │ │ │ │ │ +359 std::cout << " EssentialMatrixFactor4 with measurements\n (" │ │ │ │ │ +360 << pA_.transpose() << ")' and (" << pB_.transpose() << ")'" │ │ │ │ │ +361 << std::endl; │ │ │ │ │ +362 } │ │ │ │ │ +363 │ │ │ │ │ +_3_7_3 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r( │ │ │ │ │ +374 const _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x& E, const CALIBRATION& K, │ │ │ │ │ +375 boost::optional H1 = boost::none, │ │ │ │ │ +376 boost::optional H2 = boost::none) const override { │ │ │ │ │ +377 // converting from pixel coordinates to normalized coordinates cA and cB │ │ │ │ │ +378 JacobianCalibration cA_H_K; // dcA/dK │ │ │ │ │ +379 JacobianCalibration cB_H_K; // dcB/dK │ │ │ │ │ +380 _P_o_i_n_t_2 cA = K.calibrate(pA_, H2 ? &cA_H_K : 0, boost::none); │ │ │ │ │ +381 _P_o_i_n_t_2 cB = K.calibrate(pB_, H2 ? &cB_H_K : 0, boost::none); │ │ │ │ │ +382 │ │ │ │ │ +383 // convert to homogeneous coordinates │ │ │ │ │ +384 Vector3 vA = _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_:_:_H_o_m_o_g_e_n_e_o_u_s(cA); │ │ │ │ │ +385 Vector3 vB = _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_:_:_H_o_m_o_g_e_n_e_o_u_s(cB); │ │ │ │ │ +386 │ │ │ │ │ +387 if (H2) { │ │ │ │ │ +388 // compute the jacobian of error w.r.t K │ │ │ │ │ +389 │ │ │ │ │ +390 // error function f = vA.T * E * vB │ │ │ │ │ +391 // H2 = df/dK = vB.T * E.T * dvA/dK + vA.T * E * dvB/dK │ │ │ │ │ +392 // where dvA/dK = dvA/dcA * dcA/dK, dVB/dK = dvB/dcB * dcB/dK │ │ │ │ │ +393 // and dvA/dcA = dvB/dcB = [[1, 0], [0, 1], [0, 0]] │ │ │ │ │ +394 *H2 = vB.transpose() * E.matrix().transpose().leftCols<2>() * cA_H_K + │ │ │ │ │ +395 vA.transpose() * E.matrix().leftCols<2>() * cB_H_K; │ │ │ │ │ +396 } │ │ │ │ │ +397 │ │ │ │ │ +398 Vector _e_r_r_o_r(1); │ │ │ │ │ +399 _e_r_r_o_r << E.error(vA, vB, H1); │ │ │ │ │ +400 │ │ │ │ │ +401 return _e_r_r_o_r; │ │ │ │ │ +402 } │ │ │ │ │ +403 │ │ │ │ │ +404 public: │ │ │ │ │ +405 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +406}; │ │ │ │ │ +407// EssentialMatrixFactor4 │ │ │ │ │ +408 │ │ │ │ │ +409} // namespace gtsam │ │ │ │ │ +_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ +This marks a GTSAM object to require alignment. │ │ │ │ │ +DDeeffiinniittiioonn types.h:308 │ │ │ │ │ +_P_i_n_h_o_l_e_C_a_m_e_r_a_._h │ │ │ │ │ +Base class for all pinhole cameras. │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ +Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_n_s_e_I_n_d_e_x │ │ │ │ │ -ptrdiff_t DenseIndex │ │ │ │ │ -The index type for Eigen objects. │ │ │ │ │ -DDeeffiinniittiioonn types.h:106 │ │ │ │ │ -_g_t_s_a_m_:_:_s_a_v_e │ │ │ │ │ -void save(const Matrix &A, const string &s, const string &filename) │ │ │ │ │ -save a matrix to file, which can be loaded by matlab │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:167 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ -Point2 operator*(double s, const Point2 &p) │ │ │ │ │ -multiply with scalar │ │ │ │ │ -DDeeffiinniittiioonn Point2.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_Q_R │ │ │ │ │ -std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > │ │ │ │ │ -EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys) │ │ │ │ │ -Multiply all factors and eliminate the given keys from the resulting factor │ │ │ │ │ -using a QR variant that h... │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.cpp:789 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ +Vector2 Point2 │ │ │ │ │ +As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ +typedef Point2 to Vector2... │ │ │ │ │ +DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ +Vector3 Point3 │ │ │ │ │ +As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ +typedef Point3 to Vector3... │ │ │ │ │ +DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ +Aliases. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ -of blocks. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ -of vertical blocks. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_r_a_n_g_e │ │ │ │ │ -Block range(DenseIndex startBlock, DenseIndex endBlock) │ │ │ │ │ -access ranges of blocks at a time │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:130 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_r_o_w_s │ │ │ │ │ -DenseIndex rows() const │ │ │ │ │ -Row size. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:115 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_c_o_l_s │ │ │ │ │ -DenseIndex cols() const │ │ │ │ │ -Column size. │ │ │ │ │ -DDeeffiinniittiioonn VerticalBlockMatrix.h:118 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_c_o_n_s_t___i_t_e_r_a_t_o_r │ │ │ │ │ -KeyVector::const_iterator const_iterator │ │ │ │ │ -Const iterator over keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:80 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_i_t_e_r_a_t_o_r │ │ │ │ │ -KeyVector::iterator iterator │ │ │ │ │ -Iterator over keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_S_l_o_t_s │ │ │ │ │ -A combined factor is assembled as one block of rows for each component factor. │ │ │ │ │ -DDeeffiinniittiioonn VariableSlots.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -An abstract virtual base class for JacobianFactor and HessianFactor. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactor.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -A Gaussian factor using the canonical parameters (information form) │ │ │ │ │ -DDeeffiinniittiioonn HessianFactor.h:101 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ -A Gaussian factor in the squared-error form. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:91 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_e_t_b │ │ │ │ │ -const constBVector getb() const │ │ │ │ │ -Get a view of the r.h.s. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:297 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_e_t_b │ │ │ │ │ -BVector getb() │ │ │ │ │ -Get a view of the r.h.s. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:306 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_e_t___m_o_d_e_l │ │ │ │ │ -const SharedDiagonal & get_model() const │ │ │ │ │ -get a copy of model │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:291 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ -JacobianFactor(const JacobianFactor &jf) │ │ │ │ │ -Copy constructor. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:114 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_T_h_i_s │ │ │ │ │ -JacobianFactor This │ │ │ │ │ -Typedef to this class. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:94 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_i_s_C_o_n_s_t_r_a_i_n_e_d │ │ │ │ │ -bool isConstrained() const │ │ │ │ │ -is noise model constrained ? │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:269 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_e_t_A │ │ │ │ │ -constABlock getA() const │ │ │ │ │ -Get a view of the A matrix, not weighted by noise. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:303 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ -GaussianFactor::shared_ptr clone() const override │ │ │ │ │ -Clone this JacobianFactor. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:189 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_e_t_A │ │ │ │ │ -ABlock getA(iterator variable) │ │ │ │ │ -Get a view of the A matrix for the variable pointed to by the given key │ │ │ │ │ -iterator (non-const version) │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:309 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_e_t___m_o_d_e_l │ │ │ │ │ -SharedDiagonal & get_model() │ │ │ │ │ -get a copy of model (non-const version) │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:294 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_e_t_D_i_m │ │ │ │ │ -DenseIndex getDim(const_iterator variable) const override │ │ │ │ │ -Return the dimension of the variable pointed to by the given key iterator todo: │ │ │ │ │ -Remove this in favor ... │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:276 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_m_a_t_r_i_x_O_b_j_e_c_t │ │ │ │ │ -VerticalBlockMatrix & matrixObject() │ │ │ │ │ -Mutable access to the full augmented Jacobian matrix of this factor as a │ │ │ │ │ -VerticalBlockMatrix object. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:259 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_e_t_A │ │ │ │ │ -ABlock getA() │ │ │ │ │ -Get a view of the A matrix. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:312 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_B_a_s_e │ │ │ │ │ -GaussianFactor Base │ │ │ │ │ -Typedef to base class. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:95 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_~_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ -~JacobianFactor() override │ │ │ │ │ -Virtual destructor. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:186 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_m_a_t_r_i_x_O_b_j_e_c_t │ │ │ │ │ -const VerticalBlockMatrix & matrixObject() const │ │ │ │ │ -Return the full augmented Jacobian matrix of this factor as a │ │ │ │ │ -VerticalBlockMatrix object. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:256 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_r_o_w_s │ │ │ │ │ -size_t rows() const │ │ │ │ │ -return the number of rows in the corresponding linear system │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:283 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_c_o_l_s │ │ │ │ │ -size_t cols() const │ │ │ │ │ -return the number of columns in the corresponding linear system │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:288 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:96 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_e_t_A │ │ │ │ │ -constABlock getA(const_iterator variable) const │ │ │ │ │ -Get a view of the A matrix for the variable pointed to by the given key │ │ │ │ │ -iterator. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:300 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ -with a unique integer... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ -_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ -In Gaussian factors, the error function returns either the negative log- │ │ │ │ │ -likelihood,... │ │ │ │ │ -_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -is the normalization constant. │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -The Factor::error simply extracts the. │ │ │ │ │ +_g_t_s_a_m_:_:_F_i_x_e_d_D_i_m_e_n_s_i_o_n │ │ │ │ │ +Give fixed size dimension of a type, fails at compile time if dynamic. │ │ │ │ │ +DDeeffiinniittiioonn Manifold.h:164 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_:_:_P_r_o_j_e_c_t │ │ │ │ │ +static Point2 Project(const Point3 &pc, OptionalJacobian< 2, 3 > Dpoint=boost:: │ │ │ │ │ +none) │ │ │ │ │ +Project from 3D point in camera coordinates into image Does not throw a │ │ │ │ │ +CheiralityException,... │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.cpp:88 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x │ │ │ │ │ +An essential matrix is like a Pose3, except with translation up to scale It is │ │ │ │ │ +named after the 3*3 ma... │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrix.h:26 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_:_:_H_o_m_o_g_e_n_e_o_u_s │ │ │ │ │ +static Vector3 Homogeneous(const Point2 &p) │ │ │ │ │ +Static function to convert Point2 to homogeneous coordinates. │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrix.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ +Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor │ │ │ │ │ +symbol GTSAM_USE_QUATERNIO... │ │ │ │ │ +DDeeffiinniittiioonn Rot3.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +virtual void print(const std::string &s="Factor", const KeyFormatter │ │ │ │ │ +&formatter=DefaultKeyFormatter) const │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn Factor.cpp:29 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_e_r_r_o_r │ │ │ │ │ +double error(const Values &c) const override │ │ │ │ │ +Calculate the error of the factor. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.cpp:138 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ +A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ +variables. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_ _E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_ _>_:_:_k_e_y │ │ │ │ │ +Key key() const │ │ │ │ │ +Returns a key. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:518 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r │ │ │ │ │ +Factor that evaluates epipolar error p'Ep for given essential matrix. │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrixFactor.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ +gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrixFactor.h:76 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r │ │ │ │ │ +EssentialMatrixFactor(Key key, const Point2 &pA, const Point2 &pB, const │ │ │ │ │ +SharedNoiseModel &model) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrixFactor.h:49 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrixFactor.h:82 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r │ │ │ │ │ +EssentialMatrixFactor(Key key, const Point2 &pA, const Point2 &pB, const │ │ │ │ │ +SharedNoiseModel &model, boost::shared_ptr< CALIBRATION > K) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrixFactor.h:66 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ +Vector evaluateError(const EssentialMatrix &E, boost::optional< Matrix & > │ │ │ │ │ +H=boost::none) const override │ │ │ │ │ +vector of errors returns 1D vector │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrixFactor.h:92 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2 │ │ │ │ │ +Binary factor that optimizes for E and inverse depth d: assumes measurement in │ │ │ │ │ +image 2 is perfect,... │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrixFactor.h:109 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2_:_:_c_l_o_n_e │ │ │ │ │ +gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrixFactor.h:154 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2 │ │ │ │ │ +EssentialMatrixFactor2(Key key1, Key key2, const Point2 &pA, const Point2 &pB, │ │ │ │ │ +const SharedNoiseModel &model, boost::shared_ptr< CALIBRATION > K) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrixFactor.h:144 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2 │ │ │ │ │ +EssentialMatrixFactor2(Key key1, Key key2, const Point2 &pA, const Point2 &pB, │ │ │ │ │ +const SharedNoiseModel &model) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrixFactor.h:126 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_2_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrixFactor.h:160 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_3 │ │ │ │ │ +Binary factor that optimizes for E and inverse depth d: assumes measurement in │ │ │ │ │ +image 2 is perfect,... │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrixFactor.h:231 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_3_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrixFactor.h:273 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_3_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_3 │ │ │ │ │ +EssentialMatrixFactor3(Key key1, Key key2, const Point2 &pA, const Point2 &pB, │ │ │ │ │ +const Rot3 &cRb, const SharedNoiseModel &model) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrixFactor.h:247 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_3_:_:_c_l_o_n_e │ │ │ │ │ +gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrixFactor.h:267 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_3_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_3 │ │ │ │ │ +EssentialMatrixFactor3(Key key1, Key key2, const Point2 &pA, const Point2 &pB, │ │ │ │ │ +const Rot3 &cRb, const SharedNoiseModel &model, boost::shared_ptr< CALIBRATION │ │ │ │ │ +> K) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrixFactor.h:261 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_4 │ │ │ │ │ +Binary factor that optimizes for E and calibration K using the algebraic │ │ │ │ │ +epipolar error (K^-1 pA)'E (... │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrixFactor.h:324 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_4_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrixFactor.h:355 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_4_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ +Vector evaluateError(const EssentialMatrix &E, const CALIBRATION &K, boost:: │ │ │ │ │ +optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost:: │ │ │ │ │ +none) const override │ │ │ │ │ +Calculate the algebraic epipolar error pA' (K^-1)' E K pB. │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrixFactor.h:373 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_4_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_4 │ │ │ │ │ +EssentialMatrixFactor4(Key keyE, Key keyK, const Point2 &pA, const Point2 &pB, │ │ │ │ │ +const SharedNoiseModel &model) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrixFactor.h:344 │ │ │ │ │ +_g_t_s_a_m_:_:_E_s_s_e_n_t_i_a_l_M_a_t_r_i_x_F_a_c_t_o_r_4_:_:_c_l_o_n_e │ │ │ │ │ +gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn EssentialMatrixFactor.h:349 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _l_i_n_e_a_r │ │ │ │ │ - * _J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ + * _s_l_a_m │ │ │ │ │ + * EEsssseennttiiaallMMaattrriixxFFaaccttoorr..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00767_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00512_source.html │ │ │ │┄ Files 92% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/SparseEigen.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridSmoother.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
SparseEigen.h
│ │ │ │ +
HybridSmoother.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
24#pragma once
│ │ │ │ -
25
│ │ │ │ -
26#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ -
27#include <gtsam/linear/VectorValues.h>
│ │ │ │ -
28
│ │ │ │ -
29#include <Eigen/Sparse>
│ │ │ │ -
30
│ │ │ │ -
31namespace gtsam {
│ │ │ │ -
32
│ │ │ │ -
35typedef Eigen::SparseMatrix<double, Eigen::ColMajor, int> SparseEigen;
│ │ │ │ -
36
│ │ │ │ -
│ │ │ │ -
38SparseEigen sparseJacobianEigen(
│ │ │ │ -
39 const GaussianFactorGraph &gfg, const Ordering &ordering) {
│ │ │ │ -
40 gttic_(SparseEigen_sparseJacobianEigen);
│ │ │ │ -
41 // intermediate `entries` vector is kind of unavoidable due to how expensive
│ │ │ │ -
42 // factor->rows() is, which prevents us from populating SparseEigen directly.
│ │ │ │ -
43 size_t nrows, ncols;
│ │ │ │ -
44 auto entries = gfg.sparseJacobian(ordering, nrows, ncols);
│ │ │ │ -
45 // declare sparse matrix
│ │ │ │ -
46 SparseEigen Ab(nrows, ncols);
│ │ │ │ -
47 // See Eigen::set_from_triplets. This is about 5% faster.
│ │ │ │ -
48 // pass 1: count the nnz per inner-vector
│ │ │ │ -
49 std::vector<int> nnz(ncols, 0);
│ │ │ │ -
50 for (const auto &entry : entries) nnz[std::get<1>(entry)]++;
│ │ │ │ -
51 Ab.reserve(nnz);
│ │ │ │ -
52 // pass 2: insert the elements
│ │ │ │ -
53 for (const auto &entry : entries)
│ │ │ │ -
54 Ab.insert(std::get<0>(entry), std::get<1>(entry)) = std::get<2>(entry);
│ │ │ │ -
55 return Ab;
│ │ │ │ -
56}
│ │ │ │ -
│ │ │ │ -
57
│ │ │ │ -
58SparseEigen sparseJacobianEigen(const GaussianFactorGraph &gfg) {
│ │ │ │ -
59 gttic_(SparseEigen_sparseJacobianEigen_defaultOrdering);
│ │ │ │ -
60 return sparseJacobianEigen(gfg, Ordering(gfg.keys()));
│ │ │ │ -
61}
│ │ │ │ -
62
│ │ │ │ -
63} // namespace gtsam
│ │ │ │ -
VectorValues.h
Factor Graph Values.
│ │ │ │ -
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ +
19#include <gtsam/discrete/DiscreteFactorGraph.h>
│ │ │ │ +
20#include <gtsam/hybrid/HybridBayesNet.h>
│ │ │ │ +
21#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
│ │ │ │ +
22
│ │ │ │ +
23namespace gtsam {
│ │ │ │ +
24
│ │ │ │ +
│ │ │ │ +
25class HybridSmoother {
│ │ │ │ +
26 private:
│ │ │ │ +
27 HybridBayesNet hybridBayesNet_;
│ │ │ │ +
28 HybridGaussianFactorGraph remainingFactorGraph_;
│ │ │ │ +
29
│ │ │ │ +
30 public:
│ │ │ │ +
50 void update(HybridGaussianFactorGraph graph, const Ordering& ordering,
│ │ │ │ +
51 boost::optional<size_t> maxNrLeaves = boost::none);
│ │ │ │ +
52
│ │ │ │ +
53 Ordering getOrdering(const HybridGaussianFactorGraph& newFactors);
│ │ │ │ +
54
│ │ │ │ +
64 std::pair<HybridGaussianFactorGraph, HybridBayesNet> addConditionals(
│ │ │ │ +
65 const HybridGaussianFactorGraph& graph,
│ │ │ │ +
66 const HybridBayesNet& hybridBayesNet, const Ordering& ordering) const;
│ │ │ │ +
67
│ │ │ │ +
69 GaussianMixture::shared_ptr gaussianMixture(size_t index) const;
│ │ │ │ +
70
│ │ │ │ +
72 const HybridBayesNet& hybridBayesNet() const;
│ │ │ │ +
73};
│ │ │ │ +
│ │ │ │ +
74
│ │ │ │ +
75}; // namespace gtsam
│ │ │ │ + │ │ │ │ +
HybridGaussianFactorGraph.h
Linearized Hybrid factor graph that uses type erasure.
│ │ │ │ +
HybridBayesNet.h
A Bayes net of Gaussian Conditionals indexed by discrete keys.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::SparseEigen
Eigen::SparseMatrix< double, Eigen::ColMajor, int > SparseEigen
Eigen-format sparse matrix.
Definition SparseEigen.h:35
│ │ │ │ -
gtsam::sparseJacobianEigen
SparseEigen sparseJacobianEigen(const GaussianFactorGraph &gfg, const Ordering &ordering)
Constructs an Eigen-format SparseMatrix of a GaussianFactorGraph.
Definition SparseEigen.h:38
│ │ │ │ +
gtsam::HybridBayesNet
A hybrid Bayes net is a collection of HybridConditionals, which can have discrete conditionals,...
Definition HybridBayesNet.h:35
│ │ │ │ +
gtsam::HybridGaussianFactorGraph
Definition HybridGaussianFactorGraph.h:102
│ │ │ │ +
gtsam::HybridSmoother
Definition HybridSmoother.h:25
│ │ │ │ +
gtsam::HybridSmoother::gaussianMixture
GaussianMixture::shared_ptr gaussianMixture(size_t index) const
Get the Gaussian Mixture from the Bayes Net posterior at index.
Definition HybridSmoother.cpp:131
│ │ │ │ +
gtsam::HybridSmoother::update
void update(HybridGaussianFactorGraph graph, const Ordering &ordering, boost::optional< size_t > maxNrLeaves=boost::none)
Given new factors, perform an incremental update.
Definition HybridSmoother.cpp:58
│ │ │ │ +
gtsam::HybridSmoother::addConditionals
std::pair< HybridGaussianFactorGraph, HybridBayesNet > addConditionals(const HybridGaussianFactorGraph &graph, const HybridBayesNet &hybridBayesNet, const Ordering &ordering) const
Add conditionals from previous timestep as part of liquefication.
Definition HybridSmoother.cpp:85
│ │ │ │ +
gtsam::HybridSmoother::hybridBayesNet
const HybridBayesNet & hybridBayesNet() const
Return the Bayes Net posterior.
Definition HybridSmoother.cpp:137
│ │ │ │
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ -
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ -
gtsam::GaussianFactorGraph::sparseJacobian
std::vector< std::tuple< int, int, double > > sparseJacobian(const Ordering &ordering, size_t &nrows, size_t &ncols) const
Returns a sparse augmented Jacbian matrix as a vector of i, j, and s, where i(k) and j(k) are the bas...
Definition GaussianFactorGraph.cpp:119
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,89 +1,88 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SparseEigen.h │ │ │ │ │ +HybridSmoother.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -24#pragma once │ │ │ │ │ -25 │ │ │ │ │ -26#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -27#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ -28 │ │ │ │ │ -29#include │ │ │ │ │ -30 │ │ │ │ │ -31namespace _g_t_s_a_m { │ │ │ │ │ -32 │ │ │ │ │ -_3_5typedef Eigen::SparseMatrix _S_p_a_r_s_e_E_i_g_e_n; │ │ │ │ │ -36 │ │ │ │ │ -_3_8_S_p_a_r_s_e_E_i_g_e_n _s_p_a_r_s_e_J_a_c_o_b_i_a_n_E_i_g_e_n( │ │ │ │ │ -39 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, const _O_r_d_e_r_i_n_g &ordering) { │ │ │ │ │ -40 gttic_(SparseEigen_sparseJacobianEigen); │ │ │ │ │ -41 // intermediate `entries` vector is kind of unavoidable due to how expensive │ │ │ │ │ -42 // factor->rows() is, which prevents us from populating SparseEigen │ │ │ │ │ -directly. │ │ │ │ │ -43 size_t nrows, ncols; │ │ │ │ │ -44 auto entries = gfg._s_p_a_r_s_e_J_a_c_o_b_i_a_n(ordering, nrows, ncols); │ │ │ │ │ -45 // declare sparse matrix │ │ │ │ │ -46 _S_p_a_r_s_e_E_i_g_e_n Ab(nrows, ncols); │ │ │ │ │ -47 // See Eigen::set_from_triplets. This is about 5% faster. │ │ │ │ │ -48 // pass 1: count the nnz per inner-vector │ │ │ │ │ -49 std::vector nnz(ncols, 0); │ │ │ │ │ -50 for (const auto &entry : entries) nnz[std::get<1>(entry)]++; │ │ │ │ │ -51 Ab.reserve(nnz); │ │ │ │ │ -52 // pass 2: insert the elements │ │ │ │ │ -53 for (const auto &entry : entries) │ │ │ │ │ -54 Ab.insert(std::get<0>(entry), std::get<1>(entry)) = std::get<2>(entry); │ │ │ │ │ -55 return Ab; │ │ │ │ │ -56} │ │ │ │ │ -57 │ │ │ │ │ -58_S_p_a_r_s_e_E_i_g_e_n _s_p_a_r_s_e_J_a_c_o_b_i_a_n_E_i_g_e_n(const GaussianFactorGraph &gfg) { │ │ │ │ │ -59 gttic_(SparseEigen_sparseJacobianEigen_defaultOrdering); │ │ │ │ │ -60 return _s_p_a_r_s_e_J_a_c_o_b_i_a_n_E_i_g_e_n(gfg, Ordering(gfg.keys())); │ │ │ │ │ -61} │ │ │ │ │ -62 │ │ │ │ │ -63} // namespace gtsam │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ -Factor Graph Values. │ │ │ │ │ -_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ +19#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +20#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +22 │ │ │ │ │ +23namespace _g_t_s_a_m { │ │ │ │ │ +24 │ │ │ │ │ +_2_5class _H_y_b_r_i_d_S_m_o_o_t_h_e_r { │ │ │ │ │ +26 private: │ │ │ │ │ +27 _H_y_b_r_i_d_B_a_y_e_s_N_e_t hybridBayesNet_; │ │ │ │ │ +28 _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h remainingFactorGraph_; │ │ │ │ │ +29 │ │ │ │ │ +30 public: │ │ │ │ │ +50 void _u_p_d_a_t_e(_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h graph, const _O_r_d_e_r_i_n_g& ordering, │ │ │ │ │ +51 boost::optional maxNrLeaves = boost::none); │ │ │ │ │ +52 │ │ │ │ │ +53 _O_r_d_e_r_i_n_g getOrdering(const _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& newFactors); │ │ │ │ │ +54 │ │ │ │ │ +64 std::pair _a_d_d_C_o_n_d_i_t_i_o_n_a_l_s( │ │ │ │ │ +65 const _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& graph, │ │ │ │ │ +66 const _H_y_b_r_i_d_B_a_y_e_s_N_e_t& _h_y_b_r_i_d_B_a_y_e_s_N_e_t, const _O_r_d_e_r_i_n_g& ordering) const; │ │ │ │ │ +67 │ │ │ │ │ +69 GaussianMixture::shared_ptr _g_a_u_s_s_i_a_n_M_i_x_t_u_r_e(size_t index) const; │ │ │ │ │ +70 │ │ │ │ │ +72 const _H_y_b_r_i_d_B_a_y_e_s_N_e_t& _h_y_b_r_i_d_B_a_y_e_s_N_e_t() const; │ │ │ │ │ +73}; │ │ │ │ │ +74 │ │ │ │ │ +75}; // namespace gtsam │ │ │ │ │ +_D_i_s_c_r_e_t_e_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Linearized Hybrid factor graph that uses type erasure. │ │ │ │ │ +_H_y_b_r_i_d_B_a_y_e_s_N_e_t_._h │ │ │ │ │ +A Bayes net of Gaussian Conditionals indexed by discrete keys. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_S_p_a_r_s_e_E_i_g_e_n │ │ │ │ │ -Eigen::SparseMatrix< double, Eigen::ColMajor, int > SparseEigen │ │ │ │ │ -Eigen-format sparse matrix. │ │ │ │ │ -DDeeffiinniittiioonn SparseEigen.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_s_p_a_r_s_e_J_a_c_o_b_i_a_n_E_i_g_e_n │ │ │ │ │ -SparseEigen sparseJacobianEigen(const GaussianFactorGraph &gfg, const Ordering │ │ │ │ │ -&ordering) │ │ │ │ │ -Constructs an Eigen-format SparseMatrix of a GaussianFactorGraph. │ │ │ │ │ -DDeeffiinniittiioonn SparseEigen.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_B_a_y_e_s_N_e_t │ │ │ │ │ +A hybrid Bayes net is a collection of HybridConditionals, which can have │ │ │ │ │ +discrete conditionals,... │ │ │ │ │ +DDeeffiinniittiioonn HybridBayesNet.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +DDeeffiinniittiioonn HybridGaussianFactorGraph.h:102 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_S_m_o_o_t_h_e_r │ │ │ │ │ +DDeeffiinniittiioonn HybridSmoother.h:25 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_S_m_o_o_t_h_e_r_:_:_g_a_u_s_s_i_a_n_M_i_x_t_u_r_e │ │ │ │ │ +GaussianMixture::shared_ptr gaussianMixture(size_t index) const │ │ │ │ │ +Get the Gaussian Mixture from the Bayes Net posterior at index. │ │ │ │ │ +DDeeffiinniittiioonn HybridSmoother.cpp:131 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_S_m_o_o_t_h_e_r_:_:_u_p_d_a_t_e │ │ │ │ │ +void update(HybridGaussianFactorGraph graph, const Ordering &ordering, boost:: │ │ │ │ │ +optional< size_t > maxNrLeaves=boost::none) │ │ │ │ │ +Given new factors, perform an incremental update. │ │ │ │ │ +DDeeffiinniittiioonn HybridSmoother.cpp:58 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_S_m_o_o_t_h_e_r_:_:_a_d_d_C_o_n_d_i_t_i_o_n_a_l_s │ │ │ │ │ +std::pair< HybridGaussianFactorGraph, HybridBayesNet > addConditionals(const │ │ │ │ │ +HybridGaussianFactorGraph &graph, const HybridBayesNet &hybridBayesNet, const │ │ │ │ │ +Ordering &ordering) const │ │ │ │ │ +Add conditionals from previous timestep as part of liquefication. │ │ │ │ │ +DDeeffiinniittiioonn HybridSmoother.cpp:85 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_S_m_o_o_t_h_e_r_:_:_h_y_b_r_i_d_B_a_y_e_s_N_e_t │ │ │ │ │ +const HybridBayesNet & hybridBayesNet() const │ │ │ │ │ +Return the Bayes Net posterior. │ │ │ │ │ +DDeeffiinniittiioonn HybridSmoother.cpp:137 │ │ │ │ │ _g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_p_a_r_s_e_J_a_c_o_b_i_a_n │ │ │ │ │ -std::vector< std::tuple< int, int, double > > sparseJacobian(const Ordering │ │ │ │ │ -&ordering, size_t &nrows, size_t &ncols) const │ │ │ │ │ -Returns a sparse augmented Jacbian matrix as a vector of i, j, and s, where i │ │ │ │ │ -(k) and j(k) are the bas... │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.cpp:119 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _l_i_n_e_a_r │ │ │ │ │ - * _S_p_a_r_s_e_E_i_g_e_n_._h │ │ │ │ │ + * _h_y_b_r_i_d │ │ │ │ │ + * _H_y_b_r_i_d_S_m_o_o_t_h_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00779.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00671.html │ │ │ │┄ Files 94% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/IterativeSolver.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/EliminationTree.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
IterativeSolver.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
EliminationTree.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Some support classes for iterative solvers. │ │ │ │ -More...

│ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::EliminationTree< BAYESNET, GRAPH >
 An elimination tree is a data structure used intermediately during elimination. More...
 
struct  gtsam::EliminationTree< BAYESNET, GRAPH >::Node
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -ostream & gtsam::operator<< (ostream &os, const IterativeOptimizationParameters &p)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Some support classes for iterative solvers.

│ │ │ │ -
Date
Sep 3, 2012
│ │ │ │ -
Author
Yong-Dian Jian
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │ +
Date
Oct 13, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,26 +1,29 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -IterativeSolver.cpp File Reference │ │ │ │ │ -Some support classes for iterative solvers. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +EliminationTree.h File Reference │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_ _B_A_Y_E_S_N_E_T_,_ _G_R_A_P_H_ _> │ │ │ │ │ +  An elimination tree is a data structure used intermediately during │ │ │ │ │ + elimination. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_<_ _B_A_Y_E_S_N_E_T_,_ _G_R_A_P_H_ _>_:_:_N_o_d_e │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -ostream &  ggttssaamm::::ooppeerraattoorr<<<< (ostream &os, const │ │ │ │ │ - _I_t_e_r_a_t_i_v_e_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s &p) │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Some support classes for iterative solvers. │ │ │ │ │ - Date │ │ │ │ │ - Sep 3, 2012 │ │ │ │ │ Author │ │ │ │ │ - Yong-Dian Jian │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Oct 13, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _l_i_n_e_a_r │ │ │ │ │ - * _I_t_e_r_a_t_i_v_e_S_o_l_v_e_r_._c_p_p │ │ │ │ │ + * _i_n_f_e_r_e_n_c_e │ │ │ │ │ + * _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00821_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00719_source.html │ │ │ │┄ Files 99% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
linearAlgorithms-inst.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
18#pragma once
│ │ │ │
19
│ │ │ │ -
20#include <gtsam/linear/VectorValues.h>
│ │ │ │ -
21#include <gtsam/linear/GaussianConditional.h>
│ │ │ │ -
22#include <gtsam/base/treeTraversal-inst.h>
│ │ │ │ +
20#include <gtsam/linear/VectorValues.h>
│ │ │ │ +
21#include <gtsam/linear/GaussianConditional.h>
│ │ │ │ +
22#include <gtsam/base/treeTraversal-inst.h>
│ │ │ │
23
│ │ │ │
24#include <boost/optional.hpp>
│ │ │ │
25#include <boost/shared_ptr.hpp>
│ │ │ │
26
│ │ │ │
27namespace gtsam
│ │ │ │
28{
│ │ │ │
29 namespace internal
│ │ │ │ @@ -250,17 +250,17 @@ │ │ │ │
151 TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP
│ │ │ │
152 treeTraversal::DepthFirstForestParallel(bayesTree, rootData, preVisitor, postVisitor);
│ │ │ │
153 return preVisitor.collectedResult;
│ │ │ │
154 }
│ │ │ │
155 }
│ │ │ │
156 }
│ │ │ │
157}
│ │ │ │ - │ │ │ │ -
VectorValues.h
Factor Graph Values.
│ │ │ │ -
GaussianConditional.h
Conditional Gaussian Base class.
│ │ │ │ + │ │ │ │ +
GaussianConditional.h
Conditional Gaussian Base class.
│ │ │ │ +
VectorValues.h
Factor Graph Values.
│ │ │ │
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::treeTraversal::DepthFirstForestParallel
void DepthFirstForestParallel(FOREST &forest, DATA &rootData, VISITOR_PRE &visitorPre, VISITOR_POST &visitorPost, int problemSizeThreshold=10)
Traverse a forest depth-first with pre-order and post-order visits.
Definition treeTraversal-inst.h:154
│ │ │ │
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │
gtsam::TbbOpenMPMixedScope
An object whose scope defines a block where TBB and OpenMP parallelism are mixed.
Definition types.h:192
│ │ │ │ @@ -281,13 +281,13 @@ │ │ │ │
gtsam::VectorValues::const_iterator
Values::const_iterator const_iterator
Const iterator over vector values.
Definition VectorValues.h:82
│ │ │ │
gtsam::VectorValues::emplace
std::pair< VectorValues::iterator, bool > emplace(Key j, Args &&... args)
Emplace a vector value with key j.
Definition VectorValues.h:185
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -168,18 +168,18 @@ │ │ │ │ │ postVisitor); │ │ │ │ │ 153 return preVisitor.collectedResult; │ │ │ │ │ 154 } │ │ │ │ │ 155 } │ │ │ │ │ 156 } │ │ │ │ │ 157} │ │ │ │ │ _t_r_e_e_T_r_a_v_e_r_s_a_l_-_i_n_s_t_._h │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ -Factor Graph Values. │ │ │ │ │ _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ Conditional Gaussian Base class. │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ +Factor Graph Values. │ │ │ │ │ _g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ FastVector │ │ │ │ │ FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00830.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00884.html │ │ │ │┄ Files 98% similar despite different names │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
VectorValues.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │

Factor Graph Values. │ │ │ │ More...

│ │ │ │ │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -128,13 +128,13 @@ │ │ │ │

Factor Graph Values.

│ │ │ │
Author
Richard Roberts
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00830_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00884_source.html │ │ │ │┄ Files 99% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
VectorValues.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
18#pragma once
│ │ │ │
19
│ │ │ │ -
20#include <gtsam/linear/Scatter.h>
│ │ │ │ -
21#include <gtsam/inference/Ordering.h>
│ │ │ │ -
22#include <gtsam/base/Vector.h>
│ │ │ │ +
20#include <gtsam/linear/Scatter.h>
│ │ │ │ +
21#include <gtsam/inference/Ordering.h>
│ │ │ │ +
22#include <gtsam/base/Vector.h>
│ │ │ │
23#include <gtsam/base/ConcurrentMap.h>
│ │ │ │ -
24#include <gtsam/base/FastVector.h>
│ │ │ │ -
25#include <gtsam/global_includes.h>
│ │ │ │ +
24#include <gtsam/base/FastVector.h>
│ │ │ │ +
25#include <gtsam/global_includes.h>
│ │ │ │
26
│ │ │ │
27#include <boost/shared_ptr.hpp>
│ │ │ │
28
│ │ │ │
29
│ │ │ │
30#include <map>
│ │ │ │
31#include <string>
│ │ │ │
32#include <iosfwd>
│ │ │ │ @@ -359,19 +359,19 @@ │ │ │ │
│ │ │ │
383
│ │ │ │
384} // \namespace gtsam
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ -
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ -
Ordering.h
Variable ordering for the elimination algorithm.
│ │ │ │ -
Scatter.h
Maps global variable indices to slot indices.
│ │ │ │ -
global_includes.h
Included from all GTSAM files.
│ │ │ │ +
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ +
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ +
Ordering.h
Variable ordering for the elimination algorithm.
│ │ │ │ +
Scatter.h
Maps global variable indices to slot indices.
│ │ │ │ +
global_includes.h
Included from all GTSAM files.
│ │ │ │
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::html
string html(const DiscreteValues &values, const KeyFormatter &keyFormatter, const DiscreteValues::Names &names)
Free version of html.
Definition DiscreteValues.cpp:134
│ │ │ │
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │
gtsam::operator+
Errors operator+(const Errors &a, const Errors &b)
Addition.
Definition Errors.cpp:60
│ │ │ │
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
│ │ │ │ @@ -419,13 +419,13 @@ │ │ │ │
gtsam::VectorValues::begin
iterator begin()
Iterator over variables.
Definition VectorValues.h:236
│ │ │ │
gtsam::Values::KeyValuePair
A key-value pair, which you get by dereferencing iterators.
Definition Values.h:93
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00833.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01322.html │ │ │ │┄ Files 94% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianConditional.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/SmartProjectionPoseFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
GaussianConditional.h File Reference
│ │ │ │ +
SmartProjectionPoseFactor.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Conditional Gaussian Base class. │ │ │ │ +

Smart factor on poses, assuming camera calibration is fixed. │ │ │ │ More...

│ │ │ │ │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Go to the source code of this file.

│ │ │ │

│ │ │ │ Classes

class  gtsam::VectorValues
 VectorValues represents a collection of vector-valued variables associated each with a unique integer index. More...
 
struct  gtsam::traits< VectorValues >
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::GaussianConditional
 A GaussianConditional functions as the node in a Bayes network. More...
class  gtsam::SmartProjectionPoseFactor< CALIBRATION >
 If you are using the factor, please cite: L. More...
 
struct  gtsam::traits< GaussianConditional >
 traits More...
struct  gtsam::traits< SmartProjectionPoseFactor< CALIBRATION > >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Conditional Gaussian Base class.

│ │ │ │ -
Author
Christian Potthast
│ │ │ │ +

Smart factor on poses, assuming camera calibration is fixed.

│ │ │ │ +
Author
Luca Carlone
│ │ │ │ +
│ │ │ │ +Chris Beall
│ │ │ │ +
│ │ │ │ +Zsolt Kira
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,29 +1,30 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -GaussianConditional.h File Reference │ │ │ │ │ -Conditional Gaussian Base class. _M_o_r_e_._._. │ │ │ │ │ +SmartProjectionPoseFactor.h File Reference │ │ │ │ │ +Smart factor on poses, assuming camera calibration is fixed. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -  A _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l functions as the node in a Bayes network. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_<_ _C_A_L_I_B_R_A_T_I_O_N_ _> │ │ │ │ │ +  If you are using the factor, please cite: L. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_ _> │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_<_ _C_A_L_I_B_R_A_T_I_O_N_ _>_ _> │ │ │ │ │   traits _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Conditional Gaussian Base class. │ │ │ │ │ +Smart factor on poses, assuming camera calibration is fixed. │ │ │ │ │ Author │ │ │ │ │ - Christian Potthast │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Chris Beall │ │ │ │ │ + Zsolt Kira │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _l_i_n_e_a_r │ │ │ │ │ - * _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ + * _s_l_a_m │ │ │ │ │ + * _S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_o_s_e_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00842_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00707_source.html │ │ │ │┄ Files 96% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/SubgraphBuilder.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Preconditioner.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
SubgraphBuilder.h
│ │ │ │ +
Preconditioner.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ -
2
│ │ │ │ -
3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
│ │ │ │ -
4 * Atlanta, Georgia 30332-0415
│ │ │ │ -
5 * All Rights Reserved
│ │ │ │ -
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ -
7
│ │ │ │ -
8 * See LICENSE for the license information
│ │ │ │ -
9
│ │ │ │ -
10 * -------------------------------------------------------------------------- */
│ │ │ │ -
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/base/FastMap.h>
│ │ │ │ -
21#include <gtsam/base/types.h>
│ │ │ │ -
22#include <gtsam/dllexport.h>
│ │ │ │ -
23
│ │ │ │ -
24#include <boost/serialization/version.hpp>
│ │ │ │ -
25#include <boost/serialization/nvp.hpp>
│ │ │ │ -
26#include <boost/shared_ptr.hpp>
│ │ │ │ +
1/*
│ │ │ │ +
2 * Preconditioner.h
│ │ │ │ +
3 *
│ │ │ │ +
4 * Created on: Jun 2, 2014
│ │ │ │ +
5 * Author: Yong-Dian Jian
│ │ │ │ +
6 * Author: Sungtae An
│ │ │ │ +
7 */
│ │ │ │ +
8
│ │ │ │ +
9#pragma once
│ │ │ │ +
10
│ │ │ │ +
11#include <gtsam/base/Vector.h>
│ │ │ │ +
12#include <boost/shared_ptr.hpp>
│ │ │ │ +
13#include <iosfwd>
│ │ │ │ +
14#include <map>
│ │ │ │ +
15#include <string>
│ │ │ │ +
16
│ │ │ │ +
17namespace gtsam {
│ │ │ │ +
18
│ │ │ │ +
19class GaussianFactorGraph;
│ │ │ │ +
20class KeyInfo;
│ │ │ │ +
21class VectorValues;
│ │ │ │ +
22
│ │ │ │ +
23/* parameters for the preconditioner */
│ │ │ │ +
│ │ │ │ +
24struct GTSAM_EXPORT PreconditionerParameters {
│ │ │ │ +
25
│ │ │ │ +
26 typedef boost::shared_ptr<PreconditionerParameters> shared_ptr;
│ │ │ │
27
│ │ │ │ -
28#include <vector>
│ │ │ │ -
29
│ │ │ │ -
30namespace boost {
│ │ │ │ -
31namespace serialization {
│ │ │ │ -
32class access;
│ │ │ │ -
33} /* namespace serialization */
│ │ │ │ -
34} /* namespace boost */
│ │ │ │ -
35
│ │ │ │ -
36namespace gtsam {
│ │ │ │ -
37
│ │ │ │ -
38// Forward declarations
│ │ │ │ -
39class GaussianFactorGraph;
│ │ │ │ -
40struct PreconditionerParameters;
│ │ │ │ -
41
│ │ │ │ -
42/**************************************************************************/
│ │ │ │ -
│ │ │ │ -
43class GTSAM_EXPORT Subgraph {
│ │ │ │ -
44 public:
│ │ │ │ -
│ │ │ │ -
45 struct GTSAM_EXPORT Edge {
│ │ │ │ -
46 size_t index; /* edge id */
│ │ │ │ -
47 double weight; /* edge weight */
│ │ │ │ -
48 inline bool isUnitWeight() const { return (weight == 1.0); }
│ │ │ │ -
49 friend std::ostream &operator<<(std::ostream &os, const Edge &edge);
│ │ │ │ +
28 enum Kernel { /* Preconditioner Kernel */
│ │ │ │ +
29 GTSAM = 0,
│ │ │ │ +
30 CHOLMOD /* experimental */
│ │ │ │ +
31 } kernel_ ;
│ │ │ │ +
32
│ │ │ │ +
33 enum Verbosity {
│ │ │ │ +
34 SILENT = 0,
│ │ │ │ +
35 COMPLEXITY = 1,
│ │ │ │ +
36 ERROR = 2
│ │ │ │ +
37 } verbosity_ ;
│ │ │ │ +
38
│ │ │ │ +
39 PreconditionerParameters(): kernel_(GTSAM), verbosity_(SILENT) {}
│ │ │ │ +
40 PreconditionerParameters(const PreconditionerParameters &p) : kernel_(p.kernel_), verbosity_(p.verbosity_) {}
│ │ │ │ +
41 virtual ~PreconditionerParameters() {}
│ │ │ │ +
42
│ │ │ │ +
43 /* general interface */
│ │ │ │ +
44 inline Kernel kernel() const { return kernel_; }
│ │ │ │ +
45 inline Verbosity verbosity() const { return verbosity_; }
│ │ │ │ +
46
│ │ │ │ +
47 void print() const;
│ │ │ │ +
48
│ │ │ │ +
49 virtual void print(std::ostream &os) const;
│ │ │ │
50
│ │ │ │ -
51 private:
│ │ │ │ -
52 friend class boost::serialization::access;
│ │ │ │ -
53 template <class Archive>
│ │ │ │ -
54 void serialize(Archive &ar, const unsigned int /*version*/) {
│ │ │ │ -
55 ar &BOOST_SERIALIZATION_NVP(index);
│ │ │ │ -
56 ar &BOOST_SERIALIZATION_NVP(weight);
│ │ │ │ -
57 }
│ │ │ │ -
58 };
│ │ │ │ +
51 static Kernel kernelTranslator(const std::string &s);
│ │ │ │ +
52 static Verbosity verbosityTranslator(const std::string &s);
│ │ │ │ +
53 static std::string kernelTranslator(Kernel k);
│ │ │ │ +
54 static std::string verbosityTranslator(Verbosity v);
│ │ │ │ +
55
│ │ │ │ +
56 /* for serialization */
│ │ │ │ +
57 friend std::ostream& operator<<(std::ostream &os, const PreconditionerParameters &p);
│ │ │ │ +
58 };
│ │ │ │
│ │ │ │
59
│ │ │ │ -
60 typedef std::vector<Edge> Edges;
│ │ │ │ -
61 typedef std::vector<size_t> EdgeIndices;
│ │ │ │ -
62 typedef Edges::iterator iterator;
│ │ │ │ -
63 typedef Edges::const_iterator const_iterator;
│ │ │ │ -
64
│ │ │ │ -
65 protected:
│ │ │ │ -
66 Edges edges_; /* index to the factors */
│ │ │ │ -
67
│ │ │ │ -
68 public:
│ │ │ │ -
69 Subgraph() {}
│ │ │ │ -
70 Subgraph(const Subgraph &subgraph) : edges_(subgraph.edges()) {}
│ │ │ │ -
71 Subgraph(const Edges &edges) : edges_(edges) {}
│ │ │ │ -
72 Subgraph(const std::vector<size_t> &indices);
│ │ │ │ -
73
│ │ │ │ -
74 inline const Edges &edges() const { return edges_; }
│ │ │ │ -
75 inline size_t size() const { return edges_.size(); }
│ │ │ │ -
76 EdgeIndices edgeIndices() const;
│ │ │ │ -
77
│ │ │ │ -
78 iterator begin() { return edges_.begin(); }
│ │ │ │ -
79 const_iterator begin() const { return edges_.begin(); }
│ │ │ │ -
80 iterator end() { return edges_.end(); }
│ │ │ │ -
81 const_iterator end() const { return edges_.end(); }
│ │ │ │ +
60/* PCG aims to solve the problem: A x = b by reparametrizing it as
│ │ │ │ +
61 * L^{-1} A L^{-T} y = L^{-1} b or M^{-1} A x = M^{-1} b,
│ │ │ │ +
62 * where A \approx L L^{T}, or A \approx M
│ │ │ │ +
63 * The goal of this class is to provide a general interface to all preconditioners */
│ │ │ │ +
│ │ │ │ +
64class GTSAM_EXPORT Preconditioner {
│ │ │ │ +
65public:
│ │ │ │ +
66 typedef boost::shared_ptr<Preconditioner> shared_ptr;
│ │ │ │ +
67 typedef std::vector<size_t> Dimensions;
│ │ │ │ +
68
│ │ │ │ +
69 /* Generic Constructor and Destructor */
│ │ │ │ +
70 Preconditioner() {}
│ │ │ │ +
71 virtual ~Preconditioner() {}
│ │ │ │ +
72
│ │ │ │ +
73 /*
│ │ │ │ +
74 * Abstract interface for raw vectors. VectorValues is a speed bottleneck
│ │ │ │ +
75 * and Yong-Dian has profiled preconditioners (outside GTSAM) with the the
│ │ │ │ +
76 * three methods below. In GTSAM, unfortunately, we are still using the
│ │ │ │ +
77 * VectorValues methods called in iterative-inl.h
│ │ │ │ +
78 */
│ │ │ │ +
79
│ │ │ │ +
81 virtual void solve(const Vector& y, Vector &x) const = 0;
│ │ │ │
82
│ │ │ │ -
83 void save(const std::string &fn) const;
│ │ │ │ -
84 static Subgraph load(const std::string &fn);
│ │ │ │ -
85 friend std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph);
│ │ │ │ -
86
│ │ │ │ -
87 private:
│ │ │ │ -
88 friend class boost::serialization::access;
│ │ │ │ -
89 template <class Archive>
│ │ │ │ -
90 void serialize(Archive &ar, const unsigned int /*version*/) {
│ │ │ │ -
91 ar &BOOST_SERIALIZATION_NVP(edges_);
│ │ │ │ -
92 }
│ │ │ │ -
93};
│ │ │ │ -
│ │ │ │ -
94
│ │ │ │ -
95/****************************************************************************/
│ │ │ │ -
│ │ │ │ -
96struct GTSAM_EXPORT SubgraphBuilderParameters {
│ │ │ │ -
97 typedef boost::shared_ptr<SubgraphBuilderParameters> shared_ptr;
│ │ │ │ -
98
│ │ │ │ -
99 enum Skeleton {
│ │ │ │ -
100 /* augmented tree */
│ │ │ │ -
101 NATURALCHAIN = 0, /* natural ordering of the graph */
│ │ │ │ -
102 BFS, /* breadth-first search tree */
│ │ │ │ -
103 KRUSKAL, /* maximum weighted spanning tree */
│ │ │ │ -
104 } skeletonType;
│ │ │ │ -
105
│ │ │ │ -
106 enum SkeletonWeight { /* how to weigh the graph edges */
│ │ │ │ -
107 EQUAL = 0, /* every block edge has equal weight */
│ │ │ │ -
108 RHS_2NORM, /* use the 2-norm of the rhs */
│ │ │ │ -
109 LHS_FNORM, /* use the frobenius norm of the lhs */
│ │ │ │ -
110 RANDOM, /* bounded random edge weight */
│ │ │ │ -
111 } skeletonWeight;
│ │ │ │ +
84 virtual void transposeSolve(const Vector& y, Vector& x) const = 0;
│ │ │ │ +
85
│ │ │ │ +
87 virtual void build(
│ │ │ │ +
88 const GaussianFactorGraph &gfg,
│ │ │ │ +
89 const KeyInfo &info,
│ │ │ │ +
90 const std::map<Key,Vector> &lambda
│ │ │ │ +
91 ) = 0;
│ │ │ │ +
92};
│ │ │ │ +
│ │ │ │ +
93
│ │ │ │ +
94/*******************************************************************************************/
│ │ │ │ +
│ │ │ │ +
95struct GTSAM_EXPORT DummyPreconditionerParameters : public PreconditionerParameters {
│ │ │ │ +
96 typedef PreconditionerParameters Base;
│ │ │ │ +
97 typedef boost::shared_ptr<DummyPreconditionerParameters> shared_ptr;
│ │ │ │ +
98 DummyPreconditionerParameters() : Base() {}
│ │ │ │ +
99 ~DummyPreconditionerParameters() override {}
│ │ │ │ +
100};
│ │ │ │ +
│ │ │ │ +
101
│ │ │ │ +
102/*******************************************************************************************/
│ │ │ │ +
│ │ │ │ +
103class GTSAM_EXPORT DummyPreconditioner : public Preconditioner {
│ │ │ │ +
104public:
│ │ │ │ +
105 typedef Preconditioner Base;
│ │ │ │ +
106 typedef boost::shared_ptr<DummyPreconditioner> shared_ptr;
│ │ │ │ +
107
│ │ │ │ +
108public:
│ │ │ │ +
109
│ │ │ │ +
110 DummyPreconditioner() : Base() {}
│ │ │ │ +
111 ~DummyPreconditioner() override {}
│ │ │ │
112
│ │ │ │ -
113 enum AugmentationWeight { /* how to weigh the graph edges */
│ │ │ │ -
114 SKELETON = 0, /* use the same weights in building
│ │ │ │ -
115 the skeleton */
│ │ │ │ -
116 // STRETCH, /* stretch in the
│ │ │ │ -
117 // laplacian sense */ GENERALIZED_STRETCH /*
│ │ │ │ -
118 // the generalized stretch defined in
│ │ │ │ -
119 // jian2013iros */
│ │ │ │ -
120 } augmentationWeight;
│ │ │ │ -
121
│ │ │ │ -
123 double augmentationFactor;
│ │ │ │ -
124
│ │ │ │ -
125 SubgraphBuilderParameters()
│ │ │ │ -
126 : skeletonType(KRUSKAL),
│ │ │ │ -
127 skeletonWeight(RANDOM),
│ │ │ │ -
128 augmentationWeight(SKELETON),
│ │ │ │ -
129 augmentationFactor(1.0) {}
│ │ │ │ -
130 virtual ~SubgraphBuilderParameters() {}
│ │ │ │ -
131
│ │ │ │ -
132 /* for serialization */
│ │ │ │ -
133 void print() const;
│ │ │ │ -
134 virtual void print(std::ostream &os) const;
│ │ │ │ -
135 friend std::ostream &operator<<(std::ostream &os,
│ │ │ │ -
136 const PreconditionerParameters &p);
│ │ │ │ -
137
│ │ │ │ -
138 static Skeleton skeletonTranslator(const std::string &s);
│ │ │ │ -
139 static std::string skeletonTranslator(Skeleton s);
│ │ │ │ -
140 static SkeletonWeight skeletonWeightTranslator(const std::string &s);
│ │ │ │ -
141 static std::string skeletonWeightTranslator(SkeletonWeight w);
│ │ │ │ -
142 static AugmentationWeight augmentationWeightTranslator(const std::string &s);
│ │ │ │ -
143 static std::string augmentationWeightTranslator(AugmentationWeight w);
│ │ │ │ -
144};
│ │ │ │ -
│ │ │ │ +
113 /* Computation Interfaces for raw vector */
│ │ │ │ +
114 void solve(const Vector& y, Vector &x) const override { x = y; }
│ │ │ │ +
115 void transposeSolve(const Vector& y, Vector& x) const override { x = y; }
│ │ │ │ +
│ │ │ │ +
116 void build(
│ │ │ │ +
117 const GaussianFactorGraph &gfg,
│ │ │ │ +
118 const KeyInfo &info,
│ │ │ │ +
119 const std::map<Key,Vector> &lambda
│ │ │ │ +
120 ) override {}
│ │ │ │ +
│ │ │ │ +
121};
│ │ │ │ +
│ │ │ │ +
122
│ │ │ │ +
123/*******************************************************************************************/
│ │ │ │ +
│ │ │ │ +
124struct GTSAM_EXPORT BlockJacobiPreconditionerParameters : public PreconditionerParameters {
│ │ │ │ +
125 typedef PreconditionerParameters Base;
│ │ │ │ +
126 BlockJacobiPreconditionerParameters() : Base() {}
│ │ │ │ +
127 ~BlockJacobiPreconditionerParameters() override {}
│ │ │ │ +
128};
│ │ │ │ +
│ │ │ │ +
129
│ │ │ │ +
130/*******************************************************************************************/
│ │ │ │ +
│ │ │ │ +
131class GTSAM_EXPORT BlockJacobiPreconditioner : public Preconditioner {
│ │ │ │ +
132public:
│ │ │ │ +
133 typedef Preconditioner Base;
│ │ │ │ +
134 BlockJacobiPreconditioner() ;
│ │ │ │ +
135 ~BlockJacobiPreconditioner() override ;
│ │ │ │ +
136
│ │ │ │ +
137 /* Computation Interfaces for raw vector */
│ │ │ │ +
138 void solve(const Vector& y, Vector &x) const override;
│ │ │ │ +
139 void transposeSolve(const Vector& y, Vector& x) const override;
│ │ │ │ +
140 void build(
│ │ │ │ +
141 const GaussianFactorGraph &gfg,
│ │ │ │ +
142 const KeyInfo &info,
│ │ │ │ +
143 const std::map<Key,Vector> &lambda
│ │ │ │ +
144 ) override;
│ │ │ │
145
│ │ │ │ -
146/*****************************************************************************/
│ │ │ │ -
│ │ │ │ -
147class GTSAM_EXPORT SubgraphBuilder {
│ │ │ │ -
148 public:
│ │ │ │ -
149 typedef SubgraphBuilder Base;
│ │ │ │ -
150 typedef std::vector<double> Weights;
│ │ │ │ -
151
│ │ │ │ -
152 SubgraphBuilder(
│ │ │ │ -
153 const SubgraphBuilderParameters &p = SubgraphBuilderParameters())
│ │ │ │ -
154 : parameters_(p) {}
│ │ │ │ -
155 virtual ~SubgraphBuilder() {}
│ │ │ │ -
156 virtual Subgraph operator()(const GaussianFactorGraph &jfg) const;
│ │ │ │ -
157
│ │ │ │ -
158 private:
│ │ │ │ -
159 std::vector<size_t> buildTree(const GaussianFactorGraph &gfg,
│ │ │ │ -
160 const FastMap<Key, size_t> &ordering,
│ │ │ │ -
161 const std::vector<double> &weights) const;
│ │ │ │ -
162 std::vector<size_t> unary(const GaussianFactorGraph &gfg) const;
│ │ │ │ -
163 std::vector<size_t> natural_chain(const GaussianFactorGraph &gfg) const;
│ │ │ │ -
164 std::vector<size_t> bfs(const GaussianFactorGraph &gfg) const;
│ │ │ │ -
165 std::vector<size_t> kruskal(const GaussianFactorGraph &gfg,
│ │ │ │ -
166 const FastMap<Key, size_t> &ordering,
│ │ │ │ -
167 const std::vector<double> &weights) const;
│ │ │ │ -
168 std::vector<size_t> sample(const std::vector<double> &weights,
│ │ │ │ -
169 const size_t t) const;
│ │ │ │ -
170 Weights weights(const GaussianFactorGraph &gfg) const;
│ │ │ │ -
171 SubgraphBuilderParameters parameters_;
│ │ │ │ -
172};
│ │ │ │ -
│ │ │ │ -
173
│ │ │ │ -
175GaussianFactorGraph buildFactorSubgraph(const GaussianFactorGraph &gfg,
│ │ │ │ -
176 const Subgraph &subgraph,
│ │ │ │ -
177 const bool clone);
│ │ │ │ -
178
│ │ │ │ -
181std::pair<GaussianFactorGraph, GaussianFactorGraph> splitFactorGraph(
│ │ │ │ -
182 const GaussianFactorGraph &factorGraph, const Subgraph &subgraph);
│ │ │ │ -
183
│ │ │ │ -
184} // namespace gtsam
│ │ │ │ -
FastMap.h
A thin wrapper around std::map that uses boost's fast_pool_allocator.
│ │ │ │ -
types.h
Typedefs for easier changing of types.
│ │ │ │ +
146protected:
│ │ │ │ +
147
│ │ │ │ +
148 void clean() ;
│ │ │ │ +
149
│ │ │ │ +
150 std::vector<size_t> dims_;
│ │ │ │ +
151 double *buffer_;
│ │ │ │ +
152 size_t bufferSize_;
│ │ │ │ +
153 size_t nnz_;
│ │ │ │ +
154};
│ │ │ │ +
│ │ │ │ +
155
│ │ │ │ +
156/*********************************************************************************************/
│ │ │ │ +
157/* factory method to create preconditioners */
│ │ │ │ +
158boost::shared_ptr<Preconditioner> createPreconditioner(const boost::shared_ptr<PreconditionerParameters> parameters);
│ │ │ │ +
159
│ │ │ │ +
160}
│ │ │ │ +
161
│ │ │ │ +
162
│ │ │ │ +
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::buildFactorSubgraph
GaussianFactorGraph buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone)
Select the factors in a factor graph according to the subgraph.
Definition SubgraphBuilder.cpp:447
│ │ │ │ -
gtsam::splitFactorGraph
std::pair< GaussianFactorGraph, GaussianFactorGraph > splitFactorGraph(const GaussianFactorGraph &factorGraph, const Subgraph &subgraph)
Split the graph into a subgraph and the remaining edges.
Definition SubgraphBuilder.cpp:460
│ │ │ │ -
gtsam::save
void save(const Matrix &A, const string &s, const string &filename)
save a matrix to file, which can be loaded by matlab
Definition Matrix.cpp:167
│ │ │ │ -
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ -
gtsam::Subgraph
Definition SubgraphBuilder.h:43
│ │ │ │ -
gtsam::Subgraph::Edge
Definition SubgraphBuilder.h:45
│ │ │ │ -
gtsam::SubgraphBuilderParameters
Definition SubgraphBuilder.h:96
│ │ │ │ -
gtsam::SubgraphBuilderParameters::augmentationFactor
double augmentationFactor
factor multiplied with n, yields number of extra edges.
Definition SubgraphBuilder.h:123
│ │ │ │ -
gtsam::SubgraphBuilder
Definition SubgraphBuilder.h:147
│ │ │ │ +
gtsam::KeyInfo
Handy data structure for iterative solvers.
Definition IterativeSolver.h:126
│ │ │ │ +
gtsam::PreconditionerParameters
Definition Preconditioner.h:24
│ │ │ │ +
gtsam::Preconditioner
Definition Preconditioner.h:64
│ │ │ │ +
gtsam::Preconditioner::solve
virtual void solve(const Vector &y, Vector &x) const =0
implement x = L^{-1} y
│ │ │ │ +
gtsam::Preconditioner::build
virtual void build(const GaussianFactorGraph &gfg, const KeyInfo &info, const std::map< Key, Vector > &lambda)=0
build/factorize the preconditioner
│ │ │ │ +
gtsam::Preconditioner::transposeSolve
virtual void transposeSolve(const Vector &y, Vector &x) const =0
implement x = L^{-T} y
│ │ │ │ +
gtsam::DummyPreconditionerParameters
Definition Preconditioner.h:95
│ │ │ │ +
gtsam::DummyPreconditioner
Definition Preconditioner.h:103
│ │ │ │ +
gtsam::DummyPreconditioner::build
void build(const GaussianFactorGraph &gfg, const KeyInfo &info, const std::map< Key, Vector > &lambda) override
build/factorize the preconditioner
Definition Preconditioner.h:116
│ │ │ │ +
gtsam::DummyPreconditioner::solve
void solve(const Vector &y, Vector &x) const override
implement x = L^{-1} y
Definition Preconditioner.h:114
│ │ │ │ +
gtsam::DummyPreconditioner::transposeSolve
void transposeSolve(const Vector &y, Vector &x) const override
implement x = L^{-T} y
Definition Preconditioner.h:115
│ │ │ │ +
gtsam::BlockJacobiPreconditionerParameters
Definition Preconditioner.h:124
│ │ │ │ +
gtsam::BlockJacobiPreconditioner
Definition Preconditioner.h:131
│ │ │ │ +
VectorValues
The Factor::error simply extracts the.
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,230 +1,232 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SubgraphBuilder.h │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -1/* --------------------------------------------------------------------------- │ │ │ │ │ -- │ │ │ │ │ -2 │ │ │ │ │ -3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, │ │ │ │ │ -4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -5 * All Rights Reserved │ │ │ │ │ -6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ -7 │ │ │ │ │ -8 * See LICENSE for the license information │ │ │ │ │ -9 │ │ │ │ │ -10 * ------------------------------------------------------------------------- │ │ │ │ │ -- */ │ │ │ │ │ -11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_M_a_p_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ -22#include │ │ │ │ │ -23 │ │ │ │ │ -24#include │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ +Preconditioner.h │ │ │ │ │ +1/* │ │ │ │ │ +2 * Preconditioner.h │ │ │ │ │ +3 * │ │ │ │ │ +4 * Created on: Jun 2, 2014 │ │ │ │ │ +5 * Author: Yong-Dian Jian │ │ │ │ │ +6 * Author: Sungtae An │ │ │ │ │ +7 */ │ │ │ │ │ +8 │ │ │ │ │ +9#pragma once │ │ │ │ │ +10 │ │ │ │ │ +11#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_c_t_o_r_._h> │ │ │ │ │ +12#include │ │ │ │ │ +13#include │ │ │ │ │ +14#include │ │ │ │ │ +15#include │ │ │ │ │ +16 │ │ │ │ │ +17namespace _g_t_s_a_m { │ │ │ │ │ +18 │ │ │ │ │ +19class GaussianFactorGraph; │ │ │ │ │ +20class KeyInfo; │ │ │ │ │ +21class _V_e_c_t_o_r_V_a_l_u_e_s; │ │ │ │ │ +22 │ │ │ │ │ +23/* parameters for the preconditioner */ │ │ │ │ │ +_2_4struct GTSAM_EXPORT _P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s { │ │ │ │ │ +25 │ │ │ │ │ +26 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ 27 │ │ │ │ │ -28#include │ │ │ │ │ -29 │ │ │ │ │ -30namespace boost { │ │ │ │ │ -31namespace serialization { │ │ │ │ │ -32class access; │ │ │ │ │ -33} /* namespace serialization */ │ │ │ │ │ -34} /* namespace boost */ │ │ │ │ │ -35 │ │ │ │ │ -36namespace _g_t_s_a_m { │ │ │ │ │ -37 │ │ │ │ │ -38// Forward declarations │ │ │ │ │ -39class GaussianFactorGraph; │ │ │ │ │ -40struct PreconditionerParameters; │ │ │ │ │ -41 │ │ │ │ │ -42/**************************************************************************/ │ │ │ │ │ -_4_3class GTSAM_EXPORT _S_u_b_g_r_a_p_h { │ │ │ │ │ -44 public: │ │ │ │ │ -_4_5 struct GTSAM_EXPORT _E_d_g_e { │ │ │ │ │ -46 size_t index; /* edge id */ │ │ │ │ │ -47 double weight; /* edge weight */ │ │ │ │ │ -48 inline bool isUnitWeight() const { return (weight == 1.0); } │ │ │ │ │ -49 friend std::ostream &operator<<(std::ostream &os, const _E_d_g_e &edge); │ │ │ │ │ +28 enum Kernel { /* Preconditioner Kernel */ │ │ │ │ │ +29 GTSAM = 0, │ │ │ │ │ +30 CHOLMOD /* experimental */ │ │ │ │ │ +31 } kernel_ ; │ │ │ │ │ +32 │ │ │ │ │ +33 enum Verbosity { │ │ │ │ │ +34 SILENT = 0, │ │ │ │ │ +35 COMPLEXITY = 1, │ │ │ │ │ +36 ERROR = 2 │ │ │ │ │ +37 } verbosity_ ; │ │ │ │ │ +38 │ │ │ │ │ +39 _P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s(): kernel_(GTSAM), verbosity_(SILENT) {} │ │ │ │ │ +40 _P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s(const _P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s &p) : kernel_ │ │ │ │ │ +(p.kernel_), verbosity_(p.verbosity_) {} │ │ │ │ │ +41 virtual _~_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s() {} │ │ │ │ │ +42 │ │ │ │ │ +43 /* general interface */ │ │ │ │ │ +44 inline Kernel kernel() const { return kernel_; } │ │ │ │ │ +45 inline Verbosity verbosity() const { return verbosity_; } │ │ │ │ │ +46 │ │ │ │ │ +47 void _p_r_i_n_t() const; │ │ │ │ │ +48 │ │ │ │ │ +49 virtual void _p_r_i_n_t(std::ostream &os) const; │ │ │ │ │ 50 │ │ │ │ │ -51 private: │ │ │ │ │ -52 friend class boost::serialization::access; │ │ │ │ │ -53 template │ │ │ │ │ -54 void serialize(Archive &ar, const unsigned int /*version*/) { │ │ │ │ │ -55 ar &BOOST_SERIALIZATION_NVP(index); │ │ │ │ │ -56 ar &BOOST_SERIALIZATION_NVP(weight); │ │ │ │ │ -57 } │ │ │ │ │ +51 static Kernel kernelTranslator(const std::string &s); │ │ │ │ │ +52 static Verbosity verbosityTranslator(const std::string &s); │ │ │ │ │ +53 static std::string kernelTranslator(Kernel k); │ │ │ │ │ +54 static std::string verbosityTranslator(Verbosity v); │ │ │ │ │ +55 │ │ │ │ │ +56 /* for serialization */ │ │ │ │ │ +57 friend std::ostream& operator<<(std::ostream &os, const │ │ │ │ │ +_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s &p); │ │ │ │ │ 58 }; │ │ │ │ │ 59 │ │ │ │ │ -60 typedef std::vector Edges; │ │ │ │ │ -61 typedef std::vector EdgeIndices; │ │ │ │ │ -62 typedef Edges::iterator iterator; │ │ │ │ │ -63 typedef Edges::const_iterator const_iterator; │ │ │ │ │ -64 │ │ │ │ │ -65 protected: │ │ │ │ │ -66 Edges edges_; /* index to the factors */ │ │ │ │ │ -67 │ │ │ │ │ -68 public: │ │ │ │ │ -69 _S_u_b_g_r_a_p_h() {} │ │ │ │ │ -70 _S_u_b_g_r_a_p_h(const _S_u_b_g_r_a_p_h &subgraph) : edges_(subgraph.edges()) {} │ │ │ │ │ -71 Subgraph(const Edges &edges) : edges_(edges) {} │ │ │ │ │ -72 Subgraph(const std::vector &indices); │ │ │ │ │ -73 │ │ │ │ │ -74 inline const Edges &edges() const { return edges_; } │ │ │ │ │ -75 inline size_t size() const { return edges_.size(); } │ │ │ │ │ -76 EdgeIndices edgeIndices() const; │ │ │ │ │ -77 │ │ │ │ │ -78 iterator begin() { return edges_.begin(); } │ │ │ │ │ -79 const_iterator begin() const { return edges_.begin(); } │ │ │ │ │ -80 iterator end() { return edges_.end(); } │ │ │ │ │ -81 const_iterator end() const { return edges_.end(); } │ │ │ │ │ +60/* PCG aims to solve the problem: A x = b by reparametrizing it as │ │ │ │ │ +61 * L^{-1} A L^{-T} y = L^{-1} b or M^{-1} A x = M^{-1} b, │ │ │ │ │ +62 * where A \approx L L^{T}, or A \approx M │ │ │ │ │ +63 * The goal of this class is to provide a general interface to all │ │ │ │ │ +preconditioners */ │ │ │ │ │ +_6_4class GTSAM_EXPORT _P_r_e_c_o_n_d_i_t_i_o_n_e_r { │ │ │ │ │ +65public: │ │ │ │ │ +66 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +67 typedef std::vector Dimensions; │ │ │ │ │ +68 │ │ │ │ │ +69 /* Generic Constructor and Destructor */ │ │ │ │ │ +70 _P_r_e_c_o_n_d_i_t_i_o_n_e_r() {} │ │ │ │ │ +71 virtual _~_P_r_e_c_o_n_d_i_t_i_o_n_e_r() {} │ │ │ │ │ +72 │ │ │ │ │ +73 /* │ │ │ │ │ +74 * Abstract interface for raw vectors. VectorValues is a speed bottleneck │ │ │ │ │ +75 * and Yong-Dian has profiled preconditioners (outside GTSAM) with the the │ │ │ │ │ +76 * three methods below. In GTSAM, unfortunately, we are still using the │ │ │ │ │ +77 * VectorValues methods called in iterative-inl.h │ │ │ │ │ +78 */ │ │ │ │ │ +79 │ │ │ │ │ +_8_1 virtual void _s_o_l_v_e(const Vector& y, Vector &x) const = 0; │ │ │ │ │ 82 │ │ │ │ │ -83 void _s_a_v_e(const std::string &fn) const; │ │ │ │ │ -84 static Subgraph load(const std::string &fn); │ │ │ │ │ -85 friend std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph); │ │ │ │ │ -86 │ │ │ │ │ -87 private: │ │ │ │ │ -88 friend class boost::serialization::access; │ │ │ │ │ -89 template │ │ │ │ │ -90 void serialize(Archive &ar, const unsigned int /*version*/) { │ │ │ │ │ -91 ar &BOOST_SERIALIZATION_NVP(edges_); │ │ │ │ │ -92 } │ │ │ │ │ -93}; │ │ │ │ │ -94 │ │ │ │ │ -95/ │ │ │ │ │ -****************************************************************************/ │ │ │ │ │ -_9_6struct GTSAM_EXPORT _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s { │ │ │ │ │ -97 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -98 │ │ │ │ │ -99 enum Skeleton { │ │ │ │ │ -100 /* augmented tree */ │ │ │ │ │ -101 NATURALCHAIN = 0, /* natural ordering of the graph */ │ │ │ │ │ -102 BFS, /* breadth-first search tree */ │ │ │ │ │ -103 KRUSKAL, /* maximum weighted spanning tree */ │ │ │ │ │ -104 } skeletonType; │ │ │ │ │ -105 │ │ │ │ │ -106 enum SkeletonWeight { /* how to weigh the graph edges */ │ │ │ │ │ -107 EQUAL = 0, /* every block edge has equal weight */ │ │ │ │ │ -108 RHS_2NORM, /* use the 2-norm of the rhs */ │ │ │ │ │ -109 LHS_FNORM, /* use the frobenius norm of the lhs */ │ │ │ │ │ -110 RANDOM, /* bounded random edge weight */ │ │ │ │ │ -111 } skeletonWeight; │ │ │ │ │ +_8_4 virtual void _t_r_a_n_s_p_o_s_e_S_o_l_v_e(const Vector& y, Vector& x) const = 0; │ │ │ │ │ +85 │ │ │ │ │ +_8_7 virtual void _b_u_i_l_d( │ │ │ │ │ +88 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, │ │ │ │ │ +89 const _K_e_y_I_n_f_o &info, │ │ │ │ │ +90 const std::map &lambda │ │ │ │ │ +91 ) = 0; │ │ │ │ │ +92}; │ │ │ │ │ +93 │ │ │ │ │ +94/ │ │ │ │ │ +*******************************************************************************************/ │ │ │ │ │ +_9_5struct GTSAM_EXPORT _D_u_m_m_y_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s : public │ │ │ │ │ +_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s { │ │ │ │ │ +96 typedef _P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s _B_a_s_e; │ │ │ │ │ +97 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +98 _D_u_m_m_y_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s() : _B_a_s_e() {} │ │ │ │ │ +99 _~_D_u_m_m_y_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s() override {} │ │ │ │ │ +100}; │ │ │ │ │ +101 │ │ │ │ │ +102/ │ │ │ │ │ +*******************************************************************************************/ │ │ │ │ │ +_1_0_3class GTSAM_EXPORT _D_u_m_m_y_P_r_e_c_o_n_d_i_t_i_o_n_e_r : public _P_r_e_c_o_n_d_i_t_i_o_n_e_r { │ │ │ │ │ +104public: │ │ │ │ │ +105 typedef _P_r_e_c_o_n_d_i_t_i_o_n_e_r _B_a_s_e; │ │ │ │ │ +106 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +107 │ │ │ │ │ +108public: │ │ │ │ │ +109 │ │ │ │ │ +110 _D_u_m_m_y_P_r_e_c_o_n_d_i_t_i_o_n_e_r() : _B_a_s_e() {} │ │ │ │ │ +111 _~_D_u_m_m_y_P_r_e_c_o_n_d_i_t_i_o_n_e_r() override {} │ │ │ │ │ 112 │ │ │ │ │ -113 enum AugmentationWeight { /* how to weigh the graph edges */ │ │ │ │ │ -114 SKELETON = 0, /* use the same weights in building │ │ │ │ │ -115 the skeleton */ │ │ │ │ │ -116 // STRETCH, /* stretch in the │ │ │ │ │ -117 // laplacian sense */ GENERALIZED_STRETCH /* │ │ │ │ │ -118 // the generalized stretch defined in │ │ │ │ │ -119 // jian2013iros */ │ │ │ │ │ -120 } augmentationWeight; │ │ │ │ │ -121 │ │ │ │ │ -_1_2_3 double _a_u_g_m_e_n_t_a_t_i_o_n_F_a_c_t_o_r; │ │ │ │ │ -124 │ │ │ │ │ -125 _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s() │ │ │ │ │ -126 : skeletonType(KRUSKAL), │ │ │ │ │ -127 skeletonWeight(RANDOM), │ │ │ │ │ -128 augmentationWeight(SKELETON), │ │ │ │ │ -129 augmentationFactor(1.0) {} │ │ │ │ │ -130 virtual _~_S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s() {} │ │ │ │ │ -131 │ │ │ │ │ -132 /* for serialization */ │ │ │ │ │ -133 void print() const; │ │ │ │ │ -134 virtual void print(std::ostream &os) const; │ │ │ │ │ -135 friend std::ostream &operator<<(std::ostream &os, │ │ │ │ │ -136 const PreconditionerParameters &p); │ │ │ │ │ -137 │ │ │ │ │ -138 static Skeleton skeletonTranslator(const std::string &s); │ │ │ │ │ -139 static std::string skeletonTranslator(Skeleton s); │ │ │ │ │ -140 static SkeletonWeight skeletonWeightTranslator(const std::string &s); │ │ │ │ │ -141 static std::string skeletonWeightTranslator(SkeletonWeight w); │ │ │ │ │ -142 static AugmentationWeight augmentationWeightTranslator(const std::string │ │ │ │ │ -&s); │ │ │ │ │ -143 static std::string augmentationWeightTranslator(AugmentationWeight w); │ │ │ │ │ -144}; │ │ │ │ │ +113 /* Computation Interfaces for raw vector */ │ │ │ │ │ +_1_1_4 void _s_o_l_v_e(const Vector& y, Vector &x) const override { x = y; } │ │ │ │ │ +_1_1_5 void _t_r_a_n_s_p_o_s_e_S_o_l_v_e(const Vector& y, Vector& x) const override { x = y; } │ │ │ │ │ +_1_1_6 void _b_u_i_l_d( │ │ │ │ │ +117 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, │ │ │ │ │ +118 const _K_e_y_I_n_f_o &info, │ │ │ │ │ +119 const std::map &lambda │ │ │ │ │ +120 ) override {} │ │ │ │ │ +121}; │ │ │ │ │ +122 │ │ │ │ │ +123/ │ │ │ │ │ +*******************************************************************************************/ │ │ │ │ │ +_1_2_4struct GTSAM_EXPORT _B_l_o_c_k_J_a_c_o_b_i_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s : public │ │ │ │ │ +_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s { │ │ │ │ │ +125 typedef _P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s _B_a_s_e; │ │ │ │ │ +126 _B_l_o_c_k_J_a_c_o_b_i_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s() : _B_a_s_e() {} │ │ │ │ │ +127 _~_B_l_o_c_k_J_a_c_o_b_i_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s() override {} │ │ │ │ │ +128}; │ │ │ │ │ +129 │ │ │ │ │ +130/ │ │ │ │ │ +*******************************************************************************************/ │ │ │ │ │ +_1_3_1class GTSAM_EXPORT _B_l_o_c_k_J_a_c_o_b_i_P_r_e_c_o_n_d_i_t_i_o_n_e_r : public _P_r_e_c_o_n_d_i_t_i_o_n_e_r { │ │ │ │ │ +132public: │ │ │ │ │ +133 typedef _P_r_e_c_o_n_d_i_t_i_o_n_e_r _B_a_s_e; │ │ │ │ │ +134 _B_l_o_c_k_J_a_c_o_b_i_P_r_e_c_o_n_d_i_t_i_o_n_e_r() ; │ │ │ │ │ +135 _~_B_l_o_c_k_J_a_c_o_b_i_P_r_e_c_o_n_d_i_t_i_o_n_e_r() override ; │ │ │ │ │ +136 │ │ │ │ │ +137 /* Computation Interfaces for raw vector */ │ │ │ │ │ +138 void solve(const Vector& y, Vector &x) const override; │ │ │ │ │ +139 void transposeSolve(const Vector& y, Vector& x) const override; │ │ │ │ │ +140 void build( │ │ │ │ │ +141 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, │ │ │ │ │ +142 const _K_e_y_I_n_f_o &info, │ │ │ │ │ +143 const std::map &lambda │ │ │ │ │ +144 ) override; │ │ │ │ │ 145 │ │ │ │ │ -146/ │ │ │ │ │ -*****************************************************************************/ │ │ │ │ │ -_1_4_7class GTSAM_EXPORT _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r { │ │ │ │ │ -148 public: │ │ │ │ │ -149 typedef _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r _B_a_s_e; │ │ │ │ │ -150 typedef std::vector Weights; │ │ │ │ │ -151 │ │ │ │ │ -152 _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r( │ │ │ │ │ -153 const _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s &p = _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s()) │ │ │ │ │ -154 : parameters_(p) {} │ │ │ │ │ -155 virtual _~_S_u_b_g_r_a_p_h_B_u_i_l_d_e_r() {} │ │ │ │ │ -156 virtual _S_u_b_g_r_a_p_h operator()(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &jfg) const; │ │ │ │ │ -157 │ │ │ │ │ -158 private: │ │ │ │ │ -159 std::vector buildTree(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, │ │ │ │ │ -160 const _F_a_s_t_M_a_p_<_K_e_y_,_ _s_i_z_e___t_> &ordering, │ │ │ │ │ -161 const std::vector &weights) const; │ │ │ │ │ -162 std::vector unary(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg) const; │ │ │ │ │ -163 std::vector natural_chain(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg) const; │ │ │ │ │ -164 std::vector bfs(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg) const; │ │ │ │ │ -165 std::vector kruskal(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, │ │ │ │ │ -166 const _F_a_s_t_M_a_p_<_K_e_y_,_ _s_i_z_e___t_> &ordering, │ │ │ │ │ -167 const std::vector &weights) const; │ │ │ │ │ -168 std::vector sample(const std::vector &weights, │ │ │ │ │ -169 const size_t t) const; │ │ │ │ │ -170 Weights weights(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg) const; │ │ │ │ │ -171 _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s parameters_; │ │ │ │ │ -172}; │ │ │ │ │ -173 │ │ │ │ │ -175_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h _b_u_i_l_d_F_a_c_t_o_r_S_u_b_g_r_a_p_h(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, │ │ │ │ │ -176 const _S_u_b_g_r_a_p_h &subgraph, │ │ │ │ │ -177 const bool clone); │ │ │ │ │ -178 │ │ │ │ │ -181std::pair _s_p_l_i_t_F_a_c_t_o_r_G_r_a_p_h( │ │ │ │ │ -182 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &factorGraph, const _S_u_b_g_r_a_p_h &subgraph); │ │ │ │ │ -183 │ │ │ │ │ -184} // namespace gtsam │ │ │ │ │ -_F_a_s_t_M_a_p_._h │ │ │ │ │ -A thin wrapper around std::map that uses boost's fast_pool_allocator. │ │ │ │ │ -_t_y_p_e_s_._h │ │ │ │ │ -Typedefs for easier changing of types. │ │ │ │ │ +146protected: │ │ │ │ │ +147 │ │ │ │ │ +148 void clean() ; │ │ │ │ │ +149 │ │ │ │ │ +150 std::vector dims_; │ │ │ │ │ +151 double *buffer_; │ │ │ │ │ +152 size_t bufferSize_; │ │ │ │ │ +153 size_t nnz_; │ │ │ │ │ +154}; │ │ │ │ │ +155 │ │ │ │ │ +156/ │ │ │ │ │ +*********************************************************************************************/ │ │ │ │ │ +157/* factory method to create preconditioners */ │ │ │ │ │ +158boost::shared_ptr createPreconditioner(const boost:: │ │ │ │ │ +shared_ptr parameters); │ │ │ │ │ +159 │ │ │ │ │ +160} │ │ │ │ │ +161 │ │ │ │ │ +162 │ │ │ │ │ +_V_e_c_t_o_r_._h │ │ │ │ │ +typedef and functions to augment Eigen's VectorXd │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_b_u_i_l_d_F_a_c_t_o_r_S_u_b_g_r_a_p_h │ │ │ │ │ -GaussianFactorGraph buildFactorSubgraph(const GaussianFactorGraph &gfg, const │ │ │ │ │ -Subgraph &subgraph, const bool clone) │ │ │ │ │ -Select the factors in a factor graph according to the subgraph. │ │ │ │ │ -DDeeffiinniittiioonn SubgraphBuilder.cpp:447 │ │ │ │ │ -_g_t_s_a_m_:_:_s_p_l_i_t_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -std::pair< GaussianFactorGraph, GaussianFactorGraph > splitFactorGraph(const │ │ │ │ │ -GaussianFactorGraph &factorGraph, const Subgraph &subgraph) │ │ │ │ │ -Split the graph into a subgraph and the remaining edges. │ │ │ │ │ -DDeeffiinniittiioonn SubgraphBuilder.cpp:460 │ │ │ │ │ -_g_t_s_a_m_:_:_s_a_v_e │ │ │ │ │ -void save(const Matrix &A, const string &s, const string &filename) │ │ │ │ │ -save a matrix to file, which can be loaded by matlab │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:167 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ -FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ -fast_pool_allocator instead of the defa... │ │ │ │ │ -DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h │ │ │ │ │ -DDeeffiinniittiioonn SubgraphBuilder.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_:_:_E_d_g_e │ │ │ │ │ -DDeeffiinniittiioonn SubgraphBuilder.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ -DDeeffiinniittiioonn SubgraphBuilder.h:96 │ │ │ │ │ -_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s_:_:_a_u_g_m_e_n_t_a_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ -double augmentationFactor │ │ │ │ │ -factor multiplied with n, yields number of extra edges. │ │ │ │ │ -DDeeffiinniittiioonn SubgraphBuilder.h:123 │ │ │ │ │ -_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_B_u_i_l_d_e_r │ │ │ │ │ -DDeeffiinniittiioonn SubgraphBuilder.h:147 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_I_n_f_o │ │ │ │ │ +Handy data structure for iterative solvers. │ │ │ │ │ +DDeeffiinniittiioonn IterativeSolver.h:126 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ +DDeeffiinniittiioonn Preconditioner.h:24 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_c_o_n_d_i_t_i_o_n_e_r │ │ │ │ │ +DDeeffiinniittiioonn Preconditioner.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_c_o_n_d_i_t_i_o_n_e_r_:_:_s_o_l_v_e │ │ │ │ │ +virtual void solve(const Vector &y, Vector &x) const =0 │ │ │ │ │ +implement x = L^{-1} y │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_c_o_n_d_i_t_i_o_n_e_r_:_:_b_u_i_l_d │ │ │ │ │ +virtual void build(const GaussianFactorGraph &gfg, const KeyInfo &info, const │ │ │ │ │ +std::map< Key, Vector > &lambda)=0 │ │ │ │ │ +build/factorize the preconditioner │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_c_o_n_d_i_t_i_o_n_e_r_:_:_t_r_a_n_s_p_o_s_e_S_o_l_v_e │ │ │ │ │ +virtual void transposeSolve(const Vector &y, Vector &x) const =0 │ │ │ │ │ +implement x = L^{-T} y │ │ │ │ │ +_g_t_s_a_m_:_:_D_u_m_m_y_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ +DDeeffiinniittiioonn Preconditioner.h:95 │ │ │ │ │ +_g_t_s_a_m_:_:_D_u_m_m_y_P_r_e_c_o_n_d_i_t_i_o_n_e_r │ │ │ │ │ +DDeeffiinniittiioonn Preconditioner.h:103 │ │ │ │ │ +_g_t_s_a_m_:_:_D_u_m_m_y_P_r_e_c_o_n_d_i_t_i_o_n_e_r_:_:_b_u_i_l_d │ │ │ │ │ +void build(const GaussianFactorGraph &gfg, const KeyInfo &info, const std::map< │ │ │ │ │ +Key, Vector > &lambda) override │ │ │ │ │ +build/factorize the preconditioner │ │ │ │ │ +DDeeffiinniittiioonn Preconditioner.h:116 │ │ │ │ │ +_g_t_s_a_m_:_:_D_u_m_m_y_P_r_e_c_o_n_d_i_t_i_o_n_e_r_:_:_s_o_l_v_e │ │ │ │ │ +void solve(const Vector &y, Vector &x) const override │ │ │ │ │ +implement x = L^{-1} y │ │ │ │ │ +DDeeffiinniittiioonn Preconditioner.h:114 │ │ │ │ │ +_g_t_s_a_m_:_:_D_u_m_m_y_P_r_e_c_o_n_d_i_t_i_o_n_e_r_:_:_t_r_a_n_s_p_o_s_e_S_o_l_v_e │ │ │ │ │ +void transposeSolve(const Vector &y, Vector &x) const override │ │ │ │ │ +implement x = L^{-T} y │ │ │ │ │ +DDeeffiinniittiioonn Preconditioner.h:115 │ │ │ │ │ +_g_t_s_a_m_:_:_B_l_o_c_k_J_a_c_o_b_i_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ +DDeeffiinniittiioonn Preconditioner.h:124 │ │ │ │ │ +_g_t_s_a_m_:_:_B_l_o_c_k_J_a_c_o_b_i_P_r_e_c_o_n_d_i_t_i_o_n_e_r │ │ │ │ │ +DDeeffiinniittiioonn Preconditioner.h:131 │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +The Factor::error simply extracts the. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_._h │ │ │ │ │ + * PPrreeccoonnddiittiioonneerr..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00863.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00929.html │ │ │ │┄ Files 92% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/KalmanFilter.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/PreintegrationParams.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ -Namespaces | │ │ │ │ -Macros
│ │ │ │ -
KalmanFilter.h File Reference
│ │ │ │ +Namespaces
│ │ │ │ +
PreintegrationParams.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Simple linear Kalman filter. │ │ │ │ -More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::KalmanFilter
 Kalman Filter class. More...
struct  gtsam::PreintegrationParams
 Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the constructor. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Macros

│ │ │ │ -#define KALMANFILTER_DEFAULT_FACTORIZATION   QR
 
│ │ │ │

Detailed Description

│ │ │ │ -

Simple linear Kalman filter.

│ │ │ │ -

Implemented using factor graphs, i.e., does Cholesky-based SRIF, really.

Date
Sep 3, 2011
│ │ │ │ -
Author
Stephen Williams
│ │ │ │ +
Author
Luca Carlone
│ │ │ │ +
│ │ │ │ +Stephen Williams
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │ +
│ │ │ │ +Vadim Indelman
│ │ │ │ +
│ │ │ │ +David Jensen
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │
│ │ │ │ Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,32 +1,30 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s │ │ │ │ │ -KalmanFilter.h File Reference │ │ │ │ │ -Simple linear Kalman filter. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +PreintegrationParams.h File Reference │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_K_a_l_m_a_n_F_i_l_t_e_r │ │ │ │ │ -  Kalman Filter class. _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ +  Parameters for pre-integration: Usage: Create just a single Params and │ │ │ │ │ + pass a shared pointer to the constructor. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -MMaaccrrooss │ │ │ │ │ -#define  KKAALLMMAANNFFIILLTTEERR__DDEEFFAAUULLTT__FFAACCTTOORRIIZZAATTIIOONN   QR │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Simple linear Kalman filter. │ │ │ │ │ -Implemented using factor graphs, i.e., does Cholesky-based SRIF, really. │ │ │ │ │ - Date │ │ │ │ │ - Sep 3, 2011 │ │ │ │ │ Author │ │ │ │ │ + Luca Carlone │ │ │ │ │ Stephen Williams │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Vadim Indelman │ │ │ │ │ + David Jensen │ │ │ │ │ + Frank Dellaert │ │ │ │ │ Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _l_i_n_e_a_r │ │ │ │ │ - * _K_a_l_m_a_n_F_i_l_t_e_r_._h │ │ │ │ │ + * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ + * _P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00863_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00062_source.html │ │ │ │┄ Files 94% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/KalmanFilter.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/DSFMap.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
KalmanFilter.h
│ │ │ │ +
DSFMap.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/linear/GaussianDensity.h>
│ │ │ │ -
23#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ -
24#include <gtsam/linear/NoiseModel.h>
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <cstdlib> // Provides size_t
│ │ │ │ +
22#include <map>
│ │ │ │ +
23#include <set>
│ │ │ │ +
24#include <vector>
│ │ │ │
25
│ │ │ │ -
26#ifndef KALMANFILTER_DEFAULT_FACTORIZATION
│ │ │ │ -
27#define KALMANFILTER_DEFAULT_FACTORIZATION QR
│ │ │ │ -
28#endif
│ │ │ │ -
29
│ │ │ │ -
30namespace gtsam {
│ │ │ │ -
31
│ │ │ │ -
│ │ │ │ -
41class GTSAM_EXPORT KalmanFilter {
│ │ │ │ +
26namespace gtsam {
│ │ │ │ +
27
│ │ │ │ +
33template <class KEY>
│ │ │ │ +
│ │ │ │ +
34class DSFMap {
│ │ │ │ +
35 protected:
│ │ │ │ +
│ │ │ │ +
37 struct Entry {
│ │ │ │ +
38 typename std::map<KEY, Entry>::iterator parent_;
│ │ │ │ +
39 size_t rank_;
│ │ │ │ +
40 Entry() {}
│ │ │ │ +
41 };
│ │ │ │ +
│ │ │ │
42
│ │ │ │ -
43public:
│ │ │ │ -
44
│ │ │ │ -
│ │ │ │ -
49 enum Factorization {
│ │ │ │ -
50 QR, CHOLESKY
│ │ │ │ -
51 };
│ │ │ │ -
│ │ │ │ -
52
│ │ │ │ -
56 typedef GaussianDensity::shared_ptr State;
│ │ │ │ -
57
│ │ │ │ -
58private:
│ │ │ │ +
43 typedef typename std::map<KEY, Entry> Map;
│ │ │ │ +
44 typedef typename Map::iterator iterator;
│ │ │ │ +
45 mutable Map entries_;
│ │ │ │ +
46
│ │ │ │ +
│ │ │ │ +
48 iterator find__(const KEY& key) const {
│ │ │ │ +
49 static const Entry empty;
│ │ │ │ +
50 iterator it = entries_.find(key);
│ │ │ │ +
51 // if key does not exist, create and return itself
│ │ │ │ +
52 if (it == entries_.end()) {
│ │ │ │ +
53 it = entries_.insert(std::make_pair(key, empty)).first;
│ │ │ │ +
54 it->second.parent_ = it;
│ │ │ │ +
55 it->second.rank_ = 0;
│ │ │ │ +
56 }
│ │ │ │ +
57 return it;
│ │ │ │ +
58 }
│ │ │ │ +
│ │ │ │
59
│ │ │ │ -
60 const size_t n_;
│ │ │ │ -
61 const Matrix I_;
│ │ │ │ -
62 const GaussianFactorGraph::Eliminate function_;
│ │ │ │ -
64 State solve(const GaussianFactorGraph& factorGraph) const;
│ │ │ │ -
65 State fuse(const State& p, GaussianFactor::shared_ptr newFactor) const;
│ │ │ │ -
66
│ │ │ │ -
67public:
│ │ │ │ -
68
│ │ │ │ -
69 // Constructor
│ │ │ │ -
70 KalmanFilter(size_t n, Factorization method =
│ │ │ │ -
71 KALMANFILTER_DEFAULT_FACTORIZATION) :
│ │ │ │ -
72 n_(n), I_(Matrix::Identity(n_, n_)), function_(
│ │ │ │ -
73 method == QR ? GaussianFactorGraph::Eliminate(EliminateQR) :
│ │ │ │ -
74 GaussianFactorGraph::Eliminate(EliminateCholesky)) {
│ │ │ │ -
75 }
│ │ │ │ +
│ │ │ │ +
61 iterator find_(const iterator& it) const {
│ │ │ │ +
62 // follow parent pointers until we reach set representative
│ │ │ │ +
63 iterator& parent = it->second.parent_;
│ │ │ │ +
64 if (parent != it) parent = find_(parent); // not yet, recurse!
│ │ │ │ +
65 return parent;
│ │ │ │ +
66 }
│ │ │ │ +
│ │ │ │ +
67
│ │ │ │ +
│ │ │ │ +
69 inline iterator find_(const KEY& key) const {
│ │ │ │ +
70 iterator initial = find__(key);
│ │ │ │ +
71 return find_(initial);
│ │ │ │ +
72 }
│ │ │ │ +
│ │ │ │ +
73
│ │ │ │ +
74 public:
│ │ │ │ +
75 typedef std::set<KEY> Set;
│ │ │ │
76
│ │ │ │ -
83 State init(const Vector& x0, const SharedDiagonal& P0) const;
│ │ │ │ -
84
│ │ │ │ -
86 State init(const Vector& x0, const Matrix& P0) const;
│ │ │ │ -
87
│ │ │ │ -
89 void print(const std::string& s = "") const;
│ │ │ │ -
90
│ │ │ │ -
│ │ │ │ -
92 static Key step(const State& p) {
│ │ │ │ -
93 return p->firstFrontalKey();
│ │ │ │ -
94 }
│ │ │ │ -
│ │ │ │ -
95
│ │ │ │ -
104 State predict(const State& p, const Matrix& F, const Matrix& B,
│ │ │ │ -
105 const Vector& u, const SharedDiagonal& modelQ) const;
│ │ │ │ -
106
│ │ │ │ -
107 /*
│ │ │ │ -
108 * Version of predict with full covariance
│ │ │ │ -
109 * Q is normally derived as G*w*G^T where w models uncertainty of some
│ │ │ │ -
110 * physical property, such as velocity or acceleration, and G is derived from physics.
│ │ │ │ -
111 * This version allows more realistic models than a diagonal covariance matrix.
│ │ │ │ -
112 */
│ │ │ │ -
113 State predictQ(const State& p, const Matrix& F, const Matrix& B,
│ │ │ │ -
114 const Vector& u, const Matrix& Q) const;
│ │ │ │ +
78 DSFMap() {}
│ │ │ │ +
79
│ │ │ │ +
│ │ │ │ +
81 inline KEY find(const KEY& key) const {
│ │ │ │ +
82 iterator root = find_(key);
│ │ │ │ +
83 return root->first;
│ │ │ │ +
84 }
│ │ │ │ +
│ │ │ │ +
85
│ │ │ │ +
│ │ │ │ +
87 void merge(const KEY& x, const KEY& y) {
│ │ │ │ +
88 // straight from http://en.wikipedia.org/wiki/Disjoint-set_data_structure
│ │ │ │ +
89 iterator xRoot = find_(x);
│ │ │ │ +
90 iterator yRoot = find_(y);
│ │ │ │ +
91 if (xRoot == yRoot) return;
│ │ │ │ +
92
│ │ │ │ +
93 // Merge sets
│ │ │ │ +
94 if (xRoot->second.rank_ < yRoot->second.rank_)
│ │ │ │ +
95 xRoot->second.parent_ = yRoot;
│ │ │ │ +
96 else if (xRoot->second.rank_ > yRoot->second.rank_)
│ │ │ │ +
97 yRoot->second.parent_ = xRoot;
│ │ │ │ +
98 else {
│ │ │ │ +
99 yRoot->second.parent_ = xRoot;
│ │ │ │ +
100 xRoot->second.rank_ = xRoot->second.rank_ + 1;
│ │ │ │ +
101 }
│ │ │ │ +
102 }
│ │ │ │ +
│ │ │ │ +
103
│ │ │ │ +
│ │ │ │ +
105 std::map<KEY, Set> sets() const {
│ │ │ │ +
106 std::map<KEY, Set> sets;
│ │ │ │ +
107 iterator it = entries_.begin();
│ │ │ │ +
108 for (; it != entries_.end(); it++) {
│ │ │ │ +
109 iterator root = find_(it);
│ │ │ │ +
110 sets[root->first].insert(it->first);
│ │ │ │ +
111 }
│ │ │ │ +
112 return sets;
│ │ │ │ +
113 }
│ │ │ │ +
│ │ │ │ +
114};
│ │ │ │ +
│ │ │ │
115
│ │ │ │ -
124 State predict2(const State& p, const Matrix& A0, const Matrix& A1,
│ │ │ │ -
125 const Vector& b, const SharedDiagonal& model) const;
│ │ │ │ -
126
│ │ │ │ -
135 State update(const State& p, const Matrix& H, const Vector& z,
│ │ │ │ -
136 const SharedDiagonal& model) const;
│ │ │ │ -
137
│ │ │ │ -
138 /*
│ │ │ │ -
139 * Version of update with full covariance
│ │ │ │ -
140 * Q is normally derived as G*w*G^T where w models uncertainty of some
│ │ │ │ -
141 * physical property, such as velocity or acceleration, and G is derived from physics.
│ │ │ │ -
142 * This version allows more realistic models than a diagonal covariance matrix.
│ │ │ │ -
143 */
│ │ │ │ -
144 State updateQ(const State& p, const Matrix& H, const Vector& z,
│ │ │ │ -
145 const Matrix& Q) const;
│ │ │ │ -
146};
│ │ │ │ -
│ │ │ │ -
147
│ │ │ │ -
148} // \namespace gtsam
│ │ │ │ -
149
│ │ │ │ -
150/* ************************************************************************* */
│ │ │ │ -
151
│ │ │ │ - │ │ │ │ -
GaussianDensity.h
A Gaussian Density.
│ │ │ │ -
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ -
gtsam::EliminateCholesky
std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< HessianFactor > > EliminateCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
Densely partially eliminate with Cholesky factorization.
Definition HessianFactor.cpp:525
│ │ │ │ +
│ │ │ │ +
117class IndexPair : public std::pair<size_t,size_t> {
│ │ │ │ +
118 public:
│ │ │ │ +
119 inline IndexPair(): std::pair<size_t,size_t>(0,0) {}
│ │ │ │ +
120 inline IndexPair(size_t i, size_t j) : std::pair<size_t,size_t>(i,j) {}
│ │ │ │ +
121 inline size_t i() const { return first; };
│ │ │ │ +
122 inline size_t j() const { return second; };
│ │ │ │ +
123};
│ │ │ │ +
│ │ │ │ +
124
│ │ │ │ +
125typedef std::vector<IndexPair> IndexPairVector;
│ │ │ │ +
126typedef std::set<IndexPair> IndexPairSet;
│ │ │ │ +
127
│ │ │ │ +
128inline IndexPairVector IndexPairSetAsArray(IndexPairSet& set) { return IndexPairVector(set.begin(), set.end()); }
│ │ │ │ +
129
│ │ │ │ +
130typedef std::map<IndexPair, IndexPairSet> IndexPairSetMap;
│ │ │ │ +
131typedef DSFMap<IndexPair> DSFMapIndexPair;
│ │ │ │ +
132} // namespace gtsam
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::EliminateQR
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Multiply all factors and eliminate the given keys from the resulting factor using a QR variant that h...
Definition JacobianFactor.cpp:789
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::EliminateableFactorGraph< GaussianFactorGraph >::Eliminate
std::function< EliminationResult(const FactorGraphType &, const Ordering &)> Eliminate
The function type that does a single dense elimination step on a subgraph.
Definition EliminateableFactorGraph.h:89
│ │ │ │ -
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactor.h:42
│ │ │ │ -
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ -
gtsam::KalmanFilter
Kalman Filter class.
Definition KalmanFilter.h:41
│ │ │ │ -
gtsam::KalmanFilter::Factorization
Factorization
This Kalman filter is a Square-root Information filter The type below allows you to specify the facto...
Definition KalmanFilter.h:49
│ │ │ │ -
gtsam::KalmanFilter::step
static Key step(const State &p)
Return step index k, starts at 0, incremented at each predict.
Definition KalmanFilter.h:92
│ │ │ │ -
gtsam::KalmanFilter::State
GaussianDensity::shared_ptr State
The Kalman filter state is simply a GaussianDensity.
Definition KalmanFilter.h:56
│ │ │ │ +
gtsam::DSFMap
Disjoint set forest using an STL map data structure underneath Uses rank compression and union by ran...
Definition DSFMap.h:34
│ │ │ │ +
gtsam::DSFMap::find__
iterator find__(const KEY &key) const
Given key, find iterator to initial entry.
Definition DSFMap.h:48
│ │ │ │ +
gtsam::DSFMap::find_
iterator find_(const iterator &it) const
Given iterator to initial entry, find the root Entry.
Definition DSFMap.h:61
│ │ │ │ +
gtsam::DSFMap::find_
iterator find_(const KEY &key) const
Given key, find the root Entry.
Definition DSFMap.h:69
│ │ │ │ +
gtsam::DSFMap::sets
std::map< KEY, Set > sets() const
return all sets, i.e. a partition of all elements
Definition DSFMap.h:105
│ │ │ │ +
gtsam::DSFMap::DSFMap
DSFMap()
constructor
Definition DSFMap.h:78
│ │ │ │ +
gtsam::DSFMap::find
KEY find(const KEY &key) const
Given key, find the representative key for the set in which it lives.
Definition DSFMap.h:81
│ │ │ │ +
gtsam::DSFMap::merge
void merge(const KEY &x, const KEY &y)
Merge two sets.
Definition DSFMap.h:87
│ │ │ │ +
gtsam::DSFMap::Entry
We store the forest in an STL map, but parents are done with pointers.
Definition DSFMap.h:37
│ │ │ │ +
gtsam::IndexPair
Small utility class for representing a wrappable pairs of ints.
Definition DSFMap.h:117
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,167 +1,166 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -KalmanFilter.h │ │ │ │ │ +DSFMap.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_D_e_n_s_i_t_y_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_N_o_i_s_e_M_o_d_e_l_._h> │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include // Provides size_t │ │ │ │ │ +22#include │ │ │ │ │ +23#include │ │ │ │ │ +24#include │ │ │ │ │ 25 │ │ │ │ │ -26#ifndef KALMANFILTER_DEFAULT_FACTORIZATION │ │ │ │ │ -27#define KALMANFILTER_DEFAULT_FACTORIZATION QR │ │ │ │ │ -28#endif │ │ │ │ │ -29 │ │ │ │ │ -30namespace _g_t_s_a_m { │ │ │ │ │ -31 │ │ │ │ │ -_4_1class GTSAM_EXPORT _K_a_l_m_a_n_F_i_l_t_e_r { │ │ │ │ │ +26namespace _g_t_s_a_m { │ │ │ │ │ +27 │ │ │ │ │ +33template │ │ │ │ │ +_3_4class _D_S_F_M_a_p { │ │ │ │ │ +35 protected: │ │ │ │ │ +_3_7 struct _E_n_t_r_y { │ │ │ │ │ +38 typename std::map::iterator parent_; │ │ │ │ │ +39 size_t rank_; │ │ │ │ │ +40 _E_n_t_r_y() {} │ │ │ │ │ +41 }; │ │ │ │ │ 42 │ │ │ │ │ -43public: │ │ │ │ │ -44 │ │ │ │ │ -_4_9 enum _F_a_c_t_o_r_i_z_a_t_i_o_n { │ │ │ │ │ -50 QR, CHOLESKY │ │ │ │ │ -51 }; │ │ │ │ │ -52 │ │ │ │ │ -_5_6 typedef GaussianDensity::shared_ptr _S_t_a_t_e; │ │ │ │ │ -57 │ │ │ │ │ -58private: │ │ │ │ │ +43 typedef typename std::map Map; │ │ │ │ │ +44 typedef typename Map::iterator iterator; │ │ │ │ │ +45 mutable Map entries_; │ │ │ │ │ +46 │ │ │ │ │ +_4_8 iterator _f_i_n_d____(const KEY& key) const { │ │ │ │ │ +49 static const _E_n_t_r_y empty; │ │ │ │ │ +50 iterator it = entries_.find(key); │ │ │ │ │ +51 // if key does not exist, create and return itself │ │ │ │ │ +52 if (it == entries_.end()) { │ │ │ │ │ +53 it = entries_.insert(std::make_pair(key, empty)).first; │ │ │ │ │ +54 it->second.parent_ = it; │ │ │ │ │ +55 it->second.rank_ = 0; │ │ │ │ │ +56 } │ │ │ │ │ +57 return it; │ │ │ │ │ +58 } │ │ │ │ │ 59 │ │ │ │ │ -60 const size_t n_; │ │ │ │ │ -61 const Matrix I_; │ │ │ │ │ -62 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_E_l_i_m_i_n_a_t_e function_; │ │ │ │ │ -64 _S_t_a_t_e solve(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& factorGraph) const; │ │ │ │ │ -65 _S_t_a_t_e fuse(const _S_t_a_t_e& p, _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r newFactor) const; │ │ │ │ │ -66 │ │ │ │ │ -67public: │ │ │ │ │ -68 │ │ │ │ │ -69 // Constructor │ │ │ │ │ -70 _K_a_l_m_a_n_F_i_l_t_e_r(size_t n, _F_a_c_t_o_r_i_z_a_t_i_o_n method = │ │ │ │ │ -71 KALMANFILTER_DEFAULT_FACTORIZATION) : │ │ │ │ │ -72 n_(n), I_(Matrix::Identity(n_, n_)), function_( │ │ │ │ │ -73 method == QR ? _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h::Eliminate(_E_l_i_m_i_n_a_t_e_Q_R) : │ │ │ │ │ -74 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h::Eliminate(_E_l_i_m_i_n_a_t_e_C_h_o_l_e_s_k_y)) { │ │ │ │ │ -75 } │ │ │ │ │ +_6_1 iterator _f_i_n_d__(const iterator& it) const { │ │ │ │ │ +62 // follow parent pointers until we reach set representative │ │ │ │ │ +63 iterator& parent = it->second.parent_; │ │ │ │ │ +64 if (parent != it) parent = _f_i_n_d__(parent); // not yet, recurse! │ │ │ │ │ +65 return parent; │ │ │ │ │ +66 } │ │ │ │ │ +67 │ │ │ │ │ +_6_9 inline iterator _f_i_n_d__(const KEY& key) const { │ │ │ │ │ +70 iterator initial = _f_i_n_d____(key); │ │ │ │ │ +71 return _f_i_n_d__(initial); │ │ │ │ │ +72 } │ │ │ │ │ +73 │ │ │ │ │ +74 public: │ │ │ │ │ +75 typedef std::set Set; │ │ │ │ │ 76 │ │ │ │ │ -83 State init(const Vector& x0, const SharedDiagonal& P0) const; │ │ │ │ │ -84 │ │ │ │ │ -86 State init(const Vector& x0, const Matrix& P0) const; │ │ │ │ │ -87 │ │ │ │ │ -89 void _p_r_i_n_t(const std::string& s = "") const; │ │ │ │ │ -90 │ │ │ │ │ -_9_2 static _K_e_y _s_t_e_p(const _S_t_a_t_e& p) { │ │ │ │ │ -93 return p->firstFrontalKey(); │ │ │ │ │ -94 } │ │ │ │ │ -95 │ │ │ │ │ -104 State predict(const State& p, const Matrix& F, const Matrix& B, │ │ │ │ │ -105 const Vector& u, const SharedDiagonal& modelQ) const; │ │ │ │ │ -106 │ │ │ │ │ -107 /* │ │ │ │ │ -108 * Version of predict with full covariance │ │ │ │ │ -109 * Q is normally derived as G*w*G^T where w models uncertainty of some │ │ │ │ │ -110 * physical property, such as velocity or acceleration, and G is derived │ │ │ │ │ -from physics. │ │ │ │ │ -111 * This version allows more realistic models than a diagonal covariance │ │ │ │ │ -matrix. │ │ │ │ │ -112 */ │ │ │ │ │ -113 State predictQ(const State& p, const Matrix& F, const Matrix& B, │ │ │ │ │ -114 const Vector& u, const Matrix& Q) const; │ │ │ │ │ +_7_8 _D_S_F_M_a_p() {} │ │ │ │ │ +79 │ │ │ │ │ +_8_1 inline KEY _f_i_n_d(const KEY& key) const { │ │ │ │ │ +82 iterator root = _f_i_n_d__(key); │ │ │ │ │ +83 return root->first; │ │ │ │ │ +84 } │ │ │ │ │ +85 │ │ │ │ │ +_8_7 void _m_e_r_g_e(const KEY& x, const KEY& y) { │ │ │ │ │ +88 // straight from http://en.wikipedia.org/wiki/Disjoint-set_data_structure │ │ │ │ │ +89 iterator xRoot = _f_i_n_d__(x); │ │ │ │ │ +90 iterator yRoot = _f_i_n_d__(y); │ │ │ │ │ +91 if (xRoot == yRoot) return; │ │ │ │ │ +92 │ │ │ │ │ +93 // Merge sets │ │ │ │ │ +94 if (xRoot->second.rank_ < yRoot->second.rank_) │ │ │ │ │ +95 xRoot->second.parent_ = yRoot; │ │ │ │ │ +96 else if (xRoot->second.rank_ > yRoot->second.rank_) │ │ │ │ │ +97 yRoot->second.parent_ = xRoot; │ │ │ │ │ +98 else { │ │ │ │ │ +99 yRoot->second.parent_ = xRoot; │ │ │ │ │ +100 xRoot->second.rank_ = xRoot->second.rank_ + 1; │ │ │ │ │ +101 } │ │ │ │ │ +102 } │ │ │ │ │ +103 │ │ │ │ │ +_1_0_5 std::map _s_e_t_s() const { │ │ │ │ │ +106 std::map _s_e_t_s; │ │ │ │ │ +107 iterator it = entries_.begin(); │ │ │ │ │ +108 for (; it != entries_.end(); it++) { │ │ │ │ │ +109 iterator root = _f_i_n_d__(it); │ │ │ │ │ +110 _s_e_t_s[root->first].insert(it->first); │ │ │ │ │ +111 } │ │ │ │ │ +112 return _s_e_t_s; │ │ │ │ │ +113 } │ │ │ │ │ +114}; │ │ │ │ │ 115 │ │ │ │ │ -124 State predict2(const State& p, const Matrix& A0, const Matrix& A1, │ │ │ │ │ -125 const Vector& b, const SharedDiagonal& model) const; │ │ │ │ │ -126 │ │ │ │ │ -135 State update(const State& p, const Matrix& H, const Vector& z, │ │ │ │ │ -136 const SharedDiagonal& model) const; │ │ │ │ │ -137 │ │ │ │ │ -138 /* │ │ │ │ │ -139 * Version of update with full covariance │ │ │ │ │ -140 * Q is normally derived as G*w*G^T where w models uncertainty of some │ │ │ │ │ -141 * physical property, such as velocity or acceleration, and G is derived │ │ │ │ │ -from physics. │ │ │ │ │ -142 * This version allows more realistic models than a diagonal covariance │ │ │ │ │ -matrix. │ │ │ │ │ -143 */ │ │ │ │ │ -144 State updateQ(const State& p, const Matrix& H, const Vector& z, │ │ │ │ │ -145 const Matrix& Q) const; │ │ │ │ │ -146}; │ │ │ │ │ -147 │ │ │ │ │ -148} // \namespace gtsam │ │ │ │ │ -149 │ │ │ │ │ -150/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -151 │ │ │ │ │ -_N_o_i_s_e_M_o_d_e_l_._h │ │ │ │ │ -_G_a_u_s_s_i_a_n_D_e_n_s_i_t_y_._h │ │ │ │ │ -A Gaussian Density. │ │ │ │ │ -_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_C_h_o_l_e_s_k_y │ │ │ │ │ -std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< │ │ │ │ │ -HessianFactor > > EliminateCholesky(const GaussianFactorGraph &factors, const │ │ │ │ │ -Ordering &keys) │ │ │ │ │ -Densely partially eliminate with Cholesky factorization. │ │ │ │ │ -DDeeffiinniittiioonn HessianFactor.cpp:525 │ │ │ │ │ +_1_1_7class _I_n_d_e_x_P_a_i_r : public std::pair { │ │ │ │ │ +118 public: │ │ │ │ │ +119 inline _I_n_d_e_x_P_a_i_r(): std::pair(0,0) {} │ │ │ │ │ +120 inline _I_n_d_e_x_P_a_i_r(size_t i, size_t j) : std::pair(i,j) {} │ │ │ │ │ +121 inline size_t i() const { return first; }; │ │ │ │ │ +122 inline size_t j() const { return second; }; │ │ │ │ │ +123}; │ │ │ │ │ +124 │ │ │ │ │ +125typedef std::vector IndexPairVector; │ │ │ │ │ +126typedef std::set IndexPairSet; │ │ │ │ │ +127 │ │ │ │ │ +128inline IndexPairVector IndexPairSetAsArray(IndexPairSet& set) { return │ │ │ │ │ +IndexPairVector(set.begin(), set.end()); } │ │ │ │ │ +129 │ │ │ │ │ +130typedef std::map IndexPairSetMap; │ │ │ │ │ +131typedef DSFMap DSFMapIndexPair; │ │ │ │ │ +132} // namespace gtsam │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_Q_R │ │ │ │ │ -std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > │ │ │ │ │ -EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys) │ │ │ │ │ -Multiply all factors and eliminate the given keys from the resulting factor │ │ │ │ │ -using a QR variant that h... │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.cpp:789 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_E_l_i_m_i_n_a_t_e │ │ │ │ │ -std::function< EliminationResult(const FactorGraphType &, const Ordering &)> │ │ │ │ │ -Eliminate │ │ │ │ │ -The function type that does a single dense elimination step on a subgraph. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph.h:89 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_K_a_l_m_a_n_F_i_l_t_e_r │ │ │ │ │ -Kalman Filter class. │ │ │ │ │ -DDeeffiinniittiioonn KalmanFilter.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_K_a_l_m_a_n_F_i_l_t_e_r_:_:_F_a_c_t_o_r_i_z_a_t_i_o_n │ │ │ │ │ -Factorization │ │ │ │ │ -This Kalman filter is a Square-root Information filter The type below allows │ │ │ │ │ -you to specify the facto... │ │ │ │ │ -DDeeffiinniittiioonn KalmanFilter.h:49 │ │ │ │ │ -_g_t_s_a_m_:_:_K_a_l_m_a_n_F_i_l_t_e_r_:_:_s_t_e_p │ │ │ │ │ -static Key step(const State &p) │ │ │ │ │ -Return step index k, starts at 0, incremented at each predict. │ │ │ │ │ -DDeeffiinniittiioonn KalmanFilter.h:92 │ │ │ │ │ -_g_t_s_a_m_:_:_K_a_l_m_a_n_F_i_l_t_e_r_:_:_S_t_a_t_e │ │ │ │ │ -GaussianDensity::shared_ptr State │ │ │ │ │ -The Kalman filter state is simply a GaussianDensity. │ │ │ │ │ -DDeeffiinniittiioonn KalmanFilter.h:56 │ │ │ │ │ +_g_t_s_a_m_:_:_D_S_F_M_a_p │ │ │ │ │ +Disjoint set forest using an STL map data structure underneath Uses rank │ │ │ │ │ +compression and union by ran... │ │ │ │ │ +DDeeffiinniittiioonn DSFMap.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_D_S_F_M_a_p_:_:_f_i_n_d____ │ │ │ │ │ +iterator find__(const KEY &key) const │ │ │ │ │ +Given key, find iterator to initial entry. │ │ │ │ │ +DDeeffiinniittiioonn DSFMap.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_D_S_F_M_a_p_:_:_f_i_n_d__ │ │ │ │ │ +iterator find_(const iterator &it) const │ │ │ │ │ +Given iterator to initial entry, find the root Entry. │ │ │ │ │ +DDeeffiinniittiioonn DSFMap.h:61 │ │ │ │ │ +_g_t_s_a_m_:_:_D_S_F_M_a_p_:_:_f_i_n_d__ │ │ │ │ │ +iterator find_(const KEY &key) const │ │ │ │ │ +Given key, find the root Entry. │ │ │ │ │ +DDeeffiinniittiioonn DSFMap.h:69 │ │ │ │ │ +_g_t_s_a_m_:_:_D_S_F_M_a_p_:_:_s_e_t_s │ │ │ │ │ +std::map< KEY, Set > sets() const │ │ │ │ │ +return all sets, i.e. a partition of all elements │ │ │ │ │ +DDeeffiinniittiioonn DSFMap.h:105 │ │ │ │ │ +_g_t_s_a_m_:_:_D_S_F_M_a_p_:_:_D_S_F_M_a_p │ │ │ │ │ +DSFMap() │ │ │ │ │ +constructor │ │ │ │ │ +DDeeffiinniittiioonn DSFMap.h:78 │ │ │ │ │ +_g_t_s_a_m_:_:_D_S_F_M_a_p_:_:_f_i_n_d │ │ │ │ │ +KEY find(const KEY &key) const │ │ │ │ │ +Given key, find the representative key for the set in which it lives. │ │ │ │ │ +DDeeffiinniittiioonn DSFMap.h:81 │ │ │ │ │ +_g_t_s_a_m_:_:_D_S_F_M_a_p_:_:_m_e_r_g_e │ │ │ │ │ +void merge(const KEY &x, const KEY &y) │ │ │ │ │ +Merge two sets. │ │ │ │ │ +DDeeffiinniittiioonn DSFMap.h:87 │ │ │ │ │ +_g_t_s_a_m_:_:_D_S_F_M_a_p_:_:_E_n_t_r_y │ │ │ │ │ +We store the forest in an STL map, but parents are done with pointers. │ │ │ │ │ +DDeeffiinniittiioonn DSFMap.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_I_n_d_e_x_P_a_i_r │ │ │ │ │ +Small utility class for representing a wrappable pairs of ints. │ │ │ │ │ +DDeeffiinniittiioonn DSFMap.h:117 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _l_i_n_e_a_r │ │ │ │ │ - * _K_a_l_m_a_n_F_i_l_t_e_r_._h │ │ │ │ │ + * _b_a_s_e │ │ │ │ │ + * _D_S_F_M_a_p_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00866_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00746_source.html │ │ │ │┄ Files 98% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
GaussianISAM.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/linear/GaussianBayesTree.h>
│ │ │ │ -
22#include <gtsam/inference/ISAM.h>
│ │ │ │ -
23#include <gtsam/base/Testable.h>
│ │ │ │ +
21#include <gtsam/linear/GaussianBayesTree.h>
│ │ │ │ +
22#include <gtsam/inference/ISAM.h>
│ │ │ │ +
23#include <gtsam/base/Testable.h>
│ │ │ │
24
│ │ │ │
25namespace gtsam {
│ │ │ │
26
│ │ │ │
│ │ │ │
27 class GTSAM_EXPORT GaussianISAM : public ISAM<GaussianBayesTree>
│ │ │ │
28 {
│ │ │ │
29 public:
│ │ │ │ @@ -141,27 +141,27 @@ │ │ │ │
45 };
│ │ │ │
│ │ │ │
46
│ │ │ │
48 template <>
│ │ │ │
49 struct traits<GaussianISAM> : public Testable<GaussianISAM> {};
│ │ │ │
50
│ │ │ │
51}
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ -
ISAM.h
Incremental update functionality (iSAM) for BayesTree.
│ │ │ │ -
GaussianBayesTree.h
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
ISAM.h
Incremental update functionality (iSAM) for BayesTree.
│ │ │ │ +
GaussianBayesTree.h
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │
gtsam::ISAM
A Bayes tree with an update methods that implements the iSAM algorithm.
Definition ISAM.h:31
│ │ │ │
gtsam::GaussianBayesTree
A Bayes tree representing a Gaussian density.
Definition GaussianBayesTree.h:52
│ │ │ │
gtsam::GaussianISAM
Definition GaussianISAM.h:28
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00890_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00935_source.html │ │ │ │┄ Files 86% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/PCGSolver.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ScenarioRunner.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
PCGSolver.h
│ │ │ │ +
ScenarioRunner.h
│ │ │ │
│ │ │ │
│ │ │ │ -
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
12/*
│ │ │ │ -
13 * @file PCGSolver.h
│ │ │ │ -
14 * @brief Preconditioned Conjugate Gradient Solver for linear systems
│ │ │ │ -
15 * @date Jan 31, 2012
│ │ │ │ -
16 * @author Yong-Dian Jian
│ │ │ │ -
17 * @author Sungtae An
│ │ │ │ -
18 */
│ │ │ │ -
19
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/linear/ConjugateGradientSolver.h>
│ │ │ │ -
23#include <string>
│ │ │ │ -
24
│ │ │ │ -
25namespace gtsam {
│ │ │ │ -
26
│ │ │ │ -
27class GaussianFactorGraph;
│ │ │ │ -
28class KeyInfo;
│ │ │ │ -
29class Preconditioner;
│ │ │ │ -
30class VectorValues;
│ │ │ │ -
31struct PreconditionerParameters;
│ │ │ │ -
32
│ │ │ │ -
│ │ │ │ -
36struct GTSAM_EXPORT PCGSolverParameters: public ConjugateGradientParameters {
│ │ │ │ -
37public:
│ │ │ │ -
38 typedef ConjugateGradientParameters Base;
│ │ │ │ -
39 typedef boost::shared_ptr<PCGSolverParameters> shared_ptr;
│ │ │ │ -
40
│ │ │ │ -
41 PCGSolverParameters() {
│ │ │ │ -
42 }
│ │ │ │ -
43
│ │ │ │ -
44 void print(std::ostream &os) const override;
│ │ │ │ -
45
│ │ │ │ -
46 /* interface to preconditioner parameters */
│ │ │ │ -
47 inline const PreconditionerParameters& preconditioner() const {
│ │ │ │ -
48 return *preconditioner_;
│ │ │ │ -
49 }
│ │ │ │ +
18#pragma once
│ │ │ │ +
19#include <gtsam/linear/Sampler.h>
│ │ │ │ +
20#include <gtsam/navigation/CombinedImuFactor.h>
│ │ │ │ +
21#include <gtsam/navigation/ImuFactor.h>
│ │ │ │ +
22#include <gtsam/navigation/Scenario.h>
│ │ │ │ +
23
│ │ │ │ +
24namespace gtsam {
│ │ │ │ +
25
│ │ │ │ +
26// Convert covariance to diagonal noise model, if possible, otherwise throw
│ │ │ │ +
27static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) {
│ │ │ │ +
28 bool smart = true;
│ │ │ │ +
29 auto model = noiseModel::Gaussian::Covariance(covariance, smart);
│ │ │ │ +
30 auto diagonal = boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
│ │ │ │ +
31 if (!diagonal)
│ │ │ │ +
32 throw std::invalid_argument("ScenarioRunner::Diagonal: not a diagonal");
│ │ │ │ +
33 return diagonal;
│ │ │ │ +
34}
│ │ │ │ +
35
│ │ │ │ +
36/*
│ │ │ │ +
37 * Simple class to test navigation scenarios.
│ │ │ │ +
38 * Takes a trajectory scenario as input, and can generate IMU measurements
│ │ │ │ +
39 */
│ │ │ │ +
│ │ │ │ +
40class GTSAM_EXPORT ScenarioRunner {
│ │ │ │ +
41 public:
│ │ │ │ +
42 typedef imuBias::ConstantBias Bias;
│ │ │ │ +
43 typedef boost::shared_ptr<PreintegrationParams> SharedParams;
│ │ │ │ +
44
│ │ │ │ +
45 private:
│ │ │ │ +
46 const Scenario& scenario_;
│ │ │ │ +
47 const SharedParams p_;
│ │ │ │ +
48 const double imuSampleTime_, sqrt_dt_;
│ │ │ │ +
49 const Bias estimatedBias_;
│ │ │ │
50
│ │ │ │ -
51 // needed for python wrapper
│ │ │ │ -
52 void print(const std::string &s) const;
│ │ │ │ +
51 // Create two samplers for acceleration and omega noise
│ │ │ │ +
52 Sampler gyroSampler_, accSampler_;
│ │ │ │
53
│ │ │ │ -
54 boost::shared_ptr<PreconditionerParameters> preconditioner_;
│ │ │ │ -
55
│ │ │ │ -
56 void setPreconditionerParams(const boost::shared_ptr<PreconditionerParameters> preconditioner);
│ │ │ │ -
57};
│ │ │ │ -
│ │ │ │ -
58
│ │ │ │ -
│ │ │ │ -
62class GTSAM_EXPORT PCGSolver: public IterativeSolver {
│ │ │ │ -
63public:
│ │ │ │ -
64 typedef IterativeSolver Base;
│ │ │ │ -
65 typedef boost::shared_ptr<PCGSolver> shared_ptr;
│ │ │ │ -
66
│ │ │ │ -
67protected:
│ │ │ │ -
68
│ │ │ │ -
69 PCGSolverParameters parameters_;
│ │ │ │ -
70 boost::shared_ptr<Preconditioner> preconditioner_;
│ │ │ │ +
54 public:
│ │ │ │ +
55 ScenarioRunner(const Scenario& scenario, const SharedParams& p,
│ │ │ │ +
56 double imuSampleTime = 1.0 / 100.0, const Bias& bias = Bias())
│ │ │ │ +
57 : scenario_(scenario),
│ │ │ │ +
58 p_(p),
│ │ │ │ +
59 imuSampleTime_(imuSampleTime),
│ │ │ │ +
60 sqrt_dt_(std::sqrt(imuSampleTime)),
│ │ │ │ +
61 estimatedBias_(bias),
│ │ │ │ +
62 // NOTE(duy): random seeds that work well:
│ │ │ │ +
63 gyroSampler_(Diagonal(p->gyroscopeCovariance), 10),
│ │ │ │ +
64 accSampler_(Diagonal(p->accelerometerCovariance), 29284) {}
│ │ │ │ +
65
│ │ │ │ +
66 // NOTE(frank): hardcoded for now with Z up (gravity points in negative Z)
│ │ │ │ +
67 // also, uses g=10 for easy debugging
│ │ │ │ +
68 const Vector3& gravity_n() const { return p_->n_gravity; }
│ │ │ │ +
69
│ │ │ │ +
70 const Scenario& scenario() const { return scenario_; }
│ │ │ │
71
│ │ │ │ -
72public:
│ │ │ │ -
73 /* Interface to initialize a solver without a problem */
│ │ │ │ -
74 PCGSolver(const PCGSolverParameters &p);
│ │ │ │ -
75 ~PCGSolver() override {
│ │ │ │ -
76 }
│ │ │ │ -
77
│ │ │ │ -
78 using IterativeSolver::optimize;
│ │ │ │ -
79
│ │ │ │ -
80 VectorValues optimize(const GaussianFactorGraph &gfg,
│ │ │ │ -
81 const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
│ │ │ │ -
82 const VectorValues &initial) override;
│ │ │ │ -
83
│ │ │ │ -
84};
│ │ │ │ -
│ │ │ │ -
85
│ │ │ │ -
│ │ │ │ -
89class GTSAM_EXPORT GaussianFactorGraphSystem {
│ │ │ │ -
90public:
│ │ │ │ -
91
│ │ │ │ -
92 GaussianFactorGraphSystem(const GaussianFactorGraph &gfg,
│ │ │ │ -
93 const Preconditioner &preconditioner, const KeyInfo &info,
│ │ │ │ -
94 const std::map<Key, Vector> &lambda);
│ │ │ │ -
95
│ │ │ │ -
96 const GaussianFactorGraph &gfg_;
│ │ │ │ -
97 const Preconditioner &preconditioner_;
│ │ │ │ -
98 const KeyInfo &keyInfo_;
│ │ │ │ -
99 const std::map<Key, Vector> &lambda_;
│ │ │ │ -
100
│ │ │ │ -
101 void residual(const Vector &x, Vector &r) const;
│ │ │ │ -
102 void multiply(const Vector &x, Vector& y) const;
│ │ │ │ -
103 void leftPrecondition(const Vector &x, Vector &y) const;
│ │ │ │ -
104 void rightPrecondition(const Vector &x, Vector &y) const;
│ │ │ │ -
105 void scal(const double alpha, Vector &x) const;
│ │ │ │ -
106 double dot(const Vector &x, const Vector &y) const;
│ │ │ │ -
107 void axpy(const double alpha, const Vector &x, Vector &y) const;
│ │ │ │ -
108
│ │ │ │ -
109 void getb(Vector &b) const;
│ │ │ │ -
110};
│ │ │ │ -
│ │ │ │ -
111
│ │ │ │ -
114
│ │ │ │ -
116VectorValues buildVectorValues(const Vector &v, const Ordering &ordering,
│ │ │ │ -
117 const std::map<Key, size_t> & dimensions);
│ │ │ │ -
118
│ │ │ │ -
120VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo);
│ │ │ │ +
72 // A gyro simply measures angular velocity in body frame
│ │ │ │ +
73 Vector3 actualAngularVelocity(double t) const { return scenario_.omega_b(t); }
│ │ │ │ +
74
│ │ │ │ +
75 // An accelerometer measures acceleration in body, but not gravity
│ │ │ │ +
76 Vector3 actualSpecificForce(double t) const {
│ │ │ │ +
77 Rot3 bRn(scenario_.rotation(t).transpose());
│ │ │ │ +
78 return scenario_.acceleration_b(t) - bRn * gravity_n();
│ │ │ │ +
79 }
│ │ │ │ +
80
│ │ │ │ +
81 // versions corrupted by bias and noise
│ │ │ │ +
82 Vector3 measuredAngularVelocity(double t) const {
│ │ │ │ +
83 return actualAngularVelocity(t) + estimatedBias_.gyroscope() +
│ │ │ │ +
84 gyroSampler_.sample() / sqrt_dt_;
│ │ │ │ +
85 }
│ │ │ │ +
86 Vector3 measuredSpecificForce(double t) const {
│ │ │ │ +
87 return actualSpecificForce(t) + estimatedBias_.accelerometer() +
│ │ │ │ +
88 accSampler_.sample() / sqrt_dt_;
│ │ │ │ +
89 }
│ │ │ │ +
90
│ │ │ │ +
91 const double& imuSampleTime() const { return imuSampleTime_; }
│ │ │ │ +
92
│ │ │ │ +
94 PreintegratedImuMeasurements integrate(double T,
│ │ │ │ +
95 const Bias& estimatedBias = Bias(),
│ │ │ │ +
96 bool corrupted = false) const;
│ │ │ │ +
97
│ │ │ │ +
99 NavState predict(const PreintegratedImuMeasurements& pim,
│ │ │ │ +
100 const Bias& estimatedBias = Bias()) const;
│ │ │ │ +
101
│ │ │ │ +
103 Matrix9 estimateCovariance(double T, size_t N = 1000,
│ │ │ │ +
104 const Bias& estimatedBias = Bias()) const;
│ │ │ │ +
105
│ │ │ │ +
107 Matrix6 estimateNoiseCovariance(size_t N = 1000) const;
│ │ │ │ +
108};
│ │ │ │ +
│ │ │ │ +
109
│ │ │ │ +
110/*
│ │ │ │ +
111 * Simple class to test navigation scenarios with CombinedImuMeasurements.
│ │ │ │ +
112 * Takes a trajectory scenario as input, and can generate IMU measurements
│ │ │ │ +
113 */
│ │ │ │ +
│ │ │ │ +
114class GTSAM_EXPORT CombinedScenarioRunner : public ScenarioRunner {
│ │ │ │ +
115 public:
│ │ │ │ +
116 typedef boost::shared_ptr<PreintegrationCombinedParams> SharedParams;
│ │ │ │ +
117
│ │ │ │ +
118 private:
│ │ │ │ +
119 const SharedParams p_;
│ │ │ │ +
120 const Bias estimatedBias_;
│ │ │ │
121
│ │ │ │ -
123
│ │ │ │ -
124}
│ │ │ │ -
125
│ │ │ │ -
ConjugateGradientSolver.h
Implementation of Conjugate Gradient solver for a linear system.
│ │ │ │ +
122 public:
│ │ │ │ +
123 CombinedScenarioRunner(const Scenario& scenario, const SharedParams& p,
│ │ │ │ +
124 double imuSampleTime = 1.0 / 100.0,
│ │ │ │ +
125 const Bias& bias = Bias())
│ │ │ │ +
126 : ScenarioRunner(scenario, static_cast<ScenarioRunner::SharedParams>(p),
│ │ │ │ +
127 imuSampleTime, bias),
│ │ │ │ +
128 p_(p),
│ │ │ │ +
129 estimatedBias_(bias) {}
│ │ │ │ +
130
│ │ │ │ +
132 PreintegratedCombinedMeasurements integrate(
│ │ │ │ +
133 double T, const Bias& estimatedBias = Bias(),
│ │ │ │ +
134 bool corrupted = false) const;
│ │ │ │ +
135
│ │ │ │ +
137 NavState predict(const PreintegratedCombinedMeasurements& pim,
│ │ │ │ +
138 const Bias& estimatedBias = Bias()) const;
│ │ │ │ +
139
│ │ │ │ +
141 Eigen::Matrix<double, 15, 15> estimateCovariance(
│ │ │ │ +
142 double T, size_t N = 1000, const Bias& estimatedBias = Bias()) const;
│ │ │ │ +
143};
│ │ │ │ +
│ │ │ │ +
144
│ │ │ │ +
145} // namespace gtsam
│ │ │ │ +
Sampler.h
sampling from a NoiseModel
│ │ │ │ + │ │ │ │ +
Scenario.h
Simple class to test navigation scenarios.
│ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::buildVectorValues
VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, const map< Key, size_t > &dimensions)
Create VectorValues from a Vector.
Definition PCGSolver.cpp:151
│ │ │ │ -
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::axpy
void axpy(double alpha, const Errors &x, Errors &y)
BLAS level 2 style AXPY, y := alpha*x + y
Definition Errors.cpp:111
│ │ │ │ -
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ -
gtsam::ConjugateGradientParameters
parameters for the conjugate gradient method
Definition ConjugateGradientSolver.h:29
│ │ │ │ -
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ -
gtsam::IterativeSolver
Base class for Iterative Solvers like SubgraphSolver.
Definition IterativeSolver.h:86
│ │ │ │ -
gtsam::KeyInfo
Handy data structure for iterative solvers.
Definition IterativeSolver.h:126
│ │ │ │ -
gtsam::PCGSolverParameters
Parameters for PCG.
Definition PCGSolver.h:36
│ │ │ │ -
gtsam::PCGSolver
A virtual base class for the preconditioned conjugate gradient solver.
Definition PCGSolver.h:62
│ │ │ │ -
gtsam::GaussianFactorGraphSystem
System class needed for calling preconditionedConjugateGradient.
Definition PCGSolver.h:89
│ │ │ │ -
gtsam::PreconditionerParameters
Definition Preconditioner.h:24
│ │ │ │ -
gtsam::Preconditioner
Definition Preconditioner.h:64
│ │ │ │ -
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ -
VectorValues
The Factor::error simply extracts the.
│ │ │ │ +
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
│ │ │ │ +
gtsam::Rot3::transpose
Matrix3 transpose() const
Return 3*3 transpose (inverse) rotation matrix.
Definition Rot3M.cpp:144
│ │ │ │ +
gtsam::noiseModel::Gaussian::Covariance
static shared_ptr Covariance(const Matrix &covariance, bool smart=true)
A Gaussian noise model created by specifying a covariance matrix.
Definition NoiseModel.cpp:117
│ │ │ │ +
gtsam::Sampler
Sampling structure that keeps internal random number generators for diagonal distributions specified ...
Definition Sampler.h:31
│ │ │ │ +
gtsam::Sampler::sample
Vector sample() const
sample from distribution
Definition Sampler.cpp:59
│ │ │ │ +
gtsam::PreintegratedCombinedMeasurements
PreintegratedCombinedMeasurements integrates the IMU measurements (rotation rates and accelerations) ...
Definition CombinedImuFactor.h:129
│ │ │ │ +
gtsam::imuBias::ConstantBias
Definition ImuBias.h:30
│ │ │ │ +
gtsam::imuBias::ConstantBias::gyroscope
const Vector3 & gyroscope() const
get gyroscope bias
Definition ImuBias.h:69
│ │ │ │ +
gtsam::imuBias::ConstantBias::accelerometer
const Vector3 & accelerometer() const
get accelerometer bias
Definition ImuBias.h:64
│ │ │ │ +
gtsam::PreintegratedImuMeasurements
PreintegratedImuMeasurements accumulates (integrates) the IMU measurements (rotation rates and accele...
Definition ImuFactor.h:72
│ │ │ │ +
gtsam::NavState
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition NavState.h:34
│ │ │ │ +
gtsam::Scenario
Simple trajectory simulator.
Definition Scenario.h:25
│ │ │ │ +
gtsam::Scenario::omega_b
virtual Vector3 omega_b(double t) const =0
angular velocity in body frame
│ │ │ │ +
gtsam::ScenarioRunner
Definition ScenarioRunner.h:40
│ │ │ │ +
gtsam::CombinedScenarioRunner
Definition ScenarioRunner.h:114
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,184 +1,207 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -PCGSolver.h │ │ │ │ │ +ScenarioRunner.h │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -12/* │ │ │ │ │ -13 * @file PCGSolver.h │ │ │ │ │ -14 * @brief Preconditioned Conjugate Gradient Solver for linear systems │ │ │ │ │ -15 * @date Jan 31, 2012 │ │ │ │ │ -16 * @author Yong-Dian Jian │ │ │ │ │ -17 * @author Sungtae An │ │ │ │ │ -18 */ │ │ │ │ │ -19 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_S_o_l_v_e_r_._h> │ │ │ │ │ -23#include │ │ │ │ │ -24 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ -26 │ │ │ │ │ -27class GaussianFactorGraph; │ │ │ │ │ -28class KeyInfo; │ │ │ │ │ -29class Preconditioner; │ │ │ │ │ -30class _V_e_c_t_o_r_V_a_l_u_e_s; │ │ │ │ │ -31struct PreconditionerParameters; │ │ │ │ │ -32 │ │ │ │ │ -_3_6struct GTSAM_EXPORT _P_C_G_S_o_l_v_e_r_P_a_r_a_m_e_t_e_r_s: public _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s { │ │ │ │ │ -37public: │ │ │ │ │ -38 typedef _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s _B_a_s_e; │ │ │ │ │ -39 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -40 │ │ │ │ │ -41 _P_C_G_S_o_l_v_e_r_P_a_r_a_m_e_t_e_r_s() { │ │ │ │ │ -42 } │ │ │ │ │ -43 │ │ │ │ │ -44 void _p_r_i_n_t(std::ostream &os) const override; │ │ │ │ │ -45 │ │ │ │ │ -46 /* interface to preconditioner parameters */ │ │ │ │ │ -47 inline const _P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s& preconditioner() const { │ │ │ │ │ -48 return *preconditioner_; │ │ │ │ │ -49 } │ │ │ │ │ +18#pragma once │ │ │ │ │ +19#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_S_a_m_p_l_e_r_._h> │ │ │ │ │ +20#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_I_m_u_F_a_c_t_o_r_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_S_c_e_n_a_r_i_o_._h> │ │ │ │ │ +23 │ │ │ │ │ +24namespace _g_t_s_a_m { │ │ │ │ │ +25 │ │ │ │ │ +26// Convert covariance to diagonal noise model, if possible, otherwise throw │ │ │ │ │ +27static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) { │ │ │ │ │ +28 bool smart = true; │ │ │ │ │ +29 auto model = _n_o_i_s_e_M_o_d_e_l_:_:_G_a_u_s_s_i_a_n_:_:_C_o_v_a_r_i_a_n_c_e(covariance, smart); │ │ │ │ │ +30 auto diagonal = boost::dynamic_pointer_cast(model); │ │ │ │ │ +31 if (!diagonal) │ │ │ │ │ +32 throw std::invalid_argument("ScenarioRunner::Diagonal: not a diagonal"); │ │ │ │ │ +33 return diagonal; │ │ │ │ │ +34} │ │ │ │ │ +35 │ │ │ │ │ +36/* │ │ │ │ │ +37 * Simple class to test navigation scenarios. │ │ │ │ │ +38 * Takes a trajectory scenario as input, and can generate IMU measurements │ │ │ │ │ +39 */ │ │ │ │ │ +_4_0class GTSAM_EXPORT _S_c_e_n_a_r_i_o_R_u_n_n_e_r { │ │ │ │ │ +41 public: │ │ │ │ │ +42 typedef _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s _B_i_a_s; │ │ │ │ │ +43 typedef boost::shared_ptr SharedParams; │ │ │ │ │ +44 │ │ │ │ │ +45 private: │ │ │ │ │ +46 const _S_c_e_n_a_r_i_o& scenario_; │ │ │ │ │ +47 const SharedParams p_; │ │ │ │ │ +48 const double imuSampleTime_, sqrt_dt_; │ │ │ │ │ +49 const _B_i_a_s estimatedBias_; │ │ │ │ │ 50 │ │ │ │ │ -51 // needed for python wrapper │ │ │ │ │ -52 void _p_r_i_n_t(const std::string &s) const; │ │ │ │ │ +51 // Create two samplers for acceleration and omega noise │ │ │ │ │ +52 _S_a_m_p_l_e_r gyroSampler_, accSampler_; │ │ │ │ │ 53 │ │ │ │ │ -54 boost::shared_ptr preconditioner_; │ │ │ │ │ -55 │ │ │ │ │ -56 void setPreconditionerParams(const boost:: │ │ │ │ │ -shared_ptr preconditioner); │ │ │ │ │ -57}; │ │ │ │ │ -58 │ │ │ │ │ -_6_2class GTSAM_EXPORT _P_C_G_S_o_l_v_e_r: public _I_t_e_r_a_t_i_v_e_S_o_l_v_e_r { │ │ │ │ │ -63public: │ │ │ │ │ -64 typedef _I_t_e_r_a_t_i_v_e_S_o_l_v_e_r _B_a_s_e; │ │ │ │ │ -65 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -66 │ │ │ │ │ -67protected: │ │ │ │ │ -68 │ │ │ │ │ -69 _P_C_G_S_o_l_v_e_r_P_a_r_a_m_e_t_e_r_s parameters_; │ │ │ │ │ -70 boost::shared_ptr preconditioner_; │ │ │ │ │ +54 public: │ │ │ │ │ +55 _S_c_e_n_a_r_i_o_R_u_n_n_e_r(const _S_c_e_n_a_r_i_o& scenario, const SharedParams& p, │ │ │ │ │ +56 double imuSampleTime = 1.0 / 100.0, const _B_i_a_s& bias = _B_i_a_s()) │ │ │ │ │ +57 : scenario_(scenario), │ │ │ │ │ +58 p_(p), │ │ │ │ │ +59 imuSampleTime_(imuSampleTime), │ │ │ │ │ +60 sqrt_dt_(std::sqrt(imuSampleTime)), │ │ │ │ │ +61 estimatedBias_(bias), │ │ │ │ │ +62 // NOTE(duy): random seeds that work well: │ │ │ │ │ +63 gyroSampler_(Diagonal(p->gyroscopeCovariance), 10), │ │ │ │ │ +64 accSampler_(Diagonal(p->accelerometerCovariance), 29284) {} │ │ │ │ │ +65 │ │ │ │ │ +66 // NOTE(frank): hardcoded for now with Z up (gravity points in negative Z) │ │ │ │ │ +67 // also, uses g=10 for easy debugging │ │ │ │ │ +68 const Vector3& gravity_n() const { return p_->n_gravity; } │ │ │ │ │ +69 │ │ │ │ │ +70 const _S_c_e_n_a_r_i_o& scenario() const { return scenario_; } │ │ │ │ │ 71 │ │ │ │ │ -72public: │ │ │ │ │ -73 /* Interface to initialize a solver without a problem */ │ │ │ │ │ -74 _P_C_G_S_o_l_v_e_r(const _P_C_G_S_o_l_v_e_r_P_a_r_a_m_e_t_e_r_s &p); │ │ │ │ │ -75 _~_P_C_G_S_o_l_v_e_r() override { │ │ │ │ │ -76 } │ │ │ │ │ -77 │ │ │ │ │ -78 using IterativeSolver::optimize; │ │ │ │ │ -79 │ │ │ │ │ -80 _V_e_c_t_o_r_V_a_l_u_e_s _o_p_t_i_m_i_z_e(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, │ │ │ │ │ -81 const _K_e_y_I_n_f_o &keyInfo, const std::map &lambda, │ │ │ │ │ -82 const _V_e_c_t_o_r_V_a_l_u_e_s &initial) override; │ │ │ │ │ -83 │ │ │ │ │ -84}; │ │ │ │ │ -85 │ │ │ │ │ -_8_9class GTSAM_EXPORT _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_S_y_s_t_e_m { │ │ │ │ │ -90public: │ │ │ │ │ -91 │ │ │ │ │ -92 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_S_y_s_t_e_m(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, │ │ │ │ │ -93 const _P_r_e_c_o_n_d_i_t_i_o_n_e_r &preconditioner, const _K_e_y_I_n_f_o &info, │ │ │ │ │ -94 const std::map &lambda); │ │ │ │ │ -95 │ │ │ │ │ -96 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg_; │ │ │ │ │ -97 const _P_r_e_c_o_n_d_i_t_i_o_n_e_r &preconditioner_; │ │ │ │ │ -98 const _K_e_y_I_n_f_o &keyInfo_; │ │ │ │ │ -99 const std::map &lambda_; │ │ │ │ │ -100 │ │ │ │ │ -101 void residual(const Vector &x, Vector &r) const; │ │ │ │ │ -102 void multiply(const Vector &x, Vector& y) const; │ │ │ │ │ -103 void leftPrecondition(const Vector &x, Vector &y) const; │ │ │ │ │ -104 void rightPrecondition(const Vector &x, Vector &y) const; │ │ │ │ │ -105 void scal(const double alpha, Vector &x) const; │ │ │ │ │ -106 double _d_o_t(const Vector &x, const Vector &y) const; │ │ │ │ │ -107 void _a_x_p_y(const double alpha, const Vector &x, Vector &y) const; │ │ │ │ │ -108 │ │ │ │ │ -109 void getb(Vector &b) const; │ │ │ │ │ -110}; │ │ │ │ │ -111 │ │ │ │ │ -114 │ │ │ │ │ -116_V_e_c_t_o_r_V_a_l_u_e_s _b_u_i_l_d_V_e_c_t_o_r_V_a_l_u_e_s(const Vector &v, const _O_r_d_e_r_i_n_g &ordering, │ │ │ │ │ -117 const std::map & dimensions); │ │ │ │ │ -118 │ │ │ │ │ -120_V_e_c_t_o_r_V_a_l_u_e_s _b_u_i_l_d_V_e_c_t_o_r_V_a_l_u_e_s(const Vector &v, const _K_e_y_I_n_f_o &keyInfo); │ │ │ │ │ +72 // A gyro simply measures angular velocity in body frame │ │ │ │ │ +73 Vector3 actualAngularVelocity(double t) const { return scenario_._o_m_e_g_a___b(t); │ │ │ │ │ +} │ │ │ │ │ +74 │ │ │ │ │ +75 // An accelerometer measures acceleration in body, but not gravity │ │ │ │ │ +76 Vector3 actualSpecificForce(double t) const { │ │ │ │ │ +77 _R_o_t_3 bRn(scenario_.rotation(t)._t_r_a_n_s_p_o_s_e()); │ │ │ │ │ +78 return scenario_.acceleration_b(t) - bRn * gravity_n(); │ │ │ │ │ +79 } │ │ │ │ │ +80 │ │ │ │ │ +81 // versions corrupted by bias and noise │ │ │ │ │ +82 Vector3 measuredAngularVelocity(double t) const { │ │ │ │ │ +83 return actualAngularVelocity(t) + estimatedBias_._g_y_r_o_s_c_o_p_e() + │ │ │ │ │ +84 gyroSampler_._s_a_m_p_l_e() / sqrt_dt_; │ │ │ │ │ +85 } │ │ │ │ │ +86 Vector3 measuredSpecificForce(double t) const { │ │ │ │ │ +87 return actualSpecificForce(t) + estimatedBias_._a_c_c_e_l_e_r_o_m_e_t_e_r() + │ │ │ │ │ +88 accSampler_._s_a_m_p_l_e() / sqrt_dt_; │ │ │ │ │ +89 } │ │ │ │ │ +90 │ │ │ │ │ +91 const double& imuSampleTime() const { return imuSampleTime_; } │ │ │ │ │ +92 │ │ │ │ │ +94 _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s integrate(double T, │ │ │ │ │ +95 const _B_i_a_s& estimatedBias = _B_i_a_s(), │ │ │ │ │ +96 bool corrupted = false) const; │ │ │ │ │ +97 │ │ │ │ │ +99 _N_a_v_S_t_a_t_e predict(const _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s& pim, │ │ │ │ │ +100 const _B_i_a_s& estimatedBias = _B_i_a_s()) const; │ │ │ │ │ +101 │ │ │ │ │ +103 Matrix9 estimateCovariance(double T, size_t N = 1000, │ │ │ │ │ +104 const _B_i_a_s& estimatedBias = _B_i_a_s()) const; │ │ │ │ │ +105 │ │ │ │ │ +107 Matrix6 estimateNoiseCovariance(size_t N = 1000) const; │ │ │ │ │ +108}; │ │ │ │ │ +109 │ │ │ │ │ +110/* │ │ │ │ │ +111 * Simple class to test navigation scenarios with CombinedImuMeasurements. │ │ │ │ │ +112 * Takes a trajectory scenario as input, and can generate IMU measurements │ │ │ │ │ +113 */ │ │ │ │ │ +_1_1_4class GTSAM_EXPORT _C_o_m_b_i_n_e_d_S_c_e_n_a_r_i_o_R_u_n_n_e_r : public _S_c_e_n_a_r_i_o_R_u_n_n_e_r { │ │ │ │ │ +115 public: │ │ │ │ │ +116 typedef boost::shared_ptr SharedParams; │ │ │ │ │ +117 │ │ │ │ │ +118 private: │ │ │ │ │ +119 const SharedParams p_; │ │ │ │ │ +120 const _B_i_a_s estimatedBias_; │ │ │ │ │ 121 │ │ │ │ │ -123 │ │ │ │ │ -124} │ │ │ │ │ -125 │ │ │ │ │ -_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_S_o_l_v_e_r_._h │ │ │ │ │ -Implementation of Conjugate Gradient solver for a linear system. │ │ │ │ │ +122 public: │ │ │ │ │ +123 _C_o_m_b_i_n_e_d_S_c_e_n_a_r_i_o_R_u_n_n_e_r(const _S_c_e_n_a_r_i_o& scenario, const SharedParams& p, │ │ │ │ │ +124 double imuSampleTime = 1.0 / 100.0, │ │ │ │ │ +125 const _B_i_a_s& bias = _B_i_a_s()) │ │ │ │ │ +126 : _S_c_e_n_a_r_i_o_R_u_n_n_e_r(scenario, static_cast(p), │ │ │ │ │ +127 imuSampleTime, bias), │ │ │ │ │ +128 p_(p), │ │ │ │ │ +129 estimatedBias_(bias) {} │ │ │ │ │ +130 │ │ │ │ │ +132 _P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s integrate( │ │ │ │ │ +133 double T, const _B_i_a_s& estimatedBias = _B_i_a_s(), │ │ │ │ │ +134 bool corrupted = false) const; │ │ │ │ │ +135 │ │ │ │ │ +137 _N_a_v_S_t_a_t_e predict(const _P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s& pim, │ │ │ │ │ +138 const _B_i_a_s& estimatedBias = _B_i_a_s()) const; │ │ │ │ │ +139 │ │ │ │ │ +141 Eigen::Matrix estimateCovariance( │ │ │ │ │ +142 double T, size_t N = 1000, const _B_i_a_s& estimatedBias = _B_i_a_s()) const; │ │ │ │ │ +143}; │ │ │ │ │ +144 │ │ │ │ │ +145} // namespace gtsam │ │ │ │ │ +_S_a_m_p_l_e_r_._h │ │ │ │ │ +sampling from a NoiseModel │ │ │ │ │ +_I_m_u_F_a_c_t_o_r_._h │ │ │ │ │ +_S_c_e_n_a_r_i_o_._h │ │ │ │ │ +Simple class to test navigation scenarios. │ │ │ │ │ +_C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_b_u_i_l_d_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, const │ │ │ │ │ -map< Key, size_t > &dimensions) │ │ │ │ │ -Create VectorValues from a Vector. │ │ │ │ │ -DDeeffiinniittiioonn PCGSolver.cpp:151 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ -Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key │ │ │ │ │ -landmarkKey) │ │ │ │ │ -Optimize for triangulation. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.cpp:155 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_a_x_p_y │ │ │ │ │ -void axpy(double alpha, const Errors &x, Errors &y) │ │ │ │ │ -BLAS level 2 style AXPY, y := alpha*x + y │ │ │ │ │ -DDeeffiinniittiioonn Errors.cpp:111 │ │ │ │ │ -_g_t_s_a_m_:_:_d_o_t │ │ │ │ │ -double dot(const V1 &a, const V2 &b) │ │ │ │ │ -Dot product. │ │ │ │ │ -DDeeffiinniittiioonn Vector.h:195 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ -parameters for the conjugate gradient method │ │ │ │ │ -DDeeffiinniittiioonn ConjugateGradientSolver.h:29 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_I_t_e_r_a_t_i_v_e_S_o_l_v_e_r │ │ │ │ │ -Base class for Iterative Solvers like SubgraphSolver. │ │ │ │ │ -DDeeffiinniittiioonn IterativeSolver.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_I_n_f_o │ │ │ │ │ -Handy data structure for iterative solvers. │ │ │ │ │ -DDeeffiinniittiioonn IterativeSolver.h:126 │ │ │ │ │ -_g_t_s_a_m_:_:_P_C_G_S_o_l_v_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ -Parameters for PCG. │ │ │ │ │ -DDeeffiinniittiioonn PCGSolver.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_P_C_G_S_o_l_v_e_r │ │ │ │ │ -A virtual base class for the preconditioned conjugate gradient solver. │ │ │ │ │ -DDeeffiinniittiioonn PCGSolver.h:62 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_S_y_s_t_e_m │ │ │ │ │ -System class needed for calling preconditionedConjugateGradient. │ │ │ │ │ -DDeeffiinniittiioonn PCGSolver.h:89 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ -DDeeffiinniittiioonn Preconditioner.h:24 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_c_o_n_d_i_t_i_o_n_e_r │ │ │ │ │ -DDeeffiinniittiioonn Preconditioner.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ -with a unique integer... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -The Factor::error simply extracts the. │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ +Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor │ │ │ │ │ +symbol GTSAM_USE_QUATERNIO... │ │ │ │ │ +DDeeffiinniittiioonn Rot3.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_3_:_:_t_r_a_n_s_p_o_s_e │ │ │ │ │ +Matrix3 transpose() const │ │ │ │ │ +Return 3*3 transpose (inverse) rotation matrix. │ │ │ │ │ +DDeeffiinniittiioonn Rot3M.cpp:144 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_G_a_u_s_s_i_a_n_:_:_C_o_v_a_r_i_a_n_c_e │ │ │ │ │ +static shared_ptr Covariance(const Matrix &covariance, bool smart=true) │ │ │ │ │ +A Gaussian noise model created by specifying a covariance matrix. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.cpp:117 │ │ │ │ │ +_g_t_s_a_m_:_:_S_a_m_p_l_e_r │ │ │ │ │ +Sampling structure that keeps internal random number generators for diagonal │ │ │ │ │ +distributions specified ... │ │ │ │ │ +DDeeffiinniittiioonn Sampler.h:31 │ │ │ │ │ +_g_t_s_a_m_:_:_S_a_m_p_l_e_r_:_:_s_a_m_p_l_e │ │ │ │ │ +Vector sample() const │ │ │ │ │ +sample from distribution │ │ │ │ │ +DDeeffiinniittiioonn Sampler.cpp:59 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ +PreintegratedCombinedMeasurements integrates the IMU measurements (rotation │ │ │ │ │ +rates and accelerations) ... │ │ │ │ │ +DDeeffiinniittiioonn CombinedImuFactor.h:129 │ │ │ │ │ +_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s │ │ │ │ │ +DDeeffiinniittiioonn ImuBias.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_:_:_g_y_r_o_s_c_o_p_e │ │ │ │ │ +const Vector3 & gyroscope() const │ │ │ │ │ +get gyroscope bias │ │ │ │ │ +DDeeffiinniittiioonn ImuBias.h:69 │ │ │ │ │ +_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_:_:_a_c_c_e_l_e_r_o_m_e_t_e_r │ │ │ │ │ +const Vector3 & accelerometer() const │ │ │ │ │ +get accelerometer bias │ │ │ │ │ +DDeeffiinniittiioonn ImuBias.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ +PreintegratedImuMeasurements accumulates (integrates) the IMU measurements │ │ │ │ │ +(rotation rates and accele... │ │ │ │ │ +DDeeffiinniittiioonn ImuFactor.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_N_a_v_S_t_a_t_e │ │ │ │ │ +Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does │ │ │ │ │ +not make sense to make... │ │ │ │ │ +DDeeffiinniittiioonn NavState.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_S_c_e_n_a_r_i_o │ │ │ │ │ +Simple trajectory simulator. │ │ │ │ │ +DDeeffiinniittiioonn Scenario.h:25 │ │ │ │ │ +_g_t_s_a_m_:_:_S_c_e_n_a_r_i_o_:_:_o_m_e_g_a___b │ │ │ │ │ +virtual Vector3 omega_b(double t) const =0 │ │ │ │ │ +angular velocity in body frame │ │ │ │ │ +_g_t_s_a_m_:_:_S_c_e_n_a_r_i_o_R_u_n_n_e_r │ │ │ │ │ +DDeeffiinniittiioonn ScenarioRunner.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_C_o_m_b_i_n_e_d_S_c_e_n_a_r_i_o_R_u_n_n_e_r │ │ │ │ │ +DDeeffiinniittiioonn ScenarioRunner.h:114 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _l_i_n_e_a_r │ │ │ │ │ - * PPCCGGSSoollvveerr..hh │ │ │ │ │ + * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ + * _S_c_e_n_a_r_i_o_R_u_n_n_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00893_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01304_source.html │ │ │ │┄ Files 92% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/RegularHessianFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/ProjectionFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
RegularHessianFactor.h
│ │ │ │ +
ProjectionFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/linear/HessianFactor.h>
│ │ │ │ -
22#include <gtsam/linear/RegularJacobianFactor.h>
│ │ │ │ -
23#include <vector>
│ │ │ │ -
24
│ │ │ │ -
25namespace gtsam {
│ │ │ │ -
26
│ │ │ │ -
27template<size_t D>
│ │ │ │ -
│ │ │ │ -
28class RegularHessianFactor: public HessianFactor {
│ │ │ │ +
21#pragma once
│ │ │ │ +
22
│ │ │ │ +
23#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
24#include <gtsam/geometry/PinholeCamera.h>
│ │ │ │ +
25#include <gtsam/geometry/Pose3.h>
│ │ │ │ +
26#include <gtsam/geometry/Point3.h>
│ │ │ │ +
27#include <gtsam/geometry/Cal3_S2.h>
│ │ │ │ +
28#include <boost/optional.hpp>
│ │ │ │
29
│ │ │ │ -
30public:
│ │ │ │ +
30namespace gtsam {
│ │ │ │
31
│ │ │ │ -
32 typedef Eigen::Matrix<double, D, 1> VectorD;
│ │ │ │ -
33 typedef Eigen::Matrix<double, D, D> MatrixD;
│ │ │ │ -
34
│ │ │ │ -
│ │ │ │ -
39 RegularHessianFactor(const KeyVector& js,
│ │ │ │ -
40 const std::vector<Matrix>& Gs, const std::vector<Vector>& gs, double f) :
│ │ │ │ -
41 HessianFactor(js, Gs, gs, f) {
│ │ │ │ -
42 checkInvariants();
│ │ │ │ -
43 }
│ │ │ │ -
│ │ │ │ -
44
│ │ │ │ -
│ │ │ │ -
49 RegularHessianFactor(Key j1, Key j2, const MatrixD& G11, const MatrixD& G12,
│ │ │ │ -
50 const VectorD& g1, const MatrixD& G22, const VectorD& g2, double f) :
│ │ │ │ -
51 HessianFactor(j1, j2, G11, G12, g1, G22, g2, f) {
│ │ │ │ -
52 }
│ │ │ │ -
│ │ │ │ +
38 template <class POSE = Pose3, class LANDMARK = Point3,
│ │ │ │ +
39 class CALIBRATION = Cal3_S2>
│ │ │ │ +
│ │ │ │ +
40 class GenericProjectionFactor: public NoiseModelFactorN<POSE, LANDMARK> {
│ │ │ │ +
41 protected:
│ │ │ │ +
42
│ │ │ │ +
43 // Keep a copy of measurement and calibration for I/O
│ │ │ │ +
44 Point2 measured_;
│ │ │ │ +
45 boost::shared_ptr<CALIBRATION> K_;
│ │ │ │ +
46 boost::optional<POSE> body_P_sensor_;
│ │ │ │ +
47
│ │ │ │ +
48 // verbosity handling for Cheirality Exceptions
│ │ │ │ +
49 bool throwCheirality_;
│ │ │ │ +
50 bool verboseCheirality_;
│ │ │ │ +
51
│ │ │ │ +
52 public:
│ │ │ │
53
│ │ │ │ -
│ │ │ │ -
58 RegularHessianFactor(Key j1, Key j2, Key j3,
│ │ │ │ -
59 const MatrixD& G11, const MatrixD& G12, const MatrixD& G13, const VectorD& g1,
│ │ │ │ -
60 const MatrixD& G22, const MatrixD& G23, const VectorD& g2,
│ │ │ │ -
61 const MatrixD& G33, const VectorD& g3, double f) :
│ │ │ │ -
62 HessianFactor(j1, j2, j3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f) {
│ │ │ │ -
63 }
│ │ │ │ -
│ │ │ │ -
64
│ │ │ │ -
67 template<typename KEYS>
│ │ │ │ -
│ │ │ │ -
68 RegularHessianFactor(const KEYS& keys,
│ │ │ │ -
69 const SymmetricBlockMatrix& augmentedInformation) :
│ │ │ │ -
70 HessianFactor(keys, augmentedInformation) {
│ │ │ │ -
71 checkInvariants();
│ │ │ │ -
72 }
│ │ │ │ -
│ │ │ │ -
73
│ │ │ │ -
│ │ │ │ -
75 RegularHessianFactor(const RegularJacobianFactor<D>& jf)
│ │ │ │ -
76 : HessianFactor(jf) {}
│ │ │ │ -
│ │ │ │ -
77
│ │ │ │ -
│ │ │ │ -
79 RegularHessianFactor(const GaussianFactorGraph& factors,
│ │ │ │ -
80 const Scatter& scatter)
│ │ │ │ -
81 : HessianFactor(factors, scatter) {
│ │ │ │ -
82 checkInvariants();
│ │ │ │ -
83 }
│ │ │ │ -
│ │ │ │ -
84
│ │ │ │ -
│ │ │ │ -
86 RegularHessianFactor(const GaussianFactorGraph& factors)
│ │ │ │ -
87 : HessianFactor(factors) {
│ │ │ │ -
88 checkInvariants();
│ │ │ │ -
89 }
│ │ │ │ -
│ │ │ │ -
90
│ │ │ │ -
91private:
│ │ │ │ -
92
│ │ │ │ -
94 void checkInvariants() {
│ │ │ │ -
95 if (info_.cols() != 1 + (info_.nBlocks()-1) * (DenseIndex)D)
│ │ │ │ -
96 throw std::invalid_argument(
│ │ │ │ -
97 "RegularHessianFactor constructor was given non-regular factors");
│ │ │ │ -
98 }
│ │ │ │ -
99
│ │ │ │ -
100 // Use Eigen magic to access raw memory
│ │ │ │ -
101 typedef Eigen::Map<VectorD> DMap;
│ │ │ │ -
102 typedef Eigen::Map<const VectorD> ConstDMap;
│ │ │ │ -
103
│ │ │ │ -
104 // Scratch space for multiplyHessianAdd
│ │ │ │ -
105 // According to link below this is thread-safe.
│ │ │ │ -
106 // http://stackoverflow.com/questions/11160964/multiple-copies-of-the-same-object-c-thread-safe
│ │ │ │ -
107 mutable std::vector<VectorD> y_;
│ │ │ │ -
108
│ │ │ │ -
109public:
│ │ │ │ +
55 typedef NoiseModelFactorN<POSE, LANDMARK> Base;
│ │ │ │ +
56
│ │ │ │ +
58 typedef GenericProjectionFactor<POSE, LANDMARK, CALIBRATION> This;
│ │ │ │ +
59
│ │ │ │ +
61 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
62
│ │ │ │ +
│ │ │ │ +
64 GenericProjectionFactor() :
│ │ │ │ +
65 measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) {
│ │ │ │ +
66 }
│ │ │ │ +
│ │ │ │ +
67
│ │ │ │ +
│ │ │ │ +
78 GenericProjectionFactor(const Point2& measured, const SharedNoiseModel& model,
│ │ │ │ +
79 Key poseKey, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
│ │ │ │ +
80 boost::optional<POSE> body_P_sensor = boost::none) :
│ │ │ │ +
81 Base(model, poseKey, pointKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
│ │ │ │ +
82 throwCheirality_(false), verboseCheirality_(false) {}
│ │ │ │ +
│ │ │ │ +
83
│ │ │ │ +
│ │ │ │ +
96 GenericProjectionFactor(const Point2& measured, const SharedNoiseModel& model,
│ │ │ │ +
97 Key poseKey, Key pointKey, const boost::shared_ptr<CALIBRATION>& K,
│ │ │ │ +
98 bool throwCheirality, bool verboseCheirality,
│ │ │ │ +
99 boost::optional<POSE> body_P_sensor = boost::none) :
│ │ │ │ +
100 Base(model, poseKey, pointKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
│ │ │ │ +
101 throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
│ │ │ │ +
│ │ │ │ +
102
│ │ │ │ +
104 ~GenericProjectionFactor() override {}
│ │ │ │ +
105
│ │ │ │ +
│ │ │ │ +
107 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
108 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ +
109 gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
│ │ │ │ +
│ │ │ │
110
│ │ │ │ -
│ │ │ │ -
112 void multiplyHessianAdd(double alpha, const VectorValues& x,
│ │ │ │ -
113 VectorValues& y) const override {
│ │ │ │ -
114 HessianFactor::multiplyHessianAdd(alpha, x, y);
│ │ │ │ -
115 }
│ │ │ │ -
│ │ │ │ -
116
│ │ │ │ -
│ │ │ │ -
118 void multiplyHessianAdd(double alpha, const double* x,
│ │ │ │ -
119 double* yvalues) const {
│ │ │ │ -
120 // Create a vector of temporary y_ values, corresponding to rows i
│ │ │ │ -
121 y_.resize(size());
│ │ │ │ -
122 for(VectorD & yi: y_)
│ │ │ │ -
123 yi.setZero();
│ │ │ │ -
124
│ │ │ │ -
125 // Accessing the VectorValues one by one is expensive
│ │ │ │ -
126 // So we will loop over columns to access x only once per column
│ │ │ │ -
127 // And fill the above temporary y_ values, to be added into yvalues after
│ │ │ │ -
128 VectorD xj(D);
│ │ │ │ -
129 for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
│ │ │ │ -
130 Key key = keys_[j];
│ │ │ │ -
131 const double* xj = x + key * D;
│ │ │ │ -
132 DenseIndex i = 0;
│ │ │ │ -
133 for (; i < j; ++i)
│ │ │ │ -
134 y_[i] += info_.aboveDiagonalBlock(i, j) * ConstDMap(xj);
│ │ │ │ -
135 // blocks on the diagonal are only half
│ │ │ │ -
136 y_[i] += info_.diagonalBlock(j) * ConstDMap(xj);
│ │ │ │ -
137 // for below diagonal, we take transpose block from upper triangular part
│ │ │ │ -
138 for (i = j + 1; i < (DenseIndex) size(); ++i)
│ │ │ │ -
139 y_[i] += info_.aboveDiagonalBlock(j, i).transpose() * ConstDMap(xj);
│ │ │ │ -
140 }
│ │ │ │ -
141
│ │ │ │ -
142 // copy to yvalues
│ │ │ │ -
143 for (DenseIndex i = 0; i < (DenseIndex) size(); ++i) {
│ │ │ │ -
144 Key key = keys_[i];
│ │ │ │ -
145 DMap(yvalues + key * D) += alpha * y_[i];
│ │ │ │ -
146 }
│ │ │ │ -
147 }
│ │ │ │ -
│ │ │ │ -
148
│ │ │ │ -
│ │ │ │ -
150 void multiplyHessianAdd(double alpha, const double* x, double* yvalues,
│ │ │ │ -
151 std::vector<size_t> offsets) const {
│ │ │ │ -
152
│ │ │ │ -
153 // Create a vector of temporary y_ values, corresponding to rows i
│ │ │ │ -
154 y_.resize(size());
│ │ │ │ -
155 for(VectorD & yi: y_)
│ │ │ │ -
156 yi.setZero();
│ │ │ │ -
157
│ │ │ │ -
158 // Accessing the VectorValues one by one is expensive
│ │ │ │ -
159 // So we will loop over columns to access x only once per column
│ │ │ │ -
160 // And fill the above temporary y_ values, to be added into yvalues after
│ │ │ │ -
161 for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
│ │ │ │ -
162 DenseIndex i = 0;
│ │ │ │ -
163 for (; i < j; ++i)
│ │ │ │ -
164 y_[i] += info_.aboveDiagonalBlock(i, j)
│ │ │ │ -
165 * ConstDMap(x + offsets[keys_[j]],
│ │ │ │ -
166 offsets[keys_[j] + 1] - offsets[keys_[j]]);
│ │ │ │ -
167 // blocks on the diagonal are only half
│ │ │ │ -
168 y_[i] += info_.diagonalBlock(j)
│ │ │ │ -
169 * ConstDMap(x + offsets[keys_[j]],
│ │ │ │ -
170 offsets[keys_[j] + 1] - offsets[keys_[j]]);
│ │ │ │ -
171 // for below diagonal, we take transpose block from upper triangular part
│ │ │ │ -
172 for (i = j + 1; i < (DenseIndex) size(); ++i)
│ │ │ │ -
173 y_[i] += info_.aboveDiagonalBlock(j, i).transpose()
│ │ │ │ -
174 * ConstDMap(x + offsets[keys_[j]],
│ │ │ │ -
175 offsets[keys_[j] + 1] - offsets[keys_[j]]);
│ │ │ │ -
176 }
│ │ │ │ -
177
│ │ │ │ -
178 // copy to yvalues
│ │ │ │ -
179 for (DenseIndex i = 0; i < (DenseIndex) size(); ++i)
│ │ │ │ -
180 DMap(yvalues + offsets[keys_[i]],
│ │ │ │ -
181 offsets[keys_[i] + 1] - offsets[keys_[i]]) += alpha * y_[i];
│ │ │ │ -
182 }
│ │ │ │ -
│ │ │ │ -
183
│ │ │ │ -
│ │ │ │ -
185 void hessianDiagonal(double* d) const override {
│ │ │ │ -
186
│ │ │ │ -
187 // Loop over all variables in the factor
│ │ │ │ -
188 for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) {
│ │ │ │ -
189 Key j = keys_[pos];
│ │ │ │ -
190 // Get the diagonal block, and insert its diagonal
│ │ │ │ -
191 DMap(d + D * j) += info_.diagonal(pos);
│ │ │ │ -
192 }
│ │ │ │ -
193 }
│ │ │ │ -
│ │ │ │ -
194
│ │ │ │ -
│ │ │ │ -
196 void gradientAtZero(double* d) const override {
│ │ │ │ -
197
│ │ │ │ -
198 // Loop over all variables in the factor
│ │ │ │ -
199 for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) {
│ │ │ │ -
200 Key j = keys_[pos];
│ │ │ │ -
201 // Get the diagonal block, and insert its diagonal
│ │ │ │ -
202 DMap(d + D * j) -= info_.aboveDiagonalBlock(pos, size());;
│ │ │ │ -
203 }
│ │ │ │ -
204 }
│ │ │ │ -
│ │ │ │ -
205
│ │ │ │ -
206 /* ************************************************************************* */
│ │ │ │ -
207
│ │ │ │ -
208};
│ │ │ │ -
│ │ │ │ -
209// end class RegularHessianFactor
│ │ │ │ -
210
│ │ │ │ -
211// traits
│ │ │ │ -
│ │ │ │ -
212template<size_t D> struct traits<RegularHessianFactor<D> > : public Testable<
│ │ │ │ -
213 RegularHessianFactor<D> > {
│ │ │ │ -
214};
│ │ │ │ -
│ │ │ │ -
215
│ │ │ │ -
216}
│ │ │ │ -
217
│ │ │ │ -
RegularJacobianFactor.h
JacobianFactor class with fixed sized blcoks.
│ │ │ │ -
HessianFactor.h
Contains the HessianFactor class, a general quadratic factor.
│ │ │ │ +
│ │ │ │ +
116 void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ +
117 std::cout << s << "GenericProjectionFactor, z = ";
│ │ │ │ +
118 traits<Point2>::Print(measured_);
│ │ │ │ +
119 if(this->body_P_sensor_)
│ │ │ │ +
120 this->body_P_sensor_->print(" sensor pose in body frame: ");
│ │ │ │ +
121 Base::print("", keyFormatter);
│ │ │ │ +
122 }
│ │ │ │ +
│ │ │ │ +
123
│ │ │ │ +
│ │ │ │ +
125 bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
│ │ │ │ +
126 const This *e = dynamic_cast<const This*>(&p);
│ │ │ │ +
127 return e
│ │ │ │ +
128 && Base::equals(p, tol)
│ │ │ │ +
129 && traits<Point2>::Equals(this->measured_, e->measured_, tol)
│ │ │ │ +
130 && this->K_->equals(*e->K_, tol)
│ │ │ │ +
131 && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
│ │ │ │ +
132 }
│ │ │ │ +
│ │ │ │ +
133
│ │ │ │ +
│ │ │ │ +
135 Vector evaluateError(const Pose3& pose, const Point3& point,
│ │ │ │ +
136 boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
│ │ │ │ +
137 try {
│ │ │ │ +
138 if(body_P_sensor_) {
│ │ │ │ +
139 if(H1) {
│ │ │ │ +
140 gtsam::Matrix H0;
│ │ │ │ +
141 PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
│ │ │ │ +
142 Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_);
│ │ │ │ +
143 *H1 = *H1 * H0;
│ │ │ │ +
144 return reprojectionError;
│ │ │ │ +
145 } else {
│ │ │ │ +
146 PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
│ │ │ │ +
147 return camera.project(point, H1, H2, boost::none) - measured_;
│ │ │ │ +
148 }
│ │ │ │ +
149 } else {
│ │ │ │ +
150 PinholeCamera<CALIBRATION> camera(pose, *K_);
│ │ │ │ +
151 return camera.project(point, H1, H2, boost::none) - measured_;
│ │ │ │ +
152 }
│ │ │ │ +
153 } catch( CheiralityException& e) {
│ │ │ │ +
154 if (H1) *H1 = Matrix::Zero(2,6);
│ │ │ │ +
155 if (H2) *H2 = Matrix::Zero(2,3);
│ │ │ │ +
156 if (verboseCheirality_)
│ │ │ │ +
157 std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
│ │ │ │ +
158 " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
│ │ │ │ +
159 if (throwCheirality_)
│ │ │ │ +
160 throw CheiralityException(this->key2());
│ │ │ │ +
161 }
│ │ │ │ +
162 return Vector2::Constant(2.0 * K_->fx());
│ │ │ │ +
163 }
│ │ │ │ +
│ │ │ │ +
164
│ │ │ │ +
│ │ │ │ +
166 const Point2& measured() const {
│ │ │ │ +
167 return measured_;
│ │ │ │ +
168 }
│ │ │ │ +
│ │ │ │ +
169
│ │ │ │ +
│ │ │ │ +
171 const boost::shared_ptr<CALIBRATION> calibration() const {
│ │ │ │ +
172 return K_;
│ │ │ │ +
173 }
│ │ │ │ +
│ │ │ │ +
174
│ │ │ │ +
│ │ │ │ +
176 const boost::optional<POSE>& body_P_sensor() const {
│ │ │ │ +
177 return body_P_sensor_;
│ │ │ │ +
178 }
│ │ │ │ +
│ │ │ │ +
179
│ │ │ │ +
181 inline bool verboseCheirality() const { return verboseCheirality_; }
│ │ │ │ +
182
│ │ │ │ +
184 inline bool throwCheirality() const { return throwCheirality_; }
│ │ │ │ +
185
│ │ │ │ +
186 private:
│ │ │ │ +
187
│ │ │ │ +
189 friend class boost::serialization::access;
│ │ │ │ +
190 template<class ARCHIVE>
│ │ │ │ +
191 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
192 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
193 ar & BOOST_SERIALIZATION_NVP(measured_);
│ │ │ │ +
194 ar & BOOST_SERIALIZATION_NVP(K_);
│ │ │ │ +
195 ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
│ │ │ │ +
196 ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
│ │ │ │ +
197 ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
│ │ │ │ +
198 }
│ │ │ │ +
199
│ │ │ │ +
200 public:
│ │ │ │ +
201 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
202};
│ │ │ │ +
│ │ │ │ +
203
│ │ │ │ +
205 template<class POSE, class LANDMARK, class CALIBRATION>
│ │ │ │ +
│ │ │ │ +
206 struct traits<GenericProjectionFactor<POSE, LANDMARK, CALIBRATION> > :
│ │ │ │ +
207 public Testable<GenericProjectionFactor<POSE, LANDMARK, CALIBRATION> > {
│ │ │ │ +
208 };
│ │ │ │ +
│ │ │ │ +
209
│ │ │ │ +
210} // \ namespace gtsam
│ │ │ │ +
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ +
Pose3.h
3D Pose
│ │ │ │ +
Point3.h
3D Point
│ │ │ │ +
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
│ │ │ │ +
PinholeCamera.h
Base class for all pinhole cameras.
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │ +
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │ +
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ +
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
│ │ │ │ -
gtsam::SymmetricBlockMatrix::diagonal
Vector diagonal(DenseIndex J) const
Get the diagonal of the J'th diagonal block.
Definition SymmetricBlockMatrix.h:145
│ │ │ │ -
gtsam::SymmetricBlockMatrix::cols
DenseIndex cols() const
Column size.
Definition SymmetricBlockMatrix.h:117
│ │ │ │ -
gtsam::SymmetricBlockMatrix::aboveDiagonalBlock
constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
Get block above the diagonal (I, J).
Definition SymmetricBlockMatrix.h:150
│ │ │ │ -
gtsam::SymmetricBlockMatrix::diagonalBlock
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J'th diagonal block as a self adjoint view.
Definition SymmetricBlockMatrix.h:135
│ │ │ │ -
gtsam::SymmetricBlockMatrix::nBlocks
DenseIndex nBlocks() const
Block count.
Definition SymmetricBlockMatrix.h:120
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
│ │ │ │ -
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
│ │ │ │ -
gtsam::Factor::size
size_t size() const
Definition Factor.h:157
│ │ │ │ -
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ -
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition HessianFactor.h:101
│ │ │ │ -
gtsam::HessianFactor::augmentedInformation
Matrix augmentedInformation() const override
Return the augmented information matrix represented by this GaussianFactor.
Definition HessianFactor.cpp:282
│ │ │ │ -
gtsam::HessianFactor::info_
SymmetricBlockMatrix info_
The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'H[x -1].
Definition HessianFactor.h:104
│ │ │ │ -
gtsam::HessianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
y += alpha * A'*A*x
Definition HessianFactor.cpp:391
│ │ │ │ -
gtsam::RegularHessianFactor
Definition RegularHessianFactor.h:28
│ │ │ │ -
gtsam::RegularHessianFactor::RegularHessianFactor
RegularHessianFactor(const GaussianFactorGraph &factors)
Construct from a GaussianFactorGraph.
Definition RegularHessianFactor.h:86
│ │ │ │ -
gtsam::RegularHessianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
y += alpha * A'*A*x
Definition RegularHessianFactor.h:112
│ │ │ │ -
gtsam::RegularHessianFactor::RegularHessianFactor
RegularHessianFactor(Key j1, Key j2, Key j3, const MatrixD &G11, const MatrixD &G12, const MatrixD &G13, const VectorD &g1, const MatrixD &G22, const MatrixD &G23, const VectorD &g2, const MatrixD &G33, const VectorD &g3, double f)
Construct a ternary factor.
Definition RegularHessianFactor.h:58
│ │ │ │ -
gtsam::RegularHessianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const double *x, double *yvalues, std::vector< size_t > offsets) const
Raw memory version, with offsets TODO document reasoning.
Definition RegularHessianFactor.h:150
│ │ │ │ -
gtsam::RegularHessianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const double *x, double *yvalues) const
y += alpha * A'*A*x
Definition RegularHessianFactor.h:118
│ │ │ │ -
gtsam::RegularHessianFactor::hessianDiagonal
void hessianDiagonal(double *d) const override
Return the diagonal of the Hessian for this factor (raw memory version)
Definition RegularHessianFactor.h:185
│ │ │ │ -
gtsam::RegularHessianFactor::RegularHessianFactor
RegularHessianFactor(const KEYS &keys, const SymmetricBlockMatrix &augmentedInformation)
Constructor with an arbitrary number of keys and with the augmented information matrix specified as a...
Definition RegularHessianFactor.h:68
│ │ │ │ -
gtsam::RegularHessianFactor::gradientAtZero
void gradientAtZero(double *d) const override
Add gradient at zero to d TODO: is it really the goal to add ??
Definition RegularHessianFactor.h:196
│ │ │ │ -
gtsam::RegularHessianFactor::RegularHessianFactor
RegularHessianFactor(const GaussianFactorGraph &factors, const Scatter &scatter)
Construct from a GaussianFactorGraph.
Definition RegularHessianFactor.h:79
│ │ │ │ -
gtsam::RegularHessianFactor::RegularHessianFactor
RegularHessianFactor(const KeyVector &js, const std::vector< Matrix > &Gs, const std::vector< Vector > &gs, double f)
Construct an n-way factor.
Definition RegularHessianFactor.h:39
│ │ │ │ -
gtsam::RegularHessianFactor::RegularHessianFactor
RegularHessianFactor(Key j1, Key j2, const MatrixD &G11, const MatrixD &G12, const VectorD &g1, const MatrixD &G22, const VectorD &g2, double f)
Construct a binary factor.
Definition RegularHessianFactor.h:49
│ │ │ │ -
gtsam::RegularHessianFactor::RegularHessianFactor
RegularHessianFactor(const RegularJacobianFactor< D > &jf)
Construct from RegularJacobianFactor.
Definition RegularHessianFactor.h:75
│ │ │ │ -
gtsam::RegularJacobianFactor
JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator.
Definition RegularJacobianFactor.h:32
│ │ │ │ -
gtsam::Scatter
Scatter is an intermediate data structure used when building a HessianFactor incrementally,...
Definition Scatter.h:49
│ │ │ │ -
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
gtsam::CheiralityException
Definition CalibratedCamera.h:32
│ │ │ │ +
gtsam::PinholeCamera
A pinhole camera class that has a Pose3 and a Calibration.
Definition PinholeCamera.h:33
│ │ │ │ +
gtsam::PinholeBaseK::project
Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > Dcal=boost::none) const
project a 3D point from world coordinates into the image
Definition PinholePose.h:118
│ │ │ │ +
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ +
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
│ │ │ │ +
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
│ │ │ │ +
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ +
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ +
gtsam::GenericProjectionFactor
Non-linear factor for a constraint derived from a 2D measurement.
Definition ProjectionFactor.h:40
│ │ │ │ +
gtsam::GenericProjectionFactor::verboseCheirality
bool verboseCheirality() const
return verbosity
Definition ProjectionFactor.h:181
│ │ │ │ +
gtsam::GenericProjectionFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition ProjectionFactor.h:116
│ │ │ │ +
gtsam::GenericProjectionFactor::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition ProjectionFactor.h:125
│ │ │ │ +
gtsam::GenericProjectionFactor::measured_
Point2 measured_
2D measurement
Definition ProjectionFactor.h:44
│ │ │ │ +
gtsam::GenericProjectionFactor::This
GenericProjectionFactor< POSE, LANDMARK, CALIBRATION > This
shorthand for this class
Definition ProjectionFactor.h:58
│ │ │ │ +
gtsam::GenericProjectionFactor::measured
const Point2 & measured() const
return the measurement
Definition ProjectionFactor.h:166
│ │ │ │ +
gtsam::GenericProjectionFactor::body_P_sensor_
boost::optional< POSE > body_P_sensor_
The pose of the sensor in the body frame.
Definition ProjectionFactor.h:46
│ │ │ │ +
gtsam::GenericProjectionFactor::K_
boost::shared_ptr< CALIBRATION > K_
shared pointer to calibration object
Definition ProjectionFactor.h:45
│ │ │ │ +
gtsam::GenericProjectionFactor::~GenericProjectionFactor
~GenericProjectionFactor() override
Virtual destructor.
Definition ProjectionFactor.h:104
│ │ │ │ +
gtsam::GenericProjectionFactor::evaluateError
Vector evaluateError(const Pose3 &pose, const Point3 &point, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Evaluate error h(x)-z and optionally derivatives.
Definition ProjectionFactor.h:135
│ │ │ │ +
gtsam::GenericProjectionFactor::GenericProjectionFactor
GenericProjectionFactor()
Default constructor.
Definition ProjectionFactor.h:64
│ │ │ │ +
gtsam::GenericProjectionFactor::throwCheirality_
bool throwCheirality_
If true, rethrows Cheirality exceptions (default: false)
Definition ProjectionFactor.h:49
│ │ │ │ +
gtsam::GenericProjectionFactor::Base
NoiseModelFactorN< POSE, LANDMARK > Base
shorthand for base class type
Definition ProjectionFactor.h:55
│ │ │ │ +
gtsam::GenericProjectionFactor::body_P_sensor
const boost::optional< POSE > & body_P_sensor() const
return the (optional) sensor pose with respect to the vehicle frame
Definition ProjectionFactor.h:176
│ │ │ │ +
gtsam::GenericProjectionFactor::throwCheirality
bool throwCheirality() const
return flag for throwing cheirality exceptions
Definition ProjectionFactor.h:184
│ │ │ │ +
gtsam::GenericProjectionFactor::GenericProjectionFactor
GenericProjectionFactor(const Point2 &measured, const SharedNoiseModel &model, Key poseKey, Key pointKey, const boost::shared_ptr< CALIBRATION > &K, bool throwCheirality, bool verboseCheirality, boost::optional< POSE > body_P_sensor=boost::none)
Constructor with exception-handling flags TODO: Mark argument order standard (keys,...
Definition ProjectionFactor.h:96
│ │ │ │ +
gtsam::GenericProjectionFactor::access
friend class boost::serialization::access
Serialization function.
Definition ProjectionFactor.h:189
│ │ │ │ +
gtsam::GenericProjectionFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition ProjectionFactor.h:107
│ │ │ │ +
gtsam::GenericProjectionFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition ProjectionFactor.h:61
│ │ │ │ +
gtsam::GenericProjectionFactor::verboseCheirality_
bool verboseCheirality_
If true, prints text for Cheirality exceptions (default: false)
Definition ProjectionFactor.h:50
│ │ │ │ +
gtsam::GenericProjectionFactor::GenericProjectionFactor
GenericProjectionFactor(const Point2 &measured, const SharedNoiseModel &model, Key poseKey, Key pointKey, const boost::shared_ptr< CALIBRATION > &K, boost::optional< POSE > body_P_sensor=boost::none)
Constructor TODO: Mark argument order standard (keys, measurement, parameters)
Definition ProjectionFactor.h:78
│ │ │ │ +
gtsam::GenericProjectionFactor::calibration
const boost::shared_ptr< CALIBRATION > calibration() const
return the calibration object
Definition ProjectionFactor.h:171
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,356 +1,350 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -RegularHessianFactor.h │ │ │ │ │ +ProjectionFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_H_e_s_s_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ -23#include │ │ │ │ │ -24 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ -26 │ │ │ │ │ -27template │ │ │ │ │ -_2_8class _R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r: public _H_e_s_s_i_a_n_F_a_c_t_o_r { │ │ │ │ │ +21#pragma once │ │ │ │ │ +22 │ │ │ │ │ +23#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_i_n_h_o_l_e_C_a_m_e_r_a_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_3_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_3_._h> │ │ │ │ │ +27#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3___S_2_._h> │ │ │ │ │ +28#include │ │ │ │ │ 29 │ │ │ │ │ -30public: │ │ │ │ │ +30namespace _g_t_s_a_m { │ │ │ │ │ 31 │ │ │ │ │ -32 typedef Eigen::Matrix VectorD; │ │ │ │ │ -33 typedef Eigen::Matrix MatrixD; │ │ │ │ │ -34 │ │ │ │ │ -_3_9 _R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r(const _K_e_y_V_e_c_t_o_r& js, │ │ │ │ │ -40 const std::vector& Gs, const std::vector& gs, double f) : │ │ │ │ │ -41 _H_e_s_s_i_a_n_F_a_c_t_o_r(js, Gs, gs, f) { │ │ │ │ │ -42 checkInvariants(); │ │ │ │ │ -43 } │ │ │ │ │ -44 │ │ │ │ │ -_4_9 _R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r(_K_e_y j1, _K_e_y j2, const MatrixD& G11, const MatrixD& G12, │ │ │ │ │ -50 const VectorD& g1, const MatrixD& G22, const VectorD& g2, double f) : │ │ │ │ │ -51 _H_e_s_s_i_a_n_F_a_c_t_o_r(j1, j2, G11, G12, g1, G22, g2, f) { │ │ │ │ │ -52 } │ │ │ │ │ +38 template │ │ │ │ │ +_4_0 class _G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +41 protected: │ │ │ │ │ +42 │ │ │ │ │ +43 // Keep a copy of measurement and calibration for I/O │ │ │ │ │ +_4_4 _P_o_i_n_t_2 _m_e_a_s_u_r_e_d__; │ │ │ │ │ +_4_5 boost::shared_ptr _K__; │ │ │ │ │ +_4_6 boost::optional _b_o_d_y___P___s_e_n_s_o_r__; │ │ │ │ │ +47 │ │ │ │ │ +48 // verbosity handling for Cheirality Exceptions │ │ │ │ │ +_4_9 bool _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__; │ │ │ │ │ +_5_0 bool _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__; │ │ │ │ │ +51 │ │ │ │ │ +52 public: │ │ │ │ │ 53 │ │ │ │ │ -_5_8 _R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r(_K_e_y j1, _K_e_y j2, _K_e_y j3, │ │ │ │ │ -59 const MatrixD& G11, const MatrixD& G12, const MatrixD& G13, const VectorD& │ │ │ │ │ -g1, │ │ │ │ │ -60 const MatrixD& G22, const MatrixD& G23, const VectorD& g2, │ │ │ │ │ -61 const MatrixD& G33, const VectorD& g3, double f) : │ │ │ │ │ -62 _H_e_s_s_i_a_n_F_a_c_t_o_r(j1, j2, j3, G11, G12, G13, g1, G22, G23, g2, G33, g3, f) { │ │ │ │ │ -63 } │ │ │ │ │ -64 │ │ │ │ │ -67 template │ │ │ │ │ -_6_8 _R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r(const KEYS& _k_e_y_s, │ │ │ │ │ -69 const _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x& _a_u_g_m_e_n_t_e_d_I_n_f_o_r_m_a_t_i_o_n) : │ │ │ │ │ -70 _H_e_s_s_i_a_n_F_a_c_t_o_r(_k_e_y_s, _a_u_g_m_e_n_t_e_d_I_n_f_o_r_m_a_t_i_o_n) { │ │ │ │ │ -71 checkInvariants(); │ │ │ │ │ -72 } │ │ │ │ │ -73 │ │ │ │ │ -_7_5 _R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r(const _R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_<_D_>& jf) │ │ │ │ │ -76 : _H_e_s_s_i_a_n_F_a_c_t_o_r(jf) {} │ │ │ │ │ -77 │ │ │ │ │ -_7_9 _R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& factors, │ │ │ │ │ -80 const _S_c_a_t_t_e_r& scatter) │ │ │ │ │ -81 : _H_e_s_s_i_a_n_F_a_c_t_o_r(factors, scatter) { │ │ │ │ │ -82 checkInvariants(); │ │ │ │ │ -83 } │ │ │ │ │ -84 │ │ │ │ │ -_8_6 _R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& factors) │ │ │ │ │ -87 : _H_e_s_s_i_a_n_F_a_c_t_o_r(factors) { │ │ │ │ │ -88 checkInvariants(); │ │ │ │ │ -89 } │ │ │ │ │ -90 │ │ │ │ │ -91private: │ │ │ │ │ -92 │ │ │ │ │ -94 void checkInvariants() { │ │ │ │ │ -95 if (_i_n_f_o__._c_o_l_s() != 1 + (_i_n_f_o__._n_B_l_o_c_k_s()-1) * (_D_e_n_s_e_I_n_d_e_x)D) │ │ │ │ │ -96 throw std::invalid_argument( │ │ │ │ │ -97 "RegularHessianFactor constructor was given non-regular factors"); │ │ │ │ │ -98 } │ │ │ │ │ -99 │ │ │ │ │ -100 // Use Eigen magic to access raw memory │ │ │ │ │ -101 typedef Eigen::Map DMap; │ │ │ │ │ -102 typedef Eigen::Map ConstDMap; │ │ │ │ │ -103 │ │ │ │ │ -104 // Scratch space for multiplyHessianAdd │ │ │ │ │ -105 // According to link below this is thread-safe. │ │ │ │ │ -106 // http://stackoverflow.com/questions/11160964/multiple-copies-of-the-same- │ │ │ │ │ -object-c-thread-safe │ │ │ │ │ -107 mutable std::vector y_; │ │ │ │ │ -108 │ │ │ │ │ -109public: │ │ │ │ │ +_5_5 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_P_O_S_E_,_ _L_A_N_D_M_A_R_K_> _B_a_s_e; │ │ │ │ │ +56 │ │ │ │ │ +_5_8 typedef _G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_<_P_O_S_E_,_ _L_A_N_D_M_A_R_K_,_ _C_A_L_I_B_R_A_T_I_O_N_> _T_h_i_s; │ │ │ │ │ +59 │ │ │ │ │ +_6_1 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +62 │ │ │ │ │ +_6_4 _G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r() : │ │ │ │ │ +65 _m_e_a_s_u_r_e_d__(0, 0), _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__(false), _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__(false) { │ │ │ │ │ +66 } │ │ │ │ │ +67 │ │ │ │ │ +_7_8 _G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r(const _P_o_i_n_t_2& _m_e_a_s_u_r_e_d, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& │ │ │ │ │ +model, │ │ │ │ │ +79 _K_e_y poseKey, _K_e_y pointKey, const boost::shared_ptr& K, │ │ │ │ │ +80 boost::optional _b_o_d_y___P___s_e_n_s_o_r = boost::none) : │ │ │ │ │ +81 _B_a_s_e(model, poseKey, pointKey), _m_e_a_s_u_r_e_d__(_m_e_a_s_u_r_e_d), _K__(K), _b_o_d_y___P___s_e_n_s_o_r__ │ │ │ │ │ +(_b_o_d_y___P___s_e_n_s_o_r), │ │ │ │ │ +82 _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__(false), _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__(false) {} │ │ │ │ │ +83 │ │ │ │ │ +_9_6 _G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r(const _P_o_i_n_t_2& _m_e_a_s_u_r_e_d, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& │ │ │ │ │ +model, │ │ │ │ │ +97 _K_e_y poseKey, _K_e_y pointKey, const boost::shared_ptr& K, │ │ │ │ │ +98 bool _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y, bool _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y, │ │ │ │ │ +99 boost::optional _b_o_d_y___P___s_e_n_s_o_r = boost::none) : │ │ │ │ │ +100 _B_a_s_e(model, poseKey, pointKey), _m_e_a_s_u_r_e_d__(_m_e_a_s_u_r_e_d), _K__(K), _b_o_d_y___P___s_e_n_s_o_r__ │ │ │ │ │ +(_b_o_d_y___P___s_e_n_s_o_r), │ │ │ │ │ +101 _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__(_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y), _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__(_v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y) {} │ │ │ │ │ +102 │ │ │ │ │ +_1_0_4 _~_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r() override {} │ │ │ │ │ +105 │ │ │ │ │ +_1_0_7 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +108 return boost::static_pointer_cast( │ │ │ │ │ +109 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); } │ │ │ │ │ 110 │ │ │ │ │ -_1_1_2 void _m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d(double alpha, const _V_e_c_t_o_r_V_a_l_u_e_s& x, │ │ │ │ │ -113 _V_e_c_t_o_r_V_a_l_u_e_s& y) const override { │ │ │ │ │ -114 _H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d(alpha, x, y); │ │ │ │ │ -115 } │ │ │ │ │ -116 │ │ │ │ │ -_1_1_8 void _m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d(double alpha, const double* x, │ │ │ │ │ -119 double* yvalues) const { │ │ │ │ │ -120 // Create a vector of temporary y_ values, corresponding to rows i │ │ │ │ │ -121 y_.resize(_s_i_z_e()); │ │ │ │ │ -122 for(VectorD & yi: y_) │ │ │ │ │ -123 yi.setZero(); │ │ │ │ │ -124 │ │ │ │ │ -125 // Accessing the VectorValues one by one is expensive │ │ │ │ │ -126 // So we will loop over columns to access x only once per column │ │ │ │ │ -127 // And fill the above temporary y_ values, to be added into yvalues after │ │ │ │ │ -128 VectorD xj(D); │ │ │ │ │ -129 for (_D_e_n_s_e_I_n_d_e_x j = 0; j < (_D_e_n_s_e_I_n_d_e_x) _s_i_z_e(); ++j) { │ │ │ │ │ -130 _K_e_y key = _k_e_y_s__[j]; │ │ │ │ │ -131 const double* xj = x + key * D; │ │ │ │ │ -132 _D_e_n_s_e_I_n_d_e_x i = 0; │ │ │ │ │ -133 for (; i < j; ++i) │ │ │ │ │ -134 y_[i] += _i_n_f_o__._a_b_o_v_e_D_i_a_g_o_n_a_l_B_l_o_c_k(i, j) * ConstDMap(xj); │ │ │ │ │ -135 // blocks on the diagonal are only half │ │ │ │ │ -136 y_[i] += _i_n_f_o__._d_i_a_g_o_n_a_l_B_l_o_c_k(j) * ConstDMap(xj); │ │ │ │ │ -137 // for below diagonal, we take transpose block from upper triangular part │ │ │ │ │ -138 for (i = j + 1; i < (_D_e_n_s_e_I_n_d_e_x) _s_i_z_e(); ++i) │ │ │ │ │ -139 y_[i] += _i_n_f_o__._a_b_o_v_e_D_i_a_g_o_n_a_l_B_l_o_c_k(j, i).transpose() * ConstDMap(xj); │ │ │ │ │ -140 } │ │ │ │ │ -141 │ │ │ │ │ -142 // copy to yvalues │ │ │ │ │ -143 for (_D_e_n_s_e_I_n_d_e_x i = 0; i < (_D_e_n_s_e_I_n_d_e_x) _s_i_z_e(); ++i) { │ │ │ │ │ -144 _K_e_y key = _k_e_y_s__[i]; │ │ │ │ │ -145 DMap(yvalues + key * D) += alpha * y_[i]; │ │ │ │ │ -146 } │ │ │ │ │ -147 } │ │ │ │ │ -148 │ │ │ │ │ -_1_5_0 void _m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d(double alpha, const double* x, double* yvalues, │ │ │ │ │ -151 std::vector offsets) const { │ │ │ │ │ -152 │ │ │ │ │ -153 // Create a vector of temporary y_ values, corresponding to rows i │ │ │ │ │ -154 y_.resize(_s_i_z_e()); │ │ │ │ │ -155 for(VectorD & yi: y_) │ │ │ │ │ -156 yi.setZero(); │ │ │ │ │ -157 │ │ │ │ │ -158 // Accessing the VectorValues one by one is expensive │ │ │ │ │ -159 // So we will loop over columns to access x only once per column │ │ │ │ │ -160 // And fill the above temporary y_ values, to be added into yvalues after │ │ │ │ │ -161 for (_D_e_n_s_e_I_n_d_e_x j = 0; j < (_D_e_n_s_e_I_n_d_e_x) _s_i_z_e(); ++j) { │ │ │ │ │ -162 _D_e_n_s_e_I_n_d_e_x i = 0; │ │ │ │ │ -163 for (; i < j; ++i) │ │ │ │ │ -164 y_[i] += _i_n_f_o__._a_b_o_v_e_D_i_a_g_o_n_a_l_B_l_o_c_k(i, j) │ │ │ │ │ -165 * ConstDMap(x + offsets[_k_e_y_s__[j]], │ │ │ │ │ -166 offsets[_k_e_y_s__[j] + 1] - offsets[_k_e_y_s__[j]]); │ │ │ │ │ -167 // blocks on the diagonal are only half │ │ │ │ │ -168 y_[i] += _i_n_f_o__._d_i_a_g_o_n_a_l_B_l_o_c_k(j) │ │ │ │ │ -169 * ConstDMap(x + offsets[_k_e_y_s__[j]], │ │ │ │ │ -170 offsets[_k_e_y_s__[j] + 1] - offsets[_k_e_y_s__[j]]); │ │ │ │ │ -171 // for below diagonal, we take transpose block from upper triangular part │ │ │ │ │ -172 for (i = j + 1; i < (_D_e_n_s_e_I_n_d_e_x) _s_i_z_e(); ++i) │ │ │ │ │ -173 y_[i] += _i_n_f_o__._a_b_o_v_e_D_i_a_g_o_n_a_l_B_l_o_c_k(j, i).transpose() │ │ │ │ │ -174 * ConstDMap(x + offsets[_k_e_y_s__[j]], │ │ │ │ │ -175 offsets[_k_e_y_s__[j] + 1] - offsets[_k_e_y_s__[j]]); │ │ │ │ │ -176 } │ │ │ │ │ -177 │ │ │ │ │ -178 // copy to yvalues │ │ │ │ │ -179 for (_D_e_n_s_e_I_n_d_e_x i = 0; i < (_D_e_n_s_e_I_n_d_e_x) _s_i_z_e(); ++i) │ │ │ │ │ -180 DMap(yvalues + offsets[_k_e_y_s__[i]], │ │ │ │ │ -181 offsets[_k_e_y_s__[i] + 1] - offsets[_k_e_y_s__[i]]) += alpha * y_[i]; │ │ │ │ │ -182 } │ │ │ │ │ -183 │ │ │ │ │ -_1_8_5 void _h_e_s_s_i_a_n_D_i_a_g_o_n_a_l(double* d) const override { │ │ │ │ │ -186 │ │ │ │ │ -187 // Loop over all variables in the factor │ │ │ │ │ -188 for (_D_e_n_s_e_I_n_d_e_x pos = 0; pos < (_D_e_n_s_e_I_n_d_e_x) _s_i_z_e(); ++pos) { │ │ │ │ │ -189 _K_e_y j = _k_e_y_s__[pos]; │ │ │ │ │ -190 // Get the diagonal block, and insert its diagonal │ │ │ │ │ -191 DMap(d + D * j) += _i_n_f_o__._d_i_a_g_o_n_a_l(pos); │ │ │ │ │ -192 } │ │ │ │ │ -193 } │ │ │ │ │ -194 │ │ │ │ │ -_1_9_6 void _g_r_a_d_i_e_n_t_A_t_Z_e_r_o(double* d) const override { │ │ │ │ │ -197 │ │ │ │ │ -198 // Loop over all variables in the factor │ │ │ │ │ -199 for (_D_e_n_s_e_I_n_d_e_x pos = 0; pos < (_D_e_n_s_e_I_n_d_e_x) _s_i_z_e(); ++pos) { │ │ │ │ │ -200 _K_e_y j = _k_e_y_s__[pos]; │ │ │ │ │ -201 // Get the diagonal block, and insert its diagonal │ │ │ │ │ -202 DMap(d + D * j) -= _i_n_f_o__._a_b_o_v_e_D_i_a_g_o_n_a_l_B_l_o_c_k(pos, _s_i_z_e());; │ │ │ │ │ -203 } │ │ │ │ │ -204 } │ │ │ │ │ -205 │ │ │ │ │ -206 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -207 │ │ │ │ │ -208}; │ │ │ │ │ -209// end class RegularHessianFactor │ │ │ │ │ -210 │ │ │ │ │ -211// traits │ │ │ │ │ -_2_1_2template struct _t_r_a_i_t_s<_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r > : public │ │ │ │ │ -_T_e_s_t_a_b_l_e< │ │ │ │ │ -213 RegularHessianFactor > { │ │ │ │ │ -214}; │ │ │ │ │ -215 │ │ │ │ │ -216} │ │ │ │ │ -217 │ │ │ │ │ -_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ -JacobianFactor class with fixed sized blcoks. │ │ │ │ │ -_H_e_s_s_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ -Contains the HessianFactor class, a general quadratic factor. │ │ │ │ │ +_1_1_6 void _p_r_i_n_t(const std::string& s = "", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ +DefaultKeyFormatter) const override { │ │ │ │ │ +117 std::cout << s << "GenericProjectionFactor, z = "; │ │ │ │ │ +118 _t_r_a_i_t_s_<_P_o_i_n_t_2_>_:_:_P_r_i_n_t(_m_e_a_s_u_r_e_d__); │ │ │ │ │ +119 if(this->body_P_sensor_) │ │ │ │ │ +120 this->body_P_sensor_->print(" sensor pose in body frame: "); │ │ │ │ │ +121 _B_a_s_e_:_:_p_r_i_n_t("", keyFormatter); │ │ │ │ │ +122 } │ │ │ │ │ +123 │ │ │ │ │ +_1_2_5 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& p, double tol = 1e-9) const override { │ │ │ │ │ +126 const _T_h_i_s *e = dynamic_cast(&p); │ │ │ │ │ +127 return e │ │ │ │ │ +128 && _B_a_s_e_:_:_e_q_u_a_l_s(p, tol) │ │ │ │ │ +129 && _t_r_a_i_t_s_<_P_o_i_n_t_2_>_:_:_E_q_u_a_l_s(this->measured_, e->measured_, tol) │ │ │ │ │ +130 && this->K_->equals(*e->K_, tol) │ │ │ │ │ +131 && ((!_b_o_d_y___P___s_e_n_s_o_r__ && !e->body_P_sensor_) || (_b_o_d_y___P___s_e_n_s_o_r__ && e- │ │ │ │ │ +>body_P_sensor_ && _b_o_d_y___P___s_e_n_s_o_r__->equals(*e->body_P_sensor_))); │ │ │ │ │ +132 } │ │ │ │ │ +133 │ │ │ │ │ +_1_3_5 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const _P_o_s_e_3& pose, const _P_o_i_n_t_3& point, │ │ │ │ │ +136 boost::optional H1 = boost::none, boost::optional H2 = │ │ │ │ │ +boost::none) const override { │ │ │ │ │ +137 try { │ │ │ │ │ +138 if(_b_o_d_y___P___s_e_n_s_o_r__) { │ │ │ │ │ +139 if(H1) { │ │ │ │ │ +140 gtsam::Matrix H0; │ │ │ │ │ +141 _P_i_n_h_o_l_e_C_a_m_e_r_a_<_C_A_L_I_B_R_A_T_I_O_N_> camera(pose.compose(*_b_o_d_y___P___s_e_n_s_o_r__, H0), *_K__); │ │ │ │ │ +142 _P_o_i_n_t_2 reprojectionError(camera._p_r_o_j_e_c_t(point, H1, H2, boost::none) - │ │ │ │ │ +_m_e_a_s_u_r_e_d__); │ │ │ │ │ +143 *H1 = *H1 * H0; │ │ │ │ │ +144 return reprojectionError; │ │ │ │ │ +145 } else { │ │ │ │ │ +146 _P_i_n_h_o_l_e_C_a_m_e_r_a_<_C_A_L_I_B_R_A_T_I_O_N_> camera(pose.compose(*_b_o_d_y___P___s_e_n_s_o_r__), *_K__); │ │ │ │ │ +147 return camera._p_r_o_j_e_c_t(point, H1, H2, boost::none) - _m_e_a_s_u_r_e_d__; │ │ │ │ │ +148 } │ │ │ │ │ +149 } else { │ │ │ │ │ +150 _P_i_n_h_o_l_e_C_a_m_e_r_a_<_C_A_L_I_B_R_A_T_I_O_N_> camera(pose, *_K__); │ │ │ │ │ +151 return camera._p_r_o_j_e_c_t(point, H1, H2, boost::none) - _m_e_a_s_u_r_e_d__; │ │ │ │ │ +152 } │ │ │ │ │ +153 } catch( _C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n& e) { │ │ │ │ │ +154 if (H1) *H1 = Matrix::Zero(2,6); │ │ │ │ │ +155 if (H2) *H2 = Matrix::Zero(2,3); │ │ │ │ │ +156 if (_v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__) │ │ │ │ │ +157 std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) │ │ │ │ │ +<< │ │ │ │ │ +158 " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; │ │ │ │ │ +159 if (_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__) │ │ │ │ │ +160 throw _C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n(this->key2()); │ │ │ │ │ +161 } │ │ │ │ │ +162 return Vector2::Constant(2.0 * _K__->fx()); │ │ │ │ │ +163 } │ │ │ │ │ +164 │ │ │ │ │ +_1_6_6 const _P_o_i_n_t_2& _m_e_a_s_u_r_e_d() const { │ │ │ │ │ +167 return _m_e_a_s_u_r_e_d__; │ │ │ │ │ +168 } │ │ │ │ │ +169 │ │ │ │ │ +_1_7_1 const boost::shared_ptr _c_a_l_i_b_r_a_t_i_o_n() const { │ │ │ │ │ +172 return _K__; │ │ │ │ │ +173 } │ │ │ │ │ +174 │ │ │ │ │ +_1_7_6 const boost::optional& _b_o_d_y___P___s_e_n_s_o_r() const { │ │ │ │ │ +177 return _b_o_d_y___P___s_e_n_s_o_r__; │ │ │ │ │ +178 } │ │ │ │ │ +179 │ │ │ │ │ +_1_8_1 inline bool _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y() const { return _v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__; } │ │ │ │ │ +182 │ │ │ │ │ +_1_8_4 inline bool _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y() const { return _t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__; } │ │ │ │ │ +185 │ │ │ │ │ +186 private: │ │ │ │ │ +187 │ │ │ │ │ +_1_8_9 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +190 template │ │ │ │ │ +191 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +192 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +193 ar & BOOST_SERIALIZATION_NVP(_m_e_a_s_u_r_e_d__); │ │ │ │ │ +194 ar & BOOST_SERIALIZATION_NVP(_K__); │ │ │ │ │ +195 ar & BOOST_SERIALIZATION_NVP(_b_o_d_y___P___s_e_n_s_o_r__); │ │ │ │ │ +196 ar & BOOST_SERIALIZATION_NVP(_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__); │ │ │ │ │ +197 ar & BOOST_SERIALIZATION_NVP(_v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__); │ │ │ │ │ +198 } │ │ │ │ │ +199 │ │ │ │ │ +200 public: │ │ │ │ │ +201 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +202}; │ │ │ │ │ +203 │ │ │ │ │ +205 template │ │ │ │ │ +_2_0_6 struct _t_r_a_i_t_s<_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r > : │ │ │ │ │ +207 public _T_e_s_t_a_b_l_e > { │ │ │ │ │ +208 }; │ │ │ │ │ +209 │ │ │ │ │ +210} // \ namespace gtsam │ │ │ │ │ +_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ +This marks a GTSAM object to require alignment. │ │ │ │ │ +DDeeffiinniittiioonn types.h:308 │ │ │ │ │ +_P_o_s_e_3_._h │ │ │ │ │ +3D Pose │ │ │ │ │ +_P_o_i_n_t_3_._h │ │ │ │ │ +3D Point │ │ │ │ │ +_C_a_l_3___S_2_._h │ │ │ │ │ +The most common 5DOF 3D->2D calibration. │ │ │ │ │ +_P_i_n_h_o_l_e_C_a_m_e_r_a_._h │ │ │ │ │ +Base class for all pinhole cameras. │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ +Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_D_e_n_s_e_I_n_d_e_x │ │ │ │ │ -ptrdiff_t DenseIndex │ │ │ │ │ -The index type for Eigen objects. │ │ │ │ │ -DDeeffiinniittiioonn types.h:106 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ +Vector2 Point2 │ │ │ │ │ +As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ +typedef Point2 to Vector2... │ │ │ │ │ +DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ +Vector3 Point3 │ │ │ │ │ +As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ +typedef Point3 to Vector3... │ │ │ │ │ +DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ +Aliases. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ -of blocks. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_d_i_a_g_o_n_a_l │ │ │ │ │ -Vector diagonal(DenseIndex J) const │ │ │ │ │ -Get the diagonal of the J'th diagonal block. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:145 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_c_o_l_s │ │ │ │ │ -DenseIndex cols() const │ │ │ │ │ -Column size. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:117 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_a_b_o_v_e_D_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ -constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const │ │ │ │ │ -Get block above the diagonal (I, J). │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:150 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_d_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ -Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J) │ │ │ │ │ -Return the J'th diagonal block as a self adjoint view. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:135 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_n_B_l_o_c_k_s │ │ │ │ │ -DenseIndex nBlocks() const │ │ │ │ │ -Block count. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:120 │ │ │ │ │ _g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ A helper that implements the traits interface for GTSAM types. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s │ │ │ │ │ -const KeyVector & keys() const │ │ │ │ │ -Access the factor's involved variable keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:140 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s__ │ │ │ │ │ -KeyVector keys_ │ │ │ │ │ -The keys involved in this factor. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_s_i_z_e │ │ │ │ │ -size_t size() const │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:157 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -A Gaussian factor using the canonical parameters (information form) │ │ │ │ │ -DDeeffiinniittiioonn HessianFactor.h:101 │ │ │ │ │ -_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_a_u_g_m_e_n_t_e_d_I_n_f_o_r_m_a_t_i_o_n │ │ │ │ │ -Matrix augmentedInformation() const override │ │ │ │ │ -Return the augmented information matrix represented by this GaussianFactor. │ │ │ │ │ -DDeeffiinniittiioonn HessianFactor.cpp:282 │ │ │ │ │ -_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_i_n_f_o__ │ │ │ │ │ -SymmetricBlockMatrix info_ │ │ │ │ │ -The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'H │ │ │ │ │ -[x -1]. │ │ │ │ │ -DDeeffiinniittiioonn HessianFactor.h:104 │ │ │ │ │ -_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d │ │ │ │ │ -void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) │ │ │ │ │ -const override │ │ │ │ │ -y += alpha * A'*A*x │ │ │ │ │ -DDeeffiinniittiioonn HessianFactor.cpp:391 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -DDeeffiinniittiioonn RegularHessianFactor.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -RegularHessianFactor(const GaussianFactorGraph &factors) │ │ │ │ │ -Construct from a GaussianFactorGraph. │ │ │ │ │ -DDeeffiinniittiioonn RegularHessianFactor.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d │ │ │ │ │ -void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) │ │ │ │ │ -const override │ │ │ │ │ -y += alpha * A'*A*x │ │ │ │ │ -DDeeffiinniittiioonn RegularHessianFactor.h:112 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -RegularHessianFactor(Key j1, Key j2, Key j3, const MatrixD &G11, const MatrixD │ │ │ │ │ -&G12, const MatrixD &G13, const VectorD &g1, const MatrixD &G22, const MatrixD │ │ │ │ │ -&G23, const VectorD &g2, const MatrixD &G33, const VectorD &g3, double f) │ │ │ │ │ -Construct a ternary factor. │ │ │ │ │ -DDeeffiinniittiioonn RegularHessianFactor.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d │ │ │ │ │ -void multiplyHessianAdd(double alpha, const double *x, double *yvalues, std:: │ │ │ │ │ -vector< size_t > offsets) const │ │ │ │ │ -Raw memory version, with offsets TODO document reasoning. │ │ │ │ │ -DDeeffiinniittiioonn RegularHessianFactor.h:150 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d │ │ │ │ │ -void multiplyHessianAdd(double alpha, const double *x, double *yvalues) const │ │ │ │ │ -y += alpha * A'*A*x │ │ │ │ │ -DDeeffiinniittiioonn RegularHessianFactor.h:118 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_h_e_s_s_i_a_n_D_i_a_g_o_n_a_l │ │ │ │ │ -void hessianDiagonal(double *d) const override │ │ │ │ │ -Return the diagonal of the Hessian for this factor (raw memory version) │ │ │ │ │ -DDeeffiinniittiioonn RegularHessianFactor.h:185 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -RegularHessianFactor(const KEYS &keys, const SymmetricBlockMatrix │ │ │ │ │ -&augmentedInformation) │ │ │ │ │ -Constructor with an arbitrary number of keys and with the augmented information │ │ │ │ │ -matrix specified as a... │ │ │ │ │ -DDeeffiinniittiioonn RegularHessianFactor.h:68 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_g_r_a_d_i_e_n_t_A_t_Z_e_r_o │ │ │ │ │ -void gradientAtZero(double *d) const override │ │ │ │ │ -Add gradient at zero to d TODO: is it really the goal to add ?? │ │ │ │ │ -DDeeffiinniittiioonn RegularHessianFactor.h:196 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -RegularHessianFactor(const GaussianFactorGraph &factors, const Scatter │ │ │ │ │ -&scatter) │ │ │ │ │ -Construct from a GaussianFactorGraph. │ │ │ │ │ -DDeeffiinniittiioonn RegularHessianFactor.h:79 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -RegularHessianFactor(const KeyVector &js, const std::vector< Matrix > &Gs, │ │ │ │ │ -const std::vector< Vector > &gs, double f) │ │ │ │ │ -Construct an n-way factor. │ │ │ │ │ -DDeeffiinniittiioonn RegularHessianFactor.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -RegularHessianFactor(Key j1, Key j2, const MatrixD &G11, const MatrixD &G12, │ │ │ │ │ -const VectorD &g1, const MatrixD &G22, const VectorD &g2, double f) │ │ │ │ │ -Construct a binary factor. │ │ │ │ │ -DDeeffiinniittiioonn RegularHessianFactor.h:49 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -RegularHessianFactor(const RegularJacobianFactor< D > &jf) │ │ │ │ │ -Construct from RegularJacobianFactor. │ │ │ │ │ -DDeeffiinniittiioonn RegularHessianFactor.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ -JacobianFactor with constant sized blocks Provides raw memory access versions │ │ │ │ │ -of linear operator. │ │ │ │ │ -DDeeffiinniittiioonn RegularJacobianFactor.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_S_c_a_t_t_e_r │ │ │ │ │ -Scatter is an intermediate data structure used when building a HessianFactor │ │ │ │ │ -incrementally,... │ │ │ │ │ -DDeeffiinniittiioonn Scatter.h:49 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ -with a unique integer... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_C_h_e_i_r_a_l_i_t_y_E_x_c_e_p_t_i_o_n │ │ │ │ │ +DDeeffiinniittiioonn CalibratedCamera.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a │ │ │ │ │ +A pinhole camera class that has a Pose3 and a Calibration. │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:33 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_B_a_s_e_K_:_:_p_r_o_j_e_c_t │ │ │ │ │ +Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, │ │ │ │ │ +OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > │ │ │ │ │ +Dcal=boost::none) const │ │ │ │ │ +project a 3D point from world coordinates into the image │ │ │ │ │ +DDeeffiinniittiioonn PinholePose.h:118 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ +A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +virtual void print(const std::string &s="Factor", const KeyFormatter │ │ │ │ │ +&formatter=DefaultKeyFormatter) const │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn Factor.cpp:29 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const This &other, double tol=1e-9) const │ │ │ │ │ +check equality │ │ │ │ │ +DDeeffiinniittiioonn Factor.cpp:42 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ +Nonlinear factor base class. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ +A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ +variables. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +Non-linear factor for a constraint derived from a 2D measurement. │ │ │ │ │ +DDeeffiinniittiioonn ProjectionFactor.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y │ │ │ │ │ +bool verboseCheirality() const │ │ │ │ │ +return verbosity │ │ │ │ │ +DDeeffiinniittiioonn ProjectionFactor.h:181 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn ProjectionFactor.h:116 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const NonlinearFactor &p, double tol=1e-9) const override │ │ │ │ │ +equals │ │ │ │ │ +DDeeffiinniittiioonn ProjectionFactor.h:125 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_m_e_a_s_u_r_e_d__ │ │ │ │ │ +Point2 measured_ │ │ │ │ │ +2D measurement │ │ │ │ │ +DDeeffiinniittiioonn ProjectionFactor.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_T_h_i_s │ │ │ │ │ +GenericProjectionFactor< POSE, LANDMARK, CALIBRATION > This │ │ │ │ │ +shorthand for this class │ │ │ │ │ +DDeeffiinniittiioonn ProjectionFactor.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_m_e_a_s_u_r_e_d │ │ │ │ │ +const Point2 & measured() const │ │ │ │ │ +return the measurement │ │ │ │ │ +DDeeffiinniittiioonn ProjectionFactor.h:166 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_b_o_d_y___P___s_e_n_s_o_r__ │ │ │ │ │ +boost::optional< POSE > body_P_sensor_ │ │ │ │ │ +The pose of the sensor in the body frame. │ │ │ │ │ +DDeeffiinniittiioonn ProjectionFactor.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_K__ │ │ │ │ │ +boost::shared_ptr< CALIBRATION > K_ │ │ │ │ │ +shared pointer to calibration object │ │ │ │ │ +DDeeffiinniittiioonn ProjectionFactor.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_~_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +~GenericProjectionFactor() override │ │ │ │ │ +Virtual destructor. │ │ │ │ │ +DDeeffiinniittiioonn ProjectionFactor.h:104 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ +Vector evaluateError(const Pose3 &pose, const Point3 &point, boost::optional< │ │ │ │ │ +Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const │ │ │ │ │ +override │ │ │ │ │ +Evaluate error h(x)-z and optionally derivatives. │ │ │ │ │ +DDeeffiinniittiioonn ProjectionFactor.h:135 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +GenericProjectionFactor() │ │ │ │ │ +Default constructor. │ │ │ │ │ +DDeeffiinniittiioonn ProjectionFactor.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y__ │ │ │ │ │ +bool throwCheirality_ │ │ │ │ │ +If true, rethrows Cheirality exceptions (default: false) │ │ │ │ │ +DDeeffiinniittiioonn ProjectionFactor.h:49 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_B_a_s_e │ │ │ │ │ +NoiseModelFactorN< POSE, LANDMARK > Base │ │ │ │ │ +shorthand for base class type │ │ │ │ │ +DDeeffiinniittiioonn ProjectionFactor.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_b_o_d_y___P___s_e_n_s_o_r │ │ │ │ │ +const boost::optional< POSE > & body_P_sensor() const │ │ │ │ │ +return the (optional) sensor pose with respect to the vehicle frame │ │ │ │ │ +DDeeffiinniittiioonn ProjectionFactor.h:176 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_t_h_r_o_w_C_h_e_i_r_a_l_i_t_y │ │ │ │ │ +bool throwCheirality() const │ │ │ │ │ +return flag for throwing cheirality exceptions │ │ │ │ │ +DDeeffiinniittiioonn ProjectionFactor.h:184 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +GenericProjectionFactor(const Point2 &measured, const SharedNoiseModel &model, │ │ │ │ │ +Key poseKey, Key pointKey, const boost::shared_ptr< CALIBRATION > &K, bool │ │ │ │ │ +throwCheirality, bool verboseCheirality, boost::optional< POSE > │ │ │ │ │ +body_P_sensor=boost::none) │ │ │ │ │ +Constructor with exception-handling flags TODO: Mark argument order standard │ │ │ │ │ +(keys,... │ │ │ │ │ +DDeeffiinniittiioonn ProjectionFactor.h:96 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn ProjectionFactor.h:189 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ +gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn ProjectionFactor.h:107 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +shorthand for a smart pointer to a factor │ │ │ │ │ +DDeeffiinniittiioonn ProjectionFactor.h:61 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_v_e_r_b_o_s_e_C_h_e_i_r_a_l_i_t_y__ │ │ │ │ │ +bool verboseCheirality_ │ │ │ │ │ +If true, prints text for Cheirality exceptions (default: false) │ │ │ │ │ +DDeeffiinniittiioonn ProjectionFactor.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r │ │ │ │ │ +GenericProjectionFactor(const Point2 &measured, const SharedNoiseModel &model, │ │ │ │ │ +Key poseKey, Key pointKey, const boost::shared_ptr< CALIBRATION > &K, boost:: │ │ │ │ │ +optional< POSE > body_P_sensor=boost::none) │ │ │ │ │ +Constructor TODO: Mark argument order standard (keys, measurement, parameters) │ │ │ │ │ +DDeeffiinniittiioonn ProjectionFactor.h:78 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_:_:_c_a_l_i_b_r_a_t_i_o_n │ │ │ │ │ +const boost::shared_ptr< CALIBRATION > calibration() const │ │ │ │ │ +return the calibration object │ │ │ │ │ +DDeeffiinniittiioonn ProjectionFactor.h:171 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _l_i_n_e_a_r │ │ │ │ │ - * _R_e_g_u_l_a_r_H_e_s_s_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ + * _s_l_a_m │ │ │ │ │ + * _P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00896_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00782_source.html │ │ │ │┄ Files 88% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/SubgraphPreconditioner.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/IterativeSolver.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
SubgraphPreconditioner.h
│ │ │ │ +
IterativeSolver.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/linear/SubgraphBuilder.h>
│ │ │ │ -
21#include <gtsam/linear/Errors.h>
│ │ │ │ -
22#include <gtsam/linear/GaussianBayesNet.h>
│ │ │ │ -
23#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ -
24#include <gtsam/linear/IterativeSolver.h>
│ │ │ │ -
25#include <gtsam/linear/Preconditioner.h>
│ │ │ │ -
26#include <gtsam/linear/VectorValues.h>
│ │ │ │ -
27#include <gtsam/dllexport.h>
│ │ │ │ -
28
│ │ │ │ -
29#include <boost/shared_ptr.hpp>
│ │ │ │ -
30
│ │ │ │ -
31#include <map>
│ │ │ │ -
32
│ │ │ │ -
33namespace gtsam {
│ │ │ │ -
34
│ │ │ │ -
35 // Forward declarations
│ │ │ │ -
36 class GaussianBayesNet;
│ │ │ │ -
37 class GaussianFactorGraph;
│ │ │ │ -
38 class VectorValues;
│ │ │ │ -
39
│ │ │ │ -
│ │ │ │ -
40 struct GTSAM_EXPORT SubgraphPreconditionerParameters : public PreconditionerParameters {
│ │ │ │ -
41 typedef boost::shared_ptr<SubgraphPreconditionerParameters> shared_ptr;
│ │ │ │ -
42 SubgraphPreconditionerParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters())
│ │ │ │ -
43 : builderParams(p) {}
│ │ │ │ -
44 SubgraphBuilderParameters builderParams;
│ │ │ │ -
45 };
│ │ │ │ -
│ │ │ │ -
46
│ │ │ │ -
│ │ │ │ -
54 class GTSAM_EXPORT SubgraphPreconditioner : public Preconditioner {
│ │ │ │ -
55
│ │ │ │ -
56 public:
│ │ │ │ -
57 typedef boost::shared_ptr<SubgraphPreconditioner> shared_ptr;
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/inference/Ordering.h>
│ │ │ │ +
22#include <gtsam/base/Vector.h>
│ │ │ │ +
23
│ │ │ │ +
24#include <boost/tuple/tuple.hpp>
│ │ │ │ +
25#include <boost/shared_ptr.hpp>
│ │ │ │ +
26#include <boost/optional.hpp>
│ │ │ │ +
27
│ │ │ │ +
28#include <iosfwd>
│ │ │ │ +
29#include <string>
│ │ │ │ +
30#include <map>
│ │ │ │ +
31
│ │ │ │ +
32namespace gtsam {
│ │ │ │ +
33
│ │ │ │ +
34// Forward declarations
│ │ │ │ +
35struct KeyInfoEntry;
│ │ │ │ +
36class KeyInfo;
│ │ │ │ +
37class GaussianFactorGraph;
│ │ │ │ +
38class Values;
│ │ │ │ +
39class VectorValues;
│ │ │ │ +
40
│ │ │ │ +
│ │ │ │ +
44class IterativeOptimizationParameters {
│ │ │ │ +
45
│ │ │ │ +
46public:
│ │ │ │ +
47
│ │ │ │ +
48 typedef boost::shared_ptr<IterativeOptimizationParameters> shared_ptr;
│ │ │ │ +
49 enum Verbosity {
│ │ │ │ +
50 SILENT = 0, COMPLEXITY, ERROR
│ │ │ │ +
51 } verbosity_;
│ │ │ │ +
52
│ │ │ │ +
53public:
│ │ │ │ +
54
│ │ │ │ +
55 IterativeOptimizationParameters(Verbosity v = SILENT) :
│ │ │ │ +
56 verbosity_(v) {
│ │ │ │ +
57 }
│ │ │ │
58
│ │ │ │ -
59 private:
│ │ │ │ -
60 GaussianFactorGraph Ab2_;
│ │ │ │ -
61 GaussianBayesNet Rc1_;
│ │ │ │ -
62 VectorValues xbar_;
│ │ │ │ -
63 Errors b2bar_;
│ │ │ │ -
64
│ │ │ │ -
65 KeyInfo keyInfo_;
│ │ │ │ -
66 SubgraphPreconditionerParameters parameters_;
│ │ │ │ -
67
│ │ │ │ -
68 public:
│ │ │ │ -
69
│ │ │ │ -
70 SubgraphPreconditioner(const SubgraphPreconditionerParameters &p = SubgraphPreconditionerParameters());
│ │ │ │ +
59 virtual ~IterativeOptimizationParameters() {
│ │ │ │ +
60 }
│ │ │ │ +
61
│ │ │ │ +
62 /* utility */
│ │ │ │ +
63 inline Verbosity verbosity() const {
│ │ │ │ +
64 return verbosity_;
│ │ │ │ +
65 }
│ │ │ │ +
66 GTSAM_EXPORT std::string getVerbosity() const;
│ │ │ │ +
67 GTSAM_EXPORT void setVerbosity(const std::string &s);
│ │ │ │ +
68
│ │ │ │ +
69 /* matlab interface */
│ │ │ │ +
70 GTSAM_EXPORT void print() const;
│ │ │ │
71
│ │ │ │ -
78 SubgraphPreconditioner(const GaussianFactorGraph& Ab2, const GaussianBayesNet& Rc1, const VectorValues& xbar,
│ │ │ │ -
79 const SubgraphPreconditionerParameters &p = SubgraphPreconditionerParameters());
│ │ │ │ -
80
│ │ │ │ -
81 ~SubgraphPreconditioner() override {}
│ │ │ │ +
72 /* virtual print function */
│ │ │ │ +
73 GTSAM_EXPORT virtual void print(std::ostream &os) const;
│ │ │ │ +
74
│ │ │ │ +
75 /* for serialization */
│ │ │ │ +
76 friend std::ostream& operator<<(std::ostream &os,
│ │ │ │ +
77 const IterativeOptimizationParameters &p);
│ │ │ │ +
78
│ │ │ │ +
79 GTSAM_EXPORT static Verbosity verbosityTranslator(const std::string &s);
│ │ │ │ +
80 GTSAM_EXPORT static std::string verbosityTranslator(Verbosity v);
│ │ │ │ +
81};
│ │ │ │ +
│ │ │ │
82
│ │ │ │ -
84 void print(const std::string& s = "SubgraphPreconditioner") const;
│ │ │ │ -
85
│ │ │ │ -
87 const GaussianFactorGraph& Ab2() const { return Ab2_; }
│ │ │ │ -
88
│ │ │ │ -
90 const GaussianBayesNet& Rc1() const { return Rc1_; }
│ │ │ │ -
91
│ │ │ │ -
93 const Errors b2bar() const { return b2bar_; }
│ │ │ │ -
94
│ │ │ │ -
100 /* x = xbar + inv(R1)*y */
│ │ │ │ -
101 VectorValues x(const VectorValues& y) const;
│ │ │ │ +
│ │ │ │ +
86class IterativeSolver {
│ │ │ │ +
87public:
│ │ │ │ +
88 typedef boost::shared_ptr<IterativeSolver> shared_ptr;
│ │ │ │ +
89 IterativeSolver() {
│ │ │ │ +
90 }
│ │ │ │ +
91 virtual ~IterativeSolver() {
│ │ │ │ +
92 }
│ │ │ │ +
93
│ │ │ │ +
94 /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */
│ │ │ │ +
95 GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg,
│ │ │ │ +
96 boost::optional<const KeyInfo&> = boost::none,
│ │ │ │ +
97 boost::optional<const std::map<Key, Vector>&> lambda = boost::none);
│ │ │ │ +
98
│ │ │ │ +
99 /* interface to the nonlinear optimizer, without initial estimate */
│ │ │ │ +
100 GTSAM_EXPORT VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo,
│ │ │ │ +
101 const std::map<Key, Vector> &lambda);
│ │ │ │
102
│ │ │ │ -
103 /* A zero VectorValues with the structure of xbar */
│ │ │ │ -
104 VectorValues zero() const {
│ │ │ │ -
105 return VectorValues::Zero(xbar_);
│ │ │ │ -
106 }
│ │ │ │ +
103 /* interface to the nonlinear optimizer that the subclasses have to implement */
│ │ │ │ +
104 virtual VectorValues optimize(const GaussianFactorGraph &gfg,
│ │ │ │ +
105 const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
│ │ │ │ +
106 const VectorValues &initial) = 0;
│ │ │ │
107
│ │ │ │ -
113 void transposeMultiplyAdd2(double alpha, Errors::const_iterator begin,
│ │ │ │ -
114 Errors::const_iterator end, VectorValues& y) const;
│ │ │ │ -
115
│ │ │ │ -
116 /* error, given y */
│ │ │ │ -
117 double error(const VectorValues& y) const;
│ │ │ │ -
118
│ │ │ │ -
120 VectorValues gradient(const VectorValues& y) const;
│ │ │ │ -
121
│ │ │ │ -
123 Errors operator*(const VectorValues& y) const;
│ │ │ │ -
124
│ │ │ │ -
126 void multiplyInPlace(const VectorValues& y, Errors& e) const;
│ │ │ │ +
108};
│ │ │ │ +
│ │ │ │ +
109
│ │ │ │ +
│ │ │ │ +
114struct GTSAM_EXPORT KeyInfoEntry {
│ │ │ │ +
115 size_t index, dim, start;
│ │ │ │ +
116 KeyInfoEntry() {
│ │ │ │ +
117 }
│ │ │ │ +
118 KeyInfoEntry(size_t idx, size_t d, Key start) :
│ │ │ │ +
119 index(idx), dim(d), start(start) {
│ │ │ │ +
120 }
│ │ │ │ +
121};
│ │ │ │ +
│ │ │ │ +
122
│ │ │ │ +
│ │ │ │ +
126class GTSAM_EXPORT KeyInfo: public std::map<Key, KeyInfoEntry> {
│ │ │ │
127
│ │ │ │ -
129 VectorValues operator^(const Errors& e) const;
│ │ │ │ -
130
│ │ │ │ -
135 void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& y) const;
│ │ │ │ +
128public:
│ │ │ │ +
129
│ │ │ │ +
130 typedef std::map<Key, KeyInfoEntry> Base;
│ │ │ │ +
131
│ │ │ │ +
132protected:
│ │ │ │ +
133
│ │ │ │ +
134 Ordering ordering_;
│ │ │ │ +
135 size_t numCols_;
│ │ │ │
136
│ │ │ │ -
137 /*****************************************************************************/
│ │ │ │ -
138 /* implement virtual functions of Preconditioner */
│ │ │ │ -
139
│ │ │ │ -
141 void solve(const Vector& y, Vector &x) const override;
│ │ │ │ -
142
│ │ │ │ -
144 void transposeSolve(const Vector& y, Vector& x) const override;
│ │ │ │ +
137 void initialize(const GaussianFactorGraph &fg);
│ │ │ │ +
138
│ │ │ │ +
139public:
│ │ │ │ +
140
│ │ │ │ +
│ │ │ │ +
142 KeyInfo() :
│ │ │ │ +
143 numCols_(0) {
│ │ │ │ +
144 }
│ │ │ │ +
│ │ │ │
145
│ │ │ │ -
147 void build(
│ │ │ │ -
148 const GaussianFactorGraph &gfg,
│ │ │ │ -
149 const KeyInfo &info,
│ │ │ │ -
150 const std::map<Key,Vector> &lambda
│ │ │ │ -
151 ) override;
│ │ │ │ -
152 /*****************************************************************************/
│ │ │ │ -
153 };
│ │ │ │ -
│ │ │ │ -
154
│ │ │ │ -
155} // namespace gtsam
│ │ │ │ -
IterativeSolver.h
Some support classes for iterative solvers.
│ │ │ │ -
VectorValues.h
Factor Graph Values.
│ │ │ │ - │ │ │ │ -
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
│ │ │ │ -
Errors.h
vector of errors
│ │ │ │ -
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ +
147 KeyInfo(const GaussianFactorGraph &fg);
│ │ │ │ +
148
│ │ │ │ +
150 KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering);
│ │ │ │ +
151
│ │ │ │ +
│ │ │ │ +
153 inline size_t numCols() const {
│ │ │ │ +
154 return numCols_;
│ │ │ │ +
155 }
│ │ │ │ +
│ │ │ │ +
156
│ │ │ │ +
│ │ │ │ +
158 inline const Ordering & ordering() const {
│ │ │ │ +
159 return ordering_;
│ │ │ │ +
160 }
│ │ │ │ +
│ │ │ │ +
161
│ │ │ │ +
163 std::vector<size_t> colSpec() const;
│ │ │ │ +
164
│ │ │ │ +
166 VectorValues x0() const;
│ │ │ │ +
167
│ │ │ │ +
169 Vector x0vector() const;
│ │ │ │ +
170
│ │ │ │ +
171};
│ │ │ │ +
│ │ │ │ +
172
│ │ │ │ +
173} // \ namespace gtsam
│ │ │ │ +
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ +
Ordering.h
Variable ordering for the elimination algorithm.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ - │ │ │ │ -
gtsam::GaussianBayesNet
GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals.
Definition GaussianBayesNet.h:36
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ +
gtsam::IterativeOptimizationParameters
parameters for iterative linear solvers
Definition IterativeSolver.h:44
│ │ │ │ +
gtsam::IterativeSolver
Base class for Iterative Solvers like SubgraphSolver.
Definition IterativeSolver.h:86
│ │ │ │ +
gtsam::KeyInfoEntry
Handy data structure for iterative solvers key to (index, dimension, start)
Definition IterativeSolver.h:114
│ │ │ │
gtsam::KeyInfo
Handy data structure for iterative solvers.
Definition IterativeSolver.h:126
│ │ │ │ -
gtsam::PreconditionerParameters
Definition Preconditioner.h:24
│ │ │ │ -
gtsam::Preconditioner
Definition Preconditioner.h:64
│ │ │ │ -
gtsam::SubgraphBuilderParameters
Definition SubgraphBuilder.h:96
│ │ │ │ -
gtsam::SubgraphPreconditionerParameters
Definition SubgraphPreconditioner.h:40
│ │ │ │ -
gtsam::SubgraphPreconditioner
Subgraph conditioner class, as explained in the RSS 2010 submission.
Definition SubgraphPreconditioner.h:54
│ │ │ │ -
gtsam::SubgraphPreconditioner::Ab2
const GaussianFactorGraph & Ab2() const
Access Ab2.
Definition SubgraphPreconditioner.h:87
│ │ │ │ -
gtsam::SubgraphPreconditioner::b2bar
const Errors b2bar() const
Access b2bar.
Definition SubgraphPreconditioner.h:93
│ │ │ │ -
gtsam::SubgraphPreconditioner::Rc1
const GaussianBayesNet & Rc1() const
Access Rc1.
Definition SubgraphPreconditioner.h:90
│ │ │ │ +
gtsam::KeyInfo::ordering
const Ordering & ordering() const
Return the ordering.
Definition IterativeSolver.h:158
│ │ │ │ +
gtsam::KeyInfo::numCols
size_t numCols() const
Return the total number of columns (scalar variables = sum of dimensions)
Definition IterativeSolver.h:153
│ │ │ │ +
gtsam::KeyInfo::KeyInfo
KeyInfo()
Default Constructor.
Definition IterativeSolver.h:142
│ │ │ │
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │
VectorValues
The Factor::error simply extracts the.
│ │ │ │ +
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,189 +1,219 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SubgraphPreconditioner.h │ │ │ │ │ +IterativeSolver.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_E_r_r_o_r_s_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_I_t_e_r_a_t_i_v_e_S_o_l_v_e_r_._h> │ │ │ │ │ -25#include │ │ │ │ │ -26#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ -27#include │ │ │ │ │ -28 │ │ │ │ │ -29#include │ │ │ │ │ -30 │ │ │ │ │ -31#include │ │ │ │ │ -32 │ │ │ │ │ -33namespace _g_t_s_a_m { │ │ │ │ │ -34 │ │ │ │ │ -35 // Forward declarations │ │ │ │ │ -36 class GaussianBayesNet; │ │ │ │ │ -37 class GaussianFactorGraph; │ │ │ │ │ -38 class _V_e_c_t_o_r_V_a_l_u_e_s; │ │ │ │ │ -39 │ │ │ │ │ -_4_0 struct GTSAM_EXPORT _S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s : public │ │ │ │ │ -_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s { │ │ │ │ │ -41 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -42 _S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s(const _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s &p = │ │ │ │ │ -_S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s()) │ │ │ │ │ -43 : builderParams(p) {} │ │ │ │ │ -44 _S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s builderParams; │ │ │ │ │ -45 }; │ │ │ │ │ -46 │ │ │ │ │ -_5_4 class GTSAM_EXPORT _S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r : public _P_r_e_c_o_n_d_i_t_i_o_n_e_r { │ │ │ │ │ -55 │ │ │ │ │ -56 public: │ │ │ │ │ -57 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_O_r_d_e_r_i_n_g_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_c_t_o_r_._h> │ │ │ │ │ +23 │ │ │ │ │ +24#include │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ +27 │ │ │ │ │ +28#include │ │ │ │ │ +29#include │ │ │ │ │ +30#include │ │ │ │ │ +31 │ │ │ │ │ +32namespace _g_t_s_a_m { │ │ │ │ │ +33 │ │ │ │ │ +34// Forward declarations │ │ │ │ │ +35struct KeyInfoEntry; │ │ │ │ │ +36class KeyInfo; │ │ │ │ │ +37class GaussianFactorGraph; │ │ │ │ │ +38class _V_a_l_u_e_s; │ │ │ │ │ +39class _V_e_c_t_o_r_V_a_l_u_e_s; │ │ │ │ │ +40 │ │ │ │ │ +_4_4class _I_t_e_r_a_t_i_v_e_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s { │ │ │ │ │ +45 │ │ │ │ │ +46public: │ │ │ │ │ +47 │ │ │ │ │ +48 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +49 enum Verbosity { │ │ │ │ │ +50 SILENT = 0, COMPLEXITY, ERROR │ │ │ │ │ +51 } verbosity_; │ │ │ │ │ +52 │ │ │ │ │ +53public: │ │ │ │ │ +54 │ │ │ │ │ +55 _I_t_e_r_a_t_i_v_e_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s(Verbosity v = SILENT) : │ │ │ │ │ +56 verbosity_(v) { │ │ │ │ │ +57 } │ │ │ │ │ 58 │ │ │ │ │ -59 private: │ │ │ │ │ -60 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h Ab2_; │ │ │ │ │ -61 _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t Rc1_; │ │ │ │ │ -62 _V_e_c_t_o_r_V_a_l_u_e_s xbar_; │ │ │ │ │ -63 _E_r_r_o_r_s b2bar_; │ │ │ │ │ -64 │ │ │ │ │ -65 _K_e_y_I_n_f_o keyInfo_; │ │ │ │ │ -66 _S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s parameters_; │ │ │ │ │ -67 │ │ │ │ │ -68 public: │ │ │ │ │ -69 │ │ │ │ │ -70 _S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r(const _S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s &p = │ │ │ │ │ -_S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s()); │ │ │ │ │ +59 virtual _~_I_t_e_r_a_t_i_v_e_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s() { │ │ │ │ │ +60 } │ │ │ │ │ +61 │ │ │ │ │ +62 /* utility */ │ │ │ │ │ +63 inline Verbosity verbosity() const { │ │ │ │ │ +64 return verbosity_; │ │ │ │ │ +65 } │ │ │ │ │ +66 GTSAM_EXPORT std::string getVerbosity() const; │ │ │ │ │ +67 GTSAM_EXPORT void setVerbosity(const std::string &s); │ │ │ │ │ +68 │ │ │ │ │ +69 /* matlab interface */ │ │ │ │ │ +70 GTSAM_EXPORT void print() const; │ │ │ │ │ 71 │ │ │ │ │ -78 _S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& Ab2, const │ │ │ │ │ -_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t& Rc1, const _V_e_c_t_o_r_V_a_l_u_e_s& xbar, │ │ │ │ │ -79 const _S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s &p = _S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ -()); │ │ │ │ │ -80 │ │ │ │ │ -81 _~_S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r() override {} │ │ │ │ │ +72 /* virtual print function */ │ │ │ │ │ +73 GTSAM_EXPORT virtual void _p_r_i_n_t(std::ostream &os) const; │ │ │ │ │ +74 │ │ │ │ │ +75 /* for serialization */ │ │ │ │ │ +76 friend std::ostream& operator<<(std::ostream &os, │ │ │ │ │ +77 const _I_t_e_r_a_t_i_v_e_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s &p); │ │ │ │ │ +78 │ │ │ │ │ +79 GTSAM_EXPORT static Verbosity verbosityTranslator(const std::string &s); │ │ │ │ │ +80 GTSAM_EXPORT static std::string verbosityTranslator(Verbosity v); │ │ │ │ │ +81}; │ │ │ │ │ 82 │ │ │ │ │ -84 void _p_r_i_n_t(const std::string& s = "SubgraphPreconditioner") const; │ │ │ │ │ -85 │ │ │ │ │ -_8_7 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& _A_b_2() const { return Ab2_; } │ │ │ │ │ -88 │ │ │ │ │ -_9_0 const _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t& _R_c_1() const { return Rc1_; } │ │ │ │ │ -91 │ │ │ │ │ -_9_3 const _E_r_r_o_r_s _b_2_b_a_r() const { return b2bar_; } │ │ │ │ │ -94 │ │ │ │ │ -100 /* x = xbar + inv(R1)*y */ │ │ │ │ │ -101 _V_e_c_t_o_r_V_a_l_u_e_s x(const _V_e_c_t_o_r_V_a_l_u_e_s& y) const; │ │ │ │ │ +_8_6class _I_t_e_r_a_t_i_v_e_S_o_l_v_e_r { │ │ │ │ │ +87public: │ │ │ │ │ +88 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +89 _I_t_e_r_a_t_i_v_e_S_o_l_v_e_r() { │ │ │ │ │ +90 } │ │ │ │ │ +91 virtual _~_I_t_e_r_a_t_i_v_e_S_o_l_v_e_r() { │ │ │ │ │ +92 } │ │ │ │ │ +93 │ │ │ │ │ +94 /* interface to the nonlinear optimizer, without metadata, damping and │ │ │ │ │ +initial estimate */ │ │ │ │ │ +95 GTSAM_EXPORT _V_e_c_t_o_r_V_a_l_u_e_s optimize(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, │ │ │ │ │ +96 boost::optional = boost::none, │ │ │ │ │ +97 boost::optional&> lambda = boost::none); │ │ │ │ │ +98 │ │ │ │ │ +99 /* interface to the nonlinear optimizer, without initial estimate */ │ │ │ │ │ +100 GTSAM_EXPORT _V_e_c_t_o_r_V_a_l_u_e_s _o_p_t_i_m_i_z_e(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, const │ │ │ │ │ +_K_e_y_I_n_f_o &keyInfo, │ │ │ │ │ +101 const std::map &lambda); │ │ │ │ │ 102 │ │ │ │ │ -103 /* A zero VectorValues with the structure of xbar */ │ │ │ │ │ -104 _V_e_c_t_o_r_V_a_l_u_e_s zero() const { │ │ │ │ │ -105 return VectorValues::Zero(xbar_); │ │ │ │ │ -106 } │ │ │ │ │ +103 /* interface to the nonlinear optimizer that the subclasses have to │ │ │ │ │ +implement */ │ │ │ │ │ +104 virtual _V_e_c_t_o_r_V_a_l_u_e_s _o_p_t_i_m_i_z_e(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, │ │ │ │ │ +105 const _K_e_y_I_n_f_o &keyInfo, const std::map &lambda, │ │ │ │ │ +106 const _V_e_c_t_o_r_V_a_l_u_e_s &initial) = 0; │ │ │ │ │ 107 │ │ │ │ │ -113 void transposeMultiplyAdd2(double alpha, Errors::const_iterator begin, │ │ │ │ │ -114 Errors::const_iterator end, _V_e_c_t_o_r_V_a_l_u_e_s& y) const; │ │ │ │ │ -115 │ │ │ │ │ -116 /* error, given y */ │ │ │ │ │ -117 double error(const _V_e_c_t_o_r_V_a_l_u_e_s& y) const; │ │ │ │ │ -118 │ │ │ │ │ -120 _V_e_c_t_o_r_V_a_l_u_e_s gradient(const _V_e_c_t_o_r_V_a_l_u_e_s& y) const; │ │ │ │ │ -121 │ │ │ │ │ -123 Errors operator*(const _V_e_c_t_o_r_V_a_l_u_e_s& y) const; │ │ │ │ │ -124 │ │ │ │ │ -126 void multiplyInPlace(const _V_e_c_t_o_r_V_a_l_u_e_s& y, Errors& e) const; │ │ │ │ │ +108}; │ │ │ │ │ +109 │ │ │ │ │ +_1_1_4struct GTSAM_EXPORT _K_e_y_I_n_f_o_E_n_t_r_y { │ │ │ │ │ +115 size_t index, dim, start; │ │ │ │ │ +116 _K_e_y_I_n_f_o_E_n_t_r_y() { │ │ │ │ │ +117 } │ │ │ │ │ +118 _K_e_y_I_n_f_o_E_n_t_r_y(size_t idx, size_t d, _K_e_y start) : │ │ │ │ │ +119 index(idx), dim(d), start(start) { │ │ │ │ │ +120 } │ │ │ │ │ +121}; │ │ │ │ │ +122 │ │ │ │ │ +_1_2_6class GTSAM_EXPORT _K_e_y_I_n_f_o: public std::map { │ │ │ │ │ 127 │ │ │ │ │ -129 _V_e_c_t_o_r_V_a_l_u_e_s operator^(const Errors& e) const; │ │ │ │ │ -130 │ │ │ │ │ -135 void transposeMultiplyAdd(double alpha, const Errors& e, _V_e_c_t_o_r_V_a_l_u_e_s& y) │ │ │ │ │ -const; │ │ │ │ │ +128public: │ │ │ │ │ +129 │ │ │ │ │ +130 typedef std::map Base; │ │ │ │ │ +131 │ │ │ │ │ +132protected: │ │ │ │ │ +133 │ │ │ │ │ +134 _O_r_d_e_r_i_n_g ordering_; │ │ │ │ │ +135 size_t numCols_; │ │ │ │ │ 136 │ │ │ │ │ -137 / │ │ │ │ │ -*****************************************************************************/ │ │ │ │ │ -138 /* implement virtual functions of Preconditioner */ │ │ │ │ │ -139 │ │ │ │ │ -141 void solve(const Vector& y, Vector &x) const override; │ │ │ │ │ -142 │ │ │ │ │ -144 void transposeSolve(const Vector& y, Vector& x) const override; │ │ │ │ │ +137 void initialize(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &fg); │ │ │ │ │ +138 │ │ │ │ │ +139public: │ │ │ │ │ +140 │ │ │ │ │ +_1_4_2 _K_e_y_I_n_f_o() : │ │ │ │ │ +143 numCols_(0) { │ │ │ │ │ +144 } │ │ │ │ │ 145 │ │ │ │ │ -147 void build( │ │ │ │ │ -148 const GaussianFactorGraph &gfg, │ │ │ │ │ -149 const KeyInfo &info, │ │ │ │ │ -150 const std::map &lambda │ │ │ │ │ -151 ) override; │ │ │ │ │ -152 / │ │ │ │ │ -*****************************************************************************/ │ │ │ │ │ -153 }; │ │ │ │ │ -154 │ │ │ │ │ -155} // namespace gtsam │ │ │ │ │ -_I_t_e_r_a_t_i_v_e_S_o_l_v_e_r_._h │ │ │ │ │ -Some support classes for iterative solvers. │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ -Factor Graph Values. │ │ │ │ │ -_S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_._h │ │ │ │ │ -_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h │ │ │ │ │ -Chordal Bayes Net, the result of eliminating a factor graph. │ │ │ │ │ -_E_r_r_o_r_s_._h │ │ │ │ │ -vector of errors │ │ │ │ │ -_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ +147 _K_e_y_I_n_f_o(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &fg); │ │ │ │ │ +148 │ │ │ │ │ +150 _K_e_y_I_n_f_o(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &fg, const _O_r_d_e_r_i_n_g &ordering); │ │ │ │ │ +151 │ │ │ │ │ +_1_5_3 inline size_t _n_u_m_C_o_l_s() const { │ │ │ │ │ +154 return numCols_; │ │ │ │ │ +155 } │ │ │ │ │ +156 │ │ │ │ │ +_1_5_8 inline const _O_r_d_e_r_i_n_g & _o_r_d_e_r_i_n_g() const { │ │ │ │ │ +159 return ordering_; │ │ │ │ │ +160 } │ │ │ │ │ +161 │ │ │ │ │ +163 std::vector colSpec() const; │ │ │ │ │ +164 │ │ │ │ │ +166 _V_e_c_t_o_r_V_a_l_u_e_s x0() const; │ │ │ │ │ +167 │ │ │ │ │ +169 Vector x0vector() const; │ │ │ │ │ +170 │ │ │ │ │ +171}; │ │ │ │ │ +172 │ │ │ │ │ +173} // \ namespace gtsam │ │ │ │ │ +_V_e_c_t_o_r_._h │ │ │ │ │ +typedef and functions to augment Eigen's VectorXd │ │ │ │ │ +_O_r_d_e_r_i_n_g_._h │ │ │ │ │ +Variable ordering for the elimination algorithm. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ +Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key │ │ │ │ │ +landmarkKey) │ │ │ │ │ +Optimize for triangulation. │ │ │ │ │ +DDeeffiinniittiioonn triangulation.cpp:155 │ │ │ │ │ _g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ print without optional string, must specify cout yourself │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t_<_ _V_e_c_t_o_r_ _> │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t │ │ │ │ │ -GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals. │ │ │ │ │ -DDeeffiinniittiioonn GaussianBayesNet.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_I_t_e_r_a_t_i_v_e_O_p_t_i_m_i_z_a_t_i_o_n_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ +parameters for iterative linear solvers │ │ │ │ │ +DDeeffiinniittiioonn IterativeSolver.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_I_t_e_r_a_t_i_v_e_S_o_l_v_e_r │ │ │ │ │ +Base class for Iterative Solvers like SubgraphSolver. │ │ │ │ │ +DDeeffiinniittiioonn IterativeSolver.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_I_n_f_o_E_n_t_r_y │ │ │ │ │ +Handy data structure for iterative solvers key to (index, dimension, start) │ │ │ │ │ +DDeeffiinniittiioonn IterativeSolver.h:114 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_I_n_f_o │ │ │ │ │ Handy data structure for iterative solvers. │ │ │ │ │ DDeeffiinniittiioonn IterativeSolver.h:126 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ -DDeeffiinniittiioonn Preconditioner.h:24 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_c_o_n_d_i_t_i_o_n_e_r │ │ │ │ │ -DDeeffiinniittiioonn Preconditioner.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_B_u_i_l_d_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ -DDeeffiinniittiioonn SubgraphBuilder.h:96 │ │ │ │ │ -_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ -DDeeffiinniittiioonn SubgraphPreconditioner.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r │ │ │ │ │ -Subgraph conditioner class, as explained in the RSS 2010 submission. │ │ │ │ │ -DDeeffiinniittiioonn SubgraphPreconditioner.h:54 │ │ │ │ │ -_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_:_:_A_b_2 │ │ │ │ │ -const GaussianFactorGraph & Ab2() const │ │ │ │ │ -Access Ab2. │ │ │ │ │ -DDeeffiinniittiioonn SubgraphPreconditioner.h:87 │ │ │ │ │ -_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_:_:_b_2_b_a_r │ │ │ │ │ -const Errors b2bar() const │ │ │ │ │ -Access b2bar. │ │ │ │ │ -DDeeffiinniittiioonn SubgraphPreconditioner.h:93 │ │ │ │ │ -_g_t_s_a_m_:_:_S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_:_:_R_c_1 │ │ │ │ │ -const GaussianBayesNet & Rc1() const │ │ │ │ │ -Access Rc1. │ │ │ │ │ -DDeeffiinniittiioonn SubgraphPreconditioner.h:90 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_I_n_f_o_:_:_o_r_d_e_r_i_n_g │ │ │ │ │ +const Ordering & ordering() const │ │ │ │ │ +Return the ordering. │ │ │ │ │ +DDeeffiinniittiioonn IterativeSolver.h:158 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_I_n_f_o_:_:_n_u_m_C_o_l_s │ │ │ │ │ +size_t numCols() const │ │ │ │ │ +Return the total number of columns (scalar variables = sum of dimensions) │ │ │ │ │ +DDeeffiinniittiioonn IterativeSolver.h:153 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_I_n_f_o_:_:_K_e_y_I_n_f_o │ │ │ │ │ +KeyInfo() │ │ │ │ │ +Default Constructor. │ │ │ │ │ +DDeeffiinniittiioonn IterativeSolver.h:142 │ │ │ │ │ _g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ with a unique integer... │ │ │ │ │ DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ _V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ The Factor::error simply extracts the. │ │ │ │ │ +_V_a_l_u_e_s │ │ │ │ │ +In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ +a non-linear function... │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _l_i_n_e_a_r │ │ │ │ │ - * _S_u_b_g_r_a_p_h_P_r_e_c_o_n_d_i_t_i_o_n_e_r_._h │ │ │ │ │ + * _I_t_e_r_a_t_i_v_e_S_o_l_v_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00917.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00953.html │ │ │ │┄ Files 98% similar despite different names │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
Date
July 2014
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00920.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01037.html │ │ │ │┄ Files 95% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ScenarioRunner.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/LevenbergMarquardtParams.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
ScenarioRunner.h File Reference
│ │ │ │ +
LevenbergMarquardtParams.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Simple class to test navigation scenarios. │ │ │ │ +

Parameters for Levenberg-Marquardt trust-region scheme. │ │ │ │ More...

│ │ │ │ │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::ScenarioRunner
 
class  gtsam::CombinedScenarioRunner
class  gtsam::LevenbergMarquardtParams
 Parameters for Levenberg-Marquardt optimization. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Simple class to test navigation scenarios.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +

Parameters for Levenberg-Marquardt trust-region scheme.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │ +
│ │ │ │ +Luca Carlone
│ │ │ │ +
Date
Feb 26, 2012
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,26 +1,29 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -ScenarioRunner.h File Reference │ │ │ │ │ -Simple class to test navigation scenarios. _M_o_r_e_._._. │ │ │ │ │ +LevenbergMarquardtParams.h File Reference │ │ │ │ │ +Parameters for Levenberg-Marquardt trust-region scheme. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_S_c_e_n_a_r_i_o_R_u_n_n_e_r │ │ │ │ │ -  │ │ │ │ │ -class   _g_t_s_a_m_:_:_C_o_m_b_i_n_e_d_S_c_e_n_a_r_i_o_R_u_n_n_e_r │ │ │ │ │ +class   _g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s │ │ │ │ │ +  Parameters for Levenberg-Marquardt optimization. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Simple class to test navigation scenarios. │ │ │ │ │ +Parameters for Levenberg-Marquardt trust-region scheme. │ │ │ │ │ Author │ │ │ │ │ + Richard Roberts │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Date │ │ │ │ │ + Feb 26, 2012 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _S_c_e_n_a_r_i_o_R_u_n_n_e_r_._h │ │ │ │ │ + * _n_o_n_l_i_n_e_a_r │ │ │ │ │ + * _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00920_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00080_source.html │ │ │ │┄ Files 83% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ScenarioRunner.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/Group.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ScenarioRunner.h
│ │ │ │ +
Group.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19#include <gtsam/linear/Sampler.h>
│ │ │ │ -
20#include <gtsam/navigation/CombinedImuFactor.h>
│ │ │ │ -
21#include <gtsam/navigation/ImuFactor.h>
│ │ │ │ -
22#include <gtsam/navigation/Scenario.h>
│ │ │ │ -
23
│ │ │ │ -
24namespace gtsam {
│ │ │ │ -
25
│ │ │ │ -
26// Convert covariance to diagonal noise model, if possible, otherwise throw
│ │ │ │ -
27static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) {
│ │ │ │ -
28 bool smart = true;
│ │ │ │ -
29 auto model = noiseModel::Gaussian::Covariance(covariance, smart);
│ │ │ │ -
30 auto diagonal = boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
│ │ │ │ -
31 if (!diagonal)
│ │ │ │ -
32 throw std::invalid_argument("ScenarioRunner::Diagonal: not a diagonal");
│ │ │ │ -
33 return diagonal;
│ │ │ │ -
34}
│ │ │ │ +
21#pragma once
│ │ │ │ +
22
│ │ │ │ +
23#include <gtsam/base/Testable.h>
│ │ │ │ +
24
│ │ │ │ +
25#include <boost/concept_check.hpp>
│ │ │ │ +
26#include <boost/concept/requires.hpp>
│ │ │ │ +
27#include <boost/type_traits/is_base_of.hpp>
│ │ │ │ +
28#include <boost/static_assert.hpp>
│ │ │ │ +
29#include <utility>
│ │ │ │ +
30
│ │ │ │ +
31namespace gtsam {
│ │ │ │ +
32
│ │ │ │ +
34struct group_tag {};
│ │ │ │
35
│ │ │ │ -
36/*
│ │ │ │ -
37 * Simple class to test navigation scenarios.
│ │ │ │ -
38 * Takes a trajectory scenario as input, and can generate IMU measurements
│ │ │ │ -
39 */
│ │ │ │ -
│ │ │ │ -
40class GTSAM_EXPORT ScenarioRunner {
│ │ │ │ -
41 public:
│ │ │ │ -
42 typedef imuBias::ConstantBias Bias;
│ │ │ │ -
43 typedef boost::shared_ptr<PreintegrationParams> SharedParams;
│ │ │ │ -
44
│ │ │ │ -
45 private:
│ │ │ │ -
46 const Scenario& scenario_;
│ │ │ │ -
47 const SharedParams p_;
│ │ │ │ -
48 const double imuSampleTime_, sqrt_dt_;
│ │ │ │ -
49 const Bias estimatedBias_;
│ │ │ │ -
50
│ │ │ │ -
51 // Create two samplers for acceleration and omega noise
│ │ │ │ -
52 Sampler gyroSampler_, accSampler_;
│ │ │ │ -
53
│ │ │ │ -
54 public:
│ │ │ │ -
55 ScenarioRunner(const Scenario& scenario, const SharedParams& p,
│ │ │ │ -
56 double imuSampleTime = 1.0 / 100.0, const Bias& bias = Bias())
│ │ │ │ -
57 : scenario_(scenario),
│ │ │ │ -
58 p_(p),
│ │ │ │ -
59 imuSampleTime_(imuSampleTime),
│ │ │ │ -
60 sqrt_dt_(std::sqrt(imuSampleTime)),
│ │ │ │ -
61 estimatedBias_(bias),
│ │ │ │ -
62 // NOTE(duy): random seeds that work well:
│ │ │ │ -
63 gyroSampler_(Diagonal(p->gyroscopeCovariance), 10),
│ │ │ │ -
64 accSampler_(Diagonal(p->accelerometerCovariance), 29284) {}
│ │ │ │ -
65
│ │ │ │ -
66 // NOTE(frank): hardcoded for now with Z up (gravity points in negative Z)
│ │ │ │ -
67 // also, uses g=10 for easy debugging
│ │ │ │ -
68 const Vector3& gravity_n() const { return p_->n_gravity; }
│ │ │ │ -
69
│ │ │ │ -
70 const Scenario& scenario() const { return scenario_; }
│ │ │ │ -
71
│ │ │ │ -
72 // A gyro simply measures angular velocity in body frame
│ │ │ │ -
73 Vector3 actualAngularVelocity(double t) const { return scenario_.omega_b(t); }
│ │ │ │ +
37struct multiplicative_group_tag {};
│ │ │ │ +
38struct additive_group_tag {};
│ │ │ │ +
39
│ │ │ │ +
40template <typename T> struct traits;
│ │ │ │ +
41
│ │ │ │ +
45template<typename G>
│ │ │ │ +
│ │ │ │ +
46class IsGroup {
│ │ │ │ +
47public:
│ │ │ │ +
48 typedef typename traits<G>::structure_category structure_category_tag;
│ │ │ │ +
49 typedef typename traits<G>::group_flavor flavor_tag;
│ │ │ │ +
50 //typedef typename traits<G>::identity::value_type identity_value_type;
│ │ │ │ +
51
│ │ │ │ +
52 BOOST_CONCEPT_USAGE(IsGroup) {
│ │ │ │ +
53 BOOST_STATIC_ASSERT_MSG(
│ │ │ │ +
54 (boost::is_base_of<group_tag, structure_category_tag>::value),
│ │ │ │ +
55 "This type's structure_category trait does not assert it as a group (or derived)");
│ │ │ │ +
56 e = traits<G>::Identity();
│ │ │ │ +
57 e = traits<G>::Compose(g, h);
│ │ │ │ +
58 e = traits<G>::Between(g, h);
│ │ │ │ +
59 e = traits<G>::Inverse(g);
│ │ │ │ +
60 operator_usage(flavor);
│ │ │ │ +
61 // todo: how do we test the act concept? or do we even need to?
│ │ │ │ +
62 }
│ │ │ │ +
63
│ │ │ │ +
64private:
│ │ │ │ +
65 void operator_usage(multiplicative_group_tag) {
│ │ │ │ +
66 e = g * h;
│ │ │ │ +
67 //e = -g; // todo this should work, but it is failing for Quaternions
│ │ │ │ +
68 }
│ │ │ │ +
69 void operator_usage(additive_group_tag) {
│ │ │ │ +
70 e = g + h;
│ │ │ │ +
71 e = h - g;
│ │ │ │ +
72 e = -g;
│ │ │ │ +
73 }
│ │ │ │
74
│ │ │ │ -
75 // An accelerometer measures acceleration in body, but not gravity
│ │ │ │ -
76 Vector3 actualSpecificForce(double t) const {
│ │ │ │ -
77 Rot3 bRn(scenario_.rotation(t).transpose());
│ │ │ │ -
78 return scenario_.acceleration_b(t) - bRn * gravity_n();
│ │ │ │ -
79 }
│ │ │ │ -
80
│ │ │ │ -
81 // versions corrupted by bias and noise
│ │ │ │ -
82 Vector3 measuredAngularVelocity(double t) const {
│ │ │ │ -
83 return actualAngularVelocity(t) + estimatedBias_.gyroscope() +
│ │ │ │ -
84 gyroSampler_.sample() / sqrt_dt_;
│ │ │ │ -
85 }
│ │ │ │ -
86 Vector3 measuredSpecificForce(double t) const {
│ │ │ │ -
87 return actualSpecificForce(t) + estimatedBias_.accelerometer() +
│ │ │ │ -
88 accSampler_.sample() / sqrt_dt_;
│ │ │ │ -
89 }
│ │ │ │ -
90
│ │ │ │ -
91 const double& imuSampleTime() const { return imuSampleTime_; }
│ │ │ │ -
92
│ │ │ │ -
94 PreintegratedImuMeasurements integrate(double T,
│ │ │ │ -
95 const Bias& estimatedBias = Bias(),
│ │ │ │ -
96 bool corrupted = false) const;
│ │ │ │ -
97
│ │ │ │ -
99 NavState predict(const PreintegratedImuMeasurements& pim,
│ │ │ │ -
100 const Bias& estimatedBias = Bias()) const;
│ │ │ │ -
101
│ │ │ │ -
103 Matrix9 estimateCovariance(double T, size_t N = 1000,
│ │ │ │ -
104 const Bias& estimatedBias = Bias()) const;
│ │ │ │ -
105
│ │ │ │ -
107 Matrix6 estimateNoiseCovariance(size_t N = 1000) const;
│ │ │ │ -
108};
│ │ │ │ -
│ │ │ │ -
109
│ │ │ │ -
110/*
│ │ │ │ -
111 * Simple class to test navigation scenarios with CombinedImuMeasurements.
│ │ │ │ -
112 * Takes a trajectory scenario as input, and can generate IMU measurements
│ │ │ │ -
113 */
│ │ │ │ -
│ │ │ │ -
114class GTSAM_EXPORT CombinedScenarioRunner : public ScenarioRunner {
│ │ │ │ -
115 public:
│ │ │ │ -
116 typedef boost::shared_ptr<PreintegrationCombinedParams> SharedParams;
│ │ │ │ -
117
│ │ │ │ -
118 private:
│ │ │ │ -
119 const SharedParams p_;
│ │ │ │ -
120 const Bias estimatedBias_;
│ │ │ │ -
121
│ │ │ │ -
122 public:
│ │ │ │ -
123 CombinedScenarioRunner(const Scenario& scenario, const SharedParams& p,
│ │ │ │ -
124 double imuSampleTime = 1.0 / 100.0,
│ │ │ │ -
125 const Bias& bias = Bias())
│ │ │ │ -
126 : ScenarioRunner(scenario, static_cast<ScenarioRunner::SharedParams>(p),
│ │ │ │ -
127 imuSampleTime, bias),
│ │ │ │ -
128 p_(p),
│ │ │ │ -
129 estimatedBias_(bias) {}
│ │ │ │ -
130
│ │ │ │ -
132 PreintegratedCombinedMeasurements integrate(
│ │ │ │ -
133 double T, const Bias& estimatedBias = Bias(),
│ │ │ │ -
134 bool corrupted = false) const;
│ │ │ │ -
135
│ │ │ │ -
137 NavState predict(const PreintegratedCombinedMeasurements& pim,
│ │ │ │ -
138 const Bias& estimatedBias = Bias()) const;
│ │ │ │ -
139
│ │ │ │ -
141 Eigen::Matrix<double, 15, 15> estimateCovariance(
│ │ │ │ -
142 double T, size_t N = 1000, const Bias& estimatedBias = Bias()) const;
│ │ │ │ -
143};
│ │ │ │ -
│ │ │ │ -
144
│ │ │ │ -
145} // namespace gtsam
│ │ │ │ -
Sampler.h
sampling from a NoiseModel
│ │ │ │ -
Scenario.h
Simple class to test navigation scenarios.
│ │ │ │ - │ │ │ │ - │ │ │ │ +
75 flavor_tag flavor;
│ │ │ │ +
76 G e, g, h;
│ │ │ │ +
77 bool b;
│ │ │ │ +
78};
│ │ │ │ +
│ │ │ │ +
79
│ │ │ │ +
81template<typename G>
│ │ │ │ +
82BOOST_CONCEPT_REQUIRES(((IsGroup<G>)),(bool)) //
│ │ │ │ +
83check_group_invariants(const G& a, const G& b, double tol = 1e-9) {
│ │ │ │ +
84 G e = traits<G>::Identity();
│ │ │ │ +
85 return traits<G>::Equals(traits<G>::Compose(a, traits<G>::Inverse(a)), e, tol)
│ │ │ │ +
86 && traits<G>::Equals(traits<G>::Between(a, b), traits<G>::Compose(traits<G>::Inverse(a), b), tol)
│ │ │ │ +
87 && traits<G>::Equals(traits<G>::Compose(a, traits<G>::Between(a, b)), b, tol);
│ │ │ │ +
88}
│ │ │ │ +
89
│ │ │ │ +
90namespace internal {
│ │ │ │ +
91
│ │ │ │ +
94template<class Class>
│ │ │ │ +
95struct MultiplicativeGroupTraits {
│ │ │ │ +
96 typedef group_tag structure_category;
│ │ │ │ +
97 typedef multiplicative_group_tag group_flavor;
│ │ │ │ +
98 static Class Identity() { return Class::Identity(); }
│ │ │ │ +
99 static Class Compose(const Class &g, const Class & h) { return g * h;}
│ │ │ │ +
100 static Class Between(const Class &g, const Class & h) { return g.inverse() * h;}
│ │ │ │ +
101 static Class Inverse(const Class &g) { return g.inverse();}
│ │ │ │ +
102};
│ │ │ │ +
103
│ │ │ │ +
105template<class Class>
│ │ │ │ +
106struct MultiplicativeGroup : MultiplicativeGroupTraits<Class>, Testable<Class> {};
│ │ │ │ +
107
│ │ │ │ +
110template<class Class>
│ │ │ │ +
111struct AdditiveGroupTraits {
│ │ │ │ +
112 typedef group_tag structure_category;
│ │ │ │ +
113 typedef additive_group_tag group_flavor;
│ │ │ │ +
114 static Class Identity() { return Class::Identity(); }
│ │ │ │ +
115 static Class Compose(const Class &g, const Class & h) { return g + h;}
│ │ │ │ +
116 static Class Between(const Class &g, const Class & h) { return h - g;}
│ │ │ │ +
117 static Class Inverse(const Class &g) { return -g;}
│ │ │ │ +
118};
│ │ │ │ +
119
│ │ │ │ +
121template<class Class>
│ │ │ │ +
122struct AdditiveGroup : AdditiveGroupTraits<Class>, Testable<Class> {};
│ │ │ │ +
123
│ │ │ │ +
124} // namespace internal
│ │ │ │ +
125
│ │ │ │ +
127template<typename G>
│ │ │ │ +
128BOOST_CONCEPT_REQUIRES(((IsGroup<G>)),(G)) //
│ │ │ │ +
129compose_pow(const G& g, size_t n) {
│ │ │ │ +
130 if (n == 0) return traits<G>::Identity();
│ │ │ │ +
131 else if (n == 1) return g;
│ │ │ │ +
132 else return traits<G>::Compose(compose_pow(g, n - 1), g);
│ │ │ │ +
133}
│ │ │ │ +
134
│ │ │ │ +
137template<typename G, typename H>
│ │ │ │ +
│ │ │ │ +
138class DirectProduct: public std::pair<G, H> {
│ │ │ │ +
139 BOOST_CONCEPT_ASSERT((IsGroup<G>));
│ │ │ │ +
140 BOOST_CONCEPT_ASSERT((IsGroup<H>));
│ │ │ │ +
141
│ │ │ │ +
142public:
│ │ │ │ +
144 DirectProduct():std::pair<G,H>(traits<G>::Identity(),traits<H>::Identity()) {}
│ │ │ │ +
145
│ │ │ │ +
146 // Construct from two subgroup elements
│ │ │ │ +
147 DirectProduct(const G& g, const H& h):std::pair<G,H>(g,h) {}
│ │ │ │ +
148
│ │ │ │ +
149 // identity
│ │ │ │ +
150 static DirectProduct Identity() { return DirectProduct(); }
│ │ │ │ +
151
│ │ │ │ +
152 DirectProduct operator*(const DirectProduct& other) const {
│ │ │ │ +
153 return DirectProduct(traits<G>::Compose(this->first, other.first),
│ │ │ │ +
154 traits<H>::Compose(this->second, other.second));
│ │ │ │ +
155 }
│ │ │ │ +
156 DirectProduct inverse() const {
│ │ │ │ +
157 return DirectProduct(this->first.inverse(), this->second.inverse());
│ │ │ │ +
158 }
│ │ │ │ +
159};
│ │ │ │ +
│ │ │ │ +
160
│ │ │ │ +
161// Define any direct product group to be a model of the multiplicative Group concept
│ │ │ │ +
162template<typename G, typename H>
│ │ │ │ +
│ │ │ │ +
163struct traits<DirectProduct<G, H> > :
│ │ │ │ +
164 internal::MultiplicativeGroupTraits<DirectProduct<G, H> > {};
│ │ │ │ +
│ │ │ │ +
165
│ │ │ │ +
168template<typename G, typename H>
│ │ │ │ +
│ │ │ │ +
169class DirectSum: public std::pair<G, H> {
│ │ │ │ +
170 BOOST_CONCEPT_ASSERT((IsGroup<G>)); // TODO(frank): check additive
│ │ │ │ +
171 BOOST_CONCEPT_ASSERT((IsGroup<H>)); // TODO(frank): check additive
│ │ │ │ +
172
│ │ │ │ +
173 const G& g() const { return this->first; }
│ │ │ │ +
174 const H& h() const { return this->second;}
│ │ │ │ +
175
│ │ │ │ +
176public:
│ │ │ │ +
178 DirectSum():std::pair<G,H>(traits<G>::Identity(),traits<H>::Identity()) {}
│ │ │ │ +
179
│ │ │ │ +
180 // Construct from two subgroup elements
│ │ │ │ +
181 DirectSum(const G& g, const H& h):std::pair<G,H>(g,h) {}
│ │ │ │ +
182
│ │ │ │ +
183 // identity
│ │ │ │ +
184 static DirectSum Identity() { return DirectSum(); }
│ │ │ │ +
185
│ │ │ │ +
186 DirectSum operator+(const DirectSum& other) const {
│ │ │ │ +
187 return DirectSum(g()+other.g(), h()+other.h());
│ │ │ │ +
188 }
│ │ │ │ +
189 DirectSum operator-(const DirectSum& other) const {
│ │ │ │ +
190 return DirectSum(g()-other.g(), h()-other.h());
│ │ │ │ +
191 }
│ │ │ │ +
192 DirectSum operator-() const {
│ │ │ │ +
193 return DirectSum(- g(), - h());
│ │ │ │ +
194 }
│ │ │ │ +
195};
│ │ │ │ +
│ │ │ │ +
196
│ │ │ │ +
197// Define direct sums to be a model of the Additive Group concept
│ │ │ │ +
198template<typename G, typename H>
│ │ │ │ +
│ │ │ │ +
199struct traits<DirectSum<G, H> > :
│ │ │ │ +
200 internal::AdditiveGroupTraits<DirectSum<G, H> > {};
│ │ │ │ +
│ │ │ │ +
201
│ │ │ │ +
202} // namespace gtsam
│ │ │ │ +
203
│ │ │ │ +
212#define GTSAM_CONCEPT_GROUP_INST(T) template class gtsam::IsGroup<T>;
│ │ │ │ +
213#define GTSAM_CONCEPT_GROUP_TYPE(T) typedef gtsam::IsGroup<T> _gtsam_IsGroup_##T;
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
│ │ │ │ -
gtsam::Rot3::transpose
Matrix3 transpose() const
Return 3*3 transpose (inverse) rotation matrix.
Definition Rot3M.cpp:144
│ │ │ │ -
gtsam::noiseModel::Gaussian::Covariance
static shared_ptr Covariance(const Matrix &covariance, bool smart=true)
A Gaussian noise model created by specifying a covariance matrix.
Definition NoiseModel.cpp:117
│ │ │ │ -
gtsam::Sampler
Sampling structure that keeps internal random number generators for diagonal distributions specified ...
Definition Sampler.h:31
│ │ │ │ -
gtsam::Sampler::sample
Vector sample() const
sample from distribution
Definition Sampler.cpp:59
│ │ │ │ -
gtsam::PreintegratedCombinedMeasurements
PreintegratedCombinedMeasurements integrates the IMU measurements (rotation rates and accelerations) ...
Definition CombinedImuFactor.h:129
│ │ │ │ -
gtsam::imuBias::ConstantBias
Definition ImuBias.h:30
│ │ │ │ -
gtsam::imuBias::ConstantBias::gyroscope
const Vector3 & gyroscope() const
get gyroscope bias
Definition ImuBias.h:69
│ │ │ │ -
gtsam::imuBias::ConstantBias::accelerometer
const Vector3 & accelerometer() const
get accelerometer bias
Definition ImuBias.h:64
│ │ │ │ -
gtsam::PreintegratedImuMeasurements
PreintegratedImuMeasurements accumulates (integrates) the IMU measurements (rotation rates and accele...
Definition ImuFactor.h:72
│ │ │ │ -
gtsam::NavState
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition NavState.h:34
│ │ │ │ -
gtsam::Scenario
Simple trajectory simulator.
Definition Scenario.h:25
│ │ │ │ -
gtsam::Scenario::omega_b
virtual Vector3 omega_b(double t) const =0
angular velocity in body frame
│ │ │ │ -
gtsam::ScenarioRunner
Definition ScenarioRunner.h:40
│ │ │ │ -
gtsam::CombinedScenarioRunner
Definition ScenarioRunner.h:114
│ │ │ │ +
gtsam::BOOST_CONCEPT_REQUIRES
BOOST_CONCEPT_REQUIRES(((IsGroup< G >)),(bool)) check_group_invariants(const G &a
Check invariants.
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::group_tag
tag to assert a type is a group
Definition Group.h:34
│ │ │ │ +
gtsam::multiplicative_group_tag
Group operator syntax flavors.
Definition Group.h:37
│ │ │ │ +
gtsam::additive_group_tag
Definition Group.h:38
│ │ │ │ +
gtsam::IsGroup
Group Concept.
Definition Group.h:46
│ │ │ │ +
gtsam::DirectProduct
Definition Group.h:138
│ │ │ │ +
gtsam::DirectProduct::DirectProduct
DirectProduct()
Default constructor yields identity.
Definition Group.h:144
│ │ │ │ +
gtsam::DirectSum
Template to construct the direct sum of two additive groups Assumes existence of three additive opera...
Definition Group.h:169
│ │ │ │ +
gtsam::DirectSum::DirectSum
DirectSum()
Default constructor yields identity.
Definition Group.h:178
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,207 +1,237 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ScenarioRunner.h │ │ │ │ │ +Group.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_S_a_m_p_l_e_r_._h> │ │ │ │ │ -20#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_I_m_u_F_a_c_t_o_r_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_S_c_e_n_a_r_i_o_._h> │ │ │ │ │ -23 │ │ │ │ │ -24namespace _g_t_s_a_m { │ │ │ │ │ -25 │ │ │ │ │ -26// Convert covariance to diagonal noise model, if possible, otherwise throw │ │ │ │ │ -27static noiseModel::Diagonal::shared_ptr Diagonal(const Matrix& covariance) { │ │ │ │ │ -28 bool smart = true; │ │ │ │ │ -29 auto model = _n_o_i_s_e_M_o_d_e_l_:_:_G_a_u_s_s_i_a_n_:_:_C_o_v_a_r_i_a_n_c_e(covariance, smart); │ │ │ │ │ -30 auto diagonal = boost::dynamic_pointer_cast(model); │ │ │ │ │ -31 if (!diagonal) │ │ │ │ │ -32 throw std::invalid_argument("ScenarioRunner::Diagonal: not a diagonal"); │ │ │ │ │ -33 return diagonal; │ │ │ │ │ -34} │ │ │ │ │ +21#pragma once │ │ │ │ │ +22 │ │ │ │ │ +23#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +24 │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28#include │ │ │ │ │ +29#include │ │ │ │ │ +30 │ │ │ │ │ +31namespace _g_t_s_a_m { │ │ │ │ │ +32 │ │ │ │ │ +_3_4struct _g_r_o_u_p___t_a_g {}; │ │ │ │ │ 35 │ │ │ │ │ -36/* │ │ │ │ │ -37 * Simple class to test navigation scenarios. │ │ │ │ │ -38 * Takes a trajectory scenario as input, and can generate IMU measurements │ │ │ │ │ -39 */ │ │ │ │ │ -_4_0class GTSAM_EXPORT _S_c_e_n_a_r_i_o_R_u_n_n_e_r { │ │ │ │ │ -41 public: │ │ │ │ │ -42 typedef _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s _B_i_a_s; │ │ │ │ │ -43 typedef boost::shared_ptr SharedParams; │ │ │ │ │ -44 │ │ │ │ │ -45 private: │ │ │ │ │ -46 const _S_c_e_n_a_r_i_o& scenario_; │ │ │ │ │ -47 const SharedParams p_; │ │ │ │ │ -48 const double imuSampleTime_, sqrt_dt_; │ │ │ │ │ -49 const _B_i_a_s estimatedBias_; │ │ │ │ │ -50 │ │ │ │ │ -51 // Create two samplers for acceleration and omega noise │ │ │ │ │ -52 _S_a_m_p_l_e_r gyroSampler_, accSampler_; │ │ │ │ │ -53 │ │ │ │ │ -54 public: │ │ │ │ │ -55 _S_c_e_n_a_r_i_o_R_u_n_n_e_r(const _S_c_e_n_a_r_i_o& scenario, const SharedParams& p, │ │ │ │ │ -56 double imuSampleTime = 1.0 / 100.0, const _B_i_a_s& bias = _B_i_a_s()) │ │ │ │ │ -57 : scenario_(scenario), │ │ │ │ │ -58 p_(p), │ │ │ │ │ -59 imuSampleTime_(imuSampleTime), │ │ │ │ │ -60 sqrt_dt_(std::sqrt(imuSampleTime)), │ │ │ │ │ -61 estimatedBias_(bias), │ │ │ │ │ -62 // NOTE(duy): random seeds that work well: │ │ │ │ │ -63 gyroSampler_(Diagonal(p->gyroscopeCovariance), 10), │ │ │ │ │ -64 accSampler_(Diagonal(p->accelerometerCovariance), 29284) {} │ │ │ │ │ -65 │ │ │ │ │ -66 // NOTE(frank): hardcoded for now with Z up (gravity points in negative Z) │ │ │ │ │ -67 // also, uses g=10 for easy debugging │ │ │ │ │ -68 const Vector3& gravity_n() const { return p_->n_gravity; } │ │ │ │ │ -69 │ │ │ │ │ -70 const _S_c_e_n_a_r_i_o& scenario() const { return scenario_; } │ │ │ │ │ -71 │ │ │ │ │ -72 // A gyro simply measures angular velocity in body frame │ │ │ │ │ -73 Vector3 actualAngularVelocity(double t) const { return scenario_._o_m_e_g_a___b(t); │ │ │ │ │ -} │ │ │ │ │ +_3_7struct _m_u_l_t_i_p_l_i_c_a_t_i_v_e___g_r_o_u_p___t_a_g {}; │ │ │ │ │ +_3_8struct _a_d_d_i_t_i_v_e___g_r_o_u_p___t_a_g {}; │ │ │ │ │ +39 │ │ │ │ │ +40template struct _t_r_a_i_t_s; │ │ │ │ │ +41 │ │ │ │ │ +45template │ │ │ │ │ +_4_6class _I_s_G_r_o_u_p { │ │ │ │ │ +47public: │ │ │ │ │ +48 typedef typename _t_r_a_i_t_s_<_G_>_:_:_s_t_r_u_c_t_u_r_e___c_a_t_e_g_o_r_y structure_category_tag; │ │ │ │ │ +49 typedef typename _t_r_a_i_t_s_<_G_>_:_:_g_r_o_u_p___f_l_a_v_o_r flavor_tag; │ │ │ │ │ +50 //typedef typename traits::identity::value_type identity_value_type; │ │ │ │ │ +51 │ │ │ │ │ +52 BOOST_CONCEPT_USAGE(_I_s_G_r_o_u_p) { │ │ │ │ │ +53 BOOST_STATIC_ASSERT_MSG( │ │ │ │ │ +54 (boost::is_base_of::value), │ │ │ │ │ +55 "This type's structure_category trait does not assert it as a group (or │ │ │ │ │ +derived)"); │ │ │ │ │ +56 e = _t_r_a_i_t_s_<_G_>_:_:_I_d_e_n_t_i_t_y(); │ │ │ │ │ +57 e = _t_r_a_i_t_s_<_G_>_:_:_C_o_m_p_o_s_e(g, h); │ │ │ │ │ +58 e = _t_r_a_i_t_s_<_G_>_:_:_B_e_t_w_e_e_n(g, h); │ │ │ │ │ +59 e = _t_r_a_i_t_s_<_G_>_:_:_I_n_v_e_r_s_e(g); │ │ │ │ │ +60 operator_usage(flavor); │ │ │ │ │ +61 // todo: how do we test the act concept? or do we even need to? │ │ │ │ │ +62 } │ │ │ │ │ +63 │ │ │ │ │ +64private: │ │ │ │ │ +65 void operator_usage(_m_u_l_t_i_p_l_i_c_a_t_i_v_e___g_r_o_u_p___t_a_g) { │ │ │ │ │ +66 e = g * h; │ │ │ │ │ +67 //e = -g; // todo this should work, but it is failing for Quaternions │ │ │ │ │ +68 } │ │ │ │ │ +69 void operator_usage(_a_d_d_i_t_i_v_e___g_r_o_u_p___t_a_g) { │ │ │ │ │ +70 e = g + h; │ │ │ │ │ +71 e = h - g; │ │ │ │ │ +72 e = -g; │ │ │ │ │ +73 } │ │ │ │ │ 74 │ │ │ │ │ -75 // An accelerometer measures acceleration in body, but not gravity │ │ │ │ │ -76 Vector3 actualSpecificForce(double t) const { │ │ │ │ │ -77 _R_o_t_3 bRn(scenario_.rotation(t)._t_r_a_n_s_p_o_s_e()); │ │ │ │ │ -78 return scenario_.acceleration_b(t) - bRn * gravity_n(); │ │ │ │ │ -79 } │ │ │ │ │ -80 │ │ │ │ │ -81 // versions corrupted by bias and noise │ │ │ │ │ -82 Vector3 measuredAngularVelocity(double t) const { │ │ │ │ │ -83 return actualAngularVelocity(t) + estimatedBias_._g_y_r_o_s_c_o_p_e() + │ │ │ │ │ -84 gyroSampler_._s_a_m_p_l_e() / sqrt_dt_; │ │ │ │ │ -85 } │ │ │ │ │ -86 Vector3 measuredSpecificForce(double t) const { │ │ │ │ │ -87 return actualSpecificForce(t) + estimatedBias_._a_c_c_e_l_e_r_o_m_e_t_e_r() + │ │ │ │ │ -88 accSampler_._s_a_m_p_l_e() / sqrt_dt_; │ │ │ │ │ -89 } │ │ │ │ │ -90 │ │ │ │ │ -91 const double& imuSampleTime() const { return imuSampleTime_; } │ │ │ │ │ -92 │ │ │ │ │ -94 _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s integrate(double T, │ │ │ │ │ -95 const _B_i_a_s& estimatedBias = _B_i_a_s(), │ │ │ │ │ -96 bool corrupted = false) const; │ │ │ │ │ -97 │ │ │ │ │ -99 _N_a_v_S_t_a_t_e predict(const _P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s& pim, │ │ │ │ │ -100 const _B_i_a_s& estimatedBias = _B_i_a_s()) const; │ │ │ │ │ -101 │ │ │ │ │ -103 Matrix9 estimateCovariance(double T, size_t N = 1000, │ │ │ │ │ -104 const _B_i_a_s& estimatedBias = _B_i_a_s()) const; │ │ │ │ │ -105 │ │ │ │ │ -107 Matrix6 estimateNoiseCovariance(size_t N = 1000) const; │ │ │ │ │ -108}; │ │ │ │ │ -109 │ │ │ │ │ -110/* │ │ │ │ │ -111 * Simple class to test navigation scenarios with CombinedImuMeasurements. │ │ │ │ │ -112 * Takes a trajectory scenario as input, and can generate IMU measurements │ │ │ │ │ -113 */ │ │ │ │ │ -_1_1_4class GTSAM_EXPORT _C_o_m_b_i_n_e_d_S_c_e_n_a_r_i_o_R_u_n_n_e_r : public _S_c_e_n_a_r_i_o_R_u_n_n_e_r { │ │ │ │ │ -115 public: │ │ │ │ │ -116 typedef boost::shared_ptr SharedParams; │ │ │ │ │ -117 │ │ │ │ │ -118 private: │ │ │ │ │ -119 const SharedParams p_; │ │ │ │ │ -120 const _B_i_a_s estimatedBias_; │ │ │ │ │ -121 │ │ │ │ │ -122 public: │ │ │ │ │ -123 _C_o_m_b_i_n_e_d_S_c_e_n_a_r_i_o_R_u_n_n_e_r(const _S_c_e_n_a_r_i_o& scenario, const SharedParams& p, │ │ │ │ │ -124 double imuSampleTime = 1.0 / 100.0, │ │ │ │ │ -125 const _B_i_a_s& bias = _B_i_a_s()) │ │ │ │ │ -126 : _S_c_e_n_a_r_i_o_R_u_n_n_e_r(scenario, static_cast(p), │ │ │ │ │ -127 imuSampleTime, bias), │ │ │ │ │ -128 p_(p), │ │ │ │ │ -129 estimatedBias_(bias) {} │ │ │ │ │ -130 │ │ │ │ │ -132 _P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s integrate( │ │ │ │ │ -133 double T, const _B_i_a_s& estimatedBias = _B_i_a_s(), │ │ │ │ │ -134 bool corrupted = false) const; │ │ │ │ │ -135 │ │ │ │ │ -137 _N_a_v_S_t_a_t_e predict(const _P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s& pim, │ │ │ │ │ -138 const _B_i_a_s& estimatedBias = _B_i_a_s()) const; │ │ │ │ │ -139 │ │ │ │ │ -141 Eigen::Matrix estimateCovariance( │ │ │ │ │ -142 double T, size_t N = 1000, const _B_i_a_s& estimatedBias = _B_i_a_s()) const; │ │ │ │ │ -143}; │ │ │ │ │ -144 │ │ │ │ │ -145} // namespace gtsam │ │ │ │ │ -_S_a_m_p_l_e_r_._h │ │ │ │ │ -sampling from a NoiseModel │ │ │ │ │ -_S_c_e_n_a_r_i_o_._h │ │ │ │ │ -Simple class to test navigation scenarios. │ │ │ │ │ -_C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r_._h │ │ │ │ │ -_I_m_u_F_a_c_t_o_r_._h │ │ │ │ │ +75 flavor_tag flavor; │ │ │ │ │ +76 G e, g, h; │ │ │ │ │ +77 bool b; │ │ │ │ │ +78}; │ │ │ │ │ +79 │ │ │ │ │ +81template │ │ │ │ │ +_8_2_B_O_O_S_T___C_O_N_C_E_P_T___R_E_Q_U_I_R_E_S(((_I_s_G_r_o_u_p_<_G_>)),(bool)) // │ │ │ │ │ +83check_group_invariants(const G& a, const G& b, double tol = 1e-9) { │ │ │ │ │ +84 G e = _t_r_a_i_t_s_<_G_>_:_:_I_d_e_n_t_i_t_y(); │ │ │ │ │ +85 return _t_r_a_i_t_s_<_G_>_:_:_E_q_u_a_l_s(_t_r_a_i_t_s_<_G_>_:_:_C_o_m_p_o_s_e(a, _t_r_a_i_t_s_<_G_>_:_:_I_n_v_e_r_s_e(a)), e, │ │ │ │ │ +tol) │ │ │ │ │ +86 && _t_r_a_i_t_s_<_G_>_:_:_E_q_u_a_l_s(_t_r_a_i_t_s_<_G_>_:_:_B_e_t_w_e_e_n(a, b), _t_r_a_i_t_s_<_G_>_:_:_C_o_m_p_o_s_e │ │ │ │ │ +(_t_r_a_i_t_s_<_G_>_:_:_I_n_v_e_r_s_e(a), b), tol) │ │ │ │ │ +87 && _t_r_a_i_t_s_<_G_>_:_:_E_q_u_a_l_s(_t_r_a_i_t_s_<_G_>_:_:_C_o_m_p_o_s_e(a, _t_r_a_i_t_s_<_G_>_:_:_B_e_t_w_e_e_n(a, b)), b, │ │ │ │ │ +tol); │ │ │ │ │ +88} │ │ │ │ │ +89 │ │ │ │ │ +90namespace internal { │ │ │ │ │ +91 │ │ │ │ │ +94template │ │ │ │ │ +95struct MultiplicativeGroupTraits { │ │ │ │ │ +96 typedef group_tag structure_category; │ │ │ │ │ +97 typedef multiplicative_group_tag group_flavor; │ │ │ │ │ +98 static Class Identity() { return Class::Identity(); } │ │ │ │ │ +99 static Class Compose(const Class &g, const Class & h) { return g * h;} │ │ │ │ │ +100 static Class Between(const Class &g, const Class & h) { return g.inverse() │ │ │ │ │ +* h;} │ │ │ │ │ +101 static Class Inverse(const Class &g) { return g.inverse();} │ │ │ │ │ +102}; │ │ │ │ │ +103 │ │ │ │ │ +105template │ │ │ │ │ +106struct MultiplicativeGroup : MultiplicativeGroupTraits, │ │ │ │ │ +Testable {}; │ │ │ │ │ +107 │ │ │ │ │ +110template │ │ │ │ │ +111struct AdditiveGroupTraits { │ │ │ │ │ +112 typedef group_tag structure_category; │ │ │ │ │ +113 typedef additive_group_tag group_flavor; │ │ │ │ │ +114 static Class Identity() { return Class::Identity(); } │ │ │ │ │ +115 static Class Compose(const Class &g, const Class & h) { return g + h;} │ │ │ │ │ +116 static Class Between(const Class &g, const Class & h) { return h - g;} │ │ │ │ │ +117 static Class Inverse(const Class &g) { return -g;} │ │ │ │ │ +118}; │ │ │ │ │ +119 │ │ │ │ │ +121template │ │ │ │ │ +122struct AdditiveGroup : AdditiveGroupTraits, Testable {}; │ │ │ │ │ +123 │ │ │ │ │ +124} // namespace internal │ │ │ │ │ +125 │ │ │ │ │ +127template │ │ │ │ │ +128_B_O_O_S_T___C_O_N_C_E_P_T___R_E_Q_U_I_R_E_S(((IsGroup)),(G)) // │ │ │ │ │ +129compose_pow(const G& g, size_t n) { │ │ │ │ │ +130 if (n == 0) return traits::Identity(); │ │ │ │ │ +131 else if (n == 1) return g; │ │ │ │ │ +132 else return traits::Compose(compose_pow(g, n - 1), g); │ │ │ │ │ +133} │ │ │ │ │ +134 │ │ │ │ │ +137template │ │ │ │ │ +_1_3_8class _D_i_r_e_c_t_P_r_o_d_u_c_t: public std::pair { │ │ │ │ │ +139 BOOST_CONCEPT_ASSERT((_I_s_G_r_o_u_p_<_G_>)); │ │ │ │ │ +140 BOOST_CONCEPT_ASSERT((_I_s_G_r_o_u_p_<_H_>)); │ │ │ │ │ +141 │ │ │ │ │ +142public: │ │ │ │ │ +_1_4_4 _D_i_r_e_c_t_P_r_o_d_u_c_t():std::pair(_t_r_a_i_t_s::Identity(),_t_r_a_i_t_s::Identity()) │ │ │ │ │ +{} │ │ │ │ │ +145 │ │ │ │ │ +146 // Construct from two subgroup elements │ │ │ │ │ +147 _D_i_r_e_c_t_P_r_o_d_u_c_t(const G& g, const H& h):std::pair(g,h) {} │ │ │ │ │ +148 │ │ │ │ │ +149 // identity │ │ │ │ │ +150 static _D_i_r_e_c_t_P_r_o_d_u_c_t Identity() { return _D_i_r_e_c_t_P_r_o_d_u_c_t(); } │ │ │ │ │ +151 │ │ │ │ │ +152 _D_i_r_e_c_t_P_r_o_d_u_c_t operator*(const _D_i_r_e_c_t_P_r_o_d_u_c_t& other) const { │ │ │ │ │ +153 return _D_i_r_e_c_t_P_r_o_d_u_c_t(traits::Compose(this->first, other.first), │ │ │ │ │ +154 traits::Compose(this->second, other.second)); │ │ │ │ │ +155 } │ │ │ │ │ +156 _D_i_r_e_c_t_P_r_o_d_u_c_t inverse() const { │ │ │ │ │ +157 return _D_i_r_e_c_t_P_r_o_d_u_c_t(this->first.inverse(), this->second.inverse()); │ │ │ │ │ +158 } │ │ │ │ │ +159}; │ │ │ │ │ +160 │ │ │ │ │ +161// Define any direct product group to be a model of the multiplicative Group │ │ │ │ │ +concept │ │ │ │ │ +162template │ │ │ │ │ +_1_6_3struct _t_r_a_i_t_s<_D_i_r_e_c_t_P_r_o_d_u_c_t > : │ │ │ │ │ +164 internal::MultiplicativeGroupTraits > {}; │ │ │ │ │ +165 │ │ │ │ │ +168template │ │ │ │ │ +_1_6_9class _D_i_r_e_c_t_S_u_m: public std::pair { │ │ │ │ │ +170 BOOST_CONCEPT_ASSERT((_I_s_G_r_o_u_p_<_G_>)); // TODO(frank): check additive │ │ │ │ │ +171 BOOST_CONCEPT_ASSERT((_I_s_G_r_o_u_p_<_H_>)); // TODO(frank): check additive │ │ │ │ │ +172 │ │ │ │ │ +173 const G& g() const { return this->first; } │ │ │ │ │ +174 const H& h() const { return this->second;} │ │ │ │ │ +175 │ │ │ │ │ +176public: │ │ │ │ │ +_1_7_8 _D_i_r_e_c_t_S_u_m():std::pair(_t_r_a_i_t_s::Identity(),_t_r_a_i_t_s::Identity()) {} │ │ │ │ │ +179 │ │ │ │ │ +180 // Construct from two subgroup elements │ │ │ │ │ +181 _D_i_r_e_c_t_S_u_m(const G& g, const H& h):std::pair(g,h) {} │ │ │ │ │ +182 │ │ │ │ │ +183 // identity │ │ │ │ │ +184 static _D_i_r_e_c_t_S_u_m Identity() { return _D_i_r_e_c_t_S_u_m(); } │ │ │ │ │ +185 │ │ │ │ │ +186 _D_i_r_e_c_t_S_u_m operator+(const _D_i_r_e_c_t_S_u_m& other) const { │ │ │ │ │ +187 return _D_i_r_e_c_t_S_u_m(g()+other.g(), h()+other.h()); │ │ │ │ │ +188 } │ │ │ │ │ +189 _D_i_r_e_c_t_S_u_m operator-(const _D_i_r_e_c_t_S_u_m& other) const { │ │ │ │ │ +190 return _D_i_r_e_c_t_S_u_m(g()-other.g(), h()-other.h()); │ │ │ │ │ +191 } │ │ │ │ │ +192 _D_i_r_e_c_t_S_u_m operator-() const { │ │ │ │ │ +193 return _D_i_r_e_c_t_S_u_m(- g(), - h()); │ │ │ │ │ +194 } │ │ │ │ │ +195}; │ │ │ │ │ +196 │ │ │ │ │ +197// Define direct sums to be a model of the Additive Group concept │ │ │ │ │ +198template │ │ │ │ │ +_1_9_9struct _t_r_a_i_t_s<_D_i_r_e_c_t_S_u_m > : │ │ │ │ │ +200 internal::AdditiveGroupTraits > {}; │ │ │ │ │ +201 │ │ │ │ │ +202} // namespace gtsam │ │ │ │ │ +203 │ │ │ │ │ +_2_1_2#define GTSAM_CONCEPT_GROUP_INST(T) template class gtsam::IsGroup; │ │ │ │ │ +213#define GTSAM_CONCEPT_GROUP_TYPE(T) typedef gtsam::IsGroup │ │ │ │ │ +_gtsam_IsGroup_##T; │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ -Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor │ │ │ │ │ -symbol GTSAM_USE_QUATERNIO... │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3_:_:_t_r_a_n_s_p_o_s_e │ │ │ │ │ -Matrix3 transpose() const │ │ │ │ │ -Return 3*3 transpose (inverse) rotation matrix. │ │ │ │ │ -DDeeffiinniittiioonn Rot3M.cpp:144 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_G_a_u_s_s_i_a_n_:_:_C_o_v_a_r_i_a_n_c_e │ │ │ │ │ -static shared_ptr Covariance(const Matrix &covariance, bool smart=true) │ │ │ │ │ -A Gaussian noise model created by specifying a covariance matrix. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.cpp:117 │ │ │ │ │ -_g_t_s_a_m_:_:_S_a_m_p_l_e_r │ │ │ │ │ -Sampling structure that keeps internal random number generators for diagonal │ │ │ │ │ -distributions specified ... │ │ │ │ │ -DDeeffiinniittiioonn Sampler.h:31 │ │ │ │ │ -_g_t_s_a_m_:_:_S_a_m_p_l_e_r_:_:_s_a_m_p_l_e │ │ │ │ │ -Vector sample() const │ │ │ │ │ -sample from distribution │ │ │ │ │ -DDeeffiinniittiioonn Sampler.cpp:59 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_C_o_m_b_i_n_e_d_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ -PreintegratedCombinedMeasurements integrates the IMU measurements (rotation │ │ │ │ │ -rates and accelerations) ... │ │ │ │ │ -DDeeffiinniittiioonn CombinedImuFactor.h:129 │ │ │ │ │ -_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s │ │ │ │ │ -DDeeffiinniittiioonn ImuBias.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_:_:_g_y_r_o_s_c_o_p_e │ │ │ │ │ -const Vector3 & gyroscope() const │ │ │ │ │ -get gyroscope bias │ │ │ │ │ -DDeeffiinniittiioonn ImuBias.h:69 │ │ │ │ │ -_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_:_:_a_c_c_e_l_e_r_o_m_e_t_e_r │ │ │ │ │ -const Vector3 & accelerometer() const │ │ │ │ │ -get accelerometer bias │ │ │ │ │ -DDeeffiinniittiioonn ImuBias.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_I_m_u_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ -PreintegratedImuMeasurements accumulates (integrates) the IMU measurements │ │ │ │ │ -(rotation rates and accele... │ │ │ │ │ -DDeeffiinniittiioonn ImuFactor.h:72 │ │ │ │ │ -_g_t_s_a_m_:_:_N_a_v_S_t_a_t_e │ │ │ │ │ -Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does │ │ │ │ │ -not make sense to make... │ │ │ │ │ -DDeeffiinniittiioonn NavState.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_S_c_e_n_a_r_i_o │ │ │ │ │ -Simple trajectory simulator. │ │ │ │ │ -DDeeffiinniittiioonn Scenario.h:25 │ │ │ │ │ -_g_t_s_a_m_:_:_S_c_e_n_a_r_i_o_:_:_o_m_e_g_a___b │ │ │ │ │ -virtual Vector3 omega_b(double t) const =0 │ │ │ │ │ -angular velocity in body frame │ │ │ │ │ -_g_t_s_a_m_:_:_S_c_e_n_a_r_i_o_R_u_n_n_e_r │ │ │ │ │ -DDeeffiinniittiioonn ScenarioRunner.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_C_o_m_b_i_n_e_d_S_c_e_n_a_r_i_o_R_u_n_n_e_r │ │ │ │ │ -DDeeffiinniittiioonn ScenarioRunner.h:114 │ │ │ │ │ +_g_t_s_a_m_:_:_B_O_O_S_T___C_O_N_C_E_P_T___R_E_Q_U_I_R_E_S │ │ │ │ │ +BOOST_CONCEPT_REQUIRES(((IsGroup< G >)),(bool)) check_group_invariants(const G │ │ │ │ │ +&a │ │ │ │ │ +Check invariants. │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_g_r_o_u_p___t_a_g │ │ │ │ │ +tag to assert a type is a group │ │ │ │ │ +DDeeffiinniittiioonn Group.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_m_u_l_t_i_p_l_i_c_a_t_i_v_e___g_r_o_u_p___t_a_g │ │ │ │ │ +Group operator syntax flavors. │ │ │ │ │ +DDeeffiinniittiioonn Group.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_a_d_d_i_t_i_v_e___g_r_o_u_p___t_a_g │ │ │ │ │ +DDeeffiinniittiioonn Group.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_I_s_G_r_o_u_p │ │ │ │ │ +Group Concept. │ │ │ │ │ +DDeeffiinniittiioonn Group.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_r_e_c_t_P_r_o_d_u_c_t │ │ │ │ │ +DDeeffiinniittiioonn Group.h:138 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_r_e_c_t_P_r_o_d_u_c_t_:_:_D_i_r_e_c_t_P_r_o_d_u_c_t │ │ │ │ │ +DirectProduct() │ │ │ │ │ +Default constructor yields identity. │ │ │ │ │ +DDeeffiinniittiioonn Group.h:144 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_r_e_c_t_S_u_m │ │ │ │ │ +Template to construct the direct sum of two additive groups Assumes existence │ │ │ │ │ +of three additive opera... │ │ │ │ │ +DDeeffiinniittiioonn Group.h:169 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_r_e_c_t_S_u_m_:_:_D_i_r_e_c_t_S_u_m │ │ │ │ │ +DirectSum() │ │ │ │ │ +Default constructor yields identity. │ │ │ │ │ +DDeeffiinniittiioonn Group.h:178 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _S_c_e_n_a_r_i_o_R_u_n_n_e_r_._h │ │ │ │ │ + * _b_a_s_e │ │ │ │ │ + * _G_r_o_u_p_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00926_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00971_source.html │ │ │ │┄ Files 99% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
ConstantVelocityFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
18#pragma once
│ │ │ │
19
│ │ │ │ -
20#include <gtsam/navigation/NavState.h>
│ │ │ │ -
21#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
20#include <gtsam/navigation/NavState.h>
│ │ │ │ +
21#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │
22
│ │ │ │
23namespace gtsam {
│ │ │ │
24
│ │ │ │
│ │ │ │
29class ConstantVelocityFactor : public NoiseModelFactorN<NavState, NavState> {
│ │ │ │
30 double dt_;
│ │ │ │
31
│ │ │ │ @@ -155,16 +155,16 @@ │ │ │ │
66 return error;
│ │ │ │
67 }
│ │ │ │
│ │ │ │
68};
│ │ │ │
│ │ │ │
69
│ │ │ │
70} // namespace gtsam
│ │ │ │ -
NavState.h
Navigation state composing of attitude, position, and velocity.
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
NavState.h
Navigation state composing of attitude, position, and velocity.
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::ConstantVelocityFactor
Binary factor for applying a constant velocity model to a moving body represented as a NavState.
Definition ConstantVelocityFactor.h:29
│ │ │ │
gtsam::ConstantVelocityFactor::evaluateError
gtsam::Vector evaluateError(const NavState &x1, const NavState &x2, boost::optional< gtsam::Matrix & > H1=boost::none, boost::optional< gtsam::Matrix & > H2=boost::none) const override
Caclulate error: (x2 - x1.update(dt))) where X1 and X1 are NavStates and dt is the time difference in...
Definition ConstantVelocityFactor.h:50
│ │ │ │
gtsam::NavState
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition NavState.h:34
│ │ │ │
gtsam::NavState::localCoordinates
Vector9 localCoordinates(const NavState &g, OptionalJacobian< 9, 9 > H1=boost::none, OptionalJacobian< 9, 9 > H2=boost::none) const
localCoordinates with optional derivatives
Definition NavState.cpp:135
│ │ │ │ @@ -172,13 +172,13 @@ │ │ │ │
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Calculate the error of the factor.
Definition NonlinearFactor.cpp:138
│ │ │ │
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00941_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00839_source.html │ │ │ │┄ Files 94% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AHRSFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/RegularJacobianFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
AHRSFactor.h
│ │ │ │ +
RegularJacobianFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22/* GTSAM includes */
│ │ │ │ -
23#include <gtsam/navigation/PreintegratedRotation.h>
│ │ │ │ -
24#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
25#include <gtsam/geometry/Pose3.h>
│ │ │ │ -
26
│ │ │ │ -
27namespace gtsam {
│ │ │ │ -
28
│ │ │ │ -
│ │ │ │ -
34class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation {
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/linear/JacobianFactor.h>
│ │ │ │ +
22#include <gtsam/linear/VectorValues.h>
│ │ │ │ +
23
│ │ │ │ +
24namespace gtsam {
│ │ │ │ +
25
│ │ │ │ +
31template<size_t D>
│ │ │ │ +
│ │ │ │ +
32class RegularJacobianFactor: public JacobianFactor {
│ │ │ │ +
33
│ │ │ │ +
34private:
│ │ │ │
35
│ │ │ │ -
36 protected:
│ │ │ │ -
37
│ │ │ │ -
38 Vector3 biasHat_;
│ │ │ │ -
39 Matrix3 preintMeasCov_;
│ │ │ │ +
36 // Use eigen magic to access raw memory
│ │ │ │ +
37 typedef Eigen::Matrix<double, D, 1> DVector;
│ │ │ │ +
38 typedef Eigen::Map<DVector> DMap;
│ │ │ │ +
39 typedef Eigen::Map<const DVector> ConstDMap;
│ │ │ │
40
│ │ │ │ -
41 friend class AHRSFactor;
│ │ │ │ +
41public:
│ │ │ │
42
│ │ │ │ -
43 public:
│ │ │ │ -
44
│ │ │ │ -
46 PreintegratedAhrsMeasurements() {}
│ │ │ │ -
47
│ │ │ │ +
44 RegularJacobianFactor() {}
│ │ │ │ +
45
│ │ │ │ +
51 template<typename TERMS>
│ │ │ │
│ │ │ │ -
52 PreintegratedAhrsMeasurements(const boost::shared_ptr<Params>& p,
│ │ │ │ -
53 const Vector3& biasHat) :
│ │ │ │ -
54 PreintegratedRotation(p), biasHat_(biasHat) {
│ │ │ │ -
55 resetIntegration();
│ │ │ │ -
56 }
│ │ │ │ -
│ │ │ │ -
57
│ │ │ │ -
│ │ │ │ -
67 PreintegratedAhrsMeasurements(
│ │ │ │ -
68 const boost::shared_ptr<Params>& p,
│ │ │ │ -
69 const Vector3& bias_hat,
│ │ │ │ -
70 double deltaTij,
│ │ │ │ -
71 const Rot3& deltaRij,
│ │ │ │ -
72 const Matrix3& delRdelBiasOmega,
│ │ │ │ -
73 const Matrix3& preint_meas_cov) :
│ │ │ │ -
74 PreintegratedRotation(p, deltaTij, deltaRij, delRdelBiasOmega),
│ │ │ │ -
75 biasHat_(bias_hat),
│ │ │ │ -
76 preintMeasCov_(preint_meas_cov) {}
│ │ │ │ +
52 RegularJacobianFactor(const TERMS& terms, const Vector& b,
│ │ │ │ +
53 const SharedDiagonal& model = SharedDiagonal()) :
│ │ │ │ +
54 JacobianFactor(terms, b, model) {
│ │ │ │ +
55 }
│ │ │ │ +
│ │ │ │ +
56
│ │ │ │ +
63 template<typename KEYS>
│ │ │ │ +
│ │ │ │ +
64 RegularJacobianFactor(const KEYS& keys,
│ │ │ │ +
65 const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas =
│ │ │ │ +
66 SharedDiagonal()) :
│ │ │ │ +
67 JacobianFactor(keys, augmentedMatrix, sigmas) {
│ │ │ │ +
68 }
│ │ │ │ +
│ │ │ │ +
69
│ │ │ │ +
70 using JacobianFactor::multiplyHessianAdd;
│ │ │ │ +
71
│ │ │ │ +
│ │ │ │ +
73 void multiplyHessianAdd(double alpha, const VectorValues& x,
│ │ │ │ +
74 VectorValues& y) const override {
│ │ │ │ +
75 JacobianFactor::multiplyHessianAdd(alpha, x, y);
│ │ │ │ +
76 }
│ │ │ │
│ │ │ │
77
│ │ │ │ -
78 Params& p() const { return *boost::static_pointer_cast<Params>(p_);}
│ │ │ │ -
79 const Vector3& biasHat() const { return biasHat_; }
│ │ │ │ -
80 const Matrix3& preintMeasCov() const { return preintMeasCov_; }
│ │ │ │ -
81
│ │ │ │ -
83 void print(const std::string& s = "Preintegrated Measurements: ") const;
│ │ │ │ -
84
│ │ │ │ -
86 bool equals(const PreintegratedAhrsMeasurements&, double tol = 1e-9) const;
│ │ │ │ -
87
│ │ │ │ -
89 void resetIntegration();
│ │ │ │ +
│ │ │ │ +
82 void multiplyHessianAdd(double alpha, const double* x, double* y) const {
│ │ │ │ +
83 if (empty())
│ │ │ │ +
84 return;
│ │ │ │ +
85 Vector Ax = Vector::Zero(Ab_.rows());
│ │ │ │ +
86
│ │ │ │ +
87 // Just iterate over all A matrices and multiply in correct config part
│ │ │ │ +
88 for (size_t pos = 0; pos < size(); ++pos)
│ │ │ │ +
89 Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]);
│ │ │ │
90
│ │ │ │ -
100 void integrateMeasurement(const Vector3& measuredOmega, double deltaT);
│ │ │ │ -
101
│ │ │ │ -
104 Vector3 predict(const Vector3& bias, OptionalJacobian<3,3> H = boost::none) const;
│ │ │ │ -
105
│ │ │ │ -
106 // This function is only used for test purposes
│ │ │ │ -
107 // (compare numerical derivatives wrt analytic ones)
│ │ │ │ -
108 static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt,
│ │ │ │ -
109 const Vector3& delta_angles);
│ │ │ │ -
110
│ │ │ │ -
│ │ │ │ -
112 PreintegratedAhrsMeasurements(const Vector3& biasHat,
│ │ │ │ -
113 const Matrix3& measuredOmegaCovariance)
│ │ │ │ -
114 : PreintegratedRotation(boost::make_shared<Params>()), biasHat_(biasHat) {
│ │ │ │ -
115 p_->gyroscopeCovariance = measuredOmegaCovariance;
│ │ │ │ -
116 resetIntegration();
│ │ │ │ -
117 }
│ │ │ │ -
│ │ │ │ -
118
│ │ │ │ -
119private:
│ │ │ │ -
120
│ │ │ │ -
122 friend class boost::serialization::access;
│ │ │ │ -
123 template<class ARCHIVE>
│ │ │ │ -
124 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
125 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation);
│ │ │ │ -
126 ar & BOOST_SERIALIZATION_NVP(p_);
│ │ │ │ -
127 ar & BOOST_SERIALIZATION_NVP(biasHat_);
│ │ │ │ -
128 }
│ │ │ │ -
129};
│ │ │ │ -
│ │ │ │ -
130
│ │ │ │ -
│ │ │ │ -
131class GTSAM_EXPORT AHRSFactor: public NoiseModelFactorN<Rot3, Rot3, Vector3> {
│ │ │ │ -
132
│ │ │ │ -
133 typedef AHRSFactor This;
│ │ │ │ -
134 typedef NoiseModelFactorN<Rot3, Rot3, Vector3> Base;
│ │ │ │ -
135
│ │ │ │ -
136 PreintegratedAhrsMeasurements _PIM_;
│ │ │ │ +
91 // Deal with noise properly, need to Double* whiten as we are dividing by variance
│ │ │ │ +
92 if (model_) {
│ │ │ │ +
93 model_->whitenInPlace(Ax);
│ │ │ │ +
94 model_->whitenInPlace(Ax);
│ │ │ │ +
95 }
│ │ │ │ +
96
│ │ │ │ +
97 // multiply with alpha
│ │ │ │ +
98 Ax *= alpha;
│ │ │ │ +
99
│ │ │ │ +
100 // Again iterate over all A matrices and insert Ai^e into y
│ │ │ │ +
101 for (size_t pos = 0; pos < size(); ++pos)
│ │ │ │ +
102 DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax;
│ │ │ │ +
103 }
│ │ │ │ +
│ │ │ │ +
104
│ │ │ │ +
106 using GaussianFactor::hessianDiagonal;
│ │ │ │ +
107
│ │ │ │ +
│ │ │ │ +
109 void hessianDiagonal(double* d) const override {
│ │ │ │ +
110 // Loop over all variables in the factor
│ │ │ │ +
111 for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
│ │ │ │ +
112 // Get the diagonal block, and insert its diagonal
│ │ │ │ +
113 DVector dj;
│ │ │ │ +
114 for (size_t k = 0; k < D; ++k) {
│ │ │ │ +
115 if (model_) {
│ │ │ │ +
116 Vector column_k = Ab_(j).col(k);
│ │ │ │ +
117 column_k = model_->whiten(column_k);
│ │ │ │ +
118 dj(k) = dot(column_k, column_k);
│ │ │ │ +
119 } else {
│ │ │ │ +
120 dj(k) = Ab_(j).col(k).squaredNorm();
│ │ │ │ +
121 }
│ │ │ │ +
122 }
│ │ │ │ +
123 DMap(d + D * j) += dj;
│ │ │ │ +
124 }
│ │ │ │ +
125 }
│ │ │ │ +
│ │ │ │ +
126
│ │ │ │ +
│ │ │ │ +
128 VectorValues gradientAtZero() const override {
│ │ │ │ +
129 return JacobianFactor::gradientAtZero();
│ │ │ │ +
130 }
│ │ │ │ +
│ │ │ │ +
131
│ │ │ │ +
│ │ │ │ +
133 void gradientAtZero(double* d) const override {
│ │ │ │ +
134
│ │ │ │ +
135 // Get vector b not weighted
│ │ │ │ +
136 Vector b = getb();
│ │ │ │
137
│ │ │ │ -
139 AHRSFactor() {}
│ │ │ │ -
140
│ │ │ │ -
141public:
│ │ │ │ -
142
│ │ │ │ -
144#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
│ │ │ │ -
145 typedef typename boost::shared_ptr<AHRSFactor> shared_ptr;
│ │ │ │ -
146#else
│ │ │ │ -
147 typedef boost::shared_ptr<AHRSFactor> shared_ptr;
│ │ │ │ -
148#endif
│ │ │ │ -
149
│ │ │ │ -
157 AHRSFactor(Key rot_i, Key rot_j, Key bias,
│ │ │ │ -
158 const PreintegratedAhrsMeasurements& preintegratedMeasurements);
│ │ │ │ -
159
│ │ │ │ -
160 ~AHRSFactor() override {
│ │ │ │ -
161 }
│ │ │ │ -
162
│ │ │ │ -
164 gtsam::NonlinearFactor::shared_ptr clone() const override;
│ │ │ │ -
165
│ │ │ │ -
167 void print(const std::string& s, const KeyFormatter& keyFormatter =
│ │ │ │ -
168 DefaultKeyFormatter) const override;
│ │ │ │ -
169
│ │ │ │ -
171 bool equals(const NonlinearFactor&, double tol = 1e-9) const override;
│ │ │ │ -
172
│ │ │ │ -
│ │ │ │ -
174 const PreintegratedAhrsMeasurements& preintegratedMeasurements() const {
│ │ │ │ -
175 return _PIM_;
│ │ │ │ -
176 }
│ │ │ │ -
│ │ │ │ -
177
│ │ │ │ -
181 Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j,
│ │ │ │ -
182 const Vector3& bias, boost::optional<Matrix&> H1 = boost::none,
│ │ │ │ -
183 boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
│ │ │ │ -
184 boost::none) const override;
│ │ │ │ -
185
│ │ │ │ -
188 static Rot3 Predict(const Rot3& rot_i, const Vector3& bias,
│ │ │ │ -
189 const PreintegratedAhrsMeasurements& pim);
│ │ │ │ -
190
│ │ │ │ -
192 AHRSFactor(Key rot_i, Key rot_j, Key bias,
│ │ │ │ -
193 const PreintegratedAhrsMeasurements& pim,
│ │ │ │ -
194 const Vector3& omegaCoriolis,
│ │ │ │ -
195 const boost::optional<Pose3>& body_P_sensor = boost::none);
│ │ │ │ -
196
│ │ │ │ -
198 static Rot3 predict(
│ │ │ │ -
199 const Rot3& rot_i, const Vector3& bias,
│ │ │ │ -
200 const PreintegratedAhrsMeasurements& pim, const Vector3& omegaCoriolis,
│ │ │ │ -
201 const boost::optional<Pose3>& body_P_sensor = boost::none);
│ │ │ │ -
202
│ │ │ │ -
203#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ -
205 typedef PreintegratedAhrsMeasurements PreintegratedMeasurements;
│ │ │ │ -
206
│ │ │ │ -
207#endif
│ │ │ │ -
208
│ │ │ │ -
209private:
│ │ │ │ -
210
│ │ │ │ -
212 friend class boost::serialization::access;
│ │ │ │ -
213 template<class ARCHIVE>
│ │ │ │ -
214 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
215 // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ -
216 ar
│ │ │ │ -
217 & boost::serialization::make_nvp("NoiseModelFactor3",
│ │ │ │ -
218 boost::serialization::base_object<Base>(*this));
│ │ │ │ -
219 ar & BOOST_SERIALIZATION_NVP(_PIM_);
│ │ │ │ -
220 }
│ │ │ │ -
221
│ │ │ │ -
222};
│ │ │ │ -
│ │ │ │ -
223// AHRSFactor
│ │ │ │ -
224
│ │ │ │ -
225} //namespace gtsam
│ │ │ │ -
Pose3.h
3D Pose
│ │ │ │ - │ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
138 // Whitening b
│ │ │ │ +
139 if (model_) {
│ │ │ │ +
140 b = model_->whiten(b);
│ │ │ │ +
141 b = model_->whiten(b);
│ │ │ │ +
142 }
│ │ │ │ +
143
│ │ │ │ +
144 // Just iterate over all A matrices
│ │ │ │ +
145 for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
│ │ │ │ +
146 DVector dj;
│ │ │ │ +
147 // gradient -= A'*b/sigma^2
│ │ │ │ +
148 // Computing with each column
│ │ │ │ +
149 for (size_t k = 0; k < D; ++k) {
│ │ │ │ +
150 Vector column_k = Ab_(j).col(k);
│ │ │ │ +
151 dj(k) = -1.0 * dot(b, column_k);
│ │ │ │ +
152 }
│ │ │ │ +
153 DMap(d + D * j) += dj;
│ │ │ │ +
154 }
│ │ │ │ +
155 }
│ │ │ │ +
│ │ │ │ +
156
│ │ │ │ +
│ │ │ │ +
161 void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const {
│ │ │ │ +
162 Vector E = alpha * (model_ ? model_->whiten(e) : e);
│ │ │ │ +
163 // Just iterate over all A matrices and insert Ai^e into y
│ │ │ │ +
164 for (size_t pos = 0; pos < size(); ++pos)
│ │ │ │ +
165 DMap(x + D * keys_[pos]) += Ab_(pos).transpose() * E;
│ │ │ │ +
166 }
│ │ │ │ +
│ │ │ │ +
167
│ │ │ │ +
│ │ │ │ +
172 Vector operator*(const double* x) const {
│ │ │ │ +
173 Vector Ax = Vector::Zero(Ab_.rows());
│ │ │ │ +
174 if (empty())
│ │ │ │ +
175 return Ax;
│ │ │ │ +
176
│ │ │ │ +
177 // Just iterate over all A matrices and multiply in correct config part
│ │ │ │ +
178 for (size_t pos = 0; pos < size(); ++pos)
│ │ │ │ +
179 Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]);
│ │ │ │ +
180
│ │ │ │ +
181 return model_ ? model_->whiten(Ax) : Ax;
│ │ │ │ +
182 }
│ │ │ │ +
│ │ │ │ +
183
│ │ │ │ +
184};
│ │ │ │ +
│ │ │ │ +
185// end class RegularJacobianFactor
│ │ │ │ +
186
│ │ │ │ +
187}// end namespace gtsam
│ │ │ │ + │ │ │ │ +
VectorValues.h
Factor Graph Values.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::make_shared
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost::shared_ptr< T > > make_shared(Args &&... args)
Add our own make_shared as a layer of wrapping on boost::make_shared This solves the problem with the...
Definition make_shared.h:57
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
│ │ │ │ -
gtsam::PreintegratedAhrsMeasurements
PreintegratedAHRSMeasurements accumulates (integrates) the Gyroscope measurements (rotation rates) an...
Definition AHRSFactor.h:34
│ │ │ │ -
gtsam::PreintegratedAhrsMeasurements::biasHat_
Vector3 biasHat_
Angular rate bias values used during preintegration.
Definition AHRSFactor.h:38
│ │ │ │ -
gtsam::PreintegratedAhrsMeasurements::PreintegratedAhrsMeasurements
PreintegratedAhrsMeasurements(const Vector3 &biasHat, const Matrix3 &measuredOmegaCovariance)
Definition AHRSFactor.h:112
│ │ │ │ -
gtsam::PreintegratedAhrsMeasurements::PreintegratedAhrsMeasurements
PreintegratedAhrsMeasurements(const boost::shared_ptr< Params > &p, const Vector3 &biasHat)
Default constructor, initialize with no measurements.
Definition AHRSFactor.h:52
│ │ │ │ -
gtsam::PreintegratedAhrsMeasurements::preintMeasCov_
Matrix3 preintMeasCov_
Covariance matrix of the preintegrated measurements (first-order propagation from measurementCovarian...
Definition AHRSFactor.h:39
│ │ │ │ -
gtsam::PreintegratedAhrsMeasurements::PreintegratedAhrsMeasurements
PreintegratedAhrsMeasurements(const boost::shared_ptr< Params > &p, const Vector3 &bias_hat, double deltaTij, const Rot3 &deltaRij, const Matrix3 &delRdelBiasOmega, const Matrix3 &preint_meas_cov)
Non-Default constructor, initialize with measurements.
Definition AHRSFactor.h:67
│ │ │ │ -
gtsam::PreintegratedAhrsMeasurements::PreintegratedAhrsMeasurements
PreintegratedAhrsMeasurements()
Default constructor, only for serialization and wrappers.
Definition AHRSFactor.h:46
│ │ │ │ -
gtsam::AHRSFactor
Definition AHRSFactor.h:131
│ │ │ │ -
gtsam::AHRSFactor::preintegratedMeasurements
const PreintegratedAhrsMeasurements & preintegratedMeasurements() const
Access the preintegrated measurements.
Definition AHRSFactor.h:174
│ │ │ │ -
gtsam::AHRSFactor::shared_ptr
boost::shared_ptr< AHRSFactor > shared_ptr
Shorthand for a smart pointer to a factor.
Definition AHRSFactor.h:147
│ │ │ │ -
gtsam::PreintegratedRotationParams
Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
Definition PreintegratedRotation.h:31
│ │ │ │ -
gtsam::PreintegratedRotation
PreintegratedRotation is the base class for all PreintegratedMeasurements classes (in AHRSFactor,...
Definition PreintegratedRotation.h:89
│ │ │ │ -
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ -
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ +
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │ +
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
│ │ │ │ +
gtsam::VerticalBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks.
Definition VerticalBlockMatrix.h:43
│ │ │ │ +
gtsam::VerticalBlockMatrix::rows
DenseIndex rows() const
Row size.
Definition VerticalBlockMatrix.h:115
│ │ │ │ +
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
│ │ │ │ +
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
│ │ │ │ +
gtsam::Factor::empty
bool empty() const
Whether the factor is empty (involves zero variables).
Definition Factor.h:128
│ │ │ │ +
gtsam::Factor::size
size_t size() const
Definition Factor.h:157
│ │ │ │ +
gtsam::GaussianFactor::hessianDiagonal
VectorValues hessianDiagonal() const
Return the diagonal of the Hessian for this factor.
Definition GaussianFactor.cpp:35
│ │ │ │ +
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition JacobianFactor.h:91
│ │ │ │ +
gtsam::JacobianFactor::getb
const constBVector getb() const
Get a view of the r.h.s.
Definition JacobianFactor.h:297
│ │ │ │ +
gtsam::JacobianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
y += alpha * A'*A*x
Definition JacobianFactor.cpp:649
│ │ │ │ +
gtsam::JacobianFactor::gradientAtZero
VectorValues gradientAtZero() const override
A'*b for Jacobian.
Definition JacobianFactor.cpp:701
│ │ │ │ +
gtsam::RegularJacobianFactor
JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator.
Definition RegularJacobianFactor.h:32
│ │ │ │ +
gtsam::RegularJacobianFactor::operator*
Vector operator*(const double *x) const
double* Matrix-vector multiply, i.e.
Definition RegularJacobianFactor.h:172
│ │ │ │ +
gtsam::RegularJacobianFactor::hessianDiagonal
void hessianDiagonal(double *d) const override
Raw memory access version of hessianDiagonal.
Definition RegularJacobianFactor.h:109
│ │ │ │ +
gtsam::RegularJacobianFactor::gradientAtZero
void gradientAtZero(double *d) const override
Raw memory access version of gradientAtZero.
Definition RegularJacobianFactor.h:133
│ │ │ │ +
gtsam::RegularJacobianFactor::transposeMultiplyAdd
void transposeMultiplyAdd(double alpha, const Vector &e, double *x) const
double* Transpose Matrix-vector multiply, i.e.
Definition RegularJacobianFactor.h:161
│ │ │ │ +
gtsam::RegularJacobianFactor::gradientAtZero
VectorValues gradientAtZero() const override
Expose base class gradientAtZero.
Definition RegularJacobianFactor.h:128
│ │ │ │ +
gtsam::RegularJacobianFactor::RegularJacobianFactor
RegularJacobianFactor(const TERMS &terms, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Construct an n-ary factor.
Definition RegularJacobianFactor.h:52
│ │ │ │ +
gtsam::RegularJacobianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) const override
y += alpha * A'*A*x
Definition RegularJacobianFactor.h:73
│ │ │ │ +
gtsam::RegularJacobianFactor::RegularJacobianFactor
RegularJacobianFactor()
Default constructor.
Definition RegularJacobianFactor.h:44
│ │ │ │ +
gtsam::RegularJacobianFactor::RegularJacobianFactor
RegularJacobianFactor(const KEYS &keys, const VerticalBlockMatrix &augmentedMatrix, const SharedDiagonal &sigmas=SharedDiagonal())
Constructor with arbitrary number keys, and where the augmented matrix is given all together instead ...
Definition RegularJacobianFactor.h:64
│ │ │ │ +
gtsam::RegularJacobianFactor::multiplyHessianAdd
void multiplyHessianAdd(double alpha, const double *x, double *y) const
double* Hessian-vector multiply, i.e.
Definition RegularJacobianFactor.h:82
│ │ │ │ +
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,267 +1,269 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -AHRSFactor.h │ │ │ │ │ +RegularJacobianFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22/* GTSAM includes */ │ │ │ │ │ -23#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_3_._h> │ │ │ │ │ -26 │ │ │ │ │ -27namespace _g_t_s_a_m { │ │ │ │ │ -28 │ │ │ │ │ -_3_4class GTSAM_EXPORT _P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s : public │ │ │ │ │ -_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n { │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ +23 │ │ │ │ │ +24namespace _g_t_s_a_m { │ │ │ │ │ +25 │ │ │ │ │ +31template │ │ │ │ │ +_3_2class _R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r: public _J_a_c_o_b_i_a_n_F_a_c_t_o_r { │ │ │ │ │ +33 │ │ │ │ │ +34private: │ │ │ │ │ 35 │ │ │ │ │ -36 protected: │ │ │ │ │ -37 │ │ │ │ │ -_3_8 Vector3 _b_i_a_s_H_a_t__; │ │ │ │ │ -_3_9 Matrix3 _p_r_e_i_n_t_M_e_a_s_C_o_v__; │ │ │ │ │ +36 // Use eigen magic to access raw memory │ │ │ │ │ +37 typedef Eigen::Matrix DVector; │ │ │ │ │ +38 typedef Eigen::Map DMap; │ │ │ │ │ +39 typedef Eigen::Map ConstDMap; │ │ │ │ │ 40 │ │ │ │ │ -41 friend class _A_H_R_S_F_a_c_t_o_r; │ │ │ │ │ +41public: │ │ │ │ │ 42 │ │ │ │ │ -43 public: │ │ │ │ │ -44 │ │ │ │ │ -_4_6 _P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s() {} │ │ │ │ │ -47 │ │ │ │ │ -_5_2 _P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s(const boost::shared_ptr& p, │ │ │ │ │ -53 const Vector3& biasHat) : │ │ │ │ │ -54 _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n(p), biasHat_(biasHat) { │ │ │ │ │ -55 resetIntegration(); │ │ │ │ │ -56 } │ │ │ │ │ -57 │ │ │ │ │ -_6_7 _P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s( │ │ │ │ │ -68 const boost::shared_ptr& p, │ │ │ │ │ -69 const Vector3& bias_hat, │ │ │ │ │ -70 double deltaTij, │ │ │ │ │ -71 const _R_o_t_3& deltaRij, │ │ │ │ │ -72 const Matrix3& delRdelBiasOmega, │ │ │ │ │ -73 const Matrix3& preint_meas_cov) : │ │ │ │ │ -74 _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n(p, deltaTij, deltaRij, delRdelBiasOmega), │ │ │ │ │ -75 biasHat_(bias_hat), │ │ │ │ │ -76 preintMeasCov_(preint_meas_cov) {} │ │ │ │ │ +_4_4 _R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r() {} │ │ │ │ │ +45 │ │ │ │ │ +51 template │ │ │ │ │ +_5_2 _R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r(const TERMS& terms, const Vector& b, │ │ │ │ │ +53 const SharedDiagonal& model = SharedDiagonal()) : │ │ │ │ │ +54 _J_a_c_o_b_i_a_n_F_a_c_t_o_r(terms, b, model) { │ │ │ │ │ +55 } │ │ │ │ │ +56 │ │ │ │ │ +63 template │ │ │ │ │ +_6_4 _R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r(const KEYS& _k_e_y_s, │ │ │ │ │ +65 const _V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x& augmentedMatrix, const SharedDiagonal& sigmas = │ │ │ │ │ +66 SharedDiagonal()) : │ │ │ │ │ +67 _J_a_c_o_b_i_a_n_F_a_c_t_o_r(_k_e_y_s, augmentedMatrix, sigmas) { │ │ │ │ │ +68 } │ │ │ │ │ +69 │ │ │ │ │ +70 using _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d; │ │ │ │ │ +71 │ │ │ │ │ +_7_3 void _m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d(double alpha, const _V_e_c_t_o_r_V_a_l_u_e_s& x, │ │ │ │ │ +74 _V_e_c_t_o_r_V_a_l_u_e_s& y) const override { │ │ │ │ │ +75 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d(alpha, x, y); │ │ │ │ │ +76 } │ │ │ │ │ 77 │ │ │ │ │ -78 Params& p() const { return *boost::static_pointer_cast(p_);} │ │ │ │ │ -79 const Vector3& biasHat() const { return biasHat_; } │ │ │ │ │ -80 const Matrix3& preintMeasCov() const { return preintMeasCov_; } │ │ │ │ │ -81 │ │ │ │ │ -83 void _p_r_i_n_t(const std::string& s = "Preintegrated Measurements: ") const; │ │ │ │ │ -84 │ │ │ │ │ -86 bool equals(const PreintegratedAhrsMeasurements&, double tol = 1e-9) const; │ │ │ │ │ -87 │ │ │ │ │ -89 void resetIntegration(); │ │ │ │ │ +_8_2 void _m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d(double alpha, const double* x, double* y) const { │ │ │ │ │ +83 if (_e_m_p_t_y()) │ │ │ │ │ +84 return; │ │ │ │ │ +85 Vector Ax = Vector::Zero(Ab_._r_o_w_s()); │ │ │ │ │ +86 │ │ │ │ │ +87 // Just iterate over all A matrices and multiply in correct config part │ │ │ │ │ +88 for (size_t pos = 0; pos < _s_i_z_e(); ++pos) │ │ │ │ │ +89 Ax += Ab_(pos) * ConstDMap(x + D * _k_e_y_s__[pos]); │ │ │ │ │ 90 │ │ │ │ │ -100 void integrateMeasurement(const Vector3& measuredOmega, double deltaT); │ │ │ │ │ -101 │ │ │ │ │ -104 Vector3 predict(const Vector3& bias, OptionalJacobian<3,3> H = boost::none) │ │ │ │ │ -const; │ │ │ │ │ -105 │ │ │ │ │ -106 // This function is only used for test purposes │ │ │ │ │ -107 // (compare numerical derivatives wrt analytic ones) │ │ │ │ │ -108 static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt, │ │ │ │ │ -109 const Vector3& delta_angles); │ │ │ │ │ -110 │ │ │ │ │ -_1_1_2 _P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s(const Vector3& biasHat, │ │ │ │ │ -113 const Matrix3& measuredOmegaCovariance) │ │ │ │ │ -114 : _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n(boost::_m_a_k_e___s_h_a_r_e_d<_P_a_r_a_m_s>()), biasHat_(biasHat) { │ │ │ │ │ -115 p_->gyroscopeCovariance = measuredOmegaCovariance; │ │ │ │ │ -116 resetIntegration(); │ │ │ │ │ -117 } │ │ │ │ │ -118 │ │ │ │ │ -119private: │ │ │ │ │ -120 │ │ │ │ │ -_1_2_2 friend class boost::serialization::access; │ │ │ │ │ -123 template │ │ │ │ │ -124 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -125 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n); │ │ │ │ │ -126 ar & BOOST_SERIALIZATION_NVP(p_); │ │ │ │ │ -127 ar & BOOST_SERIALIZATION_NVP(biasHat_); │ │ │ │ │ -128 } │ │ │ │ │ -129}; │ │ │ │ │ -130 │ │ │ │ │ -_1_3_1class GTSAM_EXPORT _A_H_R_S_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ -{ │ │ │ │ │ -132 │ │ │ │ │ -133 typedef _A_H_R_S_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ -134 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_R_o_t_3_,_ _R_o_t_3_,_ _V_e_c_t_o_r_3_> _B_a_s_e; │ │ │ │ │ -135 │ │ │ │ │ -136 _P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s _PIM_; │ │ │ │ │ +91 // Deal with noise properly, need to Double* whiten as we are dividing by │ │ │ │ │ +variance │ │ │ │ │ +92 if (model_) { │ │ │ │ │ +93 model_->whitenInPlace(Ax); │ │ │ │ │ +94 model_->whitenInPlace(Ax); │ │ │ │ │ +95 } │ │ │ │ │ +96 │ │ │ │ │ +97 // multiply with alpha │ │ │ │ │ +98 Ax *= alpha; │ │ │ │ │ +99 │ │ │ │ │ +100 // Again iterate over all A matrices and insert Ai^e into y │ │ │ │ │ +101 for (size_t pos = 0; pos < _s_i_z_e(); ++pos) │ │ │ │ │ +102 DMap(y + D * _k_e_y_s__[pos]) += Ab_(pos).transpose() * Ax; │ │ │ │ │ +103 } │ │ │ │ │ +104 │ │ │ │ │ +106 using _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_h_e_s_s_i_a_n_D_i_a_g_o_n_a_l; │ │ │ │ │ +107 │ │ │ │ │ +_1_0_9 void _h_e_s_s_i_a_n_D_i_a_g_o_n_a_l(double* d) const override { │ │ │ │ │ +110 // Loop over all variables in the factor │ │ │ │ │ +111 for (_D_e_n_s_e_I_n_d_e_x j = 0; j < (_D_e_n_s_e_I_n_d_e_x) _s_i_z_e(); ++j) { │ │ │ │ │ +112 // Get the diagonal block, and insert its diagonal │ │ │ │ │ +113 DVector dj; │ │ │ │ │ +114 for (size_t k = 0; k < D; ++k) { │ │ │ │ │ +115 if (model_) { │ │ │ │ │ +116 Vector column_k = Ab_(j).col(k); │ │ │ │ │ +117 column_k = model_->whiten(column_k); │ │ │ │ │ +118 dj(k) = _d_o_t(column_k, column_k); │ │ │ │ │ +119 } else { │ │ │ │ │ +120 dj(k) = Ab_(j).col(k).squaredNorm(); │ │ │ │ │ +121 } │ │ │ │ │ +122 } │ │ │ │ │ +123 DMap(d + D * j) += dj; │ │ │ │ │ +124 } │ │ │ │ │ +125 } │ │ │ │ │ +126 │ │ │ │ │ +_1_2_8 _V_e_c_t_o_r_V_a_l_u_e_s _g_r_a_d_i_e_n_t_A_t_Z_e_r_o() const override { │ │ │ │ │ +129 return _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_r_a_d_i_e_n_t_A_t_Z_e_r_o(); │ │ │ │ │ +130 } │ │ │ │ │ +131 │ │ │ │ │ +_1_3_3 void _g_r_a_d_i_e_n_t_A_t_Z_e_r_o(double* d) const override { │ │ │ │ │ +134 │ │ │ │ │ +135 // Get vector b not weighted │ │ │ │ │ +136 Vector b = _g_e_t_b(); │ │ │ │ │ 137 │ │ │ │ │ -139 _A_H_R_S_F_a_c_t_o_r() {} │ │ │ │ │ -140 │ │ │ │ │ -141public: │ │ │ │ │ -142 │ │ │ │ │ -144#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 │ │ │ │ │ -145 typedef typename boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -146#else │ │ │ │ │ -_1_4_7 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -148#endif │ │ │ │ │ -149 │ │ │ │ │ -157 _A_H_R_S_F_a_c_t_o_r(_K_e_y rot_i, _K_e_y rot_j, _K_e_y bias, │ │ │ │ │ -158 const _P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s& preintegratedMeasurements); │ │ │ │ │ -159 │ │ │ │ │ -160 _~_A_H_R_S_F_a_c_t_o_r() override { │ │ │ │ │ -161 } │ │ │ │ │ -162 │ │ │ │ │ -164 gtsam::NonlinearFactor::shared_ptr clone() const override; │ │ │ │ │ -165 │ │ │ │ │ -167 void _p_r_i_n_t(const std::string& s, const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ -168 DefaultKeyFormatter) const override; │ │ │ │ │ -169 │ │ │ │ │ -171 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r&, double tol = 1e-9) const override; │ │ │ │ │ -172 │ │ │ │ │ -_1_7_4 const _P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s& _p_r_e_i_n_t_e_g_r_a_t_e_d_M_e_a_s_u_r_e_m_e_n_t_s() const { │ │ │ │ │ -175 return _PIM_; │ │ │ │ │ -176 } │ │ │ │ │ -177 │ │ │ │ │ -181 Vector evaluateError(const _R_o_t_3& rot_i, const _R_o_t_3& rot_j, │ │ │ │ │ -182 const Vector3& bias, boost::optional H1 = boost::none, │ │ │ │ │ -183 boost::optional H2 = boost::none, boost::optional H3 = │ │ │ │ │ -184 boost::none) const override; │ │ │ │ │ -185 │ │ │ │ │ -188 static _R_o_t_3 Predict(const _R_o_t_3& rot_i, const Vector3& bias, │ │ │ │ │ -189 const _P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s& pim); │ │ │ │ │ -190 │ │ │ │ │ -192 _A_H_R_S_F_a_c_t_o_r(_K_e_y rot_i, _K_e_y rot_j, _K_e_y bias, │ │ │ │ │ -193 const _P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s& pim, │ │ │ │ │ -194 const Vector3& omegaCoriolis, │ │ │ │ │ -195 const boost::optional& body_P_sensor = boost::none); │ │ │ │ │ -196 │ │ │ │ │ -198 static _R_o_t_3 predict( │ │ │ │ │ -199 const _R_o_t_3& rot_i, const Vector3& bias, │ │ │ │ │ -200 const _P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s& pim, const Vector3& omegaCoriolis, │ │ │ │ │ -201 const boost::optional& body_P_sensor = boost::none); │ │ │ │ │ -202 │ │ │ │ │ -203#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ -205 typedef _P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s PreintegratedMeasurements; │ │ │ │ │ -206 │ │ │ │ │ -207#endif │ │ │ │ │ -208 │ │ │ │ │ -209private: │ │ │ │ │ -210 │ │ │ │ │ -_2_1_2 friend class boost::serialization::access; │ │ │ │ │ -213 template │ │ │ │ │ -214 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -215 // NoiseModelFactor3 instead of NoiseModelFactorN for backward │ │ │ │ │ -compatibility │ │ │ │ │ -216 ar │ │ │ │ │ -217 & boost::serialization::make_nvp("NoiseModelFactor3", │ │ │ │ │ -218 boost::serialization::base_object(*this)); │ │ │ │ │ -219 ar & BOOST_SERIALIZATION_NVP(_PIM_); │ │ │ │ │ -220 } │ │ │ │ │ -221 │ │ │ │ │ -222}; │ │ │ │ │ -223// AHRSFactor │ │ │ │ │ -224 │ │ │ │ │ -225} //namespace gtsam │ │ │ │ │ -_P_o_s_e_3_._h │ │ │ │ │ -3D Pose │ │ │ │ │ -_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_._h │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ -Non-linear factor base classes. │ │ │ │ │ +138 // Whitening b │ │ │ │ │ +139 if (model_) { │ │ │ │ │ +140 b = model_->whiten(b); │ │ │ │ │ +141 b = model_->whiten(b); │ │ │ │ │ +142 } │ │ │ │ │ +143 │ │ │ │ │ +144 // Just iterate over all A matrices │ │ │ │ │ +145 for (_D_e_n_s_e_I_n_d_e_x j = 0; j < (_D_e_n_s_e_I_n_d_e_x) _s_i_z_e(); ++j) { │ │ │ │ │ +146 DVector dj; │ │ │ │ │ +147 // gradient -= A'*b/sigma^2 │ │ │ │ │ +148 // Computing with each column │ │ │ │ │ +149 for (size_t k = 0; k < D; ++k) { │ │ │ │ │ +150 Vector column_k = Ab_(j).col(k); │ │ │ │ │ +151 dj(k) = -1.0 * _d_o_t(b, column_k); │ │ │ │ │ +152 } │ │ │ │ │ +153 DMap(d + D * j) += dj; │ │ │ │ │ +154 } │ │ │ │ │ +155 } │ │ │ │ │ +156 │ │ │ │ │ +_1_6_1 void _t_r_a_n_s_p_o_s_e_M_u_l_t_i_p_l_y_A_d_d(double alpha, const Vector& e, double* x) const { │ │ │ │ │ +162 Vector E = alpha * (model_ ? model_->whiten(e) : e); │ │ │ │ │ +163 // Just iterate over all A matrices and insert Ai^e into y │ │ │ │ │ +164 for (size_t pos = 0; pos < _s_i_z_e(); ++pos) │ │ │ │ │ +165 DMap(x + D * _k_e_y_s__[pos]) += Ab_(pos).transpose() * E; │ │ │ │ │ +166 } │ │ │ │ │ +167 │ │ │ │ │ +_1_7_2 Vector _o_p_e_r_a_t_o_r_*(const double* x) const { │ │ │ │ │ +173 Vector Ax = Vector::Zero(Ab_._r_o_w_s()); │ │ │ │ │ +174 if (_e_m_p_t_y()) │ │ │ │ │ +175 return Ax; │ │ │ │ │ +176 │ │ │ │ │ +177 // Just iterate over all A matrices and multiply in correct config part │ │ │ │ │ +178 for (size_t pos = 0; pos < _s_i_z_e(); ++pos) │ │ │ │ │ +179 Ax += Ab_(pos) * ConstDMap(x + D * _k_e_y_s__[pos]); │ │ │ │ │ +180 │ │ │ │ │ +181 return model_ ? model_->whiten(Ax) : Ax; │ │ │ │ │ +182 } │ │ │ │ │ +183 │ │ │ │ │ +184}; │ │ │ │ │ +185// end class RegularJacobianFactor │ │ │ │ │ +186 │ │ │ │ │ +187}// end namespace gtsam │ │ │ │ │ +_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ +Factor Graph Values. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_m_a_k_e___s_h_a_r_e_d │ │ │ │ │ -gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost:: │ │ │ │ │ -shared_ptr< T > > make_shared(Args &&... args) │ │ │ │ │ -Add our own make_shared as a layer of wrapping on boost::make_shared This │ │ │ │ │ -solves the problem with the... │ │ │ │ │ -DDeeffiinniittiioonn make_shared.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ -Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor │ │ │ │ │ -symbol GTSAM_USE_QUATERNIO... │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ -PreintegratedAHRSMeasurements accumulates (integrates) the Gyroscope │ │ │ │ │ -measurements (rotation rates) an... │ │ │ │ │ -DDeeffiinniittiioonn AHRSFactor.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s_:_:_b_i_a_s_H_a_t__ │ │ │ │ │ -Vector3 biasHat_ │ │ │ │ │ -Angular rate bias values used during preintegration. │ │ │ │ │ -DDeeffiinniittiioonn AHRSFactor.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ -PreintegratedAhrsMeasurements(const Vector3 &biasHat, const Matrix3 │ │ │ │ │ -&measuredOmegaCovariance) │ │ │ │ │ -DDeeffiinniittiioonn AHRSFactor.h:112 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ -PreintegratedAhrsMeasurements(const boost::shared_ptr< Params > &p, const │ │ │ │ │ -Vector3 &biasHat) │ │ │ │ │ -Default constructor, initialize with no measurements. │ │ │ │ │ -DDeeffiinniittiioonn AHRSFactor.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s_:_:_p_r_e_i_n_t_M_e_a_s_C_o_v__ │ │ │ │ │ -Matrix3 preintMeasCov_ │ │ │ │ │ -Covariance matrix of the preintegrated measurements (first-order propagation │ │ │ │ │ -from measurementCovarian... │ │ │ │ │ -DDeeffiinniittiioonn AHRSFactor.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ -PreintegratedAhrsMeasurements(const boost::shared_ptr< Params > &p, const │ │ │ │ │ -Vector3 &bias_hat, double deltaTij, const Rot3 &deltaRij, const Matrix3 │ │ │ │ │ -&delRdelBiasOmega, const Matrix3 &preint_meas_cov) │ │ │ │ │ -Non-Default constructor, initialize with measurements. │ │ │ │ │ -DDeeffiinniittiioonn AHRSFactor.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_A_h_r_s_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ -PreintegratedAhrsMeasurements() │ │ │ │ │ -Default constructor, only for serialization and wrappers. │ │ │ │ │ -DDeeffiinniittiioonn AHRSFactor.h:46 │ │ │ │ │ -_g_t_s_a_m_:_:_A_H_R_S_F_a_c_t_o_r │ │ │ │ │ -DDeeffiinniittiioonn AHRSFactor.h:131 │ │ │ │ │ -_g_t_s_a_m_:_:_A_H_R_S_F_a_c_t_o_r_:_:_p_r_e_i_n_t_e_g_r_a_t_e_d_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ -const PreintegratedAhrsMeasurements & preintegratedMeasurements() const │ │ │ │ │ -Access the preintegrated measurements. │ │ │ │ │ -DDeeffiinniittiioonn AHRSFactor.h:174 │ │ │ │ │ -_g_t_s_a_m_:_:_A_H_R_S_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< AHRSFactor > shared_ptr │ │ │ │ │ -Shorthand for a smart pointer to a factor. │ │ │ │ │ -DDeeffiinniittiioonn AHRSFactor.h:147 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ -Parameters for pre-integration: Usage: Create just a single Params and pass a │ │ │ │ │ -shared pointer to the c... │ │ │ │ │ -DDeeffiinniittiioonn PreintegratedRotation.h:31 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n │ │ │ │ │ -PreintegratedRotation is the base class for all PreintegratedMeasurements │ │ │ │ │ -classes (in AHRSFactor,... │ │ │ │ │ -DDeeffiinniittiioonn PreintegratedRotation.h:89 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ -Nonlinear factor base class. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ -A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ -variables. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_n_s_e_I_n_d_e_x │ │ │ │ │ +ptrdiff_t DenseIndex │ │ │ │ │ +The index type for Eigen objects. │ │ │ │ │ +DDeeffiinniittiioonn types.h:106 │ │ │ │ │ +_g_t_s_a_m_:_:_d_o_t │ │ │ │ │ +double dot(const V1 &a, const V2 &b) │ │ │ │ │ +Dot product. │ │ │ │ │ +DDeeffiinniittiioonn Vector.h:195 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ +of vertical blocks. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_r_o_w_s │ │ │ │ │ +DenseIndex rows() const │ │ │ │ │ +Row size. │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:115 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s │ │ │ │ │ +const KeyVector & keys() const │ │ │ │ │ +Access the factor's involved variable keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:140 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s__ │ │ │ │ │ +KeyVector keys_ │ │ │ │ │ +The keys involved in this factor. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_m_p_t_y │ │ │ │ │ +bool empty() const │ │ │ │ │ +Whether the factor is empty (involves zero variables). │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:128 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_s_i_z_e │ │ │ │ │ +size_t size() const │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:157 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_h_e_s_s_i_a_n_D_i_a_g_o_n_a_l │ │ │ │ │ +VectorValues hessianDiagonal() const │ │ │ │ │ +Return the diagonal of the Hessian for this factor. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactor.cpp:35 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ +A Gaussian factor in the squared-error form. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:91 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_e_t_b │ │ │ │ │ +const constBVector getb() const │ │ │ │ │ +Get a view of the r.h.s. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:297 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d │ │ │ │ │ +void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) │ │ │ │ │ +const override │ │ │ │ │ +y += alpha * A'*A*x │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.cpp:649 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_r_a_d_i_e_n_t_A_t_Z_e_r_o │ │ │ │ │ +VectorValues gradientAtZero() const override │ │ │ │ │ +A'*b for Jacobian. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.cpp:701 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ +JacobianFactor with constant sized blocks Provides raw memory access versions │ │ │ │ │ +of linear operator. │ │ │ │ │ +DDeeffiinniittiioonn RegularJacobianFactor.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ +Vector operator*(const double *x) const │ │ │ │ │ +double* Matrix-vector multiply, i.e. │ │ │ │ │ +DDeeffiinniittiioonn RegularJacobianFactor.h:172 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_h_e_s_s_i_a_n_D_i_a_g_o_n_a_l │ │ │ │ │ +void hessianDiagonal(double *d) const override │ │ │ │ │ +Raw memory access version of hessianDiagonal. │ │ │ │ │ +DDeeffiinniittiioonn RegularJacobianFactor.h:109 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_r_a_d_i_e_n_t_A_t_Z_e_r_o │ │ │ │ │ +void gradientAtZero(double *d) const override │ │ │ │ │ +Raw memory access version of gradientAtZero. │ │ │ │ │ +DDeeffiinniittiioonn RegularJacobianFactor.h:133 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_t_r_a_n_s_p_o_s_e_M_u_l_t_i_p_l_y_A_d_d │ │ │ │ │ +void transposeMultiplyAdd(double alpha, const Vector &e, double *x) const │ │ │ │ │ +double* Transpose Matrix-vector multiply, i.e. │ │ │ │ │ +DDeeffiinniittiioonn RegularJacobianFactor.h:161 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_r_a_d_i_e_n_t_A_t_Z_e_r_o │ │ │ │ │ +VectorValues gradientAtZero() const override │ │ │ │ │ +Expose base class gradientAtZero. │ │ │ │ │ +DDeeffiinniittiioonn RegularJacobianFactor.h:128 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ +RegularJacobianFactor(const TERMS &terms, const Vector &b, const SharedDiagonal │ │ │ │ │ +&model=SharedDiagonal()) │ │ │ │ │ +Construct an n-ary factor. │ │ │ │ │ +DDeeffiinniittiioonn RegularJacobianFactor.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d │ │ │ │ │ +void multiplyHessianAdd(double alpha, const VectorValues &x, VectorValues &y) │ │ │ │ │ +const override │ │ │ │ │ +y += alpha * A'*A*x │ │ │ │ │ +DDeeffiinniittiioonn RegularJacobianFactor.h:73 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ +RegularJacobianFactor() │ │ │ │ │ +Default constructor. │ │ │ │ │ +DDeeffiinniittiioonn RegularJacobianFactor.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ +RegularJacobianFactor(const KEYS &keys, const VerticalBlockMatrix │ │ │ │ │ +&augmentedMatrix, const SharedDiagonal &sigmas=SharedDiagonal()) │ │ │ │ │ +Constructor with arbitrary number keys, and where the augmented matrix is given │ │ │ │ │ +all together instead ... │ │ │ │ │ +DDeeffiinniittiioonn RegularJacobianFactor.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_m_u_l_t_i_p_l_y_H_e_s_s_i_a_n_A_d_d │ │ │ │ │ +void multiplyHessianAdd(double alpha, const double *x, double *y) const │ │ │ │ │ +double* Hessian-vector multiply, i.e. │ │ │ │ │ +DDeeffiinniittiioonn RegularJacobianFactor.h:82 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ +with a unique integer... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _A_H_R_S_F_a_c_t_o_r_._h │ │ │ │ │ + * _l_i_n_e_a_r │ │ │ │ │ + * _R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00944_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01226_source.html │ │ │ │┄ Files 86% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ImuBias.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/SfmData.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ImuBias.h
│ │ │ │ +
SfmData.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/base/OptionalJacobian.h>
│ │ │ │ -
21#include <gtsam/base/VectorSpace.h>
│ │ │ │ -
22#include <iosfwd>
│ │ │ │ -
23#include <boost/serialization/nvp.hpp>
│ │ │ │ -
24
│ │ │ │ -
25namespace gtsam {
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/geometry/Cal3Bundler.h>
│ │ │ │ +
22#include <gtsam/geometry/PinholeCamera.h>
│ │ │ │ +
23#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ +
24#include <gtsam/nonlinear/Values.h>
│ │ │ │ +
25#include <gtsam/sfm/SfmTrack.h>
│ │ │ │
26
│ │ │ │ -
28namespace imuBias {
│ │ │ │ +
27#include <string>
│ │ │ │ +
28#include <vector>
│ │ │ │
29
│ │ │ │ -
│ │ │ │ -
30class GTSAM_EXPORT ConstantBias {
│ │ │ │ -
31private:
│ │ │ │ -
32 Vector3 biasAcc_;
│ │ │ │ -
33 Vector3 biasGyro_;
│ │ │ │ +
30namespace gtsam {
│ │ │ │ +
31
│ │ │ │ +
33typedef PinholeCamera<Cal3Bundler> SfmCamera;
│ │ │ │
34
│ │ │ │ -
35public:
│ │ │ │ -
37 static const size_t dimension = 6;
│ │ │ │ -
38
│ │ │ │ +
│ │ │ │ +
39struct GTSAM_EXPORT SfmData {
│ │ │ │ +
40 std::vector<SfmCamera> cameras;
│ │ │ │
41
│ │ │ │ -
42 ConstantBias() :
│ │ │ │ -
43 biasAcc_(0.0, 0.0, 0.0), biasGyro_(0.0, 0.0, 0.0) {
│ │ │ │ -
44 }
│ │ │ │ -
45
│ │ │ │ -
46 ConstantBias(const Vector3& biasAcc, const Vector3& biasGyro) :
│ │ │ │ -
47 biasAcc_(biasAcc), biasGyro_(biasGyro) {
│ │ │ │ -
48 }
│ │ │ │ -
49
│ │ │ │ -
50 explicit ConstantBias(const Vector6& v) :
│ │ │ │ -
51 biasAcc_(v.head<3>()), biasGyro_(v.tail<3>()) {
│ │ │ │ -
52 }
│ │ │ │ -
53
│ │ │ │ -
55
│ │ │ │ -
│ │ │ │ -
57 Vector6 vector() const {
│ │ │ │ -
58 Vector6 v;
│ │ │ │ -
59 v << biasAcc_, biasGyro_;
│ │ │ │ -
60 return v;
│ │ │ │ -
61 }
│ │ │ │ -
│ │ │ │ +
42 std::vector<SfmTrack> tracks;
│ │ │ │ +
43
│ │ │ │ +
46
│ │ │ │ +
53 static SfmData FromBundlerFile(const std::string& filename);
│ │ │ │ +
54
│ │ │ │ +
61 static SfmData FromBalFile(const std::string& filename);
│ │ │ │
62
│ │ │ │ -
│ │ │ │ -
64 const Vector3& accelerometer() const {
│ │ │ │ -
65 return biasAcc_;
│ │ │ │ -
66 }
│ │ │ │ -
│ │ │ │ -
67
│ │ │ │ -
│ │ │ │ -
69 const Vector3& gyroscope() const {
│ │ │ │ -
70 return biasGyro_;
│ │ │ │ -
71 }
│ │ │ │ -
│ │ │ │ +
66
│ │ │ │ +
68 void addTrack(const SfmTrack& t) { tracks.push_back(t); }
│ │ │ │ +
69
│ │ │ │ +
71 void addCamera(const SfmCamera& cam) { cameras.push_back(cam); }
│ │ │ │
72
│ │ │ │ -
│ │ │ │ -
74 Vector3 correctAccelerometer(const Vector3& measurement,
│ │ │ │ -
75 OptionalJacobian<3, 6> H1 = boost::none,
│ │ │ │ -
76 OptionalJacobian<3, 3> H2 = boost::none) const {
│ │ │ │ -
77 if (H1) (*H1) << -I_3x3, Z_3x3;
│ │ │ │ -
78 if (H2) (*H2) << I_3x3;
│ │ │ │ -
79 return measurement - biasAcc_;
│ │ │ │ -
80 }
│ │ │ │ -
│ │ │ │ +
74 size_t numberTracks() const { return tracks.size(); }
│ │ │ │ +
75
│ │ │ │ +
77 size_t numberCameras() const { return cameras.size(); }
│ │ │ │ +
78
│ │ │ │ +
80 const SfmTrack& track(size_t idx) const { return tracks[idx]; }
│ │ │ │
81
│ │ │ │ -
│ │ │ │ -
83 Vector3 correctGyroscope(const Vector3& measurement,
│ │ │ │ -
84 OptionalJacobian<3, 6> H1 = boost::none,
│ │ │ │ -
85 OptionalJacobian<3, 3> H2 = boost::none) const {
│ │ │ │ -
86 if (H1) (*H1) << Z_3x3, -I_3x3;
│ │ │ │ -
87 if (H2) (*H2) << I_3x3;
│ │ │ │ -
88 return measurement - biasGyro_;
│ │ │ │ -
89 }
│ │ │ │ -
│ │ │ │ -
90
│ │ │ │ -
93
│ │ │ │ -
95 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
│ │ │ │ -
96 const ConstantBias& bias);
│ │ │ │ -
97
│ │ │ │ -
99 void print(const std::string& s = "") const;
│ │ │ │ -
100
│ │ │ │ -
│ │ │ │ -
102 inline bool equals(const ConstantBias& expected, double tol = 1e-5) const {
│ │ │ │ -
103 return equal_with_abs_tol(biasAcc_, expected.biasAcc_, tol)
│ │ │ │ -
104 && equal_with_abs_tol(biasGyro_, expected.biasGyro_, tol);
│ │ │ │ -
105 }
│ │ │ │ -
│ │ │ │ -
106
│ │ │ │ -
110
│ │ │ │ -
│ │ │ │ -
112 static ConstantBias Identity() {
│ │ │ │ -
113 return ConstantBias();
│ │ │ │ -
114 }
│ │ │ │ -
│ │ │ │ -
115
│ │ │ │ -
│ │ │ │ -
117 inline ConstantBias operator-() const {
│ │ │ │ -
118 return ConstantBias(-biasAcc_, -biasGyro_);
│ │ │ │ -
119 }
│ │ │ │ -
│ │ │ │ +
83 const SfmCamera& camera(size_t idx) const { return cameras[idx]; }
│ │ │ │ +
84
│ │ │ │ +
86 const std::vector<SfmCamera>& cameraList() const { return cameras; }
│ │ │ │ +
87 const std::vector<SfmTrack>& trackList() const { return tracks; }
│ │ │ │ +
88
│ │ │ │ +
95 NonlinearFactorGraph generalSfmFactors(
│ │ │ │ +
96 const SharedNoiseModel& model = noiseModel::Isotropic::Sigma(2,
│ │ │ │ +
97 1.0)) const;
│ │ │ │ +
98
│ │ │ │ +
109 NonlinearFactorGraph sfmFactorGraph(
│ │ │ │ +
110 const SharedNoiseModel& model = noiseModel::Isotropic::Sigma(2, 1.0),
│ │ │ │ +
111 boost::optional<size_t> fixedCamera = 0,
│ │ │ │ +
112 boost::optional<size_t> fixedPoint = 0) const;
│ │ │ │ +
113
│ │ │ │ +
117
│ │ │ │ +
119 void print(const std::string& s = "") const;
│ │ │ │
120
│ │ │ │ -
│ │ │ │ -
122 ConstantBias operator+(const Vector6& v) const {
│ │ │ │ -
123 return ConstantBias(biasAcc_ + v.head<3>(), biasGyro_ + v.tail<3>());
│ │ │ │ -
124 }
│ │ │ │ -
│ │ │ │ -
125
│ │ │ │ -
│ │ │ │ -
127 ConstantBias operator+(const ConstantBias& b) const {
│ │ │ │ -
128 return ConstantBias(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_);
│ │ │ │ -
129 }
│ │ │ │ -
│ │ │ │ -
130
│ │ │ │ -
│ │ │ │ -
132 ConstantBias operator-(const ConstantBias& b) const {
│ │ │ │ -
133 return ConstantBias(biasAcc_ - b.biasAcc_, biasGyro_ - b.biasGyro_);
│ │ │ │ -
134 }
│ │ │ │ -
│ │ │ │ -
135
│ │ │ │ -
137
│ │ │ │ -
138#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ -
141 ConstantBias GTSAM_DEPRECATED inverse() { return -(*this); }
│ │ │ │ -
142 ConstantBias GTSAM_DEPRECATED compose(const ConstantBias& q) {
│ │ │ │ -
143 return (*this) + q;
│ │ │ │ -
144 }
│ │ │ │ -
145 ConstantBias GTSAM_DEPRECATED between(const ConstantBias& q) {
│ │ │ │ -
146 return q - (*this);
│ │ │ │ -
147 }
│ │ │ │ -
148 Vector6 GTSAM_DEPRECATED localCoordinates(const ConstantBias& q) {
│ │ │ │ -
149 return (q - (*this)).vector();
│ │ │ │ -
150 }
│ │ │ │ -
151 ConstantBias GTSAM_DEPRECATED retract(const Vector6& v) {
│ │ │ │ -
152 return (*this) + ConstantBias(v);
│ │ │ │ -
153 }
│ │ │ │ -
154 static Vector6 GTSAM_DEPRECATED Logmap(const ConstantBias& p) {
│ │ │ │ -
155 return p.vector();
│ │ │ │ -
156 }
│ │ │ │ -
157 static ConstantBias GTSAM_DEPRECATED Expmap(const Vector6& v) {
│ │ │ │ -
158 return ConstantBias(v);
│ │ │ │ -
159 }
│ │ │ │ -
161#endif
│ │ │ │ -
162
│ │ │ │ -
163private:
│ │ │ │ -
164
│ │ │ │ -
167
│ │ │ │ -
169 friend class boost::serialization::access;
│ │ │ │ -
170 template<class ARCHIVE>
│ │ │ │ -
171 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
172 ar & BOOST_SERIALIZATION_NVP(biasAcc_);
│ │ │ │ -
173 ar & BOOST_SERIALIZATION_NVP(biasGyro_);
│ │ │ │ -
174 }
│ │ │ │ -
175
│ │ │ │ -
176
│ │ │ │ -
177public:
│ │ │ │ -
178 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
180
│ │ │ │ -
181}; // ConstantBias class
│ │ │ │ -
│ │ │ │ -
182} // namespace imuBias
│ │ │ │ -
183
│ │ │ │ -
184template<>
│ │ │ │ -
│ │ │ │ -
185struct traits<imuBias::ConstantBias> : public internal::VectorSpace<
│ │ │ │ -
186 imuBias::ConstantBias> {
│ │ │ │ -
187};
│ │ │ │ -
│ │ │ │ -
188
│ │ │ │ -
189} // namespace gtsam
│ │ │ │ -
190
│ │ │ │ -
OptionalJacobian.h
Special class for optional Jacobian arguments.
│ │ │ │ -
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ +
122 bool equals(const SfmData& sfmData, double tol = 1e-9) const;
│ │ │ │ +
123
│ │ │ │ +
125#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ +
128 void GTSAM_DEPRECATED add_track(const SfmTrack& t) { tracks.push_back(t); }
│ │ │ │ +
129 void GTSAM_DEPRECATED add_camera(const SfmCamera& cam) {
│ │ │ │ +
130 cameras.push_back(cam);
│ │ │ │ +
131 }
│ │ │ │ +
132 size_t GTSAM_DEPRECATED number_tracks() const { return tracks.size(); }
│ │ │ │ +
133 size_t GTSAM_DEPRECATED number_cameras() const { return cameras.size(); }
│ │ │ │ +
135#endif
│ │ │ │ +
138
│ │ │ │ +
140 friend class boost::serialization::access;
│ │ │ │ +
141 template <class Archive>
│ │ │ │ +
142 void serialize(Archive& ar, const unsigned int /*version*/) {
│ │ │ │ +
143 ar& BOOST_SERIALIZATION_NVP(cameras);
│ │ │ │ +
144 ar& BOOST_SERIALIZATION_NVP(tracks);
│ │ │ │ +
145 }
│ │ │ │ +
146
│ │ │ │ +
148};
│ │ │ │ +
│ │ │ │ +
149
│ │ │ │ +
151template <>
│ │ │ │ +
152struct traits<SfmData> : public Testable<SfmData> {};
│ │ │ │ +
153
│ │ │ │ +
154#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ +
155GTSAM_EXPORT bool GTSAM_DEPRECATED readBundler(const std::string& filename,
│ │ │ │ +
156 SfmData& data);
│ │ │ │ +
157GTSAM_EXPORT bool GTSAM_DEPRECATED readBAL(const std::string& filename,
│ │ │ │ +
158 SfmData& data);
│ │ │ │ +
159#endif
│ │ │ │ +
160
│ │ │ │ +
167GTSAM_EXPORT SfmData readBal(const std::string& filename);
│ │ │ │ +
168
│ │ │ │ +
176GTSAM_EXPORT bool writeBAL(const std::string& filename, const SfmData& data);
│ │ │ │ +
177
│ │ │ │ +
190GTSAM_EXPORT bool writeBALfromValues(const std::string& filename,
│ │ │ │ +
191 const SfmData& data, const Values& values);
│ │ │ │ +
192
│ │ │ │ +
201GTSAM_EXPORT Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz);
│ │ │ │ +
202
│ │ │ │ +
211GTSAM_EXPORT Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz);
│ │ │ │ +
212
│ │ │ │ +
218GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM);
│ │ │ │ +
219
│ │ │ │ +
228GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db);
│ │ │ │ +
229
│ │ │ │ +
238GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db);
│ │ │ │ +
239
│ │ │ │ +
240} // namespace gtsam
│ │ │ │ +
Cal3Bundler.h
Calibration used by Bundler.
│ │ │ │ +
PinholeCamera.h
Base class for all pinhole cameras.
│ │ │ │ +
Values.h
A non-templated config holding any types of Manifold-group elements.
│ │ │ │ +
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │ +
SfmTrack.h
A simple data structure for a track in Structure from Motion.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition Matrix.h:81
│ │ │ │ +
gtsam::writeBAL
bool writeBAL(const std::string &filename, const SfmData &data)
This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure.
Definition SfmData.cpp:249
│ │ │ │ +
gtsam::writeBALfromValues
bool writeBALfromValues(const std::string &filename, const SfmData &data, const Values &values)
This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure and a val...
Definition SfmData.cpp:349
│ │ │ │ +
gtsam::readBal
SfmData readBal(const std::string &filename)
This function parses a "Bundle Adjustment in the Large" (BAL) file and returns the data as a SfmData ...
Definition SfmData.cpp:344
│ │ │ │ +
gtsam::gtsam2openGL
Pose3 gtsam2openGL(const Rot3 &R, double tx, double ty, double tz)
This function converts a GTSAM camera pose to an openGL camera pose.
Definition SfmData.cpp:88
│ │ │ │ +
gtsam::initialCamerasAndPointsEstimate
Values initialCamerasAndPointsEstimate(const SfmData &db)
This function creates initial values for cameras and points from db.
Definition SfmData.cpp:449
│ │ │ │ +
gtsam::SfmCamera
PinholeCamera< Cal3Bundler > SfmCamera
Define the structure for the camera poses.
Definition SfmData.h:33
│ │ │ │ +
gtsam::initialCamerasEstimate
Values initialCamerasEstimate(const SfmData &db)
This function creates initial values for cameras from db.
Definition SfmData.cpp:441
│ │ │ │ +
gtsam::openGL2gtsam
Pose3 openGL2gtsam(const Rot3 &R, double tx, double ty, double tz)
This function converts an openGL camera pose to an GTSAM camera pose.
Definition SfmData.cpp:79
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
gtsam::internal::VectorSpace
VectorSpace provides both Testable and VectorSpaceTraits.
Definition VectorSpace.h:207
│ │ │ │ -
gtsam::imuBias::ConstantBias
Definition ImuBias.h:30
│ │ │ │ -
gtsam::imuBias::ConstantBias::gyroscope
const Vector3 & gyroscope() const
get gyroscope bias
Definition ImuBias.h:69
│ │ │ │ -
gtsam::imuBias::ConstantBias::Identity
static ConstantBias Identity()
identity for group operation
Definition ImuBias.h:112
│ │ │ │ -
gtsam::imuBias::ConstantBias::equals
bool equals(const ConstantBias &expected, double tol=1e-5) const
equality up to tolerance
Definition ImuBias.h:102
│ │ │ │ -
gtsam::imuBias::ConstantBias::operator-
ConstantBias operator-(const ConstantBias &b) const
subtraction
Definition ImuBias.h:132
│ │ │ │ -
gtsam::imuBias::ConstantBias::correctAccelerometer
Vector3 correctAccelerometer(const Vector3 &measurement, OptionalJacobian< 3, 6 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
Correct an accelerometer measurement using this bias model, and optionally compute Jacobians.
Definition ImuBias.h:74
│ │ │ │ -
gtsam::imuBias::ConstantBias::operator-
ConstantBias operator-() const
inverse
Definition ImuBias.h:117
│ │ │ │ -
gtsam::imuBias::ConstantBias::correctGyroscope
Vector3 correctGyroscope(const Vector3 &measurement, OptionalJacobian< 3, 6 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
Correct a gyroscope measurement using this bias model, and optionally compute Jacobians.
Definition ImuBias.h:83
│ │ │ │ -
gtsam::imuBias::ConstantBias::vector
Vector6 vector() const
return the accelerometer and gyro biases in a single vector
Definition ImuBias.h:57
│ │ │ │ -
gtsam::imuBias::ConstantBias::operator+
ConstantBias operator+(const Vector6 &v) const
addition of vector on right
Definition ImuBias.h:122
│ │ │ │ -
gtsam::imuBias::ConstantBias::operator+
ConstantBias operator+(const ConstantBias &b) const
addition
Definition ImuBias.h:127
│ │ │ │ -
gtsam::imuBias::ConstantBias::accelerometer
const Vector3 & accelerometer() const
get accelerometer bias
Definition ImuBias.h:64
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::PinholeCamera
A pinhole camera class that has a Pose3 and a Calibration.
Definition PinholeCamera.h:33
│ │ │ │ +
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ +
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
│ │ │ │ +
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
gtsam::SfmData
SfmData stores a bunch of SfmTracks.
Definition SfmData.h:39
│ │ │ │ +
gtsam::SfmData::cameraList
const std::vector< SfmCamera > & cameraList() const
Getters.
Definition SfmData.h:86
│ │ │ │ +
gtsam::SfmData::addCamera
void addCamera(const SfmCamera &cam)
Add a camera to SfmData.
Definition SfmData.h:71
│ │ │ │ +
gtsam::SfmData::numberCameras
size_t numberCameras() const
The number of cameras.
Definition SfmData.h:77
│ │ │ │ +
gtsam::SfmData::numberTracks
size_t numberTracks() const
The number of reconstructed 3D points.
Definition SfmData.h:74
│ │ │ │ +
gtsam::SfmData::track
const SfmTrack & track(size_t idx) const
The track formed by series of landmark measurements.
Definition SfmData.h:80
│ │ │ │ +
gtsam::SfmData::addTrack
void addTrack(const SfmTrack &t)
Add a track to SfmData.
Definition SfmData.h:68
│ │ │ │ +
gtsam::SfmData::camera
const SfmCamera & camera(size_t idx) const
The camera pose at frame index idx
Definition SfmData.h:83
│ │ │ │ +
gtsam::SfmData::cameras
std::vector< SfmCamera > cameras
Set of cameras.
Definition SfmData.h:40
│ │ │ │ +
gtsam::SfmData::tracks
std::vector< SfmTrack > tracks
Sparse set of points.
Definition SfmData.h:42
│ │ │ │ +
gtsam::SfmTrack
Definition SfmTrack.h:126
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,245 +1,238 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ImuBias.h │ │ │ │ │ +SfmData.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_b_a_s_e_/_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h> │ │ │ │ │ -21#include │ │ │ │ │ -22#include │ │ │ │ │ -23#include │ │ │ │ │ -24 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3_B_u_n_d_l_e_r_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_i_n_h_o_l_e_C_a_m_e_r_a_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_V_a_l_u_e_s_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_s_f_m_/_S_f_m_T_r_a_c_k_._h> │ │ │ │ │ 26 │ │ │ │ │ -28namespace imuBias { │ │ │ │ │ +27#include │ │ │ │ │ +28#include │ │ │ │ │ 29 │ │ │ │ │ -_3_0class GTSAM_EXPORT _C_o_n_s_t_a_n_t_B_i_a_s { │ │ │ │ │ -31private: │ │ │ │ │ -32 Vector3 biasAcc_; │ │ │ │ │ -33 Vector3 biasGyro_; │ │ │ │ │ +30namespace _g_t_s_a_m { │ │ │ │ │ +31 │ │ │ │ │ +_3_3typedef _P_i_n_h_o_l_e_C_a_m_e_r_a_<_C_a_l_3_B_u_n_d_l_e_r_> _S_f_m_C_a_m_e_r_a; │ │ │ │ │ 34 │ │ │ │ │ -35public: │ │ │ │ │ -_3_7 static const size_t dimension = 6; │ │ │ │ │ -38 │ │ │ │ │ +_3_9struct GTSAM_EXPORT _S_f_m_D_a_t_a { │ │ │ │ │ +_4_0 std::vector _c_a_m_e_r_a_s; │ │ │ │ │ 41 │ │ │ │ │ -42 _C_o_n_s_t_a_n_t_B_i_a_s() : │ │ │ │ │ -43 biasAcc_(0.0, 0.0, 0.0), biasGyro_(0.0, 0.0, 0.0) { │ │ │ │ │ -44 } │ │ │ │ │ -45 │ │ │ │ │ -46 _C_o_n_s_t_a_n_t_B_i_a_s(const Vector3& biasAcc, const Vector3& biasGyro) : │ │ │ │ │ -47 biasAcc_(biasAcc), biasGyro_(biasGyro) { │ │ │ │ │ -48 } │ │ │ │ │ -49 │ │ │ │ │ -50 explicit ConstantBias(const Vector6& v) : │ │ │ │ │ -51 biasAcc_(v.head<3>()), biasGyro_(v.tail<3>()) { │ │ │ │ │ -52 } │ │ │ │ │ -53 │ │ │ │ │ -55 │ │ │ │ │ -_5_7 Vector6 _v_e_c_t_o_r() const { │ │ │ │ │ -58 Vector6 v; │ │ │ │ │ -59 v << biasAcc_, biasGyro_; │ │ │ │ │ -60 return v; │ │ │ │ │ -61 } │ │ │ │ │ +_4_2 std::vector _t_r_a_c_k_s; │ │ │ │ │ +43 │ │ │ │ │ +46 │ │ │ │ │ +53 static _S_f_m_D_a_t_a FromBundlerFile(const std::string& filename); │ │ │ │ │ +54 │ │ │ │ │ +61 static _S_f_m_D_a_t_a FromBalFile(const std::string& filename); │ │ │ │ │ 62 │ │ │ │ │ -_6_4 const Vector3& _a_c_c_e_l_e_r_o_m_e_t_e_r() const { │ │ │ │ │ -65 return biasAcc_; │ │ │ │ │ -66 } │ │ │ │ │ -67 │ │ │ │ │ -_6_9 const Vector3& _g_y_r_o_s_c_o_p_e() const { │ │ │ │ │ -70 return biasGyro_; │ │ │ │ │ -71 } │ │ │ │ │ +66 │ │ │ │ │ +_6_8 void _a_d_d_T_r_a_c_k(const _S_f_m_T_r_a_c_k& t) { tracks.push_back(t); } │ │ │ │ │ +69 │ │ │ │ │ +_7_1 void _a_d_d_C_a_m_e_r_a(const _S_f_m_C_a_m_e_r_a& cam) { cameras.push_back(cam); } │ │ │ │ │ 72 │ │ │ │ │ -_7_4 Vector3 _c_o_r_r_e_c_t_A_c_c_e_l_e_r_o_m_e_t_e_r(const Vector3& measurement, │ │ │ │ │ -75 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _6_> H1 = boost::none, │ │ │ │ │ -76 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H2 = boost::none) const { │ │ │ │ │ -77 if (H1) (*H1) << -I_3x3, Z_3x3; │ │ │ │ │ -78 if (H2) (*H2) << I_3x3; │ │ │ │ │ -79 return measurement - biasAcc_; │ │ │ │ │ -80 } │ │ │ │ │ +_7_4 size_t _n_u_m_b_e_r_T_r_a_c_k_s() const { return tracks.size(); } │ │ │ │ │ +75 │ │ │ │ │ +_7_7 size_t _n_u_m_b_e_r_C_a_m_e_r_a_s() const { return cameras.size(); } │ │ │ │ │ +78 │ │ │ │ │ +_8_0 const _S_f_m_T_r_a_c_k& _t_r_a_c_k(size_t idx) const { return tracks[idx]; } │ │ │ │ │ 81 │ │ │ │ │ -_8_3 Vector3 _c_o_r_r_e_c_t_G_y_r_o_s_c_o_p_e(const Vector3& measurement, │ │ │ │ │ -84 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _6_> H1 = boost::none, │ │ │ │ │ -85 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_3_,_ _3_> H2 = boost::none) const { │ │ │ │ │ -86 if (H1) (*H1) << Z_3x3, -I_3x3; │ │ │ │ │ -87 if (H2) (*H2) << I_3x3; │ │ │ │ │ -88 return measurement - biasGyro_; │ │ │ │ │ -89 } │ │ │ │ │ -90 │ │ │ │ │ -93 │ │ │ │ │ -95 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, │ │ │ │ │ -96 const _C_o_n_s_t_a_n_t_B_i_a_s& bias); │ │ │ │ │ -97 │ │ │ │ │ -99 void _p_r_i_n_t(const std::string& s = "") const; │ │ │ │ │ -100 │ │ │ │ │ -_1_0_2 inline bool _e_q_u_a_l_s(const _C_o_n_s_t_a_n_t_B_i_a_s& expected, double tol = 1e-5) const { │ │ │ │ │ -103 return _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(biasAcc_, expected.biasAcc_, tol) │ │ │ │ │ -104 && _e_q_u_a_l___w_i_t_h___a_b_s___t_o_l(biasGyro_, expected.biasGyro_, tol); │ │ │ │ │ -105 } │ │ │ │ │ -106 │ │ │ │ │ -110 │ │ │ │ │ -_1_1_2 static _C_o_n_s_t_a_n_t_B_i_a_s _I_d_e_n_t_i_t_y() { │ │ │ │ │ -113 return _C_o_n_s_t_a_n_t_B_i_a_s(); │ │ │ │ │ -114 } │ │ │ │ │ -115 │ │ │ │ │ -_1_1_7 inline _C_o_n_s_t_a_n_t_B_i_a_s _o_p_e_r_a_t_o_r_-() const { │ │ │ │ │ -118 return _C_o_n_s_t_a_n_t_B_i_a_s(-biasAcc_, -biasGyro_); │ │ │ │ │ -119 } │ │ │ │ │ +_8_3 const _S_f_m_C_a_m_e_r_a& _c_a_m_e_r_a(size_t idx) const { return cameras[idx]; } │ │ │ │ │ +84 │ │ │ │ │ +_8_6 const std::vector& _c_a_m_e_r_a_L_i_s_t() const { return cameras; } │ │ │ │ │ +87 const std::vector& trackList() const { return tracks; } │ │ │ │ │ +88 │ │ │ │ │ +95 NonlinearFactorGraph generalSfmFactors( │ │ │ │ │ +96 const SharedNoiseModel& model = noiseModel::Isotropic::Sigma(2, │ │ │ │ │ +97 1.0)) const; │ │ │ │ │ +98 │ │ │ │ │ +109 NonlinearFactorGraph sfmFactorGraph( │ │ │ │ │ +110 const SharedNoiseModel& model = noiseModel::Isotropic::Sigma(2, 1.0), │ │ │ │ │ +111 boost::optional fixedCamera = 0, │ │ │ │ │ +112 boost::optional fixedPoint = 0) const; │ │ │ │ │ +113 │ │ │ │ │ +117 │ │ │ │ │ +119 void print(const std::string& s = "") const; │ │ │ │ │ 120 │ │ │ │ │ -_1_2_2 _C_o_n_s_t_a_n_t_B_i_a_s _o_p_e_r_a_t_o_r_+(const Vector6& v) const { │ │ │ │ │ -123 return _C_o_n_s_t_a_n_t_B_i_a_s(biasAcc_ + v.head<3>(), biasGyro_ + v.tail<3>()); │ │ │ │ │ -124 } │ │ │ │ │ -125 │ │ │ │ │ -_1_2_7 _C_o_n_s_t_a_n_t_B_i_a_s _o_p_e_r_a_t_o_r_+(const _C_o_n_s_t_a_n_t_B_i_a_s& b) const { │ │ │ │ │ -128 return _C_o_n_s_t_a_n_t_B_i_a_s(biasAcc_ + b.biasAcc_, biasGyro_ + b.biasGyro_); │ │ │ │ │ -129 } │ │ │ │ │ -130 │ │ │ │ │ -_1_3_2 _C_o_n_s_t_a_n_t_B_i_a_s _o_p_e_r_a_t_o_r_-(const _C_o_n_s_t_a_n_t_B_i_a_s& b) const { │ │ │ │ │ -133 return _C_o_n_s_t_a_n_t_B_i_a_s(biasAcc_ - b.biasAcc_, biasGyro_ - b.biasGyro_); │ │ │ │ │ -134 } │ │ │ │ │ -135 │ │ │ │ │ -137 │ │ │ │ │ -138#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ -141 _C_o_n_s_t_a_n_t_B_i_a_s GTSAM_DEPRECATED inverse() { return -(*this); } │ │ │ │ │ -142 ConstantBias GTSAM_DEPRECATED compose(const ConstantBias& q) { │ │ │ │ │ -143 return (*this) + q; │ │ │ │ │ -144 } │ │ │ │ │ -145 ConstantBias GTSAM_DEPRECATED between(const ConstantBias& q) { │ │ │ │ │ -146 return q - (*this); │ │ │ │ │ -147 } │ │ │ │ │ -148 Vector6 GTSAM_DEPRECATED localCoordinates(const ConstantBias& q) { │ │ │ │ │ -149 return (q - (*this)).vector(); │ │ │ │ │ -150 } │ │ │ │ │ -151 ConstantBias GTSAM_DEPRECATED retract(const Vector6& v) { │ │ │ │ │ -152 return (*this) + ConstantBias(v); │ │ │ │ │ -153 } │ │ │ │ │ -154 static Vector6 GTSAM_DEPRECATED Logmap(const ConstantBias& p) { │ │ │ │ │ -155 return p.vector(); │ │ │ │ │ -156 } │ │ │ │ │ -157 static ConstantBias GTSAM_DEPRECATED Expmap(const Vector6& v) { │ │ │ │ │ -158 return ConstantBias(v); │ │ │ │ │ -159 } │ │ │ │ │ -161#endif │ │ │ │ │ -162 │ │ │ │ │ -163private: │ │ │ │ │ -164 │ │ │ │ │ -167 │ │ │ │ │ -_1_6_9 friend class boost::serialization::access; │ │ │ │ │ -170 template │ │ │ │ │ -171 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -172 ar & BOOST_SERIALIZATION_NVP(biasAcc_); │ │ │ │ │ -173 ar & BOOST_SERIALIZATION_NVP(biasGyro_); │ │ │ │ │ -174 } │ │ │ │ │ -175 │ │ │ │ │ -176 │ │ │ │ │ -177public: │ │ │ │ │ -178 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -180 │ │ │ │ │ -181}; // ConstantBias class │ │ │ │ │ -182} // namespace imuBias │ │ │ │ │ -183 │ │ │ │ │ -184template<> │ │ │ │ │ -_1_8_5struct _t_r_a_i_t_s : public _i_n_t_e_r_n_a_l_:_:_V_e_c_t_o_r_S_p_a_c_e< │ │ │ │ │ -186 imuBias::ConstantBias> { │ │ │ │ │ -187}; │ │ │ │ │ -188 │ │ │ │ │ -189} // namespace gtsam │ │ │ │ │ -190 │ │ │ │ │ -_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h │ │ │ │ │ -Special class for optional Jacobian arguments. │ │ │ │ │ -_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ -This marks a GTSAM object to require alignment. │ │ │ │ │ -DDeeffiinniittiioonn types.h:308 │ │ │ │ │ +122 bool equals(const SfmData& sfmData, double tol = 1e-9) const; │ │ │ │ │ +123 │ │ │ │ │ +125#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ +128 void GTSAM_DEPRECATED add_track(const SfmTrack& t) { tracks.push_back(t); } │ │ │ │ │ +129 void GTSAM_DEPRECATED add_camera(const SfmCamera& cam) { │ │ │ │ │ +130 cameras.push_back(cam); │ │ │ │ │ +131 } │ │ │ │ │ +132 size_t GTSAM_DEPRECATED number_tracks() const { return tracks.size(); } │ │ │ │ │ +133 size_t GTSAM_DEPRECATED number_cameras() const { return cameras.size(); } │ │ │ │ │ +135#endif │ │ │ │ │ +138 │ │ │ │ │ +_1_4_0 friend class boost::serialization::access; │ │ │ │ │ +141 template │ │ │ │ │ +142 void serialize(Archive& ar, const unsigned int /*version*/) { │ │ │ │ │ +143 ar& BOOST_SERIALIZATION_NVP(cameras); │ │ │ │ │ +144 ar& BOOST_SERIALIZATION_NVP(tracks); │ │ │ │ │ +145 } │ │ │ │ │ +146 │ │ │ │ │ +148}; │ │ │ │ │ +149 │ │ │ │ │ +151template <> │ │ │ │ │ +_1_5_2struct _t_r_a_i_t_s<_S_f_m_D_a_t_a> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ +153 │ │ │ │ │ +154#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ +155GTSAM_EXPORT bool GTSAM_DEPRECATED readBundler(const std::string& filename, │ │ │ │ │ +156 _S_f_m_D_a_t_a& data); │ │ │ │ │ +157GTSAM_EXPORT bool GTSAM_DEPRECATED readBAL(const std::string& filename, │ │ │ │ │ +158 _S_f_m_D_a_t_a& data); │ │ │ │ │ +159#endif │ │ │ │ │ +160 │ │ │ │ │ +167GTSAM_EXPORT _S_f_m_D_a_t_a _r_e_a_d_B_a_l(const std::string& filename); │ │ │ │ │ +168 │ │ │ │ │ +176GTSAM_EXPORT bool _w_r_i_t_e_B_A_L(const std::string& filename, const _S_f_m_D_a_t_a& │ │ │ │ │ +data); │ │ │ │ │ +177 │ │ │ │ │ +190GTSAM_EXPORT bool _w_r_i_t_e_B_A_L_f_r_o_m_V_a_l_u_e_s(const std::string& filename, │ │ │ │ │ +191 const _S_f_m_D_a_t_a& data, const _V_a_l_u_e_s& values); │ │ │ │ │ +192 │ │ │ │ │ +201GTSAM_EXPORT _P_o_s_e_3 _o_p_e_n_G_L_2_g_t_s_a_m(const _R_o_t_3& R, double tx, double ty, double │ │ │ │ │ +tz); │ │ │ │ │ +202 │ │ │ │ │ +211GTSAM_EXPORT _P_o_s_e_3 _g_t_s_a_m_2_o_p_e_n_G_L(const _R_o_t_3& R, double tx, double ty, double │ │ │ │ │ +tz); │ │ │ │ │ +212 │ │ │ │ │ +218GTSAM_EXPORT _P_o_s_e_3 _g_t_s_a_m_2_o_p_e_n_G_L(const _P_o_s_e_3& PoseGTSAM); │ │ │ │ │ +219 │ │ │ │ │ +228GTSAM_EXPORT _V_a_l_u_e_s _i_n_i_t_i_a_l_C_a_m_e_r_a_s_E_s_t_i_m_a_t_e(const _S_f_m_D_a_t_a& db); │ │ │ │ │ +229 │ │ │ │ │ +238GTSAM_EXPORT _V_a_l_u_e_s _i_n_i_t_i_a_l_C_a_m_e_r_a_s_A_n_d_P_o_i_n_t_s_E_s_t_i_m_a_t_e(const _S_f_m_D_a_t_a& db); │ │ │ │ │ +239 │ │ │ │ │ +240} // namespace gtsam │ │ │ │ │ +_C_a_l_3_B_u_n_d_l_e_r_._h │ │ │ │ │ +Calibration used by Bundler. │ │ │ │ │ +_P_i_n_h_o_l_e_C_a_m_e_r_a_._h │ │ │ │ │ +Base class for all pinhole cameras. │ │ │ │ │ +_V_a_l_u_e_s_._h │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Factor Graph consisting of non-linear factors. │ │ │ │ │ +_S_f_m_T_r_a_c_k_._h │ │ │ │ │ +A simple data structure for a track in Structure from Motion. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l___w_i_t_h___a_b_s___t_o_l │ │ │ │ │ -bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen:: │ │ │ │ │ -DenseBase< MATRIX > &B, double tol=1e-9) │ │ │ │ │ -equals with a tolerance │ │ │ │ │ -DDeeffiinniittiioonn Matrix.h:81 │ │ │ │ │ +_g_t_s_a_m_:_:_w_r_i_t_e_B_A_L │ │ │ │ │ +bool writeBAL(const std::string &filename, const SfmData &data) │ │ │ │ │ +This function writes a "Bundle Adjustment in the Large" (BAL) file from a │ │ │ │ │ +SfmData structure. │ │ │ │ │ +DDeeffiinniittiioonn SfmData.cpp:249 │ │ │ │ │ +_g_t_s_a_m_:_:_w_r_i_t_e_B_A_L_f_r_o_m_V_a_l_u_e_s │ │ │ │ │ +bool writeBALfromValues(const std::string &filename, const SfmData &data, const │ │ │ │ │ +Values &values) │ │ │ │ │ +This function writes a "Bundle Adjustment in the Large" (BAL) file from a │ │ │ │ │ +SfmData structure and a val... │ │ │ │ │ +DDeeffiinniittiioonn SfmData.cpp:349 │ │ │ │ │ +_g_t_s_a_m_:_:_r_e_a_d_B_a_l │ │ │ │ │ +SfmData readBal(const std::string &filename) │ │ │ │ │ +This function parses a "Bundle Adjustment in the Large" (BAL) file and returns │ │ │ │ │ +the data as a SfmData ... │ │ │ │ │ +DDeeffiinniittiioonn SfmData.cpp:344 │ │ │ │ │ +_g_t_s_a_m_:_:_g_t_s_a_m_2_o_p_e_n_G_L │ │ │ │ │ +Pose3 gtsam2openGL(const Rot3 &R, double tx, double ty, double tz) │ │ │ │ │ +This function converts a GTSAM camera pose to an openGL camera pose. │ │ │ │ │ +DDeeffiinniittiioonn SfmData.cpp:88 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_i_t_i_a_l_C_a_m_e_r_a_s_A_n_d_P_o_i_n_t_s_E_s_t_i_m_a_t_e │ │ │ │ │ +Values initialCamerasAndPointsEstimate(const SfmData &db) │ │ │ │ │ +This function creates initial values for cameras and points from db. │ │ │ │ │ +DDeeffiinniittiioonn SfmData.cpp:449 │ │ │ │ │ +_g_t_s_a_m_:_:_S_f_m_C_a_m_e_r_a │ │ │ │ │ +PinholeCamera< Cal3Bundler > SfmCamera │ │ │ │ │ +Define the structure for the camera poses. │ │ │ │ │ +DDeeffiinniittiioonn SfmData.h:33 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_i_t_i_a_l_C_a_m_e_r_a_s_E_s_t_i_m_a_t_e │ │ │ │ │ +Values initialCamerasEstimate(const SfmData &db) │ │ │ │ │ +This function creates initial values for cameras from db. │ │ │ │ │ +DDeeffiinniittiioonn SfmData.cpp:441 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_e_n_G_L_2_g_t_s_a_m │ │ │ │ │ +Pose3 openGL2gtsam(const Rot3 &R, double tx, double ty, double tz) │ │ │ │ │ +This function converts an openGL camera pose to an GTSAM camera pose. │ │ │ │ │ +DDeeffiinniittiioonn SfmData.cpp:79 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ -either a fixed size o... │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_V_e_c_t_o_r_S_p_a_c_e │ │ │ │ │ -VectorSpace provides both Testable and VectorSpaceTraits. │ │ │ │ │ -DDeeffiinniittiioonn VectorSpace.h:207 │ │ │ │ │ -_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s │ │ │ │ │ -DDeeffiinniittiioonn ImuBias.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_:_:_g_y_r_o_s_c_o_p_e │ │ │ │ │ -const Vector3 & gyroscope() const │ │ │ │ │ -get gyroscope bias │ │ │ │ │ -DDeeffiinniittiioonn ImuBias.h:69 │ │ │ │ │ -_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_:_:_I_d_e_n_t_i_t_y │ │ │ │ │ -static ConstantBias Identity() │ │ │ │ │ -identity for group operation │ │ │ │ │ -DDeeffiinniittiioonn ImuBias.h:112 │ │ │ │ │ -_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const ConstantBias &expected, double tol=1e-5) const │ │ │ │ │ -equality up to tolerance │ │ │ │ │ -DDeeffiinniittiioonn ImuBias.h:102 │ │ │ │ │ -_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_:_:_o_p_e_r_a_t_o_r_- │ │ │ │ │ -ConstantBias operator-(const ConstantBias &b) const │ │ │ │ │ -subtraction │ │ │ │ │ -DDeeffiinniittiioonn ImuBias.h:132 │ │ │ │ │ -_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_:_:_c_o_r_r_e_c_t_A_c_c_e_l_e_r_o_m_e_t_e_r │ │ │ │ │ -Vector3 correctAccelerometer(const Vector3 &measurement, OptionalJacobian< 3, 6 │ │ │ │ │ -> H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const │ │ │ │ │ -Correct an accelerometer measurement using this bias model, and optionally │ │ │ │ │ -compute Jacobians. │ │ │ │ │ -DDeeffiinniittiioonn ImuBias.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_:_:_o_p_e_r_a_t_o_r_- │ │ │ │ │ -ConstantBias operator-() const │ │ │ │ │ -inverse │ │ │ │ │ -DDeeffiinniittiioonn ImuBias.h:117 │ │ │ │ │ -_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_:_:_c_o_r_r_e_c_t_G_y_r_o_s_c_o_p_e │ │ │ │ │ -Vector3 correctGyroscope(const Vector3 &measurement, OptionalJacobian< 3, 6 > │ │ │ │ │ -H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const │ │ │ │ │ -Correct a gyroscope measurement using this bias model, and optionally compute │ │ │ │ │ -Jacobians. │ │ │ │ │ -DDeeffiinniittiioonn ImuBias.h:83 │ │ │ │ │ -_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_:_:_v_e_c_t_o_r │ │ │ │ │ -Vector6 vector() const │ │ │ │ │ -return the accelerometer and gyro biases in a single vector │ │ │ │ │ -DDeeffiinniittiioonn ImuBias.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_:_:_o_p_e_r_a_t_o_r_+ │ │ │ │ │ -ConstantBias operator+(const Vector6 &v) const │ │ │ │ │ -addition of vector on right │ │ │ │ │ -DDeeffiinniittiioonn ImuBias.h:122 │ │ │ │ │ -_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_:_:_o_p_e_r_a_t_o_r_+ │ │ │ │ │ -ConstantBias operator+(const ConstantBias &b) const │ │ │ │ │ -addition │ │ │ │ │ -DDeeffiinniittiioonn ImuBias.h:127 │ │ │ │ │ -_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_:_:_a_c_c_e_l_e_r_o_m_e_t_e_r │ │ │ │ │ -const Vector3 & accelerometer() const │ │ │ │ │ -get accelerometer bias │ │ │ │ │ -DDeeffiinniittiioonn ImuBias.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a │ │ │ │ │ +A pinhole camera class that has a Pose3 and a Calibration. │ │ │ │ │ +DDeeffiinniittiioonn PinholeCamera.h:33 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ +A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ +Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor │ │ │ │ │ +symbol GTSAM_USE_QUATERNIO... │ │ │ │ │ +DDeeffiinniittiioonn Rot3.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_S_f_m_D_a_t_a │ │ │ │ │ +SfmData stores a bunch of SfmTracks. │ │ │ │ │ +DDeeffiinniittiioonn SfmData.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_S_f_m_D_a_t_a_:_:_c_a_m_e_r_a_L_i_s_t │ │ │ │ │ +const std::vector< SfmCamera > & cameraList() const │ │ │ │ │ +Getters. │ │ │ │ │ +DDeeffiinniittiioonn SfmData.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_S_f_m_D_a_t_a_:_:_a_d_d_C_a_m_e_r_a │ │ │ │ │ +void addCamera(const SfmCamera &cam) │ │ │ │ │ +Add a camera to SfmData. │ │ │ │ │ +DDeeffiinniittiioonn SfmData.h:71 │ │ │ │ │ +_g_t_s_a_m_:_:_S_f_m_D_a_t_a_:_:_n_u_m_b_e_r_C_a_m_e_r_a_s │ │ │ │ │ +size_t numberCameras() const │ │ │ │ │ +The number of cameras. │ │ │ │ │ +DDeeffiinniittiioonn SfmData.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_S_f_m_D_a_t_a_:_:_n_u_m_b_e_r_T_r_a_c_k_s │ │ │ │ │ +size_t numberTracks() const │ │ │ │ │ +The number of reconstructed 3D points. │ │ │ │ │ +DDeeffiinniittiioonn SfmData.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_S_f_m_D_a_t_a_:_:_t_r_a_c_k │ │ │ │ │ +const SfmTrack & track(size_t idx) const │ │ │ │ │ +The track formed by series of landmark measurements. │ │ │ │ │ +DDeeffiinniittiioonn SfmData.h:80 │ │ │ │ │ +_g_t_s_a_m_:_:_S_f_m_D_a_t_a_:_:_a_d_d_T_r_a_c_k │ │ │ │ │ +void addTrack(const SfmTrack &t) │ │ │ │ │ +Add a track to SfmData. │ │ │ │ │ +DDeeffiinniittiioonn SfmData.h:68 │ │ │ │ │ +_g_t_s_a_m_:_:_S_f_m_D_a_t_a_:_:_c_a_m_e_r_a │ │ │ │ │ +const SfmCamera & camera(size_t idx) const │ │ │ │ │ +The camera pose at frame index idx │ │ │ │ │ +DDeeffiinniittiioonn SfmData.h:83 │ │ │ │ │ +_g_t_s_a_m_:_:_S_f_m_D_a_t_a_:_:_c_a_m_e_r_a_s │ │ │ │ │ +std::vector< SfmCamera > cameras │ │ │ │ │ +Set of cameras. │ │ │ │ │ +DDeeffiinniittiioonn SfmData.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_S_f_m_D_a_t_a_:_:_t_r_a_c_k_s │ │ │ │ │ +std::vector< SfmTrack > tracks │ │ │ │ │ +Sparse set of points. │ │ │ │ │ +DDeeffiinniittiioonn SfmData.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k │ │ │ │ │ +DDeeffiinniittiioonn SfmTrack.h:126 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _I_m_u_B_i_a_s_._h │ │ │ │ │ + * _s_f_m │ │ │ │ │ + * _S_f_m_D_a_t_a_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00956.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00776.html │ │ │ │┄ Files 93% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/ImuBias.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/GaussianBayesNet.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Namespaces | │ │ │ │ -Functions
│ │ │ │ -
ImuBias.cpp File Reference
│ │ │ │ +Classes | │ │ │ │ +Namespaces
│ │ │ │ +
GaussianBayesNet.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ + │ │ │ │ +

Chordal Bayes Net, the result of eliminating a factor graph. │ │ │ │ +More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::GaussianBayesNet
 GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals. More...
 
struct  gtsam::traits< GaussianBayesNet >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
namespace  gtsam::imuBias
 All bias models live in the imuBias namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │

│ │ │ │ -Functions

│ │ │ │ -std::ostream & gtsam::imuBias::operator<< (std::ostream &os, const ConstantBias &bias)
 ostream operator
 
│ │ │ │

Detailed Description

│ │ │ │ -
Date
Feb 2, 2012
│ │ │ │ -
Author
Vadim Indelman, Stephen Williams
│ │ │ │ +

Chordal Bayes Net, the result of eliminating a factor graph.

│ │ │ │ +

GaussianBayesNet

Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,28 +1,30 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ -ImuBias.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +GaussianBayesNet.h File Reference │ │ │ │ │ +Chordal Bayes Net, the result of eliminating a factor graph. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ + class   _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t │ │ │ │ │ +  _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t is a Bayes net made from linear-Gaussian │ │ │ │ │ + conditionals. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_ _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -namespace   _g_t_s_a_m_:_:_i_m_u_B_i_a_s │ │ │ │ │ -  All bias models live in the _i_m_u_B_i_a_s namespace. │ │ │ │ │ -  │ │ │ │ │ -FFuunnccttiioonnss │ │ │ │ │ -std::ostream &  ggttssaamm::::iimmuuBBiiaass::::ooppeerraattoorr<<<< (std::ostream &os, const │ │ │ │ │ - _C_o_n_s_t_a_n_t_B_i_a_s &bias) │ │ │ │ │ -  ostream operator │ │ │ │ │ -  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Feb 2, 2012 │ │ │ │ │ +Chordal Bayes Net, the result of eliminating a factor graph. │ │ │ │ │ +GaussianBayesNet │ │ │ │ │ Author │ │ │ │ │ - Vadim Indelman, Stephen Williams │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _I_m_u_B_i_a_s_._c_p_p │ │ │ │ │ + * _l_i_n_e_a_r │ │ │ │ │ + * _G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00965.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01028.html │ │ │ │┄ Files 79% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/TangentPreintegration.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/expressionTesting.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces | │ │ │ │ -Macros
│ │ │ │ -
TangentPreintegration.cpp File Reference
│ │ │ │ +Macros | │ │ │ │ +Functions
│ │ │ │ +
expressionTesting.h File Reference
│ │ │ │ │ │ │ │
│ │ │ │ + │ │ │ │ +

Test harness methods for expressions. │ │ │ │ +More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ Macros

│ │ │ │ -#define D_R_R(H)   (H)->block<3,3>(0,0)
 
│ │ │ │ -#define D_R_t(H)   (H)->block<3,3>(0,3)
 
│ │ │ │ -#define D_R_v(H)   (H)->block<3,3>(0,6)
 
│ │ │ │ -#define D_t_R(H)   (H)->block<3,3>(3,0)
 
│ │ │ │ -#define D_t_t(H)   (H)->block<3,3>(3,3)
 
│ │ │ │ -#define D_t_v(H)   (H)->block<3,3>(3,6)
 
│ │ │ │ -#define D_v_R(H)   (H)->block<3,3>(6,0)
 
│ │ │ │ -#define D_v_t(H)   (H)->block<3,3>(6,3)
 
│ │ │ │ -#define D_v_v(H)   (H)->block<3,3>(6,6)
 
#define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, numerical_derivative_step, tolerance)    { EXPECT(gtsam::internal::testExpressionJacobians(name_, expression, values, numerical_derivative_step, tolerance)); }
 Check the Jacobians produced by an expression against finite differences.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +template<typename T >
bool gtsam::internal::testExpressionJacobians (const std::string &name_, const gtsam::Expression< T > &expression, const gtsam::Values &values, double nd_step, double tolerance)
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +

Test harness methods for expressions.

│ │ │ │ +
Date
September 18, 2014
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │ -Adam Bry
│ │ │ │ -
│ │ │ │ +Paul Furgale │ │ │ │ +

Macro Definition Documentation

│ │ │ │ + │ │ │ │ +

◆ EXPECT_CORRECT_EXPRESSION_JACOBIANS

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
#define EXPECT_CORRECT_EXPRESSION_JACOBIANS( expression,
 values,
 numerical_derivative_step,
 tolerance 
)    { EXPECT(gtsam::internal::testExpressionJacobians(name_, expression, values, numerical_derivative_step, tolerance)); }
│ │ │ │ +
│ │ │ │ + │ │ │ │ +

Check the Jacobians produced by an expression against finite differences.

│ │ │ │ +
Parameters
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
expressionThe expression to test.
valuesValues filled in for testing the Jacobians.
numerical_derivative_stepThe step to use when computing the finite difference Jacobians
toleranceThe numerical tolerance to use when comparing Jacobians.
│ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,38 +1,58 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s │ │ │ │ │ -TangentPreintegration.cpp File Reference │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _M_a_c_r_o_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +expressionTesting.h File Reference │ │ │ │ │ +Test harness methods for expressions. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ MMaaccrrooss │ │ │ │ │ -#define  DD__RR__RR(H)   (H)->block<3,3>(0,0) │ │ │ │ │ -  │ │ │ │ │ -#define  DD__RR__tt(H)   (H)->block<3,3>(0,3) │ │ │ │ │ -  │ │ │ │ │ -#define  DD__RR__vv(H)   (H)->block<3,3>(0,6) │ │ │ │ │ -  │ │ │ │ │ -#define  DD__tt__RR(H)   (H)->block<3,3>(3,0) │ │ │ │ │ -  │ │ │ │ │ -#define  DD__tt__tt(H)   (H)->block<3,3>(3,3) │ │ │ │ │ -  │ │ │ │ │ -#define  DD__tt__vv(H)   (H)->block<3,3>(3,6) │ │ │ │ │ -  │ │ │ │ │ -#define  DD__vv__RR(H)   (H)->block<3,3>(6,0) │ │ │ │ │ -  │ │ │ │ │ -#define  DD__vv__tt(H)   (H)->block<3,3>(6,3) │ │ │ │ │ -  │ │ │ │ │ -#define  DD__vv__vv(H)   (H)->block<3,3>(6,6) │ │ │ │ │ +#define  _E_X_P_E_C_T___C_O_R_R_E_C_T___E_X_P_R_E_S_S_I_O_N___J_A_C_O_B_I_A_N_S(expression, values, │ │ │ │ │ + numerical_derivative_step, tolerance)    { EXPECT(gtsam::internal:: │ │ │ │ │ + testExpressionJacobians(name_, expression, values, │ │ │ │ │ + numerical_derivative_step, tolerance)); } │ │ │ │ │ +  Check the Jacobians produced by an expression against finite │ │ │ │ │ + differences. │ │ │ │ │ +  │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +template │ │ │ │ │ +bool  ggttssaamm::::iinntteerrnnaall::::tteessttEExxpprreessssiioonnJJaaccoobbiiaannss (const std::string &name_, const │ │ │ │ │ + _g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n< T > &expression, const _g_t_s_a_m_:_:_V_a_l_u_e_s &values, double │ │ │ │ │ + nd_step, double tolerance) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Test harness methods for expressions. │ │ │ │ │ + Date │ │ │ │ │ + September 18, 2014 │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Adam Bry │ │ │ │ │ + Paul Furgale │ │ │ │ │ +********** MMaaccrroo DDeeffiinniittiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ +********** _?◆_? EEXXPPEECCTT__CCOORRRREECCTT__EEXXPPRREESSSSIIOONN__JJAACCOOBBIIAANNSS ********** │ │ │ │ │ +#define (   expression, │ │ │ │ │ +EXPECT_CORRECT_EXPRESSION_JACOBIANS │ │ │ │ │ +   values, │ │ │ │ │ +   numerical_derivative_step, │ │ │ │ │ +   tolerance  │ │ │ │ │ +     { EXPECT(gtsam::internal:: │ │ │ │ │ + testExpressionJacobians(name_, │ │ │ │ │ + ) expression, values, │ │ │ │ │ + numerical_derivative_step, tolerance)); │ │ │ │ │ + } │ │ │ │ │ +Check the Jacobians produced by an expression against finite differences. │ │ │ │ │ + Parameters │ │ │ │ │ + expression The expression to test. │ │ │ │ │ + values _V_a_l_u_e_s filled in for testing the Jacobians. │ │ │ │ │ + numerical_derivative_step The step to use when computing the finite │ │ │ │ │ + difference Jacobians │ │ │ │ │ + tolerance The numerical tolerance to use when comparing │ │ │ │ │ + Jacobians. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n_._c_p_p │ │ │ │ │ + * _n_o_n_l_i_n_e_a_r │ │ │ │ │ + * _e_x_p_r_e_s_s_i_o_n_T_e_s_t_i_n_g_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00974_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00923_source.html │ │ │ │┄ Files 99% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
AttitudeFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
18#pragma once
│ │ │ │
19
│ │ │ │ -
20#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
21#include <gtsam/geometry/Pose3.h>
│ │ │ │ +
20#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
21#include <gtsam/geometry/Pose3.h>
│ │ │ │
22#include <gtsam/geometry/Unit3.h>
│ │ │ │
23
│ │ │ │
24namespace gtsam {
│ │ │ │
25
│ │ │ │
│ │ │ │
34class AttitudeFactor {
│ │ │ │
35
│ │ │ │ @@ -217,15 +217,15 @@ │ │ │ │
136 ar & boost::serialization::make_nvp("NoiseModelFactor1",
│ │ │ │
137 boost::serialization::base_object<Base>(*this));
│ │ │ │
138 ar & boost::serialization::make_nvp("AttitudeFactor",
│ │ │ │
139 boost::serialization::base_object<AttitudeFactor>(*this));
│ │ │ │
140 }
│ │ │ │
141
│ │ │ │
142public:
│ │ │ │ -
143 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
143 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │
144};
│ │ │ │
│ │ │ │
145
│ │ │ │
147template<> struct traits<Rot3AttitudeFactor> : public Testable<Rot3AttitudeFactor> {};
│ │ │ │
148
│ │ │ │
│ │ │ │
153class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactorN<Pose3>,
│ │ │ │ @@ -288,25 +288,25 @@ │ │ │ │
217 ar & boost::serialization::make_nvp("NoiseModelFactor1",
│ │ │ │
218 boost::serialization::base_object<Base>(*this));
│ │ │ │
219 ar & boost::serialization::make_nvp("AttitudeFactor",
│ │ │ │
220 boost::serialization::base_object<AttitudeFactor>(*this));
│ │ │ │
221 }
│ │ │ │
222
│ │ │ │
223public:
│ │ │ │ -
224 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
224 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │
225};
│ │ │ │
│ │ │ │
226
│ │ │ │
228template<> struct traits<Pose3AttitudeFactor> : public Testable<Pose3AttitudeFactor> {};
│ │ │ │
229
│ │ │ │
230}
│ │ │ │
231
│ │ │ │ -
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ -
Pose3.h
3D Pose
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ +
Pose3.h
3D Pose
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ @@ -339,13 +339,13 @@ │ │ │ │
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00977.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01304.html │ │ │ │┄ Files 94% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/TangentPreintegration.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/ProjectionFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
TangentPreintegration.h File Reference
│ │ │ │ +
ProjectionFactor.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Reprojection of a LANDMARK to a 2D point. │ │ │ │ +More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::TangentPreintegration
 Integrate on the 9D tangent space of the NavState manifold. More...
class  gtsam::GenericProjectionFactor< POSE, LANDMARK, CALIBRATION >
 Non-linear factor for a constraint derived from a 2D measurement. More...
 
struct  gtsam::traits< GenericProjectionFactor< POSE, LANDMARK, CALIBRATION > >
 traits More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +

Reprojection of a LANDMARK to a 2D point.

│ │ │ │ +
Author
Chris Beall
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │
│ │ │ │ -Adam Bry
│ │ │ │ +Alex Cunningham
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,24 +1,33 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -TangentPreintegration.h File Reference │ │ │ │ │ +ProjectionFactor.h File Reference │ │ │ │ │ +Reprojection of a LANDMARK to a 2D point. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n │ │ │ │ │ -  Integrate on the 9D tangent space of the _N_a_v_S_t_a_t_e manifold. _M_o_r_e_._._. │ │ │ │ │ + class   _g_t_s_a_m_:_:_G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_<_ _P_O_S_E_,_ _L_A_N_D_M_A_R_K_,_ _C_A_L_I_B_R_A_T_I_O_N_ _> │ │ │ │ │ +  Non-linear factor for a constraint derived from a 2D measurement. │ │ │ │ │ + _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _G_e_n_e_r_i_c_P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_<_ _P_O_S_E_,_ _L_A_N_D_M_A_R_K_,_ _C_A_L_I_B_R_A_T_I_O_N_ _> │ │ │ │ │ + _> │ │ │ │ │ +  traits _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Reprojection of a LANDMARK to a 2D point. │ │ │ │ │ Author │ │ │ │ │ + Chris Beall │ │ │ │ │ + Richard Roberts │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Adam Bry │ │ │ │ │ + Alex Cunningham │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _T_a_n_g_e_n_t_P_r_e_i_n_t_e_g_r_a_t_i_o_n_._h │ │ │ │ │ + * _s_l_a_m │ │ │ │ │ + * _P_r_o_j_e_c_t_i_o_n_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00983.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00356.html │ │ │ │┄ Files 89% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/PreintegrationBase.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cal3DS2_Base.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
PreintegrationBase.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
Cal3DS2_Base.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::PreintegrationBase
 PreintegrationBase is the base class for PreintegratedMeasurements (in ImuFactor) and CombinedPreintegratedMeasurements (in CombinedImuFactor). More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +std::ostream & gtsam::operator<< (std::ostream &os, const Cal3DS2_Base &cal)
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Luca Carlone
│ │ │ │ -
│ │ │ │ -Stephen Williams
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ -
│ │ │ │ -Vadim Indelman
│ │ │ │ -
│ │ │ │ -David Jensen
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ -
│ │ │ │ -Varun Agrawal
│ │ │ │ -
│ │ │ │ -Luca Carlone
│ │ │ │ -
│ │ │ │ -Stephen Williams
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ -
│ │ │ │ -Vadim Indelman
│ │ │ │ -
│ │ │ │ -David Jensen
│ │ │ │ +
Date
Feb 28, 2010
│ │ │ │ +
Author
ydjian
│ │ │ │
│ │ │ │ -Frank Dellaert
│ │ │ │ +Varun Agrawal
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,37 +1,24 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -PreintegrationBase.h File Reference │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e │ │ │ │ │ - _P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e is the base class for PreintegratedMeasurements (in │ │ │ │ │ -  _I_m_u_F_a_c_t_o_r) and CombinedPreintegratedMeasurements (in │ │ │ │ │ - _C_o_m_b_i_n_e_d_I_m_u_F_a_c_t_o_r). _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +Cal3DS2_Base.cpp File Reference │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +std::ostream &  ggttssaamm::::ooppeerraattoorr<<<< (std::ostream &os, const _C_a_l_3_D_S_2___B_a_s_e &cal) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ + Date │ │ │ │ │ + Feb 28, 2010 │ │ │ │ │ Author │ │ │ │ │ - Luca Carlone │ │ │ │ │ - Stephen Williams │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Vadim Indelman │ │ │ │ │ - David Jensen │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + ydjian │ │ │ │ │ Varun Agrawal │ │ │ │ │ - Luca Carlone │ │ │ │ │ - Stephen Williams │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Vadim Indelman │ │ │ │ │ - David Jensen │ │ │ │ │ - Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_._h │ │ │ │ │ + * _g_e_o_m_e_t_r_y │ │ │ │ │ + * _C_a_l_3_D_S_2___B_a_s_e_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a00983_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01037_source.html │ │ │ │┄ Files 90% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/PreintegrationBase.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/LevenbergMarquardtParams.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
PreintegrationBase.h
│ │ │ │ +
LevenbergMarquardtParams.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
22#pragma once
│ │ │ │ -
23
│ │ │ │ -
24#include <gtsam/navigation/PreintegrationParams.h>
│ │ │ │ -
25#include <gtsam/navigation/NavState.h>
│ │ │ │ -
26#include <gtsam/navigation/ImuBias.h>
│ │ │ │ -
27#include <gtsam/linear/NoiseModel.h>
│ │ │ │ -
28
│ │ │ │ -
29#include <iosfwd>
│ │ │ │ -
30#include <string>
│ │ │ │ -
31#include <utility>
│ │ │ │ -
32
│ │ │ │ -
33namespace gtsam {
│ │ │ │ -
34
│ │ │ │ -
│ │ │ │ -
41class GTSAM_EXPORT PreintegrationBase {
│ │ │ │ -
42 public:
│ │ │ │ -
43 typedef imuBias::ConstantBias Bias;
│ │ │ │ -
44 typedef PreintegrationParams Params;
│ │ │ │ -
45
│ │ │ │ -
46 protected:
│ │ │ │ -
47 boost::shared_ptr<Params> p_;
│ │ │ │ +
21#pragma once
│ │ │ │ +
22
│ │ │ │ +
23#include <gtsam/nonlinear/NonlinearOptimizerParams.h>
│ │ │ │ +
24#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ +
25
│ │ │ │ +
26namespace gtsam {
│ │ │ │ +
27
│ │ │ │ +
28class LevenbergMarquardtOptimizer;
│ │ │ │ +
29
│ │ │ │ +
│ │ │ │ +
35class GTSAM_EXPORT LevenbergMarquardtParams: public NonlinearOptimizerParams {
│ │ │ │ +
36
│ │ │ │ +
37public:
│ │ │ │ +
│ │ │ │ +
39 enum VerbosityLM {
│ │ │ │ +
40 SILENT = 0, SUMMARY, TERMINATION, LAMBDA, TRYLAMBDA, TRYCONFIG, DAMPED, TRYDELTA
│ │ │ │ +
41 };
│ │ │ │ +
│ │ │ │ +
42
│ │ │ │ +
43 static VerbosityLM verbosityLMTranslator(const std::string &s);
│ │ │ │ +
44 static std::string verbosityLMTranslator(VerbosityLM value);
│ │ │ │ +
45 using OptimizerType = LevenbergMarquardtOptimizer;
│ │ │ │ +
46
│ │ │ │ +
47public:
│ │ │ │
48
│ │ │ │ -
50 Bias biasHat_;
│ │ │ │ -
51
│ │ │ │ -
53 double deltaTij_;
│ │ │ │ -
54
│ │ │ │ -
56 PreintegrationBase() {}
│ │ │ │ -
57
│ │ │ │ -
59 virtual ~PreintegrationBase() {}
│ │ │ │ +
49 double lambdaInitial;
│ │ │ │ +
50 double lambdaFactor;
│ │ │ │ +
51 double lambdaUpperBound;
│ │ │ │ +
52 double lambdaLowerBound;
│ │ │ │ +
53 VerbosityLM verbosityLM;
│ │ │ │ +
54 double minModelFidelity;
│ │ │ │ +
55 std::string logFile;
│ │ │ │ +
56 bool diagonalDamping;
│ │ │ │ +
57 bool useFixedLambdaFactor;
│ │ │ │ +
58 double minDiagonal;
│ │ │ │ +
59 double maxDiagonal;
│ │ │ │
60
│ │ │ │ -
61 public:
│ │ │ │ -
64
│ │ │ │ -
70 PreintegrationBase(const boost::shared_ptr<Params>& p,
│ │ │ │ -
71 const imuBias::ConstantBias& biasHat = imuBias::ConstantBias());
│ │ │ │ -
72
│ │ │ │ -
74
│ │ │ │ -
78 virtual void resetIntegration() = 0;
│ │ │ │ -
79
│ │ │ │ -
83 void resetIntegrationAndSetBias(const Bias& biasHat);
│ │ │ │ -
84
│ │ │ │ -
│ │ │ │ -
86 bool matchesParamsWith(const PreintegrationBase& other) const {
│ │ │ │ -
87 return p_.get() == other.p_.get();
│ │ │ │ -
88 }
│ │ │ │ -
│ │ │ │ -
89
│ │ │ │ -
│ │ │ │ -
91 const boost::shared_ptr<Params>& params() const {
│ │ │ │ -
92 return p_;
│ │ │ │ -
93 }
│ │ │ │ -
│ │ │ │ -
94
│ │ │ │ -
│ │ │ │ -
96 Params& p() const {
│ │ │ │ -
97 return *p_;
│ │ │ │ +
61 LevenbergMarquardtParams()
│ │ │ │ +
62 : verbosityLM(SILENT),
│ │ │ │ +
63 diagonalDamping(false),
│ │ │ │ +
64 minDiagonal(1e-6),
│ │ │ │ +
65 maxDiagonal(1e32) {
│ │ │ │ +
66 SetLegacyDefaults(this);
│ │ │ │ +
67 }
│ │ │ │ +
68
│ │ │ │ +
69 static void SetLegacyDefaults(LevenbergMarquardtParams* p) {
│ │ │ │ +
70 // Relevant NonlinearOptimizerParams:
│ │ │ │ +
71 p->maxIterations = 100;
│ │ │ │ +
72 p->relativeErrorTol = 1e-5;
│ │ │ │ +
73 p->absoluteErrorTol = 1e-5;
│ │ │ │ +
74 // LM-specific:
│ │ │ │ +
75 p->lambdaInitial = 1e-5;
│ │ │ │ +
76 p->lambdaFactor = 10.0;
│ │ │ │ +
77 p->lambdaUpperBound = 1e5;
│ │ │ │ +
78 p->lambdaLowerBound = 0.0;
│ │ │ │ +
79 p->minModelFidelity = 1e-3;
│ │ │ │ +
80 p->diagonalDamping = false;
│ │ │ │ +
81 p->useFixedLambdaFactor = true;
│ │ │ │ +
82 }
│ │ │ │ +
83
│ │ │ │ +
84 // these do seem to work better for SFM
│ │ │ │ +
85 static void SetCeresDefaults(LevenbergMarquardtParams* p) {
│ │ │ │ +
86 // Relevant NonlinearOptimizerParams:
│ │ │ │ +
87 p->maxIterations = 50;
│ │ │ │ +
88 p->absoluteErrorTol = 0; // No corresponding option in CERES
│ │ │ │ +
89 p->relativeErrorTol = 1e-6; // This is function_tolerance
│ │ │ │ +
90 // LM-specific:
│ │ │ │ +
91 p->lambdaUpperBound = 1e32;
│ │ │ │ +
92 p->lambdaLowerBound = 1e-16;
│ │ │ │ +
93 p->lambdaInitial = 1e-04;
│ │ │ │ +
94 p->lambdaFactor = 2.0;
│ │ │ │ +
95 p->minModelFidelity = 1e-3; // options.min_relative_decrease in CERES
│ │ │ │ +
96 p->diagonalDamping = true;
│ │ │ │ +
97 p->useFixedLambdaFactor = false; // This is important
│ │ │ │
98 }
│ │ │ │ -
│ │ │ │
99
│ │ │ │ -
101
│ │ │ │ -
104 const imuBias::ConstantBias& biasHat() const { return biasHat_; }
│ │ │ │ -
105 double deltaTij() const { return deltaTij_; }
│ │ │ │ -
106
│ │ │ │ -
107 virtual Vector3 deltaPij() const = 0;
│ │ │ │ -
108 virtual Vector3 deltaVij() const = 0;
│ │ │ │ -
109 virtual Rot3 deltaRij() const = 0;
│ │ │ │ -
110 virtual NavState deltaXij() const = 0;
│ │ │ │ +
100 static LevenbergMarquardtParams LegacyDefaults() {
│ │ │ │ +
101 LevenbergMarquardtParams p;
│ │ │ │ +
102 SetLegacyDefaults(&p);
│ │ │ │ +
103 return p;
│ │ │ │ +
104 }
│ │ │ │ +
105
│ │ │ │ +
106 static LevenbergMarquardtParams CeresDefaults() {
│ │ │ │ +
107 LevenbergMarquardtParams p;
│ │ │ │ +
108 SetCeresDefaults(&p);
│ │ │ │ +
109 return p;
│ │ │ │ +
110 }
│ │ │ │
111
│ │ │ │ -
112 // Exposed for MATLAB
│ │ │ │ -
113 Vector6 biasHatVector() const { return biasHat_.vector(); }
│ │ │ │ -
115
│ │ │ │ -
118 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const PreintegrationBase& pim);
│ │ │ │ -
119 virtual void print(const std::string& s="") const;
│ │ │ │ -
121
│ │ │ │ +
112 static LevenbergMarquardtParams EnsureHasOrdering(LevenbergMarquardtParams params,
│ │ │ │ +
113 const NonlinearFactorGraph& graph) {
│ │ │ │ +
114 if (!params.ordering)
│ │ │ │ +
115 params.ordering = Ordering::Create(params.orderingType, graph);
│ │ │ │ +
116 return params;
│ │ │ │ +
117 }
│ │ │ │ +
118
│ │ │ │ +
119 static LevenbergMarquardtParams ReplaceOrdering(LevenbergMarquardtParams params,
│ │ │ │ +
120 const Ordering& ordering) {
│ │ │ │ +
121 params.ordering = ordering;
│ │ │ │ +
122 return params;
│ │ │ │ +
123 }
│ │ │ │
124
│ │ │ │ -
130 std::pair<Vector3, Vector3> correctMeasurementsBySensorPose(
│ │ │ │ -
131 const Vector3& unbiasedAcc, const Vector3& unbiasedOmega,
│ │ │ │ -
132 OptionalJacobian<3, 3> correctedAcc_H_unbiasedAcc = boost::none,
│ │ │ │ -
133 OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none,
│ │ │ │ -
134 OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = boost::none) const;
│ │ │ │ -
135
│ │ │ │ -
141 virtual void update(const Vector3& measuredAcc, const Vector3& measuredOmega,
│ │ │ │ -
142 const double dt, Matrix9* A, Matrix93* B, Matrix93* C) = 0;
│ │ │ │ -
143
│ │ │ │ -
145 virtual void integrateMeasurement(const Vector3& measuredAcc,
│ │ │ │ -
146 const Vector3& measuredOmega, const double dt);
│ │ │ │ -
147
│ │ │ │ -
150 virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
│ │ │ │ -
151 OptionalJacobian<9, 6> H = boost::none) const = 0;
│ │ │ │ -
152
│ │ │ │ -
154 NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
│ │ │ │ -
155 OptionalJacobian<9, 9> H1 = boost::none,
│ │ │ │ -
156 OptionalJacobian<9, 6> H2 = boost::none) const;
│ │ │ │ -
157
│ │ │ │ -
159 Vector9 computeError(const NavState& state_i, const NavState& state_j,
│ │ │ │ -
160 const imuBias::ConstantBias& bias_i,
│ │ │ │ -
161 OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2,
│ │ │ │ -
162 OptionalJacobian<9, 6> H3) const;
│ │ │ │ -
163
│ │ │ │ -
168 Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
│ │ │ │ -
169 const Pose3& pose_j, const Vector3& vel_j,
│ │ │ │ -
170 const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H1 =
│ │ │ │ -
171 boost::none, OptionalJacobian<9, 3> H2 = boost::none,
│ │ │ │ -
172 OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
│ │ │ │ -
173 boost::none, OptionalJacobian<9, 6> H5 = boost::none) const;
│ │ │ │ -
174
│ │ │ │ -
175 private:
│ │ │ │ -
177 friend class boost::serialization::access;
│ │ │ │ -
178 template<class ARCHIVE>
│ │ │ │ -
179 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
180 ar & BOOST_SERIALIZATION_NVP(p_);
│ │ │ │ -
181 ar & BOOST_SERIALIZATION_NVP(biasHat_);
│ │ │ │ -
182 ar & BOOST_SERIALIZATION_NVP(deltaTij_);
│ │ │ │ -
183 }
│ │ │ │ -
184
│ │ │ │ -
185 public:
│ │ │ │ -
186 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
187};
│ │ │ │ -
│ │ │ │ -
188
│ │ │ │ -
189}
│ │ │ │ -
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -
NavState.h
Navigation state composing of attitude, position, and velocity.
│ │ │ │ +
125 ~LevenbergMarquardtParams() override {}
│ │ │ │ +
126 void print(const std::string& str = "") const override;
│ │ │ │ +
127
│ │ │ │ +
130 bool getDiagonalDamping() const { return diagonalDamping; }
│ │ │ │ +
131 double getlambdaFactor() const { return lambdaFactor; }
│ │ │ │ +
132 double getlambdaInitial() const { return lambdaInitial; }
│ │ │ │ +
133 double getlambdaLowerBound() const { return lambdaLowerBound; }
│ │ │ │ +
134 double getlambdaUpperBound() const { return lambdaUpperBound; }
│ │ │ │ +
135 bool getUseFixedLambdaFactor() { return useFixedLambdaFactor; }
│ │ │ │ +
136 std::string getLogFile() const { return logFile; }
│ │ │ │ +
137 std::string getVerbosityLM() const { return verbosityLMTranslator(verbosityLM);}
│ │ │ │ +
138
│ │ │ │ +
139 void setDiagonalDamping(bool flag) { diagonalDamping = flag; }
│ │ │ │ +
140 void setlambdaFactor(double value) { lambdaFactor = value; }
│ │ │ │ +
141 void setlambdaInitial(double value) { lambdaInitial = value; }
│ │ │ │ +
142 void setlambdaLowerBound(double value) { lambdaLowerBound = value; }
│ │ │ │ +
143 void setlambdaUpperBound(double value) { lambdaUpperBound = value; }
│ │ │ │ +
144 void setUseFixedLambdaFactor(bool flag) { useFixedLambdaFactor = flag;}
│ │ │ │ +
145 void setLogFile(const std::string& s) { logFile = s; }
│ │ │ │ +
146 void setVerbosityLM(const std::string& s) { verbosityLM = verbosityLMTranslator(s);}
│ │ │ │ +
147 // @}
│ │ │ │ +
150
│ │ │ │ +
│ │ │ │ +
152 boost::shared_ptr<NonlinearOptimizerParams> clone() const {
│ │ │ │ +
153 return boost::shared_ptr<NonlinearOptimizerParams>(new LevenbergMarquardtParams(*this));
│ │ │ │ +
154 }
│ │ │ │ +
│ │ │ │ +
155
│ │ │ │ +
157};
│ │ │ │ +
│ │ │ │ +
158
│ │ │ │ +
159}
│ │ │ │ +
NonlinearOptimizerParams.h
Parameters for nonlinear optimization.
│ │ │ │ +
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ -
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ -
gtsam::imuBias::ConstantBias
Definition ImuBias.h:30
│ │ │ │ -
gtsam::imuBias::ConstantBias::vector
Vector6 vector() const
return the accelerometer and gyro biases in a single vector
Definition ImuBias.h:57
│ │ │ │ -
gtsam::NavState
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition NavState.h:34
│ │ │ │ -
gtsam::PreintegrationBase
PreintegrationBase is the base class for PreintegratedMeasurements (in ImuFactor) and CombinedPreinte...
Definition PreintegrationBase.h:41
│ │ │ │ -
gtsam::PreintegrationBase::deltaTij_
double deltaTij_
Time interval from i to j.
Definition PreintegrationBase.h:53
│ │ │ │ -
gtsam::PreintegrationBase::~PreintegrationBase
virtual ~PreintegrationBase()
Virtual destructor for serialization.
Definition PreintegrationBase.h:59
│ │ │ │ -
gtsam::PreintegrationBase::matchesParamsWith
bool matchesParamsWith(const PreintegrationBase &other) const
check parameters equality: checks whether shared pointer points to same Params object.
Definition PreintegrationBase.h:86
│ │ │ │ -
gtsam::PreintegrationBase::biasCorrectedDelta
virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias &bias_i, OptionalJacobian< 9, 6 > H=boost::none) const =0
Given the estimate of the bias, return a NavState tangent vector summarizing the preintegrated IMU me...
│ │ │ │ -
gtsam::PreintegrationBase::p
Params & p() const
const reference to params
Definition PreintegrationBase.h:96
│ │ │ │ -
gtsam::PreintegrationBase::biasHat_
Bias biasHat_
Acceleration and gyro bias used for preintegration.
Definition PreintegrationBase.h:50
│ │ │ │ -
gtsam::PreintegrationBase::PreintegrationBase
PreintegrationBase()
Default constructor for serialization.
Definition PreintegrationBase.h:56
│ │ │ │ -
gtsam::PreintegrationBase::params
const boost::shared_ptr< Params > & params() const
shared pointer to params
Definition PreintegrationBase.h:91
│ │ │ │ -
gtsam::PreintegrationBase::update
virtual void update(const Vector3 &measuredAcc, const Vector3 &measuredOmega, const double dt, Matrix9 *A, Matrix93 *B, Matrix93 *C)=0
Update preintegrated measurements and get derivatives It takes measured quantities in the j frame Mod...
│ │ │ │ -
gtsam::PreintegrationParams
Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
Definition PreintegrationParams.h:26
│ │ │ │ +
gtsam::LevenbergMarquardtOptimizer
This class performs Levenberg-Marquardt nonlinear optimization.
Definition LevenbergMarquardtOptimizer.h:35
│ │ │ │ +
gtsam::LevenbergMarquardtParams
Parameters for Levenberg-Marquardt optimization.
Definition LevenbergMarquardtParams.h:35
│ │ │ │ +
gtsam::LevenbergMarquardtParams::lambdaFactor
double lambdaFactor
The amount by which to multiply or divide lambda when adjusting lambda (default: 10....
Definition LevenbergMarquardtParams.h:50
│ │ │ │ +
gtsam::LevenbergMarquardtParams::minDiagonal
double minDiagonal
when using diagonal damping saturates the minimum diagonal entries (default: 1e-6)
Definition LevenbergMarquardtParams.h:58
│ │ │ │ +
gtsam::LevenbergMarquardtParams::lambdaUpperBound
double lambdaUpperBound
The maximum lambda to try before assuming the optimization has failed (default: 1e5)
Definition LevenbergMarquardtParams.h:51
│ │ │ │ +
gtsam::LevenbergMarquardtParams::lambdaInitial
double lambdaInitial
The initial Levenberg-Marquardt damping term (default: 1e-5)
Definition LevenbergMarquardtParams.h:49
│ │ │ │ +
gtsam::LevenbergMarquardtParams::maxDiagonal
double maxDiagonal
when using diagonal damping saturates the maximum diagonal entries (default: 1e32)
Definition LevenbergMarquardtParams.h:59
│ │ │ │ +
gtsam::LevenbergMarquardtParams::minModelFidelity
double minModelFidelity
Lower bound for the modelFidelity to accept the result of an LM iteration.
Definition LevenbergMarquardtParams.h:54
│ │ │ │ +
gtsam::LevenbergMarquardtParams::lambdaLowerBound
double lambdaLowerBound
The minimum lambda used in LM (default: 0)
Definition LevenbergMarquardtParams.h:52
│ │ │ │ +
gtsam::LevenbergMarquardtParams::clone
boost::shared_ptr< NonlinearOptimizerParams > clone() const
Definition LevenbergMarquardtParams.h:152
│ │ │ │ +
gtsam::LevenbergMarquardtParams::diagonalDamping
bool diagonalDamping
if true, use diagonal of Hessian
Definition LevenbergMarquardtParams.h:56
│ │ │ │ +
gtsam::LevenbergMarquardtParams::useFixedLambdaFactor
bool useFixedLambdaFactor
if true applies constant increase (or decrease) to lambda according to lambdaFactor
Definition LevenbergMarquardtParams.h:57
│ │ │ │ +
gtsam::LevenbergMarquardtParams::logFile
std::string logFile
an optional CSV log file, with [iteration, time, error, lambda]
Definition LevenbergMarquardtParams.h:55
│ │ │ │ +
gtsam::LevenbergMarquardtParams::VerbosityLM
VerbosityLM
See LevenbergMarquardtParams::verbosityLM.
Definition LevenbergMarquardtParams.h:39
│ │ │ │ +
gtsam::LevenbergMarquardtParams::verbosityLM
VerbosityLM verbosityLM
The verbosity level for Levenberg-Marquardt (default: SILENT), see also NonlinearOptimizerParams::ver...
Definition LevenbergMarquardtParams.h:53
│ │ │ │ +
gtsam::NonlinearOptimizerParams
The common parameters for Nonlinear optimizers.
Definition NonlinearOptimizerParams.h:34
│ │ │ │ +
gtsam::NonlinearOptimizerParams::absoluteErrorTol
double absoluteErrorTol
The maximum absolute error decrease to stop iterating (default 1e-5)
Definition NonlinearOptimizerParams.h:43
│ │ │ │ +
gtsam::NonlinearOptimizerParams::maxIterations
size_t maxIterations
The maximum iterations to stop iterating (default 100)
Definition NonlinearOptimizerParams.h:41
│ │ │ │ +
gtsam::NonlinearOptimizerParams::relativeErrorTol
double relativeErrorTol
The maximum relative error decrease to stop iterating (default 1e-5)
Definition NonlinearOptimizerParams.h:42
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,219 +1,247 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -PreintegrationBase.h │ │ │ │ │ +LevenbergMarquardtParams.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -22#pragma once │ │ │ │ │ -23 │ │ │ │ │ -24#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_N_a_v_S_t_a_t_e_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_I_m_u_B_i_a_s_._h> │ │ │ │ │ -27#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_N_o_i_s_e_M_o_d_e_l_._h> │ │ │ │ │ -28 │ │ │ │ │ -29#include │ │ │ │ │ -30#include │ │ │ │ │ -31#include │ │ │ │ │ -32 │ │ │ │ │ -33namespace _g_t_s_a_m { │ │ │ │ │ -34 │ │ │ │ │ -_4_1class GTSAM_EXPORT _P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e { │ │ │ │ │ -42 public: │ │ │ │ │ -43 typedef _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s _B_i_a_s; │ │ │ │ │ -44 typedef _P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s _P_a_r_a_m_s; │ │ │ │ │ -45 │ │ │ │ │ -46 protected: │ │ │ │ │ -47 boost::shared_ptr p_; │ │ │ │ │ +21#pragma once │ │ │ │ │ +22 │ │ │ │ │ +23#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +25 │ │ │ │ │ +26namespace _g_t_s_a_m { │ │ │ │ │ +27 │ │ │ │ │ +28class LevenbergMarquardtOptimizer; │ │ │ │ │ +29 │ │ │ │ │ +_3_5class GTSAM_EXPORT _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s: public _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s │ │ │ │ │ +{ │ │ │ │ │ +36 │ │ │ │ │ +37public: │ │ │ │ │ +_3_9 enum _V_e_r_b_o_s_i_t_y_L_M { │ │ │ │ │ +40 SILENT = 0, SUMMARY, TERMINATION, LAMBDA, TRYLAMBDA, TRYCONFIG, DAMPED, │ │ │ │ │ +TRYDELTA │ │ │ │ │ +41 }; │ │ │ │ │ +42 │ │ │ │ │ +43 static VerbosityLM verbosityLMTranslator(const std::string &s); │ │ │ │ │ +44 static std::string verbosityLMTranslator(VerbosityLM value); │ │ │ │ │ +45 using OptimizerType = _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r; │ │ │ │ │ +46 │ │ │ │ │ +47public: │ │ │ │ │ 48 │ │ │ │ │ -_5_0 _B_i_a_s _b_i_a_s_H_a_t__; │ │ │ │ │ -51 │ │ │ │ │ -_5_3 double _d_e_l_t_a_T_i_j__; │ │ │ │ │ -54 │ │ │ │ │ -_5_6 _P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e() {} │ │ │ │ │ -57 │ │ │ │ │ -_5_9 virtual _~_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e() {} │ │ │ │ │ +_4_9 double _l_a_m_b_d_a_I_n_i_t_i_a_l; │ │ │ │ │ +_5_0 double _l_a_m_b_d_a_F_a_c_t_o_r; │ │ │ │ │ +_5_1 double _l_a_m_b_d_a_U_p_p_e_r_B_o_u_n_d; │ │ │ │ │ +_5_2 double _l_a_m_b_d_a_L_o_w_e_r_B_o_u_n_d; │ │ │ │ │ +_5_3 _V_e_r_b_o_s_i_t_y_L_M _v_e_r_b_o_s_i_t_y_L_M; │ │ │ │ │ +_5_4 double _m_i_n_M_o_d_e_l_F_i_d_e_l_i_t_y; │ │ │ │ │ +_5_5 std::string _l_o_g_F_i_l_e; │ │ │ │ │ +_5_6 bool _d_i_a_g_o_n_a_l_D_a_m_p_i_n_g; │ │ │ │ │ +_5_7 bool _u_s_e_F_i_x_e_d_L_a_m_b_d_a_F_a_c_t_o_r; │ │ │ │ │ +_5_8 double _m_i_n_D_i_a_g_o_n_a_l; │ │ │ │ │ +_5_9 double _m_a_x_D_i_a_g_o_n_a_l; │ │ │ │ │ 60 │ │ │ │ │ -61 public: │ │ │ │ │ -64 │ │ │ │ │ -70 _P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e(const boost::shared_ptr& p, │ │ │ │ │ -71 const _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s& biasHat = _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s()); │ │ │ │ │ -72 │ │ │ │ │ -74 │ │ │ │ │ -78 virtual void resetIntegration() = 0; │ │ │ │ │ -79 │ │ │ │ │ -83 void resetIntegrationAndSetBias(const Bias& biasHat); │ │ │ │ │ -84 │ │ │ │ │ -_8_6 bool _m_a_t_c_h_e_s_P_a_r_a_m_s_W_i_t_h(const _P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e& other) const { │ │ │ │ │ -87 return p_.get() == other.p_.get(); │ │ │ │ │ -88 } │ │ │ │ │ -89 │ │ │ │ │ -_9_1 const boost::shared_ptr& _p_a_r_a_m_s() const { │ │ │ │ │ -92 return p_; │ │ │ │ │ -93 } │ │ │ │ │ -94 │ │ │ │ │ -_9_6 _P_a_r_a_m_s& _p() const { │ │ │ │ │ -97 return *p_; │ │ │ │ │ +61 _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s() │ │ │ │ │ +62 : verbosityLM(SILENT), │ │ │ │ │ +63 diagonalDamping(false), │ │ │ │ │ +64 minDiagonal(1e-6), │ │ │ │ │ +65 maxDiagonal(1e32) { │ │ │ │ │ +66 SetLegacyDefaults(this); │ │ │ │ │ +67 } │ │ │ │ │ +68 │ │ │ │ │ +69 static void SetLegacyDefaults(_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s* p) { │ │ │ │ │ +70 // Relevant NonlinearOptimizerParams: │ │ │ │ │ +71 p->_m_a_x_I_t_e_r_a_t_i_o_n_s = 100; │ │ │ │ │ +72 p->_r_e_l_a_t_i_v_e_E_r_r_o_r_T_o_l = 1e-5; │ │ │ │ │ +73 p->_a_b_s_o_l_u_t_e_E_r_r_o_r_T_o_l = 1e-5; │ │ │ │ │ +74 // LM-specific: │ │ │ │ │ +75 p->_l_a_m_b_d_a_I_n_i_t_i_a_l = 1e-5; │ │ │ │ │ +76 p->_l_a_m_b_d_a_F_a_c_t_o_r = 10.0; │ │ │ │ │ +77 p->_l_a_m_b_d_a_U_p_p_e_r_B_o_u_n_d = 1e5; │ │ │ │ │ +78 p->_l_a_m_b_d_a_L_o_w_e_r_B_o_u_n_d = 0.0; │ │ │ │ │ +79 p->_m_i_n_M_o_d_e_l_F_i_d_e_l_i_t_y = 1e-3; │ │ │ │ │ +80 p->_d_i_a_g_o_n_a_l_D_a_m_p_i_n_g = false; │ │ │ │ │ +81 p->_u_s_e_F_i_x_e_d_L_a_m_b_d_a_F_a_c_t_o_r = true; │ │ │ │ │ +82 } │ │ │ │ │ +83 │ │ │ │ │ +84 // these do seem to work better for SFM │ │ │ │ │ +85 static void SetCeresDefaults(LevenbergMarquardtParams* p) { │ │ │ │ │ +86 // Relevant NonlinearOptimizerParams: │ │ │ │ │ +87 p->maxIterations = 50; │ │ │ │ │ +88 p->absoluteErrorTol = 0; // No corresponding option in CERES │ │ │ │ │ +89 p->relativeErrorTol = 1e-6; // This is function_tolerance │ │ │ │ │ +90 // LM-specific: │ │ │ │ │ +91 p->lambdaUpperBound = 1e32; │ │ │ │ │ +92 p->lambdaLowerBound = 1e-16; │ │ │ │ │ +93 p->lambdaInitial = 1e-04; │ │ │ │ │ +94 p->lambdaFactor = 2.0; │ │ │ │ │ +95 p->minModelFidelity = 1e-3; // options.min_relative_decrease in CERES │ │ │ │ │ +96 p->diagonalDamping = true; │ │ │ │ │ +97 p->useFixedLambdaFactor = false; // This is important │ │ │ │ │ 98 } │ │ │ │ │ 99 │ │ │ │ │ -101 │ │ │ │ │ -104 const _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s& biasHat() const { return biasHat_; } │ │ │ │ │ -105 double deltaTij() const { return deltaTij_; } │ │ │ │ │ -106 │ │ │ │ │ -107 virtual Vector3 deltaPij() const = 0; │ │ │ │ │ -108 virtual Vector3 deltaVij() const = 0; │ │ │ │ │ -109 virtual Rot3 deltaRij() const = 0; │ │ │ │ │ -110 virtual NavState deltaXij() const = 0; │ │ │ │ │ +100 static LevenbergMarquardtParams LegacyDefaults() { │ │ │ │ │ +101 LevenbergMarquardtParams p; │ │ │ │ │ +102 SetLegacyDefaults(&p); │ │ │ │ │ +103 return p; │ │ │ │ │ +104 } │ │ │ │ │ +105 │ │ │ │ │ +106 static LevenbergMarquardtParams CeresDefaults() { │ │ │ │ │ +107 LevenbergMarquardtParams p; │ │ │ │ │ +108 SetCeresDefaults(&p); │ │ │ │ │ +109 return p; │ │ │ │ │ +110 } │ │ │ │ │ 111 │ │ │ │ │ -112 // Exposed for MATLAB │ │ │ │ │ -113 Vector6 biasHatVector() const { return biasHat_._v_e_c_t_o_r(); } │ │ │ │ │ -115 │ │ │ │ │ -118 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const │ │ │ │ │ -PreintegrationBase& pim); │ │ │ │ │ -119 virtual void _p_r_i_n_t(const std::string& s="") const; │ │ │ │ │ -121 │ │ │ │ │ +112 static LevenbergMarquardtParams EnsureHasOrdering(LevenbergMarquardtParams │ │ │ │ │ +params, │ │ │ │ │ +113 const NonlinearFactorGraph& graph) { │ │ │ │ │ +114 if (!params.ordering) │ │ │ │ │ +115 params.ordering = Ordering::Create(params.orderingType, graph); │ │ │ │ │ +116 return params; │ │ │ │ │ +117 } │ │ │ │ │ +118 │ │ │ │ │ +119 static LevenbergMarquardtParams ReplaceOrdering(LevenbergMarquardtParams │ │ │ │ │ +params, │ │ │ │ │ +120 const Ordering& ordering) { │ │ │ │ │ +121 params.ordering = ordering; │ │ │ │ │ +122 return params; │ │ │ │ │ +123 } │ │ │ │ │ 124 │ │ │ │ │ -130 std::pair correctMeasurementsBySensorPose( │ │ │ │ │ -131 const Vector3& unbiasedAcc, const Vector3& unbiasedOmega, │ │ │ │ │ -132 OptionalJacobian<3, 3> correctedAcc_H_unbiasedAcc = boost::none, │ │ │ │ │ -133 OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none, │ │ │ │ │ -134 OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = boost::none) const; │ │ │ │ │ -135 │ │ │ │ │ -_1_4_1 virtual void _u_p_d_a_t_e(const Vector3& measuredAcc, const Vector3& │ │ │ │ │ -measuredOmega, │ │ │ │ │ -142 const double dt, Matrix9* A, Matrix93* B, Matrix93* C) = 0; │ │ │ │ │ -143 │ │ │ │ │ -145 virtual void integrateMeasurement(const Vector3& measuredAcc, │ │ │ │ │ -146 const Vector3& measuredOmega, const double dt); │ │ │ │ │ -147 │ │ │ │ │ -_1_5_0 virtual Vector9 _b_i_a_s_C_o_r_r_e_c_t_e_d_D_e_l_t_a(const _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s& bias_i, │ │ │ │ │ -151 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _6_> H = boost::none) const = 0; │ │ │ │ │ -152 │ │ │ │ │ -154 _N_a_v_S_t_a_t_e predict(const _N_a_v_S_t_a_t_e& state_i, const _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s& │ │ │ │ │ -bias_i, │ │ │ │ │ -155 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _9_> H1 = boost::none, │ │ │ │ │ -156 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _6_> H2 = boost::none) const; │ │ │ │ │ -157 │ │ │ │ │ -159 Vector9 computeError(const _N_a_v_S_t_a_t_e& state_i, const _N_a_v_S_t_a_t_e& state_j, │ │ │ │ │ -160 const _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s& bias_i, │ │ │ │ │ -161 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _9_> H1, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _9_> H2, │ │ │ │ │ -162 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _6_> H3) const; │ │ │ │ │ -163 │ │ │ │ │ -168 Vector9 computeErrorAndJacobians(const _P_o_s_e_3& pose_i, const Vector3& vel_i, │ │ │ │ │ -169 const _P_o_s_e_3& pose_j, const Vector3& vel_j, │ │ │ │ │ -170 const _i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s& bias_i, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _6_> H1 = │ │ │ │ │ -171 boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _3_> H2 = boost::none, │ │ │ │ │ -172 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _6_> H3 = boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _3_> H4 = │ │ │ │ │ -173 boost::none, _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_9_,_ _6_> H5 = boost::none) const; │ │ │ │ │ -174 │ │ │ │ │ -175 private: │ │ │ │ │ -_1_7_7 friend class boost::serialization::access; │ │ │ │ │ -178 template │ │ │ │ │ -179 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -180 ar & BOOST_SERIALIZATION_NVP(p_); │ │ │ │ │ -181 ar & BOOST_SERIALIZATION_NVP(biasHat_); │ │ │ │ │ -182 ar & BOOST_SERIALIZATION_NVP(deltaTij_); │ │ │ │ │ -183 } │ │ │ │ │ -184 │ │ │ │ │ -185 public: │ │ │ │ │ -186 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -187}; │ │ │ │ │ -188 │ │ │ │ │ -189} │ │ │ │ │ -_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ -This marks a GTSAM object to require alignment. │ │ │ │ │ -DDeeffiinniittiioonn types.h:308 │ │ │ │ │ -_N_o_i_s_e_M_o_d_e_l_._h │ │ │ │ │ -_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s_._h │ │ │ │ │ -_I_m_u_B_i_a_s_._h │ │ │ │ │ -_N_a_v_S_t_a_t_e_._h │ │ │ │ │ -Navigation state composing of attitude, position, and velocity. │ │ │ │ │ +125 ~LevenbergMarquardtParams() override {} │ │ │ │ │ +126 void _p_r_i_n_t(const std::string& str = "") const override; │ │ │ │ │ +127 │ │ │ │ │ +130 bool getDiagonalDamping() const { return diagonalDamping; } │ │ │ │ │ +131 double getlambdaFactor() const { return lambdaFactor; } │ │ │ │ │ +132 double getlambdaInitial() const { return lambdaInitial; } │ │ │ │ │ +133 double getlambdaLowerBound() const { return lambdaLowerBound; } │ │ │ │ │ +134 double getlambdaUpperBound() const { return lambdaUpperBound; } │ │ │ │ │ +135 bool getUseFixedLambdaFactor() { return useFixedLambdaFactor; } │ │ │ │ │ +136 std::string getLogFile() const { return logFile; } │ │ │ │ │ +137 std::string getVerbosityLM() const { return verbosityLMTranslator │ │ │ │ │ +(verbosityLM);} │ │ │ │ │ +138 │ │ │ │ │ +139 void setDiagonalDamping(bool flag) { diagonalDamping = flag; } │ │ │ │ │ +140 void setlambdaFactor(double value) { lambdaFactor = value; } │ │ │ │ │ +141 void setlambdaInitial(double value) { lambdaInitial = value; } │ │ │ │ │ +142 void setlambdaLowerBound(double value) { lambdaLowerBound = value; } │ │ │ │ │ +143 void setlambdaUpperBound(double value) { lambdaUpperBound = value; } │ │ │ │ │ +144 void setUseFixedLambdaFactor(bool flag) { useFixedLambdaFactor = flag;} │ │ │ │ │ +145 void setLogFile(const std::string& s) { logFile = s; } │ │ │ │ │ +146 void setVerbosityLM(const std::string& s) { verbosityLM = │ │ │ │ │ +verbosityLMTranslator(s);} │ │ │ │ │ +147 // @} │ │ │ │ │ +150 │ │ │ │ │ +_1_5_2 boost::shared_ptr _c_l_o_n_e() const { │ │ │ │ │ +153 return boost::shared_ptr(new │ │ │ │ │ +_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s(*this)); │ │ │ │ │ +154 } │ │ │ │ │ +155 │ │ │ │ │ +157}; │ │ │ │ │ +158 │ │ │ │ │ +159} │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_._h │ │ │ │ │ +Parameters for nonlinear optimization. │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Factor Graph consisting of non-linear factors. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ print without optional string, must specify cout yourself │ │ │ │ │ DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ -OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ -either a fixed size o... │ │ │ │ │ -DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ -A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s │ │ │ │ │ -DDeeffiinniittiioonn ImuBias.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_i_m_u_B_i_a_s_:_:_C_o_n_s_t_a_n_t_B_i_a_s_:_:_v_e_c_t_o_r │ │ │ │ │ -Vector6 vector() const │ │ │ │ │ -return the accelerometer and gyro biases in a single vector │ │ │ │ │ -DDeeffiinniittiioonn ImuBias.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_N_a_v_S_t_a_t_e │ │ │ │ │ -Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does │ │ │ │ │ -not make sense to make... │ │ │ │ │ -DDeeffiinniittiioonn NavState.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e │ │ │ │ │ -PreintegrationBase is the base class for PreintegratedMeasurements (in │ │ │ │ │ -ImuFactor) and CombinedPreinte... │ │ │ │ │ -DDeeffiinniittiioonn PreintegrationBase.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_:_:_d_e_l_t_a_T_i_j__ │ │ │ │ │ -double deltaTij_ │ │ │ │ │ -Time interval from i to j. │ │ │ │ │ -DDeeffiinniittiioonn PreintegrationBase.h:53 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_:_:_~_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e │ │ │ │ │ -virtual ~PreintegrationBase() │ │ │ │ │ -Virtual destructor for serialization. │ │ │ │ │ -DDeeffiinniittiioonn PreintegrationBase.h:59 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_:_:_m_a_t_c_h_e_s_P_a_r_a_m_s_W_i_t_h │ │ │ │ │ -bool matchesParamsWith(const PreintegrationBase &other) const │ │ │ │ │ -check parameters equality: checks whether shared pointer points to same Params │ │ │ │ │ -object. │ │ │ │ │ -DDeeffiinniittiioonn PreintegrationBase.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_:_:_b_i_a_s_C_o_r_r_e_c_t_e_d_D_e_l_t_a │ │ │ │ │ -virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias &bias_i, │ │ │ │ │ -OptionalJacobian< 9, 6 > H=boost::none) const =0 │ │ │ │ │ -Given the estimate of the bias, return a NavState tangent vector summarizing │ │ │ │ │ -the preintegrated IMU me... │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_:_:_p │ │ │ │ │ -Params & p() const │ │ │ │ │ -const reference to params │ │ │ │ │ -DDeeffiinniittiioonn PreintegrationBase.h:96 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_:_:_b_i_a_s_H_a_t__ │ │ │ │ │ -Bias biasHat_ │ │ │ │ │ -Acceleration and gyro bias used for preintegration. │ │ │ │ │ -DDeeffiinniittiioonn PreintegrationBase.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e │ │ │ │ │ -PreintegrationBase() │ │ │ │ │ -Default constructor for serialization. │ │ │ │ │ -DDeeffiinniittiioonn PreintegrationBase.h:56 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_:_:_p_a_r_a_m_s │ │ │ │ │ -const boost::shared_ptr< Params > & params() const │ │ │ │ │ -shared pointer to params │ │ │ │ │ -DDeeffiinniittiioonn PreintegrationBase.h:91 │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_:_:_u_p_d_a_t_e │ │ │ │ │ -virtual void update(const Vector3 &measuredAcc, const Vector3 &measuredOmega, │ │ │ │ │ -const double dt, Matrix9 *A, Matrix93 *B, Matrix93 *C)=0 │ │ │ │ │ -Update preintegrated measurements and get derivatives It takes measured │ │ │ │ │ -quantities in the j frame Mod... │ │ │ │ │ -_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ -Parameters for pre-integration: Usage: Create just a single Params and pass a │ │ │ │ │ -shared pointer to the c... │ │ │ │ │ -DDeeffiinniittiioonn PreintegrationParams.h:26 │ │ │ │ │ +_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r │ │ │ │ │ +This class performs Levenberg-Marquardt nonlinear optimization. │ │ │ │ │ +DDeeffiinniittiioonn LevenbergMarquardtOptimizer.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s │ │ │ │ │ +Parameters for Levenberg-Marquardt optimization. │ │ │ │ │ +DDeeffiinniittiioonn LevenbergMarquardtParams.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_:_:_l_a_m_b_d_a_F_a_c_t_o_r │ │ │ │ │ +double lambdaFactor │ │ │ │ │ +The amount by which to multiply or divide lambda when adjusting lambda │ │ │ │ │ +(default: 10.... │ │ │ │ │ +DDeeffiinniittiioonn LevenbergMarquardtParams.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_:_:_m_i_n_D_i_a_g_o_n_a_l │ │ │ │ │ +double minDiagonal │ │ │ │ │ +when using diagonal damping saturates the minimum diagonal entries (default: │ │ │ │ │ +1e-6) │ │ │ │ │ +DDeeffiinniittiioonn LevenbergMarquardtParams.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_:_:_l_a_m_b_d_a_U_p_p_e_r_B_o_u_n_d │ │ │ │ │ +double lambdaUpperBound │ │ │ │ │ +The maximum lambda to try before assuming the optimization has failed (default: │ │ │ │ │ +1e5) │ │ │ │ │ +DDeeffiinniittiioonn LevenbergMarquardtParams.h:51 │ │ │ │ │ +_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_:_:_l_a_m_b_d_a_I_n_i_t_i_a_l │ │ │ │ │ +double lambdaInitial │ │ │ │ │ +The initial Levenberg-Marquardt damping term (default: 1e-5) │ │ │ │ │ +DDeeffiinniittiioonn LevenbergMarquardtParams.h:49 │ │ │ │ │ +_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_:_:_m_a_x_D_i_a_g_o_n_a_l │ │ │ │ │ +double maxDiagonal │ │ │ │ │ +when using diagonal damping saturates the maximum diagonal entries (default: │ │ │ │ │ +1e32) │ │ │ │ │ +DDeeffiinniittiioonn LevenbergMarquardtParams.h:59 │ │ │ │ │ +_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_:_:_m_i_n_M_o_d_e_l_F_i_d_e_l_i_t_y │ │ │ │ │ +double minModelFidelity │ │ │ │ │ +Lower bound for the modelFidelity to accept the result of an LM iteration. │ │ │ │ │ +DDeeffiinniittiioonn LevenbergMarquardtParams.h:54 │ │ │ │ │ +_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_:_:_l_a_m_b_d_a_L_o_w_e_r_B_o_u_n_d │ │ │ │ │ +double lambdaLowerBound │ │ │ │ │ +The minimum lambda used in LM (default: 0) │ │ │ │ │ +DDeeffiinniittiioonn LevenbergMarquardtParams.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_:_:_c_l_o_n_e │ │ │ │ │ +boost::shared_ptr< NonlinearOptimizerParams > clone() const │ │ │ │ │ +DDeeffiinniittiioonn LevenbergMarquardtParams.h:152 │ │ │ │ │ +_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_:_:_d_i_a_g_o_n_a_l_D_a_m_p_i_n_g │ │ │ │ │ +bool diagonalDamping │ │ │ │ │ +if true, use diagonal of Hessian │ │ │ │ │ +DDeeffiinniittiioonn LevenbergMarquardtParams.h:56 │ │ │ │ │ +_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_:_:_u_s_e_F_i_x_e_d_L_a_m_b_d_a_F_a_c_t_o_r │ │ │ │ │ +bool useFixedLambdaFactor │ │ │ │ │ +if true applies constant increase (or decrease) to lambda according to │ │ │ │ │ +lambdaFactor │ │ │ │ │ +DDeeffiinniittiioonn LevenbergMarquardtParams.h:57 │ │ │ │ │ +_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_:_:_l_o_g_F_i_l_e │ │ │ │ │ +std::string logFile │ │ │ │ │ +an optional CSV log file, with [iteration, time, error, lambda] │ │ │ │ │ +DDeeffiinniittiioonn LevenbergMarquardtParams.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_:_:_V_e_r_b_o_s_i_t_y_L_M │ │ │ │ │ +VerbosityLM │ │ │ │ │ +See LevenbergMarquardtParams::verbosityLM. │ │ │ │ │ +DDeeffiinniittiioonn LevenbergMarquardtParams.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_:_:_v_e_r_b_o_s_i_t_y_L_M │ │ │ │ │ +VerbosityLM verbosityLM │ │ │ │ │ +The verbosity level for Levenberg-Marquardt (default: SILENT), see also │ │ │ │ │ +NonlinearOptimizerParams::ver... │ │ │ │ │ +DDeeffiinniittiioonn LevenbergMarquardtParams.h:53 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s │ │ │ │ │ +The common parameters for Nonlinear optimizers. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizerParams.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_a_b_s_o_l_u_t_e_E_r_r_o_r_T_o_l │ │ │ │ │ +double absoluteErrorTol │ │ │ │ │ +The maximum absolute error decrease to stop iterating (default 1e-5) │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizerParams.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_m_a_x_I_t_e_r_a_t_i_o_n_s │ │ │ │ │ +size_t maxIterations │ │ │ │ │ +The maximum iterations to stop iterating (default 100) │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizerParams.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_r_e_l_a_t_i_v_e_E_r_r_o_r_T_o_l │ │ │ │ │ +double relativeErrorTol │ │ │ │ │ +The maximum relative error decrease to stop iterating (default 1e-5) │ │ │ │ │ +DDeeffiinniittiioonn NonlinearOptimizerParams.h:42 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ - * _P_r_e_i_n_t_e_g_r_a_t_i_o_n_B_a_s_e_._h │ │ │ │ │ + * _n_o_n_l_i_n_e_a_r │ │ │ │ │ + * _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01013_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00410_source.html │ │ │ │┄ Files 94% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ExtendedKalmanFilter.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/Cyclic.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ExtendedKalmanFilter.h
│ │ │ │ +
Cyclic.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19// \callgraph
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ -
23#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
24
│ │ │ │ -
25namespace gtsam {
│ │ │ │ -
26
│ │ │ │ -
44template <class VALUE>
│ │ │ │ -
│ │ │ │ -
45class ExtendedKalmanFilter {
│ │ │ │ -
46 // Check that VALUE type is a testable Manifold
│ │ │ │ -
47 BOOST_CONCEPT_ASSERT((IsTestable<VALUE>));
│ │ │ │ -
48 BOOST_CONCEPT_ASSERT((IsManifold<VALUE>));
│ │ │ │ -
49
│ │ │ │ -
50 public:
│ │ │ │ -
51 typedef boost::shared_ptr<ExtendedKalmanFilter<VALUE> > shared_ptr;
│ │ │ │ -
52 typedef VALUE T;
│ │ │ │ -
53
│ │ │ │ -
54#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ -
55 //@deprecated: any NoiseModelFactor will do, as long as they have the right keys
│ │ │ │ -
56 typedef NoiseModelFactorN<VALUE, VALUE> MotionFactor;
│ │ │ │ -
57 typedef NoiseModelFactorN<VALUE> MeasurementFactor;
│ │ │ │ -
58#endif
│ │ │ │ -
59
│ │ │ │ -
60 protected:
│ │ │ │ -
61 T x_; // linearization point
│ │ │ │ -
62 JacobianFactor::shared_ptr priorFactor_; // Gaussian density on x_
│ │ │ │ -
63
│ │ │ │ -
64 static T solve_(const GaussianFactorGraph& linearFactorGraph, const Values& linearizationPoints,
│ │ │ │ -
65 Key x, JacobianFactor::shared_ptr* newPrior);
│ │ │ │ -
66
│ │ │ │ -
67 public:
│ │ │ │ -
70
│ │ │ │ -
71 ExtendedKalmanFilter(Key key_initial, T x_initial, noiseModel::Gaussian::shared_ptr P_initial);
│ │ │ │ -
72
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/base/Group.h>
│ │ │ │ +
21#include <gtsam/base/Testable.h>
│ │ │ │ +
22
│ │ │ │ +
23#include <cassert>
│ │ │ │ +
24#include <iostream> // for cout :-(
│ │ │ │ +
25
│ │ │ │ +
26namespace gtsam {
│ │ │ │ +
27
│ │ │ │ +
29template<size_t N>
│ │ │ │ +
│ │ │ │ +
30class Cyclic {
│ │ │ │ +
31 size_t i_;
│ │ │ │ +
32public:
│ │ │ │ +
│ │ │ │ +
34 Cyclic(size_t i) :
│ │ │ │ +
35 i_(i) {
│ │ │ │ +
36 assert(i < N);
│ │ │ │ +
37 }
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
39 Cyclic():i_(0) {
│ │ │ │ +
40 }
│ │ │ │ +
│ │ │ │ +
41 static Cyclic Identity() { return Cyclic();}
│ │ │ │ +
42
│ │ │ │ +
│ │ │ │ +
44 operator size_t() const {
│ │ │ │ +
45 return i_;
│ │ │ │ +
46 }
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
48 Cyclic operator+(const Cyclic& h) const {
│ │ │ │ +
49 return (i_ + h.i_) % N;
│ │ │ │ +
50 }
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
52 Cyclic operator-(const Cyclic& h) const {
│ │ │ │ +
53 return (N + i_ - h.i_) % N;
│ │ │ │ +
54 }
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
56 Cyclic operator-() const {
│ │ │ │ +
57 return (N - i_) % N;
│ │ │ │ +
58 }
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
60 void print(const std::string& s = "") const {
│ │ │ │ +
61 std::cout << s << i_ << std::endl;
│ │ │ │ +
62 }
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
64 bool equals(const Cyclic& other, double tol = 1e-9) const {
│ │ │ │ +
65 return other.i_ == i_;
│ │ │ │ +
66 }
│ │ │ │ +
│ │ │ │ +
67};
│ │ │ │ +
│ │ │ │ +
68
│ │ │ │ +
70template<size_t N>
│ │ │ │ +
│ │ │ │ +
71struct traits<Cyclic<N> > : internal::AdditiveGroupTraits<Cyclic<N> >, //
│ │ │ │ +
72 Testable<Cyclic<N> > {
│ │ │ │ +
73};
│ │ │ │ +
│ │ │ │ +
74
│ │ │ │ +
75} // \namespace gtsam
│ │ │ │
76
│ │ │ │ -
│ │ │ │ -
78 void print(const std::string& s = "") const {
│ │ │ │ -
79 std::cout << s << "\n";
│ │ │ │ -
80 x_.print(s + "x");
│ │ │ │ -
81 priorFactor_->print(s + "density");
│ │ │ │ -
82 }
│ │ │ │ -
│ │ │ │ -
83
│ │ │ │ -
87
│ │ │ │ -
93 T predict(const NoiseModelFactor& motionFactor);
│ │ │ │ -
94
│ │ │ │ -
99 T update(const NoiseModelFactor& measurementFactor);
│ │ │ │ -
100
│ │ │ │ -
│ │ │ │ -
102 const JacobianFactor::shared_ptr Density() const {
│ │ │ │ -
103 return priorFactor_;
│ │ │ │ -
104 }
│ │ │ │ -
│ │ │ │ -
105
│ │ │ │ -
107};
│ │ │ │ -
│ │ │ │ -
108
│ │ │ │ -
109} // namespace
│ │ │ │ -
110
│ │ │ │ -
111#include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>
│ │ │ │ -
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │ -
ExtendedKalmanFilter-inl.h
Class to perform generic Kalman Filtering using nonlinear factor graphs.
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
Group.h
Concept check class for variable types with Group properties.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::IsTestable
A testable concept check that should be placed in applicable unit tests and in generic algorithms.
Definition Testable.h:58
│ │ │ │ -
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ -
gtsam::JacobianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition JacobianFactor.h:96
│ │ │ │ -
gtsam::ExtendedKalmanFilter
This is a generic Extended Kalman Filter class implemented using nonlinear factors.
Definition ExtendedKalmanFilter.h:45
│ │ │ │ -
gtsam::ExtendedKalmanFilter::update
T update(const NoiseModelFactor &measurementFactor)
Calculate posterior density P(x_) ~ L(z|x) P(x) The likelihood L(z|x) should be given as a unary fact...
Definition ExtendedKalmanFilter-inl.h:105
│ │ │ │ -
gtsam::ExtendedKalmanFilter::Density
const JacobianFactor::shared_ptr Density() const
Return current predictive (if called after predict)/posterior (if called after update)
Definition ExtendedKalmanFilter.h:102
│ │ │ │ -
gtsam::ExtendedKalmanFilter::predict
T predict(const NoiseModelFactor &motionFactor)
Calculate predictive density The motion model should be given as a factor with key1 for and key2 fo...
Definition ExtendedKalmanFilter-inl.h:80
│ │ │ │ -
gtsam::ExtendedKalmanFilter::print
void print(const std::string &s="") const
print
Definition ExtendedKalmanFilter.h:78
│ │ │ │ -
gtsam::NoiseModelFactor
A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
Definition NonlinearFactor.h:174
│ │ │ │ -
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ -
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::Cyclic
Cyclic group of order N.
Definition Cyclic.h:30
│ │ │ │ +
gtsam::Cyclic::equals
bool equals(const Cyclic &other, double tol=1e-9) const
equals with an tolerance, prints out message if unequal
Definition Cyclic.h:64
│ │ │ │ +
gtsam::Cyclic::print
void print(const std::string &s="") const
print with optional string
Definition Cyclic.h:60
│ │ │ │ +
gtsam::Cyclic::Cyclic
Cyclic()
Default constructor yields identity.
Definition Cyclic.h:39
│ │ │ │ +
gtsam::Cyclic::Cyclic
Cyclic(size_t i)
Constructor.
Definition Cyclic.h:34
│ │ │ │ +
gtsam::Cyclic::operator-
Cyclic operator-(const Cyclic &h) const
Subtraction modulo N.
Definition Cyclic.h:52
│ │ │ │ +
gtsam::Cyclic::operator+
Cyclic operator+(const Cyclic &h) const
Addition modulo N.
Definition Cyclic.h:48
│ │ │ │ +
gtsam::Cyclic::operator-
Cyclic operator-() const
Inverse.
Definition Cyclic.h:56
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,141 +1,118 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ExtendedKalmanFilter.h │ │ │ │ │ +Cyclic.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19// \callgraph │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -24 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ -26 │ │ │ │ │ -44template │ │ │ │ │ -_4_5class _E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r { │ │ │ │ │ -46 // Check that VALUE type is a testable Manifold │ │ │ │ │ -47 BOOST_CONCEPT_ASSERT((_I_s_T_e_s_t_a_b_l_e_<_V_A_L_U_E_>)); │ │ │ │ │ -48 BOOST_CONCEPT_ASSERT((IsManifold)); │ │ │ │ │ -49 │ │ │ │ │ -50 public: │ │ │ │ │ -51 typedef boost::shared_ptr > shared_ptr; │ │ │ │ │ -52 typedef VALUE T; │ │ │ │ │ -53 │ │ │ │ │ -54#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ -55 //@deprecated: any NoiseModelFactor will do, as long as they have the right │ │ │ │ │ -keys │ │ │ │ │ -56 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_V_A_L_U_E_,_ _V_A_L_U_E_> MotionFactor; │ │ │ │ │ -57 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_V_A_L_U_E_> MeasurementFactor; │ │ │ │ │ -58#endif │ │ │ │ │ -59 │ │ │ │ │ -60 protected: │ │ │ │ │ -61 T x_; // linearization point │ │ │ │ │ -62 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r priorFactor_; // Gaussian density on x_ │ │ │ │ │ -63 │ │ │ │ │ -64 static T solve_(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& linearFactorGraph, const _V_a_l_u_e_s& │ │ │ │ │ -linearizationPoints, │ │ │ │ │ -65 _K_e_y x, _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r* newPrior); │ │ │ │ │ -66 │ │ │ │ │ -67 public: │ │ │ │ │ -70 │ │ │ │ │ -71 _E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r(_K_e_y key_initial, T x_initial, noiseModel::Gaussian:: │ │ │ │ │ -shared_ptr P_initial); │ │ │ │ │ -72 │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_b_a_s_e_/_G_r_o_u_p_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +22 │ │ │ │ │ +23#include │ │ │ │ │ +24#include // for cout :-( │ │ │ │ │ +25 │ │ │ │ │ +26namespace _g_t_s_a_m { │ │ │ │ │ +27 │ │ │ │ │ +29template │ │ │ │ │ +_3_0class _C_y_c_l_i_c { │ │ │ │ │ +31 size_t i_; │ │ │ │ │ +32public: │ │ │ │ │ +_3_4 _C_y_c_l_i_c(size_t i) : │ │ │ │ │ +35 i_(i) { │ │ │ │ │ +36 assert(i < N); │ │ │ │ │ +37 } │ │ │ │ │ +_3_9 _C_y_c_l_i_c():i_(0) { │ │ │ │ │ +40 } │ │ │ │ │ +41 static _C_y_c_l_i_c Identity() { return _C_y_c_l_i_c();} │ │ │ │ │ +42 │ │ │ │ │ +_4_4 operator size_t() const { │ │ │ │ │ +45 return i_; │ │ │ │ │ +46 } │ │ │ │ │ +_4_8 _C_y_c_l_i_c _o_p_e_r_a_t_o_r_+(const _C_y_c_l_i_c& h) const { │ │ │ │ │ +49 return (i_ + h.i_) % N; │ │ │ │ │ +50 } │ │ │ │ │ +_5_2 _C_y_c_l_i_c _o_p_e_r_a_t_o_r_-(const _C_y_c_l_i_c& h) const { │ │ │ │ │ +53 return (N + i_ - h.i_) % N; │ │ │ │ │ +54 } │ │ │ │ │ +_5_6 _C_y_c_l_i_c _o_p_e_r_a_t_o_r_-() const { │ │ │ │ │ +57 return (N - i_) % N; │ │ │ │ │ +58 } │ │ │ │ │ +_6_0 void _p_r_i_n_t(const std::string& s = "") const { │ │ │ │ │ +61 std::cout << s << i_ << std::endl; │ │ │ │ │ +62 } │ │ │ │ │ +_6_4 bool _e_q_u_a_l_s(const _C_y_c_l_i_c& other, double tol = 1e-9) const { │ │ │ │ │ +65 return other.i_ == i_; │ │ │ │ │ +66 } │ │ │ │ │ +67}; │ │ │ │ │ +68 │ │ │ │ │ +70template │ │ │ │ │ +_7_1struct _t_r_a_i_t_s<_C_y_c_l_i_c > : internal::AdditiveGroupTraits >, // │ │ │ │ │ +72 _T_e_s_t_a_b_l_e > { │ │ │ │ │ +73}; │ │ │ │ │ +74 │ │ │ │ │ +75} // \namespace gtsam │ │ │ │ │ 76 │ │ │ │ │ -_7_8 void _p_r_i_n_t(const std::string& s = "") const { │ │ │ │ │ -79 std::cout << s << "\n"; │ │ │ │ │ -80 x_.print(s + "x"); │ │ │ │ │ -81 priorFactor_->print(s + "density"); │ │ │ │ │ -82 } │ │ │ │ │ -83 │ │ │ │ │ -87 │ │ │ │ │ -93 T _p_r_e_d_i_c_t(const _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r& motionFactor); │ │ │ │ │ -94 │ │ │ │ │ -99 T _u_p_d_a_t_e(const _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r& measurementFactor); │ │ │ │ │ -100 │ │ │ │ │ -_1_0_2 const _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r _D_e_n_s_i_t_y() const { │ │ │ │ │ -103 return priorFactor_; │ │ │ │ │ -104 } │ │ │ │ │ -105 │ │ │ │ │ -107}; │ │ │ │ │ -108 │ │ │ │ │ -109} // namespace │ │ │ │ │ -110 │ │ │ │ │ -111#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_-_i_n_l_._h> │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Factor Graph consisting of non-linear factors. │ │ │ │ │ -_E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_-_i_n_l_._h │ │ │ │ │ -Class to perform generic Kalman Filtering using nonlinear factor graphs. │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ -Non-linear factor base classes. │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ +_G_r_o_u_p_._h │ │ │ │ │ +Concept check class for variable types with Group properties. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_I_s_T_e_s_t_a_b_l_e │ │ │ │ │ -A testable concept check that should be placed in applicable unit tests and in │ │ │ │ │ -generic algorithms. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:96 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r │ │ │ │ │ -This is a generic Extended Kalman Filter class implemented using nonlinear │ │ │ │ │ -factors. │ │ │ │ │ -DDeeffiinniittiioonn ExtendedKalmanFilter.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_:_:_u_p_d_a_t_e │ │ │ │ │ -T update(const NoiseModelFactor &measurementFactor) │ │ │ │ │ -Calculate posterior density P(x_) ~ L(z|x) P(x) The likelihood L(z|x) should be │ │ │ │ │ -given as a unary fact... │ │ │ │ │ -DDeeffiinniittiioonn ExtendedKalmanFilter-inl.h:105 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_:_:_D_e_n_s_i_t_y │ │ │ │ │ -const JacobianFactor::shared_ptr Density() const │ │ │ │ │ -Return current predictive (if called after predict)/posterior (if called after │ │ │ │ │ -update) │ │ │ │ │ -DDeeffiinniittiioonn ExtendedKalmanFilter.h:102 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_:_:_p_r_e_d_i_c_t │ │ │ │ │ -T predict(const NoiseModelFactor &motionFactor) │ │ │ │ │ -Calculate predictive density The motion model should be given as a factor with │ │ │ │ │ -key1 for and key2 fo... │ │ │ │ │ -DDeeffiinniittiioonn ExtendedKalmanFilter-inl.h:80 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_:_:_p_r_i_n_t │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_C_y_c_l_i_c │ │ │ │ │ +Cyclic group of order N. │ │ │ │ │ +DDeeffiinniittiioonn Cyclic.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_C_y_c_l_i_c_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const Cyclic &other, double tol=1e-9) const │ │ │ │ │ +equals with an tolerance, prints out message if unequal │ │ │ │ │ +DDeeffiinniittiioonn Cyclic.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_C_y_c_l_i_c_:_:_p_r_i_n_t │ │ │ │ │ void print(const std::string &s="") const │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn ExtendedKalmanFilter.h:78 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r │ │ │ │ │ -A nonlinear sum-of-squares factor with a zero-mean noise model implementing the │ │ │ │ │ -density Templated on... │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:174 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ -A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ -variables. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +print with optional string │ │ │ │ │ +DDeeffiinniittiioonn Cyclic.h:60 │ │ │ │ │ +_g_t_s_a_m_:_:_C_y_c_l_i_c_:_:_C_y_c_l_i_c │ │ │ │ │ +Cyclic() │ │ │ │ │ +Default constructor yields identity. │ │ │ │ │ +DDeeffiinniittiioonn Cyclic.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_C_y_c_l_i_c_:_:_C_y_c_l_i_c │ │ │ │ │ +Cyclic(size_t i) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn Cyclic.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_C_y_c_l_i_c_:_:_o_p_e_r_a_t_o_r_- │ │ │ │ │ +Cyclic operator-(const Cyclic &h) const │ │ │ │ │ +Subtraction modulo N. │ │ │ │ │ +DDeeffiinniittiioonn Cyclic.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_C_y_c_l_i_c_:_:_o_p_e_r_a_t_o_r_+ │ │ │ │ │ +Cyclic operator+(const Cyclic &h) const │ │ │ │ │ +Addition modulo N. │ │ │ │ │ +DDeeffiinniittiioonn Cyclic.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_C_y_c_l_i_c_:_:_o_p_e_r_a_t_o_r_- │ │ │ │ │ +Cyclic operator-() const │ │ │ │ │ +Inverse. │ │ │ │ │ +DDeeffiinniittiioonn Cyclic.h:56 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _E_x_t_e_n_d_e_d_K_a_l_m_a_n_F_i_l_t_e_r_._h │ │ │ │ │ + * _g_e_o_m_e_t_r_y │ │ │ │ │ + * _C_y_c_l_i_c_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01019_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01151_source.html │ │ │ │┄ Files 94% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearOptimizer.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/LevenbergMarquardtOptimizer.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
NonlinearOptimizer.h
│ │ │ │ +
LevenbergMarquardtOptimizer.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ -
22#include <gtsam/nonlinear/NonlinearOptimizerParams.h>
│ │ │ │ -
23
│ │ │ │ -
24namespace gtsam {
│ │ │ │ -
25
│ │ │ │ -
26namespace internal { struct NonlinearOptimizerState; }
│ │ │ │ +
21#pragma once
│ │ │ │ +
22
│ │ │ │ +
23#include <gtsam/nonlinear/NonlinearOptimizer.h>
│ │ │ │ +
24#include <gtsam/nonlinear/LevenbergMarquardtParams.h>
│ │ │ │ +
25#include <gtsam/linear/VectorValues.h>
│ │ │ │ +
26#include <boost/date_time/posix_time/posix_time.hpp>
│ │ │ │
27
│ │ │ │ -
│ │ │ │ -
75class GTSAM_EXPORT NonlinearOptimizer {
│ │ │ │ +
28class NonlinearOptimizerMoreOptimizationTest;
│ │ │ │ +
29
│ │ │ │ +
30namespace gtsam {
│ │ │ │ +
31
│ │ │ │ +
│ │ │ │ +
35class GTSAM_EXPORT LevenbergMarquardtOptimizer: public NonlinearOptimizer {
│ │ │ │ +
36
│ │ │ │ +
37protected:
│ │ │ │ +
38 const LevenbergMarquardtParams params_;
│ │ │ │ +
39 boost::posix_time::ptime startTime_;
│ │ │ │ +
40
│ │ │ │ +
41 void initTime();
│ │ │ │ +
42
│ │ │ │ +
43public:
│ │ │ │ +
44 typedef boost::shared_ptr<LevenbergMarquardtOptimizer> shared_ptr;
│ │ │ │ +
45
│ │ │ │ +
48
│ │ │ │ +
57 LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
│ │ │ │ +
58 const LevenbergMarquardtParams& params = LevenbergMarquardtParams());
│ │ │ │ +
59
│ │ │ │ +
67 LevenbergMarquardtOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
│ │ │ │ +
68 const Ordering& ordering,
│ │ │ │ +
69 const LevenbergMarquardtParams& params = LevenbergMarquardtParams());
│ │ │ │ +
70
│ │ │ │ +
│ │ │ │ +
72 ~LevenbergMarquardtOptimizer() override {
│ │ │ │ +
73 }
│ │ │ │ +
│ │ │ │ +
74
│ │ │ │
76
│ │ │ │ -
77protected:
│ │ │ │ -
78 NonlinearFactorGraph graph_;
│ │ │ │
79
│ │ │ │ -
80 std::unique_ptr<internal::NonlinearOptimizerState> state_;
│ │ │ │ -
81
│ │ │ │ -
82public:
│ │ │ │ -
84 using shared_ptr = boost::shared_ptr<const NonlinearOptimizer>;
│ │ │ │ +
81 double lambda() const;
│ │ │ │ +
82
│ │ │ │ +
84 int getInnerIterations() const;
│ │ │ │
85
│ │ │ │ -
88
│ │ │ │ -
98 virtual const Values& optimize() { defaultOptimize(); return values(); }
│ │ │ │ -
99
│ │ │ │ -
106 const Values& optimizeSafely();
│ │ │ │ +
│ │ │ │ +
87 void print(const std::string& str = "") const {
│ │ │ │ +
88 std::cout << str << "LevenbergMarquardtOptimizer" << std::endl;
│ │ │ │ +
89 this->params_.print(" parameters:\n");
│ │ │ │ +
90 }
│ │ │ │ +
│ │ │ │ +
91
│ │ │ │ +
93
│ │ │ │ +
96
│ │ │ │ +
101 GaussianFactorGraph::shared_ptr iterate() override;
│ │ │ │ +
102
│ │ │ │ +
│ │ │ │ +
104 const LevenbergMarquardtParams& params() const {
│ │ │ │ +
105 return params_;
│ │ │ │ +
106 }
│ │ │ │ +
│ │ │ │
107
│ │ │ │ -
109 double error() const;
│ │ │ │ -
110
│ │ │ │ -
112 size_t iterations() const;
│ │ │ │ -
113
│ │ │ │ -
115 const Values &values() const;
│ │ │ │ +
108 void writeLogFile(double currentError);
│ │ │ │ +
109
│ │ │ │ +
111 virtual GaussianFactorGraph::shared_ptr linearize() const;
│ │ │ │ +
112
│ │ │ │ +
114 GaussianFactorGraph buildDampedSystem(const GaussianFactorGraph& linear,
│ │ │ │ +
115 const VectorValues& sqrtHessianDiagonal) const;
│ │ │ │
116
│ │ │ │ -
118 const NonlinearFactorGraph &graph() const { return graph_; }
│ │ │ │ +
118 bool tryLambda(const GaussianFactorGraph& linear, const VectorValues& sqrtHessianDiagonal);
│ │ │ │
119
│ │ │ │
121
│ │ │ │ -
124
│ │ │ │ -
126 virtual ~NonlinearOptimizer();
│ │ │ │ -
127
│ │ │ │ -
129 virtual VectorValues solve(const GaussianFactorGraph &gfg,
│ │ │ │ -
130 const NonlinearOptimizerParams& params) const;
│ │ │ │ -
131
│ │ │ │ -
136 virtual GaussianFactorGraph::shared_ptr iterate() = 0;
│ │ │ │ -
137
│ │ │ │ -
139
│ │ │ │ -
140protected:
│ │ │ │ -
144 void defaultOptimize();
│ │ │ │ -
145
│ │ │ │ -
146 virtual const NonlinearOptimizerParams& _params() const = 0;
│ │ │ │ -
147
│ │ │ │ -
149 NonlinearOptimizer(const NonlinearFactorGraph& graph,
│ │ │ │ -
150 std::unique_ptr<internal::NonlinearOptimizerState> state);
│ │ │ │ -
151};
│ │ │ │ -
│ │ │ │ -
152
│ │ │ │ -
157GTSAM_EXPORT bool checkConvergence(double relativeErrorTreshold,
│ │ │ │ -
158 double absoluteErrorTreshold, double errorThreshold,
│ │ │ │ -
159 double currentError, double newError, NonlinearOptimizerParams::Verbosity verbosity = NonlinearOptimizerParams::SILENT);
│ │ │ │ -
160
│ │ │ │ -
161GTSAM_EXPORT bool checkConvergence(const NonlinearOptimizerParams& params, double currentError,
│ │ │ │ -
162 double newError);
│ │ │ │ -
163
│ │ │ │ -
164} // gtsam
│ │ │ │ -
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │ -
NonlinearOptimizerParams.h
Parameters for nonlinear optimization.
│ │ │ │ +
122protected:
│ │ │ │ +
123
│ │ │ │ +
│ │ │ │ +
125 const NonlinearOptimizerParams& _params() const override {
│ │ │ │ +
126 return params_;
│ │ │ │ +
127 }
│ │ │ │ +
│ │ │ │ +
128};
│ │ │ │ +
│ │ │ │ +
129
│ │ │ │ +
130}
│ │ │ │ +
VectorValues.h
Factor Graph Values.
│ │ │ │ +
LevenbergMarquardtParams.h
Parameters for Levenberg-Marquardt trust-region scheme.
│ │ │ │ +
NonlinearOptimizer.h
Base class and parameters for nonlinear optimization algorithms.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::checkConvergence
bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold, double errorThreshold, double currentError, double newError, NonlinearOptimizerParams::Verbosity verbosity)
Check whether the relative error decrease is less than relativeErrorTreshold, the absolute error decr...
Definition NonlinearOptimizer.cpp:185
│ │ │ │ +
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │
gtsam::GaussianFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactorGraph.h:81
│ │ │ │
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
gtsam::LevenbergMarquardtOptimizer
This class performs Levenberg-Marquardt nonlinear optimization.
Definition LevenbergMarquardtOptimizer.h:35
│ │ │ │ +
gtsam::LevenbergMarquardtOptimizer::~LevenbergMarquardtOptimizer
~LevenbergMarquardtOptimizer() override
Virtual destructor.
Definition LevenbergMarquardtOptimizer.h:72
│ │ │ │ +
gtsam::LevenbergMarquardtOptimizer::params
const LevenbergMarquardtParams & params() const
Read-only access the parameters.
Definition LevenbergMarquardtOptimizer.h:104
│ │ │ │ +
gtsam::LevenbergMarquardtOptimizer::_params
const NonlinearOptimizerParams & _params() const override
Access the parameters (base class version)
Definition LevenbergMarquardtOptimizer.h:125
│ │ │ │ +
gtsam::LevenbergMarquardtOptimizer::params_
const LevenbergMarquardtParams params_
LM parameters.
Definition LevenbergMarquardtOptimizer.h:38
│ │ │ │ +
gtsam::LevenbergMarquardtOptimizer::print
void print(const std::string &str="") const
print
Definition LevenbergMarquardtOptimizer.h:87
│ │ │ │ +
gtsam::LevenbergMarquardtParams
Parameters for Levenberg-Marquardt optimization.
Definition LevenbergMarquardtParams.h:35
│ │ │ │
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │
gtsam::NonlinearOptimizer
This is the abstract interface for classes that can optimize for the maximum-likelihood estimate of a...
Definition NonlinearOptimizer.h:75
│ │ │ │ -
gtsam::NonlinearOptimizer::graph
const NonlinearFactorGraph & graph() const
return the graph with nonlinear factors
Definition NonlinearOptimizer.h:118
│ │ │ │ -
gtsam::NonlinearOptimizer::graph_
NonlinearFactorGraph graph_
The graph with nonlinear factors.
Definition NonlinearOptimizer.h:78
│ │ │ │ -
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Optimize for the maximum-likelihood estimate, returning a the optimized variable assignments.
Definition NonlinearOptimizer.h:98
│ │ │ │ -
gtsam::NonlinearOptimizer::state_
std::unique_ptr< internal::NonlinearOptimizerState > state_
PIMPL'd state.
Definition NonlinearOptimizer.h:80
│ │ │ │ -
gtsam::NonlinearOptimizer::shared_ptr
boost::shared_ptr< const NonlinearOptimizer > shared_ptr
A shared pointer to this class.
Definition NonlinearOptimizer.h:84
│ │ │ │ -
gtsam::NonlinearOptimizer::iterate
virtual GaussianFactorGraph::shared_ptr iterate()=0
Perform a single iteration, returning GaussianFactorGraph corresponding to the linearized factor grap...
│ │ │ │
gtsam::NonlinearOptimizerParams
The common parameters for Nonlinear optimizers.
Definition NonlinearOptimizerParams.h:34
│ │ │ │ -
gtsam::NonlinearOptimizerParams::Verbosity
Verbosity
See NonlinearOptimizerParams::verbosity.
Definition NonlinearOptimizerParams.h:37
│ │ │ │
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,151 +1,157 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -NonlinearOptimizer.h │ │ │ │ │ +LevenbergMarquardtOptimizer.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_._h> │ │ │ │ │ -23 │ │ │ │ │ -24namespace _g_t_s_a_m { │ │ │ │ │ -25 │ │ │ │ │ -26namespace internal { struct NonlinearOptimizerState; } │ │ │ │ │ +21#pragma once │ │ │ │ │ +22 │ │ │ │ │ +23#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ +26#include │ │ │ │ │ 27 │ │ │ │ │ -_7_5class GTSAM_EXPORT _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r { │ │ │ │ │ +28class NonlinearOptimizerMoreOptimizationTest; │ │ │ │ │ +29 │ │ │ │ │ +30namespace _g_t_s_a_m { │ │ │ │ │ +31 │ │ │ │ │ +_3_5class GTSAM_EXPORT _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r: public _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r { │ │ │ │ │ +36 │ │ │ │ │ +37protected: │ │ │ │ │ +_3_8 const _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s _p_a_r_a_m_s__; │ │ │ │ │ +39 boost::posix_time::ptime startTime_; │ │ │ │ │ +40 │ │ │ │ │ +41 void initTime(); │ │ │ │ │ +42 │ │ │ │ │ +43public: │ │ │ │ │ +44 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +45 │ │ │ │ │ +48 │ │ │ │ │ +57 _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, const _V_a_l_u_e_s& │ │ │ │ │ +initialValues, │ │ │ │ │ +58 const _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s& params = _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s()); │ │ │ │ │ +59 │ │ │ │ │ +67 _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, const _V_a_l_u_e_s& │ │ │ │ │ +initialValues, │ │ │ │ │ +68 const _O_r_d_e_r_i_n_g& ordering, │ │ │ │ │ +69 const _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s& params = _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s()); │ │ │ │ │ +70 │ │ │ │ │ +_7_2 _~_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r() override { │ │ │ │ │ +73 } │ │ │ │ │ +74 │ │ │ │ │ 76 │ │ │ │ │ -77protected: │ │ │ │ │ -_7_8 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h _g_r_a_p_h__; │ │ │ │ │ 79 │ │ │ │ │ -_8_0 std::unique_ptr _s_t_a_t_e__; │ │ │ │ │ -81 │ │ │ │ │ -82public: │ │ │ │ │ -_8_4 using _s_h_a_r_e_d___p_t_r = boost::shared_ptr; │ │ │ │ │ +81 double lambda() const; │ │ │ │ │ +82 │ │ │ │ │ +84 int getInnerIterations() const; │ │ │ │ │ 85 │ │ │ │ │ -88 │ │ │ │ │ -_9_8 virtual const _V_a_l_u_e_s& _o_p_t_i_m_i_z_e() { defaultOptimize(); return values(); } │ │ │ │ │ -99 │ │ │ │ │ -106 const _V_a_l_u_e_s& optimizeSafely(); │ │ │ │ │ +_8_7 void _p_r_i_n_t(const std::string& str = "") const { │ │ │ │ │ +88 std::cout << str << "LevenbergMarquardtOptimizer" << std::endl; │ │ │ │ │ +89 this->params_.print(" parameters:\n"); │ │ │ │ │ +90 } │ │ │ │ │ +91 │ │ │ │ │ +93 │ │ │ │ │ +96 │ │ │ │ │ +101 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r iterate() override; │ │ │ │ │ +102 │ │ │ │ │ +_1_0_4 const _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s& _p_a_r_a_m_s() const { │ │ │ │ │ +105 return params_; │ │ │ │ │ +106 } │ │ │ │ │ 107 │ │ │ │ │ -109 double error() const; │ │ │ │ │ -110 │ │ │ │ │ -112 size_t iterations() const; │ │ │ │ │ -113 │ │ │ │ │ -115 const _V_a_l_u_e_s &values() const; │ │ │ │ │ +108 void writeLogFile(double currentError); │ │ │ │ │ +109 │ │ │ │ │ +111 virtual _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r linearize() const; │ │ │ │ │ +112 │ │ │ │ │ +114 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h buildDampedSystem(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& linear, │ │ │ │ │ +115 const _V_e_c_t_o_r_V_a_l_u_e_s& sqrtHessianDiagonal) const; │ │ │ │ │ 116 │ │ │ │ │ -_1_1_8 const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h &_g_r_a_p_h() const { return graph_; } │ │ │ │ │ +118 bool tryLambda(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& linear, const _V_e_c_t_o_r_V_a_l_u_e_s& │ │ │ │ │ +sqrtHessianDiagonal); │ │ │ │ │ 119 │ │ │ │ │ 121 │ │ │ │ │ -124 │ │ │ │ │ -126 virtual _~_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r(); │ │ │ │ │ -127 │ │ │ │ │ -129 virtual _V_e_c_t_o_r_V_a_l_u_e_s solve(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h &gfg, │ │ │ │ │ -130 const _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s& params) const; │ │ │ │ │ -131 │ │ │ │ │ -_1_3_6 virtual _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r _i_t_e_r_a_t_e() = 0; │ │ │ │ │ -137 │ │ │ │ │ -139 │ │ │ │ │ -140protected: │ │ │ │ │ -144 void defaultOptimize(); │ │ │ │ │ -145 │ │ │ │ │ -146 virtual const _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s& _params() const = 0; │ │ │ │ │ -147 │ │ │ │ │ -149 _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, │ │ │ │ │ -150 std::unique_ptr state); │ │ │ │ │ -151}; │ │ │ │ │ -152 │ │ │ │ │ -157GTSAM_EXPORT bool _c_h_e_c_k_C_o_n_v_e_r_g_e_n_c_e(double relativeErrorTreshold, │ │ │ │ │ -158 double absoluteErrorTreshold, double errorThreshold, │ │ │ │ │ -159 double currentError, double newError, _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_V_e_r_b_o_s_i_t_y │ │ │ │ │ -verbosity = NonlinearOptimizerParams::SILENT); │ │ │ │ │ -160 │ │ │ │ │ -161GTSAM_EXPORT bool _c_h_e_c_k_C_o_n_v_e_r_g_e_n_c_e(const _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s& params, │ │ │ │ │ -double currentError, │ │ │ │ │ -162 double newError); │ │ │ │ │ -163 │ │ │ │ │ -164} // gtsam │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Factor Graph consisting of non-linear factors. │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_._h │ │ │ │ │ -Parameters for nonlinear optimization. │ │ │ │ │ +122protected: │ │ │ │ │ +123 │ │ │ │ │ +_1_2_5 const _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s& ___p_a_r_a_m_s() const override { │ │ │ │ │ +126 return params_; │ │ │ │ │ +127 } │ │ │ │ │ +128}; │ │ │ │ │ +129 │ │ │ │ │ +130} │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ +Factor Graph Values. │ │ │ │ │ +_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_._h │ │ │ │ │ +Parameters for Levenberg-Marquardt trust-region scheme. │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ +Base class and parameters for nonlinear optimization algorithms. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_c_h_e_c_k_C_o_n_v_e_r_g_e_n_c_e │ │ │ │ │ -bool checkConvergence(double relativeErrorTreshold, double │ │ │ │ │ -absoluteErrorTreshold, double errorThreshold, double currentError, double │ │ │ │ │ -newError, NonlinearOptimizerParams::Verbosity verbosity) │ │ │ │ │ -Check whether the relative error decrease is less than relativeErrorTreshold, │ │ │ │ │ -the absolute error decr... │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizer.cpp:185 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ boost::shared_ptr< This > shared_ptr │ │ │ │ │ shared_ptr to this class │ │ │ │ │ DDeeffiinniittiioonn GaussianFactorGraph.h:81 │ │ │ │ │ _g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ with a unique integer... │ │ │ │ │ DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r │ │ │ │ │ +This class performs Levenberg-Marquardt nonlinear optimization. │ │ │ │ │ +DDeeffiinniittiioonn LevenbergMarquardtOptimizer.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r_:_:_~_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r │ │ │ │ │ +~LevenbergMarquardtOptimizer() override │ │ │ │ │ +Virtual destructor. │ │ │ │ │ +DDeeffiinniittiioonn LevenbergMarquardtOptimizer.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r_:_:_p_a_r_a_m_s │ │ │ │ │ +const LevenbergMarquardtParams & params() const │ │ │ │ │ +Read-only access the parameters. │ │ │ │ │ +DDeeffiinniittiioonn LevenbergMarquardtOptimizer.h:104 │ │ │ │ │ +_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r_:_:___p_a_r_a_m_s │ │ │ │ │ +const NonlinearOptimizerParams & _params() const override │ │ │ │ │ +Access the parameters (base class version) │ │ │ │ │ +DDeeffiinniittiioonn LevenbergMarquardtOptimizer.h:125 │ │ │ │ │ +_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r_:_:_p_a_r_a_m_s__ │ │ │ │ │ +const LevenbergMarquardtParams params_ │ │ │ │ │ +LM parameters. │ │ │ │ │ +DDeeffiinniittiioonn LevenbergMarquardtOptimizer.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &str="") const │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn LevenbergMarquardtOptimizer.h:87 │ │ │ │ │ +_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s │ │ │ │ │ +Parameters for Levenberg-Marquardt optimization. │ │ │ │ │ +DDeeffiinniittiioonn LevenbergMarquardtParams.h:35 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r │ │ │ │ │ This is the abstract interface for classes that can optimize for the maximum- │ │ │ │ │ likelihood estimate of a... │ │ │ │ │ DDeeffiinniittiioonn NonlinearOptimizer.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_:_:_g_r_a_p_h │ │ │ │ │ -const NonlinearFactorGraph & graph() const │ │ │ │ │ -return the graph with nonlinear factors │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizer.h:118 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_:_:_g_r_a_p_h__ │ │ │ │ │ -NonlinearFactorGraph graph_ │ │ │ │ │ -The graph with nonlinear factors. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizer.h:78 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ -virtual const Values & optimize() │ │ │ │ │ -Optimize for the maximum-likelihood estimate, returning a the optimized │ │ │ │ │ -variable assignments. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizer.h:98 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_:_:_s_t_a_t_e__ │ │ │ │ │ -std::unique_ptr< internal::NonlinearOptimizerState > state_ │ │ │ │ │ -PIMPL'd state. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizer.h:80 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< const NonlinearOptimizer > shared_ptr │ │ │ │ │ -A shared pointer to this class. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizer.h:84 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_:_:_i_t_e_r_a_t_e │ │ │ │ │ -virtual GaussianFactorGraph::shared_ptr iterate()=0 │ │ │ │ │ -Perform a single iteration, returning GaussianFactorGraph corresponding to the │ │ │ │ │ -linearized factor grap... │ │ │ │ │ _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s │ │ │ │ │ The common parameters for Nonlinear optimizers. │ │ │ │ │ DDeeffiinniittiioonn NonlinearOptimizerParams.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_V_e_r_b_o_s_i_t_y │ │ │ │ │ -Verbosity │ │ │ │ │ -See NonlinearOptimizerParams::verbosity. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizerParams.h:37 │ │ │ │ │ _g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ + * _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01031_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01235_source.html │ │ │ │┄ Files 94% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/CustomFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/PoseTranslationPrior.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
CustomFactor.h
│ │ │ │ +
PoseTranslationPrior.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ -
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ -
4 * Atlanta, Georgia 30332-0415
│ │ │ │ -
5 * All Rights Reserved
│ │ │ │ -
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ -
7
│ │ │ │ -
8 * See LICENSE for the license information
│ │ │ │ -
9
│ │ │ │ -
10 * -------------------------------------------------------------------------- */
│ │ │ │ +Go to the documentation of this file.
1
│ │ │ │ +
10#pragma once
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
21
│ │ │ │ -
22namespace gtsam {
│ │ │ │ -
23
│ │ │ │ -
24using JacobianVector = std::vector<Matrix>;
│ │ │ │ -
25
│ │ │ │ -
26class CustomFactor;
│ │ │ │ -
27
│ │ │ │ -
28/*
│ │ │ │ -
29 * NOTE
│ │ │ │ -
30 * ==========
│ │ │ │ -
31 * pybind11 will invoke a copy if this is `JacobianVector &`, and modifications in Python will not be reflected.
│ │ │ │ -
32 *
│ │ │ │ -
33 * This is safe because this is passing a const pointer, and pybind11 will maintain the `std::vector` memory layout.
│ │ │ │ -
34 * Thus the pointer will never be invalidated.
│ │ │ │ -
35 */
│ │ │ │ -
36using CustomErrorFunction = std::function<Vector(const CustomFactor &, const Values &, const JacobianVector *)>;
│ │ │ │ -
37
│ │ │ │ -
│ │ │ │ -
45class CustomFactor: public NoiseModelFactor {
│ │ │ │ -
46protected:
│ │ │ │ -
47 CustomErrorFunction error_function_;
│ │ │ │ -
48
│ │ │ │ -
49protected:
│ │ │ │ -
50
│ │ │ │ -
51 using Base = NoiseModelFactor;
│ │ │ │ -
52 using This = CustomFactor;
│ │ │ │ +
12#include <gtsam/geometry/concepts.h>
│ │ │ │ +
13#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
14
│ │ │ │ +
15namespace gtsam {
│ │ │ │ +
16
│ │ │ │ +
20template<class POSE>
│ │ │ │ +
│ │ │ │ +
21class PoseTranslationPrior : public NoiseModelFactorN<POSE> {
│ │ │ │ +
22public:
│ │ │ │ +
23 typedef PoseTranslationPrior<POSE> This;
│ │ │ │ +
24 typedef NoiseModelFactorN<POSE> Base;
│ │ │ │ +
25 typedef POSE Pose;
│ │ │ │ +
26 typedef typename POSE::Translation Translation;
│ │ │ │ +
27 typedef typename POSE::Rotation Rotation;
│ │ │ │ +
28
│ │ │ │ +
29 GTSAM_CONCEPT_POSE_TYPE(Pose)
│ │ │ │ +
30 GTSAM_CONCEPT_GROUP_TYPE(Pose)
│ │ │ │ +
31 GTSAM_CONCEPT_LIE_TYPE(Translation)
│ │ │ │ +
32
│ │ │ │ +
33protected:
│ │ │ │ +
34
│ │ │ │ +
35 Translation measured_;
│ │ │ │ +
36
│ │ │ │ +
37public:
│ │ │ │ +
38
│ │ │ │ +
40 PoseTranslationPrior() {}
│ │ │ │ +
41
│ │ │ │ +
│ │ │ │ +
43 PoseTranslationPrior(Key key, const Translation& measured, const noiseModel::Base::shared_ptr& model)
│ │ │ │ +
44 : Base(model, key), measured_(measured) {
│ │ │ │ +
45 }
│ │ │ │ +
│ │ │ │ +
46
│ │ │ │ +
│ │ │ │ +
48 PoseTranslationPrior(Key key, const POSE& pose_z, const noiseModel::Base::shared_ptr& model)
│ │ │ │ +
49 : Base(model, key), measured_(pose_z.translation()) {
│ │ │ │ +
50 }
│ │ │ │ +
│ │ │ │ +
51
│ │ │ │ +
52 ~PoseTranslationPrior() override {}
│ │ │ │
53
│ │ │ │ -
54public:
│ │ │ │ +
54 const Translation& measured() const { return measured_; }
│ │ │ │
55
│ │ │ │ -
59 CustomFactor() = default;
│ │ │ │ +
│ │ │ │ +
57 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
58 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ +
59 gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
│ │ │ │ +
│ │ │ │
60
│ │ │ │ -
│ │ │ │ -
67 CustomFactor(const SharedNoiseModel &noiseModel, const KeyVector &keys, const CustomErrorFunction &errorFunction) :
│ │ │ │ -
68 Base(noiseModel, keys) {
│ │ │ │ -
69 this->error_function_ = errorFunction;
│ │ │ │ -
70 }
│ │ │ │ -
│ │ │ │ -
71
│ │ │ │ -
72 ~CustomFactor() override = default;
│ │ │ │ -
73
│ │ │ │ -
78 Vector unwhitenedError(const Values &x, boost::optional<std::vector<Matrix> &> H = boost::none) const override;
│ │ │ │ -
79
│ │ │ │ -
81 void print(const std::string &s,
│ │ │ │ -
82 const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override;
│ │ │ │ -
83
│ │ │ │ -
│ │ │ │ -
87 bool sendable() const override {
│ │ │ │ -
88 return false;
│ │ │ │ -
89 }
│ │ │ │ -
│ │ │ │ -
90
│ │ │ │ -
91private:
│ │ │ │ -
92
│ │ │ │ -
94 friend class boost::serialization::access;
│ │ │ │ -
95 template<class ARCHIVE>
│ │ │ │ -
96 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ -
97 ar & boost::serialization::make_nvp("CustomFactor",
│ │ │ │ -
98 boost::serialization::base_object<Base>(*this));
│ │ │ │ -
99 }
│ │ │ │ +
│ │ │ │ +
62 Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const override {
│ │ │ │ +
63 const Translation& newTrans = pose.translation();
│ │ │ │ +
64 const Rotation& R = pose.rotation();
│ │ │ │ +
65 const int tDim = traits<Translation>::GetDimension(newTrans);
│ │ │ │ +
66 const int xDim = traits<Pose>::GetDimension(pose);
│ │ │ │ +
67 if (H) {
│ │ │ │ +
68 *H = Matrix::Zero(tDim, xDim);
│ │ │ │ +
69 std::pair<size_t, size_t> transInterval = POSE::translationInterval();
│ │ │ │ +
70 (*H).middleCols(transInterval.first, tDim) = R.matrix();
│ │ │ │ +
71 }
│ │ │ │ +
72
│ │ │ │ +
73 return traits<Translation>::Local(measured_, newTrans);
│ │ │ │ +
74 }
│ │ │ │ +
│ │ │ │ +
75
│ │ │ │ +
│ │ │ │ +
77 bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
│ │ │ │ +
78 const This *e = dynamic_cast<const This*> (&expected);
│ │ │ │ +
79 return e != nullptr && Base::equals(*e, tol) && traits<Translation>::Equals(measured_, e->measured_, tol);
│ │ │ │ +
80 }
│ │ │ │ +
│ │ │ │ +
81
│ │ │ │ +
│ │ │ │ +
83 void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ +
84 Base::print(s + "PoseTranslationPrior", keyFormatter);
│ │ │ │ +
85 traits<Translation>::Print(measured_, "Measured Translation");
│ │ │ │ +
86 }
│ │ │ │ +
│ │ │ │ +
87
│ │ │ │ +
88private:
│ │ │ │ +
89
│ │ │ │ +
91 friend class boost::serialization::access;
│ │ │ │ +
92 template<class ARCHIVE>
│ │ │ │ +
93 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
94 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ +
95 ar & boost::serialization::make_nvp("NoiseModelFactor1",
│ │ │ │ +
96 boost::serialization::base_object<Base>(*this));
│ │ │ │ +
97 ar & BOOST_SERIALIZATION_NVP(measured_);
│ │ │ │ +
98 }
│ │ │ │ +
99
│ │ │ │
100};
│ │ │ │
│ │ │ │
101
│ │ │ │ -
102}
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
102} // \namespace gtsam
│ │ │ │ +
103
│ │ │ │ +
104
│ │ │ │ +
105
│ │ │ │ +
106
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
│ │ │ │ -
gtsam::CustomFactor
Definition CustomFactor.h:45
│ │ │ │ -
gtsam::CustomFactor::unwhitenedError
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
Calls the errorFunction closure, which is a std::function object One can check if a derivative is nee...
Definition CustomFactor.cpp:25
│ │ │ │ -
gtsam::CustomFactor::CustomFactor
CustomFactor(const SharedNoiseModel &noiseModel, const KeyVector &keys, const CustomErrorFunction &errorFunction)
Constructor.
Definition CustomFactor.h:67
│ │ │ │ -
gtsam::CustomFactor::CustomFactor
CustomFactor()=default
Default Constructor for I/O.
│ │ │ │ -
gtsam::CustomFactor::access
friend class boost::serialization::access
Serialization function.
Definition CustomFactor.h:94
│ │ │ │ -
gtsam::CustomFactor::sendable
bool sendable() const override
Mark not sendable.
Definition CustomFactor.h:87
│ │ │ │ -
gtsam::CustomFactor::print
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition CustomFactor.cpp:59
│ │ │ │ -
gtsam::NoiseModelFactor
A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density Templated on...
Definition NonlinearFactor.h:174
│ │ │ │ -
gtsam::NoiseModelFactor::NoiseModelFactor
NoiseModelFactor()
Default constructor for I/O only.
Definition NonlinearFactor.h:189
│ │ │ │ -
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition NonlinearFactor.h:223
│ │ │ │ -
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
│ │ │ │ +
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
│ │ │ │ +
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ +
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ +
gtsam::NoiseModelFactorN< POSE >::key
Key key() const
Returns a key.
Definition NonlinearFactor.h:518
│ │ │ │ +
gtsam::PoseTranslationPrior
A prior on the translation part of a pose.
Definition PoseTranslationPrior.h:21
│ │ │ │ +
gtsam::PoseTranslationPrior::PoseTranslationPrior
PoseTranslationPrior(Key key, const Translation &measured, const noiseModel::Base::shared_ptr &model)
standard constructor
Definition PoseTranslationPrior.h:43
│ │ │ │ +
gtsam::PoseTranslationPrior::PoseTranslationPrior
PoseTranslationPrior(Key key, const POSE &pose_z, const noiseModel::Base::shared_ptr &model)
Constructor that pulls the translation from an incoming POSE.
Definition PoseTranslationPrior.h:48
│ │ │ │ +
gtsam::PoseTranslationPrior::evaluateError
Vector evaluateError(const Pose &pose, boost::optional< Matrix & > H=boost::none) const override
h(x)-z
Definition PoseTranslationPrior.h:62
│ │ │ │ +
gtsam::PoseTranslationPrior::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
equals specialized to this factor
Definition PoseTranslationPrior.h:77
│ │ │ │ +
gtsam::PoseTranslationPrior::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition PoseTranslationPrior.h:57
│ │ │ │ +
gtsam::PoseTranslationPrior::access
friend class boost::serialization::access
Serialization function.
Definition PoseTranslationPrior.h:91
│ │ │ │ +
gtsam::PoseTranslationPrior::PoseTranslationPrior
PoseTranslationPrior()
default constructor - only use for serialization
Definition PoseTranslationPrior.h:40
│ │ │ │ +
gtsam::PoseTranslationPrior::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print contents
Definition PoseTranslationPrior.h:83
│ │ │ │ +
concepts.h
Concept-checking macros for geometric objects Each macro instantiates a concept check structure,...
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,157 +1,182 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -CustomFactor.h │ │ │ │ │ +PoseTranslationPrior.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -1/* --------------------------------------------------------------------------- │ │ │ │ │ -- │ │ │ │ │ -2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ -4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -5 * All Rights Reserved │ │ │ │ │ -6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ -7 │ │ │ │ │ -8 * See LICENSE for the license information │ │ │ │ │ -9 │ │ │ │ │ -10 * ------------------------------------------------------------------------- │ │ │ │ │ -- */ │ │ │ │ │ +1 │ │ │ │ │ +10#pragma once │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -21 │ │ │ │ │ -22namespace _g_t_s_a_m { │ │ │ │ │ -23 │ │ │ │ │ -24using JacobianVector = std::vector; │ │ │ │ │ -25 │ │ │ │ │ -26class CustomFactor; │ │ │ │ │ -27 │ │ │ │ │ -28/* │ │ │ │ │ -29 * NOTE │ │ │ │ │ -30 * ========== │ │ │ │ │ -31 * pybind11 will invoke a copy if this is `JacobianVector &`, and │ │ │ │ │ -modifications in Python will not be reflected. │ │ │ │ │ -32 * │ │ │ │ │ -33 * This is safe because this is passing a const pointer, and pybind11 will │ │ │ │ │ -maintain the `std::vector` memory layout. │ │ │ │ │ -34 * Thus the pointer will never be invalidated. │ │ │ │ │ -35 */ │ │ │ │ │ -36using CustomErrorFunction = std::function; │ │ │ │ │ -37 │ │ │ │ │ -_4_5class _C_u_s_t_o_m_F_a_c_t_o_r: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r { │ │ │ │ │ -46protected: │ │ │ │ │ -47 CustomErrorFunction error_function_; │ │ │ │ │ -48 │ │ │ │ │ -49protected: │ │ │ │ │ -50 │ │ │ │ │ -51 using _B_a_s_e = _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r; │ │ │ │ │ -52 using _T_h_i_s = _C_u_s_t_o_m_F_a_c_t_o_r; │ │ │ │ │ +12#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_c_o_n_c_e_p_t_s_._h> │ │ │ │ │ +13#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +14 │ │ │ │ │ +15namespace _g_t_s_a_m { │ │ │ │ │ +16 │ │ │ │ │ +20template │ │ │ │ │ +_2_1class _P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +22public: │ │ │ │ │ +23 typedef _P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r_<_P_O_S_E_> This; │ │ │ │ │ +24 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_P_O_S_E_> _B_a_s_e; │ │ │ │ │ +25 typedef POSE Pose; │ │ │ │ │ +26 typedef typename POSE::Translation Translation; │ │ │ │ │ +27 typedef typename POSE::Rotation Rotation; │ │ │ │ │ +28 │ │ │ │ │ +29 GTSAM_CONCEPT_POSE_TYPE(Pose) │ │ │ │ │ +30 GTSAM_CONCEPT_GROUP_TYPE(Pose) │ │ │ │ │ +31 GTSAM_CONCEPT_LIE_TYPE(Translation) │ │ │ │ │ +32 │ │ │ │ │ +33protected: │ │ │ │ │ +34 │ │ │ │ │ +35 Translation measured_; │ │ │ │ │ +36 │ │ │ │ │ +37public: │ │ │ │ │ +38 │ │ │ │ │ +_4_0 _P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r() {} │ │ │ │ │ +41 │ │ │ │ │ +_4_3 _P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r(_K_e_y _k_e_y, const Translation& measured, const │ │ │ │ │ +noiseModel::Base::shared_ptr& model) │ │ │ │ │ +44 : _B_a_s_e(model, _k_e_y), measured_(measured) { │ │ │ │ │ +45 } │ │ │ │ │ +46 │ │ │ │ │ +_4_8 _P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r(_K_e_y _k_e_y, const POSE& pose_z, const noiseModel::Base:: │ │ │ │ │ +shared_ptr& model) │ │ │ │ │ +49 : _B_a_s_e(model, _k_e_y), measured_(pose_z.translation()) { │ │ │ │ │ +50 } │ │ │ │ │ +51 │ │ │ │ │ +52 _~_P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r() override {} │ │ │ │ │ 53 │ │ │ │ │ -54public: │ │ │ │ │ +54 const Translation& measured() const { return measured_; } │ │ │ │ │ 55 │ │ │ │ │ -_5_9 _C_u_s_t_o_m_F_a_c_t_o_r() = default; │ │ │ │ │ +_5_7 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +58 return boost::static_pointer_cast( │ │ │ │ │ +59 gtsam::NonlinearFactor::shared_ptr(new This(*this))); } │ │ │ │ │ 60 │ │ │ │ │ -_6_7 _C_u_s_t_o_m_F_a_c_t_o_r(const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &_n_o_i_s_e_M_o_d_e_l, const _K_e_y_V_e_c_t_o_r &_k_e_y_s, │ │ │ │ │ -const CustomErrorFunction &errorFunction) : │ │ │ │ │ -68 _B_a_s_e(_n_o_i_s_e_M_o_d_e_l, _k_e_y_s) { │ │ │ │ │ -69 this->error_function_ = errorFunction; │ │ │ │ │ -70 } │ │ │ │ │ -71 │ │ │ │ │ -72 _~_C_u_s_t_o_m_F_a_c_t_o_r() override = default; │ │ │ │ │ -73 │ │ │ │ │ -78 Vector _u_n_w_h_i_t_e_n_e_d_E_r_r_o_r(const _V_a_l_u_e_s &x, boost::optional │ │ │ │ │ -&> H = boost::none) const override; │ │ │ │ │ -79 │ │ │ │ │ -81 void _p_r_i_n_t(const std::string &s, │ │ │ │ │ -82 const _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter = DefaultKeyFormatter) const override; │ │ │ │ │ -83 │ │ │ │ │ -_8_7 bool _s_e_n_d_a_b_l_e() const override { │ │ │ │ │ -88 return false; │ │ │ │ │ -89 } │ │ │ │ │ -90 │ │ │ │ │ -91private: │ │ │ │ │ -92 │ │ │ │ │ -_9_4 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -95 template │ │ │ │ │ -96 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ -97 ar & boost::serialization::make_nvp("CustomFactor", │ │ │ │ │ -98 boost::serialization::base_object(*this)); │ │ │ │ │ -99 } │ │ │ │ │ +_6_2 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const Pose& pose, boost::optional H = boost:: │ │ │ │ │ +none) const override { │ │ │ │ │ +63 const Translation& newTrans = pose.translation(); │ │ │ │ │ +64 const Rotation& R = pose.rotation(); │ │ │ │ │ +65 const int tDim = _t_r_a_i_t_s_<_T_r_a_n_s_l_a_t_i_o_n_>_:_:_G_e_t_D_i_m_e_n_s_i_o_n(newTrans); │ │ │ │ │ +66 const int xDim = _t_r_a_i_t_s_<_P_o_s_e_>_:_:_G_e_t_D_i_m_e_n_s_i_o_n(pose); │ │ │ │ │ +67 if (H) { │ │ │ │ │ +68 *H = Matrix::Zero(tDim, xDim); │ │ │ │ │ +69 std::pair transInterval = POSE::translationInterval(); │ │ │ │ │ +70 (*H).middleCols(transInterval.first, tDim) = R.matrix(); │ │ │ │ │ +71 } │ │ │ │ │ +72 │ │ │ │ │ +73 return _t_r_a_i_t_s_<_T_r_a_n_s_l_a_t_i_o_n_>_:_:_L_o_c_a_l(measured_, newTrans); │ │ │ │ │ +74 } │ │ │ │ │ +75 │ │ │ │ │ +_7_7 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& expected, double tol=1e-9) const override │ │ │ │ │ +{ │ │ │ │ │ +78 const This *e = dynamic_cast (&expected); │ │ │ │ │ +79 return e != nullptr && _B_a_s_e_:_:_e_q_u_a_l_s(*e, tol) && _t_r_a_i_t_s_<_T_r_a_n_s_l_a_t_i_o_n_>_:_:_E_q_u_a_l_s │ │ │ │ │ +(measured_, e->measured_, tol); │ │ │ │ │ +80 } │ │ │ │ │ +81 │ │ │ │ │ +_8_3 void _p_r_i_n_t(const std::string& s="", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ +DefaultKeyFormatter) const override { │ │ │ │ │ +84 _B_a_s_e_:_:_p_r_i_n_t(s + "PoseTranslationPrior", keyFormatter); │ │ │ │ │ +85 _t_r_a_i_t_s_<_T_r_a_n_s_l_a_t_i_o_n_>_:_:_P_r_i_n_t(measured_, "Measured Translation"); │ │ │ │ │ +86 } │ │ │ │ │ +87 │ │ │ │ │ +88private: │ │ │ │ │ +89 │ │ │ │ │ +_9_1 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +92 template │ │ │ │ │ +93 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +94 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility │ │ │ │ │ +95 ar & boost::serialization::make_nvp("NoiseModelFactor1", │ │ │ │ │ +96 boost::serialization::base_object(*this)); │ │ │ │ │ +97 ar & BOOST_SERIALIZATION_NVP(measured_); │ │ │ │ │ +98 } │ │ │ │ │ +99 │ │ │ │ │ 100}; │ │ │ │ │ 101 │ │ │ │ │ -102} │ │ │ │ │ +102} // \namespace gtsam │ │ │ │ │ +103 │ │ │ │ │ +104 │ │ │ │ │ +105 │ │ │ │ │ +106 │ │ │ │ │ _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ -Aliases. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s │ │ │ │ │ -const KeyVector & keys() const │ │ │ │ │ -Access the factor's involved variable keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:140 │ │ │ │ │ -_g_t_s_a_m_:_:_C_u_s_t_o_m_F_a_c_t_o_r │ │ │ │ │ -DDeeffiinniittiioonn CustomFactor.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_C_u_s_t_o_m_F_a_c_t_o_r_:_:_u_n_w_h_i_t_e_n_e_d_E_r_r_o_r │ │ │ │ │ -Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > │ │ │ │ │ -& > H=boost::none) const override │ │ │ │ │ -Calls the errorFunction closure, which is a std::function object One can check │ │ │ │ │ -if a derivative is nee... │ │ │ │ │ -DDeeffiinniittiioonn CustomFactor.cpp:25 │ │ │ │ │ -_g_t_s_a_m_:_:_C_u_s_t_o_m_F_a_c_t_o_r_:_:_C_u_s_t_o_m_F_a_c_t_o_r │ │ │ │ │ -CustomFactor(const SharedNoiseModel &noiseModel, const KeyVector &keys, const │ │ │ │ │ -CustomErrorFunction &errorFunction) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn CustomFactor.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_C_u_s_t_o_m_F_a_c_t_o_r_:_:_C_u_s_t_o_m_F_a_c_t_o_r │ │ │ │ │ -CustomFactor()=default │ │ │ │ │ -Default Constructor for I/O. │ │ │ │ │ -_g_t_s_a_m_:_:_C_u_s_t_o_m_F_a_c_t_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +virtual void print(const std::string &s="Factor", const KeyFormatter │ │ │ │ │ +&formatter=DefaultKeyFormatter) const │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn Factor.cpp:29 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const This &other, double tol=1e-9) const │ │ │ │ │ +check equality │ │ │ │ │ +DDeeffiinniittiioonn Factor.cpp:42 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ +Nonlinear factor base class. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ +A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ +variables. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_ _P_O_S_E_ _>_:_:_k_e_y │ │ │ │ │ +Key key() const │ │ │ │ │ +Returns a key. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:518 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r │ │ │ │ │ +A prior on the translation part of a pose. │ │ │ │ │ +DDeeffiinniittiioonn PoseTranslationPrior.h:21 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r_:_:_P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r │ │ │ │ │ +PoseTranslationPrior(Key key, const Translation &measured, const noiseModel:: │ │ │ │ │ +Base::shared_ptr &model) │ │ │ │ │ +standard constructor │ │ │ │ │ +DDeeffiinniittiioonn PoseTranslationPrior.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r_:_:_P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r │ │ │ │ │ +PoseTranslationPrior(Key key, const POSE &pose_z, const noiseModel::Base:: │ │ │ │ │ +shared_ptr &model) │ │ │ │ │ +Constructor that pulls the translation from an incoming POSE. │ │ │ │ │ +DDeeffiinniittiioonn PoseTranslationPrior.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ +Vector evaluateError(const Pose &pose, boost::optional< Matrix & > H=boost:: │ │ │ │ │ +none) const override │ │ │ │ │ +h(x)-z │ │ │ │ │ +DDeeffiinniittiioonn PoseTranslationPrior.h:62 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const NonlinearFactor &expected, double tol=1e-9) const override │ │ │ │ │ +equals specialized to this factor │ │ │ │ │ +DDeeffiinniittiioonn PoseTranslationPrior.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r_:_:_c_l_o_n_e │ │ │ │ │ +gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn PoseTranslationPrior.h:57 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ friend class boost::serialization::access │ │ │ │ │ Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn CustomFactor.h:94 │ │ │ │ │ -_g_t_s_a_m_:_:_C_u_s_t_o_m_F_a_c_t_o_r_:_:_s_e_n_d_a_b_l_e │ │ │ │ │ -bool sendable() const override │ │ │ │ │ -Mark not sendable. │ │ │ │ │ -DDeeffiinniittiioonn CustomFactor.h:87 │ │ │ │ │ -_g_t_s_a_m_:_:_C_u_s_t_o_m_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s, const KeyFormatter │ │ │ │ │ +DDeeffiinniittiioonn PoseTranslationPrior.h:91 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r_:_:_P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r │ │ │ │ │ +PoseTranslationPrior() │ │ │ │ │ +default constructor - only use for serialization │ │ │ │ │ +DDeeffiinniittiioonn PoseTranslationPrior.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ &keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn CustomFactor.cpp:59 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r │ │ │ │ │ -A nonlinear sum-of-squares factor with a zero-mean noise model implementing the │ │ │ │ │ -density Templated on... │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:174 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r │ │ │ │ │ -NoiseModelFactor() │ │ │ │ │ -Default constructor for I/O only. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:189 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_n_o_i_s_e_M_o_d_e_l │ │ │ │ │ -const SharedNoiseModel & noiseModel() const │ │ │ │ │ -access to the noise model │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:223 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_V_a_l_u_e_s │ │ │ │ │ -In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ -a non-linear function... │ │ │ │ │ +print contents │ │ │ │ │ +DDeeffiinniittiioonn PoseTranslationPrior.h:83 │ │ │ │ │ +_c_o_n_c_e_p_t_s_._h │ │ │ │ │ +Concept-checking macros for geometric objects Each macro instantiates a concept │ │ │ │ │ +check structure,... │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _C_u_s_t_o_m_F_a_c_t_o_r_._h │ │ │ │ │ + * _s_l_a_m │ │ │ │ │ + * _P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01040_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00845_source.html │ │ │ │┄ Files 83% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Clique.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/BinaryJacobianFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ISAM2Clique.h
│ │ │ │ +
BinaryJacobianFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18// \callgraph
│ │ │ │ -
19
│ │ │ │ -
20#pragma once
│ │ │ │ -
21
│ │ │ │ -
22#include <gtsam/inference/BayesTreeCliqueBase.h>
│ │ │ │ -
23#include <gtsam/inference/Key.h>
│ │ │ │ -
24#include <gtsam/linear/GaussianBayesNet.h>
│ │ │ │ -
25#include <gtsam/linear/GaussianConditional.h>
│ │ │ │ -
26#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ -
27#include <string>
│ │ │ │ +
21#pragma once
│ │ │ │ +
22
│ │ │ │ +
23#include <gtsam/linear/JacobianFactor.h>
│ │ │ │ +
24#include <gtsam/base/SymmetricBlockMatrix.h>
│ │ │ │ +
25#include <gtsam/base/timing.h>
│ │ │ │ +
26
│ │ │ │ +
27namespace gtsam {
│ │ │ │
28
│ │ │ │ -
29namespace gtsam {
│ │ │ │ -
30
│ │ │ │ -
│ │ │ │ -
36class GTSAM_EXPORT ISAM2Clique
│ │ │ │ -
37 : public BayesTreeCliqueBase<ISAM2Clique, GaussianFactorGraph> {
│ │ │ │ -
38 public:
│ │ │ │ -
39 typedef ISAM2Clique This;
│ │ │ │ -
40 typedef BayesTreeCliqueBase<This, GaussianFactorGraph> Base;
│ │ │ │ -
41 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
42 typedef boost::weak_ptr<This> weak_ptr;
│ │ │ │ -
43 typedef GaussianConditional ConditionalType;
│ │ │ │ -
44 typedef ConditionalType::shared_ptr sharedConditional;
│ │ │ │ -
45
│ │ │ │ -
46 Base::FactorType::shared_ptr cachedFactor_;
│ │ │ │ -
47 Vector gradientContribution_;
│ │ │ │ -
48#ifdef USE_BROKEN_FAST_BACKSUBSTITUTE
│ │ │ │ -
49 mutable FastMap<Key, VectorValues::iterator> solnPointers_;
│ │ │ │ -
50#endif
│ │ │ │ -
51
│ │ │ │ -
53 ISAM2Clique() : Base() {}
│ │ │ │ -
54 virtual ~ISAM2Clique() = default;
│ │ │ │ -
55
│ │ │ │ -
│ │ │ │ -
58 ISAM2Clique(const ISAM2Clique& other)
│ │ │ │ -
59 : Base(other),
│ │ │ │ -
60 cachedFactor_(other.cachedFactor_),
│ │ │ │ -
61 gradientContribution_(other.gradientContribution_) {}
│ │ │ │ -
│ │ │ │ -
62
│ │ │ │ -
│ │ │ │ -
65 ISAM2Clique& operator=(const ISAM2Clique& other) {
│ │ │ │ -
66 Base::operator=(other);
│ │ │ │ -
67 cachedFactor_ = other.cachedFactor_;
│ │ │ │ -
68 gradientContribution_ = other.gradientContribution_;
│ │ │ │ -
69 return *this;
│ │ │ │ -
70 }
│ │ │ │ -
│ │ │ │ -
71
│ │ │ │ -
73 void setEliminationResult(
│ │ │ │ -
74 const FactorGraphType::EliminationResult& eliminationResult);
│ │ │ │ -
75
│ │ │ │ -
77 Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; }
│ │ │ │ -
78
│ │ │ │ -
80 const Vector& gradientContribution() const { return gradientContribution_; }
│ │ │ │ -
81
│ │ │ │ -
83 void addGradientAtZero(VectorValues* g) const;
│ │ │ │ -
84
│ │ │ │ -
85 bool equals(const This& other, double tol = 1e-9) const;
│ │ │ │ -
86
│ │ │ │ -
88 void print(const std::string& s = "",
│ │ │ │ -
89 const KeyFormatter& formatter = DefaultKeyFormatter) const override;
│ │ │ │ +
32template<int M, int N1, int N2>
│ │ │ │ +
│ │ │ │ +
33struct BinaryJacobianFactor: JacobianFactor {
│ │ │ │ +
34
│ │ │ │ +
│ │ │ │ +
36 BinaryJacobianFactor(Key key1, const Eigen::Matrix<double, M, N1>& A1,
│ │ │ │ +
37 Key key2, const Eigen::Matrix<double, M, N2>& A2,
│ │ │ │ +
38 const Eigen::Matrix<double, M, 1>& b, //
│ │ │ │ +
39 const SharedDiagonal& model = SharedDiagonal()) :
│ │ │ │ +
40 JacobianFactor(key1, A1, key2, A2, b, model) {
│ │ │ │ +
41 }
│ │ │ │ +
│ │ │ │ +
42
│ │ │ │ +
43 inline Key key1() const {
│ │ │ │ +
44 return keys_[0];
│ │ │ │ +
45 }
│ │ │ │ +
46 inline Key key2() const {
│ │ │ │ +
47 return keys_[1];
│ │ │ │ +
48 }
│ │ │ │ +
49
│ │ │ │ +
50 // Fixed-size matrix update
│ │ │ │ +
│ │ │ │ +
51 void updateHessian(const KeyVector& infoKeys,
│ │ │ │ +
52 SymmetricBlockMatrix* info) const override {
│ │ │ │ +
53 gttic(updateHessian_BinaryJacobianFactor);
│ │ │ │ +
54 // Whiten the factor if it has a noise model
│ │ │ │ +
55 const SharedDiagonal& model = get_model();
│ │ │ │ +
56 if (model && !model->isUnit()) {
│ │ │ │ +
57 if (model->isConstrained())
│ │ │ │ +
58 throw std::invalid_argument(
│ │ │ │ +
59 "BinaryJacobianFactor::updateHessian: cannot update information with "
│ │ │ │ +
60 "constrained noise model");
│ │ │ │ +
61 BinaryJacobianFactor whitenedFactor(key1(), model->Whiten(getA(begin())),
│ │ │ │ +
62 key2(), model->Whiten(getA(end())), model->whiten(getb()));
│ │ │ │ +
63 whitenedFactor.updateHessian(infoKeys, info);
│ │ │ │ +
64 } else {
│ │ │ │ +
65 // First build an array of slots
│ │ │ │ +
66 DenseIndex slot1 = Slot(infoKeys, key1());
│ │ │ │ +
67 DenseIndex slot2 = Slot(infoKeys, key2());
│ │ │ │ +
68 DenseIndex slotB = info->nBlocks() - 1;
│ │ │ │ +
69
│ │ │ │ +
70 const Matrix& Ab = Ab_.matrix();
│ │ │ │ +
71 Eigen::Block<const Matrix, M, N1> A1(Ab, 0, 0);
│ │ │ │ +
72 Eigen::Block<const Matrix, M, N2> A2(Ab, 0, N1);
│ │ │ │ +
73 Eigen::Block<const Matrix, M, 1> b(Ab, 0, N1 + N2);
│ │ │ │ +
74
│ │ │ │ +
75 // We perform I += A'*A to the upper triangle
│ │ │ │ +
76 info->diagonalBlock(slot1).rankUpdate(A1.transpose());
│ │ │ │ +
77 info->updateOffDiagonalBlock(slot1, slot2, A1.transpose() * A2);
│ │ │ │ +
78 info->updateOffDiagonalBlock(slot1, slotB, A1.transpose() * b);
│ │ │ │ +
79 info->diagonalBlock(slot2).rankUpdate(A2.transpose());
│ │ │ │ +
80 info->updateOffDiagonalBlock(slot2, slotB, A2.transpose() * b);
│ │ │ │ +
81 info->updateDiagonalBlock(slotB, b.transpose() * b);
│ │ │ │ +
82 }
│ │ │ │ +
83 }
│ │ │ │ +
│ │ │ │ +
84};
│ │ │ │ +
│ │ │ │ +
85
│ │ │ │ +
86template<int M, int N1, int N2>
│ │ │ │ +
│ │ │ │ +
87struct traits<BinaryJacobianFactor<M, N1, N2> > : Testable<
│ │ │ │ +
88 BinaryJacobianFactor<M, N1, N2> > {
│ │ │ │ +
89};
│ │ │ │ +
│ │ │ │
90
│ │ │ │ -
91 void optimizeWildfire(const KeySet& replaced, double threshold,
│ │ │ │ -
92 KeySet* changed, VectorValues* delta,
│ │ │ │ -
93 size_t* count) const;
│ │ │ │ -
94
│ │ │ │ -
95 bool optimizeWildfireNode(const KeySet& replaced, double threshold,
│ │ │ │ -
96 KeySet* changed, VectorValues* delta,
│ │ │ │ -
97 size_t* count) const;
│ │ │ │ -
98
│ │ │ │ -
103 void nnz_internal(size_t* result) const;
│ │ │ │ -
104 size_t calculate_nnz() const;
│ │ │ │ -
105
│ │ │ │ -
121 void findAll(const KeySet& markedMask, KeySet* keys) const;
│ │ │ │ -
122
│ │ │ │ -
123 private:
│ │ │ │ -
128 bool isDirty(const KeySet& replaced, const KeySet& changed) const;
│ │ │ │ -
129
│ │ │ │ -
134 void fastBackSubstitute(VectorValues* delta) const;
│ │ │ │ -
135
│ │ │ │ -
136 /*
│ │ │ │ -
137 * Check whether the values changed above a threshold, or always true if the
│ │ │ │ -
138 * clique was replaced.
│ │ │ │ -
139 */
│ │ │ │ -
140 bool valuesChanged(const KeySet& replaced, const Vector& originalValues,
│ │ │ │ -
141 const VectorValues& delta, double threshold) const;
│ │ │ │ -
142
│ │ │ │ -
144 void markFrontalsAsChanged(KeySet* changed) const;
│ │ │ │ -
145
│ │ │ │ -
147 void restoreFromOriginals(const Vector& originalValues,
│ │ │ │ -
148 VectorValues* delta) const;
│ │ │ │ -
149
│ │ │ │ -
151 friend class boost::serialization::access;
│ │ │ │ -
152 template <class ARCHIVE>
│ │ │ │ -
153 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ -
154 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
155 ar& BOOST_SERIALIZATION_NVP(cachedFactor_);
│ │ │ │ -
156 ar& BOOST_SERIALIZATION_NVP(gradientContribution_);
│ │ │ │ -
157 }
│ │ │ │ -
158}; // \struct ISAM2Clique
│ │ │ │ -
│ │ │ │ -
159
│ │ │ │ -
171size_t optimizeWildfire(const ISAM2Clique::shared_ptr& root, double threshold,
│ │ │ │ -
172 const KeySet& replaced, VectorValues* delta);
│ │ │ │ -
173
│ │ │ │ -
174size_t optimizeWildfireNonRecursive(const ISAM2Clique::shared_ptr& root,
│ │ │ │ -
175 double threshold, const KeySet& replaced,
│ │ │ │ -
176 VectorValues* delta);
│ │ │ │ -
177
│ │ │ │ -
178} // namespace gtsam
│ │ │ │ - │ │ │ │ -
BayesTreeCliqueBase.h
Base class for cliques of a BayesTree.
│ │ │ │ -
GaussianConditional.h
Conditional Gaussian Base class.
│ │ │ │ -
GaussianBayesNet.h
Chordal Bayes Net, the result of eliminating a factor graph.
│ │ │ │ -
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ +
91} //namespace gtsam
│ │ │ │ +
SymmetricBlockMatrix.h
Access to matrices via blocks of pre-defined sizes.
│ │ │ │ +
timing.h
Timing utilities.
│ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::optimizeWildfire
size_t optimizeWildfire(const ISAM2Clique::shared_ptr &root, double threshold, const KeySet &keys, VectorValues *delta)
Optimize the BayesTree, starting from the root.
Definition ISAM2Clique.cpp:226
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ - │ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::BayesTreeCliqueBase
This is the base class for BayesTree cliques.
Definition BayesTreeCliqueBase.h:50
│ │ │ │ -
gtsam::GaussianConditional
A GaussianConditional functions as the node in a Bayes network.
Definition GaussianConditional.h:43
│ │ │ │ -
gtsam::GaussianConditional::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianConditional.h:46
│ │ │ │ -
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ -
gtsam::ISAM2Clique
Specialized Clique structure for ISAM2, incorporating caching and gradient contribution TODO: more do...
Definition ISAM2Clique.h:37
│ │ │ │ -
gtsam::ISAM2Clique::gradientContribution
const Vector & gradientContribution() const
Access the gradient contribution.
Definition ISAM2Clique.h:80
│ │ │ │ -
gtsam::ISAM2Clique::ISAM2Clique
ISAM2Clique(const ISAM2Clique &other)
Copy constructor, does not copy solution pointers as these are invalid in different trees.
Definition ISAM2Clique.h:58
│ │ │ │ -
gtsam::ISAM2Clique::operator=
ISAM2Clique & operator=(const ISAM2Clique &other)
Assignment operator, does not copy solution pointers as these are invalid in different trees.
Definition ISAM2Clique.h:65
│ │ │ │ -
gtsam::ISAM2Clique::cachedFactor
Base::FactorType::shared_ptr & cachedFactor()
Access the cached factor.
Definition ISAM2Clique.h:77
│ │ │ │ -
gtsam::ISAM2Clique::ISAM2Clique
ISAM2Clique()
Default constructor.
Definition ISAM2Clique.h:53
│ │ │ │ -
VectorValues
The Factor::error simply extracts the.
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::DenseIndex
ptrdiff_t DenseIndex
The index type for Eigen objects.
Definition types.h:106
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
│ │ │ │ +
gtsam::SymmetricBlockMatrix::updateOffDiagonalBlock
void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr)
Update an off diagonal block.
Definition SymmetricBlockMatrix.h:228
│ │ │ │ +
gtsam::SymmetricBlockMatrix::diagonalBlock
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J'th diagonal block as a self adjoint view.
Definition SymmetricBlockMatrix.h:135
│ │ │ │ +
gtsam::SymmetricBlockMatrix::updateDiagonalBlock
void updateDiagonalBlock(DenseIndex I, const XprType &xpr)
Increment the diagonal block by the values in xpr. Only reads the upper triangular part of xpr.
Definition SymmetricBlockMatrix.h:212
│ │ │ │ +
gtsam::SymmetricBlockMatrix::nBlocks
DenseIndex nBlocks() const
Block count.
Definition SymmetricBlockMatrix.h:120
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ +
gtsam::VerticalBlockMatrix::matrix
const Matrix & matrix() const
Access to full matrix (including any portions excluded by rowStart(), rowEnd(), and firstBlock())
Definition VerticalBlockMatrix.h:188
│ │ │ │ +
gtsam::Factor::keys_
KeyVector keys_
The keys involved in this factor.
Definition Factor.h:85
│ │ │ │ +
gtsam::Factor::begin
const_iterator begin() const
Iterator at beginning of involved variable keys.
Definition Factor.h:143
│ │ │ │ +
gtsam::Factor::end
const_iterator end() const
Iterator at end of involved variable keys.
Definition Factor.h:146
│ │ │ │ +
gtsam::BinaryJacobianFactor
A binary JacobianFactor specialization that uses fixed matrix math for speed.
Definition BinaryJacobianFactor.h:33
│ │ │ │ +
gtsam::BinaryJacobianFactor::BinaryJacobianFactor
BinaryJacobianFactor(Key key1, const Eigen::Matrix< double, M, N1 > &A1, Key key2, const Eigen::Matrix< double, M, N2 > &A2, const Eigen::Matrix< double, M, 1 > &b, const SharedDiagonal &model=SharedDiagonal())
Constructor.
Definition BinaryJacobianFactor.h:36
│ │ │ │ +
gtsam::BinaryJacobianFactor::updateHessian
void updateHessian(const KeyVector &infoKeys, SymmetricBlockMatrix *info) const override
Update an information matrix by adding the information corresponding to this factor (used internally ...
Definition BinaryJacobianFactor.h:51
│ │ │ │ +
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition JacobianFactor.h:91
│ │ │ │ +
gtsam::JacobianFactor::getb
const constBVector getb() const
Get a view of the r.h.s.
Definition JacobianFactor.h:297
│ │ │ │ +
gtsam::JacobianFactor::get_model
const SharedDiagonal & get_model() const
get a copy of model
Definition JacobianFactor.h:291
│ │ │ │ +
gtsam::JacobianFactor::getA
constABlock getA() const
Get a view of the A matrix, not weighted by noise.
Definition JacobianFactor.h:303
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,205 +1,186 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ISAM2Clique.h │ │ │ │ │ +BinaryJacobianFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18// \callgraph │ │ │ │ │ -19 │ │ │ │ │ -20#pragma once │ │ │ │ │ -21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -27#include │ │ │ │ │ +21#pragma once │ │ │ │ │ +22 │ │ │ │ │ +23#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_b_a_s_e_/_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_b_a_s_e_/_t_i_m_i_n_g_._h> │ │ │ │ │ +26 │ │ │ │ │ +27namespace _g_t_s_a_m { │ │ │ │ │ 28 │ │ │ │ │ -29namespace _g_t_s_a_m { │ │ │ │ │ -30 │ │ │ │ │ -_3_6class GTSAM_EXPORT _I_S_A_M_2_C_l_i_q_u_e │ │ │ │ │ -37 : public _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e { │ │ │ │ │ -38 public: │ │ │ │ │ -39 typedef _I_S_A_M_2_C_l_i_q_u_e _T_h_i_s; │ │ │ │ │ -40 typedef _B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_<_T_h_i_s_,_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_> Base; │ │ │ │ │ -41 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ -42 typedef boost::weak_ptr weak_ptr; │ │ │ │ │ -43 typedef _G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l _C_o_n_d_i_t_i_o_n_a_l_T_y_p_e; │ │ │ │ │ -44 typedef _C_o_n_d_i_t_i_o_n_a_l_T_y_p_e_:_:_s_h_a_r_e_d___p_t_r sharedConditional; │ │ │ │ │ -45 │ │ │ │ │ -46 Base::FactorType::shared_ptr cachedFactor_; │ │ │ │ │ -47 Vector gradientContribution_; │ │ │ │ │ -48#ifdef USE_BROKEN_FAST_BACKSUBSTITUTE │ │ │ │ │ -49 mutable _F_a_s_t_M_a_p_<_K_e_y_,_ _V_e_c_t_o_r_V_a_l_u_e_s_:_:_i_t_e_r_a_t_o_r_> solnPointers_; │ │ │ │ │ -50#endif │ │ │ │ │ -51 │ │ │ │ │ -_5_3 _I_S_A_M_2_C_l_i_q_u_e() : Base() {} │ │ │ │ │ -54 virtual _~_I_S_A_M_2_C_l_i_q_u_e() = default; │ │ │ │ │ -55 │ │ │ │ │ -_5_8 _I_S_A_M_2_C_l_i_q_u_e(const _I_S_A_M_2_C_l_i_q_u_e& other) │ │ │ │ │ -59 : Base(other), │ │ │ │ │ -60 cachedFactor_(other.cachedFactor_), │ │ │ │ │ -61 gradientContribution_(other.gradientContribution_) {} │ │ │ │ │ -62 │ │ │ │ │ -_6_5 _I_S_A_M_2_C_l_i_q_u_e& _o_p_e_r_a_t_o_r_=(const _I_S_A_M_2_C_l_i_q_u_e& other) { │ │ │ │ │ -66 Base::operator=(other); │ │ │ │ │ -67 cachedFactor_ = other.cachedFactor_; │ │ │ │ │ -68 gradientContribution_ = other.gradientContribution_; │ │ │ │ │ -69 return *this; │ │ │ │ │ -70 } │ │ │ │ │ -71 │ │ │ │ │ -73 void setEliminationResult( │ │ │ │ │ -74 const FactorGraphType::EliminationResult& eliminationResult); │ │ │ │ │ -75 │ │ │ │ │ -_7_7 Base::FactorType::shared_ptr& _c_a_c_h_e_d_F_a_c_t_o_r() { return cachedFactor_; } │ │ │ │ │ -78 │ │ │ │ │ -_8_0 const Vector& _g_r_a_d_i_e_n_t_C_o_n_t_r_i_b_u_t_i_o_n() const { return gradientContribution_; } │ │ │ │ │ -81 │ │ │ │ │ -83 void addGradientAtZero(_V_e_c_t_o_r_V_a_l_u_e_s* g) const; │ │ │ │ │ -84 │ │ │ │ │ -85 bool _e_q_u_a_l_s(const This& other, double tol = 1e-9) const; │ │ │ │ │ -86 │ │ │ │ │ -88 void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ -89 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override; │ │ │ │ │ +32template │ │ │ │ │ +_3_3struct _B_i_n_a_r_y_J_a_c_o_b_i_a_n_F_a_c_t_o_r: _J_a_c_o_b_i_a_n_F_a_c_t_o_r { │ │ │ │ │ +34 │ │ │ │ │ +_3_6 _B_i_n_a_r_y_J_a_c_o_b_i_a_n_F_a_c_t_o_r(_K_e_y key1, const Eigen::Matrix& A1, │ │ │ │ │ +37 _K_e_y key2, const Eigen::Matrix& A2, │ │ │ │ │ +38 const Eigen::Matrix& b, // │ │ │ │ │ +39 const SharedDiagonal& model = SharedDiagonal()) : │ │ │ │ │ +40 _J_a_c_o_b_i_a_n_F_a_c_t_o_r(key1, A1, key2, A2, b, model) { │ │ │ │ │ +41 } │ │ │ │ │ +42 │ │ │ │ │ +43 inline _K_e_y key1() const { │ │ │ │ │ +44 return _k_e_y_s__[0]; │ │ │ │ │ +45 } │ │ │ │ │ +46 inline _K_e_y key2() const { │ │ │ │ │ +47 return _k_e_y_s__[1]; │ │ │ │ │ +48 } │ │ │ │ │ +49 │ │ │ │ │ +50 // Fixed-size matrix update │ │ │ │ │ +_5_1 void _u_p_d_a_t_e_H_e_s_s_i_a_n(const _K_e_y_V_e_c_t_o_r& infoKeys, │ │ │ │ │ +52 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x* info) const override { │ │ │ │ │ +53 gttic(updateHessian_BinaryJacobianFactor); │ │ │ │ │ +54 // Whiten the factor if it has a noise model │ │ │ │ │ +55 const SharedDiagonal& model = _g_e_t___m_o_d_e_l(); │ │ │ │ │ +56 if (model && !model->isUnit()) { │ │ │ │ │ +57 if (model->isConstrained()) │ │ │ │ │ +58 throw std::invalid_argument( │ │ │ │ │ +59 "BinaryJacobianFactor::updateHessian: cannot update information with " │ │ │ │ │ +60 "constrained noise model"); │ │ │ │ │ +61 _B_i_n_a_r_y_J_a_c_o_b_i_a_n_F_a_c_t_o_r whitenedFactor(key1(), model->Whiten(_g_e_t_A(_b_e_g_i_n())), │ │ │ │ │ +62 key2(), model->Whiten(_g_e_t_A(_e_n_d())), model->whiten(_g_e_t_b())); │ │ │ │ │ +63 whitenedFactor._u_p_d_a_t_e_H_e_s_s_i_a_n(infoKeys, info); │ │ │ │ │ +64 } else { │ │ │ │ │ +65 // First build an array of slots │ │ │ │ │ +66 _D_e_n_s_e_I_n_d_e_x slot1 = Slot(infoKeys, key1()); │ │ │ │ │ +67 _D_e_n_s_e_I_n_d_e_x slot2 = Slot(infoKeys, key2()); │ │ │ │ │ +68 _D_e_n_s_e_I_n_d_e_x slotB = info->_n_B_l_o_c_k_s() - 1; │ │ │ │ │ +69 │ │ │ │ │ +70 const Matrix& Ab = Ab_._m_a_t_r_i_x(); │ │ │ │ │ +71 Eigen::Block A1(Ab, 0, 0); │ │ │ │ │ +72 Eigen::Block A2(Ab, 0, N1); │ │ │ │ │ +73 Eigen::Block b(Ab, 0, N1 + N2); │ │ │ │ │ +74 │ │ │ │ │ +75 // We perform I += A'*A to the upper triangle │ │ │ │ │ +76 info->_d_i_a_g_o_n_a_l_B_l_o_c_k(slot1).rankUpdate(A1.transpose()); │ │ │ │ │ +77 info->_u_p_d_a_t_e_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k(slot1, slot2, A1.transpose() * A2); │ │ │ │ │ +78 info->_u_p_d_a_t_e_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k(slot1, slotB, A1.transpose() * b); │ │ │ │ │ +79 info->_d_i_a_g_o_n_a_l_B_l_o_c_k(slot2).rankUpdate(A2.transpose()); │ │ │ │ │ +80 info->_u_p_d_a_t_e_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k(slot2, slotB, A2.transpose() * b); │ │ │ │ │ +81 info->_u_p_d_a_t_e_D_i_a_g_o_n_a_l_B_l_o_c_k(slotB, b.transpose() * b); │ │ │ │ │ +82 } │ │ │ │ │ +83 } │ │ │ │ │ +84}; │ │ │ │ │ +85 │ │ │ │ │ +86template │ │ │ │ │ +_8_7struct _t_r_a_i_t_s<_B_i_n_a_r_y_J_a_c_o_b_i_a_n_F_a_c_t_o_r > : _T_e_s_t_a_b_l_e< │ │ │ │ │ +88 BinaryJacobianFactor > { │ │ │ │ │ +89}; │ │ │ │ │ 90 │ │ │ │ │ -91 void _o_p_t_i_m_i_z_e_W_i_l_d_f_i_r_e(const _K_e_y_S_e_t& replaced, double threshold, │ │ │ │ │ -92 _K_e_y_S_e_t* changed, _V_e_c_t_o_r_V_a_l_u_e_s* delta, │ │ │ │ │ -93 size_t* count) const; │ │ │ │ │ -94 │ │ │ │ │ -95 bool optimizeWildfireNode(const _K_e_y_S_e_t& replaced, double threshold, │ │ │ │ │ -96 _K_e_y_S_e_t* changed, _V_e_c_t_o_r_V_a_l_u_e_s* delta, │ │ │ │ │ -97 size_t* count) const; │ │ │ │ │ -98 │ │ │ │ │ -103 void nnz_internal(size_t* result) const; │ │ │ │ │ -104 size_t calculate_nnz() const; │ │ │ │ │ -105 │ │ │ │ │ -121 void findAll(const _K_e_y_S_e_t& markedMask, _K_e_y_S_e_t* keys) const; │ │ │ │ │ -122 │ │ │ │ │ -123 private: │ │ │ │ │ -128 bool isDirty(const _K_e_y_S_e_t& replaced, const _K_e_y_S_e_t& changed) const; │ │ │ │ │ -129 │ │ │ │ │ -134 void fastBackSubstitute(_V_e_c_t_o_r_V_a_l_u_e_s* delta) const; │ │ │ │ │ -135 │ │ │ │ │ -136 /* │ │ │ │ │ -137 * Check whether the values changed above a threshold, or always true if the │ │ │ │ │ -138 * clique was replaced. │ │ │ │ │ -139 */ │ │ │ │ │ -140 bool valuesChanged(const _K_e_y_S_e_t& replaced, const Vector& originalValues, │ │ │ │ │ -141 const _V_e_c_t_o_r_V_a_l_u_e_s& delta, double threshold) const; │ │ │ │ │ -142 │ │ │ │ │ -144 void markFrontalsAsChanged(_K_e_y_S_e_t* changed) const; │ │ │ │ │ -145 │ │ │ │ │ -147 void restoreFromOriginals(const Vector& originalValues, │ │ │ │ │ -148 _V_e_c_t_o_r_V_a_l_u_e_s* delta) const; │ │ │ │ │ -149 │ │ │ │ │ -_1_5_1 friend class boost::serialization::access; │ │ │ │ │ -152 template │ │ │ │ │ -153 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ -154 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); │ │ │ │ │ -155 ar& BOOST_SERIALIZATION_NVP(cachedFactor_); │ │ │ │ │ -156 ar& BOOST_SERIALIZATION_NVP(gradientContribution_); │ │ │ │ │ -157 } │ │ │ │ │ -158}; // \struct ISAM2Clique │ │ │ │ │ -159 │ │ │ │ │ -171size_t _o_p_t_i_m_i_z_e_W_i_l_d_f_i_r_e(const ISAM2Clique::shared_ptr& root, double │ │ │ │ │ -threshold, │ │ │ │ │ -172 const KeySet& replaced, _V_e_c_t_o_r_V_a_l_u_e_s* delta); │ │ │ │ │ -173 │ │ │ │ │ -174size_t optimizeWildfireNonRecursive(const ISAM2Clique::shared_ptr& root, │ │ │ │ │ -175 double threshold, const KeySet& replaced, │ │ │ │ │ -176 _V_e_c_t_o_r_V_a_l_u_e_s* delta); │ │ │ │ │ -177 │ │ │ │ │ -178} // namespace gtsam │ │ │ │ │ -_K_e_y_._h │ │ │ │ │ -_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e_._h │ │ │ │ │ -Base class for cliques of a BayesTree. │ │ │ │ │ -_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ -Conditional Gaussian Base class. │ │ │ │ │ -_G_a_u_s_s_i_a_n_B_a_y_e_s_N_e_t_._h │ │ │ │ │ -Chordal Bayes Net, the result of eliminating a factor graph. │ │ │ │ │ -_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ +91} //namespace gtsam │ │ │ │ │ +_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_._h │ │ │ │ │ +Access to matrices via blocks of pre-defined sizes. │ │ │ │ │ +_t_i_m_i_n_g_._h │ │ │ │ │ +Timing utilities. │ │ │ │ │ +_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_t_i_m_i_z_e_W_i_l_d_f_i_r_e │ │ │ │ │ -size_t optimizeWildfire(const ISAM2Clique::shared_ptr &root, double threshold, │ │ │ │ │ -const KeySet &keys, VectorValues *delta) │ │ │ │ │ -Optimize the BayesTree, starting from the root. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Clique.cpp:226 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ -FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ -fast_pool_allocator instead of the defa... │ │ │ │ │ -DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_C_l_i_q_u_e_B_a_s_e │ │ │ │ │ -This is the base class for BayesTree cliques. │ │ │ │ │ -DDeeffiinniittiioonn BayesTreeCliqueBase.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -A GaussianConditional functions as the node in a Bayes network. │ │ │ │ │ -DDeeffiinniittiioonn GaussianConditional.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_C_o_n_d_i_t_i_o_n_a_l_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn GaussianConditional.h:46 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ -with a unique integer... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_C_l_i_q_u_e │ │ │ │ │ -Specialized Clique structure for ISAM2, incorporating caching and gradient │ │ │ │ │ -contribution TODO: more do... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Clique.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_C_l_i_q_u_e_:_:_g_r_a_d_i_e_n_t_C_o_n_t_r_i_b_u_t_i_o_n │ │ │ │ │ -const Vector & gradientContribution() const │ │ │ │ │ -Access the gradient contribution. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Clique.h:80 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_C_l_i_q_u_e_:_:_I_S_A_M_2_C_l_i_q_u_e │ │ │ │ │ -ISAM2Clique(const ISAM2Clique &other) │ │ │ │ │ -Copy constructor, does not copy solution pointers as these are invalid in │ │ │ │ │ -different trees. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Clique.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_C_l_i_q_u_e_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ -ISAM2Clique & operator=(const ISAM2Clique &other) │ │ │ │ │ -Assignment operator, does not copy solution pointers as these are invalid in │ │ │ │ │ -different trees. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Clique.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_C_l_i_q_u_e_:_:_c_a_c_h_e_d_F_a_c_t_o_r │ │ │ │ │ -Base::FactorType::shared_ptr & cachedFactor() │ │ │ │ │ -Access the cached factor. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Clique.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_C_l_i_q_u_e_:_:_I_S_A_M_2_C_l_i_q_u_e │ │ │ │ │ -ISAM2Clique() │ │ │ │ │ -Default constructor. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Clique.h:53 │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -The Factor::error simply extracts the. │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_n_s_e_I_n_d_e_x │ │ │ │ │ +ptrdiff_t DenseIndex │ │ │ │ │ +The index type for Eigen objects. │ │ │ │ │ +DDeeffiinniittiioonn types.h:106 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ +This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ +of blocks. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_u_p_d_a_t_e_O_f_f_D_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ +void updateOffDiagonalBlock(DenseIndex I, DenseIndex J, const XprType &xpr) │ │ │ │ │ +Update an off diagonal block. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:228 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_d_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ +Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J) │ │ │ │ │ +Return the J'th diagonal block as a self adjoint view. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:135 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_u_p_d_a_t_e_D_i_a_g_o_n_a_l_B_l_o_c_k │ │ │ │ │ +void updateDiagonalBlock(DenseIndex I, const XprType &xpr) │ │ │ │ │ +Increment the diagonal block by the values in xpr. Only reads the upper │ │ │ │ │ +triangular part of xpr. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:212 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_n_B_l_o_c_k_s │ │ │ │ │ +DenseIndex nBlocks() const │ │ │ │ │ +Block count. │ │ │ │ │ +DDeeffiinniittiioonn SymmetricBlockMatrix.h:120 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_r_t_i_c_a_l_B_l_o_c_k_M_a_t_r_i_x_:_:_m_a_t_r_i_x │ │ │ │ │ +const Matrix & matrix() const │ │ │ │ │ +Access to full matrix (including any portions excluded by rowStart(), rowEnd(), │ │ │ │ │ +and firstBlock()) │ │ │ │ │ +DDeeffiinniittiioonn VerticalBlockMatrix.h:188 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s__ │ │ │ │ │ +KeyVector keys_ │ │ │ │ │ +The keys involved in this factor. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_b_e_g_i_n │ │ │ │ │ +const_iterator begin() const │ │ │ │ │ +Iterator at beginning of involved variable keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:143 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_n_d │ │ │ │ │ +const_iterator end() const │ │ │ │ │ +Iterator at end of involved variable keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:146 │ │ │ │ │ +_g_t_s_a_m_:_:_B_i_n_a_r_y_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ +A binary JacobianFactor specialization that uses fixed matrix math for speed. │ │ │ │ │ +DDeeffiinniittiioonn BinaryJacobianFactor.h:33 │ │ │ │ │ +_g_t_s_a_m_:_:_B_i_n_a_r_y_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_B_i_n_a_r_y_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ +BinaryJacobianFactor(Key key1, const Eigen::Matrix< double, M, N1 > &A1, Key │ │ │ │ │ +key2, const Eigen::Matrix< double, M, N2 > &A2, const Eigen::Matrix< double, M, │ │ │ │ │ +1 > &b, const SharedDiagonal &model=SharedDiagonal()) │ │ │ │ │ +Constructor. │ │ │ │ │ +DDeeffiinniittiioonn BinaryJacobianFactor.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_B_i_n_a_r_y_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_u_p_d_a_t_e_H_e_s_s_i_a_n │ │ │ │ │ +void updateHessian(const KeyVector &infoKeys, SymmetricBlockMatrix *info) const │ │ │ │ │ +override │ │ │ │ │ +Update an information matrix by adding the information corresponding to this │ │ │ │ │ +factor (used internally ... │ │ │ │ │ +DDeeffiinniittiioonn BinaryJacobianFactor.h:51 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ +A Gaussian factor in the squared-error form. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:91 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_e_t_b │ │ │ │ │ +const constBVector getb() const │ │ │ │ │ +Get a view of the r.h.s. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:297 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_e_t___m_o_d_e_l │ │ │ │ │ +const SharedDiagonal & get_model() const │ │ │ │ │ +get a copy of model │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:291 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_g_e_t_A │ │ │ │ │ +constABlock getA() const │ │ │ │ │ +Get a view of the A matrix, not weighted by noise. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:303 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _I_S_A_M_2_C_l_i_q_u_e_._h │ │ │ │ │ + * _l_i_n_e_a_r │ │ │ │ │ + * _B_i_n_a_r_y_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01052.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01232.html │ │ │ │┄ Files 96% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Params.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/SfmTrack.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Namespaces
│ │ │ │ -
ISAM2Params.cpp File Reference
│ │ │ │ +
SfmTrack.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Parameters for iSAM 2. │ │ │ │ +

A simple data structure for a track in Structure from Motion. │ │ │ │ More...

│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Parameters for iSAM 2.

│ │ │ │ -
Author
Michael Kaess, Richard Roberts, Frank Dellaert
│ │ │ │ +

A simple data structure for a track in Structure from Motion.

│ │ │ │ +
Date
January 2022
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,22 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -ISAM2Params.cpp File Reference │ │ │ │ │ -Parameters for iSAM 2. _M_o_r_e_._._. │ │ │ │ │ +SfmTrack.cpp File Reference │ │ │ │ │ +A simple data structure for a track in Structure from Motion. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Parameters for iSAM 2. │ │ │ │ │ +A simple data structure for a track in Structure from Motion. │ │ │ │ │ + Date │ │ │ │ │ + January 2022 │ │ │ │ │ Author │ │ │ │ │ - Michael Kaess, Richard Roberts, Frank Dellaert │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _I_S_A_M_2_P_a_r_a_m_s_._c_p_p │ │ │ │ │ + * _s_f_m │ │ │ │ │ + * _S_f_m_T_r_a_c_k_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01058.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01406.html │ │ │ │┄ Files 91% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/LinearContainerFactor.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/precompiled_header.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
│ │ │ │ -Namespaces
│ │ │ │ -
LinearContainerFactor.cpp File Reference
│ │ │ │ +
precompiled_header.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ +

> │ │ │ │ +More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │

Detailed Description

│ │ │ │ -
Date
Jul 6, 2012
│ │ │ │ -
Author
Alex Cunningham
│ │ │ │ +

>

│ │ │ │ +

Include headers that will be included nearly everywhere

Author
Frank Dellaert
│ │ │ │ +
Date
November 2018
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,20 +1,18 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -LinearContainerFactor.cpp File Reference │ │ │ │ │ -NNaammeessppaacceess │ │ │ │ │ -namespace   _g_t_s_a_m │ │ │ │ │ -  Global functions in a separate testing namespace. │ │ │ │ │ -  │ │ │ │ │ +precompiled_header.h File Reference │ │ │ │ │ +> _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ - Date │ │ │ │ │ - Jul 6, 2012 │ │ │ │ │ +> │ │ │ │ │ +Include headers that will be included nearly everywhere │ │ │ │ │ Author │ │ │ │ │ - Alex Cunningham │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + November 2018 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r_._c_p_p │ │ │ │ │ + * _p_r_e_c_o_m_p_i_l_e_d___h_e_a_d_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01070_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01022_source.html │ │ │ │┄ Files 89% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Marginals.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/LinearContainerFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Marginals.h
│ │ │ │ +
LinearContainerFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ -
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ -
4 * Atlanta, Georgia 30332-0415
│ │ │ │ -
5 * All Rights Reserved
│ │ │ │ -
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ -
7
│ │ │ │ -
8 * See LICENSE for the license information
│ │ │ │ -
9
│ │ │ │ -
10 * -------------------------------------------------------------------------- */
│ │ │ │ +Go to the documentation of this file.
1
│ │ │ │ +
10#pragma once
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/linear/GaussianBayesTree.h>
│ │ │ │ -
22#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ -
23#include <gtsam/nonlinear/Values.h>
│ │ │ │ -
24
│ │ │ │ -
25namespace gtsam {
│ │ │ │ -
26
│ │ │ │ -
27class JointMarginal;
│ │ │ │ +
12#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ +
13
│ │ │ │ +
14namespace gtsam {
│ │ │ │ +
15
│ │ │ │ +
16 // Forward declarations
│ │ │ │ +
17 class JacobianFactor;
│ │ │ │ +
18 class HessianFactor;
│ │ │ │ +
19
│ │ │ │ +
│ │ │ │ +
26class GTSAM_EXPORT LinearContainerFactor : public NonlinearFactor {
│ │ │ │ +
27protected:
│ │ │ │
28
│ │ │ │ -
│ │ │ │ -
32class GTSAM_EXPORT Marginals {
│ │ │ │ -
33
│ │ │ │ -
34public:
│ │ │ │ -
35
│ │ │ │ -
│ │ │ │ -
37 enum Factorization {
│ │ │ │ -
38 CHOLESKY,
│ │ │ │ -
39 QR
│ │ │ │ -
40 };
│ │ │ │ -
│ │ │ │ -
41
│ │ │ │ -
42protected:
│ │ │ │ -
43
│ │ │ │ -
44 GaussianFactorGraph graph_;
│ │ │ │ -
45 Values values_;
│ │ │ │ -
46 Factorization factorization_;
│ │ │ │ -
47 GaussianBayesTree bayesTree_;
│ │ │ │ +
29 GaussianFactor::shared_ptr factor_;
│ │ │ │ +
30 boost::optional<Values> linearizationPoint_;
│ │ │ │ +
31
│ │ │ │ +
33 LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const boost::optional<Values>& linearizationPoint);
│ │ │ │ +
34
│ │ │ │ +
35 // Some handy typedefs
│ │ │ │ +
36 typedef NonlinearFactor Base;
│ │ │ │ +
37 typedef LinearContainerFactor This;
│ │ │ │ +
38
│ │ │ │ +
39public:
│ │ │ │ +
40
│ │ │ │ +
41 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
42
│ │ │ │ +
44 LinearContainerFactor() {}
│ │ │ │ +
45
│ │ │ │ +
47 LinearContainerFactor(const JacobianFactor& factor, const Values& linearizationPoint = Values());
│ │ │ │
48
│ │ │ │ -
49public:
│ │ │ │ -
50
│ │ │ │ -
52 Marginals(){}
│ │ │ │ -
53
│ │ │ │ -
59 Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY);
│ │ │ │ +
50 LinearContainerFactor(const HessianFactor& factor, const Values& linearizationPoint = Values());
│ │ │ │ +
51
│ │ │ │ +
53 LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const Values& linearizationPoint = Values());
│ │ │ │ +
54
│ │ │ │ +
55 // Access
│ │ │ │ +
56
│ │ │ │ +
57 const GaussianFactor::shared_ptr& factor() const { return factor_; }
│ │ │ │ +
58
│ │ │ │ +
59 // Testable
│ │ │ │
60
│ │ │ │ -
67 Marginals(const NonlinearFactorGraph& graph, const Values& solution, const Ordering& ordering,
│ │ │ │ -
68 Factorization factorization = CHOLESKY);
│ │ │ │ -
69
│ │ │ │ -
75 Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY);
│ │ │ │ -
76
│ │ │ │ -
83 Marginals(const GaussianFactorGraph& graph, const Values& solution, const Ordering& ordering,
│ │ │ │ -
84 Factorization factorization = CHOLESKY);
│ │ │ │ -
85
│ │ │ │ -
92 Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization = CHOLESKY);
│ │ │ │ -
93
│ │ │ │ -
100 Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, const Ordering& ordering,
│ │ │ │ -
101 Factorization factorization = CHOLESKY);
│ │ │ │ +
62 void print(const std::string& s = "", const KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const override;
│ │ │ │ +
63
│ │ │ │ +
65 bool equals(const NonlinearFactor& f, double tol = 1e-9) const override;
│ │ │ │ +
66
│ │ │ │ +
67 // NonlinearFactor
│ │ │ │ +
68
│ │ │ │ +
77 double error(const Values& c) const override;
│ │ │ │ +
78
│ │ │ │ +
80 size_t dim() const override;
│ │ │ │ +
81
│ │ │ │ +
83 const boost::optional<Values>& linearizationPoint() const { return linearizationPoint_; }
│ │ │ │ +
84
│ │ │ │ +
101 GaussianFactor::shared_ptr linearize(const Values& c) const override;
│ │ │ │
102
│ │ │ │ -
104 void print(const std::string& str = "Marginals: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ -
105
│ │ │ │ -
107 GaussianFactor::shared_ptr marginalFactor(Key variable) const;
│ │ │ │ -
108
│ │ │ │ -
111 Matrix marginalInformation(Key variable) const;
│ │ │ │ +
106 GaussianFactor::shared_ptr negateToGaussian() const;
│ │ │ │ +
107
│ │ │ │ +
111 NonlinearFactor::shared_ptr negateToNonlinear() const;
│ │ │ │
112
│ │ │ │ -
114 Matrix marginalCovariance(Key variable) const;
│ │ │ │ -
115
│ │ │ │ -
117 JointMarginal jointMarginalCovariance(const KeyVector& variables) const;
│ │ │ │ -
118
│ │ │ │ -
120 JointMarginal jointMarginalInformation(const KeyVector& variables) const;
│ │ │ │ -
121
│ │ │ │ -
123 VectorValues optimize() const;
│ │ │ │ -
124
│ │ │ │ -
125protected:
│ │ │ │ -
126
│ │ │ │ -
128 void computeBayesTree();
│ │ │ │ -
129
│ │ │ │ -
131 void computeBayesTree(const Ordering& ordering);
│ │ │ │ -
132
│ │ │ │ -
133public:
│ │ │ │ -
134#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ -
136 GTSAM_DEPRECATED Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization,
│ │ │ │ -
137 const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {}
│ │ │ │ -
138
│ │ │ │ -
140 GTSAM_DEPRECATED Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization,
│ │ │ │ -
141 const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {}
│ │ │ │ -
142
│ │ │ │ -
144 GTSAM_DEPRECATED Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization,
│ │ │ │ -
145 const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {}
│ │ │ │ -
146#endif
│ │ │ │ -
147
│ │ │ │ -
148};
│ │ │ │ -
│ │ │ │ -
149
│ │ │ │ -
│ │ │ │ -
153class GTSAM_EXPORT JointMarginal {
│ │ │ │ -
154
│ │ │ │ -
155protected:
│ │ │ │ -
156 SymmetricBlockMatrix blockMatrix_;
│ │ │ │ -
157 KeyVector keys_;
│ │ │ │ -
158 FastMap<Key, size_t> indices_;
│ │ │ │ -
159
│ │ │ │ -
160public:
│ │ │ │ -
162 JointMarginal() {}
│ │ │ │ -
163
│ │ │ │ -
│ │ │ │ -
177 Matrix operator()(Key iVariable, Key jVariable) const {
│ │ │ │ -
178 const auto indexI = indices_.at(iVariable);
│ │ │ │ -
179 const auto indexJ = indices_.at(jVariable);
│ │ │ │ -
180 return blockMatrix_.block(indexI, indexJ);
│ │ │ │ -
181 }
│ │ │ │ -
│ │ │ │ -
182
│ │ │ │ -
│ │ │ │ -
184 Matrix at(Key iVariable, Key jVariable) const {
│ │ │ │ -
185 return (*this)(iVariable, jVariable);
│ │ │ │ -
186 }
│ │ │ │ -
│ │ │ │ -
187
│ │ │ │ -
│ │ │ │ -
189 Matrix fullMatrix() const {
│ │ │ │ -
190 return blockMatrix_.selfadjointView();
│ │ │ │ -
191 }
│ │ │ │ -
│ │ │ │ -
192
│ │ │ │ -
194 void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const;
│ │ │ │ -
195
│ │ │ │ -
196protected:
│ │ │ │ -
197 JointMarginal(const Matrix& fullMatrix, const std::vector<size_t>& dims, const KeyVector& keys) :
│ │ │ │ -
198 blockMatrix_(dims, fullMatrix), keys_(keys), indices_(Ordering(keys).invert()) {}
│ │ │ │ -
199
│ │ │ │ -
200 friend class Marginals;
│ │ │ │ -
201
│ │ │ │ -
202};
│ │ │ │ -
│ │ │ │ -
203
│ │ │ │ -
204} /* namespace gtsam */
│ │ │ │ -
GaussianBayesTree.h
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
│ │ │ │ -
Values.h
A non-templated config holding any types of Manifold-group elements.
│ │ │ │ -
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │ +
│ │ │ │ +
119 NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
120 return NonlinearFactor::shared_ptr(new LinearContainerFactor(factor_,linearizationPoint_));
│ │ │ │ +
121 }
│ │ │ │ +
│ │ │ │ +
122
│ │ │ │ +
128 NonlinearFactor::shared_ptr rekey(
│ │ │ │ +
129 const std::map<Key, Key>& rekey_mapping) const override;
│ │ │ │ +
130
│ │ │ │ +
135 NonlinearFactor::shared_ptr rekey(const KeyVector& new_keys) const override;
│ │ │ │ +
136
│ │ │ │ +
138 inline bool hasLinearizationPoint() const { return linearizationPoint_.is_initialized(); }
│ │ │ │ +
139
│ │ │ │ +
143 bool isJacobian() const;
│ │ │ │ +
144 bool isHessian() const;
│ │ │ │ +
145
│ │ │ │ +
147 boost::shared_ptr<JacobianFactor> toJacobian() const;
│ │ │ │ +
148
│ │ │ │ +
150 boost::shared_ptr<HessianFactor> toHessian() const;
│ │ │ │ +
151
│ │ │ │ +
156 static NonlinearFactorGraph ConvertLinearGraph(const GaussianFactorGraph& linear_graph,
│ │ │ │ +
157 const Values& linearizationPoint = Values());
│ │ │ │ +
158
│ │ │ │ +
159 protected:
│ │ │ │ +
160 void initializeLinearizationPoint(const Values& linearizationPoint);
│ │ │ │ +
161
│ │ │ │ +
162 private:
│ │ │ │ +
164 friend class boost::serialization::access;
│ │ │ │ +
165 template<class ARCHIVE>
│ │ │ │ +
166 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
167 ar & boost::serialization::make_nvp("NonlinearFactor",
│ │ │ │ +
168 boost::serialization::base_object<Base>(*this));
│ │ │ │ +
169 ar & BOOST_SERIALIZATION_NVP(factor_);
│ │ │ │ +
170 ar & BOOST_SERIALIZATION_NVP(linearizationPoint_);
│ │ │ │ +
171 }
│ │ │ │ +
172
│ │ │ │ +
173}; // \class LinearContainerFactor
│ │ │ │ +
│ │ │ │ +
174
│ │ │ │ +
175template<> struct traits<LinearContainerFactor> : public Testable<LinearContainerFactor> {};
│ │ │ │ +
176
│ │ │ │ +
177} // \namespace gtsam
│ │ │ │ +
178
│ │ │ │ +
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ -
gtsam::SymmetricBlockMatrix
This class stores a dense matrix and allows it to be accessed as a collection of blocks.
Definition SymmetricBlockMatrix.h:52
│ │ │ │ -
gtsam::SymmetricBlockMatrix::block
Matrix block(DenseIndex I, DenseIndex J) const
Get a copy of a block (anywhere in the matrix).
Definition SymmetricBlockMatrix.cpp:60
│ │ │ │ -
gtsam::SymmetricBlockMatrix::selfadjointView
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j).
Definition SymmetricBlockMatrix.h:156
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ -
gtsam::GaussianBayesTree
A Bayes tree representing a Gaussian density.
Definition GaussianBayesTree.h:52
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactor.h:42
│ │ │ │
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ -
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ -
gtsam::Marginals
A class for computing Gaussian marginals of variables in a NonlinearFactorGraph.
Definition Marginals.h:32
│ │ │ │ -
gtsam::Marginals::Factorization
Factorization
The linear factorization mode - either CHOLESKY (faster and suitable for most problems) or QR (slower...
Definition Marginals.h:37
│ │ │ │ -
gtsam::Marginals::Marginals
Marginals()
Default constructor only for wrappers.
Definition Marginals.h:52
│ │ │ │ -
gtsam::JointMarginal
A class to store and access a joint marginal, returned from Marginals::jointMarginalCovariance and Ma...
Definition Marginals.h:153
│ │ │ │ -
gtsam::JointMarginal::at
Matrix at(Key iVariable, Key jVariable) const
Synonym for operator()
Definition Marginals.h:184
│ │ │ │ -
gtsam::JointMarginal::fullMatrix
Matrix fullMatrix() const
The full, dense covariance/information matrix of the joint marginal.
Definition Marginals.h:189
│ │ │ │ -
gtsam::JointMarginal::operator()
Matrix operator()(Key iVariable, Key jVariable) const
Access a block, corresponding to a pair of variables, of the joint marginal.
Definition Marginals.h:177
│ │ │ │ -
gtsam::JointMarginal::JointMarginal
JointMarginal()
Default constructor only for wrappers.
Definition Marginals.h:162
│ │ │ │ +
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition HessianFactor.h:101
│ │ │ │ +
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition JacobianFactor.h:91
│ │ │ │ +
gtsam::LinearContainerFactor
Dummy version of a generic linear factor to be injected into a nonlinear factor graph.
Definition LinearContainerFactor.h:26
│ │ │ │ +
gtsam::LinearContainerFactor::clone
NonlinearFactor::shared_ptr clone() const override
Creates a shared_ptr clone of the factor - needs to be specialized to allow for subclasses.
Definition LinearContainerFactor.h:119
│ │ │ │ +
gtsam::LinearContainerFactor::LinearContainerFactor
LinearContainerFactor()
Default constructor - necessary for serialization.
Definition LinearContainerFactor.h:44
│ │ │ │ +
gtsam::LinearContainerFactor::linearizationPoint
const boost::optional< Values > & linearizationPoint() const
Extract the linearization point used in recalculating error.
Definition LinearContainerFactor.h:83
│ │ │ │ +
gtsam::LinearContainerFactor::hasLinearizationPoint
bool hasLinearizationPoint() const
Casting syntactic sugar.
Definition LinearContainerFactor.h:138
│ │ │ │ +
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
VectorValues
The Factor::error simply extracts the.
│ │ │ │ +
JacobianFactor
In Gaussian factors, the error function returns either the negative log-likelihood,...
│ │ │ │
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,260 +1,185 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Marginals.h │ │ │ │ │ +LinearContainerFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -1/* --------------------------------------------------------------------------- │ │ │ │ │ -- │ │ │ │ │ -2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ -4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -5 * All Rights Reserved │ │ │ │ │ -6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ -7 │ │ │ │ │ -8 * See LICENSE for the license information │ │ │ │ │ -9 │ │ │ │ │ -10 * ------------------------------------------------------------------------- │ │ │ │ │ -- */ │ │ │ │ │ +1 │ │ │ │ │ +10#pragma once │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_V_a_l_u_e_s_._h> │ │ │ │ │ -24 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ -26 │ │ │ │ │ -27class JointMarginal; │ │ │ │ │ +12#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +13 │ │ │ │ │ +14namespace _g_t_s_a_m { │ │ │ │ │ +15 │ │ │ │ │ +16 // Forward declarations │ │ │ │ │ +17 class _J_a_c_o_b_i_a_n_F_a_c_t_o_r; │ │ │ │ │ +18 class HessianFactor; │ │ │ │ │ +19 │ │ │ │ │ +_2_6class GTSAM_EXPORT _L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r : public _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r { │ │ │ │ │ +27protected: │ │ │ │ │ 28 │ │ │ │ │ -_3_2class GTSAM_EXPORT _M_a_r_g_i_n_a_l_s { │ │ │ │ │ -33 │ │ │ │ │ -34public: │ │ │ │ │ -35 │ │ │ │ │ -_3_7 enum _F_a_c_t_o_r_i_z_a_t_i_o_n { │ │ │ │ │ -38 CHOLESKY, │ │ │ │ │ -39 QR │ │ │ │ │ -40 }; │ │ │ │ │ -41 │ │ │ │ │ -42protected: │ │ │ │ │ -43 │ │ │ │ │ -44 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h graph_; │ │ │ │ │ -45 _V_a_l_u_e_s values_; │ │ │ │ │ -46 Factorization factorization_; │ │ │ │ │ -47 _G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e bayesTree_; │ │ │ │ │ +29 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r factor_; │ │ │ │ │ +30 boost::optional linearizationPoint_; │ │ │ │ │ +31 │ │ │ │ │ +33 _L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r& factor, const │ │ │ │ │ +boost::optional& linearizationPoint); │ │ │ │ │ +34 │ │ │ │ │ +35 // Some handy typedefs │ │ │ │ │ +36 typedef _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r _B_a_s_e; │ │ │ │ │ +37 typedef _L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ +38 │ │ │ │ │ +39public: │ │ │ │ │ +40 │ │ │ │ │ +41 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +42 │ │ │ │ │ +_4_4 _L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r() {} │ │ │ │ │ +45 │ │ │ │ │ +47 _L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r(const _J_a_c_o_b_i_a_n_F_a_c_t_o_r& factor, const _V_a_l_u_e_s& │ │ │ │ │ +linearizationPoint = _V_a_l_u_e_s()); │ │ │ │ │ 48 │ │ │ │ │ -49public: │ │ │ │ │ -50 │ │ │ │ │ -_5_2 _M_a_r_g_i_n_a_l_s(){} │ │ │ │ │ -53 │ │ │ │ │ -59 _M_a_r_g_i_n_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, const _V_a_l_u_e_s& solution, │ │ │ │ │ -Factorization factorization = CHOLESKY); │ │ │ │ │ +50 _L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r(const _H_e_s_s_i_a_n_F_a_c_t_o_r& factor, const _V_a_l_u_e_s& │ │ │ │ │ +linearizationPoint = _V_a_l_u_e_s()); │ │ │ │ │ +51 │ │ │ │ │ +53 _L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r& factor, const │ │ │ │ │ +_V_a_l_u_e_s& linearizationPoint = _V_a_l_u_e_s()); │ │ │ │ │ +54 │ │ │ │ │ +55 // Access │ │ │ │ │ +56 │ │ │ │ │ +57 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r& factor() const { return factor_; } │ │ │ │ │ +58 │ │ │ │ │ +59 // Testable │ │ │ │ │ 60 │ │ │ │ │ -67 _M_a_r_g_i_n_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, const _V_a_l_u_e_s& solution, const │ │ │ │ │ -_O_r_d_e_r_i_n_g& ordering, │ │ │ │ │ -68 Factorization factorization = CHOLESKY); │ │ │ │ │ -69 │ │ │ │ │ -75 _M_a_r_g_i_n_a_l_s(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& graph, const _V_a_l_u_e_s& solution, │ │ │ │ │ -Factorization factorization = CHOLESKY); │ │ │ │ │ -76 │ │ │ │ │ -83 _M_a_r_g_i_n_a_l_s(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& graph, const _V_a_l_u_e_s& solution, const │ │ │ │ │ -_O_r_d_e_r_i_n_g& ordering, │ │ │ │ │ -84 Factorization factorization = CHOLESKY); │ │ │ │ │ -85 │ │ │ │ │ -92 _M_a_r_g_i_n_a_l_s(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& graph, const _V_e_c_t_o_r_V_a_l_u_e_s& solution, │ │ │ │ │ -Factorization factorization = CHOLESKY); │ │ │ │ │ -93 │ │ │ │ │ -100 _M_a_r_g_i_n_a_l_s(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& graph, const _V_e_c_t_o_r_V_a_l_u_e_s& solution, │ │ │ │ │ -const _O_r_d_e_r_i_n_g& ordering, │ │ │ │ │ -101 Factorization factorization = CHOLESKY); │ │ │ │ │ +62 void print(const std::string& s = "", const KeyFormatter& keyFormatter = │ │ │ │ │ +gtsam::DefaultKeyFormatter) const override; │ │ │ │ │ +63 │ │ │ │ │ +65 bool equals(const NonlinearFactor& f, double tol = 1e-9) const override; │ │ │ │ │ +66 │ │ │ │ │ +67 // NonlinearFactor │ │ │ │ │ +68 │ │ │ │ │ +77 double error(const _V_a_l_u_e_s& c) const override; │ │ │ │ │ +78 │ │ │ │ │ +80 size_t dim() const override; │ │ │ │ │ +81 │ │ │ │ │ +_8_3 const boost::optional& _l_i_n_e_a_r_i_z_a_t_i_o_n_P_o_i_n_t() const { return │ │ │ │ │ +linearizationPoint_; } │ │ │ │ │ +84 │ │ │ │ │ +101 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r linearize(const _V_a_l_u_e_s& c) const override; │ │ │ │ │ 102 │ │ │ │ │ -104 void _p_r_i_n_t(const std::string& str = "Marginals: ", const _K_e_y_F_o_r_m_a_t_t_e_r& │ │ │ │ │ -keyFormatter = DefaultKeyFormatter) const; │ │ │ │ │ -105 │ │ │ │ │ -107 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r marginalFactor(_K_e_y variable) const; │ │ │ │ │ -108 │ │ │ │ │ -111 Matrix marginalInformation(_K_e_y variable) const; │ │ │ │ │ +106 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r negateToGaussian() const; │ │ │ │ │ +107 │ │ │ │ │ +111 NonlinearFactor::shared_ptr negateToNonlinear() const; │ │ │ │ │ 112 │ │ │ │ │ -114 Matrix marginalCovariance(_K_e_y variable) const; │ │ │ │ │ -115 │ │ │ │ │ -117 _J_o_i_n_t_M_a_r_g_i_n_a_l jointMarginalCovariance(const _K_e_y_V_e_c_t_o_r& variables) const; │ │ │ │ │ -118 │ │ │ │ │ -120 _J_o_i_n_t_M_a_r_g_i_n_a_l jointMarginalInformation(const _K_e_y_V_e_c_t_o_r& variables) const; │ │ │ │ │ -121 │ │ │ │ │ -123 _V_e_c_t_o_r_V_a_l_u_e_s _o_p_t_i_m_i_z_e() const; │ │ │ │ │ -124 │ │ │ │ │ -125protected: │ │ │ │ │ -126 │ │ │ │ │ -128 void computeBayesTree(); │ │ │ │ │ -129 │ │ │ │ │ -131 void computeBayesTree(const _O_r_d_e_r_i_n_g& ordering); │ │ │ │ │ -132 │ │ │ │ │ -133public: │ │ │ │ │ -134#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ -136 GTSAM_DEPRECATED _M_a_r_g_i_n_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, const _V_a_l_u_e_s& │ │ │ │ │ -solution, Factorization factorization, │ │ │ │ │ -137 const _O_r_d_e_r_i_n_g& ordering) : _M_a_r_g_i_n_a_l_s(graph, solution, ordering, │ │ │ │ │ -factorization) {} │ │ │ │ │ -138 │ │ │ │ │ -140 GTSAM_DEPRECATED Marginals(const GaussianFactorGraph& graph, const _V_a_l_u_e_s& │ │ │ │ │ -solution, Factorization factorization, │ │ │ │ │ -141 const Ordering& ordering) : Marginals(graph, solution, ordering, │ │ │ │ │ -factorization) {} │ │ │ │ │ -142 │ │ │ │ │ -144 GTSAM_DEPRECATED Marginals(const GaussianFactorGraph& graph, const │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s& solution, Factorization factorization, │ │ │ │ │ -145 const Ordering& ordering) : Marginals(graph, solution, ordering, │ │ │ │ │ -factorization) {} │ │ │ │ │ -146#endif │ │ │ │ │ -147 │ │ │ │ │ -148}; │ │ │ │ │ -149 │ │ │ │ │ -_1_5_3class GTSAM_EXPORT _J_o_i_n_t_M_a_r_g_i_n_a_l { │ │ │ │ │ -154 │ │ │ │ │ -155protected: │ │ │ │ │ -156 _S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x blockMatrix_; │ │ │ │ │ -157 _K_e_y_V_e_c_t_o_r keys_; │ │ │ │ │ -158 _F_a_s_t_M_a_p_<_K_e_y_,_ _s_i_z_e___t_> indices_; │ │ │ │ │ -159 │ │ │ │ │ -160public: │ │ │ │ │ -_1_6_2 _J_o_i_n_t_M_a_r_g_i_n_a_l() {} │ │ │ │ │ -163 │ │ │ │ │ -_1_7_7 Matrix _o_p_e_r_a_t_o_r_(_)(_K_e_y iVariable, _K_e_y jVariable) const { │ │ │ │ │ -178 const auto indexI = indices_.at(iVariable); │ │ │ │ │ -179 const auto indexJ = indices_.at(jVariable); │ │ │ │ │ -180 return blockMatrix_._b_l_o_c_k(indexI, indexJ); │ │ │ │ │ -181 } │ │ │ │ │ -182 │ │ │ │ │ -_1_8_4 Matrix _a_t(_K_e_y iVariable, _K_e_y jVariable) const { │ │ │ │ │ -185 return (*this)(iVariable, jVariable); │ │ │ │ │ -186 } │ │ │ │ │ -187 │ │ │ │ │ -_1_8_9 Matrix _f_u_l_l_M_a_t_r_i_x() const { │ │ │ │ │ -190 return blockMatrix_._s_e_l_f_a_d_j_o_i_n_t_V_i_e_w(); │ │ │ │ │ -191 } │ │ │ │ │ -192 │ │ │ │ │ -194 void _p_r_i_n_t(const std::string& s = "", const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = │ │ │ │ │ -DefaultKeyFormatter) const; │ │ │ │ │ -195 │ │ │ │ │ -196protected: │ │ │ │ │ -197 _J_o_i_n_t_M_a_r_g_i_n_a_l(const Matrix& fullMatrix, const std::vector& dims, │ │ │ │ │ -const _K_e_y_V_e_c_t_o_r& keys) : │ │ │ │ │ -198 blockMatrix_(dims, fullMatrix), keys_(keys), indices_(_O_r_d_e_r_i_n_g(keys).invert │ │ │ │ │ -()) {} │ │ │ │ │ -199 │ │ │ │ │ -200 friend class Marginals; │ │ │ │ │ -201 │ │ │ │ │ -202}; │ │ │ │ │ -203 │ │ │ │ │ -204} /* namespace gtsam */ │ │ │ │ │ -_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ -Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ │ -_V_a_l_u_e_s_._h │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +_1_1_9 NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +120 return NonlinearFactor::shared_ptr(new _L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r │ │ │ │ │ +(factor_,linearizationPoint_)); │ │ │ │ │ +121 } │ │ │ │ │ +122 │ │ │ │ │ +128 NonlinearFactor::shared_ptr rekey( │ │ │ │ │ +129 const std::map& rekey_mapping) const override; │ │ │ │ │ +130 │ │ │ │ │ +135 NonlinearFactor::shared_ptr rekey(const _K_e_y_V_e_c_t_o_r& new_keys) const │ │ │ │ │ +override; │ │ │ │ │ +136 │ │ │ │ │ +_1_3_8 inline bool _h_a_s_L_i_n_e_a_r_i_z_a_t_i_o_n_P_o_i_n_t() const { return │ │ │ │ │ +linearizationPoint_.is_initialized(); } │ │ │ │ │ +139 │ │ │ │ │ +143 bool isJacobian() const; │ │ │ │ │ +144 bool isHessian() const; │ │ │ │ │ +145 │ │ │ │ │ +147 boost::shared_ptr toJacobian() const; │ │ │ │ │ +148 │ │ │ │ │ +150 boost::shared_ptr toHessian() const; │ │ │ │ │ +151 │ │ │ │ │ +156 static _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h ConvertLinearGraph(const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h& │ │ │ │ │ +linear_graph, │ │ │ │ │ +157 const _V_a_l_u_e_s& linearizationPoint = _V_a_l_u_e_s()); │ │ │ │ │ +158 │ │ │ │ │ +159 protected: │ │ │ │ │ +160 void initializeLinearizationPoint(const _V_a_l_u_e_s& linearizationPoint); │ │ │ │ │ +161 │ │ │ │ │ +162 private: │ │ │ │ │ +_1_6_4 friend class boost::serialization::access; │ │ │ │ │ +165 template │ │ │ │ │ +166 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +167 ar & boost::serialization::make_nvp("NonlinearFactor", │ │ │ │ │ +168 boost::serialization::base_object(*this)); │ │ │ │ │ +169 ar & BOOST_SERIALIZATION_NVP(factor_); │ │ │ │ │ +170 ar & BOOST_SERIALIZATION_NVP(linearizationPoint_); │ │ │ │ │ +171 } │ │ │ │ │ +172 │ │ │ │ │ +173}; // \class LinearContainerFactor │ │ │ │ │ +174 │ │ │ │ │ +_1_7_5template<> struct _t_r_a_i_t_s<_L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r> : public │ │ │ │ │ +_T_e_s_t_a_b_l_e {}; │ │ │ │ │ +176 │ │ │ │ │ +177} // \namespace gtsam │ │ │ │ │ +178 │ │ │ │ │ _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ Factor Graph consisting of non-linear factors. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ FastVector< Key > KeyVector │ │ │ │ │ Define collection type once and for all - also used in wrappers. │ │ │ │ │ DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ -Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key │ │ │ │ │ -landmarkKey) │ │ │ │ │ -Optimize for triangulation. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.cpp:155 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ -FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ -fast_pool_allocator instead of the defa... │ │ │ │ │ -DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x │ │ │ │ │ -This class stores a dense matrix and allows it to be accessed as a collection │ │ │ │ │ -of blocks. │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_b_l_o_c_k │ │ │ │ │ -Matrix block(DenseIndex I, DenseIndex J) const │ │ │ │ │ -Get a copy of a block (anywhere in the matrix). │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.cpp:60 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_m_e_t_r_i_c_B_l_o_c_k_M_a_t_r_i_x_:_:_s_e_l_f_a_d_j_o_i_n_t_V_i_e_w │ │ │ │ │ -Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex │ │ │ │ │ -I, DenseIndex J) const │ │ │ │ │ -Return the square sub-matrix that contains blocks(i:j, i:j). │ │ │ │ │ -DDeeffiinniittiioonn SymmetricBlockMatrix.h:156 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e │ │ │ │ │ -A Bayes tree representing a Gaussian density. │ │ │ │ │ -DDeeffiinniittiioonn GaussianBayesTree.h:52 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ +A helper that implements the traits interface for GTSAM types. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ boost::shared_ptr< This > shared_ptr │ │ │ │ │ shared_ptr to this class │ │ │ │ │ DDeeffiinniittiioonn GaussianFactor.h:42 │ │ │ │ │ _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ -with a unique integer... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_r_g_i_n_a_l_s │ │ │ │ │ -A class for computing Gaussian marginals of variables in a │ │ │ │ │ -NonlinearFactorGraph. │ │ │ │ │ -DDeeffiinniittiioonn Marginals.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_r_g_i_n_a_l_s_:_:_F_a_c_t_o_r_i_z_a_t_i_o_n │ │ │ │ │ -Factorization │ │ │ │ │ -The linear factorization mode - either CHOLESKY (faster and suitable for most │ │ │ │ │ -problems) or QR (slower... │ │ │ │ │ -DDeeffiinniittiioonn Marginals.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_r_g_i_n_a_l_s_:_:_M_a_r_g_i_n_a_l_s │ │ │ │ │ -Marginals() │ │ │ │ │ -Default constructor only for wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Marginals.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_J_o_i_n_t_M_a_r_g_i_n_a_l │ │ │ │ │ -A class to store and access a joint marginal, returned from Marginals:: │ │ │ │ │ -jointMarginalCovariance and Ma... │ │ │ │ │ -DDeeffiinniittiioonn Marginals.h:153 │ │ │ │ │ -_g_t_s_a_m_:_:_J_o_i_n_t_M_a_r_g_i_n_a_l_:_:_a_t │ │ │ │ │ -Matrix at(Key iVariable, Key jVariable) const │ │ │ │ │ -Synonym for operator() │ │ │ │ │ -DDeeffiinniittiioonn Marginals.h:184 │ │ │ │ │ -_g_t_s_a_m_:_:_J_o_i_n_t_M_a_r_g_i_n_a_l_:_:_f_u_l_l_M_a_t_r_i_x │ │ │ │ │ -Matrix fullMatrix() const │ │ │ │ │ -The full, dense covariance/information matrix of the joint marginal. │ │ │ │ │ -DDeeffiinniittiioonn Marginals.h:189 │ │ │ │ │ -_g_t_s_a_m_:_:_J_o_i_n_t_M_a_r_g_i_n_a_l_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ -Matrix operator()(Key iVariable, Key jVariable) const │ │ │ │ │ -Access a block, corresponding to a pair of variables, of the joint marginal. │ │ │ │ │ -DDeeffiinniittiioonn Marginals.h:177 │ │ │ │ │ -_g_t_s_a_m_:_:_J_o_i_n_t_M_a_r_g_i_n_a_l_:_:_J_o_i_n_t_M_a_r_g_i_n_a_l │ │ │ │ │ -JointMarginal() │ │ │ │ │ -Default constructor only for wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Marginals.h:162 │ │ │ │ │ +_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ +A Gaussian factor using the canonical parameters (information form) │ │ │ │ │ +DDeeffiinniittiioonn HessianFactor.h:101 │ │ │ │ │ +_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ +A Gaussian factor in the squared-error form. │ │ │ │ │ +DDeeffiinniittiioonn JacobianFactor.h:91 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r │ │ │ │ │ +Dummy version of a generic linear factor to be injected into a nonlinear factor │ │ │ │ │ +graph. │ │ │ │ │ +DDeeffiinniittiioonn LinearContainerFactor.h:26 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ +NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +Creates a shared_ptr clone of the factor - needs to be specialized to allow for │ │ │ │ │ +subclasses. │ │ │ │ │ +DDeeffiinniittiioonn LinearContainerFactor.h:119 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r_:_:_L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r │ │ │ │ │ +LinearContainerFactor() │ │ │ │ │ +Default constructor - necessary for serialization. │ │ │ │ │ +DDeeffiinniittiioonn LinearContainerFactor.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_a_t_i_o_n_P_o_i_n_t │ │ │ │ │ +const boost::optional< Values > & linearizationPoint() const │ │ │ │ │ +Extract the linearization point used in recalculating error. │ │ │ │ │ +DDeeffiinniittiioonn LinearContainerFactor.h:83 │ │ │ │ │ +_g_t_s_a_m_:_:_L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r_:_:_h_a_s_L_i_n_e_a_r_i_z_a_t_i_o_n_P_o_i_n_t │ │ │ │ │ +bool hasLinearizationPoint() const │ │ │ │ │ +Casting syntactic sugar. │ │ │ │ │ +DDeeffiinniittiioonn LinearContainerFactor.h:138 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ +Nonlinear factor base class. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ _g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -The Factor::error simply extracts the. │ │ │ │ │ +_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ +In Gaussian factors, the error function returns either the negative log- │ │ │ │ │ +likelihood,... │ │ │ │ │ _V_a_l_u_e_s │ │ │ │ │ In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ a non-linear function... │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _M_a_r_g_i_n_a_l_s_._h │ │ │ │ │ + * _L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01076_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00206_source.html │ │ │ │┄ Files 93% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/DoglegOptimizer.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/discrete/Signature.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
DoglegOptimizer.h
│ │ │ │ +
Signature.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/nonlinear/NonlinearOptimizer.h>
│ │ │ │ -
22
│ │ │ │ -
23namespace gtsam {
│ │ │ │ +
20#include <string>
│ │ │ │ +
21#include <vector>
│ │ │ │ +
22#include <boost/optional.hpp>
│ │ │ │ +
23#include <gtsam/discrete/DiscreteKey.h>
│ │ │ │
24
│ │ │ │ -
25class DoglegOptimizer;
│ │ │ │ +
25namespace gtsam {
│ │ │ │
26
│ │ │ │ -
│ │ │ │ -
32class GTSAM_EXPORT DoglegParams : public NonlinearOptimizerParams {
│ │ │ │ -
33public:
│ │ │ │ -
│ │ │ │ -
35 enum VerbosityDL {
│ │ │ │ -
36 SILENT,
│ │ │ │ -
37 VERBOSE
│ │ │ │ -
38 };
│ │ │ │ -
│ │ │ │ -
39
│ │ │ │ -
40 double deltaInitial;
│ │ │ │ -
41 VerbosityDL verbosityDL;
│ │ │ │ -
42
│ │ │ │ -
43 DoglegParams() :
│ │ │ │ -
44 deltaInitial(1.0), verbosityDL(SILENT) {}
│ │ │ │ -
45
│ │ │ │ -
46 ~DoglegParams() override {}
│ │ │ │ -
47
│ │ │ │ -
48 void print(const std::string& str = "") const override {
│ │ │ │ -
49 NonlinearOptimizerParams::print(str);
│ │ │ │ -
50 std::cout << " deltaInitial: " << deltaInitial << "\n";
│ │ │ │ -
51 std::cout.flush();
│ │ │ │ -
52 }
│ │ │ │ -
53
│ │ │ │ -
54 double getDeltaInitial() const { return deltaInitial; }
│ │ │ │ -
55 std::string getVerbosityDL() const { return verbosityDLTranslator(verbosityDL); }
│ │ │ │ -
56
│ │ │ │ -
57 void setDeltaInitial(double deltaInitial) { this->deltaInitial = deltaInitial; }
│ │ │ │ -
58 void setVerbosityDL(const std::string& verbosityDL) { this->verbosityDL = verbosityDLTranslator(verbosityDL); }
│ │ │ │ -
59
│ │ │ │ -
60private:
│ │ │ │ -
61 VerbosityDL verbosityDLTranslator(const std::string& verbosityDL) const;
│ │ │ │ -
62 std::string verbosityDLTranslator(VerbosityDL verbosityDL) const;
│ │ │ │ -
63};
│ │ │ │ -
│ │ │ │ -
64
│ │ │ │ -
│ │ │ │ -
68class GTSAM_EXPORT DoglegOptimizer : public NonlinearOptimizer {
│ │ │ │ +
│ │ │ │ +
54 class GTSAM_EXPORT Signature {
│ │ │ │ +
55
│ │ │ │ +
56 public:
│ │ │ │ +
57
│ │ │ │ +
59 typedef std::vector<double> Row;
│ │ │ │ +
60 typedef std::vector<Row> Table;
│ │ │ │ +
61
│ │ │ │ +
62 private:
│ │ │ │ +
63
│ │ │ │ +
65 DiscreteKey key_;
│ │ │ │ +
66
│ │ │ │ +
68 DiscreteKeys parents_;
│ │ │ │
69
│ │ │ │ -
70protected:
│ │ │ │ -
71 DoglegParams params_;
│ │ │ │ +
70 // the given CPT specification string
│ │ │ │ +
71 boost::optional<std::string> spec_;
│ │ │ │
72
│ │ │ │ -
73public:
│ │ │ │ -
74 typedef boost::shared_ptr<DoglegOptimizer> shared_ptr;
│ │ │ │ +
73 // the CPT as parsed, if successful
│ │ │ │ +
74 boost::optional<Table> table_;
│ │ │ │
75
│ │ │ │ -
78
│ │ │ │ -
87 DoglegOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
│ │ │ │ -
88 const DoglegParams& params = DoglegParams());
│ │ │ │ -
89
│ │ │ │ -
97 DoglegOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
│ │ │ │ -
98 const Ordering& ordering);
│ │ │ │ -
99
│ │ │ │ -
101
│ │ │ │ +
76 public:
│ │ │ │ +
88 Signature(const DiscreteKey& key, const DiscreteKeys& parents,
│ │ │ │ +
89 const Table& table);
│ │ │ │ +
90
│ │ │ │ +
102 Signature(const DiscreteKey& key, const DiscreteKeys& parents,
│ │ │ │ +
103 const std::string& spec);
│ │ │ │
104
│ │ │ │ -
106 ~DoglegOptimizer() override {}
│ │ │ │ -
107
│ │ │ │ -
112 GaussianFactorGraph::shared_ptr iterate() override;
│ │ │ │ +
112 Signature(const DiscreteKey& key);
│ │ │ │
113
│ │ │ │ -
115 const DoglegParams& params() const { return params_; }
│ │ │ │ +
115 const DiscreteKey& key() const { return key_; }
│ │ │ │
116
│ │ │ │ -
118 double getDelta() const;
│ │ │ │ +
118 const DiscreteKeys& parents() const { return parents_; }
│ │ │ │
119
│ │ │ │ -
121
│ │ │ │ -
122protected:
│ │ │ │ -
124 const NonlinearOptimizerParams& _params() const override { return params_; }
│ │ │ │ +
121 DiscreteKeys discreteKeys() const;
│ │ │ │ +
122
│ │ │ │ +
124 KeyVector indices() const;
│ │ │ │
125
│ │ │ │ -
127 DoglegParams ensureHasOrdering(DoglegParams params, const NonlinearFactorGraph& graph) const;
│ │ │ │ -
128};
│ │ │ │ -
│ │ │ │ -
129
│ │ │ │ -
130}
│ │ │ │ -
NonlinearOptimizer.h
Base class and parameters for nonlinear optimization algorithms.
│ │ │ │ +
126 // the CPT as parsed, if successful
│ │ │ │ +
127 const boost::optional<Table>& table() const { return table_; }
│ │ │ │ +
128
│ │ │ │ +
129 // the CPT as a vector of doubles, with key's values most rapidly changing
│ │ │ │ +
130 std::vector<double> cpt() const;
│ │ │ │ +
131
│ │ │ │ +
133 Signature& operator,(const DiscreteKey& parent);
│ │ │ │ +
134
│ │ │ │ +
136 Signature& operator=(const std::string& spec);
│ │ │ │ +
137
│ │ │ │ +
139 Signature& operator=(const Table& table);
│ │ │ │ +
140
│ │ │ │ +
142 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
│ │ │ │ +
143 const Signature& s);
│ │ │ │ +
144 };
│ │ │ │ +
│ │ │ │ +
145
│ │ │ │ +
150 GTSAM_EXPORT Signature operator|(const DiscreteKey& key, const DiscreteKey& parent);
│ │ │ │ +
151
│ │ │ │ +
156 GTSAM_EXPORT Signature operator%(const DiscreteKey& key, const std::string& parent);
│ │ │ │ +
157
│ │ │ │ +
162 GTSAM_EXPORT Signature operator%(const DiscreteKey& key, const Signature::Table& parent);
│ │ │ │ +
163
│ │ │ │ +
164}
│ │ │ │ +
DiscreteKey.h
specialized key for discrete variables
│ │ │ │ +
gtsam::DiscreteKey
std::pair< Key, size_t > DiscreteKey
Key type for discrete variables.
Definition DiscreteKey.h:36
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ -
gtsam::GaussianFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactorGraph.h:81
│ │ │ │ -
gtsam::DoglegParams
Parameters for Levenberg-Marquardt optimization.
Definition DoglegOptimizer.h:32
│ │ │ │ -
gtsam::DoglegParams::VerbosityDL
VerbosityDL
See DoglegParams::dlVerbosity.
Definition DoglegOptimizer.h:35
│ │ │ │ -
gtsam::DoglegParams::verbosityDL
VerbosityDL verbosityDL
The verbosity level for Dogleg (default: SILENT), see also NonlinearOptimizerParams::verbosity.
Definition DoglegOptimizer.h:41
│ │ │ │ -
gtsam::DoglegParams::deltaInitial
double deltaInitial
The initial trust region radius (default: 10.0)
Definition DoglegOptimizer.h:40
│ │ │ │ -
gtsam::DoglegOptimizer
This class performs Dogleg nonlinear optimization.
Definition DoglegOptimizer.h:68
│ │ │ │ -
gtsam::DoglegOptimizer::~DoglegOptimizer
~DoglegOptimizer() override
Virtual destructor.
Definition DoglegOptimizer.h:106
│ │ │ │ -
gtsam::DoglegOptimizer::_params
const NonlinearOptimizerParams & _params() const override
Access the parameters (base class version)
Definition DoglegOptimizer.h:124
│ │ │ │ -
gtsam::DoglegOptimizer::params
const DoglegParams & params() const
Read-only access the parameters.
Definition DoglegOptimizer.h:115
│ │ │ │ -
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │ -
gtsam::NonlinearOptimizer
This is the abstract interface for classes that can optimize for the maximum-likelihood estimate of a...
Definition NonlinearOptimizer.h:75
│ │ │ │ -
gtsam::NonlinearOptimizerParams
The common parameters for Nonlinear optimizers.
Definition NonlinearOptimizerParams.h:34
│ │ │ │ -
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::operator|
Signature operator|(const DiscreteKey &key, const DiscreteKey &parent)
Helper function to create Signature objects example: Signature s = D | E;.
Definition Signature.cpp:178
│ │ │ │ +
gtsam::operator%
Signature operator%(const DiscreteKey &key, const string &parent)
Helper function to create Signature objects example: Signature s(D % "99/1");.
Definition Signature.cpp:183
│ │ │ │ +
gtsam::DiscreteKeys
DiscreteKeys is a set of keys that can be assembled using the & operator.
Definition DiscreteKey.h:39
│ │ │ │ +
gtsam::Signature
Signature for a discrete conditional density, used to construct conditionals.
Definition Signature.h:54
│ │ │ │ +
gtsam::Signature::Row
std::vector< double > Row
Data type for the CPT.
Definition Signature.h:59
│ │ │ │ +
gtsam::Signature::key
const DiscreteKey & key() const
the variable key
Definition Signature.h:115
│ │ │ │ +
gtsam::Signature::parents
const DiscreteKeys & parents() const
the parent keys
Definition Signature.h:118
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DoglegOptimizer.h │ │ │ │ │ +Signature.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ @@ -15,144 +15,119 @@ │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_._h> │ │ │ │ │ -22 │ │ │ │ │ -23namespace _g_t_s_a_m { │ │ │ │ │ +20#include │ │ │ │ │ +21#include │ │ │ │ │ +22#include │ │ │ │ │ +23#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_K_e_y_._h> │ │ │ │ │ 24 │ │ │ │ │ -25class DoglegOptimizer; │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ 26 │ │ │ │ │ -_3_2class GTSAM_EXPORT _D_o_g_l_e_g_P_a_r_a_m_s : public _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s { │ │ │ │ │ -33public: │ │ │ │ │ -_3_5 enum _V_e_r_b_o_s_i_t_y_D_L { │ │ │ │ │ -36 SILENT, │ │ │ │ │ -37 VERBOSE │ │ │ │ │ -38 }; │ │ │ │ │ -39 │ │ │ │ │ -_4_0 double _d_e_l_t_a_I_n_i_t_i_a_l; │ │ │ │ │ -_4_1 _V_e_r_b_o_s_i_t_y_D_L _v_e_r_b_o_s_i_t_y_D_L; │ │ │ │ │ -42 │ │ │ │ │ -43 _D_o_g_l_e_g_P_a_r_a_m_s() : │ │ │ │ │ -44 deltaInitial(1.0), verbosityDL(SILENT) {} │ │ │ │ │ -45 │ │ │ │ │ -46 _~_D_o_g_l_e_g_P_a_r_a_m_s() override {} │ │ │ │ │ -47 │ │ │ │ │ -48 void print(const std::string& str = "") const override { │ │ │ │ │ -49 NonlinearOptimizerParams::print(str); │ │ │ │ │ -50 std::cout << " deltaInitial: " << deltaInitial << "\n"; │ │ │ │ │ -51 std::cout.flush(); │ │ │ │ │ -52 } │ │ │ │ │ -53 │ │ │ │ │ -54 double getDeltaInitial() const { return deltaInitial; } │ │ │ │ │ -55 std::string getVerbosityDL() const { return verbosityDLTranslator │ │ │ │ │ -(verbosityDL); } │ │ │ │ │ -56 │ │ │ │ │ -57 void setDeltaInitial(double deltaInitial) { this->deltaInitial = │ │ │ │ │ -deltaInitial; } │ │ │ │ │ -58 void setVerbosityDL(const std::string& verbosityDL) { this->verbosityDL = │ │ │ │ │ -verbosityDLTranslator(verbosityDL); } │ │ │ │ │ -59 │ │ │ │ │ -60private: │ │ │ │ │ -61 VerbosityDL verbosityDLTranslator(const std::string& verbosityDL) const; │ │ │ │ │ -62 std::string verbosityDLTranslator(VerbosityDL verbosityDL) const; │ │ │ │ │ -63}; │ │ │ │ │ -64 │ │ │ │ │ -_6_8class GTSAM_EXPORT _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r : public _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r { │ │ │ │ │ +_5_4 class GTSAM_EXPORT _S_i_g_n_a_t_u_r_e { │ │ │ │ │ +55 │ │ │ │ │ +56 public: │ │ │ │ │ +57 │ │ │ │ │ +_5_9 typedef std::vector _R_o_w; │ │ │ │ │ +60 typedef std::vector Table; │ │ │ │ │ +61 │ │ │ │ │ +62 private: │ │ │ │ │ +63 │ │ │ │ │ +65 _D_i_s_c_r_e_t_e_K_e_y key_; │ │ │ │ │ +66 │ │ │ │ │ +68 _D_i_s_c_r_e_t_e_K_e_y_s parents_; │ │ │ │ │ 69 │ │ │ │ │ -70protected: │ │ │ │ │ -71 _D_o_g_l_e_g_P_a_r_a_m_s params_; │ │ │ │ │ +70 // the given CPT specification string │ │ │ │ │ +71 boost::optional spec_; │ │ │ │ │ 72 │ │ │ │ │ -73public: │ │ │ │ │ -74 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +73 // the CPT as parsed, if successful │ │ │ │ │ +74 boost::optional table_; │ │ │ │ │ 75 │ │ │ │ │ -78 │ │ │ │ │ -87 _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, const _V_a_l_u_e_s& │ │ │ │ │ -initialValues, │ │ │ │ │ -88 const _D_o_g_l_e_g_P_a_r_a_m_s& params = _D_o_g_l_e_g_P_a_r_a_m_s()); │ │ │ │ │ -89 │ │ │ │ │ -97 _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, const _V_a_l_u_e_s& │ │ │ │ │ -initialValues, │ │ │ │ │ -98 const _O_r_d_e_r_i_n_g& ordering); │ │ │ │ │ -99 │ │ │ │ │ -101 │ │ │ │ │ +76 public: │ │ │ │ │ +88 _S_i_g_n_a_t_u_r_e(const _D_i_s_c_r_e_t_e_K_e_y& key, const _D_i_s_c_r_e_t_e_K_e_y_s& parents, │ │ │ │ │ +89 const Table& table); │ │ │ │ │ +90 │ │ │ │ │ +102 _S_i_g_n_a_t_u_r_e(const _D_i_s_c_r_e_t_e_K_e_y& key, const _D_i_s_c_r_e_t_e_K_e_y_s& parents, │ │ │ │ │ +103 const std::string& spec); │ │ │ │ │ 104 │ │ │ │ │ -_1_0_6 _~_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r() override {} │ │ │ │ │ -107 │ │ │ │ │ -112 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r iterate() override; │ │ │ │ │ +112 _S_i_g_n_a_t_u_r_e(const _D_i_s_c_r_e_t_e_K_e_y& key); │ │ │ │ │ 113 │ │ │ │ │ -_1_1_5 const _D_o_g_l_e_g_P_a_r_a_m_s& _p_a_r_a_m_s() const { return params_; } │ │ │ │ │ +_1_1_5 const _D_i_s_c_r_e_t_e_K_e_y& _k_e_y() const { return key_; } │ │ │ │ │ 116 │ │ │ │ │ -118 double getDelta() const; │ │ │ │ │ +_1_1_8 const _D_i_s_c_r_e_t_e_K_e_y_s& _p_a_r_e_n_t_s() const { return parents_; } │ │ │ │ │ 119 │ │ │ │ │ -121 │ │ │ │ │ -122protected: │ │ │ │ │ -_1_2_4 const _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s& ___p_a_r_a_m_s() const override { return params_; │ │ │ │ │ -} │ │ │ │ │ +121 _D_i_s_c_r_e_t_e_K_e_y_s discreteKeys() const; │ │ │ │ │ +122 │ │ │ │ │ +124 _K_e_y_V_e_c_t_o_r indices() const; │ │ │ │ │ 125 │ │ │ │ │ -127 _D_o_g_l_e_g_P_a_r_a_m_s ensureHasOrdering(_D_o_g_l_e_g_P_a_r_a_m_s params, const │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph) const; │ │ │ │ │ -128}; │ │ │ │ │ -129 │ │ │ │ │ -130} │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ -Base class and parameters for nonlinear optimization algorithms. │ │ │ │ │ +126 // the CPT as parsed, if successful │ │ │ │ │ +127 const boost::optional
& table() const { return table_; } │ │ │ │ │ +128 │ │ │ │ │ +129 // the CPT as a vector of doubles, with key's values most rapidly changing │ │ │ │ │ +130 std::vector cpt() const; │ │ │ │ │ +131 │ │ │ │ │ +133 Signature& operator,(const DiscreteKey& parent); │ │ │ │ │ +134 │ │ │ │ │ +136 Signature& operator=(const std::string& spec); │ │ │ │ │ +137 │ │ │ │ │ +139 Signature& operator=(const Table& table); │ │ │ │ │ +140 │ │ │ │ │ +142 GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, │ │ │ │ │ +143 const Signature& s); │ │ │ │ │ +144 }; │ │ │ │ │ +145 │ │ │ │ │ +150 GTSAM_EXPORT Signature _o_p_e_r_a_t_o_r_|(const _D_i_s_c_r_e_t_e_K_e_y& key, const _D_i_s_c_r_e_t_e_K_e_y& │ │ │ │ │ +parent); │ │ │ │ │ +151 │ │ │ │ │ +156 GTSAM_EXPORT Signature _o_p_e_r_a_t_o_r_%(const _D_i_s_c_r_e_t_e_K_e_y& key, const std::string& │ │ │ │ │ +parent); │ │ │ │ │ +157 │ │ │ │ │ +162 GTSAM_EXPORT Signature _o_p_e_r_a_t_o_r_%(const _D_i_s_c_r_e_t_e_K_e_y& key, const Signature:: │ │ │ │ │ +Table& parent); │ │ │ │ │ +163 │ │ │ │ │ +164} │ │ │ │ │ +_D_i_s_c_r_e_t_e_K_e_y_._h │ │ │ │ │ +specialized key for discrete variables │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y │ │ │ │ │ +std::pair< Key, size_t > DiscreteKey │ │ │ │ │ +Key type for discrete variables. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteKey.h:36 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:81 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_g_l_e_g_P_a_r_a_m_s │ │ │ │ │ -Parameters for Levenberg-Marquardt optimization. │ │ │ │ │ -DDeeffiinniittiioonn DoglegOptimizer.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_g_l_e_g_P_a_r_a_m_s_:_:_V_e_r_b_o_s_i_t_y_D_L │ │ │ │ │ -VerbosityDL │ │ │ │ │ -See DoglegParams::dlVerbosity. │ │ │ │ │ -DDeeffiinniittiioonn DoglegOptimizer.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_g_l_e_g_P_a_r_a_m_s_:_:_v_e_r_b_o_s_i_t_y_D_L │ │ │ │ │ -VerbosityDL verbosityDL │ │ │ │ │ -The verbosity level for Dogleg (default: SILENT), see also │ │ │ │ │ -NonlinearOptimizerParams::verbosity. │ │ │ │ │ -DDeeffiinniittiioonn DoglegOptimizer.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_g_l_e_g_P_a_r_a_m_s_:_:_d_e_l_t_a_I_n_i_t_i_a_l │ │ │ │ │ -double deltaInitial │ │ │ │ │ -The initial trust region radius (default: 10.0) │ │ │ │ │ -DDeeffiinniittiioonn DoglegOptimizer.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r │ │ │ │ │ -This class performs Dogleg nonlinear optimization. │ │ │ │ │ -DDeeffiinniittiioonn DoglegOptimizer.h:68 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_:_:_~_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r │ │ │ │ │ -~DoglegOptimizer() override │ │ │ │ │ -Virtual destructor. │ │ │ │ │ -DDeeffiinniittiioonn DoglegOptimizer.h:106 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_:_:___p_a_r_a_m_s │ │ │ │ │ -const NonlinearOptimizerParams & _params() const override │ │ │ │ │ -Access the parameters (base class version) │ │ │ │ │ -DDeeffiinniittiioonn DoglegOptimizer.h:124 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_:_:_p_a_r_a_m_s │ │ │ │ │ -const DoglegParams & params() const │ │ │ │ │ -Read-only access the parameters. │ │ │ │ │ -DDeeffiinniittiioonn DoglegOptimizer.h:115 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r │ │ │ │ │ -This is the abstract interface for classes that can optimize for the maximum- │ │ │ │ │ -likelihood estimate of a... │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizer.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s │ │ │ │ │ -The common parameters for Nonlinear optimizers. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizerParams.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_| │ │ │ │ │ +Signature operator|(const DiscreteKey &key, const DiscreteKey &parent) │ │ │ │ │ +Helper function to create Signature objects example: Signature s = D | E;. │ │ │ │ │ +DDeeffiinniittiioonn Signature.cpp:178 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_% │ │ │ │ │ +Signature operator%(const DiscreteKey &key, const string &parent) │ │ │ │ │ +Helper function to create Signature objects example: Signature s(D % "99/1");. │ │ │ │ │ +DDeeffiinniittiioonn Signature.cpp:183 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_K_e_y_s │ │ │ │ │ +DiscreteKeys is a set of keys that can be assembled using the & operator. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteKey.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_S_i_g_n_a_t_u_r_e │ │ │ │ │ +Signature for a discrete conditional density, used to construct conditionals. │ │ │ │ │ +DDeeffiinniittiioonn Signature.h:54 │ │ │ │ │ +_g_t_s_a_m_:_:_S_i_g_n_a_t_u_r_e_:_:_R_o_w │ │ │ │ │ +std::vector< double > Row │ │ │ │ │ +Data type for the CPT. │ │ │ │ │ +DDeeffiinniittiioonn Signature.h:59 │ │ │ │ │ +_g_t_s_a_m_:_:_S_i_g_n_a_t_u_r_e_:_:_k_e_y │ │ │ │ │ +const DiscreteKey & key() const │ │ │ │ │ +the variable key │ │ │ │ │ +DDeeffiinniittiioonn Signature.h:115 │ │ │ │ │ +_g_t_s_a_m_:_:_S_i_g_n_a_t_u_r_e_:_:_p_a_r_e_n_t_s │ │ │ │ │ +const DiscreteKeys & parents() const │ │ │ │ │ +the parent keys │ │ │ │ │ +DDeeffiinniittiioonn Signature.h:118 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ + * _d_i_s_c_r_e_t_e │ │ │ │ │ + * _S_i_g_n_a_t_u_r_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01088_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01247_source.html │ │ │ │┄ Files 90% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/WhiteNoiseFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/PoseRotationPrior.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
WhiteNoiseFactor.h
│ │ │ │ +
PoseRotationPrior.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ -
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ -
4 * Atlanta, Georgia 30332-0415
│ │ │ │ -
5 * All Rights Reserved
│ │ │ │ -
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ -
7
│ │ │ │ -
8 * See LICENSE for the license information
│ │ │ │ -
9
│ │ │ │ -
10 * -------------------------------------------------------------------------- */
│ │ │ │ +Go to the documentation of this file.
1
│ │ │ │ +
10#pragma once
│ │ │ │
11
│ │ │ │ -
20#pragma once
│ │ │ │ +
12#include <gtsam/geometry/concepts.h>
│ │ │ │ +
13#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
14
│ │ │ │ +
15
│ │ │ │ +
16namespace gtsam {
│ │ │ │ +
17
│ │ │ │ +
18template<class POSE>
│ │ │ │ +
│ │ │ │ +
19class PoseRotationPrior : public NoiseModelFactorN<POSE> {
│ │ │ │ +
20public:
│ │ │ │
21
│ │ │ │ -
22#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
23#include <gtsam/linear/HessianFactor.h>
│ │ │ │ -
24#include <cmath>
│ │ │ │ -
25
│ │ │ │ -
26namespace gtsam {
│ │ │ │ +
22 typedef PoseRotationPrior<POSE> This;
│ │ │ │ +
23 typedef NoiseModelFactorN<POSE> Base;
│ │ │ │ +
24 typedef POSE Pose;
│ │ │ │ +
25 typedef typename POSE::Translation Translation;
│ │ │ │ +
26 typedef typename POSE::Rotation Rotation;
│ │ │ │
27
│ │ │ │ -
28 const double logSqrt2PI = log(std::sqrt(2.0 * M_PI));
│ │ │ │ -
29
│ │ │ │ -
│ │ │ │ -
41 class WhiteNoiseFactor: public NonlinearFactor {
│ │ │ │ -
42
│ │ │ │ -
43 private:
│ │ │ │ +
28 GTSAM_CONCEPT_POSE_TYPE(Pose)
│ │ │ │ +
29 GTSAM_CONCEPT_GROUP_TYPE(Pose)
│ │ │ │ +
30 GTSAM_CONCEPT_LIE_TYPE(Rotation)
│ │ │ │ +
31
│ │ │ │ +
32 // Get dimensions of pose and rotation type at compile time
│ │ │ │ +
33 static const int xDim = FixedDimension<POSE>::value;
│ │ │ │ +
34 static const int rDim = FixedDimension<typename POSE::Rotation>::value;
│ │ │ │ +
35
│ │ │ │ +
36protected:
│ │ │ │ +
37
│ │ │ │ +
38 Rotation measured_;
│ │ │ │ +
39
│ │ │ │ +
40public:
│ │ │ │ +
41
│ │ │ │ +
43 PoseRotationPrior() {}
│ │ │ │
44
│ │ │ │ -
45 double z_;
│ │ │ │ -
46
│ │ │ │ -
47 Key meanKey_;
│ │ │ │ -
48 Key precisionKey_;
│ │ │ │ -
49
│ │ │ │ -
50 typedef NonlinearFactor Base;
│ │ │ │ -
51
│ │ │ │ -
52 public:
│ │ │ │ -
53
│ │ │ │ -
│ │ │ │ -
61 static double f(double z, double u, double p) {
│ │ │ │ -
62 return logSqrt2PI - 0.5 * log(p) + 0.5 * (z - u) * (z - u) * p;
│ │ │ │ -
63 }
│ │ │ │ -
│ │ │ │ +
│ │ │ │ +
46 PoseRotationPrior(Key key, const Rotation& rot_z, const SharedNoiseModel& model)
│ │ │ │ +
47 : Base(model, key), measured_(rot_z) {}
│ │ │ │ +
│ │ │ │ +
48
│ │ │ │ +
│ │ │ │ +
50 PoseRotationPrior(Key key, const POSE& pose_z, const SharedNoiseModel& model)
│ │ │ │ +
51 : Base(model, key), measured_(pose_z.rotation()) {}
│ │ │ │ +
│ │ │ │ +
52
│ │ │ │ +
53 ~PoseRotationPrior() override {}
│ │ │ │ +
54
│ │ │ │ +
│ │ │ │ +
56 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
57 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ +
58 gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
│ │ │ │ +
│ │ │ │ +
59
│ │ │ │ +
60 // access
│ │ │ │ +
61 const Rotation& measured() const { return measured_; }
│ │ │ │ +
62
│ │ │ │ +
63 // testable
│ │ │ │
64
│ │ │ │ -
│ │ │ │ -
75 static HessianFactor::shared_ptr linearize(double z, double u, double p,
│ │ │ │ -
76 Key j1, Key j2) {
│ │ │ │ -
77 double e = u - z, e2 = e * e;
│ │ │ │ -
78 double c = 2 * logSqrt2PI - log(p) + e2 * p;
│ │ │ │ -
79 Vector g1 = (Vector(1) << -e * p).finished();
│ │ │ │ -
80 Vector g2 = (Vector(1) << 0.5 / p - 0.5 * e2).finished();
│ │ │ │ -
81 Matrix G11 = (Matrix(1, 1) << p).finished();
│ │ │ │ -
82 Matrix G12 = (Matrix(1, 1) << e).finished();
│ │ │ │ -
83 Matrix G22 = (Matrix(1, 1) << 0.5 / (p * p)).finished();
│ │ │ │ -
84 return HessianFactor::shared_ptr(
│ │ │ │ -
85 new HessianFactor(j1, j2, G11, G12, g1, G22, g2, c));
│ │ │ │ -
86 }
│ │ │ │ +
│ │ │ │ +
66 bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
│ │ │ │ +
67 const This *e = dynamic_cast<const This*> (&expected);
│ │ │ │ +
68 return e != nullptr && Base::equals(*e, tol) && measured_.equals(e->measured_, tol);
│ │ │ │ +
69 }
│ │ │ │ +
│ │ │ │ +
70
│ │ │ │ +
│ │ │ │ +
72 void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ +
73 Base::print(s + "PoseRotationPrior", keyFormatter);
│ │ │ │ +
74 measured_.print("Measured Rotation");
│ │ │ │ +
75 }
│ │ │ │ +
│ │ │ │ +
76
│ │ │ │ +
│ │ │ │ +
78 Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const override {
│ │ │ │ +
79 const Rotation& newR = pose.rotation();
│ │ │ │ +
80 if (H) {
│ │ │ │ +
81 *H = Matrix::Zero(rDim, xDim);
│ │ │ │ +
82 std::pair<size_t, size_t> rotInterval = POSE::rotationInterval();
│ │ │ │ +
83 (*H).middleCols(rotInterval.first, rDim).setIdentity(rDim, rDim);
│ │ │ │ +
84 }
│ │ │ │ +
85
│ │ │ │ +
86 return measured_.localCoordinates(newR);
│ │ │ │ +
87 }
│ │ │ │
│ │ │ │ -
87
│ │ │ │ +
88
│ │ │ │ +
89private:
│ │ │ │
90
│ │ │ │ -
│ │ │ │ -
96 WhiteNoiseFactor(double z, Key meanKey, Key precisionKey) :
│ │ │ │ -
97 Base(), z_(z), meanKey_(meanKey), precisionKey_(precisionKey) {
│ │ │ │ -
98 }
│ │ │ │ +
92 friend class boost::serialization::access;
│ │ │ │ +
93 template<class ARCHIVE>
│ │ │ │ +
94 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
95 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ +
96 ar & boost::serialization::make_nvp("NoiseModelFactor1",
│ │ │ │ +
97 boost::serialization::base_object<Base>(*this));
│ │ │ │ +
98 ar & BOOST_SERIALIZATION_NVP(measured_);
│ │ │ │ +
99 }
│ │ │ │ +
100};
│ │ │ │
│ │ │ │ -
99
│ │ │ │ +
101
│ │ │ │ +
102} // \namespace gtsam
│ │ │ │
103
│ │ │ │ -
│ │ │ │ -
105 ~WhiteNoiseFactor() override {
│ │ │ │ -
106 }
│ │ │ │ -
│ │ │ │ -
107
│ │ │ │ -
111
│ │ │ │ -
│ │ │ │ -
113 void print(const std::string& p = "WhiteNoiseFactor",
│ │ │ │ -
114 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ -
115 Base::print(p, keyFormatter);
│ │ │ │ -
116 std::cout << p + ".z: " << z_ << std::endl;
│ │ │ │ -
117 }
│ │ │ │ -
│ │ │ │ -
118
│ │ │ │ -
122
│ │ │ │ -
│ │ │ │ -
124 size_t dim() const override {
│ │ │ │ -
125 return 2;
│ │ │ │ -
126 }
│ │ │ │ -
│ │ │ │ -
127
│ │ │ │ -
│ │ │ │ -
129 double error(const Values& x) const override {
│ │ │ │ -
130 return f(z_, x.at<double>(meanKey_), x.at<double>(precisionKey_));
│ │ │ │ -
131 }
│ │ │ │ -
│ │ │ │ -
132
│ │ │ │ -
│ │ │ │ -
140 virtual Vector unwhitenedError(const Values& x) const {
│ │ │ │ -
141 return (Vector(1) << std::sqrt(2 * error(x))).finished();
│ │ │ │ -
142 }
│ │ │ │ -
│ │ │ │ -
143
│ │ │ │ -
148// virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
│ │ │ │ -
149// const Key j1 = ordering[meanKey_], j2 = ordering[precisionKey_];
│ │ │ │ -
150// return IndexFactor::shared_ptr(new IndexFactor(j1, j2));
│ │ │ │ -
151// }
│ │ │ │ -
152
│ │ │ │ -
156
│ │ │ │ -
│ │ │ │ -
158 boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override {
│ │ │ │ -
159 double u = x.at<double>(meanKey_);
│ │ │ │ -
160 double p = x.at<double>(precisionKey_);
│ │ │ │ -
161 Key j1 = meanKey_;
│ │ │ │ -
162 Key j2 = precisionKey_;
│ │ │ │ -
163 return linearize(z_, u, p, j1, j2);
│ │ │ │ -
164 }
│ │ │ │ -
│ │ │ │ -
165
│ │ │ │ -
166 // TODO: Frank commented this out for now, can it go?
│ │ │ │ -
167 // /// @return a deep copy of this factor
│ │ │ │ -
168 // gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
169 // return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ -
170 // gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
│ │ │ │ -
171
│ │ │ │ -
173
│ │ │ │ -
174 };
│ │ │ │ -
│ │ │ │ -
175// WhiteNoiseFactor
│ │ │ │ -
176
│ │ │ │ -
177}// namespace gtsam
│ │ │ │ -
178
│ │ │ │ -
HessianFactor.h
Contains the HessianFactor class, a general quadratic factor.
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
104
│ │ │ │ +
105
│ │ │ │ +
106
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::logSqrt2PI
const double logSqrt2PI
constant needed below
Definition WhiteNoiseFactor.h:28
│ │ │ │ +
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::HessianFactor
A Gaussian factor using the canonical parameters (information form)
Definition HessianFactor.h:101
│ │ │ │ -
gtsam::HessianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
A shared_ptr to this class.
Definition HessianFactor.h:110
│ │ │ │ +
gtsam::FixedDimension
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition Manifold.h:164
│ │ │ │ +
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
│ │ │ │ +
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
│ │ │ │
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ -
gtsam::NonlinearFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition NonlinearFactor.cpp:37
│ │ │ │ -
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
gtsam::Values::at
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
│ │ │ │ -
gtsam::WhiteNoiseFactor
Binary factor to estimate parameters of zero-mean Gaussian white noise.
Definition WhiteNoiseFactor.h:41
│ │ │ │ -
gtsam::WhiteNoiseFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override
linearize returns a Hessianfactor that is an approximation of error(p)
Definition WhiteNoiseFactor.h:158
│ │ │ │ -
gtsam::WhiteNoiseFactor::~WhiteNoiseFactor
~WhiteNoiseFactor() override
Destructor.
Definition WhiteNoiseFactor.h:105
│ │ │ │ -
gtsam::WhiteNoiseFactor::linearize
static HessianFactor::shared_ptr linearize(double z, double u, double p, Key j1, Key j2)
linearize returns a Hessianfactor that approximates error Hessian is
Definition WhiteNoiseFactor.h:75
│ │ │ │ -
gtsam::WhiteNoiseFactor::print
void print(const std::string &p="WhiteNoiseFactor", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition WhiteNoiseFactor.h:113
│ │ │ │ -
gtsam::WhiteNoiseFactor::WhiteNoiseFactor
WhiteNoiseFactor(double z, Key meanKey, Key precisionKey)
Construct from measurement.
Definition WhiteNoiseFactor.h:96
│ │ │ │ -
gtsam::WhiteNoiseFactor::unwhitenedError
virtual Vector unwhitenedError(const Values &x) const
Vector of errors "unwhitened" does not make sense for this factor What is meant typically is only "e"...
Definition WhiteNoiseFactor.h:140
│ │ │ │ -
gtsam::WhiteNoiseFactor::f
static double f(double z, double u, double p)
negative log likelihood as a function of mean and precision
Definition WhiteNoiseFactor.h:61
│ │ │ │ -
gtsam::WhiteNoiseFactor::error
double error(const Values &x) const override
Calculate the error of the factor, typically equal to log-likelihood.
Definition WhiteNoiseFactor.h:129
│ │ │ │ -
gtsam::WhiteNoiseFactor::dim
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition WhiteNoiseFactor.h:124
│ │ │ │ +
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ +
gtsam::NoiseModelFactorN< POSE >::key
Key key() const
Returns a key.
Definition NonlinearFactor.h:518
│ │ │ │ +
gtsam::PoseRotationPrior
Definition PoseRotationPrior.h:19
│ │ │ │ +
gtsam::PoseRotationPrior::PoseRotationPrior
PoseRotationPrior(Key key, const Rotation &rot_z, const SharedNoiseModel &model)
standard constructor
Definition PoseRotationPrior.h:46
│ │ │ │ +
gtsam::PoseRotationPrior::PoseRotationPrior
PoseRotationPrior(Key key, const POSE &pose_z, const SharedNoiseModel &model)
Constructor that pulls the translation from an incoming POSE.
Definition PoseRotationPrior.h:50
│ │ │ │ +
gtsam::PoseRotationPrior::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition PoseRotationPrior.h:56
│ │ │ │ +
gtsam::PoseRotationPrior::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
equals specialized to this factor
Definition PoseRotationPrior.h:66
│ │ │ │ +
gtsam::PoseRotationPrior::PoseRotationPrior
PoseRotationPrior()
default constructor - only use for serialization
Definition PoseRotationPrior.h:43
│ │ │ │ +
gtsam::PoseRotationPrior::evaluateError
Vector evaluateError(const Pose &pose, boost::optional< Matrix & > H=boost::none) const override
h(x)-z
Definition PoseRotationPrior.h:78
│ │ │ │ +
gtsam::PoseRotationPrior::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print contents
Definition PoseRotationPrior.h:72
│ │ │ │ +
gtsam::PoseRotationPrior::access
friend class boost::serialization::access
Serialization function.
Definition PoseRotationPrior.h:92
│ │ │ │ +
concepts.h
Concept-checking macros for geometric objects Each macro instantiates a concept check structure,...
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,204 +1,186 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -WhiteNoiseFactor.h │ │ │ │ │ +PoseRotationPrior.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -1/* --------------------------------------------------------------------------- │ │ │ │ │ -- │ │ │ │ │ -2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ -4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ -5 * All Rights Reserved │ │ │ │ │ -6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ -7 │ │ │ │ │ -8 * See LICENSE for the license information │ │ │ │ │ -9 │ │ │ │ │ -10 * ------------------------------------------------------------------------- │ │ │ │ │ -- */ │ │ │ │ │ +1 │ │ │ │ │ +10#pragma once │ │ │ │ │ 11 │ │ │ │ │ -20#pragma once │ │ │ │ │ +12#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_c_o_n_c_e_p_t_s_._h> │ │ │ │ │ +13#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +14 │ │ │ │ │ +15 │ │ │ │ │ +16namespace _g_t_s_a_m { │ │ │ │ │ +17 │ │ │ │ │ +18template │ │ │ │ │ +_1_9class _P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +20public: │ │ │ │ │ 21 │ │ │ │ │ -22#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_H_e_s_s_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ -24#include │ │ │ │ │ -25 │ │ │ │ │ -26namespace _g_t_s_a_m { │ │ │ │ │ +22 typedef _P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r_<_P_O_S_E_> This; │ │ │ │ │ +23 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_P_O_S_E_> _B_a_s_e; │ │ │ │ │ +24 typedef POSE Pose; │ │ │ │ │ +25 typedef typename POSE::Translation Translation; │ │ │ │ │ +26 typedef typename POSE::Rotation Rotation; │ │ │ │ │ 27 │ │ │ │ │ -_2_8 const double _l_o_g_S_q_r_t_2_P_I = log(std::sqrt(2.0 * M_PI)); │ │ │ │ │ -29 │ │ │ │ │ -_4_1 class _W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r: public _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r { │ │ │ │ │ -42 │ │ │ │ │ -43 private: │ │ │ │ │ +28 GTSAM_CONCEPT_POSE_TYPE(Pose) │ │ │ │ │ +29 GTSAM_CONCEPT_GROUP_TYPE(Pose) │ │ │ │ │ +30 GTSAM_CONCEPT_LIE_TYPE(Rotation) │ │ │ │ │ +31 │ │ │ │ │ +32 // Get dimensions of pose and rotation type at compile time │ │ │ │ │ +33 static const int xDim = _F_i_x_e_d_D_i_m_e_n_s_i_o_n_<_P_O_S_E_>_:_:_v_a_l_u_e; │ │ │ │ │ +34 static const int rDim = _F_i_x_e_d_D_i_m_e_n_s_i_o_n_<_t_y_p_e_n_a_m_e_ _P_O_S_E_:_:_R_o_t_a_t_i_o_n_>_:_:_v_a_l_u_e; │ │ │ │ │ +35 │ │ │ │ │ +36protected: │ │ │ │ │ +37 │ │ │ │ │ +38 Rotation measured_; │ │ │ │ │ +39 │ │ │ │ │ +40public: │ │ │ │ │ +41 │ │ │ │ │ +_4_3 _P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r() {} │ │ │ │ │ 44 │ │ │ │ │ -45 double z_; │ │ │ │ │ -46 │ │ │ │ │ -47 _K_e_y meanKey_; │ │ │ │ │ -48 _K_e_y precisionKey_; │ │ │ │ │ -49 │ │ │ │ │ -50 typedef _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r _B_a_s_e; │ │ │ │ │ -51 │ │ │ │ │ -52 public: │ │ │ │ │ -53 │ │ │ │ │ -_6_1 static double _f(double z, double u, double p) { │ │ │ │ │ -62 return _l_o_g_S_q_r_t_2_P_I - 0.5 * log(p) + 0.5 * (z - u) * (z - u) * p; │ │ │ │ │ -63 } │ │ │ │ │ +_4_6 _P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r(_K_e_y _k_e_y, const Rotation& rot_z, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& │ │ │ │ │ +model) │ │ │ │ │ +47 : _B_a_s_e(model, _k_e_y), measured_(rot_z) {} │ │ │ │ │ +48 │ │ │ │ │ +_5_0 _P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r(_K_e_y _k_e_y, const POSE& pose_z, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& │ │ │ │ │ +model) │ │ │ │ │ +51 : _B_a_s_e(model, _k_e_y), measured_(pose_z.rotation()) {} │ │ │ │ │ +52 │ │ │ │ │ +53 _~_P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r() override {} │ │ │ │ │ +54 │ │ │ │ │ +_5_6 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +57 return boost::static_pointer_cast( │ │ │ │ │ +58 gtsam::NonlinearFactor::shared_ptr(new This(*this))); } │ │ │ │ │ +59 │ │ │ │ │ +60 // access │ │ │ │ │ +61 const Rotation& measured() const { return measured_; } │ │ │ │ │ +62 │ │ │ │ │ +63 // testable │ │ │ │ │ 64 │ │ │ │ │ -_7_5 static _H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r _l_i_n_e_a_r_i_z_e(double z, double u, double p, │ │ │ │ │ -76 _K_e_y j1, _K_e_y j2) { │ │ │ │ │ -77 double e = u - z, e2 = e * e; │ │ │ │ │ -78 double c = 2 * _l_o_g_S_q_r_t_2_P_I - log(p) + e2 * p; │ │ │ │ │ -79 Vector g1 = (Vector(1) << -e * p).finished(); │ │ │ │ │ -80 Vector g2 = (Vector(1) << 0.5 / p - 0.5 * e2).finished(); │ │ │ │ │ -81 Matrix G11 = (Matrix(1, 1) << p).finished(); │ │ │ │ │ -82 Matrix G12 = (Matrix(1, 1) << e).finished(); │ │ │ │ │ -83 Matrix G22 = (Matrix(1, 1) << 0.5 / (p * p)).finished(); │ │ │ │ │ -84 return _H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r( │ │ │ │ │ -85 new _H_e_s_s_i_a_n_F_a_c_t_o_r(j1, j2, G11, G12, g1, G22, g2, c)); │ │ │ │ │ -86 } │ │ │ │ │ -87 │ │ │ │ │ +_6_6 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& expected, double tol=1e-9) const override │ │ │ │ │ +{ │ │ │ │ │ +67 const This *e = dynamic_cast (&expected); │ │ │ │ │ +68 return e != nullptr && _B_a_s_e_:_:_e_q_u_a_l_s(*e, tol) && measured_.equals(e- │ │ │ │ │ +>measured_, tol); │ │ │ │ │ +69 } │ │ │ │ │ +70 │ │ │ │ │ +_7_2 void _p_r_i_n_t(const std::string& s="", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ +DefaultKeyFormatter) const override { │ │ │ │ │ +73 _B_a_s_e_:_:_p_r_i_n_t(s + "PoseRotationPrior", keyFormatter); │ │ │ │ │ +74 measured_.print("Measured Rotation"); │ │ │ │ │ +75 } │ │ │ │ │ +76 │ │ │ │ │ +_7_8 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const Pose& pose, boost::optional H = boost:: │ │ │ │ │ +none) const override { │ │ │ │ │ +79 const Rotation& newR = pose.rotation(); │ │ │ │ │ +80 if (H) { │ │ │ │ │ +81 *H = Matrix::Zero(rDim, xDim); │ │ │ │ │ +82 std::pair rotInterval = POSE::rotationInterval(); │ │ │ │ │ +83 (*H).middleCols(rotInterval.first, rDim).setIdentity(rDim, rDim); │ │ │ │ │ +84 } │ │ │ │ │ +85 │ │ │ │ │ +86 return measured_.localCoordinates(newR); │ │ │ │ │ +87 } │ │ │ │ │ +88 │ │ │ │ │ +89private: │ │ │ │ │ 90 │ │ │ │ │ -_9_6 _W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r(double z, _K_e_y meanKey, _K_e_y precisionKey) : │ │ │ │ │ -97 _B_a_s_e(), z_(z), meanKey_(meanKey), precisionKey_(precisionKey) { │ │ │ │ │ -98 } │ │ │ │ │ -99 │ │ │ │ │ +_9_2 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +93 template │ │ │ │ │ +94 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +95 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility │ │ │ │ │ +96 ar & boost::serialization::make_nvp("NoiseModelFactor1", │ │ │ │ │ +97 boost::serialization::base_object(*this)); │ │ │ │ │ +98 ar & BOOST_SERIALIZATION_NVP(measured_); │ │ │ │ │ +99 } │ │ │ │ │ +100}; │ │ │ │ │ +101 │ │ │ │ │ +102} // \namespace gtsam │ │ │ │ │ 103 │ │ │ │ │ -_1_0_5 _~_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r() override { │ │ │ │ │ -106 } │ │ │ │ │ -107 │ │ │ │ │ -111 │ │ │ │ │ -_1_1_3 void _p_r_i_n_t(const std::string& p = "WhiteNoiseFactor", │ │ │ │ │ -114 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ -115 _B_a_s_e_:_:_p_r_i_n_t(p, keyFormatter); │ │ │ │ │ -116 std::cout << p + ".z: " << z_ << std::endl; │ │ │ │ │ -117 } │ │ │ │ │ -118 │ │ │ │ │ -122 │ │ │ │ │ -_1_2_4 size_t _d_i_m() const override { │ │ │ │ │ -125 return 2; │ │ │ │ │ -126 } │ │ │ │ │ -127 │ │ │ │ │ -_1_2_9 double _e_r_r_o_r(const _V_a_l_u_e_s& x) const override { │ │ │ │ │ -130 return _f(z_, x._a_t(meanKey_), x._a_t(precisionKey_)); │ │ │ │ │ -131 } │ │ │ │ │ -132 │ │ │ │ │ -_1_4_0 virtual Vector _u_n_w_h_i_t_e_n_e_d_E_r_r_o_r(const _V_a_l_u_e_s& x) const { │ │ │ │ │ -141 return (Vector(1) << std::sqrt(2 * _e_r_r_o_r(x))).finished(); │ │ │ │ │ -142 } │ │ │ │ │ -143 │ │ │ │ │ -148// virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const │ │ │ │ │ -{ │ │ │ │ │ -149// const Key j1 = ordering[meanKey_], j2 = ordering[precisionKey_]; │ │ │ │ │ -150// return IndexFactor::shared_ptr(new IndexFactor(j1, j2)); │ │ │ │ │ -151// } │ │ │ │ │ -152 │ │ │ │ │ -156 │ │ │ │ │ -_1_5_8 boost::shared_ptr _l_i_n_e_a_r_i_z_e(const _V_a_l_u_e_s& x) const override │ │ │ │ │ -{ │ │ │ │ │ -159 double u = x._a_t(meanKey_); │ │ │ │ │ -160 double p = x._a_t(precisionKey_); │ │ │ │ │ -161 _K_e_y j1 = meanKey_; │ │ │ │ │ -162 _K_e_y j2 = precisionKey_; │ │ │ │ │ -163 return _l_i_n_e_a_r_i_z_e(z_, u, p, j1, j2); │ │ │ │ │ -164 } │ │ │ │ │ -165 │ │ │ │ │ -166 // TODO: Frank commented this out for now, can it go? │ │ │ │ │ -167 // /// @return a deep copy of this factor │ │ │ │ │ -168 // gtsam::NonlinearFactor::shared_ptr clone() const override { │ │ │ │ │ -169 // return boost::static_pointer_cast( │ │ │ │ │ -170 // gtsam::NonlinearFactor::shared_ptr(new This(*this))); } │ │ │ │ │ -171 │ │ │ │ │ -173 │ │ │ │ │ -174 }; │ │ │ │ │ -175// WhiteNoiseFactor │ │ │ │ │ -176 │ │ │ │ │ -177}// namespace gtsam │ │ │ │ │ -178 │ │ │ │ │ -_H_e_s_s_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ -Contains the HessianFactor class, a general quadratic factor. │ │ │ │ │ +104 │ │ │ │ │ +105 │ │ │ │ │ +106 │ │ │ │ │ _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_l_o_g_S_q_r_t_2_P_I │ │ │ │ │ -const double logSqrt2PI │ │ │ │ │ -constant needed below │ │ │ │ │ -DDeeffiinniittiioonn WhiteNoiseFactor.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ +Aliases. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r │ │ │ │ │ -A Gaussian factor using the canonical parameters (information form) │ │ │ │ │ -DDeeffiinniittiioonn HessianFactor.h:101 │ │ │ │ │ -_g_t_s_a_m_:_:_H_e_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -A shared_ptr to this class. │ │ │ │ │ -DDeeffiinniittiioonn HessianFactor.h:110 │ │ │ │ │ +_g_t_s_a_m_:_:_F_i_x_e_d_D_i_m_e_n_s_i_o_n │ │ │ │ │ +Give fixed size dimension of a type, fails at compile time if dynamic. │ │ │ │ │ +DDeeffiinniittiioonn Manifold.h:164 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +virtual void print(const std::string &s="Factor", const KeyFormatter │ │ │ │ │ +&formatter=DefaultKeyFormatter) const │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn Factor.cpp:29 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const This &other, double tol=1e-9) const │ │ │ │ │ +check equality │ │ │ │ │ +DDeeffiinniittiioonn Factor.cpp:42 │ │ │ │ │ _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ Nonlinear factor base class. │ │ │ │ │ DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ +A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ +variables. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_ _P_O_S_E_ _>_:_:_k_e_y │ │ │ │ │ +Key key() const │ │ │ │ │ +Returns a key. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:518 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r │ │ │ │ │ +DDeeffiinniittiioonn PoseRotationPrior.h:19 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r_:_:_P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r │ │ │ │ │ +PoseRotationPrior(Key key, const Rotation &rot_z, const SharedNoiseModel │ │ │ │ │ +&model) │ │ │ │ │ +standard constructor │ │ │ │ │ +DDeeffiinniittiioonn PoseRotationPrior.h:46 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r_:_:_P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r │ │ │ │ │ +PoseRotationPrior(Key key, const POSE &pose_z, const SharedNoiseModel &model) │ │ │ │ │ +Constructor that pulls the translation from an incoming POSE. │ │ │ │ │ +DDeeffiinniittiioonn PoseRotationPrior.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r_:_:_c_l_o_n_e │ │ │ │ │ +gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn PoseRotationPrior.h:56 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const NonlinearFactor &expected, double tol=1e-9) const override │ │ │ │ │ +equals specialized to this factor │ │ │ │ │ +DDeeffiinniittiioonn PoseRotationPrior.h:66 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r_:_:_P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r │ │ │ │ │ +PoseRotationPrior() │ │ │ │ │ +default constructor - only use for serialization │ │ │ │ │ +DDeeffiinniittiioonn PoseRotationPrior.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ +Vector evaluateError(const Pose &pose, boost::optional< Matrix & > H=boost:: │ │ │ │ │ +none) const override │ │ │ │ │ +h(x)-z │ │ │ │ │ +DDeeffiinniittiioonn PoseRotationPrior.h:78 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r_:_:_p_r_i_n_t │ │ │ │ │ void print(const std::string &s="", const KeyFormatter │ │ │ │ │ &keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.cpp:37 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_a_t │ │ │ │ │ -const ValueType at(Key j) const │ │ │ │ │ -Retrieve a variable by key j. │ │ │ │ │ -DDeeffiinniittiioonn Values-inl.h:361 │ │ │ │ │ -_g_t_s_a_m_:_:_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r │ │ │ │ │ -Binary factor to estimate parameters of zero-mean Gaussian white noise. │ │ │ │ │ -DDeeffiinniittiioonn WhiteNoiseFactor.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e │ │ │ │ │ -boost::shared_ptr< GaussianFactor > linearize(const Values &x) const override │ │ │ │ │ -linearize returns a Hessianfactor that is an approximation of error(p) │ │ │ │ │ -DDeeffiinniittiioonn WhiteNoiseFactor.h:158 │ │ │ │ │ -_g_t_s_a_m_:_:_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r_:_:_~_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r │ │ │ │ │ -~WhiteNoiseFactor() override │ │ │ │ │ -Destructor. │ │ │ │ │ -DDeeffiinniittiioonn WhiteNoiseFactor.h:105 │ │ │ │ │ -_g_t_s_a_m_:_:_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e │ │ │ │ │ -static HessianFactor::shared_ptr linearize(double z, double u, double p, Key │ │ │ │ │ -j1, Key j2) │ │ │ │ │ -linearize returns a Hessianfactor that approximates error Hessian is │ │ │ │ │ -DDeeffiinniittiioonn WhiteNoiseFactor.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &p="WhiteNoiseFactor", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -Print. │ │ │ │ │ -DDeeffiinniittiioonn WhiteNoiseFactor.h:113 │ │ │ │ │ -_g_t_s_a_m_:_:_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r_:_:_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r │ │ │ │ │ -WhiteNoiseFactor(double z, Key meanKey, Key precisionKey) │ │ │ │ │ -Construct from measurement. │ │ │ │ │ -DDeeffiinniittiioonn WhiteNoiseFactor.h:96 │ │ │ │ │ -_g_t_s_a_m_:_:_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r_:_:_u_n_w_h_i_t_e_n_e_d_E_r_r_o_r │ │ │ │ │ -virtual Vector unwhitenedError(const Values &x) const │ │ │ │ │ -Vector of errors "unwhitened" does not make sense for this factor What is meant │ │ │ │ │ -typically is only "e"... │ │ │ │ │ -DDeeffiinniittiioonn WhiteNoiseFactor.h:140 │ │ │ │ │ -_g_t_s_a_m_:_:_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r_:_:_f │ │ │ │ │ -static double f(double z, double u, double p) │ │ │ │ │ -negative log likelihood as a function of mean and precision │ │ │ │ │ -DDeeffiinniittiioonn WhiteNoiseFactor.h:61 │ │ │ │ │ -_g_t_s_a_m_:_:_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r_:_:_e_r_r_o_r │ │ │ │ │ -double error(const Values &x) const override │ │ │ │ │ -Calculate the error of the factor, typically equal to log-likelihood. │ │ │ │ │ -DDeeffiinniittiioonn WhiteNoiseFactor.h:129 │ │ │ │ │ -_g_t_s_a_m_:_:_W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r_:_:_d_i_m │ │ │ │ │ -size_t dim() const override │ │ │ │ │ -get the dimension of the factor (number of rows on linearization) │ │ │ │ │ -DDeeffiinniittiioonn WhiteNoiseFactor.h:124 │ │ │ │ │ +print contents │ │ │ │ │ +DDeeffiinniittiioonn PoseRotationPrior.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn PoseRotationPrior.h:92 │ │ │ │ │ +_c_o_n_c_e_p_t_s_._h │ │ │ │ │ +Concept-checking macros for geometric objects Each macro instantiates a concept │ │ │ │ │ +check structure,... │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _W_h_i_t_e_N_o_i_s_e_F_a_c_t_o_r_._h │ │ │ │ │ + * _s_l_a_m │ │ │ │ │ + * _P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01094_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01082_source.html │ │ │ │┄ Files 91% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearFactorGraph.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/FunctorizedFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
NonlinearFactorGraph.h
│ │ │ │ +
FunctorizedFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
20// \callgraph
│ │ │ │ -
21
│ │ │ │ -
22#pragma once
│ │ │ │ -
23
│ │ │ │ -
24#include <gtsam/geometry/Point2.h>
│ │ │ │ -
25#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
26#include <gtsam/nonlinear/GraphvizFormatting.h>
│ │ │ │ -
27#include <gtsam/inference/FactorGraph.h>
│ │ │ │ -
28#include <gtsam/nonlinear/PriorFactor.h>
│ │ │ │ -
29
│ │ │ │ -
30#include <boost/shared_ptr.hpp>
│ │ │ │ -
31#include <functional>
│ │ │ │ -
32
│ │ │ │ -
33namespace gtsam {
│ │ │ │ -
34
│ │ │ │ -
35 // Forward declarations
│ │ │ │ -
36 class Values;
│ │ │ │ -
37 class Ordering;
│ │ │ │ -
38 class GaussianFactorGraph;
│ │ │ │ -
39 class SymbolicFactorGraph;
│ │ │ │ -
40 template<typename T>
│ │ │ │ -
41 class Expression;
│ │ │ │ -
42 template<typename T>
│ │ │ │ -
43 class ExpressionFactor;
│ │ │ │ -
44
│ │ │ │ -
│ │ │ │ -
55 class GTSAM_EXPORT NonlinearFactorGraph: public FactorGraph<NonlinearFactor> {
│ │ │ │ -
56
│ │ │ │ -
57 public:
│ │ │ │ -
58
│ │ │ │ -
59 typedef FactorGraph<NonlinearFactor> Base;
│ │ │ │ -
60 typedef NonlinearFactorGraph This;
│ │ │ │ -
61 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/base/Testable.h>
│ │ │ │ +
21#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
22
│ │ │ │ +
23#include <cmath>
│ │ │ │ +
24
│ │ │ │ +
25namespace gtsam {
│ │ │ │ +
26
│ │ │ │ +
58template <typename R, typename T>
│ │ │ │ +
│ │ │ │ +
59class FunctorizedFactor : public NoiseModelFactorN<T> {
│ │ │ │ +
60 private:
│ │ │ │ +
61 using Base = NoiseModelFactorN<T>;
│ │ │ │
62
│ │ │ │ -
65
│ │ │ │ -
67 NonlinearFactorGraph() {}
│ │ │ │ -
68
│ │ │ │ -
70 template<typename ITERATOR>
│ │ │ │ -
71 NonlinearFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {}
│ │ │ │ -
72
│ │ │ │ -
74 template<class CONTAINER>
│ │ │ │ -
75 explicit NonlinearFactorGraph(const CONTAINER& factors) : Base(factors) {}
│ │ │ │ -
76
│ │ │ │ -
78 template<class DERIVEDFACTOR>
│ │ │ │ -
79 NonlinearFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
│ │ │ │ -
80
│ │ │ │ -
82 virtual ~NonlinearFactorGraph() {}
│ │ │ │ +
63 R measured_;
│ │ │ │ +
64 SharedNoiseModel noiseModel_;
│ │ │ │ +
65 std::function<R(T, boost::optional<Matrix &>)> func_;
│ │ │ │ +
66
│ │ │ │ +
67 public:
│ │ │ │ +
69 FunctorizedFactor() {}
│ │ │ │ +
70
│ │ │ │ +
│ │ │ │ +
78 FunctorizedFactor(Key key, const R &z, const SharedNoiseModel &model,
│ │ │ │ +
79 const std::function<R(T, boost::optional<Matrix &>)> func)
│ │ │ │ +
80 : Base(model, key), measured_(z), noiseModel_(model), func_(func) {}
│ │ │ │ +
│ │ │ │ +
81
│ │ │ │ +
82 ~FunctorizedFactor() override {}
│ │ │ │
83
│ │ │ │ -
87
│ │ │ │ -
89 void print(
│ │ │ │ -
90 const std::string& str = "NonlinearFactorGraph: ",
│ │ │ │ -
91 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
│ │ │ │ -
92
│ │ │ │ -
94 void printErrors(const Values& values, const std::string& str = "NonlinearFactorGraph: ",
│ │ │ │ -
95 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
96 const std::function<bool(const Factor* /*factor*/, double /*whitenedError*/, size_t /*index*/)>&
│ │ │ │ -
97 printCondition = [](const Factor *,double, size_t) {return true;}) const;
│ │ │ │ -
98
│ │ │ │ -
100 bool equals(const NonlinearFactorGraph& other, double tol = 1e-9) const;
│ │ │ │ -
101
│ │ │ │ -
105
│ │ │ │ -
107 double error(const Values& values) const;
│ │ │ │ -
108
│ │ │ │ -
110 double probPrime(const Values& values) const;
│ │ │ │ -
111
│ │ │ │ -
115 boost::shared_ptr<SymbolicFactorGraph> symbolic() const;
│ │ │ │ -
116
│ │ │ │ -
120 Ordering orderingCOLAMD() const;
│ │ │ │ -
121
│ │ │ │ -
130 Ordering orderingCOLAMDConstrained(const FastMap<Key, int>& constraints) const;
│ │ │ │ -
131
│ │ │ │ -
133 boost::shared_ptr<GaussianFactorGraph> linearize(const Values& linearizationPoint) const;
│ │ │ │ -
134
│ │ │ │ -
136 typedef std::function<void(const boost::shared_ptr<HessianFactor>& hessianFactor)> Dampen;
│ │ │ │ -
137
│ │ │ │ -
145 boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
│ │ │ │ -
146 const Values& values, const Dampen& dampen = nullptr) const;
│ │ │ │ -
147
│ │ │ │ -
156 boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
│ │ │ │ -
157 const Values& values, const Ordering& ordering, const Dampen& dampen = nullptr) const;
│ │ │ │ -
158
│ │ │ │ -
161 Values updateCholesky(const Values& values,
│ │ │ │ -
162 const Dampen& dampen = nullptr) const;
│ │ │ │ -
163
│ │ │ │ -
166 Values updateCholesky(const Values& values, const Ordering& ordering,
│ │ │ │ -
167 const Dampen& dampen = nullptr) const;
│ │ │ │ +
│ │ │ │ +
85 NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
86 return boost::static_pointer_cast<NonlinearFactor>(
│ │ │ │ +
87 NonlinearFactor::shared_ptr(new FunctorizedFactor<R, T>(*this)));
│ │ │ │ +
88 }
│ │ │ │ +
│ │ │ │ +
89
│ │ │ │ +
90 Vector evaluateError(const T &params, boost::optional<Matrix &> H =
│ │ │ │ +
91 boost::none) const override {
│ │ │ │ +
92 R x = func_(params, H);
│ │ │ │ +
93 Vector error = traits<R>::Local(measured_, x);
│ │ │ │ +
94 return error;
│ │ │ │ +
95 }
│ │ │ │ +
96
│ │ │ │ +
│ │ │ │ +
99 void print(
│ │ │ │ +
100 const std::string &s = "",
│ │ │ │ +
101 const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ +
102 Base::print(s, keyFormatter);
│ │ │ │ +
103 std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor("
│ │ │ │ +
104 << keyFormatter(this->key1()) << ")" << std::endl;
│ │ │ │ +
105 traits<R>::Print(measured_, " measurement: ");
│ │ │ │ +
106 std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose()
│ │ │ │ +
107 << std::endl;
│ │ │ │ +
108 }
│ │ │ │ +
│ │ │ │ +
109
│ │ │ │ +
│ │ │ │ +
110 bool equals(const NonlinearFactor &other, double tol = 1e-9) const override {
│ │ │ │ +
111 const FunctorizedFactor<R, T> *e =
│ │ │ │ +
112 dynamic_cast<const FunctorizedFactor<R, T> *>(&other);
│ │ │ │ +
113 return e != nullptr && Base::equals(other, tol) &&
│ │ │ │ +
114 traits<R>::Equals(this->measured_, e->measured_, tol);
│ │ │ │ +
115 }
│ │ │ │ +
│ │ │ │ +
117
│ │ │ │ +
118 private:
│ │ │ │ +
120 friend class boost::serialization::access;
│ │ │ │ +
121 template <class ARCHIVE>
│ │ │ │ +
122 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ +
123 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ +
124 ar &boost::serialization::make_nvp(
│ │ │ │ +
125 "NoiseModelFactor1", boost::serialization::base_object<Base>(*this));
│ │ │ │ +
126 ar &BOOST_SERIALIZATION_NVP(measured_);
│ │ │ │ +
127 ar &BOOST_SERIALIZATION_NVP(func_);
│ │ │ │ +
128 }
│ │ │ │ +
129};
│ │ │ │ +
│ │ │ │ +
130
│ │ │ │ +
132template <typename R, typename T>
│ │ │ │ +
│ │ │ │ +
133struct traits<FunctorizedFactor<R, T>>
│ │ │ │ +
134 : public Testable<FunctorizedFactor<R, T>> {};
│ │ │ │ +
│ │ │ │ +
135
│ │ │ │ +
142template <typename T, typename R, typename FUNC>
│ │ │ │ +
│ │ │ │ +
143FunctorizedFactor<R, T> MakeFunctorizedFactor(Key key, const R &z,
│ │ │ │ +
144 const SharedNoiseModel &model,
│ │ │ │ +
145 const FUNC func) {
│ │ │ │ +
146 return FunctorizedFactor<R, T>(key, z, model, func);
│ │ │ │ +
147}
│ │ │ │ +
│ │ │ │ +
148
│ │ │ │ +
158template <typename R, typename T1, typename T2>
│ │ │ │ +
│ │ │ │ +
159class FunctorizedFactor2 : public NoiseModelFactorN<T1, T2> {
│ │ │ │ +
160 private:
│ │ │ │ +
161 using Base = NoiseModelFactorN<T1, T2>;
│ │ │ │ +
162
│ │ │ │ +
163 R measured_;
│ │ │ │ +
164 SharedNoiseModel noiseModel_;
│ │ │ │ +
165 using FunctionType = std::function<R(T1, T2, boost::optional<Matrix &>,
│ │ │ │ +
166 boost::optional<Matrix &>)>;
│ │ │ │ +
167 FunctionType func_;
│ │ │ │
168
│ │ │ │ -
170 NonlinearFactorGraph clone() const;
│ │ │ │ -
171
│ │ │ │ -
181 NonlinearFactorGraph rekey(const std::map<Key,Key>& rekey_mapping) const;
│ │ │ │ -
182
│ │ │ │ -
189 template<typename T>
│ │ │ │ +
169 public:
│ │ │ │ +
171 FunctorizedFactor2() {}
│ │ │ │ +
172
│ │ │ │ +
│ │ │ │ +
180 FunctorizedFactor2(Key key1, Key key2, const R &z,
│ │ │ │ +
181 const SharedNoiseModel &model, const FunctionType func)
│ │ │ │ +
182 : Base(model, key1, key2),
│ │ │ │ +
183 measured_(z),
│ │ │ │ +
184 noiseModel_(model),
│ │ │ │ +
185 func_(func) {}
│ │ │ │ +
│ │ │ │ +
186
│ │ │ │ +
187 ~FunctorizedFactor2() override {}
│ │ │ │ +
188
│ │ │ │
│ │ │ │ -
190 void addExpressionFactor(const SharedNoiseModel& R, const T& z,
│ │ │ │ -
191 const Expression<T>& h) {
│ │ │ │ -
192 push_back(boost::make_shared<ExpressionFactor<T> >(R, z, h));
│ │ │ │ -
193 }
│ │ │ │ +
190 NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
191 return boost::static_pointer_cast<NonlinearFactor>(
│ │ │ │ +
192 NonlinearFactor::shared_ptr(new FunctorizedFactor2<R, T1, T2>(*this)));
│ │ │ │ +
193 }
│ │ │ │
│ │ │ │
194
│ │ │ │ -
201 template<typename T>
│ │ │ │ -
│ │ │ │ -
202 void addPrior(Key key, const T& prior,
│ │ │ │ -
203 const SharedNoiseModel& model = nullptr) {
│ │ │ │ -
204 emplace_shared<PriorFactor<T>>(key, prior, model);
│ │ │ │ -
205 }
│ │ │ │ +
195 Vector evaluateError(
│ │ │ │ +
196 const T1 &params1, const T2 &params2,
│ │ │ │ +
197 boost::optional<Matrix &> H1 = boost::none,
│ │ │ │ +
198 boost::optional<Matrix &> H2 = boost::none) const override {
│ │ │ │ +
199 R x = func_(params1, params2, H1, H2);
│ │ │ │ +
200 Vector error = traits<R>::Local(measured_, x);
│ │ │ │ +
201 return error;
│ │ │ │ +
202 }
│ │ │ │ +
203
│ │ │ │ +
│ │ │ │ +
206 void print(
│ │ │ │ +
207 const std::string &s = "",
│ │ │ │ +
208 const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ +
209 Base::print(s, keyFormatter);
│ │ │ │ +
210 std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor2("
│ │ │ │ +
211 << keyFormatter(this->key1()) << ", "
│ │ │ │ +
212 << keyFormatter(this->key2()) << ")" << std::endl;
│ │ │ │ +
213 traits<R>::Print(measured_, " measurement: ");
│ │ │ │ +
214 std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose()
│ │ │ │ +
215 << std::endl;
│ │ │ │ +
216 }
│ │ │ │
│ │ │ │ -
206
│ │ │ │ -
217 template<typename T>
│ │ │ │ +
217
│ │ │ │
│ │ │ │ -
218 void addPrior(Key key, const T& prior, const Matrix& covariance) {
│ │ │ │ -
219 emplace_shared<PriorFactor<T>>(key, prior, covariance);
│ │ │ │ -
220 }
│ │ │ │ +
218 bool equals(const NonlinearFactor &other, double tol = 1e-9) const override {
│ │ │ │ +
219 const FunctorizedFactor2<R, T1, T2> *e =
│ │ │ │ +
220 dynamic_cast<const FunctorizedFactor2<R, T1, T2> *>(&other);
│ │ │ │ +
221 return e && Base::equals(other, tol) &&
│ │ │ │ +
222 traits<R>::Equals(this->measured_, e->measured_, tol);
│ │ │ │ +
223 }
│ │ │ │
│ │ │ │ -
221
│ │ │ │
225
│ │ │ │ -
226 using FactorGraph::dot;
│ │ │ │ -
227 using FactorGraph::saveGraph;
│ │ │ │ -
228
│ │ │ │ -
230 void dot(std::ostream& os, const Values& values,
│ │ │ │ -
231 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
232 const GraphvizFormatting& writer = GraphvizFormatting()) const;
│ │ │ │ -
233
│ │ │ │ -
235 std::string dot(
│ │ │ │ -
236 const Values& values,
│ │ │ │ -
237 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
238 const GraphvizFormatting& writer = GraphvizFormatting()) const;
│ │ │ │ -
239
│ │ │ │ -
241 void saveGraph(
│ │ │ │ -
242 const std::string& filename, const Values& values,
│ │ │ │ -
243 const KeyFormatter& keyFormatter = DefaultKeyFormatter,
│ │ │ │ -
244 const GraphvizFormatting& writer = GraphvizFormatting()) const;
│ │ │ │ -
246
│ │ │ │ -
247 private:
│ │ │ │ -
248
│ │ │ │ -
253 boost::shared_ptr<HessianFactor> linearizeToHessianFactor(
│ │ │ │ -
254 const Values& values, const Scatter& scatter, const Dampen& dampen = nullptr) const;
│ │ │ │ -
255
│ │ │ │ -
257 friend class boost::serialization::access;
│ │ │ │ -
258 template<class ARCHIVE>
│ │ │ │ -
259 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
260 ar & boost::serialization::make_nvp("NonlinearFactorGraph",
│ │ │ │ -
261 boost::serialization::base_object<Base>(*this));
│ │ │ │ -
262 }
│ │ │ │ -
263
│ │ │ │ -
264 public:
│ │ │ │ -
265
│ │ │ │ -
266#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ -
269
│ │ │ │ -
270 boost::shared_ptr<HessianFactor> GTSAM_DEPRECATED linearizeToHessianFactor(
│ │ │ │ -
271 const Values& values, boost::none_t, const Dampen& dampen = nullptr) const
│ │ │ │ -
272 {return linearizeToHessianFactor(values, dampen);}
│ │ │ │ -
273
│ │ │ │ -
275 Values GTSAM_DEPRECATED updateCholesky(const Values& values, boost::none_t,
│ │ │ │ -
276 const Dampen& dampen = nullptr) const
│ │ │ │ -
277 {return updateCholesky(values, dampen);}
│ │ │ │ -
278
│ │ │ │ -
280 void GTSAM_DEPRECATED saveGraph(
│ │ │ │ -
281 std::ostream& os, const Values& values = Values(),
│ │ │ │ -
282 const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(),
│ │ │ │ -
283 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
│ │ │ │ -
284 dot(os, values, keyFormatter, graphvizFormatting);
│ │ │ │ -
285 }
│ │ │ │ -
287 void GTSAM_DEPRECATED
│ │ │ │ -
288 saveGraph(const std::string& filename, const Values& values,
│ │ │ │ -
289 const GraphvizFormatting& graphvizFormatting,
│ │ │ │ -
290 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
│ │ │ │ -
291 saveGraph(filename, values, keyFormatter, graphvizFormatting);
│ │ │ │ -
292 }
│ │ │ │ -
294#endif
│ │ │ │ -
295
│ │ │ │ -
296 };
│ │ │ │ -
│ │ │ │ -
297
│ │ │ │ -
299template<>
│ │ │ │ -
│ │ │ │ -
300struct traits<NonlinearFactorGraph> : public Testable<NonlinearFactorGraph> {
│ │ │ │ -
301};
│ │ │ │ -
│ │ │ │ -
302
│ │ │ │ -
303} //\ namespace gtsam
│ │ │ │ -
304
│ │ │ │ -
Point2.h
2D Point
│ │ │ │ -
FactorGraph.h
Factor Graph Base Class.
│ │ │ │ -
GraphvizFormatting.h
Graphviz formatter for NonlinearFactorGraph.
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
226 private:
│ │ │ │ +
228 friend class boost::serialization::access;
│ │ │ │ +
229 template <class ARCHIVE>
│ │ │ │ +
230 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ +
231 // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ +
232 ar &boost::serialization::make_nvp(
│ │ │ │ +
233 "NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
│ │ │ │ +
234 ar &BOOST_SERIALIZATION_NVP(measured_);
│ │ │ │ +
235 ar &BOOST_SERIALIZATION_NVP(func_);
│ │ │ │ +
236 }
│ │ │ │ +
237};
│ │ │ │ +
│ │ │ │ +
238
│ │ │ │ +
240template <typename R, typename T1, typename T2>
│ │ │ │ +
│ │ │ │ +
241struct traits<FunctorizedFactor2<R, T1, T2>>
│ │ │ │ +
242 : public Testable<FunctorizedFactor2<R, T1, T2>> {};
│ │ │ │ +
│ │ │ │ +
243
│ │ │ │ +
250template <typename T1, typename T2, typename R, typename FUNC>
│ │ │ │ +
│ │ │ │ +
251FunctorizedFactor2<R, T1, T2> MakeFunctorizedFactor2(
│ │ │ │ +
252 Key key1, Key key2, const R &z, const SharedNoiseModel &model,
│ │ │ │ +
253 const FUNC func) {
│ │ │ │ +
254 return FunctorizedFactor2<R, T1, T2>(key1, key2, z, model, func);
│ │ │ │ +
255}
│ │ │ │ +
│ │ │ │ +
256
│ │ │ │ +
257} // namespace gtsam
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::MakeFunctorizedFactor2
FunctorizedFactor2< R, T1, T2 > MakeFunctorizedFactor2(Key key1, Key key2, const R &z, const SharedNoiseModel &model, const FUNC func)
Helper function to create a functorized factor.
Definition FunctorizedFactor.h:251
│ │ │ │ +
gtsam::MakeFunctorizedFactor
FunctorizedFactor< R, T > MakeFunctorizedFactor(Key key, const R &z, const SharedNoiseModel &model, const FUNC func)
Helper function to create a functorized factor.
Definition FunctorizedFactor.h:143
│ │ │ │
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ -
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ -
gtsam::Factor
Definition Factor.h:68
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ -
gtsam::Scatter
Scatter is an intermediate data structure used when building a HessianFactor incrementally,...
Definition Scatter.h:49
│ │ │ │ -
gtsam::ExpressionFactor
Factor that supports arbitrary expressions via AD.
Definition ExpressionFactor.h:44
│ │ │ │ -
gtsam::Expression
Expression class that supports automatic differentiation.
Definition Expression.h:48
│ │ │ │ -
gtsam::GraphvizFormatting
Formatting options and functions for saving a NonlinearFactorGraph instance in GraphViz format.
Definition GraphvizFormatting.h:32
│ │ │ │ -
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │ -
gtsam::NonlinearFactorGraph::NonlinearFactorGraph
NonlinearFactorGraph()
Default constructor.
Definition NonlinearFactorGraph.h:67
│ │ │ │ -
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Convenience method which adds a PriorFactor to the factor graph.
Definition NonlinearFactorGraph.h:202
│ │ │ │ -
gtsam::NonlinearFactorGraph::NonlinearFactorGraph
NonlinearFactorGraph(const CONTAINER &factors)
Construct from container of factors (shared_ptr or plain objects)
Definition NonlinearFactorGraph.h:75
│ │ │ │ -
gtsam::NonlinearFactorGraph::addExpressionFactor
void addExpressionFactor(const SharedNoiseModel &R, const T &z, const Expression< T > &h)
Directly add ExpressionFactor that implements |h(x)-z|^2_R.
Definition NonlinearFactorGraph.h:190
│ │ │ │ -
gtsam::NonlinearFactorGraph::addPrior
void addPrior(Key key, const T &prior, const Matrix &covariance)
Convenience method which adds a PriorFactor to the factor graph.
Definition NonlinearFactorGraph.h:218
│ │ │ │ -
gtsam::NonlinearFactorGraph::NonlinearFactorGraph
NonlinearFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
Construct from iterator over factors.
Definition NonlinearFactorGraph.h:71
│ │ │ │ -
gtsam::NonlinearFactorGraph::Dampen
std::function< void(const boost::shared_ptr< HessianFactor > &hessianFactor)> Dampen
typdef for dampen functions used below
Definition NonlinearFactorGraph.h:136
│ │ │ │ -
gtsam::NonlinearFactorGraph::NonlinearFactorGraph
NonlinearFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
Implicit copy/downcast constructor to override explicit template container constructor.
Definition NonlinearFactorGraph.h:79
│ │ │ │ -
gtsam::NonlinearFactorGraph::~NonlinearFactorGraph
virtual ~NonlinearFactorGraph()
Destructor.
Definition NonlinearFactorGraph.h:82
│ │ │ │ -
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │ - │ │ │ │ +
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
│ │ │ │ +
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
│ │ │ │ +
gtsam::FunctorizedFactor
Factor which evaluates provided unary functor and uses the result to compute error with respect to th...
Definition FunctorizedFactor.h:59
│ │ │ │ +
gtsam::FunctorizedFactor::FunctorizedFactor
FunctorizedFactor(Key key, const R &z, const SharedNoiseModel &model, const std::function< R(T, boost::optional< Matrix & >)> func)
Construct with given x and the parameters of the basis.
Definition FunctorizedFactor.h:78
│ │ │ │ +
gtsam::FunctorizedFactor::equals
bool equals(const NonlinearFactor &other, double tol=1e-9) const override
Check if two factors are equal.
Definition FunctorizedFactor.h:110
│ │ │ │ +
gtsam::FunctorizedFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition FunctorizedFactor.h:99
│ │ │ │ +
gtsam::FunctorizedFactor::FunctorizedFactor
FunctorizedFactor()
default constructor - only use for serialization
Definition FunctorizedFactor.h:69
│ │ │ │ +
gtsam::FunctorizedFactor::access
friend class boost::serialization::access
Serialization function.
Definition FunctorizedFactor.h:120
│ │ │ │ +
gtsam::FunctorizedFactor::clone
NonlinearFactor::shared_ptr clone() const override
Definition FunctorizedFactor.h:85
│ │ │ │ +
gtsam::FunctorizedFactor2
Factor which evaluates provided binary functor and uses the result to compute error with respect to t...
Definition FunctorizedFactor.h:159
│ │ │ │ +
gtsam::FunctorizedFactor2::FunctorizedFactor2
FunctorizedFactor2()
default constructor - only use for serialization
Definition FunctorizedFactor.h:171
│ │ │ │ +
gtsam::FunctorizedFactor2::clone
NonlinearFactor::shared_ptr clone() const override
Definition FunctorizedFactor.h:190
│ │ │ │ +
gtsam::FunctorizedFactor2::FunctorizedFactor2
FunctorizedFactor2(Key key1, Key key2, const R &z, const SharedNoiseModel &model, const FunctionType func)
Construct with given x and the parameters of the basis.
Definition FunctorizedFactor.h:180
│ │ │ │ +
gtsam::FunctorizedFactor2::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition FunctorizedFactor.h:206
│ │ │ │ +
gtsam::FunctorizedFactor2::equals
bool equals(const NonlinearFactor &other, double tol=1e-9) const override
Check if two factors are equal.
Definition FunctorizedFactor.h:218
│ │ │ │ +
gtsam::FunctorizedFactor2::access
friend class boost::serialization::access
Serialization function.
Definition FunctorizedFactor.h:228
│ │ │ │ +
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ +
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Calculate the error of the factor.
Definition NonlinearFactor.cpp:138
│ │ │ │ +
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ +
gtsam::NoiseModelFactorN< T >::key
Key key() const
Returns a key.
Definition NonlinearFactor.h:518
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,232 +1,212 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -NonlinearFactorGraph.h │ │ │ │ │ +FunctorizedFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -20// \callgraph │ │ │ │ │ -21 │ │ │ │ │ -22#pragma once │ │ │ │ │ -23 │ │ │ │ │ -24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g_._h> │ │ │ │ │ -27#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -28#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_P_r_i_o_r_F_a_c_t_o_r_._h> │ │ │ │ │ -29 │ │ │ │ │ -30#include │ │ │ │ │ -31#include │ │ │ │ │ -32 │ │ │ │ │ -33namespace _g_t_s_a_m { │ │ │ │ │ -34 │ │ │ │ │ -35 // Forward declarations │ │ │ │ │ -36 class _V_a_l_u_e_s; │ │ │ │ │ -37 class Ordering; │ │ │ │ │ -38 class GaussianFactorGraph; │ │ │ │ │ -39 class SymbolicFactorGraph; │ │ │ │ │ -40 template │ │ │ │ │ -41 class Expression; │ │ │ │ │ -42 template │ │ │ │ │ -43 class ExpressionFactor; │ │ │ │ │ -44 │ │ │ │ │ -_5_5 class GTSAM_EXPORT _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h: public _F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -{ │ │ │ │ │ -56 │ │ │ │ │ -57 public: │ │ │ │ │ -58 │ │ │ │ │ -59 typedef _F_a_c_t_o_r_G_r_a_p_h_<_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_> _B_a_s_e; │ │ │ │ │ -60 typedef _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h _T_h_i_s; │ │ │ │ │ -61 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +22 │ │ │ │ │ +23#include │ │ │ │ │ +24 │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ +26 │ │ │ │ │ +58template │ │ │ │ │ +_5_9class _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +60 private: │ │ │ │ │ +61 using _B_a_s_e = _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_T_>; │ │ │ │ │ 62 │ │ │ │ │ -65 │ │ │ │ │ -_6_7 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h() {} │ │ │ │ │ -68 │ │ │ │ │ -70 template │ │ │ │ │ -_7_1 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h(ITERATOR firstFactor, ITERATOR lastFactor) : _B_a_s_e │ │ │ │ │ -(firstFactor, lastFactor) {} │ │ │ │ │ -72 │ │ │ │ │ -74 template │ │ │ │ │ -_7_5 explicit _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h(const CONTAINER& factors) : _B_a_s_e(factors) {} │ │ │ │ │ -76 │ │ │ │ │ -78 template │ │ │ │ │ -_7_9 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h(const _F_a_c_t_o_r_G_r_a_p_h_<_D_E_R_I_V_E_D_F_A_C_T_O_R_>& graph) : _B_a_s_e(graph) │ │ │ │ │ -{} │ │ │ │ │ -80 │ │ │ │ │ -_8_2 virtual _~_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h() {} │ │ │ │ │ +63 R measured_; │ │ │ │ │ +64 _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l noiseModel_; │ │ │ │ │ +65 std::function)> func_; │ │ │ │ │ +66 │ │ │ │ │ +67 public: │ │ │ │ │ +_6_9 _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r() {} │ │ │ │ │ +70 │ │ │ │ │ +_7_8 _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r(_K_e_y _k_e_y, const R &z, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, │ │ │ │ │ +79 const std::function)> func) │ │ │ │ │ +80 : _B_a_s_e(model, _k_e_y), measured_(z), noiseModel_(model), func_(func) {} │ │ │ │ │ +81 │ │ │ │ │ +82 _~_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r() override {} │ │ │ │ │ 83 │ │ │ │ │ -87 │ │ │ │ │ -89 void _p_r_i_n_t( │ │ │ │ │ -90 const std::string& str = "NonlinearFactorGraph: ", │ │ │ │ │ -91 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override; │ │ │ │ │ -92 │ │ │ │ │ -94 void printErrors(const _V_a_l_u_e_s& values, const std::string& str = │ │ │ │ │ -"NonlinearFactorGraph: ", │ │ │ │ │ -95 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ -96 const std::function& │ │ │ │ │ -97 printCondition = [](const _F_a_c_t_o_r *,double, size_t) {return true;}) const; │ │ │ │ │ -98 │ │ │ │ │ -100 bool equals(const NonlinearFactorGraph& other, double tol = 1e-9) const; │ │ │ │ │ -101 │ │ │ │ │ -105 │ │ │ │ │ -107 double error(const _V_a_l_u_e_s& values) const; │ │ │ │ │ -108 │ │ │ │ │ -110 double probPrime(const _V_a_l_u_e_s& values) const; │ │ │ │ │ -111 │ │ │ │ │ -115 boost::shared_ptr symbolic() const; │ │ │ │ │ -116 │ │ │ │ │ -120 Ordering orderingCOLAMD() const; │ │ │ │ │ -121 │ │ │ │ │ -130 Ordering orderingCOLAMDConstrained(const FastMap& constraints) │ │ │ │ │ -const; │ │ │ │ │ -131 │ │ │ │ │ -133 boost::shared_ptr linearize(const _V_a_l_u_e_s& │ │ │ │ │ -linearizationPoint) const; │ │ │ │ │ -134 │ │ │ │ │ -_1_3_6 typedef std::function& │ │ │ │ │ -hessianFactor)> _D_a_m_p_e_n; │ │ │ │ │ -137 │ │ │ │ │ -145 boost::shared_ptr linearizeToHessianFactor( │ │ │ │ │ -146 const _V_a_l_u_e_s& values, const _D_a_m_p_e_n& dampen = nullptr) const; │ │ │ │ │ -147 │ │ │ │ │ -156 boost::shared_ptr linearizeToHessianFactor( │ │ │ │ │ -157 const _V_a_l_u_e_s& values, const _O_r_d_e_r_i_n_g& ordering, const _D_a_m_p_e_n& dampen = │ │ │ │ │ -nullptr) const; │ │ │ │ │ -158 │ │ │ │ │ -161 _V_a_l_u_e_s updateCholesky(const _V_a_l_u_e_s& values, │ │ │ │ │ -162 const _D_a_m_p_e_n& dampen = nullptr) const; │ │ │ │ │ -163 │ │ │ │ │ -166 _V_a_l_u_e_s updateCholesky(const _V_a_l_u_e_s& values, const _O_r_d_e_r_i_n_g& ordering, │ │ │ │ │ -167 const _D_a_m_p_e_n& dampen = nullptr) const; │ │ │ │ │ +_8_5 NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +86 return boost::static_pointer_cast( │ │ │ │ │ +87 NonlinearFactor::shared_ptr(new _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_<_R_,_ _T_>(*this))); │ │ │ │ │ +88 } │ │ │ │ │ +89 │ │ │ │ │ +90 Vector evaluateError(const T ¶ms, boost::optional H = │ │ │ │ │ +91 boost::none) const override { │ │ │ │ │ +92 R x = func_(params, H); │ │ │ │ │ +93 Vector _e_r_r_o_r = _t_r_a_i_t_s_<_R_>_:_:_L_o_c_a_l(measured_, x); │ │ │ │ │ +94 return _e_r_r_o_r; │ │ │ │ │ +95 } │ │ │ │ │ +96 │ │ │ │ │ +_9_9 void _p_r_i_n_t( │ │ │ │ │ +100 const std::string &s = "", │ │ │ │ │ +101 const _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ +102 _B_a_s_e_:_:_p_r_i_n_t(s, keyFormatter); │ │ │ │ │ +103 std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor(" │ │ │ │ │ +104 << keyFormatter(this->key1()) << ")" << std::endl; │ │ │ │ │ +105 _t_r_a_i_t_s_<_R_>_:_:_P_r_i_n_t(measured_, " measurement: "); │ │ │ │ │ +106 std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose() │ │ │ │ │ +107 << std::endl; │ │ │ │ │ +108 } │ │ │ │ │ +109 │ │ │ │ │ +_1_1_0 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r &other, double tol = 1e-9) const override │ │ │ │ │ +{ │ │ │ │ │ +111 const _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_<_R_,_ _T_> *e = │ │ │ │ │ +112 dynamic_cast *>(&other); │ │ │ │ │ +113 return e != nullptr && _B_a_s_e_:_:_e_q_u_a_l_s(other, tol) && │ │ │ │ │ +114 _t_r_a_i_t_s_<_R_>_:_:_E_q_u_a_l_s(this->measured_, e->measured_, tol); │ │ │ │ │ +115 } │ │ │ │ │ +117 │ │ │ │ │ +118 private: │ │ │ │ │ +_1_2_0 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +121 template │ │ │ │ │ +122 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ +123 // NoiseModelFactor1 instead of NoiseModelFactorN for backward │ │ │ │ │ +compatibility │ │ │ │ │ +124 ar &boost::serialization::make_nvp( │ │ │ │ │ +125 "NoiseModelFactor1", boost::serialization::base_object(*this)); │ │ │ │ │ +126 ar &BOOST_SERIALIZATION_NVP(measured_); │ │ │ │ │ +127 ar &BOOST_SERIALIZATION_NVP(func_); │ │ │ │ │ +128 } │ │ │ │ │ +129}; │ │ │ │ │ +130 │ │ │ │ │ +132template │ │ │ │ │ +_1_3_3struct _t_r_a_i_t_s<_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r> │ │ │ │ │ +134 : public _T_e_s_t_a_b_l_e> {}; │ │ │ │ │ +135 │ │ │ │ │ +142template │ │ │ │ │ +_1_4_3_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_<_R_,_ _T_> _M_a_k_e_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r(_K_e_y key, const R &z, │ │ │ │ │ +144 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, │ │ │ │ │ +145 const FUNC func) { │ │ │ │ │ +146 return _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_<_R_,_ _T_>(key, z, model, func); │ │ │ │ │ +147} │ │ │ │ │ +148 │ │ │ │ │ +158template │ │ │ │ │ +_1_5_9class _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2 : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ +160 private: │ │ │ │ │ +161 using _B_a_s_e = _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_T_1_,_ _T_2_>; │ │ │ │ │ +162 │ │ │ │ │ +163 R measured_; │ │ │ │ │ +164 _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l noiseModel_; │ │ │ │ │ +165 using FunctionType = std::function, │ │ │ │ │ +166 boost::optional)>; │ │ │ │ │ +167 FunctionType func_; │ │ │ │ │ 168 │ │ │ │ │ -170 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h clone() const; │ │ │ │ │ -171 │ │ │ │ │ -181 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h rekey(const std::map& rekey_mapping) const; │ │ │ │ │ -182 │ │ │ │ │ -189 template │ │ │ │ │ -_1_9_0 void _a_d_d_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r(const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& R, const T& z, │ │ │ │ │ -191 const _E_x_p_r_e_s_s_i_o_n_<_T_>& h) { │ │ │ │ │ -192 push_back(boost::make_shared<_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r_<_T_> >(R, z, h)); │ │ │ │ │ +169 public: │ │ │ │ │ +_1_7_1 _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2() {} │ │ │ │ │ +172 │ │ │ │ │ +_1_8_0 _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2(_K_e_y key1, _K_e_y key2, const R &z, │ │ │ │ │ +181 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, const FunctionType func) │ │ │ │ │ +182 : _B_a_s_e(model, key1, key2), │ │ │ │ │ +183 measured_(z), │ │ │ │ │ +184 noiseModel_(model), │ │ │ │ │ +185 func_(func) {} │ │ │ │ │ +186 │ │ │ │ │ +187 _~_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2() override {} │ │ │ │ │ +188 │ │ │ │ │ +_1_9_0 NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +191 return boost::static_pointer_cast( │ │ │ │ │ +192 NonlinearFactor::shared_ptr(new _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2_<_R_,_ _T_1_,_ _T_2_>(*this))); │ │ │ │ │ 193 } │ │ │ │ │ 194 │ │ │ │ │ -201 template │ │ │ │ │ -_2_0_2 void _a_d_d_P_r_i_o_r(_K_e_y key, const T& prior, │ │ │ │ │ -203 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model = nullptr) { │ │ │ │ │ -204 emplace_shared>(key, prior, model); │ │ │ │ │ -205 } │ │ │ │ │ -206 │ │ │ │ │ -217 template │ │ │ │ │ -_2_1_8 void _a_d_d_P_r_i_o_r(_K_e_y key, const T& prior, const Matrix& covariance) { │ │ │ │ │ -219 emplace_shared>(key, prior, covariance); │ │ │ │ │ -220 } │ │ │ │ │ -221 │ │ │ │ │ -225 │ │ │ │ │ -226 using FactorGraph::dot; │ │ │ │ │ -227 using FactorGraph::saveGraph; │ │ │ │ │ -228 │ │ │ │ │ -230 void _d_o_t(std::ostream& os, const _V_a_l_u_e_s& values, │ │ │ │ │ -231 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ -232 const _G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g& writer = _G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g()) const; │ │ │ │ │ -233 │ │ │ │ │ -235 std::string _d_o_t( │ │ │ │ │ -236 const _V_a_l_u_e_s& values, │ │ │ │ │ -237 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ -238 const _G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g& writer = _G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g()) const; │ │ │ │ │ -239 │ │ │ │ │ -241 void saveGraph( │ │ │ │ │ -242 const std::string& filename, const _V_a_l_u_e_s& values, │ │ │ │ │ -243 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter, │ │ │ │ │ -244 const _G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g& writer = _G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g()) const; │ │ │ │ │ -246 │ │ │ │ │ -247 private: │ │ │ │ │ -248 │ │ │ │ │ -253 boost::shared_ptr linearizeToHessianFactor( │ │ │ │ │ -254 const _V_a_l_u_e_s& values, const _S_c_a_t_t_e_r& scatter, const Dampen& dampen = │ │ │ │ │ -nullptr) const; │ │ │ │ │ -255 │ │ │ │ │ -_2_5_7 friend class boost::serialization::access; │ │ │ │ │ -258 template │ │ │ │ │ -259 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -260 ar & boost::serialization::make_nvp("NonlinearFactorGraph", │ │ │ │ │ -261 boost::serialization::base_object(*this)); │ │ │ │ │ -262 } │ │ │ │ │ -263 │ │ │ │ │ -264 public: │ │ │ │ │ -265 │ │ │ │ │ -266#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ -269 │ │ │ │ │ -270 boost::shared_ptr GTSAM_DEPRECATED linearizeToHessianFactor( │ │ │ │ │ -271 const _V_a_l_u_e_s& values, boost::none_t, const Dampen& dampen = nullptr) const │ │ │ │ │ -272 {return linearizeToHessianFactor(values, dampen);} │ │ │ │ │ -273 │ │ │ │ │ -275 _V_a_l_u_e_s GTSAM_DEPRECATED updateCholesky(const _V_a_l_u_e_s& values, boost::none_t, │ │ │ │ │ -276 const Dampen& dampen = nullptr) const │ │ │ │ │ -277 {return updateCholesky(values, dampen);} │ │ │ │ │ -278 │ │ │ │ │ -280 void GTSAM_DEPRECATED saveGraph( │ │ │ │ │ -281 std::ostream& os, const _V_a_l_u_e_s& values = _V_a_l_u_e_s(), │ │ │ │ │ -282 const GraphvizFormatting& graphvizFormatting = GraphvizFormatting(), │ │ │ │ │ -283 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { │ │ │ │ │ -284 _d_o_t(os, values, keyFormatter, graphvizFormatting); │ │ │ │ │ -285 } │ │ │ │ │ -287 void GTSAM_DEPRECATED │ │ │ │ │ -288 saveGraph(const std::string& filename, const _V_a_l_u_e_s& values, │ │ │ │ │ -289 const GraphvizFormatting& graphvizFormatting, │ │ │ │ │ -290 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { │ │ │ │ │ -291 saveGraph(filename, values, keyFormatter, graphvizFormatting); │ │ │ │ │ -292 } │ │ │ │ │ -294#endif │ │ │ │ │ -295 │ │ │ │ │ -296 }; │ │ │ │ │ -297 │ │ │ │ │ -299template<> │ │ │ │ │ -_3_0_0struct _t_r_a_i_t_s<_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h> : public _T_e_s_t_a_b_l_e │ │ │ │ │ +195 Vector evaluateError( │ │ │ │ │ +196 const T1 ¶ms1, const T2 ¶ms2, │ │ │ │ │ +197 boost::optional H1 = boost::none, │ │ │ │ │ +198 boost::optional H2 = boost::none) const override { │ │ │ │ │ +199 R x = func_(params1, params2, H1, H2); │ │ │ │ │ +200 Vector _e_r_r_o_r = _t_r_a_i_t_s_<_R_>_:_:_L_o_c_a_l(measured_, x); │ │ │ │ │ +201 return _e_r_r_o_r; │ │ │ │ │ +202 } │ │ │ │ │ +203 │ │ │ │ │ +_2_0_6 void _p_r_i_n_t( │ │ │ │ │ +207 const std::string &s = "", │ │ │ │ │ +208 const _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ +209 _B_a_s_e_:_:_p_r_i_n_t(s, keyFormatter); │ │ │ │ │ +210 std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor2(" │ │ │ │ │ +211 << keyFormatter(this->key1()) << ", " │ │ │ │ │ +212 << keyFormatter(this->key2()) << ")" << std::endl; │ │ │ │ │ +213 _t_r_a_i_t_s_<_R_>_:_:_P_r_i_n_t(measured_, " measurement: "); │ │ │ │ │ +214 std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose() │ │ │ │ │ +215 << std::endl; │ │ │ │ │ +216 } │ │ │ │ │ +217 │ │ │ │ │ +_2_1_8 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r &other, double tol = 1e-9) const override │ │ │ │ │ { │ │ │ │ │ -301}; │ │ │ │ │ -302 │ │ │ │ │ -303} //\ namespace gtsam │ │ │ │ │ -304 │ │ │ │ │ -_P_o_i_n_t_2_._h │ │ │ │ │ -2D Point │ │ │ │ │ -_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Factor Graph Base Class. │ │ │ │ │ -_G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g_._h │ │ │ │ │ -Graphviz formatter for NonlinearFactorGraph. │ │ │ │ │ +219 const _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2_<_R_,_ _T_1_,_ _T_2_> *e = │ │ │ │ │ +220 dynamic_cast *>(&other); │ │ │ │ │ +221 return e && _B_a_s_e_:_:_e_q_u_a_l_s(other, tol) && │ │ │ │ │ +222 _t_r_a_i_t_s_<_R_>_:_:_E_q_u_a_l_s(this->measured_, e->measured_, tol); │ │ │ │ │ +223 } │ │ │ │ │ +225 │ │ │ │ │ +226 private: │ │ │ │ │ +_2_2_8 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +229 template │ │ │ │ │ +230 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ +231 // NoiseModelFactor2 instead of NoiseModelFactorN for backward │ │ │ │ │ +compatibility │ │ │ │ │ +232 ar &boost::serialization::make_nvp( │ │ │ │ │ +233 "NoiseModelFactor2", boost::serialization::base_object(*this)); │ │ │ │ │ +234 ar &BOOST_SERIALIZATION_NVP(measured_); │ │ │ │ │ +235 ar &BOOST_SERIALIZATION_NVP(func_); │ │ │ │ │ +236 } │ │ │ │ │ +237}; │ │ │ │ │ +238 │ │ │ │ │ +240template │ │ │ │ │ +_2_4_1struct _t_r_a_i_t_s<_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2> │ │ │ │ │ +242 : public _T_e_s_t_a_b_l_e> {}; │ │ │ │ │ +243 │ │ │ │ │ +250template │ │ │ │ │ +_2_5_1_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2_<_R_,_ _T_1_,_ _T_2_> _M_a_k_e_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2( │ │ │ │ │ +252 _K_e_y key1, _K_e_y key2, const R &z, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, │ │ │ │ │ +253 const FUNC func) { │ │ │ │ │ +254 return _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2_<_R_,_ _T_1_,_ _T_2_>(key1, key2, z, model, func); │ │ │ │ │ +255} │ │ │ │ │ +256 │ │ │ │ │ +257} // namespace gtsam │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_k_e_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2 │ │ │ │ │ +FunctorizedFactor2< R, T1, T2 > MakeFunctorizedFactor2(Key key1, Key key2, │ │ │ │ │ +const R &z, const SharedNoiseModel &model, const FUNC func) │ │ │ │ │ +Helper function to create a functorized factor. │ │ │ │ │ +DDeeffiinniittiioonn FunctorizedFactor.h:251 │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_k_e_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r │ │ │ │ │ +FunctorizedFactor< R, T > MakeFunctorizedFactor(Key key, const R &z, const │ │ │ │ │ +SharedNoiseModel &model, const FUNC func) │ │ │ │ │ +Helper function to create a functorized factor. │ │ │ │ │ +DDeeffiinniittiioonn FunctorizedFactor.h:143 │ │ │ │ │ _g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ Aliases. │ │ │ │ │ DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ -_g_t_s_a_m_:_:_d_o_t │ │ │ │ │ -double dot(const V1 &a, const V2 &b) │ │ │ │ │ -Dot product. │ │ │ │ │ -DDeeffiinniittiioonn Vector.h:195 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ @@ -234,81 +214,93 @@ │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ _g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ A helper that implements the traits interface for GTSAM types. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ -nodes. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:68 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_S_c_a_t_t_e_r │ │ │ │ │ -Scatter is an intermediate data structure used when building a HessianFactor │ │ │ │ │ -incrementally,... │ │ │ │ │ -DDeeffiinniittiioonn Scatter.h:49 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r │ │ │ │ │ -Factor that supports arbitrary expressions via AD. │ │ │ │ │ -DDeeffiinniittiioonn ExpressionFactor.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n │ │ │ │ │ -Expression class that supports automatic differentiation. │ │ │ │ │ -DDeeffiinniittiioonn Expression.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_G_r_a_p_h_v_i_z_F_o_r_m_a_t_t_i_n_g │ │ │ │ │ -Formatting options and functions for saving a NonlinearFactorGraph instance in │ │ │ │ │ -GraphViz format. │ │ │ │ │ -DDeeffiinniittiioonn GraphvizFormatting.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -NonlinearFactorGraph() │ │ │ │ │ -Default constructor. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactorGraph.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d_P_r_i_o_r │ │ │ │ │ -void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr) │ │ │ │ │ -Convenience method which adds a PriorFactor to the factor graph. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactorGraph.h:202 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -NonlinearFactorGraph(const CONTAINER &factors) │ │ │ │ │ -Construct from container of factors (shared_ptr or plain objects) │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactorGraph.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r │ │ │ │ │ -void addExpressionFactor(const SharedNoiseModel &R, const T &z, const │ │ │ │ │ -Expression< T > &h) │ │ │ │ │ -Directly add ExpressionFactor that implements |h(x)-z|^2_R. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactorGraph.h:190 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d_P_r_i_o_r │ │ │ │ │ -void addPrior(Key key, const T &prior, const Matrix &covariance) │ │ │ │ │ -Convenience method which adds a PriorFactor to the factor graph. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactorGraph.h:218 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -NonlinearFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) │ │ │ │ │ -Construct from iterator over factors. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactorGraph.h:71 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_D_a_m_p_e_n │ │ │ │ │ -std::function< void(const boost::shared_ptr< HessianFactor > &hessianFactor)> │ │ │ │ │ -Dampen │ │ │ │ │ -typdef for dampen functions used below │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactorGraph.h:136 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -NonlinearFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph) │ │ │ │ │ -Implicit copy/downcast constructor to override explicit template container │ │ │ │ │ -constructor. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactorGraph.h:79 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_~_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -virtual ~NonlinearFactorGraph() │ │ │ │ │ -Destructor. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactorGraph.h:82 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_V_a_l_u_e_s │ │ │ │ │ -In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ -a non-linear function... │ │ │ │ │ -_P_r_i_o_r_F_a_c_t_o_r_._h │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +virtual void print(const std::string &s="Factor", const KeyFormatter │ │ │ │ │ +&formatter=DefaultKeyFormatter) const │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn Factor.cpp:29 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const This &other, double tol=1e-9) const │ │ │ │ │ +check equality │ │ │ │ │ +DDeeffiinniittiioonn Factor.cpp:42 │ │ │ │ │ +_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r │ │ │ │ │ +Factor which evaluates provided unary functor and uses the result to compute │ │ │ │ │ +error with respect to th... │ │ │ │ │ +DDeeffiinniittiioonn FunctorizedFactor.h:59 │ │ │ │ │ +_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r │ │ │ │ │ +FunctorizedFactor(Key key, const R &z, const SharedNoiseModel &model, const │ │ │ │ │ +std::function< R(T, boost::optional< Matrix & >)> func) │ │ │ │ │ +Construct with given x and the parameters of the basis. │ │ │ │ │ +DDeeffiinniittiioonn FunctorizedFactor.h:78 │ │ │ │ │ +_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const NonlinearFactor &other, double tol=1e-9) const override │ │ │ │ │ +Check if two factors are equal. │ │ │ │ │ +DDeeffiinniittiioonn FunctorizedFactor.h:110 │ │ │ │ │ +_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn FunctorizedFactor.h:99 │ │ │ │ │ +_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r │ │ │ │ │ +FunctorizedFactor() │ │ │ │ │ +default constructor - only use for serialization │ │ │ │ │ +DDeeffiinniittiioonn FunctorizedFactor.h:69 │ │ │ │ │ +_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn FunctorizedFactor.h:120 │ │ │ │ │ +_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ +NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn FunctorizedFactor.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2 │ │ │ │ │ +Factor which evaluates provided binary functor and uses the result to compute │ │ │ │ │ +error with respect to t... │ │ │ │ │ +DDeeffiinniittiioonn FunctorizedFactor.h:159 │ │ │ │ │ +_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2 │ │ │ │ │ +FunctorizedFactor2() │ │ │ │ │ +default constructor - only use for serialization │ │ │ │ │ +DDeeffiinniittiioonn FunctorizedFactor.h:171 │ │ │ │ │ +_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2_:_:_c_l_o_n_e │ │ │ │ │ +NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn FunctorizedFactor.h:190 │ │ │ │ │ +_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2 │ │ │ │ │ +FunctorizedFactor2(Key key1, Key key2, const R &z, const SharedNoiseModel │ │ │ │ │ +&model, const FunctionType func) │ │ │ │ │ +Construct with given x and the parameters of the basis. │ │ │ │ │ +DDeeffiinniittiioonn FunctorizedFactor.h:180 │ │ │ │ │ +_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn FunctorizedFactor.h:206 │ │ │ │ │ +_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const NonlinearFactor &other, double tol=1e-9) const override │ │ │ │ │ +Check if two factors are equal. │ │ │ │ │ +DDeeffiinniittiioonn FunctorizedFactor.h:218 │ │ │ │ │ +_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn FunctorizedFactor.h:228 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ +Nonlinear factor base class. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_e_r_r_o_r │ │ │ │ │ +double error(const Values &c) const override │ │ │ │ │ +Calculate the error of the factor. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.cpp:138 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ +A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ +variables. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_ _T_ _>_:_:_k_e_y │ │ │ │ │ +Key key() const │ │ │ │ │ +Returns a key. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:518 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ + * _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01097_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01103_source.html │ │ │ │┄ Files 85% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Values-inl.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2-impl.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
Values-inl.h
│ │ │ │ +
ISAM2-impl.h
│ │ │ │
│ │ │ │
│ │ │ │ -
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
25#pragma once
│ │ │ │ -
26
│ │ │ │ -
27#include <utility>
│ │ │ │ -
28#include <boost/bind/bind.hpp>
│ │ │ │ -
29#include <gtsam/nonlinear/Values.h>
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/nonlinear/ISAM2.h>
│ │ │ │ +
22#include <gtsam/nonlinear/ISAM2Result.h>
│ │ │ │ +
23
│ │ │ │ +
24#include <gtsam/base/debug.h>
│ │ │ │ +
25#include <gtsam/inference/JunctionTree-inst.h> // We need the inst file because we'll make a special JT templated on ISAM2
│ │ │ │ +
26#include <gtsam/inference/Symbol.h>
│ │ │ │ +
27#include <gtsam/inference/VariableIndex.h>
│ │ │ │ +
28#include <gtsam/linear/GaussianBayesTree.h>
│ │ │ │ +
29#include <gtsam/linear/GaussianEliminationTree.h>
│ │ │ │
30
│ │ │ │ -
31namespace gtsam {
│ │ │ │ -
32
│ │ │ │ -
33
│ │ │ │ -
34 /* ************************************************************************* */
│ │ │ │ -
35 template<class ValueType>
│ │ │ │ -
│ │ │ │ -
36 struct _ValuesKeyValuePair {
│ │ │ │ -
37 const Key key;
│ │ │ │ -
38 ValueType& value;
│ │ │ │ -
39
│ │ │ │ -
40 _ValuesKeyValuePair(Key _key, ValueType& _value) : key(_key), value(_value) {}
│ │ │ │ -
41 };
│ │ │ │ -
│ │ │ │ +
31#include <boost/range/adaptors.hpp>
│ │ │ │ +
32#include <boost/range/algorithm/copy.hpp>
│ │ │ │ +
33namespace br {
│ │ │ │ +
34using namespace boost::range;
│ │ │ │ +
35using namespace boost::adaptors;
│ │ │ │ +
36} // namespace br
│ │ │ │ +
37
│ │ │ │ +
38#include <algorithm>
│ │ │ │ +
39#include <limits>
│ │ │ │ +
40#include <string>
│ │ │ │ +
41#include <utility>
│ │ │ │
42
│ │ │ │ -
43 /* ************************************************************************* */
│ │ │ │ -
44 template<class ValueType>
│ │ │ │ -
│ │ │ │ -
45 struct _ValuesConstKeyValuePair {
│ │ │ │ -
46 const Key key;
│ │ │ │ -
47 const ValueType& value;
│ │ │ │ -
48
│ │ │ │ -
49 _ValuesConstKeyValuePair(Key _key, const ValueType& _value) :
│ │ │ │ -
50 key(_key), value(_value) {
│ │ │ │ -
51 }
│ │ │ │ -
52 _ValuesConstKeyValuePair(const _ValuesKeyValuePair<ValueType>& rhs) :
│ │ │ │ -
53 key(rhs.key), value(rhs.value) {
│ │ │ │ -
54 }
│ │ │ │ -
55 };
│ │ │ │ +
43namespace gtsam {
│ │ │ │ +
44
│ │ │ │ +
45/* ************************************************************************* */
│ │ │ │ +
46// Special BayesTree class that uses ISAM2 cliques - this is the result of
│ │ │ │ +
47// reeliminating ISAM2 subtrees.
│ │ │ │ +
│ │ │ │ +
48class ISAM2BayesTree : public ISAM2::Base {
│ │ │ │ +
49 public:
│ │ │ │ +
50 typedef ISAM2::Base Base;
│ │ │ │ +
51 typedef ISAM2BayesTree This;
│ │ │ │ +
52 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
53
│ │ │ │ +
54 ISAM2BayesTree() {}
│ │ │ │ +
55};
│ │ │ │
│ │ │ │
56
│ │ │ │ -
57 /* ************************************************************************* */
│ │ │ │ -
58
│ │ │ │ -
59 // Cast helpers for making _Values[Const]KeyValuePair's from Values::[Const]KeyValuePair
│ │ │ │ -
60 // need to use a struct here for later partial specialization
│ │ │ │ -
61 template<class ValueType, class CastedKeyValuePairType, class KeyValuePairType>
│ │ │ │ -
│ │ │ │ -
62 struct ValuesCastHelper {
│ │ │ │ -
63 static CastedKeyValuePairType cast(KeyValuePairType key_value) {
│ │ │ │ -
64 // Static cast because we already checked the type during filtering
│ │ │ │ -
65 return CastedKeyValuePairType(key_value.key,
│ │ │ │ -
66 const_cast<GenericValue<ValueType>&>(static_cast<const GenericValue<
│ │ │ │ -
67 ValueType>&>(key_value.value)).value());
│ │ │ │ -
68 }
│ │ │ │ -
69 };
│ │ │ │ +
57/* ************************************************************************* */
│ │ │ │ +
58// Special JunctionTree class that produces ISAM2 BayesTree cliques, used for
│ │ │ │ +
59// reeliminating ISAM2 subtrees.
│ │ │ │ +
│ │ │ │ +
60class ISAM2JunctionTree
│ │ │ │ +
61 : public JunctionTree<ISAM2BayesTree, GaussianFactorGraph> {
│ │ │ │ +
62 public:
│ │ │ │ +
63 typedef JunctionTree<ISAM2BayesTree, GaussianFactorGraph> Base;
│ │ │ │ +
64 typedef ISAM2JunctionTree This;
│ │ │ │ +
65 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
66
│ │ │ │ +
67 explicit ISAM2JunctionTree(const GaussianEliminationTree& eliminationTree)
│ │ │ │ +
68 : Base(eliminationTree) {}
│ │ │ │ +
69};
│ │ │ │
│ │ │ │ -
70 // partial specialized version for ValueType == Value
│ │ │ │ -
71 template<class CastedKeyValuePairType, class KeyValuePairType>
│ │ │ │ +
70
│ │ │ │ +
71/* ************************************************************************* */
│ │ │ │
│ │ │ │ -
72 struct ValuesCastHelper<Value, CastedKeyValuePairType, KeyValuePairType> {
│ │ │ │ -
73 static CastedKeyValuePairType cast(KeyValuePairType key_value) {
│ │ │ │ -
74 // Static cast because we already checked the type during filtering
│ │ │ │ -
75 // in this case the casted and keyvalue pair are essentially the same type
│ │ │ │ -
76 // (key, Value&) so perhaps this could be done with just a cast of the key_value?
│ │ │ │ -
77 return CastedKeyValuePairType(key_value.key, key_value.value);
│ │ │ │ -
78 }
│ │ │ │ -
79 };
│ │ │ │ -
│ │ │ │ -
80 // partial specialized version for ValueType == Value
│ │ │ │ -
81 template<class CastedKeyValuePairType, class KeyValuePairType>
│ │ │ │ -
│ │ │ │ -
82 struct ValuesCastHelper<const Value, CastedKeyValuePairType, KeyValuePairType> {
│ │ │ │ -
83 static CastedKeyValuePairType cast(KeyValuePairType key_value) {
│ │ │ │ -
84 // Static cast because we already checked the type during filtering
│ │ │ │ -
85 // in this case the casted and keyvalue pair are essentially the same type
│ │ │ │ -
86 // (key, Value&) so perhaps this could be done with just a cast of the key_value?
│ │ │ │ -
87 return CastedKeyValuePairType(key_value.key, key_value.value);
│ │ │ │ -
88 }
│ │ │ │ -
89 };
│ │ │ │ -
│ │ │ │ -
90
│ │ │ │ -
91/* ************************************************************************* */
│ │ │ │ -
92 template <class ValueType>
│ │ │ │ -
93 std::map<Key, ValueType>
│ │ │ │ -
│ │ │ │ -
94 Values::extract(const std::function<bool(Key)>& filterFcn) const {
│ │ │ │ -
95 std::map<Key, ValueType> result;
│ │ │ │ -
96 for (const auto& key_value : values_) {
│ │ │ │ -
97 // Check if key matches
│ │ │ │ -
98 if (filterFcn(key_value.first)) {
│ │ │ │ -
99 // Check if type matches (typically does as symbols matched with types)
│ │ │ │ -
100 if (auto t =
│ │ │ │ -
101 dynamic_cast<const GenericValue<ValueType>*>(key_value.second))
│ │ │ │ -
102 result[key_value.first] = t->value();
│ │ │ │ -
103 }
│ │ │ │ -
104 }
│ │ │ │ -
105 return result;
│ │ │ │ -
106 }
│ │ │ │ +
72struct GTSAM_EXPORT DeltaImpl {
│ │ │ │ +
│ │ │ │ +
73 struct GTSAM_EXPORT PartialSolveResult {
│ │ │ │ +
74 ISAM2::sharedClique bayesTree;
│ │ │ │ +
75 };
│ │ │ │ +
│ │ │ │ +
76
│ │ │ │ +
│ │ │ │ +
77 struct GTSAM_EXPORT ReorderingMode {
│ │ │ │ +
78 size_t nFullSystemVars;
│ │ │ │ +
79 enum { /*AS_ADDED,*/ COLAMD } algorithm;
│ │ │ │ +
80 enum { NO_CONSTRAINT, CONSTRAIN_LAST } constrain;
│ │ │ │ +
81 boost::optional<FastMap<Key, int> > constrainedKeys;
│ │ │ │ +
82 };
│ │ │ │ +
│ │ │ │ +
83
│ │ │ │ +
87 static size_t UpdateGaussNewtonDelta(const ISAM2::Roots& roots,
│ │ │ │ +
88 const KeySet& replacedKeys,
│ │ │ │ +
89 double wildfireThreshold,
│ │ │ │ +
90 VectorValues* delta);
│ │ │ │ +
91
│ │ │ │ +
96 static size_t UpdateRgProd(const ISAM2::Roots& roots,
│ │ │ │ +
97 const KeySet& replacedKeys,
│ │ │ │ +
98 const VectorValues& gradAtZero,
│ │ │ │ +
99 VectorValues* RgProd);
│ │ │ │ +
100
│ │ │ │ +
104 static VectorValues ComputeGradientSearch(const VectorValues& gradAtZero,
│ │ │ │ +
105 const VectorValues& RgProd);
│ │ │ │ +
106};
│ │ │ │
│ │ │ │
107
│ │ │ │
108/* ************************************************************************* */
│ │ │ │ -
109#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ -
110#include <boost/bind/bind.hpp>
│ │ │ │ -
111
│ │ │ │ -
112 template<class ValueType>
│ │ │ │ -
113 class Values::Filtered {
│ │ │ │ -
114 public:
│ │ │ │ -
116 typedef _ValuesKeyValuePair<ValueType> KeyValuePair;
│ │ │ │ -
117 typedef _ValuesConstKeyValuePair<ValueType> ConstKeyValuePair;
│ │ │ │ -
118 typedef KeyValuePair value_type;
│ │ │ │ +
│ │ │ │ +
114struct GTSAM_EXPORT UpdateImpl {
│ │ │ │ +
115 const ISAM2Params& params_;
│ │ │ │ +
116 const ISAM2UpdateParams& updateParams_;
│ │ │ │ +
117 UpdateImpl(const ISAM2Params& params, const ISAM2UpdateParams& updateParams)
│ │ │ │ +
118 : params_(params), updateParams_(updateParams) {}
│ │ │ │
119
│ │ │ │ -
120 typedef
│ │ │ │ -
121 boost::transform_iterator<
│ │ │ │ -
122 KeyValuePair(*)(Values::KeyValuePair),
│ │ │ │ -
123 boost::filter_iterator<
│ │ │ │ -
124 std::function<bool(const Values::ConstKeyValuePair&)>,
│ │ │ │ -
125 Values::iterator> >
│ │ │ │ -
126 iterator;
│ │ │ │ -
127
│ │ │ │ -
128 typedef iterator const_iterator;
│ │ │ │ -
129
│ │ │ │ -
130 typedef
│ │ │ │ -
131 boost::transform_iterator<
│ │ │ │ -
132 ConstKeyValuePair(*)(Values::ConstKeyValuePair),
│ │ │ │ -
133 boost::filter_iterator<
│ │ │ │ -
134 std::function<bool(const Values::ConstKeyValuePair&)>,
│ │ │ │ -
135 Values::const_iterator> >
│ │ │ │ -
136 const_const_iterator;
│ │ │ │ -
137
│ │ │ │ -
138 iterator begin() { return begin_; }
│ │ │ │ -
139 iterator end() { return end_; }
│ │ │ │ -
140 const_iterator begin() const { return begin_; }
│ │ │ │ -
141 const_iterator end() const { return end_; }
│ │ │ │ -
142 const_const_iterator beginConst() const { return constBegin_; }
│ │ │ │ -
143 const_const_iterator endConst() const { return constEnd_; }
│ │ │ │ +
120 // Provide some debugging information at the start of update
│ │ │ │ +
121 static void LogStartingUpdate(const NonlinearFactorGraph& newFactors,
│ │ │ │ +
122 const ISAM2& isam2) {
│ │ │ │ +
123 gttic(pushBackFactors);
│ │ │ │ +
124 const bool debug = ISDEBUG("ISAM2 update");
│ │ │ │ +
125 const bool verbose = ISDEBUG("ISAM2 update verbose");
│ │ │ │ +
126
│ │ │ │ +
127 if (verbose) {
│ │ │ │ +
128 std::cout << "ISAM2::update\n";
│ │ │ │ +
129 isam2.print("ISAM2: ");
│ │ │ │ +
130 }
│ │ │ │ +
131
│ │ │ │ +
132 if (debug || verbose) {
│ │ │ │ +
133 newFactors.print("The new factors are: ");
│ │ │ │ +
134 }
│ │ │ │ +
135 }
│ │ │ │ +
136
│ │ │ │ +
137 // Check relinearization if we're at the nth step, or we are using a looser
│ │ │ │ +
138 // loop relinerization threshold.
│ │ │ │ +
139 bool relinarizationNeeded(size_t update_count) const {
│ │ │ │ +
140 return updateParams_.force_relinearize ||
│ │ │ │ +
141 (params_.enableRelinearization &&
│ │ │ │ +
142 update_count % params_.relinearizeSkip == 0);
│ │ │ │ +
143 }
│ │ │ │
144
│ │ │ │ -
146 size_t size() const {
│ │ │ │ -
147 size_t i = 0;
│ │ │ │ -
148 for (const_const_iterator it = beginConst(); it != endConst(); ++it)
│ │ │ │ -
149 ++i;
│ │ │ │ -
150 return i;
│ │ │ │ -
151 }
│ │ │ │ -
152
│ │ │ │ -
153 private:
│ │ │ │ -
154 Filtered(
│ │ │ │ -
155 const std::function<bool(const Values::ConstKeyValuePair&)>& filter,
│ │ │ │ -
156 Values& values) :
│ │ │ │ -
157 begin_(
│ │ │ │ -
158 boost::make_transform_iterator(
│ │ │ │ -
159 boost::make_filter_iterator(filter, values.begin(), values.end()),
│ │ │ │ -
160 &ValuesCastHelper<ValueType, KeyValuePair, Values::KeyValuePair>::cast)), end_(
│ │ │ │ -
161 boost::make_transform_iterator(
│ │ │ │ -
162 boost::make_filter_iterator(filter, values.end(), values.end()),
│ │ │ │ -
163 &ValuesCastHelper<ValueType, KeyValuePair, Values::KeyValuePair>::cast)), constBegin_(
│ │ │ │ -
164 boost::make_transform_iterator(
│ │ │ │ -
165 boost::make_filter_iterator(filter,
│ │ │ │ -
166 values._begin(),
│ │ │ │ -
167 values._end()),
│ │ │ │ -
168 &ValuesCastHelper<ValueType, ConstKeyValuePair,
│ │ │ │ -
169 Values::ConstKeyValuePair>::cast)), constEnd_(
│ │ │ │ -
170 boost::make_transform_iterator(
│ │ │ │ -
171 boost::make_filter_iterator(filter,
│ │ │ │ -
172 values._end(),
│ │ │ │ -
173 values._end()),
│ │ │ │ -
174 &ValuesCastHelper<ValueType, ConstKeyValuePair,
│ │ │ │ -
175 Values::ConstKeyValuePair>::cast)) {
│ │ │ │ -
176 }
│ │ │ │ +
145 // Add any new factors \Factors:=\Factors\cup\Factors'.
│ │ │ │ +
146 void pushBackFactors(const NonlinearFactorGraph& newFactors,
│ │ │ │ +
147 NonlinearFactorGraph* nonlinearFactors,
│ │ │ │ +
148 GaussianFactorGraph* linearFactors,
│ │ │ │ +
149 VariableIndex* variableIndex,
│ │ │ │ +
150 FactorIndices* newFactorsIndices,
│ │ │ │ +
151 KeySet* keysWithRemovedFactors) const {
│ │ │ │ +
152 gttic(pushBackFactors);
│ │ │ │ +
153
│ │ │ │ +
154 // Perform the first part of the bookkeeping updates for adding new factors.
│ │ │ │ +
155 // Adds them to the complete list of nonlinear factors, and populates the
│ │ │ │ +
156 // list of new factor indices, both optionally finding and reusing empty
│ │ │ │ +
157 // factor slots.
│ │ │ │ +
158 *newFactorsIndices = nonlinearFactors->add_factors(
│ │ │ │ +
159 newFactors, params_.findUnusedFactorSlots);
│ │ │ │ +
160
│ │ │ │ +
161 // Remove the removed factors
│ │ │ │ +
162 NonlinearFactorGraph removedFactors;
│ │ │ │ +
163 removedFactors.reserve(updateParams_.removeFactorIndices.size());
│ │ │ │ +
164 for (const auto index : updateParams_.removeFactorIndices) {
│ │ │ │ +
165 removedFactors.push_back(nonlinearFactors->at(index));
│ │ │ │ +
166 nonlinearFactors->remove(index);
│ │ │ │ +
167 if (params_.cacheLinearizedFactors) linearFactors->remove(index);
│ │ │ │ +
168 }
│ │ │ │ +
169
│ │ │ │ +
170 // Remove removed factors from the variable index so we do not attempt to
│ │ │ │ +
171 // relinearize them
│ │ │ │ +
172 variableIndex->remove(updateParams_.removeFactorIndices.begin(),
│ │ │ │ +
173 updateParams_.removeFactorIndices.end(),
│ │ │ │ +
174 removedFactors);
│ │ │ │ +
175 *keysWithRemovedFactors = removedFactors.keys();
│ │ │ │ +
176 }
│ │ │ │
177
│ │ │ │ -
178 friend class Values;
│ │ │ │ -
179 iterator begin_;
│ │ │ │ -
180 iterator end_;
│ │ │ │ -
181 const_const_iterator constBegin_;
│ │ │ │ -
182 const_const_iterator constEnd_;
│ │ │ │ -
183 };
│ │ │ │ -
184
│ │ │ │ -
185 template<class ValueType>
│ │ │ │ -
186 class Values::ConstFiltered {
│ │ │ │ -
187 public:
│ │ │ │ -
189 typedef _ValuesConstKeyValuePair<ValueType> KeyValuePair;
│ │ │ │ -
190 typedef KeyValuePair value_type;
│ │ │ │ -
191
│ │ │ │ -
192 typedef typename Filtered<ValueType>::const_const_iterator iterator;
│ │ │ │ -
193 typedef typename Filtered<ValueType>::const_const_iterator const_iterator;
│ │ │ │ -
194
│ │ │ │ -
196 ConstFiltered(const Filtered<ValueType>& rhs) :
│ │ │ │ -
197 begin_(rhs.beginConst()),
│ │ │ │ -
198 end_(rhs.endConst()) {}
│ │ │ │ -
199
│ │ │ │ -
200 iterator begin() { return begin_; }
│ │ │ │ -
201 iterator end() { return end_; }
│ │ │ │ -
202 const_iterator begin() const { return begin_; }
│ │ │ │ -
203 const_iterator end() const { return end_; }
│ │ │ │ -
204
│ │ │ │ -
206 size_t size() const {
│ │ │ │ -
207 size_t i = 0;
│ │ │ │ -
208 for (const_iterator it = begin(); it != end(); ++it)
│ │ │ │ -
209 ++i;
│ │ │ │ -
210 return i;
│ │ │ │ -
211 }
│ │ │ │ -
212
│ │ │ │ -
213 FastList<Key> keys() const {
│ │ │ │ -
214 FastList<Key> result;
│ │ │ │ -
215 for(const_iterator it = begin(); it != end(); ++it)
│ │ │ │ -
216 result.push_back(it->key);
│ │ │ │ -
217 return result;
│ │ │ │ -
218 }
│ │ │ │ -
219
│ │ │ │ -
220 private:
│ │ │ │ -
221 friend class Values;
│ │ │ │ -
222 const_iterator begin_;
│ │ │ │ -
223 const_iterator end_;
│ │ │ │ -
224 ConstFiltered(
│ │ │ │ -
225 const std::function<bool(const Values::ConstKeyValuePair&)>& filter,
│ │ │ │ -
226 const Values& values) {
│ │ │ │ -
227 // We remove the const from values to create a non-const Filtered
│ │ │ │ -
228 // view, then pull the const_iterators out of it.
│ │ │ │ -
229 const Filtered<ValueType> filtered(filter, const_cast<Values&>(values));
│ │ │ │ -
230 begin_ = filtered.beginConst();
│ │ │ │ -
231 end_ = filtered.endConst();
│ │ │ │ -
232 }
│ │ │ │ -
233 };
│ │ │ │ -
234
│ │ │ │ -
235 template<class ValueType>
│ │ │ │ -
236 Values::Values(const Values::Filtered<ValueType>& view) {
│ │ │ │ -
237 for(const auto key_value: view) {
│ │ │ │ -
238 Key key = key_value.key;
│ │ │ │ -
239 insert(key, static_cast<const ValueType&>(key_value.value));
│ │ │ │ +
178 // Get keys from removed factors and new factors, and compute unused keys,
│ │ │ │ +
179 // i.e., keys that are empty now and do not appear in the new factors.
│ │ │ │ +
180 void computeUnusedKeys(const NonlinearFactorGraph& newFactors,
│ │ │ │ +
181 const VariableIndex& variableIndex,
│ │ │ │ +
182 const KeySet& keysWithRemovedFactors,
│ │ │ │ +
183 KeySet* unusedKeys) const {
│ │ │ │ +
184 gttic(computeUnusedKeys);
│ │ │ │ +
185 KeySet removedAndEmpty;
│ │ │ │ +
186 for (Key key : keysWithRemovedFactors) {
│ │ │ │ +
187 if (variableIndex.empty(key))
│ │ │ │ +
188 removedAndEmpty.insert(removedAndEmpty.end(), key);
│ │ │ │ +
189 }
│ │ │ │ +
190 KeySet newFactorSymbKeys = newFactors.keys();
│ │ │ │ +
191 std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(),
│ │ │ │ +
192 newFactorSymbKeys.begin(), newFactorSymbKeys.end(),
│ │ │ │ +
193 std::inserter(*unusedKeys, unusedKeys->end()));
│ │ │ │ +
194 }
│ │ │ │ +
195
│ │ │ │ +
196 // Calculate nonlinear error
│ │ │ │ +
197 void error(const NonlinearFactorGraph& nonlinearFactors,
│ │ │ │ +
198 const Values& estimate, boost::optional<double>* result) const {
│ │ │ │ +
199 gttic(error);
│ │ │ │ +
200 result->reset(nonlinearFactors.error(estimate));
│ │ │ │ +
201 }
│ │ │ │ +
202
│ │ │ │ +
203 // Mark linear update
│ │ │ │ +
204 void gatherInvolvedKeys(const NonlinearFactorGraph& newFactors,
│ │ │ │ +
205 const NonlinearFactorGraph& nonlinearFactors,
│ │ │ │ +
206 const KeySet& keysWithRemovedFactors,
│ │ │ │ +
207 KeySet* markedKeys) const {
│ │ │ │ +
208 gttic(gatherInvolvedKeys);
│ │ │ │ +
209 *markedKeys = newFactors.keys(); // Get keys from new factors
│ │ │ │ +
210 // Also mark keys involved in removed factors
│ │ │ │ +
211 markedKeys->insert(keysWithRemovedFactors.begin(),
│ │ │ │ +
212 keysWithRemovedFactors.end());
│ │ │ │ +
213
│ │ │ │ +
214 // Also mark any provided extra re-eliminate keys
│ │ │ │ +
215 if (updateParams_.extraReelimKeys) {
│ │ │ │ +
216 for (Key key : *updateParams_.extraReelimKeys) {
│ │ │ │ +
217 markedKeys->insert(key);
│ │ │ │ +
218 }
│ │ │ │ +
219 }
│ │ │ │ +
220
│ │ │ │ +
221 // Also, keys that were not observed in existing factors, but whose affected
│ │ │ │ +
222 // keys have been extended now (e.g. smart factors)
│ │ │ │ +
223 if (updateParams_.newAffectedKeys) {
│ │ │ │ +
224 for (const auto& factorAddedKeys : *updateParams_.newAffectedKeys) {
│ │ │ │ +
225 const auto factorIdx = factorAddedKeys.first;
│ │ │ │ +
226 const auto& affectedKeys = nonlinearFactors.at(factorIdx)->keys();
│ │ │ │ +
227 markedKeys->insert(affectedKeys.begin(), affectedKeys.end());
│ │ │ │ +
228 }
│ │ │ │ +
229 }
│ │ │ │ +
230 }
│ │ │ │ +
231
│ │ │ │ +
232 // Update detail, unused, and observed keys from markedKeys
│ │ │ │ +
233 void updateKeys(const KeySet& markedKeys, ISAM2Result* result) const {
│ │ │ │ +
234 gttic(updateKeys);
│ │ │ │ +
235 // Observed keys for detailed results
│ │ │ │ +
236 if (result->detail && params_.enableDetailedResults) {
│ │ │ │ +
237 for (Key key : markedKeys) {
│ │ │ │ +
238 result->detail->variableStatus[key].isObserved = true;
│ │ │ │ +
239 }
│ │ │ │
240 }
│ │ │ │ -
241 }
│ │ │ │ -
242
│ │ │ │ -
243 template<class ValueType>
│ │ │ │ -
244 Values::Values(const Values::ConstFiltered<ValueType>& view) {
│ │ │ │ -
245 for(const auto key_value: view) {
│ │ │ │ -
246 Key key = key_value.key;
│ │ │ │ -
247 insert(key, static_cast<const ValueType&>(key_value.value));
│ │ │ │ -
248 }
│ │ │ │ -
249 }
│ │ │ │ -
250
│ │ │ │ -
251 Values::Filtered<Value>
│ │ │ │ -
252 inline Values::filter(const std::function<bool(Key)>& filterFcn) {
│ │ │ │ -
253 return filter<Value>(filterFcn);
│ │ │ │ -
254 }
│ │ │ │ -
255
│ │ │ │ -
256 template<class ValueType>
│ │ │ │ -
257 Values::Filtered<ValueType>
│ │ │ │ -
258 Values::filter(const std::function<bool(Key)>& filterFcn) {
│ │ │ │ -
259 return Filtered<ValueType>(std::bind(&filterHelper<ValueType>, filterFcn,
│ │ │ │ -
260 std::placeholders::_1), *this);
│ │ │ │ -
261 }
│ │ │ │ -
262
│ │ │ │ -
263 Values::ConstFiltered<Value>
│ │ │ │ -
264 inline Values::filter(const std::function<bool(Key)>& filterFcn) const {
│ │ │ │ -
265 return filter<Value>(filterFcn);
│ │ │ │ -
266 }
│ │ │ │ -
267
│ │ │ │ -
268 template<class ValueType>
│ │ │ │ -
269 Values::ConstFiltered<ValueType>
│ │ │ │ -
270 Values::filter(const std::function<bool(Key)>& filterFcn) const {
│ │ │ │ -
271 return ConstFiltered<ValueType>(std::bind(&filterHelper<ValueType>,
│ │ │ │ -
272 filterFcn, std::placeholders::_1), *this);
│ │ │ │ -
273 }
│ │ │ │ -
274#endif
│ │ │ │ +
241
│ │ │ │ +
242 for (Key index : markedKeys) {
│ │ │ │ +
243 // Only add if not unused
│ │ │ │ +
244 if (result->unusedKeys.find(index) == result->unusedKeys.end())
│ │ │ │ +
245 // Make a copy of these, as we'll soon add to them
│ │ │ │ +
246 result->observedKeys.push_back(index);
│ │ │ │ +
247 }
│ │ │ │ +
248 }
│ │ │ │ +
249
│ │ │ │ +
250 static void CheckRelinearizationRecursiveMap(
│ │ │ │ +
251 const FastMap<char, Vector>& thresholds, const VectorValues& delta,
│ │ │ │ +
252 const ISAM2::sharedClique& clique, KeySet* relinKeys) {
│ │ │ │ +
253 // Check the current clique for relinearization
│ │ │ │ +
254 bool relinearize = false;
│ │ │ │ +
255 for (Key var : *clique->conditional()) {
│ │ │ │ +
256 // Find the threshold for this variable type
│ │ │ │ +
257 const Vector& threshold = thresholds.find(Symbol(var).chr())->second;
│ │ │ │ +
258
│ │ │ │ +
259 const Vector& deltaVar = delta[var];
│ │ │ │ +
260
│ │ │ │ +
261 // Verify the threshold vector matches the actual variable size
│ │ │ │ +
262 if (threshold.rows() != deltaVar.rows())
│ │ │ │ +
263 throw std::invalid_argument(
│ │ │ │ +
264 "Relinearization threshold vector dimensionality for '" +
│ │ │ │ +
265 std::string(1, Symbol(var).chr()) +
│ │ │ │ +
266 "' passed into iSAM2 parameters does not match actual variable "
│ │ │ │ +
267 "dimensionality.");
│ │ │ │ +
268
│ │ │ │ +
269 // Check for relinearization
│ │ │ │ +
270 if ((deltaVar.array().abs() > threshold.array()).any()) {
│ │ │ │ +
271 relinKeys->insert(var);
│ │ │ │ +
272 relinearize = true;
│ │ │ │ +
273 }
│ │ │ │ +
274 }
│ │ │ │
275
│ │ │ │ -
276 /* ************************************************************************* */
│ │ │ │ -
277 template<>
│ │ │ │ -
278 inline bool Values::filterHelper<Value>(const std::function<bool(Key)> filter,
│ │ │ │ -
279 const ConstKeyValuePair& key_value) {
│ │ │ │ -
280 // Filter and check the type
│ │ │ │ -
281 return filter(key_value.key);
│ │ │ │ -
282 }
│ │ │ │ +
276 // If this node was relinearized, also check its children
│ │ │ │ +
277 if (relinearize) {
│ │ │ │ +
278 for (const ISAM2::sharedClique& child : clique->children) {
│ │ │ │ +
279 CheckRelinearizationRecursiveMap(thresholds, delta, child, relinKeys);
│ │ │ │ +
280 }
│ │ │ │ +
281 }
│ │ │ │ +
282 }
│ │ │ │
283
│ │ │ │ -
284 /* ************************************************************************* */
│ │ │ │ -
285
│ │ │ │ -
286 namespace internal {
│ │ │ │ -
287
│ │ │ │ -
288 // Check the type and throw exception if incorrect
│ │ │ │ -
289 // Generic version, partially specialized below for various Eigen Matrix types
│ │ │ │ -
290 template <typename ValueType>
│ │ │ │ -
│ │ │ │ -
291 struct handle {
│ │ │ │ -
292 ValueType operator()(Key j, const Value* const pointer) {
│ │ │ │ -
293 auto ptr = dynamic_cast<const GenericValue<ValueType>*>(pointer);
│ │ │ │ -
294 if (ptr) {
│ │ │ │ -
295 // value returns a const ValueType&, and the return makes a copy !!!!!
│ │ │ │ -
296 return ptr->value();
│ │ │ │ -
297 } else {
│ │ │ │ -
298 throw ValuesIncorrectType(j, typeid(*pointer), typeid(ValueType));
│ │ │ │ -
299 }
│ │ │ │ -
300 }
│ │ │ │ -
301 };
│ │ │ │ -
│ │ │ │ -
302
│ │ │ │ -
303 template <typename MatrixType, bool isDynamic>
│ │ │ │ -
304 struct handle_matrix;
│ │ │ │ -
305
│ │ │ │ -
306 // Handle dynamic matrices
│ │ │ │ -
307 template <int M, int N>
│ │ │ │ -
│ │ │ │ -
308 struct handle_matrix<Eigen::Matrix<double, M, N>, true> {
│ │ │ │ -
309 inline Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
│ │ │ │ -
310 auto ptr = dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>*>(pointer);
│ │ │ │ -
311 if (ptr) {
│ │ │ │ -
312 // value returns a const Matrix&, and the return makes a copy !!!!!
│ │ │ │ -
313 return ptr->value();
│ │ │ │ -
314 } else {
│ │ │ │ -
315 // If a fixed matrix was stored, we end up here as well.
│ │ │ │ -
316 throw ValuesIncorrectType(j, typeid(*pointer), typeid(Eigen::Matrix<double, M, N>));
│ │ │ │ -
317 }
│ │ │ │ -
318 }
│ │ │ │ -
319 };
│ │ │ │ -
│ │ │ │ -
320
│ │ │ │ -
321 // Handle fixed matrices
│ │ │ │ -
322 template <int M, int N>
│ │ │ │ -
│ │ │ │ -
323 struct handle_matrix<Eigen::Matrix<double, M, N>, false> {
│ │ │ │ -
324 inline Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
│ │ │ │ -
325 auto ptr = dynamic_cast<const GenericValue<Eigen::Matrix<double, M, N>>*>(pointer);
│ │ │ │ -
326 if (ptr) {
│ │ │ │ -
327 // value returns a const MatrixMN&, and the return makes a copy !!!!!
│ │ │ │ -
328 return ptr->value();
│ │ │ │ -
329 } else {
│ │ │ │ -
330 Matrix A;
│ │ │ │ -
331 // Check if a dynamic matrix was stored
│ │ │ │ -
332 auto ptr = dynamic_cast<const GenericValue<Eigen::MatrixXd>*>(pointer);
│ │ │ │ -
333 if (ptr) {
│ │ │ │ -
334 A = ptr->value();
│ │ │ │ -
335 } else {
│ │ │ │ -
336 // Or a dynamic vector
│ │ │ │ -
337 A = handle_matrix<Eigen::VectorXd, true>()(j, pointer); // will throw if not....
│ │ │ │ -
338 }
│ │ │ │ -
339 // Yes: check size, and throw if not a match
│ │ │ │ -
340 if (A.rows() != M || A.cols() != N)
│ │ │ │ -
341 throw NoMatchFoundForFixed(M, N, A.rows(), A.cols());
│ │ │ │ -
342 else
│ │ │ │ -
343 return A; // copy but not malloc
│ │ │ │ -
344 }
│ │ │ │ -
345 }
│ │ │ │ -
346 };
│ │ │ │ -
│ │ │ │ -
347
│ │ │ │ -
348 // Handle matrices
│ │ │ │ -
349 template <int M, int N>
│ │ │ │ -
│ │ │ │ -
350 struct handle<Eigen::Matrix<double, M, N>> {
│ │ │ │ -
351 Eigen::Matrix<double, M, N> operator()(Key j, const Value* const pointer) {
│ │ │ │ -
352 return handle_matrix<Eigen::Matrix<double, M, N>,
│ │ │ │ -
353 (M == Eigen::Dynamic || N == Eigen::Dynamic)>()(j, pointer);
│ │ │ │ -
354 }
│ │ │ │ -
355 };
│ │ │ │ -
│ │ │ │ -
356
│ │ │ │ -
357 } // internal
│ │ │ │ -
358
│ │ │ │ -
359 /* ************************************************************************* */
│ │ │ │ -
360 template <typename ValueType>
│ │ │ │ -
│ │ │ │ -
361 const ValueType Values::at(Key j) const {
│ │ │ │ -
362 // Find the item
│ │ │ │ -
363 KeyValueMap::const_iterator item = values_.find(j);
│ │ │ │ -
364
│ │ │ │ -
365 // Throw exception if it does not exist
│ │ │ │ -
366 if (item == values_.end()) throw ValuesKeyDoesNotExist("at", j);
│ │ │ │ -
367
│ │ │ │ -
368 // Check the type and throw exception if incorrect
│ │ │ │ -
369 // h() split in two lines to avoid internal compiler error (MSVC2017)
│ │ │ │ -
370 auto h = internal::handle<ValueType>();
│ │ │ │ -
371 return h(j, item->second);
│ │ │ │ +
284 static void CheckRelinearizationRecursiveDouble(
│ │ │ │ +
285 double threshold, const VectorValues& delta,
│ │ │ │ +
286 const ISAM2::sharedClique& clique, KeySet* relinKeys) {
│ │ │ │ +
287 // Check the current clique for relinearization
│ │ │ │ +
288 bool relinearize = false;
│ │ │ │ +
289 for (Key var : *clique->conditional()) {
│ │ │ │ +
290 double maxDelta = delta[var].lpNorm<Eigen::Infinity>();
│ │ │ │ +
291 if (maxDelta >= threshold) {
│ │ │ │ +
292 relinKeys->insert(var);
│ │ │ │ +
293 relinearize = true;
│ │ │ │ +
294 }
│ │ │ │ +
295 }
│ │ │ │ +
296
│ │ │ │ +
297 // If this node was relinearized, also check its children
│ │ │ │ +
298 if (relinearize) {
│ │ │ │ +
299 for (const ISAM2::sharedClique& child : clique->children) {
│ │ │ │ +
300 CheckRelinearizationRecursiveDouble(threshold, delta, child, relinKeys);
│ │ │ │ +
301 }
│ │ │ │ +
302 }
│ │ │ │ +
303 }
│ │ │ │ +
304
│ │ │ │ +
│ │ │ │ +
318 static KeySet CheckRelinearizationPartial(
│ │ │ │ +
319 const ISAM2::Roots& roots, const VectorValues& delta,
│ │ │ │ +
320 const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
│ │ │ │ +
321 KeySet relinKeys;
│ │ │ │ +
322 for (const ISAM2::sharedClique& root : roots) {
│ │ │ │ +
323 if (relinearizeThreshold.type() == typeid(double))
│ │ │ │ +
324 CheckRelinearizationRecursiveDouble(
│ │ │ │ +
325 boost::get<double>(relinearizeThreshold), delta, root, &relinKeys);
│ │ │ │ +
326 else if (relinearizeThreshold.type() == typeid(FastMap<char, Vector>))
│ │ │ │ +
327 CheckRelinearizationRecursiveMap(
│ │ │ │ +
328 boost::get<FastMap<char, Vector> >(relinearizeThreshold), delta,
│ │ │ │ +
329 root, &relinKeys);
│ │ │ │ +
330 }
│ │ │ │ +
331 return relinKeys;
│ │ │ │ +
332 }
│ │ │ │ +
│ │ │ │ +
333
│ │ │ │ +
│ │ │ │ +
345 static KeySet CheckRelinearizationFull(
│ │ │ │ +
346 const VectorValues& delta,
│ │ │ │ +
347 const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
│ │ │ │ +
348 KeySet relinKeys;
│ │ │ │ +
349
│ │ │ │ +
350 if (const double* threshold = boost::get<double>(&relinearizeThreshold)) {
│ │ │ │ +
351 for (const VectorValues::KeyValuePair& key_delta : delta) {
│ │ │ │ +
352 double maxDelta = key_delta.second.lpNorm<Eigen::Infinity>();
│ │ │ │ +
353 if (maxDelta >= *threshold) relinKeys.insert(key_delta.first);
│ │ │ │ +
354 }
│ │ │ │ +
355 } else if (const FastMap<char, Vector>* thresholds =
│ │ │ │ +
356 boost::get<FastMap<char, Vector> >(&relinearizeThreshold)) {
│ │ │ │ +
357 for (const VectorValues::KeyValuePair& key_delta : delta) {
│ │ │ │ +
358 const Vector& threshold =
│ │ │ │ +
359 thresholds->find(Symbol(key_delta.first).chr())->second;
│ │ │ │ +
360 if (threshold.rows() != key_delta.second.rows())
│ │ │ │ +
361 throw std::invalid_argument(
│ │ │ │ +
362 "Relinearization threshold vector dimensionality for '" +
│ │ │ │ +
363 std::string(1, Symbol(key_delta.first).chr()) +
│ │ │ │ +
364 "' passed into iSAM2 parameters does not match actual variable "
│ │ │ │ +
365 "dimensionality.");
│ │ │ │ +
366 if ((key_delta.second.array().abs() > threshold.array()).any())
│ │ │ │ +
367 relinKeys.insert(key_delta.first);
│ │ │ │ +
368 }
│ │ │ │ +
369 }
│ │ │ │ +
370
│ │ │ │ +
371 return relinKeys;
│ │ │ │
372 }
│ │ │ │
│ │ │ │
373
│ │ │ │ -
374 /* ************************************************************************* */
│ │ │ │ -
375 template<typename ValueType>
│ │ │ │ -
│ │ │ │ -
376 boost::optional<const ValueType&> Values::exists(Key j) const {
│ │ │ │ -
377 // Find the item
│ │ │ │ -
378 KeyValueMap::const_iterator item = values_.find(j);
│ │ │ │ -
379
│ │ │ │ -
380 if(item != values_.end()) {
│ │ │ │ -
381 // dynamic cast the type and throw exception if incorrect
│ │ │ │ -
382 auto ptr = dynamic_cast<const GenericValue<ValueType>*>(item->second);
│ │ │ │ -
383 if (ptr) {
│ │ │ │ -
384 return ptr->value();
│ │ │ │ -
385 } else {
│ │ │ │ -
386 // NOTE(abe): clang warns about potential side effects if done in typeid
│ │ │ │ -
387 const Value* value = item->second;
│ │ │ │ -
388 throw ValuesIncorrectType(j, typeid(*value), typeid(ValueType));
│ │ │ │ -
389 }
│ │ │ │ -
390 } else {
│ │ │ │ -
391 return boost::none;
│ │ │ │ +
374 // Mark keys in \Delta above threshold \beta:
│ │ │ │ +
375 KeySet gatherRelinearizeKeys(const ISAM2::Roots& roots,
│ │ │ │ +
376 const VectorValues& delta,
│ │ │ │ +
377 const KeySet& fixedVariables,
│ │ │ │ +
378 KeySet* markedKeys) const {
│ │ │ │ +
379 gttic(gatherRelinearizeKeys);
│ │ │ │ +
380 // J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
│ │ │ │ +
381 KeySet relinKeys =
│ │ │ │ +
382 params_.enablePartialRelinearizationCheck
│ │ │ │ +
383 ? CheckRelinearizationPartial(roots, delta,
│ │ │ │ +
384 params_.relinearizeThreshold)
│ │ │ │ +
385 : CheckRelinearizationFull(delta, params_.relinearizeThreshold);
│ │ │ │ +
386 if (updateParams_.forceFullSolve)
│ │ │ │ +
387 relinKeys = CheckRelinearizationFull(delta, 0.0); // for debugging
│ │ │ │ +
388
│ │ │ │ +
389 // Remove from relinKeys any keys whose linearization points are fixed
│ │ │ │ +
390 for (Key key : fixedVariables) {
│ │ │ │ +
391 relinKeys.erase(key);
│ │ │ │
392 }
│ │ │ │ -
393 }
│ │ │ │ -
│ │ │ │ -
394
│ │ │ │ -
395 /* ************************************************************************* */
│ │ │ │ -
396
│ │ │ │ -
397 // insert a templated value
│ │ │ │ -
398 template<typename ValueType>
│ │ │ │ -
│ │ │ │ -
399 void Values::insert(Key j, const ValueType& val) {
│ │ │ │ -
400 insert(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
│ │ │ │ -
401 }
│ │ │ │ -
│ │ │ │ -
402
│ │ │ │ -
403 // update with templated value
│ │ │ │ -
404 template <typename ValueType>
│ │ │ │ -
405 void Values::update(Key j, const ValueType& val) {
│ │ │ │ -
406 update(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
│ │ │ │ -
407 }
│ │ │ │ -
408
│ │ │ │ -
409 // insert_or_assign with templated value
│ │ │ │ -
410 template <typename ValueType>
│ │ │ │ -
│ │ │ │ -
411 void Values::insert_or_assign(Key j, const ValueType& val) {
│ │ │ │ -
412 insert_or_assign(j, static_cast<const Value&>(GenericValue<ValueType>(val)));
│ │ │ │ +
393 if (updateParams_.noRelinKeys) {
│ │ │ │ +
394 for (Key key : *updateParams_.noRelinKeys) {
│ │ │ │ +
395 relinKeys.erase(key);
│ │ │ │ +
396 }
│ │ │ │ +
397 }
│ │ │ │ +
398
│ │ │ │ +
399 // Add the variables being relinearized to the marked keys
│ │ │ │ +
400 markedKeys->insert(relinKeys.begin(), relinKeys.end());
│ │ │ │ +
401 return relinKeys;
│ │ │ │ +
402 }
│ │ │ │ +
403
│ │ │ │ +
404 // Record relinerization threshold keys in detailed results
│ │ │ │ +
405 void recordRelinearizeDetail(const KeySet& relinKeys,
│ │ │ │ +
406 ISAM2Result::DetailedResults* detail) const {
│ │ │ │ +
407 if (detail && params_.enableDetailedResults) {
│ │ │ │ +
408 for (Key key : relinKeys) {
│ │ │ │ +
409 detail->variableStatus[key].isAboveRelinThreshold = true;
│ │ │ │ +
410 detail->variableStatus[key].isRelinearized = true;
│ │ │ │ +
411 }
│ │ │ │ +
412 }
│ │ │ │
413 }
│ │ │ │ -
│ │ │ │
414
│ │ │ │ -
415}
│ │ │ │ -
Values.h
A non-templated config holding any types of Manifold-group elements.
│ │ │ │ +
415 // Mark all cliques that involve marked variables \Theta_{J} and all
│ │ │ │ +
416 // their ancestors.
│ │ │ │ +
417 void findFluid(const ISAM2::Roots& roots, const KeySet& relinKeys,
│ │ │ │ +
418 KeySet* markedKeys,
│ │ │ │ +
419 ISAM2Result::DetailedResults* detail) const {
│ │ │ │ +
420 gttic(findFluid);
│ │ │ │ +
421 for (const auto& root : roots)
│ │ │ │ +
422 // add other cliques that have the marked ones in the separator
│ │ │ │ +
423 root->findAll(relinKeys, markedKeys);
│ │ │ │ +
424
│ │ │ │ +
425 // Relinearization-involved keys for detailed results
│ │ │ │ +
426 if (detail && params_.enableDetailedResults) {
│ │ │ │ +
427 KeySet involvedRelinKeys;
│ │ │ │ +
428 for (const auto& root : roots)
│ │ │ │ +
429 root->findAll(relinKeys, &involvedRelinKeys);
│ │ │ │ +
430 for (Key key : involvedRelinKeys) {
│ │ │ │ +
431 if (!detail->variableStatus[key].isAboveRelinThreshold) {
│ │ │ │ +
432 detail->variableStatus[key].isRelinearizeInvolved = true;
│ │ │ │ +
433 detail->variableStatus[key].isRelinearized = true;
│ │ │ │ +
434 }
│ │ │ │ +
435 }
│ │ │ │ +
436 }
│ │ │ │ +
437 }
│ │ │ │ +
438
│ │ │ │ +
439 // Linearize new factors
│ │ │ │ +
440 void linearizeNewFactors(const NonlinearFactorGraph& newFactors,
│ │ │ │ +
441 const Values& theta, size_t numNonlinearFactors,
│ │ │ │ +
442 const FactorIndices& newFactorsIndices,
│ │ │ │ +
443 GaussianFactorGraph* linearFactors) const {
│ │ │ │ +
444 gttic(linearizeNewFactors);
│ │ │ │ +
445 auto linearized = newFactors.linearize(theta);
│ │ │ │ +
446 if (params_.findUnusedFactorSlots) {
│ │ │ │ +
447 linearFactors->resize(numNonlinearFactors);
│ │ │ │ +
448 for (size_t i = 0; i < newFactors.size(); ++i)
│ │ │ │ +
449 (*linearFactors)[newFactorsIndices[i]] = (*linearized)[i];
│ │ │ │ +
450 } else {
│ │ │ │ +
451 linearFactors->push_back(*linearized);
│ │ │ │ +
452 }
│ │ │ │ +
453 assert(linearFactors->size() == numNonlinearFactors);
│ │ │ │ +
454 }
│ │ │ │ +
455
│ │ │ │ +
456 void augmentVariableIndex(const NonlinearFactorGraph& newFactors,
│ │ │ │ +
457 const FactorIndices& newFactorsIndices,
│ │ │ │ +
458 VariableIndex* variableIndex) const {
│ │ │ │ +
459 gttic(augmentVariableIndex);
│ │ │ │ +
460 // Augment the variable index with the new factors
│ │ │ │ +
461 if (params_.findUnusedFactorSlots)
│ │ │ │ +
462 variableIndex->augment(newFactors, newFactorsIndices);
│ │ │ │ +
463 else
│ │ │ │ +
464 variableIndex->augment(newFactors);
│ │ │ │ +
465
│ │ │ │ +
466 // Augment it with existing factors which now affect to more variables:
│ │ │ │ +
467 if (updateParams_.newAffectedKeys) {
│ │ │ │ +
468 for (const auto& factorAddedKeys : *updateParams_.newAffectedKeys) {
│ │ │ │ +
469 const auto factorIdx = factorAddedKeys.first;
│ │ │ │ +
470 variableIndex->augmentExistingFactor(factorIdx, factorAddedKeys.second);
│ │ │ │ +
471 }
│ │ │ │ +
472 }
│ │ │ │ +
473 }
│ │ │ │ +
474
│ │ │ │ +
475 static void LogRecalculateKeys(const ISAM2Result& result) {
│ │ │ │ +
476 const bool debug = ISDEBUG("ISAM2 recalculate");
│ │ │ │ +
477
│ │ │ │ +
478 if (debug) {
│ │ │ │ +
479 std::cout << "markedKeys: ";
│ │ │ │ +
480 for (const Key key : result.markedKeys) {
│ │ │ │ +
481 std::cout << key << " ";
│ │ │ │ +
482 }
│ │ │ │ +
483 std::cout << std::endl;
│ │ │ │ +
484 std::cout << "observedKeys: ";
│ │ │ │ +
485 for (const Key key : result.observedKeys) {
│ │ │ │ +
486 std::cout << key << " ";
│ │ │ │ +
487 }
│ │ │ │ +
488 std::cout << std::endl;
│ │ │ │ +
489 }
│ │ │ │ +
490 }
│ │ │ │ +
491
│ │ │ │ +
492 static FactorIndexSet GetAffectedFactors(const KeyList& keys,
│ │ │ │ +
493 const VariableIndex& variableIndex) {
│ │ │ │ +
494 gttic(GetAffectedFactors);
│ │ │ │ +
495 FactorIndexSet indices;
│ │ │ │ +
496 for (const Key key : keys) {
│ │ │ │ +
497 const FactorIndices& factors(variableIndex[key]);
│ │ │ │ +
498 indices.insert(factors.begin(), factors.end());
│ │ │ │ +
499 }
│ │ │ │ +
500 return indices;
│ │ │ │ +
501 }
│ │ │ │ +
502
│ │ │ │ +
503 // find intermediate (linearized) factors from cache that are passed into
│ │ │ │ +
504 // the affected area
│ │ │ │ +
505 static GaussianFactorGraph GetCachedBoundaryFactors(
│ │ │ │ +
506 const ISAM2::Cliques& orphans) {
│ │ │ │ +
507 GaussianFactorGraph cachedBoundary;
│ │ │ │ +
508
│ │ │ │ +
509 for (const auto& orphan : orphans) {
│ │ │ │ +
510 // retrieve the cached factor and add to boundary
│ │ │ │ +
511 cachedBoundary.push_back(orphan->cachedFactor());
│ │ │ │ +
512 }
│ │ │ │ +
513
│ │ │ │ +
514 return cachedBoundary;
│ │ │ │ +
515 }
│ │ │ │ +
516};
│ │ │ │ +
│ │ │ │ +
517
│ │ │ │ +
518} // namespace gtsam
│ │ │ │ +
debug.h
Global debugging flags.
│ │ │ │ + │ │ │ │ +
JunctionTree-inst.h
The junction tree, template bodies.
│ │ │ │ +
GaussianBayesTree.h
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
│ │ │ │ + │ │ │ │ +
ISAM2Result.h
Class that stores detailed iSAM2 result.
│ │ │ │ +
ISAM2.h
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ +
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition Factor.h:34
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::GenericValue
Wraps any type T so it can play as a Value.
Definition GenericValue.h:47
│ │ │ │ -
gtsam::GenericValue::value
const T & value() const
Return a constant value.
Definition GenericValue.h:67
│ │ │ │ -
gtsam::Value
This is the base class for any type to be stored in Values.
Definition Value.h:37
│ │ │ │ -
gtsam::_ValuesKeyValuePair
Definition Values-inl.h:36
│ │ │ │ -
gtsam::_ValuesKeyValuePair::key
const Key key
The key.
Definition Values-inl.h:37
│ │ │ │ -
gtsam::_ValuesKeyValuePair::value
ValueType & value
The value.
Definition Values-inl.h:38
│ │ │ │ -
gtsam::_ValuesConstKeyValuePair
Definition Values-inl.h:45
│ │ │ │ -
gtsam::_ValuesConstKeyValuePair::key
const Key key
The key.
Definition Values-inl.h:46
│ │ │ │ -
gtsam::_ValuesConstKeyValuePair::value
const ValueType & value
The value.
Definition Values-inl.h:47
│ │ │ │ -
gtsam::ValuesCastHelper
Definition Values-inl.h:62
│ │ │ │ -
gtsam::internal::handle
Definition Values-inl.h:291
│ │ │ │ -
gtsam::internal::handle_matrix
Definition Values-inl.h:304
│ │ │ │ -
gtsam::Values::update
void update(Key j, const Value &val)
single element change of existing element
Definition Values.cpp:180
│ │ │ │ -
gtsam::Values::extract
std::map< Key, ValueType > extract(const std::function< bool(Key)> &filterFcn=&_truePredicate< Key >) const
Extract a subset of values of the given type ValueType.
Definition Values-inl.h:94
│ │ │ │ -
gtsam::Values::at
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
│ │ │ │ -
gtsam::Values::insert
void insert(Key j, const Value &val)
Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present.
Definition Values.cpp:157
│ │ │ │ -
gtsam::Values::insert_or_assign
void insert_or_assign(Key j, const Value &val)
If key j exists, update value, else perform an insert.
Definition Values.cpp:203
│ │ │ │ -
gtsam::Values::Values
Values()=default
Default constructor creates an empty Values class.
│ │ │ │ -
gtsam::Values::exists
bool exists(Key j) const
Check if a value exists with key j.
Definition Values.cpp:94
│ │ │ │ +
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ + │ │ │ │ +
gtsam::FactorGraph::keys
KeySet keys() const
Potentially slow function to return all keys involved, sorted, as a set.
Definition FactorGraph-inst.h:85
│ │ │ │ +
gtsam::FactorGraph::add_factors
FactorIndices add_factors(const CONTAINER &factors, bool useEmptySlots=false)
Add new factors to a factor graph and returns a list of new factor indices, optionally finding and re...
Definition FactorGraph-inst.h:109
│ │ │ │ +
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition FactorGraph.h:186
│ │ │ │ +
gtsam::FactorGraph::remove
void remove(size_t i)
delete factor without re-arranging indexes by inserting a nullptr pointer
Definition FactorGraph.h:385
│ │ │ │ +
gtsam::FactorGraph::at
const sharedFactor at(size_t i) const
Get a specific factor by index (this checks array bounds and may throw an exception,...
Definition FactorGraph.h:335
│ │ │ │ +
gtsam::FactorGraph::reserve
void reserve(size_t size)
Reserve space for the specified number of factors if you know in advance how many there will be (work...
Definition FactorGraph.h:182
│ │ │ │ + │ │ │ │ +
gtsam::BayesTree::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print
Definition BayesTree-inst.h:212
│ │ │ │ +
gtsam::BayesTree< ISAM2Clique >::Roots
FastVector< sharedClique > Roots
Root cliques.
Definition BayesTree.h:95
│ │ │ │ +
gtsam::JunctionTree
A JunctionTree is a cluster tree, a set of variable clusters with factors, arranged in a tree,...
Definition JunctionTree.h:50
│ │ │ │ +
gtsam::Symbol
Character and index key used to refer to variables.
Definition Symbol.h:35
│ │ │ │ +
gtsam::Symbol::chr
unsigned char chr() const
Retrieve key character.
Definition Symbol.h:73
│ │ │ │ +
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition VariableIndex.h:43
│ │ │ │ +
gtsam::VariableIndex::remove
void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG &factors)
Remove entries corresponding to the specified factors.
Definition VariableIndex-inl.h:54
│ │ │ │ +
gtsam::VariableIndex::empty
bool empty(Key variable) const
Return true if no factors associated with a variable.
Definition VariableIndex.h:98
│ │ │ │ +
gtsam::GaussianEliminationTree
Definition GaussianEliminationTree.h:29
│ │ │ │ +
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ +
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
gtsam::ISAM2BayesTree
Definition ISAM2-impl.h:48
│ │ │ │ +
gtsam::ISAM2JunctionTree
Definition ISAM2-impl.h:61
│ │ │ │ +
gtsam::DeltaImpl
Definition ISAM2-impl.h:72
│ │ │ │ +
gtsam::DeltaImpl::PartialSolveResult
Definition ISAM2-impl.h:73
│ │ │ │ +
gtsam::DeltaImpl::ReorderingMode
Definition ISAM2-impl.h:77
│ │ │ │ +
gtsam::UpdateImpl
Implementation functions for update method All of the methods below have clear inputs and outputs,...
Definition ISAM2-impl.h:114
│ │ │ │ +
gtsam::UpdateImpl::CheckRelinearizationFull
static KeySet CheckRelinearizationFull(const VectorValues &delta, const ISAM2Params::RelinearizationThreshold &relinearizeThreshold)
Find the set of variables to be relinearized according to relinearizeThreshold.
Definition ISAM2-impl.h:345
│ │ │ │ +
gtsam::UpdateImpl::CheckRelinearizationPartial
static KeySet CheckRelinearizationPartial(const ISAM2::Roots &roots, const VectorValues &delta, const ISAM2Params::RelinearizationThreshold &relinearizeThreshold)
Find the set of variables to be relinearized according to relinearizeThreshold.
Definition ISAM2-impl.h:318
│ │ │ │ +
gtsam::ISAM2
Implementation of the full ISAM2 algorithm for incremental nonlinear optimization.
Definition ISAM2.h:45
│ │ │ │ +
gtsam::ISAM2::sharedClique
Base::sharedClique sharedClique
Shared pointer to a clique.
Definition ISAM2.h:103
│ │ │ │ +
gtsam::ISAM2Params
Definition ISAM2Params.h:135
│ │ │ │ +
gtsam::ISAM2Params::RelinearizationThreshold
boost::variant< double, FastMap< char, Vector > > RelinearizationThreshold
Either a constant relinearization threshold or a per-variable-type set of thresholds.
Definition ISAM2Params.h:140
│ │ │ │ +
gtsam::ISAM2Result
This struct is returned from ISAM2::update() and contains information about the update that is useful...
Definition ISAM2Result.h:41
│ │ │ │ +
gtsam::ISAM2UpdateParams
This struct is used by ISAM2::update() to pass additional parameters to give the user a fine-grained ...
Definition ISAM2UpdateParams.h:32
│ │ │ │ +
gtsam::ISAM2UpdateParams::force_relinearize
bool force_relinearize
Relinearize any variables whose delta magnitude is sufficiently large (Params::relinearizeThreshold),...
Definition ISAM2UpdateParams.h:54
│ │ │ │ +
gtsam::ISAM2UpdateParams::removeFactorIndices
FactorIndices removeFactorIndices
Indices of factors to remove from system (default: empty)
Definition ISAM2UpdateParams.h:36
│ │ │ │ +
gtsam::ISAM2UpdateParams::forceFullSolve
bool forceFullSolve
By default, iSAM2 uses a wildfire update scheme that stops updating when the deltas become too small ...
Definition ISAM2UpdateParams.h:71
│ │ │ │ +
gtsam::ISAM2UpdateParams::extraReelimKeys
boost::optional< FastList< Key > > extraReelimKeys
An optional set of nonlinear keys that iSAM2 will re-eliminate, regardless of the size of the linear ...
Definition ISAM2UpdateParams.h:49
│ │ │ │ +
gtsam::ISAM2UpdateParams::noRelinKeys
boost::optional< FastList< Key > > noRelinKeys
An optional set of nonlinear keys that iSAM2 will hold at a constant linearization point,...
Definition ISAM2UpdateParams.h:44
│ │ │ │ +
gtsam::ISAM2UpdateParams::newAffectedKeys
boost::optional< FastMap< FactorIndex, KeySet > > newAffectedKeys
An optional set of new Keys that are now affected by factors, indexed by factor indices (as returned ...
Definition ISAM2UpdateParams.h:66
│ │ │ │ +
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │ +
gtsam::NonlinearFactorGraph::print
void print(const std::string &str="NonlinearFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition NonlinearFactorGraph.cpp:55
│ │ │ │ +
gtsam::NonlinearFactorGraph::error
double error(const Values &values) const
unnormalized error, in the most common case
Definition NonlinearFactorGraph.cpp:170
│ │ │ │ +
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │
gtsam::Values::KeyValuePair
A key-value pair, which you get by dereferencing iterators.
Definition Values.h:93
│ │ │ │ -
gtsam::Values::ConstKeyValuePair
A key-value pair, which you get by dereferencing iterators.
Definition Values.h:101
│ │ │ │ -
gtsam::ValuesKeyDoesNotExist
Definition Values.h:475
│ │ │ │ -
gtsam::ValuesIncorrectType
Definition Values.h:498
│ │ │ │ -
gtsam::NoMatchFoundForFixed
Definition Values.h:542
│ │ │ │
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │ + │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,525 +1,689 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -Values-inl.h │ │ │ │ │ +ISAM2-impl.h │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -25#pragma once │ │ │ │ │ -26 │ │ │ │ │ -27#include │ │ │ │ │ -28#include │ │ │ │ │ -29#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_V_a_l_u_e_s_._h> │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_I_S_A_M_2_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_I_S_A_M_2_R_e_s_u_l_t_._h> │ │ │ │ │ +23 │ │ │ │ │ +24#include <_g_t_s_a_m_/_b_a_s_e_/_d_e_b_u_g_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_J_u_n_c_t_i_o_n_T_r_e_e_-_i_n_s_t_._h> // We need the inst file │ │ │ │ │ +because we'll make a special JT templated on ISAM2 │ │ │ │ │ +26#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_S_y_m_b_o_l_._h> │ │ │ │ │ +27#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_V_a_r_i_a_b_l_e_I_n_d_e_x_._h> │ │ │ │ │ +28#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ +29#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h> │ │ │ │ │ 30 │ │ │ │ │ -31namespace _g_t_s_a_m { │ │ │ │ │ -32 │ │ │ │ │ -33 │ │ │ │ │ -34 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -35 template │ │ │ │ │ -_3_6 struct ___V_a_l_u_e_s_K_e_y_V_a_l_u_e_P_a_i_r { │ │ │ │ │ -_3_7 const _K_e_y _k_e_y; │ │ │ │ │ -_3_8 ValueType& _v_a_l_u_e; │ │ │ │ │ -39 │ │ │ │ │ -40 ___V_a_l_u_e_s_K_e_y_V_a_l_u_e_P_a_i_r(_K_e_y _key, ValueType& _value) : _k_e_y(_key), _v_a_l_u_e(_value) │ │ │ │ │ -{} │ │ │ │ │ -41 }; │ │ │ │ │ +31#include │ │ │ │ │ +32#include │ │ │ │ │ +33namespace br { │ │ │ │ │ +34using namespace boost::range; │ │ │ │ │ +35using namespace boost::adaptors; │ │ │ │ │ +36} // namespace br │ │ │ │ │ +37 │ │ │ │ │ +38#include │ │ │ │ │ +39#include │ │ │ │ │ +40#include │ │ │ │ │ +41#include │ │ │ │ │ 42 │ │ │ │ │ -43 /* ************************************************************************* │ │ │ │ │ +43namespace _g_t_s_a_m { │ │ │ │ │ +44 │ │ │ │ │ +45/* ************************************************************************* │ │ │ │ │ */ │ │ │ │ │ -44 template │ │ │ │ │ -_4_5 struct ___V_a_l_u_e_s_C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r { │ │ │ │ │ -_4_6 const _K_e_y _k_e_y; │ │ │ │ │ -_4_7 const ValueType& _v_a_l_u_e; │ │ │ │ │ -48 │ │ │ │ │ -49 ___V_a_l_u_e_s_C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r(_K_e_y _key, const ValueType& _value) : │ │ │ │ │ -50 _k_e_y(_key), _v_a_l_u_e(_value) { │ │ │ │ │ -51 } │ │ │ │ │ -52 ___V_a_l_u_e_s_C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r(const ___V_a_l_u_e_s_K_e_y_V_a_l_u_e_P_a_i_r_<_V_a_l_u_e_T_y_p_e_>& rhs) : │ │ │ │ │ -53 _k_e_y(rhs._k_e_y), _v_a_l_u_e(rhs._v_a_l_u_e) { │ │ │ │ │ -54 } │ │ │ │ │ -55 }; │ │ │ │ │ +46// Special BayesTree class that uses ISAM2 cliques - this is the result of │ │ │ │ │ +47// reeliminating ISAM2 subtrees. │ │ │ │ │ +_4_8class _I_S_A_M_2_B_a_y_e_s_T_r_e_e : public _I_S_A_M_2_:_:_B_a_s_e { │ │ │ │ │ +49 public: │ │ │ │ │ +50 typedef _I_S_A_M_2_:_:_B_a_s_e _B_a_s_e; │ │ │ │ │ +51 typedef _I_S_A_M_2_B_a_y_e_s_T_r_e_e _T_h_i_s; │ │ │ │ │ +52 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +53 │ │ │ │ │ +54 _I_S_A_M_2_B_a_y_e_s_T_r_e_e() {} │ │ │ │ │ +55}; │ │ │ │ │ 56 │ │ │ │ │ -57 /* ************************************************************************* │ │ │ │ │ +57/* ************************************************************************* │ │ │ │ │ */ │ │ │ │ │ -58 │ │ │ │ │ -59 // Cast helpers for making _Values[Const]KeyValuePair's from Values:: │ │ │ │ │ -[Const]KeyValuePair │ │ │ │ │ -60 // need to use a struct here for later partial specialization │ │ │ │ │ -61 template │ │ │ │ │ -_6_2 struct _V_a_l_u_e_s_C_a_s_t_H_e_l_p_e_r { │ │ │ │ │ -63 static CastedKeyValuePairType cast(KeyValuePairType key_value) { │ │ │ │ │ -64 // Static cast because we already checked the type during filtering │ │ │ │ │ -65 return CastedKeyValuePairType(key_value.key, │ │ │ │ │ -66 const_cast<_G_e_n_e_r_i_c_V_a_l_u_e_<_V_a_l_u_e_T_y_p_e_>&>(static_cast&>(key_value.value))._v_a_l_u_e()); │ │ │ │ │ -68 } │ │ │ │ │ -69 }; │ │ │ │ │ -70 // partial specialized version for ValueType == Value │ │ │ │ │ -71 template │ │ │ │ │ -_7_2 struct _V_a_l_u_e_s_C_a_s_t_H_e_l_p_e_r<_V_a_l_u_e, CastedKeyValuePairType, KeyValuePairType> { │ │ │ │ │ -73 static CastedKeyValuePairType cast(KeyValuePairType key_value) { │ │ │ │ │ -74 // Static cast because we already checked the type during filtering │ │ │ │ │ -75 // in this case the casted and keyvalue pair are essentially the same type │ │ │ │ │ -76 // (key, Value&) so perhaps this could be done with just a cast of the │ │ │ │ │ -key_value? │ │ │ │ │ -77 return CastedKeyValuePairType(key_value.key, key_value.value); │ │ │ │ │ -78 } │ │ │ │ │ -79 }; │ │ │ │ │ -80 // partial specialized version for ValueType == Value │ │ │ │ │ -81 template │ │ │ │ │ -_8_2 struct _V_a_l_u_e_s_C_a_s_t_H_e_l_p_e_r { │ │ │ │ │ -83 static CastedKeyValuePairType cast(KeyValuePairType key_value) { │ │ │ │ │ -84 // Static cast because we already checked the type during filtering │ │ │ │ │ -85 // in this case the casted and keyvalue pair are essentially the same type │ │ │ │ │ -86 // (key, Value&) so perhaps this could be done with just a cast of the │ │ │ │ │ -key_value? │ │ │ │ │ -87 return CastedKeyValuePairType(key_value.key, key_value.value); │ │ │ │ │ -88 } │ │ │ │ │ -89 }; │ │ │ │ │ -90 │ │ │ │ │ -91/* ************************************************************************* │ │ │ │ │ +58// Special JunctionTree class that produces ISAM2 BayesTree cliques, used for │ │ │ │ │ +59// reeliminating ISAM2 subtrees. │ │ │ │ │ +_6_0class _I_S_A_M_2_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ +61 : public _J_u_n_c_t_i_o_n_T_r_e_e { │ │ │ │ │ +62 public: │ │ │ │ │ +63 typedef _J_u_n_c_t_i_o_n_T_r_e_e_<_I_S_A_M_2_B_a_y_e_s_T_r_e_e_,_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_> _B_a_s_e; │ │ │ │ │ +64 typedef _I_S_A_M_2_J_u_n_c_t_i_o_n_T_r_e_e _T_h_i_s; │ │ │ │ │ +65 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +66 │ │ │ │ │ +67 explicit _I_S_A_M_2_J_u_n_c_t_i_o_n_T_r_e_e(const _G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e& eliminationTree) │ │ │ │ │ +68 : _B_a_s_e(eliminationTree) {} │ │ │ │ │ +69}; │ │ │ │ │ +70 │ │ │ │ │ +71/* ************************************************************************* │ │ │ │ │ */ │ │ │ │ │ -92 template │ │ │ │ │ -93 std::map │ │ │ │ │ -_9_4 _V_a_l_u_e_s_:_:_e_x_t_r_a_c_t(const std::function& filterFcn) const { │ │ │ │ │ -95 std::map result; │ │ │ │ │ -96 for (const auto& key_value : values_) { │ │ │ │ │ -97 // Check if key matches │ │ │ │ │ -98 if (filterFcn(key_value.first)) { │ │ │ │ │ -99 // Check if type matches (typically does as symbols matched with types) │ │ │ │ │ -100 if (auto t = │ │ │ │ │ -101 dynamic_cast*>(key_value.second)) │ │ │ │ │ -102 result[key_value.first] = t->value(); │ │ │ │ │ -103 } │ │ │ │ │ -104 } │ │ │ │ │ -105 return result; │ │ │ │ │ -106 } │ │ │ │ │ +_7_2struct GTSAM_EXPORT _D_e_l_t_a_I_m_p_l { │ │ │ │ │ +_7_3 struct GTSAM_EXPORT _P_a_r_t_i_a_l_S_o_l_v_e_R_e_s_u_l_t { │ │ │ │ │ +74 _I_S_A_M_2_:_:_s_h_a_r_e_d_C_l_i_q_u_e bayesTree; │ │ │ │ │ +75 }; │ │ │ │ │ +76 │ │ │ │ │ +_7_7 struct GTSAM_EXPORT _R_e_o_r_d_e_r_i_n_g_M_o_d_e { │ │ │ │ │ +78 size_t nFullSystemVars; │ │ │ │ │ +79 enum { /*AS_ADDED,*/ COLAMD } algorithm; │ │ │ │ │ +80 enum { NO_CONSTRAINT, CONSTRAIN_LAST } constrain; │ │ │ │ │ +81 boost::optional > constrainedKeys; │ │ │ │ │ +82 }; │ │ │ │ │ +83 │ │ │ │ │ +87 static size_t UpdateGaussNewtonDelta(const ISAM2::Roots& roots, │ │ │ │ │ +88 const _K_e_y_S_e_t& replacedKeys, │ │ │ │ │ +89 double wildfireThreshold, │ │ │ │ │ +90 _V_e_c_t_o_r_V_a_l_u_e_s* delta); │ │ │ │ │ +91 │ │ │ │ │ +96 static size_t UpdateRgProd(const ISAM2::Roots& roots, │ │ │ │ │ +97 const _K_e_y_S_e_t& replacedKeys, │ │ │ │ │ +98 const _V_e_c_t_o_r_V_a_l_u_e_s& gradAtZero, │ │ │ │ │ +99 _V_e_c_t_o_r_V_a_l_u_e_s* RgProd); │ │ │ │ │ +100 │ │ │ │ │ +104 static _V_e_c_t_o_r_V_a_l_u_e_s ComputeGradientSearch(const _V_e_c_t_o_r_V_a_l_u_e_s& gradAtZero, │ │ │ │ │ +105 const _V_e_c_t_o_r_V_a_l_u_e_s& RgProd); │ │ │ │ │ +106}; │ │ │ │ │ 107 │ │ │ │ │ 108/* ************************************************************************* │ │ │ │ │ */ │ │ │ │ │ -109#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ -110#include │ │ │ │ │ -111 │ │ │ │ │ -112 template │ │ │ │ │ -113 class Values::Filtered { │ │ │ │ │ -114 public: │ │ │ │ │ -116 typedef ___V_a_l_u_e_s_K_e_y_V_a_l_u_e_P_a_i_r_<_V_a_l_u_e_T_y_p_e_> KeyValuePair; │ │ │ │ │ -117 typedef ___V_a_l_u_e_s_C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r_<_V_a_l_u_e_T_y_p_e_> ConstKeyValuePair; │ │ │ │ │ -118 typedef KeyValuePair value_type; │ │ │ │ │ +_1_1_4struct GTSAM_EXPORT _U_p_d_a_t_e_I_m_p_l { │ │ │ │ │ +115 const _I_S_A_M_2_P_a_r_a_m_s& params_; │ │ │ │ │ +116 const _I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s& updateParams_; │ │ │ │ │ +117 _U_p_d_a_t_e_I_m_p_l(const _I_S_A_M_2_P_a_r_a_m_s& params, const _I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s& │ │ │ │ │ +updateParams) │ │ │ │ │ +118 : params_(params), updateParams_(updateParams) {} │ │ │ │ │ 119 │ │ │ │ │ -120 typedef │ │ │ │ │ -121 boost::transform_iterator< │ │ │ │ │ -122 KeyValuePair(*)(_V_a_l_u_e_s_:_:_K_e_y_V_a_l_u_e_P_a_i_r), │ │ │ │ │ -123 boost::filter_iterator< │ │ │ │ │ -124 std::function, │ │ │ │ │ -125 Values::iterator> > │ │ │ │ │ -126 iterator; │ │ │ │ │ -127 │ │ │ │ │ -128 typedef iterator const_iterator; │ │ │ │ │ -129 │ │ │ │ │ -130 typedef │ │ │ │ │ -131 boost::transform_iterator< │ │ │ │ │ -132 ConstKeyValuePair(*)(_V_a_l_u_e_s_:_:_C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r), │ │ │ │ │ -133 boost::filter_iterator< │ │ │ │ │ -134 std::function, │ │ │ │ │ -135 Values::const_iterator> > │ │ │ │ │ -136 const_const_iterator; │ │ │ │ │ -137 │ │ │ │ │ -138 iterator begin() { return begin_; } │ │ │ │ │ -139 iterator end() { return end_; } │ │ │ │ │ -140 const_iterator begin() const { return begin_; } │ │ │ │ │ -141 const_iterator end() const { return end_; } │ │ │ │ │ -142 const_const_iterator beginConst() const { return constBegin_; } │ │ │ │ │ -143 const_const_iterator endConst() const { return constEnd_; } │ │ │ │ │ +120 // Provide some debugging information at the start of update │ │ │ │ │ +121 static void LogStartingUpdate(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& newFactors, │ │ │ │ │ +122 const _I_S_A_M_2& isam2) { │ │ │ │ │ +123 gttic(pushBackFactors); │ │ │ │ │ +124 const bool debug = ISDEBUG("ISAM2 update"); │ │ │ │ │ +125 const bool verbose = ISDEBUG("ISAM2 update verbose"); │ │ │ │ │ +126 │ │ │ │ │ +127 if (verbose) { │ │ │ │ │ +128 std::cout << "ISAM2::update\n"; │ │ │ │ │ +129 isam2._p_r_i_n_t("ISAM2: "); │ │ │ │ │ +130 } │ │ │ │ │ +131 │ │ │ │ │ +132 if (debug || verbose) { │ │ │ │ │ +133 newFactors._p_r_i_n_t("The new factors are: "); │ │ │ │ │ +134 } │ │ │ │ │ +135 } │ │ │ │ │ +136 │ │ │ │ │ +137 // Check relinearization if we're at the nth step, or we are using a looser │ │ │ │ │ +138 // loop relinerization threshold. │ │ │ │ │ +139 bool relinarizationNeeded(size_t update_count) const { │ │ │ │ │ +140 return updateParams_._f_o_r_c_e___r_e_l_i_n_e_a_r_i_z_e || │ │ │ │ │ +141 (params_.enableRelinearization && │ │ │ │ │ +142 update_count % params_.relinearizeSkip == 0); │ │ │ │ │ +143 } │ │ │ │ │ 144 │ │ │ │ │ -146 size_t size() const { │ │ │ │ │ -147 size_t i = 0; │ │ │ │ │ -148 for (const_const_iterator it = beginConst(); it != endConst(); ++it) │ │ │ │ │ -149 ++i; │ │ │ │ │ -150 return i; │ │ │ │ │ -151 } │ │ │ │ │ -152 │ │ │ │ │ -153 private: │ │ │ │ │ -154 Filtered( │ │ │ │ │ -155 const std::function& filter, │ │ │ │ │ -156 _V_a_l_u_e_s& values) : │ │ │ │ │ -157 begin_( │ │ │ │ │ -158 boost::make_transform_iterator( │ │ │ │ │ -159 boost::make_filter_iterator(filter, values.begin(), values.end()), │ │ │ │ │ -160 &ValuesCastHelper::cast)), │ │ │ │ │ -end_( │ │ │ │ │ -161 boost::make_transform_iterator( │ │ │ │ │ -162 boost::make_filter_iterator(filter, values.end(), values.end()), │ │ │ │ │ -163 &ValuesCastHelper::cast)), │ │ │ │ │ -constBegin_( │ │ │ │ │ -164 boost::make_transform_iterator( │ │ │ │ │ -165 boost::make_filter_iterator(filter, │ │ │ │ │ -166 values._begin(), │ │ │ │ │ -167 values._end()), │ │ │ │ │ -168 &ValuesCastHelper::cast)), constEnd_( │ │ │ │ │ -170 boost::make_transform_iterator( │ │ │ │ │ -171 boost::make_filter_iterator(filter, │ │ │ │ │ -172 values._end(), │ │ │ │ │ -173 values._end()), │ │ │ │ │ -174 &ValuesCastHelper::cast)) { │ │ │ │ │ +145 // Add any new factors \Factors:=\Factors\cup\Factors'. │ │ │ │ │ +146 void pushBackFactors(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& newFactors, │ │ │ │ │ +147 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h* nonlinearFactors, │ │ │ │ │ +148 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h* linearFactors, │ │ │ │ │ +149 _V_a_r_i_a_b_l_e_I_n_d_e_x* variableIndex, │ │ │ │ │ +150 _F_a_c_t_o_r_I_n_d_i_c_e_s* newFactorsIndices, │ │ │ │ │ +151 _K_e_y_S_e_t* keysWithRemovedFactors) const { │ │ │ │ │ +152 gttic(pushBackFactors); │ │ │ │ │ +153 │ │ │ │ │ +154 // Perform the first part of the bookkeeping updates for adding new │ │ │ │ │ +factors. │ │ │ │ │ +155 // Adds them to the complete list of nonlinear factors, and populates the │ │ │ │ │ +156 // list of new factor indices, both optionally finding and reusing empty │ │ │ │ │ +157 // factor slots. │ │ │ │ │ +158 *newFactorsIndices = nonlinearFactors->_a_d_d___f_a_c_t_o_r_s( │ │ │ │ │ +159 newFactors, params_.findUnusedFactorSlots); │ │ │ │ │ +160 │ │ │ │ │ +161 // Remove the removed factors │ │ │ │ │ +162 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h removedFactors; │ │ │ │ │ +163 removedFactors._r_e_s_e_r_v_e(updateParams_._r_e_m_o_v_e_F_a_c_t_o_r_I_n_d_i_c_e_s.size()); │ │ │ │ │ +164 for (const auto index : updateParams_._r_e_m_o_v_e_F_a_c_t_o_r_I_n_d_i_c_e_s) { │ │ │ │ │ +165 removedFactors._p_u_s_h___b_a_c_k(nonlinearFactors->_a_t(index)); │ │ │ │ │ +166 nonlinearFactors->_r_e_m_o_v_e(index); │ │ │ │ │ +167 if (params_.cacheLinearizedFactors) linearFactors->_r_e_m_o_v_e(index); │ │ │ │ │ +168 } │ │ │ │ │ +169 │ │ │ │ │ +170 // Remove removed factors from the variable index so we do not attempt to │ │ │ │ │ +171 // relinearize them │ │ │ │ │ +172 variableIndex->_r_e_m_o_v_e(updateParams_._r_e_m_o_v_e_F_a_c_t_o_r_I_n_d_i_c_e_s.begin(), │ │ │ │ │ +173 updateParams_._r_e_m_o_v_e_F_a_c_t_o_r_I_n_d_i_c_e_s.end(), │ │ │ │ │ +174 removedFactors); │ │ │ │ │ +175 *keysWithRemovedFactors = removedFactors._k_e_y_s(); │ │ │ │ │ 176 } │ │ │ │ │ 177 │ │ │ │ │ -178 friend class _V_a_l_u_e_s; │ │ │ │ │ -179 iterator begin_; │ │ │ │ │ -180 iterator end_; │ │ │ │ │ -181 const_const_iterator constBegin_; │ │ │ │ │ -182 const_const_iterator constEnd_; │ │ │ │ │ -183 }; │ │ │ │ │ -184 │ │ │ │ │ -185 template │ │ │ │ │ -186 class Values::ConstFiltered { │ │ │ │ │ -187 public: │ │ │ │ │ -189 typedef _ValuesConstKeyValuePair KeyValuePair; │ │ │ │ │ -190 typedef KeyValuePair value_type; │ │ │ │ │ -191 │ │ │ │ │ -192 typedef typename Filtered::const_const_iterator iterator; │ │ │ │ │ -193 typedef typename Filtered::const_const_iterator const_iterator; │ │ │ │ │ -194 │ │ │ │ │ -196 ConstFiltered(const Filtered& rhs) : │ │ │ │ │ -197 begin_(rhs.beginConst()), │ │ │ │ │ -198 end_(rhs.endConst()) {} │ │ │ │ │ -199 │ │ │ │ │ -200 iterator begin() { return begin_; } │ │ │ │ │ -201 iterator end() { return end_; } │ │ │ │ │ -202 const_iterator begin() const { return begin_; } │ │ │ │ │ -203 const_iterator end() const { return end_; } │ │ │ │ │ -204 │ │ │ │ │ -206 size_t size() const { │ │ │ │ │ -207 size_t i = 0; │ │ │ │ │ -208 for (const_iterator it = begin(); it != end(); ++it) │ │ │ │ │ -209 ++i; │ │ │ │ │ -210 return i; │ │ │ │ │ -211 } │ │ │ │ │ -212 │ │ │ │ │ -213 FastList keys() const { │ │ │ │ │ -214 FastList result; │ │ │ │ │ -215 for(const_iterator it = begin(); it != end(); ++it) │ │ │ │ │ -216 result.push_back(it->key); │ │ │ │ │ -217 return result; │ │ │ │ │ +178 // Get keys from removed factors and new factors, and compute unused keys, │ │ │ │ │ +179 // i.e., keys that are empty now and do not appear in the new factors. │ │ │ │ │ +180 void computeUnusedKeys(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& newFactors, │ │ │ │ │ +181 const _V_a_r_i_a_b_l_e_I_n_d_e_x& variableIndex, │ │ │ │ │ +182 const _K_e_y_S_e_t& keysWithRemovedFactors, │ │ │ │ │ +183 _K_e_y_S_e_t* unusedKeys) const { │ │ │ │ │ +184 gttic(computeUnusedKeys); │ │ │ │ │ +185 _K_e_y_S_e_t removedAndEmpty; │ │ │ │ │ +186 for (_K_e_y key : keysWithRemovedFactors) { │ │ │ │ │ +187 if (variableIndex._e_m_p_t_y(key)) │ │ │ │ │ +188 removedAndEmpty.insert(removedAndEmpty.end(), key); │ │ │ │ │ +189 } │ │ │ │ │ +190 _K_e_y_S_e_t newFactorSymbKeys = newFactors._k_e_y_s(); │ │ │ │ │ +191 std::set_difference(removedAndEmpty.begin(), removedAndEmpty.end(), │ │ │ │ │ +192 newFactorSymbKeys.begin(), newFactorSymbKeys.end(), │ │ │ │ │ +193 std::inserter(*unusedKeys, unusedKeys->end())); │ │ │ │ │ +194 } │ │ │ │ │ +195 │ │ │ │ │ +196 // Calculate nonlinear error │ │ │ │ │ +197 void error(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& nonlinearFactors, │ │ │ │ │ +198 const _V_a_l_u_e_s& estimate, boost::optional* result) const { │ │ │ │ │ +199 gttic(error); │ │ │ │ │ +200 result->reset(nonlinearFactors._e_r_r_o_r(estimate)); │ │ │ │ │ +201 } │ │ │ │ │ +202 │ │ │ │ │ +203 // Mark linear update │ │ │ │ │ +204 void gatherInvolvedKeys(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& newFactors, │ │ │ │ │ +205 const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& nonlinearFactors, │ │ │ │ │ +206 const _K_e_y_S_e_t& keysWithRemovedFactors, │ │ │ │ │ +207 _K_e_y_S_e_t* markedKeys) const { │ │ │ │ │ +208 gttic(gatherInvolvedKeys); │ │ │ │ │ +209 *markedKeys = newFactors._k_e_y_s(); // Get keys from new factors │ │ │ │ │ +210 // Also mark keys involved in removed factors │ │ │ │ │ +211 markedKeys->insert(keysWithRemovedFactors.begin(), │ │ │ │ │ +212 keysWithRemovedFactors.end()); │ │ │ │ │ +213 │ │ │ │ │ +214 // Also mark any provided extra re-eliminate keys │ │ │ │ │ +215 if (updateParams_._e_x_t_r_a_R_e_e_l_i_m_K_e_y_s) { │ │ │ │ │ +216 for (_K_e_y key : *updateParams_._e_x_t_r_a_R_e_e_l_i_m_K_e_y_s) { │ │ │ │ │ +217 markedKeys->insert(key); │ │ │ │ │ 218 } │ │ │ │ │ -219 │ │ │ │ │ -220 private: │ │ │ │ │ -221 friend class _V_a_l_u_e_s; │ │ │ │ │ -222 const_iterator begin_; │ │ │ │ │ -223 const_iterator end_; │ │ │ │ │ -224 ConstFiltered( │ │ │ │ │ -225 const std::function& filter, │ │ │ │ │ -226 const _V_a_l_u_e_s& values) { │ │ │ │ │ -227 // We remove the const from values to create a non-const Filtered │ │ │ │ │ -228 // view, then pull the const_iterators out of it. │ │ │ │ │ -229 const Filtered filtered(filter, const_cast<_V_a_l_u_e_s&>(values)); │ │ │ │ │ -230 begin_ = filtered.beginConst(); │ │ │ │ │ -231 end_ = filtered.endConst(); │ │ │ │ │ -232 } │ │ │ │ │ -233 }; │ │ │ │ │ -234 │ │ │ │ │ -235 template │ │ │ │ │ -236 _V_a_l_u_e_s_:_:_V_a_l_u_e_s(const Values::Filtered& view) { │ │ │ │ │ -237 for(const auto key_value: view) { │ │ │ │ │ -238 _K_e_y key = key_value.key; │ │ │ │ │ -239 _i_n_s_e_r_t(key, static_cast(key_value.value)); │ │ │ │ │ +219 } │ │ │ │ │ +220 │ │ │ │ │ +221 // Also, keys that were not observed in existing factors, but whose │ │ │ │ │ +affected │ │ │ │ │ +222 // keys have been extended now (e.g. smart factors) │ │ │ │ │ +223 if (updateParams_._n_e_w_A_f_f_e_c_t_e_d_K_e_y_s) { │ │ │ │ │ +224 for (const auto& factorAddedKeys : *updateParams_._n_e_w_A_f_f_e_c_t_e_d_K_e_y_s) { │ │ │ │ │ +225 const auto factorIdx = factorAddedKeys.first; │ │ │ │ │ +226 const auto& affectedKeys = nonlinearFactors._a_t(factorIdx)->keys(); │ │ │ │ │ +227 markedKeys->insert(affectedKeys.begin(), affectedKeys.end()); │ │ │ │ │ +228 } │ │ │ │ │ +229 } │ │ │ │ │ +230 } │ │ │ │ │ +231 │ │ │ │ │ +232 // Update detail, unused, and observed keys from markedKeys │ │ │ │ │ +233 void updateKeys(const _K_e_y_S_e_t& markedKeys, _I_S_A_M_2_R_e_s_u_l_t* result) const { │ │ │ │ │ +234 gttic(updateKeys); │ │ │ │ │ +235 // Observed keys for detailed results │ │ │ │ │ +236 if (result->detail && params_.enableDetailedResults) { │ │ │ │ │ +237 for (_K_e_y key : markedKeys) { │ │ │ │ │ +238 result->detail->variableStatus[key].isObserved = true; │ │ │ │ │ +239 } │ │ │ │ │ 240 } │ │ │ │ │ -241 } │ │ │ │ │ -242 │ │ │ │ │ -243 template │ │ │ │ │ -244 _V_a_l_u_e_s_:_:_V_a_l_u_e_s(const Values::ConstFiltered& view) { │ │ │ │ │ -245 for(const auto key_value: view) { │ │ │ │ │ -246 _K_e_y key = key_value.key; │ │ │ │ │ -247 _i_n_s_e_r_t(key, static_cast(key_value.value)); │ │ │ │ │ +241 │ │ │ │ │ +242 for (_K_e_y index : markedKeys) { │ │ │ │ │ +243 // Only add if not unused │ │ │ │ │ +244 if (result->unusedKeys.find(index) == result->unusedKeys.end()) │ │ │ │ │ +245 // Make a copy of these, as we'll soon add to them │ │ │ │ │ +246 result->observedKeys.push_back(index); │ │ │ │ │ +247 } │ │ │ │ │ 248 } │ │ │ │ │ -249 } │ │ │ │ │ -250 │ │ │ │ │ -251 Values::Filtered │ │ │ │ │ -252 inline Values::filter(const std::function& filterFcn) { │ │ │ │ │ -253 return filter(filterFcn); │ │ │ │ │ -254 } │ │ │ │ │ -255 │ │ │ │ │ -256 template │ │ │ │ │ -257 Values::Filtered │ │ │ │ │ -258 Values::filter(const std::function& filterFcn) { │ │ │ │ │ -259 return Filtered(std::bind(&filterHelper, filterFcn, │ │ │ │ │ -260 std::placeholders::_1), *this); │ │ │ │ │ -261 } │ │ │ │ │ -262 │ │ │ │ │ -263 Values::ConstFiltered │ │ │ │ │ -264 inline Values::filter(const std::function& filterFcn) const { │ │ │ │ │ -265 return filter(filterFcn); │ │ │ │ │ -266 } │ │ │ │ │ -267 │ │ │ │ │ -268 template │ │ │ │ │ -269 Values::ConstFiltered │ │ │ │ │ -270 Values::filter(const std::function& filterFcn) const { │ │ │ │ │ -271 return ConstFiltered(std::bind(&filterHelper, │ │ │ │ │ -272 filterFcn, std::placeholders::_1), *this); │ │ │ │ │ +249 │ │ │ │ │ +250 static void CheckRelinearizationRecursiveMap( │ │ │ │ │ +251 const _F_a_s_t_M_a_p_<_c_h_a_r_,_ _V_e_c_t_o_r_>& thresholds, const _V_e_c_t_o_r_V_a_l_u_e_s& delta, │ │ │ │ │ +252 const _I_S_A_M_2_:_:_s_h_a_r_e_d_C_l_i_q_u_e& clique, _K_e_y_S_e_t* relinKeys) { │ │ │ │ │ +253 // Check the current clique for relinearization │ │ │ │ │ +254 bool relinearize = false; │ │ │ │ │ +255 for (_K_e_y var : *clique->conditional()) { │ │ │ │ │ +256 // Find the threshold for this variable type │ │ │ │ │ +257 const Vector& threshold = thresholds.find(_S_y_m_b_o_l(var)._c_h_r())->second; │ │ │ │ │ +258 │ │ │ │ │ +259 const Vector& deltaVar = delta[var]; │ │ │ │ │ +260 │ │ │ │ │ +261 // Verify the threshold vector matches the actual variable size │ │ │ │ │ +262 if (threshold.rows() != deltaVar.rows()) │ │ │ │ │ +263 throw std::invalid_argument( │ │ │ │ │ +264 "Relinearization threshold vector dimensionality for '" + │ │ │ │ │ +265 std::string(1, _S_y_m_b_o_l(var).chr()) + │ │ │ │ │ +266 "' passed into iSAM2 parameters does not match actual variable " │ │ │ │ │ +267 "dimensionality."); │ │ │ │ │ +268 │ │ │ │ │ +269 // Check for relinearization │ │ │ │ │ +270 if ((deltaVar.array().abs() > threshold.array()).any()) { │ │ │ │ │ +271 relinKeys->insert(var); │ │ │ │ │ +272 relinearize = true; │ │ │ │ │ 273 } │ │ │ │ │ -274#endif │ │ │ │ │ +274 } │ │ │ │ │ 275 │ │ │ │ │ -276 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -277 template<> │ │ │ │ │ -278 inline bool Values::filterHelper(const std::function │ │ │ │ │ -filter, │ │ │ │ │ -279 const ConstKeyValuePair& key_value) { │ │ │ │ │ -280 // Filter and check the type │ │ │ │ │ -281 return filter(key_value.key); │ │ │ │ │ +276 // If this node was relinearized, also check its children │ │ │ │ │ +277 if (relinearize) { │ │ │ │ │ +278 for (const _I_S_A_M_2_:_:_s_h_a_r_e_d_C_l_i_q_u_e& child : clique->children) { │ │ │ │ │ +279 CheckRelinearizationRecursiveMap(thresholds, delta, child, relinKeys); │ │ │ │ │ +280 } │ │ │ │ │ +281 } │ │ │ │ │ 282 } │ │ │ │ │ 283 │ │ │ │ │ -284 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -285 │ │ │ │ │ -286 namespace internal { │ │ │ │ │ -287 │ │ │ │ │ -288 // Check the type and throw exception if incorrect │ │ │ │ │ -289 // Generic version, partially specialized below for various Eigen Matrix │ │ │ │ │ -types │ │ │ │ │ -290 template │ │ │ │ │ -_2_9_1 struct _h_a_n_d_l_e { │ │ │ │ │ -292 ValueType operator()(_K_e_y j, const _V_a_l_u_e* const pointer) { │ │ │ │ │ -293 auto ptr = dynamic_cast*>(pointer); │ │ │ │ │ -294 if (ptr) { │ │ │ │ │ -295 // value returns a const ValueType&, and the return makes a copy !!!!! │ │ │ │ │ -296 return ptr->_v_a_l_u_e(); │ │ │ │ │ -297 } else { │ │ │ │ │ -298 throw _V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e(j, typeid(*pointer), typeid(ValueType)); │ │ │ │ │ -299 } │ │ │ │ │ -300 } │ │ │ │ │ -301 }; │ │ │ │ │ -302 │ │ │ │ │ -303 template │ │ │ │ │ -_3_0_4 struct _h_a_n_d_l_e___m_a_t_r_i_x; │ │ │ │ │ -305 │ │ │ │ │ -306 // Handle dynamic matrices │ │ │ │ │ -307 template │ │ │ │ │ -_3_0_8 struct _h_a_n_d_l_e___m_a_t_r_i_x, true> { │ │ │ │ │ -309 inline Eigen::Matrix operator()(_K_e_y j, const _V_a_l_u_e* const │ │ │ │ │ -pointer) { │ │ │ │ │ -310 auto ptr = dynamic_cast>*> │ │ │ │ │ -(pointer); │ │ │ │ │ -311 if (ptr) { │ │ │ │ │ -312 // value returns a const Matrix&, and the return makes a copy !!!!! │ │ │ │ │ -313 return ptr->_v_a_l_u_e(); │ │ │ │ │ -314 } else { │ │ │ │ │ -315 // If a fixed matrix was stored, we end up here as well. │ │ │ │ │ -316 throw _V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e(j, typeid(*pointer), typeid(Eigen::Matrix)); │ │ │ │ │ -317 } │ │ │ │ │ -318 } │ │ │ │ │ -319 }; │ │ │ │ │ -320 │ │ │ │ │ -321 // Handle fixed matrices │ │ │ │ │ -322 template │ │ │ │ │ -_3_2_3 struct _h_a_n_d_l_e___m_a_t_r_i_x, false> { │ │ │ │ │ -324 inline Eigen::Matrix operator()(_K_e_y j, const _V_a_l_u_e* const │ │ │ │ │ -pointer) { │ │ │ │ │ -325 auto ptr = dynamic_cast>*> │ │ │ │ │ -(pointer); │ │ │ │ │ -326 if (ptr) { │ │ │ │ │ -327 // value returns a const MatrixMN&, and the return makes a copy !!!!! │ │ │ │ │ -328 return ptr->_v_a_l_u_e(); │ │ │ │ │ -329 } else { │ │ │ │ │ -330 Matrix A; │ │ │ │ │ -331 // Check if a dynamic matrix was stored │ │ │ │ │ -332 auto ptr = dynamic_cast*>(pointer); │ │ │ │ │ -333 if (ptr) { │ │ │ │ │ -334 A = ptr->value(); │ │ │ │ │ -335 } else { │ │ │ │ │ -336 // Or a dynamic vector │ │ │ │ │ -337 A = _h_a_n_d_l_e___m_a_t_r_i_x_<_E_i_g_e_n_:_:_V_e_c_t_o_r_X_d_,_ _t_r_u_e_>()(j, pointer); // will throw if │ │ │ │ │ -not.... │ │ │ │ │ -338 } │ │ │ │ │ -339 // Yes: check size, and throw if not a match │ │ │ │ │ -340 if (A.rows() != M || A.cols() != N) │ │ │ │ │ -341 throw _N_o_M_a_t_c_h_F_o_u_n_d_F_o_r_F_i_x_e_d(M, N, A.rows(), A.cols()); │ │ │ │ │ -342 else │ │ │ │ │ -343 return A; // copy but not malloc │ │ │ │ │ -344 } │ │ │ │ │ -345 } │ │ │ │ │ -346 }; │ │ │ │ │ -347 │ │ │ │ │ -348 // Handle matrices │ │ │ │ │ -349 template │ │ │ │ │ -_3_5_0 struct _h_a_n_d_l_e> { │ │ │ │ │ -351 Eigen::Matrix operator()(_K_e_y j, const _V_a_l_u_e* const pointer) { │ │ │ │ │ -352 return _h_a_n_d_l_e___m_a_t_r_i_x_<_E_i_g_e_n_:_:_M_a_t_r_i_x_<_d_o_u_b_l_e_,_ _M_,_ _N_>, │ │ │ │ │ -353 (M == Eigen::Dynamic || N == Eigen::Dynamic)>()(j, pointer); │ │ │ │ │ +284 static void CheckRelinearizationRecursiveDouble( │ │ │ │ │ +285 double threshold, const _V_e_c_t_o_r_V_a_l_u_e_s& delta, │ │ │ │ │ +286 const _I_S_A_M_2_:_:_s_h_a_r_e_d_C_l_i_q_u_e& clique, _K_e_y_S_e_t* relinKeys) { │ │ │ │ │ +287 // Check the current clique for relinearization │ │ │ │ │ +288 bool relinearize = false; │ │ │ │ │ +289 for (_K_e_y var : *clique->conditional()) { │ │ │ │ │ +290 double maxDelta = delta[var].lpNorm(); │ │ │ │ │ +291 if (maxDelta >= threshold) { │ │ │ │ │ +292 relinKeys->insert(var); │ │ │ │ │ +293 relinearize = true; │ │ │ │ │ +294 } │ │ │ │ │ +295 } │ │ │ │ │ +296 │ │ │ │ │ +297 // If this node was relinearized, also check its children │ │ │ │ │ +298 if (relinearize) { │ │ │ │ │ +299 for (const _I_S_A_M_2_:_:_s_h_a_r_e_d_C_l_i_q_u_e& child : clique->children) { │ │ │ │ │ +300 CheckRelinearizationRecursiveDouble(threshold, delta, child, relinKeys); │ │ │ │ │ +301 } │ │ │ │ │ +302 } │ │ │ │ │ +303 } │ │ │ │ │ +304 │ │ │ │ │ +_3_1_8 static _K_e_y_S_e_t _C_h_e_c_k_R_e_l_i_n_e_a_r_i_z_a_t_i_o_n_P_a_r_t_i_a_l( │ │ │ │ │ +319 const _I_S_A_M_2_:_:_R_o_o_t_s& roots, const _V_e_c_t_o_r_V_a_l_u_e_s& delta, │ │ │ │ │ +320 const _I_S_A_M_2_P_a_r_a_m_s_:_:_R_e_l_i_n_e_a_r_i_z_a_t_i_o_n_T_h_r_e_s_h_o_l_d& relinearizeThreshold) { │ │ │ │ │ +321 _K_e_y_S_e_t relinKeys; │ │ │ │ │ +322 for (const _I_S_A_M_2_:_:_s_h_a_r_e_d_C_l_i_q_u_e& root : roots) { │ │ │ │ │ +323 if (relinearizeThreshold.type() == typeid(double)) │ │ │ │ │ +324 CheckRelinearizationRecursiveDouble( │ │ │ │ │ +325 boost::get(relinearizeThreshold), delta, root, &relinKeys); │ │ │ │ │ +326 else if (relinearizeThreshold.type() == typeid(_F_a_s_t_M_a_p_<_c_h_a_r_,_ _V_e_c_t_o_r_>)) │ │ │ │ │ +327 CheckRelinearizationRecursiveMap( │ │ │ │ │ +328 boost::get<_F_a_s_t_M_a_p_<_c_h_a_r_,_ _V_e_c_t_o_r_> >(relinearizeThreshold), delta, │ │ │ │ │ +329 root, &relinKeys); │ │ │ │ │ +330 } │ │ │ │ │ +331 return relinKeys; │ │ │ │ │ +332 } │ │ │ │ │ +333 │ │ │ │ │ +_3_4_5 static _K_e_y_S_e_t _C_h_e_c_k_R_e_l_i_n_e_a_r_i_z_a_t_i_o_n_F_u_l_l( │ │ │ │ │ +346 const _V_e_c_t_o_r_V_a_l_u_e_s& delta, │ │ │ │ │ +347 const _I_S_A_M_2_P_a_r_a_m_s_:_:_R_e_l_i_n_e_a_r_i_z_a_t_i_o_n_T_h_r_e_s_h_o_l_d& relinearizeThreshold) { │ │ │ │ │ +348 _K_e_y_S_e_t relinKeys; │ │ │ │ │ +349 │ │ │ │ │ +350 if (const double* threshold = boost::get(&relinearizeThreshold)) { │ │ │ │ │ +351 for (const _V_e_c_t_o_r_V_a_l_u_e_s_:_:_K_e_y_V_a_l_u_e_P_a_i_r& key_delta : delta) { │ │ │ │ │ +352 double maxDelta = key_delta.second.lpNorm(); │ │ │ │ │ +353 if (maxDelta >= *threshold) relinKeys.insert(key_delta.first); │ │ │ │ │ 354 } │ │ │ │ │ -355 }; │ │ │ │ │ -356 │ │ │ │ │ -357 } // internal │ │ │ │ │ -358 │ │ │ │ │ -359 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -360 template │ │ │ │ │ -_3_6_1 const ValueType _V_a_l_u_e_s_:_:_a_t(_K_e_y j) const { │ │ │ │ │ -362 // Find the item │ │ │ │ │ -363 KeyValueMap::const_iterator item = values_.find(j); │ │ │ │ │ -364 │ │ │ │ │ -365 // Throw exception if it does not exist │ │ │ │ │ -366 if (item == values_.end()) throw _V_a_l_u_e_s_K_e_y_D_o_e_s_N_o_t_E_x_i_s_t("at", j); │ │ │ │ │ -367 │ │ │ │ │ -368 // Check the type and throw exception if incorrect │ │ │ │ │ -369 // h() split in two lines to avoid internal compiler error (MSVC2017) │ │ │ │ │ -370 auto h = _i_n_t_e_r_n_a_l_:_:_h_a_n_d_l_e_<_V_a_l_u_e_T_y_p_e_>(); │ │ │ │ │ -371 return h(j, item->second); │ │ │ │ │ +355 } else if (const _F_a_s_t_M_a_p_<_c_h_a_r_,_ _V_e_c_t_o_r_>* thresholds = │ │ │ │ │ +356 boost::get<_F_a_s_t_M_a_p_<_c_h_a_r_,_ _V_e_c_t_o_r_> >(&relinearizeThreshold)) { │ │ │ │ │ +357 for (const _V_e_c_t_o_r_V_a_l_u_e_s_:_:_K_e_y_V_a_l_u_e_P_a_i_r& key_delta : delta) { │ │ │ │ │ +358 const Vector& threshold = │ │ │ │ │ +359 thresholds->find(_S_y_m_b_o_l(key_delta.first)._c_h_r())->second; │ │ │ │ │ +360 if (threshold.rows() != key_delta.second.rows()) │ │ │ │ │ +361 throw std::invalid_argument( │ │ │ │ │ +362 "Relinearization threshold vector dimensionality for '" + │ │ │ │ │ +363 std::string(1, _S_y_m_b_o_l(key_delta.first)._c_h_r()) + │ │ │ │ │ +364 "' passed into iSAM2 parameters does not match actual variable " │ │ │ │ │ +365 "dimensionality."); │ │ │ │ │ +366 if ((key_delta.second.array().abs() > threshold.array()).any()) │ │ │ │ │ +367 relinKeys.insert(key_delta.first); │ │ │ │ │ +368 } │ │ │ │ │ +369 } │ │ │ │ │ +370 │ │ │ │ │ +371 return relinKeys; │ │ │ │ │ 372 } │ │ │ │ │ 373 │ │ │ │ │ -374 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -375 template │ │ │ │ │ -_3_7_6 boost::optional _V_a_l_u_e_s_:_:_e_x_i_s_t_s(_K_e_y j) const { │ │ │ │ │ -377 // Find the item │ │ │ │ │ -378 KeyValueMap::const_iterator item = values_.find(j); │ │ │ │ │ -379 │ │ │ │ │ -380 if(item != values_.end()) { │ │ │ │ │ -381 // dynamic cast the type and throw exception if incorrect │ │ │ │ │ -382 auto ptr = dynamic_cast*>(item->second); │ │ │ │ │ -383 if (ptr) { │ │ │ │ │ -384 return ptr->_v_a_l_u_e(); │ │ │ │ │ -385 } else { │ │ │ │ │ -386 // NOTE(abe): clang warns about potential side effects if done in typeid │ │ │ │ │ -387 const _V_a_l_u_e* value = item->second; │ │ │ │ │ -388 throw _V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e(j, typeid(*value), typeid(ValueType)); │ │ │ │ │ -389 } │ │ │ │ │ -390 } else { │ │ │ │ │ -391 return boost::none; │ │ │ │ │ +374 // Mark keys in \Delta above threshold \beta: │ │ │ │ │ +375 _K_e_y_S_e_t gatherRelinearizeKeys(const _I_S_A_M_2_:_:_R_o_o_t_s& roots, │ │ │ │ │ +376 const _V_e_c_t_o_r_V_a_l_u_e_s& delta, │ │ │ │ │ +377 const _K_e_y_S_e_t& fixedVariables, │ │ │ │ │ +378 _K_e_y_S_e_t* markedKeys) const { │ │ │ │ │ +379 gttic(gatherRelinearizeKeys); │ │ │ │ │ +380 // J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. │ │ │ │ │ +381 _K_e_y_S_e_t relinKeys = │ │ │ │ │ +382 params_.enablePartialRelinearizationCheck │ │ │ │ │ +383 ? CheckRelinearizationPartial(roots, delta, │ │ │ │ │ +384 params_.relinearizeThreshold) │ │ │ │ │ +385 : CheckRelinearizationFull(delta, params_.relinearizeThreshold); │ │ │ │ │ +386 if (updateParams_._f_o_r_c_e_F_u_l_l_S_o_l_v_e) │ │ │ │ │ +387 relinKeys = CheckRelinearizationFull(delta, 0.0); // for debugging │ │ │ │ │ +388 │ │ │ │ │ +389 // Remove from relinKeys any keys whose linearization points are fixed │ │ │ │ │ +390 for (_K_e_y key : fixedVariables) { │ │ │ │ │ +391 relinKeys.erase(key); │ │ │ │ │ 392 } │ │ │ │ │ -393 } │ │ │ │ │ -394 │ │ │ │ │ -395 / │ │ │ │ │ -* ************************************************************************* */ │ │ │ │ │ -396 │ │ │ │ │ -397 // insert a templated value │ │ │ │ │ -398 template │ │ │ │ │ -_3_9_9 void _V_a_l_u_e_s_:_:_i_n_s_e_r_t(_K_e_y j, const ValueType& val) { │ │ │ │ │ -400 _i_n_s_e_r_t(j, static_cast(_G_e_n_e_r_i_c_V_a_l_u_e_<_V_a_l_u_e_T_y_p_e_>(val))); │ │ │ │ │ -401 } │ │ │ │ │ -402 │ │ │ │ │ -403 // update with templated value │ │ │ │ │ -404 template │ │ │ │ │ -405 void _V_a_l_u_e_s_:_:_u_p_d_a_t_e(_K_e_y j, const ValueType& val) { │ │ │ │ │ -406 _u_p_d_a_t_e(j, static_cast(_G_e_n_e_r_i_c_V_a_l_u_e_<_V_a_l_u_e_T_y_p_e_>(val))); │ │ │ │ │ -407 } │ │ │ │ │ -408 │ │ │ │ │ -409 // insert_or_assign with templated value │ │ │ │ │ -410 template │ │ │ │ │ -_4_1_1 void _V_a_l_u_e_s_:_:_i_n_s_e_r_t___o_r___a_s_s_i_g_n(_K_e_y j, const ValueType& val) { │ │ │ │ │ -412 _i_n_s_e_r_t___o_r___a_s_s_i_g_n(j, static_cast(_G_e_n_e_r_i_c_V_a_l_u_e_<_V_a_l_u_e_T_y_p_e_> │ │ │ │ │ -(val))); │ │ │ │ │ +393 if (updateParams_._n_o_R_e_l_i_n_K_e_y_s) { │ │ │ │ │ +394 for (Key key : *updateParams_.noRelinKeys) { │ │ │ │ │ +395 relinKeys.erase(key); │ │ │ │ │ +396 } │ │ │ │ │ +397 } │ │ │ │ │ +398 │ │ │ │ │ +399 // Add the variables being relinearized to the marked keys │ │ │ │ │ +400 markedKeys->insert(relinKeys.begin(), relinKeys.end()); │ │ │ │ │ +401 return relinKeys; │ │ │ │ │ +402 } │ │ │ │ │ +403 │ │ │ │ │ +404 // Record relinerization threshold keys in detailed results │ │ │ │ │ +405 void recordRelinearizeDetail(const KeySet& relinKeys, │ │ │ │ │ +406 ISAM2Result::DetailedResults* detail) const { │ │ │ │ │ +407 if (detail && params_.enableDetailedResults) { │ │ │ │ │ +408 for (Key key : relinKeys) { │ │ │ │ │ +409 detail->variableStatus[key].isAboveRelinThreshold = true; │ │ │ │ │ +410 detail->variableStatus[key].isRelinearized = true; │ │ │ │ │ +411 } │ │ │ │ │ +412 } │ │ │ │ │ 413 } │ │ │ │ │ 414 │ │ │ │ │ -415} │ │ │ │ │ -_V_a_l_u_e_s_._h │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +415 // Mark all cliques that involve marked variables \Theta_{J} and all │ │ │ │ │ +416 // their ancestors. │ │ │ │ │ +417 void findFluid(const ISAM2::Roots& roots, const KeySet& relinKeys, │ │ │ │ │ +418 KeySet* markedKeys, │ │ │ │ │ +419 ISAM2Result::DetailedResults* detail) const { │ │ │ │ │ +420 gttic(findFluid); │ │ │ │ │ +421 for (const auto& root : roots) │ │ │ │ │ +422 // add other cliques that have the marked ones in the separator │ │ │ │ │ +423 root->findAll(relinKeys, markedKeys); │ │ │ │ │ +424 │ │ │ │ │ +425 // Relinearization-involved keys for detailed results │ │ │ │ │ +426 if (detail && params_.enableDetailedResults) { │ │ │ │ │ +427 KeySet involvedRelinKeys; │ │ │ │ │ +428 for (const auto& root : roots) │ │ │ │ │ +429 root->findAll(relinKeys, &involvedRelinKeys); │ │ │ │ │ +430 for (Key key : involvedRelinKeys) { │ │ │ │ │ +431 if (!detail->variableStatus[key].isAboveRelinThreshold) { │ │ │ │ │ +432 detail->variableStatus[key].isRelinearizeInvolved = true; │ │ │ │ │ +433 detail->variableStatus[key].isRelinearized = true; │ │ │ │ │ +434 } │ │ │ │ │ +435 } │ │ │ │ │ +436 } │ │ │ │ │ +437 } │ │ │ │ │ +438 │ │ │ │ │ +439 // Linearize new factors │ │ │ │ │ +440 void linearizeNewFactors(const NonlinearFactorGraph& newFactors, │ │ │ │ │ +441 const _V_a_l_u_e_s& theta, size_t numNonlinearFactors, │ │ │ │ │ +442 const FactorIndices& newFactorsIndices, │ │ │ │ │ +443 GaussianFactorGraph* linearFactors) const { │ │ │ │ │ +444 gttic(linearizeNewFactors); │ │ │ │ │ +445 auto linearized = newFactors.linearize(theta); │ │ │ │ │ +446 if (params_.findUnusedFactorSlots) { │ │ │ │ │ +447 linearFactors->resize(numNonlinearFactors); │ │ │ │ │ +448 for (size_t i = 0; i < newFactors.size(); ++i) │ │ │ │ │ +449 (*linearFactors)[newFactorsIndices[i]] = (*linearized)[i]; │ │ │ │ │ +450 } else { │ │ │ │ │ +451 linearFactors->push_back(*linearized); │ │ │ │ │ +452 } │ │ │ │ │ +453 assert(linearFactors->size() == numNonlinearFactors); │ │ │ │ │ +454 } │ │ │ │ │ +455 │ │ │ │ │ +456 void augmentVariableIndex(const NonlinearFactorGraph& newFactors, │ │ │ │ │ +457 const FactorIndices& newFactorsIndices, │ │ │ │ │ +458 VariableIndex* variableIndex) const { │ │ │ │ │ +459 gttic(augmentVariableIndex); │ │ │ │ │ +460 // Augment the variable index with the new factors │ │ │ │ │ +461 if (params_.findUnusedFactorSlots) │ │ │ │ │ +462 variableIndex->augment(newFactors, newFactorsIndices); │ │ │ │ │ +463 else │ │ │ │ │ +464 variableIndex->augment(newFactors); │ │ │ │ │ +465 │ │ │ │ │ +466 // Augment it with existing factors which now affect to more variables: │ │ │ │ │ +467 if (updateParams_._n_e_w_A_f_f_e_c_t_e_d_K_e_y_s) { │ │ │ │ │ +468 for (const auto& factorAddedKeys : *updateParams_.newAffectedKeys) { │ │ │ │ │ +469 const auto factorIdx = factorAddedKeys.first; │ │ │ │ │ +470 variableIndex->augmentExistingFactor(factorIdx, factorAddedKeys.second); │ │ │ │ │ +471 } │ │ │ │ │ +472 } │ │ │ │ │ +473 } │ │ │ │ │ +474 │ │ │ │ │ +475 static void LogRecalculateKeys(const ISAM2Result& result) { │ │ │ │ │ +476 const bool debug = ISDEBUG("ISAM2 recalculate"); │ │ │ │ │ +477 │ │ │ │ │ +478 if (debug) { │ │ │ │ │ +479 std::cout << "markedKeys: "; │ │ │ │ │ +480 for (const Key key : result.markedKeys) { │ │ │ │ │ +481 std::cout << key << " "; │ │ │ │ │ +482 } │ │ │ │ │ +483 std::cout << std::endl; │ │ │ │ │ +484 std::cout << "observedKeys: "; │ │ │ │ │ +485 for (const Key key : result.observedKeys) { │ │ │ │ │ +486 std::cout << key << " "; │ │ │ │ │ +487 } │ │ │ │ │ +488 std::cout << std::endl; │ │ │ │ │ +489 } │ │ │ │ │ +490 } │ │ │ │ │ +491 │ │ │ │ │ +492 static FactorIndexSet GetAffectedFactors(const KeyList& keys, │ │ │ │ │ +493 const VariableIndex& variableIndex) { │ │ │ │ │ +494 gttic(GetAffectedFactors); │ │ │ │ │ +495 FactorIndexSet indices; │ │ │ │ │ +496 for (const Key key : keys) { │ │ │ │ │ +497 const _F_a_c_t_o_r_I_n_d_i_c_e_s& factors(variableIndex[key]); │ │ │ │ │ +498 indices.insert(factors.begin(), factors.end()); │ │ │ │ │ +499 } │ │ │ │ │ +500 return indices; │ │ │ │ │ +501 } │ │ │ │ │ +502 │ │ │ │ │ +503 // find intermediate (linearized) factors from cache that are passed into │ │ │ │ │ +504 // the affected area │ │ │ │ │ +505 static GaussianFactorGraph GetCachedBoundaryFactors( │ │ │ │ │ +506 const ISAM2::Cliques& orphans) { │ │ │ │ │ +507 GaussianFactorGraph cachedBoundary; │ │ │ │ │ +508 │ │ │ │ │ +509 for (const auto& orphan : orphans) { │ │ │ │ │ +510 // retrieve the cached factor and add to boundary │ │ │ │ │ +511 cachedBoundary.push_back(orphan->cachedFactor()); │ │ │ │ │ +512 } │ │ │ │ │ +513 │ │ │ │ │ +514 return cachedBoundary; │ │ │ │ │ +515 } │ │ │ │ │ +516}; │ │ │ │ │ +517 │ │ │ │ │ +518} // namespace gtsam │ │ │ │ │ +_d_e_b_u_g_._h │ │ │ │ │ +Global debugging flags. │ │ │ │ │ +_V_a_r_i_a_b_l_e_I_n_d_e_x_._h │ │ │ │ │ +_J_u_n_c_t_i_o_n_T_r_e_e_-_i_n_s_t_._h │ │ │ │ │ +The junction tree, template bodies. │ │ │ │ │ +_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ +Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ │ +_G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_._h │ │ │ │ │ +_I_S_A_M_2_R_e_s_u_l_t_._h │ │ │ │ │ +Class that stores detailed iSAM2 result. │ │ │ │ │ +_I_S_A_M_2_._h │ │ │ │ │ +Incremental update functionality (ISAM2) for BayesTree, with fluid │ │ │ │ │ +relinearization. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_I_n_d_i_c_e_s │ │ │ │ │ +FastVector< FactorIndex > FactorIndices │ │ │ │ │ +Define collection types: │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:34 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e │ │ │ │ │ -Wraps any type T so it can play as a Value. │ │ │ │ │ -DDeeffiinniittiioonn GenericValue.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_v_a_l_u_e │ │ │ │ │ -const T & value() const │ │ │ │ │ -Return a constant value. │ │ │ │ │ -DDeeffiinniittiioonn GenericValue.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e │ │ │ │ │ -This is the base class for any type to be stored in Values. │ │ │ │ │ -DDeeffiinniittiioonn Value.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:___V_a_l_u_e_s_K_e_y_V_a_l_u_e_P_a_i_r │ │ │ │ │ -DDeeffiinniittiioonn Values-inl.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:___V_a_l_u_e_s_K_e_y_V_a_l_u_e_P_a_i_r_:_:_k_e_y │ │ │ │ │ -const Key key │ │ │ │ │ -The key. │ │ │ │ │ -DDeeffiinniittiioonn Values-inl.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:___V_a_l_u_e_s_K_e_y_V_a_l_u_e_P_a_i_r_:_:_v_a_l_u_e │ │ │ │ │ -ValueType & value │ │ │ │ │ -The value. │ │ │ │ │ -DDeeffiinniittiioonn Values-inl.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:___V_a_l_u_e_s_C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r │ │ │ │ │ -DDeeffiinniittiioonn Values-inl.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:___V_a_l_u_e_s_C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r_:_:_k_e_y │ │ │ │ │ -const Key key │ │ │ │ │ -The key. │ │ │ │ │ -DDeeffiinniittiioonn Values-inl.h:46 │ │ │ │ │ -_g_t_s_a_m_:_:___V_a_l_u_e_s_C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r_:_:_v_a_l_u_e │ │ │ │ │ -const ValueType & value │ │ │ │ │ -The value. │ │ │ │ │ -DDeeffiinniittiioonn Values-inl.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_C_a_s_t_H_e_l_p_e_r │ │ │ │ │ -DDeeffiinniittiioonn Values-inl.h:62 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_h_a_n_d_l_e │ │ │ │ │ -DDeeffiinniittiioonn Values-inl.h:291 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_h_a_n_d_l_e___m_a_t_r_i_x │ │ │ │ │ -DDeeffiinniittiioonn Values-inl.h:304 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_u_p_d_a_t_e │ │ │ │ │ -void update(Key j, const Value &val) │ │ │ │ │ -single element change of existing element │ │ │ │ │ -DDeeffiinniittiioonn Values.cpp:180 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_e_x_t_r_a_c_t │ │ │ │ │ -std::map< Key, ValueType > extract(const std::function< bool(Key)> │ │ │ │ │ -&filterFcn=&_truePredicate< Key >) const │ │ │ │ │ -Extract a subset of values of the given type ValueType. │ │ │ │ │ -DDeeffiinniittiioonn Values-inl.h:94 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_a_t │ │ │ │ │ -const ValueType at(Key j) const │ │ │ │ │ -Retrieve a variable by key j. │ │ │ │ │ -DDeeffiinniittiioonn Values-inl.h:361 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_i_n_s_e_r_t │ │ │ │ │ -void insert(Key j, const Value &val) │ │ │ │ │ -Add a variable with the given j, throws KeyAlreadyExists if j is already │ │ │ │ │ -present. │ │ │ │ │ -DDeeffiinniittiioonn Values.cpp:157 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_i_n_s_e_r_t___o_r___a_s_s_i_g_n │ │ │ │ │ -void insert_or_assign(Key j, const Value &val) │ │ │ │ │ -If key j exists, update value, else perform an insert. │ │ │ │ │ -DDeeffiinniittiioonn Values.cpp:203 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_V_a_l_u_e_s │ │ │ │ │ -Values()=default │ │ │ │ │ -Default constructor creates an empty Values class. │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_e_x_i_s_t_s │ │ │ │ │ -bool exists(Key j) const │ │ │ │ │ -Check if a value exists with key j. │ │ │ │ │ -DDeeffiinniittiioonn Values.cpp:94 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ +FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ +fast_pool_allocator instead of the defa... │ │ │ │ │ +DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_k_e_y_s │ │ │ │ │ +KeySet keys() const │ │ │ │ │ +Potentially slow function to return all keys involved, sorted, as a set. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph-inst.h:85 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_a_d_d___f_a_c_t_o_r_s │ │ │ │ │ +FactorIndices add_factors(const CONTAINER &factors, bool useEmptySlots=false) │ │ │ │ │ +Add new factors to a factor graph and returns a list of new factor indices, │ │ │ │ │ +optionally finding and re... │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph-inst.h:109 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_p_u_s_h___b_a_c_k │ │ │ │ │ +IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor) │ │ │ │ │ +Add a factor directly using a shared_ptr. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:186 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_r_e_m_o_v_e │ │ │ │ │ +void remove(size_t i) │ │ │ │ │ +delete factor without re-arranging indexes by inserting a nullptr pointer │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:385 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_a_t │ │ │ │ │ +const sharedFactor at(size_t i) const │ │ │ │ │ +Get a specific factor by index (this checks array bounds and may throw an │ │ │ │ │ +exception,... │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:335 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_r_e_s_e_r_v_e │ │ │ │ │ +void reserve(size_t size) │ │ │ │ │ +Reserve space for the specified number of factors if you know in advance how │ │ │ │ │ +many there will be (work... │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:182 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_<_ _I_S_A_M_2_C_l_i_q_u_e_ _> │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn BayesTree-inst.h:212 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_<_ _I_S_A_M_2_C_l_i_q_u_e_ _>_:_:_R_o_o_t_s │ │ │ │ │ +FastVector< sharedClique > Roots │ │ │ │ │ +Root cliques. │ │ │ │ │ +DDeeffiinniittiioonn BayesTree.h:95 │ │ │ │ │ +_g_t_s_a_m_:_:_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ +A JunctionTree is a cluster tree, a set of variable clusters with factors, │ │ │ │ │ +arranged in a tree,... │ │ │ │ │ +DDeeffiinniittiioonn JunctionTree.h:50 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l │ │ │ │ │ +Character and index key used to refer to variables. │ │ │ │ │ +DDeeffiinniittiioonn Symbol.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_:_:_c_h_r │ │ │ │ │ +unsigned char chr() const │ │ │ │ │ +Retrieve key character. │ │ │ │ │ +DDeeffiinniittiioonn Symbol.h:73 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ +The VariableIndex class computes and stores the block column structure of a │ │ │ │ │ +factor graph. │ │ │ │ │ +DDeeffiinniittiioonn VariableIndex.h:43 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_r_e_m_o_v_e │ │ │ │ │ +void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG &factors) │ │ │ │ │ +Remove entries corresponding to the specified factors. │ │ │ │ │ +DDeeffiinniittiioonn VariableIndex-inl.h:54 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x_:_:_e_m_p_t_y │ │ │ │ │ +bool empty(Key variable) const │ │ │ │ │ +Return true if no factors associated with a variable. │ │ │ │ │ +DDeeffiinniittiioonn VariableIndex.h:98 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ +DDeeffiinniittiioonn GaussianEliminationTree.h:29 │ │ │ │ │ +_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ +DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ +with a unique integer... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_B_a_y_e_s_T_r_e_e │ │ │ │ │ +DDeeffiinniittiioonn ISAM2-impl.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ +DDeeffiinniittiioonn ISAM2-impl.h:61 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_l_t_a_I_m_p_l │ │ │ │ │ +DDeeffiinniittiioonn ISAM2-impl.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_l_t_a_I_m_p_l_:_:_P_a_r_t_i_a_l_S_o_l_v_e_R_e_s_u_l_t │ │ │ │ │ +DDeeffiinniittiioonn ISAM2-impl.h:73 │ │ │ │ │ +_g_t_s_a_m_:_:_D_e_l_t_a_I_m_p_l_:_:_R_e_o_r_d_e_r_i_n_g_M_o_d_e │ │ │ │ │ +DDeeffiinniittiioonn ISAM2-impl.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_U_p_d_a_t_e_I_m_p_l │ │ │ │ │ +Implementation functions for update method All of the methods below have clear │ │ │ │ │ +inputs and outputs,... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2-impl.h:114 │ │ │ │ │ +_g_t_s_a_m_:_:_U_p_d_a_t_e_I_m_p_l_:_:_C_h_e_c_k_R_e_l_i_n_e_a_r_i_z_a_t_i_o_n_F_u_l_l │ │ │ │ │ +static KeySet CheckRelinearizationFull(const VectorValues &delta, const │ │ │ │ │ +ISAM2Params::RelinearizationThreshold &relinearizeThreshold) │ │ │ │ │ +Find the set of variables to be relinearized according to relinearizeThreshold. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2-impl.h:345 │ │ │ │ │ +_g_t_s_a_m_:_:_U_p_d_a_t_e_I_m_p_l_:_:_C_h_e_c_k_R_e_l_i_n_e_a_r_i_z_a_t_i_o_n_P_a_r_t_i_a_l │ │ │ │ │ +static KeySet CheckRelinearizationPartial(const ISAM2::Roots &roots, const │ │ │ │ │ +VectorValues &delta, const ISAM2Params::RelinearizationThreshold │ │ │ │ │ +&relinearizeThreshold) │ │ │ │ │ +Find the set of variables to be relinearized according to relinearizeThreshold. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2-impl.h:318 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2 │ │ │ │ │ +Implementation of the full ISAM2 algorithm for incremental nonlinear │ │ │ │ │ +optimization. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_:_:_s_h_a_r_e_d_C_l_i_q_u_e │ │ │ │ │ +Base::sharedClique sharedClique │ │ │ │ │ +Shared pointer to a clique. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2.h:103 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:135 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s_:_:_R_e_l_i_n_e_a_r_i_z_a_t_i_o_n_T_h_r_e_s_h_o_l_d │ │ │ │ │ +boost::variant< double, FastMap< char, Vector > > RelinearizationThreshold │ │ │ │ │ +Either a constant relinearization threshold or a per-variable-type set of │ │ │ │ │ +thresholds. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Params.h:140 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t │ │ │ │ │ +This struct is returned from ISAM2::update() and contains information about the │ │ │ │ │ +update that is useful... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Result.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s │ │ │ │ │ +This struct is used by ISAM2::update() to pass additional parameters to give │ │ │ │ │ +the user a fine-grained ... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2UpdateParams.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_:_:_f_o_r_c_e___r_e_l_i_n_e_a_r_i_z_e │ │ │ │ │ +bool force_relinearize │ │ │ │ │ +Relinearize any variables whose delta magnitude is sufficiently large (Params:: │ │ │ │ │ +relinearizeThreshold),... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2UpdateParams.h:54 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_:_:_r_e_m_o_v_e_F_a_c_t_o_r_I_n_d_i_c_e_s │ │ │ │ │ +FactorIndices removeFactorIndices │ │ │ │ │ +Indices of factors to remove from system (default: empty) │ │ │ │ │ +DDeeffiinniittiioonn ISAM2UpdateParams.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_:_:_f_o_r_c_e_F_u_l_l_S_o_l_v_e │ │ │ │ │ +bool forceFullSolve │ │ │ │ │ +By default, iSAM2 uses a wildfire update scheme that stops updating when the │ │ │ │ │ +deltas become too small ... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2UpdateParams.h:71 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_:_:_e_x_t_r_a_R_e_e_l_i_m_K_e_y_s │ │ │ │ │ +boost::optional< FastList< Key > > extraReelimKeys │ │ │ │ │ +An optional set of nonlinear keys that iSAM2 will re-eliminate, regardless of │ │ │ │ │ +the size of the linear ... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2UpdateParams.h:49 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_:_:_n_o_R_e_l_i_n_K_e_y_s │ │ │ │ │ +boost::optional< FastList< Key > > noRelinKeys │ │ │ │ │ +An optional set of nonlinear keys that iSAM2 will hold at a constant │ │ │ │ │ +linearization point,... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2UpdateParams.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_:_:_n_e_w_A_f_f_e_c_t_e_d_K_e_y_s │ │ │ │ │ +boost::optional< FastMap< FactorIndex, KeySet > > newAffectedKeys │ │ │ │ │ +An optional set of new Keys that are now affected by factors, indexed by factor │ │ │ │ │ +indices (as returned ... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2UpdateParams.h:66 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &str="NonlinearFactorGraph: ", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactorGraph.cpp:55 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_:_:_e_r_r_o_r │ │ │ │ │ +double error(const Values &values) const │ │ │ │ │ +unnormalized error, in the most common case │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactorGraph.cpp:170 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ _g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_K_e_y_V_a_l_u_e_P_a_i_r │ │ │ │ │ A key-value pair, which you get by dereferencing iterators. │ │ │ │ │ DDeeffiinniittiioonn Values.h:93 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_C_o_n_s_t_K_e_y_V_a_l_u_e_P_a_i_r │ │ │ │ │ -A key-value pair, which you get by dereferencing iterators. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:101 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_K_e_y_D_o_e_s_N_o_t_E_x_i_s_t │ │ │ │ │ -DDeeffiinniittiioonn Values.h:475 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_I_n_c_o_r_r_e_c_t_T_y_p_e │ │ │ │ │ -DDeeffiinniittiioonn Values.h:498 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_M_a_t_c_h_F_o_u_n_d_F_o_r_F_i_x_e_d │ │ │ │ │ -DDeeffiinniittiioonn Values.h:542 │ │ │ │ │ _V_a_l_u_e_s │ │ │ │ │ In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ a non-linear function... │ │ │ │ │ +_S_y_m_b_o_l_._h │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * VVaalluueess--iinnll..hh │ │ │ │ │ + * _I_S_A_M_2_-_i_m_p_l_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01109_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01034_source.html │ │ │ │┄ Files 90% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearEquality.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/Expression.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
NonlinearEquality.h
│ │ │ │ +
Expression.h
│ │ │ │
│ │ │ │
│ │ │ │ -
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
12/*
│ │ │ │ -
13 * @file NonlinearEquality.h
│ │ │ │ -
14 * @brief Factor to handle enforced equality between factors
│ │ │ │ -
15 * @author Alex Cunningham
│ │ │ │ -
16 */
│ │ │ │ -
17
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
21#include <gtsam/base/Testable.h>
│ │ │ │ -
22#include <gtsam/base/Manifold.h>
│ │ │ │ -
23
│ │ │ │ -
24#include <boost/bind/bind.hpp>
│ │ │ │ -
25
│ │ │ │ -
26#include <limits>
│ │ │ │ -
27#include <iostream>
│ │ │ │ -
28#include <cmath>
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/nonlinear/internal/JacobianMap.h>
│ │ │ │ +
23#include <gtsam/inference/Symbol.h>
│ │ │ │ +
24#include <gtsam/base/OptionalJacobian.h>
│ │ │ │ +
25#include <gtsam/base/VectorSpace.h>
│ │ │ │ +
26
│ │ │ │ +
27#include <boost/make_shared.hpp>
│ │ │ │ +
28#include <map>
│ │ │ │
29
│ │ │ │ -
30namespace gtsam {
│ │ │ │ -
31
│ │ │ │ -
44template<class VALUE>
│ │ │ │ -
│ │ │ │ -
45class NonlinearEquality: public NoiseModelFactorN<VALUE> {
│ │ │ │ -
46
│ │ │ │ -
47public:
│ │ │ │ -
48 typedef VALUE T;
│ │ │ │ +
30// Forward declare tests
│ │ │ │ +
31class ExpressionFactorShallowTest;
│ │ │ │ +
32
│ │ │ │ +
33namespace gtsam {
│ │ │ │ +
34
│ │ │ │ +
35// Forward declares
│ │ │ │ +
36class Values;
│ │ │ │ +
37template<typename T> class ExpressionFactor;
│ │ │ │ +
38
│ │ │ │ +
39namespace internal {
│ │ │ │ +
40template<typename T> class ExecutionTrace;
│ │ │ │ +
41template<typename T> class ExpressionNode;
│ │ │ │ +
42}
│ │ │ │ +
43
│ │ │ │ +
47template<typename T>
│ │ │ │ +
│ │ │ │ +
48class Expression {
│ │ │ │
49
│ │ │ │ -
50private:
│ │ │ │ +
50public:
│ │ │ │
51
│ │ │ │ -
52 // feasible value
│ │ │ │ -
53 T feasible_;
│ │ │ │ +
53 typedef Expression<T> type;
│ │ │ │
54
│ │ │ │ -
55 // error handling flag
│ │ │ │ -
56 bool allow_error_;
│ │ │ │ -
57
│ │ │ │ -
58 // error gain in allow error case
│ │ │ │ -
59 double error_gain_;
│ │ │ │ -
60
│ │ │ │ -
61 // typedef to this class
│ │ │ │ -
62 using This = NonlinearEquality<VALUE>;
│ │ │ │ -
63
│ │ │ │ -
64 // typedef to base class
│ │ │ │ -
65 using Base = NoiseModelFactorN<VALUE>;
│ │ │ │ -
66
│ │ │ │ -
67public:
│ │ │ │ -
68
│ │ │ │ -
70 using CompareFunction = std::function<bool(const T&, const T&)>;
│ │ │ │ -
71 CompareFunction compare_;
│ │ │ │ -
72
│ │ │ │ -
│ │ │ │ -
74 NonlinearEquality() {
│ │ │ │ -
75 }
│ │ │ │ -
│ │ │ │ -
76
│ │ │ │ -
77 ~NonlinearEquality() override {
│ │ │ │ -
78 }
│ │ │ │ -
79
│ │ │ │ -
82
│ │ │ │ -
│ │ │ │ -
86 NonlinearEquality(Key j, const T& feasible,
│ │ │ │ -
87 const CompareFunction &_compare = std::bind(traits<T>::Equals,
│ │ │ │ -
88 std::placeholders::_1, std::placeholders::_2, 1e-9)) :
│ │ │ │ -
89 Base(noiseModel::Constrained::All(traits<T>::GetDimension(feasible)),
│ │ │ │ -
90 j), feasible_(feasible), allow_error_(false), error_gain_(0.0), //
│ │ │ │ -
91 compare_(_compare) {
│ │ │ │ -
92 }
│ │ │ │ +
55protected:
│ │ │ │ +
56
│ │ │ │ +
57 // Paul's trick shared pointer, polymorphic root of entire expression tree
│ │ │ │ +
58 boost::shared_ptr<internal::ExpressionNode<T> > root_;
│ │ │ │ +
59
│ │ │ │ +
61 Expression(const boost::shared_ptr<internal::ExpressionNode<T> >& root) : root_(root) {}
│ │ │ │ +
62
│ │ │ │ +
63public:
│ │ │ │ +
64
│ │ │ │ +
65 // Expressions wrap trees of functions that can evaluate their own derivatives.
│ │ │ │ +
66 // The meta-functions below are useful to specify the type of those functions.
│ │ │ │ +
67 // Example, a function taking a camera and a 3D point and yielding a 2D point:
│ │ │ │ +
68 // Expression<Point2>::BinaryFunction<PinholeCamera<Cal3_S2>,Point3>::type
│ │ │ │ +
69 template<class A1>
│ │ │ │ +
│ │ │ │ +
70 struct UnaryFunction {
│ │ │ │ +
71 typedef std::function<
│ │ │ │ +
72 T(const A1&, typename MakeOptionalJacobian<T, A1>::type)> type;
│ │ │ │ +
73 };
│ │ │ │ +
│ │ │ │ +
74
│ │ │ │ +
75 template<class A1, class A2>
│ │ │ │ +
│ │ │ │ +
76 struct BinaryFunction {
│ │ │ │ +
77 typedef std::function<
│ │ │ │ +
78 T(const A1&, const A2&, typename MakeOptionalJacobian<T, A1>::type,
│ │ │ │ +
79 typename MakeOptionalJacobian<T, A2>::type)> type;
│ │ │ │ +
80 };
│ │ │ │ +
│ │ │ │ +
81
│ │ │ │ +
82 template<class A1, class A2, class A3>
│ │ │ │ +
│ │ │ │ +
83 struct TernaryFunction {
│ │ │ │ +
84 typedef std::function<
│ │ │ │ +
85 T(const A1&, const A2&, const A3&,
│ │ │ │ +
86 typename MakeOptionalJacobian<T, A1>::type,
│ │ │ │ +
87 typename MakeOptionalJacobian<T, A2>::type,
│ │ │ │ +
88 typename MakeOptionalJacobian<T, A3>::type)> type;
│ │ │ │ +
89 };
│ │ │ │
│ │ │ │ +
90
│ │ │ │ +
92 Expression(const T& value);
│ │ │ │
93
│ │ │ │ -
│ │ │ │ -
97 NonlinearEquality(Key j, const T& feasible, double error_gain,
│ │ │ │ -
98 const CompareFunction &_compare = std::bind(traits<T>::Equals,
│ │ │ │ -
99 std::placeholders::_1, std::placeholders::_2, 1e-9)) :
│ │ │ │ -
100 Base(noiseModel::Constrained::All(traits<T>::GetDimension(feasible)),
│ │ │ │ -
101 j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), //
│ │ │ │ -
102 compare_(_compare) {
│ │ │ │ -
103 }
│ │ │ │ -
│ │ │ │ -
104
│ │ │ │ -
108
│ │ │ │ -
│ │ │ │ -
109 void print(const std::string& s = "",
│ │ │ │ -
110 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ -
111 std::cout << (s.empty() ? s : s + " ") << "Constraint: on ["
│ │ │ │ -
112 << keyFormatter(this->key()) << "]\n";
│ │ │ │ -
113 traits<VALUE>::Print(feasible_, "Feasible Point:\n");
│ │ │ │ -
114 std::cout << "Variable Dimension: " << traits<T>::GetDimension(feasible_)
│ │ │ │ -
115 << std::endl;
│ │ │ │ -
116 }
│ │ │ │ -
│ │ │ │ -
117
│ │ │ │ -
│ │ │ │ -
119 bool equals(const NonlinearFactor& f, double tol = 1e-9) const override {
│ │ │ │ -
120 const This* e = dynamic_cast<const This*>(&f);
│ │ │ │ -
121 return e && Base::equals(f) && traits<T>::Equals(feasible_,e->feasible_, tol)
│ │ │ │ -
122 && std::abs(error_gain_ - e->error_gain_) < tol;
│ │ │ │ -
123 }
│ │ │ │ -
│ │ │ │ -
124
│ │ │ │ -
128
│ │ │ │ -
│ │ │ │ -
130 double error(const Values& c) const override {
│ │ │ │ -
131 const T& xj = c.at<T>(this->key());
│ │ │ │ -
132 Vector e = this->unwhitenedError(c);
│ │ │ │ -
133 if (allow_error_ || !compare_(xj, feasible_)) {
│ │ │ │ -
134 return error_gain_ * dot(e, e);
│ │ │ │ -
135 } else {
│ │ │ │ -
136 return 0.0;
│ │ │ │ -
137 }
│ │ │ │ -
138 }
│ │ │ │ -
│ │ │ │ +
95 Expression(const Key& key);
│ │ │ │ +
96
│ │ │ │ +
98 Expression(const Symbol& symbol);
│ │ │ │ +
99
│ │ │ │ +
101 Expression(unsigned char c, std::uint64_t j);
│ │ │ │ +
102
│ │ │ │ +
104 template<typename A>
│ │ │ │ +
105 Expression(typename UnaryFunction<A>::type function,
│ │ │ │ +
106 const Expression<A>& expression);
│ │ │ │ +
107
│ │ │ │ +
109 template<typename A1, typename A2>
│ │ │ │ +
110 Expression(typename BinaryFunction<A1, A2>::type function,
│ │ │ │ +
111 const Expression<A1>& expression1, const Expression<A2>& expression2);
│ │ │ │ +
112
│ │ │ │ +
114 template<typename A1, typename A2, typename A3>
│ │ │ │ +
115 Expression(typename TernaryFunction<A1, A2, A3>::type function,
│ │ │ │ +
116 const Expression<A1>& expression1, const Expression<A2>& expression2,
│ │ │ │ +
117 const Expression<A3>& expression3);
│ │ │ │ +
118
│ │ │ │ +
120 template<typename A>
│ │ │ │ +
121 Expression(const Expression<A>& expression,
│ │ │ │ +
122 T (A::*method)(typename MakeOptionalJacobian<T, A>::type) const);
│ │ │ │ +
123
│ │ │ │ +
125 template<typename A1, typename A2>
│ │ │ │ +
126 Expression(const Expression<A1>& expression1,
│ │ │ │ +
127 T (A1::*method)(const A2&, typename MakeOptionalJacobian<T, A1>::type,
│ │ │ │ +
128 typename MakeOptionalJacobian<T, A2>::type) const,
│ │ │ │ +
129 const Expression<A2>& expression2);
│ │ │ │ +
130
│ │ │ │ +
132 template<typename A1, typename A2, typename A3>
│ │ │ │ +
133 Expression(const Expression<A1>& expression1,
│ │ │ │ +
134 T (A1::*method)(const A2&, const A3&,
│ │ │ │ +
135 typename MakeOptionalJacobian<T, A1>::type,
│ │ │ │ +
136 typename MakeOptionalJacobian<T, A2>::type,
│ │ │ │ +
137 typename MakeOptionalJacobian<T, A3>::type) const,
│ │ │ │ +
138 const Expression<A2>& expression2, const Expression<A3>& expression3);
│ │ │ │
139
│ │ │ │
│ │ │ │ -
141 Vector evaluateError(const T& xj,
│ │ │ │ -
142 boost::optional<Matrix&> H = boost::none) const override {
│ │ │ │ -
143 const size_t nj = traits<T>::GetDimension(feasible_);
│ │ │ │ -
144 if (allow_error_) {
│ │ │ │ -
145 if (H)
│ │ │ │ -
146 *H = Matrix::Identity(nj,nj); // FIXME: this is not the right linearization for nonlinear compare
│ │ │ │ -
147 return traits<T>::Local(xj,feasible_);
│ │ │ │ -
148 } else if (compare_(feasible_, xj)) {
│ │ │ │ -
149 if (H)
│ │ │ │ -
150 *H = Matrix::Identity(nj,nj);
│ │ │ │ -
151 return Vector::Zero(nj); // set error to zero if equal
│ │ │ │ -
152 } else {
│ │ │ │ -
153 if (H)
│ │ │ │ -
154 throw std::invalid_argument(
│ │ │ │ -
155 "Linearization point not feasible for "
│ │ │ │ -
156 + DefaultKeyFormatter(this->key()) + "!");
│ │ │ │ -
157 return Vector::Constant(nj, std::numeric_limits<double>::infinity()); // set error to infinity if not equal
│ │ │ │ -
158 }
│ │ │ │ -
159 }
│ │ │ │ +
141 virtual ~Expression() {
│ │ │ │ +
142 }
│ │ │ │
│ │ │ │ +
143
│ │ │ │ +
145 std::set<Key> keys() const;
│ │ │ │ +
146
│ │ │ │ +
148 void dims(std::map<Key, int>& map) const;
│ │ │ │ +
149
│ │ │ │ +
151 void print(const std::string& s) const;
│ │ │ │ +
152
│ │ │ │ +
158 T value(const Values& values, boost::optional<std::vector<Matrix>&> H =
│ │ │ │ +
159 boost::none) const;
│ │ │ │
160
│ │ │ │ -
│ │ │ │ -
162 GaussianFactor::shared_ptr linearize(const Values& x) const override {
│ │ │ │ -
163 const T& xj = x.at<T>(this->key());
│ │ │ │ -
164 Matrix A;
│ │ │ │ -
165 Vector b = evaluateError(xj, A);
│ │ │ │ -
166 SharedDiagonal model = noiseModel::Constrained::All(b.size());
│ │ │ │ -
167 return GaussianFactor::shared_ptr(
│ │ │ │ -
168 new JacobianFactor(this->key(), A, b, model));
│ │ │ │ -
169 }
│ │ │ │ -
│ │ │ │ -
170
│ │ │ │ -
│ │ │ │ -
172 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
173 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ -
174 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
│ │ │ │ -
175 }
│ │ │ │ -
│ │ │ │ -
176
│ │ │ │ +
│ │ │ │ +
166 virtual boost::shared_ptr<Expression> clone() const {
│ │ │ │ +
167 return boost::make_shared<Expression>(*this);
│ │ │ │ +
168 }
│ │ │ │ +
│ │ │ │ +
169
│ │ │ │ +
171 const boost::shared_ptr<internal::ExpressionNode<T> >& root() const;
│ │ │ │ +
172
│ │ │ │ +
174 size_t traceSize() const;
│ │ │ │ +
175
│ │ │ │ +
177 Expression<T>& operator+=(const Expression<T>& e);
│ │ │ │
178
│ │ │ │ -
179 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
179protected:
│ │ │ │
180
│ │ │ │ -
181private:
│ │ │ │ -
182
│ │ │ │ -
184 friend class boost::serialization::access;
│ │ │ │ -
185 template<class ARCHIVE>
│ │ │ │ -
186 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
187 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ -
188 ar
│ │ │ │ -
189 & boost::serialization::make_nvp("NoiseModelFactor1",
│ │ │ │ -
190 boost::serialization::base_object<Base>(*this));
│ │ │ │ -
191 ar & BOOST_SERIALIZATION_NVP(feasible_);
│ │ │ │ -
192 ar & BOOST_SERIALIZATION_NVP(allow_error_);
│ │ │ │ -
193 ar & BOOST_SERIALIZATION_NVP(error_gain_);
│ │ │ │ -
194 }
│ │ │ │ +
182 Expression() {}
│ │ │ │ +
183
│ │ │ │ +
185 typedef std::pair<KeyVector, FastVector<int> > KeysAndDims;
│ │ │ │ +
186 KeysAndDims keysAndDims() const;
│ │ │ │ +
187
│ │ │ │ +
189 T valueAndDerivatives(const Values& values, const KeyVector& keys,
│ │ │ │ +
190 const FastVector<int>& dims, std::vector<Matrix>& H) const;
│ │ │ │ +
191
│ │ │ │ +
193 T traceExecution(const Values& values, internal::ExecutionTrace<T>& trace,
│ │ │ │ +
194 void* traceStorage) const;
│ │ │ │
195
│ │ │ │ -
196};
│ │ │ │ -
│ │ │ │ -
197// \class NonlinearEquality
│ │ │ │ -
198
│ │ │ │ -
199template <typename VALUE>
│ │ │ │ -
200struct traits<NonlinearEquality<VALUE>> : Testable<NonlinearEquality<VALUE>> {};
│ │ │ │ -
201
│ │ │ │ -
202/* ************************************************************************* */
│ │ │ │ -
206template<class VALUE>
│ │ │ │ -
│ │ │ │ -
207class NonlinearEquality1: public NoiseModelFactorN<VALUE> {
│ │ │ │ -
208
│ │ │ │ -
209public:
│ │ │ │ -
210 typedef VALUE X;
│ │ │ │ -
211
│ │ │ │ -
212protected:
│ │ │ │ -
213 typedef NoiseModelFactorN<VALUE> Base;
│ │ │ │ -
214 typedef NonlinearEquality1<VALUE> This;
│ │ │ │ -
215
│ │ │ │ -
│ │ │ │ -
217 NonlinearEquality1() {
│ │ │ │ -
218 }
│ │ │ │ -
│ │ │ │ -
219
│ │ │ │ -
220 X value_;
│ │ │ │ -
221
│ │ │ │ -
222 GTSAM_CONCEPT_MANIFOLD_TYPE(X)
│ │ │ │ -
223 GTSAM_CONCEPT_TESTABLE_TYPE(X)
│ │ │ │ -
224
│ │ │ │ -
225public:
│ │ │ │ -
226
│ │ │ │ -
227 typedef boost::shared_ptr<NonlinearEquality1<VALUE> > shared_ptr;
│ │ │ │ -
228
│ │ │ │ -
│ │ │ │ -
235 NonlinearEquality1(const X& value, Key key, double mu = 1000.0)
│ │ │ │ -
236 : Base(noiseModel::Constrained::All(traits<X>::GetDimension(value),
│ │ │ │ -
237 std::abs(mu)),
│ │ │ │ -
238 key),
│ │ │ │ -
239 value_(value) {}
│ │ │ │ -
│ │ │ │ -
240
│ │ │ │ -
241 ~NonlinearEquality1() override {
│ │ │ │ -
242 }
│ │ │ │ -
243
│ │ │ │ -
│ │ │ │ -
245 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
246 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ -
247 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
│ │ │ │ -
248 }
│ │ │ │ -
│ │ │ │ -
249
│ │ │ │ -
│ │ │ │ -
251 Vector evaluateError(const X& x1,
│ │ │ │ -
252 boost::optional<Matrix&> H = boost::none) const override {
│ │ │ │ -
253 if (H)
│ │ │ │ -
254 (*H) = Matrix::Identity(traits<X>::GetDimension(x1),traits<X>::GetDimension(x1));
│ │ │ │ -
255 // manifold equivalent of h(x)-z -> log(z,h(x))
│ │ │ │ -
256 return traits<X>::Local(value_,x1);
│ │ │ │ -
257 }
│ │ │ │ -
│ │ │ │ -
258
│ │ │ │ -
│ │ │ │ -
260 void print(const std::string& s = "",
│ │ │ │ -
261 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ -
262 std::cout << s << ": NonlinearEquality1(" << keyFormatter(this->key())
│ │ │ │ -
263 << ")," << "\n";
│ │ │ │ -
264 this->noiseModel_->print();
│ │ │ │ -
265 traits<X>::Print(value_, "Value");
│ │ │ │ -
266 }
│ │ │ │ -
│ │ │ │ -
267
│ │ │ │ -
268 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
269
│ │ │ │ -
270private:
│ │ │ │ -
271
│ │ │ │ -
273 friend class boost::serialization::access;
│ │ │ │ -
274 template<class ARCHIVE>
│ │ │ │ -
275 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
276 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ -
277 ar
│ │ │ │ -
278 & boost::serialization::make_nvp("NoiseModelFactor1",
│ │ │ │ -
279 boost::serialization::base_object<Base>(*this));
│ │ │ │ -
280 ar & BOOST_SERIALIZATION_NVP(value_);
│ │ │ │ -
281 }
│ │ │ │ -
282};
│ │ │ │ -
│ │ │ │ -
283// \NonlinearEquality1
│ │ │ │ -
284
│ │ │ │ -
285template <typename VALUE>
│ │ │ │ -
│ │ │ │ -
286struct traits<NonlinearEquality1<VALUE> >
│ │ │ │ -
287 : Testable<NonlinearEquality1<VALUE> > {};
│ │ │ │ -
│ │ │ │ -
288
│ │ │ │ -
289/* ************************************************************************* */
│ │ │ │ -
294template <class T>
│ │ │ │ -
│ │ │ │ -
295class NonlinearEquality2 : public NoiseModelFactorN<T, T> {
│ │ │ │ -
296 protected:
│ │ │ │ -
297 using Base = NoiseModelFactorN<T, T>;
│ │ │ │ -
298 using This = NonlinearEquality2<T>;
│ │ │ │ -
299
│ │ │ │ -
300 GTSAM_CONCEPT_MANIFOLD_TYPE(T)
│ │ │ │ -
301
│ │ │ │ -
302
│ │ │ │ -
303 NonlinearEquality2() {}
│ │ │ │ -
304
│ │ │ │ -
305 public:
│ │ │ │ -
306 typedef boost::shared_ptr<NonlinearEquality2<T>> shared_ptr;
│ │ │ │ -
307
│ │ │ │ -
│ │ │ │ -
314 NonlinearEquality2(Key key1, Key key2, double mu = 1e4)
│ │ │ │ -
315 : Base(noiseModel::Constrained::All(traits<T>::dimension, std::abs(mu)),
│ │ │ │ -
316 key1, key2) {}
│ │ │ │ -
│ │ │ │ -
317 ~NonlinearEquality2() override {}
│ │ │ │ -
318
│ │ │ │ -
│ │ │ │ -
320 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
321 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ -
322 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
│ │ │ │ -
323 }
│ │ │ │ -
│ │ │ │ -
324
│ │ │ │ -
│ │ │ │ -
326 Vector evaluateError(
│ │ │ │ -
327 const T& x1, const T& x2, boost::optional<Matrix&> H1 = boost::none,
│ │ │ │ -
328 boost::optional<Matrix&> H2 = boost::none) const override {
│ │ │ │ -
329 static const size_t p = traits<T>::dimension;
│ │ │ │ -
330 if (H1) *H1 = -Matrix::Identity(p, p);
│ │ │ │ -
331 if (H2) *H2 = Matrix::Identity(p, p);
│ │ │ │ -
332 return traits<T>::Local(x1, x2);
│ │ │ │ -
333 }
│ │ │ │ -
│ │ │ │ -
334
│ │ │ │ -
335 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ -
336
│ │ │ │ -
337 private:
│ │ │ │ -
339 friend class boost::serialization::access;
│ │ │ │ -
340 template <class ARCHIVE>
│ │ │ │ -
341 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ -
342 // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ -
343 ar& boost::serialization::make_nvp(
│ │ │ │ -
344 "NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
│ │ │ │ -
345 }
│ │ │ │ -
346};
│ │ │ │ -
│ │ │ │ -
347// \NonlinearEquality2
│ │ │ │ -
348
│ │ │ │ -
349template <typename VALUE>
│ │ │ │ -
│ │ │ │ -
350struct traits<NonlinearEquality2<VALUE>> : Testable<NonlinearEquality2<VALUE>> {
│ │ │ │ -
351};
│ │ │ │ -
│ │ │ │ -
352
│ │ │ │ -
353}// namespace gtsam
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ -
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ -
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
197 T valueAndJacobianMap(const Values& values,
│ │ │ │ +
198 internal::JacobianMap& jacobians) const;
│ │ │ │ +
199
│ │ │ │ +
200 // be very selective on who can access these private methods:
│ │ │ │ +
201 friend class ExpressionFactor<T> ;
│ │ │ │ +
202 friend class internal::ExpressionNode<T>;
│ │ │ │ +
203
│ │ │ │ +
204 // and add tests
│ │ │ │ +
205 friend class ::ExpressionFactorShallowTest;
│ │ │ │ +
206};
│ │ │ │ +
│ │ │ │ +
207
│ │ │ │ +
212template <typename T>
│ │ │ │ +
│ │ │ │ +
213class ScalarMultiplyExpression : public Expression<T> {
│ │ │ │ +
214 // Check that T is a vector space
│ │ │ │ +
215 BOOST_CONCEPT_ASSERT((gtsam::IsVectorSpace<T>));
│ │ │ │ +
216
│ │ │ │ +
217 public:
│ │ │ │ +
218 explicit ScalarMultiplyExpression(double s, const Expression<T>& e);
│ │ │ │ +
219};
│ │ │ │ +
│ │ │ │ +
220
│ │ │ │ +
225template <typename T>
│ │ │ │ +
│ │ │ │ +
226class BinarySumExpression : public Expression<T> {
│ │ │ │ +
227 // Check that T is a vector space
│ │ │ │ +
228 BOOST_CONCEPT_ASSERT((gtsam::IsVectorSpace<T>));
│ │ │ │ +
229
│ │ │ │ +
230 public:
│ │ │ │ +
231 explicit BinarySumExpression(const Expression<T>& e1, const Expression<T>& e2);
│ │ │ │ +
232};
│ │ │ │ +
│ │ │ │ +
233
│ │ │ │ +
234
│ │ │ │ +
240template <typename T, typename A>
│ │ │ │ +
│ │ │ │ +
241Expression<T> linearExpression(
│ │ │ │ +
242 const std::function<T(A)>& f, const Expression<A>& expression,
│ │ │ │ +
243 const Eigen::Matrix<double, traits<T>::dimension, traits<A>::dimension>& dTdA) {
│ │ │ │ +
244 // Use lambda to endow f with a linear Jacobian
│ │ │ │ +
245 typename Expression<T>::template UnaryFunction<A>::type g =
│ │ │ │ +
246 [=](const A& value, typename MakeOptionalJacobian<T, A>::type H) {
│ │ │ │ +
247 if (H)
│ │ │ │ +
248 *H << dTdA;
│ │ │ │ +
249 return f(value);
│ │ │ │ +
250 };
│ │ │ │ +
251 return Expression<T>(g, expression);
│ │ │ │ +
252}
│ │ │ │ +
│ │ │ │ +
253
│ │ │ │ +
260template <typename T>
│ │ │ │ +
│ │ │ │ +
261ScalarMultiplyExpression<T> operator*(double s, const Expression<T>& e) {
│ │ │ │ +
262 return ScalarMultiplyExpression<T>(s, e);
│ │ │ │ +
263}
│ │ │ │ +
│ │ │ │ +
264
│ │ │ │ +
271template <typename T>
│ │ │ │ +
│ │ │ │ +
272BinarySumExpression<T> operator+(const Expression<T>& e1, const Expression<T>& e2) {
│ │ │ │ +
273 return BinarySumExpression<T>(e1, e2);
│ │ │ │ +
274}
│ │ │ │ +
│ │ │ │ +
275
│ │ │ │ +
277template <typename T>
│ │ │ │ +
│ │ │ │ +
278BinarySumExpression<T> operator-(const Expression<T>& e1, const Expression<T>& e2) {
│ │ │ │ +
279 // TODO(frank, abe): Implement an actual negate operator instead of multiplying by -1
│ │ │ │ +
280 return e1 + (-1.0) * e2;
│ │ │ │ +
281}
│ │ │ │ +
│ │ │ │ +
282
│ │ │ │ +
288template<typename T>
│ │ │ │ +
289Expression<T> operator*(const Expression<T>& e1, const Expression<T>& e2);
│ │ │ │ +
290
│ │ │ │ +
296template<typename T>
│ │ │ │ +
297std::vector<Expression<T> > createUnknowns(size_t n, char c, size_t start = 0);
│ │ │ │ +
298
│ │ │ │ +
299} // namespace gtsam
│ │ │ │ +
300
│ │ │ │ +
301#include <gtsam/nonlinear/Expression-inl.h>
│ │ │ │ +
302
│ │ │ │ +
OptionalJacobian.h
Special class for optional Jacobian arguments.
│ │ │ │ +
Expression-inl.h
Internals for Expression.h, not for general consumption.
│ │ │ │ +
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::symbol
Key symbol(unsigned char c, std::uint64_t j)
Create a symbol key from a character and index, i.e.
Definition Symbol.h:135
│ │ │ │ +
gtsam::createUnknowns
std::vector< Expression< T > > createUnknowns(size_t n, char c, size_t start)
Construct an array of leaves.
Definition Expression-inl.h:284
│ │ │ │ +
gtsam::operator+
Errors operator+(const Errors &a, const Errors &b)
Addition.
Definition Errors.cpp:60
│ │ │ │ +
gtsam::operator*
Point2 operator*(double s, const Point2 &p)
multiply with scalar
Definition Point2.h:47
│ │ │ │ +
gtsam::linearExpression
Expression< T > linearExpression(const std::function< T(A)> &f, const Expression< A > &expression, const Eigen::Matrix< double, traits< T >::dimension, traits< A >::dimension > &dTdA)
Create an expression out of a linear function f:T->A with (constant) Jacobian dTdA TODO(frank): creat...
Definition Expression.h:241
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::operator-
Errors operator-(const Errors &a, const Errors &b)
Subtraction.
Definition Errors.cpp:75
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
│ │ │ │ -
gtsam::GaussianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactor.h:42
│ │ │ │ -
gtsam::JacobianFactor
A Gaussian factor in the squared-error form.
Definition JacobianFactor.h:91
│ │ │ │ -
gtsam::noiseModel::Constrained::All
static shared_ptr All(size_t dim)
Fully constrained variations.
Definition NoiseModel.h:457
│ │ │ │ -
gtsam::NonlinearEquality
An equality factor that forces either one variable to a constant, or a set of variables to be equal t...
Definition NonlinearEquality.h:45
│ │ │ │ -
gtsam::NonlinearEquality::error
double error(const Values &c) const override
Actual error function calculation.
Definition NonlinearEquality.h:130
│ │ │ │ -
gtsam::NonlinearEquality::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition NonlinearEquality.h:109
│ │ │ │ -
gtsam::NonlinearEquality::CompareFunction
std::function< bool(const T &, const T &)> CompareFunction
Function that compares two values.
Definition NonlinearEquality.h:70
│ │ │ │ -
gtsam::NonlinearEquality::NonlinearEquality
NonlinearEquality()
Default constructor - only for serialization.
Definition NonlinearEquality.h:74
│ │ │ │ -
gtsam::NonlinearEquality::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Check if two factors are equal.
Definition NonlinearEquality.h:119
│ │ │ │ -
gtsam::NonlinearEquality::NonlinearEquality
NonlinearEquality(Key j, const T &feasible, double error_gain, const CompareFunction &_compare=std::bind(traits< T >::Equals, std::placeholders::_1, std::placeholders::_2, 1e-9))
Constructor - allows inexact evaluation.
Definition NonlinearEquality.h:97
│ │ │ │ -
gtsam::NonlinearEquality::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition NonlinearEquality.h:172
│ │ │ │ -
gtsam::NonlinearEquality::evaluateError
Vector evaluateError(const T &xj, boost::optional< Matrix & > H=boost::none) const override
Error function.
Definition NonlinearEquality.h:141
│ │ │ │ -
gtsam::NonlinearEquality::access
friend class boost::serialization::access
Serialization function.
Definition NonlinearEquality.h:184
│ │ │ │ -
gtsam::NonlinearEquality::NonlinearEquality
NonlinearEquality(Key j, const T &feasible, const CompareFunction &_compare=std::bind(traits< T >::Equals, std::placeholders::_1, std::placeholders::_2, 1e-9))
Constructor - forces exact evaluation.
Definition NonlinearEquality.h:86
│ │ │ │ -
gtsam::NonlinearEquality::linearize
GaussianFactor::shared_ptr linearize(const Values &x) const override
Linearize is over-written, because base linearization tries to whiten.
Definition NonlinearEquality.h:162
│ │ │ │ -
gtsam::NonlinearEquality1
Simple unary equality constraint - fixes a value for a variable.
Definition NonlinearEquality.h:207
│ │ │ │ -
gtsam::NonlinearEquality1::evaluateError
Vector evaluateError(const X &x1, boost::optional< Matrix & > H=boost::none) const override
g(x) with optional derivative
Definition NonlinearEquality.h:251
│ │ │ │ -
gtsam::NonlinearEquality1::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition NonlinearEquality.h:245
│ │ │ │ -
gtsam::NonlinearEquality1::shared_ptr
boost::shared_ptr< NonlinearEquality1< VALUE > > shared_ptr
fixed value for variable
Definition NonlinearEquality.h:227
│ │ │ │ -
gtsam::NonlinearEquality1::NonlinearEquality1
NonlinearEquality1(const X &value, Key key, double mu=1000.0)
Constructor.
Definition NonlinearEquality.h:235
│ │ │ │ -
gtsam::NonlinearEquality1::NonlinearEquality1
NonlinearEquality1()
Default constructor to allow for serialization.
Definition NonlinearEquality.h:217
│ │ │ │ -
gtsam::NonlinearEquality1::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition NonlinearEquality.h:260
│ │ │ │ -
gtsam::NonlinearEquality1::access
friend class boost::serialization::access
Serialization function.
Definition NonlinearEquality.h:273
│ │ │ │ -
gtsam::NonlinearEquality2
Simple binary equality constraint - this constraint forces two variables to be the same.
Definition NonlinearEquality.h:295
│ │ │ │ -
gtsam::NonlinearEquality2::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition NonlinearEquality.h:320
│ │ │ │ -
gtsam::NonlinearEquality2::evaluateError
Vector evaluateError(const T &x1, const T &x2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
g(x) with optional derivative2
Definition NonlinearEquality.h:326
│ │ │ │ -
gtsam::NonlinearEquality2::NonlinearEquality2
NonlinearEquality2(Key key1, Key key2, double mu=1e4)
Constructor.
Definition NonlinearEquality.h:314
│ │ │ │ -
gtsam::NonlinearEquality2::access
friend class boost::serialization::access
Serialization function.
Definition NonlinearEquality.h:339
│ │ │ │ -
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ -
gtsam::NoiseModelFactor::noiseModel
const SharedNoiseModel & noiseModel() const
access to the noise model
Definition NonlinearFactor.h:223
│ │ │ │ -
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ -
gtsam::NoiseModelFactorN< VALUE >::unwhitenedError
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
This implements the unwhitenedError virtual function by calling the n-key specific version of evaluat...
Definition NonlinearFactor.h:542
│ │ │ │ -
gtsam::NoiseModelFactorN< VALUE >::key
Key key() const
Returns a key.
Definition NonlinearFactor.h:518
│ │ │ │ +
gtsam::MakeOptionalJacobian
: meta-function to generate JacobianTA optional reference Used mainly by Expressions
Definition OptionalJacobian.h:261
│ │ │ │ +
gtsam::IsVectorSpace
Vector Space concept.
Definition VectorSpace.h:470
│ │ │ │ +
gtsam::Symbol
Character and index key used to refer to variables.
Definition Symbol.h:35
│ │ │ │ +
gtsam::ExpressionFactor
Factor that supports arbitrary expressions via AD.
Definition ExpressionFactor.h:44
│ │ │ │ +
gtsam::internal::ExecutionTrace
Definition Expression.h:40
│ │ │ │ +
gtsam::internal::ExpressionNode
Definition Expression.h:41
│ │ │ │ +
gtsam::Expression
Expression class that supports automatic differentiation.
Definition Expression.h:48
│ │ │ │ +
gtsam::Expression::root
const boost::shared_ptr< internal::ExpressionNode< T > > & root() const
Return root.
Definition Expression-inl.h:162
│ │ │ │ +
gtsam::Expression::type
Expression< T > type
Define type so we can apply it as a meta-function.
Definition Expression.h:53
│ │ │ │ +
gtsam::Expression::Expression
Expression()
Default constructor, for serialization.
Definition Expression.h:182
│ │ │ │ +
gtsam::Expression::~Expression
virtual ~Expression()
Destructor.
Definition Expression.h:141
│ │ │ │ +
gtsam::Expression::clone
virtual boost::shared_ptr< Expression > clone() const
Definition Expression.h:166
│ │ │ │ +
gtsam::Expression::Expression
Expression(const boost::shared_ptr< internal::ExpressionNode< T > > &root)
Construct with a custom root.
Definition Expression.h:61
│ │ │ │ +
gtsam::Expression::keys
std::set< Key > keys() const
Return keys that play in this expression.
Definition Expression-inl.h:132
│ │ │ │ +
gtsam::Expression::KeysAndDims
std::pair< KeyVector, FastVector< int > > KeysAndDims
Keys and dimensions in same order.
Definition Expression.h:185
│ │ │ │ +
gtsam::Expression::dims
void dims(std::map< Key, int > &map) const
Return dimensions for each argument, as a map.
Definition Expression-inl.h:137
│ │ │ │ +
gtsam::Expression::print
void print(const std::string &s) const
Print.
Definition Expression-inl.h:142
│ │ │ │ +
gtsam::Expression::valueAndJacobianMap
T valueAndJacobianMap(const Values &values, internal::JacobianMap &jacobians) const
brief Return value and derivatives, reverse AD version
Definition Expression-inl.h:205
│ │ │ │ +
gtsam::Expression::valueAndDerivatives
T valueAndDerivatives(const Values &values, const KeyVector &keys, const FastVector< int > &dims, std::vector< Matrix > &H) const
private version that takes keys and dimensions, returns derivatives
Definition Expression-inl.h:174
│ │ │ │ +
gtsam::Expression::value
T value(const Values &values, boost::optional< std::vector< Matrix > & > H=boost::none) const
Return value and optional derivatives, reverse AD version Notes: this is not terribly efficient,...
Definition Expression-inl.h:147
│ │ │ │ +
gtsam::Expression::operator+=
Expression< T > & operator+=(const Expression< T > &e)
Add another expression to this expression.
Definition Expression-inl.h:302
│ │ │ │ +
gtsam::Expression::traceSize
size_t traceSize() const
Return size needed for memory buffer in traceExecution.
Definition Expression-inl.h:167
│ │ │ │ +
gtsam::Expression::traceExecution
T traceExecution(const Values &values, internal::ExecutionTrace< T > &trace, void *traceStorage) const
trace execution, very unsafe
Definition Expression-inl.h:198
│ │ │ │ +
gtsam::Expression::UnaryFunction
Definition Expression.h:70
│ │ │ │ +
gtsam::Expression::BinaryFunction
Definition Expression.h:76
│ │ │ │ +
gtsam::Expression::TernaryFunction
Definition Expression.h:83
│ │ │ │ +
gtsam::ScalarMultiplyExpression
A ScalarMultiplyExpression is a specialization of Expression that multiplies with a scalar It optimiz...
Definition Expression.h:213
│ │ │ │ +
gtsam::BinarySumExpression
A BinarySumExpression is a specialization of Expression that adds two expressions together It optimiz...
Definition Expression.h:226
│ │ │ │
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
gtsam::Values::at
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
│ │ │ │ +
Values
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...
│ │ │ │ + │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,494 +1,404 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -NonlinearEquality.h │ │ │ │ │ +Expression.h │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -12/* │ │ │ │ │ -13 * @file NonlinearEquality.h │ │ │ │ │ -14 * @brief Factor to handle enforced equality between factors │ │ │ │ │ -15 * @author Alex Cunningham │ │ │ │ │ -16 */ │ │ │ │ │ -17 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ -23 │ │ │ │ │ -24#include │ │ │ │ │ -25 │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ -28#include │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include │ │ │ │ │ +23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_S_y_m_b_o_l_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_b_a_s_e_/_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h> │ │ │ │ │ +25#include │ │ │ │ │ +26 │ │ │ │ │ +27#include │ │ │ │ │ +28#include │ │ │ │ │ 29 │ │ │ │ │ -30namespace _g_t_s_a_m { │ │ │ │ │ -31 │ │ │ │ │ -44template │ │ │ │ │ -_4_5class _N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -46 │ │ │ │ │ -47public: │ │ │ │ │ -48 typedef VALUE T; │ │ │ │ │ +30// Forward declare tests │ │ │ │ │ +31class ExpressionFactorShallowTest; │ │ │ │ │ +32 │ │ │ │ │ +33namespace _g_t_s_a_m { │ │ │ │ │ +34 │ │ │ │ │ +35// Forward declares │ │ │ │ │ +36class _V_a_l_u_e_s; │ │ │ │ │ +37template class ExpressionFactor; │ │ │ │ │ +38 │ │ │ │ │ +39namespace internal { │ │ │ │ │ +_4_0template class _E_x_e_c_u_t_i_o_n_T_r_a_c_e; │ │ │ │ │ +_4_1template class _E_x_p_r_e_s_s_i_o_n_N_o_d_e; │ │ │ │ │ +42} │ │ │ │ │ +43 │ │ │ │ │ +47template │ │ │ │ │ +_4_8class _E_x_p_r_e_s_s_i_o_n { │ │ │ │ │ 49 │ │ │ │ │ -50private: │ │ │ │ │ +50public: │ │ │ │ │ 51 │ │ │ │ │ -52 // feasible value │ │ │ │ │ -53 T feasible_; │ │ │ │ │ +_5_3 typedef _E_x_p_r_e_s_s_i_o_n_<_T_> _t_y_p_e; │ │ │ │ │ 54 │ │ │ │ │ -55 // error handling flag │ │ │ │ │ -56 bool allow_error_; │ │ │ │ │ -57 │ │ │ │ │ -58 // error gain in allow error case │ │ │ │ │ -59 double error_gain_; │ │ │ │ │ -60 │ │ │ │ │ -61 // typedef to this class │ │ │ │ │ -62 using This = _N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_<_V_A_L_U_E_>; │ │ │ │ │ -63 │ │ │ │ │ -64 // typedef to base class │ │ │ │ │ -65 using _B_a_s_e = _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_V_A_L_U_E_>; │ │ │ │ │ -66 │ │ │ │ │ -67public: │ │ │ │ │ -68 │ │ │ │ │ -_7_0 using _C_o_m_p_a_r_e_F_u_n_c_t_i_o_n = std::function; │ │ │ │ │ -71 _C_o_m_p_a_r_e_F_u_n_c_t_i_o_n compare_; │ │ │ │ │ -72 │ │ │ │ │ -_7_4 _N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y() { │ │ │ │ │ -75 } │ │ │ │ │ -76 │ │ │ │ │ -77 _~_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y() override { │ │ │ │ │ -78 } │ │ │ │ │ -79 │ │ │ │ │ -82 │ │ │ │ │ -_8_6 _N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y(_K_e_y j, const T& feasible, │ │ │ │ │ -87 const _C_o_m_p_a_r_e_F_u_n_c_t_i_o_n &_compare = std::bind(_t_r_a_i_t_s_<_T_>_:_:_E_q_u_a_l_s, │ │ │ │ │ -88 std::placeholders::_1, std::placeholders::_2, 1e-9)) : │ │ │ │ │ -89 _B_a_s_e(_n_o_i_s_e_M_o_d_e_l::Constrained::All(_t_r_a_i_t_s::GetDimension(feasible)), │ │ │ │ │ -90 j), feasible_(feasible), allow_error_(false), error_gain_(0.0), // │ │ │ │ │ -91 compare_(_compare) { │ │ │ │ │ -92 } │ │ │ │ │ +55protected: │ │ │ │ │ +56 │ │ │ │ │ +57 // Paul's trick shared pointer, polymorphic root of entire expression tree │ │ │ │ │ +58 boost::shared_ptr > root_; │ │ │ │ │ +59 │ │ │ │ │ +_6_1 _E_x_p_r_e_s_s_i_o_n(const boost::shared_ptr<_i_n_t_e_r_n_a_l_:_:_E_x_p_r_e_s_s_i_o_n_N_o_d_e_<_T_> >& _r_o_o_t) : │ │ │ │ │ +root_(_r_o_o_t) {} │ │ │ │ │ +62 │ │ │ │ │ +63public: │ │ │ │ │ +64 │ │ │ │ │ +65 // Expressions wrap trees of functions that can evaluate their own │ │ │ │ │ +derivatives. │ │ │ │ │ +66 // The meta-functions below are useful to specify the type of those │ │ │ │ │ +functions. │ │ │ │ │ +67 // Example, a function taking a camera and a 3D point and yielding a 2D │ │ │ │ │ +point: │ │ │ │ │ +68 // Expression::BinaryFunction,Point3>::type │ │ │ │ │ +69 template │ │ │ │ │ +_7_0 struct _U_n_a_r_y_F_u_n_c_t_i_o_n { │ │ │ │ │ +71 typedef std::function< │ │ │ │ │ +72 T(const A1&, typename MakeOptionalJacobian::type)> type; │ │ │ │ │ +73 }; │ │ │ │ │ +74 │ │ │ │ │ +75 template │ │ │ │ │ +_7_6 struct _B_i_n_a_r_y_F_u_n_c_t_i_o_n { │ │ │ │ │ +77 typedef std::function< │ │ │ │ │ +78 T(const A1&, const A2&, typename MakeOptionalJacobian::type, │ │ │ │ │ +79 typename MakeOptionalJacobian::type)> type; │ │ │ │ │ +80 }; │ │ │ │ │ +81 │ │ │ │ │ +82 template │ │ │ │ │ +_8_3 struct _T_e_r_n_a_r_y_F_u_n_c_t_i_o_n { │ │ │ │ │ +84 typedef std::function< │ │ │ │ │ +85 T(const A1&, const A2&, const A3&, │ │ │ │ │ +86 typename MakeOptionalJacobian::type, │ │ │ │ │ +87 typename MakeOptionalJacobian::type, │ │ │ │ │ +88 typename MakeOptionalJacobian::type)> type; │ │ │ │ │ +89 }; │ │ │ │ │ +90 │ │ │ │ │ +92 _E_x_p_r_e_s_s_i_o_n(const T& _v_a_l_u_e); │ │ │ │ │ 93 │ │ │ │ │ -_9_7 _N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y(_K_e_y j, const T& feasible, double error_gain, │ │ │ │ │ -98 const _C_o_m_p_a_r_e_F_u_n_c_t_i_o_n &_compare = std::bind(_t_r_a_i_t_s_<_T_>_:_:_E_q_u_a_l_s, │ │ │ │ │ -99 std::placeholders::_1, std::placeholders::_2, 1e-9)) : │ │ │ │ │ -100 _B_a_s_e(_n_o_i_s_e_M_o_d_e_l::Constrained::All(_t_r_a_i_t_s::GetDimension(feasible)), │ │ │ │ │ -101 j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), // │ │ │ │ │ -102 compare_(_compare) { │ │ │ │ │ -103 } │ │ │ │ │ -104 │ │ │ │ │ -108 │ │ │ │ │ -_1_0_9 void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ -110 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ -111 std::cout << (s.empty() ? s : s + " ") << "Constraint: on [" │ │ │ │ │ -112 << keyFormatter(this->_k_e_y()) << "]\n"; │ │ │ │ │ -113 _t_r_a_i_t_s_<_V_A_L_U_E_>_:_:_P_r_i_n_t(feasible_, "Feasible Point:\n"); │ │ │ │ │ -114 std::cout << "Variable Dimension: " << _t_r_a_i_t_s_<_T_>_:_:_G_e_t_D_i_m_e_n_s_i_o_n(feasible_) │ │ │ │ │ -115 << std::endl; │ │ │ │ │ -116 } │ │ │ │ │ -117 │ │ │ │ │ -_1_1_9 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& f, double tol = 1e-9) const override { │ │ │ │ │ -120 const This* e = dynamic_cast(&f); │ │ │ │ │ -121 return e && _B_a_s_e_:_:_e_q_u_a_l_s(f) && _t_r_a_i_t_s_<_T_>_:_:_E_q_u_a_l_s(feasible_,e->feasible_, │ │ │ │ │ -tol) │ │ │ │ │ -122 && std::abs(error_gain_ - e->error_gain_) < tol; │ │ │ │ │ -123 } │ │ │ │ │ -124 │ │ │ │ │ -128 │ │ │ │ │ -_1_3_0 double _e_r_r_o_r(const _V_a_l_u_e_s& c) const override { │ │ │ │ │ -131 const T& xj = c._a_t(this->_k_e_y()); │ │ │ │ │ -132 Vector e = this->_u_n_w_h_i_t_e_n_e_d_E_r_r_o_r(c); │ │ │ │ │ -133 if (allow_error_ || !compare_(xj, feasible_)) { │ │ │ │ │ -134 return error_gain_ * _d_o_t(e, e); │ │ │ │ │ -135 } else { │ │ │ │ │ -136 return 0.0; │ │ │ │ │ -137 } │ │ │ │ │ -138 } │ │ │ │ │ +95 _E_x_p_r_e_s_s_i_o_n(const _K_e_y& key); │ │ │ │ │ +96 │ │ │ │ │ +98 _E_x_p_r_e_s_s_i_o_n(const _S_y_m_b_o_l& _s_y_m_b_o_l); │ │ │ │ │ +99 │ │ │ │ │ +101 _E_x_p_r_e_s_s_i_o_n(unsigned char c, std::uint64_t j); │ │ │ │ │ +102 │ │ │ │ │ +104 template │ │ │ │ │ +105 _E_x_p_r_e_s_s_i_o_n(typename UnaryFunction::type function, │ │ │ │ │ +106 const _E_x_p_r_e_s_s_i_o_n_<_A_>& expression); │ │ │ │ │ +107 │ │ │ │ │ +109 template │ │ │ │ │ +110 _E_x_p_r_e_s_s_i_o_n(typename BinaryFunction::type function, │ │ │ │ │ +111 const _E_x_p_r_e_s_s_i_o_n_<_A_1_>& expression1, const _E_x_p_r_e_s_s_i_o_n_<_A_2_>& expression2); │ │ │ │ │ +112 │ │ │ │ │ +114 template │ │ │ │ │ +115 _E_x_p_r_e_s_s_i_o_n(typename TernaryFunction::type function, │ │ │ │ │ +116 const _E_x_p_r_e_s_s_i_o_n_<_A_1_>& expression1, const _E_x_p_r_e_s_s_i_o_n_<_A_2_>& expression2, │ │ │ │ │ +117 const _E_x_p_r_e_s_s_i_o_n_<_A_3_>& expression3); │ │ │ │ │ +118 │ │ │ │ │ +120 template │ │ │ │ │ +121 _E_x_p_r_e_s_s_i_o_n(const _E_x_p_r_e_s_s_i_o_n_<_A_>& expression, │ │ │ │ │ +122 T (A::*method)(typename _M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n::_t_y_p_e) const); │ │ │ │ │ +123 │ │ │ │ │ +125 template │ │ │ │ │ +126 _E_x_p_r_e_s_s_i_o_n(const _E_x_p_r_e_s_s_i_o_n_<_A_1_>& expression1, │ │ │ │ │ +127 T (A1::*method)(const A2&, typename _M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n::_t_y_p_e, │ │ │ │ │ +128 typename _M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n::_t_y_p_e) const, │ │ │ │ │ +129 const _E_x_p_r_e_s_s_i_o_n& expression2); │ │ │ │ │ +130 │ │ │ │ │ +132 template │ │ │ │ │ +133 _E_x_p_r_e_s_s_i_o_n(const _E_x_p_r_e_s_s_i_o_n_<_A_1_>& expression1, │ │ │ │ │ +134 T (A1::*method)(const A2&, const A3&, │ │ │ │ │ +135 typename _M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n::_t_y_p_e, │ │ │ │ │ +136 typename _M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n::_t_y_p_e, │ │ │ │ │ +137 typename _M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n::_t_y_p_e) const, │ │ │ │ │ +138 const _E_x_p_r_e_s_s_i_o_n& expression2, const _E_x_p_r_e_s_s_i_o_n& expression3); │ │ │ │ │ 139 │ │ │ │ │ -_1_4_1 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const T& xj, │ │ │ │ │ -142 boost::optional H = boost::none) const override { │ │ │ │ │ -143 const size_t nj = _t_r_a_i_t_s_<_T_>_:_:_G_e_t_D_i_m_e_n_s_i_o_n(feasible_); │ │ │ │ │ -144 if (allow_error_) { │ │ │ │ │ -145 if (H) │ │ │ │ │ -146 *H = Matrix::Identity(nj,nj); // FIXME: this is not the right linearization │ │ │ │ │ -for nonlinear compare │ │ │ │ │ -147 return _t_r_a_i_t_s_<_T_>_:_:_L_o_c_a_l(xj,feasible_); │ │ │ │ │ -148 } else if (compare_(feasible_, xj)) { │ │ │ │ │ -149 if (H) │ │ │ │ │ -150 *H = Matrix::Identity(nj,nj); │ │ │ │ │ -151 return Vector::Zero(nj); // set error to zero if equal │ │ │ │ │ -152 } else { │ │ │ │ │ -153 if (H) │ │ │ │ │ -154 throw std::invalid_argument( │ │ │ │ │ -155 "Linearization point not feasible for " │ │ │ │ │ -156 + DefaultKeyFormatter(this->_k_e_y()) + "!"); │ │ │ │ │ -157 return Vector::Constant(nj, std::numeric_limits::infinity()); / │ │ │ │ │ -/ set error to infinity if not equal │ │ │ │ │ -158 } │ │ │ │ │ -159 } │ │ │ │ │ +_1_4_1 virtual _~_E_x_p_r_e_s_s_i_o_n() { │ │ │ │ │ +142 } │ │ │ │ │ +143 │ │ │ │ │ +145 std::set _k_e_y_s() const; │ │ │ │ │ +146 │ │ │ │ │ +148 void _d_i_m_s(std::map& map) const; │ │ │ │ │ +149 │ │ │ │ │ +151 void _p_r_i_n_t(const std::string& s) const; │ │ │ │ │ +152 │ │ │ │ │ +158 T _v_a_l_u_e(const _V_a_l_u_e_s& values, boost::optional&> H = │ │ │ │ │ +159 boost::none) const; │ │ │ │ │ 160 │ │ │ │ │ -_1_6_2 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r _l_i_n_e_a_r_i_z_e(const _V_a_l_u_e_s& x) const override { │ │ │ │ │ -163 const T& xj = x._a_t(this->_k_e_y()); │ │ │ │ │ -164 Matrix A; │ │ │ │ │ -165 Vector b = _e_v_a_l_u_a_t_e_E_r_r_o_r(xj, A); │ │ │ │ │ -166 SharedDiagonal model = _n_o_i_s_e_M_o_d_e_l_:_:_C_o_n_s_t_r_a_i_n_e_d_:_:_A_l_l(b.size()); │ │ │ │ │ -167 return _G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r( │ │ │ │ │ -168 new _J_a_c_o_b_i_a_n_F_a_c_t_o_r(this->_k_e_y(), A, b, model)); │ │ │ │ │ -169 } │ │ │ │ │ -170 │ │ │ │ │ -_1_7_2 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -173 return boost::static_pointer_cast( │ │ │ │ │ -174 gtsam::NonlinearFactor::shared_ptr(new This(*this))); │ │ │ │ │ -175 } │ │ │ │ │ -176 │ │ │ │ │ +_1_6_6 virtual boost::shared_ptr _c_l_o_n_e() const { │ │ │ │ │ +167 return boost::make_shared(*this); │ │ │ │ │ +168 } │ │ │ │ │ +169 │ │ │ │ │ +171 const boost::shared_ptr >& _r_o_o_t() const; │ │ │ │ │ +172 │ │ │ │ │ +174 size_t _t_r_a_c_e_S_i_z_e() const; │ │ │ │ │ +175 │ │ │ │ │ +177 _E_x_p_r_e_s_s_i_o_n_<_T_>& _o_p_e_r_a_t_o_r_+_=(const _E_x_p_r_e_s_s_i_o_n_<_T_>& e); │ │ │ │ │ 178 │ │ │ │ │ -179 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +179protected: │ │ │ │ │ 180 │ │ │ │ │ -181private: │ │ │ │ │ -182 │ │ │ │ │ -_1_8_4 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -185 template │ │ │ │ │ -186 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -187 // NoiseModelFactor1 instead of NoiseModelFactorN for backward │ │ │ │ │ -compatibility │ │ │ │ │ -188 ar │ │ │ │ │ -189 & boost::serialization::make_nvp("NoiseModelFactor1", │ │ │ │ │ -190 boost::serialization::base_object(*this)); │ │ │ │ │ -191 ar & BOOST_SERIALIZATION_NVP(feasible_); │ │ │ │ │ -192 ar & BOOST_SERIALIZATION_NVP(allow_error_); │ │ │ │ │ -193 ar & BOOST_SERIALIZATION_NVP(error_gain_); │ │ │ │ │ -194 } │ │ │ │ │ +_1_8_2 _E_x_p_r_e_s_s_i_o_n() {} │ │ │ │ │ +183 │ │ │ │ │ +_1_8_5 typedef std::pair > _K_e_y_s_A_n_d_D_i_m_s; │ │ │ │ │ +186 _K_e_y_s_A_n_d_D_i_m_s keysAndDims() const; │ │ │ │ │ +187 │ │ │ │ │ +189 T _v_a_l_u_e_A_n_d_D_e_r_i_v_a_t_i_v_e_s(const _V_a_l_u_e_s& values, const _K_e_y_V_e_c_t_o_r& _k_e_y_s, │ │ │ │ │ +190 const _F_a_s_t_V_e_c_t_o_r_<_i_n_t_>& _d_i_m_s, std::vector& H) const; │ │ │ │ │ +191 │ │ │ │ │ +193 T _t_r_a_c_e_E_x_e_c_u_t_i_o_n(const _V_a_l_u_e_s& values, _i_n_t_e_r_n_a_l_:_:_E_x_e_c_u_t_i_o_n_T_r_a_c_e_<_T_>& trace, │ │ │ │ │ +194 void* traceStorage) const; │ │ │ │ │ 195 │ │ │ │ │ -196}; │ │ │ │ │ -197// \class NonlinearEquality │ │ │ │ │ -198 │ │ │ │ │ -199template │ │ │ │ │ -_2_0_0struct _t_r_a_i_t_s<_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y> : _T_e_s_t_a_b_l_e> │ │ │ │ │ -{}; │ │ │ │ │ -201 │ │ │ │ │ -202/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -206template │ │ │ │ │ -_2_0_7class _N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1: public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -208 │ │ │ │ │ -209public: │ │ │ │ │ -210 typedef VALUE X; │ │ │ │ │ -211 │ │ │ │ │ -212protected: │ │ │ │ │ -213 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_V_A_L_U_E_> _B_a_s_e; │ │ │ │ │ -214 typedef _N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1_<_V_A_L_U_E_> This; │ │ │ │ │ -215 │ │ │ │ │ -_2_1_7 _N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1() { │ │ │ │ │ -218 } │ │ │ │ │ -219 │ │ │ │ │ -220 X value_; │ │ │ │ │ -221 │ │ │ │ │ -222 GTSAM_CONCEPT_MANIFOLD_TYPE(X) │ │ │ │ │ -223 GTSAM_CONCEPT_TESTABLE_TYPE(X) │ │ │ │ │ -224 │ │ │ │ │ -225public: │ │ │ │ │ -226 │ │ │ │ │ -_2_2_7 typedef boost::shared_ptr > _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -228 │ │ │ │ │ -_2_3_5 _N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1(const X& value, _K_e_y _k_e_y, double mu = 1000.0) │ │ │ │ │ -236 : _B_a_s_e(_n_o_i_s_e_M_o_d_e_l::Constrained::All(_t_r_a_i_t_s::GetDimension(value), │ │ │ │ │ -237 std::abs(mu)), │ │ │ │ │ -238 _k_e_y), │ │ │ │ │ -239 value_(value) {} │ │ │ │ │ -240 │ │ │ │ │ -241 _~_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1() override { │ │ │ │ │ -242 } │ │ │ │ │ -243 │ │ │ │ │ -_2_4_5 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -246 return boost::static_pointer_cast( │ │ │ │ │ -247 gtsam::NonlinearFactor::shared_ptr(new This(*this))); │ │ │ │ │ -248 } │ │ │ │ │ -249 │ │ │ │ │ -_2_5_1 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r(const X& x1, │ │ │ │ │ -252 boost::optional H = boost::none) const override { │ │ │ │ │ -253 if (H) │ │ │ │ │ -254 (*H) = Matrix::Identity(_t_r_a_i_t_s_<_X_>_:_:_G_e_t_D_i_m_e_n_s_i_o_n(x1),_t_r_a_i_t_s_<_X_>_:_:_G_e_t_D_i_m_e_n_s_i_o_n │ │ │ │ │ -(x1)); │ │ │ │ │ -255 // manifold equivalent of h(x)-z -> log(z,h(x)) │ │ │ │ │ -256 return _t_r_a_i_t_s_<_X_>_:_:_L_o_c_a_l(value_,x1); │ │ │ │ │ -257 } │ │ │ │ │ -258 │ │ │ │ │ -_2_6_0 void _p_r_i_n_t(const std::string& s = "", │ │ │ │ │ -261 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ -262 std::cout << s << ": NonlinearEquality1(" << keyFormatter(this->_k_e_y()) │ │ │ │ │ -263 << ")," << "\n"; │ │ │ │ │ -264 this->noiseModel_->print(); │ │ │ │ │ -265 _t_r_a_i_t_s_<_X_>_:_:_P_r_i_n_t(value_, "Value"); │ │ │ │ │ -266 } │ │ │ │ │ -267 │ │ │ │ │ -268 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -269 │ │ │ │ │ -270private: │ │ │ │ │ -271 │ │ │ │ │ -_2_7_3 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -274 template │ │ │ │ │ -275 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -276 // NoiseModelFactor1 instead of NoiseModelFactorN for backward │ │ │ │ │ -compatibility │ │ │ │ │ -277 ar │ │ │ │ │ -278 & boost::serialization::make_nvp("NoiseModelFactor1", │ │ │ │ │ -279 boost::serialization::base_object(*this)); │ │ │ │ │ -280 ar & BOOST_SERIALIZATION_NVP(value_); │ │ │ │ │ -281 } │ │ │ │ │ -282}; │ │ │ │ │ -283// \NonlinearEquality1 │ │ │ │ │ -284 │ │ │ │ │ -285template │ │ │ │ │ -_2_8_6struct _t_r_a_i_t_s<_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1 > │ │ │ │ │ -287 : _T_e_s_t_a_b_l_e > {}; │ │ │ │ │ -288 │ │ │ │ │ -289/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -294template │ │ │ │ │ -_2_9_5class _N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_2 : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -296 protected: │ │ │ │ │ -297 using _B_a_s_e = _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_T_,_ _T_>; │ │ │ │ │ -298 using This = _N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_2_<_T_>; │ │ │ │ │ -299 │ │ │ │ │ -300 GTSAM_CONCEPT_MANIFOLD_TYPE(T) │ │ │ │ │ -301 │ │ │ │ │ +197 T _v_a_l_u_e_A_n_d_J_a_c_o_b_i_a_n_M_a_p(const _V_a_l_u_e_s& values, │ │ │ │ │ +198 internal::JacobianMap& jacobians) const; │ │ │ │ │ +199 │ │ │ │ │ +200 // be very selective on who can access these private methods: │ │ │ │ │ +201 friend class _E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r ; │ │ │ │ │ +202 friend class _i_n_t_e_r_n_a_l_:_:_E_x_p_r_e_s_s_i_o_n_N_o_d_e; │ │ │ │ │ +203 │ │ │ │ │ +204 // and add tests │ │ │ │ │ +205 friend class ::ExpressionFactorShallowTest; │ │ │ │ │ +206}; │ │ │ │ │ +207 │ │ │ │ │ +212template │ │ │ │ │ +_2_1_3class _S_c_a_l_a_r_M_u_l_t_i_p_l_y_E_x_p_r_e_s_s_i_o_n : public _E_x_p_r_e_s_s_i_o_n { │ │ │ │ │ +214 // Check that T is a vector space │ │ │ │ │ +215 BOOST_CONCEPT_ASSERT((_g_t_s_a_m_:_:_I_s_V_e_c_t_o_r_S_p_a_c_e_<_T_>)); │ │ │ │ │ +216 │ │ │ │ │ +217 public: │ │ │ │ │ +218 explicit _S_c_a_l_a_r_M_u_l_t_i_p_l_y_E_x_p_r_e_s_s_i_o_n(double s, const _E_x_p_r_e_s_s_i_o_n_<_T_>& e); │ │ │ │ │ +219}; │ │ │ │ │ +220 │ │ │ │ │ +225template │ │ │ │ │ +_2_2_6class _B_i_n_a_r_y_S_u_m_E_x_p_r_e_s_s_i_o_n : public _E_x_p_r_e_s_s_i_o_n { │ │ │ │ │ +227 // Check that T is a vector space │ │ │ │ │ +228 BOOST_CONCEPT_ASSERT((_g_t_s_a_m_:_:_I_s_V_e_c_t_o_r_S_p_a_c_e_<_T_>)); │ │ │ │ │ +229 │ │ │ │ │ +230 public: │ │ │ │ │ +231 explicit _B_i_n_a_r_y_S_u_m_E_x_p_r_e_s_s_i_o_n(const _E_x_p_r_e_s_s_i_o_n_<_T_>& e1, const _E_x_p_r_e_s_s_i_o_n_<_T_>& │ │ │ │ │ +e2); │ │ │ │ │ +232}; │ │ │ │ │ +233 │ │ │ │ │ +234 │ │ │ │ │ +240template │ │ │ │ │ +_2_4_1_E_x_p_r_e_s_s_i_o_n_<_T_> _l_i_n_e_a_r_E_x_p_r_e_s_s_i_o_n( │ │ │ │ │ +242 const std::function& f, const _E_x_p_r_e_s_s_i_o_n_<_A_>& expression, │ │ │ │ │ +243 const Eigen::Matrix_:_:_d_i_m_e_n_s_i_o_n, _t_r_a_i_t_s_<_A_>_:_:_d_i_m_e_n_s_i_o_n>& │ │ │ │ │ +dTdA) { │ │ │ │ │ +244 // Use lambda to endow f with a linear Jacobian │ │ │ │ │ +245 typename _E_x_p_r_e_s_s_i_o_n_<_T_>_:_:_t_e_m_p_l_a_t_e UnaryFunction::type g = │ │ │ │ │ +246 [=](const A& value, typename MakeOptionalJacobian::type H) { │ │ │ │ │ +247 if (H) │ │ │ │ │ +248 *H << dTdA; │ │ │ │ │ +249 return f(value); │ │ │ │ │ +250 }; │ │ │ │ │ +251 return _E_x_p_r_e_s_s_i_o_n_<_T_>(g, expression); │ │ │ │ │ +252} │ │ │ │ │ +253 │ │ │ │ │ +260template │ │ │ │ │ +_2_6_1_S_c_a_l_a_r_M_u_l_t_i_p_l_y_E_x_p_r_e_s_s_i_o_n_<_T_> _o_p_e_r_a_t_o_r_*(double s, const _E_x_p_r_e_s_s_i_o_n_<_T_>& e) { │ │ │ │ │ +262 return _S_c_a_l_a_r_M_u_l_t_i_p_l_y_E_x_p_r_e_s_s_i_o_n_<_T_>(s, e); │ │ │ │ │ +263} │ │ │ │ │ +264 │ │ │ │ │ +271template │ │ │ │ │ +_2_7_2_B_i_n_a_r_y_S_u_m_E_x_p_r_e_s_s_i_o_n_<_T_> _o_p_e_r_a_t_o_r_+(const _E_x_p_r_e_s_s_i_o_n_<_T_>& e1, const │ │ │ │ │ +_E_x_p_r_e_s_s_i_o_n_<_T_>& e2) { │ │ │ │ │ +273 return _B_i_n_a_r_y_S_u_m_E_x_p_r_e_s_s_i_o_n_<_T_>(e1, e2); │ │ │ │ │ +274} │ │ │ │ │ +275 │ │ │ │ │ +277template │ │ │ │ │ +_2_7_8_B_i_n_a_r_y_S_u_m_E_x_p_r_e_s_s_i_o_n_<_T_> _o_p_e_r_a_t_o_r_-(const _E_x_p_r_e_s_s_i_o_n_<_T_>& e1, const │ │ │ │ │ +_E_x_p_r_e_s_s_i_o_n_<_T_>& e2) { │ │ │ │ │ +279 // TODO(frank, abe): Implement an actual negate operator instead of │ │ │ │ │ +multiplying by -1 │ │ │ │ │ +280 return e1 + (-1.0) * e2; │ │ │ │ │ +281} │ │ │ │ │ +282 │ │ │ │ │ +288template │ │ │ │ │ +289Expression _o_p_e_r_a_t_o_r_*(const Expression& e1, const Expression& e2); │ │ │ │ │ +290 │ │ │ │ │ +296template │ │ │ │ │ +297std::vector > _c_r_e_a_t_e_U_n_k_n_o_w_n_s(size_t n, char c, size_t start = │ │ │ │ │ +0); │ │ │ │ │ +298 │ │ │ │ │ +299} // namespace gtsam │ │ │ │ │ +300 │ │ │ │ │ +301#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_E_x_p_r_e_s_s_i_o_n_-_i_n_l_._h> │ │ │ │ │ 302 │ │ │ │ │ -_3_0_3 _N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_2() {} │ │ │ │ │ -304 │ │ │ │ │ -305 public: │ │ │ │ │ -306 typedef boost::shared_ptr> shared_ptr; │ │ │ │ │ -307 │ │ │ │ │ -_3_1_4 _N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_2(_K_e_y key1, _K_e_y key2, double mu = 1e4) │ │ │ │ │ -315 : _B_a_s_e(_n_o_i_s_e_M_o_d_e_l::Constrained::All(_t_r_a_i_t_s::dimension, std::abs(mu)), │ │ │ │ │ -316 key1, key2) {} │ │ │ │ │ -317 _~_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_2() override {} │ │ │ │ │ -318 │ │ │ │ │ -_3_2_0 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -321 return boost::static_pointer_cast( │ │ │ │ │ -322 gtsam::NonlinearFactor::shared_ptr(new This(*this))); │ │ │ │ │ -323 } │ │ │ │ │ -324 │ │ │ │ │ -_3_2_6 Vector _e_v_a_l_u_a_t_e_E_r_r_o_r( │ │ │ │ │ -327 const T& x1, const T& x2, boost::optional H1 = boost::none, │ │ │ │ │ -328 boost::optional H2 = boost::none) const override { │ │ │ │ │ -329 static const size_t p = _t_r_a_i_t_s_<_T_>_:_:_d_i_m_e_n_s_i_o_n; │ │ │ │ │ -330 if (H1) *H1 = -Matrix::Identity(p, p); │ │ │ │ │ -331 if (H2) *H2 = Matrix::Identity(p, p); │ │ │ │ │ -332 return _t_r_a_i_t_s_<_T_>_:_:_L_o_c_a_l(x1, x2); │ │ │ │ │ -333 } │ │ │ │ │ -334 │ │ │ │ │ -335 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -336 │ │ │ │ │ -337 private: │ │ │ │ │ -_3_3_9 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -340 template │ │ │ │ │ -341 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ -342 // NoiseModelFactor2 instead of NoiseModelFactorN for backward │ │ │ │ │ -compatibility │ │ │ │ │ -343 ar& boost::serialization::make_nvp( │ │ │ │ │ -344 "NoiseModelFactor2", boost::serialization::base_object(*this)); │ │ │ │ │ -345 } │ │ │ │ │ -346}; │ │ │ │ │ -347// \NonlinearEquality2 │ │ │ │ │ -348 │ │ │ │ │ -349template │ │ │ │ │ -_3_5_0struct _t_r_a_i_t_s<_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_2> : │ │ │ │ │ -_T_e_s_t_a_b_l_e> { │ │ │ │ │ -351}; │ │ │ │ │ -352 │ │ │ │ │ -353}// namespace gtsam │ │ │ │ │ -_T_e_s_t_a_b_l_e_._h │ │ │ │ │ -Concept check for values that can be used in unit tests. │ │ │ │ │ -_M_a_n_i_f_o_l_d_._h │ │ │ │ │ -Base class and basic functions for Manifold types. │ │ │ │ │ -_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ -#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ -This marks a GTSAM object to require alignment. │ │ │ │ │ -DDeeffiinniittiioonn types.h:308 │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ -Non-linear factor base classes. │ │ │ │ │ +_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h │ │ │ │ │ +Special class for optional Jacobian arguments. │ │ │ │ │ +_E_x_p_r_e_s_s_i_o_n_-_i_n_l_._h │ │ │ │ │ +Internals for Expression.h, not for general consumption. │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_V_e_c_t_o_r │ │ │ │ │ +std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > │ │ │ │ │ +FastVector │ │ │ │ │ +FastVector is a type alias to a std::vector with a custom memory allocator. │ │ │ │ │ +DDeeffiinniittiioonn FastVector.h:34 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_d_o_t │ │ │ │ │ -double dot(const V1 &a, const V2 &b) │ │ │ │ │ -Dot product. │ │ │ │ │ -DDeeffiinniittiioonn Vector.h:195 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_s_y_m_b_o_l │ │ │ │ │ +Key symbol(unsigned char c, std::uint64_t j) │ │ │ │ │ +Create a symbol key from a character and index, i.e. │ │ │ │ │ +DDeeffiinniittiioonn Symbol.h:135 │ │ │ │ │ +_g_t_s_a_m_:_:_c_r_e_a_t_e_U_n_k_n_o_w_n_s │ │ │ │ │ +std::vector< Expression< T > > createUnknowns(size_t n, char c, size_t start) │ │ │ │ │ +Construct an array of leaves. │ │ │ │ │ +DDeeffiinniittiioonn Expression-inl.h:284 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_+ │ │ │ │ │ +Errors operator+(const Errors &a, const Errors &b) │ │ │ │ │ +Addition. │ │ │ │ │ +DDeeffiinniittiioonn Errors.cpp:60 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_* │ │ │ │ │ +Point2 operator*(double s, const Point2 &p) │ │ │ │ │ +multiply with scalar │ │ │ │ │ +DDeeffiinniittiioonn Point2.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_l_i_n_e_a_r_E_x_p_r_e_s_s_i_o_n │ │ │ │ │ +Expression< T > linearExpression(const std::function< T(A)> &f, const │ │ │ │ │ +Expression< A > &expression, const Eigen::Matrix< double, traits< T >:: │ │ │ │ │ +dimension, traits< A >::dimension > &dTdA) │ │ │ │ │ +Create an expression out of a linear function f:T->A with (constant) Jacobian │ │ │ │ │ +dTdA TODO(frank): creat... │ │ │ │ │ +DDeeffiinniittiioonn Expression.h:241 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_- │ │ │ │ │ +Errors operator-(const Errors &a, const Errors &b) │ │ │ │ │ +Subtraction. │ │ │ │ │ +DDeeffiinniittiioonn Errors.cpp:75 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const This &other, double tol=1e-9) const │ │ │ │ │ -check equality │ │ │ │ │ -DDeeffiinniittiioonn Factor.cpp:42 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ -A Gaussian factor in the squared-error form. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.h:91 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_C_o_n_s_t_r_a_i_n_e_d_:_:_A_l_l │ │ │ │ │ -static shared_ptr All(size_t dim) │ │ │ │ │ -Fully constrained variations. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:457 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y │ │ │ │ │ -An equality factor that forces either one variable to a constant, or a set of │ │ │ │ │ -variables to be equal t... │ │ │ │ │ -DDeeffiinniittiioonn NonlinearEquality.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_:_:_e_r_r_o_r │ │ │ │ │ -double error(const Values &c) const override │ │ │ │ │ -Actual error function calculation. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearEquality.h:130 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ +_g_t_s_a_m_:_:_M_a_k_e_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +: meta-function to generate JacobianTA optional reference Used mainly by │ │ │ │ │ +Expressions │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:261 │ │ │ │ │ +_g_t_s_a_m_:_:_I_s_V_e_c_t_o_r_S_p_a_c_e │ │ │ │ │ +Vector Space concept. │ │ │ │ │ +DDeeffiinniittiioonn VectorSpace.h:470 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l │ │ │ │ │ +Character and index key used to refer to variables. │ │ │ │ │ +DDeeffiinniittiioonn Symbol.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_F_a_c_t_o_r │ │ │ │ │ +Factor that supports arbitrary expressions via AD. │ │ │ │ │ +DDeeffiinniittiioonn ExpressionFactor.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_E_x_e_c_u_t_i_o_n_T_r_a_c_e │ │ │ │ │ +DDeeffiinniittiioonn Expression.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_E_x_p_r_e_s_s_i_o_n_N_o_d_e │ │ │ │ │ +DDeeffiinniittiioonn Expression.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n │ │ │ │ │ +Expression class that supports automatic differentiation. │ │ │ │ │ +DDeeffiinniittiioonn Expression.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_r_o_o_t │ │ │ │ │ +const boost::shared_ptr< internal::ExpressionNode< T > > & root() const │ │ │ │ │ +Return root. │ │ │ │ │ +DDeeffiinniittiioonn Expression-inl.h:162 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_t_y_p_e │ │ │ │ │ +Expression< T > type │ │ │ │ │ +Define type so we can apply it as a meta-function. │ │ │ │ │ +DDeeffiinniittiioonn Expression.h:53 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_E_x_p_r_e_s_s_i_o_n │ │ │ │ │ +Expression() │ │ │ │ │ +Default constructor, for serialization. │ │ │ │ │ +DDeeffiinniittiioonn Expression.h:182 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_~_E_x_p_r_e_s_s_i_o_n │ │ │ │ │ +virtual ~Expression() │ │ │ │ │ +Destructor. │ │ │ │ │ +DDeeffiinniittiioonn Expression.h:141 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_c_l_o_n_e │ │ │ │ │ +virtual boost::shared_ptr< Expression > clone() const │ │ │ │ │ +DDeeffiinniittiioonn Expression.h:166 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_E_x_p_r_e_s_s_i_o_n │ │ │ │ │ +Expression(const boost::shared_ptr< internal::ExpressionNode< T > > &root) │ │ │ │ │ +Construct with a custom root. │ │ │ │ │ +DDeeffiinniittiioonn Expression.h:61 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_k_e_y_s │ │ │ │ │ +std::set< Key > keys() const │ │ │ │ │ +Return keys that play in this expression. │ │ │ │ │ +DDeeffiinniittiioonn Expression-inl.h:132 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_K_e_y_s_A_n_d_D_i_m_s │ │ │ │ │ +std::pair< KeyVector, FastVector< int > > KeysAndDims │ │ │ │ │ +Keys and dimensions in same order. │ │ │ │ │ +DDeeffiinniittiioonn Expression.h:185 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_d_i_m_s │ │ │ │ │ +void dims(std::map< Key, int > &map) const │ │ │ │ │ +Return dimensions for each argument, as a map. │ │ │ │ │ +DDeeffiinniittiioonn Expression-inl.h:137 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s) const │ │ │ │ │ Print. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearEquality.h:109 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_:_:_C_o_m_p_a_r_e_F_u_n_c_t_i_o_n │ │ │ │ │ -std::function< bool(const T &, const T &)> CompareFunction │ │ │ │ │ -Function that compares two values. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearEquality.h:70 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y │ │ │ │ │ -NonlinearEquality() │ │ │ │ │ -Default constructor - only for serialization. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearEquality.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const NonlinearFactor &f, double tol=1e-9) const override │ │ │ │ │ -Check if two factors are equal. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearEquality.h:119 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y │ │ │ │ │ -NonlinearEquality(Key j, const T &feasible, double error_gain, const │ │ │ │ │ -CompareFunction &_compare=std::bind(traits< T >::Equals, std::placeholders::_1, │ │ │ │ │ -std::placeholders::_2, 1e-9)) │ │ │ │ │ -Constructor - allows inexact evaluation. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearEquality.h:97 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_:_:_c_l_o_n_e │ │ │ │ │ -gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn NonlinearEquality.h:172 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ -Vector evaluateError(const T &xj, boost::optional< Matrix & > H=boost::none) │ │ │ │ │ -const override │ │ │ │ │ -Error function. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearEquality.h:141 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearEquality.h:184 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y │ │ │ │ │ -NonlinearEquality(Key j, const T &feasible, const CompareFunction │ │ │ │ │ -&_compare=std::bind(traits< T >::Equals, std::placeholders::_1, std:: │ │ │ │ │ -placeholders::_2, 1e-9)) │ │ │ │ │ -Constructor - forces exact evaluation. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearEquality.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_:_:_l_i_n_e_a_r_i_z_e │ │ │ │ │ -GaussianFactor::shared_ptr linearize(const Values &x) const override │ │ │ │ │ -Linearize is over-written, because base linearization tries to whiten. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearEquality.h:162 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1 │ │ │ │ │ -Simple unary equality constraint - fixes a value for a variable. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearEquality.h:207 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ -Vector evaluateError(const X &x1, boost::optional< Matrix & > H=boost::none) │ │ │ │ │ -const override │ │ │ │ │ -g(x) with optional derivative │ │ │ │ │ -DDeeffiinniittiioonn NonlinearEquality.h:251 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1_:_:_c_l_o_n_e │ │ │ │ │ -gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn NonlinearEquality.h:245 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< NonlinearEquality1< VALUE > > shared_ptr │ │ │ │ │ -fixed value for variable │ │ │ │ │ -DDeeffiinniittiioonn NonlinearEquality.h:227 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1 │ │ │ │ │ -NonlinearEquality1(const X &value, Key key, double mu=1000.0) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearEquality.h:235 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1 │ │ │ │ │ -NonlinearEquality1() │ │ │ │ │ -Default constructor to allow for serialization. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearEquality.h:217 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -Print. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearEquality.h:260 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_1_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearEquality.h:273 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_2 │ │ │ │ │ -Simple binary equality constraint - this constraint forces two variables to be │ │ │ │ │ -the same. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearEquality.h:295 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_2_:_:_c_l_o_n_e │ │ │ │ │ -gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn NonlinearEquality.h:320 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_2_:_:_e_v_a_l_u_a_t_e_E_r_r_o_r │ │ │ │ │ -Vector evaluateError(const T &x1, const T &x2, boost::optional< Matrix & > │ │ │ │ │ -H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override │ │ │ │ │ -g(x) with optional derivative2 │ │ │ │ │ -DDeeffiinniittiioonn NonlinearEquality.h:326 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_2_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_2 │ │ │ │ │ -NonlinearEquality2(Key key1, Key key2, double mu=1e4) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearEquality.h:314 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_E_q_u_a_l_i_t_y_2_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearEquality.h:339 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ -Nonlinear factor base class. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_n_o_i_s_e_M_o_d_e_l │ │ │ │ │ -const SharedNoiseModel & noiseModel() const │ │ │ │ │ -access to the noise model │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:223 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ -A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ -variables. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_ _V_A_L_U_E_ _>_:_:_u_n_w_h_i_t_e_n_e_d_E_r_r_o_r │ │ │ │ │ -Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > │ │ │ │ │ -& > H=boost::none) const override │ │ │ │ │ -This implements the unwhitenedError virtual function by calling the n-key │ │ │ │ │ -specific version of evaluat... │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:542 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_ _V_A_L_U_E_ _>_:_:_k_e_y │ │ │ │ │ -Key key() const │ │ │ │ │ -Returns a key. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:518 │ │ │ │ │ +DDeeffiinniittiioonn Expression-inl.h:142 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_v_a_l_u_e_A_n_d_J_a_c_o_b_i_a_n_M_a_p │ │ │ │ │ +T valueAndJacobianMap(const Values &values, internal::JacobianMap &jacobians) │ │ │ │ │ +const │ │ │ │ │ +brief Return value and derivatives, reverse AD version │ │ │ │ │ +DDeeffiinniittiioonn Expression-inl.h:205 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_v_a_l_u_e_A_n_d_D_e_r_i_v_a_t_i_v_e_s │ │ │ │ │ +T valueAndDerivatives(const Values &values, const KeyVector &keys, const │ │ │ │ │ +FastVector< int > &dims, std::vector< Matrix > &H) const │ │ │ │ │ +private version that takes keys and dimensions, returns derivatives │ │ │ │ │ +DDeeffiinniittiioonn Expression-inl.h:174 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_v_a_l_u_e │ │ │ │ │ +T value(const Values &values, boost::optional< std::vector< Matrix > & > │ │ │ │ │ +H=boost::none) const │ │ │ │ │ +Return value and optional derivatives, reverse AD version Notes: this is not │ │ │ │ │ +terribly efficient,... │ │ │ │ │ +DDeeffiinniittiioonn Expression-inl.h:147 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_o_p_e_r_a_t_o_r_+_= │ │ │ │ │ +Expression< T > & operator+=(const Expression< T > &e) │ │ │ │ │ +Add another expression to this expression. │ │ │ │ │ +DDeeffiinniittiioonn Expression-inl.h:302 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_t_r_a_c_e_S_i_z_e │ │ │ │ │ +size_t traceSize() const │ │ │ │ │ +Return size needed for memory buffer in traceExecution. │ │ │ │ │ +DDeeffiinniittiioonn Expression-inl.h:167 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_t_r_a_c_e_E_x_e_c_u_t_i_o_n │ │ │ │ │ +T traceExecution(const Values &values, internal::ExecutionTrace< T > &trace, │ │ │ │ │ +void *traceStorage) const │ │ │ │ │ +trace execution, very unsafe │ │ │ │ │ +DDeeffiinniittiioonn Expression-inl.h:198 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_U_n_a_r_y_F_u_n_c_t_i_o_n │ │ │ │ │ +DDeeffiinniittiioonn Expression.h:70 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_B_i_n_a_r_y_F_u_n_c_t_i_o_n │ │ │ │ │ +DDeeffiinniittiioonn Expression.h:76 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_T_e_r_n_a_r_y_F_u_n_c_t_i_o_n │ │ │ │ │ +DDeeffiinniittiioonn Expression.h:83 │ │ │ │ │ +_g_t_s_a_m_:_:_S_c_a_l_a_r_M_u_l_t_i_p_l_y_E_x_p_r_e_s_s_i_o_n │ │ │ │ │ +A ScalarMultiplyExpression is a specialization of Expression that multiplies │ │ │ │ │ +with a scalar It optimiz... │ │ │ │ │ +DDeeffiinniittiioonn Expression.h:213 │ │ │ │ │ +_g_t_s_a_m_:_:_B_i_n_a_r_y_S_u_m_E_x_p_r_e_s_s_i_o_n │ │ │ │ │ +A BinarySumExpression is a specialization of Expression that adds two │ │ │ │ │ +expressions together It optimiz... │ │ │ │ │ +DDeeffiinniittiioonn Expression.h:226 │ │ │ │ │ _g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_a_t │ │ │ │ │ -const ValueType at(Key j) const │ │ │ │ │ -Retrieve a variable by key j. │ │ │ │ │ -DDeeffiinniittiioonn Values-inl.h:361 │ │ │ │ │ +_V_a_l_u_e_s │ │ │ │ │ +In nonlinear factors, the error function returns the negative log-likelihood as │ │ │ │ │ +a non-linear function... │ │ │ │ │ +_S_y_m_b_o_l_._h │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * NNoonnlliinneeaarrEEqquuaalliittyy..hh │ │ │ │ │ + * _E_x_p_r_e_s_s_i_o_n_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01112_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01058_source.html │ │ │ │┄ Files 99% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
27#pragma once
│ │ │ │
28
│ │ │ │ -
29#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
│ │ │ │ -
30#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
│ │ │ │ +
29#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
│ │ │ │ +
30#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
│ │ │ │
31
│ │ │ │
32namespace gtsam {
│ │ │ │
33
│ │ │ │
34/* ************************************************************************* */
│ │ │ │
│ │ │ │
36enum GncLossType {
│ │ │ │
37 GM /*Geman McClure*/,
│ │ │ │ @@ -264,16 +264,16 @@ │ │ │ │
172 baseOptimizerParams.print("Base optimizer params: ");
│ │ │ │
173 }
│ │ │ │
│ │ │ │
174};
│ │ │ │
│ │ │ │
175
│ │ │ │
176}
│ │ │ │ - │ │ │ │ -
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
│ │ │ │ + │ │ │ │ +
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
│ │ │ │
gtsam::FastVector
std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > FastVector
FastVector is a type alias to a std::vector with a custom memory allocator.
Definition FastVector.h:34
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::GncLossType
GncLossType
Choice of robust loss function for GNC.
Definition GncParams.h:36
│ │ │ │
gtsam::GncParams
Definition GncParams.h:42
│ │ │ │
gtsam::GncParams::baseOptimizerParams
BaseOptimizerParameters baseOptimizerParams
GNC parameters.
Definition GncParams.h:67
│ │ │ │
gtsam::GncParams::knownInliers
IndexVector knownInliers
Slots in the factor graph corresponding to measurements that we know are outliers.
Definition GncParams.h:80
│ │ │ │
gtsam::GncParams::GncParams
GncParams()
Default constructor.
Definition GncParams.h:62
│ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01115.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00845.html │ │ │ │┄ Files 86% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Result.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/BinaryJacobianFactor.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
ISAM2Result.h File Reference
│ │ │ │ +
BinaryJacobianFactor.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Class that stores detailed iSAM2 result. │ │ │ │ +

A binary JacobianFactor specialization that uses fixed matrix math for speed. │ │ │ │ More...

│ │ │ │ │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Go to the source code of this file.

│ │ │ │
│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

struct  gtsam::ISAM2Result
 This struct is returned from ISAM2::update() and contains information about the update that is useful for determining whether the solution is converging, and about how much work was required for the update. More...
struct  gtsam::BinaryJacobianFactor< M, N1, N2 >
 A binary JacobianFactor specialization that uses fixed matrix math for speed. More...
 
struct  gtsam::ISAM2Result::DetailedResults
 A struct holding detailed results, which must be enabled with ISAM2Params::enableDetailedResults. More...
 
struct  gtsam::ISAM2Result::DetailedResults::VariableStatus
 The status of a single variable, this struct is stored in DetailedResults::variableStatus. More...
struct  gtsam::traits< BinaryJacobianFactor< M, N1, N2 > >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Class that stores detailed iSAM2 result.

│ │ │ │ -
Author
Michael Kaess, Richard Roberts, Frank Dellaert
│ │ │ │ +

A binary JacobianFactor specialization that uses fixed matrix math for speed.

│ │ │ │ +
Date
June 2015
│ │ │ │ +
Author
Frank Dellaert
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,36 +1,31 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -ISAM2Result.h File Reference │ │ │ │ │ -Class that stores detailed iSAM2 result. _M_o_r_e_._._. │ │ │ │ │ +BinaryJacobianFactor.h File Reference │ │ │ │ │ +A binary _J_a_c_o_b_i_a_n_F_a_c_t_o_r specialization that uses fixed matrix math for speed. │ │ │ │ │ +_M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t │ │ │ │ │ - This struct is returned from _I_S_A_M_2_:_:_u_p_d_a_t_e_(_) and contains information │ │ │ │ │ -  about the update that is useful for determining whether the solution │ │ │ │ │ - is converging, and about how much work was required for the update. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_B_i_n_a_r_y_J_a_c_o_b_i_a_n_F_a_c_t_o_r_<_ _M_,_ _N_1_,_ _N_2_ _> │ │ │ │ │ +  A binary _J_a_c_o_b_i_a_n_F_a_c_t_o_r specialization that uses fixed matrix math for │ │ │ │ │ + speed. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ -struct   _g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s │ │ │ │ │ -  A struct holding detailed results, which must be enabled with │ │ │ │ │ - _I_S_A_M_2_P_a_r_a_m_s_:_:_e_n_a_b_l_e_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s_:_:_V_a_r_i_a_b_l_e_S_t_a_t_u_s │ │ │ │ │ -  The status of a single variable, this struct is stored in │ │ │ │ │ - _D_e_t_a_i_l_e_d_R_e_s_u_l_t_s_:_:_v_a_r_i_a_b_l_e_S_t_a_t_u_s. _M_o_r_e_._._. │ │ │ │ │ +struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _B_i_n_a_r_y_J_a_c_o_b_i_a_n_F_a_c_t_o_r_<_ _M_,_ _N_1_,_ _N_2_ _>_ _> │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Class that stores detailed iSAM2 result. │ │ │ │ │ +A binary _J_a_c_o_b_i_a_n_F_a_c_t_o_r specialization that uses fixed matrix math for speed. │ │ │ │ │ + Date │ │ │ │ │ + June 2015 │ │ │ │ │ Author │ │ │ │ │ - Michael Kaess, Richard Roberts, Frank Dellaert │ │ │ │ │ + Frank Dellaert │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _I_S_A_M_2_R_e_s_u_l_t_._h │ │ │ │ │ + * _l_i_n_e_a_r │ │ │ │ │ + * _B_i_n_a_r_y_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01118_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01184_source.html │ │ │ │┄ Files 92% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearISAM.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/ShonanGaugeFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
NonlinearISAM.h
│ │ │ │ +
ShonanGaugeFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ -
21#include <gtsam/linear/GaussianISAM.h>
│ │ │ │ -
22
│ │ │ │ -
23namespace gtsam {
│ │ │ │ -
│ │ │ │ -
27class GTSAM_EXPORT NonlinearISAM {
│ │ │ │ -
28protected:
│ │ │ │ -
29
│ │ │ │ -
31 gtsam::GaussianISAM isam_;
│ │ │ │ -
32
│ │ │ │ -
34 Values linPoint_;
│ │ │ │ -
35
│ │ │ │ -
37 NonlinearFactorGraph factors_;
│ │ │ │ -
38
│ │ │ │ -
40 int reorderInterval_;
│ │ │ │ -
41 int reorderCounter_;
│ │ │ │ -
42
│ │ │ │ -
44 GaussianFactorGraph::Eliminate eliminationFunction_;
│ │ │ │ -
45
│ │ │ │ -
46public:
│ │ │ │ -
47
│ │ │ │ -
50
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/geometry/SOn.h>
│ │ │ │ +
22#include <gtsam/linear/JacobianFactor.h>
│ │ │ │ +
23#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ +
24
│ │ │ │ +
25namespace gtsam {
│ │ │ │ +
│ │ │ │ +
45class GTSAM_EXPORT ShonanGaugeFactor : public NonlinearFactor {
│ │ │ │ +
46 // Row dimension, equal to the dimensionality of SO(p-d)
│ │ │ │ +
47 size_t rows_;
│ │ │ │ +
48
│ │ │ │ +
50 boost::shared_ptr<JacobianFactor> whitenedJacobian_;
│ │ │ │ +
51
│ │ │ │ +
52public:
│ │ │ │
│ │ │ │ -
58 NonlinearISAM(int reorderInterval = 1,
│ │ │ │ -
59 const GaussianFactorGraph::Eliminate& eliminationFunction = GaussianFactorGraph::EliminationTraitsType::DefaultEliminate) :
│ │ │ │ -
60 reorderInterval_(reorderInterval), reorderCounter_(0), eliminationFunction_(eliminationFunction) {}
│ │ │ │ -
│ │ │ │ -
61
│ │ │ │ -
65
│ │ │ │ -
67 Values estimate() const;
│ │ │ │ +
58 ShonanGaugeFactor(Key key, size_t p, size_t d = 3,
│ │ │ │ +
59 boost::optional<double> gamma = boost::none)
│ │ │ │ +
60 : NonlinearFactor(KeyVector{key}) {
│ │ │ │ +
61 if (p < d) {
│ │ │ │ +
62 throw std::invalid_argument("ShonanGaugeFactor must have p>=d.");
│ │ │ │ +
63 }
│ │ │ │ +
64 // Calculate dimensions
│ │ │ │ +
65 size_t q = p - d;
│ │ │ │ +
66 size_t P = SOn::Dimension(p); // dimensionality of SO(p)
│ │ │ │ +
67 rows_ = SOn::Dimension(q); // dimensionality of SO(q), the gauge
│ │ │ │
68
│ │ │ │ -
70 Matrix marginalCovariance(Key key) const;
│ │ │ │ -
71
│ │ │ │ -
72 // access
│ │ │ │ -
73
│ │ │ │ -
75 const GaussianISAM& bayesTree() const { return isam_; }
│ │ │ │ -
76
│ │ │ │ -
78 const Values& getLinearizationPoint() const { return linPoint_; }
│ │ │ │ -
79
│ │ │ │ -
81 const NonlinearFactorGraph& getFactorsUnsafe() const { return factors_; }
│ │ │ │ -
82
│ │ │ │ -
84 int reorderInterval() const { return reorderInterval_; }
│ │ │ │ -
│ │ │ │ -
85 int reorderCounter() const { return reorderCounter_; }
│ │ │ │ -
86
│ │ │ │ -
88 void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ +
69 // Create constant Jacobian as a rows_*P matrix: there are rows_ penalized
│ │ │ │ +
70 // dimensions, but it is a bit tricky to find them among the P columns.
│ │ │ │ +
71 // The key is to look at how skew-symmetric matrices are laid out in SOn.h:
│ │ │ │ +
72 // the first tangent dimension will always be included, but beyond that we
│ │ │ │ +
73 // have to be careful. We always need to skip the d top-rows of the skew-
│ │ │ │ +
74 // symmetric matrix as they below to K, part of the Stiefel manifold.
│ │ │ │ +
75 Matrix A(rows_, P);
│ │ │ │ +
76 A.setZero();
│ │ │ │ +
77 double invSigma = gamma ? std::sqrt(*gamma) : 1.0;
│ │ │ │ +
78 size_t i = 0, j = 0, n = p - 1 - d;
│ │ │ │ +
79 while (i < rows_) {
│ │ │ │ +
80 A.block(i, j, n, n) = invSigma * Matrix::Identity(n, n);
│ │ │ │ +
81 i += n;
│ │ │ │ +
82 j += n + d; // skip d columns
│ │ │ │ +
83 n -= 1;
│ │ │ │ +
84 }
│ │ │ │ +
85 // TODO(frank): assign the right one in the right columns
│ │ │ │ +
86 whitenedJacobian_ =
│ │ │ │ +
87 boost::make_shared<JacobianFactor>(key, A, Vector::Zero(rows_));
│ │ │ │ +
88 }
│ │ │ │ +
│ │ │ │
89
│ │ │ │ -
91 void printStats() const;
│ │ │ │ +
91 ~ShonanGaugeFactor() override {}
│ │ │ │
92
│ │ │ │ -
94 void saveGraph(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
│ │ │ │ +
94 double error(const Values &c) const override { return 0; }
│ │ │ │
95
│ │ │ │ -
99
│ │ │ │ -
101 void update(const NonlinearFactorGraph& newFactors, const Values& initialValues);
│ │ │ │ -
102
│ │ │ │ -
104 void reorder_relinearize();
│ │ │ │ -
105
│ │ │ │ -
107
│ │ │ │ -
108};
│ │ │ │ -
109
│ │ │ │ -
110} // \namespace gtsam
│ │ │ │ +
97 size_t dim() const override { return rows_; }
│ │ │ │ +
98
│ │ │ │ +
│ │ │ │ +
100 boost::shared_ptr<GaussianFactor> linearize(const Values &c) const override {
│ │ │ │ +
101 return whitenedJacobian_;
│ │ │ │ +
102 }
│ │ │ │
│ │ │ │ +
103};
│ │ │ │
│ │ │ │ - │ │ │ │ -
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │ +
104// \ShonanGaugeFactor
│ │ │ │ +
105
│ │ │ │ +
106} // namespace gtsam
│ │ │ │ +
SOn.h
N*N matrix representation of SO(N).
│ │ │ │ + │ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ -
gtsam::EliminateableFactorGraph< GaussianFactorGraph >::Eliminate
std::function< EliminationResult(const FactorGraphType &, const Ordering &)> Eliminate
The function type that does a single dense elimination step on a subgraph.
Definition EliminateableFactorGraph.h:89
│ │ │ │ -
gtsam::GaussianISAM
Definition GaussianISAM.h:28
│ │ │ │ -
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │ -
gtsam::NonlinearISAM
Wrapper class to manage ISAM in a nonlinear context.
Definition NonlinearISAM.h:27
│ │ │ │ -
gtsam::NonlinearISAM::eliminationFunction_
GaussianFactorGraph::Eliminate eliminationFunction_
The elimination function.
Definition NonlinearISAM.h:44
│ │ │ │ -
gtsam::NonlinearISAM::factors_
NonlinearFactorGraph factors_
The original factors, used when relinearizing.
Definition NonlinearISAM.h:37
│ │ │ │ -
gtsam::NonlinearISAM::bayesTree
const GaussianISAM & bayesTree() const
access the underlying bayes tree
Definition NonlinearISAM.h:75
│ │ │ │ -
gtsam::NonlinearISAM::reorderInterval
int reorderInterval() const
get counters
Definition NonlinearISAM.h:84
│ │ │ │ -
gtsam::NonlinearISAM::NonlinearISAM
NonlinearISAM(int reorderInterval=1, const GaussianFactorGraph::Eliminate &eliminationFunction=GaussianFactorGraph::EliminationTraitsType::DefaultEliminate)
Periodically reorder and relinearize.
Definition NonlinearISAM.h:58
│ │ │ │ -
gtsam::NonlinearISAM::reorderCounter
int reorderCounter() const
TODO: comment.
Definition NonlinearISAM.h:85
│ │ │ │ -
gtsam::NonlinearISAM::getFactorsUnsafe
const NonlinearFactorGraph & getFactorsUnsafe() const
get underlying nonlinear graph
Definition NonlinearISAM.h:81
│ │ │ │ -
gtsam::NonlinearISAM::reorderInterval_
int reorderInterval_
The reordering interval and counter.
Definition NonlinearISAM.h:40
│ │ │ │ -
gtsam::NonlinearISAM::linPoint_
Values linPoint_
The current linearization point.
Definition NonlinearISAM.h:34
│ │ │ │ -
gtsam::NonlinearISAM::isam_
gtsam::GaussianISAM isam_
The internal iSAM object.
Definition NonlinearISAM.h:31
│ │ │ │ -
gtsam::NonlinearISAM::getLinearizationPoint
const Values & getLinearizationPoint() const
Return the current linearization point.
Definition NonlinearISAM.h:78
│ │ │ │ +
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
gtsam::ShonanGaugeFactor
The ShonanGaugeFactor creates a constraint on a single SO(n) to avoid moving in the stabilizer.
Definition ShonanGaugeFactor.h:45
│ │ │ │ +
gtsam::ShonanGaugeFactor::error
double error(const Values &c) const override
Calculate the error of the factor: always zero.
Definition ShonanGaugeFactor.h:94
│ │ │ │ +
gtsam::ShonanGaugeFactor::~ShonanGaugeFactor
~ShonanGaugeFactor() override
Destructor.
Definition ShonanGaugeFactor.h:91
│ │ │ │ +
gtsam::ShonanGaugeFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &c) const override
linearize to a GaussianFactor
Definition ShonanGaugeFactor.h:100
│ │ │ │ +
gtsam::ShonanGaugeFactor::dim
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition ShonanGaugeFactor.h:97
│ │ │ │ +
gtsam::ShonanGaugeFactor::ShonanGaugeFactor
ShonanGaugeFactor(Key key, size_t p, size_t d=3, boost::optional< double > gamma=boost::none)
Construct from key for an SO(p) matrix, for base dimension d (2 or 3) If parameter gamma is given,...
Definition ShonanGaugeFactor.h:58
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,166 +1,132 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -NonlinearISAM.h │ │ │ │ │ +ShonanGaugeFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_I_S_A_M_._h> │ │ │ │ │ -22 │ │ │ │ │ -23namespace _g_t_s_a_m { │ │ │ │ │ -_2_7class GTSAM_EXPORT _N_o_n_l_i_n_e_a_r_I_S_A_M { │ │ │ │ │ -28protected: │ │ │ │ │ -29 │ │ │ │ │ -_3_1 _g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_I_S_A_M _i_s_a_m__; │ │ │ │ │ -32 │ │ │ │ │ -_3_4 _V_a_l_u_e_s _l_i_n_P_o_i_n_t__; │ │ │ │ │ -35 │ │ │ │ │ -_3_7 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h _f_a_c_t_o_r_s__; │ │ │ │ │ -38 │ │ │ │ │ -_4_0 int _r_e_o_r_d_e_r_I_n_t_e_r_v_a_l__; │ │ │ │ │ -41 int reorderCounter_; │ │ │ │ │ -42 │ │ │ │ │ -_4_4 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_E_l_i_m_i_n_a_t_e _e_l_i_m_i_n_a_t_i_o_n_F_u_n_c_t_i_o_n__; │ │ │ │ │ -45 │ │ │ │ │ -46public: │ │ │ │ │ -47 │ │ │ │ │ -50 │ │ │ │ │ -_5_8 _N_o_n_l_i_n_e_a_r_I_S_A_M(int reorderInterval = 1, │ │ │ │ │ -59 const _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_E_l_i_m_i_n_a_t_e& eliminationFunction = │ │ │ │ │ -GaussianFactorGraph::EliminationTraitsType::DefaultEliminate) : │ │ │ │ │ -60 reorderInterval_(reorderInterval), reorderCounter_(0), eliminationFunction_ │ │ │ │ │ -(eliminationFunction) {} │ │ │ │ │ -61 │ │ │ │ │ -65 │ │ │ │ │ -67 _V_a_l_u_e_s estimate() const; │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_S_O_n_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ +24 │ │ │ │ │ +25namespace _g_t_s_a_m { │ │ │ │ │ +_4_5class GTSAM_EXPORT _S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r : public _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r { │ │ │ │ │ +46 // Row dimension, equal to the dimensionality of SO(p-d) │ │ │ │ │ +47 size_t rows_; │ │ │ │ │ +48 │ │ │ │ │ +50 boost::shared_ptr whitenedJacobian_; │ │ │ │ │ +51 │ │ │ │ │ +52public: │ │ │ │ │ +_5_8 _S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r(_K_e_y key, size_t p, size_t d = 3, │ │ │ │ │ +59 boost::optional gamma = boost::none) │ │ │ │ │ +60 : _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r(_K_e_y_V_e_c_t_o_r{key}) { │ │ │ │ │ +61 if (p < d) { │ │ │ │ │ +62 throw std::invalid_argument("ShonanGaugeFactor must have p>=d."); │ │ │ │ │ +63 } │ │ │ │ │ +64 // Calculate dimensions │ │ │ │ │ +65 size_t q = p - d; │ │ │ │ │ +66 size_t P = SOn::Dimension(p); // dimensionality of SO(p) │ │ │ │ │ +67 rows_ = SOn::Dimension(q); // dimensionality of SO(q), the gauge │ │ │ │ │ 68 │ │ │ │ │ -70 Matrix marginalCovariance(_K_e_y key) const; │ │ │ │ │ -71 │ │ │ │ │ -72 // access │ │ │ │ │ -73 │ │ │ │ │ -_7_5 const _G_a_u_s_s_i_a_n_I_S_A_M& _b_a_y_e_s_T_r_e_e() const { return isam_; } │ │ │ │ │ -76 │ │ │ │ │ -_7_8 const _V_a_l_u_e_s& _g_e_t_L_i_n_e_a_r_i_z_a_t_i_o_n_P_o_i_n_t() const { return linPoint_; } │ │ │ │ │ -79 │ │ │ │ │ -_8_1 const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& _g_e_t_F_a_c_t_o_r_s_U_n_s_a_f_e() const { return factors_; } │ │ │ │ │ -82 │ │ │ │ │ -_8_4 int _r_e_o_r_d_e_r_I_n_t_e_r_v_a_l() const { return reorderInterval_; } │ │ │ │ │ -_8_5 int _r_e_o_r_d_e_r_C_o_u_n_t_e_r() const { return reorderCounter_; } │ │ │ │ │ -86 │ │ │ │ │ -88 void _p_r_i_n_t(const std::string& s="", const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ -DefaultKeyFormatter) const; │ │ │ │ │ +69 // Create constant Jacobian as a rows_*P matrix: there are rows_ penalized │ │ │ │ │ +70 // dimensions, but it is a bit tricky to find them among the P columns. │ │ │ │ │ +71 // The key is to look at how skew-symmetric matrices are laid out in SOn.h: │ │ │ │ │ +72 // the first tangent dimension will always be included, but beyond that we │ │ │ │ │ +73 // have to be careful. We always need to skip the d top-rows of the skew- │ │ │ │ │ +74 // symmetric matrix as they below to K, part of the Stiefel manifold. │ │ │ │ │ +75 Matrix A(rows_, P); │ │ │ │ │ +76 A.setZero(); │ │ │ │ │ +77 double invSigma = gamma ? std::sqrt(*gamma) : 1.0; │ │ │ │ │ +78 size_t i = 0, j = 0, n = p - 1 - d; │ │ │ │ │ +79 while (i < rows_) { │ │ │ │ │ +80 A.block(i, j, n, n) = invSigma * Matrix::Identity(n, n); │ │ │ │ │ +81 i += n; │ │ │ │ │ +82 j += n + d; // skip d columns │ │ │ │ │ +83 n -= 1; │ │ │ │ │ +84 } │ │ │ │ │ +85 // TODO(frank): assign the right one in the right columns │ │ │ │ │ +86 whitenedJacobian_ = │ │ │ │ │ +87 boost::make_shared(key, A, Vector::Zero(rows_)); │ │ │ │ │ +88 } │ │ │ │ │ 89 │ │ │ │ │ -91 void printStats() const; │ │ │ │ │ +_9_1 _~_S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r() override {} │ │ │ │ │ 92 │ │ │ │ │ -94 void saveGraph(const std::string& s, const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = │ │ │ │ │ -DefaultKeyFormatter) const; │ │ │ │ │ +_9_4 double _e_r_r_o_r(const _V_a_l_u_e_s &c) const override { return 0; } │ │ │ │ │ 95 │ │ │ │ │ -99 │ │ │ │ │ -101 void update(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& newFactors, const _V_a_l_u_e_s& │ │ │ │ │ -initialValues); │ │ │ │ │ -102 │ │ │ │ │ -104 void reorder_relinearize(); │ │ │ │ │ +_9_7 size_t _d_i_m() const override { return rows_; } │ │ │ │ │ +98 │ │ │ │ │ +_1_0_0 boost::shared_ptr _l_i_n_e_a_r_i_z_e(const _V_a_l_u_e_s &c) const override │ │ │ │ │ +{ │ │ │ │ │ +101 return whitenedJacobian_; │ │ │ │ │ +102 } │ │ │ │ │ +103}; │ │ │ │ │ +104// \ShonanGaugeFactor │ │ │ │ │ 105 │ │ │ │ │ -107 │ │ │ │ │ -108}; │ │ │ │ │ -109 │ │ │ │ │ -110} // \namespace gtsam │ │ │ │ │ -_G_a_u_s_s_i_a_n_I_S_A_M_._h │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Factor Graph consisting of non-linear factors. │ │ │ │ │ +106} // namespace gtsam │ │ │ │ │ +_S_O_n_._h │ │ │ │ │ +N*N matrix representation of SO(N). │ │ │ │ │ +_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ +Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ -std::function< std::string(Key)> KeyFormatter │ │ │ │ │ -Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_ _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_E_l_i_m_i_n_a_t_e │ │ │ │ │ -std::function< EliminationResult(const FactorGraphType &, const Ordering &)> │ │ │ │ │ -Eliminate │ │ │ │ │ -The function type that does a single dense elimination step on a subgraph. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph.h:89 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_I_S_A_M │ │ │ │ │ -DDeeffiinniittiioonn GaussianISAM.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_I_S_A_M │ │ │ │ │ -Wrapper class to manage ISAM in a nonlinear context. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearISAM.h:27 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_e_l_i_m_i_n_a_t_i_o_n_F_u_n_c_t_i_o_n__ │ │ │ │ │ -GaussianFactorGraph::Eliminate eliminationFunction_ │ │ │ │ │ -The elimination function. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearISAM.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_f_a_c_t_o_r_s__ │ │ │ │ │ -NonlinearFactorGraph factors_ │ │ │ │ │ -The original factors, used when relinearizing. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearISAM.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_b_a_y_e_s_T_r_e_e │ │ │ │ │ -const GaussianISAM & bayesTree() const │ │ │ │ │ -access the underlying bayes tree │ │ │ │ │ -DDeeffiinniittiioonn NonlinearISAM.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_r_e_o_r_d_e_r_I_n_t_e_r_v_a_l │ │ │ │ │ -int reorderInterval() const │ │ │ │ │ -get counters │ │ │ │ │ -DDeeffiinniittiioonn NonlinearISAM.h:84 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_N_o_n_l_i_n_e_a_r_I_S_A_M │ │ │ │ │ -NonlinearISAM(int reorderInterval=1, const GaussianFactorGraph::Eliminate │ │ │ │ │ -&eliminationFunction=GaussianFactorGraph::EliminationTraitsType:: │ │ │ │ │ -DefaultEliminate) │ │ │ │ │ -Periodically reorder and relinearize. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearISAM.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_r_e_o_r_d_e_r_C_o_u_n_t_e_r │ │ │ │ │ -int reorderCounter() const │ │ │ │ │ -TODO: comment. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearISAM.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_g_e_t_F_a_c_t_o_r_s_U_n_s_a_f_e │ │ │ │ │ -const NonlinearFactorGraph & getFactorsUnsafe() const │ │ │ │ │ -get underlying nonlinear graph │ │ │ │ │ -DDeeffiinniittiioonn NonlinearISAM.h:81 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_r_e_o_r_d_e_r_I_n_t_e_r_v_a_l__ │ │ │ │ │ -int reorderInterval_ │ │ │ │ │ -The reordering interval and counter. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearISAM.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_l_i_n_P_o_i_n_t__ │ │ │ │ │ -Values linPoint_ │ │ │ │ │ -The current linearization point. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearISAM.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_i_s_a_m__ │ │ │ │ │ -gtsam::GaussianISAM isam_ │ │ │ │ │ -The internal iSAM object. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearISAM.h:31 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_I_S_A_M_:_:_g_e_t_L_i_n_e_a_r_i_z_a_t_i_o_n_P_o_i_n_t │ │ │ │ │ -const Values & getLinearizationPoint() const │ │ │ │ │ -Return the current linearization point. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearISAM.h:78 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ +Nonlinear factor base class. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ _g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r │ │ │ │ │ +The ShonanGaugeFactor creates a constraint on a single SO(n) to avoid moving in │ │ │ │ │ +the stabilizer. │ │ │ │ │ +DDeeffiinniittiioonn ShonanGaugeFactor.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r_:_:_e_r_r_o_r │ │ │ │ │ +double error(const Values &c) const override │ │ │ │ │ +Calculate the error of the factor: always zero. │ │ │ │ │ +DDeeffiinniittiioonn ShonanGaugeFactor.h:94 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r_:_:_~_S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r │ │ │ │ │ +~ShonanGaugeFactor() override │ │ │ │ │ +Destructor. │ │ │ │ │ +DDeeffiinniittiioonn ShonanGaugeFactor.h:91 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e │ │ │ │ │ +boost::shared_ptr< GaussianFactor > linearize(const Values &c) const override │ │ │ │ │ +linearize to a GaussianFactor │ │ │ │ │ +DDeeffiinniittiioonn ShonanGaugeFactor.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r_:_:_d_i_m │ │ │ │ │ +size_t dim() const override │ │ │ │ │ +get the dimension of the factor (number of rows on linearization) │ │ │ │ │ +DDeeffiinniittiioonn ShonanGaugeFactor.h:97 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r_:_:_S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r │ │ │ │ │ +ShonanGaugeFactor(Key key, size_t p, size_t d=3, boost::optional< double > │ │ │ │ │ +gamma=boost::none) │ │ │ │ │ +Construct from key for an SO(p) matrix, for base dimension d (2 or 3) If │ │ │ │ │ +parameter gamma is given,... │ │ │ │ │ +DDeeffiinniittiioonn ShonanGaugeFactor.h:58 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _N_o_n_l_i_n_e_a_r_I_S_A_M_._h │ │ │ │ │ + * _s_f_m │ │ │ │ │ + * _S_h_o_n_a_n_G_a_u_g_e_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01124_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00911_source.html │ │ │ │┄ Files 74% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/DoglegOptimizerImpl.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/BarometricFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
DoglegOptimizerImpl.h
│ │ │ │ +
BarometricFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
17#pragma once
│ │ │ │ -
18
│ │ │ │ -
19#include <iomanip>
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/linear/VectorValues.h>
│ │ │ │ -
22#include <gtsam/inference/Ordering.h>
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/geometry/Pose3.h>
│ │ │ │ +
21#include <gtsam/navigation/NavState.h>
│ │ │ │ +
22#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │
23
│ │ │ │
24namespace gtsam {
│ │ │ │
25
│ │ │ │ -
│ │ │ │ -
32struct GTSAM_EXPORT DoglegOptimizerImpl {
│ │ │ │ -
33
│ │ │ │
│ │ │ │ -
34 struct GTSAM_EXPORT IterationResult {
│ │ │ │ -
35 double delta;
│ │ │ │ -
36 VectorValues dx_d;
│ │ │ │ -
37 double f_error;
│ │ │ │ -
38 };
│ │ │ │ -
│ │ │ │ +
34class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN<Pose3, double> {
│ │ │ │ +
35 private:
│ │ │ │ +
36 typedef NoiseModelFactorN<Pose3, double> Base;
│ │ │ │ +
37
│ │ │ │ +
38 double nT_;
│ │ │ │
39
│ │ │ │ -
│ │ │ │ -
53 enum TrustRegionAdaptationMode {
│ │ │ │ -
54 SEARCH_EACH_ITERATION,
│ │ │ │ -
55 SEARCH_REDUCE_ONLY,
│ │ │ │ -
56 ONE_STEP_PER_ITERATION
│ │ │ │ -
57 };
│ │ │ │ -
│ │ │ │ -
58
│ │ │ │ -
94 template<class M, class F, class VALUES>
│ │ │ │ -
95 static IterationResult Iterate(
│ │ │ │ -
96 double delta, TrustRegionAdaptationMode mode, const VectorValues& dx_u, const VectorValues& dx_n,
│ │ │ │ -
97 const M& Rd, const F& f, const VALUES& x0, const double f_error, const bool verbose=false);
│ │ │ │ -
98
│ │ │ │ -
121 static VectorValues ComputeDoglegPoint(double delta, const VectorValues& dx_u, const VectorValues& dx_n, const bool verbose=false);
│ │ │ │ -
122
│ │ │ │ -
132 static VectorValues ComputeBlend(double delta, const VectorValues& x_u, const VectorValues& x_n, const bool verbose=false);
│ │ │ │ -
133};
│ │ │ │ -
│ │ │ │ -
134
│ │ │ │ -
135
│ │ │ │ -
136/* ************************************************************************* */
│ │ │ │ -
137template<class M, class F, class VALUES>
│ │ │ │ -
│ │ │ │ -
138typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
│ │ │ │ -
139 double delta, TrustRegionAdaptationMode mode, const VectorValues& dx_u, const VectorValues& dx_n,
│ │ │ │ -
140 const M& Rd, const F& f, const VALUES& x0, const double f_error, const bool verbose)
│ │ │ │ -
141{
│ │ │ │ -
142 gttic(M_error);
│ │ │ │ -
143 const double M_error = Rd.error(VectorValues::Zero(dx_u));
│ │ │ │ -
144 gttoc(M_error);
│ │ │ │ -
145
│ │ │ │ -
146 // Result to return
│ │ │ │ -
147 IterationResult result;
│ │ │ │ -
148
│ │ │ │ -
149 bool stay = true;
│ │ │ │ -
150 enum { NONE, INCREASED_DELTA, DECREASED_DELTA } lastAction = NONE; // Used to prevent alternating between increasing and decreasing in one iteration
│ │ │ │ -
151 while(stay) {
│ │ │ │ -
152 gttic(Dog_leg_point);
│ │ │ │ -
153 // Compute dog leg point
│ │ │ │ -
154 result.dx_d = ComputeDoglegPoint(delta, dx_u, dx_n, verbose);
│ │ │ │ -
155 gttoc(Dog_leg_point);
│ │ │ │ -
156
│ │ │ │ -
157 if(verbose) std::cout << "delta = " << delta << ", dx_d_norm = " << result.dx_d.norm() << std::endl;
│ │ │ │ -
158
│ │ │ │ -
159 gttic(retract);
│ │ │ │ -
160 // Compute expmapped solution
│ │ │ │ -
161 const VALUES x_d(x0.retract(result.dx_d));
│ │ │ │ -
162 gttoc(retract);
│ │ │ │ -
163
│ │ │ │ -
164 gttic(decrease_in_f);
│ │ │ │ -
165 // Compute decrease in f
│ │ │ │ -
166 result.f_error = f.error(x_d);
│ │ │ │ -
167 gttoc(decrease_in_f);
│ │ │ │ -
168
│ │ │ │ -
169 gttic(new_M_error);
│ │ │ │ -
170 // Compute decrease in M
│ │ │ │ -
171 const double new_M_error = Rd.error(result.dx_d);
│ │ │ │ -
172 gttoc(new_M_error);
│ │ │ │ -
173
│ │ │ │ -
174 if(verbose) std::cout << std::setprecision(15) << "f error: " << f_error << " -> " << result.f_error << std::endl;
│ │ │ │ -
175 if(verbose) std::cout << std::setprecision(15) << "M error: " << M_error << " -> " << new_M_error << std::endl;
│ │ │ │ -
176
│ │ │ │ -
177 gttic(adjust_delta);
│ │ │ │ -
178 // Compute gain ratio. Here we take advantage of the invariant that the
│ │ │ │ -
179 // Bayes' net error at zero is equal to the nonlinear error
│ │ │ │ -
180 const double rho = std::abs(f_error - result.f_error) < 1e-15 || std::abs(M_error - new_M_error) < 1e-15 ?
│ │ │ │ -
181 0.5 :
│ │ │ │ -
182 (f_error - result.f_error) / (M_error - new_M_error);
│ │ │ │ -
183
│ │ │ │ -
184 if(verbose) std::cout << std::setprecision(15) << "rho = " << rho << std::endl;
│ │ │ │ -
185
│ │ │ │ -
186 if(rho >= 0.75) {
│ │ │ │ -
187 // M agrees very well with f, so try to increase lambda
│ │ │ │ -
188 const double dx_d_norm = result.dx_d.norm();
│ │ │ │ -
189 const double newDelta = std::max(delta, 3.0 * dx_d_norm); // Compute new delta
│ │ │ │ -
190
│ │ │ │ -
191 if(mode == ONE_STEP_PER_ITERATION || mode == SEARCH_REDUCE_ONLY)
│ │ │ │ -
192 stay = false; // If not searching, just return with the new delta
│ │ │ │ -
193 else if(mode == SEARCH_EACH_ITERATION) {
│ │ │ │ -
194 if(std::abs(newDelta - delta) < 1e-15 || lastAction == DECREASED_DELTA)
│ │ │ │ -
195 stay = false; // Searching, but Newton's solution is within trust region so keep the same trust region
│ │ │ │ -
196 else {
│ │ │ │ -
197 stay = true; // Searching and increased delta, so try again to increase delta
│ │ │ │ -
198 lastAction = INCREASED_DELTA;
│ │ │ │ -
199 }
│ │ │ │ -
200 } else {
│ │ │ │ -
201 assert(false); }
│ │ │ │ -
202
│ │ │ │ -
203 delta = newDelta; // Update delta from new delta
│ │ │ │ -
204
│ │ │ │ -
205 } else if(0.75 > rho && rho >= 0.25) {
│ │ │ │ -
206 // M agrees so-so with f, keep the same delta
│ │ │ │ -
207 stay = false;
│ │ │ │ -
208
│ │ │ │ -
209 } else if(0.25 > rho && rho >= 0.0) {
│ │ │ │ -
210 // M does not agree well with f, decrease delta until it does
│ │ │ │ -
211 double newDelta;
│ │ │ │ -
212 bool hitMinimumDelta;
│ │ │ │ -
213 if(delta > 1e-5) {
│ │ │ │ -
214 newDelta = 0.5 * delta;
│ │ │ │ -
215 hitMinimumDelta = false;
│ │ │ │ -
216 } else {
│ │ │ │ -
217 newDelta = delta;
│ │ │ │ -
218 hitMinimumDelta = true;
│ │ │ │ -
219 }
│ │ │ │ -
220 if(mode == ONE_STEP_PER_ITERATION || /* mode == SEARCH_EACH_ITERATION && */ lastAction == INCREASED_DELTA || hitMinimumDelta)
│ │ │ │ -
221 stay = false; // If not searching, just return with the new smaller delta
│ │ │ │ -
222 else if(mode == SEARCH_EACH_ITERATION || mode == SEARCH_REDUCE_ONLY) {
│ │ │ │ -
223 stay = true;
│ │ │ │ -
224 lastAction = DECREASED_DELTA;
│ │ │ │ -
225 } else {
│ │ │ │ -
226 assert(false); }
│ │ │ │ -
227
│ │ │ │ -
228 delta = newDelta; // Update delta from new delta
│ │ │ │ -
229
│ │ │ │ -
230 } else {
│ │ │ │ -
231 // f actually increased, so keep decreasing delta until f does not decrease.
│ │ │ │ -
232 // NOTE: NaN and Inf solutions also will fall into this case, so that we
│ │ │ │ -
233 // decrease delta if the solution becomes undetermined.
│ │ │ │ -
234 assert(0.0 > rho);
│ │ │ │ -
235 if(delta > 1e-5) {
│ │ │ │ -
236 delta *= 0.5;
│ │ │ │ -
237 stay = true;
│ │ │ │ -
238 lastAction = DECREASED_DELTA;
│ │ │ │ -
239 } else {
│ │ │ │ -
240 if(verbose) std::cout << "Warning: Dog leg stopping because cannot decrease error with minimum delta" << std::endl;
│ │ │ │ -
241 result.dx_d.setZero(); // Set delta to zero - don't allow error to increase
│ │ │ │ -
242 result.f_error = f_error;
│ │ │ │ -
243 stay = false;
│ │ │ │ -
244 }
│ │ │ │ -
245 }
│ │ │ │ -
246 gttoc(adjust_delta);
│ │ │ │ -
247 }
│ │ │ │ -
248
│ │ │ │ -
249 // dx_d and f_error have already been filled in during the loop
│ │ │ │ -
250 result.delta = delta;
│ │ │ │ -
251 return result;
│ │ │ │ -
252}
│ │ │ │ -
│ │ │ │ -
253
│ │ │ │ -
254}
│ │ │ │ -
Ordering.h
Variable ordering for the elimination algorithm.
│ │ │ │ -
VectorValues.h
Factor Graph Values.
│ │ │ │ +
40 public:
│ │ │ │ +
42 typedef boost::shared_ptr<BarometricFactor> shared_ptr;
│ │ │ │ +
43
│ │ │ │ +
45 typedef BarometricFactor This;
│ │ │ │ +
46
│ │ │ │ +
48 BarometricFactor() : nT_(0) {}
│ │ │ │ +
49
│ │ │ │ +
50 ~BarometricFactor() override {}
│ │ │ │ +
51
│ │ │ │ +
│ │ │ │ +
59 BarometricFactor(Key key, Key baroKey, const double& baroIn,
│ │ │ │ +
60 const SharedNoiseModel& model)
│ │ │ │ +
61 : Base(model, key, baroKey), nT_(heightOut(baroIn)) {}
│ │ │ │ +
│ │ │ │ +
62
│ │ │ │ +
│ │ │ │ +
64 gtsam::NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ +
65 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
│ │ │ │ +
66 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
│ │ │ │ +
67 }
│ │ │ │ +
│ │ │ │ +
68
│ │ │ │ +
70 void print(
│ │ │ │ +
71 const std::string& s = "",
│ │ │ │ +
72 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
│ │ │ │ +
73
│ │ │ │ +
75 bool equals(const NonlinearFactor& expected,
│ │ │ │ +
76 double tol = 1e-9) const override;
│ │ │ │ +
77
│ │ │ │ +
79 Vector evaluateError(
│ │ │ │ +
80 const Pose3& p, const double& b,
│ │ │ │ +
81 boost::optional<Matrix&> H = boost::none,
│ │ │ │ +
82 boost::optional<Matrix&> H2 = boost::none) const override;
│ │ │ │ +
83
│ │ │ │ +
84 inline const double& measurementIn() const { return nT_; }
│ │ │ │ +
85
│ │ │ │ +
86 inline double heightOut(double n) const {
│ │ │ │ +
87 // From https://www.grc.nasa.gov/www/k-12/airplane/atmosmet.html
│ │ │ │ +
88 return (std::pow(n / 101.29, 1. / 5.256) * 288.08 - 273.1 - 15.04) /
│ │ │ │ +
89 -0.00649;
│ │ │ │ +
90 };
│ │ │ │ +
91
│ │ │ │ +
92 inline double baroOut(const double& meters) {
│ │ │ │ +
93 double temp = 15.04 - 0.00649 * meters;
│ │ │ │ +
94 return 101.29 * std::pow(((temp + 273.1) / 288.08), 5.256);
│ │ │ │ +
95 };
│ │ │ │ +
96
│ │ │ │ +
97 private:
│ │ │ │ +
99 friend class boost::serialization::access;
│ │ │ │ +
100 template <class ARCHIVE>
│ │ │ │ +
101 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ +
102 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ +
103 ar& boost::serialization::make_nvp(
│ │ │ │ +
104 "NoiseModelFactor1",
│ │ │ │ +
105 boost::serialization::base_object<Base>(*this));
│ │ │ │ +
106 ar& BOOST_SERIALIZATION_NVP(nT_);
│ │ │ │ +
107 }
│ │ │ │ +
108};
│ │ │ │ +
│ │ │ │ +
109
│ │ │ │ +
110} // namespace gtsam
│ │ │ │ +
Pose3.h
3D Pose
│ │ │ │ +
NavState.h
Navigation state composing of attitude, position, and velocity.
│ │ │ │ +
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ -
gtsam::VectorValues::Zero
static VectorValues Zero(const VectorValues &other)
Create a VectorValues with the same structure as other, but filled with zeros.
Definition VectorValues.cpp:78
│ │ │ │ -
gtsam::DoglegOptimizerImpl
This class contains the implementation of the Dogleg algorithm.
Definition DoglegOptimizerImpl.h:32
│ │ │ │ -
gtsam::DoglegOptimizerImpl::TrustRegionAdaptationMode
TrustRegionAdaptationMode
Specifies how the trust region is adapted at each Dogleg iteration.
Definition DoglegOptimizerImpl.h:53
│ │ │ │ -
gtsam::DoglegOptimizerImpl::Iterate
static IterationResult Iterate(double delta, TrustRegionAdaptationMode mode, const VectorValues &dx_u, const VectorValues &dx_n, const M &Rd, const F &f, const VALUES &x0, const double f_error, const bool verbose=false)
Compute the update point for one iteration of the Dogleg algorithm, given an initial trust region rad...
Definition DoglegOptimizerImpl.h:138
│ │ │ │ -
gtsam::DoglegOptimizerImpl::ComputeDoglegPoint
static VectorValues ComputeDoglegPoint(double delta, const VectorValues &dx_u, const VectorValues &dx_n, const bool verbose=false)
Compute the dogleg point given a trust region radius .
Definition DoglegOptimizerImpl.cpp:25
│ │ │ │ -
gtsam::DoglegOptimizerImpl::IterationResult
Definition DoglegOptimizerImpl.h:34
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ +
gtsam::BarometricFactor
Prior on height in a cartesian frame.
Definition BarometricFactor.h:34
│ │ │ │ +
gtsam::BarometricFactor::This
BarometricFactor This
Typedef to this class.
Definition BarometricFactor.h:45
│ │ │ │ +
gtsam::BarometricFactor::BarometricFactor
BarometricFactor(Key key, Key baroKey, const double &baroIn, const SharedNoiseModel &model)
Constructor from a measurement of pressure in KPa.
Definition BarometricFactor.h:59
│ │ │ │ +
gtsam::BarometricFactor::shared_ptr
boost::shared_ptr< BarometricFactor > shared_ptr
shorthand for a smart pointer to a factor
Definition BarometricFactor.h:42
│ │ │ │ +
gtsam::BarometricFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition BarometricFactor.h:64
│ │ │ │ +
gtsam::BarometricFactor::BarometricFactor
BarometricFactor()
default constructor - only use for serialization
Definition BarometricFactor.h:48
│ │ │ │ +
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ +
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,232 +1,157 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -DoglegOptimizerImpl.h │ │ │ │ │ +BarometricFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -17#pragma once │ │ │ │ │ -18 │ │ │ │ │ -19#include │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_O_r_d_e_r_i_n_g_._h> │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_s_e_3_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_N_a_v_S_t_a_t_e_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ 23 │ │ │ │ │ 24namespace _g_t_s_a_m { │ │ │ │ │ 25 │ │ │ │ │ -_3_2struct GTSAM_EXPORT _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l { │ │ │ │ │ -33 │ │ │ │ │ -_3_4 struct GTSAM_EXPORT _I_t_e_r_a_t_i_o_n_R_e_s_u_l_t { │ │ │ │ │ -35 double delta; │ │ │ │ │ -36 _V_e_c_t_o_r_V_a_l_u_e_s dx_d; │ │ │ │ │ -37 double f_error; │ │ │ │ │ -38 }; │ │ │ │ │ +_3_4class GTSAM_EXPORT _B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ +{ │ │ │ │ │ +35 private: │ │ │ │ │ +36 typedef _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_P_o_s_e_3_,_ _d_o_u_b_l_e_> _B_a_s_e; │ │ │ │ │ +37 │ │ │ │ │ +38 double nT_; │ │ │ │ │ 39 │ │ │ │ │ -_5_3 enum _T_r_u_s_t_R_e_g_i_o_n_A_d_a_p_t_a_t_i_o_n_M_o_d_e { │ │ │ │ │ -54 SEARCH_EACH_ITERATION, │ │ │ │ │ -55 SEARCH_REDUCE_ONLY, │ │ │ │ │ -56 ONE_STEP_PER_ITERATION │ │ │ │ │ -57 }; │ │ │ │ │ -58 │ │ │ │ │ -94 template │ │ │ │ │ -95 static IterationResult Iterate( │ │ │ │ │ -96 double delta, TrustRegionAdaptationMode mode, const _V_e_c_t_o_r_V_a_l_u_e_s& dx_u, │ │ │ │ │ -const _V_e_c_t_o_r_V_a_l_u_e_s& dx_n, │ │ │ │ │ -97 const M& Rd, const F& f, const VALUES& x0, const double f_error, const bool │ │ │ │ │ -verbose=false); │ │ │ │ │ -98 │ │ │ │ │ -121 static _V_e_c_t_o_r_V_a_l_u_e_s ComputeDoglegPoint(double delta, const _V_e_c_t_o_r_V_a_l_u_e_s& │ │ │ │ │ -dx_u, const _V_e_c_t_o_r_V_a_l_u_e_s& dx_n, const bool verbose=false); │ │ │ │ │ -122 │ │ │ │ │ -132 static _V_e_c_t_o_r_V_a_l_u_e_s ComputeBlend(double delta, const _V_e_c_t_o_r_V_a_l_u_e_s& x_u, │ │ │ │ │ -const _V_e_c_t_o_r_V_a_l_u_e_s& x_n, const bool verbose=false); │ │ │ │ │ -133}; │ │ │ │ │ -134 │ │ │ │ │ -135 │ │ │ │ │ -136/* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -137template │ │ │ │ │ -_1_3_8typename _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_:_:_I_t_e_r_a_t_i_o_n_R_e_s_u_l_t _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_:_:_I_t_e_r_a_t_e( │ │ │ │ │ -139 double delta, _T_r_u_s_t_R_e_g_i_o_n_A_d_a_p_t_a_t_i_o_n_M_o_d_e mode, const _V_e_c_t_o_r_V_a_l_u_e_s& dx_u, │ │ │ │ │ -const _V_e_c_t_o_r_V_a_l_u_e_s& dx_n, │ │ │ │ │ -140 const M& Rd, const F& f, const VALUES& x0, const double f_error, const bool │ │ │ │ │ -verbose) │ │ │ │ │ -141{ │ │ │ │ │ -142 gttic(M_error); │ │ │ │ │ -143 const double M_error = Rd.error(_V_e_c_t_o_r_V_a_l_u_e_s_:_:_Z_e_r_o(dx_u)); │ │ │ │ │ -144 gttoc(M_error); │ │ │ │ │ -145 │ │ │ │ │ -146 // Result to return │ │ │ │ │ -147 _I_t_e_r_a_t_i_o_n_R_e_s_u_l_t result; │ │ │ │ │ -148 │ │ │ │ │ -149 bool stay = true; │ │ │ │ │ -150 enum { NONE, INCREASED_DELTA, DECREASED_DELTA } lastAction = NONE; // Used │ │ │ │ │ -to prevent alternating between increasing and decreasing in one iteration │ │ │ │ │ -151 while(stay) { │ │ │ │ │ -152 gttic(Dog_leg_point); │ │ │ │ │ -153 // Compute dog leg point │ │ │ │ │ -154 result.dx_d = _C_o_m_p_u_t_e_D_o_g_l_e_g_P_o_i_n_t(delta, dx_u, dx_n, verbose); │ │ │ │ │ -155 gttoc(Dog_leg_point); │ │ │ │ │ -156 │ │ │ │ │ -157 if(verbose) std::cout << "delta = " << delta << ", dx_d_norm = " << │ │ │ │ │ -result.dx_d.norm() << std::endl; │ │ │ │ │ -158 │ │ │ │ │ -159 gttic(retract); │ │ │ │ │ -160 // Compute expmapped solution │ │ │ │ │ -161 const VALUES x_d(x0.retract(result.dx_d)); │ │ │ │ │ -162 gttoc(retract); │ │ │ │ │ -163 │ │ │ │ │ -164 gttic(decrease_in_f); │ │ │ │ │ -165 // Compute decrease in f │ │ │ │ │ -166 result.f_error = f.error(x_d); │ │ │ │ │ -167 gttoc(decrease_in_f); │ │ │ │ │ -168 │ │ │ │ │ -169 gttic(new_M_error); │ │ │ │ │ -170 // Compute decrease in M │ │ │ │ │ -171 const double new_M_error = Rd.error(result.dx_d); │ │ │ │ │ -172 gttoc(new_M_error); │ │ │ │ │ -173 │ │ │ │ │ -174 if(verbose) std::cout << std::setprecision(15) << "f error: " << f_error << │ │ │ │ │ -" -> " << result.f_error << std::endl; │ │ │ │ │ -175 if(verbose) std::cout << std::setprecision(15) << "M error: " << M_error << │ │ │ │ │ -" -> " << new_M_error << std::endl; │ │ │ │ │ -176 │ │ │ │ │ -177 gttic(adjust_delta); │ │ │ │ │ -178 // Compute gain ratio. Here we take advantage of the invariant that the │ │ │ │ │ -179 // Bayes' net error at zero is equal to the nonlinear error │ │ │ │ │ -180 const double rho = std::abs(f_error - result.f_error) < 1e-15 || std::abs │ │ │ │ │ -(M_error - new_M_error) < 1e-15 ? │ │ │ │ │ -181 0.5 : │ │ │ │ │ -182 (f_error - result.f_error) / (M_error - new_M_error); │ │ │ │ │ -183 │ │ │ │ │ -184 if(verbose) std::cout << std::setprecision(15) << "rho = " << rho << std:: │ │ │ │ │ -endl; │ │ │ │ │ -185 │ │ │ │ │ -186 if(rho >= 0.75) { │ │ │ │ │ -187 // M agrees very well with f, so try to increase lambda │ │ │ │ │ -188 const double dx_d_norm = result.dx_d.norm(); │ │ │ │ │ -189 const double newDelta = std::max(delta, 3.0 * dx_d_norm); // Compute new │ │ │ │ │ -delta │ │ │ │ │ -190 │ │ │ │ │ -191 if(mode == ONE_STEP_PER_ITERATION || mode == SEARCH_REDUCE_ONLY) │ │ │ │ │ -192 stay = false; // If not searching, just return with the new delta │ │ │ │ │ -193 else if(mode == SEARCH_EACH_ITERATION) { │ │ │ │ │ -194 if(std::abs(newDelta - delta) < 1e-15 || lastAction == DECREASED_DELTA) │ │ │ │ │ -195 stay = false; // Searching, but Newton's solution is within trust region so │ │ │ │ │ -keep the same trust region │ │ │ │ │ -196 else { │ │ │ │ │ -197 stay = true; // Searching and increased delta, so try again to increase │ │ │ │ │ -delta │ │ │ │ │ -198 lastAction = INCREASED_DELTA; │ │ │ │ │ -199 } │ │ │ │ │ -200 } else { │ │ │ │ │ -201 assert(false); } │ │ │ │ │ -202 │ │ │ │ │ -203 delta = newDelta; // Update delta from new delta │ │ │ │ │ -204 │ │ │ │ │ -205 } else if(0.75 > rho && rho >= 0.25) { │ │ │ │ │ -206 // M agrees so-so with f, keep the same delta │ │ │ │ │ -207 stay = false; │ │ │ │ │ -208 │ │ │ │ │ -209 } else if(0.25 > rho && rho >= 0.0) { │ │ │ │ │ -210 // M does not agree well with f, decrease delta until it does │ │ │ │ │ -211 double newDelta; │ │ │ │ │ -212 bool hitMinimumDelta; │ │ │ │ │ -213 if(delta > 1e-5) { │ │ │ │ │ -214 newDelta = 0.5 * delta; │ │ │ │ │ -215 hitMinimumDelta = false; │ │ │ │ │ -216 } else { │ │ │ │ │ -217 newDelta = delta; │ │ │ │ │ -218 hitMinimumDelta = true; │ │ │ │ │ -219 } │ │ │ │ │ -220 if(mode == ONE_STEP_PER_ITERATION || /* mode == SEARCH_EACH_ITERATION && */ │ │ │ │ │ -lastAction == INCREASED_DELTA || hitMinimumDelta) │ │ │ │ │ -221 stay = false; // If not searching, just return with the new smaller delta │ │ │ │ │ -222 else if(mode == SEARCH_EACH_ITERATION || mode == SEARCH_REDUCE_ONLY) { │ │ │ │ │ -223 stay = true; │ │ │ │ │ -224 lastAction = DECREASED_DELTA; │ │ │ │ │ -225 } else { │ │ │ │ │ -226 assert(false); } │ │ │ │ │ -227 │ │ │ │ │ -228 delta = newDelta; // Update delta from new delta │ │ │ │ │ -229 │ │ │ │ │ -230 } else { │ │ │ │ │ -231 // f actually increased, so keep decreasing delta until f does not │ │ │ │ │ -decrease. │ │ │ │ │ -232 // NOTE: NaN and Inf solutions also will fall into this case, so that we │ │ │ │ │ -233 // decrease delta if the solution becomes undetermined. │ │ │ │ │ -234 assert(0.0 > rho); │ │ │ │ │ -235 if(delta > 1e-5) { │ │ │ │ │ -236 delta *= 0.5; │ │ │ │ │ -237 stay = true; │ │ │ │ │ -238 lastAction = DECREASED_DELTA; │ │ │ │ │ -239 } else { │ │ │ │ │ -240 if(verbose) std::cout << "Warning: Dog leg stopping because cannot decrease │ │ │ │ │ -error with minimum delta" << std::endl; │ │ │ │ │ -241 result.dx_d.setZero(); // Set delta to zero - don't allow error to increase │ │ │ │ │ -242 result.f_error = f_error; │ │ │ │ │ -243 stay = false; │ │ │ │ │ -244 } │ │ │ │ │ -245 } │ │ │ │ │ -246 gttoc(adjust_delta); │ │ │ │ │ -247 } │ │ │ │ │ -248 │ │ │ │ │ -249 // dx_d and f_error have already been filled in during the loop │ │ │ │ │ -250 result.delta = delta; │ │ │ │ │ -251 return result; │ │ │ │ │ -252} │ │ │ │ │ -253 │ │ │ │ │ -254} │ │ │ │ │ -_O_r_d_e_r_i_n_g_._h │ │ │ │ │ -Variable ordering for the elimination algorithm. │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ -Factor Graph Values. │ │ │ │ │ +40 public: │ │ │ │ │ +_4_2 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +43 │ │ │ │ │ +_4_5 typedef _B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ +46 │ │ │ │ │ +_4_8 _B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r() : nT_(0) {} │ │ │ │ │ +49 │ │ │ │ │ +50 _~_B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r() override {} │ │ │ │ │ +51 │ │ │ │ │ +_5_9 _B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r(_K_e_y key, _K_e_y baroKey, const double& baroIn, │ │ │ │ │ +60 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l& model) │ │ │ │ │ +61 : _B_a_s_e(model, key, baroKey), nT_(heightOut(baroIn)) {} │ │ │ │ │ +62 │ │ │ │ │ +_6_4 gtsam::NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +65 return boost::static_pointer_cast( │ │ │ │ │ +66 gtsam::NonlinearFactor::shared_ptr(new _T_h_i_s(*this))); │ │ │ │ │ +67 } │ │ │ │ │ +68 │ │ │ │ │ +70 void _p_r_i_n_t( │ │ │ │ │ +71 const std::string& s = "", │ │ │ │ │ +72 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const override; │ │ │ │ │ +73 │ │ │ │ │ +75 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r& expected, │ │ │ │ │ +76 double tol = 1e-9) const override; │ │ │ │ │ +77 │ │ │ │ │ +79 Vector evaluateError( │ │ │ │ │ +80 const _P_o_s_e_3& p, const double& b, │ │ │ │ │ +81 boost::optional H = boost::none, │ │ │ │ │ +82 boost::optional H2 = boost::none) const override; │ │ │ │ │ +83 │ │ │ │ │ +84 inline const double& measurementIn() const { return nT_; } │ │ │ │ │ +85 │ │ │ │ │ +86 inline double heightOut(double n) const { │ │ │ │ │ +87 // From https://www.grc.nasa.gov/www/k-12/airplane/atmosmet.html │ │ │ │ │ +88 return (std::pow(n / 101.29, 1. / 5.256) * 288.08 - 273.1 - 15.04) / │ │ │ │ │ +89 -0.00649; │ │ │ │ │ +90 }; │ │ │ │ │ +91 │ │ │ │ │ +92 inline double baroOut(const double& meters) { │ │ │ │ │ +93 double temp = 15.04 - 0.00649 * meters; │ │ │ │ │ +94 return 101.29 * std::pow(((temp + 273.1) / 288.08), 5.256); │ │ │ │ │ +95 }; │ │ │ │ │ +96 │ │ │ │ │ +97 private: │ │ │ │ │ +_9_9 friend class boost::serialization::access; │ │ │ │ │ +100 template │ │ │ │ │ +101 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ +102 // NoiseModelFactor1 instead of NoiseModelFactorN for backward │ │ │ │ │ +compatibility │ │ │ │ │ +103 ar& boost::serialization::make_nvp( │ │ │ │ │ +104 "NoiseModelFactor1", │ │ │ │ │ +105 boost::serialization::base_object(*this)); │ │ │ │ │ +106 ar& BOOST_SERIALIZATION_NVP(nT_); │ │ │ │ │ +107 } │ │ │ │ │ +108}; │ │ │ │ │ +109 │ │ │ │ │ +110} // namespace gtsam │ │ │ │ │ +_P_o_s_e_3_._h │ │ │ │ │ +3D Pose │ │ │ │ │ +_N_a_v_S_t_a_t_e_._h │ │ │ │ │ +Navigation state composing of attitude, position, and velocity. │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ +Non-linear factor base classes. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ -with a unique integer... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_Z_e_r_o │ │ │ │ │ -static VectorValues Zero(const VectorValues &other) │ │ │ │ │ -Create a VectorValues with the same structure as other, but filled with zeros. │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.cpp:78 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l │ │ │ │ │ -This class contains the implementation of the Dogleg algorithm. │ │ │ │ │ -DDeeffiinniittiioonn DoglegOptimizerImpl.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_:_:_T_r_u_s_t_R_e_g_i_o_n_A_d_a_p_t_a_t_i_o_n_M_o_d_e │ │ │ │ │ -TrustRegionAdaptationMode │ │ │ │ │ -Specifies how the trust region is adapted at each Dogleg iteration. │ │ │ │ │ -DDeeffiinniittiioonn DoglegOptimizerImpl.h:53 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_:_:_I_t_e_r_a_t_e │ │ │ │ │ -static IterationResult Iterate(double delta, TrustRegionAdaptationMode mode, │ │ │ │ │ -const VectorValues &dx_u, const VectorValues &dx_n, const M &Rd, const F &f, │ │ │ │ │ -const VALUES &x0, const double f_error, const bool verbose=false) │ │ │ │ │ -Compute the update point for one iteration of the Dogleg algorithm, given an │ │ │ │ │ -initial trust region rad... │ │ │ │ │ -DDeeffiinniittiioonn DoglegOptimizerImpl.h:138 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_:_:_C_o_m_p_u_t_e_D_o_g_l_e_g_P_o_i_n_t │ │ │ │ │ -static VectorValues ComputeDoglegPoint(double delta, const VectorValues &dx_u, │ │ │ │ │ -const VectorValues &dx_n, const bool verbose=false) │ │ │ │ │ -Compute the dogleg point given a trust region radius . │ │ │ │ │ -DDeeffiinniittiioonn DoglegOptimizerImpl.cpp:25 │ │ │ │ │ -_g_t_s_a_m_:_:_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_:_:_I_t_e_r_a_t_i_o_n_R_e_s_u_l_t │ │ │ │ │ -DDeeffiinniittiioonn DoglegOptimizerImpl.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ +noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ +Aliases. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ +A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ +DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r │ │ │ │ │ +Prior on height in a cartesian frame. │ │ │ │ │ +DDeeffiinniittiioonn BarometricFactor.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r_:_:_T_h_i_s │ │ │ │ │ +BarometricFactor This │ │ │ │ │ +Typedef to this class. │ │ │ │ │ +DDeeffiinniittiioonn BarometricFactor.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r_:_:_B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r │ │ │ │ │ +BarometricFactor(Key key, Key baroKey, const double &baroIn, const │ │ │ │ │ +SharedNoiseModel &model) │ │ │ │ │ +Constructor from a measurement of pressure in KPa. │ │ │ │ │ +DDeeffiinniittiioonn BarometricFactor.h:59 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< BarometricFactor > shared_ptr │ │ │ │ │ +shorthand for a smart pointer to a factor │ │ │ │ │ +DDeeffiinniittiioonn BarometricFactor.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ +gtsam::NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ +DDeeffiinniittiioonn BarometricFactor.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r_:_:_B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r │ │ │ │ │ +BarometricFactor() │ │ │ │ │ +default constructor - only use for serialization │ │ │ │ │ +DDeeffiinniittiioonn BarometricFactor.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ +Nonlinear factor base class. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ +A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ +variables. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_._h │ │ │ │ │ + * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ + * _B_a_r_o_m_e_t_r_i_c_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01127.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00053.html │ │ │ │┄ Files 85% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearOptimizerParams.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/types.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs
│ │ │ │ -
NonlinearOptimizerParams.h File Reference
│ │ │ │ +Functions
│ │ │ │ +
types.cpp File Reference
Base
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Parameters for nonlinear optimization. │ │ │ │ +

Functions for handling type information. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::NonlinearOptimizerParams
 The common parameters for Nonlinear optimizers. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ -Typedefs

│ │ │ │ -typedef NonlinearOptimizerParams gtsam::SuccessiveLinearizationParams
 

│ │ │ │ +Functions

std::string gtsam::demangle (const char *name)
 Pretty print Value type name.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Parameters for nonlinear optimization.

│ │ │ │ -
Author
Yong-Dian Jian
│ │ │ │ -
│ │ │ │ -Richard Roberts
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ -
│ │ │ │ -Andrew Melim
│ │ │ │ -
Date
Apr 1, 2012
│ │ │ │ +

Functions for handling type information.

│ │ │ │ +
Author
Varun Agrawal
│ │ │ │ +
Date
May 18, 2020
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,33 +1,27 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ -NonlinearOptimizerParams.h File Reference │ │ │ │ │ -Parameters for nonlinear optimization. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s │ │ │ │ │ -  The common parameters for Nonlinear optimizers. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +types.cpp File Reference │ │ │ │ │ +_B_a_s_e │ │ │ │ │ +Functions for handling type information. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -typedef _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s  ggttssaamm::::SSuucccceessssiivveeLLiinneeaarriizzaattiioonnPPaarraammss │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +std::string  _g_t_s_a_m_:_:_d_e_m_a_n_g_l_e (const char *name) │ │ │ │ │ +  Pretty print _V_a_l_u_e type name. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Parameters for nonlinear optimization. │ │ │ │ │ +Functions for handling type information. │ │ │ │ │ Author │ │ │ │ │ - Yong-Dian Jian │ │ │ │ │ - Richard Roberts │ │ │ │ │ - Frank Dellaert │ │ │ │ │ - Andrew Melim │ │ │ │ │ + Varun Agrawal │ │ │ │ │ Date │ │ │ │ │ - Apr 1, 2012 │ │ │ │ │ + May 18, 2020 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_._h │ │ │ │ │ + * _b_a_s_e │ │ │ │ │ + * _t_y_p_e_s_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01127_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01049_source.html │ │ │ │┄ Files 77% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearOptimizerParams.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2Result.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
NonlinearOptimizerParams.h
│ │ │ │ +
ISAM2Result.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
22#pragma once
│ │ │ │ -
23
│ │ │ │ -
24#include <gtsam/linear/GaussianFactorGraph.h>
│ │ │ │ -
25#include <gtsam/linear/SubgraphSolver.h>
│ │ │ │ -
26#include <boost/optional.hpp>
│ │ │ │ -
27#include <string>
│ │ │ │ -
28
│ │ │ │ -
29namespace gtsam {
│ │ │ │ -
30
│ │ │ │ -
│ │ │ │ -
34class GTSAM_EXPORT NonlinearOptimizerParams {
│ │ │ │ -
35public:
│ │ │ │ -
│ │ │ │ -
37 enum Verbosity {
│ │ │ │ -
38 SILENT, TERMINATION, ERROR, VALUES, DELTA, LINEAR
│ │ │ │ -
39 };
│ │ │ │ -
│ │ │ │ -
40
│ │ │ │ -
41 size_t maxIterations = 100;
│ │ │ │ -
42 double relativeErrorTol = 1e-5;
│ │ │ │ -
43 double absoluteErrorTol = 1e-5;
│ │ │ │ -
44 double errorTol = 0.0;
│ │ │ │ -
45 Verbosity verbosity = SILENT;
│ │ │ │ -
46 Ordering::OrderingType orderingType = Ordering::COLAMD;
│ │ │ │ -
47
│ │ │ │ -
48 size_t getMaxIterations() const { return maxIterations; }
│ │ │ │ -
49 double getRelativeErrorTol() const { return relativeErrorTol; }
│ │ │ │ -
50 double getAbsoluteErrorTol() const { return absoluteErrorTol; }
│ │ │ │ -
51 double getErrorTol() const { return errorTol; }
│ │ │ │ -
52 std::string getVerbosity() const { return verbosityTranslator(verbosity); }
│ │ │ │ -
53
│ │ │ │ -
54 void setMaxIterations(int value) { maxIterations = value; }
│ │ │ │ -
55 void setRelativeErrorTol(double value) { relativeErrorTol = value; }
│ │ │ │ -
56 void setAbsoluteErrorTol(double value) { absoluteErrorTol = value; }
│ │ │ │ -
57 void setErrorTol(double value) { errorTol = value; }
│ │ │ │ -
58 void setVerbosity(const std::string& src) {
│ │ │ │ -
59 verbosity = verbosityTranslator(src);
│ │ │ │ -
60 }
│ │ │ │ -
61
│ │ │ │ -
62 static Verbosity verbosityTranslator(const std::string &s) ;
│ │ │ │ -
63 static std::string verbosityTranslator(Verbosity value) ;
│ │ │ │ -
64
│ │ │ │ -
67 using IterationHook = std::function<
│ │ │ │ -
68 void(size_t /*iteration*/, double/*errorBefore*/, double/*errorAfter*/)>;
│ │ │ │ -
69
│ │ │ │ -
94 IterationHook iterationHook;
│ │ │ │ -
95
│ │ │ │ -
│ │ │ │ -
97 enum LinearSolverType {
│ │ │ │ -
98 MULTIFRONTAL_CHOLESKY,
│ │ │ │ -
99 MULTIFRONTAL_QR,
│ │ │ │ -
100 SEQUENTIAL_CHOLESKY,
│ │ │ │ -
101 SEQUENTIAL_QR,
│ │ │ │ -
102 Iterative, /* Experimental Flag */
│ │ │ │ -
103 CHOLMOD, /* Experimental Flag */
│ │ │ │ -
104 };
│ │ │ │ -
│ │ │ │ -
105
│ │ │ │ -
106 LinearSolverType linearSolverType = MULTIFRONTAL_CHOLESKY;
│ │ │ │ -
107 boost::optional<Ordering> ordering;
│ │ │ │ -
108 IterativeOptimizationParameters::shared_ptr iterativeParams;
│ │ │ │ +
18// \callgraph
│ │ │ │ +
19
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <string>
│ │ │ │ +
23#include <vector>
│ │ │ │ +
24
│ │ │ │ +
25#include <gtsam/linear/GaussianBayesTree.h>
│ │ │ │ +
26#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
│ │ │ │ +
27#include <gtsam/nonlinear/ISAM2Params.h>
│ │ │ │ +
28#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ +
29
│ │ │ │ +
30#include <boost/variant.hpp>
│ │ │ │ +
31
│ │ │ │ +
32namespace gtsam {
│ │ │ │ +
33
│ │ │ │ +
│ │ │ │ +
41struct ISAM2Result {
│ │ │ │ +
54 boost::optional<double> errorBefore;
│ │ │ │ +
55
│ │ │ │ +
66 boost::optional<double> errorAfter;
│ │ │ │ +
67
│ │ │ │ +
76 size_t variablesRelinearized;
│ │ │ │ +
77
│ │ │ │ +
84 size_t variablesReeliminated;
│ │ │ │ +
85
│ │ │ │ +
88 size_t factorsRecalculated;
│ │ │ │ +
89
│ │ │ │ +
91 size_t cliques;
│ │ │ │ +
92
│ │ │ │ +
97 FactorIndices newFactorsIndices;
│ │ │ │ +
98
│ │ │ │ +
102 KeySet unusedKeys;
│ │ │ │ +
103
│ │ │ │ +
105 KeyVector observedKeys;
│ │ │ │ +
106
│ │ │ │ +
108 KeySet keysWithRemovedFactors;
│ │ │ │
109
│ │ │ │ -
110 NonlinearOptimizerParams() = default;
│ │ │ │ -
111 virtual ~NonlinearOptimizerParams() {
│ │ │ │ -
112 }
│ │ │ │ -
113
│ │ │ │ -
114 virtual void print(const std::string& str = "") const;
│ │ │ │ -
115
│ │ │ │ -
116 bool equals(const NonlinearOptimizerParams& other, double tol = 1e-9) const {
│ │ │ │ -
117 return maxIterations == other.getMaxIterations()
│ │ │ │ -
118 && std::abs(relativeErrorTol - other.getRelativeErrorTol()) <= tol
│ │ │ │ -
119 && std::abs(absoluteErrorTol - other.getAbsoluteErrorTol()) <= tol
│ │ │ │ -
120 && std::abs(errorTol - other.getErrorTol()) <= tol
│ │ │ │ -
121 && verbosityTranslator(verbosity) == other.getVerbosity();
│ │ │ │ -
122 // && orderingType.equals(other.getOrderingType()_;
│ │ │ │ -
123 // && linearSolverType == other.getLinearSolverType();
│ │ │ │ -
124 // TODO: check ordering, iterativeParams, and iterationsHook
│ │ │ │ -
125 }
│ │ │ │ -
126
│ │ │ │ -
127 inline bool isMultifrontal() const {
│ │ │ │ -
128 return (linearSolverType == MULTIFRONTAL_CHOLESKY)
│ │ │ │ -
129 || (linearSolverType == MULTIFRONTAL_QR);
│ │ │ │ -
130 }
│ │ │ │ -
131
│ │ │ │ -
132 inline bool isSequential() const {
│ │ │ │ -
133 return (linearSolverType == SEQUENTIAL_CHOLESKY)
│ │ │ │ -
134 || (linearSolverType == SEQUENTIAL_QR);
│ │ │ │ -
135 }
│ │ │ │ -
136
│ │ │ │ -
137 inline bool isCholmod() const {
│ │ │ │ -
138 return (linearSolverType == CHOLMOD);
│ │ │ │ -
139 }
│ │ │ │ -
140
│ │ │ │ -
141 inline bool isIterative() const {
│ │ │ │ -
142 return (linearSolverType == Iterative);
│ │ │ │ -
143 }
│ │ │ │ -
144
│ │ │ │ -
145 GaussianFactorGraph::Eliminate getEliminationFunction() const {
│ │ │ │ -
146 switch (linearSolverType) {
│ │ │ │ -
147 case MULTIFRONTAL_CHOLESKY:
│ │ │ │ -
148 case SEQUENTIAL_CHOLESKY:
│ │ │ │ -
149 return EliminatePreferCholesky;
│ │ │ │ -
150
│ │ │ │ -
151 case MULTIFRONTAL_QR:
│ │ │ │ -
152 case SEQUENTIAL_QR:
│ │ │ │ -
153 return EliminateQR;
│ │ │ │ -
154
│ │ │ │ -
155 default:
│ │ │ │ -
156 throw std::runtime_error(
│ │ │ │ -
157 "Nonlinear optimization parameter \"factorization\" is invalid");
│ │ │ │ -
158 }
│ │ │ │ -
159 }
│ │ │ │ -
160
│ │ │ │ -
161 std::string getLinearSolverType() const {
│ │ │ │ -
162 return linearSolverTranslator(linearSolverType);
│ │ │ │ -
163 }
│ │ │ │ -
164
│ │ │ │ -
165 void setLinearSolverType(const std::string& solver) {
│ │ │ │ -
166 linearSolverType = linearSolverTranslator(solver);
│ │ │ │ -
167 }
│ │ │ │ -
168
│ │ │ │ -
169 void setIterativeParams(const boost::shared_ptr<IterativeOptimizationParameters> params);
│ │ │ │ -
170
│ │ │ │ -
171 void setOrdering(const Ordering& ordering) {
│ │ │ │ -
172 this->ordering = ordering;
│ │ │ │ -
173 this->orderingType = Ordering::CUSTOM;
│ │ │ │ -
174 }
│ │ │ │ -
175
│ │ │ │ -
176 std::string getOrderingType() const {
│ │ │ │ -
177 return orderingTypeTranslator(orderingType);
│ │ │ │ -
178 }
│ │ │ │ -
179
│ │ │ │ -
180 // Note that if you want to use a custom ordering, you must set the ordering directly, this will switch to custom type
│ │ │ │ -
181 void setOrderingType(const std::string& ordering){
│ │ │ │ -
182 orderingType = orderingTypeTranslator(ordering);
│ │ │ │ -
183 }
│ │ │ │ -
184
│ │ │ │ -
185private:
│ │ │ │ -
186 std::string linearSolverTranslator(LinearSolverType linearSolverType) const;
│ │ │ │ -
187 LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const;
│ │ │ │ -
188 std::string orderingTypeTranslator(Ordering::OrderingType type) const;
│ │ │ │ -
189 Ordering::OrderingType orderingTypeTranslator(const std::string& type) const;
│ │ │ │ -
190};
│ │ │ │ -
│ │ │ │ -
191
│ │ │ │ -
192// For backward compatibility:
│ │ │ │ -
193typedef NonlinearOptimizerParams SuccessiveLinearizationParams;
│ │ │ │ -
194
│ │ │ │ -
195} /* namespace gtsam */
│ │ │ │ -
SubgraphSolver.h
Subgraph Solver from IROS 2010.
│ │ │ │ -
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
│ │ │ │ -
gtsam::EliminatePreferCholesky
std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< GaussianFactor > > EliminatePreferCholesky(const GaussianFactorGraph &factors, const Ordering &keys)
Densely partially eliminate with Cholesky factorization.
Definition HessianFactor.cpp:548
│ │ │ │ +
111 KeySet markedKeys;
│ │ │ │ +
112
│ │ │ │ +
│ │ │ │ +
117 struct DetailedResults {
│ │ │ │ +
│ │ │ │ +
120 struct VariableStatus {
│ │ │ │ +
124 bool isReeliminated;
│ │ │ │ +
125 bool isAboveRelinThreshold;
│ │ │ │ +
128 bool isRelinearizeInvolved;
│ │ │ │ +
133 bool isRelinearized;
│ │ │ │ +
136 bool isObserved;
│ │ │ │ +
138 bool isNew;
│ │ │ │ +
139 bool inRootClique;
│ │ │ │ +
140 VariableStatus()
│ │ │ │ +
141 : isReeliminated(false),
│ │ │ │ +
142 isAboveRelinThreshold(false),
│ │ │ │ +
143 isRelinearizeInvolved(false),
│ │ │ │ +
144 isRelinearized(false),
│ │ │ │ +
145 isObserved(false),
│ │ │ │ +
146 isNew(false),
│ │ │ │ +
147 inRootClique(false) {}
│ │ │ │ +
148 };
│ │ │ │ +
│ │ │ │ +
149
│ │ │ │ +
150 using StatusMap = FastMap<Key, VariableStatus>;
│ │ │ │ +
151
│ │ │ │ +
153 StatusMap variableStatus;
│ │ │ │ +
154 };
│ │ │ │ +
│ │ │ │ +
155
│ │ │ │ +
158 boost::optional<DetailedResults> detail;
│ │ │ │ +
159
│ │ │ │ +
160 explicit ISAM2Result(bool enableDetailedResults = false) {
│ │ │ │ +
161 if (enableDetailedResults) detail.reset(DetailedResults());
│ │ │ │ +
162 }
│ │ │ │ +
163
│ │ │ │ +
165 DetailedResults* details() { return detail.get_ptr(); }
│ │ │ │ +
166
│ │ │ │ +
│ │ │ │ +
168 void print(const std::string str = "") const {
│ │ │ │ +
169 using std::cout;
│ │ │ │ +
170 cout << str << " Reelimintated: " << variablesReeliminated
│ │ │ │ +
171 << " Relinearized: " << variablesRelinearized
│ │ │ │ +
172 << " Cliques: " << cliques << std::endl;
│ │ │ │ +
173 }
│ │ │ │ +
│ │ │ │ +
174
│ │ │ │ +
176 size_t getVariablesRelinearized() const { return variablesRelinearized; }
│ │ │ │ +
177 size_t getVariablesReeliminated() const { return variablesReeliminated; }
│ │ │ │ +
178 FactorIndices getNewFactorsIndices() const { return newFactorsIndices; }
│ │ │ │ +
179 size_t getCliques() const { return cliques; }
│ │ │ │ +
180 double getErrorBefore() const { return errorBefore ? *errorBefore : std::nan(""); }
│ │ │ │ +
181 double getErrorAfter() const { return errorAfter ? *errorAfter : std::nan(""); }
│ │ │ │ +
182};
│ │ │ │ +
│ │ │ │ +
183
│ │ │ │ +
184} // namespace gtsam
│ │ │ │ +
GaussianBayesTree.h
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
│ │ │ │ +
DoglegOptimizerImpl.h
Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail implementation)
│ │ │ │ +
ISAM2Params.h
Parameters for iSAM 2.
│ │ │ │ +
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::EliminateQR
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys)
Multiply all factors and eliminate the given keys from the resulting factor using a QR variant that h...
Definition JacobianFactor.cpp:789
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ -
gtsam::Ordering::OrderingType
OrderingType
Type of ordering to use.
Definition Ordering.h:41
│ │ │ │ -
gtsam::NonlinearOptimizerParams
The common parameters for Nonlinear optimizers.
Definition NonlinearOptimizerParams.h:34
│ │ │ │ -
gtsam::NonlinearOptimizerParams::iterationHook
IterationHook iterationHook
Optional user-provided iteration hook to be called after each optimization iteration (Default: none).
Definition NonlinearOptimizerParams.h:94
│ │ │ │ -
gtsam::NonlinearOptimizerParams::iterativeParams
IterativeOptimizationParameters::shared_ptr iterativeParams
The container for iterativeOptimization parameters. used in CG Solvers.
Definition NonlinearOptimizerParams.h:108
│ │ │ │ -
gtsam::NonlinearOptimizerParams::Verbosity
Verbosity
See NonlinearOptimizerParams::verbosity.
Definition NonlinearOptimizerParams.h:37
│ │ │ │ -
gtsam::NonlinearOptimizerParams::LinearSolverType
LinearSolverType
See NonlinearOptimizerParams::linearSolverType.
Definition NonlinearOptimizerParams.h:97
│ │ │ │ -
gtsam::NonlinearOptimizerParams::IterationHook
std::function< void(size_t, double, double)> IterationHook
Type for an optional user-provided hook to be called after each internal optimizer iteration.
Definition NonlinearOptimizerParams.h:68
│ │ │ │ -
gtsam::NonlinearOptimizerParams::ordering
boost::optional< Ordering > ordering
The optional variable elimination ordering, or empty to use COLAMD (default: empty)
Definition NonlinearOptimizerParams.h:107
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition Factor.h:34
│ │ │ │ +
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ + │ │ │ │ +
gtsam::ISAM2Result
This struct is returned from ISAM2::update() and contains information about the update that is useful...
Definition ISAM2Result.h:41
│ │ │ │ +
gtsam::ISAM2Result::variablesRelinearized
size_t variablesRelinearized
The number of variables that were relinearized because their linear deltas exceeded the reslinearizat...
Definition ISAM2Result.h:76
│ │ │ │ +
gtsam::ISAM2Result::getVariablesRelinearized
size_t getVariablesRelinearized() const
Getters and Setters.
Definition ISAM2Result.h:176
│ │ │ │ +
gtsam::ISAM2Result::factorsRecalculated
size_t factorsRecalculated
The number of factors that were included in reelimination of the Bayes' tree.
Definition ISAM2Result.h:88
│ │ │ │ +
gtsam::ISAM2Result::details
DetailedResults * details()
Return pointer to detail, 0 if no detail requested.
Definition ISAM2Result.h:165
│ │ │ │ +
gtsam::ISAM2Result::errorBefore
boost::optional< double > errorBefore
The nonlinear error of all of the factors, including new factors and variables added during the curre...
Definition ISAM2Result.h:54
│ │ │ │ +
gtsam::ISAM2Result::print
void print(const std::string str="") const
Print results.
Definition ISAM2Result.h:168
│ │ │ │ +
gtsam::ISAM2Result::detail
boost::optional< DetailedResults > detail
Detailed results, if enabled by ISAM2Params::enableDetailedResults.
Definition ISAM2Result.h:158
│ │ │ │ +
gtsam::ISAM2Result::cliques
size_t cliques
The number of cliques in the Bayes' Tree.
Definition ISAM2Result.h:91
│ │ │ │ +
gtsam::ISAM2Result::keysWithRemovedFactors
KeySet keysWithRemovedFactors
Keys of variables that had factors removed.
Definition ISAM2Result.h:108
│ │ │ │ +
gtsam::ISAM2Result::variablesReeliminated
size_t variablesReeliminated
The number of variables that were reeliminated as parts of the Bayes' Tree were recalculated,...
Definition ISAM2Result.h:84
│ │ │ │ +
gtsam::ISAM2Result::unusedKeys
KeySet unusedKeys
Unused keys, and indices for unused keys, i.e., keys that are empty now and do not appear in the new ...
Definition ISAM2Result.h:102
│ │ │ │ +
gtsam::ISAM2Result::observedKeys
KeyVector observedKeys
keys for variables that were observed, i.e., not unused.
Definition ISAM2Result.h:105
│ │ │ │ +
gtsam::ISAM2Result::markedKeys
KeySet markedKeys
All keys that were marked during the update process.
Definition ISAM2Result.h:111
│ │ │ │ +
gtsam::ISAM2Result::newFactorsIndices
FactorIndices newFactorsIndices
The indices of the newly-added factors, in 1-to-1 correspondence with the factors passed as newFactor...
Definition ISAM2Result.h:97
│ │ │ │ +
gtsam::ISAM2Result::errorAfter
boost::optional< double > errorAfter
The nonlinear error of all of the factors computed after the current update, meaning that variables a...
Definition ISAM2Result.h:66
│ │ │ │ +
gtsam::ISAM2Result::DetailedResults
A struct holding detailed results, which must be enabled with ISAM2Params::enableDetailedResults.
Definition ISAM2Result.h:117
│ │ │ │ +
gtsam::ISAM2Result::DetailedResults::variableStatus
StatusMap variableStatus
The status of each variable during this update, see VariableStatus.
Definition ISAM2Result.h:153
│ │ │ │ +
gtsam::ISAM2Result::DetailedResults::VariableStatus
The status of a single variable, this struct is stored in DetailedResults::variableStatus.
Definition ISAM2Result.h:120
│ │ │ │ +
gtsam::ISAM2Result::DetailedResults::VariableStatus::isRelinearizeInvolved
bool isRelinearizeInvolved
Whether the variable was below the relinearization threshold but was relinearized by being involved i...
Definition ISAM2Result.h:128
│ │ │ │ +
gtsam::ISAM2Result::DetailedResults::VariableStatus::isObserved
bool isObserved
Whether the variable was relinearized, either by being above the relinearization threshold or by invo...
Definition ISAM2Result.h:136
│ │ │ │ +
gtsam::ISAM2Result::DetailedResults::VariableStatus::isNew
bool isNew
Whether the variable itself was just added.
Definition ISAM2Result.h:138
│ │ │ │ +
gtsam::ISAM2Result::DetailedResults::VariableStatus::isAboveRelinThreshold
bool isAboveRelinThreshold
Whether the variable was just relinearized due to being above the relinearization threshold.
Definition ISAM2Result.h:125
│ │ │ │ +
gtsam::ISAM2Result::DetailedResults::VariableStatus::isReeliminated
bool isReeliminated
Whether the variable was just reeliminated, due to being relinearized, observed, new,...
Definition ISAM2Result.h:124
│ │ │ │ +
gtsam::ISAM2Result::DetailedResults::VariableStatus::inRootClique
bool inRootClique
Whether the variable is in the root clique.
Definition ISAM2Result.h:139
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,233 +1,248 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -NonlinearOptimizerParams.h │ │ │ │ │ +ISAM2Result.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -22#pragma once │ │ │ │ │ -23 │ │ │ │ │ -24#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_S_u_b_g_r_a_p_h_S_o_l_v_e_r_._h> │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ -28 │ │ │ │ │ -29namespace _g_t_s_a_m { │ │ │ │ │ -30 │ │ │ │ │ -_3_4class GTSAM_EXPORT _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s { │ │ │ │ │ -35public: │ │ │ │ │ -_3_7 enum _V_e_r_b_o_s_i_t_y { │ │ │ │ │ -38 SILENT, TERMINATION, ERROR, VALUES, DELTA, LINEAR │ │ │ │ │ -39 }; │ │ │ │ │ -40 │ │ │ │ │ -_4_1 size_t maxIterations = 100; │ │ │ │ │ -_4_2 double relativeErrorTol = 1e-5; │ │ │ │ │ -_4_3 double absoluteErrorTol = 1e-5; │ │ │ │ │ -_4_4 double errorTol = 0.0; │ │ │ │ │ -_4_5 _V_e_r_b_o_s_i_t_y verbosity = SILENT; │ │ │ │ │ -_4_6 _O_r_d_e_r_i_n_g_:_:_O_r_d_e_r_i_n_g_T_y_p_e orderingType = Ordering::COLAMD; │ │ │ │ │ -47 │ │ │ │ │ -48 size_t getMaxIterations() const { return maxIterations; } │ │ │ │ │ -49 double getRelativeErrorTol() const { return relativeErrorTol; } │ │ │ │ │ -50 double getAbsoluteErrorTol() const { return absoluteErrorTol; } │ │ │ │ │ -51 double getErrorTol() const { return errorTol; } │ │ │ │ │ -52 std::string getVerbosity() const { return verbosityTranslator(verbosity); } │ │ │ │ │ -53 │ │ │ │ │ -54 void setMaxIterations(int value) { maxIterations = value; } │ │ │ │ │ -55 void setRelativeErrorTol(double value) { relativeErrorTol = value; } │ │ │ │ │ -56 void setAbsoluteErrorTol(double value) { absoluteErrorTol = value; } │ │ │ │ │ -57 void setErrorTol(double value) { errorTol = value; } │ │ │ │ │ -58 void setVerbosity(const std::string& src) { │ │ │ │ │ -59 verbosity = verbosityTranslator(src); │ │ │ │ │ -60 } │ │ │ │ │ -61 │ │ │ │ │ -62 static Verbosity verbosityTranslator(const std::string &s) ; │ │ │ │ │ -63 static std::string verbosityTranslator(Verbosity value) ; │ │ │ │ │ -64 │ │ │ │ │ -_6_7 using _I_t_e_r_a_t_i_o_n_H_o_o_k = std::function< │ │ │ │ │ -68 void(size_t /*iteration*/, double/*errorBefore*/, double/*errorAfter*/)>; │ │ │ │ │ -69 │ │ │ │ │ -_9_4 _I_t_e_r_a_t_i_o_n_H_o_o_k _i_t_e_r_a_t_i_o_n_H_o_o_k; │ │ │ │ │ -95 │ │ │ │ │ -_9_7 enum _L_i_n_e_a_r_S_o_l_v_e_r_T_y_p_e { │ │ │ │ │ -98 MULTIFRONTAL_CHOLESKY, │ │ │ │ │ -99 MULTIFRONTAL_QR, │ │ │ │ │ -100 SEQUENTIAL_CHOLESKY, │ │ │ │ │ -101 SEQUENTIAL_QR, │ │ │ │ │ -102 Iterative, /* Experimental Flag */ │ │ │ │ │ -103 CHOLMOD, /* Experimental Flag */ │ │ │ │ │ -104 }; │ │ │ │ │ -105 │ │ │ │ │ -_1_0_6 _L_i_n_e_a_r_S_o_l_v_e_r_T_y_p_e linearSolverType = MULTIFRONTAL_CHOLESKY; │ │ │ │ │ -_1_0_7 boost::optional _o_r_d_e_r_i_n_g; │ │ │ │ │ -_1_0_8 IterativeOptimizationParameters::shared_ptr _i_t_e_r_a_t_i_v_e_P_a_r_a_m_s; │ │ │ │ │ +18// \callgraph │ │ │ │ │ +19 │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include │ │ │ │ │ +23#include │ │ │ │ │ +24 │ │ │ │ │ +25#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ +26#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_._h> │ │ │ │ │ +27#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_I_S_A_M_2_P_a_r_a_m_s_._h> │ │ │ │ │ +28#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +29 │ │ │ │ │ +30#include │ │ │ │ │ +31 │ │ │ │ │ +32namespace _g_t_s_a_m { │ │ │ │ │ +33 │ │ │ │ │ +_4_1struct _I_S_A_M_2_R_e_s_u_l_t { │ │ │ │ │ +_5_4 boost::optional _e_r_r_o_r_B_e_f_o_r_e; │ │ │ │ │ +55 │ │ │ │ │ +_6_6 boost::optional _e_r_r_o_r_A_f_t_e_r; │ │ │ │ │ +67 │ │ │ │ │ +_7_6 size_t _v_a_r_i_a_b_l_e_s_R_e_l_i_n_e_a_r_i_z_e_d; │ │ │ │ │ +77 │ │ │ │ │ +_8_4 size_t _v_a_r_i_a_b_l_e_s_R_e_e_l_i_m_i_n_a_t_e_d; │ │ │ │ │ +85 │ │ │ │ │ +_8_8 size_t _f_a_c_t_o_r_s_R_e_c_a_l_c_u_l_a_t_e_d; │ │ │ │ │ +89 │ │ │ │ │ +_9_1 size_t _c_l_i_q_u_e_s; │ │ │ │ │ +92 │ │ │ │ │ +_9_7 _F_a_c_t_o_r_I_n_d_i_c_e_s _n_e_w_F_a_c_t_o_r_s_I_n_d_i_c_e_s; │ │ │ │ │ +98 │ │ │ │ │ +_1_0_2 _K_e_y_S_e_t _u_n_u_s_e_d_K_e_y_s; │ │ │ │ │ +103 │ │ │ │ │ +_1_0_5 _K_e_y_V_e_c_t_o_r _o_b_s_e_r_v_e_d_K_e_y_s; │ │ │ │ │ +106 │ │ │ │ │ +_1_0_8 _K_e_y_S_e_t _k_e_y_s_W_i_t_h_R_e_m_o_v_e_d_F_a_c_t_o_r_s; │ │ │ │ │ 109 │ │ │ │ │ -110 _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s() = default; │ │ │ │ │ -111 virtual _~_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s() { │ │ │ │ │ -112 } │ │ │ │ │ -113 │ │ │ │ │ -114 virtual void _p_r_i_n_t(const std::string& str = "") const; │ │ │ │ │ -115 │ │ │ │ │ -116 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s& other, double tol = 1e-9) const │ │ │ │ │ -{ │ │ │ │ │ -117 return maxIterations == other.getMaxIterations() │ │ │ │ │ -118 && std::abs(relativeErrorTol - other.getRelativeErrorTol()) <= tol │ │ │ │ │ -119 && std::abs(absoluteErrorTol - other.getAbsoluteErrorTol()) <= tol │ │ │ │ │ -120 && std::abs(errorTol - other.getErrorTol()) <= tol │ │ │ │ │ -121 && verbosityTranslator(verbosity) == other.getVerbosity(); │ │ │ │ │ -122 // && orderingType.equals(other.getOrderingType()_; │ │ │ │ │ -123 // && linearSolverType == other.getLinearSolverType(); │ │ │ │ │ -124 // TODO: check ordering, iterativeParams, and iterationsHook │ │ │ │ │ -125 } │ │ │ │ │ -126 │ │ │ │ │ -127 inline bool isMultifrontal() const { │ │ │ │ │ -128 return (linearSolverType == MULTIFRONTAL_CHOLESKY) │ │ │ │ │ -129 || (linearSolverType == MULTIFRONTAL_QR); │ │ │ │ │ -130 } │ │ │ │ │ -131 │ │ │ │ │ -132 inline bool isSequential() const { │ │ │ │ │ -133 return (linearSolverType == SEQUENTIAL_CHOLESKY) │ │ │ │ │ -134 || (linearSolverType == SEQUENTIAL_QR); │ │ │ │ │ -135 } │ │ │ │ │ -136 │ │ │ │ │ -137 inline bool isCholmod() const { │ │ │ │ │ -138 return (linearSolverType == CHOLMOD); │ │ │ │ │ -139 } │ │ │ │ │ -140 │ │ │ │ │ -141 inline bool isIterative() const { │ │ │ │ │ -142 return (linearSolverType == Iterative); │ │ │ │ │ -143 } │ │ │ │ │ -144 │ │ │ │ │ -145 GaussianFactorGraph::Eliminate getEliminationFunction() const { │ │ │ │ │ -146 switch (linearSolverType) { │ │ │ │ │ -147 case MULTIFRONTAL_CHOLESKY: │ │ │ │ │ -148 case SEQUENTIAL_CHOLESKY: │ │ │ │ │ -149 return _E_l_i_m_i_n_a_t_e_P_r_e_f_e_r_C_h_o_l_e_s_k_y; │ │ │ │ │ -150 │ │ │ │ │ -151 case MULTIFRONTAL_QR: │ │ │ │ │ -152 case SEQUENTIAL_QR: │ │ │ │ │ -153 return _E_l_i_m_i_n_a_t_e_Q_R; │ │ │ │ │ -154 │ │ │ │ │ -155 default: │ │ │ │ │ -156 throw std::runtime_error( │ │ │ │ │ -157 "Nonlinear optimization parameter \"factorization\" is invalid"); │ │ │ │ │ -158 } │ │ │ │ │ -159 } │ │ │ │ │ -160 │ │ │ │ │ -161 std::string getLinearSolverType() const { │ │ │ │ │ -162 return linearSolverTranslator(linearSolverType); │ │ │ │ │ -163 } │ │ │ │ │ -164 │ │ │ │ │ -165 void setLinearSolverType(const std::string& solver) { │ │ │ │ │ -166 linearSolverType = linearSolverTranslator(solver); │ │ │ │ │ -167 } │ │ │ │ │ -168 │ │ │ │ │ -169 void setIterativeParams(const boost:: │ │ │ │ │ -shared_ptr params); │ │ │ │ │ -170 │ │ │ │ │ -171 void setOrdering(const Ordering& ordering) { │ │ │ │ │ -172 this->ordering = ordering; │ │ │ │ │ -173 this->orderingType = Ordering::CUSTOM; │ │ │ │ │ -174 } │ │ │ │ │ -175 │ │ │ │ │ -176 std::string getOrderingType() const { │ │ │ │ │ -177 return orderingTypeTranslator(orderingType); │ │ │ │ │ -178 } │ │ │ │ │ -179 │ │ │ │ │ -180 // Note that if you want to use a custom ordering, you must set the │ │ │ │ │ -ordering directly, this will switch to custom type │ │ │ │ │ -181 void setOrderingType(const std::string& ordering){ │ │ │ │ │ -182 orderingType = orderingTypeTranslator(ordering); │ │ │ │ │ -183 } │ │ │ │ │ -184 │ │ │ │ │ -185private: │ │ │ │ │ -186 std::string linearSolverTranslator(LinearSolverType linearSolverType) │ │ │ │ │ -const; │ │ │ │ │ -187 LinearSolverType linearSolverTranslator(const std::string& │ │ │ │ │ -linearSolverType) const; │ │ │ │ │ -188 std::string orderingTypeTranslator(Ordering::OrderingType type) const; │ │ │ │ │ -189 Ordering::OrderingType orderingTypeTranslator(const std::string& type) │ │ │ │ │ -const; │ │ │ │ │ -190}; │ │ │ │ │ -191 │ │ │ │ │ -192// For backward compatibility: │ │ │ │ │ -193typedef NonlinearOptimizerParams SuccessiveLinearizationParams; │ │ │ │ │ -194 │ │ │ │ │ -195} /* namespace gtsam */ │ │ │ │ │ -_S_u_b_g_r_a_p_h_S_o_l_v_e_r_._h │ │ │ │ │ -Subgraph Solver from IROS 2010. │ │ │ │ │ -_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Linear Factor Graph where all factors are Gaussians. │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_P_r_e_f_e_r_C_h_o_l_e_s_k_y │ │ │ │ │ -std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< │ │ │ │ │ -GaussianFactor > > EliminatePreferCholesky(const GaussianFactorGraph &factors, │ │ │ │ │ -const Ordering &keys) │ │ │ │ │ -Densely partially eliminate with Cholesky factorization. │ │ │ │ │ -DDeeffiinniittiioonn HessianFactor.cpp:548 │ │ │ │ │ +_1_1_1 _K_e_y_S_e_t _m_a_r_k_e_d_K_e_y_s; │ │ │ │ │ +112 │ │ │ │ │ +_1_1_7 struct _D_e_t_a_i_l_e_d_R_e_s_u_l_t_s { │ │ │ │ │ +_1_2_0 struct _V_a_r_i_a_b_l_e_S_t_a_t_u_s { │ │ │ │ │ +_1_2_4 bool _i_s_R_e_e_l_i_m_i_n_a_t_e_d; │ │ │ │ │ +_1_2_5 bool _i_s_A_b_o_v_e_R_e_l_i_n_T_h_r_e_s_h_o_l_d; │ │ │ │ │ +_1_2_8 bool _i_s_R_e_l_i_n_e_a_r_i_z_e_I_n_v_o_l_v_e_d; │ │ │ │ │ +133 bool isRelinearized; │ │ │ │ │ +_1_3_6 bool _i_s_O_b_s_e_r_v_e_d; │ │ │ │ │ +_1_3_8 bool _i_s_N_e_w; │ │ │ │ │ +_1_3_9 bool _i_n_R_o_o_t_C_l_i_q_u_e; │ │ │ │ │ +140 _V_a_r_i_a_b_l_e_S_t_a_t_u_s() │ │ │ │ │ +141 : _i_s_R_e_e_l_i_m_i_n_a_t_e_d(false), │ │ │ │ │ +142 _i_s_A_b_o_v_e_R_e_l_i_n_T_h_r_e_s_h_o_l_d(false), │ │ │ │ │ +143 _i_s_R_e_l_i_n_e_a_r_i_z_e_I_n_v_o_l_v_e_d(false), │ │ │ │ │ +144 isRelinearized(false), │ │ │ │ │ +145 _i_s_O_b_s_e_r_v_e_d(false), │ │ │ │ │ +146 _i_s_N_e_w(false), │ │ │ │ │ +147 _i_n_R_o_o_t_C_l_i_q_u_e(false) {} │ │ │ │ │ +148 }; │ │ │ │ │ +149 │ │ │ │ │ +150 using StatusMap = _F_a_s_t_M_a_p_<_K_e_y_,_ _V_a_r_i_a_b_l_e_S_t_a_t_u_s_>; │ │ │ │ │ +151 │ │ │ │ │ +_1_5_3 _S_t_a_t_u_s_M_a_p _v_a_r_i_a_b_l_e_S_t_a_t_u_s; │ │ │ │ │ +154 }; │ │ │ │ │ +155 │ │ │ │ │ +_1_5_8 boost::optional _d_e_t_a_i_l; │ │ │ │ │ +159 │ │ │ │ │ +160 explicit _I_S_A_M_2_R_e_s_u_l_t(bool enableDetailedResults = false) { │ │ │ │ │ +161 if (enableDetailedResults) _d_e_t_a_i_l.reset(_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s()); │ │ │ │ │ +162 } │ │ │ │ │ +163 │ │ │ │ │ +_1_6_5 _D_e_t_a_i_l_e_d_R_e_s_u_l_t_s* _d_e_t_a_i_l_s() { return _d_e_t_a_i_l.get_ptr(); } │ │ │ │ │ +166 │ │ │ │ │ +_1_6_8 void _p_r_i_n_t(const std::string str = "") const { │ │ │ │ │ +169 using std::cout; │ │ │ │ │ +170 cout << str << " Reelimintated: " << _v_a_r_i_a_b_l_e_s_R_e_e_l_i_m_i_n_a_t_e_d │ │ │ │ │ +171 << " Relinearized: " << _v_a_r_i_a_b_l_e_s_R_e_l_i_n_e_a_r_i_z_e_d │ │ │ │ │ +172 << " Cliques: " << _c_l_i_q_u_e_s << std::endl; │ │ │ │ │ +173 } │ │ │ │ │ +174 │ │ │ │ │ +_1_7_6 size_t _g_e_t_V_a_r_i_a_b_l_e_s_R_e_l_i_n_e_a_r_i_z_e_d() const { return _v_a_r_i_a_b_l_e_s_R_e_l_i_n_e_a_r_i_z_e_d; } │ │ │ │ │ +177 size_t getVariablesReeliminated() const { return _v_a_r_i_a_b_l_e_s_R_e_e_l_i_m_i_n_a_t_e_d; } │ │ │ │ │ +178 _F_a_c_t_o_r_I_n_d_i_c_e_s getNewFactorsIndices() const { return _n_e_w_F_a_c_t_o_r_s_I_n_d_i_c_e_s; } │ │ │ │ │ +179 size_t getCliques() const { return _c_l_i_q_u_e_s; } │ │ │ │ │ +180 double getErrorBefore() const { return _e_r_r_o_r_B_e_f_o_r_e ? *_e_r_r_o_r_B_e_f_o_r_e : std:: │ │ │ │ │ +nan(""); } │ │ │ │ │ +181 double getErrorAfter() const { return _e_r_r_o_r_A_f_t_e_r ? *_e_r_r_o_r_A_f_t_e_r : std::nan │ │ │ │ │ +(""); } │ │ │ │ │ +182}; │ │ │ │ │ +183 │ │ │ │ │ +184} // namespace gtsam │ │ │ │ │ +_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ +Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ │ +_D_o_g_l_e_g_O_p_t_i_m_i_z_e_r_I_m_p_l_._h │ │ │ │ │ +Nonlinear factor graph optimizer using Powell's Dogleg algorithm (detail │ │ │ │ │ +implementation) │ │ │ │ │ +_I_S_A_M_2_P_a_r_a_m_s_._h │ │ │ │ │ +Parameters for iSAM 2. │ │ │ │ │ +_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Factor Graph consisting of non-linear factors. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_Q_R │ │ │ │ │ -std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr > │ │ │ │ │ -EliminateQR(const GaussianFactorGraph &factors, const Ordering &keys) │ │ │ │ │ -Multiply all factors and eliminate the given keys from the resulting factor │ │ │ │ │ -using a QR variant that h... │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor.cpp:789 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_O_r_d_e_r_i_n_g_T_y_p_e │ │ │ │ │ -OrderingType │ │ │ │ │ -Type of ordering to use. │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s │ │ │ │ │ -The common parameters for Nonlinear optimizers. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizerParams.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_i_t_e_r_a_t_i_o_n_H_o_o_k │ │ │ │ │ -IterationHook iterationHook │ │ │ │ │ -Optional user-provided iteration hook to be called after each optimization │ │ │ │ │ -iteration (Default: none). │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizerParams.h:94 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_i_t_e_r_a_t_i_v_e_P_a_r_a_m_s │ │ │ │ │ -IterativeOptimizationParameters::shared_ptr iterativeParams │ │ │ │ │ -The container for iterativeOptimization parameters. used in CG Solvers. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizerParams.h:108 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_V_e_r_b_o_s_i_t_y │ │ │ │ │ -Verbosity │ │ │ │ │ -See NonlinearOptimizerParams::verbosity. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizerParams.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_L_i_n_e_a_r_S_o_l_v_e_r_T_y_p_e │ │ │ │ │ -LinearSolverType │ │ │ │ │ -See NonlinearOptimizerParams::linearSolverType. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizerParams.h:97 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_I_t_e_r_a_t_i_o_n_H_o_o_k │ │ │ │ │ -std::function< void(size_t, double, double)> IterationHook │ │ │ │ │ -Type for an optional user-provided hook to be called after each internal │ │ │ │ │ -optimizer iteration. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizerParams.h:68 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_o_r_d_e_r_i_n_g │ │ │ │ │ -boost::optional< Ordering > ordering │ │ │ │ │ -The optional variable elimination ordering, or empty to use COLAMD (default: │ │ │ │ │ -empty) │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizerParams.h:107 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_I_n_d_i_c_e_s │ │ │ │ │ +FastVector< FactorIndex > FactorIndices │ │ │ │ │ +Define collection types: │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ +FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ +fast_pool_allocator instead of the defa... │ │ │ │ │ +DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t │ │ │ │ │ +This struct is returned from ISAM2::update() and contains information about the │ │ │ │ │ +update that is useful... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Result.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_v_a_r_i_a_b_l_e_s_R_e_l_i_n_e_a_r_i_z_e_d │ │ │ │ │ +size_t variablesRelinearized │ │ │ │ │ +The number of variables that were relinearized because their linear deltas │ │ │ │ │ +exceeded the reslinearizat... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Result.h:76 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_g_e_t_V_a_r_i_a_b_l_e_s_R_e_l_i_n_e_a_r_i_z_e_d │ │ │ │ │ +size_t getVariablesRelinearized() const │ │ │ │ │ +Getters and Setters. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Result.h:176 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_f_a_c_t_o_r_s_R_e_c_a_l_c_u_l_a_t_e_d │ │ │ │ │ +size_t factorsRecalculated │ │ │ │ │ +The number of factors that were included in reelimination of the Bayes' tree. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Result.h:88 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_d_e_t_a_i_l_s │ │ │ │ │ +DetailedResults * details() │ │ │ │ │ +Return pointer to detail, 0 if no detail requested. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Result.h:165 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_e_r_r_o_r_B_e_f_o_r_e │ │ │ │ │ +boost::optional< double > errorBefore │ │ │ │ │ +The nonlinear error of all of the factors, including new factors and variables │ │ │ │ │ +added during the curre... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Result.h:54 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string str="") const │ │ │ │ │ +Print results. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Result.h:168 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_d_e_t_a_i_l │ │ │ │ │ +boost::optional< DetailedResults > detail │ │ │ │ │ +Detailed results, if enabled by ISAM2Params::enableDetailedResults. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Result.h:158 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_c_l_i_q_u_e_s │ │ │ │ │ +size_t cliques │ │ │ │ │ +The number of cliques in the Bayes' Tree. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Result.h:91 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_k_e_y_s_W_i_t_h_R_e_m_o_v_e_d_F_a_c_t_o_r_s │ │ │ │ │ +KeySet keysWithRemovedFactors │ │ │ │ │ +Keys of variables that had factors removed. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Result.h:108 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_v_a_r_i_a_b_l_e_s_R_e_e_l_i_m_i_n_a_t_e_d │ │ │ │ │ +size_t variablesReeliminated │ │ │ │ │ +The number of variables that were reeliminated as parts of the Bayes' Tree were │ │ │ │ │ +recalculated,... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Result.h:84 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_u_n_u_s_e_d_K_e_y_s │ │ │ │ │ +KeySet unusedKeys │ │ │ │ │ +Unused keys, and indices for unused keys, i.e., keys that are empty now and do │ │ │ │ │ +not appear in the new ... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Result.h:102 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_o_b_s_e_r_v_e_d_K_e_y_s │ │ │ │ │ +KeyVector observedKeys │ │ │ │ │ +keys for variables that were observed, i.e., not unused. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Result.h:105 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_m_a_r_k_e_d_K_e_y_s │ │ │ │ │ +KeySet markedKeys │ │ │ │ │ +All keys that were marked during the update process. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Result.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_n_e_w_F_a_c_t_o_r_s_I_n_d_i_c_e_s │ │ │ │ │ +FactorIndices newFactorsIndices │ │ │ │ │ +The indices of the newly-added factors, in 1-to-1 correspondence with the │ │ │ │ │ +factors passed as newFactor... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Result.h:97 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_e_r_r_o_r_A_f_t_e_r │ │ │ │ │ +boost::optional< double > errorAfter │ │ │ │ │ +The nonlinear error of all of the factors computed after the current update, │ │ │ │ │ +meaning that variables a... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Result.h:66 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s │ │ │ │ │ +A struct holding detailed results, which must be enabled with ISAM2Params:: │ │ │ │ │ +enableDetailedResults. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Result.h:117 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s_:_:_v_a_r_i_a_b_l_e_S_t_a_t_u_s │ │ │ │ │ +StatusMap variableStatus │ │ │ │ │ +The status of each variable during this update, see VariableStatus. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Result.h:153 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s_:_:_V_a_r_i_a_b_l_e_S_t_a_t_u_s │ │ │ │ │ +The status of a single variable, this struct is stored in DetailedResults:: │ │ │ │ │ +variableStatus. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Result.h:120 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s_:_:_V_a_r_i_a_b_l_e_S_t_a_t_u_s_:_:_i_s_R_e_l_i_n_e_a_r_i_z_e_I_n_v_o_l_v_e_d │ │ │ │ │ +bool isRelinearizeInvolved │ │ │ │ │ +Whether the variable was below the relinearization threshold but was │ │ │ │ │ +relinearized by being involved i... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Result.h:128 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s_:_:_V_a_r_i_a_b_l_e_S_t_a_t_u_s_:_:_i_s_O_b_s_e_r_v_e_d │ │ │ │ │ +bool isObserved │ │ │ │ │ +Whether the variable was relinearized, either by being above the │ │ │ │ │ +relinearization threshold or by invo... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Result.h:136 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s_:_:_V_a_r_i_a_b_l_e_S_t_a_t_u_s_:_:_i_s_N_e_w │ │ │ │ │ +bool isNew │ │ │ │ │ +Whether the variable itself was just added. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Result.h:138 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s_:_:_V_a_r_i_a_b_l_e_S_t_a_t_u_s_:_:_i_s_A_b_o_v_e_R_e_l_i_n_T_h_r_e_s_h_o_l_d │ │ │ │ │ +bool isAboveRelinThreshold │ │ │ │ │ +Whether the variable was just relinearized due to being above the │ │ │ │ │ +relinearization threshold. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Result.h:125 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s_:_:_V_a_r_i_a_b_l_e_S_t_a_t_u_s_:_:_i_s_R_e_e_l_i_m_i_n_a_t_e_d │ │ │ │ │ +bool isReeliminated │ │ │ │ │ +Whether the variable was just reeliminated, due to being relinearized, │ │ │ │ │ +observed, new,... │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Result.h:124 │ │ │ │ │ +_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s_:_:_V_a_r_i_a_b_l_e_S_t_a_t_u_s_:_:_i_n_R_o_o_t_C_l_i_q_u_e │ │ │ │ │ +bool inRootClique │ │ │ │ │ +Whether the variable is in the root clique. │ │ │ │ │ +DDeeffiinniittiioonn ISAM2Result.h:139 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_._h │ │ │ │ │ + * _I_S_A_M_2_R_e_s_u_l_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01136.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01265.html │ │ │ │┄ Files 92% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearFactor.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/SmartFactorBase.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
NonlinearFactor.cpp File Reference
│ │ │ │ +
SmartFactorBase.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Nonlinear Factor base classes. │ │ │ │ +

Base class to create smart factors on poses or cameras. │ │ │ │ More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

class  gtsam::SmartFactorBase< CAMERA >
 Base class for smart factors. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Nonlinear Factor base classes.

│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ +

Base class to create smart factors on poses or cameras.

│ │ │ │ +
Author
Luca Carlone
│ │ │ │ +
│ │ │ │ +Antoni Rosinol
│ │ │ │ +
│ │ │ │ +Zsolt Kira
│ │ │ │ +
│ │ │ │ +Frank Dellaert
│ │ │ │
│ │ │ │ -Richard Roberts
│ │ │ │ +Chris Beall
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,29 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -NonlinearFactor.cpp File Reference │ │ │ │ │ -Nonlinear Factor base classes. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +SmartFactorBase.h File Reference │ │ │ │ │ +Base class to create smart factors on poses or cameras. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +class   _g_t_s_a_m_:_:_S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_<_ _C_A_M_E_R_A_ _> │ │ │ │ │ +  Base class for smart factors. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Nonlinear Factor base classes. │ │ │ │ │ +Base class to create smart factors on poses or cameras. │ │ │ │ │ Author │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Antoni Rosinol │ │ │ │ │ + Zsolt Kira │ │ │ │ │ Frank Dellaert │ │ │ │ │ - Richard Roberts │ │ │ │ │ + Chris Beall │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._c_p_p │ │ │ │ │ + * _s_l_a_m │ │ │ │ │ + * _S_m_a_r_t_F_a_c_t_o_r_B_a_s_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01142_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00086_source.html │ │ │ │┄ Files 94% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/GenericValue.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ISAM2.h
│ │ │ │ +
GenericValue.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19// \callgraph
│ │ │ │ -
20
│ │ │ │ -
21#pragma once
│ │ │ │ -
22
│ │ │ │ -
23#include <gtsam/linear/GaussianBayesTree.h>
│ │ │ │ -
24#include <gtsam/nonlinear/ISAM2Clique.h>
│ │ │ │ -
25#include <gtsam/nonlinear/ISAM2Params.h>
│ │ │ │ -
26#include <gtsam/nonlinear/ISAM2Result.h>
│ │ │ │ -
27#include <gtsam/nonlinear/ISAM2UpdateParams.h>
│ │ │ │ -
28#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ -
29
│ │ │ │ -
30#include <vector>
│ │ │ │ -
31
│ │ │ │ -
32namespace gtsam {
│ │ │ │ -
33
│ │ │ │ -
│ │ │ │ -
45class GTSAM_EXPORT ISAM2 : public BayesTree<ISAM2Clique> {
│ │ │ │ -
46 protected:
│ │ │ │ -
48 Values theta_;
│ │ │ │ -
49
│ │ │ │ -
52 VariableIndex variableIndex_;
│ │ │ │ -
53
│ │ │ │ -
61 mutable VectorValues delta_;
│ │ │ │ -
62
│ │ │ │ -
63 mutable VectorValues deltaNewton_; // Only used when using Dogleg - stores
│ │ │ │ -
64 // the Gauss-Newton update
│ │ │ │ -
65 mutable VectorValues RgProd_; // Only used when using Dogleg - stores R*g and
│ │ │ │ -
66 // is updated incrementally
│ │ │ │ -
67
│ │ │ │ -
76 mutable KeySet deltaReplacedMask_; // TODO(dellaert): Make sure accessed in
│ │ │ │ -
77 // the right way
│ │ │ │ -
78
│ │ │ │ -
81 NonlinearFactorGraph nonlinearFactors_;
│ │ │ │ -
82
│ │ │ │ -
84 mutable GaussianFactorGraph linearFactors_;
│ │ │ │ -
85
│ │ │ │ -
87 ISAM2Params params_;
│ │ │ │ -
88
│ │ │ │ -
90 mutable boost::optional<double> doglegDelta_;
│ │ │ │ -
91
│ │ │ │ -
94 KeySet fixedVariables_;
│ │ │ │ -
95
│ │ │ │ -
96 int update_count_;
│ │ │ │ +
12/*
│ │ │ │ +
13 * @file GenericValue.h
│ │ │ │ +
14 * @brief Wraps any type T so it can play as a Value
│ │ │ │ +
15 * @date October, 2014
│ │ │ │ +
16 * @author Michael Bosse, Abel Gawel, Renaud Dube
│ │ │ │ +
17 * based on DerivedValue.h by Duy Nguyen Ta
│ │ │ │ +
18 */
│ │ │ │ +
19
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/base/Manifold.h>
│ │ │ │ +
23#include <gtsam/base/types.h>
│ │ │ │ +
24#include <gtsam/base/Value.h>
│ │ │ │ +
25
│ │ │ │ +
26#include <boost/make_shared.hpp>
│ │ │ │ +
27#include <boost/pool/pool_alloc.hpp>
│ │ │ │ +
28
│ │ │ │ +
29#include <cmath>
│ │ │ │ +
30#include <iostream>
│ │ │ │ +
31#include <typeinfo> // operator typeid
│ │ │ │ +
32
│ │ │ │ +
33#ifdef _WIN32
│ │ │ │ +
34#define GENERICVALUE_VISIBILITY
│ │ │ │ +
35#else
│ │ │ │ +
36// This will trigger a LNKxxxx on MSVC, so disable for MSVC build
│ │ │ │ +
37// Please refer to https://github.com/borglab/gtsam/blob/develop/Using-GTSAM-EXPORT.md
│ │ │ │ +
38#define GENERICVALUE_VISIBILITY GTSAM_EXPORT
│ │ │ │ +
39#endif
│ │ │ │ +
40
│ │ │ │ +
41namespace gtsam {
│ │ │ │ +
42
│ │ │ │ +
46template<class T>
│ │ │ │ +
│ │ │ │ +
47class GenericValue: public Value {
│ │ │ │ +
48
│ │ │ │ +
49public:
│ │ │ │ +
50
│ │ │ │ +
51 typedef T type;
│ │ │ │ +
52
│ │ │ │ +
53protected:
│ │ │ │ +
54
│ │ │ │ +
55 T value_;
│ │ │ │ +
56
│ │ │ │ +
57public:
│ │ │ │ +
58 // Only needed for serialization.
│ │ │ │ +
59 GenericValue(){}
│ │ │ │ +
60
│ │ │ │ +
│ │ │ │ +
62 GenericValue(const T& value) :
│ │ │ │ +
63 value_(value) {
│ │ │ │ +
64 }
│ │ │ │ +
│ │ │ │ +
65
│ │ │ │ +
│ │ │ │ +
67 const T& value() const {
│ │ │ │ +
68 return value_;
│ │ │ │ +
69 }
│ │ │ │ +
│ │ │ │ +
70
│ │ │ │ +
│ │ │ │ +
72 T& value() {
│ │ │ │ +
73 return value_;
│ │ │ │ +
74 }
│ │ │ │ +
│ │ │ │ +
75
│ │ │ │ +
│ │ │ │ +
77 ~GenericValue() override {
│ │ │ │ +
78 }
│ │ │ │ +
│ │ │ │ +
79
│ │ │ │ +
│ │ │ │ +
81 bool equals_(const Value& p, double tol = 1e-9) const override {
│ │ │ │ +
82 // Cast the base class Value pointer to a templated generic class pointer
│ │ │ │ +
83 const GenericValue& genericValue2 = static_cast<const GenericValue&>(p);
│ │ │ │ +
84 // Return the result of using the equals traits for the derived class
│ │ │ │ +
85 return traits<T>::Equals(this->value_, genericValue2.value_, tol);
│ │ │ │ +
86 }
│ │ │ │ +
│ │ │ │ +
87
│ │ │ │ +
│ │ │ │ +
89 bool equals(const GenericValue &other, double tol = 1e-9) const {
│ │ │ │ +
90 return traits<T>::Equals(this->value(), other.value(), tol);
│ │ │ │ +
91 }
│ │ │ │ +
│ │ │ │ +
92
│ │ │ │ +
│ │ │ │ +
94 void print(const std::string& str) const override {
│ │ │ │ +
95 std::cout << "(" << demangle(typeid(T).name()) << ")\n";
│ │ │ │ +
96 traits<T>::Print(value_, str);
│ │ │ │ +
97 }
│ │ │ │ +
│ │ │ │
98
│ │ │ │ -
99 public:
│ │ │ │ -
100 using This = ISAM2;
│ │ │ │ -
101 using Base = BayesTree<ISAM2Clique>;
│ │ │ │ -
102 using Clique = Base::Clique;
│ │ │ │ -
103 using sharedClique = Base::sharedClique;
│ │ │ │ -
104 using Cliques = Base::Cliques;
│ │ │ │ -
105
│ │ │ │ -
107 explicit ISAM2(const ISAM2Params& params);
│ │ │ │ -
108
│ │ │ │ -
111 ISAM2();
│ │ │ │ -
112
│ │ │ │ -
114 virtual ~ISAM2() {}
│ │ │ │ -
115
│ │ │ │ -
117 virtual bool equals(const ISAM2& other, double tol = 1e-9) const;
│ │ │ │ -
118
│ │ │ │ -
151 virtual ISAM2Result update(
│ │ │ │ -
152 const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
│ │ │ │ -
153 const Values& newTheta = Values(),
│ │ │ │ -
154 const FactorIndices& removeFactorIndices = FactorIndices(),
│ │ │ │ -
155 const boost::optional<FastMap<Key, int> >& constrainedKeys = boost::none,
│ │ │ │ -
156 const boost::optional<FastList<Key> >& noRelinKeys = boost::none,
│ │ │ │ -
157 const boost::optional<FastList<Key> >& extraReelimKeys = boost::none,
│ │ │ │ -
158 bool force_relinearize = false);
│ │ │ │ -
159
│ │ │ │ -
178 virtual ISAM2Result update(const NonlinearFactorGraph& newFactors,
│ │ │ │ -
179 const Values& newTheta,
│ │ │ │ -
180 const ISAM2UpdateParams& updateParams);
│ │ │ │ -
181
│ │ │ │ -
199 void marginalizeLeaves(
│ │ │ │ -
200 const FastList<Key>& leafKeys,
│ │ │ │ -
201 boost::optional<FactorIndices&> marginalFactorsIndices = boost::none,
│ │ │ │ -
202 boost::optional<FactorIndices&> deletedFactorsIndices = boost::none);
│ │ │ │ -
203
│ │ │ │ -
205 const Values& getLinearizationPoint() const { return theta_; }
│ │ │ │ -
206
│ │ │ │ -
208 bool valueExists(Key key) const { return theta_.exists(key); }
│ │ │ │ -
209
│ │ │ │ -
215 Values calculateEstimate() const;
│ │ │ │ +
│ │ │ │ +
102 Value* clone_() const override {
│ │ │ │ +
103 GenericValue* ptr = new GenericValue(*this); // calls copy constructor to fill in
│ │ │ │ +
104 return ptr;
│ │ │ │ +
105 }
│ │ │ │ +
│ │ │ │ +
106
│ │ │ │ +
│ │ │ │ +
110 void deallocate_() const override {
│ │ │ │ +
111 delete this;
│ │ │ │ +
112 }
│ │ │ │ +
│ │ │ │ +
113
│ │ │ │ +
│ │ │ │ +
117 boost::shared_ptr<Value> clone() const override {
│ │ │ │ +
118 return boost::allocate_shared<GenericValue>(Eigen::aligned_allocator<GenericValue>(), *this);
│ │ │ │ +
119 }
│ │ │ │ +
│ │ │ │ +
120
│ │ │ │ +
│ │ │ │ +
122 Value* retract_(const Vector& delta) const override {
│ │ │ │ +
123 // Call retract on the derived class using the retract trait function
│ │ │ │ +
124 const T retractResult = traits<T>::Retract(GenericValue<T>::value(), delta);
│ │ │ │ +
125
│ │ │ │ +
126 Value* resultAsValue = new GenericValue(retractResult);
│ │ │ │ +
127
│ │ │ │ +
128 // Return the pointer to the Value base class
│ │ │ │ +
129 return resultAsValue;
│ │ │ │ +
130 }
│ │ │ │ +
│ │ │ │ +
131
│ │ │ │ +
│ │ │ │ +
133 Vector localCoordinates_(const Value& value2) const override {
│ │ │ │ +
134 // Cast the base class Value pointer to a templated generic class pointer
│ │ │ │ +
135 const GenericValue<T>& genericValue2 =
│ │ │ │ +
136 static_cast<const GenericValue<T>&>(value2);
│ │ │ │ +
137
│ │ │ │ +
138 // Return the result of calling localCoordinates trait on the derived class
│ │ │ │ +
139 return traits<T>::Local(GenericValue<T>::value(), genericValue2.value());
│ │ │ │ +
140 }
│ │ │ │ +
│ │ │ │ +
141
│ │ │ │ +
│ │ │ │ +
143 GenericValue retract(const Vector& delta) const {
│ │ │ │ +
144 return GenericValue(traits<T>::Retract(GenericValue<T>::value(), delta));
│ │ │ │ +
145 }
│ │ │ │ +
│ │ │ │ +
146
│ │ │ │ +
│ │ │ │ +
148 Vector localCoordinates(const GenericValue& value2) const {
│ │ │ │ +
149 return localCoordinates_(value2);
│ │ │ │ +
150 }
│ │ │ │ +
│ │ │ │ +
151
│ │ │ │ +
│ │ │ │ +
153 size_t dim() const override {
│ │ │ │ +
154 return traits<T>::GetDimension(value_);
│ │ │ │ +
155 }
│ │ │ │ +
│ │ │ │ +
156
│ │ │ │ +
│ │ │ │ +
158 Value& operator=(const Value& rhs) override {
│ │ │ │ +
159 // Cast the base class Value pointer to a derived class pointer
│ │ │ │ +
160 const GenericValue& derivedRhs = static_cast<const GenericValue&>(rhs);
│ │ │ │ +
161
│ │ │ │ +
162 // Do the assignment and return the result
│ │ │ │ +
163 *this = GenericValue(derivedRhs); // calls copy constructor
│ │ │ │ +
164 return *this;
│ │ │ │ +
165 }
│ │ │ │ +
│ │ │ │ +
166
│ │ │ │ +
167 protected:
│ │ │ │ +
168
│ │ │ │ +
│ │ │ │ +
171 GenericValue<T>& operator=(const GenericValue<T>& rhs) {
│ │ │ │ +
172 Value::operator=(static_cast<Value const&>(rhs));
│ │ │ │ +
173 value_ = rhs.value_;
│ │ │ │ +
174 return *this;
│ │ │ │ +
175 }
│ │ │ │ +
│ │ │ │ +
176
│ │ │ │ +
177 private:
│ │ │ │ +
178
│ │ │ │ +
180 friend class boost::serialization::access;
│ │ │ │ +
181 template<class ARCHIVE>
│ │ │ │ +
182 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
183 ar & boost::serialization::make_nvp("GenericValue",
│ │ │ │ +
184 boost::serialization::base_object<Value>(*this));
│ │ │ │ +
185 ar & boost::serialization::make_nvp("value", value_);
│ │ │ │ +
186 }
│ │ │ │ +
187
│ │ │ │ +
188
│ │ │ │ +
189 // Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
│ │ │ │ +
190 enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
│ │ │ │ +
191public:
│ │ │ │ +
192 GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
│ │ │ │ +
193};
│ │ │ │ +
│ │ │ │ +
194
│ │ │ │ +
196#define GTSAM_VALUE_EXPORT(Type) BOOST_CLASS_EXPORT(gtsam::GenericValue<Type>)
│ │ │ │ +
197
│ │ │ │ +
198// traits
│ │ │ │ +
199template <typename ValueType>
│ │ │ │ +
│ │ │ │ +
200struct traits<GenericValue<ValueType> >
│ │ │ │ +
201 : public Testable<GenericValue<ValueType> > {};
│ │ │ │ +
│ │ │ │ +
202
│ │ │ │ +
203// define Value::cast here since now GenericValue has been declared
│ │ │ │ +
204template<typename ValueType>
│ │ │ │ +
│ │ │ │ +
205const ValueType& Value::cast() const {
│ │ │ │ +
206 return dynamic_cast<const GenericValue<ValueType>&>(*this).value();
│ │ │ │ +
207}
│ │ │ │ +
│ │ │ │ +
208
│ │ │ │ +
211template<class T>
│ │ │ │ +
│ │ │ │ +
212GenericValue<T> genericValue(const T& v) {
│ │ │ │ +
213 return GenericValue<T>(v);
│ │ │ │ +
214}
│ │ │ │ +
│ │ │ │ +
215
│ │ │ │
216
│ │ │ │ -
223 template <class VALUE>
│ │ │ │ -
│ │ │ │ -
224 VALUE calculateEstimate(Key key) const {
│ │ │ │ -
225 const Vector& delta = getDelta()[key];
│ │ │ │ -
226 return traits<VALUE>::Retract(theta_.at<VALUE>(key), delta);
│ │ │ │ -
227 }
│ │ │ │ -
│ │ │ │ -
228
│ │ │ │ -
237 const Value& calculateEstimate(Key key) const;
│ │ │ │ -
238
│ │ │ │ -
240 Matrix marginalCovariance(Key key) const;
│ │ │ │ -
241
│ │ │ │ -
244
│ │ │ │ -
248 Values calculateBestEstimate() const;
│ │ │ │ -
249
│ │ │ │ -
251 const VectorValues& getDelta() const;
│ │ │ │ -
252
│ │ │ │ -
254 double error(const VectorValues& x) const;
│ │ │ │ -
255
│ │ │ │ -
│ │ │ │ -
257 const NonlinearFactorGraph& getFactorsUnsafe() const {
│ │ │ │ -
258 return nonlinearFactors_;
│ │ │ │ -
259 }
│ │ │ │ -
│ │ │ │ -
260
│ │ │ │ -
262 const VariableIndex& getVariableIndex() const { return variableIndex_; }
│ │ │ │ -
263
│ │ │ │ -
265 const KeySet& getFixedVariables() const { return fixedVariables_; }
│ │ │ │ -
266
│ │ │ │ -
267 const ISAM2Params& params() const { return params_; }
│ │ │ │ -
268
│ │ │ │ -
270 void printStats() const { getCliqueData().getStats().print(); }
│ │ │ │ -
271
│ │ │ │ -
279 VectorValues gradientAtZero() const;
│ │ │ │ -
280
│ │ │ │ -
282
│ │ │ │ -
283 protected:
│ │ │ │ -
285 void recalculate(const ISAM2UpdateParams& updateParams,
│ │ │ │ -
286 const KeySet& relinKeys, ISAM2Result* result);
│ │ │ │ -
287
│ │ │ │ -
288 // Do a batch step - reorder and relinearize all variables
│ │ │ │ -
289 void recalculateBatch(const ISAM2UpdateParams& updateParams,
│ │ │ │ -
290 KeySet* affectedKeysSet, ISAM2Result* result);
│ │ │ │ -
291
│ │ │ │ -
292 // retrieve all factors that ONLY contain the affected variables
│ │ │ │ -
293 // (note that the remaining stuff is summarized in the cached factors)
│ │ │ │ -
294 GaussianFactorGraph relinearizeAffectedFactors(
│ │ │ │ -
295 const ISAM2UpdateParams& updateParams, const FastList<Key>& affectedKeys,
│ │ │ │ -
296 const KeySet& relinKeys);
│ │ │ │ -
297
│ │ │ │ -
309 void recalculateIncremental(const ISAM2UpdateParams& updateParams,
│ │ │ │ -
310 const KeySet& relinKeys,
│ │ │ │ -
311 const FastList<Key>& affectedKeys,
│ │ │ │ -
312 KeySet* affectedKeysSet, Cliques* orphans,
│ │ │ │ -
313 ISAM2Result* result);
│ │ │ │ -
314
│ │ │ │ -
320 void addVariables(const Values& newTheta,
│ │ │ │ -
321 ISAM2Result::DetailedResults* detail = 0);
│ │ │ │ -
322
│ │ │ │ -
326 void removeVariables(const KeySet& unusedKeys);
│ │ │ │ -
327
│ │ │ │ -
328 void updateDelta(bool forceFullSolve = false) const;
│ │ │ │ -
329
│ │ │ │ -
330 private:
│ │ │ │ -
332 friend class boost::serialization::access;
│ │ │ │ -
333 template<class ARCHIVE>
│ │ │ │ -
334 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
335 ar & boost::serialization::base_object<BayesTree<ISAM2Clique> >(*this);
│ │ │ │ -
336 ar & BOOST_SERIALIZATION_NVP(theta_);
│ │ │ │ -
337 ar & BOOST_SERIALIZATION_NVP(variableIndex_);
│ │ │ │ -
338 ar & BOOST_SERIALIZATION_NVP(delta_);
│ │ │ │ -
339 ar & BOOST_SERIALIZATION_NVP(deltaNewton_);
│ │ │ │ -
340 ar & BOOST_SERIALIZATION_NVP(RgProd_);
│ │ │ │ -
341 ar & BOOST_SERIALIZATION_NVP(deltaReplacedMask_);
│ │ │ │ -
342 ar & BOOST_SERIALIZATION_NVP(nonlinearFactors_);
│ │ │ │ -
343 ar & BOOST_SERIALIZATION_NVP(linearFactors_);
│ │ │ │ -
344 ar & BOOST_SERIALIZATION_NVP(doglegDelta_);
│ │ │ │ -
345 ar & BOOST_SERIALIZATION_NVP(fixedVariables_);
│ │ │ │ -
346 ar & BOOST_SERIALIZATION_NVP(update_count_);
│ │ │ │ -
347 }
│ │ │ │ -
348
│ │ │ │ -
349}; // ISAM2
│ │ │ │ -
│ │ │ │ -
350
│ │ │ │ -
352template <>
│ │ │ │ -
353struct traits<ISAM2> : public Testable<ISAM2> {};
│ │ │ │ -
354
│ │ │ │ -
355} // namespace gtsam
│ │ │ │ -
GaussianBayesTree.h
Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree.
│ │ │ │ -
ISAM2Params.h
Parameters for iSAM 2.
│ │ │ │ -
ISAM2Clique.h
Specialized iSAM2 Clique.
│ │ │ │ -
ISAM2UpdateParams.h
Class that stores extra params for ISAM2::update()
│ │ │ │ -
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │ -
ISAM2Result.h
Class that stores detailed iSAM2 result.
│ │ │ │ +
217} /* namespace gtsam */
│ │ │ │ +
Value.h
The base class for any variable that can be optimized or used in a factor.
│ │ │ │ +
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ +
types.h
Typedefs for easier changing of types.
│ │ │ │ +
GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
This marks a GTSAM object to require alignment.
Definition types.h:317
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::FactorIndices
FastVector< FactorIndex > FactorIndices
Define collection types:
Definition Factor.h:34
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::demangle
std::string demangle(const char *name)
Pretty print Value type name.
Definition types.cpp:37
│ │ │ │ +
gtsam::genericValue
GenericValue< T > genericValue(const T &v)
Functional constructor of GenericValue<T> so T can be automatically deduced.
Definition GenericValue.h:212
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::FastList
FastList is a thin wrapper around std::list that uses the boost fast_pool_allocator instead of the de...
Definition FastList.h:40
│ │ │ │ -
gtsam::FastMap
FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the defa...
Definition FastMap.h:38
│ │ │ │ - │ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ +
gtsam::GenericValue
Wraps any type T so it can play as a Value.
Definition GenericValue.h:47
│ │ │ │ +
gtsam::GenericValue::clone_
Value * clone_() const override
Create a duplicate object returned as a pointer to the generic Value interface.
Definition GenericValue.h:102
│ │ │ │ +
gtsam::GenericValue::value
T & value()
Return the value.
Definition GenericValue.h:72
│ │ │ │ +
gtsam::GenericValue::retract_
Value * retract_(const Vector &delta) const override
Generic Value interface version of retract.
Definition GenericValue.h:122
│ │ │ │ +
gtsam::GenericValue::operator=
Value & operator=(const Value &rhs) override
Assignment operator.
Definition GenericValue.h:158
│ │ │ │ +
gtsam::GenericValue::print
void print(const std::string &str) const override
Virtual print function, uses traits.
Definition GenericValue.h:94
│ │ │ │ +
gtsam::GenericValue::equals
bool equals(const GenericValue &other, double tol=1e-9) const
non virtual equals function, uses traits
Definition GenericValue.h:89
│ │ │ │ +
gtsam::GenericValue::dim
size_t dim() const override
Return run-time dimensionality.
Definition GenericValue.h:153
│ │ │ │ +
gtsam::GenericValue::GenericValue
GenericValue(const T &value)
Construct from value.
Definition GenericValue.h:62
│ │ │ │ +
gtsam::GenericValue::deallocate_
void deallocate_() const override
Destroy and deallocate this object, only if it was originally allocated using clone_().
Definition GenericValue.h:110
│ │ │ │ +
gtsam::GenericValue::value_
T value_
The wrapped value.
Definition GenericValue.h:55
│ │ │ │ +
gtsam::GenericValue::operator=
GenericValue< T > & operator=(const GenericValue< T > &rhs)
Assignment operator, protected because only the Value or DERIVED assignment operators should be used.
Definition GenericValue.h:171
│ │ │ │ +
gtsam::GenericValue::localCoordinates
Vector localCoordinates(const GenericValue &value2) const
Non-virtual version of localCoordinates.
Definition GenericValue.h:148
│ │ │ │ +
gtsam::GenericValue::access
friend class boost::serialization::access
Serialization function.
Definition GenericValue.h:180
│ │ │ │ +
gtsam::GenericValue::equals_
bool equals_(const Value &p, double tol=1e-9) const override
equals implementing generic Value interface
Definition GenericValue.h:81
│ │ │ │ +
gtsam::GenericValue::value
const T & value() const
Return a constant value.
Definition GenericValue.h:67
│ │ │ │ +
gtsam::GenericValue::~GenericValue
~GenericValue() override
Destructor.
Definition GenericValue.h:77
│ │ │ │ +
gtsam::GenericValue::retract
GenericValue retract(const Vector &delta) const
Non-virtual version of retract.
Definition GenericValue.h:143
│ │ │ │ +
gtsam::GenericValue::localCoordinates_
Vector localCoordinates_(const Value &value2) const override
Generic Value interface version of localCoordinates.
Definition GenericValue.h:133
│ │ │ │ +
gtsam::GenericValue::clone
boost::shared_ptr< Value > clone() const override
Clone this value (normal clone on the heap, delete with 'delete' operator)
Definition GenericValue.h:117
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │
gtsam::Value
This is the base class for any type to be stored in Values.
Definition Value.h:37
│ │ │ │ -
gtsam::BayesTree
Bayes tree.
Definition BayesTree.h:67
│ │ │ │ -
gtsam::BayesTree< ISAM2Clique >::sharedClique
boost::shared_ptr< Clique > sharedClique
Shared pointer to a clique.
Definition BayesTree.h:74
│ │ │ │ -
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition VariableIndex.h:43
│ │ │ │ -
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition GaussianFactorGraph.h:75
│ │ │ │ -
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ -
gtsam::ISAM2
Implementation of the full ISAM2 algorithm for incremental nonlinear optimization.
Definition ISAM2.h:45
│ │ │ │ -
gtsam::ISAM2::fixedVariables_
KeySet fixedVariables_
Set of variables that are involved with linear factors from marginalized variables and thus cannot ha...
Definition ISAM2.h:94
│ │ │ │ -
gtsam::ISAM2::deltaReplacedMask_
KeySet deltaReplacedMask_
A cumulative mask for the variables that were replaced and have not yet been updated in the linear so...
Definition ISAM2.h:76
│ │ │ │ -
gtsam::ISAM2::update_count_
int update_count_
Counter incremented every update(), used to determine periodic relinearization.
Definition ISAM2.h:96
│ │ │ │ -
gtsam::ISAM2::calculateEstimate
VALUE calculateEstimate(Key key) const
Compute an estimate for a single variable using its incomplete linear delta computed during the last ...
Definition ISAM2.h:224
│ │ │ │ -
gtsam::ISAM2::~ISAM2
virtual ~ISAM2()
default virtual destructor
Definition ISAM2.h:114
│ │ │ │ -
gtsam::ISAM2::getFixedVariables
const KeySet & getFixedVariables() const
Access the nonlinear variable index.
Definition ISAM2.h:265
│ │ │ │ -
gtsam::ISAM2::Cliques
Base::Cliques Cliques
List of Cliques.
Definition ISAM2.h:104
│ │ │ │ -
gtsam::ISAM2::printStats
void printStats() const
prints out clique statistics
Definition ISAM2.h:270
│ │ │ │ -
gtsam::ISAM2::nonlinearFactors_
NonlinearFactorGraph nonlinearFactors_
All original nonlinear factors are stored here to use during relinearization.
Definition ISAM2.h:81
│ │ │ │ -
gtsam::ISAM2::getVariableIndex
const VariableIndex & getVariableIndex() const
Access the nonlinear variable index.
Definition ISAM2.h:262
│ │ │ │ -
gtsam::ISAM2::sharedClique
Base::sharedClique sharedClique
Shared pointer to a clique.
Definition ISAM2.h:103
│ │ │ │ -
gtsam::ISAM2::delta_
VectorValues delta_
The linear delta from the last linear solution, an update to the estimate in theta.
Definition ISAM2.h:61
│ │ │ │ -
gtsam::ISAM2::variableIndex_
VariableIndex variableIndex_
VariableIndex lets us look up factors by involved variable and keeps track of dimensions.
Definition ISAM2.h:52
│ │ │ │ -
gtsam::ISAM2::getLinearizationPoint
const Values & getLinearizationPoint() const
Access the current linearization point.
Definition ISAM2.h:205
│ │ │ │ -
gtsam::ISAM2::theta_
Values theta_
The current linearization point.
Definition ISAM2.h:48
│ │ │ │ -
gtsam::ISAM2::params_
ISAM2Params params_
The current parameters.
Definition ISAM2.h:87
│ │ │ │ -
gtsam::ISAM2::doglegDelta_
boost::optional< double > doglegDelta_
The current Dogleg Delta (trust region radius)
Definition ISAM2.h:90
│ │ │ │ -
gtsam::ISAM2::linearFactors_
GaussianFactorGraph linearFactors_
The current linear factors, which are only updated as needed.
Definition ISAM2.h:84
│ │ │ │ -
gtsam::ISAM2::getFactorsUnsafe
const NonlinearFactorGraph & getFactorsUnsafe() const
Access the set of nonlinear factors.
Definition ISAM2.h:257
│ │ │ │ -
gtsam::ISAM2::valueExists
bool valueExists(Key key) const
Check whether variable with given key exists in linearization point.
Definition ISAM2.h:208
│ │ │ │ -
gtsam::ISAM2Clique
Specialized Clique structure for ISAM2, incorporating caching and gradient contribution TODO: more do...
Definition ISAM2Clique.h:37
│ │ │ │ -
gtsam::ISAM2Params
Definition ISAM2Params.h:135
│ │ │ │ -
gtsam::ISAM2Result
This struct is returned from ISAM2::update() and contains information about the update that is useful...
Definition ISAM2Result.h:41
│ │ │ │ -
gtsam::ISAM2Result::DetailedResults
A struct holding detailed results, which must be enabled with ISAM2Params::enableDetailedResults.
Definition ISAM2Result.h:117
│ │ │ │ -
gtsam::ISAM2UpdateParams
This struct is used by ISAM2::update() to pass additional parameters to give the user a fine-grained ...
Definition ISAM2UpdateParams.h:32
│ │ │ │ -
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │ -
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
gtsam::Values::at
const ValueType at(Key j) const
Retrieve a variable by key j.
Definition Values-inl.h:361
│ │ │ │ -
gtsam::Values::exists
bool exists(Key j) const
Check if a value exists with key j.
Definition Values.cpp:94
│ │ │ │ +
gtsam::Value::cast
const ValueType & cast() const
Cast to known ValueType.
Definition GenericValue.h:205
│ │ │ │ +
gtsam::Value::operator=
virtual Value & operator=(const Value &)
Assignment operator.
Definition Value.h:79
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,373 +1,324 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ISAM2.h │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +GenericValue.h │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19// \callgraph │ │ │ │ │ -20 │ │ │ │ │ -21#pragma once │ │ │ │ │ -22 │ │ │ │ │ -23#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_I_S_A_M_2_C_l_i_q_u_e_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_I_S_A_M_2_P_a_r_a_m_s_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_I_S_A_M_2_R_e_s_u_l_t_._h> │ │ │ │ │ -27#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_._h> │ │ │ │ │ -28#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -29 │ │ │ │ │ -30#include │ │ │ │ │ -31 │ │ │ │ │ -32namespace _g_t_s_a_m { │ │ │ │ │ -33 │ │ │ │ │ -_4_5class GTSAM_EXPORT _I_S_A_M_2 : public _B_a_y_e_s_T_r_e_e { │ │ │ │ │ -46 protected: │ │ │ │ │ -_4_8 _V_a_l_u_e_s _t_h_e_t_a__; │ │ │ │ │ -49 │ │ │ │ │ -_5_2 _V_a_r_i_a_b_l_e_I_n_d_e_x _v_a_r_i_a_b_l_e_I_n_d_e_x__; │ │ │ │ │ -53 │ │ │ │ │ -_6_1 mutable _V_e_c_t_o_r_V_a_l_u_e_s _d_e_l_t_a__; │ │ │ │ │ -62 │ │ │ │ │ -63 mutable _V_e_c_t_o_r_V_a_l_u_e_s deltaNewton_; // Only used when using Dogleg - stores │ │ │ │ │ -64 // the Gauss-Newton update │ │ │ │ │ -65 mutable _V_e_c_t_o_r_V_a_l_u_e_s RgProd_; // Only used when using Dogleg - stores R*g │ │ │ │ │ -and │ │ │ │ │ -66 // is updated incrementally │ │ │ │ │ -67 │ │ │ │ │ -_7_6 mutable _K_e_y_S_e_t _d_e_l_t_a_R_e_p_l_a_c_e_d_M_a_s_k__; // TODO(dellaert): Make sure accessed in │ │ │ │ │ -77 // the right way │ │ │ │ │ -78 │ │ │ │ │ -_8_1 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h _n_o_n_l_i_n_e_a_r_F_a_c_t_o_r_s__; │ │ │ │ │ -82 │ │ │ │ │ -_8_4 mutable _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h _l_i_n_e_a_r_F_a_c_t_o_r_s__; │ │ │ │ │ -85 │ │ │ │ │ -_8_7 _I_S_A_M_2_P_a_r_a_m_s _p_a_r_a_m_s__; │ │ │ │ │ -88 │ │ │ │ │ -_9_0 mutable boost::optional _d_o_g_l_e_g_D_e_l_t_a__; │ │ │ │ │ -91 │ │ │ │ │ -_9_4 _K_e_y_S_e_t _f_i_x_e_d_V_a_r_i_a_b_l_e_s__; │ │ │ │ │ -95 │ │ │ │ │ -_9_6 int _u_p_d_a_t_e___c_o_u_n_t__; │ │ │ │ │ +12/* │ │ │ │ │ +13 * @file GenericValue.h │ │ │ │ │ +14 * @brief Wraps any type T so it can play as a Value │ │ │ │ │ +15 * @date October, 2014 │ │ │ │ │ +16 * @author Michael Bosse, Abel Gawel, Renaud Dube │ │ │ │ │ +17 * based on DerivedValue.h by Duy Nguyen Ta │ │ │ │ │ +18 */ │ │ │ │ │ +19 │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_b_a_s_e_/_V_a_l_u_e_._h> │ │ │ │ │ +25 │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28 │ │ │ │ │ +29#include │ │ │ │ │ +30#include │ │ │ │ │ +31#include // operator typeid │ │ │ │ │ +32 │ │ │ │ │ +33#ifdef _WIN32 │ │ │ │ │ +34#define GENERICVALUE_VISIBILITY │ │ │ │ │ +35#else │ │ │ │ │ +36// This will trigger a LNKxxxx on MSVC, so disable for MSVC build │ │ │ │ │ +37// Please refer to https://github.com/borglab/gtsam/blob/develop/Using-GTSAM- │ │ │ │ │ +EXPORT.md │ │ │ │ │ +38#define GENERICVALUE_VISIBILITY GTSAM_EXPORT │ │ │ │ │ +39#endif │ │ │ │ │ +40 │ │ │ │ │ +41namespace _g_t_s_a_m { │ │ │ │ │ +42 │ │ │ │ │ +46template │ │ │ │ │ +_4_7class _G_e_n_e_r_i_c_V_a_l_u_e: public _V_a_l_u_e { │ │ │ │ │ +48 │ │ │ │ │ +49public: │ │ │ │ │ +50 │ │ │ │ │ +51 typedef T type; │ │ │ │ │ +52 │ │ │ │ │ +53protected: │ │ │ │ │ +54 │ │ │ │ │ +_5_5 T _v_a_l_u_e__; │ │ │ │ │ +56 │ │ │ │ │ +57public: │ │ │ │ │ +58 // Only needed for serialization. │ │ │ │ │ +59 _G_e_n_e_r_i_c_V_a_l_u_e(){} │ │ │ │ │ +60 │ │ │ │ │ +_6_2 _G_e_n_e_r_i_c_V_a_l_u_e(const T& _v_a_l_u_e) : │ │ │ │ │ +63 _v_a_l_u_e__(_v_a_l_u_e) { │ │ │ │ │ +64 } │ │ │ │ │ +65 │ │ │ │ │ +_6_7 const T& _v_a_l_u_e() const { │ │ │ │ │ +68 return _v_a_l_u_e__; │ │ │ │ │ +69 } │ │ │ │ │ +70 │ │ │ │ │ +_7_2 T& _v_a_l_u_e() { │ │ │ │ │ +73 return _v_a_l_u_e__; │ │ │ │ │ +74 } │ │ │ │ │ +75 │ │ │ │ │ +_7_7 _~_G_e_n_e_r_i_c_V_a_l_u_e() override { │ │ │ │ │ +78 } │ │ │ │ │ +79 │ │ │ │ │ +_8_1 bool _e_q_u_a_l_s__(const _V_a_l_u_e& p, double tol = 1e-9) const override { │ │ │ │ │ +82 // Cast the base class Value pointer to a templated generic class pointer │ │ │ │ │ +83 const _G_e_n_e_r_i_c_V_a_l_u_e& genericValue2 = static_cast(p); │ │ │ │ │ +84 // Return the result of using the equals traits for the derived class │ │ │ │ │ +85 return _t_r_a_i_t_s_<_T_>_:_:_E_q_u_a_l_s(this->value_, genericValue2._v_a_l_u_e__, tol); │ │ │ │ │ +86 } │ │ │ │ │ +87 │ │ │ │ │ +_8_9 bool _e_q_u_a_l_s(const _G_e_n_e_r_i_c_V_a_l_u_e &other, double tol = 1e-9) const { │ │ │ │ │ +90 return _t_r_a_i_t_s_<_T_>_:_:_E_q_u_a_l_s(this->_v_a_l_u_e(), other._v_a_l_u_e(), tol); │ │ │ │ │ +91 } │ │ │ │ │ +92 │ │ │ │ │ +_9_4 void _p_r_i_n_t(const std::string& str) const override { │ │ │ │ │ +95 std::cout << "(" << _d_e_m_a_n_g_l_e(typeid(T).name()) << ")\n"; │ │ │ │ │ +96 _t_r_a_i_t_s_<_T_>_:_:_P_r_i_n_t(_v_a_l_u_e__, str); │ │ │ │ │ +97 } │ │ │ │ │ 98 │ │ │ │ │ -99 public: │ │ │ │ │ -_1_0_0 using _T_h_i_s = _I_S_A_M_2; │ │ │ │ │ -_1_0_1 using _B_a_s_e = _B_a_y_e_s_T_r_e_e_<_I_S_A_M_2_C_l_i_q_u_e_>; │ │ │ │ │ -_1_0_2 using _C_l_i_q_u_e = _B_a_s_e_:_:_C_l_i_q_u_e; │ │ │ │ │ -_1_0_3 using _s_h_a_r_e_d_C_l_i_q_u_e = _B_a_s_e_:_:_s_h_a_r_e_d_C_l_i_q_u_e; │ │ │ │ │ -_1_0_4 using _C_l_i_q_u_e_s = Base::Cliques; │ │ │ │ │ -105 │ │ │ │ │ -107 explicit _I_S_A_M_2(const _I_S_A_M_2_P_a_r_a_m_s& params); │ │ │ │ │ -108 │ │ │ │ │ -111 _I_S_A_M_2(); │ │ │ │ │ -112 │ │ │ │ │ -_1_1_4 virtual _~_I_S_A_M_2() {} │ │ │ │ │ -115 │ │ │ │ │ -117 virtual bool _e_q_u_a_l_s(const _I_S_A_M_2& other, double tol = 1e-9) const; │ │ │ │ │ -118 │ │ │ │ │ -151 virtual _I_S_A_M_2_R_e_s_u_l_t update( │ │ │ │ │ -152 const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& newFactors = _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h(), │ │ │ │ │ -153 const _V_a_l_u_e_s& newTheta = _V_a_l_u_e_s(), │ │ │ │ │ -154 const _F_a_c_t_o_r_I_n_d_i_c_e_s& removeFactorIndices = _F_a_c_t_o_r_I_n_d_i_c_e_s(), │ │ │ │ │ -155 const boost::optional<_F_a_s_t_M_a_p_<_K_e_y_,_ _i_n_t_> >& constrainedKeys = boost::none, │ │ │ │ │ -156 const boost::optional<_F_a_s_t_L_i_s_t_<_K_e_y_> >& noRelinKeys = boost::none, │ │ │ │ │ -157 const boost::optional<_F_a_s_t_L_i_s_t_<_K_e_y_> >& extraReelimKeys = boost::none, │ │ │ │ │ -158 bool force_relinearize = false); │ │ │ │ │ -159 │ │ │ │ │ -178 virtual _I_S_A_M_2_R_e_s_u_l_t update(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& newFactors, │ │ │ │ │ -179 const _V_a_l_u_e_s& newTheta, │ │ │ │ │ -180 const _I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s& updateParams); │ │ │ │ │ -181 │ │ │ │ │ -199 void marginalizeLeaves( │ │ │ │ │ -200 const _F_a_s_t_L_i_s_t_<_K_e_y_>& leafKeys, │ │ │ │ │ -201 boost::optional marginalFactorsIndices = boost::none, │ │ │ │ │ -202 boost::optional deletedFactorsIndices = boost::none); │ │ │ │ │ -203 │ │ │ │ │ -_2_0_5 const _V_a_l_u_e_s& _g_e_t_L_i_n_e_a_r_i_z_a_t_i_o_n_P_o_i_n_t() const { return theta_; } │ │ │ │ │ -206 │ │ │ │ │ -_2_0_8 bool _v_a_l_u_e_E_x_i_s_t_s(_K_e_y key) const { return theta_._e_x_i_s_t_s(key); } │ │ │ │ │ -209 │ │ │ │ │ -215 _V_a_l_u_e_s calculateEstimate() const; │ │ │ │ │ +_1_0_2 _V_a_l_u_e* _c_l_o_n_e__() const override { │ │ │ │ │ +103 _G_e_n_e_r_i_c_V_a_l_u_e* ptr = new _G_e_n_e_r_i_c_V_a_l_u_e(*this); // calls copy constructor to │ │ │ │ │ +fill in │ │ │ │ │ +104 return ptr; │ │ │ │ │ +105 } │ │ │ │ │ +106 │ │ │ │ │ +_1_1_0 void _d_e_a_l_l_o_c_a_t_e__() const override { │ │ │ │ │ +111 delete this; │ │ │ │ │ +112 } │ │ │ │ │ +113 │ │ │ │ │ +_1_1_7 boost::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ +118 return boost::allocate_shared(Eigen:: │ │ │ │ │ +aligned_allocator(), *this); │ │ │ │ │ +119 } │ │ │ │ │ +120 │ │ │ │ │ +_1_2_2 _V_a_l_u_e* _r_e_t_r_a_c_t__(const Vector& delta) const override { │ │ │ │ │ +123 // Call retract on the derived class using the retract trait function │ │ │ │ │ +124 const T retractResult = _t_r_a_i_t_s_<_T_>_:_:_R_e_t_r_a_c_t(_G_e_n_e_r_i_c_V_a_l_u_e_<_T_>_:_:_v_a_l_u_e(), │ │ │ │ │ +delta); │ │ │ │ │ +125 │ │ │ │ │ +126 _V_a_l_u_e* resultAsValue = new _G_e_n_e_r_i_c_V_a_l_u_e(retractResult); │ │ │ │ │ +127 │ │ │ │ │ +128 // Return the pointer to the Value base class │ │ │ │ │ +129 return resultAsValue; │ │ │ │ │ +130 } │ │ │ │ │ +131 │ │ │ │ │ +_1_3_3 Vector _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s__(const _V_a_l_u_e& value2) const override { │ │ │ │ │ +134 // Cast the base class Value pointer to a templated generic class pointer │ │ │ │ │ +135 const _G_e_n_e_r_i_c_V_a_l_u_e_<_T_>& genericValue2 = │ │ │ │ │ +136 static_cast&>(value2); │ │ │ │ │ +137 │ │ │ │ │ +138 // Return the result of calling localCoordinates trait on the derived class │ │ │ │ │ +139 return _t_r_a_i_t_s_<_T_>_:_:_L_o_c_a_l(_G_e_n_e_r_i_c_V_a_l_u_e_<_T_>_:_:_v_a_l_u_e(), genericValue2._v_a_l_u_e()); │ │ │ │ │ +140 } │ │ │ │ │ +141 │ │ │ │ │ +_1_4_3 _G_e_n_e_r_i_c_V_a_l_u_e _r_e_t_r_a_c_t(const Vector& delta) const { │ │ │ │ │ +144 return _G_e_n_e_r_i_c_V_a_l_u_e(_t_r_a_i_t_s_<_T_>_:_:_R_e_t_r_a_c_t(_G_e_n_e_r_i_c_V_a_l_u_e_<_T_>_:_:_v_a_l_u_e(), delta)); │ │ │ │ │ +145 } │ │ │ │ │ +146 │ │ │ │ │ +_1_4_8 Vector _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s(const _G_e_n_e_r_i_c_V_a_l_u_e& value2) const { │ │ │ │ │ +149 return _l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s__(value2); │ │ │ │ │ +150 } │ │ │ │ │ +151 │ │ │ │ │ +_1_5_3 size_t _d_i_m() const override { │ │ │ │ │ +154 return _t_r_a_i_t_s_<_T_>_:_:_G_e_t_D_i_m_e_n_s_i_o_n(_v_a_l_u_e__); │ │ │ │ │ +155 } │ │ │ │ │ +156 │ │ │ │ │ +_1_5_8 _V_a_l_u_e& _o_p_e_r_a_t_o_r_=(const _V_a_l_u_e& rhs) override { │ │ │ │ │ +159 // Cast the base class Value pointer to a derived class pointer │ │ │ │ │ +160 const _G_e_n_e_r_i_c_V_a_l_u_e& derivedRhs = static_cast(rhs); │ │ │ │ │ +161 │ │ │ │ │ +162 // Do the assignment and return the result │ │ │ │ │ +163 *this = _G_e_n_e_r_i_c_V_a_l_u_e(derivedRhs); // calls copy constructor │ │ │ │ │ +164 return *this; │ │ │ │ │ +165 } │ │ │ │ │ +166 │ │ │ │ │ +167 protected: │ │ │ │ │ +168 │ │ │ │ │ +_1_7_1 _G_e_n_e_r_i_c_V_a_l_u_e_<_T_>& _o_p_e_r_a_t_o_r_=(const _G_e_n_e_r_i_c_V_a_l_u_e_<_T_>& rhs) { │ │ │ │ │ +172 _V_a_l_u_e_:_:_o_p_e_r_a_t_o_r_=(static_cast<_V_a_l_u_e const&>(rhs)); │ │ │ │ │ +173 _v_a_l_u_e__ = rhs._v_a_l_u_e__; │ │ │ │ │ +174 return *this; │ │ │ │ │ +175 } │ │ │ │ │ +176 │ │ │ │ │ +177 private: │ │ │ │ │ +178 │ │ │ │ │ +_1_8_0 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +181 template │ │ │ │ │ +182 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +183 ar & boost::serialization::make_nvp("GenericValue", │ │ │ │ │ +184 boost::serialization::base_object(*this)); │ │ │ │ │ +185 ar & boost::serialization::make_nvp("value", _v_a_l_u_e__); │ │ │ │ │ +186 } │ │ │ │ │ +187 │ │ │ │ │ +188 │ │ │ │ │ +189 // Alignment, see https://eigen.tuxfamily.org/dox/ │ │ │ │ │ +group__TopicStructHavingEigenMembers.html │ │ │ │ │ +190 enum { NeedsToAlign = (sizeof(T) % 16) == 0 }; │ │ │ │ │ +191public: │ │ │ │ │ +192 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W___I_F(NeedsToAlign) │ │ │ │ │ +193}; │ │ │ │ │ +194 │ │ │ │ │ +196#define GTSAM_VALUE_EXPORT(Type) BOOST_CLASS_EXPORT(gtsam:: │ │ │ │ │ +GenericValue) │ │ │ │ │ +197 │ │ │ │ │ +198// traits │ │ │ │ │ +199template │ │ │ │ │ +_2_0_0struct _t_r_a_i_t_s<_G_e_n_e_r_i_c_V_a_l_u_e > │ │ │ │ │ +201 : public _T_e_s_t_a_b_l_e > {}; │ │ │ │ │ +202 │ │ │ │ │ +203// define Value::cast here since now GenericValue has been declared │ │ │ │ │ +204template │ │ │ │ │ +_2_0_5const ValueType& _V_a_l_u_e_:_:_c_a_s_t() const { │ │ │ │ │ +206 return dynamic_cast&>(*this)._v_a_l_u_e(); │ │ │ │ │ +207} │ │ │ │ │ +208 │ │ │ │ │ +211template │ │ │ │ │ +_2_1_2_G_e_n_e_r_i_c_V_a_l_u_e_<_T_> _g_e_n_e_r_i_c_V_a_l_u_e(const T& v) { │ │ │ │ │ +213 return _G_e_n_e_r_i_c_V_a_l_u_e_<_T_>(v); │ │ │ │ │ +214} │ │ │ │ │ +215 │ │ │ │ │ 216 │ │ │ │ │ -223 template │ │ │ │ │ -_2_2_4 VALUE _c_a_l_c_u_l_a_t_e_E_s_t_i_m_a_t_e(_K_e_y key) const { │ │ │ │ │ -225 const Vector& delta = getDelta()[key]; │ │ │ │ │ -226 return _t_r_a_i_t_s_<_V_A_L_U_E_>_:_:_R_e_t_r_a_c_t(theta_._a_t(key), delta); │ │ │ │ │ -227 } │ │ │ │ │ -228 │ │ │ │ │ -237 const _V_a_l_u_e& calculateEstimate(_K_e_y key) const; │ │ │ │ │ -238 │ │ │ │ │ -240 Matrix marginalCovariance(_K_e_y key) const; │ │ │ │ │ -241 │ │ │ │ │ -244 │ │ │ │ │ -248 _V_a_l_u_e_s calculateBestEstimate() const; │ │ │ │ │ -249 │ │ │ │ │ -251 const _V_e_c_t_o_r_V_a_l_u_e_s& getDelta() const; │ │ │ │ │ -252 │ │ │ │ │ -254 double error(const _V_e_c_t_o_r_V_a_l_u_e_s& x) const; │ │ │ │ │ -255 │ │ │ │ │ -_2_5_7 const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& _g_e_t_F_a_c_t_o_r_s_U_n_s_a_f_e() const { │ │ │ │ │ -258 return nonlinearFactors_; │ │ │ │ │ -259 } │ │ │ │ │ -260 │ │ │ │ │ -_2_6_2 const _V_a_r_i_a_b_l_e_I_n_d_e_x& _g_e_t_V_a_r_i_a_b_l_e_I_n_d_e_x() const { return variableIndex_; } │ │ │ │ │ -263 │ │ │ │ │ -_2_6_5 const _K_e_y_S_e_t& _g_e_t_F_i_x_e_d_V_a_r_i_a_b_l_e_s() const { return fixedVariables_; } │ │ │ │ │ -266 │ │ │ │ │ -267 const _I_S_A_M_2_P_a_r_a_m_s& params() const { return params_; } │ │ │ │ │ -268 │ │ │ │ │ -_2_7_0 void _p_r_i_n_t_S_t_a_t_s() const { getCliqueData().getStats().print(); } │ │ │ │ │ -271 │ │ │ │ │ -279 _V_e_c_t_o_r_V_a_l_u_e_s gradientAtZero() const; │ │ │ │ │ -280 │ │ │ │ │ -282 │ │ │ │ │ -283 protected: │ │ │ │ │ -285 void recalculate(const _I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s& updateParams, │ │ │ │ │ -286 const _K_e_y_S_e_t& relinKeys, _I_S_A_M_2_R_e_s_u_l_t* result); │ │ │ │ │ -287 │ │ │ │ │ -288 // Do a batch step - reorder and relinearize all variables │ │ │ │ │ -289 void recalculateBatch(const _I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s& updateParams, │ │ │ │ │ -290 _K_e_y_S_e_t* affectedKeysSet, _I_S_A_M_2_R_e_s_u_l_t* result); │ │ │ │ │ -291 │ │ │ │ │ -292 // retrieve all factors that ONLY contain the affected variables │ │ │ │ │ -293 // (note that the remaining stuff is summarized in the cached factors) │ │ │ │ │ -294 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h relinearizeAffectedFactors( │ │ │ │ │ -295 const _I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s& updateParams, const _F_a_s_t_L_i_s_t_<_K_e_y_>& affectedKeys, │ │ │ │ │ -296 const _K_e_y_S_e_t& relinKeys); │ │ │ │ │ -297 │ │ │ │ │ -309 void recalculateIncremental(const _I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s& updateParams, │ │ │ │ │ -310 const _K_e_y_S_e_t& relinKeys, │ │ │ │ │ -311 const _F_a_s_t_L_i_s_t_<_K_e_y_>& affectedKeys, │ │ │ │ │ -312 _K_e_y_S_e_t* affectedKeysSet, Cliques* orphans, │ │ │ │ │ -313 _I_S_A_M_2_R_e_s_u_l_t* result); │ │ │ │ │ -314 │ │ │ │ │ -320 void addVariables(const _V_a_l_u_e_s& newTheta, │ │ │ │ │ -321 _I_S_A_M_2_R_e_s_u_l_t_:_:_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s* detail = 0); │ │ │ │ │ -322 │ │ │ │ │ -326 void removeVariables(const _K_e_y_S_e_t& unusedKeys); │ │ │ │ │ -327 │ │ │ │ │ -328 void updateDelta(bool forceFullSolve = false) const; │ │ │ │ │ -329 │ │ │ │ │ -330 private: │ │ │ │ │ -_3_3_2 friend class boost::serialization::access; │ │ │ │ │ -333 template │ │ │ │ │ -334 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -335 ar & boost::serialization::base_object >(*this); │ │ │ │ │ -336 ar & BOOST_SERIALIZATION_NVP(theta_); │ │ │ │ │ -337 ar & BOOST_SERIALIZATION_NVP(variableIndex_); │ │ │ │ │ -338 ar & BOOST_SERIALIZATION_NVP(delta_); │ │ │ │ │ -339 ar & BOOST_SERIALIZATION_NVP(deltaNewton_); │ │ │ │ │ -340 ar & BOOST_SERIALIZATION_NVP(RgProd_); │ │ │ │ │ -341 ar & BOOST_SERIALIZATION_NVP(deltaReplacedMask_); │ │ │ │ │ -342 ar & BOOST_SERIALIZATION_NVP(nonlinearFactors_); │ │ │ │ │ -343 ar & BOOST_SERIALIZATION_NVP(linearFactors_); │ │ │ │ │ -344 ar & BOOST_SERIALIZATION_NVP(doglegDelta_); │ │ │ │ │ -345 ar & BOOST_SERIALIZATION_NVP(fixedVariables_); │ │ │ │ │ -346 ar & BOOST_SERIALIZATION_NVP(update_count_); │ │ │ │ │ -347 } │ │ │ │ │ -348 │ │ │ │ │ -349}; // ISAM2 │ │ │ │ │ -350 │ │ │ │ │ -352template <> │ │ │ │ │ -_3_5_3struct _t_r_a_i_t_s<_I_S_A_M_2> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ -354 │ │ │ │ │ -355} // namespace gtsam │ │ │ │ │ -_G_a_u_s_s_i_a_n_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ -Gaussian Bayes Tree, the result of eliminating a GaussianJunctionTree. │ │ │ │ │ -_I_S_A_M_2_P_a_r_a_m_s_._h │ │ │ │ │ -Parameters for iSAM 2. │ │ │ │ │ -_I_S_A_M_2_C_l_i_q_u_e_._h │ │ │ │ │ -Specialized iSAM2 Clique. │ │ │ │ │ -_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s_._h │ │ │ │ │ -Class that stores extra params for ISAM2::update() │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Factor Graph consisting of non-linear factors. │ │ │ │ │ -_I_S_A_M_2_R_e_s_u_l_t_._h │ │ │ │ │ -Class that stores detailed iSAM2 result. │ │ │ │ │ +217} /* namespace gtsam */ │ │ │ │ │ +_V_a_l_u_e_._h │ │ │ │ │ +The base class for any variable that can be optimized or used in a factor. │ │ │ │ │ +_M_a_n_i_f_o_l_d_._h │ │ │ │ │ +Base class and basic functions for Manifold types. │ │ │ │ │ +_t_y_p_e_s_._h │ │ │ │ │ +Typedefs for easier changing of types. │ │ │ │ │ +_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W___I_F │ │ │ │ │ +#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) │ │ │ │ │ +This marks a GTSAM object to require alignment. │ │ │ │ │ +DDeeffiinniittiioonn types.h:317 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_I_n_d_i_c_e_s │ │ │ │ │ -FastVector< FactorIndex > FactorIndices │ │ │ │ │ -Define collection types: │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_d_e_m_a_n_g_l_e │ │ │ │ │ +std::string demangle(const char *name) │ │ │ │ │ +Pretty print Value type name. │ │ │ │ │ +DDeeffiinniittiioonn types.cpp:37 │ │ │ │ │ +_g_t_s_a_m_:_:_g_e_n_e_r_i_c_V_a_l_u_e │ │ │ │ │ +GenericValue< T > genericValue(const T &v) │ │ │ │ │ +Functional constructor of GenericValue so T can be automatically deduced. │ │ │ │ │ +DDeeffiinniittiioonn GenericValue.h:212 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t │ │ │ │ │ -FastList is a thin wrapper around std::list that uses the boost │ │ │ │ │ -fast_pool_allocator instead of the de... │ │ │ │ │ -DDeeffiinniittiioonn FastList.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_M_a_p │ │ │ │ │ -FastMap is a thin wrapper around std::map that uses the boost │ │ │ │ │ -fast_pool_allocator instead of the defa... │ │ │ │ │ -DDeeffiinniittiioonn FastMap.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e │ │ │ │ │ +Wraps any type T so it can play as a Value. │ │ │ │ │ +DDeeffiinniittiioonn GenericValue.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_c_l_o_n_e__ │ │ │ │ │ +Value * clone_() const override │ │ │ │ │ +Create a duplicate object returned as a pointer to the generic Value interface. │ │ │ │ │ +DDeeffiinniittiioonn GenericValue.h:102 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_v_a_l_u_e │ │ │ │ │ +T & value() │ │ │ │ │ +Return the value. │ │ │ │ │ +DDeeffiinniittiioonn GenericValue.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_r_e_t_r_a_c_t__ │ │ │ │ │ +Value * retract_(const Vector &delta) const override │ │ │ │ │ +Generic Value interface version of retract. │ │ │ │ │ +DDeeffiinniittiioonn GenericValue.h:122 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ +Value & operator=(const Value &rhs) override │ │ │ │ │ +Assignment operator. │ │ │ │ │ +DDeeffiinniittiioonn GenericValue.h:158 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &str) const override │ │ │ │ │ +Virtual print function, uses traits. │ │ │ │ │ +DDeeffiinniittiioonn GenericValue.h:94 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const GenericValue &other, double tol=1e-9) const │ │ │ │ │ +non virtual equals function, uses traits │ │ │ │ │ +DDeeffiinniittiioonn GenericValue.h:89 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_d_i_m │ │ │ │ │ +size_t dim() const override │ │ │ │ │ +Return run-time dimensionality. │ │ │ │ │ +DDeeffiinniittiioonn GenericValue.h:153 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_G_e_n_e_r_i_c_V_a_l_u_e │ │ │ │ │ +GenericValue(const T &value) │ │ │ │ │ +Construct from value. │ │ │ │ │ +DDeeffiinniittiioonn GenericValue.h:62 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_d_e_a_l_l_o_c_a_t_e__ │ │ │ │ │ +void deallocate_() const override │ │ │ │ │ +Destroy and deallocate this object, only if it was originally allocated using │ │ │ │ │ +clone_(). │ │ │ │ │ +DDeeffiinniittiioonn GenericValue.h:110 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_v_a_l_u_e__ │ │ │ │ │ +T value_ │ │ │ │ │ +The wrapped value. │ │ │ │ │ +DDeeffiinniittiioonn GenericValue.h:55 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ +GenericValue< T > & operator=(const GenericValue< T > &rhs) │ │ │ │ │ +Assignment operator, protected because only the Value or DERIVED assignment │ │ │ │ │ +operators should be used. │ │ │ │ │ +DDeeffiinniittiioonn GenericValue.h:171 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s │ │ │ │ │ +Vector localCoordinates(const GenericValue &value2) const │ │ │ │ │ +Non-virtual version of localCoordinates. │ │ │ │ │ +DDeeffiinniittiioonn GenericValue.h:148 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn GenericValue.h:180 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_e_q_u_a_l_s__ │ │ │ │ │ +bool equals_(const Value &p, double tol=1e-9) const override │ │ │ │ │ +equals implementing generic Value interface │ │ │ │ │ +DDeeffiinniittiioonn GenericValue.h:81 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_v_a_l_u_e │ │ │ │ │ +const T & value() const │ │ │ │ │ +Return a constant value. │ │ │ │ │ +DDeeffiinniittiioonn GenericValue.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_~_G_e_n_e_r_i_c_V_a_l_u_e │ │ │ │ │ +~GenericValue() override │ │ │ │ │ +Destructor. │ │ │ │ │ +DDeeffiinniittiioonn GenericValue.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_r_e_t_r_a_c_t │ │ │ │ │ +GenericValue retract(const Vector &delta) const │ │ │ │ │ +Non-virtual version of retract. │ │ │ │ │ +DDeeffiinniittiioonn GenericValue.h:143 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_l_o_c_a_l_C_o_o_r_d_i_n_a_t_e_s__ │ │ │ │ │ +Vector localCoordinates_(const Value &value2) const override │ │ │ │ │ +Generic Value interface version of localCoordinates. │ │ │ │ │ +DDeeffiinniittiioonn GenericValue.h:133 │ │ │ │ │ +_g_t_s_a_m_:_:_G_e_n_e_r_i_c_V_a_l_u_e_:_:_c_l_o_n_e │ │ │ │ │ +boost::shared_ptr< Value > clone() const override │ │ │ │ │ +Clone this value (normal clone on the heap, delete with 'delete' operator) │ │ │ │ │ +DDeeffiinniittiioonn GenericValue.h:117 │ │ │ │ │ _g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ A helper that implements the traits interface for GTSAM types. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ _g_t_s_a_m_:_:_V_a_l_u_e │ │ │ │ │ This is the base class for any type to be stored in Values. │ │ │ │ │ DDeeffiinniittiioonn Value.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e │ │ │ │ │ -Bayes tree. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_B_a_y_e_s_T_r_e_e_<_ _I_S_A_M_2_C_l_i_q_u_e_ _>_:_:_s_h_a_r_e_d_C_l_i_q_u_e │ │ │ │ │ -boost::shared_ptr< Clique > sharedClique │ │ │ │ │ -Shared pointer to a clique. │ │ │ │ │ -DDeeffiinniittiioonn BayesTree.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ -The VariableIndex class computes and stores the block column structure of a │ │ │ │ │ -factor graph. │ │ │ │ │ -DDeeffiinniittiioonn VariableIndex.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ -with a unique integer... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2 │ │ │ │ │ -Implementation of the full ISAM2 algorithm for incremental nonlinear │ │ │ │ │ -optimization. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_:_:_f_i_x_e_d_V_a_r_i_a_b_l_e_s__ │ │ │ │ │ -KeySet fixedVariables_ │ │ │ │ │ -Set of variables that are involved with linear factors from marginalized │ │ │ │ │ -variables and thus cannot ha... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2.h:94 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_:_:_d_e_l_t_a_R_e_p_l_a_c_e_d_M_a_s_k__ │ │ │ │ │ -KeySet deltaReplacedMask_ │ │ │ │ │ -A cumulative mask for the variables that were replaced and have not yet been │ │ │ │ │ -updated in the linear so... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2.h:76 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_:_:_u_p_d_a_t_e___c_o_u_n_t__ │ │ │ │ │ -int update_count_ │ │ │ │ │ -Counter incremented every update(), used to determine periodic relinearization. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2.h:96 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_:_:_c_a_l_c_u_l_a_t_e_E_s_t_i_m_a_t_e │ │ │ │ │ -VALUE calculateEstimate(Key key) const │ │ │ │ │ -Compute an estimate for a single variable using its incomplete linear delta │ │ │ │ │ -computed during the last ... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2.h:224 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_:_:_~_I_S_A_M_2 │ │ │ │ │ -virtual ~ISAM2() │ │ │ │ │ -default virtual destructor │ │ │ │ │ -DDeeffiinniittiioonn ISAM2.h:114 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_:_:_g_e_t_F_i_x_e_d_V_a_r_i_a_b_l_e_s │ │ │ │ │ -const KeySet & getFixedVariables() const │ │ │ │ │ -Access the nonlinear variable index. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2.h:265 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_:_:_C_l_i_q_u_e_s │ │ │ │ │ -Base::Cliques Cliques │ │ │ │ │ -List of Cliques. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2.h:104 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_:_:_p_r_i_n_t_S_t_a_t_s │ │ │ │ │ -void printStats() const │ │ │ │ │ -prints out clique statistics │ │ │ │ │ -DDeeffiinniittiioonn ISAM2.h:270 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_:_:_n_o_n_l_i_n_e_a_r_F_a_c_t_o_r_s__ │ │ │ │ │ -NonlinearFactorGraph nonlinearFactors_ │ │ │ │ │ -All original nonlinear factors are stored here to use during relinearization. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2.h:81 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_:_:_g_e_t_V_a_r_i_a_b_l_e_I_n_d_e_x │ │ │ │ │ -const VariableIndex & getVariableIndex() const │ │ │ │ │ -Access the nonlinear variable index. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2.h:262 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_:_:_s_h_a_r_e_d_C_l_i_q_u_e │ │ │ │ │ -Base::sharedClique sharedClique │ │ │ │ │ -Shared pointer to a clique. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2.h:103 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_:_:_d_e_l_t_a__ │ │ │ │ │ -VectorValues delta_ │ │ │ │ │ -The linear delta from the last linear solution, an update to the estimate in │ │ │ │ │ -theta. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2.h:61 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_:_:_v_a_r_i_a_b_l_e_I_n_d_e_x__ │ │ │ │ │ -VariableIndex variableIndex_ │ │ │ │ │ -VariableIndex lets us look up factors by involved variable and keeps track of │ │ │ │ │ -dimensions. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_:_:_g_e_t_L_i_n_e_a_r_i_z_a_t_i_o_n_P_o_i_n_t │ │ │ │ │ -const Values & getLinearizationPoint() const │ │ │ │ │ -Access the current linearization point. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2.h:205 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_:_:_t_h_e_t_a__ │ │ │ │ │ -Values theta_ │ │ │ │ │ -The current linearization point. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_:_:_p_a_r_a_m_s__ │ │ │ │ │ -ISAM2Params params_ │ │ │ │ │ -The current parameters. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2.h:87 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_:_:_d_o_g_l_e_g_D_e_l_t_a__ │ │ │ │ │ -boost::optional< double > doglegDelta_ │ │ │ │ │ -The current Dogleg Delta (trust region radius) │ │ │ │ │ -DDeeffiinniittiioonn ISAM2.h:90 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_:_:_l_i_n_e_a_r_F_a_c_t_o_r_s__ │ │ │ │ │ -GaussianFactorGraph linearFactors_ │ │ │ │ │ -The current linear factors, which are only updated as needed. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2.h:84 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_:_:_g_e_t_F_a_c_t_o_r_s_U_n_s_a_f_e │ │ │ │ │ -const NonlinearFactorGraph & getFactorsUnsafe() const │ │ │ │ │ -Access the set of nonlinear factors. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2.h:257 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_:_:_v_a_l_u_e_E_x_i_s_t_s │ │ │ │ │ -bool valueExists(Key key) const │ │ │ │ │ -Check whether variable with given key exists in linearization point. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2.h:208 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_C_l_i_q_u_e │ │ │ │ │ -Specialized Clique structure for ISAM2, incorporating caching and gradient │ │ │ │ │ -contribution TODO: more do... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Clique.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_P_a_r_a_m_s │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Params.h:135 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t │ │ │ │ │ -This struct is returned from ISAM2::update() and contains information about the │ │ │ │ │ -update that is useful... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Result.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_R_e_s_u_l_t_:_:_D_e_t_a_i_l_e_d_R_e_s_u_l_t_s │ │ │ │ │ -A struct holding detailed results, which must be enabled with ISAM2Params:: │ │ │ │ │ -enableDetailedResults. │ │ │ │ │ -DDeeffiinniittiioonn ISAM2Result.h:117 │ │ │ │ │ -_g_t_s_a_m_:_:_I_S_A_M_2_U_p_d_a_t_e_P_a_r_a_m_s │ │ │ │ │ -This struct is used by ISAM2::update() to pass additional parameters to give │ │ │ │ │ -the user a fine-grained ... │ │ │ │ │ -DDeeffiinniittiioonn ISAM2UpdateParams.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_a_t │ │ │ │ │ -const ValueType at(Key j) const │ │ │ │ │ -Retrieve a variable by key j. │ │ │ │ │ -DDeeffiinniittiioonn Values-inl.h:361 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_e_x_i_s_t_s │ │ │ │ │ -bool exists(Key j) const │ │ │ │ │ -Check if a value exists with key j. │ │ │ │ │ -DDeeffiinniittiioonn Values.cpp:94 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_:_:_c_a_s_t │ │ │ │ │ +const ValueType & cast() const │ │ │ │ │ +Cast to known ValueType. │ │ │ │ │ +DDeeffiinniittiioonn GenericValue.h:205 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_:_:_o_p_e_r_a_t_o_r_= │ │ │ │ │ +virtual Value & operator=(const Value &) │ │ │ │ │ +Assignment operator. │ │ │ │ │ +DDeeffiinniittiioonn Value.h:79 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _I_S_A_M_2_._h │ │ │ │ │ + * _b_a_s_e │ │ │ │ │ + * GGeenneerriiccVVaalluuee..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01157_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01388_source.html │ │ │ │┄ Files 80% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/FunctorizedFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
FunctorizedFactor.h
│ │ │ │ +
SymbolicFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
18#pragma once
│ │ │ │
19
│ │ │ │ -
20#include <gtsam/base/Testable.h>
│ │ │ │ -
21#include <gtsam/nonlinear/NonlinearFactor.h>
│ │ │ │ -
22
│ │ │ │ -
23#include <cmath>
│ │ │ │ -
24
│ │ │ │ -
25namespace gtsam {
│ │ │ │ +
20#include <gtsam/inference/Factor.h>
│ │ │ │ +
21#include <gtsam/inference/Key.h>
│ │ │ │ +
22#include <gtsam/base/Testable.h>
│ │ │ │ +
23
│ │ │ │ +
24#include <boost/shared_ptr.hpp>
│ │ │ │ +
25#include <boost/make_shared.hpp>
│ │ │ │
26
│ │ │ │ -
58template <typename R, typename T>
│ │ │ │ -
│ │ │ │ -
59class FunctorizedFactor : public NoiseModelFactorN<T> {
│ │ │ │ -
60 private:
│ │ │ │ -
61 using Base = NoiseModelFactorN<T>;
│ │ │ │ -
62
│ │ │ │ -
63 R measured_;
│ │ │ │ -
64 SharedNoiseModel noiseModel_;
│ │ │ │ -
65 std::function<R(T, boost::optional<Matrix &>)> func_;
│ │ │ │ -
66
│ │ │ │ -
67 public:
│ │ │ │ -
69 FunctorizedFactor() {}
│ │ │ │ -
70
│ │ │ │ -
│ │ │ │ -
78 FunctorizedFactor(Key key, const R &z, const SharedNoiseModel &model,
│ │ │ │ -
79 const std::function<R(T, boost::optional<Matrix &>)> func)
│ │ │ │ -
80 : Base(model, key), measured_(z), noiseModel_(model), func_(func) {}
│ │ │ │ -
│ │ │ │ -
81
│ │ │ │ -
82 ~FunctorizedFactor() override {}
│ │ │ │ -
83
│ │ │ │ -
│ │ │ │ -
85 NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
86 return boost::static_pointer_cast<NonlinearFactor>(
│ │ │ │ -
87 NonlinearFactor::shared_ptr(new FunctorizedFactor<R, T>(*this)));
│ │ │ │ -
88 }
│ │ │ │ -
│ │ │ │ +
27#include <utility>
│ │ │ │ +
28
│ │ │ │ +
29namespace gtsam {
│ │ │ │ +
30
│ │ │ │ +
31 // Forward declarations
│ │ │ │ +
32 class SymbolicConditional;
│ │ │ │ +
33 class HybridValues;
│ │ │ │ +
34 class Ordering;
│ │ │ │ +
35
│ │ │ │ +
│ │ │ │ +
39 class GTSAM_EXPORT SymbolicFactor : public Factor {
│ │ │ │ +
40
│ │ │ │ +
41 public:
│ │ │ │ +
42
│ │ │ │ +
43 typedef SymbolicFactor This;
│ │ │ │ +
44 typedef Factor Base;
│ │ │ │ +
45 typedef SymbolicConditional ConditionalType;
│ │ │ │ +
46
│ │ │ │ +
48 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
49
│ │ │ │ +
52
│ │ │ │ +
54 SymbolicFactor() {}
│ │ │ │ +
55
│ │ │ │ +
│ │ │ │ +
57 explicit SymbolicFactor(Key j) :
│ │ │ │ +
58 Base(KeyVector{j}) {}
│ │ │ │ +
│ │ │ │ +
59
│ │ │ │ +
│ │ │ │ +
61 SymbolicFactor(Key j1, Key j2) :
│ │ │ │ +
62 Base(KeyVector{j1, j2}) {}
│ │ │ │ +
│ │ │ │ +
63
│ │ │ │ +
│ │ │ │ +
65 SymbolicFactor(Key j1, Key j2, Key j3) :
│ │ │ │ +
66 Base(KeyVector{j1, j2, j3}) {}
│ │ │ │ +
│ │ │ │ +
67
│ │ │ │ +
│ │ │ │ +
69 SymbolicFactor(Key j1, Key j2, Key j3, Key j4) :
│ │ │ │ +
70 Base(KeyVector{j1, j2, j3, j4}) {}
│ │ │ │ +
│ │ │ │ +
71
│ │ │ │ +
│ │ │ │ +
73 SymbolicFactor(Key j1, Key j2, Key j3, Key j4, Key j5) :
│ │ │ │ +
74 Base(KeyVector{j1, j2, j3, j4, j5}) {}
│ │ │ │ +
│ │ │ │ +
75
│ │ │ │ +
│ │ │ │ +
77 SymbolicFactor(Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) :
│ │ │ │ +
78 Base(KeyVector{j1, j2, j3, j4, j5, j6}) {}
│ │ │ │ +
│ │ │ │ +
79
│ │ │ │ +
81 explicit SymbolicFactor(const Factor& factor) : Base(factor.keys()) {}
│ │ │ │ +
82
│ │ │ │ +
83 virtual ~SymbolicFactor() {}
│ │ │ │ +
84
│ │ │ │ +
86 SymbolicFactor::shared_ptr clone() const { return boost::make_shared<This>(*this); }
│ │ │ │ +
87
│ │ │ │
89
│ │ │ │ -
90 Vector evaluateError(const T &params, boost::optional<Matrix &> H =
│ │ │ │ -
91 boost::none) const override {
│ │ │ │ -
92 R x = func_(params, H);
│ │ │ │ -
93 Vector error = traits<R>::Local(measured_, x);
│ │ │ │ -
94 return error;
│ │ │ │ -
95 }
│ │ │ │ -
96
│ │ │ │ -
│ │ │ │ -
99 void print(
│ │ │ │ -
100 const std::string &s = "",
│ │ │ │ -
101 const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ -
102 Base::print(s, keyFormatter);
│ │ │ │ -
103 std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor("
│ │ │ │ -
104 << keyFormatter(this->key1()) << ")" << std::endl;
│ │ │ │ -
105 traits<R>::Print(measured_, " measurement: ");
│ │ │ │ -
106 std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose()
│ │ │ │ -
107 << std::endl;
│ │ │ │ -
108 }
│ │ │ │ -
│ │ │ │ -
109
│ │ │ │ -
│ │ │ │ -
110 bool equals(const NonlinearFactor &other, double tol = 1e-9) const override {
│ │ │ │ -
111 const FunctorizedFactor<R, T> *e =
│ │ │ │ -
112 dynamic_cast<const FunctorizedFactor<R, T> *>(&other);
│ │ │ │ -
113 return e != nullptr && Base::equals(other, tol) &&
│ │ │ │ -
114 traits<R>::Equals(this->measured_, e->measured_, tol);
│ │ │ │ -
115 }
│ │ │ │ -
│ │ │ │ -
117
│ │ │ │ -
118 private:
│ │ │ │ -
120 friend class boost::serialization::access;
│ │ │ │ -
121 template <class ARCHIVE>
│ │ │ │ -
122 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ -
123 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ -
124 ar &boost::serialization::make_nvp(
│ │ │ │ -
125 "NoiseModelFactor1", boost::serialization::base_object<Base>(*this));
│ │ │ │ -
126 ar &BOOST_SERIALIZATION_NVP(measured_);
│ │ │ │ -
127 ar &BOOST_SERIALIZATION_NVP(func_);
│ │ │ │ -
128 }
│ │ │ │ -
129};
│ │ │ │ -
│ │ │ │ -
130
│ │ │ │ -
132template <typename R, typename T>
│ │ │ │ -
│ │ │ │ -
133struct traits<FunctorizedFactor<R, T>>
│ │ │ │ -
134 : public Testable<FunctorizedFactor<R, T>> {};
│ │ │ │ -
│ │ │ │ -
135
│ │ │ │ -
142template <typename T, typename R, typename FUNC>
│ │ │ │ -
│ │ │ │ -
143FunctorizedFactor<R, T> MakeFunctorizedFactor(Key key, const R &z,
│ │ │ │ -
144 const SharedNoiseModel &model,
│ │ │ │ -
145 const FUNC func) {
│ │ │ │ -
146 return FunctorizedFactor<R, T>(key, z, model, func);
│ │ │ │ -
147}
│ │ │ │ -
│ │ │ │ +
92
│ │ │ │ +
93 bool equals(const This& other, double tol = 1e-9) const;
│ │ │ │ +
94
│ │ │ │ +
│ │ │ │ +
96 void print(
│ │ │ │ +
97 const std::string& s = "SymbolicFactor",
│ │ │ │ +
98 const KeyFormatter& formatter = DefaultKeyFormatter) const override {
│ │ │ │ +
99 Base::print(s, formatter);
│ │ │ │ +
100 }
│ │ │ │ +
│ │ │ │ +
101
│ │ │ │ +
│ │ │ │ +
103 void printKeys(
│ │ │ │ +
104 const std::string& s = "SymbolicFactor",
│ │ │ │ +
105 const KeyFormatter& formatter = DefaultKeyFormatter) const override {
│ │ │ │ +
106 Base::printKeys(s, formatter);
│ │ │ │ +
107 }
│ │ │ │ +
│ │ │ │ +
108
│ │ │ │ +
112
│ │ │ │ +
114 template<typename KEYITERATOR>
│ │ │ │ +
│ │ │ │ +
115 static SymbolicFactor FromIterators(KEYITERATOR beginKey, KEYITERATOR endKey) {
│ │ │ │ +
116 return SymbolicFactor(Base::FromIterators(beginKey, endKey));
│ │ │ │ +
117 }
│ │ │ │ +
│ │ │ │ +
118
│ │ │ │ +
120 template<typename KEYITERATOR>
│ │ │ │ +
│ │ │ │ +
121 static SymbolicFactor::shared_ptr FromIteratorsShared(KEYITERATOR beginKey, KEYITERATOR endKey) {
│ │ │ │ +
122 SymbolicFactor::shared_ptr result = boost::make_shared<SymbolicFactor>();
│ │ │ │ +
123 result->keys_.assign(beginKey, endKey);
│ │ │ │ +
124 return result;
│ │ │ │ +
125 }
│ │ │ │ +
│ │ │ │ +
126
│ │ │ │ +
129 template<class CONTAINER>
│ │ │ │ +
│ │ │ │ +
130 static SymbolicFactor FromKeys(const CONTAINER& keys) {
│ │ │ │ +
131 return SymbolicFactor(Base::FromKeys(keys));
│ │ │ │ +
132 }
│ │ │ │ +
│ │ │ │ +
133
│ │ │ │ +
136 template<class CONTAINER>
│ │ │ │ +
│ │ │ │ +
137 static SymbolicFactor::shared_ptr FromKeysShared(const CONTAINER& keys) {
│ │ │ │ +
138 return FromIteratorsShared(keys.begin(), keys.end());
│ │ │ │ +
139 }
│ │ │ │ +
│ │ │ │ +
140
│ │ │ │ +
142
│ │ │ │ +
145
│ │ │ │ +
147 double error(const HybridValues& c) const override;
│ │ │ │
148
│ │ │ │ -
158template <typename R, typename T1, typename T2>
│ │ │ │ -
│ │ │ │ -
159class FunctorizedFactor2 : public NoiseModelFactorN<T1, T2> {
│ │ │ │ -
160 private:
│ │ │ │ -
161 using Base = NoiseModelFactorN<T1, T2>;
│ │ │ │ -
162
│ │ │ │ -
163 R measured_;
│ │ │ │ -
164 SharedNoiseModel noiseModel_;
│ │ │ │ -
165 using FunctionType = std::function<R(T1, T2, boost::optional<Matrix &>,
│ │ │ │ -
166 boost::optional<Matrix &>)>;
│ │ │ │ -
167 FunctionType func_;
│ │ │ │ +
151 std::pair<boost::shared_ptr<SymbolicConditional>, boost::shared_ptr<SymbolicFactor> >
│ │ │ │ +
152 eliminate(const Ordering& keys) const;
│ │ │ │ +
153
│ │ │ │ +
155
│ │ │ │ +
156 private:
│ │ │ │ +
158 friend class boost::serialization::access;
│ │ │ │ +
159 template<class ARCHIVE>
│ │ │ │ +
160 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
161 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
162 }
│ │ │ │ +
163 }; // IndexFactor
│ │ │ │ +
│ │ │ │ +
164
│ │ │ │ +
165 // Forward declarations
│ │ │ │ +
166 class SymbolicFactorGraph;
│ │ │ │ +
167 class Ordering;
│ │ │ │
168
│ │ │ │ -
169 public:
│ │ │ │ -
171 FunctorizedFactor2() {}
│ │ │ │ -
172
│ │ │ │ -
│ │ │ │ -
180 FunctorizedFactor2(Key key1, Key key2, const R &z,
│ │ │ │ -
181 const SharedNoiseModel &model, const FunctionType func)
│ │ │ │ -
182 : Base(model, key1, key2),
│ │ │ │ -
183 measured_(z),
│ │ │ │ -
184 noiseModel_(model),
│ │ │ │ -
185 func_(func) {}
│ │ │ │ -
│ │ │ │ -
186
│ │ │ │ -
187 ~FunctorizedFactor2() override {}
│ │ │ │ -
188
│ │ │ │ -
│ │ │ │ -
190 NonlinearFactor::shared_ptr clone() const override {
│ │ │ │ -
191 return boost::static_pointer_cast<NonlinearFactor>(
│ │ │ │ -
192 NonlinearFactor::shared_ptr(new FunctorizedFactor2<R, T1, T2>(*this)));
│ │ │ │ -
193 }
│ │ │ │ -
│ │ │ │ -
194
│ │ │ │ -
195 Vector evaluateError(
│ │ │ │ -
196 const T1 &params1, const T2 &params2,
│ │ │ │ -
197 boost::optional<Matrix &> H1 = boost::none,
│ │ │ │ -
198 boost::optional<Matrix &> H2 = boost::none) const override {
│ │ │ │ -
199 R x = func_(params1, params2, H1, H2);
│ │ │ │ -
200 Vector error = traits<R>::Local(measured_, x);
│ │ │ │ -
201 return error;
│ │ │ │ -
202 }
│ │ │ │ -
203
│ │ │ │ -
│ │ │ │ -
206 void print(
│ │ │ │ -
207 const std::string &s = "",
│ │ │ │ -
208 const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
│ │ │ │ -
209 Base::print(s, keyFormatter);
│ │ │ │ -
210 std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor2("
│ │ │ │ -
211 << keyFormatter(this->key1()) << ", "
│ │ │ │ -
212 << keyFormatter(this->key2()) << ")" << std::endl;
│ │ │ │ -
213 traits<R>::Print(measured_, " measurement: ");
│ │ │ │ -
214 std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose()
│ │ │ │ -
215 << std::endl;
│ │ │ │ -
216 }
│ │ │ │ -
│ │ │ │ -
217
│ │ │ │ -
│ │ │ │ -
218 bool equals(const NonlinearFactor &other, double tol = 1e-9) const override {
│ │ │ │ -
219 const FunctorizedFactor2<R, T1, T2> *e =
│ │ │ │ -
220 dynamic_cast<const FunctorizedFactor2<R, T1, T2> *>(&other);
│ │ │ │ -
221 return e && Base::equals(other, tol) &&
│ │ │ │ -
222 traits<R>::Equals(this->measured_, e->measured_, tol);
│ │ │ │ -
223 }
│ │ │ │ -
│ │ │ │ -
225
│ │ │ │ -
226 private:
│ │ │ │ -
228 friend class boost::serialization::access;
│ │ │ │ -
229 template <class ARCHIVE>
│ │ │ │ -
230 void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
│ │ │ │ -
231 // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
│ │ │ │ -
232 ar &boost::serialization::make_nvp(
│ │ │ │ -
233 "NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
│ │ │ │ -
234 ar &BOOST_SERIALIZATION_NVP(measured_);
│ │ │ │ -
235 ar &BOOST_SERIALIZATION_NVP(func_);
│ │ │ │ -
236 }
│ │ │ │ -
237};
│ │ │ │ -
│ │ │ │ -
238
│ │ │ │ -
240template <typename R, typename T1, typename T2>
│ │ │ │ -
│ │ │ │ -
241struct traits<FunctorizedFactor2<R, T1, T2>>
│ │ │ │ -
242 : public Testable<FunctorizedFactor2<R, T1, T2>> {};
│ │ │ │ -
│ │ │ │ -
243
│ │ │ │ -
250template <typename T1, typename T2, typename R, typename FUNC>
│ │ │ │ -
│ │ │ │ -
251FunctorizedFactor2<R, T1, T2> MakeFunctorizedFactor2(
│ │ │ │ -
252 Key key1, Key key2, const R &z, const SharedNoiseModel &model,
│ │ │ │ -
253 const FUNC func) {
│ │ │ │ -
254 return FunctorizedFactor2<R, T1, T2>(key1, key2, z, model, func);
│ │ │ │ -
255}
│ │ │ │ -
│ │ │ │ -
256
│ │ │ │ -
257} // namespace gtsam
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ -
NonlinearFactor.h
Non-linear factor base classes.
│ │ │ │ +
173 GTSAM_EXPORT std::pair<boost::shared_ptr<SymbolicConditional>, boost::shared_ptr<SymbolicFactor> >
│ │ │ │ +
174 EliminateSymbolic(const SymbolicFactorGraph& factors, const Ordering& keys);
│ │ │ │ +
175
│ │ │ │ +
177 template<>
│ │ │ │ +
│ │ │ │ +
178 struct traits<SymbolicFactor> : public Testable<SymbolicFactor> {
│ │ │ │ +
179 };
│ │ │ │ +
│ │ │ │ +
180
│ │ │ │ +
181} //\ namespace gtsam
│ │ │ │ +
182
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
Factor.h
The base class for all factors.
│ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::MakeFunctorizedFactor2
FunctorizedFactor2< R, T1, T2 > MakeFunctorizedFactor2(Key key1, Key key2, const R &z, const SharedNoiseModel &model, const FUNC func)
Helper function to create a functorized factor.
Definition FunctorizedFactor.h:251
│ │ │ │ -
gtsam::MakeFunctorizedFactor
FunctorizedFactor< R, T > MakeFunctorizedFactor(Key key, const R &z, const SharedNoiseModel &model, const FUNC func)
Helper function to create a functorized factor.
Definition FunctorizedFactor.h:143
│ │ │ │ -
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::EliminateSymbolic
std::pair< boost::shared_ptr< SymbolicConditional >, boost::shared_ptr< SymbolicFactor > > EliminateSymbolic(const SymbolicFactorGraph &factors, const Ordering &keys)
Dense elimination function for symbolic factors.
Definition SymbolicFactor.cpp:36
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
│ │ │ │ -
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
│ │ │ │ -
gtsam::FunctorizedFactor
Factor which evaluates provided unary functor and uses the result to compute error with respect to th...
Definition FunctorizedFactor.h:59
│ │ │ │ -
gtsam::FunctorizedFactor::FunctorizedFactor
FunctorizedFactor(Key key, const R &z, const SharedNoiseModel &model, const std::function< R(T, boost::optional< Matrix & >)> func)
Construct with given x and the parameters of the basis.
Definition FunctorizedFactor.h:78
│ │ │ │ -
gtsam::FunctorizedFactor::equals
bool equals(const NonlinearFactor &other, double tol=1e-9) const override
Check if two factors are equal.
Definition FunctorizedFactor.h:110
│ │ │ │ -
gtsam::FunctorizedFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition FunctorizedFactor.h:99
│ │ │ │ -
gtsam::FunctorizedFactor::FunctorizedFactor
FunctorizedFactor()
default constructor - only use for serialization
Definition FunctorizedFactor.h:69
│ │ │ │ -
gtsam::FunctorizedFactor::access
friend class boost::serialization::access
Serialization function.
Definition FunctorizedFactor.h:120
│ │ │ │ -
gtsam::FunctorizedFactor::clone
NonlinearFactor::shared_ptr clone() const override
Definition FunctorizedFactor.h:85
│ │ │ │ -
gtsam::FunctorizedFactor2
Factor which evaluates provided binary functor and uses the result to compute error with respect to t...
Definition FunctorizedFactor.h:159
│ │ │ │ -
gtsam::FunctorizedFactor2::FunctorizedFactor2
FunctorizedFactor2()
default constructor - only use for serialization
Definition FunctorizedFactor.h:171
│ │ │ │ -
gtsam::FunctorizedFactor2::clone
NonlinearFactor::shared_ptr clone() const override
Definition FunctorizedFactor.h:190
│ │ │ │ -
gtsam::FunctorizedFactor2::FunctorizedFactor2
FunctorizedFactor2(Key key1, Key key2, const R &z, const SharedNoiseModel &model, const FunctionType func)
Construct with given x and the parameters of the basis.
Definition FunctorizedFactor.h:180
│ │ │ │ -
gtsam::FunctorizedFactor2::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition FunctorizedFactor.h:206
│ │ │ │ -
gtsam::FunctorizedFactor2::equals
bool equals(const NonlinearFactor &other, double tol=1e-9) const override
Check if two factors are equal.
Definition FunctorizedFactor.h:218
│ │ │ │ -
gtsam::FunctorizedFactor2::access
friend class boost::serialization::access
Serialization function.
Definition FunctorizedFactor.h:228
│ │ │ │ -
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ -
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Calculate the error of the factor.
Definition NonlinearFactor.cpp:138
│ │ │ │ -
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
│ │ │ │ -
gtsam::NoiseModelFactorN< T >::key
Key key() const
Returns a key.
Definition NonlinearFactor.h:518
│ │ │ │ +
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ +
gtsam::Factor
Definition Factor.h:68
│ │ │ │ +
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ +
gtsam::SymbolicConditional
SymbolicConditional is a conditional with keys but no probability data, produced by symbolic eliminat...
Definition SymbolicConditional.h:38
│ │ │ │ +
gtsam::SymbolicFactor
SymbolicFactor represents a symbolic factor that specifies graph topology but is not associated with ...
Definition SymbolicFactor.h:39
│ │ │ │ +
gtsam::SymbolicFactor::print
void print(const std::string &s="SymbolicFactor", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print
Definition SymbolicFactor.h:96
│ │ │ │ +
gtsam::SymbolicFactor::SymbolicFactor
SymbolicFactor(Key j1, Key j2, Key j3, Key j4, Key j5, Key j6)
Construct 6-way factor.
Definition SymbolicFactor.h:77
│ │ │ │ +
gtsam::SymbolicFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
Overriding the shared_ptr typedef.
Definition SymbolicFactor.h:48
│ │ │ │ +
gtsam::SymbolicFactor::FromKeys
static SymbolicFactor FromKeys(const CONTAINER &keys)
Constructor from a collection of keys - compatible with boost assign::list_of and boost assign::cref_...
Definition SymbolicFactor.h:130
│ │ │ │ +
gtsam::SymbolicFactor::SymbolicFactor
SymbolicFactor(const Factor &factor)
Create symbolic version of any factor.
Definition SymbolicFactor.h:81
│ │ │ │ +
gtsam::SymbolicFactor::SymbolicFactor
SymbolicFactor(Key j1, Key j2, Key j3, Key j4)
Construct 4-way factor.
Definition SymbolicFactor.h:69
│ │ │ │ +
gtsam::SymbolicFactor::SymbolicFactor
SymbolicFactor(Key j1, Key j2)
Construct binary factor.
Definition SymbolicFactor.h:61
│ │ │ │ +
gtsam::SymbolicFactor::FromIterators
static SymbolicFactor FromIterators(KEYITERATOR beginKey, KEYITERATOR endKey)
Constructor from a collection of keys.
Definition SymbolicFactor.h:115
│ │ │ │ +
gtsam::SymbolicFactor::printKeys
void printKeys(const std::string &s="SymbolicFactor", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print only keys
Definition SymbolicFactor.h:103
│ │ │ │ +
gtsam::SymbolicFactor::FromKeysShared
static SymbolicFactor::shared_ptr FromKeysShared(const CONTAINER &keys)
Constructor from a collection of keys - compatible with boost assign::list_of and boost assign::cref_...
Definition SymbolicFactor.h:137
│ │ │ │ +
gtsam::SymbolicFactor::SymbolicFactor
SymbolicFactor(Key j1, Key j2, Key j3)
Construct ternary factor.
Definition SymbolicFactor.h:65
│ │ │ │ +
gtsam::SymbolicFactor::SymbolicFactor
SymbolicFactor(Key j)
Construct unary factor.
Definition SymbolicFactor.h:57
│ │ │ │ +
gtsam::SymbolicFactor::SymbolicFactor
SymbolicFactor(Key j1, Key j2, Key j3, Key j4, Key j5)
Construct 5-way factor.
Definition SymbolicFactor.h:73
│ │ │ │ +
gtsam::SymbolicFactor::FromIteratorsShared
static SymbolicFactor::shared_ptr FromIteratorsShared(KEYITERATOR beginKey, KEYITERATOR endKey)
Constructor from a collection of keys.
Definition SymbolicFactor.h:121
│ │ │ │ +
gtsam::SymbolicFactor::SymbolicFactor
SymbolicFactor()
Default constructor for I/O.
Definition SymbolicFactor.h:54
│ │ │ │ +
gtsam::SymbolicFactor::clone
SymbolicFactor::shared_ptr clone() const
Copy this object as its actual derived type.
Definition SymbolicFactor.h:86
│ │ │ │ +
HybridValues
the error.
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -FunctorizedFactor.h │ │ │ │ │ +SymbolicFactor.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ @@ -16,291 +16,260 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 18#pragma once │ │ │ │ │ 19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h> │ │ │ │ │ -22 │ │ │ │ │ -23#include │ │ │ │ │ -24 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ +20#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +23 │ │ │ │ │ +24#include │ │ │ │ │ +25#include │ │ │ │ │ 26 │ │ │ │ │ -58template │ │ │ │ │ -_5_9class _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -60 private: │ │ │ │ │ -61 using _B_a_s_e = _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_T_>; │ │ │ │ │ -62 │ │ │ │ │ -63 R measured_; │ │ │ │ │ -64 _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l noiseModel_; │ │ │ │ │ -65 std::function)> func_; │ │ │ │ │ -66 │ │ │ │ │ -67 public: │ │ │ │ │ -_6_9 _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r() {} │ │ │ │ │ -70 │ │ │ │ │ -_7_8 _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r(_K_e_y _k_e_y, const R &z, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, │ │ │ │ │ -79 const std::function)> func) │ │ │ │ │ -80 : _B_a_s_e(model, _k_e_y), measured_(z), noiseModel_(model), func_(func) {} │ │ │ │ │ -81 │ │ │ │ │ -82 _~_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r() override {} │ │ │ │ │ -83 │ │ │ │ │ -_8_5 NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -86 return boost::static_pointer_cast( │ │ │ │ │ -87 NonlinearFactor::shared_ptr(new _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_<_R_,_ _T_>(*this))); │ │ │ │ │ -88 } │ │ │ │ │ +27#include │ │ │ │ │ +28 │ │ │ │ │ +29namespace _g_t_s_a_m { │ │ │ │ │ +30 │ │ │ │ │ +31 // Forward declarations │ │ │ │ │ +32 class SymbolicConditional; │ │ │ │ │ +33 class _H_y_b_r_i_d_V_a_l_u_e_s; │ │ │ │ │ +34 class Ordering; │ │ │ │ │ +35 │ │ │ │ │ +_3_9 class GTSAM_EXPORT _S_y_m_b_o_l_i_c_F_a_c_t_o_r : public _F_a_c_t_o_r { │ │ │ │ │ +40 │ │ │ │ │ +41 public: │ │ │ │ │ +42 │ │ │ │ │ +43 typedef _S_y_m_b_o_l_i_c_F_a_c_t_o_r _T_h_i_s; │ │ │ │ │ +44 typedef _F_a_c_t_o_r _B_a_s_e; │ │ │ │ │ +45 typedef _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l _C_o_n_d_i_t_i_o_n_a_l_T_y_p_e; │ │ │ │ │ +46 │ │ │ │ │ +_4_8 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +49 │ │ │ │ │ +52 │ │ │ │ │ +_5_4 _S_y_m_b_o_l_i_c_F_a_c_t_o_r() {} │ │ │ │ │ +55 │ │ │ │ │ +_5_7 explicit _S_y_m_b_o_l_i_c_F_a_c_t_o_r(_K_e_y j) : │ │ │ │ │ +58 _B_a_s_e(_K_e_y_V_e_c_t_o_r{j}) {} │ │ │ │ │ +59 │ │ │ │ │ +_6_1 _S_y_m_b_o_l_i_c_F_a_c_t_o_r(_K_e_y j1, _K_e_y j2) : │ │ │ │ │ +62 _B_a_s_e(_K_e_y_V_e_c_t_o_r{j1, j2}) {} │ │ │ │ │ +63 │ │ │ │ │ +_6_5 _S_y_m_b_o_l_i_c_F_a_c_t_o_r(_K_e_y j1, _K_e_y j2, _K_e_y j3) : │ │ │ │ │ +66 _B_a_s_e(_K_e_y_V_e_c_t_o_r{j1, j2, j3}) {} │ │ │ │ │ +67 │ │ │ │ │ +_6_9 _S_y_m_b_o_l_i_c_F_a_c_t_o_r(_K_e_y j1, _K_e_y j2, _K_e_y j3, _K_e_y j4) : │ │ │ │ │ +70 _B_a_s_e(_K_e_y_V_e_c_t_o_r{j1, j2, j3, j4}) {} │ │ │ │ │ +71 │ │ │ │ │ +_7_3 _S_y_m_b_o_l_i_c_F_a_c_t_o_r(_K_e_y j1, _K_e_y j2, _K_e_y j3, _K_e_y j4, _K_e_y j5) : │ │ │ │ │ +74 _B_a_s_e(_K_e_y_V_e_c_t_o_r{j1, j2, j3, j4, j5}) {} │ │ │ │ │ +75 │ │ │ │ │ +_7_7 _S_y_m_b_o_l_i_c_F_a_c_t_o_r(_K_e_y j1, _K_e_y j2, _K_e_y j3, _K_e_y j4, _K_e_y j5, _K_e_y j6) : │ │ │ │ │ +78 _B_a_s_e(_K_e_y_V_e_c_t_o_r{j1, j2, j3, j4, j5, j6}) {} │ │ │ │ │ +79 │ │ │ │ │ +_8_1 explicit _S_y_m_b_o_l_i_c_F_a_c_t_o_r(const _F_a_c_t_o_r& factor) : _B_a_s_e(factor.keys()) {} │ │ │ │ │ +82 │ │ │ │ │ +83 virtual _~_S_y_m_b_o_l_i_c_F_a_c_t_o_r() {} │ │ │ │ │ +84 │ │ │ │ │ +_8_6 _S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r _c_l_o_n_e() const { return boost::make_shared │ │ │ │ │ +(*this); } │ │ │ │ │ +87 │ │ │ │ │ 89 │ │ │ │ │ -90 Vector evaluateError(const T ¶ms, boost::optional H = │ │ │ │ │ -91 boost::none) const override { │ │ │ │ │ -92 R x = func_(params, H); │ │ │ │ │ -93 Vector _e_r_r_o_r = _t_r_a_i_t_s_<_R_>_:_:_L_o_c_a_l(measured_, x); │ │ │ │ │ -94 return _e_r_r_o_r; │ │ │ │ │ -95 } │ │ │ │ │ -96 │ │ │ │ │ -_9_9 void _p_r_i_n_t( │ │ │ │ │ -100 const std::string &s = "", │ │ │ │ │ -101 const _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ -102 _B_a_s_e_:_:_p_r_i_n_t(s, keyFormatter); │ │ │ │ │ -103 std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor(" │ │ │ │ │ -104 << keyFormatter(this->key1()) << ")" << std::endl; │ │ │ │ │ -105 _t_r_a_i_t_s_<_R_>_:_:_P_r_i_n_t(measured_, " measurement: "); │ │ │ │ │ -106 std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose() │ │ │ │ │ -107 << std::endl; │ │ │ │ │ -108 } │ │ │ │ │ -109 │ │ │ │ │ -_1_1_0 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r &other, double tol = 1e-9) const override │ │ │ │ │ -{ │ │ │ │ │ -111 const _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_<_R_,_ _T_> *e = │ │ │ │ │ -112 dynamic_cast *>(&other); │ │ │ │ │ -113 return e != nullptr && _B_a_s_e_:_:_e_q_u_a_l_s(other, tol) && │ │ │ │ │ -114 _t_r_a_i_t_s_<_R_>_:_:_E_q_u_a_l_s(this->measured_, e->measured_, tol); │ │ │ │ │ -115 } │ │ │ │ │ -117 │ │ │ │ │ -118 private: │ │ │ │ │ -_1_2_0 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -121 template │ │ │ │ │ -122 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ -123 // NoiseModelFactor1 instead of NoiseModelFactorN for backward │ │ │ │ │ -compatibility │ │ │ │ │ -124 ar &boost::serialization::make_nvp( │ │ │ │ │ -125 "NoiseModelFactor1", boost::serialization::base_object(*this)); │ │ │ │ │ -126 ar &BOOST_SERIALIZATION_NVP(measured_); │ │ │ │ │ -127 ar &BOOST_SERIALIZATION_NVP(func_); │ │ │ │ │ -128 } │ │ │ │ │ -129}; │ │ │ │ │ -130 │ │ │ │ │ -132template │ │ │ │ │ -_1_3_3struct _t_r_a_i_t_s<_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r> │ │ │ │ │ -134 : public _T_e_s_t_a_b_l_e> {}; │ │ │ │ │ -135 │ │ │ │ │ -142template │ │ │ │ │ -_1_4_3_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_<_R_,_ _T_> _M_a_k_e_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r(_K_e_y key, const R &z, │ │ │ │ │ -144 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, │ │ │ │ │ -145 const FUNC func) { │ │ │ │ │ -146 return _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_<_R_,_ _T_>(key, z, model, func); │ │ │ │ │ -147} │ │ │ │ │ +92 │ │ │ │ │ +93 bool _e_q_u_a_l_s(const This& other, double tol = 1e-9) const; │ │ │ │ │ +94 │ │ │ │ │ +_9_6 void _p_r_i_n_t( │ │ │ │ │ +97 const std::string& s = "SymbolicFactor", │ │ │ │ │ +98 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override { │ │ │ │ │ +99 Base::print(s, formatter); │ │ │ │ │ +100 } │ │ │ │ │ +101 │ │ │ │ │ +_1_0_3 void _p_r_i_n_t_K_e_y_s( │ │ │ │ │ +104 const std::string& s = "SymbolicFactor", │ │ │ │ │ +105 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override { │ │ │ │ │ +106 Base::printKeys(s, formatter); │ │ │ │ │ +107 } │ │ │ │ │ +108 │ │ │ │ │ +112 │ │ │ │ │ +114 template │ │ │ │ │ +_1_1_5 static _S_y_m_b_o_l_i_c_F_a_c_t_o_r _F_r_o_m_I_t_e_r_a_t_o_r_s(KEYITERATOR beginKey, KEYITERATOR │ │ │ │ │ +endKey) { │ │ │ │ │ +116 return _S_y_m_b_o_l_i_c_F_a_c_t_o_r(Base::FromIterators(beginKey, endKey)); │ │ │ │ │ +117 } │ │ │ │ │ +118 │ │ │ │ │ +120 template │ │ │ │ │ +_1_2_1 static _S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r _F_r_o_m_I_t_e_r_a_t_o_r_s_S_h_a_r_e_d(KEYITERATOR beginKey, │ │ │ │ │ +KEYITERATOR endKey) { │ │ │ │ │ +122 _S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r result = boost::make_shared(); │ │ │ │ │ +123 result->keys_.assign(beginKey, endKey); │ │ │ │ │ +124 return result; │ │ │ │ │ +125 } │ │ │ │ │ +126 │ │ │ │ │ +129 template │ │ │ │ │ +_1_3_0 static _S_y_m_b_o_l_i_c_F_a_c_t_o_r _F_r_o_m_K_e_y_s(const CONTAINER& keys) { │ │ │ │ │ +131 return _S_y_m_b_o_l_i_c_F_a_c_t_o_r(Base::FromKeys(keys)); │ │ │ │ │ +132 } │ │ │ │ │ +133 │ │ │ │ │ +136 template │ │ │ │ │ +_1_3_7 static _S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r _F_r_o_m_K_e_y_s_S_h_a_r_e_d(const CONTAINER& keys) { │ │ │ │ │ +138 return FromIteratorsShared(keys.begin(), keys.end()); │ │ │ │ │ +139 } │ │ │ │ │ +140 │ │ │ │ │ +142 │ │ │ │ │ +145 │ │ │ │ │ +147 double error(const _H_y_b_r_i_d_V_a_l_u_e_s& c) const override; │ │ │ │ │ 148 │ │ │ │ │ -158template │ │ │ │ │ -_1_5_9class _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2 : public _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N { │ │ │ │ │ -160 private: │ │ │ │ │ -161 using _B_a_s_e = _N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_T_1_,_ _T_2_>; │ │ │ │ │ -162 │ │ │ │ │ -163 R measured_; │ │ │ │ │ -164 _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l noiseModel_; │ │ │ │ │ -165 using FunctionType = std::function, │ │ │ │ │ -166 boost::optional)>; │ │ │ │ │ -167 FunctionType func_; │ │ │ │ │ +151 std::pair, boost:: │ │ │ │ │ +shared_ptr > │ │ │ │ │ +152 eliminate(const _O_r_d_e_r_i_n_g& keys) const; │ │ │ │ │ +153 │ │ │ │ │ +155 │ │ │ │ │ +156 private: │ │ │ │ │ +_1_5_8 friend class boost::serialization::access; │ │ │ │ │ +159 template │ │ │ │ │ +160 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +161 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +162 } │ │ │ │ │ +163 }; // IndexFactor │ │ │ │ │ +164 │ │ │ │ │ +165 // Forward declarations │ │ │ │ │ +166 class SymbolicFactorGraph; │ │ │ │ │ +167 class Ordering; │ │ │ │ │ 168 │ │ │ │ │ -169 public: │ │ │ │ │ -_1_7_1 _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2() {} │ │ │ │ │ -172 │ │ │ │ │ -_1_8_0 _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2(_K_e_y key1, _K_e_y key2, const R &z, │ │ │ │ │ -181 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, const FunctionType func) │ │ │ │ │ -182 : _B_a_s_e(model, key1, key2), │ │ │ │ │ -183 measured_(z), │ │ │ │ │ -184 noiseModel_(model), │ │ │ │ │ -185 func_(func) {} │ │ │ │ │ -186 │ │ │ │ │ -187 _~_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2() override {} │ │ │ │ │ -188 │ │ │ │ │ -_1_9_0 NonlinearFactor::shared_ptr _c_l_o_n_e() const override { │ │ │ │ │ -191 return boost::static_pointer_cast( │ │ │ │ │ -192 NonlinearFactor::shared_ptr(new _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2_<_R_,_ _T_1_,_ _T_2_>(*this))); │ │ │ │ │ -193 } │ │ │ │ │ -194 │ │ │ │ │ -195 Vector evaluateError( │ │ │ │ │ -196 const T1 ¶ms1, const T2 ¶ms2, │ │ │ │ │ -197 boost::optional H1 = boost::none, │ │ │ │ │ -198 boost::optional H2 = boost::none) const override { │ │ │ │ │ -199 R x = func_(params1, params2, H1, H2); │ │ │ │ │ -200 Vector _e_r_r_o_r = _t_r_a_i_t_s_<_R_>_:_:_L_o_c_a_l(measured_, x); │ │ │ │ │ -201 return _e_r_r_o_r; │ │ │ │ │ -202 } │ │ │ │ │ -203 │ │ │ │ │ -_2_0_6 void _p_r_i_n_t( │ │ │ │ │ -207 const std::string &s = "", │ │ │ │ │ -208 const _K_e_y_F_o_r_m_a_t_t_e_r &keyFormatter = DefaultKeyFormatter) const override { │ │ │ │ │ -209 _B_a_s_e_:_:_p_r_i_n_t(s, keyFormatter); │ │ │ │ │ -210 std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor2(" │ │ │ │ │ -211 << keyFormatter(this->key1()) << ", " │ │ │ │ │ -212 << keyFormatter(this->key2()) << ")" << std::endl; │ │ │ │ │ -213 _t_r_a_i_t_s_<_R_>_:_:_P_r_i_n_t(measured_, " measurement: "); │ │ │ │ │ -214 std::cout << " noise model sigmas: " << noiseModel_->sigmas().transpose() │ │ │ │ │ -215 << std::endl; │ │ │ │ │ -216 } │ │ │ │ │ -217 │ │ │ │ │ -_2_1_8 bool _e_q_u_a_l_s(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r &other, double tol = 1e-9) const override │ │ │ │ │ -{ │ │ │ │ │ -219 const _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2_<_R_,_ _T_1_,_ _T_2_> *e = │ │ │ │ │ -220 dynamic_cast *>(&other); │ │ │ │ │ -221 return e && _B_a_s_e_:_:_e_q_u_a_l_s(other, tol) && │ │ │ │ │ -222 _t_r_a_i_t_s_<_R_>_:_:_E_q_u_a_l_s(this->measured_, e->measured_, tol); │ │ │ │ │ -223 } │ │ │ │ │ -225 │ │ │ │ │ -226 private: │ │ │ │ │ -_2_2_8 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ -229 template │ │ │ │ │ -230 void serialize(ARCHIVE &ar, const unsigned int /*version*/) { │ │ │ │ │ -231 // NoiseModelFactor2 instead of NoiseModelFactorN for backward │ │ │ │ │ -compatibility │ │ │ │ │ -232 ar &boost::serialization::make_nvp( │ │ │ │ │ -233 "NoiseModelFactor2", boost::serialization::base_object(*this)); │ │ │ │ │ -234 ar &BOOST_SERIALIZATION_NVP(measured_); │ │ │ │ │ -235 ar &BOOST_SERIALIZATION_NVP(func_); │ │ │ │ │ -236 } │ │ │ │ │ -237}; │ │ │ │ │ -238 │ │ │ │ │ -240template │ │ │ │ │ -_2_4_1struct _t_r_a_i_t_s<_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2> │ │ │ │ │ -242 : public _T_e_s_t_a_b_l_e> {}; │ │ │ │ │ -243 │ │ │ │ │ -250template │ │ │ │ │ -_2_5_1_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2_<_R_,_ _T_1_,_ _T_2_> _M_a_k_e_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2( │ │ │ │ │ -252 _K_e_y key1, _K_e_y key2, const R &z, const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &model, │ │ │ │ │ -253 const FUNC func) { │ │ │ │ │ -254 return _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2_<_R_,_ _T_1_,_ _T_2_>(key1, key2, z, model, func); │ │ │ │ │ -255} │ │ │ │ │ -256 │ │ │ │ │ -257} // namespace gtsam │ │ │ │ │ +173 GTSAM_EXPORT std::pair, boost:: │ │ │ │ │ +shared_ptr > │ │ │ │ │ +174 _E_l_i_m_i_n_a_t_e_S_y_m_b_o_l_i_c(const SymbolicFactorGraph& factors, const Ordering& │ │ │ │ │ +keys); │ │ │ │ │ +175 │ │ │ │ │ +177 template<> │ │ │ │ │ +_1_7_8 struct _t_r_a_i_t_s<_S_y_m_b_o_l_i_c_F_a_c_t_o_r> : public _T_e_s_t_a_b_l_e { │ │ │ │ │ +179 }; │ │ │ │ │ +180 │ │ │ │ │ +181} //\ namespace gtsam │ │ │ │ │ +182 │ │ │ │ │ _T_e_s_t_a_b_l_e_._h │ │ │ │ │ Concept check for values that can be used in unit tests. │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_._h │ │ │ │ │ -Non-linear factor base classes. │ │ │ │ │ +_F_a_c_t_o_r_._h │ │ │ │ │ +The base class for all factors. │ │ │ │ │ +_K_e_y_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_k_e_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2 │ │ │ │ │ -FunctorizedFactor2< R, T1, T2 > MakeFunctorizedFactor2(Key key1, Key key2, │ │ │ │ │ -const R &z, const SharedNoiseModel &model, const FUNC func) │ │ │ │ │ -Helper function to create a functorized factor. │ │ │ │ │ -DDeeffiinniittiioonn FunctorizedFactor.h:251 │ │ │ │ │ -_g_t_s_a_m_:_:_M_a_k_e_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r │ │ │ │ │ -FunctorizedFactor< R, T > MakeFunctorizedFactor(Key key, const R &z, const │ │ │ │ │ -SharedNoiseModel &model, const FUNC func) │ │ │ │ │ -Helper function to create a functorized factor. │ │ │ │ │ -DDeeffiinniittiioonn FunctorizedFactor.h:143 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ -Aliases. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_S_y_m_b_o_l_i_c │ │ │ │ │ +std::pair< boost::shared_ptr< SymbolicConditional >, boost::shared_ptr< │ │ │ │ │ +SymbolicFactor > > EliminateSymbolic(const SymbolicFactorGraph &factors, const │ │ │ │ │ +Ordering &keys) │ │ │ │ │ +Dense elimination function for symbolic factors. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactor.cpp:36 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ +Template to create a binary predicate. │ │ │ │ │ +DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ _g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ A helper that implements the traits interface for GTSAM types. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ -virtual void print(const std::string &s="Factor", const KeyFormatter │ │ │ │ │ -&formatter=DefaultKeyFormatter) const │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:68 │ │ │ │ │ +_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ +DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +SymbolicConditional is a conditional with keys but no probability data, │ │ │ │ │ +produced by symbolic eliminat... │ │ │ │ │ +DDeeffiinniittiioonn SymbolicConditional.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r │ │ │ │ │ +SymbolicFactor represents a symbolic factor that specifies graph topology but │ │ │ │ │ +is not associated with ... │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactor.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="SymbolicFactor", const KeyFormatter │ │ │ │ │ +&formatter=DefaultKeyFormatter) const override │ │ │ │ │ print │ │ │ │ │ -DDeeffiinniittiioonn Factor.cpp:29 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const This &other, double tol=1e-9) const │ │ │ │ │ -check equality │ │ │ │ │ -DDeeffiinniittiioonn Factor.cpp:42 │ │ │ │ │ -_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r │ │ │ │ │ -Factor which evaluates provided unary functor and uses the result to compute │ │ │ │ │ -error with respect to th... │ │ │ │ │ -DDeeffiinniittiioonn FunctorizedFactor.h:59 │ │ │ │ │ -_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r │ │ │ │ │ -FunctorizedFactor(Key key, const R &z, const SharedNoiseModel &model, const │ │ │ │ │ -std::function< R(T, boost::optional< Matrix & >)> func) │ │ │ │ │ -Construct with given x and the parameters of the basis. │ │ │ │ │ -DDeeffiinniittiioonn FunctorizedFactor.h:78 │ │ │ │ │ -_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const NonlinearFactor &other, double tol=1e-9) const override │ │ │ │ │ -Check if two factors are equal. │ │ │ │ │ -DDeeffiinniittiioonn FunctorizedFactor.h:110 │ │ │ │ │ -_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn FunctorizedFactor.h:99 │ │ │ │ │ -_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r │ │ │ │ │ -FunctorizedFactor() │ │ │ │ │ -default constructor - only use for serialization │ │ │ │ │ -DDeeffiinniittiioonn FunctorizedFactor.h:69 │ │ │ │ │ -_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn FunctorizedFactor.h:120 │ │ │ │ │ -_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ -NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn FunctorizedFactor.h:85 │ │ │ │ │ -_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2 │ │ │ │ │ -Factor which evaluates provided binary functor and uses the result to compute │ │ │ │ │ -error with respect to t... │ │ │ │ │ -DDeeffiinniittiioonn FunctorizedFactor.h:159 │ │ │ │ │ -_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2 │ │ │ │ │ -FunctorizedFactor2() │ │ │ │ │ -default constructor - only use for serialization │ │ │ │ │ -DDeeffiinniittiioonn FunctorizedFactor.h:171 │ │ │ │ │ -_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2_:_:_c_l_o_n_e │ │ │ │ │ -NonlinearFactor::shared_ptr clone() const override │ │ │ │ │ -DDeeffiinniittiioonn FunctorizedFactor.h:190 │ │ │ │ │ -_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2 │ │ │ │ │ -FunctorizedFactor2(Key key1, Key key2, const R &z, const SharedNoiseModel │ │ │ │ │ -&model, const FunctionType func) │ │ │ │ │ -Construct with given x and the parameters of the basis. │ │ │ │ │ -DDeeffiinniittiioonn FunctorizedFactor.h:180 │ │ │ │ │ -_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="", const KeyFormatter │ │ │ │ │ -&keyFormatter=DefaultKeyFormatter) const override │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn FunctorizedFactor.h:206 │ │ │ │ │ -_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2_:_:_e_q_u_a_l_s │ │ │ │ │ -bool equals(const NonlinearFactor &other, double tol=1e-9) const override │ │ │ │ │ -Check if two factors are equal. │ │ │ │ │ -DDeeffiinniittiioonn FunctorizedFactor.h:218 │ │ │ │ │ -_g_t_s_a_m_:_:_F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_2_:_:_a_c_c_e_s_s │ │ │ │ │ -friend class boost::serialization::access │ │ │ │ │ -Serialization function. │ │ │ │ │ -DDeeffiinniittiioonn FunctorizedFactor.h:228 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ -Nonlinear factor base class. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_:_:_e_r_r_o_r │ │ │ │ │ -double error(const Values &c) const override │ │ │ │ │ -Calculate the error of the factor. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.cpp:138 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N │ │ │ │ │ -A convenient base class for creating your own NoiseModelFactor with n │ │ │ │ │ -variables. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:400 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_i_s_e_M_o_d_e_l_F_a_c_t_o_r_N_<_ _T_ _>_:_:_k_e_y │ │ │ │ │ -Key key() const │ │ │ │ │ -Returns a key. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:518 │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactor.h:96 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r │ │ │ │ │ +SymbolicFactor(Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) │ │ │ │ │ +Construct 6-way factor. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactor.h:77 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +Overriding the shared_ptr typedef. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactor.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_F_r_o_m_K_e_y_s │ │ │ │ │ +static SymbolicFactor FromKeys(const CONTAINER &keys) │ │ │ │ │ +Constructor from a collection of keys - compatible with boost assign::list_of │ │ │ │ │ +and boost assign::cref_... │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactor.h:130 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r │ │ │ │ │ +SymbolicFactor(const Factor &factor) │ │ │ │ │ +Create symbolic version of any factor. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactor.h:81 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r │ │ │ │ │ +SymbolicFactor(Key j1, Key j2, Key j3, Key j4) │ │ │ │ │ +Construct 4-way factor. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactor.h:69 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r │ │ │ │ │ +SymbolicFactor(Key j1, Key j2) │ │ │ │ │ +Construct binary factor. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactor.h:61 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_F_r_o_m_I_t_e_r_a_t_o_r_s │ │ │ │ │ +static SymbolicFactor FromIterators(KEYITERATOR beginKey, KEYITERATOR endKey) │ │ │ │ │ +Constructor from a collection of keys. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactor.h:115 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_p_r_i_n_t_K_e_y_s │ │ │ │ │ +void printKeys(const std::string &s="SymbolicFactor", const KeyFormatter │ │ │ │ │ +&formatter=DefaultKeyFormatter) const override │ │ │ │ │ +print only keys │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactor.h:103 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_F_r_o_m_K_e_y_s_S_h_a_r_e_d │ │ │ │ │ +static SymbolicFactor::shared_ptr FromKeysShared(const CONTAINER &keys) │ │ │ │ │ +Constructor from a collection of keys - compatible with boost assign::list_of │ │ │ │ │ +and boost assign::cref_... │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactor.h:137 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r │ │ │ │ │ +SymbolicFactor(Key j1, Key j2, Key j3) │ │ │ │ │ +Construct ternary factor. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactor.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r │ │ │ │ │ +SymbolicFactor(Key j) │ │ │ │ │ +Construct unary factor. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactor.h:57 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r │ │ │ │ │ +SymbolicFactor(Key j1, Key j2, Key j3, Key j4, Key j5) │ │ │ │ │ +Construct 5-way factor. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactor.h:73 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_F_r_o_m_I_t_e_r_a_t_o_r_s_S_h_a_r_e_d │ │ │ │ │ +static SymbolicFactor::shared_ptr FromIteratorsShared(KEYITERATOR beginKey, │ │ │ │ │ +KEYITERATOR endKey) │ │ │ │ │ +Constructor from a collection of keys. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactor.h:121 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r │ │ │ │ │ +SymbolicFactor() │ │ │ │ │ +Default constructor for I/O. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactor.h:54 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_c_l_o_n_e │ │ │ │ │ +SymbolicFactor::shared_ptr clone() const │ │ │ │ │ +Copy this object as its actual derived type. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicFactor.h:86 │ │ │ │ │ +_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +the error. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _F_u_n_c_t_o_r_i_z_e_d_F_a_c_t_o_r_._h │ │ │ │ │ + * _s_y_m_b_o_l_i_c │ │ │ │ │ + * _S_y_m_b_o_l_i_c_F_a_c_t_o_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01160_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00023_source.html │ │ │ │┄ Files 81% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/serialization.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
NonlinearConjugateGradientOptimizer.h
│ │ │ │ +
serialization.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/base/Manifold.h>
│ │ │ │ -
22#include <gtsam/nonlinear/NonlinearOptimizer.h>
│ │ │ │ -
23#include <boost/tuple/tuple.hpp>
│ │ │ │ -
24
│ │ │ │ -
25namespace gtsam {
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <Eigen/Core>
│ │ │ │ +
23#include <fstream>
│ │ │ │ +
24#include <sstream>
│ │ │ │ +
25#include <string>
│ │ │ │
26
│ │ │ │ -
│ │ │ │ -
28class GTSAM_EXPORT NonlinearConjugateGradientOptimizer : public NonlinearOptimizer {
│ │ │ │ -
29
│ │ │ │ -
30 /* a class for the nonlinearConjugateGradient template */
│ │ │ │ -
31 class System {
│ │ │ │ -
32 public:
│ │ │ │ -
33 typedef Values State;
│ │ │ │ -
34 typedef VectorValues Gradient;
│ │ │ │ -
35 typedef NonlinearOptimizerParams Parameters;
│ │ │ │ +
27// includes for standard serialization types
│ │ │ │ +
28#include <boost/serialization/version.hpp>
│ │ │ │ +
29#include <boost/serialization/optional.hpp>
│ │ │ │ +
30#include <boost/serialization/shared_ptr.hpp>
│ │ │ │ +
31#include <boost/serialization/vector.hpp>
│ │ │ │ +
32#include <boost/serialization/map.hpp>
│ │ │ │ +
33#include <boost/serialization/list.hpp>
│ │ │ │ +
34#include <boost/serialization/deque.hpp>
│ │ │ │ +
35#include <boost/serialization/weak_ptr.hpp>
│ │ │ │
36
│ │ │ │ -
37 protected:
│ │ │ │ -
38 const NonlinearFactorGraph &graph_;
│ │ │ │ -
39
│ │ │ │ -
40 public:
│ │ │ │ -
41 System(const NonlinearFactorGraph &graph) :
│ │ │ │ -
42 graph_(graph) {
│ │ │ │ -
43 }
│ │ │ │ -
44 double error(const State &state) const;
│ │ │ │ -
45 Gradient gradient(const State &state) const;
│ │ │ │ -
46 State advance(const State &current, const double alpha,
│ │ │ │ -
47 const Gradient &g) const;
│ │ │ │ -
48 };
│ │ │ │ -
49
│ │ │ │ -
50public:
│ │ │ │ -
51
│ │ │ │ -
52 typedef NonlinearOptimizer Base;
│ │ │ │ -
53 typedef NonlinearOptimizerParams Parameters;
│ │ │ │ -
54 typedef boost::shared_ptr<NonlinearConjugateGradientOptimizer> shared_ptr;
│ │ │ │ +
37#include <boost/archive/text_oarchive.hpp>
│ │ │ │ +
38#include <boost/archive/text_iarchive.hpp>
│ │ │ │ +
39#include <boost/archive/xml_iarchive.hpp>
│ │ │ │ +
40#include <boost/archive/xml_oarchive.hpp>
│ │ │ │ +
41#include <boost/archive/binary_iarchive.hpp>
│ │ │ │ +
42#include <boost/archive/binary_oarchive.hpp>
│ │ │ │ +
43#include <boost/serialization/export.hpp>
│ │ │ │ +
44
│ │ │ │ +
45// Workaround a bug in GCC >= 7 and C++17
│ │ │ │ +
46// ref. https://gitlab.com/libeigen/eigen/-/issues/1676
│ │ │ │ +
47#ifdef __GNUC__
│ │ │ │ +
48#if __GNUC__ >= 7 && __cplusplus >= 201703L
│ │ │ │ +
49namespace boost { namespace serialization { struct U; } }
│ │ │ │ +
50namespace Eigen { namespace internal {
│ │ │ │ +
51template<> struct traits<boost::serialization::U> {enum {Flags=0};};
│ │ │ │ +
52} }
│ │ │ │ +
53#endif
│ │ │ │ +
54#endif
│ │ │ │
55
│ │ │ │ -
56protected:
│ │ │ │ -
57 Parameters params_;
│ │ │ │ -
58
│ │ │ │ -
59 const NonlinearOptimizerParams& _params() const override {
│ │ │ │ -
60 return params_;
│ │ │ │ -
61 }
│ │ │ │ -
62
│ │ │ │ -
63public:
│ │ │ │ -
64
│ │ │ │ -
66 NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph& graph,
│ │ │ │ -
67 const Values& initialValues, const Parameters& params = Parameters());
│ │ │ │ +
56namespace gtsam {
│ │ │ │ +
57
│ │ │ │ +
63template <class T>
│ │ │ │ +
│ │ │ │ +
64void serializeToStream(const T& input, std::ostream& out_archive_stream) {
│ │ │ │ +
65 boost::archive::text_oarchive out_archive(out_archive_stream);
│ │ │ │ +
66 out_archive << input;
│ │ │ │ +
67}
│ │ │ │ +
│ │ │ │
68
│ │ │ │ -
│ │ │ │ -
70 ~NonlinearConjugateGradientOptimizer() override {
│ │ │ │ -
71 }
│ │ │ │ -
│ │ │ │ -
72
│ │ │ │ -
77 GaussianFactorGraph::shared_ptr iterate() override;
│ │ │ │ -
78
│ │ │ │ -
83 const Values& optimize() override;
│ │ │ │ -
84};
│ │ │ │ -
│ │ │ │ -
85
│ │ │ │ -
87template<class S, class V, class W>
│ │ │ │ -
│ │ │ │ -
88double lineSearch(const S &system, const V currentValues, const W &gradient) {
│ │ │ │ -
89
│ │ │ │ -
90 /* normalize it such that it becomes a unit vector */
│ │ │ │ -
91 const double g = gradient.norm();
│ │ │ │ -
92
│ │ │ │ -
93 // perform the golden section search algorithm to decide the the optimal step size
│ │ │ │ -
94 // detail refer to http://en.wikipedia.org/wiki/Golden_section_search
│ │ │ │ -
95 const double phi = 0.5 * (1.0 + std::sqrt(5.0)), resphi = 2.0 - phi, tau =
│ │ │ │ -
96 1e-5;
│ │ │ │ -
97 double minStep = -1.0 / g, maxStep = 0, newStep = minStep
│ │ │ │ -
98 + (maxStep - minStep) / (phi + 1.0);
│ │ │ │ -
99
│ │ │ │ -
100 V newValues = system.advance(currentValues, newStep, gradient);
│ │ │ │ -
101 double newError = system.error(newValues);
│ │ │ │ -
102
│ │ │ │ -
103 while (true) {
│ │ │ │ -
104 const bool flag = (maxStep - newStep > newStep - minStep) ? true : false;
│ │ │ │ -
105 const double testStep =
│ │ │ │ -
106 flag ? newStep + resphi * (maxStep - newStep) :
│ │ │ │ -
107 newStep - resphi * (newStep - minStep);
│ │ │ │ -
108
│ │ │ │ -
109 if ((maxStep - minStep)
│ │ │ │ -
110 < tau * (std::abs(testStep) + std::abs(newStep))) {
│ │ │ │ -
111 return 0.5 * (minStep + maxStep);
│ │ │ │ -
112 }
│ │ │ │ -
113
│ │ │ │ -
114 const V testValues = system.advance(currentValues, testStep, gradient);
│ │ │ │ -
115 const double testError = system.error(testValues);
│ │ │ │ +
70template <class T>
│ │ │ │ +
│ │ │ │ +
71void deserializeFromStream(std::istream& in_archive_stream, T& output) {
│ │ │ │ +
72 boost::archive::text_iarchive in_archive(in_archive_stream);
│ │ │ │ +
73 in_archive >> output;
│ │ │ │ +
74}
│ │ │ │ +
│ │ │ │ +
75
│ │ │ │ +
77template <class T>
│ │ │ │ +
│ │ │ │ +
78std::string serializeToString(const T& input) {
│ │ │ │ +
79 std::ostringstream out_archive_stream;
│ │ │ │ +
80 serializeToStream(input, out_archive_stream);
│ │ │ │ +
81 return out_archive_stream.str();
│ │ │ │ +
82}
│ │ │ │ +
│ │ │ │ +
83
│ │ │ │ +
85template <class T>
│ │ │ │ +
│ │ │ │ +
86void deserializeFromString(const std::string& serialized, T& output) {
│ │ │ │ +
87 std::istringstream in_archive_stream(serialized);
│ │ │ │ +
88 deserializeFromStream(in_archive_stream, output);
│ │ │ │ +
89}
│ │ │ │ +
│ │ │ │ +
90
│ │ │ │ +
92template <class T>
│ │ │ │ +
│ │ │ │ +
93bool serializeToFile(const T& input, const std::string& filename) {
│ │ │ │ +
94 std::ofstream out_archive_stream(filename.c_str());
│ │ │ │ +
95 if (!out_archive_stream.is_open()) return false;
│ │ │ │ +
96 serializeToStream(input, out_archive_stream);
│ │ │ │ +
97 out_archive_stream.close();
│ │ │ │ +
98 return true;
│ │ │ │ +
99}
│ │ │ │ +
│ │ │ │ +
100
│ │ │ │ +
102template <class T>
│ │ │ │ +
│ │ │ │ +
103bool deserializeFromFile(const std::string& filename, T& output) {
│ │ │ │ +
104 std::ifstream in_archive_stream(filename.c_str());
│ │ │ │ +
105 if (!in_archive_stream.is_open()) return false;
│ │ │ │ +
106 deserializeFromStream(in_archive_stream, output);
│ │ │ │ +
107 in_archive_stream.close();
│ │ │ │ +
108 return true;
│ │ │ │ +
109}
│ │ │ │ +
│ │ │ │ +
110
│ │ │ │ +
112template <class T>
│ │ │ │ +
│ │ │ │ +
113std::string serialize(const T& input) {
│ │ │ │ +
114 return serializeToString(input);
│ │ │ │ +
115}
│ │ │ │ +
│ │ │ │
116
│ │ │ │ -
117 // update the working range
│ │ │ │ -
118 if (testError >= newError) {
│ │ │ │ -
119 if (flag)
│ │ │ │ -
120 maxStep = testStep;
│ │ │ │ -
121 else
│ │ │ │ -
122 minStep = testStep;
│ │ │ │ -
123 } else {
│ │ │ │ -
124 if (flag) {
│ │ │ │ -
125 minStep = newStep;
│ │ │ │ -
126 newStep = testStep;
│ │ │ │ -
127 newError = testError;
│ │ │ │ -
128 } else {
│ │ │ │ -
129 maxStep = newStep;
│ │ │ │ -
130 newStep = testStep;
│ │ │ │ -
131 newError = testError;
│ │ │ │ -
132 }
│ │ │ │ -
133 }
│ │ │ │ -
134 }
│ │ │ │ -
135 return 0.0;
│ │ │ │ -
136}
│ │ │ │ -
│ │ │ │ -
137
│ │ │ │ -
147template<class S, class V>
│ │ │ │ -
│ │ │ │ -
148boost::tuple<V, int> nonlinearConjugateGradient(const S &system,
│ │ │ │ -
149 const V &initial, const NonlinearOptimizerParams &params,
│ │ │ │ -
150 const bool singleIteration, const bool gradientDescent = false) {
│ │ │ │ -
151
│ │ │ │ -
152 // GTSAM_CONCEPT_MANIFOLD_TYPE(V)
│ │ │ │ -
153
│ │ │ │ -
154 size_t iteration = 0;
│ │ │ │ -
155
│ │ │ │ -
156 // check if we're already close enough
│ │ │ │ -
157 double currentError = system.error(initial);
│ │ │ │ -
158 if (currentError <= params.errorTol) {
│ │ │ │ -
159 if (params.verbosity >= NonlinearOptimizerParams::ERROR) {
│ │ │ │ -
160 std::cout << "Exiting, as error = " << currentError << " < "
│ │ │ │ -
161 << params.errorTol << std::endl;
│ │ │ │ -
162 }
│ │ │ │ -
163 return boost::tie(initial, iteration);
│ │ │ │ -
164 }
│ │ │ │ -
165
│ │ │ │ -
166 V currentValues = initial;
│ │ │ │ -
167 typename S::Gradient currentGradient = system.gradient(currentValues),
│ │ │ │ -
168 prevGradient, direction = currentGradient;
│ │ │ │ -
169
│ │ │ │ -
170 /* do one step of gradient descent */
│ │ │ │ -
171 V prevValues = currentValues;
│ │ │ │ -
172 double prevError = currentError;
│ │ │ │ -
173 double alpha = lineSearch(system, currentValues, direction);
│ │ │ │ -
174 currentValues = system.advance(prevValues, alpha, direction);
│ │ │ │ -
175 currentError = system.error(currentValues);
│ │ │ │ -
176
│ │ │ │ -
177 // Maybe show output
│ │ │ │ -
178 if (params.verbosity >= NonlinearOptimizerParams::ERROR)
│ │ │ │ -
179 std::cout << "Initial error: " << currentError << std::endl;
│ │ │ │ -
180
│ │ │ │ -
181 // Iterative loop
│ │ │ │ -
182 do {
│ │ │ │ -
183 if (gradientDescent == true) {
│ │ │ │ -
184 direction = system.gradient(currentValues);
│ │ │ │ -
185 } else {
│ │ │ │ -
186 prevGradient = currentGradient;
│ │ │ │ -
187 currentGradient = system.gradient(currentValues);
│ │ │ │ -
188 // Polak-Ribiere: beta = g'*(g_n-g_n-1)/g_n-1'*g_n-1
│ │ │ │ -
189 const double beta = std::max(0.0,
│ │ │ │ -
190 currentGradient.dot(currentGradient - prevGradient)
│ │ │ │ -
191 / prevGradient.dot(prevGradient));
│ │ │ │ -
192 direction = currentGradient + (beta * direction);
│ │ │ │ -
193 }
│ │ │ │ -
194
│ │ │ │ -
195 alpha = lineSearch(system, currentValues, direction);
│ │ │ │ -
196
│ │ │ │ -
197 prevValues = currentValues;
│ │ │ │ -
198 prevError = currentError;
│ │ │ │ -
199
│ │ │ │ -
200 currentValues = system.advance(prevValues, alpha, direction);
│ │ │ │ -
201 currentError = system.error(currentValues);
│ │ │ │ -
202
│ │ │ │ -
203 // User hook:
│ │ │ │ -
204 if (params.iterationHook)
│ │ │ │ -
205 params.iterationHook(iteration, prevError, currentError);
│ │ │ │ -
206
│ │ │ │ -
207 // Maybe show output
│ │ │ │ -
208 if (params.verbosity >= NonlinearOptimizerParams::ERROR)
│ │ │ │ -
209 std::cout << "iteration: " << iteration << ", currentError: " << currentError << std::endl;
│ │ │ │ -
210 } while (++iteration < params.maxIterations && !singleIteration
│ │ │ │ -
211 && !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol,
│ │ │ │ -
212 params.errorTol, prevError, currentError, params.verbosity));
│ │ │ │ -
213
│ │ │ │ -
214 // Printing if verbose
│ │ │ │ -
215 if (params.verbosity >= NonlinearOptimizerParams::ERROR
│ │ │ │ -
216 && iteration >= params.maxIterations)
│ │ │ │ -
217 std::cout
│ │ │ │ -
218 << "nonlinearConjugateGradient: Terminating because reached maximum iterations"
│ │ │ │ -
219 << std::endl;
│ │ │ │ -
220
│ │ │ │ -
221 return boost::tie(currentValues, iteration);
│ │ │ │ -
222}
│ │ │ │ -
│ │ │ │ -
223
│ │ │ │ -
224} // \ namespace gtsam
│ │ │ │ -
225
│ │ │ │ -
Manifold.h
Base class and basic functions for Manifold types.
│ │ │ │ -
NonlinearOptimizer.h
Base class and parameters for nonlinear optimization algorithms.
│ │ │ │ +
118template <class T>
│ │ │ │ +
│ │ │ │ +
119void deserialize(const std::string& serialized, T& output) {
│ │ │ │ +
120 deserializeFromString(serialized, output);
│ │ │ │ +
121}
│ │ │ │ +
│ │ │ │ +
123
│ │ │ │ +
129template <class T>
│ │ │ │ +
│ │ │ │ +
130void serializeToXMLStream(const T& input, std::ostream& out_archive_stream,
│ │ │ │ +
131 const std::string& name = "data") {
│ │ │ │ +
132 boost::archive::xml_oarchive out_archive(out_archive_stream);
│ │ │ │ +
133 out_archive << boost::serialization::make_nvp(name.c_str(), input);
│ │ │ │ +
134}
│ │ │ │ +
│ │ │ │ +
135
│ │ │ │ +
137template <class T>
│ │ │ │ +
│ │ │ │ +
138void deserializeFromXMLStream(std::istream& in_archive_stream, T& output,
│ │ │ │ +
139 const std::string& name = "data") {
│ │ │ │ +
140 boost::archive::xml_iarchive in_archive(in_archive_stream);
│ │ │ │ +
141 in_archive >> boost::serialization::make_nvp(name.c_str(), output);
│ │ │ │ +
142}
│ │ │ │ +
│ │ │ │ +
143
│ │ │ │ +
145template <class T>
│ │ │ │ +
│ │ │ │ +
146std::string serializeToXMLString(const T& input,
│ │ │ │ +
147 const std::string& name = "data") {
│ │ │ │ +
148 std::ostringstream out_archive_stream;
│ │ │ │ +
149 serializeToXMLStream(input, out_archive_stream, name);
│ │ │ │ +
150 return out_archive_stream.str();
│ │ │ │ +
151}
│ │ │ │ +
│ │ │ │ +
152
│ │ │ │ +
154template <class T>
│ │ │ │ +
│ │ │ │ +
155void deserializeFromXMLString(const std::string& serialized, T& output,
│ │ │ │ +
156 const std::string& name = "data") {
│ │ │ │ +
157 std::istringstream in_archive_stream(serialized);
│ │ │ │ +
158 deserializeFromXMLStream(in_archive_stream, output, name);
│ │ │ │ +
159}
│ │ │ │ +
│ │ │ │ +
160
│ │ │ │ +
162template <class T>
│ │ │ │ +
│ │ │ │ +
163bool serializeToXMLFile(const T& input, const std::string& filename,
│ │ │ │ +
164 const std::string& name = "data") {
│ │ │ │ +
165 std::ofstream out_archive_stream(filename.c_str());
│ │ │ │ +
166 if (!out_archive_stream.is_open()) return false;
│ │ │ │ +
167 serializeToXMLStream(input, out_archive_stream, name);
│ │ │ │ +
168 out_archive_stream.close();
│ │ │ │ +
169 return true;
│ │ │ │ +
170}
│ │ │ │ +
│ │ │ │ +
171
│ │ │ │ +
173template <class T>
│ │ │ │ +
│ │ │ │ +
174bool deserializeFromXMLFile(const std::string& filename, T& output,
│ │ │ │ +
175 const std::string& name = "data") {
│ │ │ │ +
176 std::ifstream in_archive_stream(filename.c_str());
│ │ │ │ +
177 if (!in_archive_stream.is_open()) return false;
│ │ │ │ +
178 deserializeFromXMLStream(in_archive_stream, output, name);
│ │ │ │ +
179 in_archive_stream.close();
│ │ │ │ +
180 return true;
│ │ │ │ +
181}
│ │ │ │ +
│ │ │ │ +
182
│ │ │ │ +
184template <class T>
│ │ │ │ +
│ │ │ │ +
185std::string serializeXML(const T& input,
│ │ │ │ +
186 const std::string& name = "data") {
│ │ │ │ +
187 return serializeToXMLString(input, name);
│ │ │ │ +
188}
│ │ │ │ +
│ │ │ │ +
189
│ │ │ │ +
191template <class T>
│ │ │ │ +
│ │ │ │ +
192void deserializeXML(const std::string& serialized, T& output,
│ │ │ │ +
193 const std::string& name = "data") {
│ │ │ │ +
194 deserializeFromXMLString(serialized, output, name);
│ │ │ │ +
195}
│ │ │ │ +
│ │ │ │ +
197
│ │ │ │ +
203template <class T>
│ │ │ │ +
│ │ │ │ +
204void serializeToBinaryStream(const T& input, std::ostream& out_archive_stream,
│ │ │ │ +
205 const std::string& name = "data") {
│ │ │ │ +
206 boost::archive::binary_oarchive out_archive(out_archive_stream);
│ │ │ │ +
207 out_archive << boost::serialization::make_nvp(name.c_str(), input);
│ │ │ │ +
208}
│ │ │ │ +
│ │ │ │ +
209
│ │ │ │ +
211template <class T>
│ │ │ │ +
│ │ │ │ +
212void deserializeFromBinaryStream(std::istream& in_archive_stream, T& output,
│ │ │ │ +
213 const std::string& name = "data") {
│ │ │ │ +
214 boost::archive::binary_iarchive in_archive(in_archive_stream);
│ │ │ │ +
215 in_archive >> boost::serialization::make_nvp(name.c_str(), output);
│ │ │ │ +
216}
│ │ │ │ +
│ │ │ │ +
217
│ │ │ │ +
219template <class T>
│ │ │ │ +
│ │ │ │ +
220std::string serializeToBinaryString(const T& input,
│ │ │ │ +
221 const std::string& name = "data") {
│ │ │ │ +
222 std::ostringstream out_archive_stream;
│ │ │ │ +
223 serializeToBinaryStream(input, out_archive_stream, name);
│ │ │ │ +
224 return out_archive_stream.str();
│ │ │ │ +
225}
│ │ │ │ +
│ │ │ │ +
226
│ │ │ │ +
228template <class T>
│ │ │ │ +
│ │ │ │ +
229void deserializeFromBinaryString(const std::string& serialized, T& output,
│ │ │ │ +
230 const std::string& name = "data") {
│ │ │ │ +
231 std::istringstream in_archive_stream(serialized);
│ │ │ │ +
232 deserializeFromBinaryStream(in_archive_stream, output, name);
│ │ │ │ +
233}
│ │ │ │ +
│ │ │ │ +
234
│ │ │ │ +
236template <class T>
│ │ │ │ +
│ │ │ │ +
237bool serializeToBinaryFile(const T& input, const std::string& filename,
│ │ │ │ +
238 const std::string& name = "data") {
│ │ │ │ +
239 std::ofstream out_archive_stream(filename.c_str());
│ │ │ │ +
240 if (!out_archive_stream.is_open()) return false;
│ │ │ │ +
241 serializeToBinaryStream(input, out_archive_stream, name);
│ │ │ │ +
242 out_archive_stream.close();
│ │ │ │ +
243 return true;
│ │ │ │ +
244}
│ │ │ │ +
│ │ │ │ +
245
│ │ │ │ +
247template <class T>
│ │ │ │ +
│ │ │ │ +
248bool deserializeFromBinaryFile(const std::string& filename, T& output,
│ │ │ │ +
249 const std::string& name = "data") {
│ │ │ │ +
250 std::ifstream in_archive_stream(filename.c_str());
│ │ │ │ +
251 if (!in_archive_stream.is_open()) return false;
│ │ │ │ +
252 deserializeFromBinaryStream(in_archive_stream, output, name);
│ │ │ │ +
253 in_archive_stream.close();
│ │ │ │ +
254 return true;
│ │ │ │ +
255}
│ │ │ │ +
│ │ │ │ +
256
│ │ │ │ +
258template <class T>
│ │ │ │ +
│ │ │ │ +
259std::string serializeBinary(const T& input,
│ │ │ │ +
260 const std::string& name = "data") {
│ │ │ │ +
261 return serializeToBinaryString(input, name);
│ │ │ │ +
262}
│ │ │ │ +
│ │ │ │ +
263
│ │ │ │ +
265template <class T>
│ │ │ │ +
│ │ │ │ +
266void deserializeBinary(const std::string& serialized, T& output,
│ │ │ │ +
267 const std::string& name = "data") {
│ │ │ │ +
268 deserializeFromBinaryString(serialized, output, name);
│ │ │ │ +
269}
│ │ │ │ +
│ │ │ │ +
271
│ │ │ │ +
272} // namespace gtsam
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::lineSearch
double lineSearch(const S &system, const V currentValues, const W &gradient)
Implement the golden-section line search algorithm.
Definition NonlinearConjugateGradientOptimizer.h:88
│ │ │ │ -
gtsam::checkConvergence
bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold, double errorThreshold, double currentError, double newError, NonlinearOptimizerParams::Verbosity verbosity)
Check whether the relative error decrease is less than relativeErrorTreshold, the absolute error decr...
Definition NonlinearOptimizer.cpp:185
│ │ │ │ -
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition triangulation.cpp:155
│ │ │ │ -
gtsam::nonlinearConjugateGradient
boost::tuple< V, int > nonlinearConjugateGradient(const S &system, const V &initial, const NonlinearOptimizerParams &params, const bool singleIteration, const bool gradientDescent=false)
Implement the nonlinear conjugate gradient method using the Polak-Ribiere formula suggested in http:/...
Definition NonlinearConjugateGradientOptimizer.h:148
│ │ │ │ -
gtsam::GaussianFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition GaussianFactorGraph.h:81
│ │ │ │ -
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ -
gtsam::NonlinearConjugateGradientOptimizer
An implementation of the nonlinear CG method using the template below.
Definition NonlinearConjugateGradientOptimizer.h:28
│ │ │ │ -
gtsam::NonlinearConjugateGradientOptimizer::~NonlinearConjugateGradientOptimizer
~NonlinearConjugateGradientOptimizer() override
Destructor.
Definition NonlinearConjugateGradientOptimizer.h:70
│ │ │ │ -
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │ -
gtsam::NonlinearOptimizer
This is the abstract interface for classes that can optimize for the maximum-likelihood estimate of a...
Definition NonlinearOptimizer.h:75
│ │ │ │ -
gtsam::NonlinearOptimizerParams
The common parameters for Nonlinear optimizers.
Definition NonlinearOptimizerParams.h:34
│ │ │ │ -
gtsam::NonlinearOptimizerParams::absoluteErrorTol
double absoluteErrorTol
The maximum absolute error decrease to stop iterating (default 1e-5)
Definition NonlinearOptimizerParams.h:43
│ │ │ │ -
gtsam::NonlinearOptimizerParams::iterationHook
IterationHook iterationHook
Optional user-provided iteration hook to be called after each optimization iteration (Default: none).
Definition NonlinearOptimizerParams.h:94
│ │ │ │ -
gtsam::NonlinearOptimizerParams::maxIterations
size_t maxIterations
The maximum iterations to stop iterating (default 100)
Definition NonlinearOptimizerParams.h:41
│ │ │ │ -
gtsam::NonlinearOptimizerParams::verbosity
Verbosity verbosity
The printing verbosity during optimization (default SILENT)
Definition NonlinearOptimizerParams.h:45
│ │ │ │ -
gtsam::NonlinearOptimizerParams::relativeErrorTol
double relativeErrorTol
The maximum relative error decrease to stop iterating (default 1e-5)
Definition NonlinearOptimizerParams.h:42
│ │ │ │ -
gtsam::NonlinearOptimizerParams::errorTol
double errorTol
The maximum total error to stop iterating (default 0.0)
Definition NonlinearOptimizerParams.h:44
│ │ │ │ -
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
gtsam::serializeToStream
void serializeToStream(const T &input, std::ostream &out_archive_stream)
Definition serialization.h:64
│ │ │ │ +
gtsam::deserializeFromXMLFile
bool deserializeFromXMLFile(const std::string &filename, T &output, const std::string &name="data")
deserializes from an XML file
Definition serialization.h:174
│ │ │ │ +
gtsam::serializeToXMLString
std::string serializeToXMLString(const T &input, const std::string &name="data")
serializes to a string in XML
Definition serialization.h:146
│ │ │ │ +
gtsam::deserializeFromString
void deserializeFromString(const std::string &serialized, T &output)
deserializes from a string
Definition serialization.h:86
│ │ │ │ +
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition serialization.h:113
│ │ │ │ +
gtsam::deserializeFromBinaryString
void deserializeFromBinaryString(const std::string &serialized, T &output, const std::string &name="data")
deserializes from a string in binary
Definition serialization.h:229
│ │ │ │ +
gtsam::deserializeBinary
void deserializeBinary(const std::string &serialized, T &output, const std::string &name="data")
deserializes from a string in binary
Definition serialization.h:266
│ │ │ │ +
gtsam::deserializeFromXMLString
void deserializeFromXMLString(const std::string &serialized, T &output, const std::string &name="data")
deserializes from a string in XML
Definition serialization.h:155
│ │ │ │ +
gtsam::serializeToBinaryString
std::string serializeToBinaryString(const T &input, const std::string &name="data")
serializes to a string in binary
Definition serialization.h:220
│ │ │ │ +
gtsam::deserializeXML
void deserializeXML(const std::string &serialized, T &output, const std::string &name="data")
deserializes from a string in XML
Definition serialization.h:192
│ │ │ │ +
gtsam::serializeToXMLFile
bool serializeToXMLFile(const T &input, const std::string &filename, const std::string &name="data")
serializes to an XML file
Definition serialization.h:163
│ │ │ │ +
gtsam::deserializeFromXMLStream
void deserializeFromXMLStream(std::istream &in_archive_stream, T &output, const std::string &name="data")
deserializes from a stream in XML
Definition serialization.h:138
│ │ │ │ +
gtsam::deserializeFromBinaryStream
void deserializeFromBinaryStream(std::istream &in_archive_stream, T &output, const std::string &name="data")
deserializes from a stream in binary
Definition serialization.h:212
│ │ │ │ +
gtsam::serializeXML
std::string serializeXML(const T &input, const std::string &name="data")
serializes to a string in XML
Definition serialization.h:185
│ │ │ │ +
gtsam::serializeToString
std::string serializeToString(const T &input)
serializes to a string
Definition serialization.h:78
│ │ │ │ +
gtsam::serializeToFile
bool serializeToFile(const T &input, const std::string &filename)
serializes to a file
Definition serialization.h:93
│ │ │ │ +
gtsam::deserialize
void deserialize(const std::string &serialized, T &output)
deserializes from a string
Definition serialization.h:119
│ │ │ │ +
gtsam::serializeToBinaryStream
void serializeToBinaryStream(const T &input, std::ostream &out_archive_stream, const std::string &name="data")
Definition serialization.h:204
│ │ │ │ +
gtsam::deserializeFromStream
void deserializeFromStream(std::istream &in_archive_stream, T &output)
deserializes from a stream
Definition serialization.h:71
│ │ │ │ +
gtsam::serializeBinary
std::string serializeBinary(const T &input, const std::string &name="data")
serializes to a string in binary
Definition serialization.h:259
│ │ │ │ +
gtsam::serializeToBinaryFile
bool serializeToBinaryFile(const T &input, const std::string &filename, const std::string &name="data")
serializes to a binary file
Definition serialization.h:237
│ │ │ │ +
gtsam::deserializeFromFile
bool deserializeFromFile(const std::string &filename, T &output)
deserializes from a file
Definition serialization.h:103
│ │ │ │ +
gtsam::serializeToXMLStream
void serializeToXMLStream(const T &input, std::ostream &out_archive_stream, const std::string &name="data")
Definition serialization.h:130
│ │ │ │ +
gtsam::deserializeFromBinaryFile
bool deserializeFromBinaryFile(const std::string &filename, T &output, const std::string &name="data")
deserializes from a binary file
Definition serialization.h:248
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,298 +1,349 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -NonlinearConjugateGradientOptimizer.h │ │ │ │ │ +serialization.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_n_i_f_o_l_d_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_._h> │ │ │ │ │ -23#include │ │ │ │ │ -24 │ │ │ │ │ -25namespace _g_t_s_a_m { │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include │ │ │ │ │ +23#include │ │ │ │ │ +24#include │ │ │ │ │ +25#include │ │ │ │ │ 26 │ │ │ │ │ -_2_8class GTSAM_EXPORT _N_o_n_l_i_n_e_a_r_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_O_p_t_i_m_i_z_e_r : public │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r { │ │ │ │ │ -29 │ │ │ │ │ -30 /* a class for the nonlinearConjugateGradient template */ │ │ │ │ │ -31 class System { │ │ │ │ │ -32 public: │ │ │ │ │ -33 typedef _V_a_l_u_e_s State; │ │ │ │ │ -34 typedef _V_e_c_t_o_r_V_a_l_u_e_s Gradient; │ │ │ │ │ -35 typedef _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s _P_a_r_a_m_e_t_e_r_s; │ │ │ │ │ +27// includes for standard serialization types │ │ │ │ │ +28#include │ │ │ │ │ +29#include │ │ │ │ │ +30#include │ │ │ │ │ +31#include │ │ │ │ │ +32#include │ │ │ │ │ +33#include │ │ │ │ │ +34#include │ │ │ │ │ +35#include │ │ │ │ │ 36 │ │ │ │ │ -37 protected: │ │ │ │ │ -38 const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h &graph_; │ │ │ │ │ -39 │ │ │ │ │ -40 public: │ │ │ │ │ -41 System(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h &graph) : │ │ │ │ │ -42 graph_(graph) { │ │ │ │ │ -43 } │ │ │ │ │ -44 double error(const State &state) const; │ │ │ │ │ -45 Gradient gradient(const State &state) const; │ │ │ │ │ -46 State advance(const State ¤t, const double alpha, │ │ │ │ │ -47 const Gradient &g) const; │ │ │ │ │ -48 }; │ │ │ │ │ -49 │ │ │ │ │ -50public: │ │ │ │ │ -51 │ │ │ │ │ -52 typedef _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r _B_a_s_e; │ │ │ │ │ -53 typedef _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s _P_a_r_a_m_e_t_e_r_s; │ │ │ │ │ -54 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +37#include │ │ │ │ │ +38#include │ │ │ │ │ +39#include │ │ │ │ │ +40#include │ │ │ │ │ +41#include │ │ │ │ │ +42#include │ │ │ │ │ +43#include │ │ │ │ │ +44 │ │ │ │ │ +45// Workaround a bug in GCC >= 7 and C++17 │ │ │ │ │ +46// ref. https://gitlab.com/libeigen/eigen/-/issues/1676 │ │ │ │ │ +47#ifdef __GNUC__ │ │ │ │ │ +48#if __GNUC__ >= 7 && __cplusplus >= 201703L │ │ │ │ │ +49namespace boost { namespace serialization { struct U; } } │ │ │ │ │ +50namespace Eigen { namespace internal { │ │ │ │ │ +51template<> struct traits {enum {Flags=0};}; │ │ │ │ │ +52} } │ │ │ │ │ +53#endif │ │ │ │ │ +54#endif │ │ │ │ │ 55 │ │ │ │ │ -56protected: │ │ │ │ │ -57 _P_a_r_a_m_e_t_e_r_s params_; │ │ │ │ │ -58 │ │ │ │ │ -59 const _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s& _params() const override { │ │ │ │ │ -60 return params_; │ │ │ │ │ -61 } │ │ │ │ │ -62 │ │ │ │ │ -63public: │ │ │ │ │ -64 │ │ │ │ │ -66 _N_o_n_l_i_n_e_a_r_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_O_p_t_i_m_i_z_e_r(const _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h& graph, │ │ │ │ │ -67 const _V_a_l_u_e_s& initialValues, const _P_a_r_a_m_e_t_e_r_s& params = _P_a_r_a_m_e_t_e_r_s()); │ │ │ │ │ +56namespace _g_t_s_a_m { │ │ │ │ │ +57 │ │ │ │ │ +63template │ │ │ │ │ +_6_4void _s_e_r_i_a_l_i_z_e_T_o_S_t_r_e_a_m(const T& input, std::ostream& out_archive_stream) { │ │ │ │ │ +65 boost::archive::text_oarchive out_archive(out_archive_stream); │ │ │ │ │ +66 out_archive << input; │ │ │ │ │ +67} │ │ │ │ │ 68 │ │ │ │ │ -_7_0 _~_N_o_n_l_i_n_e_a_r_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_O_p_t_i_m_i_z_e_r() override { │ │ │ │ │ -71 } │ │ │ │ │ -72 │ │ │ │ │ -77 _G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r iterate() override; │ │ │ │ │ -78 │ │ │ │ │ -83 const _V_a_l_u_e_s& _o_p_t_i_m_i_z_e() override; │ │ │ │ │ -84}; │ │ │ │ │ -85 │ │ │ │ │ -87template │ │ │ │ │ -_8_8double _l_i_n_e_S_e_a_r_c_h(const S &system, const V currentValues, const W &gradient) │ │ │ │ │ -{ │ │ │ │ │ -89 │ │ │ │ │ -90 /* normalize it such that it becomes a unit vector */ │ │ │ │ │ -91 const double g = gradient.norm(); │ │ │ │ │ -92 │ │ │ │ │ -93 // perform the golden section search algorithm to decide the the optimal │ │ │ │ │ -step size │ │ │ │ │ -94 // detail refer to http://en.wikipedia.org/wiki/Golden_section_search │ │ │ │ │ -95 const double phi = 0.5 * (1.0 + std::sqrt(5.0)), resphi = 2.0 - phi, tau = │ │ │ │ │ -96 1e-5; │ │ │ │ │ -97 double minStep = -1.0 / g, maxStep = 0, newStep = minStep │ │ │ │ │ -98 + (maxStep - minStep) / (phi + 1.0); │ │ │ │ │ -99 │ │ │ │ │ -100 V newValues = system.advance(currentValues, newStep, gradient); │ │ │ │ │ -101 double newError = system.error(newValues); │ │ │ │ │ -102 │ │ │ │ │ -103 while (true) { │ │ │ │ │ -104 const bool flag = (maxStep - newStep > newStep - minStep) ? true : false; │ │ │ │ │ -105 const double testStep = │ │ │ │ │ -106 flag ? newStep + resphi * (maxStep - newStep) : │ │ │ │ │ -107 newStep - resphi * (newStep - minStep); │ │ │ │ │ -108 │ │ │ │ │ -109 if ((maxStep - minStep) │ │ │ │ │ -110 < tau * (std::abs(testStep) + std::abs(newStep))) { │ │ │ │ │ -111 return 0.5 * (minStep + maxStep); │ │ │ │ │ -112 } │ │ │ │ │ -113 │ │ │ │ │ -114 const V testValues = system.advance(currentValues, testStep, gradient); │ │ │ │ │ -115 const double testError = system.error(testValues); │ │ │ │ │ +70template │ │ │ │ │ +_7_1void _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_S_t_r_e_a_m(std::istream& in_archive_stream, T& output) { │ │ │ │ │ +72 boost::archive::text_iarchive in_archive(in_archive_stream); │ │ │ │ │ +73 in_archive >> output; │ │ │ │ │ +74} │ │ │ │ │ +75 │ │ │ │ │ +77template │ │ │ │ │ +_7_8std::string _s_e_r_i_a_l_i_z_e_T_o_S_t_r_i_n_g(const T& input) { │ │ │ │ │ +79 std::ostringstream out_archive_stream; │ │ │ │ │ +80 _s_e_r_i_a_l_i_z_e_T_o_S_t_r_e_a_m(input, out_archive_stream); │ │ │ │ │ +81 return out_archive_stream.str(); │ │ │ │ │ +82} │ │ │ │ │ +83 │ │ │ │ │ +85template │ │ │ │ │ +_8_6void _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_S_t_r_i_n_g(const std::string& serialized, T& output) { │ │ │ │ │ +87 std::istringstream in_archive_stream(serialized); │ │ │ │ │ +88 _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_S_t_r_e_a_m(in_archive_stream, output); │ │ │ │ │ +89} │ │ │ │ │ +90 │ │ │ │ │ +92template │ │ │ │ │ +_9_3bool _s_e_r_i_a_l_i_z_e_T_o_F_i_l_e(const T& input, const std::string& filename) { │ │ │ │ │ +94 std::ofstream out_archive_stream(filename.c_str()); │ │ │ │ │ +95 if (!out_archive_stream.is_open()) return false; │ │ │ │ │ +96 _s_e_r_i_a_l_i_z_e_T_o_S_t_r_e_a_m(input, out_archive_stream); │ │ │ │ │ +97 out_archive_stream.close(); │ │ │ │ │ +98 return true; │ │ │ │ │ +99} │ │ │ │ │ +100 │ │ │ │ │ +102template │ │ │ │ │ +_1_0_3bool _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_F_i_l_e(const std::string& filename, T& output) { │ │ │ │ │ +104 std::ifstream in_archive_stream(filename.c_str()); │ │ │ │ │ +105 if (!in_archive_stream.is_open()) return false; │ │ │ │ │ +106 _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_S_t_r_e_a_m(in_archive_stream, output); │ │ │ │ │ +107 in_archive_stream.close(); │ │ │ │ │ +108 return true; │ │ │ │ │ +109} │ │ │ │ │ +110 │ │ │ │ │ +112template │ │ │ │ │ +_1_1_3std::string _s_e_r_i_a_l_i_z_e(const T& input) { │ │ │ │ │ +114 return _s_e_r_i_a_l_i_z_e_T_o_S_t_r_i_n_g(input); │ │ │ │ │ +115} │ │ │ │ │ 116 │ │ │ │ │ -117 // update the working range │ │ │ │ │ -118 if (testError >= newError) { │ │ │ │ │ -119 if (flag) │ │ │ │ │ -120 maxStep = testStep; │ │ │ │ │ -121 else │ │ │ │ │ -122 minStep = testStep; │ │ │ │ │ -123 } else { │ │ │ │ │ -124 if (flag) { │ │ │ │ │ -125 minStep = newStep; │ │ │ │ │ -126 newStep = testStep; │ │ │ │ │ -127 newError = testError; │ │ │ │ │ -128 } else { │ │ │ │ │ -129 maxStep = newStep; │ │ │ │ │ -130 newStep = testStep; │ │ │ │ │ -131 newError = testError; │ │ │ │ │ -132 } │ │ │ │ │ -133 } │ │ │ │ │ -134 } │ │ │ │ │ -135 return 0.0; │ │ │ │ │ -136} │ │ │ │ │ -137 │ │ │ │ │ -147template │ │ │ │ │ -_1_4_8boost::tuple _n_o_n_l_i_n_e_a_r_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t(const S &system, │ │ │ │ │ -149 const V &initial, const _N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s ¶ms, │ │ │ │ │ -150 const bool singleIteration, const bool gradientDescent = false) { │ │ │ │ │ -151 │ │ │ │ │ -152 // GTSAM_CONCEPT_MANIFOLD_TYPE(V) │ │ │ │ │ -153 │ │ │ │ │ -154 size_t iteration = 0; │ │ │ │ │ -155 │ │ │ │ │ -156 // check if we're already close enough │ │ │ │ │ -157 double currentError = system.error(initial); │ │ │ │ │ -158 if (currentError <= params._e_r_r_o_r_T_o_l) { │ │ │ │ │ -159 if (params._v_e_r_b_o_s_i_t_y >= NonlinearOptimizerParams::ERROR) { │ │ │ │ │ -160 std::cout << "Exiting, as error = " << currentError << " < " │ │ │ │ │ -161 << params._e_r_r_o_r_T_o_l << std::endl; │ │ │ │ │ -162 } │ │ │ │ │ -163 return boost::tie(initial, iteration); │ │ │ │ │ -164 } │ │ │ │ │ -165 │ │ │ │ │ -166 V currentValues = initial; │ │ │ │ │ -167 typename S::Gradient currentGradient = system.gradient(currentValues), │ │ │ │ │ -168 prevGradient, direction = currentGradient; │ │ │ │ │ -169 │ │ │ │ │ -170 /* do one step of gradient descent */ │ │ │ │ │ -171 V prevValues = currentValues; │ │ │ │ │ -172 double prevError = currentError; │ │ │ │ │ -173 double alpha = _l_i_n_e_S_e_a_r_c_h(system, currentValues, direction); │ │ │ │ │ -174 currentValues = system.advance(prevValues, alpha, direction); │ │ │ │ │ -175 currentError = system.error(currentValues); │ │ │ │ │ -176 │ │ │ │ │ -177 // Maybe show output │ │ │ │ │ -178 if (params._v_e_r_b_o_s_i_t_y >= NonlinearOptimizerParams::ERROR) │ │ │ │ │ -179 std::cout << "Initial error: " << currentError << std::endl; │ │ │ │ │ -180 │ │ │ │ │ -181 // Iterative loop │ │ │ │ │ -182 do { │ │ │ │ │ -183 if (gradientDescent == true) { │ │ │ │ │ -184 direction = system.gradient(currentValues); │ │ │ │ │ -185 } else { │ │ │ │ │ -186 prevGradient = currentGradient; │ │ │ │ │ -187 currentGradient = system.gradient(currentValues); │ │ │ │ │ -188 // Polak-Ribiere: beta = g'*(g_n-g_n-1)/g_n-1'*g_n-1 │ │ │ │ │ -189 const double beta = std::max(0.0, │ │ │ │ │ -190 currentGradient.dot(currentGradient - prevGradient) │ │ │ │ │ -191 / prevGradient.dot(prevGradient)); │ │ │ │ │ -192 direction = currentGradient + (beta * direction); │ │ │ │ │ -193 } │ │ │ │ │ -194 │ │ │ │ │ -195 alpha = _l_i_n_e_S_e_a_r_c_h(system, currentValues, direction); │ │ │ │ │ -196 │ │ │ │ │ -197 prevValues = currentValues; │ │ │ │ │ -198 prevError = currentError; │ │ │ │ │ -199 │ │ │ │ │ -200 currentValues = system.advance(prevValues, alpha, direction); │ │ │ │ │ -201 currentError = system.error(currentValues); │ │ │ │ │ -202 │ │ │ │ │ -203 // User hook: │ │ │ │ │ -204 if (params._i_t_e_r_a_t_i_o_n_H_o_o_k) │ │ │ │ │ -205 params._i_t_e_r_a_t_i_o_n_H_o_o_k(iteration, prevError, currentError); │ │ │ │ │ -206 │ │ │ │ │ -207 // Maybe show output │ │ │ │ │ -208 if (params._v_e_r_b_o_s_i_t_y >= NonlinearOptimizerParams::ERROR) │ │ │ │ │ -209 std::cout << "iteration: " << iteration << ", currentError: " << │ │ │ │ │ -currentError << std::endl; │ │ │ │ │ -210 } while (++iteration < params._m_a_x_I_t_e_r_a_t_i_o_n_s && !singleIteration │ │ │ │ │ -211 && !_c_h_e_c_k_C_o_n_v_e_r_g_e_n_c_e(params._r_e_l_a_t_i_v_e_E_r_r_o_r_T_o_l, params._a_b_s_o_l_u_t_e_E_r_r_o_r_T_o_l, │ │ │ │ │ -212 params._e_r_r_o_r_T_o_l, prevError, currentError, params._v_e_r_b_o_s_i_t_y)); │ │ │ │ │ -213 │ │ │ │ │ -214 // Printing if verbose │ │ │ │ │ -215 if (params._v_e_r_b_o_s_i_t_y >= NonlinearOptimizerParams::ERROR │ │ │ │ │ -216 && iteration >= params._m_a_x_I_t_e_r_a_t_i_o_n_s) │ │ │ │ │ -217 std::cout │ │ │ │ │ -218 << "nonlinearConjugateGradient: Terminating because reached maximum │ │ │ │ │ -iterations" │ │ │ │ │ -219 << std::endl; │ │ │ │ │ -220 │ │ │ │ │ -221 return boost::tie(currentValues, iteration); │ │ │ │ │ -222} │ │ │ │ │ -223 │ │ │ │ │ -224} // \ namespace gtsam │ │ │ │ │ -225 │ │ │ │ │ -_M_a_n_i_f_o_l_d_._h │ │ │ │ │ -Base class and basic functions for Manifold types. │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ -Base class and parameters for nonlinear optimization algorithms. │ │ │ │ │ +118template │ │ │ │ │ +_1_1_9void _d_e_s_e_r_i_a_l_i_z_e(const std::string& serialized, T& output) { │ │ │ │ │ +120 _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_S_t_r_i_n_g(serialized, output); │ │ │ │ │ +121} │ │ │ │ │ +123 │ │ │ │ │ +129template │ │ │ │ │ +_1_3_0void _s_e_r_i_a_l_i_z_e_T_o_X_M_L_S_t_r_e_a_m(const T& input, std::ostream& out_archive_stream, │ │ │ │ │ +131 const std::string& name = "data") { │ │ │ │ │ +132 boost::archive::xml_oarchive out_archive(out_archive_stream); │ │ │ │ │ +133 out_archive << boost::serialization::make_nvp(name.c_str(), input); │ │ │ │ │ +134} │ │ │ │ │ +135 │ │ │ │ │ +137template │ │ │ │ │ +_1_3_8void _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_X_M_L_S_t_r_e_a_m(std::istream& in_archive_stream, T& output, │ │ │ │ │ +139 const std::string& name = "data") { │ │ │ │ │ +140 boost::archive::xml_iarchive in_archive(in_archive_stream); │ │ │ │ │ +141 in_archive >> boost::serialization::make_nvp(name.c_str(), output); │ │ │ │ │ +142} │ │ │ │ │ +143 │ │ │ │ │ +145template │ │ │ │ │ +_1_4_6std::string _s_e_r_i_a_l_i_z_e_T_o_X_M_L_S_t_r_i_n_g(const T& input, │ │ │ │ │ +147 const std::string& name = "data") { │ │ │ │ │ +148 std::ostringstream out_archive_stream; │ │ │ │ │ +149 _s_e_r_i_a_l_i_z_e_T_o_X_M_L_S_t_r_e_a_m(input, out_archive_stream, name); │ │ │ │ │ +150 return out_archive_stream.str(); │ │ │ │ │ +151} │ │ │ │ │ +152 │ │ │ │ │ +154template │ │ │ │ │ +_1_5_5void _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_X_M_L_S_t_r_i_n_g(const std::string& serialized, T& output, │ │ │ │ │ +156 const std::string& name = "data") { │ │ │ │ │ +157 std::istringstream in_archive_stream(serialized); │ │ │ │ │ +158 _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_X_M_L_S_t_r_e_a_m(in_archive_stream, output, name); │ │ │ │ │ +159} │ │ │ │ │ +160 │ │ │ │ │ +162template │ │ │ │ │ +_1_6_3bool _s_e_r_i_a_l_i_z_e_T_o_X_M_L_F_i_l_e(const T& input, const std::string& filename, │ │ │ │ │ +164 const std::string& name = "data") { │ │ │ │ │ +165 std::ofstream out_archive_stream(filename.c_str()); │ │ │ │ │ +166 if (!out_archive_stream.is_open()) return false; │ │ │ │ │ +167 _s_e_r_i_a_l_i_z_e_T_o_X_M_L_S_t_r_e_a_m(input, out_archive_stream, name); │ │ │ │ │ +168 out_archive_stream.close(); │ │ │ │ │ +169 return true; │ │ │ │ │ +170} │ │ │ │ │ +171 │ │ │ │ │ +173template │ │ │ │ │ +_1_7_4bool _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_X_M_L_F_i_l_e(const std::string& filename, T& output, │ │ │ │ │ +175 const std::string& name = "data") { │ │ │ │ │ +176 std::ifstream in_archive_stream(filename.c_str()); │ │ │ │ │ +177 if (!in_archive_stream.is_open()) return false; │ │ │ │ │ +178 _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_X_M_L_S_t_r_e_a_m(in_archive_stream, output, name); │ │ │ │ │ +179 in_archive_stream.close(); │ │ │ │ │ +180 return true; │ │ │ │ │ +181} │ │ │ │ │ +182 │ │ │ │ │ +184template │ │ │ │ │ +_1_8_5std::string _s_e_r_i_a_l_i_z_e_X_M_L(const T& input, │ │ │ │ │ +186 const std::string& name = "data") { │ │ │ │ │ +187 return _s_e_r_i_a_l_i_z_e_T_o_X_M_L_S_t_r_i_n_g(input, name); │ │ │ │ │ +188} │ │ │ │ │ +189 │ │ │ │ │ +191template │ │ │ │ │ +_1_9_2void _d_e_s_e_r_i_a_l_i_z_e_X_M_L(const std::string& serialized, T& output, │ │ │ │ │ +193 const std::string& name = "data") { │ │ │ │ │ +194 _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_X_M_L_S_t_r_i_n_g(serialized, output, name); │ │ │ │ │ +195} │ │ │ │ │ +197 │ │ │ │ │ +203template │ │ │ │ │ +_2_0_4void _s_e_r_i_a_l_i_z_e_T_o_B_i_n_a_r_y_S_t_r_e_a_m(const T& input, std::ostream& │ │ │ │ │ +out_archive_stream, │ │ │ │ │ +205 const std::string& name = "data") { │ │ │ │ │ +206 boost::archive::binary_oarchive out_archive(out_archive_stream); │ │ │ │ │ +207 out_archive << boost::serialization::make_nvp(name.c_str(), input); │ │ │ │ │ +208} │ │ │ │ │ +209 │ │ │ │ │ +211template │ │ │ │ │ +_2_1_2void _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_B_i_n_a_r_y_S_t_r_e_a_m(std::istream& in_archive_stream, T& output, │ │ │ │ │ +213 const std::string& name = "data") { │ │ │ │ │ +214 boost::archive::binary_iarchive in_archive(in_archive_stream); │ │ │ │ │ +215 in_archive >> boost::serialization::make_nvp(name.c_str(), output); │ │ │ │ │ +216} │ │ │ │ │ +217 │ │ │ │ │ +219template │ │ │ │ │ +_2_2_0std::string _s_e_r_i_a_l_i_z_e_T_o_B_i_n_a_r_y_S_t_r_i_n_g(const T& input, │ │ │ │ │ +221 const std::string& name = "data") { │ │ │ │ │ +222 std::ostringstream out_archive_stream; │ │ │ │ │ +223 _s_e_r_i_a_l_i_z_e_T_o_B_i_n_a_r_y_S_t_r_e_a_m(input, out_archive_stream, name); │ │ │ │ │ +224 return out_archive_stream.str(); │ │ │ │ │ +225} │ │ │ │ │ +226 │ │ │ │ │ +228template │ │ │ │ │ +_2_2_9void _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_B_i_n_a_r_y_S_t_r_i_n_g(const std::string& serialized, T& output, │ │ │ │ │ +230 const std::string& name = "data") { │ │ │ │ │ +231 std::istringstream in_archive_stream(serialized); │ │ │ │ │ +232 _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_B_i_n_a_r_y_S_t_r_e_a_m(in_archive_stream, output, name); │ │ │ │ │ +233} │ │ │ │ │ +234 │ │ │ │ │ +236template │ │ │ │ │ +_2_3_7bool _s_e_r_i_a_l_i_z_e_T_o_B_i_n_a_r_y_F_i_l_e(const T& input, const std::string& filename, │ │ │ │ │ +238 const std::string& name = "data") { │ │ │ │ │ +239 std::ofstream out_archive_stream(filename.c_str()); │ │ │ │ │ +240 if (!out_archive_stream.is_open()) return false; │ │ │ │ │ +241 _s_e_r_i_a_l_i_z_e_T_o_B_i_n_a_r_y_S_t_r_e_a_m(input, out_archive_stream, name); │ │ │ │ │ +242 out_archive_stream.close(); │ │ │ │ │ +243 return true; │ │ │ │ │ +244} │ │ │ │ │ +245 │ │ │ │ │ +247template │ │ │ │ │ +_2_4_8bool _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_B_i_n_a_r_y_F_i_l_e(const std::string& filename, T& output, │ │ │ │ │ +249 const std::string& name = "data") { │ │ │ │ │ +250 std::ifstream in_archive_stream(filename.c_str()); │ │ │ │ │ +251 if (!in_archive_stream.is_open()) return false; │ │ │ │ │ +252 _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_B_i_n_a_r_y_S_t_r_e_a_m(in_archive_stream, output, name); │ │ │ │ │ +253 in_archive_stream.close(); │ │ │ │ │ +254 return true; │ │ │ │ │ +255} │ │ │ │ │ +256 │ │ │ │ │ +258template │ │ │ │ │ +_2_5_9std::string _s_e_r_i_a_l_i_z_e_B_i_n_a_r_y(const T& input, │ │ │ │ │ +260 const std::string& name = "data") { │ │ │ │ │ +261 return _s_e_r_i_a_l_i_z_e_T_o_B_i_n_a_r_y_S_t_r_i_n_g(input, name); │ │ │ │ │ +262} │ │ │ │ │ +263 │ │ │ │ │ +265template │ │ │ │ │ +_2_6_6void _d_e_s_e_r_i_a_l_i_z_e_B_i_n_a_r_y(const std::string& serialized, T& output, │ │ │ │ │ +267 const std::string& name = "data") { │ │ │ │ │ +268 _d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_B_i_n_a_r_y_S_t_r_i_n_g(serialized, output, name); │ │ │ │ │ +269} │ │ │ │ │ +271 │ │ │ │ │ +272} // namespace gtsam │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_l_i_n_e_S_e_a_r_c_h │ │ │ │ │ -double lineSearch(const S &system, const V currentValues, const W &gradient) │ │ │ │ │ -Implement the golden-section line search algorithm. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearConjugateGradientOptimizer.h:88 │ │ │ │ │ -_g_t_s_a_m_:_:_c_h_e_c_k_C_o_n_v_e_r_g_e_n_c_e │ │ │ │ │ -bool checkConvergence(double relativeErrorTreshold, double │ │ │ │ │ -absoluteErrorTreshold, double errorThreshold, double currentError, double │ │ │ │ │ -newError, NonlinearOptimizerParams::Verbosity verbosity) │ │ │ │ │ -Check whether the relative error decrease is less than relativeErrorTreshold, │ │ │ │ │ -the absolute error decr... │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizer.cpp:185 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_t_i_m_i_z_e │ │ │ │ │ -Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key │ │ │ │ │ -landmarkKey) │ │ │ │ │ -Optimize for triangulation. │ │ │ │ │ -DDeeffiinniittiioonn triangulation.cpp:155 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_n_l_i_n_e_a_r_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t │ │ │ │ │ -boost::tuple< V, int > nonlinearConjugateGradient(const S &system, const V │ │ │ │ │ -&initial, const NonlinearOptimizerParams ¶ms, const bool singleIteration, │ │ │ │ │ -const bool gradientDescent=false) │ │ │ │ │ -Implement the nonlinear conjugate gradient method using the Polak-Ribiere │ │ │ │ │ -formula suggested in http:/... │ │ │ │ │ -DDeeffiinniittiioonn NonlinearConjugateGradientOptimizer.h:148 │ │ │ │ │ -_g_t_s_a_m_:_:_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn GaussianFactorGraph.h:81 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ -with a unique integer... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_O_p_t_i_m_i_z_e_r │ │ │ │ │ -An implementation of the nonlinear CG method using the template below. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearConjugateGradientOptimizer.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_O_p_t_i_m_i_z_e_r_:_: │ │ │ │ │ -_~_N_o_n_l_i_n_e_a_r_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_O_p_t_i_m_i_z_e_r │ │ │ │ │ -~NonlinearConjugateGradientOptimizer() override │ │ │ │ │ -Destructor. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearConjugateGradientOptimizer.h:70 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r │ │ │ │ │ -This is the abstract interface for classes that can optimize for the maximum- │ │ │ │ │ -likelihood estimate of a... │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizer.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s │ │ │ │ │ -The common parameters for Nonlinear optimizers. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizerParams.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_a_b_s_o_l_u_t_e_E_r_r_o_r_T_o_l │ │ │ │ │ -double absoluteErrorTol │ │ │ │ │ -The maximum absolute error decrease to stop iterating (default 1e-5) │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizerParams.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_i_t_e_r_a_t_i_o_n_H_o_o_k │ │ │ │ │ -IterationHook iterationHook │ │ │ │ │ -Optional user-provided iteration hook to be called after each optimization │ │ │ │ │ -iteration (Default: none). │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizerParams.h:94 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_m_a_x_I_t_e_r_a_t_i_o_n_s │ │ │ │ │ -size_t maxIterations │ │ │ │ │ -The maximum iterations to stop iterating (default 100) │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizerParams.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_v_e_r_b_o_s_i_t_y │ │ │ │ │ -Verbosity verbosity │ │ │ │ │ -The printing verbosity during optimization (default SILENT) │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizerParams.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_r_e_l_a_t_i_v_e_E_r_r_o_r_T_o_l │ │ │ │ │ -double relativeErrorTol │ │ │ │ │ -The maximum relative error decrease to stop iterating (default 1e-5) │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizerParams.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_O_p_t_i_m_i_z_e_r_P_a_r_a_m_s_:_:_e_r_r_o_r_T_o_l │ │ │ │ │ -double errorTol │ │ │ │ │ -The maximum total error to stop iterating (default 0.0) │ │ │ │ │ -DDeeffiinniittiioonn NonlinearOptimizerParams.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_S_t_r_e_a_m │ │ │ │ │ +void serializeToStream(const T &input, std::ostream &out_archive_stream) │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:64 │ │ │ │ │ +_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_X_M_L_F_i_l_e │ │ │ │ │ +bool deserializeFromXMLFile(const std::string &filename, T &output, const std:: │ │ │ │ │ +string &name="data") │ │ │ │ │ +deserializes from an XML file │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:174 │ │ │ │ │ +_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_X_M_L_S_t_r_i_n_g │ │ │ │ │ +std::string serializeToXMLString(const T &input, const std::string │ │ │ │ │ +&name="data") │ │ │ │ │ +serializes to a string in XML │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:146 │ │ │ │ │ +_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_S_t_r_i_n_g │ │ │ │ │ +void deserializeFromString(const std::string &serialized, T &output) │ │ │ │ │ +deserializes from a string │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e │ │ │ │ │ +std::string serialize(const T &input) │ │ │ │ │ +serializes to a string │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:113 │ │ │ │ │ +_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_B_i_n_a_r_y_S_t_r_i_n_g │ │ │ │ │ +void deserializeFromBinaryString(const std::string &serialized, T &output, │ │ │ │ │ +const std::string &name="data") │ │ │ │ │ +deserializes from a string in binary │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:229 │ │ │ │ │ +_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_B_i_n_a_r_y │ │ │ │ │ +void deserializeBinary(const std::string &serialized, T &output, const std:: │ │ │ │ │ +string &name="data") │ │ │ │ │ +deserializes from a string in binary │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:266 │ │ │ │ │ +_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_X_M_L_S_t_r_i_n_g │ │ │ │ │ +void deserializeFromXMLString(const std::string &serialized, T &output, const │ │ │ │ │ +std::string &name="data") │ │ │ │ │ +deserializes from a string in XML │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:155 │ │ │ │ │ +_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_B_i_n_a_r_y_S_t_r_i_n_g │ │ │ │ │ +std::string serializeToBinaryString(const T &input, const std::string │ │ │ │ │ +&name="data") │ │ │ │ │ +serializes to a string in binary │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:220 │ │ │ │ │ +_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_X_M_L │ │ │ │ │ +void deserializeXML(const std::string &serialized, T &output, const std::string │ │ │ │ │ +&name="data") │ │ │ │ │ +deserializes from a string in XML │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:192 │ │ │ │ │ +_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_X_M_L_F_i_l_e │ │ │ │ │ +bool serializeToXMLFile(const T &input, const std::string &filename, const │ │ │ │ │ +std::string &name="data") │ │ │ │ │ +serializes to an XML file │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:163 │ │ │ │ │ +_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_X_M_L_S_t_r_e_a_m │ │ │ │ │ +void deserializeFromXMLStream(std::istream &in_archive_stream, T &output, const │ │ │ │ │ +std::string &name="data") │ │ │ │ │ +deserializes from a stream in XML │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:138 │ │ │ │ │ +_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_B_i_n_a_r_y_S_t_r_e_a_m │ │ │ │ │ +void deserializeFromBinaryStream(std::istream &in_archive_stream, T &output, │ │ │ │ │ +const std::string &name="data") │ │ │ │ │ +deserializes from a stream in binary │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:212 │ │ │ │ │ +_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_X_M_L │ │ │ │ │ +std::string serializeXML(const T &input, const std::string &name="data") │ │ │ │ │ +serializes to a string in XML │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:185 │ │ │ │ │ +_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_S_t_r_i_n_g │ │ │ │ │ +std::string serializeToString(const T &input) │ │ │ │ │ +serializes to a string │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:78 │ │ │ │ │ +_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_F_i_l_e │ │ │ │ │ +bool serializeToFile(const T &input, const std::string &filename) │ │ │ │ │ +serializes to a file │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:93 │ │ │ │ │ +_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e │ │ │ │ │ +void deserialize(const std::string &serialized, T &output) │ │ │ │ │ +deserializes from a string │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:119 │ │ │ │ │ +_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_B_i_n_a_r_y_S_t_r_e_a_m │ │ │ │ │ +void serializeToBinaryStream(const T &input, std::ostream &out_archive_stream, │ │ │ │ │ +const std::string &name="data") │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:204 │ │ │ │ │ +_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_S_t_r_e_a_m │ │ │ │ │ +void deserializeFromStream(std::istream &in_archive_stream, T &output) │ │ │ │ │ +deserializes from a stream │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:71 │ │ │ │ │ +_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_B_i_n_a_r_y │ │ │ │ │ +std::string serializeBinary(const T &input, const std::string &name="data") │ │ │ │ │ +serializes to a string in binary │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:259 │ │ │ │ │ +_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_B_i_n_a_r_y_F_i_l_e │ │ │ │ │ +bool serializeToBinaryFile(const T &input, const std::string &filename, const │ │ │ │ │ +std::string &name="data") │ │ │ │ │ +serializes to a binary file │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:237 │ │ │ │ │ +_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_F_i_l_e │ │ │ │ │ +bool deserializeFromFile(const std::string &filename, T &output) │ │ │ │ │ +deserializes from a file │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:103 │ │ │ │ │ +_g_t_s_a_m_:_:_s_e_r_i_a_l_i_z_e_T_o_X_M_L_S_t_r_e_a_m │ │ │ │ │ +void serializeToXMLStream(const T &input, std::ostream &out_archive_stream, │ │ │ │ │ +const std::string &name="data") │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:130 │ │ │ │ │ +_g_t_s_a_m_:_:_d_e_s_e_r_i_a_l_i_z_e_F_r_o_m_B_i_n_a_r_y_F_i_l_e │ │ │ │ │ +bool deserializeFromBinaryFile(const std::string &filename, T &output, const │ │ │ │ │ +std::string &name="data") │ │ │ │ │ +deserializes from a binary file │ │ │ │ │ +DDeeffiinniittiioonn serialization.h:248 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _N_o_n_l_i_n_e_a_r_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ + * _b_a_s_e │ │ │ │ │ + * _s_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01172.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00038.html │ │ │ │┄ Files 90% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/LinearContainerFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/base/FastMap.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces
│ │ │ │ -
LinearContainerFactor.h File Reference
│ │ │ │ +
FastMap.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph. │ │ │ │ +

A thin wrapper around std::map that uses boost's fast_pool_allocator. │ │ │ │ More...

│ │ │ │ │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

class  gtsam::LinearContainerFactor
 Dummy version of a generic linear factor to be injected into a nonlinear factor graph. More...
 
struct  gtsam::traits< LinearContainerFactor >
class  gtsam::FastMap< KEY, VALUE >
 FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the default STL allocator. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Wrap Jacobian and Hessian linear factors to allow simple injection into a nonlinear graph.

│ │ │ │ -
Date
Jul 6, 2012
│ │ │ │ -
Author
Alex Cunningham
│ │ │ │ +

A thin wrapper around std::map that uses boost's fast_pool_allocator.

│ │ │ │ +
Author
Richard Roberts
│ │ │ │ +
Date
Oct 17, 2010
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,32 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ _C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -LinearContainerFactor.h File Reference │ │ │ │ │ -Wrap Jacobian and Hessian linear factors to allow simple injection into a │ │ │ │ │ -nonlinear graph. _M_o_r_e_._._. │ │ │ │ │ +FastMap.h File Reference │ │ │ │ │ +A thin wrapper around std::map that uses boost's fast_pool_allocator. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ - class   _g_t_s_a_m_:_:_L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r │ │ │ │ │ -  Dummy version of a generic linear factor to be injected into a │ │ │ │ │ - nonlinear factor graph. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r_ _> │ │ │ │ │ +class   _g_t_s_a_m_:_:_F_a_s_t_M_a_p_<_ _K_E_Y_,_ _V_A_L_U_E_ _> │ │ │ │ │ +  _F_a_s_t_M_a_p is a thin wrapper around std::map that uses the boost │ │ │ │ │ + fast_pool_allocator instead of the default STL allocator. _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Wrap Jacobian and Hessian linear factors to allow simple injection into a │ │ │ │ │ -nonlinear graph. │ │ │ │ │ - Date │ │ │ │ │ - Jul 6, 2012 │ │ │ │ │ +A thin wrapper around std::map that uses boost's fast_pool_allocator. │ │ │ │ │ Author │ │ │ │ │ - Alex Cunningham │ │ │ │ │ + Richard Roberts │ │ │ │ │ + Date │ │ │ │ │ + Oct 17, 2010 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _n_o_n_l_i_n_e_a_r │ │ │ │ │ - * _L_i_n_e_a_r_C_o_n_t_a_i_n_e_r_F_a_c_t_o_r_._h │ │ │ │ │ + * _b_a_s_e │ │ │ │ │ + * _F_a_s_t_M_a_p_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01187_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00806_source.html │ │ │ │┄ Files 85% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/TranslationRecovery.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/Errors.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
TranslationRecovery.h
│ │ │ │ +
Errors.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#include <gtsam/geometry/Unit3.h>
│ │ │ │ -
20#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
│ │ │ │ -
21#include <gtsam/nonlinear/Values.h>
│ │ │ │ -
22#include <gtsam/sfm/BinaryMeasurement.h>
│ │ │ │ -
23
│ │ │ │ -
24#include <map>
│ │ │ │ -
25#include <set>
│ │ │ │ -
26#include <utility>
│ │ │ │ -
27#include <vector>
│ │ │ │ -
28
│ │ │ │ -
29namespace gtsam {
│ │ │ │ -
30
│ │ │ │ -
31// Set up an optimization problem for the unknown translations Ti in the world
│ │ │ │ -
32// coordinate frame, given the known camera attitudes wRi with respect to the
│ │ │ │ -
33// world frame, and a set of (noisy) translation directions of type Unit3,
│ │ │ │ -
34// w_aZb. The measurement equation is
│ │ │ │ -
35// w_aZb = Unit3(Tb - Ta) (1)
│ │ │ │ -
36// i.e., w_aZb is the translation direction from frame A to B, in world
│ │ │ │ -
37// coordinates. Although Unit3 instances live on a manifold, following
│ │ │ │ -
38// Wilson14eccv_1DSfM.pdf error we compute the *chordal distance* in the
│ │ │ │ -
39// ambient world coordinate frame.
│ │ │ │ -
40//
│ │ │ │ -
41// It is clear that we cannot recover the scale, nor the absolute position,
│ │ │ │ -
42// so the gauge freedom in this case is 3 + 1 = 4. We fix these by taking fixing
│ │ │ │ -
43// the translations Ta and Tb associated with the first measurement w_aZb,
│ │ │ │ -
44// clamping them to their initial values as given to this method. If no initial
│ │ │ │ -
45// values are given, we use the origin for Tb and set Tb to make (1) come
│ │ │ │ -
46// through, i.e.,
│ │ │ │ -
47// Tb = s * wRa * Point3(w_aZb) (2)
│ │ │ │ -
48// where s is an arbitrary scale that can be supplied, default 1.0. Hence, two
│ │ │ │ -
49// versions are supplied below corresponding to whether we have initial values
│ │ │ │ -
50// or not.
│ │ │ │ -
│ │ │ │ -
51class TranslationRecovery {
│ │ │ │ -
52 public:
│ │ │ │ -
53 using KeyPair = std::pair<Key, Key>;
│ │ │ │ -
54 using TranslationEdges = std::vector<BinaryMeasurement<Unit3>>;
│ │ │ │ -
55
│ │ │ │ -
56 private:
│ │ │ │ -
57 // Translation directions between camera pairs.
│ │ │ │ -
58 TranslationEdges relativeTranslations_;
│ │ │ │ -
59
│ │ │ │ -
60 // Parameters.
│ │ │ │ -
61 LevenbergMarquardtParams lmParams_;
│ │ │ │ -
62
│ │ │ │ -
63 public:
│ │ │ │ -
│ │ │ │ -
69 TranslationRecovery(const LevenbergMarquardtParams &lmParams)
│ │ │ │ -
70 : lmParams_(lmParams) {}
│ │ │ │ -
│ │ │ │ -
71
│ │ │ │ -
75 TranslationRecovery() = default;
│ │ │ │ -
76
│ │ │ │ -
84 NonlinearFactorGraph buildGraph(
│ │ │ │ -
85 const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations) const;
│ │ │ │ -
86
│ │ │ │ -
102 void addPrior(
│ │ │ │ -
103 const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
│ │ │ │ -
104 const double scale,
│ │ │ │ -
105 const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
│ │ │ │ -
106 NonlinearFactorGraph *graph,
│ │ │ │ -
107 const SharedNoiseModel &priorNoiseModel =
│ │ │ │ -
108 noiseModel::Isotropic::Sigma(3, 0.01)) const;
│ │ │ │ -
109
│ │ │ │ -
121 Values initializeRandomly(
│ │ │ │ -
122 const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
│ │ │ │ -
123 const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
│ │ │ │ -
124 std::mt19937 *rng, const Values &initialValues = Values()) const;
│ │ │ │ -
125
│ │ │ │ -
136 Values initializeRandomly(
│ │ │ │ -
137 const std::vector<BinaryMeasurement<Unit3>> &relativeTranslations,
│ │ │ │ -
138 const std::vector<BinaryMeasurement<Point3>> &betweenTranslations,
│ │ │ │ -
139 const Values &initialValues = Values()) const;
│ │ │ │ -
140
│ │ │ │ -
159 Values run(
│ │ │ │ -
160 const TranslationEdges &relativeTranslations, const double scale = 1.0,
│ │ │ │ -
161 const std::vector<BinaryMeasurement<Point3>> &betweenTranslations = {},
│ │ │ │ -
162 const Values &initialValues = Values()) const;
│ │ │ │ -
163
│ │ │ │ -
173 static TranslationEdges SimulateMeasurements(
│ │ │ │ -
174 const Values &poses, const std::vector<KeyPair> &edges);
│ │ │ │ -
175};
│ │ │ │ -
│ │ │ │ -
176} // namespace gtsam
│ │ │ │ -
Values.h
A non-templated config holding any types of Manifold-group elements.
│ │ │ │ -
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
│ │ │ │ -
BinaryMeasurement.h
Binary measurement represents a measurement between two keys in a graph. A binary measurement is simi...
│ │ │ │ +
18// \callgraph
│ │ │ │ +
19
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/base/FastList.h>
│ │ │ │ +
23#include <gtsam/base/Testable.h>
│ │ │ │ +
24#include <gtsam/base/Vector.h>
│ │ │ │ +
25
│ │ │ │ +
26#include <string>
│ │ │ │ +
27
│ │ │ │ +
28namespace gtsam {
│ │ │ │ +
29
│ │ │ │ +
30// Forward declarations
│ │ │ │ +
31class VectorValues;
│ │ │ │ +
32
│ │ │ │ +
34using Errors = FastList<Vector>;
│ │ │ │ +
35
│ │ │ │ +
37GTSAM_EXPORT Errors createErrors(const VectorValues& V);
│ │ │ │ +
38
│ │ │ │ +
40GTSAM_EXPORT void print(const Errors& e, const std::string& s = "Errors");
│ │ │ │ +
41
│ │ │ │ +
42// Check equality for unit testing.
│ │ │ │ +
43GTSAM_EXPORT bool equality(const Errors& actual, const Errors& expected,
│ │ │ │ +
44 double tol = 1e-9);
│ │ │ │ +
45
│ │ │ │ +
47GTSAM_EXPORT Errors operator+(const Errors& a, const Errors& b);
│ │ │ │ +
48
│ │ │ │ +
50GTSAM_EXPORT Errors operator-(const Errors& a, const Errors& b);
│ │ │ │ +
51
│ │ │ │ +
53GTSAM_EXPORT Errors operator-(const Errors& a);
│ │ │ │ +
54
│ │ │ │ +
56GTSAM_EXPORT double dot(const Errors& a, const Errors& b);
│ │ │ │ +
57
│ │ │ │ +
59GTSAM_EXPORT void axpy(double alpha, const Errors& x, Errors& y);
│ │ │ │ +
60
│ │ │ │ +
62template <>
│ │ │ │ +
│ │ │ │ +
63struct traits<Errors> {
│ │ │ │ +
64 static void Print(const Errors& e, const std::string& str = "") {
│ │ │ │ +
65 print(e, str);
│ │ │ │ +
66 }
│ │ │ │ +
67 static bool Equals(const Errors& actual, const Errors& expected,
│ │ │ │ +
68 double tol = 1e-8) {
│ │ │ │ +
69 return equality(actual, expected, tol);
│ │ │ │ +
70 }
│ │ │ │ +
71};
│ │ │ │ +
│ │ │ │ +
72
│ │ │ │ +
73} // namespace gtsam
│ │ │ │ +
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ +
FastList.h
A thin wrapper around std::list that uses boost's fast_pool_allocator.
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ -
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
An isotropic noise model created by specifying a standard devation sigma.
Definition NoiseModel.cpp:597
│ │ │ │ -
gtsam::LevenbergMarquardtParams
Parameters for Levenberg-Marquardt optimization.
Definition LevenbergMarquardtParams.h:35
│ │ │ │ -
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │ -
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
gtsam::BinaryMeasurement
Definition BinaryMeasurement.h:36
│ │ │ │ -
gtsam::TranslationRecovery
Definition TranslationRecovery.h:51
│ │ │ │ -
gtsam::TranslationRecovery::buildGraph
NonlinearFactorGraph buildGraph(const std::vector< BinaryMeasurement< Unit3 > > &relativeTranslations) const
Build the factor graph to do the optimization.
Definition TranslationRecovery.cpp:99
│ │ │ │ -
gtsam::TranslationRecovery::SimulateMeasurements
static TranslationEdges SimulateMeasurements(const Values &poses, const std::vector< KeyPair > &edges)
Simulate translation direction measurements.
Definition TranslationRecovery.cpp:216
│ │ │ │ -
gtsam::TranslationRecovery::TranslationRecovery
TranslationRecovery()=default
Default constructor.
│ │ │ │ -
gtsam::TranslationRecovery::run
Values run(const TranslationEdges &relativeTranslations, const double scale=1.0, const std::vector< BinaryMeasurement< Point3 > > &betweenTranslations={}, const Values &initialValues=Values()) const
Build and optimize factor graph.
Definition TranslationRecovery.cpp:177
│ │ │ │ -
gtsam::TranslationRecovery::addPrior
void addPrior(const std::vector< BinaryMeasurement< Unit3 > > &relativeTranslations, const double scale, const std::vector< BinaryMeasurement< Point3 > > &betweenTranslations, NonlinearFactorGraph *graph, const SharedNoiseModel &priorNoiseModel=noiseModel::Isotropic::Sigma(3, 0.01)) const
Add 3 factors to the graph:
Definition TranslationRecovery.cpp:111
│ │ │ │ -
gtsam::TranslationRecovery::TranslationRecovery
TranslationRecovery(const LevenbergMarquardtParams &lmParams)
Construct a new Translation Recovery object.
Definition TranslationRecovery.h:69
│ │ │ │ -
gtsam::TranslationRecovery::initializeRandomly
Values initializeRandomly(const std::vector< BinaryMeasurement< Unit3 > > &relativeTranslations, const std::vector< BinaryMeasurement< Point3 > > &betweenTranslations, std::mt19937 *rng, const Values &initialValues=Values()) const
Create random initial translations.
Definition TranslationRecovery.cpp:137
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::operator+
Errors operator+(const Errors &a, const Errors &b)
Addition.
Definition Errors.cpp:60
│ │ │ │ +
gtsam::axpy
void axpy(double alpha, const Errors &x, Errors &y)
BLAS level 2 style AXPY, y := alpha*x + y
Definition Errors.cpp:111
│ │ │ │ +
gtsam::createErrors
Errors createErrors(const VectorValues &V)
Break V into pieces according to its start indices.
Definition Errors.cpp:29
│ │ │ │ +
gtsam::dot
double dot(const V1 &a, const V2 &b)
Dot product.
Definition Vector.h:195
│ │ │ │ +
gtsam::operator-
Errors operator-(const Errors &a, const Errors &b)
Subtraction.
Definition Errors.cpp:75
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ + │ │ │ │ +
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
VectorValues
The Factor::error simply extracts the.
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,177 +1,115 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -TranslationRecovery.h │ │ │ │ │ +Errors.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#include │ │ │ │ │ -20#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_V_a_l_u_e_s_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_s_f_m_/_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_._h> │ │ │ │ │ -23 │ │ │ │ │ -24#include │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ -28 │ │ │ │ │ -29namespace _g_t_s_a_m { │ │ │ │ │ -30 │ │ │ │ │ -31// Set up an optimization problem for the unknown translations Ti in the │ │ │ │ │ -world │ │ │ │ │ -32// coordinate frame, given the known camera attitudes wRi with respect to the │ │ │ │ │ -33// world frame, and a set of (noisy) translation directions of type Unit3, │ │ │ │ │ -34// w_aZb. The measurement equation is │ │ │ │ │ -35// w_aZb = Unit3(Tb - Ta) (1) │ │ │ │ │ -36// i.e., w_aZb is the translation direction from frame A to B, in world │ │ │ │ │ -37// coordinates. Although Unit3 instances live on a manifold, following │ │ │ │ │ -38// Wilson14eccv_1DSfM.pdf error we compute the *chordal distance* in the │ │ │ │ │ -39// ambient world coordinate frame. │ │ │ │ │ -40// │ │ │ │ │ -41// It is clear that we cannot recover the scale, nor the absolute position, │ │ │ │ │ -42// so the gauge freedom in this case is 3 + 1 = 4. We fix these by taking │ │ │ │ │ -fixing │ │ │ │ │ -43// the translations Ta and Tb associated with the first measurement w_aZb, │ │ │ │ │ -44// clamping them to their initial values as given to this method. If no │ │ │ │ │ -initial │ │ │ │ │ -45// values are given, we use the origin for Tb and set Tb to make (1) come │ │ │ │ │ -46// through, i.e., │ │ │ │ │ -47// Tb = s * wRa * Point3(w_aZb) (2) │ │ │ │ │ -48// where s is an arbitrary scale that can be supplied, default 1.0. Hence, │ │ │ │ │ -two │ │ │ │ │ -49// versions are supplied below corresponding to whether we have initial │ │ │ │ │ -values │ │ │ │ │ -50// or not. │ │ │ │ │ -_5_1class _T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y { │ │ │ │ │ -52 public: │ │ │ │ │ -53 using KeyPair = std::pair; │ │ │ │ │ -54 using TranslationEdges = std::vector>; │ │ │ │ │ -55 │ │ │ │ │ -56 private: │ │ │ │ │ -57 // Translation directions between camera pairs. │ │ │ │ │ -58 TranslationEdges relativeTranslations_; │ │ │ │ │ -59 │ │ │ │ │ -60 // Parameters. │ │ │ │ │ -61 _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s lmParams_; │ │ │ │ │ -62 │ │ │ │ │ -63 public: │ │ │ │ │ -_6_9 _T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y(const _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s &lmParams) │ │ │ │ │ -70 : lmParams_(lmParams) {} │ │ │ │ │ -71 │ │ │ │ │ -_7_5 _T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y() = default; │ │ │ │ │ -76 │ │ │ │ │ -84 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h _b_u_i_l_d_G_r_a_p_h( │ │ │ │ │ -85 const std::vector<_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_<_U_n_i_t_3_>> &relativeTranslations) const; │ │ │ │ │ -86 │ │ │ │ │ -102 void _a_d_d_P_r_i_o_r( │ │ │ │ │ -103 const std::vector<_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_<_U_n_i_t_3_>> &relativeTranslations, │ │ │ │ │ -104 const double scale, │ │ │ │ │ -105 const std::vector<_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_<_P_o_i_n_t_3_>> &betweenTranslations, │ │ │ │ │ -106 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h *graph, │ │ │ │ │ -107 const _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l &priorNoiseModel = │ │ │ │ │ -108 _n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c_:_:_S_i_g_m_a(3, 0.01)) const; │ │ │ │ │ -109 │ │ │ │ │ -121 _V_a_l_u_e_s _i_n_i_t_i_a_l_i_z_e_R_a_n_d_o_m_l_y( │ │ │ │ │ -122 const std::vector<_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_<_U_n_i_t_3_>> &relativeTranslations, │ │ │ │ │ -123 const std::vector<_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_<_P_o_i_n_t_3_>> &betweenTranslations, │ │ │ │ │ -124 std::mt19937 *rng, const _V_a_l_u_e_s &initialValues = _V_a_l_u_e_s()) const; │ │ │ │ │ -125 │ │ │ │ │ -136 _V_a_l_u_e_s _i_n_i_t_i_a_l_i_z_e_R_a_n_d_o_m_l_y( │ │ │ │ │ -137 const std::vector<_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_<_U_n_i_t_3_>> &relativeTranslations, │ │ │ │ │ -138 const std::vector<_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_<_P_o_i_n_t_3_>> &betweenTranslations, │ │ │ │ │ -139 const _V_a_l_u_e_s &initialValues = _V_a_l_u_e_s()) const; │ │ │ │ │ -140 │ │ │ │ │ -159 _V_a_l_u_e_s _r_u_n( │ │ │ │ │ -160 const TranslationEdges &relativeTranslations, const double scale = 1.0, │ │ │ │ │ -161 const std::vector<_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_<_P_o_i_n_t_3_>> &betweenTranslations = {}, │ │ │ │ │ -162 const _V_a_l_u_e_s &initialValues = _V_a_l_u_e_s()) const; │ │ │ │ │ -163 │ │ │ │ │ -173 static TranslationEdges _S_i_m_u_l_a_t_e_M_e_a_s_u_r_e_m_e_n_t_s( │ │ │ │ │ -174 const _V_a_l_u_e_s &poses, const std::vector &edges); │ │ │ │ │ -175}; │ │ │ │ │ -176} // namespace gtsam │ │ │ │ │ -_V_a_l_u_e_s_._h │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ -_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_O_p_t_i_m_i_z_e_r_._h │ │ │ │ │ -A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme. │ │ │ │ │ -_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_._h │ │ │ │ │ -Binary measurement represents a measurement between two keys in a graph. A │ │ │ │ │ -binary measurement is simi... │ │ │ │ │ +18// \callgraph │ │ │ │ │ +19 │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_L_i_s_t_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_c_t_o_r_._h> │ │ │ │ │ +25 │ │ │ │ │ +26#include │ │ │ │ │ +27 │ │ │ │ │ +28namespace _g_t_s_a_m { │ │ │ │ │ +29 │ │ │ │ │ +30// Forward declarations │ │ │ │ │ +31class _V_e_c_t_o_r_V_a_l_u_e_s; │ │ │ │ │ +32 │ │ │ │ │ +_3_4using _E_r_r_o_r_s = _F_a_s_t_L_i_s_t_<_V_e_c_t_o_r_>; │ │ │ │ │ +35 │ │ │ │ │ +37GTSAM_EXPORT _E_r_r_o_r_s _c_r_e_a_t_e_E_r_r_o_r_s(const _V_e_c_t_o_r_V_a_l_u_e_s& V); │ │ │ │ │ +38 │ │ │ │ │ +40GTSAM_EXPORT void _p_r_i_n_t(const _E_r_r_o_r_s& e, const std::string& s = "Errors"); │ │ │ │ │ +41 │ │ │ │ │ +42// Check equality for unit testing. │ │ │ │ │ +43GTSAM_EXPORT bool equality(const _E_r_r_o_r_s& actual, const _E_r_r_o_r_s& expected, │ │ │ │ │ +44 double tol = 1e-9); │ │ │ │ │ +45 │ │ │ │ │ +47GTSAM_EXPORT _E_r_r_o_r_s _o_p_e_r_a_t_o_r_+(const _E_r_r_o_r_s& a, const _E_r_r_o_r_s& b); │ │ │ │ │ +48 │ │ │ │ │ +50GTSAM_EXPORT _E_r_r_o_r_s _o_p_e_r_a_t_o_r_-(const _E_r_r_o_r_s& a, const _E_r_r_o_r_s& b); │ │ │ │ │ +51 │ │ │ │ │ +53GTSAM_EXPORT _E_r_r_o_r_s _o_p_e_r_a_t_o_r_-(const _E_r_r_o_r_s& a); │ │ │ │ │ +54 │ │ │ │ │ +56GTSAM_EXPORT double _d_o_t(const _E_r_r_o_r_s& a, const _E_r_r_o_r_s& b); │ │ │ │ │ +57 │ │ │ │ │ +59GTSAM_EXPORT void _a_x_p_y(double alpha, const _E_r_r_o_r_s& x, _E_r_r_o_r_s& y); │ │ │ │ │ +60 │ │ │ │ │ +62template <> │ │ │ │ │ +_6_3struct _t_r_a_i_t_s<_E_r_r_o_r_s> { │ │ │ │ │ +64 static void Print(const _E_r_r_o_r_s& e, const std::string& str = "") { │ │ │ │ │ +65 _p_r_i_n_t(e, str); │ │ │ │ │ +66 } │ │ │ │ │ +67 static bool Equals(const _E_r_r_o_r_s& actual, const _E_r_r_o_r_s& expected, │ │ │ │ │ +68 double tol = 1e-8) { │ │ │ │ │ +69 return equality(actual, expected, tol); │ │ │ │ │ +70 } │ │ │ │ │ +71}; │ │ │ │ │ +72 │ │ │ │ │ +73} // namespace gtsam │ │ │ │ │ +_V_e_c_t_o_r_._h │ │ │ │ │ +typedef and functions to augment Eigen's VectorXd │ │ │ │ │ +_F_a_s_t_L_i_s_t_._h │ │ │ │ │ +A thin wrapper around std::list that uses boost's fast_pool_allocator. │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ -Aliases. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ -_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_I_s_o_t_r_o_p_i_c_:_:_S_i_g_m_a │ │ │ │ │ -static shared_ptr Sigma(size_t dim, double sigma, bool smart=true) │ │ │ │ │ -An isotropic noise model created by specifying a standard devation sigma. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.cpp:597 │ │ │ │ │ -_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s │ │ │ │ │ -Parameters for Levenberg-Marquardt optimization. │ │ │ │ │ -DDeeffiinniittiioonn LevenbergMarquardtParams.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t │ │ │ │ │ -DDeeffiinniittiioonn BinaryMeasurement.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y │ │ │ │ │ -DDeeffiinniittiioonn TranslationRecovery.h:51 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y_:_:_b_u_i_l_d_G_r_a_p_h │ │ │ │ │ -NonlinearFactorGraph buildGraph(const std::vector< BinaryMeasurement< Unit3 > > │ │ │ │ │ -&relativeTranslations) const │ │ │ │ │ -Build the factor graph to do the optimization. │ │ │ │ │ -DDeeffiinniittiioonn TranslationRecovery.cpp:99 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y_:_:_S_i_m_u_l_a_t_e_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ -static TranslationEdges SimulateMeasurements(const Values &poses, const std:: │ │ │ │ │ -vector< KeyPair > &edges) │ │ │ │ │ -Simulate translation direction measurements. │ │ │ │ │ -DDeeffiinniittiioonn TranslationRecovery.cpp:216 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y_:_:_T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y │ │ │ │ │ -TranslationRecovery()=default │ │ │ │ │ -Default constructor. │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y_:_:_r_u_n │ │ │ │ │ -Values run(const TranslationEdges &relativeTranslations, const double │ │ │ │ │ -scale=1.0, const std::vector< BinaryMeasurement< Point3 > > │ │ │ │ │ -&betweenTranslations={}, const Values &initialValues=Values()) const │ │ │ │ │ -Build and optimize factor graph. │ │ │ │ │ -DDeeffiinniittiioonn TranslationRecovery.cpp:177 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y_:_:_a_d_d_P_r_i_o_r │ │ │ │ │ -void addPrior(const std::vector< BinaryMeasurement< Unit3 > > │ │ │ │ │ -&relativeTranslations, const double scale, const std::vector< │ │ │ │ │ -BinaryMeasurement< Point3 > > &betweenTranslations, NonlinearFactorGraph │ │ │ │ │ -*graph, const SharedNoiseModel &priorNoiseModel=noiseModel::Isotropic::Sigma(3, │ │ │ │ │ -0.01)) const │ │ │ │ │ -Add 3 factors to the graph: │ │ │ │ │ -DDeeffiinniittiioonn TranslationRecovery.cpp:111 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y_:_:_T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y │ │ │ │ │ -TranslationRecovery(const LevenbergMarquardtParams &lmParams) │ │ │ │ │ -Construct a new Translation Recovery object. │ │ │ │ │ -DDeeffiinniittiioonn TranslationRecovery.h:69 │ │ │ │ │ -_g_t_s_a_m_:_:_T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y_:_:_i_n_i_t_i_a_l_i_z_e_R_a_n_d_o_m_l_y │ │ │ │ │ -Values initializeRandomly(const std::vector< BinaryMeasurement< Unit3 > > │ │ │ │ │ -&relativeTranslations, const std::vector< BinaryMeasurement< Point3 > > │ │ │ │ │ -&betweenTranslations, std::mt19937 *rng, const Values &initialValues=Values()) │ │ │ │ │ -const │ │ │ │ │ -Create random initial translations. │ │ │ │ │ -DDeeffiinniittiioonn TranslationRecovery.cpp:137 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_+ │ │ │ │ │ +Errors operator+(const Errors &a, const Errors &b) │ │ │ │ │ +Addition. │ │ │ │ │ +DDeeffiinniittiioonn Errors.cpp:60 │ │ │ │ │ +_g_t_s_a_m_:_:_a_x_p_y │ │ │ │ │ +void axpy(double alpha, const Errors &x, Errors &y) │ │ │ │ │ +BLAS level 2 style AXPY, y := alpha*x + y │ │ │ │ │ +DDeeffiinniittiioonn Errors.cpp:111 │ │ │ │ │ +_g_t_s_a_m_:_:_c_r_e_a_t_e_E_r_r_o_r_s │ │ │ │ │ +Errors createErrors(const VectorValues &V) │ │ │ │ │ +Break V into pieces according to its start indices. │ │ │ │ │ +DDeeffiinniittiioonn Errors.cpp:29 │ │ │ │ │ +_g_t_s_a_m_:_:_d_o_t │ │ │ │ │ +double dot(const V1 &a, const V2 &b) │ │ │ │ │ +Dot product. │ │ │ │ │ +DDeeffiinniittiioonn Vector.h:195 │ │ │ │ │ +_g_t_s_a_m_:_:_o_p_e_r_a_t_o_r_- │ │ │ │ │ +Errors operator-(const Errors &a, const Errors &b) │ │ │ │ │ +Subtraction. │ │ │ │ │ +DDeeffiinniittiioonn Errors.cpp:75 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_s_t_L_i_s_t_<_ _V_e_c_t_o_r_ _> │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ +with a unique integer... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +The Factor::error simply extracts the. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _s_f_m │ │ │ │ │ - * _T_r_a_n_s_l_a_t_i_o_n_R_e_c_o_v_e_r_y_._h │ │ │ │ │ + * _l_i_n_e_a_r │ │ │ │ │ + * _E_r_r_o_r_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01199_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01331_source.html │ │ │ │┄ Files 88% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/MFAS.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/KarcherMeanFactor.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
MFAS.h
│ │ │ │ +
KarcherMeanFactor.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
12#pragma once
│ │ │ │ -
13
│ │ │ │ -
21#include <gtsam/geometry/Unit3.h>
│ │ │ │ -
22#include <gtsam/inference/Key.h>
│ │ │ │ -
23#include <gtsam/sfm/BinaryMeasurement.h>
│ │ │ │ -
24
│ │ │ │ -
25#include <memory>
│ │ │ │ -
26#include <unordered_map>
│ │ │ │ -
27#include <vector>
│ │ │ │ -
28
│ │ │ │ -
29namespace gtsam {
│ │ │ │ -
30
│ │ │ │ -
│ │ │ │ -
51class GTSAM_EXPORT MFAS {
│ │ │ │ -
52 public:
│ │ │ │ -
53 // used to represent edges between two nodes in the graph. When used in
│ │ │ │ -
54 // translation averaging for global SfM
│ │ │ │ -
55 using KeyPair = std::pair<Key, Key>;
│ │ │ │ -
56 using TranslationEdges = std::vector<BinaryMeasurement<Unit3>>;
│ │ │ │ -
57
│ │ │ │ -
58 private:
│ │ │ │ -
59 // edges with a direction such that all weights are positive
│ │ │ │ -
60 // i.e, edges that originally had negative weights are flipped
│ │ │ │ -
61 std::map<KeyPair, double> edgeWeights_;
│ │ │ │ -
62
│ │ │ │ -
63 public:
│ │ │ │ -
│ │ │ │ -
69 MFAS(const std::map<KeyPair, double> &edgeWeights)
│ │ │ │ -
70 : edgeWeights_(edgeWeights) {}
│ │ │ │ -
│ │ │ │ -
71
│ │ │ │ -
80 MFAS(const TranslationEdges &relativeTranslations,
│ │ │ │ -
81 const Unit3 &projectionDirection);
│ │ │ │ -
82
│ │ │ │ -
87 KeyVector computeOrdering() const;
│ │ │ │ -
88
│ │ │ │ -
96 std::map<KeyPair, double> computeOutlierWeights() const;
│ │ │ │ -
97};
│ │ │ │ -
│ │ │ │ -
98
│ │ │ │ -
99typedef std::map<std::pair<Key, Key>, double> KeyPairDoubleMap;
│ │ │ │ -
100
│ │ │ │ -
101} // namespace gtsam
│ │ │ │ - │ │ │ │ -
BinaryMeasurement.h
Binary measurement represents a measurement between two keys in a graph. A binary measurement is simi...
│ │ │ │ +
12/*
│ │ │ │ +
13 * @file KarcherMeanFactor.h
│ │ │ │ +
14 * @author Frank Dellaert
│ │ │ │ +
15 * @date March 2019
│ │ │ │ +
16 */
│ │ │ │ +
17
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/base/Matrix.h>
│ │ │ │ +
21#include <gtsam/linear/JacobianFactor.h>
│ │ │ │ +
22
│ │ │ │ +
23#include <map>
│ │ │ │ +
24#include <vector>
│ │ │ │ +
25
│ │ │ │ +
26namespace gtsam {
│ │ │ │ +
32template <class T>
│ │ │ │ +
33T FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>> &rotations);
│ │ │ │ +
34
│ │ │ │ +
35template <class T> T FindKarcherMean(std::initializer_list<T> &&rotations);
│ │ │ │ +
36
│ │ │ │ +
│ │ │ │ +
45template <class T> class KarcherMeanFactor : public NonlinearFactor {
│ │ │ │ +
46 // Compile time dimension: can be -1
│ │ │ │ +
47 enum { D = traits<T>::dimension };
│ │ │ │ +
48
│ │ │ │ +
49 // Runtime dimension: always >=0
│ │ │ │ +
50 size_t d_;
│ │ │ │ +
51
│ │ │ │ +
53 boost::shared_ptr<JacobianFactor> whitenedJacobian_;
│ │ │ │ +
54
│ │ │ │ +
55public:
│ │ │ │ +
61 template <typename CONTAINER>
│ │ │ │ +
62 KarcherMeanFactor(const CONTAINER &keys, int d = D,
│ │ │ │ +
63 boost::optional<double> beta = boost::none);
│ │ │ │ +
64
│ │ │ │ +
66 ~KarcherMeanFactor() override {}
│ │ │ │ +
67
│ │ │ │ +
69 double error(const Values &c) const override { return 0; }
│ │ │ │ +
70
│ │ │ │ +
72 size_t dim() const override { return d_; }
│ │ │ │ +
73
│ │ │ │ +
│ │ │ │ +
75 boost::shared_ptr<GaussianFactor> linearize(const Values &c) const override {
│ │ │ │ +
76 return whitenedJacobian_;
│ │ │ │ +
77 }
│ │ │ │ +
│ │ │ │ +
78};
│ │ │ │ +
│ │ │ │ +
79// \KarcherMeanFactor
│ │ │ │ +
80} // namespace gtsam
│ │ │ │ +
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
│ │ │ │ -
gtsam::MFAS
The MFAS class to solve a Minimum feedback arc set (MFAS) problem.
Definition MFAS.h:51
│ │ │ │ -
gtsam::MFAS::MFAS
MFAS(const std::map< KeyPair, double > &edgeWeights)
Construct from the weighted directed edges between the nodes.
Definition MFAS.h:69
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
│ │ │ │ +
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ +
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
gtsam::KarcherMeanFactor
The KarcherMeanFactor creates a constraint on all SO(n) variables with given keys that the Karcher me...
Definition KarcherMeanFactor.h:45
│ │ │ │ +
gtsam::KarcherMeanFactor::error
double error(const Values &c) const override
Calculate the error of the factor: always zero.
Definition KarcherMeanFactor.h:69
│ │ │ │ +
gtsam::KarcherMeanFactor::dim
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition KarcherMeanFactor.h:72
│ │ │ │ +
gtsam::KarcherMeanFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &c) const override
linearize to a GaussianFactor
Definition KarcherMeanFactor.h:75
│ │ │ │ +
gtsam::KarcherMeanFactor::~KarcherMeanFactor
~KarcherMeanFactor() override
Destructor.
Definition KarcherMeanFactor.h:66
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,84 +1,111 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -MFAS.h │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +KarcherMeanFactor.h │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -12#pragma once │ │ │ │ │ -13 │ │ │ │ │ -21#include │ │ │ │ │ -22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_s_f_m_/_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_._h> │ │ │ │ │ -24 │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ -28 │ │ │ │ │ -29namespace _g_t_s_a_m { │ │ │ │ │ -30 │ │ │ │ │ -_5_1class GTSAM_EXPORT _M_F_A_S { │ │ │ │ │ -52 public: │ │ │ │ │ -53 // used to represent edges between two nodes in the graph. When used in │ │ │ │ │ -54 // translation averaging for global SfM │ │ │ │ │ -55 using KeyPair = std::pair; │ │ │ │ │ -56 using TranslationEdges = std::vector>; │ │ │ │ │ -57 │ │ │ │ │ -58 private: │ │ │ │ │ -59 // edges with a direction such that all weights are positive │ │ │ │ │ -60 // i.e, edges that originally had negative weights are flipped │ │ │ │ │ -61 std::map edgeWeights_; │ │ │ │ │ -62 │ │ │ │ │ -63 public: │ │ │ │ │ -_6_9 _M_F_A_S(const std::map &edgeWeights) │ │ │ │ │ -70 : edgeWeights_(edgeWeights) {} │ │ │ │ │ -71 │ │ │ │ │ -80 _M_F_A_S(const TranslationEdges &relativeTranslations, │ │ │ │ │ -81 const _U_n_i_t_3 &projectionDirection); │ │ │ │ │ -82 │ │ │ │ │ -87 _K_e_y_V_e_c_t_o_r computeOrdering() const; │ │ │ │ │ -88 │ │ │ │ │ -96 std::map computeOutlierWeights() const; │ │ │ │ │ -97}; │ │ │ │ │ -98 │ │ │ │ │ -99typedef std::map, double> KeyPairDoubleMap; │ │ │ │ │ -100 │ │ │ │ │ -101} // namespace gtsam │ │ │ │ │ -_K_e_y_._h │ │ │ │ │ -_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_._h │ │ │ │ │ -Binary measurement represents a measurement between two keys in a graph. A │ │ │ │ │ -binary measurement is simi... │ │ │ │ │ +12/* │ │ │ │ │ +13 * @file KarcherMeanFactor.h │ │ │ │ │ +14 * @author Frank Dellaert │ │ │ │ │ +15 * @date March 2019 │ │ │ │ │ +16 */ │ │ │ │ │ +17 │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ +22 │ │ │ │ │ +23#include │ │ │ │ │ +24#include │ │ │ │ │ +25 │ │ │ │ │ +26namespace _g_t_s_a_m { │ │ │ │ │ +32template │ │ │ │ │ +33T FindKarcherMean(const std::vector> │ │ │ │ │ +&rotations); │ │ │ │ │ +34 │ │ │ │ │ +35template T FindKarcherMean(std::initializer_list &&rotations); │ │ │ │ │ +36 │ │ │ │ │ +_4_5template class _K_a_r_c_h_e_r_M_e_a_n_F_a_c_t_o_r : public _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r { │ │ │ │ │ +46 // Compile time dimension: can be -1 │ │ │ │ │ +47 enum { D = _t_r_a_i_t_s_<_T_>_:_:_d_i_m_e_n_s_i_o_n }; │ │ │ │ │ +48 │ │ │ │ │ +49 // Runtime dimension: always >=0 │ │ │ │ │ +50 size_t d_; │ │ │ │ │ +51 │ │ │ │ │ +53 boost::shared_ptr whitenedJacobian_; │ │ │ │ │ +54 │ │ │ │ │ +55public: │ │ │ │ │ +61 template │ │ │ │ │ +62 _K_a_r_c_h_e_r_M_e_a_n_F_a_c_t_o_r(const CONTAINER &_k_e_y_s, int d = D, │ │ │ │ │ +63 boost::optional beta = boost::none); │ │ │ │ │ +64 │ │ │ │ │ +_6_6 _~_K_a_r_c_h_e_r_M_e_a_n_F_a_c_t_o_r() override {} │ │ │ │ │ +67 │ │ │ │ │ +_6_9 double _e_r_r_o_r(const _V_a_l_u_e_s &c) const override { return 0; } │ │ │ │ │ +70 │ │ │ │ │ +_7_2 size_t _d_i_m() const override { return d_; } │ │ │ │ │ +73 │ │ │ │ │ +_7_5 boost::shared_ptr _l_i_n_e_a_r_i_z_e(const _V_a_l_u_e_s &c) const override │ │ │ │ │ +{ │ │ │ │ │ +76 return whitenedJacobian_; │ │ │ │ │ +77 } │ │ │ │ │ +78}; │ │ │ │ │ +79// \KarcherMeanFactor │ │ │ │ │ +80} // namespace gtsam │ │ │ │ │ +_M_a_t_r_i_x_._h │ │ │ │ │ +typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ +_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_U_n_i_t_3 │ │ │ │ │ -Represents a 3D point on a unit sphere. │ │ │ │ │ -DDeeffiinniittiioonn Unit3.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_M_F_A_S │ │ │ │ │ -The MFAS class to solve a Minimum feedback arc set (MFAS) problem. │ │ │ │ │ -DDeeffiinniittiioonn MFAS.h:51 │ │ │ │ │ -_g_t_s_a_m_:_:_M_F_A_S_:_:_M_F_A_S │ │ │ │ │ -MFAS(const std::map< KeyPair, double > &edgeWeights) │ │ │ │ │ -Construct from the weighted directed edges between the nodes. │ │ │ │ │ -DDeeffiinniittiioonn MFAS.h:69 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s │ │ │ │ │ +const KeyVector & keys() const │ │ │ │ │ +Access the factor's involved variable keys. │ │ │ │ │ +DDeeffiinniittiioonn Factor.h:140 │ │ │ │ │ +_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ +Nonlinear factor base class. │ │ │ │ │ +DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_K_a_r_c_h_e_r_M_e_a_n_F_a_c_t_o_r │ │ │ │ │ +The KarcherMeanFactor creates a constraint on all SO(n) variables with given │ │ │ │ │ +keys that the Karcher me... │ │ │ │ │ +DDeeffiinniittiioonn KarcherMeanFactor.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_K_a_r_c_h_e_r_M_e_a_n_F_a_c_t_o_r_:_:_e_r_r_o_r │ │ │ │ │ +double error(const Values &c) const override │ │ │ │ │ +Calculate the error of the factor: always zero. │ │ │ │ │ +DDeeffiinniittiioonn KarcherMeanFactor.h:69 │ │ │ │ │ +_g_t_s_a_m_:_:_K_a_r_c_h_e_r_M_e_a_n_F_a_c_t_o_r_:_:_d_i_m │ │ │ │ │ +size_t dim() const override │ │ │ │ │ +get the dimension of the factor (number of rows on linearization) │ │ │ │ │ +DDeeffiinniittiioonn KarcherMeanFactor.h:72 │ │ │ │ │ +_g_t_s_a_m_:_:_K_a_r_c_h_e_r_M_e_a_n_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e │ │ │ │ │ +boost::shared_ptr< GaussianFactor > linearize(const Values &c) const override │ │ │ │ │ +linearize to a GaussianFactor │ │ │ │ │ +DDeeffiinniittiioonn KarcherMeanFactor.h:75 │ │ │ │ │ +_g_t_s_a_m_:_:_K_a_r_c_h_e_r_M_e_a_n_F_a_c_t_o_r_:_:_~_K_a_r_c_h_e_r_M_e_a_n_F_a_c_t_o_r │ │ │ │ │ +~KarcherMeanFactor() override │ │ │ │ │ +Destructor. │ │ │ │ │ +DDeeffiinniittiioonn KarcherMeanFactor.h:66 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _s_f_m │ │ │ │ │ - * _M_F_A_S_._h │ │ │ │ │ + * _s_l_a_m │ │ │ │ │ + * KKaarrcchheerrMMeeaannFFaaccttoorr..hh │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01202.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00389.html │ │ │ │┄ Files 84% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/SfmTrack.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/geometry/SO3.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces | │ │ │ │ -Typedefs
│ │ │ │ -
SfmTrack.h File Reference
│ │ │ │ +Functions
│ │ │ │ +
SO3.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

A simple data structure for a track in Structure from Motion. │ │ │ │ +

3*3 matrix representation of SO(3) │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

struct  gtsam::SfmTrack2d
 Track containing 2D measurements associated with a single 3D point. More...
 
struct  gtsam::SfmTrack
 
struct  gtsam::traits< SfmTrack >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ -Typedefs

│ │ │ │ -typedef std::pair< size_t, Point2 > gtsam::SfmMeasurement
 A measurement with its camera index.
 
│ │ │ │ -typedef std::pair< size_t, size_t > gtsam::SiftIndex
 Sift index for SfmTrack.
 
│ │ │ │ -using gtsam::SfmTrack2dVector = std::vector< SfmTrack2d >
 

│ │ │ │ +Functions

│ │ │ │ +GTSAM_EXPORT Matrix99 gtsam::so3::Dcompose (const SO3 &R)
 (constant) Jacobian of compose wrpt M
 
GTSAM_EXPORT Matrix3 gtsam::so3::compose (const Matrix3 &M, const SO3 &R, OptionalJacobian< 9, 9 > H=boost::none)
 Compose general matrix with an SO(3) element.
 
│ │ │ │

Detailed Description

│ │ │ │ -

A simple data structure for a track in Structure from Motion.

│ │ │ │ -
Date
January 2022
│ │ │ │ -
Author
Frank Dellaert
│ │ │ │ -
│ │ │ │ +

3*3 matrix representation of SO(3)

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
│ │ │ │ +Luca Carlone
│ │ │ │ +
│ │ │ │ +Duy Nguyen Ta
│ │ │ │ +
Date
December 2014
│ │ │ │ +

Function Documentation

│ │ │ │ + │ │ │ │ +

◆ compose()

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +
GTSAM_EXPORT Matrix3 gtsam::so3::compose (const Matrix3 & M,
const SO3 & R,
OptionalJacobian< 9, 9 > H = boost::none 
)
│ │ │ │ +
│ │ │ │ + │ │ │ │ +

Compose general matrix with an SO(3) element.

│ │ │ │ +

We only provide the 9*9 derivative in the first argument M.

│ │ │ │ + │ │ │ │ +
│ │ │ │ +
│ │ │ │ + │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,42 +1,41 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _T_y_p_e_d_e_f_s │ │ │ │ │ -SfmTrack.h File Reference │ │ │ │ │ -A simple data structure for a track in Structure from Motion. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_2_d │ │ │ │ │ -  Track containing 2D measurements associated with a single 3D point. │ │ │ │ │ - _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_S_f_m_T_r_a_c_k │ │ │ │ │ -  │ │ │ │ │ -struct   _g_t_s_a_m_:_:_t_r_a_i_t_s_<_ _S_f_m_T_r_a_c_k_ _> │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +SO3.cpp File Reference │ │ │ │ │ +3*3 matrix representation of SO(3) _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -TTyyppeeddeeffss │ │ │ │ │ -typedef std::pair< size_t, _P_o_i_n_t_2 >  ggttssaamm::::SSffmmMMeeaassuurreemmeenntt │ │ │ │ │ -  A measurement with its camera index. │ │ │ │ │ -  │ │ │ │ │ -typedef std::pair< size_t, size_t >  ggttssaamm::::SSiiffttIInnddeexx │ │ │ │ │ -  Sift index for _S_f_m_T_r_a_c_k. │ │ │ │ │ -  │ │ │ │ │ - using  ggttssaamm::::SSffmmTTrraacckk22ddVVeeccttoorr = std::vector< │ │ │ │ │ - _S_f_m_T_r_a_c_k_2_d > │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +GTSAM_EXPORT Matrix99  ggttssaamm::::ssoo33::::DDccoommppoossee (const _S_O_3 &R) │ │ │ │ │ +  (constant) Jacobian of compose wrpt M │ │ │ │ │ +  │ │ │ │ │ + GTSAM_EXPORT Matrix3  _g_t_s_a_m_:_:_s_o_3_:_:_c_o_m_p_o_s_e (const Matrix3 &M, const _S_O_3 &R, │ │ │ │ │ + _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 9, 9 > H=boost::none) │ │ │ │ │ +  Compose general matrix with an SO(3) element. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -A simple data structure for a track in Structure from Motion. │ │ │ │ │ - Date │ │ │ │ │ - January 2022 │ │ │ │ │ +3*3 matrix representation of SO(3) │ │ │ │ │ Author │ │ │ │ │ Frank Dellaert │ │ │ │ │ + Luca Carlone │ │ │ │ │ + Duy Nguyen Ta │ │ │ │ │ + Date │ │ │ │ │ + December 2014 │ │ │ │ │ +********** FFuunnccttiioonn DDooccuummeennttaattiioonn ********** │ │ │ │ │ +********** _?◆_? ccoommppoossee(()) ********** │ │ │ │ │ +GTSAM_EXPORT Matrix3 gtsam::so3:: ( const Matrix3 &  MM, │ │ │ │ │ +compose │ │ │ │ │ + const _S_O_3 &  RR, │ │ │ │ │ + _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n< 9, 9 >  HH = boost::none  │ │ │ │ │ + ) │ │ │ │ │ +Compose general matrix with an SO(3) element. │ │ │ │ │ +We only provide the 9*9 derivative in the first argument M. │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _s_f_m │ │ │ │ │ - * _S_f_m_T_r_a_c_k_._h │ │ │ │ │ + * _g_e_o_m_e_t_r_y │ │ │ │ │ + * _S_O_3_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01202_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01370_source.html │ │ │ │┄ Files 81% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/SfmTrack.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicBayesNet.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
SfmTrack.h
│ │ │ │ +
SymbolicBayesNet.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/base/serialization.h>
│ │ │ │ -
22#include <gtsam/geometry/Point2.h>
│ │ │ │ -
23#include <gtsam/geometry/Point3.h>
│ │ │ │ -
24
│ │ │ │ -
25#include <Eigen/Core>
│ │ │ │ -
26#include <string>
│ │ │ │ -
27#include <utility>
│ │ │ │ -
28#include <vector>
│ │ │ │ -
29
│ │ │ │ -
30namespace gtsam {
│ │ │ │ -
31
│ │ │ │ -
33typedef std::pair<size_t, Point2> SfmMeasurement;
│ │ │ │ -
34
│ │ │ │ -
36typedef std::pair<size_t, size_t> SiftIndex;
│ │ │ │ -
37
│ │ │ │ -
│ │ │ │ -
43struct GTSAM_EXPORT SfmTrack2d {
│ │ │ │ -
45 std::vector<SfmMeasurement> measurements;
│ │ │ │ -
46
│ │ │ │ -
48 std::vector<SiftIndex> siftIndices;
│ │ │ │ -
49
│ │ │ │ -
52
│ │ │ │ -
53 // Default constructor.
│ │ │ │ -
54 SfmTrack2d() = default;
│ │ │ │ -
55
│ │ │ │ -
56 // Constructor from measurements.
│ │ │ │ -
57 explicit SfmTrack2d(const std::vector<SfmMeasurement>& measurements)
│ │ │ │ -
58 : measurements(measurements) {}
│ │ │ │ -
59
│ │ │ │ -
63
│ │ │ │ -
│ │ │ │ -
65 void addMeasurement(size_t idx, const gtsam::Point2& m) {
│ │ │ │ -
66 measurements.emplace_back(idx, m);
│ │ │ │ -
67 }
│ │ │ │ -
│ │ │ │ -
68
│ │ │ │ -
70 size_t numberMeasurements() const { return measurements.size(); }
│ │ │ │ -
71
│ │ │ │ -
│ │ │ │ -
73 const SfmMeasurement& measurement(size_t idx) const {
│ │ │ │ -
74 return measurements[idx];
│ │ │ │ -
75 }
│ │ │ │ -
│ │ │ │ -
76
│ │ │ │ -
78 const SiftIndex& siftIndex(size_t idx) const { return siftIndices[idx]; }
│ │ │ │ -
79
│ │ │ │ -
│ │ │ │ -
84 bool hasUniqueCameras() const {
│ │ │ │ -
85 std::vector<int> track_cam_indices;
│ │ │ │ -
86 for (auto& measurement : measurements) {
│ │ │ │ -
87 track_cam_indices.emplace_back(measurement.first);
│ │ │ │ -
88 }
│ │ │ │ -
89 auto i =
│ │ │ │ -
90 std::adjacent_find(track_cam_indices.begin(), track_cam_indices.end());
│ │ │ │ -
91 bool all_cameras_unique = (i == track_cam_indices.end());
│ │ │ │ -
92 return all_cameras_unique;
│ │ │ │ -
93 }
│ │ │ │ -
│ │ │ │ -
94
│ │ │ │ -
98
│ │ │ │ -
│ │ │ │ -
100 Eigen::MatrixX2d measurementMatrix() const {
│ │ │ │ -
101 Eigen::MatrixX2d m(numberMeasurements(), 2);
│ │ │ │ -
102 for (size_t i = 0; i < numberMeasurements(); i++) {
│ │ │ │ -
103 m.row(i) = measurement(i).second;
│ │ │ │ -
104 }
│ │ │ │ -
105 return m;
│ │ │ │ -
106 }
│ │ │ │ -
│ │ │ │ -
107
│ │ │ │ -
│ │ │ │ -
109 Eigen::VectorXi indexVector() const {
│ │ │ │ -
110 Eigen::VectorXi v(numberMeasurements());
│ │ │ │ -
111 for (size_t i = 0; i < numberMeasurements(); i++) {
│ │ │ │ -
112 v(i) = measurement(i).first;
│ │ │ │ -
113 }
│ │ │ │ -
114 return v;
│ │ │ │ -
115 }
│ │ │ │ -
│ │ │ │ -
116
│ │ │ │ -
118};
│ │ │ │ +
21#include <gtsam/symbolic/SymbolicConditional.h>
│ │ │ │ +
22#include <gtsam/inference/BayesNet.h>
│ │ │ │ +
23#include <gtsam/inference/FactorGraph.h>
│ │ │ │ +
24#include <gtsam/base/types.h>
│ │ │ │ +
25
│ │ │ │ +
26namespace gtsam {
│ │ │ │ +
27
│ │ │ │ +
│ │ │ │ +
32 class SymbolicBayesNet : public BayesNet<SymbolicConditional> {
│ │ │ │ +
33 public:
│ │ │ │ +
34 typedef BayesNet<SymbolicConditional> Base;
│ │ │ │ +
35 typedef SymbolicBayesNet This;
│ │ │ │ +
36 typedef SymbolicConditional ConditionalType;
│ │ │ │ +
37 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
38 typedef boost::shared_ptr<ConditionalType> sharedConditional;
│ │ │ │ +
39
│ │ │ │ +
42
│ │ │ │ +
44 SymbolicBayesNet() {}
│ │ │ │ +
45
│ │ │ │ +
47 template <typename ITERATOR>
│ │ │ │ +
│ │ │ │ +
48 SymbolicBayesNet(ITERATOR firstConditional, ITERATOR lastConditional)
│ │ │ │ +
49 : Base(firstConditional, lastConditional) {}
│ │ │ │ +
│ │ │ │ +
50
│ │ │ │ +
52 template <class CONTAINER>
│ │ │ │ +
│ │ │ │ +
53 explicit SymbolicBayesNet(const CONTAINER& conditionals) {
│ │ │ │ +
54 push_back(conditionals);
│ │ │ │ +
55 }
│ │ │ │ +
│ │ │ │ +
56
│ │ │ │ +
59 template <class DERIVEDCONDITIONAL>
│ │ │ │ +
│ │ │ │ +
60 explicit SymbolicBayesNet(const FactorGraph<DERIVEDCONDITIONAL>& graph)
│ │ │ │ +
61 : Base(graph) {}
│ │ │ │ +
│ │ │ │ +
62
│ │ │ │ +
│ │ │ │ +
67 SymbolicBayesNet(std::initializer_list<boost::shared_ptr<SymbolicConditional>> conditionals)
│ │ │ │ +
68 : Base(conditionals) {}
│ │ │ │ +
│ │ │ │ +
69
│ │ │ │ +
│ │ │ │ +
71 SymbolicBayesNet(SymbolicConditional&& c) {
│ │ │ │ +
72 push_back(boost::make_shared<SymbolicConditional>(c));
│ │ │ │ +
73 }
│ │ │ │ +
│ │ │ │ +
74
│ │ │ │ +
│ │ │ │ +
81 SymbolicBayesNet& operator()(SymbolicConditional&& c) {
│ │ │ │ +
82 push_back(boost::make_shared<SymbolicConditional>(c));
│ │ │ │ +
83 return *this;
│ │ │ │ +
84 }
│ │ │ │ +
│ │ │ │ +
85
│ │ │ │ +
87 virtual ~SymbolicBayesNet() {}
│ │ │ │ +
88
│ │ │ │ +
90
│ │ │ │ +
93
│ │ │ │ +
95 GTSAM_EXPORT bool equals(const This& bn, double tol = 1e-9) const;
│ │ │ │ +
96
│ │ │ │ +
│ │ │ │ +
98 GTSAM_EXPORT void print(
│ │ │ │ +
99 const std::string& s = "SymbolicBayesNet",
│ │ │ │ +
100 const KeyFormatter& formatter = DefaultKeyFormatter) const override {
│ │ │ │ +
101 Base::print(s, formatter);
│ │ │ │ +
102 }
│ │ │ │ +
│ │ │ │ +
103
│ │ │ │ +
105
│ │ │ │ +
106 private:
│ │ │ │ +
108 friend class boost::serialization::access;
│ │ │ │ +
109 template<class ARCHIVE>
│ │ │ │ +
110 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
111 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ +
112 }
│ │ │ │ +
113};
│ │ │ │ +
│ │ │ │ +
114
│ │ │ │ +
116 template<>
│ │ │ │ +
│ │ │ │ +
117 struct traits<SymbolicBayesNet> : public Testable<SymbolicBayesNet> {
│ │ │ │ +
118 };
│ │ │ │
│ │ │ │
119
│ │ │ │ -
120using SfmTrack2dVector = std::vector<SfmTrack2d>;
│ │ │ │ -
121
│ │ │ │ -
│ │ │ │ -
126struct GTSAM_EXPORT SfmTrack : SfmTrack2d {
│ │ │ │ -
127 Point3 p;
│ │ │ │ -
128 float r, g, b;
│ │ │ │ -
129
│ │ │ │ -
132
│ │ │ │ -
133 explicit SfmTrack(float r = 0, float g = 0, float b = 0)
│ │ │ │ -
134 : p(0, 0, 0), r(r), g(g), b(b) {}
│ │ │ │ -
135
│ │ │ │ -
136 explicit SfmTrack(const gtsam::Point3& pt, float r = 0, float g = 0,
│ │ │ │ -
137 float b = 0)
│ │ │ │ -
138 : p(pt), r(r), g(g), b(b) {}
│ │ │ │ -
139
│ │ │ │ -
143
│ │ │ │ -
145 const Point3& point3() const { return p; }
│ │ │ │ -
146
│ │ │ │ -
148 Point3 rgb() const { return Point3(r, g, b); }
│ │ │ │ -
149
│ │ │ │ -
153
│ │ │ │ -
155 void print(const std::string& s = "") const;
│ │ │ │ -
156
│ │ │ │ -
158 bool equals(const SfmTrack& sfmTrack, double tol = 1e-9) const;
│ │ │ │ -
159
│ │ │ │ -
161#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ -
164 void GTSAM_DEPRECATED add_measurement(size_t idx, const gtsam::Point2& m) {
│ │ │ │ -
165 measurements.emplace_back(idx, m);
│ │ │ │ -
166 }
│ │ │ │ -
167
│ │ │ │ -
168 size_t GTSAM_DEPRECATED number_measurements() const {
│ │ │ │ -
169 return measurements.size();
│ │ │ │ -
170 }
│ │ │ │ -
172#endif
│ │ │ │ -
175
│ │ │ │ -
177 friend class boost::serialization::access;
│ │ │ │ -
178 template <class ARCHIVE>
│ │ │ │ -
179 void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
│ │ │ │ -
180 ar& BOOST_SERIALIZATION_NVP(p);
│ │ │ │ -
181 ar& BOOST_SERIALIZATION_NVP(r);
│ │ │ │ -
182 ar& BOOST_SERIALIZATION_NVP(g);
│ │ │ │ -
183 ar& BOOST_SERIALIZATION_NVP(b);
│ │ │ │ -
184 ar& BOOST_SERIALIZATION_NVP(measurements);
│ │ │ │ -
185 ar& BOOST_SERIALIZATION_NVP(siftIndices);
│ │ │ │ -
186 }
│ │ │ │ -
188};
│ │ │ │ -
│ │ │ │ -
189
│ │ │ │ -
190template <typename T>
│ │ │ │ -
191struct traits;
│ │ │ │ -
192
│ │ │ │ -
193template <>
│ │ │ │ -
194struct traits<SfmTrack> : public Testable<SfmTrack> {};
│ │ │ │ -
195
│ │ │ │ -
196} // namespace gtsam
│ │ │ │ -
serialization.h
Convenience functions for serializing data structures via boost.serialization.
│ │ │ │ -
Point3.h
3D Point
│ │ │ │ -
Point2.h
2D Point
│ │ │ │ +
120} //\ namespace gtsam
│ │ │ │ +
types.h
Typedefs for easier changing of types.
│ │ │ │ +
FactorGraph.h
Factor Graph Base Class.
│ │ │ │ +
BayesNet.h
Bayes network.
│ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::SiftIndex
std::pair< size_t, size_t > SiftIndex
Sift index for SfmTrack.
Definition SfmTrack.h:36
│ │ │ │ -
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ -
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
│ │ │ │ -
gtsam::SfmMeasurement
std::pair< size_t, Point2 > SfmMeasurement
A measurement with its camera index.
Definition SfmTrack.h:33
│ │ │ │ -
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::SfmTrack2d
Track containing 2D measurements associated with a single 3D point.
Definition SfmTrack.h:43
│ │ │ │ -
gtsam::SfmTrack2d::addMeasurement
void addMeasurement(size_t idx, const gtsam::Point2 &m)
Add measurement (camera_idx, Point2) to track.
Definition SfmTrack.h:65
│ │ │ │ -
gtsam::SfmTrack2d::measurement
const SfmMeasurement & measurement(size_t idx) const
Get the measurement (camera index, Point2) at pose index idx
Definition SfmTrack.h:73
│ │ │ │ -
gtsam::SfmTrack2d::siftIndices
std::vector< SiftIndex > siftIndices
The feature descriptors (optional)
Definition SfmTrack.h:48
│ │ │ │ -
gtsam::SfmTrack2d::siftIndex
const SiftIndex & siftIndex(size_t idx) const
Get the SIFT feature index corresponding to the measurement at idx
Definition SfmTrack.h:78
│ │ │ │ -
gtsam::SfmTrack2d::hasUniqueCameras
bool hasUniqueCameras() const
Check that no two measurements are from the same camera.
Definition SfmTrack.h:84
│ │ │ │ -
gtsam::SfmTrack2d::measurementMatrix
Eigen::MatrixX2d measurementMatrix() const
Return the measurements as a 2D matrix.
Definition SfmTrack.h:100
│ │ │ │ -
gtsam::SfmTrack2d::numberMeasurements
size_t numberMeasurements() const
Total number of measurements in this track.
Definition SfmTrack.h:70
│ │ │ │ -
gtsam::SfmTrack2d::measurements
std::vector< SfmMeasurement > measurements
The 2D image projections (id,(u,v))
Definition SfmTrack.h:45
│ │ │ │ -
gtsam::SfmTrack2d::indexVector
Eigen::VectorXi indexVector() const
Return the camera indices of the measurements.
Definition SfmTrack.h:109
│ │ │ │ -
gtsam::SfmTrack
Definition SfmTrack.h:126
│ │ │ │ -
gtsam::SfmTrack::rgb
Point3 rgb() const
Get RGB values describing 3d point.
Definition SfmTrack.h:148
│ │ │ │ -
gtsam::SfmTrack::b
float b
RGB color of the 3D point.
Definition SfmTrack.h:128
│ │ │ │ -
gtsam::SfmTrack::point3
const Point3 & point3() const
Get 3D point.
Definition SfmTrack.h:145
│ │ │ │ -
gtsam::SfmTrack::p
Point3 p
3D position of the point
Definition SfmTrack.h:127
│ │ │ │ +
gtsam::BayesNet
A BayesNet is a tree of conditionals, stored in elimination order.
Definition BayesNet.h:35
│ │ │ │ +
gtsam::BayesNet< SymbolicConditional >::print
void print(const std::string &s="BayesNet", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print out graph
Definition BayesNet-inst.h:32
│ │ │ │ +
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ +
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition FactorGraph.h:186
│ │ │ │ +
gtsam::SymbolicBayesNet
A SymbolicBayesNet is a Bayes Net of purely symbolic conditionals.
Definition SymbolicBayesNet.h:32
│ │ │ │ +
gtsam::SymbolicBayesNet::~SymbolicBayesNet
virtual ~SymbolicBayesNet()
Destructor.
Definition SymbolicBayesNet.h:87
│ │ │ │ +
gtsam::SymbolicBayesNet::SymbolicBayesNet
SymbolicBayesNet(SymbolicConditional &&c)
Construct from a single conditional.
Definition SymbolicBayesNet.h:71
│ │ │ │ +
gtsam::SymbolicBayesNet::print
GTSAM_EXPORT void print(const std::string &s="SymbolicBayesNet", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print
Definition SymbolicBayesNet.h:98
│ │ │ │ +
gtsam::SymbolicBayesNet::SymbolicBayesNet
SymbolicBayesNet(ITERATOR firstConditional, ITERATOR lastConditional)
Construct from iterator over conditionals.
Definition SymbolicBayesNet.h:48
│ │ │ │ +
gtsam::SymbolicBayesNet::SymbolicBayesNet
SymbolicBayesNet(std::initializer_list< boost::shared_ptr< SymbolicConditional > > conditionals)
Constructor that takes an initializer list of shared pointers.
Definition SymbolicBayesNet.h:67
│ │ │ │ +
gtsam::SymbolicBayesNet::SymbolicBayesNet
SymbolicBayesNet(const CONTAINER &conditionals)
Construct from container of factors (shared_ptr or plain objects)
Definition SymbolicBayesNet.h:53
│ │ │ │ +
gtsam::SymbolicBayesNet::SymbolicBayesNet
SymbolicBayesNet()
Construct empty factor graph.
Definition SymbolicBayesNet.h:44
│ │ │ │ +
gtsam::SymbolicBayesNet::access
friend class boost::serialization::access
Serialization function.
Definition SymbolicBayesNet.h:108
│ │ │ │ +
gtsam::SymbolicBayesNet::operator()
SymbolicBayesNet & operator()(SymbolicConditional &&c)
Add a single conditional and return a reference.
Definition SymbolicBayesNet.h:81
│ │ │ │ +
gtsam::SymbolicBayesNet::SymbolicBayesNet
SymbolicBayesNet(const FactorGraph< DERIVEDCONDITIONAL > &graph)
Implicit copy/downcast constructor to override explicit template container constructor.
Definition SymbolicBayesNet.h:60
│ │ │ │ +
gtsam::SymbolicConditional
SymbolicConditional is a conditional with keys but no probability data, produced by symbolic eliminat...
Definition SymbolicConditional.h:38
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SfmTrack.h │ │ │ │ │ +SymbolicBayesNet.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ @@ -16,233 +16,171 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 19#pragma once │ │ │ │ │ 20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_b_a_s_e_/_s_e_r_i_a_l_i_z_a_t_i_o_n_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_2_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_o_i_n_t_3_._h> │ │ │ │ │ -24 │ │ │ │ │ -25#include │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ -28#include │ │ │ │ │ -29 │ │ │ │ │ -30namespace _g_t_s_a_m { │ │ │ │ │ -31 │ │ │ │ │ -_3_3typedef std::pair _S_f_m_M_e_a_s_u_r_e_m_e_n_t; │ │ │ │ │ -34 │ │ │ │ │ -_3_6typedef std::pair _S_i_f_t_I_n_d_e_x; │ │ │ │ │ -37 │ │ │ │ │ -_4_3struct GTSAM_EXPORT _S_f_m_T_r_a_c_k_2_d { │ │ │ │ │ -_4_5 std::vector _m_e_a_s_u_r_e_m_e_n_t_s; │ │ │ │ │ -46 │ │ │ │ │ -_4_8 std::vector _s_i_f_t_I_n_d_i_c_e_s; │ │ │ │ │ -49 │ │ │ │ │ -52 │ │ │ │ │ -53 // Default constructor. │ │ │ │ │ -54 _S_f_m_T_r_a_c_k_2_d() = default; │ │ │ │ │ -55 │ │ │ │ │ -56 // Constructor from measurements. │ │ │ │ │ -57 explicit _S_f_m_T_r_a_c_k_2_d(const std::vector& measurements) │ │ │ │ │ -58 : measurements(measurements) {} │ │ │ │ │ -59 │ │ │ │ │ -63 │ │ │ │ │ -_6_5 void _a_d_d_M_e_a_s_u_r_e_m_e_n_t(size_t idx, const _g_t_s_a_m_:_:_P_o_i_n_t_2& m) { │ │ │ │ │ -66 measurements.emplace_back(idx, m); │ │ │ │ │ -67 } │ │ │ │ │ -68 │ │ │ │ │ -_7_0 size_t _n_u_m_b_e_r_M_e_a_s_u_r_e_m_e_n_t_s() const { return measurements.size(); } │ │ │ │ │ -71 │ │ │ │ │ -_7_3 const _S_f_m_M_e_a_s_u_r_e_m_e_n_t& _m_e_a_s_u_r_e_m_e_n_t(size_t idx) const { │ │ │ │ │ -74 return measurements[idx]; │ │ │ │ │ -75 } │ │ │ │ │ -76 │ │ │ │ │ -_7_8 const _S_i_f_t_I_n_d_e_x& _s_i_f_t_I_n_d_e_x(size_t idx) const { return siftIndices[idx]; } │ │ │ │ │ -79 │ │ │ │ │ -_8_4 bool _h_a_s_U_n_i_q_u_e_C_a_m_e_r_a_s() const { │ │ │ │ │ -85 std::vector track_cam_indices; │ │ │ │ │ -86 for (auto& measurement : measurements) { │ │ │ │ │ -87 track_cam_indices.emplace_back(measurement.first); │ │ │ │ │ -88 } │ │ │ │ │ -89 auto i = │ │ │ │ │ -90 std::adjacent_find(track_cam_indices.begin(), track_cam_indices.end()); │ │ │ │ │ -91 bool all_cameras_unique = (i == track_cam_indices.end()); │ │ │ │ │ -92 return all_cameras_unique; │ │ │ │ │ -93 } │ │ │ │ │ -94 │ │ │ │ │ -98 │ │ │ │ │ -_1_0_0 Eigen::MatrixX2d _m_e_a_s_u_r_e_m_e_n_t_M_a_t_r_i_x() const { │ │ │ │ │ -101 Eigen::MatrixX2d m(numberMeasurements(), 2); │ │ │ │ │ -102 for (size_t i = 0; i < numberMeasurements(); i++) { │ │ │ │ │ -103 m.row(i) = measurement(i).second; │ │ │ │ │ -104 } │ │ │ │ │ -105 return m; │ │ │ │ │ -106 } │ │ │ │ │ -107 │ │ │ │ │ -_1_0_9 Eigen::VectorXi _i_n_d_e_x_V_e_c_t_o_r() const { │ │ │ │ │ -110 Eigen::VectorXi v(numberMeasurements()); │ │ │ │ │ -111 for (size_t i = 0; i < numberMeasurements(); i++) { │ │ │ │ │ -112 v(i) = measurement(i).first; │ │ │ │ │ -113 } │ │ │ │ │ -114 return v; │ │ │ │ │ -115 } │ │ │ │ │ -116 │ │ │ │ │ -118}; │ │ │ │ │ +21#include <_g_t_s_a_m_/_s_y_m_b_o_l_i_c_/_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_B_a_y_e_s_N_e_t_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ +25 │ │ │ │ │ +26namespace _g_t_s_a_m { │ │ │ │ │ +27 │ │ │ │ │ +_3_2 class _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t : public _B_a_y_e_s_N_e_t { │ │ │ │ │ +33 public: │ │ │ │ │ +34 typedef _B_a_y_e_s_N_e_t_<_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_> _B_a_s_e; │ │ │ │ │ +35 typedef _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t _T_h_i_s; │ │ │ │ │ +36 typedef _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l _C_o_n_d_i_t_i_o_n_a_l_T_y_p_e; │ │ │ │ │ +37 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +38 typedef boost::shared_ptr sharedConditional; │ │ │ │ │ +39 │ │ │ │ │ +42 │ │ │ │ │ +_4_4 _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t() {} │ │ │ │ │ +45 │ │ │ │ │ +47 template │ │ │ │ │ +_4_8 _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t(ITERATOR firstConditional, ITERATOR lastConditional) │ │ │ │ │ +49 : _B_a_s_e(firstConditional, lastConditional) {} │ │ │ │ │ +50 │ │ │ │ │ +52 template │ │ │ │ │ +_5_3 explicit _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t(const CONTAINER& conditionals) { │ │ │ │ │ +54 _p_u_s_h___b_a_c_k(conditionals); │ │ │ │ │ +55 } │ │ │ │ │ +56 │ │ │ │ │ +59 template │ │ │ │ │ +_6_0 explicit _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t(const _F_a_c_t_o_r_G_r_a_p_h_<_D_E_R_I_V_E_D_C_O_N_D_I_T_I_O_N_A_L_>& graph) │ │ │ │ │ +61 : _B_a_s_e(graph) {} │ │ │ │ │ +62 │ │ │ │ │ +_6_7 _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t(std::initializer_list> conditionals) │ │ │ │ │ +68 : _B_a_s_e(conditionals) {} │ │ │ │ │ +69 │ │ │ │ │ +_7_1 _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t(_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l&& c) { │ │ │ │ │ +72 _p_u_s_h___b_a_c_k(boost::make_shared(c)); │ │ │ │ │ +73 } │ │ │ │ │ +74 │ │ │ │ │ +_8_1 _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t& _o_p_e_r_a_t_o_r_(_)(_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l&& c) { │ │ │ │ │ +82 _p_u_s_h___b_a_c_k(boost::make_shared(c)); │ │ │ │ │ +83 return *this; │ │ │ │ │ +84 } │ │ │ │ │ +85 │ │ │ │ │ +_8_7 virtual _~_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t() {} │ │ │ │ │ +88 │ │ │ │ │ +90 │ │ │ │ │ +93 │ │ │ │ │ +95 GTSAM_EXPORT bool _e_q_u_a_l_s(const This& bn, double tol = 1e-9) const; │ │ │ │ │ +96 │ │ │ │ │ +_9_8 GTSAM_EXPORT void _p_r_i_n_t( │ │ │ │ │ +99 const std::string& s = "SymbolicBayesNet", │ │ │ │ │ +100 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override { │ │ │ │ │ +101 _B_a_s_e_:_:_p_r_i_n_t(s, formatter); │ │ │ │ │ +102 } │ │ │ │ │ +103 │ │ │ │ │ +105 │ │ │ │ │ +106 private: │ │ │ │ │ +_1_0_8 friend class _b_o_o_s_t_:_:_s_e_r_i_a_l_i_z_a_t_i_o_n_:_:_a_c_c_e_s_s; │ │ │ │ │ +109 template │ │ │ │ │ +110 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +111 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ +112 } │ │ │ │ │ +113}; │ │ │ │ │ +114 │ │ │ │ │ +116 template<> │ │ │ │ │ +_1_1_7 struct _t_r_a_i_t_s<_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t> : public _T_e_s_t_a_b_l_e { │ │ │ │ │ +118 }; │ │ │ │ │ 119 │ │ │ │ │ -120using SfmTrack2dVector = std::vector; │ │ │ │ │ -121 │ │ │ │ │ -_1_2_6struct GTSAM_EXPORT _S_f_m_T_r_a_c_k : _S_f_m_T_r_a_c_k_2_d { │ │ │ │ │ -_1_2_7 _P_o_i_n_t_3 _p; │ │ │ │ │ -_1_2_8 float r, g, _b; │ │ │ │ │ -129 │ │ │ │ │ -132 │ │ │ │ │ -133 explicit _S_f_m_T_r_a_c_k(float r = 0, float g = 0, float b = 0) │ │ │ │ │ -134 : p(0, 0, 0), r(r), g(g), b(b) {} │ │ │ │ │ -135 │ │ │ │ │ -136 explicit _S_f_m_T_r_a_c_k(const _g_t_s_a_m_:_:_P_o_i_n_t_3& pt, float r = 0, float g = 0, │ │ │ │ │ -137 float b = 0) │ │ │ │ │ -138 : p(pt), r(r), g(g), b(b) {} │ │ │ │ │ -139 │ │ │ │ │ -143 │ │ │ │ │ -_1_4_5 const _P_o_i_n_t_3& _p_o_i_n_t_3() const { return p; } │ │ │ │ │ -146 │ │ │ │ │ -_1_4_8 _P_o_i_n_t_3 _r_g_b() const { return _P_o_i_n_t_3(r, g, b); } │ │ │ │ │ -149 │ │ │ │ │ -153 │ │ │ │ │ -155 void _p_r_i_n_t(const std::string& s = "") const; │ │ │ │ │ -156 │ │ │ │ │ -158 bool _e_q_u_a_l_s(const _S_f_m_T_r_a_c_k& sfmTrack, double tol = 1e-9) const; │ │ │ │ │ -159 │ │ │ │ │ -161#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ -164 void GTSAM_DEPRECATED add_measurement(size_t idx, const _g_t_s_a_m_:_:_P_o_i_n_t_2& m) { │ │ │ │ │ -165 measurements.emplace_back(idx, m); │ │ │ │ │ -166 } │ │ │ │ │ -167 │ │ │ │ │ -168 size_t GTSAM_DEPRECATED number_measurements() const { │ │ │ │ │ -169 return measurements.size(); │ │ │ │ │ -170 } │ │ │ │ │ -172#endif │ │ │ │ │ -175 │ │ │ │ │ -_1_7_7 friend class boost::serialization::access; │ │ │ │ │ -178 template │ │ │ │ │ -179 void serialize(ARCHIVE& ar, const unsigned int /*version*/) { │ │ │ │ │ -180 ar& BOOST_SERIALIZATION_NVP(p); │ │ │ │ │ -181 ar& BOOST_SERIALIZATION_NVP(r); │ │ │ │ │ -182 ar& BOOST_SERIALIZATION_NVP(g); │ │ │ │ │ -183 ar& BOOST_SERIALIZATION_NVP(b); │ │ │ │ │ -184 ar& BOOST_SERIALIZATION_NVP(measurements); │ │ │ │ │ -185 ar& BOOST_SERIALIZATION_NVP(siftIndices); │ │ │ │ │ -186 } │ │ │ │ │ -188}; │ │ │ │ │ -189 │ │ │ │ │ -190template │ │ │ │ │ -191struct traits; │ │ │ │ │ -192 │ │ │ │ │ -193template <> │ │ │ │ │ -_1_9_4struct _t_r_a_i_t_s<_S_f_m_T_r_a_c_k> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ -195 │ │ │ │ │ -196} // namespace gtsam │ │ │ │ │ -_s_e_r_i_a_l_i_z_a_t_i_o_n_._h │ │ │ │ │ -Convenience functions for serializing data structures via boost.serialization. │ │ │ │ │ -_P_o_i_n_t_3_._h │ │ │ │ │ -3D Point │ │ │ │ │ -_P_o_i_n_t_2_._h │ │ │ │ │ -2D Point │ │ │ │ │ +120} //\ namespace gtsam │ │ │ │ │ +_t_y_p_e_s_._h │ │ │ │ │ +Typedefs for easier changing of types. │ │ │ │ │ +_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Factor Graph Base Class. │ │ │ │ │ +_B_a_y_e_s_N_e_t_._h │ │ │ │ │ +Bayes network. │ │ │ │ │ +_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_S_i_f_t_I_n_d_e_x │ │ │ │ │ -std::pair< size_t, size_t > SiftIndex │ │ │ │ │ -Sift index for SfmTrack. │ │ │ │ │ -DDeeffiinniittiioonn SfmTrack.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ -void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ -print without optional string, must specify cout yourself │ │ │ │ │ -DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_2 │ │ │ │ │ -Vector2 Point2 │ │ │ │ │ -As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point2 to Vector2... │ │ │ │ │ -DDeeffiinniittiioonn Point2.h:27 │ │ │ │ │ -_g_t_s_a_m_:_:_S_f_m_M_e_a_s_u_r_e_m_e_n_t │ │ │ │ │ -std::pair< size_t, Point2 > SfmMeasurement │ │ │ │ │ -A measurement with its camera index. │ │ │ │ │ -DDeeffiinniittiioonn SfmTrack.h:33 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_i_n_t_3 │ │ │ │ │ -Vector3 Point3 │ │ │ │ │ -As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just │ │ │ │ │ -typedef Point3 to Vector3... │ │ │ │ │ -DDeeffiinniittiioonn Point3.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ _g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ Template to create a binary predicate. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ _g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ A helper that implements the traits interface for GTSAM types. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_2_d │ │ │ │ │ -Track containing 2D measurements associated with a single 3D point. │ │ │ │ │ -DDeeffiinniittiioonn SfmTrack.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_2_d_:_:_a_d_d_M_e_a_s_u_r_e_m_e_n_t │ │ │ │ │ -void addMeasurement(size_t idx, const gtsam::Point2 &m) │ │ │ │ │ -Add measurement (camera_idx, Point2) to track. │ │ │ │ │ -DDeeffiinniittiioonn SfmTrack.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_2_d_:_:_m_e_a_s_u_r_e_m_e_n_t │ │ │ │ │ -const SfmMeasurement & measurement(size_t idx) const │ │ │ │ │ -Get the measurement (camera index, Point2) at pose index idx │ │ │ │ │ -DDeeffiinniittiioonn SfmTrack.h:73 │ │ │ │ │ -_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_2_d_:_:_s_i_f_t_I_n_d_i_c_e_s │ │ │ │ │ -std::vector< SiftIndex > siftIndices │ │ │ │ │ -The feature descriptors (optional) │ │ │ │ │ -DDeeffiinniittiioonn SfmTrack.h:48 │ │ │ │ │ -_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_2_d_:_:_s_i_f_t_I_n_d_e_x │ │ │ │ │ -const SiftIndex & siftIndex(size_t idx) const │ │ │ │ │ -Get the SIFT feature index corresponding to the measurement at idx │ │ │ │ │ -DDeeffiinniittiioonn SfmTrack.h:78 │ │ │ │ │ -_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_2_d_:_:_h_a_s_U_n_i_q_u_e_C_a_m_e_r_a_s │ │ │ │ │ -bool hasUniqueCameras() const │ │ │ │ │ -Check that no two measurements are from the same camera. │ │ │ │ │ -DDeeffiinniittiioonn SfmTrack.h:84 │ │ │ │ │ -_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_2_d_:_:_m_e_a_s_u_r_e_m_e_n_t_M_a_t_r_i_x │ │ │ │ │ -Eigen::MatrixX2d measurementMatrix() const │ │ │ │ │ -Return the measurements as a 2D matrix. │ │ │ │ │ -DDeeffiinniittiioonn SfmTrack.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_2_d_:_:_n_u_m_b_e_r_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ -size_t numberMeasurements() const │ │ │ │ │ -Total number of measurements in this track. │ │ │ │ │ -DDeeffiinniittiioonn SfmTrack.h:70 │ │ │ │ │ -_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_2_d_:_:_m_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ -std::vector< SfmMeasurement > measurements │ │ │ │ │ -The 2D image projections (id,(u,v)) │ │ │ │ │ -DDeeffiinniittiioonn SfmTrack.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_2_d_:_:_i_n_d_e_x_V_e_c_t_o_r │ │ │ │ │ -Eigen::VectorXi indexVector() const │ │ │ │ │ -Return the camera indices of the measurements. │ │ │ │ │ -DDeeffiinniittiioonn SfmTrack.h:109 │ │ │ │ │ -_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k │ │ │ │ │ -DDeeffiinniittiioonn SfmTrack.h:126 │ │ │ │ │ -_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_:_:_r_g_b │ │ │ │ │ -Point3 rgb() const │ │ │ │ │ -Get RGB values describing 3d point. │ │ │ │ │ -DDeeffiinniittiioonn SfmTrack.h:148 │ │ │ │ │ -_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_:_:_b │ │ │ │ │ -float b │ │ │ │ │ -RGB color of the 3D point. │ │ │ │ │ -DDeeffiinniittiioonn SfmTrack.h:128 │ │ │ │ │ -_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_:_:_p_o_i_n_t_3 │ │ │ │ │ -const Point3 & point3() const │ │ │ │ │ -Get 3D point. │ │ │ │ │ -DDeeffiinniittiioonn SfmTrack.h:145 │ │ │ │ │ -_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k_:_:_p │ │ │ │ │ -Point3 p │ │ │ │ │ -3D position of the point │ │ │ │ │ -DDeeffiinniittiioonn SfmTrack.h:127 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t │ │ │ │ │ +A BayesNet is a tree of conditionals, stored in elimination order. │ │ │ │ │ +DDeeffiinniittiioonn BayesNet.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_y_e_s_N_e_t_<_ _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_ _>_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="BayesNet", const KeyFormatter │ │ │ │ │ +&formatter=DefaultKeyFormatter) const override │ │ │ │ │ +print out graph │ │ │ │ │ +DDeeffiinniittiioonn BayesNet-inst.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ +A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ +nodes. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ +_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h_:_:_p_u_s_h___b_a_c_k │ │ │ │ │ +IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor) │ │ │ │ │ +Add a factor directly using a shared_ptr. │ │ │ │ │ +DDeeffiinniittiioonn FactorGraph.h:186 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t │ │ │ │ │ +A SymbolicBayesNet is a Bayes Net of purely symbolic conditionals. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicBayesNet.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_:_:_~_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t │ │ │ │ │ +virtual ~SymbolicBayesNet() │ │ │ │ │ +Destructor. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicBayesNet.h:87 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t │ │ │ │ │ +SymbolicBayesNet(SymbolicConditional &&c) │ │ │ │ │ +Construct from a single conditional. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicBayesNet.h:71 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_:_:_p_r_i_n_t │ │ │ │ │ +GTSAM_EXPORT void print(const std::string &s="SymbolicBayesNet", const │ │ │ │ │ +KeyFormatter &formatter=DefaultKeyFormatter) const override │ │ │ │ │ +print │ │ │ │ │ +DDeeffiinniittiioonn SymbolicBayesNet.h:98 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t │ │ │ │ │ +SymbolicBayesNet(ITERATOR firstConditional, ITERATOR lastConditional) │ │ │ │ │ +Construct from iterator over conditionals. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicBayesNet.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t │ │ │ │ │ +SymbolicBayesNet(std::initializer_list< boost::shared_ptr< SymbolicConditional │ │ │ │ │ +> > conditionals) │ │ │ │ │ +Constructor that takes an initializer list of shared pointers. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicBayesNet.h:67 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t │ │ │ │ │ +SymbolicBayesNet(const CONTAINER &conditionals) │ │ │ │ │ +Construct from container of factors (shared_ptr or plain objects) │ │ │ │ │ +DDeeffiinniittiioonn SymbolicBayesNet.h:53 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t │ │ │ │ │ +SymbolicBayesNet() │ │ │ │ │ +Construct empty factor graph. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicBayesNet.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_:_:_a_c_c_e_s_s │ │ │ │ │ +friend class boost::serialization::access │ │ │ │ │ +Serialization function. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicBayesNet.h:108 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ +SymbolicBayesNet & operator()(SymbolicConditional &&c) │ │ │ │ │ +Add a single conditional and return a reference. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicBayesNet.h:81 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t │ │ │ │ │ +SymbolicBayesNet(const FactorGraph< DERIVEDCONDITIONAL > &graph) │ │ │ │ │ +Implicit copy/downcast constructor to override explicit template container │ │ │ │ │ +constructor. │ │ │ │ │ +DDeeffiinniittiioonn SymbolicBayesNet.h:60 │ │ │ │ │ +_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ +SymbolicConditional is a conditional with keys but no probability data, │ │ │ │ │ +produced by symbolic eliminat... │ │ │ │ │ +DDeeffiinniittiioonn SymbolicConditional.h:38 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _s_f_m │ │ │ │ │ - * _S_f_m_T_r_a_c_k_._h │ │ │ │ │ + * _s_y_m_b_o_l_i_c │ │ │ │ │ + * _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01211_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01208_source.html │ │ │ │┄ Files 99% similar despite different names │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
BinaryMeasurement.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
12#pragma once
│ │ │ │
13
│ │ │ │ -
26#include <gtsam/base/Testable.h>
│ │ │ │ -
27#include <gtsam/inference/Factor.h>
│ │ │ │ -
28#include <gtsam/inference/Key.h>
│ │ │ │ -
29#include <gtsam/linear/NoiseModel.h>
│ │ │ │ +
26#include <gtsam/base/Testable.h>
│ │ │ │ +
27#include <gtsam/inference/Factor.h>
│ │ │ │ +
28#include <gtsam/inference/Key.h>
│ │ │ │ +
29#include <gtsam/linear/NoiseModel.h>
│ │ │ │
30
│ │ │ │
31#include <iostream>
│ │ │ │
32#include <vector>
│ │ │ │
33
│ │ │ │
34namespace gtsam {
│ │ │ │
35
│ │ │ │
│ │ │ │ @@ -173,18 +173,18 @@ │ │ │ │
81 return e != nullptr && Factor::equals(*e) &&
│ │ │ │
82 traits<T>::Equals(this->measured_, e->measured_, tol) &&
│ │ │ │
83 noiseModel_->equals(*expected.noiseModel());
│ │ │ │
84 }
│ │ │ │
86};
│ │ │ │
│ │ │ │
87} // namespace gtsam
│ │ │ │ -
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ - │ │ │ │ -
Factor.h
The base class for all factors.
│ │ │ │ - │ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
Factor.h
The base class for all factors.
│ │ │ │ + │ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │
gtsam::IsTestable
A testable concept check that should be placed in applicable unit tests and in generic algorithms.
Definition Testable.h:58
│ │ │ │
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │ @@ -196,13 +196,13 @@ │ │ │ │
gtsam::BinaryMeasurement::print
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition BinaryMeasurement.h:70
│ │ │ │
gtsam::BinaryMeasurement::~BinaryMeasurement
virtual ~BinaryMeasurement()
Destructor.
Definition BinaryMeasurement.h:56
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -73,17 +73,17 @@ │ │ │ │ │ 82 _t_r_a_i_t_s_<_T_>_:_:_E_q_u_a_l_s(this->measured_, e->measured_, tol) && │ │ │ │ │ 83 noiseModel_->equals(*expected.noiseModel()); │ │ │ │ │ 84 } │ │ │ │ │ 86}; │ │ │ │ │ 87} // namespace gtsam │ │ │ │ │ _T_e_s_t_a_b_l_e_._h │ │ │ │ │ Concept check for values that can be used in unit tests. │ │ │ │ │ -_K_e_y_._h │ │ │ │ │ _F_a_c_t_o_r_._h │ │ │ │ │ The base class for all factors. │ │ │ │ │ +_K_e_y_._h │ │ │ │ │ _N_o_i_s_e_M_o_d_e_l_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ _g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ Aliases. │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01217.js │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01226.js │ │ │ │┄ Files 80% similar despite different names │ │ │ │ ├── js-beautify {} │ │ │ │ │ @@ -1,12 +1,12 @@ │ │ │ │ │ -var a01217 = [ │ │ │ │ │ +var a01226 = [ │ │ │ │ │ ["gtsam::traits< SfmData >", "a04708.html", null], │ │ │ │ │ - ["SfmCamera", "a01217.html#acab0207730cf78822bdf274b50739d31", null], │ │ │ │ │ - ["gtsam2openGL", "a01217.html#ad632ed56581bf174eef379bbe272a06a", null], │ │ │ │ │ - ["gtsam2openGL", "a01217.html#aaff07ef70e6161c4d1d2bd7546ff91e6", null], │ │ │ │ │ - ["initialCamerasAndPointsEstimate", "a01217.html#aba34ed4a103d9057536ee5cac27cfdef", null], │ │ │ │ │ - ["initialCamerasEstimate", "a01217.html#ad8b55993353cbab0fde06b872783fe92", null], │ │ │ │ │ - ["openGL2gtsam", "a01217.html#ade6eca65dc0168a65f641961ca97924c", null], │ │ │ │ │ - ["readBal", "a01217.html#a710ec0322f4a2c0883841b58516b7a6a", null], │ │ │ │ │ - ["writeBAL", "a01217.html#a43c2c2ead4fddd79d9c3f9a5bfa764ae", null], │ │ │ │ │ - ["writeBALfromValues", "a01217.html#a4a2433812cf34ee359e3ec99f4f5bcc4", null] │ │ │ │ │ + ["SfmCamera", "a01226.html#acab0207730cf78822bdf274b50739d31", null], │ │ │ │ │ + ["gtsam2openGL", "a01226.html#ad632ed56581bf174eef379bbe272a06a", null], │ │ │ │ │ + ["gtsam2openGL", "a01226.html#aaff07ef70e6161c4d1d2bd7546ff91e6", null], │ │ │ │ │ + ["initialCamerasAndPointsEstimate", "a01226.html#aba34ed4a103d9057536ee5cac27cfdef", null], │ │ │ │ │ + ["initialCamerasEstimate", "a01226.html#ad8b55993353cbab0fde06b872783fe92", null], │ │ │ │ │ + ["openGL2gtsam", "a01226.html#ade6eca65dc0168a65f641961ca97924c", null], │ │ │ │ │ + ["readBal", "a01226.html#a710ec0322f4a2c0883841b58516b7a6a", null], │ │ │ │ │ + ["writeBAL", "a01226.html#a43c2c2ead4fddd79d9c3f9a5bfa764ae", null], │ │ │ │ │ + ["writeBALfromValues", "a01226.html#a4a2433812cf34ee359e3ec99f4f5bcc4", null] │ │ │ │ │ ]; │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01217_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00929_source.html │ │ │ │┄ Files 76% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/SfmData.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/PreintegrationParams.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
SfmData.h
│ │ │ │ +
PreintegrationParams.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/geometry/Cal3Bundler.h>
│ │ │ │ -
22#include <gtsam/geometry/PinholeCamera.h>
│ │ │ │ -
23#include <gtsam/nonlinear/NonlinearFactorGraph.h>
│ │ │ │ -
24#include <gtsam/nonlinear/Values.h>
│ │ │ │ -
25#include <gtsam/sfm/SfmTrack.h>
│ │ │ │ -
26
│ │ │ │ -
27#include <string>
│ │ │ │ -
28#include <vector>
│ │ │ │ -
29
│ │ │ │ -
30namespace gtsam {
│ │ │ │ -
31
│ │ │ │ -
33typedef PinholeCamera<Cal3Bundler> SfmCamera;
│ │ │ │ -
34
│ │ │ │ -
│ │ │ │ -
39struct GTSAM_EXPORT SfmData {
│ │ │ │ -
40 std::vector<SfmCamera> cameras;
│ │ │ │ +
17#pragma once
│ │ │ │ +
18
│ │ │ │ +
19#include <gtsam/navigation/PreintegratedRotation.h>
│ │ │ │ +
20#include <boost/make_shared.hpp>
│ │ │ │ +
21
│ │ │ │ +
22namespace gtsam {
│ │ │ │ +
23
│ │ │ │ +
│ │ │ │ +
26struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
│ │ │ │ +
29 Matrix3 accelerometerCovariance;
│ │ │ │ +
30 Matrix3 integrationCovariance;
│ │ │ │ +
31 bool use2ndOrderCoriolis;
│ │ │ │ +
32 Vector3 n_gravity;
│ │ │ │ +
33
│ │ │ │ +
│ │ │ │ +
35 PreintegrationParams()
│ │ │ │ +
36 : PreintegratedRotationParams(),
│ │ │ │ +
37 accelerometerCovariance(I_3x3),
│ │ │ │ +
38 integrationCovariance(I_3x3),
│ │ │ │ +
39 use2ndOrderCoriolis(false),
│ │ │ │ +
40 n_gravity(0, 0, -1) {}
│ │ │ │ +
│ │ │ │
41
│ │ │ │ -
42 std::vector<SfmTrack> tracks;
│ │ │ │ -
43
│ │ │ │ -
46
│ │ │ │ -
53 static SfmData FromBundlerFile(const std::string& filename);
│ │ │ │ -
54
│ │ │ │ -
61 static SfmData FromBalFile(const std::string& filename);
│ │ │ │ -
62
│ │ │ │ -
66
│ │ │ │ -
68 void addTrack(const SfmTrack& t) { tracks.push_back(t); }
│ │ │ │ -
69
│ │ │ │ -
71 void addCamera(const SfmCamera& cam) { cameras.push_back(cam); }
│ │ │ │ +
│ │ │ │ +
44 PreintegrationParams(const Vector3& n_gravity)
│ │ │ │ +
45 : PreintegratedRotationParams(),
│ │ │ │ +
46 accelerometerCovariance(I_3x3),
│ │ │ │ +
47 integrationCovariance(I_3x3),
│ │ │ │ +
48 use2ndOrderCoriolis(false),
│ │ │ │ +
49 n_gravity(n_gravity) {}
│ │ │ │ +
│ │ │ │ +
50
│ │ │ │ +
51 // Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
│ │ │ │ +
52 static boost::shared_ptr<PreintegrationParams> MakeSharedD(double g = 9.81) {
│ │ │ │ +
53 return boost::shared_ptr<PreintegrationParams>(new PreintegrationParams(Vector3(0, 0, g)));
│ │ │ │ +
54 }
│ │ │ │ +
55
│ │ │ │ +
56 // Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
│ │ │ │ +
57 static boost::shared_ptr<PreintegrationParams> MakeSharedU(double g = 9.81) {
│ │ │ │ +
58 return boost::shared_ptr<PreintegrationParams>(new PreintegrationParams(Vector3(0, 0, -g)));
│ │ │ │ +
59 }
│ │ │ │ +
60
│ │ │ │ +
61 void print(const std::string& s="") const override;
│ │ │ │ +
62 bool equals(const PreintegratedRotationParams& other, double tol) const override;
│ │ │ │ +
63
│ │ │ │ +
64 void setAccelerometerCovariance(const Matrix3& cov) { accelerometerCovariance = cov; }
│ │ │ │ +
65 void setIntegrationCovariance(const Matrix3& cov) { integrationCovariance = cov; }
│ │ │ │ +
66 void setUse2ndOrderCoriolis(bool flag) { use2ndOrderCoriolis = flag; }
│ │ │ │ +
67
│ │ │ │ +
68 const Matrix3& getAccelerometerCovariance() const { return accelerometerCovariance; }
│ │ │ │ +
69 const Matrix3& getIntegrationCovariance() const { return integrationCovariance; }
│ │ │ │ +
70 const Vector3& getGravity() const { return n_gravity; }
│ │ │ │ +
71 bool getUse2ndOrderCoriolis() const { return use2ndOrderCoriolis; }
│ │ │ │
72
│ │ │ │ -
74 size_t numberTracks() const { return tracks.size(); }
│ │ │ │ -
75
│ │ │ │ -
77 size_t numberCameras() const { return cameras.size(); }
│ │ │ │ -
78
│ │ │ │ -
80 const SfmTrack& track(size_t idx) const { return tracks[idx]; }
│ │ │ │ -
81
│ │ │ │ -
83 const SfmCamera& camera(size_t idx) const { return cameras[idx]; }
│ │ │ │ -
84
│ │ │ │ -
86 const std::vector<SfmCamera>& cameraList() const { return cameras; }
│ │ │ │ -
87 const std::vector<SfmTrack>& trackList() const { return tracks; }
│ │ │ │ -
88
│ │ │ │ -
95 NonlinearFactorGraph generalSfmFactors(
│ │ │ │ -
96 const SharedNoiseModel& model = noiseModel::Isotropic::Sigma(2,
│ │ │ │ -
97 1.0)) const;
│ │ │ │ -
98
│ │ │ │ -
109 NonlinearFactorGraph sfmFactorGraph(
│ │ │ │ -
110 const SharedNoiseModel& model = noiseModel::Isotropic::Sigma(2, 1.0),
│ │ │ │ -
111 boost::optional<size_t> fixedCamera = 0,
│ │ │ │ -
112 boost::optional<size_t> fixedPoint = 0) const;
│ │ │ │ -
113
│ │ │ │ -
117
│ │ │ │ -
119 void print(const std::string& s = "") const;
│ │ │ │ -
120
│ │ │ │ -
122 bool equals(const SfmData& sfmData, double tol = 1e-9) const;
│ │ │ │ -
123
│ │ │ │ -
125#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ -
128 void GTSAM_DEPRECATED add_track(const SfmTrack& t) { tracks.push_back(t); }
│ │ │ │ -
129 void GTSAM_DEPRECATED add_camera(const SfmCamera& cam) {
│ │ │ │ -
130 cameras.push_back(cam);
│ │ │ │ -
131 }
│ │ │ │ -
132 size_t GTSAM_DEPRECATED number_tracks() const { return tracks.size(); }
│ │ │ │ -
133 size_t GTSAM_DEPRECATED number_cameras() const { return cameras.size(); }
│ │ │ │ -
135#endif
│ │ │ │ -
138
│ │ │ │ -
140 friend class boost::serialization::access;
│ │ │ │ -
141 template <class Archive>
│ │ │ │ -
142 void serialize(Archive& ar, const unsigned int /*version*/) {
│ │ │ │ -
143 ar& BOOST_SERIALIZATION_NVP(cameras);
│ │ │ │ -
144 ar& BOOST_SERIALIZATION_NVP(tracks);
│ │ │ │ -
145 }
│ │ │ │ -
146
│ │ │ │ -
148};
│ │ │ │ -
│ │ │ │ -
149
│ │ │ │ -
151template <>
│ │ │ │ -
152struct traits<SfmData> : public Testable<SfmData> {};
│ │ │ │ -
153
│ │ │ │ -
154#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
│ │ │ │ -
155GTSAM_EXPORT bool GTSAM_DEPRECATED readBundler(const std::string& filename,
│ │ │ │ -
156 SfmData& data);
│ │ │ │ -
157GTSAM_EXPORT bool GTSAM_DEPRECATED readBAL(const std::string& filename,
│ │ │ │ -
158 SfmData& data);
│ │ │ │ -
159#endif
│ │ │ │ -
160
│ │ │ │ -
167GTSAM_EXPORT SfmData readBal(const std::string& filename);
│ │ │ │ -
168
│ │ │ │ -
176GTSAM_EXPORT bool writeBAL(const std::string& filename, const SfmData& data);
│ │ │ │ -
177
│ │ │ │ -
190GTSAM_EXPORT bool writeBALfromValues(const std::string& filename,
│ │ │ │ -
191 const SfmData& data, const Values& values);
│ │ │ │ -
192
│ │ │ │ -
201GTSAM_EXPORT Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz);
│ │ │ │ -
202
│ │ │ │ -
211GTSAM_EXPORT Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz);
│ │ │ │ -
212
│ │ │ │ -
218GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM);
│ │ │ │ -
219
│ │ │ │ -
228GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db);
│ │ │ │ -
229
│ │ │ │ -
238GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db);
│ │ │ │ -
239
│ │ │ │ -
240} // namespace gtsam
│ │ │ │ -
Cal3Bundler.h
Calibration used by Bundler.
│ │ │ │ -
PinholeCamera.h
Base class for all pinhole cameras.
│ │ │ │ -
Values.h
A non-templated config holding any types of Manifold-group elements.
│ │ │ │ -
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
│ │ │ │ -
SfmTrack.h
A simple data structure for a track in Structure from Motion.
│ │ │ │ +
73protected:
│ │ │ │ +
74
│ │ │ │ +
76 friend class boost::serialization::access;
│ │ │ │ +
77 template<class ARCHIVE>
│ │ │ │ +
78 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ +
79 namespace bs = ::boost::serialization;
│ │ │ │ +
80 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotationParams);
│ │ │ │ +
81 ar & BOOST_SERIALIZATION_NVP(accelerometerCovariance);
│ │ │ │ +
82 ar & BOOST_SERIALIZATION_NVP(integrationCovariance);
│ │ │ │ +
83 ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);
│ │ │ │ +
84 ar & BOOST_SERIALIZATION_NVP(n_gravity);
│ │ │ │ +
85 }
│ │ │ │ +
86
│ │ │ │ +
87#ifdef GTSAM_USE_QUATERNIONS
│ │ │ │ +
88 // Align if we are using Quaternions
│ │ │ │ +
89public:
│ │ │ │ +
90 GTSAM_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
91#endif
│ │ │ │ +
92};
│ │ │ │ +
│ │ │ │ +
93
│ │ │ │ +
94} // namespace gtsam
│ │ │ │ +
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
│ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::writeBAL
bool writeBAL(const std::string &filename, const SfmData &data)
This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure.
Definition SfmData.cpp:249
│ │ │ │ -
gtsam::writeBALfromValues
bool writeBALfromValues(const std::string &filename, const SfmData &data, const Values &values)
This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure and a val...
Definition SfmData.cpp:349
│ │ │ │ -
gtsam::readBal
SfmData readBal(const std::string &filename)
This function parses a "Bundle Adjustment in the Large" (BAL) file and returns the data as a SfmData ...
Definition SfmData.cpp:344
│ │ │ │ -
gtsam::gtsam2openGL
Pose3 gtsam2openGL(const Rot3 &R, double tx, double ty, double tz)
This function converts a GTSAM camera pose to an openGL camera pose.
Definition SfmData.cpp:88
│ │ │ │ -
gtsam::initialCamerasAndPointsEstimate
Values initialCamerasAndPointsEstimate(const SfmData &db)
This function creates initial values for cameras and points from db.
Definition SfmData.cpp:449
│ │ │ │ -
gtsam::SfmCamera
PinholeCamera< Cal3Bundler > SfmCamera
Define the structure for the camera poses.
Definition SfmData.h:33
│ │ │ │ -
gtsam::initialCamerasEstimate
Values initialCamerasEstimate(const SfmData &db)
This function creates initial values for cameras from db.
Definition SfmData.cpp:441
│ │ │ │ -
gtsam::openGL2gtsam
Pose3 openGL2gtsam(const Rot3 &R, double tx, double ty, double tz)
This function converts an openGL camera pose to an GTSAM camera pose.
Definition SfmData.cpp:79
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::PinholeCamera
A pinhole camera class that has a Pose3 and a Calibration.
Definition PinholeCamera.h:33
│ │ │ │ -
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
│ │ │ │ -
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
│ │ │ │ -
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
gtsam::SfmData
SfmData stores a bunch of SfmTracks.
Definition SfmData.h:39
│ │ │ │ -
gtsam::SfmData::cameraList
const std::vector< SfmCamera > & cameraList() const
Getters.
Definition SfmData.h:86
│ │ │ │ -
gtsam::SfmData::addCamera
void addCamera(const SfmCamera &cam)
Add a camera to SfmData.
Definition SfmData.h:71
│ │ │ │ -
gtsam::SfmData::numberCameras
size_t numberCameras() const
The number of cameras.
Definition SfmData.h:77
│ │ │ │ -
gtsam::SfmData::numberTracks
size_t numberTracks() const
The number of reconstructed 3D points.
Definition SfmData.h:74
│ │ │ │ -
gtsam::SfmData::track
const SfmTrack & track(size_t idx) const
The track formed by series of landmark measurements.
Definition SfmData.h:80
│ │ │ │ -
gtsam::SfmData::addTrack
void addTrack(const SfmTrack &t)
Add a track to SfmData.
Definition SfmData.h:68
│ │ │ │ -
gtsam::SfmData::camera
const SfmCamera & camera(size_t idx) const
The camera pose at frame index idx
Definition SfmData.h:83
│ │ │ │ -
gtsam::SfmData::cameras
std::vector< SfmCamera > cameras
Set of cameras.
Definition SfmData.h:40
│ │ │ │ -
gtsam::SfmData::tracks
std::vector< SfmTrack > tracks
Sparse set of points.
Definition SfmData.h:42
│ │ │ │ -
gtsam::SfmTrack
Definition SfmTrack.h:126
│ │ │ │ +
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
│ │ │ │ +
gtsam::PreintegratedRotationParams
Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
Definition PreintegratedRotation.h:31
│ │ │ │ +
gtsam::PreintegrationParams
Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the c...
Definition PreintegrationParams.h:26
│ │ │ │ +
gtsam::PreintegrationParams::accelerometerCovariance
Matrix3 accelerometerCovariance
Continuous-time "Covariance" of accelerometer The units for stddev are σ = m/s²/√Hz.
Definition PreintegrationParams.h:29
│ │ │ │ +
gtsam::PreintegrationParams::use2ndOrderCoriolis
bool use2ndOrderCoriolis
Whether to use second order Coriolis integration.
Definition PreintegrationParams.h:31
│ │ │ │ +
gtsam::PreintegrationParams::PreintegrationParams
PreintegrationParams()
Default constructor for serialization only.
Definition PreintegrationParams.h:35
│ │ │ │ +
gtsam::PreintegrationParams::n_gravity
Vector3 n_gravity
Gravity vector in nav frame.
Definition PreintegrationParams.h:32
│ │ │ │ +
gtsam::PreintegrationParams::PreintegrationParams
PreintegrationParams(const Vector3 &n_gravity)
The Params constructor insists on getting the navigation frame gravity vector For convenience,...
Definition PreintegrationParams.h:44
│ │ │ │ +
gtsam::PreintegrationParams::integrationCovariance
Matrix3 integrationCovariance
continuous-time "Covariance" describing integration uncertainty
Definition PreintegrationParams.h:30
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── encoding │ │ │ │ │ @@ -1 +1 @@ │ │ │ │ │ -us-ascii │ │ │ │ │ +utf-8 │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,238 +1,151 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SfmData.h │ │ │ │ │ +PreintegrationParams.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_C_a_l_3_B_u_n_d_l_e_r_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_P_i_n_h_o_l_e_C_a_m_e_r_a_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_V_a_l_u_e_s_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_s_f_m_/_S_f_m_T_r_a_c_k_._h> │ │ │ │ │ -26 │ │ │ │ │ -27#include │ │ │ │ │ -28#include │ │ │ │ │ -29 │ │ │ │ │ -30namespace _g_t_s_a_m { │ │ │ │ │ -31 │ │ │ │ │ -_3_3typedef _P_i_n_h_o_l_e_C_a_m_e_r_a_<_C_a_l_3_B_u_n_d_l_e_r_> _S_f_m_C_a_m_e_r_a; │ │ │ │ │ -34 │ │ │ │ │ -_3_9struct GTSAM_EXPORT _S_f_m_D_a_t_a { │ │ │ │ │ -_4_0 std::vector _c_a_m_e_r_a_s; │ │ │ │ │ +17#pragma once │ │ │ │ │ +18 │ │ │ │ │ +19#include <_g_t_s_a_m_/_n_a_v_i_g_a_t_i_o_n_/_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_._h> │ │ │ │ │ +20#include │ │ │ │ │ +21 │ │ │ │ │ +22namespace _g_t_s_a_m { │ │ │ │ │ +23 │ │ │ │ │ +_2_6struct GTSAM_EXPORT _P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s: _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s { │ │ │ │ │ +_2_9 Matrix3 _a_c_c_e_l_e_r_o_m_e_t_e_r_C_o_v_a_r_i_a_n_c_e; │ │ │ │ │ +_3_0 Matrix3 _i_n_t_e_g_r_a_t_i_o_n_C_o_v_a_r_i_a_n_c_e; │ │ │ │ │ +_3_1 bool _u_s_e_2_n_d_O_r_d_e_r_C_o_r_i_o_l_i_s; │ │ │ │ │ +_3_2 Vector3 _n___g_r_a_v_i_t_y; │ │ │ │ │ +33 │ │ │ │ │ +_3_5 _P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s() │ │ │ │ │ +36 : _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s(), │ │ │ │ │ +37 accelerometerCovariance(I_3x3), │ │ │ │ │ +38 integrationCovariance(I_3x3), │ │ │ │ │ +39 use2ndOrderCoriolis(false), │ │ │ │ │ +40 n_gravity(0, 0, -1) {} │ │ │ │ │ 41 │ │ │ │ │ -_4_2 std::vector _t_r_a_c_k_s; │ │ │ │ │ -43 │ │ │ │ │ -46 │ │ │ │ │ -53 static _S_f_m_D_a_t_a FromBundlerFile(const std::string& filename); │ │ │ │ │ -54 │ │ │ │ │ -61 static _S_f_m_D_a_t_a FromBalFile(const std::string& filename); │ │ │ │ │ -62 │ │ │ │ │ -66 │ │ │ │ │ -_6_8 void _a_d_d_T_r_a_c_k(const _S_f_m_T_r_a_c_k& t) { tracks.push_back(t); } │ │ │ │ │ -69 │ │ │ │ │ -_7_1 void _a_d_d_C_a_m_e_r_a(const _S_f_m_C_a_m_e_r_a& cam) { cameras.push_back(cam); } │ │ │ │ │ +_4_4 _P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s(const Vector3& n_gravity) │ │ │ │ │ +45 : _P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s(), │ │ │ │ │ +46 accelerometerCovariance(I_3x3), │ │ │ │ │ +47 integrationCovariance(I_3x3), │ │ │ │ │ +48 use2ndOrderCoriolis(false), │ │ │ │ │ +49 n_gravity(n_gravity) {} │ │ │ │ │ +50 │ │ │ │ │ +51 // Default Params for a Z-down navigation frame, such as NED: gravity points │ │ │ │ │ +along positive Z-axis │ │ │ │ │ +52 static boost::shared_ptr MakeSharedD(double g = 9.81) │ │ │ │ │ +{ │ │ │ │ │ +53 return boost::shared_ptr(new _P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ +(Vector3(0, 0, g))); │ │ │ │ │ +54 } │ │ │ │ │ +55 │ │ │ │ │ +56 // Default Params for a Z-up navigation frame, such as ENU: gravity points │ │ │ │ │ +along negative Z-axis │ │ │ │ │ +57 static boost::shared_ptr MakeSharedU(double g = 9.81) │ │ │ │ │ +{ │ │ │ │ │ +58 return boost::shared_ptr(new PreintegrationParams │ │ │ │ │ +(Vector3(0, 0, -g))); │ │ │ │ │ +59 } │ │ │ │ │ +60 │ │ │ │ │ +61 void _p_r_i_n_t(const std::string& s="") const override; │ │ │ │ │ +62 bool equals(const PreintegratedRotationParams& other, double tol) const │ │ │ │ │ +override; │ │ │ │ │ +63 │ │ │ │ │ +64 void setAccelerometerCovariance(const Matrix3& cov) │ │ │ │ │ +{ accelerometerCovariance = cov; } │ │ │ │ │ +65 void setIntegrationCovariance(const Matrix3& cov) { integrationCovariance = │ │ │ │ │ +cov; } │ │ │ │ │ +66 void setUse2ndOrderCoriolis(bool flag) { use2ndOrderCoriolis = flag; } │ │ │ │ │ +67 │ │ │ │ │ +68 const Matrix3& getAccelerometerCovariance() const { return │ │ │ │ │ +accelerometerCovariance; } │ │ │ │ │ +69 const Matrix3& getIntegrationCovariance() const { return │ │ │ │ │ +integrationCovariance; } │ │ │ │ │ +70 const Vector3& getGravity() const { return n_gravity; } │ │ │ │ │ +71 bool getUse2ndOrderCoriolis() const { return use2ndOrderCoriolis; } │ │ │ │ │ 72 │ │ │ │ │ -_7_4 size_t _n_u_m_b_e_r_T_r_a_c_k_s() const { return tracks.size(); } │ │ │ │ │ -75 │ │ │ │ │ -_7_7 size_t _n_u_m_b_e_r_C_a_m_e_r_a_s() const { return cameras.size(); } │ │ │ │ │ -78 │ │ │ │ │ -_8_0 const _S_f_m_T_r_a_c_k& _t_r_a_c_k(size_t idx) const { return tracks[idx]; } │ │ │ │ │ -81 │ │ │ │ │ -_8_3 const _S_f_m_C_a_m_e_r_a& _c_a_m_e_r_a(size_t idx) const { return cameras[idx]; } │ │ │ │ │ -84 │ │ │ │ │ -_8_6 const std::vector& _c_a_m_e_r_a_L_i_s_t() const { return cameras; } │ │ │ │ │ -87 const std::vector& trackList() const { return tracks; } │ │ │ │ │ -88 │ │ │ │ │ -95 NonlinearFactorGraph generalSfmFactors( │ │ │ │ │ -96 const SharedNoiseModel& model = noiseModel::Isotropic::Sigma(2, │ │ │ │ │ -97 1.0)) const; │ │ │ │ │ -98 │ │ │ │ │ -109 NonlinearFactorGraph sfmFactorGraph( │ │ │ │ │ -110 const SharedNoiseModel& model = noiseModel::Isotropic::Sigma(2, 1.0), │ │ │ │ │ -111 boost::optional fixedCamera = 0, │ │ │ │ │ -112 boost::optional fixedPoint = 0) const; │ │ │ │ │ -113 │ │ │ │ │ -117 │ │ │ │ │ -119 void print(const std::string& s = "") const; │ │ │ │ │ -120 │ │ │ │ │ -122 bool equals(const SfmData& sfmData, double tol = 1e-9) const; │ │ │ │ │ -123 │ │ │ │ │ -125#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ -128 void GTSAM_DEPRECATED add_track(const SfmTrack& t) { tracks.push_back(t); } │ │ │ │ │ -129 void GTSAM_DEPRECATED add_camera(const SfmCamera& cam) { │ │ │ │ │ -130 cameras.push_back(cam); │ │ │ │ │ -131 } │ │ │ │ │ -132 size_t GTSAM_DEPRECATED number_tracks() const { return tracks.size(); } │ │ │ │ │ -133 size_t GTSAM_DEPRECATED number_cameras() const { return cameras.size(); } │ │ │ │ │ -135#endif │ │ │ │ │ -138 │ │ │ │ │ -_1_4_0 friend class boost::serialization::access; │ │ │ │ │ -141 template │ │ │ │ │ -142 void serialize(Archive& ar, const unsigned int /*version*/) { │ │ │ │ │ -143 ar& BOOST_SERIALIZATION_NVP(cameras); │ │ │ │ │ -144 ar& BOOST_SERIALIZATION_NVP(tracks); │ │ │ │ │ -145 } │ │ │ │ │ -146 │ │ │ │ │ -148}; │ │ │ │ │ -149 │ │ │ │ │ -151template <> │ │ │ │ │ -_1_5_2struct _t_r_a_i_t_s<_S_f_m_D_a_t_a> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ -153 │ │ │ │ │ -154#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 │ │ │ │ │ -155GTSAM_EXPORT bool GTSAM_DEPRECATED readBundler(const std::string& filename, │ │ │ │ │ -156 _S_f_m_D_a_t_a& data); │ │ │ │ │ -157GTSAM_EXPORT bool GTSAM_DEPRECATED readBAL(const std::string& filename, │ │ │ │ │ -158 _S_f_m_D_a_t_a& data); │ │ │ │ │ -159#endif │ │ │ │ │ -160 │ │ │ │ │ -167GTSAM_EXPORT _S_f_m_D_a_t_a _r_e_a_d_B_a_l(const std::string& filename); │ │ │ │ │ -168 │ │ │ │ │ -176GTSAM_EXPORT bool _w_r_i_t_e_B_A_L(const std::string& filename, const _S_f_m_D_a_t_a& │ │ │ │ │ -data); │ │ │ │ │ -177 │ │ │ │ │ -190GTSAM_EXPORT bool _w_r_i_t_e_B_A_L_f_r_o_m_V_a_l_u_e_s(const std::string& filename, │ │ │ │ │ -191 const _S_f_m_D_a_t_a& data, const _V_a_l_u_e_s& values); │ │ │ │ │ -192 │ │ │ │ │ -201GTSAM_EXPORT _P_o_s_e_3 _o_p_e_n_G_L_2_g_t_s_a_m(const _R_o_t_3& R, double tx, double ty, double │ │ │ │ │ -tz); │ │ │ │ │ -202 │ │ │ │ │ -211GTSAM_EXPORT _P_o_s_e_3 _g_t_s_a_m_2_o_p_e_n_G_L(const _R_o_t_3& R, double tx, double ty, double │ │ │ │ │ -tz); │ │ │ │ │ -212 │ │ │ │ │ -218GTSAM_EXPORT _P_o_s_e_3 _g_t_s_a_m_2_o_p_e_n_G_L(const _P_o_s_e_3& PoseGTSAM); │ │ │ │ │ -219 │ │ │ │ │ -228GTSAM_EXPORT _V_a_l_u_e_s _i_n_i_t_i_a_l_C_a_m_e_r_a_s_E_s_t_i_m_a_t_e(const _S_f_m_D_a_t_a& db); │ │ │ │ │ -229 │ │ │ │ │ -238GTSAM_EXPORT _V_a_l_u_e_s _i_n_i_t_i_a_l_C_a_m_e_r_a_s_A_n_d_P_o_i_n_t_s_E_s_t_i_m_a_t_e(const _S_f_m_D_a_t_a& db); │ │ │ │ │ -239 │ │ │ │ │ -240} // namespace gtsam │ │ │ │ │ -_C_a_l_3_B_u_n_d_l_e_r_._h │ │ │ │ │ -Calibration used by Bundler. │ │ │ │ │ -_P_i_n_h_o_l_e_C_a_m_e_r_a_._h │ │ │ │ │ -Base class for all pinhole cameras. │ │ │ │ │ -_V_a_l_u_e_s_._h │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ -_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Factor Graph consisting of non-linear factors. │ │ │ │ │ -_S_f_m_T_r_a_c_k_._h │ │ │ │ │ -A simple data structure for a track in Structure from Motion. │ │ │ │ │ +73protected: │ │ │ │ │ +74 │ │ │ │ │ +_7_6 friend class boost::serialization::access; │ │ │ │ │ +77 template │ │ │ │ │ +78 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ +79 namespace bs = ::boost::serialization; │ │ │ │ │ +80 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s); │ │ │ │ │ +81 ar & BOOST_SERIALIZATION_NVP(accelerometerCovariance); │ │ │ │ │ +82 ar & BOOST_SERIALIZATION_NVP(integrationCovariance); │ │ │ │ │ +83 ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis); │ │ │ │ │ +84 ar & BOOST_SERIALIZATION_NVP(n_gravity); │ │ │ │ │ +85 } │ │ │ │ │ +86 │ │ │ │ │ +87#ifdef GTSAM_USE_QUATERNIONS │ │ │ │ │ +88 // Align if we are using Quaternions │ │ │ │ │ +89public: │ │ │ │ │ +90 _G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +91#endif │ │ │ │ │ +92}; │ │ │ │ │ +93 │ │ │ │ │ +94} // namespace gtsam │ │ │ │ │ +_G_T_S_A_M___M_A_K_E___A_L_I_G_N_E_D___O_P_E_R_A_T_O_R___N_E_W │ │ │ │ │ +#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ +This marks a GTSAM object to require alignment. │ │ │ │ │ +DDeeffiinniittiioonn types.h:308 │ │ │ │ │ +_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_w_r_i_t_e_B_A_L │ │ │ │ │ -bool writeBAL(const std::string &filename, const SfmData &data) │ │ │ │ │ -This function writes a "Bundle Adjustment in the Large" (BAL) file from a │ │ │ │ │ -SfmData structure. │ │ │ │ │ -DDeeffiinniittiioonn SfmData.cpp:249 │ │ │ │ │ -_g_t_s_a_m_:_:_w_r_i_t_e_B_A_L_f_r_o_m_V_a_l_u_e_s │ │ │ │ │ -bool writeBALfromValues(const std::string &filename, const SfmData &data, const │ │ │ │ │ -Values &values) │ │ │ │ │ -This function writes a "Bundle Adjustment in the Large" (BAL) file from a │ │ │ │ │ -SfmData structure and a val... │ │ │ │ │ -DDeeffiinniittiioonn SfmData.cpp:349 │ │ │ │ │ -_g_t_s_a_m_:_:_r_e_a_d_B_a_l │ │ │ │ │ -SfmData readBal(const std::string &filename) │ │ │ │ │ -This function parses a "Bundle Adjustment in the Large" (BAL) file and returns │ │ │ │ │ -the data as a SfmData ... │ │ │ │ │ -DDeeffiinniittiioonn SfmData.cpp:344 │ │ │ │ │ -_g_t_s_a_m_:_:_g_t_s_a_m_2_o_p_e_n_G_L │ │ │ │ │ -Pose3 gtsam2openGL(const Rot3 &R, double tx, double ty, double tz) │ │ │ │ │ -This function converts a GTSAM camera pose to an openGL camera pose. │ │ │ │ │ -DDeeffiinniittiioonn SfmData.cpp:88 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_i_t_i_a_l_C_a_m_e_r_a_s_A_n_d_P_o_i_n_t_s_E_s_t_i_m_a_t_e │ │ │ │ │ -Values initialCamerasAndPointsEstimate(const SfmData &db) │ │ │ │ │ -This function creates initial values for cameras and points from db. │ │ │ │ │ -DDeeffiinniittiioonn SfmData.cpp:449 │ │ │ │ │ -_g_t_s_a_m_:_:_S_f_m_C_a_m_e_r_a │ │ │ │ │ -PinholeCamera< Cal3Bundler > SfmCamera │ │ │ │ │ -Define the structure for the camera poses. │ │ │ │ │ -DDeeffiinniittiioonn SfmData.h:33 │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_i_t_i_a_l_C_a_m_e_r_a_s_E_s_t_i_m_a_t_e │ │ │ │ │ -Values initialCamerasEstimate(const SfmData &db) │ │ │ │ │ -This function creates initial values for cameras from db. │ │ │ │ │ -DDeeffiinniittiioonn SfmData.cpp:441 │ │ │ │ │ -_g_t_s_a_m_:_:_o_p_e_n_G_L_2_g_t_s_a_m │ │ │ │ │ -Pose3 openGL2gtsam(const Rot3 &R, double tx, double ty, double tz) │ │ │ │ │ -This function converts an openGL camera pose to an GTSAM camera pose. │ │ │ │ │ -DDeeffiinniittiioonn SfmData.cpp:79 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_P_i_n_h_o_l_e_C_a_m_e_r_a │ │ │ │ │ -A pinhole camera class that has a Pose3 and a Calibration. │ │ │ │ │ -DDeeffiinniittiioonn PinholeCamera.h:33 │ │ │ │ │ -_g_t_s_a_m_:_:_P_o_s_e_3 │ │ │ │ │ -A 3D pose (R,t) : (Rot3,Point3) │ │ │ │ │ -DDeeffiinniittiioonn Pose3.h:37 │ │ │ │ │ -_g_t_s_a_m_:_:_R_o_t_3 │ │ │ │ │ -Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor │ │ │ │ │ -symbol GTSAM_USE_QUATERNIO... │ │ │ │ │ -DDeeffiinniittiioonn Rot3.h:58 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_S_f_m_D_a_t_a │ │ │ │ │ -SfmData stores a bunch of SfmTracks. │ │ │ │ │ -DDeeffiinniittiioonn SfmData.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_S_f_m_D_a_t_a_:_:_c_a_m_e_r_a_L_i_s_t │ │ │ │ │ -const std::vector< SfmCamera > & cameraList() const │ │ │ │ │ -Getters. │ │ │ │ │ -DDeeffiinniittiioonn SfmData.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_S_f_m_D_a_t_a_:_:_a_d_d_C_a_m_e_r_a │ │ │ │ │ -void addCamera(const SfmCamera &cam) │ │ │ │ │ -Add a camera to SfmData. │ │ │ │ │ -DDeeffiinniittiioonn SfmData.h:71 │ │ │ │ │ -_g_t_s_a_m_:_:_S_f_m_D_a_t_a_:_:_n_u_m_b_e_r_C_a_m_e_r_a_s │ │ │ │ │ -size_t numberCameras() const │ │ │ │ │ -The number of cameras. │ │ │ │ │ -DDeeffiinniittiioonn SfmData.h:77 │ │ │ │ │ -_g_t_s_a_m_:_:_S_f_m_D_a_t_a_:_:_n_u_m_b_e_r_T_r_a_c_k_s │ │ │ │ │ -size_t numberTracks() const │ │ │ │ │ -The number of reconstructed 3D points. │ │ │ │ │ -DDeeffiinniittiioonn SfmData.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_S_f_m_D_a_t_a_:_:_t_r_a_c_k │ │ │ │ │ -const SfmTrack & track(size_t idx) const │ │ │ │ │ -The track formed by series of landmark measurements. │ │ │ │ │ -DDeeffiinniittiioonn SfmData.h:80 │ │ │ │ │ -_g_t_s_a_m_:_:_S_f_m_D_a_t_a_:_:_a_d_d_T_r_a_c_k │ │ │ │ │ -void addTrack(const SfmTrack &t) │ │ │ │ │ -Add a track to SfmData. │ │ │ │ │ -DDeeffiinniittiioonn SfmData.h:68 │ │ │ │ │ -_g_t_s_a_m_:_:_S_f_m_D_a_t_a_:_:_c_a_m_e_r_a │ │ │ │ │ -const SfmCamera & camera(size_t idx) const │ │ │ │ │ -The camera pose at frame index idx │ │ │ │ │ -DDeeffiinniittiioonn SfmData.h:83 │ │ │ │ │ -_g_t_s_a_m_:_:_S_f_m_D_a_t_a_:_:_c_a_m_e_r_a_s │ │ │ │ │ -std::vector< SfmCamera > cameras │ │ │ │ │ -Set of cameras. │ │ │ │ │ -DDeeffiinniittiioonn SfmData.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_S_f_m_D_a_t_a_:_:_t_r_a_c_k_s │ │ │ │ │ -std::vector< SfmTrack > tracks │ │ │ │ │ -Sparse set of points. │ │ │ │ │ -DDeeffiinniittiioonn SfmData.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_S_f_m_T_r_a_c_k │ │ │ │ │ -DDeeffiinniittiioonn SfmTrack.h:126 │ │ │ │ │ +_g_t_s_a_m_:_:_p_r_i_n_t │ │ │ │ │ +void print(const Matrix &A, const string &s, ostream &stream) │ │ │ │ │ +print without optional string, must specify cout yourself │ │ │ │ │ +DDeeffiinniittiioonn Matrix.cpp:156 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_e_d_R_o_t_a_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ +Parameters for pre-integration: Usage: Create just a single Params and pass a │ │ │ │ │ +shared pointer to the c... │ │ │ │ │ +DDeeffiinniittiioonn PreintegratedRotation.h:31 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ +Parameters for pre-integration: Usage: Create just a single Params and pass a │ │ │ │ │ +shared pointer to the c... │ │ │ │ │ +DDeeffiinniittiioonn PreintegrationParams.h:26 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s_:_:_a_c_c_e_l_e_r_o_m_e_t_e_r_C_o_v_a_r_i_a_n_c_e │ │ │ │ │ +Matrix3 accelerometerCovariance │ │ │ │ │ +Continuous-time "Covariance" of accelerometer The units for stddev are σ = m/ │ │ │ │ │ +s²/√Hz. │ │ │ │ │ +DDeeffiinniittiioonn PreintegrationParams.h:29 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s_:_:_u_s_e_2_n_d_O_r_d_e_r_C_o_r_i_o_l_i_s │ │ │ │ │ +bool use2ndOrderCoriolis │ │ │ │ │ +Whether to use second order Coriolis integration. │ │ │ │ │ +DDeeffiinniittiioonn PreintegrationParams.h:31 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ +PreintegrationParams() │ │ │ │ │ +Default constructor for serialization only. │ │ │ │ │ +DDeeffiinniittiioonn PreintegrationParams.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s_:_:_n___g_r_a_v_i_t_y │ │ │ │ │ +Vector3 n_gravity │ │ │ │ │ +Gravity vector in nav frame. │ │ │ │ │ +DDeeffiinniittiioonn PreintegrationParams.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ +PreintegrationParams(const Vector3 &n_gravity) │ │ │ │ │ +The Params constructor insists on getting the navigation frame gravity vector │ │ │ │ │ +For convenience,... │ │ │ │ │ +DDeeffiinniittiioonn PreintegrationParams.h:44 │ │ │ │ │ +_g_t_s_a_m_:_:_P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s_:_:_i_n_t_e_g_r_a_t_i_o_n_C_o_v_a_r_i_a_n_c_e │ │ │ │ │ +Matrix3 integrationCovariance │ │ │ │ │ +continuous-time "Covariance" describing integration uncertainty │ │ │ │ │ +DDeeffiinniittiioonn PreintegrationParams.h:30 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _s_f_m │ │ │ │ │ - * _S_f_m_D_a_t_a_._h │ │ │ │ │ + * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ + * _P_r_e_i_n_t_e_g_r_a_t_i_o_n_P_a_r_a_m_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01223_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00164_source.html │ │ │ │┄ Files 92% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/sfm/ShonanAveraging.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/basis/Basis.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
ShonanAveraging.h
│ │ │ │ +
Basis.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
19#pragma once
│ │ │ │
20
│ │ │ │ -
21#include <gtsam/base/Matrix.h>
│ │ │ │ -
22#include <gtsam/base/Vector.h>
│ │ │ │ -
23#include <gtsam/dllexport.h>
│ │ │ │ -
24#include <gtsam/geometry/Rot2.h>
│ │ │ │ -
25#include <gtsam/geometry/Rot3.h>
│ │ │ │ -
26#include <gtsam/linear/VectorValues.h>
│ │ │ │ -
27#include <gtsam/nonlinear/LevenbergMarquardtParams.h>
│ │ │ │ -
28#include <gtsam/sfm/BinaryMeasurement.h>
│ │ │ │ -
29#include <gtsam/linear/PowerMethod.h>
│ │ │ │ -
30#include <gtsam/linear/AcceleratedPowerMethod.h>
│ │ │ │ -
31#include <gtsam/slam/dataset.h>
│ │ │ │ -
32
│ │ │ │ -
33#include <Eigen/Sparse>
│ │ │ │ -
34#include <map>
│ │ │ │ -
35#include <string>
│ │ │ │ -
36#include <type_traits>
│ │ │ │ -
37#include <utility>
│ │ │ │ -
38#include <vector>
│ │ │ │ -
39
│ │ │ │ -
40namespace gtsam {
│ │ │ │ -
41class NonlinearFactorGraph;
│ │ │ │ -
42class LevenbergMarquardtOptimizer;
│ │ │ │ -
43
│ │ │ │ -
45template <size_t d>
│ │ │ │ -
│ │ │ │ -
46struct GTSAM_EXPORT ShonanAveragingParameters {
│ │ │ │ -
47 // Select Rot2 or Rot3 interface based template parameter d
│ │ │ │ -
48 using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;
│ │ │ │ -
49 using Anchor = std::pair<size_t, Rot>;
│ │ │ │ -
50
│ │ │ │ -
51 // Parameters themselves:
│ │ │ │ -
52 LevenbergMarquardtParams lm;
│ │ │ │ -
53 double optimalityThreshold;
│ │ │ │ -
54 Anchor anchor;
│ │ │ │ -
55 double alpha;
│ │ │ │ -
56 double beta;
│ │ │ │ -
57 double gamma;
│ │ │ │ -
59 bool useHuber;
│ │ │ │ -
61 bool certifyOptimality;
│ │ │ │ -
62
│ │ │ │ -
63 ShonanAveragingParameters(const LevenbergMarquardtParams &lm =
│ │ │ │ -
64 LevenbergMarquardtParams::CeresDefaults(),
│ │ │ │ -
65 const std::string &method = "JACOBI",
│ │ │ │ -
66 double optimalityThreshold = -1e-4,
│ │ │ │ -
67 double alpha = 0.0, double beta = 1.0,
│ │ │ │ -
68 double gamma = 0.0);
│ │ │ │ +
21#include <gtsam/base/Matrix.h>
│ │ │ │ +
22#include <gtsam/base/OptionalJacobian.h>
│ │ │ │ +
23#include <gtsam/basis/ParameterMatrix.h>
│ │ │ │ +
24
│ │ │ │ +
25#include <iostream>
│ │ │ │ +
26
│ │ │ │ +
68namespace gtsam {
│ │ │ │
69
│ │ │ │ -
70 LevenbergMarquardtParams getLMParams() const { return lm; }
│ │ │ │ +
70using Weights = Eigen::Matrix<double, 1, -1>; /* 1xN vector */
│ │ │ │
71
│ │ │ │ -
72 void setOptimalityThreshold(double value) { optimalityThreshold = value; }
│ │ │ │ -
73 double getOptimalityThreshold() const { return optimalityThreshold; }
│ │ │ │ -
74
│ │ │ │ -
75 void setAnchor(size_t index, const Rot &value) { anchor = {index, value}; }
│ │ │ │ -
76 std::pair<size_t, Rot> getAnchor() const { return anchor; }
│ │ │ │ -
77
│ │ │ │ -
78 void setAnchorWeight(double value) { alpha = value; }
│ │ │ │ -
79 double getAnchorWeight() const { return alpha; }
│ │ │ │ -
80
│ │ │ │ -
81 void setKarcherWeight(double value) { beta = value; }
│ │ │ │ -
82 double getKarcherWeight() const { return beta; }
│ │ │ │ -
83
│ │ │ │ -
84 void setGaugesWeight(double value) { gamma = value; }
│ │ │ │ -
85 double getGaugesWeight() const { return gamma; }
│ │ │ │ -
86
│ │ │ │ -
87 void setUseHuber(bool value) { useHuber = value; }
│ │ │ │ -
88 bool getUseHuber() const { return useHuber; }
│ │ │ │ -
89
│ │ │ │ -
90 void setCertifyOptimality(bool value) { certifyOptimality = value; }
│ │ │ │ -
91 bool getCertifyOptimality() const { return certifyOptimality; }
│ │ │ │ -
92
│ │ │ │ -
│ │ │ │ -
94 void print(const std::string &s = "") const {
│ │ │ │ -
95 std::cout << (s.empty() ? s : s + " ");
│ │ │ │ -
96 std::cout << " ShonanAveragingParameters: " << std::endl;
│ │ │ │ -
97 std::cout << " alpha: " << alpha << std::endl;
│ │ │ │ -
98 std::cout << " beta: " << beta << std::endl;
│ │ │ │ -
99 std::cout << " gamma: " << gamma << std::endl;
│ │ │ │ -
100 std::cout << " useHuber: " << useHuber << std::endl;
│ │ │ │ -
101 }
│ │ │ │ -
│ │ │ │ -
102};
│ │ │ │ -
│ │ │ │ -
103
│ │ │ │ -
104using ShonanAveragingParameters2 = ShonanAveragingParameters<2>;
│ │ │ │ -
105using ShonanAveragingParameters3 = ShonanAveragingParameters<3>;
│ │ │ │ -
106
│ │ │ │ -
122template <size_t d>
│ │ │ │ -
│ │ │ │ -
123class GTSAM_EXPORT ShonanAveraging {
│ │ │ │ -
124 public:
│ │ │ │ -
125 using Sparse = Eigen::SparseMatrix<double>;
│ │ │ │ -
126
│ │ │ │ -
127 // Define the Parameters type and use its typedef of the rotation type:
│ │ │ │ -
128 using Parameters = ShonanAveragingParameters<d>;
│ │ │ │ -
129 using Rot = typename Parameters::Rot;
│ │ │ │ -
130
│ │ │ │ -
131 // We store SO(d) BetweenFactors to get noise model
│ │ │ │ -
132 using Measurements = std::vector<BinaryMeasurement<Rot>>;
│ │ │ │ -
133
│ │ │ │ -
134 private:
│ │ │ │ -
135 Parameters parameters_;
│ │ │ │ -
136 Measurements measurements_;
│ │ │ │ -
137 size_t nrUnknowns_;
│ │ │ │ -
138 Sparse D_; // Sparse (diagonal) degree matrix
│ │ │ │ -
139 Sparse Q_; // Sparse measurement matrix, == \tilde{R} in Eriksson18cvpr
│ │ │ │ -
140 Sparse L_; // connection Laplacian L = D - Q, needed for optimality check
│ │ │ │ -
141
│ │ │ │ -
146 Sparse buildQ() const;
│ │ │ │ -
147
│ │ │ │ -
149 Sparse buildD() const;
│ │ │ │ -
150
│ │ │ │ -
151 public:
│ │ │ │ -
154
│ │ │ │ -
157 ShonanAveraging(const Measurements &measurements,
│ │ │ │ -
158 const Parameters &parameters = Parameters());
│ │ │ │ +
84template <size_t M>
│ │ │ │ +
│ │ │ │ +
85Matrix kroneckerProductIdentity(const Weights& w) {
│ │ │ │ +
86 Matrix result(M, w.cols() * M);
│ │ │ │ +
87 result.setZero();
│ │ │ │ +
88
│ │ │ │ +
89 for (int i = 0; i < w.cols(); i++) {
│ │ │ │ +
90 result.block(0, i * M, M, M).diagonal().array() = w(i);
│ │ │ │ +
91 }
│ │ │ │ +
92 return result;
│ │ │ │ +
93}
│ │ │ │ +
│ │ │ │ +
94
│ │ │ │ +
99template <typename DERIVED>
│ │ │ │ +
│ │ │ │ +
100class Basis {
│ │ │ │ +
101 public:
│ │ │ │ +
│ │ │ │ +
107 static Matrix WeightMatrix(size_t N, const Vector& X) {
│ │ │ │ +
108 Matrix W(X.size(), N);
│ │ │ │ +
109 for (int i = 0; i < X.size(); i++)
│ │ │ │ +
110 W.row(i) = DERIVED::CalculateWeights(N, X(i));
│ │ │ │ +
111 return W;
│ │ │ │ +
112 }
│ │ │ │ +
│ │ │ │ +
113
│ │ │ │ +
│ │ │ │ +
123 static Matrix WeightMatrix(size_t N, const Vector& X, double a, double b) {
│ │ │ │ +
124 Matrix W(X.size(), N);
│ │ │ │ +
125 for (int i = 0; i < X.size(); i++)
│ │ │ │ +
126 W.row(i) = DERIVED::CalculateWeights(N, X(i), a, b);
│ │ │ │ +
127 return W;
│ │ │ │ +
128 }
│ │ │ │ +
│ │ │ │ +
129
│ │ │ │ +
│ │ │ │ +
137 class EvaluationFunctor {
│ │ │ │ +
138 protected:
│ │ │ │ +
139 Weights weights_;
│ │ │ │ +
140
│ │ │ │ +
141 public:
│ │ │ │ +
143 EvaluationFunctor() {}
│ │ │ │ +
144
│ │ │ │ +
│ │ │ │ +
146 EvaluationFunctor(size_t N, double x)
│ │ │ │ +
147 : weights_(DERIVED::CalculateWeights(N, x)) {}
│ │ │ │ +
│ │ │ │ +
148
│ │ │ │ +
│ │ │ │ +
150 EvaluationFunctor(size_t N, double x, double a, double b)
│ │ │ │ +
151 : weights_(DERIVED::CalculateWeights(N, x, a, b)) {}
│ │ │ │ +
│ │ │ │ +
152
│ │ │ │ +
│ │ │ │ +
154 double apply(const typename DERIVED::Parameters& p,
│ │ │ │ +
155 OptionalJacobian<-1, -1> H = boost::none) const {
│ │ │ │ +
156 if (H) *H = weights_;
│ │ │ │ +
157 return (weights_ * p)(0);
│ │ │ │ +
158 }
│ │ │ │ +
│ │ │ │
159
│ │ │ │ -
163
│ │ │ │ -
165 size_t nrUnknowns() const { return nrUnknowns_; }
│ │ │ │ -
166
│ │ │ │ -
168 size_t numberMeasurements() const { return measurements_.size(); }
│ │ │ │ -
169
│ │ │ │ -
│ │ │ │ -
171 const BinaryMeasurement<Rot> &measurement(size_t k) const {
│ │ │ │ -
172 return measurements_[k];
│ │ │ │ -
173 }
│ │ │ │ -
│ │ │ │ -
174
│ │ │ │ -
│ │ │ │ -
181 Measurements makeNoiseModelRobust(const Measurements &measurements,
│ │ │ │ -
182 double k = 1.345) const {
│ │ │ │ -
183 Measurements robustMeasurements;
│ │ │ │ -
184 for (auto &measurement : measurements) {
│ │ │ │ -
185 auto model = measurement.noiseModel();
│ │ │ │ -
186 const auto &robust =
│ │ │ │ -
187 boost::dynamic_pointer_cast<noiseModel::Robust>(model);
│ │ │ │ -
188
│ │ │ │ -
189 SharedNoiseModel robust_model;
│ │ │ │ -
190 // Check if the noise model is already robust
│ │ │ │ -
191 if (robust) {
│ │ │ │ -
192 robust_model = model;
│ │ │ │ -
193 } else {
│ │ │ │ -
194 // make robust
│ │ │ │ -
195 robust_model = noiseModel::Robust::Create(
│ │ │ │ -
196 noiseModel::mEstimator::Huber::Create(k), model);
│ │ │ │ -
197 }
│ │ │ │ -
198 BinaryMeasurement<Rot> meas(measurement.key1(), measurement.key2(),
│ │ │ │ -
199 measurement.measured(), robust_model);
│ │ │ │ -
200 robustMeasurements.push_back(meas);
│ │ │ │ -
201 }
│ │ │ │ -
202 return robustMeasurements;
│ │ │ │ -
203 }
│ │ │ │ -
│ │ │ │ -
204
│ │ │ │ -
206 const Rot &measured(size_t k) const { return measurements_[k].measured(); }
│ │ │ │ -
207
│ │ │ │ -
209 const KeyVector &keys(size_t k) const { return measurements_[k].keys(); }
│ │ │ │ -
210
│ │ │ │ -
214
│ │ │ │ -
215 Sparse D() const { return D_; }
│ │ │ │ -
│ │ │ │ -
216 Matrix denseD() const { return Matrix(D_); }
│ │ │ │ -
│ │ │ │ -
217 Sparse Q() const { return Q_; }
│ │ │ │ -
│ │ │ │ -
218 Matrix denseQ() const { return Matrix(Q_); }
│ │ │ │ -
│ │ │ │ -
219 Sparse L() const { return L_; }
│ │ │ │ -
│ │ │ │ -
220 Matrix denseL() const { return Matrix(L_); }
│ │ │ │ -
221
│ │ │ │ -
223 Sparse computeLambda(const Matrix &S) const;
│ │ │ │ -
224
│ │ │ │ -
│ │ │ │ -
226 Matrix computeLambda_(const Values &values) const {
│ │ │ │ -
227 return Matrix(computeLambda(values));
│ │ │ │ -
228 }
│ │ │ │ -
│ │ │ │ -
229
│ │ │ │ -
│ │ │ │ -
231 Matrix computeLambda_(const Matrix &S) const {
│ │ │ │ -
232 return Matrix(computeLambda(S));
│ │ │ │ -
233 }
│ │ │ │ -
│ │ │ │ -
234
│ │ │ │ -
236 Sparse computeA(const Values &values) const;
│ │ │ │ -
237
│ │ │ │ -
239 Sparse computeA(const Matrix &S) const;
│ │ │ │ +
│ │ │ │ +
161 double operator()(const typename DERIVED::Parameters& p,
│ │ │ │ +
162 OptionalJacobian<-1, -1> H = boost::none) const {
│ │ │ │ +
163 return apply(p, H); // might call apply in derived
│ │ │ │ +
164 }
│ │ │ │ +
│ │ │ │ +
165
│ │ │ │ +
166 void print(const std::string& s = "") const {
│ │ │ │ +
167 std::cout << s << (s != "" ? " " : "") << weights_ << std::endl;
│ │ │ │ +
168 }
│ │ │ │ +
169 };
│ │ │ │ +
│ │ │ │ +
170
│ │ │ │ +
177 template <int M>
│ │ │ │ +
│ │ │ │ +
178 class VectorEvaluationFunctor : protected EvaluationFunctor {
│ │ │ │ +
179 protected:
│ │ │ │ +
180 using VectorM = Eigen::Matrix<double, M, 1>;
│ │ │ │ +
181 using Jacobian = Eigen::Matrix<double, /*MxMN*/ M, -1>;
│ │ │ │ +
182 Jacobian H_;
│ │ │ │ +
183
│ │ │ │ +
│ │ │ │ +
192 void calculateJacobian() {
│ │ │ │ +
193 H_ = kroneckerProductIdentity<M>(this->weights_);
│ │ │ │ +
194 }
│ │ │ │ +
│ │ │ │ +
195
│ │ │ │ +
196 public:
│ │ │ │ +
197 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
198
│ │ │ │ +
200 VectorEvaluationFunctor() {}
│ │ │ │ +
201
│ │ │ │ +
│ │ │ │ +
203 VectorEvaluationFunctor(size_t N, double x) : EvaluationFunctor(N, x) {
│ │ │ │ +
204 calculateJacobian();
│ │ │ │ +
205 }
│ │ │ │ +
│ │ │ │ +
206
│ │ │ │ +
│ │ │ │ +
208 VectorEvaluationFunctor(size_t N, double x, double a, double b)
│ │ │ │ +
209 : EvaluationFunctor(N, x, a, b) {
│ │ │ │ +
210 calculateJacobian();
│ │ │ │ +
211 }
│ │ │ │ +
│ │ │ │ +
212
│ │ │ │ +
│ │ │ │ +
214 VectorM apply(const ParameterMatrix<M>& P,
│ │ │ │ +
215 OptionalJacobian</*MxN*/ -1, -1> H = boost::none) const {
│ │ │ │ +
216 if (H) *H = H_;
│ │ │ │ +
217 return P.matrix() * this->weights_.transpose();
│ │ │ │ +
218 }
│ │ │ │ +
│ │ │ │ +
219
│ │ │ │ +
│ │ │ │ +
221 VectorM operator()(const ParameterMatrix<M>& P,
│ │ │ │ +
222 OptionalJacobian</*MxN*/ -1, -1> H = boost::none) const {
│ │ │ │ +
223 return apply(P, H);
│ │ │ │ +
224 }
│ │ │ │ +
│ │ │ │ +
225 };
│ │ │ │ +
│ │ │ │ +
226
│ │ │ │ +
234 template <int M>
│ │ │ │ +
│ │ │ │ +
235 class VectorComponentFunctor : public EvaluationFunctor {
│ │ │ │ +
236 protected:
│ │ │ │ +
237 using Jacobian = Eigen::Matrix<double, /*1xMN*/ 1, -1>;
│ │ │ │ +
238 size_t rowIndex_;
│ │ │ │ +
239 Jacobian H_;
│ │ │ │
240
│ │ │ │ -
│ │ │ │ -
242 Matrix computeA_(const Values &values) const {
│ │ │ │ -
243 return Matrix(computeA(values));
│ │ │ │ -
244 }
│ │ │ │ -
│ │ │ │ -
245
│ │ │ │ -
247 static Matrix StiefelElementMatrix(const Values &values);
│ │ │ │ -
248
│ │ │ │ -
253 double computeMinEigenValue(const Values &values,
│ │ │ │ -
254 Vector *minEigenVector = nullptr) const;
│ │ │ │ +
241 /*
│ │ │ │ +
242 * Calculate the `1*(M*N)` Jacobian of this functor with respect to
│ │ │ │ +
243 * the M*N parameter matrix `P`.
│ │ │ │ +
244 * We flatten assuming column-major order, e.g., if N=3 and M=2, we have
│ │ │ │ +
245 * H=[w(0) 0 w(1) 0 w(2) 0] for rowIndex==0
│ │ │ │ +
246 * H=[0 w(0) 0 w(1) 0 w(2)] for rowIndex==1
│ │ │ │ +
247 * i.e., one row of the Kronecker product of weights_ with the
│ │ │ │ +
248 * MxM identity matrix. See also VectorEvaluationFunctor.
│ │ │ │ +
249 */
│ │ │ │ +
250 void calculateJacobian(size_t N) {
│ │ │ │ +
251 H_.setZero(1, M * N);
│ │ │ │ +
252 for (int j = 0; j < EvaluationFunctor::weights_.size(); j++)
│ │ │ │ +
253 H_(0, rowIndex_ + j * M) = EvaluationFunctor::weights_(j);
│ │ │ │ +
254 }
│ │ │ │
255
│ │ │ │ -
260 double computeMinEigenValueAP(const Values &values,
│ │ │ │ -
261 Vector *minEigenVector = nullptr) const;
│ │ │ │ -
262
│ │ │ │ -
264 Values roundSolutionS(const Matrix &S) const;
│ │ │ │ +
256 public:
│ │ │ │ +
258 VectorComponentFunctor() {}
│ │ │ │ +
259
│ │ │ │ +
│ │ │ │ +
261 VectorComponentFunctor(size_t N, size_t i, double x)
│ │ │ │ +
262 : EvaluationFunctor(N, x), rowIndex_(i) {
│ │ │ │ +
263 calculateJacobian(N);
│ │ │ │ +
264 }
│ │ │ │ +
│ │ │ │
265
│ │ │ │ -
267 static VectorValues TangentVectorValues(size_t p, const Vector &v);
│ │ │ │ -
268
│ │ │ │ -
270 Matrix riemannianGradient(size_t p, const Values &values) const;
│ │ │ │ +
│ │ │ │ +
267 VectorComponentFunctor(size_t N, size_t i, double x, double a, double b)
│ │ │ │ +
268 : EvaluationFunctor(N, x, a, b), rowIndex_(i) {
│ │ │ │ +
269 calculateJacobian(N);
│ │ │ │ +
270 }
│ │ │ │ +
│ │ │ │
271
│ │ │ │ -
276 static Values LiftwithDescent(size_t p, const Values &values,
│ │ │ │ -
277 const Vector &minEigenVector);
│ │ │ │ +
│ │ │ │ +
273 double apply(const ParameterMatrix<M>& P,
│ │ │ │ +
274 OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
│ │ │ │ +
275 if (H) *H = H_;
│ │ │ │ +
276 return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose();
│ │ │ │ +
277 }
│ │ │ │ +
│ │ │ │
278
│ │ │ │ -
286 Values initializeWithDescent(
│ │ │ │ -
287 size_t p, const Values &values, const Vector &minEigenVector,
│ │ │ │ -
288 double minEigenValue, double gradienTolerance = 1e-2,
│ │ │ │ -
289 double preconditionedGradNormTolerance = 1e-4) const;
│ │ │ │ -
293
│ │ │ │ -
298 NonlinearFactorGraph buildGraphAt(size_t p) const;
│ │ │ │ -
299
│ │ │ │ -
305 Values initializeRandomlyAt(size_t p, std::mt19937 &rng) const;
│ │ │ │ -
306
│ │ │ │ -
308 Values initializeRandomlyAt(size_t p) const;
│ │ │ │ -
309
│ │ │ │ -
314 double costAt(size_t p, const Values &values) const;
│ │ │ │ -
315
│ │ │ │ -
321 Sparse computeLambda(const Values &values) const;
│ │ │ │ -
322
│ │ │ │ -
328 std::pair<double, Vector> computeMinEigenVector(const Values &values) const;
│ │ │ │ -
329
│ │ │ │ -
334 bool checkOptimality(const Values &values) const;
│ │ │ │ -
335
│ │ │ │ -
342 boost::shared_ptr<LevenbergMarquardtOptimizer> createOptimizerAt(
│ │ │ │ -
343 size_t p, const Values &initial) const;
│ │ │ │ -
344
│ │ │ │ -
351 Values tryOptimizingAt(size_t p, const Values &initial) const;
│ │ │ │ -
352
│ │ │ │ -
357 Values projectFrom(size_t p, const Values &values) const;
│ │ │ │ -
358
│ │ │ │ -
363 Values roundSolution(const Values &values) const;
│ │ │ │ -
364
│ │ │ │ -
366 template <class T>
│ │ │ │ -
│ │ │ │ -
367 static Values LiftTo(size_t p, const Values &values) {
│ │ │ │ -
368 Values result;
│ │ │ │ -
369 for (const auto it : values.extract<T>()) {
│ │ │ │ -
370 result.insert(it.first, SOn::Lift(p, it.second.matrix()));
│ │ │ │ -
371 }
│ │ │ │ -
372 return result;
│ │ │ │ -
373 }
│ │ │ │ +
│ │ │ │ +
280 double operator()(const ParameterMatrix<M>& P,
│ │ │ │ +
281 OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
│ │ │ │ +
282 return apply(P, H);
│ │ │ │ +
283 }
│ │ │ │ +
│ │ │ │ +
284 };
│ │ │ │ +
│ │ │ │ +
285
│ │ │ │ +
299 template <class T>
│ │ │ │ +
│ │ │ │ +
300 class ManifoldEvaluationFunctor
│ │ │ │ +
301 : public VectorEvaluationFunctor<traits<T>::dimension> {
│ │ │ │ +
302 enum { M = traits<T>::dimension };
│ │ │ │ +
303 using Base = VectorEvaluationFunctor<M>;
│ │ │ │ +
304
│ │ │ │ +
305 public:
│ │ │ │ +
307 ManifoldEvaluationFunctor() {}
│ │ │ │ +
308
│ │ │ │ +
310 ManifoldEvaluationFunctor(size_t N, double x) : Base(N, x) {}
│ │ │ │ +
311
│ │ │ │ +
│ │ │ │ +
313 ManifoldEvaluationFunctor(size_t N, double x, double a, double b)
│ │ │ │ +
314 : Base(N, x, a, b) {}
│ │ │ │
│ │ │ │ -
374
│ │ │ │ +
315
│ │ │ │ +
│ │ │ │ +
317 T apply(const ParameterMatrix<M>& P,
│ │ │ │ +
318 OptionalJacobian</*MxMN*/ -1, -1> H = boost::none) const {
│ │ │ │ +
319 // Interpolate the M-dimensional vector to yield a vector in tangent space
│ │ │ │ +
320 Eigen::Matrix<double, M, 1> xi = Base::operator()(P, H);
│ │ │ │ +
321
│ │ │ │ +
322 // Now call retract with this M-vector, possibly with derivatives
│ │ │ │ +
323 Eigen::Matrix<double, M, M> D_result_xi;
│ │ │ │ +
324 T result = T::ChartAtOrigin::Retract(xi, H ? &D_result_xi : 0);
│ │ │ │ +
325
│ │ │ │ +
326 // Finally, if derivatives are asked, apply chain rule where H is Mx(M*N)
│ │ │ │ +
327 // derivative of interpolation and D_result_xi is MxM derivative of
│ │ │ │ +
328 // retract.
│ │ │ │ +
329 if (H) *H = D_result_xi * (*H);
│ │ │ │ +
330
│ │ │ │ +
331 // and return a T
│ │ │ │ +
332 return result;
│ │ │ │ +
333 }
│ │ │ │ +
│ │ │ │ +
334
│ │ │ │ +
│ │ │ │ +
336 T operator()(const ParameterMatrix<M>& P,
│ │ │ │ +
337 OptionalJacobian</*MxN*/ -1, -1> H = boost::none) const {
│ │ │ │ +
338 return apply(P, H); // might call apply in derived
│ │ │ │ +
339 }
│ │ │ │ +
│ │ │ │ +
340 };
│ │ │ │ +
│ │ │ │ +
341
│ │ │ │ +
│ │ │ │ +
343 class DerivativeFunctorBase {
│ │ │ │ +
344 protected:
│ │ │ │ +
345 Weights weights_;
│ │ │ │ +
346
│ │ │ │ +
347 public:
│ │ │ │ +
349 DerivativeFunctorBase() {}
│ │ │ │ +
350
│ │ │ │ +
351 DerivativeFunctorBase(size_t N, double x)
│ │ │ │ +
352 : weights_(DERIVED::DerivativeWeights(N, x)) {}
│ │ │ │ +
353
│ │ │ │ +
354 DerivativeFunctorBase(size_t N, double x, double a, double b)
│ │ │ │ +
355 : weights_(DERIVED::DerivativeWeights(N, x, a, b)) {}
│ │ │ │ +
356
│ │ │ │ +
357 void print(const std::string& s = "") const {
│ │ │ │ +
358 std::cout << s << (s != "" ? " " : "") << weights_ << std::endl;
│ │ │ │ +
359 }
│ │ │ │ +
360 };
│ │ │ │ +
│ │ │ │ +
361
│ │ │ │ +
│ │ │ │ +
369 class DerivativeFunctor : protected DerivativeFunctorBase {
│ │ │ │ +
370 public:
│ │ │ │ +
372 DerivativeFunctor() {}
│ │ │ │ +
373
│ │ │ │ +
374 DerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) {}
│ │ │ │ +
375
│ │ │ │ +
376 DerivativeFunctor(size_t N, double x, double a, double b)
│ │ │ │ +
377 : DerivativeFunctorBase(N, x, a, b) {}
│ │ │ │
378
│ │ │ │ -
383 double cost(const Values &values) const;
│ │ │ │ -
384
│ │ │ │ -
392 Values initializeRandomly(std::mt19937 &rng) const;
│ │ │ │ -
393
│ │ │ │ -
395 Values initializeRandomly() const;
│ │ │ │ -
396
│ │ │ │ -
404 std::pair<Values, double> run(const Values &initialEstimate, size_t pMin = d,
│ │ │ │ -
405 size_t pMax = 10) const;
│ │ │ │ -
407
│ │ │ │ -
417 template <typename T>
│ │ │ │ -
│ │ │ │ -
418 inline std::vector<BinaryMeasurement<T>> maybeRobust(
│ │ │ │ -
419 const std::vector<BinaryMeasurement<T>> &measurements,
│ │ │ │ -
420 bool useRobustModel = false) const {
│ │ │ │ -
421 return useRobustModel ? makeNoiseModelRobust(measurements) : measurements;
│ │ │ │ -
422 }
│ │ │ │ -
│ │ │ │ -
423};
│ │ │ │ -
424
│ │ │ │ -
425// Subclasses for d=2 and d=3 that explicitly instantiate, as well as provide a
│ │ │ │ -
426// convenience interface with file access.
│ │ │ │ -
427
│ │ │ │ -
│ │ │ │ -
428class GTSAM_EXPORT ShonanAveraging2 : public ShonanAveraging<2> {
│ │ │ │ -
429 public:
│ │ │ │ -
430 ShonanAveraging2(const Measurements &measurements,
│ │ │ │ -
431 const Parameters &parameters = Parameters());
│ │ │ │ -
432 explicit ShonanAveraging2(std::string g2oFile,
│ │ │ │ -
433 const Parameters &parameters = Parameters());
│ │ │ │ -
434 ShonanAveraging2(const BetweenFactorPose2s &factors,
│ │ │ │ -
435 const Parameters &parameters = Parameters());
│ │ │ │ -
436};
│ │ │ │ -
│ │ │ │ -
437
│ │ │ │ -
│ │ │ │ -
438class GTSAM_EXPORT ShonanAveraging3 : public ShonanAveraging<3> {
│ │ │ │ -
439 public:
│ │ │ │ -
440 ShonanAveraging3(const Measurements &measurements,
│ │ │ │ -
441 const Parameters &parameters = Parameters());
│ │ │ │ -
442 explicit ShonanAveraging3(std::string g2oFile,
│ │ │ │ -
443 const Parameters &parameters = Parameters());
│ │ │ │ -
444
│ │ │ │ -
445 // TODO(frank): Deprecate after we land pybind wrapper
│ │ │ │ -
446 ShonanAveraging3(const BetweenFactorPose3s &factors,
│ │ │ │ -
447 const Parameters &parameters = Parameters());
│ │ │ │ -
448};
│ │ │ │ -
│ │ │ │ -
449} // namespace gtsam
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ -
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ -
Rot3.h
3D rotation represented as a rotation matrix or quaternion
│ │ │ │ -
Rot2.h
2D rotation
│ │ │ │ -
PowerMethod.h
Power method for fast eigenvalue and eigenvector computation.
│ │ │ │ -
AcceleratedPowerMethod.h
accelerated power method for fast eigenvalue and eigenvector computation
│ │ │ │ -
VectorValues.h
Factor Graph Values.
│ │ │ │ -
LevenbergMarquardtParams.h
Parameters for Levenberg-Marquardt trust-region scheme.
│ │ │ │ -
BinaryMeasurement.h
Binary measurement represents a measurement between two keys in a graph. A binary measurement is simi...
│ │ │ │ -
dataset.h
utility functions for loading datasets
│ │ │ │ +
379 double apply(const typename DERIVED::Parameters& p,
│ │ │ │ +
380 OptionalJacobian</*1xN*/ -1, -1> H = boost::none) const {
│ │ │ │ +
381 if (H) *H = this->weights_;
│ │ │ │ +
382 return (this->weights_ * p)(0);
│ │ │ │ +
383 }
│ │ │ │ +
│ │ │ │ +
385 double operator()(const typename DERIVED::Parameters& p,
│ │ │ │ +
386 OptionalJacobian</*1xN*/ -1, -1> H = boost::none) const {
│ │ │ │ +
387 return apply(p, H); // might call apply in derived
│ │ │ │ +
388 }
│ │ │ │ +
│ │ │ │ +
389 };
│ │ │ │ +
│ │ │ │ +
390
│ │ │ │ +
399 template <int M>
│ │ │ │ +
│ │ │ │ +
400 class VectorDerivativeFunctor : protected DerivativeFunctorBase {
│ │ │ │ +
401 protected:
│ │ │ │ +
402 using VectorM = Eigen::Matrix<double, M, 1>;
│ │ │ │ +
403 using Jacobian = Eigen::Matrix<double, /*MxMN*/ M, -1>;
│ │ │ │ +
404 Jacobian H_;
│ │ │ │ +
405
│ │ │ │ +
│ │ │ │ +
414 void calculateJacobian() {
│ │ │ │ +
415 H_ = kroneckerProductIdentity<M>(this->weights_);
│ │ │ │ +
416 }
│ │ │ │ +
│ │ │ │ +
417
│ │ │ │ +
418 public:
│ │ │ │ +
419 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
│ │ │ │ +
420
│ │ │ │ +
422 VectorDerivativeFunctor() {}
│ │ │ │ +
423
│ │ │ │ +
│ │ │ │ +
425 VectorDerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) {
│ │ │ │ +
426 calculateJacobian();
│ │ │ │ +
427 }
│ │ │ │ +
│ │ │ │ +
428
│ │ │ │ +
│ │ │ │ +
430 VectorDerivativeFunctor(size_t N, double x, double a, double b)
│ │ │ │ +
431 : DerivativeFunctorBase(N, x, a, b) {
│ │ │ │ +
432 calculateJacobian();
│ │ │ │ +
433 }
│ │ │ │ +
│ │ │ │ +
434
│ │ │ │ +
435 VectorM apply(const ParameterMatrix<M>& P,
│ │ │ │ +
436 OptionalJacobian</*MxMN*/ -1, -1> H = boost::none) const {
│ │ │ │ +
437 if (H) *H = H_;
│ │ │ │ +
438 return P.matrix() * this->weights_.transpose();
│ │ │ │ +
439 }
│ │ │ │ +
│ │ │ │ +
441 VectorM operator()(
│ │ │ │ +
442 const ParameterMatrix<M>& P,
│ │ │ │ +
443 OptionalJacobian</*MxMN*/ -1, -1> H = boost::none) const {
│ │ │ │ +
444 return apply(P, H);
│ │ │ │ +
445 }
│ │ │ │ +
│ │ │ │ +
446 };
│ │ │ │ +
│ │ │ │ +
447
│ │ │ │ +
455 template <int M>
│ │ │ │ +
│ │ │ │ +
456 class ComponentDerivativeFunctor : protected DerivativeFunctorBase {
│ │ │ │ +
457 protected:
│ │ │ │ +
458 using Jacobian = Eigen::Matrix<double, /*1xMN*/ 1, -1>;
│ │ │ │ +
459 size_t rowIndex_;
│ │ │ │ +
460 Jacobian H_;
│ │ │ │ +
461
│ │ │ │ +
462 /*
│ │ │ │ +
463 * Calculate the `1*(M*N)` Jacobian of this functor with respect to
│ │ │ │ +
464 * the M*N parameter matrix `P`.
│ │ │ │ +
465 * We flatten assuming column-major order, e.g., if N=3 and M=2, we have
│ │ │ │ +
466 * H=[w(0) 0 w(1) 0 w(2) 0] for rowIndex==0
│ │ │ │ +
467 * H=[0 w(0) 0 w(1) 0 w(2)] for rowIndex==1
│ │ │ │ +
468 * i.e., one row of the Kronecker product of weights_ with the
│ │ │ │ +
469 * MxM identity matrix. See also VectorDerivativeFunctor.
│ │ │ │ +
470 */
│ │ │ │ +
471 void calculateJacobian(size_t N) {
│ │ │ │ +
472 H_.setZero(1, M * N);
│ │ │ │ +
473 for (int j = 0; j < this->weights_.size(); j++)
│ │ │ │ +
474 H_(0, rowIndex_ + j * M) = this->weights_(j);
│ │ │ │ +
475 }
│ │ │ │ +
476
│ │ │ │ +
477 public:
│ │ │ │ +
479 ComponentDerivativeFunctor() {}
│ │ │ │ +
480
│ │ │ │ +
│ │ │ │ +
482 ComponentDerivativeFunctor(size_t N, size_t i, double x)
│ │ │ │ +
483 : DerivativeFunctorBase(N, x), rowIndex_(i) {
│ │ │ │ +
484 calculateJacobian(N);
│ │ │ │ +
485 }
│ │ │ │ +
│ │ │ │ +
486
│ │ │ │ +
│ │ │ │ +
488 ComponentDerivativeFunctor(size_t N, size_t i, double x, double a, double b)
│ │ │ │ +
489 : DerivativeFunctorBase(N, x, a, b), rowIndex_(i) {
│ │ │ │ +
490 calculateJacobian(N);
│ │ │ │ +
491 }
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
493 double apply(const ParameterMatrix<M>& P,
│ │ │ │ +
494 OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
│ │ │ │ +
495 if (H) *H = H_;
│ │ │ │ +
496 return P.row(rowIndex_) * this->weights_.transpose();
│ │ │ │ +
497 }
│ │ │ │ +
│ │ │ │ +
│ │ │ │ +
499 double operator()(const ParameterMatrix<M>& P,
│ │ │ │ +
500 OptionalJacobian</*1xMN*/ -1, -1> H = boost::none) const {
│ │ │ │ +
501 return apply(P, H);
│ │ │ │ +
502 }
│ │ │ │ +
│ │ │ │ +
503 };
│ │ │ │ +
│ │ │ │ +
504
│ │ │ │ +
505};
│ │ │ │ +
│ │ │ │ +
506
│ │ │ │ +
507} // namespace gtsam
│ │ │ │ +
OptionalJacobian.h
Special class for optional Jacobian arguments.
│ │ │ │ +
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ +
ParameterMatrix.h
Define ParameterMatrix class which is used to store values at interpolation points.
│ │ │ │ +
gtsam::kroneckerProductIdentity
Matrix kroneckerProductIdentity(const Weights &w)
Function for computing the kronecker product of the 1*N Weight vector w with the MxM identity matrix ...
Definition Basis.h:85
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
│ │ │ │ -
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ -
gtsam::LevenbergMarquardtParams
Parameters for Levenberg-Marquardt optimization.
Definition LevenbergMarquardtParams.h:35
│ │ │ │ -
gtsam::NonlinearFactorGraph
Definition NonlinearFactorGraph.h:55
│ │ │ │ -
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
gtsam::Values::extract
std::map< Key, ValueType > extract(const std::function< bool(Key)> &filterFcn=&_truePredicate< Key >) const
Extract a subset of values of the given type ValueType.
Definition Values-inl.h:94
│ │ │ │ -
gtsam::BinaryMeasurement
Definition BinaryMeasurement.h:36
│ │ │ │ -
gtsam::ShonanAveragingParameters
Parameters governing optimization etc.
Definition ShonanAveraging.h:46
│ │ │ │ -
gtsam::ShonanAveragingParameters::alpha
double alpha
weight of anchor-based prior (default 0)
Definition ShonanAveraging.h:55
│ │ │ │ -
gtsam::ShonanAveragingParameters::print
void print(const std::string &s="") const
Print the parameters and flags used for rotation averaging.
Definition ShonanAveraging.h:94
│ │ │ │ -
gtsam::ShonanAveragingParameters::lm
LevenbergMarquardtParams lm
LM parameters.
Definition ShonanAveraging.h:52
│ │ │ │ -
gtsam::ShonanAveragingParameters::useHuber
bool useHuber
if enabled, the Huber loss is used (default false)
Definition ShonanAveraging.h:59
│ │ │ │ -
gtsam::ShonanAveragingParameters::optimalityThreshold
double optimalityThreshold
threshold used in checkOptimality
Definition ShonanAveraging.h:53
│ │ │ │ -
gtsam::ShonanAveragingParameters::beta
double beta
weight of Karcher-based prior (default 1)
Definition ShonanAveraging.h:56
│ │ │ │ -
gtsam::ShonanAveragingParameters::gamma
double gamma
weight of gauge-fixing factors (default 0)
Definition ShonanAveraging.h:57
│ │ │ │ -
gtsam::ShonanAveragingParameters::anchor
Anchor anchor
pose to use as anchor if not Karcher
Definition ShonanAveraging.h:54
│ │ │ │ -
gtsam::ShonanAveragingParameters::certifyOptimality
bool certifyOptimality
if enabled solution optimality is certified (default true)
Definition ShonanAveraging.h:61
│ │ │ │ -
gtsam::ShonanAveraging
Class that implements Shonan Averaging from our ECCV'20 paper.
Definition ShonanAveraging.h:123
│ │ │ │ -
gtsam::ShonanAveraging::D
Sparse D() const
Sparse version of D.
Definition ShonanAveraging.h:215
│ │ │ │ -
gtsam::ShonanAveraging::measurement
const BinaryMeasurement< Rot > & measurement(size_t k) const
k^th binary measurement
Definition ShonanAveraging.h:171
│ │ │ │ -
gtsam::ShonanAveraging::makeNoiseModelRobust
Measurements makeNoiseModelRobust(const Measurements &measurements, double k=1.345) const
Update factors to use robust Huber loss.
Definition ShonanAveraging.h:181
│ │ │ │ -
gtsam::ShonanAveraging::roundSolutionS
Values roundSolutionS(const Matrix &S) const
Project pxdN Stiefel manifold matrix S to Rot3^N.
│ │ │ │ -
gtsam::ShonanAveraging::maybeRobust
std::vector< BinaryMeasurement< T > > maybeRobust(const std::vector< BinaryMeasurement< T > > &measurements, bool useRobustModel=false) const
Helper function to convert measurements to robust noise model if flag is set.
Definition ShonanAveraging.h:418
│ │ │ │ -
gtsam::ShonanAveraging::L
Sparse L() const
Sparse version of L.
Definition ShonanAveraging.h:219
│ │ │ │ -
gtsam::ShonanAveraging::measured
const Rot & measured(size_t k) const
k^th measurement, as a Rot.
Definition ShonanAveraging.h:206
│ │ │ │ -
gtsam::ShonanAveraging::Q
Sparse Q() const
Sparse version of Q.
Definition ShonanAveraging.h:217
│ │ │ │ -
gtsam::ShonanAveraging::denseD
Matrix denseD() const
Dense version of D.
Definition ShonanAveraging.h:216
│ │ │ │ -
gtsam::ShonanAveraging::nrUnknowns
size_t nrUnknowns() const
Return number of unknowns.
Definition ShonanAveraging.h:165
│ │ │ │ -
gtsam::ShonanAveraging::projectFrom
Values projectFrom(size_t p, const Values &values) const
Project from SO(p) to Rot2 or Rot3 Values should be of type SO(p)
│ │ │ │ -
gtsam::ShonanAveraging::numberMeasurements
size_t numberMeasurements() const
Return number of measurements.
Definition ShonanAveraging.h:168
│ │ │ │ -
gtsam::ShonanAveraging::denseL
Matrix denseL() const
Dense version of L.
Definition ShonanAveraging.h:220
│ │ │ │ -
gtsam::ShonanAveraging::computeLambda_
Matrix computeLambda_(const Matrix &S) const
Dense versions of computeLambda for wrapper/testing.
Definition ShonanAveraging.h:231
│ │ │ │ -
gtsam::ShonanAveraging::denseQ
Matrix denseQ() const
Dense version of Q.
Definition ShonanAveraging.h:218
│ │ │ │ -
gtsam::ShonanAveraging::keys
const KeyVector & keys(size_t k) const
Keys for k^th measurement, as a vector of Key values.
Definition ShonanAveraging.h:209
│ │ │ │ -
gtsam::ShonanAveraging::computeLambda_
Matrix computeLambda_(const Values &values) const
Dense versions of computeLambda for wrapper/testing.
Definition ShonanAveraging.h:226
│ │ │ │ -
gtsam::ShonanAveraging::computeA_
Matrix computeA_(const Values &values) const
Dense version of computeA for wrapper/testing.
Definition ShonanAveraging.h:242
│ │ │ │ -
gtsam::ShonanAveraging::LiftTo
static Values LiftTo(size_t p, const Values &values)
Lift Values of type T to SO(p)
Definition ShonanAveraging.h:367
│ │ │ │ -
gtsam::ShonanAveraging2
Definition ShonanAveraging.h:428
│ │ │ │ -
gtsam::ShonanAveraging3
Definition ShonanAveraging.h:438
│ │ │ │ +
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ +
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
│ │ │ │ +
gtsam::Basis
CRTP Base class for function bases.
Definition Basis.h:100
│ │ │ │ +
gtsam::Basis::WeightMatrix
static Matrix WeightMatrix(size_t N, const Vector &X, double a, double b)
Calculate weights for all x in vector X, with interval [a,b].
Definition Basis.h:123
│ │ │ │ +
gtsam::Basis::WeightMatrix
static Matrix WeightMatrix(size_t N, const Vector &X)
Calculate weights for all x in vector X.
Definition Basis.h:107
│ │ │ │ +
gtsam::Basis::EvaluationFunctor
An instance of an EvaluationFunctor calculates f(x;p) at a given x, applied to Parameters p.
Definition Basis.h:137
│ │ │ │ +
gtsam::Basis::EvaluationFunctor::EvaluationFunctor
EvaluationFunctor(size_t N, double x)
Constructor with interval [a,b].
Definition Basis.h:146
│ │ │ │ +
gtsam::Basis::EvaluationFunctor::EvaluationFunctor
EvaluationFunctor(size_t N, double x, double a, double b)
Constructor with interval [a,b].
Definition Basis.h:150
│ │ │ │ +
gtsam::Basis::EvaluationFunctor::apply
double apply(const typename DERIVED::Parameters &p, OptionalJacobian<-1, -1 > H=boost::none) const
Regular 1D evaluation.
Definition Basis.h:154
│ │ │ │ +
gtsam::Basis::EvaluationFunctor::EvaluationFunctor
EvaluationFunctor()
For serialization.
Definition Basis.h:143
│ │ │ │ +
gtsam::Basis::EvaluationFunctor::operator()
double operator()(const typename DERIVED::Parameters &p, OptionalJacobian<-1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:161
│ │ │ │ +
gtsam::Basis::VectorEvaluationFunctor
VectorEvaluationFunctor at a given x, applied to ParameterMatrix<M>.
Definition Basis.h:178
│ │ │ │ +
gtsam::Basis::VectorEvaluationFunctor::VectorEvaluationFunctor
VectorEvaluationFunctor(size_t N, double x, double a, double b)
Constructor, with interval [a,b].
Definition Basis.h:208
│ │ │ │ +
gtsam::Basis::VectorEvaluationFunctor::VectorEvaluationFunctor
EIGEN_MAKE_ALIGNED_OPERATOR_NEW VectorEvaluationFunctor()
For serialization.
Definition Basis.h:200
│ │ │ │ +
gtsam::Basis::VectorEvaluationFunctor::VectorEvaluationFunctor
VectorEvaluationFunctor(size_t N, double x)
Default Constructor.
Definition Basis.h:203
│ │ │ │ +
gtsam::Basis::VectorEvaluationFunctor::calculateJacobian
void calculateJacobian()
Calculate the M*(M*N) Jacobian of this functor with respect to the M*N parameter matrix P.
Definition Basis.h:192
│ │ │ │ +
gtsam::Basis::VectorEvaluationFunctor::operator()
VectorM operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:221
│ │ │ │ +
gtsam::Basis::VectorEvaluationFunctor::apply
VectorM apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
M-dimensional evaluation.
Definition Basis.h:214
│ │ │ │ +
gtsam::Basis::VectorComponentFunctor
Given a M*N Matrix of M-vectors at N polynomial points, an instance of VectorComponentFunctor compute...
Definition Basis.h:235
│ │ │ │ +
gtsam::Basis::VectorComponentFunctor::operator()
double operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:280
│ │ │ │ +
gtsam::Basis::VectorComponentFunctor::VectorComponentFunctor
VectorComponentFunctor()
For serialization.
Definition Basis.h:258
│ │ │ │ +
gtsam::Basis::VectorComponentFunctor::VectorComponentFunctor
VectorComponentFunctor(size_t N, size_t i, double x)
Construct with row index.
Definition Basis.h:261
│ │ │ │ +
gtsam::Basis::VectorComponentFunctor::VectorComponentFunctor
VectorComponentFunctor(size_t N, size_t i, double x, double a, double b)
Construct with row index and interval.
Definition Basis.h:267
│ │ │ │ +
gtsam::Basis::VectorComponentFunctor::apply
double apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
Calculate component of component rowIndex_ of P.
Definition Basis.h:273
│ │ │ │ +
gtsam::Basis::ManifoldEvaluationFunctor
Manifold EvaluationFunctor at a given x, applied to ParameterMatrix<M>.
Definition Basis.h:301
│ │ │ │ +
gtsam::Basis::ManifoldEvaluationFunctor::ManifoldEvaluationFunctor
ManifoldEvaluationFunctor(size_t N, double x, double a, double b)
Constructor, with interval [a,b].
Definition Basis.h:313
│ │ │ │ +
gtsam::Basis::ManifoldEvaluationFunctor::apply
T apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
Manifold evaluation.
Definition Basis.h:317
│ │ │ │ +
gtsam::Basis::ManifoldEvaluationFunctor::ManifoldEvaluationFunctor
ManifoldEvaluationFunctor()
For serialization.
Definition Basis.h:307
│ │ │ │ +
gtsam::Basis::ManifoldEvaluationFunctor::ManifoldEvaluationFunctor
ManifoldEvaluationFunctor(size_t N, double x)
Default Constructor.
Definition Basis.h:310
│ │ │ │ +
gtsam::Basis::ManifoldEvaluationFunctor::operator()
T operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:336
│ │ │ │ +
gtsam::Basis::DerivativeFunctorBase
Base class for functors below that calculate derivative weights.
Definition Basis.h:343
│ │ │ │ +
gtsam::Basis::DerivativeFunctorBase::DerivativeFunctorBase
DerivativeFunctorBase()
For serialization.
Definition Basis.h:349
│ │ │ │ +
gtsam::Basis::DerivativeFunctor
An instance of a DerivativeFunctor calculates f'(x;p) at a given x, applied to Parameters p.
Definition Basis.h:369
│ │ │ │ +
gtsam::Basis::DerivativeFunctor::DerivativeFunctor
DerivativeFunctor()
For serialization.
Definition Basis.h:372
│ │ │ │ +
gtsam::Basis::DerivativeFunctor::operator()
double operator()(const typename DERIVED::Parameters &p, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:385
│ │ │ │ +
gtsam::Basis::VectorDerivativeFunctor
VectorDerivativeFunctor at a given x, applied to ParameterMatrix<M>.
Definition Basis.h:400
│ │ │ │ +
gtsam::Basis::VectorDerivativeFunctor::calculateJacobian
void calculateJacobian()
Calculate the M*(M*N) Jacobian of this functor with respect to the M*N parameter matrix P.
Definition Basis.h:414
│ │ │ │ +
gtsam::Basis::VectorDerivativeFunctor::VectorDerivativeFunctor
VectorDerivativeFunctor(size_t N, double x, double a, double b)
Constructor, with optional interval [a,b].
Definition Basis.h:430
│ │ │ │ +
gtsam::Basis::VectorDerivativeFunctor::operator()
VectorM operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:441
│ │ │ │ +
gtsam::Basis::VectorDerivativeFunctor::VectorDerivativeFunctor
EIGEN_MAKE_ALIGNED_OPERATOR_NEW VectorDerivativeFunctor()
For serialization.
Definition Basis.h:422
│ │ │ │ +
gtsam::Basis::VectorDerivativeFunctor::VectorDerivativeFunctor
VectorDerivativeFunctor(size_t N, double x)
Default Constructor.
Definition Basis.h:425
│ │ │ │ +
gtsam::Basis::ComponentDerivativeFunctor
Given a M*N Matrix of M-vectors at N polynomial points, an instance of ComponentDerivativeFunctor com...
Definition Basis.h:456
│ │ │ │ +
gtsam::Basis::ComponentDerivativeFunctor::ComponentDerivativeFunctor
ComponentDerivativeFunctor(size_t N, size_t i, double x)
Construct with row index.
Definition Basis.h:482
│ │ │ │ +
gtsam::Basis::ComponentDerivativeFunctor::operator()
double operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
c++ sugar
Definition Basis.h:499
│ │ │ │ +
gtsam::Basis::ComponentDerivativeFunctor::ComponentDerivativeFunctor
ComponentDerivativeFunctor()
For serialization.
Definition Basis.h:479
│ │ │ │ +
gtsam::Basis::ComponentDerivativeFunctor::ComponentDerivativeFunctor
ComponentDerivativeFunctor(size_t N, size_t i, double x, double a, double b)
Construct with row index and interval.
Definition Basis.h:488
│ │ │ │ +
gtsam::Basis::ComponentDerivativeFunctor::apply
double apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost::none) const
Calculate derivative of component rowIndex_ of F.
Definition Basis.h:493
│ │ │ │ +
gtsam::ParameterMatrix
A matrix abstraction of MxN values at the Basis points.
Definition ParameterMatrix.h:38
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,473 +1,554 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -ShonanAveraging.h │ │ │ │ │ +Basis.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 19#pragma once │ │ │ │ │ 20 │ │ │ │ │ 21#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_c_t_o_r_._h> │ │ │ │ │ -23#include │ │ │ │ │ -24#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_2_._h> │ │ │ │ │ -25#include <_g_t_s_a_m_/_g_e_o_m_e_t_r_y_/_R_o_t_3_._h> │ │ │ │ │ -26#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ -27#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_._h> │ │ │ │ │ -28#include <_g_t_s_a_m_/_s_f_m_/_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_._h> │ │ │ │ │ -29#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_P_o_w_e_r_M_e_t_h_o_d_._h> │ │ │ │ │ -30#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_A_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_M_e_t_h_o_d_._h> │ │ │ │ │ -31#include <_g_t_s_a_m_/_s_l_a_m_/_d_a_t_a_s_e_t_._h> │ │ │ │ │ -32 │ │ │ │ │ -33#include │ │ │ │ │ -34#include │ │ │ │ │ -35#include │ │ │ │ │ -36#include │ │ │ │ │ -37#include │ │ │ │ │ -38#include │ │ │ │ │ -39 │ │ │ │ │ -40namespace _g_t_s_a_m { │ │ │ │ │ -41class NonlinearFactorGraph; │ │ │ │ │ -42class LevenbergMarquardtOptimizer; │ │ │ │ │ -43 │ │ │ │ │ -45template │ │ │ │ │ -_4_6struct GTSAM_EXPORT _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s { │ │ │ │ │ -47 // Select Rot2 or Rot3 interface based template parameter d │ │ │ │ │ -48 using Rot = typename std::conditional::type; │ │ │ │ │ -49 using Anchor = std::pair; │ │ │ │ │ -50 │ │ │ │ │ -51 // Parameters themselves: │ │ │ │ │ -_5_2 _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s _l_m; │ │ │ │ │ -_5_3 double _o_p_t_i_m_a_l_i_t_y_T_h_r_e_s_h_o_l_d; │ │ │ │ │ -_5_4 Anchor _a_n_c_h_o_r; │ │ │ │ │ -_5_5 double _a_l_p_h_a; │ │ │ │ │ -_5_6 double _b_e_t_a; │ │ │ │ │ -_5_7 double _g_a_m_m_a; │ │ │ │ │ -_5_9 bool _u_s_e_H_u_b_e_r; │ │ │ │ │ -_6_1 bool _c_e_r_t_i_f_y_O_p_t_i_m_a_l_i_t_y; │ │ │ │ │ -62 │ │ │ │ │ -63 _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s(const _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s &lm = │ │ │ │ │ -64 LevenbergMarquardtParams::CeresDefaults(), │ │ │ │ │ -65 const std::string &method = "JACOBI", │ │ │ │ │ -66 double optimalityThreshold = -1e-4, │ │ │ │ │ -67 double alpha = 0.0, double beta = 1.0, │ │ │ │ │ -68 double gamma = 0.0); │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_b_a_s_i_s_/_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_._h> │ │ │ │ │ +24 │ │ │ │ │ +25#include │ │ │ │ │ +26 │ │ │ │ │ +68namespace _g_t_s_a_m { │ │ │ │ │ 69 │ │ │ │ │ -70 _L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s getLMParams() const { return lm; } │ │ │ │ │ +70using Weights = Eigen::Matrix; /* 1xN vector */ │ │ │ │ │ 71 │ │ │ │ │ -72 void setOptimalityThreshold(double value) { optimalityThreshold = value; } │ │ │ │ │ -73 double getOptimalityThreshold() const { return optimalityThreshold; } │ │ │ │ │ -74 │ │ │ │ │ -75 void setAnchor(size_t index, const Rot &value) { anchor = {index, value}; } │ │ │ │ │ -76 std::pair getAnchor() const { return anchor; } │ │ │ │ │ -77 │ │ │ │ │ -78 void setAnchorWeight(double value) { alpha = value; } │ │ │ │ │ -79 double getAnchorWeight() const { return alpha; } │ │ │ │ │ -80 │ │ │ │ │ -81 void setKarcherWeight(double value) { beta = value; } │ │ │ │ │ -82 double getKarcherWeight() const { return beta; } │ │ │ │ │ -83 │ │ │ │ │ -84 void setGaugesWeight(double value) { gamma = value; } │ │ │ │ │ -85 double getGaugesWeight() const { return gamma; } │ │ │ │ │ -86 │ │ │ │ │ -87 void setUseHuber(bool value) { useHuber = value; } │ │ │ │ │ -88 bool getUseHuber() const { return useHuber; } │ │ │ │ │ -89 │ │ │ │ │ -90 void setCertifyOptimality(bool value) { certifyOptimality = value; } │ │ │ │ │ -91 bool getCertifyOptimality() const { return certifyOptimality; } │ │ │ │ │ -92 │ │ │ │ │ -_9_4 void _p_r_i_n_t(const std::string &s = "") const { │ │ │ │ │ -95 std::cout << (s.empty() ? s : s + " "); │ │ │ │ │ -96 std::cout << " ShonanAveragingParameters: " << std::endl; │ │ │ │ │ -97 std::cout << " alpha: " << alpha << std::endl; │ │ │ │ │ -98 std::cout << " beta: " << beta << std::endl; │ │ │ │ │ -99 std::cout << " gamma: " << gamma << std::endl; │ │ │ │ │ -100 std::cout << " useHuber: " << useHuber << std::endl; │ │ │ │ │ -101 } │ │ │ │ │ -102}; │ │ │ │ │ -103 │ │ │ │ │ -104using ShonanAveragingParameters2 = ShonanAveragingParameters<2>; │ │ │ │ │ -105using ShonanAveragingParameters3 = ShonanAveragingParameters<3>; │ │ │ │ │ -106 │ │ │ │ │ -122template │ │ │ │ │ -_1_2_3class GTSAM_EXPORT _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g { │ │ │ │ │ -124 public: │ │ │ │ │ -125 using Sparse = Eigen::SparseMatrix; │ │ │ │ │ -126 │ │ │ │ │ -127 // Define the Parameters type and use its typedef of the rotation type: │ │ │ │ │ -128 using _P_a_r_a_m_e_t_e_r_s = _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s_<_d_>; │ │ │ │ │ -129 using Rot = typename Parameters::Rot; │ │ │ │ │ -130 │ │ │ │ │ -131 // We store SO(d) BetweenFactors to get noise model │ │ │ │ │ -132 using Measurements = std::vector>; │ │ │ │ │ -133 │ │ │ │ │ -134 private: │ │ │ │ │ -135 _P_a_r_a_m_e_t_e_r_s parameters_; │ │ │ │ │ -136 Measurements measurements_; │ │ │ │ │ -137 size_t nrUnknowns_; │ │ │ │ │ -138 Sparse D_; // Sparse (diagonal) degree matrix │ │ │ │ │ -139 Sparse Q_; // Sparse measurement matrix, == \tilde{R} in Eriksson18cvpr │ │ │ │ │ -140 Sparse L_; // connection Laplacian L = D - Q, needed for optimality check │ │ │ │ │ -141 │ │ │ │ │ -146 Sparse buildQ() const; │ │ │ │ │ -147 │ │ │ │ │ -149 Sparse buildD() const; │ │ │ │ │ -150 │ │ │ │ │ -151 public: │ │ │ │ │ -154 │ │ │ │ │ -157 _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g(const Measurements &measurements, │ │ │ │ │ -158 const _P_a_r_a_m_e_t_e_r_s ¶meters = _P_a_r_a_m_e_t_e_r_s()); │ │ │ │ │ +84template │ │ │ │ │ +_8_5Matrix _k_r_o_n_e_c_k_e_r_P_r_o_d_u_c_t_I_d_e_n_t_i_t_y(const Weights& w) { │ │ │ │ │ +86 Matrix result(M, w.cols() * M); │ │ │ │ │ +87 result.setZero(); │ │ │ │ │ +88 │ │ │ │ │ +89 for (int i = 0; i < w.cols(); i++) { │ │ │ │ │ +90 result.block(0, i * M, M, M).diagonal().array() = w(i); │ │ │ │ │ +91 } │ │ │ │ │ +92 return result; │ │ │ │ │ +93} │ │ │ │ │ +94 │ │ │ │ │ +99template │ │ │ │ │ +_1_0_0class _B_a_s_i_s { │ │ │ │ │ +101 public: │ │ │ │ │ +_1_0_7 static Matrix _W_e_i_g_h_t_M_a_t_r_i_x(size_t N, const Vector& X) { │ │ │ │ │ +108 Matrix W(X.size(), N); │ │ │ │ │ +109 for (int i = 0; i < X.size(); i++) │ │ │ │ │ +110 W.row(i) = DERIVED::CalculateWeights(N, X(i)); │ │ │ │ │ +111 return W; │ │ │ │ │ +112 } │ │ │ │ │ +113 │ │ │ │ │ +_1_2_3 static Matrix _W_e_i_g_h_t_M_a_t_r_i_x(size_t N, const Vector& X, double a, double b) { │ │ │ │ │ +124 Matrix W(X.size(), N); │ │ │ │ │ +125 for (int i = 0; i < X.size(); i++) │ │ │ │ │ +126 W.row(i) = DERIVED::CalculateWeights(N, X(i), a, b); │ │ │ │ │ +127 return W; │ │ │ │ │ +128 } │ │ │ │ │ +129 │ │ │ │ │ +_1_3_7 class _E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r { │ │ │ │ │ +138 protected: │ │ │ │ │ +139 Weights weights_; │ │ │ │ │ +140 │ │ │ │ │ +141 public: │ │ │ │ │ +_1_4_3 _E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r() {} │ │ │ │ │ +144 │ │ │ │ │ +_1_4_6 _E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r(size_t N, double x) │ │ │ │ │ +147 : weights_(DERIVED::CalculateWeights(N, x)) {} │ │ │ │ │ +148 │ │ │ │ │ +_1_5_0 _E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r(size_t N, double x, double a, double b) │ │ │ │ │ +151 : weights_(DERIVED::CalculateWeights(N, x, a, b)) {} │ │ │ │ │ +152 │ │ │ │ │ +_1_5_4 double _a_p_p_l_y(const typename DERIVED::Parameters& p, │ │ │ │ │ +155 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_-_1_,_ _-_1_> H = boost::none) const { │ │ │ │ │ +156 if (H) *H = weights_; │ │ │ │ │ +157 return (weights_ * p)(0); │ │ │ │ │ +158 } │ │ │ │ │ 159 │ │ │ │ │ -163 │ │ │ │ │ -_1_6_5 size_t _n_r_U_n_k_n_o_w_n_s() const { return nrUnknowns_; } │ │ │ │ │ -166 │ │ │ │ │ -_1_6_8 size_t _n_u_m_b_e_r_M_e_a_s_u_r_e_m_e_n_t_s() const { return measurements_.size(); } │ │ │ │ │ -169 │ │ │ │ │ -_1_7_1 const _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_<_R_o_t_> &_m_e_a_s_u_r_e_m_e_n_t(size_t k) const { │ │ │ │ │ -172 return measurements_[k]; │ │ │ │ │ -173 } │ │ │ │ │ -174 │ │ │ │ │ -_1_8_1 Measurements _m_a_k_e_N_o_i_s_e_M_o_d_e_l_R_o_b_u_s_t(const Measurements &measurements, │ │ │ │ │ -182 double k = 1.345) const { │ │ │ │ │ -183 Measurements robustMeasurements; │ │ │ │ │ -184 for (auto &measurement : measurements) { │ │ │ │ │ -185 auto model = measurement.noiseModel(); │ │ │ │ │ -186 const auto &robust = │ │ │ │ │ -187 boost::dynamic_pointer_cast(model); │ │ │ │ │ -188 │ │ │ │ │ -189 _S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l robust_model; │ │ │ │ │ -190 // Check if the noise model is already robust │ │ │ │ │ -191 if (robust) { │ │ │ │ │ -192 robust_model = model; │ │ │ │ │ -193 } else { │ │ │ │ │ -194 // make robust │ │ │ │ │ -195 robust_model = noiseModel::Robust::Create( │ │ │ │ │ -196 noiseModel::mEstimator::Huber::Create(k), model); │ │ │ │ │ -197 } │ │ │ │ │ -198 _B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_<_R_o_t_> meas(measurement.key1(), measurement.key2(), │ │ │ │ │ -199 measurement.measured(), robust_model); │ │ │ │ │ -200 robustMeasurements.push_back(meas); │ │ │ │ │ -201 } │ │ │ │ │ -202 return robustMeasurements; │ │ │ │ │ -203 } │ │ │ │ │ -204 │ │ │ │ │ -_2_0_6 const Rot &_m_e_a_s_u_r_e_d(size_t k) const { return measurements_[k].measured(); } │ │ │ │ │ -207 │ │ │ │ │ -_2_0_9 const _K_e_y_V_e_c_t_o_r &_k_e_y_s(size_t k) const { return measurements_[k].keys(); } │ │ │ │ │ -210 │ │ │ │ │ -214 │ │ │ │ │ -_2_1_5 Sparse _D() const { return D_; } │ │ │ │ │ -_2_1_6 Matrix _d_e_n_s_e_D() const { return Matrix(D_); } │ │ │ │ │ -_2_1_7 Sparse _Q() const { return Q_; } │ │ │ │ │ -_2_1_8 Matrix _d_e_n_s_e_Q() const { return Matrix(Q_); } │ │ │ │ │ -_2_1_9 Sparse _L() const { return L_; } │ │ │ │ │ -_2_2_0 Matrix _d_e_n_s_e_L() const { return Matrix(L_); } │ │ │ │ │ -221 │ │ │ │ │ -223 Sparse computeLambda(const Matrix &S) const; │ │ │ │ │ -224 │ │ │ │ │ -_2_2_6 Matrix _c_o_m_p_u_t_e_L_a_m_b_d_a__(const _V_a_l_u_e_s &values) const { │ │ │ │ │ -227 return Matrix(computeLambda(values)); │ │ │ │ │ -228 } │ │ │ │ │ -229 │ │ │ │ │ -_2_3_1 Matrix _c_o_m_p_u_t_e_L_a_m_b_d_a__(const Matrix &S) const { │ │ │ │ │ -232 return Matrix(computeLambda(S)); │ │ │ │ │ -233 } │ │ │ │ │ -234 │ │ │ │ │ -236 Sparse computeA(const _V_a_l_u_e_s &values) const; │ │ │ │ │ -237 │ │ │ │ │ -239 Sparse computeA(const Matrix &S) const; │ │ │ │ │ +_1_6_1 double _o_p_e_r_a_t_o_r_(_)(const typename DERIVED::Parameters& p, │ │ │ │ │ +162 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_-_1_,_ _-_1_> H = boost::none) const { │ │ │ │ │ +163 return _a_p_p_l_y(p, H); // might call apply in derived │ │ │ │ │ +164 } │ │ │ │ │ +165 │ │ │ │ │ +166 void print(const std::string& s = "") const { │ │ │ │ │ +167 std::cout << s << (s != "" ? " " : "") << weights_ << std::endl; │ │ │ │ │ +168 } │ │ │ │ │ +169 }; │ │ │ │ │ +170 │ │ │ │ │ +177 template │ │ │ │ │ +_1_7_8 class _V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r : protected _E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r { │ │ │ │ │ +179 protected: │ │ │ │ │ +180 using VectorM = Eigen::Matrix; │ │ │ │ │ +181 using Jacobian = Eigen::Matrix; │ │ │ │ │ +182 Jacobian H_; │ │ │ │ │ +183 │ │ │ │ │ +_1_9_2 void _c_a_l_c_u_l_a_t_e_J_a_c_o_b_i_a_n() { │ │ │ │ │ +193 H_ = kroneckerProductIdentity(this->weights_); │ │ │ │ │ +194 } │ │ │ │ │ +195 │ │ │ │ │ +196 public: │ │ │ │ │ +197 EIGEN_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ +198 │ │ │ │ │ +_2_0_0 _V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r() {} │ │ │ │ │ +201 │ │ │ │ │ +_2_0_3 _V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r(size_t N, double x) : _E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r(N, x) { │ │ │ │ │ +204 _c_a_l_c_u_l_a_t_e_J_a_c_o_b_i_a_n(); │ │ │ │ │ +205 } │ │ │ │ │ +206 │ │ │ │ │ +_2_0_8 _V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r(size_t N, double x, double a, double b) │ │ │ │ │ +209 : _E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r(N, x, a, b) { │ │ │ │ │ +210 _c_a_l_c_u_l_a_t_e_J_a_c_o_b_i_a_n(); │ │ │ │ │ +211 } │ │ │ │ │ +212 │ │ │ │ │ +_2_1_4 VectorM _a_p_p_l_y(const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>& P, │ │ │ │ │ +215 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_/_*_M_x_N_*_/_ _-_1_,_ _-_1_> H = boost::none) const { │ │ │ │ │ +216 if (H) *H = H_; │ │ │ │ │ +217 return P.matrix() * this->weights_.transpose(); │ │ │ │ │ +218 } │ │ │ │ │ +219 │ │ │ │ │ +_2_2_1 VectorM _o_p_e_r_a_t_o_r_(_)(const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>& P, │ │ │ │ │ +222 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_/_*_M_x_N_*_/_ _-_1_,_ _-_1_> H = boost::none) const { │ │ │ │ │ +223 return _a_p_p_l_y(P, H); │ │ │ │ │ +224 } │ │ │ │ │ +225 }; │ │ │ │ │ +226 │ │ │ │ │ +234 template │ │ │ │ │ +_2_3_5 class _V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r : public _E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r { │ │ │ │ │ +236 protected: │ │ │ │ │ +237 using Jacobian = Eigen::Matrix; │ │ │ │ │ +238 size_t rowIndex_; │ │ │ │ │ +239 Jacobian H_; │ │ │ │ │ 240 │ │ │ │ │ -_2_4_2 Matrix _c_o_m_p_u_t_e_A__(const _V_a_l_u_e_s &values) const { │ │ │ │ │ -243 return Matrix(computeA(values)); │ │ │ │ │ -244 } │ │ │ │ │ -245 │ │ │ │ │ -247 static Matrix StiefelElementMatrix(const _V_a_l_u_e_s &values); │ │ │ │ │ -248 │ │ │ │ │ -253 double computeMinEigenValue(const _V_a_l_u_e_s &values, │ │ │ │ │ -254 Vector *minEigenVector = nullptr) const; │ │ │ │ │ +241 /* │ │ │ │ │ +242 * Calculate the `1*(M*N)` Jacobian of this functor with respect to │ │ │ │ │ +243 * the M*N parameter matrix `P`. │ │ │ │ │ +244 * We flatten assuming column-major order, e.g., if N=3 and M=2, we have │ │ │ │ │ +245 * H=[w(0) 0 w(1) 0 w(2) 0] for rowIndex==0 │ │ │ │ │ +246 * H=[0 w(0) 0 w(1) 0 w(2)] for rowIndex==1 │ │ │ │ │ +247 * i.e., one row of the Kronecker product of weights_ with the │ │ │ │ │ +248 * MxM identity matrix. See also VectorEvaluationFunctor. │ │ │ │ │ +249 */ │ │ │ │ │ +250 void calculateJacobian(size_t N) { │ │ │ │ │ +251 H_.setZero(1, M * N); │ │ │ │ │ +252 for (int j = 0; j < EvaluationFunctor::weights_.size(); j++) │ │ │ │ │ +253 H_(0, rowIndex_ + j * M) = EvaluationFunctor::weights_(j); │ │ │ │ │ +254 } │ │ │ │ │ 255 │ │ │ │ │ -260 double computeMinEigenValueAP(const _V_a_l_u_e_s &values, │ │ │ │ │ -261 Vector *minEigenVector = nullptr) const; │ │ │ │ │ -262 │ │ │ │ │ -_2_6_4 _V_a_l_u_e_s _r_o_u_n_d_S_o_l_u_t_i_o_n_S(const Matrix &S) const; │ │ │ │ │ +256 public: │ │ │ │ │ +_2_5_8 _V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r() {} │ │ │ │ │ +259 │ │ │ │ │ +_2_6_1 _V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r(size_t N, size_t i, double x) │ │ │ │ │ +262 : _E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r(N, x), rowIndex_(i) { │ │ │ │ │ +263 calculateJacobian(N); │ │ │ │ │ +264 } │ │ │ │ │ 265 │ │ │ │ │ -267 static _V_e_c_t_o_r_V_a_l_u_e_s TangentVectorValues(size_t p, const Vector &v); │ │ │ │ │ -268 │ │ │ │ │ -270 Matrix riemannianGradient(size_t p, const _V_a_l_u_e_s &values) const; │ │ │ │ │ +_2_6_7 _V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r(size_t N, size_t i, double x, double a, double b) │ │ │ │ │ +268 : _E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r(N, x, a, b), rowIndex_(i) { │ │ │ │ │ +269 calculateJacobian(N); │ │ │ │ │ +270 } │ │ │ │ │ 271 │ │ │ │ │ -276 static _V_a_l_u_e_s LiftwithDescent(size_t p, const _V_a_l_u_e_s &values, │ │ │ │ │ -277 const Vector &minEigenVector); │ │ │ │ │ +_2_7_3 double _a_p_p_l_y(const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>& P, │ │ │ │ │ +274 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_/_*_1_x_M_N_*_/_ _-_1_,_ _-_1_> H = boost::none) const { │ │ │ │ │ +275 if (H) *H = H_; │ │ │ │ │ +276 return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose(); │ │ │ │ │ +277 } │ │ │ │ │ 278 │ │ │ │ │ -286 _V_a_l_u_e_s initializeWithDescent( │ │ │ │ │ -287 size_t p, const _V_a_l_u_e_s &values, const Vector &minEigenVector, │ │ │ │ │ -288 double minEigenValue, double gradienTolerance = 1e-2, │ │ │ │ │ -289 double preconditionedGradNormTolerance = 1e-4) const; │ │ │ │ │ -293 │ │ │ │ │ -298 _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h buildGraphAt(size_t p) const; │ │ │ │ │ -299 │ │ │ │ │ -305 _V_a_l_u_e_s initializeRandomlyAt(size_t p, std::mt19937 &rng) const; │ │ │ │ │ -306 │ │ │ │ │ -308 _V_a_l_u_e_s initializeRandomlyAt(size_t p) const; │ │ │ │ │ -309 │ │ │ │ │ -314 double costAt(size_t p, const _V_a_l_u_e_s &values) const; │ │ │ │ │ +_2_8_0 double _o_p_e_r_a_t_o_r_(_)(const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>& P, │ │ │ │ │ +281 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_/_*_1_x_M_N_*_/_ _-_1_,_ _-_1_> H = boost::none) const { │ │ │ │ │ +282 return _a_p_p_l_y(P, H); │ │ │ │ │ +283 } │ │ │ │ │ +284 }; │ │ │ │ │ +285 │ │ │ │ │ +299 template │ │ │ │ │ +_3_0_0 class _M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r │ │ │ │ │ +301 : public _V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r::dimension> { │ │ │ │ │ +302 enum { M = _t_r_a_i_t_s_<_T_>_:_:_d_i_m_e_n_s_i_o_n }; │ │ │ │ │ +303 using Base = _V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_<_M_>; │ │ │ │ │ +304 │ │ │ │ │ +305 public: │ │ │ │ │ +_3_0_7 _M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r() {} │ │ │ │ │ +308 │ │ │ │ │ +_3_1_0 _M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r(size_t N, double x) : Base(N, x) {} │ │ │ │ │ +311 │ │ │ │ │ +_3_1_3 _M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r(size_t N, double x, double a, double b) │ │ │ │ │ +314 : Base(N, x, a, b) {} │ │ │ │ │ 315 │ │ │ │ │ -321 Sparse computeLambda(const _V_a_l_u_e_s &values) const; │ │ │ │ │ -322 │ │ │ │ │ -328 std::pair computeMinEigenVector(const _V_a_l_u_e_s &values) │ │ │ │ │ -const; │ │ │ │ │ -329 │ │ │ │ │ -334 bool checkOptimality(const _V_a_l_u_e_s &values) const; │ │ │ │ │ -335 │ │ │ │ │ -342 boost::shared_ptr createOptimizerAt( │ │ │ │ │ -343 size_t p, const _V_a_l_u_e_s &initial) const; │ │ │ │ │ -344 │ │ │ │ │ -351 _V_a_l_u_e_s tryOptimizingAt(size_t p, const _V_a_l_u_e_s &initial) const; │ │ │ │ │ -352 │ │ │ │ │ -_3_5_7 _V_a_l_u_e_s _p_r_o_j_e_c_t_F_r_o_m(size_t p, const _V_a_l_u_e_s &values) const; │ │ │ │ │ -358 │ │ │ │ │ -363 _V_a_l_u_e_s roundSolution(const _V_a_l_u_e_s &values) const; │ │ │ │ │ -364 │ │ │ │ │ -366 template │ │ │ │ │ -_3_6_7 static _V_a_l_u_e_s _L_i_f_t_T_o(size_t p, const _V_a_l_u_e_s &values) { │ │ │ │ │ -368 _V_a_l_u_e_s result; │ │ │ │ │ -369 for (const auto it : values._e_x_t_r_a_c_t()) { │ │ │ │ │ -370 result.insert(it.first, SOn::Lift(p, it.second.matrix())); │ │ │ │ │ -371 } │ │ │ │ │ -372 return result; │ │ │ │ │ -373 } │ │ │ │ │ -374 │ │ │ │ │ +_3_1_7 T _a_p_p_l_y(const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>& P, │ │ │ │ │ +318 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_/_*_M_x_M_N_*_/_ _-_1_,_ _-_1_> H = boost::none) const { │ │ │ │ │ +319 // Interpolate the M-dimensional vector to yield a vector in tangent space │ │ │ │ │ +320 Eigen::Matrix xi = Base::operator()(P, H); │ │ │ │ │ +321 │ │ │ │ │ +322 // Now call retract with this M-vector, possibly with derivatives │ │ │ │ │ +323 Eigen::Matrix D_result_xi; │ │ │ │ │ +324 T result = T::ChartAtOrigin::Retract(xi, H ? &D_result_xi : 0); │ │ │ │ │ +325 │ │ │ │ │ +326 // Finally, if derivatives are asked, apply chain rule where H is Mx(M*N) │ │ │ │ │ +327 // derivative of interpolation and D_result_xi is MxM derivative of │ │ │ │ │ +328 // retract. │ │ │ │ │ +329 if (H) *H = D_result_xi * (*H); │ │ │ │ │ +330 │ │ │ │ │ +331 // and return a T │ │ │ │ │ +332 return result; │ │ │ │ │ +333 } │ │ │ │ │ +334 │ │ │ │ │ +_3_3_6 T _o_p_e_r_a_t_o_r_(_)(const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>& P, │ │ │ │ │ +337 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_/_*_M_x_N_*_/_ _-_1_,_ _-_1_> H = boost::none) const { │ │ │ │ │ +338 return _a_p_p_l_y(P, H); // might call apply in derived │ │ │ │ │ +339 } │ │ │ │ │ +340 }; │ │ │ │ │ +341 │ │ │ │ │ +_3_4_3 class _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e { │ │ │ │ │ +344 protected: │ │ │ │ │ +345 Weights weights_; │ │ │ │ │ +346 │ │ │ │ │ +347 public: │ │ │ │ │ +_3_4_9 _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e() {} │ │ │ │ │ +350 │ │ │ │ │ +351 _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e(size_t N, double x) │ │ │ │ │ +352 : weights_(DERIVED::DerivativeWeights(N, x)) {} │ │ │ │ │ +353 │ │ │ │ │ +354 _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e(size_t N, double x, double a, double b) │ │ │ │ │ +355 : weights_(DERIVED::DerivativeWeights(N, x, a, b)) {} │ │ │ │ │ +356 │ │ │ │ │ +357 void print(const std::string& s = "") const { │ │ │ │ │ +358 std::cout << s << (s != "" ? " " : "") << weights_ << std::endl; │ │ │ │ │ +359 } │ │ │ │ │ +360 }; │ │ │ │ │ +361 │ │ │ │ │ +_3_6_9 class _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r : protected _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e { │ │ │ │ │ +370 public: │ │ │ │ │ +_3_7_2 _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r() {} │ │ │ │ │ +373 │ │ │ │ │ +374 _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r(size_t N, double x) : _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e(N, x) {} │ │ │ │ │ +375 │ │ │ │ │ +376 _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r(size_t N, double x, double a, double b) │ │ │ │ │ +377 : _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e(N, x, a, b) {} │ │ │ │ │ 378 │ │ │ │ │ -383 double cost(const _V_a_l_u_e_s &values) const; │ │ │ │ │ -384 │ │ │ │ │ -392 _V_a_l_u_e_s initializeRandomly(std::mt19937 &rng) const; │ │ │ │ │ -393 │ │ │ │ │ -395 _V_a_l_u_e_s initializeRandomly() const; │ │ │ │ │ -396 │ │ │ │ │ -404 std::pair run(const _V_a_l_u_e_s &initialEstimate, size_t pMin = │ │ │ │ │ -d, │ │ │ │ │ -405 size_t pMax = 10) const; │ │ │ │ │ -407 │ │ │ │ │ -417 template │ │ │ │ │ -_4_1_8 inline std::vector> _m_a_y_b_e_R_o_b_u_s_t( │ │ │ │ │ -419 const std::vector<_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_<_T_>> &measurements, │ │ │ │ │ -420 bool useRobustModel = false) const { │ │ │ │ │ -421 return useRobustModel ? makeNoiseModelRobust(measurements) : measurements; │ │ │ │ │ -422 } │ │ │ │ │ -423}; │ │ │ │ │ -424 │ │ │ │ │ -425// Subclasses for d=2 and d=3 that explicitly instantiate, as well as │ │ │ │ │ -provide a │ │ │ │ │ -426// convenience interface with file access. │ │ │ │ │ -427 │ │ │ │ │ -_4_2_8class GTSAM_EXPORT _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_2 : public _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g<2> { │ │ │ │ │ -429 public: │ │ │ │ │ -430 _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_2(const Measurements &measurements, │ │ │ │ │ -431 const Parameters ¶meters = Parameters()); │ │ │ │ │ -432 explicit _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_2(std::string g2oFile, │ │ │ │ │ -433 const Parameters ¶meters = Parameters()); │ │ │ │ │ -434 _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_2(const BetweenFactorPose2s &factors, │ │ │ │ │ -435 const Parameters ¶meters = Parameters()); │ │ │ │ │ -436}; │ │ │ │ │ -437 │ │ │ │ │ -_4_3_8class GTSAM_EXPORT _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_3 : public _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g<3> { │ │ │ │ │ -439 public: │ │ │ │ │ -440 _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_3(const Measurements &measurements, │ │ │ │ │ -441 const Parameters ¶meters = Parameters()); │ │ │ │ │ -442 explicit _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_3(std::string g2oFile, │ │ │ │ │ -443 const Parameters ¶meters = Parameters()); │ │ │ │ │ -444 │ │ │ │ │ -445 // TODO(frank): Deprecate after we land pybind wrapper │ │ │ │ │ -446 _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_3(const BetweenFactorPose3s &factors, │ │ │ │ │ -447 const Parameters ¶meters = Parameters()); │ │ │ │ │ -448}; │ │ │ │ │ -449} // namespace gtsam │ │ │ │ │ -_V_e_c_t_o_r_._h │ │ │ │ │ -typedef and functions to augment Eigen's VectorXd │ │ │ │ │ +379 double apply(const typename DERIVED::Parameters& p, │ │ │ │ │ +380 OptionalJacobian H = boost::none) const { │ │ │ │ │ +381 if (H) *H = this->weights_; │ │ │ │ │ +382 return (this->weights_ * p)(0); │ │ │ │ │ +383 } │ │ │ │ │ +_3_8_5 double _o_p_e_r_a_t_o_r_(_)(const typename DERIVED::Parameters& p, │ │ │ │ │ +386 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_/_*_1_x_N_*_/_ _-_1_,_ _-_1_> H = boost::none) const { │ │ │ │ │ +387 return apply(p, H); // might call apply in derived │ │ │ │ │ +388 } │ │ │ │ │ +389 }; │ │ │ │ │ +390 │ │ │ │ │ +399 template │ │ │ │ │ +_4_0_0 class _V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r : protected _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e { │ │ │ │ │ +401 protected: │ │ │ │ │ +402 using VectorM = Eigen::Matrix; │ │ │ │ │ +403 using Jacobian = Eigen::Matrix; │ │ │ │ │ +404 Jacobian H_; │ │ │ │ │ +405 │ │ │ │ │ +_4_1_4 void _c_a_l_c_u_l_a_t_e_J_a_c_o_b_i_a_n() { │ │ │ │ │ +415 H_ = kroneckerProductIdentity(this->weights_); │ │ │ │ │ +416 } │ │ │ │ │ +417 │ │ │ │ │ +418 public: │ │ │ │ │ +419 EIGEN_MAKE_ALIGNED_OPERATOR_NEW │ │ │ │ │ +420 │ │ │ │ │ +_4_2_2 _V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r() {} │ │ │ │ │ +423 │ │ │ │ │ +_4_2_5 _V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r(size_t N, double x) : _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e(N, x) { │ │ │ │ │ +426 _c_a_l_c_u_l_a_t_e_J_a_c_o_b_i_a_n(); │ │ │ │ │ +427 } │ │ │ │ │ +428 │ │ │ │ │ +_4_3_0 _V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r(size_t N, double x, double a, double b) │ │ │ │ │ +431 : _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e(N, x, a, b) { │ │ │ │ │ +432 _c_a_l_c_u_l_a_t_e_J_a_c_o_b_i_a_n(); │ │ │ │ │ +433 } │ │ │ │ │ +434 │ │ │ │ │ +435 VectorM apply(const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>& P, │ │ │ │ │ +436 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_/_*_M_x_M_N_*_/_ _-_1_,_ _-_1_> H = boost::none) const { │ │ │ │ │ +437 if (H) *H = H_; │ │ │ │ │ +438 return P.matrix() * this->weights_.transpose(); │ │ │ │ │ +439 } │ │ │ │ │ +_4_4_1 VectorM _o_p_e_r_a_t_o_r_(_)( │ │ │ │ │ +442 const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>& P, │ │ │ │ │ +443 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_/_*_M_x_M_N_*_/_ _-_1_,_ _-_1_> H = boost::none) const { │ │ │ │ │ +444 return apply(P, H); │ │ │ │ │ +445 } │ │ │ │ │ +446 }; │ │ │ │ │ +447 │ │ │ │ │ +455 template │ │ │ │ │ +_4_5_6 class _C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r : protected _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e { │ │ │ │ │ +457 protected: │ │ │ │ │ +458 using Jacobian = Eigen::Matrix; │ │ │ │ │ +459 size_t rowIndex_; │ │ │ │ │ +460 Jacobian H_; │ │ │ │ │ +461 │ │ │ │ │ +462 /* │ │ │ │ │ +463 * Calculate the `1*(M*N)` Jacobian of this functor with respect to │ │ │ │ │ +464 * the M*N parameter matrix `P`. │ │ │ │ │ +465 * We flatten assuming column-major order, e.g., if N=3 and M=2, we have │ │ │ │ │ +466 * H=[w(0) 0 w(1) 0 w(2) 0] for rowIndex==0 │ │ │ │ │ +467 * H=[0 w(0) 0 w(1) 0 w(2)] for rowIndex==1 │ │ │ │ │ +468 * i.e., one row of the Kronecker product of weights_ with the │ │ │ │ │ +469 * MxM identity matrix. See also VectorDerivativeFunctor. │ │ │ │ │ +470 */ │ │ │ │ │ +471 void calculateJacobian(size_t N) { │ │ │ │ │ +472 H_.setZero(1, M * N); │ │ │ │ │ +473 for (int j = 0; j < this->weights_.size(); j++) │ │ │ │ │ +474 H_(0, rowIndex_ + j * M) = this->weights_(j); │ │ │ │ │ +475 } │ │ │ │ │ +476 │ │ │ │ │ +477 public: │ │ │ │ │ +_4_7_9 _C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r() {} │ │ │ │ │ +480 │ │ │ │ │ +_4_8_2 _C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r(size_t N, size_t i, double x) │ │ │ │ │ +483 : _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e(N, x), rowIndex_(i) { │ │ │ │ │ +484 calculateJacobian(N); │ │ │ │ │ +485 } │ │ │ │ │ +486 │ │ │ │ │ +_4_8_8 _C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r(size_t N, size_t i, double x, double a, double │ │ │ │ │ +b) │ │ │ │ │ +489 : _D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e(N, x, a, b), rowIndex_(i) { │ │ │ │ │ +490 calculateJacobian(N); │ │ │ │ │ +491 } │ │ │ │ │ +_4_9_3 double _a_p_p_l_y(const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>& P, │ │ │ │ │ +494 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_/_*_1_x_M_N_*_/_ _-_1_,_ _-_1_> H = boost::none) const { │ │ │ │ │ +495 if (H) *H = H_; │ │ │ │ │ +496 return P.row(rowIndex_) * this->weights_.transpose(); │ │ │ │ │ +497 } │ │ │ │ │ +_4_9_9 double _o_p_e_r_a_t_o_r_(_)(const _P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_<_M_>& P, │ │ │ │ │ +500 _O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_<_/_*_1_x_M_N_*_/_ _-_1_,_ _-_1_> H = boost::none) const { │ │ │ │ │ +501 return _a_p_p_l_y(P, H); │ │ │ │ │ +502 } │ │ │ │ │ +503 }; │ │ │ │ │ +504 │ │ │ │ │ +505}; │ │ │ │ │ +506 │ │ │ │ │ +507} // namespace gtsam │ │ │ │ │ +_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n_._h │ │ │ │ │ +Special class for optional Jacobian arguments. │ │ │ │ │ _M_a_t_r_i_x_._h │ │ │ │ │ typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ -_R_o_t_3_._h │ │ │ │ │ -3D rotation represented as a rotation matrix or quaternion │ │ │ │ │ -_R_o_t_2_._h │ │ │ │ │ -2D rotation │ │ │ │ │ -_P_o_w_e_r_M_e_t_h_o_d_._h │ │ │ │ │ -Power method for fast eigenvalue and eigenvector computation. │ │ │ │ │ -_A_c_c_e_l_e_r_a_t_e_d_P_o_w_e_r_M_e_t_h_o_d_._h │ │ │ │ │ -accelerated power method for fast eigenvalue and eigenvector computation │ │ │ │ │ -_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ -Factor Graph Values. │ │ │ │ │ -_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s_._h │ │ │ │ │ -Parameters for Levenberg-Marquardt trust-region scheme. │ │ │ │ │ -_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t_._h │ │ │ │ │ -Binary measurement represents a measurement between two keys in a graph. A │ │ │ │ │ -binary measurement is simi... │ │ │ │ │ -_d_a_t_a_s_e_t_._h │ │ │ │ │ -utility functions for loading datasets │ │ │ │ │ +_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x_._h │ │ │ │ │ +Define ParameterMatrix class which is used to store values at interpolation │ │ │ │ │ +points. │ │ │ │ │ +_g_t_s_a_m_:_:_k_r_o_n_e_c_k_e_r_P_r_o_d_u_c_t_I_d_e_n_t_i_t_y │ │ │ │ │ +Matrix kroneckerProductIdentity(const Weights &w) │ │ │ │ │ +Function for computing the kronecker product of the 1*N Weight vector w with │ │ │ │ │ +the MxM identity matrix ... │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:85 │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_a_r_e_d_N_o_i_s_e_M_o_d_e_l │ │ │ │ │ -noiseModel::Base::shared_ptr SharedNoiseModel │ │ │ │ │ -Aliases. │ │ │ │ │ -DDeeffiinniittiioonn NoiseModel.h:724 │ │ │ │ │ -_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ -VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ -with a unique integer... │ │ │ │ │ -DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ -_g_t_s_a_m_:_:_L_e_v_e_n_b_e_r_g_M_a_r_q_u_a_r_d_t_P_a_r_a_m_s │ │ │ │ │ -Parameters for Levenberg-Marquardt optimization. │ │ │ │ │ -DDeeffiinniittiioonn LevenbergMarquardtParams.h:35 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactorGraph.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_e_x_t_r_a_c_t │ │ │ │ │ -std::map< Key, ValueType > extract(const std::function< bool(Key)> │ │ │ │ │ -&filterFcn=&_truePredicate< Key >) const │ │ │ │ │ -Extract a subset of values of the given type ValueType. │ │ │ │ │ -DDeeffiinniittiioonn Values-inl.h:94 │ │ │ │ │ -_g_t_s_a_m_:_:_B_i_n_a_r_y_M_e_a_s_u_r_e_m_e_n_t │ │ │ │ │ -DDeeffiinniittiioonn BinaryMeasurement.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ -Parameters governing optimization etc. │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:46 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s_:_:_a_l_p_h_a │ │ │ │ │ -double alpha │ │ │ │ │ -weight of anchor-based prior (default 0) │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:55 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="") const │ │ │ │ │ -Print the parameters and flags used for rotation averaging. │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:94 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s_:_:_l_m │ │ │ │ │ -LevenbergMarquardtParams lm │ │ │ │ │ -LM parameters. │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:52 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s_:_:_u_s_e_H_u_b_e_r │ │ │ │ │ -bool useHuber │ │ │ │ │ -if enabled, the Huber loss is used (default false) │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:59 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s_:_:_o_p_t_i_m_a_l_i_t_y_T_h_r_e_s_h_o_l_d │ │ │ │ │ -double optimalityThreshold │ │ │ │ │ -threshold used in checkOptimality │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:53 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s_:_:_b_e_t_a │ │ │ │ │ -double beta │ │ │ │ │ -weight of Karcher-based prior (default 1) │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:56 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s_:_:_g_a_m_m_a │ │ │ │ │ -double gamma │ │ │ │ │ -weight of gauge-fixing factors (default 0) │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s_:_:_a_n_c_h_o_r │ │ │ │ │ -Anchor anchor │ │ │ │ │ -pose to use as anchor if not Karcher │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:54 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_P_a_r_a_m_e_t_e_r_s_:_:_c_e_r_t_i_f_y_O_p_t_i_m_a_l_i_t_y │ │ │ │ │ -bool certifyOptimality │ │ │ │ │ -if enabled solution optimality is certified (default true) │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:61 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g │ │ │ │ │ -Class that implements Shonan Averaging from our ECCV'20 paper. │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:123 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_D │ │ │ │ │ -Sparse D() const │ │ │ │ │ -Sparse version of D. │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:215 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_m_e_a_s_u_r_e_m_e_n_t │ │ │ │ │ -const BinaryMeasurement< Rot > & measurement(size_t k) const │ │ │ │ │ -k^th binary measurement │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:171 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_m_a_k_e_N_o_i_s_e_M_o_d_e_l_R_o_b_u_s_t │ │ │ │ │ -Measurements makeNoiseModelRobust(const Measurements &measurements, double │ │ │ │ │ -k=1.345) const │ │ │ │ │ -Update factors to use robust Huber loss. │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:181 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_r_o_u_n_d_S_o_l_u_t_i_o_n_S │ │ │ │ │ -Values roundSolutionS(const Matrix &S) const │ │ │ │ │ -Project pxdN Stiefel manifold matrix S to Rot3^N. │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_m_a_y_b_e_R_o_b_u_s_t │ │ │ │ │ -std::vector< BinaryMeasurement< T > > maybeRobust(const std::vector< │ │ │ │ │ -BinaryMeasurement< T > > &measurements, bool useRobustModel=false) const │ │ │ │ │ -Helper function to convert measurements to robust noise model if flag is set. │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:418 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_L │ │ │ │ │ -Sparse L() const │ │ │ │ │ -Sparse version of L. │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:219 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_m_e_a_s_u_r_e_d │ │ │ │ │ -const Rot & measured(size_t k) const │ │ │ │ │ -k^th measurement, as a Rot. │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:206 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_Q │ │ │ │ │ -Sparse Q() const │ │ │ │ │ -Sparse version of Q. │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:217 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_d_e_n_s_e_D │ │ │ │ │ -Matrix denseD() const │ │ │ │ │ -Dense version of D. │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:216 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_n_r_U_n_k_n_o_w_n_s │ │ │ │ │ -size_t nrUnknowns() const │ │ │ │ │ -Return number of unknowns. │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:165 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_p_r_o_j_e_c_t_F_r_o_m │ │ │ │ │ -Values projectFrom(size_t p, const Values &values) const │ │ │ │ │ -Project from SO(p) to Rot2 or Rot3 Values should be of type SO(p) │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_n_u_m_b_e_r_M_e_a_s_u_r_e_m_e_n_t_s │ │ │ │ │ -size_t numberMeasurements() const │ │ │ │ │ -Return number of measurements. │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:168 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_d_e_n_s_e_L │ │ │ │ │ -Matrix denseL() const │ │ │ │ │ -Dense version of L. │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:220 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_c_o_m_p_u_t_e_L_a_m_b_d_a__ │ │ │ │ │ -Matrix computeLambda_(const Matrix &S) const │ │ │ │ │ -Dense versions of computeLambda for wrapper/testing. │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:231 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_d_e_n_s_e_Q │ │ │ │ │ -Matrix denseQ() const │ │ │ │ │ -Dense version of Q. │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:218 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_k_e_y_s │ │ │ │ │ -const KeyVector & keys(size_t k) const │ │ │ │ │ -Keys for k^th measurement, as a vector of Key values. │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:209 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_c_o_m_p_u_t_e_L_a_m_b_d_a__ │ │ │ │ │ -Matrix computeLambda_(const Values &values) const │ │ │ │ │ -Dense versions of computeLambda for wrapper/testing. │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:226 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_c_o_m_p_u_t_e_A__ │ │ │ │ │ -Matrix computeA_(const Values &values) const │ │ │ │ │ -Dense version of computeA for wrapper/testing. │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:242 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_:_:_L_i_f_t_T_o │ │ │ │ │ -static Values LiftTo(size_t p, const Values &values) │ │ │ │ │ -Lift Values of type T to SO(p) │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:367 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_2 │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:428 │ │ │ │ │ -_g_t_s_a_m_:_:_S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_3 │ │ │ │ │ -DDeeffiinniittiioonn ShonanAveraging.h:438 │ │ │ │ │ +_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ +A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ +that can be centered ... │ │ │ │ │ +DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ +_g_t_s_a_m_:_:_O_p_t_i_o_n_a_l_J_a_c_o_b_i_a_n │ │ │ │ │ +OptionalJacobian is an Eigen::Ref like class that can take be constructed using │ │ │ │ │ +either a fixed size o... │ │ │ │ │ +DDeeffiinniittiioonn OptionalJacobian.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s │ │ │ │ │ +CRTP Base class for function bases. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_W_e_i_g_h_t_M_a_t_r_i_x │ │ │ │ │ +static Matrix WeightMatrix(size_t N, const Vector &X, double a, double b) │ │ │ │ │ +Calculate weights for all x in vector X, with interval [a,b]. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:123 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_W_e_i_g_h_t_M_a_t_r_i_x │ │ │ │ │ +static Matrix WeightMatrix(size_t N, const Vector &X) │ │ │ │ │ +Calculate weights for all x in vector X. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:107 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r │ │ │ │ │ +An instance of an EvaluationFunctor calculates f(x;p) at a given x, applied to │ │ │ │ │ +Parameters p. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:137 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r │ │ │ │ │ +EvaluationFunctor(size_t N, double x) │ │ │ │ │ +Constructor with interval [a,b]. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:146 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r │ │ │ │ │ +EvaluationFunctor(size_t N, double x, double a, double b) │ │ │ │ │ +Constructor with interval [a,b]. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:150 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_a_p_p_l_y │ │ │ │ │ +double apply(const typename DERIVED::Parameters &p, OptionalJacobian<-1, -1 > │ │ │ │ │ +H=boost::none) const │ │ │ │ │ +Regular 1D evaluation. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:154 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r │ │ │ │ │ +EvaluationFunctor() │ │ │ │ │ +For serialization. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:143 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ +double operator()(const typename DERIVED::Parameters &p, OptionalJacobian<-1, - │ │ │ │ │ +1 > H=boost::none) const │ │ │ │ │ +c++ sugar │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:161 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r │ │ │ │ │ +VectorEvaluationFunctor at a given x, applied to ParameterMatrix. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:178 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r │ │ │ │ │ +VectorEvaluationFunctor(size_t N, double x, double a, double b) │ │ │ │ │ +Constructor, with interval [a,b]. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:208 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r │ │ │ │ │ +EIGEN_MAKE_ALIGNED_OPERATOR_NEW VectorEvaluationFunctor() │ │ │ │ │ +For serialization. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:200 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r │ │ │ │ │ +VectorEvaluationFunctor(size_t N, double x) │ │ │ │ │ +Default Constructor. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:203 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_c_a_l_c_u_l_a_t_e_J_a_c_o_b_i_a_n │ │ │ │ │ +void calculateJacobian() │ │ │ │ │ +Calculate the M*(M*N) Jacobian of this functor with respect to the M*N │ │ │ │ │ +parameter matrix P. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:192 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ +VectorM operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > │ │ │ │ │ +H=boost::none) const │ │ │ │ │ +c++ sugar │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:221 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_a_p_p_l_y │ │ │ │ │ +VectorM apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > │ │ │ │ │ +H=boost::none) const │ │ │ │ │ +M-dimensional evaluation. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:214 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r │ │ │ │ │ +Given a M*N Matrix of M-vectors at N polynomial points, an instance of │ │ │ │ │ +VectorComponentFunctor compute... │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:235 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ +double operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > │ │ │ │ │ +H=boost::none) const │ │ │ │ │ +c++ sugar │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:280 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r │ │ │ │ │ +VectorComponentFunctor() │ │ │ │ │ +For serialization. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:258 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r │ │ │ │ │ +VectorComponentFunctor(size_t N, size_t i, double x) │ │ │ │ │ +Construct with row index. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:261 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r │ │ │ │ │ +VectorComponentFunctor(size_t N, size_t i, double x, double a, double b) │ │ │ │ │ +Construct with row index and interval. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:267 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_C_o_m_p_o_n_e_n_t_F_u_n_c_t_o_r_:_:_a_p_p_l_y │ │ │ │ │ +double apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > │ │ │ │ │ +H=boost::none) const │ │ │ │ │ +Calculate component of component rowIndex_ of P. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:273 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r │ │ │ │ │ +Manifold EvaluationFunctor at a given x, applied to ParameterMatrix. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:301 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r │ │ │ │ │ +ManifoldEvaluationFunctor(size_t N, double x, double a, double b) │ │ │ │ │ +Constructor, with interval [a,b]. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:313 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_a_p_p_l_y │ │ │ │ │ +T apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > H=boost:: │ │ │ │ │ +none) const │ │ │ │ │ +Manifold evaluation. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:317 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r │ │ │ │ │ +ManifoldEvaluationFunctor() │ │ │ │ │ +For serialization. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:307 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r │ │ │ │ │ +ManifoldEvaluationFunctor(size_t N, double x) │ │ │ │ │ +Default Constructor. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:310 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_M_a_n_i_f_o_l_d_E_v_a_l_u_a_t_i_o_n_F_u_n_c_t_o_r_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ +T operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > │ │ │ │ │ +H=boost::none) const │ │ │ │ │ +c++ sugar │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:336 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e │ │ │ │ │ +Base class for functors below that calculate derivative weights. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:343 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e_:_:_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_B_a_s_e │ │ │ │ │ +DerivativeFunctorBase() │ │ │ │ │ +For serialization. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:349 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r │ │ │ │ │ +An instance of a DerivativeFunctor calculates f'(x;p) at a given x, applied to │ │ │ │ │ +Parameters p. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:369 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_:_:_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r │ │ │ │ │ +DerivativeFunctor() │ │ │ │ │ +For serialization. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:372 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ +double operator()(const typename DERIVED::Parameters &p, OptionalJacobian< -1, │ │ │ │ │ +-1 > H=boost::none) const │ │ │ │ │ +c++ sugar │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:385 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r │ │ │ │ │ +VectorDerivativeFunctor at a given x, applied to ParameterMatrix. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:400 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_:_:_c_a_l_c_u_l_a_t_e_J_a_c_o_b_i_a_n │ │ │ │ │ +void calculateJacobian() │ │ │ │ │ +Calculate the M*(M*N) Jacobian of this functor with respect to the M*N │ │ │ │ │ +parameter matrix P. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:414 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r │ │ │ │ │ +VectorDerivativeFunctor(size_t N, double x, double a, double b) │ │ │ │ │ +Constructor, with optional interval [a,b]. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:430 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ +VectorM operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > │ │ │ │ │ +H=boost::none) const │ │ │ │ │ +c++ sugar │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:441 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r │ │ │ │ │ +EIGEN_MAKE_ALIGNED_OPERATOR_NEW VectorDerivativeFunctor() │ │ │ │ │ +For serialization. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:422 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_:_:_V_e_c_t_o_r_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r │ │ │ │ │ +VectorDerivativeFunctor(size_t N, double x) │ │ │ │ │ +Default Constructor. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:425 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r │ │ │ │ │ +Given a M*N Matrix of M-vectors at N polynomial points, an instance of │ │ │ │ │ +ComponentDerivativeFunctor com... │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:456 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r │ │ │ │ │ +ComponentDerivativeFunctor(size_t N, size_t i, double x) │ │ │ │ │ +Construct with row index. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:482 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ +double operator()(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > │ │ │ │ │ +H=boost::none) const │ │ │ │ │ +c++ sugar │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:499 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r │ │ │ │ │ +ComponentDerivativeFunctor() │ │ │ │ │ +For serialization. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:479 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r │ │ │ │ │ +ComponentDerivativeFunctor(size_t N, size_t i, double x, double a, double b) │ │ │ │ │ +Construct with row index and interval. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:488 │ │ │ │ │ +_g_t_s_a_m_:_:_B_a_s_i_s_:_:_C_o_m_p_o_n_e_n_t_D_e_r_i_v_a_t_i_v_e_F_u_n_c_t_o_r_:_:_a_p_p_l_y │ │ │ │ │ +double apply(const ParameterMatrix< M > &P, OptionalJacobian< -1, -1 > │ │ │ │ │ +H=boost::none) const │ │ │ │ │ +Calculate derivative of component rowIndex_ of F. │ │ │ │ │ +DDeeffiinniittiioonn Basis.h:493 │ │ │ │ │ +_g_t_s_a_m_:_:_P_a_r_a_m_e_t_e_r_M_a_t_r_i_x │ │ │ │ │ +A matrix abstraction of MxN values at the Basis points. │ │ │ │ │ +DDeeffiinniittiioonn ParameterMatrix.h:38 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _s_f_m │ │ │ │ │ - * _S_h_o_n_a_n_A_v_e_r_a_g_i_n_g_._h │ │ │ │ │ + * _b_a_s_i_s │ │ │ │ │ + * _B_a_s_i_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01238_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00617_source.html │ │ │ │┄ Files 88% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/KarcherMeanFactor.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/DotWriter.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
KarcherMeanFactor.h
│ │ │ │ +
DotWriter.h
│ │ │ │
│ │ │ │
│ │ │ │ -
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │ -
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
3 * GTSAM Copyright 2010-2021, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
12/*
│ │ │ │ -
13 * @file KarcherMeanFactor.h
│ │ │ │ -
14 * @author Frank Dellaert
│ │ │ │ -
15 * @date March 2019
│ │ │ │ -
16 */
│ │ │ │ -
17
│ │ │ │ -
18#pragma once
│ │ │ │ -
19
│ │ │ │ -
20#include <gtsam/base/Matrix.h>
│ │ │ │ -
21#include <gtsam/linear/JacobianFactor.h>
│ │ │ │ -
22
│ │ │ │ -
23#include <map>
│ │ │ │ -
24#include <vector>
│ │ │ │ -
25
│ │ │ │ -
26namespace gtsam {
│ │ │ │ -
32template <class T>
│ │ │ │ -
33T FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>> &rotations);
│ │ │ │ -
34
│ │ │ │ -
35template <class T> T FindKarcherMean(std::initializer_list<T> &&rotations);
│ │ │ │ -
36
│ │ │ │ -
│ │ │ │ -
45template <class T> class KarcherMeanFactor : public NonlinearFactor {
│ │ │ │ -
46 // Compile time dimension: can be -1
│ │ │ │ -
47 enum { D = traits<T>::dimension };
│ │ │ │ +
19#pragma once
│ │ │ │ +
20
│ │ │ │ +
21#include <gtsam/base/FastVector.h>
│ │ │ │ +
22#include <gtsam/base/Vector.h>
│ │ │ │ +
23#include <gtsam/inference/Key.h>
│ │ │ │ +
24
│ │ │ │ +
25#include <iosfwd>
│ │ │ │ +
26#include <map>
│ │ │ │ +
27#include <set>
│ │ │ │ +
28
│ │ │ │ +
29namespace gtsam {
│ │ │ │ +
30
│ │ │ │ +
│ │ │ │ +
35struct GTSAM_EXPORT DotWriter {
│ │ │ │ +
36 double figureWidthInches;
│ │ │ │ +
37 double figureHeightInches;
│ │ │ │ +
38 bool plotFactorPoints;
│ │ │ │ +
39 bool connectKeysToFactor;
│ │ │ │ +
41 bool binaryEdges;
│ │ │ │ +
42
│ │ │ │ +
47 std::map<Key, Vector2> variablePositions;
│ │ │ │
48
│ │ │ │ -
49 // Runtime dimension: always >=0
│ │ │ │ -
50 size_t d_;
│ │ │ │ -
51
│ │ │ │ -
53 boost::shared_ptr<JacobianFactor> whitenedJacobian_;
│ │ │ │ -
54
│ │ │ │ -
55public:
│ │ │ │ -
61 template <typename CONTAINER>
│ │ │ │ -
62 KarcherMeanFactor(const CONTAINER &keys, int d = D,
│ │ │ │ -
63 boost::optional<double> beta = boost::none);
│ │ │ │ +
54 std::map<char, double> positionHints;
│ │ │ │ +
55
│ │ │ │ +
57 std::set<Key> boxes;
│ │ │ │ +
58
│ │ │ │ +
63 std::map<size_t, Vector2> factorPositions;
│ │ │ │
64
│ │ │ │ -
66 ~KarcherMeanFactor() override {}
│ │ │ │ -
67
│ │ │ │ -
69 double error(const Values &c) const override { return 0; }
│ │ │ │ -
70
│ │ │ │ -
72 size_t dim() const override { return d_; }
│ │ │ │ -
73
│ │ │ │ -
│ │ │ │ -
75 boost::shared_ptr<GaussianFactor> linearize(const Values &c) const override {
│ │ │ │ -
76 return whitenedJacobian_;
│ │ │ │ -
77 }
│ │ │ │ -
│ │ │ │ -
78};
│ │ │ │ -
│ │ │ │ -
79// \KarcherMeanFactor
│ │ │ │ -
80} // namespace gtsam
│ │ │ │ -
Matrix.h
typedef and functions to augment Eigen's MatrixXd
│ │ │ │ - │ │ │ │ +
65 explicit DotWriter(double figureWidthInches = 5,
│ │ │ │ +
66 double figureHeightInches = 5,
│ │ │ │ +
67 bool plotFactorPoints = true,
│ │ │ │ +
68 bool connectKeysToFactor = true, bool binaryEdges = false)
│ │ │ │ +
69 : figureWidthInches(figureWidthInches),
│ │ │ │ +
70 figureHeightInches(figureHeightInches),
│ │ │ │ +
71 plotFactorPoints(plotFactorPoints),
│ │ │ │ +
72 connectKeysToFactor(connectKeysToFactor),
│ │ │ │ +
73 binaryEdges(binaryEdges) {}
│ │ │ │ +
74
│ │ │ │ +
76 void graphPreamble(std::ostream* os) const;
│ │ │ │ +
77
│ │ │ │ +
79 void digraphPreamble(std::ostream* os) const;
│ │ │ │ +
80
│ │ │ │ +
82 void drawVariable(Key key, const KeyFormatter& keyFormatter,
│ │ │ │ +
83 const boost::optional<Vector2>& position,
│ │ │ │ +
84 std::ostream* os) const;
│ │ │ │ +
85
│ │ │ │ +
87 static void DrawFactor(size_t i, const boost::optional<Vector2>& position,
│ │ │ │ +
88 std::ostream* os);
│ │ │ │ +
89
│ │ │ │ +
91 boost::optional<Vector2> variablePos(Key key) const;
│ │ │ │ +
92
│ │ │ │ +
94 void processFactor(size_t i, const KeyVector& keys,
│ │ │ │ +
95 const KeyFormatter& keyFormatter,
│ │ │ │ +
96 const boost::optional<Vector2>& position,
│ │ │ │ +
97 std::ostream* os) const;
│ │ │ │ +
98};
│ │ │ │ +
│ │ │ │ +
99
│ │ │ │ +
100} // namespace gtsam
│ │ │ │ +
Vector.h
typedef and functions to augment Eigen's VectorXd
│ │ │ │ +
FastVector.h
A thin wrapper around std::vector that uses a custom allocator.
│ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
│ │ │ │ -
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
│ │ │ │ -
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ -
gtsam::KarcherMeanFactor
The KarcherMeanFactor creates a constraint on all SO(n) variables with given keys that the Karcher me...
Definition KarcherMeanFactor.h:45
│ │ │ │ -
gtsam::KarcherMeanFactor::error
double error(const Values &c) const override
Calculate the error of the factor: always zero.
Definition KarcherMeanFactor.h:69
│ │ │ │ -
gtsam::KarcherMeanFactor::dim
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition KarcherMeanFactor.h:72
│ │ │ │ -
gtsam::KarcherMeanFactor::linearize
boost::shared_ptr< GaussianFactor > linearize(const Values &c) const override
linearize to a GaussianFactor
Definition KarcherMeanFactor.h:75
│ │ │ │ -
gtsam::KarcherMeanFactor::~KarcherMeanFactor
~KarcherMeanFactor() override
Destructor.
Definition KarcherMeanFactor.h:66
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ +
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ +
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │ +
gtsam::DotWriter
DotWriter is a helper class for writing graphviz .dot files.
Definition DotWriter.h:35
│ │ │ │ +
gtsam::DotWriter::figureWidthInches
double figureWidthInches
The figure width on paper in inches.
Definition DotWriter.h:36
│ │ │ │ +
gtsam::DotWriter::plotFactorPoints
bool plotFactorPoints
Plots each factor as a dot between the variables.
Definition DotWriter.h:38
│ │ │ │ +
gtsam::DotWriter::connectKeysToFactor
bool connectKeysToFactor
Draw a line from each key within a factor to the dot of the factor.
Definition DotWriter.h:39
│ │ │ │ +
gtsam::DotWriter::variablePositions
std::map< Key, Vector2 > variablePositions
Variable positions can be optionally specified and will be included in the dot file with a "!...
Definition DotWriter.h:47
│ │ │ │ +
gtsam::DotWriter::binaryEdges
bool binaryEdges
just use non-dotted edges for binary factors
Definition DotWriter.h:41
│ │ │ │ +
gtsam::DotWriter::positionHints
std::map< char, double > positionHints
The position hints allow one to use symbol character and index to specify position.
Definition DotWriter.h:54
│ │ │ │ +
gtsam::DotWriter::boxes
std::set< Key > boxes
A set of keys that will be displayed as a box.
Definition DotWriter.h:57
│ │ │ │ +
gtsam::DotWriter::factorPositions
std::map< size_t, Vector2 > factorPositions
Factor positions can be optionally specified and will be included in the dot file with a "!...
Definition DotWriter.h:63
│ │ │ │ +
gtsam::DotWriter::figureHeightInches
double figureHeightInches
The figure height on paper in inches.
Definition DotWriter.h:37
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,111 +1,143 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -KarcherMeanFactor.h │ │ │ │ │ +DotWriter.h │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ -3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +3 * GTSAM Copyright 2010-2021, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -12/* │ │ │ │ │ -13 * @file KarcherMeanFactor.h │ │ │ │ │ -14 * @author Frank Dellaert │ │ │ │ │ -15 * @date March 2019 │ │ │ │ │ -16 */ │ │ │ │ │ -17 │ │ │ │ │ -18#pragma once │ │ │ │ │ -19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_b_a_s_e_/_M_a_t_r_i_x_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ -22 │ │ │ │ │ -23#include │ │ │ │ │ -24#include │ │ │ │ │ -25 │ │ │ │ │ -26namespace _g_t_s_a_m { │ │ │ │ │ -32template │ │ │ │ │ -33T FindKarcherMean(const std::vector> │ │ │ │ │ -&rotations); │ │ │ │ │ -34 │ │ │ │ │ -35template T FindKarcherMean(std::initializer_list &&rotations); │ │ │ │ │ -36 │ │ │ │ │ -_4_5template class _K_a_r_c_h_e_r_M_e_a_n_F_a_c_t_o_r : public _N_o_n_l_i_n_e_a_r_F_a_c_t_o_r { │ │ │ │ │ -46 // Compile time dimension: can be -1 │ │ │ │ │ -47 enum { D = _t_r_a_i_t_s_<_T_>_:_:_d_i_m_e_n_s_i_o_n }; │ │ │ │ │ +19#pragma once │ │ │ │ │ +20 │ │ │ │ │ +21#include <_g_t_s_a_m_/_b_a_s_e_/_F_a_s_t_V_e_c_t_o_r_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_V_e_c_t_o_r_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ +24 │ │ │ │ │ +25#include │ │ │ │ │ +26#include │ │ │ │ │ +27#include │ │ │ │ │ +28 │ │ │ │ │ +29namespace _g_t_s_a_m { │ │ │ │ │ +30 │ │ │ │ │ +_3_5struct GTSAM_EXPORT _D_o_t_W_r_i_t_e_r { │ │ │ │ │ +_3_6 double _f_i_g_u_r_e_W_i_d_t_h_I_n_c_h_e_s; │ │ │ │ │ +_3_7 double _f_i_g_u_r_e_H_e_i_g_h_t_I_n_c_h_e_s; │ │ │ │ │ +_3_8 bool _p_l_o_t_F_a_c_t_o_r_P_o_i_n_t_s; │ │ │ │ │ +_3_9 bool _c_o_n_n_e_c_t_K_e_y_s_T_o_F_a_c_t_o_r; │ │ │ │ │ +_4_1 bool _b_i_n_a_r_y_E_d_g_e_s; │ │ │ │ │ +42 │ │ │ │ │ +_4_7 std::map _v_a_r_i_a_b_l_e_P_o_s_i_t_i_o_n_s; │ │ │ │ │ 48 │ │ │ │ │ -49 // Runtime dimension: always >=0 │ │ │ │ │ -50 size_t d_; │ │ │ │ │ -51 │ │ │ │ │ -53 boost::shared_ptr whitenedJacobian_; │ │ │ │ │ -54 │ │ │ │ │ -55public: │ │ │ │ │ -61 template │ │ │ │ │ -62 _K_a_r_c_h_e_r_M_e_a_n_F_a_c_t_o_r(const CONTAINER &_k_e_y_s, int d = D, │ │ │ │ │ -63 boost::optional beta = boost::none); │ │ │ │ │ +_5_4 std::map _p_o_s_i_t_i_o_n_H_i_n_t_s; │ │ │ │ │ +55 │ │ │ │ │ +_5_7 std::set _b_o_x_e_s; │ │ │ │ │ +58 │ │ │ │ │ +_6_3 std::map _f_a_c_t_o_r_P_o_s_i_t_i_o_n_s; │ │ │ │ │ 64 │ │ │ │ │ -_6_6 _~_K_a_r_c_h_e_r_M_e_a_n_F_a_c_t_o_r() override {} │ │ │ │ │ -67 │ │ │ │ │ -_6_9 double _e_r_r_o_r(const _V_a_l_u_e_s &c) const override { return 0; } │ │ │ │ │ -70 │ │ │ │ │ -_7_2 size_t _d_i_m() const override { return d_; } │ │ │ │ │ -73 │ │ │ │ │ -_7_5 boost::shared_ptr _l_i_n_e_a_r_i_z_e(const _V_a_l_u_e_s &c) const override │ │ │ │ │ -{ │ │ │ │ │ -76 return whitenedJacobian_; │ │ │ │ │ -77 } │ │ │ │ │ -78}; │ │ │ │ │ -79// \KarcherMeanFactor │ │ │ │ │ -80} // namespace gtsam │ │ │ │ │ -_M_a_t_r_i_x_._h │ │ │ │ │ -typedef and functions to augment Eigen's MatrixXd │ │ │ │ │ -_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ +65 explicit _D_o_t_W_r_i_t_e_r(double figureWidthInches = 5, │ │ │ │ │ +66 double figureHeightInches = 5, │ │ │ │ │ +67 bool plotFactorPoints = true, │ │ │ │ │ +68 bool connectKeysToFactor = true, bool binaryEdges = false) │ │ │ │ │ +69 : figureWidthInches(figureWidthInches), │ │ │ │ │ +70 figureHeightInches(figureHeightInches), │ │ │ │ │ +71 plotFactorPoints(plotFactorPoints), │ │ │ │ │ +72 connectKeysToFactor(connectKeysToFactor), │ │ │ │ │ +73 binaryEdges(binaryEdges) {} │ │ │ │ │ +74 │ │ │ │ │ +76 void graphPreamble(std::ostream* os) const; │ │ │ │ │ +77 │ │ │ │ │ +79 void digraphPreamble(std::ostream* os) const; │ │ │ │ │ +80 │ │ │ │ │ +82 void drawVariable(_K_e_y key, const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter, │ │ │ │ │ +83 const boost::optional& position, │ │ │ │ │ +84 std::ostream* os) const; │ │ │ │ │ +85 │ │ │ │ │ +87 static void DrawFactor(size_t i, const boost::optional& position, │ │ │ │ │ +88 std::ostream* os); │ │ │ │ │ +89 │ │ │ │ │ +91 boost::optional variablePos(_K_e_y key) const; │ │ │ │ │ +92 │ │ │ │ │ +94 void processFactor(size_t i, const _K_e_y_V_e_c_t_o_r& keys, │ │ │ │ │ +95 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter, │ │ │ │ │ +96 const boost::optional& position, │ │ │ │ │ +97 std::ostream* os) const; │ │ │ │ │ +98}; │ │ │ │ │ +99 │ │ │ │ │ +100} // namespace gtsam │ │ │ │ │ +_V_e_c_t_o_r_._h │ │ │ │ │ +typedef and functions to augment Eigen's VectorXd │ │ │ │ │ +_F_a_s_t_V_e_c_t_o_r_._h │ │ │ │ │ +A thin wrapper around std::vector that uses a custom allocator. │ │ │ │ │ +_K_e_y_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s │ │ │ │ │ -const KeyVector & keys() const │ │ │ │ │ -Access the factor's involved variable keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:140 │ │ │ │ │ -_g_t_s_a_m_:_:_N_o_n_l_i_n_e_a_r_F_a_c_t_o_r │ │ │ │ │ -Nonlinear factor base class. │ │ │ │ │ -DDeeffiinniittiioonn NonlinearFactor.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ -A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ -DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ -_g_t_s_a_m_:_:_K_a_r_c_h_e_r_M_e_a_n_F_a_c_t_o_r │ │ │ │ │ -The KarcherMeanFactor creates a constraint on all SO(n) variables with given │ │ │ │ │ -keys that the Karcher me... │ │ │ │ │ -DDeeffiinniittiioonn KarcherMeanFactor.h:45 │ │ │ │ │ -_g_t_s_a_m_:_:_K_a_r_c_h_e_r_M_e_a_n_F_a_c_t_o_r_:_:_e_r_r_o_r │ │ │ │ │ -double error(const Values &c) const override │ │ │ │ │ -Calculate the error of the factor: always zero. │ │ │ │ │ -DDeeffiinniittiioonn KarcherMeanFactor.h:69 │ │ │ │ │ -_g_t_s_a_m_:_:_K_a_r_c_h_e_r_M_e_a_n_F_a_c_t_o_r_:_:_d_i_m │ │ │ │ │ -size_t dim() const override │ │ │ │ │ -get the dimension of the factor (number of rows on linearization) │ │ │ │ │ -DDeeffiinniittiioonn KarcherMeanFactor.h:72 │ │ │ │ │ -_g_t_s_a_m_:_:_K_a_r_c_h_e_r_M_e_a_n_F_a_c_t_o_r_:_:_l_i_n_e_a_r_i_z_e │ │ │ │ │ -boost::shared_ptr< GaussianFactor > linearize(const Values &c) const override │ │ │ │ │ -linearize to a GaussianFactor │ │ │ │ │ -DDeeffiinniittiioonn KarcherMeanFactor.h:75 │ │ │ │ │ -_g_t_s_a_m_:_:_K_a_r_c_h_e_r_M_e_a_n_F_a_c_t_o_r_:_:_~_K_a_r_c_h_e_r_M_e_a_n_F_a_c_t_o_r │ │ │ │ │ -~KarcherMeanFactor() override │ │ │ │ │ -Destructor. │ │ │ │ │ -DDeeffiinniittiioonn KarcherMeanFactor.h:66 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ +std::uint64_t Key │ │ │ │ │ +Integer nonlinear key type. │ │ │ │ │ +DDeeffiinniittiioonn types.h:100 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ +std::function< std::string(Key)> KeyFormatter │ │ │ │ │ +Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r │ │ │ │ │ +DotWriter is a helper class for writing graphviz .dot files. │ │ │ │ │ +DDeeffiinniittiioonn DotWriter.h:35 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_f_i_g_u_r_e_W_i_d_t_h_I_n_c_h_e_s │ │ │ │ │ +double figureWidthInches │ │ │ │ │ +The figure width on paper in inches. │ │ │ │ │ +DDeeffiinniittiioonn DotWriter.h:36 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_p_l_o_t_F_a_c_t_o_r_P_o_i_n_t_s │ │ │ │ │ +bool plotFactorPoints │ │ │ │ │ +Plots each factor as a dot between the variables. │ │ │ │ │ +DDeeffiinniittiioonn DotWriter.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_c_o_n_n_e_c_t_K_e_y_s_T_o_F_a_c_t_o_r │ │ │ │ │ +bool connectKeysToFactor │ │ │ │ │ +Draw a line from each key within a factor to the dot of the factor. │ │ │ │ │ +DDeeffiinniittiioonn DotWriter.h:39 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_v_a_r_i_a_b_l_e_P_o_s_i_t_i_o_n_s │ │ │ │ │ +std::map< Key, Vector2 > variablePositions │ │ │ │ │ +Variable positions can be optionally specified and will be included in the dot │ │ │ │ │ +file with a "!... │ │ │ │ │ +DDeeffiinniittiioonn DotWriter.h:47 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_b_i_n_a_r_y_E_d_g_e_s │ │ │ │ │ +bool binaryEdges │ │ │ │ │ +just use non-dotted edges for binary factors │ │ │ │ │ +DDeeffiinniittiioonn DotWriter.h:41 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_p_o_s_i_t_i_o_n_H_i_n_t_s │ │ │ │ │ +std::map< char, double > positionHints │ │ │ │ │ +The position hints allow one to use symbol character and index to specify │ │ │ │ │ +position. │ │ │ │ │ +DDeeffiinniittiioonn DotWriter.h:54 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_b_o_x_e_s │ │ │ │ │ +std::set< Key > boxes │ │ │ │ │ +A set of keys that will be displayed as a box. │ │ │ │ │ +DDeeffiinniittiioonn DotWriter.h:57 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_f_a_c_t_o_r_P_o_s_i_t_i_o_n_s │ │ │ │ │ +std::map< size_t, Vector2 > factorPositions │ │ │ │ │ +Factor positions can be optionally specified and will be included in the dot │ │ │ │ │ +file with a "!... │ │ │ │ │ +DDeeffiinniittiioonn DotWriter.h:63 │ │ │ │ │ +_g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r_:_:_f_i_g_u_r_e_H_e_i_g_h_t_I_n_c_h_e_s │ │ │ │ │ +double figureHeightInches │ │ │ │ │ +The figure height on paper in inches. │ │ │ │ │ +DDeeffiinniittiioonn DotWriter.h:37 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _s_l_a_m │ │ │ │ │ - * KKaarrcchheerrMMeeaannFFaaccttoorr..hh │ │ │ │ │ + * _i_n_f_e_r_e_n_c_e │ │ │ │ │ + * _D_o_t_W_r_i_t_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01250.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00728.html │ │ │ │┄ Files 81% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/SmartFactorParams.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/linear/ConjugateGradientSolver.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ Classes | │ │ │ │ Namespaces | │ │ │ │ -Enumerations
│ │ │ │ -
SmartFactorParams.h File Reference
│ │ │ │ +Functions
│ │ │ │ +
ConjugateGradientSolver.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Collect common parameters for SmartProjection and SmartStereoProjection factors. │ │ │ │ +

Implementation of Conjugate Gradient solver for a linear system. │ │ │ │ More...

│ │ │ │ │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ │ │ │ │

│ │ │ │ Classes

struct  gtsam::SmartProjectionParams
class  gtsam::ConjugateGradientParameters
 parameters for the conjugate gradient method More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ -Enumerations

enum  gtsam::LinearizationMode { HESSIAN │ │ │ │ -, IMPLICIT_SCHUR │ │ │ │ -, JACOBIAN_Q │ │ │ │ -, JACOBIAN_SVD │ │ │ │ - }
 SmartFactorParams: parameters and (linearization/degeneracy) modes for SmartProjection and SmartStereoProjection factors. More...
 
enum  gtsam::DegeneracyMode { IGNORE_DEGENERACY │ │ │ │ -, ZERO_ON_DEGENERACY │ │ │ │ -, HANDLE_INFINITY │ │ │ │ - }
 How to manage degeneracy.
 

│ │ │ │ +Functions

│ │ │ │ +template<class S , class V >
V gtsam::preconditionedConjugateGradient (const S &system, const V &initial, const ConjugateGradientParameters &parameters)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Collect common parameters for SmartProjection and SmartStereoProjection factors.

│ │ │ │ -
Author
Luca Carlone
│ │ │ │ +

Implementation of Conjugate Gradient solver for a linear system.

│ │ │ │ +
Author
Yong-Dian Jian
│ │ │ │
│ │ │ │ -Zsolt Kira
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ +Sungtae An
│ │ │ │ +
Date
Nov 6, 2014
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,38 +1,33 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _E_n_u_m_e_r_a_t_i_o_n_s │ │ │ │ │ -SmartFactorParams.h File Reference │ │ │ │ │ -Collect common parameters for SmartProjection and SmartStereoProjection │ │ │ │ │ -factors. _M_o_r_e_._._. │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +ConjugateGradientSolver.h File Reference │ │ │ │ │ +Implementation of Conjugate Gradient solver for a linear system. _M_o_r_e_._._. │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ CCllaasssseess │ │ │ │ │ -struct   _g_t_s_a_m_:_:_S_m_a_r_t_P_r_o_j_e_c_t_i_o_n_P_a_r_a_m_s │ │ │ │ │ +class   _g_t_s_a_m_:_:_C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s │ │ │ │ │ +  parameters for the conjugate gradient method _M_o_r_e_._._. │ │ │ │ │   │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ -EEnnuummeerraattiioonnss │ │ │ │ │ -enum   _g_t_s_a_m_:_:_L_i_n_e_a_r_i_z_a_t_i_o_n_M_o_d_e { HHEESSSSIIAANN , IIMMPPLLIICCIITT__SSCCHHUURR , JJAACCOOBBIIAANN__QQ , │ │ │ │ │ - JJAACCOOBBIIAANN__SSVVDD } │ │ │ │ │ -  SmartFactorParams: parameters and (linearization/degeneracy) modes for │ │ │ │ │ - SmartProjection and SmartStereoProjection factors. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ -enum   _g_t_s_a_m_:_:_D_e_g_e_n_e_r_a_c_y_M_o_d_e { IIGGNNOORREE__DDEEGGEENNEERRAACCYY , ZZEERROO__OONN__DDEEGGEENNEERRAACCYY , │ │ │ │ │ - HHAANNDDLLEE__IINNFFIINNIITTYY } │ │ │ │ │ -  How to manage degeneracy. │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +template │ │ │ │ │ +V  ggttssaamm::::pprreeccoonnddiittiioonneeddCCoonnjjuuggaatteeGGrraaddiieenntt (const S &system, const V &initial, │ │ │ │ │ + const _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_P_a_r_a_m_e_t_e_r_s ¶meters) │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Collect common parameters for SmartProjection and SmartStereoProjection │ │ │ │ │ -factors. │ │ │ │ │ +Implementation of Conjugate Gradient solver for a linear system. │ │ │ │ │ Author │ │ │ │ │ - Luca Carlone │ │ │ │ │ - Zsolt Kira │ │ │ │ │ - Frank Dellaert │ │ │ │ │ + Yong-Dian Jian │ │ │ │ │ + Sungtae An │ │ │ │ │ + Date │ │ │ │ │ + Nov 6, 2014 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _s_l_a_m │ │ │ │ │ - * _S_m_a_r_t_F_a_c_t_o_r_P_a_r_a_m_s_._h │ │ │ │ │ + * _l_i_n_e_a_r │ │ │ │ │ + * _C_o_n_j_u_g_a_t_e_G_r_a_d_i_e_n_t_S_o_l_v_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01256.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00950.html │ │ │ │┄ Files 94% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/AntiFactor.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/AttitudeFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
AntiFactor.h File Reference
│ │ │ │ +
AttitudeFactor.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Go to the source code of this file.

│ │ │ │ +

Implementation file for Attitude factor. │ │ │ │ +More...

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::AntiFactor
 A class for downdating an existing factor from a graph. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Stephen Williams
│ │ │ │ +

Implementation file for Attitude factor.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
Date
January 28, 2014
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,23 +1,22 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -AntiFactor.h File Reference │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_A_n_t_i_F_a_c_t_o_r │ │ │ │ │ -  A class for downdating an existing factor from a graph. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +AttitudeFactor.cpp File Reference │ │ │ │ │ +Implementation file for Attitude factor. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Implementation file for Attitude factor. │ │ │ │ │ Author │ │ │ │ │ - Stephen Williams │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + January 28, 2014 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _s_l_a_m │ │ │ │ │ - * _A_n_t_i_F_a_c_t_o_r_._h │ │ │ │ │ + * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ + * _A_t_t_i_t_u_d_e_F_a_c_t_o_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01316_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01028_source.html │ │ │ │┄ Files 73% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/JacobianFactorSVD.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/expressionTesting.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
JacobianFactorSVD.h
│ │ │ │ +
expressionTesting.h
│ │ │ │
│ │ │ │
│ │ │ │ -
1/*
│ │ │ │ -
2 * @file JacobianFactorSVD.h
│ │ │ │ -
3 * @date Oct 27, 2013
│ │ │ │ -
4 * @uthor Frank Dellaert
│ │ │ │ -
5 */
│ │ │ │ -
6
│ │ │ │ -
7#pragma once
│ │ │ │ -
8#include <gtsam/linear/RegularJacobianFactor.h>
│ │ │ │ -
9
│ │ │ │ -
10namespace gtsam {
│ │ │ │ -
28template<size_t D, size_t ZDim>
│ │ │ │ -
│ │ │ │ -
29class JacobianFactorSVD: public RegularJacobianFactor<D> {
│ │ │ │ -
30
│ │ │ │ -
31 typedef RegularJacobianFactor<D> Base;
│ │ │ │ -
32 typedef Eigen::Matrix<double, ZDim, D> MatrixZD; // e.g 2 x 6 with Z=Point2
│ │ │ │ -
33 typedef std::pair<Key, Matrix> KeyMatrix;
│ │ │ │ -
34
│ │ │ │ -
35public:
│ │ │ │ -
36
│ │ │ │ -
│ │ │ │ -
38 JacobianFactorSVD() {
│ │ │ │ -
39 }
│ │ │ │ -
│ │ │ │ -
40
│ │ │ │ -
│ │ │ │ -
42 JacobianFactorSVD(const KeyVector& keys,
│ │ │ │ -
43 const SharedDiagonal& model = SharedDiagonal())
│ │ │ │ -
44 : Base() {
│ │ │ │ -
45 Matrix zeroMatrix = Matrix::Zero(0, D);
│ │ │ │ -
46 Vector zeroVector = Vector::Zero(0);
│ │ │ │ -
47 std::vector<KeyMatrix> QF;
│ │ │ │ -
48 QF.reserve(keys.size());
│ │ │ │ -
49 for(const Key& key: keys)
│ │ │ │ -
50 QF.push_back(KeyMatrix(key, zeroMatrix));
│ │ │ │ -
51 JacobianFactor::fillTerms(QF, zeroVector, model);
│ │ │ │ -
52 }
│ │ │ │ -
│ │ │ │ -
53
│ │ │ │ -
│ │ │ │ -
64 JacobianFactorSVD(
│ │ │ │ -
65 const KeyVector& keys,
│ │ │ │ -
66 const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& Fblocks,
│ │ │ │ -
67 const Matrix& Enull, const Vector& b,
│ │ │ │ -
68 const SharedDiagonal& model = SharedDiagonal())
│ │ │ │ -
69 : Base() {
│ │ │ │ -
70 size_t numKeys = Enull.rows() / ZDim;
│ │ │ │ -
71 size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()?
│ │ │ │ -
72 // PLAIN nullptr SPACE TRICK
│ │ │ │ -
73 // Matrix Q = Enull * Enull.transpose();
│ │ │ │ -
74 // for(const KeyMatrixZD& it: Fblocks)
│ │ │ │ -
75 // QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second));
│ │ │ │ -
76 // JacobianFactor factor(QF, Q * b);
│ │ │ │ -
77 std::vector<KeyMatrix> QF;
│ │ │ │ -
78 QF.reserve(numKeys);
│ │ │ │ -
79 for (size_t k = 0; k < Fblocks.size(); ++k) {
│ │ │ │ -
80 Key key = keys[k];
│ │ │ │ -
81 QF.emplace_back(
│ │ │ │ -
82 key, (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k]);
│ │ │ │ -
83 }
│ │ │ │ -
84 JacobianFactor::fillTerms(QF, Enull.transpose() * b, model);
│ │ │ │ -
85 }
│ │ │ │ -
│ │ │ │ -
86};
│ │ │ │ -
│ │ │ │ -
87
│ │ │ │ -
88}
│ │ │ │ -
RegularJacobianFactor.h
JacobianFactor class with fixed sized blcoks.
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +
2
│ │ │ │ +
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │ +
4 * Atlanta, Georgia 30332-0415
│ │ │ │ +
5 * All Rights Reserved
│ │ │ │ +
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │ +
7
│ │ │ │ +
8 * See LICENSE for the license information
│ │ │ │ +
9
│ │ │ │ +
10 * -------------------------------------------------------------------------- */
│ │ │ │ +
11
│ │ │ │ +
20#pragma once
│ │ │ │ +
21
│ │ │ │ +
22#include <gtsam/nonlinear/ExpressionFactor.h>
│ │ │ │ +
23#include <gtsam/nonlinear/factorTesting.h>
│ │ │ │ +
24#include <gtsam/base/Testable.h>
│ │ │ │ +
25
│ │ │ │ +
26namespace gtsam {
│ │ │ │ +
27
│ │ │ │ +
28namespace internal {
│ │ │ │ +
29// CPPUnitLite-style test for linearization of an ExpressionFactor
│ │ │ │ +
30template<typename T>
│ │ │ │ +
31bool testExpressionJacobians(const std::string& name_,
│ │ │ │ +
32 const gtsam::Expression<T>& expression, const gtsam::Values& values,
│ │ │ │ +
33 double nd_step, double tolerance) {
│ │ │ │ +
34 // Create factor
│ │ │ │ +
35 size_t size = traits<T>::dimension;
│ │ │ │ +
36 ExpressionFactor<T> f(noiseModel::Unit::Create(size),
│ │ │ │ +
37 expression.value(values), expression);
│ │ │ │ +
38 return testFactorJacobians(name_, f, values, nd_step, tolerance);
│ │ │ │ +
39}
│ │ │ │ +
40} // namespace internal
│ │ │ │ +
41} // namespace gtsam
│ │ │ │ +
42
│ │ │ │ +
│ │ │ │ +
48#define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, numerical_derivative_step, tolerance) \
│ │ │ │ +
49 { EXPECT(gtsam::internal::testExpressionJacobians(name_, expression, values, numerical_derivative_step, tolerance)); }
│ │ │ │ +
│ │ │ │ +
Testable.h
Concept check for values that can be used in unit tests.
│ │ │ │ +
factorTesting.h
Evaluate derivatives of a nonlinear factor numerically.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
│ │ │ │ -
gtsam::JacobianFactor::fillTerms
void fillTerms(const TERMS &terms, const Vector &b, const SharedDiagonal &noiseModel)
Internal function to fill blocks and set dimensions.
Definition JacobianFactor-inl.h:60
│ │ │ │ -
gtsam::RegularJacobianFactor
JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator.
Definition RegularJacobianFactor.h:32
│ │ │ │ -
gtsam::JacobianFactorSVD
JacobianFactor for Schur complement that uses the "Nullspace Trick" by Mourikis et al.
Definition JacobianFactorSVD.h:29
│ │ │ │ -
gtsam::JacobianFactorSVD::JacobianFactorSVD
JacobianFactorSVD()
Default constructor.
Definition JacobianFactorSVD.h:38
│ │ │ │ -
gtsam::JacobianFactorSVD::JacobianFactorSVD
JacobianFactorSVD(const KeyVector &keys, const std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > &Fblocks, const Matrix &Enull, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Construct a new JacobianFactorSVD object, createing a reduced-rank Jacobian factor on the CameraSet.
Definition JacobianFactorSVD.h:64
│ │ │ │ -
gtsam::JacobianFactorSVD::JacobianFactorSVD
JacobianFactorSVD(const KeyVector &keys, const SharedDiagonal &model=SharedDiagonal())
Empty constructor with keys.
Definition JacobianFactorSVD.h:42
│ │ │ │ +
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Create a unit covariance noise model.
Definition NoiseModel.h:597
│ │ │ │ +
gtsam::Expression
Expression class that supports automatic differentiation.
Definition Expression.h:48
│ │ │ │ +
gtsam::Expression::value
T value(const Values &values, boost::optional< std::vector< Matrix > & > H=boost::none) const
Return value and optional derivatives, reverse AD version Notes: this is not terribly efficient,...
Definition Expression-inl.h:147
│ │ │ │ +
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,116 +1,74 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -JacobianFactorSVD.h │ │ │ │ │ -1/* │ │ │ │ │ -2 * @file JacobianFactorSVD.h │ │ │ │ │ -3 * @date Oct 27, 2013 │ │ │ │ │ -4 * @uthor Frank Dellaert │ │ │ │ │ -5 */ │ │ │ │ │ -6 │ │ │ │ │ -7#pragma once │ │ │ │ │ -8#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ +expressionTesting.h │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +1/* --------------------------------------------------------------------------- │ │ │ │ │ +- │ │ │ │ │ +2 │ │ │ │ │ +3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ +4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ +5 * All Rights Reserved │ │ │ │ │ +6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ +7 │ │ │ │ │ +8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ -10namespace _g_t_s_a_m { │ │ │ │ │ -28template │ │ │ │ │ -_2_9class _J_a_c_o_b_i_a_n_F_a_c_t_o_r_S_V_D: public _R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r { │ │ │ │ │ -30 │ │ │ │ │ -31 typedef _R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_<_D_> Base; │ │ │ │ │ -32 typedef Eigen::Matrix MatrixZD; // e.g 2 x 6 with Z=Point2 │ │ │ │ │ -33 typedef std::pair KeyMatrix; │ │ │ │ │ -34 │ │ │ │ │ -35public: │ │ │ │ │ -36 │ │ │ │ │ -_3_8 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_S_V_D() { │ │ │ │ │ -39 } │ │ │ │ │ -40 │ │ │ │ │ -_4_2 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_S_V_D(const _K_e_y_V_e_c_t_o_r& _k_e_y_s, │ │ │ │ │ -43 const SharedDiagonal& model = SharedDiagonal()) │ │ │ │ │ -44 : Base() { │ │ │ │ │ -45 Matrix zeroMatrix = Matrix::Zero(0, D); │ │ │ │ │ -46 Vector zeroVector = Vector::Zero(0); │ │ │ │ │ -47 std::vector QF; │ │ │ │ │ -48 QF.reserve(_k_e_y_s.size()); │ │ │ │ │ -49 for(const _K_e_y& key: _k_e_y_s) │ │ │ │ │ -50 QF.push_back(KeyMatrix(key, zeroMatrix)); │ │ │ │ │ -51 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_f_i_l_l_T_e_r_m_s(QF, zeroVector, model); │ │ │ │ │ -52 } │ │ │ │ │ -53 │ │ │ │ │ -_6_4 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_S_V_D( │ │ │ │ │ -65 const _K_e_y_V_e_c_t_o_r& _k_e_y_s, │ │ │ │ │ -66 const std::vector >& Fblocks, │ │ │ │ │ -67 const Matrix& Enull, const Vector& b, │ │ │ │ │ -68 const SharedDiagonal& model = SharedDiagonal()) │ │ │ │ │ -69 : Base() { │ │ │ │ │ -70 size_t numKeys = Enull.rows() / ZDim; │ │ │ │ │ -71 size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()? │ │ │ │ │ -72 // PLAIN nullptr SPACE TRICK │ │ │ │ │ -73 // Matrix Q = Enull * Enull.transpose(); │ │ │ │ │ -74 // for(const KeyMatrixZD& it: Fblocks) │ │ │ │ │ -75 // QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * │ │ │ │ │ -it.second)); │ │ │ │ │ -76 // JacobianFactor factor(QF, Q * b); │ │ │ │ │ -77 std::vector QF; │ │ │ │ │ -78 QF.reserve(numKeys); │ │ │ │ │ -79 for (size_t k = 0; k < Fblocks.size(); ++k) { │ │ │ │ │ -80 _K_e_y key = _k_e_y_s[k]; │ │ │ │ │ -81 QF.emplace_back( │ │ │ │ │ -82 key, (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k]); │ │ │ │ │ -83 } │ │ │ │ │ -84 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_f_i_l_l_T_e_r_m_s(QF, Enull.transpose() * b, model); │ │ │ │ │ -85 } │ │ │ │ │ -86}; │ │ │ │ │ -87 │ │ │ │ │ -88} │ │ │ │ │ -_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ -JacobianFactor class with fixed sized blcoks. │ │ │ │ │ +10 * ------------------------------------------------------------------------- │ │ │ │ │ +- */ │ │ │ │ │ +11 │ │ │ │ │ +20#pragma once │ │ │ │ │ +21 │ │ │ │ │ +22#include │ │ │ │ │ +23#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_f_a_c_t_o_r_T_e_s_t_i_n_g_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_b_a_s_e_/_T_e_s_t_a_b_l_e_._h> │ │ │ │ │ +25 │ │ │ │ │ +26namespace _g_t_s_a_m { │ │ │ │ │ +27 │ │ │ │ │ +28namespace internal { │ │ │ │ │ +29// CPPUnitLite-style test for linearization of an ExpressionFactor │ │ │ │ │ +30template │ │ │ │ │ +31bool testExpressionJacobians(const std::string& name_, │ │ │ │ │ +32 const _g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_<_T_>& expression, const _g_t_s_a_m_:_:_V_a_l_u_e_s& values, │ │ │ │ │ +33 double nd_step, double tolerance) { │ │ │ │ │ +34 // Create factor │ │ │ │ │ +35 size_t size = traits::dimension; │ │ │ │ │ +36 ExpressionFactor f(_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_C_r_e_a_t_e(size), │ │ │ │ │ +37 expression._v_a_l_u_e(values), expression); │ │ │ │ │ +38 return testFactorJacobians(name_, f, values, nd_step, tolerance); │ │ │ │ │ +39} │ │ │ │ │ +40} // namespace internal │ │ │ │ │ +41} // namespace gtsam │ │ │ │ │ +42 │ │ │ │ │ +_4_8#define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, │ │ │ │ │ +numerical_derivative_step, tolerance) \ │ │ │ │ │ +49 { EXPECT(gtsam::internal::testExpressionJacobians(name_, expression, values, │ │ │ │ │ +numerical_derivative_step, tolerance)); } │ │ │ │ │ +_T_e_s_t_a_b_l_e_._h │ │ │ │ │ +Concept check for values that can be used in unit tests. │ │ │ │ │ +_f_a_c_t_o_r_T_e_s_t_i_n_g_._h │ │ │ │ │ +Evaluate derivatives of a nonlinear factor numerically. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s │ │ │ │ │ -const KeyVector & keys() const │ │ │ │ │ -Access the factor's involved variable keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:140 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_f_i_l_l_T_e_r_m_s │ │ │ │ │ -void fillTerms(const TERMS &terms, const Vector &b, const SharedDiagonal │ │ │ │ │ -&noiseModel) │ │ │ │ │ -Internal function to fill blocks and set dimensions. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor-inl.h:60 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ -JacobianFactor with constant sized blocks Provides raw memory access versions │ │ │ │ │ -of linear operator. │ │ │ │ │ -DDeeffiinniittiioonn RegularJacobianFactor.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_S_V_D │ │ │ │ │ -JacobianFactor for Schur complement that uses the "Nullspace Trick" by Mourikis │ │ │ │ │ -et al. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactorSVD.h:29 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_S_V_D_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_S_V_D │ │ │ │ │ -JacobianFactorSVD() │ │ │ │ │ -Default constructor. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactorSVD.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_S_V_D_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_S_V_D │ │ │ │ │ -JacobianFactorSVD(const KeyVector &keys, const std::vector< MatrixZD, Eigen:: │ │ │ │ │ -aligned_allocator< MatrixZD > > &Fblocks, const Matrix &Enull, const Vector &b, │ │ │ │ │ -const SharedDiagonal &model=SharedDiagonal()) │ │ │ │ │ -Construct a new JacobianFactorSVD object, createing a reduced-rank Jacobian │ │ │ │ │ -factor on the CameraSet. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactorSVD.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_S_V_D_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_S_V_D │ │ │ │ │ -JacobianFactorSVD(const KeyVector &keys, const SharedDiagonal │ │ │ │ │ -&model=SharedDiagonal()) │ │ │ │ │ -Empty constructor with keys. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactorSVD.h:42 │ │ │ │ │ +_g_t_s_a_m_:_:_n_o_i_s_e_M_o_d_e_l_:_:_U_n_i_t_:_:_C_r_e_a_t_e │ │ │ │ │ +static shared_ptr Create(size_t dim) │ │ │ │ │ +Create a unit covariance noise model. │ │ │ │ │ +DDeeffiinniittiioonn NoiseModel.h:597 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n │ │ │ │ │ +Expression class that supports automatic differentiation. │ │ │ │ │ +DDeeffiinniittiioonn Expression.h:48 │ │ │ │ │ +_g_t_s_a_m_:_:_E_x_p_r_e_s_s_i_o_n_:_:_v_a_l_u_e │ │ │ │ │ +T value(const Values &values, boost::optional< std::vector< Matrix > & > │ │ │ │ │ +H=boost::none) const │ │ │ │ │ +Return value and optional derivatives, reverse AD version Notes: this is not │ │ │ │ │ +terribly efficient,... │ │ │ │ │ +DDeeffiinniittiioonn Expression-inl.h:147 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _s_l_a_m │ │ │ │ │ - * JJaaccoobbiiaannFFaaccttoorrSSVVDD..hh │ │ │ │ │ + * _n_o_n_l_i_n_e_a_r │ │ │ │ │ + * _e_x_p_r_e_s_s_i_o_n_T_e_s_t_i_n_g_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01325.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00902.html │ │ │ │┄ Files 94% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/PoseRotationPrior.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/navigation/GPSFactor.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ Namespaces
│ │ │ │ -
PoseRotationPrior.h File Reference
│ │ │ │ +
GPSFactor.cpp File Reference
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ -

Implements a prior on the rotation component of a pose. │ │ │ │ +

Implementation file for GPS factor. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::PoseRotationPrior< POSE >
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -

Implements a prior on the rotation component of a pose.

│ │ │ │ -
Date
Jun 14, 2012
│ │ │ │ -
Author
Alex Cunningham
│ │ │ │ +

Implementation file for GPS factor.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
Date
January 28, 2014
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,26 +1,22 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -PoseRotationPrior.h File Reference │ │ │ │ │ -Implements a prior on the rotation component of a pose. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r_<_ _P_O_S_E_ _> │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +GPSFactor.cpp File Reference │ │ │ │ │ +Implementation file for GPS factor. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Implements a prior on the rotation component of a pose. │ │ │ │ │ - Date │ │ │ │ │ - Jun 14, 2012 │ │ │ │ │ +Implementation file for GPS factor. │ │ │ │ │ Author │ │ │ │ │ - Alex Cunningham │ │ │ │ │ + Frank Dellaert │ │ │ │ │ + Date │ │ │ │ │ + January 28, 2014 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _s_l_a_m │ │ │ │ │ - * _P_o_s_e_R_o_t_a_t_i_o_n_P_r_i_o_r_._h │ │ │ │ │ + * _n_a_v_i_g_a_t_i_o_n │ │ │ │ │ + * _G_P_S_F_a_c_t_o_r_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01331.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00617.html │ │ │ │┄ Files 84% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/InitializePose3.cpp File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/DotWriter.h File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ +Classes | │ │ │ │ Namespaces
│ │ │ │ -
InitializePose3.cpp File Reference
│ │ │ │ +
DotWriter.h File Reference
│ │ │ │
│ │ │ │
│ │ │ │ + │ │ │ │ +

Graphviz formatter. │ │ │ │ +More...

│ │ │ │ + │ │ │ │ +

Go to the source code of this file.

│ │ │ │ │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │ +

│ │ │ │ +Classes

struct  gtsam::DotWriter
 DotWriter is a helper class for writing graphviz .dot files. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │

Detailed Description

│ │ │ │ -
Author
Luca Carlone
│ │ │ │ -
│ │ │ │ -Frank Dellaert
│ │ │ │ -
Date
August, 2014
│ │ │ │ +

Graphviz formatter.

│ │ │ │ +
Author
Frank Dellaert
│ │ │ │ +
Date
December, 2021
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,21 +1,27 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -InitializePose3.cpp File Reference │ │ │ │ │ +_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ +DotWriter.h File Reference │ │ │ │ │ +Graphviz formatter. _M_o_r_e_._._. │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ +CCllaasssseess │ │ │ │ │ +struct   _g_t_s_a_m_:_:_D_o_t_W_r_i_t_e_r │ │ │ │ │ +  _D_o_t_W_r_i_t_e_r is a helper class for writing graphviz .dot files. _M_o_r_e_._._. │ │ │ │ │ +  │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ +Graphviz formatter. │ │ │ │ │ Author │ │ │ │ │ - Luca Carlone │ │ │ │ │ Frank Dellaert │ │ │ │ │ Date │ │ │ │ │ - August, 2014 │ │ │ │ │ + December, 2021 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _s_l_a_m │ │ │ │ │ - * _I_n_i_t_i_a_l_i_z_e_P_o_s_e_3_._c_p_p │ │ │ │ │ + * _i_n_f_e_r_e_n_c_e │ │ │ │ │ + * _D_o_t_W_r_i_t_e_r_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01334.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a01097.html │ │ │ │┄ Files 88% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/PoseTranslationPrior.h File Reference │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/nonlinear/ISAM2-impl.cpp File Reference │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -67,15 +67,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ -Classes | │ │ │ │ -Namespaces
│ │ │ │ -
PoseTranslationPrior.h File Reference
│ │ │ │ +Namespaces | │ │ │ │ +Functions
│ │ │ │ +
ISAM2-impl.cpp File Reference
│ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │ -

Implements a prior on the translation component of a pose. │ │ │ │ +

Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization. │ │ │ │ More...

│ │ │ │ - │ │ │ │ -

Go to the source code of this file.

│ │ │ │ │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ - │ │ │ │ -

│ │ │ │ -Classes

class  gtsam::PoseTranslationPrior< POSE >
 A prior on the translation part of a pose. More...
 
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ +

│ │ │ │ Namespaces

namespace  gtsam
 Global functions in a separate testing namespace.
 
│ │ │ │ + │ │ │ │ + │ │ │ │ + │ │ │ │

│ │ │ │ +Functions

│ │ │ │ +void gtsam::internal::updateRgProd (const ISAM2::sharedClique &clique, const KeySet &replacedKeys, const VectorValues &grad, VectorValues *RgProd, size_t *varsUpdated)
 
│ │ │ │

Detailed Description

│ │ │ │ -

Implements a prior on the translation component of a pose.

│ │ │ │ -
Date
Jun 14, 2012
│ │ │ │ -
Author
Alex Cunningham
│ │ │ │ +

Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.

│ │ │ │ +
Author
Michael Kaess
│ │ │ │ +
│ │ │ │ +Richard Roberts
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,27 +1,28 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -_C_l_a_s_s_e_s | _N_a_m_e_s_p_a_c_e_s │ │ │ │ │ -PoseTranslationPrior.h File Reference │ │ │ │ │ -Implements a prior on the translation component of a pose. _M_o_r_e_._._. │ │ │ │ │ -_G_o_ _t_o_ _t_h_e_ _s_o_u_r_c_e_ _c_o_d_e_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ -CCllaasssseess │ │ │ │ │ -class   _g_t_s_a_m_:_:_P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r_<_ _P_O_S_E_ _> │ │ │ │ │ -  A prior on the translation part of a pose. _M_o_r_e_._._. │ │ │ │ │ -  │ │ │ │ │ +_N_a_m_e_s_p_a_c_e_s | _F_u_n_c_t_i_o_n_s │ │ │ │ │ +ISAM2-impl.cpp File Reference │ │ │ │ │ +Incremental update functionality (ISAM2) for BayesTree, with fluid │ │ │ │ │ +relinearization. _M_o_r_e_._._. │ │ │ │ │ NNaammeessppaacceess │ │ │ │ │ namespace   _g_t_s_a_m │ │ │ │ │   Global functions in a separate testing namespace. │ │ │ │ │   │ │ │ │ │ +FFuunnccttiioonnss │ │ │ │ │ +void  ggttssaamm::::iinntteerrnnaall::::uuppddaatteeRRggPPrroodd (const _I_S_A_M_2_:_:_s_h_a_r_e_d_C_l_i_q_u_e &clique, const │ │ │ │ │ + _K_e_y_S_e_t &replacedKeys, const _V_e_c_t_o_r_V_a_l_u_e_s &grad, _V_e_c_t_o_r_V_a_l_u_e_s *RgProd, │ │ │ │ │ + size_t *varsUpdated) │ │ │ │ │ +  │ │ │ │ │ ********** DDeettaaiilleedd DDeessccrriippttiioonn ********** │ │ │ │ │ -Implements a prior on the translation component of a pose. │ │ │ │ │ - Date │ │ │ │ │ - Jun 14, 2012 │ │ │ │ │ +Incremental update functionality (ISAM2) for BayesTree, with fluid │ │ │ │ │ +relinearization. │ │ │ │ │ Author │ │ │ │ │ - Alex Cunningham │ │ │ │ │ + Michael Kaess │ │ │ │ │ + Richard Roberts │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _s_l_a_m │ │ │ │ │ - * _P_o_s_e_T_r_a_n_s_l_a_t_i_o_n_P_r_i_o_r_._h │ │ │ │ │ + * _n_o_n_l_i_n_e_a_r │ │ │ │ │ + * _I_S_A_M_2_-_i_m_p_l_._c_p_p │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01340_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00584_source.html │ │ │ │┄ Files 73% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/slam/JacobianFactorQ.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/inference/MetisIndex.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
JacobianFactorQ.h
│ │ │ │ +
MetisIndex.h
│ │ │ │
│ │ │ │
│ │ │ │ -
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │ -
9
│ │ │ │ -
10 * -------------------------------------------------------------------------- */
│ │ │ │ -
11
│ │ │ │ -
12/*
│ │ │ │ -
13 * @file JacobianFactorQ.h
│ │ │ │ -
14 * @date Oct 27, 2013
│ │ │ │ -
15 * @uthor Frank Dellaert
│ │ │ │ -
16 */
│ │ │ │ -
17
│ │ │ │ -
18#pragma once
│ │ │ │ +
9 * -------------------------------------------------------------------------- */
│ │ │ │ +
10
│ │ │ │ +
17#pragma once
│ │ │ │ +
18
│ │ │ │
19
│ │ │ │ -
20#include <gtsam/linear/RegularJacobianFactor.h>
│ │ │ │ -
21
│ │ │ │ -
22namespace gtsam {
│ │ │ │ -
26template<size_t D, size_t ZDim>
│ │ │ │ -
│ │ │ │ -
27class JacobianFactorQ: public RegularJacobianFactor<D> {
│ │ │ │ -
28
│ │ │ │ -
29 typedef RegularJacobianFactor<D> Base;
│ │ │ │ -
30 typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
│ │ │ │ -
31 typedef std::pair<Key, Matrix> KeyMatrix;
│ │ │ │ -
32
│ │ │ │ -
33public:
│ │ │ │ +
20#include <gtsam/inference/Key.h>
│ │ │ │ +
21#include <gtsam/inference/FactorGraph.h>
│ │ │ │ +
22#include <gtsam/base/types.h>
│ │ │ │ +
23#include <gtsam/base/timing.h>
│ │ │ │ +
24
│ │ │ │ +
25// Boost bimap generates many ugly warnings in CLANG
│ │ │ │ +
26#ifdef __clang__
│ │ │ │ +
27# pragma clang diagnostic push
│ │ │ │ +
28# pragma clang diagnostic ignored "-Wredeclared-class-member"
│ │ │ │ +
29#endif
│ │ │ │ +
30#include <boost/bimap.hpp>
│ │ │ │ +
31#ifdef __clang__
│ │ │ │ +
32# pragma clang diagnostic pop
│ │ │ │ +
33#endif
│ │ │ │
34
│ │ │ │ -
│ │ │ │ -
36 JacobianFactorQ() {
│ │ │ │ -
37 }
│ │ │ │ -
│ │ │ │ -
38
│ │ │ │ -
│ │ │ │ -
40 JacobianFactorQ(const KeyVector& keys, //
│ │ │ │ -
41 const SharedDiagonal& model = SharedDiagonal()) :
│ │ │ │ -
42 Base() {
│ │ │ │ -
43 Matrix zeroMatrix = Matrix::Zero(0, D);
│ │ │ │ -
44 Vector zeroVector = Vector::Zero(0);
│ │ │ │ -
45 std::vector<KeyMatrix> QF;
│ │ │ │ -
46 QF.reserve(keys.size());
│ │ │ │ -
47 for(const Key& key: keys)
│ │ │ │ -
48 QF.push_back(KeyMatrix(key, zeroMatrix));
│ │ │ │ -
49 JacobianFactor::fillTerms(QF, zeroVector, model);
│ │ │ │ -
50 }
│ │ │ │ -
│ │ │ │ -
51
│ │ │ │ -
│ │ │ │ -
53 JacobianFactorQ(const KeyVector& keys,
│ │ │ │ -
54 const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& FBlocks, const Matrix& E, const Matrix3& P,
│ │ │ │ -
55 const Vector& b, const SharedDiagonal& model = SharedDiagonal()) :
│ │ │ │ -
56 Base() {
│ │ │ │ -
57 size_t j = 0, m2 = E.rows(), m = m2 / ZDim;
│ │ │ │ -
58 // Calculate projector Q
│ │ │ │ -
59 Matrix Q = Matrix::Identity(m2,m2) - E * P * E.transpose();
│ │ │ │ -
60 // Calculate pre-computed Jacobian matrices
│ │ │ │ -
61 // TODO: can we do better ?
│ │ │ │ -
62 std::vector<KeyMatrix> QF;
│ │ │ │ -
63 QF.reserve(m);
│ │ │ │ -
64 // Below, we compute each mZDim*D block A_j = Q_j * F_j = (mZDim*ZDim) * (Zdim*D)
│ │ │ │ -
65 for (size_t k = 0; k < FBlocks.size(); ++k) {
│ │ │ │ -
66 Key key = keys[k];
│ │ │ │ -
67 QF.push_back(
│ │ │ │ -
68 KeyMatrix(key, - Q.block(0, ZDim * j++, m2, ZDim) * FBlocks[k]));
│ │ │ │ -
69 }
│ │ │ │ -
70 // Which is then passed to the normal JacobianFactor constructor
│ │ │ │ -
71 JacobianFactor::fillTerms(QF, - Q * b, model);
│ │ │ │ +
35#include <vector>
│ │ │ │ +
36
│ │ │ │ +
37namespace gtsam {
│ │ │ │ +
│ │ │ │ +
45class GTSAM_EXPORT MetisIndex {
│ │ │ │ +
46public:
│ │ │ │ +
47 typedef boost::shared_ptr<MetisIndex> shared_ptr;
│ │ │ │ +
48 typedef boost::bimap<Key, int32_t> bm_type;
│ │ │ │ +
49
│ │ │ │ +
50private:
│ │ │ │ +
51 std::vector<int32_t> xadj_; // Index of node's adjacency list in adj
│ │ │ │ +
52 std::vector<int32_t> adj_; // Stores ajacency lists of all nodes, appended into a single vector
│ │ │ │ +
53 boost::bimap<Key, int32_t> intKeyBMap_; // Stores Key <-> integer value relationship
│ │ │ │ +
54 size_t nKeys_;
│ │ │ │ +
55
│ │ │ │ +
56public:
│ │ │ │ +
59
│ │ │ │ +
│ │ │ │ +
61 MetisIndex() :
│ │ │ │ +
62 nKeys_(0) {
│ │ │ │ +
63 }
│ │ │ │ +
│ │ │ │ +
64
│ │ │ │ +
65 template<class FACTORGRAPH>
│ │ │ │ +
66 MetisIndex(const FACTORGRAPH& factorGraph) :
│ │ │ │ +
67 nKeys_(0) {
│ │ │ │ +
68 augment(factorGraph);
│ │ │ │ +
69 }
│ │ │ │ +
70
│ │ │ │ +
71 ~MetisIndex() {
│ │ │ │
72 }
│ │ │ │ -
│ │ │ │ -
73};
│ │ │ │ -
│ │ │ │ -
74// end class JacobianFactorQ
│ │ │ │ -
75
│ │ │ │ -
76// traits
│ │ │ │ -
│ │ │ │ -
77template<size_t D, size_t ZDim> struct traits<JacobianFactorQ<D, ZDim> > : public Testable<
│ │ │ │ -
78 JacobianFactorQ<D, ZDim> > {
│ │ │ │ -
79};
│ │ │ │ -
│ │ │ │ -
80
│ │ │ │ -
81}
│ │ │ │ -
RegularJacobianFactor.h
JacobianFactor class with fixed sized blcoks.
│ │ │ │ +
76
│ │ │ │ +
81 template<class FACTORGRAPH>
│ │ │ │ +
82 void augment(const FACTORGRAPH& factors);
│ │ │ │ +
83
│ │ │ │ +
84 const std::vector<int32_t>& xadj() const {
│ │ │ │ +
85 return xadj_;
│ │ │ │ +
86 }
│ │ │ │ +
87 const std::vector<int32_t>& adj() const {
│ │ │ │ +
88 return adj_;
│ │ │ │ +
89 }
│ │ │ │ +
90 size_t nValues() const {
│ │ │ │ +
91 return nKeys_;
│ │ │ │ +
92 }
│ │ │ │ +
93 Key intToKey(int32_t value) const {
│ │ │ │ +
94 assert(value >= 0);
│ │ │ │ +
95 return intKeyBMap_.right.find(value)->second;
│ │ │ │ +
96 }
│ │ │ │ +
97
│ │ │ │ +
99};
│ │ │ │ +
│ │ │ │ +
100
│ │ │ │ +
101} // \ namesace gtsam
│ │ │ │ +
102
│ │ │ │ +
103#include <gtsam/inference/MetisIndex-inl.h>
│ │ │ │ +
timing.h
Timing utilities.
│ │ │ │ +
types.h
Typedefs for easier changing of types.
│ │ │ │ + │ │ │ │ +
FactorGraph.h
Factor Graph Base Class.
│ │ │ │ + │ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ -
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
│ │ │ │ -
gtsam::JacobianFactor::fillTerms
void fillTerms(const TERMS &terms, const Vector &b, const SharedDiagonal &noiseModel)
Internal function to fill blocks and set dimensions.
Definition JacobianFactor-inl.h:60
│ │ │ │ -
gtsam::RegularJacobianFactor
JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator.
Definition RegularJacobianFactor.h:32
│ │ │ │ -
gtsam::JacobianFactorQ
JacobianFactor for Schur complement that uses Q noise model.
Definition JacobianFactorQ.h:27
│ │ │ │ -
gtsam::JacobianFactorQ::JacobianFactorQ
JacobianFactorQ()
Default constructor.
Definition JacobianFactorQ.h:36
│ │ │ │ -
gtsam::JacobianFactorQ::JacobianFactorQ
JacobianFactorQ(const KeyVector &keys, const std::vector< MatrixZD, Eigen::aligned_allocator< MatrixZD > > &FBlocks, const Matrix &E, const Matrix3 &P, const Vector &b, const SharedDiagonal &model=SharedDiagonal())
Constructor.
Definition JacobianFactorQ.h:53
│ │ │ │ -
gtsam::JacobianFactorQ::JacobianFactorQ
JacobianFactorQ(const KeyVector &keys, const SharedDiagonal &model=SharedDiagonal())
Empty constructor with keys.
Definition JacobianFactorQ.h:40
│ │ │ │ +
gtsam::MetisIndex
The MetisIndex class converts a factor graph into the Compressed Sparse Row format for use in METIS a...
Definition MetisIndex.h:45
│ │ │ │ +
gtsam::MetisIndex::MetisIndex
MetisIndex()
Default constructor, creates empty MetisIndex.
Definition MetisIndex.h:61
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,141 +1,117 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -JacobianFactorQ.h │ │ │ │ │ +MetisIndex.h │ │ │ │ │ +_G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ -9 │ │ │ │ │ -10 * ------------------------------------------------------------------------- │ │ │ │ │ +9 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ -11 │ │ │ │ │ -12/* │ │ │ │ │ -13 * @file JacobianFactorQ.h │ │ │ │ │ -14 * @date Oct 27, 2013 │ │ │ │ │ -15 * @uthor Frank Dellaert │ │ │ │ │ -16 */ │ │ │ │ │ -17 │ │ │ │ │ -18#pragma once │ │ │ │ │ +10 │ │ │ │ │ +17#pragma once │ │ │ │ │ +18 │ │ │ │ │ 19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h> │ │ │ │ │ -21 │ │ │ │ │ -22namespace _g_t_s_a_m { │ │ │ │ │ -26template │ │ │ │ │ -_2_7class _J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q: public _R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r { │ │ │ │ │ -28 │ │ │ │ │ -29 typedef _R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_<_D_> Base; │ │ │ │ │ -30 typedef Eigen::Matrix MatrixZD; │ │ │ │ │ -31 typedef std::pair KeyMatrix; │ │ │ │ │ -32 │ │ │ │ │ -33public: │ │ │ │ │ +20#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_b_a_s_e_/_t_i_m_i_n_g_._h> │ │ │ │ │ +24 │ │ │ │ │ +25// Boost bimap generates many ugly warnings in CLANG │ │ │ │ │ +26#ifdef __clang__ │ │ │ │ │ +27# pragma clang diagnostic push │ │ │ │ │ +28# pragma clang diagnostic ignored "-Wredeclared-class-member" │ │ │ │ │ +29#endif │ │ │ │ │ +30#include │ │ │ │ │ +31#ifdef __clang__ │ │ │ │ │ +32# pragma clang diagnostic pop │ │ │ │ │ +33#endif │ │ │ │ │ 34 │ │ │ │ │ -_3_6 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q() { │ │ │ │ │ -37 } │ │ │ │ │ -38 │ │ │ │ │ -_4_0 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q(const _K_e_y_V_e_c_t_o_r& _k_e_y_s, // │ │ │ │ │ -41 const SharedDiagonal& model = SharedDiagonal()) : │ │ │ │ │ -42 Base() { │ │ │ │ │ -43 Matrix zeroMatrix = Matrix::Zero(0, D); │ │ │ │ │ -44 Vector zeroVector = Vector::Zero(0); │ │ │ │ │ -45 std::vector QF; │ │ │ │ │ -46 QF.reserve(_k_e_y_s.size()); │ │ │ │ │ -47 for(const _K_e_y& key: _k_e_y_s) │ │ │ │ │ -48 QF.push_back(KeyMatrix(key, zeroMatrix)); │ │ │ │ │ -49 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_f_i_l_l_T_e_r_m_s(QF, zeroVector, model); │ │ │ │ │ -50 } │ │ │ │ │ -51 │ │ │ │ │ -_5_3 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q(const _K_e_y_V_e_c_t_o_r& _k_e_y_s, │ │ │ │ │ -54 const std::vector >& FBlocks, │ │ │ │ │ -const Matrix& E, const Matrix3& P, │ │ │ │ │ -55 const Vector& b, const SharedDiagonal& model = SharedDiagonal()) : │ │ │ │ │ -56 Base() { │ │ │ │ │ -57 size_t j = 0, m2 = E.rows(), m = m2 / ZDim; │ │ │ │ │ -58 // Calculate projector Q │ │ │ │ │ -59 Matrix Q = Matrix::Identity(m2,m2) - E * P * E.transpose(); │ │ │ │ │ -60 // Calculate pre-computed Jacobian matrices │ │ │ │ │ -61 // TODO: can we do better ? │ │ │ │ │ -62 std::vector QF; │ │ │ │ │ -63 QF.reserve(m); │ │ │ │ │ -64 // Below, we compute each mZDim*D block A_j = Q_j * F_j = (mZDim*ZDim) * │ │ │ │ │ -(Zdim*D) │ │ │ │ │ -65 for (size_t k = 0; k < FBlocks.size(); ++k) { │ │ │ │ │ -66 _K_e_y key = _k_e_y_s[k]; │ │ │ │ │ -67 QF.push_back( │ │ │ │ │ -68 KeyMatrix(key, - Q.block(0, ZDim * j++, m2, ZDim) * FBlocks[k])); │ │ │ │ │ +35#include │ │ │ │ │ +36 │ │ │ │ │ +37namespace _g_t_s_a_m { │ │ │ │ │ +_4_5class GTSAM_EXPORT _M_e_t_i_s_I_n_d_e_x { │ │ │ │ │ +46public: │ │ │ │ │ +47 typedef boost::shared_ptr shared_ptr; │ │ │ │ │ +48 typedef boost::bimap bm_type; │ │ │ │ │ +49 │ │ │ │ │ +50private: │ │ │ │ │ +51 std::vector xadj_; // Index of node's adjacency list in adj │ │ │ │ │ +52 std::vector adj_; // Stores ajacency lists of all nodes, appended │ │ │ │ │ +into a single vector │ │ │ │ │ +53 boost::bimap intKeyBMap_; // Stores Key <-> integer value │ │ │ │ │ +relationship │ │ │ │ │ +54 size_t nKeys_; │ │ │ │ │ +55 │ │ │ │ │ +56public: │ │ │ │ │ +59 │ │ │ │ │ +_6_1 _M_e_t_i_s_I_n_d_e_x() : │ │ │ │ │ +62 nKeys_(0) { │ │ │ │ │ +63 } │ │ │ │ │ +64 │ │ │ │ │ +65 template │ │ │ │ │ +66 _M_e_t_i_s_I_n_d_e_x(const FACTORGRAPH& factorGraph) : │ │ │ │ │ +67 nKeys_(0) { │ │ │ │ │ +68 augment(factorGraph); │ │ │ │ │ 69 } │ │ │ │ │ -70 // Which is then passed to the normal JacobianFactor constructor │ │ │ │ │ -71 _J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_f_i_l_l_T_e_r_m_s(QF, - Q * b, model); │ │ │ │ │ +70 │ │ │ │ │ +71 ~MetisIndex() { │ │ │ │ │ 72 } │ │ │ │ │ -73}; │ │ │ │ │ -74// end class JacobianFactorQ │ │ │ │ │ -75 │ │ │ │ │ -76// traits │ │ │ │ │ -_7_7template struct _t_r_a_i_t_s<_J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q > : │ │ │ │ │ -public _T_e_s_t_a_b_l_e< │ │ │ │ │ -78 JacobianFactorQ > { │ │ │ │ │ -79}; │ │ │ │ │ -80 │ │ │ │ │ -81} │ │ │ │ │ -_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r_._h │ │ │ │ │ -JacobianFactor class with fixed sized blcoks. │ │ │ │ │ +76 │ │ │ │ │ +81 template │ │ │ │ │ +82 void augment(const FACTORGRAPH& factors); │ │ │ │ │ +83 │ │ │ │ │ +84 const std::vector& xadj() const { │ │ │ │ │ +85 return xadj_; │ │ │ │ │ +86 } │ │ │ │ │ +87 const std::vector& adj() const { │ │ │ │ │ +88 return adj_; │ │ │ │ │ +89 } │ │ │ │ │ +90 size_t nValues() const { │ │ │ │ │ +91 return nKeys_; │ │ │ │ │ +92 } │ │ │ │ │ +93 _K_e_y intToKey(int32_t value) const { │ │ │ │ │ +94 assert(value >= 0); │ │ │ │ │ +95 return intKeyBMap_.right.find(value)->second; │ │ │ │ │ +96 } │ │ │ │ │ +97 │ │ │ │ │ +99}; │ │ │ │ │ +100 │ │ │ │ │ +101} // \ namesace gtsam │ │ │ │ │ +102 │ │ │ │ │ +103#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_M_e_t_i_s_I_n_d_e_x_-_i_n_l_._h> │ │ │ │ │ +_t_i_m_i_n_g_._h │ │ │ │ │ +Timing utilities. │ │ │ │ │ +_t_y_p_e_s_._h │ │ │ │ │ +Typedefs for easier changing of types. │ │ │ │ │ +_K_e_y_._h │ │ │ │ │ +_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Factor Graph Base Class. │ │ │ │ │ +_M_e_t_i_s_I_n_d_e_x_-_i_n_l_._h │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ -A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ -that can be centered ... │ │ │ │ │ -DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ -A helper that implements the traits interface for GTSAM types. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_:_:_k_e_y_s │ │ │ │ │ -const KeyVector & keys() const │ │ │ │ │ -Access the factor's involved variable keys. │ │ │ │ │ -DDeeffiinniittiioonn Factor.h:140 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_:_:_f_i_l_l_T_e_r_m_s │ │ │ │ │ -void fillTerms(const TERMS &terms, const Vector &b, const SharedDiagonal │ │ │ │ │ -&noiseModel) │ │ │ │ │ -Internal function to fill blocks and set dimensions. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactor-inl.h:60 │ │ │ │ │ -_g_t_s_a_m_:_:_R_e_g_u_l_a_r_J_a_c_o_b_i_a_n_F_a_c_t_o_r │ │ │ │ │ -JacobianFactor with constant sized blocks Provides raw memory access versions │ │ │ │ │ -of linear operator. │ │ │ │ │ -DDeeffiinniittiioonn RegularJacobianFactor.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q │ │ │ │ │ -JacobianFactor for Schur complement that uses Q noise model. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactorQ.h:27 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q │ │ │ │ │ -JacobianFactorQ() │ │ │ │ │ -Default constructor. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactorQ.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q │ │ │ │ │ -JacobianFactorQ(const KeyVector &keys, const std::vector< MatrixZD, Eigen:: │ │ │ │ │ -aligned_allocator< MatrixZD > > &FBlocks, const Matrix &E, const Matrix3 &P, │ │ │ │ │ -const Vector &b, const SharedDiagonal &model=SharedDiagonal()) │ │ │ │ │ -Constructor. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactorQ.h:53 │ │ │ │ │ -_g_t_s_a_m_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q_:_:_J_a_c_o_b_i_a_n_F_a_c_t_o_r_Q │ │ │ │ │ -JacobianFactorQ(const KeyVector &keys, const SharedDiagonal │ │ │ │ │ -&model=SharedDiagonal()) │ │ │ │ │ -Empty constructor with keys. │ │ │ │ │ -DDeeffiinniittiioonn JacobianFactorQ.h:40 │ │ │ │ │ +_g_t_s_a_m_:_:_M_e_t_i_s_I_n_d_e_x │ │ │ │ │ +The MetisIndex class converts a factor graph into the Compressed Sparse Row │ │ │ │ │ +format for use in METIS a... │ │ │ │ │ +DDeeffiinniittiioonn MetisIndex.h:45 │ │ │ │ │ +_g_t_s_a_m_:_:_M_e_t_i_s_I_n_d_e_x_:_:_M_e_t_i_s_I_n_d_e_x │ │ │ │ │ +MetisIndex() │ │ │ │ │ +Default constructor, creates empty MetisIndex. │ │ │ │ │ +DDeeffiinniittiioonn MetisIndex.h:61 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _s_l_a_m │ │ │ │ │ - * JJaaccoobbiiaannFFaaccttoorrQQ..hh │ │ │ │ │ + * _i_n_f_e_r_e_n_c_e │ │ │ │ │ + * _M_e_t_i_s_I_n_d_e_x_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01343_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00545_source.html │ │ │ │┄ Files 76% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicFactor-inst.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridJunctionTree.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
SymbolicFactor-inst.h
│ │ │ │ +
HybridJunctionTree.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │
18#pragma once
│ │ │ │
19
│ │ │ │ -
20#include <gtsam/symbolic/SymbolicFactor.h>
│ │ │ │ -
21#include <gtsam/symbolic/SymbolicConditional.h>
│ │ │ │ -
22#include <gtsam/inference/Factor.h>
│ │ │ │ -
23#include <gtsam/inference/Key.h>
│ │ │ │ -
24#include <gtsam/base/timing.h>
│ │ │ │ +
20#include <gtsam/hybrid/HybridBayesTree.h>
│ │ │ │ +
21#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
│ │ │ │ +
22#include <gtsam/inference/JunctionTree.h>
│ │ │ │ +
23
│ │ │ │ +
24namespace gtsam {
│ │ │ │
25
│ │ │ │ -
26#include <boost/shared_ptr.hpp>
│ │ │ │ -
27#include <boost/make_shared.hpp>
│ │ │ │ +
26// Forward declarations
│ │ │ │ +
27class HybridEliminationTree;
│ │ │ │
28
│ │ │ │ -
29#include <utility>
│ │ │ │ -
30
│ │ │ │ -
31namespace gtsam
│ │ │ │ -
32{
│ │ │ │ -
33 namespace internal
│ │ │ │ -
34 {
│ │ │ │ -
37 template<class FACTOR>
│ │ │ │ -
38 std::pair<boost::shared_ptr<SymbolicConditional>, boost::shared_ptr<SymbolicFactor> >
│ │ │ │ -
│ │ │ │ -
39 EliminateSymbolic(const FactorGraph<FACTOR>& factors, const Ordering& keys)
│ │ │ │ -
40 {
│ │ │ │ -
41 gttic(EliminateSymbolic);
│ │ │ │ -
42
│ │ │ │ -
43 // Gather all keys
│ │ │ │ -
44 KeySet allKeys;
│ │ │ │ -
45 for(const boost::shared_ptr<FACTOR>& factor: factors) {
│ │ │ │ -
46 allKeys.insert(factor->begin(), factor->end());
│ │ │ │ -
47 }
│ │ │ │ -
48
│ │ │ │ -
49 // Check keys
│ │ │ │ -
50 for(Key key: keys) {
│ │ │ │ -
51 if(allKeys.find(key) == allKeys.end())
│ │ │ │ -
52 throw std::runtime_error("Requested to eliminate a key that is not in the factors");
│ │ │ │ -
53 }
│ │ │ │ +
│ │ │ │ +
52class GTSAM_EXPORT HybridJunctionTree
│ │ │ │ +
53 : public JunctionTree<HybridBayesTree, HybridGaussianFactorGraph> {
│ │ │ │
54
│ │ │ │ -
55 // Sort frontal keys
│ │ │ │ -
56 KeySet frontals(keys);
│ │ │ │ -
57 const size_t nFrontals = keys.size();
│ │ │ │ -
58
│ │ │ │ -
59 // Build a key vector with the frontals followed by the separator
│ │ │ │ -
60 KeyVector orderedKeys(allKeys.size());
│ │ │ │ -
61 std::copy(keys.begin(), keys.end(), orderedKeys.begin());
│ │ │ │ -
62 std::set_difference(allKeys.begin(), allKeys.end(), frontals.begin(), frontals.end(), orderedKeys.begin() + nFrontals);
│ │ │ │ -
63
│ │ │ │ -
64 // Return resulting conditional and factor
│ │ │ │ -
65 return std::make_pair(
│ │ │ │ -
66 SymbolicConditional::FromKeysShared(orderedKeys, nFrontals),
│ │ │ │ -
67 SymbolicFactor::FromIteratorsShared(orderedKeys.begin() + nFrontals, orderedKeys.end()));
│ │ │ │ -
68 }
│ │ │ │ -
│ │ │ │ -
69 }
│ │ │ │ -
70}
│ │ │ │ -
timing.h
Timing utilities.
│ │ │ │ - │ │ │ │ -
Factor.h
The base class for all factors.
│ │ │ │ -
gtsam::internal::EliminateSymbolic
std::pair< boost::shared_ptr< SymbolicConditional >, boost::shared_ptr< SymbolicFactor > > EliminateSymbolic(const FactorGraph< FACTOR > &factors, const Ordering &keys)
Implementation of dense elimination function for symbolic factors.
Definition SymbolicFactor-inst.h:39
│ │ │ │ - │ │ │ │ - │ │ │ │ +
55 public:
│ │ │ │ +
56 typedef JunctionTree<HybridBayesTree, HybridGaussianFactorGraph>
│ │ │ │ +
57 Base;
│ │ │ │ +
58 typedef HybridJunctionTree This;
│ │ │ │ +
59 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ +
60
│ │ │ │ +
70 HybridJunctionTree(const HybridEliminationTree& eliminationTree);
│ │ │ │ +
71};
│ │ │ │ +
│ │ │ │ +
72
│ │ │ │ +
73} // namespace gtsam
│ │ │ │ +
HybridBayesTree.h
Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree.
│ │ │ │ +
HybridGaussianFactorGraph.h
Linearized Hybrid factor graph that uses type erasure.
│ │ │ │ +
JunctionTree.h
The junction tree.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │ -
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │ - │ │ │ │ -
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ -
gtsam::SymbolicConditional::FromKeysShared
static SymbolicConditional::shared_ptr FromKeysShared(const CONTAINER &keys, size_t nrFrontals)
Named constructor from an arbitrary number of keys and frontals.
Definition SymbolicConditional.h:94
│ │ │ │ -
gtsam::SymbolicFactor::FromIteratorsShared
static SymbolicFactor::shared_ptr FromIteratorsShared(KEYITERATOR beginKey, KEYITERATOR endKey)
Constructor from a collection of keys.
Definition SymbolicFactor.h:121
│ │ │ │ +
gtsam::HybridEliminationTree
Elimination Tree type for Hybrid Factor Graphs.
Definition HybridEliminationTree.h:32
│ │ │ │ +
gtsam::HybridJunctionTree
An EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree,...
Definition HybridJunctionTree.h:53
│ │ │ │ +
gtsam::HybridJunctionTree::shared_ptr
boost::shared_ptr< This > shared_ptr
Shared pointer to this class.
Definition HybridJunctionTree.h:59
│ │ │ │ +
gtsam::HybridJunctionTree::Base
JunctionTree< HybridBayesTree, HybridGaussianFactorGraph > Base
Base class.
Definition HybridJunctionTree.h:57
│ │ │ │ +
gtsam::HybridJunctionTree::This
HybridJunctionTree This
This class.
Definition HybridJunctionTree.h:58
│ │ │ │ +
gtsam::JunctionTree
A JunctionTree is a cluster tree, a set of variable clusters with factors, arranged in a tree,...
Definition JunctionTree.h:50
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,13 +1,13 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SymbolicFactor-inst.h │ │ │ │ │ +HybridJunctionTree.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ @@ -16,105 +16,65 @@ │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ 18#pragma once │ │ │ │ │ 19 │ │ │ │ │ -20#include <_g_t_s_a_m_/_s_y_m_b_o_l_i_c_/_S_y_m_b_o_l_i_c_F_a_c_t_o_r_._h> │ │ │ │ │ -21#include <_g_t_s_a_m_/_s_y_m_b_o_l_i_c_/_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_b_a_s_e_/_t_i_m_i_n_g_._h> │ │ │ │ │ +20#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_h_y_b_r_i_d_/_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_J_u_n_c_t_i_o_n_T_r_e_e_._h> │ │ │ │ │ +23 │ │ │ │ │ +24namespace _g_t_s_a_m { │ │ │ │ │ 25 │ │ │ │ │ -26#include │ │ │ │ │ -27#include │ │ │ │ │ +26// Forward declarations │ │ │ │ │ +27class HybridEliminationTree; │ │ │ │ │ 28 │ │ │ │ │ -29#include │ │ │ │ │ -30 │ │ │ │ │ -31namespace _g_t_s_a_m │ │ │ │ │ -32{ │ │ │ │ │ -33 namespace internal │ │ │ │ │ -34 { │ │ │ │ │ -37 template │ │ │ │ │ -38 std::pair, boost:: │ │ │ │ │ -shared_ptr > │ │ │ │ │ -_3_9 _E_l_i_m_i_n_a_t_e_S_y_m_b_o_l_i_c(const _F_a_c_t_o_r_G_r_a_p_h_<_F_A_C_T_O_R_>& factors, const _O_r_d_e_r_i_n_g& keys) │ │ │ │ │ -40 { │ │ │ │ │ -41 gttic(_E_l_i_m_i_n_a_t_e_S_y_m_b_o_l_i_c); │ │ │ │ │ -42 │ │ │ │ │ -43 // Gather all keys │ │ │ │ │ -44 _K_e_y_S_e_t allKeys; │ │ │ │ │ -45 for(const boost::shared_ptr& factor: factors) { │ │ │ │ │ -46 allKeys.insert(factor->begin(), factor->end()); │ │ │ │ │ -47 } │ │ │ │ │ -48 │ │ │ │ │ -49 // Check keys │ │ │ │ │ -50 for(_K_e_y key: keys) { │ │ │ │ │ -51 if(allKeys.find(key) == allKeys.end()) │ │ │ │ │ -52 throw std::runtime_error("Requested to eliminate a key that is not in the │ │ │ │ │ -factors"); │ │ │ │ │ -53 } │ │ │ │ │ +_5_2class GTSAM_EXPORT _H_y_b_r_i_d_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ +53 : public _J_u_n_c_t_i_o_n_T_r_e_e { │ │ │ │ │ 54 │ │ │ │ │ -55 // Sort frontal keys │ │ │ │ │ -56 _K_e_y_S_e_t frontals(keys); │ │ │ │ │ -57 const size_t nFrontals = keys.size(); │ │ │ │ │ -58 │ │ │ │ │ -59 // Build a key vector with the frontals followed by the separator │ │ │ │ │ -60 _K_e_y_V_e_c_t_o_r orderedKeys(allKeys.size()); │ │ │ │ │ -61 std::copy(keys.begin(), keys.end(), orderedKeys.begin()); │ │ │ │ │ -62 std::set_difference(allKeys.begin(), allKeys.end(), frontals.begin(), │ │ │ │ │ -frontals.end(), orderedKeys.begin() + nFrontals); │ │ │ │ │ -63 │ │ │ │ │ -64 // Return resulting conditional and factor │ │ │ │ │ -65 return std::make_pair( │ │ │ │ │ -66 _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_F_r_o_m_K_e_y_s_S_h_a_r_e_d(orderedKeys, nFrontals), │ │ │ │ │ -67 _S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_F_r_o_m_I_t_e_r_a_t_o_r_s_S_h_a_r_e_d(orderedKeys.begin() + nFrontals, │ │ │ │ │ -orderedKeys.end())); │ │ │ │ │ -68 } │ │ │ │ │ -69 } │ │ │ │ │ -70} │ │ │ │ │ -_t_i_m_i_n_g_._h │ │ │ │ │ -Timing utilities. │ │ │ │ │ -_K_e_y_._h │ │ │ │ │ -_F_a_c_t_o_r_._h │ │ │ │ │ -The base class for all factors. │ │ │ │ │ -_g_t_s_a_m_:_:_i_n_t_e_r_n_a_l_:_:_E_l_i_m_i_n_a_t_e_S_y_m_b_o_l_i_c │ │ │ │ │ -std::pair< boost::shared_ptr< SymbolicConditional >, boost::shared_ptr< │ │ │ │ │ -SymbolicFactor > > EliminateSymbolic(const FactorGraph< FACTOR > &factors, │ │ │ │ │ -const Ordering &keys) │ │ │ │ │ -Implementation of dense elimination function for symbolic factors. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactor-inst.h:39 │ │ │ │ │ -_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_._h │ │ │ │ │ -_S_y_m_b_o_l_i_c_F_a_c_t_o_r_._h │ │ │ │ │ +55 public: │ │ │ │ │ +56 typedef _J_u_n_c_t_i_o_n_T_r_e_e_<_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_,_ _H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_> │ │ │ │ │ +_5_7 _B_a_s_e; │ │ │ │ │ +_5_8 typedef _H_y_b_r_i_d_J_u_n_c_t_i_o_n_T_r_e_e _T_h_i_s; │ │ │ │ │ +_5_9 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ +60 │ │ │ │ │ +70 _H_y_b_r_i_d_J_u_n_c_t_i_o_n_T_r_e_e(const _H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e& eliminationTree); │ │ │ │ │ +71}; │ │ │ │ │ +72 │ │ │ │ │ +73} // namespace gtsam │ │ │ │ │ +_H_y_b_r_i_d_B_a_y_e_s_T_r_e_e_._h │ │ │ │ │ +Hybrid Bayes Tree, the result of eliminating a HybridJunctionTree. │ │ │ │ │ +_H_y_b_r_i_d_G_a_u_s_s_i_a_n_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ +Linearized Hybrid factor graph that uses type erasure. │ │ │ │ │ +_J_u_n_c_t_i_o_n_T_r_e_e_._h │ │ │ │ │ +The junction tree. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ -FastVector< Key > KeyVector │ │ │ │ │ -Define collection type once and for all - also used in wrappers. │ │ │ │ │ -DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ -_g_t_s_a_m_:_:_K_e_y │ │ │ │ │ -std::uint64_t Key │ │ │ │ │ -Integer nonlinear key type. │ │ │ │ │ -DDeeffiinniittiioonn types.h:100 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_s_t_S_e_t_<_ _K_e_y_ _> │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ -nodes. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l_:_:_F_r_o_m_K_e_y_s_S_h_a_r_e_d │ │ │ │ │ -static SymbolicConditional::shared_ptr FromKeysShared(const CONTAINER &keys, │ │ │ │ │ -size_t nrFrontals) │ │ │ │ │ -Named constructor from an arbitrary number of keys and frontals. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicConditional.h:94 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_:_:_F_r_o_m_I_t_e_r_a_t_o_r_s_S_h_a_r_e_d │ │ │ │ │ -static SymbolicFactor::shared_ptr FromIteratorsShared(KEYITERATOR beginKey, │ │ │ │ │ -KEYITERATOR endKey) │ │ │ │ │ -Constructor from a collection of keys. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactor.h:121 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ +Elimination Tree type for Hybrid Factor Graphs. │ │ │ │ │ +DDeeffiinniittiioonn HybridEliminationTree.h:32 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ +An EliminatableClusterTree, i.e., a set of variable clusters with factors, │ │ │ │ │ +arranged in a tree,... │ │ │ │ │ +DDeeffiinniittiioonn HybridJunctionTree.h:53 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_J_u_n_c_t_i_o_n_T_r_e_e_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ +boost::shared_ptr< This > shared_ptr │ │ │ │ │ +Shared pointer to this class. │ │ │ │ │ +DDeeffiinniittiioonn HybridJunctionTree.h:59 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_J_u_n_c_t_i_o_n_T_r_e_e_:_:_B_a_s_e │ │ │ │ │ +JunctionTree< HybridBayesTree, HybridGaussianFactorGraph > Base │ │ │ │ │ +Base class. │ │ │ │ │ +DDeeffiinniittiioonn HybridJunctionTree.h:57 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_J_u_n_c_t_i_o_n_T_r_e_e_:_:_T_h_i_s │ │ │ │ │ +HybridJunctionTree This │ │ │ │ │ +This class. │ │ │ │ │ +DDeeffiinniittiioonn HybridJunctionTree.h:58 │ │ │ │ │ +_g_t_s_a_m_:_:_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ +A JunctionTree is a cluster tree, a set of variable clusters with factors, │ │ │ │ │ +arranged in a tree,... │ │ │ │ │ +DDeeffiinniittiioonn JunctionTree.h:50 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _s_y_m_b_o_l_i_c │ │ │ │ │ - * _S_y_m_b_o_l_i_c_F_a_c_t_o_r_-_i_n_s_t_._h │ │ │ │ │ + * _h_y_b_r_i_d │ │ │ │ │ + * _H_y_b_r_i_d_J_u_n_c_t_i_o_n_T_r_e_e_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8 │ │ │ │ --- ./usr/share/doc/libgtsam-dev/html/a01379_source.html │ │ │ ├── +++ ./usr/share/doc/libgtsam-dev/html/a00566_source.html │ │ │ │┄ Files 79% similar despite different names │ │ │ │ @@ -1,15 +1,15 @@ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ -gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/symbolic/SymbolicFactorGraph.h Source File │ │ │ │ +gtsam: /build/reproducible-path/gtsam-4.2.0+dfsg/gtsam/hybrid/HybridValues.h Source File │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ @@ -72,15 +72,15 @@ │ │ │ │ │ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ │ │ │ │
No Matches
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │
│ │ │ │ -
SymbolicFactorGraph.h
│ │ │ │ +
HybridValues.h
│ │ │ │
│ │ │ │
│ │ │ │ -Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │ +Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
│ │ │ │
2
│ │ │ │
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
│ │ │ │
4 * Atlanta, Georgia 30332-0415
│ │ │ │
5 * All Rights Reserved
│ │ │ │
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
│ │ │ │
7
│ │ │ │
8 * See LICENSE for the license information
│ │ │ │
9
│ │ │ │
10 * -------------------------------------------------------------------------- */
│ │ │ │
11
│ │ │ │ -
19#pragma once
│ │ │ │ -
20
│ │ │ │ -
21#include <gtsam/symbolic/SymbolicFactor.h>
│ │ │ │ -
22#include <gtsam/inference/FactorGraph.h>
│ │ │ │ -
23#include <gtsam/inference/EliminateableFactorGraph.h>
│ │ │ │ -
24#include <gtsam/base/types.h>
│ │ │ │ -
25
│ │ │ │ -
26namespace gtsam {
│ │ │ │ -
27
│ │ │ │ -
28 class SymbolicFactorGraph;
│ │ │ │ -
29 class SymbolicConditional;
│ │ │ │ -
30 class SymbolicBayesNet;
│ │ │ │ -
31 class SymbolicEliminationTree;
│ │ │ │ -
32 class SymbolicBayesTree;
│ │ │ │ -
33 class SymbolicJunctionTree;
│ │ │ │ -
34
│ │ │ │ -
35 /* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
36 template<> struct EliminationTraits<SymbolicFactorGraph>
│ │ │ │ -
37 {
│ │ │ │ -
38 typedef SymbolicFactor FactorType;
│ │ │ │ -
39 typedef SymbolicFactorGraph FactorGraphType;
│ │ │ │ -
40 typedef SymbolicConditional ConditionalType;
│ │ │ │ -
41 typedef SymbolicBayesNet BayesNetType;
│ │ │ │ -
42 typedef SymbolicEliminationTree EliminationTreeType;
│ │ │ │ -
43 typedef SymbolicBayesTree BayesTreeType;
│ │ │ │ -
44 typedef SymbolicJunctionTree JunctionTreeType;
│ │ │ │ -
46 static std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> >
│ │ │ │ -
│ │ │ │ -
47 DefaultEliminate(const FactorGraphType& factors, const Ordering& keys) {
│ │ │ │ -
48 return EliminateSymbolic(factors, keys); }
│ │ │ │ -
│ │ │ │ -
│ │ │ │ -
50 static Ordering DefaultOrderingFunc(
│ │ │ │ -
51 const FactorGraphType& graph,
│ │ │ │ -
52 boost::optional<const VariableIndex&> variableIndex) {
│ │ │ │ -
53 return Ordering::Colamd(*variableIndex);
│ │ │ │ -
54 }
│ │ │ │ -
│ │ │ │ -
55 };
│ │ │ │ -
│ │ │ │ -
56
│ │ │ │ -
57 /* ************************************************************************* */
│ │ │ │ -
│ │ │ │ -
61 class GTSAM_EXPORT SymbolicFactorGraph :
│ │ │ │ -
62 public FactorGraph<SymbolicFactor>,
│ │ │ │ -
63 public EliminateableFactorGraph<SymbolicFactorGraph>
│ │ │ │ -
64 {
│ │ │ │ -
65 public:
│ │ │ │ -
66
│ │ │ │ -
67 typedef SymbolicFactorGraph This;
│ │ │ │ -
68 typedef FactorGraph<SymbolicFactor> Base;
│ │ │ │ -
69 typedef EliminateableFactorGraph<This> BaseEliminateable;
│ │ │ │ -
70 typedef boost::shared_ptr<This> shared_ptr;
│ │ │ │ -
71
│ │ │ │ -
74
│ │ │ │ -
76 SymbolicFactorGraph() {}
│ │ │ │ -
77
│ │ │ │ -
79 template<typename ITERATOR>
│ │ │ │ -
80 SymbolicFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) : Base(firstFactor, lastFactor) {}
│ │ │ │ -
81
│ │ │ │ -
83 template<class CONTAINER>
│ │ │ │ -
84 explicit SymbolicFactorGraph(const CONTAINER& factors) : Base(factors) {}
│ │ │ │ -
85
│ │ │ │ -
87 template<class DERIVEDFACTOR>
│ │ │ │ -
88 SymbolicFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
│ │ │ │ -
89
│ │ │ │ -
│ │ │ │ -
94 SymbolicFactorGraph(
│ │ │ │ -
95 std::initializer_list<boost::shared_ptr<SymbolicFactor>> sharedFactors)
│ │ │ │ -
96 : Base(sharedFactors) {}
│ │ │ │ -
│ │ │ │ -
97
│ │ │ │ -
│ │ │ │ -
99 SymbolicFactorGraph(SymbolicFactor&& c) {
│ │ │ │ -
100 push_back(boost::make_shared<SymbolicFactor>(c));
│ │ │ │ -
101 }
│ │ │ │ +
18#pragma once
│ │ │ │ +
19
│ │ │ │ +
20#include <gtsam/discrete/Assignment.h>
│ │ │ │ +
21#include <gtsam/discrete/DiscreteKey.h>
│ │ │ │ +
22#include <gtsam/discrete/DiscreteValues.h>
│ │ │ │ +
23#include <gtsam/inference/Key.h>
│ │ │ │ +
24#include <gtsam/linear/VectorValues.h>
│ │ │ │ +
25#include <gtsam/nonlinear/Values.h>
│ │ │ │ +
26
│ │ │ │ +
27#include <map>
│ │ │ │ +
28#include <string>
│ │ │ │ +
29#include <vector>
│ │ │ │ +
30
│ │ │ │ +
31namespace gtsam {
│ │ │ │ +
32
│ │ │ │ +
│ │ │ │ +
38class GTSAM_EXPORT HybridValues {
│ │ │ │ +
39 private:
│ │ │ │ +
41 VectorValues continuous_;
│ │ │ │ +
42
│ │ │ │ +
44 DiscreteValues discrete_;
│ │ │ │ +
45
│ │ │ │ +
47 Values nonlinear_;
│ │ │ │ +
48
│ │ │ │ +
49 public:
│ │ │ │ +
52
│ │ │ │ +
54 HybridValues() = default;
│ │ │ │ +
55
│ │ │ │ +
│ │ │ │ +
57 HybridValues(const VectorValues& cv, const DiscreteValues& dv)
│ │ │ │ +
58 : continuous_(cv), discrete_(dv){};
│ │ │ │ +
│ │ │ │ +
59
│ │ │ │ +
│ │ │ │ +
61 HybridValues(const VectorValues& cv, const DiscreteValues& dv,
│ │ │ │ +
62 const Values& v)
│ │ │ │ +
63 : continuous_(cv), discrete_(dv), nonlinear_(v){};
│ │ │ │ +
│ │ │ │ +
64
│ │ │ │ +
68
│ │ │ │ +
│ │ │ │ +
70 void print(const std::string& s = "HybridValues",
│ │ │ │ +
71 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
│ │ │ │ +
72 std::cout << s << ": \n";
│ │ │ │ +
73 continuous_.print(" Continuous",
│ │ │ │ +
74 keyFormatter); // print continuous components
│ │ │ │ +
75 discrete_.print(" Discrete", keyFormatter); // print discrete components
│ │ │ │ +
76 };
│ │ │ │
│ │ │ │ +
77
│ │ │ │ +
│ │ │ │ +
79 bool equals(const HybridValues& other, double tol = 1e-9) const {
│ │ │ │ +
80 return continuous_.equals(other.continuous_, tol) &&
│ │ │ │ +
81 discrete_.equals(other.discrete_, tol);
│ │ │ │ +
82 }
│ │ │ │ +
│ │ │ │ +
83
│ │ │ │ +
87
│ │ │ │ +
89 const VectorValues& continuous() const { return continuous_; }
│ │ │ │ +
90
│ │ │ │ +
92 const DiscreteValues& discrete() const { return discrete_; }
│ │ │ │ +
93
│ │ │ │ +
95 const Values& nonlinear() const { return nonlinear_; }
│ │ │ │ +
96
│ │ │ │ +
98 bool existsVector(Key j) { return continuous_.exists(j); };
│ │ │ │ +
99
│ │ │ │ +
101 bool existsDiscrete(Key j) { return (discrete_.find(j) != discrete_.end()); };
│ │ │ │
102
│ │ │ │ +
│ │ │ │ +
104 bool existsNonlinear(Key j) {
│ │ │ │ +
105 return nonlinear_.exists(j);
│ │ │ │ +
106 };
│ │ │ │ +
│ │ │ │ +
107
│ │ │ │
│ │ │ │ -
109 SymbolicFactorGraph& operator()(SymbolicFactor&& c) {
│ │ │ │ -
110 push_back(boost::make_shared<SymbolicFactor>(c));
│ │ │ │ -
111 return *this;
│ │ │ │ -
112 }
│ │ │ │ -
│ │ │ │ -
113
│ │ │ │ -
115 virtual ~SymbolicFactorGraph() {}
│ │ │ │ -
116
│ │ │ │ +
109 bool exists(Key j) {
│ │ │ │ +
110 return existsVector(j) || existsDiscrete(j) || existsNonlinear(j);
│ │ │ │ +
111 };
│ │ │ │ +
│ │ │ │ +
112
│ │ │ │ +
117 void insert(Key j, const Vector& value) { continuous_.insert(j, value); }
│ │ │ │
118
│ │ │ │ -
121
│ │ │ │ -
122 bool equals(const This& fg, double tol = 1e-9) const;
│ │ │ │ -
123
│ │ │ │ -
│ │ │ │ -
125 void print(
│ │ │ │ -
126 const std::string& s = "SymbolicFactorGraph",
│ │ │ │ -
127 const KeyFormatter& formatter = DefaultKeyFormatter) const override {
│ │ │ │ -
128 Base::print(s, formatter);
│ │ │ │ -
129 }
│ │ │ │ -
│ │ │ │ -
130
│ │ │ │ -
132
│ │ │ │ -
135
│ │ │ │ -
137 void push_factor(Key key);
│ │ │ │ -
138
│ │ │ │ -
140 void push_factor(Key key1, Key key2);
│ │ │ │ +
123 void insert(Key j, size_t value) { discrete_[j] = value; };
│ │ │ │ +
124
│ │ │ │ +
│ │ │ │ +
126 void insert_or_assign(Key j, const Vector& value) {
│ │ │ │ +
127 continuous_.insert_or_assign(j, value);
│ │ │ │ +
128 }
│ │ │ │ +
│ │ │ │ +
129
│ │ │ │ +
│ │ │ │ +
131 void insert_or_assign(Key j, size_t value) {
│ │ │ │ +
132 discrete_[j] = value;
│ │ │ │ +
133 }
│ │ │ │ +
│ │ │ │ +
134
│ │ │ │ +
│ │ │ │ +
137 HybridValues& insert(const VectorValues& values) {
│ │ │ │ +
138 continuous_.insert(values);
│ │ │ │ +
139 return *this;
│ │ │ │ +
140 }
│ │ │ │ +
│ │ │ │
141
│ │ │ │ -
143 void push_factor(Key key1, Key key2, Key key3);
│ │ │ │ -
144
│ │ │ │ -
146 void push_factor(Key key1, Key key2, Key key3, Key key4);
│ │ │ │ -
147
│ │ │ │ -
149
│ │ │ │ -
150 private:
│ │ │ │ -
152 friend class boost::serialization::access;
│ │ │ │ -
153 template<class ARCHIVE>
│ │ │ │ -
154 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
│ │ │ │ -
155 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
│ │ │ │ -
156 }
│ │ │ │ -
157 };
│ │ │ │ -
│ │ │ │ -
158
│ │ │ │ -
160template<>
│ │ │ │ -
│ │ │ │ -
161struct traits<SymbolicFactorGraph> : public Testable<SymbolicFactorGraph> {
│ │ │ │ -
162};
│ │ │ │ -
│ │ │ │ -
163
│ │ │ │ -
164} //\ namespace gtsam
│ │ │ │ -
types.h
Typedefs for easier changing of types.
│ │ │ │ -
EliminateableFactorGraph.h
Variable elimination algorithms for factor graphs.
│ │ │ │ -
FactorGraph.h
Factor Graph Base Class.
│ │ │ │ - │ │ │ │ +
│ │ │ │ +
144 HybridValues& insert(const DiscreteValues& values) {
│ │ │ │ +
145 discrete_.insert(values);
│ │ │ │ +
146 return *this;
│ │ │ │ +
147 }
│ │ │ │ +
│ │ │ │ +
148
│ │ │ │ +
│ │ │ │ +
151 HybridValues& insert(const Values& values) {
│ │ │ │ +
152 nonlinear_.insert(values);
│ │ │ │ +
153 return *this;
│ │ │ │ +
154 }
│ │ │ │ +
│ │ │ │ +
155
│ │ │ │ +
│ │ │ │ +
158 HybridValues& insert(const HybridValues& values) {
│ │ │ │ +
159 continuous_.insert(values.continuous());
│ │ │ │ +
160 discrete_.insert(values.discrete());
│ │ │ │ +
161 nonlinear_.insert(values.nonlinear());
│ │ │ │ +
162 return *this;
│ │ │ │ +
163 }
│ │ │ │ +
│ │ │ │ +
164
│ │ │ │ +
169 Vector& at(Key j) { return continuous_.at(j); };
│ │ │ │ +
170
│ │ │ │ +
175 size_t& atDiscrete(Key j) { return discrete_.at(j); };
│ │ │ │ +
176
│ │ │ │ +
│ │ │ │ +
181 HybridValues& update(const VectorValues& values) {
│ │ │ │ +
182 continuous_.update(values);
│ │ │ │ +
183 return *this;
│ │ │ │ +
184 }
│ │ │ │ +
│ │ │ │ +
185
│ │ │ │ +
│ │ │ │ +
190 HybridValues& update(const DiscreteValues& values) {
│ │ │ │ +
191 discrete_.update(values);
│ │ │ │ +
192 return *this;
│ │ │ │ +
193 }
│ │ │ │ +
│ │ │ │ +
194
│ │ │ │ +
│ │ │ │ +
199 HybridValues& update(const HybridValues& values) {
│ │ │ │ +
200 continuous_.update(values.continuous());
│ │ │ │ +
201 discrete_.update(values.discrete());
│ │ │ │ +
202 return *this;
│ │ │ │ +
203 }
│ │ │ │ +
│ │ │ │ +
204
│ │ │ │ +
│ │ │ │ +
206 VectorValues continuousSubset(const KeyVector& keys) const {
│ │ │ │ +
207 VectorValues measurements;
│ │ │ │ +
208 for (const auto& key : keys) {
│ │ │ │ +
209 measurements.insert(key, continuous_.at(key));
│ │ │ │ +
210 }
│ │ │ │ +
211 return measurements;
│ │ │ │ +
212 }
│ │ │ │ +
│ │ │ │ +
213
│ │ │ │ +
217
│ │ │ │ +
│ │ │ │ +
224 std::string html(
│ │ │ │ +
225 const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
│ │ │ │ +
226 std::stringstream ss;
│ │ │ │ +
227 ss << this->continuous_.html(keyFormatter);
│ │ │ │ +
228 ss << this->discrete_.html(keyFormatter);
│ │ │ │ +
229 return ss.str();
│ │ │ │ +
230 };
│ │ │ │ +
│ │ │ │ +
231
│ │ │ │ +
233};
│ │ │ │ +
│ │ │ │ +
234
│ │ │ │ +
235// traits
│ │ │ │ +
236template <>
│ │ │ │ +
237struct traits<HybridValues> : public Testable<HybridValues> {};
│ │ │ │ +
238
│ │ │ │ +
239} // namespace gtsam
│ │ │ │ +
Assignment.h
An assignment from labels to a discrete value index (size_t)
│ │ │ │ + │ │ │ │ +
DiscreteKey.h
specialized key for discrete variables
│ │ │ │ + │ │ │ │ +
VectorValues.h
Factor Graph Values.
│ │ │ │ +
Values.h
A non-templated config holding any types of Manifold-group elements.
│ │ │ │
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
│ │ │ │ -
gtsam::EliminateSymbolic
std::pair< boost::shared_ptr< SymbolicConditional >, boost::shared_ptr< SymbolicFactor > > EliminateSymbolic(const SymbolicFactorGraph &factors, const Ordering &keys)
Dense elimination function for symbolic factors.
Definition SymbolicFactor.cpp:36
│ │ │ │ +
gtsam::KeyVector
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition Key.h:86
│ │ │ │
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
│ │ │ │
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
│ │ │ │
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
│ │ │ │ -
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
│ │ │ │
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
│ │ │ │ -
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition FactorGraph.h:97
│ │ │ │ -
gtsam::EliminationTraits
Traits class for eliminateable factor graphs, specifies the types that result from elimination,...
Definition EliminateableFactorGraph.h:36
│ │ │ │ -
gtsam::EliminateableFactorGraph
EliminateableFactorGraph is a base class for factor graphs that contains elimination algorithms.
Definition EliminateableFactorGraph.h:57
│ │ │ │ -
gtsam::Ordering
Definition Ordering.h:34
│ │ │ │ -
gtsam::Ordering::Colamd
static Ordering Colamd(const FACTOR_GRAPH &graph)
Compute a fill-reducing ordering using COLAMD from a factor graph (see details for note on performanc...
Definition Ordering.h:95
│ │ │ │ -
gtsam::SymbolicBayesNet
A SymbolicBayesNet is a Bayes Net of purely symbolic conditionals.
Definition SymbolicBayesNet.h:32
│ │ │ │ -
gtsam::SymbolicBayesTree
A Bayes tree that represents the connectivity between variables but is not associated with any probab...
Definition SymbolicBayesTree.h:51
│ │ │ │ -
gtsam::SymbolicConditional
SymbolicConditional is a conditional with keys but no probability data, produced by symbolic eliminat...
Definition SymbolicConditional.h:38
│ │ │ │ -
gtsam::SymbolicEliminationTree
Definition SymbolicEliminationTree.h:29
│ │ │ │ -
gtsam::SymbolicFactor
SymbolicFactor represents a symbolic factor that specifies graph topology but is not associated with ...
Definition SymbolicFactor.h:39
│ │ │ │ -
gtsam::EliminationTraits< SymbolicFactorGraph >::FactorType
SymbolicFactor FactorType
Type of factors in factor graph.
Definition SymbolicFactorGraph.h:38
│ │ │ │ -
gtsam::EliminationTraits< SymbolicFactorGraph >::FactorGraphType
SymbolicFactorGraph FactorGraphType
Type of the factor graph (e.g. GaussianFactorGraph)
Definition SymbolicFactorGraph.h:39
│ │ │ │ -
gtsam::EliminationTraits< SymbolicFactorGraph >::EliminationTreeType
SymbolicEliminationTree EliminationTreeType
Type of elimination tree.
Definition SymbolicFactorGraph.h:42
│ │ │ │ -
gtsam::EliminationTraits< SymbolicFactorGraph >::BayesNetType
SymbolicBayesNet BayesNetType
Type of Bayes net from sequential elimination.
Definition SymbolicFactorGraph.h:41
│ │ │ │ -
gtsam::EliminationTraits< SymbolicFactorGraph >::DefaultEliminate
static std::pair< boost::shared_ptr< ConditionalType >, boost::shared_ptr< FactorType > > DefaultEliminate(const FactorGraphType &factors, const Ordering &keys)
The default dense elimination function.
Definition SymbolicFactorGraph.h:47
│ │ │ │ -
gtsam::EliminationTraits< SymbolicFactorGraph >::DefaultOrderingFunc
static Ordering DefaultOrderingFunc(const FactorGraphType &graph, boost::optional< const VariableIndex & > variableIndex)
The default ordering generation function.
Definition SymbolicFactorGraph.h:50
│ │ │ │ -
gtsam::EliminationTraits< SymbolicFactorGraph >::JunctionTreeType
SymbolicJunctionTree JunctionTreeType
Type of Junction tree.
Definition SymbolicFactorGraph.h:44
│ │ │ │ -
gtsam::EliminationTraits< SymbolicFactorGraph >::BayesTreeType
SymbolicBayesTree BayesTreeType
Type of Bayes tree.
Definition SymbolicFactorGraph.h:43
│ │ │ │ -
gtsam::EliminationTraits< SymbolicFactorGraph >::ConditionalType
SymbolicConditional ConditionalType
Type of conditionals from elimination.
Definition SymbolicFactorGraph.h:40
│ │ │ │ -
gtsam::SymbolicFactorGraph
Symbolic Factor Graph.
Definition SymbolicFactorGraph.h:64
│ │ │ │ -
gtsam::SymbolicFactorGraph::SymbolicFactorGraph
SymbolicFactorGraph(SymbolicFactor &&c)
Construct from a single factor.
Definition SymbolicFactorGraph.h:99
│ │ │ │ -
gtsam::SymbolicFactorGraph::operator()
SymbolicFactorGraph & operator()(SymbolicFactor &&c)
Add a single factor and return a reference.
Definition SymbolicFactorGraph.h:109
│ │ │ │ -
gtsam::SymbolicFactorGraph::SymbolicFactorGraph
SymbolicFactorGraph()
Construct empty factor graph.
Definition SymbolicFactorGraph.h:76
│ │ │ │ -
gtsam::SymbolicFactorGraph::SymbolicFactorGraph
SymbolicFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph)
Implicit copy/downcast constructor to override explicit template container constructor.
Definition SymbolicFactorGraph.h:88
│ │ │ │ -
gtsam::SymbolicFactorGraph::SymbolicFactorGraph
SymbolicFactorGraph(std::initializer_list< boost::shared_ptr< SymbolicFactor > > sharedFactors)
Constructor that takes an initializer list of shared pointers.
Definition SymbolicFactorGraph.h:94
│ │ │ │ -
gtsam::SymbolicFactorGraph::print
void print(const std::string &s="SymbolicFactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print
Definition SymbolicFactorGraph.h:125
│ │ │ │ -
gtsam::SymbolicFactorGraph::BaseEliminateable
EliminateableFactorGraph< This > BaseEliminateable
Typedef to base elimination class.
Definition SymbolicFactorGraph.h:69
│ │ │ │ -
gtsam::SymbolicFactorGraph::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition SymbolicFactorGraph.h:70
│ │ │ │ -
gtsam::SymbolicFactorGraph::This
SymbolicFactorGraph This
Typedef to this class.
Definition SymbolicFactorGraph.h:67
│ │ │ │ -
gtsam::SymbolicFactorGraph::SymbolicFactorGraph
SymbolicFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor)
Construct from iterator over factors.
Definition SymbolicFactorGraph.h:80
│ │ │ │ -
gtsam::SymbolicFactorGraph::~SymbolicFactorGraph
virtual ~SymbolicFactorGraph()
Destructor.
Definition SymbolicFactorGraph.h:115
│ │ │ │ -
gtsam::SymbolicFactorGraph::Base
FactorGraph< SymbolicFactor > Base
Typedef to base factor graph type.
Definition SymbolicFactorGraph.h:68
│ │ │ │ -
gtsam::SymbolicFactorGraph::SymbolicFactorGraph
SymbolicFactorGraph(const CONTAINER &factors)
Construct from container of factors (shared_ptr or plain objects)
Definition SymbolicFactorGraph.h:84
│ │ │ │ -
gtsam::SymbolicJunctionTree
A EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree,...
Definition SymbolicJunctionTree.h:51
│ │ │ │ +
gtsam::DiscreteValues
A map from keys to values.
Definition DiscreteValues.h:34
│ │ │ │ +
gtsam::DiscreteValues::html
std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter, const Names &names={}) const
Output as a html table.
Definition DiscreteValues.cpp:104
│ │ │ │ +
gtsam::DiscreteValues::update
DiscreteValues & update(const DiscreteValues &values)
For all key/value pairs in values, replace values with corresponding keys in this object with those i...
Definition DiscreteValues.cpp:63
│ │ │ │ +
gtsam::DiscreteValues::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print required by Testable.
Definition DiscreteValues.cpp:31
│ │ │ │ +
gtsam::DiscreteValues::equals
bool equals(const DiscreteValues &x, double tol=1e-9) const
equals required by Testable for unit testing.
Definition DiscreteValues.cpp:40
│ │ │ │ +
gtsam::HybridValues
HybridValues represents a collection of DiscreteValues and VectorValues.
Definition HybridValues.h:38
│ │ │ │ +
gtsam::HybridValues::print
void print(const std::string &s="HybridValues", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print required by Testable for unit testing
Definition HybridValues.h:70
│ │ │ │ +
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Insert a vector value with key j.
Definition HybridValues.h:117
│ │ │ │ +
gtsam::HybridValues::insert
HybridValues & insert(const DiscreteValues &values)
Insert all discrete values from values.
Definition HybridValues.h:144
│ │ │ │ +
gtsam::HybridValues::HybridValues
HybridValues(const VectorValues &cv, const DiscreteValues &dv, const Values &v)
Construct from all values types.
Definition HybridValues.h:61
│ │ │ │ +
gtsam::HybridValues::html
std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Output as a html table.
Definition HybridValues.h:224
│ │ │ │ +
gtsam::HybridValues::continuousSubset
VectorValues continuousSubset(const KeyVector &keys) const
Extract continuous values with given keys.
Definition HybridValues.h:206
│ │ │ │ +
gtsam::HybridValues::insert_or_assign
void insert_or_assign(Key j, const Vector &value)
insert_or_assign() , similar to Values.h
Definition HybridValues.h:126
│ │ │ │ +
gtsam::HybridValues::insert
HybridValues & insert(const Values &values)
Insert all values from values.
Definition HybridValues.h:151
│ │ │ │ +
gtsam::HybridValues::equals
bool equals(const HybridValues &other, double tol=1e-9) const
equals required by Testable for unit testing
Definition HybridValues.h:79
│ │ │ │ +
gtsam::HybridValues::update
HybridValues & update(const VectorValues &values)
For all key/value pairs in values, replace continuous values with corresponding keys in this object w...
Definition HybridValues.h:181
│ │ │ │ +
gtsam::HybridValues::discrete
const DiscreteValues & discrete() const
Return the discrete values.
Definition HybridValues.h:92
│ │ │ │ +
gtsam::HybridValues::existsDiscrete
bool existsDiscrete(Key j)
Check whether a variable with key j exists in DiscreteValues.
Definition HybridValues.h:101
│ │ │ │ +
gtsam::HybridValues::continuous
const VectorValues & continuous() const
Return the multi-dimensional vector values.
Definition HybridValues.h:89
│ │ │ │ +
gtsam::HybridValues::at
Vector & at(Key j)
Read/write access to the vector value with key j, throws std::out_of_range if j does not exist.
Definition HybridValues.h:169
│ │ │ │ +
gtsam::HybridValues::exists
bool exists(Key j)
Check whether a variable with key j exists.
Definition HybridValues.h:109
│ │ │ │ +
gtsam::HybridValues::update
HybridValues & update(const DiscreteValues &values)
For all key/value pairs in values, replace discrete values with corresponding keys in this object wit...
Definition HybridValues.h:190
│ │ │ │ +
gtsam::HybridValues::insert
HybridValues & insert(const HybridValues &values)
Insert all values from values.
Definition HybridValues.h:158
│ │ │ │ +
gtsam::HybridValues::insert
HybridValues & insert(const VectorValues &values)
Insert all continuous values from values.
Definition HybridValues.h:137
│ │ │ │ +
gtsam::HybridValues::insert
void insert(Key j, size_t value)
Insert a discrete value with key j.
Definition HybridValues.h:123
│ │ │ │ +
gtsam::HybridValues::nonlinear
const Values & nonlinear() const
Return the nonlinear values.
Definition HybridValues.h:95
│ │ │ │ +
gtsam::HybridValues::existsNonlinear
bool existsNonlinear(Key j)
Check whether a variable with key j exists in values.
Definition HybridValues.h:104
│ │ │ │ +
gtsam::HybridValues::update
HybridValues & update(const HybridValues &values)
For all key/value pairs in values, replace all values with corresponding keys in this object with tho...
Definition HybridValues.h:199
│ │ │ │ +
gtsam::HybridValues::HybridValues
HybridValues()=default
Default constructor creates an empty HybridValues.
│ │ │ │ +
gtsam::HybridValues::existsVector
bool existsVector(Key j)
Check whether a variable with key j exists in VectorValues.
Definition HybridValues.h:98
│ │ │ │ +
gtsam::HybridValues::HybridValues
HybridValues(const VectorValues &cv, const DiscreteValues &dv)
Construct from DiscreteValues and VectorValues.
Definition HybridValues.h:57
│ │ │ │ +
gtsam::HybridValues::atDiscrete
size_t & atDiscrete(Key j)
Read/write access to the discrete value with key j, throws std::out_of_range if j does not exist.
Definition HybridValues.h:175
│ │ │ │ +
gtsam::HybridValues::insert_or_assign
void insert_or_assign(Key j, size_t value)
insert_or_assign() , similar to Values.h
Definition HybridValues.h:131
│ │ │ │ +
gtsam::VectorValues
VectorValues represents a collection of vector-valued variables associated each with a unique integer...
Definition VectorValues.h:74
│ │ │ │ +
gtsam::VectorValues::insert
iterator insert(const std::pair< Key, Vector > &key_value)
Insert a vector value with key j.
Definition VectorValues.cpp:91
│ │ │ │ +
gtsam::VectorValues::print
void print(const std::string &str="VectorValues", const KeyFormatter &formatter=DefaultKeyFormatter) const
print required by Testable for unit testing
Definition VectorValues.cpp:158
│ │ │ │ +
gtsam::VectorValues::equals
bool equals(const VectorValues &x, double tol=1e-9) const
equals required by Testable for unit testing
Definition VectorValues.cpp:166
│ │ │ │ +
gtsam::VectorValues::exists
bool exists(Key j) const
Check whether a variable with key j exists.
Definition VectorValues.h:133
│ │ │ │ +
gtsam::VectorValues::html
std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Output as a html table.
Definition VectorValues.cpp:387
│ │ │ │ +
gtsam::VectorValues::insert_or_assign
void insert_or_assign(Key j, const Vector &value)
insert_or_assign that mimics the STL map insert_or_assign - if the value already exists,...
Definition VectorValues.h:219
│ │ │ │ +
gtsam::VectorValues::at
Vector & at(Key j)
Read/write access to the vector value with key j, throws std::out_of_range if j does not exist,...
Definition VectorValues.h:139
│ │ │ │ +
gtsam::VectorValues::update
VectorValues & update(const VectorValues &values)
For all key/value pairs in values, replace values with corresponding keys in this class with those in...
Definition VectorValues.cpp:101
│ │ │ │ +
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition Values.h:65
│ │ │ │ +
gtsam::Values::insert
void insert(Key j, const Value &val)
Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present.
Definition Values.cpp:157
│ │ │ │ +
gtsam::Values::exists
bool exists(Key j) const
Check if a value exists with key j.
Definition Values.cpp:94
│ │ │ │
│ │ │ │
│ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ │ ├── html2text {} │ │ │ │ │ @@ -1,309 +1,392 @@ │ │ │ │ │ gtsam 4.2.0 │ │ │ │ │ gtsam │ │ │ │ │ Loading... │ │ │ │ │ Searching... │ │ │ │ │ No Matches │ │ │ │ │ -SymbolicFactorGraph.h │ │ │ │ │ +HybridValues.h │ │ │ │ │ _G_o_ _t_o_ _t_h_e_ _d_o_c_u_m_e_n_t_a_t_i_o_n_ _o_f_ _t_h_i_s_ _f_i_l_e_. │ │ │ │ │ 1/* --------------------------------------------------------------------------- │ │ │ │ │ - │ │ │ │ │ 2 │ │ │ │ │ 3 * GTSAM Copyright 2010, Georgia Tech Research Corporation, │ │ │ │ │ 4 * Atlanta, Georgia 30332-0415 │ │ │ │ │ 5 * All Rights Reserved │ │ │ │ │ 6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list) │ │ │ │ │ 7 │ │ │ │ │ 8 * See LICENSE for the license information │ │ │ │ │ 9 │ │ │ │ │ 10 * ------------------------------------------------------------------------- │ │ │ │ │ - */ │ │ │ │ │ 11 │ │ │ │ │ -19#pragma once │ │ │ │ │ -20 │ │ │ │ │ -21#include <_g_t_s_a_m_/_s_y_m_b_o_l_i_c_/_S_y_m_b_o_l_i_c_F_a_c_t_o_r_._h> │ │ │ │ │ -22#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_._h> │ │ │ │ │ -24#include <_g_t_s_a_m_/_b_a_s_e_/_t_y_p_e_s_._h> │ │ │ │ │ -25 │ │ │ │ │ -26namespace _g_t_s_a_m { │ │ │ │ │ -27 │ │ │ │ │ -28 class SymbolicFactorGraph; │ │ │ │ │ -29 class SymbolicConditional; │ │ │ │ │ -30 class SymbolicBayesNet; │ │ │ │ │ -31 class SymbolicEliminationTree; │ │ │ │ │ -32 class SymbolicBayesTree; │ │ │ │ │ -33 class SymbolicJunctionTree; │ │ │ │ │ -34 │ │ │ │ │ -35 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -_3_6 template<> struct _E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s<_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h> │ │ │ │ │ -37 { │ │ │ │ │ -_3_8 typedef _S_y_m_b_o_l_i_c_F_a_c_t_o_r _F_a_c_t_o_r_T_y_p_e; │ │ │ │ │ -_3_9 typedef _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e; │ │ │ │ │ -_4_0 typedef _S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l _C_o_n_d_i_t_i_o_n_a_l_T_y_p_e; │ │ │ │ │ -_4_1 typedef _S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t _B_a_y_e_s_N_e_t_T_y_p_e; │ │ │ │ │ -_4_2 typedef _S_y_m_b_o_l_i_c_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e _E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_T_y_p_e; │ │ │ │ │ -_4_3 typedef _S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e _B_a_y_e_s_T_r_e_e_T_y_p_e; │ │ │ │ │ -_4_4 typedef _S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e _J_u_n_c_t_i_o_n_T_r_e_e_T_y_p_e; │ │ │ │ │ -46 static std::pair, boost:: │ │ │ │ │ -shared_ptr > │ │ │ │ │ -_4_7 _D_e_f_a_u_l_t_E_l_i_m_i_n_a_t_e(const _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e& factors, const _O_r_d_e_r_i_n_g& keys) { │ │ │ │ │ -48 return _E_l_i_m_i_n_a_t_e_S_y_m_b_o_l_i_c(factors, keys); } │ │ │ │ │ -_5_0 static _O_r_d_e_r_i_n_g _D_e_f_a_u_l_t_O_r_d_e_r_i_n_g_F_u_n_c( │ │ │ │ │ -51 const _F_a_c_t_o_r_G_r_a_p_h_T_y_p_e& graph, │ │ │ │ │ -52 boost::optional variableIndex) { │ │ │ │ │ -53 return _O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d(*variableIndex); │ │ │ │ │ -54 } │ │ │ │ │ -55 }; │ │ │ │ │ -56 │ │ │ │ │ -57 /* ************************************************************************* │ │ │ │ │ -*/ │ │ │ │ │ -_6_1 class GTSAM_EXPORT _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h : │ │ │ │ │ -62 public _F_a_c_t_o_r_G_r_a_p_h, │ │ │ │ │ -63 public _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -64 { │ │ │ │ │ -65 public: │ │ │ │ │ -66 │ │ │ │ │ -_6_7 typedef _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h _T_h_i_s; │ │ │ │ │ -_6_8 typedef _F_a_c_t_o_r_G_r_a_p_h_<_S_y_m_b_o_l_i_c_F_a_c_t_o_r_> _B_a_s_e; │ │ │ │ │ -_6_9 typedef _E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_<_T_h_i_s_> _B_a_s_e_E_l_i_m_i_n_a_t_e_a_b_l_e; │ │ │ │ │ -_7_0 typedef boost::shared_ptr _s_h_a_r_e_d___p_t_r; │ │ │ │ │ -71 │ │ │ │ │ -74 │ │ │ │ │ -_7_6 _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h() {} │ │ │ │ │ +18#pragma once │ │ │ │ │ +19 │ │ │ │ │ +20#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_A_s_s_i_g_n_m_e_n_t_._h> │ │ │ │ │ +21#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_K_e_y_._h> │ │ │ │ │ +22#include <_g_t_s_a_m_/_d_i_s_c_r_e_t_e_/_D_i_s_c_r_e_t_e_V_a_l_u_e_s_._h> │ │ │ │ │ +23#include <_g_t_s_a_m_/_i_n_f_e_r_e_n_c_e_/_K_e_y_._h> │ │ │ │ │ +24#include <_g_t_s_a_m_/_l_i_n_e_a_r_/_V_e_c_t_o_r_V_a_l_u_e_s_._h> │ │ │ │ │ +25#include <_g_t_s_a_m_/_n_o_n_l_i_n_e_a_r_/_V_a_l_u_e_s_._h> │ │ │ │ │ +26 │ │ │ │ │ +27#include │ │ │ │ │ +28#include │ │ │ │ │ +29#include │ │ │ │ │ +30 │ │ │ │ │ +31namespace _g_t_s_a_m { │ │ │ │ │ +32 │ │ │ │ │ +_3_8class GTSAM_EXPORT _H_y_b_r_i_d_V_a_l_u_e_s { │ │ │ │ │ +39 private: │ │ │ │ │ +41 _V_e_c_t_o_r_V_a_l_u_e_s continuous_; │ │ │ │ │ +42 │ │ │ │ │ +44 _D_i_s_c_r_e_t_e_V_a_l_u_e_s discrete_; │ │ │ │ │ +45 │ │ │ │ │ +47 _V_a_l_u_e_s nonlinear_; │ │ │ │ │ +48 │ │ │ │ │ +49 public: │ │ │ │ │ +52 │ │ │ │ │ +_5_4 _H_y_b_r_i_d_V_a_l_u_e_s() = default; │ │ │ │ │ +55 │ │ │ │ │ +_5_7 _H_y_b_r_i_d_V_a_l_u_e_s(const _V_e_c_t_o_r_V_a_l_u_e_s& cv, const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& dv) │ │ │ │ │ +58 : continuous_(cv), discrete_(dv){}; │ │ │ │ │ +59 │ │ │ │ │ +_6_1 _H_y_b_r_i_d_V_a_l_u_e_s(const _V_e_c_t_o_r_V_a_l_u_e_s& cv, const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& dv, │ │ │ │ │ +62 const _V_a_l_u_e_s& v) │ │ │ │ │ +63 : continuous_(cv), discrete_(dv), nonlinear_(v){}; │ │ │ │ │ +64 │ │ │ │ │ +68 │ │ │ │ │ +_7_0 void _p_r_i_n_t(const std::string& s = "HybridValues", │ │ │ │ │ +71 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const { │ │ │ │ │ +72 std::cout << s << ": \n"; │ │ │ │ │ +73 continuous_._p_r_i_n_t(" Continuous", │ │ │ │ │ +74 keyFormatter); // print continuous components │ │ │ │ │ +75 discrete_._p_r_i_n_t(" Discrete", keyFormatter); // print discrete components │ │ │ │ │ +76 }; │ │ │ │ │ 77 │ │ │ │ │ -79 template │ │ │ │ │ -_8_0 _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h(ITERATOR firstFactor, ITERATOR lastFactor) : _B_a_s_e │ │ │ │ │ -(firstFactor, lastFactor) {} │ │ │ │ │ -81 │ │ │ │ │ -83 template │ │ │ │ │ -_8_4 explicit _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h(const CONTAINER& factors) : _B_a_s_e(factors) {} │ │ │ │ │ -85 │ │ │ │ │ -87 template │ │ │ │ │ -_8_8 _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h(const _F_a_c_t_o_r_G_r_a_p_h_<_D_E_R_I_V_E_D_F_A_C_T_O_R_>& graph) : _B_a_s_e(graph) │ │ │ │ │ -{} │ │ │ │ │ -89 │ │ │ │ │ -_9_4 _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h( │ │ │ │ │ -95 std::initializer_list> sharedFactors) │ │ │ │ │ -96 : _B_a_s_e(sharedFactors) {} │ │ │ │ │ -97 │ │ │ │ │ -_9_9 _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h(_S_y_m_b_o_l_i_c_F_a_c_t_o_r&& c) { │ │ │ │ │ -100 push_back(boost::make_shared(c)); │ │ │ │ │ -101 } │ │ │ │ │ +_7_9 bool _e_q_u_a_l_s(const _H_y_b_r_i_d_V_a_l_u_e_s& other, double tol = 1e-9) const { │ │ │ │ │ +80 return continuous_._e_q_u_a_l_s(other.continuous_, tol) && │ │ │ │ │ +81 discrete_._e_q_u_a_l_s(other.discrete_, tol); │ │ │ │ │ +82 } │ │ │ │ │ +83 │ │ │ │ │ +87 │ │ │ │ │ +_8_9 const _V_e_c_t_o_r_V_a_l_u_e_s& _c_o_n_t_i_n_u_o_u_s() const { return continuous_; } │ │ │ │ │ +90 │ │ │ │ │ +_9_2 const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& _d_i_s_c_r_e_t_e() const { return discrete_; } │ │ │ │ │ +93 │ │ │ │ │ +_9_5 const _V_a_l_u_e_s& _n_o_n_l_i_n_e_a_r() const { return nonlinear_; } │ │ │ │ │ +96 │ │ │ │ │ +_9_8 bool _e_x_i_s_t_s_V_e_c_t_o_r(_K_e_y j) { return continuous_._e_x_i_s_t_s(j); }; │ │ │ │ │ +99 │ │ │ │ │ +_1_0_1 bool _e_x_i_s_t_s_D_i_s_c_r_e_t_e(_K_e_y j) { return (discrete_.find(j) != discrete_.end()); │ │ │ │ │ +}; │ │ │ │ │ 102 │ │ │ │ │ -_1_0_9 _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h& _o_p_e_r_a_t_o_r_(_)(_S_y_m_b_o_l_i_c_F_a_c_t_o_r&& c) { │ │ │ │ │ -110 push_back(boost::make_shared(c)); │ │ │ │ │ -111 return *this; │ │ │ │ │ -112 } │ │ │ │ │ -113 │ │ │ │ │ -_1_1_5 virtual _~_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h() {} │ │ │ │ │ -116 │ │ │ │ │ +_1_0_4 bool _e_x_i_s_t_s_N_o_n_l_i_n_e_a_r(_K_e_y j) { │ │ │ │ │ +105 return nonlinear_._e_x_i_s_t_s(j); │ │ │ │ │ +106 }; │ │ │ │ │ +107 │ │ │ │ │ +_1_0_9 bool _e_x_i_s_t_s(_K_e_y j) { │ │ │ │ │ +110 return existsVector(j) || existsDiscrete(j) || existsNonlinear(j); │ │ │ │ │ +111 }; │ │ │ │ │ +112 │ │ │ │ │ +_1_1_7 void _i_n_s_e_r_t(_K_e_y j, const Vector& value) { continuous_._i_n_s_e_r_t(j, value); } │ │ │ │ │ 118 │ │ │ │ │ -121 │ │ │ │ │ -122 bool _e_q_u_a_l_s(const This& fg, double tol = 1e-9) const; │ │ │ │ │ -123 │ │ │ │ │ -_1_2_5 void _p_r_i_n_t( │ │ │ │ │ -126 const std::string& s = "SymbolicFactorGraph", │ │ │ │ │ -127 const _K_e_y_F_o_r_m_a_t_t_e_r& formatter = DefaultKeyFormatter) const override { │ │ │ │ │ -128 Base::print(s, formatter); │ │ │ │ │ -129 } │ │ │ │ │ -130 │ │ │ │ │ -132 │ │ │ │ │ -135 │ │ │ │ │ -137 void push_factor(_K_e_y key); │ │ │ │ │ -138 │ │ │ │ │ -140 void push_factor(_K_e_y key1, _K_e_y key2); │ │ │ │ │ +_1_2_3 void _i_n_s_e_r_t(_K_e_y j, size_t value) { discrete_[j] = value; }; │ │ │ │ │ +124 │ │ │ │ │ +_1_2_6 void _i_n_s_e_r_t___o_r___a_s_s_i_g_n(_K_e_y j, const Vector& value) { │ │ │ │ │ +127 continuous_._i_n_s_e_r_t___o_r___a_s_s_i_g_n(j, value); │ │ │ │ │ +128 } │ │ │ │ │ +129 │ │ │ │ │ +_1_3_1 void _i_n_s_e_r_t___o_r___a_s_s_i_g_n(_K_e_y j, size_t value) { │ │ │ │ │ +132 discrete_[j] = value; │ │ │ │ │ +133 } │ │ │ │ │ +134 │ │ │ │ │ +_1_3_7 _H_y_b_r_i_d_V_a_l_u_e_s& _i_n_s_e_r_t(const _V_e_c_t_o_r_V_a_l_u_e_s& values) { │ │ │ │ │ +138 continuous_._i_n_s_e_r_t(values); │ │ │ │ │ +139 return *this; │ │ │ │ │ +140 } │ │ │ │ │ 141 │ │ │ │ │ -143 void push_factor(_K_e_y key1, _K_e_y key2, _K_e_y key3); │ │ │ │ │ -144 │ │ │ │ │ -146 void push_factor(_K_e_y key1, _K_e_y key2, _K_e_y key3, _K_e_y key4); │ │ │ │ │ -147 │ │ │ │ │ -149 │ │ │ │ │ -150 private: │ │ │ │ │ -_1_5_2 friend class boost::serialization::access; │ │ │ │ │ -153 template │ │ │ │ │ -154 void serialize(ARCHIVE & ar, const unsigned int /*version*/) { │ │ │ │ │ -155 ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(_B_a_s_e); │ │ │ │ │ -156 } │ │ │ │ │ -157 }; │ │ │ │ │ -158 │ │ │ │ │ -160template<> │ │ │ │ │ -_1_6_1struct _t_r_a_i_t_s<_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h> : public _T_e_s_t_a_b_l_e { │ │ │ │ │ -162}; │ │ │ │ │ -163 │ │ │ │ │ -164} //\ namespace gtsam │ │ │ │ │ -_t_y_p_e_s_._h │ │ │ │ │ -Typedefs for easier changing of types. │ │ │ │ │ -_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Variable elimination algorithms for factor graphs. │ │ │ │ │ -_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ -Factor Graph Base Class. │ │ │ │ │ -_S_y_m_b_o_l_i_c_F_a_c_t_o_r_._h │ │ │ │ │ +_1_4_4 _H_y_b_r_i_d_V_a_l_u_e_s& _i_n_s_e_r_t(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values) { │ │ │ │ │ +145 discrete_.insert(values); │ │ │ │ │ +146 return *this; │ │ │ │ │ +147 } │ │ │ │ │ +148 │ │ │ │ │ +_1_5_1 _H_y_b_r_i_d_V_a_l_u_e_s& _i_n_s_e_r_t(const _V_a_l_u_e_s& values) { │ │ │ │ │ +152 nonlinear_._i_n_s_e_r_t(values); │ │ │ │ │ +153 return *this; │ │ │ │ │ +154 } │ │ │ │ │ +155 │ │ │ │ │ +_1_5_8 _H_y_b_r_i_d_V_a_l_u_e_s& _i_n_s_e_r_t(const _H_y_b_r_i_d_V_a_l_u_e_s& values) { │ │ │ │ │ +159 continuous_._i_n_s_e_r_t(values._c_o_n_t_i_n_u_o_u_s()); │ │ │ │ │ +160 discrete_.insert(values._d_i_s_c_r_e_t_e()); │ │ │ │ │ +161 nonlinear_._i_n_s_e_r_t(values._n_o_n_l_i_n_e_a_r()); │ │ │ │ │ +162 return *this; │ │ │ │ │ +163 } │ │ │ │ │ +164 │ │ │ │ │ +_1_6_9 Vector& _a_t(_K_e_y j) { return continuous_._a_t(j); }; │ │ │ │ │ +170 │ │ │ │ │ +_1_7_5 size_t& _a_t_D_i_s_c_r_e_t_e(_K_e_y j) { return discrete_.at(j); }; │ │ │ │ │ +176 │ │ │ │ │ +_1_8_1 _H_y_b_r_i_d_V_a_l_u_e_s& _u_p_d_a_t_e(const _V_e_c_t_o_r_V_a_l_u_e_s& values) { │ │ │ │ │ +182 continuous_._u_p_d_a_t_e(values); │ │ │ │ │ +183 return *this; │ │ │ │ │ +184 } │ │ │ │ │ +185 │ │ │ │ │ +_1_9_0 _H_y_b_r_i_d_V_a_l_u_e_s& _u_p_d_a_t_e(const _D_i_s_c_r_e_t_e_V_a_l_u_e_s& values) { │ │ │ │ │ +191 discrete_._u_p_d_a_t_e(values); │ │ │ │ │ +192 return *this; │ │ │ │ │ +193 } │ │ │ │ │ +194 │ │ │ │ │ +_1_9_9 _H_y_b_r_i_d_V_a_l_u_e_s& _u_p_d_a_t_e(const _H_y_b_r_i_d_V_a_l_u_e_s& values) { │ │ │ │ │ +200 continuous_._u_p_d_a_t_e(values._c_o_n_t_i_n_u_o_u_s()); │ │ │ │ │ +201 discrete_._u_p_d_a_t_e(values._d_i_s_c_r_e_t_e()); │ │ │ │ │ +202 return *this; │ │ │ │ │ +203 } │ │ │ │ │ +204 │ │ │ │ │ +_2_0_6 _V_e_c_t_o_r_V_a_l_u_e_s _c_o_n_t_i_n_u_o_u_s_S_u_b_s_e_t(const _K_e_y_V_e_c_t_o_r& keys) const { │ │ │ │ │ +207 _V_e_c_t_o_r_V_a_l_u_e_s measurements; │ │ │ │ │ +208 for (const auto& key : keys) { │ │ │ │ │ +209 measurements._i_n_s_e_r_t(key, continuous_._a_t(key)); │ │ │ │ │ +210 } │ │ │ │ │ +211 return measurements; │ │ │ │ │ +212 } │ │ │ │ │ +213 │ │ │ │ │ +217 │ │ │ │ │ +_2_2_4 std::string _h_t_m_l( │ │ │ │ │ +225 const _K_e_y_F_o_r_m_a_t_t_e_r& keyFormatter = DefaultKeyFormatter) const { │ │ │ │ │ +226 std::stringstream ss; │ │ │ │ │ +227 ss << this->continuous_._h_t_m_l(keyFormatter); │ │ │ │ │ +228 ss << this->discrete_._h_t_m_l(keyFormatter); │ │ │ │ │ +229 return ss.str(); │ │ │ │ │ +230 }; │ │ │ │ │ +231 │ │ │ │ │ +233}; │ │ │ │ │ +234 │ │ │ │ │ +235// traits │ │ │ │ │ +236template <> │ │ │ │ │ +_2_3_7struct _t_r_a_i_t_s<_H_y_b_r_i_d_V_a_l_u_e_s> : public _T_e_s_t_a_b_l_e {}; │ │ │ │ │ +238 │ │ │ │ │ +239} // namespace gtsam │ │ │ │ │ +_A_s_s_i_g_n_m_e_n_t_._h │ │ │ │ │ +An assignment from labels to a discrete value index (size_t) │ │ │ │ │ +_D_i_s_c_r_e_t_e_V_a_l_u_e_s_._h │ │ │ │ │ +_D_i_s_c_r_e_t_e_K_e_y_._h │ │ │ │ │ +specialized key for discrete variables │ │ │ │ │ +_K_e_y_._h │ │ │ │ │ +_V_e_c_t_o_r_V_a_l_u_e_s_._h │ │ │ │ │ +Factor Graph Values. │ │ │ │ │ +_V_a_l_u_e_s_._h │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ _g_t_s_a_m │ │ │ │ │ Global functions in a separate testing namespace. │ │ │ │ │ DDeeffiinniittiioonn chartTesting.h:28 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_S_y_m_b_o_l_i_c │ │ │ │ │ -std::pair< boost::shared_ptr< SymbolicConditional >, boost::shared_ptr< │ │ │ │ │ -SymbolicFactor > > EliminateSymbolic(const SymbolicFactorGraph &factors, const │ │ │ │ │ -Ordering &keys) │ │ │ │ │ -Dense elimination function for symbolic factors. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactor.cpp:36 │ │ │ │ │ +_g_t_s_a_m_:_:_K_e_y_V_e_c_t_o_r │ │ │ │ │ +FastVector< Key > KeyVector │ │ │ │ │ +Define collection type once and for all - also used in wrappers. │ │ │ │ │ +DDeeffiinniittiioonn Key.h:86 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y │ │ │ │ │ std::uint64_t Key │ │ │ │ │ Integer nonlinear key type. │ │ │ │ │ DDeeffiinniittiioonn types.h:100 │ │ │ │ │ _g_t_s_a_m_:_:_K_e_y_F_o_r_m_a_t_t_e_r │ │ │ │ │ std::function< std::string(Key)> KeyFormatter │ │ │ │ │ Typedef for a function to format a key, i.e. to convert it to a string. │ │ │ │ │ DDeeffiinniittiioonn Key.h:35 │ │ │ │ │ _g_t_s_a_m_:_:_t_r_a_i_t_s │ │ │ │ │ A manifold defines a space in which there is a notion of a linear tangent space │ │ │ │ │ that can be centered ... │ │ │ │ │ DDeeffiinniittiioonn concepts.h:30 │ │ │ │ │ -_g_t_s_a_m_:_:_e_q_u_a_l_s │ │ │ │ │ -Template to create a binary predicate. │ │ │ │ │ -DDeeffiinniittiioonn Testable.h:111 │ │ │ │ │ _g_t_s_a_m_:_:_T_e_s_t_a_b_l_e │ │ │ │ │ A helper that implements the traits interface for GTSAM types. │ │ │ │ │ DDeeffiinniittiioonn Testable.h:151 │ │ │ │ │ -_g_t_s_a_m_:_:_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -A factor graph is a bipartite graph with factor nodes connected to variable │ │ │ │ │ -nodes. │ │ │ │ │ -DDeeffiinniittiioonn FactorGraph.h:97 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s │ │ │ │ │ -Traits class for eliminateable factor graphs, specifies the types that result │ │ │ │ │ -from elimination,... │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph.h:36 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_e_a_b_l_e_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -EliminateableFactorGraph is a base class for factor graphs that contains │ │ │ │ │ -elimination algorithms. │ │ │ │ │ -DDeeffiinniittiioonn EliminateableFactorGraph.h:57 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:34 │ │ │ │ │ -_g_t_s_a_m_:_:_O_r_d_e_r_i_n_g_:_:_C_o_l_a_m_d │ │ │ │ │ -static Ordering Colamd(const FACTOR_GRAPH &graph) │ │ │ │ │ -Compute a fill-reducing ordering using COLAMD from a factor graph (see details │ │ │ │ │ -for note on performanc... │ │ │ │ │ -DDeeffiinniittiioonn Ordering.h:95 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_N_e_t │ │ │ │ │ -A SymbolicBayesNet is a Bayes Net of purely symbolic conditionals. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicBayesNet.h:32 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_B_a_y_e_s_T_r_e_e │ │ │ │ │ -A Bayes tree that represents the connectivity between variables but is not │ │ │ │ │ -associated with any probab... │ │ │ │ │ -DDeeffiinniittiioonn SymbolicBayesTree.h:51 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_C_o_n_d_i_t_i_o_n_a_l │ │ │ │ │ -SymbolicConditional is a conditional with keys but no probability data, │ │ │ │ │ -produced by symbolic eliminat... │ │ │ │ │ -DDeeffiinniittiioonn SymbolicConditional.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e │ │ │ │ │ -DDeeffiinniittiioonn SymbolicEliminationTree.h:29 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r │ │ │ │ │ -SymbolicFactor represents a symbolic factor that specifies graph topology but │ │ │ │ │ -is not associated with ... │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactor.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_F_a_c_t_o_r_T_y_p_e │ │ │ │ │ -SymbolicFactor FactorType │ │ │ │ │ -Type of factors in factor graph. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactorGraph.h:38 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_F_a_c_t_o_r_G_r_a_p_h_T_y_p_e │ │ │ │ │ -SymbolicFactorGraph FactorGraphType │ │ │ │ │ -Type of the factor graph (e.g. GaussianFactorGraph) │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactorGraph.h:39 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_e_e_T_y_p_e │ │ │ │ │ -SymbolicEliminationTree EliminationTreeType │ │ │ │ │ -Type of elimination tree. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactorGraph.h:42 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_B_a_y_e_s_N_e_t_T_y_p_e │ │ │ │ │ -SymbolicBayesNet BayesNetType │ │ │ │ │ -Type of Bayes net from sequential elimination. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactorGraph.h:41 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_D_e_f_a_u_l_t_E_l_i_m_i_n_a_t_e │ │ │ │ │ -static std::pair< boost::shared_ptr< ConditionalType >, boost::shared_ptr< │ │ │ │ │ -FactorType > > DefaultEliminate(const FactorGraphType &factors, const Ordering │ │ │ │ │ -&keys) │ │ │ │ │ -The default dense elimination function. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactorGraph.h:47 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_D_e_f_a_u_l_t_O_r_d_e_r_i_n_g_F_u_n_c │ │ │ │ │ -static Ordering DefaultOrderingFunc(const FactorGraphType &graph, boost:: │ │ │ │ │ -optional< const VariableIndex & > variableIndex) │ │ │ │ │ -The default ordering generation function. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactorGraph.h:50 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_J_u_n_c_t_i_o_n_T_r_e_e_T_y_p_e │ │ │ │ │ -SymbolicJunctionTree JunctionTreeType │ │ │ │ │ -Type of Junction tree. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactorGraph.h:44 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_B_a_y_e_s_T_r_e_e_T_y_p_e │ │ │ │ │ -SymbolicBayesTree BayesTreeType │ │ │ │ │ -Type of Bayes tree. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactorGraph.h:43 │ │ │ │ │ -_g_t_s_a_m_:_:_E_l_i_m_i_n_a_t_i_o_n_T_r_a_i_t_s_<_ _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_ _>_:_:_C_o_n_d_i_t_i_o_n_a_l_T_y_p_e │ │ │ │ │ -SymbolicConditional ConditionalType │ │ │ │ │ -Type of conditionals from elimination. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactorGraph.h:40 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -Symbolic Factor Graph. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactorGraph.h:64 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -SymbolicFactorGraph(SymbolicFactor &&c) │ │ │ │ │ -Construct from a single factor. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactorGraph.h:99 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_:_:_o_p_e_r_a_t_o_r_(_) │ │ │ │ │ -SymbolicFactorGraph & operator()(SymbolicFactor &&c) │ │ │ │ │ -Add a single factor and return a reference. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactorGraph.h:109 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -SymbolicFactorGraph() │ │ │ │ │ -Construct empty factor graph. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactorGraph.h:76 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -SymbolicFactorGraph(const FactorGraph< DERIVEDFACTOR > &graph) │ │ │ │ │ -Implicit copy/downcast constructor to override explicit template container │ │ │ │ │ -constructor. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactorGraph.h:88 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -SymbolicFactorGraph(std::initializer_list< boost::shared_ptr< SymbolicFactor > │ │ │ │ │ -> sharedFactors) │ │ │ │ │ -Constructor that takes an initializer list of shared pointers. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactorGraph.h:94 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_:_:_p_r_i_n_t │ │ │ │ │ -void print(const std::string &s="SymbolicFactorGraph", const KeyFormatter │ │ │ │ │ -&formatter=DefaultKeyFormatter) const override │ │ │ │ │ -print │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactorGraph.h:125 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_:_:_B_a_s_e_E_l_i_m_i_n_a_t_e_a_b_l_e │ │ │ │ │ -EliminateableFactorGraph< This > BaseEliminateable │ │ │ │ │ -Typedef to base elimination class. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactorGraph.h:69 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_:_:_s_h_a_r_e_d___p_t_r │ │ │ │ │ -boost::shared_ptr< This > shared_ptr │ │ │ │ │ -shared_ptr to this class │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactorGraph.h:70 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_:_:_T_h_i_s │ │ │ │ │ -SymbolicFactorGraph This │ │ │ │ │ -Typedef to this class. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactorGraph.h:67 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -SymbolicFactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) │ │ │ │ │ -Construct from iterator over factors. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactorGraph.h:80 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_:_:_~_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -virtual ~SymbolicFactorGraph() │ │ │ │ │ -Destructor. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactorGraph.h:115 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_:_:_B_a_s_e │ │ │ │ │ -FactorGraph< SymbolicFactor > Base │ │ │ │ │ -Typedef to base factor graph type. │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactorGraph.h:68 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_:_:_S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h │ │ │ │ │ -SymbolicFactorGraph(const CONTAINER &factors) │ │ │ │ │ -Construct from container of factors (shared_ptr or plain objects) │ │ │ │ │ -DDeeffiinniittiioonn SymbolicFactorGraph.h:84 │ │ │ │ │ -_g_t_s_a_m_:_:_S_y_m_b_o_l_i_c_J_u_n_c_t_i_o_n_T_r_e_e │ │ │ │ │ -A EliminatableClusterTree, i.e., a set of variable clusters with factors, │ │ │ │ │ -arranged in a tree,... │ │ │ │ │ -DDeeffiinniittiioonn SymbolicJunctionTree.h:51 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s │ │ │ │ │ +A map from keys to values. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteValues.h:34 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_h_t_m_l │ │ │ │ │ +std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter, const │ │ │ │ │ +Names &names={}) const │ │ │ │ │ +Output as a html table. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteValues.cpp:104 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_u_p_d_a_t_e │ │ │ │ │ +DiscreteValues & update(const DiscreteValues &values) │ │ │ │ │ +For all key/value pairs in values, replace values with corresponding keys in │ │ │ │ │ +this object with those i... │ │ │ │ │ +DDeeffiinniittiioonn DiscreteValues.cpp:63 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ +print required by Testable. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteValues.cpp:31 │ │ │ │ │ +_g_t_s_a_m_:_:_D_i_s_c_r_e_t_e_V_a_l_u_e_s_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const DiscreteValues &x, double tol=1e-9) const │ │ │ │ │ +equals required by Testable for unit testing. │ │ │ │ │ +DDeeffiinniittiioonn DiscreteValues.cpp:40 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +HybridValues represents a collection of DiscreteValues and VectorValues. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:38 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &s="HybridValues", const KeyFormatter │ │ │ │ │ +&keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ +print required by Testable for unit testing │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:70 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_i_n_s_e_r_t │ │ │ │ │ +void insert(Key j, const Vector &value) │ │ │ │ │ +Insert a vector value with key j. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:117 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_i_n_s_e_r_t │ │ │ │ │ +HybridValues & insert(const DiscreteValues &values) │ │ │ │ │ +Insert all discrete values from values. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:144 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +HybridValues(const VectorValues &cv, const DiscreteValues &dv, const Values &v) │ │ │ │ │ +Construct from all values types. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:61 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_h_t_m_l │ │ │ │ │ +std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ +Output as a html table. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:224 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_c_o_n_t_i_n_u_o_u_s_S_u_b_s_e_t │ │ │ │ │ +VectorValues continuousSubset(const KeyVector &keys) const │ │ │ │ │ +Extract continuous values with given keys. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:206 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_i_n_s_e_r_t___o_r___a_s_s_i_g_n │ │ │ │ │ +void insert_or_assign(Key j, const Vector &value) │ │ │ │ │ +insert_or_assign() , similar to Values.h │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:126 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_i_n_s_e_r_t │ │ │ │ │ +HybridValues & insert(const Values &values) │ │ │ │ │ +Insert all values from values. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:151 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const HybridValues &other, double tol=1e-9) const │ │ │ │ │ +equals required by Testable for unit testing │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:79 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_u_p_d_a_t_e │ │ │ │ │ +HybridValues & update(const VectorValues &values) │ │ │ │ │ +For all key/value pairs in values, replace continuous values with corresponding │ │ │ │ │ +keys in this object w... │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:181 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_d_i_s_c_r_e_t_e │ │ │ │ │ +const DiscreteValues & discrete() const │ │ │ │ │ +Return the discrete values. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:92 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_e_x_i_s_t_s_D_i_s_c_r_e_t_e │ │ │ │ │ +bool existsDiscrete(Key j) │ │ │ │ │ +Check whether a variable with key j exists in DiscreteValues. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:101 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_c_o_n_t_i_n_u_o_u_s │ │ │ │ │ +const VectorValues & continuous() const │ │ │ │ │ +Return the multi-dimensional vector values. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:89 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_a_t │ │ │ │ │ +Vector & at(Key j) │ │ │ │ │ +Read/write access to the vector value with key j, throws std::out_of_range if j │ │ │ │ │ +does not exist. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:169 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_e_x_i_s_t_s │ │ │ │ │ +bool exists(Key j) │ │ │ │ │ +Check whether a variable with key j exists. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:109 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_u_p_d_a_t_e │ │ │ │ │ +HybridValues & update(const DiscreteValues &values) │ │ │ │ │ +For all key/value pairs in values, replace discrete values with corresponding │ │ │ │ │ +keys in this object wit... │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:190 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_i_n_s_e_r_t │ │ │ │ │ +HybridValues & insert(const HybridValues &values) │ │ │ │ │ +Insert all values from values. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:158 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_i_n_s_e_r_t │ │ │ │ │ +HybridValues & insert(const VectorValues &values) │ │ │ │ │ +Insert all continuous values from values. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:137 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_i_n_s_e_r_t │ │ │ │ │ +void insert(Key j, size_t value) │ │ │ │ │ +Insert a discrete value with key j. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:123 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_n_o_n_l_i_n_e_a_r │ │ │ │ │ +const Values & nonlinear() const │ │ │ │ │ +Return the nonlinear values. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:95 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_e_x_i_s_t_s_N_o_n_l_i_n_e_a_r │ │ │ │ │ +bool existsNonlinear(Key j) │ │ │ │ │ +Check whether a variable with key j exists in values. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:104 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_u_p_d_a_t_e │ │ │ │ │ +HybridValues & update(const HybridValues &values) │ │ │ │ │ +For all key/value pairs in values, replace all values with corresponding keys │ │ │ │ │ +in this object with tho... │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:199 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +HybridValues()=default │ │ │ │ │ +Default constructor creates an empty HybridValues. │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_e_x_i_s_t_s_V_e_c_t_o_r │ │ │ │ │ +bool existsVector(Key j) │ │ │ │ │ +Check whether a variable with key j exists in VectorValues. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:98 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_H_y_b_r_i_d_V_a_l_u_e_s │ │ │ │ │ +HybridValues(const VectorValues &cv, const DiscreteValues &dv) │ │ │ │ │ +Construct from DiscreteValues and VectorValues. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:57 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_a_t_D_i_s_c_r_e_t_e │ │ │ │ │ +size_t & atDiscrete(Key j) │ │ │ │ │ +Read/write access to the discrete value with key j, throws std::out_of_range if │ │ │ │ │ +j does not exist. │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:175 │ │ │ │ │ +_g_t_s_a_m_:_:_H_y_b_r_i_d_V_a_l_u_e_s_:_:_i_n_s_e_r_t___o_r___a_s_s_i_g_n │ │ │ │ │ +void insert_or_assign(Key j, size_t value) │ │ │ │ │ +insert_or_assign() , similar to Values.h │ │ │ │ │ +DDeeffiinniittiioonn HybridValues.h:131 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s │ │ │ │ │ +VectorValues represents a collection of vector-valued variables associated each │ │ │ │ │ +with a unique integer... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:74 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_i_n_s_e_r_t │ │ │ │ │ +iterator insert(const std::pair< Key, Vector > &key_value) │ │ │ │ │ +Insert a vector value with key j. │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.cpp:91 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_p_r_i_n_t │ │ │ │ │ +void print(const std::string &str="VectorValues", const KeyFormatter │ │ │ │ │ +&formatter=DefaultKeyFormatter) const │ │ │ │ │ +print required by Testable for unit testing │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.cpp:158 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_e_q_u_a_l_s │ │ │ │ │ +bool equals(const VectorValues &x, double tol=1e-9) const │ │ │ │ │ +equals required by Testable for unit testing │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.cpp:166 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_e_x_i_s_t_s │ │ │ │ │ +bool exists(Key j) const │ │ │ │ │ +Check whether a variable with key j exists. │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:133 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_h_t_m_l │ │ │ │ │ +std::string html(const KeyFormatter &keyFormatter=DefaultKeyFormatter) const │ │ │ │ │ +Output as a html table. │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.cpp:387 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_i_n_s_e_r_t___o_r___a_s_s_i_g_n │ │ │ │ │ +void insert_or_assign(Key j, const Vector &value) │ │ │ │ │ +insert_or_assign that mimics the STL map insert_or_assign - if the value │ │ │ │ │ +already exists,... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:219 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_a_t │ │ │ │ │ +Vector & at(Key j) │ │ │ │ │ +Read/write access to the vector value with key j, throws std::out_of_range if j │ │ │ │ │ +does not exist,... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.h:139 │ │ │ │ │ +_g_t_s_a_m_:_:_V_e_c_t_o_r_V_a_l_u_e_s_:_:_u_p_d_a_t_e │ │ │ │ │ +VectorValues & update(const VectorValues &values) │ │ │ │ │ +For all key/value pairs in values, replace values with corresponding keys in │ │ │ │ │ +this class with those in... │ │ │ │ │ +DDeeffiinniittiioonn VectorValues.cpp:101 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s │ │ │ │ │ +A non-templated config holding any types of Manifold-group elements. │ │ │ │ │ +DDeeffiinniittiioonn Values.h:65 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_i_n_s_e_r_t │ │ │ │ │ +void insert(Key j, const Value &val) │ │ │ │ │ +Add a variable with the given j, throws KeyAlreadyExists if j is already │ │ │ │ │ +present. │ │ │ │ │ +DDeeffiinniittiioonn Values.cpp:157 │ │ │ │ │ +_g_t_s_a_m_:_:_V_a_l_u_e_s_:_:_e_x_i_s_t_s │ │ │ │ │ +bool exists(Key j) const │ │ │ │ │ +Check if a value exists with key j. │ │ │ │ │ +DDeeffiinniittiioonn Values.cpp:94 │ │ │ │ │ * _g_t_s_a_m │ │ │ │ │ - * _s_y_m_b_o_l_i_c │ │ │ │ │ - * _S_y_m_b_o_l_i_c_F_a_c_t_o_r_G_r_a_p_h_._h │ │ │ │ │ + * _h_y_b_r_i_d │ │ │ │ │ + * _H_y_b_r_i_d_V_a_l_u_e_s_._h │ │ │ │ │ * Generated on Tue Jan 9 2024 05:51:35 for gtsam by _[_d_o_x_y_g_e_n_]1.9.8